3158 files changed, 243009 insertions, 102776 deletions

diff --git a/drivers/Kconfig b/drivers/Kconfig
index 78a956e286e6..9953a42809ec 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -120,6 +120,8 @@ source "drivers/vfio/Kconfig"
 
 source "drivers/vlynq/Kconfig"
 
+source "drivers/virt/Kconfig"
+
 source "drivers/virtio/Kconfig"
 
 source "drivers/hv/Kconfig"
@@ -144,8 +146,6 @@ source "drivers/remoteproc/Kconfig"
 
 source "drivers/rpmsg/Kconfig"
 
-source "drivers/virt/Kconfig"
-
 source "drivers/devfreq/Kconfig"
 
 source "drivers/extcon/Kconfig"
@@ -164,4 +164,6 @@ source "drivers/irqchip/Kconfig"
 
 source "drivers/ipack/Kconfig"
 
+source "drivers/reset/Kconfig"
+
 endmenu
diff --git a/drivers/Makefile b/drivers/Makefile
index 33360de63650..130abc1dfd65 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -37,6 +37,9 @@ obj-$(CONFIG_XEN)		+= xen/
 # regulators early, since some subsystems rely on them to initialize
 obj-$(CONFIG_REGULATOR)		+= regulator/
 
+# reset controllers early, since gpu drivers might rely on them to initialize
+obj-$(CONFIG_RESET_CONTROLLER)	+= reset/
+
 # tty/ comes before char/ so that the VT console is the boot-time
 # default.
 obj-y				+= tty/
@@ -124,7 +127,7 @@ obj-$(CONFIG_PPC_PS3)		+= ps3/
 obj-$(CONFIG_OF)		+= of/
 obj-$(CONFIG_SSB)		+= ssb/
 obj-$(CONFIG_BCMA)		+= bcma/
-obj-$(CONFIG_VHOST_NET)		+= vhost/
+obj-$(CONFIG_VHOST_RING)	+= vhost/
 obj-$(CONFIG_VLYNQ)		+= vlynq/
 obj-$(CONFIG_STAGING)		+= staging/
 obj-y				+= platform/
diff --git a/drivers/acpi/Kconfig b/drivers/acpi/Kconfig
index 4bf68c8d4797..100bd724f648 100644
--- a/drivers/acpi/Kconfig
+++ b/drivers/acpi/Kconfig
@@ -298,14 +298,6 @@ config ACPI_DEBUG
 	  Documentation/kernel-parameters.txt to control the type and
 	  amount of debug output.
 
-config ACPI_DEBUG_FUNC_TRACE
-	bool "Additionally enable ACPI function tracing"
-	default n
-	depends on ACPI_DEBUG
-	help
-	  ACPI Debug Statements slow down ACPI processing. Function trace
-	  is about half of the penalty and is rarely useful.
-
 config ACPI_PCI_SLOT
 	bool "PCI slot detection driver"
 	depends on SYSFS
@@ -334,7 +326,7 @@ config X86_PM_TIMER
 
 config ACPI_CONTAINER
 	bool "Container and Module Devices"
-	default (ACPI_HOTPLUG_MEMORY || ACPI_HOTPLUG_CPU || ACPI_HOTPLUG_IO)
+	default (ACPI_HOTPLUG_MEMORY || ACPI_HOTPLUG_CPU)
 	help
 	  This driver supports ACPI Container and Module devices (IDs
 	  ACPI0004, PNP0A05, and PNP0A06).
@@ -345,9 +337,8 @@ config ACPI_CONTAINER
 	  the module will be called container.
 
 config ACPI_HOTPLUG_MEMORY
-	tristate "Memory Hotplug"
+	bool "Memory Hotplug"
 	depends on MEMORY_HOTPLUG
-	default n
 	help
 	  This driver supports ACPI memory hotplug.  The driver
 	  fields notifications on ACPI memory devices (PNP0C80),
diff --git a/drivers/acpi/Makefile b/drivers/acpi/Makefile
index 474fcfeba66c..ecb743bf05a5 100644
--- a/drivers/acpi/Makefile
+++ b/drivers/acpi/Makefile
@@ -39,6 +39,7 @@ acpi-y				+= ec.o
 acpi-$(CONFIG_ACPI_DOCK)	+= dock.o
 acpi-y				+= pci_root.o pci_link.o pci_irq.o
 acpi-y				+= csrt.o
+acpi-$(CONFIG_X86_INTEL_LPSS)	+= acpi_lpss.o
 acpi-y				+= acpi_platform.o
 acpi-y				+= power.o
 acpi-y				+= event.o
diff --git a/drivers/acpi/ac.c b/drivers/acpi/ac.c
index 6d5bf649196d..00d2efd674df 100644
--- a/drivers/acpi/ac.c
+++ b/drivers/acpi/ac.c
@@ -194,7 +194,7 @@ static int acpi_ac_seq_show(struct seq_file *seq, void *offset)
 
 static int acpi_ac_open_fs(struct inode *inode, struct file *file)
 {
-	return single_open(file, acpi_ac_seq_show, PDE(inode)->data);
+	return single_open(file, acpi_ac_seq_show, PDE_DATA(inode));
 }
 
 static int acpi_ac_add_fs(struct acpi_device *device)
diff --git a/drivers/acpi/acpi_lpss.c b/drivers/acpi/acpi_lpss.c
new file mode 100644
index 000000000000..b1c95422ce74
--- /dev/null
+++ b/drivers/acpi/acpi_lpss.c
@@ -0,0 +1,292 @@
+/*
+ * ACPI support for Intel Lynxpoint LPSS.
+ *
+ * Copyright (C) 2013, Intel Corporation
+ * Authors: Mika Westerberg <mika.westerberg@linux.intel.com>
+ *          Rafael J. Wysocki <rafael.j.wysocki@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/acpi.h>
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/platform_data/clk-lpss.h>
+#include <linux/pm_runtime.h>
+
+#include "internal.h"
+
+ACPI_MODULE_NAME("acpi_lpss");
+
+#define LPSS_CLK_SIZE	0x04
+#define LPSS_LTR_SIZE	0x18
+
+/* Offsets relative to LPSS_PRIVATE_OFFSET */
+#define LPSS_GENERAL			0x08
+#define LPSS_GENERAL_LTR_MODE_SW	BIT(2)
+#define LPSS_SW_LTR			0x10
+#define LPSS_AUTO_LTR			0x14
+
+struct lpss_device_desc {
+	bool clk_required;
+	const char *clk_parent;
+	bool ltr_required;
+	unsigned int prv_offset;
+};
+
+struct lpss_private_data {
+	void __iomem *mmio_base;
+	resource_size_t mmio_size;
+	struct clk *clk;
+	const struct lpss_device_desc *dev_desc;
+};
+
+static struct lpss_device_desc lpt_dev_desc = {
+	.clk_required = true,
+	.clk_parent = "lpss_clk",
+	.prv_offset = 0x800,
+	.ltr_required = true,
+};
+
+static struct lpss_device_desc lpt_sdio_dev_desc = {
+	.prv_offset = 0x1000,
+	.ltr_required = true,
+};
+
+static const struct acpi_device_id acpi_lpss_device_ids[] = {
+	/* Lynxpoint LPSS devices */
+	{ "INT33C0", (unsigned long)&lpt_dev_desc },
+	{ "INT33C1", (unsigned long)&lpt_dev_desc },
+	{ "INT33C2", (unsigned long)&lpt_dev_desc },
+	{ "INT33C3", (unsigned long)&lpt_dev_desc },
+	{ "INT33C4", (unsigned long)&lpt_dev_desc },
+	{ "INT33C5", (unsigned long)&lpt_dev_desc },
+	{ "INT33C6", (unsigned long)&lpt_sdio_dev_desc },
+	{ "INT33C7", },
+
+	{ }
+};
+
+static int is_memory(struct acpi_resource *res, void *not_used)
+{
+	struct resource r;
+	return !acpi_dev_resource_memory(res, &r);
+}
+
+/* LPSS main clock device. */
+static struct platform_device *lpss_clk_dev;
+
+static inline void lpt_register_clock_device(void)
+{
+	lpss_clk_dev = platform_device_register_simple("clk-lpt", -1, NULL, 0);
+}
+
+static int register_device_clock(struct acpi_device *adev,
+				 struct lpss_private_data *pdata)
+{
+	const struct lpss_device_desc *dev_desc = pdata->dev_desc;
+
+	if (!lpss_clk_dev)
+		lpt_register_clock_device();
+
+	if (!dev_desc->clk_parent || !pdata->mmio_base
+	    || pdata->mmio_size < dev_desc->prv_offset + LPSS_CLK_SIZE)
+		return -ENODATA;
+
+	pdata->clk = clk_register_gate(NULL, dev_name(&adev->dev),
+				       dev_desc->clk_parent, 0,
+				       pdata->mmio_base + dev_desc->prv_offset,
+				       0, 0, NULL);
+	if (IS_ERR(pdata->clk))
+		return PTR_ERR(pdata->clk);
+
+	clk_register_clkdev(pdata->clk, NULL, dev_name(&adev->dev));
+	return 0;
+}
+
+static int acpi_lpss_create_device(struct acpi_device *adev,
+				   const struct acpi_device_id *id)
+{
+	struct lpss_device_desc *dev_desc;
+	struct lpss_private_data *pdata;
+	struct resource_list_entry *rentry;
+	struct list_head resource_list;
+	int ret;
+
+	dev_desc = (struct lpss_device_desc *)id->driver_data;
+	if (!dev_desc)
+		return acpi_create_platform_device(adev, id);
+
+	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&resource_list);
+	ret = acpi_dev_get_resources(adev, &resource_list, is_memory, NULL);
+	if (ret < 0)
+		goto err_out;
+
+	list_for_each_entry(rentry, &resource_list, node)
+		if (resource_type(&rentry->res) == IORESOURCE_MEM) {
+			pdata->mmio_size = resource_size(&rentry->res);
+			pdata->mmio_base = ioremap(rentry->res.start,
+						   pdata->mmio_size);
+			pdata->dev_desc = dev_desc;
+			break;
+		}
+
+	acpi_dev_free_resource_list(&resource_list);
+
+	if (dev_desc->clk_required) {
+		ret = register_device_clock(adev, pdata);
+		if (ret) {
+			/*
+			 * Skip the device, but don't terminate the namespace
+			 * scan.
+			 */
+			kfree(pdata);
+			return 0;
+		}
+	}
+
+	adev->driver_data = pdata;
+	ret = acpi_create_platform_device(adev, id);
+	if (ret > 0)
+		return ret;
+
+	adev->driver_data = NULL;
+
+ err_out:
+	kfree(pdata);
+	return ret;
+}
+
+static int lpss_reg_read(struct device *dev, unsigned int reg, u32 *val)
+{
+	struct acpi_device *adev;
+	struct lpss_private_data *pdata;
+	unsigned long flags;
+	int ret;
+
+	ret = acpi_bus_get_device(ACPI_HANDLE(dev), &adev);
+	if (WARN_ON(ret))
+		return ret;
+
+	spin_lock_irqsave(&dev->power.lock, flags);
+	if (pm_runtime_suspended(dev)) {
+		ret = -EAGAIN;
+		goto out;
+	}
+	pdata = acpi_driver_data(adev);
+	if (WARN_ON(!pdata || !pdata->mmio_base)) {
+		ret = -ENODEV;
+		goto out;
+	}
+	*val = readl(pdata->mmio_base + pdata->dev_desc->prv_offset + reg);
+
+ out:
+	spin_unlock_irqrestore(&dev->power.lock, flags);
+	return ret;
+}
+
+static ssize_t lpss_ltr_show(struct device *dev, struct device_attribute *attr,
+			     char *buf)
+{
+	u32 ltr_value = 0;
+	unsigned int reg;
+	int ret;
+
+	reg = strcmp(attr->attr.name, "auto_ltr") ? LPSS_SW_LTR : LPSS_AUTO_LTR;
+	ret = lpss_reg_read(dev, reg, &ltr_value);
+	if (ret)
+		return ret;
+
+	return snprintf(buf, PAGE_SIZE, "%08x\n", ltr_value);
+}
+
+static ssize_t lpss_ltr_mode_show(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	u32 ltr_mode = 0;
+	char *outstr;
+	int ret;
+
+	ret = lpss_reg_read(dev, LPSS_GENERAL, &ltr_mode);
+	if (ret)
+		return ret;
+
+	outstr = (ltr_mode & LPSS_GENERAL_LTR_MODE_SW) ? "sw" : "auto";
+	return sprintf(buf, "%s\n", outstr);
+}
+
+static DEVICE_ATTR(auto_ltr, S_IRUSR, lpss_ltr_show, NULL);
+static DEVICE_ATTR(sw_ltr, S_IRUSR, lpss_ltr_show, NULL);
+static DEVICE_ATTR(ltr_mode, S_IRUSR, lpss_ltr_mode_show, NULL);
+
+static struct attribute *lpss_attrs[] = {
+	&dev_attr_auto_ltr.attr,
+	&dev_attr_sw_ltr.attr,
+	&dev_attr_ltr_mode.attr,
+	NULL,
+};
+
+static struct attribute_group lpss_attr_group = {
+	.attrs = lpss_attrs,
+	.name = "lpss_ltr",
+};
+
+static int acpi_lpss_platform_notify(struct notifier_block *nb,
+				     unsigned long action, void *data)
+{
+	struct platform_device *pdev = to_platform_device(data);
+	struct lpss_private_data *pdata;
+	struct acpi_device *adev;
+	const struct acpi_device_id *id;
+	int ret = 0;
+
+	id = acpi_match_device(acpi_lpss_device_ids, &pdev->dev);
+	if (!id || !id->driver_data)
+		return 0;
+
+	if (acpi_bus_get_device(ACPI_HANDLE(&pdev->dev), &adev))
+		return 0;
+
+	pdata = acpi_driver_data(adev);
+	if (!pdata || !pdata->mmio_base || !pdata->dev_desc->ltr_required)
+		return 0;
+
+	if (pdata->mmio_size < pdata->dev_desc->prv_offset + LPSS_LTR_SIZE) {
+		dev_err(&pdev->dev, "MMIO size insufficient to access LTR\n");
+		return 0;
+	}
+
+	if (action == BUS_NOTIFY_ADD_DEVICE)
+		ret = sysfs_create_group(&pdev->dev.kobj, &lpss_attr_group);
+	else if (action == BUS_NOTIFY_DEL_DEVICE)
+		sysfs_remove_group(&pdev->dev.kobj, &lpss_attr_group);
+
+	return ret;
+}
+
+static struct notifier_block acpi_lpss_nb = {
+	.notifier_call = acpi_lpss_platform_notify,
+};
+
+static struct acpi_scan_handler lpss_handler = {
+	.ids = acpi_lpss_device_ids,
+	.attach = acpi_lpss_create_device,
+};
+
+void __init acpi_lpss_init(void)
+{
+	if (!lpt_clk_init()) {
+		bus_register_notifier(&platform_bus_type, &acpi_lpss_nb);
+		acpi_scan_add_handler(&lpss_handler);
+	}
+}
diff --git a/drivers/acpi/acpi_memhotplug.c b/drivers/acpi/acpi_memhotplug.c
index da1f82b445e0..5e6301e94920 100644
--- a/drivers/acpi/acpi_memhotplug.c
+++ b/drivers/acpi/acpi_memhotplug.c
@@ -1,5 +1,7 @@
 /*
- * Copyright (C) 2004 Intel Corporation <naveen.b.s@intel.com>
+ * Copyright (C) 2004, 2013 Intel Corporation
+ * Author: Naveen B S <naveen.b.s@intel.com>
+ * Author: Rafael J. Wysocki <rafael.j.wysocki@intel.com>
  *
  * All rights reserved.
  *
@@ -25,14 +27,10 @@
  * ranges.
  */
 
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/types.h>
-#include <linux/memory_hotplug.h>
-#include <linux/slab.h>
 #include <linux/acpi.h>
-#include <acpi/acpi_drivers.h>
+#include <linux/memory_hotplug.h>
+
+#include "internal.h"
 
 #define ACPI_MEMORY_DEVICE_CLASS		"memory"
 #define ACPI_MEMORY_DEVICE_HID			"PNP0C80"
@@ -44,32 +42,28 @@
 #define 	PREFIX		"ACPI:memory_hp:"
 
 ACPI_MODULE_NAME("acpi_memhotplug");
-MODULE_AUTHOR("Naveen B S <naveen.b.s@intel.com>");
-MODULE_DESCRIPTION("Hotplug Mem Driver");
-MODULE_LICENSE("GPL");
 
 /* Memory Device States */
 #define MEMORY_INVALID_STATE	0
 #define MEMORY_POWER_ON_STATE	1
 #define MEMORY_POWER_OFF_STATE	2
 
-static int acpi_memory_device_add(struct acpi_device *device);
-static int acpi_memory_device_remove(struct acpi_device *device);
+static int acpi_memory_device_add(struct acpi_device *device,
+				  const struct acpi_device_id *not_used);
+static void acpi_memory_device_remove(struct acpi_device *device);
 
 static const struct acpi_device_id memory_device_ids[] = {
 	{ACPI_MEMORY_DEVICE_HID, 0},
 	{"", 0},
 };
-MODULE_DEVICE_TABLE(acpi, memory_device_ids);
 
-static struct acpi_driver acpi_memory_device_driver = {
-	.name = "acpi_memhotplug",
-	.class = ACPI_MEMORY_DEVICE_CLASS,
+static struct acpi_scan_handler memory_device_handler = {
 	.ids = memory_device_ids,
-	.ops = {
-		.add = acpi_memory_device_add,
-		.remove = acpi_memory_device_remove,
-		},
+	.attach = acpi_memory_device_add,
+	.detach = acpi_memory_device_remove,
+	.hotplug = {
+		.enabled = true,
+	},
 };
 
 struct acpi_memory_info {
@@ -79,7 +73,6 @@ struct acpi_memory_info {
 	unsigned short caching;	/* memory cache attribute */
 	unsigned short write_protect;	/* memory read/write attribute */
 	unsigned int enabled:1;
-	unsigned int failed:1;
 };
 
 struct acpi_memory_device {
@@ -153,48 +146,6 @@ acpi_memory_get_device_resources(struct acpi_memory_device *mem_device)
 	return 0;
 }
 
-static int acpi_memory_get_device(acpi_handle handle,
-				  struct acpi_memory_device **mem_device)
-{
-	struct acpi_device *device = NULL;
-	int result = 0;
-
-	acpi_scan_lock_acquire();
-
-	acpi_bus_get_device(handle, &device);
-	if (device)
-		goto end;
-
-	/*
-	 * Now add the notified device.  This creates the acpi_device
-	 * and invokes .add function
-	 */
-	result = acpi_bus_scan(handle);
-	if (result) {
-		acpi_handle_warn(handle, "ACPI namespace scan failed\n");
-		result = -EINVAL;
-		goto out;
-	}
-	result = acpi_bus_get_device(handle, &device);
-	if (result) {
-		acpi_handle_warn(handle, "Missing device object\n");
-		result = -EINVAL;
-		goto out;
-	}
-
- end:
-	*mem_device = acpi_driver_data(device);
-	if (!(*mem_device)) {
-		dev_err(&device->dev, "driver data not found\n");
-		result = -ENODEV;
-		goto out;
-	}
-
- out:
-	acpi_scan_lock_release();
-	return result;
-}
-
 static int acpi_memory_check_device(struct acpi_memory_device *mem_device)
 {
 	unsigned long long current_status;
@@ -249,13 +200,11 @@ static int acpi_memory_enable_device(struct acpi_memory_device *mem_device)
 		 * returns -EEXIST. If add_memory() returns the other error, it
 		 * means that this memory block is not used by the kernel.
 		 */
-		if (result && result != -EEXIST) {
-			info->failed = 1;
+		if (result && result != -EEXIST)
 			continue;
-		}
 
-		if (!result)
-			info->enabled = 1;
+		info->enabled = 1;
+
 		/*
 		 * Add num_enable even if add_memory() returns -EEXIST, so the
 		 * device is bound to this driver.
@@ -286,16 +235,8 @@ static int acpi_memory_remove_memory(struct acpi_memory_device *mem_device)
 	nid = acpi_get_node(mem_device->device->handle);
 
 	list_for_each_entry_safe(info, n, &mem_device->res_list, list) {
-		if (info->failed)
-			/* The kernel does not use this memory block */
-			continue;
-
 		if (!info->enabled)
-			/*
-			 * The kernel uses this memory block, but it may be not
-			 * managed by us.
-			 */
-			return -EBUSY;
+			continue;
 
 		if (nid < 0)
 			nid = memory_add_physaddr_to_nid(info->start_addr);
@@ -310,95 +251,21 @@ static int acpi_memory_remove_memory(struct acpi_memory_device *mem_device)
 	return result;
 }
 
-static void acpi_memory_device_notify(acpi_handle handle, u32 event, void *data)
-{
-	struct acpi_memory_device *mem_device;
-	struct acpi_device *device;
-	struct acpi_eject_event *ej_event = NULL;
-	u32 ost_code = ACPI_OST_SC_NON_SPECIFIC_FAILURE; /* default */
-	acpi_status status;
-
-	switch (event) {
-	case ACPI_NOTIFY_BUS_CHECK:
-		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-				  "\nReceived BUS CHECK notification for device\n"));
-		/* Fall Through */
-	case ACPI_NOTIFY_DEVICE_CHECK:
-		if (event == ACPI_NOTIFY_DEVICE_CHECK)
-			ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-					  "\nReceived DEVICE CHECK notification for device\n"));
-		if (acpi_memory_get_device(handle, &mem_device)) {
-			acpi_handle_err(handle, "Cannot find driver data\n");
-			break;
-		}
-
-		ost_code = ACPI_OST_SC_SUCCESS;
-		break;
-
-	case ACPI_NOTIFY_EJECT_REQUEST:
-		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-				  "\nReceived EJECT REQUEST notification for device\n"));
-
-		status = AE_ERROR;
-		acpi_scan_lock_acquire();
-
-		if (acpi_bus_get_device(handle, &device)) {
-			acpi_handle_err(handle, "Device doesn't exist\n");
-			goto unlock;
-		}
-		mem_device = acpi_driver_data(device);
-		if (!mem_device) {
-			acpi_handle_err(handle, "Driver Data is NULL\n");
-			goto unlock;
-		}
-
-		ej_event = kmalloc(sizeof(*ej_event), GFP_KERNEL);
-		if (!ej_event) {
-			pr_err(PREFIX "No memory, dropping EJECT\n");
-			goto unlock;
-		}
-
-		get_device(&device->dev);
-		ej_event->device = device;
-		ej_event->event = ACPI_NOTIFY_EJECT_REQUEST;
-		/* The eject is carried out asynchronously. */
-		status = acpi_os_hotplug_execute(acpi_bus_hot_remove_device,
-						 ej_event);
-		if (ACPI_FAILURE(status)) {
-			put_device(&device->dev);
-			kfree(ej_event);
-		}
-
- unlock:
-		acpi_scan_lock_release();
-		if (ACPI_SUCCESS(status))
-			return;
-	default:
-		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-				  "Unsupported event [0x%x]\n", event));
-
-		/* non-hotplug event; possibly handled by other handler */
-		return;
-	}
-
-	/* Inform firmware that the hotplug operation has completed */
-	(void) acpi_evaluate_hotplug_ost(handle, event, ost_code, NULL);
-}
-
 static void acpi_memory_device_free(struct acpi_memory_device *mem_device)
 {
 	if (!mem_device)
 		return;
 
 	acpi_memory_free_device_resources(mem_device);
+	mem_device->device->driver_data = NULL;
 	kfree(mem_device);
 }
 
-static int acpi_memory_device_add(struct acpi_device *device)
+static int acpi_memory_device_add(struct acpi_device *device,
+				  const struct acpi_device_id *not_used)
 {
+	struct acpi_memory_device *mem_device;
 	int result;
-	struct acpi_memory_device *mem_device = NULL;
-
 
 	if (!device)
 		return -EINVAL;
@@ -423,147 +290,36 @@ static int acpi_memory_device_add(struct acpi_device *device)
 	/* Set the device state */
 	mem_device->state = MEMORY_POWER_ON_STATE;
 
-	pr_debug("%s\n", acpi_device_name(device));
+	result = acpi_memory_check_device(mem_device);
+	if (result) {
+		acpi_memory_device_free(mem_device);
+		return 0;
+	}
 
-	if (!acpi_memory_check_device(mem_device)) {
-		/* call add_memory func */
-		result = acpi_memory_enable_device(mem_device);
-		if (result) {
-			dev_err(&device->dev,
-				"Error in acpi_memory_enable_device\n");
-			acpi_memory_device_free(mem_device);
-		}
+	result = acpi_memory_enable_device(mem_device);
+	if (result) {
+		dev_err(&device->dev, "acpi_memory_enable_device() error\n");
+		acpi_memory_device_free(mem_device);
+		return -ENODEV;
 	}
-	return result;
+
+	dev_dbg(&device->dev, "Memory device configured by ACPI\n");
+	return 1;
 }
 
-static int acpi_memory_device_remove(struct acpi_device *device)
+static void acpi_memory_device_remove(struct acpi_device *device)
 {
-	struct acpi_memory_device *mem_device = NULL;
-	int result;
+	struct acpi_memory_device *mem_device;
 
 	if (!device || !acpi_driver_data(device))
-		return -EINVAL;
+		return;
 
 	mem_device = acpi_driver_data(device);
-
-	result = acpi_memory_remove_memory(mem_device);
-	if (result)
-		return result;
-
+	acpi_memory_remove_memory(mem_device);
 	acpi_memory_device_free(mem_device);
-
-	return 0;
-}
-
-/*
- * Helper function to check for memory device
- */
-static acpi_status is_memory_device(acpi_handle handle)
-{
-	char *hardware_id;
-	acpi_status status;
-	struct acpi_device_info *info;
-
-	status = acpi_get_object_info(handle, &info);
-	if (ACPI_FAILURE(status))
-		return status;
-
-	if (!(info->valid & ACPI_VALID_HID)) {
-		kfree(info);
-		return AE_ERROR;
-	}
-
-	hardware_id = info->hardware_id.string;
-	if ((hardware_id == NULL) ||
-	    (strcmp(hardware_id, ACPI_MEMORY_DEVICE_HID)))
-		status = AE_ERROR;
-
-	kfree(info);
-	return status;
-}
-
-static acpi_status
-acpi_memory_register_notify_handler(acpi_handle handle,
-				    u32 level, void *ctxt, void **retv)
-{
-	acpi_status status;
-
-
-	status = is_memory_device(handle);
-	if (ACPI_FAILURE(status))
-		return AE_OK;	/* continue */
-
-	status = acpi_install_notify_handler(handle, ACPI_SYSTEM_NOTIFY,
-					     acpi_memory_device_notify, NULL);
-	/* continue */
-	return AE_OK;
-}
-
-static acpi_status
-acpi_memory_deregister_notify_handler(acpi_handle handle,
-				      u32 level, void *ctxt, void **retv)
-{
-	acpi_status status;
-
-
-	status = is_memory_device(handle);
-	if (ACPI_FAILURE(status))
-		return AE_OK;	/* continue */
-
-	status = acpi_remove_notify_handler(handle,
-					    ACPI_SYSTEM_NOTIFY,
-					    acpi_memory_device_notify);
-
-	return AE_OK;	/* continue */
-}
-
-static int __init acpi_memory_device_init(void)
-{
-	int result;
-	acpi_status status;
-
-
-	result = acpi_bus_register_driver(&acpi_memory_device_driver);
-
-	if (result < 0)
-		return -ENODEV;
-
-	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, ACPI_ROOT_OBJECT,
-				     ACPI_UINT32_MAX,
-				     acpi_memory_register_notify_handler, NULL,
-				     NULL, NULL);
-
-	if (ACPI_FAILURE(status)) {
-		ACPI_EXCEPTION((AE_INFO, status, "walk_namespace failed"));
-		acpi_bus_unregister_driver(&acpi_memory_device_driver);
-		return -ENODEV;
-	}
-
-	return 0;
 }
 
-static void __exit acpi_memory_device_exit(void)
+void __init acpi_memory_hotplug_init(void)
 {
-	acpi_status status;
-
-
-	/*
-	 * Adding this to un-install notification handlers for all the device
-	 * handles.
-	 */
-	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, ACPI_ROOT_OBJECT,
-				     ACPI_UINT32_MAX,
-				     acpi_memory_deregister_notify_handler, NULL,
-				     NULL, NULL);
-
-	if (ACPI_FAILURE(status))
-		ACPI_EXCEPTION((AE_INFO, status, "walk_namespace failed"));
-
-	acpi_bus_unregister_driver(&acpi_memory_device_driver);
-
-	return;
+	acpi_scan_add_handler_with_hotplug(&memory_device_handler, "memory");
 }
-
-module_init(acpi_memory_device_init);
-module_exit(acpi_memory_device_exit);
diff --git a/drivers/acpi/acpi_pad.c b/drivers/acpi/acpi_pad.c
index 31de1043eea0..27bb6a91de5f 100644
--- a/drivers/acpi/acpi_pad.c
+++ b/drivers/acpi/acpi_pad.c
@@ -236,7 +236,7 @@ static int create_power_saving_task(void)
 	ps_tsks[ps_tsk_num] = kthread_run(power_saving_thread,
 		(void *)(unsigned long)ps_tsk_num,
 		"acpi_pad/%d", ps_tsk_num);
-	rc = IS_ERR(ps_tsks[ps_tsk_num]) ? PTR_ERR(ps_tsks[ps_tsk_num]) : 0;
+	rc = PTR_RET(ps_tsks[ps_tsk_num]);
 	if (!rc)
 		ps_tsk_num++;
 	else
diff --git a/drivers/acpi/acpi_platform.c b/drivers/acpi/acpi_platform.c
index 26fce4b8a632..fafec5ddf17f 100644
--- a/drivers/acpi/acpi_platform.c
+++ b/drivers/acpi/acpi_platform.c
@@ -22,9 +22,6 @@
 
 ACPI_MODULE_NAME("platform");
 
-/* Flags for acpi_create_platform_device */
-#define ACPI_PLATFORM_CLK	BIT(0)
-
 /*
  * The following ACPI IDs are known to be suitable for representing as
  * platform devices.
@@ -33,33 +30,9 @@ static const struct acpi_device_id acpi_platform_device_ids[] = {
 
 	{ "PNP0D40" },
 
-	/* Haswell LPSS devices */
-	{ "INT33C0", ACPI_PLATFORM_CLK },
-	{ "INT33C1", ACPI_PLATFORM_CLK },
-	{ "INT33C2", ACPI_PLATFORM_CLK },
-	{ "INT33C3", ACPI_PLATFORM_CLK },
-	{ "INT33C4", ACPI_PLATFORM_CLK },
-	{ "INT33C5", ACPI_PLATFORM_CLK },
-	{ "INT33C6", ACPI_PLATFORM_CLK },
-	{ "INT33C7", ACPI_PLATFORM_CLK },
-
 	{ }
 };
 
-static int acpi_create_platform_clks(struct acpi_device *adev)
-{
-	static struct platform_device *pdev;
-
-	/* Create Lynxpoint LPSS clocks */
-	if (!pdev && !strncmp(acpi_device_hid(adev), "INT33C", 6)) {
-		pdev = platform_device_register_simple("clk-lpt", -1, NULL, 0);
-		if (IS_ERR(pdev))
-			return PTR_ERR(pdev);
-	}
-
-	return 0;
-}
-
 /**
  * acpi_create_platform_device - Create platform device for ACPI device node
  * @adev: ACPI device node to create a platform device for.
@@ -71,10 +44,9 @@ static int acpi_create_platform_clks(struct acpi_device *adev)
  *
  * Name of the platform device will be the same as @adev's.
  */
-static int acpi_create_platform_device(struct acpi_device *adev,
-				       const struct acpi_device_id *id)
+int acpi_create_platform_device(struct acpi_device *adev,
+				const struct acpi_device_id *id)
 {
-	unsigned long flags = id->driver_data;
 	struct platform_device *pdev = NULL;
 	struct acpi_device *acpi_parent;
 	struct platform_device_info pdevinfo;
@@ -83,14 +55,6 @@ static int acpi_create_platform_device(struct acpi_device *adev,
 	struct resource *resources;
 	int count;
 
-	if (flags & ACPI_PLATFORM_CLK) {
-		int ret = acpi_create_platform_clks(adev);
-		if (ret) {
-			dev_err(&adev->dev, "failed to create clocks\n");
-			return ret;
-		}
-	}
-
 	/* If the ACPI node already has a physical device attached, skip it. */
 	if (adev->physical_node_count)
 		return 0;
diff --git a/drivers/acpi/acpica/Makefile b/drivers/acpi/acpica/Makefile
index a1b9bf5085a2..7ddf29eca9f5 100644
--- a/drivers/acpi/acpica/Makefile
+++ b/drivers/acpi/acpica/Makefile
@@ -83,6 +83,7 @@ acpi-$(ACPI_FUTURE_USAGE) += hwtimer.o
 acpi-y +=		\
 	nsaccess.o	\
 	nsalloc.o	\
+	nsconvert.o	\
 	nsdump.o	\
 	nseval.o	\
 	nsinit.o	\
@@ -160,6 +161,7 @@ acpi-y +=		\
 	utobject.o	\
 	utosi.o		\
 	utownerid.o	\
+	utpredef.o	\
 	utresrc.o	\
 	utstate.o	\
 	utstring.o	\
diff --git a/drivers/acpi/acpica/acglobal.h b/drivers/acpi/acpica/acglobal.h
index ecb49927b817..07160928ca25 100644
--- a/drivers/acpi/acpica/acglobal.h
+++ b/drivers/acpi/acpica/acglobal.h
@@ -224,6 +224,7 @@ ACPI_EXTERN u8 acpi_gbl_global_lock_pending;
  */
 ACPI_EXTERN acpi_spinlock acpi_gbl_gpe_lock;	/* For GPE data structs and registers */
 ACPI_EXTERN acpi_spinlock acpi_gbl_hardware_lock;	/* For ACPI H/W except GPE registers */
+ACPI_EXTERN acpi_spinlock acpi_gbl_reference_count_lock;
 
 /* Mutex for _OSI support */
 
@@ -413,10 +414,12 @@ ACPI_EXTERN u8 acpi_gbl_db_output_flags;
 
 #ifdef ACPI_DISASSEMBLER
 
-u8 ACPI_INIT_GLOBAL(acpi_gbl_ignore_noop_operator, FALSE);
+ACPI_EXTERN u8 ACPI_INIT_GLOBAL(acpi_gbl_ignore_noop_operator, FALSE);
 
 ACPI_EXTERN u8 acpi_gbl_db_opt_disasm;
 ACPI_EXTERN u8 acpi_gbl_db_opt_verbose;
+ACPI_EXTERN u8 acpi_gbl_num_external_methods;
+ACPI_EXTERN u32 acpi_gbl_resolved_external_methods;
 ACPI_EXTERN struct acpi_external_list *acpi_gbl_external_list;
 ACPI_EXTERN struct acpi_external_file *acpi_gbl_external_file_list;
 #endif
diff --git a/drivers/acpi/acpica/aclocal.h b/drivers/acpi/acpica/aclocal.h
index 805f419086ab..d5bfbd331bfd 100644
--- a/drivers/acpi/acpica/aclocal.h
+++ b/drivers/acpi/acpica/aclocal.h
@@ -294,6 +294,8 @@ acpi_status(*acpi_internal_method) (struct acpi_walk_state * walk_state);
 #define ACPI_BTYPE_OBJECTS_AND_REFS     0x0001FFFF	/* ARG or LOCAL */
 #define ACPI_BTYPE_ALL_OBJECTS          0x0000FFFF
 
+#pragma pack(1)
+
 /*
  * Information structure for ACPI predefined names.
  * Each entry in the table contains the following items:
@@ -304,7 +306,7 @@ acpi_status(*acpi_internal_method) (struct acpi_walk_state * walk_state);
  */
 struct acpi_name_info {
 	char name[ACPI_NAME_SIZE];
-	u8 param_count;
+	u16 argument_list;
 	u8 expected_btypes;
 };
 
@@ -327,7 +329,7 @@ struct acpi_package_info {
 	u8 count1;
 	u8 object_type2;
 	u8 count2;
-	u8 reserved;
+	u16 reserved;
 };
 
 /* Used for ACPI_PTYPE2_FIXED */
@@ -336,6 +338,7 @@ struct acpi_package_info2 {
 	u8 type;
 	u8 count;
 	u8 object_type[4];
+	u8 reserved;
 };
 
 /* Used for ACPI_PTYPE1_OPTION */
@@ -345,7 +348,7 @@ struct acpi_package_info3 {
 	u8 count;
 	u8 object_type[2];
 	u8 tail_object_type;
-	u8 reserved;
+	u16 reserved;
 };
 
 union acpi_predefined_info {
@@ -355,6 +358,10 @@ union acpi_predefined_info {
 	struct acpi_package_info3 ret_info3;
 };
 
+/* Reset to default packing */
+
+#pragma pack()
+
 /* Data block used during object validation */
 
 struct acpi_predefined_data {
@@ -363,6 +370,7 @@ struct acpi_predefined_data {
 	union acpi_operand_object *parent_package;
 	struct acpi_namespace_node *node;
 	u32 flags;
+	u32 return_btype;
 	u8 node_flags;
 };
 
@@ -371,6 +379,20 @@ struct acpi_predefined_data {
 #define ACPI_OBJECT_REPAIRED    1
 #define ACPI_OBJECT_WRAPPED     2
 
+/* Return object auto-repair info */
+
+typedef acpi_status(*acpi_object_converter) (union acpi_operand_object
+					     *original_object,
+					     union acpi_operand_object
+					     **converted_object);
+
+struct acpi_simple_repair_info {
+	char name[ACPI_NAME_SIZE];
+	u32 unexpected_btypes;
+	u32 package_index;
+	acpi_object_converter object_converter;
+};
+
 /*
  * Bitmapped return value types
  * Note: the actual data types must be contiguous, a loop in nspredef.c
@@ -1037,6 +1059,7 @@ struct acpi_external_list {
 	u16 length;
 	u8 type;
 	u8 flags;
+	u8 resolved;
 };
 
 /* Values for Flags field above */
diff --git a/drivers/acpi/acpica/acmacros.h b/drivers/acpi/acpica/acmacros.h
index ed7943b9044f..53666bd9193d 100644
--- a/drivers/acpi/acpica/acmacros.h
+++ b/drivers/acpi/acpica/acmacros.h
@@ -322,10 +322,12 @@
  * where a pointer to an object of type union acpi_operand_object can also
  * appear. This macro is used to distinguish them.
  *
- * The "Descriptor" field is the first field in both structures.
+ * The "DescriptorType" field is the second field in both structures.
  */
+#define ACPI_GET_DESCRIPTOR_PTR(d)      (((union acpi_descriptor *)(void *)(d))->common.common_pointer)
+#define ACPI_SET_DESCRIPTOR_PTR(d, p)   (((union acpi_descriptor *)(void *)(d))->common.common_pointer = (p))
 #define ACPI_GET_DESCRIPTOR_TYPE(d)     (((union acpi_descriptor *)(void *)(d))->common.descriptor_type)
-#define ACPI_SET_DESCRIPTOR_TYPE(d, t)  (((union acpi_descriptor *)(void *)(d))->common.descriptor_type = t)
+#define ACPI_SET_DESCRIPTOR_TYPE(d, t)  (((union acpi_descriptor *)(void *)(d))->common.descriptor_type = (t))
 
 /*
  * Macros for the master AML opcode table
diff --git a/drivers/acpi/acpica/acnamesp.h b/drivers/acpi/acpica/acnamesp.h
index 02cd5482ff8b..d2e491876bc0 100644
--- a/drivers/acpi/acpica/acnamesp.h
+++ b/drivers/acpi/acpica/acnamesp.h
@@ -167,6 +167,29 @@ void acpi_ns_delete_children(struct acpi_namespace_node *parent);
 int acpi_ns_compare_names(char *name1, char *name2);
 
 /*
+ * nsconvert - Dynamic object conversion routines
+ */
+acpi_status
+acpi_ns_convert_to_integer(union acpi_operand_object *original_object,
+			   union acpi_operand_object **return_object);
+
+acpi_status
+acpi_ns_convert_to_string(union acpi_operand_object *original_object,
+			  union acpi_operand_object **return_object);
+
+acpi_status
+acpi_ns_convert_to_buffer(union acpi_operand_object *original_object,
+			  union acpi_operand_object **return_object);
+
+acpi_status
+acpi_ns_convert_to_unicode(union acpi_operand_object *original_object,
+			   union acpi_operand_object **return_object);
+
+acpi_status
+acpi_ns_convert_to_resource(union acpi_operand_object *original_object,
+			    union acpi_operand_object **return_object);
+
+/*
  * nsdump - Namespace dump/print utilities
  */
 #ifdef	ACPI_FUTURE_USAGE
@@ -208,10 +231,6 @@ acpi_ns_check_predefined_names(struct acpi_namespace_node *node,
 			       acpi_status return_status,
 			       union acpi_operand_object **return_object);
 
-const union acpi_predefined_info *acpi_ns_check_for_predefined_name(struct
-								    acpi_namespace_node
-								    *node);
-
 void
 acpi_ns_check_parameter_count(char *pathname,
 			      struct acpi_namespace_node *node,
@@ -289,7 +308,7 @@ acpi_ns_get_attached_data(struct acpi_namespace_node *node,
  * predefined methods/objects
  */
 acpi_status
-acpi_ns_repair_object(struct acpi_predefined_data *data,
+acpi_ns_simple_repair(struct acpi_predefined_data *data,
 		      u32 expected_btypes,
 		      u32 package_index,
 		      union acpi_operand_object **return_object_ptr);
diff --git a/drivers/acpi/acpica/acpredef.h b/drivers/acpi/acpica/acpredef.h
index 752cc40cdc1e..b22b70944fd6 100644
--- a/drivers/acpi/acpica/acpredef.h
+++ b/drivers/acpi/acpica/acpredef.h
@@ -56,7 +56,7 @@
  *      object type
  *      count
  *
- * ACPI_PTYPE1_VAR: Variable-length length:
+ * ACPI_PTYPE1_VAR: Variable-length length. Zero-length package is allowed:
  *      object type (Int/Buf/Ref)
  *
  * ACPI_PTYPE1_OPTION: Package has some required and some optional elements
@@ -66,14 +66,16 @@
  * 2) PTYPE2 packages contain a Variable-length number of sub-packages. Each
  *    of the different types describe the contents of each of the sub-packages.
  *
- * ACPI_PTYPE2: Each subpackage contains 1 or 2 object types:
+ * ACPI_PTYPE2: Each subpackage contains 1 or 2 object types. Zero-length
+ *      parent package is allowed:
  *      object type
  *      count
  *      object type
  *      count
  *      (Used for _ALR,_MLS,_PSS,_TRT,_TSS)
  *
- * ACPI_PTYPE2_COUNT: Each subpackage has a count as first element:
+ * ACPI_PTYPE2_COUNT: Each subpackage has a count as first element.
+ *      Zero-length parent package is allowed:
  *      object type
  *      (Used for _CSD,_PSD,_TSD)
  *
@@ -84,17 +86,19 @@
  *      count
  *      (Used for _CST)
  *
- * ACPI_PTYPE2_FIXED: Each subpackage is of Fixed-length
+ * ACPI_PTYPE2_FIXED: Each subpackage is of Fixed-length. Zero-length
+ *      parent package is allowed.
  *      (Used for _PRT)
  *
- * ACPI_PTYPE2_MIN: Each subpackage has a Variable-length but minimum length
+ * ACPI_PTYPE2_MIN: Each subpackage has a Variable-length but minimum length.
+ *      Zero-length parent package is allowed:
  *      (Used for _HPX)
  *
  * ACPI_PTYPE2_REV_FIXED: Revision at start, each subpackage is Fixed-length
  *      (Used for _ART, _FPS)
  *
  * ACPI_PTYPE2_FIX_VAR: Each subpackage consists of some fixed-length elements
- *      followed by an optional element
+ *      followed by an optional element. Zero-length parent package is allowed.
  *      object type
  *      count
  *      object type
@@ -116,8 +120,47 @@ enum acpi_return_package_types {
 	ACPI_PTYPE2_FIX_VAR = 10
 };
 
+/* Support macros for users of the predefined info table */
+
+#define METHOD_PREDEF_ARGS_MAX          4
+#define METHOD_ARG_BIT_WIDTH            3
+#define METHOD_ARG_MASK                 0x0007
+#define ARG_COUNT_IS_MINIMUM            0x8000
+#define METHOD_MAX_ARG_TYPE             ACPI_TYPE_PACKAGE
+
+#define METHOD_GET_COUNT(arg_list)      (arg_list & METHOD_ARG_MASK)
+#define METHOD_GET_NEXT_ARG(arg_list)   (arg_list >> METHOD_ARG_BIT_WIDTH)
+
+/* Macros used to build the predefined info table */
+
+#define METHOD_0ARGS                    0
+#define METHOD_1ARGS(a1)                (1 | (a1 << 3))
+#define METHOD_2ARGS(a1,a2)             (2 | (a1 << 3) | (a2 << 6))
+#define METHOD_3ARGS(a1,a2,a3)          (3 | (a1 << 3) | (a2 << 6) | (a3 << 9))
+#define METHOD_4ARGS(a1,a2,a3,a4)       (4 | (a1 << 3) | (a2 << 6) | (a3 << 9) | (a4 << 12))
+
+#define METHOD_RETURNS(type)            (type)
+#define METHOD_NO_RETURN_VALUE          0
+
+#define PACKAGE_INFO(a,b,c,d,e,f)       {{{(a),(b),(c),(d)}, ((((u16)(f)) << 8) | (e)), 0}}
+
+/* Support macros for the resource descriptor info table */
+
+#define WIDTH_1                         0x0001
+#define WIDTH_2                         0x0002
+#define WIDTH_3                         0x0004
+#define WIDTH_8                         0x0008
+#define WIDTH_16                        0x0010
+#define WIDTH_32                        0x0020
+#define WIDTH_64                        0x0040
+#define VARIABLE_DATA                   0x0080
+#define NUM_RESOURCE_WIDTHS             8
+
+#define WIDTH_ADDRESS                   WIDTH_16 | WIDTH_32 | WIDTH_64
+
 #ifdef ACPI_CREATE_PREDEFINED_TABLE
-/*
+/******************************************************************************
+ *
  * Predefined method/object information table.
  *
  * These are the names that can actually be evaluated via acpi_evaluate_object.
@@ -125,23 +168,24 @@ enum acpi_return_package_types {
  *
  *      1) Predefined/Reserved names that are never evaluated via
  *         acpi_evaluate_object:
- *          _Lxx and _Exx GPE methods
- *          _Qxx EC methods
- *          _T_x compiler temporary variables
+ *              _Lxx and _Exx GPE methods
+ *              _Qxx EC methods
+ *              _T_x compiler temporary variables
+ *              _Wxx wake events
  *
  *      2) Predefined names that never actually exist within the AML code:
- *          Predefined resource descriptor field names
+ *              Predefined resource descriptor field names
  *
  *      3) Predefined names that are implemented within ACPICA:
- *          _OSI
- *
- *      4) Some predefined names that are not documented within the ACPI spec.
- *          _WDG, _WED
+ *              _OSI
  *
  * The main entries in the table each contain the following items:
  *
  * name                 - The ACPI reserved name
- * param_count          - Number of arguments to the method
+ * argument_list        - Contains (in 16 bits), the number of required
+ *                        arguments to the method (3 bits), and a 3-bit type
+ *                        field for each argument (up to 4 arguments). The
+ *                        METHOD_?ARGS macros generate the correct packed data.
  * expected_btypes      - Allowed type(s) for the return value.
  *                        0 means that no return value is expected.
  *
@@ -151,256 +195,511 @@ enum acpi_return_package_types {
  * overall size of the stored data.
  *
  * Note: The additional braces are intended to promote portability.
- */
-static const union acpi_predefined_info predefined_names[] = {
-	{{"_AC0", 0, ACPI_RTYPE_INTEGER}},
-	{{"_AC1", 0, ACPI_RTYPE_INTEGER}},
-	{{"_AC2", 0, ACPI_RTYPE_INTEGER}},
-	{{"_AC3", 0, ACPI_RTYPE_INTEGER}},
-	{{"_AC4", 0, ACPI_RTYPE_INTEGER}},
-	{{"_AC5", 0, ACPI_RTYPE_INTEGER}},
-	{{"_AC6", 0, ACPI_RTYPE_INTEGER}},
-	{{"_AC7", 0, ACPI_RTYPE_INTEGER}},
-	{{"_AC8", 0, ACPI_RTYPE_INTEGER}},
-	{{"_AC9", 0, ACPI_RTYPE_INTEGER}},
-	{{"_ADR", 0, ACPI_RTYPE_INTEGER}},
-	{{"_AEI", 0, ACPI_RTYPE_BUFFER}},
-	{{"_AL0", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_AL1", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_AL2", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_AL3", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_AL4", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_AL5", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_AL6", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_AL7", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_AL8", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_AL9", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_ALC", 0, ACPI_RTYPE_INTEGER}},
-	{{"_ALI", 0, ACPI_RTYPE_INTEGER}},
-	{{"_ALP", 0, ACPI_RTYPE_INTEGER}},
-	{{"_ALR", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Pkgs) each 2 (Ints) */
-			  {{{ACPI_PTYPE2, ACPI_RTYPE_INTEGER, 2,0}, 0,0}},
-
-	{{"_ALT", 0, ACPI_RTYPE_INTEGER}},
-	{{"_ART", 0, ACPI_RTYPE_PACKAGE}},	/* Variable-length (1 Int(rev), n Pkg (2 Ref/11 Int) */
-	{{{ACPI_PTYPE2_REV_FIXED, ACPI_RTYPE_REFERENCE, 2, ACPI_RTYPE_INTEGER},
-	  11, 0}},
-
-	{{"_BBN", 0, ACPI_RTYPE_INTEGER}},
-	{{"_BCL", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Ints) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 0,0}, 0,0}},
-
-	{{"_BCM", 1, 0}},
-	{{"_BCT", 1, ACPI_RTYPE_INTEGER}},
-	{{"_BDN", 0, ACPI_RTYPE_INTEGER}},
-	{{"_BFS", 1, 0}},
-	{{"_BIF", 0, ACPI_RTYPE_PACKAGE}},	/* Fixed-length (9 Int),(4 Str) */
-	{{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 9, ACPI_RTYPE_STRING}, 4, 0}},
-
-	{{"_BIX", 0, ACPI_RTYPE_PACKAGE}},	/* Fixed-length (16 Int),(4 Str) */
-	{{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 16, ACPI_RTYPE_STRING}, 4,
-	  0}},
-
-	{{"_BLT", 3, 0}},
-	{{"_BMA", 1, ACPI_RTYPE_INTEGER}},
-	{{"_BMC", 1, 0}},
-	{{"_BMD", 0, ACPI_RTYPE_PACKAGE}}, /* Fixed-length (5 Int) */
-			  {{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 5,0}, 0,0}},
-
-	{{"_BMS", 1, ACPI_RTYPE_INTEGER}},
-	{{"_BQC", 0, ACPI_RTYPE_INTEGER}},
-	{{"_BST", 0, ACPI_RTYPE_PACKAGE}}, /* Fixed-length (4 Int) */
-			  {{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 4,0}, 0,0}},
-
-	{{"_BTM", 1, ACPI_RTYPE_INTEGER}},
-	{{"_BTP", 1, 0}},
-	{{"_CBA", 0, ACPI_RTYPE_INTEGER}}, /* See PCI firmware spec 3.0 */
-	{{"_CDM", 0, ACPI_RTYPE_INTEGER}},
-	{{"_CID", 0, ACPI_RTYPE_INTEGER | ACPI_RTYPE_STRING | ACPI_RTYPE_PACKAGE}}, /* Variable-length (Ints/Strs) */
-	{{{ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER | ACPI_RTYPE_STRING, 0, 0}, 0,
-	  0}},
-
-	{{"_CLS", 0, ACPI_RTYPE_PACKAGE}},	/* Fixed-length (3 Int) */
-	{{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 3, 0}, 0, 0}},
-
-	{{"_CPC", 0, ACPI_RTYPE_PACKAGE}},	/* Variable-length (Ints/Bufs) */
-	{{{ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER | ACPI_RTYPE_BUFFER, 0, 0}, 0,
-	  0}},
-
-	{{"_CRS", 0, ACPI_RTYPE_BUFFER}},
-	{{"_CRT", 0, ACPI_RTYPE_INTEGER}},
-	{{"_CSD", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (1 Int(n), n-1 Int) */
-			  {{{ACPI_PTYPE2_COUNT, ACPI_RTYPE_INTEGER, 0,0}, 0,0}},
-
-	{{"_CST", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (1 Int(n), n Pkg (1 Buf/3 Int) */
-	{{{ACPI_PTYPE2_PKG_COUNT, ACPI_RTYPE_BUFFER, 1, ACPI_RTYPE_INTEGER}, 3,
-	  0}},
-
-	{{"_CWS", 1, ACPI_RTYPE_INTEGER}},
-	{{"_DCK", 1, ACPI_RTYPE_INTEGER}},
-	{{"_DCS", 0, ACPI_RTYPE_INTEGER}},
-	{{"_DDC", 1, ACPI_RTYPE_INTEGER | ACPI_RTYPE_BUFFER}},
-	{{"_DDN", 0, ACPI_RTYPE_STRING}},
-	{{"_DEP", 0, ACPI_RTYPE_PACKAGE}},	/* Variable-length (Refs) */
-	{{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0}, 0, 0}},
-
-	{{"_DGS", 0, ACPI_RTYPE_INTEGER}},
-	{{"_DIS", 0, 0}},
-
-	{{"_DLM", 0, ACPI_RTYPE_PACKAGE}},	/* Variable-length (Pkgs) each (1 Ref, 0/1 Optional Buf/Ref) */
-	{{{ACPI_PTYPE2_FIX_VAR, ACPI_RTYPE_REFERENCE, 1,
-	   ACPI_RTYPE_REFERENCE | ACPI_RTYPE_BUFFER}, 0, 0}},
-
-	{{"_DMA", 0, ACPI_RTYPE_BUFFER}},
-	{{"_DOD", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Ints) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 0,0}, 0,0}},
-
-	{{"_DOS", 1, 0}},
-	{{"_DSM", 4, ACPI_RTYPE_ALL}},     /* Must return a type, but it can be of any type */
-	{{"_DSS", 1, 0}},
-	{{"_DSW", 3, 0}},
-	{{"_DTI", 1, 0}},
-	{{"_EC_", 0, ACPI_RTYPE_INTEGER}},
-	{{"_EDL", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs)*/
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_EJ0", 1, 0}},
-	{{"_EJ1", 1, 0}},
-	{{"_EJ2", 1, 0}},
-	{{"_EJ3", 1, 0}},
-	{{"_EJ4", 1, 0}},
-	{{"_EJD", 0, ACPI_RTYPE_STRING}},
-	{{"_EVT", 1, 0}},
-	{{"_FDE", 0, ACPI_RTYPE_BUFFER}},
-	{{"_FDI", 0, ACPI_RTYPE_PACKAGE}}, /* Fixed-length (16 Int) */
-			  {{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 16,0}, 0,0}},
-
-	{{"_FDM", 1, 0}},
-	{{"_FIF", 0, ACPI_RTYPE_PACKAGE}},	/* Fixed-length (4 Int) */
-			  {{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 4, 0}, 0, 0}},
-
-	{{"_FIX", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Ints) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 0,0}, 0,0}},
-
-	{{"_FPS", 0, ACPI_RTYPE_PACKAGE}},	/* Variable-length (1 Int(rev), n Pkg (5 Int) */
-	{{{ACPI_PTYPE2_REV_FIXED, ACPI_RTYPE_INTEGER, 5, 0}, 0, 0}},
-
-	{{"_FSL", 1, 0}},
-	{{"_FST", 0, ACPI_RTYPE_PACKAGE}},	/* Fixed-length (3 Int) */
-	{{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 3, 0}, 0, 0}},
-
-	{{"_GAI", 0, ACPI_RTYPE_INTEGER}},
-	{{"_GCP", 0, ACPI_RTYPE_INTEGER}},
-	{{"_GHL", 0, ACPI_RTYPE_INTEGER}},
-	{{"_GLK", 0, ACPI_RTYPE_INTEGER}},
-	{{"_GPD", 0, ACPI_RTYPE_INTEGER}},
-	{{"_GPE", 0, ACPI_RTYPE_INTEGER}}, /* _GPE method, not _GPE scope */
-	{{"_GRT", 0, ACPI_RTYPE_BUFFER}},
-	{{"_GSB", 0, ACPI_RTYPE_INTEGER}},
-	{{"_GTF", 0, ACPI_RTYPE_BUFFER}},
-	{{"_GTM", 0, ACPI_RTYPE_BUFFER}},
-	{{"_GTS", 1, 0}},
-	{{"_GWS", 1, ACPI_RTYPE_INTEGER}},
-	{{"_HID", 0, ACPI_RTYPE_INTEGER | ACPI_RTYPE_STRING}},
-	{{"_HOT", 0, ACPI_RTYPE_INTEGER}},
-	{{"_HPP", 0, ACPI_RTYPE_PACKAGE}}, /* Fixed-length (4 Int) */
-			  {{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 4,0}, 0,0}},
+ *
+ * Note2: Table is used by the kernel-resident subsystem, the iASL compiler,
+ * and the acpi_help utility.
+ *
+ * TBD: _PRT - currently ignore reversed entries. Attempt to fix in nsrepair.
+ * Possibly fixing package elements like _BIF, etc.
+ *
+ *****************************************************************************/
+
+const union acpi_predefined_info acpi_gbl_predefined_methods[] = {
+	{{"_AC0", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_AC1", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_AC2", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_AC3", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_AC4", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_AC5", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_AC6", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_AC7", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_AC8", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_AC9", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_ADR", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_AEI", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
+
+	{{"_AL0", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_AL1", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_AL2", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_AL3", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_AL4", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_AL5", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_AL6", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_AL7", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_AL8", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_AL9", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_ALC", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_ALI", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_ALP", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_ALR", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Pkgs) each 2 (Ints) */
+	PACKAGE_INFO(ACPI_PTYPE2, ACPI_RTYPE_INTEGER, 2, 0, 0, 0),
+
+	{{"_ALT", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_ART", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (1 Int(rev), n Pkg (2 Ref/11 Int) */
+	PACKAGE_INFO(ACPI_PTYPE2_REV_FIXED, ACPI_RTYPE_REFERENCE, 2,
+		     ACPI_RTYPE_INTEGER, 11, 0),
+
+	{{"_BBN", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_BCL", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Ints) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 0, 0, 0, 0),
+
+	{{"_BCM", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_BCT", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_BDN", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_BFS", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_BIF", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (9 Int),(4 Str) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 9,
+		     ACPI_RTYPE_STRING, 4, 0),
+
+	{{"_BIX", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (16 Int),(4 Str) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 16,
+		     ACPI_RTYPE_STRING, 4, 0),
+
+	{{"_BLT",
+	  METHOD_3ARGS(ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_BMA", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_BMC", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_BMD", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (5 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 5, 0, 0, 0),
+
+	{{"_BMS", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_BQC", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_BST", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (4 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 4, 0, 0, 0),
+
+	{{"_BTM", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_BTP", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_CBA", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},	/* See PCI firmware spec 3.0 */
+
+	{{"_CDM", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_CID", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER | ACPI_RTYPE_STRING | ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Ints/Strs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER | ACPI_RTYPE_STRING, 0,
+		     0, 0, 0),
+
+	{{"_CLS", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (3 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 3, 0, 0, 0),
+
+	{{"_CPC", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Ints/Bufs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER | ACPI_RTYPE_BUFFER, 0,
+		     0, 0, 0),
+
+	{{"_CRS", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
+
+	{{"_CRT", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_CSD", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (1 Int(n), n-1 Int) */
+	PACKAGE_INFO(ACPI_PTYPE2_COUNT, ACPI_RTYPE_INTEGER, 0, 0, 0, 0),
+
+	{{"_CST", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (1 Int(n), n Pkg (1 Buf/3 Int) */
+	PACKAGE_INFO(ACPI_PTYPE2_PKG_COUNT, ACPI_RTYPE_BUFFER, 1,
+		     ACPI_RTYPE_INTEGER, 3, 0),
+
+	{{"_CWS", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_DCK", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_DCS", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_DDC", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER | ACPI_RTYPE_BUFFER)}},
+
+	{{"_DDN", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_STRING)}},
+
+	{{"_DEP", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_DGS", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_DIS", METHOD_0ARGS,
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_DLM", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Pkgs) each (1 Ref, 0/1 Optional Buf/Ref) */
+	PACKAGE_INFO(ACPI_PTYPE2_FIX_VAR, ACPI_RTYPE_REFERENCE, 1,
+		     ACPI_RTYPE_REFERENCE | ACPI_RTYPE_BUFFER, 0, 0),
+
+	{{"_DMA", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
+
+	{{"_DOD", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Ints) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 0, 0, 0, 0),
+
+	{{"_DOS", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_DSM",
+	  METHOD_4ARGS(ACPI_TYPE_BUFFER, ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER,
+		       ACPI_TYPE_PACKAGE),
+	  METHOD_RETURNS(ACPI_RTYPE_ALL)}},	/* Must return a value, but it can be of any type */
+
+	{{"_DSS", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_DSW",
+	  METHOD_3ARGS(ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_DTI", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_EC_", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_EDL", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_EJ0", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_EJ1", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_EJ2", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_EJ3", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_EJ4", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_EJD", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_STRING)}},
+
+	{{"_ERR",
+	  METHOD_3ARGS(ACPI_TYPE_INTEGER, ACPI_TYPE_STRING, ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},	/* Internal use only, used by ACPICA test suites */
+
+	{{"_EVT", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_FDE", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
+
+	{{"_FDI", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (16 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 16, 0, 0, 0),
+
+	{{"_FDM", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_FIF", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (4 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 4, 0, 0, 0),
+
+	{{"_FIX", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Ints) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 0, 0, 0, 0),
+
+	{{"_FPS", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (1 Int(rev), n Pkg (5 Int) */
+	PACKAGE_INFO(ACPI_PTYPE2_REV_FIXED, ACPI_RTYPE_INTEGER, 5, 0, 0, 0),
+
+	{{"_FSL", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_FST", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (3 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 3, 0, 0, 0),
+
+	{{"_GAI", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_GCP", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_GHL", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_GLK", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_GPD", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_GPE", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},	/* _GPE method, not _GPE scope */
+
+	{{"_GRT", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
+
+	{{"_GSB", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_GTF", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
+
+	{{"_GTM", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
+
+	{{"_GTS", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_GWS", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_HID", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER | ACPI_RTYPE_STRING)}},
+
+	{{"_HOT", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_HPP", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (4 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 4, 0, 0, 0),
 
 	/*
-	 * For _HPX, a single package is returned, containing a Variable-length number
+	 * For _HPX, a single package is returned, containing a variable-length number
 	 * of sub-packages. Each sub-package contains a PCI record setting.
 	 * There are several different type of record settings, of different
 	 * lengths, but all elements of all settings are Integers.
 	 */
-	{{"_HPX", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Pkgs) each (var Ints) */
-			  {{{ACPI_PTYPE2_MIN, ACPI_RTYPE_INTEGER, 5,0}, 0,0}},
-
-	{{"_HRV", 0, ACPI_RTYPE_INTEGER}},
-	{{"_IFT", 0, ACPI_RTYPE_INTEGER}}, /* See IPMI spec */
-	{{"_INI", 0, 0}},
-	{{"_IRC", 0, 0}},
-	{{"_LCK", 1, 0}},
-	{{"_LID", 0, ACPI_RTYPE_INTEGER}},
-	{{"_MAT", 0, ACPI_RTYPE_BUFFER}},
-	{{"_MBM", 0, ACPI_RTYPE_PACKAGE}},	/* Fixed-length (8 Int) */
-	{{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 8, 0}, 0, 0}},
-
-	{{"_MLS", 0, ACPI_RTYPE_PACKAGE}},	/* Variable-length (Pkgs) each (1 Str/1 Buf) */
-	{{{ACPI_PTYPE2, ACPI_RTYPE_STRING, 1, ACPI_RTYPE_BUFFER}, 1, 0}},
-
-	{{"_MSG", 1, 0}},
-	{{"_MSM", 4, ACPI_RTYPE_INTEGER}},
-	{{"_NTT", 0, ACPI_RTYPE_INTEGER}},
-	{{"_OFF", 0, 0}},
-	{{"_ON_", 0, 0}},
-	{{"_OS_", 0, ACPI_RTYPE_STRING}},
-	{{"_OSC", 4, ACPI_RTYPE_BUFFER}},
-	{{"_OST", 3, 0}},
-	{{"_PAI", 1, ACPI_RTYPE_INTEGER}},
-	{{"_PCL", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_PCT", 0, ACPI_RTYPE_PACKAGE}}, /* Fixed-length (2 Buf) */
-			  {{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_BUFFER, 2,0}, 0,0}},
-
-	{{"_PDC", 1, 0}},
-	{{"_PDL", 0, ACPI_RTYPE_INTEGER}},
-	{{"_PIC", 1, 0}},
-	{{"_PIF", 0, ACPI_RTYPE_PACKAGE}},	/* Fixed-length (3 Int),(3 Str) */
-	{{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 3, ACPI_RTYPE_STRING}, 3, 0}},
-
-	{{"_PLD", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Bufs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_BUFFER, 0,0}, 0,0}},
-
-	{{"_PMC", 0, ACPI_RTYPE_PACKAGE}},	/* Fixed-length (11 Int),(3 Str) */
-	{{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 11, ACPI_RTYPE_STRING}, 3,
-	  0}},
-
-	{{"_PMD", 0, ACPI_RTYPE_PACKAGE}},	/* Variable-length (Refs) */
-	{{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0}, 0, 0}},
-
-	{{"_PMM", 0, ACPI_RTYPE_INTEGER}},
-	{{"_PPC", 0, ACPI_RTYPE_INTEGER}},
-	{{"_PPE", 0, ACPI_RTYPE_INTEGER}}, /* See dig64 spec */
-	{{"_PR0", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_PR1", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_PR2", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_PR3", 0, ACPI_RTYPE_PACKAGE}},	/* Variable-length (Refs) */
-	{{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0}, 0, 0}},
-
-	{{"_PRE", 0, ACPI_RTYPE_PACKAGE}},	/* Variable-length (Refs) */
-	{{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0}, 0, 0}},
-
-	{{"_PRL", 0, ACPI_RTYPE_PACKAGE}},	/* Variable-length (Refs) */
-	{{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0}, 0, 0}},
-
-	{{"_PRS", 0, ACPI_RTYPE_BUFFER}},
+	{{"_HPX", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Pkgs) each (var Ints) */
+	PACKAGE_INFO(ACPI_PTYPE2_MIN, ACPI_RTYPE_INTEGER, 5, 0, 0, 0),
+
+	{{"_HRV", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_IFT", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},	/* See IPMI spec */
+
+	{{"_INI", METHOD_0ARGS,
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_IRC", METHOD_0ARGS,
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_LCK", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_LID", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_MAT", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
+
+	{{"_MBM", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (8 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 8, 0, 0, 0),
+
+	{{"_MLS", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Pkgs) each (1 Str/1 Buf) */
+	PACKAGE_INFO(ACPI_PTYPE2, ACPI_RTYPE_STRING, 1, ACPI_RTYPE_BUFFER, 1,
+		     0),
+
+	{{"_MSG", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_MSM",
+	  METHOD_4ARGS(ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER,
+		       ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_NTT", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_OFF", METHOD_0ARGS,
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_ON_", METHOD_0ARGS,
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_OS_", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_STRING)}},
+
+	{{"_OSC",
+	  METHOD_4ARGS(ACPI_TYPE_BUFFER, ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER,
+		       ACPI_TYPE_BUFFER),
+	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
+
+	{{"_OST",
+	  METHOD_3ARGS(ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER, ACPI_TYPE_BUFFER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_PAI", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_PCL", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_PCT", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (2 Buf) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_BUFFER, 2, 0, 0, 0),
+
+	{{"_PDC", METHOD_1ARGS(ACPI_TYPE_BUFFER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_PDL", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_PIC", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_PIF", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (3 Int),(3 Str) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 3,
+		     ACPI_RTYPE_STRING, 3, 0),
+
+	{{"_PLD", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Bufs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_BUFFER, 0, 0, 0, 0),
+
+	{{"_PMC", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (11 Int),(3 Str) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 11,
+		     ACPI_RTYPE_STRING, 3, 0),
+
+	{{"_PMD", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_PMM", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_PPC", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_PPE", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},	/* See dig64 spec */
+
+	{{"_PR0", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_PR1", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_PR2", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_PR3", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_PRE", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_PRL", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_PRS", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
 
 	/*
 	 * For _PRT, many BIOSs reverse the 3rd and 4th Package elements (Source
@@ -410,47 +709,89 @@ static const union acpi_predefined_info predefined_names[] = {
 	 * warning, add the ACPI_RTYPE_REFERENCE type to the 4th element (index 3)
 	 * in the statement below.
 	 */
-	{{"_PRT", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Pkgs) each (4): Int,Int,Int/Ref,Int */
-			  {{{ACPI_PTYPE2_FIXED, 4, ACPI_RTYPE_INTEGER,ACPI_RTYPE_INTEGER},
-			  ACPI_RTYPE_INTEGER | ACPI_RTYPE_REFERENCE,
-			  ACPI_RTYPE_INTEGER}},
-
-	{{"_PRW", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Pkgs) each: Pkg/Int,Int,[Variable-length Refs] (Pkg is Ref/Int) */
-			  {{{ACPI_PTYPE1_OPTION, 2, ACPI_RTYPE_INTEGER | ACPI_RTYPE_PACKAGE,
-			  ACPI_RTYPE_INTEGER}, ACPI_RTYPE_REFERENCE,0}},
-
-	{{"_PS0", 0, 0}},
-	{{"_PS1", 0, 0}},
-	{{"_PS2", 0, 0}},
-	{{"_PS3", 0, 0}},
-	{{"_PSC", 0, ACPI_RTYPE_INTEGER}},
-	{{"_PSD", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Pkgs) each (5 Int) with count */
-			  {{{ACPI_PTYPE2_COUNT, ACPI_RTYPE_INTEGER,0,0}, 0,0}},
-
-	{{"_PSE", 1, 0}},
-	{{"_PSL", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_PSR", 0, ACPI_RTYPE_INTEGER}},
-	{{"_PSS", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Pkgs) each (6 Int) */
-			  {{{ACPI_PTYPE2, ACPI_RTYPE_INTEGER, 6,0}, 0,0}},
-
-	{{"_PSV", 0, ACPI_RTYPE_INTEGER}},
-	{{"_PSW", 1, 0}},
-	{{"_PTC", 0, ACPI_RTYPE_PACKAGE}}, /* Fixed-length (2 Buf) */
-			  {{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_BUFFER, 2,0}, 0,0}},
-
-	{{"_PTP", 2, ACPI_RTYPE_INTEGER}},
-	{{"_PTS", 1, 0}},
-	{{"_PUR", 0, ACPI_RTYPE_PACKAGE}},	/* Fixed-length (2 Int) */
-	{{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 2, 0}, 0, 0}},
-
-	{{"_PXM", 0, ACPI_RTYPE_INTEGER}},
-	{{"_REG", 2, 0}},
-	{{"_REV", 0, ACPI_RTYPE_INTEGER}},
-	{{"_RMV", 0, ACPI_RTYPE_INTEGER}},
-	{{"_ROM", 2, ACPI_RTYPE_BUFFER}},
-	{{"_RTV", 0, ACPI_RTYPE_INTEGER}},
+	{{"_PRT", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Pkgs) each (4): Int,Int,Int/Ref,Int */
+	PACKAGE_INFO(ACPI_PTYPE2_FIXED, 4, ACPI_RTYPE_INTEGER,
+		     ACPI_RTYPE_INTEGER,
+		     ACPI_RTYPE_INTEGER | ACPI_RTYPE_REFERENCE,
+		     ACPI_RTYPE_INTEGER),
+
+	{{"_PRW", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Pkgs) each: Pkg/Int,Int,[Variable-length Refs] (Pkg is Ref/Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_OPTION, 2,
+		     ACPI_RTYPE_INTEGER | ACPI_RTYPE_PACKAGE,
+		     ACPI_RTYPE_INTEGER, ACPI_RTYPE_REFERENCE, 0),
+
+	{{"_PS0", METHOD_0ARGS,
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_PS1", METHOD_0ARGS,
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_PS2", METHOD_0ARGS,
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_PS3", METHOD_0ARGS,
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_PSC", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_PSD", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Pkgs) each (5 Int) with count */
+	PACKAGE_INFO(ACPI_PTYPE2_COUNT, ACPI_RTYPE_INTEGER, 0, 0, 0, 0),
+
+	{{"_PSE", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_PSL", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_PSR", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_PSS", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Pkgs) each (6 Int) */
+	PACKAGE_INFO(ACPI_PTYPE2, ACPI_RTYPE_INTEGER, 6, 0, 0, 0),
+
+	{{"_PSV", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_PSW", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_PTC", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (2 Buf) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_BUFFER, 2, 0, 0, 0),
+
+	{{"_PTP", METHOD_2ARGS(ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_PTS", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_PUR", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (2 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 2, 0, 0, 0),
+
+	{{"_PXM", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_REG", METHOD_2ARGS(ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_REV", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_RMV", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_ROM", METHOD_2ARGS(ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
+
+	{{"_RTV", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
 
 	/*
 	 * For _S0_ through _S5_, the ACPI spec defines a return Package
@@ -458,111 +799,285 @@ static const union acpi_predefined_info predefined_names[] = {
 	 * Allow this by making the objects "Variable-length length", but all elements
 	 * must be Integers.
 	 */
-	{{"_S0_", 0, ACPI_RTYPE_PACKAGE}}, /* Fixed-length (1 Int) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 1,0}, 0,0}},
-
-	{{"_S1_", 0, ACPI_RTYPE_PACKAGE}}, /* Fixed-length (1 Int) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 1,0}, 0,0}},
-
-	{{"_S2_", 0, ACPI_RTYPE_PACKAGE}}, /* Fixed-length (1 Int) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 1,0}, 0,0}},
-
-	{{"_S3_", 0, ACPI_RTYPE_PACKAGE}}, /* Fixed-length (1 Int) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 1,0}, 0,0}},
-
-	{{"_S4_", 0, ACPI_RTYPE_PACKAGE}}, /* Fixed-length (1 Int) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 1,0}, 0,0}},
-
-	{{"_S5_", 0, ACPI_RTYPE_PACKAGE}}, /* Fixed-length (1 Int) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 1,0}, 0,0}},
-
-	{{"_S1D", 0, ACPI_RTYPE_INTEGER}},
-	{{"_S2D", 0, ACPI_RTYPE_INTEGER}},
-	{{"_S3D", 0, ACPI_RTYPE_INTEGER}},
-	{{"_S4D", 0, ACPI_RTYPE_INTEGER}},
-	{{"_S0W", 0, ACPI_RTYPE_INTEGER}},
-	{{"_S1W", 0, ACPI_RTYPE_INTEGER}},
-	{{"_S2W", 0, ACPI_RTYPE_INTEGER}},
-	{{"_S3W", 0, ACPI_RTYPE_INTEGER}},
-	{{"_S4W", 0, ACPI_RTYPE_INTEGER}},
-	{{"_SBS", 0, ACPI_RTYPE_INTEGER}},
-	{{"_SCP", 0x13, 0}},               /* Acpi 1.0 allowed 1 arg. Acpi 3.0 expanded to 3 args. Allow both. */
-			   /* Note: the 3-arg definition may be removed for ACPI 4.0 */
-	{{"_SDD", 1, 0}},
-	{{"_SEG", 0, ACPI_RTYPE_INTEGER}},
-	{{"_SHL", 1, ACPI_RTYPE_INTEGER}},
-	{{"_SLI", 0, ACPI_RTYPE_BUFFER}},
-	{{"_SPD", 1, ACPI_RTYPE_INTEGER}},
-	{{"_SRS", 1, 0}},
-	{{"_SRT", 1, ACPI_RTYPE_INTEGER}},
-	{{"_SRV", 0, ACPI_RTYPE_INTEGER}}, /* See IPMI spec */
-	{{"_SST", 1, 0}},
-	{{"_STA", 0, ACPI_RTYPE_INTEGER}},
-	{{"_STM", 3, 0}},
-	{{"_STP", 2, ACPI_RTYPE_INTEGER}},
-	{{"_STR", 0, ACPI_RTYPE_BUFFER}},
-	{{"_STV", 2, ACPI_RTYPE_INTEGER}},
-	{{"_SUB", 0, ACPI_RTYPE_STRING}},
-	{{"_SUN", 0, ACPI_RTYPE_INTEGER}},
-	{{"_SWS", 0, ACPI_RTYPE_INTEGER}},
-	{{"_TC1", 0, ACPI_RTYPE_INTEGER}},
-	{{"_TC2", 0, ACPI_RTYPE_INTEGER}},
-	{{"_TDL", 0, ACPI_RTYPE_INTEGER}},
-	{{"_TIP", 1, ACPI_RTYPE_INTEGER}},
-	{{"_TIV", 1, ACPI_RTYPE_INTEGER}},
-	{{"_TMP", 0, ACPI_RTYPE_INTEGER}},
-	{{"_TPC", 0, ACPI_RTYPE_INTEGER}},
-	{{"_TPT", 1, 0}},
-	{{"_TRT", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Pkgs) each 2 Ref/6 Int */
-			  {{{ACPI_PTYPE2, ACPI_RTYPE_REFERENCE, 2, ACPI_RTYPE_INTEGER}, 6, 0}},
-
-	{{"_TSD", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Pkgs) each 5 Int with count */
-			  {{{ACPI_PTYPE2_COUNT,ACPI_RTYPE_INTEGER, 5,0}, 0,0}},
-
-	{{"_TSP", 0, ACPI_RTYPE_INTEGER}},
-	{{"_TSS", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Pkgs) each 5 Int */
-			  {{{ACPI_PTYPE2, ACPI_RTYPE_INTEGER, 5,0}, 0,0}},
-
-	{{"_TST", 0, ACPI_RTYPE_INTEGER}},
-	{{"_TTS", 1, 0}},
-	{{"_TZD", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Refs) */
-			  {{{ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0,0}, 0,0}},
-
-	{{"_TZM", 0, ACPI_RTYPE_REFERENCE}},
-	{{"_TZP", 0, ACPI_RTYPE_INTEGER}},
-	{{"_UID", 0, ACPI_RTYPE_INTEGER | ACPI_RTYPE_STRING}},
-	{{"_UPC", 0, ACPI_RTYPE_PACKAGE}}, /* Fixed-length (4 Int) */
-			  {{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 4,0}, 0,0}},
-
-	{{"_UPD", 0, ACPI_RTYPE_INTEGER}},
-	{{"_UPP", 0, ACPI_RTYPE_INTEGER}},
-	{{"_VPO", 0, ACPI_RTYPE_INTEGER}},
+	{{"_S0_", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (1 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 1, 0, 0, 0),
+
+	{{"_S1_", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (1 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 1, 0, 0, 0),
+
+	{{"_S2_", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (1 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 1, 0, 0, 0),
+
+	{{"_S3_", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (1 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 1, 0, 0, 0),
+
+	{{"_S4_", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (1 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 1, 0, 0, 0),
+
+	{{"_S5_", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (1 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER, 1, 0, 0, 0),
+
+	{{"_S1D", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_S2D", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_S3D", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_S4D", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_S0W", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_S1W", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_S2W", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_S3W", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_S4W", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_SBS", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_SCP", METHOD_1ARGS(ACPI_TYPE_INTEGER) | ARG_COUNT_IS_MINIMUM,
+	  METHOD_NO_RETURN_VALUE}},	/* Acpi 1.0 allowed 1 integer arg. Acpi 3.0 expanded to 3 args. Allow both. */
+
+	{{"_SDD", METHOD_1ARGS(ACPI_TYPE_BUFFER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_SEG", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_SHL", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_SLI", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
+
+	{{"_SPD", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_SRS", METHOD_1ARGS(ACPI_TYPE_BUFFER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_SRT", METHOD_1ARGS(ACPI_TYPE_BUFFER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_SRV", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},	/* See IPMI spec */
+
+	{{"_SST", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_STA", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_STM",
+	  METHOD_3ARGS(ACPI_TYPE_BUFFER, ACPI_TYPE_BUFFER, ACPI_TYPE_BUFFER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_STP", METHOD_2ARGS(ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_STR", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
+
+	{{"_STV", METHOD_2ARGS(ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_SUB", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_STRING)}},
+
+	{{"_SUN", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_SWS", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_TC1", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_TC2", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_TDL", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_TIP", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_TIV", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_TMP", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_TPC", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_TPT", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_TRT", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Pkgs) each 2 Ref/6 Int */
+	PACKAGE_INFO(ACPI_PTYPE2, ACPI_RTYPE_REFERENCE, 2, ACPI_RTYPE_INTEGER,
+		     6, 0),
+
+	{{"_TSD", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Pkgs) each 5 Int with count */
+	PACKAGE_INFO(ACPI_PTYPE2_COUNT, ACPI_RTYPE_INTEGER, 5, 0, 0, 0),
+
+	{{"_TSP", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_TSS", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Pkgs) each 5 Int */
+	PACKAGE_INFO(ACPI_PTYPE2, ACPI_RTYPE_INTEGER, 5, 0, 0, 0),
+
+	{{"_TST", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_TTS", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_NO_RETURN_VALUE}},
+
+	{{"_TZD", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
+	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
+
+	{{"_TZM", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_REFERENCE)}},
+
+	{{"_TZP", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_UID", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER | ACPI_RTYPE_STRING)}},
+
+	{{"_UPC", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (4 Int) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 4, 0, 0, 0),
+
+	{{"_UPD", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_UPP", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
+	{{"_VPO", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
 
 	/* Acpi 1.0 defined _WAK with no return value. Later, it was changed to return a package */
 
-	{{"_WAK", 1,
-          ACPI_RTYPE_NONE | ACPI_RTYPE_INTEGER | ACPI_RTYPE_PACKAGE}},
-			  {{{ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 2,0}, 0,0}}, /* Fixed-length (2 Int), but is optional */
+	{{"_WAK", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_NONE | ACPI_RTYPE_INTEGER |
+			 ACPI_RTYPE_PACKAGE)}},
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 2, 0, 0, 0),	/* Fixed-length (2 Int), but is optional */
 
 	/* _WDG/_WED are MS extensions defined by "Windows Instrumentation" */
 
-	{{"_WDG", 0, ACPI_RTYPE_BUFFER}},
-	{{"_WED", 1,
-	  ACPI_RTYPE_INTEGER | ACPI_RTYPE_STRING | ACPI_RTYPE_BUFFER}},
+	{{"_WDG", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
+
+	{{"_WED", METHOD_1ARGS(ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER | ACPI_RTYPE_STRING |
+			 ACPI_RTYPE_BUFFER)}},
 
-	{{{0, 0, 0, 0}, 0, 0}}  /* Table terminator */
+	PACKAGE_INFO(0, 0, 0, 0, 0, 0)	/* Table terminator */
 };
+#else
+extern const union acpi_predefined_info acpi_gbl_predefined_methods[];
+#endif
 
-#if 0
+#if (defined ACPI_CREATE_RESOURCE_TABLE && defined ACPI_APPLICATION)
+/******************************************************************************
+ *
+ * Predefined names for use in Resource Descriptors. These names do not
+ * appear in the global Predefined Name table (since these names never
+ * appear in actual AML byte code, only in the original ASL)
+ *
+ * Note: Used by iASL compiler and acpi_help utility only.
+ *
+ *****************************************************************************/
 
-	/* This is an internally implemented control method, no need to check */
-{ {
-"_OSI", 1, ACPI_RTYPE_INTEGER}},
+const union acpi_predefined_info acpi_gbl_resource_names[] = {
+	{{"_ADR", WIDTH_16 | WIDTH_64, 0}},
+	{{"_ALN", WIDTH_8 | WIDTH_16 | WIDTH_32, 0}},
+	{{"_ASI", WIDTH_8, 0}},
+	{{"_ASZ", WIDTH_8, 0}},
+	{{"_ATT", WIDTH_64, 0}},
+	{{"_BAS", WIDTH_16 | WIDTH_32, 0}},
+	{{"_BM_", WIDTH_1, 0}},
+	{{"_DBT", WIDTH_16, 0}},	/* Acpi 5.0 */
+	{{"_DEC", WIDTH_1, 0}},
+	{{"_DMA", WIDTH_8, 0}},
+	{{"_DPL", WIDTH_1, 0}},	/* Acpi 5.0 */
+	{{"_DRS", WIDTH_16, 0}},	/* Acpi 5.0 */
+	{{"_END", WIDTH_1, 0}},	/* Acpi 5.0 */
+	{{"_FLC", WIDTH_2, 0}},	/* Acpi 5.0 */
+	{{"_GRA", WIDTH_ADDRESS, 0}},
+	{{"_HE_", WIDTH_1, 0}},
+	{{"_INT", WIDTH_16 | WIDTH_32, 0}},
+	{{"_IOR", WIDTH_2, 0}},	/* Acpi 5.0 */
+	{{"_LEN", WIDTH_8 | WIDTH_ADDRESS, 0}},
+	{{"_LIN", WIDTH_8, 0}},	/* Acpi 5.0 */
+	{{"_LL_", WIDTH_1, 0}},
+	{{"_MAF", WIDTH_1, 0}},
+	{{"_MAX", WIDTH_ADDRESS, 0}},
+	{{"_MEM", WIDTH_2, 0}},
+	{{"_MIF", WIDTH_1, 0}},
+	{{"_MIN", WIDTH_ADDRESS, 0}},
+	{{"_MOD", WIDTH_1, 0}},	/* Acpi 5.0 */
+	{{"_MTP", WIDTH_2, 0}},
+	{{"_PAR", WIDTH_8, 0}},	/* Acpi 5.0 */
+	{{"_PHA", WIDTH_1, 0}},	/* Acpi 5.0 */
+	{{"_PIN", WIDTH_16, 0}},	/* Acpi 5.0 */
+	{{"_PPI", WIDTH_8, 0}},	/* Acpi 5.0 */
+	{{"_POL", WIDTH_1 | WIDTH_2, 0}},	/* Acpi 5.0 */
+	{{"_RBO", WIDTH_8, 0}},
+	{{"_RBW", WIDTH_8, 0}},
+	{{"_RNG", WIDTH_1, 0}},
+	{{"_RT_", WIDTH_8, 0}},	/* Acpi 3.0 */
+	{{"_RW_", WIDTH_1, 0}},
+	{{"_RXL", WIDTH_16, 0}},	/* Acpi 5.0 */
+	{{"_SHR", WIDTH_2, 0}},
+	{{"_SIZ", WIDTH_2, 0}},
+	{{"_SLV", WIDTH_1, 0}},	/* Acpi 5.0 */
+	{{"_SPE", WIDTH_32, 0}},	/* Acpi 5.0 */
+	{{"_STB", WIDTH_2, 0}},	/* Acpi 5.0 */
+	{{"_TRA", WIDTH_ADDRESS, 0}},
+	{{"_TRS", WIDTH_1, 0}},
+	{{"_TSF", WIDTH_8, 0}},	/* Acpi 3.0 */
+	{{"_TTP", WIDTH_1, 0}},
+	{{"_TXL", WIDTH_16, 0}},	/* Acpi 5.0 */
+	{{"_TYP", WIDTH_2 | WIDTH_16, 0}},
+	{{"_VEN", VARIABLE_DATA, 0}},	/* Acpi 5.0 */
+	PACKAGE_INFO(0, 0, 0, 0, 0, 0)	/* Table terminator */
+};
 
-	/* TBD: */
-	_PRT - currently ignore reversed entries. attempt to fix here?
-	think about possibly fixing package elements like _BIF, etc.
+static const union acpi_predefined_info acpi_gbl_scope_names[] = {
+	{{"_GPE", 0, 0}},
+	{{"_PR_", 0, 0}},
+	{{"_SB_", 0, 0}},
+	{{"_SI_", 0, 0}},
+	{{"_TZ_", 0, 0}},
+	PACKAGE_INFO(0, 0, 0, 0, 0, 0)	/* Table terminator */
+};
+#else
+extern const union acpi_predefined_info acpi_gbl_resource_names[];
 #endif
 
 #endif
-#endif
diff --git a/drivers/acpi/acpica/acutils.h b/drivers/acpi/acpica/acutils.h
index 0082fa0a6139..202f4f12d3e2 100644
--- a/drivers/acpi/acpica/acutils.h
+++ b/drivers/acpi/acpica/acutils.h
@@ -113,9 +113,10 @@ struct acpi_pkg_info {
 	u32 num_packages;
 };
 
+/* Object reference counts */
+
 #define REF_INCREMENT       (u16) 0
 #define REF_DECREMENT       (u16) 1
-#define REF_FORCE_DELETE    (u16) 2
 
 /* acpi_ut_dump_buffer */
 
@@ -421,7 +422,7 @@ acpi_ut_get_object_size(union acpi_operand_object *obj, acpi_size * obj_length);
  */
 acpi_status acpi_ut_initialize_interfaces(void);
 
-void acpi_ut_interface_terminate(void);
+acpi_status acpi_ut_interface_terminate(void);
 
 acpi_status acpi_ut_install_interface(acpi_string interface_name);
 
@@ -432,6 +433,26 @@ struct acpi_interface_info *acpi_ut_get_interface(acpi_string interface_name);
 acpi_status acpi_ut_osi_implementation(struct acpi_walk_state *walk_state);
 
 /*
+ * utpredef - support for predefined names
+ */
+const union acpi_predefined_info *acpi_ut_get_next_predefined_method(const union
+								     acpi_predefined_info
+								     *this_name);
+
+const union acpi_predefined_info *acpi_ut_match_predefined_method(char *name);
+
+const union acpi_predefined_info *acpi_ut_match_resource_name(char *name);
+
+void
+acpi_ut_display_predefined_method(char *buffer,
+				  const union acpi_predefined_info *this_name,
+				  u8 multi_line);
+
+void acpi_ut_get_expected_return_types(char *buffer, u32 expected_btypes);
+
+u32 acpi_ut_get_resource_bit_width(char *buffer, u16 types);
+
+/*
  * utstate - Generic state creation/cache routines
  */
 void
@@ -483,7 +504,8 @@ acpi_ut_short_divide(u64 in_dividend,
 /*
  * utmisc
  */
-const char *acpi_ut_validate_exception(acpi_status status);
+const struct acpi_exception_info *acpi_ut_validate_exception(acpi_status
+							     status);
 
 u8 acpi_ut_is_pci_root_bridge(char *id);
 
diff --git a/drivers/acpi/acpica/dsutils.c b/drivers/acpi/acpica/dsutils.c
index 4d8c992a51d8..99778997c35a 100644
--- a/drivers/acpi/acpica/dsutils.c
+++ b/drivers/acpi/acpica/dsutils.c
@@ -178,7 +178,7 @@ acpi_ds_is_result_used(union acpi_parse_object * op,
 
 	if (!op) {
 		ACPI_ERROR((AE_INFO, "Null Op"));
-		return_VALUE(TRUE);
+		return_UINT8(TRUE);
 	}
 
 	/*
@@ -210,7 +210,7 @@ acpi_ds_is_result_used(union acpi_parse_object * op,
 				  "At Method level, result of [%s] not used\n",
 				  acpi_ps_get_opcode_name(op->common.
 							  aml_opcode)));
-		return_VALUE(FALSE);
+		return_UINT8(FALSE);
 	}
 
 	/* Get info on the parent. The root_op is AML_SCOPE */
@@ -219,7 +219,7 @@ acpi_ds_is_result_used(union acpi_parse_object * op,
 	    acpi_ps_get_opcode_info(op->common.parent->common.aml_opcode);
 	if (parent_info->class == AML_CLASS_UNKNOWN) {
 		ACPI_ERROR((AE_INFO, "Unknown parent opcode Op=%p", op));
-		return_VALUE(FALSE);
+		return_UINT8(FALSE);
 	}
 
 	/*
@@ -307,7 +307,7 @@ acpi_ds_is_result_used(union acpi_parse_object * op,
 			  acpi_ps_get_opcode_name(op->common.parent->common.
 						  aml_opcode), op));
 
-	return_VALUE(TRUE);
+	return_UINT8(TRUE);
 
       result_not_used:
 	ACPI_DEBUG_PRINT((ACPI_DB_DISPATCH,
@@ -316,7 +316,7 @@ acpi_ds_is_result_used(union acpi_parse_object * op,
 			  acpi_ps_get_opcode_name(op->common.parent->common.
 						  aml_opcode), op));
 
-	return_VALUE(FALSE);
+	return_UINT8(FALSE);
 }
 
 /*******************************************************************************
diff --git a/drivers/acpi/acpica/dswexec.c b/drivers/acpi/acpica/dswexec.c
index 44f8325c2bae..e2199a947470 100644
--- a/drivers/acpi/acpica/dswexec.c
+++ b/drivers/acpi/acpica/dswexec.c
@@ -693,7 +693,7 @@ acpi_status acpi_ds_exec_end_op(struct acpi_walk_state *walk_state)
 		default:
 
 			ACPI_ERROR((AE_INFO,
-				    "Unimplemented opcode, class=0x%X type=0x%X Opcode=-0x%X Op=%p",
+				    "Unimplemented opcode, class=0x%X type=0x%X Opcode=0x%X Op=%p",
 				    op_class, op_type, op->common.aml_opcode,
 				    op));
 
diff --git a/drivers/acpi/acpica/evevent.c b/drivers/acpi/acpica/evevent.c
index b8ea0b26cde3..83cd45f4a870 100644
--- a/drivers/acpi/acpica/evevent.c
+++ b/drivers/acpi/acpica/evevent.c
@@ -257,6 +257,8 @@ u32 acpi_ev_fixed_event_detect(void)
  *
  * DESCRIPTION: Clears the status bit for the requested event, calls the
  *              handler that previously registered for the event.
+ *              NOTE: If there is no handler for the event, the event is
+ *              disabled to prevent further interrupts.
  *
  ******************************************************************************/
 
@@ -271,17 +273,17 @@ static u32 acpi_ev_fixed_event_dispatch(u32 event)
 				      status_register_id, ACPI_CLEAR_STATUS);
 
 	/*
-	 * Make sure we've got a handler. If not, report an error. The event is
-	 * disabled to prevent further interrupts.
+	 * Make sure that a handler exists. If not, report an error
+	 * and disable the event to prevent further interrupts.
 	 */
-	if (NULL == acpi_gbl_fixed_event_handlers[event].handler) {
+	if (!acpi_gbl_fixed_event_handlers[event].handler) {
 		(void)acpi_write_bit_register(acpi_gbl_fixed_event_info[event].
 					      enable_register_id,
 					      ACPI_DISABLE_EVENT);
 
 		ACPI_ERROR((AE_INFO,
-			    "No installed handler for fixed event [0x%08X]",
-			    event));
+			    "No installed handler for fixed event - %s (%u), disabling",
+			    acpi_ut_get_event_name(event), event));
 
 		return (ACPI_INTERRUPT_NOT_HANDLED);
 	}
diff --git a/drivers/acpi/acpica/evgpe.c b/drivers/acpi/acpica/evgpe.c
index b9adb9a7ed85..a493b528f8f9 100644
--- a/drivers/acpi/acpica/evgpe.c
+++ b/drivers/acpi/acpica/evgpe.c
@@ -707,7 +707,7 @@ acpi_ev_gpe_dispatch(struct acpi_namespace_node *gpe_device,
 		if (ACPI_FAILURE(status)) {
 			ACPI_EXCEPTION((AE_INFO, status,
 					"Unable to clear GPE%02X", gpe_number));
-			return_VALUE(ACPI_INTERRUPT_NOT_HANDLED);
+			return_UINT32(ACPI_INTERRUPT_NOT_HANDLED);
 		}
 	}
 
@@ -724,7 +724,7 @@ acpi_ev_gpe_dispatch(struct acpi_namespace_node *gpe_device,
 	if (ACPI_FAILURE(status)) {
 		ACPI_EXCEPTION((AE_INFO, status,
 				"Unable to disable GPE%02X", gpe_number));
-		return_VALUE(ACPI_INTERRUPT_NOT_HANDLED);
+		return_UINT32(ACPI_INTERRUPT_NOT_HANDLED);
 	}
 
 	/*
@@ -784,7 +784,7 @@ acpi_ev_gpe_dispatch(struct acpi_namespace_node *gpe_device,
 		break;
 	}
 
-	return_VALUE(ACPI_INTERRUPT_HANDLED);
+	return_UINT32(ACPI_INTERRUPT_HANDLED);
 }
 
 #endif				/* !ACPI_REDUCED_HARDWARE */
diff --git a/drivers/acpi/acpica/evsci.c b/drivers/acpi/acpica/evsci.c
index f4b43bede015..b905acf7aacd 100644
--- a/drivers/acpi/acpica/evsci.c
+++ b/drivers/acpi/acpica/evsci.c
@@ -89,7 +89,7 @@ static u32 ACPI_SYSTEM_XFACE acpi_ev_sci_xrupt_handler(void *context)
 	 */
 	interrupt_handled |= acpi_ev_gpe_detect(gpe_xrupt_list);
 
-	return_VALUE(interrupt_handled);
+	return_UINT32(interrupt_handled);
 }
 
 /*******************************************************************************
@@ -120,7 +120,7 @@ u32 ACPI_SYSTEM_XFACE acpi_ev_gpe_xrupt_handler(void *context)
 
 	interrupt_handled |= acpi_ev_gpe_detect(gpe_xrupt_list);
 
-	return_VALUE(interrupt_handled);
+	return_UINT32(interrupt_handled);
 }
 
 /******************************************************************************
diff --git a/drivers/acpi/acpica/evxface.c b/drivers/acpi/acpica/evxface.c
index ddffd6847914..ca5fba99c33b 100644
--- a/drivers/acpi/acpica/evxface.c
+++ b/drivers/acpi/acpica/evxface.c
@@ -467,9 +467,9 @@ acpi_install_fixed_event_handler(u32 event,
 		return_ACPI_STATUS(status);
 	}
 
-	/* Don't allow two handlers. */
+	/* Do not allow multiple handlers */
 
-	if (NULL != acpi_gbl_fixed_event_handlers[event].handler) {
+	if (acpi_gbl_fixed_event_handlers[event].handler) {
 		status = AE_ALREADY_EXISTS;
 		goto cleanup;
 	}
@@ -483,8 +483,9 @@ acpi_install_fixed_event_handler(u32 event,
 	if (ACPI_SUCCESS(status))
 		status = acpi_enable_event(event, 0);
 	if (ACPI_FAILURE(status)) {
-		ACPI_WARNING((AE_INFO, "Could not enable fixed event 0x%X",
-			      event));
+		ACPI_WARNING((AE_INFO,
+			      "Could not enable fixed event - %s (%u)",
+			      acpi_ut_get_event_name(event), event));
 
 		/* Remove the handler */
 
@@ -492,7 +493,8 @@ acpi_install_fixed_event_handler(u32 event,
 		acpi_gbl_fixed_event_handlers[event].context = NULL;
 	} else {
 		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-				  "Enabled fixed event %X, Handler=%p\n", event,
+				  "Enabled fixed event %s (%X), Handler=%p\n",
+				  acpi_ut_get_event_name(event), event,
 				  handler));
 	}
 
@@ -544,11 +546,12 @@ acpi_remove_fixed_event_handler(u32 event, acpi_event_handler handler)
 
 	if (ACPI_FAILURE(status)) {
 		ACPI_WARNING((AE_INFO,
-			      "Could not write to fixed event enable register 0x%X",
-			      event));
+			      "Could not disable fixed event - %s (%u)",
+			      acpi_ut_get_event_name(event), event));
 	} else {
-		ACPI_DEBUG_PRINT((ACPI_DB_INFO, "Disabled fixed event %X\n",
-				  event));
+		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
+				  "Disabled fixed event - %s (%X)\n",
+				  acpi_ut_get_event_name(event), event));
 	}
 
 	(void)acpi_ut_release_mutex(ACPI_MTX_EVENTS);
diff --git a/drivers/acpi/acpica/evxfevnt.c b/drivers/acpi/acpica/evxfevnt.c
index d6e4e42316db..7039606a0ba8 100644
--- a/drivers/acpi/acpica/evxfevnt.c
+++ b/drivers/acpi/acpica/evxfevnt.c
@@ -74,6 +74,12 @@ acpi_status acpi_enable(void)
 		return_ACPI_STATUS(AE_NO_ACPI_TABLES);
 	}
 
+	/* If the Hardware Reduced flag is set, machine is always in acpi mode */
+
+	if (acpi_gbl_reduced_hardware) {
+		return_ACPI_STATUS(AE_OK);
+	}
+
 	/* Check current mode */
 
 	if (acpi_hw_get_mode() == ACPI_SYS_MODE_ACPI) {
@@ -126,6 +132,12 @@ acpi_status acpi_disable(void)
 
 	ACPI_FUNCTION_TRACE(acpi_disable);
 
+	/* If the Hardware Reduced flag is set, machine is always in acpi mode */
+
+	if (acpi_gbl_reduced_hardware) {
+		return_ACPI_STATUS(AE_OK);
+	}
+
 	if (acpi_hw_get_mode() == ACPI_SYS_MODE_LEGACY) {
 		ACPI_DEBUG_PRINT((ACPI_DB_INIT,
 				  "System is already in legacy (non-ACPI) mode\n"));
diff --git a/drivers/acpi/acpica/exfldio.c b/drivers/acpi/acpica/exfldio.c
index ec7f5690031b..c84ee956fa4c 100644
--- a/drivers/acpi/acpica/exfldio.c
+++ b/drivers/acpi/acpica/exfldio.c
@@ -720,7 +720,19 @@ acpi_ex_extract_from_field(union acpi_operand_object *obj_desc,
 
 	if ((obj_desc->common_field.start_field_bit_offset == 0) &&
 	    (obj_desc->common_field.bit_length == access_bit_width)) {
-		status = acpi_ex_field_datum_io(obj_desc, 0, buffer, ACPI_READ);
+		if (buffer_length >= sizeof(u64)) {
+			status =
+			    acpi_ex_field_datum_io(obj_desc, 0, buffer,
+						   ACPI_READ);
+		} else {
+			/* Use raw_datum (u64) to handle buffers < 64 bits */
+
+			status =
+			    acpi_ex_field_datum_io(obj_desc, 0, &raw_datum,
+						   ACPI_READ);
+			ACPI_MEMCPY(buffer, &raw_datum, buffer_length);
+		}
+
 		return_ACPI_STATUS(status);
 	}
 
diff --git a/drivers/acpi/acpica/exoparg2.c b/drivers/acpi/acpica/exoparg2.c
index e491e46f17df..b0838a4ea53e 100644
--- a/drivers/acpi/acpica/exoparg2.c
+++ b/drivers/acpi/acpica/exoparg2.c
@@ -257,7 +257,7 @@ acpi_status acpi_ex_opcode_2A_1T_1R(struct acpi_walk_state *walk_state)
 	union acpi_operand_object *return_desc = NULL;
 	u64 index;
 	acpi_status status = AE_OK;
-	acpi_size length;
+	acpi_size length = 0;
 
 	ACPI_FUNCTION_TRACE_STR(ex_opcode_2A_1T_1R,
 				acpi_ps_get_opcode_name(walk_state->opcode));
@@ -320,7 +320,6 @@ acpi_status acpi_ex_opcode_2A_1T_1R(struct acpi_walk_state *walk_state)
 		 * NOTE: A length of zero is ok, and will create a zero-length, null
 		 *       terminated string.
 		 */
-		length = 0;
 		while ((length < operand[0]->buffer.length) &&
 		       (length < operand[1]->integer.value) &&
 		       (operand[0]->buffer.pointer[length])) {
@@ -376,6 +375,7 @@ acpi_status acpi_ex_opcode_2A_1T_1R(struct acpi_walk_state *walk_state)
 		case ACPI_TYPE_STRING:
 
 			if (index >= operand[0]->string.length) {
+				length = operand[0]->string.length;
 				status = AE_AML_STRING_LIMIT;
 			}
 
@@ -386,6 +386,7 @@ acpi_status acpi_ex_opcode_2A_1T_1R(struct acpi_walk_state *walk_state)
 		case ACPI_TYPE_BUFFER:
 
 			if (index >= operand[0]->buffer.length) {
+				length = operand[0]->buffer.length;
 				status = AE_AML_BUFFER_LIMIT;
 			}
 
@@ -396,6 +397,7 @@ acpi_status acpi_ex_opcode_2A_1T_1R(struct acpi_walk_state *walk_state)
 		case ACPI_TYPE_PACKAGE:
 
 			if (index >= operand[0]->package.count) {
+				length = operand[0]->package.count;
 				status = AE_AML_PACKAGE_LIMIT;
 			}
 
@@ -414,8 +416,9 @@ acpi_status acpi_ex_opcode_2A_1T_1R(struct acpi_walk_state *walk_state)
 
 		if (ACPI_FAILURE(status)) {
 			ACPI_EXCEPTION((AE_INFO, status,
-					"Index (0x%8.8X%8.8X) is beyond end of object",
-					ACPI_FORMAT_UINT64(index)));
+					"Index (0x%X%8.8X) is beyond end of object (length 0x%X)",
+					ACPI_FORMAT_UINT64(index),
+					(u32)length));
 			goto cleanup;
 		}
 
diff --git a/drivers/acpi/acpica/exprep.c b/drivers/acpi/acpica/exprep.c
index d6eab81f54fb..6b728aef2dca 100644
--- a/drivers/acpi/acpica/exprep.c
+++ b/drivers/acpi/acpica/exprep.c
@@ -276,7 +276,7 @@ acpi_ex_decode_field_access(union acpi_operand_object *obj_desc,
 		/* Invalid field access type */
 
 		ACPI_ERROR((AE_INFO, "Unknown field access type 0x%X", access));
-		return_VALUE(0);
+		return_UINT32(0);
 	}
 
 	if (obj_desc->common.type == ACPI_TYPE_BUFFER_FIELD) {
@@ -289,7 +289,7 @@ acpi_ex_decode_field_access(union acpi_operand_object *obj_desc,
 	}
 
 	*return_byte_alignment = byte_alignment;
-	return_VALUE(bit_length);
+	return_UINT32(bit_length);
 }
 
 /*******************************************************************************
diff --git a/drivers/acpi/acpica/exutils.c b/drivers/acpi/acpica/exutils.c
index b205cbb4b50c..99dc7b287d55 100644
--- a/drivers/acpi/acpica/exutils.c
+++ b/drivers/acpi/acpica/exutils.c
@@ -340,7 +340,7 @@ static u32 acpi_ex_digits_needed(u64 value, u32 base)
 	/* u64 is unsigned, so we don't worry about a '-' prefix */
 
 	if (value == 0) {
-		return_VALUE(1);
+		return_UINT32(1);
 	}
 
 	current_value = value;
@@ -354,7 +354,7 @@ static u32 acpi_ex_digits_needed(u64 value, u32 base)
 		num_digits++;
 	}
 
-	return_VALUE(num_digits);
+	return_UINT32(num_digits);
 }
 
 /*******************************************************************************
diff --git a/drivers/acpi/acpica/hwacpi.c b/drivers/acpi/acpica/hwacpi.c
index deb3f61e2bd1..579c3a53ac87 100644
--- a/drivers/acpi/acpica/hwacpi.c
+++ b/drivers/acpi/acpica/hwacpi.c
@@ -66,6 +66,12 @@ acpi_status acpi_hw_set_mode(u32 mode)
 
 	ACPI_FUNCTION_TRACE(hw_set_mode);
 
+	/* If the Hardware Reduced flag is set, machine is always in acpi mode */
+
+	if (acpi_gbl_reduced_hardware) {
+		return_ACPI_STATUS(AE_OK);
+	}
+
 	/*
 	 * ACPI 2.0 clarified that if SMI_CMD in FADT is zero,
 	 * system does not support mode transition.
@@ -146,23 +152,29 @@ u32 acpi_hw_get_mode(void)
 
 	ACPI_FUNCTION_TRACE(hw_get_mode);
 
+	/* If the Hardware Reduced flag is set, machine is always in acpi mode */
+
+	if (acpi_gbl_reduced_hardware) {
+		return_UINT32(ACPI_SYS_MODE_ACPI);
+	}
+
 	/*
 	 * ACPI 2.0 clarified that if SMI_CMD in FADT is zero,
 	 * system does not support mode transition.
 	 */
 	if (!acpi_gbl_FADT.smi_command) {
-		return_VALUE(ACPI_SYS_MODE_ACPI);
+		return_UINT32(ACPI_SYS_MODE_ACPI);
 	}
 
 	status = acpi_read_bit_register(ACPI_BITREG_SCI_ENABLE, &value);
 	if (ACPI_FAILURE(status)) {
-		return_VALUE(ACPI_SYS_MODE_LEGACY);
+		return_UINT32(ACPI_SYS_MODE_LEGACY);
 	}
 
 	if (value) {
-		return_VALUE(ACPI_SYS_MODE_ACPI);
+		return_UINT32(ACPI_SYS_MODE_ACPI);
 	} else {
-		return_VALUE(ACPI_SYS_MODE_LEGACY);
+		return_UINT32(ACPI_SYS_MODE_LEGACY);
 	}
 }
 
diff --git a/drivers/acpi/acpica/nsconvert.c b/drivers/acpi/acpica/nsconvert.c
new file mode 100644
index 000000000000..8f79a9d2d50e
--- /dev/null
+++ b/drivers/acpi/acpica/nsconvert.c
@@ -0,0 +1,443 @@
+/******************************************************************************
+ *
+ * Module Name: nsconvert - Object conversions for objects returned by
+ *                          predefined methods
+ *
+ *****************************************************************************/
+
+/*
+ * Copyright (C) 2000 - 2013, Intel Corp.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce at minimum a disclaimer
+ *    substantially similar to the "NO WARRANTY" disclaimer below
+ *    ("Disclaimer") and any redistribution must be conditioned upon
+ *    including a substantially similar Disclaimer requirement for further
+ *    binary redistribution.
+ * 3. Neither the names of the above-listed copyright holders nor the names
+ *    of any contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * NO WARRANTY
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDERS OR CONTRIBUTORS BE LIABLE FOR SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+ * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGES.
+ */
+
+#include <acpi/acpi.h>
+#include "accommon.h"
+#include "acnamesp.h"
+#include "acinterp.h"
+#include "acpredef.h"
+#include "amlresrc.h"
+
+#define _COMPONENT          ACPI_NAMESPACE
+ACPI_MODULE_NAME("nsconvert")
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ns_convert_to_integer
+ *
+ * PARAMETERS:  original_object     - Object to be converted
+ *              return_object       - Where the new converted object is returned
+ *
+ * RETURN:      Status. AE_OK if conversion was successful.
+ *
+ * DESCRIPTION: Attempt to convert a String/Buffer object to an Integer.
+ *
+ ******************************************************************************/
+acpi_status
+acpi_ns_convert_to_integer(union acpi_operand_object *original_object,
+			   union acpi_operand_object **return_object)
+{
+	union acpi_operand_object *new_object;
+	acpi_status status;
+	u64 value = 0;
+	u32 i;
+
+	switch (original_object->common.type) {
+	case ACPI_TYPE_STRING:
+
+		/* String-to-Integer conversion */
+
+		status = acpi_ut_strtoul64(original_object->string.pointer,
+					   ACPI_ANY_BASE, &value);
+		if (ACPI_FAILURE(status)) {
+			return (status);
+		}
+		break;
+
+	case ACPI_TYPE_BUFFER:
+
+		/* Buffer-to-Integer conversion. Max buffer size is 64 bits. */
+
+		if (original_object->buffer.length > 8) {
+			return (AE_AML_OPERAND_TYPE);
+		}
+
+		/* Extract each buffer byte to create the integer */
+
+		for (i = 0; i < original_object->buffer.length; i++) {
+			value |=
+			    ((u64)original_object->buffer.
+			     pointer[i] << (i * 8));
+		}
+		break;
+
+	default:
+		return (AE_AML_OPERAND_TYPE);
+	}
+
+	new_object = acpi_ut_create_integer_object(value);
+	if (!new_object) {
+		return (AE_NO_MEMORY);
+	}
+
+	*return_object = new_object;
+	return (AE_OK);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ns_convert_to_string
+ *
+ * PARAMETERS:  original_object     - Object to be converted
+ *              return_object       - Where the new converted object is returned
+ *
+ * RETURN:      Status. AE_OK if conversion was successful.
+ *
+ * DESCRIPTION: Attempt to convert a Integer/Buffer object to a String.
+ *
+ ******************************************************************************/
+
+acpi_status
+acpi_ns_convert_to_string(union acpi_operand_object *original_object,
+			  union acpi_operand_object **return_object)
+{
+	union acpi_operand_object *new_object;
+	acpi_size length;
+	acpi_status status;
+
+	switch (original_object->common.type) {
+	case ACPI_TYPE_INTEGER:
+		/*
+		 * Integer-to-String conversion. Commonly, convert
+		 * an integer of value 0 to a NULL string. The last element of
+		 * _BIF and _BIX packages occasionally need this fix.
+		 */
+		if (original_object->integer.value == 0) {
+
+			/* Allocate a new NULL string object */
+
+			new_object = acpi_ut_create_string_object(0);
+			if (!new_object) {
+				return (AE_NO_MEMORY);
+			}
+		} else {
+			status =
+			    acpi_ex_convert_to_string(original_object,
+						      &new_object,
+						      ACPI_IMPLICIT_CONVERT_HEX);
+			if (ACPI_FAILURE(status)) {
+				return (status);
+			}
+		}
+		break;
+
+	case ACPI_TYPE_BUFFER:
+		/*
+		 * Buffer-to-String conversion. Use a to_string
+		 * conversion, no transform performed on the buffer data. The best
+		 * example of this is the _BIF method, where the string data from
+		 * the battery is often (incorrectly) returned as buffer object(s).
+		 */
+		length = 0;
+		while ((length < original_object->buffer.length) &&
+		       (original_object->buffer.pointer[length])) {
+			length++;
+		}
+
+		/* Allocate a new string object */
+
+		new_object = acpi_ut_create_string_object(length);
+		if (!new_object) {
+			return (AE_NO_MEMORY);
+		}
+
+		/*
+		 * Copy the raw buffer data with no transform. String is already NULL
+		 * terminated at Length+1.
+		 */
+		ACPI_MEMCPY(new_object->string.pointer,
+			    original_object->buffer.pointer, length);
+		break;
+
+	default:
+		return (AE_AML_OPERAND_TYPE);
+	}
+
+	*return_object = new_object;
+	return (AE_OK);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ns_convert_to_buffer
+ *
+ * PARAMETERS:  original_object     - Object to be converted
+ *              return_object       - Where the new converted object is returned
+ *
+ * RETURN:      Status. AE_OK if conversion was successful.
+ *
+ * DESCRIPTION: Attempt to convert a Integer/String/Package object to a Buffer.
+ *
+ ******************************************************************************/
+
+acpi_status
+acpi_ns_convert_to_buffer(union acpi_operand_object *original_object,
+			  union acpi_operand_object **return_object)
+{
+	union acpi_operand_object *new_object;
+	acpi_status status;
+	union acpi_operand_object **elements;
+	u32 *dword_buffer;
+	u32 count;
+	u32 i;
+
+	switch (original_object->common.type) {
+	case ACPI_TYPE_INTEGER:
+		/*
+		 * Integer-to-Buffer conversion.
+		 * Convert the Integer to a packed-byte buffer. _MAT and other
+		 * objects need this sometimes, if a read has been performed on a
+		 * Field object that is less than or equal to the global integer
+		 * size (32 or 64 bits).
+		 */
+		status =
+		    acpi_ex_convert_to_buffer(original_object, &new_object);
+		if (ACPI_FAILURE(status)) {
+			return (status);
+		}
+		break;
+
+	case ACPI_TYPE_STRING:
+
+		/* String-to-Buffer conversion. Simple data copy */
+
+		new_object =
+		    acpi_ut_create_buffer_object(original_object->string.
+						 length);
+		if (!new_object) {
+			return (AE_NO_MEMORY);
+		}
+
+		ACPI_MEMCPY(new_object->buffer.pointer,
+			    original_object->string.pointer,
+			    original_object->string.length);
+		break;
+
+	case ACPI_TYPE_PACKAGE:
+		/*
+		 * This case is often seen for predefined names that must return a
+		 * Buffer object with multiple DWORD integers within. For example,
+		 * _FDE and _GTM. The Package can be converted to a Buffer.
+		 */
+
+		/* All elements of the Package must be integers */
+
+		elements = original_object->package.elements;
+		count = original_object->package.count;
+
+		for (i = 0; i < count; i++) {
+			if ((!*elements) ||
+			    ((*elements)->common.type != ACPI_TYPE_INTEGER)) {
+				return (AE_AML_OPERAND_TYPE);
+			}
+			elements++;
+		}
+
+		/* Create the new buffer object to replace the Package */
+
+		new_object = acpi_ut_create_buffer_object(ACPI_MUL_4(count));
+		if (!new_object) {
+			return (AE_NO_MEMORY);
+		}
+
+		/* Copy the package elements (integers) to the buffer as DWORDs */
+
+		elements = original_object->package.elements;
+		dword_buffer = ACPI_CAST_PTR(u32, new_object->buffer.pointer);
+
+		for (i = 0; i < count; i++) {
+			*dword_buffer = (u32)(*elements)->integer.value;
+			dword_buffer++;
+			elements++;
+		}
+		break;
+
+	default:
+		return (AE_AML_OPERAND_TYPE);
+	}
+
+	*return_object = new_object;
+	return (AE_OK);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ns_convert_to_unicode
+ *
+ * PARAMETERS:  original_object     - ASCII String Object to be converted
+ *              return_object       - Where the new converted object is returned
+ *
+ * RETURN:      Status. AE_OK if conversion was successful.
+ *
+ * DESCRIPTION: Attempt to convert a String object to a Unicode string Buffer.
+ *
+ ******************************************************************************/
+
+acpi_status
+acpi_ns_convert_to_unicode(union acpi_operand_object *original_object,
+			   union acpi_operand_object **return_object)
+{
+	union acpi_operand_object *new_object;
+	char *ascii_string;
+	u16 *unicode_buffer;
+	u32 unicode_length;
+	u32 i;
+
+	if (!original_object) {
+		return (AE_OK);
+	}
+
+	/* If a Buffer was returned, it must be at least two bytes long */
+
+	if (original_object->common.type == ACPI_TYPE_BUFFER) {
+		if (original_object->buffer.length < 2) {
+			return (AE_AML_OPERAND_VALUE);
+		}
+
+		*return_object = NULL;
+		return (AE_OK);
+	}
+
+	/*
+	 * The original object is an ASCII string. Convert this string to
+	 * a unicode buffer.
+	 */
+	ascii_string = original_object->string.pointer;
+	unicode_length = (original_object->string.length * 2) + 2;
+
+	/* Create a new buffer object for the Unicode data */
+
+	new_object = acpi_ut_create_buffer_object(unicode_length);
+	if (!new_object) {
+		return (AE_NO_MEMORY);
+	}
+
+	unicode_buffer = ACPI_CAST_PTR(u16, new_object->buffer.pointer);
+
+	/* Convert ASCII to Unicode */
+
+	for (i = 0; i < original_object->string.length; i++) {
+		unicode_buffer[i] = (u16)ascii_string[i];
+	}
+
+	*return_object = new_object;
+	return (AE_OK);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ns_convert_to_resource
+ *
+ * PARAMETERS:  original_object     - Object to be converted
+ *              return_object       - Where the new converted object is returned
+ *
+ * RETURN:      Status. AE_OK if conversion was successful
+ *
+ * DESCRIPTION: Attempt to convert a Integer object to a resource_template
+ *              Buffer.
+ *
+ ******************************************************************************/
+
+acpi_status
+acpi_ns_convert_to_resource(union acpi_operand_object *original_object,
+			    union acpi_operand_object **return_object)
+{
+	union acpi_operand_object *new_object;
+	u8 *buffer;
+
+	/*
+	 * We can fix the following cases for an expected resource template:
+	 * 1. No return value (interpreter slack mode is disabled)
+	 * 2. A "Return (Zero)" statement
+	 * 3. A "Return empty buffer" statement
+	 *
+	 * We will return a buffer containing a single end_tag
+	 * resource descriptor.
+	 */
+	if (original_object) {
+		switch (original_object->common.type) {
+		case ACPI_TYPE_INTEGER:
+
+			/* We can only repair an Integer==0 */
+
+			if (original_object->integer.value) {
+				return (AE_AML_OPERAND_TYPE);
+			}
+			break;
+
+		case ACPI_TYPE_BUFFER:
+
+			if (original_object->buffer.length) {
+
+				/* Additional checks can be added in the future */
+
+				*return_object = NULL;
+				return (AE_OK);
+			}
+			break;
+
+		case ACPI_TYPE_STRING:
+		default:
+
+			return (AE_AML_OPERAND_TYPE);
+		}
+	}
+
+	/* Create the new buffer object for the resource descriptor */
+
+	new_object = acpi_ut_create_buffer_object(2);
+	if (!new_object) {
+		return (AE_NO_MEMORY);
+	}
+
+	buffer = ACPI_CAST_PTR(u8, new_object->buffer.pointer);
+
+	/* Initialize the Buffer with a single end_tag descriptor */
+
+	buffer[0] = (ACPI_RESOURCE_NAME_END_TAG | ASL_RDESC_END_TAG_SIZE);
+	buffer[1] = 0x00;
+
+	*return_object = new_object;
+	return (AE_OK);
+}
diff --git a/drivers/acpi/acpica/nseval.c b/drivers/acpi/acpica/nseval.c
index 1538f3eb2a8f..b61db69d5675 100644
--- a/drivers/acpi/acpica/nseval.c
+++ b/drivers/acpi/acpica/nseval.c
@@ -98,17 +98,21 @@ acpi_status acpi_ns_evaluate(struct acpi_evaluate_info * info)
 	info->return_object = NULL;
 	info->param_count = 0;
 
-	/*
-	 * Get the actual namespace node for the target object. Handles these cases:
-	 *
-	 * 1) Null node, Pathname (absolute path)
-	 * 2) Node, Pathname (path relative to Node)
-	 * 3) Node, Null Pathname
-	 */
-	status = acpi_ns_get_node(info->prefix_node, info->pathname,
-				  ACPI_NS_NO_UPSEARCH, &info->resolved_node);
-	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
+	if (!info->resolved_node) {
+		/*
+		 * Get the actual namespace node for the target object if we need to.
+		 * Handles these cases:
+		 *
+		 * 1) Null node, Pathname (absolute path)
+		 * 2) Node, Pathname (path relative to Node)
+		 * 3) Node, Null Pathname
+		 */
+		status = acpi_ns_get_node(info->prefix_node, info->pathname,
+					  ACPI_NS_NO_UPSEARCH,
+					  &info->resolved_node);
+		if (ACPI_FAILURE(status)) {
+			return_ACPI_STATUS(status);
+		}
 	}
 
 	/*
diff --git a/drivers/acpi/acpica/nsinit.c b/drivers/acpi/acpica/nsinit.c
index 2a431ec50a25..46f0f83417a1 100644
--- a/drivers/acpi/acpica/nsinit.c
+++ b/drivers/acpi/acpica/nsinit.c
@@ -558,6 +558,7 @@ acpi_ns_init_one_device(acpi_handle obj_handle,
 	ACPI_DEBUG_EXEC(acpi_ut_display_init_pathname
 			(ACPI_TYPE_METHOD, device_node, METHOD_NAME__INI));
 
+	ACPI_MEMSET(info, 0, sizeof(struct acpi_evaluate_info));
 	info->prefix_node = device_node;
 	info->pathname = METHOD_NAME__INI;
 	info->parameters = NULL;
diff --git a/drivers/acpi/acpica/nspredef.c b/drivers/acpi/acpica/nspredef.c
index 224c30053401..8a52916148cb 100644
--- a/drivers/acpi/acpica/nspredef.c
+++ b/drivers/acpi/acpica/nspredef.c
@@ -76,19 +76,7 @@ static acpi_status
 acpi_ns_check_reference(struct acpi_predefined_data *data,
 			union acpi_operand_object *return_object);
 
-static void acpi_ns_get_expected_types(char *buffer, u32 expected_btypes);
-
-/*
- * Names for the types that can be returned by the predefined objects.
- * Used for warning messages. Must be in the same order as the ACPI_RTYPEs
- */
-static const char *acpi_rtype_names[] = {
-	"/Integer",
-	"/String",
-	"/Buffer",
-	"/Package",
-	"/Reference",
-};
+static u32 acpi_ns_get_bitmapped_type(union acpi_operand_object *return_object);
 
 /*******************************************************************************
  *
@@ -112,7 +100,6 @@ acpi_ns_check_predefined_names(struct acpi_namespace_node *node,
 			       acpi_status return_status,
 			       union acpi_operand_object **return_object_ptr)
 {
-	union acpi_operand_object *return_object = *return_object_ptr;
 	acpi_status status = AE_OK;
 	const union acpi_predefined_info *predefined;
 	char *pathname;
@@ -120,7 +107,7 @@ acpi_ns_check_predefined_names(struct acpi_namespace_node *node,
 
 	/* Match the name for this method/object against the predefined list */
 
-	predefined = acpi_ns_check_for_predefined_name(node);
+	predefined = acpi_ut_match_predefined_method(node->name.ascii);
 
 	/* Get the full pathname to the object, for use in warning messages */
 
@@ -152,25 +139,6 @@ acpi_ns_check_predefined_names(struct acpi_namespace_node *node,
 	}
 
 	/*
-	 * If there is no return value, check if we require a return value for
-	 * this predefined name. Either one return value is expected, or none,
-	 * for both methods and other objects.
-	 *
-	 * Exit now if there is no return object. Warning if one was expected.
-	 */
-	if (!return_object) {
-		if ((predefined->info.expected_btypes) &&
-		    (!(predefined->info.expected_btypes & ACPI_RTYPE_NONE))) {
-			ACPI_WARN_PREDEFINED((AE_INFO, pathname,
-					      ACPI_WARN_ALWAYS,
-					      "Missing expected return value"));
-
-			status = AE_AML_NO_RETURN_VALUE;
-		}
-		goto cleanup;
-	}
-
-	/*
 	 * Return value validation and possible repair.
 	 *
 	 * 1) Don't perform return value validation/repair if this feature
@@ -310,8 +278,10 @@ acpi_ns_check_parameter_count(char *pathname,
 	 * Validate the user-supplied parameter count.
 	 * Allow two different legal argument counts (_SCP, etc.)
 	 */
-	required_params_current = predefined->info.param_count & 0x0F;
-	required_params_old = predefined->info.param_count >> 4;
+	required_params_current =
+	    predefined->info.argument_list & METHOD_ARG_MASK;
+	required_params_old =
+	    predefined->info.argument_list >> METHOD_ARG_BIT_WIDTH;
 
 	if (user_param_count != ACPI_UINT32_MAX) {
 		if ((user_param_count != required_params_current) &&
@@ -340,52 +310,6 @@ acpi_ns_check_parameter_count(char *pathname,
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_ns_check_for_predefined_name
- *
- * PARAMETERS:  node            - Namespace node for the method/object
- *
- * RETURN:      Pointer to entry in predefined table. NULL indicates not found.
- *
- * DESCRIPTION: Check an object name against the predefined object list.
- *
- ******************************************************************************/
-
-const union acpi_predefined_info *acpi_ns_check_for_predefined_name(struct
-								    acpi_namespace_node
-								    *node)
-{
-	const union acpi_predefined_info *this_name;
-
-	/* Quick check for a predefined name, first character must be underscore */
-
-	if (node->name.ascii[0] != '_') {
-		return (NULL);
-	}
-
-	/* Search info table for a predefined method/object name */
-
-	this_name = predefined_names;
-	while (this_name->info.name[0]) {
-		if (ACPI_COMPARE_NAME(node->name.ascii, this_name->info.name)) {
-			return (this_name);
-		}
-
-		/*
-		 * Skip next entry in the table if this name returns a Package
-		 * (next entry contains the package info)
-		 */
-		if (this_name->info.expected_btypes & ACPI_RTYPE_PACKAGE) {
-			this_name++;
-		}
-
-		this_name++;
-	}
-
-	return (NULL);		/* Not found */
-}
-
-/*******************************************************************************
- *
  * FUNCTION:    acpi_ns_check_object_type
  *
  * PARAMETERS:  data            - Pointer to validation data structure
@@ -410,28 +334,12 @@ acpi_ns_check_object_type(struct acpi_predefined_data *data,
 {
 	union acpi_operand_object *return_object = *return_object_ptr;
 	acpi_status status = AE_OK;
-	u32 return_btype;
 	char type_buffer[48];	/* Room for 5 types */
 
-	/*
-	 * If we get a NULL return_object here, it is a NULL package element.
-	 * Since all extraneous NULL package elements were removed earlier by a
-	 * call to acpi_ns_remove_null_elements, this is an unexpected NULL element.
-	 * We will attempt to repair it.
-	 */
-	if (!return_object) {
-		status = acpi_ns_repair_null_element(data, expected_btypes,
-						     package_index,
-						     return_object_ptr);
-		if (ACPI_SUCCESS(status)) {
-			return (AE_OK);	/* Repair was successful */
-		}
-		goto type_error_exit;
-	}
-
 	/* A Namespace node should not get here, but make sure */
 
-	if (ACPI_GET_DESCRIPTOR_TYPE(return_object) == ACPI_DESC_TYPE_NAMED) {
+	if (return_object &&
+	    ACPI_GET_DESCRIPTOR_TYPE(return_object) == ACPI_DESC_TYPE_NAMED) {
 		ACPI_WARN_PREDEFINED((AE_INFO, data->pathname, data->node_flags,
 				      "Invalid return type - Found a Namespace node [%4.4s] type %s",
 				      return_object->node.name.ascii,
@@ -448,59 +356,31 @@ acpi_ns_check_object_type(struct acpi_predefined_data *data,
 	 * from all of the predefined names (including elements of returned
 	 * packages)
 	 */
-	switch (return_object->common.type) {
-	case ACPI_TYPE_INTEGER:
-		return_btype = ACPI_RTYPE_INTEGER;
-		break;
-
-	case ACPI_TYPE_BUFFER:
-		return_btype = ACPI_RTYPE_BUFFER;
-		break;
-
-	case ACPI_TYPE_STRING:
-		return_btype = ACPI_RTYPE_STRING;
-		break;
+	data->return_btype = acpi_ns_get_bitmapped_type(return_object);
+	if (data->return_btype == ACPI_RTYPE_ANY) {
 
-	case ACPI_TYPE_PACKAGE:
-		return_btype = ACPI_RTYPE_PACKAGE;
-		break;
-
-	case ACPI_TYPE_LOCAL_REFERENCE:
-		return_btype = ACPI_RTYPE_REFERENCE;
-		break;
-
-	default:
 		/* Not one of the supported objects, must be incorrect */
-
 		goto type_error_exit;
 	}
 
-	/* Is the object one of the expected types? */
-
-	if (return_btype & expected_btypes) {
-
-		/* For reference objects, check that the reference type is correct */
-
-		if (return_object->common.type == ACPI_TYPE_LOCAL_REFERENCE) {
-			status = acpi_ns_check_reference(data, return_object);
-		}
+	/* For reference objects, check that the reference type is correct */
 
+	if ((data->return_btype & expected_btypes) == ACPI_RTYPE_REFERENCE) {
+		status = acpi_ns_check_reference(data, return_object);
 		return (status);
 	}
 
-	/* Type mismatch -- attempt repair of the returned object */
+	/* Attempt simple repair of the returned object if necessary */
 
-	status = acpi_ns_repair_object(data, expected_btypes,
+	status = acpi_ns_simple_repair(data, expected_btypes,
 				       package_index, return_object_ptr);
-	if (ACPI_SUCCESS(status)) {
-		return (AE_OK);	/* Repair was successful */
-	}
+	return (status);
 
       type_error_exit:
 
 	/* Create a string with all expected types for this predefined object */
 
-	acpi_ns_get_expected_types(type_buffer, expected_btypes);
+	acpi_ut_get_expected_return_types(type_buffer, expected_btypes);
 
 	if (package_index == ACPI_NOT_PACKAGE_ELEMENT) {
 		ACPI_WARN_PREDEFINED((AE_INFO, data->pathname, data->node_flags,
@@ -558,36 +438,55 @@ acpi_ns_check_reference(struct acpi_predefined_data *data,
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_ns_get_expected_types
+ * FUNCTION:    acpi_ns_get_bitmapped_type
  *
- * PARAMETERS:  buffer          - Pointer to where the string is returned
- *              expected_btypes - Bitmap of expected return type(s)
+ * PARAMETERS:  return_object   - Object returned from method/obj evaluation
  *
- * RETURN:      Buffer is populated with type names.
+ * RETURN:      Object return type. ACPI_RTYPE_ANY indicates that the object
+ *              type is not supported. ACPI_RTYPE_NONE indicates that no
+ *              object was returned (return_object is NULL).
  *
- * DESCRIPTION: Translate the expected types bitmap into a string of ascii
- *              names of expected types, for use in warning messages.
+ * DESCRIPTION: Convert object type into a bitmapped object return type.
  *
  ******************************************************************************/
 
-static void acpi_ns_get_expected_types(char *buffer, u32 expected_btypes)
+static u32 acpi_ns_get_bitmapped_type(union acpi_operand_object *return_object)
 {
-	u32 this_rtype;
-	u32 i;
-	u32 j;
+	u32 return_btype;
 
-	j = 1;
-	buffer[0] = 0;
-	this_rtype = ACPI_RTYPE_INTEGER;
+	if (!return_object) {
+		return (ACPI_RTYPE_NONE);
+	}
 
-	for (i = 0; i < ACPI_NUM_RTYPES; i++) {
+	/* Map acpi_object_type to internal bitmapped type */
 
-		/* If one of the expected types, concatenate the name of this type */
+	switch (return_object->common.type) {
+	case ACPI_TYPE_INTEGER:
+		return_btype = ACPI_RTYPE_INTEGER;
+		break;
 
-		if (expected_btypes & this_rtype) {
-			ACPI_STRCAT(buffer, &acpi_rtype_names[i][j]);
-			j = 0;	/* Use name separator from now on */
-		}
-		this_rtype <<= 1;	/* Next Rtype */
+	case ACPI_TYPE_BUFFER:
+		return_btype = ACPI_RTYPE_BUFFER;
+		break;
+
+	case ACPI_TYPE_STRING:
+		return_btype = ACPI_RTYPE_STRING;
+		break;
+
+	case ACPI_TYPE_PACKAGE:
+		return_btype = ACPI_RTYPE_PACKAGE;
+		break;
+
+	case ACPI_TYPE_LOCAL_REFERENCE:
+		return_btype = ACPI_RTYPE_REFERENCE;
+		break;
+
+	default:
+		/* Not one of the supported objects, must be incorrect */
+
+		return_btype = ACPI_RTYPE_ANY;
+		break;
 	}
+
+	return (return_btype);
 }
diff --git a/drivers/acpi/acpica/nsprepkg.c b/drivers/acpi/acpica/nsprepkg.c
index a40155467d2e..77cdd539de16 100644
--- a/drivers/acpi/acpica/nsprepkg.c
+++ b/drivers/acpi/acpica/nsprepkg.c
@@ -112,9 +112,15 @@ acpi_ns_check_package(struct acpi_predefined_data *data,
 	elements = return_object->package.elements;
 	count = return_object->package.count;
 
-	/* The package must have at least one element, else invalid */
-
+	/*
+	 * Most packages must have at least one element. The only exception
+	 * is the variable-length package (ACPI_PTYPE1_VAR).
+	 */
 	if (!count) {
+		if (package->ret_info.type == ACPI_PTYPE1_VAR) {
+			return (AE_OK);
+		}
+
 		ACPI_WARN_PREDEFINED((AE_INFO, data->pathname, data->node_flags,
 				      "Return Package has no elements (empty)"));
 
diff --git a/drivers/acpi/acpica/nsrepair.c b/drivers/acpi/acpica/nsrepair.c
index 9e833353c06a..18f02e4ece01 100644
--- a/drivers/acpi/acpica/nsrepair.c
+++ b/drivers/acpi/acpica/nsrepair.c
@@ -46,6 +46,7 @@
 #include "acnamesp.h"
 #include "acinterp.h"
 #include "acpredef.h"
+#include "amlresrc.h"
 
 #define _COMPONENT          ACPI_NAMESPACE
 ACPI_MODULE_NAME("nsrepair")
@@ -71,6 +72,11 @@ ACPI_MODULE_NAME("nsrepair")
  * Buffer  -> String
  * Buffer  -> Package of Integers
  * Package -> Package of one Package
+ *
+ * Additional conversions that are available:
+ *  Convert a null return or zero return value to an end_tag descriptor
+ *  Convert an ASCII string to a Unicode buffer
+ *
  * An incorrect standalone object is wrapped with required outer package
  *
  * Additional possible repairs:
@@ -78,21 +84,51 @@ ACPI_MODULE_NAME("nsrepair")
  *
  ******************************************************************************/
 /* Local prototypes */
-static acpi_status
-acpi_ns_convert_to_integer(union acpi_operand_object *original_object,
-			   union acpi_operand_object **return_object);
-
-static acpi_status
-acpi_ns_convert_to_string(union acpi_operand_object *original_object,
-			  union acpi_operand_object **return_object);
+static const struct acpi_simple_repair_info *acpi_ns_match_simple_repair(struct
+									 acpi_namespace_node
+									 *node,
+									 u32
+									 return_btype,
+									 u32
+									 package_index);
 
-static acpi_status
-acpi_ns_convert_to_buffer(union acpi_operand_object *original_object,
-			  union acpi_operand_object **return_object);
+/*
+ * Special but simple repairs for some names.
+ *
+ * 2nd argument: Unexpected types that can be repaired
+ */
+static const struct acpi_simple_repair_info acpi_object_repair_info[] = {
+	/* Resource descriptor conversions */
+
+	{"_CRS",
+	 ACPI_RTYPE_INTEGER | ACPI_RTYPE_STRING | ACPI_RTYPE_BUFFER |
+	 ACPI_RTYPE_NONE,
+	 ACPI_NOT_PACKAGE_ELEMENT,
+	 acpi_ns_convert_to_resource},
+	{"_DMA",
+	 ACPI_RTYPE_INTEGER | ACPI_RTYPE_STRING | ACPI_RTYPE_BUFFER |
+	 ACPI_RTYPE_NONE,
+	 ACPI_NOT_PACKAGE_ELEMENT,
+	 acpi_ns_convert_to_resource},
+	{"_PRS",
+	 ACPI_RTYPE_INTEGER | ACPI_RTYPE_STRING | ACPI_RTYPE_BUFFER |
+	 ACPI_RTYPE_NONE,
+	 ACPI_NOT_PACKAGE_ELEMENT,
+	 acpi_ns_convert_to_resource},
+
+	/* Unicode conversions */
+
+	{"_MLS", ACPI_RTYPE_STRING, 1,
+	 acpi_ns_convert_to_unicode},
+	{"_STR", ACPI_RTYPE_STRING | ACPI_RTYPE_BUFFER,
+	 ACPI_NOT_PACKAGE_ELEMENT,
+	 acpi_ns_convert_to_unicode},
+	{{0, 0, 0, 0}, 0, 0, NULL}	/* Table terminator */
+};
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_ns_repair_object
+ * FUNCTION:    acpi_ns_simple_repair
  *
  * PARAMETERS:  data                - Pointer to validation data structure
  *              expected_btypes     - Object types expected
@@ -110,16 +146,54 @@ acpi_ns_convert_to_buffer(union acpi_operand_object *original_object,
  ******************************************************************************/
 
 acpi_status
-acpi_ns_repair_object(struct acpi_predefined_data *data,
+acpi_ns_simple_repair(struct acpi_predefined_data *data,
 		      u32 expected_btypes,
 		      u32 package_index,
 		      union acpi_operand_object **return_object_ptr)
 {
 	union acpi_operand_object *return_object = *return_object_ptr;
-	union acpi_operand_object *new_object;
+	union acpi_operand_object *new_object = NULL;
 	acpi_status status;
+	const struct acpi_simple_repair_info *predefined;
+
+	ACPI_FUNCTION_NAME(ns_simple_repair);
+
+	/*
+	 * Special repairs for certain names that are in the repair table.
+	 * Check if this name is in the list of repairable names.
+	 */
+	predefined = acpi_ns_match_simple_repair(data->node,
+						 data->return_btype,
+						 package_index);
+	if (predefined) {
+		if (!return_object) {
+			ACPI_WARN_PREDEFINED((AE_INFO, data->pathname,
+					      ACPI_WARN_ALWAYS,
+					      "Missing expected return value"));
+		}
+
+		status =
+		    predefined->object_converter(return_object, &new_object);
+		if (ACPI_FAILURE(status)) {
+
+			/* A fatal error occurred during a conversion */
+
+			ACPI_EXCEPTION((AE_INFO, status,
+					"During return object analysis"));
+			return (status);
+		}
+		if (new_object) {
+			goto object_repaired;
+		}
+	}
 
-	ACPI_FUNCTION_NAME(ns_repair_object);
+	/*
+	 * Do not perform simple object repair unless the return type is not
+	 * expected.
+	 */
+	if (data->return_btype & expected_btypes) {
+		return (AE_OK);
+	}
 
 	/*
 	 * At this point, we know that the type of the returned object was not
@@ -127,6 +201,24 @@ acpi_ns_repair_object(struct acpi_predefined_data *data,
 	 * repair the object by converting it to one of the expected object
 	 * types for this predefined name.
 	 */
+
+	/*
+	 * If there is no return value, check if we require a return value for
+	 * this predefined name. Either one return value is expected, or none,
+	 * for both methods and other objects.
+	 *
+	 * Exit now if there is no return object. Warning if one was expected.
+	 */
+	if (!return_object) {
+		if (expected_btypes && (!(expected_btypes & ACPI_RTYPE_NONE))) {
+			ACPI_WARN_PREDEFINED((AE_INFO, data->pathname,
+					      ACPI_WARN_ALWAYS,
+					      "Missing expected return value"));
+
+			return (AE_AML_NO_RETURN_VALUE);
+		}
+	}
+
 	if (expected_btypes & ACPI_RTYPE_INTEGER) {
 		status = acpi_ns_convert_to_integer(return_object, &new_object);
 		if (ACPI_SUCCESS(status)) {
@@ -216,254 +308,51 @@ acpi_ns_repair_object(struct acpi_predefined_data *data,
 	return (AE_OK);
 }
 
-/*******************************************************************************
- *
- * FUNCTION:    acpi_ns_convert_to_integer
- *
- * PARAMETERS:  original_object     - Object to be converted
- *              return_object       - Where the new converted object is returned
- *
- * RETURN:      Status. AE_OK if conversion was successful.
- *
- * DESCRIPTION: Attempt to convert a String/Buffer object to an Integer.
- *
- ******************************************************************************/
-
-static acpi_status
-acpi_ns_convert_to_integer(union acpi_operand_object *original_object,
-			   union acpi_operand_object **return_object)
-{
-	union acpi_operand_object *new_object;
-	acpi_status status;
-	u64 value = 0;
-	u32 i;
-
-	switch (original_object->common.type) {
-	case ACPI_TYPE_STRING:
-
-		/* String-to-Integer conversion */
-
-		status = acpi_ut_strtoul64(original_object->string.pointer,
-					   ACPI_ANY_BASE, &value);
-		if (ACPI_FAILURE(status)) {
-			return (status);
-		}
-		break;
-
-	case ACPI_TYPE_BUFFER:
-
-		/* Buffer-to-Integer conversion. Max buffer size is 64 bits. */
-
-		if (original_object->buffer.length > 8) {
-			return (AE_AML_OPERAND_TYPE);
-		}
-
-		/* Extract each buffer byte to create the integer */
-
-		for (i = 0; i < original_object->buffer.length; i++) {
-			value |=
-			    ((u64) original_object->buffer.
-			     pointer[i] << (i * 8));
-		}
-		break;
-
-	default:
-		return (AE_AML_OPERAND_TYPE);
-	}
-
-	new_object = acpi_ut_create_integer_object(value);
-	if (!new_object) {
-		return (AE_NO_MEMORY);
-	}
-
-	*return_object = new_object;
-	return (AE_OK);
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_ns_convert_to_string
- *
- * PARAMETERS:  original_object     - Object to be converted
- *              return_object       - Where the new converted object is returned
- *
- * RETURN:      Status. AE_OK if conversion was successful.
- *
- * DESCRIPTION: Attempt to convert a Integer/Buffer object to a String.
- *
- ******************************************************************************/
-
-static acpi_status
-acpi_ns_convert_to_string(union acpi_operand_object *original_object,
-			  union acpi_operand_object **return_object)
-{
-	union acpi_operand_object *new_object;
-	acpi_size length;
-	acpi_status status;
-
-	switch (original_object->common.type) {
-	case ACPI_TYPE_INTEGER:
-		/*
-		 * Integer-to-String conversion. Commonly, convert
-		 * an integer of value 0 to a NULL string. The last element of
-		 * _BIF and _BIX packages occasionally need this fix.
-		 */
-		if (original_object->integer.value == 0) {
-
-			/* Allocate a new NULL string object */
-
-			new_object = acpi_ut_create_string_object(0);
-			if (!new_object) {
-				return (AE_NO_MEMORY);
-			}
-		} else {
-			status =
-			    acpi_ex_convert_to_string(original_object,
-						      &new_object,
-						      ACPI_IMPLICIT_CONVERT_HEX);
-			if (ACPI_FAILURE(status)) {
-				return (status);
-			}
-		}
-		break;
-
-	case ACPI_TYPE_BUFFER:
-		/*
-		 * Buffer-to-String conversion. Use a to_string
-		 * conversion, no transform performed on the buffer data. The best
-		 * example of this is the _BIF method, where the string data from
-		 * the battery is often (incorrectly) returned as buffer object(s).
-		 */
-		length = 0;
-		while ((length < original_object->buffer.length) &&
-		       (original_object->buffer.pointer[length])) {
-			length++;
-		}
-
-		/* Allocate a new string object */
-
-		new_object = acpi_ut_create_string_object(length);
-		if (!new_object) {
-			return (AE_NO_MEMORY);
-		}
-
-		/*
-		 * Copy the raw buffer data with no transform. String is already NULL
-		 * terminated at Length+1.
-		 */
-		ACPI_MEMCPY(new_object->string.pointer,
-			    original_object->buffer.pointer, length);
-		break;
-
-	default:
-		return (AE_AML_OPERAND_TYPE);
-	}
-
-	*return_object = new_object;
-	return (AE_OK);
-}
-
-/*******************************************************************************
+/******************************************************************************
  *
- * FUNCTION:    acpi_ns_convert_to_buffer
+ * FUNCTION:    acpi_ns_match_simple_repair
  *
- * PARAMETERS:  original_object     - Object to be converted
- *              return_object       - Where the new converted object is returned
+ * PARAMETERS:  node                - Namespace node for the method/object
+ *              return_btype        - Object type that was returned
+ *              package_index       - Index of object within parent package (if
+ *                                    applicable - ACPI_NOT_PACKAGE_ELEMENT
+ *                                    otherwise)
  *
- * RETURN:      Status. AE_OK if conversion was successful.
+ * RETURN:      Pointer to entry in repair table. NULL indicates not found.
  *
- * DESCRIPTION: Attempt to convert a Integer/String/Package object to a Buffer.
+ * DESCRIPTION: Check an object name against the repairable object list.
  *
- ******************************************************************************/
+ *****************************************************************************/
 
-static acpi_status
-acpi_ns_convert_to_buffer(union acpi_operand_object *original_object,
-			  union acpi_operand_object **return_object)
+static const struct acpi_simple_repair_info *acpi_ns_match_simple_repair(struct
+									 acpi_namespace_node
+									 *node,
+									 u32
+									 return_btype,
+									 u32
+									 package_index)
 {
-	union acpi_operand_object *new_object;
-	acpi_status status;
-	union acpi_operand_object **elements;
-	u32 *dword_buffer;
-	u32 count;
-	u32 i;
+	const struct acpi_simple_repair_info *this_name;
 
-	switch (original_object->common.type) {
-	case ACPI_TYPE_INTEGER:
-		/*
-		 * Integer-to-Buffer conversion.
-		 * Convert the Integer to a packed-byte buffer. _MAT and other
-		 * objects need this sometimes, if a read has been performed on a
-		 * Field object that is less than or equal to the global integer
-		 * size (32 or 64 bits).
-		 */
-		status =
-		    acpi_ex_convert_to_buffer(original_object, &new_object);
-		if (ACPI_FAILURE(status)) {
-			return (status);
-		}
-		break;
+	/* Search info table for a repairable predefined method/object name */
 
-	case ACPI_TYPE_STRING:
+	this_name = acpi_object_repair_info;
+	while (this_name->object_converter) {
+		if (ACPI_COMPARE_NAME(node->name.ascii, this_name->name)) {
 
-		/* String-to-Buffer conversion. Simple data copy */
-
-		new_object =
-		    acpi_ut_create_buffer_object(original_object->string.
-						 length);
-		if (!new_object) {
-			return (AE_NO_MEMORY);
-		}
+			/* Check if we can actually repair this name/type combination */
 
-		ACPI_MEMCPY(new_object->buffer.pointer,
-			    original_object->string.pointer,
-			    original_object->string.length);
-		break;
-
-	case ACPI_TYPE_PACKAGE:
-		/*
-		 * This case is often seen for predefined names that must return a
-		 * Buffer object with multiple DWORD integers within. For example,
-		 * _FDE and _GTM. The Package can be converted to a Buffer.
-		 */
-
-		/* All elements of the Package must be integers */
-
-		elements = original_object->package.elements;
-		count = original_object->package.count;
-
-		for (i = 0; i < count; i++) {
-			if ((!*elements) ||
-			    ((*elements)->common.type != ACPI_TYPE_INTEGER)) {
-				return (AE_AML_OPERAND_TYPE);
+			if ((return_btype & this_name->unexpected_btypes) &&
+			    (package_index == this_name->package_index)) {
+				return (this_name);
 			}
-			elements++;
-		}
-
-		/* Create the new buffer object to replace the Package */
 
-		new_object = acpi_ut_create_buffer_object(ACPI_MUL_4(count));
-		if (!new_object) {
-			return (AE_NO_MEMORY);
+			return (NULL);
 		}
-
-		/* Copy the package elements (integers) to the buffer as DWORDs */
-
-		elements = original_object->package.elements;
-		dword_buffer = ACPI_CAST_PTR(u32, new_object->buffer.pointer);
-
-		for (i = 0; i < count; i++) {
-			*dword_buffer = (u32) (*elements)->integer.value;
-			dword_buffer++;
-			elements++;
-		}
-		break;
-
-	default:
-		return (AE_AML_OPERAND_TYPE);
+		this_name++;
 	}
 
-	*return_object = new_object;
-	return (AE_OK);
+	return (NULL);		/* Name was not found in the repair table */
 }
 
 /*******************************************************************************
diff --git a/drivers/acpi/acpica/nsrepair2.c b/drivers/acpi/acpica/nsrepair2.c
index ba4d98287c6a..149e9b9c2c1b 100644
--- a/drivers/acpi/acpica/nsrepair2.c
+++ b/drivers/acpi/acpica/nsrepair2.c
@@ -66,9 +66,9 @@ typedef struct acpi_repair_info {
 
 /* Local prototypes */
 
-static const struct acpi_repair_info *acpi_ns_match_repairable_name(struct
-								    acpi_namespace_node
-								    *node);
+static const struct acpi_repair_info *acpi_ns_match_complex_repair(struct
+								   acpi_namespace_node
+								   *node);
 
 static acpi_status
 acpi_ns_repair_ALR(struct acpi_predefined_data *data,
@@ -175,7 +175,7 @@ acpi_ns_complex_repairs(struct acpi_predefined_data *data,
 
 	/* Check if this name is in the list of repairable names */
 
-	predefined = acpi_ns_match_repairable_name(node);
+	predefined = acpi_ns_match_complex_repair(node);
 	if (!predefined) {
 		return (validate_status);
 	}
@@ -186,7 +186,7 @@ acpi_ns_complex_repairs(struct acpi_predefined_data *data,
 
 /******************************************************************************
  *
- * FUNCTION:    acpi_ns_match_repairable_name
+ * FUNCTION:    acpi_ns_match_complex_repair
  *
  * PARAMETERS:  node                - Namespace node for the method/object
  *
@@ -196,9 +196,9 @@ acpi_ns_complex_repairs(struct acpi_predefined_data *data,
  *
  *****************************************************************************/
 
-static const struct acpi_repair_info *acpi_ns_match_repairable_name(struct
-								    acpi_namespace_node
-								    *node)
+static const struct acpi_repair_info *acpi_ns_match_complex_repair(struct
+								   acpi_namespace_node
+								   *node)
 {
 	const struct acpi_repair_info *this_name;
 
diff --git a/drivers/acpi/acpica/nsutils.c b/drivers/acpi/acpica/nsutils.c
index 686420df684f..2808586fad30 100644
--- a/drivers/acpi/acpica/nsutils.c
+++ b/drivers/acpi/acpica/nsutils.c
@@ -112,10 +112,10 @@ acpi_object_type acpi_ns_get_type(struct acpi_namespace_node * node)
 
 	if (!node) {
 		ACPI_WARNING((AE_INFO, "Null Node parameter"));
-		return_VALUE(ACPI_TYPE_ANY);
+		return_UINT8(ACPI_TYPE_ANY);
 	}
 
-	return_VALUE(node->type);
+	return_UINT8(node->type);
 }
 
 /*******************************************************************************
@@ -140,10 +140,10 @@ u32 acpi_ns_local(acpi_object_type type)
 		/* Type code out of range  */
 
 		ACPI_WARNING((AE_INFO, "Invalid Object Type 0x%X", type));
-		return_VALUE(ACPI_NS_NORMAL);
+		return_UINT32(ACPI_NS_NORMAL);
 	}
 
-	return_VALUE(acpi_gbl_ns_properties[type] & ACPI_NS_LOCAL);
+	return_UINT32(acpi_gbl_ns_properties[type] & ACPI_NS_LOCAL);
 }
 
 /*******************************************************************************
diff --git a/drivers/acpi/acpica/psargs.c b/drivers/acpi/acpica/psargs.c
index f51308cdbc65..9f25a3d4e992 100644
--- a/drivers/acpi/acpica/psargs.c
+++ b/drivers/acpi/acpica/psargs.c
@@ -108,7 +108,7 @@ acpi_ps_get_next_package_length(struct acpi_parse_state *parser_state)
 	/* Byte 0 is a special case, either bits [0:3] or [0:5] are used */
 
 	package_length |= (aml[0] & byte_zero_mask);
-	return_VALUE(package_length);
+	return_UINT32(package_length);
 }
 
 /*******************************************************************************
diff --git a/drivers/acpi/acpica/rscalc.c b/drivers/acpi/acpica/rscalc.c
index 7816d4eef04e..72077fa1eea5 100644
--- a/drivers/acpi/acpica/rscalc.c
+++ b/drivers/acpi/acpica/rscalc.c
@@ -202,6 +202,12 @@ acpi_rs_get_aml_length(struct acpi_resource * resource, acpi_size * size_needed)
 			return_ACPI_STATUS(AE_AML_INVALID_RESOURCE_TYPE);
 		}
 
+		/* Sanity check the length. It must not be zero, or we loop forever */
+
+		if (!resource->length) {
+			return_ACPI_STATUS(AE_AML_BAD_RESOURCE_LENGTH);
+		}
+
 		/* Get the base size of the (external stream) resource descriptor */
 
 		total_size = acpi_gbl_aml_resource_sizes[resource->type];
diff --git a/drivers/acpi/acpica/rsdump.c b/drivers/acpi/acpica/rsdump.c
index cab51445189d..b5fc0db2e87b 100644
--- a/drivers/acpi/acpica/rsdump.c
+++ b/drivers/acpi/acpica/rsdump.c
@@ -385,6 +385,14 @@ void acpi_rs_dump_resource_list(struct acpi_resource *resource_list)
 			return;
 		}
 
+		/* Sanity check the length. It must not be zero, or we loop forever */
+
+		if (!resource_list->length) {
+			acpi_os_printf
+			    ("Invalid zero length descriptor in resource list\n");
+			return;
+		}
+
 		/* Dump the resource descriptor */
 
 		if (type == ACPI_RESOURCE_TYPE_SERIAL_BUS) {
diff --git a/drivers/acpi/acpica/rslist.c b/drivers/acpi/acpica/rslist.c
index ee2e206fc6c8..6053aa182093 100644
--- a/drivers/acpi/acpica/rslist.c
+++ b/drivers/acpi/acpica/rslist.c
@@ -178,6 +178,14 @@ acpi_rs_convert_resources_to_aml(struct acpi_resource *resource,
 			return_ACPI_STATUS(AE_BAD_DATA);
 		}
 
+		/* Sanity check the length. It must not be zero, or we loop forever */
+
+		if (!resource->length) {
+			ACPI_ERROR((AE_INFO,
+				    "Invalid zero length descriptor in resource list\n"));
+			return_ACPI_STATUS(AE_AML_BAD_RESOURCE_LENGTH);
+		}
+
 		/* Perform the conversion */
 
 		if (resource->type == ACPI_RESOURCE_TYPE_SERIAL_BUS) {
diff --git a/drivers/acpi/acpica/rsxface.c b/drivers/acpi/acpica/rsxface.c
index 15d6eaef0e28..c0e5d2d3ce67 100644
--- a/drivers/acpi/acpica/rsxface.c
+++ b/drivers/acpi/acpica/rsxface.c
@@ -563,13 +563,19 @@ acpi_walk_resource_buffer(struct acpi_buffer * buffer,
 
 	while (resource < resource_end) {
 
-		/* Sanity check the resource */
+		/* Sanity check the resource type */
 
 		if (resource->type > ACPI_RESOURCE_TYPE_MAX) {
 			status = AE_AML_INVALID_RESOURCE_TYPE;
 			break;
 		}
 
+		/* Sanity check the length. It must not be zero, or we loop forever */
+
+		if (!resource->length) {
+			return_ACPI_STATUS(AE_AML_BAD_RESOURCE_LENGTH);
+		}
+
 		/* Invoke the user function, abort on any error returned */
 
 		status = user_function(resource, context);
diff --git a/drivers/acpi/acpica/tbfadt.c b/drivers/acpi/acpica/tbfadt.c
index 74181bf181ec..33b00d22300a 100644
--- a/drivers/acpi/acpica/tbfadt.c
+++ b/drivers/acpi/acpica/tbfadt.c
@@ -559,8 +559,12 @@ static void acpi_tb_validate_fadt(void)
 		/*
 		 * For each extended field, check for length mismatch between the
 		 * legacy length field and the corresponding 64-bit X length field.
+		 * Note: If the legacy length field is > 0xFF bits, ignore this
+		 * check. (GPE registers can be larger than the 64-bit GAS structure
+		 * can accomodate, 0xFF bits).
 		 */
 		if (address64->address &&
+		    (ACPI_MUL_8(length) <= ACPI_UINT8_MAX) &&
 		    (address64->bit_width != ACPI_MUL_8(length))) {
 			ACPI_BIOS_WARNING((AE_INFO,
 					   "32/64X length mismatch in FADT/%s: %u/%u",
diff --git a/drivers/acpi/acpica/tbxface.c b/drivers/acpi/acpica/tbxface.c
index b35a5e6d653a..ad11162482ff 100644
--- a/drivers/acpi/acpica/tbxface.c
+++ b/drivers/acpi/acpica/tbxface.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Module Name: tbxface - ACPI table oriented external interfaces
+ * Module Name: tbxface - ACPI table-oriented external interfaces
  *
  *****************************************************************************/
 
@@ -80,7 +80,7 @@ acpi_status acpi_allocate_root_table(u32 initial_table_count)
  *                                    array is dynamically allocated.
  *              initial_table_count - Size of initial_table_array, in number of
  *                                    struct acpi_table_desc structures
- *              allow_realloc       - Flag to tell Table Manager if resize of
+ *              allow_resize        - Flag to tell Table Manager if resize of
  *                                    pre-allocated array is allowed. Ignored
  *                                    if initial_table_array is NULL.
  *
@@ -107,8 +107,8 @@ acpi_initialize_tables(struct acpi_table_desc * initial_table_array,
 	ACPI_FUNCTION_TRACE(acpi_initialize_tables);
 
 	/*
-	 * Set up the Root Table Array
-	 * Allocate the table array if requested
+	 * Setup the Root Table Array and allocate the table array
+	 * if requested
 	 */
 	if (!initial_table_array) {
 		status = acpi_allocate_root_table(initial_table_count);
@@ -305,9 +305,10 @@ ACPI_EXPORT_SYMBOL(acpi_unload_table_id)
  *              instance            - Which instance (for SSDTs)
  *              out_table           - Where the pointer to the table is returned
  *
- * RETURN:      Status and pointer to table
+ * RETURN:      Status and pointer to the requested table
  *
- * DESCRIPTION: Finds and verifies an ACPI table.
+ * DESCRIPTION: Finds and verifies an ACPI table. Table must be in the
+ *              RSDT/XSDT.
  *
  ******************************************************************************/
 acpi_status
@@ -375,9 +376,10 @@ ACPI_EXPORT_SYMBOL(acpi_get_table)
  * PARAMETERS:  table_index         - Table index
  *              table               - Where the pointer to the table is returned
  *
- * RETURN:      Status and pointer to the table
+ * RETURN:      Status and pointer to the requested table
  *
- * DESCRIPTION: Obtain a table by an index into the global table list.
+ * DESCRIPTION: Obtain a table by an index into the global table list. Used
+ *              internally also.
  *
  ******************************************************************************/
 acpi_status
@@ -432,7 +434,7 @@ ACPI_EXPORT_SYMBOL(acpi_get_table_by_index)
  *
  * RETURN:      Status
  *
- * DESCRIPTION: Install table event handler
+ * DESCRIPTION: Install a global table event handler.
  *
  ******************************************************************************/
 acpi_status
@@ -479,7 +481,7 @@ ACPI_EXPORT_SYMBOL(acpi_install_table_handler)
  *
  * RETURN:      Status
  *
- * DESCRIPTION: Remove table event handler
+ * DESCRIPTION: Remove a table event handler
  *
  ******************************************************************************/
 acpi_status acpi_remove_table_handler(acpi_table_handler handler)
diff --git a/drivers/acpi/acpica/utaddress.c b/drivers/acpi/acpica/utaddress.c
index 698b9d385516..e0a2e2779c2e 100644
--- a/drivers/acpi/acpica/utaddress.c
+++ b/drivers/acpi/acpica/utaddress.c
@@ -214,7 +214,7 @@ acpi_ut_check_address_range(acpi_adr_space_type space_id,
 
 	if ((space_id != ACPI_ADR_SPACE_SYSTEM_MEMORY) &&
 	    (space_id != ACPI_ADR_SPACE_SYSTEM_IO)) {
-		return_VALUE(0);
+		return_UINT32(0);
 	}
 
 	range_info = acpi_gbl_address_range_list[space_id];
@@ -256,7 +256,7 @@ acpi_ut_check_address_range(acpi_adr_space_type space_id,
 		range_info = range_info->next;
 	}
 
-	return_VALUE(overlap_count);
+	return_UINT32(overlap_count);
 }
 
 /*******************************************************************************
diff --git a/drivers/acpi/acpica/utcache.c b/drivers/acpi/acpica/utcache.c
index e0e8579deaac..a877a9647fd9 100644
--- a/drivers/acpi/acpica/utcache.c
+++ b/drivers/acpi/acpica/utcache.c
@@ -85,7 +85,6 @@ acpi_os_create_cache(char *cache_name,
 	/* Populate the cache object and return it */
 
 	ACPI_MEMSET(cache, 0, sizeof(struct acpi_memory_list));
-	cache->link_offset = 8;
 	cache->list_name = cache_name;
 	cache->object_size = object_size;
 	cache->max_depth = max_depth;
@@ -108,7 +107,7 @@ acpi_os_create_cache(char *cache_name,
 
 acpi_status acpi_os_purge_cache(struct acpi_memory_list * cache)
 {
-	char *next;
+	void *next;
 	acpi_status status;
 
 	ACPI_FUNCTION_ENTRY();
@@ -128,10 +127,7 @@ acpi_status acpi_os_purge_cache(struct acpi_memory_list * cache)
 
 		/* Delete and unlink one cached state object */
 
-		next = *(ACPI_CAST_INDIRECT_PTR(char,
-						&(((char *)cache->
-						   list_head)[cache->
-							      link_offset])));
+		next = ACPI_GET_DESCRIPTOR_PTR(cache->list_head);
 		ACPI_FREE(cache->list_head);
 
 		cache->list_head = next;
@@ -221,10 +217,7 @@ acpi_os_release_object(struct acpi_memory_list * cache, void *object)
 
 		/* Put the object at the head of the cache list */
 
-		*(ACPI_CAST_INDIRECT_PTR(char,
-					 &(((char *)object)[cache->
-							    link_offset]))) =
-		    cache->list_head;
+		ACPI_SET_DESCRIPTOR_PTR(object, cache->list_head);
 		cache->list_head = object;
 		cache->current_depth++;
 
@@ -272,10 +265,7 @@ void *acpi_os_acquire_object(struct acpi_memory_list *cache)
 		/* There is an object available, use it */
 
 		object = cache->list_head;
-		cache->list_head = *(ACPI_CAST_INDIRECT_PTR(char,
-							    &(((char *)
-							       object)[cache->
-								       link_offset])));
+		cache->list_head = ACPI_GET_DESCRIPTOR_PTR(object);
 
 		cache->current_depth--;
 
diff --git a/drivers/acpi/acpica/utdelete.c b/drivers/acpi/acpica/utdelete.c
index 2541de420249..29b930250b6f 100644
--- a/drivers/acpi/acpica/utdelete.c
+++ b/drivers/acpi/acpica/utdelete.c
@@ -359,19 +359,20 @@ void acpi_ut_delete_internal_object_list(union acpi_operand_object **obj_list)
  * FUNCTION:    acpi_ut_update_ref_count
  *
  * PARAMETERS:  object          - Object whose ref count is to be updated
- *              action          - What to do
+ *              action          - What to do (REF_INCREMENT or REF_DECREMENT)
  *
- * RETURN:      New ref count
+ * RETURN:      None. Sets new reference count within the object
  *
- * DESCRIPTION: Modify the ref count and return it.
+ * DESCRIPTION: Modify the reference count for an internal acpi object
  *
  ******************************************************************************/
 
 static void
 acpi_ut_update_ref_count(union acpi_operand_object *object, u32 action)
 {
-	u16 count;
-	u16 new_count;
+	u16 original_count;
+	u16 new_count = 0;
+	acpi_cpu_flags lock_flags;
 
 	ACPI_FUNCTION_NAME(ut_update_ref_count);
 
@@ -379,76 +380,79 @@ acpi_ut_update_ref_count(union acpi_operand_object *object, u32 action)
 		return;
 	}
 
-	count = object->common.reference_count;
-	new_count = count;
-
 	/*
-	 * Perform the reference count action (increment, decrement, force delete)
+	 * Always get the reference count lock. Note: Interpreter and/or
+	 * Namespace is not always locked when this function is called.
 	 */
+	lock_flags = acpi_os_acquire_lock(acpi_gbl_reference_count_lock);
+	original_count = object->common.reference_count;
+
+	/* Perform the reference count action (increment, decrement) */
+
 	switch (action) {
 	case REF_INCREMENT:
 
-		new_count++;
+		new_count = original_count + 1;
 		object->common.reference_count = new_count;
+		acpi_os_release_lock(acpi_gbl_reference_count_lock, lock_flags);
+
+		/* The current reference count should never be zero here */
+
+		if (!original_count) {
+			ACPI_WARNING((AE_INFO,
+				      "Obj %p, Reference Count was zero before increment\n",
+				      object));
+		}
 
 		ACPI_DEBUG_PRINT((ACPI_DB_ALLOCATIONS,
-				  "Obj %p Refs=%X, [Incremented]\n",
-				  object, new_count));
+				  "Obj %p Type %.2X Refs %.2X [Incremented]\n",
+				  object, object->common.type, new_count));
 		break;
 
 	case REF_DECREMENT:
 
-		if (count < 1) {
-			ACPI_DEBUG_PRINT((ACPI_DB_ALLOCATIONS,
-					  "Obj %p Refs=%X, can't decrement! (Set to 0)\n",
-					  object, new_count));
-
-			new_count = 0;
-		} else {
-			new_count--;
+		/* The current reference count must be non-zero */
 
-			ACPI_DEBUG_PRINT((ACPI_DB_ALLOCATIONS,
-					  "Obj %p Refs=%X, [Decremented]\n",
-					  object, new_count));
+		if (original_count) {
+			new_count = original_count - 1;
+			object->common.reference_count = new_count;
 		}
 
-		if (object->common.type == ACPI_TYPE_METHOD) {
-			ACPI_DEBUG_PRINT((ACPI_DB_ALLOCATIONS,
-					  "Method Obj %p Refs=%X, [Decremented]\n",
-					  object, new_count));
-		}
+		acpi_os_release_lock(acpi_gbl_reference_count_lock, lock_flags);
 
-		object->common.reference_count = new_count;
-		if (new_count == 0) {
-			acpi_ut_delete_internal_obj(object);
+		if (!original_count) {
+			ACPI_WARNING((AE_INFO,
+				      "Obj %p, Reference Count is already zero, cannot decrement\n",
+				      object));
 		}
-		break;
-
-	case REF_FORCE_DELETE:
 
 		ACPI_DEBUG_PRINT((ACPI_DB_ALLOCATIONS,
-				  "Obj %p Refs=%X, Force delete! (Set to 0)\n",
-				  object, count));
+				  "Obj %p Type %.2X Refs %.2X [Decremented]\n",
+				  object, object->common.type, new_count));
 
-		new_count = 0;
-		object->common.reference_count = new_count;
-		acpi_ut_delete_internal_obj(object);
+		/* Actually delete the object on a reference count of zero */
+
+		if (new_count == 0) {
+			acpi_ut_delete_internal_obj(object);
+		}
 		break;
 
 	default:
 
-		ACPI_ERROR((AE_INFO, "Unknown action (0x%X)", action));
-		break;
+		acpi_os_release_lock(acpi_gbl_reference_count_lock, lock_flags);
+		ACPI_ERROR((AE_INFO, "Unknown Reference Count action (0x%X)",
+			    action));
+		return;
 	}
 
 	/*
 	 * Sanity check the reference count, for debug purposes only.
 	 * (A deleted object will have a huge reference count)
 	 */
-	if (count > ACPI_MAX_REFERENCE_COUNT) {
+	if (new_count > ACPI_MAX_REFERENCE_COUNT) {
 		ACPI_WARNING((AE_INFO,
-			      "Large Reference Count (0x%X) in object %p",
-			      count, object));
+			      "Large Reference Count (0x%X) in object %p, Type=0x%.2X",
+			      new_count, object, object->common.type));
 	}
 }
 
@@ -458,8 +462,7 @@ acpi_ut_update_ref_count(union acpi_operand_object *object, u32 action)
  *
  * PARAMETERS:  object              - Increment ref count for this object
  *                                    and all sub-objects
- *              action              - Either REF_INCREMENT or REF_DECREMENT or
- *                                    REF_FORCE_DELETE
+ *              action              - Either REF_INCREMENT or REF_DECREMENT
  *
  * RETURN:      Status
  *
@@ -714,7 +717,6 @@ void acpi_ut_remove_reference(union acpi_operand_object *object)
 	/*
 	 * Allow a NULL pointer to be passed in, just ignore it. This saves
 	 * each caller from having to check. Also, ignore NS nodes.
-	 *
 	 */
 	if (!object ||
 	    (ACPI_GET_DESCRIPTOR_TYPE(object) == ACPI_DESC_TYPE_NAMED)) {
diff --git a/drivers/acpi/acpica/utexcep.c b/drivers/acpi/acpica/utexcep.c
index a0ab7c02e87c..b543a144941a 100644
--- a/drivers/acpi/acpica/utexcep.c
+++ b/drivers/acpi/acpica/utexcep.c
@@ -64,7 +64,7 @@ ACPI_MODULE_NAME("utexcep")
  ******************************************************************************/
 const char *acpi_format_exception(acpi_status status)
 {
-	const char *exception = NULL;
+	const struct acpi_exception_info *exception;
 
 	ACPI_FUNCTION_ENTRY();
 
@@ -76,10 +76,10 @@ const char *acpi_format_exception(acpi_status status)
 		ACPI_ERROR((AE_INFO,
 			    "Unknown exception code: 0x%8.8X", status));
 
-		exception = "UNKNOWN_STATUS_CODE";
+		return ("UNKNOWN_STATUS_CODE");
 	}
 
-	return (ACPI_CAST_PTR(const char, exception));
+	return (exception->name);
 }
 
 ACPI_EXPORT_SYMBOL(acpi_format_exception)
@@ -97,10 +97,10 @@ ACPI_EXPORT_SYMBOL(acpi_format_exception)
  *              an ASCII string.
  *
  ******************************************************************************/
-const char *acpi_ut_validate_exception(acpi_status status)
+const struct acpi_exception_info *acpi_ut_validate_exception(acpi_status status)
 {
 	u32 sub_status;
-	const char *exception = NULL;
+	const struct acpi_exception_info *exception = NULL;
 
 	ACPI_FUNCTION_ENTRY();
 
@@ -113,35 +113,35 @@ const char *acpi_ut_validate_exception(acpi_status status)
 	case AE_CODE_ENVIRONMENTAL:
 
 		if (sub_status <= AE_CODE_ENV_MAX) {
-			exception = acpi_gbl_exception_names_env[sub_status];
+			exception = &acpi_gbl_exception_names_env[sub_status];
 		}
 		break;
 
 	case AE_CODE_PROGRAMMER:
 
 		if (sub_status <= AE_CODE_PGM_MAX) {
-			exception = acpi_gbl_exception_names_pgm[sub_status];
+			exception = &acpi_gbl_exception_names_pgm[sub_status];
 		}
 		break;
 
 	case AE_CODE_ACPI_TABLES:
 
 		if (sub_status <= AE_CODE_TBL_MAX) {
-			exception = acpi_gbl_exception_names_tbl[sub_status];
+			exception = &acpi_gbl_exception_names_tbl[sub_status];
 		}
 		break;
 
 	case AE_CODE_AML:
 
 		if (sub_status <= AE_CODE_AML_MAX) {
-			exception = acpi_gbl_exception_names_aml[sub_status];
+			exception = &acpi_gbl_exception_names_aml[sub_status];
 		}
 		break;
 
 	case AE_CODE_CONTROL:
 
 		if (sub_status <= AE_CODE_CTRL_MAX) {
-			exception = acpi_gbl_exception_names_ctrl[sub_status];
+			exception = &acpi_gbl_exception_names_ctrl[sub_status];
 		}
 		break;
 
@@ -149,5 +149,9 @@ const char *acpi_ut_validate_exception(acpi_status status)
 		break;
 	}
 
-	return (ACPI_CAST_PTR(const char, exception));
+	if (!exception || !exception->name) {
+		return (NULL);
+	}
+
+	return (exception);
 }
diff --git a/drivers/acpi/acpica/utglobal.c b/drivers/acpi/acpica/utglobal.c
index ffecf4b4f0dd..f736448a8606 100644
--- a/drivers/acpi/acpica/utglobal.c
+++ b/drivers/acpi/acpica/utglobal.c
@@ -359,6 +359,8 @@ acpi_status acpi_ut_init_globals(void)
 
 #ifdef ACPI_DISASSEMBLER
 	acpi_gbl_external_list = NULL;
+	acpi_gbl_num_external_methods = 0;
+	acpi_gbl_resolved_external_methods = 0;
 #endif
 
 #ifdef ACPI_DEBUG_OUTPUT
diff --git a/drivers/acpi/acpica/utmutex.c b/drivers/acpi/acpica/utmutex.c
index 22feb99b8e35..08c323245584 100644
--- a/drivers/acpi/acpica/utmutex.c
+++ b/drivers/acpi/acpica/utmutex.c
@@ -81,7 +81,7 @@ acpi_status acpi_ut_mutex_initialize(void)
 		}
 	}
 
-	/* Create the spinlocks for use at interrupt level */
+	/* Create the spinlocks for use at interrupt level or for speed */
 
 	status = acpi_os_create_lock (&acpi_gbl_gpe_lock);
 	if (ACPI_FAILURE (status)) {
@@ -93,7 +93,13 @@ acpi_status acpi_ut_mutex_initialize(void)
 		return_ACPI_STATUS (status);
 	}
 
+	status = acpi_os_create_lock(&acpi_gbl_reference_count_lock);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
+
 	/* Mutex for _OSI support */
+
 	status = acpi_os_create_mutex(&acpi_gbl_osi_mutex);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
@@ -136,6 +142,7 @@ void acpi_ut_mutex_terminate(void)
 
 	acpi_os_delete_lock(acpi_gbl_gpe_lock);
 	acpi_os_delete_lock(acpi_gbl_hardware_lock);
+	acpi_os_delete_lock(acpi_gbl_reference_count_lock);
 
 	/* Delete the reader/writer lock */
 
diff --git a/drivers/acpi/acpica/utosi.c b/drivers/acpi/acpica/utosi.c
index 36a7d361d7cb..7e807725c636 100644
--- a/drivers/acpi/acpica/utosi.c
+++ b/drivers/acpi/acpica/utosi.c
@@ -108,9 +108,14 @@ static struct acpi_interface_info acpi_default_supported_interfaces[] = {
 
 acpi_status acpi_ut_initialize_interfaces(void)
 {
+	acpi_status status;
 	u32 i;
 
-	(void)acpi_os_acquire_mutex(acpi_gbl_osi_mutex, ACPI_WAIT_FOREVER);
+	status = acpi_os_acquire_mutex(acpi_gbl_osi_mutex, ACPI_WAIT_FOREVER);
+	if (ACPI_FAILURE(status)) {
+		return (status);
+	}
+
 	acpi_gbl_supported_interfaces = acpi_default_supported_interfaces;
 
 	/* Link the static list of supported interfaces */
@@ -132,20 +137,24 @@ acpi_status acpi_ut_initialize_interfaces(void)
  *
  * PARAMETERS:  None
  *
- * RETURN:      None
+ * RETURN:      Status
  *
  * DESCRIPTION: Delete all interfaces in the global list. Sets
  *              acpi_gbl_supported_interfaces to NULL.
  *
  ******************************************************************************/
 
-void acpi_ut_interface_terminate(void)
+acpi_status acpi_ut_interface_terminate(void)
 {
+	acpi_status status;
 	struct acpi_interface_info *next_interface;
 
-	(void)acpi_os_acquire_mutex(acpi_gbl_osi_mutex, ACPI_WAIT_FOREVER);
-	next_interface = acpi_gbl_supported_interfaces;
+	status = acpi_os_acquire_mutex(acpi_gbl_osi_mutex, ACPI_WAIT_FOREVER);
+	if (ACPI_FAILURE(status)) {
+		return (status);
+	}
 
+	next_interface = acpi_gbl_supported_interfaces;
 	while (next_interface) {
 		acpi_gbl_supported_interfaces = next_interface->next;
 
@@ -160,6 +169,7 @@ void acpi_ut_interface_terminate(void)
 	}
 
 	acpi_os_release_mutex(acpi_gbl_osi_mutex);
+	return (AE_OK);
 }
 
 /*******************************************************************************
@@ -315,6 +325,7 @@ acpi_status acpi_ut_osi_implementation(struct acpi_walk_state * walk_state)
 	union acpi_operand_object *return_desc;
 	struct acpi_interface_info *interface_info;
 	acpi_interface_handler interface_handler;
+	acpi_status status;
 	u32 return_value;
 
 	ACPI_FUNCTION_TRACE(ut_osi_implementation);
@@ -336,7 +347,11 @@ acpi_status acpi_ut_osi_implementation(struct acpi_walk_state * walk_state)
 	/* Default return value is 0, NOT SUPPORTED */
 
 	return_value = 0;
-	(void)acpi_os_acquire_mutex(acpi_gbl_osi_mutex, ACPI_WAIT_FOREVER);
+	status = acpi_os_acquire_mutex(acpi_gbl_osi_mutex, ACPI_WAIT_FOREVER);
+	if (ACPI_FAILURE(status)) {
+		acpi_ut_remove_reference(return_desc);
+		return_ACPI_STATUS(status);
+	}
 
 	/* Lookup the interface in the global _OSI list */
 
diff --git a/drivers/acpi/acpica/utpredef.c b/drivers/acpi/acpica/utpredef.c
new file mode 100644
index 000000000000..29459479148f
--- /dev/null
+++ b/drivers/acpi/acpica/utpredef.c
@@ -0,0 +1,399 @@
+/******************************************************************************
+ *
+ * Module Name: utpredef - support functions for predefined names
+ *
+ *****************************************************************************/
+
+/*
+ * Copyright (C) 2000 - 2013, Intel Corp.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce at minimum a disclaimer
+ *    substantially similar to the "NO WARRANTY" disclaimer below
+ *    ("Disclaimer") and any redistribution must be conditioned upon
+ *    including a substantially similar Disclaimer requirement for further
+ *    binary redistribution.
+ * 3. Neither the names of the above-listed copyright holders nor the names
+ *    of any contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * NO WARRANTY
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDERS OR CONTRIBUTORS BE LIABLE FOR SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+ * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGES.
+ */
+
+#include <acpi/acpi.h>
+#include "accommon.h"
+#include "acpredef.h"
+
+#define _COMPONENT          ACPI_UTILITIES
+ACPI_MODULE_NAME("utpredef")
+
+/*
+ * Names for the types that can be returned by the predefined objects.
+ * Used for warning messages. Must be in the same order as the ACPI_RTYPEs
+ */
+static const char *ut_rtype_names[] = {
+	"/Integer",
+	"/String",
+	"/Buffer",
+	"/Package",
+	"/Reference",
+};
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ut_get_next_predefined_method
+ *
+ * PARAMETERS:  this_name           - Entry in the predefined method/name table
+ *
+ * RETURN:      Pointer to next entry in predefined table.
+ *
+ * DESCRIPTION: Get the next entry in the predefine method table. Handles the
+ *              cases where a package info entry follows a method name that
+ *              returns a package.
+ *
+ ******************************************************************************/
+
+const union acpi_predefined_info *acpi_ut_get_next_predefined_method(const union
+								     acpi_predefined_info
+								     *this_name)
+{
+
+	/*
+	 * Skip next entry in the table if this name returns a Package
+	 * (next entry contains the package info)
+	 */
+	if ((this_name->info.expected_btypes & ACPI_RTYPE_PACKAGE) &&
+	    (this_name->info.expected_btypes != ACPI_RTYPE_ALL)) {
+		this_name++;
+	}
+
+	this_name++;
+	return (this_name);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ut_match_predefined_method
+ *
+ * PARAMETERS:  name                - Name to find
+ *
+ * RETURN:      Pointer to entry in predefined table. NULL indicates not found.
+ *
+ * DESCRIPTION: Check an object name against the predefined object list.
+ *
+ ******************************************************************************/
+
+const union acpi_predefined_info *acpi_ut_match_predefined_method(char *name)
+{
+	const union acpi_predefined_info *this_name;
+
+	/* Quick check for a predefined name, first character must be underscore */
+
+	if (name[0] != '_') {
+		return (NULL);
+	}
+
+	/* Search info table for a predefined method/object name */
+
+	this_name = acpi_gbl_predefined_methods;
+	while (this_name->info.name[0]) {
+		if (ACPI_COMPARE_NAME(name, this_name->info.name)) {
+			return (this_name);
+		}
+
+		this_name = acpi_ut_get_next_predefined_method(this_name);
+	}
+
+	return (NULL);		/* Not found */
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ut_get_expected_return_types
+ *
+ * PARAMETERS:  buffer              - Where the formatted string is returned
+ *              expected_Btypes     - Bitfield of expected data types
+ *
+ * RETURN:      Formatted string in Buffer.
+ *
+ * DESCRIPTION: Format the expected object types into a printable string.
+ *
+ ******************************************************************************/
+
+void acpi_ut_get_expected_return_types(char *buffer, u32 expected_btypes)
+{
+	u32 this_rtype;
+	u32 i;
+	u32 j;
+
+	j = 1;
+	buffer[0] = 0;
+	this_rtype = ACPI_RTYPE_INTEGER;
+
+	for (i = 0; i < ACPI_NUM_RTYPES; i++) {
+
+		/* If one of the expected types, concatenate the name of this type */
+
+		if (expected_btypes & this_rtype) {
+			ACPI_STRCAT(buffer, &ut_rtype_names[i][j]);
+			j = 0;	/* Use name separator from now on */
+		}
+
+		this_rtype <<= 1;	/* Next Rtype */
+	}
+}
+
+/*******************************************************************************
+ *
+ * The remaining functions are used by iASL and acpi_help only
+ *
+ ******************************************************************************/
+
+#if (defined ACPI_ASL_COMPILER || defined ACPI_HELP_APP)
+#include <stdio.h>
+#include <string.h>
+
+/* Local prototypes */
+
+static u32 acpi_ut_get_argument_types(char *buffer, u16 argument_types);
+
+/* Types that can be returned externally by a predefined name */
+
+static const char *ut_external_type_names[] =	/* Indexed by ACPI_TYPE_* */
+{
+	", UNSUPPORTED-TYPE",
+	", Integer",
+	", String",
+	", Buffer",
+	", Package"
+};
+
+/* Bit widths for resource descriptor predefined names */
+
+static const char *ut_resource_type_names[] = {
+	"/1",
+	"/2",
+	"/3",
+	"/8",
+	"/16",
+	"/32",
+	"/64",
+	"/variable",
+};
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ut_match_resource_name
+ *
+ * PARAMETERS:  name                - Name to find
+ *
+ * RETURN:      Pointer to entry in the resource table. NULL indicates not
+ *              found.
+ *
+ * DESCRIPTION: Check an object name against the predefined resource
+ *              descriptor object list.
+ *
+ ******************************************************************************/
+
+const union acpi_predefined_info *acpi_ut_match_resource_name(char *name)
+{
+	const union acpi_predefined_info *this_name;
+
+	/* Quick check for a predefined name, first character must be underscore */
+
+	if (name[0] != '_') {
+		return (NULL);
+	}
+
+	/* Search info table for a predefined method/object name */
+
+	this_name = acpi_gbl_resource_names;
+	while (this_name->info.name[0]) {
+		if (ACPI_COMPARE_NAME(name, this_name->info.name)) {
+			return (this_name);
+		}
+
+		this_name++;
+	}
+
+	return (NULL);		/* Not found */
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ut_display_predefined_method
+ *
+ * PARAMETERS:  buffer              - Scratch buffer for this function
+ *              this_name           - Entry in the predefined method/name table
+ *              multi_line          - TRUE if output should be on >1 line
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Display information about a predefined method. Number and
+ *              type of the input arguments, and expected type(s) for the
+ *              return value, if any.
+ *
+ ******************************************************************************/
+
+void
+acpi_ut_display_predefined_method(char *buffer,
+				  const union acpi_predefined_info *this_name,
+				  u8 multi_line)
+{
+	u32 arg_count;
+
+	/*
+	 * Get the argument count and the string buffer
+	 * containing all argument types
+	 */
+	arg_count = acpi_ut_get_argument_types(buffer,
+					       this_name->info.argument_list);
+
+	if (multi_line) {
+		printf("      ");
+	}
+
+	printf("%4.4s    Requires %s%u argument%s",
+	       this_name->info.name,
+	       (this_name->info.argument_list & ARG_COUNT_IS_MINIMUM) ?
+	       "(at least) " : "", arg_count, arg_count != 1 ? "s" : "");
+
+	/* Display the types for any arguments */
+
+	if (arg_count > 0) {
+		printf(" (%s)", buffer);
+	}
+
+	if (multi_line) {
+		printf("\n    ");
+	}
+
+	/* Get the return value type(s) allowed */
+
+	if (this_name->info.expected_btypes) {
+		acpi_ut_get_expected_return_types(buffer,
+						  this_name->info.
+						  expected_btypes);
+		printf("  Return value types: %s\n", buffer);
+	} else {
+		printf("  No return value\n");
+	}
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ut_get_argument_types
+ *
+ * PARAMETERS:  buffer              - Where to return the formatted types
+ *              argument_types      - Types field for this method
+ *
+ * RETURN:      count - the number of arguments required for this method
+ *
+ * DESCRIPTION: Format the required data types for this method (Integer,
+ *              String, Buffer, or Package) and return the required argument
+ *              count.
+ *
+ ******************************************************************************/
+
+static u32 acpi_ut_get_argument_types(char *buffer, u16 argument_types)
+{
+	u16 this_argument_type;
+	u16 sub_index;
+	u16 arg_count;
+	u32 i;
+
+	*buffer = 0;
+	sub_index = 2;
+
+	/* First field in the types list is the count of args to follow */
+
+	arg_count = (argument_types & METHOD_ARG_MASK);
+	argument_types >>= METHOD_ARG_BIT_WIDTH;
+
+	if (arg_count > METHOD_PREDEF_ARGS_MAX) {
+		printf("**** Invalid argument count (%u) "
+		       "in predefined info structure\n", arg_count);
+		return (arg_count);
+	}
+
+	/* Get each argument from the list, convert to ascii, store to buffer */
+
+	for (i = 0; i < arg_count; i++) {
+		this_argument_type = (argument_types & METHOD_ARG_MASK);
+		if (!this_argument_type
+		    || (this_argument_type > METHOD_MAX_ARG_TYPE)) {
+			printf("**** Invalid argument type (%u) "
+			       "in predefined info structure\n",
+			       this_argument_type);
+			return (arg_count);
+		}
+
+		strcat(buffer,
+		       ut_external_type_names[this_argument_type] + sub_index);
+
+		/* Shift to next argument type field */
+
+		argument_types >>= METHOD_ARG_BIT_WIDTH;
+		sub_index = 0;
+	}
+
+	return (arg_count);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ut_get_resource_bit_width
+ *
+ * PARAMETERS:  buffer              - Where the formatted string is returned
+ *              types               - Bitfield of expected data types
+ *
+ * RETURN:      Count of return types. Formatted string in Buffer.
+ *
+ * DESCRIPTION: Format the resource bit widths into a printable string.
+ *
+ ******************************************************************************/
+
+u32 acpi_ut_get_resource_bit_width(char *buffer, u16 types)
+{
+	u32 i;
+	u16 sub_index;
+	u32 found;
+
+	*buffer = 0;
+	sub_index = 1;
+	found = 0;
+
+	for (i = 0; i < NUM_RESOURCE_WIDTHS; i++) {
+		if (types & 1) {
+			strcat(buffer, &(ut_resource_type_names[i][sub_index]));
+			sub_index = 0;
+			found++;
+		}
+
+		types >>= 1;
+	}
+
+	return (found);
+}
+#endif
diff --git a/drivers/acpi/acpica/utxface.c b/drivers/acpi/acpica/utxface.c
index 48efb446258c..6505774f223e 100644
--- a/drivers/acpi/acpica/utxface.c
+++ b/drivers/acpi/acpica/utxface.c
@@ -287,7 +287,10 @@ acpi_status acpi_install_interface(acpi_string interface_name)
 		return (AE_BAD_PARAMETER);
 	}
 
-	(void)acpi_os_acquire_mutex(acpi_gbl_osi_mutex, ACPI_WAIT_FOREVER);
+	status = acpi_os_acquire_mutex(acpi_gbl_osi_mutex, ACPI_WAIT_FOREVER);
+	if (ACPI_FAILURE(status)) {
+		return (status);
+	}
 
 	/* Check if the interface name is already in the global list */
 
@@ -336,7 +339,10 @@ acpi_status acpi_remove_interface(acpi_string interface_name)
 		return (AE_BAD_PARAMETER);
 	}
 
-	(void)acpi_os_acquire_mutex(acpi_gbl_osi_mutex, ACPI_WAIT_FOREVER);
+	status = acpi_os_acquire_mutex(acpi_gbl_osi_mutex, ACPI_WAIT_FOREVER);
+	if (ACPI_FAILURE(status)) {
+		return (status);
+	}
 
 	status = acpi_ut_remove_interface(interface_name);
 
@@ -362,9 +368,12 @@ ACPI_EXPORT_SYMBOL(acpi_remove_interface)
  ****************************************************************************/
 acpi_status acpi_install_interface_handler(acpi_interface_handler handler)
 {
-	acpi_status status = AE_OK;
+	acpi_status status;
 
-	(void)acpi_os_acquire_mutex(acpi_gbl_osi_mutex, ACPI_WAIT_FOREVER);
+	status = acpi_os_acquire_mutex(acpi_gbl_osi_mutex, ACPI_WAIT_FOREVER);
+	if (ACPI_FAILURE(status)) {
+		return (status);
+	}
 
 	if (handler && acpi_gbl_interface_handler) {
 		status = AE_ALREADY_EXISTS;
diff --git a/drivers/acpi/battery.c b/drivers/acpi/battery.c
index c5cd5b5513e6..e7100459ac4a 100644
--- a/drivers/acpi/battery.c
+++ b/drivers/acpi/battery.c
@@ -146,7 +146,7 @@ struct acpi_battery {
 
 #define to_acpi_battery(x) container_of(x, struct acpi_battery, bat)
 
-inline int acpi_battery_present(struct acpi_battery *battery)
+static inline int acpi_battery_present(struct acpi_battery *battery)
 {
 	return battery->device->status.battery_present;
 }
@@ -929,7 +929,7 @@ static int acpi_battery_read_##_name(struct seq_file *seq, void *offset) \
 } \
 static int acpi_battery_##_name##_open_fs(struct inode *inode, struct file *file) \
 { \
-	return single_open(file, acpi_battery_read_##_name, PDE(inode)->data); \
+	return single_open(file, acpi_battery_read_##_name, PDE_DATA(inode)); \
 }
 
 DECLARE_FILE_FUNCTIONS(info);
diff --git a/drivers/acpi/bus.c b/drivers/acpi/bus.c
index 01708a165368..292de3cab9cc 100644
--- a/drivers/acpi/bus.c
+++ b/drivers/acpi/bus.c
@@ -288,13 +288,12 @@ acpi_status acpi_run_osc(acpi_handle handle, struct acpi_osc_context *context)
 	}
 out_success:
 	context->ret.length = out_obj->buffer.length;
-	context->ret.pointer = kmalloc(context->ret.length, GFP_KERNEL);
+	context->ret.pointer = kmemdup(out_obj->buffer.pointer,
+				       context->ret.length, GFP_KERNEL);
 	if (!context->ret.pointer) {
 		status =  AE_NO_MEMORY;
 		goto out_kfree;
 	}
-	memcpy(context->ret.pointer, out_obj->buffer.pointer,
-		context->ret.length);
 	status =  AE_OK;
 
 out_kfree:
diff --git a/drivers/acpi/button.c b/drivers/acpi/button.c
index 86c7d5445c38..d2e617b5b3f6 100644
--- a/drivers/acpi/button.c
+++ b/drivers/acpi/button.c
@@ -33,6 +33,7 @@
 #include <linux/slab.h>
 #include <acpi/acpi_bus.h>
 #include <acpi/acpi_drivers.h>
+#include <acpi/button.h>
 
 #define PREFIX "ACPI: "
 
@@ -128,7 +129,7 @@ static int acpi_button_state_seq_show(struct seq_file *seq, void *offset)
 
 static int acpi_button_state_open_fs(struct inode *inode, struct file *file)
 {
-	return single_open(file, acpi_button_state_seq_show, PDE(inode)->data);
+	return single_open(file, acpi_button_state_seq_show, PDE_DATA(inode));
 }
 
 static const struct file_operations acpi_button_state_fops = {
diff --git a/drivers/acpi/container.c b/drivers/acpi/container.c
index 5523ba7d764d..e23151667655 100644
--- a/drivers/acpi/container.c
+++ b/drivers/acpi/container.c
@@ -1,12 +1,12 @@
 /*
- * acpi_container.c  - ACPI Generic Container Driver
- * ($Revision: )
+ * container.c  - ACPI Generic Container Driver
  *
  * Copyright (C) 2004 Anil S Keshavamurthy (anil.s.keshavamurthy@intel.com)
  * Copyright (C) 2004 Keiichiro Tokunaga (tokunaga.keiich@jp.fujitsu.com)
  * Copyright (C) 2004 Motoyuki Ito (motoyuki@soft.fujitsu.com)
- * Copyright (C) 2004 Intel Corp.
  * Copyright (C) 2004 FUJITSU LIMITED
+ * Copyright (C) 2004, 2013 Intel Corp.
+ * Author: Rafael J. Wysocki <rafael.j.wysocki@intel.com>
  *
  * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  *
@@ -26,14 +26,11 @@
  *
  * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  */
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/slab.h>
-#include <linux/types.h>
 #include <linux/acpi.h>
-#include <acpi/acpi_bus.h>
-#include <acpi/acpi_drivers.h>
+
+#include "internal.h"
+
+#include "internal.h"
 
 #define PREFIX "ACPI: "
 
@@ -50,141 +47,20 @@ static const struct acpi_device_id container_device_ids[] = {
 static int container_device_attach(struct acpi_device *device,
 				   const struct acpi_device_id *not_used)
 {
-	/*
-	 * FIXME: This is necessary, so that acpi_eject_store() doesn't return
-	 * -ENODEV for containers.
-	 */
+	/* This is necessary for container hotplug to work. */
 	return 1;
 }
 
-static struct acpi_scan_handler container_device_handler = {
+static struct acpi_scan_handler container_handler = {
 	.ids = container_device_ids,
 	.attach = container_device_attach,
+	.hotplug = {
+		.enabled = true,
+		.mode = AHM_CONTAINER,
+	},
 };
 
-static int is_device_present(acpi_handle handle)
-{
-	acpi_handle temp;
-	acpi_status status;
-	unsigned long long sta;
-
-
-	status = acpi_get_handle(handle, "_STA", &temp);
-	if (ACPI_FAILURE(status))
-		return 1;	/* _STA not found, assume device present */
-
-	status = acpi_evaluate_integer(handle, "_STA", NULL, &sta);
-	if (ACPI_FAILURE(status))
-		return 0;	/* Firmware error */
-
-	return ((sta & ACPI_STA_DEVICE_PRESENT) == ACPI_STA_DEVICE_PRESENT);
-}
-
-static void container_notify_cb(acpi_handle handle, u32 type, void *context)
-{
-	struct acpi_device *device = NULL;
-	int result;
-	int present;
-	acpi_status status;
-	u32 ost_code = ACPI_OST_SC_NON_SPECIFIC_FAILURE; /* default */
-
-	acpi_scan_lock_acquire();
-
-	switch (type) {
-	case ACPI_NOTIFY_BUS_CHECK:
-		/* Fall through */
-	case ACPI_NOTIFY_DEVICE_CHECK:
-		pr_debug("Container driver received %s event\n",
-		       (type == ACPI_NOTIFY_BUS_CHECK) ?
-		       "ACPI_NOTIFY_BUS_CHECK" : "ACPI_NOTIFY_DEVICE_CHECK");
-
-		present = is_device_present(handle);
-		status = acpi_bus_get_device(handle, &device);
-		if (!present) {
-			if (ACPI_SUCCESS(status)) {
-				/* device exist and this is a remove request */
-				device->flags.eject_pending = 1;
-				kobject_uevent(&device->dev.kobj, KOBJ_OFFLINE);
-				goto out;
-			}
-			break;
-		}
-
-		if (!ACPI_FAILURE(status) || device)
-			break;
-
-		result = acpi_bus_scan(handle);
-		if (result) {
-			acpi_handle_warn(handle, "Failed to add container\n");
-			break;
-		}
-		result = acpi_bus_get_device(handle, &device);
-		if (result) {
-			acpi_handle_warn(handle, "Missing device object\n");
-			break;
-		}
-
-		kobject_uevent(&device->dev.kobj, KOBJ_ONLINE);
-		ost_code = ACPI_OST_SC_SUCCESS;
-		break;
-
-	case ACPI_NOTIFY_EJECT_REQUEST:
-		if (!acpi_bus_get_device(handle, &device) && device) {
-			device->flags.eject_pending = 1;
-			kobject_uevent(&device->dev.kobj, KOBJ_OFFLINE);
-			goto out;
-		}
-		break;
-
-	default:
-		/* non-hotplug event; possibly handled by other handler */
-		goto out;
-	}
-
-	/* Inform firmware that the hotplug operation has completed */
-	(void) acpi_evaluate_hotplug_ost(handle, type, ost_code, NULL);
-
- out:
-	acpi_scan_lock_release();
-}
-
-static bool is_container(acpi_handle handle)
-{
-	struct acpi_device_info *info;
-	bool ret = false;
-
-	if (ACPI_FAILURE(acpi_get_object_info(handle, &info)))
-		return false;
-
-	if (info->valid & ACPI_VALID_HID) {
-		const struct acpi_device_id *id;
-
-		for (id = container_device_ids; id->id[0]; id++) {
-			ret = !strcmp((char *)id->id, info->hardware_id.string);
-			if (ret)
-				break;
-		}
-	}
-	kfree(info);
-	return ret;
-}
-
-static acpi_status acpi_container_register_notify_handler(acpi_handle handle,
-							  u32 lvl, void *ctxt,
-							  void **retv)
-{
-	if (is_container(handle))
-		acpi_install_notify_handler(handle, ACPI_SYSTEM_NOTIFY,
-					    container_notify_cb, NULL);
-
-	return AE_OK;
-}
-
 void __init acpi_container_init(void)
 {
-	acpi_walk_namespace(ACPI_TYPE_DEVICE, ACPI_ROOT_OBJECT, ACPI_UINT32_MAX,
-			    acpi_container_register_notify_handler, NULL,
-			    NULL, NULL);
-
-	acpi_scan_add_handler(&container_device_handler);
+	acpi_scan_add_handler_with_hotplug(&container_handler, "container");
 }
diff --git a/drivers/acpi/device_pm.c b/drivers/acpi/device_pm.c
index dd314ef9bff1..96de787e6104 100644
--- a/drivers/acpi/device_pm.c
+++ b/drivers/acpi/device_pm.c
@@ -145,27 +145,36 @@ int acpi_device_get_power(struct acpi_device *device, int *state)
 	}
 
 	/*
-	 * Get the device's power state either directly (via _PSC) or
-	 * indirectly (via power resources).
+	 * Get the device's power state from power resources settings and _PSC,
+	 * if available.
 	 */
+	if (device->power.flags.power_resources) {
+		int error = acpi_power_get_inferred_state(device, &result);
+		if (error)
+			return error;
+	}
 	if (device->power.flags.explicit_get) {
+		acpi_handle handle = device->handle;
 		unsigned long long psc;
-		acpi_status status = acpi_evaluate_integer(device->handle,
-							   "_PSC", NULL, &psc);
+		acpi_status status;
+
+		status = acpi_evaluate_integer(handle, "_PSC", NULL, &psc);
 		if (ACPI_FAILURE(status))
 			return -ENODEV;
 
-		result = psc;
-	}
-	/* The test below covers ACPI_STATE_UNKNOWN too. */
-	if (result <= ACPI_STATE_D2) {
-	  ; /* Do nothing. */
-	} else if (device->power.flags.power_resources) {
-		int error = acpi_power_get_inferred_state(device, &result);
-		if (error)
-			return error;
-	} else if (result == ACPI_STATE_D3_HOT) {
-		result = ACPI_STATE_D3;
+		/*
+		 * The power resources settings may indicate a power state
+		 * shallower than the actual power state of the device.
+		 *
+		 * Moreover, on systems predating ACPI 4.0, if the device
+		 * doesn't depend on any power resources and _PSC returns 3,
+		 * that means "power off".  We need to maintain compatibility
+		 * with those systems.
+		 */
+		if (psc > result && psc < ACPI_STATE_D3_COLD)
+			result = psc;
+		else if (result == ACPI_STATE_UNKNOWN)
+			result = psc > ACPI_STATE_D2 ? ACPI_STATE_D3_COLD : psc;
 	}
 
 	/*
diff --git a/drivers/acpi/fan.c b/drivers/acpi/fan.c
index f815da82c765..8d1c0105e113 100644
--- a/drivers/acpi/fan.c
+++ b/drivers/acpi/fan.c
@@ -174,9 +174,13 @@ static int acpi_fan_add(struct acpi_device *device)
 
 static int acpi_fan_remove(struct acpi_device *device)
 {
-	struct thermal_cooling_device *cdev = acpi_driver_data(device);
+	struct thermal_cooling_device *cdev;
+
+	if (!device)
+		return -EINVAL;
 
-	if (!device || !cdev)
+	cdev =  acpi_driver_data(device);
+	if (!cdev)
 		return -EINVAL;
 
 	sysfs_remove_link(&device->dev.kobj, "thermal_cooling");
diff --git a/drivers/acpi/internal.h b/drivers/acpi/internal.h
index 3c94a732b4b3..6f1afd9118c8 100644
--- a/drivers/acpi/internal.h
+++ b/drivers/acpi/internal.h
@@ -41,6 +41,17 @@ void acpi_container_init(void);
 #else
 static inline void acpi_container_init(void) {}
 #endif
+#ifdef CONFIG_ACPI_HOTPLUG_MEMORY
+void acpi_memory_hotplug_init(void);
+#else
+static inline void acpi_memory_hotplug_init(void) {}
+#endif
+
+void acpi_sysfs_add_hotplug_profile(struct acpi_hotplug_profile *hotplug,
+				    const char *name);
+int acpi_scan_add_handler_with_hotplug(struct acpi_scan_handler *handler,
+				       const char *hotplug_profile_name);
+void acpi_scan_hotplug_enabled(struct acpi_hotplug_profile *hotplug, bool val);
 
 #ifdef CONFIG_DEBUG_FS
 extern struct dentry *acpi_debugfs_dir;
@@ -48,6 +59,11 @@ int acpi_debugfs_init(void);
 #else
 static inline void acpi_debugfs_init(void) { return; }
 #endif
+#ifdef CONFIG_X86_INTEL_LPSS
+void acpi_lpss_init(void);
+#else
+static inline void acpi_lpss_init(void) {}
+#endif
 
 /* --------------------------------------------------------------------------
                      Device Node Initialization / Removal
@@ -60,7 +76,7 @@ int acpi_device_add(struct acpi_device *device,
 void acpi_init_device_object(struct acpi_device *device, acpi_handle handle,
 			     int type, unsigned long long sta);
 void acpi_device_add_finalize(struct acpi_device *device);
-void acpi_free_ids(struct acpi_device *device);
+void acpi_free_pnp_ids(struct acpi_device_pnp *pnp);
 
 /* --------------------------------------------------------------------------
                                   Power Resource
@@ -131,4 +147,7 @@ static inline void suspend_nvs_restore(void) {}
   -------------------------------------------------------------------------- */
 struct platform_device;
 
+int acpi_create_platform_device(struct acpi_device *adev,
+				const struct acpi_device_id *id);
+
 #endif /* _ACPI_INTERNAL_H_ */
diff --git a/drivers/acpi/osl.c b/drivers/acpi/osl.c
index 586e7e993d3d..e72186340fec 100644
--- a/drivers/acpi/osl.c
+++ b/drivers/acpi/osl.c
@@ -641,7 +641,7 @@ void __init acpi_initrd_override(void *data, size_t size)
 	 * Both memblock_reserve and e820_add_region (via arch_reserve_mem_area)
 	 * works fine.
 	 */
-	memblock_reserve(acpi_tables_addr, acpi_tables_addr + all_tables_size);
+	memblock_reserve(acpi_tables_addr, all_tables_size);
 	arch_reserve_mem_area(acpi_tables_addr, all_tables_size);
 
 	p = early_ioremap(acpi_tables_addr, all_tables_size);
@@ -1555,7 +1555,7 @@ int acpi_check_resource_conflict(const struct resource *res)
 	else
 		space_id = ACPI_ADR_SPACE_SYSTEM_MEMORY;
 
-	length = res->end - res->start + 1;
+	length = resource_size(res);
 	if (acpi_enforce_resources != ENFORCE_RESOURCES_NO)
 		warn = 1;
 	clash = acpi_check_address_range(space_id, res->start, length, warn);
diff --git a/drivers/acpi/pci_link.c b/drivers/acpi/pci_link.c
index ab764ed34a50..2652a614deeb 100644
--- a/drivers/acpi/pci_link.c
+++ b/drivers/acpi/pci_link.c
@@ -354,6 +354,7 @@ static int acpi_pci_link_set(struct acpi_pci_link *link, int irq)
 
 	}
 	resource->end.type = ACPI_RESOURCE_TYPE_END_TAG;
+	resource->end.length = sizeof(struct acpi_resource);
 
 	/* Attempt to set the resource */
 	status = acpi_set_current_resources(link->device->handle, &buffer);
diff --git a/drivers/acpi/pci_root.c b/drivers/acpi/pci_root.c
index ac8688b89705..1dd6f6c85874 100644
--- a/drivers/acpi/pci_root.c
+++ b/drivers/acpi/pci_root.c
@@ -169,8 +169,8 @@ static acpi_status acpi_pci_query_osc(struct acpi_pci_root *root,
 		*control &= OSC_PCI_CONTROL_MASKS;
 		capbuf[OSC_CONTROL_TYPE] = *control | root->osc_control_set;
 	} else {
-		/* Run _OSC query for all possible controls. */
-		capbuf[OSC_CONTROL_TYPE] = OSC_PCI_CONTROL_MASKS;
+		/* Run _OSC query only with existing controls. */
+		capbuf[OSC_CONTROL_TYPE] = root->osc_control_set;
 	}
 
 	status = acpi_pci_run_osc(root->device->handle, capbuf, &result);
diff --git a/drivers/acpi/power.c b/drivers/acpi/power.c
index 34f5ef11d427..f962047c6c85 100644
--- a/drivers/acpi/power.c
+++ b/drivers/acpi/power.c
@@ -459,57 +459,79 @@ static struct attribute_group attr_groups[] = {
 	},
 };
 
-static void acpi_power_hide_list(struct acpi_device *adev, int state)
+static struct attribute_group wakeup_attr_group = {
+	.name = "power_resources_wakeup",
+	.attrs = attrs,
+};
+
+static void acpi_power_hide_list(struct acpi_device *adev,
+				 struct list_head *resources,
+				 struct attribute_group *attr_group)
 {
-	struct acpi_device_power_state *ps = &adev->power.states[state];
 	struct acpi_power_resource_entry *entry;
 
-	if (list_empty(&ps->resources))
+	if (list_empty(resources))
 		return;
 
-	list_for_each_entry_reverse(entry, &ps->resources, node) {
+	list_for_each_entry_reverse(entry, resources, node) {
 		struct acpi_device *res_dev = &entry->resource->device;
 
 		sysfs_remove_link_from_group(&adev->dev.kobj,
-					     attr_groups[state].name,
+					     attr_group->name,
 					     dev_name(&res_dev->dev));
 	}
-	sysfs_remove_group(&adev->dev.kobj, &attr_groups[state]);
+	sysfs_remove_group(&adev->dev.kobj, attr_group);
 }
 
-static void acpi_power_expose_list(struct acpi_device *adev, int state)
+static void acpi_power_expose_list(struct acpi_device *adev,
+				   struct list_head *resources,
+				   struct attribute_group *attr_group)
 {
-	struct acpi_device_power_state *ps = &adev->power.states[state];
 	struct acpi_power_resource_entry *entry;
 	int ret;
 
-	if (list_empty(&ps->resources))
+	if (list_empty(resources))
 		return;
 
-	ret = sysfs_create_group(&adev->dev.kobj, &attr_groups[state]);
+	ret = sysfs_create_group(&adev->dev.kobj, attr_group);
 	if (ret)
 		return;
 
-	list_for_each_entry(entry, &ps->resources, node) {
+	list_for_each_entry(entry, resources, node) {
 		struct acpi_device *res_dev = &entry->resource->device;
 
 		ret = sysfs_add_link_to_group(&adev->dev.kobj,
-					      attr_groups[state].name,
+					      attr_group->name,
 					      &res_dev->dev.kobj,
 					      dev_name(&res_dev->dev));
 		if (ret) {
-			acpi_power_hide_list(adev, state);
+			acpi_power_hide_list(adev, resources, attr_group);
 			break;
 		}
 	}
 }
 
+static void acpi_power_expose_hide(struct acpi_device *adev,
+				   struct list_head *resources,
+				   struct attribute_group *attr_group,
+				   bool expose)
+{
+	if (expose)
+		acpi_power_expose_list(adev, resources, attr_group);
+	else
+		acpi_power_hide_list(adev, resources, attr_group);
+}
+
 void acpi_power_add_remove_device(struct acpi_device *adev, bool add)
 {
 	struct acpi_device_power_state *ps;
 	struct acpi_power_resource_entry *entry;
 	int state;
 
+	if (adev->wakeup.flags.valid)
+		acpi_power_expose_hide(adev, &adev->wakeup.resources,
+				       &wakeup_attr_group, add);
+
 	if (!adev->power.flags.power_resources)
 		return;
 
@@ -523,12 +545,10 @@ void acpi_power_add_remove_device(struct acpi_device *adev, bool add)
 			acpi_power_remove_dependent(resource, adev);
 	}
 
-	for (state = ACPI_STATE_D0; state <= ACPI_STATE_D3_HOT; state++) {
-		if (add)
-			acpi_power_expose_list(adev, state);
-		else
-			acpi_power_hide_list(adev, state);
-	}
+	for (state = ACPI_STATE_D0; state <= ACPI_STATE_D3_HOT; state++)
+		acpi_power_expose_hide(adev,
+				       &adev->power.states[state].resources,
+				       &attr_groups[state], add);
 }
 
 int acpi_power_wakeup_list_init(struct list_head *list, int *system_level_p)
@@ -824,7 +844,7 @@ static void acpi_release_power_resource(struct device *dev)
 	list_del(&resource->list_node);
 	mutex_unlock(&power_resource_list_lock);
 
-	acpi_free_ids(device);
+	acpi_free_pnp_ids(&device->pnp);
 	kfree(resource);
 }
 
diff --git a/drivers/acpi/proc.c b/drivers/acpi/proc.c
index 52ce76725c20..aa1227a7e3f2 100644
--- a/drivers/acpi/proc.c
+++ b/drivers/acpi/proc.c
@@ -120,7 +120,7 @@ static int acpi_system_alarm_seq_show(struct seq_file *seq, void *offset)
 
 static int acpi_system_alarm_open_fs(struct inode *inode, struct file *file)
 {
-	return single_open(file, acpi_system_alarm_seq_show, PDE(inode)->data);
+	return single_open(file, acpi_system_alarm_seq_show, PDE_DATA(inode));
 }
 
 static int get_date_field(char **p, u32 * value)
@@ -397,7 +397,7 @@ static int
 acpi_system_wakeup_device_open_fs(struct inode *inode, struct file *file)
 {
 	return single_open(file, acpi_system_wakeup_device_seq_show,
-			   PDE(inode)->data);
+			   PDE_DATA(inode));
 }
 
 static const struct file_operations acpi_system_wakeup_device_fops = {
diff --git a/drivers/acpi/processor_idle.c b/drivers/acpi/processor_idle.c
index ee255c60bdac..f0df2c9434d2 100644
--- a/drivers/acpi/processor_idle.c
+++ b/drivers/acpi/processor_idle.c
@@ -918,7 +918,6 @@ static int acpi_idle_enter_bm(struct cpuidle_device *dev,
 struct cpuidle_driver acpi_idle_driver = {
 	.name =		"acpi_idle",
 	.owner =	THIS_MODULE,
-	.en_core_tk_irqen = 1,
 };
 
 /**
diff --git a/drivers/acpi/processor_thermal.c b/drivers/acpi/processor_thermal.c
index 641b5450a0db..e8e652710e65 100644
--- a/drivers/acpi/processor_thermal.c
+++ b/drivers/acpi/processor_thermal.c
@@ -218,9 +218,13 @@ processor_get_max_state(struct thermal_cooling_device *cdev,
 			unsigned long *state)
 {
 	struct acpi_device *device = cdev->devdata;
-	struct acpi_processor *pr = acpi_driver_data(device);
+	struct acpi_processor *pr;
 
-	if (!device || !pr)
+	if (!device)
+		return -EINVAL;
+
+	pr = acpi_driver_data(device);
+	if (!pr)
 		return -EINVAL;
 
 	*state = acpi_processor_max_state(pr);
@@ -232,9 +236,13 @@ processor_get_cur_state(struct thermal_cooling_device *cdev,
 			unsigned long *cur_state)
 {
 	struct acpi_device *device = cdev->devdata;
-	struct acpi_processor *pr = acpi_driver_data(device);
+	struct acpi_processor *pr;
 
-	if (!device || !pr)
+	if (!device)
+		return -EINVAL;
+
+	pr = acpi_driver_data(device);
+	if (!pr)
 		return -EINVAL;
 
 	*cur_state = cpufreq_get_cur_state(pr->id);
@@ -248,11 +256,15 @@ processor_set_cur_state(struct thermal_cooling_device *cdev,
 			unsigned long state)
 {
 	struct acpi_device *device = cdev->devdata;
-	struct acpi_processor *pr = acpi_driver_data(device);
+	struct acpi_processor *pr;
 	int result = 0;
 	int max_pstate;
 
-	if (!device || !pr)
+	if (!device)
+		return -EINVAL;
+
+	pr = acpi_driver_data(device);
+	if (!pr)
 		return -EINVAL;
 
 	max_pstate = cpufreq_get_max_state(pr->id);
diff --git a/drivers/acpi/processor_throttling.c b/drivers/acpi/processor_throttling.c
index 1d02b7b5ade0..e7dd2c1fee79 100644
--- a/drivers/acpi/processor_throttling.c
+++ b/drivers/acpi/processor_throttling.c
@@ -211,9 +211,10 @@ err_ret:
  */
 void acpi_processor_throttling_init(void)
 {
-	if (acpi_processor_update_tsd_coord())
+	if (acpi_processor_update_tsd_coord()) {
 		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
 			"Assume no T-state coordination\n"));
+	}
 
 	return;
 }
diff --git a/drivers/acpi/sbs.c b/drivers/acpi/sbs.c
index e523245643ac..b6241eeb1132 100644
--- a/drivers/acpi/sbs.c
+++ b/drivers/acpi/sbs.c
@@ -521,19 +521,6 @@ acpi_sbs_add_fs(struct proc_dir_entry **dir,
 	return 0;
 }
 
-static void
-acpi_sbs_remove_fs(struct proc_dir_entry **dir,
-			   struct proc_dir_entry *parent_dir)
-{
-	if (*dir) {
-		remove_proc_entry(ACPI_SBS_FILE_INFO, *dir);
-		remove_proc_entry(ACPI_SBS_FILE_STATE, *dir);
-		remove_proc_entry(ACPI_SBS_FILE_ALARM, *dir);
-		remove_proc_entry((*dir)->name, parent_dir);
-		*dir = NULL;
-	}
-}
-
 /* Smart Battery Interface */
 static struct proc_dir_entry *acpi_battery_dir = NULL;
 
@@ -584,7 +571,7 @@ static int acpi_battery_read_info(struct seq_file *seq, void *offset)
 
 static int acpi_battery_info_open_fs(struct inode *inode, struct file *file)
 {
-	return single_open(file, acpi_battery_read_info, PDE(inode)->data);
+	return single_open(file, acpi_battery_read_info, PDE_DATA(inode));
 }
 
 static int acpi_battery_read_state(struct seq_file *seq, void *offset)
@@ -623,7 +610,7 @@ static int acpi_battery_read_state(struct seq_file *seq, void *offset)
 
 static int acpi_battery_state_open_fs(struct inode *inode, struct file *file)
 {
-	return single_open(file, acpi_battery_read_state, PDE(inode)->data);
+	return single_open(file, acpi_battery_read_state, PDE_DATA(inode));
 }
 
 static int acpi_battery_read_alarm(struct seq_file *seq, void *offset)
@@ -688,7 +675,7 @@ acpi_battery_write_alarm(struct file *file, const char __user * buffer,
 
 static int acpi_battery_alarm_open_fs(struct inode *inode, struct file *file)
 {
-	return single_open(file, acpi_battery_read_alarm, PDE(inode)->data);
+	return single_open(file, acpi_battery_read_alarm, PDE_DATA(inode));
 }
 
 static const struct file_operations acpi_battery_info_fops = {
@@ -736,7 +723,7 @@ static int acpi_ac_read_state(struct seq_file *seq, void *offset)
 
 static int acpi_ac_state_open_fs(struct inode *inode, struct file *file)
 {
-	return single_open(file, acpi_ac_read_state, PDE(inode)->data);
+	return single_open(file, acpi_ac_read_state, PDE_DATA(inode));
 }
 
 static const struct file_operations acpi_ac_state_fops = {
@@ -836,8 +823,8 @@ static void acpi_battery_remove(struct acpi_sbs *sbs, int id)
 		power_supply_unregister(&battery->bat);
 	}
 #ifdef CONFIG_ACPI_PROCFS_POWER
-	if (battery->proc_entry)
-		acpi_sbs_remove_fs(&battery->proc_entry, acpi_battery_dir);
+	proc_remove(battery->proc_entry);
+	battery->proc_entry = NULL;
 #endif
 }
 
@@ -873,8 +860,8 @@ static void acpi_charger_remove(struct acpi_sbs *sbs)
 	if (sbs->charger.dev)
 		power_supply_unregister(&sbs->charger);
 #ifdef CONFIG_ACPI_PROCFS_POWER
-	if (sbs->charger_entry)
-		acpi_sbs_remove_fs(&sbs->charger_entry, acpi_ac_dir);
+	proc_remove(sbs->charger_entry);
+	sbs->charger_entry = NULL;
 #endif
 }
 
diff --git a/drivers/acpi/scan.c b/drivers/acpi/scan.c
index f54d1985e594..fe158fd4f1df 100644
--- a/drivers/acpi/scan.c
+++ b/drivers/acpi/scan.c
@@ -63,6 +63,19 @@ int acpi_scan_add_handler(struct acpi_scan_handler *handler)
 	return 0;
 }
 
+int acpi_scan_add_handler_with_hotplug(struct acpi_scan_handler *handler,
+				       const char *hotplug_profile_name)
+{
+	int error;
+
+	error = acpi_scan_add_handler(handler);
+	if (error)
+		return error;
+
+	acpi_sysfs_add_hotplug_profile(&handler->hotplug, hotplug_profile_name);
+	return 0;
+}
+
 /*
  * Creates hid/cid(s) string needed for modalias and uevent
  * e.g. on a device with hid:IBM0001 and cid:ACPI0001 you get:
@@ -107,32 +120,20 @@ acpi_device_modalias_show(struct device *dev, struct device_attribute *attr, cha
 }
 static DEVICE_ATTR(modalias, 0444, acpi_device_modalias_show, NULL);
 
-/**
- * acpi_bus_hot_remove_device: hot-remove a device and its children
- * @context: struct acpi_eject_event pointer (freed in this func)
- *
- * Hot-remove a device and its children. This function frees up the
- * memory space passed by arg context, so that the caller may call
- * this function asynchronously through acpi_os_hotplug_execute().
- */
-void acpi_bus_hot_remove_device(void *context)
+static int acpi_scan_hot_remove(struct acpi_device *device)
 {
-	struct acpi_eject_event *ej_event = context;
-	struct acpi_device *device = ej_event->device;
 	acpi_handle handle = device->handle;
-	acpi_handle temp;
+	acpi_handle not_used;
 	struct acpi_object_list arg_list;
 	union acpi_object arg;
-	acpi_status status = AE_OK;
-	u32 ost_code = ACPI_OST_SC_NON_SPECIFIC_FAILURE; /* default */
-
-	mutex_lock(&acpi_scan_lock);
+	acpi_status status;
+	unsigned long long sta;
 
 	/* If there is no handle, the device node has been unregistered. */
-	if (!device->handle) {
+	if (!handle) {
 		dev_dbg(&device->dev, "ACPI handle missing\n");
 		put_device(&device->dev);
-		goto out;
+		return -EINVAL;
 	}
 
 	ACPI_DEBUG_PRINT((ACPI_DB_INFO,
@@ -143,7 +144,7 @@ void acpi_bus_hot_remove_device(void *context)
 	put_device(&device->dev);
 	device = NULL;
 
-	if (ACPI_SUCCESS(acpi_get_handle(handle, "_LCK", &temp))) {
+	if (ACPI_SUCCESS(acpi_get_handle(handle, "_LCK", &not_used))) {
 		arg_list.count = 1;
 		arg_list.pointer = &arg;
 		arg.type = ACPI_TYPE_INTEGER;
@@ -161,18 +162,205 @@ void acpi_bus_hot_remove_device(void *context)
 	 */
 	status = acpi_evaluate_object(handle, "_EJ0", &arg_list, NULL);
 	if (ACPI_FAILURE(status)) {
-		if (status != AE_NOT_FOUND)
-			acpi_handle_warn(handle, "Eject failed\n");
+		if (status == AE_NOT_FOUND) {
+			return -ENODEV;
+		} else {
+			acpi_handle_warn(handle, "Eject failed (0x%x)\n",
+								status);
+			return -EIO;
+		}
+	}
 
-		/* Tell the firmware the hot-remove operation has failed. */
-		acpi_evaluate_hotplug_ost(handle, ej_event->event,
-					  ost_code, NULL);
+	/*
+	 * Verify if eject was indeed successful.  If not, log an error
+	 * message.  No need to call _OST since _EJ0 call was made OK.
+	 */
+	status = acpi_evaluate_integer(handle, "_STA", NULL, &sta);
+	if (ACPI_FAILURE(status)) {
+		acpi_handle_warn(handle,
+			"Status check after eject failed (0x%x)\n", status);
+	} else if (sta & ACPI_STA_DEVICE_ENABLED) {
+		acpi_handle_warn(handle,
+			"Eject incomplete - status 0x%llx\n", sta);
+	}
+
+	return 0;
+}
+
+static void acpi_bus_device_eject(void *context)
+{
+	acpi_handle handle = context;
+	struct acpi_device *device = NULL;
+	struct acpi_scan_handler *handler;
+	u32 ost_code = ACPI_OST_SC_NON_SPECIFIC_FAILURE;
+
+	mutex_lock(&acpi_scan_lock);
+
+	acpi_bus_get_device(handle, &device);
+	if (!device)
+		goto err_out;
+
+	handler = device->handler;
+	if (!handler || !handler->hotplug.enabled) {
+		ost_code = ACPI_OST_SC_EJECT_NOT_SUPPORTED;
+		goto err_out;
+	}
+	acpi_evaluate_hotplug_ost(handle, ACPI_NOTIFY_EJECT_REQUEST,
+				  ACPI_OST_SC_EJECT_IN_PROGRESS, NULL);
+	if (handler->hotplug.mode == AHM_CONTAINER) {
+		device->flags.eject_pending = true;
+		kobject_uevent(&device->dev.kobj, KOBJ_OFFLINE);
+	} else {
+		int error;
+
+		get_device(&device->dev);
+		error = acpi_scan_hot_remove(device);
+		if (error)
+			goto err_out;
 	}
 
  out:
 	mutex_unlock(&acpi_scan_lock);
-	kfree(context);
 	return;
+
+ err_out:
+	acpi_evaluate_hotplug_ost(handle, ACPI_NOTIFY_EJECT_REQUEST, ost_code,
+				  NULL);
+	goto out;
+}
+
+static void acpi_scan_bus_device_check(acpi_handle handle, u32 ost_source)
+{
+	struct acpi_device *device = NULL;
+	u32 ost_code = ACPI_OST_SC_NON_SPECIFIC_FAILURE;
+	int error;
+
+	mutex_lock(&acpi_scan_lock);
+
+	acpi_bus_get_device(handle, &device);
+	if (device) {
+		dev_warn(&device->dev, "Attempt to re-insert\n");
+		goto out;
+	}
+	acpi_evaluate_hotplug_ost(handle, ost_source,
+				  ACPI_OST_SC_INSERT_IN_PROGRESS, NULL);
+	error = acpi_bus_scan(handle);
+	if (error) {
+		acpi_handle_warn(handle, "Namespace scan failure\n");
+		goto out;
+	}
+	error = acpi_bus_get_device(handle, &device);
+	if (error) {
+		acpi_handle_warn(handle, "Missing device node object\n");
+		goto out;
+	}
+	ost_code = ACPI_OST_SC_SUCCESS;
+	if (device->handler && device->handler->hotplug.mode == AHM_CONTAINER)
+		kobject_uevent(&device->dev.kobj, KOBJ_ONLINE);
+
+ out:
+	acpi_evaluate_hotplug_ost(handle, ost_source, ost_code, NULL);
+	mutex_unlock(&acpi_scan_lock);
+}
+
+static void acpi_scan_bus_check(void *context)
+{
+	acpi_scan_bus_device_check((acpi_handle)context,
+				   ACPI_NOTIFY_BUS_CHECK);
+}
+
+static void acpi_scan_device_check(void *context)
+{
+	acpi_scan_bus_device_check((acpi_handle)context,
+				   ACPI_NOTIFY_DEVICE_CHECK);
+}
+
+static void acpi_hotplug_unsupported(acpi_handle handle, u32 type)
+{
+	u32 ost_status;
+
+	switch (type) {
+	case ACPI_NOTIFY_BUS_CHECK:
+		acpi_handle_debug(handle,
+			"ACPI_NOTIFY_BUS_CHECK event: unsupported\n");
+		ost_status = ACPI_OST_SC_INSERT_NOT_SUPPORTED;
+		break;
+	case ACPI_NOTIFY_DEVICE_CHECK:
+		acpi_handle_debug(handle,
+			"ACPI_NOTIFY_DEVICE_CHECK event: unsupported\n");
+		ost_status = ACPI_OST_SC_INSERT_NOT_SUPPORTED;
+		break;
+	case ACPI_NOTIFY_EJECT_REQUEST:
+		acpi_handle_debug(handle,
+			"ACPI_NOTIFY_EJECT_REQUEST event: unsupported\n");
+		ost_status = ACPI_OST_SC_EJECT_NOT_SUPPORTED;
+		break;
+	default:
+		/* non-hotplug event; possibly handled by other handler */
+		return;
+	}
+
+	acpi_evaluate_hotplug_ost(handle, type, ost_status, NULL);
+}
+
+static void acpi_hotplug_notify_cb(acpi_handle handle, u32 type, void *data)
+{
+	acpi_osd_exec_callback callback;
+	struct acpi_scan_handler *handler = data;
+	acpi_status status;
+
+	if (!handler->hotplug.enabled)
+		return acpi_hotplug_unsupported(handle, type);
+
+	switch (type) {
+	case ACPI_NOTIFY_BUS_CHECK:
+		acpi_handle_debug(handle, "ACPI_NOTIFY_BUS_CHECK event\n");
+		callback = acpi_scan_bus_check;
+		break;
+	case ACPI_NOTIFY_DEVICE_CHECK:
+		acpi_handle_debug(handle, "ACPI_NOTIFY_DEVICE_CHECK event\n");
+		callback = acpi_scan_device_check;
+		break;
+	case ACPI_NOTIFY_EJECT_REQUEST:
+		acpi_handle_debug(handle, "ACPI_NOTIFY_EJECT_REQUEST event\n");
+		callback = acpi_bus_device_eject;
+		break;
+	default:
+		/* non-hotplug event; possibly handled by other handler */
+		return;
+	}
+	status = acpi_os_hotplug_execute(callback, handle);
+	if (ACPI_FAILURE(status))
+		acpi_evaluate_hotplug_ost(handle, type,
+					  ACPI_OST_SC_NON_SPECIFIC_FAILURE,
+					  NULL);
+}
+
+/**
+ * acpi_bus_hot_remove_device: hot-remove a device and its children
+ * @context: struct acpi_eject_event pointer (freed in this func)
+ *
+ * Hot-remove a device and its children. This function frees up the
+ * memory space passed by arg context, so that the caller may call
+ * this function asynchronously through acpi_os_hotplug_execute().
+ */
+void acpi_bus_hot_remove_device(void *context)
+{
+	struct acpi_eject_event *ej_event = context;
+	struct acpi_device *device = ej_event->device;
+	acpi_handle handle = device->handle;
+	int error;
+
+	mutex_lock(&acpi_scan_lock);
+
+	error = acpi_scan_hot_remove(device);
+	if (error && handle)
+		acpi_evaluate_hotplug_ost(handle, ej_event->event,
+					  ACPI_OST_SC_NON_SPECIFIC_FAILURE,
+					  NULL);
+
+	mutex_unlock(&acpi_scan_lock);
+	kfree(context);
 }
 EXPORT_SYMBOL(acpi_bus_hot_remove_device);
 
@@ -206,51 +394,61 @@ static ssize_t
 acpi_eject_store(struct device *d, struct device_attribute *attr,
 		const char *buf, size_t count)
 {
-	int ret = count;
-	acpi_status status;
-	acpi_object_type type = 0;
 	struct acpi_device *acpi_device = to_acpi_device(d);
 	struct acpi_eject_event *ej_event;
+	acpi_object_type not_used;
+	acpi_status status;
+	u32 ost_source;
+	int ret;
 
-	if ((!count) || (buf[0] != '1')) {
+	if (!count || buf[0] != '1')
 		return -EINVAL;
-	}
-	if (!acpi_device->driver && !acpi_device->handler) {
-		ret = -ENODEV;
-		goto err;
-	}
-	status = acpi_get_type(acpi_device->handle, &type);
-	if (ACPI_FAILURE(status) || (!acpi_device->flags.ejectable)) {
-		ret = -ENODEV;
-		goto err;
-	}
 
-	ej_event = kmalloc(sizeof(*ej_event), GFP_KERNEL);
-	if (!ej_event) {
-		ret = -ENOMEM;
-		goto err;
-	}
+	if ((!acpi_device->handler || !acpi_device->handler->hotplug.enabled)
+	    && !acpi_device->driver)
+		return -ENODEV;
+
+	status = acpi_get_type(acpi_device->handle, &not_used);
+	if (ACPI_FAILURE(status) || !acpi_device->flags.ejectable)
+		return -ENODEV;
+
+	mutex_lock(&acpi_scan_lock);
 
-	get_device(&acpi_device->dev);
-	ej_event->device = acpi_device;
 	if (acpi_device->flags.eject_pending) {
-		/* event originated from ACPI eject notification */
-		ej_event->event = ACPI_NOTIFY_EJECT_REQUEST;
+		/* ACPI eject notification event. */
+		ost_source = ACPI_NOTIFY_EJECT_REQUEST;
 		acpi_device->flags.eject_pending = 0;
 	} else {
-		/* event originated from user */
-		ej_event->event = ACPI_OST_EC_OSPM_EJECT;
-		(void) acpi_evaluate_hotplug_ost(acpi_device->handle,
-			ej_event->event, ACPI_OST_SC_EJECT_IN_PROGRESS, NULL);
+		/* Eject initiated by user space. */
+		ost_source = ACPI_OST_EC_OSPM_EJECT;
 	}
-
+	ej_event = kmalloc(sizeof(*ej_event), GFP_KERNEL);
+	if (!ej_event) {
+		ret = -ENOMEM;
+		goto err_out;
+	}
+	acpi_evaluate_hotplug_ost(acpi_device->handle, ost_source,
+				  ACPI_OST_SC_EJECT_IN_PROGRESS, NULL);
+	ej_event->device = acpi_device;
+	ej_event->event = ost_source;
+	get_device(&acpi_device->dev);
 	status = acpi_os_hotplug_execute(acpi_bus_hot_remove_device, ej_event);
 	if (ACPI_FAILURE(status)) {
 		put_device(&acpi_device->dev);
 		kfree(ej_event);
+		ret = status == AE_NO_MEMORY ? -ENOMEM : -EAGAIN;
+		goto err_out;
 	}
-err:
+	ret = count;
+
+ out:
+	mutex_unlock(&acpi_scan_lock);
 	return ret;
+
+ err_out:
+	acpi_evaluate_hotplug_ost(acpi_device->handle, ost_source,
+				  ACPI_OST_SC_NON_SPECIFIC_FAILURE, NULL);
+	goto out;
 }
 
 static DEVICE_ATTR(eject, 0200, NULL, acpi_eject_store);
@@ -376,7 +574,7 @@ static int acpi_device_setup_files(struct acpi_device *dev)
 			goto end;
 	}
 
-	if (dev->flags.bus_address)
+	if (dev->pnp.type.bus_address)
 		result = device_create_file(&dev->dev, &dev_attr_adr);
 	if (dev->pnp.unique_id)
 		result = device_create_file(&dev->dev, &dev_attr_uid);
@@ -449,7 +647,7 @@ static void acpi_device_remove_files(struct acpi_device *dev)
 
 	if (dev->pnp.unique_id)
 		device_remove_file(&dev->dev, &dev_attr_uid);
-	if (dev->flags.bus_address)
+	if (dev->pnp.type.bus_address)
 		device_remove_file(&dev->dev, &dev_attr_adr);
 	device_remove_file(&dev->dev, &dev_attr_modalias);
 	device_remove_file(&dev->dev, &dev_attr_hid);
@@ -512,17 +710,6 @@ int acpi_match_device_ids(struct acpi_device *device,
 }
 EXPORT_SYMBOL(acpi_match_device_ids);
 
-void acpi_free_ids(struct acpi_device *device)
-{
-	struct acpi_hardware_id *id, *tmp;
-
-	list_for_each_entry_safe(id, tmp, &device->pnp.ids, list) {
-		kfree(id->id);
-		kfree(id);
-	}
-	kfree(device->pnp.unique_id);
-}
-
 static void acpi_free_power_resources_lists(struct acpi_device *device)
 {
 	int i;
@@ -543,7 +730,7 @@ static void acpi_device_release(struct device *dev)
 {
 	struct acpi_device *acpi_dev = to_acpi_device(dev);
 
-	acpi_free_ids(acpi_dev);
+	acpi_free_pnp_ids(&acpi_dev->pnp);
 	acpi_free_power_resources_lists(acpi_dev);
 	kfree(acpi_dev);
 }
@@ -1256,19 +1443,17 @@ static void acpi_device_get_busid(struct acpi_device *device)
 }
 
 /*
- * acpi_bay_match - see if a device is an ejectable driver bay
+ * acpi_bay_match - see if an acpi object is an ejectable driver bay
  *
  * If an acpi object is ejectable and has one of the ACPI ATA methods defined,
  * then we can safely call it an ejectable drive bay
  */
-static int acpi_bay_match(struct acpi_device *device){
+static int acpi_bay_match(acpi_handle handle)
+{
 	acpi_status status;
-	acpi_handle handle;
 	acpi_handle tmp;
 	acpi_handle phandle;
 
-	handle = device->handle;
-
 	status = acpi_get_handle(handle, "_EJ0", &tmp);
 	if (ACPI_FAILURE(status))
 		return -ENODEV;
@@ -1292,12 +1477,12 @@ static int acpi_bay_match(struct acpi_device *device){
 }
 
 /*
- * acpi_dock_match - see if a device has a _DCK method
+ * acpi_dock_match - see if an acpi object has a _DCK method
  */
-static int acpi_dock_match(struct acpi_device *device)
+static int acpi_dock_match(acpi_handle handle)
 {
 	acpi_handle tmp;
-	return acpi_get_handle(device->handle, "_DCK", &tmp);
+	return acpi_get_handle(handle, "_DCK", &tmp);
 }
 
 const char *acpi_device_hid(struct acpi_device *device)
@@ -1312,7 +1497,7 @@ const char *acpi_device_hid(struct acpi_device *device)
 }
 EXPORT_SYMBOL(acpi_device_hid);
 
-static void acpi_add_id(struct acpi_device *device, const char *dev_id)
+static void acpi_add_id(struct acpi_device_pnp *pnp, const char *dev_id)
 {
 	struct acpi_hardware_id *id;
 
@@ -1326,7 +1511,8 @@ static void acpi_add_id(struct acpi_device *device, const char *dev_id)
 		return;
 	}
 
-	list_add_tail(&id->list, &device->pnp.ids);
+	list_add_tail(&id->list, &pnp->ids);
+	pnp->type.hardware_id = 1;
 }
 
 /*
@@ -1334,7 +1520,7 @@ static void acpi_add_id(struct acpi_device *device, const char *dev_id)
  * lacks the SMBUS01 HID and the methods do not have the necessary "_"
  * prefix.  Work around this.
  */
-static int acpi_ibm_smbus_match(struct acpi_device *device)
+static int acpi_ibm_smbus_match(acpi_handle handle)
 {
 	acpi_handle h_dummy;
 	struct acpi_buffer path = {ACPI_ALLOCATE_BUFFER, NULL};
@@ -1344,7 +1530,7 @@ static int acpi_ibm_smbus_match(struct acpi_device *device)
 		return -ENODEV;
 
 	/* Look for SMBS object */
-	result = acpi_get_name(device->handle, ACPI_SINGLE_NAME, &path);
+	result = acpi_get_name(handle, ACPI_SINGLE_NAME, &path);
 	if (result)
 		return result;
 
@@ -1355,48 +1541,50 @@ static int acpi_ibm_smbus_match(struct acpi_device *device)
 
 	/* Does it have the necessary (but misnamed) methods? */
 	result = -ENODEV;
-	if (ACPI_SUCCESS(acpi_get_handle(device->handle, "SBI", &h_dummy)) &&
-	    ACPI_SUCCESS(acpi_get_handle(device->handle, "SBR", &h_dummy)) &&
-	    ACPI_SUCCESS(acpi_get_handle(device->handle, "SBW", &h_dummy)))
+	if (ACPI_SUCCESS(acpi_get_handle(handle, "SBI", &h_dummy)) &&
+	    ACPI_SUCCESS(acpi_get_handle(handle, "SBR", &h_dummy)) &&
+	    ACPI_SUCCESS(acpi_get_handle(handle, "SBW", &h_dummy)))
 		result = 0;
 out:
 	kfree(path.pointer);
 	return result;
 }
 
-static void acpi_device_set_id(struct acpi_device *device)
+static void acpi_set_pnp_ids(acpi_handle handle, struct acpi_device_pnp *pnp,
+				int device_type)
 {
 	acpi_status status;
 	struct acpi_device_info *info;
 	struct acpi_pnp_device_id_list *cid_list;
 	int i;
 
-	switch (device->device_type) {
+	switch (device_type) {
 	case ACPI_BUS_TYPE_DEVICE:
-		if (ACPI_IS_ROOT_DEVICE(device)) {
-			acpi_add_id(device, ACPI_SYSTEM_HID);
+		if (handle == ACPI_ROOT_OBJECT) {
+			acpi_add_id(pnp, ACPI_SYSTEM_HID);
 			break;
 		}
 
-		status = acpi_get_object_info(device->handle, &info);
+		status = acpi_get_object_info(handle, &info);
 		if (ACPI_FAILURE(status)) {
-			printk(KERN_ERR PREFIX "%s: Error reading device info\n", __func__);
+			pr_err(PREFIX "%s: Error reading device info\n",
+					__func__);
 			return;
 		}
 
 		if (info->valid & ACPI_VALID_HID)
-			acpi_add_id(device, info->hardware_id.string);
+			acpi_add_id(pnp, info->hardware_id.string);
 		if (info->valid & ACPI_VALID_CID) {
 			cid_list = &info->compatible_id_list;
 			for (i = 0; i < cid_list->count; i++)
-				acpi_add_id(device, cid_list->ids[i].string);
+				acpi_add_id(pnp, cid_list->ids[i].string);
 		}
 		if (info->valid & ACPI_VALID_ADR) {
-			device->pnp.bus_address = info->address;
-			device->flags.bus_address = 1;
+			pnp->bus_address = info->address;
+			pnp->type.bus_address = 1;
 		}
 		if (info->valid & ACPI_VALID_UID)
-			device->pnp.unique_id = kstrdup(info->unique_id.string,
+			pnp->unique_id = kstrdup(info->unique_id.string,
 							GFP_KERNEL);
 
 		kfree(info);
@@ -1405,40 +1593,50 @@ static void acpi_device_set_id(struct acpi_device *device)
 		 * Some devices don't reliably have _HIDs & _CIDs, so add
 		 * synthetic HIDs to make sure drivers can find them.
 		 */
-		if (acpi_is_video_device(device))
-			acpi_add_id(device, ACPI_VIDEO_HID);
-		else if (ACPI_SUCCESS(acpi_bay_match(device)))
-			acpi_add_id(device, ACPI_BAY_HID);
-		else if (ACPI_SUCCESS(acpi_dock_match(device)))
-			acpi_add_id(device, ACPI_DOCK_HID);
-		else if (!acpi_ibm_smbus_match(device))
-			acpi_add_id(device, ACPI_SMBUS_IBM_HID);
-		else if (list_empty(&device->pnp.ids) &&
-			 ACPI_IS_ROOT_DEVICE(device->parent)) {
-			acpi_add_id(device, ACPI_BUS_HID); /* \_SB, LNXSYBUS */
-			strcpy(device->pnp.device_name, ACPI_BUS_DEVICE_NAME);
-			strcpy(device->pnp.device_class, ACPI_BUS_CLASS);
+		if (acpi_is_video_device(handle))
+			acpi_add_id(pnp, ACPI_VIDEO_HID);
+		else if (ACPI_SUCCESS(acpi_bay_match(handle)))
+			acpi_add_id(pnp, ACPI_BAY_HID);
+		else if (ACPI_SUCCESS(acpi_dock_match(handle)))
+			acpi_add_id(pnp, ACPI_DOCK_HID);
+		else if (!acpi_ibm_smbus_match(handle))
+			acpi_add_id(pnp, ACPI_SMBUS_IBM_HID);
+		else if (list_empty(&pnp->ids) && handle == ACPI_ROOT_OBJECT) {
+			acpi_add_id(pnp, ACPI_BUS_HID); /* \_SB, LNXSYBUS */
+			strcpy(pnp->device_name, ACPI_BUS_DEVICE_NAME);
+			strcpy(pnp->device_class, ACPI_BUS_CLASS);
 		}
 
 		break;
 	case ACPI_BUS_TYPE_POWER:
-		acpi_add_id(device, ACPI_POWER_HID);
+		acpi_add_id(pnp, ACPI_POWER_HID);
 		break;
 	case ACPI_BUS_TYPE_PROCESSOR:
-		acpi_add_id(device, ACPI_PROCESSOR_OBJECT_HID);
+		acpi_add_id(pnp, ACPI_PROCESSOR_OBJECT_HID);
 		break;
 	case ACPI_BUS_TYPE_THERMAL:
-		acpi_add_id(device, ACPI_THERMAL_HID);
+		acpi_add_id(pnp, ACPI_THERMAL_HID);
 		break;
 	case ACPI_BUS_TYPE_POWER_BUTTON:
-		acpi_add_id(device, ACPI_BUTTON_HID_POWERF);
+		acpi_add_id(pnp, ACPI_BUTTON_HID_POWERF);
 		break;
 	case ACPI_BUS_TYPE_SLEEP_BUTTON:
-		acpi_add_id(device, ACPI_BUTTON_HID_SLEEPF);
+		acpi_add_id(pnp, ACPI_BUTTON_HID_SLEEPF);
 		break;
 	}
 }
 
+void acpi_free_pnp_ids(struct acpi_device_pnp *pnp)
+{
+	struct acpi_hardware_id *id, *tmp;
+
+	list_for_each_entry_safe(id, tmp, &pnp->ids, list) {
+		kfree(id->id);
+		kfree(id);
+	}
+	kfree(pnp->unique_id);
+}
+
 void acpi_init_device_object(struct acpi_device *device, acpi_handle handle,
 			     int type, unsigned long long sta)
 {
@@ -1448,7 +1646,7 @@ void acpi_init_device_object(struct acpi_device *device, acpi_handle handle,
 	device->parent = acpi_bus_get_parent(handle);
 	STRUCT_TO_INT(device->status) = sta;
 	acpi_device_get_busid(device);
-	acpi_device_set_id(device);
+	acpi_set_pnp_ids(handle, &device->pnp, type);
 	acpi_bus_get_flags(device);
 	device->flags.match_driver = false;
 	device_initialize(&device->dev);
@@ -1536,6 +1734,75 @@ static int acpi_bus_type_and_status(acpi_handle handle, int *type,
 	return 0;
 }
 
+static bool acpi_scan_handler_matching(struct acpi_scan_handler *handler,
+				       char *idstr,
+				       const struct acpi_device_id **matchid)
+{
+	const struct acpi_device_id *devid;
+
+	for (devid = handler->ids; devid->id[0]; devid++)
+		if (!strcmp((char *)devid->id, idstr)) {
+			if (matchid)
+				*matchid = devid;
+
+			return true;
+		}
+
+	return false;
+}
+
+static struct acpi_scan_handler *acpi_scan_match_handler(char *idstr,
+					const struct acpi_device_id **matchid)
+{
+	struct acpi_scan_handler *handler;
+
+	list_for_each_entry(handler, &acpi_scan_handlers_list, list_node)
+		if (acpi_scan_handler_matching(handler, idstr, matchid))
+			return handler;
+
+	return NULL;
+}
+
+void acpi_scan_hotplug_enabled(struct acpi_hotplug_profile *hotplug, bool val)
+{
+	if (!!hotplug->enabled == !!val)
+		return;
+
+	mutex_lock(&acpi_scan_lock);
+
+	hotplug->enabled = val;
+
+	mutex_unlock(&acpi_scan_lock);
+}
+
+static void acpi_scan_init_hotplug(acpi_handle handle, int type)
+{
+	struct acpi_device_pnp pnp = {};
+	struct acpi_hardware_id *hwid;
+	struct acpi_scan_handler *handler;
+
+	INIT_LIST_HEAD(&pnp.ids);
+	acpi_set_pnp_ids(handle, &pnp, type);
+
+	if (!pnp.type.hardware_id)
+		return;
+
+	/*
+	 * This relies on the fact that acpi_install_notify_handler() will not
+	 * install the same notify handler routine twice for the same handle.
+	 */
+	list_for_each_entry(hwid, &pnp.ids, list) {
+		handler = acpi_scan_match_handler(hwid->id, NULL);
+		if (handler) {
+			acpi_install_notify_handler(handle, ACPI_SYSTEM_NOTIFY,
+					acpi_hotplug_notify_cb, handler);
+			break;
+		}
+	}
+
+	acpi_free_pnp_ids(&pnp);
+}
+
 static acpi_status acpi_bus_check_add(acpi_handle handle, u32 lvl_not_used,
 				      void *not_used, void **return_value)
 {
@@ -1558,6 +1825,8 @@ static acpi_status acpi_bus_check_add(acpi_handle handle, u32 lvl_not_used,
 		return AE_OK;
 	}
 
+	acpi_scan_init_hotplug(handle, type);
+
 	if (!(sta & ACPI_STA_DEVICE_PRESENT) &&
 	    !(sta & ACPI_STA_DEVICE_FUNCTIONING)) {
 		struct acpi_device_wakeup wakeup;
@@ -1583,42 +1852,26 @@ static acpi_status acpi_bus_check_add(acpi_handle handle, u32 lvl_not_used,
 	return AE_OK;
 }
 
-static int acpi_scan_do_attach_handler(struct acpi_device *device, char *id)
+static int acpi_scan_attach_handler(struct acpi_device *device)
 {
-	struct acpi_scan_handler *handler;
+	struct acpi_hardware_id *hwid;
+	int ret = 0;
 
-	list_for_each_entry(handler, &acpi_scan_handlers_list, list_node) {
+	list_for_each_entry(hwid, &device->pnp.ids, list) {
 		const struct acpi_device_id *devid;
+		struct acpi_scan_handler *handler;
 
-		for (devid = handler->ids; devid->id[0]; devid++) {
-			int ret;
-
-			if (strcmp((char *)devid->id, id))
-				continue;
-
+		handler = acpi_scan_match_handler(hwid->id, &devid);
+		if (handler) {
 			ret = handler->attach(device, devid);
 			if (ret > 0) {
 				device->handler = handler;
-				return ret;
+				break;
 			} else if (ret < 0) {
-				return ret;
+				break;
 			}
 		}
 	}
-	return 0;
-}
-
-static int acpi_scan_attach_handler(struct acpi_device *device)
-{
-	struct acpi_hardware_id *hwid;
-	int ret = 0;
-
-	list_for_each_entry(hwid, &device->pnp.ids, list) {
-		ret = acpi_scan_do_attach_handler(device, hwid->id);
-		if (ret)
-			break;
-
-	}
 	return ret;
 }
 
@@ -1788,8 +2041,10 @@ int __init acpi_scan_init(void)
 	acpi_pci_root_init();
 	acpi_pci_link_init();
 	acpi_platform_init();
+	acpi_lpss_init();
 	acpi_csrt_init();
 	acpi_container_init();
+	acpi_memory_hotplug_init();
 
 	mutex_lock(&acpi_scan_lock);
 	/*
diff --git a/drivers/acpi/sysfs.c b/drivers/acpi/sysfs.c
index 41c0504470db..fcae5fa2e1b3 100644
--- a/drivers/acpi/sysfs.c
+++ b/drivers/acpi/sysfs.c
@@ -7,6 +7,8 @@
 #include <linux/moduleparam.h>
 #include <acpi/acpi_drivers.h>
 
+#include "internal.h"
+
 #define _COMPONENT		ACPI_SYSTEM_COMPONENT
 ACPI_MODULE_NAME("sysfs");
 
@@ -249,6 +251,7 @@ module_param_call(acpica_version, NULL, param_get_acpica_version, NULL, 0444);
 static LIST_HEAD(acpi_table_attr_list);
 static struct kobject *tables_kobj;
 static struct kobject *dynamic_tables_kobj;
+static struct kobject *hotplug_kobj;
 
 struct acpi_table_attr {
 	struct bin_attribute attr;
@@ -716,6 +719,67 @@ acpi_show_profile(struct device *dev, struct device_attribute *attr,
 static const struct device_attribute pm_profile_attr =
 	__ATTR(pm_profile, S_IRUGO, acpi_show_profile, NULL);
 
+static ssize_t hotplug_enabled_show(struct kobject *kobj,
+				    struct kobj_attribute *attr, char *buf)
+{
+	struct acpi_hotplug_profile *hotplug = to_acpi_hotplug_profile(kobj);
+
+	return sprintf(buf, "%d\n", hotplug->enabled);
+}
+
+static ssize_t hotplug_enabled_store(struct kobject *kobj,
+				     struct kobj_attribute *attr,
+				     const char *buf, size_t size)
+{
+	struct acpi_hotplug_profile *hotplug = to_acpi_hotplug_profile(kobj);
+	unsigned int val;
+
+	if (kstrtouint(buf, 10, &val) || val > 1)
+		return -EINVAL;
+
+	acpi_scan_hotplug_enabled(hotplug, val);
+	return size;
+}
+
+static struct kobj_attribute hotplug_enabled_attr =
+	__ATTR(enabled, S_IRUGO | S_IWUSR, hotplug_enabled_show,
+		hotplug_enabled_store);
+
+static struct attribute *hotplug_profile_attrs[] = {
+	&hotplug_enabled_attr.attr,
+	NULL
+};
+
+static struct kobj_type acpi_hotplug_profile_ktype = {
+	.sysfs_ops = &kobj_sysfs_ops,
+	.default_attrs = hotplug_profile_attrs,
+};
+
+void acpi_sysfs_add_hotplug_profile(struct acpi_hotplug_profile *hotplug,
+				    const char *name)
+{
+	int error;
+
+	if (!hotplug_kobj)
+		goto err_out;
+
+	kobject_init(&hotplug->kobj, &acpi_hotplug_profile_ktype);
+	error = kobject_set_name(&hotplug->kobj, "%s", name);
+	if (error)
+		goto err_out;
+
+	hotplug->kobj.parent = hotplug_kobj;
+	error = kobject_add(&hotplug->kobj, hotplug_kobj, NULL);
+	if (error)
+		goto err_out;
+
+	kobject_uevent(&hotplug->kobj, KOBJ_ADD);
+	return;
+
+ err_out:
+	pr_err(PREFIX "Unable to add hotplug profile '%s'\n", name);
+}
+
 int __init acpi_sysfs_init(void)
 {
 	int result;
@@ -723,6 +787,8 @@ int __init acpi_sysfs_init(void)
 	result = acpi_tables_sysfs_init();
 	if (result)
 		return result;
+
+	hotplug_kobj = kobject_create_and_add("hotplug", acpi_kobj);
 	result = sysfs_create_file(acpi_kobj, &pm_profile_attr.attr);
 	return result;
 }
diff --git a/drivers/acpi/thermal.c b/drivers/acpi/thermal.c
index 8470771e5eae..a33821ca3895 100644
--- a/drivers/acpi/thermal.c
+++ b/drivers/acpi/thermal.c
@@ -723,9 +723,19 @@ static int thermal_get_trend(struct thermal_zone_device *thermal,
 		return -EINVAL;
 
 	if (type == THERMAL_TRIP_ACTIVE) {
-		/* aggressive active cooling */
-		*trend = THERMAL_TREND_RAISING;
-		return 0;
+		unsigned long trip_temp;
+		unsigned long temp = KELVIN_TO_MILLICELSIUS(tz->temperature,
+							tz->kelvin_offset);
+		if (thermal_get_trip_temp(thermal, trip, &trip_temp))
+			return -EINVAL;
+
+		if (temp > trip_temp) {
+			*trend = THERMAL_TREND_RAISING;
+			return 0;
+		} else {
+			/* Fall back on default trend */
+			return -EINVAL;
+		}
 	}
 
 	/*
diff --git a/drivers/acpi/video.c b/drivers/acpi/video.c
index 313f959413dc..c3932d0876e0 100644
--- a/drivers/acpi/video.c
+++ b/drivers/acpi/video.c
@@ -167,7 +167,8 @@ struct acpi_video_device_flags {
 	u8 dvi:1;
 	u8 bios:1;
 	u8 unknown:1;
-	u8 reserved:2;
+	u8 notify:1;
+	u8 reserved:1;
 };
 
 struct acpi_video_device_cap {
@@ -222,7 +223,7 @@ static int acpi_video_device_lcd_set_level(struct acpi_video_device *device,
 			int level);
 static int acpi_video_device_lcd_get_level_current(
 			struct acpi_video_device *device,
-			unsigned long long *level, int init);
+			unsigned long long *level, bool raw);
 static int acpi_video_get_next_level(struct acpi_video_device *device,
 				     u32 level_current, u32 event);
 static int acpi_video_switch_brightness(struct acpi_video_device *device,
@@ -236,7 +237,7 @@ static int acpi_video_get_brightness(struct backlight_device *bd)
 	struct acpi_video_device *vd =
 		(struct acpi_video_device *)bl_get_data(bd);
 
-	if (acpi_video_device_lcd_get_level_current(vd, &cur_level, 0))
+	if (acpi_video_device_lcd_get_level_current(vd, &cur_level, false))
 		return -EINVAL;
 	for (i = 2; i < vd->brightness->count; i++) {
 		if (vd->brightness->levels[i] == cur_level)
@@ -281,7 +282,7 @@ static int video_get_cur_state(struct thermal_cooling_device *cooling_dev, unsig
 	unsigned long long level;
 	int offset;
 
-	if (acpi_video_device_lcd_get_level_current(video, &level, 0))
+	if (acpi_video_device_lcd_get_level_current(video, &level, false))
 		return -EINVAL;
 	for (offset = 2; offset < video->brightness->count; offset++)
 		if (level == video->brightness->levels[offset]) {
@@ -447,12 +448,45 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 		DMI_MATCH(DMI_PRODUCT_NAME, "HP Folio 13 - 2000 Notebook PC"),
 		},
 	},
+	{
+	 .callback = video_ignore_initial_backlight,
+	 .ident = "HP Pavilion dm4",
+	 .matches = {
+		DMI_MATCH(DMI_BOARD_VENDOR, "Hewlett-Packard"),
+		DMI_MATCH(DMI_PRODUCT_NAME, "HP Pavilion dm4 Notebook PC"),
+		},
+	},
 	{}
 };
 
+static unsigned long long
+acpi_video_bqc_value_to_level(struct acpi_video_device *device,
+			      unsigned long long bqc_value)
+{
+	unsigned long long level;
+
+	if (device->brightness->flags._BQC_use_index) {
+		/*
+		 * _BQC returns an index that doesn't account for
+		 * the first 2 items with special meaning, so we need
+		 * to compensate for that by offsetting ourselves
+		 */
+		if (device->brightness->flags._BCL_reversed)
+			bqc_value = device->brightness->count - 3 - bqc_value;
+
+		level = device->brightness->levels[bqc_value + 2];
+	} else {
+		level = bqc_value;
+	}
+
+	level += bqc_offset_aml_bug_workaround;
+
+	return level;
+}
+
 static int
 acpi_video_device_lcd_get_level_current(struct acpi_video_device *device,
-					unsigned long long *level, int init)
+					unsigned long long *level, bool raw)
 {
 	acpi_status status = AE_OK;
 	int i;
@@ -463,29 +497,30 @@ acpi_video_device_lcd_get_level_current(struct acpi_video_device *device,
 		status = acpi_evaluate_integer(device->dev->handle, buf,
 						NULL, level);
 		if (ACPI_SUCCESS(status)) {
-			if (device->brightness->flags._BQC_use_index) {
-				if (device->brightness->flags._BCL_reversed)
-					*level = device->brightness->count
-								 - 3 - (*level);
-				*level = device->brightness->levels[*level + 2];
-
+			if (raw) {
+				/*
+				 * Caller has indicated he wants the raw
+				 * value returned by _BQC, so don't furtherly
+				 * mess with the value.
+				 */
+				return 0;
 			}
-			*level += bqc_offset_aml_bug_workaround;
+
+			*level = acpi_video_bqc_value_to_level(device, *level);
+
 			for (i = 2; i < device->brightness->count; i++)
 				if (device->brightness->levels[i] == *level) {
 					device->brightness->curr = *level;
 					return 0;
 			}
-			if (!init) {
-				/*
-				 * BQC returned an invalid level.
-				 * Stop using it.
-				 */
-				ACPI_WARNING((AE_INFO,
-					      "%s returned an invalid level",
-					      buf));
-				device->cap._BQC = device->cap._BCQ = 0;
-			}
+			/*
+			 * BQC returned an invalid level.
+			 * Stop using it.
+			 */
+			ACPI_WARNING((AE_INFO,
+				      "%s returned an invalid level",
+				      buf));
+			device->cap._BQC = device->cap._BCQ = 0;
 		} else {
 			/* Fixme:
 			 * should we return an error or ignore this failure?
@@ -598,6 +633,56 @@ acpi_video_cmp_level(const void *a, const void *b)
 }
 
 /*
+ * Decides if _BQC/_BCQ for this system is usable
+ *
+ * We do this by changing the level first and then read out the current
+ * brightness level, if the value does not match, find out if it is using
+ * index. If not, clear the _BQC/_BCQ capability.
+ */
+static int acpi_video_bqc_quirk(struct acpi_video_device *device,
+				int max_level, int current_level)
+{
+	struct acpi_video_device_brightness *br = device->brightness;
+	int result;
+	unsigned long long level;
+	int test_level;
+
+	/* don't mess with existing known broken systems */
+	if (bqc_offset_aml_bug_workaround)
+		return 0;
+
+	/*
+	 * Some systems always report current brightness level as maximum
+	 * through _BQC, we need to test another value for them.
+	 */
+	test_level = current_level == max_level ? br->levels[2] : max_level;
+
+	result = acpi_video_device_lcd_set_level(device, test_level);
+	if (result)
+		return result;
+
+	result = acpi_video_device_lcd_get_level_current(device, &level, true);
+	if (result)
+		return result;
+
+	if (level != test_level) {
+		/* buggy _BQC found, need to find out if it uses index */
+		if (level < br->count) {
+			if (br->flags._BCL_reversed)
+				level = br->count - 3 - level;
+			if (br->levels[level + 2] == test_level)
+				br->flags._BQC_use_index = 1;
+		}
+
+		if (!br->flags._BQC_use_index)
+			device->cap._BQC = device->cap._BCQ = 0;
+	}
+
+	return 0;
+}
+
+
+/*
  *  Arg:	
  *  	device	: video output device (LCD, CRT, ..)
  *
@@ -703,42 +788,36 @@ acpi_video_init_brightness(struct acpi_video_device *device)
 	if (!device->cap._BQC)
 		goto set_level;
 
-	result = acpi_video_device_lcd_get_level_current(device, &level_old, 1);
-	if (result)
-		goto out_free_levels;
-
-	/*
-	 * Set the level to maximum and check if _BQC uses indexed value
-	 */
-	result = acpi_video_device_lcd_set_level(device, max_level);
+	result = acpi_video_device_lcd_get_level_current(device,
+							 &level_old, true);
 	if (result)
 		goto out_free_levels;
 
-	result = acpi_video_device_lcd_get_level_current(device, &level, 0);
+	result = acpi_video_bqc_quirk(device, max_level, level_old);
 	if (result)
 		goto out_free_levels;
+	/*
+	 * cap._BQC may get cleared due to _BQC is found to be broken
+	 * in acpi_video_bqc_quirk, so check again here.
+	 */
+	if (!device->cap._BQC)
+		goto set_level;
 
-	br->flags._BQC_use_index = (level == max_level ? 0 : 1);
-
-	if (!br->flags._BQC_use_index) {
+	if (use_bios_initial_backlight) {
+		level = acpi_video_bqc_value_to_level(device, level_old);
 		/*
-		 * Set the backlight to the initial state.
-		 * On some buggy laptops, _BQC returns an uninitialized value
-		 * when invoked for the first time, i.e. level_old is invalid.
-		 * set the backlight to max_level in this case
+		 * On some buggy laptops, _BQC returns an uninitialized
+		 * value when invoked for the first time, i.e.
+		 * level_old is invalid (no matter whether it's a level
+		 * or an index). Set the backlight to max_level in this case.
 		 */
-		if (use_bios_initial_backlight) {
-			for (i = 2; i < br->count; i++)
-				if (level_old == br->levels[i])
-					level = level_old;
-		}
-		goto set_level;
+		for (i = 2; i < br->count; i++)
+			if (level_old == br->levels[i])
+				break;
+		if (i == br->count)
+			level = max_level;
 	}
 
-	if (br->flags._BCL_reversed)
-		level_old = (br->count - 1) - level_old;
-	level = br->levels[level_old];
-
 set_level:
 	result = acpi_video_device_lcd_set_level(device, level);
 	if (result)
@@ -996,53 +1075,51 @@ acpi_video_bus_get_one_device(struct acpi_device *device,
 	struct acpi_video_device *data;
 	struct acpi_video_device_attrib* attribute;
 
-	if (!device || !video)
-		return -EINVAL;
-
 	status =
 	    acpi_evaluate_integer(device->handle, "_ADR", NULL, &device_id);
-	if (ACPI_SUCCESS(status)) {
-
-		data = kzalloc(sizeof(struct acpi_video_device), GFP_KERNEL);
-		if (!data)
-			return -ENOMEM;
-
-		strcpy(acpi_device_name(device), ACPI_VIDEO_DEVICE_NAME);
-		strcpy(acpi_device_class(device), ACPI_VIDEO_CLASS);
-		device->driver_data = data;
-
-		data->device_id = device_id;
-		data->video = video;
-		data->dev = device;
+	/* Some device omits _ADR, we skip them instead of fail */
+	if (ACPI_FAILURE(status))
+		return 0;
 
-		attribute = acpi_video_get_device_attr(video, device_id);
+	data = kzalloc(sizeof(struct acpi_video_device), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
-		if((attribute != NULL) && attribute->device_id_scheme) {
-			switch (attribute->display_type) {
-			case ACPI_VIDEO_DISPLAY_CRT:
-				data->flags.crt = 1;
-				break;
-			case ACPI_VIDEO_DISPLAY_TV:
-				data->flags.tvout = 1;
-				break;
-			case ACPI_VIDEO_DISPLAY_DVI:
-				data->flags.dvi = 1;
-				break;
-			case ACPI_VIDEO_DISPLAY_LCD:
-				data->flags.lcd = 1;
-				break;
-			default:
-				data->flags.unknown = 1;
-				break;
-			}
-			if(attribute->bios_can_detect)
-				data->flags.bios = 1;
-		} else {
-			/* Check for legacy IDs */
-			device_type = acpi_video_get_device_type(video,
-								 device_id);
-			/* Ignore bits 16 and 18-20 */
-			switch (device_type & 0xffe2ffff) {
+	strcpy(acpi_device_name(device), ACPI_VIDEO_DEVICE_NAME);
+	strcpy(acpi_device_class(device), ACPI_VIDEO_CLASS);
+	device->driver_data = data;
+
+	data->device_id = device_id;
+	data->video = video;
+	data->dev = device;
+
+	attribute = acpi_video_get_device_attr(video, device_id);
+
+	if((attribute != NULL) && attribute->device_id_scheme) {
+		switch (attribute->display_type) {
+		case ACPI_VIDEO_DISPLAY_CRT:
+			data->flags.crt = 1;
+			break;
+		case ACPI_VIDEO_DISPLAY_TV:
+			data->flags.tvout = 1;
+			break;
+		case ACPI_VIDEO_DISPLAY_DVI:
+			data->flags.dvi = 1;
+			break;
+		case ACPI_VIDEO_DISPLAY_LCD:
+			data->flags.lcd = 1;
+			break;
+		default:
+			data->flags.unknown = 1;
+			break;
+		}
+		if(attribute->bios_can_detect)
+			data->flags.bios = 1;
+	} else {
+		/* Check for legacy IDs */
+		device_type = acpi_video_get_device_type(video, device_id);
+		/* Ignore bits 16 and 18-20 */
+		switch (device_type & 0xffe2ffff) {
 			case ACPI_VIDEO_DISPLAY_LEGACY_MONITOR:
 				data->flags.crt = 1;
 				break;
@@ -1054,34 +1131,24 @@ acpi_video_bus_get_one_device(struct acpi_device *device,
 				break;
 			default:
 				data->flags.unknown = 1;
-			}
 		}
+	}
 
-		acpi_video_device_bind(video, data);
-		acpi_video_device_find_cap(data);
-
-		status = acpi_install_notify_handler(device->handle,
-						     ACPI_DEVICE_NOTIFY,
-						     acpi_video_device_notify,
-						     data);
-		if (ACPI_FAILURE(status)) {
-			printk(KERN_ERR PREFIX
-					  "Error installing notify handler\n");
-			if(data->brightness)
-				kfree(data->brightness->levels);
-			kfree(data->brightness);
-			kfree(data);
-			return -ENODEV;
-		}
+	acpi_video_device_bind(video, data);
+	acpi_video_device_find_cap(data);
 
-		mutex_lock(&video->device_list_lock);
-		list_add_tail(&data->entry, &video->video_device_list);
-		mutex_unlock(&video->device_list_lock);
+	status = acpi_install_notify_handler(device->handle, ACPI_DEVICE_NOTIFY,
+					     acpi_video_device_notify, data);
+	if (ACPI_FAILURE(status))
+		dev_err(&device->dev, "Error installing notify handler\n");
+	else
+		data->flags.notify = 1;
 
-		return 0;
-	}
+	mutex_lock(&video->device_list_lock);
+	list_add_tail(&data->entry, &video->video_device_list);
+	mutex_unlock(&video->device_list_lock);
 
-	return -ENOENT;
+	return status;
 }
 
 /*
@@ -1268,7 +1335,8 @@ acpi_video_switch_brightness(struct acpi_video_device *device, int event)
 		goto out;
 
 	result = acpi_video_device_lcd_get_level_current(device,
-							 &level_current, 0);
+							 &level_current,
+							 false);
 	if (result)
 		goto out;
 
@@ -1373,9 +1441,8 @@ acpi_video_bus_get_devices(struct acpi_video_bus *video,
 
 		status = acpi_video_bus_get_one_device(dev, video);
 		if (status) {
-			printk(KERN_WARNING PREFIX
-					"Can't attach device\n");
-			continue;
+			dev_err(&dev->dev, "Can't attach device\n");
+			break;
 		}
 	}
 	return status;
@@ -1388,13 +1455,14 @@ static int acpi_video_bus_put_one_device(struct acpi_video_device *device)
 	if (!device || !device->video)
 		return -ENOENT;
 
-	status = acpi_remove_notify_handler(device->dev->handle,
-					    ACPI_DEVICE_NOTIFY,
-					    acpi_video_device_notify);
-	if (ACPI_FAILURE(status)) {
-		printk(KERN_WARNING PREFIX
-		       "Can't remove video notify handler\n");
+	if (device->flags.notify) {
+		status = acpi_remove_notify_handler(device->dev->handle,
+				ACPI_DEVICE_NOTIFY, acpi_video_device_notify);
+		if (ACPI_FAILURE(status))
+			dev_err(&device->dev->dev,
+					"Can't remove video notify handler\n");
 	}
+
 	if (device->backlight) {
 		backlight_device_unregister(device->backlight);
 		device->backlight = NULL;
@@ -1676,7 +1744,7 @@ static int acpi_video_bus_add(struct acpi_device *device)
 
 	error = acpi_video_bus_get_devices(video, device);
 	if (error)
-		goto err_free_video;
+		goto err_put_video;
 
 	video->input = input = input_allocate_device();
 	if (!input) {
diff --git a/drivers/acpi/video_detect.c b/drivers/acpi/video_detect.c
index 4ac2593234e7..66f67626f02e 100644
--- a/drivers/acpi/video_detect.c
+++ b/drivers/acpi/video_detect.c
@@ -67,40 +67,37 @@ acpi_backlight_cap_match(acpi_handle handle, u32 level, void *context,
 	return 0;
 }
 
-/* Returns true if the device is a video device which can be handled by
- * video.ko.
+/* Returns true if the ACPI object is a video device which can be
+ * handled by video.ko.
  * The device will get a Linux specific CID added in scan.c to
  * identify the device as an ACPI graphics device
  * Be aware that the graphics device may not be physically present
  * Use acpi_video_get_capabilities() to detect general ACPI video
  * capabilities of present cards
  */
-long acpi_is_video_device(struct acpi_device *device)
+long acpi_is_video_device(acpi_handle handle)
 {
 	acpi_handle h_dummy;
 	long video_caps = 0;
 
-	if (!device)
-		return 0;
-
 	/* Is this device able to support video switching ? */
-	if (ACPI_SUCCESS(acpi_get_handle(device->handle, "_DOD", &h_dummy)) ||
-	    ACPI_SUCCESS(acpi_get_handle(device->handle, "_DOS", &h_dummy)))
+	if (ACPI_SUCCESS(acpi_get_handle(handle, "_DOD", &h_dummy)) ||
+	    ACPI_SUCCESS(acpi_get_handle(handle, "_DOS", &h_dummy)))
 		video_caps |= ACPI_VIDEO_OUTPUT_SWITCHING;
 
 	/* Is this device able to retrieve a video ROM ? */
-	if (ACPI_SUCCESS(acpi_get_handle(device->handle, "_ROM", &h_dummy)))
+	if (ACPI_SUCCESS(acpi_get_handle(handle, "_ROM", &h_dummy)))
 		video_caps |= ACPI_VIDEO_ROM_AVAILABLE;
 
 	/* Is this device able to configure which video head to be POSTed ? */
-	if (ACPI_SUCCESS(acpi_get_handle(device->handle, "_VPO", &h_dummy)) &&
-	    ACPI_SUCCESS(acpi_get_handle(device->handle, "_GPD", &h_dummy)) &&
-	    ACPI_SUCCESS(acpi_get_handle(device->handle, "_SPD", &h_dummy)))
+	if (ACPI_SUCCESS(acpi_get_handle(handle, "_VPO", &h_dummy)) &&
+	    ACPI_SUCCESS(acpi_get_handle(handle, "_GPD", &h_dummy)) &&
+	    ACPI_SUCCESS(acpi_get_handle(handle, "_SPD", &h_dummy)))
 		video_caps |= ACPI_VIDEO_DEVICE_POSTING;
 
 	/* Only check for backlight functionality if one of the above hit. */
 	if (video_caps)
-		acpi_walk_namespace(ACPI_TYPE_DEVICE, device->handle,
+		acpi_walk_namespace(ACPI_TYPE_DEVICE, handle,
 				    ACPI_UINT32_MAX, acpi_backlight_cap_match, NULL,
 				    &video_caps, NULL);
 
@@ -127,7 +124,7 @@ find_video(acpi_handle handle, u32 lvl, void *context, void **rv)
 		if (!dev)
 			return AE_OK;
 		pci_dev_put(dev);
-		*cap |= acpi_is_video_device(acpi_dev);
+		*cap |= acpi_is_video_device(handle);
 	}
 	return AE_OK;
 }
diff --git a/drivers/ata/libata-acpi.c b/drivers/ata/libata-acpi.c
index 8a52dab412e2..87f2f395d79a 100644
--- a/drivers/ata/libata-acpi.c
+++ b/drivers/ata/libata-acpi.c
@@ -17,7 +17,6 @@
 #include <linux/pci.h>
 #include <linux/slab.h>
 #include <linux/pm_runtime.h>
-#include <linux/pm_qos.h>
 #include <scsi/scsi_device.h>
 #include "libata.h"
 
@@ -61,7 +60,8 @@ acpi_handle ata_ap_acpi_handle(struct ata_port *ap)
 	if (ap->flags & ATA_FLAG_ACPI_SATA)
 		return NULL;
 
-	return acpi_get_child(DEVICE_ACPI_HANDLE(ap->host->dev), ap->port_no);
+	return ap->scsi_host ?
+		DEVICE_ACPI_HANDLE(&ap->scsi_host->shost_gendev) : NULL;
 }
 EXPORT_SYMBOL(ata_ap_acpi_handle);
 
@@ -77,7 +77,7 @@ acpi_handle ata_dev_acpi_handle(struct ata_device *dev)
 	acpi_integer adr;
 	struct ata_port *ap = dev->link->ap;
 
-	if (dev->flags & ATA_DFLAG_ACPI_DISABLED)
+	if (libata_noacpi || dev->flags & ATA_DFLAG_ACPI_DISABLED)
 		return NULL;
 
 	if (ap->flags & ATA_FLAG_ACPI_SATA) {
@@ -240,28 +240,15 @@ void ata_acpi_dissociate(struct ata_host *host)
 	}
 }
 
-/**
- * ata_acpi_gtm - execute _GTM
- * @ap: target ATA port
- * @gtm: out parameter for _GTM result
- *
- * Evaluate _GTM and store the result in @gtm.
- *
- * LOCKING:
- * EH context.
- *
- * RETURNS:
- * 0 on success, -ENOENT if _GTM doesn't exist, -errno on failure.
- */
-int ata_acpi_gtm(struct ata_port *ap, struct ata_acpi_gtm *gtm)
+static int __ata_acpi_gtm(struct ata_port *ap, acpi_handle handle,
+			  struct ata_acpi_gtm *gtm)
 {
 	struct acpi_buffer output = { .length = ACPI_ALLOCATE_BUFFER };
 	union acpi_object *out_obj;
 	acpi_status status;
 	int rc = 0;
 
-	status = acpi_evaluate_object(ata_ap_acpi_handle(ap), "_GTM", NULL,
-				      &output);
+	status = acpi_evaluate_object(handle, "_GTM", NULL, &output);
 
 	rc = -ENOENT;
 	if (status == AE_NOT_FOUND)
@@ -295,6 +282,27 @@ int ata_acpi_gtm(struct ata_port *ap, struct ata_acpi_gtm *gtm)
 	return rc;
 }
 
+/**
+ * ata_acpi_gtm - execute _GTM
+ * @ap: target ATA port
+ * @gtm: out parameter for _GTM result
+ *
+ * Evaluate _GTM and store the result in @gtm.
+ *
+ * LOCKING:
+ * EH context.
+ *
+ * RETURNS:
+ * 0 on success, -ENOENT if _GTM doesn't exist, -errno on failure.
+ */
+int ata_acpi_gtm(struct ata_port *ap, struct ata_acpi_gtm *gtm)
+{
+	if (ata_ap_acpi_handle(ap))
+		return __ata_acpi_gtm(ap, ata_ap_acpi_handle(ap), gtm);
+	else
+		return -EINVAL;
+}
+
 EXPORT_SYMBOL_GPL(ata_acpi_gtm);
 
 /**
@@ -1020,38 +1028,6 @@ void ata_acpi_on_disable(struct ata_device *dev)
 	ata_acpi_clear_gtf(dev);
 }
 
-static void ata_acpi_register_power_resource(struct ata_device *dev)
-{
-	struct scsi_device *sdev = dev->sdev;
-	acpi_handle handle;
-
-	handle = ata_dev_acpi_handle(dev);
-	if (handle)
-		acpi_dev_pm_add_dependent(handle, &sdev->sdev_gendev);
-}
-
-static void ata_acpi_unregister_power_resource(struct ata_device *dev)
-{
-	struct scsi_device *sdev = dev->sdev;
-	acpi_handle handle;
-
-	handle = ata_dev_acpi_handle(dev);
-	if (handle)
-		acpi_dev_pm_remove_dependent(handle, &sdev->sdev_gendev);
-}
-
-void ata_acpi_bind(struct ata_device *dev)
-{
-	ata_acpi_register_power_resource(dev);
-	if (zpodd_dev_enabled(dev))
-		dev_pm_qos_expose_flags(&dev->sdev->sdev_gendev, 0);
-}
-
-void ata_acpi_unbind(struct ata_device *dev)
-{
-	ata_acpi_unregister_power_resource(dev);
-}
-
 static int compat_pci_ata(struct ata_port *ap)
 {
 	struct device *dev = ap->tdev.parent;
@@ -1071,7 +1047,7 @@ static int compat_pci_ata(struct ata_port *ap)
 
 static int ata_acpi_bind_host(struct ata_port *ap, acpi_handle *handle)
 {
-	if (ap->flags & ATA_FLAG_ACPI_SATA)
+	if (libata_noacpi || ap->flags & ATA_FLAG_ACPI_SATA)
 		return -ENODEV;
 
 	*handle = acpi_get_child(DEVICE_ACPI_HANDLE(ap->tdev.parent),
@@ -1080,7 +1056,7 @@ static int ata_acpi_bind_host(struct ata_port *ap, acpi_handle *handle)
 	if (!*handle)
 		return -ENODEV;
 
-	if (ata_acpi_gtm(ap, &ap->__acpi_init_gtm) == 0)
+	if (__ata_acpi_gtm(ap, *handle, &ap->__acpi_init_gtm) == 0)
 		ap->pflags |= ATA_PFLAG_INIT_GTM_VALID;
 
 	return 0;
diff --git a/drivers/ata/libata-scsi.c b/drivers/ata/libata-scsi.c
index ff44787e5a45..dd310b27b24c 100644
--- a/drivers/ata/libata-scsi.c
+++ b/drivers/ata/libata-scsi.c
@@ -49,6 +49,7 @@
 #include <linux/hdreg.h>
 #include <linux/uaccess.h>
 #include <linux/suspend.h>
+#include <linux/pm_qos.h>
 #include <asm/unaligned.h>
 
 #include "libata.h"
@@ -2126,7 +2127,6 @@ static unsigned int ata_scsiop_inq_89(struct ata_scsi_args *args, u8 *rbuf)
 	memcpy(&rbuf[8], "linux   ", 8);
 	memcpy(&rbuf[16], "libata          ", 16);
 	memcpy(&rbuf[32], DRV_VERSION, 4);
-	ata_id_string(args->id, &rbuf[32], ATA_ID_FW_REV, 4);
 
 	/* we don't store the ATA device signature, so we fake it */
 
@@ -3668,7 +3668,9 @@ void ata_scsi_scan_host(struct ata_port *ap, int sync)
 			if (!IS_ERR(sdev)) {
 				dev->sdev = sdev;
 				scsi_device_put(sdev);
-				ata_acpi_bind(dev);
+				if (zpodd_dev_enabled(dev))
+					dev_pm_qos_expose_flags(
+							&sdev->sdev_gendev, 0);
 			} else {
 				dev->sdev = NULL;
 			}
@@ -3767,7 +3769,6 @@ static void ata_scsi_remove_dev(struct ata_device *dev)
 
 	if (zpodd_dev_enabled(dev))
 		zpodd_exit(dev);
-	ata_acpi_unbind(dev);
 
 	/* clearing dev->sdev is protected by host lock */
 	sdev = dev->sdev;
diff --git a/drivers/ata/pata_arasan_cf.c b/drivers/ata/pata_arasan_cf.c
index 405022d302c3..7638121cb5d1 100644
--- a/drivers/ata/pata_arasan_cf.c
+++ b/drivers/ata/pata_arasan_cf.c
@@ -209,8 +209,6 @@ struct arasan_cf_dev {
 	struct dma_chan *dma_chan;
 	/* Mask for DMA transfers */
 	dma_cap_mask_t mask;
-	/* dma channel private data */
-	void *dma_priv;
 	/* DMA transfer work */
 	struct work_struct work;
 	/* DMA delayed finish work */
@@ -308,6 +306,7 @@ static void cf_card_detect(struct arasan_cf_dev *acdev, bool hotplugged)
 static int cf_init(struct arasan_cf_dev *acdev)
 {
 	struct arasan_cf_pdata *pdata = dev_get_platdata(acdev->host->dev);
+	unsigned int if_clk;
 	unsigned long flags;
 	int ret = 0;
 
@@ -325,8 +324,12 @@ static int cf_init(struct arasan_cf_dev *acdev)
 
 	spin_lock_irqsave(&acdev->host->lock, flags);
 	/* configure CF interface clock */
-	writel((pdata->cf_if_clk <= CF_IF_CLK_200M) ? pdata->cf_if_clk :
-			CF_IF_CLK_166M, acdev->vbase + CLK_CFG);
+	/* TODO: read from device tree */
+	if_clk = CF_IF_CLK_166M;
+	if (pdata && pdata->cf_if_clk <= CF_IF_CLK_200M)
+		if_clk = pdata->cf_if_clk;
+
+	writel(if_clk, acdev->vbase + CLK_CFG);
 
 	writel(TRUE_IDE_MODE | CFHOST_ENB, acdev->vbase + OP_MODE);
 	cf_interrupt_enable(acdev, CARD_DETECT_IRQ, 1);
@@ -357,12 +360,6 @@ static void dma_callback(void *dev)
 	complete(&acdev->dma_completion);
 }
 
-static bool filter(struct dma_chan *chan, void *slave)
-{
-	chan->private = slave;
-	return true;
-}
-
 static inline void dma_complete(struct arasan_cf_dev *acdev)
 {
 	struct ata_queued_cmd *qc = acdev->qc;
@@ -530,8 +527,7 @@ static void data_xfer(struct work_struct *work)
 
 	/* request dma channels */
 	/* dma_request_channel may sleep, so calling from process context */
-	acdev->dma_chan = dma_request_channel(acdev->mask, filter,
-			acdev->dma_priv);
+	acdev->dma_chan = dma_request_slave_channel(acdev->host->dev, "data");
 	if (!acdev->dma_chan) {
 		dev_err(acdev->host->dev, "Unable to get dma_chan\n");
 		goto chan_request_fail;
@@ -798,6 +794,7 @@ static int arasan_cf_probe(struct platform_device *pdev)
 	struct ata_host *host;
 	struct ata_port *ap;
 	struct resource *res;
+	u32 quirk;
 	irq_handler_t irq_handler = NULL;
 	int ret = 0;
 
@@ -817,12 +814,17 @@ static int arasan_cf_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
+	if (pdata)
+		quirk = pdata->quirk;
+	else
+		quirk = CF_BROKEN_UDMA; /* as it is on spear1340 */
+
 	/* if irq is 0, support only PIO */
 	acdev->irq = platform_get_irq(pdev, 0);
 	if (acdev->irq)
 		irq_handler = arasan_cf_interrupt;
 	else
-		pdata->quirk |= CF_BROKEN_MWDMA | CF_BROKEN_UDMA;
+		quirk |= CF_BROKEN_MWDMA | CF_BROKEN_UDMA;
 
 	acdev->pbase = res->start;
 	acdev->vbase = devm_ioremap_nocache(&pdev->dev, res->start,
@@ -859,17 +861,16 @@ static int arasan_cf_probe(struct platform_device *pdev)
 	INIT_WORK(&acdev->work, data_xfer);
 	INIT_DELAYED_WORK(&acdev->dwork, delayed_finish);
 	dma_cap_set(DMA_MEMCPY, acdev->mask);
-	acdev->dma_priv = pdata->dma_priv;
 
 	/* Handle platform specific quirks */
-	if (pdata->quirk) {
-		if (pdata->quirk & CF_BROKEN_PIO) {
+	if (quirk) {
+		if (quirk & CF_BROKEN_PIO) {
 			ap->ops->set_piomode = NULL;
 			ap->pio_mask = 0;
 		}
-		if (pdata->quirk & CF_BROKEN_MWDMA)
+		if (quirk & CF_BROKEN_MWDMA)
 			ap->mwdma_mask = 0;
-		if (pdata->quirk & CF_BROKEN_UDMA)
+		if (quirk & CF_BROKEN_UDMA)
 			ap->udma_mask = 0;
 	}
 	ap->flags |= ATA_FLAG_PIO_POLLING | ATA_FLAG_NO_ATAPI;
diff --git a/drivers/ata/pata_at32.c b/drivers/ata/pata_at32.c
index 36f189c7ee8c..8d493b4a0961 100644
--- a/drivers/ata/pata_at32.c
+++ b/drivers/ata/pata_at32.c
@@ -393,18 +393,7 @@ static struct platform_driver pata_at32_driver = {
 	},
 };
 
-static int __init pata_at32_init(void)
-{
-	return platform_driver_probe(&pata_at32_driver, pata_at32_probe);
-}
-
-static void __exit pata_at32_exit(void)
-{
-	platform_driver_unregister(&pata_at32_driver);
-}
-
-module_init(pata_at32_init);
-module_exit(pata_at32_exit);
+module_platform_driver_probe(pata_at32_driver, pata_at32_probe);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("AVR32 SMC/CFC PATA Driver");
diff --git a/drivers/ata/pata_imx.c b/drivers/ata/pata_imx.c
index 40849445a9dc..aa3d166e02eb 100644
--- a/drivers/ata/pata_imx.c
+++ b/drivers/ata/pata_imx.c
@@ -37,7 +37,7 @@
 struct pata_imx_priv {
 	struct clk *clk;
 	/* timings/interrupt/control regs */
-	u8 *host_regs;
+	void __iomem *host_regs;
 	u32 ata_ctl;
 };
 
@@ -98,6 +98,7 @@ static int pata_imx_probe(struct platform_device *pdev)
 	struct pata_imx_priv *priv;
 	int irq = 0;
 	struct resource *io_res;
+	int ret;
 
 	io_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (io_res == NULL)
@@ -112,7 +113,7 @@ static int pata_imx_probe(struct platform_device *pdev)
 	if (!priv)
 		return -ENOMEM;
 
-	priv->clk = clk_get(&pdev->dev, NULL);
+	priv->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(priv->clk)) {
 		dev_err(&pdev->dev, "Failed to get clock\n");
 		return PTR_ERR(priv->clk);
@@ -121,8 +122,10 @@ static int pata_imx_probe(struct platform_device *pdev)
 	clk_prepare_enable(priv->clk);
 
 	host = ata_host_alloc(&pdev->dev, 1);
-	if (!host)
-		goto free_priv;
+	if (!host) {
+		ret = -ENOMEM;
+		goto err;
+	}
 
 	host->private_data = priv;
 	ap = host->ports[0];
@@ -135,7 +138,8 @@ static int pata_imx_probe(struct platform_device *pdev)
 		resource_size(io_res));
 	if (!priv->host_regs) {
 		dev_err(&pdev->dev, "failed to map IO/CTL base\n");
-		goto free_priv;
+		ret = -EBUSY;
+		goto err;
 	}
 
 	ap->ioaddr.cmd_addr = priv->host_regs + PATA_IMX_DRIVE_DATA;
@@ -158,13 +162,17 @@ static int pata_imx_probe(struct platform_device *pdev)
 			priv->host_regs + PATA_IMX_ATA_INT_EN);
 
 	/* activate */
-	return ata_host_activate(host, irq, ata_sff_interrupt, 0,
+	ret = ata_host_activate(host, irq, ata_sff_interrupt, 0,
 				&pata_imx_sht);
 
-free_priv:
+	if (ret)
+		goto err;
+
+	return 0;
+err:
 	clk_disable_unprepare(priv->clk);
-	clk_put(priv->clk);
-	return -ENOMEM;
+
+	return ret;
 }
 
 static int pata_imx_remove(struct platform_device *pdev)
@@ -177,7 +185,6 @@ static int pata_imx_remove(struct platform_device *pdev)
 	__raw_writel(0, priv->host_regs + PATA_IMX_ATA_INT_EN);
 
 	clk_disable_unprepare(priv->clk);
-	clk_put(priv->clk);
 
 	return 0;
 }
@@ -223,11 +230,20 @@ static const struct dev_pm_ops pata_imx_pm_ops = {
 };
 #endif
 
+static const struct of_device_id imx_pata_dt_ids[] = {
+	{
+		.compatible = "fsl,imx27-pata",
+	}, {
+		/* sentinel */
+	}
+};
+
 static struct platform_driver pata_imx_driver = {
 	.probe		= pata_imx_probe,
 	.remove		= pata_imx_remove,
 	.driver = {
 		.name		= DRV_NAME,
+		.of_match_table	= imx_pata_dt_ids,
 		.owner		= THIS_MODULE,
 #ifdef CONFIG_PM
 		.pm		= &pata_imx_pm_ops,
diff --git a/drivers/ata/pata_legacy.c b/drivers/ata/pata_legacy.c
index 4fe9d2138d48..be816428b430 100644
--- a/drivers/ata/pata_legacy.c
+++ b/drivers/ata/pata_legacy.c
@@ -542,7 +542,7 @@ static void opti82c46x_set_piomode(struct ata_port *ap, struct ata_device *adev)
 	u8 sysclk;
 
 	/* Get the clock */
-	sysclk = opti_syscfg(0xAC) & 0xC0;	/* BIOS set */
+	sysclk = (opti_syscfg(0xAC) & 0xC0) >> 6;	/* BIOS set */
 
 	/* Enter configuration mode */
 	ioread16(ap->ioaddr.error_addr);
diff --git a/drivers/ata/pata_octeon_cf.c b/drivers/ata/pata_octeon_cf.c
index ff2e57f3b597..e73bef3093d2 100644
--- a/drivers/ata/pata_octeon_cf.c
+++ b/drivers/ata/pata_octeon_cf.c
@@ -926,7 +926,7 @@ static int octeon_cf_probe(struct platform_device *pdev)
 			goto free_cf_port;
 		}
 		cs1 = devm_ioremap_nocache(&pdev->dev, res_cs1->start,
-					   res_cs1->end - res_cs1->start + 1);
+					   resource_size(res_cs1));
 
 		if (!cs1)
 			goto free_cf_port;
diff --git a/drivers/ata/sata_fsl.c b/drivers/ata/sata_fsl.c
index dfc6fd0ac119..d40e403e82dd 100644
--- a/drivers/ata/sata_fsl.c
+++ b/drivers/ata/sata_fsl.c
@@ -285,6 +285,7 @@ struct sata_fsl_host_priv {
 	int irq;
 	int data_snoop;
 	struct device_attribute intr_coalescing;
+	struct device_attribute rx_watermark;
 };
 
 static void fsl_sata_set_irq_coalescing(struct ata_host *host,
@@ -343,6 +344,48 @@ static ssize_t fsl_sata_intr_coalescing_store(struct device *dev,
 	return strlen(buf);
 }
 
+static ssize_t fsl_sata_rx_watermark_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	unsigned int rx_watermark;
+	unsigned long flags;
+	struct ata_host *host = dev_get_drvdata(dev);
+	struct sata_fsl_host_priv *host_priv = host->private_data;
+	void __iomem *csr_base = host_priv->csr_base;
+
+	spin_lock_irqsave(&host->lock, flags);
+	rx_watermark = ioread32(csr_base + TRANSCFG);
+	rx_watermark &= 0x1f;
+
+	spin_unlock_irqrestore(&host->lock, flags);
+	return sprintf(buf, "%d\n", rx_watermark);
+}
+
+static ssize_t fsl_sata_rx_watermark_store(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	unsigned int rx_watermark;
+	unsigned long flags;
+	struct ata_host *host = dev_get_drvdata(dev);
+	struct sata_fsl_host_priv *host_priv = host->private_data;
+	void __iomem *csr_base = host_priv->csr_base;
+	u32 temp;
+
+	if (sscanf(buf, "%d", &rx_watermark) != 1) {
+		printk(KERN_ERR "fsl-sata: wrong parameter format.\n");
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&host->lock, flags);
+	temp = ioread32(csr_base + TRANSCFG);
+	temp &= 0xffffffe0;
+	iowrite32(temp | rx_watermark, csr_base + TRANSCFG);
+
+	spin_unlock_irqrestore(&host->lock, flags);
+	return strlen(buf);
+}
+
 static inline unsigned int sata_fsl_tag(unsigned int tag,
 					void __iomem *hcr_base)
 {
@@ -1500,6 +1543,17 @@ static int sata_fsl_probe(struct platform_device *ofdev)
 	if (retval)
 		goto error_exit_with_cleanup;
 
+	host_priv->rx_watermark.show = fsl_sata_rx_watermark_show;
+	host_priv->rx_watermark.store = fsl_sata_rx_watermark_store;
+	sysfs_attr_init(&host_priv->rx_watermark.attr);
+	host_priv->rx_watermark.attr.name = "rx_watermark";
+	host_priv->rx_watermark.attr.mode = S_IRUGO | S_IWUSR;
+	retval = device_create_file(host->dev, &host_priv->rx_watermark);
+	if (retval) {
+		device_remove_file(&ofdev->dev, &host_priv->intr_coalescing);
+		goto error_exit_with_cleanup;
+	}
+
 	return 0;
 
 error_exit_with_cleanup:
@@ -1522,6 +1576,7 @@ static int sata_fsl_remove(struct platform_device *ofdev)
 	struct sata_fsl_host_priv *host_priv = host->private_data;
 
 	device_remove_file(&ofdev->dev, &host_priv->intr_coalescing);
+	device_remove_file(&ofdev->dev, &host_priv->rx_watermark);
 
 	ata_host_detach(host);
 
diff --git a/drivers/ata/sata_highbank.c b/drivers/ata/sata_highbank.c
index 5dba77ccaa0b..b20aa96b958d 100644
--- a/drivers/ata/sata_highbank.c
+++ b/drivers/ata/sata_highbank.c
@@ -251,7 +251,7 @@ static const struct ata_port_info ahci_highbank_port_info = {
 };
 
 static struct scsi_host_template ahci_highbank_platform_sht = {
-	AHCI_SHT("highbank-ahci"),
+	AHCI_SHT("sata_highbank"),
 };
 
 static const struct of_device_id ahci_of_match[] = {
@@ -418,7 +418,7 @@ static int ahci_highbank_resume(struct device *dev)
 }
 #endif
 
-SIMPLE_DEV_PM_OPS(ahci_highbank_pm_ops,
+static SIMPLE_DEV_PM_OPS(ahci_highbank_pm_ops,
 		  ahci_highbank_suspend, ahci_highbank_resume);
 
 static struct platform_driver ahci_highbank_driver = {
diff --git a/drivers/ata/sata_rcar.c b/drivers/ata/sata_rcar.c
index caf33f620c35..4799868bd733 100644
--- a/drivers/ata/sata_rcar.c
+++ b/drivers/ata/sata_rcar.c
@@ -17,6 +17,7 @@
 #include <linux/libata.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
+#include <linux/err.h>
 
 #define DRV_NAME "sata_rcar"
 
@@ -799,9 +800,9 @@ static int sata_rcar_probe(struct platform_device *pdev)
 
 	host->private_data = priv;
 
-	priv->base = devm_request_and_ioremap(&pdev->dev, mem);
-	if (!priv->base) {
-		ret = -EADDRNOTAVAIL;
+	priv->base = devm_ioremap_resource(&pdev->dev, mem);
+	if (IS_ERR(priv->base)) {
+		ret = PTR_ERR(priv->base);
 		goto cleanup;
 	}
 
diff --git a/drivers/ata/sata_svw.c b/drivers/ata/sata_svw.c
index 08608de87e4e..dc4f70179e7d 100644
--- a/drivers/ata/sata_svw.c
+++ b/drivers/ata/sata_svw.c
@@ -322,23 +322,11 @@ static u8 k2_stat_check_status(struct ata_port *ap)
 }
 
 #ifdef CONFIG_PPC_OF
-/*
- * k2_sata_proc_info
- * inout : decides on the direction of the dataflow and the meaning of the
- *	   variables
- * buffer: If inout==FALSE data is being written to it else read from it
- * *start: If inout==FALSE start of the valid data in the buffer
- * offset: If inout==FALSE offset from the beginning of the imaginary file
- *	   from which we start writing into the buffer
- * length: If inout==FALSE max number of bytes to be written into the buffer
- *	   else number of bytes in the buffer
- */
-static int k2_sata_proc_info(struct Scsi_Host *shost, char *page, char **start,
-			     off_t offset, int count, int inout)
+static int k2_sata_show_info(struct seq_file *m, struct Scsi_Host *shost)
 {
 	struct ata_port *ap;
 	struct device_node *np;
-	int len, index;
+	int index;
 
 	/* Find  the ata_port */
 	ap = ata_shost_to_port(shost);
@@ -356,15 +344,12 @@ static int k2_sata_proc_info(struct Scsi_Host *shost, char *page, char **start,
 		const u32 *reg = of_get_property(np, "reg", NULL);
 		if (!reg)
 			continue;
-		if (index == *reg)
+		if (index == *reg) {
+			seq_printf(m, "devspec: %s\n", np->full_name);
 			break;
+		}
 	}
-	if (np == NULL)
-		return 0;
-
-	len = sprintf(page, "devspec: %s\n", np->full_name);
-
-	return len;
+	return 0;
 }
 #endif /* CONFIG_PPC_OF */
 
@@ -372,7 +357,7 @@ static int k2_sata_proc_info(struct Scsi_Host *shost, char *page, char **start,
 static struct scsi_host_template k2_sata_sht = {
 	ATA_BMDMA_SHT(DRV_NAME),
 #ifdef CONFIG_PPC_OF
-	.proc_info		= k2_sata_proc_info,
+	.show_info		= k2_sata_show_info,
 #endif
 };
 
diff --git a/drivers/atm/he.c b/drivers/atm/he.c
index d6891267f5bb..507362a76a73 100644
--- a/drivers/atm/he.c
+++ b/drivers/atm/he.c
@@ -1055,7 +1055,7 @@ static int he_start(struct atm_dev *dev)
 	he_writel(he_dev, 0x0, RESET_CNTL);
 	he_writel(he_dev, 0xff, RESET_CNTL);
 
-	udelay(16*1000);	/* 16 ms */
+	msleep(16);	/* 16 ms */
 	status = he_readl(he_dev, RESET_CNTL);
 	if ((status & BOARD_RST_STATUS) == 0) {
 		hprintk("reset failed\n");
diff --git a/drivers/base/devres.c b/drivers/base/devres.c
index 668390664764..507379e7b763 100644
--- a/drivers/base/devres.c
+++ b/drivers/base/devres.c
@@ -671,6 +671,80 @@ int devres_release_group(struct device *dev, void *id)
 EXPORT_SYMBOL_GPL(devres_release_group);
 
 /*
+ * Custom devres actions allow inserting a simple function call
+ * into the teadown sequence.
+ */
+
+struct action_devres {
+	void *data;
+	void (*action)(void *);
+};
+
+static int devm_action_match(struct device *dev, void *res, void *p)
+{
+	struct action_devres *devres = res;
+	struct action_devres *target = p;
+
+	return devres->action == target->action &&
+	       devres->data == target->data;
+}
+
+static void devm_action_release(struct device *dev, void *res)
+{
+	struct action_devres *devres = res;
+
+	devres->action(devres->data);
+}
+
+/**
+ * devm_add_action() - add a custom action to list of managed resources
+ * @dev: Device that owns the action
+ * @action: Function that should be called
+ * @data: Pointer to data passed to @action implementation
+ *
+ * This adds a custom action to the list of managed resources so that
+ * it gets executed as part of standard resource unwinding.
+ */
+int devm_add_action(struct device *dev, void (*action)(void *), void *data)
+{
+	struct action_devres *devres;
+
+	devres = devres_alloc(devm_action_release,
+			      sizeof(struct action_devres), GFP_KERNEL);
+	if (!devres)
+		return -ENOMEM;
+
+	devres->data = data;
+	devres->action = action;
+
+	devres_add(dev, devres);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(devm_add_action);
+
+/**
+ * devm_remove_action() - removes previously added custom action
+ * @dev: Device that owns the action
+ * @action: Function implementing the action
+ * @data: Pointer to data passed to @action implementation
+ *
+ * Removes instance of @action previously added by devm_add_action().
+ * Both action and data should match one of the existing entries.
+ */
+void devm_remove_action(struct device *dev, void (*action)(void *), void *data)
+{
+	struct action_devres devres = {
+		.data = data,
+		.action = action,
+	};
+
+	WARN_ON(devres_destroy(dev, devm_action_release, devm_action_match,
+			       &devres));
+
+}
+EXPORT_SYMBOL_GPL(devm_remove_action);
+
+/*
  * Managed kzalloc/kfree
  */
 static void devm_kzalloc_release(struct device *dev, void *res)
diff --git a/drivers/base/dma-buf.c b/drivers/base/dma-buf.c
index 2a7cb0df176b..08fe897c0b4c 100644
--- a/drivers/base/dma-buf.c
+++ b/drivers/base/dma-buf.c
@@ -27,9 +27,18 @@
 #include <linux/dma-buf.h>
 #include <linux/anon_inodes.h>
 #include <linux/export.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
 
 static inline int is_dma_buf_file(struct file *);
 
+struct dma_buf_list {
+	struct list_head head;
+	struct mutex lock;
+};
+
+static struct dma_buf_list db_list;
+
 static int dma_buf_release(struct inode *inode, struct file *file)
 {
 	struct dma_buf *dmabuf;
@@ -42,6 +51,11 @@ static int dma_buf_release(struct inode *inode, struct file *file)
 	BUG_ON(dmabuf->vmapping_counter);
 
 	dmabuf->ops->release(dmabuf);
+
+	mutex_lock(&db_list.lock);
+	list_del(&dmabuf->list_node);
+	mutex_unlock(&db_list.lock);
+
 	kfree(dmabuf);
 	return 0;
 }
@@ -77,22 +91,24 @@ static inline int is_dma_buf_file(struct file *file)
 }
 
 /**
- * dma_buf_export - Creates a new dma_buf, and associates an anon file
+ * dma_buf_export_named - Creates a new dma_buf, and associates an anon file
  * with this buffer, so it can be exported.
  * Also connect the allocator specific data and ops to the buffer.
+ * Additionally, provide a name string for exporter; useful in debugging.
  *
  * @priv:	[in]	Attach private data of allocator to this buffer
  * @ops:	[in]	Attach allocator-defined dma buf ops to the new buffer.
  * @size:	[in]	Size of the buffer
  * @flags:	[in]	mode flags for the file.
+ * @exp_name:	[in]	name of the exporting module - useful for debugging.
  *
  * Returns, on success, a newly created dma_buf object, which wraps the
  * supplied private data and operations for dma_buf_ops. On either missing
  * ops, or error in allocating struct dma_buf, will return negative error.
  *
  */
-struct dma_buf *dma_buf_export(void *priv, const struct dma_buf_ops *ops,
-				size_t size, int flags)
+struct dma_buf *dma_buf_export_named(void *priv, const struct dma_buf_ops *ops,
+				size_t size, int flags, const char *exp_name)
 {
 	struct dma_buf *dmabuf;
 	struct file *file;
@@ -114,6 +130,7 @@ struct dma_buf *dma_buf_export(void *priv, const struct dma_buf_ops *ops,
 	dmabuf->priv = priv;
 	dmabuf->ops = ops;
 	dmabuf->size = size;
+	dmabuf->exp_name = exp_name;
 
 	file = anon_inode_getfile("dmabuf", &dma_buf_fops, dmabuf, flags);
 
@@ -122,9 +139,13 @@ struct dma_buf *dma_buf_export(void *priv, const struct dma_buf_ops *ops,
 	mutex_init(&dmabuf->lock);
 	INIT_LIST_HEAD(&dmabuf->attachments);
 
+	mutex_lock(&db_list.lock);
+	list_add(&dmabuf->list_node, &db_list.head);
+	mutex_unlock(&db_list.lock);
+
 	return dmabuf;
 }
-EXPORT_SYMBOL_GPL(dma_buf_export);
+EXPORT_SYMBOL_GPL(dma_buf_export_named);
 
 
 /**
@@ -548,3 +569,143 @@ void dma_buf_vunmap(struct dma_buf *dmabuf, void *vaddr)
 	mutex_unlock(&dmabuf->lock);
 }
 EXPORT_SYMBOL_GPL(dma_buf_vunmap);
+
+#ifdef CONFIG_DEBUG_FS
+static int dma_buf_describe(struct seq_file *s)
+{
+	int ret;
+	struct dma_buf *buf_obj;
+	struct dma_buf_attachment *attach_obj;
+	int count = 0, attach_count;
+	size_t size = 0;
+
+	ret = mutex_lock_interruptible(&db_list.lock);
+
+	if (ret)
+		return ret;
+
+	seq_printf(s, "\nDma-buf Objects:\n");
+	seq_printf(s, "\texp_name\tsize\tflags\tmode\tcount\n");
+
+	list_for_each_entry(buf_obj, &db_list.head, list_node) {
+		ret = mutex_lock_interruptible(&buf_obj->lock);
+
+		if (ret) {
+			seq_printf(s,
+				  "\tERROR locking buffer object: skipping\n");
+			continue;
+		}
+
+		seq_printf(s, "\t");
+
+		seq_printf(s, "\t%s\t%08zu\t%08x\t%08x\t%08ld\n",
+				buf_obj->exp_name, buf_obj->size,
+				buf_obj->file->f_flags, buf_obj->file->f_mode,
+				(long)(buf_obj->file->f_count.counter));
+
+		seq_printf(s, "\t\tAttached Devices:\n");
+		attach_count = 0;
+
+		list_for_each_entry(attach_obj, &buf_obj->attachments, node) {
+			seq_printf(s, "\t\t");
+
+			seq_printf(s, "%s\n", attach_obj->dev->init_name);
+			attach_count++;
+		}
+
+		seq_printf(s, "\n\t\tTotal %d devices attached\n",
+				attach_count);
+
+		count++;
+		size += buf_obj->size;
+		mutex_unlock(&buf_obj->lock);
+	}
+
+	seq_printf(s, "\nTotal %d objects, %zu bytes\n", count, size);
+
+	mutex_unlock(&db_list.lock);
+	return 0;
+}
+
+static int dma_buf_show(struct seq_file *s, void *unused)
+{
+	void (*func)(struct seq_file *) = s->private;
+	func(s);
+	return 0;
+}
+
+static int dma_buf_debug_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dma_buf_show, inode->i_private);
+}
+
+static const struct file_operations dma_buf_debug_fops = {
+	.open           = dma_buf_debug_open,
+	.read           = seq_read,
+	.llseek         = seq_lseek,
+	.release        = single_release,
+};
+
+static struct dentry *dma_buf_debugfs_dir;
+
+static int dma_buf_init_debugfs(void)
+{
+	int err = 0;
+	dma_buf_debugfs_dir = debugfs_create_dir("dma_buf", NULL);
+	if (IS_ERR(dma_buf_debugfs_dir)) {
+		err = PTR_ERR(dma_buf_debugfs_dir);
+		dma_buf_debugfs_dir = NULL;
+		return err;
+	}
+
+	err = dma_buf_debugfs_create_file("bufinfo", dma_buf_describe);
+
+	if (err)
+		pr_debug("dma_buf: debugfs: failed to create node bufinfo\n");
+
+	return err;
+}
+
+static void dma_buf_uninit_debugfs(void)
+{
+	if (dma_buf_debugfs_dir)
+		debugfs_remove_recursive(dma_buf_debugfs_dir);
+}
+
+int dma_buf_debugfs_create_file(const char *name,
+				int (*write)(struct seq_file *))
+{
+	struct dentry *d;
+
+	d = debugfs_create_file(name, S_IRUGO, dma_buf_debugfs_dir,
+			write, &dma_buf_debug_fops);
+
+	if (IS_ERR(d))
+		return PTR_ERR(d);
+
+	return 0;
+}
+#else
+static inline int dma_buf_init_debugfs(void)
+{
+	return 0;
+}
+static inline void dma_buf_uninit_debugfs(void)
+{
+}
+#endif
+
+static int __init dma_buf_init(void)
+{
+	mutex_init(&db_list.lock);
+	INIT_LIST_HEAD(&db_list.head);
+	dma_buf_init_debugfs();
+	return 0;
+}
+subsys_initcall(dma_buf_init);
+
+static void __exit dma_buf_deinit(void)
+{
+	dma_buf_uninit_debugfs();
+}
+__exitcall(dma_buf_deinit);
diff --git a/drivers/base/power/domain.c b/drivers/base/power/domain.c
index 9a6b05a35603..7072404c8b6d 100644
--- a/drivers/base/power/domain.c
+++ b/drivers/base/power/domain.c
@@ -920,7 +920,7 @@ static int pm_genpd_prepare(struct device *dev)
 		pm_wakeup_event(dev, 0);
 
 	if (pm_wakeup_pending()) {
-		pm_runtime_put_sync(dev);
+		pm_runtime_put(dev);
 		return -EBUSY;
 	}
 
@@ -961,7 +961,7 @@ static int pm_genpd_prepare(struct device *dev)
 		pm_runtime_enable(dev);
 	}
 
-	pm_runtime_put_sync(dev);
+	pm_runtime_put(dev);
 	return ret;
 }
 
@@ -1327,7 +1327,7 @@ static void pm_genpd_complete(struct device *dev)
 		pm_generic_complete(dev);
 		pm_runtime_set_active(dev);
 		pm_runtime_enable(dev);
-		pm_runtime_idle(dev);
+		pm_request_idle(dev);
 	}
 }
 
diff --git a/drivers/base/power/generic_ops.c b/drivers/base/power/generic_ops.c
index d03d290f31c2..bfd898b8988e 100644
--- a/drivers/base/power/generic_ops.c
+++ b/drivers/base/power/generic_ops.c
@@ -324,6 +324,6 @@ void pm_generic_complete(struct device *dev)
 	 * Let runtime PM try to suspend devices that haven't been in use before
 	 * going into the system-wide sleep state we're resuming from.
 	 */
-	pm_runtime_idle(dev);
+	pm_request_idle(dev);
 }
 #endif /* CONFIG_PM_SLEEP */
diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index 15beb500a4e4..5a9b6569dd74 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -756,7 +756,7 @@ static void device_complete(struct device *dev, pm_message_t state)
 
 	device_unlock(dev);
 
-	pm_runtime_put_sync(dev);
+	pm_runtime_put(dev);
 }
 
 /**
diff --git a/drivers/base/power/opp.c b/drivers/base/power/opp.c
index 32ee0fc7ea54..f0077cb8e249 100644
--- a/drivers/base/power/opp.c
+++ b/drivers/base/power/opp.c
@@ -55,6 +55,7 @@
  * @rate:	Frequency in hertz
  * @u_volt:	Nominal voltage in microvolts corresponding to this OPP
  * @dev_opp:	points back to the device_opp struct this opp belongs to
+ * @head:	RCU callback head used for deferred freeing
  *
  * This structure stores the OPP information for a given device.
  */
diff --git a/drivers/base/power/runtime.c b/drivers/base/power/runtime.c
index 1244930e3d7a..ef13ad08afb2 100644
--- a/drivers/base/power/runtime.c
+++ b/drivers/base/power/runtime.c
@@ -1400,5 +1400,5 @@ void pm_runtime_remove(struct device *dev)
 	if (dev->power.runtime_status == RPM_ACTIVE)
 		pm_runtime_set_suspended(dev);
 	if (dev->power.irq_safe && dev->parent)
-		pm_runtime_put_sync(dev->parent);
+		pm_runtime_put(dev->parent);
 }
diff --git a/drivers/bcma/core.c b/drivers/bcma/core.c
index 03bbe104338f..17b26ce7e051 100644
--- a/drivers/bcma/core.c
+++ b/drivers/bcma/core.c
@@ -104,7 +104,13 @@ void bcma_core_pll_ctl(struct bcma_device *core, u32 req, u32 status, bool on)
 		if (i)
 			bcma_err(core->bus, "PLL enable timeout\n");
 	} else {
-		bcma_warn(core->bus, "Disabling PLL not supported yet!\n");
+		/*
+		 * Mask the PLL but don't wait for it to be disabled. PLL may be
+		 * shared between cores and will be still up if there is another
+		 * core using it.
+		 */
+		bcma_mask32(core, BCMA_CLKCTLST, ~req);
+		bcma_read32(core, BCMA_CLKCTLST);
 	}
 }
 EXPORT_SYMBOL_GPL(bcma_core_pll_ctl);
diff --git a/drivers/bcma/driver_chipcommon.c b/drivers/bcma/driver_chipcommon.c
index 28fa50ad87be..036c6744b39b 100644
--- a/drivers/bcma/driver_chipcommon.c
+++ b/drivers/bcma/driver_chipcommon.c
@@ -25,13 +25,14 @@ static inline u32 bcma_cc_write32_masked(struct bcma_drv_cc *cc, u16 offset,
 	return value;
 }
 
-static u32 bcma_chipco_get_alp_clock(struct bcma_drv_cc *cc)
+u32 bcma_chipco_get_alp_clock(struct bcma_drv_cc *cc)
 {
 	if (cc->capabilities & BCMA_CC_CAP_PMU)
 		return bcma_pmu_get_alp_clock(cc);
 
 	return 20000000;
 }
+EXPORT_SYMBOL_GPL(bcma_chipco_get_alp_clock);
 
 static u32 bcma_chipco_watchdog_get_max_timer(struct bcma_drv_cc *cc)
 {
@@ -213,6 +214,7 @@ u32 bcma_chipco_gpio_out(struct bcma_drv_cc *cc, u32 mask, u32 value)
 
 	return res;
 }
+EXPORT_SYMBOL_GPL(bcma_chipco_gpio_out);
 
 u32 bcma_chipco_gpio_outen(struct bcma_drv_cc *cc, u32 mask, u32 value)
 {
@@ -225,6 +227,7 @@ u32 bcma_chipco_gpio_outen(struct bcma_drv_cc *cc, u32 mask, u32 value)
 
 	return res;
 }
+EXPORT_SYMBOL_GPL(bcma_chipco_gpio_outen);
 
 /*
  * If the bit is set to 0, chipcommon controlls this GPIO,
diff --git a/drivers/bcma/driver_chipcommon_pmu.c b/drivers/bcma/driver_chipcommon_pmu.c
index 932b101dee36..edca73af3cc0 100644
--- a/drivers/bcma/driver_chipcommon_pmu.c
+++ b/drivers/bcma/driver_chipcommon_pmu.c
@@ -174,19 +174,35 @@ u32 bcma_pmu_get_alp_clock(struct bcma_drv_cc *cc)
 	struct bcma_bus *bus = cc->core->bus;
 
 	switch (bus->chipinfo.id) {
+	case BCMA_CHIP_ID_BCM4313:
+	case BCMA_CHIP_ID_BCM43224:
+	case BCMA_CHIP_ID_BCM43225:
+	case BCMA_CHIP_ID_BCM43227:
+	case BCMA_CHIP_ID_BCM43228:
+	case BCMA_CHIP_ID_BCM4331:
+	case BCMA_CHIP_ID_BCM43421:
+	case BCMA_CHIP_ID_BCM43428:
+	case BCMA_CHIP_ID_BCM43431:
 	case BCMA_CHIP_ID_BCM4716:
-	case BCMA_CHIP_ID_BCM4748:
 	case BCMA_CHIP_ID_BCM47162:
-	case BCMA_CHIP_ID_BCM4313:
-	case BCMA_CHIP_ID_BCM5357:
+	case BCMA_CHIP_ID_BCM4748:
 	case BCMA_CHIP_ID_BCM4749:
+	case BCMA_CHIP_ID_BCM5357:
 	case BCMA_CHIP_ID_BCM53572:
+	case BCMA_CHIP_ID_BCM6362:
 		/* always 20Mhz */
 		return 20000 * 1000;
-	case BCMA_CHIP_ID_BCM5356:
 	case BCMA_CHIP_ID_BCM4706:
+	case BCMA_CHIP_ID_BCM5356:
 		/* always 25Mhz */
 		return 25000 * 1000;
+	case BCMA_CHIP_ID_BCM43460:
+	case BCMA_CHIP_ID_BCM4352:
+	case BCMA_CHIP_ID_BCM4360:
+		if (cc->status & BCMA_CC_CHIPST_4360_XTAL_40MZ)
+			return 40000 * 1000;
+		else
+			return 20000 * 1000;
 	default:
 		bcma_warn(bus, "No ALP clock specified for %04X device, pmu rev. %d, using default %d Hz\n",
 			  bus->chipinfo.id, cc->pmu.rev, BCMA_CC_PMU_ALP_CLOCK);
@@ -373,7 +389,7 @@ void bcma_pmu_spuravoid_pllupdate(struct bcma_drv_cc *cc, int spuravoid)
 		tmp |= (bcm5357_bcm43236_ndiv[spuravoid]) << BCMA_CC_PMU1_PLL0_PC2_NDIV_INT_SHIFT;
 		bcma_cc_write32(cc, BCMA_CC_PLLCTL_DATA, tmp);
 
-		tmp = 1 << 10;
+		tmp = BCMA_CC_PMU_CTL_PLL_UPD;
 		break;
 
 	case BCMA_CHIP_ID_BCM4331:
@@ -394,7 +410,7 @@ void bcma_pmu_spuravoid_pllupdate(struct bcma_drv_cc *cc, int spuravoid)
 			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL2,
 						     0x03000a08);
 		}
-		tmp = 1 << 10;
+		tmp = BCMA_CC_PMU_CTL_PLL_UPD;
 		break;
 
 	case BCMA_CHIP_ID_BCM43224:
@@ -427,7 +443,7 @@ void bcma_pmu_spuravoid_pllupdate(struct bcma_drv_cc *cc, int spuravoid)
 			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL5,
 						     0x88888815);
 		}
-		tmp = 1 << 10;
+		tmp = BCMA_CC_PMU_CTL_PLL_UPD;
 		break;
 
 	case BCMA_CHIP_ID_BCM4716:
@@ -461,7 +477,7 @@ void bcma_pmu_spuravoid_pllupdate(struct bcma_drv_cc *cc, int spuravoid)
 						     0x88888815);
 		}
 
-		tmp = 3 << 9;
+		tmp = BCMA_CC_PMU_CTL_PLL_UPD | BCMA_CC_PMU_CTL_NOILPONW;
 		break;
 
 	case BCMA_CHIP_ID_BCM43227:
@@ -497,7 +513,7 @@ void bcma_pmu_spuravoid_pllupdate(struct bcma_drv_cc *cc, int spuravoid)
 			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL5,
 						     0x88888815);
 		}
-		tmp = 1 << 10;
+		tmp = BCMA_CC_PMU_CTL_PLL_UPD;
 		break;
 	default:
 		bcma_err(bus, "Unknown spuravoidance settings for chip 0x%04X, not changing PLL\n",
diff --git a/drivers/bcma/driver_mips.c b/drivers/bcma/driver_mips.c
index 9a7f0e3ab5a3..11115bbe115c 100644
--- a/drivers/bcma/driver_mips.c
+++ b/drivers/bcma/driver_mips.c
@@ -21,7 +21,7 @@
 #include <linux/serial_reg.h>
 #include <linux/time.h>
 
-static const char *part_probes[] = { "bcm47xxpart", NULL };
+static const char * const part_probes[] = { "bcm47xxpart", NULL };
 
 static struct physmap_flash_data bcma_pflash_data = {
 	.part_probe_types	= part_probes,
diff --git a/drivers/bcma/main.c b/drivers/bcma/main.c
index 9a6188add590..f72f52b4b1dd 100644
--- a/drivers/bcma/main.c
+++ b/drivers/bcma/main.c
@@ -120,6 +120,11 @@ static int bcma_register_cores(struct bcma_bus *bus)
 			continue;
 		}
 
+		/* Only first GMAC core on BCM4706 is connected and working */
+		if (core->id.id == BCMA_CORE_4706_MAC_GBIT &&
+		    core->core_unit > 0)
+			continue;
+
 		core->dev.release = bcma_release_core_dev;
 		core->dev.bus = &bcma_bus_type;
 		dev_set_name(&core->dev, "bcma%d:%d", bus->num, dev_id);
diff --git a/drivers/bcma/scan.c b/drivers/bcma/scan.c
index 8d0b57164018..bca9c80056fe 100644
--- a/drivers/bcma/scan.c
+++ b/drivers/bcma/scan.c
@@ -137,19 +137,19 @@ static void bcma_scan_switch_core(struct bcma_bus *bus, u32 addr)
 				       addr);
 }
 
-static u32 bcma_erom_get_ent(struct bcma_bus *bus, u32 **eromptr)
+static u32 bcma_erom_get_ent(struct bcma_bus *bus, u32 __iomem **eromptr)
 {
 	u32 ent = readl(*eromptr);
 	(*eromptr)++;
 	return ent;
 }
 
-static void bcma_erom_push_ent(u32 **eromptr)
+static void bcma_erom_push_ent(u32 __iomem **eromptr)
 {
 	(*eromptr)--;
 }
 
-static s32 bcma_erom_get_ci(struct bcma_bus *bus, u32 **eromptr)
+static s32 bcma_erom_get_ci(struct bcma_bus *bus, u32 __iomem **eromptr)
 {
 	u32 ent = bcma_erom_get_ent(bus, eromptr);
 	if (!(ent & SCAN_ER_VALID))
@@ -159,14 +159,14 @@ static s32 bcma_erom_get_ci(struct bcma_bus *bus, u32 **eromptr)
 	return ent;
 }
 
-static bool bcma_erom_is_end(struct bcma_bus *bus, u32 **eromptr)
+static bool bcma_erom_is_end(struct bcma_bus *bus, u32 __iomem **eromptr)
 {
 	u32 ent = bcma_erom_get_ent(bus, eromptr);
 	bcma_erom_push_ent(eromptr);
 	return (ent == (SCAN_ER_TAG_END | SCAN_ER_VALID));
 }
 
-static bool bcma_erom_is_bridge(struct bcma_bus *bus, u32 **eromptr)
+static bool bcma_erom_is_bridge(struct bcma_bus *bus, u32 __iomem **eromptr)
 {
 	u32 ent = bcma_erom_get_ent(bus, eromptr);
 	bcma_erom_push_ent(eromptr);
@@ -175,7 +175,7 @@ static bool bcma_erom_is_bridge(struct bcma_bus *bus, u32 **eromptr)
 		((ent & SCAN_ADDR_TYPE) == SCAN_ADDR_TYPE_BRIDGE));
 }
 
-static void bcma_erom_skip_component(struct bcma_bus *bus, u32 **eromptr)
+static void bcma_erom_skip_component(struct bcma_bus *bus, u32 __iomem **eromptr)
 {
 	u32 ent;
 	while (1) {
@@ -189,7 +189,7 @@ static void bcma_erom_skip_component(struct bcma_bus *bus, u32 **eromptr)
 	bcma_erom_push_ent(eromptr);
 }
 
-static s32 bcma_erom_get_mst_port(struct bcma_bus *bus, u32 **eromptr)
+static s32 bcma_erom_get_mst_port(struct bcma_bus *bus, u32 __iomem **eromptr)
 {
 	u32 ent = bcma_erom_get_ent(bus, eromptr);
 	if (!(ent & SCAN_ER_VALID))
@@ -199,7 +199,7 @@ static s32 bcma_erom_get_mst_port(struct bcma_bus *bus, u32 **eromptr)
 	return ent;
 }
 
-static s32 bcma_erom_get_addr_desc(struct bcma_bus *bus, u32 **eromptr,
+static s32 bcma_erom_get_addr_desc(struct bcma_bus *bus, u32 __iomem **eromptr,
 				  u32 type, u8 port)
 {
 	u32 addrl, addrh, sizel, sizeh = 0;
diff --git a/drivers/bcma/sprom.c b/drivers/bcma/sprom.c
index 4adf9ef9a113..8934298a638d 100644
--- a/drivers/bcma/sprom.c
+++ b/drivers/bcma/sprom.c
@@ -217,6 +217,7 @@ static void bcma_sprom_extract_r8(struct bcma_bus *bus, const u16 *sprom)
 	}
 
 	SPEX(board_rev, SSB_SPROM8_BOARDREV, ~0, 0);
+	SPEX(board_type, SSB_SPROM1_SPID, ~0, 0);
 
 	SPEX(txpid2g[0], SSB_SPROM4_TXPID2G01, SSB_SPROM4_TXPID2G0,
 	     SSB_SPROM4_TXPID2G0_SHIFT);
diff --git a/drivers/block/DAC960.c b/drivers/block/DAC960.c
index 5b5ee79ff236..eb3950113e42 100644
--- a/drivers/block/DAC960.c
+++ b/drivers/block/DAC960.c
@@ -6473,7 +6473,7 @@ static int dac960_initial_status_proc_show(struct seq_file *m, void *v)
 
 static int dac960_initial_status_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, dac960_initial_status_proc_show, PDE(inode)->data);
+	return single_open(file, dac960_initial_status_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations dac960_initial_status_proc_fops = {
@@ -6519,7 +6519,7 @@ static int dac960_current_status_proc_show(struct seq_file *m, void *v)
 
 static int dac960_current_status_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, dac960_current_status_proc_show, PDE(inode)->data);
+	return single_open(file, dac960_current_status_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations dac960_current_status_proc_fops = {
@@ -6540,14 +6540,14 @@ static int dac960_user_command_proc_show(struct seq_file *m, void *v)
 
 static int dac960_user_command_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, dac960_user_command_proc_show, PDE(inode)->data);
+	return single_open(file, dac960_user_command_proc_show, PDE_DATA(inode));
 }
 
 static ssize_t dac960_user_command_proc_write(struct file *file,
 				       const char __user *Buffer,
 				       size_t Count, loff_t *pos)
 {
-  DAC960_Controller_T *Controller = (DAC960_Controller_T *) PDE(file_inode(file))->data;
+  DAC960_Controller_T *Controller = PDE_DATA(file_inode(file));
   unsigned char CommandBuffer[80];
   int Length;
   if (Count > sizeof(CommandBuffer)-1) return -EINVAL;
diff --git a/drivers/block/Makefile b/drivers/block/Makefile
index a3b40232c6ab..ca07399a8d99 100644
--- a/drivers/block/Makefile
+++ b/drivers/block/Makefile
@@ -42,4 +42,5 @@ obj-$(CONFIG_BLK_DEV_PCIESSD_MTIP32XX)	+= mtip32xx/
 
 obj-$(CONFIG_BLK_DEV_RSXX) += rsxx/
 
+nvme-y		:= nvme-core.o nvme-scsi.o
 swim_mod-y	:= swim.o swim_asm.o
diff --git a/drivers/block/amiflop.c b/drivers/block/amiflop.c
index 386146d792d1..4ff85b8785ee 100644
--- a/drivers/block/amiflop.c
+++ b/drivers/block/amiflop.c
@@ -1634,7 +1634,7 @@ static int floppy_open(struct block_device *bdev, fmode_t mode)
 	return 0;
 }
 
-static int floppy_release(struct gendisk *disk, fmode_t mode)
+static void floppy_release(struct gendisk *disk, fmode_t mode)
 {
 	struct amiga_floppy_struct *p = disk->private_data;
 	int drive = p - unit;
@@ -1654,7 +1654,6 @@ static int floppy_release(struct gendisk *disk, fmode_t mode)
 	floppy_off (drive | 0x40000000);
 #endif
 	mutex_unlock(&amiflop_mutex);
-	return 0;
 }
 
 /*
diff --git a/drivers/block/aoe/aoeblk.c b/drivers/block/aoe/aoeblk.c
index a129f8c8073d..916d9ed5c8aa 100644
--- a/drivers/block/aoe/aoeblk.c
+++ b/drivers/block/aoe/aoeblk.c
@@ -169,7 +169,7 @@ aoeblk_open(struct block_device *bdev, fmode_t mode)
 	return -ENODEV;
 }
 
-static int
+static void
 aoeblk_release(struct gendisk *disk, fmode_t mode)
 {
 	struct aoedev *d = disk->private_data;
@@ -180,11 +180,9 @@ aoeblk_release(struct gendisk *disk, fmode_t mode)
 	if (--d->nopen == 0) {
 		spin_unlock_irqrestore(&d->lock, flags);
 		aoecmd_cfg(d->aoemajor, d->aoeminor);
-		return 0;
+		return;
 	}
 	spin_unlock_irqrestore(&d->lock, flags);
-
-	return 0;
 }
 
 static void
diff --git a/drivers/block/aoe/aoechr.c b/drivers/block/aoe/aoechr.c
index 42e67ad6bd20..ab41be625a53 100644
--- a/drivers/block/aoe/aoechr.c
+++ b/drivers/block/aoe/aoechr.c
@@ -139,13 +139,12 @@ bail:		spin_unlock_irqrestore(&emsgs_lock, flags);
 		return;
 	}
 
-	mp = kmalloc(n, GFP_ATOMIC);
+	mp = kmemdup(msg, n, GFP_ATOMIC);
 	if (mp == NULL) {
 		printk(KERN_ERR "aoe: allocation failure, len=%ld\n", n);
 		goto bail;
 	}
 
-	memcpy(mp, msg, n);
 	em->msg = mp;
 	em->flags |= EMFL_VALID;
 	em->len = n;
diff --git a/drivers/block/aoe/aoecmd.c b/drivers/block/aoe/aoecmd.c
index 92b6d7c51e39..fc803ecbbce4 100644
--- a/drivers/block/aoe/aoecmd.c
+++ b/drivers/block/aoe/aoecmd.c
@@ -920,16 +920,14 @@ bio_pagedec(struct bio *bio)
 static void
 bufinit(struct buf *buf, struct request *rq, struct bio *bio)
 {
-	struct bio_vec *bv;
-
 	memset(buf, 0, sizeof(*buf));
 	buf->rq = rq;
 	buf->bio = bio;
 	buf->resid = bio->bi_size;
 	buf->sector = bio->bi_sector;
 	bio_pageinc(bio);
-	buf->bv = bv = &bio->bi_io_vec[bio->bi_idx];
-	buf->bv_resid = bv->bv_len;
+	buf->bv = bio_iovec(bio);
+	buf->bv_resid = buf->bv->bv_len;
 	WARN_ON(buf->bv_resid == 0);
 }
 
diff --git a/drivers/block/ataflop.c b/drivers/block/ataflop.c
index ede16c64ff07..0e30c6e5492a 100644
--- a/drivers/block/ataflop.c
+++ b/drivers/block/ataflop.c
@@ -367,7 +367,7 @@ static void fd_probe( int drive );
 static int fd_test_drive_present( int drive );
 static void config_types( void );
 static int floppy_open(struct block_device *bdev, fmode_t mode);
-static int floppy_release(struct gendisk *disk, fmode_t mode);
+static void floppy_release(struct gendisk *disk, fmode_t mode);
 
 /************************* End of Prototypes **************************/
 
@@ -1886,7 +1886,7 @@ static int floppy_unlocked_open(struct block_device *bdev, fmode_t mode)
 	return ret;
 }
 
-static int floppy_release(struct gendisk *disk, fmode_t mode)
+static void floppy_release(struct gendisk *disk, fmode_t mode)
 {
 	struct atari_floppy_struct *p = disk->private_data;
 	mutex_lock(&ataflop_mutex);
@@ -1897,7 +1897,6 @@ static int floppy_release(struct gendisk *disk, fmode_t mode)
 		p->ref = 0;
 	}
 	mutex_unlock(&ataflop_mutex);
-	return 0;
 }
 
 static const struct block_device_operations floppy_fops = {
diff --git a/drivers/block/brd.c b/drivers/block/brd.c
index 531ceb31d0ff..f1a29f8e9d33 100644
--- a/drivers/block/brd.c
+++ b/drivers/block/brd.c
@@ -334,8 +334,7 @@ static void brd_make_request(struct request_queue *q, struct bio *bio)
 	int err = -EIO;
 
 	sector = bio->bi_sector;
-	if (sector + (bio->bi_size >> SECTOR_SHIFT) >
-						get_capacity(bdev->bd_disk))
+	if (bio_end_sector(bio) > get_capacity(bdev->bd_disk))
 		goto out;
 
 	if (unlikely(bio->bi_rw & REQ_DISCARD)) {
diff --git a/drivers/block/cciss.c b/drivers/block/cciss.c
index 1c1b8e544aa2..6374dc103521 100644
--- a/drivers/block/cciss.c
+++ b/drivers/block/cciss.c
@@ -75,6 +75,12 @@ module_param(cciss_simple_mode, int, S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(cciss_simple_mode,
 	"Use 'simple mode' rather than 'performant mode'");
 
+static int cciss_allow_hpsa;
+module_param(cciss_allow_hpsa, int, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(cciss_allow_hpsa,
+	"Prevent cciss driver from accessing hardware known to be "
+	" supported by the hpsa driver");
+
 static DEFINE_MUTEX(cciss_mutex);
 static struct proc_dir_entry *proc_cciss;
 
@@ -161,7 +167,7 @@ static irqreturn_t do_cciss_intx(int irq, void *dev_id);
 static irqreturn_t do_cciss_msix_intr(int irq, void *dev_id);
 static int cciss_open(struct block_device *bdev, fmode_t mode);
 static int cciss_unlocked_open(struct block_device *bdev, fmode_t mode);
-static int cciss_release(struct gendisk *disk, fmode_t mode);
+static void cciss_release(struct gendisk *disk, fmode_t mode);
 static int do_ioctl(struct block_device *bdev, fmode_t mode,
 		    unsigned int cmd, unsigned long arg);
 static int cciss_ioctl(struct block_device *bdev, fmode_t mode,
@@ -493,7 +499,7 @@ static int cciss_seq_open(struct inode *inode, struct file *file)
 	struct seq_file *seq = file->private_data;
 
 	if (!ret)
-		seq->private = PDE(inode)->data;
+		seq->private = PDE_DATA(inode);
 
 	return ret;
 }
@@ -1123,7 +1129,7 @@ static int cciss_unlocked_open(struct block_device *bdev, fmode_t mode)
 /*
  * Close.  Sync first.
  */
-static int cciss_release(struct gendisk *disk, fmode_t mode)
+static void cciss_release(struct gendisk *disk, fmode_t mode)
 {
 	ctlr_info_t *h;
 	drive_info_struct *drv;
@@ -1135,7 +1141,6 @@ static int cciss_release(struct gendisk *disk, fmode_t mode)
 	drv->usage_count--;
 	h->usage_count--;
 	mutex_unlock(&cciss_mutex);
-	return 0;
 }
 
 static int do_ioctl(struct block_device *bdev, fmode_t mode,
@@ -4116,9 +4121,13 @@ static int cciss_lookup_board_id(struct pci_dev *pdev, u32 *board_id)
 	*board_id = ((subsystem_device_id << 16) & 0xffff0000) |
 			subsystem_vendor_id;
 
-	for (i = 0; i < ARRAY_SIZE(products); i++)
+	for (i = 0; i < ARRAY_SIZE(products); i++) {
+		/* Stand aside for hpsa driver on request */
+		if (cciss_allow_hpsa)
+			return -ENODEV;
 		if (*board_id == products[i].board_id)
 			return i;
+	}
 	dev_warn(&pdev->dev, "unrecognized board ID: 0x%08x, ignoring.\n",
 		*board_id);
 	return -ENODEV;
@@ -4960,6 +4969,16 @@ static int cciss_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	ctlr_info_t *h;
 	unsigned long flags;
 
+	/*
+	 * By default the cciss driver is used for all older HP Smart Array
+	 * controllers. There are module paramaters that allow a user to
+	 * override this behavior and instead use the hpsa SCSI driver. If
+	 * this is the case cciss may be loaded first from the kdump initrd
+	 * image and cause a kernel panic. So if reset_devices is true and
+	 * cciss_allow_hpsa is set just bail.
+	 */
+	if ((reset_devices) && (cciss_allow_hpsa == 1))
+		return -ENODEV;
 	rc = cciss_init_reset_devices(pdev);
 	if (rc) {
 		if (rc != -ENOTSUPP)
diff --git a/drivers/block/cciss_scsi.c b/drivers/block/cciss_scsi.c
index da3311129a0c..ecd845cd28d8 100644
--- a/drivers/block/cciss_scsi.c
+++ b/drivers/block/cciss_scsi.c
@@ -54,13 +54,11 @@ static CommandList_struct *cmd_special_alloc(ctlr_info_t *h);
 static void cmd_free(ctlr_info_t *h, CommandList_struct *c);
 static void cmd_special_free(ctlr_info_t *h, CommandList_struct *c);
 
-static int cciss_scsi_proc_info(
-		struct Scsi_Host *sh,
+static int cciss_scsi_write_info(struct Scsi_Host *sh,
 		char *buffer, /* data buffer */
-		char **start, 	   /* where data in buffer starts */
-		off_t offset,	   /* offset from start of imaginary file */
-		int length, 	   /* length of data in buffer */
-		int func);	   /* 0 == read, 1 == write */
+		int length); 	   /* length of data in buffer */
+static int cciss_scsi_show_info(struct seq_file *m,
+				struct Scsi_Host *sh);
 
 static int cciss_scsi_queue_command (struct Scsi_Host *h,
 				     struct scsi_cmnd *cmd);
@@ -82,7 +80,8 @@ static struct scsi_host_template cciss_driver_template = {
 	.module			= THIS_MODULE,
 	.name			= "cciss",
 	.proc_name		= "cciss",
-	.proc_info		= cciss_scsi_proc_info,
+	.write_info		= cciss_scsi_write_info,
+	.show_info		= cciss_scsi_show_info,
 	.queuecommand		= cciss_scsi_queue_command,
 	.this_id		= 7,
 	.cmd_per_lun		= 1,
@@ -1302,59 +1301,54 @@ cciss_scsi_user_command(ctlr_info_t *h, int hostno, char *buffer, int length)
 	return length;
 }
 
-
 static int
-cciss_scsi_proc_info(struct Scsi_Host *sh,
+cciss_scsi_write_info(struct Scsi_Host *sh,
 		char *buffer, /* data buffer */
-		char **start, 	   /* where data in buffer starts */
-		off_t offset,	   /* offset from start of imaginary file */
-		int length, 	   /* length of data in buffer */
-		int func)	   /* 0 == read, 1 == write */
+		int length) 	   /* length of data in buffer */
 {
+	ctlr_info_t *h = (ctlr_info_t *) sh->hostdata[0];
+	if (h == NULL)  /* This really shouldn't ever happen. */
+		return -EINVAL;
 
-	int buflen, datalen;
-	ctlr_info_t *h;
+	return cciss_scsi_user_command(h, sh->host_no,
+			buffer, length);	
+} 
+
+static int
+cciss_scsi_show_info(struct seq_file *m, struct Scsi_Host *sh)
+{
+
+	ctlr_info_t *h = (ctlr_info_t *) sh->hostdata[0];
 	int i;
 
-	h = (ctlr_info_t *) sh->hostdata[0];
 	if (h == NULL)  /* This really shouldn't ever happen. */
 		return -EINVAL;
 
-	if (func == 0) {	/* User is reading from /proc/scsi/ciss*?/?*  */
-		buflen = sprintf(buffer, "cciss%d: SCSI host: %d\n",
-				h->ctlr, sh->host_no);
-
-		/* this information is needed by apps to know which cciss
-		   device corresponds to which scsi host number without
-		   having to open a scsi target device node.  The device
-		   information is not a duplicate of /proc/scsi/scsi because
-		   the two may be out of sync due to scsi hotplug, rather
-		   this info is for an app to be able to use to know how to
-		   get them back in sync. */
-
-		for (i = 0; i < ccissscsi[h->ctlr].ndevices; i++) {
-			struct cciss_scsi_dev_t *sd =
-				&ccissscsi[h->ctlr].dev[i];
-			buflen += sprintf(&buffer[buflen], "c%db%dt%dl%d %02d "
-				"0x%02x%02x%02x%02x%02x%02x%02x%02x\n",
-				sh->host_no, sd->bus, sd->target, sd->lun,
-				sd->devtype,
-				sd->scsi3addr[0], sd->scsi3addr[1],
-				sd->scsi3addr[2], sd->scsi3addr[3],
-				sd->scsi3addr[4], sd->scsi3addr[5],
-				sd->scsi3addr[6], sd->scsi3addr[7]);
-		}
-		datalen = buflen - offset;
-		if (datalen < 0) { 	/* they're reading past EOF. */
-			datalen = 0;
-			*start = buffer+buflen;	
-		} else
-			*start = buffer + offset;
-		return(datalen);
-	} else 	/* User is writing to /proc/scsi/cciss*?/?*  ... */
-		return cciss_scsi_user_command(h, sh->host_no,
-			buffer, length);	
-} 
+	seq_printf(m, "cciss%d: SCSI host: %d\n",
+			h->ctlr, sh->host_no);
+
+	/* this information is needed by apps to know which cciss
+	   device corresponds to which scsi host number without
+	   having to open a scsi target device node.  The device
+	   information is not a duplicate of /proc/scsi/scsi because
+	   the two may be out of sync due to scsi hotplug, rather
+	   this info is for an app to be able to use to know how to
+	   get them back in sync. */
+
+	for (i = 0; i < ccissscsi[h->ctlr].ndevices; i++) {
+		struct cciss_scsi_dev_t *sd =
+			&ccissscsi[h->ctlr].dev[i];
+		seq_printf(m, "c%db%dt%dl%d %02d "
+			"0x%02x%02x%02x%02x%02x%02x%02x%02x\n",
+			sh->host_no, sd->bus, sd->target, sd->lun,
+			sd->devtype,
+			sd->scsi3addr[0], sd->scsi3addr[1],
+			sd->scsi3addr[2], sd->scsi3addr[3],
+			sd->scsi3addr[4], sd->scsi3addr[5],
+			sd->scsi3addr[6], sd->scsi3addr[7]);
+	}
+	return 0;
+}
 
 /* cciss_scatter_gather takes a struct scsi_cmnd, (cmd), and does the pci 
    dma mapping  and fills in the scatter gather entries of the 
diff --git a/drivers/block/cpqarray.c b/drivers/block/cpqarray.c
index 3f087133a25a..639d26b90b91 100644
--- a/drivers/block/cpqarray.c
+++ b/drivers/block/cpqarray.c
@@ -160,7 +160,7 @@ static int sendcmd(
 	unsigned int log_unit );
 
 static int ida_unlocked_open(struct block_device *bdev, fmode_t mode);
-static int ida_release(struct gendisk *disk, fmode_t mode);
+static void ida_release(struct gendisk *disk, fmode_t mode);
 static int ida_ioctl(struct block_device *bdev, fmode_t mode, unsigned int cmd, unsigned long arg);
 static int ida_getgeo(struct block_device *bdev, struct hd_geometry *geo);
 static int ida_ctlr_ioctl(ctlr_info_t *h, int dsk, ida_ioctl_t *io);
@@ -296,7 +296,7 @@ static int ida_proc_show(struct seq_file *m, void *v)
 
 static int ida_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, ida_proc_show, PDE(inode)->data);
+	return single_open(file, ida_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations ida_proc_fops = {
@@ -856,7 +856,7 @@ static int ida_unlocked_open(struct block_device *bdev, fmode_t mode)
 /*
  * Close.  Sync first.
  */
-static int ida_release(struct gendisk *disk, fmode_t mode)
+static void ida_release(struct gendisk *disk, fmode_t mode)
 {
 	ctlr_info_t *host;
 
@@ -864,8 +864,6 @@ static int ida_release(struct gendisk *disk, fmode_t mode)
 	host = get_host(disk);
 	host->usage_count--;
 	mutex_unlock(&cpqarray_mutex);
-
-	return 0;
 }
 
 /*
diff --git a/drivers/block/drbd/drbd_actlog.c b/drivers/block/drbd/drbd_actlog.c
index 92510f8ad013..6608076dc39e 100644
--- a/drivers/block/drbd/drbd_actlog.c
+++ b/drivers/block/drbd/drbd_actlog.c
@@ -104,7 +104,6 @@ struct update_al_work {
 	int err;
 };
 
-static int al_write_transaction(struct drbd_conf *mdev);
 
 void *drbd_md_get_buffer(struct drbd_conf *mdev)
 {
@@ -168,7 +167,11 @@ static int _drbd_md_sync_page_io(struct drbd_conf *mdev,
 	bio->bi_end_io = drbd_md_io_complete;
 	bio->bi_rw = rw;
 
-	if (!get_ldev_if_state(mdev, D_ATTACHING)) {  /* Corresponding put_ldev in drbd_md_io_complete() */
+	if (!(rw & WRITE) && mdev->state.disk == D_DISKLESS && mdev->ldev == NULL)
+		/* special case, drbd_md_read() during drbd_adm_attach(): no get_ldev */
+		;
+	else if (!get_ldev_if_state(mdev, D_ATTACHING)) {
+		/* Corresponding put_ldev in drbd_md_io_complete() */
 		dev_err(DEV, "ASSERT FAILED: get_ldev_if_state() == 1 in _drbd_md_sync_page_io()\n");
 		err = -ENODEV;
 		goto out;
@@ -199,9 +202,10 @@ int drbd_md_sync_page_io(struct drbd_conf *mdev, struct drbd_backing_dev *bdev,
 
 	BUG_ON(!bdev->md_bdev);
 
-	dev_dbg(DEV, "meta_data io: %s [%d]:%s(,%llus,%s)\n",
+	dev_dbg(DEV, "meta_data io: %s [%d]:%s(,%llus,%s) %pS\n",
 	     current->comm, current->pid, __func__,
-	     (unsigned long long)sector, (rw & WRITE) ? "WRITE" : "READ");
+	     (unsigned long long)sector, (rw & WRITE) ? "WRITE" : "READ",
+	     (void*)_RET_IP_ );
 
 	if (sector < drbd_md_first_sector(bdev) ||
 	    sector + 7 > drbd_md_last_sector(bdev))
@@ -209,7 +213,8 @@ int drbd_md_sync_page_io(struct drbd_conf *mdev, struct drbd_backing_dev *bdev,
 		     current->comm, current->pid, __func__,
 		     (unsigned long long)sector, (rw & WRITE) ? "WRITE" : "READ");
 
-	err = _drbd_md_sync_page_io(mdev, bdev, iop, sector, rw, MD_BLOCK_SIZE);
+	/* we do all our meta data IO in aligned 4k blocks. */
+	err = _drbd_md_sync_page_io(mdev, bdev, iop, sector, rw, 4096);
 	if (err) {
 		dev_err(DEV, "drbd_md_sync_page_io(,%llus,%s) failed with error %d\n",
 		    (unsigned long long)sector, (rw & WRITE) ? "WRITE" : "READ", err);
@@ -217,44 +222,99 @@ int drbd_md_sync_page_io(struct drbd_conf *mdev, struct drbd_backing_dev *bdev,
 	return err;
 }
 
-static struct lc_element *_al_get(struct drbd_conf *mdev, unsigned int enr)
+static struct bm_extent *find_active_resync_extent(struct drbd_conf *mdev, unsigned int enr)
 {
-	struct lc_element *al_ext;
 	struct lc_element *tmp;
-	int wake;
-
-	spin_lock_irq(&mdev->al_lock);
 	tmp = lc_find(mdev->resync, enr/AL_EXT_PER_BM_SECT);
 	if (unlikely(tmp != NULL)) {
 		struct bm_extent  *bm_ext = lc_entry(tmp, struct bm_extent, lce);
-		if (test_bit(BME_NO_WRITES, &bm_ext->flags)) {
-			wake = !test_and_set_bit(BME_PRIORITY, &bm_ext->flags);
-			spin_unlock_irq(&mdev->al_lock);
-			if (wake)
-				wake_up(&mdev->al_wait);
-			return NULL;
-		}
+		if (test_bit(BME_NO_WRITES, &bm_ext->flags))
+			return bm_ext;
+	}
+	return NULL;
+}
+
+static struct lc_element *_al_get(struct drbd_conf *mdev, unsigned int enr, bool nonblock)
+{
+	struct lc_element *al_ext;
+	struct bm_extent *bm_ext;
+	int wake;
+
+	spin_lock_irq(&mdev->al_lock);
+	bm_ext = find_active_resync_extent(mdev, enr);
+	if (bm_ext) {
+		wake = !test_and_set_bit(BME_PRIORITY, &bm_ext->flags);
+		spin_unlock_irq(&mdev->al_lock);
+		if (wake)
+			wake_up(&mdev->al_wait);
+		return NULL;
 	}
-	al_ext = lc_get(mdev->act_log, enr);
+	if (nonblock)
+		al_ext = lc_try_get(mdev->act_log, enr);
+	else
+		al_ext = lc_get(mdev->act_log, enr);
 	spin_unlock_irq(&mdev->al_lock);
 	return al_ext;
 }
 
-void drbd_al_begin_io(struct drbd_conf *mdev, struct drbd_interval *i)
+bool drbd_al_begin_io_fastpath(struct drbd_conf *mdev, struct drbd_interval *i)
 {
 	/* for bios crossing activity log extent boundaries,
 	 * we may need to activate two extents in one go */
 	unsigned first = i->sector >> (AL_EXTENT_SHIFT-9);
 	unsigned last = i->size == 0 ? first : (i->sector + (i->size >> 9) - 1) >> (AL_EXTENT_SHIFT-9);
-	unsigned enr;
-	bool locked = false;
 
+	D_ASSERT((unsigned)(last - first) <= 1);
+	D_ASSERT(atomic_read(&mdev->local_cnt) > 0);
+
+	/* FIXME figure out a fast path for bios crossing AL extent boundaries */
+	if (first != last)
+		return false;
+
+	return _al_get(mdev, first, true);
+}
+
+bool drbd_al_begin_io_prepare(struct drbd_conf *mdev, struct drbd_interval *i)
+{
+	/* for bios crossing activity log extent boundaries,
+	 * we may need to activate two extents in one go */
+	unsigned first = i->sector >> (AL_EXTENT_SHIFT-9);
+	unsigned last = i->size == 0 ? first : (i->sector + (i->size >> 9) - 1) >> (AL_EXTENT_SHIFT-9);
+	unsigned enr;
+	bool need_transaction = false;
 
 	D_ASSERT(first <= last);
 	D_ASSERT(atomic_read(&mdev->local_cnt) > 0);
 
-	for (enr = first; enr <= last; enr++)
-		wait_event(mdev->al_wait, _al_get(mdev, enr) != NULL);
+	for (enr = first; enr <= last; enr++) {
+		struct lc_element *al_ext;
+		wait_event(mdev->al_wait,
+				(al_ext = _al_get(mdev, enr, false)) != NULL);
+		if (al_ext->lc_number != enr)
+			need_transaction = true;
+	}
+	return need_transaction;
+}
+
+static int al_write_transaction(struct drbd_conf *mdev, bool delegate);
+
+/* When called through generic_make_request(), we must delegate
+ * activity log I/O to the worker thread: a further request
+ * submitted via generic_make_request() within the same task
+ * would be queued on current->bio_list, and would only start
+ * after this function returns (see generic_make_request()).
+ *
+ * However, if we *are* the worker, we must not delegate to ourselves.
+ */
+
+/*
+ * @delegate:   delegate activity log I/O to the worker thread
+ */
+void drbd_al_begin_io_commit(struct drbd_conf *mdev, bool delegate)
+{
+	bool locked = false;
+
+	BUG_ON(delegate && current == mdev->tconn->worker.task);
 
 	/* Serialize multiple transactions.
 	 * This uses test_and_set_bit, memory barrier is implicit.
@@ -264,13 +324,6 @@ void drbd_al_begin_io(struct drbd_conf *mdev, struct drbd_interval *i)
 			(locked = lc_try_lock_for_transaction(mdev->act_log)));
 
 	if (locked) {
-		/* drbd_al_write_transaction(mdev,al_ext,enr);
-		 * recurses into generic_make_request(), which
-		 * disallows recursion, bios being serialized on the
-		 * current->bio_tail list now.
-		 * we have to delegate updates to the activity log
-		 * to the worker thread. */
-
 		/* Double check: it may have been committed by someone else,
 		 * while we have been waiting for the lock. */
 		if (mdev->act_log->pending_changes) {
@@ -280,11 +333,8 @@ void drbd_al_begin_io(struct drbd_conf *mdev, struct drbd_interval *i)
 			write_al_updates = rcu_dereference(mdev->ldev->disk_conf)->al_updates;
 			rcu_read_unlock();
 
-			if (write_al_updates) {
-				al_write_transaction(mdev);
-				mdev->al_writ_cnt++;
-			}
-
+			if (write_al_updates)
+				al_write_transaction(mdev, delegate);
 			spin_lock_irq(&mdev->al_lock);
 			/* FIXME
 			if (err)
@@ -298,6 +348,66 @@ void drbd_al_begin_io(struct drbd_conf *mdev, struct drbd_interval *i)
 	}
 }
 
+/*
+ * @delegate:   delegate activity log I/O to the worker thread
+ */
+void drbd_al_begin_io(struct drbd_conf *mdev, struct drbd_interval *i, bool delegate)
+{
+	BUG_ON(delegate && current == mdev->tconn->worker.task);
+
+	if (drbd_al_begin_io_prepare(mdev, i))
+		drbd_al_begin_io_commit(mdev, delegate);
+}
+
+int drbd_al_begin_io_nonblock(struct drbd_conf *mdev, struct drbd_interval *i)
+{
+	struct lru_cache *al = mdev->act_log;
+	/* for bios crossing activity log extent boundaries,
+	 * we may need to activate two extents in one go */
+	unsigned first = i->sector >> (AL_EXTENT_SHIFT-9);
+	unsigned last = i->size == 0 ? first : (i->sector + (i->size >> 9) - 1) >> (AL_EXTENT_SHIFT-9);
+	unsigned nr_al_extents;
+	unsigned available_update_slots;
+	unsigned enr;
+
+	D_ASSERT(first <= last);
+
+	nr_al_extents = 1 + last - first; /* worst case: all touched extends are cold. */
+	available_update_slots = min(al->nr_elements - al->used,
+				al->max_pending_changes - al->pending_changes);
+
+	/* We want all necessary updates for a given request within the same transaction
+	 * We could first check how many updates are *actually* needed,
+	 * and use that instead of the worst-case nr_al_extents */
+	if (available_update_slots < nr_al_extents)
+		return -EWOULDBLOCK;
+
+	/* Is resync active in this area? */
+	for (enr = first; enr <= last; enr++) {
+		struct lc_element *tmp;
+		tmp = lc_find(mdev->resync, enr/AL_EXT_PER_BM_SECT);
+		if (unlikely(tmp != NULL)) {
+			struct bm_extent  *bm_ext = lc_entry(tmp, struct bm_extent, lce);
+			if (test_bit(BME_NO_WRITES, &bm_ext->flags)) {
+				if (!test_and_set_bit(BME_PRIORITY, &bm_ext->flags))
+					return -EBUSY;
+				return -EWOULDBLOCK;
+			}
+		}
+	}
+
+	/* Checkout the refcounts.
+	 * Given that we checked for available elements and update slots above,
+	 * this has to be successful. */
+	for (enr = first; enr <= last; enr++) {
+		struct lc_element *al_ext;
+		al_ext = lc_get_cumulative(mdev->act_log, enr);
+		if (!al_ext)
+			dev_info(DEV, "LOGIC BUG for enr=%u\n", enr);
+	}
+	return 0;
+}
+
 void drbd_al_complete_io(struct drbd_conf *mdev, struct drbd_interval *i)
 {
 	/* for bios crossing activity log extent boundaries,
@@ -350,6 +460,24 @@ static unsigned int rs_extent_to_bm_page(unsigned int rs_enr)
 		 (BM_EXT_SHIFT - BM_BLOCK_SHIFT));
 }
 
+static sector_t al_tr_number_to_on_disk_sector(struct drbd_conf *mdev)
+{
+	const unsigned int stripes = mdev->ldev->md.al_stripes;
+	const unsigned int stripe_size_4kB = mdev->ldev->md.al_stripe_size_4k;
+
+	/* transaction number, modulo on-disk ring buffer wrap around */
+	unsigned int t = mdev->al_tr_number % (mdev->ldev->md.al_size_4k);
+
+	/* ... to aligned 4k on disk block */
+	t = ((t % stripes) * stripe_size_4kB) + t/stripes;
+
+	/* ... to 512 byte sector in activity log */
+	t *= 8;
+
+	/* ... plus offset to the on disk position */
+	return mdev->ldev->md.md_offset + mdev->ldev->md.al_offset + t;
+}
+
 static int
 _al_write_transaction(struct drbd_conf *mdev)
 {
@@ -432,23 +560,27 @@ _al_write_transaction(struct drbd_conf *mdev)
 	if (mdev->al_tr_cycle >= mdev->act_log->nr_elements)
 		mdev->al_tr_cycle = 0;
 
-	sector =  mdev->ldev->md.md_offset
-		+ mdev->ldev->md.al_offset
-		+ mdev->al_tr_pos * (MD_BLOCK_SIZE>>9);
+	sector = al_tr_number_to_on_disk_sector(mdev);
 
 	crc = crc32c(0, buffer, 4096);
 	buffer->crc32c = cpu_to_be32(crc);
 
 	if (drbd_bm_write_hinted(mdev))
 		err = -EIO;
-		/* drbd_chk_io_error done already */
-	else if (drbd_md_sync_page_io(mdev, mdev->ldev, sector, WRITE)) {
-		err = -EIO;
-		drbd_chk_io_error(mdev, 1, DRBD_META_IO_ERROR);
-	} else {
-		/* advance ringbuffer position and transaction counter */
-		mdev->al_tr_pos = (mdev->al_tr_pos + 1) % (MD_AL_SECTORS*512/MD_BLOCK_SIZE);
-		mdev->al_tr_number++;
+	else {
+		bool write_al_updates;
+		rcu_read_lock();
+		write_al_updates = rcu_dereference(mdev->ldev->disk_conf)->al_updates;
+		rcu_read_unlock();
+		if (write_al_updates) {
+			if (drbd_md_sync_page_io(mdev, mdev->ldev, sector, WRITE)) {
+				err = -EIO;
+				drbd_chk_io_error(mdev, 1, DRBD_META_IO_ERROR);
+			} else {
+				mdev->al_tr_number++;
+				mdev->al_writ_cnt++;
+			}
+		}
 	}
 
 	drbd_md_put_buffer(mdev);
@@ -474,20 +606,18 @@ static int w_al_write_transaction(struct drbd_work *w, int unused)
 /* Calls from worker context (see w_restart_disk_io()) need to write the
    transaction directly. Others came through generic_make_request(),
    those need to delegate it to the worker. */
-static int al_write_transaction(struct drbd_conf *mdev)
+static int al_write_transaction(struct drbd_conf *mdev, bool delegate)
 {
-	struct update_al_work al_work;
-
-	if (current == mdev->tconn->worker.task)
+	if (delegate) {
+		struct update_al_work al_work;
+		init_completion(&al_work.event);
+		al_work.w.cb = w_al_write_transaction;
+		al_work.w.mdev = mdev;
+		drbd_queue_work_front(&mdev->tconn->sender_work, &al_work.w);
+		wait_for_completion(&al_work.event);
+		return al_work.err;
+	} else
 		return _al_write_transaction(mdev);
-
-	init_completion(&al_work.event);
-	al_work.w.cb = w_al_write_transaction;
-	al_work.w.mdev = mdev;
-	drbd_queue_work_front(&mdev->tconn->sender_work, &al_work.w);
-	wait_for_completion(&al_work.event);
-
-	return al_work.err;
 }
 
 static int _try_lc_del(struct drbd_conf *mdev, struct lc_element *al_ext)
diff --git a/drivers/block/drbd/drbd_bitmap.c b/drivers/block/drbd/drbd_bitmap.c
index 8dc29502dc08..64fbb8385cdc 100644
--- a/drivers/block/drbd/drbd_bitmap.c
+++ b/drivers/block/drbd/drbd_bitmap.c
@@ -612,6 +612,17 @@ static void bm_memset(struct drbd_bitmap *b, size_t offset, int c, size_t len)
 	}
 }
 
+/* For the layout, see comment above drbd_md_set_sector_offsets(). */
+static u64 drbd_md_on_disk_bits(struct drbd_backing_dev *ldev)
+{
+	u64 bitmap_sectors;
+	if (ldev->md.al_offset == 8)
+		bitmap_sectors = ldev->md.md_size_sect - ldev->md.bm_offset;
+	else
+		bitmap_sectors = ldev->md.al_offset - ldev->md.bm_offset;
+	return bitmap_sectors << (9 + 3);
+}
+
 /*
  * make sure the bitmap has enough room for the attached storage,
  * if necessary, resize.
@@ -668,7 +679,7 @@ int drbd_bm_resize(struct drbd_conf *mdev, sector_t capacity, int set_new_bits)
 	words = ALIGN(bits, 64) >> LN2_BPL;
 
 	if (get_ldev(mdev)) {
-		u64 bits_on_disk = ((u64)mdev->ldev->md.md_size_sect-MD_BM_OFFSET) << 12;
+		u64 bits_on_disk = drbd_md_on_disk_bits(mdev->ldev);
 		put_ldev(mdev);
 		if (bits > bits_on_disk) {
 			dev_info(DEV, "bits = %lu\n", bits);
diff --git a/drivers/block/drbd/drbd_int.h b/drivers/block/drbd/drbd_int.h
index 6b51afa1aae1..f943aacfdad8 100644
--- a/drivers/block/drbd/drbd_int.h
+++ b/drivers/block/drbd/drbd_int.h
@@ -753,13 +753,16 @@ struct drbd_md {
 	u32 flags;
 	u32 md_size_sect;
 
-	s32 al_offset;	/* signed relative sector offset to al area */
+	s32 al_offset;	/* signed relative sector offset to activity log */
 	s32 bm_offset;	/* signed relative sector offset to bitmap */
 
-	/* u32 al_nr_extents;	   important for restoring the AL
-	 * is stored into  ldev->dc.al_extents, which in turn
-	 * gets applied to act_log->nr_elements
-	 */
+	/* cached value of bdev->disk_conf->meta_dev_idx (see below) */
+	s32 meta_dev_idx;
+
+	/* see al_tr_number_to_on_disk_sector() */
+	u32 al_stripes;
+	u32 al_stripe_size_4k;
+	u32 al_size_4k; /* cached product of the above */
 };
 
 struct drbd_backing_dev {
@@ -891,6 +894,14 @@ struct drbd_tconn {			/* is a resource from the config file */
 	} send;
 };
 
+struct submit_worker {
+	struct workqueue_struct *wq;
+	struct work_struct worker;
+
+	spinlock_t lock;
+	struct list_head writes;
+};
+
 struct drbd_conf {
 	struct drbd_tconn *tconn;
 	int vnr;			/* volume number within the connection */
@@ -1009,7 +1020,6 @@ struct drbd_conf {
 	struct lru_cache *act_log;	/* activity log */
 	unsigned int al_tr_number;
 	int al_tr_cycle;
-	int al_tr_pos;   /* position of the next transaction in the journal */
 	wait_queue_head_t seq_wait;
 	atomic_t packet_seq;
 	unsigned int peer_seq;
@@ -1032,6 +1042,10 @@ struct drbd_conf {
 	atomic_t ap_in_flight; /* App sectors in flight (waiting for ack) */
 	unsigned int peer_max_bio_size;
 	unsigned int local_max_bio_size;
+
+	/* any requests that would block in drbd_make_request()
+	 * are deferred to this single-threaded work queue */
+	struct submit_worker submit;
 };
 
 static inline struct drbd_conf *minor_to_mdev(unsigned int minor)
@@ -1148,25 +1162,44 @@ extern int drbd_bitmap_io_from_worker(struct drbd_conf *mdev,
 		char *why, enum bm_flag flags);
 extern int drbd_bmio_set_n_write(struct drbd_conf *mdev);
 extern int drbd_bmio_clear_n_write(struct drbd_conf *mdev);
-extern void drbd_go_diskless(struct drbd_conf *mdev);
 extern void drbd_ldev_destroy(struct drbd_conf *mdev);
 
 /* Meta data layout
-   We reserve a 128MB Block (4k aligned)
-   * either at the end of the backing device
-   * or on a separate meta data device. */
+ *
+ * We currently have two possible layouts.
+ * Offsets in (512 byte) sectors.
+ * external:
+ *   |----------- md_size_sect ------------------|
+ *   [ 4k superblock ][ activity log ][  Bitmap  ]
+ *   | al_offset == 8 |
+ *   | bm_offset = al_offset + X      |
+ *  ==> bitmap sectors = md_size_sect - bm_offset
+ *
+ *  Variants:
+ *     old, indexed fixed size meta data:
+ *
+ * internal:
+ *            |----------- md_size_sect ------------------|
+ * [data.....][  Bitmap  ][ activity log ][ 4k superblock ][padding*]
+ *                        | al_offset < 0 |
+ *            | bm_offset = al_offset - Y |
+ *  ==> bitmap sectors = Y = al_offset - bm_offset
+ *
+ *  [padding*] are zero or up to 7 unused 512 Byte sectors to the
+ *  end of the device, so that the [4k superblock] will be 4k aligned.
+ *
+ *  The activity log consists of 4k transaction blocks,
+ *  which are written in a ring-buffer, or striped ring-buffer like fashion,
+ *  which are writtensize used to be fixed 32kB,
+ *  but is about to become configurable.
+ */
 
-/* The following numbers are sectors */
-/* Allows up to about 3.8TB, so if you want more,
+/* Our old fixed size meta data layout
+ * allows up to about 3.8TB, so if you want more,
  * you need to use the "flexible" meta data format. */
-#define MD_RESERVED_SECT (128LU << 11)  /* 128 MB, unit sectors */
-#define MD_AL_OFFSET	8    /* 8 Sectors after start of meta area */
-#define MD_AL_SECTORS	64   /* = 32 kB on disk activity log ring buffer */
-#define MD_BM_OFFSET (MD_AL_OFFSET + MD_AL_SECTORS)
-
-/* we do all meta data IO in 4k blocks */
-#define MD_BLOCK_SHIFT	12
-#define MD_BLOCK_SIZE	(1<<MD_BLOCK_SHIFT)
+#define MD_128MB_SECT (128LLU << 11)  /* 128 MB, unit sectors */
+#define MD_4kB_SECT	 8
+#define MD_32kB_SECT	64
 
 /* One activity log extent represents 4M of storage */
 #define AL_EXTENT_SHIFT 22
@@ -1256,7 +1289,6 @@ struct bm_extent {
 
 /* in one sector of the bitmap, we have this many activity_log extents. */
 #define AL_EXT_PER_BM_SECT  (1 << (BM_EXT_SHIFT - AL_EXTENT_SHIFT))
-#define BM_WORDS_PER_AL_EXT (1 << (AL_EXTENT_SHIFT-BM_BLOCK_SHIFT-LN2_BPL))
 
 #define BM_BLOCKS_PER_BM_EXT_B (BM_EXT_SHIFT - BM_BLOCK_SHIFT)
 #define BM_BLOCKS_PER_BM_EXT_MASK  ((1<<BM_BLOCKS_PER_BM_EXT_B) - 1)
@@ -1276,16 +1308,18 @@ struct bm_extent {
  */
 
 #define DRBD_MAX_SECTORS_32 (0xffffffffLU)
-#define DRBD_MAX_SECTORS_BM \
-	  ((MD_RESERVED_SECT - MD_BM_OFFSET) * (1LL<<(BM_EXT_SHIFT-9)))
-#if DRBD_MAX_SECTORS_BM < DRBD_MAX_SECTORS_32
-#define DRBD_MAX_SECTORS      DRBD_MAX_SECTORS_BM
-#define DRBD_MAX_SECTORS_FLEX DRBD_MAX_SECTORS_BM
-#elif !defined(CONFIG_LBDAF) && BITS_PER_LONG == 32
+/* we have a certain meta data variant that has a fixed on-disk size of 128
+ * MiB, of which 4k are our "superblock", and 32k are the fixed size activity
+ * log, leaving this many sectors for the bitmap.
+ */
+
+#define DRBD_MAX_SECTORS_FIXED_BM \
+	  ((MD_128MB_SECT - MD_32kB_SECT - MD_4kB_SECT) * (1LL<<(BM_EXT_SHIFT-9)))
+#if !defined(CONFIG_LBDAF) && BITS_PER_LONG == 32
 #define DRBD_MAX_SECTORS      DRBD_MAX_SECTORS_32
 #define DRBD_MAX_SECTORS_FLEX DRBD_MAX_SECTORS_32
 #else
-#define DRBD_MAX_SECTORS      DRBD_MAX_SECTORS_BM
+#define DRBD_MAX_SECTORS      DRBD_MAX_SECTORS_FIXED_BM
 /* 16 TB in units of sectors */
 #if BITS_PER_LONG == 32
 /* adjust by one page worth of bitmap,
@@ -1418,6 +1452,7 @@ extern void conn_free_crypto(struct drbd_tconn *tconn);
 extern int proc_details;
 
 /* drbd_req */
+extern void do_submit(struct work_struct *ws);
 extern void __drbd_make_request(struct drbd_conf *, struct bio *, unsigned long);
 extern void drbd_make_request(struct request_queue *q, struct bio *bio);
 extern int drbd_read_remote(struct drbd_conf *mdev, struct drbd_request *req);
@@ -1576,7 +1611,10 @@ extern const char *drbd_conn_str(enum drbd_conns s);
 extern const char *drbd_role_str(enum drbd_role s);
 
 /* drbd_actlog.c */
-extern void drbd_al_begin_io(struct drbd_conf *mdev, struct drbd_interval *i);
+extern int drbd_al_begin_io_nonblock(struct drbd_conf *mdev, struct drbd_interval *i);
+extern void drbd_al_begin_io_commit(struct drbd_conf *mdev, bool delegate);
+extern bool drbd_al_begin_io_fastpath(struct drbd_conf *mdev, struct drbd_interval *i);
+extern void drbd_al_begin_io(struct drbd_conf *mdev, struct drbd_interval *i, bool delegate);
 extern void drbd_al_complete_io(struct drbd_conf *mdev, struct drbd_interval *i);
 extern void drbd_rs_complete_io(struct drbd_conf *mdev, sector_t sector);
 extern int drbd_rs_begin_io(struct drbd_conf *mdev, sector_t sector);
@@ -1755,9 +1793,9 @@ static inline void drbd_chk_io_error_(struct drbd_conf *mdev,
  * BTW, for internal meta data, this happens to be the maximum capacity
  * we could agree upon with our peer node.
  */
-static inline sector_t _drbd_md_first_sector(int meta_dev_idx, struct drbd_backing_dev *bdev)
+static inline sector_t drbd_md_first_sector(struct drbd_backing_dev *bdev)
 {
-	switch (meta_dev_idx) {
+	switch (bdev->md.meta_dev_idx) {
 	case DRBD_MD_INDEX_INTERNAL:
 	case DRBD_MD_INDEX_FLEX_INT:
 		return bdev->md.md_offset + bdev->md.bm_offset;
@@ -1767,36 +1805,19 @@ static inline sector_t _drbd_md_first_sector(int meta_dev_idx, struct drbd_backi
 	}
 }
 
-static inline sector_t drbd_md_first_sector(struct drbd_backing_dev *bdev)
-{
-	int meta_dev_idx;
-
-	rcu_read_lock();
-	meta_dev_idx = rcu_dereference(bdev->disk_conf)->meta_dev_idx;
-	rcu_read_unlock();
-
-	return _drbd_md_first_sector(meta_dev_idx, bdev);
-}
-
 /**
  * drbd_md_last_sector() - Return the last sector number of the meta data area
  * @bdev:	Meta data block device.
  */
 static inline sector_t drbd_md_last_sector(struct drbd_backing_dev *bdev)
 {
-	int meta_dev_idx;
-
-	rcu_read_lock();
-	meta_dev_idx = rcu_dereference(bdev->disk_conf)->meta_dev_idx;
-	rcu_read_unlock();
-
-	switch (meta_dev_idx) {
+	switch (bdev->md.meta_dev_idx) {
 	case DRBD_MD_INDEX_INTERNAL:
 	case DRBD_MD_INDEX_FLEX_INT:
-		return bdev->md.md_offset + MD_AL_OFFSET - 1;
+		return bdev->md.md_offset + MD_4kB_SECT -1;
 	case DRBD_MD_INDEX_FLEX_EXT:
 	default:
-		return bdev->md.md_offset + bdev->md.md_size_sect;
+		return bdev->md.md_offset + bdev->md.md_size_sect -1;
 	}
 }
 
@@ -1818,18 +1839,13 @@ static inline sector_t drbd_get_capacity(struct block_device *bdev)
 static inline sector_t drbd_get_max_capacity(struct drbd_backing_dev *bdev)
 {
 	sector_t s;
-	int meta_dev_idx;
 
-	rcu_read_lock();
-	meta_dev_idx = rcu_dereference(bdev->disk_conf)->meta_dev_idx;
-	rcu_read_unlock();
-
-	switch (meta_dev_idx) {
+	switch (bdev->md.meta_dev_idx) {
 	case DRBD_MD_INDEX_INTERNAL:
 	case DRBD_MD_INDEX_FLEX_INT:
 		s = drbd_get_capacity(bdev->backing_bdev)
 			? min_t(sector_t, DRBD_MAX_SECTORS_FLEX,
-				_drbd_md_first_sector(meta_dev_idx, bdev))
+				drbd_md_first_sector(bdev))
 			: 0;
 		break;
 	case DRBD_MD_INDEX_FLEX_EXT:
@@ -1848,39 +1864,24 @@ static inline sector_t drbd_get_max_capacity(struct drbd_backing_dev *bdev)
 }
 
 /**
- * drbd_md_ss__() - Return the sector number of our meta data super block
- * @mdev:	DRBD device.
+ * drbd_md_ss() - Return the sector number of our meta data super block
  * @bdev:	Meta data block device.
  */
-static inline sector_t drbd_md_ss__(struct drbd_conf *mdev,
-				    struct drbd_backing_dev *bdev)
+static inline sector_t drbd_md_ss(struct drbd_backing_dev *bdev)
 {
-	int meta_dev_idx;
+	const int meta_dev_idx = bdev->md.meta_dev_idx;
 
-	rcu_read_lock();
-	meta_dev_idx = rcu_dereference(bdev->disk_conf)->meta_dev_idx;
-	rcu_read_unlock();
-
-	switch (meta_dev_idx) {
-	default: /* external, some index */
-		return MD_RESERVED_SECT * meta_dev_idx;
-	case DRBD_MD_INDEX_INTERNAL:
-		/* with drbd08, internal meta data is always "flexible" */
-	case DRBD_MD_INDEX_FLEX_INT:
-		/* sizeof(struct md_on_disk_07) == 4k
-		 * position: last 4k aligned block of 4k size */
-		if (!bdev->backing_bdev) {
-			if (__ratelimit(&drbd_ratelimit_state)) {
-				dev_err(DEV, "bdev->backing_bdev==NULL\n");
-				dump_stack();
-			}
-			return 0;
-		}
-		return (drbd_get_capacity(bdev->backing_bdev) & ~7ULL)
-			- MD_AL_OFFSET;
-	case DRBD_MD_INDEX_FLEX_EXT:
+	if (meta_dev_idx == DRBD_MD_INDEX_FLEX_EXT)
 		return 0;
-	}
+
+	/* Since drbd08, internal meta data is always "flexible".
+	 * position: last 4k aligned block of 4k size */
+	if (meta_dev_idx == DRBD_MD_INDEX_INTERNAL ||
+	    meta_dev_idx == DRBD_MD_INDEX_FLEX_INT)
+		return (drbd_get_capacity(bdev->backing_bdev) & ~7ULL) - 8;
+
+	/* external, some index; this is the old fixed size layout */
+	return MD_128MB_SECT * bdev->md.meta_dev_idx;
 }
 
 static inline void
@@ -2053,9 +2054,11 @@ static inline void put_ldev(struct drbd_conf *mdev)
 		if (mdev->state.disk == D_DISKLESS)
 			/* even internal references gone, safe to destroy */
 			drbd_ldev_destroy(mdev);
-		if (mdev->state.disk == D_FAILED)
+		if (mdev->state.disk == D_FAILED) {
 			/* all application IO references gone. */
-			drbd_go_diskless(mdev);
+			if (!test_and_set_bit(GO_DISKLESS, &mdev->flags))
+				drbd_queue_work(&mdev->tconn->sender_work, &mdev->go_diskless);
+		}
 		wake_up(&mdev->misc_wait);
 	}
 }
diff --git a/drivers/block/drbd/drbd_main.c b/drivers/block/drbd/drbd_main.c
index e98da675f0c1..a5dca6affcbb 100644
--- a/drivers/block/drbd/drbd_main.c
+++ b/drivers/block/drbd/drbd_main.c
@@ -45,7 +45,7 @@
 #include <linux/reboot.h>
 #include <linux/notifier.h>
 #include <linux/kthread.h>
-
+#include <linux/workqueue.h>
 #define __KERNEL_SYSCALLS__
 #include <linux/unistd.h>
 #include <linux/vmalloc.h>
@@ -63,7 +63,7 @@ int drbd_asender(struct drbd_thread *);
 
 int drbd_init(void);
 static int drbd_open(struct block_device *bdev, fmode_t mode);
-static int drbd_release(struct gendisk *gd, fmode_t mode);
+static void drbd_release(struct gendisk *gd, fmode_t mode);
 static int w_md_sync(struct drbd_work *w, int unused);
 static void md_sync_timer_fn(unsigned long data);
 static int w_bitmap_io(struct drbd_work *w, int unused);
@@ -1849,13 +1849,12 @@ static int drbd_open(struct block_device *bdev, fmode_t mode)
 	return rv;
 }
 
-static int drbd_release(struct gendisk *gd, fmode_t mode)
+static void drbd_release(struct gendisk *gd, fmode_t mode)
 {
 	struct drbd_conf *mdev = gd->private_data;
 	mutex_lock(&drbd_main_mutex);
 	mdev->open_cnt--;
 	mutex_unlock(&drbd_main_mutex);
-	return 0;
 }
 
 static void drbd_set_defaults(struct drbd_conf *mdev)
@@ -2300,6 +2299,7 @@ static void drbd_cleanup(void)
 	idr_for_each_entry(&minors, mdev, i) {
 		idr_remove(&minors, mdev_to_minor(mdev));
 		idr_remove(&mdev->tconn->volumes, mdev->vnr);
+		destroy_workqueue(mdev->submit.wq);
 		del_gendisk(mdev->vdisk);
 		/* synchronize_rcu(); No other threads running at this point */
 		kref_put(&mdev->kref, &drbd_minor_destroy);
@@ -2589,6 +2589,21 @@ void conn_destroy(struct kref *kref)
 	kfree(tconn);
 }
 
+int init_submitter(struct drbd_conf *mdev)
+{
+	/* opencoded create_singlethread_workqueue(),
+	 * to be able to say "drbd%d", ..., minor */
+	mdev->submit.wq = alloc_workqueue("drbd%u_submit",
+			WQ_UNBOUND | WQ_MEM_RECLAIM, 1, mdev->minor);
+	if (!mdev->submit.wq)
+		return -ENOMEM;
+
+	INIT_WORK(&mdev->submit.worker, do_submit);
+	spin_lock_init(&mdev->submit.lock);
+	INIT_LIST_HEAD(&mdev->submit.writes);
+	return 0;
+}
+
 enum drbd_ret_code conn_new_minor(struct drbd_tconn *tconn, unsigned int minor, int vnr)
 {
 	struct drbd_conf *mdev;
@@ -2678,6 +2693,12 @@ enum drbd_ret_code conn_new_minor(struct drbd_tconn *tconn, unsigned int minor,
 		goto out_idr_remove_minor;
 	}
 
+	if (init_submitter(mdev)) {
+		err = ERR_NOMEM;
+		drbd_msg_put_info("unable to create submit workqueue");
+		goto out_idr_remove_vol;
+	}
+
 	add_disk(disk);
 	kref_init(&mdev->kref); /* one ref for both idrs and the the add_disk */
 
@@ -2688,6 +2709,8 @@ enum drbd_ret_code conn_new_minor(struct drbd_tconn *tconn, unsigned int minor,
 
 	return NO_ERROR;
 
+out_idr_remove_vol:
+	idr_remove(&tconn->volumes, vnr_got);
 out_idr_remove_minor:
 	idr_remove(&minors, minor_got);
 	synchronize_rcu();
@@ -2795,6 +2818,7 @@ void drbd_free_bc(struct drbd_backing_dev *ldev)
 	blkdev_put(ldev->backing_bdev, FMODE_READ | FMODE_WRITE | FMODE_EXCL);
 	blkdev_put(ldev->md_bdev, FMODE_READ | FMODE_WRITE | FMODE_EXCL);
 
+	kfree(ldev->disk_conf);
 	kfree(ldev);
 }
 
@@ -2834,8 +2858,9 @@ void conn_md_sync(struct drbd_tconn *tconn)
 	rcu_read_unlock();
 }
 
+/* aligned 4kByte */
 struct meta_data_on_disk {
-	u64 la_size;           /* last agreed size. */
+	u64 la_size_sect;      /* last agreed size. */
 	u64 uuid[UI_SIZE];   /* UUIDs. */
 	u64 device_uuid;
 	u64 reserved_u64_1;
@@ -2843,13 +2868,17 @@ struct meta_data_on_disk {
 	u32 magic;
 	u32 md_size_sect;
 	u32 al_offset;         /* offset to this block */
-	u32 al_nr_extents;     /* important for restoring the AL */
+	u32 al_nr_extents;     /* important for restoring the AL (userspace) */
 	      /* `-- act_log->nr_elements <-- ldev->dc.al_extents */
 	u32 bm_offset;         /* offset to the bitmap, from here */
 	u32 bm_bytes_per_bit;  /* BM_BLOCK_SIZE */
 	u32 la_peer_max_bio_size;   /* last peer max_bio_size */
-	u32 reserved_u32[3];
 
+	/* see al_tr_number_to_on_disk_sector() */
+	u32 al_stripes;
+	u32 al_stripe_size_4k;
+
+	u8 reserved_u8[4096 - (7*8 + 10*4)];
 } __packed;
 
 /**
@@ -2862,6 +2891,10 @@ void drbd_md_sync(struct drbd_conf *mdev)
 	sector_t sector;
 	int i;
 
+	/* Don't accidentally change the DRBD meta data layout. */
+	BUILD_BUG_ON(UI_SIZE != 4);
+	BUILD_BUG_ON(sizeof(struct meta_data_on_disk) != 4096);
+
 	del_timer(&mdev->md_sync_timer);
 	/* timer may be rearmed by drbd_md_mark_dirty() now. */
 	if (!test_and_clear_bit(MD_DIRTY, &mdev->flags))
@@ -2876,9 +2909,9 @@ void drbd_md_sync(struct drbd_conf *mdev)
 	if (!buffer)
 		goto out;
 
-	memset(buffer, 0, 512);
+	memset(buffer, 0, sizeof(*buffer));
 
-	buffer->la_size = cpu_to_be64(drbd_get_capacity(mdev->this_bdev));
+	buffer->la_size_sect = cpu_to_be64(drbd_get_capacity(mdev->this_bdev));
 	for (i = UI_CURRENT; i < UI_SIZE; i++)
 		buffer->uuid[i] = cpu_to_be64(mdev->ldev->md.uuid[i]);
 	buffer->flags = cpu_to_be32(mdev->ldev->md.flags);
@@ -2893,7 +2926,10 @@ void drbd_md_sync(struct drbd_conf *mdev)
 	buffer->bm_offset = cpu_to_be32(mdev->ldev->md.bm_offset);
 	buffer->la_peer_max_bio_size = cpu_to_be32(mdev->peer_max_bio_size);
 
-	D_ASSERT(drbd_md_ss__(mdev, mdev->ldev) == mdev->ldev->md.md_offset);
+	buffer->al_stripes = cpu_to_be32(mdev->ldev->md.al_stripes);
+	buffer->al_stripe_size_4k = cpu_to_be32(mdev->ldev->md.al_stripe_size_4k);
+
+	D_ASSERT(drbd_md_ss(mdev->ldev) == mdev->ldev->md.md_offset);
 	sector = mdev->ldev->md.md_offset;
 
 	if (drbd_md_sync_page_io(mdev, mdev->ldev, sector, WRITE)) {
@@ -2911,13 +2947,141 @@ out:
 	put_ldev(mdev);
 }
 
+static int check_activity_log_stripe_size(struct drbd_conf *mdev,
+		struct meta_data_on_disk *on_disk,
+		struct drbd_md *in_core)
+{
+	u32 al_stripes = be32_to_cpu(on_disk->al_stripes);
+	u32 al_stripe_size_4k = be32_to_cpu(on_disk->al_stripe_size_4k);
+	u64 al_size_4k;
+
+	/* both not set: default to old fixed size activity log */
+	if (al_stripes == 0 && al_stripe_size_4k == 0) {
+		al_stripes = 1;
+		al_stripe_size_4k = MD_32kB_SECT/8;
+	}
+
+	/* some paranoia plausibility checks */
+
+	/* we need both values to be set */
+	if (al_stripes == 0 || al_stripe_size_4k == 0)
+		goto err;
+
+	al_size_4k = (u64)al_stripes * al_stripe_size_4k;
+
+	/* Upper limit of activity log area, to avoid potential overflow
+	 * problems in al_tr_number_to_on_disk_sector(). As right now, more
+	 * than 72 * 4k blocks total only increases the amount of history,
+	 * limiting this arbitrarily to 16 GB is not a real limitation ;-)  */
+	if (al_size_4k > (16 * 1024 * 1024/4))
+		goto err;
+
+	/* Lower limit: we need at least 8 transaction slots (32kB)
+	 * to not break existing setups */
+	if (al_size_4k < MD_32kB_SECT/8)
+		goto err;
+
+	in_core->al_stripe_size_4k = al_stripe_size_4k;
+	in_core->al_stripes = al_stripes;
+	in_core->al_size_4k = al_size_4k;
+
+	return 0;
+err:
+	dev_err(DEV, "invalid activity log striping: al_stripes=%u, al_stripe_size_4k=%u\n",
+			al_stripes, al_stripe_size_4k);
+	return -EINVAL;
+}
+
+static int check_offsets_and_sizes(struct drbd_conf *mdev, struct drbd_backing_dev *bdev)
+{
+	sector_t capacity = drbd_get_capacity(bdev->md_bdev);
+	struct drbd_md *in_core = &bdev->md;
+	s32 on_disk_al_sect;
+	s32 on_disk_bm_sect;
+
+	/* The on-disk size of the activity log, calculated from offsets, and
+	 * the size of the activity log calculated from the stripe settings,
+	 * should match.
+	 * Though we could relax this a bit: it is ok, if the striped activity log
+	 * fits in the available on-disk activity log size.
+	 * Right now, that would break how resize is implemented.
+	 * TODO: make drbd_determine_dev_size() (and the drbdmeta tool) aware
+	 * of possible unused padding space in the on disk layout. */
+	if (in_core->al_offset < 0) {
+		if (in_core->bm_offset > in_core->al_offset)
+			goto err;
+		on_disk_al_sect = -in_core->al_offset;
+		on_disk_bm_sect = in_core->al_offset - in_core->bm_offset;
+	} else {
+		if (in_core->al_offset != MD_4kB_SECT)
+			goto err;
+		if (in_core->bm_offset < in_core->al_offset + in_core->al_size_4k * MD_4kB_SECT)
+			goto err;
+
+		on_disk_al_sect = in_core->bm_offset - MD_4kB_SECT;
+		on_disk_bm_sect = in_core->md_size_sect - in_core->bm_offset;
+	}
+
+	/* old fixed size meta data is exactly that: fixed. */
+	if (in_core->meta_dev_idx >= 0) {
+		if (in_core->md_size_sect != MD_128MB_SECT
+		||  in_core->al_offset != MD_4kB_SECT
+		||  in_core->bm_offset != MD_4kB_SECT + MD_32kB_SECT
+		||  in_core->al_stripes != 1
+		||  in_core->al_stripe_size_4k != MD_32kB_SECT/8)
+			goto err;
+	}
+
+	if (capacity < in_core->md_size_sect)
+		goto err;
+	if (capacity - in_core->md_size_sect < drbd_md_first_sector(bdev))
+		goto err;
+
+	/* should be aligned, and at least 32k */
+	if ((on_disk_al_sect & 7) || (on_disk_al_sect < MD_32kB_SECT))
+		goto err;
+
+	/* should fit (for now: exactly) into the available on-disk space;
+	 * overflow prevention is in check_activity_log_stripe_size() above. */
+	if (on_disk_al_sect != in_core->al_size_4k * MD_4kB_SECT)
+		goto err;
+
+	/* again, should be aligned */
+	if (in_core->bm_offset & 7)
+		goto err;
+
+	/* FIXME check for device grow with flex external meta data? */
+
+	/* can the available bitmap space cover the last agreed device size? */
+	if (on_disk_bm_sect < (in_core->la_size_sect+7)/MD_4kB_SECT/8/512)
+		goto err;
+
+	return 0;
+
+err:
+	dev_err(DEV, "meta data offsets don't make sense: idx=%d "
+			"al_s=%u, al_sz4k=%u, al_offset=%d, bm_offset=%d, "
+			"md_size_sect=%u, la_size=%llu, md_capacity=%llu\n",
+			in_core->meta_dev_idx,
+			in_core->al_stripes, in_core->al_stripe_size_4k,
+			in_core->al_offset, in_core->bm_offset, in_core->md_size_sect,
+			(unsigned long long)in_core->la_size_sect,
+			(unsigned long long)capacity);
+
+	return -EINVAL;
+}
+
+
 /**
  * drbd_md_read() - Reads in the meta data super block
  * @mdev:	DRBD device.
  * @bdev:	Device from which the meta data should be read in.
  *
- * Return 0 (NO_ERROR) on success, and an enum drbd_ret_code in case
+ * Return NO_ERROR on success, and an enum drbd_ret_code in case
  * something goes wrong.
+ *
+ * Called exactly once during drbd_adm_attach(), while still being D_DISKLESS,
+ * even before @bdev is assigned to @mdev->ldev.
  */
 int drbd_md_read(struct drbd_conf *mdev, struct drbd_backing_dev *bdev)
 {
@@ -2925,12 +3089,17 @@ int drbd_md_read(struct drbd_conf *mdev, struct drbd_backing_dev *bdev)
 	u32 magic, flags;
 	int i, rv = NO_ERROR;
 
-	if (!get_ldev_if_state(mdev, D_ATTACHING))
-		return ERR_IO_MD_DISK;
+	if (mdev->state.disk != D_DISKLESS)
+		return ERR_DISK_CONFIGURED;
 
 	buffer = drbd_md_get_buffer(mdev);
 	if (!buffer)
-		goto out;
+		return ERR_NOMEM;
+
+	/* First, figure out where our meta data superblock is located,
+	 * and read it. */
+	bdev->md.meta_dev_idx = bdev->disk_conf->meta_dev_idx;
+	bdev->md.md_offset = drbd_md_ss(bdev);
 
 	if (drbd_md_sync_page_io(mdev, bdev, bdev->md.md_offset, READ)) {
 		/* NOTE: can't do normal error processing here as this is
@@ -2949,45 +3118,51 @@ int drbd_md_read(struct drbd_conf *mdev, struct drbd_backing_dev *bdev)
 		rv = ERR_MD_UNCLEAN;
 		goto err;
 	}
+
+	rv = ERR_MD_INVALID;
 	if (magic != DRBD_MD_MAGIC_08) {
 		if (magic == DRBD_MD_MAGIC_07)
 			dev_err(DEV, "Found old (0.7) meta data magic. Did you \"drbdadm create-md\"?\n");
 		else
 			dev_err(DEV, "Meta data magic not found. Did you \"drbdadm create-md\"?\n");
-		rv = ERR_MD_INVALID;
 		goto err;
 	}
-	if (be32_to_cpu(buffer->al_offset) != bdev->md.al_offset) {
-		dev_err(DEV, "unexpected al_offset: %d (expected %d)\n",
-		    be32_to_cpu(buffer->al_offset), bdev->md.al_offset);
-		rv = ERR_MD_INVALID;
+
+	if (be32_to_cpu(buffer->bm_bytes_per_bit) != BM_BLOCK_SIZE) {
+		dev_err(DEV, "unexpected bm_bytes_per_bit: %u (expected %u)\n",
+		    be32_to_cpu(buffer->bm_bytes_per_bit), BM_BLOCK_SIZE);
 		goto err;
 	}
+
+
+	/* convert to in_core endian */
+	bdev->md.la_size_sect = be64_to_cpu(buffer->la_size_sect);
+	for (i = UI_CURRENT; i < UI_SIZE; i++)
+		bdev->md.uuid[i] = be64_to_cpu(buffer->uuid[i]);
+	bdev->md.flags = be32_to_cpu(buffer->flags);
+	bdev->md.device_uuid = be64_to_cpu(buffer->device_uuid);
+
+	bdev->md.md_size_sect = be32_to_cpu(buffer->md_size_sect);
+	bdev->md.al_offset = be32_to_cpu(buffer->al_offset);
+	bdev->md.bm_offset = be32_to_cpu(buffer->bm_offset);
+
+	if (check_activity_log_stripe_size(mdev, buffer, &bdev->md))
+		goto err;
+	if (check_offsets_and_sizes(mdev, bdev))
+		goto err;
+
 	if (be32_to_cpu(buffer->bm_offset) != bdev->md.bm_offset) {
 		dev_err(DEV, "unexpected bm_offset: %d (expected %d)\n",
 		    be32_to_cpu(buffer->bm_offset), bdev->md.bm_offset);
-		rv = ERR_MD_INVALID;
 		goto err;
 	}
 	if (be32_to_cpu(buffer->md_size_sect) != bdev->md.md_size_sect) {
 		dev_err(DEV, "unexpected md_size: %u (expected %u)\n",
 		    be32_to_cpu(buffer->md_size_sect), bdev->md.md_size_sect);
-		rv = ERR_MD_INVALID;
 		goto err;
 	}
 
-	if (be32_to_cpu(buffer->bm_bytes_per_bit) != BM_BLOCK_SIZE) {
-		dev_err(DEV, "unexpected bm_bytes_per_bit: %u (expected %u)\n",
-		    be32_to_cpu(buffer->bm_bytes_per_bit), BM_BLOCK_SIZE);
-		rv = ERR_MD_INVALID;
-		goto err;
-	}
-
-	bdev->md.la_size_sect = be64_to_cpu(buffer->la_size);
-	for (i = UI_CURRENT; i < UI_SIZE; i++)
-		bdev->md.uuid[i] = be64_to_cpu(buffer->uuid[i]);
-	bdev->md.flags = be32_to_cpu(buffer->flags);
-	bdev->md.device_uuid = be64_to_cpu(buffer->device_uuid);
+	rv = NO_ERROR;
 
 	spin_lock_irq(&mdev->tconn->req_lock);
 	if (mdev->state.conn < C_CONNECTED) {
@@ -3000,8 +3175,6 @@ int drbd_md_read(struct drbd_conf *mdev, struct drbd_backing_dev *bdev)
 
  err:
 	drbd_md_put_buffer(mdev);
- out:
-	put_ldev(mdev);
 
 	return rv;
 }
@@ -3239,8 +3412,12 @@ static int w_go_diskless(struct drbd_work *w, int unused)
 	 * end up here after a failed attach, before ldev was even assigned.
 	 */
 	if (mdev->bitmap && mdev->ldev) {
+		/* An interrupted resync or similar is allowed to recounts bits
+		 * while we detach.
+		 * Any modifications would not be expected anymore, though.
+		 */
 		if (drbd_bitmap_io_from_worker(mdev, drbd_bm_write,
-					"detach", BM_LOCKED_MASK)) {
+					"detach", BM_LOCKED_TEST_ALLOWED)) {
 			if (test_bit(WAS_READ_ERROR, &mdev->flags)) {
 				drbd_md_set_flag(mdev, MDF_FULL_SYNC);
 				drbd_md_sync(mdev);
@@ -3252,13 +3429,6 @@ static int w_go_diskless(struct drbd_work *w, int unused)
 	return 0;
 }
 
-void drbd_go_diskless(struct drbd_conf *mdev)
-{
-	D_ASSERT(mdev->state.disk == D_FAILED);
-	if (!test_and_set_bit(GO_DISKLESS, &mdev->flags))
-		drbd_queue_work(&mdev->tconn->sender_work, &mdev->go_diskless);
-}
-
 /**
  * drbd_queue_bitmap_io() - Queues an IO operation on the whole bitmap
  * @mdev:	DRBD device.
diff --git a/drivers/block/drbd/drbd_nl.c b/drivers/block/drbd/drbd_nl.c
index 2af26fc95280..9e3f441e7e84 100644
--- a/drivers/block/drbd/drbd_nl.c
+++ b/drivers/block/drbd/drbd_nl.c
@@ -696,37 +696,52 @@ out:
 	return 0;
 }
 
-/* initializes the md.*_offset members, so we are able to find
- * the on disk meta data */
+/* Initializes the md.*_offset members, so we are able to find
+ * the on disk meta data.
+ *
+ * We currently have two possible layouts:
+ * external:
+ *   |----------- md_size_sect ------------------|
+ *   [ 4k superblock ][ activity log ][  Bitmap  ]
+ *   | al_offset == 8 |
+ *   | bm_offset = al_offset + X      |
+ *  ==> bitmap sectors = md_size_sect - bm_offset
+ *
+ * internal:
+ *            |----------- md_size_sect ------------------|
+ * [data.....][  Bitmap  ][ activity log ][ 4k superblock ]
+ *                        | al_offset < 0 |
+ *            | bm_offset = al_offset - Y |
+ *  ==> bitmap sectors = Y = al_offset - bm_offset
+ *
+ *  Activity log size used to be fixed 32kB,
+ *  but is about to become configurable.
+ */
 static void drbd_md_set_sector_offsets(struct drbd_conf *mdev,
 				       struct drbd_backing_dev *bdev)
 {
 	sector_t md_size_sect = 0;
-	int meta_dev_idx;
+	unsigned int al_size_sect = bdev->md.al_size_4k * 8;
 
-	rcu_read_lock();
-	meta_dev_idx = rcu_dereference(bdev->disk_conf)->meta_dev_idx;
+	bdev->md.md_offset = drbd_md_ss(bdev);
 
-	switch (meta_dev_idx) {
+	switch (bdev->md.meta_dev_idx) {
 	default:
 		/* v07 style fixed size indexed meta data */
-		bdev->md.md_size_sect = MD_RESERVED_SECT;
-		bdev->md.md_offset = drbd_md_ss__(mdev, bdev);
-		bdev->md.al_offset = MD_AL_OFFSET;
-		bdev->md.bm_offset = MD_BM_OFFSET;
+		bdev->md.md_size_sect = MD_128MB_SECT;
+		bdev->md.al_offset = MD_4kB_SECT;
+		bdev->md.bm_offset = MD_4kB_SECT + al_size_sect;
 		break;
 	case DRBD_MD_INDEX_FLEX_EXT:
 		/* just occupy the full device; unit: sectors */
 		bdev->md.md_size_sect = drbd_get_capacity(bdev->md_bdev);
-		bdev->md.md_offset = 0;
-		bdev->md.al_offset = MD_AL_OFFSET;
-		bdev->md.bm_offset = MD_BM_OFFSET;
+		bdev->md.al_offset = MD_4kB_SECT;
+		bdev->md.bm_offset = MD_4kB_SECT + al_size_sect;
 		break;
 	case DRBD_MD_INDEX_INTERNAL:
 	case DRBD_MD_INDEX_FLEX_INT:
-		bdev->md.md_offset = drbd_md_ss__(mdev, bdev);
 		/* al size is still fixed */
-		bdev->md.al_offset = -MD_AL_SECTORS;
+		bdev->md.al_offset = -al_size_sect;
 		/* we need (slightly less than) ~ this much bitmap sectors: */
 		md_size_sect = drbd_get_capacity(bdev->backing_bdev);
 		md_size_sect = ALIGN(md_size_sect, BM_SECT_PER_EXT);
@@ -735,14 +750,13 @@ static void drbd_md_set_sector_offsets(struct drbd_conf *mdev,
 
 		/* plus the "drbd meta data super block",
 		 * and the activity log; */
-		md_size_sect += MD_BM_OFFSET;
+		md_size_sect += MD_4kB_SECT + al_size_sect;
 
 		bdev->md.md_size_sect = md_size_sect;
 		/* bitmap offset is adjusted by 'super' block size */
-		bdev->md.bm_offset   = -md_size_sect + MD_AL_OFFSET;
+		bdev->md.bm_offset   = -md_size_sect + MD_4kB_SECT;
 		break;
 	}
-	rcu_read_unlock();
 }
 
 /* input size is expected to be in KB */
@@ -805,7 +819,7 @@ void drbd_resume_io(struct drbd_conf *mdev)
 enum determine_dev_size drbd_determine_dev_size(struct drbd_conf *mdev, enum dds_flags flags) __must_hold(local)
 {
 	sector_t prev_first_sect, prev_size; /* previous meta location */
-	sector_t la_size, u_size;
+	sector_t la_size_sect, u_size;
 	sector_t size;
 	char ppb[10];
 
@@ -828,7 +842,7 @@ enum determine_dev_size drbd_determine_dev_size(struct drbd_conf *mdev, enum dds
 
 	prev_first_sect = drbd_md_first_sector(mdev->ldev);
 	prev_size = mdev->ldev->md.md_size_sect;
-	la_size = mdev->ldev->md.la_size_sect;
+	la_size_sect = mdev->ldev->md.la_size_sect;
 
 	/* TODO: should only be some assert here, not (re)init... */
 	drbd_md_set_sector_offsets(mdev, mdev->ldev);
@@ -864,7 +878,7 @@ enum determine_dev_size drbd_determine_dev_size(struct drbd_conf *mdev, enum dds
 	if (rv == dev_size_error)
 		goto out;
 
-	la_size_changed = (la_size != mdev->ldev->md.la_size_sect);
+	la_size_changed = (la_size_sect != mdev->ldev->md.la_size_sect);
 
 	md_moved = prev_first_sect != drbd_md_first_sector(mdev->ldev)
 		|| prev_size	   != mdev->ldev->md.md_size_sect;
@@ -886,9 +900,9 @@ enum determine_dev_size drbd_determine_dev_size(struct drbd_conf *mdev, enum dds
 		drbd_md_mark_dirty(mdev);
 	}
 
-	if (size > la_size)
+	if (size > la_size_sect)
 		rv = grew;
-	if (size < la_size)
+	if (size < la_size_sect)
 		rv = shrunk;
 out:
 	lc_unlock(mdev->act_log);
@@ -903,7 +917,7 @@ drbd_new_dev_size(struct drbd_conf *mdev, struct drbd_backing_dev *bdev,
 		  sector_t u_size, int assume_peer_has_space)
 {
 	sector_t p_size = mdev->p_size;   /* partner's disk size. */
-	sector_t la_size = bdev->md.la_size_sect; /* last agreed size. */
+	sector_t la_size_sect = bdev->md.la_size_sect; /* last agreed size. */
 	sector_t m_size; /* my size */
 	sector_t size = 0;
 
@@ -917,8 +931,8 @@ drbd_new_dev_size(struct drbd_conf *mdev, struct drbd_backing_dev *bdev,
 	if (p_size && m_size) {
 		size = min_t(sector_t, p_size, m_size);
 	} else {
-		if (la_size) {
-			size = la_size;
+		if (la_size_sect) {
+			size = la_size_sect;
 			if (m_size && m_size < size)
 				size = m_size;
 			if (p_size && p_size < size)
@@ -1127,15 +1141,32 @@ static bool should_set_defaults(struct genl_info *info)
 	return 0 != (flags & DRBD_GENL_F_SET_DEFAULTS);
 }
 
-static void enforce_disk_conf_limits(struct disk_conf *dc)
+static unsigned int drbd_al_extents_max(struct drbd_backing_dev *bdev)
 {
-	if (dc->al_extents < DRBD_AL_EXTENTS_MIN)
-		dc->al_extents = DRBD_AL_EXTENTS_MIN;
-	if (dc->al_extents > DRBD_AL_EXTENTS_MAX)
-		dc->al_extents = DRBD_AL_EXTENTS_MAX;
+	/* This is limited by 16 bit "slot" numbers,
+	 * and by available on-disk context storage.
+	 *
+	 * Also (u16)~0 is special (denotes a "free" extent).
+	 *
+	 * One transaction occupies one 4kB on-disk block,
+	 * we have n such blocks in the on disk ring buffer,
+	 * the "current" transaction may fail (n-1),
+	 * and there is 919 slot numbers context information per transaction.
+	 *
+	 * 72 transaction blocks amounts to more than 2**16 context slots,
+	 * so cap there first.
+	 */
+	const unsigned int max_al_nr = DRBD_AL_EXTENTS_MAX;
+	const unsigned int sufficient_on_disk =
+		(max_al_nr + AL_CONTEXT_PER_TRANSACTION -1)
+		/AL_CONTEXT_PER_TRANSACTION;
+
+	unsigned int al_size_4k = bdev->md.al_size_4k;
+
+	if (al_size_4k > sufficient_on_disk)
+		return max_al_nr;
 
-	if (dc->c_plan_ahead > DRBD_C_PLAN_AHEAD_MAX)
-		dc->c_plan_ahead = DRBD_C_PLAN_AHEAD_MAX;
+	return (al_size_4k - 1) * AL_CONTEXT_PER_TRANSACTION;
 }
 
 int drbd_adm_disk_opts(struct sk_buff *skb, struct genl_info *info)
@@ -1182,7 +1213,13 @@ int drbd_adm_disk_opts(struct sk_buff *skb, struct genl_info *info)
 	if (!expect(new_disk_conf->resync_rate >= 1))
 		new_disk_conf->resync_rate = 1;
 
-	enforce_disk_conf_limits(new_disk_conf);
+	if (new_disk_conf->al_extents < DRBD_AL_EXTENTS_MIN)
+		new_disk_conf->al_extents = DRBD_AL_EXTENTS_MIN;
+	if (new_disk_conf->al_extents > drbd_al_extents_max(mdev->ldev))
+		new_disk_conf->al_extents = drbd_al_extents_max(mdev->ldev);
+
+	if (new_disk_conf->c_plan_ahead > DRBD_C_PLAN_AHEAD_MAX)
+		new_disk_conf->c_plan_ahead = DRBD_C_PLAN_AHEAD_MAX;
 
 	fifo_size = (new_disk_conf->c_plan_ahead * 10 * SLEEP_TIME) / HZ;
 	if (fifo_size != mdev->rs_plan_s->size) {
@@ -1330,7 +1367,8 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 		goto fail;
 	}
 
-	enforce_disk_conf_limits(new_disk_conf);
+	if (new_disk_conf->c_plan_ahead > DRBD_C_PLAN_AHEAD_MAX)
+		new_disk_conf->c_plan_ahead = DRBD_C_PLAN_AHEAD_MAX;
 
 	new_plan = fifo_alloc((new_disk_conf->c_plan_ahead * 10 * SLEEP_TIME) / HZ);
 	if (!new_plan) {
@@ -1343,6 +1381,12 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 		goto fail;
 	}
 
+	write_lock_irq(&global_state_lock);
+	retcode = drbd_resync_after_valid(mdev, new_disk_conf->resync_after);
+	write_unlock_irq(&global_state_lock);
+	if (retcode != NO_ERROR)
+		goto fail;
+
 	rcu_read_lock();
 	nc = rcu_dereference(mdev->tconn->net_conf);
 	if (nc) {
@@ -1399,8 +1443,16 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 		goto fail;
 	}
 
-	/* RT - for drbd_get_max_capacity() DRBD_MD_INDEX_FLEX_INT */
-	drbd_md_set_sector_offsets(mdev, nbc);
+	/* Read our meta data super block early.
+	 * This also sets other on-disk offsets. */
+	retcode = drbd_md_read(mdev, nbc);
+	if (retcode != NO_ERROR)
+		goto fail;
+
+	if (new_disk_conf->al_extents < DRBD_AL_EXTENTS_MIN)
+		new_disk_conf->al_extents = DRBD_AL_EXTENTS_MIN;
+	if (new_disk_conf->al_extents > drbd_al_extents_max(nbc))
+		new_disk_conf->al_extents = drbd_al_extents_max(nbc);
 
 	if (drbd_get_max_capacity(nbc) < new_disk_conf->disk_size) {
 		dev_err(DEV, "max capacity %llu smaller than disk size %llu\n",
@@ -1416,7 +1468,7 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 		min_md_device_sectors = (2<<10);
 	} else {
 		max_possible_sectors = DRBD_MAX_SECTORS;
-		min_md_device_sectors = MD_RESERVED_SECT * (new_disk_conf->meta_dev_idx + 1);
+		min_md_device_sectors = MD_128MB_SECT * (new_disk_conf->meta_dev_idx + 1);
 	}
 
 	if (drbd_get_capacity(nbc->md_bdev) < min_md_device_sectors) {
@@ -1467,8 +1519,6 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 	if (!get_ldev_if_state(mdev, D_ATTACHING))
 		goto force_diskless;
 
-	drbd_md_set_sector_offsets(mdev, nbc);
-
 	if (!mdev->bitmap) {
 		if (drbd_bm_init(mdev)) {
 			retcode = ERR_NOMEM;
@@ -1476,10 +1526,6 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 		}
 	}
 
-	retcode = drbd_md_read(mdev, nbc);
-	if (retcode != NO_ERROR)
-		goto force_diskless_dec;
-
 	if (mdev->state.conn < C_CONNECTED &&
 	    mdev->state.role == R_PRIMARY &&
 	    (mdev->ed_uuid & ~((u64)1)) != (nbc->md.uuid[UI_CURRENT] & ~((u64)1))) {
@@ -2158,8 +2204,11 @@ static enum drbd_state_rv conn_try_disconnect(struct drbd_tconn *tconn, bool for
 		return SS_SUCCESS;
 	case SS_PRIMARY_NOP:
 		/* Our state checking code wants to see the peer outdated. */
-		rv = conn_request_state(tconn, NS2(conn, C_DISCONNECTING,
-						pdsk, D_OUTDATED), CS_VERBOSE);
+		rv = conn_request_state(tconn, NS2(conn, C_DISCONNECTING, pdsk, D_OUTDATED), 0);
+
+		if (rv == SS_OUTDATE_WO_CONN) /* lost connection before graceful disconnect succeeded */
+			rv = conn_request_state(tconn, NS(conn, C_DISCONNECTING), CS_VERBOSE);
+
 		break;
 	case SS_CW_FAILED_BY_PEER:
 		/* The peer probably wants to see us outdated. */
@@ -2406,22 +2455,19 @@ int drbd_adm_invalidate(struct sk_buff *skb, struct genl_info *info)
 	wait_event(mdev->misc_wait, !test_bit(BITMAP_IO, &mdev->flags));
 	drbd_flush_workqueue(mdev);
 
-	retcode = _drbd_request_state(mdev, NS(conn, C_STARTING_SYNC_T), CS_ORDERED);
-
-	if (retcode < SS_SUCCESS && retcode != SS_NEED_CONNECTION)
-		retcode = drbd_request_state(mdev, NS(conn, C_STARTING_SYNC_T));
-
-	while (retcode == SS_NEED_CONNECTION) {
-		spin_lock_irq(&mdev->tconn->req_lock);
-		if (mdev->state.conn < C_CONNECTED)
-			retcode = _drbd_set_state(_NS(mdev, disk, D_INCONSISTENT), CS_VERBOSE, NULL);
-		spin_unlock_irq(&mdev->tconn->req_lock);
-
-		if (retcode != SS_NEED_CONNECTION)
-			break;
-
+	/* If we happen to be C_STANDALONE R_SECONDARY, just change to
+	 * D_INCONSISTENT, and set all bits in the bitmap.  Otherwise,
+	 * try to start a resync handshake as sync target for full sync.
+	 */
+	if (mdev->state.conn == C_STANDALONE && mdev->state.role == R_SECONDARY) {
+		retcode = drbd_request_state(mdev, NS(disk, D_INCONSISTENT));
+		if (retcode >= SS_SUCCESS) {
+			if (drbd_bitmap_io(mdev, &drbd_bmio_set_n_write,
+				"set_n_write from invalidate", BM_LOCKED_MASK))
+				retcode = ERR_IO_MD_DISK;
+		}
+	} else
 		retcode = drbd_request_state(mdev, NS(conn, C_STARTING_SYNC_T));
-	}
 	drbd_resume_io(mdev);
 
 out:
@@ -2475,21 +2521,22 @@ int drbd_adm_invalidate_peer(struct sk_buff *skb, struct genl_info *info)
 	wait_event(mdev->misc_wait, !test_bit(BITMAP_IO, &mdev->flags));
 	drbd_flush_workqueue(mdev);
 
-	retcode = _drbd_request_state(mdev, NS(conn, C_STARTING_SYNC_S), CS_ORDERED);
-	if (retcode < SS_SUCCESS) {
-		if (retcode == SS_NEED_CONNECTION && mdev->state.role == R_PRIMARY) {
-			/* The peer will get a resync upon connect anyways.
-			 * Just make that into a full resync. */
-			retcode = drbd_request_state(mdev, NS(pdsk, D_INCONSISTENT));
-			if (retcode >= SS_SUCCESS) {
-				if (drbd_bitmap_io(mdev, &drbd_bmio_set_susp_al,
-						   "set_n_write from invalidate_peer",
-						   BM_LOCKED_SET_ALLOWED))
-					retcode = ERR_IO_MD_DISK;
-			}
-		} else
-			retcode = drbd_request_state(mdev, NS(conn, C_STARTING_SYNC_S));
-	}
+	/* If we happen to be C_STANDALONE R_PRIMARY, just set all bits
+	 * in the bitmap.  Otherwise, try to start a resync handshake
+	 * as sync source for full sync.
+	 */
+	if (mdev->state.conn == C_STANDALONE && mdev->state.role == R_PRIMARY) {
+		/* The peer will get a resync upon connect anyways. Just make that
+		   into a full resync. */
+		retcode = drbd_request_state(mdev, NS(pdsk, D_INCONSISTENT));
+		if (retcode >= SS_SUCCESS) {
+			if (drbd_bitmap_io(mdev, &drbd_bmio_set_susp_al,
+				"set_n_write from invalidate_peer",
+				BM_LOCKED_SET_ALLOWED))
+				retcode = ERR_IO_MD_DISK;
+		}
+	} else
+		retcode = drbd_request_state(mdev, NS(conn, C_STARTING_SYNC_S));
 	drbd_resume_io(mdev);
 
 out:
@@ -3162,6 +3209,7 @@ static enum drbd_ret_code adm_delete_minor(struct drbd_conf *mdev)
 				    CS_VERBOSE + CS_WAIT_COMPLETE);
 		idr_remove(&mdev->tconn->volumes, mdev->vnr);
 		idr_remove(&minors, mdev_to_minor(mdev));
+		destroy_workqueue(mdev->submit.wq);
 		del_gendisk(mdev->vdisk);
 		synchronize_rcu();
 		kref_put(&mdev->kref, &drbd_minor_destroy);
diff --git a/drivers/block/drbd/drbd_proc.c b/drivers/block/drbd/drbd_proc.c
index 56672a61eb94..bf31d41dbaad 100644
--- a/drivers/block/drbd/drbd_proc.c
+++ b/drivers/block/drbd/drbd_proc.c
@@ -313,8 +313,14 @@ static int drbd_seq_show(struct seq_file *seq, void *v)
 
 static int drbd_proc_open(struct inode *inode, struct file *file)
 {
-	if (try_module_get(THIS_MODULE))
-		return single_open(file, drbd_seq_show, PDE(inode)->data);
+	int err;
+
+	if (try_module_get(THIS_MODULE)) {
+		err = single_open(file, drbd_seq_show, PDE_DATA(inode));
+		if (err)
+			module_put(THIS_MODULE);
+		return err;
+	}
 	return -ENODEV;
 }
 
diff --git a/drivers/block/drbd/drbd_receiver.c b/drivers/block/drbd/drbd_receiver.c
index 83c5ae0ed56b..4222affff488 100644
--- a/drivers/block/drbd/drbd_receiver.c
+++ b/drivers/block/drbd/drbd_receiver.c
@@ -850,6 +850,7 @@ int drbd_connected(struct drbd_conf *mdev)
 		err = drbd_send_current_state(mdev);
 	clear_bit(USE_DEGR_WFC_T, &mdev->flags);
 	clear_bit(RESIZE_PENDING, &mdev->flags);
+	atomic_set(&mdev->ap_in_flight, 0);
 	mod_timer(&mdev->request_timer, jiffies + HZ); /* just start it here. */
 	return err;
 }
@@ -2266,7 +2267,7 @@ static int receive_Data(struct drbd_tconn *tconn, struct packet_info *pi)
 		drbd_set_out_of_sync(mdev, peer_req->i.sector, peer_req->i.size);
 		peer_req->flags |= EE_CALL_AL_COMPLETE_IO;
 		peer_req->flags &= ~EE_MAY_SET_IN_SYNC;
-		drbd_al_begin_io(mdev, &peer_req->i);
+		drbd_al_begin_io(mdev, &peer_req->i, true);
 	}
 
 	err = drbd_submit_peer_request(mdev, peer_req, rw, DRBD_FAULT_DT_WR);
@@ -2662,7 +2663,6 @@ static int drbd_asb_recover_1p(struct drbd_conf *mdev) __must_hold(local)
 		if (hg == -1 && mdev->state.role == R_PRIMARY) {
 			enum drbd_state_rv rv2;
 
-			drbd_set_role(mdev, R_SECONDARY, 0);
 			 /* drbd_change_state() does not sleep while in SS_IN_TRANSIENT_STATE,
 			  * we might be here in C_WF_REPORT_PARAMS which is transient.
 			  * we do not need to wait for the after state change work either. */
@@ -3993,7 +3993,7 @@ static int receive_state(struct drbd_tconn *tconn, struct packet_info *pi)
 
 	clear_bit(DISCARD_MY_DATA, &mdev->flags);
 
-	drbd_md_sync(mdev); /* update connected indicator, la_size, ... */
+	drbd_md_sync(mdev); /* update connected indicator, la_size_sect, ... */
 
 	return 0;
 }
@@ -4660,8 +4660,8 @@ static int drbd_do_features(struct drbd_tconn *tconn)
 #if !defined(CONFIG_CRYPTO_HMAC) && !defined(CONFIG_CRYPTO_HMAC_MODULE)
 static int drbd_do_auth(struct drbd_tconn *tconn)
 {
-	dev_err(DEV, "This kernel was build without CONFIG_CRYPTO_HMAC.\n");
-	dev_err(DEV, "You need to disable 'cram-hmac-alg' in drbd.conf.\n");
+	conn_err(tconn, "This kernel was build without CONFIG_CRYPTO_HMAC.\n");
+	conn_err(tconn, "You need to disable 'cram-hmac-alg' in drbd.conf.\n");
 	return -1;
 }
 #else
@@ -5258,9 +5258,11 @@ int drbd_asender(struct drbd_thread *thi)
 	bool ping_timeout_active = false;
 	struct net_conf *nc;
 	int ping_timeo, tcp_cork, ping_int;
+	struct sched_param param = { .sched_priority = 2 };
 
-	current->policy = SCHED_RR;  /* Make this a realtime task! */
-	current->rt_priority = 2;    /* more important than all other tasks */
+	rv = sched_setscheduler(current, SCHED_RR, &param);
+	if (rv < 0)
+		conn_err(tconn, "drbd_asender: ERROR set priority, ret=%d\n", rv);
 
 	while (get_t_state(thi) == RUNNING) {
 		drbd_thread_current_set_cpu(thi);
diff --git a/drivers/block/drbd/drbd_req.c b/drivers/block/drbd/drbd_req.c
index 2b8303ad63c9..c24379ffd4e3 100644
--- a/drivers/block/drbd/drbd_req.c
+++ b/drivers/block/drbd/drbd_req.c
@@ -34,14 +34,14 @@
 static bool drbd_may_do_local_read(struct drbd_conf *mdev, sector_t sector, int size);
 
 /* Update disk stats at start of I/O request */
-static void _drbd_start_io_acct(struct drbd_conf *mdev, struct drbd_request *req, struct bio *bio)
+static void _drbd_start_io_acct(struct drbd_conf *mdev, struct drbd_request *req)
 {
-	const int rw = bio_data_dir(bio);
+	const int rw = bio_data_dir(req->master_bio);
 	int cpu;
 	cpu = part_stat_lock();
 	part_round_stats(cpu, &mdev->vdisk->part0);
 	part_stat_inc(cpu, &mdev->vdisk->part0, ios[rw]);
-	part_stat_add(cpu, &mdev->vdisk->part0, sectors[rw], bio_sectors(bio));
+	part_stat_add(cpu, &mdev->vdisk->part0, sectors[rw], req->i.size >> 9);
 	(void) cpu; /* The macro invocations above want the cpu argument, I do not like
 		       the compiler warning about cpu only assigned but never used... */
 	part_inc_in_flight(&mdev->vdisk->part0, rw);
@@ -263,8 +263,7 @@ void drbd_req_complete(struct drbd_request *req, struct bio_and_error *m)
 		else
 			root = &mdev->read_requests;
 		drbd_remove_request_interval(root, req);
-	} else if (!(s & RQ_POSTPONED))
-		D_ASSERT((s & (RQ_NET_MASK & ~RQ_NET_DONE)) == 0);
+	}
 
 	/* Before we can signal completion to the upper layers,
 	 * we may need to close the current transfer log epoch.
@@ -755,6 +754,11 @@ int __req_mod(struct drbd_request *req, enum drbd_req_event what,
 		D_ASSERT(req->rq_state & RQ_NET_PENDING);
 		mod_rq_state(req, m, RQ_NET_PENDING, RQ_NET_OK|RQ_NET_DONE);
 		break;
+
+	case QUEUE_AS_DRBD_BARRIER:
+		start_new_tl_epoch(mdev->tconn);
+		mod_rq_state(req, m, 0, RQ_NET_OK|RQ_NET_DONE);
+		break;
 	};
 
 	return rv;
@@ -861,8 +865,10 @@ static void maybe_pull_ahead(struct drbd_conf *mdev)
 	bool congested = false;
 	enum drbd_on_congestion on_congestion;
 
+	rcu_read_lock();
 	nc = rcu_dereference(tconn->net_conf);
 	on_congestion = nc ? nc->on_congestion : OC_BLOCK;
+	rcu_read_unlock();
 	if (on_congestion == OC_BLOCK ||
 	    tconn->agreed_pro_version < 96)
 		return;
@@ -956,14 +962,8 @@ static int drbd_process_write_request(struct drbd_request *req)
 	struct drbd_conf *mdev = req->w.mdev;
 	int remote, send_oos;
 
-	rcu_read_lock();
 	remote = drbd_should_do_remote(mdev->state);
-	if (remote) {
-		maybe_pull_ahead(mdev);
-		remote = drbd_should_do_remote(mdev->state);
-	}
 	send_oos = drbd_should_send_out_of_sync(mdev->state);
-	rcu_read_unlock();
 
 	/* Need to replicate writes.  Unless it is an empty flush,
 	 * which is better mapped to a DRBD P_BARRIER packet,
@@ -975,8 +975,8 @@ static int drbd_process_write_request(struct drbd_request *req)
 		/* The only size==0 bios we expect are empty flushes. */
 		D_ASSERT(req->master_bio->bi_rw & REQ_FLUSH);
 		if (remote)
-			start_new_tl_epoch(mdev->tconn);
-		return 0;
+			_req_mod(req, QUEUE_AS_DRBD_BARRIER);
+		return remote;
 	}
 
 	if (!remote && !send_oos)
@@ -1020,12 +1020,24 @@ drbd_submit_req_private_bio(struct drbd_request *req)
 		bio_endio(bio, -EIO);
 }
 
-void __drbd_make_request(struct drbd_conf *mdev, struct bio *bio, unsigned long start_time)
+static void drbd_queue_write(struct drbd_conf *mdev, struct drbd_request *req)
 {
-	const int rw = bio_rw(bio);
-	struct bio_and_error m = { NULL, };
+	spin_lock(&mdev->submit.lock);
+	list_add_tail(&req->tl_requests, &mdev->submit.writes);
+	spin_unlock(&mdev->submit.lock);
+	queue_work(mdev->submit.wq, &mdev->submit.worker);
+}
+
+/* returns the new drbd_request pointer, if the caller is expected to
+ * drbd_send_and_submit() it (to save latency), or NULL if we queued the
+ * request on the submitter thread.
+ * Returns ERR_PTR(-ENOMEM) if we cannot allocate a drbd_request.
+ */
+struct drbd_request *
+drbd_request_prepare(struct drbd_conf *mdev, struct bio *bio, unsigned long start_time)
+{
+	const int rw = bio_data_dir(bio);
 	struct drbd_request *req;
-	bool no_remote = false;
 
 	/* allocate outside of all locks; */
 	req = drbd_req_new(mdev, bio);
@@ -1035,7 +1047,7 @@ void __drbd_make_request(struct drbd_conf *mdev, struct bio *bio, unsigned long
 		 * if user cannot handle io errors, that's not our business. */
 		dev_err(DEV, "could not kmalloc() req\n");
 		bio_endio(bio, -ENOMEM);
-		return;
+		return ERR_PTR(-ENOMEM);
 	}
 	req->start_time = start_time;
 
@@ -1044,28 +1056,40 @@ void __drbd_make_request(struct drbd_conf *mdev, struct bio *bio, unsigned long
 		req->private_bio = NULL;
 	}
 
-	/* For WRITES going to the local disk, grab a reference on the target
-	 * extent.  This waits for any resync activity in the corresponding
-	 * resync extent to finish, and, if necessary, pulls in the target
-	 * extent into the activity log, which involves further disk io because
-	 * of transactional on-disk meta data updates.
-	 * Empty flushes don't need to go into the activity log, they can only
-	 * flush data for pending writes which are already in there. */
+	/* Update disk stats */
+	_drbd_start_io_acct(mdev, req);
+
 	if (rw == WRITE && req->private_bio && req->i.size
 	&& !test_bit(AL_SUSPENDED, &mdev->flags)) {
+		if (!drbd_al_begin_io_fastpath(mdev, &req->i)) {
+			drbd_queue_write(mdev, req);
+			return NULL;
+		}
 		req->rq_state |= RQ_IN_ACT_LOG;
-		drbd_al_begin_io(mdev, &req->i);
 	}
 
+	return req;
+}
+
+static void drbd_send_and_submit(struct drbd_conf *mdev, struct drbd_request *req)
+{
+	const int rw = bio_rw(req->master_bio);
+	struct bio_and_error m = { NULL, };
+	bool no_remote = false;
+
 	spin_lock_irq(&mdev->tconn->req_lock);
 	if (rw == WRITE) {
 		/* This may temporarily give up the req_lock,
 		 * but will re-aquire it before it returns here.
 		 * Needs to be before the check on drbd_suspended() */
 		complete_conflicting_writes(req);
+		/* no more giving up req_lock from now on! */
+
+		/* check for congestion, and potentially stop sending
+		 * full data updates, but start sending "dirty bits" only. */
+		maybe_pull_ahead(mdev);
 	}
 
-	/* no more giving up req_lock from now on! */
 
 	if (drbd_suspended(mdev)) {
 		/* push back and retry: */
@@ -1078,9 +1102,6 @@ void __drbd_make_request(struct drbd_conf *mdev, struct bio *bio, unsigned long
 		goto out;
 	}
 
-	/* Update disk stats */
-	_drbd_start_io_acct(mdev, req, bio);
-
 	/* We fail READ/READA early, if we can not serve it.
 	 * We must do this before req is registered on any lists.
 	 * Otherwise, drbd_req_complete() will queue failed READ for retry. */
@@ -1137,7 +1158,116 @@ out:
 
 	if (m.bio)
 		complete_master_bio(mdev, &m);
-	return;
+}
+
+void __drbd_make_request(struct drbd_conf *mdev, struct bio *bio, unsigned long start_time)
+{
+	struct drbd_request *req = drbd_request_prepare(mdev, bio, start_time);
+	if (IS_ERR_OR_NULL(req))
+		return;
+	drbd_send_and_submit(mdev, req);
+}
+
+static void submit_fast_path(struct drbd_conf *mdev, struct list_head *incoming)
+{
+	struct drbd_request *req, *tmp;
+	list_for_each_entry_safe(req, tmp, incoming, tl_requests) {
+		const int rw = bio_data_dir(req->master_bio);
+
+		if (rw == WRITE /* rw != WRITE should not even end up here! */
+		&& req->private_bio && req->i.size
+		&& !test_bit(AL_SUSPENDED, &mdev->flags)) {
+			if (!drbd_al_begin_io_fastpath(mdev, &req->i))
+				continue;
+
+			req->rq_state |= RQ_IN_ACT_LOG;
+		}
+
+		list_del_init(&req->tl_requests);
+		drbd_send_and_submit(mdev, req);
+	}
+}
+
+static bool prepare_al_transaction_nonblock(struct drbd_conf *mdev,
+					    struct list_head *incoming,
+					    struct list_head *pending)
+{
+	struct drbd_request *req, *tmp;
+	int wake = 0;
+	int err;
+
+	spin_lock_irq(&mdev->al_lock);
+	list_for_each_entry_safe(req, tmp, incoming, tl_requests) {
+		err = drbd_al_begin_io_nonblock(mdev, &req->i);
+		if (err == -EBUSY)
+			wake = 1;
+		if (err)
+			continue;
+		req->rq_state |= RQ_IN_ACT_LOG;
+		list_move_tail(&req->tl_requests, pending);
+	}
+	spin_unlock_irq(&mdev->al_lock);
+	if (wake)
+		wake_up(&mdev->al_wait);
+
+	return !list_empty(pending);
+}
+
+void do_submit(struct work_struct *ws)
+{
+	struct drbd_conf *mdev = container_of(ws, struct drbd_conf, submit.worker);
+	LIST_HEAD(incoming);
+	LIST_HEAD(pending);
+	struct drbd_request *req, *tmp;
+
+	for (;;) {
+		spin_lock(&mdev->submit.lock);
+		list_splice_tail_init(&mdev->submit.writes, &incoming);
+		spin_unlock(&mdev->submit.lock);
+
+		submit_fast_path(mdev, &incoming);
+		if (list_empty(&incoming))
+			break;
+
+		wait_event(mdev->al_wait, prepare_al_transaction_nonblock(mdev, &incoming, &pending));
+		/* Maybe more was queued, while we prepared the transaction?
+		 * Try to stuff them into this transaction as well.
+		 * Be strictly non-blocking here, no wait_event, we already
+		 * have something to commit.
+		 * Stop if we don't make any more progres.
+		 */
+		for (;;) {
+			LIST_HEAD(more_pending);
+			LIST_HEAD(more_incoming);
+			bool made_progress;
+
+			/* It is ok to look outside the lock,
+			 * it's only an optimization anyways */
+			if (list_empty(&mdev->submit.writes))
+				break;
+
+			spin_lock(&mdev->submit.lock);
+			list_splice_tail_init(&mdev->submit.writes, &more_incoming);
+			spin_unlock(&mdev->submit.lock);
+
+			if (list_empty(&more_incoming))
+				break;
+
+			made_progress = prepare_al_transaction_nonblock(mdev, &more_incoming, &more_pending);
+
+			list_splice_tail_init(&more_pending, &pending);
+			list_splice_tail_init(&more_incoming, &incoming);
+
+			if (!made_progress)
+				break;
+		}
+		drbd_al_begin_io_commit(mdev, false);
+
+		list_for_each_entry_safe(req, tmp, &pending, tl_requests) {
+			list_del_init(&req->tl_requests);
+			drbd_send_and_submit(mdev, req);
+		}
+	}
 }
 
 void drbd_make_request(struct request_queue *q, struct bio *bio)
diff --git a/drivers/block/drbd/drbd_req.h b/drivers/block/drbd/drbd_req.h
index c08d22964d06..978cb1addc98 100644
--- a/drivers/block/drbd/drbd_req.h
+++ b/drivers/block/drbd/drbd_req.h
@@ -88,6 +88,14 @@ enum drbd_req_event {
 	QUEUE_FOR_NET_READ,
 	QUEUE_FOR_SEND_OOS,
 
+	/* An empty flush is queued as P_BARRIER,
+	 * which will cause it to complete "successfully",
+	 * even if the local disk flush failed.
+	 *
+	 * Just like "real" requests, empty flushes (blkdev_issue_flush()) will
+	 * only see an error if neither local nor remote data is reachable. */
+	QUEUE_AS_DRBD_BARRIER,
+
 	SEND_CANCELED,
 	SEND_FAILED,
 	HANDED_OVER_TO_NETWORK,
diff --git a/drivers/block/drbd/drbd_state.c b/drivers/block/drbd/drbd_state.c
index 0fe220cfb9e9..90c5be2b1d30 100644
--- a/drivers/block/drbd/drbd_state.c
+++ b/drivers/block/drbd/drbd_state.c
@@ -570,6 +570,13 @@ is_valid_state(struct drbd_conf *mdev, union drbd_state ns)
 		  mdev->tconn->agreed_pro_version < 88)
 		rv = SS_NOT_SUPPORTED;
 
+	else if (ns.role == R_PRIMARY && ns.disk < D_UP_TO_DATE && ns.pdsk < D_UP_TO_DATE)
+		rv = SS_NO_UP_TO_DATE_DISK;
+
+	else if ((ns.conn == C_STARTING_SYNC_S || ns.conn == C_STARTING_SYNC_T) &&
+                 ns.pdsk == D_UNKNOWN)
+		rv = SS_NEED_CONNECTION;
+
 	else if (ns.conn >= C_CONNECTED && ns.pdsk == D_UNKNOWN)
 		rv = SS_CONNECTED_OUTDATES;
 
@@ -635,6 +642,10 @@ is_valid_soft_transition(union drbd_state os, union drbd_state ns, struct drbd_t
 	    && os.conn < C_WF_REPORT_PARAMS)
 		rv = SS_NEED_CONNECTION; /* No NetworkFailure -> SyncTarget etc... */
 
+	if (ns.conn == C_DISCONNECTING && ns.pdsk == D_OUTDATED &&
+	    os.conn < C_CONNECTED && os.pdsk > D_OUTDATED)
+		rv = SS_OUTDATE_WO_CONN;
+
 	return rv;
 }
 
@@ -1377,13 +1388,6 @@ static void after_state_ch(struct drbd_conf *mdev, union drbd_state os,
 			&drbd_bmio_set_n_write, &abw_start_sync,
 			"set_n_write from StartingSync", BM_LOCKED_TEST_ALLOWED);
 
-	/* We are invalidating our self... */
-	if (os.conn < C_CONNECTED && ns.conn < C_CONNECTED &&
-	    os.disk > D_INCONSISTENT && ns.disk == D_INCONSISTENT)
-		/* other bitmap operation expected during this phase */
-		drbd_queue_bitmap_io(mdev, &drbd_bmio_set_n_write, NULL,
-			"set_n_write from invalidate", BM_LOCKED_MASK);
-
 	/* first half of local IO error, failure to attach,
 	 * or administrative detach */
 	if (os.disk != D_FAILED && ns.disk == D_FAILED) {
@@ -1748,13 +1752,9 @@ _conn_rq_cond(struct drbd_tconn *tconn, union drbd_state mask, union drbd_state
 	if (test_and_clear_bit(CONN_WD_ST_CHG_FAIL, &tconn->flags))
 		return SS_CW_FAILED_BY_PEER;
 
-	rv = tconn->cstate != C_WF_REPORT_PARAMS ? SS_CW_NO_NEED : SS_UNKNOWN_ERROR;
-
-	if (rv == SS_UNKNOWN_ERROR)
-		rv = conn_is_valid_transition(tconn, mask, val, 0);
-
-	if (rv == SS_SUCCESS)
-		rv = SS_UNKNOWN_ERROR; /* cont waiting, otherwise fail. */
+	rv = conn_is_valid_transition(tconn, mask, val, 0);
+	if (rv == SS_SUCCESS && tconn->cstate == C_WF_REPORT_PARAMS)
+		rv = SS_UNKNOWN_ERROR; /* continue waiting */
 
 	return rv;
 }
diff --git a/drivers/block/drbd/drbd_strings.c b/drivers/block/drbd/drbd_strings.c
index 9a664bd27404..58e08ff2b2ce 100644
--- a/drivers/block/drbd/drbd_strings.c
+++ b/drivers/block/drbd/drbd_strings.c
@@ -89,6 +89,7 @@ static const char *drbd_state_sw_errors[] = {
 	[-SS_LOWER_THAN_OUTDATED] = "Disk state is lower than outdated",
 	[-SS_IN_TRANSIENT_STATE] = "In transient state, retry after next state change",
 	[-SS_CONCURRENT_ST_CHG] = "Concurrent state changes detected and aborted",
+	[-SS_OUTDATE_WO_CONN] = "Need a connection for a graceful disconnect/outdate peer",
 	[-SS_O_VOL_PEER_PRI] = "Other vol primary on peer not allowed by config",
 };
 
diff --git a/drivers/block/drbd/drbd_worker.c b/drivers/block/drbd/drbd_worker.c
index 424dc7bdf9b7..891c0ecaa292 100644
--- a/drivers/block/drbd/drbd_worker.c
+++ b/drivers/block/drbd/drbd_worker.c
@@ -89,7 +89,8 @@ void drbd_md_io_complete(struct bio *bio, int error)
 	md_io->done = 1;
 	wake_up(&mdev->misc_wait);
 	bio_put(bio);
-	put_ldev(mdev);
+	if (mdev->ldev) /* special case: drbd_md_read() during drbd_adm_attach() */
+		put_ldev(mdev);
 }
 
 /* reads on behalf of the partner,
@@ -1410,7 +1411,7 @@ int w_restart_disk_io(struct drbd_work *w, int cancel)
 	struct drbd_conf *mdev = w->mdev;
 
 	if (bio_data_dir(req->master_bio) == WRITE && req->rq_state & RQ_IN_ACT_LOG)
-		drbd_al_begin_io(mdev, &req->i);
+		drbd_al_begin_io(mdev, &req->i, false);
 
 	drbd_req_make_private_bio(req, req->master_bio);
 	req->private_bio->bi_bdev = mdev->ldev->backing_bdev;
@@ -1425,7 +1426,7 @@ static int _drbd_may_sync_now(struct drbd_conf *mdev)
 	int resync_after;
 
 	while (1) {
-		if (!odev->ldev)
+		if (!odev->ldev || odev->state.disk == D_DISKLESS)
 			return 1;
 		rcu_read_lock();
 		resync_after = rcu_dereference(odev->ldev->disk_conf)->resync_after;
@@ -1433,7 +1434,7 @@ static int _drbd_may_sync_now(struct drbd_conf *mdev)
 		if (resync_after == -1)
 			return 1;
 		odev = minor_to_mdev(resync_after);
-		if (!expect(odev))
+		if (!odev)
 			return 1;
 		if ((odev->state.conn >= C_SYNC_SOURCE &&
 		     odev->state.conn <= C_PAUSED_SYNC_T) ||
@@ -1515,7 +1516,7 @@ enum drbd_ret_code drbd_resync_after_valid(struct drbd_conf *mdev, int o_minor)
 
 	if (o_minor == -1)
 		return NO_ERROR;
-	if (o_minor < -1 || minor_to_mdev(o_minor) == NULL)
+	if (o_minor < -1 || o_minor > MINORMASK)
 		return ERR_RESYNC_AFTER;
 
 	/* check for loops */
@@ -1524,6 +1525,15 @@ enum drbd_ret_code drbd_resync_after_valid(struct drbd_conf *mdev, int o_minor)
 		if (odev == mdev)
 			return ERR_RESYNC_AFTER_CYCLE;
 
+		/* You are free to depend on diskless, non-existing,
+		 * or not yet/no longer existing minors.
+		 * We only reject dependency loops.
+		 * We cannot follow the dependency chain beyond a detached or
+		 * missing minor.
+		 */
+		if (!odev || !odev->ldev || odev->state.disk == D_DISKLESS)
+			return NO_ERROR;
+
 		rcu_read_lock();
 		resync_after = rcu_dereference(odev->ldev->disk_conf)->resync_after;
 		rcu_read_unlock();
@@ -1652,7 +1662,9 @@ void drbd_start_resync(struct drbd_conf *mdev, enum drbd_conns side)
 	clear_bit(B_RS_H_DONE, &mdev->flags);
 
 	write_lock_irq(&global_state_lock);
-	if (!get_ldev_if_state(mdev, D_NEGOTIATING)) {
+	/* Did some connection breakage or IO error race with us? */
+	if (mdev->state.conn < C_CONNECTED
+	|| !get_ldev_if_state(mdev, D_NEGOTIATING)) {
 		write_unlock_irq(&global_state_lock);
 		mutex_unlock(mdev->state_mutex);
 		return;
diff --git a/drivers/block/floppy.c b/drivers/block/floppy.c
index 2ddd64a9ffde..04ceb7e2fadd 100644
--- a/drivers/block/floppy.c
+++ b/drivers/block/floppy.c
@@ -3601,7 +3601,7 @@ static void __init config_types(void)
 		pr_cont("\n");
 }
 
-static int floppy_release(struct gendisk *disk, fmode_t mode)
+static void floppy_release(struct gendisk *disk, fmode_t mode)
 {
 	int drive = (long)disk->private_data;
 
@@ -3615,8 +3615,6 @@ static int floppy_release(struct gendisk *disk, fmode_t mode)
 		opened_bdev[drive] = NULL;
 	mutex_unlock(&open_lock);
 	mutex_unlock(&floppy_mutex);
-
-	return 0;
 }
 
 /*
@@ -3777,7 +3775,6 @@ static int __floppy_read_block_0(struct block_device *bdev)
 	bio_vec.bv_len = size;
 	bio_vec.bv_offset = 0;
 	bio.bi_vcnt = 1;
-	bio.bi_idx = 0;
 	bio.bi_size = size;
 	bio.bi_bdev = bdev;
 	bio.bi_sector = 0;
diff --git a/drivers/block/loop.c b/drivers/block/loop.c
index dfe758382eaf..d92d50fd84b7 100644
--- a/drivers/block/loop.c
+++ b/drivers/block/loop.c
@@ -230,9 +230,11 @@ static int __do_lo_send_write(struct file *file,
 	ssize_t bw;
 	mm_segment_t old_fs = get_fs();
 
+	file_start_write(file);
 	set_fs(get_ds());
 	bw = file->f_op->write(file, buf, len, &pos);
 	set_fs(old_fs);
+	file_end_write(file);
 	if (likely(bw == len))
 		return 0;
 	printk(KERN_ERR "loop: Write error at byte offset %llu, length %i.\n",
@@ -1516,7 +1518,7 @@ out:
 	return err;
 }
 
-static int lo_release(struct gendisk *disk, fmode_t mode)
+static void lo_release(struct gendisk *disk, fmode_t mode)
 {
 	struct loop_device *lo = disk->private_data;
 	int err;
@@ -1533,7 +1535,7 @@ static int lo_release(struct gendisk *disk, fmode_t mode)
 		 */
 		err = loop_clr_fd(lo);
 		if (!err)
-			goto out_unlocked;
+			return;
 	} else {
 		/*
 		 * Otherwise keep thread (if running) and config,
@@ -1544,8 +1546,6 @@ static int lo_release(struct gendisk *disk, fmode_t mode)
 
 out:
 	mutex_unlock(&lo->lo_ctl_mutex);
-out_unlocked:
-	return 0;
 }
 
 static const struct block_device_operations lo_fops = {
diff --git a/drivers/block/mg_disk.c b/drivers/block/mg_disk.c
index 076ae7f1b781..a56cfcd5d648 100644
--- a/drivers/block/mg_disk.c
+++ b/drivers/block/mg_disk.c
@@ -780,6 +780,7 @@ static const struct block_device_operations mg_disk_ops = {
 	.getgeo = mg_getgeo
 };
 
+#ifdef CONFIG_PM_SLEEP
 static int mg_suspend(struct device *dev)
 {
 	struct mg_drv_data *prv_data = dev->platform_data;
@@ -824,6 +825,7 @@ static int mg_resume(struct device *dev)
 
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(mg_pm, mg_suspend, mg_resume);
 
diff --git a/drivers/block/mtip32xx/mtip32xx.c b/drivers/block/mtip32xx/mtip32xx.c
index 32c678028e53..847107ef0cce 100644
--- a/drivers/block/mtip32xx/mtip32xx.c
+++ b/drivers/block/mtip32xx/mtip32xx.c
@@ -728,7 +728,10 @@ static void mtip_async_complete(struct mtip_port *port,
 	atomic_set(&port->commands[tag].active, 0);
 	release_slot(port, tag);
 
-	up(&port->cmd_slot);
+	if (unlikely(command->unaligned))
+		up(&port->cmd_slot_unal);
+	else
+		up(&port->cmd_slot);
 }
 
 /*
@@ -1560,10 +1563,12 @@ static int mtip_get_identify(struct mtip_port *port, void __user *user_buffer)
 	}
 #endif
 
+#ifdef MTIP_TRIM /* Disabling TRIM support temporarily */
 	/* Demux ID.DRAT & ID.RZAT to determine trim support */
 	if (port->identify[69] & (1 << 14) && port->identify[69] & (1 << 5))
 		port->dd->trim_supp = true;
 	else
+#endif
 		port->dd->trim_supp = false;
 
 	/* Set the identify buffer as valid. */
@@ -2557,7 +2562,7 @@ static int mtip_hw_ioctl(struct driver_data *dd, unsigned int cmd,
  */
 static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
 			      int nsect, int nents, int tag, void *callback,
-			      void *data, int dir)
+			      void *data, int dir, int unaligned)
 {
 	struct host_to_dev_fis	*fis;
 	struct mtip_port *port = dd->port;
@@ -2570,6 +2575,7 @@ static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
 
 	command->scatter_ents = nents;
 
+	command->unaligned = unaligned;
 	/*
 	 * The number of retries for this command before it is
 	 * reported as a failure to the upper layers.
@@ -2598,6 +2604,9 @@ static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
 	fis->res3        = 0;
 	fill_command_sg(dd, command, nents);
 
+	if (unaligned)
+		fis->device |= 1 << 7;
+
 	/* Populate the command header */
 	command->command_header->opts =
 			__force_bit2int cpu_to_le32(
@@ -2644,9 +2653,13 @@ static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
  * return value
  *      None
  */
-static void mtip_hw_release_scatterlist(struct driver_data *dd, int tag)
+static void mtip_hw_release_scatterlist(struct driver_data *dd, int tag,
+								int unaligned)
 {
+	struct semaphore *sem = unaligned ? &dd->port->cmd_slot_unal :
+							&dd->port->cmd_slot;
 	release_slot(dd->port, tag);
+	up(sem);
 }
 
 /*
@@ -2661,22 +2674,25 @@ static void mtip_hw_release_scatterlist(struct driver_data *dd, int tag)
  *	or NULL if no command slots are available.
  */
 static struct scatterlist *mtip_hw_get_scatterlist(struct driver_data *dd,
-						   int *tag)
+						   int *tag, int unaligned)
 {
+	struct semaphore *sem = unaligned ? &dd->port->cmd_slot_unal :
+							&dd->port->cmd_slot;
+
 	/*
 	 * It is possible that, even with this semaphore, a thread
 	 * may think that no command slots are available. Therefore, we
 	 * need to make an attempt to get_slot().
 	 */
-	down(&dd->port->cmd_slot);
+	down(sem);
 	*tag = get_slot(dd->port);
 
 	if (unlikely(test_bit(MTIP_DDF_REMOVE_PENDING_BIT, &dd->dd_flag))) {
-		up(&dd->port->cmd_slot);
+		up(sem);
 		return NULL;
 	}
 	if (unlikely(*tag < 0)) {
-		up(&dd->port->cmd_slot);
+		up(sem);
 		return NULL;
 	}
 
@@ -3010,6 +3026,11 @@ static inline void hba_setup(struct driver_data *dd)
 		dd->mmio + HOST_HSORG);
 }
 
+static int mtip_device_unaligned_constrained(struct driver_data *dd)
+{
+	return (dd->pdev->device == P420M_DEVICE_ID ? 1 : 0);
+}
+
 /*
  * Detect the details of the product, and store anything needed
  * into the driver data structure.  This includes product type and
@@ -3232,8 +3253,15 @@ static int mtip_hw_init(struct driver_data *dd)
 	for (i = 0; i < MTIP_MAX_SLOT_GROUPS; i++)
 		dd->work[i].port = dd->port;
 
+	/* Enable unaligned IO constraints for some devices */
+	if (mtip_device_unaligned_constrained(dd))
+		dd->unal_qdepth = MTIP_MAX_UNALIGNED_SLOTS;
+	else
+		dd->unal_qdepth = 0;
+
 	/* Counting semaphore to track command slot usage */
-	sema_init(&dd->port->cmd_slot, num_command_slots - 1);
+	sema_init(&dd->port->cmd_slot, num_command_slots - 1 - dd->unal_qdepth);
+	sema_init(&dd->port->cmd_slot_unal, dd->unal_qdepth);
 
 	/* Spinlock to prevent concurrent issue */
 	for (i = 0; i < MTIP_MAX_SLOT_GROUPS; i++)
@@ -3836,7 +3864,7 @@ static void mtip_make_request(struct request_queue *queue, struct bio *bio)
 	struct scatterlist *sg;
 	struct bio_vec *bvec;
 	int nents = 0;
-	int tag = 0;
+	int tag = 0, unaligned = 0;
 
 	if (unlikely(dd->dd_flag & MTIP_DDF_STOP_IO)) {
 		if (unlikely(test_bit(MTIP_DDF_REMOVE_PENDING_BIT,
@@ -3872,7 +3900,15 @@ static void mtip_make_request(struct request_queue *queue, struct bio *bio)
 		return;
 	}
 
-	sg = mtip_hw_get_scatterlist(dd, &tag);
+	if (bio_data_dir(bio) == WRITE && bio_sectors(bio) <= 64 &&
+							dd->unal_qdepth) {
+		if (bio->bi_sector % 8 != 0) /* Unaligned on 4k boundaries */
+			unaligned = 1;
+		else if (bio_sectors(bio) % 8 != 0) /* Aligned but not 4k/8k */
+			unaligned = 1;
+	}
+
+	sg = mtip_hw_get_scatterlist(dd, &tag, unaligned);
 	if (likely(sg != NULL)) {
 		blk_queue_bounce(queue, &bio);
 
@@ -3880,7 +3916,7 @@ static void mtip_make_request(struct request_queue *queue, struct bio *bio)
 			dev_warn(&dd->pdev->dev,
 				"Maximum number of SGL entries exceeded\n");
 			bio_io_error(bio);
-			mtip_hw_release_scatterlist(dd, tag);
+			mtip_hw_release_scatterlist(dd, tag, unaligned);
 			return;
 		}
 
@@ -3900,7 +3936,8 @@ static void mtip_make_request(struct request_queue *queue, struct bio *bio)
 				tag,
 				bio_endio,
 				bio,
-				bio_data_dir(bio));
+				bio_data_dir(bio),
+				unaligned);
 	} else
 		bio_io_error(bio);
 }
@@ -4156,26 +4193,24 @@ static int mtip_block_remove(struct driver_data *dd)
  */
 static int mtip_block_shutdown(struct driver_data *dd)
 {
-	dev_info(&dd->pdev->dev,
-		"Shutting down %s ...\n", dd->disk->disk_name);
-
 	/* Delete our gendisk structure, and cleanup the blk queue. */
 	if (dd->disk) {
-		if (dd->disk->queue)
+		dev_info(&dd->pdev->dev,
+			"Shutting down %s ...\n", dd->disk->disk_name);
+
+		if (dd->disk->queue) {
 			del_gendisk(dd->disk);
-		else
+			blk_cleanup_queue(dd->queue);
+		} else
 			put_disk(dd->disk);
+		dd->disk  = NULL;
+		dd->queue = NULL;
 	}
 
-
 	spin_lock(&rssd_index_lock);
 	ida_remove(&rssd_index_ida, dd->index);
 	spin_unlock(&rssd_index_lock);
 
-	blk_cleanup_queue(dd->queue);
-	dd->disk  = NULL;
-	dd->queue = NULL;
-
 	mtip_hw_shutdown(dd);
 	return 0;
 }
diff --git a/drivers/block/mtip32xx/mtip32xx.h b/drivers/block/mtip32xx/mtip32xx.h
index 8e8334c9dd0f..3bb8a295fbe4 100644
--- a/drivers/block/mtip32xx/mtip32xx.h
+++ b/drivers/block/mtip32xx/mtip32xx.h
@@ -52,6 +52,9 @@
 #define MTIP_FTL_REBUILD_MAGIC		0xED51
 #define MTIP_FTL_REBUILD_TIMEOUT_MS	2400000
 
+/* unaligned IO handling */
+#define MTIP_MAX_UNALIGNED_SLOTS	8
+
 /* Macro to extract the tag bit number from a tag value. */
 #define MTIP_TAG_BIT(tag)	(tag & 0x1F)
 
@@ -333,6 +336,8 @@ struct mtip_cmd {
 
 	int scatter_ents; /* Number of scatter list entries used */
 
+	int unaligned; /* command is unaligned on 4k boundary */
+
 	struct scatterlist sg[MTIP_MAX_SG]; /* Scatter list entries */
 
 	int retries; /* The number of retries left for this command. */
@@ -452,6 +457,10 @@ struct mtip_port {
 	 * command slots available.
 	 */
 	struct semaphore cmd_slot;
+
+	/* Semaphore to control queue depth of unaligned IOs */
+	struct semaphore cmd_slot_unal;
+
 	/* Spinlock for working around command-issue bug. */
 	spinlock_t cmd_issue_lock[MTIP_MAX_SLOT_GROUPS];
 };
@@ -502,6 +511,8 @@ struct driver_data {
 
 	int isr_binding;
 
+	int unal_qdepth; /* qdepth of unaligned IO queue */
+
 	struct list_head online_list; /* linkage for online list */
 
 	struct list_head remove_list; /* linkage for removing list */
diff --git a/drivers/block/nbd.c b/drivers/block/nbd.c
index 7fecc784be01..037288e7874d 100644
--- a/drivers/block/nbd.c
+++ b/drivers/block/nbd.c
@@ -856,6 +856,8 @@ static int __init nbd_init(void)
 		disk->queue->limits.discard_granularity = 512;
 		disk->queue->limits.max_discard_sectors = UINT_MAX;
 		disk->queue->limits.discard_zeroes_data = 0;
+		blk_queue_max_hw_sectors(disk->queue, 65536);
+		disk->queue->limits.max_sectors = 256;
 	}
 
 	if (register_blkdev(NBD_MAJOR, "nbd")) {
diff --git a/drivers/block/nvme.c b/drivers/block/nvme-core.c
index 9dcefe40380b..8efdfaa44a59 100644
--- a/drivers/block/nvme.c
+++ b/drivers/block/nvme-core.c
@@ -39,14 +39,13 @@
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <linux/types.h>
-
+#include <scsi/sg.h>
 #include <asm-generic/io-64-nonatomic-lo-hi.h>
 
 #define NVME_Q_DEPTH 1024
 #define SQ_SIZE(depth)		(depth * sizeof(struct nvme_command))
 #define CQ_SIZE(depth)		(depth * sizeof(struct nvme_completion))
 #define NVME_MINORS 64
-#define NVME_IO_TIMEOUT	(5 * HZ)
 #define ADMIN_TIMEOUT	(60 * HZ)
 
 static int nvme_major;
@@ -60,43 +59,6 @@ static LIST_HEAD(dev_list);
 static struct task_struct *nvme_thread;
 
 /*
- * Represents an NVM Express device.  Each nvme_dev is a PCI function.
- */
-struct nvme_dev {
-	struct list_head node;
-	struct nvme_queue **queues;
-	u32 __iomem *dbs;
-	struct pci_dev *pci_dev;
-	struct dma_pool *prp_page_pool;
-	struct dma_pool *prp_small_pool;
-	int instance;
-	int queue_count;
-	int db_stride;
-	u32 ctrl_config;
-	struct msix_entry *entry;
-	struct nvme_bar __iomem *bar;
-	struct list_head namespaces;
-	char serial[20];
-	char model[40];
-	char firmware_rev[8];
-	u32 max_hw_sectors;
-};
-
-/*
- * An NVM Express namespace is equivalent to a SCSI LUN
- */
-struct nvme_ns {
-	struct list_head list;
-
-	struct nvme_dev *dev;
-	struct request_queue *queue;
-	struct gendisk *disk;
-
-	int ns_id;
-	int lba_shift;
-};
-
-/*
  * An NVM Express queue.  Each device has at least two (one for admin
  * commands and one for I/O commands).
  */
@@ -131,6 +93,7 @@ static inline void _nvme_check_size(void)
 	BUILD_BUG_ON(sizeof(struct nvme_create_sq) != 64);
 	BUILD_BUG_ON(sizeof(struct nvme_delete_queue) != 64);
 	BUILD_BUG_ON(sizeof(struct nvme_features) != 64);
+	BUILD_BUG_ON(sizeof(struct nvme_format_cmd) != 64);
 	BUILD_BUG_ON(sizeof(struct nvme_command) != 64);
 	BUILD_BUG_ON(sizeof(struct nvme_id_ctrl) != 4096);
 	BUILD_BUG_ON(sizeof(struct nvme_id_ns) != 4096);
@@ -261,12 +224,12 @@ static void *cancel_cmdid(struct nvme_queue *nvmeq, int cmdid,
 	return ctx;
 }
 
-static struct nvme_queue *get_nvmeq(struct nvme_dev *dev)
+struct nvme_queue *get_nvmeq(struct nvme_dev *dev)
 {
 	return dev->queues[get_cpu() + 1];
 }
 
-static void put_nvmeq(struct nvme_queue *nvmeq)
+void put_nvmeq(struct nvme_queue *nvmeq)
 {
 	put_cpu();
 }
@@ -294,22 +257,6 @@ static int nvme_submit_cmd(struct nvme_queue *nvmeq, struct nvme_command *cmd)
 	return 0;
 }
 
-/*
- * The nvme_iod describes the data in an I/O, including the list of PRP
- * entries.  You can't see it in this data structure because C doesn't let
- * me express that.  Use nvme_alloc_iod to ensure there's enough space
- * allocated to store the PRP list.
- */
-struct nvme_iod {
-	void *private;		/* For the use of the submitter of the I/O */
-	int npages;		/* In the PRP list. 0 means small pool in use */
-	int offset;		/* Of PRP list */
-	int nents;		/* Used in scatterlist */
-	int length;		/* Of data, in bytes */
-	dma_addr_t first_dma;
-	struct scatterlist sg[0];
-};
-
 static __le64 **iod_list(struct nvme_iod *iod)
 {
 	return ((void *)iod) + iod->offset;
@@ -343,7 +290,7 @@ nvme_alloc_iod(unsigned nseg, unsigned nbytes, gfp_t gfp)
 	return iod;
 }
 
-static void nvme_free_iod(struct nvme_dev *dev, struct nvme_iod *iod)
+void nvme_free_iod(struct nvme_dev *dev, struct nvme_iod *iod)
 {
 	const int last_prp = PAGE_SIZE / 8 - 1;
 	int i;
@@ -361,16 +308,6 @@ static void nvme_free_iod(struct nvme_dev *dev, struct nvme_iod *iod)
 	kfree(iod);
 }
 
-static void requeue_bio(struct nvme_dev *dev, struct bio *bio)
-{
-	struct nvme_queue *nvmeq = get_nvmeq(dev);
-	if (bio_list_empty(&nvmeq->sq_cong))
-		add_wait_queue(&nvmeq->sq_full, &nvmeq->sq_cong_wait);
-	bio_list_add(&nvmeq->sq_cong, bio);
-	put_nvmeq(nvmeq);
-	wake_up_process(nvme_thread);
-}
-
 static void bio_completion(struct nvme_dev *dev, void *ctx,
 						struct nvme_completion *cqe)
 {
@@ -382,19 +319,15 @@ static void bio_completion(struct nvme_dev *dev, void *ctx,
 		dma_unmap_sg(&dev->pci_dev->dev, iod->sg, iod->nents,
 			bio_data_dir(bio) ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
 	nvme_free_iod(dev, iod);
-	if (status) {
+	if (status)
 		bio_endio(bio, -EIO);
-	} else if (bio->bi_vcnt > bio->bi_idx) {
-		requeue_bio(dev, bio);
-	} else {
+	else
 		bio_endio(bio, 0);
-	}
 }
 
 /* length is in bytes.  gfp flags indicates whether we may sleep. */
-static int nvme_setup_prps(struct nvme_dev *dev,
-			struct nvme_common_command *cmd, struct nvme_iod *iod,
-			int total_len, gfp_t gfp)
+int nvme_setup_prps(struct nvme_dev *dev, struct nvme_common_command *cmd,
+			struct nvme_iod *iod, int total_len, gfp_t gfp)
 {
 	struct dma_pool *pool;
 	int length = total_len;
@@ -473,43 +406,193 @@ static int nvme_setup_prps(struct nvme_dev *dev,
 	return total_len;
 }
 
+struct nvme_bio_pair {
+	struct bio b1, b2, *parent;
+	struct bio_vec *bv1, *bv2;
+	int err;
+	atomic_t cnt;
+};
+
+static void nvme_bio_pair_endio(struct bio *bio, int err)
+{
+	struct nvme_bio_pair *bp = bio->bi_private;
+
+	if (err)
+		bp->err = err;
+
+	if (atomic_dec_and_test(&bp->cnt)) {
+		bio_endio(bp->parent, bp->err);
+		if (bp->bv1)
+			kfree(bp->bv1);
+		if (bp->bv2)
+			kfree(bp->bv2);
+		kfree(bp);
+	}
+}
+
+static struct nvme_bio_pair *nvme_bio_split(struct bio *bio, int idx,
+							int len, int offset)
+{
+	struct nvme_bio_pair *bp;
+
+	BUG_ON(len > bio->bi_size);
+	BUG_ON(idx > bio->bi_vcnt);
+
+	bp = kmalloc(sizeof(*bp), GFP_ATOMIC);
+	if (!bp)
+		return NULL;
+	bp->err = 0;
+
+	bp->b1 = *bio;
+	bp->b2 = *bio;
+
+	bp->b1.bi_size = len;
+	bp->b2.bi_size -= len;
+	bp->b1.bi_vcnt = idx;
+	bp->b2.bi_idx = idx;
+	bp->b2.bi_sector += len >> 9;
+
+	if (offset) {
+		bp->bv1 = kmalloc(bio->bi_max_vecs * sizeof(struct bio_vec),
+								GFP_ATOMIC);
+		if (!bp->bv1)
+			goto split_fail_1;
+
+		bp->bv2 = kmalloc(bio->bi_max_vecs * sizeof(struct bio_vec),
+								GFP_ATOMIC);
+		if (!bp->bv2)
+			goto split_fail_2;
+
+		memcpy(bp->bv1, bio->bi_io_vec,
+			bio->bi_max_vecs * sizeof(struct bio_vec));
+		memcpy(bp->bv2, bio->bi_io_vec,
+			bio->bi_max_vecs * sizeof(struct bio_vec));
+
+		bp->b1.bi_io_vec = bp->bv1;
+		bp->b2.bi_io_vec = bp->bv2;
+		bp->b2.bi_io_vec[idx].bv_offset += offset;
+		bp->b2.bi_io_vec[idx].bv_len -= offset;
+		bp->b1.bi_io_vec[idx].bv_len = offset;
+		bp->b1.bi_vcnt++;
+	} else
+		bp->bv1 = bp->bv2 = NULL;
+
+	bp->b1.bi_private = bp;
+	bp->b2.bi_private = bp;
+
+	bp->b1.bi_end_io = nvme_bio_pair_endio;
+	bp->b2.bi_end_io = nvme_bio_pair_endio;
+
+	bp->parent = bio;
+	atomic_set(&bp->cnt, 2);
+
+	return bp;
+
+ split_fail_2:
+	kfree(bp->bv1);
+ split_fail_1:
+	kfree(bp);
+	return NULL;
+}
+
+static int nvme_split_and_submit(struct bio *bio, struct nvme_queue *nvmeq,
+						int idx, int len, int offset)
+{
+	struct nvme_bio_pair *bp = nvme_bio_split(bio, idx, len, offset);
+	if (!bp)
+		return -ENOMEM;
+
+	if (bio_list_empty(&nvmeq->sq_cong))
+		add_wait_queue(&nvmeq->sq_full, &nvmeq->sq_cong_wait);
+	bio_list_add(&nvmeq->sq_cong, &bp->b1);
+	bio_list_add(&nvmeq->sq_cong, &bp->b2);
+
+	return 0;
+}
+
 /* NVMe scatterlists require no holes in the virtual address */
 #define BIOVEC_NOT_VIRT_MERGEABLE(vec1, vec2)	((vec2)->bv_offset || \
 			(((vec1)->bv_offset + (vec1)->bv_len) % PAGE_SIZE))
 
-static int nvme_map_bio(struct device *dev, struct nvme_iod *iod,
+static int nvme_map_bio(struct nvme_queue *nvmeq, struct nvme_iod *iod,
 		struct bio *bio, enum dma_data_direction dma_dir, int psegs)
 {
 	struct bio_vec *bvec, *bvprv = NULL;
 	struct scatterlist *sg = NULL;
-	int i, old_idx, length = 0, nsegs = 0;
+	int i, length = 0, nsegs = 0, split_len = bio->bi_size;
+
+	if (nvmeq->dev->stripe_size)
+		split_len = nvmeq->dev->stripe_size -
+			((bio->bi_sector << 9) & (nvmeq->dev->stripe_size - 1));
 
 	sg_init_table(iod->sg, psegs);
-	old_idx = bio->bi_idx;
 	bio_for_each_segment(bvec, bio, i) {
 		if (bvprv && BIOVEC_PHYS_MERGEABLE(bvprv, bvec)) {
 			sg->length += bvec->bv_len;
 		} else {
 			if (bvprv && BIOVEC_NOT_VIRT_MERGEABLE(bvprv, bvec))
-				break;
+				return nvme_split_and_submit(bio, nvmeq, i,
+								length, 0);
+
 			sg = sg ? sg + 1 : iod->sg;
 			sg_set_page(sg, bvec->bv_page, bvec->bv_len,
 							bvec->bv_offset);
 			nsegs++;
 		}
+
+		if (split_len - length < bvec->bv_len)
+			return nvme_split_and_submit(bio, nvmeq, i, split_len,
+							split_len - length);
 		length += bvec->bv_len;
 		bvprv = bvec;
 	}
-	bio->bi_idx = i;
 	iod->nents = nsegs;
 	sg_mark_end(sg);
-	if (dma_map_sg(dev, iod->sg, iod->nents, dma_dir) == 0) {
-		bio->bi_idx = old_idx;
+	if (dma_map_sg(nvmeq->q_dmadev, iod->sg, iod->nents, dma_dir) == 0)
 		return -ENOMEM;
-	}
+
+	BUG_ON(length != bio->bi_size);
 	return length;
 }
 
+/*
+ * We reuse the small pool to allocate the 16-byte range here as it is not
+ * worth having a special pool for these or additional cases to handle freeing
+ * the iod.
+ */
+static int nvme_submit_discard(struct nvme_queue *nvmeq, struct nvme_ns *ns,
+		struct bio *bio, struct nvme_iod *iod, int cmdid)
+{
+	struct nvme_dsm_range *range;
+	struct nvme_command *cmnd = &nvmeq->sq_cmds[nvmeq->sq_tail];
+
+	range = dma_pool_alloc(nvmeq->dev->prp_small_pool, GFP_ATOMIC,
+							&iod->first_dma);
+	if (!range)
+		return -ENOMEM;
+
+	iod_list(iod)[0] = (__le64 *)range;
+	iod->npages = 0;
+
+	range->cattr = cpu_to_le32(0);
+	range->nlb = cpu_to_le32(bio->bi_size >> ns->lba_shift);
+	range->slba = cpu_to_le64(nvme_block_nr(ns, bio->bi_sector));
+
+	memset(cmnd, 0, sizeof(*cmnd));
+	cmnd->dsm.opcode = nvme_cmd_dsm;
+	cmnd->dsm.command_id = cmdid;
+	cmnd->dsm.nsid = cpu_to_le32(ns->ns_id);
+	cmnd->dsm.prp1 = cpu_to_le64(iod->first_dma);
+	cmnd->dsm.nr = 0;
+	cmnd->dsm.attributes = cpu_to_le32(NVME_DSMGMT_AD);
+
+	if (++nvmeq->sq_tail == nvmeq->q_depth)
+		nvmeq->sq_tail = 0;
+	writel(nvmeq->sq_tail, nvmeq->q_db);
+
+	return 0;
+}
+
 static int nvme_submit_flush(struct nvme_queue *nvmeq, struct nvme_ns *ns,
 								int cmdid)
 {
@@ -527,7 +610,7 @@ static int nvme_submit_flush(struct nvme_queue *nvmeq, struct nvme_ns *ns,
 	return 0;
 }
 
-static int nvme_submit_flush_data(struct nvme_queue *nvmeq, struct nvme_ns *ns)
+int nvme_submit_flush_data(struct nvme_queue *nvmeq, struct nvme_ns *ns)
 {
 	int cmdid = alloc_cmdid(nvmeq, (void *)CMD_CTX_FLUSH,
 					special_completion, NVME_IO_TIMEOUT);
@@ -567,6 +650,12 @@ static int nvme_submit_bio_queue(struct nvme_queue *nvmeq, struct nvme_ns *ns,
 	if (unlikely(cmdid < 0))
 		goto free_iod;
 
+	if (bio->bi_rw & REQ_DISCARD) {
+		result = nvme_submit_discard(nvmeq, ns, bio, iod, cmdid);
+		if (result)
+			goto free_cmdid;
+		return result;
+	}
 	if ((bio->bi_rw & REQ_FLUSH) && !psegs)
 		return nvme_submit_flush(nvmeq, ns, cmdid);
 
@@ -591,8 +680,8 @@ static int nvme_submit_bio_queue(struct nvme_queue *nvmeq, struct nvme_ns *ns,
 		dma_dir = DMA_FROM_DEVICE;
 	}
 
-	result = nvme_map_bio(nvmeq->q_dmadev, iod, bio, dma_dir, psegs);
-	if (result < 0)
+	result = nvme_map_bio(nvmeq, iod, bio, dma_dir, psegs);
+	if (result <= 0)
 		goto free_cmdid;
 	length = result;
 
@@ -600,13 +689,11 @@ static int nvme_submit_bio_queue(struct nvme_queue *nvmeq, struct nvme_ns *ns,
 	cmnd->rw.nsid = cpu_to_le32(ns->ns_id);
 	length = nvme_setup_prps(nvmeq->dev, &cmnd->common, iod, length,
 								GFP_ATOMIC);
-	cmnd->rw.slba = cpu_to_le64(bio->bi_sector >> (ns->lba_shift - 9));
+	cmnd->rw.slba = cpu_to_le64(nvme_block_nr(ns, bio->bi_sector));
 	cmnd->rw.length = cpu_to_le16((length >> ns->lba_shift) - 1);
 	cmnd->rw.control = cpu_to_le16(control);
 	cmnd->rw.dsmgmt = cpu_to_le32(dsmgmt);
 
-	bio->bi_sector += length >> 9;
-
 	if (++nvmeq->sq_tail == nvmeq->q_depth)
 		nvmeq->sq_tail = 0;
 	writel(nvmeq->sq_tail, nvmeq->q_db);
@@ -724,8 +811,8 @@ static void sync_completion(struct nvme_dev *dev, void *ctx,
  * Returns 0 on success.  If the result is negative, it's a Linux error code;
  * if the result is positive, it's an NVM Express status code
  */
-static int nvme_submit_sync_cmd(struct nvme_queue *nvmeq,
-			struct nvme_command *cmd, u32 *result, unsigned timeout)
+int nvme_submit_sync_cmd(struct nvme_queue *nvmeq, struct nvme_command *cmd,
+						u32 *result, unsigned timeout)
 {
 	int cmdid;
 	struct sync_cmd_info cmdinfo;
@@ -741,7 +828,7 @@ static int nvme_submit_sync_cmd(struct nvme_queue *nvmeq,
 
 	set_current_state(TASK_KILLABLE);
 	nvme_submit_cmd(nvmeq, cmd);
-	schedule();
+	schedule_timeout(timeout);
 
 	if (cmdinfo.status == -EINTR) {
 		nvme_abort_command(nvmeq, cmdid);
@@ -754,7 +841,7 @@ static int nvme_submit_sync_cmd(struct nvme_queue *nvmeq,
 	return cmdinfo.status;
 }
 
-static int nvme_submit_admin_cmd(struct nvme_dev *dev, struct nvme_command *cmd,
+int nvme_submit_admin_cmd(struct nvme_dev *dev, struct nvme_command *cmd,
 								u32 *result)
 {
 	return nvme_submit_sync_cmd(dev->queues[0], cmd, result, ADMIN_TIMEOUT);
@@ -827,7 +914,7 @@ static int adapter_delete_sq(struct nvme_dev *dev, u16 sqid)
 	return adapter_delete_queue(dev, nvme_admin_delete_sq, sqid);
 }
 
-static int nvme_identify(struct nvme_dev *dev, unsigned nsid, unsigned cns,
+int nvme_identify(struct nvme_dev *dev, unsigned nsid, unsigned cns,
 							dma_addr_t dma_addr)
 {
 	struct nvme_command c;
@@ -841,7 +928,7 @@ static int nvme_identify(struct nvme_dev *dev, unsigned nsid, unsigned cns,
 	return nvme_submit_admin_cmd(dev, &c, NULL);
 }
 
-static int nvme_get_features(struct nvme_dev *dev, unsigned fid, unsigned nsid,
+int nvme_get_features(struct nvme_dev *dev, unsigned fid, unsigned nsid,
 					dma_addr_t dma_addr, u32 *result)
 {
 	struct nvme_command c;
@@ -855,8 +942,8 @@ static int nvme_get_features(struct nvme_dev *dev, unsigned fid, unsigned nsid,
 	return nvme_submit_admin_cmd(dev, &c, result);
 }
 
-static int nvme_set_features(struct nvme_dev *dev, unsigned fid,
-			unsigned dword11, dma_addr_t dma_addr, u32 *result)
+int nvme_set_features(struct nvme_dev *dev, unsigned fid, unsigned dword11,
+					dma_addr_t dma_addr, u32 *result)
 {
 	struct nvme_command c;
 
@@ -885,7 +972,7 @@ static void nvme_cancel_ios(struct nvme_queue *nvmeq, bool timeout)
 		void *ctx;
 		nvme_completion_fn fn;
 		static struct nvme_completion cqe = {
-			.status = cpu_to_le16(NVME_SC_ABORT_REQ) << 1,
+			.status = cpu_to_le16(NVME_SC_ABORT_REQ << 1),
 		};
 
 		if (timeout && !time_after(now, info[cmdid].timeout))
@@ -966,7 +1053,7 @@ static struct nvme_queue *nvme_alloc_queue(struct nvme_dev *dev, int qid,
 	return nvmeq;
 
  free_cqdma:
-	dma_free_coherent(dmadev, CQ_SIZE(nvmeq->q_depth), (void *)nvmeq->cqes,
+	dma_free_coherent(dmadev, CQ_SIZE(depth), (void *)nvmeq->cqes,
 							nvmeq->cq_dma_addr);
  free_nvmeq:
 	kfree(nvmeq);
@@ -1021,15 +1108,60 @@ static struct nvme_queue *nvme_create_queue(struct nvme_dev *dev, int qid,
 	return ERR_PTR(result);
 }
 
+static int nvme_wait_ready(struct nvme_dev *dev, u64 cap, bool enabled)
+{
+	unsigned long timeout;
+	u32 bit = enabled ? NVME_CSTS_RDY : 0;
+
+	timeout = ((NVME_CAP_TIMEOUT(cap) + 1) * HZ / 2) + jiffies;
+
+	while ((readl(&dev->bar->csts) & NVME_CSTS_RDY) != bit) {
+		msleep(100);
+		if (fatal_signal_pending(current))
+			return -EINTR;
+		if (time_after(jiffies, timeout)) {
+			dev_err(&dev->pci_dev->dev,
+				"Device not ready; aborting initialisation\n");
+			return -ENODEV;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * If the device has been passed off to us in an enabled state, just clear
+ * the enabled bit.  The spec says we should set the 'shutdown notification
+ * bits', but doing so may cause the device to complete commands to the
+ * admin queue ... and we don't know what memory that might be pointing at!
+ */
+static int nvme_disable_ctrl(struct nvme_dev *dev, u64 cap)
+{
+	u32 cc = readl(&dev->bar->cc);
+
+	if (cc & NVME_CC_ENABLE)
+		writel(cc & ~NVME_CC_ENABLE, &dev->bar->cc);
+	return nvme_wait_ready(dev, cap, false);
+}
+
+static int nvme_enable_ctrl(struct nvme_dev *dev, u64 cap)
+{
+	return nvme_wait_ready(dev, cap, true);
+}
+
 static int nvme_configure_admin_queue(struct nvme_dev *dev)
 {
-	int result = 0;
+	int result;
 	u32 aqa;
-	u64 cap;
-	unsigned long timeout;
+	u64 cap = readq(&dev->bar->cap);
 	struct nvme_queue *nvmeq;
 
 	dev->dbs = ((void __iomem *)dev->bar) + 4096;
+	dev->db_stride = NVME_CAP_STRIDE(cap);
+
+	result = nvme_disable_ctrl(dev, cap);
+	if (result < 0)
+		return result;
 
 	nvmeq = nvme_alloc_queue(dev, 0, 64, 0);
 	if (!nvmeq)
@@ -1043,38 +1175,28 @@ static int nvme_configure_admin_queue(struct nvme_dev *dev)
 	dev->ctrl_config |= NVME_CC_ARB_RR | NVME_CC_SHN_NONE;
 	dev->ctrl_config |= NVME_CC_IOSQES | NVME_CC_IOCQES;
 
-	writel(0, &dev->bar->cc);
 	writel(aqa, &dev->bar->aqa);
 	writeq(nvmeq->sq_dma_addr, &dev->bar->asq);
 	writeq(nvmeq->cq_dma_addr, &dev->bar->acq);
 	writel(dev->ctrl_config, &dev->bar->cc);
 
-	cap = readq(&dev->bar->cap);
-	timeout = ((NVME_CAP_TIMEOUT(cap) + 1) * HZ / 2) + jiffies;
-	dev->db_stride = NVME_CAP_STRIDE(cap);
-
-	while (!result && !(readl(&dev->bar->csts) & NVME_CSTS_RDY)) {
-		msleep(100);
-		if (fatal_signal_pending(current))
-			result = -EINTR;
-		if (time_after(jiffies, timeout)) {
-			dev_err(&dev->pci_dev->dev,
-				"Device not ready; aborting initialisation\n");
-			result = -ENODEV;
-		}
-	}
-
-	if (result) {
-		nvme_free_queue_mem(nvmeq);
-		return result;
-	}
+	result = nvme_enable_ctrl(dev, cap);
+	if (result)
+		goto free_q;
 
 	result = queue_request_irq(dev, nvmeq, "nvme admin");
+	if (result)
+		goto free_q;
+
 	dev->queues[0] = nvmeq;
 	return result;
+
+ free_q:
+	nvme_free_queue_mem(nvmeq);
+	return result;
 }
 
-static struct nvme_iod *nvme_map_user_pages(struct nvme_dev *dev, int write,
+struct nvme_iod *nvme_map_user_pages(struct nvme_dev *dev, int write,
 				unsigned long addr, unsigned length)
 {
 	int i, err, count, nents, offset;
@@ -1130,7 +1252,7 @@ static struct nvme_iod *nvme_map_user_pages(struct nvme_dev *dev, int write,
 	return ERR_PTR(err);
 }
 
-static void nvme_unmap_user_pages(struct nvme_dev *dev, int write,
+void nvme_unmap_user_pages(struct nvme_dev *dev, int write,
 			struct nvme_iod *iod)
 {
 	int i;
@@ -1148,13 +1270,19 @@ static int nvme_submit_io(struct nvme_ns *ns, struct nvme_user_io __user *uio)
 	struct nvme_queue *nvmeq;
 	struct nvme_user_io io;
 	struct nvme_command c;
-	unsigned length;
-	int status;
-	struct nvme_iod *iod;
+	unsigned length, meta_len;
+	int status, i;
+	struct nvme_iod *iod, *meta_iod = NULL;
+	dma_addr_t meta_dma_addr;
+	void *meta, *uninitialized_var(meta_mem);
 
 	if (copy_from_user(&io, uio, sizeof(io)))
 		return -EFAULT;
 	length = (io.nblocks + 1) << ns->lba_shift;
+	meta_len = (io.nblocks + 1) * ns->ms;
+
+	if (meta_len && ((io.metadata & 3) || !io.metadata))
+		return -EINVAL;
 
 	switch (io.opcode) {
 	case nvme_cmd_write:
@@ -1176,11 +1304,42 @@ static int nvme_submit_io(struct nvme_ns *ns, struct nvme_user_io __user *uio)
 	c.rw.slba = cpu_to_le64(io.slba);
 	c.rw.length = cpu_to_le16(io.nblocks);
 	c.rw.control = cpu_to_le16(io.control);
-	c.rw.dsmgmt = cpu_to_le16(io.dsmgmt);
-	c.rw.reftag = io.reftag;
-	c.rw.apptag = io.apptag;
-	c.rw.appmask = io.appmask;
-	/* XXX: metadata */
+	c.rw.dsmgmt = cpu_to_le32(io.dsmgmt);
+	c.rw.reftag = cpu_to_le32(io.reftag);
+	c.rw.apptag = cpu_to_le16(io.apptag);
+	c.rw.appmask = cpu_to_le16(io.appmask);
+
+	if (meta_len) {
+		meta_iod = nvme_map_user_pages(dev, io.opcode & 1, io.metadata, meta_len);
+		if (IS_ERR(meta_iod)) {
+			status = PTR_ERR(meta_iod);
+			meta_iod = NULL;
+			goto unmap;
+		}
+
+		meta_mem = dma_alloc_coherent(&dev->pci_dev->dev, meta_len,
+						&meta_dma_addr, GFP_KERNEL);
+		if (!meta_mem) {
+			status = -ENOMEM;
+			goto unmap;
+		}
+
+		if (io.opcode & 1) {
+			int meta_offset = 0;
+
+			for (i = 0; i < meta_iod->nents; i++) {
+				meta = kmap_atomic(sg_page(&meta_iod->sg[i])) +
+						meta_iod->sg[i].offset;
+				memcpy(meta_mem + meta_offset, meta,
+						meta_iod->sg[i].length);
+				kunmap_atomic(meta);
+				meta_offset += meta_iod->sg[i].length;
+			}
+		}
+
+		c.rw.metadata = cpu_to_le64(meta_dma_addr);
+	}
+
 	length = nvme_setup_prps(dev, &c.common, iod, length, GFP_KERNEL);
 
 	nvmeq = get_nvmeq(dev);
@@ -1196,8 +1355,33 @@ static int nvme_submit_io(struct nvme_ns *ns, struct nvme_user_io __user *uio)
 	else
 		status = nvme_submit_sync_cmd(nvmeq, &c, NULL, NVME_IO_TIMEOUT);
 
+	if (meta_len) {
+		if (status == NVME_SC_SUCCESS && !(io.opcode & 1)) {
+			int meta_offset = 0;
+
+			for (i = 0; i < meta_iod->nents; i++) {
+				meta = kmap_atomic(sg_page(&meta_iod->sg[i])) +
+						meta_iod->sg[i].offset;
+				memcpy(meta, meta_mem + meta_offset,
+						meta_iod->sg[i].length);
+				kunmap_atomic(meta);
+				meta_offset += meta_iod->sg[i].length;
+			}
+		}
+
+		dma_free_coherent(&dev->pci_dev->dev, meta_len, meta_mem,
+								meta_dma_addr);
+	}
+
+ unmap:
 	nvme_unmap_user_pages(dev, io.opcode & 1, iod);
 	nvme_free_iod(dev, iod);
+
+	if (meta_iod) {
+		nvme_unmap_user_pages(dev, io.opcode & 1, meta_iod);
+		nvme_free_iod(dev, meta_iod);
+	}
+
 	return status;
 }
 
@@ -1208,6 +1392,7 @@ static int nvme_user_admin_cmd(struct nvme_dev *dev,
 	struct nvme_command c;
 	int status, length;
 	struct nvme_iod *uninitialized_var(iod);
+	unsigned timeout;
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EACCES;
@@ -1237,10 +1422,13 @@ static int nvme_user_admin_cmd(struct nvme_dev *dev,
 								GFP_KERNEL);
 	}
 
+	timeout = cmd.timeout_ms ? msecs_to_jiffies(cmd.timeout_ms) :
+								ADMIN_TIMEOUT;
 	if (length != cmd.data_len)
 		status = -ENOMEM;
 	else
-		status = nvme_submit_admin_cmd(dev, &c, &cmd.result);
+		status = nvme_submit_sync_cmd(dev->queues[0], &c, &cmd.result,
+								timeout);
 
 	if (cmd.data_len) {
 		nvme_unmap_user_pages(dev, cmd.opcode & 1, iod);
@@ -1266,6 +1454,10 @@ static int nvme_ioctl(struct block_device *bdev, fmode_t mode, unsigned int cmd,
 		return nvme_user_admin_cmd(ns->dev, (void __user *)arg);
 	case NVME_IOCTL_SUBMIT_IO:
 		return nvme_submit_io(ns, (void __user *)arg);
+	case SG_GET_VERSION_NUM:
+		return nvme_sg_get_version_num((void __user *)arg);
+	case SG_IO:
+		return nvme_sg_io(ns, (void __user *)arg);
 	default:
 		return -ENOTTY;
 	}
@@ -1282,13 +1474,17 @@ static void nvme_resubmit_bios(struct nvme_queue *nvmeq)
 	while (bio_list_peek(&nvmeq->sq_cong)) {
 		struct bio *bio = bio_list_pop(&nvmeq->sq_cong);
 		struct nvme_ns *ns = bio->bi_bdev->bd_disk->private_data;
+
+		if (bio_list_empty(&nvmeq->sq_cong))
+			remove_wait_queue(&nvmeq->sq_full,
+							&nvmeq->sq_cong_wait);
 		if (nvme_submit_bio_queue(nvmeq, ns, bio)) {
+			if (bio_list_empty(&nvmeq->sq_cong))
+				add_wait_queue(&nvmeq->sq_full,
+							&nvmeq->sq_cong_wait);
 			bio_list_add_head(&nvmeq->sq_cong, bio);
 			break;
 		}
-		if (bio_list_empty(&nvmeq->sq_cong))
-			remove_wait_queue(&nvmeq->sq_full,
-							&nvmeq->sq_cong_wait);
 	}
 }
 
@@ -1297,7 +1493,7 @@ static int nvme_kthread(void *data)
 	struct nvme_dev *dev;
 
 	while (!kthread_should_stop()) {
-		__set_current_state(TASK_RUNNING);
+		set_current_state(TASK_INTERRUPTIBLE);
 		spin_lock(&dev_list_lock);
 		list_for_each_entry(dev, &dev_list, node) {
 			int i;
@@ -1314,8 +1510,7 @@ static int nvme_kthread(void *data)
 			}
 		}
 		spin_unlock(&dev_list_lock);
-		set_current_state(TASK_INTERRUPTIBLE);
-		schedule_timeout(HZ);
+		schedule_timeout(round_jiffies_relative(HZ));
 	}
 	return 0;
 }
@@ -1347,6 +1542,16 @@ static void nvme_put_ns_idx(int index)
 	spin_unlock(&dev_list_lock);
 }
 
+static void nvme_config_discard(struct nvme_ns *ns)
+{
+	u32 logical_block_size = queue_logical_block_size(ns->queue);
+	ns->queue->limits.discard_zeroes_data = 0;
+	ns->queue->limits.discard_alignment = logical_block_size;
+	ns->queue->limits.discard_granularity = logical_block_size;
+	ns->queue->limits.max_discard_sectors = 0xffffffff;
+	queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, ns->queue);
+}
+
 static struct nvme_ns *nvme_alloc_ns(struct nvme_dev *dev, int nsid,
 			struct nvme_id_ns *id, struct nvme_lba_range_type *rt)
 {
@@ -1366,7 +1571,6 @@ static struct nvme_ns *nvme_alloc_ns(struct nvme_dev *dev, int nsid,
 	ns->queue->queue_flags = QUEUE_FLAG_DEFAULT;
 	queue_flag_set_unlocked(QUEUE_FLAG_NOMERGES, ns->queue);
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, ns->queue);
-/*	queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, ns->queue); */
 	blk_queue_make_request(ns->queue, nvme_make_request);
 	ns->dev = dev;
 	ns->queue->queuedata = ns;
@@ -1378,6 +1582,7 @@ static struct nvme_ns *nvme_alloc_ns(struct nvme_dev *dev, int nsid,
 	ns->disk = disk;
 	lbaf = id->flbas & 0xf;
 	ns->lba_shift = id->lbaf[lbaf].ds;
+	ns->ms = le16_to_cpu(id->lbaf[lbaf].ms);
 	blk_queue_logical_block_size(ns->queue, 1 << ns->lba_shift);
 	if (dev->max_hw_sectors)
 		blk_queue_max_hw_sectors(ns->queue, dev->max_hw_sectors);
@@ -1392,6 +1597,9 @@ static struct nvme_ns *nvme_alloc_ns(struct nvme_dev *dev, int nsid,
 	sprintf(disk->disk_name, "nvme%dn%d", dev->instance, nsid);
 	set_capacity(disk, le64_to_cpup(&id->nsze) << (ns->lba_shift - 9));
 
+	if (dev->oncs & NVME_CTRL_ONCS_DSM)
+		nvme_config_discard(ns);
+
 	return ns;
 
  out_free_queue:
@@ -1496,14 +1704,21 @@ static void nvme_free_queues(struct nvme_dev *dev)
 		nvme_free_queue(dev, i);
 }
 
+/*
+ * Return: error value if an error occurred setting up the queues or calling
+ * Identify Device.  0 if these succeeded, even if adding some of the
+ * namespaces failed.  At the moment, these failures are silent.  TBD which
+ * failures should be reported.
+ */
 static int nvme_dev_add(struct nvme_dev *dev)
 {
 	int res, nn, i;
-	struct nvme_ns *ns, *next;
+	struct nvme_ns *ns;
 	struct nvme_id_ctrl *ctrl;
 	struct nvme_id_ns *id_ns;
 	void *mem;
 	dma_addr_t dma_addr;
+	int shift = NVME_CAP_MPSMIN(readq(&dev->bar->cap)) + 12;
 
 	res = nvme_setup_io_queues(dev);
 	if (res)
@@ -1511,22 +1726,26 @@ static int nvme_dev_add(struct nvme_dev *dev)
 
 	mem = dma_alloc_coherent(&dev->pci_dev->dev, 8192, &dma_addr,
 								GFP_KERNEL);
+	if (!mem)
+		return -ENOMEM;
 
 	res = nvme_identify(dev, 0, 1, dma_addr);
 	if (res) {
 		res = -EIO;
-		goto out_free;
+		goto out;
 	}
 
 	ctrl = mem;
 	nn = le32_to_cpup(&ctrl->nn);
+	dev->oncs = le16_to_cpup(&ctrl->oncs);
 	memcpy(dev->serial, ctrl->sn, sizeof(ctrl->sn));
 	memcpy(dev->model, ctrl->mn, sizeof(ctrl->mn));
 	memcpy(dev->firmware_rev, ctrl->fr, sizeof(ctrl->fr));
-	if (ctrl->mdts) {
-		int shift = NVME_CAP_MPSMIN(readq(&dev->bar->cap)) + 12;
+	if (ctrl->mdts)
 		dev->max_hw_sectors = 1 << (ctrl->mdts + shift - 9);
-	}
+	if ((dev->pci_dev->vendor == PCI_VENDOR_ID_INTEL) &&
+			(dev->pci_dev->device == 0x0953) && ctrl->vs[3])
+		dev->stripe_size = 1 << (ctrl->vs[3] + shift);
 
 	id_ns = mem;
 	for (i = 1; i <= nn; i++) {
@@ -1548,14 +1767,7 @@ static int nvme_dev_add(struct nvme_dev *dev)
 	}
 	list_for_each_entry(ns, &dev->namespaces, list)
 		add_disk(ns->disk);
-
-	goto out;
-
- out_free:
-	list_for_each_entry_safe(ns, next, &dev->namespaces, list) {
-		list_del(&ns->list);
-		nvme_ns_free(ns);
-	}
+	res = 0;
 
  out:
 	dma_free_coherent(&dev->pci_dev->dev, 8192, mem, dma_addr);
@@ -1634,6 +1846,56 @@ static void nvme_release_instance(struct nvme_dev *dev)
 	spin_unlock(&dev_list_lock);
 }
 
+static void nvme_free_dev(struct kref *kref)
+{
+	struct nvme_dev *dev = container_of(kref, struct nvme_dev, kref);
+	nvme_dev_remove(dev);
+	pci_disable_msix(dev->pci_dev);
+	iounmap(dev->bar);
+	nvme_release_instance(dev);
+	nvme_release_prp_pools(dev);
+	pci_disable_device(dev->pci_dev);
+	pci_release_regions(dev->pci_dev);
+	kfree(dev->queues);
+	kfree(dev->entry);
+	kfree(dev);
+}
+
+static int nvme_dev_open(struct inode *inode, struct file *f)
+{
+	struct nvme_dev *dev = container_of(f->private_data, struct nvme_dev,
+								miscdev);
+	kref_get(&dev->kref);
+	f->private_data = dev;
+	return 0;
+}
+
+static int nvme_dev_release(struct inode *inode, struct file *f)
+{
+	struct nvme_dev *dev = f->private_data;
+	kref_put(&dev->kref, nvme_free_dev);
+	return 0;
+}
+
+static long nvme_dev_ioctl(struct file *f, unsigned int cmd, unsigned long arg)
+{
+	struct nvme_dev *dev = f->private_data;
+	switch (cmd) {
+	case NVME_IOCTL_ADMIN_CMD:
+		return nvme_user_admin_cmd(dev, (void __user *)arg);
+	default:
+		return -ENOTTY;
+	}
+}
+
+static const struct file_operations nvme_dev_fops = {
+	.owner		= THIS_MODULE,
+	.open		= nvme_dev_open,
+	.release	= nvme_dev_release,
+	.unlocked_ioctl	= nvme_dev_ioctl,
+	.compat_ioctl	= nvme_dev_ioctl,
+};
+
 static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	int bars, result = -ENOMEM;
@@ -1692,8 +1954,20 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (result)
 		goto delete;
 
+	scnprintf(dev->name, sizeof(dev->name), "nvme%d", dev->instance);
+	dev->miscdev.minor = MISC_DYNAMIC_MINOR;
+	dev->miscdev.parent = &pdev->dev;
+	dev->miscdev.name = dev->name;
+	dev->miscdev.fops = &nvme_dev_fops;
+	result = misc_register(&dev->miscdev);
+	if (result)
+		goto remove;
+
+	kref_init(&dev->kref);
 	return 0;
 
+ remove:
+	nvme_dev_remove(dev);
  delete:
 	spin_lock(&dev_list_lock);
 	list_del(&dev->node);
@@ -1719,16 +1993,8 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 static void nvme_remove(struct pci_dev *pdev)
 {
 	struct nvme_dev *dev = pci_get_drvdata(pdev);
-	nvme_dev_remove(dev);
-	pci_disable_msix(pdev);
-	iounmap(dev->bar);
-	nvme_release_instance(dev);
-	nvme_release_prp_pools(dev);
-	pci_disable_device(pdev);
-	pci_release_regions(pdev);
-	kfree(dev->queues);
-	kfree(dev->entry);
-	kfree(dev);
+	misc_deregister(&dev->miscdev);
+	kref_put(&dev->kref, nvme_free_dev);
 }
 
 /* These functions are yet to be implemented */
diff --git a/drivers/block/nvme-scsi.c b/drivers/block/nvme-scsi.c
new file mode 100644
index 000000000000..fed54b039893
--- /dev/null
+++ b/drivers/block/nvme-scsi.c
@@ -0,0 +1,3053 @@
+/*
+ * NVM Express device driver
+ * Copyright (c) 2011, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+/*
+ * Refer to the SCSI-NVMe Translation spec for details on how
+ * each command is translated.
+ */
+
+#include <linux/nvme.h>
+#include <linux/bio.h>
+#include <linux/bitops.h>
+#include <linux/blkdev.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/genhd.h>
+#include <linux/idr.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kdev_t.h>
+#include <linux/kthread.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/pci.h>
+#include <linux/poison.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/version.h>
+#include <scsi/sg.h>
+#include <scsi/scsi.h>
+
+
+static int sg_version_num = 30534;	/* 2 digits for each component */
+
+#define SNTI_TRANSLATION_SUCCESS			0
+#define SNTI_INTERNAL_ERROR				1
+
+/* VPD Page Codes */
+#define VPD_SUPPORTED_PAGES				0x00
+#define VPD_SERIAL_NUMBER				0x80
+#define VPD_DEVICE_IDENTIFIERS				0x83
+#define VPD_EXTENDED_INQUIRY				0x86
+#define VPD_BLOCK_DEV_CHARACTERISTICS			0xB1
+
+/* CDB offsets */
+#define REPORT_LUNS_CDB_ALLOC_LENGTH_OFFSET		6
+#define REPORT_LUNS_SR_OFFSET				2
+#define READ_CAP_16_CDB_ALLOC_LENGTH_OFFSET		10
+#define REQUEST_SENSE_CDB_ALLOC_LENGTH_OFFSET		4
+#define REQUEST_SENSE_DESC_OFFSET			1
+#define REQUEST_SENSE_DESC_MASK				0x01
+#define DESCRIPTOR_FORMAT_SENSE_DATA_TYPE		1
+#define INQUIRY_EVPD_BYTE_OFFSET			1
+#define INQUIRY_PAGE_CODE_BYTE_OFFSET			2
+#define INQUIRY_EVPD_BIT_MASK				1
+#define INQUIRY_CDB_ALLOCATION_LENGTH_OFFSET		3
+#define START_STOP_UNIT_CDB_IMMED_OFFSET		1
+#define START_STOP_UNIT_CDB_IMMED_MASK			0x1
+#define START_STOP_UNIT_CDB_POWER_COND_MOD_OFFSET	3
+#define START_STOP_UNIT_CDB_POWER_COND_MOD_MASK		0xF
+#define START_STOP_UNIT_CDB_POWER_COND_OFFSET		4
+#define START_STOP_UNIT_CDB_POWER_COND_MASK		0xF0
+#define START_STOP_UNIT_CDB_NO_FLUSH_OFFSET		4
+#define START_STOP_UNIT_CDB_NO_FLUSH_MASK		0x4
+#define START_STOP_UNIT_CDB_START_OFFSET		4
+#define START_STOP_UNIT_CDB_START_MASK			0x1
+#define WRITE_BUFFER_CDB_MODE_OFFSET			1
+#define WRITE_BUFFER_CDB_MODE_MASK			0x1F
+#define WRITE_BUFFER_CDB_BUFFER_ID_OFFSET		2
+#define WRITE_BUFFER_CDB_BUFFER_OFFSET_OFFSET		3
+#define WRITE_BUFFER_CDB_PARM_LIST_LENGTH_OFFSET	6
+#define FORMAT_UNIT_CDB_FORMAT_PROT_INFO_OFFSET		1
+#define FORMAT_UNIT_CDB_FORMAT_PROT_INFO_MASK		0xC0
+#define FORMAT_UNIT_CDB_FORMAT_PROT_INFO_SHIFT		6
+#define FORMAT_UNIT_CDB_LONG_LIST_OFFSET		1
+#define FORMAT_UNIT_CDB_LONG_LIST_MASK			0x20
+#define FORMAT_UNIT_CDB_FORMAT_DATA_OFFSET		1
+#define FORMAT_UNIT_CDB_FORMAT_DATA_MASK		0x10
+#define FORMAT_UNIT_SHORT_PARM_LIST_LEN			4
+#define FORMAT_UNIT_LONG_PARM_LIST_LEN			8
+#define FORMAT_UNIT_PROT_INT_OFFSET			3
+#define FORMAT_UNIT_PROT_FIELD_USAGE_OFFSET		0
+#define FORMAT_UNIT_PROT_FIELD_USAGE_MASK		0x07
+#define UNMAP_CDB_PARAM_LIST_LENGTH_OFFSET		7
+
+/* Misc. defines */
+#define NIBBLE_SHIFT					4
+#define FIXED_SENSE_DATA				0x70
+#define DESC_FORMAT_SENSE_DATA				0x72
+#define FIXED_SENSE_DATA_ADD_LENGTH			10
+#define LUN_ENTRY_SIZE					8
+#define LUN_DATA_HEADER_SIZE				8
+#define ALL_LUNS_RETURNED				0x02
+#define ALL_WELL_KNOWN_LUNS_RETURNED			0x01
+#define RESTRICTED_LUNS_RETURNED			0x00
+#define NVME_POWER_STATE_START_VALID			0x00
+#define NVME_POWER_STATE_ACTIVE				0x01
+#define NVME_POWER_STATE_IDLE				0x02
+#define NVME_POWER_STATE_STANDBY			0x03
+#define NVME_POWER_STATE_LU_CONTROL			0x07
+#define POWER_STATE_0					0
+#define POWER_STATE_1					1
+#define POWER_STATE_2					2
+#define POWER_STATE_3					3
+#define DOWNLOAD_SAVE_ACTIVATE				0x05
+#define DOWNLOAD_SAVE_DEFER_ACTIVATE			0x0E
+#define ACTIVATE_DEFERRED_MICROCODE			0x0F
+#define FORMAT_UNIT_IMMED_MASK				0x2
+#define FORMAT_UNIT_IMMED_OFFSET			1
+#define KELVIN_TEMP_FACTOR				273
+#define FIXED_FMT_SENSE_DATA_SIZE			18
+#define DESC_FMT_SENSE_DATA_SIZE			8
+
+/* SCSI/NVMe defines and bit masks */
+#define INQ_STANDARD_INQUIRY_PAGE			0x00
+#define INQ_SUPPORTED_VPD_PAGES_PAGE			0x00
+#define INQ_UNIT_SERIAL_NUMBER_PAGE			0x80
+#define INQ_DEVICE_IDENTIFICATION_PAGE			0x83
+#define INQ_EXTENDED_INQUIRY_DATA_PAGE			0x86
+#define INQ_BDEV_CHARACTERISTICS_PAGE			0xB1
+#define INQ_SERIAL_NUMBER_LENGTH			0x14
+#define INQ_NUM_SUPPORTED_VPD_PAGES			5
+#define VERSION_SPC_4					0x06
+#define ACA_UNSUPPORTED					0
+#define STANDARD_INQUIRY_LENGTH				36
+#define ADDITIONAL_STD_INQ_LENGTH			31
+#define EXTENDED_INQUIRY_DATA_PAGE_LENGTH		0x3C
+#define RESERVED_FIELD					0
+
+/* SCSI READ/WRITE Defines */
+#define IO_CDB_WP_MASK					0xE0
+#define IO_CDB_WP_SHIFT					5
+#define IO_CDB_FUA_MASK					0x8
+#define IO_6_CDB_LBA_OFFSET				0
+#define IO_6_CDB_LBA_MASK				0x001FFFFF
+#define IO_6_CDB_TX_LEN_OFFSET				4
+#define IO_6_DEFAULT_TX_LEN				256
+#define IO_10_CDB_LBA_OFFSET				2
+#define IO_10_CDB_TX_LEN_OFFSET				7
+#define IO_10_CDB_WP_OFFSET				1
+#define IO_10_CDB_FUA_OFFSET				1
+#define IO_12_CDB_LBA_OFFSET				2
+#define IO_12_CDB_TX_LEN_OFFSET				6
+#define IO_12_CDB_WP_OFFSET				1
+#define IO_12_CDB_FUA_OFFSET				1
+#define IO_16_CDB_FUA_OFFSET				1
+#define IO_16_CDB_WP_OFFSET				1
+#define IO_16_CDB_LBA_OFFSET				2
+#define IO_16_CDB_TX_LEN_OFFSET				10
+
+/* Mode Sense/Select defines */
+#define MODE_PAGE_INFO_EXCEP				0x1C
+#define MODE_PAGE_CACHING				0x08
+#define MODE_PAGE_CONTROL				0x0A
+#define MODE_PAGE_POWER_CONDITION			0x1A
+#define MODE_PAGE_RETURN_ALL				0x3F
+#define MODE_PAGE_BLK_DES_LEN				0x08
+#define MODE_PAGE_LLBAA_BLK_DES_LEN			0x10
+#define MODE_PAGE_CACHING_LEN				0x14
+#define MODE_PAGE_CONTROL_LEN				0x0C
+#define MODE_PAGE_POW_CND_LEN				0x28
+#define MODE_PAGE_INF_EXC_LEN				0x0C
+#define MODE_PAGE_ALL_LEN				0x54
+#define MODE_SENSE6_MPH_SIZE				4
+#define MODE_SENSE6_ALLOC_LEN_OFFSET			4
+#define MODE_SENSE_PAGE_CONTROL_OFFSET			2
+#define MODE_SENSE_PAGE_CONTROL_MASK			0xC0
+#define MODE_SENSE_PAGE_CODE_OFFSET			2
+#define MODE_SENSE_PAGE_CODE_MASK			0x3F
+#define MODE_SENSE_LLBAA_OFFSET				1
+#define MODE_SENSE_LLBAA_MASK				0x10
+#define MODE_SENSE_LLBAA_SHIFT				4
+#define MODE_SENSE_DBD_OFFSET				1
+#define MODE_SENSE_DBD_MASK				8
+#define MODE_SENSE_DBD_SHIFT				3
+#define MODE_SENSE10_MPH_SIZE				8
+#define MODE_SENSE10_ALLOC_LEN_OFFSET			7
+#define MODE_SELECT_CDB_PAGE_FORMAT_OFFSET		1
+#define MODE_SELECT_CDB_SAVE_PAGES_OFFSET		1
+#define MODE_SELECT_6_CDB_PARAM_LIST_LENGTH_OFFSET	4
+#define MODE_SELECT_10_CDB_PARAM_LIST_LENGTH_OFFSET	7
+#define MODE_SELECT_CDB_PAGE_FORMAT_MASK		0x10
+#define MODE_SELECT_CDB_SAVE_PAGES_MASK			0x1
+#define MODE_SELECT_6_BD_OFFSET				3
+#define MODE_SELECT_10_BD_OFFSET			6
+#define MODE_SELECT_10_LLBAA_OFFSET			4
+#define MODE_SELECT_10_LLBAA_MASK			1
+#define MODE_SELECT_6_MPH_SIZE				4
+#define MODE_SELECT_10_MPH_SIZE				8
+#define CACHING_MODE_PAGE_WCE_MASK			0x04
+#define MODE_SENSE_BLK_DESC_ENABLED			0
+#define MODE_SENSE_BLK_DESC_COUNT			1
+#define MODE_SELECT_PAGE_CODE_MASK			0x3F
+#define SHORT_DESC_BLOCK				8
+#define LONG_DESC_BLOCK					16
+#define MODE_PAGE_POW_CND_LEN_FIELD			0x26
+#define MODE_PAGE_INF_EXC_LEN_FIELD			0x0A
+#define MODE_PAGE_CACHING_LEN_FIELD			0x12
+#define MODE_PAGE_CONTROL_LEN_FIELD			0x0A
+#define MODE_SENSE_PC_CURRENT_VALUES			0
+
+/* Log Sense defines */
+#define LOG_PAGE_SUPPORTED_LOG_PAGES_PAGE		0x00
+#define LOG_PAGE_SUPPORTED_LOG_PAGES_LENGTH		0x07
+#define LOG_PAGE_INFORMATIONAL_EXCEPTIONS_PAGE		0x2F
+#define LOG_PAGE_TEMPERATURE_PAGE			0x0D
+#define LOG_SENSE_CDB_SP_OFFSET				1
+#define LOG_SENSE_CDB_SP_NOT_ENABLED			0
+#define LOG_SENSE_CDB_PC_OFFSET				2
+#define LOG_SENSE_CDB_PC_MASK				0xC0
+#define LOG_SENSE_CDB_PC_SHIFT				6
+#define LOG_SENSE_CDB_PC_CUMULATIVE_VALUES		1
+#define LOG_SENSE_CDB_PAGE_CODE_MASK			0x3F
+#define LOG_SENSE_CDB_ALLOC_LENGTH_OFFSET		7
+#define REMAINING_INFO_EXCP_PAGE_LENGTH			0x8
+#define LOG_INFO_EXCP_PAGE_LENGTH			0xC
+#define REMAINING_TEMP_PAGE_LENGTH			0xC
+#define LOG_TEMP_PAGE_LENGTH				0x10
+#define LOG_TEMP_UNKNOWN				0xFF
+#define SUPPORTED_LOG_PAGES_PAGE_LENGTH			0x3
+
+/* Read Capacity defines */
+#define READ_CAP_10_RESP_SIZE				8
+#define READ_CAP_16_RESP_SIZE				32
+
+/* NVMe Namespace and Command Defines */
+#define NVME_GET_SMART_LOG_PAGE				0x02
+#define NVME_GET_FEAT_TEMP_THRESH			0x04
+#define BYTES_TO_DWORDS					4
+#define NVME_MAX_FIRMWARE_SLOT				7
+
+/* Report LUNs defines */
+#define REPORT_LUNS_FIRST_LUN_OFFSET			8
+
+/* SCSI ADDITIONAL SENSE Codes */
+
+#define SCSI_ASC_NO_SENSE				0x00
+#define SCSI_ASC_PERIPHERAL_DEV_WRITE_FAULT		0x03
+#define SCSI_ASC_LUN_NOT_READY				0x04
+#define SCSI_ASC_WARNING				0x0B
+#define SCSI_ASC_LOG_BLOCK_GUARD_CHECK_FAILED		0x10
+#define SCSI_ASC_LOG_BLOCK_APPTAG_CHECK_FAILED		0x10
+#define SCSI_ASC_LOG_BLOCK_REFTAG_CHECK_FAILED		0x10
+#define SCSI_ASC_UNRECOVERED_READ_ERROR			0x11
+#define SCSI_ASC_MISCOMPARE_DURING_VERIFY		0x1D
+#define SCSI_ASC_ACCESS_DENIED_INVALID_LUN_ID		0x20
+#define SCSI_ASC_ILLEGAL_COMMAND			0x20
+#define SCSI_ASC_ILLEGAL_BLOCK				0x21
+#define SCSI_ASC_INVALID_CDB				0x24
+#define SCSI_ASC_INVALID_LUN				0x25
+#define SCSI_ASC_INVALID_PARAMETER			0x26
+#define SCSI_ASC_FORMAT_COMMAND_FAILED			0x31
+#define SCSI_ASC_INTERNAL_TARGET_FAILURE		0x44
+
+/* SCSI ADDITIONAL SENSE Code Qualifiers */
+
+#define SCSI_ASCQ_CAUSE_NOT_REPORTABLE			0x00
+#define SCSI_ASCQ_FORMAT_COMMAND_FAILED			0x01
+#define SCSI_ASCQ_LOG_BLOCK_GUARD_CHECK_FAILED		0x01
+#define SCSI_ASCQ_LOG_BLOCK_APPTAG_CHECK_FAILED		0x02
+#define SCSI_ASCQ_LOG_BLOCK_REFTAG_CHECK_FAILED		0x03
+#define SCSI_ASCQ_FORMAT_IN_PROGRESS			0x04
+#define SCSI_ASCQ_POWER_LOSS_EXPECTED			0x08
+#define SCSI_ASCQ_INVALID_LUN_ID			0x09
+
+/**
+ * DEVICE_SPECIFIC_PARAMETER in mode parameter header (see sbc2r16) to
+ * enable DPOFUA support type 0x10 value.
+ */
+#define DEVICE_SPECIFIC_PARAMETER			0
+#define VPD_ID_DESCRIPTOR_LENGTH sizeof(VPD_IDENTIFICATION_DESCRIPTOR)
+
+/* MACROs to extract information from CDBs */
+
+#define GET_OPCODE(cdb)		cdb[0]
+
+#define GET_U8_FROM_CDB(cdb, index) (cdb[index] << 0)
+
+#define GET_U16_FROM_CDB(cdb, index) ((cdb[index] << 8) | (cdb[index + 1] << 0))
+
+#define GET_U24_FROM_CDB(cdb, index) ((cdb[index] << 16) | \
+(cdb[index + 1] <<  8) | \
+(cdb[index + 2] <<  0))
+
+#define GET_U32_FROM_CDB(cdb, index) ((cdb[index] << 24) | \
+(cdb[index + 1] << 16) | \
+(cdb[index + 2] <<  8) | \
+(cdb[index + 3] <<  0))
+
+#define GET_U64_FROM_CDB(cdb, index) ((((u64)cdb[index]) << 56) | \
+(((u64)cdb[index + 1]) << 48) | \
+(((u64)cdb[index + 2]) << 40) | \
+(((u64)cdb[index + 3]) << 32) | \
+(((u64)cdb[index + 4]) << 24) | \
+(((u64)cdb[index + 5]) << 16) | \
+(((u64)cdb[index + 6]) <<  8) | \
+(((u64)cdb[index + 7]) <<  0))
+
+/* Inquiry Helper Macros */
+#define GET_INQ_EVPD_BIT(cdb) \
+((GET_U8_FROM_CDB(cdb, INQUIRY_EVPD_BYTE_OFFSET) &		\
+INQUIRY_EVPD_BIT_MASK) ? 1 : 0)
+
+#define GET_INQ_PAGE_CODE(cdb)					\
+(GET_U8_FROM_CDB(cdb, INQUIRY_PAGE_CODE_BYTE_OFFSET))
+
+#define GET_INQ_ALLOC_LENGTH(cdb)				\
+(GET_U16_FROM_CDB(cdb, INQUIRY_CDB_ALLOCATION_LENGTH_OFFSET))
+
+/* Report LUNs Helper Macros */
+#define GET_REPORT_LUNS_ALLOC_LENGTH(cdb)			\
+(GET_U32_FROM_CDB(cdb, REPORT_LUNS_CDB_ALLOC_LENGTH_OFFSET))
+
+/* Read Capacity Helper Macros */
+#define GET_READ_CAP_16_ALLOC_LENGTH(cdb)			\
+(GET_U32_FROM_CDB(cdb, READ_CAP_16_CDB_ALLOC_LENGTH_OFFSET))
+
+#define IS_READ_CAP_16(cdb)					\
+((cdb[0] == SERVICE_ACTION_IN && cdb[1] == SAI_READ_CAPACITY_16) ? 1 : 0)
+
+/* Request Sense Helper Macros */
+#define GET_REQUEST_SENSE_ALLOC_LENGTH(cdb)			\
+(GET_U8_FROM_CDB(cdb, REQUEST_SENSE_CDB_ALLOC_LENGTH_OFFSET))
+
+/* Mode Sense Helper Macros */
+#define GET_MODE_SENSE_DBD(cdb)					\
+((GET_U8_FROM_CDB(cdb, MODE_SENSE_DBD_OFFSET) & MODE_SENSE_DBD_MASK) >>	\
+MODE_SENSE_DBD_SHIFT)
+
+#define GET_MODE_SENSE_LLBAA(cdb)				\
+((GET_U8_FROM_CDB(cdb, MODE_SENSE_LLBAA_OFFSET) &		\
+MODE_SENSE_LLBAA_MASK) >> MODE_SENSE_LLBAA_SHIFT)
+
+#define GET_MODE_SENSE_MPH_SIZE(cdb10)				\
+(cdb10 ? MODE_SENSE10_MPH_SIZE : MODE_SENSE6_MPH_SIZE)
+
+
+/* Struct to gather data that needs to be extracted from a SCSI CDB.
+   Not conforming to any particular CDB variant, but compatible with all. */
+
+struct nvme_trans_io_cdb {
+	u8 fua;
+	u8 prot_info;
+	u64 lba;
+	u32 xfer_len;
+};
+
+
+/* Internal Helper Functions */
+
+
+/* Copy data to userspace memory */
+
+static int nvme_trans_copy_to_user(struct sg_io_hdr *hdr, void *from,
+								unsigned long n)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	unsigned long not_copied;
+	int i;
+	void *index = from;
+	size_t remaining = n;
+	size_t xfer_len;
+
+	if (hdr->iovec_count > 0) {
+		struct sg_iovec sgl;
+
+		for (i = 0; i < hdr->iovec_count; i++) {
+			not_copied = copy_from_user(&sgl, hdr->dxferp +
+						i * sizeof(struct sg_iovec),
+						sizeof(struct sg_iovec));
+			if (not_copied)
+				return -EFAULT;
+			xfer_len = min(remaining, sgl.iov_len);
+			not_copied = copy_to_user(sgl.iov_base, index,
+								xfer_len);
+			if (not_copied) {
+				res = -EFAULT;
+				break;
+			}
+			index += xfer_len;
+			remaining -= xfer_len;
+			if (remaining == 0)
+				break;
+		}
+		return res;
+	}
+	not_copied = copy_to_user(hdr->dxferp, from, n);
+	if (not_copied)
+		res = -EFAULT;
+	return res;
+}
+
+/* Copy data from userspace memory */
+
+static int nvme_trans_copy_from_user(struct sg_io_hdr *hdr, void *to,
+								unsigned long n)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	unsigned long not_copied;
+	int i;
+	void *index = to;
+	size_t remaining = n;
+	size_t xfer_len;
+
+	if (hdr->iovec_count > 0) {
+		struct sg_iovec sgl;
+
+		for (i = 0; i < hdr->iovec_count; i++) {
+			not_copied = copy_from_user(&sgl, hdr->dxferp +
+						i * sizeof(struct sg_iovec),
+						sizeof(struct sg_iovec));
+			if (not_copied)
+				return -EFAULT;
+			xfer_len = min(remaining, sgl.iov_len);
+			not_copied = copy_from_user(index, sgl.iov_base,
+								xfer_len);
+			if (not_copied) {
+				res = -EFAULT;
+				break;
+			}
+			index += xfer_len;
+			remaining -= xfer_len;
+			if (remaining == 0)
+				break;
+		}
+		return res;
+	}
+
+	not_copied = copy_from_user(to, hdr->dxferp, n);
+	if (not_copied)
+		res = -EFAULT;
+	return res;
+}
+
+/* Status/Sense Buffer Writeback */
+
+static int nvme_trans_completion(struct sg_io_hdr *hdr, u8 status, u8 sense_key,
+				 u8 asc, u8 ascq)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	u8 xfer_len;
+	u8 resp[DESC_FMT_SENSE_DATA_SIZE];
+
+	if (scsi_status_is_good(status)) {
+		hdr->status = SAM_STAT_GOOD;
+		hdr->masked_status = GOOD;
+		hdr->host_status = DID_OK;
+		hdr->driver_status = DRIVER_OK;
+		hdr->sb_len_wr = 0;
+	} else {
+		hdr->status = status;
+		hdr->masked_status = status >> 1;
+		hdr->host_status = DID_OK;
+		hdr->driver_status = DRIVER_OK;
+
+		memset(resp, 0, DESC_FMT_SENSE_DATA_SIZE);
+		resp[0] = DESC_FORMAT_SENSE_DATA;
+		resp[1] = sense_key;
+		resp[2] = asc;
+		resp[3] = ascq;
+
+		xfer_len = min_t(u8, hdr->mx_sb_len, DESC_FMT_SENSE_DATA_SIZE);
+		hdr->sb_len_wr = xfer_len;
+		if (copy_to_user(hdr->sbp, resp, xfer_len) > 0)
+			res = -EFAULT;
+	}
+
+	return res;
+}
+
+static int nvme_trans_status_code(struct sg_io_hdr *hdr, int nvme_sc)
+{
+	u8 status, sense_key, asc, ascq;
+	int res = SNTI_TRANSLATION_SUCCESS;
+
+	/* For non-nvme (Linux) errors, simply return the error code */
+	if (nvme_sc < 0)
+		return nvme_sc;
+
+	/* Mask DNR, More, and reserved fields */
+	nvme_sc &= 0x7FF;
+
+	switch (nvme_sc) {
+	/* Generic Command Status */
+	case NVME_SC_SUCCESS:
+		status = SAM_STAT_GOOD;
+		sense_key = NO_SENSE;
+		asc = SCSI_ASC_NO_SENSE;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+	case NVME_SC_INVALID_OPCODE:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = ILLEGAL_REQUEST;
+		asc = SCSI_ASC_ILLEGAL_COMMAND;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+	case NVME_SC_INVALID_FIELD:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = ILLEGAL_REQUEST;
+		asc = SCSI_ASC_INVALID_CDB;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+	case NVME_SC_DATA_XFER_ERROR:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = MEDIUM_ERROR;
+		asc = SCSI_ASC_NO_SENSE;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+	case NVME_SC_POWER_LOSS:
+		status = SAM_STAT_TASK_ABORTED;
+		sense_key = ABORTED_COMMAND;
+		asc = SCSI_ASC_WARNING;
+		ascq = SCSI_ASCQ_POWER_LOSS_EXPECTED;
+		break;
+	case NVME_SC_INTERNAL:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = HARDWARE_ERROR;
+		asc = SCSI_ASC_INTERNAL_TARGET_FAILURE;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+	case NVME_SC_ABORT_REQ:
+		status = SAM_STAT_TASK_ABORTED;
+		sense_key = ABORTED_COMMAND;
+		asc = SCSI_ASC_NO_SENSE;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+	case NVME_SC_ABORT_QUEUE:
+		status = SAM_STAT_TASK_ABORTED;
+		sense_key = ABORTED_COMMAND;
+		asc = SCSI_ASC_NO_SENSE;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+	case NVME_SC_FUSED_FAIL:
+		status = SAM_STAT_TASK_ABORTED;
+		sense_key = ABORTED_COMMAND;
+		asc = SCSI_ASC_NO_SENSE;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+	case NVME_SC_FUSED_MISSING:
+		status = SAM_STAT_TASK_ABORTED;
+		sense_key = ABORTED_COMMAND;
+		asc = SCSI_ASC_NO_SENSE;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+	case NVME_SC_INVALID_NS:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = ILLEGAL_REQUEST;
+		asc = SCSI_ASC_ACCESS_DENIED_INVALID_LUN_ID;
+		ascq = SCSI_ASCQ_INVALID_LUN_ID;
+		break;
+	case NVME_SC_LBA_RANGE:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = ILLEGAL_REQUEST;
+		asc = SCSI_ASC_ILLEGAL_BLOCK;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+	case NVME_SC_CAP_EXCEEDED:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = MEDIUM_ERROR;
+		asc = SCSI_ASC_NO_SENSE;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+	case NVME_SC_NS_NOT_READY:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = NOT_READY;
+		asc = SCSI_ASC_LUN_NOT_READY;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+
+	/* Command Specific Status */
+	case NVME_SC_INVALID_FORMAT:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = ILLEGAL_REQUEST;
+		asc = SCSI_ASC_FORMAT_COMMAND_FAILED;
+		ascq = SCSI_ASCQ_FORMAT_COMMAND_FAILED;
+		break;
+	case NVME_SC_BAD_ATTRIBUTES:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = ILLEGAL_REQUEST;
+		asc = SCSI_ASC_INVALID_CDB;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+
+	/* Media Errors */
+	case NVME_SC_WRITE_FAULT:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = MEDIUM_ERROR;
+		asc = SCSI_ASC_PERIPHERAL_DEV_WRITE_FAULT;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+	case NVME_SC_READ_ERROR:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = MEDIUM_ERROR;
+		asc = SCSI_ASC_UNRECOVERED_READ_ERROR;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+	case NVME_SC_GUARD_CHECK:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = MEDIUM_ERROR;
+		asc = SCSI_ASC_LOG_BLOCK_GUARD_CHECK_FAILED;
+		ascq = SCSI_ASCQ_LOG_BLOCK_GUARD_CHECK_FAILED;
+		break;
+	case NVME_SC_APPTAG_CHECK:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = MEDIUM_ERROR;
+		asc = SCSI_ASC_LOG_BLOCK_APPTAG_CHECK_FAILED;
+		ascq = SCSI_ASCQ_LOG_BLOCK_APPTAG_CHECK_FAILED;
+		break;
+	case NVME_SC_REFTAG_CHECK:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = MEDIUM_ERROR;
+		asc = SCSI_ASC_LOG_BLOCK_REFTAG_CHECK_FAILED;
+		ascq = SCSI_ASCQ_LOG_BLOCK_REFTAG_CHECK_FAILED;
+		break;
+	case NVME_SC_COMPARE_FAILED:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = MISCOMPARE;
+		asc = SCSI_ASC_MISCOMPARE_DURING_VERIFY;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+	case NVME_SC_ACCESS_DENIED:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = ILLEGAL_REQUEST;
+		asc = SCSI_ASC_ACCESS_DENIED_INVALID_LUN_ID;
+		ascq = SCSI_ASCQ_INVALID_LUN_ID;
+		break;
+
+	/* Unspecified/Default */
+	case NVME_SC_CMDID_CONFLICT:
+	case NVME_SC_CMD_SEQ_ERROR:
+	case NVME_SC_CQ_INVALID:
+	case NVME_SC_QID_INVALID:
+	case NVME_SC_QUEUE_SIZE:
+	case NVME_SC_ABORT_LIMIT:
+	case NVME_SC_ABORT_MISSING:
+	case NVME_SC_ASYNC_LIMIT:
+	case NVME_SC_FIRMWARE_SLOT:
+	case NVME_SC_FIRMWARE_IMAGE:
+	case NVME_SC_INVALID_VECTOR:
+	case NVME_SC_INVALID_LOG_PAGE:
+	default:
+		status = SAM_STAT_CHECK_CONDITION;
+		sense_key = ILLEGAL_REQUEST;
+		asc = SCSI_ASC_NO_SENSE;
+		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		break;
+	}
+
+	res = nvme_trans_completion(hdr, status, sense_key, asc, ascq);
+
+	return res;
+}
+
+/* INQUIRY Helper Functions */
+
+static int nvme_trans_standard_inquiry_page(struct nvme_ns *ns,
+					struct sg_io_hdr *hdr, u8 *inq_response,
+					int alloc_len)
+{
+	struct nvme_dev *dev = ns->dev;
+	dma_addr_t dma_addr;
+	void *mem;
+	struct nvme_id_ns *id_ns;
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int nvme_sc;
+	int xfer_len;
+	u8 resp_data_format = 0x02;
+	u8 protect;
+	u8 cmdque = 0x01 << 1;
+
+	mem = dma_alloc_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns),
+				&dma_addr, GFP_KERNEL);
+	if (mem == NULL) {
+		res = -ENOMEM;
+		goto out_dma;
+	}
+
+	/* nvme ns identify - use DPS value for PROTECT field */
+	nvme_sc = nvme_identify(dev, ns->ns_id, 0, dma_addr);
+	res = nvme_trans_status_code(hdr, nvme_sc);
+	/*
+	 * If nvme_sc was -ve, res will be -ve here.
+	 * If nvme_sc was +ve, the status would bace been translated, and res
+	 *  can only be 0 or -ve.
+	 *    - If 0 && nvme_sc > 0, then go into next if where res gets nvme_sc
+	 *    - If -ve, return because its a Linux error.
+	 */
+	if (res)
+		goto out_free;
+	if (nvme_sc) {
+		res = nvme_sc;
+		goto out_free;
+	}
+	id_ns = mem;
+	(id_ns->dps) ? (protect = 0x01) : (protect = 0);
+
+	memset(inq_response, 0, STANDARD_INQUIRY_LENGTH);
+	inq_response[2] = VERSION_SPC_4;
+	inq_response[3] = resp_data_format;	/*normaca=0 | hisup=0 */
+	inq_response[4] = ADDITIONAL_STD_INQ_LENGTH;
+	inq_response[5] = protect;	/* sccs=0 | acc=0 | tpgs=0 | pc3=0 */
+	inq_response[7] = cmdque;	/* wbus16=0 | sync=0 | vs=0 */
+	strncpy(&inq_response[8], "NVMe    ", 8);
+	strncpy(&inq_response[16], dev->model, 16);
+	strncpy(&inq_response[32], dev->firmware_rev, 4);
+
+	xfer_len = min(alloc_len, STANDARD_INQUIRY_LENGTH);
+	res = nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
+
+ out_free:
+	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns), mem,
+			  dma_addr);
+ out_dma:
+	return res;
+}
+
+static int nvme_trans_supported_vpd_pages(struct nvme_ns *ns,
+					struct sg_io_hdr *hdr, u8 *inq_response,
+					int alloc_len)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int xfer_len;
+
+	memset(inq_response, 0, STANDARD_INQUIRY_LENGTH);
+	inq_response[1] = INQ_SUPPORTED_VPD_PAGES_PAGE;   /* Page Code */
+	inq_response[3] = INQ_NUM_SUPPORTED_VPD_PAGES;    /* Page Length */
+	inq_response[4] = INQ_SUPPORTED_VPD_PAGES_PAGE;
+	inq_response[5] = INQ_UNIT_SERIAL_NUMBER_PAGE;
+	inq_response[6] = INQ_DEVICE_IDENTIFICATION_PAGE;
+	inq_response[7] = INQ_EXTENDED_INQUIRY_DATA_PAGE;
+	inq_response[8] = INQ_BDEV_CHARACTERISTICS_PAGE;
+
+	xfer_len = min(alloc_len, STANDARD_INQUIRY_LENGTH);
+	res = nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
+
+	return res;
+}
+
+static int nvme_trans_unit_serial_page(struct nvme_ns *ns,
+					struct sg_io_hdr *hdr, u8 *inq_response,
+					int alloc_len)
+{
+	struct nvme_dev *dev = ns->dev;
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int xfer_len;
+
+	memset(inq_response, 0, STANDARD_INQUIRY_LENGTH);
+	inq_response[1] = INQ_UNIT_SERIAL_NUMBER_PAGE; /* Page Code */
+	inq_response[3] = INQ_SERIAL_NUMBER_LENGTH;    /* Page Length */
+	strncpy(&inq_response[4], dev->serial, INQ_SERIAL_NUMBER_LENGTH);
+
+	xfer_len = min(alloc_len, STANDARD_INQUIRY_LENGTH);
+	res = nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
+
+	return res;
+}
+
+static int nvme_trans_device_id_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+					u8 *inq_response, int alloc_len)
+{
+	struct nvme_dev *dev = ns->dev;
+	dma_addr_t dma_addr;
+	void *mem;
+	struct nvme_id_ctrl *id_ctrl;
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int nvme_sc;
+	u8 ieee[4];
+	int xfer_len;
+	__be32 tmp_id = cpu_to_be32(ns->ns_id);
+
+	mem = dma_alloc_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns),
+					&dma_addr, GFP_KERNEL);
+	if (mem == NULL) {
+		res = -ENOMEM;
+		goto out_dma;
+	}
+
+	/* nvme controller identify */
+	nvme_sc = nvme_identify(dev, 0, 1, dma_addr);
+	res = nvme_trans_status_code(hdr, nvme_sc);
+	if (res)
+		goto out_free;
+	if (nvme_sc) {
+		res = nvme_sc;
+		goto out_free;
+	}
+	id_ctrl = mem;
+
+	/* Since SCSI tried to save 4 bits... [SPC-4(r34) Table 591] */
+	ieee[0] = id_ctrl->ieee[0] << 4;
+	ieee[1] = id_ctrl->ieee[0] >> 4 | id_ctrl->ieee[1] << 4;
+	ieee[2] = id_ctrl->ieee[1] >> 4 | id_ctrl->ieee[2] << 4;
+	ieee[3] = id_ctrl->ieee[2] >> 4;
+
+	memset(inq_response, 0, STANDARD_INQUIRY_LENGTH);
+	inq_response[1] = INQ_DEVICE_IDENTIFICATION_PAGE;    /* Page Code */
+	inq_response[3] = 20;      /* Page Length */
+	/* Designation Descriptor start */
+	inq_response[4] = 0x01;    /* Proto ID=0h | Code set=1h */
+	inq_response[5] = 0x03;    /* PIV=0b | Asso=00b | Designator Type=3h */
+	inq_response[6] = 0x00;    /* Rsvd */
+	inq_response[7] = 16;      /* Designator Length */
+	/* Designator start */
+	inq_response[8] = 0x60 | ieee[3]; /* NAA=6h | IEEE ID MSB, High nibble*/
+	inq_response[9] = ieee[2];        /* IEEE ID */
+	inq_response[10] = ieee[1];       /* IEEE ID */
+	inq_response[11] = ieee[0];       /* IEEE ID| Vendor Specific ID... */
+	inq_response[12] = (dev->pci_dev->vendor & 0xFF00) >> 8;
+	inq_response[13] = (dev->pci_dev->vendor & 0x00FF);
+	inq_response[14] = dev->serial[0];
+	inq_response[15] = dev->serial[1];
+	inq_response[16] = dev->model[0];
+	inq_response[17] = dev->model[1];
+	memcpy(&inq_response[18], &tmp_id, sizeof(u32));
+	/* Last 2 bytes are zero */
+
+	xfer_len = min(alloc_len, STANDARD_INQUIRY_LENGTH);
+	res = nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
+
+ out_free:
+	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns), mem,
+			  dma_addr);
+ out_dma:
+	return res;
+}
+
+static int nvme_trans_ext_inq_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+					int alloc_len)
+{
+	u8 *inq_response;
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int nvme_sc;
+	struct nvme_dev *dev = ns->dev;
+	dma_addr_t dma_addr;
+	void *mem;
+	struct nvme_id_ctrl *id_ctrl;
+	struct nvme_id_ns *id_ns;
+	int xfer_len;
+	u8 microcode = 0x80;
+	u8 spt;
+	u8 spt_lut[8] = {0, 0, 2, 1, 4, 6, 5, 7};
+	u8 grd_chk, app_chk, ref_chk, protect;
+	u8 uask_sup = 0x20;
+	u8 v_sup;
+	u8 luiclr = 0x01;
+
+	inq_response = kmalloc(EXTENDED_INQUIRY_DATA_PAGE_LENGTH, GFP_KERNEL);
+	if (inq_response == NULL) {
+		res = -ENOMEM;
+		goto out_mem;
+	}
+
+	mem = dma_alloc_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns),
+							&dma_addr, GFP_KERNEL);
+	if (mem == NULL) {
+		res = -ENOMEM;
+		goto out_dma;
+	}
+
+	/* nvme ns identify */
+	nvme_sc = nvme_identify(dev, ns->ns_id, 0, dma_addr);
+	res = nvme_trans_status_code(hdr, nvme_sc);
+	if (res)
+		goto out_free;
+	if (nvme_sc) {
+		res = nvme_sc;
+		goto out_free;
+	}
+	id_ns = mem;
+	spt = spt_lut[(id_ns->dpc) & 0x07] << 3;
+	(id_ns->dps) ? (protect = 0x01) : (protect = 0);
+	grd_chk = protect << 2;
+	app_chk = protect << 1;
+	ref_chk = protect;
+
+	/* nvme controller identify */
+	nvme_sc = nvme_identify(dev, 0, 1, dma_addr);
+	res = nvme_trans_status_code(hdr, nvme_sc);
+	if (res)
+		goto out_free;
+	if (nvme_sc) {
+		res = nvme_sc;
+		goto out_free;
+	}
+	id_ctrl = mem;
+	v_sup = id_ctrl->vwc;
+
+	memset(inq_response, 0, EXTENDED_INQUIRY_DATA_PAGE_LENGTH);
+	inq_response[1] = INQ_EXTENDED_INQUIRY_DATA_PAGE;    /* Page Code */
+	inq_response[2] = 0x00;    /* Page Length MSB */
+	inq_response[3] = 0x3C;    /* Page Length LSB */
+	inq_response[4] = microcode | spt | grd_chk | app_chk | ref_chk;
+	inq_response[5] = uask_sup;
+	inq_response[6] = v_sup;
+	inq_response[7] = luiclr;
+	inq_response[8] = 0;
+	inq_response[9] = 0;
+
+	xfer_len = min(alloc_len, EXTENDED_INQUIRY_DATA_PAGE_LENGTH);
+	res = nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
+
+ out_free:
+	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns), mem,
+			  dma_addr);
+ out_dma:
+	kfree(inq_response);
+ out_mem:
+	return res;
+}
+
+static int nvme_trans_bdev_char_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+					int alloc_len)
+{
+	u8 *inq_response;
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int xfer_len;
+
+	inq_response = kmalloc(EXTENDED_INQUIRY_DATA_PAGE_LENGTH, GFP_KERNEL);
+	if (inq_response == NULL) {
+		res = -ENOMEM;
+		goto out_mem;
+	}
+
+	memset(inq_response, 0, EXTENDED_INQUIRY_DATA_PAGE_LENGTH);
+	inq_response[1] = INQ_BDEV_CHARACTERISTICS_PAGE;    /* Page Code */
+	inq_response[2] = 0x00;    /* Page Length MSB */
+	inq_response[3] = 0x3C;    /* Page Length LSB */
+	inq_response[4] = 0x00;    /* Medium Rotation Rate MSB */
+	inq_response[5] = 0x01;    /* Medium Rotation Rate LSB */
+	inq_response[6] = 0x00;    /* Form Factor */
+
+	xfer_len = min(alloc_len, EXTENDED_INQUIRY_DATA_PAGE_LENGTH);
+	res = nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
+
+	kfree(inq_response);
+ out_mem:
+	return res;
+}
+
+/* LOG SENSE Helper Functions */
+
+static int nvme_trans_log_supp_pages(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+					int alloc_len)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int xfer_len;
+	u8 *log_response;
+
+	log_response = kmalloc(LOG_PAGE_SUPPORTED_LOG_PAGES_LENGTH, GFP_KERNEL);
+	if (log_response == NULL) {
+		res = -ENOMEM;
+		goto out_mem;
+	}
+	memset(log_response, 0, LOG_PAGE_SUPPORTED_LOG_PAGES_LENGTH);
+
+	log_response[0] = LOG_PAGE_SUPPORTED_LOG_PAGES_PAGE;
+	/* Subpage=0x00, Page Length MSB=0 */
+	log_response[3] = SUPPORTED_LOG_PAGES_PAGE_LENGTH;
+	log_response[4] = LOG_PAGE_SUPPORTED_LOG_PAGES_PAGE;
+	log_response[5] = LOG_PAGE_INFORMATIONAL_EXCEPTIONS_PAGE;
+	log_response[6] = LOG_PAGE_TEMPERATURE_PAGE;
+
+	xfer_len = min(alloc_len, LOG_PAGE_SUPPORTED_LOG_PAGES_LENGTH);
+	res = nvme_trans_copy_to_user(hdr, log_response, xfer_len);
+
+	kfree(log_response);
+ out_mem:
+	return res;
+}
+
+static int nvme_trans_log_info_exceptions(struct nvme_ns *ns,
+					struct sg_io_hdr *hdr, int alloc_len)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int xfer_len;
+	u8 *log_response;
+	struct nvme_command c;
+	struct nvme_dev *dev = ns->dev;
+	struct nvme_smart_log *smart_log;
+	dma_addr_t dma_addr;
+	void *mem;
+	u8 temp_c;
+	u16 temp_k;
+
+	log_response = kmalloc(LOG_INFO_EXCP_PAGE_LENGTH, GFP_KERNEL);
+	if (log_response == NULL) {
+		res = -ENOMEM;
+		goto out_mem;
+	}
+	memset(log_response, 0, LOG_INFO_EXCP_PAGE_LENGTH);
+
+	mem = dma_alloc_coherent(&dev->pci_dev->dev,
+					sizeof(struct nvme_smart_log),
+					&dma_addr, GFP_KERNEL);
+	if (mem == NULL) {
+		res = -ENOMEM;
+		goto out_dma;
+	}
+
+	/* Get SMART Log Page */
+	memset(&c, 0, sizeof(c));
+	c.common.opcode = nvme_admin_get_log_page;
+	c.common.nsid = cpu_to_le32(0xFFFFFFFF);
+	c.common.prp1 = cpu_to_le64(dma_addr);
+	c.common.cdw10[0] = cpu_to_le32(((sizeof(struct nvme_smart_log) /
+			BYTES_TO_DWORDS) << 16) | NVME_GET_SMART_LOG_PAGE);
+	res = nvme_submit_admin_cmd(dev, &c, NULL);
+	if (res != NVME_SC_SUCCESS) {
+		temp_c = LOG_TEMP_UNKNOWN;
+	} else {
+		smart_log = mem;
+		temp_k = (smart_log->temperature[1] << 8) +
+				(smart_log->temperature[0]);
+		temp_c = temp_k - KELVIN_TEMP_FACTOR;
+	}
+
+	log_response[0] = LOG_PAGE_INFORMATIONAL_EXCEPTIONS_PAGE;
+	/* Subpage=0x00, Page Length MSB=0 */
+	log_response[3] = REMAINING_INFO_EXCP_PAGE_LENGTH;
+	/* Informational Exceptions Log Parameter 1 Start */
+	/* Parameter Code=0x0000 bytes 4,5 */
+	log_response[6] = 0x23; /* DU=0, TSD=1, ETC=0, TMC=0, FMT_AND_LNK=11b */
+	log_response[7] = 0x04; /* PARAMETER LENGTH */
+	/* Add sense Code and qualifier = 0x00 each */
+	/* Use Temperature from NVMe Get Log Page, convert to C from K */
+	log_response[10] = temp_c;
+
+	xfer_len = min(alloc_len, LOG_INFO_EXCP_PAGE_LENGTH);
+	res = nvme_trans_copy_to_user(hdr, log_response, xfer_len);
+
+	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_smart_log),
+			  mem, dma_addr);
+ out_dma:
+	kfree(log_response);
+ out_mem:
+	return res;
+}
+
+static int nvme_trans_log_temperature(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+					int alloc_len)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int xfer_len;
+	u8 *log_response;
+	struct nvme_command c;
+	struct nvme_dev *dev = ns->dev;
+	struct nvme_smart_log *smart_log;
+	dma_addr_t dma_addr;
+	void *mem;
+	u32 feature_resp;
+	u8 temp_c_cur, temp_c_thresh;
+	u16 temp_k;
+
+	log_response = kmalloc(LOG_TEMP_PAGE_LENGTH, GFP_KERNEL);
+	if (log_response == NULL) {
+		res = -ENOMEM;
+		goto out_mem;
+	}
+	memset(log_response, 0, LOG_TEMP_PAGE_LENGTH);
+
+	mem = dma_alloc_coherent(&dev->pci_dev->dev,
+					sizeof(struct nvme_smart_log),
+					&dma_addr, GFP_KERNEL);
+	if (mem == NULL) {
+		res = -ENOMEM;
+		goto out_dma;
+	}
+
+	/* Get SMART Log Page */
+	memset(&c, 0, sizeof(c));
+	c.common.opcode = nvme_admin_get_log_page;
+	c.common.nsid = cpu_to_le32(0xFFFFFFFF);
+	c.common.prp1 = cpu_to_le64(dma_addr);
+	c.common.cdw10[0] = cpu_to_le32(((sizeof(struct nvme_smart_log) /
+			BYTES_TO_DWORDS) << 16) | NVME_GET_SMART_LOG_PAGE);
+	res = nvme_submit_admin_cmd(dev, &c, NULL);
+	if (res != NVME_SC_SUCCESS) {
+		temp_c_cur = LOG_TEMP_UNKNOWN;
+	} else {
+		smart_log = mem;
+		temp_k = (smart_log->temperature[1] << 8) +
+				(smart_log->temperature[0]);
+		temp_c_cur = temp_k - KELVIN_TEMP_FACTOR;
+	}
+
+	/* Get Features for Temp Threshold */
+	res = nvme_get_features(dev, NVME_FEAT_TEMP_THRESH, 0, 0,
+								&feature_resp);
+	if (res != NVME_SC_SUCCESS)
+		temp_c_thresh = LOG_TEMP_UNKNOWN;
+	else
+		temp_c_thresh = (feature_resp & 0xFFFF) - KELVIN_TEMP_FACTOR;
+
+	log_response[0] = LOG_PAGE_TEMPERATURE_PAGE;
+	/* Subpage=0x00, Page Length MSB=0 */
+	log_response[3] = REMAINING_TEMP_PAGE_LENGTH;
+	/* Temperature Log Parameter 1 (Temperature) Start */
+	/* Parameter Code = 0x0000 */
+	log_response[6] = 0x01;		/* Format and Linking = 01b */
+	log_response[7] = 0x02;		/* Parameter Length */
+	/* Use Temperature from NVMe Get Log Page, convert to C from K */
+	log_response[9] = temp_c_cur;
+	/* Temperature Log Parameter 2 (Reference Temperature) Start */
+	log_response[11] = 0x01;	/* Parameter Code = 0x0001 */
+	log_response[12] = 0x01;	/* Format and Linking = 01b */
+	log_response[13] = 0x02;	/* Parameter Length */
+	/* Use Temperature Thresh from NVMe Get Log Page, convert to C from K */
+	log_response[15] = temp_c_thresh;
+
+	xfer_len = min(alloc_len, LOG_TEMP_PAGE_LENGTH);
+	res = nvme_trans_copy_to_user(hdr, log_response, xfer_len);
+
+	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_smart_log),
+			  mem, dma_addr);
+ out_dma:
+	kfree(log_response);
+ out_mem:
+	return res;
+}
+
+/* MODE SENSE Helper Functions */
+
+static int nvme_trans_fill_mode_parm_hdr(u8 *resp, int len, u8 cdb10, u8 llbaa,
+					u16 mode_data_length, u16 blk_desc_len)
+{
+	/* Quick check to make sure I don't stomp on my own memory... */
+	if ((cdb10 && len < 8) || (!cdb10 && len < 4))
+		return SNTI_INTERNAL_ERROR;
+
+	if (cdb10) {
+		resp[0] = (mode_data_length & 0xFF00) >> 8;
+		resp[1] = (mode_data_length & 0x00FF);
+		/* resp[2] and [3] are zero */
+		resp[4] = llbaa;
+		resp[5] = RESERVED_FIELD;
+		resp[6] = (blk_desc_len & 0xFF00) >> 8;
+		resp[7] = (blk_desc_len & 0x00FF);
+	} else {
+		resp[0] = (mode_data_length & 0x00FF);
+		/* resp[1] and [2] are zero */
+		resp[3] = (blk_desc_len & 0x00FF);
+	}
+
+	return SNTI_TRANSLATION_SUCCESS;
+}
+
+static int nvme_trans_fill_blk_desc(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+				    u8 *resp, int len, u8 llbaa)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int nvme_sc;
+	struct nvme_dev *dev = ns->dev;
+	dma_addr_t dma_addr;
+	void *mem;
+	struct nvme_id_ns *id_ns;
+	u8 flbas;
+	u32 lba_length;
+
+	if (llbaa == 0 && len < MODE_PAGE_BLK_DES_LEN)
+		return SNTI_INTERNAL_ERROR;
+	else if (llbaa > 0 && len < MODE_PAGE_LLBAA_BLK_DES_LEN)
+		return SNTI_INTERNAL_ERROR;
+
+	mem = dma_alloc_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns),
+							&dma_addr, GFP_KERNEL);
+	if (mem == NULL) {
+		res = -ENOMEM;
+		goto out;
+	}
+
+	/* nvme ns identify */
+	nvme_sc = nvme_identify(dev, ns->ns_id, 0, dma_addr);
+	res = nvme_trans_status_code(hdr, nvme_sc);
+	if (res)
+		goto out_dma;
+	if (nvme_sc) {
+		res = nvme_sc;
+		goto out_dma;
+	}
+	id_ns = mem;
+	flbas = (id_ns->flbas) & 0x0F;
+	lba_length = (1 << (id_ns->lbaf[flbas].ds));
+
+	if (llbaa == 0) {
+		__be32 tmp_cap = cpu_to_be32(le64_to_cpu(id_ns->ncap));
+		/* Byte 4 is reserved */
+		__be32 tmp_len = cpu_to_be32(lba_length & 0x00FFFFFF);
+
+		memcpy(resp, &tmp_cap, sizeof(u32));
+		memcpy(&resp[4], &tmp_len, sizeof(u32));
+	} else {
+		__be64 tmp_cap = cpu_to_be64(le64_to_cpu(id_ns->ncap));
+		__be32 tmp_len = cpu_to_be32(lba_length);
+
+		memcpy(resp, &tmp_cap, sizeof(u64));
+		/* Bytes 8, 9, 10, 11 are reserved */
+		memcpy(&resp[12], &tmp_len, sizeof(u32));
+	}
+
+ out_dma:
+	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns), mem,
+			  dma_addr);
+ out:
+	return res;
+}
+
+static int nvme_trans_fill_control_page(struct nvme_ns *ns,
+					struct sg_io_hdr *hdr, u8 *resp,
+					int len)
+{
+	if (len < MODE_PAGE_CONTROL_LEN)
+		return SNTI_INTERNAL_ERROR;
+
+	resp[0] = MODE_PAGE_CONTROL;
+	resp[1] = MODE_PAGE_CONTROL_LEN_FIELD;
+	resp[2] = 0x0E;		/* TST=000b, TMF_ONLY=0, DPICZ=1,
+				 * D_SENSE=1, GLTSD=1, RLEC=0 */
+	resp[3] = 0x12;		/* Q_ALGO_MODIFIER=1h, NUAR=0, QERR=01b */
+	/* Byte 4:  VS=0, RAC=0, UA_INT=0, SWP=0 */
+	resp[5] = 0x40;		/* ATO=0, TAS=1, ATMPE=0, RWWP=0, AUTOLOAD=0 */
+	/* resp[6] and [7] are obsolete, thus zero */
+	resp[8] = 0xFF;		/* Busy timeout period = 0xffff */
+	resp[9] = 0xFF;
+	/* Bytes 10,11: Extended selftest completion time = 0x0000 */
+
+	return SNTI_TRANSLATION_SUCCESS;
+}
+
+static int nvme_trans_fill_caching_page(struct nvme_ns *ns,
+					struct sg_io_hdr *hdr,
+					u8 *resp, int len)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int nvme_sc;
+	struct nvme_dev *dev = ns->dev;
+	u32 feature_resp;
+	u8 vwc;
+
+	if (len < MODE_PAGE_CACHING_LEN)
+		return SNTI_INTERNAL_ERROR;
+
+	nvme_sc = nvme_get_features(dev, NVME_FEAT_VOLATILE_WC, 0, 0,
+								&feature_resp);
+	res = nvme_trans_status_code(hdr, nvme_sc);
+	if (res)
+		goto out;
+	if (nvme_sc) {
+		res = nvme_sc;
+		goto out;
+	}
+	vwc = feature_resp & 0x00000001;
+
+	resp[0] = MODE_PAGE_CACHING;
+	resp[1] = MODE_PAGE_CACHING_LEN_FIELD;
+	resp[2] = vwc << 2;
+
+ out:
+	return res;
+}
+
+static int nvme_trans_fill_pow_cnd_page(struct nvme_ns *ns,
+					struct sg_io_hdr *hdr, u8 *resp,
+					int len)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+
+	if (len < MODE_PAGE_POW_CND_LEN)
+		return SNTI_INTERNAL_ERROR;
+
+	resp[0] = MODE_PAGE_POWER_CONDITION;
+	resp[1] = MODE_PAGE_POW_CND_LEN_FIELD;
+	/* All other bytes are zero */
+
+	return res;
+}
+
+static int nvme_trans_fill_inf_exc_page(struct nvme_ns *ns,
+					struct sg_io_hdr *hdr, u8 *resp,
+					int len)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+
+	if (len < MODE_PAGE_INF_EXC_LEN)
+		return SNTI_INTERNAL_ERROR;
+
+	resp[0] = MODE_PAGE_INFO_EXCEP;
+	resp[1] = MODE_PAGE_INF_EXC_LEN_FIELD;
+	resp[2] = 0x88;
+	/* All other bytes are zero */
+
+	return res;
+}
+
+static int nvme_trans_fill_all_pages(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+				     u8 *resp, int len)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	u16 mode_pages_offset_1 = 0;
+	u16 mode_pages_offset_2, mode_pages_offset_3, mode_pages_offset_4;
+
+	mode_pages_offset_2 = mode_pages_offset_1 + MODE_PAGE_CACHING_LEN;
+	mode_pages_offset_3 = mode_pages_offset_2 + MODE_PAGE_CONTROL_LEN;
+	mode_pages_offset_4 = mode_pages_offset_3 + MODE_PAGE_POW_CND_LEN;
+
+	res = nvme_trans_fill_caching_page(ns, hdr, &resp[mode_pages_offset_1],
+					MODE_PAGE_CACHING_LEN);
+	if (res != SNTI_TRANSLATION_SUCCESS)
+		goto out;
+	res = nvme_trans_fill_control_page(ns, hdr, &resp[mode_pages_offset_2],
+					MODE_PAGE_CONTROL_LEN);
+	if (res != SNTI_TRANSLATION_SUCCESS)
+		goto out;
+	res = nvme_trans_fill_pow_cnd_page(ns, hdr, &resp[mode_pages_offset_3],
+					MODE_PAGE_POW_CND_LEN);
+	if (res != SNTI_TRANSLATION_SUCCESS)
+		goto out;
+	res = nvme_trans_fill_inf_exc_page(ns, hdr, &resp[mode_pages_offset_4],
+					MODE_PAGE_INF_EXC_LEN);
+	if (res != SNTI_TRANSLATION_SUCCESS)
+		goto out;
+
+ out:
+	return res;
+}
+
+static inline int nvme_trans_get_blk_desc_len(u8 dbd, u8 llbaa)
+{
+	if (dbd == MODE_SENSE_BLK_DESC_ENABLED) {
+		/* SPC-4: len = 8 x Num_of_descriptors if llbaa = 0, 16x if 1 */
+		return 8 * (llbaa + 1) * MODE_SENSE_BLK_DESC_COUNT;
+	} else {
+		return 0;
+	}
+}
+
+static int nvme_trans_mode_page_create(struct nvme_ns *ns,
+					struct sg_io_hdr *hdr, u8 *cmd,
+					u16 alloc_len, u8 cdb10,
+					int (*mode_page_fill_func)
+					(struct nvme_ns *,
+					struct sg_io_hdr *hdr, u8 *, int),
+					u16 mode_pages_tot_len)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int xfer_len;
+	u8 *response;
+	u8 dbd, llbaa;
+	u16 resp_size;
+	int mph_size;
+	u16 mode_pages_offset_1;
+	u16 blk_desc_len, blk_desc_offset, mode_data_length;
+
+	dbd = GET_MODE_SENSE_DBD(cmd);
+	llbaa = GET_MODE_SENSE_LLBAA(cmd);
+	mph_size = GET_MODE_SENSE_MPH_SIZE(cdb10);
+	blk_desc_len = nvme_trans_get_blk_desc_len(dbd, llbaa);
+
+	resp_size = mph_size + blk_desc_len + mode_pages_tot_len;
+	/* Refer spc4r34 Table 440 for calculation of Mode data Length field */
+	mode_data_length = 3 + (3 * cdb10) + blk_desc_len + mode_pages_tot_len;
+
+	blk_desc_offset = mph_size;
+	mode_pages_offset_1 = blk_desc_offset + blk_desc_len;
+
+	response = kmalloc(resp_size, GFP_KERNEL);
+	if (response == NULL) {
+		res = -ENOMEM;
+		goto out_mem;
+	}
+	memset(response, 0, resp_size);
+
+	res = nvme_trans_fill_mode_parm_hdr(&response[0], mph_size, cdb10,
+					llbaa, mode_data_length, blk_desc_len);
+	if (res != SNTI_TRANSLATION_SUCCESS)
+		goto out_free;
+	if (blk_desc_len > 0) {
+		res = nvme_trans_fill_blk_desc(ns, hdr,
+					       &response[blk_desc_offset],
+					       blk_desc_len, llbaa);
+		if (res != SNTI_TRANSLATION_SUCCESS)
+			goto out_free;
+	}
+	res = mode_page_fill_func(ns, hdr, &response[mode_pages_offset_1],
+					mode_pages_tot_len);
+	if (res != SNTI_TRANSLATION_SUCCESS)
+		goto out_free;
+
+	xfer_len = min(alloc_len, resp_size);
+	res = nvme_trans_copy_to_user(hdr, response, xfer_len);
+
+ out_free:
+	kfree(response);
+ out_mem:
+	return res;
+}
+
+/* Read Capacity Helper Functions */
+
+static void nvme_trans_fill_read_cap(u8 *response, struct nvme_id_ns *id_ns,
+								u8 cdb16)
+{
+	u8 flbas;
+	u32 lba_length;
+	u64 rlba;
+	u8 prot_en;
+	u8 p_type_lut[4] = {0, 0, 1, 2};
+	__be64 tmp_rlba;
+	__be32 tmp_rlba_32;
+	__be32 tmp_len;
+
+	flbas = (id_ns->flbas) & 0x0F;
+	lba_length = (1 << (id_ns->lbaf[flbas].ds));
+	rlba = le64_to_cpup(&id_ns->nsze) - 1;
+	(id_ns->dps) ? (prot_en = 0x01) : (prot_en = 0);
+
+	if (!cdb16) {
+		if (rlba > 0xFFFFFFFF)
+			rlba = 0xFFFFFFFF;
+		tmp_rlba_32 = cpu_to_be32(rlba);
+		tmp_len = cpu_to_be32(lba_length);
+		memcpy(response, &tmp_rlba_32, sizeof(u32));
+		memcpy(&response[4], &tmp_len, sizeof(u32));
+	} else {
+		tmp_rlba = cpu_to_be64(rlba);
+		tmp_len = cpu_to_be32(lba_length);
+		memcpy(response, &tmp_rlba, sizeof(u64));
+		memcpy(&response[8], &tmp_len, sizeof(u32));
+		response[12] = (p_type_lut[id_ns->dps & 0x3] << 1) | prot_en;
+		/* P_I_Exponent = 0x0 | LBPPBE = 0x0 */
+		/* LBPME = 0 | LBPRZ = 0 | LALBA = 0x00 */
+		/* Bytes 16-31 - Reserved */
+	}
+}
+
+/* Start Stop Unit Helper Functions */
+
+static int nvme_trans_power_state(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+						u8 pc, u8 pcmod, u8 start)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int nvme_sc;
+	struct nvme_dev *dev = ns->dev;
+	dma_addr_t dma_addr;
+	void *mem;
+	struct nvme_id_ctrl *id_ctrl;
+	int lowest_pow_st;	/* max npss = lowest power consumption */
+	unsigned ps_desired = 0;
+
+	/* NVMe Controller Identify */
+	mem = dma_alloc_coherent(&dev->pci_dev->dev,
+				sizeof(struct nvme_id_ctrl),
+				&dma_addr, GFP_KERNEL);
+	if (mem == NULL) {
+		res = -ENOMEM;
+		goto out;
+	}
+	nvme_sc = nvme_identify(dev, 0, 1, dma_addr);
+	res = nvme_trans_status_code(hdr, nvme_sc);
+	if (res)
+		goto out_dma;
+	if (nvme_sc) {
+		res = nvme_sc;
+		goto out_dma;
+	}
+	id_ctrl = mem;
+	lowest_pow_st = id_ctrl->npss - 1;
+
+	switch (pc) {
+	case NVME_POWER_STATE_START_VALID:
+		/* Action unspecified if POWER CONDITION MODIFIER != 0 */
+		if (pcmod == 0 && start == 0x1)
+			ps_desired = POWER_STATE_0;
+		if (pcmod == 0 && start == 0x0)
+			ps_desired = lowest_pow_st;
+		break;
+	case NVME_POWER_STATE_ACTIVE:
+		/* Action unspecified if POWER CONDITION MODIFIER != 0 */
+		if (pcmod == 0)
+			ps_desired = POWER_STATE_0;
+		break;
+	case NVME_POWER_STATE_IDLE:
+		/* Action unspecified if POWER CONDITION MODIFIER != [0,1,2] */
+		/* min of desired state and (lps-1) because lps is STOP */
+		if (pcmod == 0x0)
+			ps_desired = min(POWER_STATE_1, (lowest_pow_st - 1));
+		else if (pcmod == 0x1)
+			ps_desired = min(POWER_STATE_2, (lowest_pow_st - 1));
+		else if (pcmod == 0x2)
+			ps_desired = min(POWER_STATE_3, (lowest_pow_st - 1));
+		break;
+	case NVME_POWER_STATE_STANDBY:
+		/* Action unspecified if POWER CONDITION MODIFIER != [0,1] */
+		if (pcmod == 0x0)
+			ps_desired = max(0, (lowest_pow_st - 2));
+		else if (pcmod == 0x1)
+			ps_desired = max(0, (lowest_pow_st - 1));
+		break;
+	case NVME_POWER_STATE_LU_CONTROL:
+	default:
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+				ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+				SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		break;
+	}
+	nvme_sc = nvme_set_features(dev, NVME_FEAT_POWER_MGMT, ps_desired, 0,
+				    NULL);
+	res = nvme_trans_status_code(hdr, nvme_sc);
+	if (res)
+		goto out_dma;
+	if (nvme_sc)
+		res = nvme_sc;
+ out_dma:
+	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ctrl), mem,
+			  dma_addr);
+ out:
+	return res;
+}
+
+/* Write Buffer Helper Functions */
+/* Also using this for Format Unit with hdr passed as NULL, and buffer_id, 0 */
+
+static int nvme_trans_send_fw_cmd(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+					u8 opcode, u32 tot_len, u32 offset,
+					u8 buffer_id)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int nvme_sc;
+	struct nvme_dev *dev = ns->dev;
+	struct nvme_command c;
+	struct nvme_iod *iod = NULL;
+	unsigned length;
+
+	memset(&c, 0, sizeof(c));
+	c.common.opcode = opcode;
+	if (opcode == nvme_admin_download_fw) {
+		if (hdr->iovec_count > 0) {
+			/* Assuming SGL is not allowed for this command */
+			res = nvme_trans_completion(hdr,
+						SAM_STAT_CHECK_CONDITION,
+						ILLEGAL_REQUEST,
+						SCSI_ASC_INVALID_CDB,
+						SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+			goto out;
+		}
+		iod = nvme_map_user_pages(dev, DMA_TO_DEVICE,
+				(unsigned long)hdr->dxferp, tot_len);
+		if (IS_ERR(iod)) {
+			res = PTR_ERR(iod);
+			goto out;
+		}
+		length = nvme_setup_prps(dev, &c.common, iod, tot_len,
+								GFP_KERNEL);
+		if (length != tot_len) {
+			res = -ENOMEM;
+			goto out_unmap;
+		}
+
+		c.dlfw.numd = cpu_to_le32((tot_len/BYTES_TO_DWORDS) - 1);
+		c.dlfw.offset = cpu_to_le32(offset/BYTES_TO_DWORDS);
+	} else if (opcode == nvme_admin_activate_fw) {
+		u32 cdw10 = buffer_id | NVME_FWACT_REPL_ACTV;
+		c.common.cdw10[0] = cpu_to_le32(cdw10);
+	}
+
+	nvme_sc = nvme_submit_admin_cmd(dev, &c, NULL);
+	res = nvme_trans_status_code(hdr, nvme_sc);
+	if (res)
+		goto out_unmap;
+	if (nvme_sc)
+		res = nvme_sc;
+
+ out_unmap:
+	if (opcode == nvme_admin_download_fw) {
+		nvme_unmap_user_pages(dev, DMA_TO_DEVICE, iod);
+		nvme_free_iod(dev, iod);
+	}
+ out:
+	return res;
+}
+
+/* Mode Select Helper Functions */
+
+static inline void nvme_trans_modesel_get_bd_len(u8 *parm_list, u8 cdb10,
+						u16 *bd_len, u8 *llbaa)
+{
+	if (cdb10) {
+		/* 10 Byte CDB */
+		*bd_len = (parm_list[MODE_SELECT_10_BD_OFFSET] << 8) +
+			parm_list[MODE_SELECT_10_BD_OFFSET + 1];
+		*llbaa = parm_list[MODE_SELECT_10_LLBAA_OFFSET] &&
+				MODE_SELECT_10_LLBAA_MASK;
+	} else {
+		/* 6 Byte CDB */
+		*bd_len = parm_list[MODE_SELECT_6_BD_OFFSET];
+	}
+}
+
+static void nvme_trans_modesel_save_bd(struct nvme_ns *ns, u8 *parm_list,
+					u16 idx, u16 bd_len, u8 llbaa)
+{
+	u16 bd_num;
+
+	bd_num = bd_len / ((llbaa == 0) ?
+			SHORT_DESC_BLOCK : LONG_DESC_BLOCK);
+	/* Store block descriptor info if a FORMAT UNIT comes later */
+	/* TODO Saving 1st BD info; what to do if multiple BD received? */
+	if (llbaa == 0) {
+		/* Standard Block Descriptor - spc4r34 7.5.5.1 */
+		ns->mode_select_num_blocks =
+				(parm_list[idx + 1] << 16) +
+				(parm_list[idx + 2] << 8) +
+				(parm_list[idx + 3]);
+
+		ns->mode_select_block_len =
+				(parm_list[idx + 5] << 16) +
+				(parm_list[idx + 6] << 8) +
+				(parm_list[idx + 7]);
+	} else {
+		/* Long LBA Block Descriptor - sbc3r27 6.4.2.3 */
+		ns->mode_select_num_blocks =
+				(((u64)parm_list[idx + 0]) << 56) +
+				(((u64)parm_list[idx + 1]) << 48) +
+				(((u64)parm_list[idx + 2]) << 40) +
+				(((u64)parm_list[idx + 3]) << 32) +
+				(((u64)parm_list[idx + 4]) << 24) +
+				(((u64)parm_list[idx + 5]) << 16) +
+				(((u64)parm_list[idx + 6]) << 8) +
+				((u64)parm_list[idx + 7]);
+
+		ns->mode_select_block_len =
+				(parm_list[idx + 12] << 24) +
+				(parm_list[idx + 13] << 16) +
+				(parm_list[idx + 14] << 8) +
+				(parm_list[idx + 15]);
+	}
+}
+
+static u16 nvme_trans_modesel_get_mp(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+					u8 *mode_page, u8 page_code)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int nvme_sc;
+	struct nvme_dev *dev = ns->dev;
+	unsigned dword11;
+
+	switch (page_code) {
+	case MODE_PAGE_CACHING:
+		dword11 = ((mode_page[2] & CACHING_MODE_PAGE_WCE_MASK) ? 1 : 0);
+		nvme_sc = nvme_set_features(dev, NVME_FEAT_VOLATILE_WC, dword11,
+					    0, NULL);
+		res = nvme_trans_status_code(hdr, nvme_sc);
+		if (res)
+			break;
+		if (nvme_sc) {
+			res = nvme_sc;
+			break;
+		}
+		break;
+	case MODE_PAGE_CONTROL:
+		break;
+	case MODE_PAGE_POWER_CONDITION:
+		/* Verify the OS is not trying to set timers */
+		if ((mode_page[2] & 0x01) != 0 || (mode_page[3] & 0x0F) != 0) {
+			res = nvme_trans_completion(hdr,
+						SAM_STAT_CHECK_CONDITION,
+						ILLEGAL_REQUEST,
+						SCSI_ASC_INVALID_PARAMETER,
+						SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+			if (!res)
+				res = SNTI_INTERNAL_ERROR;
+			break;
+		}
+		break;
+	default:
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		if (!res)
+			res = SNTI_INTERNAL_ERROR;
+		break;
+	}
+
+	return res;
+}
+
+static int nvme_trans_modesel_data(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+					u8 *cmd, u16 parm_list_len, u8 pf,
+					u8 sp, u8 cdb10)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	u8 *parm_list;
+	u16 bd_len;
+	u8 llbaa = 0;
+	u16 index, saved_index;
+	u8 page_code;
+	u16 mp_size;
+
+	/* Get parm list from data-in/out buffer */
+	parm_list = kmalloc(parm_list_len, GFP_KERNEL);
+	if (parm_list == NULL) {
+		res = -ENOMEM;
+		goto out;
+	}
+
+	res = nvme_trans_copy_from_user(hdr, parm_list, parm_list_len);
+	if (res != SNTI_TRANSLATION_SUCCESS)
+		goto out_mem;
+
+	nvme_trans_modesel_get_bd_len(parm_list, cdb10, &bd_len, &llbaa);
+	index = (cdb10) ? (MODE_SELECT_10_MPH_SIZE) : (MODE_SELECT_6_MPH_SIZE);
+
+	if (bd_len != 0) {
+		/* Block Descriptors present, parse */
+		nvme_trans_modesel_save_bd(ns, parm_list, index, bd_len, llbaa);
+		index += bd_len;
+	}
+	saved_index = index;
+
+	/* Multiple mode pages may be present; iterate through all */
+	/* In 1st Iteration, don't do NVME Command, only check for CDB errors */
+	do {
+		page_code = parm_list[index] & MODE_SELECT_PAGE_CODE_MASK;
+		mp_size = parm_list[index + 1] + 2;
+		if ((page_code != MODE_PAGE_CACHING) &&
+		    (page_code != MODE_PAGE_CONTROL) &&
+		    (page_code != MODE_PAGE_POWER_CONDITION)) {
+			res = nvme_trans_completion(hdr,
+						SAM_STAT_CHECK_CONDITION,
+						ILLEGAL_REQUEST,
+						SCSI_ASC_INVALID_CDB,
+						SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+			goto out_mem;
+		}
+		index += mp_size;
+	} while (index < parm_list_len);
+
+	/* In 2nd Iteration, do the NVME Commands */
+	index = saved_index;
+	do {
+		page_code = parm_list[index] & MODE_SELECT_PAGE_CODE_MASK;
+		mp_size = parm_list[index + 1] + 2;
+		res = nvme_trans_modesel_get_mp(ns, hdr, &parm_list[index],
+								page_code);
+		if (res != SNTI_TRANSLATION_SUCCESS)
+			break;
+		index += mp_size;
+	} while (index < parm_list_len);
+
+ out_mem:
+	kfree(parm_list);
+ out:
+	return res;
+}
+
+/* Format Unit Helper Functions */
+
+static int nvme_trans_fmt_set_blk_size_count(struct nvme_ns *ns,
+					     struct sg_io_hdr *hdr)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int nvme_sc;
+	struct nvme_dev *dev = ns->dev;
+	dma_addr_t dma_addr;
+	void *mem;
+	struct nvme_id_ns *id_ns;
+	u8 flbas;
+
+	/*
+	 * SCSI Expects a MODE SELECT would have been issued prior to
+	 * a FORMAT UNIT, and the block size and number would be used
+	 * from the block descriptor in it. If a MODE SELECT had not
+	 * been issued, FORMAT shall use the current values for both.
+	 */
+
+	if (ns->mode_select_num_blocks == 0 || ns->mode_select_block_len == 0) {
+		mem = dma_alloc_coherent(&dev->pci_dev->dev,
+			sizeof(struct nvme_id_ns), &dma_addr, GFP_KERNEL);
+		if (mem == NULL) {
+			res = -ENOMEM;
+			goto out;
+		}
+		/* nvme ns identify */
+		nvme_sc = nvme_identify(dev, ns->ns_id, 0, dma_addr);
+		res = nvme_trans_status_code(hdr, nvme_sc);
+		if (res)
+			goto out_dma;
+		if (nvme_sc) {
+			res = nvme_sc;
+			goto out_dma;
+		}
+		id_ns = mem;
+
+		if (ns->mode_select_num_blocks == 0)
+			ns->mode_select_num_blocks = le64_to_cpu(id_ns->ncap);
+		if (ns->mode_select_block_len == 0) {
+			flbas = (id_ns->flbas) & 0x0F;
+			ns->mode_select_block_len =
+						(1 << (id_ns->lbaf[flbas].ds));
+		}
+ out_dma:
+		dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns),
+				  mem, dma_addr);
+	}
+ out:
+	return res;
+}
+
+static int nvme_trans_fmt_get_parm_header(struct sg_io_hdr *hdr, u8 len,
+					u8 format_prot_info, u8 *nvme_pf_code)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	u8 *parm_list;
+	u8 pf_usage, pf_code;
+
+	parm_list = kmalloc(len, GFP_KERNEL);
+	if (parm_list == NULL) {
+		res = -ENOMEM;
+		goto out;
+	}
+	res = nvme_trans_copy_from_user(hdr, parm_list, len);
+	if (res != SNTI_TRANSLATION_SUCCESS)
+		goto out_mem;
+
+	if ((parm_list[FORMAT_UNIT_IMMED_OFFSET] &
+				FORMAT_UNIT_IMMED_MASK) != 0) {
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		goto out_mem;
+	}
+
+	if (len == FORMAT_UNIT_LONG_PARM_LIST_LEN &&
+	    (parm_list[FORMAT_UNIT_PROT_INT_OFFSET] & 0x0F) != 0) {
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		goto out_mem;
+	}
+	pf_usage = parm_list[FORMAT_UNIT_PROT_FIELD_USAGE_OFFSET] &
+			FORMAT_UNIT_PROT_FIELD_USAGE_MASK;
+	pf_code = (pf_usage << 2) | format_prot_info;
+	switch (pf_code) {
+	case 0:
+		*nvme_pf_code = 0;
+		break;
+	case 2:
+		*nvme_pf_code = 1;
+		break;
+	case 3:
+		*nvme_pf_code = 2;
+		break;
+	case 7:
+		*nvme_pf_code = 3;
+		break;
+	default:
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		break;
+	}
+
+ out_mem:
+	kfree(parm_list);
+ out:
+	return res;
+}
+
+static int nvme_trans_fmt_send_cmd(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+				   u8 prot_info)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int nvme_sc;
+	struct nvme_dev *dev = ns->dev;
+	dma_addr_t dma_addr;
+	void *mem;
+	struct nvme_id_ns *id_ns;
+	u8 i;
+	u8 flbas, nlbaf;
+	u8 selected_lbaf = 0xFF;
+	u32 cdw10 = 0;
+	struct nvme_command c;
+
+	/* Loop thru LBAF's in id_ns to match reqd lbaf, put in cdw10 */
+	mem = dma_alloc_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns),
+							&dma_addr, GFP_KERNEL);
+	if (mem == NULL) {
+		res = -ENOMEM;
+		goto out;
+	}
+	/* nvme ns identify */
+	nvme_sc = nvme_identify(dev, ns->ns_id, 0, dma_addr);
+	res = nvme_trans_status_code(hdr, nvme_sc);
+	if (res)
+		goto out_dma;
+	if (nvme_sc) {
+		res = nvme_sc;
+		goto out_dma;
+	}
+	id_ns = mem;
+	flbas = (id_ns->flbas) & 0x0F;
+	nlbaf = id_ns->nlbaf;
+
+	for (i = 0; i < nlbaf; i++) {
+		if (ns->mode_select_block_len == (1 << (id_ns->lbaf[i].ds))) {
+			selected_lbaf = i;
+			break;
+		}
+	}
+	if (selected_lbaf > 0x0F) {
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+				ILLEGAL_REQUEST, SCSI_ASC_INVALID_PARAMETER,
+				SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+	}
+	if (ns->mode_select_num_blocks != le64_to_cpu(id_ns->ncap)) {
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+				ILLEGAL_REQUEST, SCSI_ASC_INVALID_PARAMETER,
+				SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+	}
+
+	cdw10 |= prot_info << 5;
+	cdw10 |= selected_lbaf & 0x0F;
+	memset(&c, 0, sizeof(c));
+	c.format.opcode = nvme_admin_format_nvm;
+	c.format.nsid = cpu_to_le32(ns->ns_id);
+	c.format.cdw10 = cpu_to_le32(cdw10);
+
+	nvme_sc = nvme_submit_admin_cmd(dev, &c, NULL);
+	res = nvme_trans_status_code(hdr, nvme_sc);
+	if (res)
+		goto out_dma;
+	if (nvme_sc)
+		res = nvme_sc;
+
+ out_dma:
+	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns), mem,
+			  dma_addr);
+ out:
+	return res;
+}
+
+/* Read/Write Helper Functions */
+
+static inline void nvme_trans_get_io_cdb6(u8 *cmd,
+					struct nvme_trans_io_cdb *cdb_info)
+{
+	cdb_info->fua = 0;
+	cdb_info->prot_info = 0;
+	cdb_info->lba = GET_U32_FROM_CDB(cmd, IO_6_CDB_LBA_OFFSET) &
+					IO_6_CDB_LBA_MASK;
+	cdb_info->xfer_len = GET_U8_FROM_CDB(cmd, IO_6_CDB_TX_LEN_OFFSET);
+
+	/* sbc3r27 sec 5.32 - TRANSFER LEN of 0 implies a 256 Block transfer */
+	if (cdb_info->xfer_len == 0)
+		cdb_info->xfer_len = IO_6_DEFAULT_TX_LEN;
+}
+
+static inline void nvme_trans_get_io_cdb10(u8 *cmd,
+					struct nvme_trans_io_cdb *cdb_info)
+{
+	cdb_info->fua = GET_U8_FROM_CDB(cmd, IO_10_CDB_FUA_OFFSET) &
+					IO_CDB_FUA_MASK;
+	cdb_info->prot_info = GET_U8_FROM_CDB(cmd, IO_10_CDB_WP_OFFSET) &
+					IO_CDB_WP_MASK >> IO_CDB_WP_SHIFT;
+	cdb_info->lba = GET_U32_FROM_CDB(cmd, IO_10_CDB_LBA_OFFSET);
+	cdb_info->xfer_len = GET_U16_FROM_CDB(cmd, IO_10_CDB_TX_LEN_OFFSET);
+}
+
+static inline void nvme_trans_get_io_cdb12(u8 *cmd,
+					struct nvme_trans_io_cdb *cdb_info)
+{
+	cdb_info->fua = GET_U8_FROM_CDB(cmd, IO_12_CDB_FUA_OFFSET) &
+					IO_CDB_FUA_MASK;
+	cdb_info->prot_info = GET_U8_FROM_CDB(cmd, IO_12_CDB_WP_OFFSET) &
+					IO_CDB_WP_MASK >> IO_CDB_WP_SHIFT;
+	cdb_info->lba = GET_U32_FROM_CDB(cmd, IO_12_CDB_LBA_OFFSET);
+	cdb_info->xfer_len = GET_U32_FROM_CDB(cmd, IO_12_CDB_TX_LEN_OFFSET);
+}
+
+static inline void nvme_trans_get_io_cdb16(u8 *cmd,
+					struct nvme_trans_io_cdb *cdb_info)
+{
+	cdb_info->fua = GET_U8_FROM_CDB(cmd, IO_16_CDB_FUA_OFFSET) &
+					IO_CDB_FUA_MASK;
+	cdb_info->prot_info = GET_U8_FROM_CDB(cmd, IO_16_CDB_WP_OFFSET) &
+					IO_CDB_WP_MASK >> IO_CDB_WP_SHIFT;
+	cdb_info->lba = GET_U64_FROM_CDB(cmd, IO_16_CDB_LBA_OFFSET);
+	cdb_info->xfer_len = GET_U32_FROM_CDB(cmd, IO_16_CDB_TX_LEN_OFFSET);
+}
+
+static inline u32 nvme_trans_io_get_num_cmds(struct sg_io_hdr *hdr,
+					struct nvme_trans_io_cdb *cdb_info,
+					u32 max_blocks)
+{
+	/* If using iovecs, send one nvme command per vector */
+	if (hdr->iovec_count > 0)
+		return hdr->iovec_count;
+	else if (cdb_info->xfer_len > max_blocks)
+		return ((cdb_info->xfer_len - 1) / max_blocks) + 1;
+	else
+		return 1;
+}
+
+static u16 nvme_trans_io_get_control(struct nvme_ns *ns,
+					struct nvme_trans_io_cdb *cdb_info)
+{
+	u16 control = 0;
+
+	/* When Protection information support is added, implement here */
+
+	if (cdb_info->fua > 0)
+		control |= NVME_RW_FUA;
+
+	return control;
+}
+
+static int nvme_trans_do_nvme_io(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+				struct nvme_trans_io_cdb *cdb_info, u8 is_write)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int nvme_sc;
+	struct nvme_dev *dev = ns->dev;
+	struct nvme_queue *nvmeq;
+	u32 num_cmds;
+	struct nvme_iod *iod;
+	u64 unit_len;
+	u64 unit_num_blocks;	/* Number of blocks to xfer in each nvme cmd */
+	u32 retcode;
+	u32 i = 0;
+	u64 nvme_offset = 0;
+	void __user *next_mapping_addr;
+	struct nvme_command c;
+	u8 opcode = (is_write ? nvme_cmd_write : nvme_cmd_read);
+	u16 control;
+	u32 max_blocks = nvme_block_nr(ns, dev->max_hw_sectors);
+
+	num_cmds = nvme_trans_io_get_num_cmds(hdr, cdb_info, max_blocks);
+
+	/*
+	 * This loop handles two cases.
+	 * First, when an SGL is used in the form of an iovec list:
+	 *   - Use iov_base as the next mapping address for the nvme command_id
+	 *   - Use iov_len as the data transfer length for the command.
+	 * Second, when we have a single buffer
+	 *   - If larger than max_blocks, split into chunks, offset
+	 *        each nvme command accordingly.
+	 */
+	for (i = 0; i < num_cmds; i++) {
+		memset(&c, 0, sizeof(c));
+		if (hdr->iovec_count > 0) {
+			struct sg_iovec sgl;
+
+			retcode = copy_from_user(&sgl, hdr->dxferp +
+					i * sizeof(struct sg_iovec),
+					sizeof(struct sg_iovec));
+			if (retcode)
+				return -EFAULT;
+			unit_len = sgl.iov_len;
+			unit_num_blocks = unit_len >> ns->lba_shift;
+			next_mapping_addr = sgl.iov_base;
+		} else {
+			unit_num_blocks = min((u64)max_blocks,
+					(cdb_info->xfer_len - nvme_offset));
+			unit_len = unit_num_blocks << ns->lba_shift;
+			next_mapping_addr = hdr->dxferp +
+					((1 << ns->lba_shift) * nvme_offset);
+		}
+
+		c.rw.opcode = opcode;
+		c.rw.nsid = cpu_to_le32(ns->ns_id);
+		c.rw.slba = cpu_to_le64(cdb_info->lba + nvme_offset);
+		c.rw.length = cpu_to_le16(unit_num_blocks - 1);
+		control = nvme_trans_io_get_control(ns, cdb_info);
+		c.rw.control = cpu_to_le16(control);
+
+		iod = nvme_map_user_pages(dev,
+			(is_write) ? DMA_TO_DEVICE : DMA_FROM_DEVICE,
+			(unsigned long)next_mapping_addr, unit_len);
+		if (IS_ERR(iod)) {
+			res = PTR_ERR(iod);
+			goto out;
+		}
+		retcode = nvme_setup_prps(dev, &c.common, iod, unit_len,
+							GFP_KERNEL);
+		if (retcode != unit_len) {
+			nvme_unmap_user_pages(dev,
+				(is_write) ? DMA_TO_DEVICE : DMA_FROM_DEVICE,
+				iod);
+			nvme_free_iod(dev, iod);
+			res = -ENOMEM;
+			goto out;
+		}
+
+		nvme_offset += unit_num_blocks;
+
+		nvmeq = get_nvmeq(dev);
+		/*
+		 * Since nvme_submit_sync_cmd sleeps, we can't keep
+		 * preemption disabled.  We may be preempted at any
+		 * point, and be rescheduled to a different CPU.  That
+		 * will cause cacheline bouncing, but no additional
+		 * races since q_lock already protects against other
+		 * CPUs.
+		 */
+		put_nvmeq(nvmeq);
+		nvme_sc = nvme_submit_sync_cmd(nvmeq, &c, NULL,
+						NVME_IO_TIMEOUT);
+		if (nvme_sc != NVME_SC_SUCCESS) {
+			nvme_unmap_user_pages(dev,
+				(is_write) ? DMA_TO_DEVICE : DMA_FROM_DEVICE,
+				iod);
+			nvme_free_iod(dev, iod);
+			res = nvme_trans_status_code(hdr, nvme_sc);
+			goto out;
+		}
+		nvme_unmap_user_pages(dev,
+				(is_write) ? DMA_TO_DEVICE : DMA_FROM_DEVICE,
+				iod);
+		nvme_free_iod(dev, iod);
+	}
+	res = nvme_trans_status_code(hdr, NVME_SC_SUCCESS);
+
+ out:
+	return res;
+}
+
+
+/* SCSI Command Translation Functions */
+
+static int nvme_trans_io(struct nvme_ns *ns, struct sg_io_hdr *hdr, u8 is_write,
+							u8 *cmd)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	struct nvme_trans_io_cdb cdb_info;
+	u8 opcode = cmd[0];
+	u64 xfer_bytes;
+	u64 sum_iov_len = 0;
+	struct sg_iovec sgl;
+	int i;
+	size_t not_copied;
+
+	/* Extract Fields from CDB */
+	switch (opcode) {
+	case WRITE_6:
+	case READ_6:
+		nvme_trans_get_io_cdb6(cmd, &cdb_info);
+		break;
+	case WRITE_10:
+	case READ_10:
+		nvme_trans_get_io_cdb10(cmd, &cdb_info);
+		break;
+	case WRITE_12:
+	case READ_12:
+		nvme_trans_get_io_cdb12(cmd, &cdb_info);
+		break;
+	case WRITE_16:
+	case READ_16:
+		nvme_trans_get_io_cdb16(cmd, &cdb_info);
+		break;
+	default:
+		/* Will never really reach here */
+		res = SNTI_INTERNAL_ERROR;
+		goto out;
+	}
+
+	/* Calculate total length of transfer (in bytes) */
+	if (hdr->iovec_count > 0) {
+		for (i = 0; i < hdr->iovec_count; i++) {
+			not_copied = copy_from_user(&sgl, hdr->dxferp +
+						i * sizeof(struct sg_iovec),
+						sizeof(struct sg_iovec));
+			if (not_copied)
+				return -EFAULT;
+			sum_iov_len += sgl.iov_len;
+			/* IO vector sizes should be multiples of block size */
+			if (sgl.iov_len % (1 << ns->lba_shift) != 0) {
+				res = nvme_trans_completion(hdr,
+						SAM_STAT_CHECK_CONDITION,
+						ILLEGAL_REQUEST,
+						SCSI_ASC_INVALID_PARAMETER,
+						SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+				goto out;
+			}
+		}
+	} else {
+		sum_iov_len = hdr->dxfer_len;
+	}
+
+	/* As Per sg ioctl howto, if the lengths differ, use the lower one */
+	xfer_bytes = min(((u64)hdr->dxfer_len), sum_iov_len);
+
+	/* If block count and actual data buffer size dont match, error out */
+	if (xfer_bytes != (cdb_info.xfer_len << ns->lba_shift)) {
+		res = -EINVAL;
+		goto out;
+	}
+
+	/* Check for 0 length transfer - it is not illegal */
+	if (cdb_info.xfer_len == 0)
+		goto out;
+
+	/* Send NVMe IO Command(s) */
+	res = nvme_trans_do_nvme_io(ns, hdr, &cdb_info, is_write);
+	if (res != SNTI_TRANSLATION_SUCCESS)
+		goto out;
+
+ out:
+	return res;
+}
+
+static int nvme_trans_inquiry(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+							u8 *cmd)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	u8 evpd;
+	u8 page_code;
+	int alloc_len;
+	u8 *inq_response;
+
+	evpd = GET_INQ_EVPD_BIT(cmd);
+	page_code = GET_INQ_PAGE_CODE(cmd);
+	alloc_len = GET_INQ_ALLOC_LENGTH(cmd);
+
+	inq_response = kmalloc(STANDARD_INQUIRY_LENGTH, GFP_KERNEL);
+	if (inq_response == NULL) {
+		res = -ENOMEM;
+		goto out_mem;
+	}
+
+	if (evpd == 0) {
+		if (page_code == INQ_STANDARD_INQUIRY_PAGE) {
+			res = nvme_trans_standard_inquiry_page(ns, hdr,
+						inq_response, alloc_len);
+		} else {
+			res = nvme_trans_completion(hdr,
+						SAM_STAT_CHECK_CONDITION,
+						ILLEGAL_REQUEST,
+						SCSI_ASC_INVALID_CDB,
+						SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		}
+	} else {
+		switch (page_code) {
+		case VPD_SUPPORTED_PAGES:
+			res = nvme_trans_supported_vpd_pages(ns, hdr,
+						inq_response, alloc_len);
+			break;
+		case VPD_SERIAL_NUMBER:
+			res = nvme_trans_unit_serial_page(ns, hdr, inq_response,
+								alloc_len);
+			break;
+		case VPD_DEVICE_IDENTIFIERS:
+			res = nvme_trans_device_id_page(ns, hdr, inq_response,
+								alloc_len);
+			break;
+		case VPD_EXTENDED_INQUIRY:
+			res = nvme_trans_ext_inq_page(ns, hdr, alloc_len);
+			break;
+		case VPD_BLOCK_DEV_CHARACTERISTICS:
+			res = nvme_trans_bdev_char_page(ns, hdr, alloc_len);
+			break;
+		default:
+			res = nvme_trans_completion(hdr,
+						SAM_STAT_CHECK_CONDITION,
+						ILLEGAL_REQUEST,
+						SCSI_ASC_INVALID_CDB,
+						SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+			break;
+		}
+	}
+	kfree(inq_response);
+ out_mem:
+	return res;
+}
+
+static int nvme_trans_log_sense(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+							u8 *cmd)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	u16 alloc_len;
+	u8 sp;
+	u8 pc;
+	u8 page_code;
+
+	sp = GET_U8_FROM_CDB(cmd, LOG_SENSE_CDB_SP_OFFSET);
+	if (sp != LOG_SENSE_CDB_SP_NOT_ENABLED) {
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		goto out;
+	}
+	pc = GET_U8_FROM_CDB(cmd, LOG_SENSE_CDB_PC_OFFSET);
+	page_code = pc & LOG_SENSE_CDB_PAGE_CODE_MASK;
+	pc = (pc & LOG_SENSE_CDB_PC_MASK) >> LOG_SENSE_CDB_PC_SHIFT;
+	if (pc != LOG_SENSE_CDB_PC_CUMULATIVE_VALUES) {
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		goto out;
+	}
+	alloc_len = GET_U16_FROM_CDB(cmd, LOG_SENSE_CDB_ALLOC_LENGTH_OFFSET);
+	switch (page_code) {
+	case LOG_PAGE_SUPPORTED_LOG_PAGES_PAGE:
+		res = nvme_trans_log_supp_pages(ns, hdr, alloc_len);
+		break;
+	case LOG_PAGE_INFORMATIONAL_EXCEPTIONS_PAGE:
+		res = nvme_trans_log_info_exceptions(ns, hdr, alloc_len);
+		break;
+	case LOG_PAGE_TEMPERATURE_PAGE:
+		res = nvme_trans_log_temperature(ns, hdr, alloc_len);
+		break;
+	default:
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		break;
+	}
+
+ out:
+	return res;
+}
+
+static int nvme_trans_mode_select(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+							u8 *cmd)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	u8 cdb10 = 0;
+	u16 parm_list_len;
+	u8 page_format;
+	u8 save_pages;
+
+	page_format = GET_U8_FROM_CDB(cmd, MODE_SELECT_CDB_PAGE_FORMAT_OFFSET);
+	page_format &= MODE_SELECT_CDB_PAGE_FORMAT_MASK;
+
+	save_pages = GET_U8_FROM_CDB(cmd, MODE_SELECT_CDB_SAVE_PAGES_OFFSET);
+	save_pages &= MODE_SELECT_CDB_SAVE_PAGES_MASK;
+
+	if (GET_OPCODE(cmd) == MODE_SELECT) {
+		parm_list_len = GET_U8_FROM_CDB(cmd,
+				MODE_SELECT_6_CDB_PARAM_LIST_LENGTH_OFFSET);
+	} else {
+		parm_list_len = GET_U16_FROM_CDB(cmd,
+				MODE_SELECT_10_CDB_PARAM_LIST_LENGTH_OFFSET);
+		cdb10 = 1;
+	}
+
+	if (parm_list_len != 0) {
+		/*
+		 * According to SPC-4 r24, a paramter list length field of 0
+		 * shall not be considered an error
+		 */
+		res = nvme_trans_modesel_data(ns, hdr, cmd, parm_list_len,
+						page_format, save_pages, cdb10);
+	}
+
+	return res;
+}
+
+static int nvme_trans_mode_sense(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+							u8 *cmd)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	u16 alloc_len;
+	u8 cdb10 = 0;
+	u8 page_code;
+	u8 pc;
+
+	if (GET_OPCODE(cmd) == MODE_SENSE) {
+		alloc_len = GET_U8_FROM_CDB(cmd, MODE_SENSE6_ALLOC_LEN_OFFSET);
+	} else {
+		alloc_len = GET_U16_FROM_CDB(cmd,
+						MODE_SENSE10_ALLOC_LEN_OFFSET);
+		cdb10 = 1;
+	}
+
+	pc = GET_U8_FROM_CDB(cmd, MODE_SENSE_PAGE_CONTROL_OFFSET) &
+						MODE_SENSE_PAGE_CONTROL_MASK;
+	if (pc != MODE_SENSE_PC_CURRENT_VALUES) {
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		goto out;
+	}
+
+	page_code = GET_U8_FROM_CDB(cmd, MODE_SENSE_PAGE_CODE_OFFSET) &
+					MODE_SENSE_PAGE_CODE_MASK;
+	switch (page_code) {
+	case MODE_PAGE_CACHING:
+		res = nvme_trans_mode_page_create(ns, hdr, cmd, alloc_len,
+						cdb10,
+						&nvme_trans_fill_caching_page,
+						MODE_PAGE_CACHING_LEN);
+		break;
+	case MODE_PAGE_CONTROL:
+		res = nvme_trans_mode_page_create(ns, hdr, cmd, alloc_len,
+						cdb10,
+						&nvme_trans_fill_control_page,
+						MODE_PAGE_CONTROL_LEN);
+		break;
+	case MODE_PAGE_POWER_CONDITION:
+		res = nvme_trans_mode_page_create(ns, hdr, cmd, alloc_len,
+						cdb10,
+						&nvme_trans_fill_pow_cnd_page,
+						MODE_PAGE_POW_CND_LEN);
+		break;
+	case MODE_PAGE_INFO_EXCEP:
+		res = nvme_trans_mode_page_create(ns, hdr, cmd, alloc_len,
+						cdb10,
+						&nvme_trans_fill_inf_exc_page,
+						MODE_PAGE_INF_EXC_LEN);
+		break;
+	case MODE_PAGE_RETURN_ALL:
+		res = nvme_trans_mode_page_create(ns, hdr, cmd, alloc_len,
+						cdb10,
+						&nvme_trans_fill_all_pages,
+						MODE_PAGE_ALL_LEN);
+		break;
+	default:
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		break;
+	}
+
+ out:
+	return res;
+}
+
+static int nvme_trans_read_capacity(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+							u8 *cmd)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int nvme_sc;
+	u32 alloc_len = READ_CAP_10_RESP_SIZE;
+	u32 resp_size = READ_CAP_10_RESP_SIZE;
+	u32 xfer_len;
+	u8 cdb16;
+	struct nvme_dev *dev = ns->dev;
+	dma_addr_t dma_addr;
+	void *mem;
+	struct nvme_id_ns *id_ns;
+	u8 *response;
+
+	cdb16 = IS_READ_CAP_16(cmd);
+	if (cdb16) {
+		alloc_len = GET_READ_CAP_16_ALLOC_LENGTH(cmd);
+		resp_size = READ_CAP_16_RESP_SIZE;
+	}
+
+	mem = dma_alloc_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns),
+							&dma_addr, GFP_KERNEL);
+	if (mem == NULL) {
+		res = -ENOMEM;
+		goto out;
+	}
+	/* nvme ns identify */
+	nvme_sc = nvme_identify(dev, ns->ns_id, 0, dma_addr);
+	res = nvme_trans_status_code(hdr, nvme_sc);
+	if (res)
+		goto out_dma;
+	if (nvme_sc) {
+		res = nvme_sc;
+		goto out_dma;
+	}
+	id_ns = mem;
+
+	response = kmalloc(resp_size, GFP_KERNEL);
+	if (response == NULL) {
+		res = -ENOMEM;
+		goto out_dma;
+	}
+	memset(response, 0, resp_size);
+	nvme_trans_fill_read_cap(response, id_ns, cdb16);
+
+	xfer_len = min(alloc_len, resp_size);
+	res = nvme_trans_copy_to_user(hdr, response, xfer_len);
+
+	kfree(response);
+ out_dma:
+	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns), mem,
+			  dma_addr);
+ out:
+	return res;
+}
+
+static int nvme_trans_report_luns(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+							u8 *cmd)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int nvme_sc;
+	u32 alloc_len, xfer_len, resp_size;
+	u8 select_report;
+	u8 *response;
+	struct nvme_dev *dev = ns->dev;
+	dma_addr_t dma_addr;
+	void *mem;
+	struct nvme_id_ctrl *id_ctrl;
+	u32 ll_length, lun_id;
+	u8 lun_id_offset = REPORT_LUNS_FIRST_LUN_OFFSET;
+	__be32 tmp_len;
+
+	alloc_len = GET_REPORT_LUNS_ALLOC_LENGTH(cmd);
+	select_report = GET_U8_FROM_CDB(cmd, REPORT_LUNS_SR_OFFSET);
+
+	if ((select_report != ALL_LUNS_RETURNED) &&
+	    (select_report != ALL_WELL_KNOWN_LUNS_RETURNED) &&
+	    (select_report != RESTRICTED_LUNS_RETURNED)) {
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		goto out;
+	} else {
+		/* NVMe Controller Identify */
+		mem = dma_alloc_coherent(&dev->pci_dev->dev,
+					sizeof(struct nvme_id_ctrl),
+					&dma_addr, GFP_KERNEL);
+		if (mem == NULL) {
+			res = -ENOMEM;
+			goto out;
+		}
+		nvme_sc = nvme_identify(dev, 0, 1, dma_addr);
+		res = nvme_trans_status_code(hdr, nvme_sc);
+		if (res)
+			goto out_dma;
+		if (nvme_sc) {
+			res = nvme_sc;
+			goto out_dma;
+		}
+		id_ctrl = mem;
+		ll_length = le32_to_cpu(id_ctrl->nn) * LUN_ENTRY_SIZE;
+		resp_size = ll_length + LUN_DATA_HEADER_SIZE;
+
+		if (alloc_len < resp_size) {
+			res = nvme_trans_completion(hdr,
+					SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+			goto out_dma;
+		}
+
+		response = kmalloc(resp_size, GFP_KERNEL);
+		if (response == NULL) {
+			res = -ENOMEM;
+			goto out_dma;
+		}
+		memset(response, 0, resp_size);
+
+		/* The first LUN ID will always be 0 per the SAM spec */
+		for (lun_id = 0; lun_id < le32_to_cpu(id_ctrl->nn); lun_id++) {
+			/*
+			 * Set the LUN Id and then increment to the next LUN
+			 * location in the parameter data.
+			 */
+			__be64 tmp_id = cpu_to_be64(lun_id);
+			memcpy(&response[lun_id_offset], &tmp_id, sizeof(u64));
+			lun_id_offset += LUN_ENTRY_SIZE;
+		}
+		tmp_len = cpu_to_be32(ll_length);
+		memcpy(response, &tmp_len, sizeof(u32));
+	}
+
+	xfer_len = min(alloc_len, resp_size);
+	res = nvme_trans_copy_to_user(hdr, response, xfer_len);
+
+	kfree(response);
+ out_dma:
+	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ctrl), mem,
+			  dma_addr);
+ out:
+	return res;
+}
+
+static int nvme_trans_request_sense(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+							u8 *cmd)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	u8 alloc_len, xfer_len, resp_size;
+	u8 desc_format;
+	u8 *response;
+
+	alloc_len = GET_REQUEST_SENSE_ALLOC_LENGTH(cmd);
+	desc_format = GET_U8_FROM_CDB(cmd, REQUEST_SENSE_DESC_OFFSET);
+	desc_format &= REQUEST_SENSE_DESC_MASK;
+
+	resp_size = ((desc_format) ? (DESC_FMT_SENSE_DATA_SIZE) :
+					(FIXED_FMT_SENSE_DATA_SIZE));
+	response = kmalloc(resp_size, GFP_KERNEL);
+	if (response == NULL) {
+		res = -ENOMEM;
+		goto out;
+	}
+	memset(response, 0, resp_size);
+
+	if (desc_format == DESCRIPTOR_FORMAT_SENSE_DATA_TYPE) {
+		/* Descriptor Format Sense Data */
+		response[0] = DESC_FORMAT_SENSE_DATA;
+		response[1] = NO_SENSE;
+		/* TODO How is LOW POWER CONDITION ON handled? (byte 2) */
+		response[2] = SCSI_ASC_NO_SENSE;
+		response[3] = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		/* SDAT_OVFL = 0 | Additional Sense Length = 0 */
+	} else {
+		/* Fixed Format Sense Data */
+		response[0] = FIXED_SENSE_DATA;
+		/* Byte 1 = Obsolete */
+		response[2] = NO_SENSE; /* FM, EOM, ILI, SDAT_OVFL = 0 */
+		/* Bytes 3-6 - Information - set to zero */
+		response[7] = FIXED_SENSE_DATA_ADD_LENGTH;
+		/* Bytes 8-11 - Cmd Specific Information - set to zero */
+		response[12] = SCSI_ASC_NO_SENSE;
+		response[13] = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
+		/* Byte 14 = Field Replaceable Unit Code = 0 */
+		/* Bytes 15-17 - SKSV=0; Sense Key Specific = 0 */
+	}
+
+	xfer_len = min(alloc_len, resp_size);
+	res = nvme_trans_copy_to_user(hdr, response, xfer_len);
+
+	kfree(response);
+ out:
+	return res;
+}
+
+static int nvme_trans_security_protocol(struct nvme_ns *ns,
+					struct sg_io_hdr *hdr,
+					u8 *cmd)
+{
+	return nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+				ILLEGAL_REQUEST, SCSI_ASC_ILLEGAL_COMMAND,
+				SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+}
+
+static int nvme_trans_start_stop(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+							u8 *cmd)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int nvme_sc;
+	struct nvme_queue *nvmeq;
+	struct nvme_command c;
+	u8 immed, pcmod, pc, no_flush, start;
+
+	immed = GET_U8_FROM_CDB(cmd, START_STOP_UNIT_CDB_IMMED_OFFSET);
+	pcmod = GET_U8_FROM_CDB(cmd, START_STOP_UNIT_CDB_POWER_COND_MOD_OFFSET);
+	pc = GET_U8_FROM_CDB(cmd, START_STOP_UNIT_CDB_POWER_COND_OFFSET);
+	no_flush = GET_U8_FROM_CDB(cmd, START_STOP_UNIT_CDB_NO_FLUSH_OFFSET);
+	start = GET_U8_FROM_CDB(cmd, START_STOP_UNIT_CDB_START_OFFSET);
+
+	immed &= START_STOP_UNIT_CDB_IMMED_MASK;
+	pcmod &= START_STOP_UNIT_CDB_POWER_COND_MOD_MASK;
+	pc = (pc & START_STOP_UNIT_CDB_POWER_COND_MASK) >> NIBBLE_SHIFT;
+	no_flush &= START_STOP_UNIT_CDB_NO_FLUSH_MASK;
+	start &= START_STOP_UNIT_CDB_START_MASK;
+
+	if (immed != 0) {
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+	} else {
+		if (no_flush == 0) {
+			/* Issue NVME FLUSH command prior to START STOP UNIT */
+			memset(&c, 0, sizeof(c));
+			c.common.opcode = nvme_cmd_flush;
+			c.common.nsid = cpu_to_le32(ns->ns_id);
+
+			nvmeq = get_nvmeq(ns->dev);
+			put_nvmeq(nvmeq);
+			nvme_sc = nvme_submit_sync_cmd(nvmeq, &c, NULL, NVME_IO_TIMEOUT);
+
+			res = nvme_trans_status_code(hdr, nvme_sc);
+			if (res)
+				goto out;
+			if (nvme_sc) {
+				res = nvme_sc;
+				goto out;
+			}
+		}
+		/* Setup the expected power state transition */
+		res = nvme_trans_power_state(ns, hdr, pc, pcmod, start);
+	}
+
+ out:
+	return res;
+}
+
+static int nvme_trans_synchronize_cache(struct nvme_ns *ns,
+					struct sg_io_hdr *hdr, u8 *cmd)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	int nvme_sc;
+	struct nvme_command c;
+	struct nvme_queue *nvmeq;
+
+	memset(&c, 0, sizeof(c));
+	c.common.opcode = nvme_cmd_flush;
+	c.common.nsid = cpu_to_le32(ns->ns_id);
+
+	nvmeq = get_nvmeq(ns->dev);
+	put_nvmeq(nvmeq);
+	nvme_sc = nvme_submit_sync_cmd(nvmeq, &c, NULL, NVME_IO_TIMEOUT);
+
+	res = nvme_trans_status_code(hdr, nvme_sc);
+	if (res)
+		goto out;
+	if (nvme_sc)
+		res = nvme_sc;
+
+ out:
+	return res;
+}
+
+static int nvme_trans_format_unit(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+							u8 *cmd)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	u8 parm_hdr_len = 0;
+	u8 nvme_pf_code = 0;
+	u8 format_prot_info, long_list, format_data;
+
+	format_prot_info = GET_U8_FROM_CDB(cmd,
+				FORMAT_UNIT_CDB_FORMAT_PROT_INFO_OFFSET);
+	long_list = GET_U8_FROM_CDB(cmd, FORMAT_UNIT_CDB_LONG_LIST_OFFSET);
+	format_data = GET_U8_FROM_CDB(cmd, FORMAT_UNIT_CDB_FORMAT_DATA_OFFSET);
+
+	format_prot_info = (format_prot_info &
+				FORMAT_UNIT_CDB_FORMAT_PROT_INFO_MASK) >>
+				FORMAT_UNIT_CDB_FORMAT_PROT_INFO_SHIFT;
+	long_list &= FORMAT_UNIT_CDB_LONG_LIST_MASK;
+	format_data &= FORMAT_UNIT_CDB_FORMAT_DATA_MASK;
+
+	if (format_data != 0) {
+		if (format_prot_info != 0) {
+			if (long_list == 0)
+				parm_hdr_len = FORMAT_UNIT_SHORT_PARM_LIST_LEN;
+			else
+				parm_hdr_len = FORMAT_UNIT_LONG_PARM_LIST_LEN;
+		}
+	} else if (format_data == 0 && format_prot_info != 0) {
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		goto out;
+	}
+
+	/* Get parm header from data-in/out buffer */
+	/*
+	 * According to the translation spec, the only fields in the parameter
+	 * list we are concerned with are in the header. So allocate only that.
+	 */
+	if (parm_hdr_len > 0) {
+		res = nvme_trans_fmt_get_parm_header(hdr, parm_hdr_len,
+					format_prot_info, &nvme_pf_code);
+		if (res != SNTI_TRANSLATION_SUCCESS)
+			goto out;
+	}
+
+	/* Attempt to activate any previously downloaded firmware image */
+	res = nvme_trans_send_fw_cmd(ns, hdr, nvme_admin_activate_fw, 0, 0, 0);
+
+	/* Determine Block size and count and send format command */
+	res = nvme_trans_fmt_set_blk_size_count(ns, hdr);
+	if (res != SNTI_TRANSLATION_SUCCESS)
+		goto out;
+
+	res = nvme_trans_fmt_send_cmd(ns, hdr, nvme_pf_code);
+
+ out:
+	return res;
+}
+
+static int nvme_trans_test_unit_ready(struct nvme_ns *ns,
+					struct sg_io_hdr *hdr,
+					u8 *cmd)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	struct nvme_dev *dev = ns->dev;
+
+	if (!(readl(&dev->bar->csts) & NVME_CSTS_RDY))
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					    NOT_READY, SCSI_ASC_LUN_NOT_READY,
+					    SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+	else
+		res = nvme_trans_completion(hdr, SAM_STAT_GOOD, NO_SENSE, 0, 0);
+
+	return res;
+}
+
+static int nvme_trans_write_buffer(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+							u8 *cmd)
+{
+	int res = SNTI_TRANSLATION_SUCCESS;
+	u32 buffer_offset, parm_list_length;
+	u8 buffer_id, mode;
+
+	parm_list_length =
+		GET_U24_FROM_CDB(cmd, WRITE_BUFFER_CDB_PARM_LIST_LENGTH_OFFSET);
+	if (parm_list_length % BYTES_TO_DWORDS != 0) {
+		/* NVMe expects Firmware file to be a whole number of DWORDS */
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		goto out;
+	}
+	buffer_id = GET_U8_FROM_CDB(cmd, WRITE_BUFFER_CDB_BUFFER_ID_OFFSET);
+	if (buffer_id > NVME_MAX_FIRMWARE_SLOT) {
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		goto out;
+	}
+	mode = GET_U8_FROM_CDB(cmd, WRITE_BUFFER_CDB_MODE_OFFSET) &
+						WRITE_BUFFER_CDB_MODE_MASK;
+	buffer_offset =
+		GET_U24_FROM_CDB(cmd, WRITE_BUFFER_CDB_BUFFER_OFFSET_OFFSET);
+
+	switch (mode) {
+	case DOWNLOAD_SAVE_ACTIVATE:
+		res = nvme_trans_send_fw_cmd(ns, hdr, nvme_admin_download_fw,
+						parm_list_length, buffer_offset,
+						buffer_id);
+		if (res != SNTI_TRANSLATION_SUCCESS)
+			goto out;
+		res = nvme_trans_send_fw_cmd(ns, hdr, nvme_admin_activate_fw,
+						parm_list_length, buffer_offset,
+						buffer_id);
+		break;
+	case DOWNLOAD_SAVE_DEFER_ACTIVATE:
+		res = nvme_trans_send_fw_cmd(ns, hdr, nvme_admin_download_fw,
+						parm_list_length, buffer_offset,
+						buffer_id);
+		break;
+	case ACTIVATE_DEFERRED_MICROCODE:
+		res = nvme_trans_send_fw_cmd(ns, hdr, nvme_admin_activate_fw,
+						parm_list_length, buffer_offset,
+						buffer_id);
+		break;
+	default:
+		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		break;
+	}
+
+ out:
+	return res;
+}
+
+struct scsi_unmap_blk_desc {
+	__be64	slba;
+	__be32	nlb;
+	u32	resv;
+};
+
+struct scsi_unmap_parm_list {
+	__be16	unmap_data_len;
+	__be16	unmap_blk_desc_data_len;
+	u32	resv;
+	struct scsi_unmap_blk_desc desc[0];
+};
+
+static int nvme_trans_unmap(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+							u8 *cmd)
+{
+	struct nvme_dev *dev = ns->dev;
+	struct scsi_unmap_parm_list *plist;
+	struct nvme_dsm_range *range;
+	struct nvme_queue *nvmeq;
+	struct nvme_command c;
+	int i, nvme_sc, res = -ENOMEM;
+	u16 ndesc, list_len;
+	dma_addr_t dma_addr;
+
+	list_len = GET_U16_FROM_CDB(cmd, UNMAP_CDB_PARAM_LIST_LENGTH_OFFSET);
+	if (!list_len)
+		return -EINVAL;
+
+	plist = kmalloc(list_len, GFP_KERNEL);
+	if (!plist)
+		return -ENOMEM;
+
+	res = nvme_trans_copy_from_user(hdr, plist, list_len);
+	if (res != SNTI_TRANSLATION_SUCCESS)
+		goto out;
+
+	ndesc = be16_to_cpu(plist->unmap_blk_desc_data_len) >> 4;
+	if (!ndesc || ndesc > 256) {
+		res = -EINVAL;
+		goto out;
+	}
+
+	range = dma_alloc_coherent(&dev->pci_dev->dev, ndesc * sizeof(*range),
+							&dma_addr, GFP_KERNEL);
+	if (!range)
+		goto out;
+
+	for (i = 0; i < ndesc; i++) {
+		range[i].nlb = cpu_to_le32(be32_to_cpu(plist->desc[i].nlb));
+		range[i].slba = cpu_to_le64(be64_to_cpu(plist->desc[i].slba));
+		range[i].cattr = 0;
+	}
+
+	memset(&c, 0, sizeof(c));
+	c.dsm.opcode = nvme_cmd_dsm;
+	c.dsm.nsid = cpu_to_le32(ns->ns_id);
+	c.dsm.prp1 = cpu_to_le64(dma_addr);
+	c.dsm.nr = cpu_to_le32(ndesc - 1);
+	c.dsm.attributes = cpu_to_le32(NVME_DSMGMT_AD);
+
+	nvmeq = get_nvmeq(dev);
+	put_nvmeq(nvmeq);
+
+	nvme_sc = nvme_submit_sync_cmd(nvmeq, &c, NULL, NVME_IO_TIMEOUT);
+	res = nvme_trans_status_code(hdr, nvme_sc);
+
+	dma_free_coherent(&dev->pci_dev->dev, ndesc * sizeof(*range),
+							range, dma_addr);
+ out:
+	kfree(plist);
+	return res;
+}
+
+static int nvme_scsi_translate(struct nvme_ns *ns, struct sg_io_hdr *hdr)
+{
+	u8 cmd[BLK_MAX_CDB];
+	int retcode;
+	unsigned int opcode;
+
+	if (hdr->cmdp == NULL)
+		return -EMSGSIZE;
+	if (copy_from_user(cmd, hdr->cmdp, hdr->cmd_len))
+		return -EFAULT;
+
+	opcode = cmd[0];
+
+	switch (opcode) {
+	case READ_6:
+	case READ_10:
+	case READ_12:
+	case READ_16:
+		retcode = nvme_trans_io(ns, hdr, 0, cmd);
+		break;
+	case WRITE_6:
+	case WRITE_10:
+	case WRITE_12:
+	case WRITE_16:
+		retcode = nvme_trans_io(ns, hdr, 1, cmd);
+		break;
+	case INQUIRY:
+		retcode = nvme_trans_inquiry(ns, hdr, cmd);
+		break;
+	case LOG_SENSE:
+		retcode = nvme_trans_log_sense(ns, hdr, cmd);
+		break;
+	case MODE_SELECT:
+	case MODE_SELECT_10:
+		retcode = nvme_trans_mode_select(ns, hdr, cmd);
+		break;
+	case MODE_SENSE:
+	case MODE_SENSE_10:
+		retcode = nvme_trans_mode_sense(ns, hdr, cmd);
+		break;
+	case READ_CAPACITY:
+		retcode = nvme_trans_read_capacity(ns, hdr, cmd);
+		break;
+	case SERVICE_ACTION_IN:
+		if (IS_READ_CAP_16(cmd))
+			retcode = nvme_trans_read_capacity(ns, hdr, cmd);
+		else
+			goto out;
+		break;
+	case REPORT_LUNS:
+		retcode = nvme_trans_report_luns(ns, hdr, cmd);
+		break;
+	case REQUEST_SENSE:
+		retcode = nvme_trans_request_sense(ns, hdr, cmd);
+		break;
+	case SECURITY_PROTOCOL_IN:
+	case SECURITY_PROTOCOL_OUT:
+		retcode = nvme_trans_security_protocol(ns, hdr, cmd);
+		break;
+	case START_STOP:
+		retcode = nvme_trans_start_stop(ns, hdr, cmd);
+		break;
+	case SYNCHRONIZE_CACHE:
+		retcode = nvme_trans_synchronize_cache(ns, hdr, cmd);
+		break;
+	case FORMAT_UNIT:
+		retcode = nvme_trans_format_unit(ns, hdr, cmd);
+		break;
+	case TEST_UNIT_READY:
+		retcode = nvme_trans_test_unit_ready(ns, hdr, cmd);
+		break;
+	case WRITE_BUFFER:
+		retcode = nvme_trans_write_buffer(ns, hdr, cmd);
+		break;
+	case UNMAP:
+		retcode = nvme_trans_unmap(ns, hdr, cmd);
+		break;
+	default:
+ out:
+		retcode = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+				ILLEGAL_REQUEST, SCSI_ASC_ILLEGAL_COMMAND,
+				SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		break;
+	}
+	return retcode;
+}
+
+int nvme_sg_io(struct nvme_ns *ns, struct sg_io_hdr __user *u_hdr)
+{
+	struct sg_io_hdr hdr;
+	int retcode;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EACCES;
+	if (copy_from_user(&hdr, u_hdr, sizeof(hdr)))
+		return -EFAULT;
+	if (hdr.interface_id != 'S')
+		return -EINVAL;
+	if (hdr.cmd_len > BLK_MAX_CDB)
+		return -EINVAL;
+
+	retcode = nvme_scsi_translate(ns, &hdr);
+	if (retcode < 0)
+		return retcode;
+	if (retcode > 0)
+		retcode = SNTI_TRANSLATION_SUCCESS;
+	if (copy_to_user(u_hdr, &hdr, sizeof(sg_io_hdr_t)) > 0)
+		return -EFAULT;
+
+	return retcode;
+}
+
+int nvme_sg_get_version_num(int __user *ip)
+{
+	return put_user(sg_version_num, ip);
+}
diff --git a/drivers/block/paride/pcd.c b/drivers/block/paride/pcd.c
index ba2b6b5e5910..e76bdc074dbe 100644
--- a/drivers/block/paride/pcd.c
+++ b/drivers/block/paride/pcd.c
@@ -236,13 +236,12 @@ static int pcd_block_open(struct block_device *bdev, fmode_t mode)
 	return ret;
 }
 
-static int pcd_block_release(struct gendisk *disk, fmode_t mode)
+static void pcd_block_release(struct gendisk *disk, fmode_t mode)
 {
 	struct pcd_unit *cd = disk->private_data;
 	mutex_lock(&pcd_mutex);
 	cdrom_release(&cd->info, mode);
 	mutex_unlock(&pcd_mutex);
-	return 0;
 }
 
 static int pcd_block_ioctl(struct block_device *bdev, fmode_t mode,
diff --git a/drivers/block/paride/pd.c b/drivers/block/paride/pd.c
index 831e3ac156e6..19ad8f0c83ef 100644
--- a/drivers/block/paride/pd.c
+++ b/drivers/block/paride/pd.c
@@ -783,7 +783,7 @@ static int pd_ioctl(struct block_device *bdev, fmode_t mode,
 	}
 }
 
-static int pd_release(struct gendisk *p, fmode_t mode)
+static void pd_release(struct gendisk *p, fmode_t mode)
 {
 	struct pd_unit *disk = p->private_data;
 
@@ -791,8 +791,6 @@ static int pd_release(struct gendisk *p, fmode_t mode)
 	if (!--disk->access && disk->removable)
 		pd_special_command(disk, pd_door_unlock);
 	mutex_unlock(&pd_mutex);
-
-	return 0;
 }
 
 static unsigned int pd_check_events(struct gendisk *p, unsigned int clearing)
diff --git a/drivers/block/paride/pf.c b/drivers/block/paride/pf.c
index ec8f9ed6326e..f5c86d523ba0 100644
--- a/drivers/block/paride/pf.c
+++ b/drivers/block/paride/pf.c
@@ -211,7 +211,7 @@ static int pf_ioctl(struct block_device *bdev, fmode_t mode,
 		    unsigned int cmd, unsigned long arg);
 static int pf_getgeo(struct block_device *bdev, struct hd_geometry *geo);
 
-static int pf_release(struct gendisk *disk, fmode_t mode);
+static void pf_release(struct gendisk *disk, fmode_t mode);
 
 static int pf_detect(void);
 static void do_pf_read(void);
@@ -360,14 +360,15 @@ static int pf_ioctl(struct block_device *bdev, fmode_t mode, unsigned int cmd, u
 	return 0;
 }
 
-static int pf_release(struct gendisk *disk, fmode_t mode)
+static void pf_release(struct gendisk *disk, fmode_t mode)
 {
 	struct pf_unit *pf = disk->private_data;
 
 	mutex_lock(&pf_mutex);
 	if (pf->access <= 0) {
 		mutex_unlock(&pf_mutex);
-		return -EINVAL;
+		WARN_ON(1);
+		return;
 	}
 
 	pf->access--;
@@ -376,8 +377,6 @@ static int pf_release(struct gendisk *disk, fmode_t mode)
 		pf_lock(pf, 0);
 
 	mutex_unlock(&pf_mutex);
-	return 0;
-
 }
 
 static unsigned int pf_check_events(struct gendisk *disk, unsigned int clearing)
diff --git a/drivers/block/pktcdvd.c b/drivers/block/pktcdvd.c
index 2e7de7a59bfc..3c08983e600a 100644
--- a/drivers/block/pktcdvd.c
+++ b/drivers/block/pktcdvd.c
@@ -901,7 +901,7 @@ static void pkt_iosched_process_queue(struct pktcdvd_device *pd)
 			pd->iosched.successive_reads += bio->bi_size >> 10;
 		else {
 			pd->iosched.successive_reads = 0;
-			pd->iosched.last_write = bio->bi_sector + bio_sectors(bio);
+			pd->iosched.last_write = bio_end_sector(bio);
 		}
 		if (pd->iosched.successive_reads >= HI_SPEED_SWITCH) {
 			if (pd->read_speed == pd->write_speed) {
@@ -948,31 +948,6 @@ static int pkt_set_segment_merging(struct pktcdvd_device *pd, struct request_que
 }
 
 /*
- * Copy CD_FRAMESIZE bytes from src_bio into a destination page
- */
-static void pkt_copy_bio_data(struct bio *src_bio, int seg, int offs, struct page *dst_page, int dst_offs)
-{
-	unsigned int copy_size = CD_FRAMESIZE;
-
-	while (copy_size > 0) {
-		struct bio_vec *src_bvl = bio_iovec_idx(src_bio, seg);
-		void *vfrom = kmap_atomic(src_bvl->bv_page) +
-			src_bvl->bv_offset + offs;
-		void *vto = page_address(dst_page) + dst_offs;
-		int len = min_t(int, copy_size, src_bvl->bv_len - offs);
-
-		BUG_ON(len < 0);
-		memcpy(vto, vfrom, len);
-		kunmap_atomic(vfrom);
-
-		seg++;
-		offs = 0;
-		dst_offs += len;
-		copy_size -= len;
-	}
-}
-
-/*
  * Copy all data for this packet to pkt->pages[], so that
  * a) The number of required segments for the write bio is minimized, which
  *    is necessary for some scsi controllers.
@@ -1181,16 +1156,15 @@ static int pkt_start_recovery(struct packet_data *pkt)
 	new_sector = new_block * (CD_FRAMESIZE >> 9);
 	pkt->sector = new_sector;
 
+	bio_reset(pkt->bio);
+	pkt->bio->bi_bdev = pd->bdev;
+	pkt->bio->bi_rw = REQ_WRITE;
 	pkt->bio->bi_sector = new_sector;
-	pkt->bio->bi_next = NULL;
-	pkt->bio->bi_flags = 1 << BIO_UPTODATE;
-	pkt->bio->bi_idx = 0;
+	pkt->bio->bi_size = pkt->frames * CD_FRAMESIZE;
+	pkt->bio->bi_vcnt = pkt->frames;
 
-	BUG_ON(pkt->bio->bi_rw != REQ_WRITE);
-	BUG_ON(pkt->bio->bi_vcnt != pkt->frames);
-	BUG_ON(pkt->bio->bi_size != pkt->frames * CD_FRAMESIZE);
-	BUG_ON(pkt->bio->bi_end_io != pkt_end_io_packet_write);
-	BUG_ON(pkt->bio->bi_private != pkt);
+	pkt->bio->bi_end_io = pkt_end_io_packet_write;
+	pkt->bio->bi_private = pkt;
 
 	drop_super(sb);
 	return 1;
@@ -1325,55 +1299,35 @@ try_next_bio:
  */
 static void pkt_start_write(struct pktcdvd_device *pd, struct packet_data *pkt)
 {
-	struct bio *bio;
 	int f;
-	int frames_write;
 	struct bio_vec *bvec = pkt->w_bio->bi_io_vec;
 
+	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;
+
+	/* XXX: locking? */
 	for (f = 0; f < pkt->frames; f++) {
 		bvec[f].bv_page = pkt->pages[(f * CD_FRAMESIZE) / PAGE_SIZE];
 		bvec[f].bv_offset = (f * CD_FRAMESIZE) % PAGE_SIZE;
+		if (!bio_add_page(pkt->w_bio, bvec[f].bv_page, CD_FRAMESIZE, bvec[f].bv_offset))
+			BUG();
 	}
+	VPRINTK(DRIVER_NAME": vcnt=%d\n", pkt->w_bio->bi_vcnt);
 
 	/*
 	 * Fill-in bvec with data from orig_bios.
 	 */
-	frames_write = 0;
 	spin_lock(&pkt->lock);
-	bio_list_for_each(bio, &pkt->orig_bios) {
-		int segment = bio->bi_idx;
-		int src_offs = 0;
-		int first_frame = (bio->bi_sector - pkt->sector) / (CD_FRAMESIZE >> 9);
-		int num_frames = bio->bi_size / CD_FRAMESIZE;
-		BUG_ON(first_frame < 0);
-		BUG_ON(first_frame + num_frames > pkt->frames);
-		for (f = first_frame; f < first_frame + num_frames; f++) {
-			struct bio_vec *src_bvl = bio_iovec_idx(bio, segment);
-
-			while (src_offs >= src_bvl->bv_len) {
-				src_offs -= src_bvl->bv_len;
-				segment++;
-				BUG_ON(segment >= bio->bi_vcnt);
-				src_bvl = bio_iovec_idx(bio, segment);
-			}
+	bio_copy_data(pkt->w_bio, pkt->orig_bios.head);
 
-			if (src_bvl->bv_len - src_offs >= CD_FRAMESIZE) {
-				bvec[f].bv_page = src_bvl->bv_page;
-				bvec[f].bv_offset = src_bvl->bv_offset + src_offs;
-			} else {
-				pkt_copy_bio_data(bio, segment, src_offs,
-						  bvec[f].bv_page, bvec[f].bv_offset);
-			}
-			src_offs += CD_FRAMESIZE;
-			frames_write++;
-		}
-	}
 	pkt_set_state(pkt, PACKET_WRITE_WAIT_STATE);
 	spin_unlock(&pkt->lock);
 
 	VPRINTK("pkt_start_write: Writing %d frames for zone %llx\n",
-		frames_write, (unsigned long long)pkt->sector);
-	BUG_ON(frames_write != pkt->write_size);
+		pkt->write_size, (unsigned long long)pkt->sector);
 
 	if (test_bit(PACKET_MERGE_SEGS, &pd->flags) || (pkt->write_size < pkt->frames)) {
 		pkt_make_local_copy(pkt, bvec);
@@ -1383,16 +1337,6 @@ static void pkt_start_write(struct pktcdvd_device *pd, struct packet_data *pkt)
 	}
 
 	/* Start the write request */
-	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;
-	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();
-	VPRINTK(DRIVER_NAME": vcnt=%d\n", pkt->w_bio->bi_vcnt);
-
 	atomic_set(&pkt->io_wait, 1);
 	pkt->w_bio->bi_rw = WRITE;
 	pkt_queue_bio(pd, pkt->w_bio);
@@ -2376,10 +2320,9 @@ out:
 	return ret;
 }
 
-static int pkt_close(struct gendisk *disk, fmode_t mode)
+static void pkt_close(struct gendisk *disk, fmode_t mode)
 {
 	struct pktcdvd_device *pd = disk->private_data;
-	int ret = 0;
 
 	mutex_lock(&pktcdvd_mutex);
 	mutex_lock(&ctl_mutex);
@@ -2391,7 +2334,6 @@ static int pkt_close(struct gendisk *disk, fmode_t mode)
 	}
 	mutex_unlock(&ctl_mutex);
 	mutex_unlock(&pktcdvd_mutex);
-	return ret;
 }
 
 
@@ -2433,7 +2375,7 @@ static void pkt_make_request(struct request_queue *q, struct bio *bio)
 		cloned_bio->bi_bdev = pd->bdev;
 		cloned_bio->bi_private = psd;
 		cloned_bio->bi_end_io = pkt_end_io_read_cloned;
-		pd->stats.secs_r += bio->bi_size >> 9;
+		pd->stats.secs_r += bio_sectors(bio);
 		pkt_queue_bio(pd, cloned_bio);
 		return;
 	}
@@ -2454,7 +2396,7 @@ static void pkt_make_request(struct request_queue *q, struct bio *bio)
 	zone = ZONE(bio->bi_sector, pd);
 	VPRINTK("pkt_make_request: start = %6llx stop = %6llx\n",
 		(unsigned long long)bio->bi_sector,
-		(unsigned long long)(bio->bi_sector + bio_sectors(bio)));
+		(unsigned long long)bio_end_sector(bio));
 
 	/* Check if we have to split the bio */
 	{
@@ -2462,7 +2404,7 @@ static void pkt_make_request(struct request_queue *q, struct bio *bio)
 		sector_t last_zone;
 		int first_sectors;
 
-		last_zone = ZONE(bio->bi_sector + bio_sectors(bio) - 1, pd);
+		last_zone = ZONE(bio_end_sector(bio) - 1, pd);
 		if (last_zone != zone) {
 			BUG_ON(last_zone != zone + pd->settings.size);
 			first_sectors = last_zone - bio->bi_sector;
@@ -2648,7 +2590,7 @@ static int pkt_seq_show(struct seq_file *m, void *p)
 
 static int pkt_seq_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, pkt_seq_show, PDE(inode)->data);
+	return single_open(file, pkt_seq_show, PDE_DATA(inode));
 }
 
 static const struct file_operations pkt_proc_fops = {
diff --git a/drivers/block/ps3vram.c b/drivers/block/ps3vram.c
index 75e112d66006..06a2e53e5f37 100644
--- a/drivers/block/ps3vram.c
+++ b/drivers/block/ps3vram.c
@@ -525,7 +525,7 @@ static int ps3vram_proc_show(struct seq_file *m, void *v)
 
 static int ps3vram_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, ps3vram_proc_show, PDE(inode)->data);
+	return single_open(file, ps3vram_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations ps3vram_proc_fops = {
diff --git a/drivers/block/rbd.c b/drivers/block/rbd.c
index b7b7a88d9f68..ca63104136e0 100644
--- a/drivers/block/rbd.c
+++ b/drivers/block/rbd.c
@@ -1,3 +1,4 @@
+
 /*
    rbd.c -- Export ceph rados objects as a Linux block device
 
@@ -32,12 +33,14 @@
 #include <linux/ceph/mon_client.h>
 #include <linux/ceph/decode.h>
 #include <linux/parser.h>
+#include <linux/bsearch.h>
 
 #include <linux/kernel.h>
 #include <linux/device.h>
 #include <linux/module.h>
 #include <linux/fs.h>
 #include <linux/blkdev.h>
+#include <linux/slab.h>
 
 #include "rbd_types.h"
 
@@ -52,13 +55,6 @@
 #define	SECTOR_SHIFT	9
 #define	SECTOR_SIZE	(1ULL << SECTOR_SHIFT)
 
-/* It might be useful to have these defined elsewhere */
-
-#define	U8_MAX	((u8)	(~0U))
-#define	U16_MAX	((u16)	(~0U))
-#define	U32_MAX	((u32)	(~0U))
-#define	U64_MAX	((u64)	(~0ULL))
-
 #define RBD_DRV_NAME "rbd"
 #define RBD_DRV_NAME_LONG "rbd (rados block device)"
 
@@ -72,6 +68,8 @@
 
 #define RBD_SNAP_HEAD_NAME	"-"
 
+#define	BAD_SNAP_INDEX	U32_MAX		/* invalid index into snap array */
+
 /* This allows a single page to hold an image name sent by OSD */
 #define RBD_IMAGE_NAME_LEN_MAX	(PAGE_SIZE - sizeof (__le32) - 1)
 #define RBD_IMAGE_ID_LEN_MAX	64
@@ -80,11 +78,14 @@
 
 /* Feature bits */
 
-#define RBD_FEATURE_LAYERING      1
+#define RBD_FEATURE_LAYERING	(1<<0)
+#define RBD_FEATURE_STRIPINGV2	(1<<1)
+#define RBD_FEATURES_ALL \
+	    (RBD_FEATURE_LAYERING | RBD_FEATURE_STRIPINGV2)
 
 /* Features supported by this (client software) implementation. */
 
-#define RBD_FEATURES_ALL          (0)
+#define RBD_FEATURES_SUPPORTED	(RBD_FEATURES_ALL)
 
 /*
  * An RBD device name will be "rbd#", where the "rbd" comes from
@@ -112,7 +113,8 @@ struct rbd_image_header {
 	char *snap_names;
 	u64 *snap_sizes;
 
-	u64 obj_version;
+	u64 stripe_unit;
+	u64 stripe_count;
 };
 
 /*
@@ -142,13 +144,13 @@ struct rbd_image_header {
  */
 struct rbd_spec {
 	u64		pool_id;
-	char		*pool_name;
+	const char	*pool_name;
 
-	char		*image_id;
-	char		*image_name;
+	const char	*image_id;
+	const char	*image_name;
 
 	u64		snap_id;
-	char		*snap_name;
+	const char	*snap_name;
 
 	struct kref	kref;
 };
@@ -174,13 +176,44 @@ enum obj_request_type {
 	OBJ_REQUEST_NODATA, OBJ_REQUEST_BIO, OBJ_REQUEST_PAGES
 };
 
+enum obj_req_flags {
+	OBJ_REQ_DONE,		/* completion flag: not done = 0, done = 1 */
+	OBJ_REQ_IMG_DATA,	/* object usage: standalone = 0, image = 1 */
+	OBJ_REQ_KNOWN,		/* EXISTS flag valid: no = 0, yes = 1 */
+	OBJ_REQ_EXISTS,		/* target exists: no = 0, yes = 1 */
+};
+
 struct rbd_obj_request {
 	const char		*object_name;
 	u64			offset;		/* object start byte */
 	u64			length;		/* bytes from offset */
+	unsigned long		flags;
 
-	struct rbd_img_request	*img_request;
-	struct list_head	links;		/* img_request->obj_requests */
+	/*
+	 * An object request associated with an image will have its
+	 * img_data flag set; a standalone object request will not.
+	 *
+	 * A standalone object request will have which == BAD_WHICH
+	 * and a null obj_request pointer.
+	 *
+	 * An object request initiated in support of a layered image
+	 * object (to check for its existence before a write) will
+	 * have which == BAD_WHICH and a non-null obj_request pointer.
+	 *
+	 * Finally, an object request for rbd image data will have
+	 * which != BAD_WHICH, and will have a non-null img_request
+	 * pointer.  The value of which will be in the range
+	 * 0..(img_request->obj_request_count-1).
+	 */
+	union {
+		struct rbd_obj_request	*obj_request;	/* STAT op */
+		struct {
+			struct rbd_img_request	*img_request;
+			u64			img_offset;
+			/* links for img_request->obj_requests list */
+			struct list_head	links;
+		};
+	};
 	u32			which;		/* posn image request list */
 
 	enum obj_request_type	type;
@@ -191,13 +224,12 @@ struct rbd_obj_request {
 			u32		page_count;
 		};
 	};
+	struct page		**copyup_pages;
 
 	struct ceph_osd_request	*osd_req;
 
 	u64			xferred;	/* bytes transferred */
-	u64			version;
 	int			result;
-	atomic_t		done;
 
 	rbd_obj_callback_t	callback;
 	struct completion	completion;
@@ -205,19 +237,31 @@ struct rbd_obj_request {
 	struct kref		kref;
 };
 
+enum img_req_flags {
+	IMG_REQ_WRITE,		/* I/O direction: read = 0, write = 1 */
+	IMG_REQ_CHILD,		/* initiator: block = 0, child image = 1 */
+	IMG_REQ_LAYERED,	/* ENOENT handling: normal = 0, layered = 1 */
+};
+
 struct rbd_img_request {
-	struct request		*rq;
 	struct rbd_device	*rbd_dev;
 	u64			offset;	/* starting image byte offset */
 	u64			length;	/* byte count from offset */
-	bool			write_request;	/* false for read */
+	unsigned long		flags;
 	union {
+		u64			snap_id;	/* for reads */
 		struct ceph_snap_context *snapc;	/* for writes */
-		u64		snap_id;		/* for reads */
 	};
+	union {
+		struct request		*rq;		/* block request */
+		struct rbd_obj_request	*obj_request;	/* obj req initiator */
+	};
+	struct page		**copyup_pages;
 	spinlock_t		completion_lock;/* protects next_completion */
 	u32			next_completion;
 	rbd_img_callback_t	callback;
+	u64			xferred;/* aggregate bytes transferred */
+	int			result;	/* first nonzero obj_request result */
 
 	u32			obj_request_count;
 	struct list_head	obj_requests;	/* rbd_obj_request structs */
@@ -232,15 +276,6 @@ struct rbd_img_request {
 #define for_each_obj_request_safe(ireq, oreq, n) \
 	list_for_each_entry_safe_reverse(oreq, n, &(ireq)->obj_requests, links)
 
-struct rbd_snap {
-	struct	device		dev;
-	const char		*name;
-	u64			size;
-	struct list_head	node;
-	u64			id;
-	u64			features;
-};
-
 struct rbd_mapping {
 	u64                     size;
 	u64                     features;
@@ -276,6 +311,7 @@ struct rbd_device {
 
 	struct rbd_spec		*parent_spec;
 	u64			parent_overlap;
+	struct rbd_device	*parent;
 
 	/* protects updating the header */
 	struct rw_semaphore     header_rwsem;
@@ -284,9 +320,6 @@ struct rbd_device {
 
 	struct list_head	node;
 
-	/* list of snapshots */
-	struct list_head	snaps;
-
 	/* sysfs related */
 	struct device		dev;
 	unsigned long		open_count;	/* protected by lock */
@@ -312,16 +345,21 @@ static DEFINE_SPINLOCK(rbd_dev_list_lock);
 static LIST_HEAD(rbd_client_list);		/* clients */
 static DEFINE_SPINLOCK(rbd_client_list_lock);
 
-static int rbd_dev_snaps_update(struct rbd_device *rbd_dev);
-static int rbd_dev_snaps_register(struct rbd_device *rbd_dev);
+/* Slab caches for frequently-allocated structures */
+
+static struct kmem_cache	*rbd_img_request_cache;
+static struct kmem_cache	*rbd_obj_request_cache;
+static struct kmem_cache	*rbd_segment_name_cache;
 
-static void rbd_dev_release(struct device *dev);
-static void rbd_remove_snap_dev(struct rbd_snap *snap);
+static int rbd_img_request_submit(struct rbd_img_request *img_request);
+
+static void rbd_dev_device_release(struct device *dev);
 
 static ssize_t rbd_add(struct bus_type *bus, const char *buf,
 		       size_t count);
 static ssize_t rbd_remove(struct bus_type *bus, const char *buf,
 			  size_t count);
+static int rbd_dev_image_probe(struct rbd_device *rbd_dev);
 
 static struct bus_attribute rbd_bus_attrs[] = {
 	__ATTR(add, S_IWUSR, NULL, rbd_add),
@@ -383,8 +421,19 @@ void rbd_warn(struct rbd_device *rbd_dev, const char *fmt, ...)
 #  define rbd_assert(expr)	((void) 0)
 #endif /* !RBD_DEBUG */
 
-static int rbd_dev_refresh(struct rbd_device *rbd_dev, u64 *hver);
-static int rbd_dev_v2_refresh(struct rbd_device *rbd_dev, u64 *hver);
+static int rbd_img_obj_request_submit(struct rbd_obj_request *obj_request);
+static void rbd_img_parent_read(struct rbd_obj_request *obj_request);
+static void rbd_dev_remove_parent(struct rbd_device *rbd_dev);
+
+static int rbd_dev_refresh(struct rbd_device *rbd_dev);
+static int rbd_dev_v2_refresh(struct rbd_device *rbd_dev);
+static const char *rbd_dev_v2_snap_name(struct rbd_device *rbd_dev,
+					u64 snap_id);
+static int _rbd_dev_v2_snap_size(struct rbd_device *rbd_dev, u64 snap_id,
+				u8 *order, u64 *snap_size);
+static int _rbd_dev_v2_snap_features(struct rbd_device *rbd_dev, u64 snap_id,
+		u64 *snap_features);
+static u64 rbd_snap_id_by_name(struct rbd_device *rbd_dev, const char *name);
 
 static int rbd_open(struct block_device *bdev, fmode_t mode)
 {
@@ -411,7 +460,7 @@ static int rbd_open(struct block_device *bdev, fmode_t mode)
 	return 0;
 }
 
-static int rbd_release(struct gendisk *disk, fmode_t mode)
+static void rbd_release(struct gendisk *disk, fmode_t mode)
 {
 	struct rbd_device *rbd_dev = disk->private_data;
 	unsigned long open_count_before;
@@ -424,8 +473,6 @@ static int rbd_release(struct gendisk *disk, fmode_t mode)
 	mutex_lock_nested(&ctl_mutex, SINGLE_DEPTH_NESTING);
 	put_device(&rbd_dev->dev);
 	mutex_unlock(&ctl_mutex);
-
-	return 0;
 }
 
 static const struct block_device_operations rbd_bd_ops = {
@@ -484,6 +531,13 @@ out_opt:
 	return ERR_PTR(ret);
 }
 
+static struct rbd_client *__rbd_get_client(struct rbd_client *rbdc)
+{
+	kref_get(&rbdc->kref);
+
+	return rbdc;
+}
+
 /*
  * Find a ceph client with specific addr and configuration.  If
  * found, bump its reference count.
@@ -499,7 +553,8 @@ static struct rbd_client *rbd_client_find(struct ceph_options *ceph_opts)
 	spin_lock(&rbd_client_list_lock);
 	list_for_each_entry(client_node, &rbd_client_list, node) {
 		if (!ceph_compare_options(ceph_opts, client_node->client)) {
-			kref_get(&client_node->kref);
+			__rbd_get_client(client_node);
+
 			found = true;
 			break;
 		}
@@ -722,7 +777,6 @@ static int rbd_header_from_disk(struct rbd_image_header *header,
 			header->snap_sizes[i] =
 				le64_to_cpu(ondisk->snaps[i].image_size);
 	} else {
-		WARN_ON(ondisk->snap_names_len);
 		header->snap_names = NULL;
 		header->snap_sizes = NULL;
 	}
@@ -735,18 +789,13 @@ static int rbd_header_from_disk(struct rbd_image_header *header,
 	/* Allocate and fill in the snapshot context */
 
 	header->image_size = le64_to_cpu(ondisk->image_size);
-	size = sizeof (struct ceph_snap_context);
-	size += snap_count * sizeof (header->snapc->snaps[0]);
-	header->snapc = kzalloc(size, GFP_KERNEL);
+
+	header->snapc = ceph_create_snap_context(snap_count, GFP_KERNEL);
 	if (!header->snapc)
 		goto out_err;
-
-	atomic_set(&header->snapc->nref, 1);
 	header->snapc->seq = le64_to_cpu(ondisk->snap_seq);
-	header->snapc->num_snaps = snap_count;
 	for (i = 0; i < snap_count; i++)
-		header->snapc->snaps[i] =
-			le64_to_cpu(ondisk->snaps[i].id);
+		header->snapc->snaps[i] = le64_to_cpu(ondisk->snaps[i].id);
 
 	return 0;
 
@@ -761,70 +810,174 @@ out_err:
 	return -ENOMEM;
 }
 
-static const char *rbd_snap_name(struct rbd_device *rbd_dev, u64 snap_id)
+static const char *_rbd_dev_v1_snap_name(struct rbd_device *rbd_dev, u32 which)
+{
+	const char *snap_name;
+
+	rbd_assert(which < rbd_dev->header.snapc->num_snaps);
+
+	/* Skip over names until we find the one we are looking for */
+
+	snap_name = rbd_dev->header.snap_names;
+	while (which--)
+		snap_name += strlen(snap_name) + 1;
+
+	return kstrdup(snap_name, GFP_KERNEL);
+}
+
+/*
+ * Snapshot id comparison function for use with qsort()/bsearch().
+ * Note that result is for snapshots in *descending* order.
+ */
+static int snapid_compare_reverse(const void *s1, const void *s2)
+{
+	u64 snap_id1 = *(u64 *)s1;
+	u64 snap_id2 = *(u64 *)s2;
+
+	if (snap_id1 < snap_id2)
+		return 1;
+	return snap_id1 == snap_id2 ? 0 : -1;
+}
+
+/*
+ * Search a snapshot context to see if the given snapshot id is
+ * present.
+ *
+ * Returns the position of the snapshot id in the array if it's found,
+ * or BAD_SNAP_INDEX otherwise.
+ *
+ * Note: The snapshot array is in kept sorted (by the osd) in
+ * reverse order, highest snapshot id first.
+ */
+static u32 rbd_dev_snap_index(struct rbd_device *rbd_dev, u64 snap_id)
 {
-	struct rbd_snap *snap;
+	struct ceph_snap_context *snapc = rbd_dev->header.snapc;
+	u64 *found;
 
+	found = bsearch(&snap_id, &snapc->snaps, snapc->num_snaps,
+				sizeof (snap_id), snapid_compare_reverse);
+
+	return found ? (u32)(found - &snapc->snaps[0]) : BAD_SNAP_INDEX;
+}
+
+static const char *rbd_dev_v1_snap_name(struct rbd_device *rbd_dev,
+					u64 snap_id)
+{
+	u32 which;
+
+	which = rbd_dev_snap_index(rbd_dev, snap_id);
+	if (which == BAD_SNAP_INDEX)
+		return NULL;
+
+	return _rbd_dev_v1_snap_name(rbd_dev, which);
+}
+
+static const char *rbd_snap_name(struct rbd_device *rbd_dev, u64 snap_id)
+{
 	if (snap_id == CEPH_NOSNAP)
 		return RBD_SNAP_HEAD_NAME;
 
-	list_for_each_entry(snap, &rbd_dev->snaps, node)
-		if (snap_id == snap->id)
-			return snap->name;
+	rbd_assert(rbd_image_format_valid(rbd_dev->image_format));
+	if (rbd_dev->image_format == 1)
+		return rbd_dev_v1_snap_name(rbd_dev, snap_id);
 
-	return NULL;
+	return rbd_dev_v2_snap_name(rbd_dev, snap_id);
 }
 
-static int snap_by_name(struct rbd_device *rbd_dev, const char *snap_name)
+static int rbd_snap_size(struct rbd_device *rbd_dev, u64 snap_id,
+				u64 *snap_size)
 {
+	rbd_assert(rbd_image_format_valid(rbd_dev->image_format));
+	if (snap_id == CEPH_NOSNAP) {
+		*snap_size = rbd_dev->header.image_size;
+	} else if (rbd_dev->image_format == 1) {
+		u32 which;
 
-	struct rbd_snap *snap;
+		which = rbd_dev_snap_index(rbd_dev, snap_id);
+		if (which == BAD_SNAP_INDEX)
+			return -ENOENT;
 
-	list_for_each_entry(snap, &rbd_dev->snaps, node) {
-		if (!strcmp(snap_name, snap->name)) {
-			rbd_dev->spec->snap_id = snap->id;
-			rbd_dev->mapping.size = snap->size;
-			rbd_dev->mapping.features = snap->features;
+		*snap_size = rbd_dev->header.snap_sizes[which];
+	} else {
+		u64 size = 0;
+		int ret;
 
-			return 0;
-		}
+		ret = _rbd_dev_v2_snap_size(rbd_dev, snap_id, NULL, &size);
+		if (ret)
+			return ret;
+
+		*snap_size = size;
 	}
+	return 0;
+}
+
+static int rbd_snap_features(struct rbd_device *rbd_dev, u64 snap_id,
+			u64 *snap_features)
+{
+	rbd_assert(rbd_image_format_valid(rbd_dev->image_format));
+	if (snap_id == CEPH_NOSNAP) {
+		*snap_features = rbd_dev->header.features;
+	} else if (rbd_dev->image_format == 1) {
+		*snap_features = 0;	/* No features for format 1 */
+	} else {
+		u64 features = 0;
+		int ret;
 
-	return -ENOENT;
+		ret = _rbd_dev_v2_snap_features(rbd_dev, snap_id, &features);
+		if (ret)
+			return ret;
+
+		*snap_features = features;
+	}
+	return 0;
 }
 
-static int rbd_dev_set_mapping(struct rbd_device *rbd_dev)
+static int rbd_dev_mapping_set(struct rbd_device *rbd_dev)
 {
+	const char *snap_name = rbd_dev->spec->snap_name;
+	u64 snap_id;
+	u64 size = 0;
+	u64 features = 0;
 	int ret;
 
-	if (!memcmp(rbd_dev->spec->snap_name, RBD_SNAP_HEAD_NAME,
-		    sizeof (RBD_SNAP_HEAD_NAME))) {
-		rbd_dev->spec->snap_id = CEPH_NOSNAP;
-		rbd_dev->mapping.size = rbd_dev->header.image_size;
-		rbd_dev->mapping.features = rbd_dev->header.features;
-		ret = 0;
+	if (strcmp(snap_name, RBD_SNAP_HEAD_NAME)) {
+		snap_id = rbd_snap_id_by_name(rbd_dev, snap_name);
+		if (snap_id == CEPH_NOSNAP)
+			return -ENOENT;
 	} else {
-		ret = snap_by_name(rbd_dev, rbd_dev->spec->snap_name);
-		if (ret < 0)
-			goto done;
-		rbd_dev->mapping.read_only = true;
+		snap_id = CEPH_NOSNAP;
 	}
-	set_bit(RBD_DEV_FLAG_EXISTS, &rbd_dev->flags);
 
-done:
-	return ret;
+	ret = rbd_snap_size(rbd_dev, snap_id, &size);
+	if (ret)
+		return ret;
+	ret = rbd_snap_features(rbd_dev, snap_id, &features);
+	if (ret)
+		return ret;
+
+	rbd_dev->mapping.size = size;
+	rbd_dev->mapping.features = features;
+
+	/* If we are mapping a snapshot it must be marked read-only */
+
+	if (snap_id != CEPH_NOSNAP)
+		rbd_dev->mapping.read_only = true;
+
+	return 0;
 }
 
-static void rbd_header_free(struct rbd_image_header *header)
+static void rbd_dev_mapping_clear(struct rbd_device *rbd_dev)
 {
-	kfree(header->object_prefix);
-	header->object_prefix = NULL;
-	kfree(header->snap_sizes);
-	header->snap_sizes = NULL;
-	kfree(header->snap_names);
-	header->snap_names = NULL;
-	ceph_put_snap_context(header->snapc);
-	header->snapc = NULL;
+	rbd_dev->mapping.size = 0;
+	rbd_dev->mapping.features = 0;
+	rbd_dev->mapping.read_only = true;
+}
+
+static void rbd_dev_clear_mapping(struct rbd_device *rbd_dev)
+{
+	rbd_dev->mapping.size = 0;
+	rbd_dev->mapping.features = 0;
+	rbd_dev->mapping.read_only = true;
 }
 
 static const char *rbd_segment_name(struct rbd_device *rbd_dev, u64 offset)
@@ -833,7 +986,7 @@ static const char *rbd_segment_name(struct rbd_device *rbd_dev, u64 offset)
 	u64 segment;
 	int ret;
 
-	name = kmalloc(MAX_OBJ_NAME_SIZE + 1, GFP_NOIO);
+	name = kmem_cache_alloc(rbd_segment_name_cache, GFP_NOIO);
 	if (!name)
 		return NULL;
 	segment = offset >> rbd_dev->header.obj_order;
@@ -849,6 +1002,13 @@ static const char *rbd_segment_name(struct rbd_device *rbd_dev, u64 offset)
 	return name;
 }
 
+static void rbd_segment_name_free(const char *name)
+{
+	/* The explicit cast here is needed to drop the const qualifier */
+
+	kmem_cache_free(rbd_segment_name_cache, (void *)name);
+}
+
 static u64 rbd_segment_offset(struct rbd_device *rbd_dev, u64 offset)
 {
 	u64 segment_size = (u64) 1 << rbd_dev->header.obj_order;
@@ -921,6 +1081,37 @@ static void zero_bio_chain(struct bio *chain, int start_ofs)
 }
 
 /*
+ * similar to zero_bio_chain(), zeros data defined by a page array,
+ * starting at the given byte offset from the start of the array and
+ * continuing up to the given end offset.  The pages array is
+ * assumed to be big enough to hold all bytes up to the end.
+ */
+static void zero_pages(struct page **pages, u64 offset, u64 end)
+{
+	struct page **page = &pages[offset >> PAGE_SHIFT];
+
+	rbd_assert(end > offset);
+	rbd_assert(end - offset <= (u64)SIZE_MAX);
+	while (offset < end) {
+		size_t page_offset;
+		size_t length;
+		unsigned long flags;
+		void *kaddr;
+
+		page_offset = (size_t)(offset & ~PAGE_MASK);
+		length = min(PAGE_SIZE - page_offset, (size_t)(end - offset));
+		local_irq_save(flags);
+		kaddr = kmap_atomic(*page);
+		memset(kaddr + page_offset, 0, length);
+		kunmap_atomic(kaddr);
+		local_irq_restore(flags);
+
+		offset += length;
+		page++;
+	}
+}
+
+/*
  * Clone a portion of a bio, starting at the given byte offset
  * and continuing for the number of bytes indicated.
  */
@@ -952,7 +1143,7 @@ static struct bio *bio_clone_range(struct bio *bio_src,
 	/* Find first affected segment... */
 
 	resid = offset;
-	__bio_for_each_segment(bv, bio_src, idx, 0) {
+	bio_for_each_segment(bv, bio_src, idx) {
 		if (resid < bv->bv_len)
 			break;
 		resid -= bv->bv_len;
@@ -1064,6 +1255,77 @@ out_err:
 	return NULL;
 }
 
+/*
+ * The default/initial value for all object request flags is 0.  For
+ * each flag, once its value is set to 1 it is never reset to 0
+ * again.
+ */
+static void obj_request_img_data_set(struct rbd_obj_request *obj_request)
+{
+	if (test_and_set_bit(OBJ_REQ_IMG_DATA, &obj_request->flags)) {
+		struct rbd_device *rbd_dev;
+
+		rbd_dev = obj_request->img_request->rbd_dev;
+		rbd_warn(rbd_dev, "obj_request %p already marked img_data\n",
+			obj_request);
+	}
+}
+
+static bool obj_request_img_data_test(struct rbd_obj_request *obj_request)
+{
+	smp_mb();
+	return test_bit(OBJ_REQ_IMG_DATA, &obj_request->flags) != 0;
+}
+
+static void obj_request_done_set(struct rbd_obj_request *obj_request)
+{
+	if (test_and_set_bit(OBJ_REQ_DONE, &obj_request->flags)) {
+		struct rbd_device *rbd_dev = NULL;
+
+		if (obj_request_img_data_test(obj_request))
+			rbd_dev = obj_request->img_request->rbd_dev;
+		rbd_warn(rbd_dev, "obj_request %p already marked done\n",
+			obj_request);
+	}
+}
+
+static bool obj_request_done_test(struct rbd_obj_request *obj_request)
+{
+	smp_mb();
+	return test_bit(OBJ_REQ_DONE, &obj_request->flags) != 0;
+}
+
+/*
+ * This sets the KNOWN flag after (possibly) setting the EXISTS
+ * flag.  The latter is set based on the "exists" value provided.
+ *
+ * Note that for our purposes once an object exists it never goes
+ * away again.  It's possible that the response from two existence
+ * checks are separated by the creation of the target object, and
+ * the first ("doesn't exist") response arrives *after* the second
+ * ("does exist").  In that case we ignore the second one.
+ */
+static void obj_request_existence_set(struct rbd_obj_request *obj_request,
+				bool exists)
+{
+	if (exists)
+		set_bit(OBJ_REQ_EXISTS, &obj_request->flags);
+	set_bit(OBJ_REQ_KNOWN, &obj_request->flags);
+	smp_mb();
+}
+
+static bool obj_request_known_test(struct rbd_obj_request *obj_request)
+{
+	smp_mb();
+	return test_bit(OBJ_REQ_KNOWN, &obj_request->flags) != 0;
+}
+
+static bool obj_request_exists_test(struct rbd_obj_request *obj_request)
+{
+	smp_mb();
+	return test_bit(OBJ_REQ_EXISTS, &obj_request->flags) != 0;
+}
+
 static void rbd_obj_request_get(struct rbd_obj_request *obj_request)
 {
 	dout("%s: obj %p (was %d)\n", __func__, obj_request,
@@ -1101,9 +1363,11 @@ static inline void rbd_img_obj_request_add(struct rbd_img_request *img_request,
 {
 	rbd_assert(obj_request->img_request == NULL);
 
-	rbd_obj_request_get(obj_request);
+	/* Image request now owns object's original reference */
 	obj_request->img_request = img_request;
 	obj_request->which = img_request->obj_request_count;
+	rbd_assert(!obj_request_img_data_test(obj_request));
+	obj_request_img_data_set(obj_request);
 	rbd_assert(obj_request->which != BAD_WHICH);
 	img_request->obj_request_count++;
 	list_add_tail(&obj_request->links, &img_request->obj_requests);
@@ -1123,6 +1387,7 @@ static inline void rbd_img_obj_request_del(struct rbd_img_request *img_request,
 	img_request->obj_request_count--;
 	rbd_assert(obj_request->which == img_request->obj_request_count);
 	obj_request->which = BAD_WHICH;
+	rbd_assert(obj_request_img_data_test(obj_request));
 	rbd_assert(obj_request->img_request == img_request);
 	obj_request->img_request = NULL;
 	obj_request->callback = NULL;
@@ -1141,76 +1406,6 @@ static bool obj_request_type_valid(enum obj_request_type type)
 	}
 }
 
-static struct ceph_osd_req_op *rbd_osd_req_op_create(u16 opcode, ...)
-{
-	struct ceph_osd_req_op *op;
-	va_list args;
-	size_t size;
-
-	op = kzalloc(sizeof (*op), GFP_NOIO);
-	if (!op)
-		return NULL;
-	op->op = opcode;
-	va_start(args, opcode);
-	switch (opcode) {
-	case CEPH_OSD_OP_READ:
-	case CEPH_OSD_OP_WRITE:
-		/* rbd_osd_req_op_create(READ, offset, length) */
-		/* rbd_osd_req_op_create(WRITE, offset, length) */
-		op->extent.offset = va_arg(args, u64);
-		op->extent.length = va_arg(args, u64);
-		if (opcode == CEPH_OSD_OP_WRITE)
-			op->payload_len = op->extent.length;
-		break;
-	case CEPH_OSD_OP_STAT:
-		break;
-	case CEPH_OSD_OP_CALL:
-		/* rbd_osd_req_op_create(CALL, class, method, data, datalen) */
-		op->cls.class_name = va_arg(args, char *);
-		size = strlen(op->cls.class_name);
-		rbd_assert(size <= (size_t) U8_MAX);
-		op->cls.class_len = size;
-		op->payload_len = size;
-
-		op->cls.method_name = va_arg(args, char *);
-		size = strlen(op->cls.method_name);
-		rbd_assert(size <= (size_t) U8_MAX);
-		op->cls.method_len = size;
-		op->payload_len += size;
-
-		op->cls.argc = 0;
-		op->cls.indata = va_arg(args, void *);
-		size = va_arg(args, size_t);
-		rbd_assert(size <= (size_t) U32_MAX);
-		op->cls.indata_len = (u32) size;
-		op->payload_len += size;
-		break;
-	case CEPH_OSD_OP_NOTIFY_ACK:
-	case CEPH_OSD_OP_WATCH:
-		/* rbd_osd_req_op_create(NOTIFY_ACK, cookie, version) */
-		/* rbd_osd_req_op_create(WATCH, cookie, version, flag) */
-		op->watch.cookie = va_arg(args, u64);
-		op->watch.ver = va_arg(args, u64);
-		op->watch.ver = cpu_to_le64(op->watch.ver);
-		if (opcode == CEPH_OSD_OP_WATCH && va_arg(args, int))
-			op->watch.flag = (u8) 1;
-		break;
-	default:
-		rbd_warn(NULL, "unsupported opcode %hu\n", opcode);
-		kfree(op);
-		op = NULL;
-		break;
-	}
-	va_end(args);
-
-	return op;
-}
-
-static void rbd_osd_req_op_destroy(struct ceph_osd_req_op *op)
-{
-	kfree(op);
-}
-
 static int rbd_obj_request_submit(struct ceph_osd_client *osdc,
 				struct rbd_obj_request *obj_request)
 {
@@ -1221,7 +1416,24 @@ static int rbd_obj_request_submit(struct ceph_osd_client *osdc,
 
 static void rbd_img_request_complete(struct rbd_img_request *img_request)
 {
+
 	dout("%s: img %p\n", __func__, img_request);
+
+	/*
+	 * If no error occurred, compute the aggregate transfer
+	 * count for the image request.  We could instead use
+	 * atomic64_cmpxchg() to update it as each object request
+	 * completes; not clear which way is better off hand.
+	 */
+	if (!img_request->result) {
+		struct rbd_obj_request *obj_request;
+		u64 xferred = 0;
+
+		for_each_obj_request(img_request, obj_request)
+			xferred += obj_request->xferred;
+		img_request->xferred = xferred;
+	}
+
 	if (img_request->callback)
 		img_request->callback(img_request);
 	else
@@ -1237,39 +1449,56 @@ static int rbd_obj_request_wait(struct rbd_obj_request *obj_request)
 	return wait_for_completion_interruptible(&obj_request->completion);
 }
 
-static void obj_request_done_init(struct rbd_obj_request *obj_request)
+/*
+ * The default/initial value for all image request flags is 0.  Each
+ * is conditionally set to 1 at image request initialization time
+ * and currently never change thereafter.
+ */
+static void img_request_write_set(struct rbd_img_request *img_request)
 {
-	atomic_set(&obj_request->done, 0);
-	smp_wmb();
+	set_bit(IMG_REQ_WRITE, &img_request->flags);
+	smp_mb();
 }
 
-static void obj_request_done_set(struct rbd_obj_request *obj_request)
+static bool img_request_write_test(struct rbd_img_request *img_request)
 {
-	int done;
+	smp_mb();
+	return test_bit(IMG_REQ_WRITE, &img_request->flags) != 0;
+}
 
-	done = atomic_inc_return(&obj_request->done);
-	if (done > 1) {
-		struct rbd_img_request *img_request = obj_request->img_request;
-		struct rbd_device *rbd_dev;
+static void img_request_child_set(struct rbd_img_request *img_request)
+{
+	set_bit(IMG_REQ_CHILD, &img_request->flags);
+	smp_mb();
+}
 
-		rbd_dev = img_request ? img_request->rbd_dev : NULL;
-		rbd_warn(rbd_dev, "obj_request %p was already done\n",
-			obj_request);
-	}
+static bool img_request_child_test(struct rbd_img_request *img_request)
+{
+	smp_mb();
+	return test_bit(IMG_REQ_CHILD, &img_request->flags) != 0;
 }
 
-static bool obj_request_done_test(struct rbd_obj_request *obj_request)
+static void img_request_layered_set(struct rbd_img_request *img_request)
+{
+	set_bit(IMG_REQ_LAYERED, &img_request->flags);
+	smp_mb();
+}
+
+static bool img_request_layered_test(struct rbd_img_request *img_request)
 {
 	smp_mb();
-	return atomic_read(&obj_request->done) != 0;
+	return test_bit(IMG_REQ_LAYERED, &img_request->flags) != 0;
 }
 
 static void
 rbd_img_obj_request_read_callback(struct rbd_obj_request *obj_request)
 {
+	u64 xferred = obj_request->xferred;
+	u64 length = obj_request->length;
+
 	dout("%s: obj %p img %p result %d %llu/%llu\n", __func__,
 		obj_request, obj_request->img_request, obj_request->result,
-		obj_request->xferred, obj_request->length);
+		xferred, length);
 	/*
 	 * ENOENT means a hole in the image.  We zero-fill the
 	 * entire length of the request.  A short read also implies
@@ -1277,15 +1506,20 @@ rbd_img_obj_request_read_callback(struct rbd_obj_request *obj_request)
 	 * update the xferred count to indicate the whole request
 	 * was satisfied.
 	 */
-	BUG_ON(obj_request->type != OBJ_REQUEST_BIO);
+	rbd_assert(obj_request->type != OBJ_REQUEST_NODATA);
 	if (obj_request->result == -ENOENT) {
-		zero_bio_chain(obj_request->bio_list, 0);
+		if (obj_request->type == OBJ_REQUEST_BIO)
+			zero_bio_chain(obj_request->bio_list, 0);
+		else
+			zero_pages(obj_request->pages, 0, length);
 		obj_request->result = 0;
-		obj_request->xferred = obj_request->length;
-	} else if (obj_request->xferred < obj_request->length &&
-			!obj_request->result) {
-		zero_bio_chain(obj_request->bio_list, obj_request->xferred);
-		obj_request->xferred = obj_request->length;
+		obj_request->xferred = length;
+	} else if (xferred < length && !obj_request->result) {
+		if (obj_request->type == OBJ_REQUEST_BIO)
+			zero_bio_chain(obj_request->bio_list, xferred);
+		else
+			zero_pages(obj_request->pages, xferred, length);
+		obj_request->xferred = length;
 	}
 	obj_request_done_set(obj_request);
 }
@@ -1308,9 +1542,23 @@ static void rbd_osd_trivial_callback(struct rbd_obj_request *obj_request)
 
 static void rbd_osd_read_callback(struct rbd_obj_request *obj_request)
 {
-	dout("%s: obj %p result %d %llu/%llu\n", __func__, obj_request,
-		obj_request->result, obj_request->xferred, obj_request->length);
-	if (obj_request->img_request)
+	struct rbd_img_request *img_request = NULL;
+	struct rbd_device *rbd_dev = NULL;
+	bool layered = false;
+
+	if (obj_request_img_data_test(obj_request)) {
+		img_request = obj_request->img_request;
+		layered = img_request && img_request_layered_test(img_request);
+		rbd_dev = img_request->rbd_dev;
+	}
+
+	dout("%s: obj %p img %p result %d %llu/%llu\n", __func__,
+		obj_request, img_request, obj_request->result,
+		obj_request->xferred, obj_request->length);
+	if (layered && obj_request->result == -ENOENT &&
+			obj_request->img_offset < rbd_dev->parent_overlap)
+		rbd_img_parent_read(obj_request);
+	else if (img_request)
 		rbd_img_obj_request_read_callback(obj_request);
 	else
 		obj_request_done_set(obj_request);
@@ -1321,9 +1569,8 @@ static void rbd_osd_write_callback(struct rbd_obj_request *obj_request)
 	dout("%s: obj %p result %d %llu\n", __func__, obj_request,
 		obj_request->result, obj_request->length);
 	/*
-	 * There is no such thing as a successful short write.
-	 * Our xferred value is the number of bytes transferred
-	 * back.  Set it to our originally-requested length.
+	 * There is no such thing as a successful short write.  Set
+	 * it to our originally-requested length.
 	 */
 	obj_request->xferred = obj_request->length;
 	obj_request_done_set(obj_request);
@@ -1347,22 +1594,25 @@ static void rbd_osd_req_callback(struct ceph_osd_request *osd_req,
 
 	dout("%s: osd_req %p msg %p\n", __func__, osd_req, msg);
 	rbd_assert(osd_req == obj_request->osd_req);
-	rbd_assert(!!obj_request->img_request ^
-				(obj_request->which == BAD_WHICH));
+	if (obj_request_img_data_test(obj_request)) {
+		rbd_assert(obj_request->img_request);
+		rbd_assert(obj_request->which != BAD_WHICH);
+	} else {
+		rbd_assert(obj_request->which == BAD_WHICH);
+	}
 
 	if (osd_req->r_result < 0)
 		obj_request->result = osd_req->r_result;
-	obj_request->version = le64_to_cpu(osd_req->r_reassert_version.version);
 
-	WARN_ON(osd_req->r_num_ops != 1);	/* For now */
+	BUG_ON(osd_req->r_num_ops > 2);
 
 	/*
 	 * We support a 64-bit length, but ultimately it has to be
 	 * passed to blk_end_request(), which takes an unsigned int.
 	 */
 	obj_request->xferred = osd_req->r_reply_op_len[0];
-	rbd_assert(obj_request->xferred < (u64) UINT_MAX);
-	opcode = osd_req->r_request_ops[0].op;
+	rbd_assert(obj_request->xferred < (u64)UINT_MAX);
+	opcode = osd_req->r_ops[0].op;
 	switch (opcode) {
 	case CEPH_OSD_OP_READ:
 		rbd_osd_read_callback(obj_request);
@@ -1388,28 +1638,49 @@ static void rbd_osd_req_callback(struct ceph_osd_request *osd_req,
 		rbd_obj_request_complete(obj_request);
 }
 
+static void rbd_osd_req_format_read(struct rbd_obj_request *obj_request)
+{
+	struct rbd_img_request *img_request = obj_request->img_request;
+	struct ceph_osd_request *osd_req = obj_request->osd_req;
+	u64 snap_id;
+
+	rbd_assert(osd_req != NULL);
+
+	snap_id = img_request ? img_request->snap_id : CEPH_NOSNAP;
+	ceph_osdc_build_request(osd_req, obj_request->offset,
+			NULL, snap_id, NULL);
+}
+
+static void rbd_osd_req_format_write(struct rbd_obj_request *obj_request)
+{
+	struct rbd_img_request *img_request = obj_request->img_request;
+	struct ceph_osd_request *osd_req = obj_request->osd_req;
+	struct ceph_snap_context *snapc;
+	struct timespec mtime = CURRENT_TIME;
+
+	rbd_assert(osd_req != NULL);
+
+	snapc = img_request ? img_request->snapc : NULL;
+	ceph_osdc_build_request(osd_req, obj_request->offset,
+			snapc, CEPH_NOSNAP, &mtime);
+}
+
 static struct ceph_osd_request *rbd_osd_req_create(
 					struct rbd_device *rbd_dev,
 					bool write_request,
-					struct rbd_obj_request *obj_request,
-					struct ceph_osd_req_op *op)
+					struct rbd_obj_request *obj_request)
 {
-	struct rbd_img_request *img_request = obj_request->img_request;
 	struct ceph_snap_context *snapc = NULL;
 	struct ceph_osd_client *osdc;
 	struct ceph_osd_request *osd_req;
-	struct timespec now;
-	struct timespec *mtime;
-	u64 snap_id = CEPH_NOSNAP;
-	u64 offset = obj_request->offset;
-	u64 length = obj_request->length;
 
-	if (img_request) {
-		rbd_assert(img_request->write_request == write_request);
-		if (img_request->write_request)
+	if (obj_request_img_data_test(obj_request)) {
+		struct rbd_img_request *img_request = obj_request->img_request;
+
+		rbd_assert(write_request ==
+				img_request_write_test(img_request));
+		if (write_request)
 			snapc = img_request->snapc;
-		else
-			snap_id = img_request->snap_id;
 	}
 
 	/* Allocate and initialize the request, for the single op */
@@ -1419,31 +1690,10 @@ static struct ceph_osd_request *rbd_osd_req_create(
 	if (!osd_req)
 		return NULL;	/* ENOMEM */
 
-	rbd_assert(obj_request_type_valid(obj_request->type));
-	switch (obj_request->type) {
-	case OBJ_REQUEST_NODATA:
-		break;		/* Nothing to do */
-	case OBJ_REQUEST_BIO:
-		rbd_assert(obj_request->bio_list != NULL);
-		osd_req->r_bio = obj_request->bio_list;
-		break;
-	case OBJ_REQUEST_PAGES:
-		osd_req->r_pages = obj_request->pages;
-		osd_req->r_num_pages = obj_request->page_count;
-		osd_req->r_page_alignment = offset & ~PAGE_MASK;
-		break;
-	}
-
-	if (write_request) {
+	if (write_request)
 		osd_req->r_flags = CEPH_OSD_FLAG_WRITE | CEPH_OSD_FLAG_ONDISK;
-		now = CURRENT_TIME;
-		mtime = &now;
-	} else {
+	else
 		osd_req->r_flags = CEPH_OSD_FLAG_READ;
-		mtime = NULL;	/* not needed for reads */
-		offset = 0;	/* These are not used... */
-		length = 0;	/* ...for osd read requests */
-	}
 
 	osd_req->r_callback = rbd_osd_req_callback;
 	osd_req->r_priv = obj_request;
@@ -1454,14 +1704,51 @@ static struct ceph_osd_request *rbd_osd_req_create(
 
 	osd_req->r_file_layout = rbd_dev->layout;	/* struct */
 
-	/* osd_req will get its own reference to snapc (if non-null) */
+	return osd_req;
+}
+
+/*
+ * Create a copyup osd request based on the information in the
+ * object request supplied.  A copyup request has two osd ops,
+ * a copyup method call, and a "normal" write request.
+ */
+static struct ceph_osd_request *
+rbd_osd_req_create_copyup(struct rbd_obj_request *obj_request)
+{
+	struct rbd_img_request *img_request;
+	struct ceph_snap_context *snapc;
+	struct rbd_device *rbd_dev;
+	struct ceph_osd_client *osdc;
+	struct ceph_osd_request *osd_req;
+
+	rbd_assert(obj_request_img_data_test(obj_request));
+	img_request = obj_request->img_request;
+	rbd_assert(img_request);
+	rbd_assert(img_request_write_test(img_request));
 
-	ceph_osdc_build_request(osd_req, offset, length, 1, op,
-				snapc, snap_id, mtime);
+	/* Allocate and initialize the request, for the two ops */
+
+	snapc = img_request->snapc;
+	rbd_dev = img_request->rbd_dev;
+	osdc = &rbd_dev->rbd_client->client->osdc;
+	osd_req = ceph_osdc_alloc_request(osdc, snapc, 2, false, GFP_ATOMIC);
+	if (!osd_req)
+		return NULL;	/* ENOMEM */
+
+	osd_req->r_flags = CEPH_OSD_FLAG_WRITE | CEPH_OSD_FLAG_ONDISK;
+	osd_req->r_callback = rbd_osd_req_callback;
+	osd_req->r_priv = obj_request;
+
+	osd_req->r_oid_len = strlen(obj_request->object_name);
+	rbd_assert(osd_req->r_oid_len < sizeof (osd_req->r_oid));
+	memcpy(osd_req->r_oid, obj_request->object_name, osd_req->r_oid_len);
+
+	osd_req->r_file_layout = rbd_dev->layout;	/* struct */
 
 	return osd_req;
 }
 
+
 static void rbd_osd_req_destroy(struct ceph_osd_request *osd_req)
 {
 	ceph_osdc_put_request(osd_req);
@@ -1480,18 +1767,23 @@ static struct rbd_obj_request *rbd_obj_request_create(const char *object_name,
 	rbd_assert(obj_request_type_valid(type));
 
 	size = strlen(object_name) + 1;
-	obj_request = kzalloc(sizeof (*obj_request) + size, GFP_KERNEL);
-	if (!obj_request)
+	name = kmalloc(size, GFP_KERNEL);
+	if (!name)
+		return NULL;
+
+	obj_request = kmem_cache_zalloc(rbd_obj_request_cache, GFP_KERNEL);
+	if (!obj_request) {
+		kfree(name);
 		return NULL;
+	}
 
-	name = (char *)(obj_request + 1);
 	obj_request->object_name = memcpy(name, object_name, size);
 	obj_request->offset = offset;
 	obj_request->length = length;
+	obj_request->flags = 0;
 	obj_request->which = BAD_WHICH;
 	obj_request->type = type;
 	INIT_LIST_HEAD(&obj_request->links);
-	obj_request_done_init(obj_request);
 	init_completion(&obj_request->completion);
 	kref_init(&obj_request->kref);
 
@@ -1530,7 +1822,9 @@ static void rbd_obj_request_destroy(struct kref *kref)
 		break;
 	}
 
-	kfree(obj_request);
+	kfree(obj_request->object_name);
+	obj_request->object_name = NULL;
+	kmem_cache_free(rbd_obj_request_cache, obj_request);
 }
 
 /*
@@ -1541,37 +1835,40 @@ static void rbd_obj_request_destroy(struct kref *kref)
 static struct rbd_img_request *rbd_img_request_create(
 					struct rbd_device *rbd_dev,
 					u64 offset, u64 length,
-					bool write_request)
+					bool write_request,
+					bool child_request)
 {
 	struct rbd_img_request *img_request;
-	struct ceph_snap_context *snapc = NULL;
 
-	img_request = kmalloc(sizeof (*img_request), GFP_ATOMIC);
+	img_request = kmem_cache_alloc(rbd_img_request_cache, GFP_ATOMIC);
 	if (!img_request)
 		return NULL;
 
 	if (write_request) {
 		down_read(&rbd_dev->header_rwsem);
-		snapc = ceph_get_snap_context(rbd_dev->header.snapc);
+		ceph_get_snap_context(rbd_dev->header.snapc);
 		up_read(&rbd_dev->header_rwsem);
-		if (WARN_ON(!snapc)) {
-			kfree(img_request);
-			return NULL;	/* Shouldn't happen */
-		}
 	}
 
 	img_request->rq = NULL;
 	img_request->rbd_dev = rbd_dev;
 	img_request->offset = offset;
 	img_request->length = length;
-	img_request->write_request = write_request;
-	if (write_request)
-		img_request->snapc = snapc;
-	else
+	img_request->flags = 0;
+	if (write_request) {
+		img_request_write_set(img_request);
+		img_request->snapc = rbd_dev->header.snapc;
+	} else {
 		img_request->snap_id = rbd_dev->spec->snap_id;
+	}
+	if (child_request)
+		img_request_child_set(img_request);
+	if (rbd_dev->parent_spec)
+		img_request_layered_set(img_request);
 	spin_lock_init(&img_request->completion_lock);
 	img_request->next_completion = 0;
 	img_request->callback = NULL;
+	img_request->result = 0;
 	img_request->obj_request_count = 0;
 	INIT_LIST_HEAD(&img_request->obj_requests);
 	kref_init(&img_request->kref);
@@ -1600,78 +1897,204 @@ static void rbd_img_request_destroy(struct kref *kref)
 		rbd_img_obj_request_del(img_request, obj_request);
 	rbd_assert(img_request->obj_request_count == 0);
 
-	if (img_request->write_request)
+	if (img_request_write_test(img_request))
 		ceph_put_snap_context(img_request->snapc);
 
-	kfree(img_request);
+	if (img_request_child_test(img_request))
+		rbd_obj_request_put(img_request->obj_request);
+
+	kmem_cache_free(rbd_img_request_cache, img_request);
+}
+
+static bool rbd_img_obj_end_request(struct rbd_obj_request *obj_request)
+{
+	struct rbd_img_request *img_request;
+	unsigned int xferred;
+	int result;
+	bool more;
+
+	rbd_assert(obj_request_img_data_test(obj_request));
+	img_request = obj_request->img_request;
+
+	rbd_assert(obj_request->xferred <= (u64)UINT_MAX);
+	xferred = (unsigned int)obj_request->xferred;
+	result = obj_request->result;
+	if (result) {
+		struct rbd_device *rbd_dev = img_request->rbd_dev;
+
+		rbd_warn(rbd_dev, "%s %llx at %llx (%llx)\n",
+			img_request_write_test(img_request) ? "write" : "read",
+			obj_request->length, obj_request->img_offset,
+			obj_request->offset);
+		rbd_warn(rbd_dev, "  result %d xferred %x\n",
+			result, xferred);
+		if (!img_request->result)
+			img_request->result = result;
+	}
+
+	/* Image object requests don't own their page array */
+
+	if (obj_request->type == OBJ_REQUEST_PAGES) {
+		obj_request->pages = NULL;
+		obj_request->page_count = 0;
+	}
+
+	if (img_request_child_test(img_request)) {
+		rbd_assert(img_request->obj_request != NULL);
+		more = obj_request->which < img_request->obj_request_count - 1;
+	} else {
+		rbd_assert(img_request->rq != NULL);
+		more = blk_end_request(img_request->rq, result, xferred);
+	}
+
+	return more;
 }
 
-static int rbd_img_request_fill_bio(struct rbd_img_request *img_request,
-					struct bio *bio_list)
+static void rbd_img_obj_callback(struct rbd_obj_request *obj_request)
+{
+	struct rbd_img_request *img_request;
+	u32 which = obj_request->which;
+	bool more = true;
+
+	rbd_assert(obj_request_img_data_test(obj_request));
+	img_request = obj_request->img_request;
+
+	dout("%s: img %p obj %p\n", __func__, img_request, obj_request);
+	rbd_assert(img_request != NULL);
+	rbd_assert(img_request->obj_request_count > 0);
+	rbd_assert(which != BAD_WHICH);
+	rbd_assert(which < img_request->obj_request_count);
+	rbd_assert(which >= img_request->next_completion);
+
+	spin_lock_irq(&img_request->completion_lock);
+	if (which != img_request->next_completion)
+		goto out;
+
+	for_each_obj_request_from(img_request, obj_request) {
+		rbd_assert(more);
+		rbd_assert(which < img_request->obj_request_count);
+
+		if (!obj_request_done_test(obj_request))
+			break;
+		more = rbd_img_obj_end_request(obj_request);
+		which++;
+	}
+
+	rbd_assert(more ^ (which == img_request->obj_request_count));
+	img_request->next_completion = which;
+out:
+	spin_unlock_irq(&img_request->completion_lock);
+
+	if (!more)
+		rbd_img_request_complete(img_request);
+}
+
+/*
+ * Split up an image request into one or more object requests, each
+ * to a different object.  The "type" parameter indicates whether
+ * "data_desc" is the pointer to the head of a list of bio
+ * structures, or the base of a page array.  In either case this
+ * function assumes data_desc describes memory sufficient to hold
+ * all data described by the image request.
+ */
+static int rbd_img_request_fill(struct rbd_img_request *img_request,
+					enum obj_request_type type,
+					void *data_desc)
 {
 	struct rbd_device *rbd_dev = img_request->rbd_dev;
 	struct rbd_obj_request *obj_request = NULL;
 	struct rbd_obj_request *next_obj_request;
-	unsigned int bio_offset;
-	u64 image_offset;
+	bool write_request = img_request_write_test(img_request);
+	struct bio *bio_list;
+	unsigned int bio_offset = 0;
+	struct page **pages;
+	u64 img_offset;
 	u64 resid;
 	u16 opcode;
 
-	dout("%s: img %p bio %p\n", __func__, img_request, bio_list);
+	dout("%s: img %p type %d data_desc %p\n", __func__, img_request,
+		(int)type, data_desc);
 
-	opcode = img_request->write_request ? CEPH_OSD_OP_WRITE
-					      : CEPH_OSD_OP_READ;
-	bio_offset = 0;
-	image_offset = img_request->offset;
-	rbd_assert(image_offset == bio_list->bi_sector << SECTOR_SHIFT);
+	opcode = write_request ? CEPH_OSD_OP_WRITE : CEPH_OSD_OP_READ;
+	img_offset = img_request->offset;
 	resid = img_request->length;
 	rbd_assert(resid > 0);
+
+	if (type == OBJ_REQUEST_BIO) {
+		bio_list = data_desc;
+		rbd_assert(img_offset == bio_list->bi_sector << SECTOR_SHIFT);
+	} else {
+		rbd_assert(type == OBJ_REQUEST_PAGES);
+		pages = data_desc;
+	}
+
 	while (resid) {
+		struct ceph_osd_request *osd_req;
 		const char *object_name;
-		unsigned int clone_size;
-		struct ceph_osd_req_op *op;
 		u64 offset;
 		u64 length;
 
-		object_name = rbd_segment_name(rbd_dev, image_offset);
+		object_name = rbd_segment_name(rbd_dev, img_offset);
 		if (!object_name)
 			goto out_unwind;
-		offset = rbd_segment_offset(rbd_dev, image_offset);
-		length = rbd_segment_length(rbd_dev, image_offset, resid);
+		offset = rbd_segment_offset(rbd_dev, img_offset);
+		length = rbd_segment_length(rbd_dev, img_offset, resid);
 		obj_request = rbd_obj_request_create(object_name,
-						offset, length,
-						OBJ_REQUEST_BIO);
-		kfree(object_name);	/* object request has its own copy */
+						offset, length, type);
+		/* object request has its own copy of the object name */
+		rbd_segment_name_free(object_name);
 		if (!obj_request)
 			goto out_unwind;
 
-		rbd_assert(length <= (u64) UINT_MAX);
-		clone_size = (unsigned int) length;
-		obj_request->bio_list = bio_chain_clone_range(&bio_list,
-						&bio_offset, clone_size,
-						GFP_ATOMIC);
-		if (!obj_request->bio_list)
-			goto out_partial;
+		if (type == OBJ_REQUEST_BIO) {
+			unsigned int clone_size;
+
+			rbd_assert(length <= (u64)UINT_MAX);
+			clone_size = (unsigned int)length;
+			obj_request->bio_list =
+					bio_chain_clone_range(&bio_list,
+								&bio_offset,
+								clone_size,
+								GFP_ATOMIC);
+			if (!obj_request->bio_list)
+				goto out_partial;
+		} else {
+			unsigned int page_count;
+
+			obj_request->pages = pages;
+			page_count = (u32)calc_pages_for(offset, length);
+			obj_request->page_count = page_count;
+			if ((offset + length) & ~PAGE_MASK)
+				page_count--;	/* more on last page */
+			pages += page_count;
+		}
 
-		/*
-		 * Build up the op to use in building the osd
-		 * request.  Note that the contents of the op are
-		 * copied by rbd_osd_req_create().
-		 */
-		op = rbd_osd_req_op_create(opcode, offset, length);
-		if (!op)
-			goto out_partial;
-		obj_request->osd_req = rbd_osd_req_create(rbd_dev,
-						img_request->write_request,
-						obj_request, op);
-		rbd_osd_req_op_destroy(op);
-		if (!obj_request->osd_req)
+		osd_req = rbd_osd_req_create(rbd_dev, write_request,
+						obj_request);
+		if (!osd_req)
 			goto out_partial;
-		/* status and version are initially zero-filled */
+		obj_request->osd_req = osd_req;
+		obj_request->callback = rbd_img_obj_callback;
+
+		osd_req_op_extent_init(osd_req, 0, opcode, offset, length,
+						0, 0);
+		if (type == OBJ_REQUEST_BIO)
+			osd_req_op_extent_osd_data_bio(osd_req, 0,
+					obj_request->bio_list, length);
+		else
+			osd_req_op_extent_osd_data_pages(osd_req, 0,
+					obj_request->pages, length,
+					offset & ~PAGE_MASK, false, false);
 
+		if (write_request)
+			rbd_osd_req_format_write(obj_request);
+		else
+			rbd_osd_req_format_read(obj_request);
+
+		obj_request->img_offset = img_offset;
 		rbd_img_obj_request_add(img_request, obj_request);
 
-		image_offset += length;
+		img_offset += length;
 		resid -= length;
 	}
 
@@ -1686,61 +2109,389 @@ out_unwind:
 	return -ENOMEM;
 }
 
-static void rbd_img_obj_callback(struct rbd_obj_request *obj_request)
+static void
+rbd_img_obj_copyup_callback(struct rbd_obj_request *obj_request)
 {
 	struct rbd_img_request *img_request;
-	u32 which = obj_request->which;
-	bool more = true;
+	struct rbd_device *rbd_dev;
+	u64 length;
+	u32 page_count;
 
+	rbd_assert(obj_request->type == OBJ_REQUEST_BIO);
+	rbd_assert(obj_request_img_data_test(obj_request));
 	img_request = obj_request->img_request;
+	rbd_assert(img_request);
 
-	dout("%s: img %p obj %p\n", __func__, img_request, obj_request);
+	rbd_dev = img_request->rbd_dev;
+	rbd_assert(rbd_dev);
+	length = (u64)1 << rbd_dev->header.obj_order;
+	page_count = (u32)calc_pages_for(0, length);
+
+	rbd_assert(obj_request->copyup_pages);
+	ceph_release_page_vector(obj_request->copyup_pages, page_count);
+	obj_request->copyup_pages = NULL;
+
+	/*
+	 * We want the transfer count to reflect the size of the
+	 * original write request.  There is no such thing as a
+	 * successful short write, so if the request was successful
+	 * we can just set it to the originally-requested length.
+	 */
+	if (!obj_request->result)
+		obj_request->xferred = obj_request->length;
+
+	/* Finish up with the normal image object callback */
+
+	rbd_img_obj_callback(obj_request);
+}
+
+static void
+rbd_img_obj_parent_read_full_callback(struct rbd_img_request *img_request)
+{
+	struct rbd_obj_request *orig_request;
+	struct ceph_osd_request *osd_req;
+	struct ceph_osd_client *osdc;
+	struct rbd_device *rbd_dev;
+	struct page **pages;
+	int result;
+	u64 obj_size;
+	u64 xferred;
+
+	rbd_assert(img_request_child_test(img_request));
+
+	/* First get what we need from the image request */
+
+	pages = img_request->copyup_pages;
+	rbd_assert(pages != NULL);
+	img_request->copyup_pages = NULL;
+
+	orig_request = img_request->obj_request;
+	rbd_assert(orig_request != NULL);
+	rbd_assert(orig_request->type == OBJ_REQUEST_BIO);
+	result = img_request->result;
+	obj_size = img_request->length;
+	xferred = img_request->xferred;
+
+	rbd_dev = img_request->rbd_dev;
+	rbd_assert(rbd_dev);
+	rbd_assert(obj_size == (u64)1 << rbd_dev->header.obj_order);
+
+	rbd_img_request_put(img_request);
+
+	if (result)
+		goto out_err;
+
+	/* Allocate the new copyup osd request for the original request */
+
+	result = -ENOMEM;
+	rbd_assert(!orig_request->osd_req);
+	osd_req = rbd_osd_req_create_copyup(orig_request);
+	if (!osd_req)
+		goto out_err;
+	orig_request->osd_req = osd_req;
+	orig_request->copyup_pages = pages;
+
+	/* Initialize the copyup op */
+
+	osd_req_op_cls_init(osd_req, 0, CEPH_OSD_OP_CALL, "rbd", "copyup");
+	osd_req_op_cls_request_data_pages(osd_req, 0, pages, obj_size, 0,
+						false, false);
+
+	/* Then the original write request op */
+
+	osd_req_op_extent_init(osd_req, 1, CEPH_OSD_OP_WRITE,
+					orig_request->offset,
+					orig_request->length, 0, 0);
+	osd_req_op_extent_osd_data_bio(osd_req, 1, orig_request->bio_list,
+					orig_request->length);
+
+	rbd_osd_req_format_write(orig_request);
+
+	/* All set, send it off. */
+
+	orig_request->callback = rbd_img_obj_copyup_callback;
+	osdc = &rbd_dev->rbd_client->client->osdc;
+	result = rbd_obj_request_submit(osdc, orig_request);
+	if (!result)
+		return;
+out_err:
+	/* Record the error code and complete the request */
+
+	orig_request->result = result;
+	orig_request->xferred = 0;
+	obj_request_done_set(orig_request);
+	rbd_obj_request_complete(orig_request);
+}
+
+/*
+ * Read from the parent image the range of data that covers the
+ * entire target of the given object request.  This is used for
+ * satisfying a layered image write request when the target of an
+ * object request from the image request does not exist.
+ *
+ * A page array big enough to hold the returned data is allocated
+ * and supplied to rbd_img_request_fill() as the "data descriptor."
+ * When the read completes, this page array will be transferred to
+ * the original object request for the copyup operation.
+ *
+ * If an error occurs, record it as the result of the original
+ * object request and mark it done so it gets completed.
+ */
+static int rbd_img_obj_parent_read_full(struct rbd_obj_request *obj_request)
+{
+	struct rbd_img_request *img_request = NULL;
+	struct rbd_img_request *parent_request = NULL;
+	struct rbd_device *rbd_dev;
+	u64 img_offset;
+	u64 length;
+	struct page **pages = NULL;
+	u32 page_count;
+	int result;
+
+	rbd_assert(obj_request_img_data_test(obj_request));
+	rbd_assert(obj_request->type == OBJ_REQUEST_BIO);
+
+	img_request = obj_request->img_request;
 	rbd_assert(img_request != NULL);
-	rbd_assert(img_request->rq != NULL);
-	rbd_assert(img_request->obj_request_count > 0);
-	rbd_assert(which != BAD_WHICH);
-	rbd_assert(which < img_request->obj_request_count);
-	rbd_assert(which >= img_request->next_completion);
+	rbd_dev = img_request->rbd_dev;
+	rbd_assert(rbd_dev->parent != NULL);
 
-	spin_lock_irq(&img_request->completion_lock);
-	if (which != img_request->next_completion)
-		goto out;
+	/*
+	 * First things first.  The original osd request is of no
+	 * use to use any more, we'll need a new one that can hold
+	 * the two ops in a copyup request.  We'll get that later,
+	 * but for now we can release the old one.
+	 */
+	rbd_osd_req_destroy(obj_request->osd_req);
+	obj_request->osd_req = NULL;
 
-	for_each_obj_request_from(img_request, obj_request) {
-		unsigned int xferred;
-		int result;
+	/*
+	 * Determine the byte range covered by the object in the
+	 * child image to which the original request was to be sent.
+	 */
+	img_offset = obj_request->img_offset - obj_request->offset;
+	length = (u64)1 << rbd_dev->header.obj_order;
 
-		rbd_assert(more);
-		rbd_assert(which < img_request->obj_request_count);
+	/*
+	 * There is no defined parent data beyond the parent
+	 * overlap, so limit what we read at that boundary if
+	 * necessary.
+	 */
+	if (img_offset + length > rbd_dev->parent_overlap) {
+		rbd_assert(img_offset < rbd_dev->parent_overlap);
+		length = rbd_dev->parent_overlap - img_offset;
+	}
 
-		if (!obj_request_done_test(obj_request))
-			break;
+	/*
+	 * Allocate a page array big enough to receive the data read
+	 * from the parent.
+	 */
+	page_count = (u32)calc_pages_for(0, length);
+	pages = ceph_alloc_page_vector(page_count, GFP_KERNEL);
+	if (IS_ERR(pages)) {
+		result = PTR_ERR(pages);
+		pages = NULL;
+		goto out_err;
+	}
 
-		rbd_assert(obj_request->xferred <= (u64) UINT_MAX);
-		xferred = (unsigned int) obj_request->xferred;
-		result = (int) obj_request->result;
-		if (result)
-			rbd_warn(NULL, "obj_request %s result %d xferred %u\n",
-				img_request->write_request ? "write" : "read",
-				result, xferred);
+	result = -ENOMEM;
+	parent_request = rbd_img_request_create(rbd_dev->parent,
+						img_offset, length,
+						false, true);
+	if (!parent_request)
+		goto out_err;
+	rbd_obj_request_get(obj_request);
+	parent_request->obj_request = obj_request;
 
-		more = blk_end_request(img_request->rq, result, xferred);
-		which++;
+	result = rbd_img_request_fill(parent_request, OBJ_REQUEST_PAGES, pages);
+	if (result)
+		goto out_err;
+	parent_request->copyup_pages = pages;
+
+	parent_request->callback = rbd_img_obj_parent_read_full_callback;
+	result = rbd_img_request_submit(parent_request);
+	if (!result)
+		return 0;
+
+	parent_request->copyup_pages = NULL;
+	parent_request->obj_request = NULL;
+	rbd_obj_request_put(obj_request);
+out_err:
+	if (pages)
+		ceph_release_page_vector(pages, page_count);
+	if (parent_request)
+		rbd_img_request_put(parent_request);
+	obj_request->result = result;
+	obj_request->xferred = 0;
+	obj_request_done_set(obj_request);
+
+	return result;
+}
+
+static void rbd_img_obj_exists_callback(struct rbd_obj_request *obj_request)
+{
+	struct rbd_obj_request *orig_request;
+	int result;
+
+	rbd_assert(!obj_request_img_data_test(obj_request));
+
+	/*
+	 * All we need from the object request is the original
+	 * request and the result of the STAT op.  Grab those, then
+	 * we're done with the request.
+	 */
+	orig_request = obj_request->obj_request;
+	obj_request->obj_request = NULL;
+	rbd_assert(orig_request);
+	rbd_assert(orig_request->img_request);
+
+	result = obj_request->result;
+	obj_request->result = 0;
+
+	dout("%s: obj %p for obj %p result %d %llu/%llu\n", __func__,
+		obj_request, orig_request, result,
+		obj_request->xferred, obj_request->length);
+	rbd_obj_request_put(obj_request);
+
+	rbd_assert(orig_request);
+	rbd_assert(orig_request->img_request);
+
+	/*
+	 * Our only purpose here is to determine whether the object
+	 * exists, and we don't want to treat the non-existence as
+	 * an error.  If something else comes back, transfer the
+	 * error to the original request and complete it now.
+	 */
+	if (!result) {
+		obj_request_existence_set(orig_request, true);
+	} else if (result == -ENOENT) {
+		obj_request_existence_set(orig_request, false);
+	} else if (result) {
+		orig_request->result = result;
+		goto out;
 	}
 
-	rbd_assert(more ^ (which == img_request->obj_request_count));
-	img_request->next_completion = which;
+	/*
+	 * Resubmit the original request now that we have recorded
+	 * whether the target object exists.
+	 */
+	orig_request->result = rbd_img_obj_request_submit(orig_request);
 out:
-	spin_unlock_irq(&img_request->completion_lock);
+	if (orig_request->result)
+		rbd_obj_request_complete(orig_request);
+	rbd_obj_request_put(orig_request);
+}
 
-	if (!more)
-		rbd_img_request_complete(img_request);
+static int rbd_img_obj_exists_submit(struct rbd_obj_request *obj_request)
+{
+	struct rbd_obj_request *stat_request;
+	struct rbd_device *rbd_dev;
+	struct ceph_osd_client *osdc;
+	struct page **pages = NULL;
+	u32 page_count;
+	size_t size;
+	int ret;
+
+	/*
+	 * The response data for a STAT call consists of:
+	 *     le64 length;
+	 *     struct {
+	 *         le32 tv_sec;
+	 *         le32 tv_nsec;
+	 *     } mtime;
+	 */
+	size = sizeof (__le64) + sizeof (__le32) + sizeof (__le32);
+	page_count = (u32)calc_pages_for(0, size);
+	pages = ceph_alloc_page_vector(page_count, GFP_KERNEL);
+	if (IS_ERR(pages))
+		return PTR_ERR(pages);
+
+	ret = -ENOMEM;
+	stat_request = rbd_obj_request_create(obj_request->object_name, 0, 0,
+							OBJ_REQUEST_PAGES);
+	if (!stat_request)
+		goto out;
+
+	rbd_obj_request_get(obj_request);
+	stat_request->obj_request = obj_request;
+	stat_request->pages = pages;
+	stat_request->page_count = page_count;
+
+	rbd_assert(obj_request->img_request);
+	rbd_dev = obj_request->img_request->rbd_dev;
+	stat_request->osd_req = rbd_osd_req_create(rbd_dev, false,
+						stat_request);
+	if (!stat_request->osd_req)
+		goto out;
+	stat_request->callback = rbd_img_obj_exists_callback;
+
+	osd_req_op_init(stat_request->osd_req, 0, CEPH_OSD_OP_STAT);
+	osd_req_op_raw_data_in_pages(stat_request->osd_req, 0, pages, size, 0,
+					false, false);
+	rbd_osd_req_format_read(stat_request);
+
+	osdc = &rbd_dev->rbd_client->client->osdc;
+	ret = rbd_obj_request_submit(osdc, stat_request);
+out:
+	if (ret)
+		rbd_obj_request_put(obj_request);
+
+	return ret;
+}
+
+static int rbd_img_obj_request_submit(struct rbd_obj_request *obj_request)
+{
+	struct rbd_img_request *img_request;
+	struct rbd_device *rbd_dev;
+	bool known;
+
+	rbd_assert(obj_request_img_data_test(obj_request));
+
+	img_request = obj_request->img_request;
+	rbd_assert(img_request);
+	rbd_dev = img_request->rbd_dev;
+
+	/*
+	 * Only writes to layered images need special handling.
+	 * Reads and non-layered writes are simple object requests.
+	 * Layered writes that start beyond the end of the overlap
+	 * with the parent have no parent data, so they too are
+	 * simple object requests.  Finally, if the target object is
+	 * known to already exist, its parent data has already been
+	 * copied, so a write to the object can also be handled as a
+	 * simple object request.
+	 */
+	if (!img_request_write_test(img_request) ||
+		!img_request_layered_test(img_request) ||
+		rbd_dev->parent_overlap <= obj_request->img_offset ||
+		((known = obj_request_known_test(obj_request)) &&
+			obj_request_exists_test(obj_request))) {
+
+		struct rbd_device *rbd_dev;
+		struct ceph_osd_client *osdc;
+
+		rbd_dev = obj_request->img_request->rbd_dev;
+		osdc = &rbd_dev->rbd_client->client->osdc;
+
+		return rbd_obj_request_submit(osdc, obj_request);
+	}
+
+	/*
+	 * It's a layered write.  The target object might exist but
+	 * we may not know that yet.  If we know it doesn't exist,
+	 * start by reading the data for the full target object from
+	 * the parent so we can use it for a copyup to the target.
+	 */
+	if (known)
+		return rbd_img_obj_parent_read_full(obj_request);
+
+	/* We don't know whether the target exists.  Go find out. */
+
+	return rbd_img_obj_exists_submit(obj_request);
 }
 
 static int rbd_img_request_submit(struct rbd_img_request *img_request)
 {
-	struct rbd_device *rbd_dev = img_request->rbd_dev;
-	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
 	struct rbd_obj_request *obj_request;
 	struct rbd_obj_request *next_obj_request;
 
@@ -1748,27 +2499,105 @@ static int rbd_img_request_submit(struct rbd_img_request *img_request)
 	for_each_obj_request_safe(img_request, obj_request, next_obj_request) {
 		int ret;
 
-		obj_request->callback = rbd_img_obj_callback;
-		ret = rbd_obj_request_submit(osdc, obj_request);
+		ret = rbd_img_obj_request_submit(obj_request);
 		if (ret)
 			return ret;
-		/*
-		 * The image request has its own reference to each
-		 * of its object requests, so we can safely drop the
-		 * initial one here.
-		 */
-		rbd_obj_request_put(obj_request);
 	}
 
 	return 0;
 }
 
-static int rbd_obj_notify_ack(struct rbd_device *rbd_dev,
-				   u64 ver, u64 notify_id)
+static void rbd_img_parent_read_callback(struct rbd_img_request *img_request)
 {
 	struct rbd_obj_request *obj_request;
-	struct ceph_osd_req_op *op;
-	struct ceph_osd_client *osdc;
+	struct rbd_device *rbd_dev;
+	u64 obj_end;
+
+	rbd_assert(img_request_child_test(img_request));
+
+	obj_request = img_request->obj_request;
+	rbd_assert(obj_request);
+	rbd_assert(obj_request->img_request);
+
+	obj_request->result = img_request->result;
+	if (obj_request->result)
+		goto out;
+
+	/*
+	 * We need to zero anything beyond the parent overlap
+	 * boundary.  Since rbd_img_obj_request_read_callback()
+	 * will zero anything beyond the end of a short read, an
+	 * easy way to do this is to pretend the data from the
+	 * parent came up short--ending at the overlap boundary.
+	 */
+	rbd_assert(obj_request->img_offset < U64_MAX - obj_request->length);
+	obj_end = obj_request->img_offset + obj_request->length;
+	rbd_dev = obj_request->img_request->rbd_dev;
+	if (obj_end > rbd_dev->parent_overlap) {
+		u64 xferred = 0;
+
+		if (obj_request->img_offset < rbd_dev->parent_overlap)
+			xferred = rbd_dev->parent_overlap -
+					obj_request->img_offset;
+
+		obj_request->xferred = min(img_request->xferred, xferred);
+	} else {
+		obj_request->xferred = img_request->xferred;
+	}
+out:
+	rbd_img_request_put(img_request);
+	rbd_img_obj_request_read_callback(obj_request);
+	rbd_obj_request_complete(obj_request);
+}
+
+static void rbd_img_parent_read(struct rbd_obj_request *obj_request)
+{
+	struct rbd_device *rbd_dev;
+	struct rbd_img_request *img_request;
+	int result;
+
+	rbd_assert(obj_request_img_data_test(obj_request));
+	rbd_assert(obj_request->img_request != NULL);
+	rbd_assert(obj_request->result == (s32) -ENOENT);
+	rbd_assert(obj_request->type == OBJ_REQUEST_BIO);
+
+	rbd_dev = obj_request->img_request->rbd_dev;
+	rbd_assert(rbd_dev->parent != NULL);
+	/* rbd_read_finish(obj_request, obj_request->length); */
+	img_request = rbd_img_request_create(rbd_dev->parent,
+						obj_request->img_offset,
+						obj_request->length,
+						false, true);
+	result = -ENOMEM;
+	if (!img_request)
+		goto out_err;
+
+	rbd_obj_request_get(obj_request);
+	img_request->obj_request = obj_request;
+
+	result = rbd_img_request_fill(img_request, OBJ_REQUEST_BIO,
+					obj_request->bio_list);
+	if (result)
+		goto out_err;
+
+	img_request->callback = rbd_img_parent_read_callback;
+	result = rbd_img_request_submit(img_request);
+	if (result)
+		goto out_err;
+
+	return;
+out_err:
+	if (img_request)
+		rbd_img_request_put(img_request);
+	obj_request->result = result;
+	obj_request->xferred = 0;
+	obj_request_done_set(obj_request);
+}
+
+static int rbd_obj_notify_ack(struct rbd_device *rbd_dev, u64 notify_id)
+{
+	struct rbd_obj_request *obj_request;
+	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
 	int ret;
 
 	obj_request = rbd_obj_request_create(rbd_dev->header_name, 0, 0,
@@ -1777,17 +2606,15 @@ static int rbd_obj_notify_ack(struct rbd_device *rbd_dev,
 		return -ENOMEM;
 
 	ret = -ENOMEM;
-	op = rbd_osd_req_op_create(CEPH_OSD_OP_NOTIFY_ACK, notify_id, ver);
-	if (!op)
-		goto out;
-	obj_request->osd_req = rbd_osd_req_create(rbd_dev, false,
-						obj_request, op);
-	rbd_osd_req_op_destroy(op);
+	obj_request->osd_req = rbd_osd_req_create(rbd_dev, false, obj_request);
 	if (!obj_request->osd_req)
 		goto out;
-
-	osdc = &rbd_dev->rbd_client->client->osdc;
 	obj_request->callback = rbd_obj_request_put;
+
+	osd_req_op_watch_init(obj_request->osd_req, 0, CEPH_OSD_OP_NOTIFY_ACK,
+					notify_id, 0, 0);
+	rbd_osd_req_format_read(obj_request);
+
 	ret = rbd_obj_request_submit(osdc, obj_request);
 out:
 	if (ret)
@@ -1799,21 +2626,16 @@ out:
 static void rbd_watch_cb(u64 ver, u64 notify_id, u8 opcode, void *data)
 {
 	struct rbd_device *rbd_dev = (struct rbd_device *)data;
-	u64 hver;
-	int rc;
 
 	if (!rbd_dev)
 		return;
 
 	dout("%s: \"%s\" notify_id %llu opcode %u\n", __func__,
-		rbd_dev->header_name, (unsigned long long) notify_id,
-		(unsigned int) opcode);
-	rc = rbd_dev_refresh(rbd_dev, &hver);
-	if (rc)
-		rbd_warn(rbd_dev, "got notification but failed to "
-			   " update snaps: %d\n", rc);
+		rbd_dev->header_name, (unsigned long long)notify_id,
+		(unsigned int)opcode);
+	(void)rbd_dev_refresh(rbd_dev);
 
-	rbd_obj_notify_ack(rbd_dev, hver, notify_id);
+	rbd_obj_notify_ack(rbd_dev, notify_id);
 }
 
 /*
@@ -1824,7 +2646,6 @@ static int rbd_dev_header_watch_sync(struct rbd_device *rbd_dev, int start)
 {
 	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
 	struct rbd_obj_request *obj_request;
-	struct ceph_osd_req_op *op;
 	int ret;
 
 	rbd_assert(start ^ !!rbd_dev->watch_event);
@@ -1844,14 +2665,7 @@ static int rbd_dev_header_watch_sync(struct rbd_device *rbd_dev, int start)
 	if (!obj_request)
 		goto out_cancel;
 
-	op = rbd_osd_req_op_create(CEPH_OSD_OP_WATCH,
-				rbd_dev->watch_event->cookie,
-				rbd_dev->header.obj_version, start);
-	if (!op)
-		goto out_cancel;
-	obj_request->osd_req = rbd_osd_req_create(rbd_dev, true,
-							obj_request, op);
-	rbd_osd_req_op_destroy(op);
+	obj_request->osd_req = rbd_osd_req_create(rbd_dev, true, obj_request);
 	if (!obj_request->osd_req)
 		goto out_cancel;
 
@@ -1860,6 +2674,11 @@ static int rbd_dev_header_watch_sync(struct rbd_device *rbd_dev, int start)
 	else
 		ceph_osdc_unregister_linger_request(osdc,
 					rbd_dev->watch_request->osd_req);
+
+	osd_req_op_watch_init(obj_request->osd_req, 0, CEPH_OSD_OP_WATCH,
+				rbd_dev->watch_event->cookie, 0, start);
+	rbd_osd_req_format_write(obj_request);
+
 	ret = rbd_obj_request_submit(osdc, obj_request);
 	if (ret)
 		goto out_cancel;
@@ -1899,40 +2718,38 @@ out_cancel:
 }
 
 /*
- * Synchronous osd object method call
+ * Synchronous osd object method call.  Returns the number of bytes
+ * returned in the outbound buffer, or a negative error code.
  */
 static int rbd_obj_method_sync(struct rbd_device *rbd_dev,
 			     const char *object_name,
 			     const char *class_name,
 			     const char *method_name,
-			     const char *outbound,
+			     const void *outbound,
 			     size_t outbound_size,
-			     char *inbound,
-			     size_t inbound_size,
-			     u64 *version)
+			     void *inbound,
+			     size_t inbound_size)
 {
+	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
 	struct rbd_obj_request *obj_request;
-	struct ceph_osd_client *osdc;
-	struct ceph_osd_req_op *op;
 	struct page **pages;
 	u32 page_count;
 	int ret;
 
 	/*
-	 * Method calls are ultimately read operations but they
-	 * don't involve object data (so no offset or length).
-	 * The result should placed into the inbound buffer
-	 * provided.  They also supply outbound data--parameters for
-	 * the object method.  Currently if this is present it will
-	 * be a snapshot id.
+	 * Method calls are ultimately read operations.  The result
+	 * should placed into the inbound buffer provided.  They
+	 * also supply outbound data--parameters for the object
+	 * method.  Currently if this is present it will be a
+	 * snapshot id.
 	 */
-	page_count = (u32) calc_pages_for(0, inbound_size);
+	page_count = (u32)calc_pages_for(0, inbound_size);
 	pages = ceph_alloc_page_vector(page_count, GFP_KERNEL);
 	if (IS_ERR(pages))
 		return PTR_ERR(pages);
 
 	ret = -ENOMEM;
-	obj_request = rbd_obj_request_create(object_name, 0, 0,
+	obj_request = rbd_obj_request_create(object_name, 0, inbound_size,
 							OBJ_REQUEST_PAGES);
 	if (!obj_request)
 		goto out;
@@ -1940,17 +2757,29 @@ static int rbd_obj_method_sync(struct rbd_device *rbd_dev,
 	obj_request->pages = pages;
 	obj_request->page_count = page_count;
 
-	op = rbd_osd_req_op_create(CEPH_OSD_OP_CALL, class_name,
-					method_name, outbound, outbound_size);
-	if (!op)
-		goto out;
-	obj_request->osd_req = rbd_osd_req_create(rbd_dev, false,
-						obj_request, op);
-	rbd_osd_req_op_destroy(op);
+	obj_request->osd_req = rbd_osd_req_create(rbd_dev, false, obj_request);
 	if (!obj_request->osd_req)
 		goto out;
 
-	osdc = &rbd_dev->rbd_client->client->osdc;
+	osd_req_op_cls_init(obj_request->osd_req, 0, CEPH_OSD_OP_CALL,
+					class_name, method_name);
+	if (outbound_size) {
+		struct ceph_pagelist *pagelist;
+
+		pagelist = kmalloc(sizeof (*pagelist), GFP_NOFS);
+		if (!pagelist)
+			goto out;
+
+		ceph_pagelist_init(pagelist);
+		ceph_pagelist_append(pagelist, outbound, outbound_size);
+		osd_req_op_cls_request_data_pagelist(obj_request->osd_req, 0,
+						pagelist);
+	}
+	osd_req_op_cls_response_data_pages(obj_request->osd_req, 0,
+					obj_request->pages, inbound_size,
+					0, false, false);
+	rbd_osd_req_format_read(obj_request);
+
 	ret = rbd_obj_request_submit(osdc, obj_request);
 	if (ret)
 		goto out;
@@ -1961,10 +2790,10 @@ static int rbd_obj_method_sync(struct rbd_device *rbd_dev,
 	ret = obj_request->result;
 	if (ret < 0)
 		goto out;
-	ret = 0;
+
+	rbd_assert(obj_request->xferred < (u64)INT_MAX);
+	ret = (int)obj_request->xferred;
 	ceph_copy_from_page_vector(pages, inbound, 0, obj_request->xferred);
-	if (version)
-		*version = obj_request->version;
 out:
 	if (obj_request)
 		rbd_obj_request_put(obj_request);
@@ -2034,18 +2863,22 @@ static void rbd_request_fn(struct request_queue *q)
 		}
 
 		result = -EINVAL;
-		if (WARN_ON(offset && length > U64_MAX - offset + 1))
+		if (offset && length > U64_MAX - offset + 1) {
+			rbd_warn(rbd_dev, "bad request range (%llu~%llu)\n",
+				offset, length);
 			goto end_request;	/* Shouldn't happen */
+		}
 
 		result = -ENOMEM;
 		img_request = rbd_img_request_create(rbd_dev, offset, length,
-							write_request);
+							write_request, false);
 		if (!img_request)
 			goto end_request;
 
 		img_request->rq = rq;
 
-		result = rbd_img_request_fill_bio(img_request, rq->bio);
+		result = rbd_img_request_fill(img_request, OBJ_REQUEST_BIO,
+						rq->bio);
 		if (!result)
 			result = rbd_img_request_submit(img_request);
 		if (result)
@@ -2053,8 +2886,10 @@ static void rbd_request_fn(struct request_queue *q)
 end_request:
 		spin_lock_irq(q->queue_lock);
 		if (result < 0) {
-			rbd_warn(rbd_dev, "obj_request %s result %d\n",
-				write_request ? "write" : "read", result);
+			rbd_warn(rbd_dev, "%s %llx at %llx result %d\n",
+				write_request ? "write" : "read",
+				length, offset, result);
+
 			__blk_end_request_all(rq, result);
 		}
 	}
@@ -2113,22 +2948,22 @@ static void rbd_free_disk(struct rbd_device *rbd_dev)
 	if (!disk)
 		return;
 
-	if (disk->flags & GENHD_FL_UP)
+	rbd_dev->disk = NULL;
+	if (disk->flags & GENHD_FL_UP) {
 		del_gendisk(disk);
-	if (disk->queue)
-		blk_cleanup_queue(disk->queue);
+		if (disk->queue)
+			blk_cleanup_queue(disk->queue);
+	}
 	put_disk(disk);
 }
 
 static int rbd_obj_read_sync(struct rbd_device *rbd_dev,
 				const char *object_name,
-				u64 offset, u64 length,
-				char *buf, u64 *version)
+				u64 offset, u64 length, void *buf)
 
 {
-	struct ceph_osd_req_op *op;
+	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
 	struct rbd_obj_request *obj_request;
-	struct ceph_osd_client *osdc;
 	struct page **pages = NULL;
 	u32 page_count;
 	size_t size;
@@ -2148,16 +2983,19 @@ static int rbd_obj_read_sync(struct rbd_device *rbd_dev,
 	obj_request->pages = pages;
 	obj_request->page_count = page_count;
 
-	op = rbd_osd_req_op_create(CEPH_OSD_OP_READ, offset, length);
-	if (!op)
-		goto out;
-	obj_request->osd_req = rbd_osd_req_create(rbd_dev, false,
-						obj_request, op);
-	rbd_osd_req_op_destroy(op);
+	obj_request->osd_req = rbd_osd_req_create(rbd_dev, false, obj_request);
 	if (!obj_request->osd_req)
 		goto out;
 
-	osdc = &rbd_dev->rbd_client->client->osdc;
+	osd_req_op_extent_init(obj_request->osd_req, 0, CEPH_OSD_OP_READ,
+					offset, length, 0, 0);
+	osd_req_op_extent_osd_data_pages(obj_request->osd_req, 0,
+					obj_request->pages,
+					obj_request->length,
+					obj_request->offset & ~PAGE_MASK,
+					false, false);
+	rbd_osd_req_format_read(obj_request);
+
 	ret = rbd_obj_request_submit(osdc, obj_request);
 	if (ret)
 		goto out;
@@ -2172,10 +3010,8 @@ static int rbd_obj_read_sync(struct rbd_device *rbd_dev,
 	rbd_assert(obj_request->xferred <= (u64) SIZE_MAX);
 	size = (size_t) obj_request->xferred;
 	ceph_copy_from_page_vector(pages, buf, 0, size);
-	rbd_assert(size <= (size_t) INT_MAX);
-	ret = (int) size;
-	if (version)
-		*version = obj_request->version;
+	rbd_assert(size <= (size_t)INT_MAX);
+	ret = (int)size;
 out:
 	if (obj_request)
 		rbd_obj_request_put(obj_request);
@@ -2196,7 +3032,7 @@ out:
  * Returns a pointer-coded errno if a failure occurs.
  */
 static struct rbd_image_header_ondisk *
-rbd_dev_v1_header_read(struct rbd_device *rbd_dev, u64 *version)
+rbd_dev_v1_header_read(struct rbd_device *rbd_dev)
 {
 	struct rbd_image_header_ondisk *ondisk = NULL;
 	u32 snap_count = 0;
@@ -2224,11 +3060,10 @@ rbd_dev_v1_header_read(struct rbd_device *rbd_dev, u64 *version)
 			return ERR_PTR(-ENOMEM);
 
 		ret = rbd_obj_read_sync(rbd_dev, rbd_dev->header_name,
-				       0, size,
-				       (char *) ondisk, version);
+				       0, size, ondisk);
 		if (ret < 0)
 			goto out_err;
-		if (WARN_ON((size_t) ret < size)) {
+		if ((size_t)ret < size) {
 			ret = -ENXIO;
 			rbd_warn(rbd_dev, "short header read (want %zd got %d)",
 				size, ret);
@@ -2260,46 +3095,36 @@ static int rbd_read_header(struct rbd_device *rbd_dev,
 			   struct rbd_image_header *header)
 {
 	struct rbd_image_header_ondisk *ondisk;
-	u64 ver = 0;
 	int ret;
 
-	ondisk = rbd_dev_v1_header_read(rbd_dev, &ver);
+	ondisk = rbd_dev_v1_header_read(rbd_dev);
 	if (IS_ERR(ondisk))
 		return PTR_ERR(ondisk);
 	ret = rbd_header_from_disk(header, ondisk);
-	if (ret >= 0)
-		header->obj_version = ver;
 	kfree(ondisk);
 
 	return ret;
 }
 
-static void rbd_remove_all_snaps(struct rbd_device *rbd_dev)
-{
-	struct rbd_snap *snap;
-	struct rbd_snap *next;
-
-	list_for_each_entry_safe(snap, next, &rbd_dev->snaps, node)
-		rbd_remove_snap_dev(snap);
-}
-
 static void rbd_update_mapping_size(struct rbd_device *rbd_dev)
 {
-	sector_t size;
-
 	if (rbd_dev->spec->snap_id != CEPH_NOSNAP)
 		return;
 
-	size = (sector_t) rbd_dev->header.image_size / SECTOR_SIZE;
-	dout("setting size to %llu sectors", (unsigned long long) size);
-	rbd_dev->mapping.size = (u64) size;
-	set_capacity(rbd_dev->disk, size);
+	if (rbd_dev->mapping.size != rbd_dev->header.image_size) {
+		sector_t size;
+
+		rbd_dev->mapping.size = rbd_dev->header.image_size;
+		size = (sector_t)rbd_dev->mapping.size / SECTOR_SIZE;
+		dout("setting size to %llu sectors", (unsigned long long)size);
+		set_capacity(rbd_dev->disk, size);
+	}
 }
 
 /*
  * only read the first part of the ondisk header, without the snaps info
  */
-static int rbd_dev_v1_refresh(struct rbd_device *rbd_dev, u64 *hver)
+static int rbd_dev_v1_refresh(struct rbd_device *rbd_dev)
 {
 	int ret;
 	struct rbd_image_header h;
@@ -2320,37 +3145,61 @@ static int rbd_dev_v1_refresh(struct rbd_device *rbd_dev, u64 *hver)
 	/* osd requests may still refer to snapc */
 	ceph_put_snap_context(rbd_dev->header.snapc);
 
-	if (hver)
-		*hver = h.obj_version;
-	rbd_dev->header.obj_version = h.obj_version;
 	rbd_dev->header.image_size = h.image_size;
 	rbd_dev->header.snapc = h.snapc;
 	rbd_dev->header.snap_names = h.snap_names;
 	rbd_dev->header.snap_sizes = h.snap_sizes;
 	/* Free the extra copy of the object prefix */
-	WARN_ON(strcmp(rbd_dev->header.object_prefix, h.object_prefix));
+	if (strcmp(rbd_dev->header.object_prefix, h.object_prefix))
+		rbd_warn(rbd_dev, "object prefix changed (ignoring)");
 	kfree(h.object_prefix);
 
-	ret = rbd_dev_snaps_update(rbd_dev);
-	if (!ret)
-		ret = rbd_dev_snaps_register(rbd_dev);
-
 	up_write(&rbd_dev->header_rwsem);
 
 	return ret;
 }
 
-static int rbd_dev_refresh(struct rbd_device *rbd_dev, u64 *hver)
+/*
+ * Clear the rbd device's EXISTS flag if the snapshot it's mapped to
+ * has disappeared from the (just updated) snapshot context.
+ */
+static void rbd_exists_validate(struct rbd_device *rbd_dev)
+{
+	u64 snap_id;
+
+	if (!test_bit(RBD_DEV_FLAG_EXISTS, &rbd_dev->flags))
+		return;
+
+	snap_id = rbd_dev->spec->snap_id;
+	if (snap_id == CEPH_NOSNAP)
+		return;
+
+	if (rbd_dev_snap_index(rbd_dev, snap_id) == BAD_SNAP_INDEX)
+		clear_bit(RBD_DEV_FLAG_EXISTS, &rbd_dev->flags);
+}
+
+static int rbd_dev_refresh(struct rbd_device *rbd_dev)
 {
+	u64 image_size;
 	int ret;
 
 	rbd_assert(rbd_image_format_valid(rbd_dev->image_format));
+	image_size = rbd_dev->header.image_size;
 	mutex_lock_nested(&ctl_mutex, SINGLE_DEPTH_NESTING);
 	if (rbd_dev->image_format == 1)
-		ret = rbd_dev_v1_refresh(rbd_dev, hver);
+		ret = rbd_dev_v1_refresh(rbd_dev);
 	else
-		ret = rbd_dev_v2_refresh(rbd_dev, hver);
+		ret = rbd_dev_v2_refresh(rbd_dev);
+
+	/* If it's a mapped snapshot, validate its EXISTS flag */
+
+	rbd_exists_validate(rbd_dev);
 	mutex_unlock(&ctl_mutex);
+	if (ret)
+		rbd_warn(rbd_dev, "got notification but failed to "
+			   " update snaps: %d\n", ret);
+	if (image_size != rbd_dev->header.image_size)
+		revalidate_disk(rbd_dev->disk);
 
 	return ret;
 }
@@ -2394,8 +3243,6 @@ static int rbd_init_disk(struct rbd_device *rbd_dev)
 
 	rbd_dev->disk = disk;
 
-	set_capacity(rbd_dev->disk, rbd_dev->mapping.size / SECTOR_SIZE);
-
 	return 0;
 out_disk:
 	put_disk(disk);
@@ -2416,13 +3263,9 @@ static ssize_t rbd_size_show(struct device *dev,
 			     struct device_attribute *attr, char *buf)
 {
 	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
-	sector_t size;
-
-	down_read(&rbd_dev->header_rwsem);
-	size = get_capacity(rbd_dev->disk);
-	up_read(&rbd_dev->header_rwsem);
 
-	return sprintf(buf, "%llu\n", (unsigned long long) size * SECTOR_SIZE);
+	return sprintf(buf, "%llu\n",
+		(unsigned long long)rbd_dev->mapping.size);
 }
 
 /*
@@ -2435,7 +3278,7 @@ static ssize_t rbd_features_show(struct device *dev,
 	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
 
 	return sprintf(buf, "0x%016llx\n",
-			(unsigned long long) rbd_dev->mapping.features);
+			(unsigned long long)rbd_dev->mapping.features);
 }
 
 static ssize_t rbd_major_show(struct device *dev,
@@ -2443,7 +3286,11 @@ static ssize_t rbd_major_show(struct device *dev,
 {
 	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
 
-	return sprintf(buf, "%d\n", rbd_dev->major);
+	if (rbd_dev->major)
+		return sprintf(buf, "%d\n", rbd_dev->major);
+
+	return sprintf(buf, "(none)\n");
+
 }
 
 static ssize_t rbd_client_id_show(struct device *dev,
@@ -2469,7 +3316,7 @@ static ssize_t rbd_pool_id_show(struct device *dev,
 	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
 
 	return sprintf(buf, "%llu\n",
-		(unsigned long long) rbd_dev->spec->pool_id);
+			(unsigned long long) rbd_dev->spec->pool_id);
 }
 
 static ssize_t rbd_name_show(struct device *dev,
@@ -2555,7 +3402,7 @@ static ssize_t rbd_image_refresh(struct device *dev,
 	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
 	int ret;
 
-	ret = rbd_dev_refresh(rbd_dev, NULL);
+	ret = rbd_dev_refresh(rbd_dev);
 
 	return ret < 0 ? ret : size;
 }
@@ -2606,71 +3453,6 @@ static struct device_type rbd_device_type = {
 	.release	= rbd_sysfs_dev_release,
 };
 
-
-/*
-  sysfs - snapshots
-*/
-
-static ssize_t rbd_snap_size_show(struct device *dev,
-				  struct device_attribute *attr,
-				  char *buf)
-{
-	struct rbd_snap *snap = container_of(dev, struct rbd_snap, dev);
-
-	return sprintf(buf, "%llu\n", (unsigned long long)snap->size);
-}
-
-static ssize_t rbd_snap_id_show(struct device *dev,
-				struct device_attribute *attr,
-				char *buf)
-{
-	struct rbd_snap *snap = container_of(dev, struct rbd_snap, dev);
-
-	return sprintf(buf, "%llu\n", (unsigned long long)snap->id);
-}
-
-static ssize_t rbd_snap_features_show(struct device *dev,
-				struct device_attribute *attr,
-				char *buf)
-{
-	struct rbd_snap *snap = container_of(dev, struct rbd_snap, dev);
-
-	return sprintf(buf, "0x%016llx\n",
-			(unsigned long long) snap->features);
-}
-
-static DEVICE_ATTR(snap_size, S_IRUGO, rbd_snap_size_show, NULL);
-static DEVICE_ATTR(snap_id, S_IRUGO, rbd_snap_id_show, NULL);
-static DEVICE_ATTR(snap_features, S_IRUGO, rbd_snap_features_show, NULL);
-
-static struct attribute *rbd_snap_attrs[] = {
-	&dev_attr_snap_size.attr,
-	&dev_attr_snap_id.attr,
-	&dev_attr_snap_features.attr,
-	NULL,
-};
-
-static struct attribute_group rbd_snap_attr_group = {
-	.attrs = rbd_snap_attrs,
-};
-
-static void rbd_snap_dev_release(struct device *dev)
-{
-	struct rbd_snap *snap = container_of(dev, struct rbd_snap, dev);
-	kfree(snap->name);
-	kfree(snap);
-}
-
-static const struct attribute_group *rbd_snap_attr_groups[] = {
-	&rbd_snap_attr_group,
-	NULL
-};
-
-static struct device_type rbd_snap_device_type = {
-	.groups		= rbd_snap_attr_groups,
-	.release	= rbd_snap_dev_release,
-};
-
 static struct rbd_spec *rbd_spec_get(struct rbd_spec *spec)
 {
 	kref_get(&spec->kref);
@@ -2694,8 +3476,6 @@ static struct rbd_spec *rbd_spec_alloc(void)
 		return NULL;
 	kref_init(&spec->kref);
 
-	rbd_spec_put(rbd_spec_get(spec));	/* TEMPORARY */
-
 	return spec;
 }
 
@@ -2722,7 +3502,6 @@ static struct rbd_device *rbd_dev_create(struct rbd_client *rbdc,
 	spin_lock_init(&rbd_dev->lock);
 	rbd_dev->flags = 0;
 	INIT_LIST_HEAD(&rbd_dev->node);
-	INIT_LIST_HEAD(&rbd_dev->snaps);
 	init_rwsem(&rbd_dev->header_rwsem);
 
 	rbd_dev->spec = spec;
@@ -2740,96 +3519,11 @@ static struct rbd_device *rbd_dev_create(struct rbd_client *rbdc,
 
 static void rbd_dev_destroy(struct rbd_device *rbd_dev)
 {
-	rbd_spec_put(rbd_dev->parent_spec);
-	kfree(rbd_dev->header_name);
 	rbd_put_client(rbd_dev->rbd_client);
 	rbd_spec_put(rbd_dev->spec);
 	kfree(rbd_dev);
 }
 
-static bool rbd_snap_registered(struct rbd_snap *snap)
-{
-	bool ret = snap->dev.type == &rbd_snap_device_type;
-	bool reg = device_is_registered(&snap->dev);
-
-	rbd_assert(!ret ^ reg);
-
-	return ret;
-}
-
-static void rbd_remove_snap_dev(struct rbd_snap *snap)
-{
-	list_del(&snap->node);
-	if (device_is_registered(&snap->dev))
-		device_unregister(&snap->dev);
-}
-
-static int rbd_register_snap_dev(struct rbd_snap *snap,
-				  struct device *parent)
-{
-	struct device *dev = &snap->dev;
-	int ret;
-
-	dev->type = &rbd_snap_device_type;
-	dev->parent = parent;
-	dev->release = rbd_snap_dev_release;
-	dev_set_name(dev, "%s%s", RBD_SNAP_DEV_NAME_PREFIX, snap->name);
-	dout("%s: registering device for snapshot %s\n", __func__, snap->name);
-
-	ret = device_register(dev);
-
-	return ret;
-}
-
-static struct rbd_snap *__rbd_add_snap_dev(struct rbd_device *rbd_dev,
-						const char *snap_name,
-						u64 snap_id, u64 snap_size,
-						u64 snap_features)
-{
-	struct rbd_snap *snap;
-	int ret;
-
-	snap = kzalloc(sizeof (*snap), GFP_KERNEL);
-	if (!snap)
-		return ERR_PTR(-ENOMEM);
-
-	ret = -ENOMEM;
-	snap->name = kstrdup(snap_name, GFP_KERNEL);
-	if (!snap->name)
-		goto err;
-
-	snap->id = snap_id;
-	snap->size = snap_size;
-	snap->features = snap_features;
-
-	return snap;
-
-err:
-	kfree(snap->name);
-	kfree(snap);
-
-	return ERR_PTR(ret);
-}
-
-static char *rbd_dev_v1_snap_info(struct rbd_device *rbd_dev, u32 which,
-		u64 *snap_size, u64 *snap_features)
-{
-	char *snap_name;
-
-	rbd_assert(which < rbd_dev->header.snapc->num_snaps);
-
-	*snap_size = rbd_dev->header.snap_sizes[which];
-	*snap_features = 0;	/* No features for v1 */
-
-	/* Skip over names until we find the one we are looking for */
-
-	snap_name = rbd_dev->header.snap_names;
-	while (which--)
-		snap_name += strlen(snap_name) + 1;
-
-	return snap_name;
-}
-
 /*
  * Get the size and object order for an image snapshot, or if
  * snap_id is CEPH_NOSNAP, gets this information for the base
@@ -2847,18 +3541,21 @@ static int _rbd_dev_v2_snap_size(struct rbd_device *rbd_dev, u64 snap_id,
 
 	ret = rbd_obj_method_sync(rbd_dev, rbd_dev->header_name,
 				"rbd", "get_size",
-				(char *) &snapid, sizeof (snapid),
-				(char *) &size_buf, sizeof (size_buf), NULL);
+				&snapid, sizeof (snapid),
+				&size_buf, sizeof (size_buf));
 	dout("%s: rbd_obj_method_sync returned %d\n", __func__, ret);
 	if (ret < 0)
 		return ret;
+	if (ret < sizeof (size_buf))
+		return -ERANGE;
 
-	*order = size_buf.order;
+	if (order)
+		*order = size_buf.order;
 	*snap_size = le64_to_cpu(size_buf.size);
 
 	dout("  snap_id 0x%016llx order = %u, snap_size = %llu\n",
-		(unsigned long long) snap_id, (unsigned int) *order,
-		(unsigned long long) *snap_size);
+		(unsigned long long)snap_id, (unsigned int)*order,
+		(unsigned long long)*snap_size);
 
 	return 0;
 }
@@ -2881,17 +3578,16 @@ static int rbd_dev_v2_object_prefix(struct rbd_device *rbd_dev)
 		return -ENOMEM;
 
 	ret = rbd_obj_method_sync(rbd_dev, rbd_dev->header_name,
-				"rbd", "get_object_prefix",
-				NULL, 0,
-				reply_buf, RBD_OBJ_PREFIX_LEN_MAX, NULL);
+				"rbd", "get_object_prefix", NULL, 0,
+				reply_buf, RBD_OBJ_PREFIX_LEN_MAX);
 	dout("%s: rbd_obj_method_sync returned %d\n", __func__, ret);
 	if (ret < 0)
 		goto out;
 
 	p = reply_buf;
 	rbd_dev->header.object_prefix = ceph_extract_encoded_string(&p,
-						p + RBD_OBJ_PREFIX_LEN_MAX,
-						NULL, GFP_NOIO);
+						p + ret, NULL, GFP_NOIO);
+	ret = 0;
 
 	if (IS_ERR(rbd_dev->header.object_prefix)) {
 		ret = PTR_ERR(rbd_dev->header.object_prefix);
@@ -2899,7 +3595,6 @@ static int rbd_dev_v2_object_prefix(struct rbd_device *rbd_dev)
 	} else {
 		dout("  object_prefix = %s\n", rbd_dev->header.object_prefix);
 	}
-
 out:
 	kfree(reply_buf);
 
@@ -2913,29 +3608,30 @@ static int _rbd_dev_v2_snap_features(struct rbd_device *rbd_dev, u64 snap_id,
 	struct {
 		__le64 features;
 		__le64 incompat;
-	} features_buf = { 0 };
+	} __attribute__ ((packed)) features_buf = { 0 };
 	u64 incompat;
 	int ret;
 
 	ret = rbd_obj_method_sync(rbd_dev, rbd_dev->header_name,
 				"rbd", "get_features",
-				(char *) &snapid, sizeof (snapid),
-				(char *) &features_buf, sizeof (features_buf),
-				NULL);
+				&snapid, sizeof (snapid),
+				&features_buf, sizeof (features_buf));
 	dout("%s: rbd_obj_method_sync returned %d\n", __func__, ret);
 	if (ret < 0)
 		return ret;
+	if (ret < sizeof (features_buf))
+		return -ERANGE;
 
 	incompat = le64_to_cpu(features_buf.incompat);
-	if (incompat & ~RBD_FEATURES_ALL)
+	if (incompat & ~RBD_FEATURES_SUPPORTED)
 		return -ENXIO;
 
 	*snap_features = le64_to_cpu(features_buf.features);
 
 	dout("  snap_id 0x%016llx features = 0x%016llx incompat = 0x%016llx\n",
-		(unsigned long long) snap_id,
-		(unsigned long long) *snap_features,
-		(unsigned long long) le64_to_cpu(features_buf.incompat));
+		(unsigned long long)snap_id,
+		(unsigned long long)*snap_features,
+		(unsigned long long)le64_to_cpu(features_buf.incompat));
 
 	return 0;
 }
@@ -2975,15 +3671,15 @@ static int rbd_dev_v2_parent_info(struct rbd_device *rbd_dev)
 	snapid = cpu_to_le64(CEPH_NOSNAP);
 	ret = rbd_obj_method_sync(rbd_dev, rbd_dev->header_name,
 				"rbd", "get_parent",
-				(char *) &snapid, sizeof (snapid),
-				(char *) reply_buf, size, NULL);
+				&snapid, sizeof (snapid),
+				reply_buf, size);
 	dout("%s: rbd_obj_method_sync returned %d\n", __func__, ret);
 	if (ret < 0)
 		goto out_err;
 
-	ret = -ERANGE;
 	p = reply_buf;
-	end = (char *) reply_buf + size;
+	end = reply_buf + ret;
+	ret = -ERANGE;
 	ceph_decode_64_safe(&p, end, parent_spec->pool_id, out_err);
 	if (parent_spec->pool_id == CEPH_NOPOOL)
 		goto out;	/* No parent?  No problem. */
@@ -2991,8 +3687,11 @@ static int rbd_dev_v2_parent_info(struct rbd_device *rbd_dev)
 	/* The ceph file layout needs to fit pool id in 32 bits */
 
 	ret = -EIO;
-	if (WARN_ON(parent_spec->pool_id > (u64) U32_MAX))
-		goto out;
+	if (parent_spec->pool_id > (u64)U32_MAX) {
+		rbd_warn(NULL, "parent pool id too large (%llu > %u)\n",
+			(unsigned long long)parent_spec->pool_id, U32_MAX);
+		goto out_err;
+	}
 
 	image_id = ceph_extract_encoded_string(&p, end, NULL, GFP_KERNEL);
 	if (IS_ERR(image_id)) {
@@ -3015,6 +3714,56 @@ out_err:
 	return ret;
 }
 
+static int rbd_dev_v2_striping_info(struct rbd_device *rbd_dev)
+{
+	struct {
+		__le64 stripe_unit;
+		__le64 stripe_count;
+	} __attribute__ ((packed)) striping_info_buf = { 0 };
+	size_t size = sizeof (striping_info_buf);
+	void *p;
+	u64 obj_size;
+	u64 stripe_unit;
+	u64 stripe_count;
+	int ret;
+
+	ret = rbd_obj_method_sync(rbd_dev, rbd_dev->header_name,
+				"rbd", "get_stripe_unit_count", NULL, 0,
+				(char *)&striping_info_buf, size);
+	dout("%s: rbd_obj_method_sync returned %d\n", __func__, ret);
+	if (ret < 0)
+		return ret;
+	if (ret < size)
+		return -ERANGE;
+
+	/*
+	 * We don't actually support the "fancy striping" feature
+	 * (STRIPINGV2) yet, but if the striping sizes are the
+	 * defaults the behavior is the same as before.  So find
+	 * out, and only fail if the image has non-default values.
+	 */
+	ret = -EINVAL;
+	obj_size = (u64)1 << rbd_dev->header.obj_order;
+	p = &striping_info_buf;
+	stripe_unit = ceph_decode_64(&p);
+	if (stripe_unit != obj_size) {
+		rbd_warn(rbd_dev, "unsupported stripe unit "
+				"(got %llu want %llu)",
+				stripe_unit, obj_size);
+		return -EINVAL;
+	}
+	stripe_count = ceph_decode_64(&p);
+	if (stripe_count != 1) {
+		rbd_warn(rbd_dev, "unsupported stripe count "
+				"(got %llu want 1)", stripe_count);
+		return -EINVAL;
+	}
+	rbd_dev->header.stripe_unit = stripe_unit;
+	rbd_dev->header.stripe_count = stripe_count;
+
+	return 0;
+}
+
 static char *rbd_dev_image_name(struct rbd_device *rbd_dev)
 {
 	size_t image_id_size;
@@ -3036,8 +3785,8 @@ static char *rbd_dev_image_name(struct rbd_device *rbd_dev)
 		return NULL;
 
 	p = image_id;
-	end = (char *) image_id + image_id_size;
-	ceph_encode_string(&p, end, rbd_dev->spec->image_id, (u32) len);
+	end = image_id + image_id_size;
+	ceph_encode_string(&p, end, rbd_dev->spec->image_id, (u32)len);
 
 	size = sizeof (__le32) + RBD_IMAGE_NAME_LEN_MAX;
 	reply_buf = kmalloc(size, GFP_KERNEL);
@@ -3047,11 +3796,12 @@ static char *rbd_dev_image_name(struct rbd_device *rbd_dev)
 	ret = rbd_obj_method_sync(rbd_dev, RBD_DIRECTORY,
 				"rbd", "dir_get_name",
 				image_id, image_id_size,
-				(char *) reply_buf, size, NULL);
+				reply_buf, size);
 	if (ret < 0)
 		goto out;
 	p = reply_buf;
-	end = (char *) reply_buf + size;
+	end = reply_buf + ret;
+
 	image_name = ceph_extract_encoded_string(&p, end, &len, GFP_KERNEL);
 	if (IS_ERR(image_name))
 		image_name = NULL;
@@ -3064,69 +3814,134 @@ out:
 	return image_name;
 }
 
+static u64 rbd_v1_snap_id_by_name(struct rbd_device *rbd_dev, const char *name)
+{
+	struct ceph_snap_context *snapc = rbd_dev->header.snapc;
+	const char *snap_name;
+	u32 which = 0;
+
+	/* Skip over names until we find the one we are looking for */
+
+	snap_name = rbd_dev->header.snap_names;
+	while (which < snapc->num_snaps) {
+		if (!strcmp(name, snap_name))
+			return snapc->snaps[which];
+		snap_name += strlen(snap_name) + 1;
+		which++;
+	}
+	return CEPH_NOSNAP;
+}
+
+static u64 rbd_v2_snap_id_by_name(struct rbd_device *rbd_dev, const char *name)
+{
+	struct ceph_snap_context *snapc = rbd_dev->header.snapc;
+	u32 which;
+	bool found = false;
+	u64 snap_id;
+
+	for (which = 0; !found && which < snapc->num_snaps; which++) {
+		const char *snap_name;
+
+		snap_id = snapc->snaps[which];
+		snap_name = rbd_dev_v2_snap_name(rbd_dev, snap_id);
+		if (IS_ERR(snap_name))
+			break;
+		found = !strcmp(name, snap_name);
+		kfree(snap_name);
+	}
+	return found ? snap_id : CEPH_NOSNAP;
+}
+
 /*
- * When a parent image gets probed, we only have the pool, image,
- * and snapshot ids but not the names of any of them.  This call
- * is made later to fill in those names.  It has to be done after
- * rbd_dev_snaps_update() has completed because some of the
- * information (in particular, snapshot name) is not available
- * until then.
+ * Assumes name is never RBD_SNAP_HEAD_NAME; returns CEPH_NOSNAP if
+ * no snapshot by that name is found, or if an error occurs.
  */
-static int rbd_dev_probe_update_spec(struct rbd_device *rbd_dev)
+static u64 rbd_snap_id_by_name(struct rbd_device *rbd_dev, const char *name)
 {
-	struct ceph_osd_client *osdc;
-	const char *name;
-	void *reply_buf = NULL;
+	if (rbd_dev->image_format == 1)
+		return rbd_v1_snap_id_by_name(rbd_dev, name);
+
+	return rbd_v2_snap_id_by_name(rbd_dev, name);
+}
+
+/*
+ * When an rbd image has a parent image, it is identified by the
+ * pool, image, and snapshot ids (not names).  This function fills
+ * in the names for those ids.  (It's OK if we can't figure out the
+ * name for an image id, but the pool and snapshot ids should always
+ * exist and have names.)  All names in an rbd spec are dynamically
+ * allocated.
+ *
+ * When an image being mapped (not a parent) is probed, we have the
+ * pool name and pool id, image name and image id, and the snapshot
+ * name.  The only thing we're missing is the snapshot id.
+ */
+static int rbd_dev_spec_update(struct rbd_device *rbd_dev)
+{
+	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
+	struct rbd_spec *spec = rbd_dev->spec;
+	const char *pool_name;
+	const char *image_name;
+	const char *snap_name;
 	int ret;
 
-	if (rbd_dev->spec->pool_name)
-		return 0;	/* Already have the names */
+	/*
+	 * An image being mapped will have the pool name (etc.), but
+	 * we need to look up the snapshot id.
+	 */
+	if (spec->pool_name) {
+		if (strcmp(spec->snap_name, RBD_SNAP_HEAD_NAME)) {
+			u64 snap_id;
+
+			snap_id = rbd_snap_id_by_name(rbd_dev, spec->snap_name);
+			if (snap_id == CEPH_NOSNAP)
+				return -ENOENT;
+			spec->snap_id = snap_id;
+		} else {
+			spec->snap_id = CEPH_NOSNAP;
+		}
 
-	/* Look up the pool name */
+		return 0;
+	}
 
-	osdc = &rbd_dev->rbd_client->client->osdc;
-	name = ceph_pg_pool_name_by_id(osdc->osdmap, rbd_dev->spec->pool_id);
-	if (!name) {
-		rbd_warn(rbd_dev, "there is no pool with id %llu",
-			rbd_dev->spec->pool_id);	/* Really a BUG() */
+	/* Get the pool name; we have to make our own copy of this */
+
+	pool_name = ceph_pg_pool_name_by_id(osdc->osdmap, spec->pool_id);
+	if (!pool_name) {
+		rbd_warn(rbd_dev, "no pool with id %llu", spec->pool_id);
 		return -EIO;
 	}
-
-	rbd_dev->spec->pool_name = kstrdup(name, GFP_KERNEL);
-	if (!rbd_dev->spec->pool_name)
+	pool_name = kstrdup(pool_name, GFP_KERNEL);
+	if (!pool_name)
 		return -ENOMEM;
 
 	/* Fetch the image name; tolerate failure here */
 
-	name = rbd_dev_image_name(rbd_dev);
-	if (name)
-		rbd_dev->spec->image_name = (char *) name;
-	else
+	image_name = rbd_dev_image_name(rbd_dev);
+	if (!image_name)
 		rbd_warn(rbd_dev, "unable to get image name");
 
-	/* Look up the snapshot name. */
+	/* Look up the snapshot name, and make a copy */
 
-	name = rbd_snap_name(rbd_dev, rbd_dev->spec->snap_id);
-	if (!name) {
-		rbd_warn(rbd_dev, "no snapshot with id %llu",
-			rbd_dev->spec->snap_id);	/* Really a BUG() */
-		ret = -EIO;
+	snap_name = rbd_snap_name(rbd_dev, spec->snap_id);
+	if (!snap_name) {
+		ret = -ENOMEM;
 		goto out_err;
 	}
-	rbd_dev->spec->snap_name = kstrdup(name, GFP_KERNEL);
-	if(!rbd_dev->spec->snap_name)
-		goto out_err;
+
+	spec->pool_name = pool_name;
+	spec->image_name = image_name;
+	spec->snap_name = snap_name;
 
 	return 0;
 out_err:
-	kfree(reply_buf);
-	kfree(rbd_dev->spec->pool_name);
-	rbd_dev->spec->pool_name = NULL;
+	kfree(image_name);
+	kfree(pool_name);
 
 	return ret;
 }
 
-static int rbd_dev_v2_snap_context(struct rbd_device *rbd_dev, u64 *ver)
+static int rbd_dev_v2_snap_context(struct rbd_device *rbd_dev)
 {
 	size_t size;
 	int ret;
@@ -3151,16 +3966,15 @@ static int rbd_dev_v2_snap_context(struct rbd_device *rbd_dev, u64 *ver)
 		return -ENOMEM;
 
 	ret = rbd_obj_method_sync(rbd_dev, rbd_dev->header_name,
-				"rbd", "get_snapcontext",
-				NULL, 0,
-				reply_buf, size, ver);
+				"rbd", "get_snapcontext", NULL, 0,
+				reply_buf, size);
 	dout("%s: rbd_obj_method_sync returned %d\n", __func__, ret);
 	if (ret < 0)
 		goto out;
 
-	ret = -ERANGE;
 	p = reply_buf;
-	end = (char *) reply_buf + size;
+	end = reply_buf + ret;
+	ret = -ERANGE;
 	ceph_decode_64_safe(&p, end, seq, out);
 	ceph_decode_32_safe(&p, end, snap_count, out);
 
@@ -3177,37 +3991,33 @@ static int rbd_dev_v2_snap_context(struct rbd_device *rbd_dev, u64 *ver)
 	}
 	if (!ceph_has_room(&p, end, snap_count * sizeof (__le64)))
 		goto out;
+	ret = 0;
 
-	size = sizeof (struct ceph_snap_context) +
-				snap_count * sizeof (snapc->snaps[0]);
-	snapc = kmalloc(size, GFP_KERNEL);
+	snapc = ceph_create_snap_context(snap_count, GFP_KERNEL);
 	if (!snapc) {
 		ret = -ENOMEM;
 		goto out;
 	}
-
-	atomic_set(&snapc->nref, 1);
 	snapc->seq = seq;
-	snapc->num_snaps = snap_count;
 	for (i = 0; i < snap_count; i++)
 		snapc->snaps[i] = ceph_decode_64(&p);
 
 	rbd_dev->header.snapc = snapc;
 
 	dout("  snap context seq = %llu, snap_count = %u\n",
-		(unsigned long long) seq, (unsigned int) snap_count);
-
+		(unsigned long long)seq, (unsigned int)snap_count);
 out:
 	kfree(reply_buf);
 
-	return 0;
+	return ret;
 }
 
-static char *rbd_dev_v2_snap_name(struct rbd_device *rbd_dev, u32 which)
+static const char *rbd_dev_v2_snap_name(struct rbd_device *rbd_dev,
+					u64 snap_id)
 {
 	size_t size;
 	void *reply_buf;
-	__le64 snap_id;
+	__le64 snapid;
 	int ret;
 	void *p;
 	void *end;
@@ -3218,236 +4028,52 @@ static char *rbd_dev_v2_snap_name(struct rbd_device *rbd_dev, u32 which)
 	if (!reply_buf)
 		return ERR_PTR(-ENOMEM);
 
-	snap_id = cpu_to_le64(rbd_dev->header.snapc->snaps[which]);
+	snapid = cpu_to_le64(snap_id);
 	ret = rbd_obj_method_sync(rbd_dev, rbd_dev->header_name,
 				"rbd", "get_snapshot_name",
-				(char *) &snap_id, sizeof (snap_id),
-				reply_buf, size, NULL);
+				&snapid, sizeof (snapid),
+				reply_buf, size);
 	dout("%s: rbd_obj_method_sync returned %d\n", __func__, ret);
-	if (ret < 0)
+	if (ret < 0) {
+		snap_name = ERR_PTR(ret);
 		goto out;
+	}
 
 	p = reply_buf;
-	end = (char *) reply_buf + size;
+	end = reply_buf + ret;
 	snap_name = ceph_extract_encoded_string(&p, end, NULL, GFP_KERNEL);
-	if (IS_ERR(snap_name)) {
-		ret = PTR_ERR(snap_name);
+	if (IS_ERR(snap_name))
 		goto out;
-	} else {
-		dout("  snap_id 0x%016llx snap_name = %s\n",
-			(unsigned long long) le64_to_cpu(snap_id), snap_name);
-	}
-	kfree(reply_buf);
 
-	return snap_name;
+	dout("  snap_id 0x%016llx snap_name = %s\n",
+		(unsigned long long)snap_id, snap_name);
 out:
 	kfree(reply_buf);
 
-	return ERR_PTR(ret);
-}
-
-static char *rbd_dev_v2_snap_info(struct rbd_device *rbd_dev, u32 which,
-		u64 *snap_size, u64 *snap_features)
-{
-	u64 snap_id;
-	u8 order;
-	int ret;
-
-	snap_id = rbd_dev->header.snapc->snaps[which];
-	ret = _rbd_dev_v2_snap_size(rbd_dev, snap_id, &order, snap_size);
-	if (ret)
-		return ERR_PTR(ret);
-	ret = _rbd_dev_v2_snap_features(rbd_dev, snap_id, snap_features);
-	if (ret)
-		return ERR_PTR(ret);
-
-	return rbd_dev_v2_snap_name(rbd_dev, which);
-}
-
-static char *rbd_dev_snap_info(struct rbd_device *rbd_dev, u32 which,
-		u64 *snap_size, u64 *snap_features)
-{
-	if (rbd_dev->image_format == 1)
-		return rbd_dev_v1_snap_info(rbd_dev, which,
-					snap_size, snap_features);
-	if (rbd_dev->image_format == 2)
-		return rbd_dev_v2_snap_info(rbd_dev, which,
-					snap_size, snap_features);
-	return ERR_PTR(-EINVAL);
+	return snap_name;
 }
 
-static int rbd_dev_v2_refresh(struct rbd_device *rbd_dev, u64 *hver)
+static int rbd_dev_v2_refresh(struct rbd_device *rbd_dev)
 {
 	int ret;
-	__u8 obj_order;
 
 	down_write(&rbd_dev->header_rwsem);
 
-	/* Grab old order first, to see if it changes */
-
-	obj_order = rbd_dev->header.obj_order,
 	ret = rbd_dev_v2_image_size(rbd_dev);
 	if (ret)
 		goto out;
-	if (rbd_dev->header.obj_order != obj_order) {
-		ret = -EIO;
-		goto out;
-	}
 	rbd_update_mapping_size(rbd_dev);
 
-	ret = rbd_dev_v2_snap_context(rbd_dev, hver);
+	ret = rbd_dev_v2_snap_context(rbd_dev);
 	dout("rbd_dev_v2_snap_context returned %d\n", ret);
 	if (ret)
 		goto out;
-	ret = rbd_dev_snaps_update(rbd_dev);
-	dout("rbd_dev_snaps_update returned %d\n", ret);
-	if (ret)
-		goto out;
-	ret = rbd_dev_snaps_register(rbd_dev);
-	dout("rbd_dev_snaps_register returned %d\n", ret);
 out:
 	up_write(&rbd_dev->header_rwsem);
 
 	return ret;
 }
 
-/*
- * Scan the rbd device's current snapshot list and compare it to the
- * newly-received snapshot context.  Remove any existing snapshots
- * not present in the new snapshot context.  Add a new snapshot for
- * any snaphots in the snapshot context not in the current list.
- * And verify there are no changes to snapshots we already know
- * about.
- *
- * Assumes the snapshots in the snapshot context are sorted by
- * snapshot id, highest id first.  (Snapshots in the rbd_dev's list
- * are also maintained in that order.)
- */
-static int rbd_dev_snaps_update(struct rbd_device *rbd_dev)
-{
-	struct ceph_snap_context *snapc = rbd_dev->header.snapc;
-	const u32 snap_count = snapc->num_snaps;
-	struct list_head *head = &rbd_dev->snaps;
-	struct list_head *links = head->next;
-	u32 index = 0;
-
-	dout("%s: snap count is %u\n", __func__, (unsigned int) snap_count);
-	while (index < snap_count || links != head) {
-		u64 snap_id;
-		struct rbd_snap *snap;
-		char *snap_name;
-		u64 snap_size = 0;
-		u64 snap_features = 0;
-
-		snap_id = index < snap_count ? snapc->snaps[index]
-					     : CEPH_NOSNAP;
-		snap = links != head ? list_entry(links, struct rbd_snap, node)
-				     : NULL;
-		rbd_assert(!snap || snap->id != CEPH_NOSNAP);
-
-		if (snap_id == CEPH_NOSNAP || (snap && snap->id > snap_id)) {
-			struct list_head *next = links->next;
-
-			/*
-			 * A previously-existing snapshot is not in
-			 * the new snap context.
-			 *
-			 * If the now missing snapshot is the one the
-			 * image is mapped to, clear its exists flag
-			 * so we can avoid sending any more requests
-			 * to it.
-			 */
-			if (rbd_dev->spec->snap_id == snap->id)
-				clear_bit(RBD_DEV_FLAG_EXISTS, &rbd_dev->flags);
-			rbd_remove_snap_dev(snap);
-			dout("%ssnap id %llu has been removed\n",
-				rbd_dev->spec->snap_id == snap->id ?
-							"mapped " : "",
-				(unsigned long long) snap->id);
-
-			/* Done with this list entry; advance */
-
-			links = next;
-			continue;
-		}
-
-		snap_name = rbd_dev_snap_info(rbd_dev, index,
-					&snap_size, &snap_features);
-		if (IS_ERR(snap_name))
-			return PTR_ERR(snap_name);
-
-		dout("entry %u: snap_id = %llu\n", (unsigned int) snap_count,
-			(unsigned long long) snap_id);
-		if (!snap || (snap_id != CEPH_NOSNAP && snap->id < snap_id)) {
-			struct rbd_snap *new_snap;
-
-			/* We haven't seen this snapshot before */
-
-			new_snap = __rbd_add_snap_dev(rbd_dev, snap_name,
-					snap_id, snap_size, snap_features);
-			if (IS_ERR(new_snap)) {
-				int err = PTR_ERR(new_snap);
-
-				dout("  failed to add dev, error %d\n", err);
-
-				return err;
-			}
-
-			/* New goes before existing, or at end of list */
-
-			dout("  added dev%s\n", snap ? "" : " at end\n");
-			if (snap)
-				list_add_tail(&new_snap->node, &snap->node);
-			else
-				list_add_tail(&new_snap->node, head);
-		} else {
-			/* Already have this one */
-
-			dout("  already present\n");
-
-			rbd_assert(snap->size == snap_size);
-			rbd_assert(!strcmp(snap->name, snap_name));
-			rbd_assert(snap->features == snap_features);
-
-			/* Done with this list entry; advance */
-
-			links = links->next;
-		}
-
-		/* Advance to the next entry in the snapshot context */
-
-		index++;
-	}
-	dout("%s: done\n", __func__);
-
-	return 0;
-}
-
-/*
- * Scan the list of snapshots and register the devices for any that
- * have not already been registered.
- */
-static int rbd_dev_snaps_register(struct rbd_device *rbd_dev)
-{
-	struct rbd_snap *snap;
-	int ret = 0;
-
-	dout("%s:\n", __func__);
-	if (WARN_ON(!device_is_registered(&rbd_dev->dev)))
-		return -EIO;
-
-	list_for_each_entry(snap, &rbd_dev->snaps, node) {
-		if (!rbd_snap_registered(snap)) {
-			ret = rbd_register_snap_dev(snap, &rbd_dev->dev);
-			if (ret < 0)
-				break;
-		}
-	}
-	dout("%s: returning %d\n", __func__, ret);
-
-	return ret;
-}
-
 static int rbd_bus_add_dev(struct rbd_device *rbd_dev)
 {
 	struct device *dev;
@@ -3459,7 +4085,7 @@ static int rbd_bus_add_dev(struct rbd_device *rbd_dev)
 	dev->bus = &rbd_bus_type;
 	dev->type = &rbd_device_type;
 	dev->parent = &rbd_root_dev;
-	dev->release = rbd_dev_release;
+	dev->release = rbd_dev_device_release;
 	dev_set_name(dev, "%d", rbd_dev->dev_id);
 	ret = device_register(dev);
 
@@ -3673,6 +4299,7 @@ static int rbd_add_parse_args(const char *buf,
 	size_t len;
 	char *options;
 	const char *mon_addrs;
+	char *snap_name;
 	size_t mon_addrs_size;
 	struct rbd_spec *spec = NULL;
 	struct rbd_options *rbd_opts = NULL;
@@ -3731,10 +4358,11 @@ static int rbd_add_parse_args(const char *buf,
 		ret = -ENAMETOOLONG;
 		goto out_err;
 	}
-	spec->snap_name = kmemdup(buf, len + 1, GFP_KERNEL);
-	if (!spec->snap_name)
+	snap_name = kmemdup(buf, len + 1, GFP_KERNEL);
+	if (!snap_name)
 		goto out_mem;
-	*(spec->snap_name + len) = '\0';
+	*(snap_name + len) = '\0';
+	spec->snap_name = snap_name;
 
 	/* Initialize all rbd options to the defaults */
 
@@ -3788,15 +4416,19 @@ static int rbd_dev_image_id(struct rbd_device *rbd_dev)
 	size_t size;
 	char *object_name;
 	void *response;
-	void *p;
+	char *image_id;
 
 	/*
 	 * When probing a parent image, the image id is already
 	 * known (and the image name likely is not).  There's no
-	 * need to fetch the image id again in this case.
+	 * need to fetch the image id again in this case.  We
+	 * do still need to set the image format though.
 	 */
-	if (rbd_dev->spec->image_id)
+	if (rbd_dev->spec->image_id) {
+		rbd_dev->image_format = *rbd_dev->spec->image_id ? 2 : 1;
+
 		return 0;
+	}
 
 	/*
 	 * First, see if the format 2 image id file exists, and if
@@ -3818,23 +4450,32 @@ static int rbd_dev_image_id(struct rbd_device *rbd_dev)
 		goto out;
 	}
 
+	/* If it doesn't exist we'll assume it's a format 1 image */
+
 	ret = rbd_obj_method_sync(rbd_dev, object_name,
-				"rbd", "get_id",
-				NULL, 0,
-				response, RBD_IMAGE_ID_LEN_MAX, NULL);
+				"rbd", "get_id", NULL, 0,
+				response, RBD_IMAGE_ID_LEN_MAX);
 	dout("%s: rbd_obj_method_sync returned %d\n", __func__, ret);
-	if (ret < 0)
-		goto out;
-
-	p = response;
-	rbd_dev->spec->image_id = ceph_extract_encoded_string(&p,
-						p + RBD_IMAGE_ID_LEN_MAX,
+	if (ret == -ENOENT) {
+		image_id = kstrdup("", GFP_KERNEL);
+		ret = image_id ? 0 : -ENOMEM;
+		if (!ret)
+			rbd_dev->image_format = 1;
+	} else if (ret > sizeof (__le32)) {
+		void *p = response;
+
+		image_id = ceph_extract_encoded_string(&p, p + ret,
 						NULL, GFP_NOIO);
-	if (IS_ERR(rbd_dev->spec->image_id)) {
-		ret = PTR_ERR(rbd_dev->spec->image_id);
-		rbd_dev->spec->image_id = NULL;
+		ret = IS_ERR(image_id) ? PTR_ERR(image_id) : 0;
+		if (!ret)
+			rbd_dev->image_format = 2;
 	} else {
-		dout("image_id is %s\n", rbd_dev->spec->image_id);
+		ret = -EINVAL;
+	}
+
+	if (!ret) {
+		rbd_dev->spec->image_id = image_id;
+		dout("image_id is %s\n", image_id);
 	}
 out:
 	kfree(response);
@@ -3843,27 +4484,30 @@ out:
 	return ret;
 }
 
-static int rbd_dev_v1_probe(struct rbd_device *rbd_dev)
+/* Undo whatever state changes are made by v1 or v2 image probe */
+
+static void rbd_dev_unprobe(struct rbd_device *rbd_dev)
 {
-	int ret;
-	size_t size;
+	struct rbd_image_header	*header;
 
-	/* Version 1 images have no id; empty string is used */
+	rbd_dev_remove_parent(rbd_dev);
+	rbd_spec_put(rbd_dev->parent_spec);
+	rbd_dev->parent_spec = NULL;
+	rbd_dev->parent_overlap = 0;
 
-	rbd_dev->spec->image_id = kstrdup("", GFP_KERNEL);
-	if (!rbd_dev->spec->image_id)
-		return -ENOMEM;
+	/* Free dynamic fields from the header, then zero it out */
 
-	/* Record the header object name for this rbd image. */
+	header = &rbd_dev->header;
+	ceph_put_snap_context(header->snapc);
+	kfree(header->snap_sizes);
+	kfree(header->snap_names);
+	kfree(header->object_prefix);
+	memset(header, 0, sizeof (*header));
+}
 
-	size = strlen(rbd_dev->spec->image_name) + sizeof (RBD_SUFFIX);
-	rbd_dev->header_name = kmalloc(size, GFP_KERNEL);
-	if (!rbd_dev->header_name) {
-		ret = -ENOMEM;
-		goto out_err;
-	}
-	sprintf(rbd_dev->header_name, "%s%s",
-		rbd_dev->spec->image_name, RBD_SUFFIX);
+static int rbd_dev_v1_probe(struct rbd_device *rbd_dev)
+{
+	int ret;
 
 	/* Populate rbd image metadata */
 
@@ -3876,8 +4520,6 @@ static int rbd_dev_v1_probe(struct rbd_device *rbd_dev)
 	rbd_dev->parent_spec = NULL;
 	rbd_dev->parent_overlap = 0;
 
-	rbd_dev->image_format = 1;
-
 	dout("discovered version 1 image, header name is %s\n",
 		rbd_dev->header_name);
 
@@ -3894,43 +4536,45 @@ out_err:
 
 static int rbd_dev_v2_probe(struct rbd_device *rbd_dev)
 {
-	size_t size;
 	int ret;
-	u64 ver = 0;
-
-	/*
-	 * Image id was filled in by the caller.  Record the header
-	 * object name for this rbd image.
-	 */
-	size = sizeof (RBD_HEADER_PREFIX) + strlen(rbd_dev->spec->image_id);
-	rbd_dev->header_name = kmalloc(size, GFP_KERNEL);
-	if (!rbd_dev->header_name)
-		return -ENOMEM;
-	sprintf(rbd_dev->header_name, "%s%s",
-			RBD_HEADER_PREFIX, rbd_dev->spec->image_id);
-
-	/* Get the size and object order for the image */
 
 	ret = rbd_dev_v2_image_size(rbd_dev);
-	if (ret < 0)
+	if (ret)
 		goto out_err;
 
 	/* Get the object prefix (a.k.a. block_name) for the image */
 
 	ret = rbd_dev_v2_object_prefix(rbd_dev);
-	if (ret < 0)
+	if (ret)
 		goto out_err;
 
 	/* Get the and check features for the image */
 
 	ret = rbd_dev_v2_features(rbd_dev);
-	if (ret < 0)
+	if (ret)
 		goto out_err;
 
 	/* If the image supports layering, get the parent info */
 
 	if (rbd_dev->header.features & RBD_FEATURE_LAYERING) {
 		ret = rbd_dev_v2_parent_info(rbd_dev);
+		if (ret)
+			goto out_err;
+
+		/*
+		 * Don't print a warning for parent images.  We can
+		 * tell this point because we won't know its pool
+		 * name yet (just its pool id).
+		 */
+		if (rbd_dev->spec->pool_name)
+			rbd_warn(rbd_dev, "WARNING: kernel layering "
+					"is EXPERIMENTAL!");
+	}
+
+	/* If the image supports fancy striping, get its parameters */
+
+	if (rbd_dev->header.features & RBD_FEATURE_STRIPINGV2) {
+		ret = rbd_dev_v2_striping_info(rbd_dev);
 		if (ret < 0)
 			goto out_err;
 	}
@@ -3942,12 +4586,9 @@ static int rbd_dev_v2_probe(struct rbd_device *rbd_dev)
 
 	/* Get the snapshot context, plus the header version */
 
-	ret = rbd_dev_v2_snap_context(rbd_dev, &ver);
+	ret = rbd_dev_v2_snap_context(rbd_dev);
 	if (ret)
 		goto out_err;
-	rbd_dev->header.obj_version = ver;
-
-	rbd_dev->image_format = 2;
 
 	dout("discovered version 2 image, header name is %s\n",
 		rbd_dev->header_name);
@@ -3965,22 +4606,54 @@ out_err:
 	return ret;
 }
 
-static int rbd_dev_probe_finish(struct rbd_device *rbd_dev)
+static int rbd_dev_probe_parent(struct rbd_device *rbd_dev)
 {
+	struct rbd_device *parent = NULL;
+	struct rbd_spec *parent_spec;
+	struct rbd_client *rbdc;
 	int ret;
 
-	/* no need to lock here, as rbd_dev is not registered yet */
-	ret = rbd_dev_snaps_update(rbd_dev);
-	if (ret)
-		return ret;
+	if (!rbd_dev->parent_spec)
+		return 0;
+	/*
+	 * We need to pass a reference to the client and the parent
+	 * spec when creating the parent rbd_dev.  Images related by
+	 * parent/child relationships always share both.
+	 */
+	parent_spec = rbd_spec_get(rbd_dev->parent_spec);
+	rbdc = __rbd_get_client(rbd_dev->rbd_client);
 
-	ret = rbd_dev_probe_update_spec(rbd_dev);
-	if (ret)
-		goto err_out_snaps;
+	ret = -ENOMEM;
+	parent = rbd_dev_create(rbdc, parent_spec);
+	if (!parent)
+		goto out_err;
+
+	ret = rbd_dev_image_probe(parent);
+	if (ret < 0)
+		goto out_err;
+	rbd_dev->parent = parent;
+
+	return 0;
+out_err:
+	if (parent) {
+		rbd_spec_put(rbd_dev->parent_spec);
+		kfree(rbd_dev->header_name);
+		rbd_dev_destroy(parent);
+	} else {
+		rbd_put_client(rbdc);
+		rbd_spec_put(parent_spec);
+	}
+
+	return ret;
+}
+
+static int rbd_dev_device_setup(struct rbd_device *rbd_dev)
+{
+	int ret;
 
-	ret = rbd_dev_set_mapping(rbd_dev);
+	ret = rbd_dev_mapping_set(rbd_dev);
 	if (ret)
-		goto err_out_snaps;
+		return ret;
 
 	/* generate unique id: find highest unique id, add one */
 	rbd_dev_id_get(rbd_dev);
@@ -4007,54 +4680,81 @@ static int rbd_dev_probe_finish(struct rbd_device *rbd_dev)
 	if (ret)
 		goto err_out_disk;
 
-	/*
-	 * At this point cleanup in the event of an error is the job
-	 * of the sysfs code (initiated by rbd_bus_del_dev()).
-	 */
-	down_write(&rbd_dev->header_rwsem);
-	ret = rbd_dev_snaps_register(rbd_dev);
-	up_write(&rbd_dev->header_rwsem);
-	if (ret)
-		goto err_out_bus;
-
-	ret = rbd_dev_header_watch_sync(rbd_dev, 1);
-	if (ret)
-		goto err_out_bus;
-
 	/* Everything's ready.  Announce the disk to the world. */
 
+	set_capacity(rbd_dev->disk, rbd_dev->mapping.size / SECTOR_SIZE);
+	set_bit(RBD_DEV_FLAG_EXISTS, &rbd_dev->flags);
 	add_disk(rbd_dev->disk);
 
 	pr_info("%s: added with size 0x%llx\n", rbd_dev->disk->disk_name,
 		(unsigned long long) rbd_dev->mapping.size);
 
 	return ret;
-err_out_bus:
-	/* this will also clean up rest of rbd_dev stuff */
 
-	rbd_bus_del_dev(rbd_dev);
-
-	return ret;
 err_out_disk:
 	rbd_free_disk(rbd_dev);
 err_out_blkdev:
 	unregister_blkdev(rbd_dev->major, rbd_dev->name);
 err_out_id:
 	rbd_dev_id_put(rbd_dev);
-err_out_snaps:
-	rbd_remove_all_snaps(rbd_dev);
+	rbd_dev_mapping_clear(rbd_dev);
 
 	return ret;
 }
 
+static int rbd_dev_header_name(struct rbd_device *rbd_dev)
+{
+	struct rbd_spec *spec = rbd_dev->spec;
+	size_t size;
+
+	/* Record the header object name for this rbd image. */
+
+	rbd_assert(rbd_image_format_valid(rbd_dev->image_format));
+
+	if (rbd_dev->image_format == 1)
+		size = strlen(spec->image_name) + sizeof (RBD_SUFFIX);
+	else
+		size = sizeof (RBD_HEADER_PREFIX) + strlen(spec->image_id);
+
+	rbd_dev->header_name = kmalloc(size, GFP_KERNEL);
+	if (!rbd_dev->header_name)
+		return -ENOMEM;
+
+	if (rbd_dev->image_format == 1)
+		sprintf(rbd_dev->header_name, "%s%s",
+			spec->image_name, RBD_SUFFIX);
+	else
+		sprintf(rbd_dev->header_name, "%s%s",
+			RBD_HEADER_PREFIX, spec->image_id);
+	return 0;
+}
+
+static void rbd_dev_image_release(struct rbd_device *rbd_dev)
+{
+	int ret;
+
+	rbd_dev_unprobe(rbd_dev);
+	ret = rbd_dev_header_watch_sync(rbd_dev, 0);
+	if (ret)
+		rbd_warn(rbd_dev, "failed to cancel watch event (%d)\n", ret);
+	kfree(rbd_dev->header_name);
+	rbd_dev->header_name = NULL;
+	rbd_dev->image_format = 0;
+	kfree(rbd_dev->spec->image_id);
+	rbd_dev->spec->image_id = NULL;
+
+	rbd_dev_destroy(rbd_dev);
+}
+
 /*
  * Probe for the existence of the header object for the given rbd
  * device.  For format 2 images this includes determining the image
  * id.
  */
-static int rbd_dev_probe(struct rbd_device *rbd_dev)
+static int rbd_dev_image_probe(struct rbd_device *rbd_dev)
 {
 	int ret;
+	int tmp;
 
 	/*
 	 * Get the id from the image id object.  If it's not a
@@ -4063,18 +4763,48 @@ static int rbd_dev_probe(struct rbd_device *rbd_dev)
 	 */
 	ret = rbd_dev_image_id(rbd_dev);
 	if (ret)
+		return ret;
+	rbd_assert(rbd_dev->spec->image_id);
+	rbd_assert(rbd_image_format_valid(rbd_dev->image_format));
+
+	ret = rbd_dev_header_name(rbd_dev);
+	if (ret)
+		goto err_out_format;
+
+	ret = rbd_dev_header_watch_sync(rbd_dev, 1);
+	if (ret)
+		goto out_header_name;
+
+	if (rbd_dev->image_format == 1)
 		ret = rbd_dev_v1_probe(rbd_dev);
 	else
 		ret = rbd_dev_v2_probe(rbd_dev);
-	if (ret) {
-		dout("probe failed, returning %d\n", ret);
-
-		return ret;
-	}
+	if (ret)
+		goto err_out_watch;
 
-	ret = rbd_dev_probe_finish(rbd_dev);
+	ret = rbd_dev_spec_update(rbd_dev);
 	if (ret)
-		rbd_header_free(&rbd_dev->header);
+		goto err_out_probe;
+
+	ret = rbd_dev_probe_parent(rbd_dev);
+	if (!ret)
+		return 0;
+
+err_out_probe:
+	rbd_dev_unprobe(rbd_dev);
+err_out_watch:
+	tmp = rbd_dev_header_watch_sync(rbd_dev, 0);
+	if (tmp)
+		rbd_warn(rbd_dev, "unable to tear down watch request\n");
+out_header_name:
+	kfree(rbd_dev->header_name);
+	rbd_dev->header_name = NULL;
+err_out_format:
+	rbd_dev->image_format = 0;
+	kfree(rbd_dev->spec->image_id);
+	rbd_dev->spec->image_id = NULL;
+
+	dout("probe failed, returning %d\n", ret);
 
 	return ret;
 }
@@ -4111,11 +4841,13 @@ static ssize_t rbd_add(struct bus_type *bus,
 	rc = ceph_pg_poolid_by_name(osdc->osdmap, spec->pool_name);
 	if (rc < 0)
 		goto err_out_client;
-	spec->pool_id = (u64) rc;
+	spec->pool_id = (u64)rc;
 
 	/* The ceph file layout needs to fit pool id in 32 bits */
 
-	if (WARN_ON(spec->pool_id > (u64) U32_MAX)) {
+	if (spec->pool_id > (u64)U32_MAX) {
+		rbd_warn(NULL, "pool id too large (%llu > %u)\n",
+				(unsigned long long)spec->pool_id, U32_MAX);
 		rc = -EIO;
 		goto err_out_client;
 	}
@@ -4130,11 +4862,15 @@ static ssize_t rbd_add(struct bus_type *bus,
 	kfree(rbd_opts);
 	rbd_opts = NULL;	/* done with this */
 
-	rc = rbd_dev_probe(rbd_dev);
+	rc = rbd_dev_image_probe(rbd_dev);
 	if (rc < 0)
 		goto err_out_rbd_dev;
 
-	return count;
+	rc = rbd_dev_device_setup(rbd_dev);
+	if (!rc)
+		return count;
+
+	rbd_dev_image_release(rbd_dev);
 err_out_rbd_dev:
 	rbd_dev_destroy(rbd_dev);
 err_out_client:
@@ -4149,7 +4885,7 @@ err_out_module:
 
 	dout("Error adding device %s\n", buf);
 
-	return (ssize_t) rc;
+	return (ssize_t)rc;
 }
 
 static struct rbd_device *__rbd_get_dev(unsigned long dev_id)
@@ -4169,27 +4905,43 @@ static struct rbd_device *__rbd_get_dev(unsigned long dev_id)
 	return NULL;
 }
 
-static void rbd_dev_release(struct device *dev)
+static void rbd_dev_device_release(struct device *dev)
 {
 	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
 
-	if (rbd_dev->watch_event)
-		rbd_dev_header_watch_sync(rbd_dev, 0);
-
-	/* clean up and free blkdev */
 	rbd_free_disk(rbd_dev);
+	clear_bit(RBD_DEV_FLAG_EXISTS, &rbd_dev->flags);
+	rbd_dev_clear_mapping(rbd_dev);
 	unregister_blkdev(rbd_dev->major, rbd_dev->name);
-
-	/* release allocated disk header fields */
-	rbd_header_free(&rbd_dev->header);
-
-	/* done with the id, and with the rbd_dev */
+	rbd_dev->major = 0;
 	rbd_dev_id_put(rbd_dev);
-	rbd_assert(rbd_dev->rbd_client != NULL);
-	rbd_dev_destroy(rbd_dev);
+	rbd_dev_mapping_clear(rbd_dev);
+}
 
-	/* release module ref */
-	module_put(THIS_MODULE);
+static void rbd_dev_remove_parent(struct rbd_device *rbd_dev)
+{
+	while (rbd_dev->parent) {
+		struct rbd_device *first = rbd_dev;
+		struct rbd_device *second = first->parent;
+		struct rbd_device *third;
+
+		/*
+		 * Follow to the parent with no grandparent and
+		 * remove it.
+		 */
+		while (second && (third = second->parent)) {
+			first = second;
+			second = third;
+		}
+		rbd_assert(second);
+		rbd_dev_image_release(second);
+		first->parent = NULL;
+		first->parent_overlap = 0;
+
+		rbd_assert(first->parent_spec);
+		rbd_spec_put(first->parent_spec);
+		first->parent_spec = NULL;
+	}
 }
 
 static ssize_t rbd_remove(struct bus_type *bus,
@@ -4197,13 +4949,13 @@ static ssize_t rbd_remove(struct bus_type *bus,
 			  size_t count)
 {
 	struct rbd_device *rbd_dev = NULL;
-	int target_id, rc;
+	int target_id;
 	unsigned long ul;
-	int ret = count;
+	int ret;
 
-	rc = strict_strtoul(buf, 10, &ul);
-	if (rc)
-		return rc;
+	ret = strict_strtoul(buf, 10, &ul);
+	if (ret)
+		return ret;
 
 	/* convert to int; abort if we lost anything in the conversion */
 	target_id = (int) ul;
@@ -4226,10 +4978,10 @@ static ssize_t rbd_remove(struct bus_type *bus,
 	spin_unlock_irq(&rbd_dev->lock);
 	if (ret < 0)
 		goto done;
-
-	rbd_remove_all_snaps(rbd_dev);
+	ret = count;
 	rbd_bus_del_dev(rbd_dev);
-
+	rbd_dev_image_release(rbd_dev);
+	module_put(THIS_MODULE);
 done:
 	mutex_unlock(&ctl_mutex);
 
@@ -4261,6 +5013,56 @@ static void rbd_sysfs_cleanup(void)
 	device_unregister(&rbd_root_dev);
 }
 
+static int rbd_slab_init(void)
+{
+	rbd_assert(!rbd_img_request_cache);
+	rbd_img_request_cache = kmem_cache_create("rbd_img_request",
+					sizeof (struct rbd_img_request),
+					__alignof__(struct rbd_img_request),
+					0, NULL);
+	if (!rbd_img_request_cache)
+		return -ENOMEM;
+
+	rbd_assert(!rbd_obj_request_cache);
+	rbd_obj_request_cache = kmem_cache_create("rbd_obj_request",
+					sizeof (struct rbd_obj_request),
+					__alignof__(struct rbd_obj_request),
+					0, NULL);
+	if (!rbd_obj_request_cache)
+		goto out_err;
+
+	rbd_assert(!rbd_segment_name_cache);
+	rbd_segment_name_cache = kmem_cache_create("rbd_segment_name",
+					MAX_OBJ_NAME_SIZE + 1, 1, 0, NULL);
+	if (rbd_segment_name_cache)
+		return 0;
+out_err:
+	if (rbd_obj_request_cache) {
+		kmem_cache_destroy(rbd_obj_request_cache);
+		rbd_obj_request_cache = NULL;
+	}
+
+	kmem_cache_destroy(rbd_img_request_cache);
+	rbd_img_request_cache = NULL;
+
+	return -ENOMEM;
+}
+
+static void rbd_slab_exit(void)
+{
+	rbd_assert(rbd_segment_name_cache);
+	kmem_cache_destroy(rbd_segment_name_cache);
+	rbd_segment_name_cache = NULL;
+
+	rbd_assert(rbd_obj_request_cache);
+	kmem_cache_destroy(rbd_obj_request_cache);
+	rbd_obj_request_cache = NULL;
+
+	rbd_assert(rbd_img_request_cache);
+	kmem_cache_destroy(rbd_img_request_cache);
+	rbd_img_request_cache = NULL;
+}
+
 static int __init rbd_init(void)
 {
 	int rc;
@@ -4270,16 +5072,22 @@ static int __init rbd_init(void)
 
 		return -EINVAL;
 	}
-	rc = rbd_sysfs_init();
+	rc = rbd_slab_init();
 	if (rc)
 		return rc;
-	pr_info("loaded " RBD_DRV_NAME_LONG "\n");
-	return 0;
+	rc = rbd_sysfs_init();
+	if (rc)
+		rbd_slab_exit();
+	else
+		pr_info("loaded " RBD_DRV_NAME_LONG "\n");
+
+	return rc;
 }
 
 static void __exit rbd_exit(void)
 {
 	rbd_sysfs_cleanup();
+	rbd_slab_exit();
 }
 
 module_init(rbd_init);
diff --git a/drivers/block/swim.c b/drivers/block/swim.c
index 8766a2257091..2f445b7a174e 100644
--- a/drivers/block/swim.c
+++ b/drivers/block/swim.c
@@ -673,7 +673,7 @@ static int floppy_unlocked_open(struct block_device *bdev, fmode_t mode)
 	return ret;
 }
 
-static int floppy_release(struct gendisk *disk, fmode_t mode)
+static void floppy_release(struct gendisk *disk, fmode_t mode)
 {
 	struct floppy_state *fs = disk->private_data;
 	struct swim __iomem *base = fs->swd->base;
@@ -687,8 +687,6 @@ static int floppy_release(struct gendisk *disk, fmode_t mode)
 	if (fs->ref_count == 0)
 		swim_motor(base, OFF);
 	mutex_unlock(&swim_mutex);
-
-	return 0;
 }
 
 static int floppy_ioctl(struct block_device *bdev, fmode_t mode,
diff --git a/drivers/block/swim3.c b/drivers/block/swim3.c
index 758f2ac878cf..20e061c3e023 100644
--- a/drivers/block/swim3.c
+++ b/drivers/block/swim3.c
@@ -251,7 +251,7 @@ static int fd_eject(struct floppy_state *fs);
 static int floppy_ioctl(struct block_device *bdev, fmode_t mode,
 			unsigned int cmd, unsigned long param);
 static int floppy_open(struct block_device *bdev, fmode_t mode);
-static int floppy_release(struct gendisk *disk, fmode_t mode);
+static void floppy_release(struct gendisk *disk, fmode_t mode);
 static unsigned int floppy_check_events(struct gendisk *disk,
 					unsigned int clearing);
 static int floppy_revalidate(struct gendisk *disk);
@@ -1017,7 +1017,7 @@ static int floppy_unlocked_open(struct block_device *bdev, fmode_t mode)
 	return ret;
 }
 
-static int floppy_release(struct gendisk *disk, fmode_t mode)
+static void floppy_release(struct gendisk *disk, fmode_t mode)
 {
 	struct floppy_state *fs = disk->private_data;
 	struct swim3 __iomem *sw = fs->swim3;
@@ -1029,7 +1029,6 @@ static int floppy_release(struct gendisk *disk, fmode_t mode)
 		swim3_select(fs, RELAX);
 	}
 	mutex_unlock(&swim3_mutex);
-	return 0;
 }
 
 static unsigned int floppy_check_events(struct gendisk *disk,
diff --git a/drivers/block/virtio_blk.c b/drivers/block/virtio_blk.c
index 8ad21a25bc0d..64723953e1c9 100644
--- a/drivers/block/virtio_blk.c
+++ b/drivers/block/virtio_blk.c
@@ -100,96 +100,103 @@ static inline struct virtblk_req *virtblk_alloc_req(struct virtio_blk *vblk,
 	return vbr;
 }
 
-static void virtblk_add_buf_wait(struct virtio_blk *vblk,
-				 struct virtblk_req *vbr,
-				 unsigned long out,
-				 unsigned long in)
+static int __virtblk_add_req(struct virtqueue *vq,
+			     struct virtblk_req *vbr,
+			     struct scatterlist *data_sg,
+			     bool have_data)
 {
-	DEFINE_WAIT(wait);
+	struct scatterlist hdr, status, cmd, sense, inhdr, *sgs[6];
+	unsigned int num_out = 0, num_in = 0;
+	int type = vbr->out_hdr.type & ~VIRTIO_BLK_T_OUT;
 
-	for (;;) {
-		prepare_to_wait_exclusive(&vblk->queue_wait, &wait,
-					  TASK_UNINTERRUPTIBLE);
+	sg_init_one(&hdr, &vbr->out_hdr, sizeof(vbr->out_hdr));
+	sgs[num_out++] = &hdr;
 
-		spin_lock_irq(vblk->disk->queue->queue_lock);
-		if (virtqueue_add_buf(vblk->vq, vbr->sg, out, in, vbr,
-				      GFP_ATOMIC) < 0) {
-			spin_unlock_irq(vblk->disk->queue->queue_lock);
-			io_schedule();
-		} else {
-			virtqueue_kick(vblk->vq);
-			spin_unlock_irq(vblk->disk->queue->queue_lock);
-			break;
-		}
+	/*
+	 * If this is a packet command we need a couple of additional headers.
+	 * Behind the normal outhdr we put a segment with the scsi command
+	 * block, and before the normal inhdr we put the sense data and the
+	 * inhdr with additional status information.
+	 */
+	if (type == VIRTIO_BLK_T_SCSI_CMD) {
+		sg_init_one(&cmd, vbr->req->cmd, vbr->req->cmd_len);
+		sgs[num_out++] = &cmd;
+	}
 
+	if (have_data) {
+		if (vbr->out_hdr.type & VIRTIO_BLK_T_OUT)
+			sgs[num_out++] = data_sg;
+		else
+			sgs[num_out + num_in++] = data_sg;
 	}
 
-	finish_wait(&vblk->queue_wait, &wait);
+	if (type == VIRTIO_BLK_T_SCSI_CMD) {
+		sg_init_one(&sense, vbr->req->sense, SCSI_SENSE_BUFFERSIZE);
+		sgs[num_out + num_in++] = &sense;
+		sg_init_one(&inhdr, &vbr->in_hdr, sizeof(vbr->in_hdr));
+		sgs[num_out + num_in++] = &inhdr;
+	}
+
+	sg_init_one(&status, &vbr->status, sizeof(vbr->status));
+	sgs[num_out + num_in++] = &status;
+
+	return virtqueue_add_sgs(vq, sgs, num_out, num_in, vbr, GFP_ATOMIC);
 }
 
-static inline void virtblk_add_req(struct virtblk_req *vbr,
-				   unsigned int out, unsigned int in)
+static void virtblk_add_req(struct virtblk_req *vbr, bool have_data)
 {
 	struct virtio_blk *vblk = vbr->vblk;
+	DEFINE_WAIT(wait);
+	int ret;
 
 	spin_lock_irq(vblk->disk->queue->queue_lock);
-	if (unlikely(virtqueue_add_buf(vblk->vq, vbr->sg, out, in, vbr,
-					GFP_ATOMIC) < 0)) {
+	while (unlikely((ret = __virtblk_add_req(vblk->vq, vbr, vbr->sg,
+						 have_data)) < 0)) {
+		prepare_to_wait_exclusive(&vblk->queue_wait, &wait,
+					  TASK_UNINTERRUPTIBLE);
+
 		spin_unlock_irq(vblk->disk->queue->queue_lock);
-		virtblk_add_buf_wait(vblk, vbr, out, in);
-		return;
+		io_schedule();
+		spin_lock_irq(vblk->disk->queue->queue_lock);
+
+		finish_wait(&vblk->queue_wait, &wait);
 	}
+
 	virtqueue_kick(vblk->vq);
 	spin_unlock_irq(vblk->disk->queue->queue_lock);
 }
 
-static int virtblk_bio_send_flush(struct virtblk_req *vbr)
+static void virtblk_bio_send_flush(struct virtblk_req *vbr)
 {
-	unsigned int out = 0, in = 0;
-
 	vbr->flags |= VBLK_IS_FLUSH;
 	vbr->out_hdr.type = VIRTIO_BLK_T_FLUSH;
 	vbr->out_hdr.sector = 0;
 	vbr->out_hdr.ioprio = 0;
-	sg_set_buf(&vbr->sg[out++], &vbr->out_hdr, sizeof(vbr->out_hdr));
-	sg_set_buf(&vbr->sg[out + in++], &vbr->status, sizeof(vbr->status));
-
-	virtblk_add_req(vbr, out, in);
 
-	return 0;
+	virtblk_add_req(vbr, false);
 }
 
-static int virtblk_bio_send_data(struct virtblk_req *vbr)
+static void virtblk_bio_send_data(struct virtblk_req *vbr)
 {
 	struct virtio_blk *vblk = vbr->vblk;
-	unsigned int num, out = 0, in = 0;
 	struct bio *bio = vbr->bio;
+	bool have_data;
 
 	vbr->flags &= ~VBLK_IS_FLUSH;
 	vbr->out_hdr.type = 0;
 	vbr->out_hdr.sector = bio->bi_sector;
 	vbr->out_hdr.ioprio = bio_prio(bio);
 
-	sg_set_buf(&vbr->sg[out++], &vbr->out_hdr, sizeof(vbr->out_hdr));
-
-	num = blk_bio_map_sg(vblk->disk->queue, bio, vbr->sg + out);
-
-	sg_set_buf(&vbr->sg[num + out + in++], &vbr->status,
-		   sizeof(vbr->status));
-
-	if (num) {
-		if (bio->bi_rw & REQ_WRITE) {
+	if (blk_bio_map_sg(vblk->disk->queue, bio, vbr->sg)) {
+		have_data = true;
+		if (bio->bi_rw & REQ_WRITE)
 			vbr->out_hdr.type |= VIRTIO_BLK_T_OUT;
-			out += num;
-		} else {
+		else
 			vbr->out_hdr.type |= VIRTIO_BLK_T_IN;
-			in += num;
-		}
-	}
+	} else
+		have_data = false;
 
-	virtblk_add_req(vbr, out, in);
-
-	return 0;
+	virtblk_add_req(vbr, have_data);
 }
 
 static void virtblk_bio_send_data_work(struct work_struct *work)
@@ -298,7 +305,7 @@ static void virtblk_done(struct virtqueue *vq)
 static bool do_req(struct request_queue *q, struct virtio_blk *vblk,
 		   struct request *req)
 {
-	unsigned long num, out = 0, in = 0;
+	unsigned int num;
 	struct virtblk_req *vbr;
 
 	vbr = virtblk_alloc_req(vblk, GFP_ATOMIC);
@@ -335,40 +342,15 @@ static bool do_req(struct request_queue *q, struct virtio_blk *vblk,
 		}
 	}
 
-	sg_set_buf(&vblk->sg[out++], &vbr->out_hdr, sizeof(vbr->out_hdr));
-
-	/*
-	 * If this is a packet command we need a couple of additional headers.
-	 * Behind the normal outhdr we put a segment with the scsi command
-	 * block, and before the normal inhdr we put the sense data and the
-	 * inhdr with additional status information before the normal inhdr.
-	 */
-	if (vbr->req->cmd_type == REQ_TYPE_BLOCK_PC)
-		sg_set_buf(&vblk->sg[out++], vbr->req->cmd, vbr->req->cmd_len);
-
-	num = blk_rq_map_sg(q, vbr->req, vblk->sg + out);
-
-	if (vbr->req->cmd_type == REQ_TYPE_BLOCK_PC) {
-		sg_set_buf(&vblk->sg[num + out + in++], vbr->req->sense, SCSI_SENSE_BUFFERSIZE);
-		sg_set_buf(&vblk->sg[num + out + in++], &vbr->in_hdr,
-			   sizeof(vbr->in_hdr));
-	}
-
-	sg_set_buf(&vblk->sg[num + out + in++], &vbr->status,
-		   sizeof(vbr->status));
-
+	num = blk_rq_map_sg(q, vbr->req, vblk->sg);
 	if (num) {
-		if (rq_data_dir(vbr->req) == WRITE) {
+		if (rq_data_dir(vbr->req) == WRITE)
 			vbr->out_hdr.type |= VIRTIO_BLK_T_OUT;
-			out += num;
-		} else {
+		else
 			vbr->out_hdr.type |= VIRTIO_BLK_T_IN;
-			in += num;
-		}
 	}
 
-	if (virtqueue_add_buf(vblk->vq, vblk->sg, out, in, vbr,
-			      GFP_ATOMIC) < 0) {
+	if (__virtblk_add_req(vblk->vq, vbr, vblk->sg, num) < 0) {
 		mempool_free(vbr, vblk->pool);
 		return false;
 	}
@@ -539,6 +521,7 @@ static void virtblk_config_changed_work(struct work_struct *work)
 	struct virtio_device *vdev = vblk->vdev;
 	struct request_queue *q = vblk->disk->queue;
 	char cap_str_2[10], cap_str_10[10];
+	char *envp[] = { "RESIZE=1", NULL };
 	u64 capacity, size;
 
 	mutex_lock(&vblk->config_lock);
@@ -568,6 +551,7 @@ static void virtblk_config_changed_work(struct work_struct *work)
 
 	set_capacity(vblk->disk, capacity);
 	revalidate_disk(vblk->disk);
+	kobject_uevent_env(&disk_to_dev(vblk->disk)->kobj, KOBJ_CHANGE, envp);
 done:
 	mutex_unlock(&vblk->config_lock);
 }
diff --git a/drivers/block/xen-blkfront.c b/drivers/block/xen-blkfront.c
index a894f88762d8..d89ef86220f4 100644
--- a/drivers/block/xen-blkfront.c
+++ b/drivers/block/xen-blkfront.c
@@ -1617,7 +1617,7 @@ out:
 	return err;
 }
 
-static int blkif_release(struct gendisk *disk, fmode_t mode)
+static void blkif_release(struct gendisk *disk, fmode_t mode)
 {
 	struct blkfront_info *info = disk->private_data;
 	struct block_device *bdev;
@@ -1658,7 +1658,6 @@ static int blkif_release(struct gendisk *disk, fmode_t mode)
 out:
 	bdput(bdev);
 	mutex_unlock(&blkfront_mutex);
-	return 0;
 }
 
 static const struct block_device_operations xlvbd_block_fops =
diff --git a/drivers/block/xsysace.c b/drivers/block/xsysace.c
index 1f38643173ca..f8ef15f37c5e 100644
--- a/drivers/block/xsysace.c
+++ b/drivers/block/xsysace.c
@@ -915,7 +915,7 @@ static int ace_open(struct block_device *bdev, fmode_t mode)
 	return 0;
 }
 
-static int ace_release(struct gendisk *disk, fmode_t mode)
+static void ace_release(struct gendisk *disk, fmode_t mode)
 {
 	struct ace_device *ace = disk->private_data;
 	unsigned long flags;
@@ -932,7 +932,6 @@ static int ace_release(struct gendisk *disk, fmode_t mode)
 	}
 	spin_unlock_irqrestore(&ace->lock, flags);
 	mutex_unlock(&xsysace_mutex);
-	return 0;
 }
 
 static int ace_getgeo(struct block_device *bdev, struct hd_geometry *geo)
diff --git a/drivers/block/z2ram.c b/drivers/block/z2ram.c
index a22e3f895947..5a95baf4b104 100644
--- a/drivers/block/z2ram.c
+++ b/drivers/block/z2ram.c
@@ -309,20 +309,18 @@ err_out:
     return rc;
 }
 
-static int
+static void
 z2_release(struct gendisk *disk, fmode_t mode)
 {
     mutex_lock(&z2ram_mutex);
     if ( current_device == -1 ) {
     	mutex_unlock(&z2ram_mutex);
-    	return 0;
+    	return;
     }
     mutex_unlock(&z2ram_mutex);
     /*
      * FIXME: unmap memory
      */
-
-    return 0;
 }
 
 static const struct block_device_operations z2_fops =
diff --git a/drivers/bluetooth/ath3k.c b/drivers/bluetooth/ath3k.c
index 6aab00ef4379..11f467c00d0a 100644
--- a/drivers/bluetooth/ath3k.c
+++ b/drivers/bluetooth/ath3k.c
@@ -90,6 +90,7 @@ static struct usb_device_id ath3k_table[] = {
 	{ USB_DEVICE(0x13d3, 0x3393) },
 	{ USB_DEVICE(0x0489, 0xe04e) },
 	{ USB_DEVICE(0x0489, 0xe056) },
+	{ USB_DEVICE(0x0489, 0xe04d) },
 
 	/* Atheros AR5BBU12 with sflash firmware */
 	{ USB_DEVICE(0x0489, 0xE02C) },
@@ -126,6 +127,7 @@ static struct usb_device_id ath3k_blist_tbl[] = {
 	{ USB_DEVICE(0x13d3, 0x3393), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0489, 0xe04e), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0489, 0xe056), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xe04d), .driver_info = BTUSB_ATH3012 },
 
 	/* Atheros AR5BBU22 with sflash firmware */
 	{ USB_DEVICE(0x0489, 0xE03C), .driver_info = BTUSB_ATH3012 },
diff --git a/drivers/bluetooth/btmrvl_debugfs.c b/drivers/bluetooth/btmrvl_debugfs.c
index 428dbb7574bd..db2c3c305df8 100644
--- a/drivers/bluetooth/btmrvl_debugfs.c
+++ b/drivers/bluetooth/btmrvl_debugfs.c
@@ -29,20 +29,6 @@
 struct btmrvl_debugfs_data {
 	struct dentry *config_dir;
 	struct dentry *status_dir;
-
-	/* config */
-	struct dentry *psmode;
-	struct dentry *pscmd;
-	struct dentry *hsmode;
-	struct dentry *hscmd;
-	struct dentry *gpiogap;
-	struct dentry *hscfgcmd;
-
-	/* status */
-	struct dentry *curpsmode;
-	struct dentry *hsstate;
-	struct dentry *psstate;
-	struct dentry *txdnldready;
 };
 
 static ssize_t btmrvl_hscfgcmd_write(struct file *file,
@@ -91,47 +77,6 @@ static const struct file_operations btmrvl_hscfgcmd_fops = {
 	.llseek = default_llseek,
 };
 
-static ssize_t btmrvl_psmode_write(struct file *file, const char __user *ubuf,
-						size_t count, loff_t *ppos)
-{
-	struct btmrvl_private *priv = file->private_data;
-	char buf[16];
-	long result, ret;
-
-	memset(buf, 0, sizeof(buf));
-
-	if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
-		return -EFAULT;
-
-	ret = strict_strtol(buf, 10, &result);
-	if (ret)
-		return ret;
-
-	priv->btmrvl_dev.psmode = result;
-
-	return count;
-}
-
-static ssize_t btmrvl_psmode_read(struct file *file, char __user *userbuf,
-						size_t count, loff_t *ppos)
-{
-	struct btmrvl_private *priv = file->private_data;
-	char buf[16];
-	int ret;
-
-	ret = snprintf(buf, sizeof(buf) - 1, "%d\n",
-						priv->btmrvl_dev.psmode);
-
-	return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
-}
-
-static const struct file_operations btmrvl_psmode_fops = {
-	.read	= btmrvl_psmode_read,
-	.write	= btmrvl_psmode_write,
-	.open	= simple_open,
-	.llseek = default_llseek,
-};
-
 static ssize_t btmrvl_pscmd_write(struct file *file, const char __user *ubuf,
 						size_t count, loff_t *ppos)
 {
@@ -178,47 +123,6 @@ static const struct file_operations btmrvl_pscmd_fops = {
 	.llseek = default_llseek,
 };
 
-static ssize_t btmrvl_gpiogap_write(struct file *file, const char __user *ubuf,
-						size_t count, loff_t *ppos)
-{
-	struct btmrvl_private *priv = file->private_data;
-	char buf[16];
-	long result, ret;
-
-	memset(buf, 0, sizeof(buf));
-
-	if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
-		return -EFAULT;
-
-	ret = strict_strtol(buf, 16, &result);
-	if (ret)
-		return ret;
-
-	priv->btmrvl_dev.gpio_gap = result;
-
-	return count;
-}
-
-static ssize_t btmrvl_gpiogap_read(struct file *file, char __user *userbuf,
-						size_t count, loff_t *ppos)
-{
-	struct btmrvl_private *priv = file->private_data;
-	char buf[16];
-	int ret;
-
-	ret = snprintf(buf, sizeof(buf) - 1, "0x%x\n",
-						priv->btmrvl_dev.gpio_gap);
-
-	return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
-}
-
-static const struct file_operations btmrvl_gpiogap_fops = {
-	.read	= btmrvl_gpiogap_read,
-	.write	= btmrvl_gpiogap_write,
-	.open	= simple_open,
-	.llseek = default_llseek,
-};
-
 static ssize_t btmrvl_hscmd_write(struct file *file, const char __user *ubuf,
 						size_t count, loff_t *ppos)
 {
@@ -263,119 +167,6 @@ static const struct file_operations btmrvl_hscmd_fops = {
 	.llseek = default_llseek,
 };
 
-static ssize_t btmrvl_hsmode_write(struct file *file, const char __user *ubuf,
-						size_t count, loff_t *ppos)
-{
-	struct btmrvl_private *priv = file->private_data;
-	char buf[16];
-	long result, ret;
-
-	memset(buf, 0, sizeof(buf));
-
-	if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
-		return -EFAULT;
-
-	ret = strict_strtol(buf, 10, &result);
-	if (ret)
-		return ret;
-
-	priv->btmrvl_dev.hsmode = result;
-
-	return count;
-}
-
-static ssize_t btmrvl_hsmode_read(struct file *file, char __user * userbuf,
-						size_t count, loff_t *ppos)
-{
-	struct btmrvl_private *priv = file->private_data;
-	char buf[16];
-	int ret;
-
-	ret = snprintf(buf, sizeof(buf) - 1, "%d\n", priv->btmrvl_dev.hsmode);
-
-	return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
-}
-
-static const struct file_operations btmrvl_hsmode_fops = {
-	.read	= btmrvl_hsmode_read,
-	.write	= btmrvl_hsmode_write,
-	.open	= simple_open,
-	.llseek = default_llseek,
-};
-
-static ssize_t btmrvl_curpsmode_read(struct file *file, char __user *userbuf,
-						size_t count, loff_t *ppos)
-{
-	struct btmrvl_private *priv = file->private_data;
-	char buf[16];
-	int ret;
-
-	ret = snprintf(buf, sizeof(buf) - 1, "%d\n", priv->adapter->psmode);
-
-	return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
-}
-
-static const struct file_operations btmrvl_curpsmode_fops = {
-	.read	= btmrvl_curpsmode_read,
-	.open	= simple_open,
-	.llseek = default_llseek,
-};
-
-static ssize_t btmrvl_psstate_read(struct file *file, char __user * userbuf,
-						size_t count, loff_t *ppos)
-{
-	struct btmrvl_private *priv = file->private_data;
-	char buf[16];
-	int ret;
-
-	ret = snprintf(buf, sizeof(buf) - 1, "%d\n", priv->adapter->ps_state);
-
-	return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
-}
-
-static const struct file_operations btmrvl_psstate_fops = {
-	.read	= btmrvl_psstate_read,
-	.open	= simple_open,
-	.llseek = default_llseek,
-};
-
-static ssize_t btmrvl_hsstate_read(struct file *file, char __user *userbuf,
-						size_t count, loff_t *ppos)
-{
-	struct btmrvl_private *priv = file->private_data;
-	char buf[16];
-	int ret;
-
-	ret = snprintf(buf, sizeof(buf) - 1, "%d\n", priv->adapter->hs_state);
-
-	return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
-}
-
-static const struct file_operations btmrvl_hsstate_fops = {
-	.read	= btmrvl_hsstate_read,
-	.open	= simple_open,
-	.llseek = default_llseek,
-};
-
-static ssize_t btmrvl_txdnldready_read(struct file *file, char __user *userbuf,
-						size_t count, loff_t *ppos)
-{
-	struct btmrvl_private *priv = file->private_data;
-	char buf[16];
-	int ret;
-
-	ret = snprintf(buf, sizeof(buf) - 1, "%d\n",
-					priv->btmrvl_dev.tx_dnld_rdy);
-
-	return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
-}
-
-static const struct file_operations btmrvl_txdnldready_fops = {
-	.read	= btmrvl_txdnldready_read,
-	.open	= simple_open,
-	.llseek = default_llseek,
-};
-
 void btmrvl_debugfs_init(struct hci_dev *hdev)
 {
 	struct btmrvl_private *priv = hci_get_drvdata(hdev);
@@ -394,30 +185,28 @@ void btmrvl_debugfs_init(struct hci_dev *hdev)
 
 	dbg->config_dir = debugfs_create_dir("config", hdev->debugfs);
 
-	dbg->psmode = debugfs_create_file("psmode", 0644, dbg->config_dir,
-					  priv, &btmrvl_psmode_fops);
-	dbg->pscmd = debugfs_create_file("pscmd", 0644, dbg->config_dir,
-					 priv, &btmrvl_pscmd_fops);
-	dbg->gpiogap = debugfs_create_file("gpiogap", 0644, dbg->config_dir,
-					   priv, &btmrvl_gpiogap_fops);
-	dbg->hsmode =  debugfs_create_file("hsmode", 0644, dbg->config_dir,
-					   priv, &btmrvl_hsmode_fops);
-	dbg->hscmd = debugfs_create_file("hscmd", 0644, dbg->config_dir,
-					 priv, &btmrvl_hscmd_fops);
-	dbg->hscfgcmd = debugfs_create_file("hscfgcmd", 0644, dbg->config_dir,
-					    priv, &btmrvl_hscfgcmd_fops);
+	debugfs_create_u8("psmode", 0644, dbg->config_dir,
+			  &priv->btmrvl_dev.psmode);
+	debugfs_create_file("pscmd", 0644, dbg->config_dir,
+			    priv, &btmrvl_pscmd_fops);
+	debugfs_create_x16("gpiogap", 0644, dbg->config_dir,
+			   &priv->btmrvl_dev.gpio_gap);
+	debugfs_create_u8("hsmode", 0644, dbg->config_dir,
+			  &priv->btmrvl_dev.hsmode);
+	debugfs_create_file("hscmd", 0644, dbg->config_dir,
+			    priv, &btmrvl_hscmd_fops);
+	debugfs_create_file("hscfgcmd", 0644, dbg->config_dir,
+			    priv, &btmrvl_hscfgcmd_fops);
 
 	dbg->status_dir = debugfs_create_dir("status", hdev->debugfs);
-	dbg->curpsmode = debugfs_create_file("curpsmode", 0444,
-					     dbg->status_dir, priv,
-					     &btmrvl_curpsmode_fops);
-	dbg->psstate = debugfs_create_file("psstate", 0444, dbg->status_dir,
-					   priv, &btmrvl_psstate_fops);
-	dbg->hsstate = debugfs_create_file("hsstate", 0444, dbg->status_dir,
-					   priv, &btmrvl_hsstate_fops);
-	dbg->txdnldready = debugfs_create_file("txdnldready", 0444,
-					       dbg->status_dir, priv,
-					       &btmrvl_txdnldready_fops);
+	debugfs_create_u8("curpsmode", 0444, dbg->status_dir,
+			  &priv->adapter->psmode);
+	debugfs_create_u8("psstate", 0444, dbg->status_dir,
+			  &priv->adapter->ps_state);
+	debugfs_create_u8("hsstate", 0444, dbg->status_dir,
+			  &priv->adapter->hs_state);
+	debugfs_create_u8("txdnldready", 0444, dbg->status_dir,
+			  &priv->btmrvl_dev.tx_dnld_rdy);
 }
 
 void btmrvl_debugfs_remove(struct hci_dev *hdev)
@@ -428,19 +217,8 @@ void btmrvl_debugfs_remove(struct hci_dev *hdev)
 	if (!dbg)
 		return;
 
-	debugfs_remove(dbg->psmode);
-	debugfs_remove(dbg->pscmd);
-	debugfs_remove(dbg->gpiogap);
-	debugfs_remove(dbg->hsmode);
-	debugfs_remove(dbg->hscmd);
-	debugfs_remove(dbg->hscfgcmd);
-	debugfs_remove(dbg->config_dir);
-
-	debugfs_remove(dbg->curpsmode);
-	debugfs_remove(dbg->psstate);
-	debugfs_remove(dbg->hsstate);
-	debugfs_remove(dbg->txdnldready);
-	debugfs_remove(dbg->status_dir);
+	debugfs_remove_recursive(dbg->config_dir);
+	debugfs_remove_recursive(dbg->status_dir);
 
 	kfree(dbg);
 }
diff --git a/drivers/bluetooth/btmrvl_sdio.c b/drivers/bluetooth/btmrvl_sdio.c
index 9959d4cb23dc..c63488c54f4a 100644
--- a/drivers/bluetooth/btmrvl_sdio.c
+++ b/drivers/bluetooth/btmrvl_sdio.c
@@ -83,8 +83,8 @@ static const struct btmrvl_sdio_card_reg btmrvl_reg_87xx = {
 };
 
 static const struct btmrvl_sdio_device btmrvl_sdio_sd8688 = {
-	.helper		= "sd8688_helper.bin",
-	.firmware	= "sd8688.bin",
+	.helper		= "mrvl/sd8688_helper.bin",
+	.firmware	= "mrvl/sd8688.bin",
 	.reg		= &btmrvl_reg_8688,
 	.sd_blksz_fw_dl	= 64,
 };
@@ -228,24 +228,24 @@ failed:
 static int btmrvl_sdio_verify_fw_download(struct btmrvl_sdio_card *card,
 								int pollnum)
 {
-	int ret = -ETIMEDOUT;
 	u16 firmwarestat;
-	unsigned int tries;
+	int tries, ret;
 
 	 /* Wait for firmware to become ready */
 	for (tries = 0; tries < pollnum; tries++) {
-		if (btmrvl_sdio_read_fw_status(card, &firmwarestat) < 0)
+		sdio_claim_host(card->func);
+		ret = btmrvl_sdio_read_fw_status(card, &firmwarestat);
+		sdio_release_host(card->func);
+		if (ret < 0)
 			continue;
 
-		if (firmwarestat == FIRMWARE_READY) {
-			ret = 0;
-			break;
-		} else {
-			msleep(10);
-		}
+		if (firmwarestat == FIRMWARE_READY)
+			return 0;
+
+		msleep(10);
 	}
 
-	return ret;
+	return -ETIMEDOUT;
 }
 
 static int btmrvl_sdio_download_helper(struct btmrvl_sdio_card *card)
@@ -874,7 +874,7 @@ exit:
 
 static int btmrvl_sdio_download_fw(struct btmrvl_sdio_card *card)
 {
-	int ret = 0;
+	int ret;
 	u8 fws0;
 	int pollnum = MAX_POLL_TRIES;
 
@@ -882,13 +882,14 @@ static int btmrvl_sdio_download_fw(struct btmrvl_sdio_card *card)
 		BT_ERR("card or function is NULL!");
 		return -EINVAL;
 	}
-	sdio_claim_host(card->func);
 
 	if (!btmrvl_sdio_verify_fw_download(card, 1)) {
 		BT_DBG("Firmware already downloaded!");
-		goto done;
+		return 0;
 	}
 
+	sdio_claim_host(card->func);
+
 	/* Check if other function driver is downloading the firmware */
 	fws0 = sdio_readb(card->func, card->reg->card_fw_status0, &ret);
 	if (ret) {
@@ -918,15 +919,21 @@ static int btmrvl_sdio_download_fw(struct btmrvl_sdio_card *card)
 		}
 	}
 
+	sdio_release_host(card->func);
+
+	/*
+	 * winner or not, with this test the FW synchronizes when the
+	 * module can continue its initialization
+	 */
 	if (btmrvl_sdio_verify_fw_download(card, pollnum)) {
 		BT_ERR("FW failed to be active in time!");
-		ret = -ETIMEDOUT;
-		goto done;
+		return -ETIMEDOUT;
 	}
 
+	return 0;
+
 done:
 	sdio_release_host(card->func);
-
 	return ret;
 }
 
@@ -989,8 +996,6 @@ static int btmrvl_sdio_probe(struct sdio_func *func,
 		goto unreg_dev;
 	}
 
-	msleep(100);
-
 	btmrvl_sdio_enable_host_int(card);
 
 	priv = btmrvl_add_card(card);
@@ -1185,7 +1190,7 @@ MODULE_AUTHOR("Marvell International Ltd.");
 MODULE_DESCRIPTION("Marvell BT-over-SDIO driver ver " VERSION);
 MODULE_VERSION(VERSION);
 MODULE_LICENSE("GPL v2");
-MODULE_FIRMWARE("sd8688_helper.bin");
-MODULE_FIRMWARE("sd8688.bin");
+MODULE_FIRMWARE("mrvl/sd8688_helper.bin");
+MODULE_FIRMWARE("mrvl/sd8688.bin");
 MODULE_FIRMWARE("mrvl/sd8787_uapsta.bin");
 MODULE_FIRMWARE("mrvl/sd8797_uapsta.bin");
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index 2cc5f774a29c..7a7e5f8ecadc 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -23,6 +23,7 @@
 
 #include <linux/module.h>
 #include <linux/usb.h>
+#include <linux/firmware.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
@@ -47,6 +48,7 @@ static struct usb_driver btusb_driver;
 #define BTUSB_BROKEN_ISOC	0x20
 #define BTUSB_WRONG_SCO_MTU	0x40
 #define BTUSB_ATH3012		0x80
+#define BTUSB_INTEL		0x100
 
 static struct usb_device_id btusb_table[] = {
 	/* Generic Bluetooth USB device */
@@ -148,6 +150,7 @@ static struct usb_device_id blacklist_table[] = {
 	{ USB_DEVICE(0x13d3, 0x3393), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0489, 0xe04e), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0489, 0xe056), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xe04d), .driver_info = BTUSB_ATH3012 },
 
 	/* Atheros AR5BBU12 with sflash firmware */
 	{ USB_DEVICE(0x0489, 0xe02c), .driver_info = BTUSB_IGNORE },
@@ -206,6 +209,9 @@ static struct usb_device_id blacklist_table[] = {
 	/* Frontline ComProbe Bluetooth Sniffer */
 	{ USB_DEVICE(0x16d3, 0x0002), .driver_info = BTUSB_SNIFFER },
 
+	/* Intel Bluetooth device */
+	{ USB_DEVICE(0x8087, 0x07dc), .driver_info = BTUSB_INTEL },
+
 	{ }	/* Terminating entry */
 };
 
@@ -926,6 +932,391 @@ static void btusb_waker(struct work_struct *work)
 	usb_autopm_put_interface(data->intf);
 }
 
+static int btusb_setup_bcm92035(struct hci_dev *hdev)
+{
+	struct sk_buff *skb;
+	u8 val = 0x00;
+
+	BT_DBG("%s", hdev->name);
+
+	skb = __hci_cmd_sync(hdev, 0xfc3b, 1, &val, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb))
+		BT_ERR("BCM92035 command failed (%ld)", -PTR_ERR(skb));
+	else
+		kfree_skb(skb);
+
+	return 0;
+}
+
+struct intel_version {
+	u8 status;
+	u8 hw_platform;
+	u8 hw_variant;
+	u8 hw_revision;
+	u8 fw_variant;
+	u8 fw_revision;
+	u8 fw_build_num;
+	u8 fw_build_ww;
+	u8 fw_build_yy;
+	u8 fw_patch_num;
+} __packed;
+
+static const struct firmware *btusb_setup_intel_get_fw(struct hci_dev *hdev,
+						struct intel_version *ver)
+{
+	const struct firmware *fw;
+	char fwname[64];
+	int ret;
+
+	snprintf(fwname, sizeof(fwname),
+		 "intel/ibt-hw-%x.%x.%x-fw-%x.%x.%x.%x.%x.bseq",
+		 ver->hw_platform, ver->hw_variant, ver->hw_revision,
+		 ver->fw_variant,  ver->fw_revision, ver->fw_build_num,
+		 ver->fw_build_ww, ver->fw_build_yy);
+
+	ret = request_firmware(&fw, fwname, &hdev->dev);
+	if (ret < 0) {
+		if (ret == -EINVAL) {
+			BT_ERR("%s Intel firmware file request failed (%d)",
+			       hdev->name, ret);
+			return NULL;
+		}
+
+		BT_ERR("%s failed to open Intel firmware file: %s(%d)",
+		       hdev->name, fwname, ret);
+
+		/* If the correct firmware patch file is not found, use the
+		 * default firmware patch file instead
+		 */
+		snprintf(fwname, sizeof(fwname), "intel/ibt-hw-%x.%x.bseq",
+			 ver->hw_platform, ver->hw_variant);
+		if (request_firmware(&fw, fwname, &hdev->dev) < 0) {
+			BT_ERR("%s failed to open default Intel fw file: %s",
+			       hdev->name, fwname);
+			return NULL;
+		}
+	}
+
+	BT_INFO("%s: Intel Bluetooth firmware file: %s", hdev->name, fwname);
+
+	return fw;
+}
+
+static int btusb_setup_intel_patching(struct hci_dev *hdev,
+				      const struct firmware *fw,
+				      const u8 **fw_ptr, int *disable_patch)
+{
+	struct sk_buff *skb;
+	struct hci_command_hdr *cmd;
+	const u8 *cmd_param;
+	struct hci_event_hdr *evt = NULL;
+	const u8 *evt_param = NULL;
+	int remain = fw->size - (*fw_ptr - fw->data);
+
+	/* The first byte indicates the types of the patch command or event.
+	 * 0x01 means HCI command and 0x02 is HCI event. If the first bytes
+	 * in the current firmware buffer doesn't start with 0x01 or
+	 * the size of remain buffer is smaller than HCI command header,
+	 * the firmware file is corrupted and it should stop the patching
+	 * process.
+	 */
+	if (remain > HCI_COMMAND_HDR_SIZE && *fw_ptr[0] != 0x01) {
+		BT_ERR("%s Intel fw corrupted: invalid cmd read", hdev->name);
+		return -EINVAL;
+	}
+	(*fw_ptr)++;
+	remain--;
+
+	cmd = (struct hci_command_hdr *)(*fw_ptr);
+	*fw_ptr += sizeof(*cmd);
+	remain -= sizeof(*cmd);
+
+	/* Ensure that the remain firmware data is long enough than the length
+	 * of command parameter. If not, the firmware file is corrupted.
+	 */
+	if (remain < cmd->plen) {
+		BT_ERR("%s Intel fw corrupted: invalid cmd len", hdev->name);
+		return -EFAULT;
+	}
+
+	/* If there is a command that loads a patch in the firmware
+	 * file, then enable the patch upon success, otherwise just
+	 * disable the manufacturer mode, for example patch activation
+	 * is not required when the default firmware patch file is used
+	 * because there are no patch data to load.
+	 */
+	if (*disable_patch && le16_to_cpu(cmd->opcode) == 0xfc8e)
+		*disable_patch = 0;
+
+	cmd_param = *fw_ptr;
+	*fw_ptr += cmd->plen;
+	remain -= cmd->plen;
+
+	/* This reads the expected events when the above command is sent to the
+	 * device. Some vendor commands expects more than one events, for
+	 * example command status event followed by vendor specific event.
+	 * For this case, it only keeps the last expected event. so the command
+	 * can be sent with __hci_cmd_sync_ev() which returns the sk_buff of
+	 * last expected event.
+	 */
+	while (remain > HCI_EVENT_HDR_SIZE && *fw_ptr[0] == 0x02) {
+		(*fw_ptr)++;
+		remain--;
+
+		evt = (struct hci_event_hdr *)(*fw_ptr);
+		*fw_ptr += sizeof(*evt);
+		remain -= sizeof(*evt);
+
+		if (remain < evt->plen) {
+			BT_ERR("%s Intel fw corrupted: invalid evt len",
+			       hdev->name);
+			return -EFAULT;
+		}
+
+		evt_param = *fw_ptr;
+		*fw_ptr += evt->plen;
+		remain -= evt->plen;
+	}
+
+	/* Every HCI commands in the firmware file has its correspond event.
+	 * If event is not found or remain is smaller than zero, the firmware
+	 * file is corrupted.
+	 */
+	if (!evt || !evt_param || remain < 0) {
+		BT_ERR("%s Intel fw corrupted: invalid evt read", hdev->name);
+		return -EFAULT;
+	}
+
+	skb = __hci_cmd_sync_ev(hdev, le16_to_cpu(cmd->opcode), cmd->plen,
+				cmd_param, evt->evt, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		BT_ERR("%s sending Intel patch command (0x%4.4x) failed (%ld)",
+		       hdev->name, cmd->opcode, PTR_ERR(skb));
+		return -PTR_ERR(skb);
+	}
+
+	/* It ensures that the returned event matches the event data read from
+	 * the firmware file. At fist, it checks the length and then
+	 * the contents of the event.
+	 */
+	if (skb->len != evt->plen) {
+		BT_ERR("%s mismatch event length (opcode 0x%4.4x)", hdev->name,
+		       le16_to_cpu(cmd->opcode));
+		kfree_skb(skb);
+		return -EFAULT;
+	}
+
+	if (memcmp(skb->data, evt_param, evt->plen)) {
+		BT_ERR("%s mismatch event parameter (opcode 0x%4.4x)",
+		       hdev->name, le16_to_cpu(cmd->opcode));
+		kfree_skb(skb);
+		return -EFAULT;
+	}
+	kfree_skb(skb);
+
+	return 0;
+}
+
+static int btusb_setup_intel(struct hci_dev *hdev)
+{
+	struct sk_buff *skb;
+	const struct firmware *fw;
+	const u8 *fw_ptr;
+	int disable_patch;
+	struct intel_version *ver;
+
+	const u8 mfg_enable[] = { 0x01, 0x00 };
+	const u8 mfg_disable[] = { 0x00, 0x00 };
+	const u8 mfg_reset_deactivate[] = { 0x00, 0x01 };
+	const u8 mfg_reset_activate[] = { 0x00, 0x02 };
+
+	BT_DBG("%s", hdev->name);
+
+	/* The controller has a bug with the first HCI command sent to it
+	 * returning number of completed commands as zero. This would stall the
+	 * command processing in the Bluetooth core.
+	 *
+	 * As a workaround, send HCI Reset command first which will reset the
+	 * number of completed commands and allow normal command processing
+	 * from now on.
+	 */
+	skb = __hci_cmd_sync(hdev, HCI_OP_RESET, 0, NULL, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		BT_ERR("%s sending initial HCI reset command failed (%ld)",
+		       hdev->name, PTR_ERR(skb));
+		return -PTR_ERR(skb);
+	}
+	kfree_skb(skb);
+
+	/* Read Intel specific controller version first to allow selection of
+	 * which firmware file to load.
+	 *
+	 * The returned information are hardware variant and revision plus
+	 * firmware variant, revision and build number.
+	 */
+	skb = __hci_cmd_sync(hdev, 0xfc05, 0, NULL, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		BT_ERR("%s reading Intel fw version command failed (%ld)",
+		       hdev->name, PTR_ERR(skb));
+		return -PTR_ERR(skb);
+	}
+
+	if (skb->len != sizeof(*ver)) {
+		BT_ERR("%s Intel version event length mismatch", hdev->name);
+		kfree_skb(skb);
+		return -EIO;
+	}
+
+	ver = (struct intel_version *)skb->data;
+	if (ver->status) {
+		BT_ERR("%s Intel fw version event failed (%02x)", hdev->name,
+		       ver->status);
+		kfree_skb(skb);
+		return -bt_to_errno(ver->status);
+	}
+
+	BT_INFO("%s: read Intel version: %02x%02x%02x%02x%02x%02x%02x%02x%02x",
+		hdev->name, ver->hw_platform, ver->hw_variant,
+		ver->hw_revision, ver->fw_variant,  ver->fw_revision,
+		ver->fw_build_num, ver->fw_build_ww, ver->fw_build_yy,
+		ver->fw_patch_num);
+
+	/* fw_patch_num indicates the version of patch the device currently
+	 * have. If there is no patch data in the device, it is always 0x00.
+	 * So, if it is other than 0x00, no need to patch the deivce again.
+	 */
+	if (ver->fw_patch_num) {
+		BT_INFO("%s: Intel device is already patched. patch num: %02x",
+			hdev->name, ver->fw_patch_num);
+		kfree_skb(skb);
+		return 0;
+	}
+
+	/* Opens the firmware patch file based on the firmware version read
+	 * from the controller. If it fails to open the matching firmware
+	 * patch file, it tries to open the default firmware patch file.
+	 * If no patch file is found, allow the device to operate without
+	 * a patch.
+	 */
+	fw = btusb_setup_intel_get_fw(hdev, ver);
+	if (!fw) {
+		kfree_skb(skb);
+		return 0;
+	}
+	fw_ptr = fw->data;
+
+	/* This Intel specific command enables the manufacturer mode of the
+	 * controller.
+	 *
+	 * Only while this mode is enabled, the driver can download the
+	 * firmware patch data and configuration parameters.
+	 */
+	skb = __hci_cmd_sync(hdev, 0xfc11, 2, mfg_enable, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		BT_ERR("%s entering Intel manufacturer mode failed (%ld)",
+		       hdev->name, PTR_ERR(skb));
+		release_firmware(fw);
+		return -PTR_ERR(skb);
+	}
+
+	if (skb->data[0]) {
+		u8 evt_status = skb->data[0];
+		BT_ERR("%s enable Intel manufacturer mode event failed (%02x)",
+		       hdev->name, evt_status);
+		kfree_skb(skb);
+		release_firmware(fw);
+		return -bt_to_errno(evt_status);
+	}
+	kfree_skb(skb);
+
+	disable_patch = 1;
+
+	/* The firmware data file consists of list of Intel specific HCI
+	 * commands and its expected events. The first byte indicates the
+	 * type of the message, either HCI command or HCI event.
+	 *
+	 * It reads the command and its expected event from the firmware file,
+	 * and send to the controller. Once __hci_cmd_sync_ev() returns,
+	 * the returned event is compared with the event read from the firmware
+	 * file and it will continue until all the messages are downloaded to
+	 * the controller.
+	 *
+	 * Once the firmware patching is completed successfully,
+	 * the manufacturer mode is disabled with reset and activating the
+	 * downloaded patch.
+	 *
+	 * If the firmware patching fails, the manufacturer mode is
+	 * disabled with reset and deactivating the patch.
+	 *
+	 * If the default patch file is used, no reset is done when disabling
+	 * the manufacturer.
+	 */
+	while (fw->size > fw_ptr - fw->data) {
+		int ret;
+
+		ret = btusb_setup_intel_patching(hdev, fw, &fw_ptr,
+						 &disable_patch);
+		if (ret < 0)
+			goto exit_mfg_deactivate;
+	}
+
+	release_firmware(fw);
+
+	if (disable_patch)
+		goto exit_mfg_disable;
+
+	/* Patching completed successfully and disable the manufacturer mode
+	 * with reset and activate the downloaded firmware patches.
+	 */
+	skb = __hci_cmd_sync(hdev, 0xfc11, sizeof(mfg_reset_activate),
+			     mfg_reset_activate, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		BT_ERR("%s exiting Intel manufacturer mode failed (%ld)",
+		       hdev->name, PTR_ERR(skb));
+		return -PTR_ERR(skb);
+	}
+	kfree_skb(skb);
+
+	BT_INFO("%s: Intel Bluetooth firmware patch completed and activated",
+		hdev->name);
+
+	return 0;
+
+exit_mfg_disable:
+	/* Disable the manufacturer mode without reset */
+	skb = __hci_cmd_sync(hdev, 0xfc11, sizeof(mfg_disable), mfg_disable,
+			     HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		BT_ERR("%s exiting Intel manufacturer mode failed (%ld)",
+		       hdev->name, PTR_ERR(skb));
+		return -PTR_ERR(skb);
+	}
+	kfree_skb(skb);
+
+	BT_INFO("%s: Intel Bluetooth firmware patch completed", hdev->name);
+	return 0;
+
+exit_mfg_deactivate:
+	release_firmware(fw);
+
+	/* Patching failed. Disable the manufacturer mode with reset and
+	 * deactivate the downloaded firmware patches.
+	 */
+	skb = __hci_cmd_sync(hdev, 0xfc11, sizeof(mfg_reset_deactivate),
+			     mfg_reset_deactivate, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		BT_ERR("%s exiting Intel manufacturer mode failed (%ld)",
+		       hdev->name, PTR_ERR(skb));
+		return -PTR_ERR(skb);
+	}
+	kfree_skb(skb);
+
+	BT_INFO("%s: Intel Bluetooth firmware patch completed and deactivated",
+		hdev->name);
+
+	return 0;
+}
+
 static int btusb_probe(struct usb_interface *intf,
 				const struct usb_device_id *id)
 {
@@ -1022,11 +1413,17 @@ static int btusb_probe(struct usb_interface *intf,
 
 	SET_HCIDEV_DEV(hdev, &intf->dev);
 
-	hdev->open     = btusb_open;
-	hdev->close    = btusb_close;
-	hdev->flush    = btusb_flush;
-	hdev->send     = btusb_send_frame;
-	hdev->notify   = btusb_notify;
+	hdev->open   = btusb_open;
+	hdev->close  = btusb_close;
+	hdev->flush  = btusb_flush;
+	hdev->send   = btusb_send_frame;
+	hdev->notify = btusb_notify;
+
+	if (id->driver_info & BTUSB_BCM92035)
+		hdev->setup = btusb_setup_bcm92035;
+
+	if (id->driver_info & BTUSB_INTEL)
+		hdev->setup = btusb_setup_intel;
 
 	/* Interface numbers are hardcoded in the specification */
 	data->isoc = usb_ifnum_to_if(data->udev, 1);
@@ -1065,17 +1462,6 @@ static int btusb_probe(struct usb_interface *intf,
 		data->isoc = NULL;
 	}
 
-	if (id->driver_info & BTUSB_BCM92035) {
-		unsigned char cmd[] = { 0x3b, 0xfc, 0x01, 0x00 };
-		struct sk_buff *skb;
-
-		skb = bt_skb_alloc(sizeof(cmd), GFP_KERNEL);
-		if (skb) {
-			memcpy(skb_put(skb, sizeof(cmd)), cmd, sizeof(cmd));
-			skb_queue_tail(&hdev->driver_init, skb);
-		}
-	}
-
 	if (data->isoc) {
 		err = usb_driver_claim_interface(&btusb_driver,
 							data->isoc, data);
diff --git a/drivers/bluetooth/hci_h4.c b/drivers/bluetooth/hci_h4.c
index c60623f206d4..8ae9f1ea2bb5 100644
--- a/drivers/bluetooth/hci_h4.c
+++ b/drivers/bluetooth/hci_h4.c
@@ -153,6 +153,9 @@ static int h4_recv(struct hci_uart *hu, void *data, int count)
 {
 	int ret;
 
+	if (!test_bit(HCI_UART_REGISTERED, &hu->flags))
+		return -EUNATCH;
+
 	ret = hci_recv_stream_fragment(hu->hdev, data, count);
 	if (ret < 0) {
 		BT_ERR("Frame Reassembly Failed");
diff --git a/drivers/bluetooth/hci_ldisc.c b/drivers/bluetooth/hci_ldisc.c
index ed0fade46aed..bc68a440d432 100644
--- a/drivers/bluetooth/hci_ldisc.c
+++ b/drivers/bluetooth/hci_ldisc.c
@@ -260,12 +260,12 @@ static int hci_uart_send_frame(struct sk_buff *skb)
 
 /* ------ LDISC part ------ */
 /* hci_uart_tty_open
- * 
+ *
  *     Called when line discipline changed to HCI_UART.
  *
  * Arguments:
  *     tty    pointer to tty info structure
- * Return Value:    
+ * Return Value:
  *     0 if success, otherwise error code
  */
 static int hci_uart_tty_open(struct tty_struct *tty)
@@ -365,15 +365,15 @@ static void hci_uart_tty_wakeup(struct tty_struct *tty)
 }
 
 /* hci_uart_tty_receive()
- * 
+ *
  *     Called by tty low level driver when receive data is
  *     available.
- *     
+ *
  * Arguments:  tty          pointer to tty isntance data
  *             data         pointer to received data
  *             flags        pointer to flags for data
  *             count        count of received data in bytes
- *     
+ *
  * Return Value:    None
  */
 static void hci_uart_tty_receive(struct tty_struct *tty, const u8 *data, char *flags, int count)
@@ -388,7 +388,10 @@ static void hci_uart_tty_receive(struct tty_struct *tty, const u8 *data, char *f
 
 	spin_lock(&hu->rx_lock);
 	hu->proto->recv(hu, (void *) data, count);
-	hu->hdev->stat.byte_rx += count;
+
+	if (hu->hdev)
+		hu->hdev->stat.byte_rx += count;
+
 	spin_unlock(&hu->rx_lock);
 
 	tty_unthrottle(tty);
diff --git a/drivers/bus/Kconfig b/drivers/bus/Kconfig
index 0f51ed687dc8..b05ecab915c4 100644
--- a/drivers/bus/Kconfig
+++ b/drivers/bus/Kconfig
@@ -4,6 +4,13 @@
 
 menu "Bus devices"
 
+config MVEBU_MBUS
+	bool
+	depends on PLAT_ORION
+	help
+	  Driver needed for the MBus configuration on Marvell EBU SoCs
+	  (Kirkwood, Dove, Orion5x, MV78XX0 and Armada 370/XP).
+
 config OMAP_OCP2SCP
 	tristate "OMAP OCP2SCP DRIVER"
 	depends on ARCH_OMAP2PLUS
diff --git a/drivers/bus/Makefile b/drivers/bus/Makefile
index 45d997c85453..3c7b53c12091 100644
--- a/drivers/bus/Makefile
+++ b/drivers/bus/Makefile
@@ -2,6 +2,7 @@
 # Makefile for the bus drivers.
 #
 
+obj-$(CONFIG_MVEBU_MBUS) += mvebu-mbus.o
 obj-$(CONFIG_OMAP_OCP2SCP)	+= omap-ocp2scp.o
 
 # Interconnect bus driver for OMAP SoCs.
diff --git a/drivers/bus/mvebu-mbus.c b/drivers/bus/mvebu-mbus.c
new file mode 100644
index 000000000000..8740f46b4d0d
--- /dev/null
+++ b/drivers/bus/mvebu-mbus.c
@@ -0,0 +1,870 @@
+/*
+ * Address map functions for Marvell EBU SoCs (Kirkwood, Armada
+ * 370/XP, Dove, Orion5x and MV78xx0)
+ *
+ * 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.
+ *
+ * The Marvell EBU SoCs have a configurable physical address space:
+ * the physical address at which certain devices (PCIe, NOR, NAND,
+ * etc.) sit can be configured. The configuration takes place through
+ * two sets of registers:
+ *
+ * - One to configure the access of the CPU to the devices. Depending
+ *   on the families, there are between 8 and 20 configurable windows,
+ *   each can be use to create a physical memory window that maps to a
+ *   specific device. Devices are identified by a tuple (target,
+ *   attribute).
+ *
+ * - One to configure the access to the CPU to the SDRAM. There are
+ *   either 2 (for Dove) or 4 (for other families) windows to map the
+ *   SDRAM into the physical address space.
+ *
+ * This driver:
+ *
+ * - Reads out the SDRAM address decoding windows at initialization
+ *   time, and fills the mvebu_mbus_dram_info structure with these
+ *   informations. The exported function mv_mbus_dram_info() allow
+ *   device drivers to get those informations related to the SDRAM
+ *   address decoding windows. This is because devices also have their
+ *   own windows (configured through registers that are part of each
+ *   device register space), and therefore the drivers for Marvell
+ *   devices have to configure those device -> SDRAM windows to ensure
+ *   that DMA works properly.
+ *
+ * - Provides an API for platform code or device drivers to
+ *   dynamically add or remove address decoding windows for the CPU ->
+ *   device accesses. This API is mvebu_mbus_add_window(),
+ *   mvebu_mbus_add_window_remap_flags() and
+ *   mvebu_mbus_del_window(). Since the (target, attribute) values
+ *   differ from one SoC family to another, the API uses a 'const char
+ *   *' string to identify devices, and this driver is responsible for
+ *   knowing the mapping between the name of a device and its
+ *   corresponding (target, attribute) in the current SoC family.
+ *
+ * - Provides a debugfs interface in /sys/kernel/debug/mvebu-mbus/ to
+ *   see the list of CPU -> SDRAM windows and their configuration
+ *   (file 'sdram') and the list of CPU -> devices windows and their
+ *   configuration (file 'devices').
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/mbus.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/debugfs.h>
+
+/*
+ * DDR target is the same on all platforms.
+ */
+#define TARGET_DDR		0
+
+/*
+ * CPU Address Decode Windows registers
+ */
+#define WIN_CTRL_OFF		0x0000
+#define   WIN_CTRL_ENABLE       BIT(0)
+#define   WIN_CTRL_TGT_MASK     0xf0
+#define   WIN_CTRL_TGT_SHIFT    4
+#define   WIN_CTRL_ATTR_MASK    0xff00
+#define   WIN_CTRL_ATTR_SHIFT   8
+#define   WIN_CTRL_SIZE_MASK    0xffff0000
+#define   WIN_CTRL_SIZE_SHIFT   16
+#define WIN_BASE_OFF		0x0004
+#define   WIN_BASE_LOW          0xffff0000
+#define   WIN_BASE_HIGH         0xf
+#define WIN_REMAP_LO_OFF	0x0008
+#define   WIN_REMAP_LOW         0xffff0000
+#define WIN_REMAP_HI_OFF	0x000c
+
+#define ATTR_HW_COHERENCY	(0x1 << 4)
+
+#define DDR_BASE_CS_OFF(n)	(0x0000 + ((n) << 3))
+#define  DDR_BASE_CS_HIGH_MASK  0xf
+#define  DDR_BASE_CS_LOW_MASK   0xff000000
+#define DDR_SIZE_CS_OFF(n)	(0x0004 + ((n) << 3))
+#define  DDR_SIZE_ENABLED       BIT(0)
+#define  DDR_SIZE_CS_MASK       0x1c
+#define  DDR_SIZE_CS_SHIFT      2
+#define  DDR_SIZE_MASK          0xff000000
+
+#define DOVE_DDR_BASE_CS_OFF(n) ((n) << 4)
+
+struct mvebu_mbus_mapping {
+	const char *name;
+	u8 target;
+	u8 attr;
+	u8 attrmask;
+};
+
+/*
+ * Masks used for the 'attrmask' field of mvebu_mbus_mapping. They
+ * allow to get the real attribute value, discarding the special bits
+ * used to select a PCI MEM region or a PCI WA region. This allows the
+ * debugfs code to reverse-match the name of a device from its
+ * target/attr values.
+ *
+ * For all devices except PCI, all bits of 'attr' must be
+ * considered. For most SoCs, only bit 3 should be ignored (it allows
+ * to select between PCI MEM and PCI I/O). On Orion5x however, there
+ * is the special bit 5 to select a PCI WA region.
+ */
+#define MAPDEF_NOMASK       0xff
+#define MAPDEF_PCIMASK      0xf7
+#define MAPDEF_ORIONPCIMASK 0xd7
+
+/* Macro used to define one mvebu_mbus_mapping entry */
+#define MAPDEF(__n, __t, __a, __m) \
+	{ .name = __n, .target = __t, .attr = __a, .attrmask = __m }
+
+struct mvebu_mbus_state;
+
+struct mvebu_mbus_soc_data {
+	unsigned int num_wins;
+	unsigned int num_remappable_wins;
+	unsigned int (*win_cfg_offset)(const int win);
+	void (*setup_cpu_target)(struct mvebu_mbus_state *s);
+	int (*show_cpu_target)(struct mvebu_mbus_state *s,
+			       struct seq_file *seq, void *v);
+	const struct mvebu_mbus_mapping *map;
+};
+
+struct mvebu_mbus_state {
+	void __iomem *mbuswins_base;
+	void __iomem *sdramwins_base;
+	struct dentry *debugfs_root;
+	struct dentry *debugfs_sdram;
+	struct dentry *debugfs_devs;
+	const struct mvebu_mbus_soc_data *soc;
+	int hw_io_coherency;
+};
+
+static struct mvebu_mbus_state mbus_state;
+
+static struct mbus_dram_target_info mvebu_mbus_dram_info;
+const struct mbus_dram_target_info *mv_mbus_dram_info(void)
+{
+	return &mvebu_mbus_dram_info;
+}
+EXPORT_SYMBOL_GPL(mv_mbus_dram_info);
+
+/*
+ * Functions to manipulate the address decoding windows
+ */
+
+static void mvebu_mbus_read_window(struct mvebu_mbus_state *mbus,
+				   int win, int *enabled, u64 *base,
+				   u32 *size, u8 *target, u8 *attr,
+				   u64 *remap)
+{
+	void __iomem *addr = mbus->mbuswins_base +
+		mbus->soc->win_cfg_offset(win);
+	u32 basereg = readl(addr + WIN_BASE_OFF);
+	u32 ctrlreg = readl(addr + WIN_CTRL_OFF);
+
+	if (!(ctrlreg & WIN_CTRL_ENABLE)) {
+		*enabled = 0;
+		return;
+	}
+
+	*enabled = 1;
+	*base = ((u64)basereg & WIN_BASE_HIGH) << 32;
+	*base |= (basereg & WIN_BASE_LOW);
+	*size = (ctrlreg | ~WIN_CTRL_SIZE_MASK) + 1;
+
+	if (target)
+		*target = (ctrlreg & WIN_CTRL_TGT_MASK) >> WIN_CTRL_TGT_SHIFT;
+
+	if (attr)
+		*attr = (ctrlreg & WIN_CTRL_ATTR_MASK) >> WIN_CTRL_ATTR_SHIFT;
+
+	if (remap) {
+		if (win < mbus->soc->num_remappable_wins) {
+			u32 remap_low = readl(addr + WIN_REMAP_LO_OFF);
+			u32 remap_hi  = readl(addr + WIN_REMAP_HI_OFF);
+			*remap = ((u64)remap_hi << 32) | remap_low;
+		} else
+			*remap = 0;
+	}
+}
+
+static void mvebu_mbus_disable_window(struct mvebu_mbus_state *mbus,
+				      int win)
+{
+	void __iomem *addr;
+
+	addr = mbus->mbuswins_base + mbus->soc->win_cfg_offset(win);
+
+	writel(0, addr + WIN_BASE_OFF);
+	writel(0, addr + WIN_CTRL_OFF);
+	if (win < mbus->soc->num_remappable_wins) {
+		writel(0, addr + WIN_REMAP_LO_OFF);
+		writel(0, addr + WIN_REMAP_HI_OFF);
+	}
+}
+
+/* Checks whether the given window number is available */
+static int mvebu_mbus_window_is_free(struct mvebu_mbus_state *mbus,
+				     const int win)
+{
+	void __iomem *addr = mbus->mbuswins_base +
+		mbus->soc->win_cfg_offset(win);
+	u32 ctrl = readl(addr + WIN_CTRL_OFF);
+	return !(ctrl & WIN_CTRL_ENABLE);
+}
+
+/*
+ * Checks whether the given (base, base+size) area doesn't overlap an
+ * existing region
+ */
+static int mvebu_mbus_window_conflicts(struct mvebu_mbus_state *mbus,
+				       phys_addr_t base, size_t size,
+				       u8 target, u8 attr)
+{
+	u64 end = (u64)base + size;
+	int win;
+
+	for (win = 0; win < mbus->soc->num_wins; win++) {
+		u64 wbase, wend;
+		u32 wsize;
+		u8 wtarget, wattr;
+		int enabled;
+
+		mvebu_mbus_read_window(mbus, win,
+				       &enabled, &wbase, &wsize,
+				       &wtarget, &wattr, NULL);
+
+		if (!enabled)
+			continue;
+
+		wend = wbase + wsize;
+
+		/*
+		 * Check if the current window overlaps with the
+		 * proposed physical range
+		 */
+		if ((u64)base < wend && end > wbase)
+			return 0;
+
+		/*
+		 * Check if target/attribute conflicts
+		 */
+		if (target == wtarget && attr == wattr)
+			return 0;
+	}
+
+	return 1;
+}
+
+static int mvebu_mbus_find_window(struct mvebu_mbus_state *mbus,
+				  phys_addr_t base, size_t size)
+{
+	int win;
+
+	for (win = 0; win < mbus->soc->num_wins; win++) {
+		u64 wbase;
+		u32 wsize;
+		int enabled;
+
+		mvebu_mbus_read_window(mbus, win,
+				       &enabled, &wbase, &wsize,
+				       NULL, NULL, NULL);
+
+		if (!enabled)
+			continue;
+
+		if (base == wbase && size == wsize)
+			return win;
+	}
+
+	return -ENODEV;
+}
+
+static int mvebu_mbus_setup_window(struct mvebu_mbus_state *mbus,
+				   int win, phys_addr_t base, size_t size,
+				   phys_addr_t remap, u8 target,
+				   u8 attr)
+{
+	void __iomem *addr = mbus->mbuswins_base +
+		mbus->soc->win_cfg_offset(win);
+	u32 ctrl, remap_addr;
+
+	ctrl = ((size - 1) & WIN_CTRL_SIZE_MASK) |
+		(attr << WIN_CTRL_ATTR_SHIFT)    |
+		(target << WIN_CTRL_TGT_SHIFT)   |
+		WIN_CTRL_ENABLE;
+
+	writel(base & WIN_BASE_LOW, addr + WIN_BASE_OFF);
+	writel(ctrl, addr + WIN_CTRL_OFF);
+	if (win < mbus->soc->num_remappable_wins) {
+		if (remap == MVEBU_MBUS_NO_REMAP)
+			remap_addr = base;
+		else
+			remap_addr = remap;
+		writel(remap_addr & WIN_REMAP_LOW, addr + WIN_REMAP_LO_OFF);
+		writel(0, addr + WIN_REMAP_HI_OFF);
+	}
+
+	return 0;
+}
+
+static int mvebu_mbus_alloc_window(struct mvebu_mbus_state *mbus,
+				   phys_addr_t base, size_t size,
+				   phys_addr_t remap, u8 target,
+				   u8 attr)
+{
+	int win;
+
+	if (remap == MVEBU_MBUS_NO_REMAP) {
+		for (win = mbus->soc->num_remappable_wins;
+		     win < mbus->soc->num_wins; win++)
+			if (mvebu_mbus_window_is_free(mbus, win))
+				return mvebu_mbus_setup_window(mbus, win, base,
+							       size, remap,
+							       target, attr);
+	}
+
+
+	for (win = 0; win < mbus->soc->num_wins; win++)
+		if (mvebu_mbus_window_is_free(mbus, win))
+			return mvebu_mbus_setup_window(mbus, win, base, size,
+						       remap, target, attr);
+
+	return -ENOMEM;
+}
+
+/*
+ * Debugfs debugging
+ */
+
+/* Common function used for Dove, Kirkwood, Armada 370/XP and Orion 5x */
+static int mvebu_sdram_debug_show_orion(struct mvebu_mbus_state *mbus,
+					struct seq_file *seq, void *v)
+{
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		u32 basereg = readl(mbus->sdramwins_base + DDR_BASE_CS_OFF(i));
+		u32 sizereg = readl(mbus->sdramwins_base + DDR_SIZE_CS_OFF(i));
+		u64 base;
+		u32 size;
+
+		if (!(sizereg & DDR_SIZE_ENABLED)) {
+			seq_printf(seq, "[%d] disabled\n", i);
+			continue;
+		}
+
+		base = ((u64)basereg & DDR_BASE_CS_HIGH_MASK) << 32;
+		base |= basereg & DDR_BASE_CS_LOW_MASK;
+		size = (sizereg | ~DDR_SIZE_MASK);
+
+		seq_printf(seq, "[%d] %016llx - %016llx : cs%d\n",
+			   i, (unsigned long long)base,
+			   (unsigned long long)base + size + 1,
+			   (sizereg & DDR_SIZE_CS_MASK) >> DDR_SIZE_CS_SHIFT);
+	}
+
+	return 0;
+}
+
+/* Special function for Dove */
+static int mvebu_sdram_debug_show_dove(struct mvebu_mbus_state *mbus,
+				       struct seq_file *seq, void *v)
+{
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		u32 map = readl(mbus->sdramwins_base + DOVE_DDR_BASE_CS_OFF(i));
+		u64 base;
+		u32 size;
+
+		if (!(map & 1)) {
+			seq_printf(seq, "[%d] disabled\n", i);
+			continue;
+		}
+
+		base = map & 0xff800000;
+		size = 0x100000 << (((map & 0x000f0000) >> 16) - 4);
+
+		seq_printf(seq, "[%d] %016llx - %016llx : cs%d\n",
+			   i, (unsigned long long)base,
+			   (unsigned long long)base + size, i);
+	}
+
+	return 0;
+}
+
+static int mvebu_sdram_debug_show(struct seq_file *seq, void *v)
+{
+	struct mvebu_mbus_state *mbus = &mbus_state;
+	return mbus->soc->show_cpu_target(mbus, seq, v);
+}
+
+static int mvebu_sdram_debug_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, mvebu_sdram_debug_show, inode->i_private);
+}
+
+static const struct file_operations mvebu_sdram_debug_fops = {
+	.open = mvebu_sdram_debug_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static int mvebu_devs_debug_show(struct seq_file *seq, void *v)
+{
+	struct mvebu_mbus_state *mbus = &mbus_state;
+	int win;
+
+	for (win = 0; win < mbus->soc->num_wins; win++) {
+		u64 wbase, wremap;
+		u32 wsize;
+		u8 wtarget, wattr;
+		int enabled, i;
+		const char *name;
+
+		mvebu_mbus_read_window(mbus, win,
+				       &enabled, &wbase, &wsize,
+				       &wtarget, &wattr, &wremap);
+
+		if (!enabled) {
+			seq_printf(seq, "[%02d] disabled\n", win);
+			continue;
+		}
+
+
+		for (i = 0; mbus->soc->map[i].name; i++)
+			if (mbus->soc->map[i].target == wtarget &&
+			    mbus->soc->map[i].attr ==
+			    (wattr & mbus->soc->map[i].attrmask))
+				break;
+
+		name = mbus->soc->map[i].name ?: "unknown";
+
+		seq_printf(seq, "[%02d] %016llx - %016llx : %s",
+			   win, (unsigned long long)wbase,
+			   (unsigned long long)(wbase + wsize), name);
+
+		if (win < mbus->soc->num_remappable_wins) {
+			seq_printf(seq, " (remap %016llx)\n",
+				   (unsigned long long)wremap);
+		} else
+			seq_printf(seq, "\n");
+	}
+
+	return 0;
+}
+
+static int mvebu_devs_debug_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, mvebu_devs_debug_show, inode->i_private);
+}
+
+static const struct file_operations mvebu_devs_debug_fops = {
+	.open = mvebu_devs_debug_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+/*
+ * SoC-specific functions and definitions
+ */
+
+static unsigned int orion_mbus_win_offset(int win)
+{
+	return win << 4;
+}
+
+static unsigned int armada_370_xp_mbus_win_offset(int win)
+{
+	/* The register layout is a bit annoying and the below code
+	 * tries to cope with it.
+	 * - At offset 0x0, there are the registers for the first 8
+	 *   windows, with 4 registers of 32 bits per window (ctrl,
+	 *   base, remap low, remap high)
+	 * - Then at offset 0x80, there is a hole of 0x10 bytes for
+	 *   the internal registers base address and internal units
+	 *   sync barrier register.
+	 * - Then at offset 0x90, there the registers for 12
+	 *   windows, with only 2 registers of 32 bits per window
+	 *   (ctrl, base).
+	 */
+	if (win < 8)
+		return win << 4;
+	else
+		return 0x90 + ((win - 8) << 3);
+}
+
+static unsigned int mv78xx0_mbus_win_offset(int win)
+{
+	if (win < 8)
+		return win << 4;
+	else
+		return 0x900 + ((win - 8) << 4);
+}
+
+static void __init
+mvebu_mbus_default_setup_cpu_target(struct mvebu_mbus_state *mbus)
+{
+	int i;
+	int cs;
+
+	mvebu_mbus_dram_info.mbus_dram_target_id = TARGET_DDR;
+
+	for (i = 0, cs = 0; i < 4; i++) {
+		u32 base = readl(mbus->sdramwins_base + DDR_BASE_CS_OFF(i));
+		u32 size = readl(mbus->sdramwins_base + DDR_SIZE_CS_OFF(i));
+
+		/*
+		 * We only take care of entries for which the chip
+		 * select is enabled, and that don't have high base
+		 * address bits set (devices can only access the first
+		 * 32 bits of the memory).
+		 */
+		if ((size & DDR_SIZE_ENABLED) &&
+		    !(base & DDR_BASE_CS_HIGH_MASK)) {
+			struct mbus_dram_window *w;
+
+			w = &mvebu_mbus_dram_info.cs[cs++];
+			w->cs_index = i;
+			w->mbus_attr = 0xf & ~(1 << i);
+			if (mbus->hw_io_coherency)
+				w->mbus_attr |= ATTR_HW_COHERENCY;
+			w->base = base & DDR_BASE_CS_LOW_MASK;
+			w->size = (size | ~DDR_SIZE_MASK) + 1;
+		}
+	}
+	mvebu_mbus_dram_info.num_cs = cs;
+}
+
+static void __init
+mvebu_mbus_dove_setup_cpu_target(struct mvebu_mbus_state *mbus)
+{
+	int i;
+	int cs;
+
+	mvebu_mbus_dram_info.mbus_dram_target_id = TARGET_DDR;
+
+	for (i = 0, cs = 0; i < 2; i++) {
+		u32 map = readl(mbus->sdramwins_base + DOVE_DDR_BASE_CS_OFF(i));
+
+		/*
+		 * Chip select enabled?
+		 */
+		if (map & 1) {
+			struct mbus_dram_window *w;
+
+			w = &mvebu_mbus_dram_info.cs[cs++];
+			w->cs_index = i;
+			w->mbus_attr = 0; /* CS address decoding done inside */
+					  /* the DDR controller, no need to  */
+					  /* provide attributes */
+			w->base = map & 0xff800000;
+			w->size = 0x100000 << (((map & 0x000f0000) >> 16) - 4);
+		}
+	}
+
+	mvebu_mbus_dram_info.num_cs = cs;
+}
+
+static const struct mvebu_mbus_mapping armada_370_map[] = {
+	MAPDEF("bootrom",     1, 0xe0, MAPDEF_NOMASK),
+	MAPDEF("devbus-boot", 1, 0x2f, MAPDEF_NOMASK),
+	MAPDEF("devbus-cs0",  1, 0x3e, MAPDEF_NOMASK),
+	MAPDEF("devbus-cs1",  1, 0x3d, MAPDEF_NOMASK),
+	MAPDEF("devbus-cs2",  1, 0x3b, MAPDEF_NOMASK),
+	MAPDEF("devbus-cs3",  1, 0x37, MAPDEF_NOMASK),
+	MAPDEF("pcie0.0",     4, 0xe0, MAPDEF_PCIMASK),
+	MAPDEF("pcie1.0",     8, 0xe0, MAPDEF_PCIMASK),
+	{},
+};
+
+static const struct mvebu_mbus_soc_data armada_370_mbus_data = {
+	.num_wins            = 20,
+	.num_remappable_wins = 8,
+	.win_cfg_offset      = armada_370_xp_mbus_win_offset,
+	.setup_cpu_target    = mvebu_mbus_default_setup_cpu_target,
+	.show_cpu_target     = mvebu_sdram_debug_show_orion,
+	.map                 = armada_370_map,
+};
+
+static const struct mvebu_mbus_mapping armada_xp_map[] = {
+	MAPDEF("bootrom",     1, 0x1d, MAPDEF_NOMASK),
+	MAPDEF("devbus-boot", 1, 0x2f, MAPDEF_NOMASK),
+	MAPDEF("devbus-cs0",  1, 0x3e, MAPDEF_NOMASK),
+	MAPDEF("devbus-cs1",  1, 0x3d, MAPDEF_NOMASK),
+	MAPDEF("devbus-cs2",  1, 0x3b, MAPDEF_NOMASK),
+	MAPDEF("devbus-cs3",  1, 0x37, MAPDEF_NOMASK),
+	MAPDEF("pcie0.0",     4, 0xe0, MAPDEF_PCIMASK),
+	MAPDEF("pcie0.1",     4, 0xd0, MAPDEF_PCIMASK),
+	MAPDEF("pcie0.2",     4, 0xb0, MAPDEF_PCIMASK),
+	MAPDEF("pcie0.3",     4, 0x70, MAPDEF_PCIMASK),
+	MAPDEF("pcie1.0",     8, 0xe0, MAPDEF_PCIMASK),
+	MAPDEF("pcie1.1",     8, 0xd0, MAPDEF_PCIMASK),
+	MAPDEF("pcie1.2",     8, 0xb0, MAPDEF_PCIMASK),
+	MAPDEF("pcie1.3",     8, 0x70, MAPDEF_PCIMASK),
+	MAPDEF("pcie2.0",     4, 0xf0, MAPDEF_PCIMASK),
+	MAPDEF("pcie3.0",     8, 0xf0, MAPDEF_PCIMASK),
+	{},
+};
+
+static const struct mvebu_mbus_soc_data armada_xp_mbus_data = {
+	.num_wins            = 20,
+	.num_remappable_wins = 8,
+	.win_cfg_offset      = armada_370_xp_mbus_win_offset,
+	.setup_cpu_target    = mvebu_mbus_default_setup_cpu_target,
+	.show_cpu_target     = mvebu_sdram_debug_show_orion,
+	.map                 = armada_xp_map,
+};
+
+static const struct mvebu_mbus_mapping kirkwood_map[] = {
+	MAPDEF("pcie0.0", 4, 0xe0, MAPDEF_PCIMASK),
+	MAPDEF("pcie1.0", 4, 0xd0, MAPDEF_PCIMASK),
+	MAPDEF("sram",    3, 0x01, MAPDEF_NOMASK),
+	MAPDEF("nand",    1, 0x2f, MAPDEF_NOMASK),
+	{},
+};
+
+static const struct mvebu_mbus_soc_data kirkwood_mbus_data = {
+	.num_wins            = 8,
+	.num_remappable_wins = 4,
+	.win_cfg_offset      = orion_mbus_win_offset,
+	.setup_cpu_target    = mvebu_mbus_default_setup_cpu_target,
+	.show_cpu_target     = mvebu_sdram_debug_show_orion,
+	.map                 = kirkwood_map,
+};
+
+static const struct mvebu_mbus_mapping dove_map[] = {
+	MAPDEF("pcie0.0",    0x4, 0xe0, MAPDEF_PCIMASK),
+	MAPDEF("pcie1.0",    0x8, 0xe0, MAPDEF_PCIMASK),
+	MAPDEF("cesa",       0x3, 0x01, MAPDEF_NOMASK),
+	MAPDEF("bootrom",    0x1, 0xfd, MAPDEF_NOMASK),
+	MAPDEF("scratchpad", 0xd, 0x0, MAPDEF_NOMASK),
+	{},
+};
+
+static const struct mvebu_mbus_soc_data dove_mbus_data = {
+	.num_wins            = 8,
+	.num_remappable_wins = 4,
+	.win_cfg_offset      = orion_mbus_win_offset,
+	.setup_cpu_target    = mvebu_mbus_dove_setup_cpu_target,
+	.show_cpu_target     = mvebu_sdram_debug_show_dove,
+	.map                 = dove_map,
+};
+
+static const struct mvebu_mbus_mapping orion5x_map[] = {
+	MAPDEF("pcie0.0",     4, 0x51, MAPDEF_ORIONPCIMASK),
+	MAPDEF("pci0.0",      3, 0x51, MAPDEF_ORIONPCIMASK),
+	MAPDEF("devbus-boot", 1, 0x0f, MAPDEF_NOMASK),
+	MAPDEF("devbus-cs0",  1, 0x1e, MAPDEF_NOMASK),
+	MAPDEF("devbus-cs1",  1, 0x1d, MAPDEF_NOMASK),
+	MAPDEF("devbus-cs2",  1, 0x1b, MAPDEF_NOMASK),
+	MAPDEF("sram",        0, 0x00, MAPDEF_NOMASK),
+	{},
+};
+
+/*
+ * Some variants of Orion5x have 4 remappable windows, some other have
+ * only two of them.
+ */
+static const struct mvebu_mbus_soc_data orion5x_4win_mbus_data = {
+	.num_wins            = 8,
+	.num_remappable_wins = 4,
+	.win_cfg_offset      = orion_mbus_win_offset,
+	.setup_cpu_target    = mvebu_mbus_default_setup_cpu_target,
+	.show_cpu_target     = mvebu_sdram_debug_show_orion,
+	.map                 = orion5x_map,
+};
+
+static const struct mvebu_mbus_soc_data orion5x_2win_mbus_data = {
+	.num_wins            = 8,
+	.num_remappable_wins = 2,
+	.win_cfg_offset      = orion_mbus_win_offset,
+	.setup_cpu_target    = mvebu_mbus_default_setup_cpu_target,
+	.show_cpu_target     = mvebu_sdram_debug_show_orion,
+	.map                 = orion5x_map,
+};
+
+static const struct mvebu_mbus_mapping mv78xx0_map[] = {
+	MAPDEF("pcie0.0", 4, 0xe0, MAPDEF_PCIMASK),
+	MAPDEF("pcie0.1", 4, 0xd0, MAPDEF_PCIMASK),
+	MAPDEF("pcie0.2", 4, 0xb0, MAPDEF_PCIMASK),
+	MAPDEF("pcie0.3", 4, 0x70, MAPDEF_PCIMASK),
+	MAPDEF("pcie1.0", 8, 0xe0, MAPDEF_PCIMASK),
+	MAPDEF("pcie1.1", 8, 0xd0, MAPDEF_PCIMASK),
+	MAPDEF("pcie1.2", 8, 0xb0, MAPDEF_PCIMASK),
+	MAPDEF("pcie1.3", 8, 0x70, MAPDEF_PCIMASK),
+	MAPDEF("pcie2.0", 4, 0xf0, MAPDEF_PCIMASK),
+	MAPDEF("pcie3.0", 8, 0xf0, MAPDEF_PCIMASK),
+	{},
+};
+
+static const struct mvebu_mbus_soc_data mv78xx0_mbus_data = {
+	.num_wins            = 14,
+	.num_remappable_wins = 8,
+	.win_cfg_offset      = mv78xx0_mbus_win_offset,
+	.setup_cpu_target    = mvebu_mbus_default_setup_cpu_target,
+	.show_cpu_target     = mvebu_sdram_debug_show_orion,
+	.map                 = mv78xx0_map,
+};
+
+/*
+ * The driver doesn't yet have a DT binding because the details of
+ * this DT binding still need to be sorted out. However, as a
+ * preparation, we already use of_device_id to match a SoC description
+ * string against the SoC specific details of this driver.
+ */
+static const struct of_device_id of_mvebu_mbus_ids[] = {
+	{ .compatible = "marvell,armada370-mbus",
+	  .data = &armada_370_mbus_data, },
+	{ .compatible = "marvell,armadaxp-mbus",
+	  .data = &armada_xp_mbus_data, },
+	{ .compatible = "marvell,kirkwood-mbus",
+	  .data = &kirkwood_mbus_data, },
+	{ .compatible = "marvell,dove-mbus",
+	  .data = &dove_mbus_data, },
+	{ .compatible = "marvell,orion5x-88f5281-mbus",
+	  .data = &orion5x_4win_mbus_data, },
+	{ .compatible = "marvell,orion5x-88f5182-mbus",
+	  .data = &orion5x_2win_mbus_data, },
+	{ .compatible = "marvell,orion5x-88f5181-mbus",
+	  .data = &orion5x_2win_mbus_data, },
+	{ .compatible = "marvell,orion5x-88f6183-mbus",
+	  .data = &orion5x_4win_mbus_data, },
+	{ .compatible = "marvell,mv78xx0-mbus",
+	  .data = &mv78xx0_mbus_data, },
+	{ },
+};
+
+/*
+ * Public API of the driver
+ */
+int mvebu_mbus_add_window_remap_flags(const char *devname, phys_addr_t base,
+				      size_t size, phys_addr_t remap,
+				      unsigned int flags)
+{
+	struct mvebu_mbus_state *s = &mbus_state;
+	u8 target, attr;
+	int i;
+
+	if (!s->soc->map)
+		return -ENODEV;
+
+	for (i = 0; s->soc->map[i].name; i++)
+		if (!strcmp(s->soc->map[i].name, devname))
+			break;
+
+	if (!s->soc->map[i].name) {
+		pr_err("mvebu-mbus: unknown device '%s'\n", devname);
+		return -ENODEV;
+	}
+
+	target = s->soc->map[i].target;
+	attr   = s->soc->map[i].attr;
+
+	if (flags == MVEBU_MBUS_PCI_MEM)
+		attr |= 0x8;
+	else if (flags == MVEBU_MBUS_PCI_WA)
+		attr |= 0x28;
+
+	if (!mvebu_mbus_window_conflicts(s, base, size, target, attr)) {
+		pr_err("mvebu-mbus: cannot add window '%s', conflicts with another window\n",
+		       devname);
+		return -EINVAL;
+	}
+
+	return mvebu_mbus_alloc_window(s, base, size, remap, target, attr);
+
+}
+
+int mvebu_mbus_add_window(const char *devname, phys_addr_t base, size_t size)
+{
+	return mvebu_mbus_add_window_remap_flags(devname, base, size,
+						 MVEBU_MBUS_NO_REMAP, 0);
+}
+
+int mvebu_mbus_del_window(phys_addr_t base, size_t size)
+{
+	int win;
+
+	win = mvebu_mbus_find_window(&mbus_state, base, size);
+	if (win < 0)
+		return win;
+
+	mvebu_mbus_disable_window(&mbus_state, win);
+	return 0;
+}
+
+static __init int mvebu_mbus_debugfs_init(void)
+{
+	struct mvebu_mbus_state *s = &mbus_state;
+
+	/*
+	 * If no base has been initialized, doesn't make sense to
+	 * register the debugfs entries. We may be on a multiplatform
+	 * kernel that isn't running a Marvell EBU SoC.
+	 */
+	if (!s->mbuswins_base)
+		return 0;
+
+	s->debugfs_root = debugfs_create_dir("mvebu-mbus", NULL);
+	if (s->debugfs_root) {
+		s->debugfs_sdram = debugfs_create_file("sdram", S_IRUGO,
+						       s->debugfs_root, NULL,
+						       &mvebu_sdram_debug_fops);
+		s->debugfs_devs = debugfs_create_file("devices", S_IRUGO,
+						      s->debugfs_root, NULL,
+						      &mvebu_devs_debug_fops);
+	}
+
+	return 0;
+}
+fs_initcall(mvebu_mbus_debugfs_init);
+
+int __init mvebu_mbus_init(const char *soc, phys_addr_t mbuswins_phys_base,
+			   size_t mbuswins_size,
+			   phys_addr_t sdramwins_phys_base,
+			   size_t sdramwins_size)
+{
+	struct mvebu_mbus_state *mbus = &mbus_state;
+	const struct of_device_id *of_id;
+	int win;
+
+	for (of_id = of_mvebu_mbus_ids; of_id->compatible; of_id++)
+		if (!strcmp(of_id->compatible, soc))
+			break;
+
+	if (!of_id->compatible) {
+		pr_err("mvebu-mbus: could not find a matching SoC family\n");
+		return -ENODEV;
+	}
+
+	mbus->soc = of_id->data;
+
+	mbus->mbuswins_base = ioremap(mbuswins_phys_base, mbuswins_size);
+	if (!mbus->mbuswins_base)
+		return -ENOMEM;
+
+	mbus->sdramwins_base = ioremap(sdramwins_phys_base, sdramwins_size);
+	if (!mbus->sdramwins_base) {
+		iounmap(mbus_state.mbuswins_base);
+		return -ENOMEM;
+	}
+
+	if (of_find_compatible_node(NULL, NULL, "marvell,coherency-fabric"))
+		mbus->hw_io_coherency = 1;
+
+	for (win = 0; win < mbus->soc->num_wins; win++)
+		mvebu_mbus_disable_window(mbus, win);
+
+	mbus->soc->setup_cpu_target(mbus);
+
+	return 0;
+}
diff --git a/drivers/cdrom/gdrom.c b/drivers/cdrom/gdrom.c
index d59cdcb8fe39..4afcb65cc623 100644
--- a/drivers/cdrom/gdrom.c
+++ b/drivers/cdrom/gdrom.c
@@ -503,12 +503,11 @@ static int gdrom_bdops_open(struct block_device *bdev, fmode_t mode)
 	return ret;
 }
 
-static int gdrom_bdops_release(struct gendisk *disk, fmode_t mode)
+static void gdrom_bdops_release(struct gendisk *disk, fmode_t mode)
 {
 	mutex_lock(&gdrom_mutex);
 	cdrom_release(gd.cd_info, mode);
 	mutex_unlock(&gdrom_mutex);
-	return 0;
 }
 
 static unsigned int gdrom_bdops_check_events(struct gendisk *disk,
diff --git a/drivers/char/ds1620.c b/drivers/char/ds1620.c
index 24ffd8cec51e..0fae5296e311 100644
--- a/drivers/char/ds1620.c
+++ b/drivers/char/ds1620.c
@@ -6,6 +6,7 @@
 #include <linux/miscdevice.h>
 #include <linux/delay.h>
 #include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 #include <linux/capability.h>
 #include <linux/init.h>
 #include <linux/mutex.h>
@@ -329,9 +330,7 @@ ds1620_unlocked_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 }
 
 #ifdef THERM_USE_PROC
-static int
-proc_therm_ds1620_read(char *buf, char **start, off_t offset,
-		       int len, int *eof, void *unused)
+static int ds1620_proc_therm_show(struct seq_file *m, void *v)
 {
 	struct therm th;
 	int temp;
@@ -339,17 +338,25 @@ proc_therm_ds1620_read(char *buf, char **start, off_t offset,
 	ds1620_read_state(&th);
 	temp =  cvt_9_to_int(ds1620_in(THERM_READ_TEMP, 9));
 
-	len = sprintf(buf, "Thermostat: HI %i.%i, LOW %i.%i; "
-		      "temperature: %i.%i C, fan %s\n",
-		      th.hi >> 1, th.hi & 1 ? 5 : 0,
-		      th.lo >> 1, th.lo & 1 ? 5 : 0,
-		      temp  >> 1, temp  & 1 ? 5 : 0,
-		      fan_state[netwinder_get_fan()]);
+	seq_printf(m, "Thermostat: HI %i.%i, LOW %i.%i; temperature: %i.%i C, fan %s\n",
+		   th.hi >> 1, th.hi & 1 ? 5 : 0,
+		   th.lo >> 1, th.lo & 1 ? 5 : 0,
+		   temp  >> 1, temp  & 1 ? 5 : 0,
+		   fan_state[netwinder_get_fan()]);
+	return 0;
+}
 
-	return len;
+static int ds1620_proc_therm_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ds1620_proc_therm_show, NULL);
 }
 
-static struct proc_dir_entry *proc_therm_ds1620;
+static const struct file_operations ds1620_proc_therm_fops = {
+	.open		= ds1620_proc_therm_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 #endif
 
 static const struct file_operations ds1620_fops = {
@@ -397,10 +404,7 @@ static int __init ds1620_init(void)
 		return ret;
 
 #ifdef THERM_USE_PROC
-	proc_therm_ds1620 = create_proc_entry("therm", 0, NULL);
-	if (proc_therm_ds1620)
-		proc_therm_ds1620->read_proc = proc_therm_ds1620_read;
-	else
+	if (!proc_create("therm", 0, NULL, &ds1620_proc_therm_fops))
 		printk(KERN_ERR "therm: unable to register /proc/therm\n");
 #endif
 
diff --git a/drivers/char/efirtc.c b/drivers/char/efirtc.c
index a082d00b0f11..e39e7402e623 100644
--- a/drivers/char/efirtc.c
+++ b/drivers/char/efirtc.c
@@ -34,6 +34,7 @@
 #include <linux/init.h>
 #include <linux/rtc.h>
 #include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 #include <linux/efi.h>
 #include <linux/uaccess.h>
 
@@ -296,12 +297,10 @@ static struct miscdevice efi_rtc_dev= {
 /*
  *	We export RAW EFI information to /proc/driver/efirtc
  */
-static int
-efi_rtc_get_status(char *buf)
+static int efi_rtc_proc_show(struct seq_file *m, void *v)
 {
 	efi_time_t 	eft, alm;
 	efi_time_cap_t	cap;
-	char		*p = buf;
 	efi_bool_t	enabled, pending;	
 	unsigned long	flags;
 
@@ -316,64 +315,63 @@ efi_rtc_get_status(char *buf)
 
 	spin_unlock_irqrestore(&efi_rtc_lock,flags);
 
-	p += sprintf(p,
-		     "Time           : %u:%u:%u.%09u\n"
-		     "Date           : %u-%u-%u\n"
-		     "Daylight       : %u\n",
-		     eft.hour, eft.minute, eft.second, eft.nanosecond, 
-		     eft.year, eft.month, eft.day,
-		     eft.daylight);
+	seq_printf(m,
+		   "Time           : %u:%u:%u.%09u\n"
+		   "Date           : %u-%u-%u\n"
+		   "Daylight       : %u\n",
+		   eft.hour, eft.minute, eft.second, eft.nanosecond, 
+		   eft.year, eft.month, eft.day,
+		   eft.daylight);
 
 	if (eft.timezone == EFI_UNSPECIFIED_TIMEZONE)
-		p += sprintf(p, "Timezone       : unspecified\n");
+		seq_puts(m, "Timezone       : unspecified\n");
 	else
 		/* XXX fixme: convert to string? */
-		p += sprintf(p, "Timezone       : %u\n", eft.timezone);
+		seq_printf(m, "Timezone       : %u\n", eft.timezone);
 		
 
-	p += sprintf(p,
-		     "Alarm Time     : %u:%u:%u.%09u\n"
-		     "Alarm Date     : %u-%u-%u\n"
-		     "Alarm Daylight : %u\n"
-		     "Enabled        : %s\n"
-		     "Pending        : %s\n",
-		     alm.hour, alm.minute, alm.second, alm.nanosecond, 
-		     alm.year, alm.month, alm.day, 
-		     alm.daylight,
-		     enabled == 1 ? "yes" : "no",
-		     pending == 1 ? "yes" : "no");
+	seq_printf(m,
+		   "Alarm Time     : %u:%u:%u.%09u\n"
+		   "Alarm Date     : %u-%u-%u\n"
+		   "Alarm Daylight : %u\n"
+		   "Enabled        : %s\n"
+		   "Pending        : %s\n",
+		   alm.hour, alm.minute, alm.second, alm.nanosecond, 
+		   alm.year, alm.month, alm.day, 
+		   alm.daylight,
+		   enabled == 1 ? "yes" : "no",
+		   pending == 1 ? "yes" : "no");
 
 	if (eft.timezone == EFI_UNSPECIFIED_TIMEZONE)
-		p += sprintf(p, "Timezone       : unspecified\n");
+		seq_puts(m, "Timezone       : unspecified\n");
 	else
 		/* XXX fixme: convert to string? */
-		p += sprintf(p, "Timezone       : %u\n", alm.timezone);
+		seq_printf(m, "Timezone       : %u\n", alm.timezone);
 
 	/*
 	 * now prints the capabilities
 	 */
-	p += sprintf(p,
-		     "Resolution     : %u\n"
-		     "Accuracy       : %u\n"
-		     "SetstoZero     : %u\n",
-		      cap.resolution, cap.accuracy, cap.sets_to_zero);
+	seq_printf(m,
+		   "Resolution     : %u\n"
+		   "Accuracy       : %u\n"
+		   "SetstoZero     : %u\n",
+		   cap.resolution, cap.accuracy, cap.sets_to_zero);
 
-	return  p - buf;
+	return 0;
 }
 
-static int
-efi_rtc_read_proc(char *page, char **start, off_t off,
-                                 int count, int *eof, void *data)
+static int efi_rtc_proc_open(struct inode *inode, struct file *file)
 {
-        int len = efi_rtc_get_status(page);
-        if (len <= off+count) *eof = 1;
-        *start = page + off;
-        len -= off;
-        if (len>count) len = count;
-        if (len<0) len = 0;
-        return len;
+	return single_open(file, efi_rtc_proc_show, NULL);
 }
 
+static const struct file_operations efi_rtc_proc_fops = {
+	.open		= efi_rtc_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
 static int __init 
 efi_rtc_init(void)
 {
@@ -389,8 +387,7 @@ efi_rtc_init(void)
 		return ret;
 	}
 
-	dir = create_proc_read_entry ("driver/efirtc", 0, NULL,
-			              efi_rtc_read_proc, NULL);
+	dir = proc_create("driver/efirtc", 0, NULL, &efi_rtc_proc_fops);
 	if (dir == NULL) {
 		printk(KERN_ERR "efirtc: can't create /proc/driver/efirtc.\n");
 		misc_deregister(&efi_rtc_dev);
diff --git a/drivers/char/genrtc.c b/drivers/char/genrtc.c
index 21cb980f1157..4f943759d376 100644
--- a/drivers/char/genrtc.c
+++ b/drivers/char/genrtc.c
@@ -52,6 +52,7 @@
 #include <linux/init.h>
 #include <linux/poll.h>
 #include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 #include <linux/mutex.h>
 #include <linux/workqueue.h>
 
@@ -386,18 +387,15 @@ static int gen_rtc_release(struct inode *inode, struct file *file)
  *	Info exported via "/proc/driver/rtc".
  */
 
-static int gen_rtc_proc_output(char *buf)
+static int gen_rtc_proc_show(struct seq_file *m, void *v)
 {
-	char *p;
 	struct rtc_time tm;
 	unsigned int flags;
 	struct rtc_pll_info pll;
 
-	p = buf;
-
 	flags = get_rtc_time(&tm);
 
-	p += sprintf(p,
+	seq_printf(m,
 		     "rtc_time\t: %02d:%02d:%02d\n"
 		     "rtc_date\t: %04d-%02d-%02d\n"
 		     "rtc_epoch\t: %04u\n",
@@ -406,23 +404,23 @@ static int gen_rtc_proc_output(char *buf)
 
 	tm.tm_hour = tm.tm_min = tm.tm_sec = 0;
 
-	p += sprintf(p, "alarm\t\t: ");
+	seq_puts(m, "alarm\t\t: ");
 	if (tm.tm_hour <= 24)
-		p += sprintf(p, "%02d:", tm.tm_hour);
+		seq_printf(m, "%02d:", tm.tm_hour);
 	else
-		p += sprintf(p, "**:");
+		seq_puts(m, "**:");
 
 	if (tm.tm_min <= 59)
-		p += sprintf(p, "%02d:", tm.tm_min);
+		seq_printf(m, "%02d:", tm.tm_min);
 	else
-		p += sprintf(p, "**:");
+		seq_puts(m, "**:");
 
 	if (tm.tm_sec <= 59)
-		p += sprintf(p, "%02d\n", tm.tm_sec);
+		seq_printf(m, "%02d\n", tm.tm_sec);
 	else
-		p += sprintf(p, "**\n");
+		seq_puts(m, "**\n");
 
-	p += sprintf(p,
+	seq_printf(m,
 		     "DST_enable\t: %s\n"
 		     "BCD\t\t: %s\n"
 		     "24hr\t\t: %s\n"
@@ -442,7 +440,7 @@ static int gen_rtc_proc_output(char *buf)
 		     0L /* freq */,
 		     (flags & RTC_BATT_BAD) ? "bad" : "okay");
 	if (!get_rtc_pll(&pll))
-	    p += sprintf(p,
+	    seq_printf(m,
 			 "PLL adjustment\t: %d\n"
 			 "PLL max +ve adjustment\t: %d\n"
 			 "PLL max -ve adjustment\t: %d\n"
@@ -455,26 +453,26 @@ static int gen_rtc_proc_output(char *buf)
 			 pll.pll_posmult,
 			 pll.pll_negmult,
 			 pll.pll_clock);
-	return p - buf;
+	return 0;
 }
 
-static int gen_rtc_read_proc(char *page, char **start, off_t off,
-			     int count, int *eof, void *data)
+static int gen_rtc_proc_open(struct inode *inode, struct file *file)
 {
-	int len = gen_rtc_proc_output (page);
-        if (len <= off+count) *eof = 1;
-	*start = page + off;
-	len -= off;
-        if (len>count) len = count;
-        if (len<0) len = 0;
-	return len;
+	return single_open(file, gen_rtc_proc_show, NULL);
 }
 
+static const struct file_operations gen_rtc_proc_fops = {
+	.open		= gen_rtc_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
 static int __init gen_rtc_proc_init(void)
 {
 	struct proc_dir_entry *r;
 
-	r = create_proc_read_entry("driver/rtc", 0, NULL, gen_rtc_read_proc, NULL);
+	r = proc_create("driver/rtc", 0, NULL, &gen_rtc_proc_fops);
 	if (!r)
 		return -ENOMEM;
 	return 0;
diff --git a/drivers/char/hw_random/Kconfig b/drivers/char/hw_random/Kconfig
index c5a0262251bc..2f9dbf7568fb 100644
--- a/drivers/char/hw_random/Kconfig
+++ b/drivers/char/hw_random/Kconfig
@@ -86,6 +86,18 @@ config HW_RANDOM_BCM63XX
 
 	  If unusure, say Y.
 
+config HW_RANDOM_BCM2835
+	tristate "Broadcom BCM2835 Random Number Generator support"
+	depends on HW_RANDOM && ARCH_BCM2835
+	default HW_RANDOM
+	---help---
+	  This driver provides kernel-side support for the Random Number
+	  Generator hardware found on the Broadcom BCM2835 SoCs.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called bcm2835-rng
+
+	  If unsure, say Y.
 
 config HW_RANDOM_GEODE
 	tristate "AMD Geode HW Random Number Generator support"
diff --git a/drivers/char/hw_random/Makefile b/drivers/char/hw_random/Makefile
index 1fd7eec9fbf6..bed467c9300e 100644
--- a/drivers/char/hw_random/Makefile
+++ b/drivers/char/hw_random/Makefile
@@ -26,3 +26,4 @@ obj-$(CONFIG_HW_RANDOM_PPC4XX) += ppc4xx-rng.o
 obj-$(CONFIG_HW_RANDOM_PSERIES) += pseries-rng.o
 obj-$(CONFIG_HW_RANDOM_EXYNOS)	+= exynos-rng.o
 obj-$(CONFIG_HW_RANDOM_TPM) += tpm-rng.o
+obj-$(CONFIG_HW_RANDOM_BCM2835) += bcm2835-rng.o
diff --git a/drivers/char/hw_random/bcm2835-rng.c b/drivers/char/hw_random/bcm2835-rng.c
new file mode 100644
index 000000000000..eb7f14725ebd
--- /dev/null
+++ b/drivers/char/hw_random/bcm2835-rng.c
@@ -0,0 +1,113 @@
+/**
+ * Copyright (c) 2010-2012 Broadcom. All rights reserved.
+ * Copyright (c) 2013 Lubomir Rintel
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License ("GPL")
+ * version 2, as published by the Free Software Foundation.
+ */
+
+#include <linux/hw_random.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/printk.h>
+
+#define RNG_CTRL	0x0
+#define RNG_STATUS	0x4
+#define RNG_DATA	0x8
+
+/* enable rng */
+#define RNG_RBGEN	0x1
+
+/* the initial numbers generated are "less random" so will be discarded */
+#define RNG_WARMUP_COUNT 0x40000
+
+static int bcm2835_rng_read(struct hwrng *rng, void *buf, size_t max,
+			       bool wait)
+{
+	void __iomem *rng_base = (void __iomem *)rng->priv;
+
+	while ((__raw_readl(rng_base + RNG_STATUS) >> 24) == 0) {
+		if (!wait)
+			return 0;
+		cpu_relax();
+	}
+
+	*(u32 *)buf = __raw_readl(rng_base + RNG_DATA);
+	return sizeof(u32);
+}
+
+static struct hwrng bcm2835_rng_ops = {
+	.name	= "bcm2835",
+	.read	= bcm2835_rng_read,
+};
+
+static int bcm2835_rng_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	void __iomem *rng_base;
+	int err;
+
+	/* map peripheral */
+	rng_base = of_iomap(np, 0);
+	if (!rng_base) {
+		dev_err(dev, "failed to remap rng regs");
+		return -ENODEV;
+	}
+	bcm2835_rng_ops.priv = (unsigned long)rng_base;
+
+	/* register driver */
+	err = hwrng_register(&bcm2835_rng_ops);
+	if (err) {
+		dev_err(dev, "hwrng registration failed\n");
+		iounmap(rng_base);
+	} else {
+		dev_info(dev, "hwrng registered\n");
+
+		/* set warm-up count & enable */
+		__raw_writel(RNG_WARMUP_COUNT, rng_base + RNG_STATUS);
+		__raw_writel(RNG_RBGEN, rng_base + RNG_CTRL);
+	}
+	return err;
+}
+
+static int bcm2835_rng_remove(struct platform_device *pdev)
+{
+	void __iomem *rng_base = (void __iomem *)bcm2835_rng_ops.priv;
+
+	/* disable rng hardware */
+	__raw_writel(0, rng_base + RNG_CTRL);
+
+	/* unregister driver */
+	hwrng_unregister(&bcm2835_rng_ops);
+	iounmap(rng_base);
+
+	return 0;
+}
+
+static const struct of_device_id bcm2835_rng_of_match[] = {
+	{ .compatible = "brcm,bcm2835-rng", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, bcm2835_rng_of_match);
+
+static struct platform_driver bcm2835_rng_driver = {
+	.driver = {
+		.name = "bcm2835-rng",
+		.owner = THIS_MODULE,
+		.of_match_table = bcm2835_rng_of_match,
+	},
+	.probe		= bcm2835_rng_probe,
+	.remove		= bcm2835_rng_remove,
+};
+module_platform_driver(bcm2835_rng_driver);
+
+MODULE_AUTHOR("Lubomir Rintel <lkundrak@v3.sk>");
+MODULE_DESCRIPTION("BCM2835 Random Number Generator (RNG) driver");
+MODULE_LICENSE("GPLv2");
diff --git a/drivers/char/hw_random/exynos-rng.c b/drivers/char/hw_random/exynos-rng.c
index ac47631ab34f..402ccfb625c5 100644
--- a/drivers/char/hw_random/exynos-rng.c
+++ b/drivers/char/hw_random/exynos-rng.c
@@ -144,6 +144,7 @@ static int exynos_rng_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#if defined(CONFIG_PM_SLEEP) || defined(CONFIG_PM_RUNTIME)
 static int exynos_rng_runtime_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
@@ -161,7 +162,7 @@ static int exynos_rng_runtime_resume(struct device *dev)
 
 	return clk_prepare_enable(exynos_rng->clk);
 }
-
+#endif
 
 static UNIVERSAL_DEV_PM_OPS(exynos_rng_pm_ops, exynos_rng_runtime_suspend,
 					exynos_rng_runtime_resume, NULL);
diff --git a/drivers/char/hw_random/mxc-rnga.c b/drivers/char/hw_random/mxc-rnga.c
index 895d0b8fb9ab..4ca35e8a5d8c 100644
--- a/drivers/char/hw_random/mxc-rnga.c
+++ b/drivers/char/hw_random/mxc-rnga.c
@@ -142,7 +142,7 @@ static void mxc_rnga_cleanup(struct hwrng *rng)
 static int __init mxc_rnga_probe(struct platform_device *pdev)
 {
 	int err = -ENODEV;
-	struct resource *res, *mem;
+	struct resource *res;
 	struct mxc_rng *mxc_rng;
 
 	mxc_rng = devm_kzalloc(&pdev->dev, sizeof(struct mxc_rng),
@@ -172,15 +172,9 @@ static int __init mxc_rnga_probe(struct platform_device *pdev)
 		goto err_region;
 	}
 
-	mem = request_mem_region(res->start, resource_size(res), pdev->name);
-	if (mem == NULL) {
-		err = -EBUSY;
-		goto err_region;
-	}
-
-	mxc_rng->mem = ioremap(res->start, resource_size(res));
-	if (!mxc_rng->mem) {
-		err = -ENOMEM;
+	mxc_rng->mem = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(mxc_rng->mem)) {
+		err = PTR_ERR(mxc_rng->mem);
 		goto err_ioremap;
 	}
 
@@ -195,8 +189,6 @@ static int __init mxc_rnga_probe(struct platform_device *pdev)
 	return 0;
 
 err_ioremap:
-	release_mem_region(res->start, resource_size(res));
-
 err_region:
 	clk_disable_unprepare(mxc_rng->clk);
 
@@ -206,15 +198,10 @@ out:
 
 static int __exit mxc_rnga_remove(struct platform_device *pdev)
 {
-	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	struct mxc_rng *mxc_rng = platform_get_drvdata(pdev);
 
 	hwrng_unregister(&mxc_rng->rng);
 
-	iounmap(mxc_rng->mem);
-
-	release_mem_region(res->start, resource_size(res));
-
 	clk_disable_unprepare(mxc_rng->clk);
 
 	return 0;
diff --git a/drivers/char/hw_random/timeriomem-rng.c b/drivers/char/hw_random/timeriomem-rng.c
index 849db199c02c..3e75737f5fe1 100644
--- a/drivers/char/hw_random/timeriomem-rng.c
+++ b/drivers/char/hw_random/timeriomem-rng.c
@@ -23,127 +23,209 @@
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/platform_device.h>
+#include <linux/of.h>
 #include <linux/hw_random.h>
 #include <linux/io.h>
+#include <linux/slab.h>
 #include <linux/timeriomem-rng.h>
 #include <linux/jiffies.h>
 #include <linux/sched.h>
 #include <linux/timer.h>
 #include <linux/completion.h>
 
-static struct timeriomem_rng_data *timeriomem_rng_data;
+struct timeriomem_rng_private_data {
+	void __iomem		*io_base;
+	unsigned int		expires;
+	unsigned int		period;
+	unsigned int		present:1;
 
-static void timeriomem_rng_trigger(unsigned long);
-static DEFINE_TIMER(timeriomem_rng_timer, timeriomem_rng_trigger, 0, 0);
+	struct timer_list	timer;
+	struct completion	completion;
+
+	struct hwrng		timeriomem_rng_ops;
+};
+
+#define to_rng_priv(rng) \
+		((struct timeriomem_rng_private_data *)rng->priv)
 
 /*
  * have data return 1, however return 0 if we have nothing
  */
 static int timeriomem_rng_data_present(struct hwrng *rng, int wait)
 {
-	if (rng->priv == 0)
-		return 1;
+	struct timeriomem_rng_private_data *priv = to_rng_priv(rng);
 
-	if (!wait || timeriomem_rng_data->present)
-		return timeriomem_rng_data->present;
+	if (!wait || priv->present)
+		return priv->present;
 
-	wait_for_completion(&timeriomem_rng_data->completion);
+	wait_for_completion(&priv->completion);
 
 	return 1;
 }
 
 static int timeriomem_rng_data_read(struct hwrng *rng, u32 *data)
 {
+	struct timeriomem_rng_private_data *priv = to_rng_priv(rng);
 	unsigned long cur;
 	s32 delay;
 
-	*data = readl(timeriomem_rng_data->address);
+	*data = readl(priv->io_base);
 
-	if (rng->priv != 0) {
-		cur = jiffies;
+	cur = jiffies;
 
-		delay = cur - timeriomem_rng_timer.expires;
-		delay = rng->priv - (delay % rng->priv);
+	delay = cur - priv->expires;
+	delay = priv->period - (delay % priv->period);
 
-		timeriomem_rng_timer.expires = cur + delay;
-		timeriomem_rng_data->present = 0;
+	priv->expires = cur + delay;
+	priv->present = 0;
 
-		init_completion(&timeriomem_rng_data->completion);
-		add_timer(&timeriomem_rng_timer);
-	}
+	INIT_COMPLETION(priv->completion);
+	mod_timer(&priv->timer, priv->expires);
 
 	return 4;
 }
 
-static void timeriomem_rng_trigger(unsigned long dummy)
+static void timeriomem_rng_trigger(unsigned long data)
 {
-	timeriomem_rng_data->present = 1;
-	complete(&timeriomem_rng_data->completion);
-}
+	struct timeriomem_rng_private_data *priv
+			= (struct timeriomem_rng_private_data *)data;
 
-static struct hwrng timeriomem_rng_ops = {
-	.name		= "timeriomem",
-	.data_present	= timeriomem_rng_data_present,
-	.data_read	= timeriomem_rng_data_read,
-	.priv		= 0,
-};
+	priv->present = 1;
+	complete(&priv->completion);
+}
 
 static int timeriomem_rng_probe(struct platform_device *pdev)
 {
+	struct timeriomem_rng_data *pdata = pdev->dev.platform_data;
+	struct timeriomem_rng_private_data *priv;
 	struct resource *res;
-	int ret;
+	int err = 0;
+	int period;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!pdev->dev.of_node && !pdata) {
+		dev_err(&pdev->dev, "timeriomem_rng_data is missing\n");
+		return -EINVAL;
+	}
 
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res)
-		return -ENOENT;
+		return -ENXIO;
 
-	timeriomem_rng_data = pdev->dev.platform_data;
+	if (res->start % 4 != 0 || resource_size(res) != 4) {
+		dev_err(&pdev->dev,
+			"address must be four bytes wide and aligned\n");
+		return -EINVAL;
+	}
 
-	timeriomem_rng_data->address = ioremap(res->start, resource_size(res));
-	if (!timeriomem_rng_data->address)
-		return -EIO;
+	/* Allocate memory for the device structure (and zero it) */
+	priv = kzalloc(sizeof(struct timeriomem_rng_private_data), GFP_KERNEL);
+	if (!priv) {
+		dev_err(&pdev->dev, "failed to allocate device structure.\n");
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pdev, priv);
+
+	if (pdev->dev.of_node) {
+		int i;
+
+		if (!of_property_read_u32(pdev->dev.of_node,
+						"period", &i))
+			period = i;
+		else {
+			dev_err(&pdev->dev, "missing period\n");
+			err = -EINVAL;
+			goto out_free;
+		}
+	} else
+		period = pdata->period;
+
+	priv->period = usecs_to_jiffies(period);
+	if (priv->period < 1) {
+		dev_err(&pdev->dev, "period is less than one jiffy\n");
+		err = -EINVAL;
+		goto out_free;
+	}
 
-	if (timeriomem_rng_data->period != 0
-		&& usecs_to_jiffies(timeriomem_rng_data->period) > 0) {
-		timeriomem_rng_timer.expires = jiffies;
+	priv->expires	= jiffies;
+	priv->present	= 1;
 
-		timeriomem_rng_ops.priv = usecs_to_jiffies(
-						timeriomem_rng_data->period);
+	init_completion(&priv->completion);
+	complete(&priv->completion);
+
+	setup_timer(&priv->timer, timeriomem_rng_trigger, (unsigned long)priv);
+
+	priv->timeriomem_rng_ops.name		= dev_name(&pdev->dev);
+	priv->timeriomem_rng_ops.data_present	= timeriomem_rng_data_present;
+	priv->timeriomem_rng_ops.data_read	= timeriomem_rng_data_read;
+	priv->timeriomem_rng_ops.priv		= (unsigned long)priv;
+
+	if (!request_mem_region(res->start, resource_size(res),
+				dev_name(&pdev->dev))) {
+		dev_err(&pdev->dev, "request_mem_region failed\n");
+		err = -EBUSY;
+		goto out_timer;
 	}
-	timeriomem_rng_data->present = 1;
 
-	ret = hwrng_register(&timeriomem_rng_ops);
-	if (ret)
-		goto failed;
+	priv->io_base = ioremap(res->start, resource_size(res));
+	if (priv->io_base == NULL) {
+		dev_err(&pdev->dev, "ioremap failed\n");
+		err = -EIO;
+		goto out_release_io;
+	}
+
+	err = hwrng_register(&priv->timeriomem_rng_ops);
+	if (err) {
+		dev_err(&pdev->dev, "problem registering\n");
+		goto out;
+	}
 
 	dev_info(&pdev->dev, "32bits from 0x%p @ %dus\n",
-			timeriomem_rng_data->address,
-			timeriomem_rng_data->period);
+			priv->io_base, period);
 
 	return 0;
 
-failed:
-	dev_err(&pdev->dev, "problem registering\n");
-	iounmap(timeriomem_rng_data->address);
-
-	return ret;
+out:
+	iounmap(priv->io_base);
+out_release_io:
+	release_mem_region(res->start, resource_size(res));
+out_timer:
+	del_timer_sync(&priv->timer);
+out_free:
+	platform_set_drvdata(pdev, NULL);
+	kfree(priv);
+	return err;
 }
 
 static int timeriomem_rng_remove(struct platform_device *pdev)
 {
-	del_timer_sync(&timeriomem_rng_timer);
-	hwrng_unregister(&timeriomem_rng_ops);
+	struct timeriomem_rng_private_data *priv = platform_get_drvdata(pdev);
+	struct resource *res;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
-	iounmap(timeriomem_rng_data->address);
+	hwrng_unregister(&priv->timeriomem_rng_ops);
+
+	del_timer_sync(&priv->timer);
+	iounmap(priv->io_base);
+	release_mem_region(res->start, resource_size(res));
+	platform_set_drvdata(pdev, NULL);
+	kfree(priv);
 
 	return 0;
 }
 
+static const struct of_device_id timeriomem_rng_match[] = {
+	{ .compatible = "timeriomem_rng" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, timeriomem_rng_match);
+
 static struct platform_driver timeriomem_rng_driver = {
 	.driver = {
 		.name		= "timeriomem_rng",
 		.owner		= THIS_MODULE,
+		.of_match_table	= timeriomem_rng_match,
 	},
 	.probe		= timeriomem_rng_probe,
 	.remove		= timeriomem_rng_remove,
diff --git a/drivers/char/hw_random/virtio-rng.c b/drivers/char/hw_random/virtio-rng.c
index 6bf4d47324eb..ef46a9cfd832 100644
--- a/drivers/char/hw_random/virtio-rng.c
+++ b/drivers/char/hw_random/virtio-rng.c
@@ -47,7 +47,7 @@ static void register_buffer(u8 *buf, size_t size)
 	sg_init_one(&sg, buf, size);
 
 	/* There should always be room for one buffer. */
-	if (virtqueue_add_buf(vq, &sg, 0, 1, buf, GFP_KERNEL) < 0)
+	if (virtqueue_add_inbuf(vq, &sg, 1, buf, GFP_KERNEL) < 0)
 		BUG();
 
 	virtqueue_kick(vq);
diff --git a/drivers/char/ipmi/ipmi_msghandler.c b/drivers/char/ipmi/ipmi_msghandler.c
index 053201b062a4..4d439d2fcfd6 100644
--- a/drivers/char/ipmi/ipmi_msghandler.c
+++ b/drivers/char/ipmi/ipmi_msghandler.c
@@ -1917,7 +1917,7 @@ static int smi_ipmb_proc_show(struct seq_file *m, void *v)
 
 static int smi_ipmb_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, smi_ipmb_proc_show, PDE(inode)->data);
+	return single_open(file, smi_ipmb_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations smi_ipmb_proc_ops = {
@@ -1938,7 +1938,7 @@ static int smi_version_proc_show(struct seq_file *m, void *v)
 
 static int smi_version_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, smi_version_proc_show, PDE(inode)->data);
+	return single_open(file, smi_version_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations smi_version_proc_ops = {
@@ -2013,7 +2013,7 @@ static int smi_stats_proc_show(struct seq_file *m, void *v)
 
 static int smi_stats_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, smi_stats_proc_show, PDE(inode)->data);
+	return single_open(file, smi_stats_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations smi_stats_proc_ops = {
@@ -4541,7 +4541,7 @@ static void __exit cleanup_ipmi(void)
 	del_timer_sync(&ipmi_timer);
 
 #ifdef CONFIG_PROC_FS
-	remove_proc_entry(proc_ipmi_root->name, NULL);
+	proc_remove(proc_ipmi_root);
 #endif /* CONFIG_PROC_FS */
 
 	driver_unregister(&ipmidriver.driver);
diff --git a/drivers/char/ipmi/ipmi_si_intf.c b/drivers/char/ipmi/ipmi_si_intf.c
index 0ac9b45a585e..313538abe63c 100644
--- a/drivers/char/ipmi/ipmi_si_intf.c
+++ b/drivers/char/ipmi/ipmi_si_intf.c
@@ -2839,7 +2839,7 @@ static int smi_type_proc_show(struct seq_file *m, void *v)
 
 static int smi_type_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, smi_type_proc_show, PDE(inode)->data);
+	return single_open(file, smi_type_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations smi_type_proc_ops = {
@@ -2882,7 +2882,7 @@ static int smi_si_stats_proc_show(struct seq_file *m, void *v)
 
 static int smi_si_stats_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, smi_si_stats_proc_show, PDE(inode)->data);
+	return single_open(file, smi_si_stats_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations smi_si_stats_proc_ops = {
@@ -2910,7 +2910,7 @@ static int smi_params_proc_show(struct seq_file *m, void *v)
 
 static int smi_params_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, smi_params_proc_show, PDE(inode)->data);
+	return single_open(file, smi_params_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations smi_params_proc_ops = {
diff --git a/drivers/char/mem.c b/drivers/char/mem.c
index 2c644afbcdd4..1ccbe9482faa 100644
--- a/drivers/char/mem.c
+++ b/drivers/char/mem.c
@@ -28,6 +28,7 @@
 #include <linux/pfn.h>
 #include <linux/export.h>
 #include <linux/io.h>
+#include <linux/aio.h>
 
 #include <asm/uaccess.h>
 
@@ -627,6 +628,18 @@ static ssize_t write_null(struct file *file, const char __user *buf,
 	return count;
 }
 
+static ssize_t aio_read_null(struct kiocb *iocb, const struct iovec *iov,
+			     unsigned long nr_segs, loff_t pos)
+{
+	return 0;
+}
+
+static ssize_t aio_write_null(struct kiocb *iocb, const struct iovec *iov,
+			      unsigned long nr_segs, loff_t pos)
+{
+	return iov_length(iov, nr_segs);
+}
+
 static int pipe_to_null(struct pipe_inode_info *info, struct pipe_buffer *buf,
 			struct splice_desc *sd)
 {
@@ -670,6 +683,24 @@ static ssize_t read_zero(struct file *file, char __user *buf,
 	return written ? written : -EFAULT;
 }
 
+static ssize_t aio_read_zero(struct kiocb *iocb, const struct iovec *iov,
+			     unsigned long nr_segs, loff_t pos)
+{
+	size_t written = 0;
+	unsigned long i;
+	ssize_t ret;
+
+	for (i = 0; i < nr_segs; i++) {
+		ret = read_zero(iocb->ki_filp, iov[i].iov_base, iov[i].iov_len,
+				&pos);
+		if (ret < 0)
+			break;
+		written += ret;
+	}
+
+	return written ? written : -EFAULT;
+}
+
 static int mmap_zero(struct file *file, struct vm_area_struct *vma)
 {
 #ifndef CONFIG_MMU
@@ -738,6 +769,7 @@ static int open_port(struct inode *inode, struct file *filp)
 #define full_lseek      null_lseek
 #define write_zero	write_null
 #define read_full       read_zero
+#define aio_write_zero	aio_write_null
 #define open_mem	open_port
 #define open_kmem	open_mem
 #define open_oldmem	open_mem
@@ -766,6 +798,8 @@ static const struct file_operations null_fops = {
 	.llseek		= null_lseek,
 	.read		= read_null,
 	.write		= write_null,
+	.aio_read	= aio_read_null,
+	.aio_write	= aio_write_null,
 	.splice_write	= splice_write_null,
 };
 
@@ -782,6 +816,8 @@ static const struct file_operations zero_fops = {
 	.llseek		= zero_lseek,
 	.read		= read_zero,
 	.write		= write_zero,
+	.aio_read	= aio_read_zero,
+	.aio_write	= aio_write_zero,
 	.mmap		= mmap_zero,
 };
 
diff --git a/drivers/char/random.c b/drivers/char/random.c
index 32a6c5764950..cd9a6211dcad 100644
--- a/drivers/char/random.c
+++ b/drivers/char/random.c
@@ -1485,6 +1485,7 @@ unsigned int get_random_int(void)
 
 	return ret;
 }
+EXPORT_SYMBOL(get_random_int);
 
 /*
  * randomize_range() returns a start address such that
diff --git a/drivers/char/tpm/tpm.c b/drivers/char/tpm/tpm.c
index 0d2e82f95577..7c3b3dcbfbc8 100644
--- a/drivers/char/tpm/tpm.c
+++ b/drivers/char/tpm/tpm.c
@@ -1337,7 +1337,7 @@ int tpm_pm_suspend(struct device *dev)
 {
 	struct tpm_chip *chip = dev_get_drvdata(dev);
 	struct tpm_cmd_t cmd;
-	int rc;
+	int rc, try;
 
 	u8 dummy_hash[TPM_DIGEST_SIZE] = { 0 };
 
@@ -1355,9 +1355,32 @@ int tpm_pm_suspend(struct device *dev)
 	}
 
 	/* now do the actual savestate */
-	cmd.header.in = savestate_header;
-	rc = transmit_cmd(chip, &cmd, SAVESTATE_RESULT_SIZE,
-			  "sending savestate before suspend");
+	for (try = 0; try < TPM_RETRY; try++) {
+		cmd.header.in = savestate_header;
+		rc = transmit_cmd(chip, &cmd, SAVESTATE_RESULT_SIZE, NULL);
+
+		/*
+		 * If the TPM indicates that it is too busy to respond to
+		 * this command then retry before giving up.  It can take
+		 * several seconds for this TPM to be ready.
+		 *
+		 * This can happen if the TPM has already been sent the
+		 * SaveState command before the driver has loaded.  TCG 1.2
+		 * specification states that any communication after SaveState
+		 * may cause the TPM to invalidate previously saved state.
+		 */
+		if (rc != TPM_WARN_RETRY)
+			break;
+		msleep(TPM_TIMEOUT_RETRY);
+	}
+
+	if (rc)
+		dev_err(chip->dev,
+			"Error (%d) sending savestate before suspend\n", rc);
+	else if (try > 0)
+		dev_warn(chip->dev, "TPM savestate took %dms\n",
+			 try * TPM_TIMEOUT_RETRY);
+
 	return rc;
 }
 EXPORT_SYMBOL_GPL(tpm_pm_suspend);
diff --git a/drivers/char/tpm/tpm.h b/drivers/char/tpm/tpm.h
index 81b52015f669..0770d1d79366 100644
--- a/drivers/char/tpm/tpm.h
+++ b/drivers/char/tpm/tpm.h
@@ -32,10 +32,12 @@ enum tpm_const {
 	TPM_MINOR = 224,	/* officially assigned */
 	TPM_BUFSIZE = 4096,
 	TPM_NUM_DEVICES = 256,
+	TPM_RETRY = 50,		/* 5 seconds */
 };
 
 enum tpm_timeout {
 	TPM_TIMEOUT = 5,	/* msecs */
+	TPM_TIMEOUT_RETRY = 100 /* msecs */
 };
 
 /* TPM addresses */
@@ -44,6 +46,7 @@ enum tpm_addr {
 	TPM_ADDR = 0x4E,
 };
 
+#define TPM_WARN_RETRY          0x800
 #define TPM_WARN_DOING_SELFTEST 0x802
 #define TPM_ERR_DEACTIVATED     0x6
 #define TPM_ERR_DISABLED        0x7
diff --git a/drivers/char/tpm/tpm_i2c_infineon.c b/drivers/char/tpm/tpm_i2c_infineon.c
index 8fe7ac3d095b..37d5dcc10ea7 100644
--- a/drivers/char/tpm/tpm_i2c_infineon.c
+++ b/drivers/char/tpm/tpm_i2c_infineon.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2012 Infineon Technologies
+ * Copyright (C) 2012,2013 Infineon Technologies
  *
  * Authors:
  * Peter Huewe <peter.huewe@infineon.com>
@@ -56,13 +56,21 @@
 #define TPM_TIMEOUT_US_HI  (TPM_TIMEOUT_US_LOW + 2000)
 
 /* expected value for DIDVID register */
-#define TPM_TIS_I2C_DID_VID 0x000b15d1L
+#define TPM_TIS_I2C_DID_VID_9635 0xd1150b00L
+#define TPM_TIS_I2C_DID_VID_9645 0x001a15d1L
+
+enum i2c_chip_type {
+	SLB9635,
+	SLB9645,
+	UNKNOWN,
+};
 
 /* Structure to store I2C TPM specific stuff */
 struct tpm_inf_dev {
 	struct i2c_client *client;
 	u8 buf[TPM_BUFSIZE + sizeof(u8)]; /* max. buffer size + addr */
 	struct tpm_chip *chip;
+	enum i2c_chip_type chip_type;
 };
 
 static struct tpm_inf_dev tpm_dev;
@@ -90,10 +98,20 @@ static struct i2c_driver tpm_tis_i2c_driver;
 static int iic_tpm_read(u8 addr, u8 *buffer, size_t len)
 {
 
-	struct i2c_msg msg1 = { tpm_dev.client->addr, 0, 1, &addr };
-	struct i2c_msg msg2 = { tpm_dev.client->addr, I2C_M_RD, len, buffer };
+	struct i2c_msg msg1 = {
+		.addr = tpm_dev.client->addr,
+		.len = 1,
+		.buf = &addr
+	};
+	struct i2c_msg msg2 = {
+		.addr = tpm_dev.client->addr,
+		.flags = I2C_M_RD,
+		.len = len,
+		.buf = buffer
+	};
+	struct i2c_msg msgs[] = {msg1, msg2};
 
-	int rc;
+	int rc = 0;
 	int count;
 
 	/* Lock the adapter for the duration of the whole sequence. */
@@ -101,30 +119,53 @@ static int iic_tpm_read(u8 addr, u8 *buffer, size_t len)
 		return -EOPNOTSUPP;
 	i2c_lock_adapter(tpm_dev.client->adapter);
 
-	for (count = 0; count < MAX_COUNT; count++) {
-		rc = __i2c_transfer(tpm_dev.client->adapter, &msg1, 1);
-		if (rc > 0)
-			break;	/* break here to skip sleep */
-
-		usleep_range(SLEEP_DURATION_LOW, SLEEP_DURATION_HI);
-	}
-
-	if (rc <= 0)
-		goto out;
-
-	/* After the TPM has successfully received the register address it needs
-	 * some time, thus we're sleeping here again, before retrieving the data
-	 */
-	for (count = 0; count < MAX_COUNT; count++) {
-		usleep_range(SLEEP_DURATION_LOW, SLEEP_DURATION_HI);
-		rc = __i2c_transfer(tpm_dev.client->adapter, &msg2, 1);
-		if (rc > 0)
-			break;
+	if (tpm_dev.chip_type == SLB9645) {
+		/* use a combined read for newer chips
+		 * unfortunately the smbus functions are not suitable due to
+		 * the 32 byte limit of the smbus.
+		 * retries should usually not be needed, but are kept just to
+		 * be on the safe side.
+		 */
+		for (count = 0; count < MAX_COUNT; count++) {
+			rc = __i2c_transfer(tpm_dev.client->adapter, msgs, 2);
+			if (rc > 0)
+				break;	/* break here to skip sleep */
+			usleep_range(SLEEP_DURATION_LOW, SLEEP_DURATION_HI);
+		}
+	} else {
+		/* slb9635 protocol should work in all cases */
+		for (count = 0; count < MAX_COUNT; count++) {
+			rc = __i2c_transfer(tpm_dev.client->adapter, &msg1, 1);
+			if (rc > 0)
+				break;	/* break here to skip sleep */
+
+			usleep_range(SLEEP_DURATION_LOW, SLEEP_DURATION_HI);
+		}
 
+		if (rc <= 0)
+			goto out;
+
+		/* After the TPM has successfully received the register address
+		 * it needs some time, thus we're sleeping here again, before
+		 * retrieving the data
+		 */
+		for (count = 0; count < MAX_COUNT; count++) {
+			usleep_range(SLEEP_DURATION_LOW, SLEEP_DURATION_HI);
+			rc = __i2c_transfer(tpm_dev.client->adapter, &msg2, 1);
+			if (rc > 0)
+				break;
+		}
 	}
 
 out:
 	i2c_unlock_adapter(tpm_dev.client->adapter);
+	/* take care of 'guard time' */
+	usleep_range(SLEEP_DURATION_LOW, SLEEP_DURATION_HI);
+
+	/* __i2c_transfer returns the number of successfully transferred
+	 * messages.
+	 * So rc should be greater than 0 here otherwise we have an error.
+	 */
 	if (rc <= 0)
 		return -EIO;
 
@@ -138,7 +179,11 @@ static int iic_tpm_write_generic(u8 addr, u8 *buffer, size_t len,
 	int rc = -EIO;
 	int count;
 
-	struct i2c_msg msg1 = { tpm_dev.client->addr, 0, len + 1, tpm_dev.buf };
+	struct i2c_msg msg1 = {
+		.addr = tpm_dev.client->addr,
+		.len = len + 1,
+		.buf = tpm_dev.buf
+	};
 
 	if (len > TPM_BUFSIZE)
 		return -EINVAL;
@@ -154,16 +199,24 @@ static int iic_tpm_write_generic(u8 addr, u8 *buffer, size_t len,
 	/*
 	 * NOTE: We have to use these special mechanisms here and unfortunately
 	 * cannot rely on the standard behavior of i2c_transfer.
+	 * Even for newer chips the smbus functions are not
+	 * suitable due to the 32 byte limit of the smbus.
 	 */
 	for (count = 0; count < max_count; count++) {
 		rc = __i2c_transfer(tpm_dev.client->adapter, &msg1, 1);
 		if (rc > 0)
 			break;
-
 		usleep_range(sleep_low, sleep_hi);
 	}
 
 	i2c_unlock_adapter(tpm_dev.client->adapter);
+	/* take care of 'guard time' */
+	usleep_range(SLEEP_DURATION_LOW, SLEEP_DURATION_HI);
+
+	/* __i2c_transfer returns the number of successfully transferred
+	 * messages.
+	 * So rc should be greater than 0 here otherwise we have an error.
+	 */
 	if (rc <= 0)
 		return -EIO;
 
@@ -283,11 +336,18 @@ static int request_locality(struct tpm_chip *chip, int loc)
 static u8 tpm_tis_i2c_status(struct tpm_chip *chip)
 {
 	/* NOTE: since I2C read may fail, return 0 in this case --> time-out */
-	u8 buf;
-	if (iic_tpm_read(TPM_STS(chip->vendor.locality), &buf, 1) < 0)
-		return 0;
-	else
-		return buf;
+	u8 buf = 0xFF;
+	u8 i = 0;
+
+	do {
+		if (iic_tpm_read(TPM_STS(chip->vendor.locality), &buf, 1) < 0)
+			return 0;
+
+		i++;
+	/* if locallity is set STS should not be 0xFF */
+	} while ((buf == 0xFF) && i < 10);
+
+	return buf;
 }
 
 static void tpm_tis_i2c_ready(struct tpm_chip *chip)
@@ -328,7 +388,7 @@ static int wait_for_stat(struct tpm_chip *chip, u8 mask, unsigned long timeout,
 
 	/* check current status */
 	*status = tpm_tis_i2c_status(chip);
-	if ((*status & mask) == mask)
+	if ((*status != 0xFF) && (*status & mask) == mask)
 		return 0;
 
 	stop = jiffies + timeout;
@@ -372,7 +432,6 @@ static int recv_data(struct tpm_chip *chip, u8 *buf, size_t count)
 		/* avoid endless loop in case of broken HW */
 		if (retries > MAX_COUNT_LONG)
 			return -EIO;
-
 	}
 	return size;
 }
@@ -480,7 +539,6 @@ static int tpm_tis_i2c_send(struct tpm_chip *chip, u8 *buf, size_t len)
 			rc = -EIO;
 			goto out_err;
 		}
-
 	}
 
 	/* write last byte */
@@ -568,6 +626,7 @@ static int tpm_tis_i2c_init(struct device *dev)
 
 	chip = tpm_register_hardware(dev, &tpm_tis_i2c);
 	if (!chip) {
+		dev_err(dev, "could not register hardware\n");
 		rc = -ENODEV;
 		goto out_err;
 	}
@@ -582,20 +641,24 @@ static int tpm_tis_i2c_init(struct device *dev)
 	chip->vendor.timeout_d = msecs_to_jiffies(TIS_SHORT_TIMEOUT);
 
 	if (request_locality(chip, 0) != 0) {
+		dev_err(dev, "could not request locality\n");
 		rc = -ENODEV;
 		goto out_vendor;
 	}
 
 	/* read four bytes from DID_VID register */
 	if (iic_tpm_read(TPM_DID_VID(0), (u8 *)&vendor, 4) < 0) {
+		dev_err(dev, "could not read vendor id\n");
 		rc = -EIO;
 		goto out_release;
 	}
 
-	/* create DID_VID register value, after swapping to little-endian */
-	vendor = be32_to_cpu((__be32) vendor);
-
-	if (vendor != TPM_TIS_I2C_DID_VID) {
+	if (vendor == TPM_TIS_I2C_DID_VID_9645) {
+		tpm_dev.chip_type = SLB9645;
+	} else if (vendor == TPM_TIS_I2C_DID_VID_9635) {
+		tpm_dev.chip_type = SLB9635;
+	} else {
+		dev_err(dev, "vendor id did not match! ID was %08x\n", vendor);
 		rc = -ENODEV;
 		goto out_release;
 	}
@@ -631,22 +694,53 @@ out_err:
 
 static const struct i2c_device_id tpm_tis_i2c_table[] = {
 	{"tpm_i2c_infineon", 0},
+	{"slb9635tt", 0},
+	{"slb9645tt", 1},
 	{},
 };
 
 MODULE_DEVICE_TABLE(i2c, tpm_tis_i2c_table);
+
+#ifdef CONFIG_OF
+static const struct of_device_id tpm_tis_i2c_of_match[] = {
+	{
+		.name = "tpm_i2c_infineon",
+		.type = "tpm",
+		.compatible = "infineon,tpm_i2c_infineon",
+		.data = (void *)0
+	},
+	{
+		.name = "slb9635tt",
+		.type = "tpm",
+		.compatible = "infineon,slb9635tt",
+		.data = (void *)0
+	},
+	{
+		.name = "slb9645tt",
+		.type = "tpm",
+		.compatible = "infineon,slb9645tt",
+		.data = (void *)1
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, tpm_tis_i2c_of_match);
+#endif
+
 static SIMPLE_DEV_PM_OPS(tpm_tis_i2c_ops, tpm_pm_suspend, tpm_pm_resume);
 
 static int tpm_tis_i2c_probe(struct i2c_client *client,
 			     const struct i2c_device_id *id)
 {
 	int rc;
-	if (tpm_dev.client != NULL)
+	struct device *dev = &(client->dev);
+
+	if (tpm_dev.client != NULL) {
+		dev_err(dev, "This driver only supports one client at a time\n");
 		return -EBUSY;	/* We only support one client */
+	}
 
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
-		dev_err(&client->dev,
-			"no algorithms associated to the i2c bus\n");
+		dev_err(dev, "no algorithms associated to the i2c bus\n");
 		return -ENODEV;
 	}
 
@@ -682,7 +776,6 @@ static int tpm_tis_i2c_remove(struct i2c_client *client)
 }
 
 static struct i2c_driver tpm_tis_i2c_driver = {
-
 	.id_table = tpm_tis_i2c_table,
 	.probe = tpm_tis_i2c_probe,
 	.remove = tpm_tis_i2c_remove,
@@ -690,11 +783,12 @@ static struct i2c_driver tpm_tis_i2c_driver = {
 		   .name = "tpm_i2c_infineon",
 		   .owner = THIS_MODULE,
 		   .pm = &tpm_tis_i2c_ops,
+		   .of_match_table = of_match_ptr(tpm_tis_i2c_of_match),
 		   },
 };
 
 module_i2c_driver(tpm_tis_i2c_driver);
 MODULE_AUTHOR("Peter Huewe <peter.huewe@infineon.com>");
 MODULE_DESCRIPTION("TPM TIS I2C Infineon Driver");
-MODULE_VERSION("2.1.5");
+MODULE_VERSION("2.2.0");
 MODULE_LICENSE("GPL");
diff --git a/drivers/char/tpm/tpm_i2c_stm_st33.c b/drivers/char/tpm/tpm_i2c_stm_st33.c
index 1f5f71e14abe..5bb8e2ddd3b3 100644
--- a/drivers/char/tpm/tpm_i2c_stm_st33.c
+++ b/drivers/char/tpm/tpm_i2c_stm_st33.c
@@ -36,7 +36,6 @@
 #include <linux/i2c.h>
 #include <linux/fs.h>
 #include <linux/miscdevice.h>
-#include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/delay.h>
 #include <linux/init.h>
@@ -50,7 +49,6 @@
 #include <linux/uaccess.h>
 #include <linux/io.h>
 #include <linux/slab.h>
-#include <linux/sched.h>
 
 #include "tpm.h"
 #include "tpm_i2c_stm_st33.h"
@@ -178,7 +176,7 @@ static long _wait_for_interrupt_serirq_timeout(struct tpm_chip *chip,
 	struct i2c_client *client;
 	struct st33zp24_platform_data *pin_infos;
 
-	client = (struct i2c_client *) TPM_VPRIV(chip);
+	client = (struct i2c_client *)TPM_VPRIV(chip);
 	pin_infos = client->dev.platform_data;
 
 	status = wait_for_completion_interruptible_timeout(
@@ -197,12 +195,12 @@ static int wait_for_serirq_timeout(struct tpm_chip *chip, bool condition,
 	int status = 2;
 	struct i2c_client *client;
 
-	client = (struct i2c_client *) TPM_VPRIV(chip);
+	client = (struct i2c_client *)TPM_VPRIV(chip);
 
 	status = _wait_for_interrupt_serirq_timeout(chip, timeout);
 	if (!status) {
 		status = -EBUSY;
-	} else{
+	} else {
 		clear_interruption(client);
 		if (condition)
 			status = 1;
@@ -219,7 +217,7 @@ static void tpm_stm_i2c_cancel(struct tpm_chip *chip)
 	struct i2c_client *client;
 	u8 data;
 
-	client = (struct i2c_client *) TPM_VPRIV(chip);
+	client = (struct i2c_client *)TPM_VPRIV(chip);
 
 	data = TPM_STS_COMMAND_READY;
 	I2C_WRITE_DATA(client, TPM_STS, &data, 1);
@@ -236,7 +234,7 @@ static u8 tpm_stm_i2c_status(struct tpm_chip *chip)
 {
 	struct i2c_client *client;
 	u8 data;
-	client = (struct i2c_client *) TPM_VPRIV(chip);
+	client = (struct i2c_client *)TPM_VPRIV(chip);
 
 	I2C_READ_DATA(client, TPM_STS, &data, 1);
 	return data;
@@ -254,7 +252,7 @@ static int check_locality(struct tpm_chip *chip)
 	u8 data;
 	u8 status;
 
-	client = (struct i2c_client *) TPM_VPRIV(chip);
+	client = (struct i2c_client *)TPM_VPRIV(chip);
 
 	status = I2C_READ_DATA(client, TPM_ACCESS, &data, 1);
 	if (status && (data &
@@ -278,7 +276,7 @@ static int request_locality(struct tpm_chip *chip)
 	struct i2c_client *client;
 	u8 data;
 
-	client = (struct i2c_client *) TPM_VPRIV(chip);
+	client = (struct i2c_client *)TPM_VPRIV(chip);
 
 	if (check_locality(chip) == chip->vendor.locality)
 		return chip->vendor.locality;
@@ -294,7 +292,7 @@ static int request_locality(struct tpm_chip *chip)
 						      chip->vendor.timeout_a);
 		if (rc > 0)
 			return chip->vendor.locality;
-	} else{
+	} else {
 		stop = jiffies + chip->vendor.timeout_a;
 		do {
 			if (check_locality(chip) >= 0)
@@ -316,7 +314,7 @@ static void release_locality(struct tpm_chip *chip)
 	struct i2c_client *client;
 	u8 data;
 
-	client = (struct i2c_client *) TPM_VPRIV(chip);
+	client = (struct i2c_client *)TPM_VPRIV(chip);
 	data = TPM_ACCESS_ACTIVE_LOCALITY;
 
 	I2C_WRITE_DATA(client, TPM_ACCESS, &data, 1);
@@ -333,7 +331,7 @@ static int get_burstcount(struct tpm_chip *chip)
 	int burstcnt, status;
 	u8 tpm_reg, temp;
 
-	struct i2c_client *client = (struct i2c_client *) TPM_VPRIV(chip);
+	struct i2c_client *client = (struct i2c_client *)TPM_VPRIV(chip);
 
 	stop = jiffies + chip->vendor.timeout_d;
 	do {
@@ -379,7 +377,7 @@ static int wait_for_stat(struct tpm_chip *chip, u8 mask, unsigned long timeout,
 						       mask), timeout);
 		if (rc > 0)
 			return 0;
-	} else{
+	} else {
 		stop = jiffies + timeout;
 		do {
 			msleep(TPM_TIMEOUT);
@@ -403,7 +401,7 @@ static int recv_data(struct tpm_chip *chip, u8 *buf, size_t count)
 	int size = 0, burstcnt, len;
 	struct i2c_client *client;
 
-	client = (struct i2c_client *) TPM_VPRIV(chip);
+	client = (struct i2c_client *)TPM_VPRIV(chip);
 
 	while (size < count &&
 	       wait_for_stat(chip,
@@ -433,7 +431,7 @@ static irqreturn_t tpm_ioserirq_handler(int irq, void *dev_id)
 
 	disable_irq_nosync(irq);
 
-	client = (struct i2c_client *) TPM_VPRIV(chip);
+	client = (struct i2c_client *)TPM_VPRIV(chip);
 	pin_infos = client->dev.platform_data;
 
 	complete(&pin_infos->irq_detection);
@@ -453,8 +451,7 @@ static irqreturn_t tpm_ioserirq_handler(int irq, void *dev_id)
 static int tpm_stm_i2c_send(struct tpm_chip *chip, unsigned char *buf,
 			    size_t len)
 {
-	u32 status,
-	    burstcnt = 0, i, size;
+	u32 status, burstcnt = 0, i, size;
 	int ret;
 	u8 data;
 	struct i2c_client *client;
@@ -483,7 +480,7 @@ static int tpm_stm_i2c_send(struct tpm_chip *chip, unsigned char *buf,
 		}
 	}
 
-	for (i = 0 ; i < len - 1 ;) {
+	for (i = 0; i < len - 1;) {
 		burstcnt = get_burstcount(chip);
 		size = min_t(int, len - i - 1, burstcnt);
 		ret = I2C_WRITE_DATA(client, TPM_DATA_FIFO, buf, size);
@@ -547,7 +544,7 @@ static int tpm_stm_i2c_recv(struct tpm_chip *chip, unsigned char *buf,
 		goto out;
 	}
 
-	expected = be32_to_cpu(*(__be32 *) (buf + 2));
+	expected = be32_to_cpu(*(__be32 *)(buf + 2));
 	if (expected > count) {
 		size = -EIO;
 		goto out;
@@ -569,7 +566,7 @@ out:
 
 static bool tpm_st33_i2c_req_canceled(struct tpm_chip *chip, u8 status)
 {
-       return (status == TPM_STS_COMMAND_READY);
+	return (status == TPM_STS_COMMAND_READY);
 }
 
 static const struct file_operations tpm_st33_i2c_fops = {
@@ -617,7 +614,7 @@ static struct tpm_vendor_specific st_i2c_tpm = {
 	.miscdev = {.fops = &tpm_st33_i2c_fops,},
 };
 
-static int interrupts ;
+static int interrupts;
 module_param(interrupts, int, 0444);
 MODULE_PARM_DESC(interrupts, "Enable interrupts");
 
@@ -714,7 +711,7 @@ tpm_st33_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
 				"TPM SERIRQ management", chip);
 		if (err < 0) {
 			dev_err(chip->dev , "TPM SERIRQ signals %d not available\n",
-			gpio_to_irq(platform_data->io_serirq));
+				gpio_to_irq(platform_data->io_serirq));
 			goto _irq_set;
 		}
 
@@ -754,7 +751,7 @@ tpm_st33_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	dev_info(chip->dev, "TPM I2C Initialized\n");
 	return 0;
 _irq_set:
-	free_irq(gpio_to_irq(platform_data->io_serirq), (void *) chip);
+	free_irq(gpio_to_irq(platform_data->io_serirq), (void *)chip);
 _gpio_init2:
 	if (interrupts)
 		gpio_free(platform_data->io_serirq);
@@ -784,7 +781,7 @@ static int tpm_st33_i2c_remove(struct i2c_client *client)
 {
 	struct tpm_chip *chip = (struct tpm_chip *)i2c_get_clientdata(client);
 	struct st33zp24_platform_data *pin_infos =
-		((struct i2c_client *) TPM_VPRIV(chip))->dev.platform_data;
+		((struct i2c_client *)TPM_VPRIV(chip))->dev.platform_data;
 
 	if (pin_infos != NULL) {
 		free_irq(pin_infos->io_serirq, chip);
@@ -823,9 +820,9 @@ static int tpm_st33_i2c_pm_suspend(struct device *dev)
 	struct st33zp24_platform_data *pin_infos = dev->platform_data;
 	int ret = 0;
 
-	if (power_mgt)
+	if (power_mgt) {
 		gpio_set_value(pin_infos->io_lpcpd, 0);
-	else{
+	} else {
 		if (chip->data_buffer == NULL)
 			chip->data_buffer = pin_infos->tpm_i2c_buffer[0];
 		ret = tpm_pm_suspend(dev);
@@ -851,12 +848,12 @@ static int tpm_st33_i2c_pm_resume(struct device *dev)
 					  (chip->vendor.status(chip) &
 					  TPM_STS_VALID) == TPM_STS_VALID,
 					  chip->vendor.timeout_b);
-	} else{
-	if (chip->data_buffer == NULL)
-		chip->data_buffer = pin_infos->tpm_i2c_buffer[0];
-	ret = tpm_pm_resume(dev);
-	if (!ret)
-		tpm_do_selftest(chip);
+	} else {
+		if (chip->data_buffer == NULL)
+			chip->data_buffer = pin_infos->tpm_i2c_buffer[0];
+		ret = tpm_pm_resume(dev);
+		if (!ret)
+			tpm_do_selftest(chip);
 	}
 	return ret;
 }				/* tpm_st33_i2c_pm_resume() */
@@ -867,7 +864,8 @@ static const struct i2c_device_id tpm_st33_i2c_id[] = {
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, tpm_st33_i2c_id);
-static SIMPLE_DEV_PM_OPS(tpm_st33_i2c_ops, tpm_st33_i2c_pm_suspend, tpm_st33_i2c_pm_resume);
+static SIMPLE_DEV_PM_OPS(tpm_st33_i2c_ops, tpm_st33_i2c_pm_suspend,
+	tpm_st33_i2c_pm_resume);
 static struct i2c_driver tpm_st33_i2c_driver = {
 	.driver = {
 		   .owner = THIS_MODULE,
diff --git a/drivers/char/tpm/tpm_ppi.c b/drivers/char/tpm/tpm_ppi.c
index 720ebcf29fdf..2168d15bc728 100644
--- a/drivers/char/tpm/tpm_ppi.c
+++ b/drivers/char/tpm/tpm_ppi.c
@@ -158,9 +158,9 @@ static ssize_t tpm_store_ppi_request(struct device *dev,
 					    ACPI_TYPE_STRING);
 	if (ACPI_FAILURE(status))
 		return -ENOMEM;
-	strncpy(version,
+	strlcpy(version,
 		((union acpi_object *)output.pointer)->string.pointer,
-		PPI_VERSION_LEN);
+		PPI_VERSION_LEN + 1);
 	kfree(output.pointer);
 	output.length = ACPI_ALLOCATE_BUFFER;
 	output.pointer = NULL;
@@ -237,9 +237,9 @@ static ssize_t tpm_show_ppi_transition_action(struct device *dev,
 					    ACPI_TYPE_STRING);
 	if (ACPI_FAILURE(status))
 		return -ENOMEM;
-	strncpy(version,
+	strlcpy(version,
 		((union acpi_object *)output.pointer)->string.pointer,
-		PPI_VERSION_LEN);
+		PPI_VERSION_LEN + 1);
 	/*
 	 * PPI spec defines params[3].type as empty package, but some platforms
 	 * (e.g. Capella with PPI 1.0) need integer/string/buffer type, so for
@@ -351,7 +351,7 @@ cleanup:
 static ssize_t show_ppi_operations(char *buf, u32 start, u32 end)
 {
 	char *str = buf;
-	char version[PPI_VERSION_LEN];
+	char version[PPI_VERSION_LEN + 1];
 	acpi_handle handle;
 	acpi_status status;
 	struct acpi_object_list input;
@@ -381,9 +381,9 @@ static ssize_t show_ppi_operations(char *buf, u32 start, u32 end)
 	if (ACPI_FAILURE(status))
 		return -ENOMEM;
 
-	strncpy(version,
+	strlcpy(version,
 		((union acpi_object *)output.pointer)->string.pointer,
-		PPI_VERSION_LEN);
+		PPI_VERSION_LEN + 1);
 	kfree(output.pointer);
 	output.length = ACPI_ALLOCATE_BUFFER;
 	output.pointer = NULL;
diff --git a/drivers/char/virtio_console.c b/drivers/char/virtio_console.c
index ce5f3fc25d6d..1b456fe9b87a 100644
--- a/drivers/char/virtio_console.c
+++ b/drivers/char/virtio_console.c
@@ -78,8 +78,8 @@ struct ports_driver_data {
 };
 static struct ports_driver_data pdrvdata;
 
-DEFINE_SPINLOCK(pdrvdata_lock);
-DECLARE_COMPLETION(early_console_added);
+static DEFINE_SPINLOCK(pdrvdata_lock);
+static DECLARE_COMPLETION(early_console_added);
 
 /* This struct holds information that's relevant only for console ports */
 struct console {
@@ -503,7 +503,7 @@ static int add_inbuf(struct virtqueue *vq, struct port_buffer *buf)
 
 	sg_init_one(sg, buf->buf, buf->size);
 
-	ret = virtqueue_add_buf(vq, sg, 0, 1, buf, GFP_ATOMIC);
+	ret = virtqueue_add_inbuf(vq, sg, 1, buf, GFP_ATOMIC);
 	virtqueue_kick(vq);
 	if (!ret)
 		ret = vq->num_free;
@@ -572,7 +572,7 @@ static ssize_t __send_control_msg(struct ports_device *portdev, u32 port_id,
 	sg_init_one(sg, &cpkt, sizeof(cpkt));
 
 	spin_lock(&portdev->c_ovq_lock);
-	if (virtqueue_add_buf(vq, sg, 1, 0, &cpkt, GFP_ATOMIC) == 0) {
+	if (virtqueue_add_outbuf(vq, sg, 1, &cpkt, GFP_ATOMIC) == 0) {
 		virtqueue_kick(vq);
 		while (!virtqueue_get_buf(vq, &len))
 			cpu_relax();
@@ -622,7 +622,7 @@ static ssize_t __send_to_port(struct port *port, struct scatterlist *sg,
 
 	reclaim_consumed_buffers(port);
 
-	err = virtqueue_add_buf(out_vq, sg, nents, 0, data, GFP_ATOMIC);
+	err = virtqueue_add_outbuf(out_vq, sg, nents, data, GFP_ATOMIC);
 
 	/* Tell Host to go! */
 	virtqueue_kick(out_vq);
@@ -1040,7 +1040,7 @@ static int port_fops_open(struct inode *inode, struct file *filp)
 	spin_lock_irq(&port->inbuf_lock);
 	if (port->guest_connected) {
 		spin_unlock_irq(&port->inbuf_lock);
-		ret = -EMFILE;
+		ret = -EBUSY;
 		goto out;
 	}
 
@@ -1202,7 +1202,7 @@ int __init virtio_cons_early_init(int (*put_chars)(u32, const char *, int))
 	return hvc_instantiate(0, 0, &hv_ops);
 }
 
-int init_port_console(struct port *port)
+static int init_port_console(struct port *port)
 {
 	int ret;
 
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index e7f7fe9b2f09..137d3e730f86 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -29,6 +29,7 @@ obj-$(CONFIG_ARCH_U8500)	+= ux500/
 obj-$(CONFIG_ARCH_VT8500)	+= clk-vt8500.o
 obj-$(CONFIG_ARCH_ZYNQ)		+= clk-zynq.o
 obj-$(CONFIG_ARCH_TEGRA)	+= tegra/
+obj-$(CONFIG_PLAT_SAMSUNG)	+= samsung/
 
 obj-$(CONFIG_X86)		+= x86/
 
diff --git a/drivers/clk/mmp/clk-mmp2.c b/drivers/clk/mmp/clk-mmp2.c
index ade435820c7e..d1f1a19d4351 100644
--- a/drivers/clk/mmp/clk-mmp2.c
+++ b/drivers/clk/mmp/clk-mmp2.c
@@ -221,7 +221,7 @@ void __init mmp2_clk_init(void)
 
 	clk = mmp_clk_register_apbc("gpio", "vctcxo",
 				apbc_base + APBC_GPIO, 10, 0, &clk_lock);
-	clk_register_clkdev(clk, NULL, "pxa-gpio");
+	clk_register_clkdev(clk, NULL, "mmp2-gpio");
 
 	clk = mmp_clk_register_apbc("kpc", "clk32",
 				apbc_base + APBC_KPC, 10, 0, &clk_lock);
diff --git a/drivers/clk/mmp/clk-pxa168.c b/drivers/clk/mmp/clk-pxa168.c
index e8d036c12cbf..28b3b51c794b 100644
--- a/drivers/clk/mmp/clk-pxa168.c
+++ b/drivers/clk/mmp/clk-pxa168.c
@@ -172,7 +172,7 @@ void __init pxa168_clk_init(void)
 
 	clk = mmp_clk_register_apbc("gpio", "vctcxo",
 				apbc_base + APBC_GPIO, 10, 0, &clk_lock);
-	clk_register_clkdev(clk, NULL, "pxa-gpio");
+	clk_register_clkdev(clk, NULL, "mmp-gpio");
 
 	clk = mmp_clk_register_apbc("kpc", "clk32",
 				apbc_base + APBC_KPC, 10, 0, &clk_lock);
diff --git a/drivers/clk/mmp/clk-pxa910.c b/drivers/clk/mmp/clk-pxa910.c
index 7048c31d6e7e..6ec05698ed38 100644
--- a/drivers/clk/mmp/clk-pxa910.c
+++ b/drivers/clk/mmp/clk-pxa910.c
@@ -177,7 +177,7 @@ void __init pxa910_clk_init(void)
 
 	clk = mmp_clk_register_apbc("gpio", "vctcxo",
 				apbc_base + APBC_GPIO, 10, 0, &clk_lock);
-	clk_register_clkdev(clk, NULL, "pxa-gpio");
+	clk_register_clkdev(clk, NULL, "mmp-gpio");
 
 	clk = mmp_clk_register_apbc("kpc", "clk32",
 				apbc_base + APBC_KPC, 10, 0, &clk_lock);
diff --git a/drivers/clk/mxs/clk-imx23.c b/drivers/clk/mxs/clk-imx23.c
index b5c06f9766f6..f6a74872f14e 100644
--- a/drivers/clk/mxs/clk-imx23.c
+++ b/drivers/clk/mxs/clk-imx23.c
@@ -15,12 +15,15 @@
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/of.h>
-#include <mach/common.h>
-#include <mach/mx23.h>
+#include <linux/of_address.h>
 #include "clk.h"
 
-#define DIGCTRL			MX23_IO_ADDRESS(MX23_DIGCTL_BASE_ADDR)
-#define CLKCTRL			MX23_IO_ADDRESS(MX23_CLKCTRL_BASE_ADDR)
+static void __iomem *clkctrl;
+static void __iomem *digctrl;
+
+#define CLKCTRL clkctrl
+#define DIGCTRL digctrl
+
 #define PLLCTRL0		(CLKCTRL + 0x0000)
 #define CPU			(CLKCTRL + 0x0020)
 #define HBUS			(CLKCTRL + 0x0030)
@@ -48,10 +51,10 @@ static void __init clk_misc_init(void)
 	u32 val;
 
 	/* Gate off cpu clock in WFI for power saving */
-	__mxs_setl(1 << BP_CPU_INTERRUPT_WAIT, CPU);
+	writel_relaxed(1 << BP_CPU_INTERRUPT_WAIT, CPU + SET);
 
 	/* Clear BYPASS for SAIF */
-	__mxs_clrl(1 << BP_CLKSEQ_BYPASS_SAIF, CLKSEQ);
+	writel_relaxed(1 << BP_CLKSEQ_BYPASS_SAIF, CLKSEQ + CLR);
 
 	/* SAIF has to use frac div for functional operation */
 	val = readl_relaxed(SAIF);
@@ -62,14 +65,14 @@ static void __init clk_misc_init(void)
 	 * Source ssp clock from ref_io than ref_xtal,
 	 * as ref_xtal only provides 24 MHz as maximum.
 	 */
-	__mxs_clrl(1 << BP_CLKSEQ_BYPASS_SSP, CLKSEQ);
+	writel_relaxed(1 << BP_CLKSEQ_BYPASS_SSP, CLKSEQ + CLR);
 
 	/*
 	 * 480 MHz seems too high to be ssp clock source directly,
 	 * so set frac to get a 288 MHz ref_io.
 	 */
-	__mxs_clrl(0x3f << BP_FRAC_IOFRAC, FRAC);
-	__mxs_setl(30 << BP_FRAC_IOFRAC, FRAC);
+	writel_relaxed(0x3f << BP_FRAC_IOFRAC, FRAC + CLR);
+	writel_relaxed(30 << BP_FRAC_IOFRAC, FRAC + SET);
 }
 
 static const char *sel_pll[]  __initconst = { "pll", "ref_xtal", };
@@ -101,6 +104,14 @@ int __init mx23_clocks_init(void)
 	struct device_node *np;
 	u32 i;
 
+	np = of_find_compatible_node(NULL, NULL, "fsl,imx23-digctl");
+	digctrl = of_iomap(np, 0);
+	WARN_ON(!digctrl);
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,imx23-clkctrl");
+	clkctrl = of_iomap(np, 0);
+	WARN_ON(!clkctrl);
+
 	clk_misc_init();
 
 	clks[ref_xtal] = mxs_clk_fixed("ref_xtal", 24000000);
@@ -153,19 +164,12 @@ int __init mx23_clocks_init(void)
 			return PTR_ERR(clks[i]);
 		}
 
-	np = of_find_compatible_node(NULL, NULL, "fsl,imx23-clkctrl");
-	if (np) {
-		clk_data.clks = clks;
-		clk_data.clk_num = ARRAY_SIZE(clks);
-		of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
-	}
-
-	clk_register_clkdev(clks[clk32k], NULL, "timrot");
+	clk_data.clks = clks;
+	clk_data.clk_num = ARRAY_SIZE(clks);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
 
 	for (i = 0; i < ARRAY_SIZE(clks_init_on); i++)
 		clk_prepare_enable(clks[clks_init_on[i]]);
 
-	mxs_timer_init();
-
 	return 0;
 }
diff --git a/drivers/clk/mxs/clk-imx28.c b/drivers/clk/mxs/clk-imx28.c
index 76ce6c6d1113..d0e5eed146de 100644
--- a/drivers/clk/mxs/clk-imx28.c
+++ b/drivers/clk/mxs/clk-imx28.c
@@ -15,11 +15,12 @@
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/of.h>
-#include <mach/common.h>
-#include <mach/mx28.h>
+#include <linux/of_address.h>
 #include "clk.h"
 
-#define CLKCTRL			MX28_IO_ADDRESS(MX28_CLKCTRL_BASE_ADDR)
+static void __iomem *clkctrl;
+#define CLKCTRL clkctrl
+
 #define PLL0CTRL0		(CLKCTRL + 0x0000)
 #define PLL1CTRL0		(CLKCTRL + 0x0020)
 #define PLL2CTRL0		(CLKCTRL + 0x0040)
@@ -53,7 +54,8 @@
 #define BP_FRAC0_IO1FRAC	16
 #define BP_FRAC0_IO0FRAC	24
 
-#define DIGCTRL			MX28_IO_ADDRESS(MX28_DIGCTL_BASE_ADDR)
+static void __iomem *digctrl;
+#define DIGCTRL digctrl
 #define BP_SAIF_CLKMUX		10
 
 /*
@@ -72,8 +74,8 @@ int mxs_saif_clkmux_select(unsigned int clkmux)
 	if (clkmux > 0x3)
 		return -EINVAL;
 
-	__mxs_clrl(0x3 << BP_SAIF_CLKMUX, DIGCTRL);
-	__mxs_setl(clkmux << BP_SAIF_CLKMUX, DIGCTRL);
+	writel_relaxed(0x3 << BP_SAIF_CLKMUX, DIGCTRL + CLR);
+	writel_relaxed(clkmux << BP_SAIF_CLKMUX, DIGCTRL + SET);
 
 	return 0;
 }
@@ -83,13 +85,13 @@ static void __init clk_misc_init(void)
 	u32 val;
 
 	/* Gate off cpu clock in WFI for power saving */
-	__mxs_setl(1 << BP_CPU_INTERRUPT_WAIT, CPU);
+	writel_relaxed(1 << BP_CPU_INTERRUPT_WAIT, CPU + SET);
 
 	/* 0 is a bad default value for a divider */
-	__mxs_setl(1 << BP_ENET_DIV_TIME, ENET);
+	writel_relaxed(1 << BP_ENET_DIV_TIME, ENET + SET);
 
 	/* Clear BYPASS for SAIF */
-	__mxs_clrl(0x3 << BP_CLKSEQ_BYPASS_SAIF0, CLKSEQ);
+	writel_relaxed(0x3 << BP_CLKSEQ_BYPASS_SAIF0, CLKSEQ + CLR);
 
 	/* SAIF has to use frac div for functional operation */
 	val = readl_relaxed(SAIF0);
@@ -109,7 +111,7 @@ static void __init clk_misc_init(void)
 	 * Source ssp clock from ref_io than ref_xtal,
 	 * as ref_xtal only provides 24 MHz as maximum.
 	 */
-	__mxs_clrl(0xf << BP_CLKSEQ_BYPASS_SSP0, CLKSEQ);
+	writel_relaxed(0xf << BP_CLKSEQ_BYPASS_SSP0, CLKSEQ + CLR);
 
 	/*
 	 * 480 MHz seems too high to be ssp clock source directly,
@@ -156,6 +158,14 @@ int __init mx28_clocks_init(void)
 	struct device_node *np;
 	u32 i;
 
+	np = of_find_compatible_node(NULL, NULL, "fsl,imx28-digctl");
+	digctrl = of_iomap(np, 0);
+	WARN_ON(!digctrl);
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,imx28-clkctrl");
+	clkctrl = of_iomap(np, 0);
+	WARN_ON(!clkctrl);
+
 	clk_misc_init();
 
 	clks[ref_xtal] = mxs_clk_fixed("ref_xtal", 24000000);
@@ -231,20 +241,14 @@ int __init mx28_clocks_init(void)
 			return PTR_ERR(clks[i]);
 		}
 
-	np = of_find_compatible_node(NULL, NULL, "fsl,imx28-clkctrl");
-	if (np) {
-		clk_data.clks = clks;
-		clk_data.clk_num = ARRAY_SIZE(clks);
-		of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
-	}
+	clk_data.clks = clks;
+	clk_data.clk_num = ARRAY_SIZE(clks);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
 
-	clk_register_clkdev(clks[xbus], NULL, "timrot");
 	clk_register_clkdev(clks[enet_out], NULL, "enet_out");
 
 	for (i = 0; i < ARRAY_SIZE(clks_init_on); i++)
 		clk_prepare_enable(clks[clks_init_on[i]]);
 
-	mxs_timer_init();
-
 	return 0;
 }
diff --git a/drivers/clk/samsung/Makefile b/drivers/clk/samsung/Makefile
new file mode 100644
index 000000000000..b7c232e67425
--- /dev/null
+++ b/drivers/clk/samsung/Makefile
@@ -0,0 +1,8 @@
+#
+# Samsung Clock specific Makefile
+#
+
+obj-$(CONFIG_COMMON_CLK)	+= clk.o clk-pll.o
+obj-$(CONFIG_ARCH_EXYNOS4)	+= clk-exynos4.o
+obj-$(CONFIG_SOC_EXYNOS5250)	+= clk-exynos5250.o
+obj-$(CONFIG_SOC_EXYNOS5440)	+= clk-exynos5440.o
diff --git a/drivers/clk/samsung/clk-exynos4.c b/drivers/clk/samsung/clk-exynos4.c
new file mode 100644
index 000000000000..d0940e69d034
--- /dev/null
+++ b/drivers/clk/samsung/clk-exynos4.c
@@ -0,0 +1,1082 @@
+/*
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ * Copyright (c) 2013 Linaro Ltd.
+ * Author: Thomas Abraham <thomas.ab@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.
+ *
+ * Common Clock Framework support for all Exynos4 SoCs.
+*/
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#include "clk.h"
+#include "clk-pll.h"
+
+/* Exynos4 clock controller register offsets */
+#define SRC_LEFTBUS		0x4200
+#define DIV_LEFTBUS		0x4500
+#define GATE_IP_LEFTBUS		0x4800
+#define E4X12_GATE_IP_IMAGE	0x4930
+#define SRC_RIGHTBUS		0x8200
+#define DIV_RIGHTBUS		0x8500
+#define GATE_IP_RIGHTBUS	0x8800
+#define E4X12_GATE_IP_PERIR	0x8960
+#define EPLL_LOCK		0xc010
+#define VPLL_LOCK		0xc020
+#define EPLL_CON0		0xc110
+#define EPLL_CON1		0xc114
+#define EPLL_CON2		0xc118
+#define VPLL_CON0		0xc120
+#define VPLL_CON1		0xc124
+#define VPLL_CON2		0xc128
+#define SRC_TOP0		0xc210
+#define SRC_TOP1		0xc214
+#define SRC_CAM			0xc220
+#define SRC_TV			0xc224
+#define SRC_MFC			0xcc28
+#define SRC_G3D			0xc22c
+#define E4210_SRC_IMAGE		0xc230
+#define SRC_LCD0		0xc234
+#define E4210_SRC_LCD1		0xc238
+#define E4X12_SRC_ISP		0xc238
+#define SRC_MAUDIO		0xc23c
+#define SRC_FSYS		0xc240
+#define SRC_PERIL0		0xc250
+#define SRC_PERIL1		0xc254
+#define E4X12_SRC_CAM1		0xc258
+#define SRC_MASK_TOP		0xc310
+#define SRC_MASK_CAM		0xc320
+#define SRC_MASK_TV		0xc324
+#define SRC_MASK_LCD0		0xc334
+#define E4210_SRC_MASK_LCD1	0xc338
+#define E4X12_SRC_MASK_ISP	0xc338
+#define SRC_MASK_MAUDIO		0xc33c
+#define SRC_MASK_FSYS		0xc340
+#define SRC_MASK_PERIL0		0xc350
+#define SRC_MASK_PERIL1		0xc354
+#define DIV_TOP			0xc510
+#define DIV_CAM			0xc520
+#define DIV_TV			0xc524
+#define DIV_MFC			0xc528
+#define DIV_G3D			0xc52c
+#define DIV_IMAGE		0xc530
+#define DIV_LCD0		0xc534
+#define E4210_DIV_LCD1		0xc538
+#define E4X12_DIV_ISP		0xc538
+#define DIV_MAUDIO		0xc53c
+#define DIV_FSYS0		0xc540
+#define DIV_FSYS1		0xc544
+#define DIV_FSYS2		0xc548
+#define DIV_FSYS3		0xc54c
+#define DIV_PERIL0		0xc550
+#define DIV_PERIL1		0xc554
+#define DIV_PERIL2		0xc558
+#define DIV_PERIL3		0xc55c
+#define DIV_PERIL4		0xc560
+#define DIV_PERIL5		0xc564
+#define E4X12_DIV_CAM1		0xc568
+#define GATE_SCLK_CAM		0xc820
+#define GATE_IP_CAM		0xc920
+#define GATE_IP_TV		0xc924
+#define GATE_IP_MFC		0xc928
+#define GATE_IP_G3D		0xc92c
+#define E4210_GATE_IP_IMAGE	0xc930
+#define GATE_IP_LCD0		0xc934
+#define E4210_GATE_IP_LCD1	0xc938
+#define E4X12_GATE_IP_ISP	0xc938
+#define E4X12_GATE_IP_MAUDIO	0xc93c
+#define GATE_IP_FSYS		0xc940
+#define GATE_IP_GPS		0xc94c
+#define GATE_IP_PERIL		0xc950
+#define E4210_GATE_IP_PERIR	0xc960
+#define GATE_BLOCK		0xc970
+#define E4X12_MPLL_CON0		0x10108
+#define SRC_DMC			0x10200
+#define SRC_MASK_DMC		0x10300
+#define DIV_DMC0		0x10500
+#define DIV_DMC1		0x10504
+#define GATE_IP_DMC		0x10900
+#define APLL_CON0		0x14100
+#define E4210_MPLL_CON0		0x14108
+#define SRC_CPU			0x14200
+#define DIV_CPU0		0x14500
+#define DIV_CPU1		0x14504
+#define GATE_SCLK_CPU		0x14800
+#define GATE_IP_CPU		0x14900
+#define E4X12_DIV_ISP0		0x18300
+#define E4X12_DIV_ISP1		0x18304
+#define E4X12_GATE_ISP0		0x18800
+#define E4X12_GATE_ISP1		0x18804
+
+/* the exynos4 soc type */
+enum exynos4_soc {
+	EXYNOS4210,
+	EXYNOS4X12,
+};
+
+/*
+ * Let each supported clock get a unique id. This id is used to lookup the clock
+ * for device tree based platforms. The clocks are categorized into three
+ * sections: core, sclk gate and bus interface gate clocks.
+ *
+ * When adding a new clock to this list, it is advised to choose a clock
+ * category and add it to the end of that category. That is because the the
+ * device tree source file is referring to these ids and any change in the
+ * sequence number of existing clocks will require corresponding change in the
+ * device tree files. This limitation would go away when pre-processor support
+ * for dtc would be available.
+ */
+enum exynos4_clks {
+	none,
+
+	/* core clocks */
+	xxti, xusbxti, fin_pll, fout_apll, fout_mpll, fout_epll, fout_vpll,
+	sclk_apll, sclk_mpll, sclk_epll, sclk_vpll, arm_clk, aclk200, aclk100,
+	aclk160, aclk133, mout_mpll_user_t, mout_mpll_user_c, mout_core,
+	mout_apll, /* 20 */
+
+	/* gate for special clocks (sclk) */
+	sclk_fimc0 = 128, sclk_fimc1, sclk_fimc2, sclk_fimc3, sclk_cam0,
+	sclk_cam1, sclk_csis0, sclk_csis1, sclk_hdmi, sclk_mixer, sclk_dac,
+	sclk_pixel, sclk_fimd0, sclk_mdnie0, sclk_mdnie_pwm0, sclk_mipi0,
+	sclk_audio0, sclk_mmc0, sclk_mmc1, sclk_mmc2, sclk_mmc3, sclk_mmc4,
+	sclk_sata, sclk_uart0, sclk_uart1, sclk_uart2, sclk_uart3, sclk_uart4,
+	sclk_audio1, sclk_audio2, sclk_spdif, sclk_spi0, sclk_spi1, sclk_spi2,
+	sclk_slimbus, sclk_fimd1, sclk_mipi1, sclk_pcm1, sclk_pcm2, sclk_i2s1,
+	sclk_i2s2, sclk_mipihsi, sclk_mfc, sclk_pcm0, sclk_g3d, sclk_pwm_isp,
+	sclk_spi0_isp, sclk_spi1_isp, sclk_uart_isp,
+
+	/* gate clocks */
+	fimc0 = 256, fimc1, fimc2, fimc3, csis0, csis1, jpeg, smmu_fimc0,
+	smmu_fimc1, smmu_fimc2, smmu_fimc3, smmu_jpeg, vp, mixer, tvenc, hdmi,
+	smmu_tv, mfc, smmu_mfcl, smmu_mfcr, g3d, g2d, rotator, mdma, smmu_g2d,
+	smmu_rotator, smmu_mdma, fimd0, mie0, mdnie0, dsim0, smmu_fimd0, fimd1,
+	mie1, dsim1, smmu_fimd1, pdma0, pdma1, pcie_phy, sata_phy, tsi, sdmmc0,
+	sdmmc1, sdmmc2, sdmmc3, sdmmc4, sata, sromc, usb_host, usb_device, pcie,
+	onenand, nfcon, smmu_pcie, gps, smmu_gps, uart0, uart1, uart2, uart3,
+	uart4, i2c0, i2c1, i2c2, i2c3, i2c4, i2c5, i2c6, i2c7, i2c_hdmi, tsadc,
+	spi0, spi1, spi2, i2s1, i2s2, pcm0, i2s0, pcm1, pcm2, pwm, slimbus,
+	spdif, ac97, modemif, chipid, sysreg, hdmi_cec, mct, wdt, rtc, keyif,
+	audss, mipi_hsi, mdma2, pixelasyncm0, pixelasyncm1, fimc_lite0,
+	fimc_lite1, ppmuispx, ppmuispmx, fimc_isp, fimc_drc, fimc_fd, mcuisp,
+	gicisp, smmu_isp, smmu_drc, smmu_fd, smmu_lite0, smmu_lite1, mcuctl_isp,
+	mpwm_isp, i2c0_isp, i2c1_isp, mtcadc_isp, pwm_isp, wdt_isp, uart_isp,
+	asyncaxim, smmu_ispcx, spi0_isp, spi1_isp, pwm_isp_sclk, spi0_isp_sclk,
+	spi1_isp_sclk, uart_isp_sclk,
+
+	/* mux clocks */
+	mout_fimc0 = 384, mout_fimc1, mout_fimc2, mout_fimc3, mout_cam0,
+	mout_cam1, mout_csis0, mout_csis1, mout_g3d0, mout_g3d1, mout_g3d,
+	aclk400_mcuisp,
+
+	/* div clocks */
+	div_isp0 = 450, div_isp1, div_mcuisp0, div_mcuisp1, div_aclk200,
+	div_aclk400_mcuisp,
+
+	nr_clks,
+};
+
+/*
+ * list of controller registers to be saved and restored during a
+ * suspend/resume cycle.
+ */
+static __initdata unsigned long exynos4210_clk_save[] = {
+	E4210_SRC_IMAGE,
+	E4210_SRC_LCD1,
+	E4210_SRC_MASK_LCD1,
+	E4210_DIV_LCD1,
+	E4210_GATE_IP_IMAGE,
+	E4210_GATE_IP_LCD1,
+	E4210_GATE_IP_PERIR,
+	E4210_MPLL_CON0,
+};
+
+static __initdata unsigned long exynos4x12_clk_save[] = {
+	E4X12_GATE_IP_IMAGE,
+	E4X12_GATE_IP_PERIR,
+	E4X12_SRC_CAM1,
+	E4X12_DIV_ISP,
+	E4X12_DIV_CAM1,
+	E4X12_MPLL_CON0,
+};
+
+static __initdata unsigned long exynos4_clk_regs[] = {
+	SRC_LEFTBUS,
+	DIV_LEFTBUS,
+	GATE_IP_LEFTBUS,
+	SRC_RIGHTBUS,
+	DIV_RIGHTBUS,
+	GATE_IP_RIGHTBUS,
+	EPLL_CON0,
+	EPLL_CON1,
+	EPLL_CON2,
+	VPLL_CON0,
+	VPLL_CON1,
+	VPLL_CON2,
+	SRC_TOP0,
+	SRC_TOP1,
+	SRC_CAM,
+	SRC_TV,
+	SRC_MFC,
+	SRC_G3D,
+	SRC_LCD0,
+	SRC_MAUDIO,
+	SRC_FSYS,
+	SRC_PERIL0,
+	SRC_PERIL1,
+	SRC_MASK_TOP,
+	SRC_MASK_CAM,
+	SRC_MASK_TV,
+	SRC_MASK_LCD0,
+	SRC_MASK_MAUDIO,
+	SRC_MASK_FSYS,
+	SRC_MASK_PERIL0,
+	SRC_MASK_PERIL1,
+	DIV_TOP,
+	DIV_CAM,
+	DIV_TV,
+	DIV_MFC,
+	DIV_G3D,
+	DIV_IMAGE,
+	DIV_LCD0,
+	DIV_MAUDIO,
+	DIV_FSYS0,
+	DIV_FSYS1,
+	DIV_FSYS2,
+	DIV_FSYS3,
+	DIV_PERIL0,
+	DIV_PERIL1,
+	DIV_PERIL2,
+	DIV_PERIL3,
+	DIV_PERIL4,
+	DIV_PERIL5,
+	GATE_SCLK_CAM,
+	GATE_IP_CAM,
+	GATE_IP_TV,
+	GATE_IP_MFC,
+	GATE_IP_G3D,
+	GATE_IP_LCD0,
+	GATE_IP_FSYS,
+	GATE_IP_GPS,
+	GATE_IP_PERIL,
+	GATE_BLOCK,
+	SRC_MASK_DMC,
+	SRC_DMC,
+	DIV_DMC0,
+	DIV_DMC1,
+	GATE_IP_DMC,
+	APLL_CON0,
+	SRC_CPU,
+	DIV_CPU0,
+	DIV_CPU1,
+	GATE_SCLK_CPU,
+	GATE_IP_CPU,
+};
+
+/* list of all parent clock list */
+PNAME(mout_apll_p)	= { "fin_pll", "fout_apll", };
+PNAME(mout_mpll_p)	= { "fin_pll", "fout_mpll", };
+PNAME(mout_epll_p)	= { "fin_pll", "fout_epll", };
+PNAME(mout_vpllsrc_p)	= { "fin_pll", "sclk_hdmi24m", };
+PNAME(mout_vpll_p)	= { "fin_pll", "fout_vpll", };
+PNAME(sclk_evpll_p)	= { "sclk_epll", "sclk_vpll", };
+PNAME(mout_mfc_p)	= { "mout_mfc0", "mout_mfc1", };
+PNAME(mout_g3d_p)	= { "mout_g3d0", "mout_g3d1", };
+PNAME(mout_g2d_p)	= { "mout_g2d0", "mout_g2d1", };
+PNAME(mout_hdmi_p)	= { "sclk_pixel", "sclk_hdmiphy", };
+PNAME(mout_jpeg_p)	= { "mout_jpeg0", "mout_jpeg1", };
+PNAME(mout_spdif_p)	= { "sclk_audio0", "sclk_audio1", "sclk_audio2",
+				"spdif_extclk", };
+PNAME(mout_onenand_p)  = {"aclk133", "aclk160", };
+PNAME(mout_onenand1_p) = {"mout_onenand", "sclk_vpll", };
+
+/* Exynos 4210-specific parent groups */
+PNAME(sclk_vpll_p4210)	= { "mout_vpllsrc", "fout_vpll", };
+PNAME(mout_core_p4210)	= { "mout_apll", "sclk_mpll", };
+PNAME(sclk_ampll_p4210)	= { "sclk_mpll", "sclk_apll", };
+PNAME(group1_p4210)	= { "xxti", "xusbxti", "sclk_hdmi24m",
+				"sclk_usbphy0", "none",	"sclk_hdmiphy",
+				"sclk_mpll", "sclk_epll", "sclk_vpll", };
+PNAME(mout_audio0_p4210) = { "cdclk0", "none", "sclk_hdmi24m",
+				"sclk_usbphy0", "xxti", "xusbxti", "sclk_mpll",
+				"sclk_epll", "sclk_vpll" };
+PNAME(mout_audio1_p4210) = { "cdclk1", "none", "sclk_hdmi24m",
+				"sclk_usbphy0", "xxti", "xusbxti", "sclk_mpll",
+				"sclk_epll", "sclk_vpll", };
+PNAME(mout_audio2_p4210) = { "cdclk2", "none", "sclk_hdmi24m",
+				"sclk_usbphy0", "xxti", "xusbxti", "sclk_mpll",
+				"sclk_epll", "sclk_vpll", };
+PNAME(mout_mixer_p4210)	= { "sclk_dac", "sclk_hdmi", };
+PNAME(mout_dac_p4210)	= { "sclk_vpll", "sclk_hdmiphy", };
+
+/* Exynos 4x12-specific parent groups */
+PNAME(mout_mpll_user_p4x12) = { "fin_pll", "sclk_mpll", };
+PNAME(mout_core_p4x12)	= { "mout_apll", "mout_mpll_user_c", };
+PNAME(sclk_ampll_p4x12)	= { "mout_mpll_user_t", "sclk_apll", };
+PNAME(group1_p4x12)	= { "xxti", "xusbxti", "sclk_hdmi24m", "sclk_usbphy0",
+				"none",	"sclk_hdmiphy", "mout_mpll_user_t",
+				"sclk_epll", "sclk_vpll", };
+PNAME(mout_audio0_p4x12) = { "cdclk0", "none", "sclk_hdmi24m",
+				"sclk_usbphy0", "xxti", "xusbxti",
+				"mout_mpll_user_t", "sclk_epll", "sclk_vpll" };
+PNAME(mout_audio1_p4x12) = { "cdclk1", "none", "sclk_hdmi24m",
+				"sclk_usbphy0", "xxti", "xusbxti",
+				"mout_mpll_user_t", "sclk_epll", "sclk_vpll", };
+PNAME(mout_audio2_p4x12) = { "cdclk2", "none", "sclk_hdmi24m",
+				"sclk_usbphy0", "xxti", "xusbxti",
+				"mout_mpll_user_t", "sclk_epll", "sclk_vpll", };
+PNAME(aclk_p4412)	= { "mout_mpll_user_t", "sclk_apll", };
+PNAME(mout_user_aclk400_mcuisp_p4x12) = {"fin_pll", "div_aclk400_mcuisp", };
+PNAME(mout_user_aclk200_p4x12) = {"fin_pll", "div_aclk200", };
+PNAME(mout_user_aclk266_gps_p4x12) = {"fin_pll", "div_aclk266_gps", };
+
+/* fixed rate clocks generated outside the soc */
+struct samsung_fixed_rate_clock exynos4_fixed_rate_ext_clks[] __initdata = {
+	FRATE(xxti, "xxti", NULL, CLK_IS_ROOT, 0),
+	FRATE(xusbxti, "xusbxti", NULL, CLK_IS_ROOT, 0),
+};
+
+/* fixed rate clocks generated inside the soc */
+struct samsung_fixed_rate_clock exynos4_fixed_rate_clks[] __initdata = {
+	FRATE(none, "sclk_hdmi24m", NULL, CLK_IS_ROOT, 24000000),
+	FRATE(none, "sclk_hdmiphy", NULL, CLK_IS_ROOT, 27000000),
+	FRATE(none, "sclk_usbphy0", NULL, CLK_IS_ROOT, 48000000),
+};
+
+struct samsung_fixed_rate_clock exynos4210_fixed_rate_clks[] __initdata = {
+	FRATE(none, "sclk_usbphy1", NULL, CLK_IS_ROOT, 48000000),
+};
+
+/* list of mux clocks supported in all exynos4 soc's */
+struct samsung_mux_clock exynos4_mux_clks[] __initdata = {
+	MUX_F(mout_apll, "mout_apll", mout_apll_p, SRC_CPU, 0, 1,
+			CLK_SET_RATE_PARENT, 0),
+	MUX(none, "mout_hdmi", mout_hdmi_p, SRC_TV, 0, 1),
+	MUX(none, "mout_mfc1", sclk_evpll_p, SRC_MFC, 4, 1),
+	MUX(none, "mout_mfc", mout_mfc_p, SRC_MFC, 8, 1),
+	MUX_F(mout_g3d1, "mout_g3d1", sclk_evpll_p, SRC_G3D, 4, 1,
+			CLK_SET_RATE_PARENT, 0),
+	MUX_F(mout_g3d, "mout_g3d", mout_g3d_p, SRC_G3D, 8, 1,
+			CLK_SET_RATE_PARENT, 0),
+	MUX(none, "mout_spdif", mout_spdif_p, SRC_PERIL1, 8, 2),
+	MUX(none, "mout_onenand1", mout_onenand1_p, SRC_TOP0, 0, 1),
+	MUX_A(sclk_epll, "sclk_epll", mout_epll_p, SRC_TOP0, 4, 1, "sclk_epll"),
+	MUX(none, "mout_onenand", mout_onenand_p, SRC_TOP0, 28, 1),
+};
+
+/* list of mux clocks supported in exynos4210 soc */
+struct samsung_mux_clock exynos4210_mux_clks[] __initdata = {
+	MUX(none, "mout_aclk200", sclk_ampll_p4210, SRC_TOP0, 12, 1),
+	MUX(none, "mout_aclk100", sclk_ampll_p4210, SRC_TOP0, 16, 1),
+	MUX(none, "mout_aclk160", sclk_ampll_p4210, SRC_TOP0, 20, 1),
+	MUX(none, "mout_aclk133", sclk_ampll_p4210, SRC_TOP0, 24, 1),
+	MUX(none, "mout_vpllsrc", mout_vpllsrc_p, SRC_TOP1, 0, 1),
+	MUX(none, "mout_mixer", mout_mixer_p4210, SRC_TV, 4, 1),
+	MUX(none, "mout_dac", mout_dac_p4210, SRC_TV, 8, 1),
+	MUX(none, "mout_g2d0", sclk_ampll_p4210, E4210_SRC_IMAGE, 0, 1),
+	MUX(none, "mout_g2d1", sclk_evpll_p, E4210_SRC_IMAGE, 4, 1),
+	MUX(none, "mout_g2d", mout_g2d_p, E4210_SRC_IMAGE, 8, 1),
+	MUX(none, "mout_fimd1", group1_p4210, E4210_SRC_LCD1, 0, 4),
+	MUX(none, "mout_mipi1", group1_p4210, E4210_SRC_LCD1, 12, 4),
+	MUX_A(sclk_mpll, "sclk_mpll", mout_mpll_p, SRC_CPU, 8, 1, "sclk_mpll"),
+	MUX_A(mout_core, "mout_core", mout_core_p4210,
+			SRC_CPU, 16, 1, "mout_core"),
+	MUX_A(sclk_vpll, "sclk_vpll", sclk_vpll_p4210,
+			SRC_TOP0, 8, 1, "sclk_vpll"),
+	MUX(mout_fimc0, "mout_fimc0", group1_p4210, SRC_CAM, 0, 4),
+	MUX(mout_fimc1, "mout_fimc1", group1_p4210, SRC_CAM, 4, 4),
+	MUX(mout_fimc2, "mout_fimc2", group1_p4210, SRC_CAM, 8, 4),
+	MUX(mout_fimc3, "mout_fimc3", group1_p4210, SRC_CAM, 12, 4),
+	MUX(mout_cam0, "mout_cam0", group1_p4210, SRC_CAM, 16, 4),
+	MUX(mout_cam1, "mout_cam1", group1_p4210, SRC_CAM, 20, 4),
+	MUX(mout_csis0, "mout_csis0", group1_p4210, SRC_CAM, 24, 4),
+	MUX(mout_csis1, "mout_csis1", group1_p4210, SRC_CAM, 28, 4),
+	MUX(none, "mout_mfc0", sclk_ampll_p4210, SRC_MFC, 0, 1),
+	MUX_F(mout_g3d0, "mout_g3d0", sclk_ampll_p4210, SRC_G3D, 0, 1,
+			CLK_SET_RATE_PARENT, 0),
+	MUX(none, "mout_fimd0", group1_p4210, SRC_LCD0, 0, 4),
+	MUX(none, "mout_mipi0", group1_p4210, SRC_LCD0, 12, 4),
+	MUX(none, "mout_audio0", mout_audio0_p4210, SRC_MAUDIO, 0, 4),
+	MUX(none, "mout_mmc0", group1_p4210, SRC_FSYS, 0, 4),
+	MUX(none, "mout_mmc1", group1_p4210, SRC_FSYS, 4, 4),
+	MUX(none, "mout_mmc2", group1_p4210, SRC_FSYS, 8, 4),
+	MUX(none, "mout_mmc3", group1_p4210, SRC_FSYS, 12, 4),
+	MUX(none, "mout_mmc4", group1_p4210, SRC_FSYS, 16, 4),
+	MUX(none, "mout_sata", sclk_ampll_p4210, SRC_FSYS, 24, 1),
+	MUX(none, "mout_uart0", group1_p4210, SRC_PERIL0, 0, 4),
+	MUX(none, "mout_uart1", group1_p4210, SRC_PERIL0, 4, 4),
+	MUX(none, "mout_uart2", group1_p4210, SRC_PERIL0, 8, 4),
+	MUX(none, "mout_uart3", group1_p4210, SRC_PERIL0, 12, 4),
+	MUX(none, "mout_uart4", group1_p4210, SRC_PERIL0, 16, 4),
+	MUX(none, "mout_audio1", mout_audio1_p4210, SRC_PERIL1, 0, 4),
+	MUX(none, "mout_audio2", mout_audio2_p4210, SRC_PERIL1, 4, 4),
+	MUX(none, "mout_spi0", group1_p4210, SRC_PERIL1, 16, 4),
+	MUX(none, "mout_spi1", group1_p4210, SRC_PERIL1, 20, 4),
+	MUX(none, "mout_spi2", group1_p4210, SRC_PERIL1, 24, 4),
+};
+
+/* list of mux clocks supported in exynos4x12 soc */
+struct samsung_mux_clock exynos4x12_mux_clks[] __initdata = {
+	MUX(mout_mpll_user_c, "mout_mpll_user_c", mout_mpll_user_p4x12,
+			SRC_CPU, 24, 1),
+	MUX(none, "mout_aclk266_gps", aclk_p4412, SRC_TOP1, 4, 1),
+	MUX(none, "mout_aclk400_mcuisp", aclk_p4412, SRC_TOP1, 8, 1),
+	MUX(mout_mpll_user_t, "mout_mpll_user_t", mout_mpll_user_p4x12,
+			SRC_TOP1, 12, 1),
+	MUX(none, "mout_user_aclk266_gps", mout_user_aclk266_gps_p4x12,
+			SRC_TOP1, 16, 1),
+	MUX(aclk200, "aclk200", mout_user_aclk200_p4x12, SRC_TOP1, 20, 1),
+	MUX(aclk400_mcuisp, "aclk400_mcuisp", mout_user_aclk400_mcuisp_p4x12,
+			SRC_TOP1, 24, 1),
+	MUX(none, "mout_aclk200", aclk_p4412, SRC_TOP0, 12, 1),
+	MUX(none, "mout_aclk100", aclk_p4412, SRC_TOP0, 16, 1),
+	MUX(none, "mout_aclk160", aclk_p4412, SRC_TOP0, 20, 1),
+	MUX(none, "mout_aclk133", aclk_p4412, SRC_TOP0, 24, 1),
+	MUX(none, "mout_mdnie0", group1_p4x12, SRC_LCD0, 4, 4),
+	MUX(none, "mout_mdnie_pwm0", group1_p4x12, SRC_LCD0, 8, 4),
+	MUX(none, "mout_sata", sclk_ampll_p4x12, SRC_FSYS, 24, 1),
+	MUX(none, "mout_jpeg0", sclk_ampll_p4x12, E4X12_SRC_CAM1, 0, 1),
+	MUX(none, "mout_jpeg1", sclk_evpll_p, E4X12_SRC_CAM1, 4, 1),
+	MUX(none, "mout_jpeg", mout_jpeg_p, E4X12_SRC_CAM1, 8, 1),
+	MUX_A(sclk_mpll, "sclk_mpll", mout_mpll_p,
+			SRC_DMC, 12, 1, "sclk_mpll"),
+	MUX_A(sclk_vpll, "sclk_vpll", mout_vpll_p,
+			SRC_TOP0, 8, 1, "sclk_vpll"),
+	MUX(mout_core, "mout_core", mout_core_p4x12, SRC_CPU, 16, 1),
+	MUX(mout_fimc0, "mout_fimc0", group1_p4x12, SRC_CAM, 0, 4),
+	MUX(mout_fimc1, "mout_fimc1", group1_p4x12, SRC_CAM, 4, 4),
+	MUX(mout_fimc2, "mout_fimc2", group1_p4x12, SRC_CAM, 8, 4),
+	MUX(mout_fimc3, "mout_fimc3", group1_p4x12, SRC_CAM, 12, 4),
+	MUX(mout_cam0, "mout_cam0", group1_p4x12, SRC_CAM, 16, 4),
+	MUX(mout_cam1, "mout_cam1", group1_p4x12, SRC_CAM, 20, 4),
+	MUX(mout_csis0, "mout_csis0", group1_p4x12, SRC_CAM, 24, 4),
+	MUX(mout_csis1, "mout_csis1", group1_p4x12, SRC_CAM, 28, 4),
+	MUX(none, "mout_mfc0", sclk_ampll_p4x12, SRC_MFC, 0, 1),
+	MUX_F(mout_g3d0, "mout_g3d0", sclk_ampll_p4x12, SRC_G3D, 0, 1,
+			CLK_SET_RATE_PARENT, 0),
+	MUX(none, "mout_fimd0", group1_p4x12, SRC_LCD0, 0, 4),
+	MUX(none, "mout_mipi0", group1_p4x12, SRC_LCD0, 12, 4),
+	MUX(none, "mout_audio0", mout_audio0_p4x12, SRC_MAUDIO, 0, 4),
+	MUX(none, "mout_mmc0", group1_p4x12, SRC_FSYS, 0, 4),
+	MUX(none, "mout_mmc1", group1_p4x12, SRC_FSYS, 4, 4),
+	MUX(none, "mout_mmc2", group1_p4x12, SRC_FSYS, 8, 4),
+	MUX(none, "mout_mmc3", group1_p4x12, SRC_FSYS, 12, 4),
+	MUX(none, "mout_mmc4", group1_p4x12, SRC_FSYS, 16, 4),
+	MUX(none, "mout_mipihsi", aclk_p4412, SRC_FSYS, 24, 1),
+	MUX(none, "mout_uart0", group1_p4x12, SRC_PERIL0, 0, 4),
+	MUX(none, "mout_uart1", group1_p4x12, SRC_PERIL0, 4, 4),
+	MUX(none, "mout_uart2", group1_p4x12, SRC_PERIL0, 8, 4),
+	MUX(none, "mout_uart3", group1_p4x12, SRC_PERIL0, 12, 4),
+	MUX(none, "mout_uart4", group1_p4x12, SRC_PERIL0, 16, 4),
+	MUX(none, "mout_audio1", mout_audio1_p4x12, SRC_PERIL1, 0, 4),
+	MUX(none, "mout_audio2", mout_audio2_p4x12, SRC_PERIL1, 4, 4),
+	MUX(none, "mout_spi0", group1_p4x12, SRC_PERIL1, 16, 4),
+	MUX(none, "mout_spi1", group1_p4x12, SRC_PERIL1, 20, 4),
+	MUX(none, "mout_spi2", group1_p4x12, SRC_PERIL1, 24, 4),
+	MUX(none, "mout_pwm_isp", group1_p4x12, E4X12_SRC_ISP, 0, 4),
+	MUX(none, "mout_spi0_isp", group1_p4x12, E4X12_SRC_ISP, 4, 4),
+	MUX(none, "mout_spi1_isp", group1_p4x12, E4X12_SRC_ISP, 8, 4),
+	MUX(none, "mout_uart_isp", group1_p4x12, E4X12_SRC_ISP, 12, 4),
+};
+
+/* list of divider clocks supported in all exynos4 soc's */
+struct samsung_div_clock exynos4_div_clks[] __initdata = {
+	DIV(none, "div_core", "mout_core", DIV_CPU0, 0, 3),
+	DIV(none, "div_core2", "div_core", DIV_CPU0, 28, 3),
+	DIV(none, "div_fimc0", "mout_fimc0", DIV_CAM, 0, 4),
+	DIV(none, "div_fimc1", "mout_fimc1", DIV_CAM, 4, 4),
+	DIV(none, "div_fimc2", "mout_fimc2", DIV_CAM, 8, 4),
+	DIV(none, "div_fimc3", "mout_fimc3", DIV_CAM, 12, 4),
+	DIV(none, "div_cam0", "mout_cam0", DIV_CAM, 16, 4),
+	DIV(none, "div_cam1", "mout_cam1", DIV_CAM, 20, 4),
+	DIV(none, "div_csis0", "mout_csis0", DIV_CAM, 24, 4),
+	DIV(none, "div_csis1", "mout_csis1", DIV_CAM, 28, 4),
+	DIV(sclk_mfc, "sclk_mfc", "mout_mfc", DIV_MFC, 0, 4),
+	DIV_F(none, "div_g3d", "mout_g3d", DIV_G3D, 0, 4,
+			CLK_SET_RATE_PARENT, 0),
+	DIV(none, "div_fimd0", "mout_fimd0", DIV_LCD0, 0, 4),
+	DIV(none, "div_mipi0", "mout_mipi0", DIV_LCD0, 16, 4),
+	DIV(none, "div_audio0", "mout_audio0", DIV_MAUDIO, 0, 4),
+	DIV(sclk_pcm0, "sclk_pcm0", "sclk_audio0", DIV_MAUDIO, 4, 8),
+	DIV(none, "div_mmc0", "mout_mmc0", DIV_FSYS1, 0, 4),
+	DIV(none, "div_mmc1", "mout_mmc1", DIV_FSYS1, 16, 4),
+	DIV(none, "div_mmc2", "mout_mmc2", DIV_FSYS2, 0, 4),
+	DIV(none, "div_mmc3", "mout_mmc3", DIV_FSYS2, 16, 4),
+	DIV(sclk_pixel, "sclk_pixel", "sclk_vpll", DIV_TV, 0, 4),
+	DIV(aclk100, "aclk100", "mout_aclk100", DIV_TOP, 4, 4),
+	DIV(aclk160, "aclk160", "mout_aclk160", DIV_TOP, 8, 3),
+	DIV(aclk133, "aclk133", "mout_aclk133", DIV_TOP, 12, 3),
+	DIV(none, "div_onenand", "mout_onenand1", DIV_TOP, 16, 3),
+	DIV(sclk_slimbus, "sclk_slimbus", "sclk_epll", DIV_PERIL3, 4, 4),
+	DIV(sclk_pcm1, "sclk_pcm1", "sclk_audio1", DIV_PERIL4, 4, 8),
+	DIV(sclk_pcm2, "sclk_pcm2", "sclk_audio2", DIV_PERIL4, 20, 8),
+	DIV(sclk_i2s1, "sclk_i2s1", "sclk_audio1", DIV_PERIL5, 0, 6),
+	DIV(sclk_i2s2, "sclk_i2s2", "sclk_audio2", DIV_PERIL5, 8, 6),
+	DIV(none, "div_mmc4", "mout_mmc4", DIV_FSYS3, 0, 4),
+	DIV(none, "div_mmc_pre4", "div_mmc4", DIV_FSYS3, 8, 8),
+	DIV(none, "div_uart0", "mout_uart0", DIV_PERIL0, 0, 4),
+	DIV(none, "div_uart1", "mout_uart1", DIV_PERIL0, 4, 4),
+	DIV(none, "div_uart2", "mout_uart2", DIV_PERIL0, 8, 4),
+	DIV(none, "div_uart3", "mout_uart3", DIV_PERIL0, 12, 4),
+	DIV(none, "div_uart4", "mout_uart4", DIV_PERIL0, 16, 4),
+	DIV(none, "div_spi0", "mout_spi0", DIV_PERIL1, 0, 4),
+	DIV(none, "div_spi_pre0", "div_spi0", DIV_PERIL1, 8, 8),
+	DIV(none, "div_spi1", "mout_spi1", DIV_PERIL1, 16, 4),
+	DIV(none, "div_spi_pre1", "div_spi1", DIV_PERIL1, 24, 8),
+	DIV(none, "div_spi2", "mout_spi2", DIV_PERIL2, 0, 4),
+	DIV(none, "div_spi_pre2", "div_spi2", DIV_PERIL2, 8, 8),
+	DIV(none, "div_audio1", "mout_audio1", DIV_PERIL4, 0, 4),
+	DIV(none, "div_audio2", "mout_audio2", DIV_PERIL4, 16, 4),
+	DIV_A(arm_clk, "arm_clk", "div_core2", DIV_CPU0, 28, 3, "arm_clk"),
+	DIV_A(sclk_apll, "sclk_apll", "mout_apll",
+			DIV_CPU0, 24, 3, "sclk_apll"),
+	DIV_F(none, "div_mipi_pre0", "div_mipi0", DIV_LCD0, 20, 4,
+			CLK_SET_RATE_PARENT, 0),
+	DIV_F(none, "div_mmc_pre0", "div_mmc0", DIV_FSYS1, 8, 8,
+			CLK_SET_RATE_PARENT, 0),
+	DIV_F(none, "div_mmc_pre1", "div_mmc1", DIV_FSYS1, 24, 8,
+			CLK_SET_RATE_PARENT, 0),
+	DIV_F(none, "div_mmc_pre2", "div_mmc2", DIV_FSYS2, 8, 8,
+			CLK_SET_RATE_PARENT, 0),
+	DIV_F(none, "div_mmc_pre3", "div_mmc3", DIV_FSYS2, 24, 8,
+			CLK_SET_RATE_PARENT, 0),
+};
+
+/* list of divider clocks supported in exynos4210 soc */
+struct samsung_div_clock exynos4210_div_clks[] __initdata = {
+	DIV(aclk200, "aclk200", "mout_aclk200", DIV_TOP, 0, 3),
+	DIV(none, "div_g2d", "mout_g2d", DIV_IMAGE, 0, 4),
+	DIV(none, "div_fimd1", "mout_fimd1", E4210_DIV_LCD1, 0, 4),
+	DIV(none, "div_mipi1", "mout_mipi1", E4210_DIV_LCD1, 16, 4),
+	DIV(none, "div_sata", "mout_sata", DIV_FSYS0, 20, 4),
+	DIV_F(none, "div_mipi_pre1", "div_mipi1", E4210_DIV_LCD1, 20, 4,
+			CLK_SET_RATE_PARENT, 0),
+};
+
+/* list of divider clocks supported in exynos4x12 soc */
+struct samsung_div_clock exynos4x12_div_clks[] __initdata = {
+	DIV(none, "div_mdnie0", "mout_mdnie0", DIV_LCD0, 4, 4),
+	DIV(none, "div_mdnie_pwm0", "mout_mdnie_pwm0", DIV_LCD0, 8, 4),
+	DIV(none, "div_mdnie_pwm_pre0", "div_mdnie_pwm0", DIV_LCD0, 12, 4),
+	DIV(none, "div_mipihsi", "mout_mipihsi", DIV_FSYS0, 20, 4),
+	DIV(none, "div_jpeg", "mout_jpeg", E4X12_DIV_CAM1, 0, 4),
+	DIV(div_aclk200, "div_aclk200", "mout_aclk200", DIV_TOP, 0, 3),
+	DIV(none, "div_aclk266_gps", "mout_aclk266_gps", DIV_TOP, 20, 3),
+	DIV(div_aclk400_mcuisp, "div_aclk400_mcuisp", "mout_aclk400_mcuisp",
+						DIV_TOP, 24, 3),
+	DIV(none, "div_pwm_isp", "mout_pwm_isp", E4X12_DIV_ISP, 0, 4),
+	DIV(none, "div_spi0_isp", "mout_spi0_isp", E4X12_DIV_ISP, 4, 4),
+	DIV(none, "div_spi0_isp_pre", "div_spi0_isp", E4X12_DIV_ISP, 8, 8),
+	DIV(none, "div_spi1_isp", "mout_spi1_isp", E4X12_DIV_ISP, 16, 4),
+	DIV(none, "div_spi1_isp_pre", "div_spi1_isp", E4X12_DIV_ISP, 20, 8),
+	DIV(none, "div_uart_isp", "mout_uart_isp", E4X12_DIV_ISP, 28, 4),
+	DIV(div_isp0, "div_isp0", "aclk200", E4X12_DIV_ISP0, 0, 3),
+	DIV(div_isp1, "div_isp1", "aclk200", E4X12_DIV_ISP0, 4, 3),
+	DIV(none, "div_mpwm", "div_isp1", E4X12_DIV_ISP1, 0, 3),
+	DIV(div_mcuisp0, "div_mcuisp0", "aclk400_mcuisp", E4X12_DIV_ISP1, 4, 3),
+	DIV(div_mcuisp1, "div_mcuisp1", "div_mcuisp0", E4X12_DIV_ISP1, 8, 3),
+};
+
+/* list of gate clocks supported in all exynos4 soc's */
+struct samsung_gate_clock exynos4_gate_clks[] __initdata = {
+	/*
+	 * After all Exynos4 based platforms are migrated to use device tree,
+	 * the device name and clock alias names specified below for some
+	 * of the clocks can be removed.
+	 */
+	GATE(sclk_hdmi, "sclk_hdmi", "mout_hdmi", SRC_MASK_TV, 0, 0, 0),
+	GATE(sclk_spdif, "sclk_spdif", "mout_spdif", SRC_MASK_PERIL1, 8, 0, 0),
+	GATE(jpeg, "jpeg", "aclk160", GATE_IP_CAM, 6, 0, 0),
+	GATE(mie0, "mie0", "aclk160", GATE_IP_LCD0, 1, 0, 0),
+	GATE(dsim0, "dsim0", "aclk160", GATE_IP_LCD0, 3, 0, 0),
+	GATE(fimd1, "fimd1", "aclk160", E4210_GATE_IP_LCD1, 0, 0, 0),
+	GATE(mie1, "mie1", "aclk160", E4210_GATE_IP_LCD1, 1, 0, 0),
+	GATE(dsim1, "dsim1", "aclk160", E4210_GATE_IP_LCD1, 3, 0, 0),
+	GATE(smmu_fimd1, "smmu_fimd1", "aclk160", E4210_GATE_IP_LCD1, 4, 0, 0),
+	GATE(tsi, "tsi", "aclk133", GATE_IP_FSYS, 4, 0, 0),
+	GATE(sromc, "sromc", "aclk133", GATE_IP_FSYS, 11, 0, 0),
+	GATE(sclk_g3d, "sclk_g3d", "div_g3d", GATE_IP_G3D, 0,
+			CLK_SET_RATE_PARENT, 0),
+	GATE(usb_device, "usb_device", "aclk133", GATE_IP_FSYS, 13, 0, 0),
+	GATE(onenand, "onenand", "aclk133", GATE_IP_FSYS, 15, 0, 0),
+	GATE(nfcon, "nfcon", "aclk133", GATE_IP_FSYS, 16, 0, 0),
+	GATE(gps, "gps", "aclk133", GATE_IP_GPS, 0, 0, 0),
+	GATE(smmu_gps, "smmu_gps", "aclk133", GATE_IP_GPS, 1, 0, 0),
+	GATE(slimbus, "slimbus", "aclk100", GATE_IP_PERIL, 25, 0, 0),
+	GATE(sclk_cam0, "sclk_cam0", "div_cam0", GATE_SCLK_CAM, 4,
+			CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_cam1, "sclk_cam1", "div_cam1", GATE_SCLK_CAM, 5,
+			CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_mipi0, "sclk_mipi0", "div_mipi_pre0",
+			SRC_MASK_LCD0, 12, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_audio0, "sclk_audio0", "div_audio0", SRC_MASK_MAUDIO, 0,
+			CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_audio1, "sclk_audio1", "div_audio1", SRC_MASK_PERIL1, 0,
+			CLK_SET_RATE_PARENT, 0),
+	GATE_D(vp, "s5p-mixer", "vp", "aclk160", GATE_IP_TV, 0, 0, 0),
+	GATE_D(mixer, "s5p-mixer", "mixer", "aclk160", GATE_IP_TV, 1, 0, 0),
+	GATE_D(hdmi, "exynos4-hdmi", "hdmi", "aclk160", GATE_IP_TV, 3, 0, 0),
+	GATE_A(pwm, "pwm", "aclk100", GATE_IP_PERIL, 24, 0, 0, "timers"),
+	GATE_A(sdmmc4, "sdmmc4", "aclk133", GATE_IP_FSYS, 9, 0, 0, "biu"),
+	GATE_A(usb_host, "usb_host", "aclk133",
+			GATE_IP_FSYS, 12, 0, 0, "usbhost"),
+	GATE_DA(sclk_fimc0, "exynos4-fimc.0", "sclk_fimc0", "div_fimc0",
+			SRC_MASK_CAM, 0, CLK_SET_RATE_PARENT, 0, "sclk_fimc"),
+	GATE_DA(sclk_fimc1, "exynos4-fimc.1", "sclk_fimc1", "div_fimc1",
+			SRC_MASK_CAM, 4, CLK_SET_RATE_PARENT, 0, "sclk_fimc"),
+	GATE_DA(sclk_fimc2, "exynos4-fimc.2", "sclk_fimc2", "div_fimc2",
+			SRC_MASK_CAM, 8, CLK_SET_RATE_PARENT, 0, "sclk_fimc"),
+	GATE_DA(sclk_fimc3, "exynos4-fimc.3", "sclk_fimc3", "div_fimc3",
+			SRC_MASK_CAM, 12, CLK_SET_RATE_PARENT, 0, "sclk_fimc"),
+	GATE_DA(sclk_csis0, "s5p-mipi-csis.0", "sclk_csis0", "div_csis0",
+			SRC_MASK_CAM, 24, CLK_SET_RATE_PARENT, 0, "sclk_csis"),
+	GATE_DA(sclk_csis1, "s5p-mipi-csis.1", "sclk_csis1", "div_csis1",
+			SRC_MASK_CAM, 28, CLK_SET_RATE_PARENT, 0, "sclk_csis"),
+	GATE_DA(sclk_fimd0, "exynos4-fb.0", "sclk_fimd0", "div_fimd0",
+			SRC_MASK_LCD0, 0, CLK_SET_RATE_PARENT, 0, "sclk_fimd"),
+	GATE_DA(sclk_mmc0, "exynos4-sdhci.0", "sclk_mmc0", "div_mmc_pre0",
+			SRC_MASK_FSYS, 0, CLK_SET_RATE_PARENT, 0,
+			"mmc_busclk.2"),
+	GATE_DA(sclk_mmc1, "exynos4-sdhci.1", "sclk_mmc1", "div_mmc_pre1",
+			SRC_MASK_FSYS, 4, CLK_SET_RATE_PARENT, 0,
+			"mmc_busclk.2"),
+	GATE_DA(sclk_mmc2, "exynos4-sdhci.2", "sclk_mmc2", "div_mmc_pre2",
+			SRC_MASK_FSYS, 8, CLK_SET_RATE_PARENT, 0,
+			"mmc_busclk.2"),
+	GATE_DA(sclk_mmc3, "exynos4-sdhci.3", "sclk_mmc3", "div_mmc_pre3",
+			SRC_MASK_FSYS, 12, CLK_SET_RATE_PARENT, 0,
+			"mmc_busclk.2"),
+	GATE_DA(sclk_mmc4, NULL, "sclk_mmc4", "div_mmc_pre4",
+			SRC_MASK_FSYS, 16, CLK_SET_RATE_PARENT, 0, "ciu"),
+	GATE_DA(sclk_uart0, "exynos4210-uart.0", "uclk0", "div_uart0",
+			SRC_MASK_PERIL0, 0, CLK_SET_RATE_PARENT,
+			0, "clk_uart_baud0"),
+	GATE_DA(sclk_uart1, "exynos4210-uart.1", "uclk1", "div_uart1",
+			SRC_MASK_PERIL0, 4, CLK_SET_RATE_PARENT,
+			0, "clk_uart_baud0"),
+	GATE_DA(sclk_uart2, "exynos4210-uart.2", "uclk2", "div_uart2",
+			SRC_MASK_PERIL0, 8, CLK_SET_RATE_PARENT,
+			0, "clk_uart_baud0"),
+	GATE_DA(sclk_uart3, "exynos4210-uart.3", "uclk3", "div_uart3",
+			SRC_MASK_PERIL0, 12, CLK_SET_RATE_PARENT,
+			0, "clk_uart_baud0"),
+	GATE_DA(sclk_uart4, "exynos4210-uart.4", "uclk4", "div_uart4",
+			SRC_MASK_PERIL0, 16, CLK_SET_RATE_PARENT,
+			0, "clk_uart_baud0"),
+	GATE(sclk_audio2, "sclk_audio2", "div_audio2", SRC_MASK_PERIL1, 4,
+			CLK_SET_RATE_PARENT, 0),
+	GATE_DA(sclk_spi0, "exynos4210-spi.0", "sclk_spi0", "div_spi_pre0",
+			SRC_MASK_PERIL1, 16, CLK_SET_RATE_PARENT,
+			0, "spi_busclk0"),
+	GATE_DA(sclk_spi1, "exynos4210-spi.1", "sclk_spi1", "div_spi_pre1",
+			SRC_MASK_PERIL1, 20, CLK_SET_RATE_PARENT,
+			0, "spi_busclk0"),
+	GATE_DA(sclk_spi2, "exynos4210-spi.2", "sclk_spi2", "div_spi_pre2",
+			SRC_MASK_PERIL1, 24, CLK_SET_RATE_PARENT,
+			0, "spi_busclk0"),
+	GATE_DA(fimc0, "exynos4-fimc.0", "fimc0", "aclk160",
+			GATE_IP_CAM, 0, 0, 0, "fimc"),
+	GATE_DA(fimc1, "exynos4-fimc.1", "fimc1", "aclk160",
+			GATE_IP_CAM, 1, 0, 0, "fimc"),
+	GATE_DA(fimc2, "exynos4-fimc.2", "fimc2", "aclk160",
+			GATE_IP_CAM, 2, 0, 0, "fimc"),
+	GATE_DA(fimc3, "exynos4-fimc.3", "fimc3", "aclk160",
+			GATE_IP_CAM, 3, 0, 0, "fimc"),
+	GATE_DA(csis0, "s5p-mipi-csis.0", "csis0", "aclk160",
+			GATE_IP_CAM, 4, 0, 0, "fimc"),
+	GATE_DA(csis1, "s5p-mipi-csis.1", "csis1", "aclk160",
+			GATE_IP_CAM, 5, 0, 0, "fimc"),
+	GATE_DA(smmu_fimc0, "exynos-sysmmu.5", "smmu_fimc0", "aclk160",
+			GATE_IP_CAM, 7, 0, 0, "sysmmu"),
+	GATE_DA(smmu_fimc1, "exynos-sysmmu.6", "smmu_fimc1", "aclk160",
+			GATE_IP_CAM, 8, 0, 0, "sysmmu"),
+	GATE_DA(smmu_fimc2, "exynos-sysmmu.7", "smmu_fimc2", "aclk160",
+			GATE_IP_CAM, 9, 0, 0, "sysmmu"),
+	GATE_DA(smmu_fimc3, "exynos-sysmmu.8", "smmu_fimc3", "aclk160",
+			GATE_IP_CAM, 10, 0, 0, "sysmmu"),
+	GATE_DA(smmu_jpeg, "exynos-sysmmu.3", "smmu_jpeg", "aclk160",
+			GATE_IP_CAM, 11, 0, 0, "sysmmu"),
+	GATE(pixelasyncm0, "pxl_async0", "aclk160", GATE_IP_CAM, 17, 0, 0),
+	GATE(pixelasyncm1, "pxl_async1", "aclk160", GATE_IP_CAM, 18, 0, 0),
+	GATE_DA(smmu_tv, "exynos-sysmmu.2", "smmu_tv", "aclk160",
+			GATE_IP_TV, 4, 0, 0, "sysmmu"),
+	GATE_DA(mfc, "s5p-mfc", "mfc", "aclk100", GATE_IP_MFC, 0, 0, 0, "mfc"),
+	GATE_DA(smmu_mfcl, "exynos-sysmmu.0", "smmu_mfcl", "aclk100",
+			GATE_IP_MFC, 1, 0, 0, "sysmmu"),
+	GATE_DA(smmu_mfcr, "exynos-sysmmu.1", "smmu_mfcr", "aclk100",
+			GATE_IP_MFC, 2, 0, 0, "sysmmu"),
+	GATE_DA(fimd0, "exynos4-fb.0", "fimd0", "aclk160",
+			GATE_IP_LCD0, 0, 0, 0, "fimd"),
+	GATE_DA(smmu_fimd0, "exynos-sysmmu.10", "smmu_fimd0", "aclk160",
+			GATE_IP_LCD0, 4, 0, 0, "sysmmu"),
+	GATE_DA(pdma0, "dma-pl330.0", "pdma0", "aclk133",
+			GATE_IP_FSYS, 0, 0, 0, "dma"),
+	GATE_DA(pdma1, "dma-pl330.1", "pdma1", "aclk133",
+			GATE_IP_FSYS, 1, 0, 0, "dma"),
+	GATE_DA(sdmmc0, "exynos4-sdhci.0", "sdmmc0", "aclk133",
+			GATE_IP_FSYS, 5, 0, 0, "hsmmc"),
+	GATE_DA(sdmmc1, "exynos4-sdhci.1", "sdmmc1", "aclk133",
+			GATE_IP_FSYS, 6, 0, 0, "hsmmc"),
+	GATE_DA(sdmmc2, "exynos4-sdhci.2", "sdmmc2", "aclk133",
+			GATE_IP_FSYS, 7, 0, 0, "hsmmc"),
+	GATE_DA(sdmmc3, "exynos4-sdhci.3", "sdmmc3", "aclk133",
+			GATE_IP_FSYS, 8, 0, 0, "hsmmc"),
+	GATE_DA(uart0, "exynos4210-uart.0", "uart0", "aclk100",
+			GATE_IP_PERIL, 0, 0, 0, "uart"),
+	GATE_DA(uart1, "exynos4210-uart.1", "uart1", "aclk100",
+			GATE_IP_PERIL, 1, 0, 0, "uart"),
+	GATE_DA(uart2, "exynos4210-uart.2", "uart2", "aclk100",
+			GATE_IP_PERIL, 2, 0, 0, "uart"),
+	GATE_DA(uart3, "exynos4210-uart.3", "uart3", "aclk100",
+			GATE_IP_PERIL, 3, 0, 0, "uart"),
+	GATE_DA(uart4, "exynos4210-uart.4", "uart4", "aclk100",
+			GATE_IP_PERIL, 4, 0, 0, "uart"),
+	GATE_DA(i2c0, "s3c2440-i2c.0", "i2c0", "aclk100",
+			GATE_IP_PERIL, 6, 0, 0, "i2c"),
+	GATE_DA(i2c1, "s3c2440-i2c.1", "i2c1", "aclk100",
+			GATE_IP_PERIL, 7, 0, 0, "i2c"),
+	GATE_DA(i2c2, "s3c2440-i2c.2", "i2c2", "aclk100",
+			GATE_IP_PERIL, 8, 0, 0, "i2c"),
+	GATE_DA(i2c3, "s3c2440-i2c.3", "i2c3", "aclk100",
+			GATE_IP_PERIL, 9, 0, 0, "i2c"),
+	GATE_DA(i2c4, "s3c2440-i2c.4", "i2c4", "aclk100",
+			GATE_IP_PERIL, 10, 0, 0, "i2c"),
+	GATE_DA(i2c5, "s3c2440-i2c.5", "i2c5", "aclk100",
+			GATE_IP_PERIL, 11, 0, 0, "i2c"),
+	GATE_DA(i2c6, "s3c2440-i2c.6", "i2c6", "aclk100",
+			GATE_IP_PERIL, 12, 0, 0, "i2c"),
+	GATE_DA(i2c7, "s3c2440-i2c.7", "i2c7", "aclk100",
+			GATE_IP_PERIL, 13, 0, 0, "i2c"),
+	GATE_DA(i2c_hdmi, "s3c2440-hdmiphy-i2c", "i2c-hdmi", "aclk100",
+			GATE_IP_PERIL, 14, 0, 0, "i2c"),
+	GATE_DA(spi0, "exynos4210-spi.0", "spi0", "aclk100",
+			GATE_IP_PERIL, 16, 0, 0, "spi"),
+	GATE_DA(spi1, "exynos4210-spi.1", "spi1", "aclk100",
+			GATE_IP_PERIL, 17, 0, 0, "spi"),
+	GATE_DA(spi2, "exynos4210-spi.2", "spi2", "aclk100",
+			GATE_IP_PERIL, 18, 0, 0, "spi"),
+	GATE_DA(i2s1, "samsung-i2s.1", "i2s1", "aclk100",
+			GATE_IP_PERIL, 20, 0, 0, "iis"),
+	GATE_DA(i2s2, "samsung-i2s.2", "i2s2", "aclk100",
+			GATE_IP_PERIL, 21, 0, 0, "iis"),
+	GATE_DA(pcm1, "samsung-pcm.1", "pcm1", "aclk100",
+			GATE_IP_PERIL, 22, 0, 0, "pcm"),
+	GATE_DA(pcm2, "samsung-pcm.2", "pcm2", "aclk100",
+			GATE_IP_PERIL, 23, 0, 0, "pcm"),
+	GATE_DA(spdif, "samsung-spdif", "spdif", "aclk100",
+			GATE_IP_PERIL, 26, 0, 0, "spdif"),
+	GATE_DA(ac97, "samsung-ac97", "ac97", "aclk100",
+			GATE_IP_PERIL, 27, 0, 0, "ac97"),
+};
+
+/* list of gate clocks supported in exynos4210 soc */
+struct samsung_gate_clock exynos4210_gate_clks[] __initdata = {
+	GATE(tvenc, "tvenc", "aclk160", GATE_IP_TV, 2, 0, 0),
+	GATE(g2d, "g2d", "aclk200", E4210_GATE_IP_IMAGE, 0, 0, 0),
+	GATE(rotator, "rotator", "aclk200", E4210_GATE_IP_IMAGE, 1, 0, 0),
+	GATE(mdma, "mdma", "aclk200", E4210_GATE_IP_IMAGE, 2, 0, 0),
+	GATE(smmu_g2d, "smmu_g2d", "aclk200", E4210_GATE_IP_IMAGE, 3, 0, 0),
+	GATE(smmu_mdma, "smmu_mdma", "aclk200", E4210_GATE_IP_IMAGE, 5, 0, 0),
+	GATE(pcie_phy, "pcie_phy", "aclk133", GATE_IP_FSYS, 2, 0, 0),
+	GATE(sata_phy, "sata_phy", "aclk133", GATE_IP_FSYS, 3, 0, 0),
+	GATE(sata, "sata", "aclk133", GATE_IP_FSYS, 10, 0, 0),
+	GATE(pcie, "pcie", "aclk133", GATE_IP_FSYS, 14, 0, 0),
+	GATE(smmu_pcie, "smmu_pcie", "aclk133", GATE_IP_FSYS, 18, 0, 0),
+	GATE(modemif, "modemif", "aclk100", GATE_IP_PERIL, 28, 0, 0),
+	GATE(chipid, "chipid", "aclk100", E4210_GATE_IP_PERIR, 0, 0, 0),
+	GATE(sysreg, "sysreg", "aclk100", E4210_GATE_IP_PERIR, 0, 0, 0),
+	GATE(hdmi_cec, "hdmi_cec", "aclk100", E4210_GATE_IP_PERIR, 11, 0, 0),
+	GATE(smmu_rotator, "smmu_rotator", "aclk200",
+			E4210_GATE_IP_IMAGE, 4, 0, 0),
+	GATE(sclk_mipi1, "sclk_mipi1", "div_mipi_pre1",
+			E4210_SRC_MASK_LCD1, 12, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_sata, "sclk_sata", "div_sata",
+			SRC_MASK_FSYS, 24, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_mixer, "sclk_mixer", "mout_mixer", SRC_MASK_TV, 4, 0, 0),
+	GATE(sclk_dac, "sclk_dac", "mout_dac", SRC_MASK_TV, 8, 0, 0),
+	GATE_A(tsadc, "tsadc", "aclk100", GATE_IP_PERIL, 15, 0, 0, "adc"),
+	GATE_A(mct, "mct", "aclk100", E4210_GATE_IP_PERIR, 13, 0, 0, "mct"),
+	GATE_A(wdt, "watchdog", "aclk100", E4210_GATE_IP_PERIR, 14, 0, 0, "watchdog"),
+	GATE_A(rtc, "rtc", "aclk100", E4210_GATE_IP_PERIR, 15, 0, 0, "rtc"),
+	GATE_A(keyif, "keyif", "aclk100", E4210_GATE_IP_PERIR, 16, 0, 0, "keypad"),
+	GATE_DA(sclk_fimd1, "exynos4-fb.1", "sclk_fimd1", "div_fimd1",
+			E4210_SRC_MASK_LCD1, 0, CLK_SET_RATE_PARENT, 0, "sclk_fimd"),
+};
+
+/* list of gate clocks supported in exynos4x12 soc */
+struct samsung_gate_clock exynos4x12_gate_clks[] __initdata = {
+	GATE(audss, "audss", "sclk_epll", E4X12_GATE_IP_MAUDIO, 0, 0, 0),
+	GATE(mdnie0, "mdnie0", "aclk160", GATE_IP_LCD0, 2, 0, 0),
+	GATE(rotator, "rotator", "aclk200", E4X12_GATE_IP_IMAGE, 1, 0, 0),
+	GATE(mdma2, "mdma2", "aclk200", E4X12_GATE_IP_IMAGE, 2, 0, 0),
+	GATE(smmu_mdma, "smmu_mdma", "aclk200", E4X12_GATE_IP_IMAGE, 5, 0, 0),
+	GATE(mipi_hsi, "mipi_hsi", "aclk133", GATE_IP_FSYS, 10, 0, 0),
+	GATE(chipid, "chipid", "aclk100", E4X12_GATE_IP_PERIR, 0, 0, 0),
+	GATE(sysreg, "sysreg", "aclk100", E4X12_GATE_IP_PERIR, 1, 0, 0),
+	GATE(hdmi_cec, "hdmi_cec", "aclk100", E4X12_GATE_IP_PERIR, 11, 0, 0),
+	GATE(sclk_mdnie0, "sclk_mdnie0", "div_mdnie0",
+			SRC_MASK_LCD0, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_mdnie_pwm0, "sclk_mdnie_pwm0", "div_mdnie_pwm_pre0",
+			SRC_MASK_LCD0, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_mipihsi, "sclk_mipihsi", "div_mipihsi",
+			SRC_MASK_FSYS, 24, CLK_SET_RATE_PARENT, 0),
+	GATE(smmu_rotator, "smmu_rotator", "aclk200",
+			E4X12_GATE_IP_IMAGE, 4, 0, 0),
+	GATE_A(mct, "mct", "aclk100", E4X12_GATE_IP_PERIR, 13, 0, 0, "mct"),
+	GATE_A(rtc, "rtc", "aclk100", E4X12_GATE_IP_PERIR, 15, 0, 0, "rtc"),
+	GATE_A(keyif, "keyif", "aclk100",
+			E4X12_GATE_IP_PERIR, 16, 0, 0, "keypad"),
+	GATE(sclk_pwm_isp, "sclk_pwm_isp", "div_pwm_isp",
+			E4X12_SRC_MASK_ISP, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_spi0_isp, "sclk_spi0_isp", "div_spi0_isp_pre",
+			E4X12_SRC_MASK_ISP, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_spi1_isp, "sclk_spi1_isp", "div_spi1_isp_pre",
+			E4X12_SRC_MASK_ISP, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_uart_isp, "sclk_uart_isp", "div_uart_isp",
+			E4X12_SRC_MASK_ISP, 12, CLK_SET_RATE_PARENT, 0),
+	GATE(pwm_isp_sclk, "pwm_isp_sclk", "sclk_pwm_isp",
+			E4X12_GATE_IP_ISP, 0, 0, 0),
+	GATE(spi0_isp_sclk, "spi0_isp_sclk", "sclk_spi0_isp",
+			E4X12_GATE_IP_ISP, 1, 0, 0),
+	GATE(spi1_isp_sclk, "spi1_isp_sclk", "sclk_spi1_isp",
+			E4X12_GATE_IP_ISP, 2, 0, 0),
+	GATE(uart_isp_sclk, "uart_isp_sclk", "sclk_uart_isp",
+			E4X12_GATE_IP_ISP, 3, 0, 0),
+	GATE_A(wdt, "watchdog", "aclk100",
+			E4X12_GATE_IP_PERIR, 14, 0, 0, "watchdog"),
+	GATE_DA(pcm0, "samsung-pcm.0", "pcm0", "aclk100",
+			E4X12_GATE_IP_MAUDIO, 2, 0, 0, "pcm"),
+	GATE_DA(i2s0, "samsung-i2s.0", "i2s0", "aclk100",
+			E4X12_GATE_IP_MAUDIO, 3, 0, 0, "iis"),
+	GATE(fimc_isp, "isp", "aclk200", E4X12_GATE_ISP0, 0,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(fimc_drc, "drc", "aclk200", E4X12_GATE_ISP0, 1,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(fimc_fd, "fd", "aclk200", E4X12_GATE_ISP0, 2,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(fimc_lite0, "lite0", "aclk200", E4X12_GATE_ISP0, 3,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(fimc_lite1, "lite1", "aclk200", E4X12_GATE_ISP0, 4,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(mcuisp, "mcuisp", "aclk200", E4X12_GATE_ISP0, 5,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(gicisp, "gicisp", "aclk200", E4X12_GATE_ISP0, 7,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(smmu_isp, "smmu_isp", "aclk200", E4X12_GATE_ISP0, 8,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(smmu_drc, "smmu_drc", "aclk200", E4X12_GATE_ISP0, 9,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(smmu_fd, "smmu_fd", "aclk200", E4X12_GATE_ISP0, 10,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(smmu_lite0, "smmu_lite0", "aclk200", E4X12_GATE_ISP0, 11,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(smmu_lite1, "smmu_lite1", "aclk200", E4X12_GATE_ISP0, 12,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(ppmuispmx, "ppmuispmx", "aclk200", E4X12_GATE_ISP0, 20,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(ppmuispx, "ppmuispx", "aclk200", E4X12_GATE_ISP0, 21,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(mcuctl_isp, "mcuctl_isp", "aclk200", E4X12_GATE_ISP0, 23,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(mpwm_isp, "mpwm_isp", "aclk200", E4X12_GATE_ISP0, 24,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(i2c0_isp, "i2c0_isp", "aclk200", E4X12_GATE_ISP0, 25,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(i2c1_isp, "i2c1_isp", "aclk200", E4X12_GATE_ISP0, 26,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(mtcadc_isp, "mtcadc_isp", "aclk200", E4X12_GATE_ISP0, 27,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(pwm_isp, "pwm_isp", "aclk200", E4X12_GATE_ISP0, 28,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(wdt_isp, "wdt_isp", "aclk200", E4X12_GATE_ISP0, 30,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(uart_isp, "uart_isp", "aclk200", E4X12_GATE_ISP0, 31,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(asyncaxim, "asyncaxim", "aclk200", E4X12_GATE_ISP1, 0,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(smmu_ispcx, "smmu_ispcx", "aclk200", E4X12_GATE_ISP1, 4,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(spi0_isp, "spi0_isp", "aclk200", E4X12_GATE_ISP1, 12,
+			CLK_IGNORE_UNUSED, 0),
+	GATE(spi1_isp, "spi1_isp", "aclk200", E4X12_GATE_ISP1, 13,
+			CLK_IGNORE_UNUSED, 0),
+};
+
+/*
+ * The parent of the fin_pll clock is selected by the XOM[0] bit. This bit
+ * resides in chipid register space, outside of the clock controller memory
+ * mapped space. So to determine the parent of fin_pll clock, the chipid
+ * controller is first remapped and the value of XOM[0] bit is read to
+ * determine the parent clock.
+ */
+static unsigned long exynos4_get_xom(void)
+{
+	unsigned long xom = 0;
+	void __iomem *chipid_base;
+	struct device_node *np;
+
+	np = of_find_compatible_node(NULL, NULL, "samsung,exynos4210-chipid");
+	if (np) {
+		chipid_base = of_iomap(np, 0);
+
+		if (chipid_base)
+			xom = readl(chipid_base + 8);
+
+		iounmap(chipid_base);
+	}
+
+	return xom;
+}
+
+static void __init exynos4_clk_register_finpll(unsigned long xom)
+{
+	struct samsung_fixed_rate_clock fclk;
+	struct clk *clk;
+	unsigned long finpll_f = 24000000;
+	char *parent_name;
+
+	parent_name = xom & 1 ? "xusbxti" : "xxti";
+	clk = clk_get(NULL, parent_name);
+	if (IS_ERR(clk)) {
+		pr_err("%s: failed to lookup parent clock %s, assuming "
+			"fin_pll clock frequency is 24MHz\n", __func__,
+			parent_name);
+	} else {
+		finpll_f = clk_get_rate(clk);
+	}
+
+	fclk.id = fin_pll;
+	fclk.name = "fin_pll";
+	fclk.parent_name = NULL;
+	fclk.flags = CLK_IS_ROOT;
+	fclk.fixed_rate = finpll_f;
+	samsung_clk_register_fixed_rate(&fclk, 1);
+
+}
+
+/*
+ * This function allows non-dt platforms to specify the clock speed of the
+ * xxti and xusbxti clocks. These clocks are then registered with the specified
+ * clock speed.
+ */
+void __init exynos4_clk_register_fixed_ext(unsigned long xxti_f,
+						unsigned long xusbxti_f)
+{
+	exynos4_fixed_rate_ext_clks[0].fixed_rate = xxti_f;
+	exynos4_fixed_rate_ext_clks[1].fixed_rate = xusbxti_f;
+	samsung_clk_register_fixed_rate(exynos4_fixed_rate_ext_clks,
+			ARRAY_SIZE(exynos4_fixed_rate_ext_clks));
+}
+
+static __initdata struct of_device_id ext_clk_match[] = {
+	{ .compatible = "samsung,clock-xxti", .data = (void *)0, },
+	{ .compatible = "samsung,clock-xusbxti", .data = (void *)1, },
+	{},
+};
+
+/* register exynos4 clocks */
+void __init exynos4_clk_init(struct device_node *np, enum exynos4_soc exynos4_soc, void __iomem *reg_base, unsigned long xom)
+{
+	struct clk *apll, *mpll, *epll, *vpll;
+
+	if (np) {
+		reg_base = of_iomap(np, 0);
+		if (!reg_base)
+			panic("%s: failed to map registers\n", __func__);
+	}
+
+	if (exynos4_soc == EXYNOS4210)
+		samsung_clk_init(np, reg_base, nr_clks,
+			exynos4_clk_regs, ARRAY_SIZE(exynos4_clk_regs),
+			exynos4210_clk_save, ARRAY_SIZE(exynos4210_clk_save));
+	else
+		samsung_clk_init(np, reg_base, nr_clks,
+			exynos4_clk_regs, ARRAY_SIZE(exynos4_clk_regs),
+			exynos4x12_clk_save, ARRAY_SIZE(exynos4x12_clk_save));
+
+	if (np)
+		samsung_clk_of_register_fixed_ext(exynos4_fixed_rate_ext_clks,
+			ARRAY_SIZE(exynos4_fixed_rate_ext_clks),
+			ext_clk_match);
+
+	exynos4_clk_register_finpll(xom);
+
+	if (exynos4_soc == EXYNOS4210) {
+		apll = samsung_clk_register_pll45xx("fout_apll", "fin_pll",
+					reg_base + APLL_CON0, pll_4508);
+		mpll = samsung_clk_register_pll45xx("fout_mpll", "fin_pll",
+					reg_base + E4210_MPLL_CON0, pll_4508);
+		epll = samsung_clk_register_pll46xx("fout_epll", "fin_pll",
+					reg_base + EPLL_CON0, pll_4600);
+		vpll = samsung_clk_register_pll46xx("fout_vpll", "mout_vpllsrc",
+					reg_base + VPLL_CON0, pll_4650c);
+	} else {
+		apll = samsung_clk_register_pll35xx("fout_apll", "fin_pll",
+					reg_base + APLL_CON0);
+		mpll = samsung_clk_register_pll35xx("fout_mpll", "fin_pll",
+					reg_base + E4X12_MPLL_CON0);
+		epll = samsung_clk_register_pll36xx("fout_epll", "fin_pll",
+					reg_base + EPLL_CON0);
+		vpll = samsung_clk_register_pll36xx("fout_vpll", "fin_pll",
+					reg_base + VPLL_CON0);
+	}
+
+	samsung_clk_add_lookup(apll, fout_apll);
+	samsung_clk_add_lookup(mpll, fout_mpll);
+	samsung_clk_add_lookup(epll, fout_epll);
+	samsung_clk_add_lookup(vpll, fout_vpll);
+
+	samsung_clk_register_fixed_rate(exynos4_fixed_rate_clks,
+			ARRAY_SIZE(exynos4_fixed_rate_clks));
+	samsung_clk_register_mux(exynos4_mux_clks,
+			ARRAY_SIZE(exynos4_mux_clks));
+	samsung_clk_register_div(exynos4_div_clks,
+			ARRAY_SIZE(exynos4_div_clks));
+	samsung_clk_register_gate(exynos4_gate_clks,
+			ARRAY_SIZE(exynos4_gate_clks));
+
+	if (exynos4_soc == EXYNOS4210) {
+		samsung_clk_register_fixed_rate(exynos4210_fixed_rate_clks,
+			ARRAY_SIZE(exynos4210_fixed_rate_clks));
+		samsung_clk_register_mux(exynos4210_mux_clks,
+			ARRAY_SIZE(exynos4210_mux_clks));
+		samsung_clk_register_div(exynos4210_div_clks,
+			ARRAY_SIZE(exynos4210_div_clks));
+		samsung_clk_register_gate(exynos4210_gate_clks,
+			ARRAY_SIZE(exynos4210_gate_clks));
+	} else {
+		samsung_clk_register_mux(exynos4x12_mux_clks,
+			ARRAY_SIZE(exynos4x12_mux_clks));
+		samsung_clk_register_div(exynos4x12_div_clks,
+			ARRAY_SIZE(exynos4x12_div_clks));
+		samsung_clk_register_gate(exynos4x12_gate_clks,
+			ARRAY_SIZE(exynos4x12_gate_clks));
+	}
+
+	pr_info("%s clocks: sclk_apll = %ld, sclk_mpll = %ld\n"
+		"\tsclk_epll = %ld, sclk_vpll = %ld, arm_clk = %ld\n",
+		exynos4_soc == EXYNOS4210 ? "Exynos4210" : "Exynos4x12",
+		_get_rate("sclk_apll"),	_get_rate("sclk_mpll"),
+		_get_rate("sclk_epll"), _get_rate("sclk_vpll"),
+		_get_rate("arm_clk"));
+}
+
+
+static void __init exynos4210_clk_init(struct device_node *np)
+{
+	exynos4_clk_init(np, EXYNOS4210, NULL, exynos4_get_xom());
+}
+CLK_OF_DECLARE(exynos4210_clk, "samsung,exynos4210-clock", exynos4210_clk_init);
+
+static void __init exynos4412_clk_init(struct device_node *np)
+{
+	exynos4_clk_init(np, EXYNOS4X12, NULL, exynos4_get_xom());
+}
+CLK_OF_DECLARE(exynos4412_clk, "samsung,exynos4412-clock", exynos4412_clk_init);
diff --git a/drivers/clk/samsung/clk-exynos5250.c b/drivers/clk/samsung/clk-exynos5250.c
new file mode 100644
index 000000000000..5c97e75924a8
--- /dev/null
+++ b/drivers/clk/samsung/clk-exynos5250.c
@@ -0,0 +1,522 @@
+/*
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ * Copyright (c) 2013 Linaro Ltd.
+ * Author: Thomas Abraham <thomas.ab@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.
+ *
+ * Common Clock Framework support for Exynos5250 SoC.
+*/
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#include "clk.h"
+#include "clk-pll.h"
+
+#define SRC_CPU			0x200
+#define DIV_CPU0		0x500
+#define SRC_CORE1		0x4204
+#define SRC_TOP0		0x10210
+#define SRC_TOP2		0x10218
+#define SRC_GSCL		0x10220
+#define SRC_DISP1_0		0x1022c
+#define SRC_MAU			0x10240
+#define SRC_FSYS		0x10244
+#define SRC_GEN			0x10248
+#define SRC_PERIC0		0x10250
+#define SRC_PERIC1		0x10254
+#define SRC_MASK_GSCL		0x10320
+#define SRC_MASK_DISP1_0	0x1032c
+#define SRC_MASK_MAU		0x10334
+#define SRC_MASK_FSYS		0x10340
+#define SRC_MASK_GEN		0x10344
+#define SRC_MASK_PERIC0		0x10350
+#define SRC_MASK_PERIC1		0x10354
+#define DIV_TOP0		0x10510
+#define DIV_TOP1		0x10514
+#define DIV_GSCL		0x10520
+#define DIV_DISP1_0		0x1052c
+#define DIV_GEN			0x1053c
+#define DIV_MAU			0x10544
+#define DIV_FSYS0		0x10548
+#define DIV_FSYS1		0x1054c
+#define DIV_FSYS2		0x10550
+#define DIV_PERIC0		0x10558
+#define DIV_PERIC1		0x1055c
+#define DIV_PERIC2		0x10560
+#define DIV_PERIC3		0x10564
+#define DIV_PERIC4		0x10568
+#define DIV_PERIC5		0x1056c
+#define GATE_IP_GSCL		0x10920
+#define GATE_IP_MFC		0x1092c
+#define GATE_IP_GEN		0x10934
+#define GATE_IP_FSYS		0x10944
+#define GATE_IP_PERIC		0x10950
+#define GATE_IP_PERIS		0x10960
+#define SRC_CDREX		0x20200
+#define PLL_DIV2_SEL		0x20a24
+#define GATE_IP_DISP1		0x10928
+
+/*
+ * Let each supported clock get a unique id. This id is used to lookup the clock
+ * for device tree based platforms. The clocks are categorized into three
+ * sections: core, sclk gate and bus interface gate clocks.
+ *
+ * When adding a new clock to this list, it is advised to choose a clock
+ * category and add it to the end of that category. That is because the the
+ * device tree source file is referring to these ids and any change in the
+ * sequence number of existing clocks will require corresponding change in the
+ * device tree files. This limitation would go away when pre-processor support
+ * for dtc would be available.
+ */
+enum exynos5250_clks {
+	none,
+
+	/* core clocks */
+	fin_pll,
+
+	/* gate for special clocks (sclk) */
+	sclk_cam_bayer = 128, sclk_cam0, sclk_cam1, sclk_gscl_wa, sclk_gscl_wb,
+	sclk_fimd1, sclk_mipi1, sclk_dp, sclk_hdmi, sclk_pixel, sclk_audio0,
+	sclk_mmc0, sclk_mmc1, sclk_mmc2, sclk_mmc3, sclk_sata, sclk_usb3,
+	sclk_jpeg, sclk_uart0, sclk_uart1, sclk_uart2, sclk_uart3, sclk_pwm,
+	sclk_audio1, sclk_audio2, sclk_spdif, sclk_spi0, sclk_spi1, sclk_spi2,
+
+	/* gate clocks */
+	gscl0 = 256, gscl1, gscl2, gscl3, gscl_wa, gscl_wb, smmu_gscl0,
+	smmu_gscl1, smmu_gscl2, smmu_gscl3, mfc, smmu_mfcl, smmu_mfcr, rotator,
+	jpeg, mdma1, smmu_rotator, smmu_jpeg, smmu_mdma1, pdma0, pdma1, sata,
+	usbotg, mipi_hsi, sdmmc0, sdmmc1, sdmmc2, sdmmc3, sromc, usb2, usb3,
+	sata_phyctrl, sata_phyi2c, uart0, uart1, uart2,	uart3, uart4, i2c0,
+	i2c1, i2c2, i2c3, i2c4, i2c5, i2c6, i2c7, i2c_hdmi, adc, spi0, spi1,
+	spi2, i2s1, i2s2, pcm1, pcm2, pwm, spdif, ac97, hsi2c0, hsi2c1, hsi2c2,
+	hsi2c3, chipid, sysreg, pmu, cmu_top, cmu_core, cmu_mem, tzpc0, tzpc1,
+	tzpc2, tzpc3, tzpc4, tzpc5, tzpc6, tzpc7, tzpc8, tzpc9, hdmi_cec, mct,
+	wdt, rtc, tmu, fimd1, mie1, dsim0, dp, mixer, hdmi,
+
+	nr_clks,
+};
+
+/*
+ * list of controller registers to be saved and restored during a
+ * suspend/resume cycle.
+ */
+static __initdata unsigned long exynos5250_clk_regs[] = {
+	SRC_CPU,
+	DIV_CPU0,
+	SRC_CORE1,
+	SRC_TOP0,
+	SRC_TOP2,
+	SRC_GSCL,
+	SRC_DISP1_0,
+	SRC_MAU,
+	SRC_FSYS,
+	SRC_GEN,
+	SRC_PERIC0,
+	SRC_PERIC1,
+	SRC_MASK_GSCL,
+	SRC_MASK_DISP1_0,
+	SRC_MASK_MAU,
+	SRC_MASK_FSYS,
+	SRC_MASK_GEN,
+	SRC_MASK_PERIC0,
+	SRC_MASK_PERIC1,
+	DIV_TOP0,
+	DIV_TOP1,
+	DIV_GSCL,
+	DIV_DISP1_0,
+	DIV_GEN,
+	DIV_MAU,
+	DIV_FSYS0,
+	DIV_FSYS1,
+	DIV_FSYS2,
+	DIV_PERIC0,
+	DIV_PERIC1,
+	DIV_PERIC2,
+	DIV_PERIC3,
+	DIV_PERIC4,
+	DIV_PERIC5,
+	GATE_IP_GSCL,
+	GATE_IP_MFC,
+	GATE_IP_GEN,
+	GATE_IP_FSYS,
+	GATE_IP_PERIC,
+	GATE_IP_PERIS,
+	SRC_CDREX,
+	PLL_DIV2_SEL,
+	GATE_IP_DISP1,
+};
+
+/* list of all parent clock list */
+PNAME(mout_apll_p)	= { "fin_pll", "fout_apll", };
+PNAME(mout_cpu_p)	= { "mout_apll", "mout_mpll", };
+PNAME(mout_mpll_fout_p)	= { "fout_mplldiv2", "fout_mpll" };
+PNAME(mout_mpll_p)	= { "fin_pll", "mout_mpll_fout" };
+PNAME(mout_bpll_fout_p)	= { "fout_bplldiv2", "fout_bpll" };
+PNAME(mout_bpll_p)	= { "fin_pll", "mout_bpll_fout" };
+PNAME(mout_vpllsrc_p)	= { "fin_pll", "sclk_hdmi27m" };
+PNAME(mout_vpll_p)	= { "mout_vpllsrc", "fout_vpll" };
+PNAME(mout_cpll_p)	= { "fin_pll", "fout_cpll" };
+PNAME(mout_epll_p)	= { "fin_pll", "fout_epll" };
+PNAME(mout_mpll_user_p)	= { "fin_pll", "sclk_mpll" };
+PNAME(mout_bpll_user_p)	= { "fin_pll", "sclk_bpll" };
+PNAME(mout_aclk166_p)	= { "sclk_cpll", "sclk_mpll_user" };
+PNAME(mout_aclk200_p)	= { "sclk_mpll_user", "sclk_bpll_user" };
+PNAME(mout_hdmi_p)	= { "div_hdmi_pixel", "sclk_hdmiphy" };
+PNAME(mout_usb3_p)	= { "sclk_mpll_user", "sclk_cpll" };
+PNAME(mout_group1_p)	= { "fin_pll", "fin_pll", "sclk_hdmi27m",
+				"sclk_dptxphy", "sclk_uhostphy", "sclk_hdmiphy",
+				"sclk_mpll_user", "sclk_epll", "sclk_vpll",
+				"sclk_cpll" };
+PNAME(mout_audio0_p)	= { "cdclk0", "fin_pll", "sclk_hdmi27m", "sclk_dptxphy",
+				"sclk_uhostphy", "sclk_hdmiphy",
+				"sclk_mpll_user", "sclk_epll", "sclk_vpll",
+				"sclk_cpll" };
+PNAME(mout_audio1_p)	= { "cdclk1", "fin_pll", "sclk_hdmi27m", "sclk_dptxphy",
+				"sclk_uhostphy", "sclk_hdmiphy",
+				"sclk_mpll_user", "sclk_epll", "sclk_vpll",
+				"sclk_cpll" };
+PNAME(mout_audio2_p)	= { "cdclk2", "fin_pll", "sclk_hdmi27m", "sclk_dptxphy",
+				"sclk_uhostphy", "sclk_hdmiphy",
+				"sclk_mpll_user", "sclk_epll", "sclk_vpll",
+				"sclk_cpll" };
+PNAME(mout_spdif_p)	= { "sclk_audio0", "sclk_audio1", "sclk_audio2",
+				"spdif_extclk" };
+
+/* fixed rate clocks generated outside the soc */
+struct samsung_fixed_rate_clock exynos5250_fixed_rate_ext_clks[] __initdata = {
+	FRATE(fin_pll, "fin_pll", NULL, CLK_IS_ROOT, 0),
+};
+
+/* fixed rate clocks generated inside the soc */
+struct samsung_fixed_rate_clock exynos5250_fixed_rate_clks[] __initdata = {
+	FRATE(none, "sclk_hdmiphy", NULL, CLK_IS_ROOT, 24000000),
+	FRATE(none, "sclk_hdmi27m", NULL, CLK_IS_ROOT, 27000000),
+	FRATE(none, "sclk_dptxphy", NULL, CLK_IS_ROOT, 24000000),
+	FRATE(none, "sclk_uhostphy", NULL, CLK_IS_ROOT, 48000000),
+};
+
+struct samsung_fixed_factor_clock exynos5250_fixed_factor_clks[] __initdata = {
+	FFACTOR(none, "fout_mplldiv2", "fout_mpll", 1, 2, 0),
+	FFACTOR(none, "fout_bplldiv2", "fout_bpll", 1, 2, 0),
+};
+
+struct samsung_mux_clock exynos5250_mux_clks[] __initdata = {
+	MUX(none, "mout_apll", mout_apll_p, SRC_CPU, 0, 1),
+	MUX(none, "mout_cpu", mout_cpu_p, SRC_CPU, 16, 1),
+	MUX(none, "mout_mpll_fout", mout_mpll_fout_p, PLL_DIV2_SEL, 4, 1),
+	MUX(none, "sclk_mpll", mout_mpll_p, SRC_CORE1, 8, 1),
+	MUX(none, "mout_bpll_fout", mout_bpll_fout_p, PLL_DIV2_SEL, 0, 1),
+	MUX(none, "sclk_bpll", mout_bpll_p, SRC_CDREX, 0, 1),
+	MUX(none, "mout_vpllsrc", mout_vpllsrc_p, SRC_TOP2, 0, 1),
+	MUX(none, "sclk_vpll", mout_vpll_p, SRC_TOP2, 16, 1),
+	MUX(none, "sclk_epll", mout_epll_p, SRC_TOP2, 12, 1),
+	MUX(none, "sclk_cpll", mout_cpll_p, SRC_TOP2, 8, 1),
+	MUX(none, "sclk_mpll_user", mout_mpll_user_p, SRC_TOP2, 20, 1),
+	MUX(none, "sclk_bpll_user", mout_bpll_user_p, SRC_TOP2, 24, 1),
+	MUX(none, "mout_aclk166", mout_aclk166_p, SRC_TOP0, 8, 1),
+	MUX(none, "mout_aclk333", mout_aclk166_p, SRC_TOP0, 16, 1),
+	MUX(none, "mout_aclk200", mout_aclk200_p, SRC_TOP0, 12, 1),
+	MUX(none, "mout_cam_bayer", mout_group1_p, SRC_GSCL, 12, 4),
+	MUX(none, "mout_cam0", mout_group1_p, SRC_GSCL, 16, 4),
+	MUX(none, "mout_cam1", mout_group1_p, SRC_GSCL, 20, 4),
+	MUX(none, "mout_gscl_wa", mout_group1_p, SRC_GSCL, 24, 4),
+	MUX(none, "mout_gscl_wb", mout_group1_p, SRC_GSCL, 28, 4),
+	MUX(none, "mout_fimd1", mout_group1_p, SRC_DISP1_0, 0, 4),
+	MUX(none, "mout_mipi1", mout_group1_p, SRC_DISP1_0, 12, 4),
+	MUX(none, "mout_dp", mout_group1_p, SRC_DISP1_0, 16, 4),
+	MUX(none, "mout_hdmi", mout_hdmi_p, SRC_DISP1_0, 20, 1),
+	MUX(none, "mout_audio0", mout_audio0_p, SRC_MAU, 0, 4),
+	MUX(none, "mout_mmc0", mout_group1_p, SRC_FSYS, 0, 4),
+	MUX(none, "mout_mmc1", mout_group1_p, SRC_FSYS, 4, 4),
+	MUX(none, "mout_mmc2", mout_group1_p, SRC_FSYS, 8, 4),
+	MUX(none, "mout_mmc3", mout_group1_p, SRC_FSYS, 12, 4),
+	MUX(none, "mout_sata", mout_aclk200_p, SRC_FSYS, 24, 1),
+	MUX(none, "mout_usb3", mout_usb3_p, SRC_FSYS, 28, 1),
+	MUX(none, "mout_jpeg", mout_group1_p, SRC_GEN, 0, 4),
+	MUX(none, "mout_uart0", mout_group1_p, SRC_PERIC0, 0, 4),
+	MUX(none, "mout_uart1", mout_group1_p, SRC_PERIC0, 4, 4),
+	MUX(none, "mout_uart2", mout_group1_p, SRC_PERIC0, 8, 4),
+	MUX(none, "mout_uart3", mout_group1_p, SRC_PERIC0, 12, 4),
+	MUX(none, "mout_pwm", mout_group1_p, SRC_PERIC0, 24, 4),
+	MUX(none, "mout_audio1", mout_audio1_p, SRC_PERIC1, 0, 4),
+	MUX(none, "mout_audio2", mout_audio2_p, SRC_PERIC1, 4, 4),
+	MUX(none, "mout_spdif", mout_spdif_p, SRC_PERIC1, 8, 2),
+	MUX(none, "mout_spi0", mout_group1_p, SRC_PERIC1, 16, 4),
+	MUX(none, "mout_spi1", mout_group1_p, SRC_PERIC1, 20, 4),
+	MUX(none, "mout_spi2", mout_group1_p, SRC_PERIC1, 24, 4),
+};
+
+struct samsung_div_clock exynos5250_div_clks[] __initdata = {
+	DIV(none, "div_arm", "mout_cpu", DIV_CPU0, 0, 3),
+	DIV(none, "sclk_apll", "mout_apll", DIV_CPU0, 24, 3),
+	DIV(none, "aclk66_pre", "sclk_mpll_user", DIV_TOP1, 24, 3),
+	DIV(none, "aclk66", "aclk66_pre", DIV_TOP0, 0, 3),
+	DIV(none, "aclk266", "sclk_mpll_user", DIV_TOP0, 16, 3),
+	DIV(none, "aclk166", "mout_aclk166", DIV_TOP0, 8, 3),
+	DIV(none, "aclk333", "mout_aclk333", DIV_TOP0, 20, 3),
+	DIV(none, "aclk200", "mout_aclk200", DIV_TOP0, 12, 3),
+	DIV(none, "div_cam_bayer", "mout_cam_bayer", DIV_GSCL, 12, 4),
+	DIV(none, "div_cam0", "mout_cam0", DIV_GSCL, 16, 4),
+	DIV(none, "div_cam1", "mout_cam1", DIV_GSCL, 20, 4),
+	DIV(none, "div_gscl_wa", "mout_gscl_wa", DIV_GSCL, 24, 4),
+	DIV(none, "div_gscl_wb", "mout_gscl_wb", DIV_GSCL, 28, 4),
+	DIV(none, "div_fimd1", "mout_fimd1", DIV_DISP1_0, 0, 4),
+	DIV(none, "div_mipi1", "mout_mipi1", DIV_DISP1_0, 16, 4),
+	DIV(none, "div_dp", "mout_dp", DIV_DISP1_0, 24, 4),
+	DIV(none, "div_jpeg", "mout_jpeg", DIV_GEN, 4, 4),
+	DIV(none, "div_audio0", "mout_audio0", DIV_MAU, 0, 4),
+	DIV(none, "div_pcm0", "sclk_audio0", DIV_MAU, 4, 8),
+	DIV(none, "div_sata", "mout_sata", DIV_FSYS0, 20, 4),
+	DIV(none, "div_usb3", "mout_usb3", DIV_FSYS0, 24, 4),
+	DIV(none, "div_mmc0", "mout_mmc0", DIV_FSYS1, 0, 4),
+	DIV(none, "div_mmc1", "mout_mmc1", DIV_FSYS1, 16, 4),
+	DIV(none, "div_mmc2", "mout_mmc2", DIV_FSYS2, 0, 4),
+	DIV(none, "div_mmc3", "mout_mmc3", DIV_FSYS2, 16, 4),
+	DIV(none, "div_uart0", "mout_uart0", DIV_PERIC0, 0, 4),
+	DIV(none, "div_uart1", "mout_uart1", DIV_PERIC0, 4, 4),
+	DIV(none, "div_uart2", "mout_uart2", DIV_PERIC0, 8, 4),
+	DIV(none, "div_uart3", "mout_uart3", DIV_PERIC0, 12, 4),
+	DIV(none, "div_spi0", "mout_spi0", DIV_PERIC1, 0, 4),
+	DIV(none, "div_spi1", "mout_spi1", DIV_PERIC1, 16, 4),
+	DIV(none, "div_spi2", "mout_spi2", DIV_PERIC2, 0, 4),
+	DIV(none, "div_pwm", "mout_pwm", DIV_PERIC3, 0, 4),
+	DIV(none, "div_audio1", "mout_audio1", DIV_PERIC4, 0, 4),
+	DIV(none, "div_pcm1", "sclk_audio1", DIV_PERIC4, 4, 8),
+	DIV(none, "div_audio2", "mout_audio2", DIV_PERIC4, 16, 4),
+	DIV(none, "div_pcm2", "sclk_audio2", DIV_PERIC4, 20, 8),
+	DIV(none, "div_i2s1", "sclk_audio1", DIV_PERIC5, 0, 6),
+	DIV(none, "div_i2s2", "sclk_audio2", DIV_PERIC5, 8, 6),
+	DIV(sclk_pixel, "div_hdmi_pixel", "sclk_vpll", DIV_DISP1_0, 28, 4),
+	DIV_A(none, "armclk", "div_arm", DIV_CPU0, 28, 3, "armclk"),
+	DIV_F(none, "div_mipi1_pre", "div_mipi1",
+			DIV_DISP1_0, 20, 4, CLK_SET_RATE_PARENT, 0),
+	DIV_F(none, "div_mmc_pre0", "div_mmc0",
+			DIV_FSYS1, 8, 8, CLK_SET_RATE_PARENT, 0),
+	DIV_F(none, "div_mmc_pre1", "div_mmc1",
+			DIV_FSYS1, 24, 8, CLK_SET_RATE_PARENT, 0),
+	DIV_F(none, "div_mmc_pre2", "div_mmc2",
+			DIV_FSYS2, 8, 8, CLK_SET_RATE_PARENT, 0),
+	DIV_F(none, "div_mmc_pre3", "div_mmc3",
+			DIV_FSYS2, 24, 8, CLK_SET_RATE_PARENT, 0),
+	DIV_F(none, "div_spi_pre0", "div_spi0",
+			DIV_PERIC1, 8, 8, CLK_SET_RATE_PARENT, 0),
+	DIV_F(none, "div_spi_pre1", "div_spi1",
+			DIV_PERIC1, 24, 8, CLK_SET_RATE_PARENT, 0),
+	DIV_F(none, "div_spi_pre2", "div_spi2",
+			DIV_PERIC2, 8, 8, CLK_SET_RATE_PARENT, 0),
+};
+
+struct samsung_gate_clock exynos5250_gate_clks[] __initdata = {
+	GATE(gscl0, "gscl0", "none", GATE_IP_GSCL, 0, 0, 0),
+	GATE(gscl1, "gscl1", "none", GATE_IP_GSCL, 1, 0, 0),
+	GATE(gscl2, "gscl2", "aclk266", GATE_IP_GSCL, 2, 0, 0),
+	GATE(gscl3, "gscl3", "aclk266", GATE_IP_GSCL, 3, 0, 0),
+	GATE(gscl_wa, "gscl_wa", "div_gscl_wa", GATE_IP_GSCL, 5, 0, 0),
+	GATE(gscl_wb, "gscl_wb", "div_gscl_wb", GATE_IP_GSCL, 6, 0, 0),
+	GATE(smmu_gscl0, "smmu_gscl0", "aclk266", GATE_IP_GSCL, 7, 0, 0),
+	GATE(smmu_gscl1, "smmu_gscl1", "aclk266", GATE_IP_GSCL, 8, 0, 0),
+	GATE(smmu_gscl2, "smmu_gscl2", "aclk266", GATE_IP_GSCL, 9, 0, 0),
+	GATE(smmu_gscl3, "smmu_gscl3", "aclk266", GATE_IP_GSCL, 10, 0, 0),
+	GATE(mfc, "mfc", "aclk333", GATE_IP_MFC, 0, 0, 0),
+	GATE(smmu_mfcl, "smmu_mfcl", "aclk333", GATE_IP_MFC, 1, 0, 0),
+	GATE(smmu_mfcr, "smmu_mfcr", "aclk333", GATE_IP_MFC, 2, 0, 0),
+	GATE(rotator, "rotator", "aclk266", GATE_IP_GEN, 1, 0, 0),
+	GATE(jpeg, "jpeg", "aclk166", GATE_IP_GEN, 2, 0, 0),
+	GATE(mdma1, "mdma1", "aclk266", GATE_IP_GEN, 4, 0, 0),
+	GATE(smmu_rotator, "smmu_rotator", "aclk266", GATE_IP_GEN, 6, 0, 0),
+	GATE(smmu_jpeg, "smmu_jpeg", "aclk166", GATE_IP_GEN, 7, 0, 0),
+	GATE(smmu_mdma1, "smmu_mdma1", "aclk266", GATE_IP_GEN, 9, 0, 0),
+	GATE(pdma0, "pdma0", "aclk200", GATE_IP_FSYS, 1, 0, 0),
+	GATE(pdma1, "pdma1", "aclk200", GATE_IP_FSYS, 2, 0, 0),
+	GATE(sata, "sata", "aclk200", GATE_IP_FSYS, 6, 0, 0),
+	GATE(usbotg, "usbotg", "aclk200", GATE_IP_FSYS, 7, 0, 0),
+	GATE(mipi_hsi, "mipi_hsi", "aclk200", GATE_IP_FSYS, 8, 0, 0),
+	GATE(sdmmc0, "sdmmc0", "aclk200", GATE_IP_FSYS, 12, 0, 0),
+	GATE(sdmmc1, "sdmmc1", "aclk200", GATE_IP_FSYS, 13, 0, 0),
+	GATE(sdmmc2, "sdmmc2", "aclk200", GATE_IP_FSYS, 14, 0, 0),
+	GATE(sdmmc3, "sdmmc3", "aclk200", GATE_IP_FSYS, 15, 0, 0),
+	GATE(sromc, "sromc", "aclk200", GATE_IP_FSYS, 17, 0, 0),
+	GATE(usb2, "usb2", "aclk200", GATE_IP_FSYS, 18, 0, 0),
+	GATE(usb3, "usb3", "aclk200", GATE_IP_FSYS, 19, 0, 0),
+	GATE(sata_phyctrl, "sata_phyctrl", "aclk200", GATE_IP_FSYS, 24, 0, 0),
+	GATE(sata_phyi2c, "sata_phyi2c", "aclk200", GATE_IP_FSYS, 25, 0, 0),
+	GATE(uart0, "uart0", "aclk66", GATE_IP_PERIC, 0, 0, 0),
+	GATE(uart1, "uart1", "aclk66", GATE_IP_PERIC, 1, 0, 0),
+	GATE(uart2, "uart2", "aclk66", GATE_IP_PERIC, 2, 0, 0),
+	GATE(uart3, "uart3", "aclk66", GATE_IP_PERIC, 3, 0, 0),
+	GATE(uart4, "uart4", "aclk66", GATE_IP_PERIC, 4, 0, 0),
+	GATE(i2c0, "i2c0", "aclk66", GATE_IP_PERIC, 6, 0, 0),
+	GATE(i2c1, "i2c1", "aclk66", GATE_IP_PERIC, 7, 0, 0),
+	GATE(i2c2, "i2c2", "aclk66", GATE_IP_PERIC, 8, 0, 0),
+	GATE(i2c3, "i2c3", "aclk66", GATE_IP_PERIC, 9, 0, 0),
+	GATE(i2c4, "i2c4", "aclk66", GATE_IP_PERIC, 10, 0, 0),
+	GATE(i2c5, "i2c5", "aclk66", GATE_IP_PERIC, 11, 0, 0),
+	GATE(i2c6, "i2c6", "aclk66", GATE_IP_PERIC, 12, 0, 0),
+	GATE(i2c7, "i2c7", "aclk66", GATE_IP_PERIC, 13, 0, 0),
+	GATE(i2c_hdmi, "i2c_hdmi", "aclk66", GATE_IP_PERIC, 14, 0, 0),
+	GATE(adc, "adc", "aclk66", GATE_IP_PERIC, 15, 0, 0),
+	GATE(spi0, "spi0", "aclk66", GATE_IP_PERIC, 16, 0, 0),
+	GATE(spi1, "spi1", "aclk66", GATE_IP_PERIC, 17, 0, 0),
+	GATE(spi2, "spi2", "aclk66", GATE_IP_PERIC, 18, 0, 0),
+	GATE(i2s1, "i2s1", "aclk66", GATE_IP_PERIC, 20, 0, 0),
+	GATE(i2s2, "i2s2", "aclk66", GATE_IP_PERIC, 21, 0, 0),
+	GATE(pcm1, "pcm1", "aclk66", GATE_IP_PERIC, 22, 0, 0),
+	GATE(pcm2, "pcm2", "aclk66", GATE_IP_PERIC, 23, 0, 0),
+	GATE(pwm, "pwm", "aclk66", GATE_IP_PERIC, 24, 0, 0),
+	GATE(spdif, "spdif", "aclk66", GATE_IP_PERIC, 26, 0, 0),
+	GATE(ac97, "ac97", "aclk66", GATE_IP_PERIC, 27, 0, 0),
+	GATE(hsi2c0, "hsi2c0", "aclk66", GATE_IP_PERIC, 28, 0, 0),
+	GATE(hsi2c1, "hsi2c1", "aclk66", GATE_IP_PERIC, 29, 0, 0),
+	GATE(hsi2c2, "hsi2c2", "aclk66", GATE_IP_PERIC, 30, 0, 0),
+	GATE(hsi2c3, "hsi2c3", "aclk66", GATE_IP_PERIC, 31, 0, 0),
+	GATE(chipid, "chipid", "aclk66", GATE_IP_PERIS, 0, 0, 0),
+	GATE(sysreg, "sysreg", "aclk66", GATE_IP_PERIS, 1, 0, 0),
+	GATE(pmu, "pmu", "aclk66", GATE_IP_PERIS, 2, 0, 0),
+	GATE(tzpc0, "tzpc0", "aclk66", GATE_IP_PERIS, 6, 0, 0),
+	GATE(tzpc1, "tzpc1", "aclk66", GATE_IP_PERIS, 7, 0, 0),
+	GATE(tzpc2, "tzpc2", "aclk66", GATE_IP_PERIS, 8, 0, 0),
+	GATE(tzpc3, "tzpc3", "aclk66", GATE_IP_PERIS, 9, 0, 0),
+	GATE(tzpc4, "tzpc4", "aclk66", GATE_IP_PERIS, 10, 0, 0),
+	GATE(tzpc5, "tzpc5", "aclk66", GATE_IP_PERIS, 11, 0, 0),
+	GATE(tzpc6, "tzpc6", "aclk66", GATE_IP_PERIS, 12, 0, 0),
+	GATE(tzpc7, "tzpc7", "aclk66", GATE_IP_PERIS, 13, 0, 0),
+	GATE(tzpc8, "tzpc8", "aclk66", GATE_IP_PERIS, 14, 0, 0),
+	GATE(tzpc9, "tzpc9", "aclk66", GATE_IP_PERIS, 15, 0, 0),
+	GATE(hdmi_cec, "hdmi_cec", "aclk66", GATE_IP_PERIS, 16, 0, 0),
+	GATE(mct, "mct", "aclk66", GATE_IP_PERIS, 18, 0, 0),
+	GATE(wdt, "wdt", "aclk66", GATE_IP_PERIS, 19, 0, 0),
+	GATE(rtc, "rtc", "aclk66", GATE_IP_PERIS, 20, 0, 0),
+	GATE(tmu, "tmu", "aclk66", GATE_IP_PERIS, 21, 0, 0),
+	GATE(cmu_top, "cmu_top", "aclk66",
+			GATE_IP_PERIS, 3, CLK_IGNORE_UNUSED, 0),
+	GATE(cmu_core, "cmu_core", "aclk66",
+			GATE_IP_PERIS, 4, CLK_IGNORE_UNUSED, 0),
+	GATE(cmu_mem, "cmu_mem", "aclk66",
+			GATE_IP_PERIS, 5, CLK_IGNORE_UNUSED, 0),
+	GATE(sclk_cam_bayer, "sclk_cam_bayer", "div_cam_bayer",
+			SRC_MASK_GSCL, 12, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_cam0, "sclk_cam0", "div_cam0",
+			SRC_MASK_GSCL, 16, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_cam1, "sclk_cam1", "div_cam1",
+			SRC_MASK_GSCL, 20, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_gscl_wa, "sclk_gscl_wa", "div_gscl_wa",
+			SRC_MASK_GSCL, 24, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_gscl_wb, "sclk_gscl_wb", "div_gscl_wb",
+			SRC_MASK_GSCL, 28, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_fimd1, "sclk_fimd1", "div_fimd1",
+			SRC_MASK_DISP1_0, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_mipi1, "sclk_mipi1", "div_mipi1",
+			SRC_MASK_DISP1_0, 12, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_dp, "sclk_dp", "div_dp",
+			SRC_MASK_DISP1_0, 16, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_hdmi, "sclk_hdmi", "mout_hdmi",
+			SRC_MASK_DISP1_0, 20, 0, 0),
+	GATE(sclk_audio0, "sclk_audio0", "div_audio0",
+			SRC_MASK_MAU, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_mmc0, "sclk_mmc0", "div_mmc_pre0",
+			SRC_MASK_FSYS, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_mmc1, "sclk_mmc1", "div_mmc_pre1",
+			SRC_MASK_FSYS, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_mmc2, "sclk_mmc2", "div_mmc_pre2",
+			SRC_MASK_FSYS, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_mmc3, "sclk_mmc3", "div_mmc_pre3",
+			SRC_MASK_FSYS, 12, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_sata, "sclk_sata", "div_sata",
+			SRC_MASK_FSYS, 24, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_usb3, "sclk_usb3", "div_usb3",
+			SRC_MASK_FSYS, 28, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_jpeg, "sclk_jpeg", "div_jpeg",
+			SRC_MASK_GEN, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_uart0, "sclk_uart0", "div_uart0",
+			SRC_MASK_PERIC0, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_uart1, "sclk_uart1", "div_uart1",
+			SRC_MASK_PERIC0, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_uart2, "sclk_uart2", "div_uart2",
+			SRC_MASK_PERIC0, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_uart3, "sclk_uart3", "div_uart3",
+			SRC_MASK_PERIC0, 12, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_pwm, "sclk_pwm", "div_pwm",
+			SRC_MASK_PERIC0, 24, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_audio1, "sclk_audio1", "div_audio1",
+			SRC_MASK_PERIC1, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_audio2, "sclk_audio2", "div_audio2",
+			SRC_MASK_PERIC1, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_spdif, "sclk_spdif", "mout_spdif",
+			SRC_MASK_PERIC1, 4, 0, 0),
+	GATE(sclk_spi0, "sclk_spi0", "div_spi_pre0",
+			SRC_MASK_PERIC1, 16, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_spi1, "sclk_spi1", "div_spi_pre1",
+			SRC_MASK_PERIC1, 20, CLK_SET_RATE_PARENT, 0),
+	GATE(sclk_spi2, "sclk_spi2", "div_spi_pre2",
+			SRC_MASK_PERIC1, 24, CLK_SET_RATE_PARENT, 0),
+	GATE(fimd1, "fimd1", "aclk200", GATE_IP_DISP1, 0, 0, 0),
+	GATE(mie1, "mie1", "aclk200", GATE_IP_DISP1, 1, 0, 0),
+	GATE(dsim0, "dsim0", "aclk200", GATE_IP_DISP1, 3, 0, 0),
+	GATE(dp, "dp", "aclk200", GATE_IP_DISP1, 4, 0, 0),
+	GATE(mixer, "mixer", "aclk200", GATE_IP_DISP1, 5, 0, 0),
+	GATE(hdmi, "hdmi", "aclk200", GATE_IP_DISP1, 6, 0, 0),
+};
+
+static __initdata struct of_device_id ext_clk_match[] = {
+	{ .compatible = "samsung,clock-xxti", .data = (void *)0, },
+	{ },
+};
+
+/* register exynox5250 clocks */
+void __init exynos5250_clk_init(struct device_node *np)
+{
+	void __iomem *reg_base;
+	struct clk *apll, *mpll, *epll, *vpll, *bpll, *gpll, *cpll;
+
+	if (np) {
+		reg_base = of_iomap(np, 0);
+		if (!reg_base)
+			panic("%s: failed to map registers\n", __func__);
+	} else {
+		panic("%s: unable to determine soc\n", __func__);
+	}
+
+	samsung_clk_init(np, reg_base, nr_clks,
+			exynos5250_clk_regs, ARRAY_SIZE(exynos5250_clk_regs),
+			NULL, 0);
+	samsung_clk_of_register_fixed_ext(exynos5250_fixed_rate_ext_clks,
+			ARRAY_SIZE(exynos5250_fixed_rate_ext_clks),
+			ext_clk_match);
+
+	apll = samsung_clk_register_pll35xx("fout_apll", "fin_pll",
+			reg_base + 0x100);
+	mpll = samsung_clk_register_pll35xx("fout_mpll", "fin_pll",
+			reg_base + 0x4100);
+	bpll = samsung_clk_register_pll35xx("fout_bpll", "fin_pll",
+			reg_base + 0x20110);
+	gpll = samsung_clk_register_pll35xx("fout_gpll", "fin_pll",
+			reg_base + 0x10150);
+	cpll = samsung_clk_register_pll35xx("fout_cpll", "fin_pll",
+			reg_base + 0x10120);
+	epll = samsung_clk_register_pll36xx("fout_epll", "fin_pll",
+			reg_base + 0x10130);
+	vpll = samsung_clk_register_pll36xx("fout_vpll", "mout_vpllsrc",
+			reg_base + 0x10140);
+
+	samsung_clk_register_fixed_rate(exynos5250_fixed_rate_clks,
+			ARRAY_SIZE(exynos5250_fixed_rate_clks));
+	samsung_clk_register_fixed_factor(exynos5250_fixed_factor_clks,
+			ARRAY_SIZE(exynos5250_fixed_factor_clks));
+	samsung_clk_register_mux(exynos5250_mux_clks,
+			ARRAY_SIZE(exynos5250_mux_clks));
+	samsung_clk_register_div(exynos5250_div_clks,
+			ARRAY_SIZE(exynos5250_div_clks));
+	samsung_clk_register_gate(exynos5250_gate_clks,
+			ARRAY_SIZE(exynos5250_gate_clks));
+
+	pr_info("Exynos5250: clock setup completed, armclk=%ld\n",
+			_get_rate("armclk"));
+}
+CLK_OF_DECLARE(exynos5250_clk, "samsung,exynos5250-clock", exynos5250_clk_init);
diff --git a/drivers/clk/samsung/clk-exynos5440.c b/drivers/clk/samsung/clk-exynos5440.c
new file mode 100644
index 000000000000..7d5434167a96
--- /dev/null
+++ b/drivers/clk/samsung/clk-exynos5440.c
@@ -0,0 +1,138 @@
+/*
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ * Author: Thomas Abraham <thomas.ab@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.
+ *
+ * Common Clock Framework support for Exynos5440 SoC.
+*/
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#include "clk.h"
+#include "clk-pll.h"
+
+#define CLKEN_OV_VAL		0xf8
+#define CPU_CLK_STATUS		0xfc
+#define MISC_DOUT1		0x558
+
+/*
+ * Let each supported clock get a unique id. This id is used to lookup the clock
+ * for device tree based platforms.
+ */
+enum exynos5440_clks {
+	none, xtal, arm_clk,
+
+	spi_baud = 16, pb0_250, pr0_250, pr1_250, b_250, b_125, b_200, sata,
+	usb, gmac0, cs250, pb0_250_o, pr0_250_o, pr1_250_o, b_250_o, b_125_o,
+	b_200_o, sata_o, usb_o, gmac0_o, cs250_o,
+
+	nr_clks,
+};
+
+/* parent clock name list */
+PNAME(mout_armclk_p)	= { "cplla", "cpllb" };
+PNAME(mout_spi_p)	= { "div125", "div200" };
+
+/* fixed rate clocks generated outside the soc */
+struct samsung_fixed_rate_clock exynos5440_fixed_rate_ext_clks[] __initdata = {
+	FRATE(none, "xtal", NULL, CLK_IS_ROOT, 0),
+};
+
+/* fixed rate clocks */
+struct samsung_fixed_rate_clock exynos5440_fixed_rate_clks[] __initdata = {
+	FRATE(none, "ppll", NULL, CLK_IS_ROOT, 1000000000),
+	FRATE(none, "usb_phy0", NULL, CLK_IS_ROOT, 60000000),
+	FRATE(none, "usb_phy1", NULL, CLK_IS_ROOT, 60000000),
+	FRATE(none, "usb_ohci12", NULL, CLK_IS_ROOT, 12000000),
+	FRATE(none, "usb_ohci48", NULL, CLK_IS_ROOT, 48000000),
+};
+
+/* fixed factor clocks */
+struct samsung_fixed_factor_clock exynos5440_fixed_factor_clks[] __initdata = {
+	FFACTOR(none, "div250", "ppll", 1, 4, 0),
+	FFACTOR(none, "div200", "ppll", 1, 5, 0),
+	FFACTOR(none, "div125", "div250", 1, 2, 0),
+};
+
+/* mux clocks */
+struct samsung_mux_clock exynos5440_mux_clks[] __initdata = {
+	MUX(none, "mout_spi", mout_spi_p, MISC_DOUT1, 5, 1),
+	MUX_A(arm_clk, "arm_clk", mout_armclk_p,
+			CPU_CLK_STATUS, 0, 1, "armclk"),
+};
+
+/* divider clocks */
+struct samsung_div_clock exynos5440_div_clks[] __initdata = {
+	DIV(spi_baud, "div_spi", "mout_spi", MISC_DOUT1, 3, 2),
+};
+
+/* gate clocks */
+struct samsung_gate_clock exynos5440_gate_clks[] __initdata = {
+	GATE(pb0_250, "pb0_250", "div250", CLKEN_OV_VAL, 3, 0, 0),
+	GATE(pr0_250, "pr0_250", "div250", CLKEN_OV_VAL, 4, 0, 0),
+	GATE(pr1_250, "pr1_250", "div250", CLKEN_OV_VAL, 5, 0, 0),
+	GATE(b_250, "b_250", "div250", CLKEN_OV_VAL, 9, 0, 0),
+	GATE(b_125, "b_125", "div125", CLKEN_OV_VAL, 10, 0, 0),
+	GATE(b_200, "b_200", "div200", CLKEN_OV_VAL, 11, 0, 0),
+	GATE(sata, "sata", "div200", CLKEN_OV_VAL, 12, 0, 0),
+	GATE(usb, "usb", "div200", CLKEN_OV_VAL, 13, 0, 0),
+	GATE(gmac0, "gmac0", "div200", CLKEN_OV_VAL, 14, 0, 0),
+	GATE(cs250, "cs250", "div250", CLKEN_OV_VAL, 19, 0, 0),
+	GATE(pb0_250_o, "pb0_250_o", "pb0_250", CLKEN_OV_VAL, 3, 0, 0),
+	GATE(pr0_250_o, "pr0_250_o", "pr0_250", CLKEN_OV_VAL, 4, 0, 0),
+	GATE(pr1_250_o, "pr1_250_o", "pr1_250", CLKEN_OV_VAL, 5, 0, 0),
+	GATE(b_250_o, "b_250_o", "b_250", CLKEN_OV_VAL, 9, 0, 0),
+	GATE(b_125_o, "b_125_o", "b_125", CLKEN_OV_VAL, 10, 0, 0),
+	GATE(b_200_o, "b_200_o", "b_200", CLKEN_OV_VAL, 11, 0, 0),
+	GATE(sata_o, "sata_o", "sata", CLKEN_OV_VAL, 12, 0, 0),
+	GATE(usb_o, "usb_o", "usb", CLKEN_OV_VAL, 13, 0, 0),
+	GATE(gmac0_o, "gmac0_o", "gmac", CLKEN_OV_VAL, 14, 0, 0),
+	GATE(cs250_o, "cs250_o", "cs250", CLKEN_OV_VAL, 19, 0, 0),
+};
+
+static __initdata struct of_device_id ext_clk_match[] = {
+	{ .compatible = "samsung,clock-xtal", .data = (void *)0, },
+	{},
+};
+
+/* register exynos5440 clocks */
+void __init exynos5440_clk_init(struct device_node *np)
+{
+	void __iomem *reg_base;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base) {
+		pr_err("%s: failed to map clock controller registers,"
+			" aborting clock initialization\n", __func__);
+		return;
+	}
+
+	samsung_clk_init(np, reg_base, nr_clks, NULL, 0, NULL, 0);
+	samsung_clk_of_register_fixed_ext(exynos5440_fixed_rate_ext_clks,
+		ARRAY_SIZE(exynos5440_fixed_rate_ext_clks), ext_clk_match);
+
+	samsung_clk_register_pll2550x("cplla", "xtal", reg_base + 0x1c, 0x10);
+	samsung_clk_register_pll2550x("cpllb", "xtal", reg_base + 0x20, 0x10);
+
+	samsung_clk_register_fixed_rate(exynos5440_fixed_rate_clks,
+			ARRAY_SIZE(exynos5440_fixed_rate_clks));
+	samsung_clk_register_fixed_factor(exynos5440_fixed_factor_clks,
+			ARRAY_SIZE(exynos5440_fixed_factor_clks));
+	samsung_clk_register_mux(exynos5440_mux_clks,
+			ARRAY_SIZE(exynos5440_mux_clks));
+	samsung_clk_register_div(exynos5440_div_clks,
+			ARRAY_SIZE(exynos5440_div_clks));
+	samsung_clk_register_gate(exynos5440_gate_clks,
+			ARRAY_SIZE(exynos5440_gate_clks));
+
+	pr_info("Exynos5440: arm_clk = %ldHz\n", _get_rate("armclk"));
+	pr_info("exynos5440 clock initialization complete\n");
+}
+CLK_OF_DECLARE(exynos5440_clk, "samsung,exynos5440-clock", exynos5440_clk_init);
diff --git a/drivers/clk/samsung/clk-pll.c b/drivers/clk/samsung/clk-pll.c
new file mode 100644
index 000000000000..89135f6be116
--- /dev/null
+++ b/drivers/clk/samsung/clk-pll.c
@@ -0,0 +1,419 @@
+/*
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ * Copyright (c) 2013 Linaro 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.
+ *
+ * This file contains the utility functions to register the pll clocks.
+*/
+
+#include <linux/errno.h>
+#include "clk.h"
+#include "clk-pll.h"
+
+/*
+ * PLL35xx Clock Type
+ */
+
+#define PLL35XX_MDIV_MASK       (0x3FF)
+#define PLL35XX_PDIV_MASK       (0x3F)
+#define PLL35XX_SDIV_MASK       (0x7)
+#define PLL35XX_MDIV_SHIFT      (16)
+#define PLL35XX_PDIV_SHIFT      (8)
+#define PLL35XX_SDIV_SHIFT      (0)
+
+struct samsung_clk_pll35xx {
+	struct clk_hw		hw;
+	const void __iomem	*con_reg;
+};
+
+#define to_clk_pll35xx(_hw) container_of(_hw, struct samsung_clk_pll35xx, hw)
+
+static unsigned long samsung_pll35xx_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct samsung_clk_pll35xx *pll = to_clk_pll35xx(hw);
+	u32 mdiv, pdiv, sdiv, pll_con;
+	u64 fvco = parent_rate;
+
+	pll_con = __raw_readl(pll->con_reg);
+	mdiv = (pll_con >> PLL35XX_MDIV_SHIFT) & PLL35XX_MDIV_MASK;
+	pdiv = (pll_con >> PLL35XX_PDIV_SHIFT) & PLL35XX_PDIV_MASK;
+	sdiv = (pll_con >> PLL35XX_SDIV_SHIFT) & PLL35XX_SDIV_MASK;
+
+	fvco *= mdiv;
+	do_div(fvco, (pdiv << sdiv));
+
+	return (unsigned long)fvco;
+}
+
+static const struct clk_ops samsung_pll35xx_clk_ops = {
+	.recalc_rate = samsung_pll35xx_recalc_rate,
+};
+
+struct clk * __init samsung_clk_register_pll35xx(const char *name,
+			const char *pname, const void __iomem *con_reg)
+{
+	struct samsung_clk_pll35xx *pll;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
+	if (!pll) {
+		pr_err("%s: could not allocate pll clk %s\n", __func__, name);
+		return NULL;
+	}
+
+	init.name = name;
+	init.ops = &samsung_pll35xx_clk_ops;
+	init.flags = CLK_GET_RATE_NOCACHE;
+	init.parent_names = &pname;
+	init.num_parents = 1;
+
+	pll->hw.init = &init;
+	pll->con_reg = con_reg;
+
+	clk = clk_register(NULL, &pll->hw);
+	if (IS_ERR(clk)) {
+		pr_err("%s: failed to register pll clock %s\n", __func__,
+				name);
+		kfree(pll);
+	}
+
+	if (clk_register_clkdev(clk, name, NULL))
+		pr_err("%s: failed to register lookup for %s", __func__, name);
+
+	return clk;
+}
+
+/*
+ * PLL36xx Clock Type
+ */
+
+#define PLL36XX_KDIV_MASK	(0xFFFF)
+#define PLL36XX_MDIV_MASK	(0x1FF)
+#define PLL36XX_PDIV_MASK	(0x3F)
+#define PLL36XX_SDIV_MASK	(0x7)
+#define PLL36XX_MDIV_SHIFT	(16)
+#define PLL36XX_PDIV_SHIFT	(8)
+#define PLL36XX_SDIV_SHIFT	(0)
+
+struct samsung_clk_pll36xx {
+	struct clk_hw		hw;
+	const void __iomem	*con_reg;
+};
+
+#define to_clk_pll36xx(_hw) container_of(_hw, struct samsung_clk_pll36xx, hw)
+
+static unsigned long samsung_pll36xx_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct samsung_clk_pll36xx *pll = to_clk_pll36xx(hw);
+	u32 mdiv, pdiv, sdiv, kdiv, pll_con0, pll_con1;
+	u64 fvco = parent_rate;
+
+	pll_con0 = __raw_readl(pll->con_reg);
+	pll_con1 = __raw_readl(pll->con_reg + 4);
+	mdiv = (pll_con0 >> PLL36XX_MDIV_SHIFT) & PLL36XX_MDIV_MASK;
+	pdiv = (pll_con0 >> PLL36XX_PDIV_SHIFT) & PLL36XX_PDIV_MASK;
+	sdiv = (pll_con0 >> PLL36XX_SDIV_SHIFT) & PLL36XX_SDIV_MASK;
+	kdiv = pll_con1 & PLL36XX_KDIV_MASK;
+
+	fvco *= (mdiv << 16) + kdiv;
+	do_div(fvco, (pdiv << sdiv));
+	fvco >>= 16;
+
+	return (unsigned long)fvco;
+}
+
+static const struct clk_ops samsung_pll36xx_clk_ops = {
+	.recalc_rate = samsung_pll36xx_recalc_rate,
+};
+
+struct clk * __init samsung_clk_register_pll36xx(const char *name,
+			const char *pname, const void __iomem *con_reg)
+{
+	struct samsung_clk_pll36xx *pll;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
+	if (!pll) {
+		pr_err("%s: could not allocate pll clk %s\n", __func__, name);
+		return NULL;
+	}
+
+	init.name = name;
+	init.ops = &samsung_pll36xx_clk_ops;
+	init.flags = CLK_GET_RATE_NOCACHE;
+	init.parent_names = &pname;
+	init.num_parents = 1;
+
+	pll->hw.init = &init;
+	pll->con_reg = con_reg;
+
+	clk = clk_register(NULL, &pll->hw);
+	if (IS_ERR(clk)) {
+		pr_err("%s: failed to register pll clock %s\n", __func__,
+				name);
+		kfree(pll);
+	}
+
+	if (clk_register_clkdev(clk, name, NULL))
+		pr_err("%s: failed to register lookup for %s", __func__, name);
+
+	return clk;
+}
+
+/*
+ * PLL45xx Clock Type
+ */
+
+#define PLL45XX_MDIV_MASK	(0x3FF)
+#define PLL45XX_PDIV_MASK	(0x3F)
+#define PLL45XX_SDIV_MASK	(0x7)
+#define PLL45XX_MDIV_SHIFT	(16)
+#define PLL45XX_PDIV_SHIFT	(8)
+#define PLL45XX_SDIV_SHIFT	(0)
+
+struct samsung_clk_pll45xx {
+	struct clk_hw		hw;
+	enum pll45xx_type	type;
+	const void __iomem	*con_reg;
+};
+
+#define to_clk_pll45xx(_hw) container_of(_hw, struct samsung_clk_pll45xx, hw)
+
+static unsigned long samsung_pll45xx_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct samsung_clk_pll45xx *pll = to_clk_pll45xx(hw);
+	u32 mdiv, pdiv, sdiv, pll_con;
+	u64 fvco = parent_rate;
+
+	pll_con = __raw_readl(pll->con_reg);
+	mdiv = (pll_con >> PLL45XX_MDIV_SHIFT) & PLL45XX_MDIV_MASK;
+	pdiv = (pll_con >> PLL45XX_PDIV_SHIFT) & PLL45XX_PDIV_MASK;
+	sdiv = (pll_con >> PLL45XX_SDIV_SHIFT) & PLL45XX_SDIV_MASK;
+
+	if (pll->type == pll_4508)
+		sdiv = sdiv - 1;
+
+	fvco *= mdiv;
+	do_div(fvco, (pdiv << sdiv));
+
+	return (unsigned long)fvco;
+}
+
+static const struct clk_ops samsung_pll45xx_clk_ops = {
+	.recalc_rate = samsung_pll45xx_recalc_rate,
+};
+
+struct clk * __init samsung_clk_register_pll45xx(const char *name,
+			const char *pname, const void __iomem *con_reg,
+			enum pll45xx_type type)
+{
+	struct samsung_clk_pll45xx *pll;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
+	if (!pll) {
+		pr_err("%s: could not allocate pll clk %s\n", __func__, name);
+		return NULL;
+	}
+
+	init.name = name;
+	init.ops = &samsung_pll45xx_clk_ops;
+	init.flags = CLK_GET_RATE_NOCACHE;
+	init.parent_names = &pname;
+	init.num_parents = 1;
+
+	pll->hw.init = &init;
+	pll->con_reg = con_reg;
+	pll->type = type;
+
+	clk = clk_register(NULL, &pll->hw);
+	if (IS_ERR(clk)) {
+		pr_err("%s: failed to register pll clock %s\n", __func__,
+				name);
+		kfree(pll);
+	}
+
+	if (clk_register_clkdev(clk, name, NULL))
+		pr_err("%s: failed to register lookup for %s", __func__, name);
+
+	return clk;
+}
+
+/*
+ * PLL46xx Clock Type
+ */
+
+#define PLL46XX_MDIV_MASK	(0x1FF)
+#define PLL46XX_PDIV_MASK	(0x3F)
+#define PLL46XX_SDIV_MASK	(0x7)
+#define PLL46XX_MDIV_SHIFT	(16)
+#define PLL46XX_PDIV_SHIFT	(8)
+#define PLL46XX_SDIV_SHIFT	(0)
+
+#define PLL46XX_KDIV_MASK	(0xFFFF)
+#define PLL4650C_KDIV_MASK	(0xFFF)
+#define PLL46XX_KDIV_SHIFT	(0)
+
+struct samsung_clk_pll46xx {
+	struct clk_hw		hw;
+	enum pll46xx_type	type;
+	const void __iomem	*con_reg;
+};
+
+#define to_clk_pll46xx(_hw) container_of(_hw, struct samsung_clk_pll46xx, hw)
+
+static unsigned long samsung_pll46xx_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct samsung_clk_pll46xx *pll = to_clk_pll46xx(hw);
+	u32 mdiv, pdiv, sdiv, kdiv, pll_con0, pll_con1, shift;
+	u64 fvco = parent_rate;
+
+	pll_con0 = __raw_readl(pll->con_reg);
+	pll_con1 = __raw_readl(pll->con_reg + 4);
+	mdiv = (pll_con0 >> PLL46XX_MDIV_SHIFT) & PLL46XX_MDIV_MASK;
+	pdiv = (pll_con0 >> PLL46XX_PDIV_SHIFT) & PLL46XX_PDIV_MASK;
+	sdiv = (pll_con0 >> PLL46XX_SDIV_SHIFT) & PLL46XX_SDIV_MASK;
+	kdiv = pll->type == pll_4650c ? pll_con1 & PLL4650C_KDIV_MASK :
+					pll_con1 & PLL46XX_KDIV_MASK;
+
+	shift = pll->type == pll_4600 ? 16 : 10;
+	fvco *= (mdiv << shift) + kdiv;
+	do_div(fvco, (pdiv << sdiv));
+	fvco >>= shift;
+
+	return (unsigned long)fvco;
+}
+
+static const struct clk_ops samsung_pll46xx_clk_ops = {
+	.recalc_rate = samsung_pll46xx_recalc_rate,
+};
+
+struct clk * __init samsung_clk_register_pll46xx(const char *name,
+			const char *pname, const void __iomem *con_reg,
+			enum pll46xx_type type)
+{
+	struct samsung_clk_pll46xx *pll;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
+	if (!pll) {
+		pr_err("%s: could not allocate pll clk %s\n", __func__, name);
+		return NULL;
+	}
+
+	init.name = name;
+	init.ops = &samsung_pll46xx_clk_ops;
+	init.flags = CLK_GET_RATE_NOCACHE;
+	init.parent_names = &pname;
+	init.num_parents = 1;
+
+	pll->hw.init = &init;
+	pll->con_reg = con_reg;
+	pll->type = type;
+
+	clk = clk_register(NULL, &pll->hw);
+	if (IS_ERR(clk)) {
+		pr_err("%s: failed to register pll clock %s\n", __func__,
+				name);
+		kfree(pll);
+	}
+
+	if (clk_register_clkdev(clk, name, NULL))
+		pr_err("%s: failed to register lookup for %s", __func__, name);
+
+	return clk;
+}
+
+/*
+ * PLL2550x Clock Type
+ */
+
+#define PLL2550X_R_MASK       (0x1)
+#define PLL2550X_P_MASK       (0x3F)
+#define PLL2550X_M_MASK       (0x3FF)
+#define PLL2550X_S_MASK       (0x7)
+#define PLL2550X_R_SHIFT      (20)
+#define PLL2550X_P_SHIFT      (14)
+#define PLL2550X_M_SHIFT      (4)
+#define PLL2550X_S_SHIFT      (0)
+
+struct samsung_clk_pll2550x {
+	struct clk_hw		hw;
+	const void __iomem	*reg_base;
+	unsigned long		offset;
+};
+
+#define to_clk_pll2550x(_hw) container_of(_hw, struct samsung_clk_pll2550x, hw)
+
+static unsigned long samsung_pll2550x_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct samsung_clk_pll2550x *pll = to_clk_pll2550x(hw);
+	u32 r, p, m, s, pll_stat;
+	u64 fvco = parent_rate;
+
+	pll_stat = __raw_readl(pll->reg_base + pll->offset * 3);
+	r = (pll_stat >> PLL2550X_R_SHIFT) & PLL2550X_R_MASK;
+	if (!r)
+		return 0;
+	p = (pll_stat >> PLL2550X_P_SHIFT) & PLL2550X_P_MASK;
+	m = (pll_stat >> PLL2550X_M_SHIFT) & PLL2550X_M_MASK;
+	s = (pll_stat >> PLL2550X_S_SHIFT) & PLL2550X_S_MASK;
+
+	fvco *= m;
+	do_div(fvco, (p << s));
+
+	return (unsigned long)fvco;
+}
+
+static const struct clk_ops samsung_pll2550x_clk_ops = {
+	.recalc_rate = samsung_pll2550x_recalc_rate,
+};
+
+struct clk * __init samsung_clk_register_pll2550x(const char *name,
+			const char *pname, const void __iomem *reg_base,
+			const unsigned long offset)
+{
+	struct samsung_clk_pll2550x *pll;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
+	if (!pll) {
+		pr_err("%s: could not allocate pll clk %s\n", __func__, name);
+		return NULL;
+	}
+
+	init.name = name;
+	init.ops = &samsung_pll2550x_clk_ops;
+	init.flags = CLK_GET_RATE_NOCACHE;
+	init.parent_names = &pname;
+	init.num_parents = 1;
+
+	pll->hw.init = &init;
+	pll->reg_base = reg_base;
+	pll->offset = offset;
+
+	clk = clk_register(NULL, &pll->hw);
+	if (IS_ERR(clk)) {
+		pr_err("%s: failed to register pll clock %s\n", __func__,
+				name);
+		kfree(pll);
+	}
+
+	if (clk_register_clkdev(clk, name, NULL))
+		pr_err("%s: failed to register lookup for %s", __func__, name);
+
+	return clk;
+}
diff --git a/drivers/clk/samsung/clk-pll.h b/drivers/clk/samsung/clk-pll.h
new file mode 100644
index 000000000000..f33786e9a78b
--- /dev/null
+++ b/drivers/clk/samsung/clk-pll.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ * Copyright (c) 2013 Linaro 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.
+ *
+ * Common Clock Framework support for all PLL's in Samsung platforms
+*/
+
+#ifndef __SAMSUNG_CLK_PLL_H
+#define __SAMSUNG_CLK_PLL_H
+
+enum pll45xx_type {
+	pll_4500,
+	pll_4502,
+	pll_4508
+};
+
+enum pll46xx_type {
+	pll_4600,
+	pll_4650,
+	pll_4650c,
+};
+
+extern struct clk * __init samsung_clk_register_pll35xx(const char *name,
+			const char *pname, const void __iomem *con_reg);
+extern struct clk * __init samsung_clk_register_pll36xx(const char *name,
+			const char *pname, const void __iomem *con_reg);
+extern struct clk * __init samsung_clk_register_pll45xx(const char *name,
+			const char *pname, const void __iomem *con_reg,
+			enum pll45xx_type type);
+extern struct clk * __init samsung_clk_register_pll46xx(const char *name,
+			const char *pname, const void __iomem *con_reg,
+			enum pll46xx_type type);
+extern struct clk * __init samsung_clk_register_pll2550x(const char *name,
+			const char *pname, const void __iomem *reg_base,
+			const unsigned long offset);
+
+#endif /* __SAMSUNG_CLK_PLL_H */
diff --git a/drivers/clk/samsung/clk.c b/drivers/clk/samsung/clk.c
new file mode 100644
index 000000000000..cd3c40ab50f3
--- /dev/null
+++ b/drivers/clk/samsung/clk.c
@@ -0,0 +1,320 @@
+/*
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ * Copyright (c) 2013 Linaro Ltd.
+ * Author: Thomas Abraham <thomas.ab@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.
+ *
+ * This file includes utility functions to register clocks to common
+ * clock framework for Samsung platforms.
+*/
+
+#include <linux/syscore_ops.h>
+#include "clk.h"
+
+static DEFINE_SPINLOCK(lock);
+static struct clk **clk_table;
+static void __iomem *reg_base;
+#ifdef CONFIG_OF
+static struct clk_onecell_data clk_data;
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+static struct samsung_clk_reg_dump *reg_dump;
+static unsigned long nr_reg_dump;
+
+static int samsung_clk_suspend(void)
+{
+	struct samsung_clk_reg_dump *rd = reg_dump;
+	unsigned long i;
+
+	for (i = 0; i < nr_reg_dump; i++, rd++)
+		rd->value = __raw_readl(reg_base + rd->offset);
+
+	return 0;
+}
+
+static void samsung_clk_resume(void)
+{
+	struct samsung_clk_reg_dump *rd = reg_dump;
+	unsigned long i;
+
+	for (i = 0; i < nr_reg_dump; i++, rd++)
+		__raw_writel(rd->value, reg_base + rd->offset);
+}
+
+static struct syscore_ops samsung_clk_syscore_ops = {
+	.suspend	= samsung_clk_suspend,
+	.resume		= samsung_clk_resume,
+};
+#endif /* CONFIG_PM_SLEEP */
+
+/* setup the essentials required to support clock lookup using ccf */
+void __init samsung_clk_init(struct device_node *np, void __iomem *base,
+		unsigned long nr_clks, unsigned long *rdump,
+		unsigned long nr_rdump, unsigned long *soc_rdump,
+		unsigned long nr_soc_rdump)
+{
+	reg_base = base;
+
+#ifdef CONFIG_PM_SLEEP
+	if (rdump && nr_rdump) {
+		unsigned int idx;
+		reg_dump = kzalloc(sizeof(struct samsung_clk_reg_dump)
+				* (nr_rdump + nr_soc_rdump), GFP_KERNEL);
+		if (!reg_dump) {
+			pr_err("%s: memory alloc for register dump failed\n",
+					__func__);
+			return;
+		}
+
+		for (idx = 0; idx < nr_rdump; idx++)
+			reg_dump[idx].offset = rdump[idx];
+		for (idx = 0; idx < nr_soc_rdump; idx++)
+			reg_dump[nr_rdump + idx].offset = soc_rdump[idx];
+		nr_reg_dump = nr_rdump + nr_soc_rdump;
+		register_syscore_ops(&samsung_clk_syscore_ops);
+	}
+#endif
+
+	clk_table = kzalloc(sizeof(struct clk *) * nr_clks, GFP_KERNEL);
+	if (!clk_table)
+		panic("could not allocate clock lookup table\n");
+
+	if (!np)
+		return;
+
+#ifdef CONFIG_OF
+	clk_data.clks = clk_table;
+	clk_data.clk_num = nr_clks;
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+#endif
+}
+
+/* add a clock instance to the clock lookup table used for dt based lookup */
+void samsung_clk_add_lookup(struct clk *clk, unsigned int id)
+{
+	if (clk_table && id)
+		clk_table[id] = clk;
+}
+
+/* register a list of aliases */
+void __init samsung_clk_register_alias(struct samsung_clock_alias *list,
+					unsigned int nr_clk)
+{
+	struct clk *clk;
+	unsigned int idx, ret;
+
+	if (!clk_table) {
+		pr_err("%s: clock table missing\n", __func__);
+		return;
+	}
+
+	for (idx = 0; idx < nr_clk; idx++, list++) {
+		if (!list->id) {
+			pr_err("%s: clock id missing for index %d\n", __func__,
+				idx);
+			continue;
+		}
+
+		clk = clk_table[list->id];
+		if (!clk) {
+			pr_err("%s: failed to find clock %d\n", __func__,
+				list->id);
+			continue;
+		}
+
+		ret = clk_register_clkdev(clk, list->alias, list->dev_name);
+		if (ret)
+			pr_err("%s: failed to register lookup %s\n",
+					__func__, list->alias);
+	}
+}
+
+/* register a list of fixed clocks */
+void __init samsung_clk_register_fixed_rate(
+		struct samsung_fixed_rate_clock *list, unsigned int nr_clk)
+{
+	struct clk *clk;
+	unsigned int idx, ret;
+
+	for (idx = 0; idx < nr_clk; idx++, list++) {
+		clk = clk_register_fixed_rate(NULL, list->name,
+			list->parent_name, list->flags, list->fixed_rate);
+		if (IS_ERR(clk)) {
+			pr_err("%s: failed to register clock %s\n", __func__,
+				list->name);
+			continue;
+		}
+
+		samsung_clk_add_lookup(clk, list->id);
+
+		/*
+		 * Unconditionally add a clock lookup for the fixed rate clocks.
+		 * There are not many of these on any of Samsung platforms.
+		 */
+		ret = clk_register_clkdev(clk, list->name, NULL);
+		if (ret)
+			pr_err("%s: failed to register clock lookup for %s",
+				__func__, list->name);
+	}
+}
+
+/* register a list of fixed factor clocks */
+void __init samsung_clk_register_fixed_factor(
+		struct samsung_fixed_factor_clock *list, unsigned int nr_clk)
+{
+	struct clk *clk;
+	unsigned int idx;
+
+	for (idx = 0; idx < nr_clk; idx++, list++) {
+		clk = clk_register_fixed_factor(NULL, list->name,
+			list->parent_name, list->flags, list->mult, list->div);
+		if (IS_ERR(clk)) {
+			pr_err("%s: failed to register clock %s\n", __func__,
+				list->name);
+			continue;
+		}
+
+		samsung_clk_add_lookup(clk, list->id);
+	}
+}
+
+/* register a list of mux clocks */
+void __init samsung_clk_register_mux(struct samsung_mux_clock *list,
+					unsigned int nr_clk)
+{
+	struct clk *clk;
+	unsigned int idx, ret;
+
+	for (idx = 0; idx < nr_clk; idx++, list++) {
+		clk = clk_register_mux(NULL, list->name, list->parent_names,
+			list->num_parents, list->flags, reg_base + list->offset,
+			list->shift, list->width, list->mux_flags, &lock);
+		if (IS_ERR(clk)) {
+			pr_err("%s: failed to register clock %s\n", __func__,
+				list->name);
+			continue;
+		}
+
+		samsung_clk_add_lookup(clk, list->id);
+
+		/* register a clock lookup only if a clock alias is specified */
+		if (list->alias) {
+			ret = clk_register_clkdev(clk, list->alias,
+						list->dev_name);
+			if (ret)
+				pr_err("%s: failed to register lookup %s\n",
+						__func__, list->alias);
+		}
+	}
+}
+
+/* register a list of div clocks */
+void __init samsung_clk_register_div(struct samsung_div_clock *list,
+					unsigned int nr_clk)
+{
+	struct clk *clk;
+	unsigned int idx, ret;
+
+	for (idx = 0; idx < nr_clk; idx++, list++) {
+		if (list->table)
+			clk = clk_register_divider_table(NULL, list->name,
+					list->parent_name, list->flags,
+					reg_base + list->offset, list->shift,
+					list->width, list->div_flags,
+					list->table, &lock);
+		else
+			clk = clk_register_divider(NULL, list->name,
+					list->parent_name, list->flags,
+					reg_base + list->offset, list->shift,
+					list->width, list->div_flags, &lock);
+		if (IS_ERR(clk)) {
+			pr_err("%s: failed to register clock %s\n", __func__,
+				list->name);
+			continue;
+		}
+
+		samsung_clk_add_lookup(clk, list->id);
+
+		/* register a clock lookup only if a clock alias is specified */
+		if (list->alias) {
+			ret = clk_register_clkdev(clk, list->alias,
+						list->dev_name);
+			if (ret)
+				pr_err("%s: failed to register lookup %s\n",
+						__func__, list->alias);
+		}
+	}
+}
+
+/* register a list of gate clocks */
+void __init samsung_clk_register_gate(struct samsung_gate_clock *list,
+						unsigned int nr_clk)
+{
+	struct clk *clk;
+	unsigned int idx, ret;
+
+	for (idx = 0; idx < nr_clk; idx++, list++) {
+		clk = clk_register_gate(NULL, list->name, list->parent_name,
+				list->flags, reg_base + list->offset,
+				list->bit_idx, list->gate_flags, &lock);
+		if (IS_ERR(clk)) {
+			pr_err("%s: failed to register clock %s\n", __func__,
+				list->name);
+			continue;
+		}
+
+		/* register a clock lookup only if a clock alias is specified */
+		if (list->alias) {
+			ret = clk_register_clkdev(clk, list->alias,
+							list->dev_name);
+			if (ret)
+				pr_err("%s: failed to register lookup %s\n",
+					__func__, list->alias);
+		}
+
+		samsung_clk_add_lookup(clk, list->id);
+	}
+}
+
+/*
+ * obtain the clock speed of all external fixed clock sources from device
+ * tree and register it
+ */
+#ifdef CONFIG_OF
+void __init samsung_clk_of_register_fixed_ext(
+			struct samsung_fixed_rate_clock *fixed_rate_clk,
+			unsigned int nr_fixed_rate_clk,
+			struct of_device_id *clk_matches)
+{
+	const struct of_device_id *match;
+	struct device_node *np;
+	u32 freq;
+
+	for_each_matching_node_and_match(np, clk_matches, &match) {
+		if (of_property_read_u32(np, "clock-frequency", &freq))
+			continue;
+		fixed_rate_clk[(u32)match->data].fixed_rate = freq;
+	}
+	samsung_clk_register_fixed_rate(fixed_rate_clk, nr_fixed_rate_clk);
+}
+#endif
+
+/* utility function to get the rate of a specified clock */
+unsigned long _get_rate(const char *clk_name)
+{
+	struct clk *clk;
+	unsigned long rate;
+
+	clk = clk_get(NULL, clk_name);
+	if (IS_ERR(clk)) {
+		pr_err("%s: could not find clock %s\n", __func__, clk_name);
+		return 0;
+	}
+	rate = clk_get_rate(clk);
+	clk_put(clk);
+	return rate;
+}
diff --git a/drivers/clk/samsung/clk.h b/drivers/clk/samsung/clk.h
new file mode 100644
index 000000000000..e4ad6ea9aa76
--- /dev/null
+++ b/drivers/clk/samsung/clk.h
@@ -0,0 +1,287 @@
+/*
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ * Copyright (c) 2013 Linaro Ltd.
+ * Author: Thomas Abraham <thomas.ab@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.
+ *
+ * Common Clock Framework support for all Samsung platforms
+*/
+
+#ifndef __SAMSUNG_CLK_H
+#define __SAMSUNG_CLK_H
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/io.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+/**
+ * struct samsung_clock_alias: information about mux clock
+ * @id: platform specific id of the clock.
+ * @dev_name: name of the device to which this clock belongs.
+ * @alias: optional clock alias name to be assigned to this clock.
+ */
+struct samsung_clock_alias {
+	unsigned int		id;
+	const char		*dev_name;
+	const char		*alias;
+};
+
+#define ALIAS(_id, dname, a)	\
+	{							\
+		.id		= _id,				\
+		.dev_name	= dname,			\
+		.alias		= a,				\
+	}
+
+/**
+ * struct samsung_fixed_rate_clock: information about fixed-rate clock
+ * @id: platform specific id of the clock.
+ * @name: name of this fixed-rate clock.
+ * @parent_name: optional parent clock name.
+ * @flags: optional fixed-rate clock flags.
+ * @fixed-rate: fixed clock rate of this clock.
+ */
+struct samsung_fixed_rate_clock {
+	unsigned int		id;
+	char			*name;
+	const char		*parent_name;
+	unsigned long		flags;
+	unsigned long		fixed_rate;
+};
+
+#define FRATE(_id, cname, pname, f, frate)		\
+	{						\
+		.id		= _id,			\
+		.name		= cname,		\
+		.parent_name	= pname,		\
+		.flags		= f,			\
+		.fixed_rate	= frate,		\
+	}
+
+/*
+ * struct samsung_fixed_factor_clock: information about fixed-factor clock
+ * @id: platform specific id of the clock.
+ * @name: name of this fixed-factor clock.
+ * @parent_name: parent clock name.
+ * @mult: fixed multiplication factor.
+ * @div: fixed division factor.
+ * @flags: optional fixed-factor clock flags.
+ */
+struct samsung_fixed_factor_clock {
+	unsigned int		id;
+	char			*name;
+	const char		*parent_name;
+	unsigned long		mult;
+	unsigned long		div;
+	unsigned long		flags;
+};
+
+#define FFACTOR(_id, cname, pname, m, d, f)		\
+	{						\
+		.id		= _id,			\
+		.name		= cname,		\
+		.parent_name	= pname,		\
+		.mult		= m,			\
+		.div		= d,			\
+		.flags		= f,			\
+	}
+
+/**
+ * struct samsung_mux_clock: information about mux clock
+ * @id: platform specific id of the clock.
+ * @dev_name: name of the device to which this clock belongs.
+ * @name: name of this mux clock.
+ * @parent_names: array of pointer to parent clock names.
+ * @num_parents: number of parents listed in @parent_names.
+ * @flags: optional flags for basic clock.
+ * @offset: offset of the register for configuring the mux.
+ * @shift: starting bit location of the mux control bit-field in @reg.
+ * @width: width of the mux control bit-field in @reg.
+ * @mux_flags: flags for mux-type clock.
+ * @alias: optional clock alias name to be assigned to this clock.
+ */
+struct samsung_mux_clock {
+	unsigned int		id;
+	const char		*dev_name;
+	const char		*name;
+	const char		**parent_names;
+	u8			num_parents;
+	unsigned long		flags;
+	unsigned long		offset;
+	u8			shift;
+	u8			width;
+	u8			mux_flags;
+	const char		*alias;
+};
+
+#define __MUX(_id, dname, cname, pnames, o, s, w, f, mf, a)	\
+	{							\
+		.id		= _id,				\
+		.dev_name	= dname,			\
+		.name		= cname,			\
+		.parent_names	= pnames,			\
+		.num_parents	= ARRAY_SIZE(pnames),		\
+		.flags		= f,				\
+		.offset		= o,				\
+		.shift		= s,				\
+		.width		= w,				\
+		.mux_flags	= mf,				\
+		.alias		= a,				\
+	}
+
+#define MUX(_id, cname, pnames, o, s, w)			\
+	__MUX(_id, NULL, cname, pnames, o, s, w, 0, 0, NULL)
+
+#define MUX_A(_id, cname, pnames, o, s, w, a)			\
+	__MUX(_id, NULL, cname, pnames, o, s, w, 0, 0, a)
+
+#define MUX_F(_id, cname, pnames, o, s, w, f, mf)		\
+	__MUX(_id, NULL, cname, pnames, o, s, w, f, mf, NULL)
+
+/**
+ * @id: platform specific id of the clock.
+ * struct samsung_div_clock: information about div clock
+ * @dev_name: name of the device to which this clock belongs.
+ * @name: name of this div clock.
+ * @parent_name: name of the parent clock.
+ * @flags: optional flags for basic clock.
+ * @offset: offset of the register for configuring the div.
+ * @shift: starting bit location of the div control bit-field in @reg.
+ * @div_flags: flags for div-type clock.
+ * @alias: optional clock alias name to be assigned to this clock.
+ */
+struct samsung_div_clock {
+	unsigned int		id;
+	const char		*dev_name;
+	const char		*name;
+	const char		*parent_name;
+	unsigned long		flags;
+	unsigned long		offset;
+	u8			shift;
+	u8			width;
+	u8			div_flags;
+	const char		*alias;
+	struct clk_div_table	*table;
+};
+
+#define __DIV(_id, dname, cname, pname, o, s, w, f, df, a, t)	\
+	{							\
+		.id		= _id,				\
+		.dev_name	= dname,			\
+		.name		= cname,			\
+		.parent_name	= pname,			\
+		.flags		= f,				\
+		.offset		= o,				\
+		.shift		= s,				\
+		.width		= w,				\
+		.div_flags	= df,				\
+		.alias		= a,				\
+		.table		= t,				\
+	}
+
+#define DIV(_id, cname, pname, o, s, w)				\
+	__DIV(_id, NULL, cname, pname, o, s, w, 0, 0, NULL, NULL)
+
+#define DIV_A(_id, cname, pname, o, s, w, a)			\
+	__DIV(_id, NULL, cname, pname, o, s, w, 0, 0, a, NULL)
+
+#define DIV_F(_id, cname, pname, o, s, w, f, df)		\
+	__DIV(_id, NULL, cname, pname, o, s, w, f, df, NULL, NULL)
+
+#define DIV_T(_id, cname, pname, o, s, w, t)			\
+	__DIV(_id, NULL, cname, pname, o, s, w, 0, 0, NULL, t)
+
+/**
+ * struct samsung_gate_clock: information about gate clock
+ * @id: platform specific id of the clock.
+ * @dev_name: name of the device to which this clock belongs.
+ * @name: name of this gate clock.
+ * @parent_name: name of the parent clock.
+ * @flags: optional flags for basic clock.
+ * @offset: offset of the register for configuring the gate.
+ * @bit_idx: bit index of the gate control bit-field in @reg.
+ * @gate_flags: flags for gate-type clock.
+ * @alias: optional clock alias name to be assigned to this clock.
+ */
+struct samsung_gate_clock {
+	unsigned int		id;
+	const char		*dev_name;
+	const char		*name;
+	const char		*parent_name;
+	unsigned long		flags;
+	unsigned long		offset;
+	u8			bit_idx;
+	u8			gate_flags;
+	const char		*alias;
+};
+
+#define __GATE(_id, dname, cname, pname, o, b, f, gf, a)	\
+	{							\
+		.id		= _id,				\
+		.dev_name	= dname,			\
+		.name		= cname,			\
+		.parent_name	= pname,			\
+		.flags		= f,				\
+		.offset		= o,				\
+		.bit_idx	= b,				\
+		.gate_flags	= gf,				\
+		.alias		= a,				\
+	}
+
+#define GATE(_id, cname, pname, o, b, f, gf)			\
+	__GATE(_id, NULL, cname, pname, o, b, f, gf, NULL)
+
+#define GATE_A(_id, cname, pname, o, b, f, gf, a)		\
+	__GATE(_id, NULL, cname, pname, o, b, f, gf, a)
+
+#define GATE_D(_id, dname, cname, pname, o, b, f, gf)		\
+	__GATE(_id, dname, cname, pname, o, b, f, gf, NULL)
+
+#define GATE_DA(_id, dname, cname, pname, o, b, f, gf, a)	\
+	__GATE(_id, dname, cname, pname, o, b, f, gf, a)
+
+#define PNAME(x) static const char *x[] __initdata
+
+/**
+ * struct samsung_clk_reg_dump: register dump of clock controller registers.
+ * @offset: clock register offset from the controller base address.
+ * @value: the value to be register at offset.
+ */
+struct samsung_clk_reg_dump {
+	u32	offset;
+	u32	value;
+};
+
+extern void __init samsung_clk_init(struct device_node *np, void __iomem *base,
+		unsigned long nr_clks, unsigned long *rdump,
+		unsigned long nr_rdump, unsigned long *soc_rdump,
+		unsigned long nr_soc_rdump);
+extern void __init samsung_clk_of_register_fixed_ext(
+		struct samsung_fixed_rate_clock *fixed_rate_clk,
+		unsigned int nr_fixed_rate_clk,
+		struct of_device_id *clk_matches);
+
+extern void samsung_clk_add_lookup(struct clk *clk, unsigned int id);
+
+extern void samsung_clk_register_alias(struct samsung_clock_alias *list,
+		unsigned int nr_clk);
+extern void __init samsung_clk_register_fixed_rate(
+		struct samsung_fixed_rate_clock *clk_list, unsigned int nr_clk);
+extern void __init samsung_clk_register_fixed_factor(
+		struct samsung_fixed_factor_clock *list, unsigned int nr_clk);
+extern void __init samsung_clk_register_mux(struct samsung_mux_clock *clk_list,
+		unsigned int nr_clk);
+extern void __init samsung_clk_register_div(struct samsung_div_clock *clk_list,
+		unsigned int nr_clk);
+extern void __init samsung_clk_register_gate(
+		struct samsung_gate_clock *clk_list, unsigned int nr_clk);
+
+extern unsigned long _get_rate(const char *clk_name);
+
+#endif /* __SAMSUNG_CLK_H */
diff --git a/drivers/clk/socfpga/clk.c b/drivers/clk/socfpga/clk.c
index 2c855a6394ff..bd11315cf5ab 100644
--- a/drivers/clk/socfpga/clk.c
+++ b/drivers/clk/socfpga/clk.c
@@ -1,5 +1,6 @@
 /*
- *  Copyright (C) 2012 Altera Corporation <www.altera.com>
+ *  Copyright 2011-2012 Calxeda, Inc.
+ *  Copyright (C) 2012-2013 Altera Corporation <www.altera.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
@@ -11,41 +12,161 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
+ * Based from clk-highbank.c
+ *
  * 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/clk.h>
 #include <linux/clkdev.h>
 #include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/of.h>
 
-#define SOCFPGA_OSC1_CLK	10000000
-#define SOCFPGA_MPU_CLK		800000000
-#define SOCFPGA_MAIN_QSPI_CLK		432000000
-#define SOCFPGA_MAIN_NAND_SDMMC_CLK	250000000
-#define SOCFPGA_S2F_USR_CLK		125000000
+/* Clock Manager offsets */
+#define CLKMGR_CTRL    0x0
+#define CLKMGR_BYPASS 0x4
 
-void __init socfpga_init_clocks(void)
+/* Clock bypass bits */
+#define MAINPLL_BYPASS (1<<0)
+#define SDRAMPLL_BYPASS (1<<1)
+#define SDRAMPLL_SRC_BYPASS (1<<2)
+#define PERPLL_BYPASS (1<<3)
+#define PERPLL_SRC_BYPASS (1<<4)
+
+#define SOCFPGA_PLL_BG_PWRDWN		0
+#define SOCFPGA_PLL_EXT_ENA		1
+#define SOCFPGA_PLL_PWR_DOWN		2
+#define SOCFPGA_PLL_DIVF_MASK		0x0000FFF8
+#define SOCFPGA_PLL_DIVF_SHIFT	3
+#define SOCFPGA_PLL_DIVQ_MASK		0x003F0000
+#define SOCFPGA_PLL_DIVQ_SHIFT	16
+
+extern void __iomem *clk_mgr_base_addr;
+
+struct socfpga_clk {
+	struct clk_gate hw;
+	char *parent_name;
+	char *clk_name;
+	u32 fixed_div;
+};
+#define to_socfpga_clk(p) container_of(p, struct socfpga_clk, hw.hw)
+
+static unsigned long clk_pll_recalc_rate(struct clk_hw *hwclk,
+					 unsigned long parent_rate)
 {
+	struct socfpga_clk *socfpgaclk = to_socfpga_clk(hwclk);
+	unsigned long divf, divq, vco_freq, reg;
+	unsigned long bypass;
+
+	reg = readl(socfpgaclk->hw.reg);
+	bypass = readl(clk_mgr_base_addr + CLKMGR_BYPASS);
+	if (bypass & MAINPLL_BYPASS)
+		return parent_rate;
+
+	divf = (reg & SOCFPGA_PLL_DIVF_MASK) >> SOCFPGA_PLL_DIVF_SHIFT;
+	divq = (reg & SOCFPGA_PLL_DIVQ_MASK) >> SOCFPGA_PLL_DIVQ_SHIFT;
+	vco_freq = parent_rate * (divf + 1);
+	return vco_freq / (1 + divq);
+}
+
+
+static struct clk_ops clk_pll_ops = {
+	.recalc_rate = clk_pll_recalc_rate,
+};
+
+static unsigned long clk_periclk_recalc_rate(struct clk_hw *hwclk,
+					     unsigned long parent_rate)
+{
+	struct socfpga_clk *socfpgaclk = to_socfpga_clk(hwclk);
+	u32 div;
+
+	if (socfpgaclk->fixed_div)
+		div = socfpgaclk->fixed_div;
+	else
+		div = ((readl(socfpgaclk->hw.reg) & 0x1ff) + 1);
+
+	return parent_rate / div;
+}
+
+static const struct clk_ops periclk_ops = {
+	.recalc_rate = clk_periclk_recalc_rate,
+};
+
+static __init struct clk *socfpga_clk_init(struct device_node *node,
+	const struct clk_ops *ops)
+{
+	u32 reg;
 	struct clk *clk;
+	struct socfpga_clk *socfpga_clk;
+	const char *clk_name = node->name;
+	const char *parent_name;
+	struct clk_init_data init;
+	int rc;
+	u32 fixed_div;
+
+	rc = of_property_read_u32(node, "reg", &reg);
+	if (WARN_ON(rc))
+		return NULL;
+
+	socfpga_clk = kzalloc(sizeof(*socfpga_clk), GFP_KERNEL);
+	if (WARN_ON(!socfpga_clk))
+		return NULL;
+
+	socfpga_clk->hw.reg = clk_mgr_base_addr + reg;
+
+	rc = of_property_read_u32(node, "fixed-divider", &fixed_div);
+	if (rc)
+		socfpga_clk->fixed_div = 0;
+	else
+		socfpga_clk->fixed_div = fixed_div;
+
+	of_property_read_string(node, "clock-output-names", &clk_name);
+
+	init.name = clk_name;
+	init.ops = ops;
+	init.flags = 0;
+	parent_name = of_clk_get_parent_name(node, 0);
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
 
-	clk = clk_register_fixed_rate(NULL, "osc1_clk", NULL, CLK_IS_ROOT, SOCFPGA_OSC1_CLK);
-	clk_register_clkdev(clk, "osc1_clk", NULL);
+	socfpga_clk->hw.hw.init = &init;
 
-	clk = clk_register_fixed_rate(NULL, "mpu_clk", NULL, CLK_IS_ROOT, SOCFPGA_MPU_CLK);
-	clk_register_clkdev(clk, "mpu_clk", NULL);
+	if (strcmp(clk_name, "main_pll") || strcmp(clk_name, "periph_pll") ||
+			strcmp(clk_name, "sdram_pll")) {
+		socfpga_clk->hw.bit_idx = SOCFPGA_PLL_EXT_ENA;
+		clk_pll_ops.enable = clk_gate_ops.enable;
+		clk_pll_ops.disable = clk_gate_ops.disable;
+	}
 
-	clk = clk_register_fixed_rate(NULL, "main_clk", NULL, CLK_IS_ROOT, SOCFPGA_MPU_CLK/2);
-	clk_register_clkdev(clk, "main_clk", NULL);
+	clk = clk_register(NULL, &socfpga_clk->hw.hw);
+	if (WARN_ON(IS_ERR(clk))) {
+		kfree(socfpga_clk);
+		return NULL;
+	}
+	rc = of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	return clk;
+}
 
-	clk = clk_register_fixed_rate(NULL, "dbg_base_clk", NULL, CLK_IS_ROOT, SOCFPGA_MPU_CLK/2);
-	clk_register_clkdev(clk, "dbg_base_clk", NULL);
+static void __init socfpga_pll_init(struct device_node *node)
+{
+	socfpga_clk_init(node, &clk_pll_ops);
+}
+CLK_OF_DECLARE(socfpga_pll, "altr,socfpga-pll-clock", socfpga_pll_init);
 
-	clk = clk_register_fixed_rate(NULL, "main_qspi_clk", NULL, CLK_IS_ROOT, SOCFPGA_MAIN_QSPI_CLK);
-	clk_register_clkdev(clk, "main_qspi_clk", NULL);
+static void __init socfpga_periph_init(struct device_node *node)
+{
+	socfpga_clk_init(node, &periclk_ops);
+}
+CLK_OF_DECLARE(socfpga_periph, "altr,socfpga-perip-clk", socfpga_periph_init);
 
-	clk = clk_register_fixed_rate(NULL, "main_nand_sdmmc_clk", NULL, CLK_IS_ROOT, SOCFPGA_MAIN_NAND_SDMMC_CLK);
-	clk_register_clkdev(clk, "main_nand_sdmmc_clk", NULL);
+void __init socfpga_init_clocks(void)
+{
+	struct clk *clk;
+	int ret;
 
-	clk = clk_register_fixed_rate(NULL, "s2f_usr_clk", NULL, CLK_IS_ROOT, SOCFPGA_S2F_USR_CLK);
-	clk_register_clkdev(clk, "s2f_usr_clk", NULL);
+	clk = clk_register_fixed_factor(NULL, "smp_twd", "mpuclk", 0, 1, 4);
+	ret = clk_register_clkdev(clk, NULL, "smp_twd");
+	if (ret)
+		pr_err("smp_twd alias not registered\n");
 }
diff --git a/drivers/clk/spear/spear1310_clock.c b/drivers/clk/spear/spear1310_clock.c
index ed9af4278619..aedbbe12f321 100644
--- a/drivers/clk/spear/spear1310_clock.c
+++ b/drivers/clk/spear/spear1310_clock.c
@@ -17,12 +17,10 @@
 #include <linux/io.h>
 #include <linux/of_platform.h>
 #include <linux/spinlock_types.h>
-#include <mach/spear.h>
 #include "clk.h"
 
-#define VA_SPEAR1310_RAS_BASE			IOMEM(UL(0xFA400000))
 /* PLL related registers and bit values */
-#define SPEAR1310_PLL_CFG			(VA_MISC_BASE + 0x210)
+#define SPEAR1310_PLL_CFG			(misc_base + 0x210)
 	/* PLL_CFG bit values */
 	#define SPEAR1310_CLCD_SYNT_CLK_MASK		1
 	#define SPEAR1310_CLCD_SYNT_CLK_SHIFT		31
@@ -35,15 +33,15 @@
 	#define SPEAR1310_PLL2_CLK_SHIFT		22
 	#define SPEAR1310_PLL1_CLK_SHIFT		20
 
-#define SPEAR1310_PLL1_CTR			(VA_MISC_BASE + 0x214)
-#define SPEAR1310_PLL1_FRQ			(VA_MISC_BASE + 0x218)
-#define SPEAR1310_PLL2_CTR			(VA_MISC_BASE + 0x220)
-#define SPEAR1310_PLL2_FRQ			(VA_MISC_BASE + 0x224)
-#define SPEAR1310_PLL3_CTR			(VA_MISC_BASE + 0x22C)
-#define SPEAR1310_PLL3_FRQ			(VA_MISC_BASE + 0x230)
-#define SPEAR1310_PLL4_CTR			(VA_MISC_BASE + 0x238)
-#define SPEAR1310_PLL4_FRQ			(VA_MISC_BASE + 0x23C)
-#define SPEAR1310_PERIP_CLK_CFG			(VA_MISC_BASE + 0x244)
+#define SPEAR1310_PLL1_CTR			(misc_base + 0x214)
+#define SPEAR1310_PLL1_FRQ			(misc_base + 0x218)
+#define SPEAR1310_PLL2_CTR			(misc_base + 0x220)
+#define SPEAR1310_PLL2_FRQ			(misc_base + 0x224)
+#define SPEAR1310_PLL3_CTR			(misc_base + 0x22C)
+#define SPEAR1310_PLL3_FRQ			(misc_base + 0x230)
+#define SPEAR1310_PLL4_CTR			(misc_base + 0x238)
+#define SPEAR1310_PLL4_FRQ			(misc_base + 0x23C)
+#define SPEAR1310_PERIP_CLK_CFG			(misc_base + 0x244)
 	/* PERIP_CLK_CFG bit values */
 	#define SPEAR1310_GPT_OSC24_VAL			0
 	#define SPEAR1310_GPT_APB_VAL			1
@@ -65,7 +63,7 @@
 	#define SPEAR1310_C3_CLK_MASK			1
 	#define SPEAR1310_C3_CLK_SHIFT			1
 
-#define SPEAR1310_GMAC_CLK_CFG			(VA_MISC_BASE + 0x248)
+#define SPEAR1310_GMAC_CLK_CFG			(misc_base + 0x248)
 	#define SPEAR1310_GMAC_PHY_IF_SEL_MASK		3
 	#define SPEAR1310_GMAC_PHY_IF_SEL_SHIFT		4
 	#define SPEAR1310_GMAC_PHY_CLK_MASK		1
@@ -73,7 +71,7 @@
 	#define SPEAR1310_GMAC_PHY_INPUT_CLK_MASK	2
 	#define SPEAR1310_GMAC_PHY_INPUT_CLK_SHIFT	1
 
-#define SPEAR1310_I2S_CLK_CFG			(VA_MISC_BASE + 0x24C)
+#define SPEAR1310_I2S_CLK_CFG			(misc_base + 0x24C)
 	/* I2S_CLK_CFG register mask */
 	#define SPEAR1310_I2S_SCLK_X_MASK		0x1F
 	#define SPEAR1310_I2S_SCLK_X_SHIFT		27
@@ -91,21 +89,21 @@
 	#define SPEAR1310_I2S_SRC_CLK_MASK		2
 	#define SPEAR1310_I2S_SRC_CLK_SHIFT		0
 
-#define SPEAR1310_C3_CLK_SYNT			(VA_MISC_BASE + 0x250)
-#define SPEAR1310_UART_CLK_SYNT			(VA_MISC_BASE + 0x254)
-#define SPEAR1310_GMAC_CLK_SYNT			(VA_MISC_BASE + 0x258)
-#define SPEAR1310_SDHCI_CLK_SYNT		(VA_MISC_BASE + 0x25C)
-#define SPEAR1310_CFXD_CLK_SYNT			(VA_MISC_BASE + 0x260)
-#define SPEAR1310_ADC_CLK_SYNT			(VA_MISC_BASE + 0x264)
-#define SPEAR1310_AMBA_CLK_SYNT			(VA_MISC_BASE + 0x268)
-#define SPEAR1310_CLCD_CLK_SYNT			(VA_MISC_BASE + 0x270)
-#define SPEAR1310_RAS_CLK_SYNT0			(VA_MISC_BASE + 0x280)
-#define SPEAR1310_RAS_CLK_SYNT1			(VA_MISC_BASE + 0x288)
-#define SPEAR1310_RAS_CLK_SYNT2			(VA_MISC_BASE + 0x290)
-#define SPEAR1310_RAS_CLK_SYNT3			(VA_MISC_BASE + 0x298)
+#define SPEAR1310_C3_CLK_SYNT			(misc_base + 0x250)
+#define SPEAR1310_UART_CLK_SYNT			(misc_base + 0x254)
+#define SPEAR1310_GMAC_CLK_SYNT			(misc_base + 0x258)
+#define SPEAR1310_SDHCI_CLK_SYNT		(misc_base + 0x25C)
+#define SPEAR1310_CFXD_CLK_SYNT			(misc_base + 0x260)
+#define SPEAR1310_ADC_CLK_SYNT			(misc_base + 0x264)
+#define SPEAR1310_AMBA_CLK_SYNT			(misc_base + 0x268)
+#define SPEAR1310_CLCD_CLK_SYNT			(misc_base + 0x270)
+#define SPEAR1310_RAS_CLK_SYNT0			(misc_base + 0x280)
+#define SPEAR1310_RAS_CLK_SYNT1			(misc_base + 0x288)
+#define SPEAR1310_RAS_CLK_SYNT2			(misc_base + 0x290)
+#define SPEAR1310_RAS_CLK_SYNT3			(misc_base + 0x298)
 	/* Check Fractional synthesizer reg masks */
 
-#define SPEAR1310_PERIP1_CLK_ENB		(VA_MISC_BASE + 0x300)
+#define SPEAR1310_PERIP1_CLK_ENB		(misc_base + 0x300)
 	/* PERIP1_CLK_ENB register masks */
 	#define SPEAR1310_RTC_CLK_ENB			31
 	#define SPEAR1310_ADC_CLK_ENB			30
@@ -138,7 +136,7 @@
 	#define SPEAR1310_SYSROM_CLK_ENB		1
 	#define SPEAR1310_BUS_CLK_ENB			0
 
-#define SPEAR1310_PERIP2_CLK_ENB		(VA_MISC_BASE + 0x304)
+#define SPEAR1310_PERIP2_CLK_ENB		(misc_base + 0x304)
 	/* PERIP2_CLK_ENB register masks */
 	#define SPEAR1310_THSENS_CLK_ENB		8
 	#define SPEAR1310_I2S_REF_PAD_CLK_ENB		7
@@ -150,7 +148,7 @@
 	#define SPEAR1310_DDR_CORE_CLK_ENB		1
 	#define SPEAR1310_DDR_CTRL_CLK_ENB		0
 
-#define SPEAR1310_RAS_CLK_ENB			(VA_MISC_BASE + 0x310)
+#define SPEAR1310_RAS_CLK_ENB			(misc_base + 0x310)
 	/* RAS_CLK_ENB register masks */
 	#define SPEAR1310_SYNT3_CLK_ENB			17
 	#define SPEAR1310_SYNT2_CLK_ENB			16
@@ -172,7 +170,7 @@
 	#define SPEAR1310_ACLK_CLK_ENB			0
 
 /* RAS Area Control Register */
-#define SPEAR1310_RAS_CTRL_REG0			(VA_SPEAR1310_RAS_BASE + 0x000)
+#define SPEAR1310_RAS_CTRL_REG0			(ras_base + 0x000)
 	#define SPEAR1310_SSP1_CLK_MASK			3
 	#define SPEAR1310_SSP1_CLK_SHIFT		26
 	#define SPEAR1310_TDM_CLK_MASK			1
@@ -197,12 +195,12 @@
 	#define SPEAR1310_PCI_CLK_MASK			1
 	#define SPEAR1310_PCI_CLK_SHIFT			0
 
-#define SPEAR1310_RAS_CTRL_REG1			(VA_SPEAR1310_RAS_BASE + 0x004)
+#define SPEAR1310_RAS_CTRL_REG1			(ras_base + 0x004)
 	#define SPEAR1310_PHY_CLK_MASK			0x3
 	#define SPEAR1310_RMII_PHY_CLK_SHIFT		0
 	#define SPEAR1310_SMII_RGMII_PHY_CLK_SHIFT	2
 
-#define SPEAR1310_RAS_SW_CLK_CTRL		(VA_SPEAR1310_RAS_BASE + 0x0148)
+#define SPEAR1310_RAS_SW_CLK_CTRL		(ras_base + 0x0148)
 	#define SPEAR1310_CAN1_CLK_ENB			25
 	#define SPEAR1310_CAN0_CLK_ENB			24
 	#define SPEAR1310_GPT64_CLK_ENB			23
@@ -385,7 +383,7 @@ static const char *ssp1_parents[] = { "ras_apb_clk", "gen_syn1_clk",
 static const char *pci_parents[] = { "ras_pll3_clk", "gen_syn2_clk", };
 static const char *tdm_parents[] = { "ras_pll3_clk", "gen_syn1_clk", };
 
-void __init spear1310_clk_init(void)
+void __init spear1310_clk_init(void __iomem *misc_base, void __iomem *ras_base)
 {
 	struct clk *clk, *clk1;
 
diff --git a/drivers/clk/spear/spear1340_clock.c b/drivers/clk/spear/spear1340_clock.c
index 35e7e2698e10..9d0b3949db30 100644
--- a/drivers/clk/spear/spear1340_clock.c
+++ b/drivers/clk/spear/spear1340_clock.c
@@ -17,18 +17,17 @@
 #include <linux/io.h>
 #include <linux/of_platform.h>
 #include <linux/spinlock_types.h>
-#include <mach/spear.h>
 #include "clk.h"
 
 /* Clock Configuration Registers */
-#define SPEAR1340_SYS_CLK_CTRL			(VA_MISC_BASE + 0x200)
+#define SPEAR1340_SYS_CLK_CTRL			(misc_base + 0x200)
 	#define SPEAR1340_HCLK_SRC_SEL_SHIFT	27
 	#define SPEAR1340_HCLK_SRC_SEL_MASK	1
 	#define SPEAR1340_SCLK_SRC_SEL_SHIFT	23
 	#define SPEAR1340_SCLK_SRC_SEL_MASK	3
 
 /* PLL related registers and bit values */
-#define SPEAR1340_PLL_CFG			(VA_MISC_BASE + 0x210)
+#define SPEAR1340_PLL_CFG			(misc_base + 0x210)
 	/* PLL_CFG bit values */
 	#define SPEAR1340_CLCD_SYNT_CLK_MASK		1
 	#define SPEAR1340_CLCD_SYNT_CLK_SHIFT		31
@@ -40,15 +39,15 @@
 	#define SPEAR1340_PLL2_CLK_SHIFT		22
 	#define SPEAR1340_PLL1_CLK_SHIFT		20
 
-#define SPEAR1340_PLL1_CTR			(VA_MISC_BASE + 0x214)
-#define SPEAR1340_PLL1_FRQ			(VA_MISC_BASE + 0x218)
-#define SPEAR1340_PLL2_CTR			(VA_MISC_BASE + 0x220)
-#define SPEAR1340_PLL2_FRQ			(VA_MISC_BASE + 0x224)
-#define SPEAR1340_PLL3_CTR			(VA_MISC_BASE + 0x22C)
-#define SPEAR1340_PLL3_FRQ			(VA_MISC_BASE + 0x230)
-#define SPEAR1340_PLL4_CTR			(VA_MISC_BASE + 0x238)
-#define SPEAR1340_PLL4_FRQ			(VA_MISC_BASE + 0x23C)
-#define SPEAR1340_PERIP_CLK_CFG			(VA_MISC_BASE + 0x244)
+#define SPEAR1340_PLL1_CTR			(misc_base + 0x214)
+#define SPEAR1340_PLL1_FRQ			(misc_base + 0x218)
+#define SPEAR1340_PLL2_CTR			(misc_base + 0x220)
+#define SPEAR1340_PLL2_FRQ			(misc_base + 0x224)
+#define SPEAR1340_PLL3_CTR			(misc_base + 0x22C)
+#define SPEAR1340_PLL3_FRQ			(misc_base + 0x230)
+#define SPEAR1340_PLL4_CTR			(misc_base + 0x238)
+#define SPEAR1340_PLL4_FRQ			(misc_base + 0x23C)
+#define SPEAR1340_PERIP_CLK_CFG			(misc_base + 0x244)
 	/* PERIP_CLK_CFG bit values */
 	#define SPEAR1340_SPDIF_CLK_MASK		1
 	#define SPEAR1340_SPDIF_OUT_CLK_SHIFT		15
@@ -66,13 +65,13 @@
 	#define SPEAR1340_C3_CLK_MASK			1
 	#define SPEAR1340_C3_CLK_SHIFT			1
 
-#define SPEAR1340_GMAC_CLK_CFG			(VA_MISC_BASE + 0x248)
+#define SPEAR1340_GMAC_CLK_CFG			(misc_base + 0x248)
 	#define SPEAR1340_GMAC_PHY_CLK_MASK		1
 	#define SPEAR1340_GMAC_PHY_CLK_SHIFT		2
 	#define SPEAR1340_GMAC_PHY_INPUT_CLK_MASK	2
 	#define SPEAR1340_GMAC_PHY_INPUT_CLK_SHIFT	0
 
-#define SPEAR1340_I2S_CLK_CFG			(VA_MISC_BASE + 0x24C)
+#define SPEAR1340_I2S_CLK_CFG			(misc_base + 0x24C)
 	/* I2S_CLK_CFG register mask */
 	#define SPEAR1340_I2S_SCLK_X_MASK		0x1F
 	#define SPEAR1340_I2S_SCLK_X_SHIFT		27
@@ -90,21 +89,21 @@
 	#define SPEAR1340_I2S_SRC_CLK_MASK		2
 	#define SPEAR1340_I2S_SRC_CLK_SHIFT		0
 
-#define SPEAR1340_C3_CLK_SYNT			(VA_MISC_BASE + 0x250)
-#define SPEAR1340_UART0_CLK_SYNT		(VA_MISC_BASE + 0x254)
-#define SPEAR1340_UART1_CLK_SYNT		(VA_MISC_BASE + 0x258)
-#define SPEAR1340_GMAC_CLK_SYNT			(VA_MISC_BASE + 0x25C)
-#define SPEAR1340_SDHCI_CLK_SYNT		(VA_MISC_BASE + 0x260)
-#define SPEAR1340_CFXD_CLK_SYNT			(VA_MISC_BASE + 0x264)
-#define SPEAR1340_ADC_CLK_SYNT			(VA_MISC_BASE + 0x270)
-#define SPEAR1340_AMBA_CLK_SYNT			(VA_MISC_BASE + 0x274)
-#define SPEAR1340_CLCD_CLK_SYNT			(VA_MISC_BASE + 0x27C)
-#define SPEAR1340_SYS_CLK_SYNT			(VA_MISC_BASE + 0x284)
-#define SPEAR1340_GEN_CLK_SYNT0			(VA_MISC_BASE + 0x28C)
-#define SPEAR1340_GEN_CLK_SYNT1			(VA_MISC_BASE + 0x294)
-#define SPEAR1340_GEN_CLK_SYNT2			(VA_MISC_BASE + 0x29C)
-#define SPEAR1340_GEN_CLK_SYNT3			(VA_MISC_BASE + 0x304)
-#define SPEAR1340_PERIP1_CLK_ENB		(VA_MISC_BASE + 0x30C)
+#define SPEAR1340_C3_CLK_SYNT			(misc_base + 0x250)
+#define SPEAR1340_UART0_CLK_SYNT		(misc_base + 0x254)
+#define SPEAR1340_UART1_CLK_SYNT		(misc_base + 0x258)
+#define SPEAR1340_GMAC_CLK_SYNT			(misc_base + 0x25C)
+#define SPEAR1340_SDHCI_CLK_SYNT		(misc_base + 0x260)
+#define SPEAR1340_CFXD_CLK_SYNT			(misc_base + 0x264)
+#define SPEAR1340_ADC_CLK_SYNT			(misc_base + 0x270)
+#define SPEAR1340_AMBA_CLK_SYNT			(misc_base + 0x274)
+#define SPEAR1340_CLCD_CLK_SYNT			(misc_base + 0x27C)
+#define SPEAR1340_SYS_CLK_SYNT			(misc_base + 0x284)
+#define SPEAR1340_GEN_CLK_SYNT0			(misc_base + 0x28C)
+#define SPEAR1340_GEN_CLK_SYNT1			(misc_base + 0x294)
+#define SPEAR1340_GEN_CLK_SYNT2			(misc_base + 0x29C)
+#define SPEAR1340_GEN_CLK_SYNT3			(misc_base + 0x304)
+#define SPEAR1340_PERIP1_CLK_ENB		(misc_base + 0x30C)
 	#define SPEAR1340_RTC_CLK_ENB			31
 	#define SPEAR1340_ADC_CLK_ENB			30
 	#define SPEAR1340_C3_CLK_ENB			29
@@ -133,7 +132,7 @@
 	#define SPEAR1340_SYSROM_CLK_ENB		1
 	#define SPEAR1340_BUS_CLK_ENB			0
 
-#define SPEAR1340_PERIP2_CLK_ENB		(VA_MISC_BASE + 0x310)
+#define SPEAR1340_PERIP2_CLK_ENB		(misc_base + 0x310)
 	#define SPEAR1340_THSENS_CLK_ENB		8
 	#define SPEAR1340_I2S_REF_PAD_CLK_ENB		7
 	#define SPEAR1340_ACP_CLK_ENB			6
@@ -144,7 +143,7 @@
 	#define SPEAR1340_DDR_CORE_CLK_ENB		1
 	#define SPEAR1340_DDR_CTRL_CLK_ENB		0
 
-#define SPEAR1340_PERIP3_CLK_ENB		(VA_MISC_BASE + 0x314)
+#define SPEAR1340_PERIP3_CLK_ENB		(misc_base + 0x314)
 	#define SPEAR1340_PLGPIO_CLK_ENB		18
 	#define SPEAR1340_VIDEO_DEC_CLK_ENB		16
 	#define SPEAR1340_VIDEO_ENC_CLK_ENB		15
@@ -441,7 +440,7 @@ static const char *gen_synth0_1_parents[] = { "vco1div4_clk", "vco3div2_clk",
 static const char *gen_synth2_3_parents[] = { "vco1div4_clk", "vco2div2_clk",
 	"pll2_clk", };
 
-void __init spear1340_clk_init(void)
+void __init spear1340_clk_init(void __iomem *misc_base)
 {
 	struct clk *clk, *clk1;
 
diff --git a/drivers/clk/spear/spear3xx_clock.c b/drivers/clk/spear/spear3xx_clock.c
index 33d3ac588da7..f9ec43fd1320 100644
--- a/drivers/clk/spear/spear3xx_clock.c
+++ b/drivers/clk/spear/spear3xx_clock.c
@@ -15,21 +15,20 @@
 #include <linux/io.h>
 #include <linux/of_platform.h>
 #include <linux/spinlock_types.h>
-#include <mach/misc_regs.h>
 #include "clk.h"
 
 static DEFINE_SPINLOCK(_lock);
 
-#define PLL1_CTR			(MISC_BASE + 0x008)
-#define PLL1_FRQ			(MISC_BASE + 0x00C)
-#define PLL2_CTR			(MISC_BASE + 0x014)
-#define PLL2_FRQ			(MISC_BASE + 0x018)
-#define PLL_CLK_CFG			(MISC_BASE + 0x020)
+#define PLL1_CTR			(misc_base + 0x008)
+#define PLL1_FRQ			(misc_base + 0x00C)
+#define PLL2_CTR			(misc_base + 0x014)
+#define PLL2_FRQ			(misc_base + 0x018)
+#define PLL_CLK_CFG			(misc_base + 0x020)
 	/* PLL_CLK_CFG register masks */
 	#define MCTR_CLK_SHIFT		28
 	#define MCTR_CLK_MASK		3
 
-#define CORE_CLK_CFG			(MISC_BASE + 0x024)
+#define CORE_CLK_CFG			(misc_base + 0x024)
 	/* CORE CLK CFG register masks */
 	#define GEN_SYNTH2_3_CLK_SHIFT	18
 	#define GEN_SYNTH2_3_CLK_MASK	1
@@ -39,7 +38,7 @@ static DEFINE_SPINLOCK(_lock);
 	#define PCLK_RATIO_SHIFT	8
 	#define PCLK_RATIO_MASK		2
 
-#define PERIP_CLK_CFG			(MISC_BASE + 0x028)
+#define PERIP_CLK_CFG			(misc_base + 0x028)
 	/* PERIP_CLK_CFG register masks */
 	#define UART_CLK_SHIFT		4
 	#define UART_CLK_MASK		1
@@ -50,7 +49,7 @@ static DEFINE_SPINLOCK(_lock);
 	#define GPT2_CLK_SHIFT		12
 	#define GPT_CLK_MASK		1
 
-#define PERIP1_CLK_ENB			(MISC_BASE + 0x02C)
+#define PERIP1_CLK_ENB			(misc_base + 0x02C)
 	/* PERIP1_CLK_ENB register masks */
 	#define UART_CLK_ENB		3
 	#define SSP_CLK_ENB		5
@@ -69,7 +68,7 @@ static DEFINE_SPINLOCK(_lock);
 	#define USBH_CLK_ENB		25
 	#define C3_CLK_ENB		31
 
-#define RAS_CLK_ENB			(MISC_BASE + 0x034)
+#define RAS_CLK_ENB			(misc_base + 0x034)
 	#define RAS_AHB_CLK_ENB		0
 	#define RAS_PLL1_CLK_ENB	1
 	#define RAS_APB_CLK_ENB		2
@@ -82,20 +81,20 @@ static DEFINE_SPINLOCK(_lock);
 	#define RAS_SYNT2_CLK_ENB	10
 	#define RAS_SYNT3_CLK_ENB	11
 
-#define PRSC0_CLK_CFG			(MISC_BASE + 0x044)
-#define PRSC1_CLK_CFG			(MISC_BASE + 0x048)
-#define PRSC2_CLK_CFG			(MISC_BASE + 0x04C)
-#define AMEM_CLK_CFG			(MISC_BASE + 0x050)
+#define PRSC0_CLK_CFG			(misc_base + 0x044)
+#define PRSC1_CLK_CFG			(misc_base + 0x048)
+#define PRSC2_CLK_CFG			(misc_base + 0x04C)
+#define AMEM_CLK_CFG			(misc_base + 0x050)
 	#define AMEM_CLK_ENB		0
 
-#define CLCD_CLK_SYNT			(MISC_BASE + 0x05C)
-#define FIRDA_CLK_SYNT			(MISC_BASE + 0x060)
-#define UART_CLK_SYNT			(MISC_BASE + 0x064)
-#define GMAC_CLK_SYNT			(MISC_BASE + 0x068)
-#define GEN0_CLK_SYNT			(MISC_BASE + 0x06C)
-#define GEN1_CLK_SYNT			(MISC_BASE + 0x070)
-#define GEN2_CLK_SYNT			(MISC_BASE + 0x074)
-#define GEN3_CLK_SYNT			(MISC_BASE + 0x078)
+#define CLCD_CLK_SYNT			(misc_base + 0x05C)
+#define FIRDA_CLK_SYNT			(misc_base + 0x060)
+#define UART_CLK_SYNT			(misc_base + 0x064)
+#define GMAC_CLK_SYNT			(misc_base + 0x068)
+#define GEN0_CLK_SYNT			(misc_base + 0x06C)
+#define GEN1_CLK_SYNT			(misc_base + 0x070)
+#define GEN2_CLK_SYNT			(misc_base + 0x074)
+#define GEN3_CLK_SYNT			(misc_base + 0x078)
 
 /* pll rate configuration table, in ascending order of rates */
 static struct pll_rate_tbl pll_rtbl[] = {
@@ -211,6 +210,17 @@ static inline void spear310_clk_init(void) { }
 
 /* array of all spear 320 clock lookups */
 #ifdef CONFIG_MACH_SPEAR320
+
+#define SPEAR320_CONTROL_REG		(soc_config_base + 0x0000)
+#define SPEAR320_EXT_CTRL_REG		(soc_config_base + 0x0018)
+
+	#define SPEAR320_UARTX_PCLK_MASK		0x1
+	#define SPEAR320_UART2_PCLK_SHIFT		8
+	#define SPEAR320_UART3_PCLK_SHIFT		9
+	#define SPEAR320_UART4_PCLK_SHIFT		10
+	#define SPEAR320_UART5_PCLK_SHIFT		11
+	#define SPEAR320_UART6_PCLK_SHIFT		12
+	#define SPEAR320_RS485_PCLK_SHIFT		13
 	#define SMII_PCLK_SHIFT				18
 	#define SMII_PCLK_MASK				2
 	#define SMII_PCLK_VAL_PAD			0x0
@@ -235,7 +245,7 @@ static const char *smii0_parents[] = { "smii_125m_pad", "ras_pll2_clk",
 	"ras_syn0_gclk", };
 static const char *uartx_parents[] = { "ras_syn1_gclk", "ras_apb_clk", };
 
-static void __init spear320_clk_init(void)
+static void __init spear320_clk_init(void __iomem *soc_config_base)
 {
 	struct clk *clk;
 
@@ -362,7 +372,7 @@ static void __init spear320_clk_init(void)
 static inline void spear320_clk_init(void) { }
 #endif
 
-void __init spear3xx_clk_init(void)
+void __init spear3xx_clk_init(void __iomem *misc_base, void __iomem *soc_config_base)
 {
 	struct clk *clk, *clk1;
 
@@ -634,5 +644,5 @@ void __init spear3xx_clk_init(void)
 	else if (of_machine_is_compatible("st,spear310"))
 		spear310_clk_init();
 	else if (of_machine_is_compatible("st,spear320"))
-		spear320_clk_init();
+		spear320_clk_init(soc_config_base);
 }
diff --git a/drivers/clk/spear/spear6xx_clock.c b/drivers/clk/spear/spear6xx_clock.c
index e862a333ad30..9406f2426d64 100644
--- a/drivers/clk/spear/spear6xx_clock.c
+++ b/drivers/clk/spear/spear6xx_clock.c
@@ -13,28 +13,27 @@
 #include <linux/clkdev.h>
 #include <linux/io.h>
 #include <linux/spinlock_types.h>
-#include <mach/misc_regs.h>
 #include "clk.h"
 
 static DEFINE_SPINLOCK(_lock);
 
-#define PLL1_CTR			(MISC_BASE + 0x008)
-#define PLL1_FRQ			(MISC_BASE + 0x00C)
-#define PLL2_CTR			(MISC_BASE + 0x014)
-#define PLL2_FRQ			(MISC_BASE + 0x018)
-#define PLL_CLK_CFG			(MISC_BASE + 0x020)
+#define PLL1_CTR			(misc_base + 0x008)
+#define PLL1_FRQ			(misc_base + 0x00C)
+#define PLL2_CTR			(misc_base + 0x014)
+#define PLL2_FRQ			(misc_base + 0x018)
+#define PLL_CLK_CFG			(misc_base + 0x020)
 	/* PLL_CLK_CFG register masks */
 	#define MCTR_CLK_SHIFT		28
 	#define MCTR_CLK_MASK		3
 
-#define CORE_CLK_CFG			(MISC_BASE + 0x024)
+#define CORE_CLK_CFG			(misc_base + 0x024)
 	/* CORE CLK CFG register masks */
 	#define HCLK_RATIO_SHIFT	10
 	#define HCLK_RATIO_MASK		2
 	#define PCLK_RATIO_SHIFT	8
 	#define PCLK_RATIO_MASK		2
 
-#define PERIP_CLK_CFG			(MISC_BASE + 0x028)
+#define PERIP_CLK_CFG			(misc_base + 0x028)
 	/* PERIP_CLK_CFG register masks */
 	#define CLCD_CLK_SHIFT		2
 	#define CLCD_CLK_MASK		2
@@ -48,7 +47,7 @@ static DEFINE_SPINLOCK(_lock);
 	#define GPT3_CLK_SHIFT		12
 	#define GPT_CLK_MASK		1
 
-#define PERIP1_CLK_ENB			(MISC_BASE + 0x02C)
+#define PERIP1_CLK_ENB			(misc_base + 0x02C)
 	/* PERIP1_CLK_ENB register masks */
 	#define UART0_CLK_ENB		3
 	#define UART1_CLK_ENB		4
@@ -74,13 +73,13 @@ static DEFINE_SPINLOCK(_lock);
 	#define USBH0_CLK_ENB		25
 	#define USBH1_CLK_ENB		26
 
-#define PRSC0_CLK_CFG			(MISC_BASE + 0x044)
-#define PRSC1_CLK_CFG			(MISC_BASE + 0x048)
-#define PRSC2_CLK_CFG			(MISC_BASE + 0x04C)
+#define PRSC0_CLK_CFG			(misc_base + 0x044)
+#define PRSC1_CLK_CFG			(misc_base + 0x048)
+#define PRSC2_CLK_CFG			(misc_base + 0x04C)
 
-#define CLCD_CLK_SYNT			(MISC_BASE + 0x05C)
-#define FIRDA_CLK_SYNT			(MISC_BASE + 0x060)
-#define UART_CLK_SYNT			(MISC_BASE + 0x064)
+#define CLCD_CLK_SYNT			(misc_base + 0x05C)
+#define FIRDA_CLK_SYNT			(misc_base + 0x060)
+#define UART_CLK_SYNT			(misc_base + 0x064)
 
 /* vco rate configuration table, in ascending order of rates */
 static struct pll_rate_tbl pll_rtbl[] = {
@@ -115,7 +114,7 @@ static struct gpt_rate_tbl gpt_rtbl[] = {
 	{.mscale = 1, .nscale = 0}, /* 83 MHz */
 };
 
-void __init spear6xx_clk_init(void)
+void __init spear6xx_clk_init(void __iomem *misc_base)
 {
 	struct clk *clk, *clk1;
 
diff --git a/drivers/clk/tegra/Makefile b/drivers/clk/tegra/Makefile
index 2b41b0f4f731..f49fac2d193a 100644
--- a/drivers/clk/tegra/Makefile
+++ b/drivers/clk/tegra/Makefile
@@ -9,3 +9,4 @@ obj-y					+= clk-super.o
 
 obj-$(CONFIG_ARCH_TEGRA_2x_SOC)         += clk-tegra20.o
 obj-$(CONFIG_ARCH_TEGRA_3x_SOC)         += clk-tegra30.o
+obj-$(CONFIG_ARCH_TEGRA_114_SOC)	+= clk-tegra114.o
diff --git a/drivers/clk/tegra/clk-periph-gate.c b/drivers/clk/tegra/clk-periph-gate.c
index 6dd533251e7b..bafee9895a24 100644
--- a/drivers/clk/tegra/clk-periph-gate.c
+++ b/drivers/clk/tegra/clk-periph-gate.c
@@ -41,7 +41,9 @@ static DEFINE_SPINLOCK(periph_ref_lock);
 #define write_rst_clr(val, gate) \
 	writel_relaxed(val, gate->clk_base + (gate->regs->rst_clr_reg))
 
-#define periph_clk_to_bit(periph) (1 << (gate->clk_num % 32))
+#define periph_clk_to_bit(gate) (1 << (gate->clk_num % 32))
+
+#define LVL2_CLK_GATE_OVRE 0x554
 
 /* Peripheral gate clock ops */
 static int clk_periph_is_enabled(struct clk_hw *hw)
@@ -83,6 +85,13 @@ static int clk_periph_enable(struct clk_hw *hw)
 		}
 	}
 
+	if (gate->flags & TEGRA_PERIPH_WAR_1005168) {
+		writel_relaxed(0, gate->clk_base + LVL2_CLK_GATE_OVRE);
+		writel_relaxed(BIT(22), gate->clk_base + LVL2_CLK_GATE_OVRE);
+		udelay(1);
+		writel_relaxed(0, gate->clk_base + LVL2_CLK_GATE_OVRE);
+	}
+
 	spin_unlock_irqrestore(&periph_ref_lock, flags);
 
 	return 0;
diff --git a/drivers/clk/tegra/clk-periph.c b/drivers/clk/tegra/clk-periph.c
index 788486e6331a..b2309d37a963 100644
--- a/drivers/clk/tegra/clk-periph.c
+++ b/drivers/clk/tegra/clk-periph.c
@@ -16,6 +16,7 @@
 
 #include <linux/clk.h>
 #include <linux/clk-provider.h>
+#include <linux/export.h>
 #include <linux/slab.h>
 #include <linux/err.h>
 
@@ -128,6 +129,7 @@ void tegra_periph_reset_deassert(struct clk *c)
 
 	tegra_periph_reset(gate, 0);
 }
+EXPORT_SYMBOL(tegra_periph_reset_deassert);
 
 void tegra_periph_reset_assert(struct clk *c)
 {
@@ -147,6 +149,7 @@ void tegra_periph_reset_assert(struct clk *c)
 
 	tegra_periph_reset(gate, 1);
 }
+EXPORT_SYMBOL(tegra_periph_reset_assert);
 
 const struct clk_ops tegra_clk_periph_ops = {
 	.get_parent = clk_periph_get_parent,
@@ -170,14 +173,15 @@ const struct clk_ops tegra_clk_periph_nodiv_ops = {
 static struct clk *_tegra_clk_register_periph(const char *name,
 			const char **parent_names, int num_parents,
 			struct tegra_clk_periph *periph,
-			void __iomem *clk_base, u32 offset, bool div)
+			void __iomem *clk_base, u32 offset, bool div,
+			unsigned long flags)
 {
 	struct clk *clk;
 	struct clk_init_data init;
 
 	init.name = name;
 	init.ops = div ? &tegra_clk_periph_ops : &tegra_clk_periph_nodiv_ops;
-	init.flags = div ? 0 : CLK_SET_RATE_PARENT;
+	init.flags = flags;
 	init.parent_names = parent_names;
 	init.num_parents = num_parents;
 
@@ -202,10 +206,10 @@ static struct clk *_tegra_clk_register_periph(const char *name,
 struct clk *tegra_clk_register_periph(const char *name,
 		const char **parent_names, int num_parents,
 		struct tegra_clk_periph *periph, void __iomem *clk_base,
-		u32 offset)
+		u32 offset, unsigned long flags)
 {
 	return _tegra_clk_register_periph(name, parent_names, num_parents,
-			periph, clk_base, offset, true);
+			periph, clk_base, offset, true, flags);
 }
 
 struct clk *tegra_clk_register_periph_nodiv(const char *name,
@@ -214,5 +218,5 @@ struct clk *tegra_clk_register_periph_nodiv(const char *name,
 		u32 offset)
 {
 	return _tegra_clk_register_periph(name, parent_names, num_parents,
-			periph, clk_base, offset, false);
+			periph, clk_base, offset, false, CLK_SET_RATE_PARENT);
 }
diff --git a/drivers/clk/tegra/clk-pll.c b/drivers/clk/tegra/clk-pll.c
index 165f24734c1b..17c2cc086eb4 100644
--- a/drivers/clk/tegra/clk-pll.c
+++ b/drivers/clk/tegra/clk-pll.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ * Copyright (c) 2012, 2013, NVIDIA CORPORATION.  All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -79,6 +79,48 @@
 #define PLLE_SS_CTRL 0x68
 #define PLLE_SS_DISABLE (7 << 10)
 
+#define PLLE_AUX_PLLP_SEL	BIT(2)
+#define PLLE_AUX_ENABLE_SWCTL	BIT(4)
+#define PLLE_AUX_SEQ_ENABLE	BIT(24)
+#define PLLE_AUX_PLLRE_SEL	BIT(28)
+
+#define PLLE_MISC_PLLE_PTS	BIT(8)
+#define PLLE_MISC_IDDQ_SW_VALUE	BIT(13)
+#define PLLE_MISC_IDDQ_SW_CTRL	BIT(14)
+#define PLLE_MISC_VREG_BG_CTRL_SHIFT	4
+#define PLLE_MISC_VREG_BG_CTRL_MASK	(3 << PLLE_MISC_VREG_BG_CTRL_SHIFT)
+#define PLLE_MISC_VREG_CTRL_SHIFT	2
+#define PLLE_MISC_VREG_CTRL_MASK	(2 << PLLE_MISC_VREG_CTRL_SHIFT)
+
+#define PLLCX_MISC_STROBE	BIT(31)
+#define PLLCX_MISC_RESET	BIT(30)
+#define PLLCX_MISC_SDM_DIV_SHIFT 28
+#define PLLCX_MISC_SDM_DIV_MASK (0x3 << PLLCX_MISC_SDM_DIV_SHIFT)
+#define PLLCX_MISC_FILT_DIV_SHIFT 26
+#define PLLCX_MISC_FILT_DIV_MASK (0x3 << PLLCX_MISC_FILT_DIV_SHIFT)
+#define PLLCX_MISC_ALPHA_SHIFT 18
+#define PLLCX_MISC_DIV_LOW_RANGE \
+		((0x1 << PLLCX_MISC_SDM_DIV_SHIFT) | \
+		(0x1 << PLLCX_MISC_FILT_DIV_SHIFT))
+#define PLLCX_MISC_DIV_HIGH_RANGE \
+		((0x2 << PLLCX_MISC_SDM_DIV_SHIFT) | \
+		(0x2 << PLLCX_MISC_FILT_DIV_SHIFT))
+#define PLLCX_MISC_COEF_LOW_RANGE \
+		((0x14 << PLLCX_MISC_KA_SHIFT) | (0x38 << PLLCX_MISC_KB_SHIFT))
+#define PLLCX_MISC_KA_SHIFT 2
+#define PLLCX_MISC_KB_SHIFT 9
+#define PLLCX_MISC_DEFAULT (PLLCX_MISC_COEF_LOW_RANGE | \
+			    (0x19 << PLLCX_MISC_ALPHA_SHIFT) | \
+			    PLLCX_MISC_DIV_LOW_RANGE | \
+			    PLLCX_MISC_RESET)
+#define PLLCX_MISC1_DEFAULT 0x000d2308
+#define PLLCX_MISC2_DEFAULT 0x30211200
+#define PLLCX_MISC3_DEFAULT 0x200
+
+#define PMC_PLLM_WB0_OVERRIDE	0x1dc
+#define PMC_PLLM_WB0_OVERRIDE_2	0x2b0
+#define PMC_PLLM_WB0_OVERRIDE_2_DIVP_MASK BIT(27)
+
 #define PMC_SATA_PWRGT 0x1ac
 #define PMC_SATA_PWRGT_PLLE_IDDQ_VALUE BIT(5)
 #define PMC_SATA_PWRGT_PLLE_IDDQ_SWCTL BIT(4)
@@ -101,6 +143,24 @@
 #define divn_max(p) (divn_mask(p))
 #define divp_max(p) (1 << (divp_mask(p)))
 
+
+#ifdef CONFIG_ARCH_TEGRA_114_SOC
+/* PLLXC has 4-bit PDIV, but entry 15 is not allowed in h/w */
+#define PLLXC_PDIV_MAX			14
+
+/* non-monotonic mapping below is not a typo */
+static u8 pllxc_p[PLLXC_PDIV_MAX + 1] = {
+	/* PDIV: 0, 1, 2, 3, 4, 5, 6,  7,  8,  9, 10, 11, 12, 13, 14 */
+	/* p: */ 1, 2, 3, 4, 5, 6, 8, 10, 12, 16, 12, 16, 20, 24, 32
+};
+
+#define PLLCX_PDIV_MAX 7
+static u8 pllcx_p[PLLCX_PDIV_MAX + 1] = {
+	/* PDIV: 0, 1, 2, 3, 4, 5,  6,  7 */
+	/* p: */ 1, 2, 3, 4, 6, 8, 12, 16
+};
+#endif
+
 static void clk_pll_enable_lock(struct tegra_clk_pll *pll)
 {
 	u32 val;
@@ -108,25 +168,36 @@ static void clk_pll_enable_lock(struct tegra_clk_pll *pll)
 	if (!(pll->flags & TEGRA_PLL_USE_LOCK))
 		return;
 
+	if (!(pll->flags & TEGRA_PLL_HAS_LOCK_ENABLE))
+		return;
+
 	val = pll_readl_misc(pll);
 	val |= BIT(pll->params->lock_enable_bit_idx);
 	pll_writel_misc(val, pll);
 }
 
-static int clk_pll_wait_for_lock(struct tegra_clk_pll *pll,
-				 void __iomem *lock_addr, u32 lock_bit_idx)
+static int clk_pll_wait_for_lock(struct tegra_clk_pll *pll)
 {
 	int i;
-	u32 val;
+	u32 val, lock_mask;
+	void __iomem *lock_addr;
 
 	if (!(pll->flags & TEGRA_PLL_USE_LOCK)) {
 		udelay(pll->params->lock_delay);
 		return 0;
 	}
 
+	lock_addr = pll->clk_base;
+	if (pll->flags & TEGRA_PLL_LOCK_MISC)
+		lock_addr += pll->params->misc_reg;
+	else
+		lock_addr += pll->params->base_reg;
+
+	lock_mask = pll->params->lock_mask;
+
 	for (i = 0; i < pll->params->lock_delay; i++) {
 		val = readl_relaxed(lock_addr);
-		if (val & BIT(lock_bit_idx)) {
+		if ((val & lock_mask) == lock_mask) {
 			udelay(PLL_POST_LOCK_DELAY);
 			return 0;
 		}
@@ -155,7 +226,7 @@ static int clk_pll_is_enabled(struct clk_hw *hw)
 	return val & PLL_BASE_ENABLE ? 1 : 0;
 }
 
-static int _clk_pll_enable(struct clk_hw *hw)
+static void _clk_pll_enable(struct clk_hw *hw)
 {
 	struct tegra_clk_pll *pll = to_clk_pll(hw);
 	u32 val;
@@ -163,7 +234,8 @@ static int _clk_pll_enable(struct clk_hw *hw)
 	clk_pll_enable_lock(pll);
 
 	val = pll_readl_base(pll);
-	val &= ~PLL_BASE_BYPASS;
+	if (pll->flags & TEGRA_PLL_BYPASS)
+		val &= ~PLL_BASE_BYPASS;
 	val |= PLL_BASE_ENABLE;
 	pll_writel_base(val, pll);
 
@@ -172,11 +244,6 @@ static int _clk_pll_enable(struct clk_hw *hw)
 		val |= PMC_PLLP_WB0_OVERRIDE_PLLM_ENABLE;
 		writel_relaxed(val, pll->pmc + PMC_PLLP_WB0_OVERRIDE);
 	}
-
-	clk_pll_wait_for_lock(pll, pll->clk_base + pll->params->base_reg,
-			      pll->params->lock_bit_idx);
-
-	return 0;
 }
 
 static void _clk_pll_disable(struct clk_hw *hw)
@@ -185,7 +252,9 @@ static void _clk_pll_disable(struct clk_hw *hw)
 	u32 val;
 
 	val = pll_readl_base(pll);
-	val &= ~(PLL_BASE_BYPASS | PLL_BASE_ENABLE);
+	if (pll->flags & TEGRA_PLL_BYPASS)
+		val &= ~PLL_BASE_BYPASS;
+	val &= ~PLL_BASE_ENABLE;
 	pll_writel_base(val, pll);
 
 	if (pll->flags & TEGRA_PLLM) {
@@ -204,7 +273,9 @@ static int clk_pll_enable(struct clk_hw *hw)
 	if (pll->lock)
 		spin_lock_irqsave(pll->lock, flags);
 
-	ret = _clk_pll_enable(hw);
+	_clk_pll_enable(hw);
+
+	ret = clk_pll_wait_for_lock(pll);
 
 	if (pll->lock)
 		spin_unlock_irqrestore(pll->lock, flags);
@@ -241,8 +312,6 @@ static int _get_table_rate(struct clk_hw *hw,
 	if (sel->input_rate == 0)
 		return -EINVAL;
 
-	BUG_ON(sel->p < 1);
-
 	cfg->input_rate = sel->input_rate;
 	cfg->output_rate = sel->output_rate;
 	cfg->m = sel->m;
@@ -257,6 +326,7 @@ static int _calc_rate(struct clk_hw *hw, struct tegra_clk_pll_freq_table *cfg,
 		      unsigned long rate, unsigned long parent_rate)
 {
 	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	struct pdiv_map *p_tohw = pll->params->pdiv_tohw;
 	unsigned long cfreq;
 	u32 p_div = 0;
 
@@ -290,88 +360,119 @@ static int _calc_rate(struct clk_hw *hw, struct tegra_clk_pll_freq_table *cfg,
 	     cfg->output_rate <<= 1)
 		p_div++;
 
-	cfg->p = 1 << p_div;
 	cfg->m = parent_rate / cfreq;
 	cfg->n = cfg->output_rate / cfreq;
 	cfg->cpcon = OUT_OF_TABLE_CPCON;
 
 	if (cfg->m > divm_max(pll) || cfg->n > divn_max(pll) ||
-	    cfg->p > divp_max(pll) || cfg->output_rate > pll->params->vco_max) {
+	    (1 << p_div) > divp_max(pll)
+	    || cfg->output_rate > pll->params->vco_max) {
 		pr_err("%s: Failed to set %s rate %lu\n",
 		       __func__, __clk_get_name(hw->clk), rate);
 		return -EINVAL;
 	}
 
+	if (p_tohw) {
+		p_div = 1 << p_div;
+		while (p_tohw->pdiv) {
+			if (p_div <= p_tohw->pdiv) {
+				cfg->p = p_tohw->hw_val;
+				break;
+			}
+			p_tohw++;
+		}
+		if (!p_tohw->pdiv)
+			return -EINVAL;
+	} else
+		cfg->p = p_div;
+
 	return 0;
 }
 
-static int _program_pll(struct clk_hw *hw, struct tegra_clk_pll_freq_table *cfg,
-			unsigned long rate)
+static void _update_pll_mnp(struct tegra_clk_pll *pll,
+			    struct tegra_clk_pll_freq_table *cfg)
 {
-	struct tegra_clk_pll *pll = to_clk_pll(hw);
-	unsigned long flags = 0;
-	u32 divp, val, old_base;
-	int state;
-
-	divp = __ffs(cfg->p);
-
-	if (pll->flags & TEGRA_PLLU)
-		divp ^= 1;
+	u32 val;
 
-	if (pll->lock)
-		spin_lock_irqsave(pll->lock, flags);
+	val = pll_readl_base(pll);
 
-	old_base = val = pll_readl_base(pll);
 	val &= ~((divm_mask(pll) << pll->divm_shift) |
 		 (divn_mask(pll) << pll->divn_shift) |
 		 (divp_mask(pll) << pll->divp_shift));
 	val |= ((cfg->m << pll->divm_shift) |
 		(cfg->n << pll->divn_shift) |
-		(divp << pll->divp_shift));
-	if (val == old_base) {
-		if (pll->lock)
-			spin_unlock_irqrestore(pll->lock, flags);
-		return 0;
+		(cfg->p << pll->divp_shift));
+
+	pll_writel_base(val, pll);
+}
+
+static void _get_pll_mnp(struct tegra_clk_pll *pll,
+			 struct tegra_clk_pll_freq_table *cfg)
+{
+	u32 val;
+
+	val = pll_readl_base(pll);
+
+	cfg->m = (val >> pll->divm_shift) & (divm_mask(pll));
+	cfg->n = (val >> pll->divn_shift) & (divn_mask(pll));
+	cfg->p = (val >> pll->divp_shift) & (divp_mask(pll));
+}
+
+static void _update_pll_cpcon(struct tegra_clk_pll *pll,
+			      struct tegra_clk_pll_freq_table *cfg,
+			      unsigned long rate)
+{
+	u32 val;
+
+	val = pll_readl_misc(pll);
+
+	val &= ~(PLL_MISC_CPCON_MASK << PLL_MISC_CPCON_SHIFT);
+	val |= cfg->cpcon << PLL_MISC_CPCON_SHIFT;
+
+	if (pll->flags & TEGRA_PLL_SET_LFCON) {
+		val &= ~(PLL_MISC_LFCON_MASK << PLL_MISC_LFCON_SHIFT);
+		if (cfg->n >= PLLDU_LFCON_SET_DIVN)
+			val |= 1 << PLL_MISC_LFCON_SHIFT;
+	} else if (pll->flags & TEGRA_PLL_SET_DCCON) {
+		val &= ~(1 << PLL_MISC_DCCON_SHIFT);
+		if (rate >= (pll->params->vco_max >> 1))
+			val |= 1 << PLL_MISC_DCCON_SHIFT;
 	}
 
+	pll_writel_misc(val, pll);
+}
+
+static int _program_pll(struct clk_hw *hw, struct tegra_clk_pll_freq_table *cfg,
+			unsigned long rate)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	int state, ret = 0;
+
 	state = clk_pll_is_enabled(hw);
 
-	if (state) {
+	if (state)
 		_clk_pll_disable(hw);
-		val &= ~(PLL_BASE_BYPASS | PLL_BASE_ENABLE);
-	}
-	pll_writel_base(val, pll);
 
-	if (pll->flags & TEGRA_PLL_HAS_CPCON) {
-		val = pll_readl_misc(pll);
-		val &= ~(PLL_MISC_CPCON_MASK << PLL_MISC_CPCON_SHIFT);
-		val |= cfg->cpcon << PLL_MISC_CPCON_SHIFT;
-		if (pll->flags & TEGRA_PLL_SET_LFCON) {
-			val &= ~(PLL_MISC_LFCON_MASK << PLL_MISC_LFCON_SHIFT);
-			if (cfg->n >= PLLDU_LFCON_SET_DIVN)
-				val |= 0x1 << PLL_MISC_LFCON_SHIFT;
-		} else if (pll->flags & TEGRA_PLL_SET_DCCON) {
-			val &= ~(0x1 << PLL_MISC_DCCON_SHIFT);
-			if (rate >= (pll->params->vco_max >> 1))
-				val |= 0x1 << PLL_MISC_DCCON_SHIFT;
-		}
-		pll_writel_misc(val, pll);
-	}
+	_update_pll_mnp(pll, cfg);
 
-	if (pll->lock)
-		spin_unlock_irqrestore(pll->lock, flags);
+	if (pll->flags & TEGRA_PLL_HAS_CPCON)
+		_update_pll_cpcon(pll, cfg, rate);
 
-	if (state)
-		clk_pll_enable(hw);
+	if (state) {
+		_clk_pll_enable(hw);
+		ret = clk_pll_wait_for_lock(pll);
+	}
 
-	return 0;
+	return ret;
 }
 
 static int clk_pll_set_rate(struct clk_hw *hw, unsigned long rate,
 			unsigned long parent_rate)
 {
 	struct tegra_clk_pll *pll = to_clk_pll(hw);
-	struct tegra_clk_pll_freq_table cfg;
+	struct tegra_clk_pll_freq_table cfg, old_cfg;
+	unsigned long flags = 0;
+	int ret = 0;
 
 	if (pll->flags & TEGRA_PLL_FIXED) {
 		if (rate != pll->fixed_rate) {
@@ -387,7 +488,18 @@ static int clk_pll_set_rate(struct clk_hw *hw, unsigned long rate,
 	    _calc_rate(hw, &cfg, rate, parent_rate))
 		return -EINVAL;
 
-	return _program_pll(hw, &cfg, rate);
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	_get_pll_mnp(pll, &old_cfg);
+
+	if (old_cfg.m != cfg.m || old_cfg.n != cfg.n || old_cfg.p != cfg.p)
+		ret = _program_pll(hw, &cfg, rate);
+
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+
+	return ret;
 }
 
 static long clk_pll_round_rate(struct clk_hw *hw, unsigned long rate,
@@ -409,7 +521,7 @@ static long clk_pll_round_rate(struct clk_hw *hw, unsigned long rate,
 		return -EINVAL;
 
 	output_rate *= cfg.n;
-	do_div(output_rate, cfg.m * cfg.p);
+	do_div(output_rate, cfg.m * (1 << cfg.p));
 
 	return output_rate;
 }
@@ -418,11 +530,15 @@ static unsigned long clk_pll_recalc_rate(struct clk_hw *hw,
 					 unsigned long parent_rate)
 {
 	struct tegra_clk_pll *pll = to_clk_pll(hw);
-	u32 val = pll_readl_base(pll);
-	u32 divn = 0, divm = 0, divp = 0;
+	struct tegra_clk_pll_freq_table cfg;
+	struct pdiv_map *p_tohw = pll->params->pdiv_tohw;
+	u32 val;
 	u64 rate = parent_rate;
+	int pdiv;
+
+	val = pll_readl_base(pll);
 
-	if (val & PLL_BASE_BYPASS)
+	if ((pll->flags & TEGRA_PLL_BYPASS) && (val & PLL_BASE_BYPASS))
 		return parent_rate;
 
 	if ((pll->flags & TEGRA_PLL_FIXED) && !(val & PLL_BASE_OVERRIDE)) {
@@ -435,16 +551,29 @@ static unsigned long clk_pll_recalc_rate(struct clk_hw *hw,
 		return pll->fixed_rate;
 	}
 
-	divp = (val >> pll->divp_shift) & (divp_mask(pll));
-	if (pll->flags & TEGRA_PLLU)
-		divp ^= 1;
+	_get_pll_mnp(pll, &cfg);
 
-	divn = (val >> pll->divn_shift) & (divn_mask(pll));
-	divm = (val >> pll->divm_shift) & (divm_mask(pll));
-	divm *= (1 << divp);
+	if (p_tohw) {
+		while (p_tohw->pdiv) {
+			if (cfg.p == p_tohw->hw_val) {
+				pdiv = p_tohw->pdiv;
+				break;
+			}
+			p_tohw++;
+		}
+
+		if (!p_tohw->pdiv) {
+			WARN_ON(1);
+			pdiv = 1;
+		}
+	} else
+		pdiv = 1 << cfg.p;
+
+	cfg.m *= pdiv;
+
+	rate *= cfg.n;
+	do_div(rate, cfg.m);
 
-	rate *= divn;
-	do_div(rate, divm);
 	return rate;
 }
 
@@ -538,8 +667,8 @@ static int clk_plle_enable(struct clk_hw *hw)
 	val |= (PLL_BASE_BYPASS | PLL_BASE_ENABLE);
 	pll_writel_base(val, pll);
 
-	clk_pll_wait_for_lock(pll, pll->clk_base + pll->params->misc_reg,
-			      pll->params->lock_bit_idx);
+	clk_pll_wait_for_lock(pll);
+
 	return 0;
 }
 
@@ -577,28 +706,531 @@ const struct clk_ops tegra_clk_plle_ops = {
 	.enable = clk_plle_enable,
 };
 
-static struct clk *_tegra_clk_register_pll(const char *name,
-		const char *parent_name, void __iomem *clk_base,
-		void __iomem *pmc, unsigned long flags,
-		unsigned long fixed_rate,
-		struct tegra_clk_pll_params *pll_params, u8 pll_flags,
-		struct tegra_clk_pll_freq_table *freq_table, spinlock_t *lock,
-		const struct clk_ops *ops)
+#ifdef CONFIG_ARCH_TEGRA_114_SOC
+
+static int _pll_fixed_mdiv(struct tegra_clk_pll_params *pll_params,
+			   unsigned long parent_rate)
+{
+	if (parent_rate > pll_params->cf_max)
+		return 2;
+	else
+		return 1;
+}
+
+static int clk_pll_iddq_enable(struct clk_hw *hw)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	unsigned long flags = 0;
+
+	u32 val;
+	int ret;
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	val = pll_readl(pll->params->iddq_reg, pll);
+	val &= ~BIT(pll->params->iddq_bit_idx);
+	pll_writel(val, pll->params->iddq_reg, pll);
+	udelay(2);
+
+	_clk_pll_enable(hw);
+
+	ret = clk_pll_wait_for_lock(pll);
+
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+
+	return 0;
+}
+
+static void clk_pll_iddq_disable(struct clk_hw *hw)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	unsigned long flags = 0;
+	u32 val;
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	_clk_pll_disable(hw);
+
+	val = pll_readl(pll->params->iddq_reg, pll);
+	val |= BIT(pll->params->iddq_bit_idx);
+	pll_writel(val, pll->params->iddq_reg, pll);
+	udelay(2);
+
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+}
+
+static int _calc_dynamic_ramp_rate(struct clk_hw *hw,
+				struct tegra_clk_pll_freq_table *cfg,
+				unsigned long rate, unsigned long parent_rate)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	unsigned int p;
+
+	if (!rate)
+		return -EINVAL;
+
+	p = DIV_ROUND_UP(pll->params->vco_min, rate);
+	cfg->m = _pll_fixed_mdiv(pll->params, parent_rate);
+	cfg->p = p;
+	cfg->output_rate = rate * cfg->p;
+	cfg->n = cfg->output_rate * cfg->m / parent_rate;
+
+	if (cfg->n > divn_max(pll) || cfg->output_rate > pll->params->vco_max)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int _pll_ramp_calc_pll(struct clk_hw *hw,
+			      struct tegra_clk_pll_freq_table *cfg,
+			      unsigned long rate, unsigned long parent_rate)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	int err = 0;
+
+	err = _get_table_rate(hw, cfg, rate, parent_rate);
+	if (err < 0)
+		err = _calc_dynamic_ramp_rate(hw, cfg, rate, parent_rate);
+	else if (cfg->m != _pll_fixed_mdiv(pll->params, parent_rate)) {
+			WARN_ON(1);
+			err = -EINVAL;
+			goto out;
+	}
+
+	if (!cfg->p || (cfg->p >  pll->params->max_p))
+		err = -EINVAL;
+
+out:
+	return err;
+}
+
+static int clk_pllxc_set_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long parent_rate)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	struct tegra_clk_pll_freq_table cfg, old_cfg;
+	unsigned long flags = 0;
+	int ret = 0;
+	u8 old_p;
+
+	ret = _pll_ramp_calc_pll(hw, &cfg, rate, parent_rate);
+	if (ret < 0)
+		return ret;
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	_get_pll_mnp(pll, &old_cfg);
+
+	old_p = pllxc_p[old_cfg.p];
+	if (old_cfg.m != cfg.m || old_cfg.n != cfg.n || old_p != cfg.p) {
+		cfg.p -= 1;
+		ret = _program_pll(hw, &cfg, rate);
+	}
+
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+
+	return ret;
+}
+
+static long clk_pll_ramp_round_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long *prate)
+{
+	struct tegra_clk_pll_freq_table cfg;
+	int ret = 0;
+	u64 output_rate = *prate;
+
+	ret = _pll_ramp_calc_pll(hw, &cfg, rate, *prate);
+	if (ret < 0)
+		return ret;
+
+	output_rate *= cfg.n;
+	do_div(output_rate, cfg.m * cfg.p);
+
+	return output_rate;
+}
+
+static int clk_pllm_set_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long parent_rate)
+{
+	struct tegra_clk_pll_freq_table cfg;
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	unsigned long flags = 0;
+	int state, ret = 0;
+	u32 val;
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	state = clk_pll_is_enabled(hw);
+	if (state) {
+		if (rate != clk_get_rate(hw->clk)) {
+			pr_err("%s: Cannot change active PLLM\n", __func__);
+			ret = -EINVAL;
+			goto out;
+		}
+		goto out;
+	}
+
+	ret = _pll_ramp_calc_pll(hw, &cfg, rate, parent_rate);
+	if (ret < 0)
+		goto out;
+
+	cfg.p -= 1;
+
+	val = readl_relaxed(pll->pmc + PMC_PLLM_WB0_OVERRIDE);
+	if (val & PMC_PLLP_WB0_OVERRIDE_PLLM_OVERRIDE) {
+		val = readl_relaxed(pll->pmc + PMC_PLLM_WB0_OVERRIDE_2);
+		val = cfg.p ? (val | PMC_PLLM_WB0_OVERRIDE_2_DIVP_MASK) :
+			(val & ~PMC_PLLM_WB0_OVERRIDE_2_DIVP_MASK);
+		writel_relaxed(val, pll->pmc + PMC_PLLM_WB0_OVERRIDE_2);
+
+		val = readl_relaxed(pll->pmc + PMC_PLLM_WB0_OVERRIDE);
+		val &= ~(divn_mask(pll) | divm_mask(pll));
+		val |= (cfg.m << pll->divm_shift) | (cfg.n << pll->divn_shift);
+		writel_relaxed(val, pll->pmc + PMC_PLLM_WB0_OVERRIDE);
+	} else
+		_update_pll_mnp(pll, &cfg);
+
+
+out:
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+
+	return ret;
+}
+
+static void _pllcx_strobe(struct tegra_clk_pll *pll)
+{
+	u32 val;
+
+	val = pll_readl_misc(pll);
+	val |= PLLCX_MISC_STROBE;
+	pll_writel_misc(val, pll);
+	udelay(2);
+
+	val &= ~PLLCX_MISC_STROBE;
+	pll_writel_misc(val, pll);
+}
+
+static int clk_pllc_enable(struct clk_hw *hw)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	u32 val;
+	int ret = 0;
+	unsigned long flags = 0;
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	_clk_pll_enable(hw);
+	udelay(2);
+
+	val = pll_readl_misc(pll);
+	val &= ~PLLCX_MISC_RESET;
+	pll_writel_misc(val, pll);
+	udelay(2);
+
+	_pllcx_strobe(pll);
+
+	ret = clk_pll_wait_for_lock(pll);
+
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+
+	return ret;
+}
+
+static void _clk_pllc_disable(struct clk_hw *hw)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	u32 val;
+
+	_clk_pll_disable(hw);
+
+	val = pll_readl_misc(pll);
+	val |= PLLCX_MISC_RESET;
+	pll_writel_misc(val, pll);
+	udelay(2);
+}
+
+static void clk_pllc_disable(struct clk_hw *hw)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	unsigned long flags = 0;
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	_clk_pllc_disable(hw);
+
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+}
+
+static int _pllcx_update_dynamic_coef(struct tegra_clk_pll *pll,
+					unsigned long input_rate, u32 n)
+{
+	u32 val, n_threshold;
+
+	switch (input_rate) {
+	case 12000000:
+		n_threshold = 70;
+		break;
+	case 13000000:
+	case 26000000:
+		n_threshold = 71;
+		break;
+	case 16800000:
+		n_threshold = 55;
+		break;
+	case 19200000:
+		n_threshold = 48;
+		break;
+	default:
+		pr_err("%s: Unexpected reference rate %lu\n",
+			__func__, input_rate);
+		return -EINVAL;
+	}
+
+	val = pll_readl_misc(pll);
+	val &= ~(PLLCX_MISC_SDM_DIV_MASK | PLLCX_MISC_FILT_DIV_MASK);
+	val |= n <= n_threshold ?
+		PLLCX_MISC_DIV_LOW_RANGE : PLLCX_MISC_DIV_HIGH_RANGE;
+	pll_writel_misc(val, pll);
+
+	return 0;
+}
+
+static int clk_pllc_set_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long parent_rate)
+{
+	struct tegra_clk_pll_freq_table cfg;
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	unsigned long flags = 0;
+	int state, ret = 0;
+	u32 val;
+	u16 old_m, old_n;
+	u8 old_p;
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	ret = _pll_ramp_calc_pll(hw, &cfg, rate, parent_rate);
+	if (ret < 0)
+		goto out;
+
+	val = pll_readl_base(pll);
+	old_m = (val >> pll->divm_shift) & (divm_mask(pll));
+	old_n = (val >> pll->divn_shift) & (divn_mask(pll));
+	old_p = pllcx_p[(val >> pll->divp_shift) & (divp_mask(pll))];
+
+	if (cfg.m != old_m) {
+		WARN_ON(1);
+		goto out;
+	}
+
+	if (old_n == cfg.n && old_p == cfg.p)
+		goto out;
+
+	cfg.p -= 1;
+
+	state = clk_pll_is_enabled(hw);
+	if (state)
+		_clk_pllc_disable(hw);
+
+	ret = _pllcx_update_dynamic_coef(pll, parent_rate, cfg.n);
+	if (ret < 0)
+		goto out;
+
+	_update_pll_mnp(pll, &cfg);
+
+	if (state)
+		ret = clk_pllc_enable(hw);
+
+out:
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+
+	return ret;
+}
+
+static long _pllre_calc_rate(struct tegra_clk_pll *pll,
+			     struct tegra_clk_pll_freq_table *cfg,
+			     unsigned long rate, unsigned long parent_rate)
+{
+	u16 m, n;
+	u64 output_rate = parent_rate;
+
+	m = _pll_fixed_mdiv(pll->params, parent_rate);
+	n = rate * m / parent_rate;
+
+	output_rate *= n;
+	do_div(output_rate, m);
+
+	if (cfg) {
+		cfg->m = m;
+		cfg->n = n;
+	}
+
+	return output_rate;
+}
+static int clk_pllre_set_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long parent_rate)
+{
+	struct tegra_clk_pll_freq_table cfg, old_cfg;
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	unsigned long flags = 0;
+	int state, ret = 0;
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	_pllre_calc_rate(pll, &cfg, rate, parent_rate);
+	_get_pll_mnp(pll, &old_cfg);
+	cfg.p = old_cfg.p;
+
+	if (cfg.m != old_cfg.m || cfg.n != old_cfg.n) {
+		state = clk_pll_is_enabled(hw);
+		if (state)
+			_clk_pll_disable(hw);
+
+		_update_pll_mnp(pll, &cfg);
+
+		if (state) {
+			_clk_pll_enable(hw);
+			ret = clk_pll_wait_for_lock(pll);
+		}
+	}
+
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+
+	return ret;
+}
+
+static unsigned long clk_pllre_recalc_rate(struct clk_hw *hw,
+					 unsigned long parent_rate)
+{
+	struct tegra_clk_pll_freq_table cfg;
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	u64 rate = parent_rate;
+
+	_get_pll_mnp(pll, &cfg);
+
+	rate *= cfg.n;
+	do_div(rate, cfg.m);
+
+	return rate;
+}
+
+static long clk_pllre_round_rate(struct clk_hw *hw, unsigned long rate,
+				 unsigned long *prate)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+
+	return _pllre_calc_rate(pll, NULL, rate, *prate);
+}
+
+static int clk_plle_tegra114_enable(struct clk_hw *hw)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	struct tegra_clk_pll_freq_table sel;
+	u32 val;
+	int ret;
+	unsigned long flags = 0;
+	unsigned long input_rate = clk_get_rate(clk_get_parent(hw->clk));
+
+	if (_get_table_rate(hw, &sel, pll->fixed_rate, input_rate))
+		return -EINVAL;
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	val = pll_readl_base(pll);
+	val &= ~BIT(29); /* Disable lock override */
+	pll_writel_base(val, pll);
+
+	val = pll_readl(pll->params->aux_reg, pll);
+	val |= PLLE_AUX_ENABLE_SWCTL;
+	val &= ~PLLE_AUX_SEQ_ENABLE;
+	pll_writel(val, pll->params->aux_reg, pll);
+	udelay(1);
+
+	val = pll_readl_misc(pll);
+	val |= PLLE_MISC_LOCK_ENABLE;
+	val |= PLLE_MISC_IDDQ_SW_CTRL;
+	val &= ~PLLE_MISC_IDDQ_SW_VALUE;
+	val |= PLLE_MISC_PLLE_PTS;
+	val |= PLLE_MISC_VREG_BG_CTRL_MASK | PLLE_MISC_VREG_CTRL_MASK;
+	pll_writel_misc(val, pll);
+	udelay(5);
+
+	val = pll_readl(PLLE_SS_CTRL, pll);
+	val |= PLLE_SS_DISABLE;
+	pll_writel(val, PLLE_SS_CTRL, pll);
+
+	val = pll_readl_base(pll);
+	val &= ~(divm_mask(pll) | divn_mask(pll) | divp_mask(pll));
+	val &= ~(PLLE_BASE_DIVCML_WIDTH << PLLE_BASE_DIVCML_SHIFT);
+	val |= sel.m << pll->divm_shift;
+	val |= sel.n << pll->divn_shift;
+	val |= sel.cpcon << PLLE_BASE_DIVCML_SHIFT;
+	pll_writel_base(val, pll);
+	udelay(1);
+
+	_clk_pll_enable(hw);
+	ret = clk_pll_wait_for_lock(pll);
+
+	if (ret < 0)
+		goto out;
+
+	/* TODO: enable hw control of xusb brick pll */
+
+out:
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+
+	return ret;
+}
+
+static void clk_plle_tegra114_disable(struct clk_hw *hw)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	unsigned long flags = 0;
+	u32 val;
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	_clk_pll_disable(hw);
+
+	val = pll_readl_misc(pll);
+	val |= PLLE_MISC_IDDQ_SW_CTRL | PLLE_MISC_IDDQ_SW_VALUE;
+	pll_writel_misc(val, pll);
+	udelay(1);
+
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+}
+#endif
+
+static struct tegra_clk_pll *_tegra_init_pll(void __iomem *clk_base,
+		void __iomem *pmc, unsigned long fixed_rate,
+		struct tegra_clk_pll_params *pll_params, u32 pll_flags,
+		struct tegra_clk_pll_freq_table *freq_table, spinlock_t *lock)
 {
 	struct tegra_clk_pll *pll;
-	struct clk *clk;
-	struct clk_init_data init;
 
 	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
 	if (!pll)
 		return ERR_PTR(-ENOMEM);
 
-	init.name = name;
-	init.ops = ops;
-	init.flags = flags;
-	init.parent_names = (parent_name ? &parent_name : NULL);
-	init.num_parents = (parent_name ? 1 : 0);
-
 	pll->clk_base = clk_base;
 	pll->pmc = pmc;
 
@@ -615,34 +1247,336 @@ static struct clk *_tegra_clk_register_pll(const char *name,
 	pll->divm_shift = PLL_BASE_DIVM_SHIFT;
 	pll->divm_width = PLL_BASE_DIVM_WIDTH;
 
+	return pll;
+}
+
+static struct clk *_tegra_clk_register_pll(struct tegra_clk_pll *pll,
+		const char *name, const char *parent_name, unsigned long flags,
+		const struct clk_ops *ops)
+{
+	struct clk_init_data init;
+
+	init.name = name;
+	init.ops = ops;
+	init.flags = flags;
+	init.parent_names = (parent_name ? &parent_name : NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+
 	/* Data in .init is copied by clk_register(), so stack variable OK */
 	pll->hw.init = &init;
 
-	clk = clk_register(NULL, &pll->hw);
-	if (IS_ERR(clk))
-		kfree(pll);
-
-	return clk;
+	return clk_register(NULL, &pll->hw);
 }
 
 struct clk *tegra_clk_register_pll(const char *name, const char *parent_name,
 		void __iomem *clk_base, void __iomem *pmc,
 		unsigned long flags, unsigned long fixed_rate,
-		struct tegra_clk_pll_params *pll_params, u8 pll_flags,
+		struct tegra_clk_pll_params *pll_params, u32 pll_flags,
 		struct tegra_clk_pll_freq_table *freq_table, spinlock_t *lock)
 {
-	return _tegra_clk_register_pll(name, parent_name, clk_base, pmc,
-			flags, fixed_rate, pll_params, pll_flags, freq_table,
-			lock, &tegra_clk_pll_ops);
+	struct tegra_clk_pll *pll;
+	struct clk *clk;
+
+	pll_flags |= TEGRA_PLL_BYPASS;
+	pll_flags |= TEGRA_PLL_HAS_LOCK_ENABLE;
+	pll = _tegra_init_pll(clk_base, pmc, fixed_rate, pll_params, pll_flags,
+			      freq_table, lock);
+	if (IS_ERR(pll))
+		return ERR_CAST(pll);
+
+	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
+				      &tegra_clk_pll_ops);
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
 }
 
 struct clk *tegra_clk_register_plle(const char *name, const char *parent_name,
 		void __iomem *clk_base, void __iomem *pmc,
 		unsigned long flags, unsigned long fixed_rate,
-		struct tegra_clk_pll_params *pll_params, u8 pll_flags,
+		struct tegra_clk_pll_params *pll_params, u32 pll_flags,
 		struct tegra_clk_pll_freq_table *freq_table, spinlock_t *lock)
 {
-	return _tegra_clk_register_pll(name, parent_name, clk_base, pmc,
-			flags, fixed_rate, pll_params, pll_flags, freq_table,
-			lock, &tegra_clk_plle_ops);
+	struct tegra_clk_pll *pll;
+	struct clk *clk;
+
+	pll_flags |= TEGRA_PLL_LOCK_MISC | TEGRA_PLL_BYPASS;
+	pll_flags |= TEGRA_PLL_HAS_LOCK_ENABLE;
+	pll = _tegra_init_pll(clk_base, pmc, fixed_rate, pll_params, pll_flags,
+			      freq_table, lock);
+	if (IS_ERR(pll))
+		return ERR_CAST(pll);
+
+	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
+				      &tegra_clk_plle_ops);
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
+}
+
+#ifdef CONFIG_ARCH_TEGRA_114_SOC
+const struct clk_ops tegra_clk_pllxc_ops = {
+	.is_enabled = clk_pll_is_enabled,
+	.enable = clk_pll_iddq_enable,
+	.disable = clk_pll_iddq_disable,
+	.recalc_rate = clk_pll_recalc_rate,
+	.round_rate = clk_pll_ramp_round_rate,
+	.set_rate = clk_pllxc_set_rate,
+};
+
+const struct clk_ops tegra_clk_pllm_ops = {
+	.is_enabled = clk_pll_is_enabled,
+	.enable = clk_pll_iddq_enable,
+	.disable = clk_pll_iddq_disable,
+	.recalc_rate = clk_pll_recalc_rate,
+	.round_rate = clk_pll_ramp_round_rate,
+	.set_rate = clk_pllm_set_rate,
+};
+
+const struct clk_ops tegra_clk_pllc_ops = {
+	.is_enabled = clk_pll_is_enabled,
+	.enable = clk_pllc_enable,
+	.disable = clk_pllc_disable,
+	.recalc_rate = clk_pll_recalc_rate,
+	.round_rate = clk_pll_ramp_round_rate,
+	.set_rate = clk_pllc_set_rate,
+};
+
+const struct clk_ops tegra_clk_pllre_ops = {
+	.is_enabled = clk_pll_is_enabled,
+	.enable = clk_pll_iddq_enable,
+	.disable = clk_pll_iddq_disable,
+	.recalc_rate = clk_pllre_recalc_rate,
+	.round_rate = clk_pllre_round_rate,
+	.set_rate = clk_pllre_set_rate,
+};
+
+const struct clk_ops tegra_clk_plle_tegra114_ops = {
+	.is_enabled =  clk_pll_is_enabled,
+	.enable = clk_plle_tegra114_enable,
+	.disable = clk_plle_tegra114_disable,
+	.recalc_rate = clk_pll_recalc_rate,
+};
+
+
+struct clk *tegra_clk_register_pllxc(const char *name, const char *parent_name,
+			  void __iomem *clk_base, void __iomem *pmc,
+			  unsigned long flags, unsigned long fixed_rate,
+			  struct tegra_clk_pll_params *pll_params,
+			  u32 pll_flags,
+			  struct tegra_clk_pll_freq_table *freq_table,
+			  spinlock_t *lock)
+{
+	struct tegra_clk_pll *pll;
+	struct clk *clk;
+
+	if (!pll_params->pdiv_tohw)
+		return ERR_PTR(-EINVAL);
+
+	pll_flags |= TEGRA_PLL_HAS_LOCK_ENABLE;
+	pll = _tegra_init_pll(clk_base, pmc, fixed_rate, pll_params, pll_flags,
+			      freq_table, lock);
+	if (IS_ERR(pll))
+		return ERR_CAST(pll);
+
+	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
+				      &tegra_clk_pllxc_ops);
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
+}
+
+struct clk *tegra_clk_register_pllre(const char *name, const char *parent_name,
+			  void __iomem *clk_base, void __iomem *pmc,
+			  unsigned long flags, unsigned long fixed_rate,
+			  struct tegra_clk_pll_params *pll_params,
+			  u32 pll_flags,
+			  struct tegra_clk_pll_freq_table *freq_table,
+			  spinlock_t *lock, unsigned long parent_rate)
+{
+	u32 val;
+	struct tegra_clk_pll *pll;
+	struct clk *clk;
+
+	pll_flags |= TEGRA_PLL_HAS_LOCK_ENABLE;
+	pll = _tegra_init_pll(clk_base, pmc, fixed_rate, pll_params, pll_flags,
+			      freq_table, lock);
+	if (IS_ERR(pll))
+		return ERR_CAST(pll);
+
+	/* program minimum rate by default */
+
+	val = pll_readl_base(pll);
+	if (val & PLL_BASE_ENABLE)
+		WARN_ON(val & pll_params->iddq_bit_idx);
+	else {
+		int m;
+
+		m = _pll_fixed_mdiv(pll_params, parent_rate);
+		val = m << PLL_BASE_DIVM_SHIFT;
+		val |= (pll_params->vco_min / parent_rate)
+				<< PLL_BASE_DIVN_SHIFT;
+		pll_writel_base(val, pll);
+	}
+
+	/* disable lock override */
+
+	val = pll_readl_misc(pll);
+	val &= ~BIT(29);
+	pll_writel_misc(val, pll);
+
+	pll_flags |= TEGRA_PLL_LOCK_MISC;
+	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
+				      &tegra_clk_pllre_ops);
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
+}
+
+struct clk *tegra_clk_register_pllm(const char *name, const char *parent_name,
+			  void __iomem *clk_base, void __iomem *pmc,
+			  unsigned long flags, unsigned long fixed_rate,
+			  struct tegra_clk_pll_params *pll_params,
+			  u32 pll_flags,
+			  struct tegra_clk_pll_freq_table *freq_table,
+			  spinlock_t *lock)
+{
+	struct tegra_clk_pll *pll;
+	struct clk *clk;
+
+	if (!pll_params->pdiv_tohw)
+		return ERR_PTR(-EINVAL);
+
+	pll_flags |= TEGRA_PLL_BYPASS;
+	pll_flags |= TEGRA_PLL_HAS_LOCK_ENABLE;
+	pll = _tegra_init_pll(clk_base, pmc, fixed_rate, pll_params, pll_flags,
+			      freq_table, lock);
+	if (IS_ERR(pll))
+		return ERR_CAST(pll);
+
+	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
+				      &tegra_clk_pllm_ops);
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
+}
+
+struct clk *tegra_clk_register_pllc(const char *name, const char *parent_name,
+			  void __iomem *clk_base, void __iomem *pmc,
+			  unsigned long flags, unsigned long fixed_rate,
+			  struct tegra_clk_pll_params *pll_params,
+			  u32 pll_flags,
+			  struct tegra_clk_pll_freq_table *freq_table,
+			  spinlock_t *lock)
+{
+	struct clk *parent, *clk;
+	struct pdiv_map *p_tohw = pll_params->pdiv_tohw;
+	struct tegra_clk_pll *pll;
+	struct tegra_clk_pll_freq_table cfg;
+	unsigned long parent_rate;
+
+	if (!p_tohw)
+		return ERR_PTR(-EINVAL);
+
+	parent = __clk_lookup(parent_name);
+	if (IS_ERR(parent)) {
+		WARN(1, "parent clk %s of %s must be registered first\n",
+			name, parent_name);
+		return ERR_PTR(-EINVAL);
+	}
+
+	pll_flags |= TEGRA_PLL_BYPASS;
+	pll = _tegra_init_pll(clk_base, pmc, fixed_rate, pll_params, pll_flags,
+			      freq_table, lock);
+	if (IS_ERR(pll))
+		return ERR_CAST(pll);
+
+	parent_rate = __clk_get_rate(parent);
+
+	/*
+	 * Most of PLLC register fields are shadowed, and can not be read
+	 * directly from PLL h/w. Hence, actual PLLC boot state is unknown.
+	 * Initialize PLL to default state: disabled, reset; shadow registers
+	 * loaded with default parameters; dividers are preset for half of
+	 * minimum VCO rate (the latter assured that shadowed divider settings
+	 * are within supported range).
+	 */
+
+	cfg.m = _pll_fixed_mdiv(pll_params, parent_rate);
+	cfg.n = cfg.m * pll_params->vco_min / parent_rate;
+
+	while (p_tohw->pdiv) {
+		if (p_tohw->pdiv == 2) {
+			cfg.p = p_tohw->hw_val;
+			break;
+		}
+		p_tohw++;
+	}
+
+	if (!p_tohw->pdiv) {
+		WARN_ON(1);
+		return ERR_PTR(-EINVAL);
+	}
+
+	pll_writel_base(0, pll);
+	_update_pll_mnp(pll, &cfg);
+
+	pll_writel_misc(PLLCX_MISC_DEFAULT, pll);
+	pll_writel(PLLCX_MISC1_DEFAULT, pll_params->ext_misc_reg[0], pll);
+	pll_writel(PLLCX_MISC2_DEFAULT, pll_params->ext_misc_reg[1], pll);
+	pll_writel(PLLCX_MISC3_DEFAULT, pll_params->ext_misc_reg[2], pll);
+
+	_pllcx_update_dynamic_coef(pll, parent_rate, cfg.n);
+
+	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
+				      &tegra_clk_pllc_ops);
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
+}
+
+struct clk *tegra_clk_register_plle_tegra114(const char *name,
+				const char *parent_name,
+				void __iomem *clk_base, unsigned long flags,
+				unsigned long fixed_rate,
+				struct tegra_clk_pll_params *pll_params,
+				struct tegra_clk_pll_freq_table *freq_table,
+				spinlock_t *lock)
+{
+	struct tegra_clk_pll *pll;
+	struct clk *clk;
+	u32 val, val_aux;
+
+	pll = _tegra_init_pll(clk_base, NULL, fixed_rate, pll_params,
+			      TEGRA_PLL_HAS_LOCK_ENABLE, freq_table, lock);
+	if (IS_ERR(pll))
+		return ERR_CAST(pll);
+
+	/* ensure parent is set to pll_re_vco */
+
+	val = pll_readl_base(pll);
+	val_aux = pll_readl(pll_params->aux_reg, pll);
+
+	if (val & PLL_BASE_ENABLE) {
+		if (!(val_aux & PLLE_AUX_PLLRE_SEL))
+			WARN(1, "pll_e enabled with unsupported parent %s\n",
+			  (val & PLLE_AUX_PLLP_SEL) ? "pllp_out0" : "pll_ref");
+	} else {
+		val_aux |= PLLE_AUX_PLLRE_SEL;
+		pll_writel(val, pll_params->aux_reg, pll);
+	}
+
+	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
+				      &tegra_clk_plle_tegra114_ops);
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
 }
+#endif
diff --git a/drivers/clk/tegra/clk-tegra114.c b/drivers/clk/tegra/clk-tegra114.c
new file mode 100644
index 000000000000..d78e16ee161c
--- /dev/null
+++ b/drivers/clk/tegra/clk-tegra114.c
@@ -0,0 +1,2085 @@
+/*
+ * Copyright (c) 2012, 2013, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/io.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/delay.h>
+#include <linux/clk/tegra.h>
+
+#include "clk.h"
+
+#define RST_DEVICES_L			0x004
+#define RST_DEVICES_H			0x008
+#define RST_DEVICES_U			0x00C
+#define RST_DEVICES_V			0x358
+#define RST_DEVICES_W			0x35C
+#define RST_DEVICES_X			0x28C
+#define RST_DEVICES_SET_L		0x300
+#define RST_DEVICES_CLR_L		0x304
+#define RST_DEVICES_SET_H		0x308
+#define RST_DEVICES_CLR_H		0x30c
+#define RST_DEVICES_SET_U		0x310
+#define RST_DEVICES_CLR_U		0x314
+#define RST_DEVICES_SET_V		0x430
+#define RST_DEVICES_CLR_V		0x434
+#define RST_DEVICES_SET_W		0x438
+#define RST_DEVICES_CLR_W		0x43c
+#define RST_DEVICES_NUM			5
+
+#define CLK_OUT_ENB_L			0x010
+#define CLK_OUT_ENB_H			0x014
+#define CLK_OUT_ENB_U			0x018
+#define CLK_OUT_ENB_V			0x360
+#define CLK_OUT_ENB_W			0x364
+#define CLK_OUT_ENB_X			0x280
+#define CLK_OUT_ENB_SET_L		0x320
+#define CLK_OUT_ENB_CLR_L		0x324
+#define CLK_OUT_ENB_SET_H		0x328
+#define CLK_OUT_ENB_CLR_H		0x32c
+#define CLK_OUT_ENB_SET_U		0x330
+#define CLK_OUT_ENB_CLR_U		0x334
+#define CLK_OUT_ENB_SET_V		0x440
+#define CLK_OUT_ENB_CLR_V		0x444
+#define CLK_OUT_ENB_SET_W		0x448
+#define CLK_OUT_ENB_CLR_W		0x44c
+#define CLK_OUT_ENB_SET_X		0x284
+#define CLK_OUT_ENB_CLR_X		0x288
+#define CLK_OUT_ENB_NUM			6
+
+#define PLLC_BASE 0x80
+#define PLLC_MISC2 0x88
+#define PLLC_MISC 0x8c
+#define PLLC2_BASE 0x4e8
+#define PLLC2_MISC 0x4ec
+#define PLLC3_BASE 0x4fc
+#define PLLC3_MISC 0x500
+#define PLLM_BASE 0x90
+#define PLLM_MISC 0x9c
+#define PLLP_BASE 0xa0
+#define PLLP_MISC 0xac
+#define PLLX_BASE 0xe0
+#define PLLX_MISC 0xe4
+#define PLLX_MISC2 0x514
+#define PLLX_MISC3 0x518
+#define PLLD_BASE 0xd0
+#define PLLD_MISC 0xdc
+#define PLLD2_BASE 0x4b8
+#define PLLD2_MISC 0x4bc
+#define PLLE_BASE 0xe8
+#define PLLE_MISC 0xec
+#define PLLA_BASE 0xb0
+#define PLLA_MISC 0xbc
+#define PLLU_BASE 0xc0
+#define PLLU_MISC 0xcc
+#define PLLRE_BASE 0x4c4
+#define PLLRE_MISC 0x4c8
+
+#define PLL_MISC_LOCK_ENABLE 18
+#define PLLC_MISC_LOCK_ENABLE 24
+#define PLLDU_MISC_LOCK_ENABLE 22
+#define PLLE_MISC_LOCK_ENABLE 9
+#define PLLRE_MISC_LOCK_ENABLE 30
+
+#define PLLC_IDDQ_BIT 26
+#define PLLX_IDDQ_BIT 3
+#define PLLRE_IDDQ_BIT 16
+
+#define PLL_BASE_LOCK BIT(27)
+#define PLLE_MISC_LOCK BIT(11)
+#define PLLRE_MISC_LOCK BIT(24)
+#define PLLCX_BASE_LOCK (BIT(26)|BIT(27))
+
+#define PLLE_AUX 0x48c
+#define PLLC_OUT 0x84
+#define PLLM_OUT 0x94
+#define PLLP_OUTA 0xa4
+#define PLLP_OUTB 0xa8
+#define PLLA_OUT 0xb4
+
+#define AUDIO_SYNC_CLK_I2S0 0x4a0
+#define AUDIO_SYNC_CLK_I2S1 0x4a4
+#define AUDIO_SYNC_CLK_I2S2 0x4a8
+#define AUDIO_SYNC_CLK_I2S3 0x4ac
+#define AUDIO_SYNC_CLK_I2S4 0x4b0
+#define AUDIO_SYNC_CLK_SPDIF 0x4b4
+
+#define AUDIO_SYNC_DOUBLER 0x49c
+
+#define PMC_CLK_OUT_CNTRL 0x1a8
+#define PMC_DPD_PADS_ORIDE 0x1c
+#define PMC_DPD_PADS_ORIDE_BLINK_ENB 20
+#define PMC_CTRL 0
+#define PMC_CTRL_BLINK_ENB 7
+
+#define OSC_CTRL			0x50
+#define OSC_CTRL_OSC_FREQ_SHIFT		28
+#define OSC_CTRL_PLL_REF_DIV_SHIFT	26
+
+#define PLLXC_SW_MAX_P			6
+
+#define CCLKG_BURST_POLICY 0x368
+#define CCLKLP_BURST_POLICY 0x370
+#define SCLK_BURST_POLICY 0x028
+#define SYSTEM_CLK_RATE 0x030
+
+#define UTMIP_PLL_CFG2 0x488
+#define UTMIP_PLL_CFG2_STABLE_COUNT(x) (((x) & 0xffff) << 6)
+#define UTMIP_PLL_CFG2_ACTIVE_DLY_COUNT(x) (((x) & 0x3f) << 18)
+#define UTMIP_PLL_CFG2_FORCE_PD_SAMP_A_POWERDOWN BIT(0)
+#define UTMIP_PLL_CFG2_FORCE_PD_SAMP_B_POWERDOWN BIT(2)
+#define UTMIP_PLL_CFG2_FORCE_PD_SAMP_C_POWERDOWN BIT(4)
+
+#define UTMIP_PLL_CFG1 0x484
+#define UTMIP_PLL_CFG1_ENABLE_DLY_COUNT(x) (((x) & 0x1f) << 6)
+#define UTMIP_PLL_CFG1_XTAL_FREQ_COUNT(x) (((x) & 0xfff) << 0)
+#define UTMIP_PLL_CFG1_FORCE_PLLU_POWERUP BIT(17)
+#define UTMIP_PLL_CFG1_FORCE_PLLU_POWERDOWN BIT(16)
+#define UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERUP BIT(15)
+#define UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERDOWN BIT(14)
+#define UTMIP_PLL_CFG1_FORCE_PLL_ACTIVE_POWERDOWN BIT(12)
+
+#define UTMIPLL_HW_PWRDN_CFG0			0x52c
+#define UTMIPLL_HW_PWRDN_CFG0_SEQ_START_STATE	BIT(25)
+#define UTMIPLL_HW_PWRDN_CFG0_SEQ_ENABLE	BIT(24)
+#define UTMIPLL_HW_PWRDN_CFG0_USE_LOCKDET	BIT(6)
+#define UTMIPLL_HW_PWRDN_CFG0_SEQ_RESET_INPUT_VALUE	BIT(5)
+#define UTMIPLL_HW_PWRDN_CFG0_SEQ_IN_SWCTL	BIT(4)
+#define UTMIPLL_HW_PWRDN_CFG0_CLK_ENABLE_SWCTL	BIT(2)
+#define UTMIPLL_HW_PWRDN_CFG0_IDDQ_OVERRIDE	BIT(1)
+#define UTMIPLL_HW_PWRDN_CFG0_IDDQ_SWCTL	BIT(0)
+
+#define CLK_SOURCE_I2S0 0x1d8
+#define CLK_SOURCE_I2S1 0x100
+#define CLK_SOURCE_I2S2 0x104
+#define CLK_SOURCE_NDFLASH 0x160
+#define CLK_SOURCE_I2S3 0x3bc
+#define CLK_SOURCE_I2S4 0x3c0
+#define CLK_SOURCE_SPDIF_OUT 0x108
+#define CLK_SOURCE_SPDIF_IN 0x10c
+#define CLK_SOURCE_PWM 0x110
+#define CLK_SOURCE_ADX 0x638
+#define CLK_SOURCE_AMX 0x63c
+#define CLK_SOURCE_HDA 0x428
+#define CLK_SOURCE_HDA2CODEC_2X 0x3e4
+#define CLK_SOURCE_SBC1 0x134
+#define CLK_SOURCE_SBC2 0x118
+#define CLK_SOURCE_SBC3 0x11c
+#define CLK_SOURCE_SBC4 0x1b4
+#define CLK_SOURCE_SBC5 0x3c8
+#define CLK_SOURCE_SBC6 0x3cc
+#define CLK_SOURCE_SATA_OOB 0x420
+#define CLK_SOURCE_SATA 0x424
+#define CLK_SOURCE_NDSPEED 0x3f8
+#define CLK_SOURCE_VFIR 0x168
+#define CLK_SOURCE_SDMMC1 0x150
+#define CLK_SOURCE_SDMMC2 0x154
+#define CLK_SOURCE_SDMMC3 0x1bc
+#define CLK_SOURCE_SDMMC4 0x164
+#define CLK_SOURCE_VDE 0x1c8
+#define CLK_SOURCE_CSITE 0x1d4
+#define CLK_SOURCE_LA 0x1f8
+#define CLK_SOURCE_TRACE 0x634
+#define CLK_SOURCE_OWR 0x1cc
+#define CLK_SOURCE_NOR 0x1d0
+#define CLK_SOURCE_MIPI 0x174
+#define CLK_SOURCE_I2C1 0x124
+#define CLK_SOURCE_I2C2 0x198
+#define CLK_SOURCE_I2C3 0x1b8
+#define CLK_SOURCE_I2C4 0x3c4
+#define CLK_SOURCE_I2C5 0x128
+#define CLK_SOURCE_UARTA 0x178
+#define CLK_SOURCE_UARTB 0x17c
+#define CLK_SOURCE_UARTC 0x1a0
+#define CLK_SOURCE_UARTD 0x1c0
+#define CLK_SOURCE_UARTE 0x1c4
+#define CLK_SOURCE_UARTA_DBG 0x178
+#define CLK_SOURCE_UARTB_DBG 0x17c
+#define CLK_SOURCE_UARTC_DBG 0x1a0
+#define CLK_SOURCE_UARTD_DBG 0x1c0
+#define CLK_SOURCE_UARTE_DBG 0x1c4
+#define CLK_SOURCE_3D 0x158
+#define CLK_SOURCE_2D 0x15c
+#define CLK_SOURCE_VI_SENSOR 0x1a8
+#define CLK_SOURCE_VI 0x148
+#define CLK_SOURCE_EPP 0x16c
+#define CLK_SOURCE_MSENC 0x1f0
+#define CLK_SOURCE_TSEC 0x1f4
+#define CLK_SOURCE_HOST1X 0x180
+#define CLK_SOURCE_HDMI 0x18c
+#define CLK_SOURCE_DISP1 0x138
+#define CLK_SOURCE_DISP2 0x13c
+#define CLK_SOURCE_CILAB 0x614
+#define CLK_SOURCE_CILCD 0x618
+#define CLK_SOURCE_CILE 0x61c
+#define CLK_SOURCE_DSIALP 0x620
+#define CLK_SOURCE_DSIBLP 0x624
+#define CLK_SOURCE_TSENSOR 0x3b8
+#define CLK_SOURCE_D_AUDIO 0x3d0
+#define CLK_SOURCE_DAM0 0x3d8
+#define CLK_SOURCE_DAM1 0x3dc
+#define CLK_SOURCE_DAM2 0x3e0
+#define CLK_SOURCE_ACTMON 0x3e8
+#define CLK_SOURCE_EXTERN1 0x3ec
+#define CLK_SOURCE_EXTERN2 0x3f0
+#define CLK_SOURCE_EXTERN3 0x3f4
+#define CLK_SOURCE_I2CSLOW 0x3fc
+#define CLK_SOURCE_SE 0x42c
+#define CLK_SOURCE_MSELECT 0x3b4
+#define CLK_SOURCE_SOC_THERM 0x644
+#define CLK_SOURCE_XUSB_HOST_SRC 0x600
+#define CLK_SOURCE_XUSB_FALCON_SRC 0x604
+#define CLK_SOURCE_XUSB_FS_SRC 0x608
+#define CLK_SOURCE_XUSB_SS_SRC 0x610
+#define CLK_SOURCE_XUSB_DEV_SRC 0x60c
+#define CLK_SOURCE_EMC 0x19c
+
+static int periph_clk_enb_refcnt[CLK_OUT_ENB_NUM * 32];
+
+static void __iomem *clk_base;
+static void __iomem *pmc_base;
+
+static DEFINE_SPINLOCK(pll_d_lock);
+static DEFINE_SPINLOCK(pll_d2_lock);
+static DEFINE_SPINLOCK(pll_u_lock);
+static DEFINE_SPINLOCK(pll_div_lock);
+static DEFINE_SPINLOCK(pll_re_lock);
+static DEFINE_SPINLOCK(clk_doubler_lock);
+static DEFINE_SPINLOCK(clk_out_lock);
+static DEFINE_SPINLOCK(sysrate_lock);
+
+static struct pdiv_map pllxc_p[] = {
+	{ .pdiv = 1, .hw_val = 0 },
+	{ .pdiv = 2, .hw_val = 1 },
+	{ .pdiv = 3, .hw_val = 2 },
+	{ .pdiv = 4, .hw_val = 3 },
+	{ .pdiv = 5, .hw_val = 4 },
+	{ .pdiv = 6, .hw_val = 5 },
+	{ .pdiv = 8, .hw_val = 6 },
+	{ .pdiv = 10, .hw_val = 7 },
+	{ .pdiv = 12, .hw_val = 8 },
+	{ .pdiv = 16, .hw_val = 9 },
+	{ .pdiv = 12, .hw_val = 10 },
+	{ .pdiv = 16, .hw_val = 11 },
+	{ .pdiv = 20, .hw_val = 12 },
+	{ .pdiv = 24, .hw_val = 13 },
+	{ .pdiv = 32, .hw_val = 14 },
+	{ .pdiv = 0, .hw_val = 0 },
+};
+
+static struct tegra_clk_pll_freq_table pll_c_freq_table[] = {
+	{ 12000000, 624000000, 104, 0, 2},
+	{ 12000000, 600000000, 100, 0, 2},
+	{ 13000000, 600000000,  92, 0, 2},	/* actual: 598.0 MHz */
+	{ 16800000, 600000000,  71, 0, 2},	/* actual: 596.4 MHz */
+	{ 19200000, 600000000,  62, 0, 2},	/* actual: 595.2 MHz */
+	{ 26000000, 600000000,  92, 1, 2},	/* actual: 598.0 MHz */
+	{ 0, 0, 0, 0, 0, 0 },
+};
+
+static struct tegra_clk_pll_params pll_c_params = {
+	.input_min = 12000000,
+	.input_max = 800000000,
+	.cf_min = 12000000,
+	.cf_max = 19200000,	/* s/w policy, h/w capability 50 MHz */
+	.vco_min = 600000000,
+	.vco_max = 1400000000,
+	.base_reg = PLLC_BASE,
+	.misc_reg = PLLC_MISC,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_enable_bit_idx = PLLC_MISC_LOCK_ENABLE,
+	.lock_delay = 300,
+	.iddq_reg = PLLC_MISC,
+	.iddq_bit_idx = PLLC_IDDQ_BIT,
+	.max_p = PLLXC_SW_MAX_P,
+	.dyn_ramp_reg = PLLC_MISC2,
+	.stepa_shift = 17,
+	.stepb_shift = 9,
+	.pdiv_tohw = pllxc_p,
+};
+
+static struct pdiv_map pllc_p[] = {
+	{ .pdiv = 1, .hw_val = 0 },
+	{ .pdiv = 2, .hw_val = 1 },
+	{ .pdiv = 4, .hw_val = 3 },
+	{ .pdiv = 8, .hw_val = 5 },
+	{ .pdiv = 16, .hw_val = 7 },
+	{ .pdiv = 0, .hw_val = 0 },
+};
+
+static struct tegra_clk_pll_freq_table pll_cx_freq_table[] = {
+	{12000000, 600000000, 100, 0, 2},
+	{13000000, 600000000, 92, 0, 2},	/* actual: 598.0 MHz */
+	{16800000, 600000000, 71, 0, 2},	/* actual: 596.4 MHz */
+	{19200000, 600000000, 62, 0, 2},	/* actual: 595.2 MHz */
+	{26000000, 600000000, 92, 1, 2},	/* actual: 598.0 MHz */
+	{0, 0, 0, 0, 0, 0},
+};
+
+static struct tegra_clk_pll_params pll_c2_params = {
+	.input_min = 12000000,
+	.input_max = 48000000,
+	.cf_min = 12000000,
+	.cf_max = 19200000,
+	.vco_min = 600000000,
+	.vco_max = 1200000000,
+	.base_reg = PLLC2_BASE,
+	.misc_reg = PLLC2_MISC,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
+	.lock_delay = 300,
+	.pdiv_tohw = pllc_p,
+	.ext_misc_reg[0] = 0x4f0,
+	.ext_misc_reg[1] = 0x4f4,
+	.ext_misc_reg[2] = 0x4f8,
+};
+
+static struct tegra_clk_pll_params pll_c3_params = {
+	.input_min = 12000000,
+	.input_max = 48000000,
+	.cf_min = 12000000,
+	.cf_max = 19200000,
+	.vco_min = 600000000,
+	.vco_max = 1200000000,
+	.base_reg = PLLC3_BASE,
+	.misc_reg = PLLC3_MISC,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
+	.lock_delay = 300,
+	.pdiv_tohw = pllc_p,
+	.ext_misc_reg[0] = 0x504,
+	.ext_misc_reg[1] = 0x508,
+	.ext_misc_reg[2] = 0x50c,
+};
+
+static struct pdiv_map pllm_p[] = {
+	{ .pdiv = 1, .hw_val = 0 },
+	{ .pdiv = 2, .hw_val = 1 },
+	{ .pdiv = 0, .hw_val = 0 },
+};
+
+static struct tegra_clk_pll_freq_table pll_m_freq_table[] = {
+	{12000000, 800000000, 66, 0, 1},	/* actual: 792.0 MHz */
+	{13000000, 800000000, 61, 0, 1},	/* actual: 793.0 MHz */
+	{16800000, 800000000, 47, 0, 1},	/* actual: 789.6 MHz */
+	{19200000, 800000000, 41, 0, 1},	/* actual: 787.2 MHz */
+	{26000000, 800000000, 61, 1, 1},	/* actual: 793.0 MHz */
+	{0, 0, 0, 0, 0, 0},
+};
+
+static struct tegra_clk_pll_params pll_m_params = {
+	.input_min = 12000000,
+	.input_max = 500000000,
+	.cf_min = 12000000,
+	.cf_max = 19200000,	/* s/w policy, h/w capability 50 MHz */
+	.vco_min = 400000000,
+	.vco_max = 1066000000,
+	.base_reg = PLLM_BASE,
+	.misc_reg = PLLM_MISC,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
+	.lock_delay = 300,
+	.max_p = 2,
+	.pdiv_tohw = pllm_p,
+};
+
+static struct tegra_clk_pll_freq_table pll_p_freq_table[] = {
+	{12000000, 216000000, 432, 12, 1, 8},
+	{13000000, 216000000, 432, 13, 1, 8},
+	{16800000, 216000000, 360, 14, 1, 8},
+	{19200000, 216000000, 360, 16, 1, 8},
+	{26000000, 216000000, 432, 26, 1, 8},
+	{0, 0, 0, 0, 0, 0},
+};
+
+static struct tegra_clk_pll_params pll_p_params = {
+	.input_min = 2000000,
+	.input_max = 31000000,
+	.cf_min = 1000000,
+	.cf_max = 6000000,
+	.vco_min = 200000000,
+	.vco_max = 700000000,
+	.base_reg = PLLP_BASE,
+	.misc_reg = PLLP_MISC,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
+	.lock_delay = 300,
+};
+
+static struct tegra_clk_pll_freq_table pll_a_freq_table[] = {
+	{9600000, 282240000, 147, 5, 0, 4},
+	{9600000, 368640000, 192, 5, 0, 4},
+	{9600000, 240000000, 200, 8, 0, 8},
+
+	{28800000, 282240000, 245, 25, 0, 8},
+	{28800000, 368640000, 320, 25, 0, 8},
+	{28800000, 240000000, 200, 24, 0, 8},
+	{0, 0, 0, 0, 0, 0},
+};
+
+
+static struct tegra_clk_pll_params pll_a_params = {
+	.input_min = 2000000,
+	.input_max = 31000000,
+	.cf_min = 1000000,
+	.cf_max = 6000000,
+	.vco_min = 200000000,
+	.vco_max = 700000000,
+	.base_reg = PLLA_BASE,
+	.misc_reg = PLLA_MISC,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
+	.lock_delay = 300,
+};
+
+static struct tegra_clk_pll_freq_table pll_d_freq_table[] = {
+	{12000000, 216000000, 864, 12, 2, 12},
+	{13000000, 216000000, 864, 13, 2, 12},
+	{16800000, 216000000, 720, 14, 2, 12},
+	{19200000, 216000000, 720, 16, 2, 12},
+	{26000000, 216000000, 864, 26, 2, 12},
+
+	{12000000, 594000000, 594, 12, 0, 12},
+	{13000000, 594000000, 594, 13, 0, 12},
+	{16800000, 594000000, 495, 14, 0, 12},
+	{19200000, 594000000, 495, 16, 0, 12},
+	{26000000, 594000000, 594, 26, 0, 12},
+
+	{12000000, 1000000000, 1000, 12, 0, 12},
+	{13000000, 1000000000, 1000, 13, 0, 12},
+	{19200000, 1000000000, 625, 12, 0, 12},
+	{26000000, 1000000000, 1000, 26, 0, 12},
+
+	{0, 0, 0, 0, 0, 0},
+};
+
+static struct tegra_clk_pll_params pll_d_params = {
+	.input_min = 2000000,
+	.input_max = 40000000,
+	.cf_min = 1000000,
+	.cf_max = 6000000,
+	.vco_min = 500000000,
+	.vco_max = 1000000000,
+	.base_reg = PLLD_BASE,
+	.misc_reg = PLLD_MISC,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_enable_bit_idx = PLLDU_MISC_LOCK_ENABLE,
+	.lock_delay = 1000,
+};
+
+static struct tegra_clk_pll_params pll_d2_params = {
+	.input_min = 2000000,
+	.input_max = 40000000,
+	.cf_min = 1000000,
+	.cf_max = 6000000,
+	.vco_min = 500000000,
+	.vco_max = 1000000000,
+	.base_reg = PLLD2_BASE,
+	.misc_reg = PLLD2_MISC,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_enable_bit_idx = PLLDU_MISC_LOCK_ENABLE,
+	.lock_delay = 1000,
+};
+
+static struct pdiv_map pllu_p[] = {
+	{ .pdiv = 1, .hw_val = 1 },
+	{ .pdiv = 2, .hw_val = 0 },
+	{ .pdiv = 0, .hw_val = 0 },
+};
+
+static struct tegra_clk_pll_freq_table pll_u_freq_table[] = {
+	{12000000, 480000000, 960, 12, 0, 12},
+	{13000000, 480000000, 960, 13, 0, 12},
+	{16800000, 480000000, 400, 7, 0, 5},
+	{19200000, 480000000, 200, 4, 0, 3},
+	{26000000, 480000000, 960, 26, 0, 12},
+	{0, 0, 0, 0, 0, 0},
+};
+
+static struct tegra_clk_pll_params pll_u_params = {
+	.input_min = 2000000,
+	.input_max = 40000000,
+	.cf_min = 1000000,
+	.cf_max = 6000000,
+	.vco_min = 480000000,
+	.vco_max = 960000000,
+	.base_reg = PLLU_BASE,
+	.misc_reg = PLLU_MISC,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_enable_bit_idx = PLLDU_MISC_LOCK_ENABLE,
+	.lock_delay = 1000,
+	.pdiv_tohw = pllu_p,
+};
+
+static struct tegra_clk_pll_freq_table pll_x_freq_table[] = {
+	/* 1 GHz */
+	{12000000, 1000000000, 83, 0, 1},	/* actual: 996.0 MHz */
+	{13000000, 1000000000, 76, 0, 1},	/* actual: 988.0 MHz */
+	{16800000, 1000000000, 59, 0, 1},	/* actual: 991.2 MHz */
+	{19200000, 1000000000, 52, 0, 1},	/* actual: 998.4 MHz */
+	{26000000, 1000000000, 76, 1, 1},	/* actual: 988.0 MHz */
+
+	{0, 0, 0, 0, 0, 0},
+};
+
+static struct tegra_clk_pll_params pll_x_params = {
+	.input_min = 12000000,
+	.input_max = 800000000,
+	.cf_min = 12000000,
+	.cf_max = 19200000,	/* s/w policy, h/w capability 50 MHz */
+	.vco_min = 700000000,
+	.vco_max = 2400000000U,
+	.base_reg = PLLX_BASE,
+	.misc_reg = PLLX_MISC,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
+	.lock_delay = 300,
+	.iddq_reg = PLLX_MISC3,
+	.iddq_bit_idx = PLLX_IDDQ_BIT,
+	.max_p = PLLXC_SW_MAX_P,
+	.dyn_ramp_reg = PLLX_MISC2,
+	.stepa_shift = 16,
+	.stepb_shift = 24,
+	.pdiv_tohw = pllxc_p,
+};
+
+static struct tegra_clk_pll_freq_table pll_e_freq_table[] = {
+	/* PLLE special case: use cpcon field to store cml divider value */
+	{336000000, 100000000, 100, 21, 16, 11},
+	{312000000, 100000000, 200, 26, 24, 13},
+	{0, 0, 0, 0, 0, 0},
+};
+
+static struct tegra_clk_pll_params pll_e_params = {
+	.input_min = 12000000,
+	.input_max = 1000000000,
+	.cf_min = 12000000,
+	.cf_max = 75000000,
+	.vco_min = 1600000000,
+	.vco_max = 2400000000U,
+	.base_reg = PLLE_BASE,
+	.misc_reg = PLLE_MISC,
+	.aux_reg = PLLE_AUX,
+	.lock_mask = PLLE_MISC_LOCK,
+	.lock_enable_bit_idx = PLLE_MISC_LOCK_ENABLE,
+	.lock_delay = 300,
+};
+
+static struct tegra_clk_pll_params pll_re_vco_params = {
+	.input_min = 12000000,
+	.input_max = 1000000000,
+	.cf_min = 12000000,
+	.cf_max = 19200000, /* s/w policy, h/w capability 38 MHz */
+	.vco_min = 300000000,
+	.vco_max = 600000000,
+	.base_reg = PLLRE_BASE,
+	.misc_reg = PLLRE_MISC,
+	.lock_mask = PLLRE_MISC_LOCK,
+	.lock_enable_bit_idx = PLLRE_MISC_LOCK_ENABLE,
+	.lock_delay = 300,
+	.iddq_reg = PLLRE_MISC,
+	.iddq_bit_idx = PLLRE_IDDQ_BIT,
+};
+
+/* Peripheral clock registers */
+
+static struct tegra_clk_periph_regs periph_l_regs = {
+	.enb_reg = CLK_OUT_ENB_L,
+	.enb_set_reg = CLK_OUT_ENB_SET_L,
+	.enb_clr_reg = CLK_OUT_ENB_CLR_L,
+	.rst_reg = RST_DEVICES_L,
+	.rst_set_reg = RST_DEVICES_SET_L,
+	.rst_clr_reg = RST_DEVICES_CLR_L,
+};
+
+static struct tegra_clk_periph_regs periph_h_regs = {
+	.enb_reg = CLK_OUT_ENB_H,
+	.enb_set_reg = CLK_OUT_ENB_SET_H,
+	.enb_clr_reg = CLK_OUT_ENB_CLR_H,
+	.rst_reg = RST_DEVICES_H,
+	.rst_set_reg = RST_DEVICES_SET_H,
+	.rst_clr_reg = RST_DEVICES_CLR_H,
+};
+
+static struct tegra_clk_periph_regs periph_u_regs = {
+	.enb_reg = CLK_OUT_ENB_U,
+	.enb_set_reg = CLK_OUT_ENB_SET_U,
+	.enb_clr_reg = CLK_OUT_ENB_CLR_U,
+	.rst_reg = RST_DEVICES_U,
+	.rst_set_reg = RST_DEVICES_SET_U,
+	.rst_clr_reg = RST_DEVICES_CLR_U,
+};
+
+static struct tegra_clk_periph_regs periph_v_regs = {
+	.enb_reg = CLK_OUT_ENB_V,
+	.enb_set_reg = CLK_OUT_ENB_SET_V,
+	.enb_clr_reg = CLK_OUT_ENB_CLR_V,
+	.rst_reg = RST_DEVICES_V,
+	.rst_set_reg = RST_DEVICES_SET_V,
+	.rst_clr_reg = RST_DEVICES_CLR_V,
+};
+
+static struct tegra_clk_periph_regs periph_w_regs = {
+	.enb_reg = CLK_OUT_ENB_W,
+	.enb_set_reg = CLK_OUT_ENB_SET_W,
+	.enb_clr_reg = CLK_OUT_ENB_CLR_W,
+	.rst_reg = RST_DEVICES_W,
+	.rst_set_reg = RST_DEVICES_SET_W,
+	.rst_clr_reg = RST_DEVICES_CLR_W,
+};
+
+/* possible OSC frequencies in Hz */
+static unsigned long tegra114_input_freq[] = {
+	[0] = 13000000,
+	[1] = 16800000,
+	[4] = 19200000,
+	[5] = 38400000,
+	[8] = 12000000,
+	[9] = 48000000,
+	[12] = 260000000,
+};
+
+#define MASK(x) (BIT(x) - 1)
+
+#define TEGRA_INIT_DATA_MUX(_name, _con_id, _dev_id, _parents, _offset,	\
+			    _clk_num, _regs, _gate_flags, _clk_id)	\
+	TEGRA_INIT_DATA_TABLE(_name, _con_id, _dev_id, _parents, _offset,\
+			30, MASK(2), 0, 0, 8, 1, 0, _regs, _clk_num,	\
+			periph_clk_enb_refcnt, _gate_flags, _clk_id,	\
+			_parents##_idx, 0)
+
+#define TEGRA_INIT_DATA_MUX_FLAGS(_name, _con_id, _dev_id, _parents, _offset,\
+			    _clk_num, _regs, _gate_flags, _clk_id, flags)\
+	TEGRA_INIT_DATA_TABLE(_name, _con_id, _dev_id, _parents, _offset,\
+			30, MASK(2), 0, 0, 8, 1, 0, _regs, _clk_num,	\
+			periph_clk_enb_refcnt, _gate_flags, _clk_id,	\
+			_parents##_idx, flags)
+
+#define TEGRA_INIT_DATA_MUX8(_name, _con_id, _dev_id, _parents, _offset, \
+			     _clk_num, _regs, _gate_flags, _clk_id)	\
+	TEGRA_INIT_DATA_TABLE(_name, _con_id, _dev_id, _parents, _offset,\
+			29, MASK(3), 0, 0, 8, 1, 0, _regs, _clk_num,	\
+			periph_clk_enb_refcnt, _gate_flags, _clk_id,	\
+			_parents##_idx, 0)
+
+#define TEGRA_INIT_DATA_INT(_name, _con_id, _dev_id, _parents, _offset,	\
+			    _clk_num, _regs, _gate_flags, _clk_id)	\
+	TEGRA_INIT_DATA_TABLE(_name, _con_id, _dev_id, _parents, _offset,\
+			30, MASK(2), 0, 0, 8, 1, TEGRA_DIVIDER_INT, _regs,\
+			_clk_num, periph_clk_enb_refcnt, _gate_flags,	\
+			_clk_id, _parents##_idx, 0)
+
+#define TEGRA_INIT_DATA_INT_FLAGS(_name, _con_id, _dev_id, _parents, _offset,\
+			    _clk_num, _regs, _gate_flags, _clk_id, flags)\
+	TEGRA_INIT_DATA_TABLE(_name, _con_id, _dev_id, _parents, _offset,\
+			30, MASK(2), 0, 0, 8, 1, TEGRA_DIVIDER_INT, _regs,\
+			_clk_num, periph_clk_enb_refcnt, _gate_flags,	\
+			_clk_id, _parents##_idx, flags)
+
+#define TEGRA_INIT_DATA_INT8(_name, _con_id, _dev_id, _parents, _offset,\
+			    _clk_num, _regs, _gate_flags, _clk_id)	\
+	TEGRA_INIT_DATA_TABLE(_name, _con_id, _dev_id, _parents, _offset,\
+			29, MASK(3), 0, 0, 8, 1, TEGRA_DIVIDER_INT, _regs,\
+			_clk_num, periph_clk_enb_refcnt, _gate_flags,	\
+			_clk_id, _parents##_idx, 0)
+
+#define TEGRA_INIT_DATA_UART(_name, _con_id, _dev_id, _parents, _offset,\
+			     _clk_num, _regs, _clk_id)			\
+	TEGRA_INIT_DATA_TABLE(_name, _con_id, _dev_id, _parents, _offset,\
+			30, MASK(2), 0, 0, 16, 1, TEGRA_DIVIDER_UART, _regs,\
+			_clk_num, periph_clk_enb_refcnt, 0, _clk_id,	\
+			_parents##_idx, 0)
+
+#define TEGRA_INIT_DATA_I2C(_name, _con_id, _dev_id, _parents, _offset,\
+			     _clk_num, _regs, _clk_id)			\
+	TEGRA_INIT_DATA_TABLE(_name, _con_id, _dev_id, _parents, _offset,\
+			30, MASK(2), 0, 0, 16, 0, 0, _regs, _clk_num,	\
+			periph_clk_enb_refcnt, 0, _clk_id, _parents##_idx, 0)
+
+#define TEGRA_INIT_DATA_NODIV(_name, _con_id, _dev_id, _parents, _offset, \
+			      _mux_shift, _mux_mask, _clk_num, _regs,	\
+			      _gate_flags, _clk_id)			\
+	TEGRA_INIT_DATA_TABLE(_name, _con_id, _dev_id, _parents, _offset,\
+			_mux_shift, _mux_mask, 0, 0, 0, 0, 0, _regs,	\
+			_clk_num, periph_clk_enb_refcnt, _gate_flags,	\
+			_clk_id, _parents##_idx, 0)
+
+#define TEGRA_INIT_DATA_XUSB(_name, _con_id, _dev_id, _parents, _offset, \
+			     _clk_num, _regs, _gate_flags, _clk_id)	 \
+	TEGRA_INIT_DATA_TABLE(_name, _con_id, _dev_id, _parents, _offset, \
+			29, MASK(3), 0, 0, 8, 1, TEGRA_DIVIDER_INT, _regs, \
+			_clk_num, periph_clk_enb_refcnt, _gate_flags,	 \
+			_clk_id, _parents##_idx, 0)
+
+#define TEGRA_INIT_DATA_AUDIO(_name, _con_id, _dev_id, _offset,  _clk_num,\
+				 _regs, _gate_flags, _clk_id)		\
+	TEGRA_INIT_DATA_TABLE(_name, _con_id, _dev_id, mux_d_audio_clk,	\
+			_offset, 16, 0xE01F, 0, 0, 8, 1, 0, _regs, _clk_num, \
+			periph_clk_enb_refcnt, _gate_flags , _clk_id,	\
+			mux_d_audio_clk_idx, 0)
+
+enum tegra114_clk {
+	rtc = 4, timer = 5, uarta = 6, sdmmc2 = 9, i2s1 = 11, i2c1 = 12,
+	ndflash = 13, sdmmc1 = 14, sdmmc4 = 15, pwm = 17, i2s2 = 18, epp = 19,
+	gr_2d = 21, usbd = 22, isp = 23, gr_3d = 24, disp2 = 26, disp1 = 27,
+	host1x = 28, vcp = 29, i2s0 = 30, apbdma = 34, kbc = 36, kfuse = 40,
+	sbc1 = 41, nor = 42, sbc2 = 44, sbc3 = 46, i2c5 = 47, dsia = 48,
+	mipi = 50, hdmi = 51, csi = 52, i2c2 = 54, uartc = 55, mipi_cal = 56,
+	emc, usb2, usb3, vde = 61, bsea = 62, bsev = 63, uartd = 65,
+	i2c3 = 67, sbc4 = 68, sdmmc3 = 69, owr = 71, csite = 73,
+	la = 76, trace = 77, soc_therm = 78, dtv = 79, ndspeed = 80,
+	i2cslow = 81, dsib = 82, tsec = 83, xusb_host = 89, msenc = 91,
+	csus = 92, mselect = 99, tsensor = 100, i2s3 = 101, i2s4 = 102,
+	i2c4 = 103, sbc5 = 104, sbc6 = 105, d_audio, apbif = 107, dam0, dam1,
+	dam2, hda2codec_2x = 111, audio0_2x = 113, audio1_2x, audio2_2x,
+	audio3_2x, audio4_2x, spdif_2x, actmon = 119, extern1 = 120,
+	extern2 = 121, extern3 = 122, hda = 125, se = 127, hda2hdmi = 128,
+	cilab = 144, cilcd = 145, cile = 146, dsialp = 147, dsiblp = 148,
+	dds = 150, dp2 = 152, amx = 153, adx = 154, xusb_ss = 156, uartb = 192,
+	vfir, spdif_in, spdif_out, vi, vi_sensor, fuse, fuse_burn, clk_32k,
+	clk_m, clk_m_div2, clk_m_div4, pll_ref, pll_c, pll_c_out1, pll_c2,
+	pll_c3, pll_m, pll_m_out1, pll_p, pll_p_out1, pll_p_out2, pll_p_out3,
+	pll_p_out4, pll_a, pll_a_out0, pll_d, pll_d_out0, pll_d2, pll_d2_out0,
+	pll_u, pll_u_480M, pll_u_60M, pll_u_48M, pll_u_12M, pll_x, pll_x_out0,
+	pll_re_vco, pll_re_out, pll_e_out0, spdif_in_sync, i2s0_sync,
+	i2s1_sync, i2s2_sync, i2s3_sync, i2s4_sync, vimclk_sync, audio0,
+	audio1, audio2, audio3, audio4, spdif, clk_out_1, clk_out_2, clk_out_3,
+	blink, xusb_host_src = 252, xusb_falcon_src, xusb_fs_src, xusb_ss_src,
+	xusb_dev_src, xusb_dev, xusb_hs_src, sclk, hclk, pclk, cclk_g, cclk_lp,
+
+	/* Mux clocks */
+
+	audio0_mux = 300, audio1_mux, audio2_mux, audio3_mux, audio4_mux,
+	spdif_mux, clk_out_1_mux, clk_out_2_mux, clk_out_3_mux, dsia_mux,
+	dsib_mux, clk_max,
+};
+
+struct utmi_clk_param {
+	/* Oscillator Frequency in KHz */
+	u32 osc_frequency;
+	/* UTMIP PLL Enable Delay Count  */
+	u8 enable_delay_count;
+	/* UTMIP PLL Stable count */
+	u8 stable_count;
+	/*  UTMIP PLL Active delay count */
+	u8 active_delay_count;
+	/* UTMIP PLL Xtal frequency count */
+	u8 xtal_freq_count;
+};
+
+static const struct utmi_clk_param utmi_parameters[] = {
+	{.osc_frequency = 13000000, .enable_delay_count = 0x02,
+	 .stable_count = 0x33, .active_delay_count = 0x05,
+	 .xtal_freq_count = 0x7F},
+	{.osc_frequency = 19200000, .enable_delay_count = 0x03,
+	 .stable_count = 0x4B, .active_delay_count = 0x06,
+	 .xtal_freq_count = 0xBB},
+	{.osc_frequency = 12000000, .enable_delay_count = 0x02,
+	 .stable_count = 0x2F, .active_delay_count = 0x04,
+	 .xtal_freq_count = 0x76},
+	{.osc_frequency = 26000000, .enable_delay_count = 0x04,
+	 .stable_count = 0x66, .active_delay_count = 0x09,
+	 .xtal_freq_count = 0xFE},
+	{.osc_frequency = 16800000, .enable_delay_count = 0x03,
+	 .stable_count = 0x41, .active_delay_count = 0x0A,
+	 .xtal_freq_count = 0xA4},
+};
+
+/* peripheral mux definitions */
+
+#define MUX_I2S_SPDIF(_id)						\
+static const char *mux_pllaout0_##_id##_2x_pllp_clkm[] = { "pll_a_out0", \
+							   #_id, "pll_p",\
+							   "clk_m"};
+MUX_I2S_SPDIF(audio0)
+MUX_I2S_SPDIF(audio1)
+MUX_I2S_SPDIF(audio2)
+MUX_I2S_SPDIF(audio3)
+MUX_I2S_SPDIF(audio4)
+MUX_I2S_SPDIF(audio)
+
+#define mux_pllaout0_audio0_2x_pllp_clkm_idx NULL
+#define mux_pllaout0_audio1_2x_pllp_clkm_idx NULL
+#define mux_pllaout0_audio2_2x_pllp_clkm_idx NULL
+#define mux_pllaout0_audio3_2x_pllp_clkm_idx NULL
+#define mux_pllaout0_audio4_2x_pllp_clkm_idx NULL
+#define mux_pllaout0_audio_2x_pllp_clkm_idx NULL
+
+static const char *mux_pllp_pllc_pllm_clkm[] = {
+	"pll_p", "pll_c", "pll_m", "clk_m"
+};
+#define mux_pllp_pllc_pllm_clkm_idx NULL
+
+static const char *mux_pllp_pllc_pllm[] = { "pll_p", "pll_c", "pll_m" };
+#define mux_pllp_pllc_pllm_idx NULL
+
+static const char *mux_pllp_pllc_clk32_clkm[] = {
+	"pll_p", "pll_c", "clk_32k", "clk_m"
+};
+#define mux_pllp_pllc_clk32_clkm_idx NULL
+
+static const char *mux_plla_pllc_pllp_clkm[] = {
+	"pll_a_out0", "pll_c", "pll_p", "clk_m"
+};
+#define mux_plla_pllc_pllp_clkm_idx mux_pllp_pllc_pllm_clkm_idx
+
+static const char *mux_pllp_pllc2_c_c3_pllm_clkm[] = {
+	"pll_p", "pll_c2", "pll_c", "pll_c3", "pll_m", "clk_m"
+};
+static u32 mux_pllp_pllc2_c_c3_pllm_clkm_idx[] = {
+	[0] = 0, [1] = 1, [2] = 2, [3] = 3, [4] = 4, [5] = 6,
+};
+
+static const char *mux_pllp_clkm[] = {
+	"pll_p", "clk_m"
+};
+static u32 mux_pllp_clkm_idx[] = {
+	[0] = 0, [1] = 3,
+};
+
+static const char *mux_pllm_pllc2_c_c3_pllp_plla[] = {
+	"pll_m", "pll_c2", "pll_c", "pll_c3", "pll_p", "pll_a_out0"
+};
+#define mux_pllm_pllc2_c_c3_pllp_plla_idx mux_pllp_pllc2_c_c3_pllm_clkm_idx
+
+static const char *mux_pllp_pllm_plld_plla_pllc_plld2_clkm[] = {
+	"pll_p", "pll_m", "pll_d_out0", "pll_a_out0", "pll_c",
+	"pll_d2_out0", "clk_m"
+};
+#define mux_pllp_pllm_plld_plla_pllc_plld2_clkm_idx NULL
+
+static const char *mux_pllm_pllc_pllp_plla[] = {
+	"pll_m", "pll_c", "pll_p", "pll_a_out0"
+};
+#define mux_pllm_pllc_pllp_plla_idx mux_pllp_pllc_pllm_clkm_idx
+
+static const char *mux_pllp_pllc_clkm[] = {
+	"pll_p", "pll_c", "pll_m"
+};
+static u32 mux_pllp_pllc_clkm_idx[] = {
+	[0] = 0, [1] = 1, [2] = 3,
+};
+
+static const char *mux_pllp_pllc_clkm_clk32[] = {
+	"pll_p", "pll_c", "clk_m", "clk_32k"
+};
+#define mux_pllp_pllc_clkm_clk32_idx NULL
+
+static const char *mux_plla_clk32_pllp_clkm_plle[] = {
+	"pll_a_out0", "clk_32k", "pll_p", "clk_m", "pll_e_out0"
+};
+#define mux_plla_clk32_pllp_clkm_plle_idx NULL
+
+static const char *mux_clkm_pllp_pllc_pllre[] = {
+	"clk_m", "pll_p", "pll_c", "pll_re_out"
+};
+static u32 mux_clkm_pllp_pllc_pllre_idx[] = {
+	[0] = 0, [1] = 1, [2] = 3, [3] = 5,
+};
+
+static const char *mux_clkm_48M_pllp_480M[] = {
+	"clk_m", "pll_u_48M", "pll_p", "pll_u_480M"
+};
+#define mux_clkm_48M_pllp_480M_idx NULL
+
+static const char *mux_clkm_pllre_clk32_480M_pllc_ref[] = {
+	"clk_m", "pll_re_out", "clk_32k", "pll_u_480M", "pll_c", "pll_ref"
+};
+static u32 mux_clkm_pllre_clk32_480M_pllc_ref_idx[] = {
+	[0] = 0, [1] = 1, [2] = 3, [3] = 3, [4] = 4, [5] = 7,
+};
+
+static const char *mux_plld_out0_plld2_out0[] = {
+	"pll_d_out0", "pll_d2_out0",
+};
+#define mux_plld_out0_plld2_out0_idx NULL
+
+static const char *mux_d_audio_clk[] = {
+	"pll_a_out0", "pll_p", "clk_m", "spdif_in_sync", "i2s0_sync",
+	"i2s1_sync", "i2s2_sync", "i2s3_sync", "i2s4_sync", "vimclk_sync",
+};
+static u32 mux_d_audio_clk_idx[] = {
+	[0] = 0, [1] = 0x8000, [2] = 0xc000, [3] = 0xE000, [4] = 0xE001,
+	[5] = 0xE002, [6] = 0xE003, [7] = 0xE004, [8] = 0xE005, [9] = 0xE007,
+};
+
+static const char *mux_pllmcp_clkm[] = {
+	"pll_m_out0", "pll_c_out0", "pll_p_out0", "clk_m", "pll_m_ud",
+};
+
+static const struct clk_div_table pll_re_div_table[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 1, .div = 2 },
+	{ .val = 2, .div = 3 },
+	{ .val = 3, .div = 4 },
+	{ .val = 4, .div = 5 },
+	{ .val = 5, .div = 6 },
+	{ .val = 0, .div = 0 },
+};
+
+static struct clk *clks[clk_max];
+static struct clk_onecell_data clk_data;
+
+static unsigned long osc_freq;
+static unsigned long pll_ref_freq;
+
+static int __init tegra114_osc_clk_init(void __iomem *clk_base)
+{
+	struct clk *clk;
+	u32 val, pll_ref_div;
+
+	val = readl_relaxed(clk_base + OSC_CTRL);
+
+	osc_freq = tegra114_input_freq[val >> OSC_CTRL_OSC_FREQ_SHIFT];
+	if (!osc_freq) {
+		WARN_ON(1);
+		return -EINVAL;
+	}
+
+	/* clk_m */
+	clk = clk_register_fixed_rate(NULL, "clk_m", NULL, CLK_IS_ROOT,
+				      osc_freq);
+	clk_register_clkdev(clk, "clk_m", NULL);
+	clks[clk_m] = clk;
+
+	/* pll_ref */
+	val = (val >> OSC_CTRL_PLL_REF_DIV_SHIFT) & 3;
+	pll_ref_div = 1 << val;
+	clk = clk_register_fixed_factor(NULL, "pll_ref", "clk_m",
+					CLK_SET_RATE_PARENT, 1, pll_ref_div);
+	clk_register_clkdev(clk, "pll_ref", NULL);
+	clks[pll_ref] = clk;
+
+	pll_ref_freq = osc_freq / pll_ref_div;
+
+	return 0;
+}
+
+static void __init tegra114_fixed_clk_init(void __iomem *clk_base)
+{
+	struct clk *clk;
+
+	/* clk_32k */
+	clk = clk_register_fixed_rate(NULL, "clk_32k", NULL, CLK_IS_ROOT,
+				      32768);
+	clk_register_clkdev(clk, "clk_32k", NULL);
+	clks[clk_32k] = clk;
+
+	/* clk_m_div2 */
+	clk = clk_register_fixed_factor(NULL, "clk_m_div2", "clk_m",
+					CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "clk_m_div2", NULL);
+	clks[clk_m_div2] = clk;
+
+	/* clk_m_div4 */
+	clk = clk_register_fixed_factor(NULL, "clk_m_div4", "clk_m",
+					CLK_SET_RATE_PARENT, 1, 4);
+	clk_register_clkdev(clk, "clk_m_div4", NULL);
+	clks[clk_m_div4] = clk;
+
+}
+
+static __init void tegra114_utmi_param_configure(void __iomem *clk_base)
+{
+	u32 reg;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(utmi_parameters); i++) {
+		if (osc_freq == utmi_parameters[i].osc_frequency)
+			break;
+	}
+
+	if (i >= ARRAY_SIZE(utmi_parameters)) {
+		pr_err("%s: Unexpected oscillator freq %lu\n", __func__,
+		       osc_freq);
+		return;
+	}
+
+	reg = readl_relaxed(clk_base + UTMIP_PLL_CFG2);
+
+	/* Program UTMIP PLL stable and active counts */
+	/* [FIXME] arclk_rst.h says WRONG! This should be 1ms -> 0x50 Check! */
+	reg &= ~UTMIP_PLL_CFG2_STABLE_COUNT(~0);
+	reg |= UTMIP_PLL_CFG2_STABLE_COUNT(utmi_parameters[i].stable_count);
+
+	reg &= ~UTMIP_PLL_CFG2_ACTIVE_DLY_COUNT(~0);
+
+	reg |= UTMIP_PLL_CFG2_ACTIVE_DLY_COUNT(utmi_parameters[i].
+					    active_delay_count);
+
+	/* Remove power downs from UTMIP PLL control bits */
+	reg &= ~UTMIP_PLL_CFG2_FORCE_PD_SAMP_A_POWERDOWN;
+	reg &= ~UTMIP_PLL_CFG2_FORCE_PD_SAMP_B_POWERDOWN;
+	reg &= ~UTMIP_PLL_CFG2_FORCE_PD_SAMP_C_POWERDOWN;
+
+	writel_relaxed(reg, clk_base + UTMIP_PLL_CFG2);
+
+	/* Program UTMIP PLL delay and oscillator frequency counts */
+	reg = readl_relaxed(clk_base + UTMIP_PLL_CFG1);
+	reg &= ~UTMIP_PLL_CFG1_ENABLE_DLY_COUNT(~0);
+
+	reg |= UTMIP_PLL_CFG1_ENABLE_DLY_COUNT(utmi_parameters[i].
+					    enable_delay_count);
+
+	reg &= ~UTMIP_PLL_CFG1_XTAL_FREQ_COUNT(~0);
+	reg |= UTMIP_PLL_CFG1_XTAL_FREQ_COUNT(utmi_parameters[i].
+					   xtal_freq_count);
+
+	/* Remove power downs from UTMIP PLL control bits */
+	reg &= ~UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERDOWN;
+	reg &= ~UTMIP_PLL_CFG1_FORCE_PLL_ACTIVE_POWERDOWN;
+	reg &= ~UTMIP_PLL_CFG1_FORCE_PLLU_POWERUP;
+	reg &= ~UTMIP_PLL_CFG1_FORCE_PLLU_POWERDOWN;
+	writel_relaxed(reg, clk_base + UTMIP_PLL_CFG1);
+
+	/* Setup HW control of UTMIPLL */
+	reg = readl_relaxed(clk_base + UTMIPLL_HW_PWRDN_CFG0);
+	reg |= UTMIPLL_HW_PWRDN_CFG0_USE_LOCKDET;
+	reg &= ~UTMIPLL_HW_PWRDN_CFG0_CLK_ENABLE_SWCTL;
+	reg |= UTMIPLL_HW_PWRDN_CFG0_SEQ_START_STATE;
+	writel_relaxed(reg, clk_base + UTMIPLL_HW_PWRDN_CFG0);
+
+	reg = readl_relaxed(clk_base + UTMIP_PLL_CFG1);
+	reg &= ~UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERUP;
+	reg &= ~UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERDOWN;
+	writel_relaxed(reg, clk_base + UTMIP_PLL_CFG1);
+
+	udelay(1);
+
+	/* Setup SW override of UTMIPLL assuming USB2.0
+	   ports are assigned to USB2 */
+	reg = readl_relaxed(clk_base + UTMIPLL_HW_PWRDN_CFG0);
+	reg |= UTMIPLL_HW_PWRDN_CFG0_IDDQ_SWCTL;
+	reg &= ~UTMIPLL_HW_PWRDN_CFG0_IDDQ_OVERRIDE;
+	writel_relaxed(reg, clk_base + UTMIPLL_HW_PWRDN_CFG0);
+
+	udelay(1);
+
+	/* Enable HW control UTMIPLL */
+	reg = readl_relaxed(clk_base + UTMIPLL_HW_PWRDN_CFG0);
+	reg |= UTMIPLL_HW_PWRDN_CFG0_SEQ_ENABLE;
+	writel_relaxed(reg, clk_base + UTMIPLL_HW_PWRDN_CFG0);
+}
+
+static void __init _clip_vco_min(struct tegra_clk_pll_params *pll_params)
+{
+	pll_params->vco_min =
+		DIV_ROUND_UP(pll_params->vco_min, pll_ref_freq) * pll_ref_freq;
+}
+
+static int __init _setup_dynamic_ramp(struct tegra_clk_pll_params *pll_params,
+				      void __iomem *clk_base)
+{
+	u32 val;
+	u32 step_a, step_b;
+
+	switch (pll_ref_freq) {
+	case 12000000:
+	case 13000000:
+	case 26000000:
+		step_a = 0x2B;
+		step_b = 0x0B;
+		break;
+	case 16800000:
+		step_a = 0x1A;
+		step_b = 0x09;
+		break;
+	case 19200000:
+		step_a = 0x12;
+		step_b = 0x08;
+		break;
+	default:
+		pr_err("%s: Unexpected reference rate %lu\n",
+			__func__, pll_ref_freq);
+		WARN_ON(1);
+		return -EINVAL;
+	}
+
+	val = step_a << pll_params->stepa_shift;
+	val |= step_b << pll_params->stepb_shift;
+	writel_relaxed(val, clk_base + pll_params->dyn_ramp_reg);
+
+	return 0;
+}
+
+static void __init _init_iddq(struct tegra_clk_pll_params *pll_params,
+			      void __iomem *clk_base)
+{
+	u32 val, val_iddq;
+
+	val = readl_relaxed(clk_base + pll_params->base_reg);
+	val_iddq = readl_relaxed(clk_base + pll_params->iddq_reg);
+
+	if (val & BIT(30))
+		WARN_ON(val_iddq & BIT(pll_params->iddq_bit_idx));
+	else {
+		val_iddq |= BIT(pll_params->iddq_bit_idx);
+		writel_relaxed(val_iddq, clk_base + pll_params->iddq_reg);
+	}
+}
+
+static void __init tegra114_pll_init(void __iomem *clk_base,
+				     void __iomem *pmc)
+{
+	u32 val;
+	struct clk *clk;
+
+	/* PLLC */
+	_clip_vco_min(&pll_c_params);
+	if (_setup_dynamic_ramp(&pll_c_params, clk_base) >= 0) {
+		_init_iddq(&pll_c_params, clk_base);
+		clk = tegra_clk_register_pllxc("pll_c", "pll_ref", clk_base,
+				pmc, 0, 0, &pll_c_params, TEGRA_PLL_USE_LOCK,
+				pll_c_freq_table, NULL);
+		clk_register_clkdev(clk, "pll_c", NULL);
+		clks[pll_c] = clk;
+
+		/* PLLC_OUT1 */
+		clk = tegra_clk_register_divider("pll_c_out1_div", "pll_c",
+				clk_base + PLLC_OUT, 0, TEGRA_DIVIDER_ROUND_UP,
+				8, 8, 1, NULL);
+		clk = tegra_clk_register_pll_out("pll_c_out1", "pll_c_out1_div",
+					clk_base + PLLC_OUT, 1, 0,
+					CLK_SET_RATE_PARENT, 0, NULL);
+		clk_register_clkdev(clk, "pll_c_out1", NULL);
+		clks[pll_c_out1] = clk;
+	}
+
+	/* PLLC2 */
+	_clip_vco_min(&pll_c2_params);
+	clk = tegra_clk_register_pllc("pll_c2", "pll_ref", clk_base, pmc, 0, 0,
+			     &pll_c2_params, TEGRA_PLL_USE_LOCK,
+			     pll_cx_freq_table, NULL);
+	clk_register_clkdev(clk, "pll_c2", NULL);
+	clks[pll_c2] = clk;
+
+	/* PLLC3 */
+	_clip_vco_min(&pll_c3_params);
+	clk = tegra_clk_register_pllc("pll_c3", "pll_ref", clk_base, pmc, 0, 0,
+			     &pll_c3_params, TEGRA_PLL_USE_LOCK,
+			     pll_cx_freq_table, NULL);
+	clk_register_clkdev(clk, "pll_c3", NULL);
+	clks[pll_c3] = clk;
+
+	/* PLLP */
+	clk = tegra_clk_register_pll("pll_p", "pll_ref", clk_base, pmc, 0,
+			    408000000, &pll_p_params,
+			    TEGRA_PLL_FIXED | TEGRA_PLL_USE_LOCK,
+			    pll_p_freq_table, NULL);
+	clk_register_clkdev(clk, "pll_p", NULL);
+	clks[pll_p] = clk;
+
+	/* PLLP_OUT1 */
+	clk = tegra_clk_register_divider("pll_p_out1_div", "pll_p",
+				clk_base + PLLP_OUTA, 0, TEGRA_DIVIDER_FIXED |
+				TEGRA_DIVIDER_ROUND_UP, 8, 8, 1, &pll_div_lock);
+	clk = tegra_clk_register_pll_out("pll_p_out1", "pll_p_out1_div",
+				clk_base + PLLP_OUTA, 1, 0,
+				CLK_IGNORE_UNUSED | CLK_SET_RATE_PARENT, 0,
+				&pll_div_lock);
+	clk_register_clkdev(clk, "pll_p_out1", NULL);
+	clks[pll_p_out1] = clk;
+
+	/* PLLP_OUT2 */
+	clk = tegra_clk_register_divider("pll_p_out2_div", "pll_p",
+				clk_base + PLLP_OUTA, 0, TEGRA_DIVIDER_FIXED |
+				TEGRA_DIVIDER_ROUND_UP, 24, 8, 1,
+				&pll_div_lock);
+	clk = tegra_clk_register_pll_out("pll_p_out2", "pll_p_out2_div",
+				clk_base + PLLP_OUTA, 17, 16,
+				CLK_IGNORE_UNUSED | CLK_SET_RATE_PARENT, 0,
+				&pll_div_lock);
+	clk_register_clkdev(clk, "pll_p_out2", NULL);
+	clks[pll_p_out2] = clk;
+
+	/* PLLP_OUT3 */
+	clk = tegra_clk_register_divider("pll_p_out3_div", "pll_p",
+				clk_base + PLLP_OUTB, 0, TEGRA_DIVIDER_FIXED |
+				TEGRA_DIVIDER_ROUND_UP, 8, 8, 1, &pll_div_lock);
+	clk = tegra_clk_register_pll_out("pll_p_out3", "pll_p_out3_div",
+				clk_base + PLLP_OUTB, 1, 0,
+				CLK_IGNORE_UNUSED | CLK_SET_RATE_PARENT, 0,
+				&pll_div_lock);
+	clk_register_clkdev(clk, "pll_p_out3", NULL);
+	clks[pll_p_out3] = clk;
+
+	/* PLLP_OUT4 */
+	clk = tegra_clk_register_divider("pll_p_out4_div", "pll_p",
+				clk_base + PLLP_OUTB, 0, TEGRA_DIVIDER_FIXED |
+				TEGRA_DIVIDER_ROUND_UP, 24, 8, 1,
+				&pll_div_lock);
+	clk = tegra_clk_register_pll_out("pll_p_out4", "pll_p_out4_div",
+				clk_base + PLLP_OUTB, 17, 16,
+				CLK_IGNORE_UNUSED | CLK_SET_RATE_PARENT, 0,
+				&pll_div_lock);
+	clk_register_clkdev(clk, "pll_p_out4", NULL);
+	clks[pll_p_out4] = clk;
+
+	/* PLLM */
+	_clip_vco_min(&pll_m_params);
+	clk = tegra_clk_register_pllm("pll_m", "pll_ref", clk_base, pmc,
+			     CLK_IGNORE_UNUSED | CLK_SET_RATE_GATE, 0,
+			     &pll_m_params, TEGRA_PLL_USE_LOCK,
+			     pll_m_freq_table, NULL);
+	clk_register_clkdev(clk, "pll_m", NULL);
+	clks[pll_m] = clk;
+
+	/* PLLM_OUT1 */
+	clk = tegra_clk_register_divider("pll_m_out1_div", "pll_m",
+				clk_base + PLLM_OUT, 0, TEGRA_DIVIDER_ROUND_UP,
+				8, 8, 1, NULL);
+	clk = tegra_clk_register_pll_out("pll_m_out1", "pll_m_out1_div",
+				clk_base + PLLM_OUT, 1, 0, CLK_IGNORE_UNUSED |
+				CLK_SET_RATE_PARENT, 0, NULL);
+	clk_register_clkdev(clk, "pll_m_out1", NULL);
+	clks[pll_m_out1] = clk;
+
+	/* PLLM_UD */
+	clk = clk_register_fixed_factor(NULL, "pll_m_ud", "pll_m",
+					CLK_SET_RATE_PARENT, 1, 1);
+
+	/* PLLX */
+	_clip_vco_min(&pll_x_params);
+	if (_setup_dynamic_ramp(&pll_x_params, clk_base) >= 0) {
+		_init_iddq(&pll_x_params, clk_base);
+		clk = tegra_clk_register_pllxc("pll_x", "pll_ref", clk_base,
+				pmc, CLK_IGNORE_UNUSED, 0, &pll_x_params,
+				TEGRA_PLL_USE_LOCK, pll_x_freq_table, NULL);
+		clk_register_clkdev(clk, "pll_x", NULL);
+		clks[pll_x] = clk;
+	}
+
+	/* PLLX_OUT0 */
+	clk = clk_register_fixed_factor(NULL, "pll_x_out0", "pll_x",
+					CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll_x_out0", NULL);
+	clks[pll_x_out0] = clk;
+
+	/* PLLU */
+	val = readl(clk_base + pll_u_params.base_reg);
+	val &= ~BIT(24); /* disable PLLU_OVERRIDE */
+	writel(val, clk_base + pll_u_params.base_reg);
+
+	clk = tegra_clk_register_pll("pll_u", "pll_ref", clk_base, pmc, 0,
+			    0, &pll_u_params, TEGRA_PLLU |
+			    TEGRA_PLL_HAS_CPCON | TEGRA_PLL_SET_LFCON |
+			    TEGRA_PLL_USE_LOCK, pll_u_freq_table, &pll_u_lock);
+	clk_register_clkdev(clk, "pll_u", NULL);
+	clks[pll_u] = clk;
+
+	tegra114_utmi_param_configure(clk_base);
+
+	/* PLLU_480M */
+	clk = clk_register_gate(NULL, "pll_u_480M", "pll_u",
+				CLK_SET_RATE_PARENT, clk_base + PLLU_BASE,
+				22, 0, &pll_u_lock);
+	clk_register_clkdev(clk, "pll_u_480M", NULL);
+	clks[pll_u_480M] = clk;
+
+	/* PLLU_60M */
+	clk = clk_register_fixed_factor(NULL, "pll_u_60M", "pll_u",
+					CLK_SET_RATE_PARENT, 1, 8);
+	clk_register_clkdev(clk, "pll_u_60M", NULL);
+	clks[pll_u_60M] = clk;
+
+	/* PLLU_48M */
+	clk = clk_register_fixed_factor(NULL, "pll_u_48M", "pll_u",
+					CLK_SET_RATE_PARENT, 1, 10);
+	clk_register_clkdev(clk, "pll_u_48M", NULL);
+	clks[pll_u_48M] = clk;
+
+	/* PLLU_12M */
+	clk = clk_register_fixed_factor(NULL, "pll_u_12M", "pll_u",
+					CLK_SET_RATE_PARENT, 1, 40);
+	clk_register_clkdev(clk, "pll_u_12M", NULL);
+	clks[pll_u_12M] = clk;
+
+	/* PLLD */
+	clk = tegra_clk_register_pll("pll_d", "pll_ref", clk_base, pmc, 0,
+			    0, &pll_d_params,
+			    TEGRA_PLL_HAS_CPCON | TEGRA_PLL_SET_LFCON |
+			    TEGRA_PLL_USE_LOCK, pll_d_freq_table, &pll_d_lock);
+	clk_register_clkdev(clk, "pll_d", NULL);
+	clks[pll_d] = clk;
+
+	/* PLLD_OUT0 */
+	clk = clk_register_fixed_factor(NULL, "pll_d_out0", "pll_d",
+					CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll_d_out0", NULL);
+	clks[pll_d_out0] = clk;
+
+	/* PLLD2 */
+	clk = tegra_clk_register_pll("pll_d2", "pll_ref", clk_base, pmc, 0,
+			    0, &pll_d2_params,
+			    TEGRA_PLL_HAS_CPCON | TEGRA_PLL_SET_LFCON |
+			    TEGRA_PLL_USE_LOCK, pll_d_freq_table, &pll_d2_lock);
+	clk_register_clkdev(clk, "pll_d2", NULL);
+	clks[pll_d2] = clk;
+
+	/* PLLD2_OUT0 */
+	clk = clk_register_fixed_factor(NULL, "pll_d2_out0", "pll_d2",
+					CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll_d2_out0", NULL);
+	clks[pll_d2_out0] = clk;
+
+	/* PLLA */
+	clk = tegra_clk_register_pll("pll_a", "pll_p_out1", clk_base, pmc, 0,
+			    0, &pll_a_params, TEGRA_PLL_HAS_CPCON |
+			    TEGRA_PLL_USE_LOCK, pll_a_freq_table, NULL);
+	clk_register_clkdev(clk, "pll_a", NULL);
+	clks[pll_a] = clk;
+
+	/* PLLA_OUT0 */
+	clk = tegra_clk_register_divider("pll_a_out0_div", "pll_a",
+				clk_base + PLLA_OUT, 0, TEGRA_DIVIDER_ROUND_UP,
+				8, 8, 1, NULL);
+	clk = tegra_clk_register_pll_out("pll_a_out0", "pll_a_out0_div",
+				clk_base + PLLA_OUT, 1, 0, CLK_IGNORE_UNUSED |
+				CLK_SET_RATE_PARENT, 0, NULL);
+	clk_register_clkdev(clk, "pll_a_out0", NULL);
+	clks[pll_a_out0] = clk;
+
+	/* PLLRE */
+	_clip_vco_min(&pll_re_vco_params);
+	clk = tegra_clk_register_pllre("pll_re_vco", "pll_ref", clk_base, pmc,
+			     0, 0, &pll_re_vco_params, TEGRA_PLL_USE_LOCK,
+			     NULL, &pll_re_lock, pll_ref_freq);
+	clk_register_clkdev(clk, "pll_re_vco", NULL);
+	clks[pll_re_vco] = clk;
+
+	clk = clk_register_divider_table(NULL, "pll_re_out", "pll_re_vco", 0,
+					 clk_base + PLLRE_BASE, 16, 4, 0,
+					 pll_re_div_table, &pll_re_lock);
+	clk_register_clkdev(clk, "pll_re_out", NULL);
+	clks[pll_re_out] = clk;
+
+	/* PLLE */
+	clk = tegra_clk_register_plle_tegra114("pll_e_out0", "pll_re_vco",
+				      clk_base, 0, 100000000, &pll_e_params,
+				      pll_e_freq_table, NULL);
+	clk_register_clkdev(clk, "pll_e_out0", NULL);
+	clks[pll_e_out0] = clk;
+}
+
+static const char *mux_audio_sync_clk[] = { "spdif_in_sync", "i2s0_sync",
+	"i2s1_sync", "i2s2_sync", "i2s3_sync", "i2s4_sync", "vimclk_sync",
+};
+
+static const char *clk_out1_parents[] = { "clk_m", "clk_m_div2",
+	"clk_m_div4", "extern1",
+};
+
+static const char *clk_out2_parents[] = { "clk_m", "clk_m_div2",
+	"clk_m_div4", "extern2",
+};
+
+static const char *clk_out3_parents[] = { "clk_m", "clk_m_div2",
+	"clk_m_div4", "extern3",
+};
+
+static void __init tegra114_audio_clk_init(void __iomem *clk_base)
+{
+	struct clk *clk;
+
+	/* spdif_in_sync */
+	clk = tegra_clk_register_sync_source("spdif_in_sync", 24000000,
+					     24000000);
+	clk_register_clkdev(clk, "spdif_in_sync", NULL);
+	clks[spdif_in_sync] = clk;
+
+	/* i2s0_sync */
+	clk = tegra_clk_register_sync_source("i2s0_sync", 24000000, 24000000);
+	clk_register_clkdev(clk, "i2s0_sync", NULL);
+	clks[i2s0_sync] = clk;
+
+	/* i2s1_sync */
+	clk = tegra_clk_register_sync_source("i2s1_sync", 24000000, 24000000);
+	clk_register_clkdev(clk, "i2s1_sync", NULL);
+	clks[i2s1_sync] = clk;
+
+	/* i2s2_sync */
+	clk = tegra_clk_register_sync_source("i2s2_sync", 24000000, 24000000);
+	clk_register_clkdev(clk, "i2s2_sync", NULL);
+	clks[i2s2_sync] = clk;
+
+	/* i2s3_sync */
+	clk = tegra_clk_register_sync_source("i2s3_sync", 24000000, 24000000);
+	clk_register_clkdev(clk, "i2s3_sync", NULL);
+	clks[i2s3_sync] = clk;
+
+	/* i2s4_sync */
+	clk = tegra_clk_register_sync_source("i2s4_sync", 24000000, 24000000);
+	clk_register_clkdev(clk, "i2s4_sync", NULL);
+	clks[i2s4_sync] = clk;
+
+	/* vimclk_sync */
+	clk = tegra_clk_register_sync_source("vimclk_sync", 24000000, 24000000);
+	clk_register_clkdev(clk, "vimclk_sync", NULL);
+	clks[vimclk_sync] = clk;
+
+	/* audio0 */
+	clk = clk_register_mux(NULL, "audio0_mux", mux_audio_sync_clk,
+			       ARRAY_SIZE(mux_audio_sync_clk), 0,
+			       clk_base + AUDIO_SYNC_CLK_I2S0, 0, 3, 0,
+			       NULL);
+	clks[audio0_mux] = clk;
+	clk = clk_register_gate(NULL, "audio0", "audio0_mux", 0,
+				clk_base + AUDIO_SYNC_CLK_I2S0, 4,
+				CLK_GATE_SET_TO_DISABLE, NULL);
+	clk_register_clkdev(clk, "audio0", NULL);
+	clks[audio0] = clk;
+
+	/* audio1 */
+	clk = clk_register_mux(NULL, "audio1_mux", mux_audio_sync_clk,
+			       ARRAY_SIZE(mux_audio_sync_clk), 0,
+			       clk_base + AUDIO_SYNC_CLK_I2S1, 0, 3, 0,
+			       NULL);
+	clks[audio1_mux] = clk;
+	clk = clk_register_gate(NULL, "audio1", "audio1_mux", 0,
+				clk_base + AUDIO_SYNC_CLK_I2S1, 4,
+				CLK_GATE_SET_TO_DISABLE, NULL);
+	clk_register_clkdev(clk, "audio1", NULL);
+	clks[audio1] = clk;
+
+	/* audio2 */
+	clk = clk_register_mux(NULL, "audio2_mux", mux_audio_sync_clk,
+			       ARRAY_SIZE(mux_audio_sync_clk), 0,
+			       clk_base + AUDIO_SYNC_CLK_I2S2, 0, 3, 0,
+			       NULL);
+	clks[audio2_mux] = clk;
+	clk = clk_register_gate(NULL, "audio2", "audio2_mux", 0,
+				clk_base + AUDIO_SYNC_CLK_I2S2, 4,
+				CLK_GATE_SET_TO_DISABLE, NULL);
+	clk_register_clkdev(clk, "audio2", NULL);
+	clks[audio2] = clk;
+
+	/* audio3 */
+	clk = clk_register_mux(NULL, "audio3_mux", mux_audio_sync_clk,
+			       ARRAY_SIZE(mux_audio_sync_clk), 0,
+			       clk_base + AUDIO_SYNC_CLK_I2S3, 0, 3, 0,
+			       NULL);
+	clks[audio3_mux] = clk;
+	clk = clk_register_gate(NULL, "audio3", "audio3_mux", 0,
+				clk_base + AUDIO_SYNC_CLK_I2S3, 4,
+				CLK_GATE_SET_TO_DISABLE, NULL);
+	clk_register_clkdev(clk, "audio3", NULL);
+	clks[audio3] = clk;
+
+	/* audio4 */
+	clk = clk_register_mux(NULL, "audio4_mux", mux_audio_sync_clk,
+			       ARRAY_SIZE(mux_audio_sync_clk), 0,
+			       clk_base + AUDIO_SYNC_CLK_I2S4, 0, 3, 0,
+			       NULL);
+	clks[audio4_mux] = clk;
+	clk = clk_register_gate(NULL, "audio4", "audio4_mux", 0,
+				clk_base + AUDIO_SYNC_CLK_I2S4, 4,
+				CLK_GATE_SET_TO_DISABLE, NULL);
+	clk_register_clkdev(clk, "audio4", NULL);
+	clks[audio4] = clk;
+
+	/* spdif */
+	clk = clk_register_mux(NULL, "spdif_mux", mux_audio_sync_clk,
+			       ARRAY_SIZE(mux_audio_sync_clk), 0,
+			       clk_base + AUDIO_SYNC_CLK_SPDIF, 0, 3, 0,
+			       NULL);
+	clks[spdif_mux] = clk;
+	clk = clk_register_gate(NULL, "spdif", "spdif_mux", 0,
+				clk_base + AUDIO_SYNC_CLK_SPDIF, 4,
+				CLK_GATE_SET_TO_DISABLE, NULL);
+	clk_register_clkdev(clk, "spdif", NULL);
+	clks[spdif] = clk;
+
+	/* audio0_2x */
+	clk = clk_register_fixed_factor(NULL, "audio0_doubler", "audio0",
+					CLK_SET_RATE_PARENT, 2, 1);
+	clk = tegra_clk_register_divider("audio0_div", "audio0_doubler",
+				clk_base + AUDIO_SYNC_DOUBLER, 0, 0, 24, 1,
+				0, &clk_doubler_lock);
+	clk = tegra_clk_register_periph_gate("audio0_2x", "audio0_div",
+				  TEGRA_PERIPH_NO_RESET, clk_base,
+				  CLK_SET_RATE_PARENT, 113, &periph_v_regs,
+				  periph_clk_enb_refcnt);
+	clk_register_clkdev(clk, "audio0_2x", NULL);
+	clks[audio0_2x] = clk;
+
+	/* audio1_2x */
+	clk = clk_register_fixed_factor(NULL, "audio1_doubler", "audio1",
+					CLK_SET_RATE_PARENT, 2, 1);
+	clk = tegra_clk_register_divider("audio1_div", "audio1_doubler",
+				clk_base + AUDIO_SYNC_DOUBLER, 0, 0, 25, 1,
+				0, &clk_doubler_lock);
+	clk = tegra_clk_register_periph_gate("audio1_2x", "audio1_div",
+				  TEGRA_PERIPH_NO_RESET, clk_base,
+				  CLK_SET_RATE_PARENT, 114, &periph_v_regs,
+				  periph_clk_enb_refcnt);
+	clk_register_clkdev(clk, "audio1_2x", NULL);
+	clks[audio1_2x] = clk;
+
+	/* audio2_2x */
+	clk = clk_register_fixed_factor(NULL, "audio2_doubler", "audio2",
+					CLK_SET_RATE_PARENT, 2, 1);
+	clk = tegra_clk_register_divider("audio2_div", "audio2_doubler",
+				clk_base + AUDIO_SYNC_DOUBLER, 0, 0, 26, 1,
+				0, &clk_doubler_lock);
+	clk = tegra_clk_register_periph_gate("audio2_2x", "audio2_div",
+				  TEGRA_PERIPH_NO_RESET, clk_base,
+				  CLK_SET_RATE_PARENT, 115, &periph_v_regs,
+				  periph_clk_enb_refcnt);
+	clk_register_clkdev(clk, "audio2_2x", NULL);
+	clks[audio2_2x] = clk;
+
+	/* audio3_2x */
+	clk = clk_register_fixed_factor(NULL, "audio3_doubler", "audio3",
+					CLK_SET_RATE_PARENT, 2, 1);
+	clk = tegra_clk_register_divider("audio3_div", "audio3_doubler",
+				clk_base + AUDIO_SYNC_DOUBLER, 0, 0, 27, 1,
+				0, &clk_doubler_lock);
+	clk = tegra_clk_register_periph_gate("audio3_2x", "audio3_div",
+				  TEGRA_PERIPH_NO_RESET, clk_base,
+				  CLK_SET_RATE_PARENT, 116, &periph_v_regs,
+				  periph_clk_enb_refcnt);
+	clk_register_clkdev(clk, "audio3_2x", NULL);
+	clks[audio3_2x] = clk;
+
+	/* audio4_2x */
+	clk = clk_register_fixed_factor(NULL, "audio4_doubler", "audio4",
+					CLK_SET_RATE_PARENT, 2, 1);
+	clk = tegra_clk_register_divider("audio4_div", "audio4_doubler",
+				clk_base + AUDIO_SYNC_DOUBLER, 0, 0, 28, 1,
+				0, &clk_doubler_lock);
+	clk = tegra_clk_register_periph_gate("audio4_2x", "audio4_div",
+				  TEGRA_PERIPH_NO_RESET, clk_base,
+				  CLK_SET_RATE_PARENT, 117, &periph_v_regs,
+				  periph_clk_enb_refcnt);
+	clk_register_clkdev(clk, "audio4_2x", NULL);
+	clks[audio4_2x] = clk;
+
+	/* spdif_2x */
+	clk = clk_register_fixed_factor(NULL, "spdif_doubler", "spdif",
+					CLK_SET_RATE_PARENT, 2, 1);
+	clk = tegra_clk_register_divider("spdif_div", "spdif_doubler",
+				clk_base + AUDIO_SYNC_DOUBLER, 0, 0, 29, 1,
+				0, &clk_doubler_lock);
+	clk = tegra_clk_register_periph_gate("spdif_2x", "spdif_div",
+				  TEGRA_PERIPH_NO_RESET, clk_base,
+				  CLK_SET_RATE_PARENT, 118,
+				  &periph_v_regs, periph_clk_enb_refcnt);
+	clk_register_clkdev(clk, "spdif_2x", NULL);
+	clks[spdif_2x] = clk;
+}
+
+static void __init tegra114_pmc_clk_init(void __iomem *pmc_base)
+{
+	struct clk *clk;
+
+	/* clk_out_1 */
+	clk = clk_register_mux(NULL, "clk_out_1_mux", clk_out1_parents,
+			       ARRAY_SIZE(clk_out1_parents), 0,
+			       pmc_base + PMC_CLK_OUT_CNTRL, 6, 3, 0,
+			       &clk_out_lock);
+	clks[clk_out_1_mux] = clk;
+	clk = clk_register_gate(NULL, "clk_out_1", "clk_out_1_mux", 0,
+				pmc_base + PMC_CLK_OUT_CNTRL, 2, 0,
+				&clk_out_lock);
+	clk_register_clkdev(clk, "extern1", "clk_out_1");
+	clks[clk_out_1] = clk;
+
+	/* clk_out_2 */
+	clk = clk_register_mux(NULL, "clk_out_2_mux", clk_out2_parents,
+			       ARRAY_SIZE(clk_out1_parents), 0,
+			       pmc_base + PMC_CLK_OUT_CNTRL, 14, 3, 0,
+			       &clk_out_lock);
+	clks[clk_out_2_mux] = clk;
+	clk = clk_register_gate(NULL, "clk_out_2", "clk_out_2_mux", 0,
+				pmc_base + PMC_CLK_OUT_CNTRL, 10, 0,
+				&clk_out_lock);
+	clk_register_clkdev(clk, "extern2", "clk_out_2");
+	clks[clk_out_2] = clk;
+
+	/* clk_out_3 */
+	clk = clk_register_mux(NULL, "clk_out_3_mux", clk_out3_parents,
+			       ARRAY_SIZE(clk_out1_parents), 0,
+			       pmc_base + PMC_CLK_OUT_CNTRL, 22, 3, 0,
+			       &clk_out_lock);
+	clks[clk_out_3_mux] = clk;
+	clk = clk_register_gate(NULL, "clk_out_3", "clk_out_3_mux", 0,
+				pmc_base + PMC_CLK_OUT_CNTRL, 18, 0,
+				&clk_out_lock);
+	clk_register_clkdev(clk, "extern3", "clk_out_3");
+	clks[clk_out_3] = clk;
+
+	/* blink */
+	clk = clk_register_gate(NULL, "blink_override", "clk_32k", 0,
+				pmc_base + PMC_DPD_PADS_ORIDE,
+				PMC_DPD_PADS_ORIDE_BLINK_ENB, 0, NULL);
+	clk = clk_register_gate(NULL, "blink", "blink_override", 0,
+				pmc_base + PMC_CTRL,
+				PMC_CTRL_BLINK_ENB, 0, NULL);
+	clk_register_clkdev(clk, "blink", NULL);
+	clks[blink] = clk;
+
+}
+
+static const char *sclk_parents[] = { "clk_m", "pll_c_out1", "pll_p_out4",
+			       "pll_p_out3", "pll_p_out2", "unused",
+			       "clk_32k", "pll_m_out1" };
+
+static const char *cclk_g_parents[] = { "clk_m", "pll_c", "clk_32k", "pll_m",
+					"pll_p", "pll_p_out4", "unused",
+					"unused", "pll_x" };
+
+static const char *cclk_lp_parents[] = { "clk_m", "pll_c", "clk_32k", "pll_m",
+					 "pll_p", "pll_p_out4", "unused",
+					 "unused", "pll_x", "pll_x_out0" };
+
+static void __init tegra114_super_clk_init(void __iomem *clk_base)
+{
+	struct clk *clk;
+
+	/* CCLKG */
+	clk = tegra_clk_register_super_mux("cclk_g", cclk_g_parents,
+					ARRAY_SIZE(cclk_g_parents),
+					CLK_SET_RATE_PARENT,
+					clk_base + CCLKG_BURST_POLICY,
+					0, 4, 0, 0, NULL);
+	clk_register_clkdev(clk, "cclk_g", NULL);
+	clks[cclk_g] = clk;
+
+	/* CCLKLP */
+	clk = tegra_clk_register_super_mux("cclk_lp", cclk_lp_parents,
+					ARRAY_SIZE(cclk_lp_parents),
+					CLK_SET_RATE_PARENT,
+					clk_base + CCLKLP_BURST_POLICY,
+					0, 4, 8, 9, NULL);
+	clk_register_clkdev(clk, "cclk_lp", NULL);
+	clks[cclk_lp] = clk;
+
+	/* SCLK */
+	clk = tegra_clk_register_super_mux("sclk", sclk_parents,
+					ARRAY_SIZE(sclk_parents),
+					CLK_SET_RATE_PARENT,
+					clk_base + SCLK_BURST_POLICY,
+					0, 4, 0, 0, NULL);
+	clk_register_clkdev(clk, "sclk", NULL);
+	clks[sclk] = clk;
+
+	/* HCLK */
+	clk = clk_register_divider(NULL, "hclk_div", "sclk", 0,
+				   clk_base + SYSTEM_CLK_RATE, 4, 2, 0,
+				   &sysrate_lock);
+	clk = clk_register_gate(NULL, "hclk", "hclk_div", CLK_SET_RATE_PARENT |
+				CLK_IGNORE_UNUSED, clk_base + SYSTEM_CLK_RATE,
+				7, CLK_GATE_SET_TO_DISABLE, &sysrate_lock);
+	clk_register_clkdev(clk, "hclk", NULL);
+	clks[hclk] = clk;
+
+	/* PCLK */
+	clk = clk_register_divider(NULL, "pclk_div", "hclk", 0,
+				   clk_base + SYSTEM_CLK_RATE, 0, 2, 0,
+				   &sysrate_lock);
+	clk = clk_register_gate(NULL, "pclk", "pclk_div", CLK_SET_RATE_PARENT |
+				CLK_IGNORE_UNUSED, clk_base + SYSTEM_CLK_RATE,
+				3, CLK_GATE_SET_TO_DISABLE, &sysrate_lock);
+	clk_register_clkdev(clk, "pclk", NULL);
+	clks[pclk] = clk;
+}
+
+static struct tegra_periph_init_data tegra_periph_clk_list[] = {
+	TEGRA_INIT_DATA_MUX("i2s0", NULL, "tegra30-i2s.0", mux_pllaout0_audio0_2x_pllp_clkm, CLK_SOURCE_I2S0, 30, &periph_l_regs, TEGRA_PERIPH_ON_APB, i2s0),
+	TEGRA_INIT_DATA_MUX("i2s1", NULL, "tegra30-i2s.1", mux_pllaout0_audio1_2x_pllp_clkm, CLK_SOURCE_I2S1, 11, &periph_l_regs, TEGRA_PERIPH_ON_APB, i2s1),
+	TEGRA_INIT_DATA_MUX("i2s2", NULL, "tegra30-i2s.2", mux_pllaout0_audio2_2x_pllp_clkm, CLK_SOURCE_I2S2, 18, &periph_l_regs, TEGRA_PERIPH_ON_APB, i2s2),
+	TEGRA_INIT_DATA_MUX("i2s3", NULL, "tegra30-i2s.3", mux_pllaout0_audio3_2x_pllp_clkm, CLK_SOURCE_I2S3, 101, &periph_v_regs, TEGRA_PERIPH_ON_APB, i2s3),
+	TEGRA_INIT_DATA_MUX("i2s4", NULL, "tegra30-i2s.4", mux_pllaout0_audio4_2x_pllp_clkm, CLK_SOURCE_I2S4, 102, &periph_v_regs, TEGRA_PERIPH_ON_APB, i2s4),
+	TEGRA_INIT_DATA_MUX("spdif_out", "spdif_out", "tegra30-spdif", mux_pllaout0_audio_2x_pllp_clkm, CLK_SOURCE_SPDIF_OUT, 10, &periph_l_regs, TEGRA_PERIPH_ON_APB, spdif_out),
+	TEGRA_INIT_DATA_MUX("spdif_in", "spdif_in", "tegra30-spdif", mux_pllp_pllc_pllm, CLK_SOURCE_SPDIF_IN, 10, &periph_l_regs, TEGRA_PERIPH_ON_APB, spdif_in),
+	TEGRA_INIT_DATA_MUX("pwm", NULL, "pwm", mux_pllp_pllc_clk32_clkm, CLK_SOURCE_PWM, 17, &periph_l_regs, TEGRA_PERIPH_ON_APB, pwm),
+	TEGRA_INIT_DATA_MUX("adx", NULL, "adx", mux_plla_pllc_pllp_clkm, CLK_SOURCE_ADX, 154, &periph_w_regs, TEGRA_PERIPH_ON_APB, adx),
+	TEGRA_INIT_DATA_MUX("amx", NULL, "amx", mux_plla_pllc_pllp_clkm, CLK_SOURCE_AMX, 153, &periph_w_regs, TEGRA_PERIPH_ON_APB, amx),
+	TEGRA_INIT_DATA_MUX("hda", "hda", "tegra30-hda", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_HDA, 125, &periph_v_regs, TEGRA_PERIPH_ON_APB, hda),
+	TEGRA_INIT_DATA_MUX("hda2codec_2x", "hda2codec", "tegra30-hda", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_HDA2CODEC_2X, 111, &periph_v_regs, TEGRA_PERIPH_ON_APB, hda2codec_2x),
+	TEGRA_INIT_DATA_MUX("sbc1", NULL, "tegra11-spi.0", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SBC1, 41, &periph_h_regs, TEGRA_PERIPH_ON_APB, sbc1),
+	TEGRA_INIT_DATA_MUX("sbc2", NULL, "tegra11-spi.1", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SBC2, 44, &periph_h_regs, TEGRA_PERIPH_ON_APB, sbc2),
+	TEGRA_INIT_DATA_MUX("sbc3", NULL, "tegra11-spi.2", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SBC3, 46, &periph_h_regs, TEGRA_PERIPH_ON_APB, sbc3),
+	TEGRA_INIT_DATA_MUX("sbc4", NULL, "tegra11-spi.3", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SBC4, 68, &periph_u_regs, TEGRA_PERIPH_ON_APB, sbc4),
+	TEGRA_INIT_DATA_MUX("sbc5", NULL, "tegra11-spi.4", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SBC5, 104, &periph_v_regs, TEGRA_PERIPH_ON_APB, sbc5),
+	TEGRA_INIT_DATA_MUX("sbc6", NULL, "tegra11-spi.5", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SBC6, 105, &periph_v_regs, TEGRA_PERIPH_ON_APB, sbc6),
+	TEGRA_INIT_DATA_MUX8("ndflash", NULL, "tegra_nand", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_NDFLASH, 13, &periph_u_regs, TEGRA_PERIPH_ON_APB, ndspeed),
+	TEGRA_INIT_DATA_MUX8("ndspeed", NULL, "tegra_nand_speed", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_NDSPEED, 80, &periph_u_regs, TEGRA_PERIPH_ON_APB, ndspeed),
+	TEGRA_INIT_DATA_MUX("vfir", NULL, "vfir", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_VFIR, 7, &periph_l_regs, TEGRA_PERIPH_ON_APB, vfir),
+	TEGRA_INIT_DATA_MUX("sdmmc1", NULL, "sdhci-tegra.0", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SDMMC1, 14, &periph_l_regs, 0, sdmmc1),
+	TEGRA_INIT_DATA_MUX("sdmmc2", NULL, "sdhci-tegra.1", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SDMMC2, 9, &periph_l_regs, 0, sdmmc2),
+	TEGRA_INIT_DATA_MUX("sdmmc3", NULL, "sdhci-tegra.2", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SDMMC3, 69, &periph_u_regs, 0, sdmmc3),
+	TEGRA_INIT_DATA_MUX("sdmmc4", NULL, "sdhci-tegra.3", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SDMMC4, 15, &periph_l_regs, 0, sdmmc4),
+	TEGRA_INIT_DATA_INT("vde", NULL, "vde", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_VDE, 61, &periph_h_regs, 0, vde),
+	TEGRA_INIT_DATA_MUX_FLAGS("csite", NULL, "csite", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_CSITE, 73, &periph_u_regs, TEGRA_PERIPH_ON_APB, csite, CLK_IGNORE_UNUSED),
+	TEGRA_INIT_DATA_MUX("la", NULL, "la", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_LA, 76, &periph_u_regs, TEGRA_PERIPH_ON_APB, la),
+	TEGRA_INIT_DATA_MUX("trace", NULL, "trace", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_TRACE, 77, &periph_u_regs, TEGRA_PERIPH_ON_APB, trace),
+	TEGRA_INIT_DATA_MUX("owr", NULL, "tegra_w1", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_OWR, 71, &periph_u_regs, TEGRA_PERIPH_ON_APB, owr),
+	TEGRA_INIT_DATA_MUX("nor", NULL, "tegra-nor", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_NOR, 42, &periph_h_regs, 0, nor),
+	TEGRA_INIT_DATA_MUX("mipi", NULL, "mipi", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_MIPI, 50, &periph_h_regs, TEGRA_PERIPH_ON_APB, mipi),
+	TEGRA_INIT_DATA_I2C("i2c1", "div-clk", "tegra11-i2c.0", mux_pllp_clkm, CLK_SOURCE_I2C1, 12, &periph_l_regs, i2c1),
+	TEGRA_INIT_DATA_I2C("i2c2", "div-clk", "tegra11-i2c.1", mux_pllp_clkm, CLK_SOURCE_I2C2, 54, &periph_h_regs, i2c2),
+	TEGRA_INIT_DATA_I2C("i2c3", "div-clk", "tegra11-i2c.2", mux_pllp_clkm, CLK_SOURCE_I2C3, 67, &periph_u_regs, i2c3),
+	TEGRA_INIT_DATA_I2C("i2c4", "div-clk", "tegra11-i2c.3", mux_pllp_clkm, CLK_SOURCE_I2C4, 103, &periph_v_regs, i2c4),
+	TEGRA_INIT_DATA_I2C("i2c5", "div-clk", "tegra11-i2c.4", mux_pllp_clkm, CLK_SOURCE_I2C5, 47, &periph_h_regs, i2c5),
+	TEGRA_INIT_DATA_UART("uarta", NULL, "tegra_uart.0", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_UARTA, 6, &periph_l_regs, uarta),
+	TEGRA_INIT_DATA_UART("uartb", NULL, "tegra_uart.1", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_UARTB, 7, &periph_l_regs, uartb),
+	TEGRA_INIT_DATA_UART("uartc", NULL, "tegra_uart.2", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_UARTC, 55, &periph_h_regs, uartc),
+	TEGRA_INIT_DATA_UART("uartd", NULL, "tegra_uart.3", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_UARTD, 65, &periph_u_regs, uartd),
+	TEGRA_INIT_DATA_INT("3d", NULL, "3d", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_3D, 24, &periph_l_regs, 0, gr_3d),
+	TEGRA_INIT_DATA_INT("2d", NULL, "2d", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_2D, 21, &periph_l_regs, 0, gr_2d),
+	TEGRA_INIT_DATA_MUX("vi_sensor", "vi_sensor", "tegra_camera", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_VI_SENSOR, 20, &periph_l_regs, TEGRA_PERIPH_NO_RESET, vi_sensor),
+	TEGRA_INIT_DATA_INT8("vi", "vi", "tegra_camera", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_VI, 20, &periph_l_regs, 0, vi),
+	TEGRA_INIT_DATA_INT8("epp", NULL, "epp", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_EPP, 19, &periph_l_regs, 0, epp),
+	TEGRA_INIT_DATA_INT8("msenc", NULL, "msenc", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_MSENC, 91, &periph_h_regs, TEGRA_PERIPH_WAR_1005168, msenc),
+	TEGRA_INIT_DATA_INT8("tsec", NULL, "tsec", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_TSEC, 83, &periph_u_regs, 0, tsec),
+	TEGRA_INIT_DATA_INT8("host1x", NULL, "host1x", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_HOST1X, 28, &periph_l_regs, 0, host1x),
+	TEGRA_INIT_DATA_MUX8("hdmi", NULL, "hdmi", mux_pllp_pllm_plld_plla_pllc_plld2_clkm, CLK_SOURCE_HDMI, 51, &periph_h_regs, 0, hdmi),
+	TEGRA_INIT_DATA_MUX("cilab", "cilab", "tegra_camera", mux_pllp_pllc_clkm, CLK_SOURCE_CILAB, 144, &periph_w_regs, 0, cilab),
+	TEGRA_INIT_DATA_MUX("cilcd", "cilcd", "tegra_camera", mux_pllp_pllc_clkm, CLK_SOURCE_CILCD, 145, &periph_w_regs, 0, cilcd),
+	TEGRA_INIT_DATA_MUX("cile", "cile", "tegra_camera", mux_pllp_pllc_clkm, CLK_SOURCE_CILE, 146, &periph_w_regs, 0, cile),
+	TEGRA_INIT_DATA_MUX("dsialp", "dsialp", "tegradc.0", mux_pllp_pllc_clkm, CLK_SOURCE_DSIALP, 147, &periph_w_regs, 0, dsialp),
+	TEGRA_INIT_DATA_MUX("dsiblp", "dsiblp", "tegradc.1", mux_pllp_pllc_clkm, CLK_SOURCE_DSIBLP, 148, &periph_w_regs, 0, dsiblp),
+	TEGRA_INIT_DATA_MUX("tsensor", NULL, "tegra-tsensor", mux_pllp_pllc_clkm_clk32, CLK_SOURCE_TSENSOR, 100, &periph_v_regs, TEGRA_PERIPH_ON_APB, tsensor),
+	TEGRA_INIT_DATA_MUX("actmon", NULL, "actmon", mux_pllp_pllc_clk32_clkm, CLK_SOURCE_ACTMON, 119, &periph_v_regs, 0, actmon),
+	TEGRA_INIT_DATA_MUX8("extern1", NULL, "extern1", mux_plla_clk32_pllp_clkm_plle, CLK_SOURCE_EXTERN1, 120, &periph_v_regs, 0, extern1),
+	TEGRA_INIT_DATA_MUX8("extern2", NULL, "extern2", mux_plla_clk32_pllp_clkm_plle, CLK_SOURCE_EXTERN2, 121, &periph_v_regs, 0, extern2),
+	TEGRA_INIT_DATA_MUX8("extern3", NULL, "extern3", mux_plla_clk32_pllp_clkm_plle, CLK_SOURCE_EXTERN3, 122, &periph_v_regs, 0, extern3),
+	TEGRA_INIT_DATA_MUX("i2cslow", NULL, "i2cslow", mux_pllp_pllc_clk32_clkm, CLK_SOURCE_I2CSLOW, 81, &periph_u_regs, TEGRA_PERIPH_ON_APB, i2cslow),
+	TEGRA_INIT_DATA_INT8("se", NULL, "se", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_SE, 127, &periph_v_regs, TEGRA_PERIPH_ON_APB, se),
+	TEGRA_INIT_DATA_INT_FLAGS("mselect", NULL, "mselect", mux_pllp_clkm, CLK_SOURCE_MSELECT, 99, &periph_v_regs, 0, mselect, CLK_IGNORE_UNUSED),
+	TEGRA_INIT_DATA_MUX8("soc_therm", NULL, "soc_therm", mux_pllm_pllc_pllp_plla, CLK_SOURCE_SOC_THERM, 78, &periph_u_regs, TEGRA_PERIPH_ON_APB, soc_therm),
+	TEGRA_INIT_DATA_XUSB("xusb_host_src", "host_src", "tegra_xhci", mux_clkm_pllp_pllc_pllre, CLK_SOURCE_XUSB_HOST_SRC, 143, &periph_w_regs, TEGRA_PERIPH_ON_APB | TEGRA_PERIPH_NO_RESET, xusb_host_src),
+	TEGRA_INIT_DATA_XUSB("xusb_falcon_src", "falcon_src", "tegra_xhci", mux_clkm_pllp_pllc_pllre, CLK_SOURCE_XUSB_FALCON_SRC, 143, &periph_w_regs, TEGRA_PERIPH_NO_RESET, xusb_falcon_src),
+	TEGRA_INIT_DATA_XUSB("xusb_fs_src", "fs_src", "tegra_xhci", mux_clkm_48M_pllp_480M, CLK_SOURCE_XUSB_FS_SRC, 143, &periph_w_regs, TEGRA_PERIPH_NO_RESET, xusb_fs_src),
+	TEGRA_INIT_DATA_XUSB("xusb_ss_src", "ss_src", "tegra_xhci", mux_clkm_pllre_clk32_480M_pllc_ref, CLK_SOURCE_XUSB_SS_SRC, 143, &periph_w_regs, TEGRA_PERIPH_NO_RESET, xusb_ss_src),
+	TEGRA_INIT_DATA_XUSB("xusb_dev_src", "dev_src", "tegra_xhci", mux_clkm_pllp_pllc_pllre, CLK_SOURCE_XUSB_DEV_SRC, 95, &periph_u_regs, TEGRA_PERIPH_ON_APB | TEGRA_PERIPH_NO_RESET, xusb_dev_src),
+	TEGRA_INIT_DATA_AUDIO("d_audio", "d_audio", "tegra30-ahub", CLK_SOURCE_D_AUDIO, 106, &periph_v_regs, TEGRA_PERIPH_ON_APB, d_audio),
+	TEGRA_INIT_DATA_AUDIO("dam0", NULL, "tegra30-dam.0", CLK_SOURCE_DAM0, 108, &periph_v_regs, TEGRA_PERIPH_ON_APB, dam0),
+	TEGRA_INIT_DATA_AUDIO("dam1", NULL, "tegra30-dam.1", CLK_SOURCE_DAM1, 109, &periph_v_regs, TEGRA_PERIPH_ON_APB, dam1),
+	TEGRA_INIT_DATA_AUDIO("dam2", NULL, "tegra30-dam.2", CLK_SOURCE_DAM2, 110, &periph_v_regs, TEGRA_PERIPH_ON_APB, dam2),
+};
+
+static struct tegra_periph_init_data tegra_periph_nodiv_clk_list[] = {
+	TEGRA_INIT_DATA_NODIV("disp1", NULL, "tegradc.0", mux_pllp_pllm_plld_plla_pllc_plld2_clkm, CLK_SOURCE_DISP1, 29, 7, 27, &periph_l_regs, 0, disp1),
+	TEGRA_INIT_DATA_NODIV("disp2", NULL, "tegradc.1", mux_pllp_pllm_plld_plla_pllc_plld2_clkm, CLK_SOURCE_DISP2, 29, 7, 26, &periph_l_regs, 0, disp2),
+};
+
+static __init void tegra114_periph_clk_init(void __iomem *clk_base)
+{
+	struct tegra_periph_init_data *data;
+	struct clk *clk;
+	int i;
+	u32 val;
+
+	/* apbdma */
+	clk = tegra_clk_register_periph_gate("apbdma", "clk_m", 0, clk_base,
+				  0, 34, &periph_h_regs,
+				  periph_clk_enb_refcnt);
+	clks[apbdma] = clk;
+
+	/* rtc */
+	clk = tegra_clk_register_periph_gate("rtc", "clk_32k",
+				    TEGRA_PERIPH_ON_APB |
+				    TEGRA_PERIPH_NO_RESET, clk_base,
+				    0, 4, &periph_l_regs,
+				    periph_clk_enb_refcnt);
+	clk_register_clkdev(clk, NULL, "rtc-tegra");
+	clks[rtc] = clk;
+
+	/* kbc */
+	clk = tegra_clk_register_periph_gate("kbc", "clk_32k",
+				    TEGRA_PERIPH_ON_APB |
+				    TEGRA_PERIPH_NO_RESET, clk_base,
+				    0, 36, &periph_h_regs,
+				    periph_clk_enb_refcnt);
+	clks[kbc] = clk;
+
+	/* timer */
+	clk = tegra_clk_register_periph_gate("timer", "clk_m", 0, clk_base,
+				  0, 5, &periph_l_regs,
+				  periph_clk_enb_refcnt);
+	clk_register_clkdev(clk, NULL, "timer");
+	clks[timer] = clk;
+
+	/* kfuse */
+	clk = tegra_clk_register_periph_gate("kfuse", "clk_m",
+				  TEGRA_PERIPH_ON_APB, clk_base,  0, 40,
+				  &periph_h_regs, periph_clk_enb_refcnt);
+	clks[kfuse] = clk;
+
+	/* fuse */
+	clk = tegra_clk_register_periph_gate("fuse", "clk_m",
+				  TEGRA_PERIPH_ON_APB, clk_base,  0, 39,
+				  &periph_h_regs, periph_clk_enb_refcnt);
+	clks[fuse] = clk;
+
+	/* fuse_burn */
+	clk = tegra_clk_register_periph_gate("fuse_burn", "clk_m",
+				  TEGRA_PERIPH_ON_APB, clk_base,  0, 39,
+				  &periph_h_regs, periph_clk_enb_refcnt);
+	clks[fuse_burn] = clk;
+
+	/* apbif */
+	clk = tegra_clk_register_periph_gate("apbif", "clk_m",
+				  TEGRA_PERIPH_ON_APB, clk_base,  0, 107,
+				  &periph_v_regs, periph_clk_enb_refcnt);
+	clks[apbif] = clk;
+
+	/* hda2hdmi */
+	clk = tegra_clk_register_periph_gate("hda2hdmi", "clk_m",
+				    TEGRA_PERIPH_ON_APB, clk_base,  0, 128,
+				    &periph_w_regs, periph_clk_enb_refcnt);
+	clks[hda2hdmi] = clk;
+
+	/* vcp */
+	clk = tegra_clk_register_periph_gate("vcp", "clk_m", 0, clk_base,  0,
+				  29, &periph_l_regs,
+				  periph_clk_enb_refcnt);
+	clks[vcp] = clk;
+
+	/* bsea */
+	clk = tegra_clk_register_periph_gate("bsea", "clk_m", 0, clk_base,
+				  0, 62, &periph_h_regs,
+				  periph_clk_enb_refcnt);
+	clks[bsea] = clk;
+
+	/* bsev */
+	clk = tegra_clk_register_periph_gate("bsev", "clk_m", 0, clk_base,
+				  0, 63, &periph_h_regs,
+				  periph_clk_enb_refcnt);
+	clks[bsev] = clk;
+
+	/* mipi-cal */
+	clk = tegra_clk_register_periph_gate("mipi-cal", "clk_m", 0, clk_base,
+				   0, 56, &periph_h_regs,
+				  periph_clk_enb_refcnt);
+	clks[mipi_cal] = clk;
+
+	/* usbd */
+	clk = tegra_clk_register_periph_gate("usbd", "clk_m", 0, clk_base,
+				  0, 22, &periph_l_regs,
+				  periph_clk_enb_refcnt);
+	clks[usbd] = clk;
+
+	/* usb2 */
+	clk = tegra_clk_register_periph_gate("usb2", "clk_m", 0, clk_base,
+				  0, 58, &periph_h_regs,
+				  periph_clk_enb_refcnt);
+	clks[usb2] = clk;
+
+	/* usb3 */
+	clk = tegra_clk_register_periph_gate("usb3", "clk_m", 0, clk_base,
+				  0, 59, &periph_h_regs,
+				  periph_clk_enb_refcnt);
+	clks[usb3] = clk;
+
+	/* csi */
+	clk = tegra_clk_register_periph_gate("csi", "pll_p_out3", 0, clk_base,
+				   0, 52, &periph_h_regs,
+				  periph_clk_enb_refcnt);
+	clks[csi] = clk;
+
+	/* isp */
+	clk = tegra_clk_register_periph_gate("isp", "clk_m", 0, clk_base, 0,
+				  23, &periph_l_regs,
+				  periph_clk_enb_refcnt);
+	clks[isp] = clk;
+
+	/* csus */
+	clk = tegra_clk_register_periph_gate("csus", "clk_m",
+				  TEGRA_PERIPH_NO_RESET, clk_base, 0, 92,
+				  &periph_u_regs, periph_clk_enb_refcnt);
+	clks[csus] = clk;
+
+	/* dds */
+	clk = tegra_clk_register_periph_gate("dds", "clk_m",
+				  TEGRA_PERIPH_ON_APB, clk_base, 0, 150,
+				  &periph_w_regs, periph_clk_enb_refcnt);
+	clks[dds] = clk;
+
+	/* dp2 */
+	clk = tegra_clk_register_periph_gate("dp2", "clk_m",
+				  TEGRA_PERIPH_ON_APB, clk_base, 0, 152,
+				  &periph_w_regs, periph_clk_enb_refcnt);
+	clks[dp2] = clk;
+
+	/* dtv */
+	clk = tegra_clk_register_periph_gate("dtv", "clk_m",
+				    TEGRA_PERIPH_ON_APB, clk_base, 0, 79,
+				    &periph_u_regs, periph_clk_enb_refcnt);
+	clks[dtv] = clk;
+
+	/* dsia */
+	clk = clk_register_mux(NULL, "dsia_mux", mux_plld_out0_plld2_out0,
+			       ARRAY_SIZE(mux_plld_out0_plld2_out0), 0,
+			       clk_base + PLLD_BASE, 25, 1, 0, &pll_d_lock);
+	clks[dsia_mux] = clk;
+	clk = tegra_clk_register_periph_gate("dsia", "dsia_mux", 0, clk_base,
+				    0, 48, &periph_h_regs,
+				    periph_clk_enb_refcnt);
+	clks[dsia] = clk;
+
+	/* dsib */
+	clk = clk_register_mux(NULL, "dsib_mux", mux_plld_out0_plld2_out0,
+			       ARRAY_SIZE(mux_plld_out0_plld2_out0), 0,
+			       clk_base + PLLD2_BASE, 25, 1, 0, &pll_d2_lock);
+	clks[dsib_mux] = clk;
+	clk = tegra_clk_register_periph_gate("dsib", "dsib_mux", 0, clk_base,
+				    0, 82, &periph_u_regs,
+				    periph_clk_enb_refcnt);
+	clks[dsib] = clk;
+
+	/* xusb_hs_src */
+	val = readl(clk_base + CLK_SOURCE_XUSB_SS_SRC);
+	val |= BIT(25); /* always select PLLU_60M */
+	writel(val, clk_base + CLK_SOURCE_XUSB_SS_SRC);
+
+	clk = clk_register_fixed_factor(NULL, "xusb_hs_src", "pll_u_60M", 0,
+					1, 1);
+	clks[xusb_hs_src] = clk;
+
+	/* xusb_host */
+	clk = tegra_clk_register_periph_gate("xusb_host", "xusb_host_src", 0,
+				    clk_base, 0, 89, &periph_u_regs,
+				    periph_clk_enb_refcnt);
+	clks[xusb_host] = clk;
+
+	/* xusb_ss */
+	clk = tegra_clk_register_periph_gate("xusb_ss", "xusb_ss_src", 0,
+				    clk_base, 0, 156, &periph_w_regs,
+				    periph_clk_enb_refcnt);
+	clks[xusb_host] = clk;
+
+	/* xusb_dev */
+	clk = tegra_clk_register_periph_gate("xusb_dev", "xusb_dev_src", 0,
+				    clk_base, 0, 95, &periph_u_regs,
+				    periph_clk_enb_refcnt);
+	clks[xusb_dev] = clk;
+
+	/* emc */
+	clk = clk_register_mux(NULL, "emc_mux", mux_pllmcp_clkm,
+			       ARRAY_SIZE(mux_pllmcp_clkm), 0,
+			       clk_base + CLK_SOURCE_EMC,
+			       29, 3, 0, NULL);
+	clk = tegra_clk_register_periph_gate("emc", "emc_mux", 0, clk_base,
+				CLK_IGNORE_UNUSED, 57, &periph_h_regs,
+				periph_clk_enb_refcnt);
+	clks[emc] = clk;
+
+	for (i = 0; i < ARRAY_SIZE(tegra_periph_clk_list); i++) {
+		data = &tegra_periph_clk_list[i];
+		clk = tegra_clk_register_periph(data->name, data->parent_names,
+				data->num_parents, &data->periph,
+				clk_base, data->offset, data->flags);
+		clks[data->clk_id] = clk;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(tegra_periph_nodiv_clk_list); i++) {
+		data = &tegra_periph_nodiv_clk_list[i];
+		clk = tegra_clk_register_periph_nodiv(data->name,
+				data->parent_names, data->num_parents,
+				&data->periph, clk_base, data->offset);
+		clks[data->clk_id] = clk;
+	}
+}
+
+static struct tegra_cpu_car_ops tegra114_cpu_car_ops;
+
+static const struct of_device_id pmc_match[] __initconst = {
+	{ .compatible = "nvidia,tegra114-pmc" },
+	{},
+};
+
+static __initdata struct tegra_clk_init_table init_table[] = {
+	{uarta, pll_p, 408000000, 0},
+	{uartb, pll_p, 408000000, 0},
+	{uartc, pll_p, 408000000, 0},
+	{uartd, pll_p, 408000000, 0},
+	{pll_a, clk_max, 564480000, 1},
+	{pll_a_out0, clk_max, 11289600, 1},
+	{extern1, pll_a_out0, 0, 1},
+	{clk_out_1_mux, extern1, 0, 1},
+	{clk_out_1, clk_max, 0, 1},
+	{i2s0, pll_a_out0, 11289600, 0},
+	{i2s1, pll_a_out0, 11289600, 0},
+	{i2s2, pll_a_out0, 11289600, 0},
+	{i2s3, pll_a_out0, 11289600, 0},
+	{i2s4, pll_a_out0, 11289600, 0},
+	{clk_max, clk_max, 0, 0}, /* This MUST be the last entry. */
+};
+
+static void __init tegra114_clock_apply_init_table(void)
+{
+	tegra_init_from_table(init_table, clks, clk_max);
+}
+
+void __init tegra114_clock_init(struct device_node *np)
+{
+	struct device_node *node;
+	int i;
+
+	clk_base = of_iomap(np, 0);
+	if (!clk_base) {
+		pr_err("ioremap tegra114 CAR failed\n");
+		return;
+	}
+
+	node = of_find_matching_node(NULL, pmc_match);
+	if (!node) {
+		pr_err("Failed to find pmc node\n");
+		WARN_ON(1);
+		return;
+	}
+
+	pmc_base = of_iomap(node, 0);
+	if (!pmc_base) {
+		pr_err("Can't map pmc registers\n");
+		WARN_ON(1);
+		return;
+	}
+
+	if (tegra114_osc_clk_init(clk_base) < 0)
+		return;
+
+	tegra114_fixed_clk_init(clk_base);
+	tegra114_pll_init(clk_base, pmc_base);
+	tegra114_periph_clk_init(clk_base);
+	tegra114_audio_clk_init(clk_base);
+	tegra114_pmc_clk_init(pmc_base);
+	tegra114_super_clk_init(clk_base);
+
+	for (i = 0; i < ARRAY_SIZE(clks); i++) {
+		if (IS_ERR(clks[i])) {
+			pr_err
+			    ("Tegra114 clk %d: register failed with %ld\n",
+			     i, PTR_ERR(clks[i]));
+		}
+		if (!clks[i])
+			clks[i] = ERR_PTR(-EINVAL);
+	}
+
+	clk_data.clks = clks;
+	clk_data.clk_num = ARRAY_SIZE(clks);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+
+	tegra_clk_apply_init_table = tegra114_clock_apply_init_table;
+
+	tegra_cpu_car_ops = &tegra114_cpu_car_ops;
+}
diff --git a/drivers/clk/tegra/clk-tegra20.c b/drivers/clk/tegra/clk-tegra20.c
index f873dcefe0de..8292a00c3de9 100644
--- a/drivers/clk/tegra/clk-tegra20.c
+++ b/drivers/clk/tegra/clk-tegra20.c
@@ -86,8 +86,8 @@
 #define PLLE_BASE 0xe8
 #define PLLE_MISC 0xec
 
-#define PLL_BASE_LOCK 27
-#define PLLE_MISC_LOCK 11
+#define PLL_BASE_LOCK BIT(27)
+#define PLLE_MISC_LOCK BIT(11)
 
 #define PLL_MISC_LOCK_ENABLE 18
 #define PLLDU_MISC_LOCK_ENABLE 22
@@ -236,7 +236,7 @@ enum tegra20_clk {
 	dvc, dsi, mipi = 50, hdmi, csi, tvdac, i2c2, uartc, emc = 57, usb2,
 	usb3, mpe, vde, bsea, bsev, speedo, uartd, uarte, i2c3, sbc4, sdmmc3,
 	pex, owr, afi, csite, pcie_xclk, avpucq = 75, la, irama = 84, iramb,
-	iramc, iramd, cram2, audio_2x, clk_d, csus = 92, cdev1, cdev2,
+	iramc, iramd, cram2, audio_2x, clk_d, csus = 92, cdev2, cdev1,
 	uartb = 96, vfir, spdif_in, spdif_out, vi, vi_sensor, tvo, cve,
 	osc, clk_32k, clk_m, sclk, cclk, hclk, pclk, blink, pll_a, pll_a_out0,
 	pll_c, pll_c_out1, pll_d, pll_d_out0, pll_e, pll_m, pll_m_out1,
@@ -248,125 +248,125 @@ static struct clk *clks[clk_max];
 static struct clk_onecell_data clk_data;
 
 static struct tegra_clk_pll_freq_table pll_c_freq_table[] = {
-	{ 12000000, 600000000, 600, 12, 1, 8 },
-	{ 13000000, 600000000, 600, 13, 1, 8 },
-	{ 19200000, 600000000, 500, 16, 1, 6 },
-	{ 26000000, 600000000, 600, 26, 1, 8 },
+	{ 12000000, 600000000, 600, 12, 0, 8 },
+	{ 13000000, 600000000, 600, 13, 0, 8 },
+	{ 19200000, 600000000, 500, 16, 0, 6 },
+	{ 26000000, 600000000, 600, 26, 0, 8 },
 	{ 0, 0, 0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_m_freq_table[] = {
-	{ 12000000, 666000000, 666, 12, 1, 8},
-	{ 13000000, 666000000, 666, 13, 1, 8},
-	{ 19200000, 666000000, 555, 16, 1, 8},
-	{ 26000000, 666000000, 666, 26, 1, 8},
-	{ 12000000, 600000000, 600, 12, 1, 8},
-	{ 13000000, 600000000, 600, 13, 1, 8},
-	{ 19200000, 600000000, 375, 12, 1, 6},
-	{ 26000000, 600000000, 600, 26, 1, 8},
+	{ 12000000, 666000000, 666, 12, 0, 8},
+	{ 13000000, 666000000, 666, 13, 0, 8},
+	{ 19200000, 666000000, 555, 16, 0, 8},
+	{ 26000000, 666000000, 666, 26, 0, 8},
+	{ 12000000, 600000000, 600, 12, 0, 8},
+	{ 13000000, 600000000, 600, 13, 0, 8},
+	{ 19200000, 600000000, 375, 12, 0, 6},
+	{ 26000000, 600000000, 600, 26, 0, 8},
 	{ 0, 0, 0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_p_freq_table[] = {
-	{ 12000000, 216000000, 432, 12, 2, 8},
-	{ 13000000, 216000000, 432, 13, 2, 8},
-	{ 19200000, 216000000, 90,   4, 2, 1},
-	{ 26000000, 216000000, 432, 26, 2, 8},
-	{ 12000000, 432000000, 432, 12, 1, 8},
-	{ 13000000, 432000000, 432, 13, 1, 8},
-	{ 19200000, 432000000, 90,   4, 1, 1},
-	{ 26000000, 432000000, 432, 26, 1, 8},
+	{ 12000000, 216000000, 432, 12, 1, 8},
+	{ 13000000, 216000000, 432, 13, 1, 8},
+	{ 19200000, 216000000, 90,   4, 1, 1},
+	{ 26000000, 216000000, 432, 26, 1, 8},
+	{ 12000000, 432000000, 432, 12, 0, 8},
+	{ 13000000, 432000000, 432, 13, 0, 8},
+	{ 19200000, 432000000, 90,   4, 0, 1},
+	{ 26000000, 432000000, 432, 26, 0, 8},
 	{ 0, 0, 0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_a_freq_table[] = {
-	{ 28800000, 56448000, 49, 25, 1, 1},
-	{ 28800000, 73728000, 64, 25, 1, 1},
-	{ 28800000, 24000000,  5,  6, 1, 1},
+	{ 28800000, 56448000, 49, 25, 0, 1},
+	{ 28800000, 73728000, 64, 25, 0, 1},
+	{ 28800000, 24000000,  5,  6, 0, 1},
 	{ 0, 0, 0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_d_freq_table[] = {
-	{ 12000000, 216000000, 216, 12, 1, 4},
-	{ 13000000, 216000000, 216, 13, 1, 4},
-	{ 19200000, 216000000, 135, 12, 1, 3},
-	{ 26000000, 216000000, 216, 26, 1, 4},
+	{ 12000000, 216000000, 216, 12, 0, 4},
+	{ 13000000, 216000000, 216, 13, 0, 4},
+	{ 19200000, 216000000, 135, 12, 0, 3},
+	{ 26000000, 216000000, 216, 26, 0, 4},
 
-	{ 12000000, 594000000, 594, 12, 1, 8},
-	{ 13000000, 594000000, 594, 13, 1, 8},
-	{ 19200000, 594000000, 495, 16, 1, 8},
-	{ 26000000, 594000000, 594, 26, 1, 8},
+	{ 12000000, 594000000, 594, 12, 0, 8},
+	{ 13000000, 594000000, 594, 13, 0, 8},
+	{ 19200000, 594000000, 495, 16, 0, 8},
+	{ 26000000, 594000000, 594, 26, 0, 8},
 
-	{ 12000000, 1000000000, 1000, 12, 1, 12},
-	{ 13000000, 1000000000, 1000, 13, 1, 12},
-	{ 19200000, 1000000000, 625,  12, 1, 8},
-	{ 26000000, 1000000000, 1000, 26, 1, 12},
+	{ 12000000, 1000000000, 1000, 12, 0, 12},
+	{ 13000000, 1000000000, 1000, 13, 0, 12},
+	{ 19200000, 1000000000, 625,  12, 0, 8},
+	{ 26000000, 1000000000, 1000, 26, 0, 12},
 
 	{ 0, 0, 0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_u_freq_table[] = {
-	{ 12000000, 480000000, 960, 12, 2, 0},
-	{ 13000000, 480000000, 960, 13, 2, 0},
-	{ 19200000, 480000000, 200, 4,  2, 0},
-	{ 26000000, 480000000, 960, 26, 2, 0},
+	{ 12000000, 480000000, 960, 12, 0, 0},
+	{ 13000000, 480000000, 960, 13, 0, 0},
+	{ 19200000, 480000000, 200, 4,  0, 0},
+	{ 26000000, 480000000, 960, 26, 0, 0},
 	{ 0, 0, 0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_x_freq_table[] = {
 	/* 1 GHz */
-	{ 12000000, 1000000000, 1000, 12, 1, 12},
-	{ 13000000, 1000000000, 1000, 13, 1, 12},
-	{ 19200000, 1000000000, 625,  12, 1, 8},
-	{ 26000000, 1000000000, 1000, 26, 1, 12},
+	{ 12000000, 1000000000, 1000, 12, 0, 12},
+	{ 13000000, 1000000000, 1000, 13, 0, 12},
+	{ 19200000, 1000000000, 625,  12, 0, 8},
+	{ 26000000, 1000000000, 1000, 26, 0, 12},
 
 	/* 912 MHz */
-	{ 12000000, 912000000,  912,  12, 1, 12},
-	{ 13000000, 912000000,  912,  13, 1, 12},
-	{ 19200000, 912000000,  760,  16, 1, 8},
-	{ 26000000, 912000000,  912,  26, 1, 12},
+	{ 12000000, 912000000,  912,  12, 0, 12},
+	{ 13000000, 912000000,  912,  13, 0, 12},
+	{ 19200000, 912000000,  760,  16, 0, 8},
+	{ 26000000, 912000000,  912,  26, 0, 12},
 
 	/* 816 MHz */
-	{ 12000000, 816000000,  816,  12, 1, 12},
-	{ 13000000, 816000000,  816,  13, 1, 12},
-	{ 19200000, 816000000,  680,  16, 1, 8},
-	{ 26000000, 816000000,  816,  26, 1, 12},
+	{ 12000000, 816000000,  816,  12, 0, 12},
+	{ 13000000, 816000000,  816,  13, 0, 12},
+	{ 19200000, 816000000,  680,  16, 0, 8},
+	{ 26000000, 816000000,  816,  26, 0, 12},
 
 	/* 760 MHz */
-	{ 12000000, 760000000,  760,  12, 1, 12},
-	{ 13000000, 760000000,  760,  13, 1, 12},
-	{ 19200000, 760000000,  950,  24, 1, 8},
-	{ 26000000, 760000000,  760,  26, 1, 12},
+	{ 12000000, 760000000,  760,  12, 0, 12},
+	{ 13000000, 760000000,  760,  13, 0, 12},
+	{ 19200000, 760000000,  950,  24, 0, 8},
+	{ 26000000, 760000000,  760,  26, 0, 12},
 
 	/* 750 MHz */
-	{ 12000000, 750000000,  750,  12, 1, 12},
-	{ 13000000, 750000000,  750,  13, 1, 12},
-	{ 19200000, 750000000,  625,  16, 1, 8},
-	{ 26000000, 750000000,  750,  26, 1, 12},
+	{ 12000000, 750000000,  750,  12, 0, 12},
+	{ 13000000, 750000000,  750,  13, 0, 12},
+	{ 19200000, 750000000,  625,  16, 0, 8},
+	{ 26000000, 750000000,  750,  26, 0, 12},
 
 	/* 608 MHz */
-	{ 12000000, 608000000,  608,  12, 1, 12},
-	{ 13000000, 608000000,  608,  13, 1, 12},
-	{ 19200000, 608000000,  380,  12, 1, 8},
-	{ 26000000, 608000000,  608,  26, 1, 12},
+	{ 12000000, 608000000,  608,  12, 0, 12},
+	{ 13000000, 608000000,  608,  13, 0, 12},
+	{ 19200000, 608000000,  380,  12, 0, 8},
+	{ 26000000, 608000000,  608,  26, 0, 12},
 
 	/* 456 MHz */
-	{ 12000000, 456000000,  456,  12, 1, 12},
-	{ 13000000, 456000000,  456,  13, 1, 12},
-	{ 19200000, 456000000,  380,  16, 1, 8},
-	{ 26000000, 456000000,  456,  26, 1, 12},
+	{ 12000000, 456000000,  456,  12, 0, 12},
+	{ 13000000, 456000000,  456,  13, 0, 12},
+	{ 19200000, 456000000,  380,  16, 0, 8},
+	{ 26000000, 456000000,  456,  26, 0, 12},
 
 	/* 312 MHz */
-	{ 12000000, 312000000,  312,  12, 1, 12},
-	{ 13000000, 312000000,  312,  13, 1, 12},
-	{ 19200000, 312000000,  260,  16, 1, 8},
-	{ 26000000, 312000000,  312,  26, 1, 12},
+	{ 12000000, 312000000,  312,  12, 0, 12},
+	{ 13000000, 312000000,  312,  13, 0, 12},
+	{ 19200000, 312000000,  260,  16, 0, 8},
+	{ 26000000, 312000000,  312,  26, 0, 12},
 
 	{ 0, 0, 0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_e_freq_table[] = {
-	{ 12000000, 100000000,  200,  24, 1, 0 },
+	{ 12000000, 100000000,  200,  24, 0, 0 },
 	{ 0, 0, 0, 0, 0, 0 },
 };
 
@@ -380,7 +380,7 @@ static struct tegra_clk_pll_params pll_c_params = {
 	.vco_max = 1400000000,
 	.base_reg = PLLC_BASE,
 	.misc_reg = PLLC_MISC,
-	.lock_bit_idx = PLL_BASE_LOCK,
+	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 };
@@ -394,7 +394,7 @@ static struct tegra_clk_pll_params pll_m_params = {
 	.vco_max = 1200000000,
 	.base_reg = PLLM_BASE,
 	.misc_reg = PLLM_MISC,
-	.lock_bit_idx = PLL_BASE_LOCK,
+	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 };
@@ -408,7 +408,7 @@ static struct tegra_clk_pll_params pll_p_params = {
 	.vco_max = 1400000000,
 	.base_reg = PLLP_BASE,
 	.misc_reg = PLLP_MISC,
-	.lock_bit_idx = PLL_BASE_LOCK,
+	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 };
@@ -422,7 +422,7 @@ static struct tegra_clk_pll_params pll_a_params = {
 	.vco_max = 1400000000,
 	.base_reg = PLLA_BASE,
 	.misc_reg = PLLA_MISC,
-	.lock_bit_idx = PLL_BASE_LOCK,
+	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 };
@@ -436,11 +436,17 @@ static struct tegra_clk_pll_params pll_d_params = {
 	.vco_max = 1000000000,
 	.base_reg = PLLD_BASE,
 	.misc_reg = PLLD_MISC,
-	.lock_bit_idx = PLL_BASE_LOCK,
+	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLLDU_MISC_LOCK_ENABLE,
 	.lock_delay = 1000,
 };
 
+static struct pdiv_map pllu_p[] = {
+	{ .pdiv = 1, .hw_val = 1 },
+	{ .pdiv = 2, .hw_val = 0 },
+	{ .pdiv = 0, .hw_val = 0 },
+};
+
 static struct tegra_clk_pll_params pll_u_params = {
 	.input_min = 2000000,
 	.input_max = 40000000,
@@ -450,9 +456,10 @@ static struct tegra_clk_pll_params pll_u_params = {
 	.vco_max = 960000000,
 	.base_reg = PLLU_BASE,
 	.misc_reg = PLLU_MISC,
-	.lock_bit_idx = PLL_BASE_LOCK,
+	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLLDU_MISC_LOCK_ENABLE,
 	.lock_delay = 1000,
+	.pdiv_tohw = pllu_p,
 };
 
 static struct tegra_clk_pll_params pll_x_params = {
@@ -464,7 +471,7 @@ static struct tegra_clk_pll_params pll_x_params = {
 	.vco_max = 1200000000,
 	.base_reg = PLLX_BASE,
 	.misc_reg = PLLX_MISC,
-	.lock_bit_idx = PLL_BASE_LOCK,
+	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 };
@@ -478,7 +485,7 @@ static struct tegra_clk_pll_params pll_e_params = {
 	.vco_max = 0,
 	.base_reg = PLLE_BASE,
 	.misc_reg = PLLE_MISC,
-	.lock_bit_idx = PLLE_MISC_LOCK,
+	.lock_mask = PLLE_MISC_LOCK,
 	.lock_enable_bit_idx = PLLE_MISC_LOCK_ENABLE,
 	.lock_delay = 0,
 };
@@ -711,8 +718,8 @@ static void tegra20_pll_init(void)
 }
 
 static const char *cclk_parents[] = { "clk_m", "pll_c", "clk_32k", "pll_m",
-				      "pll_p_cclk", "pll_p_out4_cclk",
-				      "pll_p_out3_cclk", "clk_d", "pll_x" };
+				      "pll_p", "pll_p_out4",
+				      "pll_p_out3", "clk_d", "pll_x" };
 static const char *sclk_parents[] = { "clk_m", "pll_c_out1", "pll_p_out4",
 				      "pll_p_out3", "pll_p_out2", "clk_d",
 				      "clk_32k", "pll_m_out1" };
@@ -721,38 +728,6 @@ static void tegra20_super_clk_init(void)
 {
 	struct clk *clk;
 
-	/*
-	 * DIV_U71 dividers for CCLK, these dividers are used only
-	 * if parent clock is fixed rate.
-	 */
-
-	/*
-	 * Clock input to cclk divided from pll_p using
-	 * U71 divider of cclk.
-	 */
-	clk = tegra_clk_register_divider("pll_p_cclk", "pll_p",
-				clk_base + SUPER_CCLK_DIVIDER, 0,
-				TEGRA_DIVIDER_INT, 16, 8, 1, NULL);
-	clk_register_clkdev(clk, "pll_p_cclk", NULL);
-
-	/*
-	 * Clock input to cclk divided from pll_p_out3 using
-	 * U71 divider of cclk.
-	 */
-	clk = tegra_clk_register_divider("pll_p_out3_cclk", "pll_p_out3",
-				clk_base + SUPER_CCLK_DIVIDER, 0,
-				TEGRA_DIVIDER_INT, 16, 8, 1, NULL);
-	clk_register_clkdev(clk, "pll_p_out3_cclk", NULL);
-
-	/*
-	 * Clock input to cclk divided from pll_p_out4 using
-	 * U71 divider of cclk.
-	 */
-	clk = tegra_clk_register_divider("pll_p_out4_cclk", "pll_p_out4",
-				clk_base + SUPER_CCLK_DIVIDER, 0,
-				TEGRA_DIVIDER_INT, 16, 8, 1, NULL);
-	clk_register_clkdev(clk, "pll_p_out4_cclk", NULL);
-
 	/* CCLK */
 	clk = tegra_clk_register_super_mux("cclk", cclk_parents,
 			      ARRAY_SIZE(cclk_parents), CLK_SET_RATE_PARENT,
@@ -1044,7 +1019,7 @@ static void __init tegra20_periph_clk_init(void)
 		data = &tegra_periph_clk_list[i];
 		clk = tegra_clk_register_periph(data->name, data->parent_names,
 				data->num_parents, &data->periph,
-				clk_base, data->offset);
+				clk_base, data->offset, data->flags);
 		clk_register_clkdev(clk, data->con_id, data->dev_id);
 		clks[data->clk_id] = clk;
 	}
@@ -1279,9 +1254,16 @@ static __initdata struct tegra_clk_init_table init_table[] = {
 	{host1x, pll_c, 150000000, 0},
 	{disp1, pll_p, 600000000, 0},
 	{disp2, pll_p, 600000000, 0},
+	{gr2d, pll_c, 300000000, 0},
+	{gr3d, pll_c, 300000000, 0},
 	{clk_max, clk_max, 0, 0}, /* This MUST be the last entry */
 };
 
+static void __init tegra20_clock_apply_init_table(void)
+{
+	tegra_init_from_table(init_table, clks, clk_max);
+}
+
 /*
  * Some clocks may be used by different drivers depending on the board
  * configuration.  List those here to register them twice in the clock lookup
@@ -1348,7 +1330,7 @@ void __init tegra20_clock_init(struct device_node *np)
 	clk_data.clk_num = ARRAY_SIZE(clks);
 	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
 
-	tegra_init_from_table(init_table, clks, clk_max);
+	tegra_clk_apply_init_table = tegra20_clock_apply_init_table;
 
 	tegra_cpu_car_ops = &tegra20_cpu_car_ops;
 }
diff --git a/drivers/clk/tegra/clk-tegra30.c b/drivers/clk/tegra/clk-tegra30.c
index ba6f51bc9f3b..c6921f538e28 100644
--- a/drivers/clk/tegra/clk-tegra30.c
+++ b/drivers/clk/tegra/clk-tegra30.c
@@ -22,8 +22,7 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/clk/tegra.h>
-
-#include <mach/powergate.h>
+#include <linux/tegra-powergate.h>
 
 #include "clk.h"
 
@@ -116,8 +115,8 @@
 #define PLLDU_MISC_LOCK_ENABLE 22
 #define PLLE_MISC_LOCK_ENABLE 9
 
-#define PLL_BASE_LOCK 27
-#define PLLE_MISC_LOCK 11
+#define PLL_BASE_LOCK BIT(27)
+#define PLLE_MISC_LOCK BIT(11)
 
 #define PLLE_AUX 0x48c
 #define PLLC_OUT 0x84
@@ -330,7 +329,7 @@ enum tegra30_clk {
 	usb3, mpe, vde, bsea, bsev, speedo, uartd, uarte, i2c3, sbc4, sdmmc3,
 	pcie, owr, afi, csite, pciex, avpucq, la, dtv = 79, ndspeed, i2cslow,
 	dsib, irama = 84, iramb, iramc, iramd, cram2, audio_2x = 90, csus = 92,
-	cdev1, cdev2, cpu_g = 96, cpu_lp, gr3d2, mselect, tsensor, i2s3, i2s4,
+	cdev2, cdev1, cpu_g = 96, cpu_lp, gr3d2, mselect, tsensor, i2s3, i2s4,
 	i2c4, sbc5, sbc6, d_audio, apbif, dam0, dam1, dam2, hda2codec_2x,
 	atomics, audio0_2x, audio1_2x, audio2_2x, audio3_2x, audio4_2x,
 	spdif_2x, actmon, extern1, extern2, extern3, sata_oob, sata, hda,
@@ -374,164 +373,170 @@ static const struct utmi_clk_param utmi_parameters[] = {
 };
 
 static struct tegra_clk_pll_freq_table pll_c_freq_table[] = {
-	{ 12000000, 1040000000, 520,  6, 1, 8},
-	{ 13000000, 1040000000, 480,  6, 1, 8},
-	{ 16800000, 1040000000, 495,  8, 1, 8},	/* actual: 1039.5 MHz */
-	{ 19200000, 1040000000, 325,  6, 1, 6},
-	{ 26000000, 1040000000, 520, 13, 1, 8},
-
-	{ 12000000, 832000000, 416,  6, 1, 8},
-	{ 13000000, 832000000, 832, 13, 1, 8},
-	{ 16800000, 832000000, 396,  8, 1, 8},	/* actual: 831.6 MHz */
-	{ 19200000, 832000000, 260,  6, 1, 8},
-	{ 26000000, 832000000, 416, 13, 1, 8},
-
-	{ 12000000, 624000000, 624, 12, 1, 8},
-	{ 13000000, 624000000, 624, 13, 1, 8},
-	{ 16800000, 600000000, 520, 14, 1, 8},
-	{ 19200000, 624000000, 520, 16, 1, 8},
-	{ 26000000, 624000000, 624, 26, 1, 8},
-
-	{ 12000000, 600000000, 600, 12, 1, 8},
-	{ 13000000, 600000000, 600, 13, 1, 8},
-	{ 16800000, 600000000, 500, 14, 1, 8},
-	{ 19200000, 600000000, 375, 12, 1, 6},
-	{ 26000000, 600000000, 600, 26, 1, 8},
-
-	{ 12000000, 520000000, 520, 12, 1, 8},
-	{ 13000000, 520000000, 520, 13, 1, 8},
-	{ 16800000, 520000000, 495, 16, 1, 8},	/* actual: 519.75 MHz */
-	{ 19200000, 520000000, 325, 12, 1, 6},
-	{ 26000000, 520000000, 520, 26, 1, 8},
-
-	{ 12000000, 416000000, 416, 12, 1, 8},
-	{ 13000000, 416000000, 416, 13, 1, 8},
-	{ 16800000, 416000000, 396, 16, 1, 8},	/* actual: 415.8 MHz */
-	{ 19200000, 416000000, 260, 12, 1, 6},
-	{ 26000000, 416000000, 416, 26, 1, 8},
+	{ 12000000, 1040000000, 520,  6, 0, 8},
+	{ 13000000, 1040000000, 480,  6, 0, 8},
+	{ 16800000, 1040000000, 495,  8, 0, 8},	/* actual: 1039.5 MHz */
+	{ 19200000, 1040000000, 325,  6, 0, 6},
+	{ 26000000, 1040000000, 520, 13, 0, 8},
+
+	{ 12000000, 832000000, 416,  6, 0, 8},
+	{ 13000000, 832000000, 832, 13, 0, 8},
+	{ 16800000, 832000000, 396,  8, 0, 8},	/* actual: 831.6 MHz */
+	{ 19200000, 832000000, 260,  6, 0, 8},
+	{ 26000000, 832000000, 416, 13, 0, 8},
+
+	{ 12000000, 624000000, 624, 12, 0, 8},
+	{ 13000000, 624000000, 624, 13, 0, 8},
+	{ 16800000, 600000000, 520, 14, 0, 8},
+	{ 19200000, 624000000, 520, 16, 0, 8},
+	{ 26000000, 624000000, 624, 26, 0, 8},
+
+	{ 12000000, 600000000, 600, 12, 0, 8},
+	{ 13000000, 600000000, 600, 13, 0, 8},
+	{ 16800000, 600000000, 500, 14, 0, 8},
+	{ 19200000, 600000000, 375, 12, 0, 6},
+	{ 26000000, 600000000, 600, 26, 0, 8},
+
+	{ 12000000, 520000000, 520, 12, 0, 8},
+	{ 13000000, 520000000, 520, 13, 0, 8},
+	{ 16800000, 520000000, 495, 16, 0, 8},	/* actual: 519.75 MHz */
+	{ 19200000, 520000000, 325, 12, 0, 6},
+	{ 26000000, 520000000, 520, 26, 0, 8},
+
+	{ 12000000, 416000000, 416, 12, 0, 8},
+	{ 13000000, 416000000, 416, 13, 0, 8},
+	{ 16800000, 416000000, 396, 16, 0, 8},	/* actual: 415.8 MHz */
+	{ 19200000, 416000000, 260, 12, 0, 6},
+	{ 26000000, 416000000, 416, 26, 0, 8},
 	{ 0, 0, 0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_m_freq_table[] = {
-	{ 12000000, 666000000, 666, 12, 1, 8},
-	{ 13000000, 666000000, 666, 13, 1, 8},
-	{ 16800000, 666000000, 555, 14, 1, 8},
-	{ 19200000, 666000000, 555, 16, 1, 8},
-	{ 26000000, 666000000, 666, 26, 1, 8},
-	{ 12000000, 600000000, 600, 12, 1, 8},
-	{ 13000000, 600000000, 600, 13, 1, 8},
-	{ 16800000, 600000000, 500, 14, 1, 8},
-	{ 19200000, 600000000, 375, 12, 1, 6},
-	{ 26000000, 600000000, 600, 26, 1, 8},
+	{ 12000000, 666000000, 666, 12, 0, 8},
+	{ 13000000, 666000000, 666, 13, 0, 8},
+	{ 16800000, 666000000, 555, 14, 0, 8},
+	{ 19200000, 666000000, 555, 16, 0, 8},
+	{ 26000000, 666000000, 666, 26, 0, 8},
+	{ 12000000, 600000000, 600, 12, 0, 8},
+	{ 13000000, 600000000, 600, 13, 0, 8},
+	{ 16800000, 600000000, 500, 14, 0, 8},
+	{ 19200000, 600000000, 375, 12, 0, 6},
+	{ 26000000, 600000000, 600, 26, 0, 8},
 	{ 0, 0, 0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_p_freq_table[] = {
-	{ 12000000, 216000000, 432, 12, 2, 8},
-	{ 13000000, 216000000, 432, 13, 2, 8},
-	{ 16800000, 216000000, 360, 14, 2, 8},
-	{ 19200000, 216000000, 360, 16, 2, 8},
-	{ 26000000, 216000000, 432, 26, 2, 8},
+	{ 12000000, 216000000, 432, 12, 1, 8},
+	{ 13000000, 216000000, 432, 13, 1, 8},
+	{ 16800000, 216000000, 360, 14, 1, 8},
+	{ 19200000, 216000000, 360, 16, 1, 8},
+	{ 26000000, 216000000, 432, 26, 1, 8},
 	{ 0, 0, 0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_a_freq_table[] = {
-	{ 9600000, 564480000, 294, 5, 1, 4},
-	{ 9600000, 552960000, 288, 5, 1, 4},
-	{ 9600000, 24000000,  5,   2, 1, 1},
+	{ 9600000, 564480000, 294, 5, 0, 4},
+	{ 9600000, 552960000, 288, 5, 0, 4},
+	{ 9600000, 24000000,  5,   2, 0, 1},
 
-	{ 28800000, 56448000, 49, 25, 1, 1},
-	{ 28800000, 73728000, 64, 25, 1, 1},
-	{ 28800000, 24000000,  5,  6, 1, 1},
+	{ 28800000, 56448000, 49, 25, 0, 1},
+	{ 28800000, 73728000, 64, 25, 0, 1},
+	{ 28800000, 24000000,  5,  6, 0, 1},
 	{ 0, 0, 0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_d_freq_table[] = {
-	{ 12000000, 216000000, 216, 12, 1, 4},
-	{ 13000000, 216000000, 216, 13, 1, 4},
-	{ 16800000, 216000000, 180, 14, 1, 4},
-	{ 19200000, 216000000, 180, 16, 1, 4},
-	{ 26000000, 216000000, 216, 26, 1, 4},
-
-	{ 12000000, 594000000, 594, 12, 1, 8},
-	{ 13000000, 594000000, 594, 13, 1, 8},
-	{ 16800000, 594000000, 495, 14, 1, 8},
-	{ 19200000, 594000000, 495, 16, 1, 8},
-	{ 26000000, 594000000, 594, 26, 1, 8},
-
-	{ 12000000, 1000000000, 1000, 12, 1, 12},
-	{ 13000000, 1000000000, 1000, 13, 1, 12},
-	{ 19200000, 1000000000, 625,  12, 1, 8},
-	{ 26000000, 1000000000, 1000, 26, 1, 12},
+	{ 12000000, 216000000, 216, 12, 0, 4},
+	{ 13000000, 216000000, 216, 13, 0, 4},
+	{ 16800000, 216000000, 180, 14, 0, 4},
+	{ 19200000, 216000000, 180, 16, 0, 4},
+	{ 26000000, 216000000, 216, 26, 0, 4},
+
+	{ 12000000, 594000000, 594, 12, 0, 8},
+	{ 13000000, 594000000, 594, 13, 0, 8},
+	{ 16800000, 594000000, 495, 14, 0, 8},
+	{ 19200000, 594000000, 495, 16, 0, 8},
+	{ 26000000, 594000000, 594, 26, 0, 8},
+
+	{ 12000000, 1000000000, 1000, 12, 0, 12},
+	{ 13000000, 1000000000, 1000, 13, 0, 12},
+	{ 19200000, 1000000000, 625,  12, 0, 8},
+	{ 26000000, 1000000000, 1000, 26, 0, 12},
 
 	{ 0, 0, 0, 0, 0, 0 },
 };
 
+static struct pdiv_map pllu_p[] = {
+	{ .pdiv = 1, .hw_val = 1 },
+	{ .pdiv = 2, .hw_val = 0 },
+	{ .pdiv = 0, .hw_val = 0 },
+};
+
 static struct tegra_clk_pll_freq_table pll_u_freq_table[] = {
-	{ 12000000, 480000000, 960, 12, 2, 12},
-	{ 13000000, 480000000, 960, 13, 2, 12},
-	{ 16800000, 480000000, 400, 7,  2, 5},
-	{ 19200000, 480000000, 200, 4,  2, 3},
-	{ 26000000, 480000000, 960, 26, 2, 12},
+	{ 12000000, 480000000, 960, 12, 0, 12},
+	{ 13000000, 480000000, 960, 13, 0, 12},
+	{ 16800000, 480000000, 400, 7,  0, 5},
+	{ 19200000, 480000000, 200, 4,  0, 3},
+	{ 26000000, 480000000, 960, 26, 0, 12},
 	{ 0, 0, 0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_x_freq_table[] = {
 	/* 1.7 GHz */
-	{ 12000000, 1700000000, 850,  6,  1, 8},
-	{ 13000000, 1700000000, 915,  7,  1, 8},	/* actual: 1699.2 MHz */
-	{ 16800000, 1700000000, 708,  7,  1, 8},	/* actual: 1699.2 MHz */
-	{ 19200000, 1700000000, 885,  10, 1, 8},	/* actual: 1699.2 MHz */
-	{ 26000000, 1700000000, 850,  13, 1, 8},
+	{ 12000000, 1700000000, 850,  6,  0, 8},
+	{ 13000000, 1700000000, 915,  7,  0, 8},	/* actual: 1699.2 MHz */
+	{ 16800000, 1700000000, 708,  7,  0, 8},	/* actual: 1699.2 MHz */
+	{ 19200000, 1700000000, 885,  10, 0, 8},	/* actual: 1699.2 MHz */
+	{ 26000000, 1700000000, 850,  13, 0, 8},
 
 	/* 1.6 GHz */
-	{ 12000000, 1600000000, 800,  6,  1, 8},
-	{ 13000000, 1600000000, 738,  6,  1, 8},	/* actual: 1599.0 MHz */
-	{ 16800000, 1600000000, 857,  9,  1, 8},	/* actual: 1599.7 MHz */
-	{ 19200000, 1600000000, 500,  6,  1, 8},
-	{ 26000000, 1600000000, 800,  13, 1, 8},
+	{ 12000000, 1600000000, 800,  6,  0, 8},
+	{ 13000000, 1600000000, 738,  6,  0, 8},	/* actual: 1599.0 MHz */
+	{ 16800000, 1600000000, 857,  9,  0, 8},	/* actual: 1599.7 MHz */
+	{ 19200000, 1600000000, 500,  6,  0, 8},
+	{ 26000000, 1600000000, 800,  13, 0, 8},
 
 	/* 1.5 GHz */
-	{ 12000000, 1500000000, 750,  6,  1, 8},
-	{ 13000000, 1500000000, 923,  8,  1, 8},	/* actual: 1499.8 MHz */
-	{ 16800000, 1500000000, 625,  7,  1, 8},
-	{ 19200000, 1500000000, 625,  8,  1, 8},
-	{ 26000000, 1500000000, 750,  13, 1, 8},
+	{ 12000000, 1500000000, 750,  6,  0, 8},
+	{ 13000000, 1500000000, 923,  8,  0, 8},	/* actual: 1499.8 MHz */
+	{ 16800000, 1500000000, 625,  7,  0, 8},
+	{ 19200000, 1500000000, 625,  8,  0, 8},
+	{ 26000000, 1500000000, 750,  13, 0, 8},
 
 	/* 1.4 GHz */
-	{ 12000000, 1400000000, 700,  6,  1, 8},
-	{ 13000000, 1400000000, 969,  9,  1, 8},	/* actual: 1399.7 MHz */
-	{ 16800000, 1400000000, 1000, 12, 1, 8},
-	{ 19200000, 1400000000, 875,  12, 1, 8},
-	{ 26000000, 1400000000, 700,  13, 1, 8},
+	{ 12000000, 1400000000, 700,  6,  0, 8},
+	{ 13000000, 1400000000, 969,  9,  0, 8},	/* actual: 1399.7 MHz */
+	{ 16800000, 1400000000, 1000, 12, 0, 8},
+	{ 19200000, 1400000000, 875,  12, 0, 8},
+	{ 26000000, 1400000000, 700,  13, 0, 8},
 
 	/* 1.3 GHz */
-	{ 12000000, 1300000000, 975,  9,  1, 8},
-	{ 13000000, 1300000000, 1000, 10, 1, 8},
-	{ 16800000, 1300000000, 928,  12, 1, 8},	/* actual: 1299.2 MHz */
-	{ 19200000, 1300000000, 812,  12, 1, 8},	/* actual: 1299.2 MHz */
-	{ 26000000, 1300000000, 650,  13, 1, 8},
+	{ 12000000, 1300000000, 975,  9,  0, 8},
+	{ 13000000, 1300000000, 1000, 10, 0, 8},
+	{ 16800000, 1300000000, 928,  12, 0, 8},	/* actual: 1299.2 MHz */
+	{ 19200000, 1300000000, 812,  12, 0, 8},	/* actual: 1299.2 MHz */
+	{ 26000000, 1300000000, 650,  13, 0, 8},
 
 	/* 1.2 GHz */
-	{ 12000000, 1200000000, 1000, 10, 1, 8},
-	{ 13000000, 1200000000, 923,  10, 1, 8},	/* actual: 1199.9 MHz */
-	{ 16800000, 1200000000, 1000, 14, 1, 8},
-	{ 19200000, 1200000000, 1000, 16, 1, 8},
-	{ 26000000, 1200000000, 600,  13, 1, 8},
+	{ 12000000, 1200000000, 1000, 10, 0, 8},
+	{ 13000000, 1200000000, 923,  10, 0, 8},	/* actual: 1199.9 MHz */
+	{ 16800000, 1200000000, 1000, 14, 0, 8},
+	{ 19200000, 1200000000, 1000, 16, 0, 8},
+	{ 26000000, 1200000000, 600,  13, 0, 8},
 
 	/* 1.1 GHz */
-	{ 12000000, 1100000000, 825,  9,  1, 8},
-	{ 13000000, 1100000000, 846,  10, 1, 8},	/* actual: 1099.8 MHz */
-	{ 16800000, 1100000000, 982,  15, 1, 8},	/* actual: 1099.8 MHz */
-	{ 19200000, 1100000000, 859,  15, 1, 8},	/* actual: 1099.5 MHz */
-	{ 26000000, 1100000000, 550,  13, 1, 8},
+	{ 12000000, 1100000000, 825,  9,  0, 8},
+	{ 13000000, 1100000000, 846,  10, 0, 8},	/* actual: 1099.8 MHz */
+	{ 16800000, 1100000000, 982,  15, 0, 8},	/* actual: 1099.8 MHz */
+	{ 19200000, 1100000000, 859,  15, 0, 8},	/* actual: 1099.5 MHz */
+	{ 26000000, 1100000000, 550,  13, 0, 8},
 
 	/* 1 GHz */
-	{ 12000000, 1000000000, 1000, 12, 1, 8},
-	{ 13000000, 1000000000, 1000, 13, 1, 8},
-	{ 16800000, 1000000000, 833,  14, 1, 8},	/* actual: 999.6 MHz */
-	{ 19200000, 1000000000, 625,  12, 1, 8},
-	{ 26000000, 1000000000, 1000, 26, 1, 8},
+	{ 12000000, 1000000000, 1000, 12, 0, 8},
+	{ 13000000, 1000000000, 1000, 13, 0, 8},
+	{ 16800000, 1000000000, 833,  14, 0, 8},	/* actual: 999.6 MHz */
+	{ 19200000, 1000000000, 625,  12, 0, 8},
+	{ 26000000, 1000000000, 1000, 26, 0, 8},
 
 	{ 0, 0, 0, 0, 0, 0 },
 };
@@ -553,7 +558,7 @@ static struct tegra_clk_pll_params pll_c_params = {
 	.vco_max = 1400000000,
 	.base_reg = PLLC_BASE,
 	.misc_reg = PLLC_MISC,
-	.lock_bit_idx = PLL_BASE_LOCK,
+	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 };
@@ -567,7 +572,7 @@ static struct tegra_clk_pll_params pll_m_params = {
 	.vco_max = 1200000000,
 	.base_reg = PLLM_BASE,
 	.misc_reg = PLLM_MISC,
-	.lock_bit_idx = PLL_BASE_LOCK,
+	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 };
@@ -581,7 +586,7 @@ static struct tegra_clk_pll_params pll_p_params = {
 	.vco_max = 1400000000,
 	.base_reg = PLLP_BASE,
 	.misc_reg = PLLP_MISC,
-	.lock_bit_idx = PLL_BASE_LOCK,
+	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 };
@@ -595,7 +600,7 @@ static struct tegra_clk_pll_params pll_a_params = {
 	.vco_max = 1400000000,
 	.base_reg = PLLA_BASE,
 	.misc_reg = PLLA_MISC,
-	.lock_bit_idx = PLL_BASE_LOCK,
+	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 };
@@ -609,7 +614,7 @@ static struct tegra_clk_pll_params pll_d_params = {
 	.vco_max = 1000000000,
 	.base_reg = PLLD_BASE,
 	.misc_reg = PLLD_MISC,
-	.lock_bit_idx = PLL_BASE_LOCK,
+	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLLDU_MISC_LOCK_ENABLE,
 	.lock_delay = 1000,
 };
@@ -623,7 +628,7 @@ static struct tegra_clk_pll_params pll_d2_params = {
 	.vco_max = 1000000000,
 	.base_reg = PLLD2_BASE,
 	.misc_reg = PLLD2_MISC,
-	.lock_bit_idx = PLL_BASE_LOCK,
+	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLLDU_MISC_LOCK_ENABLE,
 	.lock_delay = 1000,
 };
@@ -637,9 +642,10 @@ static struct tegra_clk_pll_params pll_u_params = {
 	.vco_max = 960000000,
 	.base_reg = PLLU_BASE,
 	.misc_reg = PLLU_MISC,
-	.lock_bit_idx = PLL_BASE_LOCK,
+	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLLDU_MISC_LOCK_ENABLE,
 	.lock_delay = 1000,
+	.pdiv_tohw = pllu_p,
 };
 
 static struct tegra_clk_pll_params pll_x_params = {
@@ -651,7 +657,7 @@ static struct tegra_clk_pll_params pll_x_params = {
 	.vco_max = 1700000000,
 	.base_reg = PLLX_BASE,
 	.misc_reg = PLLX_MISC,
-	.lock_bit_idx = PLL_BASE_LOCK,
+	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 };
@@ -665,7 +671,7 @@ static struct tegra_clk_pll_params pll_e_params = {
 	.vco_max = 2400000000U,
 	.base_reg = PLLE_BASE,
 	.misc_reg = PLLE_MISC,
-	.lock_bit_idx = PLLE_MISC_LOCK,
+	.lock_mask = PLLE_MISC_LOCK,
 	.lock_enable_bit_idx = PLLE_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 };
@@ -1661,7 +1667,7 @@ static void __init tegra30_periph_clk_init(void)
 		data = &tegra_periph_clk_list[i];
 		clk = tegra_clk_register_periph(data->name, data->parent_names,
 				data->num_parents, &data->periph,
-				clk_base, data->offset);
+				clk_base, data->offset, data->flags);
 		clk_register_clkdev(clk, data->con_id, data->dev_id);
 		clks[data->clk_id] = clk;
 	}
@@ -1911,9 +1917,16 @@ static __initdata struct tegra_clk_init_table init_table[] = {
 	{disp1, pll_p, 600000000, 0},
 	{disp2, pll_p, 600000000, 0},
 	{twd, clk_max, 0, 1},
+	{gr2d, pll_c, 300000000, 0},
+	{gr3d, pll_c, 300000000, 0},
 	{clk_max, clk_max, 0, 0}, /* This MUST be the last entry. */
 };
 
+static void __init tegra30_clock_apply_init_table(void)
+{
+	tegra_init_from_table(init_table, clks, clk_max);
+}
+
 /*
  * Some clocks may be used by different drivers depending on the board
  * configuration.  List those here to register them twice in the clock lookup
@@ -1987,7 +2000,7 @@ void __init tegra30_clock_init(struct device_node *np)
 	clk_data.clk_num = ARRAY_SIZE(clks);
 	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
 
-	tegra_init_from_table(init_table, clks, clk_max);
+	tegra_clk_apply_init_table = tegra30_clock_apply_init_table;
 
 	tegra_cpu_car_ops = &tegra30_cpu_car_ops;
 }
diff --git a/drivers/clk/tegra/clk.c b/drivers/clk/tegra/clk.c
index a603b9af0ad3..923ca7ee4694 100644
--- a/drivers/clk/tegra/clk.c
+++ b/drivers/clk/tegra/clk.c
@@ -22,7 +22,8 @@
 #include "clk.h"
 
 /* Global data of Tegra CPU CAR ops */
-struct tegra_cpu_car_ops *tegra_cpu_car_ops;
+static struct tegra_cpu_car_ops dummy_car_ops;
+struct tegra_cpu_car_ops *tegra_cpu_car_ops = &dummy_car_ops;
 
 void __init tegra_init_dup_clks(struct tegra_clk_duplicate *dup_list,
 				struct clk *clks[], int clk_max)
@@ -76,6 +77,7 @@ void __init tegra_init_from_table(struct tegra_clk_init_table *tbl,
 static const struct of_device_id tegra_dt_clk_match[] = {
 	{ .compatible = "nvidia,tegra20-car", .data = tegra20_clock_init },
 	{ .compatible = "nvidia,tegra30-car", .data = tegra30_clock_init },
+	{ .compatible = "nvidia,tegra114-car", .data = tegra114_clock_init },
 	{ }
 };
 
@@ -83,3 +85,13 @@ void __init tegra_clocks_init(void)
 {
 	of_clk_init(tegra_dt_clk_match);
 }
+
+tegra_clk_apply_init_table_func tegra_clk_apply_init_table;
+
+void __init tegra_clocks_apply_init_table(void)
+{
+	if (!tegra_clk_apply_init_table)
+		return;
+
+	tegra_clk_apply_init_table();
+}
diff --git a/drivers/clk/tegra/clk.h b/drivers/clk/tegra/clk.h
index a09d7dcaf183..e0565620d68e 100644
--- a/drivers/clk/tegra/clk.h
+++ b/drivers/clk/tegra/clk.h
@@ -1,4 +1,4 @@
-/*
+	/*
  * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify it
@@ -117,6 +117,17 @@ struct tegra_clk_pll_freq_table {
 };
 
 /**
+ * struct pdiv_map - map post divider to hw value
+ *
+ * @pdiv:		post divider
+ * @hw_val:		value to be written to the PLL hw
+ */
+struct pdiv_map {
+	u8 pdiv;
+	u8 hw_val;
+};
+
+/**
  * struct clk_pll_params - PLL parameters
  *
  * @input_min:			Minimum input frequency
@@ -143,9 +154,18 @@ struct tegra_clk_pll_params {
 	u32		base_reg;
 	u32		misc_reg;
 	u32		lock_reg;
-	u32		lock_bit_idx;
+	u32		lock_mask;
 	u32		lock_enable_bit_idx;
+	u32		iddq_reg;
+	u32		iddq_bit_idx;
+	u32		aux_reg;
+	u32		dyn_ramp_reg;
+	u32		ext_misc_reg[3];
+	int		stepa_shift;
+	int		stepb_shift;
 	int		lock_delay;
+	int		max_p;
+	struct pdiv_map *pdiv_tohw;
 };
 
 /**
@@ -182,12 +202,16 @@ struct tegra_clk_pll_params {
  * TEGRA_PLL_FIXED - We are not supposed to change output frequency
  *     of some plls.
  * TEGRA_PLLE_CONFIGURE - Configure PLLE when enabling.
+ * TEGRA_PLL_LOCK_MISC - Lock bit is in the misc register instead of the
+ *     base register.
+ * TEGRA_PLL_BYPASS - PLL has bypass bit
+ * TEGRA_PLL_HAS_LOCK_ENABLE - PLL has bit to enable lock monitoring
  */
 struct tegra_clk_pll {
 	struct clk_hw	hw;
 	void __iomem	*clk_base;
 	void __iomem	*pmc;
-	u8		flags;
+	u32		flags;
 	unsigned long	fixed_rate;
 	spinlock_t	*lock;
 	u8		divn_shift;
@@ -210,20 +234,64 @@ struct tegra_clk_pll {
 #define TEGRA_PLLM BIT(5)
 #define TEGRA_PLL_FIXED BIT(6)
 #define TEGRA_PLLE_CONFIGURE BIT(7)
+#define TEGRA_PLL_LOCK_MISC BIT(8)
+#define TEGRA_PLL_BYPASS BIT(9)
+#define TEGRA_PLL_HAS_LOCK_ENABLE BIT(10)
 
 extern const struct clk_ops tegra_clk_pll_ops;
 extern const struct clk_ops tegra_clk_plle_ops;
 struct clk *tegra_clk_register_pll(const char *name, const char *parent_name,
 		void __iomem *clk_base, void __iomem *pmc,
 		unsigned long flags, unsigned long fixed_rate,
-		struct tegra_clk_pll_params *pll_params, u8 pll_flags,
+		struct tegra_clk_pll_params *pll_params, u32 pll_flags,
 		struct tegra_clk_pll_freq_table *freq_table, spinlock_t *lock);
+
 struct clk *tegra_clk_register_plle(const char *name, const char *parent_name,
 		void __iomem *clk_base, void __iomem *pmc,
 		unsigned long flags, unsigned long fixed_rate,
-		struct tegra_clk_pll_params *pll_params, u8 pll_flags,
+		struct tegra_clk_pll_params *pll_params, u32 pll_flags,
 		struct tegra_clk_pll_freq_table *freq_table, spinlock_t *lock);
 
+struct clk *tegra_clk_register_pllxc(const char *name, const char *parent_name,
+			    void __iomem *clk_base, void __iomem *pmc,
+			    unsigned long flags, unsigned long fixed_rate,
+			    struct tegra_clk_pll_params *pll_params,
+			    u32 pll_flags,
+			    struct tegra_clk_pll_freq_table *freq_table,
+			    spinlock_t *lock);
+
+struct clk *tegra_clk_register_pllm(const char *name, const char *parent_name,
+			   void __iomem *clk_base, void __iomem *pmc,
+			   unsigned long flags, unsigned long fixed_rate,
+			   struct tegra_clk_pll_params *pll_params,
+			   u32 pll_flags,
+			   struct tegra_clk_pll_freq_table *freq_table,
+			   spinlock_t *lock);
+
+struct clk *tegra_clk_register_pllc(const char *name, const char *parent_name,
+			   void __iomem *clk_base, void __iomem *pmc,
+			   unsigned long flags, unsigned long fixed_rate,
+			   struct tegra_clk_pll_params *pll_params,
+			   u32 pll_flags,
+			   struct tegra_clk_pll_freq_table *freq_table,
+			   spinlock_t *lock);
+
+struct clk *tegra_clk_register_pllre(const char *name, const char *parent_name,
+			   void __iomem *clk_base, void __iomem *pmc,
+			   unsigned long flags, unsigned long fixed_rate,
+			   struct tegra_clk_pll_params *pll_params,
+			   u32 pll_flags,
+			   struct tegra_clk_pll_freq_table *freq_table,
+			   spinlock_t *lock, unsigned long parent_rate);
+
+struct clk *tegra_clk_register_plle_tegra114(const char *name,
+				const char *parent_name,
+				void __iomem *clk_base, unsigned long flags,
+				unsigned long fixed_rate,
+				struct tegra_clk_pll_params *pll_params,
+				struct tegra_clk_pll_freq_table *freq_table,
+				spinlock_t *lock);
+
 /**
  * struct tegra_clk_pll_out - PLL divider down clock
  *
@@ -290,6 +358,7 @@ struct tegra_clk_periph_regs {
  * TEGRA_PERIPH_ON_APB - If peripheral is in the APB bus then read the
  *     bus to flush the write operation in apb bus. This flag indicates
  *     that this peripheral is in apb bus.
+ * TEGRA_PERIPH_WAR_1005168 - Apply workaround for Tegra114 MSENC bug
  */
 struct tegra_clk_periph_gate {
 	u32			magic;
@@ -309,6 +378,7 @@ struct tegra_clk_periph_gate {
 #define TEGRA_PERIPH_NO_RESET BIT(0)
 #define TEGRA_PERIPH_MANUAL_RESET BIT(1)
 #define TEGRA_PERIPH_ON_APB BIT(2)
+#define TEGRA_PERIPH_WAR_1005168 BIT(3)
 
 void tegra_periph_reset(struct tegra_clk_periph_gate *gate, bool assert);
 extern const struct clk_ops tegra_clk_periph_gate_ops;
@@ -349,7 +419,7 @@ extern const struct clk_ops tegra_clk_periph_ops;
 struct clk *tegra_clk_register_periph(const char *name,
 		const char **parent_names, int num_parents,
 		struct tegra_clk_periph *periph, void __iomem *clk_base,
-		u32 offset);
+		u32 offset, unsigned long flags);
 struct clk *tegra_clk_register_periph_nodiv(const char *name,
 		const char **parent_names, int num_parents,
 		struct tegra_clk_periph *periph, void __iomem *clk_base,
@@ -392,12 +462,14 @@ struct tegra_periph_init_data {
 	u32 offset;
 	const char *con_id;
 	const char *dev_id;
+	unsigned long flags;
 };
 
 #define TEGRA_INIT_DATA_TABLE(_name, _con_id, _dev_id, _parent_names, _offset,\
 			_mux_shift, _mux_mask, _mux_flags, _div_shift,	\
 			_div_width, _div_frac_width, _div_flags, _regs,	\
-			_clk_num, _enb_refcnt, _gate_flags, _clk_id, _table) \
+			_clk_num, _enb_refcnt, _gate_flags, _clk_id, _table,\
+			_flags) \
 	{								\
 		.name = _name,						\
 		.clk_id = _clk_id,					\
@@ -412,6 +484,7 @@ struct tegra_periph_init_data {
 		.offset = _offset,					\
 		.con_id = _con_id,					\
 		.dev_id = _dev_id,					\
+		.flags = _flags						\
 	}
 
 #define TEGRA_INIT_DATA(_name, _con_id, _dev_id, _parent_names, _offset,\
@@ -422,7 +495,7 @@ struct tegra_periph_init_data {
 			_mux_shift, BIT(_mux_width) - 1, _mux_flags,	\
 			_div_shift, _div_width, _div_frac_width, _div_flags, \
 			_regs, _clk_num, _enb_refcnt, _gate_flags, _clk_id,\
-			NULL)
+			NULL, 0)
 
 /**
  * struct clk_super_mux - super clock
@@ -510,4 +583,13 @@ void tegra30_clock_init(struct device_node *np);
 static inline void tegra30_clock_init(struct device_node *np) {}
 #endif /* CONFIG_ARCH_TEGRA_3x_SOC */
 
+#ifdef CONFIG_ARCH_TEGRA_114_SOC
+void tegra114_clock_init(struct device_node *np);
+#else
+static inline void tegra114_clock_init(struct device_node *np) {}
+#endif /* CONFIG_ARCH_TEGRA114_SOC */
+
+typedef void (*tegra_clk_apply_init_table_func)(void);
+extern tegra_clk_apply_init_table_func tegra_clk_apply_init_table;
+
 #endif /* TEGRA_CLK_H */
diff --git a/drivers/clk/ux500/clk-prcc.c b/drivers/clk/ux500/clk-prcc.c
index 7eee7f768355..bd4769a84485 100644
--- a/drivers/clk/ux500/clk-prcc.c
+++ b/drivers/clk/ux500/clk-prcc.c
@@ -13,7 +13,6 @@
 #include <linux/io.h>
 #include <linux/err.h>
 #include <linux/types.h>
-#include <mach/hardware.h>
 
 #include "clk.h"
 
diff --git a/drivers/clk/ux500/u8500_clk.c b/drivers/clk/ux500/u8500_clk.c
index 6b889a0e90b3..0b4f35a5ffc2 100644
--- a/drivers/clk/ux500/u8500_clk.c
+++ b/drivers/clk/ux500/u8500_clk.c
@@ -12,10 +12,10 @@
 #include <linux/clk-provider.h>
 #include <linux/mfd/dbx500-prcmu.h>
 #include <linux/platform_data/clk-ux500.h>
-#include <mach/db8500-regs.h>
 #include "clk.h"
 
-void u8500_clk_init(void)
+void u8500_clk_init(u32 clkrst1_base, u32 clkrst2_base, u32 clkrst3_base,
+		    u32 clkrst5_base, u32 clkrst6_base)
 {
 	struct prcmu_fw_version *fw_version;
 	const char *sgaclk_parent = NULL;
@@ -215,147 +215,148 @@ void u8500_clk_init(void)
 	 */
 
 	/* PRCC P-clocks */
-	clk = clk_reg_prcc_pclk("p1_pclk0", "per1clk", U8500_CLKRST1_BASE,
+	clk = clk_reg_prcc_pclk("p1_pclk0", "per1clk", clkrst1_base,
 				BIT(0), 0);
 	clk_register_clkdev(clk, "apb_pclk", "uart0");
 
-	clk = clk_reg_prcc_pclk("p1_pclk1", "per1clk", U8500_CLKRST1_BASE,
+	clk = clk_reg_prcc_pclk("p1_pclk1", "per1clk", clkrst1_base,
 				BIT(1), 0);
 	clk_register_clkdev(clk, "apb_pclk", "uart1");
 
-	clk = clk_reg_prcc_pclk("p1_pclk2", "per1clk", U8500_CLKRST1_BASE,
+	clk = clk_reg_prcc_pclk("p1_pclk2", "per1clk", clkrst1_base,
 				BIT(2), 0);
 	clk_register_clkdev(clk, "apb_pclk", "nmk-i2c.1");
 
-	clk = clk_reg_prcc_pclk("p1_pclk3", "per1clk", U8500_CLKRST1_BASE,
+	clk = clk_reg_prcc_pclk("p1_pclk3", "per1clk", clkrst1_base,
 				BIT(3), 0);
 	clk_register_clkdev(clk, "apb_pclk", "msp0");
 	clk_register_clkdev(clk, "apb_pclk", "ux500-msp-i2s.0");
 
-	clk = clk_reg_prcc_pclk("p1_pclk4", "per1clk", U8500_CLKRST1_BASE,
+	clk = clk_reg_prcc_pclk("p1_pclk4", "per1clk", clkrst1_base,
 				BIT(4), 0);
 	clk_register_clkdev(clk, "apb_pclk", "msp1");
 	clk_register_clkdev(clk, "apb_pclk", "ux500-msp-i2s.1");
 
-	clk = clk_reg_prcc_pclk("p1_pclk5", "per1clk", U8500_CLKRST1_BASE,
+	clk = clk_reg_prcc_pclk("p1_pclk5", "per1clk", clkrst1_base,
 				BIT(5), 0);
 	clk_register_clkdev(clk, "apb_pclk", "sdi0");
 
-	clk = clk_reg_prcc_pclk("p1_pclk6", "per1clk", U8500_CLKRST1_BASE,
+	clk = clk_reg_prcc_pclk("p1_pclk6", "per1clk", clkrst1_base,
 				BIT(6), 0);
 	clk_register_clkdev(clk, "apb_pclk", "nmk-i2c.2");
 
-	clk = clk_reg_prcc_pclk("p1_pclk7", "per1clk", U8500_CLKRST1_BASE,
+	clk = clk_reg_prcc_pclk("p1_pclk7", "per1clk", clkrst1_base,
 				BIT(7), 0);
 	clk_register_clkdev(clk, NULL, "spi3");
 
-	clk = clk_reg_prcc_pclk("p1_pclk8", "per1clk", U8500_CLKRST1_BASE,
+	clk = clk_reg_prcc_pclk("p1_pclk8", "per1clk", clkrst1_base,
 				BIT(8), 0);
 	clk_register_clkdev(clk, "apb_pclk", "slimbus0");
 
-	clk = clk_reg_prcc_pclk("p1_pclk9", "per1clk", U8500_CLKRST1_BASE,
+	clk = clk_reg_prcc_pclk("p1_pclk9", "per1clk", clkrst1_base,
 				BIT(9), 0);
 	clk_register_clkdev(clk, NULL, "gpio.0");
 	clk_register_clkdev(clk, NULL, "gpio.1");
 	clk_register_clkdev(clk, NULL, "gpioblock0");
 
-	clk = clk_reg_prcc_pclk("p1_pclk10", "per1clk", U8500_CLKRST1_BASE,
+	clk = clk_reg_prcc_pclk("p1_pclk10", "per1clk", clkrst1_base,
 				BIT(10), 0);
 	clk_register_clkdev(clk, "apb_pclk", "nmk-i2c.4");
 
-	clk = clk_reg_prcc_pclk("p1_pclk11", "per1clk", U8500_CLKRST1_BASE,
+	clk = clk_reg_prcc_pclk("p1_pclk11", "per1clk", clkrst1_base,
 				BIT(11), 0);
 	clk_register_clkdev(clk, "apb_pclk", "msp3");
 	clk_register_clkdev(clk, "apb_pclk", "ux500-msp-i2s.3");
 
-	clk = clk_reg_prcc_pclk("p2_pclk0", "per2clk", U8500_CLKRST2_BASE,
+	clk = clk_reg_prcc_pclk("p2_pclk0", "per2clk", clkrst2_base,
 				BIT(0), 0);
 	clk_register_clkdev(clk, "apb_pclk", "nmk-i2c.3");
 
-	clk = clk_reg_prcc_pclk("p2_pclk1", "per2clk", U8500_CLKRST2_BASE,
+	clk = clk_reg_prcc_pclk("p2_pclk1", "per2clk", clkrst2_base,
 				BIT(1), 0);
 	clk_register_clkdev(clk, NULL, "spi2");
 
-	clk = clk_reg_prcc_pclk("p2_pclk2", "per2clk", U8500_CLKRST2_BASE,
+	clk = clk_reg_prcc_pclk("p2_pclk2", "per2clk", clkrst2_base,
 				BIT(2), 0);
 	clk_register_clkdev(clk, NULL, "spi1");
 
-	clk = clk_reg_prcc_pclk("p2_pclk3", "per2clk", U8500_CLKRST2_BASE,
+	clk = clk_reg_prcc_pclk("p2_pclk3", "per2clk", clkrst2_base,
 				BIT(3), 0);
 	clk_register_clkdev(clk, NULL, "pwl");
 
-	clk = clk_reg_prcc_pclk("p2_pclk4", "per2clk", U8500_CLKRST2_BASE,
+	clk = clk_reg_prcc_pclk("p2_pclk4", "per2clk", clkrst2_base,
 				BIT(4), 0);
 	clk_register_clkdev(clk, "apb_pclk", "sdi4");
 
-	clk = clk_reg_prcc_pclk("p2_pclk5", "per2clk", U8500_CLKRST2_BASE,
+	clk = clk_reg_prcc_pclk("p2_pclk5", "per2clk", clkrst2_base,
 				BIT(5), 0);
 	clk_register_clkdev(clk, "apb_pclk", "msp2");
 	clk_register_clkdev(clk, "apb_pclk", "ux500-msp-i2s.2");
 
-	clk = clk_reg_prcc_pclk("p2_pclk6", "per2clk", U8500_CLKRST2_BASE,
+	clk = clk_reg_prcc_pclk("p2_pclk6", "per2clk", clkrst2_base,
 				BIT(6), 0);
 	clk_register_clkdev(clk, "apb_pclk", "sdi1");
 
-	clk = clk_reg_prcc_pclk("p2_pclk7", "per2clk", U8500_CLKRST2_BASE,
+	clk = clk_reg_prcc_pclk("p2_pclk7", "per2clk", clkrst2_base,
 				BIT(7), 0);
 	clk_register_clkdev(clk, "apb_pclk", "sdi3");
 
-	clk = clk_reg_prcc_pclk("p2_pclk8", "per2clk", U8500_CLKRST2_BASE,
+	clk = clk_reg_prcc_pclk("p2_pclk8", "per2clk", clkrst2_base,
 				BIT(8), 0);
 	clk_register_clkdev(clk, NULL, "spi0");
 
-	clk = clk_reg_prcc_pclk("p2_pclk9", "per2clk", U8500_CLKRST2_BASE,
+	clk = clk_reg_prcc_pclk("p2_pclk9", "per2clk", clkrst2_base,
 				BIT(9), 0);
 	clk_register_clkdev(clk, "hsir_hclk", "ste_hsi.0");
 
-	clk = clk_reg_prcc_pclk("p2_pclk10", "per2clk", U8500_CLKRST2_BASE,
+	clk = clk_reg_prcc_pclk("p2_pclk10", "per2clk", clkrst2_base,
 				BIT(10), 0);
 	clk_register_clkdev(clk, "hsit_hclk", "ste_hsi.0");
 
-	clk = clk_reg_prcc_pclk("p2_pclk11", "per2clk", U8500_CLKRST2_BASE,
+	clk = clk_reg_prcc_pclk("p2_pclk11", "per2clk", clkrst2_base,
 				BIT(11), 0);
 	clk_register_clkdev(clk, NULL, "gpio.6");
 	clk_register_clkdev(clk, NULL, "gpio.7");
 	clk_register_clkdev(clk, NULL, "gpioblock1");
 
-	clk = clk_reg_prcc_pclk("p2_pclk12", "per2clk", U8500_CLKRST2_BASE,
+	clk = clk_reg_prcc_pclk("p2_pclk12", "per2clk", clkrst2_base,
 				BIT(12), 0);
 
-	clk = clk_reg_prcc_pclk("p3_pclk0", "per3clk", U8500_CLKRST3_BASE,
+	clk = clk_reg_prcc_pclk("p3_pclk0", "per3clk", clkrst3_base,
 				BIT(0), 0);
-	clk_register_clkdev(clk, NULL, "fsmc");
+	clk_register_clkdev(clk, "fsmc", NULL);
+	clk_register_clkdev(clk, NULL, "smsc911x");
 
-	clk = clk_reg_prcc_pclk("p3_pclk1", "per3clk", U8500_CLKRST3_BASE,
+	clk = clk_reg_prcc_pclk("p3_pclk1", "per3clk", clkrst3_base,
 				BIT(1), 0);
 	clk_register_clkdev(clk, "apb_pclk", "ssp0");
 
-	clk = clk_reg_prcc_pclk("p3_pclk2", "per3clk", U8500_CLKRST3_BASE,
+	clk = clk_reg_prcc_pclk("p3_pclk2", "per3clk", clkrst3_base,
 				BIT(2), 0);
 	clk_register_clkdev(clk, "apb_pclk", "ssp1");
 
-	clk = clk_reg_prcc_pclk("p3_pclk3", "per3clk", U8500_CLKRST3_BASE,
+	clk = clk_reg_prcc_pclk("p3_pclk3", "per3clk", clkrst3_base,
 				BIT(3), 0);
 	clk_register_clkdev(clk, "apb_pclk", "nmk-i2c.0");
 
-	clk = clk_reg_prcc_pclk("p3_pclk4", "per3clk", U8500_CLKRST3_BASE,
+	clk = clk_reg_prcc_pclk("p3_pclk4", "per3clk", clkrst3_base,
 				BIT(4), 0);
 	clk_register_clkdev(clk, "apb_pclk", "sdi2");
 
-	clk = clk_reg_prcc_pclk("p3_pclk5", "per3clk", U8500_CLKRST3_BASE,
+	clk = clk_reg_prcc_pclk("p3_pclk5", "per3clk", clkrst3_base,
 				BIT(5), 0);
 	clk_register_clkdev(clk, "apb_pclk", "ske");
 	clk_register_clkdev(clk, "apb_pclk", "nmk-ske-keypad");
 
-	clk = clk_reg_prcc_pclk("p3_pclk6", "per3clk", U8500_CLKRST3_BASE,
+	clk = clk_reg_prcc_pclk("p3_pclk6", "per3clk", clkrst3_base,
 				BIT(6), 0);
 	clk_register_clkdev(clk, "apb_pclk", "uart2");
 
-	clk = clk_reg_prcc_pclk("p3_pclk7", "per3clk", U8500_CLKRST3_BASE,
+	clk = clk_reg_prcc_pclk("p3_pclk7", "per3clk", clkrst3_base,
 				BIT(7), 0);
 	clk_register_clkdev(clk, "apb_pclk", "sdi5");
 
-	clk = clk_reg_prcc_pclk("p3_pclk8", "per3clk", U8500_CLKRST3_BASE,
+	clk = clk_reg_prcc_pclk("p3_pclk8", "per3clk", clkrst3_base,
 				BIT(8), 0);
 	clk_register_clkdev(clk, NULL, "gpio.2");
 	clk_register_clkdev(clk, NULL, "gpio.3");
@@ -363,45 +364,45 @@ void u8500_clk_init(void)
 	clk_register_clkdev(clk, NULL, "gpio.5");
 	clk_register_clkdev(clk, NULL, "gpioblock2");
 
-	clk = clk_reg_prcc_pclk("p5_pclk0", "per5clk", U8500_CLKRST5_BASE,
+	clk = clk_reg_prcc_pclk("p5_pclk0", "per5clk", clkrst5_base,
 				BIT(0), 0);
 	clk_register_clkdev(clk, "usb", "musb-ux500.0");
 
-	clk = clk_reg_prcc_pclk("p5_pclk1", "per5clk", U8500_CLKRST5_BASE,
+	clk = clk_reg_prcc_pclk("p5_pclk1", "per5clk", clkrst5_base,
 				BIT(1), 0);
 	clk_register_clkdev(clk, NULL, "gpio.8");
 	clk_register_clkdev(clk, NULL, "gpioblock3");
 
-	clk = clk_reg_prcc_pclk("p6_pclk0", "per6clk", U8500_CLKRST6_BASE,
+	clk = clk_reg_prcc_pclk("p6_pclk0", "per6clk", clkrst6_base,
 				BIT(0), 0);
 	clk_register_clkdev(clk, "apb_pclk", "rng");
 
-	clk = clk_reg_prcc_pclk("p6_pclk1", "per6clk", U8500_CLKRST6_BASE,
+	clk = clk_reg_prcc_pclk("p6_pclk1", "per6clk", clkrst6_base,
 				BIT(1), 0);
 	clk_register_clkdev(clk, NULL, "cryp0");
 	clk_register_clkdev(clk, NULL, "cryp1");
 
-	clk = clk_reg_prcc_pclk("p6_pclk2", "per6clk", U8500_CLKRST6_BASE,
+	clk = clk_reg_prcc_pclk("p6_pclk2", "per6clk", clkrst6_base,
 				BIT(2), 0);
 	clk_register_clkdev(clk, NULL, "hash0");
 
-	clk = clk_reg_prcc_pclk("p6_pclk3", "per6clk", U8500_CLKRST6_BASE,
+	clk = clk_reg_prcc_pclk("p6_pclk3", "per6clk", clkrst6_base,
 				BIT(3), 0);
 	clk_register_clkdev(clk, NULL, "pka");
 
-	clk = clk_reg_prcc_pclk("p6_pclk4", "per6clk", U8500_CLKRST6_BASE,
+	clk = clk_reg_prcc_pclk("p6_pclk4", "per6clk", clkrst6_base,
 				BIT(4), 0);
 	clk_register_clkdev(clk, NULL, "hash1");
 
-	clk = clk_reg_prcc_pclk("p6_pclk5", "per6clk", U8500_CLKRST6_BASE,
+	clk = clk_reg_prcc_pclk("p6_pclk5", "per6clk", clkrst6_base,
 				BIT(5), 0);
 	clk_register_clkdev(clk, NULL, "cfgreg");
 
-	clk = clk_reg_prcc_pclk("p6_pclk6", "per6clk", U8500_CLKRST6_BASE,
+	clk = clk_reg_prcc_pclk("p6_pclk6", "per6clk", clkrst6_base,
 				BIT(6), 0);
 	clk_register_clkdev(clk, "apb_pclk", "mtu0");
 
-	clk = clk_reg_prcc_pclk("p6_pclk7", "per6clk", U8500_CLKRST6_BASE,
+	clk = clk_reg_prcc_pclk("p6_pclk7", "per6clk", clkrst6_base,
 				BIT(7), 0);
 	clk_register_clkdev(clk, "apb_pclk", "mtu1");
 
@@ -415,110 +416,110 @@ void u8500_clk_init(void)
 
 	/* Periph1 */
 	clk = clk_reg_prcc_kclk("p1_uart0_kclk", "uartclk",
-			U8500_CLKRST1_BASE, BIT(0), CLK_SET_RATE_GATE);
+			clkrst1_base, BIT(0), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "uart0");
 
 	clk = clk_reg_prcc_kclk("p1_uart1_kclk", "uartclk",
-			U8500_CLKRST1_BASE, BIT(1), CLK_SET_RATE_GATE);
+			clkrst1_base, BIT(1), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "uart1");
 
 	clk = clk_reg_prcc_kclk("p1_i2c1_kclk", "i2cclk",
-			U8500_CLKRST1_BASE, BIT(2), CLK_SET_RATE_GATE);
+			clkrst1_base, BIT(2), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "nmk-i2c.1");
 
 	clk = clk_reg_prcc_kclk("p1_msp0_kclk", "msp02clk",
-			U8500_CLKRST1_BASE, BIT(3), CLK_SET_RATE_GATE);
+			clkrst1_base, BIT(3), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "msp0");
 	clk_register_clkdev(clk, NULL, "ux500-msp-i2s.0");
 
 	clk = clk_reg_prcc_kclk("p1_msp1_kclk", "msp1clk",
-			U8500_CLKRST1_BASE, BIT(4), CLK_SET_RATE_GATE);
+			clkrst1_base, BIT(4), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "msp1");
 	clk_register_clkdev(clk, NULL, "ux500-msp-i2s.1");
 
 	clk = clk_reg_prcc_kclk("p1_sdi0_kclk", "sdmmcclk",
-			U8500_CLKRST1_BASE, BIT(5), CLK_SET_RATE_GATE);
+			clkrst1_base, BIT(5), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "sdi0");
 
 	clk = clk_reg_prcc_kclk("p1_i2c2_kclk", "i2cclk",
-			U8500_CLKRST1_BASE, BIT(6), CLK_SET_RATE_GATE);
+			clkrst1_base, BIT(6), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "nmk-i2c.2");
 
 	clk = clk_reg_prcc_kclk("p1_slimbus0_kclk", "slimclk",
-			U8500_CLKRST1_BASE, BIT(8), CLK_SET_RATE_GATE);
+			clkrst1_base, BIT(8), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "slimbus0");
 
 	clk = clk_reg_prcc_kclk("p1_i2c4_kclk", "i2cclk",
-			U8500_CLKRST1_BASE, BIT(9), CLK_SET_RATE_GATE);
+			clkrst1_base, BIT(9), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "nmk-i2c.4");
 
 	clk = clk_reg_prcc_kclk("p1_msp3_kclk", "msp1clk",
-			U8500_CLKRST1_BASE, BIT(10), CLK_SET_RATE_GATE);
+			clkrst1_base, BIT(10), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "msp3");
 	clk_register_clkdev(clk, NULL, "ux500-msp-i2s.3");
 
 	/* Periph2 */
 	clk = clk_reg_prcc_kclk("p2_i2c3_kclk", "i2cclk",
-			U8500_CLKRST2_BASE, BIT(0), CLK_SET_RATE_GATE);
+			clkrst2_base, BIT(0), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "nmk-i2c.3");
 
 	clk = clk_reg_prcc_kclk("p2_sdi4_kclk", "sdmmcclk",
-			U8500_CLKRST2_BASE, BIT(2), CLK_SET_RATE_GATE);
+			clkrst2_base, BIT(2), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "sdi4");
 
 	clk = clk_reg_prcc_kclk("p2_msp2_kclk", "msp02clk",
-			U8500_CLKRST2_BASE, BIT(3), CLK_SET_RATE_GATE);
+			clkrst2_base, BIT(3), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "msp2");
 	clk_register_clkdev(clk, NULL, "ux500-msp-i2s.2");
 
 	clk = clk_reg_prcc_kclk("p2_sdi1_kclk", "sdmmcclk",
-			U8500_CLKRST2_BASE, BIT(4), CLK_SET_RATE_GATE);
+			clkrst2_base, BIT(4), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "sdi1");
 
 	clk = clk_reg_prcc_kclk("p2_sdi3_kclk", "sdmmcclk",
-			U8500_CLKRST2_BASE, BIT(5), CLK_SET_RATE_GATE);
+			clkrst2_base, BIT(5), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "sdi3");
 
 	/* Note that rate is received from parent. */
 	clk = clk_reg_prcc_kclk("p2_ssirx_kclk", "hsirxclk",
-			U8500_CLKRST2_BASE, BIT(6),
+			clkrst2_base, BIT(6),
 			CLK_SET_RATE_GATE|CLK_SET_RATE_PARENT);
 	clk = clk_reg_prcc_kclk("p2_ssitx_kclk", "hsitxclk",
-			U8500_CLKRST2_BASE, BIT(7),
+			clkrst2_base, BIT(7),
 			CLK_SET_RATE_GATE|CLK_SET_RATE_PARENT);
 
 	/* Periph3 */
 	clk = clk_reg_prcc_kclk("p3_ssp0_kclk", "sspclk",
-			U8500_CLKRST3_BASE, BIT(1), CLK_SET_RATE_GATE);
+			clkrst3_base, BIT(1), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "ssp0");
 
 	clk = clk_reg_prcc_kclk("p3_ssp1_kclk", "sspclk",
-			U8500_CLKRST3_BASE, BIT(2), CLK_SET_RATE_GATE);
+			clkrst3_base, BIT(2), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "ssp1");
 
 	clk = clk_reg_prcc_kclk("p3_i2c0_kclk", "i2cclk",
-			U8500_CLKRST3_BASE, BIT(3), CLK_SET_RATE_GATE);
+			clkrst3_base, BIT(3), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "nmk-i2c.0");
 
 	clk = clk_reg_prcc_kclk("p3_sdi2_kclk", "sdmmcclk",
-			U8500_CLKRST3_BASE, BIT(4), CLK_SET_RATE_GATE);
+			clkrst3_base, BIT(4), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "sdi2");
 
 	clk = clk_reg_prcc_kclk("p3_ske_kclk", "rtc32k",
-			U8500_CLKRST3_BASE, BIT(5), CLK_SET_RATE_GATE);
+			clkrst3_base, BIT(5), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "ske");
 	clk_register_clkdev(clk, NULL, "nmk-ske-keypad");
 
 	clk = clk_reg_prcc_kclk("p3_uart2_kclk", "uartclk",
-			U8500_CLKRST3_BASE, BIT(6), CLK_SET_RATE_GATE);
+			clkrst3_base, BIT(6), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "uart2");
 
 	clk = clk_reg_prcc_kclk("p3_sdi5_kclk", "sdmmcclk",
-			U8500_CLKRST3_BASE, BIT(7), CLK_SET_RATE_GATE);
+			clkrst3_base, BIT(7), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "sdi5");
 
 	/* Periph6 */
 	clk = clk_reg_prcc_kclk("p3_rng_kclk", "rngclk",
-			U8500_CLKRST6_BASE, BIT(0), CLK_SET_RATE_GATE);
+			clkrst6_base, BIT(0), CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "rng");
 }
diff --git a/drivers/clk/x86/Makefile b/drivers/clk/x86/Makefile
index f9ba4fab0ddc..04781389d0fb 100644
--- a/drivers/clk/x86/Makefile
+++ b/drivers/clk/x86/Makefile
@@ -1,2 +1,2 @@
-clk-x86-lpss-objs		:= clk-lpss.o clk-lpt.o
+clk-x86-lpss-objs		:= clk-lpt.o
 obj-$(CONFIG_X86_INTEL_LPSS)	+= clk-x86-lpss.o
diff --git a/drivers/clk/x86/clk-lpss.c b/drivers/clk/x86/clk-lpss.c
deleted file mode 100644
index b5e229f3c3d9..000000000000
--- a/drivers/clk/x86/clk-lpss.c
+++ /dev/null
@@ -1,99 +0,0 @@
-/*
- * Intel Low Power Subsystem clocks.
- *
- * Copyright (C) 2013, Intel Corporation
- * Authors: Mika Westerberg <mika.westerberg@linux.intel.com>
- *	    Heikki Krogerus <heikki.krogerus@linux.intel.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#include <linux/acpi.h>
-#include <linux/clk.h>
-#include <linux/clk-provider.h>
-#include <linux/err.h>
-#include <linux/io.h>
-#include <linux/module.h>
-
-static int clk_lpss_is_mmio_resource(struct acpi_resource *res, void *data)
-{
-	struct resource r;
-	return !acpi_dev_resource_memory(res, &r);
-}
-
-static acpi_status clk_lpss_find_mmio(acpi_handle handle, u32 level,
-				      void *data, void **retval)
-{
-	struct resource_list_entry *rentry;
-	struct list_head resource_list;
-	struct acpi_device *adev;
-	const char *uid = data;
-	int ret;
-
-	if (acpi_bus_get_device(handle, &adev))
-		return AE_OK;
-
-	if (uid) {
-		if (!adev->pnp.unique_id)
-			return AE_OK;
-		if (strcmp(uid, adev->pnp.unique_id))
-			return AE_OK;
-	}
-
-	INIT_LIST_HEAD(&resource_list);
-	ret = acpi_dev_get_resources(adev, &resource_list,
-				     clk_lpss_is_mmio_resource, NULL);
-	if (ret < 0)
-		return AE_NO_MEMORY;
-
-	list_for_each_entry(rentry, &resource_list, node)
-		if (resource_type(&rentry->res) == IORESOURCE_MEM) {
-			*(struct resource *)retval = rentry->res;
-			break;
-		}
-
-	acpi_dev_free_resource_list(&resource_list);
-	return AE_OK;
-}
-
-/**
- * clk_register_lpss_gate - register LPSS clock gate
- * @name: name of this clock gate
- * @parent_name: parent clock name
- * @hid: ACPI _HID of the device
- * @uid: ACPI _UID of the device (optional)
- * @offset: LPSS PRV_CLOCK_PARAMS offset
- *
- * Creates and registers LPSS clock gate.
- */
-struct clk *clk_register_lpss_gate(const char *name, const char *parent_name,
-				   const char *hid, const char *uid,
-				   unsigned offset)
-{
-	struct resource res = { };
-	void __iomem *mmio_base;
-	acpi_status status;
-	struct clk *clk;
-
-	/*
-	 * First try to look the device and its mmio resource from the
-	 * ACPI namespace.
-	 */
-	status = acpi_get_devices(hid, clk_lpss_find_mmio, (void *)uid,
-				  (void **)&res);
-	if (ACPI_FAILURE(status) || !res.start)
-		return ERR_PTR(-ENODEV);
-
-	mmio_base = ioremap(res.start, resource_size(&res));
-	if (!mmio_base)
-		return ERR_PTR(-ENOMEM);
-
-	clk = clk_register_gate(NULL, name, parent_name, 0, mmio_base + offset,
-				0, 0, NULL);
-	if (IS_ERR(clk))
-		iounmap(mmio_base);
-
-	return clk;
-}
diff --git a/drivers/clk/x86/clk-lpss.h b/drivers/clk/x86/clk-lpss.h
deleted file mode 100644
index e9460f442297..000000000000
--- a/drivers/clk/x86/clk-lpss.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * Intel Low Power Subsystem clock.
- *
- * Copyright (C) 2013, Intel Corporation
- * Authors: Mika Westerberg <mika.westerberg@linux.intel.com>
- *	    Heikki Krogerus <heikki.krogerus@linux.intel.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 __CLK_LPSS_H
-#define __CLK_LPSS_H
-
-#include <linux/err.h>
-#include <linux/errno.h>
-#include <linux/clk.h>
-
-#ifdef CONFIG_ACPI
-extern struct clk *clk_register_lpss_gate(const char *name,
-					  const char *parent_name,
-					  const char *hid, const char *uid,
-					  unsigned offset);
-#else
-static inline struct clk *clk_register_lpss_gate(const char *name,
-						 const char *parent_name,
-						 const char *hid,
-						 const char *uid,
-						 unsigned offset)
-{
-	return ERR_PTR(-ENODEV);
-}
-#endif
-
-#endif /* __CLK_LPSS_H */
diff --git a/drivers/clk/x86/clk-lpt.c b/drivers/clk/x86/clk-lpt.c
index 81298aeef7e3..5cf4f4686406 100644
--- a/drivers/clk/x86/clk-lpt.c
+++ b/drivers/clk/x86/clk-lpt.c
@@ -10,7 +10,6 @@
  * published by the Free Software Foundation.
  */
 
-#include <linux/acpi.h>
 #include <linux/clk.h>
 #include <linux/clkdev.h>
 #include <linux/clk-provider.h>
@@ -18,8 +17,6 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 
-#include "clk-lpss.h"
-
 #define PRV_CLOCK_PARAMS 0x800
 
 static int lpt_clk_probe(struct platform_device *pdev)
@@ -34,40 +31,6 @@ static int lpt_clk_probe(struct platform_device *pdev)
 
 	/* Shared DMA clock */
 	clk_register_clkdev(clk, "hclk", "INTL9C60.0.auto");
-
-	/* SPI clocks */
-	clk = clk_register_lpss_gate("spi0_clk", "lpss_clk", "INT33C0", NULL,
-				     PRV_CLOCK_PARAMS);
-	if (!IS_ERR(clk))
-		clk_register_clkdev(clk, NULL, "INT33C0:00");
-
-	clk = clk_register_lpss_gate("spi1_clk", "lpss_clk", "INT33C1", NULL,
-				     PRV_CLOCK_PARAMS);
-	if (!IS_ERR(clk))
-		clk_register_clkdev(clk, NULL, "INT33C1:00");
-
-	/* I2C clocks */
-	clk = clk_register_lpss_gate("i2c0_clk", "lpss_clk", "INT33C2", NULL,
-				     PRV_CLOCK_PARAMS);
-	if (!IS_ERR(clk))
-		clk_register_clkdev(clk, NULL, "INT33C2:00");
-
-	clk = clk_register_lpss_gate("i2c1_clk", "lpss_clk", "INT33C3", NULL,
-				     PRV_CLOCK_PARAMS);
-	if (!IS_ERR(clk))
-		clk_register_clkdev(clk, NULL, "INT33C3:00");
-
-	/* UART clocks */
-	clk = clk_register_lpss_gate("uart0_clk", "lpss_clk", "INT33C4", NULL,
-				     PRV_CLOCK_PARAMS);
-	if (!IS_ERR(clk))
-		clk_register_clkdev(clk, NULL, "INT33C4:00");
-
-	clk = clk_register_lpss_gate("uart1_clk", "lpss_clk", "INT33C5", NULL,
-				     PRV_CLOCK_PARAMS);
-	if (!IS_ERR(clk))
-		clk_register_clkdev(clk, NULL, "INT33C5:00");
-
 	return 0;
 }
 
@@ -79,8 +42,7 @@ static struct platform_driver lpt_clk_driver = {
 	.probe = lpt_clk_probe,
 };
 
-static int __init lpt_clk_init(void)
+int __init lpt_clk_init(void)
 {
 	return platform_driver_register(&lpt_clk_driver);
 }
-arch_initcall(lpt_clk_init);
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index e507ab7df60b..f151c6cf27c3 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -25,12 +25,15 @@ config DW_APB_TIMER_OF
 config ARMADA_370_XP_TIMER
 	bool
 
-config SUNXI_TIMER
+config SUN4I_TIMER
 	bool
 
 config VT8500_TIMER
 	bool
 
+config CADENCE_TTC_TIMER
+	bool
+
 config CLKSRC_NOMADIK_MTU
 	bool
 	depends on (ARCH_NOMADIK || ARCH_U8500)
@@ -62,8 +65,23 @@ config CLKSRC_DBX500_PRCMU_SCHED_CLOCK
 
 config ARM_ARCH_TIMER
 	bool
+	select CLKSRC_OF if OF
 
 config CLKSRC_METAG_GENERIC
 	def_bool y if METAG
 	help
 	  This option enables support for the Meta per-thread timers.
+
+config CLKSRC_EXYNOS_MCT
+	def_bool y if ARCH_EXYNOS
+	help
+	  Support for Multi Core Timer controller on Exynos SoCs.
+
+config CLKSRC_SAMSUNG_PWM
+	bool
+	select CLKSRC_MMIO
+	help
+	  This is a new clocksource driver for the PWM timer found in
+	  Samsung S3C, S5P and Exynos SoCs, replacing an earlier driver
+	  for all devicetree enabled platforms. This driver will be
+	  needed only on systems that do not have the Exynos MCT available.
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index 96e25319659b..8d979c72aa94 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -16,10 +16,16 @@ obj-$(CONFIG_CLKSRC_NOMADIK_MTU)	+= nomadik-mtu.o
 obj-$(CONFIG_CLKSRC_DBX500_PRCMU)	+= clksrc-dbx500-prcmu.o
 obj-$(CONFIG_ARMADA_370_XP_TIMER)	+= time-armada-370-xp.o
 obj-$(CONFIG_ARCH_BCM2835)	+= bcm2835_timer.o
-obj-$(CONFIG_SUNXI_TIMER)	+= sunxi_timer.o
+obj-$(CONFIG_ARCH_MARCO)	+= timer-marco.o
+obj-$(CONFIG_ARCH_MXS)		+= mxs_timer.o
+obj-$(CONFIG_ARCH_PRIMA2)	+= timer-prima2.o
+obj-$(CONFIG_SUN4I_TIMER)	+= sun4i_timer.o
 obj-$(CONFIG_ARCH_TEGRA)	+= tegra20_timer.o
 obj-$(CONFIG_VT8500_TIMER)	+= vt8500_timer.o
 obj-$(CONFIG_ARCH_BCM)		+= bcm_kona_timer.o
+obj-$(CONFIG_CADENCE_TTC_TIMER)	+= cadence_ttc_timer.o
+obj-$(CONFIG_CLKSRC_EXYNOS_MCT)	+= exynos_mct.o
+obj-$(CONFIG_CLKSRC_SAMSUNG_PWM)	+= samsung_pwm_timer.o
 
 obj-$(CONFIG_ARM_ARCH_TIMER)		+= arm_arch_timer.o
 obj-$(CONFIG_CLKSRC_METAG_GENERIC)	+= metag_generic.o
diff --git a/drivers/clocksource/arm_arch_timer.c b/drivers/clocksource/arm_arch_timer.c
index d7ad425ab9b3..a2b254189782 100644
--- a/drivers/clocksource/arm_arch_timer.c
+++ b/drivers/clocksource/arm_arch_timer.c
@@ -248,14 +248,16 @@ static void __cpuinit arch_timer_stop(struct clock_event_device *clk)
 static int __cpuinit arch_timer_cpu_notify(struct notifier_block *self,
 					   unsigned long action, void *hcpu)
 {
-	struct clock_event_device *evt = this_cpu_ptr(arch_timer_evt);
-
+	/*
+	 * Grab cpu pointer in each case to avoid spurious
+	 * preemptible warnings
+	 */
 	switch (action & ~CPU_TASKS_FROZEN) {
 	case CPU_STARTING:
-		arch_timer_setup(evt);
+		arch_timer_setup(this_cpu_ptr(arch_timer_evt));
 		break;
 	case CPU_DYING:
-		arch_timer_stop(evt);
+		arch_timer_stop(this_cpu_ptr(arch_timer_evt));
 		break;
 	}
 
@@ -337,22 +339,14 @@ out:
 	return err;
 }
 
-static const struct of_device_id arch_timer_of_match[] __initconst = {
-	{ .compatible	= "arm,armv7-timer",	},
-	{ .compatible	= "arm,armv8-timer",	},
-	{},
-};
-
-int __init arch_timer_init(void)
+static void __init arch_timer_init(struct device_node *np)
 {
-	struct device_node *np;
 	u32 freq;
 	int i;
 
-	np = of_find_matching_node(NULL, arch_timer_of_match);
-	if (!np) {
-		pr_err("arch_timer: can't find DT node\n");
-		return -ENODEV;
+	if (arch_timer_get_rate()) {
+		pr_warn("arch_timer: multiple nodes in dt, skipping\n");
+		return;
 	}
 
 	/* Try to determine the frequency from the device tree or CNTFRQ */
@@ -378,7 +372,7 @@ int __init arch_timer_init(void)
 		if (!arch_timer_ppi[PHYS_SECURE_PPI] ||
 		    !arch_timer_ppi[PHYS_NONSECURE_PPI]) {
 			pr_warn("arch_timer: No interrupt available, giving up\n");
-			return -EINVAL;
+			return;
 		}
 	}
 
@@ -387,5 +381,8 @@ int __init arch_timer_init(void)
 	else
 		arch_timer_read_counter = arch_counter_get_cntpct;
 
-	return arch_timer_register();
+	arch_timer_register();
+	arch_timer_arch_init();
 }
+CLOCKSOURCE_OF_DECLARE(armv7_arch_timer, "arm,armv7-timer", arch_timer_init);
+CLOCKSOURCE_OF_DECLARE(armv8_arch_timer, "arm,armv8-timer", arch_timer_init);
diff --git a/drivers/clocksource/bcm2835_timer.c b/drivers/clocksource/bcm2835_timer.c
index 50c68fef944b..766611d29945 100644
--- a/drivers/clocksource/bcm2835_timer.c
+++ b/drivers/clocksource/bcm2835_timer.c
@@ -95,23 +95,13 @@ static irqreturn_t bcm2835_time_interrupt(int irq, void *dev_id)
 	}
 }
 
-static struct of_device_id bcm2835_time_match[] __initconst = {
-	{ .compatible = "brcm,bcm2835-system-timer" },
-	{}
-};
-
-static void __init bcm2835_timer_init(void)
+static void __init bcm2835_timer_init(struct device_node *node)
 {
-	struct device_node *node;
 	void __iomem *base;
 	u32 freq;
 	int irq;
 	struct bcm2835_timer *timer;
 
-	node = of_find_matching_node(NULL, bcm2835_time_match);
-	if (!node)
-		panic("No bcm2835 timer node");
-
 	base = of_iomap(node, 0);
 	if (!base)
 		panic("Can't remap registers");
diff --git a/drivers/clocksource/cadence_ttc_timer.c b/drivers/clocksource/cadence_ttc_timer.c
new file mode 100644
index 000000000000..685bc60e210a
--- /dev/null
+++ b/drivers/clocksource/cadence_ttc_timer.c
@@ -0,0 +1,436 @@
+/*
+ * This file contains driver for the Cadence Triple Timer Counter Rev 06
+ *
+ *  Copyright (C) 2011-2013 Xilinx
+ *
+ * based on arch/mips/kernel/time.c timer driver
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ */
+
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/clockchips.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/slab.h>
+#include <linux/clk-provider.h>
+
+/*
+ * This driver configures the 2 16-bit count-up timers as follows:
+ *
+ * T1: Timer 1, clocksource for generic timekeeping
+ * T2: Timer 2, clockevent source for hrtimers
+ * T3: Timer 3, <unused>
+ *
+ * The input frequency to the timer module for emulation is 2.5MHz which is
+ * common to all the timer channels (T1, T2, and T3). With a pre-scaler of 32,
+ * the timers are clocked at 78.125KHz (12.8 us resolution).
+
+ * The input frequency to the timer module in silicon is configurable and
+ * obtained from device tree. The pre-scaler of 32 is used.
+ */
+
+/*
+ * Timer Register Offset Definitions of Timer 1, Increment base address by 4
+ * and use same offsets for Timer 2
+ */
+#define TTC_CLK_CNTRL_OFFSET		0x00 /* Clock Control Reg, RW */
+#define TTC_CNT_CNTRL_OFFSET		0x0C /* Counter Control Reg, RW */
+#define TTC_COUNT_VAL_OFFSET		0x18 /* Counter Value Reg, RO */
+#define TTC_INTR_VAL_OFFSET		0x24 /* Interval Count Reg, RW */
+#define TTC_ISR_OFFSET		0x54 /* Interrupt Status Reg, RO */
+#define TTC_IER_OFFSET		0x60 /* Interrupt Enable Reg, RW */
+
+#define TTC_CNT_CNTRL_DISABLE_MASK	0x1
+
+/*
+ * Setup the timers to use pre-scaling, using a fixed value for now that will
+ * work across most input frequency, but it may need to be more dynamic
+ */
+#define PRESCALE_EXPONENT	11	/* 2 ^ PRESCALE_EXPONENT = PRESCALE */
+#define PRESCALE		2048	/* The exponent must match this */
+#define CLK_CNTRL_PRESCALE	((PRESCALE_EXPONENT - 1) << 1)
+#define CLK_CNTRL_PRESCALE_EN	1
+#define CNT_CNTRL_RESET		(1 << 4)
+
+/**
+ * struct ttc_timer - This definition defines local timer structure
+ *
+ * @base_addr:	Base address of timer
+ * @clk:	Associated clock source
+ * @clk_rate_change_nb	Notifier block for clock rate changes
+ */
+struct ttc_timer {
+	void __iomem *base_addr;
+	struct clk *clk;
+	struct notifier_block clk_rate_change_nb;
+};
+
+#define to_ttc_timer(x) \
+		container_of(x, struct ttc_timer, clk_rate_change_nb)
+
+struct ttc_timer_clocksource {
+	struct ttc_timer	ttc;
+	struct clocksource	cs;
+};
+
+#define to_ttc_timer_clksrc(x) \
+		container_of(x, struct ttc_timer_clocksource, cs)
+
+struct ttc_timer_clockevent {
+	struct ttc_timer		ttc;
+	struct clock_event_device	ce;
+};
+
+#define to_ttc_timer_clkevent(x) \
+		container_of(x, struct ttc_timer_clockevent, ce)
+
+/**
+ * ttc_set_interval - Set the timer interval value
+ *
+ * @timer:	Pointer to the timer instance
+ * @cycles:	Timer interval ticks
+ **/
+static void ttc_set_interval(struct ttc_timer *timer,
+					unsigned long cycles)
+{
+	u32 ctrl_reg;
+
+	/* Disable the counter, set the counter value  and re-enable counter */
+	ctrl_reg = __raw_readl(timer->base_addr + TTC_CNT_CNTRL_OFFSET);
+	ctrl_reg |= TTC_CNT_CNTRL_DISABLE_MASK;
+	__raw_writel(ctrl_reg, timer->base_addr + TTC_CNT_CNTRL_OFFSET);
+
+	__raw_writel(cycles, timer->base_addr + TTC_INTR_VAL_OFFSET);
+
+	/*
+	 * Reset the counter (0x10) so that it starts from 0, one-shot
+	 * mode makes this needed for timing to be right.
+	 */
+	ctrl_reg |= CNT_CNTRL_RESET;
+	ctrl_reg &= ~TTC_CNT_CNTRL_DISABLE_MASK;
+	__raw_writel(ctrl_reg, timer->base_addr + TTC_CNT_CNTRL_OFFSET);
+}
+
+/**
+ * ttc_clock_event_interrupt - Clock event timer interrupt handler
+ *
+ * @irq:	IRQ number of the Timer
+ * @dev_id:	void pointer to the ttc_timer instance
+ *
+ * returns: Always IRQ_HANDLED - success
+ **/
+static irqreturn_t ttc_clock_event_interrupt(int irq, void *dev_id)
+{
+	struct ttc_timer_clockevent *ttce = dev_id;
+	struct ttc_timer *timer = &ttce->ttc;
+
+	/* Acknowledge the interrupt and call event handler */
+	__raw_readl(timer->base_addr + TTC_ISR_OFFSET);
+
+	ttce->ce.event_handler(&ttce->ce);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * __ttc_clocksource_read - Reads the timer counter register
+ *
+ * returns: Current timer counter register value
+ **/
+static cycle_t __ttc_clocksource_read(struct clocksource *cs)
+{
+	struct ttc_timer *timer = &to_ttc_timer_clksrc(cs)->ttc;
+
+	return (cycle_t)__raw_readl(timer->base_addr +
+				TTC_COUNT_VAL_OFFSET);
+}
+
+/**
+ * ttc_set_next_event - Sets the time interval for next event
+ *
+ * @cycles:	Timer interval ticks
+ * @evt:	Address of clock event instance
+ *
+ * returns: Always 0 - success
+ **/
+static int ttc_set_next_event(unsigned long cycles,
+					struct clock_event_device *evt)
+{
+	struct ttc_timer_clockevent *ttce = to_ttc_timer_clkevent(evt);
+	struct ttc_timer *timer = &ttce->ttc;
+
+	ttc_set_interval(timer, cycles);
+	return 0;
+}
+
+/**
+ * ttc_set_mode - Sets the mode of timer
+ *
+ * @mode:	Mode to be set
+ * @evt:	Address of clock event instance
+ **/
+static void ttc_set_mode(enum clock_event_mode mode,
+					struct clock_event_device *evt)
+{
+	struct ttc_timer_clockevent *ttce = to_ttc_timer_clkevent(evt);
+	struct ttc_timer *timer = &ttce->ttc;
+	u32 ctrl_reg;
+
+	switch (mode) {
+	case CLOCK_EVT_MODE_PERIODIC:
+		ttc_set_interval(timer,
+				DIV_ROUND_CLOSEST(clk_get_rate(ttce->ttc.clk),
+					PRESCALE * HZ));
+		break;
+	case CLOCK_EVT_MODE_ONESHOT:
+	case CLOCK_EVT_MODE_UNUSED:
+	case CLOCK_EVT_MODE_SHUTDOWN:
+		ctrl_reg = __raw_readl(timer->base_addr +
+					TTC_CNT_CNTRL_OFFSET);
+		ctrl_reg |= TTC_CNT_CNTRL_DISABLE_MASK;
+		__raw_writel(ctrl_reg,
+				timer->base_addr + TTC_CNT_CNTRL_OFFSET);
+		break;
+	case CLOCK_EVT_MODE_RESUME:
+		ctrl_reg = __raw_readl(timer->base_addr +
+					TTC_CNT_CNTRL_OFFSET);
+		ctrl_reg &= ~TTC_CNT_CNTRL_DISABLE_MASK;
+		__raw_writel(ctrl_reg,
+				timer->base_addr + TTC_CNT_CNTRL_OFFSET);
+		break;
+	}
+}
+
+static int ttc_rate_change_clocksource_cb(struct notifier_block *nb,
+		unsigned long event, void *data)
+{
+	struct clk_notifier_data *ndata = data;
+	struct ttc_timer *ttc = to_ttc_timer(nb);
+	struct ttc_timer_clocksource *ttccs = container_of(ttc,
+			struct ttc_timer_clocksource, ttc);
+
+	switch (event) {
+	case POST_RATE_CHANGE:
+		/*
+		 * Do whatever is necessary to maintain a proper time base
+		 *
+		 * I cannot find a way to adjust the currently used clocksource
+		 * to the new frequency. __clocksource_updatefreq_hz() sounds
+		 * good, but does not work. Not sure what's that missing.
+		 *
+		 * This approach works, but triggers two clocksource switches.
+		 * The first after unregister to clocksource jiffies. And
+		 * another one after the register to the newly registered timer.
+		 *
+		 * Alternatively we could 'waste' another HW timer to ping pong
+		 * between clock sources. That would also use one register and
+		 * one unregister call, but only trigger one clocksource switch
+		 * for the cost of another HW timer used by the OS.
+		 */
+		clocksource_unregister(&ttccs->cs);
+		clocksource_register_hz(&ttccs->cs,
+				ndata->new_rate / PRESCALE);
+		/* fall through */
+	case PRE_RATE_CHANGE:
+	case ABORT_RATE_CHANGE:
+	default:
+		return NOTIFY_DONE;
+	}
+}
+
+static void __init ttc_setup_clocksource(struct clk *clk, void __iomem *base)
+{
+	struct ttc_timer_clocksource *ttccs;
+	int err;
+
+	ttccs = kzalloc(sizeof(*ttccs), GFP_KERNEL);
+	if (WARN_ON(!ttccs))
+		return;
+
+	ttccs->ttc.clk = clk;
+
+	err = clk_prepare_enable(ttccs->ttc.clk);
+	if (WARN_ON(err)) {
+		kfree(ttccs);
+		return;
+	}
+
+	ttccs->ttc.clk_rate_change_nb.notifier_call =
+		ttc_rate_change_clocksource_cb;
+	ttccs->ttc.clk_rate_change_nb.next = NULL;
+	if (clk_notifier_register(ttccs->ttc.clk,
+				&ttccs->ttc.clk_rate_change_nb))
+		pr_warn("Unable to register clock notifier.\n");
+
+	ttccs->ttc.base_addr = base;
+	ttccs->cs.name = "ttc_clocksource";
+	ttccs->cs.rating = 200;
+	ttccs->cs.read = __ttc_clocksource_read;
+	ttccs->cs.mask = CLOCKSOURCE_MASK(16);
+	ttccs->cs.flags = CLOCK_SOURCE_IS_CONTINUOUS;
+
+	/*
+	 * Setup the clock source counter to be an incrementing counter
+	 * with no interrupt and it rolls over at 0xFFFF. Pre-scale
+	 * it by 32 also. Let it start running now.
+	 */
+	__raw_writel(0x0,  ttccs->ttc.base_addr + TTC_IER_OFFSET);
+	__raw_writel(CLK_CNTRL_PRESCALE | CLK_CNTRL_PRESCALE_EN,
+		     ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
+	__raw_writel(CNT_CNTRL_RESET,
+		     ttccs->ttc.base_addr + TTC_CNT_CNTRL_OFFSET);
+
+	err = clocksource_register_hz(&ttccs->cs,
+			clk_get_rate(ttccs->ttc.clk) / PRESCALE);
+	if (WARN_ON(err)) {
+		kfree(ttccs);
+		return;
+	}
+}
+
+static int ttc_rate_change_clockevent_cb(struct notifier_block *nb,
+		unsigned long event, void *data)
+{
+	struct clk_notifier_data *ndata = data;
+	struct ttc_timer *ttc = to_ttc_timer(nb);
+	struct ttc_timer_clockevent *ttcce = container_of(ttc,
+			struct ttc_timer_clockevent, ttc);
+
+	switch (event) {
+	case POST_RATE_CHANGE:
+	{
+		unsigned long flags;
+
+		/*
+		 * clockevents_update_freq should be called with IRQ disabled on
+		 * the CPU the timer provides events for. The timer we use is
+		 * common to both CPUs, not sure if we need to run on both
+		 * cores.
+		 */
+		local_irq_save(flags);
+		clockevents_update_freq(&ttcce->ce,
+				ndata->new_rate / PRESCALE);
+		local_irq_restore(flags);
+
+		/* fall through */
+	}
+	case PRE_RATE_CHANGE:
+	case ABORT_RATE_CHANGE:
+	default:
+		return NOTIFY_DONE;
+	}
+}
+
+static void __init ttc_setup_clockevent(struct clk *clk,
+						void __iomem *base, u32 irq)
+{
+	struct ttc_timer_clockevent *ttcce;
+	int err;
+
+	ttcce = kzalloc(sizeof(*ttcce), GFP_KERNEL);
+	if (WARN_ON(!ttcce))
+		return;
+
+	ttcce->ttc.clk = clk;
+
+	err = clk_prepare_enable(ttcce->ttc.clk);
+	if (WARN_ON(err)) {
+		kfree(ttcce);
+		return;
+	}
+
+	ttcce->ttc.clk_rate_change_nb.notifier_call =
+		ttc_rate_change_clockevent_cb;
+	ttcce->ttc.clk_rate_change_nb.next = NULL;
+	if (clk_notifier_register(ttcce->ttc.clk,
+				&ttcce->ttc.clk_rate_change_nb))
+		pr_warn("Unable to register clock notifier.\n");
+
+	ttcce->ttc.base_addr = base;
+	ttcce->ce.name = "ttc_clockevent";
+	ttcce->ce.features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT;
+	ttcce->ce.set_next_event = ttc_set_next_event;
+	ttcce->ce.set_mode = ttc_set_mode;
+	ttcce->ce.rating = 200;
+	ttcce->ce.irq = irq;
+	ttcce->ce.cpumask = cpu_possible_mask;
+
+	/*
+	 * Setup the clock event timer to be an interval timer which
+	 * is prescaled by 32 using the interval interrupt. Leave it
+	 * disabled for now.
+	 */
+	__raw_writel(0x23, ttcce->ttc.base_addr + TTC_CNT_CNTRL_OFFSET);
+	__raw_writel(CLK_CNTRL_PRESCALE | CLK_CNTRL_PRESCALE_EN,
+		     ttcce->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
+	__raw_writel(0x1,  ttcce->ttc.base_addr + TTC_IER_OFFSET);
+
+	err = request_irq(irq, ttc_clock_event_interrupt,
+			  IRQF_DISABLED | IRQF_TIMER,
+			  ttcce->ce.name, ttcce);
+	if (WARN_ON(err)) {
+		kfree(ttcce);
+		return;
+	}
+
+	clockevents_config_and_register(&ttcce->ce,
+			clk_get_rate(ttcce->ttc.clk) / PRESCALE, 1, 0xfffe);
+}
+
+/**
+ * ttc_timer_init - Initialize the timer
+ *
+ * Initializes the timer hardware and register the clock source and clock event
+ * timers with Linux kernal timer framework
+ */
+static void __init ttc_timer_init(struct device_node *timer)
+{
+	unsigned int irq;
+	void __iomem *timer_baseaddr;
+	struct clk *clk;
+	static int initialized;
+
+	if (initialized)
+		return;
+
+	initialized = 1;
+
+	/*
+	 * Get the 1st Triple Timer Counter (TTC) block from the device tree
+	 * and use it. Note that the event timer uses the interrupt and it's the
+	 * 2nd TTC hence the irq_of_parse_and_map(,1)
+	 */
+	timer_baseaddr = of_iomap(timer, 0);
+	if (!timer_baseaddr) {
+		pr_err("ERROR: invalid timer base address\n");
+		BUG();
+	}
+
+	irq = irq_of_parse_and_map(timer, 1);
+	if (irq <= 0) {
+		pr_err("ERROR: invalid interrupt number\n");
+		BUG();
+	}
+
+	clk = of_clk_get_by_name(timer, "cpu_1x");
+	if (IS_ERR(clk)) {
+		pr_err("ERROR: timer input clock not found\n");
+		BUG();
+	}
+
+	ttc_setup_clocksource(clk, timer_baseaddr);
+	ttc_setup_clockevent(clk, timer_baseaddr + 4, irq);
+
+	pr_info("%s #0 at %p, irq=%d\n", timer->name, timer_baseaddr, irq);
+}
+
+CLOCKSOURCE_OF_DECLARE(ttc, "cdns,ttc", ttc_timer_init);
diff --git a/drivers/clocksource/clksrc-dbx500-prcmu.c b/drivers/clocksource/clksrc-dbx500-prcmu.c
index c26c369eb9e6..54f3d119d99c 100644
--- a/drivers/clocksource/clksrc-dbx500-prcmu.c
+++ b/drivers/clocksource/clksrc-dbx500-prcmu.c
@@ -17,9 +17,6 @@
 
 #include <asm/sched_clock.h>
 
-#include <mach/setup.h>
-#include <mach/hardware.h>
-
 #define RATE_32K		32768
 
 #define TIMER_MODE_CONTINOUS	0x1
diff --git a/drivers/clocksource/clksrc-of.c b/drivers/clocksource/clksrc-of.c
index bdabdaa8d00f..37f5325bec95 100644
--- a/drivers/clocksource/clksrc-of.c
+++ b/drivers/clocksource/clksrc-of.c
@@ -16,6 +16,7 @@
 
 #include <linux/init.h>
 #include <linux/of.h>
+#include <linux/clocksource.h>
 
 extern struct of_device_id __clksrc_of_table[];
 
@@ -26,10 +27,10 @@ void __init clocksource_of_init(void)
 {
 	struct device_node *np;
 	const struct of_device_id *match;
-	void (*init_func)(void);
+	clocksource_of_init_fn init_func;
 
 	for_each_matching_node_and_match(np, __clksrc_of_table, &match) {
 		init_func = match->data;
-		init_func();
+		init_func(np);
 	}
 }
diff --git a/drivers/clocksource/em_sti.c b/drivers/clocksource/em_sti.c
index e6a553cb73e8..4329a29a5310 100644
--- a/drivers/clocksource/em_sti.c
+++ b/drivers/clocksource/em_sti.c
@@ -399,7 +399,18 @@ static struct platform_driver em_sti_device_driver = {
 	}
 };
 
-module_platform_driver(em_sti_device_driver);
+static int __init em_sti_init(void)
+{
+	return platform_driver_register(&em_sti_device_driver);
+}
+
+static void __exit em_sti_exit(void)
+{
+	platform_driver_unregister(&em_sti_device_driver);
+}
+
+subsys_initcall(em_sti_init);
+module_exit(em_sti_exit);
 
 MODULE_AUTHOR("Magnus Damm");
 MODULE_DESCRIPTION("Renesas Emma Mobile STI Timer Driver");
diff --git a/drivers/clocksource/exynos_mct.c b/drivers/clocksource/exynos_mct.c
new file mode 100644
index 000000000000..662fcc065821
--- /dev/null
+++ b/drivers/clocksource/exynos_mct.c
@@ -0,0 +1,558 @@
+/* linux/arch/arm/mach-exynos4/mct.c
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * EXYNOS4 MCT(Multi-Core Timer) support
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#include <linux/sched.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/clockchips.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/percpu.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/clocksource.h>
+
+#include <asm/localtimer.h>
+#include <asm/mach/time.h>
+
+#define EXYNOS4_MCTREG(x)		(x)
+#define EXYNOS4_MCT_G_CNT_L		EXYNOS4_MCTREG(0x100)
+#define EXYNOS4_MCT_G_CNT_U		EXYNOS4_MCTREG(0x104)
+#define EXYNOS4_MCT_G_CNT_WSTAT		EXYNOS4_MCTREG(0x110)
+#define EXYNOS4_MCT_G_COMP0_L		EXYNOS4_MCTREG(0x200)
+#define EXYNOS4_MCT_G_COMP0_U		EXYNOS4_MCTREG(0x204)
+#define EXYNOS4_MCT_G_COMP0_ADD_INCR	EXYNOS4_MCTREG(0x208)
+#define EXYNOS4_MCT_G_TCON		EXYNOS4_MCTREG(0x240)
+#define EXYNOS4_MCT_G_INT_CSTAT		EXYNOS4_MCTREG(0x244)
+#define EXYNOS4_MCT_G_INT_ENB		EXYNOS4_MCTREG(0x248)
+#define EXYNOS4_MCT_G_WSTAT		EXYNOS4_MCTREG(0x24C)
+#define _EXYNOS4_MCT_L_BASE		EXYNOS4_MCTREG(0x300)
+#define EXYNOS4_MCT_L_BASE(x)		(_EXYNOS4_MCT_L_BASE + (0x100 * x))
+#define EXYNOS4_MCT_L_MASK		(0xffffff00)
+
+#define MCT_L_TCNTB_OFFSET		(0x00)
+#define MCT_L_ICNTB_OFFSET		(0x08)
+#define MCT_L_TCON_OFFSET		(0x20)
+#define MCT_L_INT_CSTAT_OFFSET		(0x30)
+#define MCT_L_INT_ENB_OFFSET		(0x34)
+#define MCT_L_WSTAT_OFFSET		(0x40)
+#define MCT_G_TCON_START		(1 << 8)
+#define MCT_G_TCON_COMP0_AUTO_INC	(1 << 1)
+#define MCT_G_TCON_COMP0_ENABLE		(1 << 0)
+#define MCT_L_TCON_INTERVAL_MODE	(1 << 2)
+#define MCT_L_TCON_INT_START		(1 << 1)
+#define MCT_L_TCON_TIMER_START		(1 << 0)
+
+#define TICK_BASE_CNT	1
+
+enum {
+	MCT_INT_SPI,
+	MCT_INT_PPI
+};
+
+enum {
+	MCT_G0_IRQ,
+	MCT_G1_IRQ,
+	MCT_G2_IRQ,
+	MCT_G3_IRQ,
+	MCT_L0_IRQ,
+	MCT_L1_IRQ,
+	MCT_L2_IRQ,
+	MCT_L3_IRQ,
+	MCT_NR_IRQS,
+};
+
+static void __iomem *reg_base;
+static unsigned long clk_rate;
+static unsigned int mct_int_type;
+static int mct_irqs[MCT_NR_IRQS];
+
+struct mct_clock_event_device {
+	struct clock_event_device *evt;
+	unsigned long base;
+	char name[10];
+};
+
+static void exynos4_mct_write(unsigned int value, unsigned long offset)
+{
+	unsigned long stat_addr;
+	u32 mask;
+	u32 i;
+
+	__raw_writel(value, reg_base + offset);
+
+	if (likely(offset >= EXYNOS4_MCT_L_BASE(0))) {
+		stat_addr = (offset & ~EXYNOS4_MCT_L_MASK) + MCT_L_WSTAT_OFFSET;
+		switch (offset & EXYNOS4_MCT_L_MASK) {
+		case MCT_L_TCON_OFFSET:
+			mask = 1 << 3;		/* L_TCON write status */
+			break;
+		case MCT_L_ICNTB_OFFSET:
+			mask = 1 << 1;		/* L_ICNTB write status */
+			break;
+		case MCT_L_TCNTB_OFFSET:
+			mask = 1 << 0;		/* L_TCNTB write status */
+			break;
+		default:
+			return;
+		}
+	} else {
+		switch (offset) {
+		case EXYNOS4_MCT_G_TCON:
+			stat_addr = EXYNOS4_MCT_G_WSTAT;
+			mask = 1 << 16;		/* G_TCON write status */
+			break;
+		case EXYNOS4_MCT_G_COMP0_L:
+			stat_addr = EXYNOS4_MCT_G_WSTAT;
+			mask = 1 << 0;		/* G_COMP0_L write status */
+			break;
+		case EXYNOS4_MCT_G_COMP0_U:
+			stat_addr = EXYNOS4_MCT_G_WSTAT;
+			mask = 1 << 1;		/* G_COMP0_U write status */
+			break;
+		case EXYNOS4_MCT_G_COMP0_ADD_INCR:
+			stat_addr = EXYNOS4_MCT_G_WSTAT;
+			mask = 1 << 2;		/* G_COMP0_ADD_INCR w status */
+			break;
+		case EXYNOS4_MCT_G_CNT_L:
+			stat_addr = EXYNOS4_MCT_G_CNT_WSTAT;
+			mask = 1 << 0;		/* G_CNT_L write status */
+			break;
+		case EXYNOS4_MCT_G_CNT_U:
+			stat_addr = EXYNOS4_MCT_G_CNT_WSTAT;
+			mask = 1 << 1;		/* G_CNT_U write status */
+			break;
+		default:
+			return;
+		}
+	}
+
+	/* Wait maximum 1 ms until written values are applied */
+	for (i = 0; i < loops_per_jiffy / 1000 * HZ; i++)
+		if (__raw_readl(reg_base + stat_addr) & mask) {
+			__raw_writel(mask, reg_base + stat_addr);
+			return;
+		}
+
+	panic("MCT hangs after writing %d (offset:0x%lx)\n", value, offset);
+}
+
+/* Clocksource handling */
+static void exynos4_mct_frc_start(u32 hi, u32 lo)
+{
+	u32 reg;
+
+	exynos4_mct_write(lo, EXYNOS4_MCT_G_CNT_L);
+	exynos4_mct_write(hi, EXYNOS4_MCT_G_CNT_U);
+
+	reg = __raw_readl(reg_base + EXYNOS4_MCT_G_TCON);
+	reg |= MCT_G_TCON_START;
+	exynos4_mct_write(reg, EXYNOS4_MCT_G_TCON);
+}
+
+static cycle_t exynos4_frc_read(struct clocksource *cs)
+{
+	unsigned int lo, hi;
+	u32 hi2 = __raw_readl(reg_base + EXYNOS4_MCT_G_CNT_U);
+
+	do {
+		hi = hi2;
+		lo = __raw_readl(reg_base + EXYNOS4_MCT_G_CNT_L);
+		hi2 = __raw_readl(reg_base + EXYNOS4_MCT_G_CNT_U);
+	} while (hi != hi2);
+
+	return ((cycle_t)hi << 32) | lo;
+}
+
+static void exynos4_frc_resume(struct clocksource *cs)
+{
+	exynos4_mct_frc_start(0, 0);
+}
+
+struct clocksource mct_frc = {
+	.name		= "mct-frc",
+	.rating		= 400,
+	.read		= exynos4_frc_read,
+	.mask		= CLOCKSOURCE_MASK(64),
+	.flags		= CLOCK_SOURCE_IS_CONTINUOUS,
+	.resume		= exynos4_frc_resume,
+};
+
+static void __init exynos4_clocksource_init(void)
+{
+	exynos4_mct_frc_start(0, 0);
+
+	if (clocksource_register_hz(&mct_frc, clk_rate))
+		panic("%s: can't register clocksource\n", mct_frc.name);
+}
+
+static void exynos4_mct_comp0_stop(void)
+{
+	unsigned int tcon;
+
+	tcon = __raw_readl(reg_base + EXYNOS4_MCT_G_TCON);
+	tcon &= ~(MCT_G_TCON_COMP0_ENABLE | MCT_G_TCON_COMP0_AUTO_INC);
+
+	exynos4_mct_write(tcon, EXYNOS4_MCT_G_TCON);
+	exynos4_mct_write(0, EXYNOS4_MCT_G_INT_ENB);
+}
+
+static void exynos4_mct_comp0_start(enum clock_event_mode mode,
+				    unsigned long cycles)
+{
+	unsigned int tcon;
+	cycle_t comp_cycle;
+
+	tcon = __raw_readl(reg_base + EXYNOS4_MCT_G_TCON);
+
+	if (mode == CLOCK_EVT_MODE_PERIODIC) {
+		tcon |= MCT_G_TCON_COMP0_AUTO_INC;
+		exynos4_mct_write(cycles, EXYNOS4_MCT_G_COMP0_ADD_INCR);
+	}
+
+	comp_cycle = exynos4_frc_read(&mct_frc) + cycles;
+	exynos4_mct_write((u32)comp_cycle, EXYNOS4_MCT_G_COMP0_L);
+	exynos4_mct_write((u32)(comp_cycle >> 32), EXYNOS4_MCT_G_COMP0_U);
+
+	exynos4_mct_write(0x1, EXYNOS4_MCT_G_INT_ENB);
+
+	tcon |= MCT_G_TCON_COMP0_ENABLE;
+	exynos4_mct_write(tcon , EXYNOS4_MCT_G_TCON);
+}
+
+static int exynos4_comp_set_next_event(unsigned long cycles,
+				       struct clock_event_device *evt)
+{
+	exynos4_mct_comp0_start(evt->mode, cycles);
+
+	return 0;
+}
+
+static void exynos4_comp_set_mode(enum clock_event_mode mode,
+				  struct clock_event_device *evt)
+{
+	unsigned long cycles_per_jiffy;
+	exynos4_mct_comp0_stop();
+
+	switch (mode) {
+	case CLOCK_EVT_MODE_PERIODIC:
+		cycles_per_jiffy =
+			(((unsigned long long) NSEC_PER_SEC / HZ * evt->mult) >> evt->shift);
+		exynos4_mct_comp0_start(mode, cycles_per_jiffy);
+		break;
+
+	case CLOCK_EVT_MODE_ONESHOT:
+	case CLOCK_EVT_MODE_UNUSED:
+	case CLOCK_EVT_MODE_SHUTDOWN:
+	case CLOCK_EVT_MODE_RESUME:
+		break;
+	}
+}
+
+static struct clock_event_device mct_comp_device = {
+	.name		= "mct-comp",
+	.features       = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT,
+	.rating		= 250,
+	.set_next_event	= exynos4_comp_set_next_event,
+	.set_mode	= exynos4_comp_set_mode,
+};
+
+static irqreturn_t exynos4_mct_comp_isr(int irq, void *dev_id)
+{
+	struct clock_event_device *evt = dev_id;
+
+	exynos4_mct_write(0x1, EXYNOS4_MCT_G_INT_CSTAT);
+
+	evt->event_handler(evt);
+
+	return IRQ_HANDLED;
+}
+
+static struct irqaction mct_comp_event_irq = {
+	.name		= "mct_comp_irq",
+	.flags		= IRQF_TIMER | IRQF_IRQPOLL,
+	.handler	= exynos4_mct_comp_isr,
+	.dev_id		= &mct_comp_device,
+};
+
+static void exynos4_clockevent_init(void)
+{
+	mct_comp_device.cpumask = cpumask_of(0);
+	clockevents_config_and_register(&mct_comp_device, clk_rate,
+					0xf, 0xffffffff);
+	setup_irq(mct_irqs[MCT_G0_IRQ], &mct_comp_event_irq);
+}
+
+#ifdef CONFIG_LOCAL_TIMERS
+
+static DEFINE_PER_CPU(struct mct_clock_event_device, percpu_mct_tick);
+
+/* Clock event handling */
+static void exynos4_mct_tick_stop(struct mct_clock_event_device *mevt)
+{
+	unsigned long tmp;
+	unsigned long mask = MCT_L_TCON_INT_START | MCT_L_TCON_TIMER_START;
+	unsigned long offset = mevt->base + MCT_L_TCON_OFFSET;
+
+	tmp = __raw_readl(reg_base + offset);
+	if (tmp & mask) {
+		tmp &= ~mask;
+		exynos4_mct_write(tmp, offset);
+	}
+}
+
+static void exynos4_mct_tick_start(unsigned long cycles,
+				   struct mct_clock_event_device *mevt)
+{
+	unsigned long tmp;
+
+	exynos4_mct_tick_stop(mevt);
+
+	tmp = (1 << 31) | cycles;	/* MCT_L_UPDATE_ICNTB */
+
+	/* update interrupt count buffer */
+	exynos4_mct_write(tmp, mevt->base + MCT_L_ICNTB_OFFSET);
+
+	/* enable MCT tick interrupt */
+	exynos4_mct_write(0x1, mevt->base + MCT_L_INT_ENB_OFFSET);
+
+	tmp = __raw_readl(reg_base + mevt->base + MCT_L_TCON_OFFSET);
+	tmp |= MCT_L_TCON_INT_START | MCT_L_TCON_TIMER_START |
+	       MCT_L_TCON_INTERVAL_MODE;
+	exynos4_mct_write(tmp, mevt->base + MCT_L_TCON_OFFSET);
+}
+
+static int exynos4_tick_set_next_event(unsigned long cycles,
+				       struct clock_event_device *evt)
+{
+	struct mct_clock_event_device *mevt = this_cpu_ptr(&percpu_mct_tick);
+
+	exynos4_mct_tick_start(cycles, mevt);
+
+	return 0;
+}
+
+static inline void exynos4_tick_set_mode(enum clock_event_mode mode,
+					 struct clock_event_device *evt)
+{
+	struct mct_clock_event_device *mevt = this_cpu_ptr(&percpu_mct_tick);
+	unsigned long cycles_per_jiffy;
+
+	exynos4_mct_tick_stop(mevt);
+
+	switch (mode) {
+	case CLOCK_EVT_MODE_PERIODIC:
+		cycles_per_jiffy =
+			(((unsigned long long) NSEC_PER_SEC / HZ * evt->mult) >> evt->shift);
+		exynos4_mct_tick_start(cycles_per_jiffy, mevt);
+		break;
+
+	case CLOCK_EVT_MODE_ONESHOT:
+	case CLOCK_EVT_MODE_UNUSED:
+	case CLOCK_EVT_MODE_SHUTDOWN:
+	case CLOCK_EVT_MODE_RESUME:
+		break;
+	}
+}
+
+static int exynos4_mct_tick_clear(struct mct_clock_event_device *mevt)
+{
+	struct clock_event_device *evt = mevt->evt;
+
+	/*
+	 * This is for supporting oneshot mode.
+	 * Mct would generate interrupt periodically
+	 * without explicit stopping.
+	 */
+	if (evt->mode != CLOCK_EVT_MODE_PERIODIC)
+		exynos4_mct_tick_stop(mevt);
+
+	/* Clear the MCT tick interrupt */
+	if (__raw_readl(reg_base + mevt->base + MCT_L_INT_CSTAT_OFFSET) & 1) {
+		exynos4_mct_write(0x1, mevt->base + MCT_L_INT_CSTAT_OFFSET);
+		return 1;
+	} else {
+		return 0;
+	}
+}
+
+static irqreturn_t exynos4_mct_tick_isr(int irq, void *dev_id)
+{
+	struct mct_clock_event_device *mevt = dev_id;
+	struct clock_event_device *evt = mevt->evt;
+
+	exynos4_mct_tick_clear(mevt);
+
+	evt->event_handler(evt);
+
+	return IRQ_HANDLED;
+}
+
+static struct irqaction mct_tick0_event_irq = {
+	.name		= "mct_tick0_irq",
+	.flags		= IRQF_TIMER | IRQF_NOBALANCING,
+	.handler	= exynos4_mct_tick_isr,
+};
+
+static struct irqaction mct_tick1_event_irq = {
+	.name		= "mct_tick1_irq",
+	.flags		= IRQF_TIMER | IRQF_NOBALANCING,
+	.handler	= exynos4_mct_tick_isr,
+};
+
+static int __cpuinit exynos4_local_timer_setup(struct clock_event_device *evt)
+{
+	struct mct_clock_event_device *mevt;
+	unsigned int cpu = smp_processor_id();
+
+	mevt = this_cpu_ptr(&percpu_mct_tick);
+	mevt->evt = evt;
+
+	mevt->base = EXYNOS4_MCT_L_BASE(cpu);
+	sprintf(mevt->name, "mct_tick%d", cpu);
+
+	evt->name = mevt->name;
+	evt->cpumask = cpumask_of(cpu);
+	evt->set_next_event = exynos4_tick_set_next_event;
+	evt->set_mode = exynos4_tick_set_mode;
+	evt->features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT;
+	evt->rating = 450;
+	clockevents_config_and_register(evt, clk_rate / (TICK_BASE_CNT + 1),
+					0xf, 0x7fffffff);
+
+	exynos4_mct_write(TICK_BASE_CNT, mevt->base + MCT_L_TCNTB_OFFSET);
+
+	if (mct_int_type == MCT_INT_SPI) {
+		if (cpu == 0) {
+			mct_tick0_event_irq.dev_id = mevt;
+			evt->irq = mct_irqs[MCT_L0_IRQ];
+			setup_irq(evt->irq, &mct_tick0_event_irq);
+		} else {
+			mct_tick1_event_irq.dev_id = mevt;
+			evt->irq = mct_irqs[MCT_L1_IRQ];
+			setup_irq(evt->irq, &mct_tick1_event_irq);
+			irq_set_affinity(evt->irq, cpumask_of(1));
+		}
+	} else {
+		enable_percpu_irq(mct_irqs[MCT_L0_IRQ], 0);
+	}
+
+	return 0;
+}
+
+static void exynos4_local_timer_stop(struct clock_event_device *evt)
+{
+	unsigned int cpu = smp_processor_id();
+	evt->set_mode(CLOCK_EVT_MODE_UNUSED, evt);
+	if (mct_int_type == MCT_INT_SPI)
+		if (cpu == 0)
+			remove_irq(evt->irq, &mct_tick0_event_irq);
+		else
+			remove_irq(evt->irq, &mct_tick1_event_irq);
+	else
+		disable_percpu_irq(mct_irqs[MCT_L0_IRQ]);
+}
+
+static struct local_timer_ops exynos4_mct_tick_ops __cpuinitdata = {
+	.setup	= exynos4_local_timer_setup,
+	.stop	= exynos4_local_timer_stop,
+};
+#endif /* CONFIG_LOCAL_TIMERS */
+
+static void __init exynos4_timer_resources(struct device_node *np, void __iomem *base)
+{
+	struct clk *mct_clk, *tick_clk;
+
+	tick_clk = np ? of_clk_get_by_name(np, "fin_pll") :
+				clk_get(NULL, "fin_pll");
+	if (IS_ERR(tick_clk))
+		panic("%s: unable to determine tick clock rate\n", __func__);
+	clk_rate = clk_get_rate(tick_clk);
+
+	mct_clk = np ? of_clk_get_by_name(np, "mct") : clk_get(NULL, "mct");
+	if (IS_ERR(mct_clk))
+		panic("%s: unable to retrieve mct clock instance\n", __func__);
+	clk_prepare_enable(mct_clk);
+
+	reg_base = base;
+	if (!reg_base)
+		panic("%s: unable to ioremap mct address space\n", __func__);
+
+#ifdef CONFIG_LOCAL_TIMERS
+	if (mct_int_type == MCT_INT_PPI) {
+		int err;
+
+		err = request_percpu_irq(mct_irqs[MCT_L0_IRQ],
+					 exynos4_mct_tick_isr, "MCT",
+					 &percpu_mct_tick);
+		WARN(err, "MCT: can't request IRQ %d (%d)\n",
+		     mct_irqs[MCT_L0_IRQ], err);
+	}
+
+	local_timer_register(&exynos4_mct_tick_ops);
+#endif /* CONFIG_LOCAL_TIMERS */
+}
+
+void __init mct_init(void __iomem *base, int irq_g0, int irq_l0, int irq_l1)
+{
+	mct_irqs[MCT_G0_IRQ] = irq_g0;
+	mct_irqs[MCT_L0_IRQ] = irq_l0;
+	mct_irqs[MCT_L1_IRQ] = irq_l1;
+	mct_int_type = MCT_INT_SPI;
+
+	exynos4_timer_resources(NULL, base);
+	exynos4_clocksource_init();
+	exynos4_clockevent_init();
+}
+
+static void __init mct_init_dt(struct device_node *np, unsigned int int_type)
+{
+	u32 nr_irqs, i;
+
+	mct_int_type = int_type;
+
+	/* This driver uses only one global timer interrupt */
+	mct_irqs[MCT_G0_IRQ] = irq_of_parse_and_map(np, MCT_G0_IRQ);
+
+	/*
+	 * Find out the number of local irqs specified. The local
+	 * timer irqs are specified after the four global timer
+	 * irqs are specified.
+	 */
+#ifdef CONFIG_OF
+	nr_irqs = of_irq_count(np);
+#else
+	nr_irqs = 0;
+#endif
+	for (i = MCT_L0_IRQ; i < nr_irqs; i++)
+		mct_irqs[i] = irq_of_parse_and_map(np, i);
+
+	exynos4_timer_resources(np, of_iomap(np, 0));
+	exynos4_clocksource_init();
+	exynos4_clockevent_init();
+}
+
+
+static void __init mct_init_spi(struct device_node *np)
+{
+	return mct_init_dt(np, MCT_INT_SPI);
+}
+
+static void __init mct_init_ppi(struct device_node *np)
+{
+	return mct_init_dt(np, MCT_INT_PPI);
+}
+CLOCKSOURCE_OF_DECLARE(exynos4210, "samsung,exynos4210-mct", mct_init_spi);
+CLOCKSOURCE_OF_DECLARE(exynos4412, "samsung,exynos4412-mct", mct_init_ppi);
diff --git a/drivers/clocksource/mxs_timer.c b/drivers/clocksource/mxs_timer.c
new file mode 100644
index 000000000000..02af4204af86
--- /dev/null
+++ b/drivers/clocksource/mxs_timer.c
@@ -0,0 +1,304 @@
+/*
+ *  Copyright (C) 2000-2001 Deep Blue Solutions
+ *  Copyright (C) 2002 Shane Nay (shane@minirl.com)
+ *  Copyright (C) 2006-2007 Pavel Pisa (ppisa@pikron.com)
+ *  Copyright (C) 2008 Juergen Beisert (kernel@pengutronix.de)
+ *  Copyright (C) 2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+ * MA 02110-1301, USA.
+ */
+
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/clockchips.h>
+#include <linux/clk.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/stmp_device.h>
+
+#include <asm/mach/time.h>
+#include <asm/sched_clock.h>
+
+/*
+ * There are 2 versions of the timrot on Freescale MXS-based SoCs.
+ * The v1 on MX23 only gets 16 bits counter, while v2 on MX28
+ * extends the counter to 32 bits.
+ *
+ * The implementation uses two timers, one for clock_event and
+ * another for clocksource. MX28 uses timrot 0 and 1, while MX23
+ * uses 0 and 2.
+ */
+
+#define MX23_TIMROT_VERSION_OFFSET	0x0a0
+#define MX28_TIMROT_VERSION_OFFSET	0x120
+#define BP_TIMROT_MAJOR_VERSION		24
+#define BV_TIMROT_VERSION_1		0x01
+#define BV_TIMROT_VERSION_2		0x02
+#define timrot_is_v1()	(timrot_major_version == BV_TIMROT_VERSION_1)
+
+/*
+ * There are 4 registers for each timrotv2 instance, and 2 registers
+ * for each timrotv1. So address step 0x40 in macros below strides
+ * one instance of timrotv2 while two instances of timrotv1.
+ *
+ * As the result, HW_TIMROT_XXXn(1) defines the address of timrot1
+ * on MX28 while timrot2 on MX23.
+ */
+/* common between v1 and v2 */
+#define HW_TIMROT_ROTCTRL		0x00
+#define HW_TIMROT_TIMCTRLn(n)		(0x20 + (n) * 0x40)
+/* v1 only */
+#define HW_TIMROT_TIMCOUNTn(n)		(0x30 + (n) * 0x40)
+/* v2 only */
+#define HW_TIMROT_RUNNING_COUNTn(n)	(0x30 + (n) * 0x40)
+#define HW_TIMROT_FIXED_COUNTn(n)	(0x40 + (n) * 0x40)
+
+#define BM_TIMROT_TIMCTRLn_RELOAD	(1 << 6)
+#define BM_TIMROT_TIMCTRLn_UPDATE	(1 << 7)
+#define BM_TIMROT_TIMCTRLn_IRQ_EN	(1 << 14)
+#define BM_TIMROT_TIMCTRLn_IRQ		(1 << 15)
+#define BP_TIMROT_TIMCTRLn_SELECT	0
+#define BV_TIMROTv1_TIMCTRLn_SELECT__32KHZ_XTAL		0x8
+#define BV_TIMROTv2_TIMCTRLn_SELECT__32KHZ_XTAL		0xb
+#define BV_TIMROTv2_TIMCTRLn_SELECT__TICK_ALWAYS	0xf
+
+static struct clock_event_device mxs_clockevent_device;
+static enum clock_event_mode mxs_clockevent_mode = CLOCK_EVT_MODE_UNUSED;
+
+static void __iomem *mxs_timrot_base;
+static u32 timrot_major_version;
+
+static inline void timrot_irq_disable(void)
+{
+	__raw_writel(BM_TIMROT_TIMCTRLn_IRQ_EN, mxs_timrot_base +
+		     HW_TIMROT_TIMCTRLn(0) + STMP_OFFSET_REG_CLR);
+}
+
+static inline void timrot_irq_enable(void)
+{
+	__raw_writel(BM_TIMROT_TIMCTRLn_IRQ_EN, mxs_timrot_base +
+		     HW_TIMROT_TIMCTRLn(0) + STMP_OFFSET_REG_SET);
+}
+
+static void timrot_irq_acknowledge(void)
+{
+	__raw_writel(BM_TIMROT_TIMCTRLn_IRQ, mxs_timrot_base +
+		     HW_TIMROT_TIMCTRLn(0) + STMP_OFFSET_REG_CLR);
+}
+
+static cycle_t timrotv1_get_cycles(struct clocksource *cs)
+{
+	return ~((__raw_readl(mxs_timrot_base + HW_TIMROT_TIMCOUNTn(1))
+			& 0xffff0000) >> 16);
+}
+
+static int timrotv1_set_next_event(unsigned long evt,
+					struct clock_event_device *dev)
+{
+	/* timrot decrements the count */
+	__raw_writel(evt, mxs_timrot_base + HW_TIMROT_TIMCOUNTn(0));
+
+	return 0;
+}
+
+static int timrotv2_set_next_event(unsigned long evt,
+					struct clock_event_device *dev)
+{
+	/* timrot decrements the count */
+	__raw_writel(evt, mxs_timrot_base + HW_TIMROT_FIXED_COUNTn(0));
+
+	return 0;
+}
+
+static irqreturn_t mxs_timer_interrupt(int irq, void *dev_id)
+{
+	struct clock_event_device *evt = dev_id;
+
+	timrot_irq_acknowledge();
+	evt->event_handler(evt);
+
+	return IRQ_HANDLED;
+}
+
+static struct irqaction mxs_timer_irq = {
+	.name		= "MXS Timer Tick",
+	.dev_id		= &mxs_clockevent_device,
+	.flags		= IRQF_TIMER | IRQF_IRQPOLL,
+	.handler	= mxs_timer_interrupt,
+};
+
+#ifdef DEBUG
+static const char *clock_event_mode_label[] const = {
+	[CLOCK_EVT_MODE_PERIODIC] = "CLOCK_EVT_MODE_PERIODIC",
+	[CLOCK_EVT_MODE_ONESHOT]  = "CLOCK_EVT_MODE_ONESHOT",
+	[CLOCK_EVT_MODE_SHUTDOWN] = "CLOCK_EVT_MODE_SHUTDOWN",
+	[CLOCK_EVT_MODE_UNUSED]   = "CLOCK_EVT_MODE_UNUSED"
+};
+#endif /* DEBUG */
+
+static void mxs_set_mode(enum clock_event_mode mode,
+				struct clock_event_device *evt)
+{
+	/* Disable interrupt in timer module */
+	timrot_irq_disable();
+
+	if (mode != mxs_clockevent_mode) {
+		/* Set event time into the furthest future */
+		if (timrot_is_v1())
+			__raw_writel(0xffff,
+				mxs_timrot_base + HW_TIMROT_TIMCOUNTn(1));
+		else
+			__raw_writel(0xffffffff,
+				mxs_timrot_base + HW_TIMROT_FIXED_COUNTn(1));
+
+		/* Clear pending interrupt */
+		timrot_irq_acknowledge();
+	}
+
+#ifdef DEBUG
+	pr_info("%s: changing mode from %s to %s\n", __func__,
+		clock_event_mode_label[mxs_clockevent_mode],
+		clock_event_mode_label[mode]);
+#endif /* DEBUG */
+
+	/* Remember timer mode */
+	mxs_clockevent_mode = mode;
+
+	switch (mode) {
+	case CLOCK_EVT_MODE_PERIODIC:
+		pr_err("%s: Periodic mode is not implemented\n", __func__);
+		break;
+	case CLOCK_EVT_MODE_ONESHOT:
+		timrot_irq_enable();
+		break;
+	case CLOCK_EVT_MODE_SHUTDOWN:
+	case CLOCK_EVT_MODE_UNUSED:
+	case CLOCK_EVT_MODE_RESUME:
+		/* Left event sources disabled, no more interrupts appear */
+		break;
+	}
+}
+
+static struct clock_event_device mxs_clockevent_device = {
+	.name		= "mxs_timrot",
+	.features	= CLOCK_EVT_FEAT_ONESHOT,
+	.set_mode	= mxs_set_mode,
+	.set_next_event	= timrotv2_set_next_event,
+	.rating		= 200,
+};
+
+static int __init mxs_clockevent_init(struct clk *timer_clk)
+{
+	if (timrot_is_v1())
+		mxs_clockevent_device.set_next_event = timrotv1_set_next_event;
+	mxs_clockevent_device.cpumask = cpumask_of(0);
+	clockevents_config_and_register(&mxs_clockevent_device,
+					clk_get_rate(timer_clk),
+					timrot_is_v1() ? 0xf : 0x2,
+					timrot_is_v1() ? 0xfffe : 0xfffffffe);
+
+	return 0;
+}
+
+static struct clocksource clocksource_mxs = {
+	.name		= "mxs_timer",
+	.rating		= 200,
+	.read		= timrotv1_get_cycles,
+	.mask		= CLOCKSOURCE_MASK(16),
+	.flags		= CLOCK_SOURCE_IS_CONTINUOUS,
+};
+
+static u32 notrace mxs_read_sched_clock_v2(void)
+{
+	return ~readl_relaxed(mxs_timrot_base + HW_TIMROT_RUNNING_COUNTn(1));
+}
+
+static int __init mxs_clocksource_init(struct clk *timer_clk)
+{
+	unsigned int c = clk_get_rate(timer_clk);
+
+	if (timrot_is_v1())
+		clocksource_register_hz(&clocksource_mxs, c);
+	else {
+		clocksource_mmio_init(mxs_timrot_base + HW_TIMROT_RUNNING_COUNTn(1),
+			"mxs_timer", c, 200, 32, clocksource_mmio_readl_down);
+		setup_sched_clock(mxs_read_sched_clock_v2, 32, c);
+	}
+
+	return 0;
+}
+
+static void __init mxs_timer_init(struct device_node *np)
+{
+	struct clk *timer_clk;
+	int irq;
+
+	mxs_timrot_base = of_iomap(np, 0);
+	WARN_ON(!mxs_timrot_base);
+
+	timer_clk = of_clk_get(np, 0);
+	if (IS_ERR(timer_clk)) {
+		pr_err("%s: failed to get clk\n", __func__);
+		return;
+	}
+
+	clk_prepare_enable(timer_clk);
+
+	/*
+	 * Initialize timers to a known state
+	 */
+	stmp_reset_block(mxs_timrot_base + HW_TIMROT_ROTCTRL);
+
+	/* get timrot version */
+	timrot_major_version = __raw_readl(mxs_timrot_base +
+			(of_device_is_compatible(np, "fsl,imx23-timrot") ?
+						MX23_TIMROT_VERSION_OFFSET :
+						MX28_TIMROT_VERSION_OFFSET));
+	timrot_major_version >>= BP_TIMROT_MAJOR_VERSION;
+
+	/* one for clock_event */
+	__raw_writel((timrot_is_v1() ?
+			BV_TIMROTv1_TIMCTRLn_SELECT__32KHZ_XTAL :
+			BV_TIMROTv2_TIMCTRLn_SELECT__TICK_ALWAYS) |
+			BM_TIMROT_TIMCTRLn_UPDATE |
+			BM_TIMROT_TIMCTRLn_IRQ_EN,
+			mxs_timrot_base + HW_TIMROT_TIMCTRLn(0));
+
+	/* another for clocksource */
+	__raw_writel((timrot_is_v1() ?
+			BV_TIMROTv1_TIMCTRLn_SELECT__32KHZ_XTAL :
+			BV_TIMROTv2_TIMCTRLn_SELECT__TICK_ALWAYS) |
+			BM_TIMROT_TIMCTRLn_RELOAD,
+			mxs_timrot_base + HW_TIMROT_TIMCTRLn(1));
+
+	/* set clocksource timer fixed count to the maximum */
+	if (timrot_is_v1())
+		__raw_writel(0xffff,
+			mxs_timrot_base + HW_TIMROT_TIMCOUNTn(1));
+	else
+		__raw_writel(0xffffffff,
+			mxs_timrot_base + HW_TIMROT_FIXED_COUNTn(1));
+
+	/* init and register the timer to the framework */
+	mxs_clocksource_init(timer_clk);
+	mxs_clockevent_init(timer_clk);
+
+	/* Make irqs happen */
+	irq = irq_of_parse_and_map(np, 0);
+	setup_irq(irq, &mxs_timer_irq);
+}
+CLOCKSOURCE_OF_DECLARE(mxs, "fsl,timrot", mxs_timer_init);
diff --git a/drivers/clocksource/nomadik-mtu.c b/drivers/clocksource/nomadik-mtu.c
index 071f6eadfea2..e405531e1cc5 100644
--- a/drivers/clocksource/nomadik-mtu.c
+++ b/drivers/clocksource/nomadik-mtu.c
@@ -67,7 +67,7 @@ static u32 clk_prescale;
 static u32 nmdk_cycle;		/* write-once */
 static struct delay_timer mtu_delay_timer;
 
-#ifdef CONFIG_NOMADIK_MTU_SCHED_CLOCK
+#ifdef CONFIG_CLKSRC_NOMADIK_MTU_SCHED_CLOCK
 /*
  * Override the global weak sched_clock symbol with this
  * local implementation which uses the clocksource to get some
@@ -233,7 +233,7 @@ void __init nmdk_timer_init(void __iomem *base, int irq)
 		pr_err("timer: failed to initialize clock source %s\n",
 		       "mtu_0");
 
-#ifdef CONFIG_NOMADIK_MTU_SCHED_CLOCK
+#ifdef CONFIG_CLKSRC_NOMADIK_MTU_SCHED_CLOCK
 	setup_sched_clock(nomadik_read_sched_clock, 32, rate);
 #endif
 
diff --git a/drivers/clocksource/samsung_pwm_timer.c b/drivers/clocksource/samsung_pwm_timer.c
new file mode 100644
index 000000000000..0234c8d2c8f2
--- /dev/null
+++ b/drivers/clocksource/samsung_pwm_timer.c
@@ -0,0 +1,494 @@
+/*
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * samsung - Common hr-timer support (s3c and s5p)
+ *
+ * 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/interrupt.h>
+#include <linux/irq.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/clockchips.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include <clocksource/samsung_pwm.h>
+
+#include <asm/sched_clock.h>
+
+/*
+ * Clocksource driver
+ */
+
+#define REG_TCFG0			0x00
+#define REG_TCFG1			0x04
+#define REG_TCON			0x08
+#define REG_TINT_CSTAT			0x44
+
+#define REG_TCNTB(chan)			(0x0c + 12 * (chan))
+#define REG_TCMPB(chan)			(0x10 + 12 * (chan))
+
+#define TCFG0_PRESCALER_MASK		0xff
+#define TCFG0_PRESCALER1_SHIFT		8
+
+#define TCFG1_SHIFT(x)	  		((x) * 4)
+#define TCFG1_MUX_MASK	  		0xf
+
+#define TCON_START(chan)		(1 << (4 * (chan) + 0))
+#define TCON_MANUALUPDATE(chan)		(1 << (4 * (chan) + 1))
+#define TCON_INVERT(chan)		(1 << (4 * (chan) + 2))
+#define TCON_AUTORELOAD(chan)		(1 << (4 * (chan) + 3))
+
+DEFINE_SPINLOCK(samsung_pwm_lock);
+EXPORT_SYMBOL(samsung_pwm_lock);
+
+struct samsung_pwm_clocksource {
+	void __iomem *base;
+	unsigned int irq[SAMSUNG_PWM_NUM];
+	struct samsung_pwm_variant variant;
+
+	struct clk *timerclk;
+
+	unsigned int event_id;
+	unsigned int source_id;
+	unsigned int tcnt_max;
+	unsigned int tscaler_div;
+	unsigned int tdiv;
+
+	unsigned long clock_count_per_tick;
+};
+
+static struct samsung_pwm_clocksource pwm;
+
+static void samsung_timer_set_prescale(unsigned int channel, u16 prescale)
+{
+	unsigned long flags;
+	u8 shift = 0;
+	u32 reg;
+
+	if (channel >= 2)
+		shift = TCFG0_PRESCALER1_SHIFT;
+
+	spin_lock_irqsave(&samsung_pwm_lock, flags);
+
+	reg = readl(pwm.base + REG_TCFG0);
+	reg &= ~(TCFG0_PRESCALER_MASK << shift);
+	reg |= (prescale - 1) << shift;
+	writel(reg, pwm.base + REG_TCFG0);
+
+	spin_unlock_irqrestore(&samsung_pwm_lock, flags);
+}
+
+static void samsung_timer_set_divisor(unsigned int channel, u8 divisor)
+{
+	u8 shift = TCFG1_SHIFT(channel);
+	unsigned long flags;
+	u32 reg;
+	u8 bits;
+
+	bits = (fls(divisor) - 1) - pwm.variant.div_base;
+
+	spin_lock_irqsave(&samsung_pwm_lock, flags);
+
+	reg = readl(pwm.base + REG_TCFG1);
+	reg &= ~(TCFG1_MUX_MASK << shift);
+	reg |= bits << shift;
+	writel(reg, pwm.base + REG_TCFG1);
+
+	spin_unlock_irqrestore(&samsung_pwm_lock, flags);
+}
+
+static void samsung_time_stop(unsigned int channel)
+{
+	unsigned long tcon;
+	unsigned long flags;
+
+	if (channel > 0)
+		++channel;
+
+	spin_lock_irqsave(&samsung_pwm_lock, flags);
+
+	tcon = __raw_readl(pwm.base + REG_TCON);
+	tcon &= ~TCON_START(channel);
+	__raw_writel(tcon, pwm.base + REG_TCON);
+
+	spin_unlock_irqrestore(&samsung_pwm_lock, flags);
+}
+
+static void samsung_time_setup(unsigned int channel, unsigned long tcnt)
+{
+	unsigned long tcon;
+	unsigned long flags;
+	unsigned int tcon_chan = channel;
+
+	if (tcon_chan > 0)
+		++tcon_chan;
+
+	spin_lock_irqsave(&samsung_pwm_lock, flags);
+
+	tcon = __raw_readl(pwm.base + REG_TCON);
+
+	tcon &= ~(TCON_START(tcon_chan) | TCON_AUTORELOAD(tcon_chan));
+	tcon |= TCON_MANUALUPDATE(tcon_chan);
+
+	__raw_writel(tcnt, pwm.base + REG_TCNTB(channel));
+	__raw_writel(tcnt, pwm.base + REG_TCMPB(channel));
+	__raw_writel(tcon, pwm.base + REG_TCON);
+
+	spin_unlock_irqrestore(&samsung_pwm_lock, flags);
+}
+
+static void samsung_time_start(unsigned int channel, bool periodic)
+{
+	unsigned long tcon;
+	unsigned long flags;
+
+	if (channel > 0)
+		++channel;
+
+	spin_lock_irqsave(&samsung_pwm_lock, flags);
+
+	tcon = __raw_readl(pwm.base + REG_TCON);
+
+	tcon &= ~TCON_MANUALUPDATE(channel);
+	tcon |= TCON_START(channel);
+
+	if (periodic)
+		tcon |= TCON_AUTORELOAD(channel);
+	else
+		tcon &= ~TCON_AUTORELOAD(channel);
+
+	__raw_writel(tcon, pwm.base + REG_TCON);
+
+	spin_unlock_irqrestore(&samsung_pwm_lock, flags);
+}
+
+static int samsung_set_next_event(unsigned long cycles,
+				struct clock_event_device *evt)
+{
+	/*
+	 * This check is needed to account for internal rounding
+	 * errors inside clockevents core, which might result in
+	 * passing cycles = 0, which in turn would not generate any
+	 * timer interrupt and hang the system.
+	 *
+	 * Another solution would be to set up the clockevent device
+	 * with min_delta = 2, but this would unnecessarily increase
+	 * the minimum sleep period.
+	 */
+	if (!cycles)
+		cycles = 1;
+
+	samsung_time_setup(pwm.event_id, cycles);
+	samsung_time_start(pwm.event_id, false);
+
+	return 0;
+}
+
+static void samsung_timer_resume(void)
+{
+	/* event timer restart */
+	samsung_time_setup(pwm.event_id, pwm.clock_count_per_tick - 1);
+	samsung_time_start(pwm.event_id, true);
+
+	/* source timer restart */
+	samsung_time_setup(pwm.source_id, pwm.tcnt_max);
+	samsung_time_start(pwm.source_id, true);
+}
+
+static void samsung_set_mode(enum clock_event_mode mode,
+				struct clock_event_device *evt)
+{
+	samsung_time_stop(pwm.event_id);
+
+	switch (mode) {
+	case CLOCK_EVT_MODE_PERIODIC:
+		samsung_time_setup(pwm.event_id, pwm.clock_count_per_tick - 1);
+		samsung_time_start(pwm.event_id, true);
+		break;
+
+	case CLOCK_EVT_MODE_ONESHOT:
+		break;
+
+	case CLOCK_EVT_MODE_UNUSED:
+	case CLOCK_EVT_MODE_SHUTDOWN:
+		break;
+
+	case CLOCK_EVT_MODE_RESUME:
+		samsung_timer_resume();
+		break;
+	}
+}
+
+static struct clock_event_device time_event_device = {
+	.name		= "samsung_event_timer",
+	.features	= CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT,
+	.rating		= 200,
+	.set_next_event	= samsung_set_next_event,
+	.set_mode	= samsung_set_mode,
+};
+
+static irqreturn_t samsung_clock_event_isr(int irq, void *dev_id)
+{
+	struct clock_event_device *evt = dev_id;
+
+	if (pwm.variant.has_tint_cstat) {
+		u32 mask = (1 << pwm.event_id);
+		writel(mask | (mask << 5), pwm.base + REG_TINT_CSTAT);
+	}
+
+	evt->event_handler(evt);
+
+	return IRQ_HANDLED;
+}
+
+static struct irqaction samsung_clock_event_irq = {
+	.name		= "samsung_time_irq",
+	.flags		= IRQF_DISABLED | IRQF_TIMER | IRQF_IRQPOLL,
+	.handler	= samsung_clock_event_isr,
+	.dev_id		= &time_event_device,
+};
+
+static void __init samsung_clockevent_init(void)
+{
+	unsigned long pclk;
+	unsigned long clock_rate;
+	unsigned int irq_number;
+
+	pclk = clk_get_rate(pwm.timerclk);
+
+	samsung_timer_set_prescale(pwm.event_id, pwm.tscaler_div);
+	samsung_timer_set_divisor(pwm.event_id, pwm.tdiv);
+
+	clock_rate = pclk / (pwm.tscaler_div * pwm.tdiv);
+	pwm.clock_count_per_tick = clock_rate / HZ;
+
+	time_event_device.cpumask = cpumask_of(0);
+	clockevents_config_and_register(&time_event_device,
+						clock_rate, 1, pwm.tcnt_max);
+
+	irq_number = pwm.irq[pwm.event_id];
+	setup_irq(irq_number, &samsung_clock_event_irq);
+
+	if (pwm.variant.has_tint_cstat) {
+		u32 mask = (1 << pwm.event_id);
+		writel(mask | (mask << 5), pwm.base + REG_TINT_CSTAT);
+	}
+}
+
+static void __iomem *samsung_timer_reg(void)
+{
+	switch (pwm.source_id) {
+	case 0:
+	case 1:
+	case 2:
+	case 3:
+		return pwm.base + pwm.source_id * 0x0c + 0x14;
+
+	case 4:
+		return pwm.base + 0x40;
+
+	default:
+		BUG();
+	}
+}
+
+/*
+ * Override the global weak sched_clock symbol with this
+ * local implementation which uses the clocksource to get some
+ * better resolution when scheduling the kernel. We accept that
+ * this wraps around for now, since it is just a relative time
+ * stamp. (Inspired by U300 implementation.)
+ */
+static u32 notrace samsung_read_sched_clock(void)
+{
+	void __iomem *reg = samsung_timer_reg();
+
+	if (!reg)
+		return 0;
+
+	return ~__raw_readl(reg);
+}
+
+static void __init samsung_clocksource_init(void)
+{
+	void __iomem *reg = samsung_timer_reg();
+	unsigned long pclk;
+	unsigned long clock_rate;
+	int ret;
+
+	pclk = clk_get_rate(pwm.timerclk);
+
+	samsung_timer_set_prescale(pwm.source_id, pwm.tscaler_div);
+	samsung_timer_set_divisor(pwm.source_id, pwm.tdiv);
+
+	clock_rate = pclk / (pwm.tscaler_div * pwm.tdiv);
+
+	samsung_time_setup(pwm.source_id, pwm.tcnt_max);
+	samsung_time_start(pwm.source_id, true);
+
+	setup_sched_clock(samsung_read_sched_clock,
+						pwm.variant.bits, clock_rate);
+
+	ret = clocksource_mmio_init(reg, "samsung_clocksource_timer",
+					clock_rate, 250, pwm.variant.bits,
+					clocksource_mmio_readl_down);
+	if (ret)
+		panic("samsung_clocksource_timer: can't register clocksource\n");
+}
+
+static void __init samsung_timer_resources(void)
+{
+	pwm.timerclk = clk_get(NULL, "timers");
+	if (IS_ERR(pwm.timerclk))
+		panic("failed to get timers clock for timer");
+
+	clk_prepare_enable(pwm.timerclk);
+
+	pwm.tcnt_max = (1UL << pwm.variant.bits) - 1;
+	if (pwm.variant.bits == 16) {
+		pwm.tscaler_div = 25;
+		pwm.tdiv = 2;
+	} else {
+		pwm.tscaler_div = 2;
+		pwm.tdiv = 1;
+	}
+}
+
+/*
+ * PWM master driver
+ */
+static void __init _samsung_pwm_clocksource_init(void)
+{
+	u8 mask;
+	int channel;
+
+	mask = ~pwm.variant.output_mask & ((1 << SAMSUNG_PWM_NUM) - 1);
+	channel = fls(mask) - 1;
+	if (channel < 0)
+		panic("failed to find PWM channel for clocksource");
+	pwm.source_id = channel;
+
+	mask &= ~(1 << channel);
+	channel = fls(mask) - 1;
+	if (channel < 0)
+		panic("failed to find PWM channel for clock event");
+	pwm.event_id = channel;
+
+	samsung_timer_resources();
+	samsung_clockevent_init();
+	samsung_clocksource_init();
+}
+
+void __init samsung_pwm_clocksource_init(void __iomem *base,
+			unsigned int *irqs, struct samsung_pwm_variant *variant)
+{
+	pwm.base = base;
+	memcpy(&pwm.variant, variant, sizeof(pwm.variant));
+	memcpy(pwm.irq, irqs, SAMSUNG_PWM_NUM * sizeof(*irqs));
+
+	_samsung_pwm_clocksource_init();
+}
+
+#ifdef CONFIG_CLKSRC_OF
+static void __init samsung_pwm_alloc(struct device_node *np,
+				     const struct samsung_pwm_variant *variant)
+{
+	struct resource res;
+	struct property *prop;
+	const __be32 *cur;
+	u32 val;
+	int i;
+
+	memcpy(&pwm.variant, variant, sizeof(pwm.variant));
+	for (i = 0; i < SAMSUNG_PWM_NUM; ++i)
+		pwm.irq[i] = irq_of_parse_and_map(np, i);
+
+	of_property_for_each_u32(np, "samsung,pwm-outputs", prop, cur, val) {
+		if (val >= SAMSUNG_PWM_NUM) {
+			pr_warning("%s: invalid channel index in samsung,pwm-outputs property\n",
+								__func__);
+			continue;
+		}
+		pwm.variant.output_mask |= 1 << val;
+	}
+
+	of_address_to_resource(np, 0, &res);
+	if (!request_mem_region(res.start,
+				resource_size(&res), "samsung-pwm")) {
+		pr_err("%s: failed to request IO mem region\n", __func__);
+		return;
+	}
+
+	pwm.base = ioremap(res.start, resource_size(&res));
+	if (!pwm.base) {
+		pr_err("%s: failed to map PWM registers\n", __func__);
+		release_mem_region(res.start, resource_size(&res));
+		return;
+	}
+
+	_samsung_pwm_clocksource_init();
+}
+
+static const struct samsung_pwm_variant s3c24xx_variant = {
+	.bits		= 16,
+	.div_base	= 1,
+	.has_tint_cstat	= false,
+	.tclk_mask	= (1 << 4),
+};
+
+static void __init s3c2410_pwm_clocksource_init(struct device_node *np)
+{
+	samsung_pwm_alloc(np, &s3c24xx_variant);
+}
+CLOCKSOURCE_OF_DECLARE(s3c2410_pwm, "samsung,s3c2410-pwm", s3c2410_pwm_clocksource_init);
+
+static const struct samsung_pwm_variant s3c64xx_variant = {
+	.bits		= 32,
+	.div_base	= 0,
+	.has_tint_cstat	= true,
+	.tclk_mask	= (1 << 7) | (1 << 6) | (1 << 5),
+};
+
+static void __init s3c64xx_pwm_clocksource_init(struct device_node *np)
+{
+	samsung_pwm_alloc(np, &s3c64xx_variant);
+}
+CLOCKSOURCE_OF_DECLARE(s3c6400_pwm, "samsung,s3c6400-pwm", s3c64xx_pwm_clocksource_init);
+
+static const struct samsung_pwm_variant s5p64x0_variant = {
+	.bits		= 32,
+	.div_base	= 0,
+	.has_tint_cstat	= true,
+	.tclk_mask	= 0,
+};
+
+static void __init s5p64x0_pwm_clocksource_init(struct device_node *np)
+{
+	samsung_pwm_alloc(np, &s5p64x0_variant);
+}
+CLOCKSOURCE_OF_DECLARE(s5p6440_pwm, "samsung,s5p6440-pwm", s5p64x0_pwm_clocksource_init);
+
+static const struct samsung_pwm_variant s5p_variant = {
+	.bits		= 32,
+	.div_base	= 0,
+	.has_tint_cstat	= true,
+	.tclk_mask	= (1 << 5),
+};
+
+static void __init s5p_pwm_clocksource_init(struct device_node *np)
+{
+	samsung_pwm_alloc(np, &s5p_variant);
+}
+CLOCKSOURCE_OF_DECLARE(s5pc100_pwm, "samsung,s5pc100-pwm", s5p_pwm_clocksource_init);
+#endif
diff --git a/drivers/clocksource/sh_cmt.c b/drivers/clocksource/sh_cmt.c
index 488c14cc8dbf..08d0c418c94a 100644
--- a/drivers/clocksource/sh_cmt.c
+++ b/drivers/clocksource/sh_cmt.c
@@ -54,62 +54,100 @@ struct sh_cmt_priv {
 	struct clocksource cs;
 	unsigned long total_cycles;
 	bool cs_enabled;
+
+	/* callbacks for CMSTR and CMCSR access */
+	unsigned long (*read_control)(void __iomem *base, unsigned long offs);
+	void (*write_control)(void __iomem *base, unsigned long offs,
+			      unsigned long value);
+
+	/* callbacks for CMCNT and CMCOR access */
+	unsigned long (*read_count)(void __iomem *base, unsigned long offs);
+	void (*write_count)(void __iomem *base, unsigned long offs,
+			    unsigned long value);
 };
 
-static DEFINE_RAW_SPINLOCK(sh_cmt_lock);
+/* Examples of supported CMT timer register layouts and I/O access widths:
+ *
+ * "16-bit counter and 16-bit control" as found on sh7263:
+ * CMSTR 0xfffec000 16-bit
+ * CMCSR 0xfffec002 16-bit
+ * CMCNT 0xfffec004 16-bit
+ * CMCOR 0xfffec006 16-bit
+ *
+ * "32-bit counter and 16-bit control" as found on sh7372, sh73a0, r8a7740:
+ * CMSTR 0xffca0000 16-bit
+ * CMCSR 0xffca0060 16-bit
+ * CMCNT 0xffca0064 32-bit
+ * CMCOR 0xffca0068 32-bit
+ */
+
+static unsigned long sh_cmt_read16(void __iomem *base, unsigned long offs)
+{
+	return ioread16(base + (offs << 1));
+}
+
+static unsigned long sh_cmt_read32(void __iomem *base, unsigned long offs)
+{
+	return ioread32(base + (offs << 2));
+}
+
+static void sh_cmt_write16(void __iomem *base, unsigned long offs,
+			   unsigned long value)
+{
+	iowrite16(value, base + (offs << 1));
+}
+
+static void sh_cmt_write32(void __iomem *base, unsigned long offs,
+			   unsigned long value)
+{
+	iowrite32(value, base + (offs << 2));
+}
 
-#define CMSTR -1 /* shared register */
 #define CMCSR 0 /* channel register */
 #define CMCNT 1 /* channel register */
 #define CMCOR 2 /* channel register */
 
-static inline unsigned long sh_cmt_read(struct sh_cmt_priv *p, int reg_nr)
+static inline unsigned long sh_cmt_read_cmstr(struct sh_cmt_priv *p)
 {
 	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
-	void __iomem *base = p->mapbase;
-	unsigned long offs;
-
-	if (reg_nr == CMSTR) {
-		offs = 0;
-		base -= cfg->channel_offset;
-	} else
-		offs = reg_nr;
-
-	if (p->width == 16)
-		offs <<= 1;
-	else {
-		offs <<= 2;
-		if ((reg_nr == CMCNT) || (reg_nr == CMCOR))
-			return ioread32(base + offs);
-	}
 
-	return ioread16(base + offs);
+	return p->read_control(p->mapbase - cfg->channel_offset, 0);
 }
 
-static inline void sh_cmt_write(struct sh_cmt_priv *p, int reg_nr,
-				unsigned long value)
+static inline unsigned long sh_cmt_read_cmcsr(struct sh_cmt_priv *p)
+{
+	return p->read_control(p->mapbase, CMCSR);
+}
+
+static inline unsigned long sh_cmt_read_cmcnt(struct sh_cmt_priv *p)
+{
+	return p->read_count(p->mapbase, CMCNT);
+}
+
+static inline void sh_cmt_write_cmstr(struct sh_cmt_priv *p,
+				      unsigned long value)
 {
 	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
-	void __iomem *base = p->mapbase;
-	unsigned long offs;
-
-	if (reg_nr == CMSTR) {
-		offs = 0;
-		base -= cfg->channel_offset;
-	} else
-		offs = reg_nr;
-
-	if (p->width == 16)
-		offs <<= 1;
-	else {
-		offs <<= 2;
-		if ((reg_nr == CMCNT) || (reg_nr == CMCOR)) {
-			iowrite32(value, base + offs);
-			return;
-		}
-	}
 
-	iowrite16(value, base + offs);
+	p->write_control(p->mapbase - cfg->channel_offset, 0, value);
+}
+
+static inline void sh_cmt_write_cmcsr(struct sh_cmt_priv *p,
+				      unsigned long value)
+{
+	p->write_control(p->mapbase, CMCSR, value);
+}
+
+static inline void sh_cmt_write_cmcnt(struct sh_cmt_priv *p,
+				      unsigned long value)
+{
+	p->write_count(p->mapbase, CMCNT, value);
+}
+
+static inline void sh_cmt_write_cmcor(struct sh_cmt_priv *p,
+				      unsigned long value)
+{
+	p->write_count(p->mapbase, CMCOR, value);
 }
 
 static unsigned long sh_cmt_get_counter(struct sh_cmt_priv *p,
@@ -118,15 +156,15 @@ static unsigned long sh_cmt_get_counter(struct sh_cmt_priv *p,
 	unsigned long v1, v2, v3;
 	int o1, o2;
 
-	o1 = sh_cmt_read(p, CMCSR) & p->overflow_bit;
+	o1 = sh_cmt_read_cmcsr(p) & p->overflow_bit;
 
 	/* Make sure the timer value is stable. Stolen from acpi_pm.c */
 	do {
 		o2 = o1;
-		v1 = sh_cmt_read(p, CMCNT);
-		v2 = sh_cmt_read(p, CMCNT);
-		v3 = sh_cmt_read(p, CMCNT);
-		o1 = sh_cmt_read(p, CMCSR) & p->overflow_bit;
+		v1 = sh_cmt_read_cmcnt(p);
+		v2 = sh_cmt_read_cmcnt(p);
+		v3 = sh_cmt_read_cmcnt(p);
+		o1 = sh_cmt_read_cmcsr(p) & p->overflow_bit;
 	} while (unlikely((o1 != o2) || (v1 > v2 && v1 < v3)
 			  || (v2 > v3 && v2 < v1) || (v3 > v1 && v3 < v2)));
 
@@ -134,6 +172,7 @@ static unsigned long sh_cmt_get_counter(struct sh_cmt_priv *p,
 	return v2;
 }
 
+static DEFINE_RAW_SPINLOCK(sh_cmt_lock);
 
 static void sh_cmt_start_stop_ch(struct sh_cmt_priv *p, int start)
 {
@@ -142,14 +181,14 @@ static void sh_cmt_start_stop_ch(struct sh_cmt_priv *p, int start)
 
 	/* start stop register shared by multiple timer channels */
 	raw_spin_lock_irqsave(&sh_cmt_lock, flags);
-	value = sh_cmt_read(p, CMSTR);
+	value = sh_cmt_read_cmstr(p);
 
 	if (start)
 		value |= 1 << cfg->timer_bit;
 	else
 		value &= ~(1 << cfg->timer_bit);
 
-	sh_cmt_write(p, CMSTR, value);
+	sh_cmt_write_cmstr(p, value);
 	raw_spin_unlock_irqrestore(&sh_cmt_lock, flags);
 }
 
@@ -173,14 +212,14 @@ static int sh_cmt_enable(struct sh_cmt_priv *p, unsigned long *rate)
 	/* configure channel, periodic mode and maximum timeout */
 	if (p->width == 16) {
 		*rate = clk_get_rate(p->clk) / 512;
-		sh_cmt_write(p, CMCSR, 0x43);
+		sh_cmt_write_cmcsr(p, 0x43);
 	} else {
 		*rate = clk_get_rate(p->clk) / 8;
-		sh_cmt_write(p, CMCSR, 0x01a4);
+		sh_cmt_write_cmcsr(p, 0x01a4);
 	}
 
-	sh_cmt_write(p, CMCOR, 0xffffffff);
-	sh_cmt_write(p, CMCNT, 0);
+	sh_cmt_write_cmcor(p, 0xffffffff);
+	sh_cmt_write_cmcnt(p, 0);
 
 	/*
 	 * According to the sh73a0 user's manual, as CMCNT can be operated
@@ -194,12 +233,12 @@ static int sh_cmt_enable(struct sh_cmt_priv *p, unsigned long *rate)
 	 * take RCLKx2 at maximum.
 	 */
 	for (k = 0; k < 100; k++) {
-		if (!sh_cmt_read(p, CMCNT))
+		if (!sh_cmt_read_cmcnt(p))
 			break;
 		udelay(1);
 	}
 
-	if (sh_cmt_read(p, CMCNT)) {
+	if (sh_cmt_read_cmcnt(p)) {
 		dev_err(&p->pdev->dev, "cannot clear CMCNT\n");
 		ret = -ETIMEDOUT;
 		goto err1;
@@ -222,7 +261,7 @@ static void sh_cmt_disable(struct sh_cmt_priv *p)
 	sh_cmt_start_stop_ch(p, 0);
 
 	/* disable interrupts in CMT block */
-	sh_cmt_write(p, CMCSR, 0);
+	sh_cmt_write_cmcsr(p, 0);
 
 	/* stop clock */
 	clk_disable(p->clk);
@@ -270,7 +309,7 @@ static void sh_cmt_clock_event_program_verify(struct sh_cmt_priv *p,
 		if (new_match > p->max_match_value)
 			new_match = p->max_match_value;
 
-		sh_cmt_write(p, CMCOR, new_match);
+		sh_cmt_write_cmcor(p, new_match);
 
 		now = sh_cmt_get_counter(p, &has_wrapped);
 		if (has_wrapped && (new_match > p->match_value)) {
@@ -346,7 +385,7 @@ static irqreturn_t sh_cmt_interrupt(int irq, void *dev_id)
 	struct sh_cmt_priv *p = dev_id;
 
 	/* clear flags */
-	sh_cmt_write(p, CMCSR, sh_cmt_read(p, CMCSR) & p->clear_bits);
+	sh_cmt_write_cmcsr(p, sh_cmt_read_cmcsr(p) & p->clear_bits);
 
 	/* update clock source counter to begin with if enabled
 	 * the wrap flag should be cleared by the timer specific
@@ -625,14 +664,6 @@ static int sh_cmt_register(struct sh_cmt_priv *p, char *name,
 			   unsigned long clockevent_rating,
 			   unsigned long clocksource_rating)
 {
-	if (p->width == (sizeof(p->max_match_value) * 8))
-		p->max_match_value = ~0;
-	else
-		p->max_match_value = (1 << p->width) - 1;
-
-	p->match_value = p->max_match_value;
-	raw_spin_lock_init(&p->lock);
-
 	if (clockevent_rating)
 		sh_cmt_register_clockevent(p, name, clockevent_rating);
 
@@ -657,8 +688,6 @@ static int sh_cmt_setup(struct sh_cmt_priv *p, struct platform_device *pdev)
 		goto err0;
 	}
 
-	platform_set_drvdata(pdev, p);
-
 	res = platform_get_resource(p->pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_err(&p->pdev->dev, "failed to get I/O memory\n");
@@ -693,32 +722,51 @@ static int sh_cmt_setup(struct sh_cmt_priv *p, struct platform_device *pdev)
 		goto err1;
 	}
 
+	p->read_control = sh_cmt_read16;
+	p->write_control = sh_cmt_write16;
+
 	if (resource_size(res) == 6) {
 		p->width = 16;
+		p->read_count = sh_cmt_read16;
+		p->write_count = sh_cmt_write16;
 		p->overflow_bit = 0x80;
 		p->clear_bits = ~0x80;
 	} else {
 		p->width = 32;
+		p->read_count = sh_cmt_read32;
+		p->write_count = sh_cmt_write32;
 		p->overflow_bit = 0x8000;
 		p->clear_bits = ~0xc000;
 	}
 
+	if (p->width == (sizeof(p->max_match_value) * 8))
+		p->max_match_value = ~0;
+	else
+		p->max_match_value = (1 << p->width) - 1;
+
+	p->match_value = p->max_match_value;
+	raw_spin_lock_init(&p->lock);
+
 	ret = sh_cmt_register(p, (char *)dev_name(&p->pdev->dev),
 			      cfg->clockevent_rating,
 			      cfg->clocksource_rating);
 	if (ret) {
 		dev_err(&p->pdev->dev, "registration failed\n");
-		goto err1;
+		goto err2;
 	}
 	p->cs_enabled = false;
 
 	ret = setup_irq(irq, &p->irqaction);
 	if (ret) {
 		dev_err(&p->pdev->dev, "failed to request irq %d\n", irq);
-		goto err1;
+		goto err2;
 	}
 
+	platform_set_drvdata(pdev, p);
+
 	return 0;
+err2:
+	clk_put(p->clk);
 
 err1:
 	iounmap(p->mapbase);
@@ -751,7 +799,6 @@ static int sh_cmt_probe(struct platform_device *pdev)
 	ret = sh_cmt_setup(p, pdev);
 	if (ret) {
 		kfree(p);
-		platform_set_drvdata(pdev, NULL);
 		pm_runtime_idle(&pdev->dev);
 		return ret;
 	}
@@ -791,7 +838,7 @@ static void __exit sh_cmt_exit(void)
 }
 
 early_platform_init("earlytimer", &sh_cmt_device_driver);
-module_init(sh_cmt_init);
+subsys_initcall(sh_cmt_init);
 module_exit(sh_cmt_exit);
 
 MODULE_AUTHOR("Magnus Damm");
diff --git a/drivers/clocksource/sh_mtu2.c b/drivers/clocksource/sh_mtu2.c
index 83943e27cfac..4aac9ee0d0c0 100644
--- a/drivers/clocksource/sh_mtu2.c
+++ b/drivers/clocksource/sh_mtu2.c
@@ -386,7 +386,7 @@ static void __exit sh_mtu2_exit(void)
 }
 
 early_platform_init("earlytimer", &sh_mtu2_device_driver);
-module_init(sh_mtu2_init);
+subsys_initcall(sh_mtu2_init);
 module_exit(sh_mtu2_exit);
 
 MODULE_AUTHOR("Magnus Damm");
diff --git a/drivers/clocksource/sh_tmu.c b/drivers/clocksource/sh_tmu.c
index b4502edce2a1..78b8dae49628 100644
--- a/drivers/clocksource/sh_tmu.c
+++ b/drivers/clocksource/sh_tmu.c
@@ -549,7 +549,7 @@ static void __exit sh_tmu_exit(void)
 }
 
 early_platform_init("earlytimer", &sh_tmu_device_driver);
-module_init(sh_tmu_init);
+subsys_initcall(sh_tmu_init);
 module_exit(sh_tmu_exit);
 
 MODULE_AUTHOR("Magnus Damm");
diff --git a/drivers/clocksource/sunxi_timer.c b/drivers/clocksource/sun4i_timer.c
index 0ce85e29769b..d4674e78ef35 100644
--- a/drivers/clocksource/sunxi_timer.c
+++ b/drivers/clocksource/sun4i_timer.c
@@ -22,66 +22,64 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
-#include <linux/sunxi_timer.h>
-#include <linux/clk/sunxi.h>
 
-#define TIMER_CTL_REG		0x00
-#define TIMER_CTL_ENABLE		(1 << 0)
+#define TIMER_IRQ_EN_REG	0x00
+#define TIMER_IRQ_EN(val)		(1 << val)
 #define TIMER_IRQ_ST_REG	0x04
-#define TIMER0_CTL_REG		0x10
-#define TIMER0_CTL_ENABLE		(1 << 0)
-#define TIMER0_CTL_AUTORELOAD		(1 << 1)
-#define TIMER0_CTL_ONESHOT		(1 << 7)
-#define TIMER0_INTVAL_REG	0x14
-#define TIMER0_CNTVAL_REG	0x18
+#define TIMER_CTL_REG(val)	(0x10 * val + 0x10)
+#define TIMER_CTL_ENABLE		(1 << 0)
+#define TIMER_CTL_AUTORELOAD		(1 << 1)
+#define TIMER_CTL_ONESHOT		(1 << 7)
+#define TIMER_INTVAL_REG(val)	(0x10 * val + 0x14)
+#define TIMER_CNTVAL_REG(val)	(0x10 * val + 0x18)
 
 #define TIMER_SCAL		16
 
 static void __iomem *timer_base;
 
-static void sunxi_clkevt_mode(enum clock_event_mode mode,
+static void sun4i_clkevt_mode(enum clock_event_mode mode,
 			      struct clock_event_device *clk)
 {
-	u32 u = readl(timer_base + TIMER0_CTL_REG);
+	u32 u = readl(timer_base + TIMER_CTL_REG(0));
 
 	switch (mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
-		u &= ~(TIMER0_CTL_ONESHOT);
-		writel(u | TIMER0_CTL_ENABLE, timer_base + TIMER0_CTL_REG);
+		u &= ~(TIMER_CTL_ONESHOT);
+		writel(u | TIMER_CTL_ENABLE, timer_base + TIMER_CTL_REG(0));
 		break;
 
 	case CLOCK_EVT_MODE_ONESHOT:
-		writel(u | TIMER0_CTL_ONESHOT, timer_base + TIMER0_CTL_REG);
+		writel(u | TIMER_CTL_ONESHOT, timer_base + TIMER_CTL_REG(0));
 		break;
 	case CLOCK_EVT_MODE_UNUSED:
 	case CLOCK_EVT_MODE_SHUTDOWN:
 	default:
-		writel(u & ~(TIMER0_CTL_ENABLE), timer_base + TIMER0_CTL_REG);
+		writel(u & ~(TIMER_CTL_ENABLE), timer_base + TIMER_CTL_REG(0));
 		break;
 	}
 }
 
-static int sunxi_clkevt_next_event(unsigned long evt,
+static int sun4i_clkevt_next_event(unsigned long evt,
 				   struct clock_event_device *unused)
 {
-	u32 u = readl(timer_base + TIMER0_CTL_REG);
-	writel(evt, timer_base + TIMER0_CNTVAL_REG);
-	writel(u | TIMER0_CTL_ENABLE | TIMER0_CTL_AUTORELOAD,
-	       timer_base + TIMER0_CTL_REG);
+	u32 u = readl(timer_base + TIMER_CTL_REG(0));
+	writel(evt, timer_base + TIMER_CNTVAL_REG(0));
+	writel(u | TIMER_CTL_ENABLE | TIMER_CTL_AUTORELOAD,
+	       timer_base + TIMER_CTL_REG(0));
 
 	return 0;
 }
 
-static struct clock_event_device sunxi_clockevent = {
-	.name = "sunxi_tick",
+static struct clock_event_device sun4i_clockevent = {
+	.name = "sun4i_tick",
 	.rating = 300,
 	.features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT,
-	.set_mode = sunxi_clkevt_mode,
-	.set_next_event = sunxi_clkevt_next_event,
+	.set_mode = sun4i_clkevt_mode,
+	.set_next_event = sun4i_clkevt_next_event,
 };
 
 
-static irqreturn_t sunxi_timer_interrupt(int irq, void *dev_id)
+static irqreturn_t sun4i_timer_interrupt(int irq, void *dev_id)
 {
 	struct clock_event_device *evt = (struct clock_event_device *)dev_id;
 
@@ -91,30 +89,20 @@ static irqreturn_t sunxi_timer_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static struct irqaction sunxi_timer_irq = {
-	.name = "sunxi_timer0",
+static struct irqaction sun4i_timer_irq = {
+	.name = "sun4i_timer0",
 	.flags = IRQF_DISABLED | IRQF_TIMER | IRQF_IRQPOLL,
-	.handler = sunxi_timer_interrupt,
-	.dev_id = &sunxi_clockevent,
-};
-
-static struct of_device_id sunxi_timer_dt_ids[] = {
-	{ .compatible = "allwinner,sunxi-timer" },
-	{ }
+	.handler = sun4i_timer_interrupt,
+	.dev_id = &sun4i_clockevent,
 };
 
-void __init sunxi_timer_init(void)
+static void __init sun4i_timer_init(struct device_node *node)
 {
-	struct device_node *node;
 	unsigned long rate = 0;
 	struct clk *clk;
 	int ret, irq;
 	u32 val;
 
-	node = of_find_matching_node(NULL, sunxi_timer_dt_ids);
-	if (!node)
-		panic("No sunxi timer node");
-
 	timer_base = of_iomap(node, 0);
 	if (!timer_base)
 		panic("Can't map registers");
@@ -123,8 +111,6 @@ void __init sunxi_timer_init(void)
 	if (irq <= 0)
 		panic("Can't parse IRQ");
 
-	sunxi_init_clocks();
-
 	clk = of_clk_get(node, 0);
 	if (IS_ERR(clk))
 		panic("Can't get timer clock");
@@ -132,29 +118,31 @@ void __init sunxi_timer_init(void)
 	rate = clk_get_rate(clk);
 
 	writel(rate / (TIMER_SCAL * HZ),
-	       timer_base + TIMER0_INTVAL_REG);
+	       timer_base + TIMER_INTVAL_REG(0));
 
 	/* set clock source to HOSC, 16 pre-division */
-	val = readl(timer_base + TIMER0_CTL_REG);
+	val = readl(timer_base + TIMER_CTL_REG(0));
 	val &= ~(0x07 << 4);
 	val &= ~(0x03 << 2);
 	val |= (4 << 4) | (1 << 2);
-	writel(val, timer_base + TIMER0_CTL_REG);
+	writel(val, timer_base + TIMER_CTL_REG(0));
 
 	/* set mode to auto reload */
-	val = readl(timer_base + TIMER0_CTL_REG);
-	writel(val | TIMER0_CTL_AUTORELOAD, timer_base + TIMER0_CTL_REG);
+	val = readl(timer_base + TIMER_CTL_REG(0));
+	writel(val | TIMER_CTL_AUTORELOAD, timer_base + TIMER_CTL_REG(0));
 
-	ret = setup_irq(irq, &sunxi_timer_irq);
+	ret = setup_irq(irq, &sun4i_timer_irq);
 	if (ret)
 		pr_warn("failed to setup irq %d\n", irq);
 
 	/* Enable timer0 interrupt */
-	val = readl(timer_base + TIMER_CTL_REG);
-	writel(val | TIMER_CTL_ENABLE, timer_base + TIMER_CTL_REG);
+	val = readl(timer_base + TIMER_IRQ_EN_REG);
+	writel(val | TIMER_IRQ_EN(0), timer_base + TIMER_IRQ_EN_REG);
 
-	sunxi_clockevent.cpumask = cpumask_of(0);
+	sun4i_clockevent.cpumask = cpumask_of(0);
 
-	clockevents_config_and_register(&sunxi_clockevent, rate / TIMER_SCAL,
+	clockevents_config_and_register(&sun4i_clockevent, rate / TIMER_SCAL,
 					0x1, 0xff);
 }
+CLOCKSOURCE_OF_DECLARE(sun4i, "allwinner,sun4i-timer",
+		       sun4i_timer_init);
diff --git a/drivers/clocksource/tegra20_timer.c b/drivers/clocksource/tegra20_timer.c
index 0bde03feb095..ae877b021b54 100644
--- a/drivers/clocksource/tegra20_timer.c
+++ b/drivers/clocksource/tegra20_timer.c
@@ -154,29 +154,12 @@ static struct irqaction tegra_timer_irq = {
 	.dev_id		= &tegra_clockevent,
 };
 
-static const struct of_device_id timer_match[] __initconst = {
-	{ .compatible = "nvidia,tegra20-timer" },
-	{}
-};
-
-static const struct of_device_id rtc_match[] __initconst = {
-	{ .compatible = "nvidia,tegra20-rtc" },
-	{}
-};
-
-static void __init tegra20_init_timer(void)
+static void __init tegra20_init_timer(struct device_node *np)
 {
-	struct device_node *np;
 	struct clk *clk;
 	unsigned long rate;
 	int ret;
 
-	np = of_find_matching_node(NULL, timer_match);
-	if (!np) {
-		pr_err("Failed to find timer DT node\n");
-		BUG();
-	}
-
 	timer_reg_base = of_iomap(np, 0);
 	if (!timer_reg_base) {
 		pr_err("Can't map timer registers\n");
@@ -189,7 +172,7 @@ static void __init tegra20_init_timer(void)
 		BUG();
 	}
 
-	clk = clk_get_sys("timer", NULL);
+	clk = of_clk_get(np, 0);
 	if (IS_ERR(clk)) {
 		pr_warn("Unable to get timer clock. Assuming 12Mhz input clock.\n");
 		rate = 12000000;
@@ -200,30 +183,6 @@ static void __init tegra20_init_timer(void)
 
 	of_node_put(np);
 
-	np = of_find_matching_node(NULL, rtc_match);
-	if (!np) {
-		pr_err("Failed to find RTC DT node\n");
-		BUG();
-	}
-
-	rtc_base = of_iomap(np, 0);
-	if (!rtc_base) {
-		pr_err("Can't map RTC registers");
-		BUG();
-	}
-
-	/*
-	 * rtc registers are used by read_persistent_clock, keep the rtc clock
-	 * enabled
-	 */
-	clk = clk_get_sys("rtc-tegra", NULL);
-	if (IS_ERR(clk))
-		pr_warn("Unable to get rtc-tegra clock\n");
-	else
-		clk_prepare_enable(clk);
-
-	of_node_put(np);
-
 	switch (rate) {
 	case 12000000:
 		timer_writel(0x000b, TIMERUS_USEC_CFG);
@@ -259,12 +218,34 @@ static void __init tegra20_init_timer(void)
 	tegra_clockevent.irq = tegra_timer_irq.irq;
 	clockevents_config_and_register(&tegra_clockevent, 1000000,
 					0x1, 0x1fffffff);
-#ifdef CONFIG_HAVE_ARM_TWD
-	twd_local_timer_of_register();
-#endif
+}
+CLOCKSOURCE_OF_DECLARE(tegra20_timer, "nvidia,tegra20-timer", tegra20_init_timer);
+
+static void __init tegra20_init_rtc(struct device_node *np)
+{
+	struct clk *clk;
+
+	rtc_base = of_iomap(np, 0);
+	if (!rtc_base) {
+		pr_err("Can't map RTC registers");
+		BUG();
+	}
+
+	/*
+	 * rtc registers are used by read_persistent_clock, keep the rtc clock
+	 * enabled
+	 */
+	clk = of_clk_get(np, 0);
+	if (IS_ERR(clk))
+		pr_warn("Unable to get rtc-tegra clock\n");
+	else
+		clk_prepare_enable(clk);
+
+	of_node_put(np);
+
 	register_persistent_clock(NULL, tegra_read_persistent_clock);
 }
-CLOCKSOURCE_OF_DECLARE(tegra20, "nvidia,tegra20-timer", tegra20_init_timer);
+CLOCKSOURCE_OF_DECLARE(tegra20_rtc, "nvidia,tegra20-rtc", tegra20_init_rtc);
 
 #ifdef CONFIG_PM
 static u32 usec_config;
diff --git a/drivers/clocksource/timer-marco.c b/drivers/clocksource/timer-marco.c
new file mode 100644
index 000000000000..97738dbf3e3b
--- /dev/null
+++ b/drivers/clocksource/timer-marco.c
@@ -0,0 +1,299 @@
+/*
+ * System timer for CSR SiRFprimaII
+ *
+ * Copyright (c) 2011 Cambridge Silicon Radio Limited, a CSR plc group company.
+ *
+ * Licensed under GPLv2 or later.
+ */
+
+#include <linux/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/clockchips.h>
+#include <linux/clocksource.h>
+#include <linux/bitops.h>
+#include <linux/irq.h>
+#include <linux/clk.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <asm/sched_clock.h>
+#include <asm/localtimer.h>
+#include <asm/mach/time.h>
+
+#define SIRFSOC_TIMER_32COUNTER_0_CTRL			0x0000
+#define SIRFSOC_TIMER_32COUNTER_1_CTRL			0x0004
+#define SIRFSOC_TIMER_MATCH_0				0x0018
+#define SIRFSOC_TIMER_MATCH_1				0x001c
+#define SIRFSOC_TIMER_COUNTER_0				0x0048
+#define SIRFSOC_TIMER_COUNTER_1				0x004c
+#define SIRFSOC_TIMER_INTR_STATUS			0x0060
+#define SIRFSOC_TIMER_WATCHDOG_EN			0x0064
+#define SIRFSOC_TIMER_64COUNTER_CTRL			0x0068
+#define SIRFSOC_TIMER_64COUNTER_LO			0x006c
+#define SIRFSOC_TIMER_64COUNTER_HI			0x0070
+#define SIRFSOC_TIMER_64COUNTER_LOAD_LO			0x0074
+#define SIRFSOC_TIMER_64COUNTER_LOAD_HI			0x0078
+#define SIRFSOC_TIMER_64COUNTER_RLATCHED_LO		0x007c
+#define SIRFSOC_TIMER_64COUNTER_RLATCHED_HI		0x0080
+
+#define SIRFSOC_TIMER_REG_CNT 6
+
+static const u32 sirfsoc_timer_reg_list[SIRFSOC_TIMER_REG_CNT] = {
+	SIRFSOC_TIMER_WATCHDOG_EN,
+	SIRFSOC_TIMER_32COUNTER_0_CTRL,
+	SIRFSOC_TIMER_32COUNTER_1_CTRL,
+	SIRFSOC_TIMER_64COUNTER_CTRL,
+	SIRFSOC_TIMER_64COUNTER_RLATCHED_LO,
+	SIRFSOC_TIMER_64COUNTER_RLATCHED_HI,
+};
+
+static u32 sirfsoc_timer_reg_val[SIRFSOC_TIMER_REG_CNT];
+
+static void __iomem *sirfsoc_timer_base;
+
+/* disable count and interrupt */
+static inline void sirfsoc_timer_count_disable(int idx)
+{
+	writel_relaxed(readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_32COUNTER_0_CTRL + 4 * idx) & ~0x7,
+		sirfsoc_timer_base + SIRFSOC_TIMER_32COUNTER_0_CTRL + 4 * idx);
+}
+
+/* enable count and interrupt */
+static inline void sirfsoc_timer_count_enable(int idx)
+{
+	writel_relaxed(readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_32COUNTER_0_CTRL + 4 * idx) | 0x7,
+		sirfsoc_timer_base + SIRFSOC_TIMER_32COUNTER_0_CTRL + 4 * idx);
+}
+
+/* timer interrupt handler */
+static irqreturn_t sirfsoc_timer_interrupt(int irq, void *dev_id)
+{
+	struct clock_event_device *ce = dev_id;
+	int cpu = smp_processor_id();
+
+	/* clear timer interrupt */
+	writel_relaxed(BIT(cpu), sirfsoc_timer_base + SIRFSOC_TIMER_INTR_STATUS);
+
+	if (ce->mode == CLOCK_EVT_MODE_ONESHOT)
+		sirfsoc_timer_count_disable(cpu);
+
+	ce->event_handler(ce);
+
+	return IRQ_HANDLED;
+}
+
+/* read 64-bit timer counter */
+static cycle_t sirfsoc_timer_read(struct clocksource *cs)
+{
+	u64 cycles;
+
+	writel_relaxed((readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_64COUNTER_CTRL) |
+			BIT(0)) & ~BIT(1), sirfsoc_timer_base + SIRFSOC_TIMER_64COUNTER_CTRL);
+
+	cycles = readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_64COUNTER_RLATCHED_HI);
+	cycles = (cycles << 32) | readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_64COUNTER_RLATCHED_LO);
+
+	return cycles;
+}
+
+static int sirfsoc_timer_set_next_event(unsigned long delta,
+	struct clock_event_device *ce)
+{
+	int cpu = smp_processor_id();
+
+	writel_relaxed(0, sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_0 +
+		4 * cpu);
+	writel_relaxed(delta, sirfsoc_timer_base + SIRFSOC_TIMER_MATCH_0 +
+		4 * cpu);
+
+	/* enable the tick */
+	sirfsoc_timer_count_enable(cpu);
+
+	return 0;
+}
+
+static void sirfsoc_timer_set_mode(enum clock_event_mode mode,
+	struct clock_event_device *ce)
+{
+	switch (mode) {
+	case CLOCK_EVT_MODE_ONESHOT:
+		/* enable in set_next_event */
+		break;
+	default:
+		break;
+	}
+
+	sirfsoc_timer_count_disable(smp_processor_id());
+}
+
+static void sirfsoc_clocksource_suspend(struct clocksource *cs)
+{
+	int i;
+
+	for (i = 0; i < SIRFSOC_TIMER_REG_CNT; i++)
+		sirfsoc_timer_reg_val[i] = readl_relaxed(sirfsoc_timer_base + sirfsoc_timer_reg_list[i]);
+}
+
+static void sirfsoc_clocksource_resume(struct clocksource *cs)
+{
+	int i;
+
+	for (i = 0; i < SIRFSOC_TIMER_REG_CNT - 2; i++)
+		writel_relaxed(sirfsoc_timer_reg_val[i], sirfsoc_timer_base + sirfsoc_timer_reg_list[i]);
+
+	writel_relaxed(sirfsoc_timer_reg_val[SIRFSOC_TIMER_REG_CNT - 2],
+		sirfsoc_timer_base + SIRFSOC_TIMER_64COUNTER_LOAD_LO);
+	writel_relaxed(sirfsoc_timer_reg_val[SIRFSOC_TIMER_REG_CNT - 1],
+		sirfsoc_timer_base + SIRFSOC_TIMER_64COUNTER_LOAD_HI);
+
+	writel_relaxed(readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_64COUNTER_CTRL) |
+		BIT(1) | BIT(0), sirfsoc_timer_base + SIRFSOC_TIMER_64COUNTER_CTRL);
+}
+
+static struct clock_event_device sirfsoc_clockevent = {
+	.name = "sirfsoc_clockevent",
+	.rating = 200,
+	.features = CLOCK_EVT_FEAT_ONESHOT,
+	.set_mode = sirfsoc_timer_set_mode,
+	.set_next_event = sirfsoc_timer_set_next_event,
+};
+
+static struct clocksource sirfsoc_clocksource = {
+	.name = "sirfsoc_clocksource",
+	.rating = 200,
+	.mask = CLOCKSOURCE_MASK(64),
+	.flags = CLOCK_SOURCE_IS_CONTINUOUS,
+	.read = sirfsoc_timer_read,
+	.suspend = sirfsoc_clocksource_suspend,
+	.resume = sirfsoc_clocksource_resume,
+};
+
+static struct irqaction sirfsoc_timer_irq = {
+	.name = "sirfsoc_timer0",
+	.flags = IRQF_TIMER | IRQF_NOBALANCING,
+	.handler = sirfsoc_timer_interrupt,
+	.dev_id = &sirfsoc_clockevent,
+};
+
+#ifdef CONFIG_LOCAL_TIMERS
+
+static struct irqaction sirfsoc_timer1_irq = {
+	.name = "sirfsoc_timer1",
+	.flags = IRQF_TIMER | IRQF_NOBALANCING,
+	.handler = sirfsoc_timer_interrupt,
+};
+
+static int __cpuinit sirfsoc_local_timer_setup(struct clock_event_device *ce)
+{
+	/* Use existing clock_event for cpu 0 */
+	if (!smp_processor_id())
+		return 0;
+
+	ce->irq = sirfsoc_timer1_irq.irq;
+	ce->name = "local_timer";
+	ce->features = sirfsoc_clockevent.features;
+	ce->rating = sirfsoc_clockevent.rating;
+	ce->set_mode = sirfsoc_timer_set_mode;
+	ce->set_next_event = sirfsoc_timer_set_next_event;
+	ce->shift = sirfsoc_clockevent.shift;
+	ce->mult = sirfsoc_clockevent.mult;
+	ce->max_delta_ns = sirfsoc_clockevent.max_delta_ns;
+	ce->min_delta_ns = sirfsoc_clockevent.min_delta_ns;
+
+	sirfsoc_timer1_irq.dev_id = ce;
+	BUG_ON(setup_irq(ce->irq, &sirfsoc_timer1_irq));
+	irq_set_affinity(sirfsoc_timer1_irq.irq, cpumask_of(1));
+
+	clockevents_register_device(ce);
+	return 0;
+}
+
+static void sirfsoc_local_timer_stop(struct clock_event_device *ce)
+{
+	sirfsoc_timer_count_disable(1);
+
+	remove_irq(sirfsoc_timer1_irq.irq, &sirfsoc_timer1_irq);
+}
+
+static struct local_timer_ops sirfsoc_local_timer_ops __cpuinitdata = {
+	.setup	= sirfsoc_local_timer_setup,
+	.stop	= sirfsoc_local_timer_stop,
+};
+#endif /* CONFIG_LOCAL_TIMERS */
+
+static void __init sirfsoc_clockevent_init(void)
+{
+	clockevents_calc_mult_shift(&sirfsoc_clockevent, CLOCK_TICK_RATE, 60);
+
+	sirfsoc_clockevent.max_delta_ns =
+		clockevent_delta2ns(-2, &sirfsoc_clockevent);
+	sirfsoc_clockevent.min_delta_ns =
+		clockevent_delta2ns(2, &sirfsoc_clockevent);
+
+	sirfsoc_clockevent.cpumask = cpumask_of(0);
+	clockevents_register_device(&sirfsoc_clockevent);
+#ifdef CONFIG_LOCAL_TIMERS
+	local_timer_register(&sirfsoc_local_timer_ops);
+#endif
+}
+
+/* initialize the kernel jiffy timer source */
+static void __init sirfsoc_marco_timer_init(void)
+{
+	unsigned long rate;
+	u32 timer_div;
+	struct clk *clk;
+
+	/* timer's input clock is io clock */
+	clk = clk_get_sys("io", NULL);
+
+	BUG_ON(IS_ERR(clk));
+	rate = clk_get_rate(clk);
+
+	BUG_ON(rate < CLOCK_TICK_RATE);
+	BUG_ON(rate % CLOCK_TICK_RATE);
+
+	/* Initialize the timer dividers */
+	timer_div = rate / CLOCK_TICK_RATE - 1;
+	writel_relaxed(timer_div << 16, sirfsoc_timer_base + SIRFSOC_TIMER_64COUNTER_CTRL);
+	writel_relaxed(timer_div << 16, sirfsoc_timer_base + SIRFSOC_TIMER_32COUNTER_0_CTRL);
+	writel_relaxed(timer_div << 16, sirfsoc_timer_base + SIRFSOC_TIMER_32COUNTER_1_CTRL);
+
+	/* Initialize timer counters to 0 */
+	writel_relaxed(0, sirfsoc_timer_base + SIRFSOC_TIMER_64COUNTER_LOAD_LO);
+	writel_relaxed(0, sirfsoc_timer_base + SIRFSOC_TIMER_64COUNTER_LOAD_HI);
+	writel_relaxed(readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_64COUNTER_CTRL) |
+		BIT(1) | BIT(0), sirfsoc_timer_base + SIRFSOC_TIMER_64COUNTER_CTRL);
+	writel_relaxed(0, sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_0);
+	writel_relaxed(0, sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_1);
+
+	/* Clear all interrupts */
+	writel_relaxed(0xFFFF, sirfsoc_timer_base + SIRFSOC_TIMER_INTR_STATUS);
+
+	BUG_ON(clocksource_register_hz(&sirfsoc_clocksource, CLOCK_TICK_RATE));
+
+	BUG_ON(setup_irq(sirfsoc_timer_irq.irq, &sirfsoc_timer_irq));
+
+	sirfsoc_clockevent_init();
+}
+
+static void __init sirfsoc_of_timer_init(struct device_node *np)
+{
+	sirfsoc_timer_base = of_iomap(np, 0);
+	if (!sirfsoc_timer_base)
+		panic("unable to map timer cpu registers\n");
+
+	sirfsoc_timer_irq.irq = irq_of_parse_and_map(np, 0);
+	if (!sirfsoc_timer_irq.irq)
+		panic("No irq passed for timer0 via DT\n");
+
+#ifdef CONFIG_LOCAL_TIMERS
+	sirfsoc_timer1_irq.irq = irq_of_parse_and_map(np, 1);
+	if (!sirfsoc_timer1_irq.irq)
+		panic("No irq passed for timer1 via DT\n");
+#endif
+
+	sirfsoc_marco_timer_init();
+}
+CLOCKSOURCE_OF_DECLARE(sirfsoc_marco_timer, "sirf,marco-tick", sirfsoc_of_timer_init );
diff --git a/drivers/clocksource/timer-prima2.c b/drivers/clocksource/timer-prima2.c
new file mode 100644
index 000000000000..760882665d7a
--- /dev/null
+++ b/drivers/clocksource/timer-prima2.c
@@ -0,0 +1,215 @@
+/*
+ * System timer for CSR SiRFprimaII
+ *
+ * Copyright (c) 2011 Cambridge Silicon Radio Limited, a CSR plc group company.
+ *
+ * Licensed under GPLv2 or later.
+ */
+
+#include <linux/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/clockchips.h>
+#include <linux/clocksource.h>
+#include <linux/bitops.h>
+#include <linux/irq.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <asm/sched_clock.h>
+#include <asm/mach/time.h>
+
+#define SIRFSOC_TIMER_COUNTER_LO	0x0000
+#define SIRFSOC_TIMER_COUNTER_HI	0x0004
+#define SIRFSOC_TIMER_MATCH_0		0x0008
+#define SIRFSOC_TIMER_MATCH_1		0x000C
+#define SIRFSOC_TIMER_MATCH_2		0x0010
+#define SIRFSOC_TIMER_MATCH_3		0x0014
+#define SIRFSOC_TIMER_MATCH_4		0x0018
+#define SIRFSOC_TIMER_MATCH_5		0x001C
+#define SIRFSOC_TIMER_STATUS		0x0020
+#define SIRFSOC_TIMER_INT_EN		0x0024
+#define SIRFSOC_TIMER_WATCHDOG_EN	0x0028
+#define SIRFSOC_TIMER_DIV		0x002C
+#define SIRFSOC_TIMER_LATCH		0x0030
+#define SIRFSOC_TIMER_LATCHED_LO	0x0034
+#define SIRFSOC_TIMER_LATCHED_HI	0x0038
+
+#define SIRFSOC_TIMER_WDT_INDEX		5
+
+#define SIRFSOC_TIMER_LATCH_BIT	 BIT(0)
+
+#define SIRFSOC_TIMER_REG_CNT 11
+
+static const u32 sirfsoc_timer_reg_list[SIRFSOC_TIMER_REG_CNT] = {
+	SIRFSOC_TIMER_MATCH_0, SIRFSOC_TIMER_MATCH_1, SIRFSOC_TIMER_MATCH_2,
+	SIRFSOC_TIMER_MATCH_3, SIRFSOC_TIMER_MATCH_4, SIRFSOC_TIMER_MATCH_5,
+	SIRFSOC_TIMER_INT_EN, SIRFSOC_TIMER_WATCHDOG_EN, SIRFSOC_TIMER_DIV,
+	SIRFSOC_TIMER_LATCHED_LO, SIRFSOC_TIMER_LATCHED_HI,
+};
+
+static u32 sirfsoc_timer_reg_val[SIRFSOC_TIMER_REG_CNT];
+
+static void __iomem *sirfsoc_timer_base;
+
+/* timer0 interrupt handler */
+static irqreturn_t sirfsoc_timer_interrupt(int irq, void *dev_id)
+{
+	struct clock_event_device *ce = dev_id;
+
+	WARN_ON(!(readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_STATUS) & BIT(0)));
+
+	/* clear timer0 interrupt */
+	writel_relaxed(BIT(0), sirfsoc_timer_base + SIRFSOC_TIMER_STATUS);
+
+	ce->event_handler(ce);
+
+	return IRQ_HANDLED;
+}
+
+/* read 64-bit timer counter */
+static cycle_t sirfsoc_timer_read(struct clocksource *cs)
+{
+	u64 cycles;
+
+	/* latch the 64-bit timer counter */
+	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT, sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
+	cycles = readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_LATCHED_HI);
+	cycles = (cycles << 32) | readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_LATCHED_LO);
+
+	return cycles;
+}
+
+static int sirfsoc_timer_set_next_event(unsigned long delta,
+	struct clock_event_device *ce)
+{
+	unsigned long now, next;
+
+	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT, sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
+	now = readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_LATCHED_LO);
+	next = now + delta;
+	writel_relaxed(next, sirfsoc_timer_base + SIRFSOC_TIMER_MATCH_0);
+	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT, sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
+	now = readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_LATCHED_LO);
+
+	return next - now > delta ? -ETIME : 0;
+}
+
+static void sirfsoc_timer_set_mode(enum clock_event_mode mode,
+	struct clock_event_device *ce)
+{
+	u32 val = readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_INT_EN);
+	switch (mode) {
+	case CLOCK_EVT_MODE_PERIODIC:
+		WARN_ON(1);
+		break;
+	case CLOCK_EVT_MODE_ONESHOT:
+		writel_relaxed(val | BIT(0), sirfsoc_timer_base + SIRFSOC_TIMER_INT_EN);
+		break;
+	case CLOCK_EVT_MODE_SHUTDOWN:
+		writel_relaxed(val & ~BIT(0), sirfsoc_timer_base + SIRFSOC_TIMER_INT_EN);
+		break;
+	case CLOCK_EVT_MODE_UNUSED:
+	case CLOCK_EVT_MODE_RESUME:
+		break;
+	}
+}
+
+static void sirfsoc_clocksource_suspend(struct clocksource *cs)
+{
+	int i;
+
+	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT, sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
+
+	for (i = 0; i < SIRFSOC_TIMER_REG_CNT; i++)
+		sirfsoc_timer_reg_val[i] = readl_relaxed(sirfsoc_timer_base + sirfsoc_timer_reg_list[i]);
+}
+
+static void sirfsoc_clocksource_resume(struct clocksource *cs)
+{
+	int i;
+
+	for (i = 0; i < SIRFSOC_TIMER_REG_CNT - 2; i++)
+		writel_relaxed(sirfsoc_timer_reg_val[i], sirfsoc_timer_base + sirfsoc_timer_reg_list[i]);
+
+	writel_relaxed(sirfsoc_timer_reg_val[SIRFSOC_TIMER_REG_CNT - 2], sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_LO);
+	writel_relaxed(sirfsoc_timer_reg_val[SIRFSOC_TIMER_REG_CNT - 1], sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_HI);
+}
+
+static struct clock_event_device sirfsoc_clockevent = {
+	.name = "sirfsoc_clockevent",
+	.rating = 200,
+	.features = CLOCK_EVT_FEAT_ONESHOT,
+	.set_mode = sirfsoc_timer_set_mode,
+	.set_next_event = sirfsoc_timer_set_next_event,
+};
+
+static struct clocksource sirfsoc_clocksource = {
+	.name = "sirfsoc_clocksource",
+	.rating = 200,
+	.mask = CLOCKSOURCE_MASK(64),
+	.flags = CLOCK_SOURCE_IS_CONTINUOUS,
+	.read = sirfsoc_timer_read,
+	.suspend = sirfsoc_clocksource_suspend,
+	.resume = sirfsoc_clocksource_resume,
+};
+
+static struct irqaction sirfsoc_timer_irq = {
+	.name = "sirfsoc_timer0",
+	.flags = IRQF_TIMER,
+	.irq = 0,
+	.handler = sirfsoc_timer_interrupt,
+	.dev_id = &sirfsoc_clockevent,
+};
+
+/* Overwrite weak default sched_clock with more precise one */
+static u32 notrace sirfsoc_read_sched_clock(void)
+{
+	return (u32)(sirfsoc_timer_read(NULL) & 0xffffffff);
+}
+
+static void __init sirfsoc_clockevent_init(void)
+{
+	sirfsoc_clockevent.cpumask = cpumask_of(0);
+	clockevents_config_and_register(&sirfsoc_clockevent, CLOCK_TICK_RATE,
+					2, -2);
+}
+
+/* initialize the kernel jiffy timer source */
+static void __init sirfsoc_prima2_timer_init(struct device_node *np)
+{
+	unsigned long rate;
+	struct clk *clk;
+
+	/* timer's input clock is io clock */
+	clk = clk_get_sys("io", NULL);
+
+	BUG_ON(IS_ERR(clk));
+
+	rate = clk_get_rate(clk);
+
+	BUG_ON(rate < CLOCK_TICK_RATE);
+	BUG_ON(rate % CLOCK_TICK_RATE);
+
+	sirfsoc_timer_base = of_iomap(np, 0);
+	if (!sirfsoc_timer_base)
+		panic("unable to map timer cpu registers\n");
+
+	sirfsoc_timer_irq.irq = irq_of_parse_and_map(np, 0);
+
+	writel_relaxed(rate / CLOCK_TICK_RATE / 2 - 1, sirfsoc_timer_base + SIRFSOC_TIMER_DIV);
+	writel_relaxed(0, sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_LO);
+	writel_relaxed(0, sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_HI);
+	writel_relaxed(BIT(0), sirfsoc_timer_base + SIRFSOC_TIMER_STATUS);
+
+	BUG_ON(clocksource_register_hz(&sirfsoc_clocksource, CLOCK_TICK_RATE));
+
+	setup_sched_clock(sirfsoc_read_sched_clock, 32, CLOCK_TICK_RATE);
+
+	BUG_ON(setup_irq(sirfsoc_timer_irq.irq, &sirfsoc_timer_irq));
+
+	sirfsoc_clockevent_init();
+}
+CLOCKSOURCE_OF_DECLARE(sirfsoc_prima2_timer, "sirf,prima2-tick", sirfsoc_prima2_timer_init);
diff --git a/drivers/clocksource/vt8500_timer.c b/drivers/clocksource/vt8500_timer.c
index 8efc86b5b5dd..64f553f04fa4 100644
--- a/drivers/clocksource/vt8500_timer.c
+++ b/drivers/clocksource/vt8500_timer.c
@@ -129,22 +129,10 @@ static struct irqaction irq = {
 	.dev_id  = &clockevent,
 };
 
-static struct of_device_id vt8500_timer_ids[] = {
-	{ .compatible = "via,vt8500-timer" },
-	{ }
-};
-
-static void __init vt8500_timer_init(void)
+static void __init vt8500_timer_init(struct device_node *np)
 {
-	struct device_node *np;
 	int timer_irq;
 
-	np = of_find_matching_node(NULL, vt8500_timer_ids);
-	if (!np) {
-		pr_err("%s: Timer description missing from Device Tree\n",
-								__func__);
-		return;
-	}
 	regbase = of_iomap(np, 0);
 	if (!regbase) {
 		pr_err("%s: Missing iobase description in Device Tree\n",
@@ -177,4 +165,4 @@ static void __init vt8500_timer_init(void)
 					4, 0xf0000000);
 }
 
-CLOCKSOURCE_OF_DECLARE(vt8500, "via,vt8500-timer", vt8500_timer_init)
+CLOCKSOURCE_OF_DECLARE(vt8500, "via,vt8500-timer", vt8500_timer_init);
diff --git a/drivers/connector/cn_proc.c b/drivers/connector/cn_proc.c
index 1110478dd0fd..08ae128cce9b 100644
--- a/drivers/connector/cn_proc.c
+++ b/drivers/connector/cn_proc.c
@@ -232,6 +232,31 @@ void proc_comm_connector(struct task_struct *task)
 	cn_netlink_send(msg, CN_IDX_PROC, GFP_KERNEL);
 }
 
+void proc_coredump_connector(struct task_struct *task)
+{
+	struct cn_msg *msg;
+	struct proc_event *ev;
+	__u8 buffer[CN_PROC_MSG_SIZE];
+	struct timespec ts;
+
+	if (atomic_read(&proc_event_num_listeners) < 1)
+		return;
+
+	msg = (struct cn_msg *)buffer;
+	ev = (struct proc_event *)msg->data;
+	get_seq(&msg->seq, &ev->cpu);
+	ktime_get_ts(&ts); /* get high res monotonic timestamp */
+	put_unaligned(timespec_to_ns(&ts), (__u64 *)&ev->timestamp_ns);
+	ev->what = PROC_EVENT_COREDUMP;
+	ev->event_data.coredump.process_pid = task->pid;
+	ev->event_data.coredump.process_tgid = task->tgid;
+
+	memcpy(&msg->id, &cn_proc_event_id, sizeof(msg->id));
+	msg->ack = 0; /* not used */
+	msg->len = sizeof(*ev);
+	cn_netlink_send(msg, CN_IDX_PROC, GFP_KERNEL);
+}
+
 void proc_exit_connector(struct task_struct *task)
 {
 	struct cn_msg *msg;
diff --git a/drivers/connector/connector.c b/drivers/connector/connector.c
index f1b7e244bfc1..6ecfa758942c 100644
--- a/drivers/connector/connector.c
+++ b/drivers/connector/connector.c
@@ -23,7 +23,7 @@
 #include <linux/module.h>
 #include <linux/list.h>
 #include <linux/skbuff.h>
-#include <linux/netlink.h>
+#include <net/netlink.h>
 #include <linux/moduleparam.h>
 #include <linux/connector.h>
 #include <linux/slab.h>
@@ -95,13 +95,13 @@ int cn_netlink_send(struct cn_msg *msg, u32 __group, gfp_t gfp_mask)
 	if (!netlink_has_listeners(dev->nls, group))
 		return -ESRCH;
 
-	size = NLMSG_SPACE(sizeof(*msg) + msg->len);
+	size = sizeof(*msg) + msg->len;
 
-	skb = alloc_skb(size, gfp_mask);
+	skb = nlmsg_new(size, gfp_mask);
 	if (!skb)
 		return -ENOMEM;
 
-	nlh = nlmsg_put(skb, 0, msg->seq, NLMSG_DONE, size - sizeof(*nlh), 0);
+	nlh = nlmsg_put(skb, 0, msg->seq, NLMSG_DONE, size, 0);
 	if (!nlh) {
 		kfree_skb(skb);
 		return -EMSGSIZE;
@@ -124,7 +124,7 @@ static int cn_call_callback(struct sk_buff *skb)
 {
 	struct cn_callback_entry *i, *cbq = NULL;
 	struct cn_dev *dev = &cdev;
-	struct cn_msg *msg = NLMSG_DATA(nlmsg_hdr(skb));
+	struct cn_msg *msg = nlmsg_data(nlmsg_hdr(skb));
 	struct netlink_skb_parms *nsp = &NETLINK_CB(skb);
 	int err = -ENODEV;
 
@@ -162,7 +162,7 @@ static void cn_rx_skb(struct sk_buff *__skb)
 
 	skb = skb_get(__skb);
 
-	if (skb->len >= NLMSG_SPACE(0)) {
+	if (skb->len >= NLMSG_HDRLEN) {
 		nlh = nlmsg_hdr(skb);
 
 		if (nlh->nlmsg_len < sizeof(struct cn_msg) ||
diff --git a/drivers/cpufreq/Kconfig b/drivers/cpufreq/Kconfig
index cbcb21e32771..a1488f58f6ca 100644
--- a/drivers/cpufreq/Kconfig
+++ b/drivers/cpufreq/Kconfig
@@ -205,10 +205,99 @@ depends on ARM
 source "drivers/cpufreq/Kconfig.arm"
 endmenu
 
+menu "AVR32 CPU frequency scaling drivers"
+depends on AVR32
+
+config AVR32_AT32AP_CPUFREQ
+	bool "CPU frequency driver for AT32AP"
+	depends on PLATFORM_AT32AP
+	default n
+	help
+	  This enables the CPU frequency driver for AT32AP processors.
+	  If in doubt, say N.
+
+endmenu
+
+menu "CPUFreq processor drivers"
+depends on IA64
+
+config IA64_ACPI_CPUFREQ
+	tristate "ACPI Processor P-States driver"
+	select CPU_FREQ_TABLE
+	depends on ACPI_PROCESSOR
+	help
+	This driver adds a CPUFreq driver which utilizes the ACPI
+	Processor Performance States.
+
+	For details, take a look at <file:Documentation/cpu-freq/>.
+
+	If in doubt, say N.
+
+endmenu
+
+menu "MIPS CPUFreq processor drivers"
+depends on MIPS
+
+config LOONGSON2_CPUFREQ
+	tristate "Loongson2 CPUFreq Driver"
+	select CPU_FREQ_TABLE
+	help
+	  This option adds a CPUFreq driver for loongson processors which
+	  support software configurable cpu frequency.
+
+	  Loongson2F and it's successors support this feature.
+
+	  For details, take a look at <file:Documentation/cpu-freq/>.
+
+	  If in doubt, say N.
+
+endmenu
+
 menu "PowerPC CPU frequency scaling drivers"
 depends on PPC32 || PPC64
 source "drivers/cpufreq/Kconfig.powerpc"
 endmenu
 
+menu "SPARC CPU frequency scaling drivers"
+depends on SPARC64
+config SPARC_US3_CPUFREQ
+	tristate "UltraSPARC-III CPU Frequency driver"
+	select CPU_FREQ_TABLE
+	help
+	  This adds the CPUFreq driver for UltraSPARC-III processors.
+
+	  For details, take a look at <file:Documentation/cpu-freq>.
+
+	  If in doubt, say N.
+
+config SPARC_US2E_CPUFREQ
+	tristate "UltraSPARC-IIe CPU Frequency driver"
+	select CPU_FREQ_TABLE
+	help
+	  This adds the CPUFreq driver for UltraSPARC-IIe processors.
+
+	  For details, take a look at <file:Documentation/cpu-freq>.
+
+	  If in doubt, say N.
+endmenu
+
+menu "SH CPU Frequency scaling"
+depends on SUPERH
+config SH_CPU_FREQ
+	tristate "SuperH CPU Frequency driver"
+	select CPU_FREQ_TABLE
+	help
+	  This adds the cpufreq driver for SuperH. Any CPU that supports
+	  clock rate rounding through the clock framework can use this
+	  driver. While it will make the kernel slightly larger, this is
+	  harmless for CPUs that don't support rate rounding. The driver
+	  will also generate a notice in the boot log before disabling
+	  itself if the CPU in question is not capable of rate rounding.
+
+	  For details, take a look at <file:Documentation/cpu-freq>.
+
+	  If unsure, say N.
+endmenu
+
 endif
 endmenu
diff --git a/drivers/cpufreq/Kconfig.arm b/drivers/cpufreq/Kconfig.arm
index 030ddf6dd3f1..f3af18b9acc5 100644
--- a/drivers/cpufreq/Kconfig.arm
+++ b/drivers/cpufreq/Kconfig.arm
@@ -2,6 +2,93 @@
 # ARM CPU Frequency scaling drivers
 #
 
+config ARM_BIG_LITTLE_CPUFREQ
+	tristate
+	depends on ARM_CPU_TOPOLOGY
+
+config ARM_DT_BL_CPUFREQ
+	tristate "Generic ARM big LITTLE CPUfreq driver probed via DT"
+	select ARM_BIG_LITTLE_CPUFREQ
+	depends on OF && HAVE_CLK
+	help
+	  This enables the Generic CPUfreq driver for ARM big.LITTLE platform.
+	  This gets frequency tables from DT.
+
+config ARM_EXYNOS_CPUFREQ
+	bool "SAMSUNG EXYNOS SoCs"
+	depends on ARCH_EXYNOS
+	default y
+	help
+	  This adds the CPUFreq driver common part for Samsung
+	  EXYNOS SoCs.
+
+	  If in doubt, say N.
+
+config ARM_EXYNOS4210_CPUFREQ
+	def_bool CPU_EXYNOS4210
+	help
+	  This adds the CPUFreq driver for Samsung EXYNOS4210
+	  SoC (S5PV310 or S5PC210).
+
+config ARM_EXYNOS4X12_CPUFREQ
+	def_bool (SOC_EXYNOS4212 || SOC_EXYNOS4412)
+	help
+	  This adds the CPUFreq driver for Samsung EXYNOS4X12
+	  SoC (EXYNOS4212 or EXYNOS4412).
+
+config ARM_EXYNOS5250_CPUFREQ
+	def_bool SOC_EXYNOS5250
+	help
+	  This adds the CPUFreq driver for Samsung EXYNOS5250
+	  SoC.
+
+config ARM_EXYNOS5440_CPUFREQ
+	def_bool SOC_EXYNOS5440
+	depends on HAVE_CLK && PM_OPP && OF
+	help
+	  This adds the CPUFreq driver for Samsung EXYNOS5440
+	  SoC. The nature of exynos5440 clock controller is
+	  different than previous exynos controllers so not using
+	  the common exynos framework.
+
+config ARM_HIGHBANK_CPUFREQ
+	tristate "Calxeda Highbank-based"
+	depends on ARCH_HIGHBANK
+	select CPU_FREQ_TABLE
+	select GENERIC_CPUFREQ_CPU0
+	select PM_OPP
+	select REGULATOR
+
+	default m
+	help
+	  This adds the CPUFreq driver for Calxeda Highbank SoC
+	  based boards.
+
+	  If in doubt, say N.
+
+config ARM_IMX6Q_CPUFREQ
+	tristate "Freescale i.MX6Q cpufreq support"
+	depends on SOC_IMX6Q
+	depends on REGULATOR_ANATOP
+	help
+	  This adds cpufreq driver support for Freescale i.MX6Q SOC.
+
+	  If in doubt, say N.
+
+config ARM_INTEGRATOR
+	tristate "CPUfreq driver for ARM Integrator CPUs"
+	depends on ARCH_INTEGRATOR
+	default y
+	help
+	  This enables the CPUfreq driver for ARM Integrator CPUs.
+	  If in doubt, say Y.
+
+config ARM_KIRKWOOD_CPUFREQ
+	def_bool ARCH_KIRKWOOD && OF
+	help
+	  This adds the CPUFreq driver for Marvell Kirkwood
+	  SoCs.
+
 config ARM_OMAP2PLUS_CPUFREQ
 	bool "TI OMAP2+"
 	depends on ARCH_OMAP2PLUS
@@ -42,6 +129,7 @@ config ARM_S3C64XX_CPUFREQ
 config ARM_S5PV210_CPUFREQ
 	bool "Samsung S5PV210 and S5PC110"
 	depends on CPU_S5PV210
+	select CPU_FREQ_TABLE
 	default y
 	help
 	  This adds the CPUFreq driver for Samsung S5PV210 and
@@ -49,48 +137,11 @@ config ARM_S5PV210_CPUFREQ
 
 	  If in doubt, say N.
 
-config ARM_EXYNOS_CPUFREQ
-	bool "SAMSUNG EXYNOS SoCs"
-	depends on ARCH_EXYNOS
-	default y
-	help
-	  This adds the CPUFreq driver common part for Samsung
-	  EXYNOS SoCs.
-
-	  If in doubt, say N.
+config ARM_SA1100_CPUFREQ
+	bool
 
-config ARM_EXYNOS4210_CPUFREQ
-	def_bool CPU_EXYNOS4210
-	help
-	  This adds the CPUFreq driver for Samsung EXYNOS4210
-	  SoC (S5PV310 or S5PC210).
-
-config ARM_EXYNOS4X12_CPUFREQ
-	def_bool (SOC_EXYNOS4212 || SOC_EXYNOS4412)
-	help
-	  This adds the CPUFreq driver for Samsung EXYNOS4X12
-	  SoC (EXYNOS4212 or EXYNOS4412).
-
-config ARM_EXYNOS5250_CPUFREQ
-	def_bool SOC_EXYNOS5250
-	help
-	  This adds the CPUFreq driver for Samsung EXYNOS5250
-	  SoC.
-
-config ARM_KIRKWOOD_CPUFREQ
-	def_bool ARCH_KIRKWOOD && OF
-	help
-	  This adds the CPUFreq driver for Marvell Kirkwood
-	  SoCs.
-
-config ARM_IMX6Q_CPUFREQ
-	tristate "Freescale i.MX6Q cpufreq support"
-	depends on SOC_IMX6Q
-	depends on REGULATOR_ANATOP
-	help
-	  This adds cpufreq driver support for Freescale i.MX6Q SOC.
-
-	  If in doubt, say N.
+config ARM_SA1110_CPUFREQ
+	bool
 
 config ARM_SPEAR_CPUFREQ
 	bool "SPEAr CPUFreq support"
@@ -98,18 +149,3 @@ config ARM_SPEAR_CPUFREQ
 	default y
 	help
 	  This adds the CPUFreq driver support for SPEAr SOCs.
-
-config ARM_HIGHBANK_CPUFREQ
-	tristate "Calxeda Highbank-based"
-	depends on ARCH_HIGHBANK
-	select CPU_FREQ_TABLE
-	select GENERIC_CPUFREQ_CPU0
-	select PM_OPP
-	select REGULATOR
-
-	default m
-	help
-	  This adds the CPUFreq driver for Calxeda Highbank SoC
-	  based boards.
-
-	  If in doubt, say N.
diff --git a/drivers/cpufreq/Kconfig.powerpc b/drivers/cpufreq/Kconfig.powerpc
index e76992f79683..9c926ca0d718 100644
--- a/drivers/cpufreq/Kconfig.powerpc
+++ b/drivers/cpufreq/Kconfig.powerpc
@@ -1,3 +1,21 @@
+config CPU_FREQ_CBE
+	tristate "CBE frequency scaling"
+	depends on CBE_RAS && PPC_CELL
+	default m
+	help
+	  This adds the cpufreq driver for Cell BE processors.
+	  For details, take a look at <file:Documentation/cpu-freq/>.
+	  If you don't have such processor, say N
+
+config CPU_FREQ_CBE_PMI
+	bool "CBE frequency scaling using PMI interface"
+	depends on CPU_FREQ_CBE
+	default n
+	help
+	  Select this, if you want to use the PMI interface to switch
+	  frequencies. Using PMI, the processor will not only be able to run at
+	  lower speed, but also at lower core voltage.
+
 config CPU_FREQ_MAPLE
 	bool "Support for Maple 970FX Evaluation Board"
 	depends on PPC_MAPLE
diff --git a/drivers/cpufreq/Kconfig.x86 b/drivers/cpufreq/Kconfig.x86
index d7dc0ed6adb0..2b8a8c374548 100644
--- a/drivers/cpufreq/Kconfig.x86
+++ b/drivers/cpufreq/Kconfig.x86
@@ -129,6 +129,23 @@ config X86_POWERNOW_K8
 
 	  For details, take a look at <file:Documentation/cpu-freq/>.
 
+config X86_AMD_FREQ_SENSITIVITY
+	tristate "AMD frequency sensitivity feedback powersave bias"
+	depends on CPU_FREQ_GOV_ONDEMAND && X86_ACPI_CPUFREQ && CPU_SUP_AMD
+	help
+	  This adds AMD-specific powersave bias function to the ondemand
+	  governor, which allows it to make more power-conscious frequency
+	  change decisions based on feedback from hardware (availble on AMD
+	  Family 16h and above).
+
+	  Hardware feedback tells software how "sensitive" to frequency changes
+	  the CPUs' workloads are. CPU-bound workloads will be more sensitive
+	  -- they will perform better as frequency increases. Memory/IO-bound
+	  workloads will be less sensitive -- they will not necessarily perform
+	  better as frequency increases.
+
+	  If in doubt, say N.
+
 config X86_GX_SUSPMOD
 	tristate "Cyrix MediaGX/NatSemi Geode Suspend Modulation"
 	depends on X86_32 && PCI
diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
index 863fd1865d45..315b9231feb1 100644
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -41,23 +41,54 @@ obj-$(CONFIG_X86_SPEEDSTEP_CENTRINO)	+= speedstep-centrino.o
 obj-$(CONFIG_X86_P4_CLOCKMOD)		+= p4-clockmod.o
 obj-$(CONFIG_X86_CPUFREQ_NFORCE2)	+= cpufreq-nforce2.o
 obj-$(CONFIG_X86_INTEL_PSTATE)		+= intel_pstate.o
+obj-$(CONFIG_X86_AMD_FREQ_SENSITIVITY)	+= amd_freq_sensitivity.o
 
 ##################################################################################
 # ARM SoC drivers
+obj-$(CONFIG_ARM_BIG_LITTLE_CPUFREQ)	+= arm_big_little.o
+# big LITTLE per platform glues. Keep DT_BL_CPUFREQ as the last entry in all big
+# LITTLE drivers, so that it is probed last.
+obj-$(CONFIG_ARM_DT_BL_CPUFREQ)		+= arm_big_little_dt.o
+
+obj-$(CONFIG_ARCH_DAVINCI_DA850)	+= davinci-cpufreq.o
 obj-$(CONFIG_UX500_SOC_DB8500)		+= dbx500-cpufreq.o
-obj-$(CONFIG_ARM_S3C2416_CPUFREQ)	+= s3c2416-cpufreq.o
-obj-$(CONFIG_ARM_S3C64XX_CPUFREQ)	+= s3c64xx-cpufreq.o
-obj-$(CONFIG_ARM_S5PV210_CPUFREQ)	+= s5pv210-cpufreq.o
 obj-$(CONFIG_ARM_EXYNOS_CPUFREQ)	+= exynos-cpufreq.o
 obj-$(CONFIG_ARM_EXYNOS4210_CPUFREQ)	+= exynos4210-cpufreq.o
 obj-$(CONFIG_ARM_EXYNOS4X12_CPUFREQ)	+= exynos4x12-cpufreq.o
 obj-$(CONFIG_ARM_EXYNOS5250_CPUFREQ)	+= exynos5250-cpufreq.o
+obj-$(CONFIG_ARM_EXYNOS5440_CPUFREQ)	+= exynos5440-cpufreq.o
+obj-$(CONFIG_ARM_HIGHBANK_CPUFREQ)	+= highbank-cpufreq.o
+obj-$(CONFIG_ARM_IMX6Q_CPUFREQ)		+= imx6q-cpufreq.o
+obj-$(CONFIG_ARM_INTEGRATOR)		+= integrator-cpufreq.o
 obj-$(CONFIG_ARM_KIRKWOOD_CPUFREQ)	+= kirkwood-cpufreq.o
 obj-$(CONFIG_ARM_OMAP2PLUS_CPUFREQ)	+= omap-cpufreq.o
+obj-$(CONFIG_PXA25x)			+= pxa2xx-cpufreq.o
+obj-$(CONFIG_PXA27x)			+= pxa2xx-cpufreq.o
+obj-$(CONFIG_PXA3xx)			+= pxa3xx-cpufreq.o
+obj-$(CONFIG_ARM_S3C2416_CPUFREQ)	+= s3c2416-cpufreq.o
+obj-$(CONFIG_ARM_S3C64XX_CPUFREQ)	+= s3c64xx-cpufreq.o
+obj-$(CONFIG_ARM_S5PV210_CPUFREQ)	+= s5pv210-cpufreq.o
+obj-$(CONFIG_ARM_SA1100_CPUFREQ)	+= sa1100-cpufreq.o
+obj-$(CONFIG_ARM_SA1110_CPUFREQ)	+= sa1110-cpufreq.o
 obj-$(CONFIG_ARM_SPEAR_CPUFREQ)		+= spear-cpufreq.o
-obj-$(CONFIG_ARM_HIGHBANK_CPUFREQ)	+= highbank-cpufreq.o
-obj-$(CONFIG_ARM_IMX6Q_CPUFREQ)		+= imx6q-cpufreq.o
+obj-$(CONFIG_ARCH_TEGRA)		+= tegra-cpufreq.o
 
 ##################################################################################
 # PowerPC platform drivers
+obj-$(CONFIG_CPU_FREQ_CBE)		+= ppc-cbe-cpufreq.o
+ppc-cbe-cpufreq-y			+= ppc_cbe_cpufreq_pervasive.o ppc_cbe_cpufreq.o
+obj-$(CONFIG_CPU_FREQ_CBE_PMI)		+= ppc_cbe_cpufreq_pmi.o
 obj-$(CONFIG_CPU_FREQ_MAPLE)		+= maple-cpufreq.o
+
+##################################################################################
+# Other platform drivers
+obj-$(CONFIG_AVR32_AT32AP_CPUFREQ)	+= at32ap-cpufreq.o
+obj-$(CONFIG_BLACKFIN)			+= blackfin-cpufreq.o
+obj-$(CONFIG_CRIS_MACH_ARTPEC3)		+= cris-artpec3-cpufreq.o
+obj-$(CONFIG_ETRAXFS)			+= cris-etraxfs-cpufreq.o
+obj-$(CONFIG_IA64_ACPI_CPUFREQ)		+= ia64-acpi-cpufreq.o
+obj-$(CONFIG_LOONGSON2_CPUFREQ)		+= loongson2_cpufreq.o
+obj-$(CONFIG_SH_CPU_FREQ)		+= sh-cpufreq.o
+obj-$(CONFIG_SPARC_US2E_CPUFREQ)	+= sparc-us2e-cpufreq.o
+obj-$(CONFIG_SPARC_US3_CPUFREQ)		+= sparc-us3-cpufreq.o
+obj-$(CONFIG_UNICORE32)			+= unicore2-cpufreq.o
diff --git a/drivers/cpufreq/acpi-cpufreq.c b/drivers/cpufreq/acpi-cpufreq.c
index 57a8774f0b4e..11b8b4b54ceb 100644
--- a/drivers/cpufreq/acpi-cpufreq.c
+++ b/drivers/cpufreq/acpi-cpufreq.c
@@ -423,7 +423,6 @@ static int acpi_cpufreq_target(struct cpufreq_policy *policy,
 	struct drv_cmd cmd;
 	unsigned int next_state = 0; /* Index into freq_table */
 	unsigned int next_perf_state = 0; /* Index into perf table */
-	unsigned int i;
 	int result = 0;
 
 	pr_debug("acpi_cpufreq_target %d (%d)\n", target_freq, policy->cpu);
@@ -486,10 +485,7 @@ static int acpi_cpufreq_target(struct cpufreq_policy *policy,
 
 	freqs.old = perf->states[perf->state].core_frequency * 1000;
 	freqs.new = data->freq_table[next_state].frequency;
-	for_each_cpu(i, policy->cpus) {
-		freqs.cpu = i;
-		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
-	}
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	drv_write(&cmd);
 
@@ -502,10 +498,7 @@ static int acpi_cpufreq_target(struct cpufreq_policy *policy,
 		}
 	}
 
-	for_each_cpu(i, policy->cpus) {
-		freqs.cpu = i;
-		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
-	}
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 	perf->state = next_perf_state;
 
 out:
diff --git a/drivers/cpufreq/amd_freq_sensitivity.c b/drivers/cpufreq/amd_freq_sensitivity.c
new file mode 100644
index 000000000000..f6b79ab0070b
--- /dev/null
+++ b/drivers/cpufreq/amd_freq_sensitivity.c
@@ -0,0 +1,148 @@
+/*
+ * amd_freq_sensitivity.c: AMD frequency sensitivity feedback powersave bias
+ *                         for the ondemand governor.
+ *
+ * Copyright (C) 2013 Advanced Micro Devices, Inc.
+ *
+ * Author: Jacob Shin <jacob.shin@amd.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/percpu-defs.h>
+#include <linux/init.h>
+#include <linux/mod_devicetable.h>
+
+#include <asm/msr.h>
+#include <asm/cpufeature.h>
+
+#include "cpufreq_governor.h"
+
+#define MSR_AMD64_FREQ_SENSITIVITY_ACTUAL	0xc0010080
+#define MSR_AMD64_FREQ_SENSITIVITY_REFERENCE	0xc0010081
+#define CLASS_CODE_SHIFT			56
+#define POWERSAVE_BIAS_MAX			1000
+#define POWERSAVE_BIAS_DEF			400
+
+struct cpu_data_t {
+	u64 actual;
+	u64 reference;
+	unsigned int freq_prev;
+};
+
+static DEFINE_PER_CPU(struct cpu_data_t, cpu_data);
+
+static unsigned int amd_powersave_bias_target(struct cpufreq_policy *policy,
+					      unsigned int freq_next,
+					      unsigned int relation)
+{
+	int sensitivity;
+	long d_actual, d_reference;
+	struct msr actual, reference;
+	struct cpu_data_t *data = &per_cpu(cpu_data, policy->cpu);
+	struct dbs_data *od_data = policy->governor_data;
+	struct od_dbs_tuners *od_tuners = od_data->tuners;
+	struct od_cpu_dbs_info_s *od_info =
+		od_data->cdata->get_cpu_dbs_info_s(policy->cpu);
+
+	if (!od_info->freq_table)
+		return freq_next;
+
+	rdmsr_on_cpu(policy->cpu, MSR_AMD64_FREQ_SENSITIVITY_ACTUAL,
+		&actual.l, &actual.h);
+	rdmsr_on_cpu(policy->cpu, MSR_AMD64_FREQ_SENSITIVITY_REFERENCE,
+		&reference.l, &reference.h);
+	actual.h &= 0x00ffffff;
+	reference.h &= 0x00ffffff;
+
+	/* counter wrapped around, so stay on current frequency */
+	if (actual.q < data->actual || reference.q < data->reference) {
+		freq_next = policy->cur;
+		goto out;
+	}
+
+	d_actual = actual.q - data->actual;
+	d_reference = reference.q - data->reference;
+
+	/* divide by 0, so stay on current frequency as well */
+	if (d_reference == 0) {
+		freq_next = policy->cur;
+		goto out;
+	}
+
+	sensitivity = POWERSAVE_BIAS_MAX -
+		(POWERSAVE_BIAS_MAX * (d_reference - d_actual) / d_reference);
+
+	clamp(sensitivity, 0, POWERSAVE_BIAS_MAX);
+
+	/* this workload is not CPU bound, so choose a lower freq */
+	if (sensitivity < od_tuners->powersave_bias) {
+		if (data->freq_prev == policy->cur)
+			freq_next = policy->cur;
+
+		if (freq_next > policy->cur)
+			freq_next = policy->cur;
+		else if (freq_next < policy->cur)
+			freq_next = policy->min;
+		else {
+			unsigned int index;
+
+			cpufreq_frequency_table_target(policy,
+				od_info->freq_table, policy->cur - 1,
+				CPUFREQ_RELATION_H, &index);
+			freq_next = od_info->freq_table[index].frequency;
+		}
+
+		data->freq_prev = freq_next;
+	} else
+		data->freq_prev = 0;
+
+out:
+	data->actual = actual.q;
+	data->reference = reference.q;
+	return freq_next;
+}
+
+static int __init amd_freq_sensitivity_init(void)
+{
+	u64 val;
+
+	if (boot_cpu_data.x86_vendor != X86_VENDOR_AMD)
+		return -ENODEV;
+
+	if (!static_cpu_has(X86_FEATURE_PROC_FEEDBACK))
+		return -ENODEV;
+
+	if (rdmsrl_safe(MSR_AMD64_FREQ_SENSITIVITY_ACTUAL, &val))
+		return -ENODEV;
+
+	if (!(val >> CLASS_CODE_SHIFT))
+		return -ENODEV;
+
+	od_register_powersave_bias_handler(amd_powersave_bias_target,
+			POWERSAVE_BIAS_DEF);
+	return 0;
+}
+late_initcall(amd_freq_sensitivity_init);
+
+static void __exit amd_freq_sensitivity_exit(void)
+{
+	od_unregister_powersave_bias_handler();
+}
+module_exit(amd_freq_sensitivity_exit);
+
+static const struct x86_cpu_id amd_freq_sensitivity_ids[] = {
+	X86_FEATURE_MATCH(X86_FEATURE_PROC_FEEDBACK),
+	{}
+};
+MODULE_DEVICE_TABLE(x86cpu, amd_freq_sensitivity_ids);
+
+MODULE_AUTHOR("Jacob Shin <jacob.shin@amd.com>");
+MODULE_DESCRIPTION("AMD frequency sensitivity feedback powersave bias for "
+		"the ondemand governor.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/cpufreq/arm_big_little.c b/drivers/cpufreq/arm_big_little.c
new file mode 100644
index 000000000000..dbdf677d2f36
--- /dev/null
+++ b/drivers/cpufreq/arm_big_little.c
@@ -0,0 +1,278 @@
+/*
+ * ARM big.LITTLE Platforms CPUFreq support
+ *
+ * Copyright (C) 2013 ARM Ltd.
+ * Sudeep KarkadaNagesha <sudeep.karkadanagesha@arm.com>
+ *
+ * Copyright (C) 2013 Linaro.
+ * Viresh Kumar <viresh.kumar@linaro.org>
+ *
+ * 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 "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.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/clk.h>
+#include <linux/cpu.h>
+#include <linux/cpufreq.h>
+#include <linux/cpumask.h>
+#include <linux/export.h>
+#include <linux/of_platform.h>
+#include <linux/opp.h>
+#include <linux/slab.h>
+#include <linux/topology.h>
+#include <linux/types.h>
+
+#include "arm_big_little.h"
+
+/* Currently we support only two clusters */
+#define MAX_CLUSTERS	2
+
+static struct cpufreq_arm_bL_ops *arm_bL_ops;
+static struct clk *clk[MAX_CLUSTERS];
+static struct cpufreq_frequency_table *freq_table[MAX_CLUSTERS];
+static atomic_t cluster_usage[MAX_CLUSTERS] = {ATOMIC_INIT(0), ATOMIC_INIT(0)};
+
+static int cpu_to_cluster(int cpu)
+{
+	return topology_physical_package_id(cpu);
+}
+
+static unsigned int bL_cpufreq_get(unsigned int cpu)
+{
+	u32 cur_cluster = cpu_to_cluster(cpu);
+
+	return clk_get_rate(clk[cur_cluster]) / 1000;
+}
+
+/* Validate policy frequency range */
+static int bL_cpufreq_verify_policy(struct cpufreq_policy *policy)
+{
+	u32 cur_cluster = cpu_to_cluster(policy->cpu);
+
+	return cpufreq_frequency_table_verify(policy, freq_table[cur_cluster]);
+}
+
+/* Set clock frequency */
+static int bL_cpufreq_set_target(struct cpufreq_policy *policy,
+		unsigned int target_freq, unsigned int relation)
+{
+	struct cpufreq_freqs freqs;
+	u32 cpu = policy->cpu, freq_tab_idx, cur_cluster;
+	int ret = 0;
+
+	cur_cluster = cpu_to_cluster(policy->cpu);
+
+	freqs.old = bL_cpufreq_get(policy->cpu);
+
+	/* Determine valid target frequency using freq_table */
+	cpufreq_frequency_table_target(policy, freq_table[cur_cluster],
+			target_freq, relation, &freq_tab_idx);
+	freqs.new = freq_table[cur_cluster][freq_tab_idx].frequency;
+
+	pr_debug("%s: cpu: %d, cluster: %d, oldfreq: %d, target freq: %d, new freq: %d\n",
+			__func__, cpu, cur_cluster, freqs.old, target_freq,
+			freqs.new);
+
+	if (freqs.old == freqs.new)
+		return 0;
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+	ret = clk_set_rate(clk[cur_cluster], freqs.new * 1000);
+	if (ret) {
+		pr_err("clk_set_rate failed: %d\n", ret);
+		return ret;
+	}
+
+	policy->cur = freqs.new;
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	return ret;
+}
+
+static void put_cluster_clk_and_freq_table(struct device *cpu_dev)
+{
+	u32 cluster = cpu_to_cluster(cpu_dev->id);
+
+	if (!atomic_dec_return(&cluster_usage[cluster])) {
+		clk_put(clk[cluster]);
+		opp_free_cpufreq_table(cpu_dev, &freq_table[cluster]);
+		dev_dbg(cpu_dev, "%s: cluster: %d\n", __func__, cluster);
+	}
+}
+
+static int get_cluster_clk_and_freq_table(struct device *cpu_dev)
+{
+	u32 cluster = cpu_to_cluster(cpu_dev->id);
+	char name[14] = "cpu-cluster.";
+	int ret;
+
+	if (atomic_inc_return(&cluster_usage[cluster]) != 1)
+		return 0;
+
+	ret = arm_bL_ops->init_opp_table(cpu_dev);
+	if (ret) {
+		dev_err(cpu_dev, "%s: init_opp_table failed, cpu: %d, err: %d\n",
+				__func__, cpu_dev->id, ret);
+		goto atomic_dec;
+	}
+
+	ret = opp_init_cpufreq_table(cpu_dev, &freq_table[cluster]);
+	if (ret) {
+		dev_err(cpu_dev, "%s: failed to init cpufreq table, cpu: %d, err: %d\n",
+				__func__, cpu_dev->id, ret);
+		goto atomic_dec;
+	}
+
+	name[12] = cluster + '0';
+	clk[cluster] = clk_get_sys(name, NULL);
+	if (!IS_ERR(clk[cluster])) {
+		dev_dbg(cpu_dev, "%s: clk: %p & freq table: %p, cluster: %d\n",
+				__func__, clk[cluster], freq_table[cluster],
+				cluster);
+		return 0;
+	}
+
+	dev_err(cpu_dev, "%s: Failed to get clk for cpu: %d, cluster: %d\n",
+			__func__, cpu_dev->id, cluster);
+	ret = PTR_ERR(clk[cluster]);
+	opp_free_cpufreq_table(cpu_dev, &freq_table[cluster]);
+
+atomic_dec:
+	atomic_dec(&cluster_usage[cluster]);
+	dev_err(cpu_dev, "%s: Failed to get data for cluster: %d\n", __func__,
+			cluster);
+	return ret;
+}
+
+/* Per-CPU initialization */
+static int bL_cpufreq_init(struct cpufreq_policy *policy)
+{
+	u32 cur_cluster = cpu_to_cluster(policy->cpu);
+	struct device *cpu_dev;
+	int ret;
+
+	cpu_dev = get_cpu_device(policy->cpu);
+	if (!cpu_dev) {
+		pr_err("%s: failed to get cpu%d device\n", __func__,
+				policy->cpu);
+		return -ENODEV;
+	}
+
+	ret = get_cluster_clk_and_freq_table(cpu_dev);
+	if (ret)
+		return ret;
+
+	ret = cpufreq_frequency_table_cpuinfo(policy, freq_table[cur_cluster]);
+	if (ret) {
+		dev_err(cpu_dev, "CPU %d, cluster: %d invalid freq table\n",
+				policy->cpu, cur_cluster);
+		put_cluster_clk_and_freq_table(cpu_dev);
+		return ret;
+	}
+
+	cpufreq_frequency_table_get_attr(freq_table[cur_cluster], policy->cpu);
+
+	if (arm_bL_ops->get_transition_latency)
+		policy->cpuinfo.transition_latency =
+			arm_bL_ops->get_transition_latency(cpu_dev);
+	else
+		policy->cpuinfo.transition_latency = CPUFREQ_ETERNAL;
+
+	policy->cur = bL_cpufreq_get(policy->cpu);
+
+	cpumask_copy(policy->cpus, topology_core_cpumask(policy->cpu));
+
+	dev_info(cpu_dev, "CPU %d initialized\n", policy->cpu);
+	return 0;
+}
+
+static int bL_cpufreq_exit(struct cpufreq_policy *policy)
+{
+	struct device *cpu_dev;
+
+	cpu_dev = get_cpu_device(policy->cpu);
+	if (!cpu_dev) {
+		pr_err("%s: failed to get cpu%d device\n", __func__,
+				policy->cpu);
+		return -ENODEV;
+	}
+
+	put_cluster_clk_and_freq_table(cpu_dev);
+	dev_dbg(cpu_dev, "%s: Exited, cpu: %d\n", __func__, policy->cpu);
+
+	return 0;
+}
+
+/* Export freq_table to sysfs */
+static struct freq_attr *bL_cpufreq_attr[] = {
+	&cpufreq_freq_attr_scaling_available_freqs,
+	NULL,
+};
+
+static struct cpufreq_driver bL_cpufreq_driver = {
+	.name			= "arm-big-little",
+	.flags			= CPUFREQ_STICKY,
+	.verify			= bL_cpufreq_verify_policy,
+	.target			= bL_cpufreq_set_target,
+	.get			= bL_cpufreq_get,
+	.init			= bL_cpufreq_init,
+	.exit			= bL_cpufreq_exit,
+	.have_governor_per_policy = true,
+	.attr			= bL_cpufreq_attr,
+};
+
+int bL_cpufreq_register(struct cpufreq_arm_bL_ops *ops)
+{
+	int ret;
+
+	if (arm_bL_ops) {
+		pr_debug("%s: Already registered: %s, exiting\n", __func__,
+				arm_bL_ops->name);
+		return -EBUSY;
+	}
+
+	if (!ops || !strlen(ops->name) || !ops->init_opp_table) {
+		pr_err("%s: Invalid arm_bL_ops, exiting\n", __func__);
+		return -ENODEV;
+	}
+
+	arm_bL_ops = ops;
+
+	ret = cpufreq_register_driver(&bL_cpufreq_driver);
+	if (ret) {
+		pr_info("%s: Failed registering platform driver: %s, err: %d\n",
+				__func__, ops->name, ret);
+		arm_bL_ops = NULL;
+	} else {
+		pr_info("%s: Registered platform driver: %s\n", __func__,
+				ops->name);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(bL_cpufreq_register);
+
+void bL_cpufreq_unregister(struct cpufreq_arm_bL_ops *ops)
+{
+	if (arm_bL_ops != ops) {
+		pr_err("%s: Registered with: %s, can't unregister, exiting\n",
+				__func__, arm_bL_ops->name);
+		return;
+	}
+
+	cpufreq_unregister_driver(&bL_cpufreq_driver);
+	pr_info("%s: Un-registered platform driver: %s\n", __func__,
+			arm_bL_ops->name);
+	arm_bL_ops = NULL;
+}
+EXPORT_SYMBOL_GPL(bL_cpufreq_unregister);
diff --git a/drivers/cpufreq/arm_big_little.h b/drivers/cpufreq/arm_big_little.h
new file mode 100644
index 000000000000..70f18fc12d4a
--- /dev/null
+++ b/drivers/cpufreq/arm_big_little.h
@@ -0,0 +1,40 @@
+/*
+ * ARM big.LITTLE platform's CPUFreq header file
+ *
+ * Copyright (C) 2013 ARM Ltd.
+ * Sudeep KarkadaNagesha <sudeep.karkadanagesha@arm.com>
+ *
+ * Copyright (C) 2013 Linaro.
+ * Viresh Kumar <viresh.kumar@linaro.org>
+ *
+ * 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 "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.
+ */
+#ifndef CPUFREQ_ARM_BIG_LITTLE_H
+#define CPUFREQ_ARM_BIG_LITTLE_H
+
+#include <linux/cpufreq.h>
+#include <linux/device.h>
+#include <linux/types.h>
+
+struct cpufreq_arm_bL_ops {
+	char name[CPUFREQ_NAME_LEN];
+	int (*get_transition_latency)(struct device *cpu_dev);
+
+	/*
+	 * This must set opp table for cpu_dev in a similar way as done by
+	 * of_init_opp_table().
+	 */
+	int (*init_opp_table)(struct device *cpu_dev);
+};
+
+int bL_cpufreq_register(struct cpufreq_arm_bL_ops *ops);
+void bL_cpufreq_unregister(struct cpufreq_arm_bL_ops *ops);
+
+#endif /* CPUFREQ_ARM_BIG_LITTLE_H */
diff --git a/drivers/cpufreq/arm_big_little_dt.c b/drivers/cpufreq/arm_big_little_dt.c
new file mode 100644
index 000000000000..44be3115375c
--- /dev/null
+++ b/drivers/cpufreq/arm_big_little_dt.c
@@ -0,0 +1,107 @@
+/*
+ * Generic big.LITTLE CPUFreq Interface driver
+ *
+ * It provides necessary ops to arm_big_little cpufreq driver and gets
+ * Frequency information from Device Tree. Freq table in DT must be in KHz.
+ *
+ * Copyright (C) 2013 Linaro.
+ * Viresh Kumar <viresh.kumar@linaro.org>
+ *
+ * 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 "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.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/cpufreq.h>
+#include <linux/device.h>
+#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/opp.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include "arm_big_little.h"
+
+static int dt_init_opp_table(struct device *cpu_dev)
+{
+	struct device_node *np, *parent;
+	int count = 0, ret;
+
+	parent = of_find_node_by_path("/cpus");
+	if (!parent) {
+		pr_err("failed to find OF /cpus\n");
+		return -ENOENT;
+	}
+
+	for_each_child_of_node(parent, np) {
+		if (count++ != cpu_dev->id)
+			continue;
+		if (!of_get_property(np, "operating-points", NULL)) {
+			ret = -ENODATA;
+		} else {
+			cpu_dev->of_node = np;
+			ret = of_init_opp_table(cpu_dev);
+		}
+		of_node_put(np);
+		of_node_put(parent);
+
+		return ret;
+	}
+
+	return -ENODEV;
+}
+
+static int dt_get_transition_latency(struct device *cpu_dev)
+{
+	struct device_node *np, *parent;
+	u32 transition_latency = CPUFREQ_ETERNAL;
+	int count = 0;
+
+	parent = of_find_node_by_path("/cpus");
+	if (!parent) {
+		pr_err("failed to find OF /cpus\n");
+		return -ENOENT;
+	}
+
+	for_each_child_of_node(parent, np) {
+		if (count++ != cpu_dev->id)
+			continue;
+
+		of_property_read_u32(np, "clock-latency", &transition_latency);
+		of_node_put(np);
+		of_node_put(parent);
+
+		return 0;
+	}
+
+	return -ENODEV;
+}
+
+static struct cpufreq_arm_bL_ops dt_bL_ops = {
+	.name	= "dt-bl",
+	.get_transition_latency = dt_get_transition_latency,
+	.init_opp_table = dt_init_opp_table,
+};
+
+static int generic_bL_init(void)
+{
+	return bL_cpufreq_register(&dt_bL_ops);
+}
+module_init(generic_bL_init);
+
+static void generic_bL_exit(void)
+{
+	return bL_cpufreq_unregister(&dt_bL_ops);
+}
+module_exit(generic_bL_exit);
+
+MODULE_AUTHOR("Viresh Kumar <viresh.kumar@linaro.org>");
+MODULE_DESCRIPTION("Generic ARM big LITTLE cpufreq driver via DT");
+MODULE_LICENSE("GPL");
diff --git a/drivers/cpufreq/at32ap-cpufreq.c b/drivers/cpufreq/at32ap-cpufreq.c
new file mode 100644
index 000000000000..654488723cb5
--- /dev/null
+++ b/drivers/cpufreq/at32ap-cpufreq.c
@@ -0,0 +1,123 @@
+/*
+ * Copyright (C) 2004-2007 Atmel Corporation
+ *
+ * Based on MIPS implementation arch/mips/kernel/time.c
+ *   Copyright 2001 MontaVista Software Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+/*#define DEBUG*/
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/cpufreq.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/export.h>
+
+static struct clk *cpuclk;
+
+static int at32_verify_speed(struct cpufreq_policy *policy)
+{
+	if (policy->cpu != 0)
+		return -EINVAL;
+
+	cpufreq_verify_within_limits(policy, policy->cpuinfo.min_freq,
+			policy->cpuinfo.max_freq);
+	return 0;
+}
+
+static unsigned int at32_get_speed(unsigned int cpu)
+{
+	/* No SMP support */
+	if (cpu)
+		return 0;
+	return (unsigned int)((clk_get_rate(cpuclk) + 500) / 1000);
+}
+
+static unsigned int	ref_freq;
+static unsigned long	loops_per_jiffy_ref;
+
+static int at32_set_target(struct cpufreq_policy *policy,
+			  unsigned int target_freq,
+			  unsigned int relation)
+{
+	struct cpufreq_freqs freqs;
+	long freq;
+
+	/* Convert target_freq from kHz to Hz */
+	freq = clk_round_rate(cpuclk, target_freq * 1000);
+
+	/* Check if policy->min <= new_freq <= policy->max */
+	if(freq < (policy->min * 1000) || freq > (policy->max * 1000))
+		return -EINVAL;
+
+	pr_debug("cpufreq: requested frequency %u Hz\n", target_freq * 1000);
+
+	freqs.old = at32_get_speed(0);
+	freqs.new = (freq + 500) / 1000;
+	freqs.flags = 0;
+
+	if (!ref_freq) {
+		ref_freq = freqs.old;
+		loops_per_jiffy_ref = boot_cpu_data.loops_per_jiffy;
+	}
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+	if (freqs.old < freqs.new)
+		boot_cpu_data.loops_per_jiffy = cpufreq_scale(
+				loops_per_jiffy_ref, ref_freq, freqs.new);
+	clk_set_rate(cpuclk, freq);
+	if (freqs.new < freqs.old)
+		boot_cpu_data.loops_per_jiffy = cpufreq_scale(
+				loops_per_jiffy_ref, ref_freq, freqs.new);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	pr_debug("cpufreq: set frequency %lu Hz\n", freq);
+
+	return 0;
+}
+
+static int __init at32_cpufreq_driver_init(struct cpufreq_policy *policy)
+{
+	if (policy->cpu != 0)
+		return -EINVAL;
+
+	cpuclk = clk_get(NULL, "cpu");
+	if (IS_ERR(cpuclk)) {
+		pr_debug("cpufreq: could not get CPU clk\n");
+		return PTR_ERR(cpuclk);
+	}
+
+	policy->cpuinfo.min_freq = (clk_round_rate(cpuclk, 1) + 500) / 1000;
+	policy->cpuinfo.max_freq = (clk_round_rate(cpuclk, ~0UL) + 500) / 1000;
+	policy->cpuinfo.transition_latency = 0;
+	policy->cur = at32_get_speed(0);
+	policy->min = policy->cpuinfo.min_freq;
+	policy->max = policy->cpuinfo.max_freq;
+
+	printk("cpufreq: AT32AP CPU frequency driver\n");
+
+	return 0;
+}
+
+static struct cpufreq_driver at32_driver = {
+	.name		= "at32ap",
+	.owner		= THIS_MODULE,
+	.init		= at32_cpufreq_driver_init,
+	.verify		= at32_verify_speed,
+	.target		= at32_set_target,
+	.get		= at32_get_speed,
+	.flags		= CPUFREQ_STICKY,
+};
+
+static int __init at32_cpufreq_init(void)
+{
+	return cpufreq_register_driver(&at32_driver);
+}
+late_initcall(at32_cpufreq_init);
diff --git a/drivers/cpufreq/blackfin-cpufreq.c b/drivers/cpufreq/blackfin-cpufreq.c
new file mode 100644
index 000000000000..995511e80bef
--- /dev/null
+++ b/drivers/cpufreq/blackfin-cpufreq.c
@@ -0,0 +1,247 @@
+/*
+ * Blackfin core clock scaling
+ *
+ * Copyright 2008-2011 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/clk.h>
+#include <linux/cpufreq.h>
+#include <linux/fs.h>
+#include <linux/delay.h>
+#include <asm/blackfin.h>
+#include <asm/time.h>
+#include <asm/dpmc.h>
+
+
+/* this is the table of CCLK frequencies, in Hz */
+/* .index is the entry in the auxiliary dpm_state_table[] */
+static struct cpufreq_frequency_table bfin_freq_table[] = {
+	{
+		.frequency = CPUFREQ_TABLE_END,
+		.index = 0,
+	},
+	{
+		.frequency = CPUFREQ_TABLE_END,
+		.index = 1,
+	},
+	{
+		.frequency = CPUFREQ_TABLE_END,
+		.index = 2,
+	},
+	{
+		.frequency = CPUFREQ_TABLE_END,
+		.index = 0,
+	},
+};
+
+static struct bfin_dpm_state {
+	unsigned int csel; /* system clock divider */
+	unsigned int tscale; /* change the divider on the core timer interrupt */
+} dpm_state_table[3];
+
+#if defined(CONFIG_CYCLES_CLOCKSOURCE)
+/*
+ * normalized to maximum frequency offset for CYCLES,
+ * used in time-ts cycles clock source, but could be used
+ * somewhere also.
+ */
+unsigned long long __bfin_cycles_off;
+unsigned int __bfin_cycles_mod;
+#endif
+
+/**************************************************************************/
+static void __init bfin_init_tables(unsigned long cclk, unsigned long sclk)
+{
+
+	unsigned long csel, min_cclk;
+	int index;
+
+	/* Anomaly 273 seems to still exist on non-BF54x w/dcache turned on */
+#if ANOMALY_05000273 || ANOMALY_05000274 || \
+	(!(defined(CONFIG_BF54x) || defined(CONFIG_BF60x)) \
+	&& defined(CONFIG_BFIN_EXTMEM_DCACHEABLE))
+	min_cclk = sclk * 2;
+#else
+	min_cclk = sclk;
+#endif
+
+#ifndef CONFIG_BF60x
+	csel = ((bfin_read_PLL_DIV() & CSEL) >> 4);
+#else
+	csel = bfin_read32(CGU0_DIV) & 0x1F;
+#endif
+
+	for (index = 0;  (cclk >> index) >= min_cclk && csel <= 3 && index < 3; index++, csel++) {
+		bfin_freq_table[index].frequency = cclk >> index;
+#ifndef CONFIG_BF60x
+		dpm_state_table[index].csel = csel << 4; /* Shift now into PLL_DIV bitpos */
+#else
+		dpm_state_table[index].csel = csel;
+#endif
+		dpm_state_table[index].tscale =  (TIME_SCALE >> index) - 1;
+
+		pr_debug("cpufreq: freq:%d csel:0x%x tscale:%d\n",
+						 bfin_freq_table[index].frequency,
+						 dpm_state_table[index].csel,
+						 dpm_state_table[index].tscale);
+	}
+	return;
+}
+
+static void bfin_adjust_core_timer(void *info)
+{
+	unsigned int tscale;
+	unsigned int index = *(unsigned int *)info;
+
+	/* we have to adjust the core timer, because it is using cclk */
+	tscale = dpm_state_table[index].tscale;
+	bfin_write_TSCALE(tscale);
+	return;
+}
+
+static unsigned int bfin_getfreq_khz(unsigned int cpu)
+{
+	/* Both CoreA/B have the same core clock */
+	return get_cclk() / 1000;
+}
+
+#ifdef CONFIG_BF60x
+unsigned long cpu_set_cclk(int cpu, unsigned long new)
+{
+	struct clk *clk;
+	int ret;
+
+	clk = clk_get(NULL, "CCLK");
+	if (IS_ERR(clk))
+		return -ENODEV;
+
+	ret = clk_set_rate(clk, new);
+	clk_put(clk);
+	return ret;
+}
+#endif
+
+static int bfin_target(struct cpufreq_policy *policy,
+			unsigned int target_freq, unsigned int relation)
+{
+#ifndef CONFIG_BF60x
+	unsigned int plldiv;
+#endif
+	unsigned int index;
+	unsigned long cclk_hz;
+	struct cpufreq_freqs freqs;
+	static unsigned long lpj_ref;
+	static unsigned int  lpj_ref_freq;
+	int ret = 0;
+
+#if defined(CONFIG_CYCLES_CLOCKSOURCE)
+	cycles_t cycles;
+#endif
+
+	if (cpufreq_frequency_table_target(policy, bfin_freq_table, target_freq,
+				relation, &index))
+		return -EINVAL;
+
+	cclk_hz = bfin_freq_table[index].frequency;
+
+	freqs.old = bfin_getfreq_khz(0);
+	freqs.new = cclk_hz;
+
+	pr_debug("cpufreq: changing cclk to %lu; target = %u, oldfreq = %u\n",
+			cclk_hz, target_freq, freqs.old);
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+#ifndef CONFIG_BF60x
+	plldiv = (bfin_read_PLL_DIV() & SSEL) | dpm_state_table[index].csel;
+	bfin_write_PLL_DIV(plldiv);
+#else
+	ret = cpu_set_cclk(policy->cpu, freqs.new * 1000);
+	if (ret != 0) {
+		WARN_ONCE(ret, "cpufreq set freq failed %d\n", ret);
+		return ret;
+	}
+#endif
+	on_each_cpu(bfin_adjust_core_timer, &index, 1);
+#if defined(CONFIG_CYCLES_CLOCKSOURCE)
+	cycles = get_cycles();
+	SSYNC();
+	cycles += 10; /* ~10 cycles we lose after get_cycles() */
+	__bfin_cycles_off += (cycles << __bfin_cycles_mod) - (cycles << index);
+	__bfin_cycles_mod = index;
+#endif
+	if (!lpj_ref_freq) {
+		lpj_ref = loops_per_jiffy;
+		lpj_ref_freq = freqs.old;
+	}
+	if (freqs.new != freqs.old) {
+		loops_per_jiffy = cpufreq_scale(lpj_ref,
+				lpj_ref_freq, freqs.new);
+	}
+
+	/* TODO: just test case for cycles clock source, remove later */
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	pr_debug("cpufreq: done\n");
+	return ret;
+}
+
+static int bfin_verify_speed(struct cpufreq_policy *policy)
+{
+	return cpufreq_frequency_table_verify(policy, bfin_freq_table);
+}
+
+static int __bfin_cpu_init(struct cpufreq_policy *policy)
+{
+
+	unsigned long cclk, sclk;
+
+	cclk = get_cclk() / 1000;
+	sclk = get_sclk() / 1000;
+
+	if (policy->cpu == CPUFREQ_CPU)
+		bfin_init_tables(cclk, sclk);
+
+	policy->cpuinfo.transition_latency = 50000; /* 50us assumed */
+
+	policy->cur = cclk;
+	cpufreq_frequency_table_get_attr(bfin_freq_table, policy->cpu);
+	return cpufreq_frequency_table_cpuinfo(policy, bfin_freq_table);
+}
+
+static struct freq_attr *bfin_freq_attr[] = {
+	&cpufreq_freq_attr_scaling_available_freqs,
+	NULL,
+};
+
+static struct cpufreq_driver bfin_driver = {
+	.verify = bfin_verify_speed,
+	.target = bfin_target,
+	.get = bfin_getfreq_khz,
+	.init = __bfin_cpu_init,
+	.name = "bfin cpufreq",
+	.owner = THIS_MODULE,
+	.attr = bfin_freq_attr,
+};
+
+static int __init bfin_cpu_init(void)
+{
+	return cpufreq_register_driver(&bfin_driver);
+}
+
+static void __exit bfin_cpu_exit(void)
+{
+	cpufreq_unregister_driver(&bfin_driver);
+}
+
+MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
+MODULE_DESCRIPTION("cpufreq driver for Blackfin");
+MODULE_LICENSE("GPL");
+
+module_init(bfin_cpu_init);
+module_exit(bfin_cpu_exit);
diff --git a/drivers/cpufreq/cpufreq-cpu0.c b/drivers/cpufreq/cpufreq-cpu0.c
index 37d23a0f8c56..3ab8294eab04 100644
--- a/drivers/cpufreq/cpufreq-cpu0.c
+++ b/drivers/cpufreq/cpufreq-cpu0.c
@@ -44,8 +44,9 @@ static int cpu0_set_target(struct cpufreq_policy *policy,
 {
 	struct cpufreq_freqs freqs;
 	struct opp *opp;
-	unsigned long freq_Hz, volt = 0, volt_old = 0, tol = 0;
-	unsigned int index, cpu;
+	unsigned long volt = 0, volt_old = 0, tol = 0;
+	long freq_Hz;
+	unsigned int index;
 	int ret;
 
 	ret = cpufreq_frequency_table_target(policy, freq_table, target_freq,
@@ -65,10 +66,7 @@ static int cpu0_set_target(struct cpufreq_policy *policy,
 	if (freqs.old == freqs.new)
 		return 0;
 
-	for_each_online_cpu(cpu) {
-		freqs.cpu = cpu;
-		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
-	}
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	if (cpu_reg) {
 		rcu_read_lock();
@@ -76,7 +74,9 @@ static int cpu0_set_target(struct cpufreq_policy *policy,
 		if (IS_ERR(opp)) {
 			rcu_read_unlock();
 			pr_err("failed to find OPP for %ld\n", freq_Hz);
-			return PTR_ERR(opp);
+			freqs.new = freqs.old;
+			ret = PTR_ERR(opp);
+			goto post_notify;
 		}
 		volt = opp_get_voltage(opp);
 		rcu_read_unlock();
@@ -94,7 +94,7 @@ static int cpu0_set_target(struct cpufreq_policy *policy,
 		if (ret) {
 			pr_err("failed to scale voltage up: %d\n", ret);
 			freqs.new = freqs.old;
-			return ret;
+			goto post_notify;
 		}
 	}
 
@@ -103,7 +103,8 @@ static int cpu0_set_target(struct cpufreq_policy *policy,
 		pr_err("failed to set clock rate: %d\n", ret);
 		if (cpu_reg)
 			regulator_set_voltage_tol(cpu_reg, volt_old, tol);
-		return ret;
+		freqs.new = freqs.old;
+		goto post_notify;
 	}
 
 	/* scaling down?  scale voltage after frequency */
@@ -113,25 +114,19 @@ static int cpu0_set_target(struct cpufreq_policy *policy,
 			pr_err("failed to scale voltage down: %d\n", ret);
 			clk_set_rate(cpu_clk, freqs.old * 1000);
 			freqs.new = freqs.old;
-			return ret;
 		}
 	}
 
-	for_each_online_cpu(cpu) {
-		freqs.cpu = cpu;
-		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
-	}
+post_notify:
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
-	return 0;
+	return ret;
 }
 
 static int cpu0_cpufreq_init(struct cpufreq_policy *policy)
 {
 	int ret;
 
-	if (policy->cpu != 0)
-		return -EINVAL;
-
 	ret = cpufreq_frequency_table_cpuinfo(policy, freq_table);
 	if (ret) {
 		pr_err("invalid frequency table: %d\n", ret);
@@ -262,6 +257,7 @@ static int cpu0_cpufreq_probe(struct platform_device *pdev)
 	}
 
 	of_node_put(np);
+	of_node_put(parent);
 	return 0;
 
 out_free_table:
diff --git a/drivers/cpufreq/cpufreq-nforce2.c b/drivers/cpufreq/cpufreq-nforce2.c
index 13d311ee08b3..af1542d41440 100644
--- a/drivers/cpufreq/cpufreq-nforce2.c
+++ b/drivers/cpufreq/cpufreq-nforce2.c
@@ -263,7 +263,6 @@ static int nforce2_target(struct cpufreq_policy *policy,
 
 	freqs.old = nforce2_get(policy->cpu);
 	freqs.new = target_fsb * fid * 100;
-	freqs.cpu = 0;		/* Only one CPU on nForce2 platforms */
 
 	if (freqs.old == freqs.new)
 		return 0;
@@ -271,7 +270,7 @@ static int nforce2_target(struct cpufreq_policy *policy,
 	pr_debug("Old CPU frequency %d kHz, new %d kHz\n",
 	       freqs.old, freqs.new);
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	/* Disable IRQs */
 	/* local_irq_save(flags); */
@@ -286,7 +285,7 @@ static int nforce2_target(struct cpufreq_policy *policy,
 	/* Enable IRQs */
 	/* local_irq_restore(flags); */
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 	return 0;
 }
@@ -360,12 +359,10 @@ static int nforce2_cpu_init(struct cpufreq_policy *policy)
 		min_fsb = NFORCE2_MIN_FSB;
 
 	/* cpuinfo and default policy values */
-	policy->cpuinfo.min_freq = min_fsb * fid * 100;
-	policy->cpuinfo.max_freq = max_fsb * fid * 100;
+	policy->min = policy->cpuinfo.min_freq = min_fsb * fid * 100;
+	policy->max = policy->cpuinfo.max_freq = max_fsb * fid * 100;
 	policy->cpuinfo.transition_latency = CPUFREQ_ETERNAL;
 	policy->cur = nforce2_get(policy->cpu);
-	policy->min = policy->cpuinfo.min_freq;
-	policy->max = policy->cpuinfo.max_freq;
 
 	return 0;
 }
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index b02824d092e7..1b8a48eaf90f 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -45,7 +45,7 @@ static DEFINE_PER_CPU(struct cpufreq_policy *, cpufreq_cpu_data);
 /* This one keeps track of the previously set governor of a removed CPU */
 static DEFINE_PER_CPU(char[CPUFREQ_NAME_LEN], cpufreq_cpu_governor);
 #endif
-static DEFINE_SPINLOCK(cpufreq_driver_lock);
+static DEFINE_RWLOCK(cpufreq_driver_lock);
 
 /*
  * cpu_policy_rwsem is a per CPU reader-writer semaphore designed to cure
@@ -128,6 +128,11 @@ void disable_cpufreq(void)
 static LIST_HEAD(cpufreq_governor_list);
 static DEFINE_MUTEX(cpufreq_governor_mutex);
 
+bool have_governor_per_policy(void)
+{
+	return cpufreq_driver->have_governor_per_policy;
+}
+
 static struct cpufreq_policy *__cpufreq_cpu_get(unsigned int cpu, bool sysfs)
 {
 	struct cpufreq_policy *data;
@@ -137,7 +142,7 @@ static struct cpufreq_policy *__cpufreq_cpu_get(unsigned int cpu, bool sysfs)
 		goto err_out;
 
 	/* get the cpufreq driver */
-	spin_lock_irqsave(&cpufreq_driver_lock, flags);
+	read_lock_irqsave(&cpufreq_driver_lock, flags);
 
 	if (!cpufreq_driver)
 		goto err_out_unlock;
@@ -155,13 +160,13 @@ static struct cpufreq_policy *__cpufreq_cpu_get(unsigned int cpu, bool sysfs)
 	if (!sysfs && !kobject_get(&data->kobj))
 		goto err_out_put_module;
 
-	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+	read_unlock_irqrestore(&cpufreq_driver_lock, flags);
 	return data;
 
 err_out_put_module:
 	module_put(cpufreq_driver->owner);
 err_out_unlock:
-	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+	read_unlock_irqrestore(&cpufreq_driver_lock, flags);
 err_out:
 	return NULL;
 }
@@ -244,19 +249,9 @@ static inline void adjust_jiffies(unsigned long val, struct cpufreq_freqs *ci)
 #endif
 
 
-/**
- * cpufreq_notify_transition - call notifier chain and adjust_jiffies
- * on frequency transition.
- *
- * This function calls the transition notifiers and the "adjust_jiffies"
- * function. It is called twice on all CPU frequency changes that have
- * external effects.
- */
-void cpufreq_notify_transition(struct cpufreq_freqs *freqs, unsigned int state)
+void __cpufreq_notify_transition(struct cpufreq_policy *policy,
+		struct cpufreq_freqs *freqs, unsigned int state)
 {
-	struct cpufreq_policy *policy;
-	unsigned long flags;
-
 	BUG_ON(irqs_disabled());
 
 	if (cpufreq_disabled())
@@ -266,10 +261,6 @@ void cpufreq_notify_transition(struct cpufreq_freqs *freqs, unsigned int state)
 	pr_debug("notification %u of frequency transition to %u kHz\n",
 		state, freqs->new);
 
-	spin_lock_irqsave(&cpufreq_driver_lock, flags);
-	policy = per_cpu(cpufreq_cpu_data, freqs->cpu);
-	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
-
 	switch (state) {
 
 	case CPUFREQ_PRECHANGE:
@@ -303,6 +294,20 @@ void cpufreq_notify_transition(struct cpufreq_freqs *freqs, unsigned int state)
 		break;
 	}
 }
+/**
+ * cpufreq_notify_transition - call notifier chain and adjust_jiffies
+ * on frequency transition.
+ *
+ * This function calls the transition notifiers and the "adjust_jiffies"
+ * function. It is called twice on all CPU frequency changes that have
+ * external effects.
+ */
+void cpufreq_notify_transition(struct cpufreq_policy *policy,
+		struct cpufreq_freqs *freqs, unsigned int state)
+{
+	for_each_cpu(freqs->cpu, policy->cpus)
+		__cpufreq_notify_transition(policy, freqs, state);
+}
 EXPORT_SYMBOL_GPL(cpufreq_notify_transition);
 
 
@@ -765,12 +770,12 @@ static int cpufreq_add_dev_interface(unsigned int cpu,
 			goto err_out_kobj_put;
 	}
 
-	spin_lock_irqsave(&cpufreq_driver_lock, flags);
+	write_lock_irqsave(&cpufreq_driver_lock, flags);
 	for_each_cpu(j, policy->cpus) {
 		per_cpu(cpufreq_cpu_data, j) = policy;
 		per_cpu(cpufreq_policy_cpu, j) = policy->cpu;
 	}
-	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
 
 	ret = cpufreq_add_dev_symlink(cpu, policy);
 	if (ret)
@@ -803,27 +808,30 @@ static int cpufreq_add_policy_cpu(unsigned int cpu, unsigned int sibling,
 				  struct device *dev)
 {
 	struct cpufreq_policy *policy;
-	int ret = 0;
+	int ret = 0, has_target = !!cpufreq_driver->target;
 	unsigned long flags;
 
 	policy = cpufreq_cpu_get(sibling);
 	WARN_ON(!policy);
 
-	__cpufreq_governor(policy, CPUFREQ_GOV_STOP);
+	if (has_target)
+		__cpufreq_governor(policy, CPUFREQ_GOV_STOP);
 
 	lock_policy_rwsem_write(sibling);
 
-	spin_lock_irqsave(&cpufreq_driver_lock, flags);
+	write_lock_irqsave(&cpufreq_driver_lock, flags);
 
 	cpumask_set_cpu(cpu, policy->cpus);
 	per_cpu(cpufreq_policy_cpu, cpu) = policy->cpu;
 	per_cpu(cpufreq_cpu_data, cpu) = policy;
-	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
 
 	unlock_policy_rwsem_write(sibling);
 
-	__cpufreq_governor(policy, CPUFREQ_GOV_START);
-	__cpufreq_governor(policy, CPUFREQ_GOV_LIMITS);
+	if (has_target) {
+		__cpufreq_governor(policy, CPUFREQ_GOV_START);
+		__cpufreq_governor(policy, CPUFREQ_GOV_LIMITS);
+	}
 
 	ret = sysfs_create_link(&dev->kobj, &policy->kobj, "cpufreq");
 	if (ret) {
@@ -871,15 +879,15 @@ static int cpufreq_add_dev(struct device *dev, struct subsys_interface *sif)
 
 #ifdef CONFIG_HOTPLUG_CPU
 	/* Check if this cpu was hot-unplugged earlier and has siblings */
-	spin_lock_irqsave(&cpufreq_driver_lock, flags);
+	read_lock_irqsave(&cpufreq_driver_lock, flags);
 	for_each_online_cpu(sibling) {
 		struct cpufreq_policy *cp = per_cpu(cpufreq_cpu_data, sibling);
 		if (cp && cpumask_test_cpu(cpu, cp->related_cpus)) {
-			spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+			read_unlock_irqrestore(&cpufreq_driver_lock, flags);
 			return cpufreq_add_policy_cpu(cpu, sibling, dev);
 		}
 	}
-	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+	read_unlock_irqrestore(&cpufreq_driver_lock, flags);
 #endif
 #endif
 
@@ -952,10 +960,10 @@ static int cpufreq_add_dev(struct device *dev, struct subsys_interface *sif)
 	return 0;
 
 err_out_unregister:
-	spin_lock_irqsave(&cpufreq_driver_lock, flags);
+	write_lock_irqsave(&cpufreq_driver_lock, flags);
 	for_each_cpu(j, policy->cpus)
 		per_cpu(cpufreq_cpu_data, j) = NULL;
-	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
 
 	kobject_put(&policy->kobj);
 	wait_for_completion(&policy->kobj_unregister);
@@ -1008,12 +1016,12 @@ static int __cpufreq_remove_dev(struct device *dev, struct subsys_interface *sif
 
 	pr_debug("%s: unregistering CPU %u\n", __func__, cpu);
 
-	spin_lock_irqsave(&cpufreq_driver_lock, flags);
+	write_lock_irqsave(&cpufreq_driver_lock, flags);
 
 	data = per_cpu(cpufreq_cpu_data, cpu);
 	per_cpu(cpufreq_cpu_data, cpu) = NULL;
 
-	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
 
 	if (!data) {
 		pr_debug("%s: No cpu_data found\n", __func__);
@@ -1031,7 +1039,9 @@ static int __cpufreq_remove_dev(struct device *dev, struct subsys_interface *sif
 
 	WARN_ON(lock_policy_rwsem_write(cpu));
 	cpus = cpumask_weight(data->cpus);
-	cpumask_clear_cpu(cpu, data->cpus);
+
+	if (cpus > 1)
+		cpumask_clear_cpu(cpu, data->cpus);
 	unlock_policy_rwsem_write(cpu);
 
 	if (cpu != data->cpu) {
@@ -1047,9 +1057,9 @@ static int __cpufreq_remove_dev(struct device *dev, struct subsys_interface *sif
 			WARN_ON(lock_policy_rwsem_write(cpu));
 			cpumask_set_cpu(cpu, data->cpus);
 
-			spin_lock_irqsave(&cpufreq_driver_lock, flags);
+			write_lock_irqsave(&cpufreq_driver_lock, flags);
 			per_cpu(cpufreq_cpu_data, cpu) = data;
-			spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+			write_unlock_irqrestore(&cpufreq_driver_lock, flags);
 
 			unlock_policy_rwsem_write(cpu);
 
@@ -1070,6 +1080,9 @@ static int __cpufreq_remove_dev(struct device *dev, struct subsys_interface *sif
 
 	/* If cpu is last user of policy, free policy */
 	if (cpus == 1) {
+		if (cpufreq_driver->target)
+			__cpufreq_governor(data, CPUFREQ_GOV_POLICY_EXIT);
+
 		lock_policy_rwsem_read(cpu);
 		kobj = &data->kobj;
 		cmp = &data->kobj_unregister;
@@ -1134,16 +1147,23 @@ static void handle_update(struct work_struct *work)
 static void cpufreq_out_of_sync(unsigned int cpu, unsigned int old_freq,
 				unsigned int new_freq)
 {
+	struct cpufreq_policy *policy;
 	struct cpufreq_freqs freqs;
+	unsigned long flags;
+
 
 	pr_debug("Warning: CPU frequency out of sync: cpufreq and timing "
 	       "core thinks of %u, is %u kHz.\n", old_freq, new_freq);
 
-	freqs.cpu = cpu;
 	freqs.old = old_freq;
 	freqs.new = new_freq;
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+
+	read_lock_irqsave(&cpufreq_driver_lock, flags);
+	policy = per_cpu(cpufreq_cpu_data, cpu);
+	read_unlock_irqrestore(&cpufreq_driver_lock, flags);
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 }
 
 
@@ -1544,10 +1564,12 @@ static int __cpufreq_governor(struct cpufreq_policy *policy,
 						policy->cpu, event);
 	ret = policy->governor->governor(policy, event);
 
-	if (event == CPUFREQ_GOV_START)
-		policy->governor->initialized++;
-	else if (event == CPUFREQ_GOV_STOP)
-		policy->governor->initialized--;
+	if (!ret) {
+		if (event == CPUFREQ_GOV_POLICY_INIT)
+			policy->governor->initialized++;
+		else if (event == CPUFREQ_GOV_POLICY_EXIT)
+			policy->governor->initialized--;
+	}
 
 	/* we keep one module reference alive for
 			each CPU governed by this CPU */
@@ -1651,7 +1673,7 @@ EXPORT_SYMBOL(cpufreq_get_policy);
 static int __cpufreq_set_policy(struct cpufreq_policy *data,
 				struct cpufreq_policy *policy)
 {
-	int ret = 0;
+	int ret = 0, failed = 1;
 
 	pr_debug("setting new policy for CPU %u: %u - %u kHz\n", policy->cpu,
 		policy->min, policy->max);
@@ -1705,18 +1727,31 @@ static int __cpufreq_set_policy(struct cpufreq_policy *data,
 			pr_debug("governor switch\n");
 
 			/* end old governor */
-			if (data->governor)
+			if (data->governor) {
 				__cpufreq_governor(data, CPUFREQ_GOV_STOP);
+				__cpufreq_governor(data,
+						CPUFREQ_GOV_POLICY_EXIT);
+			}
 
 			/* start new governor */
 			data->governor = policy->governor;
-			if (__cpufreq_governor(data, CPUFREQ_GOV_START)) {
+			if (!__cpufreq_governor(data, CPUFREQ_GOV_POLICY_INIT)) {
+				if (!__cpufreq_governor(data, CPUFREQ_GOV_START))
+					failed = 0;
+				else
+					__cpufreq_governor(data,
+							CPUFREQ_GOV_POLICY_EXIT);
+			}
+
+			if (failed) {
 				/* new governor failed, so re-start old one */
 				pr_debug("starting governor %s failed\n",
 							data->governor->name);
 				if (old_gov) {
 					data->governor = old_gov;
 					__cpufreq_governor(data,
+							CPUFREQ_GOV_POLICY_INIT);
+					__cpufreq_governor(data,
 							   CPUFREQ_GOV_START);
 				}
 				ret = -EINVAL;
@@ -1848,13 +1883,13 @@ int cpufreq_register_driver(struct cpufreq_driver *driver_data)
 	if (driver_data->setpolicy)
 		driver_data->flags |= CPUFREQ_CONST_LOOPS;
 
-	spin_lock_irqsave(&cpufreq_driver_lock, flags);
+	write_lock_irqsave(&cpufreq_driver_lock, flags);
 	if (cpufreq_driver) {
-		spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+		write_unlock_irqrestore(&cpufreq_driver_lock, flags);
 		return -EBUSY;
 	}
 	cpufreq_driver = driver_data;
-	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
 
 	ret = subsys_interface_register(&cpufreq_interface);
 	if (ret)
@@ -1886,9 +1921,9 @@ int cpufreq_register_driver(struct cpufreq_driver *driver_data)
 err_if_unreg:
 	subsys_interface_unregister(&cpufreq_interface);
 err_null_driver:
-	spin_lock_irqsave(&cpufreq_driver_lock, flags);
+	write_lock_irqsave(&cpufreq_driver_lock, flags);
 	cpufreq_driver = NULL;
-	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(cpufreq_register_driver);
@@ -1914,9 +1949,9 @@ int cpufreq_unregister_driver(struct cpufreq_driver *driver)
 	subsys_interface_unregister(&cpufreq_interface);
 	unregister_hotcpu_notifier(&cpufreq_cpu_notifier);
 
-	spin_lock_irqsave(&cpufreq_driver_lock, flags);
+	write_lock_irqsave(&cpufreq_driver_lock, flags);
 	cpufreq_driver = NULL;
-	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
 
 	return 0;
 }
diff --git a/drivers/cpufreq/cpufreq_conservative.c b/drivers/cpufreq/cpufreq_conservative.c
index 4fd0006b1291..0ceb2eff5a7e 100644
--- a/drivers/cpufreq/cpufreq_conservative.c
+++ b/drivers/cpufreq/cpufreq_conservative.c
@@ -20,6 +20,7 @@
 #include <linux/mutex.h>
 #include <linux/notifier.h>
 #include <linux/percpu-defs.h>
+#include <linux/slab.h>
 #include <linux/sysfs.h>
 #include <linux/types.h>
 
@@ -28,25 +29,29 @@
 /* Conservative governor macros */
 #define DEF_FREQUENCY_UP_THRESHOLD		(80)
 #define DEF_FREQUENCY_DOWN_THRESHOLD		(20)
+#define DEF_FREQUENCY_STEP			(5)
 #define DEF_SAMPLING_DOWN_FACTOR		(1)
 #define MAX_SAMPLING_DOWN_FACTOR		(10)
 
-static struct dbs_data cs_dbs_data;
 static DEFINE_PER_CPU(struct cs_cpu_dbs_info_s, cs_cpu_dbs_info);
 
-static struct cs_dbs_tuners cs_tuners = {
-	.up_threshold = DEF_FREQUENCY_UP_THRESHOLD,
-	.down_threshold = DEF_FREQUENCY_DOWN_THRESHOLD,
-	.sampling_down_factor = DEF_SAMPLING_DOWN_FACTOR,
-	.ignore_nice = 0,
-	.freq_step = 5,
-};
+static inline unsigned int get_freq_target(struct cs_dbs_tuners *cs_tuners,
+					   struct cpufreq_policy *policy)
+{
+	unsigned int freq_target = (cs_tuners->freq_step * policy->max) / 100;
+
+	/* max freq cannot be less than 100. But who knows... */
+	if (unlikely(freq_target == 0))
+		freq_target = DEF_FREQUENCY_STEP;
+
+	return freq_target;
+}
 
 /*
  * Every sampling_rate, we check, if current idle time is less than 20%
- * (default), then we try to increase frequency Every sampling_rate *
- * sampling_down_factor, we check, if current idle time is more than 80%, then
- * we try to decrease frequency
+ * (default), then we try to increase frequency. Every sampling_rate *
+ * sampling_down_factor, we check, if current idle time is more than 80%
+ * (default), then we try to decrease frequency
  *
  * Any frequency increase takes it to the maximum frequency. Frequency reduction
  * happens at minimum steps of 5% (default) of maximum frequency
@@ -55,30 +60,25 @@ static void cs_check_cpu(int cpu, unsigned int load)
 {
 	struct cs_cpu_dbs_info_s *dbs_info = &per_cpu(cs_cpu_dbs_info, cpu);
 	struct cpufreq_policy *policy = dbs_info->cdbs.cur_policy;
-	unsigned int freq_target;
+	struct dbs_data *dbs_data = policy->governor_data;
+	struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
 
 	/*
 	 * break out if we 'cannot' reduce the speed as the user might
 	 * want freq_step to be zero
 	 */
-	if (cs_tuners.freq_step == 0)
+	if (cs_tuners->freq_step == 0)
 		return;
 
 	/* Check for frequency increase */
-	if (load > cs_tuners.up_threshold) {
+	if (load > cs_tuners->up_threshold) {
 		dbs_info->down_skip = 0;
 
 		/* if we are already at full speed then break out early */
 		if (dbs_info->requested_freq == policy->max)
 			return;
 
-		freq_target = (cs_tuners.freq_step * policy->max) / 100;
-
-		/* max freq cannot be less than 100. But who knows.... */
-		if (unlikely(freq_target == 0))
-			freq_target = 5;
-
-		dbs_info->requested_freq += freq_target;
+		dbs_info->requested_freq += get_freq_target(cs_tuners, policy);
 		if (dbs_info->requested_freq > policy->max)
 			dbs_info->requested_freq = policy->max;
 
@@ -87,45 +87,48 @@ static void cs_check_cpu(int cpu, unsigned int load)
 		return;
 	}
 
-	/*
-	 * The optimal frequency is the frequency that is the lowest that can
-	 * support the current CPU usage without triggering the up policy. To be
-	 * safe, we focus 10 points under the threshold.
-	 */
-	if (load < (cs_tuners.down_threshold - 10)) {
-		freq_target = (cs_tuners.freq_step * policy->max) / 100;
-
-		dbs_info->requested_freq -= freq_target;
-		if (dbs_info->requested_freq < policy->min)
-			dbs_info->requested_freq = policy->min;
+	/* if sampling_down_factor is active break out early */
+	if (++dbs_info->down_skip < cs_tuners->sampling_down_factor)
+		return;
+	dbs_info->down_skip = 0;
 
+	/* Check for frequency decrease */
+	if (load < cs_tuners->down_threshold) {
 		/*
 		 * if we cannot reduce the frequency anymore, break out early
 		 */
 		if (policy->cur == policy->min)
 			return;
 
+		dbs_info->requested_freq -= get_freq_target(cs_tuners, policy);
+		if (dbs_info->requested_freq < policy->min)
+			dbs_info->requested_freq = policy->min;
+
 		__cpufreq_driver_target(policy, dbs_info->requested_freq,
-				CPUFREQ_RELATION_H);
+				CPUFREQ_RELATION_L);
 		return;
 	}
 }
 
 static void cs_dbs_timer(struct work_struct *work)
 {
-	struct delayed_work *dw = to_delayed_work(work);
 	struct cs_cpu_dbs_info_s *dbs_info = container_of(work,
 			struct cs_cpu_dbs_info_s, cdbs.work.work);
 	unsigned int cpu = dbs_info->cdbs.cur_policy->cpu;
 	struct cs_cpu_dbs_info_s *core_dbs_info = &per_cpu(cs_cpu_dbs_info,
 			cpu);
-	int delay = delay_for_sampling_rate(cs_tuners.sampling_rate);
+	struct dbs_data *dbs_data = dbs_info->cdbs.cur_policy->governor_data;
+	struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
+	int delay = delay_for_sampling_rate(cs_tuners->sampling_rate);
+	bool modify_all = true;
 
 	mutex_lock(&core_dbs_info->cdbs.timer_mutex);
-	if (need_load_eval(&core_dbs_info->cdbs, cs_tuners.sampling_rate))
-		dbs_check_cpu(&cs_dbs_data, cpu);
+	if (!need_load_eval(&core_dbs_info->cdbs, cs_tuners->sampling_rate))
+		modify_all = false;
+	else
+		dbs_check_cpu(dbs_data, cpu);
 
-	schedule_delayed_work_on(smp_processor_id(), dw, delay);
+	gov_queue_work(dbs_data, dbs_info->cdbs.cur_policy, delay, modify_all);
 	mutex_unlock(&core_dbs_info->cdbs.timer_mutex);
 }
 
@@ -154,16 +157,12 @@ static int dbs_cpufreq_notifier(struct notifier_block *nb, unsigned long val,
 }
 
 /************************** sysfs interface ************************/
-static ssize_t show_sampling_rate_min(struct kobject *kobj,
-				      struct attribute *attr, char *buf)
-{
-	return sprintf(buf, "%u\n", cs_dbs_data.min_sampling_rate);
-}
+static struct common_dbs_data cs_dbs_cdata;
 
-static ssize_t store_sampling_down_factor(struct kobject *a,
-					  struct attribute *b,
-					  const char *buf, size_t count)
+static ssize_t store_sampling_down_factor(struct dbs_data *dbs_data,
+		const char *buf, size_t count)
 {
+	struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
 	unsigned int input;
 	int ret;
 	ret = sscanf(buf, "%u", &input);
@@ -171,13 +170,14 @@ static ssize_t store_sampling_down_factor(struct kobject *a,
 	if (ret != 1 || input > MAX_SAMPLING_DOWN_FACTOR || input < 1)
 		return -EINVAL;
 
-	cs_tuners.sampling_down_factor = input;
+	cs_tuners->sampling_down_factor = input;
 	return count;
 }
 
-static ssize_t store_sampling_rate(struct kobject *a, struct attribute *b,
-				   const char *buf, size_t count)
+static ssize_t store_sampling_rate(struct dbs_data *dbs_data, const char *buf,
+		size_t count)
 {
+	struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
 	unsigned int input;
 	int ret;
 	ret = sscanf(buf, "%u", &input);
@@ -185,43 +185,46 @@ static ssize_t store_sampling_rate(struct kobject *a, struct attribute *b,
 	if (ret != 1)
 		return -EINVAL;
 
-	cs_tuners.sampling_rate = max(input, cs_dbs_data.min_sampling_rate);
+	cs_tuners->sampling_rate = max(input, dbs_data->min_sampling_rate);
 	return count;
 }
 
-static ssize_t store_up_threshold(struct kobject *a, struct attribute *b,
-				  const char *buf, size_t count)
+static ssize_t store_up_threshold(struct dbs_data *dbs_data, const char *buf,
+		size_t count)
 {
+	struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
 	unsigned int input;
 	int ret;
 	ret = sscanf(buf, "%u", &input);
 
-	if (ret != 1 || input > 100 || input <= cs_tuners.down_threshold)
+	if (ret != 1 || input > 100 || input <= cs_tuners->down_threshold)
 		return -EINVAL;
 
-	cs_tuners.up_threshold = input;
+	cs_tuners->up_threshold = input;
 	return count;
 }
 
-static ssize_t store_down_threshold(struct kobject *a, struct attribute *b,
-				    const char *buf, size_t count)
+static ssize_t store_down_threshold(struct dbs_data *dbs_data, const char *buf,
+		size_t count)
 {
+	struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
 	unsigned int input;
 	int ret;
 	ret = sscanf(buf, "%u", &input);
 
 	/* cannot be lower than 11 otherwise freq will not fall */
 	if (ret != 1 || input < 11 || input > 100 ||
-			input >= cs_tuners.up_threshold)
+			input >= cs_tuners->up_threshold)
 		return -EINVAL;
 
-	cs_tuners.down_threshold = input;
+	cs_tuners->down_threshold = input;
 	return count;
 }
 
-static ssize_t store_ignore_nice_load(struct kobject *a, struct attribute *b,
-				      const char *buf, size_t count)
+static ssize_t store_ignore_nice(struct dbs_data *dbs_data, const char *buf,
+		size_t count)
 {
+	struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
 	unsigned int input, j;
 	int ret;
 
@@ -232,27 +235,28 @@ static ssize_t store_ignore_nice_load(struct kobject *a, struct attribute *b,
 	if (input > 1)
 		input = 1;
 
-	if (input == cs_tuners.ignore_nice) /* nothing to do */
+	if (input == cs_tuners->ignore_nice) /* nothing to do */
 		return count;
 
-	cs_tuners.ignore_nice = input;
+	cs_tuners->ignore_nice = input;
 
 	/* we need to re-evaluate prev_cpu_idle */
 	for_each_online_cpu(j) {
 		struct cs_cpu_dbs_info_s *dbs_info;
 		dbs_info = &per_cpu(cs_cpu_dbs_info, j);
 		dbs_info->cdbs.prev_cpu_idle = get_cpu_idle_time(j,
-						&dbs_info->cdbs.prev_cpu_wall);
-		if (cs_tuners.ignore_nice)
+					&dbs_info->cdbs.prev_cpu_wall, 0);
+		if (cs_tuners->ignore_nice)
 			dbs_info->cdbs.prev_cpu_nice =
 				kcpustat_cpu(j).cpustat[CPUTIME_NICE];
 	}
 	return count;
 }
 
-static ssize_t store_freq_step(struct kobject *a, struct attribute *b,
-			       const char *buf, size_t count)
+static ssize_t store_freq_step(struct dbs_data *dbs_data, const char *buf,
+		size_t count)
 {
+	struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
 	unsigned int input;
 	int ret;
 	ret = sscanf(buf, "%u", &input);
@@ -267,43 +271,88 @@ static ssize_t store_freq_step(struct kobject *a, struct attribute *b,
 	 * no need to test here if freq_step is zero as the user might actually
 	 * want this, they would be crazy though :)
 	 */
-	cs_tuners.freq_step = input;
+	cs_tuners->freq_step = input;
 	return count;
 }
 
-show_one(cs, sampling_rate, sampling_rate);
-show_one(cs, sampling_down_factor, sampling_down_factor);
-show_one(cs, up_threshold, up_threshold);
-show_one(cs, down_threshold, down_threshold);
-show_one(cs, ignore_nice_load, ignore_nice);
-show_one(cs, freq_step, freq_step);
-
-define_one_global_rw(sampling_rate);
-define_one_global_rw(sampling_down_factor);
-define_one_global_rw(up_threshold);
-define_one_global_rw(down_threshold);
-define_one_global_rw(ignore_nice_load);
-define_one_global_rw(freq_step);
-define_one_global_ro(sampling_rate_min);
-
-static struct attribute *dbs_attributes[] = {
-	&sampling_rate_min.attr,
-	&sampling_rate.attr,
-	&sampling_down_factor.attr,
-	&up_threshold.attr,
-	&down_threshold.attr,
-	&ignore_nice_load.attr,
-	&freq_step.attr,
+show_store_one(cs, sampling_rate);
+show_store_one(cs, sampling_down_factor);
+show_store_one(cs, up_threshold);
+show_store_one(cs, down_threshold);
+show_store_one(cs, ignore_nice);
+show_store_one(cs, freq_step);
+declare_show_sampling_rate_min(cs);
+
+gov_sys_pol_attr_rw(sampling_rate);
+gov_sys_pol_attr_rw(sampling_down_factor);
+gov_sys_pol_attr_rw(up_threshold);
+gov_sys_pol_attr_rw(down_threshold);
+gov_sys_pol_attr_rw(ignore_nice);
+gov_sys_pol_attr_rw(freq_step);
+gov_sys_pol_attr_ro(sampling_rate_min);
+
+static struct attribute *dbs_attributes_gov_sys[] = {
+	&sampling_rate_min_gov_sys.attr,
+	&sampling_rate_gov_sys.attr,
+	&sampling_down_factor_gov_sys.attr,
+	&up_threshold_gov_sys.attr,
+	&down_threshold_gov_sys.attr,
+	&ignore_nice_gov_sys.attr,
+	&freq_step_gov_sys.attr,
 	NULL
 };
 
-static struct attribute_group cs_attr_group = {
-	.attrs = dbs_attributes,
+static struct attribute_group cs_attr_group_gov_sys = {
+	.attrs = dbs_attributes_gov_sys,
+	.name = "conservative",
+};
+
+static struct attribute *dbs_attributes_gov_pol[] = {
+	&sampling_rate_min_gov_pol.attr,
+	&sampling_rate_gov_pol.attr,
+	&sampling_down_factor_gov_pol.attr,
+	&up_threshold_gov_pol.attr,
+	&down_threshold_gov_pol.attr,
+	&ignore_nice_gov_pol.attr,
+	&freq_step_gov_pol.attr,
+	NULL
+};
+
+static struct attribute_group cs_attr_group_gov_pol = {
+	.attrs = dbs_attributes_gov_pol,
 	.name = "conservative",
 };
 
 /************************** sysfs end ************************/
 
+static int cs_init(struct dbs_data *dbs_data)
+{
+	struct cs_dbs_tuners *tuners;
+
+	tuners = kzalloc(sizeof(struct cs_dbs_tuners), GFP_KERNEL);
+	if (!tuners) {
+		pr_err("%s: kzalloc failed\n", __func__);
+		return -ENOMEM;
+	}
+
+	tuners->up_threshold = DEF_FREQUENCY_UP_THRESHOLD;
+	tuners->down_threshold = DEF_FREQUENCY_DOWN_THRESHOLD;
+	tuners->sampling_down_factor = DEF_SAMPLING_DOWN_FACTOR;
+	tuners->ignore_nice = 0;
+	tuners->freq_step = DEF_FREQUENCY_STEP;
+
+	dbs_data->tuners = tuners;
+	dbs_data->min_sampling_rate = MIN_SAMPLING_RATE_RATIO *
+		jiffies_to_usecs(10);
+	mutex_init(&dbs_data->mutex);
+	return 0;
+}
+
+static void cs_exit(struct dbs_data *dbs_data)
+{
+	kfree(dbs_data->tuners);
+}
+
 define_get_cpu_dbs_routines(cs_cpu_dbs_info);
 
 static struct notifier_block cs_cpufreq_notifier_block = {
@@ -314,21 +363,23 @@ static struct cs_ops cs_ops = {
 	.notifier_block = &cs_cpufreq_notifier_block,
 };
 
-static struct dbs_data cs_dbs_data = {
+static struct common_dbs_data cs_dbs_cdata = {
 	.governor = GOV_CONSERVATIVE,
-	.attr_group = &cs_attr_group,
-	.tuners = &cs_tuners,
+	.attr_group_gov_sys = &cs_attr_group_gov_sys,
+	.attr_group_gov_pol = &cs_attr_group_gov_pol,
 	.get_cpu_cdbs = get_cpu_cdbs,
 	.get_cpu_dbs_info_s = get_cpu_dbs_info_s,
 	.gov_dbs_timer = cs_dbs_timer,
 	.gov_check_cpu = cs_check_cpu,
 	.gov_ops = &cs_ops,
+	.init = cs_init,
+	.exit = cs_exit,
 };
 
 static int cs_cpufreq_governor_dbs(struct cpufreq_policy *policy,
 				   unsigned int event)
 {
-	return cpufreq_governor_dbs(&cs_dbs_data, policy, event);
+	return cpufreq_governor_dbs(policy, &cs_dbs_cdata, event);
 }
 
 #ifndef CONFIG_CPU_FREQ_DEFAULT_GOV_CONSERVATIVE
@@ -343,7 +394,6 @@ struct cpufreq_governor cpufreq_gov_conservative = {
 
 static int __init cpufreq_gov_dbs_init(void)
 {
-	mutex_init(&cs_dbs_data.mutex);
 	return cpufreq_register_governor(&cpufreq_gov_conservative);
 }
 
diff --git a/drivers/cpufreq/cpufreq_governor.c b/drivers/cpufreq/cpufreq_governor.c
index 5a76086ff09b..443442df113b 100644
--- a/drivers/cpufreq/cpufreq_governor.c
+++ b/drivers/cpufreq/cpufreq_governor.c
@@ -22,12 +22,29 @@
 #include <linux/export.h>
 #include <linux/kernel_stat.h>
 #include <linux/mutex.h>
+#include <linux/slab.h>
 #include <linux/tick.h>
 #include <linux/types.h>
 #include <linux/workqueue.h>
 
 #include "cpufreq_governor.h"
 
+static struct kobject *get_governor_parent_kobj(struct cpufreq_policy *policy)
+{
+	if (have_governor_per_policy())
+		return &policy->kobj;
+	else
+		return cpufreq_global_kobject;
+}
+
+static struct attribute_group *get_sysfs_attr(struct dbs_data *dbs_data)
+{
+	if (have_governor_per_policy())
+		return dbs_data->cdata->attr_group_gov_pol;
+	else
+		return dbs_data->cdata->attr_group_gov_sys;
+}
+
 static inline u64 get_cpu_idle_time_jiffy(unsigned int cpu, u64 *wall)
 {
 	u64 idle_time;
@@ -50,13 +67,13 @@ static inline u64 get_cpu_idle_time_jiffy(unsigned int cpu, u64 *wall)
 	return cputime_to_usecs(idle_time);
 }
 
-u64 get_cpu_idle_time(unsigned int cpu, u64 *wall)
+u64 get_cpu_idle_time(unsigned int cpu, u64 *wall, int io_busy)
 {
-	u64 idle_time = get_cpu_idle_time_us(cpu, NULL);
+	u64 idle_time = get_cpu_idle_time_us(cpu, io_busy ? wall : NULL);
 
 	if (idle_time == -1ULL)
 		return get_cpu_idle_time_jiffy(cpu, wall);
-	else
+	else if (!io_busy)
 		idle_time += get_cpu_iowait_time_us(cpu, wall);
 
 	return idle_time;
@@ -65,7 +82,7 @@ EXPORT_SYMBOL_GPL(get_cpu_idle_time);
 
 void dbs_check_cpu(struct dbs_data *dbs_data, int cpu)
 {
-	struct cpu_dbs_common_info *cdbs = dbs_data->get_cpu_cdbs(cpu);
+	struct cpu_dbs_common_info *cdbs = dbs_data->cdata->get_cpu_cdbs(cpu);
 	struct od_dbs_tuners *od_tuners = dbs_data->tuners;
 	struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
 	struct cpufreq_policy *policy;
@@ -73,7 +90,7 @@ void dbs_check_cpu(struct dbs_data *dbs_data, int cpu)
 	unsigned int ignore_nice;
 	unsigned int j;
 
-	if (dbs_data->governor == GOV_ONDEMAND)
+	if (dbs_data->cdata->governor == GOV_ONDEMAND)
 		ignore_nice = od_tuners->ignore_nice;
 	else
 		ignore_nice = cs_tuners->ignore_nice;
@@ -83,13 +100,22 @@ void dbs_check_cpu(struct dbs_data *dbs_data, int cpu)
 	/* Get Absolute Load (in terms of freq for ondemand gov) */
 	for_each_cpu(j, policy->cpus) {
 		struct cpu_dbs_common_info *j_cdbs;
-		u64 cur_wall_time, cur_idle_time, cur_iowait_time;
-		unsigned int idle_time, wall_time, iowait_time;
+		u64 cur_wall_time, cur_idle_time;
+		unsigned int idle_time, wall_time;
 		unsigned int load;
+		int io_busy = 0;
 
-		j_cdbs = dbs_data->get_cpu_cdbs(j);
+		j_cdbs = dbs_data->cdata->get_cpu_cdbs(j);
 
-		cur_idle_time = get_cpu_idle_time(j, &cur_wall_time);
+		/*
+		 * For the purpose of ondemand, waiting for disk IO is
+		 * an indication that you're performance critical, and
+		 * not that the system is actually idle. So do not add
+		 * the iowait time to the cpu idle time.
+		 */
+		if (dbs_data->cdata->governor == GOV_ONDEMAND)
+			io_busy = od_tuners->io_is_busy;
+		cur_idle_time = get_cpu_idle_time(j, &cur_wall_time, io_busy);
 
 		wall_time = (unsigned int)
 			(cur_wall_time - j_cdbs->prev_cpu_wall);
@@ -117,35 +143,12 @@ void dbs_check_cpu(struct dbs_data *dbs_data, int cpu)
 			idle_time += jiffies_to_usecs(cur_nice_jiffies);
 		}
 
-		if (dbs_data->governor == GOV_ONDEMAND) {
-			struct od_cpu_dbs_info_s *od_j_dbs_info =
-				dbs_data->get_cpu_dbs_info_s(cpu);
-
-			cur_iowait_time = get_cpu_iowait_time_us(j,
-					&cur_wall_time);
-			if (cur_iowait_time == -1ULL)
-				cur_iowait_time = 0;
-
-			iowait_time = (unsigned int) (cur_iowait_time -
-					od_j_dbs_info->prev_cpu_iowait);
-			od_j_dbs_info->prev_cpu_iowait = cur_iowait_time;
-
-			/*
-			 * For the purpose of ondemand, waiting for disk IO is
-			 * an indication that you're performance critical, and
-			 * not that the system is actually idle. So subtract the
-			 * iowait time from the cpu idle time.
-			 */
-			if (od_tuners->io_is_busy && idle_time >= iowait_time)
-				idle_time -= iowait_time;
-		}
-
 		if (unlikely(!wall_time || wall_time < idle_time))
 			continue;
 
 		load = 100 * (wall_time - idle_time) / wall_time;
 
-		if (dbs_data->governor == GOV_ONDEMAND) {
+		if (dbs_data->cdata->governor == GOV_ONDEMAND) {
 			int freq_avg = __cpufreq_driver_getavg(policy, j);
 			if (freq_avg <= 0)
 				freq_avg = policy->cur;
@@ -157,24 +160,42 @@ void dbs_check_cpu(struct dbs_data *dbs_data, int cpu)
 			max_load = load;
 	}
 
-	dbs_data->gov_check_cpu(cpu, max_load);
+	dbs_data->cdata->gov_check_cpu(cpu, max_load);
 }
 EXPORT_SYMBOL_GPL(dbs_check_cpu);
 
-static inline void dbs_timer_init(struct dbs_data *dbs_data, int cpu,
-				  unsigned int sampling_rate)
+static inline void __gov_queue_work(int cpu, struct dbs_data *dbs_data,
+		unsigned int delay)
 {
-	int delay = delay_for_sampling_rate(sampling_rate);
-	struct cpu_dbs_common_info *cdbs = dbs_data->get_cpu_cdbs(cpu);
+	struct cpu_dbs_common_info *cdbs = dbs_data->cdata->get_cpu_cdbs(cpu);
 
-	schedule_delayed_work_on(cpu, &cdbs->work, delay);
+	mod_delayed_work_on(cpu, system_wq, &cdbs->work, delay);
 }
 
-static inline void dbs_timer_exit(struct dbs_data *dbs_data, int cpu)
+void gov_queue_work(struct dbs_data *dbs_data, struct cpufreq_policy *policy,
+		unsigned int delay, bool all_cpus)
 {
-	struct cpu_dbs_common_info *cdbs = dbs_data->get_cpu_cdbs(cpu);
+	int i;
 
-	cancel_delayed_work_sync(&cdbs->work);
+	if (!all_cpus) {
+		__gov_queue_work(smp_processor_id(), dbs_data, delay);
+	} else {
+		for_each_cpu(i, policy->cpus)
+			__gov_queue_work(i, dbs_data, delay);
+	}
+}
+EXPORT_SYMBOL_GPL(gov_queue_work);
+
+static inline void gov_cancel_work(struct dbs_data *dbs_data,
+		struct cpufreq_policy *policy)
+{
+	struct cpu_dbs_common_info *cdbs;
+	int i;
+
+	for_each_cpu(i, policy->cpus) {
+		cdbs = dbs_data->cdata->get_cpu_cdbs(i);
+		cancel_delayed_work_sync(&cdbs->work);
+	}
 }
 
 /* Will return if we need to evaluate cpu load again or not */
@@ -196,31 +217,130 @@ bool need_load_eval(struct cpu_dbs_common_info *cdbs,
 }
 EXPORT_SYMBOL_GPL(need_load_eval);
 
-int cpufreq_governor_dbs(struct dbs_data *dbs_data,
-		struct cpufreq_policy *policy, unsigned int event)
+static void set_sampling_rate(struct dbs_data *dbs_data,
+		unsigned int sampling_rate)
 {
+	if (dbs_data->cdata->governor == GOV_CONSERVATIVE) {
+		struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
+		cs_tuners->sampling_rate = sampling_rate;
+	} else {
+		struct od_dbs_tuners *od_tuners = dbs_data->tuners;
+		od_tuners->sampling_rate = sampling_rate;
+	}
+}
+
+int cpufreq_governor_dbs(struct cpufreq_policy *policy,
+		struct common_dbs_data *cdata, unsigned int event)
+{
+	struct dbs_data *dbs_data;
 	struct od_cpu_dbs_info_s *od_dbs_info = NULL;
 	struct cs_cpu_dbs_info_s *cs_dbs_info = NULL;
-	struct cs_ops *cs_ops = NULL;
 	struct od_ops *od_ops = NULL;
-	struct od_dbs_tuners *od_tuners = dbs_data->tuners;
-	struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
+	struct od_dbs_tuners *od_tuners = NULL;
+	struct cs_dbs_tuners *cs_tuners = NULL;
 	struct cpu_dbs_common_info *cpu_cdbs;
-	unsigned int *sampling_rate, latency, ignore_nice, j, cpu = policy->cpu;
+	unsigned int sampling_rate, latency, ignore_nice, j, cpu = policy->cpu;
+	int io_busy = 0;
 	int rc;
 
-	cpu_cdbs = dbs_data->get_cpu_cdbs(cpu);
+	if (have_governor_per_policy())
+		dbs_data = policy->governor_data;
+	else
+		dbs_data = cdata->gdbs_data;
+
+	WARN_ON(!dbs_data && (event != CPUFREQ_GOV_POLICY_INIT));
+
+	switch (event) {
+	case CPUFREQ_GOV_POLICY_INIT:
+		if (have_governor_per_policy()) {
+			WARN_ON(dbs_data);
+		} else if (dbs_data) {
+			policy->governor_data = dbs_data;
+			return 0;
+		}
+
+		dbs_data = kzalloc(sizeof(*dbs_data), GFP_KERNEL);
+		if (!dbs_data) {
+			pr_err("%s: POLICY_INIT: kzalloc failed\n", __func__);
+			return -ENOMEM;
+		}
+
+		dbs_data->cdata = cdata;
+		rc = cdata->init(dbs_data);
+		if (rc) {
+			pr_err("%s: POLICY_INIT: init() failed\n", __func__);
+			kfree(dbs_data);
+			return rc;
+		}
+
+		rc = sysfs_create_group(get_governor_parent_kobj(policy),
+				get_sysfs_attr(dbs_data));
+		if (rc) {
+			cdata->exit(dbs_data);
+			kfree(dbs_data);
+			return rc;
+		}
+
+		policy->governor_data = dbs_data;
 
-	if (dbs_data->governor == GOV_CONSERVATIVE) {
-		cs_dbs_info = dbs_data->get_cpu_dbs_info_s(cpu);
-		sampling_rate = &cs_tuners->sampling_rate;
+		/* policy latency is in nS. Convert it to uS first */
+		latency = policy->cpuinfo.transition_latency / 1000;
+		if (latency == 0)
+			latency = 1;
+
+		/* Bring kernel and HW constraints together */
+		dbs_data->min_sampling_rate = max(dbs_data->min_sampling_rate,
+				MIN_LATENCY_MULTIPLIER * latency);
+		set_sampling_rate(dbs_data, max(dbs_data->min_sampling_rate,
+					latency * LATENCY_MULTIPLIER));
+
+		if (dbs_data->cdata->governor == GOV_CONSERVATIVE) {
+			struct cs_ops *cs_ops = dbs_data->cdata->gov_ops;
+
+			cpufreq_register_notifier(cs_ops->notifier_block,
+					CPUFREQ_TRANSITION_NOTIFIER);
+		}
+
+		if (!have_governor_per_policy())
+			cdata->gdbs_data = dbs_data;
+
+		return 0;
+	case CPUFREQ_GOV_POLICY_EXIT:
+		if ((policy->governor->initialized == 1) ||
+				have_governor_per_policy()) {
+			sysfs_remove_group(get_governor_parent_kobj(policy),
+					get_sysfs_attr(dbs_data));
+
+			if (dbs_data->cdata->governor == GOV_CONSERVATIVE) {
+				struct cs_ops *cs_ops = dbs_data->cdata->gov_ops;
+
+				cpufreq_unregister_notifier(cs_ops->notifier_block,
+						CPUFREQ_TRANSITION_NOTIFIER);
+			}
+
+			cdata->exit(dbs_data);
+			kfree(dbs_data);
+			cdata->gdbs_data = NULL;
+		}
+
+		policy->governor_data = NULL;
+		return 0;
+	}
+
+	cpu_cdbs = dbs_data->cdata->get_cpu_cdbs(cpu);
+
+	if (dbs_data->cdata->governor == GOV_CONSERVATIVE) {
+		cs_tuners = dbs_data->tuners;
+		cs_dbs_info = dbs_data->cdata->get_cpu_dbs_info_s(cpu);
+		sampling_rate = cs_tuners->sampling_rate;
 		ignore_nice = cs_tuners->ignore_nice;
-		cs_ops = dbs_data->gov_ops;
 	} else {
-		od_dbs_info = dbs_data->get_cpu_dbs_info_s(cpu);
-		sampling_rate = &od_tuners->sampling_rate;
+		od_tuners = dbs_data->tuners;
+		od_dbs_info = dbs_data->cdata->get_cpu_dbs_info_s(cpu);
+		sampling_rate = od_tuners->sampling_rate;
 		ignore_nice = od_tuners->ignore_nice;
-		od_ops = dbs_data->gov_ops;
+		od_ops = dbs_data->cdata->gov_ops;
+		io_busy = od_tuners->io_is_busy;
 	}
 
 	switch (event) {
@@ -232,96 +352,53 @@ int cpufreq_governor_dbs(struct dbs_data *dbs_data,
 
 		for_each_cpu(j, policy->cpus) {
 			struct cpu_dbs_common_info *j_cdbs =
-				dbs_data->get_cpu_cdbs(j);
+				dbs_data->cdata->get_cpu_cdbs(j);
 
 			j_cdbs->cpu = j;
 			j_cdbs->cur_policy = policy;
 			j_cdbs->prev_cpu_idle = get_cpu_idle_time(j,
-					&j_cdbs->prev_cpu_wall);
+					       &j_cdbs->prev_cpu_wall, io_busy);
 			if (ignore_nice)
 				j_cdbs->prev_cpu_nice =
 					kcpustat_cpu(j).cpustat[CPUTIME_NICE];
 
 			mutex_init(&j_cdbs->timer_mutex);
 			INIT_DEFERRABLE_WORK(&j_cdbs->work,
-					     dbs_data->gov_dbs_timer);
-		}
-
-		if (!policy->governor->initialized) {
-			rc = sysfs_create_group(cpufreq_global_kobject,
-					dbs_data->attr_group);
-			if (rc) {
-				mutex_unlock(&dbs_data->mutex);
-				return rc;
-			}
+					     dbs_data->cdata->gov_dbs_timer);
 		}
 
 		/*
 		 * conservative does not implement micro like ondemand
 		 * governor, thus we are bound to jiffes/HZ
 		 */
-		if (dbs_data->governor == GOV_CONSERVATIVE) {
+		if (dbs_data->cdata->governor == GOV_CONSERVATIVE) {
 			cs_dbs_info->down_skip = 0;
 			cs_dbs_info->enable = 1;
 			cs_dbs_info->requested_freq = policy->cur;
-
-			if (!policy->governor->initialized) {
-				cpufreq_register_notifier(cs_ops->notifier_block,
-						CPUFREQ_TRANSITION_NOTIFIER);
-
-				dbs_data->min_sampling_rate =
-					MIN_SAMPLING_RATE_RATIO *
-					jiffies_to_usecs(10);
-			}
 		} else {
 			od_dbs_info->rate_mult = 1;
 			od_dbs_info->sample_type = OD_NORMAL_SAMPLE;
 			od_ops->powersave_bias_init_cpu(cpu);
-
-			if (!policy->governor->initialized)
-				od_tuners->io_is_busy = od_ops->io_busy();
 		}
 
-		if (policy->governor->initialized)
-			goto unlock;
-
-		/* policy latency is in nS. Convert it to uS first */
-		latency = policy->cpuinfo.transition_latency / 1000;
-		if (latency == 0)
-			latency = 1;
-
-		/* Bring kernel and HW constraints together */
-		dbs_data->min_sampling_rate = max(dbs_data->min_sampling_rate,
-				MIN_LATENCY_MULTIPLIER * latency);
-		*sampling_rate = max(dbs_data->min_sampling_rate, latency *
-				LATENCY_MULTIPLIER);
-unlock:
 		mutex_unlock(&dbs_data->mutex);
 
 		/* Initiate timer time stamp */
 		cpu_cdbs->time_stamp = ktime_get();
 
-		for_each_cpu(j, policy->cpus)
-			dbs_timer_init(dbs_data, j, *sampling_rate);
+		gov_queue_work(dbs_data, policy,
+				delay_for_sampling_rate(sampling_rate), true);
 		break;
 
 	case CPUFREQ_GOV_STOP:
-		if (dbs_data->governor == GOV_CONSERVATIVE)
+		if (dbs_data->cdata->governor == GOV_CONSERVATIVE)
 			cs_dbs_info->enable = 0;
 
-		for_each_cpu(j, policy->cpus)
-			dbs_timer_exit(dbs_data, j);
+		gov_cancel_work(dbs_data, policy);
 
 		mutex_lock(&dbs_data->mutex);
 		mutex_destroy(&cpu_cdbs->timer_mutex);
 
-		if (policy->governor->initialized == 1) {
-			sysfs_remove_group(cpufreq_global_kobject,
-					dbs_data->attr_group);
-			if (dbs_data->governor == GOV_CONSERVATIVE)
-				cpufreq_unregister_notifier(cs_ops->notifier_block,
-						CPUFREQ_TRANSITION_NOTIFIER);
-		}
 		mutex_unlock(&dbs_data->mutex);
 
 		break;
diff --git a/drivers/cpufreq/cpufreq_governor.h b/drivers/cpufreq/cpufreq_governor.h
index cc4bd2f6838a..8ac33538d0bd 100644
--- a/drivers/cpufreq/cpufreq_governor.h
+++ b/drivers/cpufreq/cpufreq_governor.h
@@ -34,20 +34,81 @@
  */
 #define MIN_SAMPLING_RATE_RATIO			(2)
 #define LATENCY_MULTIPLIER			(1000)
-#define MIN_LATENCY_MULTIPLIER			(100)
+#define MIN_LATENCY_MULTIPLIER			(20)
 #define TRANSITION_LATENCY_LIMIT		(10 * 1000 * 1000)
 
 /* Ondemand Sampling types */
 enum {OD_NORMAL_SAMPLE, OD_SUB_SAMPLE};
 
-/* Macro creating sysfs show routines */
-#define show_one(_gov, file_name, object)				\
-static ssize_t show_##file_name						\
+/*
+ * Macro for creating governors sysfs routines
+ *
+ * - gov_sys: One governor instance per whole system
+ * - gov_pol: One governor instance per policy
+ */
+
+/* Create attributes */
+#define gov_sys_attr_ro(_name)						\
+static struct global_attr _name##_gov_sys =				\
+__ATTR(_name, 0444, show_##_name##_gov_sys, NULL)
+
+#define gov_sys_attr_rw(_name)						\
+static struct global_attr _name##_gov_sys =				\
+__ATTR(_name, 0644, show_##_name##_gov_sys, store_##_name##_gov_sys)
+
+#define gov_pol_attr_ro(_name)						\
+static struct freq_attr _name##_gov_pol =				\
+__ATTR(_name, 0444, show_##_name##_gov_pol, NULL)
+
+#define gov_pol_attr_rw(_name)						\
+static struct freq_attr _name##_gov_pol =				\
+__ATTR(_name, 0644, show_##_name##_gov_pol, store_##_name##_gov_pol)
+
+#define gov_sys_pol_attr_rw(_name)					\
+	gov_sys_attr_rw(_name);						\
+	gov_pol_attr_rw(_name)
+
+#define gov_sys_pol_attr_ro(_name)					\
+	gov_sys_attr_ro(_name);						\
+	gov_pol_attr_ro(_name)
+
+/* Create show/store routines */
+#define show_one(_gov, file_name)					\
+static ssize_t show_##file_name##_gov_sys				\
 (struct kobject *kobj, struct attribute *attr, char *buf)		\
 {									\
-	return sprintf(buf, "%u\n", _gov##_tuners.object);		\
+	struct _gov##_dbs_tuners *tuners = _gov##_dbs_cdata.gdbs_data->tuners; \
+	return sprintf(buf, "%u\n", tuners->file_name);			\
+}									\
+									\
+static ssize_t show_##file_name##_gov_pol					\
+(struct cpufreq_policy *policy, char *buf)				\
+{									\
+	struct dbs_data *dbs_data = policy->governor_data;		\
+	struct _gov##_dbs_tuners *tuners = dbs_data->tuners;		\
+	return sprintf(buf, "%u\n", tuners->file_name);			\
+}
+
+#define store_one(_gov, file_name)					\
+static ssize_t store_##file_name##_gov_sys				\
+(struct kobject *kobj, struct attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct dbs_data *dbs_data = _gov##_dbs_cdata.gdbs_data;		\
+	return store_##file_name(dbs_data, buf, count);			\
+}									\
+									\
+static ssize_t store_##file_name##_gov_pol				\
+(struct cpufreq_policy *policy, const char *buf, size_t count)		\
+{									\
+	struct dbs_data *dbs_data = policy->governor_data;		\
+	return store_##file_name(dbs_data, buf, count);			\
 }
 
+#define show_store_one(_gov, file_name)					\
+show_one(_gov, file_name);						\
+store_one(_gov, file_name)
+
+/* create helper routines */
 #define define_get_cpu_dbs_routines(_dbs_info)				\
 static struct cpu_dbs_common_info *get_cpu_cdbs(int cpu)		\
 {									\
@@ -87,7 +148,6 @@ struct cpu_dbs_common_info {
 
 struct od_cpu_dbs_info_s {
 	struct cpu_dbs_common_info cdbs;
-	u64 prev_cpu_iowait;
 	struct cpufreq_frequency_table *freq_table;
 	unsigned int freq_lo;
 	unsigned int freq_lo_jiffies;
@@ -103,7 +163,7 @@ struct cs_cpu_dbs_info_s {
 	unsigned int enable:1;
 };
 
-/* Governers sysfs tunables */
+/* Per policy Governers sysfs tunables */
 struct od_dbs_tuners {
 	unsigned int ignore_nice;
 	unsigned int sampling_rate;
@@ -123,31 +183,42 @@ struct cs_dbs_tuners {
 	unsigned int freq_step;
 };
 
-/* Per Governer data */
-struct dbs_data {
+/* Common Governer data across policies */
+struct dbs_data;
+struct common_dbs_data {
 	/* Common across governors */
 	#define GOV_ONDEMAND		0
 	#define GOV_CONSERVATIVE	1
 	int governor;
-	unsigned int min_sampling_rate;
-	struct attribute_group *attr_group;
-	void *tuners;
+	struct attribute_group *attr_group_gov_sys; /* one governor - system */
+	struct attribute_group *attr_group_gov_pol; /* one governor - policy */
 
-	/* dbs_mutex protects dbs_enable in governor start/stop */
-	struct mutex mutex;
+	/* Common data for platforms that don't set have_governor_per_policy */
+	struct dbs_data *gdbs_data;
 
 	struct cpu_dbs_common_info *(*get_cpu_cdbs)(int cpu);
 	void *(*get_cpu_dbs_info_s)(int cpu);
 	void (*gov_dbs_timer)(struct work_struct *work);
 	void (*gov_check_cpu)(int cpu, unsigned int load);
+	int (*init)(struct dbs_data *dbs_data);
+	void (*exit)(struct dbs_data *dbs_data);
 
 	/* Governor specific ops, see below */
 	void *gov_ops;
 };
 
+/* Governer Per policy data */
+struct dbs_data {
+	struct common_dbs_data *cdata;
+	unsigned int min_sampling_rate;
+	void *tuners;
+
+	/* dbs_mutex protects dbs_enable in governor start/stop */
+	struct mutex mutex;
+};
+
 /* Governor specific ops, will be passed to dbs_data->gov_ops */
 struct od_ops {
-	int (*io_busy)(void);
 	void (*powersave_bias_init_cpu)(int cpu);
 	unsigned int (*powersave_bias_target)(struct cpufreq_policy *policy,
 			unsigned int freq_next, unsigned int relation);
@@ -169,10 +240,31 @@ static inline int delay_for_sampling_rate(unsigned int sampling_rate)
 	return delay;
 }
 
-u64 get_cpu_idle_time(unsigned int cpu, u64 *wall);
+#define declare_show_sampling_rate_min(_gov)				\
+static ssize_t show_sampling_rate_min_gov_sys				\
+(struct kobject *kobj, struct attribute *attr, char *buf)		\
+{									\
+	struct dbs_data *dbs_data = _gov##_dbs_cdata.gdbs_data;		\
+	return sprintf(buf, "%u\n", dbs_data->min_sampling_rate);	\
+}									\
+									\
+static ssize_t show_sampling_rate_min_gov_pol				\
+(struct cpufreq_policy *policy, char *buf)				\
+{									\
+	struct dbs_data *dbs_data = policy->governor_data;		\
+	return sprintf(buf, "%u\n", dbs_data->min_sampling_rate);	\
+}
+
+u64 get_cpu_idle_time(unsigned int cpu, u64 *wall, int io_busy);
 void dbs_check_cpu(struct dbs_data *dbs_data, int cpu);
 bool need_load_eval(struct cpu_dbs_common_info *cdbs,
 		unsigned int sampling_rate);
-int cpufreq_governor_dbs(struct dbs_data *dbs_data,
-		struct cpufreq_policy *policy, unsigned int event);
+int cpufreq_governor_dbs(struct cpufreq_policy *policy,
+		struct common_dbs_data *cdata, unsigned int event);
+void gov_queue_work(struct dbs_data *dbs_data, struct cpufreq_policy *policy,
+		unsigned int delay, bool all_cpus);
+void od_register_powersave_bias_handler(unsigned int (*f)
+		(struct cpufreq_policy *, unsigned int, unsigned int),
+		unsigned int powersave_bias);
+void od_unregister_powersave_bias_handler(void);
 #endif /* _CPUFREQ_GOVERNOR_H */
diff --git a/drivers/cpufreq/cpufreq_ondemand.c b/drivers/cpufreq/cpufreq_ondemand.c
index f3eb26cd848f..b0ffef96bf77 100644
--- a/drivers/cpufreq/cpufreq_ondemand.c
+++ b/drivers/cpufreq/cpufreq_ondemand.c
@@ -20,9 +20,11 @@
 #include <linux/module.h>
 #include <linux/mutex.h>
 #include <linux/percpu-defs.h>
+#include <linux/slab.h>
 #include <linux/sysfs.h>
 #include <linux/tick.h>
 #include <linux/types.h>
+#include <linux/cpu.h>
 
 #include "cpufreq_governor.h"
 
@@ -37,22 +39,14 @@
 #define MIN_FREQUENCY_UP_THRESHOLD		(11)
 #define MAX_FREQUENCY_UP_THRESHOLD		(100)
 
-static struct dbs_data od_dbs_data;
 static DEFINE_PER_CPU(struct od_cpu_dbs_info_s, od_cpu_dbs_info);
 
+static struct od_ops od_ops;
+
 #ifndef CONFIG_CPU_FREQ_DEFAULT_GOV_ONDEMAND
 static struct cpufreq_governor cpufreq_gov_ondemand;
 #endif
 
-static struct od_dbs_tuners od_tuners = {
-	.up_threshold = DEF_FREQUENCY_UP_THRESHOLD,
-	.sampling_down_factor = DEF_SAMPLING_DOWN_FACTOR,
-	.adj_up_threshold = DEF_FREQUENCY_UP_THRESHOLD -
-			    DEF_FREQUENCY_DOWN_DIFFERENTIAL,
-	.ignore_nice = 0,
-	.powersave_bias = 0,
-};
-
 static void ondemand_powersave_bias_init_cpu(int cpu)
 {
 	struct od_cpu_dbs_info_s *dbs_info = &per_cpu(od_cpu_dbs_info, cpu);
@@ -89,7 +83,7 @@ static int should_io_be_busy(void)
  * Returns the freq_hi to be used right now and will set freq_hi_jiffies,
  * freq_lo, and freq_lo_jiffies in percpu area for averaging freqs.
  */
-static unsigned int powersave_bias_target(struct cpufreq_policy *policy,
+static unsigned int generic_powersave_bias_target(struct cpufreq_policy *policy,
 		unsigned int freq_next, unsigned int relation)
 {
 	unsigned int freq_req, freq_reduc, freq_avg;
@@ -98,6 +92,8 @@ static unsigned int powersave_bias_target(struct cpufreq_policy *policy,
 	unsigned int jiffies_total, jiffies_hi, jiffies_lo;
 	struct od_cpu_dbs_info_s *dbs_info = &per_cpu(od_cpu_dbs_info,
 						   policy->cpu);
+	struct dbs_data *dbs_data = policy->governor_data;
+	struct od_dbs_tuners *od_tuners = dbs_data->tuners;
 
 	if (!dbs_info->freq_table) {
 		dbs_info->freq_lo = 0;
@@ -108,7 +104,7 @@ static unsigned int powersave_bias_target(struct cpufreq_policy *policy,
 	cpufreq_frequency_table_target(policy, dbs_info->freq_table, freq_next,
 			relation, &index);
 	freq_req = dbs_info->freq_table[index].frequency;
-	freq_reduc = freq_req * od_tuners.powersave_bias / 1000;
+	freq_reduc = freq_req * od_tuners->powersave_bias / 1000;
 	freq_avg = freq_req - freq_reduc;
 
 	/* Find freq bounds for freq_avg in freq_table */
@@ -127,7 +123,7 @@ static unsigned int powersave_bias_target(struct cpufreq_policy *policy,
 		dbs_info->freq_lo_jiffies = 0;
 		return freq_lo;
 	}
-	jiffies_total = usecs_to_jiffies(od_tuners.sampling_rate);
+	jiffies_total = usecs_to_jiffies(od_tuners->sampling_rate);
 	jiffies_hi = (freq_avg - freq_lo) * jiffies_total;
 	jiffies_hi += ((freq_hi - freq_lo) / 2);
 	jiffies_hi /= (freq_hi - freq_lo);
@@ -148,12 +144,16 @@ static void ondemand_powersave_bias_init(void)
 
 static void dbs_freq_increase(struct cpufreq_policy *p, unsigned int freq)
 {
-	if (od_tuners.powersave_bias)
-		freq = powersave_bias_target(p, freq, CPUFREQ_RELATION_H);
+	struct dbs_data *dbs_data = p->governor_data;
+	struct od_dbs_tuners *od_tuners = dbs_data->tuners;
+
+	if (od_tuners->powersave_bias)
+		freq = od_ops.powersave_bias_target(p, freq,
+				CPUFREQ_RELATION_H);
 	else if (p->cur == p->max)
 		return;
 
-	__cpufreq_driver_target(p, freq, od_tuners.powersave_bias ?
+	__cpufreq_driver_target(p, freq, od_tuners->powersave_bias ?
 			CPUFREQ_RELATION_L : CPUFREQ_RELATION_H);
 }
 
@@ -170,15 +170,17 @@ static void od_check_cpu(int cpu, unsigned int load_freq)
 {
 	struct od_cpu_dbs_info_s *dbs_info = &per_cpu(od_cpu_dbs_info, cpu);
 	struct cpufreq_policy *policy = dbs_info->cdbs.cur_policy;
+	struct dbs_data *dbs_data = policy->governor_data;
+	struct od_dbs_tuners *od_tuners = dbs_data->tuners;
 
 	dbs_info->freq_lo = 0;
 
 	/* Check for frequency increase */
-	if (load_freq > od_tuners.up_threshold * policy->cur) {
+	if (load_freq > od_tuners->up_threshold * policy->cur) {
 		/* If switching to max speed, apply sampling_down_factor */
 		if (policy->cur < policy->max)
 			dbs_info->rate_mult =
-				od_tuners.sampling_down_factor;
+				od_tuners->sampling_down_factor;
 		dbs_freq_increase(policy, policy->max);
 		return;
 	}
@@ -193,9 +195,10 @@ static void od_check_cpu(int cpu, unsigned int load_freq)
 	 * support the current CPU usage without triggering the up policy. To be
 	 * safe, we focus 10 points under the threshold.
 	 */
-	if (load_freq < od_tuners.adj_up_threshold * policy->cur) {
+	if (load_freq < od_tuners->adj_up_threshold
+			* policy->cur) {
 		unsigned int freq_next;
-		freq_next = load_freq / od_tuners.adj_up_threshold;
+		freq_next = load_freq / od_tuners->adj_up_threshold;
 
 		/* No longer fully busy, reset rate_mult */
 		dbs_info->rate_mult = 1;
@@ -203,65 +206,62 @@ static void od_check_cpu(int cpu, unsigned int load_freq)
 		if (freq_next < policy->min)
 			freq_next = policy->min;
 
-		if (!od_tuners.powersave_bias) {
+		if (!od_tuners->powersave_bias) {
 			__cpufreq_driver_target(policy, freq_next,
 					CPUFREQ_RELATION_L);
-		} else {
-			int freq = powersave_bias_target(policy, freq_next,
-					CPUFREQ_RELATION_L);
-			__cpufreq_driver_target(policy, freq,
-					CPUFREQ_RELATION_L);
+			return;
 		}
+
+		freq_next = od_ops.powersave_bias_target(policy, freq_next,
+					CPUFREQ_RELATION_L);
+		__cpufreq_driver_target(policy, freq_next, CPUFREQ_RELATION_L);
 	}
 }
 
 static void od_dbs_timer(struct work_struct *work)
 {
-	struct delayed_work *dw = to_delayed_work(work);
 	struct od_cpu_dbs_info_s *dbs_info =
 		container_of(work, struct od_cpu_dbs_info_s, cdbs.work.work);
 	unsigned int cpu = dbs_info->cdbs.cur_policy->cpu;
 	struct od_cpu_dbs_info_s *core_dbs_info = &per_cpu(od_cpu_dbs_info,
 			cpu);
-	int delay, sample_type = core_dbs_info->sample_type;
-	bool eval_load;
+	struct dbs_data *dbs_data = dbs_info->cdbs.cur_policy->governor_data;
+	struct od_dbs_tuners *od_tuners = dbs_data->tuners;
+	int delay = 0, sample_type = core_dbs_info->sample_type;
+	bool modify_all = true;
 
 	mutex_lock(&core_dbs_info->cdbs.timer_mutex);
-	eval_load = need_load_eval(&core_dbs_info->cdbs,
-			od_tuners.sampling_rate);
+	if (!need_load_eval(&core_dbs_info->cdbs, od_tuners->sampling_rate)) {
+		modify_all = false;
+		goto max_delay;
+	}
 
 	/* Common NORMAL_SAMPLE setup */
 	core_dbs_info->sample_type = OD_NORMAL_SAMPLE;
 	if (sample_type == OD_SUB_SAMPLE) {
 		delay = core_dbs_info->freq_lo_jiffies;
-		if (eval_load)
-			__cpufreq_driver_target(core_dbs_info->cdbs.cur_policy,
-						core_dbs_info->freq_lo,
-						CPUFREQ_RELATION_H);
+		__cpufreq_driver_target(core_dbs_info->cdbs.cur_policy,
+				core_dbs_info->freq_lo, CPUFREQ_RELATION_H);
 	} else {
-		if (eval_load)
-			dbs_check_cpu(&od_dbs_data, cpu);
+		dbs_check_cpu(dbs_data, cpu);
 		if (core_dbs_info->freq_lo) {
 			/* Setup timer for SUB_SAMPLE */
 			core_dbs_info->sample_type = OD_SUB_SAMPLE;
 			delay = core_dbs_info->freq_hi_jiffies;
-		} else {
-			delay = delay_for_sampling_rate(od_tuners.sampling_rate
-						* core_dbs_info->rate_mult);
 		}
 	}
 
-	schedule_delayed_work_on(smp_processor_id(), dw, delay);
+max_delay:
+	if (!delay)
+		delay = delay_for_sampling_rate(od_tuners->sampling_rate
+				* core_dbs_info->rate_mult);
+
+	gov_queue_work(dbs_data, dbs_info->cdbs.cur_policy, delay, modify_all);
 	mutex_unlock(&core_dbs_info->cdbs.timer_mutex);
 }
 
 /************************** sysfs interface ************************/
-
-static ssize_t show_sampling_rate_min(struct kobject *kobj,
-				      struct attribute *attr, char *buf)
-{
-	return sprintf(buf, "%u\n", od_dbs_data.min_sampling_rate);
-}
+static struct common_dbs_data od_dbs_cdata;
 
 /**
  * update_sampling_rate - update sampling rate effective immediately if needed.
@@ -276,12 +276,14 @@ static ssize_t show_sampling_rate_min(struct kobject *kobj,
  * reducing the sampling rate, we need to make the new value effective
  * immediately.
  */
-static void update_sampling_rate(unsigned int new_rate)
+static void update_sampling_rate(struct dbs_data *dbs_data,
+		unsigned int new_rate)
 {
+	struct od_dbs_tuners *od_tuners = dbs_data->tuners;
 	int cpu;
 
-	od_tuners.sampling_rate = new_rate = max(new_rate,
-			od_dbs_data.min_sampling_rate);
+	od_tuners->sampling_rate = new_rate = max(new_rate,
+			dbs_data->min_sampling_rate);
 
 	for_each_online_cpu(cpu) {
 		struct cpufreq_policy *policy;
@@ -314,42 +316,54 @@ static void update_sampling_rate(unsigned int new_rate)
 			cancel_delayed_work_sync(&dbs_info->cdbs.work);
 			mutex_lock(&dbs_info->cdbs.timer_mutex);
 
-			schedule_delayed_work_on(cpu, &dbs_info->cdbs.work,
-					usecs_to_jiffies(new_rate));
+			gov_queue_work(dbs_data, dbs_info->cdbs.cur_policy,
+					usecs_to_jiffies(new_rate), true);
 
 		}
 		mutex_unlock(&dbs_info->cdbs.timer_mutex);
 	}
 }
 
-static ssize_t store_sampling_rate(struct kobject *a, struct attribute *b,
-				   const char *buf, size_t count)
+static ssize_t store_sampling_rate(struct dbs_data *dbs_data, const char *buf,
+		size_t count)
 {
 	unsigned int input;
 	int ret;
 	ret = sscanf(buf, "%u", &input);
 	if (ret != 1)
 		return -EINVAL;
-	update_sampling_rate(input);
+
+	update_sampling_rate(dbs_data, input);
 	return count;
 }
 
-static ssize_t store_io_is_busy(struct kobject *a, struct attribute *b,
-				   const char *buf, size_t count)
+static ssize_t store_io_is_busy(struct dbs_data *dbs_data, const char *buf,
+		size_t count)
 {
+	struct od_dbs_tuners *od_tuners = dbs_data->tuners;
 	unsigned int input;
 	int ret;
+	unsigned int j;
 
 	ret = sscanf(buf, "%u", &input);
 	if (ret != 1)
 		return -EINVAL;
-	od_tuners.io_is_busy = !!input;
+	od_tuners->io_is_busy = !!input;
+
+	/* we need to re-evaluate prev_cpu_idle */
+	for_each_online_cpu(j) {
+		struct od_cpu_dbs_info_s *dbs_info = &per_cpu(od_cpu_dbs_info,
+									j);
+		dbs_info->cdbs.prev_cpu_idle = get_cpu_idle_time(j,
+			&dbs_info->cdbs.prev_cpu_wall, od_tuners->io_is_busy);
+	}
 	return count;
 }
 
-static ssize_t store_up_threshold(struct kobject *a, struct attribute *b,
-				  const char *buf, size_t count)
+static ssize_t store_up_threshold(struct dbs_data *dbs_data, const char *buf,
+		size_t count)
 {
+	struct od_dbs_tuners *od_tuners = dbs_data->tuners;
 	unsigned int input;
 	int ret;
 	ret = sscanf(buf, "%u", &input);
@@ -359,23 +373,24 @@ static ssize_t store_up_threshold(struct kobject *a, struct attribute *b,
 		return -EINVAL;
 	}
 	/* Calculate the new adj_up_threshold */
-	od_tuners.adj_up_threshold += input;
-	od_tuners.adj_up_threshold -= od_tuners.up_threshold;
+	od_tuners->adj_up_threshold += input;
+	od_tuners->adj_up_threshold -= od_tuners->up_threshold;
 
-	od_tuners.up_threshold = input;
+	od_tuners->up_threshold = input;
 	return count;
 }
 
-static ssize_t store_sampling_down_factor(struct kobject *a,
-			struct attribute *b, const char *buf, size_t count)
+static ssize_t store_sampling_down_factor(struct dbs_data *dbs_data,
+		const char *buf, size_t count)
 {
+	struct od_dbs_tuners *od_tuners = dbs_data->tuners;
 	unsigned int input, j;
 	int ret;
 	ret = sscanf(buf, "%u", &input);
 
 	if (ret != 1 || input > MAX_SAMPLING_DOWN_FACTOR || input < 1)
 		return -EINVAL;
-	od_tuners.sampling_down_factor = input;
+	od_tuners->sampling_down_factor = input;
 
 	/* Reset down sampling multiplier in case it was active */
 	for_each_online_cpu(j) {
@@ -386,9 +401,10 @@ static ssize_t store_sampling_down_factor(struct kobject *a,
 	return count;
 }
 
-static ssize_t store_ignore_nice_load(struct kobject *a, struct attribute *b,
-				      const char *buf, size_t count)
+static ssize_t store_ignore_nice(struct dbs_data *dbs_data, const char *buf,
+		size_t count)
 {
+	struct od_dbs_tuners *od_tuners = dbs_data->tuners;
 	unsigned int input;
 	int ret;
 
@@ -401,18 +417,18 @@ static ssize_t store_ignore_nice_load(struct kobject *a, struct attribute *b,
 	if (input > 1)
 		input = 1;
 
-	if (input == od_tuners.ignore_nice) { /* nothing to do */
+	if (input == od_tuners->ignore_nice) { /* nothing to do */
 		return count;
 	}
-	od_tuners.ignore_nice = input;
+	od_tuners->ignore_nice = input;
 
 	/* we need to re-evaluate prev_cpu_idle */
 	for_each_online_cpu(j) {
 		struct od_cpu_dbs_info_s *dbs_info;
 		dbs_info = &per_cpu(od_cpu_dbs_info, j);
 		dbs_info->cdbs.prev_cpu_idle = get_cpu_idle_time(j,
-						&dbs_info->cdbs.prev_cpu_wall);
-		if (od_tuners.ignore_nice)
+			&dbs_info->cdbs.prev_cpu_wall, od_tuners->io_is_busy);
+		if (od_tuners->ignore_nice)
 			dbs_info->cdbs.prev_cpu_nice =
 				kcpustat_cpu(j).cpustat[CPUTIME_NICE];
 
@@ -420,9 +436,10 @@ static ssize_t store_ignore_nice_load(struct kobject *a, struct attribute *b,
 	return count;
 }
 
-static ssize_t store_powersave_bias(struct kobject *a, struct attribute *b,
-				    const char *buf, size_t count)
+static ssize_t store_powersave_bias(struct dbs_data *dbs_data, const char *buf,
+		size_t count)
 {
+	struct od_dbs_tuners *od_tuners = dbs_data->tuners;
 	unsigned int input;
 	int ret;
 	ret = sscanf(buf, "%u", &input);
@@ -433,68 +450,179 @@ static ssize_t store_powersave_bias(struct kobject *a, struct attribute *b,
 	if (input > 1000)
 		input = 1000;
 
-	od_tuners.powersave_bias = input;
+	od_tuners->powersave_bias = input;
 	ondemand_powersave_bias_init();
 	return count;
 }
 
-show_one(od, sampling_rate, sampling_rate);
-show_one(od, io_is_busy, io_is_busy);
-show_one(od, up_threshold, up_threshold);
-show_one(od, sampling_down_factor, sampling_down_factor);
-show_one(od, ignore_nice_load, ignore_nice);
-show_one(od, powersave_bias, powersave_bias);
-
-define_one_global_rw(sampling_rate);
-define_one_global_rw(io_is_busy);
-define_one_global_rw(up_threshold);
-define_one_global_rw(sampling_down_factor);
-define_one_global_rw(ignore_nice_load);
-define_one_global_rw(powersave_bias);
-define_one_global_ro(sampling_rate_min);
-
-static struct attribute *dbs_attributes[] = {
-	&sampling_rate_min.attr,
-	&sampling_rate.attr,
-	&up_threshold.attr,
-	&sampling_down_factor.attr,
-	&ignore_nice_load.attr,
-	&powersave_bias.attr,
-	&io_is_busy.attr,
+show_store_one(od, sampling_rate);
+show_store_one(od, io_is_busy);
+show_store_one(od, up_threshold);
+show_store_one(od, sampling_down_factor);
+show_store_one(od, ignore_nice);
+show_store_one(od, powersave_bias);
+declare_show_sampling_rate_min(od);
+
+gov_sys_pol_attr_rw(sampling_rate);
+gov_sys_pol_attr_rw(io_is_busy);
+gov_sys_pol_attr_rw(up_threshold);
+gov_sys_pol_attr_rw(sampling_down_factor);
+gov_sys_pol_attr_rw(ignore_nice);
+gov_sys_pol_attr_rw(powersave_bias);
+gov_sys_pol_attr_ro(sampling_rate_min);
+
+static struct attribute *dbs_attributes_gov_sys[] = {
+	&sampling_rate_min_gov_sys.attr,
+	&sampling_rate_gov_sys.attr,
+	&up_threshold_gov_sys.attr,
+	&sampling_down_factor_gov_sys.attr,
+	&ignore_nice_gov_sys.attr,
+	&powersave_bias_gov_sys.attr,
+	&io_is_busy_gov_sys.attr,
+	NULL
+};
+
+static struct attribute_group od_attr_group_gov_sys = {
+	.attrs = dbs_attributes_gov_sys,
+	.name = "ondemand",
+};
+
+static struct attribute *dbs_attributes_gov_pol[] = {
+	&sampling_rate_min_gov_pol.attr,
+	&sampling_rate_gov_pol.attr,
+	&up_threshold_gov_pol.attr,
+	&sampling_down_factor_gov_pol.attr,
+	&ignore_nice_gov_pol.attr,
+	&powersave_bias_gov_pol.attr,
+	&io_is_busy_gov_pol.attr,
 	NULL
 };
 
-static struct attribute_group od_attr_group = {
-	.attrs = dbs_attributes,
+static struct attribute_group od_attr_group_gov_pol = {
+	.attrs = dbs_attributes_gov_pol,
 	.name = "ondemand",
 };
 
 /************************** sysfs end ************************/
 
+static int od_init(struct dbs_data *dbs_data)
+{
+	struct od_dbs_tuners *tuners;
+	u64 idle_time;
+	int cpu;
+
+	tuners = kzalloc(sizeof(struct od_dbs_tuners), GFP_KERNEL);
+	if (!tuners) {
+		pr_err("%s: kzalloc failed\n", __func__);
+		return -ENOMEM;
+	}
+
+	cpu = get_cpu();
+	idle_time = get_cpu_idle_time_us(cpu, NULL);
+	put_cpu();
+	if (idle_time != -1ULL) {
+		/* Idle micro accounting is supported. Use finer thresholds */
+		tuners->up_threshold = MICRO_FREQUENCY_UP_THRESHOLD;
+		tuners->adj_up_threshold = MICRO_FREQUENCY_UP_THRESHOLD -
+			MICRO_FREQUENCY_DOWN_DIFFERENTIAL;
+		/*
+		 * In nohz/micro accounting case we set the minimum frequency
+		 * not depending on HZ, but fixed (very low). The deferred
+		 * timer might skip some samples if idle/sleeping as needed.
+		*/
+		dbs_data->min_sampling_rate = MICRO_FREQUENCY_MIN_SAMPLE_RATE;
+	} else {
+		tuners->up_threshold = DEF_FREQUENCY_UP_THRESHOLD;
+		tuners->adj_up_threshold = DEF_FREQUENCY_UP_THRESHOLD -
+			DEF_FREQUENCY_DOWN_DIFFERENTIAL;
+
+		/* For correct statistics, we need 10 ticks for each measure */
+		dbs_data->min_sampling_rate = MIN_SAMPLING_RATE_RATIO *
+			jiffies_to_usecs(10);
+	}
+
+	tuners->sampling_down_factor = DEF_SAMPLING_DOWN_FACTOR;
+	tuners->ignore_nice = 0;
+	tuners->powersave_bias = 0;
+	tuners->io_is_busy = should_io_be_busy();
+
+	dbs_data->tuners = tuners;
+	pr_info("%s: tuners %p\n", __func__, tuners);
+	mutex_init(&dbs_data->mutex);
+	return 0;
+}
+
+static void od_exit(struct dbs_data *dbs_data)
+{
+	kfree(dbs_data->tuners);
+}
+
 define_get_cpu_dbs_routines(od_cpu_dbs_info);
 
 static struct od_ops od_ops = {
-	.io_busy = should_io_be_busy,
 	.powersave_bias_init_cpu = ondemand_powersave_bias_init_cpu,
-	.powersave_bias_target = powersave_bias_target,
+	.powersave_bias_target = generic_powersave_bias_target,
 	.freq_increase = dbs_freq_increase,
 };
 
-static struct dbs_data od_dbs_data = {
+static struct common_dbs_data od_dbs_cdata = {
 	.governor = GOV_ONDEMAND,
-	.attr_group = &od_attr_group,
-	.tuners = &od_tuners,
+	.attr_group_gov_sys = &od_attr_group_gov_sys,
+	.attr_group_gov_pol = &od_attr_group_gov_pol,
 	.get_cpu_cdbs = get_cpu_cdbs,
 	.get_cpu_dbs_info_s = get_cpu_dbs_info_s,
 	.gov_dbs_timer = od_dbs_timer,
 	.gov_check_cpu = od_check_cpu,
 	.gov_ops = &od_ops,
+	.init = od_init,
+	.exit = od_exit,
 };
 
+static void od_set_powersave_bias(unsigned int powersave_bias)
+{
+	struct cpufreq_policy *policy;
+	struct dbs_data *dbs_data;
+	struct od_dbs_tuners *od_tuners;
+	unsigned int cpu;
+	cpumask_t done;
+
+	cpumask_clear(&done);
+
+	get_online_cpus();
+	for_each_online_cpu(cpu) {
+		if (cpumask_test_cpu(cpu, &done))
+			continue;
+
+		policy = per_cpu(od_cpu_dbs_info, cpu).cdbs.cur_policy;
+		dbs_data = policy->governor_data;
+		od_tuners = dbs_data->tuners;
+		od_tuners->powersave_bias = powersave_bias;
+
+		cpumask_or(&done, &done, policy->cpus);
+	}
+	put_online_cpus();
+}
+
+void od_register_powersave_bias_handler(unsigned int (*f)
+		(struct cpufreq_policy *, unsigned int, unsigned int),
+		unsigned int powersave_bias)
+{
+	od_ops.powersave_bias_target = f;
+	od_set_powersave_bias(powersave_bias);
+}
+EXPORT_SYMBOL_GPL(od_register_powersave_bias_handler);
+
+void od_unregister_powersave_bias_handler(void)
+{
+	od_ops.powersave_bias_target = generic_powersave_bias_target;
+	od_set_powersave_bias(0);
+}
+EXPORT_SYMBOL_GPL(od_unregister_powersave_bias_handler);
+
 static int od_cpufreq_governor_dbs(struct cpufreq_policy *policy,
 		unsigned int event)
 {
-	return cpufreq_governor_dbs(&od_dbs_data, policy, event);
+	return cpufreq_governor_dbs(policy, &od_dbs_cdata, event);
 }
 
 #ifndef CONFIG_CPU_FREQ_DEFAULT_GOV_ONDEMAND
@@ -509,29 +637,6 @@ struct cpufreq_governor cpufreq_gov_ondemand = {
 
 static int __init cpufreq_gov_dbs_init(void)
 {
-	u64 idle_time;
-	int cpu = get_cpu();
-
-	mutex_init(&od_dbs_data.mutex);
-	idle_time = get_cpu_idle_time_us(cpu, NULL);
-	put_cpu();
-	if (idle_time != -1ULL) {
-		/* Idle micro accounting is supported. Use finer thresholds */
-		od_tuners.up_threshold = MICRO_FREQUENCY_UP_THRESHOLD;
-		od_tuners.adj_up_threshold = MICRO_FREQUENCY_UP_THRESHOLD -
-					     MICRO_FREQUENCY_DOWN_DIFFERENTIAL;
-		/*
-		 * In nohz/micro accounting case we set the minimum frequency
-		 * not depending on HZ, but fixed (very low). The deferred
-		 * timer might skip some samples if idle/sleeping as needed.
-		*/
-		od_dbs_data.min_sampling_rate = MICRO_FREQUENCY_MIN_SAMPLE_RATE;
-	} else {
-		/* For correct statistics, we need 10 ticks for each measure */
-		od_dbs_data.min_sampling_rate = MIN_SAMPLING_RATE_RATIO *
-			jiffies_to_usecs(10);
-	}
-
 	return cpufreq_register_governor(&cpufreq_gov_ondemand);
 }
 
diff --git a/drivers/cpufreq/cris-artpec3-cpufreq.c b/drivers/cpufreq/cris-artpec3-cpufreq.c
new file mode 100644
index 000000000000..ee142c490575
--- /dev/null
+++ b/drivers/cpufreq/cris-artpec3-cpufreq.c
@@ -0,0 +1,146 @@
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/cpufreq.h>
+#include <hwregs/reg_map.h>
+#include <hwregs/reg_rdwr.h>
+#include <hwregs/clkgen_defs.h>
+#include <hwregs/ddr2_defs.h>
+
+static int
+cris_sdram_freq_notifier(struct notifier_block *nb, unsigned long val,
+	void *data);
+
+static struct notifier_block cris_sdram_freq_notifier_block = {
+	.notifier_call = cris_sdram_freq_notifier
+};
+
+static struct cpufreq_frequency_table cris_freq_table[] = {
+	{0x01,	6000},
+	{0x02,	200000},
+	{0,	CPUFREQ_TABLE_END},
+};
+
+static unsigned int cris_freq_get_cpu_frequency(unsigned int cpu)
+{
+	reg_clkgen_rw_clk_ctrl clk_ctrl;
+	clk_ctrl = REG_RD(clkgen, regi_clkgen, rw_clk_ctrl);
+	return clk_ctrl.pll ? 200000 : 6000;
+}
+
+static void cris_freq_set_cpu_state(struct cpufreq_policy *policy,
+		unsigned int state)
+{
+	struct cpufreq_freqs freqs;
+	reg_clkgen_rw_clk_ctrl clk_ctrl;
+	clk_ctrl = REG_RD(clkgen, regi_clkgen, rw_clk_ctrl);
+
+	freqs.old = cris_freq_get_cpu_frequency(policy->cpu);
+	freqs.new = cris_freq_table[state].frequency;
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+	local_irq_disable();
+
+	/* Even though we may be SMP they will share the same clock
+	 * so all settings are made on CPU0. */
+	if (cris_freq_table[state].frequency == 200000)
+		clk_ctrl.pll = 1;
+	else
+		clk_ctrl.pll = 0;
+	REG_WR(clkgen, regi_clkgen, rw_clk_ctrl, clk_ctrl);
+
+	local_irq_enable();
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+};
+
+static int cris_freq_verify(struct cpufreq_policy *policy)
+{
+	return cpufreq_frequency_table_verify(policy, &cris_freq_table[0]);
+}
+
+static int cris_freq_target(struct cpufreq_policy *policy,
+			    unsigned int target_freq,
+			    unsigned int relation)
+{
+	unsigned int newstate = 0;
+
+	if (cpufreq_frequency_table_target(policy, cris_freq_table,
+			target_freq, relation, &newstate))
+		return -EINVAL;
+
+	cris_freq_set_cpu_state(policy, newstate);
+
+	return 0;
+}
+
+static int cris_freq_cpu_init(struct cpufreq_policy *policy)
+{
+	int result;
+
+	/* cpuinfo and default policy values */
+	policy->cpuinfo.transition_latency = 1000000; /* 1ms */
+	policy->cur = cris_freq_get_cpu_frequency(0);
+
+	result = cpufreq_frequency_table_cpuinfo(policy, cris_freq_table);
+	if (result)
+		return (result);
+
+	cpufreq_frequency_table_get_attr(cris_freq_table, policy->cpu);
+
+	return 0;
+}
+
+
+static int cris_freq_cpu_exit(struct cpufreq_policy *policy)
+{
+	cpufreq_frequency_table_put_attr(policy->cpu);
+	return 0;
+}
+
+
+static struct freq_attr *cris_freq_attr[] = {
+	&cpufreq_freq_attr_scaling_available_freqs,
+	NULL,
+};
+
+static struct cpufreq_driver cris_freq_driver = {
+	.get	= cris_freq_get_cpu_frequency,
+	.verify	= cris_freq_verify,
+	.target	= cris_freq_target,
+	.init	= cris_freq_cpu_init,
+	.exit	= cris_freq_cpu_exit,
+	.name	= "cris_freq",
+	.owner	= THIS_MODULE,
+	.attr	= cris_freq_attr,
+};
+
+static int __init cris_freq_init(void)
+{
+	int ret;
+	ret = cpufreq_register_driver(&cris_freq_driver);
+	cpufreq_register_notifier(&cris_sdram_freq_notifier_block,
+		CPUFREQ_TRANSITION_NOTIFIER);
+	return ret;
+}
+
+static int
+cris_sdram_freq_notifier(struct notifier_block *nb, unsigned long val,
+	void *data)
+{
+	int i;
+	struct cpufreq_freqs *freqs = data;
+	if (val == CPUFREQ_PRECHANGE) {
+		reg_ddr2_rw_cfg cfg =
+		  REG_RD(ddr2, regi_ddr2_ctrl, rw_cfg);
+		cfg.ref_interval = (freqs->new == 200000 ? 1560 : 46);
+
+		if (freqs->new == 200000)
+			for (i = 0; i < 50000; i++);
+		REG_WR(bif_core, regi_bif_core, rw_sdram_timing, timing);
+	}
+	return 0;
+}
+
+
+module_init(cris_freq_init);
diff --git a/drivers/cpufreq/cris-etraxfs-cpufreq.c b/drivers/cpufreq/cris-etraxfs-cpufreq.c
new file mode 100644
index 000000000000..12952235d5db
--- /dev/null
+++ b/drivers/cpufreq/cris-etraxfs-cpufreq.c
@@ -0,0 +1,142 @@
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/cpufreq.h>
+#include <hwregs/reg_map.h>
+#include <arch/hwregs/reg_rdwr.h>
+#include <arch/hwregs/config_defs.h>
+#include <arch/hwregs/bif_core_defs.h>
+
+static int
+cris_sdram_freq_notifier(struct notifier_block *nb, unsigned long val,
+			 void *data);
+
+static struct notifier_block cris_sdram_freq_notifier_block = {
+	.notifier_call = cris_sdram_freq_notifier
+};
+
+static struct cpufreq_frequency_table cris_freq_table[] = {
+	{0x01, 6000},
+	{0x02, 200000},
+	{0, CPUFREQ_TABLE_END},
+};
+
+static unsigned int cris_freq_get_cpu_frequency(unsigned int cpu)
+{
+	reg_config_rw_clk_ctrl clk_ctrl;
+	clk_ctrl = REG_RD(config, regi_config, rw_clk_ctrl);
+	return clk_ctrl.pll ? 200000 : 6000;
+}
+
+static void cris_freq_set_cpu_state(struct cpufreq_policy *policy,
+		unsigned int state)
+{
+	struct cpufreq_freqs freqs;
+	reg_config_rw_clk_ctrl clk_ctrl;
+	clk_ctrl = REG_RD(config, regi_config, rw_clk_ctrl);
+
+	freqs.old = cris_freq_get_cpu_frequency(policy->cpu);
+	freqs.new = cris_freq_table[state].frequency;
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+	local_irq_disable();
+
+	/* Even though we may be SMP they will share the same clock
+	 * so all settings are made on CPU0. */
+	if (cris_freq_table[state].frequency == 200000)
+		clk_ctrl.pll = 1;
+	else
+		clk_ctrl.pll = 0;
+	REG_WR(config, regi_config, rw_clk_ctrl, clk_ctrl);
+
+	local_irq_enable();
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+};
+
+static int cris_freq_verify(struct cpufreq_policy *policy)
+{
+	return cpufreq_frequency_table_verify(policy, &cris_freq_table[0]);
+}
+
+static int cris_freq_target(struct cpufreq_policy *policy,
+			    unsigned int target_freq, unsigned int relation)
+{
+	unsigned int newstate = 0;
+
+	if (cpufreq_frequency_table_target
+	    (policy, cris_freq_table, target_freq, relation, &newstate))
+		return -EINVAL;
+
+	cris_freq_set_cpu_state(policy, newstate);
+
+	return 0;
+}
+
+static int cris_freq_cpu_init(struct cpufreq_policy *policy)
+{
+	int result;
+
+	/* cpuinfo and default policy values */
+	policy->cpuinfo.transition_latency = 1000000;	/* 1ms */
+	policy->cur = cris_freq_get_cpu_frequency(0);
+
+	result = cpufreq_frequency_table_cpuinfo(policy, cris_freq_table);
+	if (result)
+		return (result);
+
+	cpufreq_frequency_table_get_attr(cris_freq_table, policy->cpu);
+
+	return 0;
+}
+
+static int cris_freq_cpu_exit(struct cpufreq_policy *policy)
+{
+	cpufreq_frequency_table_put_attr(policy->cpu);
+	return 0;
+}
+
+static struct freq_attr *cris_freq_attr[] = {
+	&cpufreq_freq_attr_scaling_available_freqs,
+	NULL,
+};
+
+static struct cpufreq_driver cris_freq_driver = {
+	.get = cris_freq_get_cpu_frequency,
+	.verify = cris_freq_verify,
+	.target = cris_freq_target,
+	.init = cris_freq_cpu_init,
+	.exit = cris_freq_cpu_exit,
+	.name = "cris_freq",
+	.owner = THIS_MODULE,
+	.attr = cris_freq_attr,
+};
+
+static int __init cris_freq_init(void)
+{
+	int ret;
+	ret = cpufreq_register_driver(&cris_freq_driver);
+	cpufreq_register_notifier(&cris_sdram_freq_notifier_block,
+				  CPUFREQ_TRANSITION_NOTIFIER);
+	return ret;
+}
+
+static int
+cris_sdram_freq_notifier(struct notifier_block *nb, unsigned long val,
+			 void *data)
+{
+	int i;
+	struct cpufreq_freqs *freqs = data;
+	if (val == CPUFREQ_PRECHANGE) {
+		reg_bif_core_rw_sdram_timing timing =
+		    REG_RD(bif_core, regi_bif_core, rw_sdram_timing);
+		timing.cpd = (freqs->new == 200000 ? 0 : 1);
+
+		if (freqs->new == 200000)
+			for (i = 0; i < 50000; i++) ;
+		REG_WR(bif_core, regi_bif_core, rw_sdram_timing, timing);
+	}
+	return 0;
+}
+
+module_init(cris_freq_init);
diff --git a/drivers/cpufreq/davinci-cpufreq.c b/drivers/cpufreq/davinci-cpufreq.c
new file mode 100644
index 000000000000..c33c76c360fa
--- /dev/null
+++ b/drivers/cpufreq/davinci-cpufreq.c
@@ -0,0 +1,231 @@
+/*
+ * CPU frequency scaling for DaVinci
+ *
+ * Copyright (C) 2009 Texas Instruments Incorporated - http://www.ti.com/
+ *
+ * Based on linux/arch/arm/plat-omap/cpu-omap.c. Original Copyright follows:
+ *
+ *  Copyright (C) 2005 Nokia Corporation
+ *  Written by Tony Lindgren <tony@atomide.com>
+ *
+ *  Based on cpu-sa1110.c, Copyright (C) 2001 Russell King
+ *
+ * Copyright (C) 2007-2008 Texas Instruments, Inc.
+ * Updated to support OMAP3
+ * Rajendra Nayak <rnayak@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.
+ */
+#include <linux/types.h>
+#include <linux/cpufreq.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/export.h>
+
+#include <mach/hardware.h>
+#include <mach/cpufreq.h>
+#include <mach/common.h>
+
+struct davinci_cpufreq {
+	struct device *dev;
+	struct clk *armclk;
+	struct clk *asyncclk;
+	unsigned long asyncrate;
+};
+static struct davinci_cpufreq cpufreq;
+
+static int davinci_verify_speed(struct cpufreq_policy *policy)
+{
+	struct davinci_cpufreq_config *pdata = cpufreq.dev->platform_data;
+	struct cpufreq_frequency_table *freq_table = pdata->freq_table;
+	struct clk *armclk = cpufreq.armclk;
+
+	if (freq_table)
+		return cpufreq_frequency_table_verify(policy, freq_table);
+
+	if (policy->cpu)
+		return -EINVAL;
+
+	cpufreq_verify_within_limits(policy, policy->cpuinfo.min_freq,
+				     policy->cpuinfo.max_freq);
+
+	policy->min = clk_round_rate(armclk, policy->min * 1000) / 1000;
+	policy->max = clk_round_rate(armclk, policy->max * 1000) / 1000;
+	cpufreq_verify_within_limits(policy, policy->cpuinfo.min_freq,
+						policy->cpuinfo.max_freq);
+	return 0;
+}
+
+static unsigned int davinci_getspeed(unsigned int cpu)
+{
+	if (cpu)
+		return 0;
+
+	return clk_get_rate(cpufreq.armclk) / 1000;
+}
+
+static int davinci_target(struct cpufreq_policy *policy,
+				unsigned int target_freq, unsigned int relation)
+{
+	int ret = 0;
+	unsigned int idx;
+	struct cpufreq_freqs freqs;
+	struct davinci_cpufreq_config *pdata = cpufreq.dev->platform_data;
+	struct clk *armclk = cpufreq.armclk;
+
+	freqs.old = davinci_getspeed(0);
+	freqs.new = clk_round_rate(armclk, target_freq * 1000) / 1000;
+
+	if (freqs.old == freqs.new)
+		return ret;
+
+	dev_dbg(cpufreq.dev, "transition: %u --> %u\n", freqs.old, freqs.new);
+
+	ret = cpufreq_frequency_table_target(policy, pdata->freq_table,
+						freqs.new, relation, &idx);
+	if (ret)
+		return -EINVAL;
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+	/* if moving to higher frequency, up the voltage beforehand */
+	if (pdata->set_voltage && freqs.new > freqs.old) {
+		ret = pdata->set_voltage(idx);
+		if (ret)
+			goto out;
+	}
+
+	ret = clk_set_rate(armclk, idx);
+	if (ret)
+		goto out;
+
+	if (cpufreq.asyncclk) {
+		ret = clk_set_rate(cpufreq.asyncclk, cpufreq.asyncrate);
+		if (ret)
+			goto out;
+	}
+
+	/* if moving to lower freq, lower the voltage after lowering freq */
+	if (pdata->set_voltage && freqs.new < freqs.old)
+		pdata->set_voltage(idx);
+
+out:
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	return ret;
+}
+
+static int davinci_cpu_init(struct cpufreq_policy *policy)
+{
+	int result = 0;
+	struct davinci_cpufreq_config *pdata = cpufreq.dev->platform_data;
+	struct cpufreq_frequency_table *freq_table = pdata->freq_table;
+
+	if (policy->cpu != 0)
+		return -EINVAL;
+
+	/* Finish platform specific initialization */
+	if (pdata->init) {
+		result = pdata->init();
+		if (result)
+			return result;
+	}
+
+	policy->cur = davinci_getspeed(0);
+
+	result = cpufreq_frequency_table_cpuinfo(policy, freq_table);
+	if (result) {
+		pr_err("%s: cpufreq_frequency_table_cpuinfo() failed",
+				__func__);
+		return result;
+	}
+
+	cpufreq_frequency_table_get_attr(freq_table, policy->cpu);
+
+	/*
+	 * Time measurement across the target() function yields ~1500-1800us
+	 * time taken with no drivers on notification list.
+	 * Setting the latency to 2000 us to accommodate addition of drivers
+	 * to pre/post change notification list.
+	 */
+	policy->cpuinfo.transition_latency = 2000 * 1000;
+	return 0;
+}
+
+static int davinci_cpu_exit(struct cpufreq_policy *policy)
+{
+	cpufreq_frequency_table_put_attr(policy->cpu);
+	return 0;
+}
+
+static struct freq_attr *davinci_cpufreq_attr[] = {
+	&cpufreq_freq_attr_scaling_available_freqs,
+	NULL,
+};
+
+static struct cpufreq_driver davinci_driver = {
+	.flags		= CPUFREQ_STICKY,
+	.verify		= davinci_verify_speed,
+	.target		= davinci_target,
+	.get		= davinci_getspeed,
+	.init		= davinci_cpu_init,
+	.exit		= davinci_cpu_exit,
+	.name		= "davinci",
+	.attr		= davinci_cpufreq_attr,
+};
+
+static int __init davinci_cpufreq_probe(struct platform_device *pdev)
+{
+	struct davinci_cpufreq_config *pdata = pdev->dev.platform_data;
+	struct clk *asyncclk;
+
+	if (!pdata)
+		return -EINVAL;
+	if (!pdata->freq_table)
+		return -EINVAL;
+
+	cpufreq.dev = &pdev->dev;
+
+	cpufreq.armclk = clk_get(NULL, "arm");
+	if (IS_ERR(cpufreq.armclk)) {
+		dev_err(cpufreq.dev, "Unable to get ARM clock\n");
+		return PTR_ERR(cpufreq.armclk);
+	}
+
+	asyncclk = clk_get(cpufreq.dev, "async");
+	if (!IS_ERR(asyncclk)) {
+		cpufreq.asyncclk = asyncclk;
+		cpufreq.asyncrate = clk_get_rate(asyncclk);
+	}
+
+	return cpufreq_register_driver(&davinci_driver);
+}
+
+static int __exit davinci_cpufreq_remove(struct platform_device *pdev)
+{
+	clk_put(cpufreq.armclk);
+
+	if (cpufreq.asyncclk)
+		clk_put(cpufreq.asyncclk);
+
+	return cpufreq_unregister_driver(&davinci_driver);
+}
+
+static struct platform_driver davinci_cpufreq_driver = {
+	.driver = {
+		.name	 = "cpufreq-davinci",
+		.owner	 = THIS_MODULE,
+	},
+	.remove = __exit_p(davinci_cpufreq_remove),
+};
+
+int __init davinci_cpufreq_init(void)
+{
+	return platform_driver_probe(&davinci_cpufreq_driver,
+							davinci_cpufreq_probe);
+}
+
diff --git a/drivers/cpufreq/dbx500-cpufreq.c b/drivers/cpufreq/dbx500-cpufreq.c
index 72f0c3efa76e..6ec6539ae041 100644
--- a/drivers/cpufreq/dbx500-cpufreq.c
+++ b/drivers/cpufreq/dbx500-cpufreq.c
@@ -37,12 +37,6 @@ static int dbx500_cpufreq_target(struct cpufreq_policy *policy,
 	unsigned int idx;
 	int ret;
 
-	/* scale the target frequency to one of the extremes supported */
-	if (target_freq < policy->cpuinfo.min_freq)
-		target_freq = policy->cpuinfo.min_freq;
-	if (target_freq > policy->cpuinfo.max_freq)
-		target_freq = policy->cpuinfo.max_freq;
-
 	/* Lookup the next frequency */
 	if (cpufreq_frequency_table_target(policy, freq_table, target_freq,
 					relation, &idx))
@@ -55,8 +49,7 @@ static int dbx500_cpufreq_target(struct cpufreq_policy *policy,
 		return 0;
 
 	/* pre-change notification */
-	for_each_cpu(freqs.cpu, policy->cpus)
-		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	/* update armss clk frequency */
 	ret = clk_set_rate(armss_clk, freqs.new * 1000);
@@ -68,8 +61,7 @@ static int dbx500_cpufreq_target(struct cpufreq_policy *policy,
 	}
 
 	/* post change notification */
-	for_each_cpu(freqs.cpu, policy->cpus)
-		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 	return 0;
 }
@@ -79,15 +71,15 @@ static unsigned int dbx500_cpufreq_getspeed(unsigned int cpu)
 	int i = 0;
 	unsigned long freq = clk_get_rate(armss_clk) / 1000;
 
-	while (freq_table[i].frequency != CPUFREQ_TABLE_END) {
-		if (freq <= freq_table[i].frequency)
+	/* The value is rounded to closest frequency in the defined table. */
+	while (freq_table[i + 1].frequency != CPUFREQ_TABLE_END) {
+		if (freq < freq_table[i].frequency +
+		   (freq_table[i + 1].frequency - freq_table[i].frequency) / 2)
 			return freq_table[i].frequency;
 		i++;
 	}
 
-	/* We could not find a corresponding frequency. */
-	pr_err("dbx500-cpufreq: Failed to find cpufreq speed\n");
-	return 0;
+	return freq_table[i].frequency;
 }
 
 static int __cpuinit dbx500_cpufreq_init(struct cpufreq_policy *policy)
diff --git a/drivers/cpufreq/e_powersaver.c b/drivers/cpufreq/e_powersaver.c
index 3fffbe6025cd..37380fb92621 100644
--- a/drivers/cpufreq/e_powersaver.c
+++ b/drivers/cpufreq/e_powersaver.c
@@ -104,7 +104,7 @@ static unsigned int eps_get(unsigned int cpu)
 }
 
 static int eps_set_state(struct eps_cpu_data *centaur,
-			 unsigned int cpu,
+			 struct cpufreq_policy *policy,
 			 u32 dest_state)
 {
 	struct cpufreq_freqs freqs;
@@ -112,10 +112,9 @@ static int eps_set_state(struct eps_cpu_data *centaur,
 	int err = 0;
 	int i;
 
-	freqs.old = eps_get(cpu);
+	freqs.old = eps_get(policy->cpu);
 	freqs.new = centaur->fsb * ((dest_state >> 8) & 0xff);
-	freqs.cpu = cpu;
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	/* Wait while CPU is busy */
 	rdmsr(MSR_IA32_PERF_STATUS, lo, hi);
@@ -162,7 +161,7 @@ postchange:
 		current_multiplier);
 	}
 #endif
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 	return err;
 }
 
@@ -190,7 +189,7 @@ static int eps_target(struct cpufreq_policy *policy,
 
 	/* Make frequency transition */
 	dest_state = centaur->freq_table[newstate].index & 0xffff;
-	ret = eps_set_state(centaur, cpu, dest_state);
+	ret = eps_set_state(centaur, policy, dest_state);
 	if (ret)
 		printk(KERN_ERR "eps: Timeout!\n");
 	return ret;
diff --git a/drivers/cpufreq/elanfreq.c b/drivers/cpufreq/elanfreq.c
index 960671fd3d7e..658d860344b0 100644
--- a/drivers/cpufreq/elanfreq.c
+++ b/drivers/cpufreq/elanfreq.c
@@ -117,15 +117,15 @@ static unsigned int elanfreq_get_cpu_frequency(unsigned int cpu)
  *	There is no return value.
  */
 
-static void elanfreq_set_cpu_state(unsigned int state)
+static void elanfreq_set_cpu_state(struct cpufreq_policy *policy,
+		unsigned int state)
 {
 	struct cpufreq_freqs    freqs;
 
 	freqs.old = elanfreq_get_cpu_frequency(0);
 	freqs.new = elan_multiplier[state].clock;
-	freqs.cpu = 0; /* elanfreq.c is UP only driver */
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	printk(KERN_INFO "elanfreq: attempting to set frequency to %i kHz\n",
 			elan_multiplier[state].clock);
@@ -161,7 +161,7 @@ static void elanfreq_set_cpu_state(unsigned int state)
 	udelay(10000);
 	local_irq_enable();
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 };
 
 
@@ -188,7 +188,7 @@ static int elanfreq_target(struct cpufreq_policy *policy,
 				target_freq, relation, &newstate))
 		return -EINVAL;
 
-	elanfreq_set_cpu_state(newstate);
+	elanfreq_set_cpu_state(policy, newstate);
 
 	return 0;
 }
diff --git a/drivers/cpufreq/exynos-cpufreq.c b/drivers/cpufreq/exynos-cpufreq.c
index 78057a357ddb..475b4f607f0d 100644
--- a/drivers/cpufreq/exynos-cpufreq.c
+++ b/drivers/cpufreq/exynos-cpufreq.c
@@ -70,7 +70,6 @@ static int exynos_cpufreq_scale(unsigned int target_freq)
 
 	freqs.old = policy->cur;
 	freqs.new = target_freq;
-	freqs.cpu = policy->cpu;
 
 	if (freqs.new == freqs.old)
 		goto out;
@@ -105,8 +104,7 @@ static int exynos_cpufreq_scale(unsigned int target_freq)
 	}
 	arm_volt = volt_table[index];
 
-	for_each_cpu(freqs.cpu, policy->cpus)
-		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	/* When the new frequency is higher than current frequency */
 	if ((freqs.new > freqs.old) && !safe_arm_volt) {
@@ -131,8 +129,7 @@ static int exynos_cpufreq_scale(unsigned int target_freq)
 
 	exynos_info->set_freq(old_index, index);
 
-	for_each_cpu(freqs.cpu, policy->cpus)
-		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 	/* When the new frequency is lower than current frequency */
 	if ((freqs.new < freqs.old) ||
@@ -297,7 +294,7 @@ static int __init exynos_cpufreq_init(void)
 	else if (soc_is_exynos5250())
 		ret = exynos5250_cpufreq_init(exynos_info);
 	else
-		pr_err("%s: CPU type not found\n", __func__);
+		return 0;
 
 	if (ret)
 		goto err_vdd_arm;
diff --git a/drivers/cpufreq/exynos5440-cpufreq.c b/drivers/cpufreq/exynos5440-cpufreq.c
new file mode 100644
index 000000000000..0c74018eda47
--- /dev/null
+++ b/drivers/cpufreq/exynos5440-cpufreq.c
@@ -0,0 +1,481 @@
+/*
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Amit Daniel Kachhap <amit.daniel@samsung.com>
+ *
+ * EXYNOS5440 - CPU frequency scaling support
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#include <linux/clk.h>
+#include <linux/cpu.h>
+#include <linux/cpufreq.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/opp.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+/* Register definitions */
+#define XMU_DVFS_CTRL		0x0060
+#define XMU_PMU_P0_7		0x0064
+#define XMU_C0_3_PSTATE		0x0090
+#define XMU_P_LIMIT		0x00a0
+#define XMU_P_STATUS		0x00a4
+#define XMU_PMUEVTEN		0x00d0
+#define XMU_PMUIRQEN		0x00d4
+#define XMU_PMUIRQ		0x00d8
+
+/* PMU mask and shift definations */
+#define P_VALUE_MASK		0x7
+
+#define XMU_DVFS_CTRL_EN_SHIFT	0
+
+#define P0_7_CPUCLKDEV_SHIFT	21
+#define P0_7_CPUCLKDEV_MASK	0x7
+#define P0_7_ATBCLKDEV_SHIFT	18
+#define P0_7_ATBCLKDEV_MASK	0x7
+#define P0_7_CSCLKDEV_SHIFT	15
+#define P0_7_CSCLKDEV_MASK	0x7
+#define P0_7_CPUEMA_SHIFT	28
+#define P0_7_CPUEMA_MASK	0xf
+#define P0_7_L2EMA_SHIFT	24
+#define P0_7_L2EMA_MASK		0xf
+#define P0_7_VDD_SHIFT		8
+#define P0_7_VDD_MASK		0x7f
+#define P0_7_FREQ_SHIFT		0
+#define P0_7_FREQ_MASK		0xff
+
+#define C0_3_PSTATE_VALID_SHIFT	8
+#define C0_3_PSTATE_CURR_SHIFT	4
+#define C0_3_PSTATE_NEW_SHIFT	0
+
+#define PSTATE_CHANGED_EVTEN_SHIFT	0
+
+#define PSTATE_CHANGED_IRQEN_SHIFT	0
+
+#define PSTATE_CHANGED_SHIFT		0
+
+/* some constant values for clock divider calculation */
+#define CPU_DIV_FREQ_MAX	500
+#define CPU_DBG_FREQ_MAX	375
+#define CPU_ATB_FREQ_MAX	500
+
+#define PMIC_LOW_VOLT		0x30
+#define PMIC_HIGH_VOLT		0x28
+
+#define CPUEMA_HIGH		0x2
+#define CPUEMA_MID		0x4
+#define CPUEMA_LOW		0x7
+
+#define L2EMA_HIGH		0x1
+#define L2EMA_MID		0x3
+#define L2EMA_LOW		0x4
+
+#define DIV_TAB_MAX	2
+/* frequency unit is 20MHZ */
+#define FREQ_UNIT	20
+#define MAX_VOLTAGE	1550000 /* In microvolt */
+#define VOLTAGE_STEP	12500	/* In microvolt */
+
+#define CPUFREQ_NAME		"exynos5440_dvfs"
+#define DEF_TRANS_LATENCY	100000
+
+enum cpufreq_level_index {
+	L0, L1, L2, L3, L4,
+	L5, L6, L7, L8, L9,
+};
+#define CPUFREQ_LEVEL_END	(L7 + 1)
+
+struct exynos_dvfs_data {
+	void __iomem *base;
+	struct resource *mem;
+	int irq;
+	struct clk *cpu_clk;
+	unsigned int cur_frequency;
+	unsigned int latency;
+	struct cpufreq_frequency_table *freq_table;
+	unsigned int freq_count;
+	struct device *dev;
+	bool dvfs_enabled;
+	struct work_struct irq_work;
+};
+
+static struct exynos_dvfs_data *dvfs_info;
+static DEFINE_MUTEX(cpufreq_lock);
+static struct cpufreq_freqs freqs;
+
+static int init_div_table(void)
+{
+	struct cpufreq_frequency_table *freq_tbl = dvfs_info->freq_table;
+	unsigned int tmp, clk_div, ema_div, freq, volt_id;
+	int i = 0;
+	struct opp *opp;
+
+	rcu_read_lock();
+	for (i = 0; freq_tbl[i].frequency != CPUFREQ_TABLE_END; i++) {
+
+		opp = opp_find_freq_exact(dvfs_info->dev,
+					freq_tbl[i].frequency * 1000, true);
+		if (IS_ERR(opp)) {
+			rcu_read_unlock();
+			dev_err(dvfs_info->dev,
+				"failed to find valid OPP for %u KHZ\n",
+				freq_tbl[i].frequency);
+			return PTR_ERR(opp);
+		}
+
+		freq = freq_tbl[i].frequency / 1000; /* In MHZ */
+		clk_div = ((freq / CPU_DIV_FREQ_MAX) & P0_7_CPUCLKDEV_MASK)
+					<< P0_7_CPUCLKDEV_SHIFT;
+		clk_div |= ((freq / CPU_ATB_FREQ_MAX) & P0_7_ATBCLKDEV_MASK)
+					<< P0_7_ATBCLKDEV_SHIFT;
+		clk_div |= ((freq / CPU_DBG_FREQ_MAX) & P0_7_CSCLKDEV_MASK)
+					<< P0_7_CSCLKDEV_SHIFT;
+
+		/* Calculate EMA */
+		volt_id = opp_get_voltage(opp);
+		volt_id = (MAX_VOLTAGE - volt_id) / VOLTAGE_STEP;
+		if (volt_id < PMIC_HIGH_VOLT) {
+			ema_div = (CPUEMA_HIGH << P0_7_CPUEMA_SHIFT) |
+				(L2EMA_HIGH << P0_7_L2EMA_SHIFT);
+		} else if (volt_id > PMIC_LOW_VOLT) {
+			ema_div = (CPUEMA_LOW << P0_7_CPUEMA_SHIFT) |
+				(L2EMA_LOW << P0_7_L2EMA_SHIFT);
+		} else {
+			ema_div = (CPUEMA_MID << P0_7_CPUEMA_SHIFT) |
+				(L2EMA_MID << P0_7_L2EMA_SHIFT);
+		}
+
+		tmp = (clk_div | ema_div | (volt_id << P0_7_VDD_SHIFT)
+			| ((freq / FREQ_UNIT) << P0_7_FREQ_SHIFT));
+
+		__raw_writel(tmp, dvfs_info->base + XMU_PMU_P0_7 + 4 * i);
+	}
+
+	rcu_read_unlock();
+	return 0;
+}
+
+static void exynos_enable_dvfs(void)
+{
+	unsigned int tmp, i, cpu;
+	struct cpufreq_frequency_table *freq_table = dvfs_info->freq_table;
+	/* Disable DVFS */
+	__raw_writel(0,	dvfs_info->base + XMU_DVFS_CTRL);
+
+	/* Enable PSTATE Change Event */
+	tmp = __raw_readl(dvfs_info->base + XMU_PMUEVTEN);
+	tmp |= (1 << PSTATE_CHANGED_EVTEN_SHIFT);
+	 __raw_writel(tmp, dvfs_info->base + XMU_PMUEVTEN);
+
+	/* Enable PSTATE Change IRQ */
+	tmp = __raw_readl(dvfs_info->base + XMU_PMUIRQEN);
+	tmp |= (1 << PSTATE_CHANGED_IRQEN_SHIFT);
+	 __raw_writel(tmp, dvfs_info->base + XMU_PMUIRQEN);
+
+	/* Set initial performance index */
+	for (i = 0; freq_table[i].frequency != CPUFREQ_TABLE_END; i++)
+		if (freq_table[i].frequency == dvfs_info->cur_frequency)
+			break;
+
+	if (freq_table[i].frequency == CPUFREQ_TABLE_END) {
+		dev_crit(dvfs_info->dev, "Boot up frequency not supported\n");
+		/* Assign the highest frequency */
+		i = 0;
+		dvfs_info->cur_frequency = freq_table[i].frequency;
+	}
+
+	dev_info(dvfs_info->dev, "Setting dvfs initial frequency = %uKHZ",
+						dvfs_info->cur_frequency);
+
+	for (cpu = 0; cpu < CONFIG_NR_CPUS; cpu++) {
+		tmp = __raw_readl(dvfs_info->base + XMU_C0_3_PSTATE + cpu * 4);
+		tmp &= ~(P_VALUE_MASK << C0_3_PSTATE_NEW_SHIFT);
+		tmp |= (i << C0_3_PSTATE_NEW_SHIFT);
+		__raw_writel(tmp, dvfs_info->base + XMU_C0_3_PSTATE + cpu * 4);
+	}
+
+	/* Enable DVFS */
+	__raw_writel(1 << XMU_DVFS_CTRL_EN_SHIFT,
+				dvfs_info->base + XMU_DVFS_CTRL);
+}
+
+static int exynos_verify_speed(struct cpufreq_policy *policy)
+{
+	return cpufreq_frequency_table_verify(policy,
+					      dvfs_info->freq_table);
+}
+
+static unsigned int exynos_getspeed(unsigned int cpu)
+{
+	return dvfs_info->cur_frequency;
+}
+
+static int exynos_target(struct cpufreq_policy *policy,
+			  unsigned int target_freq,
+			  unsigned int relation)
+{
+	unsigned int index, tmp;
+	int ret = 0, i;
+	struct cpufreq_frequency_table *freq_table = dvfs_info->freq_table;
+
+	mutex_lock(&cpufreq_lock);
+
+	ret = cpufreq_frequency_table_target(policy, freq_table,
+					   target_freq, relation, &index);
+	if (ret)
+		goto out;
+
+	freqs.old = dvfs_info->cur_frequency;
+	freqs.new = freq_table[index].frequency;
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+	/* Set the target frequency in all C0_3_PSTATE register */
+	for_each_cpu(i, policy->cpus) {
+		tmp = __raw_readl(dvfs_info->base + XMU_C0_3_PSTATE + i * 4);
+		tmp &= ~(P_VALUE_MASK << C0_3_PSTATE_NEW_SHIFT);
+		tmp |= (index << C0_3_PSTATE_NEW_SHIFT);
+
+		__raw_writel(tmp, dvfs_info->base + XMU_C0_3_PSTATE + i * 4);
+	}
+out:
+	mutex_unlock(&cpufreq_lock);
+	return ret;
+}
+
+static void exynos_cpufreq_work(struct work_struct *work)
+{
+	unsigned int cur_pstate, index;
+	struct cpufreq_policy *policy = cpufreq_cpu_get(0); /* boot CPU */
+	struct cpufreq_frequency_table *freq_table = dvfs_info->freq_table;
+
+	/* Ensure we can access cpufreq structures */
+	if (unlikely(dvfs_info->dvfs_enabled == false))
+		goto skip_work;
+
+	mutex_lock(&cpufreq_lock);
+	freqs.old = dvfs_info->cur_frequency;
+
+	cur_pstate = __raw_readl(dvfs_info->base + XMU_P_STATUS);
+	if (cur_pstate >> C0_3_PSTATE_VALID_SHIFT & 0x1)
+		index = (cur_pstate >> C0_3_PSTATE_CURR_SHIFT) & P_VALUE_MASK;
+	else
+		index = (cur_pstate >> C0_3_PSTATE_NEW_SHIFT) & P_VALUE_MASK;
+
+	if (likely(index < dvfs_info->freq_count)) {
+		freqs.new = freq_table[index].frequency;
+		dvfs_info->cur_frequency = freqs.new;
+	} else {
+		dev_crit(dvfs_info->dev, "New frequency out of range\n");
+		freqs.new = dvfs_info->cur_frequency;
+	}
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	cpufreq_cpu_put(policy);
+	mutex_unlock(&cpufreq_lock);
+skip_work:
+	enable_irq(dvfs_info->irq);
+}
+
+static irqreturn_t exynos_cpufreq_irq(int irq, void *id)
+{
+	unsigned int tmp;
+
+	tmp = __raw_readl(dvfs_info->base + XMU_PMUIRQ);
+	if (tmp >> PSTATE_CHANGED_SHIFT & 0x1) {
+		__raw_writel(tmp, dvfs_info->base + XMU_PMUIRQ);
+		disable_irq_nosync(irq);
+		schedule_work(&dvfs_info->irq_work);
+	}
+	return IRQ_HANDLED;
+}
+
+static void exynos_sort_descend_freq_table(void)
+{
+	struct cpufreq_frequency_table *freq_tbl = dvfs_info->freq_table;
+	int i = 0, index;
+	unsigned int tmp_freq;
+	/*
+	 * Exynos5440 clock controller state logic expects the cpufreq table to
+	 * be in descending order. But the OPP library constructs the table in
+	 * ascending order. So to make the table descending we just need to
+	 * swap the i element with the N - i element.
+	 */
+	for (i = 0; i < dvfs_info->freq_count / 2; i++) {
+		index = dvfs_info->freq_count - i - 1;
+		tmp_freq = freq_tbl[i].frequency;
+		freq_tbl[i].frequency = freq_tbl[index].frequency;
+		freq_tbl[index].frequency = tmp_freq;
+	}
+}
+
+static int exynos_cpufreq_cpu_init(struct cpufreq_policy *policy)
+{
+	int ret;
+
+	ret = cpufreq_frequency_table_cpuinfo(policy, dvfs_info->freq_table);
+	if (ret) {
+		dev_err(dvfs_info->dev, "Invalid frequency table: %d\n", ret);
+		return ret;
+	}
+
+	policy->cur = dvfs_info->cur_frequency;
+	policy->cpuinfo.transition_latency = dvfs_info->latency;
+	cpumask_setall(policy->cpus);
+
+	cpufreq_frequency_table_get_attr(dvfs_info->freq_table, policy->cpu);
+
+	return 0;
+}
+
+static struct cpufreq_driver exynos_driver = {
+	.flags		= CPUFREQ_STICKY,
+	.verify		= exynos_verify_speed,
+	.target		= exynos_target,
+	.get		= exynos_getspeed,
+	.init		= exynos_cpufreq_cpu_init,
+	.name		= CPUFREQ_NAME,
+};
+
+static const struct of_device_id exynos_cpufreq_match[] = {
+	{
+		.compatible = "samsung,exynos5440-cpufreq",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, exynos_cpufreq_match);
+
+static int exynos_cpufreq_probe(struct platform_device *pdev)
+{
+	int ret = -EINVAL;
+	struct device_node *np;
+	struct resource res;
+
+	np =  pdev->dev.of_node;
+	if (!np)
+		return -ENODEV;
+
+	dvfs_info = devm_kzalloc(&pdev->dev, sizeof(*dvfs_info), GFP_KERNEL);
+	if (!dvfs_info) {
+		ret = -ENOMEM;
+		goto err_put_node;
+	}
+
+	dvfs_info->dev = &pdev->dev;
+
+	ret = of_address_to_resource(np, 0, &res);
+	if (ret)
+		goto err_put_node;
+
+	dvfs_info->base = devm_ioremap_resource(dvfs_info->dev, &res);
+	if (IS_ERR(dvfs_info->base)) {
+		ret = PTR_ERR(dvfs_info->base);
+		goto err_put_node;
+	}
+
+	dvfs_info->irq = irq_of_parse_and_map(np, 0);
+	if (!dvfs_info->irq) {
+		dev_err(dvfs_info->dev, "No cpufreq irq found\n");
+		ret = -ENODEV;
+		goto err_put_node;
+	}
+
+	ret = of_init_opp_table(dvfs_info->dev);
+	if (ret) {
+		dev_err(dvfs_info->dev, "failed to init OPP table: %d\n", ret);
+		goto err_put_node;
+	}
+
+	ret = opp_init_cpufreq_table(dvfs_info->dev, &dvfs_info->freq_table);
+	if (ret) {
+		dev_err(dvfs_info->dev,
+			"failed to init cpufreq table: %d\n", ret);
+		goto err_put_node;
+	}
+	dvfs_info->freq_count = opp_get_opp_count(dvfs_info->dev);
+	exynos_sort_descend_freq_table();
+
+	if (of_property_read_u32(np, "clock-latency", &dvfs_info->latency))
+		dvfs_info->latency = DEF_TRANS_LATENCY;
+
+	dvfs_info->cpu_clk = devm_clk_get(dvfs_info->dev, "armclk");
+	if (IS_ERR(dvfs_info->cpu_clk)) {
+		dev_err(dvfs_info->dev, "Failed to get cpu clock\n");
+		ret = PTR_ERR(dvfs_info->cpu_clk);
+		goto err_free_table;
+	}
+
+	dvfs_info->cur_frequency = clk_get_rate(dvfs_info->cpu_clk);
+	if (!dvfs_info->cur_frequency) {
+		dev_err(dvfs_info->dev, "Failed to get clock rate\n");
+		ret = -EINVAL;
+		goto err_free_table;
+	}
+	dvfs_info->cur_frequency /= 1000;
+
+	INIT_WORK(&dvfs_info->irq_work, exynos_cpufreq_work);
+	ret = devm_request_irq(dvfs_info->dev, dvfs_info->irq,
+				exynos_cpufreq_irq, IRQF_TRIGGER_NONE,
+				CPUFREQ_NAME, dvfs_info);
+	if (ret) {
+		dev_err(dvfs_info->dev, "Failed to register IRQ\n");
+		goto err_free_table;
+	}
+
+	ret = init_div_table();
+	if (ret) {
+		dev_err(dvfs_info->dev, "Failed to initialise div table\n");
+		goto err_free_table;
+	}
+
+	exynos_enable_dvfs();
+	ret = cpufreq_register_driver(&exynos_driver);
+	if (ret) {
+		dev_err(dvfs_info->dev,
+			"%s: failed to register cpufreq driver\n", __func__);
+		goto err_free_table;
+	}
+
+	of_node_put(np);
+	dvfs_info->dvfs_enabled = true;
+	return 0;
+
+err_free_table:
+	opp_free_cpufreq_table(dvfs_info->dev, &dvfs_info->freq_table);
+err_put_node:
+	of_node_put(np);
+	dev_err(dvfs_info->dev, "%s: failed initialization\n", __func__);
+	return ret;
+}
+
+static int exynos_cpufreq_remove(struct platform_device *pdev)
+{
+	cpufreq_unregister_driver(&exynos_driver);
+	opp_free_cpufreq_table(dvfs_info->dev, &dvfs_info->freq_table);
+	return 0;
+}
+
+static struct platform_driver exynos_cpufreq_platdrv = {
+	.driver = {
+		.name	= "exynos5440-cpufreq",
+		.owner	= THIS_MODULE,
+		.of_match_table = exynos_cpufreq_match,
+	},
+	.probe		= exynos_cpufreq_probe,
+	.remove		= exynos_cpufreq_remove,
+};
+module_platform_driver(exynos_cpufreq_platdrv);
+
+MODULE_AUTHOR("Amit Daniel Kachhap <amit.daniel@samsung.com>");
+MODULE_DESCRIPTION("Exynos5440 cpufreq driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/cpufreq/gx-suspmod.c b/drivers/cpufreq/gx-suspmod.c
index 456bee058fe6..3dfc99b9ca86 100644
--- a/drivers/cpufreq/gx-suspmod.c
+++ b/drivers/cpufreq/gx-suspmod.c
@@ -251,14 +251,13 @@ static unsigned int gx_validate_speed(unsigned int khz, u8 *on_duration,
  * set cpu speed in khz.
  **/
 
-static void gx_set_cpuspeed(unsigned int khz)
+static void gx_set_cpuspeed(struct cpufreq_policy *policy, unsigned int khz)
 {
 	u8 suscfg, pmer1;
 	unsigned int new_khz;
 	unsigned long flags;
 	struct cpufreq_freqs freqs;
 
-	freqs.cpu = 0;
 	freqs.old = gx_get_cpuspeed(0);
 
 	new_khz = gx_validate_speed(khz, &gx_params->on_duration,
@@ -266,11 +265,9 @@ static void gx_set_cpuspeed(unsigned int khz)
 
 	freqs.new = new_khz;
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 	local_irq_save(flags);
 
-
-
 	if (new_khz != stock_freq) {
 		/* if new khz == 100% of CPU speed, it is special case */
 		switch (gx_params->cs55x0->device) {
@@ -317,7 +314,7 @@ static void gx_set_cpuspeed(unsigned int khz)
 
 	gx_params->pci_suscfg = suscfg;
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 	pr_debug("suspend modulation w/ duration of ON:%d us, OFF:%d us\n",
 		gx_params->on_duration * 32, gx_params->off_duration * 32);
@@ -397,7 +394,7 @@ static int cpufreq_gx_target(struct cpufreq_policy *policy,
 		tmp_freq = gx_validate_speed(tmp_freq, &tmp1, &tmp2);
 	}
 
-	gx_set_cpuspeed(tmp_freq);
+	gx_set_cpuspeed(policy, tmp_freq);
 
 	return 0;
 }
diff --git a/drivers/cpufreq/ia64-acpi-cpufreq.c b/drivers/cpufreq/ia64-acpi-cpufreq.c
new file mode 100644
index 000000000000..c0075dbaa633
--- /dev/null
+++ b/drivers/cpufreq/ia64-acpi-cpufreq.c
@@ -0,0 +1,438 @@
+/*
+ * This file provides the ACPI based P-state support. This
+ * module works with generic cpufreq infrastructure. Most of
+ * the code is based on i386 version
+ * (arch/i386/kernel/cpu/cpufreq/acpi-cpufreq.c)
+ *
+ * Copyright (C) 2005 Intel Corp
+ *      Venkatesh Pallipadi <venkatesh.pallipadi@intel.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/cpufreq.h>
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <asm/pal.h>
+
+#include <linux/acpi.h>
+#include <acpi/processor.h>
+
+MODULE_AUTHOR("Venkatesh Pallipadi");
+MODULE_DESCRIPTION("ACPI Processor P-States Driver");
+MODULE_LICENSE("GPL");
+
+
+struct cpufreq_acpi_io {
+	struct acpi_processor_performance	acpi_data;
+	struct cpufreq_frequency_table		*freq_table;
+	unsigned int				resume;
+};
+
+static struct cpufreq_acpi_io	*acpi_io_data[NR_CPUS];
+
+static struct cpufreq_driver acpi_cpufreq_driver;
+
+
+static int
+processor_set_pstate (
+	u32	value)
+{
+	s64 retval;
+
+	pr_debug("processor_set_pstate\n");
+
+	retval = ia64_pal_set_pstate((u64)value);
+
+	if (retval) {
+		pr_debug("Failed to set freq to 0x%x, with error 0x%lx\n",
+		        value, retval);
+		return -ENODEV;
+	}
+	return (int)retval;
+}
+
+
+static int
+processor_get_pstate (
+	u32	*value)
+{
+	u64	pstate_index = 0;
+	s64 	retval;
+
+	pr_debug("processor_get_pstate\n");
+
+	retval = ia64_pal_get_pstate(&pstate_index,
+	                             PAL_GET_PSTATE_TYPE_INSTANT);
+	*value = (u32) pstate_index;
+
+	if (retval)
+		pr_debug("Failed to get current freq with "
+			"error 0x%lx, idx 0x%x\n", retval, *value);
+
+	return (int)retval;
+}
+
+
+/* To be used only after data->acpi_data is initialized */
+static unsigned
+extract_clock (
+	struct cpufreq_acpi_io *data,
+	unsigned value,
+	unsigned int cpu)
+{
+	unsigned long i;
+
+	pr_debug("extract_clock\n");
+
+	for (i = 0; i < data->acpi_data.state_count; i++) {
+		if (value == data->acpi_data.states[i].status)
+			return data->acpi_data.states[i].core_frequency;
+	}
+	return data->acpi_data.states[i-1].core_frequency;
+}
+
+
+static unsigned int
+processor_get_freq (
+	struct cpufreq_acpi_io	*data,
+	unsigned int		cpu)
+{
+	int			ret = 0;
+	u32			value = 0;
+	cpumask_t		saved_mask;
+	unsigned long 		clock_freq;
+
+	pr_debug("processor_get_freq\n");
+
+	saved_mask = current->cpus_allowed;
+	set_cpus_allowed_ptr(current, cpumask_of(cpu));
+	if (smp_processor_id() != cpu)
+		goto migrate_end;
+
+	/* processor_get_pstate gets the instantaneous frequency */
+	ret = processor_get_pstate(&value);
+
+	if (ret) {
+		set_cpus_allowed_ptr(current, &saved_mask);
+		printk(KERN_WARNING "get performance failed with error %d\n",
+		       ret);
+		ret = 0;
+		goto migrate_end;
+	}
+	clock_freq = extract_clock(data, value, cpu);
+	ret = (clock_freq*1000);
+
+migrate_end:
+	set_cpus_allowed_ptr(current, &saved_mask);
+	return ret;
+}
+
+
+static int
+processor_set_freq (
+	struct cpufreq_acpi_io	*data,
+	struct cpufreq_policy   *policy,
+	int			state)
+{
+	int			ret = 0;
+	u32			value = 0;
+	struct cpufreq_freqs    cpufreq_freqs;
+	cpumask_t		saved_mask;
+	int			retval;
+
+	pr_debug("processor_set_freq\n");
+
+	saved_mask = current->cpus_allowed;
+	set_cpus_allowed_ptr(current, cpumask_of(policy->cpu));
+	if (smp_processor_id() != policy->cpu) {
+		retval = -EAGAIN;
+		goto migrate_end;
+	}
+
+	if (state == data->acpi_data.state) {
+		if (unlikely(data->resume)) {
+			pr_debug("Called after resume, resetting to P%d\n", state);
+			data->resume = 0;
+		} else {
+			pr_debug("Already at target state (P%d)\n", state);
+			retval = 0;
+			goto migrate_end;
+		}
+	}
+
+	pr_debug("Transitioning from P%d to P%d\n",
+		data->acpi_data.state, state);
+
+	/* cpufreq frequency struct */
+	cpufreq_freqs.old = data->freq_table[data->acpi_data.state].frequency;
+	cpufreq_freqs.new = data->freq_table[state].frequency;
+
+	/* notify cpufreq */
+	cpufreq_notify_transition(policy, &cpufreq_freqs, CPUFREQ_PRECHANGE);
+
+	/*
+	 * First we write the target state's 'control' value to the
+	 * control_register.
+	 */
+
+	value = (u32) data->acpi_data.states[state].control;
+
+	pr_debug("Transitioning to state: 0x%08x\n", value);
+
+	ret = processor_set_pstate(value);
+	if (ret) {
+		unsigned int tmp = cpufreq_freqs.new;
+		cpufreq_notify_transition(policy, &cpufreq_freqs,
+				CPUFREQ_POSTCHANGE);
+		cpufreq_freqs.new = cpufreq_freqs.old;
+		cpufreq_freqs.old = tmp;
+		cpufreq_notify_transition(policy, &cpufreq_freqs,
+				CPUFREQ_PRECHANGE);
+		cpufreq_notify_transition(policy, &cpufreq_freqs,
+				CPUFREQ_POSTCHANGE);
+		printk(KERN_WARNING "Transition failed with error %d\n", ret);
+		retval = -ENODEV;
+		goto migrate_end;
+	}
+
+	cpufreq_notify_transition(policy, &cpufreq_freqs, CPUFREQ_POSTCHANGE);
+
+	data->acpi_data.state = state;
+
+	retval = 0;
+
+migrate_end:
+	set_cpus_allowed_ptr(current, &saved_mask);
+	return (retval);
+}
+
+
+static unsigned int
+acpi_cpufreq_get (
+	unsigned int		cpu)
+{
+	struct cpufreq_acpi_io *data = acpi_io_data[cpu];
+
+	pr_debug("acpi_cpufreq_get\n");
+
+	return processor_get_freq(data, cpu);
+}
+
+
+static int
+acpi_cpufreq_target (
+	struct cpufreq_policy   *policy,
+	unsigned int target_freq,
+	unsigned int relation)
+{
+	struct cpufreq_acpi_io *data = acpi_io_data[policy->cpu];
+	unsigned int next_state = 0;
+	unsigned int result = 0;
+
+	pr_debug("acpi_cpufreq_setpolicy\n");
+
+	result = cpufreq_frequency_table_target(policy,
+			data->freq_table, target_freq, relation, &next_state);
+	if (result)
+		return (result);
+
+	result = processor_set_freq(data, policy, next_state);
+
+	return (result);
+}
+
+
+static int
+acpi_cpufreq_verify (
+	struct cpufreq_policy   *policy)
+{
+	unsigned int result = 0;
+	struct cpufreq_acpi_io *data = acpi_io_data[policy->cpu];
+
+	pr_debug("acpi_cpufreq_verify\n");
+
+	result = cpufreq_frequency_table_verify(policy,
+			data->freq_table);
+
+	return (result);
+}
+
+
+static int
+acpi_cpufreq_cpu_init (
+	struct cpufreq_policy   *policy)
+{
+	unsigned int		i;
+	unsigned int		cpu = policy->cpu;
+	struct cpufreq_acpi_io	*data;
+	unsigned int		result = 0;
+
+	pr_debug("acpi_cpufreq_cpu_init\n");
+
+	data = kzalloc(sizeof(struct cpufreq_acpi_io), GFP_KERNEL);
+	if (!data)
+		return (-ENOMEM);
+
+	acpi_io_data[cpu] = data;
+
+	result = acpi_processor_register_performance(&data->acpi_data, cpu);
+
+	if (result)
+		goto err_free;
+
+	/* capability check */
+	if (data->acpi_data.state_count <= 1) {
+		pr_debug("No P-States\n");
+		result = -ENODEV;
+		goto err_unreg;
+	}
+
+	if ((data->acpi_data.control_register.space_id !=
+					ACPI_ADR_SPACE_FIXED_HARDWARE) ||
+	    (data->acpi_data.status_register.space_id !=
+					ACPI_ADR_SPACE_FIXED_HARDWARE)) {
+		pr_debug("Unsupported address space [%d, %d]\n",
+			(u32) (data->acpi_data.control_register.space_id),
+			(u32) (data->acpi_data.status_register.space_id));
+		result = -ENODEV;
+		goto err_unreg;
+	}
+
+	/* alloc freq_table */
+	data->freq_table = kmalloc(sizeof(struct cpufreq_frequency_table) *
+	                           (data->acpi_data.state_count + 1),
+	                           GFP_KERNEL);
+	if (!data->freq_table) {
+		result = -ENOMEM;
+		goto err_unreg;
+	}
+
+	/* detect transition latency */
+	policy->cpuinfo.transition_latency = 0;
+	for (i=0; i<data->acpi_data.state_count; i++) {
+		if ((data->acpi_data.states[i].transition_latency * 1000) >
+		    policy->cpuinfo.transition_latency) {
+			policy->cpuinfo.transition_latency =
+			    data->acpi_data.states[i].transition_latency * 1000;
+		}
+	}
+	policy->cur = processor_get_freq(data, policy->cpu);
+
+	/* table init */
+	for (i = 0; i <= data->acpi_data.state_count; i++)
+	{
+		data->freq_table[i].index = i;
+		if (i < data->acpi_data.state_count) {
+			data->freq_table[i].frequency =
+			      data->acpi_data.states[i].core_frequency * 1000;
+		} else {
+			data->freq_table[i].frequency = CPUFREQ_TABLE_END;
+		}
+	}
+
+	result = cpufreq_frequency_table_cpuinfo(policy, data->freq_table);
+	if (result) {
+		goto err_freqfree;
+	}
+
+	/* notify BIOS that we exist */
+	acpi_processor_notify_smm(THIS_MODULE);
+
+	printk(KERN_INFO "acpi-cpufreq: CPU%u - ACPI performance management "
+	       "activated.\n", cpu);
+
+	for (i = 0; i < data->acpi_data.state_count; i++)
+		pr_debug("     %cP%d: %d MHz, %d mW, %d uS, %d uS, 0x%x 0x%x\n",
+			(i == data->acpi_data.state?'*':' '), i,
+			(u32) data->acpi_data.states[i].core_frequency,
+			(u32) data->acpi_data.states[i].power,
+			(u32) data->acpi_data.states[i].transition_latency,
+			(u32) data->acpi_data.states[i].bus_master_latency,
+			(u32) data->acpi_data.states[i].status,
+			(u32) data->acpi_data.states[i].control);
+
+	cpufreq_frequency_table_get_attr(data->freq_table, policy->cpu);
+
+	/* the first call to ->target() should result in us actually
+	 * writing something to the appropriate registers. */
+	data->resume = 1;
+
+	return (result);
+
+ err_freqfree:
+	kfree(data->freq_table);
+ err_unreg:
+	acpi_processor_unregister_performance(&data->acpi_data, cpu);
+ err_free:
+	kfree(data);
+	acpi_io_data[cpu] = NULL;
+
+	return (result);
+}
+
+
+static int
+acpi_cpufreq_cpu_exit (
+	struct cpufreq_policy   *policy)
+{
+	struct cpufreq_acpi_io *data = acpi_io_data[policy->cpu];
+
+	pr_debug("acpi_cpufreq_cpu_exit\n");
+
+	if (data) {
+		cpufreq_frequency_table_put_attr(policy->cpu);
+		acpi_io_data[policy->cpu] = NULL;
+		acpi_processor_unregister_performance(&data->acpi_data,
+		                                      policy->cpu);
+		kfree(data);
+	}
+
+	return (0);
+}
+
+
+static struct freq_attr* acpi_cpufreq_attr[] = {
+	&cpufreq_freq_attr_scaling_available_freqs,
+	NULL,
+};
+
+
+static struct cpufreq_driver acpi_cpufreq_driver = {
+	.verify 	= acpi_cpufreq_verify,
+	.target 	= acpi_cpufreq_target,
+	.get 		= acpi_cpufreq_get,
+	.init		= acpi_cpufreq_cpu_init,
+	.exit		= acpi_cpufreq_cpu_exit,
+	.name		= "acpi-cpufreq",
+	.owner		= THIS_MODULE,
+	.attr           = acpi_cpufreq_attr,
+};
+
+
+static int __init
+acpi_cpufreq_init (void)
+{
+	pr_debug("acpi_cpufreq_init\n");
+
+ 	return cpufreq_register_driver(&acpi_cpufreq_driver);
+}
+
+
+static void __exit
+acpi_cpufreq_exit (void)
+{
+	pr_debug("acpi_cpufreq_exit\n");
+
+	cpufreq_unregister_driver(&acpi_cpufreq_driver);
+	return;
+}
+
+
+late_initcall(acpi_cpufreq_init);
+module_exit(acpi_cpufreq_exit);
+
diff --git a/drivers/cpufreq/imx6q-cpufreq.c b/drivers/cpufreq/imx6q-cpufreq.c
index 54e336de373b..b78bc35973ba 100644
--- a/drivers/cpufreq/imx6q-cpufreq.c
+++ b/drivers/cpufreq/imx6q-cpufreq.c
@@ -50,7 +50,7 @@ static int imx6q_set_target(struct cpufreq_policy *policy,
 	struct cpufreq_freqs freqs;
 	struct opp *opp;
 	unsigned long freq_hz, volt, volt_old;
-	unsigned int index, cpu;
+	unsigned int index;
 	int ret;
 
 	ret = cpufreq_frequency_table_target(policy, freq_table, target_freq,
@@ -68,10 +68,7 @@ static int imx6q_set_target(struct cpufreq_policy *policy,
 	if (freqs.old == freqs.new)
 		return 0;
 
-	for_each_online_cpu(cpu) {
-		freqs.cpu = cpu;
-		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
-	}
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	rcu_read_lock();
 	opp = opp_find_freq_ceil(cpu_dev, &freq_hz);
@@ -166,10 +163,7 @@ static int imx6q_set_target(struct cpufreq_policy *policy,
 		}
 	}
 
-	for_each_online_cpu(cpu) {
-		freqs.cpu = cpu;
-		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
-	}
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 	return 0;
 }
diff --git a/drivers/cpufreq/integrator-cpufreq.c b/drivers/cpufreq/integrator-cpufreq.c
new file mode 100644
index 000000000000..f7c99df0880b
--- /dev/null
+++ b/drivers/cpufreq/integrator-cpufreq.c
@@ -0,0 +1,220 @@
+/*
+ *  Copyright (C) 2001-2002 Deep Blue Solutions 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.
+ *
+ * CPU support functions
+ */
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/cpufreq.h>
+#include <linux/sched.h>
+#include <linux/smp.h>
+#include <linux/init.h>
+#include <linux/io.h>
+
+#include <mach/hardware.h>
+#include <mach/platform.h>
+#include <asm/mach-types.h>
+#include <asm/hardware/icst.h>
+
+static struct cpufreq_driver integrator_driver;
+
+#define CM_ID  	__io_address(INTEGRATOR_HDR_ID)
+#define CM_OSC	__io_address(INTEGRATOR_HDR_OSC)
+#define CM_STAT __io_address(INTEGRATOR_HDR_STAT)
+#define CM_LOCK __io_address(INTEGRATOR_HDR_LOCK)
+
+static const struct icst_params lclk_params = {
+	.ref		= 24000000,
+	.vco_max	= ICST525_VCO_MAX_5V,
+	.vco_min	= ICST525_VCO_MIN,
+	.vd_min		= 8,
+	.vd_max		= 132,
+	.rd_min		= 24,
+	.rd_max		= 24,
+	.s2div		= icst525_s2div,
+	.idx2s		= icst525_idx2s,
+};
+
+static const struct icst_params cclk_params = {
+	.ref		= 24000000,
+	.vco_max	= ICST525_VCO_MAX_5V,
+	.vco_min	= ICST525_VCO_MIN,
+	.vd_min		= 12,
+	.vd_max		= 160,
+	.rd_min		= 24,
+	.rd_max		= 24,
+	.s2div		= icst525_s2div,
+	.idx2s		= icst525_idx2s,
+};
+
+/*
+ * Validate the speed policy.
+ */
+static int integrator_verify_policy(struct cpufreq_policy *policy)
+{
+	struct icst_vco vco;
+
+	cpufreq_verify_within_limits(policy, 
+				     policy->cpuinfo.min_freq, 
+				     policy->cpuinfo.max_freq);
+
+	vco = icst_hz_to_vco(&cclk_params, policy->max * 1000);
+	policy->max = icst_hz(&cclk_params, vco) / 1000;
+
+	vco = icst_hz_to_vco(&cclk_params, policy->min * 1000);
+	policy->min = icst_hz(&cclk_params, vco) / 1000;
+
+	cpufreq_verify_within_limits(policy, 
+				     policy->cpuinfo.min_freq, 
+				     policy->cpuinfo.max_freq);
+
+	return 0;
+}
+
+
+static int integrator_set_target(struct cpufreq_policy *policy,
+				 unsigned int target_freq,
+				 unsigned int relation)
+{
+	cpumask_t cpus_allowed;
+	int cpu = policy->cpu;
+	struct icst_vco vco;
+	struct cpufreq_freqs freqs;
+	u_int cm_osc;
+
+	/*
+	 * Save this threads cpus_allowed mask.
+	 */
+	cpus_allowed = current->cpus_allowed;
+
+	/*
+	 * Bind to the specified CPU.  When this call returns,
+	 * we should be running on the right CPU.
+	 */
+	set_cpus_allowed(current, cpumask_of_cpu(cpu));
+	BUG_ON(cpu != smp_processor_id());
+
+	/* get current setting */
+	cm_osc = __raw_readl(CM_OSC);
+
+	if (machine_is_integrator()) {
+		vco.s = (cm_osc >> 8) & 7;
+	} else if (machine_is_cintegrator()) {
+		vco.s = 1;
+	}
+	vco.v = cm_osc & 255;
+	vco.r = 22;
+	freqs.old = icst_hz(&cclk_params, vco) / 1000;
+
+	/* icst_hz_to_vco rounds down -- so we need the next
+	 * larger freq in case of CPUFREQ_RELATION_L.
+	 */
+	if (relation == CPUFREQ_RELATION_L)
+		target_freq += 999;
+	if (target_freq > policy->max)
+		target_freq = policy->max;
+	vco = icst_hz_to_vco(&cclk_params, target_freq * 1000);
+	freqs.new = icst_hz(&cclk_params, vco) / 1000;
+
+	if (freqs.old == freqs.new) {
+		set_cpus_allowed(current, cpus_allowed);
+		return 0;
+	}
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+	cm_osc = __raw_readl(CM_OSC);
+
+	if (machine_is_integrator()) {
+		cm_osc &= 0xfffff800;
+		cm_osc |= vco.s << 8;
+	} else if (machine_is_cintegrator()) {
+		cm_osc &= 0xffffff00;
+	}
+	cm_osc |= vco.v;
+
+	__raw_writel(0xa05f, CM_LOCK);
+	__raw_writel(cm_osc, CM_OSC);
+	__raw_writel(0, CM_LOCK);
+
+	/*
+	 * Restore the CPUs allowed mask.
+	 */
+	set_cpus_allowed(current, cpus_allowed);
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	return 0;
+}
+
+static unsigned int integrator_get(unsigned int cpu)
+{
+	cpumask_t cpus_allowed;
+	unsigned int current_freq;
+	u_int cm_osc;
+	struct icst_vco vco;
+
+	cpus_allowed = current->cpus_allowed;
+
+	set_cpus_allowed(current, cpumask_of_cpu(cpu));
+	BUG_ON(cpu != smp_processor_id());
+
+	/* detect memory etc. */
+	cm_osc = __raw_readl(CM_OSC);
+
+	if (machine_is_integrator()) {
+		vco.s = (cm_osc >> 8) & 7;
+	} else {
+		vco.s = 1;
+	}
+	vco.v = cm_osc & 255;
+	vco.r = 22;
+
+	current_freq = icst_hz(&cclk_params, vco) / 1000; /* current freq */
+
+	set_cpus_allowed(current, cpus_allowed);
+
+	return current_freq;
+}
+
+static int integrator_cpufreq_init(struct cpufreq_policy *policy)
+{
+
+	/* set default policy and cpuinfo */
+	policy->cpuinfo.max_freq = 160000;
+	policy->cpuinfo.min_freq = 12000;
+	policy->cpuinfo.transition_latency = 1000000; /* 1 ms, assumed */
+	policy->cur = policy->min = policy->max = integrator_get(policy->cpu);
+
+	return 0;
+}
+
+static struct cpufreq_driver integrator_driver = {
+	.verify		= integrator_verify_policy,
+	.target		= integrator_set_target,
+	.get		= integrator_get,
+	.init		= integrator_cpufreq_init,
+	.name		= "integrator",
+};
+
+static int __init integrator_cpu_init(void)
+{
+	return cpufreq_register_driver(&integrator_driver);
+}
+
+static void __exit integrator_cpu_exit(void)
+{
+	cpufreq_unregister_driver(&integrator_driver);
+}
+
+MODULE_AUTHOR ("Russell M. King");
+MODULE_DESCRIPTION ("cpufreq driver for ARM Integrator CPUs");
+MODULE_LICENSE ("GPL");
+
+module_init(integrator_cpu_init);
+module_exit(integrator_cpu_exit);
diff --git a/drivers/cpufreq/intel_pstate.c b/drivers/cpufreq/intel_pstate.c
index 6133ef5cf671..cc3a8e6c92be 100644
--- a/drivers/cpufreq/intel_pstate.c
+++ b/drivers/cpufreq/intel_pstate.c
@@ -1,5 +1,5 @@
 /*
- * cpufreq_snb.c: Native P state management for Intel processors
+ * intel_pstate.c: Native P state management for Intel processors
  *
  * (C) Copyright 2012 Intel Corporation
  * Author: Dirk Brandewie <dirk.j.brandewie@intel.com>
@@ -657,30 +657,27 @@ static unsigned int intel_pstate_get(unsigned int cpu_num)
 static int intel_pstate_set_policy(struct cpufreq_policy *policy)
 {
 	struct cpudata *cpu;
-	int min, max;
 
 	cpu = all_cpu_data[policy->cpu];
 
 	if (!policy->cpuinfo.max_freq)
 		return -ENODEV;
 
-	intel_pstate_get_min_max(cpu, &min, &max);
-
-	limits.min_perf_pct = (policy->min * 100) / policy->cpuinfo.max_freq;
-	limits.min_perf_pct = clamp_t(int, limits.min_perf_pct, 0 , 100);
-	limits.min_perf = div_fp(int_tofp(limits.min_perf_pct), int_tofp(100));
-
-	limits.max_perf_pct = policy->max * 100 / policy->cpuinfo.max_freq;
-	limits.max_perf_pct = clamp_t(int, limits.max_perf_pct, 0 , 100);
-	limits.max_perf = div_fp(int_tofp(limits.max_perf_pct), int_tofp(100));
-
 	if (policy->policy == CPUFREQ_POLICY_PERFORMANCE) {
 		limits.min_perf_pct = 100;
 		limits.min_perf = int_tofp(1);
 		limits.max_perf_pct = 100;
 		limits.max_perf = int_tofp(1);
 		limits.no_turbo = 0;
+		return 0;
 	}
+	limits.min_perf_pct = (policy->min * 100) / policy->cpuinfo.max_freq;
+	limits.min_perf_pct = clamp_t(int, limits.min_perf_pct, 0 , 100);
+	limits.min_perf = div_fp(int_tofp(limits.min_perf_pct), int_tofp(100));
+
+	limits.max_perf_pct = policy->max * 100 / policy->cpuinfo.max_freq;
+	limits.max_perf_pct = clamp_t(int, limits.max_perf_pct, 0 , 100);
+	limits.max_perf = div_fp(int_tofp(limits.max_perf_pct), int_tofp(100));
 
 	return 0;
 }
diff --git a/drivers/cpufreq/kirkwood-cpufreq.c b/drivers/cpufreq/kirkwood-cpufreq.c
index 0e83e3c24f5b..d36ea8dc96eb 100644
--- a/drivers/cpufreq/kirkwood-cpufreq.c
+++ b/drivers/cpufreq/kirkwood-cpufreq.c
@@ -55,7 +55,8 @@ static unsigned int kirkwood_cpufreq_get_cpu_frequency(unsigned int cpu)
 	return kirkwood_freq_table[0].frequency;
 }
 
-static void kirkwood_cpufreq_set_cpu_state(unsigned int index)
+static void kirkwood_cpufreq_set_cpu_state(struct cpufreq_policy *policy,
+		unsigned int index)
 {
 	struct cpufreq_freqs freqs;
 	unsigned int state = kirkwood_freq_table[index].index;
@@ -63,9 +64,8 @@ static void kirkwood_cpufreq_set_cpu_state(unsigned int index)
 
 	freqs.old = kirkwood_cpufreq_get_cpu_frequency(0);
 	freqs.new = kirkwood_freq_table[index].frequency;
-	freqs.cpu = 0; /* Kirkwood is UP */
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	dev_dbg(priv.dev, "Attempting to set frequency to %i KHz\n",
 		kirkwood_freq_table[index].frequency);
@@ -99,7 +99,7 @@ static void kirkwood_cpufreq_set_cpu_state(unsigned int index)
 
 		local_irq_enable();
 	}
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 };
 
 static int kirkwood_cpufreq_verify(struct cpufreq_policy *policy)
@@ -117,7 +117,7 @@ static int kirkwood_cpufreq_target(struct cpufreq_policy *policy,
 				target_freq, relation, &index))
 		return -EINVAL;
 
-	kirkwood_cpufreq_set_cpu_state(index);
+	kirkwood_cpufreq_set_cpu_state(policy, index);
 
 	return 0;
 }
@@ -175,11 +175,9 @@ static int kirkwood_cpufreq_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "Cannot get memory resource\n");
 		return -ENODEV;
 	}
-	priv.base = devm_request_and_ioremap(&pdev->dev, res);
-	if (!priv.base) {
-		dev_err(&pdev->dev, "Cannot ioremap\n");
-		return -EADDRNOTAVAIL;
-	}
+	priv.base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv.base))
+		return PTR_ERR(priv.base);
 
 	np = of_find_node_by_path("/cpus/cpu@0");
 	if (!np)
diff --git a/drivers/cpufreq/longhaul.c b/drivers/cpufreq/longhaul.c
index 1180d536d1eb..b448638e34de 100644
--- a/drivers/cpufreq/longhaul.c
+++ b/drivers/cpufreq/longhaul.c
@@ -242,7 +242,8 @@ static void do_powersaver(int cx_address, unsigned int mults_index,
  * Sets a new clock ratio.
  */
 
-static void longhaul_setstate(unsigned int table_index)
+static void longhaul_setstate(struct cpufreq_policy *policy,
+		unsigned int table_index)
 {
 	unsigned int mults_index;
 	int speed, mult;
@@ -267,9 +268,8 @@ static void longhaul_setstate(unsigned int table_index)
 
 	freqs.old = calc_speed(longhaul_get_cpu_mult());
 	freqs.new = speed;
-	freqs.cpu = 0; /* longhaul.c is UP only driver */
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	pr_debug("Setting to FSB:%dMHz Mult:%d.%dx (%s)\n",
 			fsb, mult/10, mult%10, print_speed(speed/1000));
@@ -386,7 +386,7 @@ retry_loop:
 		}
 	}
 	/* Report true CPU frequency */
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 	if (!bm_timeout)
 		printk(KERN_INFO PFX "Warning: Timeout while waiting for "
@@ -648,7 +648,7 @@ static int longhaul_target(struct cpufreq_policy *policy,
 		return 0;
 
 	if (!can_scale_voltage)
-		longhaul_setstate(table_index);
+		longhaul_setstate(policy, table_index);
 	else {
 		/* On test system voltage transitions exceeding single
 		 * step up or down were turning motherboard off. Both
@@ -663,7 +663,7 @@ static int longhaul_target(struct cpufreq_policy *policy,
 		while (i != table_index) {
 			vid = (longhaul_table[i].index >> 8) & 0x1f;
 			if (vid != current_vid) {
-				longhaul_setstate(i);
+				longhaul_setstate(policy, i);
 				current_vid = vid;
 				msleep(200);
 			}
@@ -672,7 +672,7 @@ static int longhaul_target(struct cpufreq_policy *policy,
 			else
 				i--;
 		}
-		longhaul_setstate(table_index);
+		longhaul_setstate(policy, table_index);
 	}
 	longhaul_index = table_index;
 	return 0;
@@ -998,15 +998,17 @@ static int __init longhaul_init(void)
 
 static void __exit longhaul_exit(void)
 {
+	struct cpufreq_policy *policy = cpufreq_cpu_get(0);
 	int i;
 
 	for (i = 0; i < numscales; i++) {
 		if (mults[i] == maxmult) {
-			longhaul_setstate(i);
+			longhaul_setstate(policy, i);
 			break;
 		}
 	}
 
+	cpufreq_cpu_put(policy);
 	cpufreq_unregister_driver(&longhaul_driver);
 	kfree(longhaul_table);
 }
diff --git a/drivers/cpufreq/loongson2_cpufreq.c b/drivers/cpufreq/loongson2_cpufreq.c
new file mode 100644
index 000000000000..84889573b566
--- /dev/null
+++ b/drivers/cpufreq/loongson2_cpufreq.c
@@ -0,0 +1,248 @@
+/*
+ * Cpufreq driver for the loongson-2 processors
+ *
+ * The 2E revision of loongson processor not support this feature.
+ *
+ * Copyright (C) 2006 - 2008 Lemote Inc. & Insititute of Computing Technology
+ * Author: Yanhua, yanh@lemote.com
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+#include <linux/cpufreq.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/sched.h>	/* set_cpus_allowed() */
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+
+#include <asm/clock.h>
+
+#include <asm/mach-loongson/loongson.h>
+
+static uint nowait;
+
+static struct clk *cpuclk;
+
+static void (*saved_cpu_wait) (void);
+
+static int loongson2_cpu_freq_notifier(struct notifier_block *nb,
+					unsigned long val, void *data);
+
+static struct notifier_block loongson2_cpufreq_notifier_block = {
+	.notifier_call = loongson2_cpu_freq_notifier
+};
+
+static int loongson2_cpu_freq_notifier(struct notifier_block *nb,
+					unsigned long val, void *data)
+{
+	if (val == CPUFREQ_POSTCHANGE)
+		current_cpu_data.udelay_val = loops_per_jiffy;
+
+	return 0;
+}
+
+static unsigned int loongson2_cpufreq_get(unsigned int cpu)
+{
+	return clk_get_rate(cpuclk);
+}
+
+/*
+ * Here we notify other drivers of the proposed change and the final change.
+ */
+static int loongson2_cpufreq_target(struct cpufreq_policy *policy,
+				     unsigned int target_freq,
+				     unsigned int relation)
+{
+	unsigned int cpu = policy->cpu;
+	unsigned int newstate = 0;
+	cpumask_t cpus_allowed;
+	struct cpufreq_freqs freqs;
+	unsigned int freq;
+
+	cpus_allowed = current->cpus_allowed;
+	set_cpus_allowed_ptr(current, cpumask_of(cpu));
+
+	if (cpufreq_frequency_table_target
+	    (policy, &loongson2_clockmod_table[0], target_freq, relation,
+	     &newstate))
+		return -EINVAL;
+
+	freq =
+	    ((cpu_clock_freq / 1000) *
+	     loongson2_clockmod_table[newstate].index) / 8;
+	if (freq < policy->min || freq > policy->max)
+		return -EINVAL;
+
+	pr_debug("cpufreq: requested frequency %u Hz\n", target_freq * 1000);
+
+	freqs.old = loongson2_cpufreq_get(cpu);
+	freqs.new = freq;
+	freqs.flags = 0;
+
+	if (freqs.new == freqs.old)
+		return 0;
+
+	/* notifiers */
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+	set_cpus_allowed_ptr(current, &cpus_allowed);
+
+	/* setting the cpu frequency */
+	clk_set_rate(cpuclk, freq);
+
+	/* notifiers */
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	pr_debug("cpufreq: set frequency %u kHz\n", freq);
+
+	return 0;
+}
+
+static int loongson2_cpufreq_cpu_init(struct cpufreq_policy *policy)
+{
+	int i;
+	unsigned long rate;
+	int ret;
+
+	cpuclk = clk_get(NULL, "cpu_clk");
+	if (IS_ERR(cpuclk)) {
+		printk(KERN_ERR "cpufreq: couldn't get CPU clk\n");
+		return PTR_ERR(cpuclk);
+	}
+
+	rate = cpu_clock_freq / 1000;
+	if (!rate) {
+		clk_put(cpuclk);
+		return -EINVAL;
+	}
+	ret = clk_set_rate(cpuclk, rate);
+	if (ret) {
+		clk_put(cpuclk);
+		return ret;
+	}
+
+	/* clock table init */
+	for (i = 2;
+	     (loongson2_clockmod_table[i].frequency != CPUFREQ_TABLE_END);
+	     i++)
+		loongson2_clockmod_table[i].frequency = (rate * i) / 8;
+
+	policy->cur = loongson2_cpufreq_get(policy->cpu);
+
+	cpufreq_frequency_table_get_attr(&loongson2_clockmod_table[0],
+					 policy->cpu);
+
+	return cpufreq_frequency_table_cpuinfo(policy,
+					    &loongson2_clockmod_table[0]);
+}
+
+static int loongson2_cpufreq_verify(struct cpufreq_policy *policy)
+{
+	return cpufreq_frequency_table_verify(policy,
+					      &loongson2_clockmod_table[0]);
+}
+
+static int loongson2_cpufreq_exit(struct cpufreq_policy *policy)
+{
+	clk_put(cpuclk);
+	return 0;
+}
+
+static struct freq_attr *loongson2_table_attr[] = {
+	&cpufreq_freq_attr_scaling_available_freqs,
+	NULL,
+};
+
+static struct cpufreq_driver loongson2_cpufreq_driver = {
+	.owner = THIS_MODULE,
+	.name = "loongson2",
+	.init = loongson2_cpufreq_cpu_init,
+	.verify = loongson2_cpufreq_verify,
+	.target = loongson2_cpufreq_target,
+	.get = loongson2_cpufreq_get,
+	.exit = loongson2_cpufreq_exit,
+	.attr = loongson2_table_attr,
+};
+
+static struct platform_device_id platform_device_ids[] = {
+	{
+		.name = "loongson2_cpufreq",
+	},
+	{}
+};
+
+MODULE_DEVICE_TABLE(platform, platform_device_ids);
+
+static struct platform_driver platform_driver = {
+	.driver = {
+		.name = "loongson2_cpufreq",
+		.owner = THIS_MODULE,
+	},
+	.id_table = platform_device_ids,
+};
+
+/*
+ * This is the simple version of Loongson-2 wait, Maybe we need do this in
+ * interrupt disabled context.
+ */
+
+static DEFINE_SPINLOCK(loongson2_wait_lock);
+
+static void loongson2_cpu_wait(void)
+{
+	unsigned long flags;
+	u32 cpu_freq;
+
+	spin_lock_irqsave(&loongson2_wait_lock, flags);
+	cpu_freq = LOONGSON_CHIPCFG0;
+	LOONGSON_CHIPCFG0 &= ~0x7;	/* Put CPU into wait mode */
+	LOONGSON_CHIPCFG0 = cpu_freq;	/* Restore CPU state */
+	spin_unlock_irqrestore(&loongson2_wait_lock, flags);
+}
+
+static int __init cpufreq_init(void)
+{
+	int ret;
+
+	/* Register platform stuff */
+	ret = platform_driver_register(&platform_driver);
+	if (ret)
+		return ret;
+
+	pr_info("cpufreq: Loongson-2F CPU frequency driver.\n");
+
+	cpufreq_register_notifier(&loongson2_cpufreq_notifier_block,
+				  CPUFREQ_TRANSITION_NOTIFIER);
+
+	ret = cpufreq_register_driver(&loongson2_cpufreq_driver);
+
+	if (!ret && !nowait) {
+		saved_cpu_wait = cpu_wait;
+		cpu_wait = loongson2_cpu_wait;
+	}
+
+	return ret;
+}
+
+static void __exit cpufreq_exit(void)
+{
+	if (!nowait && saved_cpu_wait)
+		cpu_wait = saved_cpu_wait;
+	cpufreq_unregister_driver(&loongson2_cpufreq_driver);
+	cpufreq_unregister_notifier(&loongson2_cpufreq_notifier_block,
+				    CPUFREQ_TRANSITION_NOTIFIER);
+
+	platform_driver_unregister(&platform_driver);
+}
+
+module_init(cpufreq_init);
+module_exit(cpufreq_exit);
+
+module_param(nowait, uint, 0644);
+MODULE_PARM_DESC(nowait, "Disable Loongson-2F specific wait");
+
+MODULE_AUTHOR("Yanhua <yanh@lemote.com>");
+MODULE_DESCRIPTION("cpufreq driver for Loongson2F");
+MODULE_LICENSE("GPL");
diff --git a/drivers/cpufreq/maple-cpufreq.c b/drivers/cpufreq/maple-cpufreq.c
index d4c4989823dc..cdd62915efaf 100644
--- a/drivers/cpufreq/maple-cpufreq.c
+++ b/drivers/cpufreq/maple-cpufreq.c
@@ -158,11 +158,10 @@ static int maple_cpufreq_target(struct cpufreq_policy *policy,
 
 	freqs.old = maple_cpu_freqs[maple_pmode_cur].frequency;
 	freqs.new = maple_cpu_freqs[newstate].frequency;
-	freqs.cpu = 0;
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 	rc = maple_scom_switch_freq(newstate);
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 	mutex_unlock(&maple_switch_mutex);
 
diff --git a/drivers/cpufreq/omap-cpufreq.c b/drivers/cpufreq/omap-cpufreq.c
index 9128c07bafba..0279d18a57f9 100644
--- a/drivers/cpufreq/omap-cpufreq.c
+++ b/drivers/cpufreq/omap-cpufreq.c
@@ -25,6 +25,7 @@
 #include <linux/opp.h>
 #include <linux/cpu.h>
 #include <linux/module.h>
+#include <linux/platform_device.h>
 #include <linux/regulator/consumer.h>
 
 #include <asm/smp_plat.h>
@@ -88,16 +89,12 @@ static int omap_target(struct cpufreq_policy *policy,
 	}
 
 	freqs.old = omap_getspeed(policy->cpu);
-	freqs.cpu = policy->cpu;
 
 	if (freqs.old == freqs.new && policy->cur == freqs.new)
 		return ret;
 
 	/* notifiers */
-	for_each_cpu(i, policy->cpus) {
-		freqs.cpu = i;
-		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
-	}
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	freq = freqs.new * 1000;
 	ret = clk_round_rate(mpu_clk, freq);
@@ -157,10 +154,7 @@ static int omap_target(struct cpufreq_policy *policy,
 
 done:
 	/* notifiers */
-	for_each_cpu(i, policy->cpus) {
-		freqs.cpu = i;
-		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
-	}
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 	return ret;
 }
@@ -184,7 +178,7 @@ static int __cpuinit omap_cpu_init(struct cpufreq_policy *policy)
 		goto fail_ck;
 	}
 
-	policy->cur = policy->min = policy->max = omap_getspeed(policy->cpu);
+	policy->cur = omap_getspeed(policy->cpu);
 
 	if (!freq_table)
 		result = opp_init_cpufreq_table(mpu_dev, &freq_table);
@@ -203,8 +197,6 @@ static int __cpuinit omap_cpu_init(struct cpufreq_policy *policy)
 
 	cpufreq_frequency_table_get_attr(freq_table, policy->cpu);
 
-	policy->min = policy->cpuinfo.min_freq;
-	policy->max = policy->cpuinfo.max_freq;
 	policy->cur = omap_getspeed(policy->cpu);
 
 	/*
@@ -252,7 +244,7 @@ static struct cpufreq_driver omap_driver = {
 	.attr		= omap_cpufreq_attr,
 };
 
-static int __init omap_cpufreq_init(void)
+static int omap_cpufreq_probe(struct platform_device *pdev)
 {
 	mpu_dev = get_cpu_device(0);
 	if (!mpu_dev) {
@@ -280,12 +272,20 @@ static int __init omap_cpufreq_init(void)
 	return cpufreq_register_driver(&omap_driver);
 }
 
-static void __exit omap_cpufreq_exit(void)
+static int omap_cpufreq_remove(struct platform_device *pdev)
 {
-	cpufreq_unregister_driver(&omap_driver);
+	return cpufreq_unregister_driver(&omap_driver);
 }
 
+static struct platform_driver omap_cpufreq_platdrv = {
+	.driver = {
+		.name	= "omap-cpufreq",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= omap_cpufreq_probe,
+	.remove		= omap_cpufreq_remove,
+};
+module_platform_driver(omap_cpufreq_platdrv);
+
 MODULE_DESCRIPTION("cpufreq driver for OMAP SoCs");
 MODULE_LICENSE("GPL");
-module_init(omap_cpufreq_init);
-module_exit(omap_cpufreq_exit);
diff --git a/drivers/cpufreq/p4-clockmod.c b/drivers/cpufreq/p4-clockmod.c
index 827629c9aad7..421ef37d0bb3 100644
--- a/drivers/cpufreq/p4-clockmod.c
+++ b/drivers/cpufreq/p4-clockmod.c
@@ -58,8 +58,7 @@ static int cpufreq_p4_setdc(unsigned int cpu, unsigned int newstate)
 {
 	u32 l, h;
 
-	if (!cpu_online(cpu) ||
-	    (newstate > DC_DISABLE) || (newstate == DC_RESV))
+	if ((newstate > DC_DISABLE) || (newstate == DC_RESV))
 		return -EINVAL;
 
 	rdmsr_on_cpu(cpu, MSR_IA32_THERM_STATUS, &l, &h);
@@ -125,10 +124,7 @@ static int cpufreq_p4_target(struct cpufreq_policy *policy,
 		return 0;
 
 	/* notifiers */
-	for_each_cpu(i, policy->cpus) {
-		freqs.cpu = i;
-		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
-	}
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	/* run on each logical CPU,
 	 * see section 13.15.3 of IA32 Intel Architecture Software
@@ -138,10 +134,7 @@ static int cpufreq_p4_target(struct cpufreq_policy *policy,
 		cpufreq_p4_setdc(i, p4clockmod_table[newstate].index);
 
 	/* notifiers */
-	for_each_cpu(i, policy->cpus) {
-		freqs.cpu = i;
-		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
-	}
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 	return 0;
 }
diff --git a/drivers/cpufreq/pcc-cpufreq.c b/drivers/cpufreq/pcc-cpufreq.c
index 503996a94a6a..0de00081a81e 100644
--- a/drivers/cpufreq/pcc-cpufreq.c
+++ b/drivers/cpufreq/pcc-cpufreq.c
@@ -215,8 +215,7 @@ static int pcc_cpufreq_target(struct cpufreq_policy *policy,
 		(pcch_virt_addr + pcc_cpu_data->input_offset));
 
 	freqs.new = target_freq;
-	freqs.cpu = cpu;
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	input_buffer = 0x1 | (((target_freq * 100)
 			       / (ioread32(&pcch_hdr->nominal) * 1000)) << 8);
@@ -237,7 +236,7 @@ static int pcc_cpufreq_target(struct cpufreq_policy *policy,
 	}
 	iowrite16(0, &pcch_hdr->status);
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 	pr_debug("target: was SUCCESSFUL for cpu %d\n", cpu);
 	spin_unlock(&pcc_lock);
 
diff --git a/drivers/cpufreq/powernow-k6.c b/drivers/cpufreq/powernow-k6.c
index af23e0b9ec92..ea0222a45b7b 100644
--- a/drivers/cpufreq/powernow-k6.c
+++ b/drivers/cpufreq/powernow-k6.c
@@ -68,7 +68,8 @@ static int powernow_k6_get_cpu_multiplier(void)
  *
  *   Tries to change the PowerNow! multiplier
  */
-static void powernow_k6_set_state(unsigned int best_i)
+static void powernow_k6_set_state(struct cpufreq_policy *policy,
+		unsigned int best_i)
 {
 	unsigned long outvalue = 0, invalue = 0;
 	unsigned long msrval;
@@ -81,9 +82,8 @@ static void powernow_k6_set_state(unsigned int best_i)
 
 	freqs.old = busfreq * powernow_k6_get_cpu_multiplier();
 	freqs.new = busfreq * clock_ratio[best_i].index;
-	freqs.cpu = 0; /* powernow-k6.c is UP only driver */
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	/* we now need to transform best_i to the BVC format, see AMD#23446 */
 
@@ -98,7 +98,7 @@ static void powernow_k6_set_state(unsigned int best_i)
 	msrval = POWERNOW_IOPORT + 0x0;
 	wrmsr(MSR_K6_EPMR, msrval, 0); /* disable it again */
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 	return;
 }
@@ -136,7 +136,7 @@ static int powernow_k6_target(struct cpufreq_policy *policy,
 				target_freq, relation, &newstate))
 		return -EINVAL;
 
-	powernow_k6_set_state(newstate);
+	powernow_k6_set_state(policy, newstate);
 
 	return 0;
 }
@@ -182,7 +182,7 @@ static int powernow_k6_cpu_exit(struct cpufreq_policy *policy)
 	unsigned int i;
 	for (i = 0; i < 8; i++) {
 		if (i == max_multiplier)
-			powernow_k6_set_state(i);
+			powernow_k6_set_state(policy, i);
 	}
 	cpufreq_frequency_table_put_attr(policy->cpu);
 	return 0;
diff --git a/drivers/cpufreq/powernow-k7.c b/drivers/cpufreq/powernow-k7.c
index 334cc2f1e9f1..53888dacbe58 100644
--- a/drivers/cpufreq/powernow-k7.c
+++ b/drivers/cpufreq/powernow-k7.c
@@ -248,7 +248,7 @@ static void change_VID(int vid)
 }
 
 
-static void change_speed(unsigned int index)
+static void change_speed(struct cpufreq_policy *policy, unsigned int index)
 {
 	u8 fid, vid;
 	struct cpufreq_freqs freqs;
@@ -263,15 +263,13 @@ static void change_speed(unsigned int index)
 	fid = powernow_table[index].index & 0xFF;
 	vid = (powernow_table[index].index & 0xFF00) >> 8;
 
-	freqs.cpu = 0;
-
 	rdmsrl(MSR_K7_FID_VID_STATUS, fidvidstatus.val);
 	cfid = fidvidstatus.bits.CFID;
 	freqs.old = fsb * fid_codes[cfid] / 10;
 
 	freqs.new = powernow_table[index].frequency;
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	/* Now do the magic poking into the MSRs.  */
 
@@ -292,7 +290,7 @@ static void change_speed(unsigned int index)
 	if (have_a0 == 1)
 		local_irq_enable();
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 }
 
 
@@ -546,7 +544,7 @@ static int powernow_target(struct cpufreq_policy *policy,
 				relation, &newstate))
 		return -EINVAL;
 
-	change_speed(newstate);
+	change_speed(policy, newstate);
 
 	return 0;
 }
diff --git a/drivers/cpufreq/powernow-k8.c b/drivers/cpufreq/powernow-k8.c
index d13a13678b5f..b828efe4b2f8 100644
--- a/drivers/cpufreq/powernow-k8.c
+++ b/drivers/cpufreq/powernow-k8.c
@@ -928,9 +928,10 @@ static int get_transition_latency(struct powernow_k8_data *data)
 static int transition_frequency_fidvid(struct powernow_k8_data *data,
 		unsigned int index)
 {
+	struct cpufreq_policy *policy;
 	u32 fid = 0;
 	u32 vid = 0;
-	int res, i;
+	int res;
 	struct cpufreq_freqs freqs;
 
 	pr_debug("cpu %d transition to index %u\n", smp_processor_id(), index);
@@ -959,10 +960,10 @@ static int transition_frequency_fidvid(struct powernow_k8_data *data,
 	freqs.old = find_khz_freq_from_fid(data->currfid);
 	freqs.new = find_khz_freq_from_fid(fid);
 
-	for_each_cpu(i, data->available_cores) {
-		freqs.cpu = i;
-		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
-	}
+	policy = cpufreq_cpu_get(smp_processor_id());
+	cpufreq_cpu_put(policy);
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	res = transition_fid_vid(data, fid, vid);
 	if (res)
@@ -970,10 +971,7 @@ static int transition_frequency_fidvid(struct powernow_k8_data *data,
 
 	freqs.new = find_khz_freq_from_fid(data->currfid);
 
-	for_each_cpu(i, data->available_cores) {
-		freqs.cpu = i;
-		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
-	}
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 	return res;
 }
 
@@ -1104,9 +1102,6 @@ static int __cpuinit powernowk8_cpu_init(struct cpufreq_policy *pol)
 	struct init_on_cpu init_on_cpu;
 	int rc;
 
-	if (!cpu_online(pol->cpu))
-		return -ENODEV;
-
 	smp_call_function_single(pol->cpu, check_supported_cpu, &rc, 1);
 	if (rc)
 		return -ENODEV;
diff --git a/drivers/cpufreq/ppc_cbe_cpufreq.c b/drivers/cpufreq/ppc_cbe_cpufreq.c
new file mode 100644
index 000000000000..e577a1dbbfcd
--- /dev/null
+++ b/drivers/cpufreq/ppc_cbe_cpufreq.c
@@ -0,0 +1,209 @@
+/*
+ * cpufreq driver for the cell processor
+ *
+ * (C) Copyright IBM Deutschland Entwicklung GmbH 2005-2007
+ *
+ * Author: Christian Krafft <krafft@de.ibm.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, 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/cpufreq.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+
+#include <asm/machdep.h>
+#include <asm/prom.h>
+#include <asm/cell-regs.h>
+
+#include "ppc_cbe_cpufreq.h"
+
+static DEFINE_MUTEX(cbe_switch_mutex);
+
+
+/* the CBE supports an 8 step frequency scaling */
+static struct cpufreq_frequency_table cbe_freqs[] = {
+	{1,	0},
+	{2,	0},
+	{3,	0},
+	{4,	0},
+	{5,	0},
+	{6,	0},
+	{8,	0},
+	{10,	0},
+	{0,	CPUFREQ_TABLE_END},
+};
+
+/*
+ * hardware specific functions
+ */
+
+static int set_pmode(unsigned int cpu, unsigned int slow_mode)
+{
+	int rc;
+
+	if (cbe_cpufreq_has_pmi)
+		rc = cbe_cpufreq_set_pmode_pmi(cpu, slow_mode);
+	else
+		rc = cbe_cpufreq_set_pmode(cpu, slow_mode);
+
+	pr_debug("register contains slow mode %d\n", cbe_cpufreq_get_pmode(cpu));
+
+	return rc;
+}
+
+/*
+ * cpufreq functions
+ */
+
+static int cbe_cpufreq_cpu_init(struct cpufreq_policy *policy)
+{
+	const u32 *max_freqp;
+	u32 max_freq;
+	int i, cur_pmode;
+	struct device_node *cpu;
+
+	cpu = of_get_cpu_node(policy->cpu, NULL);
+
+	if (!cpu)
+		return -ENODEV;
+
+	pr_debug("init cpufreq on CPU %d\n", policy->cpu);
+
+	/*
+	 * Let's check we can actually get to the CELL regs
+	 */
+	if (!cbe_get_cpu_pmd_regs(policy->cpu) ||
+	    !cbe_get_cpu_mic_tm_regs(policy->cpu)) {
+		pr_info("invalid CBE regs pointers for cpufreq\n");
+		return -EINVAL;
+	}
+
+	max_freqp = of_get_property(cpu, "clock-frequency", NULL);
+
+	of_node_put(cpu);
+
+	if (!max_freqp)
+		return -EINVAL;
+
+	/* we need the freq in kHz */
+	max_freq = *max_freqp / 1000;
+
+	pr_debug("max clock-frequency is at %u kHz\n", max_freq);
+	pr_debug("initializing frequency table\n");
+
+	/* initialize frequency table */
+	for (i=0; cbe_freqs[i].frequency!=CPUFREQ_TABLE_END; i++) {
+		cbe_freqs[i].frequency = max_freq / cbe_freqs[i].index;
+		pr_debug("%d: %d\n", i, cbe_freqs[i].frequency);
+	}
+
+	/* if DEBUG is enabled set_pmode() measures the latency
+	 * of a transition */
+	policy->cpuinfo.transition_latency = 25000;
+
+	cur_pmode = cbe_cpufreq_get_pmode(policy->cpu);
+	pr_debug("current pmode is at %d\n",cur_pmode);
+
+	policy->cur = cbe_freqs[cur_pmode].frequency;
+
+#ifdef CONFIG_SMP
+	cpumask_copy(policy->cpus, cpu_sibling_mask(policy->cpu));
+#endif
+
+	cpufreq_frequency_table_get_attr(cbe_freqs, policy->cpu);
+
+	/* this ensures that policy->cpuinfo_min
+	 * and policy->cpuinfo_max are set correctly */
+	return cpufreq_frequency_table_cpuinfo(policy, cbe_freqs);
+}
+
+static int cbe_cpufreq_cpu_exit(struct cpufreq_policy *policy)
+{
+	cpufreq_frequency_table_put_attr(policy->cpu);
+	return 0;
+}
+
+static int cbe_cpufreq_verify(struct cpufreq_policy *policy)
+{
+	return cpufreq_frequency_table_verify(policy, cbe_freqs);
+}
+
+static int cbe_cpufreq_target(struct cpufreq_policy *policy,
+			      unsigned int target_freq,
+			      unsigned int relation)
+{
+	int rc;
+	struct cpufreq_freqs freqs;
+	unsigned int cbe_pmode_new;
+
+	cpufreq_frequency_table_target(policy,
+				       cbe_freqs,
+				       target_freq,
+				       relation,
+				       &cbe_pmode_new);
+
+	freqs.old = policy->cur;
+	freqs.new = cbe_freqs[cbe_pmode_new].frequency;
+
+	mutex_lock(&cbe_switch_mutex);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+	pr_debug("setting frequency for cpu %d to %d kHz, " \
+		 "1/%d of max frequency\n",
+		 policy->cpu,
+		 cbe_freqs[cbe_pmode_new].frequency,
+		 cbe_freqs[cbe_pmode_new].index);
+
+	rc = set_pmode(policy->cpu, cbe_pmode_new);
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+	mutex_unlock(&cbe_switch_mutex);
+
+	return rc;
+}
+
+static struct cpufreq_driver cbe_cpufreq_driver = {
+	.verify		= cbe_cpufreq_verify,
+	.target		= cbe_cpufreq_target,
+	.init		= cbe_cpufreq_cpu_init,
+	.exit		= cbe_cpufreq_cpu_exit,
+	.name		= "cbe-cpufreq",
+	.owner		= THIS_MODULE,
+	.flags		= CPUFREQ_CONST_LOOPS,
+};
+
+/*
+ * module init and destoy
+ */
+
+static int __init cbe_cpufreq_init(void)
+{
+	if (!machine_is(cell))
+		return -ENODEV;
+
+	return cpufreq_register_driver(&cbe_cpufreq_driver);
+}
+
+static void __exit cbe_cpufreq_exit(void)
+{
+	cpufreq_unregister_driver(&cbe_cpufreq_driver);
+}
+
+module_init(cbe_cpufreq_init);
+module_exit(cbe_cpufreq_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Christian Krafft <krafft@de.ibm.com>");
diff --git a/drivers/cpufreq/ppc_cbe_cpufreq.h b/drivers/cpufreq/ppc_cbe_cpufreq.h
new file mode 100644
index 000000000000..b4c00a5a6a59
--- /dev/null
+++ b/drivers/cpufreq/ppc_cbe_cpufreq.h
@@ -0,0 +1,24 @@
+/*
+ * ppc_cbe_cpufreq.h
+ *
+ * This file contains the definitions used by the cbe_cpufreq driver.
+ *
+ * (C) Copyright IBM Deutschland Entwicklung GmbH 2005-2007
+ *
+ * Author: Christian Krafft <krafft@de.ibm.com>
+ *
+ */
+
+#include <linux/cpufreq.h>
+#include <linux/types.h>
+
+int cbe_cpufreq_set_pmode(int cpu, unsigned int pmode);
+int cbe_cpufreq_get_pmode(int cpu);
+
+int cbe_cpufreq_set_pmode_pmi(int cpu, unsigned int pmode);
+
+#if defined(CONFIG_CPU_FREQ_CBE_PMI) || defined(CONFIG_CPU_FREQ_CBE_PMI_MODULE)
+extern bool cbe_cpufreq_has_pmi;
+#else
+#define cbe_cpufreq_has_pmi (0)
+#endif
diff --git a/drivers/cpufreq/ppc_cbe_cpufreq_pervasive.c b/drivers/cpufreq/ppc_cbe_cpufreq_pervasive.c
new file mode 100644
index 000000000000..84d2f2cf5ba7
--- /dev/null
+++ b/drivers/cpufreq/ppc_cbe_cpufreq_pervasive.c
@@ -0,0 +1,115 @@
+/*
+ * pervasive backend for the cbe_cpufreq driver
+ *
+ * This driver makes use of the pervasive unit to
+ * engage the desired frequency.
+ *
+ * (C) Copyright IBM Deutschland Entwicklung GmbH 2005-2007
+ *
+ * Author: Christian Krafft <krafft@de.ibm.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, 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/time.h>
+#include <asm/machdep.h>
+#include <asm/hw_irq.h>
+#include <asm/cell-regs.h>
+
+#include "ppc_cbe_cpufreq.h"
+
+/* to write to MIC register */
+static u64 MIC_Slow_Fast_Timer_table[] = {
+	[0 ... 7] = 0x007fc00000000000ull,
+};
+
+/* more values for the MIC */
+static u64 MIC_Slow_Next_Timer_table[] = {
+	0x0000240000000000ull,
+	0x0000268000000000ull,
+	0x000029C000000000ull,
+	0x00002D0000000000ull,
+	0x0000300000000000ull,
+	0x0000334000000000ull,
+	0x000039C000000000ull,
+	0x00003FC000000000ull,
+};
+
+
+int cbe_cpufreq_set_pmode(int cpu, unsigned int pmode)
+{
+	struct cbe_pmd_regs __iomem *pmd_regs;
+	struct cbe_mic_tm_regs __iomem *mic_tm_regs;
+	unsigned long flags;
+	u64 value;
+#ifdef DEBUG
+	long time;
+#endif
+
+	local_irq_save(flags);
+
+	mic_tm_regs = cbe_get_cpu_mic_tm_regs(cpu);
+	pmd_regs = cbe_get_cpu_pmd_regs(cpu);
+
+#ifdef DEBUG
+	time = jiffies;
+#endif
+
+	out_be64(&mic_tm_regs->slow_fast_timer_0, MIC_Slow_Fast_Timer_table[pmode]);
+	out_be64(&mic_tm_regs->slow_fast_timer_1, MIC_Slow_Fast_Timer_table[pmode]);
+
+	out_be64(&mic_tm_regs->slow_next_timer_0, MIC_Slow_Next_Timer_table[pmode]);
+	out_be64(&mic_tm_regs->slow_next_timer_1, MIC_Slow_Next_Timer_table[pmode]);
+
+	value = in_be64(&pmd_regs->pmcr);
+	/* set bits to zero */
+	value &= 0xFFFFFFFFFFFFFFF8ull;
+	/* set bits to next pmode */
+	value |= pmode;
+
+	out_be64(&pmd_regs->pmcr, value);
+
+#ifdef DEBUG
+	/* wait until new pmode appears in status register */
+	value = in_be64(&pmd_regs->pmsr) & 0x07;
+	while (value != pmode) {
+		cpu_relax();
+		value = in_be64(&pmd_regs->pmsr) & 0x07;
+	}
+
+	time = jiffies  - time;
+	time = jiffies_to_msecs(time);
+	pr_debug("had to wait %lu ms for a transition using " \
+		 "pervasive unit\n", time);
+#endif
+	local_irq_restore(flags);
+
+	return 0;
+}
+
+
+int cbe_cpufreq_get_pmode(int cpu)
+{
+	int ret;
+	struct cbe_pmd_regs __iomem *pmd_regs;
+
+	pmd_regs = cbe_get_cpu_pmd_regs(cpu);
+	ret = in_be64(&pmd_regs->pmsr) & 0x07;
+
+	return ret;
+}
+
diff --git a/drivers/cpufreq/ppc_cbe_cpufreq_pmi.c b/drivers/cpufreq/ppc_cbe_cpufreq_pmi.c
new file mode 100644
index 000000000000..d29e8da396a0
--- /dev/null
+++ b/drivers/cpufreq/ppc_cbe_cpufreq_pmi.c
@@ -0,0 +1,156 @@
+/*
+ * pmi backend for the cbe_cpufreq driver
+ *
+ * (C) Copyright IBM Deutschland Entwicklung GmbH 2005-2007
+ *
+ * Author: Christian Krafft <krafft@de.ibm.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, 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/timer.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+
+#include <asm/processor.h>
+#include <asm/prom.h>
+#include <asm/pmi.h>
+#include <asm/cell-regs.h>
+
+#ifdef DEBUG
+#include <asm/time.h>
+#endif
+
+#include "ppc_cbe_cpufreq.h"
+
+static u8 pmi_slow_mode_limit[MAX_CBE];
+
+bool cbe_cpufreq_has_pmi = false;
+EXPORT_SYMBOL_GPL(cbe_cpufreq_has_pmi);
+
+/*
+ * hardware specific functions
+ */
+
+int cbe_cpufreq_set_pmode_pmi(int cpu, unsigned int pmode)
+{
+	int ret;
+	pmi_message_t pmi_msg;
+#ifdef DEBUG
+	long time;
+#endif
+	pmi_msg.type = PMI_TYPE_FREQ_CHANGE;
+	pmi_msg.data1 =	cbe_cpu_to_node(cpu);
+	pmi_msg.data2 = pmode;
+
+#ifdef DEBUG
+	time = jiffies;
+#endif
+	pmi_send_message(pmi_msg);
+
+#ifdef DEBUG
+	time = jiffies  - time;
+	time = jiffies_to_msecs(time);
+	pr_debug("had to wait %lu ms for a transition using " \
+		 "PMI\n", time);
+#endif
+	ret = pmi_msg.data2;
+	pr_debug("PMI returned slow mode %d\n", ret);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(cbe_cpufreq_set_pmode_pmi);
+
+
+static void cbe_cpufreq_handle_pmi(pmi_message_t pmi_msg)
+{
+	u8 node, slow_mode;
+
+	BUG_ON(pmi_msg.type != PMI_TYPE_FREQ_CHANGE);
+
+	node = pmi_msg.data1;
+	slow_mode = pmi_msg.data2;
+
+	pmi_slow_mode_limit[node] = slow_mode;
+
+	pr_debug("cbe_handle_pmi: node: %d max_freq: %d\n", node, slow_mode);
+}
+
+static int pmi_notifier(struct notifier_block *nb,
+				       unsigned long event, void *data)
+{
+	struct cpufreq_policy *policy = data;
+	struct cpufreq_frequency_table *cbe_freqs;
+	u8 node;
+
+	/* Should this really be called for CPUFREQ_ADJUST, CPUFREQ_INCOMPATIBLE
+	 * and CPUFREQ_NOTIFY policy events?)
+	 */
+	if (event == CPUFREQ_START)
+		return 0;
+
+	cbe_freqs = cpufreq_frequency_get_table(policy->cpu);
+	node = cbe_cpu_to_node(policy->cpu);
+
+	pr_debug("got notified, event=%lu, node=%u\n", event, node);
+
+	if (pmi_slow_mode_limit[node] != 0) {
+		pr_debug("limiting node %d to slow mode %d\n",
+			 node, pmi_slow_mode_limit[node]);
+
+		cpufreq_verify_within_limits(policy, 0,
+
+			cbe_freqs[pmi_slow_mode_limit[node]].frequency);
+	}
+
+	return 0;
+}
+
+static struct notifier_block pmi_notifier_block = {
+	.notifier_call = pmi_notifier,
+};
+
+static struct pmi_handler cbe_pmi_handler = {
+	.type			= PMI_TYPE_FREQ_CHANGE,
+	.handle_pmi_message	= cbe_cpufreq_handle_pmi,
+};
+
+
+
+static int __init cbe_cpufreq_pmi_init(void)
+{
+	cbe_cpufreq_has_pmi = pmi_register_handler(&cbe_pmi_handler) == 0;
+
+	if (!cbe_cpufreq_has_pmi)
+		return -ENODEV;
+
+	cpufreq_register_notifier(&pmi_notifier_block, CPUFREQ_POLICY_NOTIFIER);
+
+	return 0;
+}
+
+static void __exit cbe_cpufreq_pmi_exit(void)
+{
+	cpufreq_unregister_notifier(&pmi_notifier_block, CPUFREQ_POLICY_NOTIFIER);
+	pmi_unregister_handler(&cbe_pmi_handler);
+}
+
+module_init(cbe_cpufreq_pmi_init);
+module_exit(cbe_cpufreq_pmi_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Christian Krafft <krafft@de.ibm.com>");
diff --git a/drivers/cpufreq/pxa2xx-cpufreq.c b/drivers/cpufreq/pxa2xx-cpufreq.c
new file mode 100644
index 000000000000..9e5bc8e388a0
--- /dev/null
+++ b/drivers/cpufreq/pxa2xx-cpufreq.c
@@ -0,0 +1,492 @@
+/*
+ *  Copyright (C) 2002,2003 Intrinsyc Software
+ *
+ * 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
+ *
+ * History:
+ *   31-Jul-2002 : Initial version [FB]
+ *   29-Jan-2003 : added PXA255 support [FB]
+ *   20-Apr-2003 : ported to v2.5 (Dustin McIntire, Sensoria Corp.)
+ *
+ * Note:
+ *   This driver may change the memory bus clock rate, but will not do any
+ *   platform specific access timing changes... for example if you have flash
+ *   memory connected to CS0, you will need to register a platform specific
+ *   notifier which will adjust the memory access strobes to maintain a
+ *   minimum strobe width.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/init.h>
+#include <linux/cpufreq.h>
+#include <linux/err.h>
+#include <linux/regulator/consumer.h>
+#include <linux/io.h>
+
+#include <mach/pxa2xx-regs.h>
+#include <mach/smemc.h>
+
+#ifdef DEBUG
+static unsigned int freq_debug;
+module_param(freq_debug, uint, 0);
+MODULE_PARM_DESC(freq_debug, "Set the debug messages to on=1/off=0");
+#else
+#define freq_debug  0
+#endif
+
+static struct regulator *vcc_core;
+
+static unsigned int pxa27x_maxfreq;
+module_param(pxa27x_maxfreq, uint, 0);
+MODULE_PARM_DESC(pxa27x_maxfreq, "Set the pxa27x maxfreq in MHz"
+		 "(typically 624=>pxa270, 416=>pxa271, 520=>pxa272)");
+
+typedef struct {
+	unsigned int khz;
+	unsigned int membus;
+	unsigned int cccr;
+	unsigned int div2;
+	unsigned int cclkcfg;
+	int vmin;
+	int vmax;
+} pxa_freqs_t;
+
+/* Define the refresh period in mSec for the SDRAM and the number of rows */
+#define SDRAM_TREF	64	/* standard 64ms SDRAM */
+static unsigned int sdram_rows;
+
+#define CCLKCFG_TURBO		0x1
+#define CCLKCFG_FCS		0x2
+#define CCLKCFG_HALFTURBO	0x4
+#define CCLKCFG_FASTBUS		0x8
+#define MDREFR_DB2_MASK		(MDREFR_K2DB2 | MDREFR_K1DB2)
+#define MDREFR_DRI_MASK		0xFFF
+
+#define MDCNFG_DRAC2(mdcnfg) (((mdcnfg) >> 21) & 0x3)
+#define MDCNFG_DRAC0(mdcnfg) (((mdcnfg) >> 5) & 0x3)
+
+/*
+ * PXA255 definitions
+ */
+/* Use the run mode frequencies for the CPUFREQ_POLICY_PERFORMANCE policy */
+#define CCLKCFG			CCLKCFG_TURBO | CCLKCFG_FCS
+
+static pxa_freqs_t pxa255_run_freqs[] =
+{
+	/* CPU   MEMBUS  CCCR  DIV2 CCLKCFG	           run  turbo PXbus SDRAM */
+	{ 99500,  99500, 0x121, 1,  CCLKCFG, -1, -1},	/*  99,   99,   50,   50  */
+	{132700, 132700, 0x123, 1,  CCLKCFG, -1, -1},	/* 133,  133,   66,   66  */
+	{199100,  99500, 0x141, 0,  CCLKCFG, -1, -1},	/* 199,  199,   99,   99  */
+	{265400, 132700, 0x143, 1,  CCLKCFG, -1, -1},	/* 265,  265,  133,   66  */
+	{331800, 165900, 0x145, 1,  CCLKCFG, -1, -1},	/* 331,  331,  166,   83  */
+	{398100,  99500, 0x161, 0,  CCLKCFG, -1, -1},	/* 398,  398,  196,   99  */
+};
+
+/* Use the turbo mode frequencies for the CPUFREQ_POLICY_POWERSAVE policy */
+static pxa_freqs_t pxa255_turbo_freqs[] =
+{
+	/* CPU   MEMBUS  CCCR  DIV2 CCLKCFG	   run  turbo PXbus SDRAM */
+	{ 99500, 99500,  0x121, 1,  CCLKCFG, -1, -1},	/*  99,   99,   50,   50  */
+	{199100, 99500,  0x221, 0,  CCLKCFG, -1, -1},	/*  99,  199,   50,   99  */
+	{298500, 99500,  0x321, 0,  CCLKCFG, -1, -1},	/*  99,  287,   50,   99  */
+	{298600, 99500,  0x1c1, 0,  CCLKCFG, -1, -1},	/* 199,  287,   99,   99  */
+	{398100, 99500,  0x241, 0,  CCLKCFG, -1, -1},	/* 199,  398,   99,   99  */
+};
+
+#define NUM_PXA25x_RUN_FREQS ARRAY_SIZE(pxa255_run_freqs)
+#define NUM_PXA25x_TURBO_FREQS ARRAY_SIZE(pxa255_turbo_freqs)
+
+static struct cpufreq_frequency_table
+	pxa255_run_freq_table[NUM_PXA25x_RUN_FREQS+1];
+static struct cpufreq_frequency_table
+	pxa255_turbo_freq_table[NUM_PXA25x_TURBO_FREQS+1];
+
+static unsigned int pxa255_turbo_table;
+module_param(pxa255_turbo_table, uint, 0);
+MODULE_PARM_DESC(pxa255_turbo_table, "Selects the frequency table (0 = run table, !0 = turbo table)");
+
+/*
+ * PXA270 definitions
+ *
+ * For the PXA27x:
+ * Control variables are A, L, 2N for CCCR; B, HT, T for CLKCFG.
+ *
+ * A = 0 => memory controller clock from table 3-7,
+ * A = 1 => memory controller clock = system bus clock
+ * Run mode frequency	= 13 MHz * L
+ * Turbo mode frequency = 13 MHz * L * N
+ * System bus frequency = 13 MHz * L / (B + 1)
+ *
+ * In CCCR:
+ * A = 1
+ * L = 16	  oscillator to run mode ratio
+ * 2N = 6	  2 * (turbo mode to run mode ratio)
+ *
+ * In CCLKCFG:
+ * B = 1	  Fast bus mode
+ * HT = 0	  Half-Turbo mode
+ * T = 1	  Turbo mode
+ *
+ * For now, just support some of the combinations in table 3-7 of
+ * PXA27x Processor Family Developer's Manual to simplify frequency
+ * change sequences.
+ */
+#define PXA27x_CCCR(A, L, N2) (A << 25 | N2 << 7 | L)
+#define CCLKCFG2(B, HT, T) \
+  (CCLKCFG_FCS | \
+   ((B)  ? CCLKCFG_FASTBUS : 0) | \
+   ((HT) ? CCLKCFG_HALFTURBO : 0) | \
+   ((T)  ? CCLKCFG_TURBO : 0))
+
+static pxa_freqs_t pxa27x_freqs[] = {
+	{104000, 104000, PXA27x_CCCR(1,	 8, 2), 0, CCLKCFG2(1, 0, 1),  900000, 1705000 },
+	{156000, 104000, PXA27x_CCCR(1,	 8, 3), 0, CCLKCFG2(1, 0, 1), 1000000, 1705000 },
+	{208000, 208000, PXA27x_CCCR(0, 16, 2), 1, CCLKCFG2(0, 0, 1), 1180000, 1705000 },
+	{312000, 208000, PXA27x_CCCR(1, 16, 3), 1, CCLKCFG2(1, 0, 1), 1250000, 1705000 },
+	{416000, 208000, PXA27x_CCCR(1, 16, 4), 1, CCLKCFG2(1, 0, 1), 1350000, 1705000 },
+	{520000, 208000, PXA27x_CCCR(1, 16, 5), 1, CCLKCFG2(1, 0, 1), 1450000, 1705000 },
+	{624000, 208000, PXA27x_CCCR(1, 16, 6), 1, CCLKCFG2(1, 0, 1), 1550000, 1705000 }
+};
+
+#define NUM_PXA27x_FREQS ARRAY_SIZE(pxa27x_freqs)
+static struct cpufreq_frequency_table
+	pxa27x_freq_table[NUM_PXA27x_FREQS+1];
+
+extern unsigned get_clk_frequency_khz(int info);
+
+#ifdef CONFIG_REGULATOR
+
+static int pxa_cpufreq_change_voltage(pxa_freqs_t *pxa_freq)
+{
+	int ret = 0;
+	int vmin, vmax;
+
+	if (!cpu_is_pxa27x())
+		return 0;
+
+	vmin = pxa_freq->vmin;
+	vmax = pxa_freq->vmax;
+	if ((vmin == -1) || (vmax == -1))
+		return 0;
+
+	ret = regulator_set_voltage(vcc_core, vmin, vmax);
+	if (ret)
+		pr_err("cpufreq: Failed to set vcc_core in [%dmV..%dmV]\n",
+		       vmin, vmax);
+	return ret;
+}
+
+static __init void pxa_cpufreq_init_voltages(void)
+{
+	vcc_core = regulator_get(NULL, "vcc_core");
+	if (IS_ERR(vcc_core)) {
+		pr_info("cpufreq: Didn't find vcc_core regulator\n");
+		vcc_core = NULL;
+	} else {
+		pr_info("cpufreq: Found vcc_core regulator\n");
+	}
+}
+#else
+static int pxa_cpufreq_change_voltage(pxa_freqs_t *pxa_freq)
+{
+	return 0;
+}
+
+static __init void pxa_cpufreq_init_voltages(void) { }
+#endif
+
+static void find_freq_tables(struct cpufreq_frequency_table **freq_table,
+			     pxa_freqs_t **pxa_freqs)
+{
+	if (cpu_is_pxa25x()) {
+		if (!pxa255_turbo_table) {
+			*pxa_freqs = pxa255_run_freqs;
+			*freq_table = pxa255_run_freq_table;
+		} else {
+			*pxa_freqs = pxa255_turbo_freqs;
+			*freq_table = pxa255_turbo_freq_table;
+		}
+	} else if (cpu_is_pxa27x()) {
+		*pxa_freqs = pxa27x_freqs;
+		*freq_table = pxa27x_freq_table;
+	} else {
+		BUG();
+	}
+}
+
+static void pxa27x_guess_max_freq(void)
+{
+	if (!pxa27x_maxfreq) {
+		pxa27x_maxfreq = 416000;
+		printk(KERN_INFO "PXA CPU 27x max frequency not defined "
+		       "(pxa27x_maxfreq), assuming pxa271 with %dkHz maxfreq\n",
+		       pxa27x_maxfreq);
+	} else {
+		pxa27x_maxfreq *= 1000;
+	}
+}
+
+static void init_sdram_rows(void)
+{
+	uint32_t mdcnfg = __raw_readl(MDCNFG);
+	unsigned int drac2 = 0, drac0 = 0;
+
+	if (mdcnfg & (MDCNFG_DE2 | MDCNFG_DE3))
+		drac2 = MDCNFG_DRAC2(mdcnfg);
+
+	if (mdcnfg & (MDCNFG_DE0 | MDCNFG_DE1))
+		drac0 = MDCNFG_DRAC0(mdcnfg);
+
+	sdram_rows = 1 << (11 + max(drac0, drac2));
+}
+
+static u32 mdrefr_dri(unsigned int freq)
+{
+	u32 interval = freq * SDRAM_TREF / sdram_rows;
+
+	return (interval - (cpu_is_pxa27x() ? 31 : 0)) / 32;
+}
+
+/* find a valid frequency point */
+static int pxa_verify_policy(struct cpufreq_policy *policy)
+{
+	struct cpufreq_frequency_table *pxa_freqs_table;
+	pxa_freqs_t *pxa_freqs;
+	int ret;
+
+	find_freq_tables(&pxa_freqs_table, &pxa_freqs);
+	ret = cpufreq_frequency_table_verify(policy, pxa_freqs_table);
+
+	if (freq_debug)
+		pr_debug("Verified CPU policy: %dKhz min to %dKhz max\n",
+			 policy->min, policy->max);
+
+	return ret;
+}
+
+static unsigned int pxa_cpufreq_get(unsigned int cpu)
+{
+	return get_clk_frequency_khz(0);
+}
+
+static int pxa_set_target(struct cpufreq_policy *policy,
+			  unsigned int target_freq,
+			  unsigned int relation)
+{
+	struct cpufreq_frequency_table *pxa_freqs_table;
+	pxa_freqs_t *pxa_freq_settings;
+	struct cpufreq_freqs freqs;
+	unsigned int idx;
+	unsigned long flags;
+	unsigned int new_freq_cpu, new_freq_mem;
+	unsigned int unused, preset_mdrefr, postset_mdrefr, cclkcfg;
+	int ret = 0;
+
+	/* Get the current policy */
+	find_freq_tables(&pxa_freqs_table, &pxa_freq_settings);
+
+	/* Lookup the next frequency */
+	if (cpufreq_frequency_table_target(policy, pxa_freqs_table,
+					   target_freq, relation, &idx)) {
+		return -EINVAL;
+	}
+
+	new_freq_cpu = pxa_freq_settings[idx].khz;
+	new_freq_mem = pxa_freq_settings[idx].membus;
+	freqs.old = policy->cur;
+	freqs.new = new_freq_cpu;
+
+	if (freq_debug)
+		pr_debug("Changing CPU frequency to %d Mhz, (SDRAM %d Mhz)\n",
+			 freqs.new / 1000, (pxa_freq_settings[idx].div2) ?
+			 (new_freq_mem / 2000) : (new_freq_mem / 1000));
+
+	if (vcc_core && freqs.new > freqs.old)
+		ret = pxa_cpufreq_change_voltage(&pxa_freq_settings[idx]);
+	if (ret)
+		return ret;
+	/*
+	 * Tell everyone what we're about to do...
+	 * you should add a notify client with any platform specific
+	 * Vcc changing capability
+	 */
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+	/* Calculate the next MDREFR.  If we're slowing down the SDRAM clock
+	 * we need to preset the smaller DRI before the change.	 If we're
+	 * speeding up we need to set the larger DRI value after the change.
+	 */
+	preset_mdrefr = postset_mdrefr = __raw_readl(MDREFR);
+	if ((preset_mdrefr & MDREFR_DRI_MASK) > mdrefr_dri(new_freq_mem)) {
+		preset_mdrefr = (preset_mdrefr & ~MDREFR_DRI_MASK);
+		preset_mdrefr |= mdrefr_dri(new_freq_mem);
+	}
+	postset_mdrefr =
+		(postset_mdrefr & ~MDREFR_DRI_MASK) | mdrefr_dri(new_freq_mem);
+
+	/* If we're dividing the memory clock by two for the SDRAM clock, this
+	 * must be set prior to the change.  Clearing the divide must be done
+	 * after the change.
+	 */
+	if (pxa_freq_settings[idx].div2) {
+		preset_mdrefr  |= MDREFR_DB2_MASK;
+		postset_mdrefr |= MDREFR_DB2_MASK;
+	} else {
+		postset_mdrefr &= ~MDREFR_DB2_MASK;
+	}
+
+	local_irq_save(flags);
+
+	/* Set new the CCCR and prepare CCLKCFG */
+	CCCR = pxa_freq_settings[idx].cccr;
+	cclkcfg = pxa_freq_settings[idx].cclkcfg;
+
+	asm volatile("							\n\
+		ldr	r4, [%1]		/* load MDREFR */	\n\
+		b	2f						\n\
+		.align	5						\n\
+1:									\n\
+		str	%3, [%1]		/* preset the MDREFR */	\n\
+		mcr	p14, 0, %2, c6, c0, 0	/* set CCLKCFG[FCS] */	\n\
+		str	%4, [%1]		/* postset the MDREFR */ \n\
+									\n\
+		b	3f						\n\
+2:		b	1b						\n\
+3:		nop							\n\
+	  "
+		     : "=&r" (unused)
+		     : "r" (MDREFR), "r" (cclkcfg),
+		       "r" (preset_mdrefr), "r" (postset_mdrefr)
+		     : "r4", "r5");
+	local_irq_restore(flags);
+
+	/*
+	 * Tell everyone what we've just done...
+	 * you should add a notify client with any platform specific
+	 * SDRAM refresh timer adjustments
+	 */
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	/*
+	 * Even if voltage setting fails, we don't report it, as the frequency
+	 * change succeeded. The voltage reduction is not a critical failure,
+	 * only power savings will suffer from this.
+	 *
+	 * Note: if the voltage change fails, and a return value is returned, a
+	 * bug is triggered (seems a deadlock). Should anybody find out where,
+	 * the "return 0" should become a "return ret".
+	 */
+	if (vcc_core && freqs.new < freqs.old)
+		ret = pxa_cpufreq_change_voltage(&pxa_freq_settings[idx]);
+
+	return 0;
+}
+
+static int pxa_cpufreq_init(struct cpufreq_policy *policy)
+{
+	int i;
+	unsigned int freq;
+	struct cpufreq_frequency_table *pxa255_freq_table;
+	pxa_freqs_t *pxa255_freqs;
+
+	/* try to guess pxa27x cpu */
+	if (cpu_is_pxa27x())
+		pxa27x_guess_max_freq();
+
+	pxa_cpufreq_init_voltages();
+
+	init_sdram_rows();
+
+	/* set default policy and cpuinfo */
+	policy->cpuinfo.transition_latency = 1000; /* FIXME: 1 ms, assumed */
+	policy->cur = get_clk_frequency_khz(0);	   /* current freq */
+	policy->min = policy->max = policy->cur;
+
+	/* Generate pxa25x the run cpufreq_frequency_table struct */
+	for (i = 0; i < NUM_PXA25x_RUN_FREQS; i++) {
+		pxa255_run_freq_table[i].frequency = pxa255_run_freqs[i].khz;
+		pxa255_run_freq_table[i].index = i;
+	}
+	pxa255_run_freq_table[i].frequency = CPUFREQ_TABLE_END;
+
+	/* Generate pxa25x the turbo cpufreq_frequency_table struct */
+	for (i = 0; i < NUM_PXA25x_TURBO_FREQS; i++) {
+		pxa255_turbo_freq_table[i].frequency =
+			pxa255_turbo_freqs[i].khz;
+		pxa255_turbo_freq_table[i].index = i;
+	}
+	pxa255_turbo_freq_table[i].frequency = CPUFREQ_TABLE_END;
+
+	pxa255_turbo_table = !!pxa255_turbo_table;
+
+	/* Generate the pxa27x cpufreq_frequency_table struct */
+	for (i = 0; i < NUM_PXA27x_FREQS; i++) {
+		freq = pxa27x_freqs[i].khz;
+		if (freq > pxa27x_maxfreq)
+			break;
+		pxa27x_freq_table[i].frequency = freq;
+		pxa27x_freq_table[i].index = i;
+	}
+	pxa27x_freq_table[i].index = i;
+	pxa27x_freq_table[i].frequency = CPUFREQ_TABLE_END;
+
+	/*
+	 * Set the policy's minimum and maximum frequencies from the tables
+	 * just constructed.  This sets cpuinfo.mxx_freq, min and max.
+	 */
+	if (cpu_is_pxa25x()) {
+		find_freq_tables(&pxa255_freq_table, &pxa255_freqs);
+		pr_info("PXA255 cpufreq using %s frequency table\n",
+			pxa255_turbo_table ? "turbo" : "run");
+		cpufreq_frequency_table_cpuinfo(policy, pxa255_freq_table);
+	}
+	else if (cpu_is_pxa27x())
+		cpufreq_frequency_table_cpuinfo(policy, pxa27x_freq_table);
+
+	printk(KERN_INFO "PXA CPU frequency change support initialized\n");
+
+	return 0;
+}
+
+static struct cpufreq_driver pxa_cpufreq_driver = {
+	.verify	= pxa_verify_policy,
+	.target	= pxa_set_target,
+	.init	= pxa_cpufreq_init,
+	.get	= pxa_cpufreq_get,
+	.name	= "PXA2xx",
+};
+
+static int __init pxa_cpu_init(void)
+{
+	int ret = -ENODEV;
+	if (cpu_is_pxa25x() || cpu_is_pxa27x())
+		ret = cpufreq_register_driver(&pxa_cpufreq_driver);
+	return ret;
+}
+
+static void __exit pxa_cpu_exit(void)
+{
+	cpufreq_unregister_driver(&pxa_cpufreq_driver);
+}
+
+
+MODULE_AUTHOR("Intrinsyc Software Inc.");
+MODULE_DESCRIPTION("CPU frequency changing driver for the PXA architecture");
+MODULE_LICENSE("GPL");
+module_init(pxa_cpu_init);
+module_exit(pxa_cpu_exit);
diff --git a/drivers/cpufreq/pxa3xx-cpufreq.c b/drivers/cpufreq/pxa3xx-cpufreq.c
new file mode 100644
index 000000000000..15d60f857ad5
--- /dev/null
+++ b/drivers/cpufreq/pxa3xx-cpufreq.c
@@ -0,0 +1,254 @@
+/*
+ * Copyright (C) 2008 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 as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/init.h>
+#include <linux/cpufreq.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+
+#include <mach/generic.h>
+#include <mach/pxa3xx-regs.h>
+
+#define HSS_104M	(0)
+#define HSS_156M	(1)
+#define HSS_208M	(2)
+#define HSS_312M	(3)
+
+#define SMCFS_78M	(0)
+#define SMCFS_104M	(2)
+#define SMCFS_208M	(5)
+
+#define SFLFS_104M	(0)
+#define SFLFS_156M	(1)
+#define SFLFS_208M	(2)
+#define SFLFS_312M	(3)
+
+#define XSPCLK_156M	(0)
+#define XSPCLK_NONE	(3)
+
+#define DMCFS_26M	(0)
+#define DMCFS_260M	(3)
+
+struct pxa3xx_freq_info {
+	unsigned int cpufreq_mhz;
+	unsigned int core_xl : 5;
+	unsigned int core_xn : 3;
+	unsigned int hss : 2;
+	unsigned int dmcfs : 2;
+	unsigned int smcfs : 3;
+	unsigned int sflfs : 2;
+	unsigned int df_clkdiv : 3;
+
+	int	vcc_core;	/* in mV */
+	int	vcc_sram;	/* in mV */
+};
+
+#define OP(cpufreq, _xl, _xn, _hss, _dmc, _smc, _sfl, _dfi, vcore, vsram) \
+{									\
+	.cpufreq_mhz	= cpufreq,					\
+	.core_xl	= _xl,						\
+	.core_xn	= _xn,						\
+	.hss		= HSS_##_hss##M,				\
+	.dmcfs		= DMCFS_##_dmc##M,				\
+	.smcfs		= SMCFS_##_smc##M,				\
+	.sflfs		= SFLFS_##_sfl##M,				\
+	.df_clkdiv	= _dfi,						\
+	.vcc_core	= vcore,					\
+	.vcc_sram	= vsram,					\
+}
+
+static struct pxa3xx_freq_info pxa300_freqs[] = {
+	/*  CPU XL XN  HSS DMEM SMEM SRAM DFI VCC_CORE VCC_SRAM */
+	OP(104,  8, 1, 104, 260,  78, 104, 3, 1000, 1100), /* 104MHz */
+	OP(208, 16, 1, 104, 260, 104, 156, 2, 1000, 1100), /* 208MHz */
+	OP(416, 16, 2, 156, 260, 104, 208, 2, 1100, 1200), /* 416MHz */
+	OP(624, 24, 2, 208, 260, 208, 312, 3, 1375, 1400), /* 624MHz */
+};
+
+static struct pxa3xx_freq_info pxa320_freqs[] = {
+	/*  CPU XL XN  HSS DMEM SMEM SRAM DFI VCC_CORE VCC_SRAM */
+	OP(104,  8, 1, 104, 260,  78, 104, 3, 1000, 1100), /* 104MHz */
+	OP(208, 16, 1, 104, 260, 104, 156, 2, 1000, 1100), /* 208MHz */
+	OP(416, 16, 2, 156, 260, 104, 208, 2, 1100, 1200), /* 416MHz */
+	OP(624, 24, 2, 208, 260, 208, 312, 3, 1375, 1400), /* 624MHz */
+	OP(806, 31, 2, 208, 260, 208, 312, 3, 1400, 1400), /* 806MHz */
+};
+
+static unsigned int pxa3xx_freqs_num;
+static struct pxa3xx_freq_info *pxa3xx_freqs;
+static struct cpufreq_frequency_table *pxa3xx_freqs_table;
+
+static int setup_freqs_table(struct cpufreq_policy *policy,
+			     struct pxa3xx_freq_info *freqs, int num)
+{
+	struct cpufreq_frequency_table *table;
+	int i;
+
+	table = kzalloc((num + 1) * sizeof(*table), GFP_KERNEL);
+	if (table == NULL)
+		return -ENOMEM;
+
+	for (i = 0; i < num; i++) {
+		table[i].index = i;
+		table[i].frequency = freqs[i].cpufreq_mhz * 1000;
+	}
+	table[num].index = i;
+	table[num].frequency = CPUFREQ_TABLE_END;
+
+	pxa3xx_freqs = freqs;
+	pxa3xx_freqs_num = num;
+	pxa3xx_freqs_table = table;
+
+	return cpufreq_frequency_table_cpuinfo(policy, table);
+}
+
+static void __update_core_freq(struct pxa3xx_freq_info *info)
+{
+	uint32_t mask = ACCR_XN_MASK | ACCR_XL_MASK;
+	uint32_t accr = ACCR;
+	uint32_t xclkcfg;
+
+	accr &= ~(ACCR_XN_MASK | ACCR_XL_MASK | ACCR_XSPCLK_MASK);
+	accr |= ACCR_XN(info->core_xn) | ACCR_XL(info->core_xl);
+
+	/* No clock until core PLL is re-locked */
+	accr |= ACCR_XSPCLK(XSPCLK_NONE);
+
+	xclkcfg = (info->core_xn == 2) ? 0x3 : 0x2;	/* turbo bit */
+
+	ACCR = accr;
+	__asm__("mcr p14, 0, %0, c6, c0, 0\n" : : "r"(xclkcfg));
+
+	while ((ACSR & mask) != (accr & mask))
+		cpu_relax();
+}
+
+static void __update_bus_freq(struct pxa3xx_freq_info *info)
+{
+	uint32_t mask;
+	uint32_t accr = ACCR;
+
+	mask = ACCR_SMCFS_MASK | ACCR_SFLFS_MASK | ACCR_HSS_MASK |
+		ACCR_DMCFS_MASK;
+
+	accr &= ~mask;
+	accr |= ACCR_SMCFS(info->smcfs) | ACCR_SFLFS(info->sflfs) |
+		ACCR_HSS(info->hss) | ACCR_DMCFS(info->dmcfs);
+
+	ACCR = accr;
+
+	while ((ACSR & mask) != (accr & mask))
+		cpu_relax();
+}
+
+static int pxa3xx_cpufreq_verify(struct cpufreq_policy *policy)
+{
+	return cpufreq_frequency_table_verify(policy, pxa3xx_freqs_table);
+}
+
+static unsigned int pxa3xx_cpufreq_get(unsigned int cpu)
+{
+	return pxa3xx_get_clk_frequency_khz(0);
+}
+
+static int pxa3xx_cpufreq_set(struct cpufreq_policy *policy,
+			      unsigned int target_freq,
+			      unsigned int relation)
+{
+	struct pxa3xx_freq_info *next;
+	struct cpufreq_freqs freqs;
+	unsigned long flags;
+	int idx;
+
+	if (policy->cpu != 0)
+		return -EINVAL;
+
+	/* Lookup the next frequency */
+	if (cpufreq_frequency_table_target(policy, pxa3xx_freqs_table,
+				target_freq, relation, &idx))
+		return -EINVAL;
+
+	next = &pxa3xx_freqs[idx];
+
+	freqs.old = policy->cur;
+	freqs.new = next->cpufreq_mhz * 1000;
+
+	pr_debug("CPU frequency from %d MHz to %d MHz%s\n",
+			freqs.old / 1000, freqs.new / 1000,
+			(freqs.old == freqs.new) ? " (skipped)" : "");
+
+	if (freqs.old == target_freq)
+		return 0;
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+	local_irq_save(flags);
+	__update_core_freq(next);
+	__update_bus_freq(next);
+	local_irq_restore(flags);
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	return 0;
+}
+
+static int pxa3xx_cpufreq_init(struct cpufreq_policy *policy)
+{
+	int ret = -EINVAL;
+
+	/* set default policy and cpuinfo */
+	policy->cpuinfo.min_freq = 104000;
+	policy->cpuinfo.max_freq = (cpu_is_pxa320()) ? 806000 : 624000;
+	policy->cpuinfo.transition_latency = 1000; /* FIXME: 1 ms, assumed */
+	policy->max = pxa3xx_get_clk_frequency_khz(0);
+	policy->cur = policy->min = policy->max;
+
+	if (cpu_is_pxa300() || cpu_is_pxa310())
+		ret = setup_freqs_table(policy, ARRAY_AND_SIZE(pxa300_freqs));
+
+	if (cpu_is_pxa320())
+		ret = setup_freqs_table(policy, ARRAY_AND_SIZE(pxa320_freqs));
+
+	if (ret) {
+		pr_err("failed to setup frequency table\n");
+		return ret;
+	}
+
+	pr_info("CPUFREQ support for PXA3xx initialized\n");
+	return 0;
+}
+
+static struct cpufreq_driver pxa3xx_cpufreq_driver = {
+	.verify		= pxa3xx_cpufreq_verify,
+	.target		= pxa3xx_cpufreq_set,
+	.init		= pxa3xx_cpufreq_init,
+	.get		= pxa3xx_cpufreq_get,
+	.name		= "pxa3xx-cpufreq",
+};
+
+static int __init cpufreq_init(void)
+{
+	if (cpu_is_pxa3xx())
+		return cpufreq_register_driver(&pxa3xx_cpufreq_driver);
+
+	return 0;
+}
+module_init(cpufreq_init);
+
+static void __exit cpufreq_exit(void)
+{
+	cpufreq_unregister_driver(&pxa3xx_cpufreq_driver);
+}
+module_exit(cpufreq_exit);
+
+MODULE_DESCRIPTION("CPU frequency scaling driver for PXA3xx");
+MODULE_LICENSE("GPL");
diff --git a/drivers/cpufreq/s3c2416-cpufreq.c b/drivers/cpufreq/s3c2416-cpufreq.c
index bcc053bc02c4..4f1881eee3f1 100644
--- a/drivers/cpufreq/s3c2416-cpufreq.c
+++ b/drivers/cpufreq/s3c2416-cpufreq.c
@@ -256,7 +256,6 @@ static int s3c2416_cpufreq_set_target(struct cpufreq_policy *policy,
 		goto out;
 	}
 
-	freqs.cpu = 0;
 	freqs.flags = 0;
 	freqs.old = s3c_freq->is_dvs ? FREQ_DVS
 				     : clk_get_rate(s3c_freq->armclk) / 1000;
@@ -274,7 +273,7 @@ static int s3c2416_cpufreq_set_target(struct cpufreq_policy *policy,
 	if (!to_dvs && freqs.old == freqs.new)
 		goto out;
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	if (to_dvs) {
 		pr_debug("cpufreq: enter dvs\n");
@@ -287,7 +286,7 @@ static int s3c2416_cpufreq_set_target(struct cpufreq_policy *policy,
 		ret = s3c2416_cpufreq_set_armdiv(s3c_freq, freqs.new);
 	}
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 out:
 	mutex_unlock(&cpufreq_lock);
diff --git a/drivers/cpufreq/s3c64xx-cpufreq.c b/drivers/cpufreq/s3c64xx-cpufreq.c
index 6f9490b3c356..27cacb524796 100644
--- a/drivers/cpufreq/s3c64xx-cpufreq.c
+++ b/drivers/cpufreq/s3c64xx-cpufreq.c
@@ -84,7 +84,6 @@ static int s3c64xx_cpufreq_set_target(struct cpufreq_policy *policy,
 	if (ret != 0)
 		return ret;
 
-	freqs.cpu = 0;
 	freqs.old = clk_get_rate(armclk) / 1000;
 	freqs.new = s3c64xx_freq_table[i].frequency;
 	freqs.flags = 0;
@@ -95,7 +94,7 @@ static int s3c64xx_cpufreq_set_target(struct cpufreq_policy *policy,
 
 	pr_debug("Transition %d-%dkHz\n", freqs.old, freqs.new);
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 #ifdef CONFIG_REGULATOR
 	if (vddarm && freqs.new > freqs.old) {
@@ -117,7 +116,7 @@ static int s3c64xx_cpufreq_set_target(struct cpufreq_policy *policy,
 		goto err;
 	}
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 #ifdef CONFIG_REGULATOR
 	if (vddarm && freqs.new < freqs.old) {
@@ -141,7 +140,7 @@ err_clk:
 	if (clk_set_rate(armclk, freqs.old * 1000) < 0)
 		pr_err("Failed to restore original clock rate\n");
 err:
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 	return ret;
 }
diff --git a/drivers/cpufreq/s5pv210-cpufreq.c b/drivers/cpufreq/s5pv210-cpufreq.c
index a484aaea9809..5c7757073793 100644
--- a/drivers/cpufreq/s5pv210-cpufreq.c
+++ b/drivers/cpufreq/s5pv210-cpufreq.c
@@ -229,7 +229,6 @@ static int s5pv210_target(struct cpufreq_policy *policy,
 	}
 
 	freqs.new = s5pv210_freq_table[index].frequency;
-	freqs.cpu = 0;
 
 	if (freqs.new == freqs.old)
 		goto exit;
@@ -256,7 +255,7 @@ static int s5pv210_target(struct cpufreq_policy *policy,
 			goto exit;
 	}
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	/* Check if there need to change PLL */
 	if ((index == L0) || (priv_index == L0))
@@ -468,7 +467,7 @@ static int s5pv210_target(struct cpufreq_policy *policy,
 		}
 	}
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 	if (freqs.new < freqs.old) {
 		regulator_set_voltage(int_regulator,
diff --git a/drivers/cpufreq/sa1100-cpufreq.c b/drivers/cpufreq/sa1100-cpufreq.c
new file mode 100644
index 000000000000..cff18e87ca58
--- /dev/null
+++ b/drivers/cpufreq/sa1100-cpufreq.c
@@ -0,0 +1,247 @@
+/*
+ * cpu-sa1100.c: clock scaling for the SA1100
+ *
+ * Copyright (C) 2000 2001, The Delft University of Technology
+ *
+ * Authors:
+ * - Johan Pouwelse (J.A.Pouwelse@its.tudelft.nl): initial version
+ * - Erik Mouw (J.A.K.Mouw@its.tudelft.nl):
+ *   - major rewrite for linux-2.3.99
+ *   - rewritten for the more generic power management scheme in
+ *     linux-2.4.5-rmk1
+ *
+ * This software has been developed while working on the LART
+ * computing board (http://www.lartmaker.nl/), which is
+ * sponsored by the Mobile Multi-media Communications
+ * (http://www.mobimedia.org/) and Ubiquitous Communications
+ * (http://www.ubicom.tudelft.nl/) projects.
+ *
+ * The authors can be reached at:
+ *
+ *  Erik Mouw
+ *  Information and Communication Theory Group
+ *  Faculty of Information Technology and Systems
+ *  Delft University of Technology
+ *  P.O. Box 5031
+ *  2600 GA Delft
+ *  The Netherlands
+ *
+ *
+ * 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
+ *
+ *
+ * Theory of operations
+ * ====================
+ *
+ * Clock scaling can be used to lower the power consumption of the CPU
+ * core. This will give you a somewhat longer running time.
+ *
+ * The SA-1100 has a single register to change the core clock speed:
+ *
+ *   PPCR      0x90020014    PLL config
+ *
+ * However, the DRAM timings are closely related to the core clock
+ * speed, so we need to change these, too. The used registers are:
+ *
+ *   MDCNFG    0xA0000000    DRAM config
+ *   MDCAS0    0xA0000004    Access waveform
+ *   MDCAS1    0xA0000008    Access waveform
+ *   MDCAS2    0xA000000C    Access waveform
+ *
+ * Care must be taken to change the DRAM parameters the correct way,
+ * because otherwise the DRAM becomes unusable and the kernel will
+ * crash.
+ *
+ * The simple solution to avoid a kernel crash is to put the actual
+ * clock change in ROM and jump to that code from the kernel. The main
+ * disadvantage is that the ROM has to be modified, which is not
+ * possible on all SA-1100 platforms. Another disadvantage is that
+ * jumping to ROM makes clock switching unnecessary complicated.
+ *
+ * The idea behind this driver is that the memory configuration can be
+ * changed while running from DRAM (even with interrupts turned on!)
+ * as long as all re-configuration steps yield a valid DRAM
+ * configuration. The advantages are clear: it will run on all SA-1100
+ * platforms, and the code is very simple.
+ *
+ * If you really want to understand what is going on in
+ * sa1100_update_dram_timings(), you'll have to read sections 8.2,
+ * 9.5.7.3, and 10.2 from the "Intel StrongARM SA-1100 Microprocessor
+ * Developers Manual" (available for free from Intel).
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/cpufreq.h>
+#include <linux/io.h>
+
+#include <asm/cputype.h>
+
+#include <mach/generic.h>
+#include <mach/hardware.h>
+
+struct sa1100_dram_regs {
+	int speed;
+	u32 mdcnfg;
+	u32 mdcas0;
+	u32 mdcas1;
+	u32 mdcas2;
+};
+
+
+static struct cpufreq_driver sa1100_driver;
+
+static struct sa1100_dram_regs sa1100_dram_settings[] = {
+	/*speed,     mdcnfg,     mdcas0,     mdcas1,     mdcas2,   clock freq */
+	{ 59000, 0x00dc88a3, 0xcccccccf, 0xfffffffc, 0xffffffff},/*  59.0 MHz */
+	{ 73700, 0x011490a3, 0xcccccccf, 0xfffffffc, 0xffffffff},/*  73.7 MHz */
+	{ 88500, 0x014e90a3, 0xcccccccf, 0xfffffffc, 0xffffffff},/*  88.5 MHz */
+	{103200, 0x01889923, 0xcccccccf, 0xfffffffc, 0xffffffff},/* 103.2 MHz */
+	{118000, 0x01c29923, 0x9999998f, 0xfffffff9, 0xffffffff},/* 118.0 MHz */
+	{132700, 0x01fb2123, 0x9999998f, 0xfffffff9, 0xffffffff},/* 132.7 MHz */
+	{147500, 0x02352123, 0x3333330f, 0xfffffff3, 0xffffffff},/* 147.5 MHz */
+	{162200, 0x026b29a3, 0x38e38e1f, 0xfff8e38e, 0xffffffff},/* 162.2 MHz */
+	{176900, 0x02a329a3, 0x71c71c1f, 0xfff1c71c, 0xffffffff},/* 176.9 MHz */
+	{191700, 0x02dd31a3, 0xe38e383f, 0xffe38e38, 0xffffffff},/* 191.7 MHz */
+	{206400, 0x03153223, 0xc71c703f, 0xffc71c71, 0xffffffff},/* 206.4 MHz */
+	{221200, 0x034fba23, 0xc71c703f, 0xffc71c71, 0xffffffff},/* 221.2 MHz */
+	{235900, 0x03853a23, 0xe1e1e07f, 0xe1e1e1e1, 0xffffffe1},/* 235.9 MHz */
+	{250700, 0x03bf3aa3, 0xc3c3c07f, 0xc3c3c3c3, 0xffffffc3},/* 250.7 MHz */
+	{265400, 0x03f7c2a3, 0xc3c3c07f, 0xc3c3c3c3, 0xffffffc3},/* 265.4 MHz */
+	{280200, 0x0431c2a3, 0x878780ff, 0x87878787, 0xffffff87},/* 280.2 MHz */
+	{ 0, 0, 0, 0, 0 } /* last entry */
+};
+
+static void sa1100_update_dram_timings(int current_speed, int new_speed)
+{
+	struct sa1100_dram_regs *settings = sa1100_dram_settings;
+
+	/* find speed */
+	while (settings->speed != 0) {
+		if (new_speed == settings->speed)
+			break;
+
+		settings++;
+	}
+
+	if (settings->speed == 0) {
+		panic("%s: couldn't find dram setting for speed %d\n",
+		      __func__, new_speed);
+	}
+
+	/* No risk, no fun: run with interrupts on! */
+	if (new_speed > current_speed) {
+		/* We're going FASTER, so first relax the memory
+		 * timings before changing the core frequency
+		 */
+
+		/* Half the memory access clock */
+		MDCNFG |= MDCNFG_CDB2;
+
+		/* The order of these statements IS important, keep 8
+		 * pulses!!
+		 */
+		MDCAS2 = settings->mdcas2;
+		MDCAS1 = settings->mdcas1;
+		MDCAS0 = settings->mdcas0;
+		MDCNFG = settings->mdcnfg;
+	} else {
+		/* We're going SLOWER: first decrease the core
+		 * frequency and then tighten the memory settings.
+		 */
+
+		/* Half the memory access clock */
+		MDCNFG |= MDCNFG_CDB2;
+
+		/* The order of these statements IS important, keep 8
+		 * pulses!!
+		 */
+		MDCAS0 = settings->mdcas0;
+		MDCAS1 = settings->mdcas1;
+		MDCAS2 = settings->mdcas2;
+		MDCNFG = settings->mdcnfg;
+	}
+}
+
+static int sa1100_target(struct cpufreq_policy *policy,
+			 unsigned int target_freq,
+			 unsigned int relation)
+{
+	unsigned int cur = sa11x0_getspeed(0);
+	unsigned int new_ppcr;
+	struct cpufreq_freqs freqs;
+
+	new_ppcr = sa11x0_freq_to_ppcr(target_freq);
+	switch (relation) {
+	case CPUFREQ_RELATION_L:
+		if (sa11x0_ppcr_to_freq(new_ppcr) > policy->max)
+			new_ppcr--;
+		break;
+	case CPUFREQ_RELATION_H:
+		if ((sa11x0_ppcr_to_freq(new_ppcr) > target_freq) &&
+		    (sa11x0_ppcr_to_freq(new_ppcr - 1) >= policy->min))
+			new_ppcr--;
+		break;
+	}
+
+	freqs.old = cur;
+	freqs.new = sa11x0_ppcr_to_freq(new_ppcr);
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+	if (freqs.new > cur)
+		sa1100_update_dram_timings(cur, freqs.new);
+
+	PPCR = new_ppcr;
+
+	if (freqs.new < cur)
+		sa1100_update_dram_timings(cur, freqs.new);
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	return 0;
+}
+
+static int __init sa1100_cpu_init(struct cpufreq_policy *policy)
+{
+	if (policy->cpu != 0)
+		return -EINVAL;
+	policy->cur = policy->min = policy->max = sa11x0_getspeed(0);
+	policy->cpuinfo.min_freq = 59000;
+	policy->cpuinfo.max_freq = 287000;
+	policy->cpuinfo.transition_latency = CPUFREQ_ETERNAL;
+	return 0;
+}
+
+static struct cpufreq_driver sa1100_driver __refdata = {
+	.flags		= CPUFREQ_STICKY,
+	.verify		= sa11x0_verify_speed,
+	.target		= sa1100_target,
+	.get		= sa11x0_getspeed,
+	.init		= sa1100_cpu_init,
+	.name		= "sa1100",
+};
+
+static int __init sa1100_dram_init(void)
+{
+	if (cpu_is_sa1100())
+		return cpufreq_register_driver(&sa1100_driver);
+	else
+		return -ENODEV;
+}
+
+arch_initcall(sa1100_dram_init);
diff --git a/drivers/cpufreq/sa1110-cpufreq.c b/drivers/cpufreq/sa1110-cpufreq.c
new file mode 100644
index 000000000000..39c90b6f4286
--- /dev/null
+++ b/drivers/cpufreq/sa1110-cpufreq.c
@@ -0,0 +1,406 @@
+/*
+ *  linux/arch/arm/mach-sa1100/cpu-sa1110.c
+ *
+ *  Copyright (C) 2001 Russell King
+ *
+ * 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.
+ *
+ * Note: there are two erratas that apply to the SA1110 here:
+ *  7 - SDRAM auto-power-up failure (rev A0)
+ * 13 - Corruption of internal register reads/writes following
+ *      SDRAM reads (rev A0, B0, B1)
+ *
+ * We ignore rev. A0 and B0 devices; I don't think they're worth supporting.
+ *
+ * The SDRAM type can be passed on the command line as cpu_sa1110.sdram=type
+ */
+#include <linux/cpufreq.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/moduleparam.h>
+#include <linux/types.h>
+
+#include <asm/cputype.h>
+#include <asm/mach-types.h>
+
+#include <mach/generic.h>
+#include <mach/hardware.h>
+
+#undef DEBUG
+
+struct sdram_params {
+	const char name[20];
+	u_char  rows;		/* bits				 */
+	u_char  cas_latency;	/* cycles			 */
+	u_char  tck;		/* clock cycle time (ns)	 */
+	u_char  trcd;		/* activate to r/w (ns)		 */
+	u_char  trp;		/* precharge to activate (ns)	 */
+	u_char  twr;		/* write recovery time (ns)	 */
+	u_short refresh;	/* refresh time for array (us)	 */
+};
+
+struct sdram_info {
+	u_int	mdcnfg;
+	u_int	mdrefr;
+	u_int	mdcas[3];
+};
+
+static struct sdram_params sdram_tbl[] __initdata = {
+	{	/* Toshiba TC59SM716 CL2 */
+		.name		= "TC59SM716-CL2",
+		.rows		= 12,
+		.tck		= 10,
+		.trcd		= 20,
+		.trp		= 20,
+		.twr		= 10,
+		.refresh	= 64000,
+		.cas_latency	= 2,
+	}, {	/* Toshiba TC59SM716 CL3 */
+		.name		= "TC59SM716-CL3",
+		.rows		= 12,
+		.tck		= 8,
+		.trcd		= 20,
+		.trp		= 20,
+		.twr		= 8,
+		.refresh	= 64000,
+		.cas_latency	= 3,
+	}, {	/* Samsung K4S641632D TC75 */
+		.name		= "K4S641632D",
+		.rows		= 14,
+		.tck		= 9,
+		.trcd		= 27,
+		.trp		= 20,
+		.twr		= 9,
+		.refresh	= 64000,
+		.cas_latency	= 3,
+	}, {	/* Samsung K4S281632B-1H */
+		.name           = "K4S281632B-1H",
+		.rows		= 12,
+		.tck		= 10,
+		.trp		= 20,
+		.twr		= 10,
+		.refresh	= 64000,
+		.cas_latency	= 3,
+	}, {	/* Samsung KM416S4030CT */
+		.name		= "KM416S4030CT",
+		.rows		= 13,
+		.tck		= 8,
+		.trcd		= 24,	/* 3 CLKs */
+		.trp		= 24,	/* 3 CLKs */
+		.twr		= 16,	/* Trdl: 2 CLKs */
+		.refresh	= 64000,
+		.cas_latency	= 3,
+	}, {	/* Winbond W982516AH75L CL3 */
+		.name		= "W982516AH75L",
+		.rows		= 16,
+		.tck		= 8,
+		.trcd		= 20,
+		.trp		= 20,
+		.twr		= 8,
+		.refresh	= 64000,
+		.cas_latency	= 3,
+	}, {	/* Micron MT48LC8M16A2TG-75 */
+		.name		= "MT48LC8M16A2TG-75",
+		.rows		= 12,
+		.tck		= 8,
+		.trcd		= 20,
+		.trp		= 20,
+		.twr		= 8,
+		.refresh	= 64000,
+		.cas_latency	= 3,
+	},
+};
+
+static struct sdram_params sdram_params;
+
+/*
+ * Given a period in ns and frequency in khz, calculate the number of
+ * cycles of frequency in period.  Note that we round up to the next
+ * cycle, even if we are only slightly over.
+ */
+static inline u_int ns_to_cycles(u_int ns, u_int khz)
+{
+	return (ns * khz + 999999) / 1000000;
+}
+
+/*
+ * Create the MDCAS register bit pattern.
+ */
+static inline void set_mdcas(u_int *mdcas, int delayed, u_int rcd)
+{
+	u_int shift;
+
+	rcd = 2 * rcd - 1;
+	shift = delayed + 1 + rcd;
+
+	mdcas[0]  = (1 << rcd) - 1;
+	mdcas[0] |= 0x55555555 << shift;
+	mdcas[1]  = mdcas[2] = 0x55555555 << (shift & 1);
+}
+
+static void
+sdram_calculate_timing(struct sdram_info *sd, u_int cpu_khz,
+		       struct sdram_params *sdram)
+{
+	u_int mem_khz, sd_khz, trp, twr;
+
+	mem_khz = cpu_khz / 2;
+	sd_khz = mem_khz;
+
+	/*
+	 * If SDCLK would invalidate the SDRAM timings,
+	 * run SDCLK at half speed.
+	 *
+	 * CPU steppings prior to B2 must either run the memory at
+	 * half speed or use delayed read latching (errata 13).
+	 */
+	if ((ns_to_cycles(sdram->tck, sd_khz) > 1) ||
+	    (CPU_REVISION < CPU_SA1110_B2 && sd_khz < 62000))
+		sd_khz /= 2;
+
+	sd->mdcnfg = MDCNFG & 0x007f007f;
+
+	twr = ns_to_cycles(sdram->twr, mem_khz);
+
+	/* trp should always be >1 */
+	trp = ns_to_cycles(sdram->trp, mem_khz) - 1;
+	if (trp < 1)
+		trp = 1;
+
+	sd->mdcnfg |= trp << 8;
+	sd->mdcnfg |= trp << 24;
+	sd->mdcnfg |= sdram->cas_latency << 12;
+	sd->mdcnfg |= sdram->cas_latency << 28;
+	sd->mdcnfg |= twr << 14;
+	sd->mdcnfg |= twr << 30;
+
+	sd->mdrefr = MDREFR & 0xffbffff0;
+	sd->mdrefr |= 7;
+
+	if (sd_khz != mem_khz)
+		sd->mdrefr |= MDREFR_K1DB2;
+
+	/* initial number of '1's in MDCAS + 1 */
+	set_mdcas(sd->mdcas, sd_khz >= 62000,
+		ns_to_cycles(sdram->trcd, mem_khz));
+
+#ifdef DEBUG
+	printk(KERN_DEBUG "MDCNFG: %08x MDREFR: %08x MDCAS0: %08x MDCAS1: %08x MDCAS2: %08x\n",
+		sd->mdcnfg, sd->mdrefr, sd->mdcas[0], sd->mdcas[1],
+		sd->mdcas[2]);
+#endif
+}
+
+/*
+ * Set the SDRAM refresh rate.
+ */
+static inline void sdram_set_refresh(u_int dri)
+{
+	MDREFR = (MDREFR & 0xffff000f) | (dri << 4);
+	(void) MDREFR;
+}
+
+/*
+ * Update the refresh period.  We do this such that we always refresh
+ * the SDRAMs within their permissible period.  The refresh period is
+ * always a multiple of the memory clock (fixed at cpu_clock / 2).
+ *
+ * FIXME: we don't currently take account of burst accesses here,
+ * but neither do Intels DM nor Angel.
+ */
+static void
+sdram_update_refresh(u_int cpu_khz, struct sdram_params *sdram)
+{
+	u_int ns_row = (sdram->refresh * 1000) >> sdram->rows;
+	u_int dri = ns_to_cycles(ns_row, cpu_khz / 2) / 32;
+
+#ifdef DEBUG
+	mdelay(250);
+	printk(KERN_DEBUG "new dri value = %d\n", dri);
+#endif
+
+	sdram_set_refresh(dri);
+}
+
+/*
+ * Ok, set the CPU frequency.
+ */
+static int sa1110_target(struct cpufreq_policy *policy,
+			 unsigned int target_freq,
+			 unsigned int relation)
+{
+	struct sdram_params *sdram = &sdram_params;
+	struct cpufreq_freqs freqs;
+	struct sdram_info sd;
+	unsigned long flags;
+	unsigned int ppcr, unused;
+
+	switch (relation) {
+	case CPUFREQ_RELATION_L:
+		ppcr = sa11x0_freq_to_ppcr(target_freq);
+		if (sa11x0_ppcr_to_freq(ppcr) > policy->max)
+			ppcr--;
+		break;
+	case CPUFREQ_RELATION_H:
+		ppcr = sa11x0_freq_to_ppcr(target_freq);
+		if (ppcr && (sa11x0_ppcr_to_freq(ppcr) > target_freq) &&
+		    (sa11x0_ppcr_to_freq(ppcr-1) >= policy->min))
+			ppcr--;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	freqs.old = sa11x0_getspeed(0);
+	freqs.new = sa11x0_ppcr_to_freq(ppcr);
+
+	sdram_calculate_timing(&sd, freqs.new, sdram);
+
+#if 0
+	/*
+	 * These values are wrong according to the SA1110 documentation
+	 * and errata, but they seem to work.  Need to get a storage
+	 * scope on to the SDRAM signals to work out why.
+	 */
+	if (policy->max < 147500) {
+		sd.mdrefr |= MDREFR_K1DB2;
+		sd.mdcas[0] = 0xaaaaaa7f;
+	} else {
+		sd.mdrefr &= ~MDREFR_K1DB2;
+		sd.mdcas[0] = 0xaaaaaa9f;
+	}
+	sd.mdcas[1] = 0xaaaaaaaa;
+	sd.mdcas[2] = 0xaaaaaaaa;
+#endif
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+	/*
+	 * The clock could be going away for some time.  Set the SDRAMs
+	 * to refresh rapidly (every 64 memory clock cycles).  To get
+	 * through the whole array, we need to wait 262144 mclk cycles.
+	 * We wait 20ms to be safe.
+	 */
+	sdram_set_refresh(2);
+	if (!irqs_disabled())
+		msleep(20);
+	else
+		mdelay(20);
+
+	/*
+	 * Reprogram the DRAM timings with interrupts disabled, and
+	 * ensure that we are doing this within a complete cache line.
+	 * This means that we won't access SDRAM for the duration of
+	 * the programming.
+	 */
+	local_irq_save(flags);
+	asm("mcr p15, 0, %0, c7, c10, 4" : : "r" (0));
+	udelay(10);
+	__asm__ __volatile__("\n\
+		b	2f					\n\
+		.align	5					\n\
+1:		str	%3, [%1, #0]		@ MDCNFG	\n\
+		str	%4, [%1, #28]		@ MDREFR	\n\
+		str	%5, [%1, #4]		@ MDCAS0	\n\
+		str	%6, [%1, #8]		@ MDCAS1	\n\
+		str	%7, [%1, #12]		@ MDCAS2	\n\
+		str	%8, [%2, #0]		@ PPCR		\n\
+		ldr	%0, [%1, #0]				\n\
+		b	3f					\n\
+2:		b	1b					\n\
+3:		nop						\n\
+		nop"
+		: "=&r" (unused)
+		: "r" (&MDCNFG), "r" (&PPCR), "0" (sd.mdcnfg),
+		  "r" (sd.mdrefr), "r" (sd.mdcas[0]),
+		  "r" (sd.mdcas[1]), "r" (sd.mdcas[2]), "r" (ppcr));
+	local_irq_restore(flags);
+
+	/*
+	 * Now, return the SDRAM refresh back to normal.
+	 */
+	sdram_update_refresh(freqs.new, sdram);
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	return 0;
+}
+
+static int __init sa1110_cpu_init(struct cpufreq_policy *policy)
+{
+	if (policy->cpu != 0)
+		return -EINVAL;
+	policy->cur = policy->min = policy->max = sa11x0_getspeed(0);
+	policy->cpuinfo.min_freq = 59000;
+	policy->cpuinfo.max_freq = 287000;
+	policy->cpuinfo.transition_latency = CPUFREQ_ETERNAL;
+	return 0;
+}
+
+/* sa1110_driver needs __refdata because it must remain after init registers
+ * it with cpufreq_register_driver() */
+static struct cpufreq_driver sa1110_driver __refdata = {
+	.flags		= CPUFREQ_STICKY,
+	.verify		= sa11x0_verify_speed,
+	.target		= sa1110_target,
+	.get		= sa11x0_getspeed,
+	.init		= sa1110_cpu_init,
+	.name		= "sa1110",
+};
+
+static struct sdram_params *sa1110_find_sdram(const char *name)
+{
+	struct sdram_params *sdram;
+
+	for (sdram = sdram_tbl; sdram < sdram_tbl + ARRAY_SIZE(sdram_tbl);
+	     sdram++)
+		if (strcmp(name, sdram->name) == 0)
+			return sdram;
+
+	return NULL;
+}
+
+static char sdram_name[16];
+
+static int __init sa1110_clk_init(void)
+{
+	struct sdram_params *sdram;
+	const char *name = sdram_name;
+
+	if (!cpu_is_sa1110())
+		return -ENODEV;
+
+	if (!name[0]) {
+		if (machine_is_assabet())
+			name = "TC59SM716-CL3";
+		if (machine_is_pt_system3())
+			name = "K4S641632D";
+		if (machine_is_h3100())
+			name = "KM416S4030CT";
+		if (machine_is_jornada720())
+			name = "K4S281632B-1H";
+		if (machine_is_nanoengine())
+			name = "MT48LC8M16A2TG-75";
+	}
+
+	sdram = sa1110_find_sdram(name);
+	if (sdram) {
+		printk(KERN_DEBUG "SDRAM: tck: %d trcd: %d trp: %d"
+			" twr: %d refresh: %d cas_latency: %d\n",
+			sdram->tck, sdram->trcd, sdram->trp,
+			sdram->twr, sdram->refresh, sdram->cas_latency);
+
+		memcpy(&sdram_params, sdram, sizeof(sdram_params));
+
+		return cpufreq_register_driver(&sa1110_driver);
+	}
+
+	return 0;
+}
+
+module_param_string(sdram, sdram_name, sizeof(sdram_name), 0);
+arch_initcall(sa1110_clk_init);
diff --git a/drivers/cpufreq/sc520_freq.c b/drivers/cpufreq/sc520_freq.c
index e42e073cd9b8..f740b134d27b 100644
--- a/drivers/cpufreq/sc520_freq.c
+++ b/drivers/cpufreq/sc520_freq.c
@@ -53,7 +53,8 @@ static unsigned int sc520_freq_get_cpu_frequency(unsigned int cpu)
 	}
 }
 
-static void sc520_freq_set_cpu_state(unsigned int state)
+static void sc520_freq_set_cpu_state(struct cpufreq_policy *policy,
+		unsigned int state)
 {
 
 	struct cpufreq_freqs	freqs;
@@ -61,9 +62,8 @@ static void sc520_freq_set_cpu_state(unsigned int state)
 
 	freqs.old = sc520_freq_get_cpu_frequency(0);
 	freqs.new = sc520_freq_table[state].frequency;
-	freqs.cpu = 0; /* AMD Elan is UP */
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	pr_debug("attempting to set frequency to %i kHz\n",
 			sc520_freq_table[state].frequency);
@@ -75,7 +75,7 @@ static void sc520_freq_set_cpu_state(unsigned int state)
 
 	local_irq_enable();
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 };
 
 static int sc520_freq_verify(struct cpufreq_policy *policy)
@@ -93,7 +93,7 @@ static int sc520_freq_target(struct cpufreq_policy *policy,
 				target_freq, relation, &newstate))
 		return -EINVAL;
 
-	sc520_freq_set_cpu_state(newstate);
+	sc520_freq_set_cpu_state(policy, newstate);
 
 	return 0;
 }
diff --git a/drivers/cpufreq/sh-cpufreq.c b/drivers/cpufreq/sh-cpufreq.c
new file mode 100644
index 000000000000..73adb64651e8
--- /dev/null
+++ b/drivers/cpufreq/sh-cpufreq.c
@@ -0,0 +1,189 @@
+/*
+ * cpufreq driver for the SuperH processors.
+ *
+ * Copyright (C) 2002 - 2012 Paul Mundt
+ * Copyright (C) 2002 M. R. Brown
+ *
+ * Clock framework bits from arch/avr32/mach-at32ap/cpufreq.c
+ *
+ *   Copyright (C) 2004-2007 Atmel Corporation
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+#define pr_fmt(fmt) "cpufreq: " fmt
+
+#include <linux/types.h>
+#include <linux/cpufreq.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/cpumask.h>
+#include <linux/cpu.h>
+#include <linux/smp.h>
+#include <linux/sched.h>	/* set_cpus_allowed() */
+#include <linux/clk.h>
+#include <linux/percpu.h>
+#include <linux/sh_clk.h>
+
+static DEFINE_PER_CPU(struct clk, sh_cpuclk);
+
+static unsigned int sh_cpufreq_get(unsigned int cpu)
+{
+	return (clk_get_rate(&per_cpu(sh_cpuclk, cpu)) + 500) / 1000;
+}
+
+/*
+ * Here we notify other drivers of the proposed change and the final change.
+ */
+static int sh_cpufreq_target(struct cpufreq_policy *policy,
+			     unsigned int target_freq,
+			     unsigned int relation)
+{
+	unsigned int cpu = policy->cpu;
+	struct clk *cpuclk = &per_cpu(sh_cpuclk, cpu);
+	cpumask_t cpus_allowed;
+	struct cpufreq_freqs freqs;
+	struct device *dev;
+	long freq;
+
+	cpus_allowed = current->cpus_allowed;
+	set_cpus_allowed_ptr(current, cpumask_of(cpu));
+
+	BUG_ON(smp_processor_id() != cpu);
+
+	dev = get_cpu_device(cpu);
+
+	/* Convert target_freq from kHz to Hz */
+	freq = clk_round_rate(cpuclk, target_freq * 1000);
+
+	if (freq < (policy->min * 1000) || freq > (policy->max * 1000))
+		return -EINVAL;
+
+	dev_dbg(dev, "requested frequency %u Hz\n", target_freq * 1000);
+
+	freqs.old	= sh_cpufreq_get(cpu);
+	freqs.new	= (freq + 500) / 1000;
+	freqs.flags	= 0;
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+	set_cpus_allowed_ptr(current, &cpus_allowed);
+	clk_set_rate(cpuclk, freq);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	dev_dbg(dev, "set frequency %lu Hz\n", freq);
+
+	return 0;
+}
+
+static int sh_cpufreq_verify(struct cpufreq_policy *policy)
+{
+	struct clk *cpuclk = &per_cpu(sh_cpuclk, policy->cpu);
+	struct cpufreq_frequency_table *freq_table;
+
+	freq_table = cpuclk->nr_freqs ? cpuclk->freq_table : NULL;
+	if (freq_table)
+		return cpufreq_frequency_table_verify(policy, freq_table);
+
+	cpufreq_verify_within_limits(policy, policy->cpuinfo.min_freq,
+				     policy->cpuinfo.max_freq);
+
+	policy->min = (clk_round_rate(cpuclk, 1) + 500) / 1000;
+	policy->max = (clk_round_rate(cpuclk, ~0UL) + 500) / 1000;
+
+	cpufreq_verify_within_limits(policy, policy->cpuinfo.min_freq,
+				     policy->cpuinfo.max_freq);
+
+	return 0;
+}
+
+static int sh_cpufreq_cpu_init(struct cpufreq_policy *policy)
+{
+	unsigned int cpu = policy->cpu;
+	struct clk *cpuclk = &per_cpu(sh_cpuclk, cpu);
+	struct cpufreq_frequency_table *freq_table;
+	struct device *dev;
+
+	dev = get_cpu_device(cpu);
+
+	cpuclk = clk_get(dev, "cpu_clk");
+	if (IS_ERR(cpuclk)) {
+		dev_err(dev, "couldn't get CPU clk\n");
+		return PTR_ERR(cpuclk);
+	}
+
+	policy->cur = sh_cpufreq_get(cpu);
+
+	freq_table = cpuclk->nr_freqs ? cpuclk->freq_table : NULL;
+	if (freq_table) {
+		int result;
+
+		result = cpufreq_frequency_table_cpuinfo(policy, freq_table);
+		if (!result)
+			cpufreq_frequency_table_get_attr(freq_table, cpu);
+	} else {
+		dev_notice(dev, "no frequency table found, falling back "
+			   "to rate rounding.\n");
+
+		policy->min = policy->cpuinfo.min_freq =
+			(clk_round_rate(cpuclk, 1) + 500) / 1000;
+		policy->max = policy->cpuinfo.max_freq =
+			(clk_round_rate(cpuclk, ~0UL) + 500) / 1000;
+	}
+
+	policy->cpuinfo.transition_latency = CPUFREQ_ETERNAL;
+
+	dev_info(dev, "CPU Frequencies - Minimum %u.%03u MHz, "
+	       "Maximum %u.%03u MHz.\n",
+	       policy->min / 1000, policy->min % 1000,
+	       policy->max / 1000, policy->max % 1000);
+
+	return 0;
+}
+
+static int sh_cpufreq_cpu_exit(struct cpufreq_policy *policy)
+{
+	unsigned int cpu = policy->cpu;
+	struct clk *cpuclk = &per_cpu(sh_cpuclk, cpu);
+
+	cpufreq_frequency_table_put_attr(cpu);
+	clk_put(cpuclk);
+
+	return 0;
+}
+
+static struct freq_attr *sh_freq_attr[] = {
+	&cpufreq_freq_attr_scaling_available_freqs,
+	NULL,
+};
+
+static struct cpufreq_driver sh_cpufreq_driver = {
+	.owner		= THIS_MODULE,
+	.name		= "sh",
+	.get		= sh_cpufreq_get,
+	.target		= sh_cpufreq_target,
+	.verify		= sh_cpufreq_verify,
+	.init		= sh_cpufreq_cpu_init,
+	.exit		= sh_cpufreq_cpu_exit,
+	.attr		= sh_freq_attr,
+};
+
+static int __init sh_cpufreq_module_init(void)
+{
+	pr_notice("SuperH CPU frequency driver.\n");
+	return cpufreq_register_driver(&sh_cpufreq_driver);
+}
+
+static void __exit sh_cpufreq_module_exit(void)
+{
+	cpufreq_unregister_driver(&sh_cpufreq_driver);
+}
+
+module_init(sh_cpufreq_module_init);
+module_exit(sh_cpufreq_module_exit);
+
+MODULE_AUTHOR("Paul Mundt <lethal@linux-sh.org>");
+MODULE_DESCRIPTION("cpufreq driver for SuperH");
+MODULE_LICENSE("GPL");
diff --git a/drivers/cpufreq/sparc-us2e-cpufreq.c b/drivers/cpufreq/sparc-us2e-cpufreq.c
new file mode 100644
index 000000000000..306ae462bba6
--- /dev/null
+++ b/drivers/cpufreq/sparc-us2e-cpufreq.c
@@ -0,0 +1,408 @@
+/* us2e_cpufreq.c: UltraSPARC-IIe cpu frequency support
+ *
+ * Copyright (C) 2003 David S. Miller (davem@redhat.com)
+ *
+ * Many thanks to Dominik Brodowski for fixing up the cpufreq
+ * infrastructure in order to make this driver easier to implement.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/smp.h>
+#include <linux/cpufreq.h>
+#include <linux/threads.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+
+#include <asm/asi.h>
+#include <asm/timer.h>
+
+static struct cpufreq_driver *cpufreq_us2e_driver;
+
+struct us2e_freq_percpu_info {
+	struct cpufreq_frequency_table table[6];
+};
+
+/* Indexed by cpu number. */
+static struct us2e_freq_percpu_info *us2e_freq_table;
+
+#define HBIRD_MEM_CNTL0_ADDR	0x1fe0000f010UL
+#define HBIRD_ESTAR_MODE_ADDR	0x1fe0000f080UL
+
+/* UltraSPARC-IIe has five dividers: 1, 2, 4, 6, and 8.  These are controlled
+ * in the ESTAR mode control register.
+ */
+#define ESTAR_MODE_DIV_1	0x0000000000000000UL
+#define ESTAR_MODE_DIV_2	0x0000000000000001UL
+#define ESTAR_MODE_DIV_4	0x0000000000000003UL
+#define ESTAR_MODE_DIV_6	0x0000000000000002UL
+#define ESTAR_MODE_DIV_8	0x0000000000000004UL
+#define ESTAR_MODE_DIV_MASK	0x0000000000000007UL
+
+#define MCTRL0_SREFRESH_ENAB	0x0000000000010000UL
+#define MCTRL0_REFR_COUNT_MASK	0x0000000000007f00UL
+#define MCTRL0_REFR_COUNT_SHIFT	8
+#define MCTRL0_REFR_INTERVAL	7800
+#define MCTRL0_REFR_CLKS_P_CNT	64
+
+static unsigned long read_hbreg(unsigned long addr)
+{
+	unsigned long ret;
+
+	__asm__ __volatile__("ldxa	[%1] %2, %0"
+			     : "=&r" (ret)
+			     : "r" (addr), "i" (ASI_PHYS_BYPASS_EC_E));
+	return ret;
+}
+
+static void write_hbreg(unsigned long addr, unsigned long val)
+{
+	__asm__ __volatile__("stxa	%0, [%1] %2\n\t"
+			     "membar	#Sync"
+			     : /* no outputs */
+			     : "r" (val), "r" (addr), "i" (ASI_PHYS_BYPASS_EC_E)
+			     : "memory");
+	if (addr == HBIRD_ESTAR_MODE_ADDR) {
+		/* Need to wait 16 clock cycles for the PLL to lock.  */
+		udelay(1);
+	}
+}
+
+static void self_refresh_ctl(int enable)
+{
+	unsigned long mctrl = read_hbreg(HBIRD_MEM_CNTL0_ADDR);
+
+	if (enable)
+		mctrl |= MCTRL0_SREFRESH_ENAB;
+	else
+		mctrl &= ~MCTRL0_SREFRESH_ENAB;
+	write_hbreg(HBIRD_MEM_CNTL0_ADDR, mctrl);
+	(void) read_hbreg(HBIRD_MEM_CNTL0_ADDR);
+}
+
+static void frob_mem_refresh(int cpu_slowing_down,
+			     unsigned long clock_tick,
+			     unsigned long old_divisor, unsigned long divisor)
+{
+	unsigned long old_refr_count, refr_count, mctrl;
+
+	refr_count  = (clock_tick * MCTRL0_REFR_INTERVAL);
+	refr_count /= (MCTRL0_REFR_CLKS_P_CNT * divisor * 1000000000UL);
+
+	mctrl = read_hbreg(HBIRD_MEM_CNTL0_ADDR);
+	old_refr_count = (mctrl & MCTRL0_REFR_COUNT_MASK)
+		>> MCTRL0_REFR_COUNT_SHIFT;
+
+	mctrl &= ~MCTRL0_REFR_COUNT_MASK;
+	mctrl |= refr_count << MCTRL0_REFR_COUNT_SHIFT;
+	write_hbreg(HBIRD_MEM_CNTL0_ADDR, mctrl);
+	mctrl = read_hbreg(HBIRD_MEM_CNTL0_ADDR);
+
+	if (cpu_slowing_down && !(mctrl & MCTRL0_SREFRESH_ENAB)) {
+		unsigned long usecs;
+
+		/* We have to wait for both refresh counts (old
+		 * and new) to go to zero.
+		 */
+		usecs = (MCTRL0_REFR_CLKS_P_CNT *
+			 (refr_count + old_refr_count) *
+			 1000000UL *
+			 old_divisor) / clock_tick;
+		udelay(usecs + 1UL);
+	}
+}
+
+static void us2e_transition(unsigned long estar, unsigned long new_bits,
+			    unsigned long clock_tick,
+			    unsigned long old_divisor, unsigned long divisor)
+{
+	unsigned long flags;
+
+	local_irq_save(flags);
+
+	estar &= ~ESTAR_MODE_DIV_MASK;
+
+	/* This is based upon the state transition diagram in the IIe manual.  */
+	if (old_divisor == 2 && divisor == 1) {
+		self_refresh_ctl(0);
+		write_hbreg(HBIRD_ESTAR_MODE_ADDR, estar | new_bits);
+		frob_mem_refresh(0, clock_tick, old_divisor, divisor);
+	} else if (old_divisor == 1 && divisor == 2) {
+		frob_mem_refresh(1, clock_tick, old_divisor, divisor);
+		write_hbreg(HBIRD_ESTAR_MODE_ADDR, estar | new_bits);
+		self_refresh_ctl(1);
+	} else if (old_divisor == 1 && divisor > 2) {
+		us2e_transition(estar, ESTAR_MODE_DIV_2, clock_tick,
+				1, 2);
+		us2e_transition(estar, new_bits, clock_tick,
+				2, divisor);
+	} else if (old_divisor > 2 && divisor == 1) {
+		us2e_transition(estar, ESTAR_MODE_DIV_2, clock_tick,
+				old_divisor, 2);
+		us2e_transition(estar, new_bits, clock_tick,
+				2, divisor);
+	} else if (old_divisor < divisor) {
+		frob_mem_refresh(0, clock_tick, old_divisor, divisor);
+		write_hbreg(HBIRD_ESTAR_MODE_ADDR, estar | new_bits);
+	} else if (old_divisor > divisor) {
+		write_hbreg(HBIRD_ESTAR_MODE_ADDR, estar | new_bits);
+		frob_mem_refresh(1, clock_tick, old_divisor, divisor);
+	} else {
+		BUG();
+	}
+
+	local_irq_restore(flags);
+}
+
+static unsigned long index_to_estar_mode(unsigned int index)
+{
+	switch (index) {
+	case 0:
+		return ESTAR_MODE_DIV_1;
+
+	case 1:
+		return ESTAR_MODE_DIV_2;
+
+	case 2:
+		return ESTAR_MODE_DIV_4;
+
+	case 3:
+		return ESTAR_MODE_DIV_6;
+
+	case 4:
+		return ESTAR_MODE_DIV_8;
+
+	default:
+		BUG();
+	}
+}
+
+static unsigned long index_to_divisor(unsigned int index)
+{
+	switch (index) {
+	case 0:
+		return 1;
+
+	case 1:
+		return 2;
+
+	case 2:
+		return 4;
+
+	case 3:
+		return 6;
+
+	case 4:
+		return 8;
+
+	default:
+		BUG();
+	}
+}
+
+static unsigned long estar_to_divisor(unsigned long estar)
+{
+	unsigned long ret;
+
+	switch (estar & ESTAR_MODE_DIV_MASK) {
+	case ESTAR_MODE_DIV_1:
+		ret = 1;
+		break;
+	case ESTAR_MODE_DIV_2:
+		ret = 2;
+		break;
+	case ESTAR_MODE_DIV_4:
+		ret = 4;
+		break;
+	case ESTAR_MODE_DIV_6:
+		ret = 6;
+		break;
+	case ESTAR_MODE_DIV_8:
+		ret = 8;
+		break;
+	default:
+		BUG();
+	}
+
+	return ret;
+}
+
+static unsigned int us2e_freq_get(unsigned int cpu)
+{
+	cpumask_t cpus_allowed;
+	unsigned long clock_tick, estar;
+
+	cpumask_copy(&cpus_allowed, tsk_cpus_allowed(current));
+	set_cpus_allowed_ptr(current, cpumask_of(cpu));
+
+	clock_tick = sparc64_get_clock_tick(cpu) / 1000;
+	estar = read_hbreg(HBIRD_ESTAR_MODE_ADDR);
+
+	set_cpus_allowed_ptr(current, &cpus_allowed);
+
+	return clock_tick / estar_to_divisor(estar);
+}
+
+static void us2e_set_cpu_divider_index(struct cpufreq_policy *policy,
+		unsigned int index)
+{
+	unsigned int cpu = policy->cpu;
+	unsigned long new_bits, new_freq;
+	unsigned long clock_tick, divisor, old_divisor, estar;
+	cpumask_t cpus_allowed;
+	struct cpufreq_freqs freqs;
+
+	cpumask_copy(&cpus_allowed, tsk_cpus_allowed(current));
+	set_cpus_allowed_ptr(current, cpumask_of(cpu));
+
+	new_freq = clock_tick = sparc64_get_clock_tick(cpu) / 1000;
+	new_bits = index_to_estar_mode(index);
+	divisor = index_to_divisor(index);
+	new_freq /= divisor;
+
+	estar = read_hbreg(HBIRD_ESTAR_MODE_ADDR);
+
+	old_divisor = estar_to_divisor(estar);
+
+	freqs.old = clock_tick / old_divisor;
+	freqs.new = new_freq;
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+	if (old_divisor != divisor)
+		us2e_transition(estar, new_bits, clock_tick * 1000,
+				old_divisor, divisor);
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	set_cpus_allowed_ptr(current, &cpus_allowed);
+}
+
+static int us2e_freq_target(struct cpufreq_policy *policy,
+			  unsigned int target_freq,
+			  unsigned int relation)
+{
+	unsigned int new_index = 0;
+
+	if (cpufreq_frequency_table_target(policy,
+					   &us2e_freq_table[policy->cpu].table[0],
+					   target_freq, relation, &new_index))
+		return -EINVAL;
+
+	us2e_set_cpu_divider_index(policy, new_index);
+
+	return 0;
+}
+
+static int us2e_freq_verify(struct cpufreq_policy *policy)
+{
+	return cpufreq_frequency_table_verify(policy,
+					      &us2e_freq_table[policy->cpu].table[0]);
+}
+
+static int __init us2e_freq_cpu_init(struct cpufreq_policy *policy)
+{
+	unsigned int cpu = policy->cpu;
+	unsigned long clock_tick = sparc64_get_clock_tick(cpu) / 1000;
+	struct cpufreq_frequency_table *table =
+		&us2e_freq_table[cpu].table[0];
+
+	table[0].index = 0;
+	table[0].frequency = clock_tick / 1;
+	table[1].index = 1;
+	table[1].frequency = clock_tick / 2;
+	table[2].index = 2;
+	table[2].frequency = clock_tick / 4;
+	table[2].index = 3;
+	table[2].frequency = clock_tick / 6;
+	table[2].index = 4;
+	table[2].frequency = clock_tick / 8;
+	table[2].index = 5;
+	table[3].frequency = CPUFREQ_TABLE_END;
+
+	policy->cpuinfo.transition_latency = 0;
+	policy->cur = clock_tick;
+
+	return cpufreq_frequency_table_cpuinfo(policy, table);
+}
+
+static int us2e_freq_cpu_exit(struct cpufreq_policy *policy)
+{
+	if (cpufreq_us2e_driver)
+		us2e_set_cpu_divider_index(policy, 0);
+
+	return 0;
+}
+
+static int __init us2e_freq_init(void)
+{
+	unsigned long manuf, impl, ver;
+	int ret;
+
+	if (tlb_type != spitfire)
+		return -ENODEV;
+
+	__asm__("rdpr %%ver, %0" : "=r" (ver));
+	manuf = ((ver >> 48) & 0xffff);
+	impl  = ((ver >> 32) & 0xffff);
+
+	if (manuf == 0x17 && impl == 0x13) {
+		struct cpufreq_driver *driver;
+
+		ret = -ENOMEM;
+		driver = kzalloc(sizeof(struct cpufreq_driver), GFP_KERNEL);
+		if (!driver)
+			goto err_out;
+
+		us2e_freq_table = kzalloc(
+			(NR_CPUS * sizeof(struct us2e_freq_percpu_info)),
+			GFP_KERNEL);
+		if (!us2e_freq_table)
+			goto err_out;
+
+		driver->init = us2e_freq_cpu_init;
+		driver->verify = us2e_freq_verify;
+		driver->target = us2e_freq_target;
+		driver->get = us2e_freq_get;
+		driver->exit = us2e_freq_cpu_exit;
+		driver->owner = THIS_MODULE,
+		strcpy(driver->name, "UltraSPARC-IIe");
+
+		cpufreq_us2e_driver = driver;
+		ret = cpufreq_register_driver(driver);
+		if (ret)
+			goto err_out;
+
+		return 0;
+
+err_out:
+		if (driver) {
+			kfree(driver);
+			cpufreq_us2e_driver = NULL;
+		}
+		kfree(us2e_freq_table);
+		us2e_freq_table = NULL;
+		return ret;
+	}
+
+	return -ENODEV;
+}
+
+static void __exit us2e_freq_exit(void)
+{
+	if (cpufreq_us2e_driver) {
+		cpufreq_unregister_driver(cpufreq_us2e_driver);
+		kfree(cpufreq_us2e_driver);
+		cpufreq_us2e_driver = NULL;
+		kfree(us2e_freq_table);
+		us2e_freq_table = NULL;
+	}
+}
+
+MODULE_AUTHOR("David S. Miller <davem@redhat.com>");
+MODULE_DESCRIPTION("cpufreq driver for UltraSPARC-IIe");
+MODULE_LICENSE("GPL");
+
+module_init(us2e_freq_init);
+module_exit(us2e_freq_exit);
diff --git a/drivers/cpufreq/sparc-us3-cpufreq.c b/drivers/cpufreq/sparc-us3-cpufreq.c
new file mode 100644
index 000000000000..c71ee142347a
--- /dev/null
+++ b/drivers/cpufreq/sparc-us3-cpufreq.c
@@ -0,0 +1,269 @@
+/* us3_cpufreq.c: UltraSPARC-III cpu frequency support
+ *
+ * Copyright (C) 2003 David S. Miller (davem@redhat.com)
+ *
+ * Many thanks to Dominik Brodowski for fixing up the cpufreq
+ * infrastructure in order to make this driver easier to implement.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/smp.h>
+#include <linux/cpufreq.h>
+#include <linux/threads.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+
+#include <asm/head.h>
+#include <asm/timer.h>
+
+static struct cpufreq_driver *cpufreq_us3_driver;
+
+struct us3_freq_percpu_info {
+	struct cpufreq_frequency_table table[4];
+};
+
+/* Indexed by cpu number. */
+static struct us3_freq_percpu_info *us3_freq_table;
+
+/* UltraSPARC-III has three dividers: 1, 2, and 32.  These are controlled
+ * in the Safari config register.
+ */
+#define SAFARI_CFG_DIV_1	0x0000000000000000UL
+#define SAFARI_CFG_DIV_2	0x0000000040000000UL
+#define SAFARI_CFG_DIV_32	0x0000000080000000UL
+#define SAFARI_CFG_DIV_MASK	0x00000000C0000000UL
+
+static unsigned long read_safari_cfg(void)
+{
+	unsigned long ret;
+
+	__asm__ __volatile__("ldxa	[%%g0] %1, %0"
+			     : "=&r" (ret)
+			     : "i" (ASI_SAFARI_CONFIG));
+	return ret;
+}
+
+static void write_safari_cfg(unsigned long val)
+{
+	__asm__ __volatile__("stxa	%0, [%%g0] %1\n\t"
+			     "membar	#Sync"
+			     : /* no outputs */
+			     : "r" (val), "i" (ASI_SAFARI_CONFIG)
+			     : "memory");
+}
+
+static unsigned long get_current_freq(unsigned int cpu, unsigned long safari_cfg)
+{
+	unsigned long clock_tick = sparc64_get_clock_tick(cpu) / 1000;
+	unsigned long ret;
+
+	switch (safari_cfg & SAFARI_CFG_DIV_MASK) {
+	case SAFARI_CFG_DIV_1:
+		ret = clock_tick / 1;
+		break;
+	case SAFARI_CFG_DIV_2:
+		ret = clock_tick / 2;
+		break;
+	case SAFARI_CFG_DIV_32:
+		ret = clock_tick / 32;
+		break;
+	default:
+		BUG();
+	}
+
+	return ret;
+}
+
+static unsigned int us3_freq_get(unsigned int cpu)
+{
+	cpumask_t cpus_allowed;
+	unsigned long reg;
+	unsigned int ret;
+
+	cpumask_copy(&cpus_allowed, tsk_cpus_allowed(current));
+	set_cpus_allowed_ptr(current, cpumask_of(cpu));
+
+	reg = read_safari_cfg();
+	ret = get_current_freq(cpu, reg);
+
+	set_cpus_allowed_ptr(current, &cpus_allowed);
+
+	return ret;
+}
+
+static void us3_set_cpu_divider_index(struct cpufreq_policy *policy,
+		unsigned int index)
+{
+	unsigned int cpu = policy->cpu;
+	unsigned long new_bits, new_freq, reg;
+	cpumask_t cpus_allowed;
+	struct cpufreq_freqs freqs;
+
+	cpumask_copy(&cpus_allowed, tsk_cpus_allowed(current));
+	set_cpus_allowed_ptr(current, cpumask_of(cpu));
+
+	new_freq = sparc64_get_clock_tick(cpu) / 1000;
+	switch (index) {
+	case 0:
+		new_bits = SAFARI_CFG_DIV_1;
+		new_freq /= 1;
+		break;
+	case 1:
+		new_bits = SAFARI_CFG_DIV_2;
+		new_freq /= 2;
+		break;
+	case 2:
+		new_bits = SAFARI_CFG_DIV_32;
+		new_freq /= 32;
+		break;
+
+	default:
+		BUG();
+	}
+
+	reg = read_safari_cfg();
+
+	freqs.old = get_current_freq(cpu, reg);
+	freqs.new = new_freq;
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+	reg &= ~SAFARI_CFG_DIV_MASK;
+	reg |= new_bits;
+	write_safari_cfg(reg);
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	set_cpus_allowed_ptr(current, &cpus_allowed);
+}
+
+static int us3_freq_target(struct cpufreq_policy *policy,
+			  unsigned int target_freq,
+			  unsigned int relation)
+{
+	unsigned int new_index = 0;
+
+	if (cpufreq_frequency_table_target(policy,
+					   &us3_freq_table[policy->cpu].table[0],
+					   target_freq,
+					   relation,
+					   &new_index))
+		return -EINVAL;
+
+	us3_set_cpu_divider_index(policy, new_index);
+
+	return 0;
+}
+
+static int us3_freq_verify(struct cpufreq_policy *policy)
+{
+	return cpufreq_frequency_table_verify(policy,
+					      &us3_freq_table[policy->cpu].table[0]);
+}
+
+static int __init us3_freq_cpu_init(struct cpufreq_policy *policy)
+{
+	unsigned int cpu = policy->cpu;
+	unsigned long clock_tick = sparc64_get_clock_tick(cpu) / 1000;
+	struct cpufreq_frequency_table *table =
+		&us3_freq_table[cpu].table[0];
+
+	table[0].index = 0;
+	table[0].frequency = clock_tick / 1;
+	table[1].index = 1;
+	table[1].frequency = clock_tick / 2;
+	table[2].index = 2;
+	table[2].frequency = clock_tick / 32;
+	table[3].index = 0;
+	table[3].frequency = CPUFREQ_TABLE_END;
+
+	policy->cpuinfo.transition_latency = 0;
+	policy->cur = clock_tick;
+
+	return cpufreq_frequency_table_cpuinfo(policy, table);
+}
+
+static int us3_freq_cpu_exit(struct cpufreq_policy *policy)
+{
+	if (cpufreq_us3_driver)
+		us3_set_cpu_divider_index(policy, 0);
+
+	return 0;
+}
+
+static int __init us3_freq_init(void)
+{
+	unsigned long manuf, impl, ver;
+	int ret;
+
+	if (tlb_type != cheetah && tlb_type != cheetah_plus)
+		return -ENODEV;
+
+	__asm__("rdpr %%ver, %0" : "=r" (ver));
+	manuf = ((ver >> 48) & 0xffff);
+	impl  = ((ver >> 32) & 0xffff);
+
+	if (manuf == CHEETAH_MANUF &&
+	    (impl == CHEETAH_IMPL ||
+	     impl == CHEETAH_PLUS_IMPL ||
+	     impl == JAGUAR_IMPL ||
+	     impl == PANTHER_IMPL)) {
+		struct cpufreq_driver *driver;
+
+		ret = -ENOMEM;
+		driver = kzalloc(sizeof(struct cpufreq_driver), GFP_KERNEL);
+		if (!driver)
+			goto err_out;
+
+		us3_freq_table = kzalloc(
+			(NR_CPUS * sizeof(struct us3_freq_percpu_info)),
+			GFP_KERNEL);
+		if (!us3_freq_table)
+			goto err_out;
+
+		driver->init = us3_freq_cpu_init;
+		driver->verify = us3_freq_verify;
+		driver->target = us3_freq_target;
+		driver->get = us3_freq_get;
+		driver->exit = us3_freq_cpu_exit;
+		driver->owner = THIS_MODULE,
+		strcpy(driver->name, "UltraSPARC-III");
+
+		cpufreq_us3_driver = driver;
+		ret = cpufreq_register_driver(driver);
+		if (ret)
+			goto err_out;
+
+		return 0;
+
+err_out:
+		if (driver) {
+			kfree(driver);
+			cpufreq_us3_driver = NULL;
+		}
+		kfree(us3_freq_table);
+		us3_freq_table = NULL;
+		return ret;
+	}
+
+	return -ENODEV;
+}
+
+static void __exit us3_freq_exit(void)
+{
+	if (cpufreq_us3_driver) {
+		cpufreq_unregister_driver(cpufreq_us3_driver);
+		kfree(cpufreq_us3_driver);
+		cpufreq_us3_driver = NULL;
+		kfree(us3_freq_table);
+		us3_freq_table = NULL;
+	}
+}
+
+MODULE_AUTHOR("David S. Miller <davem@redhat.com>");
+MODULE_DESCRIPTION("cpufreq driver for UltraSPARC-III");
+MODULE_LICENSE("GPL");
+
+module_init(us3_freq_init);
+module_exit(us3_freq_exit);
diff --git a/drivers/cpufreq/spear-cpufreq.c b/drivers/cpufreq/spear-cpufreq.c
index 7e4d77327957..156829f4576d 100644
--- a/drivers/cpufreq/spear-cpufreq.c
+++ b/drivers/cpufreq/spear-cpufreq.c
@@ -121,7 +121,6 @@ static int spear_cpufreq_target(struct cpufreq_policy *policy,
 				target_freq, relation, &index))
 		return -EINVAL;
 
-	freqs.cpu = policy->cpu;
 	freqs.old = spear_cpufreq_get(0);
 
 	newfreq = spear_cpufreq.freq_tbl[index].frequency * 1000;
@@ -158,8 +157,7 @@ static int spear_cpufreq_target(struct cpufreq_policy *policy,
 	freqs.new = newfreq / 1000;
 	freqs.new /= mult;
 
-	for_each_cpu(freqs.cpu, policy->cpus)
-		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	if (mult == 2)
 		ret = spear1340_set_cpu_rate(srcclk, newfreq);
@@ -172,8 +170,7 @@ static int spear_cpufreq_target(struct cpufreq_policy *policy,
 		freqs.new = clk_get_rate(spear_cpufreq.clk) / 1000;
 	}
 
-	for_each_cpu(freqs.cpu, policy->cpus)
-		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 	return ret;
 }
 
diff --git a/drivers/cpufreq/speedstep-centrino.c b/drivers/cpufreq/speedstep-centrino.c
index 3a953d519f46..618e6f417b1c 100644
--- a/drivers/cpufreq/speedstep-centrino.c
+++ b/drivers/cpufreq/speedstep-centrino.c
@@ -457,7 +457,7 @@ static int centrino_target (struct cpufreq_policy *policy,
 	unsigned int	msr, oldmsr = 0, h = 0, cpu = policy->cpu;
 	struct cpufreq_freqs	freqs;
 	int			retval = 0;
-	unsigned int		j, k, first_cpu, tmp;
+	unsigned int		j, first_cpu, tmp;
 	cpumask_var_t covered_cpus;
 
 	if (unlikely(!zalloc_cpumask_var(&covered_cpus, GFP_KERNEL)))
@@ -481,10 +481,6 @@ static int centrino_target (struct cpufreq_policy *policy,
 	for_each_cpu(j, policy->cpus) {
 		int good_cpu;
 
-		/* cpufreq holds the hotplug lock, so we are safe here */
-		if (!cpu_online(j))
-			continue;
-
 		/*
 		 * Support for SMP systems.
 		 * Make sure we are running on CPU that wants to change freq
@@ -522,13 +518,8 @@ static int centrino_target (struct cpufreq_policy *policy,
 			pr_debug("target=%dkHz old=%d new=%d msr=%04x\n",
 				target_freq, freqs.old, freqs.new, msr);
 
-			for_each_cpu(k, policy->cpus) {
-				if (!cpu_online(k))
-					continue;
-				freqs.cpu = k;
-				cpufreq_notify_transition(&freqs,
+			cpufreq_notify_transition(policy, &freqs,
 					CPUFREQ_PRECHANGE);
-			}
 
 			first_cpu = 0;
 			/* all but 16 LSB are reserved, treat them with care */
@@ -544,12 +535,7 @@ static int centrino_target (struct cpufreq_policy *policy,
 		cpumask_set_cpu(j, covered_cpus);
 	}
 
-	for_each_cpu(k, policy->cpus) {
-		if (!cpu_online(k))
-			continue;
-		freqs.cpu = k;
-		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
-	}
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 	if (unlikely(retval)) {
 		/*
@@ -565,12 +551,8 @@ static int centrino_target (struct cpufreq_policy *policy,
 		tmp = freqs.new;
 		freqs.new = freqs.old;
 		freqs.old = tmp;
-		for_each_cpu(j, policy->cpus) {
-			if (!cpu_online(j))
-				continue;
-			cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
-			cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
-		}
+		cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+		cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 	}
 	retval = 0;
 
diff --git a/drivers/cpufreq/speedstep-ich.c b/drivers/cpufreq/speedstep-ich.c
index e29b59aa68a8..e2e5aa971452 100644
--- a/drivers/cpufreq/speedstep-ich.c
+++ b/drivers/cpufreq/speedstep-ich.c
@@ -263,7 +263,6 @@ static int speedstep_target(struct cpufreq_policy *policy,
 {
 	unsigned int newstate = 0, policy_cpu;
 	struct cpufreq_freqs freqs;
-	int i;
 
 	if (cpufreq_frequency_table_target(policy, &speedstep_freqs[0],
 				target_freq, relation, &newstate))
@@ -272,7 +271,6 @@ static int speedstep_target(struct cpufreq_policy *policy,
 	policy_cpu = cpumask_any_and(policy->cpus, cpu_online_mask);
 	freqs.old = speedstep_get(policy_cpu);
 	freqs.new = speedstep_freqs[newstate].frequency;
-	freqs.cpu = policy->cpu;
 
 	pr_debug("transiting from %u to %u kHz\n", freqs.old, freqs.new);
 
@@ -280,18 +278,12 @@ static int speedstep_target(struct cpufreq_policy *policy,
 	if (freqs.old == freqs.new)
 		return 0;
 
-	for_each_cpu(i, policy->cpus) {
-		freqs.cpu = i;
-		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
-	}
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 
 	smp_call_function_single(policy_cpu, _speedstep_set_state, &newstate,
 				 true);
 
-	for_each_cpu(i, policy->cpus) {
-		freqs.cpu = i;
-		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
-	}
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 	return 0;
 }
diff --git a/drivers/cpufreq/speedstep-smi.c b/drivers/cpufreq/speedstep-smi.c
index 6a457fcaaad5..f5a6b70ee6c0 100644
--- a/drivers/cpufreq/speedstep-smi.c
+++ b/drivers/cpufreq/speedstep-smi.c
@@ -252,14 +252,13 @@ static int speedstep_target(struct cpufreq_policy *policy,
 
 	freqs.old = speedstep_freqs[speedstep_get_state()].frequency;
 	freqs.new = speedstep_freqs[newstate].frequency;
-	freqs.cpu = 0; /* speedstep.c is UP only driver */
 
 	if (freqs.old == freqs.new)
 		return 0;
 
-	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
 	speedstep_set_state(newstate);
-	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
 
 	return 0;
 }
diff --git a/drivers/cpufreq/tegra-cpufreq.c b/drivers/cpufreq/tegra-cpufreq.c
new file mode 100644
index 000000000000..c74c0e130ef4
--- /dev/null
+++ b/drivers/cpufreq/tegra-cpufreq.c
@@ -0,0 +1,292 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * Author:
+ *	Colin Cross <ccross@google.com>
+ *	Based on arch/arm/plat-omap/cpu-omap.c, (C) 2005 Nokia Corporation
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/sched.h>
+#include <linux/cpufreq.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/suspend.h>
+
+/* Frequency table index must be sequential starting at 0 */
+static struct cpufreq_frequency_table freq_table[] = {
+	{ 0, 216000 },
+	{ 1, 312000 },
+	{ 2, 456000 },
+	{ 3, 608000 },
+	{ 4, 760000 },
+	{ 5, 816000 },
+	{ 6, 912000 },
+	{ 7, 1000000 },
+	{ 8, CPUFREQ_TABLE_END },
+};
+
+#define NUM_CPUS	2
+
+static struct clk *cpu_clk;
+static struct clk *pll_x_clk;
+static struct clk *pll_p_clk;
+static struct clk *emc_clk;
+
+static unsigned long target_cpu_speed[NUM_CPUS];
+static DEFINE_MUTEX(tegra_cpu_lock);
+static bool is_suspended;
+
+static int tegra_verify_speed(struct cpufreq_policy *policy)
+{
+	return cpufreq_frequency_table_verify(policy, freq_table);
+}
+
+static unsigned int tegra_getspeed(unsigned int cpu)
+{
+	unsigned long rate;
+
+	if (cpu >= NUM_CPUS)
+		return 0;
+
+	rate = clk_get_rate(cpu_clk) / 1000;
+	return rate;
+}
+
+static int tegra_cpu_clk_set_rate(unsigned long rate)
+{
+	int ret;
+
+	/*
+	 * Take an extra reference to the main pll so it doesn't turn
+	 * off when we move the cpu off of it
+	 */
+	clk_prepare_enable(pll_x_clk);
+
+	ret = clk_set_parent(cpu_clk, pll_p_clk);
+	if (ret) {
+		pr_err("Failed to switch cpu to clock pll_p\n");
+		goto out;
+	}
+
+	if (rate == clk_get_rate(pll_p_clk))
+		goto out;
+
+	ret = clk_set_rate(pll_x_clk, rate);
+	if (ret) {
+		pr_err("Failed to change pll_x to %lu\n", rate);
+		goto out;
+	}
+
+	ret = clk_set_parent(cpu_clk, pll_x_clk);
+	if (ret) {
+		pr_err("Failed to switch cpu to clock pll_x\n");
+		goto out;
+	}
+
+out:
+	clk_disable_unprepare(pll_x_clk);
+	return ret;
+}
+
+static int tegra_update_cpu_speed(struct cpufreq_policy *policy,
+		unsigned long rate)
+{
+	int ret = 0;
+	struct cpufreq_freqs freqs;
+
+	freqs.old = tegra_getspeed(0);
+	freqs.new = rate;
+
+	if (freqs.old == freqs.new)
+		return ret;
+
+	/*
+	 * Vote on memory bus frequency based on cpu frequency
+	 * This sets the minimum frequency, display or avp may request higher
+	 */
+	if (rate >= 816000)
+		clk_set_rate(emc_clk, 600000000); /* cpu 816 MHz, emc max */
+	else if (rate >= 456000)
+		clk_set_rate(emc_clk, 300000000); /* cpu 456 MHz, emc 150Mhz */
+	else
+		clk_set_rate(emc_clk, 100000000);  /* emc 50Mhz */
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+#ifdef CONFIG_CPU_FREQ_DEBUG
+	printk(KERN_DEBUG "cpufreq-tegra: transition: %u --> %u\n",
+	       freqs.old, freqs.new);
+#endif
+
+	ret = tegra_cpu_clk_set_rate(freqs.new * 1000);
+	if (ret) {
+		pr_err("cpu-tegra: Failed to set cpu frequency to %d kHz\n",
+			freqs.new);
+		return ret;
+	}
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	return 0;
+}
+
+static unsigned long tegra_cpu_highest_speed(void)
+{
+	unsigned long rate = 0;
+	int i;
+
+	for_each_online_cpu(i)
+		rate = max(rate, target_cpu_speed[i]);
+	return rate;
+}
+
+static int tegra_target(struct cpufreq_policy *policy,
+		       unsigned int target_freq,
+		       unsigned int relation)
+{
+	unsigned int idx;
+	unsigned int freq;
+	int ret = 0;
+
+	mutex_lock(&tegra_cpu_lock);
+
+	if (is_suspended) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	cpufreq_frequency_table_target(policy, freq_table, target_freq,
+		relation, &idx);
+
+	freq = freq_table[idx].frequency;
+
+	target_cpu_speed[policy->cpu] = freq;
+
+	ret = tegra_update_cpu_speed(policy, tegra_cpu_highest_speed());
+
+out:
+	mutex_unlock(&tegra_cpu_lock);
+	return ret;
+}
+
+static int tegra_pm_notify(struct notifier_block *nb, unsigned long event,
+	void *dummy)
+{
+	mutex_lock(&tegra_cpu_lock);
+	if (event == PM_SUSPEND_PREPARE) {
+		struct cpufreq_policy *policy = cpufreq_cpu_get(0);
+		is_suspended = true;
+		pr_info("Tegra cpufreq suspend: setting frequency to %d kHz\n",
+			freq_table[0].frequency);
+		tegra_update_cpu_speed(policy, freq_table[0].frequency);
+		cpufreq_cpu_put(policy);
+	} else if (event == PM_POST_SUSPEND) {
+		is_suspended = false;
+	}
+	mutex_unlock(&tegra_cpu_lock);
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block tegra_cpu_pm_notifier = {
+	.notifier_call = tegra_pm_notify,
+};
+
+static int tegra_cpu_init(struct cpufreq_policy *policy)
+{
+	if (policy->cpu >= NUM_CPUS)
+		return -EINVAL;
+
+	clk_prepare_enable(emc_clk);
+	clk_prepare_enable(cpu_clk);
+
+	cpufreq_frequency_table_cpuinfo(policy, freq_table);
+	cpufreq_frequency_table_get_attr(freq_table, policy->cpu);
+	policy->cur = tegra_getspeed(policy->cpu);
+	target_cpu_speed[policy->cpu] = policy->cur;
+
+	/* FIXME: what's the actual transition time? */
+	policy->cpuinfo.transition_latency = 300 * 1000;
+
+	cpumask_copy(policy->cpus, cpu_possible_mask);
+
+	if (policy->cpu == 0)
+		register_pm_notifier(&tegra_cpu_pm_notifier);
+
+	return 0;
+}
+
+static int tegra_cpu_exit(struct cpufreq_policy *policy)
+{
+	cpufreq_frequency_table_cpuinfo(policy, freq_table);
+	clk_disable_unprepare(emc_clk);
+	return 0;
+}
+
+static struct freq_attr *tegra_cpufreq_attr[] = {
+	&cpufreq_freq_attr_scaling_available_freqs,
+	NULL,
+};
+
+static struct cpufreq_driver tegra_cpufreq_driver = {
+	.verify		= tegra_verify_speed,
+	.target		= tegra_target,
+	.get		= tegra_getspeed,
+	.init		= tegra_cpu_init,
+	.exit		= tegra_cpu_exit,
+	.name		= "tegra",
+	.attr		= tegra_cpufreq_attr,
+};
+
+static int __init tegra_cpufreq_init(void)
+{
+	cpu_clk = clk_get_sys(NULL, "cpu");
+	if (IS_ERR(cpu_clk))
+		return PTR_ERR(cpu_clk);
+
+	pll_x_clk = clk_get_sys(NULL, "pll_x");
+	if (IS_ERR(pll_x_clk))
+		return PTR_ERR(pll_x_clk);
+
+	pll_p_clk = clk_get_sys(NULL, "pll_p_cclk");
+	if (IS_ERR(pll_p_clk))
+		return PTR_ERR(pll_p_clk);
+
+	emc_clk = clk_get_sys("cpu", "emc");
+	if (IS_ERR(emc_clk)) {
+		clk_put(cpu_clk);
+		return PTR_ERR(emc_clk);
+	}
+
+	return cpufreq_register_driver(&tegra_cpufreq_driver);
+}
+
+static void __exit tegra_cpufreq_exit(void)
+{
+        cpufreq_unregister_driver(&tegra_cpufreq_driver);
+	clk_put(emc_clk);
+	clk_put(cpu_clk);
+}
+
+
+MODULE_AUTHOR("Colin Cross <ccross@android.com>");
+MODULE_DESCRIPTION("cpufreq driver for Nvidia Tegra2");
+MODULE_LICENSE("GPL");
+module_init(tegra_cpufreq_init);
+module_exit(tegra_cpufreq_exit);
diff --git a/drivers/cpufreq/unicore2-cpufreq.c b/drivers/cpufreq/unicore2-cpufreq.c
new file mode 100644
index 000000000000..12fc904d7dab
--- /dev/null
+++ b/drivers/cpufreq/unicore2-cpufreq.c
@@ -0,0 +1,92 @@
+/*
+ * clock scaling for the UniCore-II
+ *
+ * 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/kernel.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/clk.h>
+#include <linux/cpufreq.h>
+
+#include <mach/hardware.h>
+
+static struct cpufreq_driver ucv2_driver;
+
+/* make sure that only the "userspace" governor is run
+ * -- anything else wouldn't make sense on this platform, anyway.
+ */
+int ucv2_verify_speed(struct cpufreq_policy *policy)
+{
+	if (policy->cpu)
+		return -EINVAL;
+
+	cpufreq_verify_within_limits(policy,
+			policy->cpuinfo.min_freq, policy->cpuinfo.max_freq);
+
+	return 0;
+}
+
+static unsigned int ucv2_getspeed(unsigned int cpu)
+{
+	struct clk *mclk = clk_get(NULL, "MAIN_CLK");
+
+	if (cpu)
+		return 0;
+	return clk_get_rate(mclk)/1000;
+}
+
+static int ucv2_target(struct cpufreq_policy *policy,
+			 unsigned int target_freq,
+			 unsigned int relation)
+{
+	unsigned int cur = ucv2_getspeed(0);
+	struct cpufreq_freqs freqs;
+	struct clk *mclk = clk_get(NULL, "MAIN_CLK");
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_PRECHANGE);
+
+	if (!clk_set_rate(mclk, target_freq * 1000)) {
+		freqs.old = cur;
+		freqs.new = target_freq;
+	}
+
+	cpufreq_notify_transition(policy, &freqs, CPUFREQ_POSTCHANGE);
+
+	return 0;
+}
+
+static int __init ucv2_cpu_init(struct cpufreq_policy *policy)
+{
+	if (policy->cpu != 0)
+		return -EINVAL;
+	policy->cur = ucv2_getspeed(0);
+	policy->min = policy->cpuinfo.min_freq = 250000;
+	policy->max = policy->cpuinfo.max_freq = 1000000;
+	policy->cpuinfo.transition_latency = CPUFREQ_ETERNAL;
+	return 0;
+}
+
+static struct cpufreq_driver ucv2_driver = {
+	.flags		= CPUFREQ_STICKY,
+	.verify		= ucv2_verify_speed,
+	.target		= ucv2_target,
+	.get		= ucv2_getspeed,
+	.init		= ucv2_cpu_init,
+	.name		= "UniCore-II",
+};
+
+static int __init ucv2_cpufreq_init(void)
+{
+	return cpufreq_register_driver(&ucv2_driver);
+}
+
+arch_initcall(ucv2_cpufreq_init);
diff --git a/drivers/cpuidle/Kconfig b/drivers/cpuidle/Kconfig
index 071e2c3eec4f..c4cc27e5c8a5 100644
--- a/drivers/cpuidle/Kconfig
+++ b/drivers/cpuidle/Kconfig
@@ -39,10 +39,4 @@ config CPU_IDLE_CALXEDA
 	help
 	  Select this to enable cpuidle on Calxeda processors.
 
-config CPU_IDLE_KIRKWOOD
-	bool "CPU Idle Driver for Kirkwood processors"
-	depends on ARCH_KIRKWOOD
-	help
-	  Select this to enable cpuidle on Kirkwood processors.
-
 endif
diff --git a/drivers/cpuidle/Makefile b/drivers/cpuidle/Makefile
index 24c6e7d945ed..0d8bd55e776f 100644
--- a/drivers/cpuidle/Makefile
+++ b/drivers/cpuidle/Makefile
@@ -6,4 +6,4 @@ obj-y += cpuidle.o driver.o governor.o sysfs.o governors/
 obj-$(CONFIG_ARCH_NEEDS_CPU_IDLE_COUPLED) += coupled.o
 
 obj-$(CONFIG_CPU_IDLE_CALXEDA) += cpuidle-calxeda.o
-obj-$(CONFIG_CPU_IDLE_KIRKWOOD) += cpuidle-kirkwood.o
+obj-$(CONFIG_ARCH_KIRKWOOD) += cpuidle-kirkwood.o
diff --git a/drivers/cpuidle/cpuidle-calxeda.c b/drivers/cpuidle/cpuidle-calxeda.c
index e1aab38c5a8d..223379169cb0 100644
--- a/drivers/cpuidle/cpuidle-calxeda.c
+++ b/drivers/cpuidle/cpuidle-calxeda.c
@@ -1,7 +1,7 @@
 /*
  * Copyright 2012 Calxeda, Inc.
  *
- * Based on arch/arm/plat-mxc/cpuidle.c:
+ * Based on arch/arm/plat-mxc/cpuidle.c: #v3.7
  * Copyright 2012 Freescale Semiconductor, Inc.
  * Copyright 2012 Linaro Ltd.
  *
@@ -16,6 +16,8 @@
  *
  * You should have received a copy of the GNU General Public License along with
  * this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Maintainer: Rob Herring <rob.herring@calxeda.com>
  */
 
 #include <linux/cpuidle.h>
@@ -35,8 +37,6 @@
 extern void highbank_set_cpu_jump(int cpu, void *jump_addr);
 extern void *scu_base_addr;
 
-static struct cpuidle_device __percpu *calxeda_idle_cpuidle_devices;
-
 static inline unsigned int get_auxcr(void)
 {
 	unsigned int val;
@@ -85,22 +85,8 @@ static int calxeda_pwrdown_idle(struct cpuidle_device *dev,
 	return index;
 }
 
-static void calxeda_idle_cpuidle_devices_uninit(void)
-{
-	int i;
-	struct cpuidle_device *dev;
-
-	for_each_possible_cpu(i) {
-		dev = per_cpu_ptr(calxeda_idle_cpuidle_devices, i);
-		cpuidle_unregister_device(dev);
-	}
-
-	free_percpu(calxeda_idle_cpuidle_devices);
-}
-
 static struct cpuidle_driver calxeda_idle_driver = {
 	.name = "calxeda_idle",
-	.en_core_tk_irqen = 1,
 	.states = {
 		ARM_CPUIDLE_WFI_STATE,
 		{
@@ -118,44 +104,9 @@ static struct cpuidle_driver calxeda_idle_driver = {
 
 static int __init calxeda_cpuidle_init(void)
 {
-	int cpu_id;
-	int ret;
-	struct cpuidle_device *dev;
-	struct cpuidle_driver *drv = &calxeda_idle_driver;
-
 	if (!of_machine_is_compatible("calxeda,highbank"))
 		return -ENODEV;
 
-	ret = cpuidle_register_driver(drv);
-	if (ret)
-		return ret;
-
-	calxeda_idle_cpuidle_devices = alloc_percpu(struct cpuidle_device);
-	if (calxeda_idle_cpuidle_devices == NULL) {
-		ret = -ENOMEM;
-		goto unregister_drv;
-	}
-
-	/* initialize state data for each cpuidle_device */
-	for_each_possible_cpu(cpu_id) {
-		dev = per_cpu_ptr(calxeda_idle_cpuidle_devices, cpu_id);
-		dev->cpu = cpu_id;
-		dev->state_count = drv->state_count;
-
-		ret = cpuidle_register_device(dev);
-		if (ret) {
-			pr_err("Failed to register cpu %u, error: %d\n",
-			       cpu_id, ret);
-			goto uninit;
-		}
-	}
-
-	return 0;
-
-uninit:
-	calxeda_idle_cpuidle_devices_uninit();
-unregister_drv:
-	cpuidle_unregister_driver(drv);
-	return ret;
+	return cpuidle_register(&calxeda_idle_driver, NULL);
 }
 module_init(calxeda_cpuidle_init);
diff --git a/drivers/cpuidle/cpuidle-kirkwood.c b/drivers/cpuidle/cpuidle-kirkwood.c
index 670aa1e55cd6..521b0a7fdd89 100644
--- a/drivers/cpuidle/cpuidle-kirkwood.c
+++ b/drivers/cpuidle/cpuidle-kirkwood.c
@@ -1,6 +1,4 @@
 /*
- * arch/arm/mach-kirkwood/cpuidle.c
- *
  * CPU idle Marvell Kirkwood SoCs
  *
  * This file is licensed under the terms of the GNU General Public
@@ -11,6 +9,9 @@
  * to implement two idle states -
  * #1 wait-for-interrupt
  * #2 wait-for-interrupt and DDR self refresh
+ *
+ * Maintainer: Jason Cooper <jason@lakedaemon.net>
+ * Maintainer: Andrew Lunn <andrew@lunn.ch>
  */
 
 #include <linux/kernel.h>
@@ -41,7 +42,6 @@ static int kirkwood_enter_idle(struct cpuidle_device *dev,
 static struct cpuidle_driver kirkwood_idle_driver = {
 	.name			= "kirkwood_idle",
 	.owner			= THIS_MODULE,
-	.en_core_tk_irqen	= 1,
 	.states[0]		= ARM_CPUIDLE_WFI_STATE,
 	.states[1]		= {
 		.enter			= kirkwood_enter_idle,
@@ -53,9 +53,6 @@ static struct cpuidle_driver kirkwood_idle_driver = {
 	},
 	.state_count = KIRKWOOD_MAX_STATES,
 };
-static struct cpuidle_device *device;
-
-static DEFINE_PER_CPU(struct cpuidle_device, kirkwood_cpuidle_device);
 
 /* Initialize CPU idle by registering the idle states */
 static int kirkwood_cpuidle_probe(struct platform_device *pdev)
@@ -66,26 +63,16 @@ static int kirkwood_cpuidle_probe(struct platform_device *pdev)
 	if (res == NULL)
 		return -EINVAL;
 
-	ddr_operation_base = devm_request_and_ioremap(&pdev->dev, res);
-	if (!ddr_operation_base)
-		return -EADDRNOTAVAIL;
+	ddr_operation_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(ddr_operation_base))
+		return PTR_ERR(ddr_operation_base);
 
-	device = &per_cpu(kirkwood_cpuidle_device, smp_processor_id());
-	device->state_count = KIRKWOOD_MAX_STATES;
-
-	cpuidle_register_driver(&kirkwood_idle_driver);
-	if (cpuidle_register_device(device)) {
-		pr_err("kirkwood_init_cpuidle: Failed registering\n");
-		return -EIO;
-	}
-	return 0;
+	return cpuidle_register(&kirkwood_idle_driver, NULL);
 }
 
 int kirkwood_cpuidle_remove(struct platform_device *pdev)
 {
-	cpuidle_unregister_device(device);
-	cpuidle_unregister_driver(&kirkwood_idle_driver);
-
+	cpuidle_unregister(&kirkwood_idle_driver);
 	return 0;
 }
 
diff --git a/drivers/cpuidle/cpuidle.c b/drivers/cpuidle/cpuidle.c
index eba69290e074..c3a93fece819 100644
--- a/drivers/cpuidle/cpuidle.c
+++ b/drivers/cpuidle/cpuidle.c
@@ -8,6 +8,7 @@
  * This code is licenced under the GPL.
  */
 
+#include <linux/clockchips.h>
 #include <linux/kernel.h>
 #include <linux/mutex.h>
 #include <linux/sched.h>
@@ -23,6 +24,7 @@
 #include "cpuidle.h"
 
 DEFINE_PER_CPU(struct cpuidle_device *, cpuidle_devices);
+DEFINE_PER_CPU(struct cpuidle_device, cpuidle_dev);
 
 DEFINE_MUTEX(cpuidle_lock);
 LIST_HEAD(cpuidle_detected_devices);
@@ -42,24 +44,6 @@ void disable_cpuidle(void)
 
 static int __cpuidle_register_device(struct cpuidle_device *dev);
 
-static inline int cpuidle_enter(struct cpuidle_device *dev,
-				struct cpuidle_driver *drv, int index)
-{
-	struct cpuidle_state *target_state = &drv->states[index];
-	return target_state->enter(dev, drv, index);
-}
-
-static inline int cpuidle_enter_tk(struct cpuidle_device *dev,
-			       struct cpuidle_driver *drv, int index)
-{
-	return cpuidle_wrap_enter(dev, drv, index, cpuidle_enter);
-}
-
-typedef int (*cpuidle_enter_t)(struct cpuidle_device *dev,
-			       struct cpuidle_driver *drv, int index);
-
-static cpuidle_enter_t cpuidle_enter_ops;
-
 /**
  * cpuidle_play_dead - cpu off-lining
  *
@@ -89,11 +73,27 @@ int cpuidle_play_dead(void)
  * @next_state: index into drv->states of the state to enter
  */
 int cpuidle_enter_state(struct cpuidle_device *dev, struct cpuidle_driver *drv,
-		int next_state)
+			int index)
 {
 	int entered_state;
 
-	entered_state = cpuidle_enter_ops(dev, drv, next_state);
+	struct cpuidle_state *target_state = &drv->states[index];
+	ktime_t time_start, time_end;
+	s64 diff;
+
+	time_start = ktime_get();
+
+	entered_state = target_state->enter(dev, drv, index);
+
+	time_end = ktime_get();
+
+	local_irq_enable();
+
+	diff = ktime_to_us(ktime_sub(time_end, time_start));
+	if (diff > INT_MAX)
+		diff = INT_MAX;
+
+	dev->last_residency = (int) diff;
 
 	if (entered_state >= 0) {
 		/* Update cpuidle counters */
@@ -146,12 +146,20 @@ int cpuidle_idle_call(void)
 
 	trace_cpu_idle_rcuidle(next_state, dev->cpu);
 
+	if (drv->states[next_state].flags & CPUIDLE_FLAG_TIMER_STOP)
+		clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER,
+				   &dev->cpu);
+
 	if (cpuidle_state_is_coupled(dev, drv, next_state))
 		entered_state = cpuidle_enter_state_coupled(dev, drv,
 							    next_state);
 	else
 		entered_state = cpuidle_enter_state(dev, drv, next_state);
 
+	if (drv->states[next_state].flags & CPUIDLE_FLAG_TIMER_STOP)
+		clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT,
+				   &dev->cpu);
+
 	trace_cpu_idle_rcuidle(PWR_EVENT_EXIT, dev->cpu);
 
 	/* give the governor an opportunity to reflect on the outcome */
@@ -222,37 +230,6 @@ void cpuidle_resume(void)
 	mutex_unlock(&cpuidle_lock);
 }
 
-/**
- * cpuidle_wrap_enter - performs timekeeping and irqen around enter function
- * @dev: pointer to a valid cpuidle_device object
- * @drv: pointer to a valid cpuidle_driver object
- * @index: index of the target cpuidle state.
- */
-int cpuidle_wrap_enter(struct cpuidle_device *dev,
-				struct cpuidle_driver *drv, int index,
-				int (*enter)(struct cpuidle_device *dev,
-					struct cpuidle_driver *drv, int index))
-{
-	ktime_t time_start, time_end;
-	s64 diff;
-
-	time_start = ktime_get();
-
-	index = enter(dev, drv, index);
-
-	time_end = ktime_get();
-
-	local_irq_enable();
-
-	diff = ktime_to_us(ktime_sub(time_end, time_start));
-	if (diff > INT_MAX)
-		diff = INT_MAX;
-
-	dev->last_residency = (int) diff;
-
-	return index;
-}
-
 #ifdef CONFIG_ARCH_HAS_CPU_RELAX
 static int poll_idle(struct cpuidle_device *dev,
 		struct cpuidle_driver *drv, int index)
@@ -324,9 +301,6 @@ int cpuidle_enable_device(struct cpuidle_device *dev)
 			return ret;
 	}
 
-	cpuidle_enter_ops = drv->en_core_tk_irqen ?
-		cpuidle_enter_tk : cpuidle_enter;
-
 	poll_idle_init(drv);
 
 	ret = cpuidle_add_device_sysfs(dev);
@@ -480,6 +454,77 @@ void cpuidle_unregister_device(struct cpuidle_device *dev)
 
 EXPORT_SYMBOL_GPL(cpuidle_unregister_device);
 
+/**
+ * cpuidle_unregister: unregister a driver and the devices. This function
+ * can be used only if the driver has been previously registered through
+ * the cpuidle_register function.
+ *
+ * @drv: a valid pointer to a struct cpuidle_driver
+ */
+void cpuidle_unregister(struct cpuidle_driver *drv)
+{
+	int cpu;
+	struct cpuidle_device *device;
+
+	for_each_possible_cpu(cpu) {
+		device = &per_cpu(cpuidle_dev, cpu);
+		cpuidle_unregister_device(device);
+	}
+
+	cpuidle_unregister_driver(drv);
+}
+EXPORT_SYMBOL_GPL(cpuidle_unregister);
+
+/**
+ * cpuidle_register: registers the driver and the cpu devices with the
+ * coupled_cpus passed as parameter. This function is used for all common
+ * initialization pattern there are in the arch specific drivers. The
+ * devices is globally defined in this file.
+ *
+ * @drv         : a valid pointer to a struct cpuidle_driver
+ * @coupled_cpus: a cpumask for the coupled states
+ *
+ * Returns 0 on success, < 0 otherwise
+ */
+int cpuidle_register(struct cpuidle_driver *drv,
+		     const struct cpumask *const coupled_cpus)
+{
+	int ret, cpu;
+	struct cpuidle_device *device;
+
+	ret = cpuidle_register_driver(drv);
+	if (ret) {
+		pr_err("failed to register cpuidle driver\n");
+		return ret;
+	}
+
+	for_each_possible_cpu(cpu) {
+		device = &per_cpu(cpuidle_dev, cpu);
+		device->cpu = cpu;
+
+#ifdef CONFIG_ARCH_NEEDS_CPU_IDLE_COUPLED
+		/*
+		 * On multiplatform for ARM, the coupled idle states could
+		 * enabled in the kernel even if the cpuidle driver does not
+		 * use it. Note, coupled_cpus is a struct copy.
+		 */
+		if (coupled_cpus)
+			device->coupled_cpus = *coupled_cpus;
+#endif
+		ret = cpuidle_register_device(device);
+		if (!ret)
+			continue;
+
+		pr_err("Failed to register cpuidle device for cpu%d\n", cpu);
+
+		cpuidle_unregister(drv);
+		break;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(cpuidle_register);
+
 #ifdef CONFIG_SMP
 
 static void smp_callback(void *v)
diff --git a/drivers/cpuidle/driver.c b/drivers/cpuidle/driver.c
index 422c7b69ba7c..8dfaaae94444 100644
--- a/drivers/cpuidle/driver.c
+++ b/drivers/cpuidle/driver.c
@@ -11,6 +11,8 @@
 #include <linux/mutex.h>
 #include <linux/module.h>
 #include <linux/cpuidle.h>
+#include <linux/cpumask.h>
+#include <linux/clockchips.h>
 
 #include "cpuidle.h"
 
@@ -19,9 +21,28 @@ DEFINE_SPINLOCK(cpuidle_driver_lock);
 static void __cpuidle_set_cpu_driver(struct cpuidle_driver *drv, int cpu);
 static struct cpuidle_driver * __cpuidle_get_cpu_driver(int cpu);
 
-static void __cpuidle_driver_init(struct cpuidle_driver *drv)
+static void cpuidle_setup_broadcast_timer(void *arg)
 {
+	int cpu = smp_processor_id();
+	clockevents_notify((long)(arg), &cpu);
+}
+
+static void __cpuidle_driver_init(struct cpuidle_driver *drv, int cpu)
+{
+	int i;
+
 	drv->refcnt = 0;
+
+	for (i = drv->state_count - 1; i >= 0 ; i--) {
+
+		if (!(drv->states[i].flags & CPUIDLE_FLAG_TIMER_STOP))
+			continue;
+
+		drv->bctimer = 1;
+		on_each_cpu_mask(get_cpu_mask(cpu), cpuidle_setup_broadcast_timer,
+				 (void *)CLOCK_EVT_NOTIFY_BROADCAST_ON, 1);
+		break;
+	}
 }
 
 static int __cpuidle_register_driver(struct cpuidle_driver *drv, int cpu)
@@ -35,7 +56,7 @@ static int __cpuidle_register_driver(struct cpuidle_driver *drv, int cpu)
 	if (__cpuidle_get_cpu_driver(cpu))
 		return -EBUSY;
 
-	__cpuidle_driver_init(drv);
+	__cpuidle_driver_init(drv, cpu);
 
 	__cpuidle_set_cpu_driver(drv, cpu);
 
@@ -49,6 +70,12 @@ static void __cpuidle_unregister_driver(struct cpuidle_driver *drv, int cpu)
 
 	if (!WARN_ON(drv->refcnt > 0))
 		__cpuidle_set_cpu_driver(NULL, cpu);
+
+	if (drv->bctimer) {
+		drv->bctimer = 0;
+		on_each_cpu_mask(get_cpu_mask(cpu), cpuidle_setup_broadcast_timer,
+				 (void *)CLOCK_EVT_NOTIFY_BROADCAST_OFF, 1);
+	}
 }
 
 #ifdef CONFIG_CPU_IDLE_MULTIPLE_DRIVERS
diff --git a/drivers/crypto/Kconfig b/drivers/crypto/Kconfig
index 87ec4d027c25..dffb85525368 100644
--- a/drivers/crypto/Kconfig
+++ b/drivers/crypto/Kconfig
@@ -276,6 +276,16 @@ config CRYPTO_DEV_PICOXCELL
 
 	  Saying m here will build a module named pipcoxcell_crypto.
 
+config CRYPTO_DEV_SAHARA
+	tristate "Support for SAHARA crypto accelerator"
+	depends on ARCH_MXC && EXPERIMENTAL && OF
+	select CRYPTO_BLKCIPHER
+	select CRYPTO_AES
+	select CRYPTO_ECB
+	help
+	  This option enables support for the SAHARA HW crypto accelerator
+	  found in some Freescale i.MX chips.
+
 config CRYPTO_DEV_S5P
 	tristate "Support for Samsung S5PV210 crypto accelerator"
 	depends on ARCH_S5PV210
@@ -361,15 +371,17 @@ config CRYPTO_DEV_ATMEL_TDES
 	  will be called atmel-tdes.
 
 config CRYPTO_DEV_ATMEL_SHA
-	tristate "Support for Atmel SHA1/SHA256 hw accelerator"
+	tristate "Support for Atmel SHA hw accelerator"
 	depends on ARCH_AT91
 	select CRYPTO_SHA1
 	select CRYPTO_SHA256
+	select CRYPTO_SHA512
 	select CRYPTO_ALGAPI
 	help
-	  Some Atmel processors have SHA1/SHA256 hw accelerator.
+	  Some Atmel processors have SHA1/SHA224/SHA256/SHA384/SHA512
+	  hw accelerator.
 	  Select this if you want to use the Atmel module for
-	  SHA1/SHA256 algorithms.
+	  SHA1/SHA224/SHA256/SHA384/SHA512 algorithms.
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called atmel-sha.
diff --git a/drivers/crypto/Makefile b/drivers/crypto/Makefile
index 880a47b0b023..38ce13d3b79b 100644
--- a/drivers/crypto/Makefile
+++ b/drivers/crypto/Makefile
@@ -12,6 +12,7 @@ obj-$(CONFIG_CRYPTO_DEV_PPC4XX) += amcc/
 obj-$(CONFIG_CRYPTO_DEV_OMAP_SHAM) += omap-sham.o
 obj-$(CONFIG_CRYPTO_DEV_OMAP_AES) += omap-aes.o
 obj-$(CONFIG_CRYPTO_DEV_PICOXCELL) += picoxcell_crypto.o
+obj-$(CONFIG_CRYPTO_DEV_SAHARA) += sahara.o
 obj-$(CONFIG_CRYPTO_DEV_S5P) += s5p-sss.o
 obj-$(CONFIG_CRYPTO_DEV_TEGRA_AES) += tegra-aes.o
 obj-$(CONFIG_CRYPTO_DEV_UX500) += ux500/
diff --git a/drivers/crypto/atmel-aes.c b/drivers/crypto/atmel-aes.c
index 6f22ba51f969..c1efd910d97b 100644
--- a/drivers/crypto/atmel-aes.c
+++ b/drivers/crypto/atmel-aes.c
@@ -38,7 +38,7 @@
 #include <crypto/aes.h>
 #include <crypto/hash.h>
 #include <crypto/internal/hash.h>
-#include <linux/platform_data/atmel-aes.h>
+#include <linux/platform_data/crypto-atmel.h>
 #include "atmel-aes-regs.h"
 
 #define CFB8_BLOCK_SIZE		1
@@ -47,7 +47,7 @@
 #define CFB64_BLOCK_SIZE	8
 
 /* AES flags */
-#define AES_FLAGS_MODE_MASK	0x01ff
+#define AES_FLAGS_MODE_MASK	0x03ff
 #define AES_FLAGS_ENCRYPT	BIT(0)
 #define AES_FLAGS_CBC		BIT(1)
 #define AES_FLAGS_CFB		BIT(2)
@@ -55,21 +55,26 @@
 #define AES_FLAGS_CFB16		BIT(4)
 #define AES_FLAGS_CFB32		BIT(5)
 #define AES_FLAGS_CFB64		BIT(6)
-#define AES_FLAGS_OFB		BIT(7)
-#define AES_FLAGS_CTR		BIT(8)
+#define AES_FLAGS_CFB128	BIT(7)
+#define AES_FLAGS_OFB		BIT(8)
+#define AES_FLAGS_CTR		BIT(9)
 
 #define AES_FLAGS_INIT		BIT(16)
 #define AES_FLAGS_DMA		BIT(17)
 #define AES_FLAGS_BUSY		BIT(18)
+#define AES_FLAGS_FAST		BIT(19)
 
-#define AES_FLAGS_DUALBUFF	BIT(24)
-
-#define ATMEL_AES_QUEUE_LENGTH	1
-#define ATMEL_AES_CACHE_SIZE	0
+#define ATMEL_AES_QUEUE_LENGTH	50
 
 #define ATMEL_AES_DMA_THRESHOLD		16
 
 
+struct atmel_aes_caps {
+	bool	has_dualbuff;
+	bool	has_cfb64;
+	u32		max_burst_size;
+};
+
 struct atmel_aes_dev;
 
 struct atmel_aes_ctx {
@@ -77,6 +82,8 @@ struct atmel_aes_ctx {
 
 	int		keylen;
 	u32		key[AES_KEYSIZE_256 / sizeof(u32)];
+
+	u16		block_size;
 };
 
 struct atmel_aes_reqctx {
@@ -112,20 +119,27 @@ struct atmel_aes_dev {
 
 	struct scatterlist	*in_sg;
 	unsigned int		nb_in_sg;
-
+	size_t				in_offset;
 	struct scatterlist	*out_sg;
 	unsigned int		nb_out_sg;
+	size_t				out_offset;
 
 	size_t	bufcnt;
+	size_t	buflen;
+	size_t	dma_size;
 
-	u8	buf_in[ATMEL_AES_DMA_THRESHOLD] __aligned(sizeof(u32));
-	int	dma_in;
+	void	*buf_in;
+	int		dma_in;
+	dma_addr_t	dma_addr_in;
 	struct atmel_aes_dma	dma_lch_in;
 
-	u8	buf_out[ATMEL_AES_DMA_THRESHOLD] __aligned(sizeof(u32));
-	int	dma_out;
+	void	*buf_out;
+	int		dma_out;
+	dma_addr_t	dma_addr_out;
 	struct atmel_aes_dma	dma_lch_out;
 
+	struct atmel_aes_caps	caps;
+
 	u32	hw_version;
 };
 
@@ -165,6 +179,37 @@ static int atmel_aes_sg_length(struct ablkcipher_request *req,
 	return sg_nb;
 }
 
+static int atmel_aes_sg_copy(struct scatterlist **sg, size_t *offset,
+			void *buf, size_t buflen, size_t total, int out)
+{
+	unsigned int count, off = 0;
+
+	while (buflen && total) {
+		count = min((*sg)->length - *offset, total);
+		count = min(count, buflen);
+
+		if (!count)
+			return off;
+
+		scatterwalk_map_and_copy(buf + off, *sg, *offset, count, out);
+
+		off += count;
+		buflen -= count;
+		*offset += count;
+		total -= count;
+
+		if (*offset == (*sg)->length) {
+			*sg = sg_next(*sg);
+			if (*sg)
+				*offset = 0;
+			else
+				total = 0;
+		}
+	}
+
+	return off;
+}
+
 static inline u32 atmel_aes_read(struct atmel_aes_dev *dd, u32 offset)
 {
 	return readl_relaxed(dd->io_base + offset);
@@ -190,14 +235,6 @@ static void atmel_aes_write_n(struct atmel_aes_dev *dd, u32 offset,
 		atmel_aes_write(dd, offset, *value);
 }
 
-static void atmel_aes_dualbuff_test(struct atmel_aes_dev *dd)
-{
-	atmel_aes_write(dd, AES_MR, AES_MR_DUALBUFF);
-
-	if (atmel_aes_read(dd, AES_MR) & AES_MR_DUALBUFF)
-		dd->flags |= AES_FLAGS_DUALBUFF;
-}
-
 static struct atmel_aes_dev *atmel_aes_find_dev(struct atmel_aes_ctx *ctx)
 {
 	struct atmel_aes_dev *aes_dd = NULL;
@@ -225,7 +262,7 @@ static int atmel_aes_hw_init(struct atmel_aes_dev *dd)
 
 	if (!(dd->flags & AES_FLAGS_INIT)) {
 		atmel_aes_write(dd, AES_CR, AES_CR_SWRST);
-		atmel_aes_dualbuff_test(dd);
+		atmel_aes_write(dd, AES_MR, 0xE << AES_MR_CKEY_OFFSET);
 		dd->flags |= AES_FLAGS_INIT;
 		dd->err = 0;
 	}
@@ -233,11 +270,19 @@ static int atmel_aes_hw_init(struct atmel_aes_dev *dd)
 	return 0;
 }
 
+static inline unsigned int atmel_aes_get_version(struct atmel_aes_dev *dd)
+{
+	return atmel_aes_read(dd, AES_HW_VERSION) & 0x00000fff;
+}
+
 static void atmel_aes_hw_version_init(struct atmel_aes_dev *dd)
 {
 	atmel_aes_hw_init(dd);
 
-	dd->hw_version = atmel_aes_read(dd, AES_HW_VERSION);
+	dd->hw_version = atmel_aes_get_version(dd);
+
+	dev_info(dd->dev,
+			"version: 0x%x\n", dd->hw_version);
 
 	clk_disable_unprepare(dd->iclk);
 }
@@ -260,50 +305,77 @@ static void atmel_aes_dma_callback(void *data)
 	tasklet_schedule(&dd->done_task);
 }
 
-static int atmel_aes_crypt_dma(struct atmel_aes_dev *dd)
+static int atmel_aes_crypt_dma(struct atmel_aes_dev *dd,
+		dma_addr_t dma_addr_in, dma_addr_t dma_addr_out, int length)
 {
+	struct scatterlist sg[2];
 	struct dma_async_tx_descriptor	*in_desc, *out_desc;
-	int nb_dma_sg_in, nb_dma_sg_out;
 
-	dd->nb_in_sg = atmel_aes_sg_length(dd->req, dd->in_sg);
-	if (!dd->nb_in_sg)
-		goto exit_err;
+	dd->dma_size = length;
 
-	nb_dma_sg_in = dma_map_sg(dd->dev, dd->in_sg, dd->nb_in_sg,
-			DMA_TO_DEVICE);
-	if (!nb_dma_sg_in)
-		goto exit_err;
+	if (!(dd->flags & AES_FLAGS_FAST)) {
+		dma_sync_single_for_device(dd->dev, dma_addr_in, length,
+					   DMA_TO_DEVICE);
+	}
 
-	in_desc = dmaengine_prep_slave_sg(dd->dma_lch_in.chan, dd->in_sg,
-				nb_dma_sg_in, DMA_MEM_TO_DEV,
-				DMA_PREP_INTERRUPT  |  DMA_CTRL_ACK);
+	if (dd->flags & AES_FLAGS_CFB8) {
+		dd->dma_lch_in.dma_conf.dst_addr_width =
+			DMA_SLAVE_BUSWIDTH_1_BYTE;
+		dd->dma_lch_out.dma_conf.src_addr_width =
+			DMA_SLAVE_BUSWIDTH_1_BYTE;
+	} else if (dd->flags & AES_FLAGS_CFB16) {
+		dd->dma_lch_in.dma_conf.dst_addr_width =
+			DMA_SLAVE_BUSWIDTH_2_BYTES;
+		dd->dma_lch_out.dma_conf.src_addr_width =
+			DMA_SLAVE_BUSWIDTH_2_BYTES;
+	} else {
+		dd->dma_lch_in.dma_conf.dst_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
+		dd->dma_lch_out.dma_conf.src_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
+	}
 
-	if (!in_desc)
-		goto unmap_in;
+	if (dd->flags & (AES_FLAGS_CFB8 | AES_FLAGS_CFB16 |
+			AES_FLAGS_CFB32 | AES_FLAGS_CFB64)) {
+		dd->dma_lch_in.dma_conf.src_maxburst = 1;
+		dd->dma_lch_in.dma_conf.dst_maxburst = 1;
+		dd->dma_lch_out.dma_conf.src_maxburst = 1;
+		dd->dma_lch_out.dma_conf.dst_maxburst = 1;
+	} else {
+		dd->dma_lch_in.dma_conf.src_maxburst = dd->caps.max_burst_size;
+		dd->dma_lch_in.dma_conf.dst_maxburst = dd->caps.max_burst_size;
+		dd->dma_lch_out.dma_conf.src_maxburst = dd->caps.max_burst_size;
+		dd->dma_lch_out.dma_conf.dst_maxburst = dd->caps.max_burst_size;
+	}
 
-	/* callback not needed */
+	dmaengine_slave_config(dd->dma_lch_in.chan, &dd->dma_lch_in.dma_conf);
+	dmaengine_slave_config(dd->dma_lch_out.chan, &dd->dma_lch_out.dma_conf);
 
-	dd->nb_out_sg = atmel_aes_sg_length(dd->req, dd->out_sg);
-	if (!dd->nb_out_sg)
-		goto unmap_in;
+	dd->flags |= AES_FLAGS_DMA;
 
-	nb_dma_sg_out = dma_map_sg(dd->dev, dd->out_sg, dd->nb_out_sg,
-			DMA_FROM_DEVICE);
-	if (!nb_dma_sg_out)
-		goto unmap_out;
+	sg_init_table(&sg[0], 1);
+	sg_dma_address(&sg[0]) = dma_addr_in;
+	sg_dma_len(&sg[0]) = length;
 
-	out_desc = dmaengine_prep_slave_sg(dd->dma_lch_out.chan, dd->out_sg,
-				nb_dma_sg_out, DMA_DEV_TO_MEM,
-				DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	sg_init_table(&sg[1], 1);
+	sg_dma_address(&sg[1]) = dma_addr_out;
+	sg_dma_len(&sg[1]) = length;
+
+	in_desc = dmaengine_prep_slave_sg(dd->dma_lch_in.chan, &sg[0],
+				1, DMA_MEM_TO_DEV,
+				DMA_PREP_INTERRUPT  |  DMA_CTRL_ACK);
+	if (!in_desc)
+		return -EINVAL;
 
+	out_desc = dmaengine_prep_slave_sg(dd->dma_lch_out.chan, &sg[1],
+				1, DMA_DEV_TO_MEM,
+				DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	if (!out_desc)
-		goto unmap_out;
+		return -EINVAL;
 
 	out_desc->callback = atmel_aes_dma_callback;
 	out_desc->callback_param = dd;
 
-	dd->total -= dd->req->nbytes;
-
 	dmaengine_submit(out_desc);
 	dma_async_issue_pending(dd->dma_lch_out.chan);
 
@@ -311,15 +383,6 @@ static int atmel_aes_crypt_dma(struct atmel_aes_dev *dd)
 	dma_async_issue_pending(dd->dma_lch_in.chan);
 
 	return 0;
-
-unmap_out:
-	dma_unmap_sg(dd->dev, dd->out_sg, dd->nb_out_sg,
-		DMA_FROM_DEVICE);
-unmap_in:
-	dma_unmap_sg(dd->dev, dd->in_sg, dd->nb_in_sg,
-		DMA_TO_DEVICE);
-exit_err:
-	return -EINVAL;
 }
 
 static int atmel_aes_crypt_cpu_start(struct atmel_aes_dev *dd)
@@ -352,30 +415,66 @@ static int atmel_aes_crypt_cpu_start(struct atmel_aes_dev *dd)
 
 static int atmel_aes_crypt_dma_start(struct atmel_aes_dev *dd)
 {
-	int err;
+	int err, fast = 0, in, out;
+	size_t count;
+	dma_addr_t addr_in, addr_out;
+
+	if ((!dd->in_offset) && (!dd->out_offset)) {
+		/* check for alignment */
+		in = IS_ALIGNED((u32)dd->in_sg->offset, sizeof(u32)) &&
+			IS_ALIGNED(dd->in_sg->length, dd->ctx->block_size);
+		out = IS_ALIGNED((u32)dd->out_sg->offset, sizeof(u32)) &&
+			IS_ALIGNED(dd->out_sg->length, dd->ctx->block_size);
+		fast = in && out;
+
+		if (sg_dma_len(dd->in_sg) != sg_dma_len(dd->out_sg))
+			fast = 0;
+	}
+
+
+	if (fast)  {
+		count = min(dd->total, sg_dma_len(dd->in_sg));
+		count = min(count, sg_dma_len(dd->out_sg));
+
+		err = dma_map_sg(dd->dev, dd->in_sg, 1, DMA_TO_DEVICE);
+		if (!err) {
+			dev_err(dd->dev, "dma_map_sg() error\n");
+			return -EINVAL;
+		}
+
+		err = dma_map_sg(dd->dev, dd->out_sg, 1,
+				DMA_FROM_DEVICE);
+		if (!err) {
+			dev_err(dd->dev, "dma_map_sg() error\n");
+			dma_unmap_sg(dd->dev, dd->in_sg, 1,
+				DMA_TO_DEVICE);
+			return -EINVAL;
+		}
+
+		addr_in = sg_dma_address(dd->in_sg);
+		addr_out = sg_dma_address(dd->out_sg);
+
+		dd->flags |= AES_FLAGS_FAST;
 
-	if (dd->flags & AES_FLAGS_CFB8) {
-		dd->dma_lch_in.dma_conf.dst_addr_width =
-			DMA_SLAVE_BUSWIDTH_1_BYTE;
-		dd->dma_lch_out.dma_conf.src_addr_width =
-			DMA_SLAVE_BUSWIDTH_1_BYTE;
-	} else if (dd->flags & AES_FLAGS_CFB16) {
-		dd->dma_lch_in.dma_conf.dst_addr_width =
-			DMA_SLAVE_BUSWIDTH_2_BYTES;
-		dd->dma_lch_out.dma_conf.src_addr_width =
-			DMA_SLAVE_BUSWIDTH_2_BYTES;
 	} else {
-		dd->dma_lch_in.dma_conf.dst_addr_width =
-			DMA_SLAVE_BUSWIDTH_4_BYTES;
-		dd->dma_lch_out.dma_conf.src_addr_width =
-			DMA_SLAVE_BUSWIDTH_4_BYTES;
+		/* use cache buffers */
+		count = atmel_aes_sg_copy(&dd->in_sg, &dd->in_offset,
+				dd->buf_in, dd->buflen, dd->total, 0);
+
+		addr_in = dd->dma_addr_in;
+		addr_out = dd->dma_addr_out;
+
+		dd->flags &= ~AES_FLAGS_FAST;
 	}
 
-	dmaengine_slave_config(dd->dma_lch_in.chan, &dd->dma_lch_in.dma_conf);
-	dmaengine_slave_config(dd->dma_lch_out.chan, &dd->dma_lch_out.dma_conf);
+	dd->total -= count;
 
-	dd->flags |= AES_FLAGS_DMA;
-	err = atmel_aes_crypt_dma(dd);
+	err = atmel_aes_crypt_dma(dd, addr_in, addr_out, count);
+
+	if (err && (dd->flags & AES_FLAGS_FAST)) {
+		dma_unmap_sg(dd->dev, dd->in_sg, 1, DMA_TO_DEVICE);
+		dma_unmap_sg(dd->dev, dd->out_sg, 1, DMA_TO_DEVICE);
+	}
 
 	return err;
 }
@@ -410,6 +509,8 @@ static int atmel_aes_write_ctrl(struct atmel_aes_dev *dd)
 			valmr |= AES_MR_CFBS_32b;
 		else if (dd->flags & AES_FLAGS_CFB64)
 			valmr |= AES_MR_CFBS_64b;
+		else if (dd->flags & AES_FLAGS_CFB128)
+			valmr |= AES_MR_CFBS_128b;
 	} else if (dd->flags & AES_FLAGS_OFB) {
 		valmr |= AES_MR_OPMOD_OFB;
 	} else if (dd->flags & AES_FLAGS_CTR) {
@@ -423,7 +524,7 @@ static int atmel_aes_write_ctrl(struct atmel_aes_dev *dd)
 
 	if (dd->total > ATMEL_AES_DMA_THRESHOLD) {
 		valmr |= AES_MR_SMOD_IDATAR0;
-		if (dd->flags & AES_FLAGS_DUALBUFF)
+		if (dd->caps.has_dualbuff)
 			valmr |= AES_MR_DUALBUFF;
 	} else {
 		valmr |= AES_MR_SMOD_AUTO;
@@ -477,7 +578,9 @@ static int atmel_aes_handle_queue(struct atmel_aes_dev *dd,
 	/* assign new request to device */
 	dd->req = req;
 	dd->total = req->nbytes;
+	dd->in_offset = 0;
 	dd->in_sg = req->src;
+	dd->out_offset = 0;
 	dd->out_sg = req->dst;
 
 	rctx = ablkcipher_request_ctx(req);
@@ -506,18 +609,86 @@ static int atmel_aes_handle_queue(struct atmel_aes_dev *dd,
 static int atmel_aes_crypt_dma_stop(struct atmel_aes_dev *dd)
 {
 	int err = -EINVAL;
+	size_t count;
 
 	if (dd->flags & AES_FLAGS_DMA) {
-		dma_unmap_sg(dd->dev, dd->out_sg,
-			dd->nb_out_sg, DMA_FROM_DEVICE);
-		dma_unmap_sg(dd->dev, dd->in_sg,
-			dd->nb_in_sg, DMA_TO_DEVICE);
 		err = 0;
+		if  (dd->flags & AES_FLAGS_FAST) {
+			dma_unmap_sg(dd->dev, dd->out_sg, 1, DMA_FROM_DEVICE);
+			dma_unmap_sg(dd->dev, dd->in_sg, 1, DMA_TO_DEVICE);
+		} else {
+			dma_sync_single_for_device(dd->dev, dd->dma_addr_out,
+				dd->dma_size, DMA_FROM_DEVICE);
+
+			/* copy data */
+			count = atmel_aes_sg_copy(&dd->out_sg, &dd->out_offset,
+				dd->buf_out, dd->buflen, dd->dma_size, 1);
+			if (count != dd->dma_size) {
+				err = -EINVAL;
+				pr_err("not all data converted: %u\n", count);
+			}
+		}
 	}
 
 	return err;
 }
 
+
+static int atmel_aes_buff_init(struct atmel_aes_dev *dd)
+{
+	int err = -ENOMEM;
+
+	dd->buf_in = (void *)__get_free_pages(GFP_KERNEL, 0);
+	dd->buf_out = (void *)__get_free_pages(GFP_KERNEL, 0);
+	dd->buflen = PAGE_SIZE;
+	dd->buflen &= ~(AES_BLOCK_SIZE - 1);
+
+	if (!dd->buf_in || !dd->buf_out) {
+		dev_err(dd->dev, "unable to alloc pages.\n");
+		goto err_alloc;
+	}
+
+	/* MAP here */
+	dd->dma_addr_in = dma_map_single(dd->dev, dd->buf_in,
+					dd->buflen, DMA_TO_DEVICE);
+	if (dma_mapping_error(dd->dev, dd->dma_addr_in)) {
+		dev_err(dd->dev, "dma %d bytes error\n", dd->buflen);
+		err = -EINVAL;
+		goto err_map_in;
+	}
+
+	dd->dma_addr_out = dma_map_single(dd->dev, dd->buf_out,
+					dd->buflen, DMA_FROM_DEVICE);
+	if (dma_mapping_error(dd->dev, dd->dma_addr_out)) {
+		dev_err(dd->dev, "dma %d bytes error\n", dd->buflen);
+		err = -EINVAL;
+		goto err_map_out;
+	}
+
+	return 0;
+
+err_map_out:
+	dma_unmap_single(dd->dev, dd->dma_addr_in, dd->buflen,
+		DMA_TO_DEVICE);
+err_map_in:
+	free_page((unsigned long)dd->buf_out);
+	free_page((unsigned long)dd->buf_in);
+err_alloc:
+	if (err)
+		pr_err("error: %d\n", err);
+	return err;
+}
+
+static void atmel_aes_buff_cleanup(struct atmel_aes_dev *dd)
+{
+	dma_unmap_single(dd->dev, dd->dma_addr_out, dd->buflen,
+			 DMA_FROM_DEVICE);
+	dma_unmap_single(dd->dev, dd->dma_addr_in, dd->buflen,
+		DMA_TO_DEVICE);
+	free_page((unsigned long)dd->buf_out);
+	free_page((unsigned long)dd->buf_in);
+}
+
 static int atmel_aes_crypt(struct ablkcipher_request *req, unsigned long mode)
 {
 	struct atmel_aes_ctx *ctx = crypto_ablkcipher_ctx(
@@ -525,9 +696,30 @@ static int atmel_aes_crypt(struct ablkcipher_request *req, unsigned long mode)
 	struct atmel_aes_reqctx *rctx = ablkcipher_request_ctx(req);
 	struct atmel_aes_dev *dd;
 
-	if (!IS_ALIGNED(req->nbytes, AES_BLOCK_SIZE)) {
-		pr_err("request size is not exact amount of AES blocks\n");
-		return -EINVAL;
+	if (mode & AES_FLAGS_CFB8) {
+		if (!IS_ALIGNED(req->nbytes, CFB8_BLOCK_SIZE)) {
+			pr_err("request size is not exact amount of CFB8 blocks\n");
+			return -EINVAL;
+		}
+		ctx->block_size = CFB8_BLOCK_SIZE;
+	} else if (mode & AES_FLAGS_CFB16) {
+		if (!IS_ALIGNED(req->nbytes, CFB16_BLOCK_SIZE)) {
+			pr_err("request size is not exact amount of CFB16 blocks\n");
+			return -EINVAL;
+		}
+		ctx->block_size = CFB16_BLOCK_SIZE;
+	} else if (mode & AES_FLAGS_CFB32) {
+		if (!IS_ALIGNED(req->nbytes, CFB32_BLOCK_SIZE)) {
+			pr_err("request size is not exact amount of CFB32 blocks\n");
+			return -EINVAL;
+		}
+		ctx->block_size = CFB32_BLOCK_SIZE;
+	} else {
+		if (!IS_ALIGNED(req->nbytes, AES_BLOCK_SIZE)) {
+			pr_err("request size is not exact amount of AES blocks\n");
+			return -EINVAL;
+		}
+		ctx->block_size = AES_BLOCK_SIZE;
 	}
 
 	dd = atmel_aes_find_dev(ctx);
@@ -551,14 +743,12 @@ static bool atmel_aes_filter(struct dma_chan *chan, void *slave)
 	}
 }
 
-static int atmel_aes_dma_init(struct atmel_aes_dev *dd)
+static int atmel_aes_dma_init(struct atmel_aes_dev *dd,
+	struct crypto_platform_data *pdata)
 {
 	int err = -ENOMEM;
-	struct aes_platform_data	*pdata;
 	dma_cap_mask_t mask_in, mask_out;
 
-	pdata = dd->dev->platform_data;
-
 	if (pdata && pdata->dma_slave->txdata.dma_dev &&
 		pdata->dma_slave->rxdata.dma_dev) {
 
@@ -568,28 +758,38 @@ static int atmel_aes_dma_init(struct atmel_aes_dev *dd)
 
 		dd->dma_lch_in.chan = dma_request_channel(mask_in,
 				atmel_aes_filter, &pdata->dma_slave->rxdata);
+
 		if (!dd->dma_lch_in.chan)
 			goto err_dma_in;
 
 		dd->dma_lch_in.dma_conf.direction = DMA_MEM_TO_DEV;
 		dd->dma_lch_in.dma_conf.dst_addr = dd->phys_base +
 			AES_IDATAR(0);
-		dd->dma_lch_in.dma_conf.src_maxburst = 1;
-		dd->dma_lch_in.dma_conf.dst_maxburst = 1;
+		dd->dma_lch_in.dma_conf.src_maxburst = dd->caps.max_burst_size;
+		dd->dma_lch_in.dma_conf.src_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
+		dd->dma_lch_in.dma_conf.dst_maxburst = dd->caps.max_burst_size;
+		dd->dma_lch_in.dma_conf.dst_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
 		dd->dma_lch_in.dma_conf.device_fc = false;
 
 		dma_cap_zero(mask_out);
 		dma_cap_set(DMA_SLAVE, mask_out);
 		dd->dma_lch_out.chan = dma_request_channel(mask_out,
 				atmel_aes_filter, &pdata->dma_slave->txdata);
+
 		if (!dd->dma_lch_out.chan)
 			goto err_dma_out;
 
 		dd->dma_lch_out.dma_conf.direction = DMA_DEV_TO_MEM;
 		dd->dma_lch_out.dma_conf.src_addr = dd->phys_base +
 			AES_ODATAR(0);
-		dd->dma_lch_out.dma_conf.src_maxburst = 1;
-		dd->dma_lch_out.dma_conf.dst_maxburst = 1;
+		dd->dma_lch_out.dma_conf.src_maxburst = dd->caps.max_burst_size;
+		dd->dma_lch_out.dma_conf.src_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
+		dd->dma_lch_out.dma_conf.dst_maxburst = dd->caps.max_burst_size;
+		dd->dma_lch_out.dma_conf.dst_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
 		dd->dma_lch_out.dma_conf.device_fc = false;
 
 		return 0;
@@ -665,13 +865,13 @@ static int atmel_aes_ofb_decrypt(struct ablkcipher_request *req)
 static int atmel_aes_cfb_encrypt(struct ablkcipher_request *req)
 {
 	return atmel_aes_crypt(req,
-		AES_FLAGS_ENCRYPT | AES_FLAGS_CFB);
+		AES_FLAGS_ENCRYPT | AES_FLAGS_CFB | AES_FLAGS_CFB128);
 }
 
 static int atmel_aes_cfb_decrypt(struct ablkcipher_request *req)
 {
 	return atmel_aes_crypt(req,
-		AES_FLAGS_CFB);
+		AES_FLAGS_CFB | AES_FLAGS_CFB128);
 }
 
 static int atmel_aes_cfb64_encrypt(struct ablkcipher_request *req)
@@ -753,7 +953,7 @@ static struct crypto_alg aes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= AES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
-	.cra_alignmask		= 0x0,
+	.cra_alignmask		= 0xf,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_aes_cra_init,
@@ -773,7 +973,7 @@ static struct crypto_alg aes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= AES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
-	.cra_alignmask		= 0x0,
+	.cra_alignmask		= 0xf,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_aes_cra_init,
@@ -794,7 +994,7 @@ static struct crypto_alg aes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= AES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
-	.cra_alignmask		= 0x0,
+	.cra_alignmask		= 0xf,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_aes_cra_init,
@@ -815,7 +1015,7 @@ static struct crypto_alg aes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= AES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
-	.cra_alignmask		= 0x0,
+	.cra_alignmask		= 0xf,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_aes_cra_init,
@@ -836,7 +1036,7 @@ static struct crypto_alg aes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= CFB32_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
-	.cra_alignmask		= 0x0,
+	.cra_alignmask		= 0x3,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_aes_cra_init,
@@ -857,7 +1057,7 @@ static struct crypto_alg aes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= CFB16_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
-	.cra_alignmask		= 0x0,
+	.cra_alignmask		= 0x1,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_aes_cra_init,
@@ -899,7 +1099,7 @@ static struct crypto_alg aes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= AES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
-	.cra_alignmask		= 0x0,
+	.cra_alignmask		= 0xf,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_aes_cra_init,
@@ -915,15 +1115,14 @@ static struct crypto_alg aes_algs[] = {
 },
 };
 
-static struct crypto_alg aes_cfb64_alg[] = {
-{
+static struct crypto_alg aes_cfb64_alg = {
 	.cra_name		= "cfb64(aes)",
 	.cra_driver_name	= "atmel-cfb64-aes",
 	.cra_priority		= 100,
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= CFB64_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
-	.cra_alignmask		= 0x0,
+	.cra_alignmask		= 0x7,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_aes_cra_init,
@@ -936,7 +1135,6 @@ static struct crypto_alg aes_cfb64_alg[] = {
 		.encrypt	= atmel_aes_cfb64_encrypt,
 		.decrypt	= atmel_aes_cfb64_decrypt,
 	}
-},
 };
 
 static void atmel_aes_queue_task(unsigned long data)
@@ -969,7 +1167,14 @@ static void atmel_aes_done_task(unsigned long data)
 	err = dd->err ? : err;
 
 	if (dd->total && !err) {
-		err = atmel_aes_crypt_dma_start(dd);
+		if (dd->flags & AES_FLAGS_FAST) {
+			dd->in_sg = sg_next(dd->in_sg);
+			dd->out_sg = sg_next(dd->out_sg);
+			if (!dd->in_sg || !dd->out_sg)
+				err = -EINVAL;
+		}
+		if (!err)
+			err = atmel_aes_crypt_dma_start(dd);
 		if (!err)
 			return; /* DMA started. Not fininishing. */
 	}
@@ -1003,8 +1208,8 @@ static void atmel_aes_unregister_algs(struct atmel_aes_dev *dd)
 
 	for (i = 0; i < ARRAY_SIZE(aes_algs); i++)
 		crypto_unregister_alg(&aes_algs[i]);
-	if (dd->hw_version >= 0x130)
-		crypto_unregister_alg(&aes_cfb64_alg[0]);
+	if (dd->caps.has_cfb64)
+		crypto_unregister_alg(&aes_cfb64_alg);
 }
 
 static int atmel_aes_register_algs(struct atmel_aes_dev *dd)
@@ -1017,10 +1222,8 @@ static int atmel_aes_register_algs(struct atmel_aes_dev *dd)
 			goto err_aes_algs;
 	}
 
-	atmel_aes_hw_version_init(dd);
-
-	if (dd->hw_version >= 0x130) {
-		err = crypto_register_alg(&aes_cfb64_alg[0]);
+	if (dd->caps.has_cfb64) {
+		err = crypto_register_alg(&aes_cfb64_alg);
 		if (err)
 			goto err_aes_cfb64_alg;
 	}
@@ -1036,10 +1239,32 @@ err_aes_algs:
 	return err;
 }
 
+static void atmel_aes_get_cap(struct atmel_aes_dev *dd)
+{
+	dd->caps.has_dualbuff = 0;
+	dd->caps.has_cfb64 = 0;
+	dd->caps.max_burst_size = 1;
+
+	/* keep only major version number */
+	switch (dd->hw_version & 0xff0) {
+	case 0x130:
+		dd->caps.has_dualbuff = 1;
+		dd->caps.has_cfb64 = 1;
+		dd->caps.max_burst_size = 4;
+		break;
+	case 0x120:
+		break;
+	default:
+		dev_warn(dd->dev,
+				"Unmanaged aes version, set minimum capabilities\n");
+		break;
+	}
+}
+
 static int atmel_aes_probe(struct platform_device *pdev)
 {
 	struct atmel_aes_dev *aes_dd;
-	struct aes_platform_data	*pdata;
+	struct crypto_platform_data *pdata;
 	struct device *dev = &pdev->dev;
 	struct resource *aes_res;
 	unsigned long aes_phys_size;
@@ -1099,7 +1324,7 @@ static int atmel_aes_probe(struct platform_device *pdev)
 	}
 
 	/* Initializing the clock */
-	aes_dd->iclk = clk_get(&pdev->dev, NULL);
+	aes_dd->iclk = clk_get(&pdev->dev, "aes_clk");
 	if (IS_ERR(aes_dd->iclk)) {
 		dev_err(dev, "clock intialization failed.\n");
 		err = PTR_ERR(aes_dd->iclk);
@@ -1113,7 +1338,15 @@ static int atmel_aes_probe(struct platform_device *pdev)
 		goto aes_io_err;
 	}
 
-	err = atmel_aes_dma_init(aes_dd);
+	atmel_aes_hw_version_init(aes_dd);
+
+	atmel_aes_get_cap(aes_dd);
+
+	err = atmel_aes_buff_init(aes_dd);
+	if (err)
+		goto err_aes_buff;
+
+	err = atmel_aes_dma_init(aes_dd, pdata);
 	if (err)
 		goto err_aes_dma;
 
@@ -1135,6 +1368,8 @@ err_algs:
 	spin_unlock(&atmel_aes.lock);
 	atmel_aes_dma_cleanup(aes_dd);
 err_aes_dma:
+	atmel_aes_buff_cleanup(aes_dd);
+err_aes_buff:
 	iounmap(aes_dd->io_base);
 aes_io_err:
 	clk_put(aes_dd->iclk);
diff --git a/drivers/crypto/atmel-sha-regs.h b/drivers/crypto/atmel-sha-regs.h
index dc53a20d7da1..83b2d7425666 100644
--- a/drivers/crypto/atmel-sha-regs.h
+++ b/drivers/crypto/atmel-sha-regs.h
@@ -14,10 +14,13 @@
 #define SHA_MR_MODE_MANUAL		0x0
 #define SHA_MR_MODE_AUTO		0x1
 #define SHA_MR_MODE_PDC			0x2
-#define	SHA_MR_DUALBUFF			(1 << 3)
 #define SHA_MR_PROCDLY			(1 << 4)
 #define SHA_MR_ALGO_SHA1		(0 << 8)
 #define SHA_MR_ALGO_SHA256		(1 << 8)
+#define SHA_MR_ALGO_SHA384		(2 << 8)
+#define SHA_MR_ALGO_SHA512		(3 << 8)
+#define SHA_MR_ALGO_SHA224		(4 << 8)
+#define	SHA_MR_DUALBUFF			(1 << 16)
 
 #define SHA_IER				0x10
 #define SHA_IDR				0x14
@@ -33,6 +36,8 @@
 #define SHA_ISR_URAT_MR			(0x2 << 12)
 #define SHA_ISR_URAT_WO			(0x5 << 12)
 
+#define	SHA_HW_VERSION		0xFC
+
 #define SHA_TPR				0x108
 #define SHA_TCR				0x10C
 #define SHA_TNPR			0x118
diff --git a/drivers/crypto/atmel-sha.c b/drivers/crypto/atmel-sha.c
index 4918e9424d31..eaed8bf183bc 100644
--- a/drivers/crypto/atmel-sha.c
+++ b/drivers/crypto/atmel-sha.c
@@ -38,6 +38,7 @@
 #include <crypto/sha.h>
 #include <crypto/hash.h>
 #include <crypto/internal/hash.h>
+#include <linux/platform_data/crypto-atmel.h>
 #include "atmel-sha-regs.h"
 
 /* SHA flags */
@@ -52,11 +53,12 @@
 #define SHA_FLAGS_FINUP		BIT(16)
 #define SHA_FLAGS_SG		BIT(17)
 #define SHA_FLAGS_SHA1		BIT(18)
-#define SHA_FLAGS_SHA256	BIT(19)
-#define SHA_FLAGS_ERROR		BIT(20)
-#define SHA_FLAGS_PAD		BIT(21)
-
-#define SHA_FLAGS_DUALBUFF	BIT(24)
+#define SHA_FLAGS_SHA224	BIT(19)
+#define SHA_FLAGS_SHA256	BIT(20)
+#define SHA_FLAGS_SHA384	BIT(21)
+#define SHA_FLAGS_SHA512	BIT(22)
+#define SHA_FLAGS_ERROR		BIT(23)
+#define SHA_FLAGS_PAD		BIT(24)
 
 #define SHA_OP_UPDATE	1
 #define SHA_OP_FINAL	2
@@ -65,6 +67,12 @@
 
 #define ATMEL_SHA_DMA_THRESHOLD		56
 
+struct atmel_sha_caps {
+	bool	has_dma;
+	bool	has_dualbuff;
+	bool	has_sha224;
+	bool	has_sha_384_512;
+};
 
 struct atmel_sha_dev;
 
@@ -73,8 +81,8 @@ struct atmel_sha_reqctx {
 	unsigned long	flags;
 	unsigned long	op;
 
-	u8	digest[SHA256_DIGEST_SIZE] __aligned(sizeof(u32));
-	size_t	digcnt;
+	u8	digest[SHA512_DIGEST_SIZE] __aligned(sizeof(u32));
+	u64	digcnt[2];
 	size_t	bufcnt;
 	size_t	buflen;
 	dma_addr_t	dma_addr;
@@ -84,6 +92,8 @@ struct atmel_sha_reqctx {
 	unsigned int	offset;	/* offset in current sg */
 	unsigned int	total;	/* total request */
 
+	size_t block_size;
+
 	u8	buffer[0] __aligned(sizeof(u32));
 };
 
@@ -97,7 +107,12 @@ struct atmel_sha_ctx {
 
 };
 
-#define ATMEL_SHA_QUEUE_LENGTH	1
+#define ATMEL_SHA_QUEUE_LENGTH	50
+
+struct atmel_sha_dma {
+	struct dma_chan			*chan;
+	struct dma_slave_config dma_conf;
+};
 
 struct atmel_sha_dev {
 	struct list_head	list;
@@ -114,6 +129,12 @@ struct atmel_sha_dev {
 	unsigned long		flags;
 	struct crypto_queue	queue;
 	struct ahash_request	*req;
+
+	struct atmel_sha_dma	dma_lch_in;
+
+	struct atmel_sha_caps	caps;
+
+	u32	hw_version;
 };
 
 struct atmel_sha_drv {
@@ -137,14 +158,6 @@ static inline void atmel_sha_write(struct atmel_sha_dev *dd,
 	writel_relaxed(value, dd->io_base + offset);
 }
 
-static void atmel_sha_dualbuff_test(struct atmel_sha_dev *dd)
-{
-	atmel_sha_write(dd, SHA_MR, SHA_MR_DUALBUFF);
-
-	if (atmel_sha_read(dd, SHA_MR) & SHA_MR_DUALBUFF)
-		dd->flags |= SHA_FLAGS_DUALBUFF;
-}
-
 static size_t atmel_sha_append_sg(struct atmel_sha_reqctx *ctx)
 {
 	size_t count;
@@ -176,31 +189,58 @@ static size_t atmel_sha_append_sg(struct atmel_sha_reqctx *ctx)
 }
 
 /*
- * The purpose of this padding is to ensure that the padded message
- * is a multiple of 512 bits. The bit "1" is appended at the end of
- * the message followed by "padlen-1" zero bits. Then a 64 bits block
- * equals to the message length in bits is appended.
+ * The purpose of this padding is to ensure that the padded message is a
+ * multiple of 512 bits (SHA1/SHA224/SHA256) or 1024 bits (SHA384/SHA512).
+ * The bit "1" is appended at the end of the message followed by
+ * "padlen-1" zero bits. Then a 64 bits block (SHA1/SHA224/SHA256) or
+ * 128 bits block (SHA384/SHA512) equals to the message length in bits
+ * is appended.
  *
- * padlen is calculated as followed:
+ * For SHA1/SHA224/SHA256, padlen is calculated as followed:
  *  - if message length < 56 bytes then padlen = 56 - message length
  *  - else padlen = 64 + 56 - message length
+ *
+ * For SHA384/SHA512, padlen is calculated as followed:
+ *  - if message length < 112 bytes then padlen = 112 - message length
+ *  - else padlen = 128 + 112 - message length
  */
 static void atmel_sha_fill_padding(struct atmel_sha_reqctx *ctx, int length)
 {
 	unsigned int index, padlen;
-	u64 bits;
-	u64 size;
-
-	bits = (ctx->bufcnt + ctx->digcnt + length) << 3;
-	size = cpu_to_be64(bits);
-
-	index = ctx->bufcnt & 0x3f;
-	padlen = (index < 56) ? (56 - index) : ((64+56) - index);
-	*(ctx->buffer + ctx->bufcnt) = 0x80;
-	memset(ctx->buffer + ctx->bufcnt + 1, 0, padlen-1);
-	memcpy(ctx->buffer + ctx->bufcnt + padlen, &size, 8);
-	ctx->bufcnt += padlen + 8;
-	ctx->flags |= SHA_FLAGS_PAD;
+	u64 bits[2];
+	u64 size[2];
+
+	size[0] = ctx->digcnt[0];
+	size[1] = ctx->digcnt[1];
+
+	size[0] += ctx->bufcnt;
+	if (size[0] < ctx->bufcnt)
+		size[1]++;
+
+	size[0] += length;
+	if (size[0]  < length)
+		size[1]++;
+
+	bits[1] = cpu_to_be64(size[0] << 3);
+	bits[0] = cpu_to_be64(size[1] << 3 | size[0] >> 61);
+
+	if (ctx->flags & (SHA_FLAGS_SHA384 | SHA_FLAGS_SHA512)) {
+		index = ctx->bufcnt & 0x7f;
+		padlen = (index < 112) ? (112 - index) : ((128+112) - index);
+		*(ctx->buffer + ctx->bufcnt) = 0x80;
+		memset(ctx->buffer + ctx->bufcnt + 1, 0, padlen-1);
+		memcpy(ctx->buffer + ctx->bufcnt + padlen, bits, 16);
+		ctx->bufcnt += padlen + 16;
+		ctx->flags |= SHA_FLAGS_PAD;
+	} else {
+		index = ctx->bufcnt & 0x3f;
+		padlen = (index < 56) ? (56 - index) : ((64+56) - index);
+		*(ctx->buffer + ctx->bufcnt) = 0x80;
+		memset(ctx->buffer + ctx->bufcnt + 1, 0, padlen-1);
+		memcpy(ctx->buffer + ctx->bufcnt + padlen, &bits[1], 8);
+		ctx->bufcnt += padlen + 8;
+		ctx->flags |= SHA_FLAGS_PAD;
+	}
 }
 
 static int atmel_sha_init(struct ahash_request *req)
@@ -231,13 +271,35 @@ static int atmel_sha_init(struct ahash_request *req)
 	dev_dbg(dd->dev, "init: digest size: %d\n",
 		crypto_ahash_digestsize(tfm));
 
-	if (crypto_ahash_digestsize(tfm) == SHA1_DIGEST_SIZE)
+	switch (crypto_ahash_digestsize(tfm)) {
+	case SHA1_DIGEST_SIZE:
 		ctx->flags |= SHA_FLAGS_SHA1;
-	else if (crypto_ahash_digestsize(tfm) == SHA256_DIGEST_SIZE)
+		ctx->block_size = SHA1_BLOCK_SIZE;
+		break;
+	case SHA224_DIGEST_SIZE:
+		ctx->flags |= SHA_FLAGS_SHA224;
+		ctx->block_size = SHA224_BLOCK_SIZE;
+		break;
+	case SHA256_DIGEST_SIZE:
 		ctx->flags |= SHA_FLAGS_SHA256;
+		ctx->block_size = SHA256_BLOCK_SIZE;
+		break;
+	case SHA384_DIGEST_SIZE:
+		ctx->flags |= SHA_FLAGS_SHA384;
+		ctx->block_size = SHA384_BLOCK_SIZE;
+		break;
+	case SHA512_DIGEST_SIZE:
+		ctx->flags |= SHA_FLAGS_SHA512;
+		ctx->block_size = SHA512_BLOCK_SIZE;
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
 
 	ctx->bufcnt = 0;
-	ctx->digcnt = 0;
+	ctx->digcnt[0] = 0;
+	ctx->digcnt[1] = 0;
 	ctx->buflen = SHA_BUFFER_LEN;
 
 	return 0;
@@ -249,19 +311,28 @@ static void atmel_sha_write_ctrl(struct atmel_sha_dev *dd, int dma)
 	u32 valcr = 0, valmr = SHA_MR_MODE_AUTO;
 
 	if (likely(dma)) {
-		atmel_sha_write(dd, SHA_IER, SHA_INT_TXBUFE);
+		if (!dd->caps.has_dma)
+			atmel_sha_write(dd, SHA_IER, SHA_INT_TXBUFE);
 		valmr = SHA_MR_MODE_PDC;
-		if (dd->flags & SHA_FLAGS_DUALBUFF)
-			valmr = SHA_MR_DUALBUFF;
+		if (dd->caps.has_dualbuff)
+			valmr |= SHA_MR_DUALBUFF;
 	} else {
 		atmel_sha_write(dd, SHA_IER, SHA_INT_DATARDY);
 	}
 
-	if (ctx->flags & SHA_FLAGS_SHA256)
+	if (ctx->flags & SHA_FLAGS_SHA1)
+		valmr |= SHA_MR_ALGO_SHA1;
+	else if (ctx->flags & SHA_FLAGS_SHA224)
+		valmr |= SHA_MR_ALGO_SHA224;
+	else if (ctx->flags & SHA_FLAGS_SHA256)
 		valmr |= SHA_MR_ALGO_SHA256;
+	else if (ctx->flags & SHA_FLAGS_SHA384)
+		valmr |= SHA_MR_ALGO_SHA384;
+	else if (ctx->flags & SHA_FLAGS_SHA512)
+		valmr |= SHA_MR_ALGO_SHA512;
 
 	/* Setting CR_FIRST only for the first iteration */
-	if (!ctx->digcnt)
+	if (!(ctx->digcnt[0] || ctx->digcnt[1]))
 		valcr = SHA_CR_FIRST;
 
 	atmel_sha_write(dd, SHA_CR, valcr);
@@ -275,13 +346,15 @@ static int atmel_sha_xmit_cpu(struct atmel_sha_dev *dd, const u8 *buf,
 	int count, len32;
 	const u32 *buffer = (const u32 *)buf;
 
-	dev_dbg(dd->dev, "xmit_cpu: digcnt: %d, length: %d, final: %d\n",
-						ctx->digcnt, length, final);
+	dev_dbg(dd->dev, "xmit_cpu: digcnt: 0x%llx 0x%llx, length: %d, final: %d\n",
+		ctx->digcnt[1], ctx->digcnt[0], length, final);
 
 	atmel_sha_write_ctrl(dd, 0);
 
 	/* should be non-zero before next lines to disable clocks later */
-	ctx->digcnt += length;
+	ctx->digcnt[0] += length;
+	if (ctx->digcnt[0] < length)
+		ctx->digcnt[1]++;
 
 	if (final)
 		dd->flags |= SHA_FLAGS_FINAL; /* catch last interrupt */
@@ -302,8 +375,8 @@ static int atmel_sha_xmit_pdc(struct atmel_sha_dev *dd, dma_addr_t dma_addr1,
 	struct atmel_sha_reqctx *ctx = ahash_request_ctx(dd->req);
 	int len32;
 
-	dev_dbg(dd->dev, "xmit_pdc: digcnt: %d, length: %d, final: %d\n",
-						ctx->digcnt, length1, final);
+	dev_dbg(dd->dev, "xmit_pdc: digcnt: 0x%llx 0x%llx, length: %d, final: %d\n",
+		ctx->digcnt[1], ctx->digcnt[0], length1, final);
 
 	len32 = DIV_ROUND_UP(length1, sizeof(u32));
 	atmel_sha_write(dd, SHA_PTCR, SHA_PTCR_TXTDIS);
@@ -317,7 +390,9 @@ static int atmel_sha_xmit_pdc(struct atmel_sha_dev *dd, dma_addr_t dma_addr1,
 	atmel_sha_write_ctrl(dd, 1);
 
 	/* should be non-zero before next lines to disable clocks later */
-	ctx->digcnt += length1;
+	ctx->digcnt[0] += length1;
+	if (ctx->digcnt[0] < length1)
+		ctx->digcnt[1]++;
 
 	if (final)
 		dd->flags |= SHA_FLAGS_FINAL; /* catch last interrupt */
@@ -330,6 +405,86 @@ static int atmel_sha_xmit_pdc(struct atmel_sha_dev *dd, dma_addr_t dma_addr1,
 	return -EINPROGRESS;
 }
 
+static void atmel_sha_dma_callback(void *data)
+{
+	struct atmel_sha_dev *dd = data;
+
+	/* dma_lch_in - completed - wait DATRDY */
+	atmel_sha_write(dd, SHA_IER, SHA_INT_DATARDY);
+}
+
+static int atmel_sha_xmit_dma(struct atmel_sha_dev *dd, dma_addr_t dma_addr1,
+		size_t length1, dma_addr_t dma_addr2, size_t length2, int final)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(dd->req);
+	struct dma_async_tx_descriptor	*in_desc;
+	struct scatterlist sg[2];
+
+	dev_dbg(dd->dev, "xmit_dma: digcnt: 0x%llx 0x%llx, length: %d, final: %d\n",
+		ctx->digcnt[1], ctx->digcnt[0], length1, final);
+
+	if (ctx->flags & (SHA_FLAGS_SHA1 | SHA_FLAGS_SHA224 |
+			SHA_FLAGS_SHA256)) {
+		dd->dma_lch_in.dma_conf.src_maxburst = 16;
+		dd->dma_lch_in.dma_conf.dst_maxburst = 16;
+	} else {
+		dd->dma_lch_in.dma_conf.src_maxburst = 32;
+		dd->dma_lch_in.dma_conf.dst_maxburst = 32;
+	}
+
+	dmaengine_slave_config(dd->dma_lch_in.chan, &dd->dma_lch_in.dma_conf);
+
+	if (length2) {
+		sg_init_table(sg, 2);
+		sg_dma_address(&sg[0]) = dma_addr1;
+		sg_dma_len(&sg[0]) = length1;
+		sg_dma_address(&sg[1]) = dma_addr2;
+		sg_dma_len(&sg[1]) = length2;
+		in_desc = dmaengine_prep_slave_sg(dd->dma_lch_in.chan, sg, 2,
+			DMA_MEM_TO_DEV, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	} else {
+		sg_init_table(sg, 1);
+		sg_dma_address(&sg[0]) = dma_addr1;
+		sg_dma_len(&sg[0]) = length1;
+		in_desc = dmaengine_prep_slave_sg(dd->dma_lch_in.chan, sg, 1,
+			DMA_MEM_TO_DEV, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	}
+	if (!in_desc)
+		return -EINVAL;
+
+	in_desc->callback = atmel_sha_dma_callback;
+	in_desc->callback_param = dd;
+
+	atmel_sha_write_ctrl(dd, 1);
+
+	/* should be non-zero before next lines to disable clocks later */
+	ctx->digcnt[0] += length1;
+	if (ctx->digcnt[0] < length1)
+		ctx->digcnt[1]++;
+
+	if (final)
+		dd->flags |= SHA_FLAGS_FINAL; /* catch last interrupt */
+
+	dd->flags |=  SHA_FLAGS_DMA_ACTIVE;
+
+	/* Start DMA transfer */
+	dmaengine_submit(in_desc);
+	dma_async_issue_pending(dd->dma_lch_in.chan);
+
+	return -EINPROGRESS;
+}
+
+static int atmel_sha_xmit_start(struct atmel_sha_dev *dd, dma_addr_t dma_addr1,
+		size_t length1, dma_addr_t dma_addr2, size_t length2, int final)
+{
+	if (dd->caps.has_dma)
+		return atmel_sha_xmit_dma(dd, dma_addr1, length1,
+				dma_addr2, length2, final);
+	else
+		return atmel_sha_xmit_pdc(dd, dma_addr1, length1,
+				dma_addr2, length2, final);
+}
+
 static int atmel_sha_update_cpu(struct atmel_sha_dev *dd)
 {
 	struct atmel_sha_reqctx *ctx = ahash_request_ctx(dd->req);
@@ -337,7 +492,6 @@ static int atmel_sha_update_cpu(struct atmel_sha_dev *dd)
 
 	atmel_sha_append_sg(ctx);
 	atmel_sha_fill_padding(ctx, 0);
-
 	bufcnt = ctx->bufcnt;
 	ctx->bufcnt = 0;
 
@@ -349,17 +503,17 @@ static int atmel_sha_xmit_dma_map(struct atmel_sha_dev *dd,
 					size_t length, int final)
 {
 	ctx->dma_addr = dma_map_single(dd->dev, ctx->buffer,
-				ctx->buflen + SHA1_BLOCK_SIZE, DMA_TO_DEVICE);
+				ctx->buflen + ctx->block_size, DMA_TO_DEVICE);
 	if (dma_mapping_error(dd->dev, ctx->dma_addr)) {
 		dev_err(dd->dev, "dma %u bytes error\n", ctx->buflen +
-				SHA1_BLOCK_SIZE);
+				ctx->block_size);
 		return -EINVAL;
 	}
 
 	ctx->flags &= ~SHA_FLAGS_SG;
 
 	/* next call does not fail... so no unmap in the case of error */
-	return atmel_sha_xmit_pdc(dd, ctx->dma_addr, length, 0, 0, final);
+	return atmel_sha_xmit_start(dd, ctx->dma_addr, length, 0, 0, final);
 }
 
 static int atmel_sha_update_dma_slow(struct atmel_sha_dev *dd)
@@ -372,8 +526,8 @@ static int atmel_sha_update_dma_slow(struct atmel_sha_dev *dd)
 
 	final = (ctx->flags & SHA_FLAGS_FINUP) && !ctx->total;
 
-	dev_dbg(dd->dev, "slow: bufcnt: %u, digcnt: %d, final: %d\n",
-					 ctx->bufcnt, ctx->digcnt, final);
+	dev_dbg(dd->dev, "slow: bufcnt: %u, digcnt: 0x%llx 0x%llx, final: %d\n",
+		 ctx->bufcnt, ctx->digcnt[1], ctx->digcnt[0], final);
 
 	if (final)
 		atmel_sha_fill_padding(ctx, 0);
@@ -400,30 +554,25 @@ static int atmel_sha_update_dma_start(struct atmel_sha_dev *dd)
 	if (ctx->bufcnt || ctx->offset)
 		return atmel_sha_update_dma_slow(dd);
 
-	dev_dbg(dd->dev, "fast: digcnt: %d, bufcnt: %u, total: %u\n",
-			ctx->digcnt, ctx->bufcnt, ctx->total);
+	dev_dbg(dd->dev, "fast: digcnt: 0x%llx 0x%llx, bufcnt: %u, total: %u\n",
+		ctx->digcnt[1], ctx->digcnt[0], ctx->bufcnt, ctx->total);
 
 	sg = ctx->sg;
 
 	if (!IS_ALIGNED(sg->offset, sizeof(u32)))
 		return atmel_sha_update_dma_slow(dd);
 
-	if (!sg_is_last(sg) && !IS_ALIGNED(sg->length, SHA1_BLOCK_SIZE))
-		/* size is not SHA1_BLOCK_SIZE aligned */
+	if (!sg_is_last(sg) && !IS_ALIGNED(sg->length, ctx->block_size))
+		/* size is not ctx->block_size aligned */
 		return atmel_sha_update_dma_slow(dd);
 
 	length = min(ctx->total, sg->length);
 
 	if (sg_is_last(sg)) {
 		if (!(ctx->flags & SHA_FLAGS_FINUP)) {
-			/* not last sg must be SHA1_BLOCK_SIZE aligned */
-			tail = length & (SHA1_BLOCK_SIZE - 1);
+			/* not last sg must be ctx->block_size aligned */
+			tail = length & (ctx->block_size - 1);
 			length -= tail;
-			if (length == 0) {
-				/* offset where to start slow */
-				ctx->offset = length;
-				return atmel_sha_update_dma_slow(dd);
-			}
 		}
 	}
 
@@ -434,7 +583,7 @@ static int atmel_sha_update_dma_start(struct atmel_sha_dev *dd)
 
 	/* Add padding */
 	if (final) {
-		tail = length & (SHA1_BLOCK_SIZE - 1);
+		tail = length & (ctx->block_size - 1);
 		length -= tail;
 		ctx->total += tail;
 		ctx->offset = length; /* offset where to start slow */
@@ -445,10 +594,10 @@ static int atmel_sha_update_dma_start(struct atmel_sha_dev *dd)
 		atmel_sha_fill_padding(ctx, length);
 
 		ctx->dma_addr = dma_map_single(dd->dev, ctx->buffer,
-			ctx->buflen + SHA1_BLOCK_SIZE, DMA_TO_DEVICE);
+			ctx->buflen + ctx->block_size, DMA_TO_DEVICE);
 		if (dma_mapping_error(dd->dev, ctx->dma_addr)) {
 			dev_err(dd->dev, "dma %u bytes error\n",
-				ctx->buflen + SHA1_BLOCK_SIZE);
+				ctx->buflen + ctx->block_size);
 			return -EINVAL;
 		}
 
@@ -456,7 +605,7 @@ static int atmel_sha_update_dma_start(struct atmel_sha_dev *dd)
 			ctx->flags &= ~SHA_FLAGS_SG;
 			count = ctx->bufcnt;
 			ctx->bufcnt = 0;
-			return atmel_sha_xmit_pdc(dd, ctx->dma_addr, count, 0,
+			return atmel_sha_xmit_start(dd, ctx->dma_addr, count, 0,
 					0, final);
 		} else {
 			ctx->sg = sg;
@@ -470,7 +619,7 @@ static int atmel_sha_update_dma_start(struct atmel_sha_dev *dd)
 
 			count = ctx->bufcnt;
 			ctx->bufcnt = 0;
-			return atmel_sha_xmit_pdc(dd, sg_dma_address(ctx->sg),
+			return atmel_sha_xmit_start(dd, sg_dma_address(ctx->sg),
 					length, ctx->dma_addr, count, final);
 		}
 	}
@@ -483,7 +632,7 @@ static int atmel_sha_update_dma_start(struct atmel_sha_dev *dd)
 	ctx->flags |= SHA_FLAGS_SG;
 
 	/* next call does not fail... so no unmap in the case of error */
-	return atmel_sha_xmit_pdc(dd, sg_dma_address(ctx->sg), length, 0,
+	return atmel_sha_xmit_start(dd, sg_dma_address(ctx->sg), length, 0,
 								0, final);
 }
 
@@ -498,12 +647,13 @@ static int atmel_sha_update_dma_stop(struct atmel_sha_dev *dd)
 			if (ctx->sg)
 				ctx->offset = 0;
 		}
-		if (ctx->flags & SHA_FLAGS_PAD)
+		if (ctx->flags & SHA_FLAGS_PAD) {
 			dma_unmap_single(dd->dev, ctx->dma_addr,
-				ctx->buflen + SHA1_BLOCK_SIZE, DMA_TO_DEVICE);
+				ctx->buflen + ctx->block_size, DMA_TO_DEVICE);
+		}
 	} else {
 		dma_unmap_single(dd->dev, ctx->dma_addr, ctx->buflen +
-						SHA1_BLOCK_SIZE, DMA_TO_DEVICE);
+						ctx->block_size, DMA_TO_DEVICE);
 	}
 
 	return 0;
@@ -515,8 +665,8 @@ static int atmel_sha_update_req(struct atmel_sha_dev *dd)
 	struct atmel_sha_reqctx *ctx = ahash_request_ctx(req);
 	int err;
 
-	dev_dbg(dd->dev, "update_req: total: %u, digcnt: %d, finup: %d\n",
-		 ctx->total, ctx->digcnt, (ctx->flags & SHA_FLAGS_FINUP) != 0);
+	dev_dbg(dd->dev, "update_req: total: %u, digcnt: 0x%llx 0x%llx\n",
+		ctx->total, ctx->digcnt[1], ctx->digcnt[0]);
 
 	if (ctx->flags & SHA_FLAGS_CPU)
 		err = atmel_sha_update_cpu(dd);
@@ -524,8 +674,8 @@ static int atmel_sha_update_req(struct atmel_sha_dev *dd)
 		err = atmel_sha_update_dma_start(dd);
 
 	/* wait for dma completion before can take more data */
-	dev_dbg(dd->dev, "update: err: %d, digcnt: %d\n",
-			err, ctx->digcnt);
+	dev_dbg(dd->dev, "update: err: %d, digcnt: 0x%llx 0%llx\n",
+			err, ctx->digcnt[1], ctx->digcnt[0]);
 
 	return err;
 }
@@ -562,12 +712,21 @@ static void atmel_sha_copy_hash(struct ahash_request *req)
 	u32 *hash = (u32 *)ctx->digest;
 	int i;
 
-	if (likely(ctx->flags & SHA_FLAGS_SHA1))
+	if (ctx->flags & SHA_FLAGS_SHA1)
 		for (i = 0; i < SHA1_DIGEST_SIZE / sizeof(u32); i++)
 			hash[i] = atmel_sha_read(ctx->dd, SHA_REG_DIGEST(i));
-	else
+	else if (ctx->flags & SHA_FLAGS_SHA224)
+		for (i = 0; i < SHA224_DIGEST_SIZE / sizeof(u32); i++)
+			hash[i] = atmel_sha_read(ctx->dd, SHA_REG_DIGEST(i));
+	else if (ctx->flags & SHA_FLAGS_SHA256)
 		for (i = 0; i < SHA256_DIGEST_SIZE / sizeof(u32); i++)
 			hash[i] = atmel_sha_read(ctx->dd, SHA_REG_DIGEST(i));
+	else if (ctx->flags & SHA_FLAGS_SHA384)
+		for (i = 0; i < SHA384_DIGEST_SIZE / sizeof(u32); i++)
+			hash[i] = atmel_sha_read(ctx->dd, SHA_REG_DIGEST(i));
+	else
+		for (i = 0; i < SHA512_DIGEST_SIZE / sizeof(u32); i++)
+			hash[i] = atmel_sha_read(ctx->dd, SHA_REG_DIGEST(i));
 }
 
 static void atmel_sha_copy_ready_hash(struct ahash_request *req)
@@ -577,10 +736,16 @@ static void atmel_sha_copy_ready_hash(struct ahash_request *req)
 	if (!req->result)
 		return;
 
-	if (likely(ctx->flags & SHA_FLAGS_SHA1))
+	if (ctx->flags & SHA_FLAGS_SHA1)
 		memcpy(req->result, ctx->digest, SHA1_DIGEST_SIZE);
-	else
+	else if (ctx->flags & SHA_FLAGS_SHA224)
+		memcpy(req->result, ctx->digest, SHA224_DIGEST_SIZE);
+	else if (ctx->flags & SHA_FLAGS_SHA256)
 		memcpy(req->result, ctx->digest, SHA256_DIGEST_SIZE);
+	else if (ctx->flags & SHA_FLAGS_SHA384)
+		memcpy(req->result, ctx->digest, SHA384_DIGEST_SIZE);
+	else
+		memcpy(req->result, ctx->digest, SHA512_DIGEST_SIZE);
 }
 
 static int atmel_sha_finish(struct ahash_request *req)
@@ -589,11 +754,11 @@ static int atmel_sha_finish(struct ahash_request *req)
 	struct atmel_sha_dev *dd = ctx->dd;
 	int err = 0;
 
-	if (ctx->digcnt)
+	if (ctx->digcnt[0] || ctx->digcnt[1])
 		atmel_sha_copy_ready_hash(req);
 
-	dev_dbg(dd->dev, "digcnt: %d, bufcnt: %d\n", ctx->digcnt,
-		ctx->bufcnt);
+	dev_dbg(dd->dev, "digcnt: 0x%llx 0x%llx, bufcnt: %d\n", ctx->digcnt[1],
+		ctx->digcnt[0], ctx->bufcnt);
 
 	return err;
 }
@@ -628,9 +793,8 @@ static int atmel_sha_hw_init(struct atmel_sha_dev *dd)
 {
 	clk_prepare_enable(dd->iclk);
 
-	if (SHA_FLAGS_INIT & dd->flags) {
+	if (!(SHA_FLAGS_INIT & dd->flags)) {
 		atmel_sha_write(dd, SHA_CR, SHA_CR_SWRST);
-		atmel_sha_dualbuff_test(dd);
 		dd->flags |= SHA_FLAGS_INIT;
 		dd->err = 0;
 	}
@@ -638,6 +802,23 @@ static int atmel_sha_hw_init(struct atmel_sha_dev *dd)
 	return 0;
 }
 
+static inline unsigned int atmel_sha_get_version(struct atmel_sha_dev *dd)
+{
+	return atmel_sha_read(dd, SHA_HW_VERSION) & 0x00000fff;
+}
+
+static void atmel_sha_hw_version_init(struct atmel_sha_dev *dd)
+{
+	atmel_sha_hw_init(dd);
+
+	dd->hw_version = atmel_sha_get_version(dd);
+
+	dev_info(dd->dev,
+			"version: 0x%x\n", dd->hw_version);
+
+	clk_disable_unprepare(dd->iclk);
+}
+
 static int atmel_sha_handle_queue(struct atmel_sha_dev *dd,
 				  struct ahash_request *req)
 {
@@ -682,10 +863,9 @@ static int atmel_sha_handle_queue(struct atmel_sha_dev *dd,
 
 	if (ctx->op == SHA_OP_UPDATE) {
 		err = atmel_sha_update_req(dd);
-		if (err != -EINPROGRESS && (ctx->flags & SHA_FLAGS_FINUP)) {
+		if (err != -EINPROGRESS && (ctx->flags & SHA_FLAGS_FINUP))
 			/* no final() after finup() */
 			err = atmel_sha_final_req(dd);
-		}
 	} else if (ctx->op == SHA_OP_FINAL) {
 		err = atmel_sha_final_req(dd);
 	}
@@ -808,7 +988,7 @@ static int atmel_sha_cra_init_alg(struct crypto_tfm *tfm, const char *alg_base)
 	}
 	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
 				 sizeof(struct atmel_sha_reqctx) +
-				 SHA_BUFFER_LEN + SHA256_BLOCK_SIZE);
+				 SHA_BUFFER_LEN + SHA512_BLOCK_SIZE);
 
 	return 0;
 }
@@ -826,7 +1006,7 @@ static void atmel_sha_cra_exit(struct crypto_tfm *tfm)
 	tctx->fallback = NULL;
 }
 
-static struct ahash_alg sha_algs[] = {
+static struct ahash_alg sha_1_256_algs[] = {
 {
 	.init		= atmel_sha_init,
 	.update		= atmel_sha_update,
@@ -875,6 +1055,79 @@ static struct ahash_alg sha_algs[] = {
 },
 };
 
+static struct ahash_alg sha_224_alg = {
+	.init		= atmel_sha_init,
+	.update		= atmel_sha_update,
+	.final		= atmel_sha_final,
+	.finup		= atmel_sha_finup,
+	.digest		= atmel_sha_digest,
+	.halg = {
+		.digestsize	= SHA224_DIGEST_SIZE,
+		.base	= {
+			.cra_name		= "sha224",
+			.cra_driver_name	= "atmel-sha224",
+			.cra_priority		= 100,
+			.cra_flags		= CRYPTO_ALG_ASYNC |
+						CRYPTO_ALG_NEED_FALLBACK,
+			.cra_blocksize		= SHA224_BLOCK_SIZE,
+			.cra_ctxsize		= sizeof(struct atmel_sha_ctx),
+			.cra_alignmask		= 0,
+			.cra_module		= THIS_MODULE,
+			.cra_init		= atmel_sha_cra_init,
+			.cra_exit		= atmel_sha_cra_exit,
+		}
+	}
+};
+
+static struct ahash_alg sha_384_512_algs[] = {
+{
+	.init		= atmel_sha_init,
+	.update		= atmel_sha_update,
+	.final		= atmel_sha_final,
+	.finup		= atmel_sha_finup,
+	.digest		= atmel_sha_digest,
+	.halg = {
+		.digestsize	= SHA384_DIGEST_SIZE,
+		.base	= {
+			.cra_name		= "sha384",
+			.cra_driver_name	= "atmel-sha384",
+			.cra_priority		= 100,
+			.cra_flags		= CRYPTO_ALG_ASYNC |
+						CRYPTO_ALG_NEED_FALLBACK,
+			.cra_blocksize		= SHA384_BLOCK_SIZE,
+			.cra_ctxsize		= sizeof(struct atmel_sha_ctx),
+			.cra_alignmask		= 0x3,
+			.cra_module		= THIS_MODULE,
+			.cra_init		= atmel_sha_cra_init,
+			.cra_exit		= atmel_sha_cra_exit,
+		}
+	}
+},
+{
+	.init		= atmel_sha_init,
+	.update		= atmel_sha_update,
+	.final		= atmel_sha_final,
+	.finup		= atmel_sha_finup,
+	.digest		= atmel_sha_digest,
+	.halg = {
+		.digestsize	= SHA512_DIGEST_SIZE,
+		.base	= {
+			.cra_name		= "sha512",
+			.cra_driver_name	= "atmel-sha512",
+			.cra_priority		= 100,
+			.cra_flags		= CRYPTO_ALG_ASYNC |
+						CRYPTO_ALG_NEED_FALLBACK,
+			.cra_blocksize		= SHA512_BLOCK_SIZE,
+			.cra_ctxsize		= sizeof(struct atmel_sha_ctx),
+			.cra_alignmask		= 0x3,
+			.cra_module		= THIS_MODULE,
+			.cra_init		= atmel_sha_cra_init,
+			.cra_exit		= atmel_sha_cra_exit,
+		}
+	}
+},
+};
+
 static void atmel_sha_done_task(unsigned long data)
 {
 	struct atmel_sha_dev *dd = (struct atmel_sha_dev *)data;
@@ -941,32 +1194,142 @@ static void atmel_sha_unregister_algs(struct atmel_sha_dev *dd)
 {
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(sha_algs); i++)
-		crypto_unregister_ahash(&sha_algs[i]);
+	for (i = 0; i < ARRAY_SIZE(sha_1_256_algs); i++)
+		crypto_unregister_ahash(&sha_1_256_algs[i]);
+
+	if (dd->caps.has_sha224)
+		crypto_unregister_ahash(&sha_224_alg);
+
+	if (dd->caps.has_sha_384_512) {
+		for (i = 0; i < ARRAY_SIZE(sha_384_512_algs); i++)
+			crypto_unregister_ahash(&sha_384_512_algs[i]);
+	}
 }
 
 static int atmel_sha_register_algs(struct atmel_sha_dev *dd)
 {
 	int err, i, j;
 
-	for (i = 0; i < ARRAY_SIZE(sha_algs); i++) {
-		err = crypto_register_ahash(&sha_algs[i]);
+	for (i = 0; i < ARRAY_SIZE(sha_1_256_algs); i++) {
+		err = crypto_register_ahash(&sha_1_256_algs[i]);
 		if (err)
-			goto err_sha_algs;
+			goto err_sha_1_256_algs;
+	}
+
+	if (dd->caps.has_sha224) {
+		err = crypto_register_ahash(&sha_224_alg);
+		if (err)
+			goto err_sha_224_algs;
+	}
+
+	if (dd->caps.has_sha_384_512) {
+		for (i = 0; i < ARRAY_SIZE(sha_384_512_algs); i++) {
+			err = crypto_register_ahash(&sha_384_512_algs[i]);
+			if (err)
+				goto err_sha_384_512_algs;
+		}
 	}
 
 	return 0;
 
-err_sha_algs:
+err_sha_384_512_algs:
+	for (j = 0; j < i; j++)
+		crypto_unregister_ahash(&sha_384_512_algs[j]);
+	crypto_unregister_ahash(&sha_224_alg);
+err_sha_224_algs:
+	i = ARRAY_SIZE(sha_1_256_algs);
+err_sha_1_256_algs:
 	for (j = 0; j < i; j++)
-		crypto_unregister_ahash(&sha_algs[j]);
+		crypto_unregister_ahash(&sha_1_256_algs[j]);
 
 	return err;
 }
 
+static bool atmel_sha_filter(struct dma_chan *chan, void *slave)
+{
+	struct at_dma_slave	*sl = slave;
+
+	if (sl && sl->dma_dev == chan->device->dev) {
+		chan->private = sl;
+		return true;
+	} else {
+		return false;
+	}
+}
+
+static int atmel_sha_dma_init(struct atmel_sha_dev *dd,
+				struct crypto_platform_data *pdata)
+{
+	int err = -ENOMEM;
+	dma_cap_mask_t mask_in;
+
+	if (pdata && pdata->dma_slave->rxdata.dma_dev) {
+		/* Try to grab DMA channel */
+		dma_cap_zero(mask_in);
+		dma_cap_set(DMA_SLAVE, mask_in);
+
+		dd->dma_lch_in.chan = dma_request_channel(mask_in,
+				atmel_sha_filter, &pdata->dma_slave->rxdata);
+
+		if (!dd->dma_lch_in.chan)
+			return err;
+
+		dd->dma_lch_in.dma_conf.direction = DMA_MEM_TO_DEV;
+		dd->dma_lch_in.dma_conf.dst_addr = dd->phys_base +
+			SHA_REG_DIN(0);
+		dd->dma_lch_in.dma_conf.src_maxburst = 1;
+		dd->dma_lch_in.dma_conf.src_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
+		dd->dma_lch_in.dma_conf.dst_maxburst = 1;
+		dd->dma_lch_in.dma_conf.dst_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
+		dd->dma_lch_in.dma_conf.device_fc = false;
+
+		return 0;
+	}
+
+	return -ENODEV;
+}
+
+static void atmel_sha_dma_cleanup(struct atmel_sha_dev *dd)
+{
+	dma_release_channel(dd->dma_lch_in.chan);
+}
+
+static void atmel_sha_get_cap(struct atmel_sha_dev *dd)
+{
+
+	dd->caps.has_dma = 0;
+	dd->caps.has_dualbuff = 0;
+	dd->caps.has_sha224 = 0;
+	dd->caps.has_sha_384_512 = 0;
+
+	/* keep only major version number */
+	switch (dd->hw_version & 0xff0) {
+	case 0x410:
+		dd->caps.has_dma = 1;
+		dd->caps.has_dualbuff = 1;
+		dd->caps.has_sha224 = 1;
+		dd->caps.has_sha_384_512 = 1;
+		break;
+	case 0x400:
+		dd->caps.has_dma = 1;
+		dd->caps.has_dualbuff = 1;
+		dd->caps.has_sha224 = 1;
+		break;
+	case 0x320:
+		break;
+	default:
+		dev_warn(dd->dev,
+				"Unmanaged sha version, set minimum capabilities\n");
+		break;
+	}
+}
+
 static int atmel_sha_probe(struct platform_device *pdev)
 {
 	struct atmel_sha_dev *sha_dd;
+	struct crypto_platform_data	*pdata;
 	struct device *dev = &pdev->dev;
 	struct resource *sha_res;
 	unsigned long sha_phys_size;
@@ -1018,7 +1381,7 @@ static int atmel_sha_probe(struct platform_device *pdev)
 	}
 
 	/* Initializing the clock */
-	sha_dd->iclk = clk_get(&pdev->dev, NULL);
+	sha_dd->iclk = clk_get(&pdev->dev, "sha_clk");
 	if (IS_ERR(sha_dd->iclk)) {
 		dev_err(dev, "clock intialization failed.\n");
 		err = PTR_ERR(sha_dd->iclk);
@@ -1032,6 +1395,22 @@ static int atmel_sha_probe(struct platform_device *pdev)
 		goto sha_io_err;
 	}
 
+	atmel_sha_hw_version_init(sha_dd);
+
+	atmel_sha_get_cap(sha_dd);
+
+	if (sha_dd->caps.has_dma) {
+		pdata = pdev->dev.platform_data;
+		if (!pdata) {
+			dev_err(&pdev->dev, "platform data not available\n");
+			err = -ENXIO;
+			goto err_pdata;
+		}
+		err = atmel_sha_dma_init(sha_dd, pdata);
+		if (err)
+			goto err_sha_dma;
+	}
+
 	spin_lock(&atmel_sha.lock);
 	list_add_tail(&sha_dd->list, &atmel_sha.dev_list);
 	spin_unlock(&atmel_sha.lock);
@@ -1048,6 +1427,10 @@ err_algs:
 	spin_lock(&atmel_sha.lock);
 	list_del(&sha_dd->list);
 	spin_unlock(&atmel_sha.lock);
+	if (sha_dd->caps.has_dma)
+		atmel_sha_dma_cleanup(sha_dd);
+err_sha_dma:
+err_pdata:
 	iounmap(sha_dd->io_base);
 sha_io_err:
 	clk_put(sha_dd->iclk);
@@ -1078,6 +1461,9 @@ static int atmel_sha_remove(struct platform_device *pdev)
 
 	tasklet_kill(&sha_dd->done_task);
 
+	if (sha_dd->caps.has_dma)
+		atmel_sha_dma_cleanup(sha_dd);
+
 	iounmap(sha_dd->io_base);
 
 	clk_put(sha_dd->iclk);
@@ -1102,6 +1488,6 @@ static struct platform_driver atmel_sha_driver = {
 
 module_platform_driver(atmel_sha_driver);
 
-MODULE_DESCRIPTION("Atmel SHA1/SHA256 hw acceleration support.");
+MODULE_DESCRIPTION("Atmel SHA (1/256/224/384/512) hw acceleration support.");
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Nicolas Royer - Eukréa Electromatique");
diff --git a/drivers/crypto/atmel-tdes-regs.h b/drivers/crypto/atmel-tdes-regs.h
index 5ac2a900d80c..f86734d0fda4 100644
--- a/drivers/crypto/atmel-tdes-regs.h
+++ b/drivers/crypto/atmel-tdes-regs.h
@@ -69,6 +69,8 @@
 #define	TDES_XTEARNDR_XTEA_RNDS_MASK	(0x3F << 0)
 #define	TDES_XTEARNDR_XTEA_RNDS_OFFSET	0
 
+#define	TDES_HW_VERSION	0xFC
+
 #define TDES_RPR		0x100
 #define TDES_RCR		0x104
 #define TDES_TPR		0x108
diff --git a/drivers/crypto/atmel-tdes.c b/drivers/crypto/atmel-tdes.c
index 7c73fbb17538..4a99564a08e6 100644
--- a/drivers/crypto/atmel-tdes.c
+++ b/drivers/crypto/atmel-tdes.c
@@ -38,29 +38,35 @@
 #include <crypto/des.h>
 #include <crypto/hash.h>
 #include <crypto/internal/hash.h>
+#include <linux/platform_data/crypto-atmel.h>
 #include "atmel-tdes-regs.h"
 
 /* TDES flags  */
-#define TDES_FLAGS_MODE_MASK		0x007f
+#define TDES_FLAGS_MODE_MASK		0x00ff
 #define TDES_FLAGS_ENCRYPT	BIT(0)
 #define TDES_FLAGS_CBC		BIT(1)
 #define TDES_FLAGS_CFB		BIT(2)
 #define TDES_FLAGS_CFB8		BIT(3)
 #define TDES_FLAGS_CFB16	BIT(4)
 #define TDES_FLAGS_CFB32	BIT(5)
-#define TDES_FLAGS_OFB		BIT(6)
+#define TDES_FLAGS_CFB64	BIT(6)
+#define TDES_FLAGS_OFB		BIT(7)
 
 #define TDES_FLAGS_INIT		BIT(16)
 #define TDES_FLAGS_FAST		BIT(17)
 #define TDES_FLAGS_BUSY		BIT(18)
+#define TDES_FLAGS_DMA		BIT(19)
 
-#define ATMEL_TDES_QUEUE_LENGTH	1
+#define ATMEL_TDES_QUEUE_LENGTH	50
 
 #define CFB8_BLOCK_SIZE		1
 #define CFB16_BLOCK_SIZE	2
 #define CFB32_BLOCK_SIZE	4
-#define CFB64_BLOCK_SIZE	8
 
+struct atmel_tdes_caps {
+	bool	has_dma;
+	u32		has_cfb_3keys;
+};
 
 struct atmel_tdes_dev;
 
@@ -70,12 +76,19 @@ struct atmel_tdes_ctx {
 	int		keylen;
 	u32		key[3*DES_KEY_SIZE / sizeof(u32)];
 	unsigned long	flags;
+
+	u16		block_size;
 };
 
 struct atmel_tdes_reqctx {
 	unsigned long mode;
 };
 
+struct atmel_tdes_dma {
+	struct dma_chan			*chan;
+	struct dma_slave_config dma_conf;
+};
+
 struct atmel_tdes_dev {
 	struct list_head	list;
 	unsigned long		phys_base;
@@ -99,8 +112,10 @@ struct atmel_tdes_dev {
 	size_t				total;
 
 	struct scatterlist	*in_sg;
+	unsigned int		nb_in_sg;
 	size_t				in_offset;
 	struct scatterlist	*out_sg;
+	unsigned int		nb_out_sg;
 	size_t				out_offset;
 
 	size_t	buflen;
@@ -109,10 +124,16 @@ struct atmel_tdes_dev {
 	void	*buf_in;
 	int		dma_in;
 	dma_addr_t	dma_addr_in;
+	struct atmel_tdes_dma	dma_lch_in;
 
 	void	*buf_out;
 	int		dma_out;
 	dma_addr_t	dma_addr_out;
+	struct atmel_tdes_dma	dma_lch_out;
+
+	struct atmel_tdes_caps	caps;
+
+	u32	hw_version;
 };
 
 struct atmel_tdes_drv {
@@ -207,6 +228,31 @@ static int atmel_tdes_hw_init(struct atmel_tdes_dev *dd)
 	return 0;
 }
 
+static inline unsigned int atmel_tdes_get_version(struct atmel_tdes_dev *dd)
+{
+	return atmel_tdes_read(dd, TDES_HW_VERSION) & 0x00000fff;
+}
+
+static void atmel_tdes_hw_version_init(struct atmel_tdes_dev *dd)
+{
+	atmel_tdes_hw_init(dd);
+
+	dd->hw_version = atmel_tdes_get_version(dd);
+
+	dev_info(dd->dev,
+			"version: 0x%x\n", dd->hw_version);
+
+	clk_disable_unprepare(dd->iclk);
+}
+
+static void atmel_tdes_dma_callback(void *data)
+{
+	struct atmel_tdes_dev *dd = data;
+
+	/* dma_lch_out - completed */
+	tasklet_schedule(&dd->done_task);
+}
+
 static int atmel_tdes_write_ctrl(struct atmel_tdes_dev *dd)
 {
 	int err;
@@ -217,7 +263,9 @@ static int atmel_tdes_write_ctrl(struct atmel_tdes_dev *dd)
 	if (err)
 		return err;
 
-	atmel_tdes_write(dd, TDES_PTCR, TDES_PTCR_TXTDIS|TDES_PTCR_RXTDIS);
+	if (!dd->caps.has_dma)
+		atmel_tdes_write(dd, TDES_PTCR,
+			TDES_PTCR_TXTDIS | TDES_PTCR_RXTDIS);
 
 	/* MR register must be set before IV registers */
 	if (dd->ctx->keylen > (DES_KEY_SIZE << 1)) {
@@ -241,6 +289,8 @@ static int atmel_tdes_write_ctrl(struct atmel_tdes_dev *dd)
 			valmr |= TDES_MR_CFBS_16b;
 		else if (dd->flags & TDES_FLAGS_CFB32)
 			valmr |= TDES_MR_CFBS_32b;
+		else if (dd->flags & TDES_FLAGS_CFB64)
+			valmr |= TDES_MR_CFBS_64b;
 	} else if (dd->flags & TDES_FLAGS_OFB) {
 		valmr |= TDES_MR_OPMOD_OFB;
 	}
@@ -262,7 +312,7 @@ static int atmel_tdes_write_ctrl(struct atmel_tdes_dev *dd)
 	return 0;
 }
 
-static int atmel_tdes_crypt_dma_stop(struct atmel_tdes_dev *dd)
+static int atmel_tdes_crypt_pdc_stop(struct atmel_tdes_dev *dd)
 {
 	int err = 0;
 	size_t count;
@@ -288,7 +338,7 @@ static int atmel_tdes_crypt_dma_stop(struct atmel_tdes_dev *dd)
 	return err;
 }
 
-static int atmel_tdes_dma_init(struct atmel_tdes_dev *dd)
+static int atmel_tdes_buff_init(struct atmel_tdes_dev *dd)
 {
 	int err = -ENOMEM;
 
@@ -333,7 +383,7 @@ err_alloc:
 	return err;
 }
 
-static void atmel_tdes_dma_cleanup(struct atmel_tdes_dev *dd)
+static void atmel_tdes_buff_cleanup(struct atmel_tdes_dev *dd)
 {
 	dma_unmap_single(dd->dev, dd->dma_addr_out, dd->buflen,
 			 DMA_FROM_DEVICE);
@@ -343,7 +393,7 @@ static void atmel_tdes_dma_cleanup(struct atmel_tdes_dev *dd)
 	free_page((unsigned long)dd->buf_in);
 }
 
-static int atmel_tdes_crypt_dma(struct crypto_tfm *tfm, dma_addr_t dma_addr_in,
+static int atmel_tdes_crypt_pdc(struct crypto_tfm *tfm, dma_addr_t dma_addr_in,
 			       dma_addr_t dma_addr_out, int length)
 {
 	struct atmel_tdes_ctx *ctx = crypto_tfm_ctx(tfm);
@@ -379,7 +429,76 @@ static int atmel_tdes_crypt_dma(struct crypto_tfm *tfm, dma_addr_t dma_addr_in,
 	return 0;
 }
 
-static int atmel_tdes_crypt_dma_start(struct atmel_tdes_dev *dd)
+static int atmel_tdes_crypt_dma(struct crypto_tfm *tfm, dma_addr_t dma_addr_in,
+			       dma_addr_t dma_addr_out, int length)
+{
+	struct atmel_tdes_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct atmel_tdes_dev *dd = ctx->dd;
+	struct scatterlist sg[2];
+	struct dma_async_tx_descriptor	*in_desc, *out_desc;
+
+	dd->dma_size = length;
+
+	if (!(dd->flags & TDES_FLAGS_FAST)) {
+		dma_sync_single_for_device(dd->dev, dma_addr_in, length,
+					   DMA_TO_DEVICE);
+	}
+
+	if (dd->flags & TDES_FLAGS_CFB8) {
+		dd->dma_lch_in.dma_conf.dst_addr_width =
+			DMA_SLAVE_BUSWIDTH_1_BYTE;
+		dd->dma_lch_out.dma_conf.src_addr_width =
+			DMA_SLAVE_BUSWIDTH_1_BYTE;
+	} else if (dd->flags & TDES_FLAGS_CFB16) {
+		dd->dma_lch_in.dma_conf.dst_addr_width =
+			DMA_SLAVE_BUSWIDTH_2_BYTES;
+		dd->dma_lch_out.dma_conf.src_addr_width =
+			DMA_SLAVE_BUSWIDTH_2_BYTES;
+	} else {
+		dd->dma_lch_in.dma_conf.dst_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
+		dd->dma_lch_out.dma_conf.src_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
+	}
+
+	dmaengine_slave_config(dd->dma_lch_in.chan, &dd->dma_lch_in.dma_conf);
+	dmaengine_slave_config(dd->dma_lch_out.chan, &dd->dma_lch_out.dma_conf);
+
+	dd->flags |= TDES_FLAGS_DMA;
+
+	sg_init_table(&sg[0], 1);
+	sg_dma_address(&sg[0]) = dma_addr_in;
+	sg_dma_len(&sg[0]) = length;
+
+	sg_init_table(&sg[1], 1);
+	sg_dma_address(&sg[1]) = dma_addr_out;
+	sg_dma_len(&sg[1]) = length;
+
+	in_desc = dmaengine_prep_slave_sg(dd->dma_lch_in.chan, &sg[0],
+				1, DMA_MEM_TO_DEV,
+				DMA_PREP_INTERRUPT  |  DMA_CTRL_ACK);
+	if (!in_desc)
+		return -EINVAL;
+
+	out_desc = dmaengine_prep_slave_sg(dd->dma_lch_out.chan, &sg[1],
+				1, DMA_DEV_TO_MEM,
+				DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!out_desc)
+		return -EINVAL;
+
+	out_desc->callback = atmel_tdes_dma_callback;
+	out_desc->callback_param = dd;
+
+	dmaengine_submit(out_desc);
+	dma_async_issue_pending(dd->dma_lch_out.chan);
+
+	dmaengine_submit(in_desc);
+	dma_async_issue_pending(dd->dma_lch_in.chan);
+
+	return 0;
+}
+
+static int atmel_tdes_crypt_start(struct atmel_tdes_dev *dd)
 {
 	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(
 					crypto_ablkcipher_reqtfm(dd->req));
@@ -387,23 +506,23 @@ static int atmel_tdes_crypt_dma_start(struct atmel_tdes_dev *dd)
 	size_t count;
 	dma_addr_t addr_in, addr_out;
 
-	if (sg_is_last(dd->in_sg) && sg_is_last(dd->out_sg)) {
+	if ((!dd->in_offset) && (!dd->out_offset)) {
 		/* check for alignment */
-		in = IS_ALIGNED((u32)dd->in_sg->offset, sizeof(u32));
-		out = IS_ALIGNED((u32)dd->out_sg->offset, sizeof(u32));
-
+		in = IS_ALIGNED((u32)dd->in_sg->offset, sizeof(u32)) &&
+			IS_ALIGNED(dd->in_sg->length, dd->ctx->block_size);
+		out = IS_ALIGNED((u32)dd->out_sg->offset, sizeof(u32)) &&
+			IS_ALIGNED(dd->out_sg->length, dd->ctx->block_size);
 		fast = in && out;
+
+		if (sg_dma_len(dd->in_sg) != sg_dma_len(dd->out_sg))
+			fast = 0;
 	}
 
+
 	if (fast)  {
 		count = min(dd->total, sg_dma_len(dd->in_sg));
 		count = min(count, sg_dma_len(dd->out_sg));
 
-		if (count != dd->total) {
-			pr_err("request length != buffer length\n");
-			return -EINVAL;
-		}
-
 		err = dma_map_sg(dd->dev, dd->in_sg, 1, DMA_TO_DEVICE);
 		if (!err) {
 			dev_err(dd->dev, "dma_map_sg() error\n");
@@ -433,13 +552,16 @@ static int atmel_tdes_crypt_dma_start(struct atmel_tdes_dev *dd)
 		addr_out = dd->dma_addr_out;
 
 		dd->flags &= ~TDES_FLAGS_FAST;
-
 	}
 
 	dd->total -= count;
 
-	err = atmel_tdes_crypt_dma(tfm, addr_in, addr_out, count);
-	if (err) {
+	if (dd->caps.has_dma)
+		err = atmel_tdes_crypt_dma(tfm, addr_in, addr_out, count);
+	else
+		err = atmel_tdes_crypt_pdc(tfm, addr_in, addr_out, count);
+
+	if (err && (dd->flags & TDES_FLAGS_FAST)) {
 		dma_unmap_sg(dd->dev, dd->in_sg, 1, DMA_TO_DEVICE);
 		dma_unmap_sg(dd->dev, dd->out_sg, 1, DMA_TO_DEVICE);
 	}
@@ -447,7 +569,6 @@ static int atmel_tdes_crypt_dma_start(struct atmel_tdes_dev *dd)
 	return err;
 }
 
-
 static void atmel_tdes_finish_req(struct atmel_tdes_dev *dd, int err)
 {
 	struct ablkcipher_request *req = dd->req;
@@ -506,7 +627,7 @@ static int atmel_tdes_handle_queue(struct atmel_tdes_dev *dd,
 
 	err = atmel_tdes_write_ctrl(dd);
 	if (!err)
-		err = atmel_tdes_crypt_dma_start(dd);
+		err = atmel_tdes_crypt_start(dd);
 	if (err) {
 		/* des_task will not finish it, so do it here */
 		atmel_tdes_finish_req(dd, err);
@@ -516,41 +637,145 @@ static int atmel_tdes_handle_queue(struct atmel_tdes_dev *dd,
 	return ret;
 }
 
+static int atmel_tdes_crypt_dma_stop(struct atmel_tdes_dev *dd)
+{
+	int err = -EINVAL;
+	size_t count;
+
+	if (dd->flags & TDES_FLAGS_DMA) {
+		err = 0;
+		if  (dd->flags & TDES_FLAGS_FAST) {
+			dma_unmap_sg(dd->dev, dd->out_sg, 1, DMA_FROM_DEVICE);
+			dma_unmap_sg(dd->dev, dd->in_sg, 1, DMA_TO_DEVICE);
+		} else {
+			dma_sync_single_for_device(dd->dev, dd->dma_addr_out,
+				dd->dma_size, DMA_FROM_DEVICE);
+
+			/* copy data */
+			count = atmel_tdes_sg_copy(&dd->out_sg, &dd->out_offset,
+				dd->buf_out, dd->buflen, dd->dma_size, 1);
+			if (count != dd->dma_size) {
+				err = -EINVAL;
+				pr_err("not all data converted: %u\n", count);
+			}
+		}
+	}
+	return err;
+}
 
 static int atmel_tdes_crypt(struct ablkcipher_request *req, unsigned long mode)
 {
 	struct atmel_tdes_ctx *ctx = crypto_ablkcipher_ctx(
 			crypto_ablkcipher_reqtfm(req));
 	struct atmel_tdes_reqctx *rctx = ablkcipher_request_ctx(req);
-	struct atmel_tdes_dev *dd;
 
 	if (mode & TDES_FLAGS_CFB8) {
 		if (!IS_ALIGNED(req->nbytes, CFB8_BLOCK_SIZE)) {
 			pr_err("request size is not exact amount of CFB8 blocks\n");
 			return -EINVAL;
 		}
+		ctx->block_size = CFB8_BLOCK_SIZE;
 	} else if (mode & TDES_FLAGS_CFB16) {
 		if (!IS_ALIGNED(req->nbytes, CFB16_BLOCK_SIZE)) {
 			pr_err("request size is not exact amount of CFB16 blocks\n");
 			return -EINVAL;
 		}
+		ctx->block_size = CFB16_BLOCK_SIZE;
 	} else if (mode & TDES_FLAGS_CFB32) {
 		if (!IS_ALIGNED(req->nbytes, CFB32_BLOCK_SIZE)) {
 			pr_err("request size is not exact amount of CFB32 blocks\n");
 			return -EINVAL;
 		}
-	} else if (!IS_ALIGNED(req->nbytes, DES_BLOCK_SIZE)) {
-		pr_err("request size is not exact amount of DES blocks\n");
-		return -EINVAL;
+		ctx->block_size = CFB32_BLOCK_SIZE;
+	} else {
+		if (!IS_ALIGNED(req->nbytes, DES_BLOCK_SIZE)) {
+			pr_err("request size is not exact amount of DES blocks\n");
+			return -EINVAL;
+		}
+		ctx->block_size = DES_BLOCK_SIZE;
 	}
 
-	dd = atmel_tdes_find_dev(ctx);
-	if (!dd)
+	rctx->mode = mode;
+
+	return atmel_tdes_handle_queue(ctx->dd, req);
+}
+
+static bool atmel_tdes_filter(struct dma_chan *chan, void *slave)
+{
+	struct at_dma_slave	*sl = slave;
+
+	if (sl && sl->dma_dev == chan->device->dev) {
+		chan->private = sl;
+		return true;
+	} else {
+		return false;
+	}
+}
+
+static int atmel_tdes_dma_init(struct atmel_tdes_dev *dd,
+			struct crypto_platform_data *pdata)
+{
+	int err = -ENOMEM;
+	dma_cap_mask_t mask_in, mask_out;
+
+	if (pdata && pdata->dma_slave->txdata.dma_dev &&
+		pdata->dma_slave->rxdata.dma_dev) {
+
+		/* Try to grab 2 DMA channels */
+		dma_cap_zero(mask_in);
+		dma_cap_set(DMA_SLAVE, mask_in);
+
+		dd->dma_lch_in.chan = dma_request_channel(mask_in,
+				atmel_tdes_filter, &pdata->dma_slave->rxdata);
+
+		if (!dd->dma_lch_in.chan)
+			goto err_dma_in;
+
+		dd->dma_lch_in.dma_conf.direction = DMA_MEM_TO_DEV;
+		dd->dma_lch_in.dma_conf.dst_addr = dd->phys_base +
+			TDES_IDATA1R;
+		dd->dma_lch_in.dma_conf.src_maxburst = 1;
+		dd->dma_lch_in.dma_conf.src_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
+		dd->dma_lch_in.dma_conf.dst_maxburst = 1;
+		dd->dma_lch_in.dma_conf.dst_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
+		dd->dma_lch_in.dma_conf.device_fc = false;
+
+		dma_cap_zero(mask_out);
+		dma_cap_set(DMA_SLAVE, mask_out);
+		dd->dma_lch_out.chan = dma_request_channel(mask_out,
+				atmel_tdes_filter, &pdata->dma_slave->txdata);
+
+		if (!dd->dma_lch_out.chan)
+			goto err_dma_out;
+
+		dd->dma_lch_out.dma_conf.direction = DMA_DEV_TO_MEM;
+		dd->dma_lch_out.dma_conf.src_addr = dd->phys_base +
+			TDES_ODATA1R;
+		dd->dma_lch_out.dma_conf.src_maxburst = 1;
+		dd->dma_lch_out.dma_conf.src_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
+		dd->dma_lch_out.dma_conf.dst_maxburst = 1;
+		dd->dma_lch_out.dma_conf.dst_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
+		dd->dma_lch_out.dma_conf.device_fc = false;
+
+		return 0;
+	} else {
 		return -ENODEV;
+	}
 
-	rctx->mode = mode;
+err_dma_out:
+	dma_release_channel(dd->dma_lch_in.chan);
+err_dma_in:
+	return err;
+}
 
-	return atmel_tdes_handle_queue(dd, req);
+static void atmel_tdes_dma_cleanup(struct atmel_tdes_dev *dd)
+{
+	dma_release_channel(dd->dma_lch_in.chan);
+	dma_release_channel(dd->dma_lch_out.chan);
 }
 
 static int atmel_des_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
@@ -590,7 +815,8 @@ static int atmel_tdes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
 	/*
 	 * HW bug in cfb 3-keys mode.
 	 */
-	if (strstr(alg_name, "cfb") && (keylen != 2*DES_KEY_SIZE)) {
+	if (!ctx->dd->caps.has_cfb_3keys && strstr(alg_name, "cfb")
+			&& (keylen != 2*DES_KEY_SIZE)) {
 		crypto_ablkcipher_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
 		return -EINVAL;
 	} else if ((keylen != 2*DES_KEY_SIZE) && (keylen != 3*DES_KEY_SIZE)) {
@@ -678,8 +904,15 @@ static int atmel_tdes_ofb_decrypt(struct ablkcipher_request *req)
 
 static int atmel_tdes_cra_init(struct crypto_tfm *tfm)
 {
+	struct atmel_tdes_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct atmel_tdes_dev *dd;
+
 	tfm->crt_ablkcipher.reqsize = sizeof(struct atmel_tdes_reqctx);
 
+	dd = atmel_tdes_find_dev(ctx);
+	if (!dd)
+		return -ENODEV;
+
 	return 0;
 }
 
@@ -695,7 +928,7 @@ static struct crypto_alg tdes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= DES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
-	.cra_alignmask		= 0,
+	.cra_alignmask		= 0x7,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_tdes_cra_init,
@@ -715,7 +948,7 @@ static struct crypto_alg tdes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= DES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
-	.cra_alignmask		= 0,
+	.cra_alignmask		= 0x7,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_tdes_cra_init,
@@ -736,7 +969,7 @@ static struct crypto_alg tdes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= DES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
-	.cra_alignmask		= 0,
+	.cra_alignmask		= 0x7,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_tdes_cra_init,
@@ -778,7 +1011,7 @@ static struct crypto_alg tdes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= CFB16_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
-	.cra_alignmask		= 0,
+	.cra_alignmask		= 0x1,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_tdes_cra_init,
@@ -799,7 +1032,7 @@ static struct crypto_alg tdes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= CFB32_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
-	.cra_alignmask		= 0,
+	.cra_alignmask		= 0x3,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_tdes_cra_init,
@@ -820,7 +1053,7 @@ static struct crypto_alg tdes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= DES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
-	.cra_alignmask		= 0,
+	.cra_alignmask		= 0x7,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_tdes_cra_init,
@@ -841,7 +1074,7 @@ static struct crypto_alg tdes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= DES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
-	.cra_alignmask		= 0,
+	.cra_alignmask		= 0x7,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_tdes_cra_init,
@@ -861,7 +1094,7 @@ static struct crypto_alg tdes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= DES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
-	.cra_alignmask		= 0,
+	.cra_alignmask		= 0x7,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_tdes_cra_init,
@@ -882,7 +1115,7 @@ static struct crypto_alg tdes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= DES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
-	.cra_alignmask		= 0,
+	.cra_alignmask		= 0x7,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_tdes_cra_init,
@@ -924,7 +1157,7 @@ static struct crypto_alg tdes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= CFB16_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
-	.cra_alignmask		= 0,
+	.cra_alignmask		= 0x1,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_tdes_cra_init,
@@ -945,7 +1178,7 @@ static struct crypto_alg tdes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= CFB32_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
-	.cra_alignmask		= 0,
+	.cra_alignmask		= 0x3,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_tdes_cra_init,
@@ -966,7 +1199,7 @@ static struct crypto_alg tdes_algs[] = {
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= DES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
-	.cra_alignmask		= 0,
+	.cra_alignmask		= 0x7,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
 	.cra_init		= atmel_tdes_cra_init,
@@ -994,14 +1227,24 @@ static void atmel_tdes_done_task(unsigned long data)
 	struct atmel_tdes_dev *dd = (struct atmel_tdes_dev *) data;
 	int err;
 
-	err = atmel_tdes_crypt_dma_stop(dd);
+	if (!(dd->flags & TDES_FLAGS_DMA))
+		err = atmel_tdes_crypt_pdc_stop(dd);
+	else
+		err = atmel_tdes_crypt_dma_stop(dd);
 
 	err = dd->err ? : err;
 
 	if (dd->total && !err) {
-		err = atmel_tdes_crypt_dma_start(dd);
+		if (dd->flags & TDES_FLAGS_FAST) {
+			dd->in_sg = sg_next(dd->in_sg);
+			dd->out_sg = sg_next(dd->out_sg);
+			if (!dd->in_sg || !dd->out_sg)
+				err = -EINVAL;
+		}
 		if (!err)
-			return;
+			err = atmel_tdes_crypt_start(dd);
+		if (!err)
+			return; /* DMA started. Not fininishing. */
 	}
 
 	atmel_tdes_finish_req(dd, err);
@@ -1053,9 +1296,31 @@ err_tdes_algs:
 	return err;
 }
 
+static void atmel_tdes_get_cap(struct atmel_tdes_dev *dd)
+{
+
+	dd->caps.has_dma = 0;
+	dd->caps.has_cfb_3keys = 0;
+
+	/* keep only major version number */
+	switch (dd->hw_version & 0xf00) {
+	case 0x700:
+		dd->caps.has_dma = 1;
+		dd->caps.has_cfb_3keys = 1;
+		break;
+	case 0x600:
+		break;
+	default:
+		dev_warn(dd->dev,
+				"Unmanaged tdes version, set minimum capabilities\n");
+		break;
+	}
+}
+
 static int atmel_tdes_probe(struct platform_device *pdev)
 {
 	struct atmel_tdes_dev *tdes_dd;
+	struct crypto_platform_data	*pdata;
 	struct device *dev = &pdev->dev;
 	struct resource *tdes_res;
 	unsigned long tdes_phys_size;
@@ -1109,7 +1374,7 @@ static int atmel_tdes_probe(struct platform_device *pdev)
 	}
 
 	/* Initializing the clock */
-	tdes_dd->iclk = clk_get(&pdev->dev, NULL);
+	tdes_dd->iclk = clk_get(&pdev->dev, "tdes_clk");
 	if (IS_ERR(tdes_dd->iclk)) {
 		dev_err(dev, "clock intialization failed.\n");
 		err = PTR_ERR(tdes_dd->iclk);
@@ -1123,9 +1388,25 @@ static int atmel_tdes_probe(struct platform_device *pdev)
 		goto tdes_io_err;
 	}
 
-	err = atmel_tdes_dma_init(tdes_dd);
+	atmel_tdes_hw_version_init(tdes_dd);
+
+	atmel_tdes_get_cap(tdes_dd);
+
+	err = atmel_tdes_buff_init(tdes_dd);
 	if (err)
-		goto err_tdes_dma;
+		goto err_tdes_buff;
+
+	if (tdes_dd->caps.has_dma) {
+		pdata = pdev->dev.platform_data;
+		if (!pdata) {
+			dev_err(&pdev->dev, "platform data not available\n");
+			err = -ENXIO;
+			goto err_pdata;
+		}
+		err = atmel_tdes_dma_init(tdes_dd, pdata);
+		if (err)
+			goto err_tdes_dma;
+	}
 
 	spin_lock(&atmel_tdes.lock);
 	list_add_tail(&tdes_dd->list, &atmel_tdes.dev_list);
@@ -1143,8 +1424,12 @@ err_algs:
 	spin_lock(&atmel_tdes.lock);
 	list_del(&tdes_dd->list);
 	spin_unlock(&atmel_tdes.lock);
-	atmel_tdes_dma_cleanup(tdes_dd);
+	if (tdes_dd->caps.has_dma)
+		atmel_tdes_dma_cleanup(tdes_dd);
 err_tdes_dma:
+err_pdata:
+	atmel_tdes_buff_cleanup(tdes_dd);
+err_tdes_buff:
 	iounmap(tdes_dd->io_base);
 tdes_io_err:
 	clk_put(tdes_dd->iclk);
@@ -1178,7 +1463,10 @@ static int atmel_tdes_remove(struct platform_device *pdev)
 	tasklet_kill(&tdes_dd->done_task);
 	tasklet_kill(&tdes_dd->queue_task);
 
-	atmel_tdes_dma_cleanup(tdes_dd);
+	if (tdes_dd->caps.has_dma)
+		atmel_tdes_dma_cleanup(tdes_dd);
+
+	atmel_tdes_buff_cleanup(tdes_dd);
 
 	iounmap(tdes_dd->io_base);
 
diff --git a/drivers/crypto/bfin_crc.c b/drivers/crypto/bfin_crc.c
index 827913d7d33a..d797f31f5d85 100644
--- a/drivers/crypto/bfin_crc.c
+++ b/drivers/crypto/bfin_crc.c
@@ -151,7 +151,7 @@ static int bfin_crypto_crc_init(struct ahash_request *req)
 	struct bfin_crypto_crc_reqctx *ctx = ahash_request_ctx(req);
 	struct bfin_crypto_crc *crc;
 
-	dev_dbg(crc->dev, "crc_init\n");
+	dev_dbg(ctx->crc->dev, "crc_init\n");
 	spin_lock_bh(&crc_list.lock);
 	list_for_each_entry(crc, &crc_list.dev_list, list) {
 		crc_ctx->crc = crc;
@@ -160,7 +160,7 @@ static int bfin_crypto_crc_init(struct ahash_request *req)
 	spin_unlock_bh(&crc_list.lock);
 
 	if (sg_count(req->src) > CRC_MAX_DMA_DESC) {
-		dev_dbg(crc->dev, "init: requested sg list is too big > %d\n",
+		dev_dbg(ctx->crc->dev, "init: requested sg list is too big > %d\n",
 			CRC_MAX_DMA_DESC);
 		return -EINVAL;
 	}
@@ -175,7 +175,7 @@ static int bfin_crypto_crc_init(struct ahash_request *req)
 	/* init crc results */
 	put_unaligned_le32(crc_ctx->key, req->result);
 
-	dev_dbg(crc->dev, "init: digest size: %d\n",
+	dev_dbg(ctx->crc->dev, "init: digest size: %d\n",
 		crypto_ahash_digestsize(tfm));
 
 	return bfin_crypto_crc_init_hw(crc, crc_ctx->key);
diff --git a/drivers/crypto/caam/Kconfig b/drivers/crypto/caam/Kconfig
index 65c7668614ab..b44091c47f75 100644
--- a/drivers/crypto/caam/Kconfig
+++ b/drivers/crypto/caam/Kconfig
@@ -78,7 +78,7 @@ config CRYPTO_DEV_FSL_CAAM_AHASH_API
 	tristate "Register hash algorithm implementations with Crypto API"
 	depends on CRYPTO_DEV_FSL_CAAM
 	default y
-	select CRYPTO_AHASH
+	select CRYPTO_HASH
 	help
 	  Selecting this will offload ahash for users of the
 	  scatterlist crypto API to the SEC4 via job ring.
diff --git a/drivers/crypto/caam/caamalg.c b/drivers/crypto/caam/caamalg.c
index cf268b14ae9a..765fdf5ce579 100644
--- a/drivers/crypto/caam/caamalg.c
+++ b/drivers/crypto/caam/caamalg.c
@@ -1693,6 +1693,7 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "authenc(hmac(sha224),cbc(aes))",
 		.driver_name = "authenc-hmac-sha224-cbc-aes-caam",
 		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_AEAD,
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
@@ -1732,6 +1733,7 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "authenc(hmac(sha384),cbc(aes))",
 		.driver_name = "authenc-hmac-sha384-cbc-aes-caam",
 		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_AEAD,
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
@@ -1810,6 +1812,7 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "authenc(hmac(sha224),cbc(des3_ede))",
 		.driver_name = "authenc-hmac-sha224-cbc-des3_ede-caam",
 		.blocksize = DES3_EDE_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_AEAD,
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
@@ -1849,6 +1852,7 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "authenc(hmac(sha384),cbc(des3_ede))",
 		.driver_name = "authenc-hmac-sha384-cbc-des3_ede-caam",
 		.blocksize = DES3_EDE_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_AEAD,
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
@@ -1926,6 +1930,7 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "authenc(hmac(sha224),cbc(des))",
 		.driver_name = "authenc-hmac-sha224-cbc-des-caam",
 		.blocksize = DES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_AEAD,
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
@@ -1965,6 +1970,7 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "authenc(hmac(sha384),cbc(des))",
 		.driver_name = "authenc-hmac-sha384-cbc-des-caam",
 		.blocksize = DES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_AEAD,
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
diff --git a/drivers/crypto/caam/caamhash.c b/drivers/crypto/caam/caamhash.c
index 32aba7a61503..5996521a1caf 100644
--- a/drivers/crypto/caam/caamhash.c
+++ b/drivers/crypto/caam/caamhash.c
@@ -411,7 +411,7 @@ static int ahash_set_sh_desc(struct crypto_ahash *ahash)
 	return 0;
 }
 
-static u32 gen_split_hash_key(struct caam_hash_ctx *ctx, const u8 *key_in,
+static int gen_split_hash_key(struct caam_hash_ctx *ctx, const u8 *key_in,
 			      u32 keylen)
 {
 	return gen_split_key(ctx->jrdev, ctx->key, ctx->split_key_len,
@@ -420,7 +420,7 @@ static u32 gen_split_hash_key(struct caam_hash_ctx *ctx, const u8 *key_in,
 }
 
 /* Digest hash size if it is too large */
-static u32 hash_digest_key(struct caam_hash_ctx *ctx, const u8 *key_in,
+static int hash_digest_key(struct caam_hash_ctx *ctx, const u8 *key_in,
 			   u32 *keylen, u8 *key_out, u32 digestsize)
 {
 	struct device *jrdev = ctx->jrdev;
diff --git a/drivers/crypto/caam/ctrl.c b/drivers/crypto/caam/ctrl.c
index 8acf00490fd5..6e94bcd94678 100644
--- a/drivers/crypto/caam/ctrl.c
+++ b/drivers/crypto/caam/ctrl.c
@@ -304,6 +304,9 @@ static int caam_probe(struct platform_device *pdev)
 			caam_remove(pdev);
 			return ret;
 		}
+
+		/* Enable RDB bit so that RNG works faster */
+		setbits32(&topregs->ctrl.scfgr, SCFGR_RDBENABLE);
 	}
 
 	/* NOTE: RTIC detection ought to go here, around Si time */
diff --git a/drivers/crypto/caam/error.c b/drivers/crypto/caam/error.c
index 30b8f74833d4..9f25f5296029 100644
--- a/drivers/crypto/caam/error.c
+++ b/drivers/crypto/caam/error.c
@@ -36,7 +36,7 @@ static void report_jump_idx(u32 status, char *outstr)
 
 static void report_ccb_status(u32 status, char *outstr)
 {
-	char *cha_id_list[] = {
+	static const char * const cha_id_list[] = {
 		"",
 		"AES",
 		"DES",
@@ -51,7 +51,7 @@ static void report_ccb_status(u32 status, char *outstr)
 		"ZUCE",
 		"ZUCA",
 	};
-	char *err_id_list[] = {
+	static const char * const err_id_list[] = {
 		"No error.",
 		"Mode error.",
 		"Data size error.",
@@ -69,7 +69,7 @@ static void report_ccb_status(u32 status, char *outstr)
 		"Invalid CHA combination was selected",
 		"Invalid CHA selected.",
 	};
-	char *rng_err_id_list[] = {
+	static const char * const rng_err_id_list[] = {
 		"",
 		"",
 		"",
@@ -117,7 +117,7 @@ static void report_jump_status(u32 status, char *outstr)
 
 static void report_deco_status(u32 status, char *outstr)
 {
-	const struct {
+	static const struct {
 		u8 value;
 		char *error_text;
 	} desc_error_list[] = {
@@ -245,7 +245,7 @@ static void report_cond_code_status(u32 status, char *outstr)
 
 char *caam_jr_strstatus(char *outstr, u32 status)
 {
-	struct stat_src {
+	static const struct stat_src {
 		void (*report_ssed)(u32 status, char *outstr);
 		char *error;
 	} status_src[] = {
diff --git a/drivers/crypto/caam/intern.h b/drivers/crypto/caam/intern.h
index 5cd4c1b268a1..e4a16b741371 100644
--- a/drivers/crypto/caam/intern.h
+++ b/drivers/crypto/caam/intern.h
@@ -41,6 +41,7 @@ struct caam_jrentry_info {
 /* Private sub-storage for a single JobR */
 struct caam_drv_private_jr {
 	struct device *parentdev;	/* points back to controller dev */
+	struct platform_device *jr_pdev;/* points to platform device for JR */
 	int ridx;
 	struct caam_job_ring __iomem *rregs;	/* JobR's register space */
 	struct tasklet_struct irqtask;
diff --git a/drivers/crypto/caam/jr.c b/drivers/crypto/caam/jr.c
index 93d14070141a..b4aa773ecbc8 100644
--- a/drivers/crypto/caam/jr.c
+++ b/drivers/crypto/caam/jr.c
@@ -407,6 +407,7 @@ int caam_jr_shutdown(struct device *dev)
 	dma_free_coherent(dev, sizeof(struct jr_outentry) * JOBR_DEPTH,
 			  jrp->outring, outbusaddr);
 	kfree(jrp->entinfo);
+	of_device_unregister(jrp->jr_pdev);
 
 	return ret;
 }
@@ -454,6 +455,8 @@ int caam_jr_probe(struct platform_device *pdev, struct device_node *np,
 		kfree(jrpriv);
 		return -EINVAL;
 	}
+
+	jrpriv->jr_pdev = jr_pdev;
 	jrdev = &jr_pdev->dev;
 	dev_set_drvdata(jrdev, jrpriv);
 	ctrlpriv->jrdev[ring] = jrdev;
@@ -472,6 +475,7 @@ int caam_jr_probe(struct platform_device *pdev, struct device_node *np,
 	/* Now do the platform independent part */
 	error = caam_jr_init(jrdev); /* now turn on hardware */
 	if (error) {
+		of_device_unregister(jr_pdev);
 		kfree(jrpriv);
 		return error;
 	}
diff --git a/drivers/crypto/caam/key_gen.c b/drivers/crypto/caam/key_gen.c
index f6dba10246c3..87138d2adb5f 100644
--- a/drivers/crypto/caam/key_gen.c
+++ b/drivers/crypto/caam/key_gen.c
@@ -44,7 +44,7 @@ Split key generation-----------------------------------------------
 [06] 0x64260028    fifostr: class2 mdsplit-jdk len=40
 			@0xffe04000
 */
-u32 gen_split_key(struct device *jrdev, u8 *key_out, int split_key_len,
+int gen_split_key(struct device *jrdev, u8 *key_out, int split_key_len,
 		  int split_key_pad_len, const u8 *key_in, u32 keylen,
 		  u32 alg_op)
 {
diff --git a/drivers/crypto/caam/key_gen.h b/drivers/crypto/caam/key_gen.h
index d95d290c6e8b..c5588f6d8109 100644
--- a/drivers/crypto/caam/key_gen.h
+++ b/drivers/crypto/caam/key_gen.h
@@ -12,6 +12,6 @@ struct split_key_result {
 
 void split_key_done(struct device *dev, u32 *desc, u32 err, void *context);
 
-u32 gen_split_key(struct device *jrdev, u8 *key_out, int split_key_len,
+int gen_split_key(struct device *jrdev, u8 *key_out, int split_key_len,
 		    int split_key_pad_len, const u8 *key_in, u32 keylen,
 		    u32 alg_op);
diff --git a/drivers/crypto/caam/regs.h b/drivers/crypto/caam/regs.h
index 3223fc6d647c..cd6fedad9935 100644
--- a/drivers/crypto/caam/regs.h
+++ b/drivers/crypto/caam/regs.h
@@ -252,7 +252,8 @@ struct caam_ctrl {
 	/* Read/Writable					        */
 	u32 rsvd1;
 	u32 mcr;		/* MCFG      Master Config Register  */
-	u32 rsvd2[2];
+	u32 rsvd2;
+	u32 scfgr;		/* SCFGR, Security Config Register */
 
 	/* Bus Access Configuration Section			010-11f */
 	/* Read/Writable                                                */
@@ -299,6 +300,7 @@ struct caam_ctrl {
 #define MCFGR_WDFAIL		0x20000000 /* DECO watchdog force-fail */
 #define MCFGR_DMA_RESET		0x10000000
 #define MCFGR_LONG_PTR		0x00010000 /* Use >32-bit desc addressing */
+#define SCFGR_RDBENABLE		0x00000400
 
 /* AXI read cache control */
 #define MCFGR_ARCACHE_SHIFT	12
diff --git a/drivers/crypto/omap-aes.c b/drivers/crypto/omap-aes.c
index 6aa425fe0ed5..ee15b0f7849a 100644
--- a/drivers/crypto/omap-aes.c
+++ b/drivers/crypto/omap-aes.c
@@ -636,7 +636,7 @@ static void omap_aes_finish_req(struct omap_aes_dev *dd, int err)
 
 	pr_debug("err: %d\n", err);
 
-	pm_runtime_put_sync(dd->dev);
+	pm_runtime_put(dd->dev);
 	dd->flags &= ~FLAGS_BUSY;
 
 	req->base.complete(&req->base, err);
@@ -1248,18 +1248,7 @@ static struct platform_driver omap_aes_driver = {
 	},
 };
 
-static int __init omap_aes_mod_init(void)
-{
-	return  platform_driver_register(&omap_aes_driver);
-}
-
-static void __exit omap_aes_mod_exit(void)
-{
-	platform_driver_unregister(&omap_aes_driver);
-}
-
-module_init(omap_aes_mod_init);
-module_exit(omap_aes_mod_exit);
+module_platform_driver(omap_aes_driver);
 
 MODULE_DESCRIPTION("OMAP AES hw acceleration support.");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/crypto/omap-sham.c b/drivers/crypto/omap-sham.c
index 3d1611f5aecf..a1e1b4756ee5 100644
--- a/drivers/crypto/omap-sham.c
+++ b/drivers/crypto/omap-sham.c
@@ -923,7 +923,7 @@ static void omap_sham_finish_req(struct ahash_request *req, int err)
 	dd->flags &= ~(BIT(FLAGS_BUSY) | BIT(FLAGS_FINAL) | BIT(FLAGS_CPU) |
 			BIT(FLAGS_DMA_READY) | BIT(FLAGS_OUTPUT_READY));
 
-	pm_runtime_put_sync(dd->dev);
+	pm_runtime_put(dd->dev);
 
 	if (req->base.complete)
 		req->base.complete(&req->base, err);
@@ -1813,18 +1813,7 @@ static struct platform_driver omap_sham_driver = {
 	},
 };
 
-static int __init omap_sham_mod_init(void)
-{
-	return platform_driver_register(&omap_sham_driver);
-}
-
-static void __exit omap_sham_mod_exit(void)
-{
-	platform_driver_unregister(&omap_sham_driver);
-}
-
-module_init(omap_sham_mod_init);
-module_exit(omap_sham_mod_exit);
+module_platform_driver(omap_sham_driver);
 
 MODULE_DESCRIPTION("OMAP SHA1/MD5 hw acceleration support.");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/crypto/picoxcell_crypto.c b/drivers/crypto/picoxcell_crypto.c
index 2096d4685a9e..ac30724d923d 100644
--- a/drivers/crypto/picoxcell_crypto.c
+++ b/drivers/crypto/picoxcell_crypto.c
@@ -1688,8 +1688,6 @@ static const struct of_device_id spacc_of_id_table[] = {
 	{ .compatible = "picochip,spacc-l2" },
 	{}
 };
-#else /* CONFIG_OF */
-#define spacc_of_id_table NULL
 #endif /* CONFIG_OF */
 
 static bool spacc_is_compatible(struct platform_device *pdev,
@@ -1874,7 +1872,7 @@ static struct platform_driver spacc_driver = {
 #ifdef CONFIG_PM
 		.pm	= &spacc_pm_ops,
 #endif /* CONFIG_PM */
-		.of_match_table	= spacc_of_id_table,
+		.of_match_table	= of_match_ptr(spacc_of_id_table),
 	},
 	.id_table	= spacc_id_table,
 };
diff --git a/drivers/crypto/sahara.c b/drivers/crypto/sahara.c
new file mode 100644
index 000000000000..a97bb6c1596c
--- /dev/null
+++ b/drivers/crypto/sahara.c
@@ -0,0 +1,1070 @@
+/*
+ * Cryptographic API.
+ *
+ * Support for SAHARA cryptographic accelerator.
+ *
+ * Copyright (c) 2013 Vista Silicon S.L.
+ * Author: Javier Martin <javier.martin@vista-silicon.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.
+ *
+ * Based on omap-aes.c and tegra-aes.c
+ */
+
+#include <crypto/algapi.h>
+#include <crypto/aes.h>
+
+#include <linux/clk.h>
+#include <linux/crypto.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+
+#define SAHARA_NAME "sahara"
+#define SAHARA_VERSION_3	3
+#define SAHARA_TIMEOUT_MS	1000
+#define SAHARA_MAX_HW_DESC	2
+#define SAHARA_MAX_HW_LINK	20
+
+#define FLAGS_MODE_MASK		0x000f
+#define FLAGS_ENCRYPT		BIT(0)
+#define FLAGS_CBC		BIT(1)
+#define FLAGS_NEW_KEY		BIT(3)
+#define FLAGS_BUSY		4
+
+#define SAHARA_HDR_BASE			0x00800000
+#define SAHARA_HDR_SKHA_ALG_AES	0
+#define SAHARA_HDR_SKHA_OP_ENC		(1 << 2)
+#define SAHARA_HDR_SKHA_MODE_ECB	(0 << 3)
+#define SAHARA_HDR_SKHA_MODE_CBC	(1 << 3)
+#define SAHARA_HDR_FORM_DATA		(5 << 16)
+#define SAHARA_HDR_FORM_KEY		(8 << 16)
+#define SAHARA_HDR_LLO			(1 << 24)
+#define SAHARA_HDR_CHA_SKHA		(1 << 28)
+#define SAHARA_HDR_CHA_MDHA		(2 << 28)
+#define SAHARA_HDR_PARITY_BIT		(1 << 31)
+
+/* SAHARA can only process one request at a time */
+#define SAHARA_QUEUE_LENGTH	1
+
+#define SAHARA_REG_VERSION	0x00
+#define SAHARA_REG_DAR		0x04
+#define SAHARA_REG_CONTROL	0x08
+#define		SAHARA_CONTROL_SET_THROTTLE(x)	(((x) & 0xff) << 24)
+#define		SAHARA_CONTROL_SET_MAXBURST(x)	(((x) & 0xff) << 16)
+#define		SAHARA_CONTROL_RNG_AUTORSD	(1 << 7)
+#define		SAHARA_CONTROL_ENABLE_INT	(1 << 4)
+#define SAHARA_REG_CMD		0x0C
+#define		SAHARA_CMD_RESET		(1 << 0)
+#define		SAHARA_CMD_CLEAR_INT		(1 << 8)
+#define		SAHARA_CMD_CLEAR_ERR		(1 << 9)
+#define		SAHARA_CMD_SINGLE_STEP		(1 << 10)
+#define		SAHARA_CMD_MODE_BATCH		(1 << 16)
+#define		SAHARA_CMD_MODE_DEBUG		(1 << 18)
+#define	SAHARA_REG_STATUS	0x10
+#define		SAHARA_STATUS_GET_STATE(x)	((x) & 0x7)
+#define			SAHARA_STATE_IDLE	0
+#define			SAHARA_STATE_BUSY	1
+#define			SAHARA_STATE_ERR	2
+#define			SAHARA_STATE_FAULT	3
+#define			SAHARA_STATE_COMPLETE	4
+#define			SAHARA_STATE_COMP_FLAG	(1 << 2)
+#define		SAHARA_STATUS_DAR_FULL		(1 << 3)
+#define		SAHARA_STATUS_ERROR		(1 << 4)
+#define		SAHARA_STATUS_SECURE		(1 << 5)
+#define		SAHARA_STATUS_FAIL		(1 << 6)
+#define		SAHARA_STATUS_INIT		(1 << 7)
+#define		SAHARA_STATUS_RNG_RESEED	(1 << 8)
+#define		SAHARA_STATUS_ACTIVE_RNG	(1 << 9)
+#define		SAHARA_STATUS_ACTIVE_MDHA	(1 << 10)
+#define		SAHARA_STATUS_ACTIVE_SKHA	(1 << 11)
+#define		SAHARA_STATUS_MODE_BATCH	(1 << 16)
+#define		SAHARA_STATUS_MODE_DEDICATED	(1 << 17)
+#define		SAHARA_STATUS_MODE_DEBUG	(1 << 18)
+#define		SAHARA_STATUS_GET_ISTATE(x)	(((x) >> 24) & 0xff)
+#define SAHARA_REG_ERRSTATUS	0x14
+#define		SAHARA_ERRSTATUS_GET_SOURCE(x)	((x) & 0xf)
+#define			SAHARA_ERRSOURCE_CHA	14
+#define			SAHARA_ERRSOURCE_DMA	15
+#define		SAHARA_ERRSTATUS_DMA_DIR	(1 << 8)
+#define		SAHARA_ERRSTATUS_GET_DMASZ(x)(((x) >> 9) & 0x3)
+#define		SAHARA_ERRSTATUS_GET_DMASRC(x) (((x) >> 13) & 0x7)
+#define		SAHARA_ERRSTATUS_GET_CHASRC(x)	(((x) >> 16) & 0xfff)
+#define		SAHARA_ERRSTATUS_GET_CHAERR(x)	(((x) >> 28) & 0x3)
+#define SAHARA_REG_FADDR	0x18
+#define SAHARA_REG_CDAR		0x1C
+#define SAHARA_REG_IDAR		0x20
+
+struct sahara_hw_desc {
+	u32		hdr;
+	u32		len1;
+	dma_addr_t	p1;
+	u32		len2;
+	dma_addr_t	p2;
+	dma_addr_t	next;
+};
+
+struct sahara_hw_link {
+	u32		len;
+	dma_addr_t	p;
+	dma_addr_t	next;
+};
+
+struct sahara_ctx {
+	struct sahara_dev *dev;
+	unsigned long flags;
+	int keylen;
+	u8 key[AES_KEYSIZE_128];
+	struct crypto_ablkcipher *fallback;
+};
+
+struct sahara_aes_reqctx {
+	unsigned long mode;
+};
+
+struct sahara_dev {
+	struct device		*device;
+	void __iomem		*regs_base;
+	struct clk		*clk_ipg;
+	struct clk		*clk_ahb;
+
+	struct sahara_ctx	*ctx;
+	spinlock_t		lock;
+	struct crypto_queue	queue;
+	unsigned long		flags;
+
+	struct tasklet_struct	done_task;
+	struct tasklet_struct	queue_task;
+
+	struct sahara_hw_desc	*hw_desc[SAHARA_MAX_HW_DESC];
+	dma_addr_t		hw_phys_desc[SAHARA_MAX_HW_DESC];
+
+	u8			*key_base;
+	dma_addr_t		key_phys_base;
+
+	u8			*iv_base;
+	dma_addr_t		iv_phys_base;
+
+	struct sahara_hw_link	*hw_link[SAHARA_MAX_HW_LINK];
+	dma_addr_t		hw_phys_link[SAHARA_MAX_HW_LINK];
+
+	struct ablkcipher_request *req;
+	size_t			total;
+	struct scatterlist	*in_sg;
+	unsigned int		nb_in_sg;
+	struct scatterlist	*out_sg;
+	unsigned int		nb_out_sg;
+
+	u32			error;
+	struct timer_list	watchdog;
+};
+
+static struct sahara_dev *dev_ptr;
+
+static inline void sahara_write(struct sahara_dev *dev, u32 data, u32 reg)
+{
+	writel(data, dev->regs_base + reg);
+}
+
+static inline unsigned int sahara_read(struct sahara_dev *dev, u32 reg)
+{
+	return readl(dev->regs_base + reg);
+}
+
+static u32 sahara_aes_key_hdr(struct sahara_dev *dev)
+{
+	u32 hdr = SAHARA_HDR_BASE | SAHARA_HDR_SKHA_ALG_AES |
+			SAHARA_HDR_FORM_KEY | SAHARA_HDR_LLO |
+			SAHARA_HDR_CHA_SKHA | SAHARA_HDR_PARITY_BIT;
+
+	if (dev->flags & FLAGS_CBC) {
+		hdr |= SAHARA_HDR_SKHA_MODE_CBC;
+		hdr ^= SAHARA_HDR_PARITY_BIT;
+	}
+
+	if (dev->flags & FLAGS_ENCRYPT) {
+		hdr |= SAHARA_HDR_SKHA_OP_ENC;
+		hdr ^= SAHARA_HDR_PARITY_BIT;
+	}
+
+	return hdr;
+}
+
+static u32 sahara_aes_data_link_hdr(struct sahara_dev *dev)
+{
+	return SAHARA_HDR_BASE | SAHARA_HDR_FORM_DATA |
+			SAHARA_HDR_CHA_SKHA | SAHARA_HDR_PARITY_BIT;
+}
+
+static int sahara_sg_length(struct scatterlist *sg,
+			    unsigned int total)
+{
+	int sg_nb;
+	unsigned int len;
+	struct scatterlist *sg_list;
+
+	sg_nb = 0;
+	sg_list = sg;
+
+	while (total) {
+		len = min(sg_list->length, total);
+
+		sg_nb++;
+		total -= len;
+
+		sg_list = sg_next(sg_list);
+		if (!sg_list)
+			total = 0;
+	}
+
+	return sg_nb;
+}
+
+static char *sahara_err_src[16] = {
+	"No error",
+	"Header error",
+	"Descriptor length error",
+	"Descriptor length or pointer error",
+	"Link length error",
+	"Link pointer error",
+	"Input buffer error",
+	"Output buffer error",
+	"Output buffer starvation",
+	"Internal state fault",
+	"General descriptor problem",
+	"Reserved",
+	"Descriptor address error",
+	"Link address error",
+	"CHA error",
+	"DMA error"
+};
+
+static char *sahara_err_dmasize[4] = {
+	"Byte transfer",
+	"Half-word transfer",
+	"Word transfer",
+	"Reserved"
+};
+
+static char *sahara_err_dmasrc[8] = {
+	"No error",
+	"AHB bus error",
+	"Internal IP bus error",
+	"Parity error",
+	"DMA crosses 256 byte boundary",
+	"DMA is busy",
+	"Reserved",
+	"DMA HW error"
+};
+
+static char *sahara_cha_errsrc[12] = {
+	"Input buffer non-empty",
+	"Illegal address",
+	"Illegal mode",
+	"Illegal data size",
+	"Illegal key size",
+	"Write during processing",
+	"CTX read during processing",
+	"HW error",
+	"Input buffer disabled/underflow",
+	"Output buffer disabled/overflow",
+	"DES key parity error",
+	"Reserved"
+};
+
+static char *sahara_cha_err[4] = { "No error", "SKHA", "MDHA", "RNG" };
+
+static void sahara_decode_error(struct sahara_dev *dev, unsigned int error)
+{
+	u8 source = SAHARA_ERRSTATUS_GET_SOURCE(error);
+	u16 chasrc = ffs(SAHARA_ERRSTATUS_GET_CHASRC(error));
+
+	dev_err(dev->device, "%s: Error Register = 0x%08x\n", __func__, error);
+
+	dev_err(dev->device, "	- %s.\n", sahara_err_src[source]);
+
+	if (source == SAHARA_ERRSOURCE_DMA) {
+		if (error & SAHARA_ERRSTATUS_DMA_DIR)
+			dev_err(dev->device, "		* DMA read.\n");
+		else
+			dev_err(dev->device, "		* DMA write.\n");
+
+		dev_err(dev->device, "		* %s.\n",
+		       sahara_err_dmasize[SAHARA_ERRSTATUS_GET_DMASZ(error)]);
+		dev_err(dev->device, "		* %s.\n",
+		       sahara_err_dmasrc[SAHARA_ERRSTATUS_GET_DMASRC(error)]);
+	} else if (source == SAHARA_ERRSOURCE_CHA) {
+		dev_err(dev->device, "		* %s.\n",
+			sahara_cha_errsrc[chasrc]);
+		dev_err(dev->device, "		* %s.\n",
+		       sahara_cha_err[SAHARA_ERRSTATUS_GET_CHAERR(error)]);
+	}
+	dev_err(dev->device, "\n");
+}
+
+static char *sahara_state[4] = { "Idle", "Busy", "Error", "HW Fault" };
+
+static void sahara_decode_status(struct sahara_dev *dev, unsigned int status)
+{
+	u8 state;
+
+	if (!IS_ENABLED(DEBUG))
+		return;
+
+	state = SAHARA_STATUS_GET_STATE(status);
+
+	dev_dbg(dev->device, "%s: Status Register = 0x%08x\n",
+		__func__, status);
+
+	dev_dbg(dev->device, "	- State = %d:\n", state);
+	if (state & SAHARA_STATE_COMP_FLAG)
+		dev_dbg(dev->device, "		* Descriptor completed. IRQ pending.\n");
+
+	dev_dbg(dev->device, "		* %s.\n",
+	       sahara_state[state & ~SAHARA_STATE_COMP_FLAG]);
+
+	if (status & SAHARA_STATUS_DAR_FULL)
+		dev_dbg(dev->device, "	- DAR Full.\n");
+	if (status & SAHARA_STATUS_ERROR)
+		dev_dbg(dev->device, "	- Error.\n");
+	if (status & SAHARA_STATUS_SECURE)
+		dev_dbg(dev->device, "	- Secure.\n");
+	if (status & SAHARA_STATUS_FAIL)
+		dev_dbg(dev->device, "	- Fail.\n");
+	if (status & SAHARA_STATUS_RNG_RESEED)
+		dev_dbg(dev->device, "	- RNG Reseed Request.\n");
+	if (status & SAHARA_STATUS_ACTIVE_RNG)
+		dev_dbg(dev->device, "	- RNG Active.\n");
+	if (status & SAHARA_STATUS_ACTIVE_MDHA)
+		dev_dbg(dev->device, "	- MDHA Active.\n");
+	if (status & SAHARA_STATUS_ACTIVE_SKHA)
+		dev_dbg(dev->device, "	- SKHA Active.\n");
+
+	if (status & SAHARA_STATUS_MODE_BATCH)
+		dev_dbg(dev->device, "	- Batch Mode.\n");
+	else if (status & SAHARA_STATUS_MODE_DEDICATED)
+		dev_dbg(dev->device, "	- Decidated Mode.\n");
+	else if (status & SAHARA_STATUS_MODE_DEBUG)
+		dev_dbg(dev->device, "	- Debug Mode.\n");
+
+	dev_dbg(dev->device, "	- Internal state = 0x%02x\n",
+	       SAHARA_STATUS_GET_ISTATE(status));
+
+	dev_dbg(dev->device, "Current DAR: 0x%08x\n",
+		sahara_read(dev, SAHARA_REG_CDAR));
+	dev_dbg(dev->device, "Initial DAR: 0x%08x\n\n",
+		sahara_read(dev, SAHARA_REG_IDAR));
+}
+
+static void sahara_dump_descriptors(struct sahara_dev *dev)
+{
+	int i;
+
+	if (!IS_ENABLED(DEBUG))
+		return;
+
+	for (i = 0; i < SAHARA_MAX_HW_DESC; i++) {
+		dev_dbg(dev->device, "Descriptor (%d) (0x%08x):\n",
+			i, dev->hw_phys_desc[i]);
+		dev_dbg(dev->device, "\thdr = 0x%08x\n", dev->hw_desc[i]->hdr);
+		dev_dbg(dev->device, "\tlen1 = %u\n", dev->hw_desc[i]->len1);
+		dev_dbg(dev->device, "\tp1 = 0x%08x\n", dev->hw_desc[i]->p1);
+		dev_dbg(dev->device, "\tlen2 = %u\n", dev->hw_desc[i]->len2);
+		dev_dbg(dev->device, "\tp2 = 0x%08x\n", dev->hw_desc[i]->p2);
+		dev_dbg(dev->device, "\tnext = 0x%08x\n",
+			dev->hw_desc[i]->next);
+	}
+	dev_dbg(dev->device, "\n");
+}
+
+static void sahara_dump_links(struct sahara_dev *dev)
+{
+	int i;
+
+	if (!IS_ENABLED(DEBUG))
+		return;
+
+	for (i = 0; i < SAHARA_MAX_HW_LINK; i++) {
+		dev_dbg(dev->device, "Link (%d) (0x%08x):\n",
+			i, dev->hw_phys_link[i]);
+		dev_dbg(dev->device, "\tlen = %u\n", dev->hw_link[i]->len);
+		dev_dbg(dev->device, "\tp = 0x%08x\n", dev->hw_link[i]->p);
+		dev_dbg(dev->device, "\tnext = 0x%08x\n",
+			dev->hw_link[i]->next);
+	}
+	dev_dbg(dev->device, "\n");
+}
+
+static void sahara_aes_done_task(unsigned long data)
+{
+	struct sahara_dev *dev = (struct sahara_dev *)data;
+
+	dma_unmap_sg(dev->device, dev->out_sg, dev->nb_out_sg,
+		DMA_TO_DEVICE);
+	dma_unmap_sg(dev->device, dev->in_sg, dev->nb_in_sg,
+		DMA_FROM_DEVICE);
+
+	spin_lock(&dev->lock);
+	clear_bit(FLAGS_BUSY, &dev->flags);
+	spin_unlock(&dev->lock);
+
+	dev->req->base.complete(&dev->req->base, dev->error);
+}
+
+void sahara_watchdog(unsigned long data)
+{
+	struct sahara_dev *dev = (struct sahara_dev *)data;
+	unsigned int err = sahara_read(dev, SAHARA_REG_ERRSTATUS);
+	unsigned int stat = sahara_read(dev, SAHARA_REG_STATUS);
+
+	sahara_decode_status(dev, stat);
+	sahara_decode_error(dev, err);
+	dev->error = -ETIMEDOUT;
+	sahara_aes_done_task(data);
+}
+
+static int sahara_hw_descriptor_create(struct sahara_dev *dev)
+{
+	struct sahara_ctx *ctx = dev->ctx;
+	struct scatterlist *sg;
+	int ret;
+	int i, j;
+
+	/* Copy new key if necessary */
+	if (ctx->flags & FLAGS_NEW_KEY) {
+		memcpy(dev->key_base, ctx->key, ctx->keylen);
+		ctx->flags &= ~FLAGS_NEW_KEY;
+
+		if (dev->flags & FLAGS_CBC) {
+			dev->hw_desc[0]->len1 = AES_BLOCK_SIZE;
+			dev->hw_desc[0]->p1 = dev->iv_phys_base;
+		} else {
+			dev->hw_desc[0]->len1 = 0;
+			dev->hw_desc[0]->p1 = 0;
+		}
+		dev->hw_desc[0]->len2 = ctx->keylen;
+		dev->hw_desc[0]->p2 = dev->key_phys_base;
+		dev->hw_desc[0]->next = dev->hw_phys_desc[1];
+	}
+	dev->hw_desc[0]->hdr = sahara_aes_key_hdr(dev);
+
+	dev->nb_in_sg = sahara_sg_length(dev->in_sg, dev->total);
+	dev->nb_out_sg = sahara_sg_length(dev->out_sg, dev->total);
+	if ((dev->nb_in_sg + dev->nb_out_sg) > SAHARA_MAX_HW_LINK) {
+		dev_err(dev->device, "not enough hw links (%d)\n",
+			dev->nb_in_sg + dev->nb_out_sg);
+		return -EINVAL;
+	}
+
+	ret = dma_map_sg(dev->device, dev->in_sg, dev->nb_in_sg,
+			 DMA_TO_DEVICE);
+	if (ret != dev->nb_in_sg) {
+		dev_err(dev->device, "couldn't map in sg\n");
+		goto unmap_in;
+	}
+	ret = dma_map_sg(dev->device, dev->out_sg, dev->nb_out_sg,
+			 DMA_FROM_DEVICE);
+	if (ret != dev->nb_out_sg) {
+		dev_err(dev->device, "couldn't map out sg\n");
+		goto unmap_out;
+	}
+
+	/* Create input links */
+	dev->hw_desc[1]->p1 = dev->hw_phys_link[0];
+	sg = dev->in_sg;
+	for (i = 0; i < dev->nb_in_sg; i++) {
+		dev->hw_link[i]->len = sg->length;
+		dev->hw_link[i]->p = sg->dma_address;
+		if (i == (dev->nb_in_sg - 1)) {
+			dev->hw_link[i]->next = 0;
+		} else {
+			dev->hw_link[i]->next = dev->hw_phys_link[i + 1];
+			sg = sg_next(sg);
+		}
+	}
+
+	/* Create output links */
+	dev->hw_desc[1]->p2 = dev->hw_phys_link[i];
+	sg = dev->out_sg;
+	for (j = i; j < dev->nb_out_sg + i; j++) {
+		dev->hw_link[j]->len = sg->length;
+		dev->hw_link[j]->p = sg->dma_address;
+		if (j == (dev->nb_out_sg + i - 1)) {
+			dev->hw_link[j]->next = 0;
+		} else {
+			dev->hw_link[j]->next = dev->hw_phys_link[j + 1];
+			sg = sg_next(sg);
+		}
+	}
+
+	/* Fill remaining fields of hw_desc[1] */
+	dev->hw_desc[1]->hdr = sahara_aes_data_link_hdr(dev);
+	dev->hw_desc[1]->len1 = dev->total;
+	dev->hw_desc[1]->len2 = dev->total;
+	dev->hw_desc[1]->next = 0;
+
+	sahara_dump_descriptors(dev);
+	sahara_dump_links(dev);
+
+	/* Start processing descriptor chain. */
+	mod_timer(&dev->watchdog,
+		  jiffies + msecs_to_jiffies(SAHARA_TIMEOUT_MS));
+	sahara_write(dev, dev->hw_phys_desc[0], SAHARA_REG_DAR);
+
+	return 0;
+
+unmap_out:
+	dma_unmap_sg(dev->device, dev->out_sg, dev->nb_out_sg,
+		DMA_TO_DEVICE);
+unmap_in:
+	dma_unmap_sg(dev->device, dev->in_sg, dev->nb_in_sg,
+		DMA_FROM_DEVICE);
+
+	return -EINVAL;
+}
+
+static void sahara_aes_queue_task(unsigned long data)
+{
+	struct sahara_dev *dev = (struct sahara_dev *)data;
+	struct crypto_async_request *async_req, *backlog;
+	struct sahara_ctx *ctx;
+	struct sahara_aes_reqctx *rctx;
+	struct ablkcipher_request *req;
+	int ret;
+
+	spin_lock(&dev->lock);
+	backlog = crypto_get_backlog(&dev->queue);
+	async_req = crypto_dequeue_request(&dev->queue);
+	if (!async_req)
+		clear_bit(FLAGS_BUSY, &dev->flags);
+	spin_unlock(&dev->lock);
+
+	if (!async_req)
+		return;
+
+	if (backlog)
+		backlog->complete(backlog, -EINPROGRESS);
+
+	req = ablkcipher_request_cast(async_req);
+
+	/* Request is ready to be dispatched by the device */
+	dev_dbg(dev->device,
+		"dispatch request (nbytes=%d, src=%p, dst=%p)\n",
+		req->nbytes, req->src, req->dst);
+
+	/* assign new request to device */
+	dev->req = req;
+	dev->total = req->nbytes;
+	dev->in_sg = req->src;
+	dev->out_sg = req->dst;
+
+	rctx = ablkcipher_request_ctx(req);
+	ctx = crypto_ablkcipher_ctx(crypto_ablkcipher_reqtfm(req));
+	rctx->mode &= FLAGS_MODE_MASK;
+	dev->flags = (dev->flags & ~FLAGS_MODE_MASK) | rctx->mode;
+
+	if ((dev->flags & FLAGS_CBC) && req->info)
+		memcpy(dev->iv_base, req->info, AES_KEYSIZE_128);
+
+	/* assign new context to device */
+	ctx->dev = dev;
+	dev->ctx = ctx;
+
+	ret = sahara_hw_descriptor_create(dev);
+	if (ret < 0) {
+		spin_lock(&dev->lock);
+		clear_bit(FLAGS_BUSY, &dev->flags);
+		spin_unlock(&dev->lock);
+		dev->req->base.complete(&dev->req->base, ret);
+	}
+}
+
+static int sahara_aes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+			     unsigned int keylen)
+{
+	struct sahara_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	int ret;
+
+	ctx->keylen = keylen;
+
+	/* SAHARA only supports 128bit keys */
+	if (keylen == AES_KEYSIZE_128) {
+		memcpy(ctx->key, key, keylen);
+		ctx->flags |= FLAGS_NEW_KEY;
+		return 0;
+	}
+
+	if (keylen != AES_KEYSIZE_128 &&
+	    keylen != AES_KEYSIZE_192 && keylen != AES_KEYSIZE_256)
+		return -EINVAL;
+
+	/*
+	 * The requested key size is not supported by HW, do a fallback.
+	 */
+	ctx->fallback->base.crt_flags &= ~CRYPTO_TFM_REQ_MASK;
+	ctx->fallback->base.crt_flags |=
+		(tfm->base.crt_flags & CRYPTO_TFM_REQ_MASK);
+
+	ret = crypto_ablkcipher_setkey(ctx->fallback, key, keylen);
+	if (ret) {
+		struct crypto_tfm *tfm_aux = crypto_ablkcipher_tfm(tfm);
+
+		tfm_aux->crt_flags &= ~CRYPTO_TFM_RES_MASK;
+		tfm_aux->crt_flags |=
+			(ctx->fallback->base.crt_flags & CRYPTO_TFM_RES_MASK);
+	}
+	return ret;
+}
+
+static int sahara_aes_crypt(struct ablkcipher_request *req, unsigned long mode)
+{
+	struct sahara_ctx *ctx = crypto_ablkcipher_ctx(
+		crypto_ablkcipher_reqtfm(req));
+	struct sahara_aes_reqctx *rctx = ablkcipher_request_ctx(req);
+	struct sahara_dev *dev = dev_ptr;
+	int err = 0;
+	int busy;
+
+	dev_dbg(dev->device, "nbytes: %d, enc: %d, cbc: %d\n",
+		req->nbytes, !!(mode & FLAGS_ENCRYPT), !!(mode & FLAGS_CBC));
+
+	if (!IS_ALIGNED(req->nbytes, AES_BLOCK_SIZE)) {
+		dev_err(dev->device,
+			"request size is not exact amount of AES blocks\n");
+		return -EINVAL;
+	}
+
+	ctx->dev = dev;
+
+	rctx->mode = mode;
+	spin_lock_bh(&dev->lock);
+	err = ablkcipher_enqueue_request(&dev->queue, req);
+	busy = test_and_set_bit(FLAGS_BUSY, &dev->flags);
+	spin_unlock_bh(&dev->lock);
+
+	if (!busy)
+		tasklet_schedule(&dev->queue_task);
+
+	return err;
+}
+
+static int sahara_aes_ecb_encrypt(struct ablkcipher_request *req)
+{
+	struct crypto_tfm *tfm =
+		crypto_ablkcipher_tfm(crypto_ablkcipher_reqtfm(req));
+	struct sahara_ctx *ctx = crypto_ablkcipher_ctx(
+		crypto_ablkcipher_reqtfm(req));
+	int err;
+
+	if (unlikely(ctx->keylen != AES_KEYSIZE_128)) {
+		ablkcipher_request_set_tfm(req, ctx->fallback);
+		err = crypto_ablkcipher_encrypt(req);
+		ablkcipher_request_set_tfm(req, __crypto_ablkcipher_cast(tfm));
+		return err;
+	}
+
+	return sahara_aes_crypt(req, FLAGS_ENCRYPT);
+}
+
+static int sahara_aes_ecb_decrypt(struct ablkcipher_request *req)
+{
+	struct crypto_tfm *tfm =
+		crypto_ablkcipher_tfm(crypto_ablkcipher_reqtfm(req));
+	struct sahara_ctx *ctx = crypto_ablkcipher_ctx(
+		crypto_ablkcipher_reqtfm(req));
+	int err;
+
+	if (unlikely(ctx->keylen != AES_KEYSIZE_128)) {
+		ablkcipher_request_set_tfm(req, ctx->fallback);
+		err = crypto_ablkcipher_decrypt(req);
+		ablkcipher_request_set_tfm(req, __crypto_ablkcipher_cast(tfm));
+		return err;
+	}
+
+	return sahara_aes_crypt(req, 0);
+}
+
+static int sahara_aes_cbc_encrypt(struct ablkcipher_request *req)
+{
+	struct crypto_tfm *tfm =
+		crypto_ablkcipher_tfm(crypto_ablkcipher_reqtfm(req));
+	struct sahara_ctx *ctx = crypto_ablkcipher_ctx(
+		crypto_ablkcipher_reqtfm(req));
+	int err;
+
+	if (unlikely(ctx->keylen != AES_KEYSIZE_128)) {
+		ablkcipher_request_set_tfm(req, ctx->fallback);
+		err = crypto_ablkcipher_encrypt(req);
+		ablkcipher_request_set_tfm(req, __crypto_ablkcipher_cast(tfm));
+		return err;
+	}
+
+	return sahara_aes_crypt(req, FLAGS_ENCRYPT | FLAGS_CBC);
+}
+
+static int sahara_aes_cbc_decrypt(struct ablkcipher_request *req)
+{
+	struct crypto_tfm *tfm =
+		crypto_ablkcipher_tfm(crypto_ablkcipher_reqtfm(req));
+	struct sahara_ctx *ctx = crypto_ablkcipher_ctx(
+		crypto_ablkcipher_reqtfm(req));
+	int err;
+
+	if (unlikely(ctx->keylen != AES_KEYSIZE_128)) {
+		ablkcipher_request_set_tfm(req, ctx->fallback);
+		err = crypto_ablkcipher_decrypt(req);
+		ablkcipher_request_set_tfm(req, __crypto_ablkcipher_cast(tfm));
+		return err;
+	}
+
+	return sahara_aes_crypt(req, FLAGS_CBC);
+}
+
+static int sahara_aes_cra_init(struct crypto_tfm *tfm)
+{
+	const char *name = tfm->__crt_alg->cra_name;
+	struct sahara_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	ctx->fallback = crypto_alloc_ablkcipher(name, 0,
+				CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK);
+	if (IS_ERR(ctx->fallback)) {
+		pr_err("Error allocating fallback algo %s\n", name);
+		return PTR_ERR(ctx->fallback);
+	}
+
+	tfm->crt_ablkcipher.reqsize = sizeof(struct sahara_aes_reqctx);
+
+	return 0;
+}
+
+static void sahara_aes_cra_exit(struct crypto_tfm *tfm)
+{
+	struct sahara_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	if (ctx->fallback)
+		crypto_free_ablkcipher(ctx->fallback);
+	ctx->fallback = NULL;
+}
+
+static struct crypto_alg aes_algs[] = {
+{
+	.cra_name		= "ecb(aes)",
+	.cra_driver_name	= "sahara-ecb-aes",
+	.cra_priority		= 300,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER |
+			CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK,
+	.cra_blocksize		= AES_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct sahara_ctx),
+	.cra_alignmask		= 0x0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= sahara_aes_cra_init,
+	.cra_exit		= sahara_aes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= AES_MIN_KEY_SIZE ,
+		.max_keysize	= AES_MAX_KEY_SIZE,
+		.setkey		= sahara_aes_setkey,
+		.encrypt	= sahara_aes_ecb_encrypt,
+		.decrypt	= sahara_aes_ecb_decrypt,
+	}
+}, {
+	.cra_name		= "cbc(aes)",
+	.cra_driver_name	= "sahara-cbc-aes",
+	.cra_priority		= 300,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER |
+			CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK,
+	.cra_blocksize		= AES_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct sahara_ctx),
+	.cra_alignmask		= 0x0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= sahara_aes_cra_init,
+	.cra_exit		= sahara_aes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= AES_MIN_KEY_SIZE ,
+		.max_keysize	= AES_MAX_KEY_SIZE,
+		.ivsize		= AES_BLOCK_SIZE,
+		.setkey		= sahara_aes_setkey,
+		.encrypt	= sahara_aes_cbc_encrypt,
+		.decrypt	= sahara_aes_cbc_decrypt,
+	}
+}
+};
+
+static irqreturn_t sahara_irq_handler(int irq, void *data)
+{
+	struct sahara_dev *dev = (struct sahara_dev *)data;
+	unsigned int stat = sahara_read(dev, SAHARA_REG_STATUS);
+	unsigned int err = sahara_read(dev, SAHARA_REG_ERRSTATUS);
+
+	del_timer(&dev->watchdog);
+
+	sahara_write(dev, SAHARA_CMD_CLEAR_INT | SAHARA_CMD_CLEAR_ERR,
+		     SAHARA_REG_CMD);
+
+	sahara_decode_status(dev, stat);
+
+	if (SAHARA_STATUS_GET_STATE(stat) == SAHARA_STATE_BUSY) {
+		return IRQ_NONE;
+	} else if (SAHARA_STATUS_GET_STATE(stat) == SAHARA_STATE_COMPLETE) {
+		dev->error = 0;
+	} else {
+		sahara_decode_error(dev, err);
+		dev->error = -EINVAL;
+	}
+
+	tasklet_schedule(&dev->done_task);
+
+	return IRQ_HANDLED;
+}
+
+
+static int sahara_register_algs(struct sahara_dev *dev)
+{
+	int err, i, j;
+
+	for (i = 0; i < ARRAY_SIZE(aes_algs); i++) {
+		INIT_LIST_HEAD(&aes_algs[i].cra_list);
+		err = crypto_register_alg(&aes_algs[i]);
+		if (err)
+			goto err_aes_algs;
+	}
+
+	return 0;
+
+err_aes_algs:
+	for (j = 0; j < i; j++)
+		crypto_unregister_alg(&aes_algs[j]);
+
+	return err;
+}
+
+static void sahara_unregister_algs(struct sahara_dev *dev)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(aes_algs); i++)
+		crypto_unregister_alg(&aes_algs[i]);
+}
+
+static struct platform_device_id sahara_platform_ids[] = {
+	{ .name = "sahara-imx27" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(platform, sahara_platform_ids);
+
+static struct of_device_id sahara_dt_ids[] = {
+	{ .compatible = "fsl,imx27-sahara" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(platform, sahara_dt_ids);
+
+static int sahara_probe(struct platform_device *pdev)
+{
+	struct sahara_dev *dev;
+	struct resource *res;
+	u32 version;
+	int irq;
+	int err;
+	int i;
+
+	dev = devm_kzalloc(&pdev->dev, sizeof(struct sahara_dev), GFP_KERNEL);
+	if (dev == NULL) {
+		dev_err(&pdev->dev, "unable to alloc data struct.\n");
+		return -ENOMEM;
+	}
+
+	dev->device = &pdev->dev;
+	platform_set_drvdata(pdev, dev);
+
+	/* Get the base address */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "failed to get memory region resource\n");
+		return -ENODEV;
+	}
+
+	if (devm_request_mem_region(&pdev->dev, res->start,
+			resource_size(res), SAHARA_NAME) == NULL) {
+		dev_err(&pdev->dev, "failed to request memory region\n");
+		return -ENOENT;
+	}
+	dev->regs_base = devm_ioremap(&pdev->dev, res->start,
+				      resource_size(res));
+	if (!dev->regs_base) {
+		dev_err(&pdev->dev, "failed to ioremap address region\n");
+		return -ENOENT;
+	}
+
+	/* Get the IRQ */
+	irq = platform_get_irq(pdev,  0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "failed to get irq resource\n");
+		return irq;
+	}
+
+	if (devm_request_irq(&pdev->dev, irq, sahara_irq_handler,
+		0, SAHARA_NAME, dev) < 0) {
+		dev_err(&pdev->dev, "failed to request irq\n");
+		return -ENOENT;
+	}
+
+	/* clocks */
+	dev->clk_ipg = devm_clk_get(&pdev->dev, "ipg");
+	if (IS_ERR(dev->clk_ipg)) {
+		dev_err(&pdev->dev, "Could not get ipg clock\n");
+		return PTR_ERR(dev->clk_ipg);
+	}
+
+	dev->clk_ahb = devm_clk_get(&pdev->dev, "ahb");
+	if (IS_ERR(dev->clk_ahb)) {
+		dev_err(&pdev->dev, "Could not get ahb clock\n");
+		return PTR_ERR(dev->clk_ahb);
+	}
+
+	/* Allocate HW descriptors */
+	dev->hw_desc[0] = dma_alloc_coherent(&pdev->dev,
+			SAHARA_MAX_HW_DESC * sizeof(struct sahara_hw_desc),
+			&dev->hw_phys_desc[0], GFP_KERNEL);
+	if (!dev->hw_desc[0]) {
+		dev_err(&pdev->dev, "Could not allocate hw descriptors\n");
+		return -ENOMEM;
+	}
+	dev->hw_desc[1] = dev->hw_desc[0] + 1;
+	dev->hw_phys_desc[1] = dev->hw_phys_desc[0] +
+				sizeof(struct sahara_hw_desc);
+
+	/* Allocate space for iv and key */
+	dev->key_base = dma_alloc_coherent(&pdev->dev, 2 * AES_KEYSIZE_128,
+				&dev->key_phys_base, GFP_KERNEL);
+	if (!dev->key_base) {
+		dev_err(&pdev->dev, "Could not allocate memory for key\n");
+		err = -ENOMEM;
+		goto err_key;
+	}
+	dev->iv_base = dev->key_base + AES_KEYSIZE_128;
+	dev->iv_phys_base = dev->key_phys_base + AES_KEYSIZE_128;
+
+	/* Allocate space for HW links */
+	dev->hw_link[0] = dma_alloc_coherent(&pdev->dev,
+			SAHARA_MAX_HW_LINK * sizeof(struct sahara_hw_link),
+			&dev->hw_phys_link[0], GFP_KERNEL);
+	if (!dev->hw_link) {
+		dev_err(&pdev->dev, "Could not allocate hw links\n");
+		err = -ENOMEM;
+		goto err_link;
+	}
+	for (i = 1; i < SAHARA_MAX_HW_LINK; i++) {
+		dev->hw_phys_link[i] = dev->hw_phys_link[i - 1] +
+					sizeof(struct sahara_hw_link);
+		dev->hw_link[i] = dev->hw_link[i - 1] + 1;
+	}
+
+	crypto_init_queue(&dev->queue, SAHARA_QUEUE_LENGTH);
+
+	dev_ptr = dev;
+
+	tasklet_init(&dev->queue_task, sahara_aes_queue_task,
+		     (unsigned long)dev);
+	tasklet_init(&dev->done_task, sahara_aes_done_task,
+		     (unsigned long)dev);
+
+	init_timer(&dev->watchdog);
+	dev->watchdog.function = &sahara_watchdog;
+	dev->watchdog.data = (unsigned long)dev;
+
+	clk_prepare_enable(dev->clk_ipg);
+	clk_prepare_enable(dev->clk_ahb);
+
+	version = sahara_read(dev, SAHARA_REG_VERSION);
+	if (version != SAHARA_VERSION_3) {
+		dev_err(&pdev->dev, "SAHARA version %d not supported\n",
+			version);
+		err = -ENODEV;
+		goto err_algs;
+	}
+
+	sahara_write(dev, SAHARA_CMD_RESET | SAHARA_CMD_MODE_BATCH,
+		     SAHARA_REG_CMD);
+	sahara_write(dev, SAHARA_CONTROL_SET_THROTTLE(0) |
+			SAHARA_CONTROL_SET_MAXBURST(8) |
+			SAHARA_CONTROL_RNG_AUTORSD |
+			SAHARA_CONTROL_ENABLE_INT,
+			SAHARA_REG_CONTROL);
+
+	err = sahara_register_algs(dev);
+	if (err)
+		goto err_algs;
+
+	dev_info(&pdev->dev, "SAHARA version %d initialized\n", version);
+
+	return 0;
+
+err_algs:
+	dma_free_coherent(&pdev->dev,
+			  SAHARA_MAX_HW_LINK * sizeof(struct sahara_hw_link),
+			  dev->hw_link[0], dev->hw_phys_link[0]);
+	clk_disable_unprepare(dev->clk_ipg);
+	clk_disable_unprepare(dev->clk_ahb);
+	dev_ptr = NULL;
+err_link:
+	dma_free_coherent(&pdev->dev,
+			  2 * AES_KEYSIZE_128,
+			  dev->key_base, dev->key_phys_base);
+err_key:
+	dma_free_coherent(&pdev->dev,
+			  SAHARA_MAX_HW_DESC * sizeof(struct sahara_hw_desc),
+			  dev->hw_desc[0], dev->hw_phys_desc[0]);
+
+	return err;
+}
+
+static int sahara_remove(struct platform_device *pdev)
+{
+	struct sahara_dev *dev = platform_get_drvdata(pdev);
+
+	dma_free_coherent(&pdev->dev,
+			  SAHARA_MAX_HW_LINK * sizeof(struct sahara_hw_link),
+			  dev->hw_link[0], dev->hw_phys_link[0]);
+	dma_free_coherent(&pdev->dev,
+			  2 * AES_KEYSIZE_128,
+			  dev->key_base, dev->key_phys_base);
+	dma_free_coherent(&pdev->dev,
+			  SAHARA_MAX_HW_DESC * sizeof(struct sahara_hw_desc),
+			  dev->hw_desc[0], dev->hw_phys_desc[0]);
+
+	tasklet_kill(&dev->done_task);
+	tasklet_kill(&dev->queue_task);
+
+	sahara_unregister_algs(dev);
+
+	clk_disable_unprepare(dev->clk_ipg);
+	clk_disable_unprepare(dev->clk_ahb);
+
+	dev_ptr = NULL;
+
+	return 0;
+}
+
+static struct platform_driver sahara_driver = {
+	.probe		= sahara_probe,
+	.remove		= sahara_remove,
+	.driver		= {
+		.name	= SAHARA_NAME,
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(sahara_dt_ids),
+	},
+	.id_table = sahara_platform_ids,
+};
+
+module_platform_driver(sahara_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Javier Martin <javier.martin@vista-silicon.com>");
+MODULE_DESCRIPTION("SAHARA2 HW crypto accelerator");
diff --git a/drivers/crypto/ux500/cryp/cryp.c b/drivers/crypto/ux500/cryp/cryp.c
index e208ceaf81c9..3eafa903ebcd 100644
--- a/drivers/crypto/ux500/cryp/cryp.c
+++ b/drivers/crypto/ux500/cryp/cryp.c
@@ -12,8 +12,6 @@
 #include <linux/kernel.h>
 #include <linux/types.h>
 
-#include <mach/hardware.h>
-
 #include "cryp_p.h"
 #include "cryp.h"
 
diff --git a/drivers/crypto/ux500/cryp/cryp_core.c b/drivers/crypto/ux500/cryp/cryp_core.c
index 22c9063e0120..32f480622b97 100644
--- a/drivers/crypto/ux500/cryp/cryp_core.c
+++ b/drivers/crypto/ux500/cryp/cryp_core.c
@@ -32,7 +32,6 @@
 #include <crypto/scatterwalk.h>
 
 #include <linux/platform_data/crypto-ux500.h>
-#include <mach/hardware.h>
 
 #include "cryp_p.h"
 #include "cryp.h"
diff --git a/drivers/crypto/ux500/hash/hash_core.c b/drivers/crypto/ux500/hash/hash_core.c
index 632c3339895f..cf5508967539 100644
--- a/drivers/crypto/ux500/hash/hash_core.c
+++ b/drivers/crypto/ux500/hash/hash_core.c
@@ -32,7 +32,6 @@
 #include <crypto/algapi.h>
 
 #include <linux/platform_data/crypto-ux500.h>
-#include <mach/hardware.h>
 
 #include "hash_alg.h"
 
@@ -939,6 +938,7 @@ static int hash_dma_final(struct ahash_request *req)
 	if (!ctx->device->dma.nents) {
 		dev_err(device_data->dev, "[%s] "
 				"ctx->device->dma.nents = 0", __func__);
+		ret = ctx->device->dma.nents;
 		goto out;
 	}
 
@@ -946,6 +946,7 @@ static int hash_dma_final(struct ahash_request *req)
 	if (bytes_written != req->nbytes) {
 		dev_err(device_data->dev, "[%s] "
 				"hash_dma_write() failed!", __func__);
+		ret = bytes_written;
 		goto out;
 	}
 
@@ -1368,14 +1369,12 @@ static int hash_setkey(struct crypto_ahash *tfm,
 	/**
 	 * Freed in final.
 	 */
-	ctx->key = kmalloc(keylen, GFP_KERNEL);
+	ctx->key = kmemdup(key, keylen, GFP_KERNEL);
 	if (!ctx->key) {
 		pr_err(DEV_DBG_NAME " [%s] Failed to allocate ctx->key "
 		       "for %d\n", __func__, alg);
 		return -ENOMEM;
 	}
-
-	memcpy(ctx->key, key, keylen);
 	ctx->keylen = keylen;
 
 	return ret;
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index aeaea32bcfda..e9924898043a 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -63,8 +63,6 @@ config INTEL_IOATDMA
 	depends on PCI && X86
 	select DMA_ENGINE
 	select DCA
-	select ASYNC_TX_DISABLE_PQ_VAL_DMA
-	select ASYNC_TX_DISABLE_XOR_VAL_DMA
 	help
 	  Enable support for the Intel(R) I/OAT DMA engine present
 	  in recent Intel Xeon chipsets.
@@ -174,15 +172,7 @@ config TEGRA20_APB_DMA
 	  This DMA controller transfers data from memory to peripheral fifo
 	  or vice versa. It does not support memory to memory data transfer.
 
-
-
-config SH_DMAE
-	tristate "Renesas SuperH DMAC support"
-	depends on (SUPERH && SH_DMA) || (ARM && ARCH_SHMOBILE)
-	depends on !SH_DMA_API
-	select DMA_ENGINE
-	help
-	  Enable support for the Renesas SuperH DMA controllers.
+source "drivers/dma/sh/Kconfig"
 
 config COH901318
 	bool "ST-Ericsson COH901318 DMA support"
@@ -328,6 +318,10 @@ config DMA_ENGINE
 config DMA_VIRTUAL_CHANNELS
 	tristate
 
+config DMA_ACPI
+	def_bool y
+	depends on ACPI
+
 config DMA_OF
 	def_bool y
 	depends on OF
diff --git a/drivers/dma/Makefile b/drivers/dma/Makefile
index 488e3ff85b52..a2b0df591f95 100644
--- a/drivers/dma/Makefile
+++ b/drivers/dma/Makefile
@@ -3,6 +3,7 @@ ccflags-$(CONFIG_DMADEVICES_VDEBUG) += -DVERBOSE_DEBUG
 
 obj-$(CONFIG_DMA_ENGINE) += dmaengine.o
 obj-$(CONFIG_DMA_VIRTUAL_CHANNELS) += virt-dma.o
+obj-$(CONFIG_DMA_ACPI) += acpi-dma.o
 obj-$(CONFIG_DMA_OF) += of-dma.o
 
 obj-$(CONFIG_NET_DMA) += iovlock.o
@@ -18,7 +19,7 @@ obj-$(CONFIG_DW_DMAC) += dw_dmac.o
 obj-$(CONFIG_AT_HDMAC) += at_hdmac.o
 obj-$(CONFIG_MX3_IPU) += ipu/
 obj-$(CONFIG_TXX9_DMAC) += txx9dmac.o
-obj-$(CONFIG_SH_DMAE) += sh/
+obj-$(CONFIG_SH_DMAE_BASE) += sh/
 obj-$(CONFIG_COH901318) += coh901318.o coh901318_lli.o
 obj-$(CONFIG_AMCC_PPC440SPE_ADMA) += ppc4xx/
 obj-$(CONFIG_IMX_SDMA) += imx-sdma.o
diff --git a/drivers/dma/acpi-dma.c b/drivers/dma/acpi-dma.c
new file mode 100644
index 000000000000..ba6fc62e9651
--- /dev/null
+++ b/drivers/dma/acpi-dma.c
@@ -0,0 +1,279 @@
+/*
+ * ACPI helpers for DMA request / controller
+ *
+ * Based on of-dma.c
+ *
+ * Copyright (C) 2013, Intel Corporation
+ * Author: Andy Shevchenko <andriy.shevchenko@linux.intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/acpi.h>
+#include <linux/acpi_dma.h>
+
+static LIST_HEAD(acpi_dma_list);
+static DEFINE_MUTEX(acpi_dma_lock);
+
+/**
+ * acpi_dma_controller_register - Register a DMA controller to ACPI DMA helpers
+ * @dev:		struct device of DMA controller
+ * @acpi_dma_xlate:	translation function which converts a dma specifier
+ *			into a dma_chan structure
+ * @data		pointer to controller specific data to be used by
+ *			translation function
+ *
+ * Returns 0 on success or appropriate errno value on error.
+ *
+ * Allocated memory should be freed with appropriate acpi_dma_controller_free()
+ * call.
+ */
+int acpi_dma_controller_register(struct device *dev,
+		struct dma_chan *(*acpi_dma_xlate)
+		(struct acpi_dma_spec *, struct acpi_dma *),
+		void *data)
+{
+	struct acpi_device *adev;
+	struct acpi_dma	*adma;
+
+	if (!dev || !acpi_dma_xlate)
+		return -EINVAL;
+
+	/* Check if the device was enumerated by ACPI */
+	if (!ACPI_HANDLE(dev))
+		return -EINVAL;
+
+	if (acpi_bus_get_device(ACPI_HANDLE(dev), &adev))
+		return -EINVAL;
+
+	adma = kzalloc(sizeof(*adma), GFP_KERNEL);
+	if (!adma)
+		return -ENOMEM;
+
+	adma->dev = dev;
+	adma->acpi_dma_xlate = acpi_dma_xlate;
+	adma->data = data;
+
+	/* Now queue acpi_dma controller structure in list */
+	mutex_lock(&acpi_dma_lock);
+	list_add_tail(&adma->dma_controllers, &acpi_dma_list);
+	mutex_unlock(&acpi_dma_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(acpi_dma_controller_register);
+
+/**
+ * acpi_dma_controller_free - Remove a DMA controller from ACPI DMA helpers list
+ * @dev:	struct device of DMA controller
+ *
+ * Memory allocated by acpi_dma_controller_register() is freed here.
+ */
+int acpi_dma_controller_free(struct device *dev)
+{
+	struct acpi_dma *adma;
+
+	if (!dev)
+		return -EINVAL;
+
+	mutex_lock(&acpi_dma_lock);
+
+	list_for_each_entry(adma, &acpi_dma_list, dma_controllers)
+		if (adma->dev == dev) {
+			list_del(&adma->dma_controllers);
+			mutex_unlock(&acpi_dma_lock);
+			kfree(adma);
+			return 0;
+		}
+
+	mutex_unlock(&acpi_dma_lock);
+	return -ENODEV;
+}
+EXPORT_SYMBOL_GPL(acpi_dma_controller_free);
+
+static void devm_acpi_dma_release(struct device *dev, void *res)
+{
+	acpi_dma_controller_free(dev);
+}
+
+/**
+ * devm_acpi_dma_controller_register - resource managed acpi_dma_controller_register()
+ * @dev:		device that is registering this DMA controller
+ * @acpi_dma_xlate:	translation function
+ * @data		pointer to controller specific data
+ *
+ * Managed acpi_dma_controller_register(). DMA controller registered by this
+ * function are automatically freed on driver detach. See
+ * acpi_dma_controller_register() for more information.
+ */
+int devm_acpi_dma_controller_register(struct device *dev,
+		struct dma_chan *(*acpi_dma_xlate)
+		(struct acpi_dma_spec *, struct acpi_dma *),
+		void *data)
+{
+	void *res;
+	int ret;
+
+	res = devres_alloc(devm_acpi_dma_release, 0, GFP_KERNEL);
+	if (!res)
+		return -ENOMEM;
+
+	ret = acpi_dma_controller_register(dev, acpi_dma_xlate, data);
+	if (ret) {
+		devres_free(res);
+		return ret;
+	}
+	devres_add(dev, res);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(devm_acpi_dma_controller_register);
+
+/**
+ * devm_acpi_dma_controller_free - resource managed acpi_dma_controller_free()
+ *
+ * Unregister a DMA controller registered with
+ * devm_acpi_dma_controller_register(). Normally this function will not need to
+ * be called and the resource management code will ensure that the resource is
+ * freed.
+ */
+void devm_acpi_dma_controller_free(struct device *dev)
+{
+	WARN_ON(devres_destroy(dev, devm_acpi_dma_release, NULL, NULL));
+}
+EXPORT_SYMBOL_GPL(devm_acpi_dma_controller_free);
+
+struct acpi_dma_parser_data {
+	struct acpi_dma_spec dma_spec;
+	size_t index;
+	size_t n;
+};
+
+/**
+ * acpi_dma_parse_fixed_dma - Parse FixedDMA ACPI resources to a DMA specifier
+ * @res:	struct acpi_resource to get FixedDMA resources from
+ * @data:	pointer to a helper struct acpi_dma_parser_data
+ */
+static int acpi_dma_parse_fixed_dma(struct acpi_resource *res, void *data)
+{
+	struct acpi_dma_parser_data *pdata = data;
+
+	if (res->type == ACPI_RESOURCE_TYPE_FIXED_DMA) {
+		struct acpi_resource_fixed_dma *dma = &res->data.fixed_dma;
+
+		if (pdata->n++ == pdata->index) {
+			pdata->dma_spec.chan_id = dma->channels;
+			pdata->dma_spec.slave_id = dma->request_lines;
+		}
+	}
+
+	/* Tell the ACPI core to skip this resource */
+	return 1;
+}
+
+/**
+ * acpi_dma_request_slave_chan_by_index - Get the DMA slave channel
+ * @dev:	struct device to get DMA request from
+ * @index:	index of FixedDMA descriptor for @dev
+ *
+ * Returns pointer to appropriate dma channel on success or NULL on error.
+ */
+struct dma_chan *acpi_dma_request_slave_chan_by_index(struct device *dev,
+		size_t index)
+{
+	struct acpi_dma_parser_data pdata;
+	struct acpi_dma_spec *dma_spec = &pdata.dma_spec;
+	struct list_head resource_list;
+	struct acpi_device *adev;
+	struct acpi_dma *adma;
+	struct dma_chan *chan = NULL;
+
+	/* Check if the device was enumerated by ACPI */
+	if (!dev || !ACPI_HANDLE(dev))
+		return NULL;
+
+	if (acpi_bus_get_device(ACPI_HANDLE(dev), &adev))
+		return NULL;
+
+	memset(&pdata, 0, sizeof(pdata));
+	pdata.index = index;
+
+	/* Initial values for the request line and channel */
+	dma_spec->chan_id = -1;
+	dma_spec->slave_id = -1;
+
+	INIT_LIST_HEAD(&resource_list);
+	acpi_dev_get_resources(adev, &resource_list,
+			acpi_dma_parse_fixed_dma, &pdata);
+	acpi_dev_free_resource_list(&resource_list);
+
+	if (dma_spec->slave_id < 0 || dma_spec->chan_id < 0)
+		return NULL;
+
+	mutex_lock(&acpi_dma_lock);
+
+	list_for_each_entry(adma, &acpi_dma_list, dma_controllers) {
+		dma_spec->dev = adma->dev;
+		chan = adma->acpi_dma_xlate(dma_spec, adma);
+		if (chan)
+			break;
+	}
+
+	mutex_unlock(&acpi_dma_lock);
+	return chan;
+}
+EXPORT_SYMBOL_GPL(acpi_dma_request_slave_chan_by_index);
+
+/**
+ * acpi_dma_request_slave_chan_by_name - Get the DMA slave channel
+ * @dev:	struct device to get DMA request from
+ * @name:	represents corresponding FixedDMA descriptor for @dev
+ *
+ * In order to support both Device Tree and ACPI in a single driver we
+ * translate the names "tx" and "rx" here based on the most common case where
+ * the first FixedDMA descriptor is TX and second is RX.
+ *
+ * Returns pointer to appropriate dma channel on success or NULL on error.
+ */
+struct dma_chan *acpi_dma_request_slave_chan_by_name(struct device *dev,
+		const char *name)
+{
+	size_t index;
+
+	if (!strcmp(name, "tx"))
+		index = 0;
+	else if (!strcmp(name, "rx"))
+		index = 1;
+	else
+		return NULL;
+
+	return acpi_dma_request_slave_chan_by_index(dev, index);
+}
+EXPORT_SYMBOL_GPL(acpi_dma_request_slave_chan_by_name);
+
+/**
+ * acpi_dma_simple_xlate - Simple ACPI DMA engine translation helper
+ * @dma_spec: pointer to ACPI DMA specifier
+ * @adma: pointer to ACPI DMA controller data
+ *
+ * A simple translation function for ACPI based devices. Passes &struct
+ * dma_spec to the DMA controller driver provided filter function. Returns
+ * pointer to the channel if found or %NULL otherwise.
+ */
+struct dma_chan *acpi_dma_simple_xlate(struct acpi_dma_spec *dma_spec,
+		struct acpi_dma *adma)
+{
+	struct acpi_dma_filter_info *info = adma->data;
+
+	if (!info || !info->filter_fn)
+		return NULL;
+
+	return dma_request_channel(info->dma_cap, info->filter_fn, dma_spec);
+}
+EXPORT_SYMBOL_GPL(acpi_dma_simple_xlate);
diff --git a/drivers/dma/at_hdmac.c b/drivers/dma/at_hdmac.c
index 88cfc61329d2..e923cda930f9 100644
--- a/drivers/dma/at_hdmac.c
+++ b/drivers/dma/at_hdmac.c
@@ -24,6 +24,7 @@
 #include <linux/slab.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
+#include <linux/of_dma.h>
 
 #include "at_hdmac_regs.h"
 #include "dmaengine.h"
@@ -677,7 +678,7 @@ atc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 		ctrlb |=  ATC_DST_ADDR_MODE_FIXED
 			| ATC_SRC_ADDR_MODE_INCR
 			| ATC_FC_MEM2PER
-			| ATC_SIF(AT_DMA_MEM_IF) | ATC_DIF(AT_DMA_PER_IF);
+			| ATC_SIF(atchan->mem_if) | ATC_DIF(atchan->per_if);
 		reg = sconfig->dst_addr;
 		for_each_sg(sgl, sg, sg_len, i) {
 			struct at_desc	*desc;
@@ -716,7 +717,7 @@ atc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 		ctrlb |=  ATC_DST_ADDR_MODE_INCR
 			| ATC_SRC_ADDR_MODE_FIXED
 			| ATC_FC_PER2MEM
-			| ATC_SIF(AT_DMA_PER_IF) | ATC_DIF(AT_DMA_MEM_IF);
+			| ATC_SIF(atchan->per_if) | ATC_DIF(atchan->mem_if);
 
 		reg = sconfig->src_addr;
 		for_each_sg(sgl, sg, sg_len, i) {
@@ -822,8 +823,8 @@ atc_dma_cyclic_fill_desc(struct dma_chan *chan, struct at_desc *desc,
 		desc->lli.ctrlb = ATC_DST_ADDR_MODE_FIXED
 				| ATC_SRC_ADDR_MODE_INCR
 				| ATC_FC_MEM2PER
-				| ATC_SIF(AT_DMA_MEM_IF)
-				| ATC_DIF(AT_DMA_PER_IF);
+				| ATC_SIF(atchan->mem_if)
+				| ATC_DIF(atchan->per_if);
 		break;
 
 	case DMA_DEV_TO_MEM:
@@ -833,8 +834,8 @@ atc_dma_cyclic_fill_desc(struct dma_chan *chan, struct at_desc *desc,
 		desc->lli.ctrlb = ATC_DST_ADDR_MODE_INCR
 				| ATC_SRC_ADDR_MODE_FIXED
 				| ATC_FC_PER2MEM
-				| ATC_SIF(AT_DMA_PER_IF)
-				| ATC_DIF(AT_DMA_MEM_IF);
+				| ATC_SIF(atchan->per_if)
+				| ATC_DIF(atchan->mem_if);
 		break;
 
 	default:
@@ -1188,6 +1189,67 @@ static void atc_free_chan_resources(struct dma_chan *chan)
 	dev_vdbg(chan2dev(chan), "free_chan_resources: done\n");
 }
 
+#ifdef CONFIG_OF
+static bool at_dma_filter(struct dma_chan *chan, void *slave)
+{
+	struct at_dma_slave *atslave = slave;
+
+	if (atslave->dma_dev == chan->device->dev) {
+		chan->private = atslave;
+		return true;
+	} else {
+		return false;
+	}
+}
+
+static struct dma_chan *at_dma_xlate(struct of_phandle_args *dma_spec,
+				     struct of_dma *of_dma)
+{
+	struct dma_chan *chan;
+	struct at_dma_chan *atchan;
+	struct at_dma_slave *atslave;
+	dma_cap_mask_t mask;
+	unsigned int per_id;
+	struct platform_device *dmac_pdev;
+
+	if (dma_spec->args_count != 2)
+		return NULL;
+
+	dmac_pdev = of_find_device_by_node(dma_spec->np);
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+
+	atslave = devm_kzalloc(&dmac_pdev->dev, sizeof(*atslave), GFP_KERNEL);
+	if (!atslave)
+		return NULL;
+	/*
+	 * We can fill both SRC_PER and DST_PER, one of these fields will be
+	 * ignored depending on DMA transfer direction.
+	 */
+	per_id = dma_spec->args[1];
+	atslave->cfg = ATC_FIFOCFG_HALFFIFO | ATC_DST_H2SEL_HW
+		      | ATC_SRC_H2SEL_HW | ATC_DST_PER(per_id)
+		      | ATC_SRC_PER(per_id);
+	atslave->dma_dev = &dmac_pdev->dev;
+
+	chan = dma_request_channel(mask, at_dma_filter, atslave);
+	if (!chan)
+		return NULL;
+
+	atchan = to_at_dma_chan(chan);
+	atchan->per_if = dma_spec->args[0] & 0xff;
+	atchan->mem_if = (dma_spec->args[0] >> 16) & 0xff;
+
+	return chan;
+}
+#else
+static struct dma_chan *at_dma_xlate(struct of_phandle_args *dma_spec,
+				     struct of_dma *of_dma)
+{
+	return NULL;
+}
+#endif
 
 /*--  Module Management  -----------------------------------------------*/
 
@@ -1342,6 +1404,8 @@ static int __init at_dma_probe(struct platform_device *pdev)
 	for (i = 0; i < plat_dat->nr_channels; i++) {
 		struct at_dma_chan	*atchan = &atdma->chan[i];
 
+		atchan->mem_if = AT_DMA_MEM_IF;
+		atchan->per_if = AT_DMA_PER_IF;
 		atchan->chan_common.device = &atdma->dma_common;
 		dma_cookie_init(&atchan->chan_common);
 		list_add_tail(&atchan->chan_common.device_node,
@@ -1388,8 +1452,25 @@ static int __init at_dma_probe(struct platform_device *pdev)
 
 	dma_async_device_register(&atdma->dma_common);
 
+	/*
+	 * Do not return an error if the dmac node is not present in order to
+	 * not break the existing way of requesting channel with
+	 * dma_request_channel().
+	 */
+	if (pdev->dev.of_node) {
+		err = of_dma_controller_register(pdev->dev.of_node,
+						 at_dma_xlate, atdma);
+		if (err) {
+			dev_err(&pdev->dev, "could not register of_dma_controller\n");
+			goto err_of_dma_controller_register;
+		}
+	}
+
 	return 0;
 
+err_of_dma_controller_register:
+	dma_async_device_unregister(&atdma->dma_common);
+	dma_pool_destroy(atdma->dma_desc_pool);
 err_pool_create:
 	platform_set_drvdata(pdev, NULL);
 	free_irq(platform_get_irq(pdev, 0), atdma);
@@ -1406,7 +1487,7 @@ err_kfree:
 	return err;
 }
 
-static int __exit at_dma_remove(struct platform_device *pdev)
+static int at_dma_remove(struct platform_device *pdev)
 {
 	struct at_dma		*atdma = platform_get_drvdata(pdev);
 	struct dma_chan		*chan, *_chan;
@@ -1564,7 +1645,7 @@ static const struct dev_pm_ops at_dma_dev_pm_ops = {
 };
 
 static struct platform_driver at_dma_driver = {
-	.remove		= __exit_p(at_dma_remove),
+	.remove		= at_dma_remove,
 	.shutdown	= at_dma_shutdown,
 	.id_table	= atdma_devtypes,
 	.driver = {
diff --git a/drivers/dma/at_hdmac_regs.h b/drivers/dma/at_hdmac_regs.h
index 0eb3c1388667..c604d26fd4d3 100644
--- a/drivers/dma/at_hdmac_regs.h
+++ b/drivers/dma/at_hdmac_regs.h
@@ -220,6 +220,8 @@ enum atc_status {
  * @device: parent device
  * @ch_regs: memory mapped register base
  * @mask: channel index in a mask
+ * @per_if: peripheral interface
+ * @mem_if: memory interface
  * @status: transmit status information from irq/prep* functions
  *                to tasklet (use atomic operations)
  * @tasklet: bottom half to finish transaction work
@@ -238,6 +240,8 @@ struct at_dma_chan {
 	struct at_dma		*device;
 	void __iomem		*ch_regs;
 	u8			mask;
+	u8			per_if;
+	u8			mem_if;
 	unsigned long		status;
 	struct tasklet_struct	tasklet;
 	u32			save_cfg;
diff --git a/drivers/dma/coh901318.c b/drivers/dma/coh901318.c
index 797940e532ff..3b23061cdb41 100644
--- a/drivers/dma/coh901318.c
+++ b/drivers/dma/coh901318.c
@@ -2748,7 +2748,7 @@ static int __init coh901318_probe(struct platform_device *pdev)
 	return err;
 }
 
-static int __exit coh901318_remove(struct platform_device *pdev)
+static int coh901318_remove(struct platform_device *pdev)
 {
 	struct coh901318_base *base = platform_get_drvdata(pdev);
 
@@ -2760,7 +2760,7 @@ static int __exit coh901318_remove(struct platform_device *pdev)
 
 
 static struct platform_driver coh901318_driver = {
-	.remove = __exit_p(coh901318_remove),
+	.remove = coh901318_remove,
 	.driver = {
 		.name	= "coh901318",
 	},
diff --git a/drivers/dma/dmaengine.c b/drivers/dma/dmaengine.c
index b2728d6ba2fd..93f7992bee5c 100644
--- a/drivers/dma/dmaengine.c
+++ b/drivers/dma/dmaengine.c
@@ -62,6 +62,8 @@
 #include <linux/rculist.h>
 #include <linux/idr.h>
 #include <linux/slab.h>
+#include <linux/acpi.h>
+#include <linux/acpi_dma.h>
 #include <linux/of_dma.h>
 
 static DEFINE_MUTEX(dma_list_mutex);
@@ -174,7 +176,8 @@ static struct class dma_devclass = {
 #define dma_device_satisfies_mask(device, mask) \
 	__dma_device_satisfies_mask((device), &(mask))
 static int
-__dma_device_satisfies_mask(struct dma_device *device, dma_cap_mask_t *want)
+__dma_device_satisfies_mask(struct dma_device *device,
+			    const dma_cap_mask_t *want)
 {
 	dma_cap_mask_t has;
 
@@ -463,7 +466,8 @@ static void dma_channel_rebalance(void)
 		}
 }
 
-static struct dma_chan *private_candidate(dma_cap_mask_t *mask, struct dma_device *dev,
+static struct dma_chan *private_candidate(const dma_cap_mask_t *mask,
+					  struct dma_device *dev,
 					  dma_filter_fn fn, void *fn_param)
 {
 	struct dma_chan *chan;
@@ -505,7 +509,8 @@ static struct dma_chan *private_candidate(dma_cap_mask_t *mask, struct dma_devic
  * @fn: optional callback to disposition available channels
  * @fn_param: opaque parameter to pass to dma_filter_fn
  */
-struct dma_chan *__dma_request_channel(dma_cap_mask_t *mask, dma_filter_fn fn, void *fn_param)
+struct dma_chan *__dma_request_channel(const dma_cap_mask_t *mask,
+				       dma_filter_fn fn, void *fn_param)
 {
 	struct dma_device *device, *_d;
 	struct dma_chan *chan = NULL;
@@ -555,12 +560,16 @@ EXPORT_SYMBOL_GPL(__dma_request_channel);
  * @dev:	pointer to client device structure
  * @name:	slave channel name
  */
-struct dma_chan *dma_request_slave_channel(struct device *dev, char *name)
+struct dma_chan *dma_request_slave_channel(struct device *dev, const char *name)
 {
 	/* If device-tree is present get slave info from here */
 	if (dev->of_node)
 		return of_dma_request_slave_channel(dev->of_node, name);
 
+	/* If device was enumerated by ACPI get slave info from here */
+	if (ACPI_HANDLE(dev))
+		return acpi_dma_request_slave_chan_by_name(dev, name);
+
 	return NULL;
 }
 EXPORT_SYMBOL_GPL(dma_request_slave_channel);
diff --git a/drivers/dma/dmatest.c b/drivers/dma/dmatest.c
index a2c8904b63ea..d8ce4ecfef18 100644
--- a/drivers/dma/dmatest.c
+++ b/drivers/dma/dmatest.c
@@ -2,6 +2,7 @@
  * DMA Engine test module
  *
  * Copyright (C) 2007 Atmel Corporation
+ * Copyright (C) 2013 Intel Corporation
  *
  * 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
@@ -18,6 +19,10 @@
 #include <linux/random.h>
 #include <linux/slab.h>
 #include <linux/wait.h>
+#include <linux/ctype.h>
+#include <linux/debugfs.h>
+#include <linux/uaccess.h>
+#include <linux/seq_file.h>
 
 static unsigned int test_buf_size = 16384;
 module_param(test_buf_size, uint, S_IRUGO);
@@ -61,6 +66,9 @@ module_param(timeout, uint, S_IRUGO);
 MODULE_PARM_DESC(timeout, "Transfer Timeout in msec (default: 3000), "
 		 "Pass -1 for infinite timeout");
 
+/* Maximum amount of mismatched bytes in buffer to print */
+#define MAX_ERROR_COUNT		32
+
 /*
  * Initialization patterns. All bytes in the source buffer has bit 7
  * set, all bytes in the destination buffer has bit 7 cleared.
@@ -78,13 +86,65 @@ MODULE_PARM_DESC(timeout, "Transfer Timeout in msec (default: 3000), "
 #define PATTERN_OVERWRITE	0x20
 #define PATTERN_COUNT_MASK	0x1f
 
+enum dmatest_error_type {
+	DMATEST_ET_OK,
+	DMATEST_ET_MAP_SRC,
+	DMATEST_ET_MAP_DST,
+	DMATEST_ET_PREP,
+	DMATEST_ET_SUBMIT,
+	DMATEST_ET_TIMEOUT,
+	DMATEST_ET_DMA_ERROR,
+	DMATEST_ET_DMA_IN_PROGRESS,
+	DMATEST_ET_VERIFY,
+	DMATEST_ET_VERIFY_BUF,
+};
+
+struct dmatest_verify_buffer {
+	unsigned int	index;
+	u8		expected;
+	u8		actual;
+};
+
+struct dmatest_verify_result {
+	unsigned int			error_count;
+	struct dmatest_verify_buffer	data[MAX_ERROR_COUNT];
+	u8				pattern;
+	bool				is_srcbuf;
+};
+
+struct dmatest_thread_result {
+	struct list_head	node;
+	unsigned int		n;
+	unsigned int		src_off;
+	unsigned int		dst_off;
+	unsigned int		len;
+	enum dmatest_error_type	type;
+	union {
+		unsigned long			data;
+		dma_cookie_t			cookie;
+		enum dma_status			status;
+		int				error;
+		struct dmatest_verify_result	*vr;
+	};
+};
+
+struct dmatest_result {
+	struct list_head	node;
+	char			*name;
+	struct list_head	results;
+};
+
+struct dmatest_info;
+
 struct dmatest_thread {
 	struct list_head	node;
+	struct dmatest_info	*info;
 	struct task_struct	*task;
 	struct dma_chan		*chan;
 	u8			**srcs;
 	u8			**dsts;
 	enum dma_transaction_type type;
+	bool			done;
 };
 
 struct dmatest_chan {
@@ -93,25 +153,69 @@ struct dmatest_chan {
 	struct list_head	threads;
 };
 
-/*
- * These are protected by dma_list_mutex since they're only used by
- * the DMA filter function callback
+/**
+ * struct dmatest_params - test parameters.
+ * @buf_size:		size of the memcpy test buffer
+ * @channel:		bus ID of the channel to test
+ * @device:		bus ID of the DMA Engine to test
+ * @threads_per_chan:	number of threads to start per channel
+ * @max_channels:	maximum number of channels to use
+ * @iterations:		iterations before stopping test
+ * @xor_sources:	number of xor source buffers
+ * @pq_sources:		number of p+q source buffers
+ * @timeout:		transfer timeout in msec, -1 for infinite timeout
  */
-static LIST_HEAD(dmatest_channels);
-static unsigned int nr_channels;
+struct dmatest_params {
+	unsigned int	buf_size;
+	char		channel[20];
+	char		device[20];
+	unsigned int	threads_per_chan;
+	unsigned int	max_channels;
+	unsigned int	iterations;
+	unsigned int	xor_sources;
+	unsigned int	pq_sources;
+	int		timeout;
+};
 
-static bool dmatest_match_channel(struct dma_chan *chan)
+/**
+ * struct dmatest_info - test information.
+ * @params:		test parameters
+ * @lock:		access protection to the fields of this structure
+ */
+struct dmatest_info {
+	/* Test parameters */
+	struct dmatest_params	params;
+
+	/* Internal state */
+	struct list_head	channels;
+	unsigned int		nr_channels;
+	struct mutex		lock;
+
+	/* debugfs related stuff */
+	struct dentry		*root;
+	struct dmatest_params	dbgfs_params;
+
+	/* Test results */
+	struct list_head	results;
+	struct mutex		results_lock;
+};
+
+static struct dmatest_info test_info;
+
+static bool dmatest_match_channel(struct dmatest_params *params,
+		struct dma_chan *chan)
 {
-	if (test_channel[0] == '\0')
+	if (params->channel[0] == '\0')
 		return true;
-	return strcmp(dma_chan_name(chan), test_channel) == 0;
+	return strcmp(dma_chan_name(chan), params->channel) == 0;
 }
 
-static bool dmatest_match_device(struct dma_device *device)
+static bool dmatest_match_device(struct dmatest_params *params,
+		struct dma_device *device)
 {
-	if (test_device[0] == '\0')
+	if (params->device[0] == '\0')
 		return true;
-	return strcmp(dev_name(device->dev), test_device) == 0;
+	return strcmp(dev_name(device->dev), params->device) == 0;
 }
 
 static unsigned long dmatest_random(void)
@@ -122,7 +226,8 @@ static unsigned long dmatest_random(void)
 	return buf;
 }
 
-static void dmatest_init_srcs(u8 **bufs, unsigned int start, unsigned int len)
+static void dmatest_init_srcs(u8 **bufs, unsigned int start, unsigned int len,
+		unsigned int buf_size)
 {
 	unsigned int i;
 	u8 *buf;
@@ -133,13 +238,14 @@ static void dmatest_init_srcs(u8 **bufs, unsigned int start, unsigned int len)
 		for ( ; i < start + len; i++)
 			buf[i] = PATTERN_SRC | PATTERN_COPY
 				| (~i & PATTERN_COUNT_MASK);
-		for ( ; i < test_buf_size; i++)
+		for ( ; i < buf_size; i++)
 			buf[i] = PATTERN_SRC | (~i & PATTERN_COUNT_MASK);
 		buf++;
 	}
 }
 
-static void dmatest_init_dsts(u8 **bufs, unsigned int start, unsigned int len)
+static void dmatest_init_dsts(u8 **bufs, unsigned int start, unsigned int len,
+		unsigned int buf_size)
 {
 	unsigned int i;
 	u8 *buf;
@@ -150,40 +256,14 @@ static void dmatest_init_dsts(u8 **bufs, unsigned int start, unsigned int len)
 		for ( ; i < start + len; i++)
 			buf[i] = PATTERN_DST | PATTERN_OVERWRITE
 				| (~i & PATTERN_COUNT_MASK);
-		for ( ; i < test_buf_size; i++)
+		for ( ; i < buf_size; i++)
 			buf[i] = PATTERN_DST | (~i & PATTERN_COUNT_MASK);
 	}
 }
 
-static void dmatest_mismatch(u8 actual, u8 pattern, unsigned int index,
-		unsigned int counter, bool is_srcbuf)
-{
-	u8		diff = actual ^ pattern;
-	u8		expected = pattern | (~counter & PATTERN_COUNT_MASK);
-	const char	*thread_name = current->comm;
-
-	if (is_srcbuf)
-		pr_warning("%s: srcbuf[0x%x] overwritten!"
-				" Expected %02x, got %02x\n",
-				thread_name, index, expected, actual);
-	else if ((pattern & PATTERN_COPY)
-			&& (diff & (PATTERN_COPY | PATTERN_OVERWRITE)))
-		pr_warning("%s: dstbuf[0x%x] not copied!"
-				" Expected %02x, got %02x\n",
-				thread_name, index, expected, actual);
-	else if (diff & PATTERN_SRC)
-		pr_warning("%s: dstbuf[0x%x] was copied!"
-				" Expected %02x, got %02x\n",
-				thread_name, index, expected, actual);
-	else
-		pr_warning("%s: dstbuf[0x%x] mismatch!"
-				" Expected %02x, got %02x\n",
-				thread_name, index, expected, actual);
-}
-
-static unsigned int dmatest_verify(u8 **bufs, unsigned int start,
-		unsigned int end, unsigned int counter, u8 pattern,
-		bool is_srcbuf)
+static unsigned int dmatest_verify(struct dmatest_verify_result *vr, u8 **bufs,
+		unsigned int start, unsigned int end, unsigned int counter,
+		u8 pattern, bool is_srcbuf)
 {
 	unsigned int i;
 	unsigned int error_count = 0;
@@ -191,6 +271,7 @@ static unsigned int dmatest_verify(u8 **bufs, unsigned int start,
 	u8 expected;
 	u8 *buf;
 	unsigned int counter_orig = counter;
+	struct dmatest_verify_buffer *vb;
 
 	for (; (buf = *bufs); bufs++) {
 		counter = counter_orig;
@@ -198,18 +279,21 @@ static unsigned int dmatest_verify(u8 **bufs, unsigned int start,
 			actual = buf[i];
 			expected = pattern | (~counter & PATTERN_COUNT_MASK);
 			if (actual != expected) {
-				if (error_count < 32)
-					dmatest_mismatch(actual, pattern, i,
-							 counter, is_srcbuf);
+				if (error_count < MAX_ERROR_COUNT && vr) {
+					vb = &vr->data[error_count];
+					vb->index = i;
+					vb->expected = expected;
+					vb->actual = actual;
+				}
 				error_count++;
 			}
 			counter++;
 		}
 	}
 
-	if (error_count > 32)
+	if (error_count > MAX_ERROR_COUNT)
 		pr_warning("%s: %u errors suppressed\n",
-			current->comm, error_count - 32);
+			current->comm, error_count - MAX_ERROR_COUNT);
 
 	return error_count;
 }
@@ -249,6 +333,170 @@ static unsigned int min_odd(unsigned int x, unsigned int y)
 	return val % 2 ? val : val - 1;
 }
 
+static char *verify_result_get_one(struct dmatest_verify_result *vr,
+		unsigned int i)
+{
+	struct dmatest_verify_buffer *vb = &vr->data[i];
+	u8 diff = vb->actual ^ vr->pattern;
+	static char buf[512];
+	char *msg;
+
+	if (vr->is_srcbuf)
+		msg = "srcbuf overwritten!";
+	else if ((vr->pattern & PATTERN_COPY)
+			&& (diff & (PATTERN_COPY | PATTERN_OVERWRITE)))
+		msg = "dstbuf not copied!";
+	else if (diff & PATTERN_SRC)
+		msg = "dstbuf was copied!";
+	else
+		msg = "dstbuf mismatch!";
+
+	snprintf(buf, sizeof(buf) - 1, "%s [0x%x] Expected %02x, got %02x", msg,
+		 vb->index, vb->expected, vb->actual);
+
+	return buf;
+}
+
+static char *thread_result_get(const char *name,
+		struct dmatest_thread_result *tr)
+{
+	static const char * const messages[] = {
+		[DMATEST_ET_OK]			= "No errors",
+		[DMATEST_ET_MAP_SRC]		= "src mapping error",
+		[DMATEST_ET_MAP_DST]		= "dst mapping error",
+		[DMATEST_ET_PREP]		= "prep error",
+		[DMATEST_ET_SUBMIT]		= "submit error",
+		[DMATEST_ET_TIMEOUT]		= "test timed out",
+		[DMATEST_ET_DMA_ERROR]		=
+			"got completion callback (DMA_ERROR)",
+		[DMATEST_ET_DMA_IN_PROGRESS]	=
+			"got completion callback (DMA_IN_PROGRESS)",
+		[DMATEST_ET_VERIFY]		= "errors",
+		[DMATEST_ET_VERIFY_BUF]		= "verify errors",
+	};
+	static char buf[512];
+
+	snprintf(buf, sizeof(buf) - 1,
+		 "%s: #%u: %s with src_off=0x%x ""dst_off=0x%x len=0x%x (%lu)",
+		 name, tr->n, messages[tr->type], tr->src_off, tr->dst_off,
+		 tr->len, tr->data);
+
+	return buf;
+}
+
+static int thread_result_add(struct dmatest_info *info,
+		struct dmatest_result *r, enum dmatest_error_type type,
+		unsigned int n, unsigned int src_off, unsigned int dst_off,
+		unsigned int len, unsigned long data)
+{
+	struct dmatest_thread_result *tr;
+
+	tr = kzalloc(sizeof(*tr), GFP_KERNEL);
+	if (!tr)
+		return -ENOMEM;
+
+	tr->type = type;
+	tr->n = n;
+	tr->src_off = src_off;
+	tr->dst_off = dst_off;
+	tr->len = len;
+	tr->data = data;
+
+	mutex_lock(&info->results_lock);
+	list_add_tail(&tr->node, &r->results);
+	mutex_unlock(&info->results_lock);
+
+	pr_warn("%s\n", thread_result_get(r->name, tr));
+	return 0;
+}
+
+static unsigned int verify_result_add(struct dmatest_info *info,
+		struct dmatest_result *r, unsigned int n,
+		unsigned int src_off, unsigned int dst_off, unsigned int len,
+		u8 **bufs, int whence, unsigned int counter, u8 pattern,
+		bool is_srcbuf)
+{
+	struct dmatest_verify_result *vr;
+	unsigned int error_count;
+	unsigned int buf_off = is_srcbuf ? src_off : dst_off;
+	unsigned int start, end;
+
+	if (whence < 0) {
+		start = 0;
+		end = buf_off;
+	} else if (whence > 0) {
+		start = buf_off + len;
+		end = info->params.buf_size;
+	} else {
+		start = buf_off;
+		end = buf_off + len;
+	}
+
+	vr = kmalloc(sizeof(*vr), GFP_KERNEL);
+	if (!vr) {
+		pr_warn("dmatest: No memory to store verify result\n");
+		return dmatest_verify(NULL, bufs, start, end, counter, pattern,
+				      is_srcbuf);
+	}
+
+	vr->pattern = pattern;
+	vr->is_srcbuf = is_srcbuf;
+
+	error_count = dmatest_verify(vr, bufs, start, end, counter, pattern,
+				     is_srcbuf);
+	if (error_count) {
+		vr->error_count = error_count;
+		thread_result_add(info, r, DMATEST_ET_VERIFY_BUF, n, src_off,
+				  dst_off, len, (unsigned long)vr);
+		return error_count;
+	}
+
+	kfree(vr);
+	return 0;
+}
+
+static void result_free(struct dmatest_info *info, const char *name)
+{
+	struct dmatest_result *r, *_r;
+
+	mutex_lock(&info->results_lock);
+	list_for_each_entry_safe(r, _r, &info->results, node) {
+		struct dmatest_thread_result *tr, *_tr;
+
+		if (name && strcmp(r->name, name))
+			continue;
+
+		list_for_each_entry_safe(tr, _tr, &r->results, node) {
+			if (tr->type == DMATEST_ET_VERIFY_BUF)
+				kfree(tr->vr);
+			list_del(&tr->node);
+			kfree(tr);
+		}
+
+		kfree(r->name);
+		list_del(&r->node);
+		kfree(r);
+	}
+
+	mutex_unlock(&info->results_lock);
+}
+
+static struct dmatest_result *result_init(struct dmatest_info *info,
+		const char *name)
+{
+	struct dmatest_result *r;
+
+	r = kzalloc(sizeof(*r), GFP_KERNEL);
+	if (r) {
+		r->name = kstrdup(name, GFP_KERNEL);
+		INIT_LIST_HEAD(&r->results);
+		mutex_lock(&info->results_lock);
+		list_add_tail(&r->node, &info->results);
+		mutex_unlock(&info->results_lock);
+	}
+	return r;
+}
+
 /*
  * This function repeatedly tests DMA transfers of various lengths and
  * offsets for a given operation type until it is told to exit by
@@ -268,6 +516,8 @@ static int dmatest_func(void *data)
 	DECLARE_WAIT_QUEUE_HEAD_ONSTACK(done_wait);
 	struct dmatest_thread	*thread = data;
 	struct dmatest_done	done = { .wait = &done_wait };
+	struct dmatest_info	*info;
+	struct dmatest_params	*params;
 	struct dma_chan		*chan;
 	struct dma_device	*dev;
 	const char		*thread_name;
@@ -278,11 +528,12 @@ static int dmatest_func(void *data)
 	dma_cookie_t		cookie;
 	enum dma_status		status;
 	enum dma_ctrl_flags 	flags;
-	u8			pq_coefs[pq_sources + 1];
+	u8			*pq_coefs = NULL;
 	int			ret;
 	int			src_cnt;
 	int			dst_cnt;
 	int			i;
+	struct dmatest_result	*result;
 
 	thread_name = current->comm;
 	set_freezable();
@@ -290,28 +541,39 @@ static int dmatest_func(void *data)
 	ret = -ENOMEM;
 
 	smp_rmb();
+	info = thread->info;
+	params = &info->params;
 	chan = thread->chan;
 	dev = chan->device;
 	if (thread->type == DMA_MEMCPY)
 		src_cnt = dst_cnt = 1;
 	else if (thread->type == DMA_XOR) {
 		/* force odd to ensure dst = src */
-		src_cnt = min_odd(xor_sources | 1, dev->max_xor);
+		src_cnt = min_odd(params->xor_sources | 1, dev->max_xor);
 		dst_cnt = 1;
 	} else if (thread->type == DMA_PQ) {
 		/* force odd to ensure dst = src */
-		src_cnt = min_odd(pq_sources | 1, dma_maxpq(dev, 0));
+		src_cnt = min_odd(params->pq_sources | 1, dma_maxpq(dev, 0));
 		dst_cnt = 2;
+
+		pq_coefs = kmalloc(params->pq_sources+1, GFP_KERNEL);
+		if (!pq_coefs)
+			goto err_thread_type;
+
 		for (i = 0; i < src_cnt; i++)
 			pq_coefs[i] = 1;
 	} else
+		goto err_thread_type;
+
+	result = result_init(info, thread_name);
+	if (!result)
 		goto err_srcs;
 
 	thread->srcs = kcalloc(src_cnt+1, sizeof(u8 *), GFP_KERNEL);
 	if (!thread->srcs)
 		goto err_srcs;
 	for (i = 0; i < src_cnt; i++) {
-		thread->srcs[i] = kmalloc(test_buf_size, GFP_KERNEL);
+		thread->srcs[i] = kmalloc(params->buf_size, GFP_KERNEL);
 		if (!thread->srcs[i])
 			goto err_srcbuf;
 	}
@@ -321,7 +583,7 @@ static int dmatest_func(void *data)
 	if (!thread->dsts)
 		goto err_dsts;
 	for (i = 0; i < dst_cnt; i++) {
-		thread->dsts[i] = kmalloc(test_buf_size, GFP_KERNEL);
+		thread->dsts[i] = kmalloc(params->buf_size, GFP_KERNEL);
 		if (!thread->dsts[i])
 			goto err_dstbuf;
 	}
@@ -337,7 +599,7 @@ static int dmatest_func(void *data)
 	      | DMA_COMPL_SKIP_DEST_UNMAP | DMA_COMPL_SRC_UNMAP_SINGLE;
 
 	while (!kthread_should_stop()
-	       && !(iterations && total_tests >= iterations)) {
+	       && !(params->iterations && total_tests >= params->iterations)) {
 		struct dma_async_tx_descriptor *tx = NULL;
 		dma_addr_t dma_srcs[src_cnt];
 		dma_addr_t dma_dsts[dst_cnt];
@@ -353,24 +615,24 @@ static int dmatest_func(void *data)
 		else if (thread->type == DMA_PQ)
 			align = dev->pq_align;
 
-		if (1 << align > test_buf_size) {
+		if (1 << align > params->buf_size) {
 			pr_err("%u-byte buffer too small for %d-byte alignment\n",
-			       test_buf_size, 1 << align);
+			       params->buf_size, 1 << align);
 			break;
 		}
 
-		len = dmatest_random() % test_buf_size + 1;
+		len = dmatest_random() % params->buf_size + 1;
 		len = (len >> align) << align;
 		if (!len)
 			len = 1 << align;
-		src_off = dmatest_random() % (test_buf_size - len + 1);
-		dst_off = dmatest_random() % (test_buf_size - len + 1);
+		src_off = dmatest_random() % (params->buf_size - len + 1);
+		dst_off = dmatest_random() % (params->buf_size - len + 1);
 
 		src_off = (src_off >> align) << align;
 		dst_off = (dst_off >> align) << align;
 
-		dmatest_init_srcs(thread->srcs, src_off, len);
-		dmatest_init_dsts(thread->dsts, dst_off, len);
+		dmatest_init_srcs(thread->srcs, src_off, len, params->buf_size);
+		dmatest_init_dsts(thread->dsts, dst_off, len, params->buf_size);
 
 		for (i = 0; i < src_cnt; i++) {
 			u8 *buf = thread->srcs[i] + src_off;
@@ -380,10 +642,10 @@ static int dmatest_func(void *data)
 			ret = dma_mapping_error(dev->dev, dma_srcs[i]);
 			if (ret) {
 				unmap_src(dev->dev, dma_srcs, len, i);
-				pr_warn("%s: #%u: mapping error %d with "
-					"src_off=0x%x len=0x%x\n",
-					thread_name, total_tests - 1, ret,
-					src_off, len);
+				thread_result_add(info, result,
+						  DMATEST_ET_MAP_SRC,
+						  total_tests, src_off, dst_off,
+						  len, ret);
 				failed_tests++;
 				continue;
 			}
@@ -391,16 +653,17 @@ static int dmatest_func(void *data)
 		/* map with DMA_BIDIRECTIONAL to force writeback/invalidate */
 		for (i = 0; i < dst_cnt; i++) {
 			dma_dsts[i] = dma_map_single(dev->dev, thread->dsts[i],
-						     test_buf_size,
+						     params->buf_size,
 						     DMA_BIDIRECTIONAL);
 			ret = dma_mapping_error(dev->dev, dma_dsts[i]);
 			if (ret) {
 				unmap_src(dev->dev, dma_srcs, len, src_cnt);
-				unmap_dst(dev->dev, dma_dsts, test_buf_size, i);
-				pr_warn("%s: #%u: mapping error %d with "
-					"dst_off=0x%x len=0x%x\n",
-					thread_name, total_tests - 1, ret,
-					dst_off, test_buf_size);
+				unmap_dst(dev->dev, dma_dsts, params->buf_size,
+					  i);
+				thread_result_add(info, result,
+						  DMATEST_ET_MAP_DST,
+						  total_tests, src_off, dst_off,
+						  len, ret);
 				failed_tests++;
 				continue;
 			}
@@ -428,11 +691,11 @@ static int dmatest_func(void *data)
 
 		if (!tx) {
 			unmap_src(dev->dev, dma_srcs, len, src_cnt);
-			unmap_dst(dev->dev, dma_dsts, test_buf_size, dst_cnt);
-			pr_warning("%s: #%u: prep error with src_off=0x%x "
-					"dst_off=0x%x len=0x%x\n",
-					thread_name, total_tests - 1,
-					src_off, dst_off, len);
+			unmap_dst(dev->dev, dma_dsts, params->buf_size,
+				  dst_cnt);
+			thread_result_add(info, result, DMATEST_ET_PREP,
+					  total_tests, src_off, dst_off,
+					  len, 0);
 			msleep(100);
 			failed_tests++;
 			continue;
@@ -444,18 +707,18 @@ static int dmatest_func(void *data)
 		cookie = tx->tx_submit(tx);
 
 		if (dma_submit_error(cookie)) {
-			pr_warning("%s: #%u: submit error %d with src_off=0x%x "
-					"dst_off=0x%x len=0x%x\n",
-					thread_name, total_tests - 1, cookie,
-					src_off, dst_off, len);
+			thread_result_add(info, result, DMATEST_ET_SUBMIT,
+					  total_tests, src_off, dst_off,
+					  len, cookie);
 			msleep(100);
 			failed_tests++;
 			continue;
 		}
 		dma_async_issue_pending(chan);
 
-		wait_event_freezable_timeout(done_wait, done.done,
-					     msecs_to_jiffies(timeout));
+		wait_event_freezable_timeout(done_wait,
+					     done.done || kthread_should_stop(),
+					     msecs_to_jiffies(params->timeout));
 
 		status = dma_async_is_tx_complete(chan, cookie, NULL, NULL);
 
@@ -468,56 +731,57 @@ static int dmatest_func(void *data)
 			 * free it this time?" dancing.  For now, just
 			 * leave it dangling.
 			 */
-			pr_warning("%s: #%u: test timed out\n",
-				   thread_name, total_tests - 1);
+			thread_result_add(info, result, DMATEST_ET_TIMEOUT,
+					  total_tests, src_off, dst_off,
+					  len, 0);
 			failed_tests++;
 			continue;
 		} else if (status != DMA_SUCCESS) {
-			pr_warning("%s: #%u: got completion callback,"
-				   " but status is \'%s\'\n",
-				   thread_name, total_tests - 1,
-				   status == DMA_ERROR ? "error" : "in progress");
+			enum dmatest_error_type type = (status == DMA_ERROR) ?
+				DMATEST_ET_DMA_ERROR : DMATEST_ET_DMA_IN_PROGRESS;
+			thread_result_add(info, result, type,
+					  total_tests, src_off, dst_off,
+					  len, status);
 			failed_tests++;
 			continue;
 		}
 
 		/* Unmap by myself (see DMA_COMPL_SKIP_DEST_UNMAP above) */
-		unmap_dst(dev->dev, dma_dsts, test_buf_size, dst_cnt);
+		unmap_dst(dev->dev, dma_dsts, params->buf_size, dst_cnt);
 
 		error_count = 0;
 
 		pr_debug("%s: verifying source buffer...\n", thread_name);
-		error_count += dmatest_verify(thread->srcs, 0, src_off,
+		error_count += verify_result_add(info, result, total_tests,
+				src_off, dst_off, len, thread->srcs, -1,
 				0, PATTERN_SRC, true);
-		error_count += dmatest_verify(thread->srcs, src_off,
-				src_off + len, src_off,
-				PATTERN_SRC | PATTERN_COPY, true);
-		error_count += dmatest_verify(thread->srcs, src_off + len,
-				test_buf_size, src_off + len,
-				PATTERN_SRC, true);
-
-		pr_debug("%s: verifying dest buffer...\n",
-				thread->task->comm);
-		error_count += dmatest_verify(thread->dsts, 0, dst_off,
+		error_count += verify_result_add(info, result, total_tests,
+				src_off, dst_off, len, thread->srcs, 0,
+				src_off, PATTERN_SRC | PATTERN_COPY, true);
+		error_count += verify_result_add(info, result, total_tests,
+				src_off, dst_off, len, thread->srcs, 1,
+				src_off + len, PATTERN_SRC, true);
+
+		pr_debug("%s: verifying dest buffer...\n", thread_name);
+		error_count += verify_result_add(info, result, total_tests,
+				src_off, dst_off, len, thread->dsts, -1,
 				0, PATTERN_DST, false);
-		error_count += dmatest_verify(thread->dsts, dst_off,
-				dst_off + len, src_off,
-				PATTERN_SRC | PATTERN_COPY, false);
-		error_count += dmatest_verify(thread->dsts, dst_off + len,
-				test_buf_size, dst_off + len,
-				PATTERN_DST, false);
+		error_count += verify_result_add(info, result, total_tests,
+				src_off, dst_off, len, thread->dsts, 0,
+				src_off, PATTERN_SRC | PATTERN_COPY, false);
+		error_count += verify_result_add(info, result, total_tests,
+				src_off, dst_off, len, thread->dsts, 1,
+				dst_off + len, PATTERN_DST, false);
 
 		if (error_count) {
-			pr_warning("%s: #%u: %u errors with "
-				"src_off=0x%x dst_off=0x%x len=0x%x\n",
-				thread_name, total_tests - 1, error_count,
-				src_off, dst_off, len);
+			thread_result_add(info, result, DMATEST_ET_VERIFY,
+					  total_tests, src_off, dst_off,
+					  len, error_count);
 			failed_tests++;
 		} else {
-			pr_debug("%s: #%u: No errors with "
-				"src_off=0x%x dst_off=0x%x len=0x%x\n",
-				thread_name, total_tests - 1,
-				src_off, dst_off, len);
+			thread_result_add(info, result, DMATEST_ET_OK,
+					  total_tests, src_off, dst_off,
+					  len, 0);
 		}
 	}
 
@@ -532,6 +796,8 @@ err_dsts:
 err_srcbuf:
 	kfree(thread->srcs);
 err_srcs:
+	kfree(pq_coefs);
+err_thread_type:
 	pr_notice("%s: terminating after %u tests, %u failures (status %d)\n",
 			thread_name, total_tests, failed_tests, ret);
 
@@ -539,7 +805,9 @@ err_srcs:
 	if (ret)
 		dmaengine_terminate_all(chan);
 
-	if (iterations > 0)
+	thread->done = true;
+
+	if (params->iterations > 0)
 		while (!kthread_should_stop()) {
 			DECLARE_WAIT_QUEUE_HEAD_ONSTACK(wait_dmatest_exit);
 			interruptible_sleep_on(&wait_dmatest_exit);
@@ -568,8 +836,10 @@ static void dmatest_cleanup_channel(struct dmatest_chan *dtc)
 	kfree(dtc);
 }
 
-static int dmatest_add_threads(struct dmatest_chan *dtc, enum dma_transaction_type type)
+static int dmatest_add_threads(struct dmatest_info *info,
+		struct dmatest_chan *dtc, enum dma_transaction_type type)
 {
+	struct dmatest_params *params = &info->params;
 	struct dmatest_thread *thread;
 	struct dma_chan *chan = dtc->chan;
 	char *op;
@@ -584,7 +854,7 @@ static int dmatest_add_threads(struct dmatest_chan *dtc, enum dma_transaction_ty
 	else
 		return -EINVAL;
 
-	for (i = 0; i < threads_per_chan; i++) {
+	for (i = 0; i < params->threads_per_chan; i++) {
 		thread = kzalloc(sizeof(struct dmatest_thread), GFP_KERNEL);
 		if (!thread) {
 			pr_warning("dmatest: No memory for %s-%s%u\n",
@@ -592,6 +862,7 @@ static int dmatest_add_threads(struct dmatest_chan *dtc, enum dma_transaction_ty
 
 			break;
 		}
+		thread->info = info;
 		thread->chan = dtc->chan;
 		thread->type = type;
 		smp_wmb();
@@ -612,7 +883,8 @@ static int dmatest_add_threads(struct dmatest_chan *dtc, enum dma_transaction_ty
 	return i;
 }
 
-static int dmatest_add_channel(struct dma_chan *chan)
+static int dmatest_add_channel(struct dmatest_info *info,
+		struct dma_chan *chan)
 {
 	struct dmatest_chan	*dtc;
 	struct dma_device	*dma_dev = chan->device;
@@ -629,75 +901,418 @@ static int dmatest_add_channel(struct dma_chan *chan)
 	INIT_LIST_HEAD(&dtc->threads);
 
 	if (dma_has_cap(DMA_MEMCPY, dma_dev->cap_mask)) {
-		cnt = dmatest_add_threads(dtc, DMA_MEMCPY);
+		cnt = dmatest_add_threads(info, dtc, DMA_MEMCPY);
 		thread_count += cnt > 0 ? cnt : 0;
 	}
 	if (dma_has_cap(DMA_XOR, dma_dev->cap_mask)) {
-		cnt = dmatest_add_threads(dtc, DMA_XOR);
+		cnt = dmatest_add_threads(info, dtc, DMA_XOR);
 		thread_count += cnt > 0 ? cnt : 0;
 	}
 	if (dma_has_cap(DMA_PQ, dma_dev->cap_mask)) {
-		cnt = dmatest_add_threads(dtc, DMA_PQ);
+		cnt = dmatest_add_threads(info, dtc, DMA_PQ);
 		thread_count += cnt > 0 ? cnt : 0;
 	}
 
 	pr_info("dmatest: Started %u threads using %s\n",
 		thread_count, dma_chan_name(chan));
 
-	list_add_tail(&dtc->node, &dmatest_channels);
-	nr_channels++;
+	list_add_tail(&dtc->node, &info->channels);
+	info->nr_channels++;
 
 	return 0;
 }
 
 static bool filter(struct dma_chan *chan, void *param)
 {
-	if (!dmatest_match_channel(chan) || !dmatest_match_device(chan->device))
+	struct dmatest_params *params = param;
+
+	if (!dmatest_match_channel(params, chan) ||
+	    !dmatest_match_device(params, chan->device))
 		return false;
 	else
 		return true;
 }
 
-static int __init dmatest_init(void)
+static int __run_threaded_test(struct dmatest_info *info)
 {
 	dma_cap_mask_t mask;
 	struct dma_chan *chan;
+	struct dmatest_params *params = &info->params;
 	int err = 0;
 
 	dma_cap_zero(mask);
 	dma_cap_set(DMA_MEMCPY, mask);
 	for (;;) {
-		chan = dma_request_channel(mask, filter, NULL);
+		chan = dma_request_channel(mask, filter, params);
 		if (chan) {
-			err = dmatest_add_channel(chan);
+			err = dmatest_add_channel(info, chan);
 			if (err) {
 				dma_release_channel(chan);
 				break; /* add_channel failed, punt */
 			}
 		} else
 			break; /* no more channels available */
-		if (max_channels && nr_channels >= max_channels)
+		if (params->max_channels &&
+		    info->nr_channels >= params->max_channels)
 			break; /* we have all we need */
 	}
-
 	return err;
 }
-/* when compiled-in wait for drivers to load first */
-late_initcall(dmatest_init);
 
-static void __exit dmatest_exit(void)
+#ifndef MODULE
+static int run_threaded_test(struct dmatest_info *info)
+{
+	int ret;
+
+	mutex_lock(&info->lock);
+	ret = __run_threaded_test(info);
+	mutex_unlock(&info->lock);
+	return ret;
+}
+#endif
+
+static void __stop_threaded_test(struct dmatest_info *info)
 {
 	struct dmatest_chan *dtc, *_dtc;
 	struct dma_chan *chan;
 
-	list_for_each_entry_safe(dtc, _dtc, &dmatest_channels, node) {
+	list_for_each_entry_safe(dtc, _dtc, &info->channels, node) {
 		list_del(&dtc->node);
 		chan = dtc->chan;
 		dmatest_cleanup_channel(dtc);
-		pr_debug("dmatest: dropped channel %s\n",
-			 dma_chan_name(chan));
+		pr_debug("dmatest: dropped channel %s\n", dma_chan_name(chan));
 		dma_release_channel(chan);
 	}
+
+	info->nr_channels = 0;
+}
+
+static void stop_threaded_test(struct dmatest_info *info)
+{
+	mutex_lock(&info->lock);
+	__stop_threaded_test(info);
+	mutex_unlock(&info->lock);
+}
+
+static int __restart_threaded_test(struct dmatest_info *info, bool run)
+{
+	struct dmatest_params *params = &info->params;
+	int ret;
+
+	/* Stop any running test first */
+	__stop_threaded_test(info);
+
+	if (run == false)
+		return 0;
+
+	/* Clear results from previous run */
+	result_free(info, NULL);
+
+	/* Copy test parameters */
+	memcpy(params, &info->dbgfs_params, sizeof(*params));
+
+	/* Run test with new parameters */
+	ret = __run_threaded_test(info);
+	if (ret) {
+		__stop_threaded_test(info);
+		pr_err("dmatest: Can't run test\n");
+	}
+
+	return ret;
+}
+
+static ssize_t dtf_write_string(void *to, size_t available, loff_t *ppos,
+		const void __user *from, size_t count)
+{
+	char tmp[20];
+	ssize_t len;
+
+	len = simple_write_to_buffer(tmp, sizeof(tmp) - 1, ppos, from, count);
+	if (len >= 0) {
+		tmp[len] = '\0';
+		strlcpy(to, strim(tmp), available);
+	}
+
+	return len;
+}
+
+static ssize_t dtf_read_channel(struct file *file, char __user *buf,
+		size_t count, loff_t *ppos)
+{
+	struct dmatest_info *info = file->private_data;
+	return simple_read_from_buffer(buf, count, ppos,
+			info->dbgfs_params.channel,
+			strlen(info->dbgfs_params.channel));
+}
+
+static ssize_t dtf_write_channel(struct file *file, const char __user *buf,
+		size_t size, loff_t *ppos)
+{
+	struct dmatest_info *info = file->private_data;
+	return dtf_write_string(info->dbgfs_params.channel,
+				sizeof(info->dbgfs_params.channel),
+				ppos, buf, size);
+}
+
+static const struct file_operations dtf_channel_fops = {
+	.read	= dtf_read_channel,
+	.write	= dtf_write_channel,
+	.open	= simple_open,
+	.llseek	= default_llseek,
+};
+
+static ssize_t dtf_read_device(struct file *file, char __user *buf,
+		size_t count, loff_t *ppos)
+{
+	struct dmatest_info *info = file->private_data;
+	return simple_read_from_buffer(buf, count, ppos,
+			info->dbgfs_params.device,
+			strlen(info->dbgfs_params.device));
+}
+
+static ssize_t dtf_write_device(struct file *file, const char __user *buf,
+		size_t size, loff_t *ppos)
+{
+	struct dmatest_info *info = file->private_data;
+	return dtf_write_string(info->dbgfs_params.device,
+				sizeof(info->dbgfs_params.device),
+				ppos, buf, size);
+}
+
+static const struct file_operations dtf_device_fops = {
+	.read	= dtf_read_device,
+	.write	= dtf_write_device,
+	.open	= simple_open,
+	.llseek	= default_llseek,
+};
+
+static ssize_t dtf_read_run(struct file *file, char __user *user_buf,
+		size_t count, loff_t *ppos)
+{
+	struct dmatest_info *info = file->private_data;
+	char buf[3];
+	struct dmatest_chan *dtc;
+	bool alive = false;
+
+	mutex_lock(&info->lock);
+	list_for_each_entry(dtc, &info->channels, node) {
+		struct dmatest_thread *thread;
+
+		list_for_each_entry(thread, &dtc->threads, node) {
+			if (!thread->done) {
+				alive = true;
+				break;
+			}
+		}
+	}
+
+	if (alive) {
+		buf[0] = 'Y';
+	} else {
+		__stop_threaded_test(info);
+		buf[0] = 'N';
+	}
+
+	mutex_unlock(&info->lock);
+	buf[1] = '\n';
+	buf[2] = 0x00;
+	return simple_read_from_buffer(user_buf, count, ppos, buf, 2);
+}
+
+static ssize_t dtf_write_run(struct file *file, const char __user *user_buf,
+		size_t count, loff_t *ppos)
+{
+	struct dmatest_info *info = file->private_data;
+	char buf[16];
+	bool bv;
+	int ret = 0;
+
+	if (copy_from_user(buf, user_buf, min(count, (sizeof(buf) - 1))))
+		return -EFAULT;
+
+	if (strtobool(buf, &bv) == 0) {
+		mutex_lock(&info->lock);
+		ret = __restart_threaded_test(info, bv);
+		mutex_unlock(&info->lock);
+	}
+
+	return ret ? ret : count;
+}
+
+static const struct file_operations dtf_run_fops = {
+	.read	= dtf_read_run,
+	.write	= dtf_write_run,
+	.open	= simple_open,
+	.llseek	= default_llseek,
+};
+
+static int dtf_results_show(struct seq_file *sf, void *data)
+{
+	struct dmatest_info *info = sf->private;
+	struct dmatest_result *result;
+	struct dmatest_thread_result *tr;
+	unsigned int i;
+
+	mutex_lock(&info->results_lock);
+	list_for_each_entry(result, &info->results, node) {
+		list_for_each_entry(tr, &result->results, node) {
+			seq_printf(sf, "%s\n",
+				thread_result_get(result->name, tr));
+			if (tr->type == DMATEST_ET_VERIFY_BUF) {
+				for (i = 0; i < tr->vr->error_count; i++) {
+					seq_printf(sf, "\t%s\n",
+						verify_result_get_one(tr->vr, i));
+				}
+			}
+		}
+	}
+
+	mutex_unlock(&info->results_lock);
+	return 0;
+}
+
+static int dtf_results_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dtf_results_show, inode->i_private);
+}
+
+static const struct file_operations dtf_results_fops = {
+	.open		= dtf_results_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int dmatest_register_dbgfs(struct dmatest_info *info)
+{
+	struct dentry *d;
+	struct dmatest_params *params = &info->dbgfs_params;
+	int ret = -ENOMEM;
+
+	d = debugfs_create_dir("dmatest", NULL);
+	if (IS_ERR(d))
+		return PTR_ERR(d);
+	if (!d)
+		goto err_root;
+
+	info->root = d;
+
+	/* Copy initial values */
+	memcpy(params, &info->params, sizeof(*params));
+
+	/* Test parameters */
+
+	d = debugfs_create_u32("test_buf_size", S_IWUSR | S_IRUGO, info->root,
+			       (u32 *)&params->buf_size);
+	if (IS_ERR_OR_NULL(d))
+		goto err_node;
+
+	d = debugfs_create_file("channel", S_IRUGO | S_IWUSR, info->root,
+				info, &dtf_channel_fops);
+	if (IS_ERR_OR_NULL(d))
+		goto err_node;
+
+	d = debugfs_create_file("device", S_IRUGO | S_IWUSR, info->root,
+				info, &dtf_device_fops);
+	if (IS_ERR_OR_NULL(d))
+		goto err_node;
+
+	d = debugfs_create_u32("threads_per_chan", S_IWUSR | S_IRUGO, info->root,
+			       (u32 *)&params->threads_per_chan);
+	if (IS_ERR_OR_NULL(d))
+		goto err_node;
+
+	d = debugfs_create_u32("max_channels", S_IWUSR | S_IRUGO, info->root,
+			       (u32 *)&params->max_channels);
+	if (IS_ERR_OR_NULL(d))
+		goto err_node;
+
+	d = debugfs_create_u32("iterations", S_IWUSR | S_IRUGO, info->root,
+			       (u32 *)&params->iterations);
+	if (IS_ERR_OR_NULL(d))
+		goto err_node;
+
+	d = debugfs_create_u32("xor_sources", S_IWUSR | S_IRUGO, info->root,
+			       (u32 *)&params->xor_sources);
+	if (IS_ERR_OR_NULL(d))
+		goto err_node;
+
+	d = debugfs_create_u32("pq_sources", S_IWUSR | S_IRUGO, info->root,
+			       (u32 *)&params->pq_sources);
+	if (IS_ERR_OR_NULL(d))
+		goto err_node;
+
+	d = debugfs_create_u32("timeout", S_IWUSR | S_IRUGO, info->root,
+			       (u32 *)&params->timeout);
+	if (IS_ERR_OR_NULL(d))
+		goto err_node;
+
+	/* Run or stop threaded test */
+	d = debugfs_create_file("run", S_IWUSR | S_IRUGO, info->root,
+				info, &dtf_run_fops);
+	if (IS_ERR_OR_NULL(d))
+		goto err_node;
+
+	/* Results of test in progress */
+	d = debugfs_create_file("results", S_IRUGO, info->root, info,
+				&dtf_results_fops);
+	if (IS_ERR_OR_NULL(d))
+		goto err_node;
+
+	return 0;
+
+err_node:
+	debugfs_remove_recursive(info->root);
+err_root:
+	pr_err("dmatest: Failed to initialize debugfs\n");
+	return ret;
+}
+
+static int __init dmatest_init(void)
+{
+	struct dmatest_info *info = &test_info;
+	struct dmatest_params *params = &info->params;
+	int ret;
+
+	memset(info, 0, sizeof(*info));
+
+	mutex_init(&info->lock);
+	INIT_LIST_HEAD(&info->channels);
+
+	mutex_init(&info->results_lock);
+	INIT_LIST_HEAD(&info->results);
+
+	/* Set default parameters */
+	params->buf_size = test_buf_size;
+	strlcpy(params->channel, test_channel, sizeof(params->channel));
+	strlcpy(params->device, test_device, sizeof(params->device));
+	params->threads_per_chan = threads_per_chan;
+	params->max_channels = max_channels;
+	params->iterations = iterations;
+	params->xor_sources = xor_sources;
+	params->pq_sources = pq_sources;
+	params->timeout = timeout;
+
+	ret = dmatest_register_dbgfs(info);
+	if (ret)
+		return ret;
+
+#ifdef MODULE
+	return 0;
+#else
+	return run_threaded_test(info);
+#endif
+}
+/* when compiled-in wait for drivers to load first */
+late_initcall(dmatest_init);
+
+static void __exit dmatest_exit(void)
+{
+	struct dmatest_info *info = &test_info;
+
+	debugfs_remove_recursive(info->root);
+	stop_threaded_test(info);
+	result_free(info, NULL);
 }
 module_exit(dmatest_exit);
 
diff --git a/drivers/dma/dw_dmac.c b/drivers/dma/dw_dmac.c
index 43a5329d4483..2e5deaa82b60 100644
--- a/drivers/dma/dw_dmac.c
+++ b/drivers/dma/dw_dmac.c
@@ -25,6 +25,8 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
+#include <linux/acpi.h>
+#include <linux/acpi_dma.h>
 
 #include "dw_dmac_regs.h"
 #include "dmaengine.h"
@@ -49,29 +51,22 @@ static inline unsigned int dwc_get_sms(struct dw_dma_slave *slave)
 	return slave ? slave->src_master : 1;
 }
 
-#define SRC_MASTER	0
-#define DST_MASTER	1
-
-static inline unsigned int dwc_get_master(struct dma_chan *chan, int master)
+static inline void dwc_set_masters(struct dw_dma_chan *dwc)
 {
-	struct dw_dma *dw = to_dw_dma(chan->device);
-	struct dw_dma_slave *dws = chan->private;
-	unsigned int m;
-
-	if (master == SRC_MASTER)
-		m = dwc_get_sms(dws);
-	else
-		m = dwc_get_dms(dws);
+	struct dw_dma *dw = to_dw_dma(dwc->chan.device);
+	struct dw_dma_slave *dws = dwc->chan.private;
+	unsigned char mmax = dw->nr_masters - 1;
 
-	return min_t(unsigned int, dw->nr_masters - 1, m);
+	if (dwc->request_line == ~0) {
+		dwc->src_master = min_t(unsigned char, mmax, dwc_get_sms(dws));
+		dwc->dst_master = min_t(unsigned char, mmax, dwc_get_dms(dws));
+	}
 }
 
 #define DWC_DEFAULT_CTLLO(_chan) ({				\
 		struct dw_dma_chan *_dwc = to_dw_dma_chan(_chan);	\
 		struct dma_slave_config	*_sconfig = &_dwc->dma_sconfig;	\
 		bool _is_slave = is_slave_direction(_dwc->direction);	\
-		int _dms = dwc_get_master(_chan, DST_MASTER);		\
-		int _sms = dwc_get_master(_chan, SRC_MASTER);		\
 		u8 _smsize = _is_slave ? _sconfig->src_maxburst :	\
 			DW_DMA_MSIZE_16;			\
 		u8 _dmsize = _is_slave ? _sconfig->dst_maxburst :	\
@@ -81,8 +76,8 @@ static inline unsigned int dwc_get_master(struct dma_chan *chan, int master)
 		 | DWC_CTLL_SRC_MSIZE(_smsize)			\
 		 | DWC_CTLL_LLP_D_EN				\
 		 | DWC_CTLL_LLP_S_EN				\
-		 | DWC_CTLL_DMS(_dms)				\
-		 | DWC_CTLL_SMS(_sms));				\
+		 | DWC_CTLL_DMS(_dwc->dst_master)		\
+		 | DWC_CTLL_SMS(_dwc->src_master));		\
 	})
 
 /*
@@ -92,13 +87,6 @@ static inline unsigned int dwc_get_master(struct dma_chan *chan, int master)
  */
 #define NR_DESCS_PER_CHANNEL	64
 
-static inline unsigned int dwc_get_data_width(struct dma_chan *chan, int master)
-{
-	struct dw_dma *dw = to_dw_dma(chan->device);
-
-	return dw->data_width[dwc_get_master(chan, master)];
-}
-
 /*----------------------------------------------------------------------*/
 
 static struct device *chan2dev(struct dma_chan *chan)
@@ -172,13 +160,7 @@ static void dwc_initialize(struct dw_dma_chan *dwc)
 	if (dwc->initialized == true)
 		return;
 
-	if (dws && dws->cfg_hi == ~0 && dws->cfg_lo == ~0) {
-		/* autoconfigure based on request line from DT */
-		if (dwc->direction == DMA_MEM_TO_DEV)
-			cfghi = DWC_CFGH_DST_PER(dwc->request_line);
-		else if (dwc->direction == DMA_DEV_TO_MEM)
-			cfghi = DWC_CFGH_SRC_PER(dwc->request_line);
-	} else if (dws) {
+	if (dws) {
 		/*
 		 * We need controller-specific data to set up slave
 		 * transfers.
@@ -189,9 +171,9 @@ static void dwc_initialize(struct dw_dma_chan *dwc)
 		cfglo |= dws->cfg_lo & ~DWC_CFGL_CH_PRIOR_MASK;
 	} else {
 		if (dwc->direction == DMA_MEM_TO_DEV)
-			cfghi = DWC_CFGH_DST_PER(dwc->dma_sconfig.slave_id);
+			cfghi = DWC_CFGH_DST_PER(dwc->request_line);
 		else if (dwc->direction == DMA_DEV_TO_MEM)
-			cfghi = DWC_CFGH_SRC_PER(dwc->dma_sconfig.slave_id);
+			cfghi = DWC_CFGH_SRC_PER(dwc->request_line);
 	}
 
 	channel_writel(dwc, CFG_LO, cfglo);
@@ -473,16 +455,16 @@ static void dwc_scan_descriptors(struct dw_dma *dw, struct dw_dma_chan *dwc)
 			(unsigned long long)llp);
 
 	list_for_each_entry_safe(desc, _desc, &dwc->active_list, desc_node) {
-		/* initial residue value */
+		/* Initial residue value */
 		dwc->residue = desc->total_len;
 
-		/* check first descriptors addr */
+		/* Check first descriptors addr */
 		if (desc->txd.phys == llp) {
 			spin_unlock_irqrestore(&dwc->lock, flags);
 			return;
 		}
 
-		/* check first descriptors llp */
+		/* Check first descriptors llp */
 		if (desc->lli.llp == llp) {
 			/* This one is currently in progress */
 			dwc->residue -= dwc_get_sent(dwc);
@@ -588,7 +570,7 @@ inline dma_addr_t dw_dma_get_dst_addr(struct dma_chan *chan)
 }
 EXPORT_SYMBOL(dw_dma_get_dst_addr);
 
-/* called with dwc->lock held and all DMAC interrupts disabled */
+/* Called with dwc->lock held and all DMAC interrupts disabled */
 static void dwc_handle_cyclic(struct dw_dma *dw, struct dw_dma_chan *dwc,
 		u32 status_err, u32 status_xfer)
 {
@@ -626,7 +608,7 @@ static void dwc_handle_cyclic(struct dw_dma *dw, struct dw_dma_chan *dwc,
 
 		dwc_chan_disable(dw, dwc);
 
-		/* make sure DMA does not restart by loading a new list */
+		/* Make sure DMA does not restart by loading a new list */
 		channel_writel(dwc, LLP, 0);
 		channel_writel(dwc, CTL_LO, 0);
 		channel_writel(dwc, CTL_HI, 0);
@@ -745,6 +727,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		*dw = to_dw_dma(chan->device);
 	struct dw_desc		*desc;
 	struct dw_desc		*first;
 	struct dw_desc		*prev;
@@ -767,8 +750,8 @@ dwc_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 
 	dwc->direction = DMA_MEM_TO_MEM;
 
-	data_width = min_t(unsigned int, dwc_get_data_width(chan, SRC_MASTER),
-			   dwc_get_data_width(chan, DST_MASTER));
+	data_width = min_t(unsigned int, dw->data_width[dwc->src_master],
+			   dw->data_width[dwc->dst_master]);
 
 	src_width = dst_width = min_t(unsigned int, data_width,
 				      dwc_fast_fls(src | dest | len));
@@ -826,6 +809,7 @@ dwc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 		unsigned long flags, void *context)
 {
 	struct dw_dma_chan	*dwc = to_dw_dma_chan(chan);
+	struct dw_dma		*dw = to_dw_dma(chan->device);
 	struct dma_slave_config	*sconfig = &dwc->dma_sconfig;
 	struct dw_desc		*prev;
 	struct dw_desc		*first;
@@ -859,7 +843,7 @@ 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_get_data_width(chan, SRC_MASTER);
+		data_width = dw->data_width[dwc->src_master];
 
 		for_each_sg(sgl, sg, sg_len, i) {
 			struct dw_desc	*desc;
@@ -919,7 +903,7 @@ 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_get_data_width(chan, DST_MASTER);
+		data_width = dw->data_width[dwc->dst_master];
 
 		for_each_sg(sgl, sg, sg_len, i) {
 			struct dw_desc	*desc;
@@ -1001,13 +985,6 @@ static inline void convert_burst(u32 *maxburst)
 		*maxburst = 0;
 }
 
-static inline void convert_slave_id(struct dw_dma_chan *dwc)
-{
-	struct dw_dma *dw = to_dw_dma(dwc->chan.device);
-
-	dwc->dma_sconfig.slave_id -= dw->request_line_base;
-}
-
 static int
 set_runtime_config(struct dma_chan *chan, struct dma_slave_config *sconfig)
 {
@@ -1020,9 +997,12 @@ set_runtime_config(struct dma_chan *chan, struct dma_slave_config *sconfig)
 	memcpy(&dwc->dma_sconfig, sconfig, sizeof(*sconfig));
 	dwc->direction = sconfig->direction;
 
+	/* Take the request line from slave_id member */
+	if (dwc->request_line == ~0)
+		dwc->request_line = sconfig->slave_id;
+
 	convert_burst(&dwc->dma_sconfig.src_maxburst);
 	convert_burst(&dwc->dma_sconfig.dst_maxburst);
-	convert_slave_id(dwc);
 
 	return 0;
 }
@@ -1030,10 +1010,11 @@ set_runtime_config(struct dma_chan *chan, struct dma_slave_config *sconfig)
 static inline void dwc_chan_pause(struct dw_dma_chan *dwc)
 {
 	u32 cfglo = channel_readl(dwc, CFG_LO);
+	unsigned int count = 20;	/* timeout iterations */
 
 	channel_writel(dwc, CFG_LO, cfglo | DWC_CFGL_CH_SUSP);
-	while (!(channel_readl(dwc, CFG_LO) & DWC_CFGL_FIFO_EMPTY))
-		cpu_relax();
+	while (!(channel_readl(dwc, CFG_LO) & DWC_CFGL_FIFO_EMPTY) && count--)
+		udelay(2);
 
 	dwc->paused = true;
 }
@@ -1169,6 +1150,8 @@ static int dwc_alloc_chan_resources(struct dma_chan *chan)
 	 * doesn't mean what you think it means), and status writeback.
 	 */
 
+	dwc_set_masters(dwc);
+
 	spin_lock_irqsave(&dwc->lock, flags);
 	i = dwc->descs_allocated;
 	while (dwc->descs_allocated < NR_DESCS_PER_CHANNEL) {
@@ -1226,6 +1209,7 @@ static void dwc_free_chan_resources(struct dma_chan *chan)
 	list_splice_init(&dwc->free_list, &list);
 	dwc->descs_allocated = 0;
 	dwc->initialized = false;
+	dwc->request_line = ~0;
 
 	/* Disable interrupts */
 	channel_clear_bit(dw, MASK.XFER, dwc->mask);
@@ -1241,42 +1225,36 @@ static void dwc_free_chan_resources(struct dma_chan *chan)
 	dev_vdbg(chan2dev(chan), "%s: done\n", __func__);
 }
 
-struct dw_dma_filter_args {
+/*----------------------------------------------------------------------*/
+
+struct dw_dma_of_filter_args {
 	struct dw_dma *dw;
 	unsigned int req;
 	unsigned int src;
 	unsigned int dst;
 };
 
-static bool dw_dma_generic_filter(struct dma_chan *chan, void *param)
+static bool dw_dma_of_filter(struct dma_chan *chan, void *param)
 {
 	struct dw_dma_chan *dwc = to_dw_dma_chan(chan);
-	struct dw_dma *dw = to_dw_dma(chan->device);
-	struct dw_dma_filter_args *fargs = param;
-	struct dw_dma_slave *dws = &dwc->slave;
+	struct dw_dma_of_filter_args *fargs = param;
 
-	/* ensure the device matches our channel */
+	/* Ensure the device matches our channel */
         if (chan->device != &fargs->dw->dma)
                 return false;
 
-	dws->dma_dev	= dw->dma.dev;
-	dws->cfg_hi	= ~0;
-	dws->cfg_lo	= ~0;
-	dws->src_master	= fargs->src;
-	dws->dst_master	= fargs->dst;
-
 	dwc->request_line = fargs->req;
-
-	chan->private = dws;
+	dwc->src_master	= fargs->src;
+	dwc->dst_master	= fargs->dst;
 
 	return true;
 }
 
-static struct dma_chan *dw_dma_xlate(struct of_phandle_args *dma_spec,
-					 struct of_dma *ofdma)
+static struct dma_chan *dw_dma_of_xlate(struct of_phandle_args *dma_spec,
+					struct of_dma *ofdma)
 {
 	struct dw_dma *dw = ofdma->of_dma_data;
-	struct dw_dma_filter_args fargs = {
+	struct dw_dma_of_filter_args fargs = {
 		.dw = dw,
 	};
 	dma_cap_mask_t cap;
@@ -1297,8 +1275,48 @@ static struct dma_chan *dw_dma_xlate(struct of_phandle_args *dma_spec,
 	dma_cap_set(DMA_SLAVE, cap);
 
 	/* TODO: there should be a simpler way to do this */
-	return dma_request_channel(cap, dw_dma_generic_filter, &fargs);
+	return dma_request_channel(cap, dw_dma_of_filter, &fargs);
+}
+
+#ifdef CONFIG_ACPI
+static bool dw_dma_acpi_filter(struct dma_chan *chan, void *param)
+{
+	struct dw_dma_chan *dwc = to_dw_dma_chan(chan);
+	struct acpi_dma_spec *dma_spec = param;
+
+	if (chan->device->dev != dma_spec->dev ||
+	    chan->chan_id != dma_spec->chan_id)
+		return false;
+
+	dwc->request_line = dma_spec->slave_id;
+	dwc->src_master = dwc_get_sms(NULL);
+	dwc->dst_master = dwc_get_dms(NULL);
+
+	return true;
+}
+
+static void dw_dma_acpi_controller_register(struct dw_dma *dw)
+{
+	struct device *dev = dw->dma.dev;
+	struct acpi_dma_filter_info *info;
+	int ret;
+
+	info = devm_kzalloc(dev, sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return;
+
+	dma_cap_zero(info->dma_cap);
+	dma_cap_set(DMA_SLAVE, info->dma_cap);
+	info->filter_fn = dw_dma_acpi_filter;
+
+	ret = devm_acpi_dma_controller_register(dev, acpi_dma_simple_xlate,
+						info);
+	if (ret)
+		dev_err(dev, "could not register acpi_dma_controller\n");
 }
+#else /* !CONFIG_ACPI */
+static inline void dw_dma_acpi_controller_register(struct dw_dma *dw) {}
+#endif /* !CONFIG_ACPI */
 
 /* --------------------- Cyclic DMA API extensions -------------------- */
 
@@ -1322,7 +1340,7 @@ int dw_dma_cyclic_start(struct dma_chan *chan)
 
 	spin_lock_irqsave(&dwc->lock, flags);
 
-	/* assert channel is idle */
+	/* Assert channel is idle */
 	if (dma_readl(dw, CH_EN) & dwc->mask) {
 		dev_err(chan2dev(&dwc->chan),
 			"BUG: Attempted to start non-idle channel\n");
@@ -1334,7 +1352,7 @@ int dw_dma_cyclic_start(struct dma_chan *chan)
 	dma_writel(dw, CLEAR.ERROR, dwc->mask);
 	dma_writel(dw, CLEAR.XFER, dwc->mask);
 
-	/* setup DMAC channel registers */
+	/* Setup DMAC channel registers */
 	channel_writel(dwc, LLP, dwc->cdesc->desc[0]->txd.phys);
 	channel_writel(dwc, CTL_LO, DWC_CTLL_LLP_D_EN | DWC_CTLL_LLP_S_EN);
 	channel_writel(dwc, CTL_HI, 0);
@@ -1501,7 +1519,7 @@ struct dw_cyclic_desc *dw_dma_cyclic_prep(struct dma_chan *chan,
 		last = desc;
 	}
 
-	/* lets make a cyclic list */
+	/* Let's make a cyclic list */
 	last->lli.llp = cdesc->desc[0]->txd.phys;
 
 	dev_dbg(chan2dev(&dwc->chan), "cyclic prepared buf 0x%llx len %zu "
@@ -1636,7 +1654,6 @@ dw_dma_parse_dt(struct platform_device *pdev)
 
 static int dw_probe(struct platform_device *pdev)
 {
-	const struct platform_device_id *match;
 	struct dw_dma_platform_data *pdata;
 	struct resource		*io;
 	struct dw_dma		*dw;
@@ -1706,7 +1723,7 @@ static int dw_probe(struct platform_device *pdev)
 
 	dw->regs = regs;
 
-	/* get hardware configuration parameters */
+	/* Get hardware configuration parameters */
 	if (autocfg) {
 		max_blk_size = dma_readl(dw, MAX_BLK_SIZE);
 
@@ -1720,18 +1737,13 @@ static int dw_probe(struct platform_device *pdev)
 		memcpy(dw->data_width, pdata->data_width, 4);
 	}
 
-	/* Get the base request line if set */
-	match = platform_get_device_id(pdev);
-	if (match)
-		dw->request_line_base = (unsigned int)match->driver_data;
-
 	/* Calculate all channel mask before DMA setup */
 	dw->all_chan_mask = (1 << nr_channels) - 1;
 
-	/* force dma off, just in case */
+	/* Force dma off, just in case */
 	dw_dma_off(dw);
 
-	/* disable BLOCK interrupts as well */
+	/* Disable BLOCK interrupts as well */
 	channel_clear_bit(dw, MASK.BLOCK, dw->all_chan_mask);
 
 	err = devm_request_irq(&pdev->dev, irq, dw_dma_interrupt, 0,
@@ -1741,7 +1753,7 @@ static int dw_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, dw);
 
-	/* create a pool of consistent memory blocks for hardware descriptors */
+	/* Create a pool of consistent memory blocks for hardware descriptors */
 	dw->desc_pool = dmam_pool_create("dw_dmac_desc_pool", &pdev->dev,
 					 sizeof(struct dw_desc), 4, 0);
 	if (!dw->desc_pool) {
@@ -1781,8 +1793,9 @@ static int dw_probe(struct platform_device *pdev)
 		channel_clear_bit(dw, CH_EN, dwc->mask);
 
 		dwc->direction = DMA_TRANS_NONE;
+		dwc->request_line = ~0;
 
-		/* hardware configuration */
+		/* Hardware configuration */
 		if (autocfg) {
 			unsigned int dwc_params;
 
@@ -1842,12 +1855,15 @@ static int dw_probe(struct platform_device *pdev)
 
 	if (pdev->dev.of_node) {
 		err = of_dma_controller_register(pdev->dev.of_node,
-						 dw_dma_xlate, dw);
-		if (err && err != -ENODEV)
+						 dw_dma_of_xlate, dw);
+		if (err)
 			dev_err(&pdev->dev,
 				"could not register of_dma_controller\n");
 	}
 
+	if (ACPI_HANDLE(&pdev->dev))
+		dw_dma_acpi_controller_register(dw);
+
 	return 0;
 }
 
@@ -1912,18 +1928,19 @@ static const struct dev_pm_ops dw_dev_pm_ops = {
 };
 
 #ifdef CONFIG_OF
-static const struct of_device_id dw_dma_id_table[] = {
+static const struct of_device_id dw_dma_of_id_table[] = {
 	{ .compatible = "snps,dma-spear1340" },
 	{}
 };
-MODULE_DEVICE_TABLE(of, dw_dma_id_table);
+MODULE_DEVICE_TABLE(of, dw_dma_of_id_table);
 #endif
 
-static const struct platform_device_id dw_dma_ids[] = {
-	/* Name,	Request Line Base */
-	{ "INTL9C60",	(kernel_ulong_t)16 },
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id dw_dma_acpi_id_table[] = {
+	{ "INTL9C60", 0 },
 	{ }
 };
+#endif
 
 static struct platform_driver dw_driver = {
 	.probe		= dw_probe,
@@ -1932,9 +1949,9 @@ static struct platform_driver dw_driver = {
 	.driver = {
 		.name	= "dw_dmac",
 		.pm	= &dw_dev_pm_ops,
-		.of_match_table = of_match_ptr(dw_dma_id_table),
+		.of_match_table = of_match_ptr(dw_dma_of_id_table),
+		.acpi_match_table = ACPI_PTR(dw_dma_acpi_id_table),
 	},
-	.id_table	= dw_dma_ids,
 };
 
 static int __init dw_init(void)
diff --git a/drivers/dma/dw_dmac_regs.h b/drivers/dma/dw_dmac_regs.h
index 4d02c3669b75..9d417200bd57 100644
--- a/drivers/dma/dw_dmac_regs.h
+++ b/drivers/dma/dw_dmac_regs.h
@@ -212,8 +212,11 @@ struct dw_dma_chan {
 	/* hardware configuration */
 	unsigned int		block_size;
 	bool			nollp;
+
+	/* custom slave configuration */
 	unsigned int		request_line;
-	struct dw_dma_slave	slave;
+	unsigned char		src_master;
+	unsigned char		dst_master;
 
 	/* configuration passed via DMA_SLAVE_CONFIG */
 	struct dma_slave_config dma_sconfig;
@@ -247,7 +250,6 @@ struct dw_dma {
 	/* hardware configuration */
 	unsigned char		nr_masters;
 	unsigned char		data_width[4];
-	unsigned int		request_line_base;
 
 	struct dw_dma_chan	chan[0];
 };
diff --git a/drivers/dma/imx-dma.c b/drivers/dma/imx-dma.c
index 70b8975d107e..f28583370d00 100644
--- a/drivers/dma/imx-dma.c
+++ b/drivers/dma/imx-dma.c
@@ -859,8 +859,7 @@ static struct dma_async_tx_descriptor *imxdma_prep_dma_cyclic(
 
 	desc = list_first_entry(&imxdmac->ld_free, struct imxdma_desc, node);
 
-	if (imxdmac->sg_list)
-		kfree(imxdmac->sg_list);
+	kfree(imxdmac->sg_list);
 
 	imxdmac->sg_list = kcalloc(periods + 1,
 			sizeof(struct scatterlist), GFP_KERNEL);
@@ -1145,7 +1144,7 @@ err:
 	return ret;
 }
 
-static int __exit imxdma_remove(struct platform_device *pdev)
+static int imxdma_remove(struct platform_device *pdev)
 {
 	struct imxdma_engine *imxdma = platform_get_drvdata(pdev);
 
@@ -1162,7 +1161,7 @@ static struct platform_driver imxdma_driver = {
 		.name	= "imx-dma",
 	},
 	.id_table	= imx_dma_devtype,
-	.remove		= __exit_p(imxdma_remove),
+	.remove		= imxdma_remove,
 };
 
 static int __init imxdma_module_init(void)
diff --git a/drivers/dma/imx-sdma.c b/drivers/dma/imx-sdma.c
index f082aa3a918c..092867bf795c 100644
--- a/drivers/dma/imx-sdma.c
+++ b/drivers/dma/imx-sdma.c
@@ -1462,7 +1462,7 @@ err_irq:
 	return ret;
 }
 
-static int __exit sdma_remove(struct platform_device *pdev)
+static int sdma_remove(struct platform_device *pdev)
 {
 	return -EBUSY;
 }
@@ -1473,7 +1473,7 @@ static struct platform_driver sdma_driver = {
 		.of_match_table = sdma_dt_ids,
 	},
 	.id_table	= sdma_devtypes,
-	.remove		= __exit_p(sdma_remove),
+	.remove		= sdma_remove,
 };
 
 static int __init sdma_module_init(void)
diff --git a/drivers/dma/ioat/dca.c b/drivers/dma/ioat/dca.c
index 9b041858d10d..9e84d5bc9307 100644
--- a/drivers/dma/ioat/dca.c
+++ b/drivers/dma/ioat/dca.c
@@ -470,8 +470,10 @@ struct dca_provider *ioat2_dca_init(struct pci_dev *pdev, void __iomem *iobase)
 	}
 
 	if (!dca2_tag_map_valid(ioatdca->tag_map)) {
-		dev_err(&pdev->dev, "APICID_TAG_MAP set incorrectly by BIOS, "
-			"disabling DCA\n");
+		WARN_TAINT_ONCE(1, TAINT_FIRMWARE_WORKAROUND,
+				"%s %s: APICID_TAG_MAP set incorrectly by BIOS, disabling DCA\n",
+				dev_driver_string(&pdev->dev),
+				dev_name(&pdev->dev));
 		free_dca_provider(dca);
 		return NULL;
 	}
@@ -689,7 +691,10 @@ struct dca_provider *ioat3_dca_init(struct pci_dev *pdev, void __iomem *iobase)
 	}
 
 	if (dca3_tag_map_invalid(ioatdca->tag_map)) {
-		dev_err(&pdev->dev, "APICID_TAG_MAP set incorrectly by BIOS, disabling DCA\n");
+		WARN_TAINT_ONCE(1, TAINT_FIRMWARE_WORKAROUND,
+				"%s %s: APICID_TAG_MAP set incorrectly by BIOS, disabling DCA\n",
+				dev_driver_string(&pdev->dev),
+				dev_name(&pdev->dev));
 		free_dca_provider(dca);
 		return NULL;
 	}
diff --git a/drivers/dma/ioat/dma.c b/drivers/dma/ioat/dma.c
index 1879a5942bfc..17a2393b3e25 100644
--- a/drivers/dma/ioat/dma.c
+++ b/drivers/dma/ioat/dma.c
@@ -892,7 +892,7 @@ MODULE_PARM_DESC(ioat_interrupt_style,
  * ioat_dma_setup_interrupts - setup interrupt handler
  * @device: ioat device
  */
-static int ioat_dma_setup_interrupts(struct ioatdma_device *device)
+int ioat_dma_setup_interrupts(struct ioatdma_device *device)
 {
 	struct ioat_chan_common *chan;
 	struct pci_dev *pdev = device->pdev;
@@ -941,6 +941,7 @@ msix:
 		}
 	}
 	intrctrl |= IOAT_INTRCTRL_MSIX_VECTOR_CONTROL;
+	device->irq_mode = IOAT_MSIX;
 	goto done;
 
 msix_single_vector:
@@ -956,6 +957,7 @@ msix_single_vector:
 		pci_disable_msix(pdev);
 		goto msi;
 	}
+	device->irq_mode = IOAT_MSIX_SINGLE;
 	goto done;
 
 msi:
@@ -969,6 +971,7 @@ msi:
 		pci_disable_msi(pdev);
 		goto intx;
 	}
+	device->irq_mode = IOAT_MSIX;
 	goto done;
 
 intx:
@@ -977,6 +980,7 @@ intx:
 	if (err)
 		goto err_no_irq;
 
+	device->irq_mode = IOAT_INTX;
 done:
 	if (device->intr_quirk)
 		device->intr_quirk(device);
@@ -987,9 +991,11 @@ done:
 err_no_irq:
 	/* Disable all interrupt generation */
 	writeb(0, device->reg_base + IOAT_INTRCTRL_OFFSET);
+	device->irq_mode = IOAT_NOIRQ;
 	dev_err(dev, "no usable interrupts\n");
 	return err;
 }
+EXPORT_SYMBOL(ioat_dma_setup_interrupts);
 
 static void ioat_disable_interrupts(struct ioatdma_device *device)
 {
diff --git a/drivers/dma/ioat/dma.h b/drivers/dma/ioat/dma.h
index 53a4cbb78f47..54fb7b9ff9aa 100644
--- a/drivers/dma/ioat/dma.h
+++ b/drivers/dma/ioat/dma.h
@@ -39,6 +39,7 @@
 #define to_ioat_desc(lh) container_of(lh, struct ioat_desc_sw, node)
 #define tx_to_ioat_desc(tx) container_of(tx, struct ioat_desc_sw, txd)
 #define to_dev(ioat_chan) (&(ioat_chan)->device->pdev->dev)
+#define to_pdev(ioat_chan) ((ioat_chan)->device->pdev)
 
 #define chan_num(ch) ((int)((ch)->reg_base - (ch)->device->reg_base) / 0x80)
 
@@ -48,6 +49,14 @@
  */
 #define NULL_DESC_BUFFER_SIZE 1
 
+enum ioat_irq_mode {
+	IOAT_NOIRQ = 0,
+	IOAT_MSIX,
+	IOAT_MSIX_SINGLE,
+	IOAT_MSI,
+	IOAT_INTX
+};
+
 /**
  * struct ioatdma_device - internal representation of a IOAT device
  * @pdev: PCI-Express device
@@ -72,11 +81,16 @@ struct ioatdma_device {
 	void __iomem *reg_base;
 	struct pci_pool *dma_pool;
 	struct pci_pool *completion_pool;
+#define MAX_SED_POOLS	5
+	struct dma_pool *sed_hw_pool[MAX_SED_POOLS];
+	struct kmem_cache *sed_pool;
 	struct dma_device common;
 	u8 version;
 	struct msix_entry msix_entries[4];
 	struct ioat_chan_common *idx[4];
 	struct dca_provider *dca;
+	enum ioat_irq_mode irq_mode;
+	u32 cap;
 	void (*intr_quirk)(struct ioatdma_device *device);
 	int (*enumerate_channels)(struct ioatdma_device *device);
 	int (*reset_hw)(struct ioat_chan_common *chan);
@@ -131,6 +145,20 @@ struct ioat_dma_chan {
 	u16 active;
 };
 
+/**
+ * struct ioat_sed_ent - wrapper around super extended hardware descriptor
+ * @hw: hardware SED
+ * @sed_dma: dma address for the SED
+ * @list: list member
+ * @parent: point to the dma descriptor that's the parent
+ */
+struct ioat_sed_ent {
+	struct ioat_sed_raw_descriptor *hw;
+	dma_addr_t dma;
+	struct ioat_ring_ent *parent;
+	unsigned int hw_pool;
+};
+
 static inline struct ioat_chan_common *to_chan_common(struct dma_chan *c)
 {
 	return container_of(c, struct ioat_chan_common, common);
@@ -179,7 +207,7 @@ __dump_desc_dbg(struct ioat_chan_common *chan, struct ioat_dma_descriptor *hw,
 	struct device *dev = to_dev(chan);
 
 	dev_dbg(dev, "desc[%d]: (%#llx->%#llx) cookie: %d flags: %#x"
-		" ctl: %#x (op: %d int_en: %d compl: %d)\n", id,
+		" ctl: %#10.8x (op: %#x int_en: %d compl: %d)\n", id,
 		(unsigned long long) tx->phys,
 		(unsigned long long) hw->next, tx->cookie, tx->flags,
 		hw->ctl, hw->ctl_f.op, hw->ctl_f.int_en, hw->ctl_f.compl_write);
@@ -201,7 +229,7 @@ ioat_chan_by_index(struct ioatdma_device *device, int index)
 	return device->idx[index];
 }
 
-static inline u64 ioat_chansts(struct ioat_chan_common *chan)
+static inline u64 ioat_chansts_32(struct ioat_chan_common *chan)
 {
 	u8 ver = chan->device->version;
 	u64 status;
@@ -218,6 +246,26 @@ static inline u64 ioat_chansts(struct ioat_chan_common *chan)
 	return status;
 }
 
+#if BITS_PER_LONG == 64
+
+static inline u64 ioat_chansts(struct ioat_chan_common *chan)
+{
+	u8 ver = chan->device->version;
+	u64 status;
+
+	 /* With IOAT v3.3 the status register is 64bit.  */
+	if (ver >= IOAT_VER_3_3)
+		status = readq(chan->reg_base + IOAT_CHANSTS_OFFSET(ver));
+	else
+		status = ioat_chansts_32(chan);
+
+	return status;
+}
+
+#else
+#define ioat_chansts ioat_chansts_32
+#endif
+
 static inline void ioat_start(struct ioat_chan_common *chan)
 {
 	u8 ver = chan->device->version;
@@ -321,6 +369,7 @@ bool ioat_cleanup_preamble(struct ioat_chan_common *chan,
 			   dma_addr_t *phys_complete);
 void ioat_kobject_add(struct ioatdma_device *device, struct kobj_type *type);
 void ioat_kobject_del(struct ioatdma_device *device);
+int ioat_dma_setup_interrupts(struct ioatdma_device *device);
 extern const struct sysfs_ops ioat_sysfs_ops;
 extern struct ioat_sysfs_entry ioat_version_attr;
 extern struct ioat_sysfs_entry ioat_cap_attr;
diff --git a/drivers/dma/ioat/dma_v2.h b/drivers/dma/ioat/dma_v2.h
index e100f644e344..29bf9448035d 100644
--- a/drivers/dma/ioat/dma_v2.h
+++ b/drivers/dma/ioat/dma_v2.h
@@ -137,6 +137,7 @@ struct ioat_ring_ent {
 	#ifdef DEBUG
 	int id;
 	#endif
+	struct ioat_sed_ent *sed;
 };
 
 static inline struct ioat_ring_ent *
@@ -157,6 +158,7 @@ static inline void ioat2_set_chainaddr(struct ioat2_dma_chan *ioat, u64 addr)
 
 int ioat2_dma_probe(struct ioatdma_device *dev, int dca);
 int ioat3_dma_probe(struct ioatdma_device *dev, int dca);
+void ioat3_dma_remove(struct ioatdma_device *dev);
 struct dca_provider *ioat2_dca_init(struct pci_dev *pdev, void __iomem *iobase);
 struct dca_provider *ioat3_dca_init(struct pci_dev *pdev, void __iomem *iobase);
 int ioat2_check_space_lock(struct ioat2_dma_chan *ioat, int num_descs);
diff --git a/drivers/dma/ioat/dma_v3.c b/drivers/dma/ioat/dma_v3.c
index e8336cce360b..ca6ea9b3551b 100644
--- a/drivers/dma/ioat/dma_v3.c
+++ b/drivers/dma/ioat/dma_v3.c
@@ -55,7 +55,7 @@
 /*
  * Support routines for v3+ hardware
  */
-
+#include <linux/module.h>
 #include <linux/pci.h>
 #include <linux/gfp.h>
 #include <linux/dmaengine.h>
@@ -70,6 +70,10 @@
 /* ioat hardware assumes at least two sources for raid operations */
 #define src_cnt_to_sw(x) ((x) + 2)
 #define src_cnt_to_hw(x) ((x) - 2)
+#define ndest_to_sw(x) ((x) + 1)
+#define ndest_to_hw(x) ((x) - 1)
+#define src16_cnt_to_sw(x) ((x) + 9)
+#define src16_cnt_to_hw(x) ((x) - 9)
 
 /* provide a lookup table for setting the source address in the base or
  * extended descriptor of an xor or pq descriptor
@@ -77,7 +81,20 @@
 static const u8 xor_idx_to_desc = 0xe0;
 static const u8 xor_idx_to_field[] = { 1, 4, 5, 6, 7, 0, 1, 2 };
 static const u8 pq_idx_to_desc = 0xf8;
+static const u8 pq16_idx_to_desc[] = { 0, 0, 1, 1, 1, 1, 1, 1, 1,
+				       2, 2, 2, 2, 2, 2, 2 };
 static const u8 pq_idx_to_field[] = { 1, 4, 5, 0, 1, 2, 4, 5 };
+static const u8 pq16_idx_to_field[] = { 1, 4, 1, 2, 3, 4, 5, 6, 7,
+					0, 1, 2, 3, 4, 5, 6 };
+
+/*
+ * technically sources 1 and 2 do not require SED, but the op will have
+ * at least 9 descriptors so that's irrelevant.
+ */
+static const u8 pq16_idx_to_sed[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0,
+				      1, 1, 1, 1, 1, 1, 1 };
+
+static void ioat3_eh(struct ioat2_dma_chan *ioat);
 
 static dma_addr_t xor_get_src(struct ioat_raw_descriptor *descs[2], int idx)
 {
@@ -101,6 +118,13 @@ static dma_addr_t pq_get_src(struct ioat_raw_descriptor *descs[2], int idx)
 	return raw->field[pq_idx_to_field[idx]];
 }
 
+static dma_addr_t pq16_get_src(struct ioat_raw_descriptor *desc[3], int idx)
+{
+	struct ioat_raw_descriptor *raw = desc[pq16_idx_to_desc[idx]];
+
+	return raw->field[pq16_idx_to_field[idx]];
+}
+
 static void pq_set_src(struct ioat_raw_descriptor *descs[2],
 		       dma_addr_t addr, u32 offset, u8 coef, int idx)
 {
@@ -111,6 +135,167 @@ static void pq_set_src(struct ioat_raw_descriptor *descs[2],
 	pq->coef[idx] = coef;
 }
 
+static int sed_get_pq16_pool_idx(int src_cnt)
+{
+
+	return pq16_idx_to_sed[src_cnt];
+}
+
+static bool is_jf_ioat(struct pci_dev *pdev)
+{
+	switch (pdev->device) {
+	case PCI_DEVICE_ID_INTEL_IOAT_JSF0:
+	case PCI_DEVICE_ID_INTEL_IOAT_JSF1:
+	case PCI_DEVICE_ID_INTEL_IOAT_JSF2:
+	case PCI_DEVICE_ID_INTEL_IOAT_JSF3:
+	case PCI_DEVICE_ID_INTEL_IOAT_JSF4:
+	case PCI_DEVICE_ID_INTEL_IOAT_JSF5:
+	case PCI_DEVICE_ID_INTEL_IOAT_JSF6:
+	case PCI_DEVICE_ID_INTEL_IOAT_JSF7:
+	case PCI_DEVICE_ID_INTEL_IOAT_JSF8:
+	case PCI_DEVICE_ID_INTEL_IOAT_JSF9:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool is_snb_ioat(struct pci_dev *pdev)
+{
+	switch (pdev->device) {
+	case PCI_DEVICE_ID_INTEL_IOAT_SNB0:
+	case PCI_DEVICE_ID_INTEL_IOAT_SNB1:
+	case PCI_DEVICE_ID_INTEL_IOAT_SNB2:
+	case PCI_DEVICE_ID_INTEL_IOAT_SNB3:
+	case PCI_DEVICE_ID_INTEL_IOAT_SNB4:
+	case PCI_DEVICE_ID_INTEL_IOAT_SNB5:
+	case PCI_DEVICE_ID_INTEL_IOAT_SNB6:
+	case PCI_DEVICE_ID_INTEL_IOAT_SNB7:
+	case PCI_DEVICE_ID_INTEL_IOAT_SNB8:
+	case PCI_DEVICE_ID_INTEL_IOAT_SNB9:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool is_ivb_ioat(struct pci_dev *pdev)
+{
+	switch (pdev->device) {
+	case PCI_DEVICE_ID_INTEL_IOAT_IVB0:
+	case PCI_DEVICE_ID_INTEL_IOAT_IVB1:
+	case PCI_DEVICE_ID_INTEL_IOAT_IVB2:
+	case PCI_DEVICE_ID_INTEL_IOAT_IVB3:
+	case PCI_DEVICE_ID_INTEL_IOAT_IVB4:
+	case PCI_DEVICE_ID_INTEL_IOAT_IVB5:
+	case PCI_DEVICE_ID_INTEL_IOAT_IVB6:
+	case PCI_DEVICE_ID_INTEL_IOAT_IVB7:
+	case PCI_DEVICE_ID_INTEL_IOAT_IVB8:
+	case PCI_DEVICE_ID_INTEL_IOAT_IVB9:
+		return true;
+	default:
+		return false;
+	}
+
+}
+
+static bool is_hsw_ioat(struct pci_dev *pdev)
+{
+	switch (pdev->device) {
+	case PCI_DEVICE_ID_INTEL_IOAT_HSW0:
+	case PCI_DEVICE_ID_INTEL_IOAT_HSW1:
+	case PCI_DEVICE_ID_INTEL_IOAT_HSW2:
+	case PCI_DEVICE_ID_INTEL_IOAT_HSW3:
+	case PCI_DEVICE_ID_INTEL_IOAT_HSW4:
+	case PCI_DEVICE_ID_INTEL_IOAT_HSW5:
+	case PCI_DEVICE_ID_INTEL_IOAT_HSW6:
+	case PCI_DEVICE_ID_INTEL_IOAT_HSW7:
+	case PCI_DEVICE_ID_INTEL_IOAT_HSW8:
+	case PCI_DEVICE_ID_INTEL_IOAT_HSW9:
+		return true;
+	default:
+		return false;
+	}
+
+}
+
+static bool is_xeon_cb32(struct pci_dev *pdev)
+{
+	return is_jf_ioat(pdev) || is_snb_ioat(pdev) || is_ivb_ioat(pdev) ||
+		is_hsw_ioat(pdev);
+}
+
+static bool is_bwd_ioat(struct pci_dev *pdev)
+{
+	switch (pdev->device) {
+	case PCI_DEVICE_ID_INTEL_IOAT_BWD0:
+	case PCI_DEVICE_ID_INTEL_IOAT_BWD1:
+	case PCI_DEVICE_ID_INTEL_IOAT_BWD2:
+	case PCI_DEVICE_ID_INTEL_IOAT_BWD3:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool is_bwd_noraid(struct pci_dev *pdev)
+{
+	switch (pdev->device) {
+	case PCI_DEVICE_ID_INTEL_IOAT_BWD2:
+	case PCI_DEVICE_ID_INTEL_IOAT_BWD3:
+		return true;
+	default:
+		return false;
+	}
+
+}
+
+static void pq16_set_src(struct ioat_raw_descriptor *desc[3],
+			dma_addr_t addr, u32 offset, u8 coef, int idx)
+{
+	struct ioat_pq_descriptor *pq = (struct ioat_pq_descriptor *)desc[0];
+	struct ioat_pq16a_descriptor *pq16 =
+		(struct ioat_pq16a_descriptor *)desc[1];
+	struct ioat_raw_descriptor *raw = desc[pq16_idx_to_desc[idx]];
+
+	raw->field[pq16_idx_to_field[idx]] = addr + offset;
+
+	if (idx < 8)
+		pq->coef[idx] = coef;
+	else
+		pq16->coef[idx - 8] = coef;
+}
+
+static struct ioat_sed_ent *
+ioat3_alloc_sed(struct ioatdma_device *device, unsigned int hw_pool)
+{
+	struct ioat_sed_ent *sed;
+	gfp_t flags = __GFP_ZERO | GFP_ATOMIC;
+
+	sed = kmem_cache_alloc(device->sed_pool, flags);
+	if (!sed)
+		return NULL;
+
+	sed->hw_pool = hw_pool;
+	sed->hw = dma_pool_alloc(device->sed_hw_pool[hw_pool],
+				 flags, &sed->dma);
+	if (!sed->hw) {
+		kmem_cache_free(device->sed_pool, sed);
+		return NULL;
+	}
+
+	return sed;
+}
+
+static void ioat3_free_sed(struct ioatdma_device *device, struct ioat_sed_ent *sed)
+{
+	if (!sed)
+		return;
+
+	dma_pool_free(device->sed_hw_pool[sed->hw_pool], sed->hw, sed->dma);
+	kmem_cache_free(device->sed_pool, sed);
+}
+
 static void ioat3_dma_unmap(struct ioat2_dma_chan *ioat,
 			    struct ioat_ring_ent *desc, int idx)
 {
@@ -223,6 +408,54 @@ static void ioat3_dma_unmap(struct ioat2_dma_chan *ioat,
 		}
 		break;
 	}
+	case IOAT_OP_PQ_16S:
+	case IOAT_OP_PQ_VAL_16S: {
+		struct ioat_pq_descriptor *pq = desc->pq;
+		int src_cnt = src16_cnt_to_sw(pq->ctl_f.src_cnt);
+		struct ioat_raw_descriptor *descs[4];
+		int i;
+
+		/* in the 'continue' case don't unmap the dests as sources */
+		if (dmaf_p_disabled_continue(flags))
+			src_cnt--;
+		else if (dmaf_continue(flags))
+			src_cnt -= 3;
+
+		if (!(flags & DMA_COMPL_SKIP_SRC_UNMAP)) {
+			descs[0] = (struct ioat_raw_descriptor *)pq;
+			descs[1] = (struct ioat_raw_descriptor *)(desc->sed->hw);
+			descs[2] = (struct ioat_raw_descriptor *)(&desc->sed->hw->b[0]);
+			for (i = 0; i < src_cnt; i++) {
+				dma_addr_t src = pq16_get_src(descs, i);
+
+				ioat_unmap(pdev, src - offset, len,
+					   PCI_DMA_TODEVICE, flags, 0);
+			}
+
+			/* the dests are sources in pq validate operations */
+			if (pq->ctl_f.op == IOAT_OP_XOR_VAL) {
+				if (!(flags & DMA_PREP_PQ_DISABLE_P))
+					ioat_unmap(pdev, pq->p_addr - offset,
+						   len, PCI_DMA_TODEVICE,
+						   flags, 0);
+				if (!(flags & DMA_PREP_PQ_DISABLE_Q))
+					ioat_unmap(pdev, pq->q_addr - offset,
+						   len, PCI_DMA_TODEVICE,
+						   flags, 0);
+				break;
+			}
+		}
+
+		if (!(flags & DMA_COMPL_SKIP_DEST_UNMAP)) {
+			if (!(flags & DMA_PREP_PQ_DISABLE_P))
+				ioat_unmap(pdev, pq->p_addr - offset, len,
+					   PCI_DMA_BIDIRECTIONAL, flags, 1);
+			if (!(flags & DMA_PREP_PQ_DISABLE_Q))
+				ioat_unmap(pdev, pq->q_addr - offset, len,
+					   PCI_DMA_BIDIRECTIONAL, flags, 1);
+		}
+		break;
+	}
 	default:
 		dev_err(&pdev->dev, "%s: unknown op type: %#x\n",
 			__func__, desc->hw->ctl_f.op);
@@ -250,6 +483,63 @@ static bool desc_has_ext(struct ioat_ring_ent *desc)
 	return false;
 }
 
+static u64 ioat3_get_current_completion(struct ioat_chan_common *chan)
+{
+	u64 phys_complete;
+	u64 completion;
+
+	completion = *chan->completion;
+	phys_complete = ioat_chansts_to_addr(completion);
+
+	dev_dbg(to_dev(chan), "%s: phys_complete: %#llx\n", __func__,
+		(unsigned long long) phys_complete);
+
+	return phys_complete;
+}
+
+static bool ioat3_cleanup_preamble(struct ioat_chan_common *chan,
+				   u64 *phys_complete)
+{
+	*phys_complete = ioat3_get_current_completion(chan);
+	if (*phys_complete == chan->last_completion)
+		return false;
+
+	clear_bit(IOAT_COMPLETION_ACK, &chan->state);
+	mod_timer(&chan->timer, jiffies + COMPLETION_TIMEOUT);
+
+	return true;
+}
+
+static void
+desc_get_errstat(struct ioat2_dma_chan *ioat, struct ioat_ring_ent *desc)
+{
+	struct ioat_dma_descriptor *hw = desc->hw;
+
+	switch (hw->ctl_f.op) {
+	case IOAT_OP_PQ_VAL:
+	case IOAT_OP_PQ_VAL_16S:
+	{
+		struct ioat_pq_descriptor *pq = desc->pq;
+
+		/* check if there's error written */
+		if (!pq->dwbes_f.wbes)
+			return;
+
+		/* need to set a chanerr var for checking to clear later */
+
+		if (pq->dwbes_f.p_val_err)
+			*desc->result |= SUM_CHECK_P_RESULT;
+
+		if (pq->dwbes_f.q_val_err)
+			*desc->result |= SUM_CHECK_Q_RESULT;
+
+		return;
+	}
+	default:
+		return;
+	}
+}
+
 /**
  * __cleanup - reclaim used descriptors
  * @ioat: channel (ring) to clean
@@ -260,6 +550,7 @@ static bool desc_has_ext(struct ioat_ring_ent *desc)
 static void __cleanup(struct ioat2_dma_chan *ioat, dma_addr_t phys_complete)
 {
 	struct ioat_chan_common *chan = &ioat->base;
+	struct ioatdma_device *device = chan->device;
 	struct ioat_ring_ent *desc;
 	bool seen_current = false;
 	int idx = ioat->tail, i;
@@ -268,6 +559,16 @@ static void __cleanup(struct ioat2_dma_chan *ioat, dma_addr_t phys_complete)
 	dev_dbg(to_dev(chan), "%s: head: %#x tail: %#x issued: %#x\n",
 		__func__, ioat->head, ioat->tail, ioat->issued);
 
+	/*
+	 * At restart of the channel, the completion address and the
+	 * channel status will be 0 due to starting a new chain. Since
+	 * it's new chain and the first descriptor "fails", there is
+	 * nothing to clean up. We do not want to reap the entire submitted
+	 * chain due to this 0 address value and then BUG.
+	 */
+	if (!phys_complete)
+		return;
+
 	active = ioat2_ring_active(ioat);
 	for (i = 0; i < active && !seen_current; i++) {
 		struct dma_async_tx_descriptor *tx;
@@ -276,6 +577,11 @@ static void __cleanup(struct ioat2_dma_chan *ioat, dma_addr_t phys_complete)
 		prefetch(ioat2_get_ring_ent(ioat, idx + i + 1));
 		desc = ioat2_get_ring_ent(ioat, idx + i);
 		dump_desc_dbg(ioat, desc);
+
+		/* set err stat if we are using dwbes */
+		if (device->cap & IOAT_CAP_DWBES)
+			desc_get_errstat(ioat, desc);
+
 		tx = &desc->txd;
 		if (tx->cookie) {
 			dma_cookie_complete(tx);
@@ -294,6 +600,12 @@ static void __cleanup(struct ioat2_dma_chan *ioat, dma_addr_t phys_complete)
 			BUG_ON(i + 1 >= active);
 			i++;
 		}
+
+		/* cleanup super extended descriptors */
+		if (desc->sed) {
+			ioat3_free_sed(device, desc->sed);
+			desc->sed = NULL;
+		}
 	}
 	smp_mb(); /* finish all descriptor reads before incrementing tail */
 	ioat->tail = idx + i;
@@ -314,11 +626,22 @@ static void __cleanup(struct ioat2_dma_chan *ioat, dma_addr_t phys_complete)
 static void ioat3_cleanup(struct ioat2_dma_chan *ioat)
 {
 	struct ioat_chan_common *chan = &ioat->base;
-	dma_addr_t phys_complete;
+	u64 phys_complete;
 
 	spin_lock_bh(&chan->cleanup_lock);
-	if (ioat_cleanup_preamble(chan, &phys_complete))
+
+	if (ioat3_cleanup_preamble(chan, &phys_complete))
 		__cleanup(ioat, phys_complete);
+
+	if (is_ioat_halted(*chan->completion)) {
+		u32 chanerr = readl(chan->reg_base + IOAT_CHANERR_OFFSET);
+
+		if (chanerr & IOAT_CHANERR_HANDLE_MASK) {
+			mod_timer(&chan->timer, jiffies + IDLE_TIMEOUT);
+			ioat3_eh(ioat);
+		}
+	}
+
 	spin_unlock_bh(&chan->cleanup_lock);
 }
 
@@ -333,15 +656,78 @@ static void ioat3_cleanup_event(unsigned long data)
 static void ioat3_restart_channel(struct ioat2_dma_chan *ioat)
 {
 	struct ioat_chan_common *chan = &ioat->base;
-	dma_addr_t phys_complete;
+	u64 phys_complete;
 
 	ioat2_quiesce(chan, 0);
-	if (ioat_cleanup_preamble(chan, &phys_complete))
+	if (ioat3_cleanup_preamble(chan, &phys_complete))
 		__cleanup(ioat, phys_complete);
 
 	__ioat2_restart_chan(ioat);
 }
 
+static void ioat3_eh(struct ioat2_dma_chan *ioat)
+{
+	struct ioat_chan_common *chan = &ioat->base;
+	struct pci_dev *pdev = to_pdev(chan);
+	struct ioat_dma_descriptor *hw;
+	u64 phys_complete;
+	struct ioat_ring_ent *desc;
+	u32 err_handled = 0;
+	u32 chanerr_int;
+	u32 chanerr;
+
+	/* cleanup so tail points to descriptor that caused the error */
+	if (ioat3_cleanup_preamble(chan, &phys_complete))
+		__cleanup(ioat, phys_complete);
+
+	chanerr = readl(chan->reg_base + IOAT_CHANERR_OFFSET);
+	pci_read_config_dword(pdev, IOAT_PCI_CHANERR_INT_OFFSET, &chanerr_int);
+
+	dev_dbg(to_dev(chan), "%s: error = %x:%x\n",
+		__func__, chanerr, chanerr_int);
+
+	desc = ioat2_get_ring_ent(ioat, ioat->tail);
+	hw = desc->hw;
+	dump_desc_dbg(ioat, desc);
+
+	switch (hw->ctl_f.op) {
+	case IOAT_OP_XOR_VAL:
+		if (chanerr & IOAT_CHANERR_XOR_P_OR_CRC_ERR) {
+			*desc->result |= SUM_CHECK_P_RESULT;
+			err_handled |= IOAT_CHANERR_XOR_P_OR_CRC_ERR;
+		}
+		break;
+	case IOAT_OP_PQ_VAL:
+	case IOAT_OP_PQ_VAL_16S:
+		if (chanerr & IOAT_CHANERR_XOR_P_OR_CRC_ERR) {
+			*desc->result |= SUM_CHECK_P_RESULT;
+			err_handled |= IOAT_CHANERR_XOR_P_OR_CRC_ERR;
+		}
+		if (chanerr & IOAT_CHANERR_XOR_Q_ERR) {
+			*desc->result |= SUM_CHECK_Q_RESULT;
+			err_handled |= IOAT_CHANERR_XOR_Q_ERR;
+		}
+		break;
+	}
+
+	/* fault on unhandled error or spurious halt */
+	if (chanerr ^ err_handled || chanerr == 0) {
+		dev_err(to_dev(chan), "%s: fatal error (%x:%x)\n",
+			__func__, chanerr, err_handled);
+		BUG();
+	}
+
+	writel(chanerr, chan->reg_base + IOAT_CHANERR_OFFSET);
+	pci_write_config_dword(pdev, IOAT_PCI_CHANERR_INT_OFFSET, chanerr_int);
+
+	/* mark faulting descriptor as complete */
+	*chan->completion = desc->txd.phys;
+
+	spin_lock_bh(&ioat->prep_lock);
+	ioat3_restart_channel(ioat);
+	spin_unlock_bh(&ioat->prep_lock);
+}
+
 static void check_active(struct ioat2_dma_chan *ioat)
 {
 	struct ioat_chan_common *chan = &ioat->base;
@@ -605,7 +991,8 @@ dump_pq_desc_dbg(struct ioat2_dma_chan *ioat, struct ioat_ring_ent *desc, struct
 	int i;
 
 	dev_dbg(dev, "desc[%d]: (%#llx->%#llx) flags: %#x"
-		" sz: %#x ctl: %#x (op: %d int: %d compl: %d pq: '%s%s' src_cnt: %d)\n",
+		" sz: %#10.8x ctl: %#x (op: %#x int: %d compl: %d pq: '%s%s'"
+		" src_cnt: %d)\n",
 		desc_id(desc), (unsigned long long) desc->txd.phys,
 		(unsigned long long) (pq_ex ? pq_ex->next : pq->next),
 		desc->txd.flags, pq->size, pq->ctl, pq->ctl_f.op, pq->ctl_f.int_en,
@@ -617,6 +1004,42 @@ dump_pq_desc_dbg(struct ioat2_dma_chan *ioat, struct ioat_ring_ent *desc, struct
 			(unsigned long long) pq_get_src(descs, i), pq->coef[i]);
 	dev_dbg(dev, "\tP: %#llx\n", pq->p_addr);
 	dev_dbg(dev, "\tQ: %#llx\n", pq->q_addr);
+	dev_dbg(dev, "\tNEXT: %#llx\n", pq->next);
+}
+
+static void dump_pq16_desc_dbg(struct ioat2_dma_chan *ioat,
+			       struct ioat_ring_ent *desc)
+{
+	struct device *dev = to_dev(&ioat->base);
+	struct ioat_pq_descriptor *pq = desc->pq;
+	struct ioat_raw_descriptor *descs[] = { (void *)pq,
+						(void *)pq,
+						(void *)pq };
+	int src_cnt = src16_cnt_to_sw(pq->ctl_f.src_cnt);
+	int i;
+
+	if (desc->sed) {
+		descs[1] = (void *)desc->sed->hw;
+		descs[2] = (void *)desc->sed->hw + 64;
+	}
+
+	dev_dbg(dev, "desc[%d]: (%#llx->%#llx) flags: %#x"
+		" sz: %#x ctl: %#x (op: %#x int: %d compl: %d pq: '%s%s'"
+		" src_cnt: %d)\n",
+		desc_id(desc), (unsigned long long) desc->txd.phys,
+		(unsigned long long) pq->next,
+		desc->txd.flags, pq->size, pq->ctl,
+		pq->ctl_f.op, pq->ctl_f.int_en,
+		pq->ctl_f.compl_write,
+		pq->ctl_f.p_disable ? "" : "p", pq->ctl_f.q_disable ? "" : "q",
+		pq->ctl_f.src_cnt);
+	for (i = 0; i < src_cnt; i++) {
+		dev_dbg(dev, "\tsrc[%d]: %#llx coef: %#x\n", i,
+			(unsigned long long) pq16_get_src(descs, i),
+			pq->coef[i]);
+	}
+	dev_dbg(dev, "\tP: %#llx\n", pq->p_addr);
+	dev_dbg(dev, "\tQ: %#llx\n", pq->q_addr);
 }
 
 static struct dma_async_tx_descriptor *
@@ -627,6 +1050,7 @@ __ioat3_prep_pq_lock(struct dma_chan *c, enum sum_check_flags *result,
 {
 	struct ioat2_dma_chan *ioat = to_ioat2_chan(c);
 	struct ioat_chan_common *chan = &ioat->base;
+	struct ioatdma_device *device = chan->device;
 	struct ioat_ring_ent *compl_desc;
 	struct ioat_ring_ent *desc;
 	struct ioat_ring_ent *ext;
@@ -637,6 +1061,7 @@ __ioat3_prep_pq_lock(struct dma_chan *c, enum sum_check_flags *result,
 	u32 offset = 0;
 	u8 op = result ? IOAT_OP_PQ_VAL : IOAT_OP_PQ;
 	int i, s, idx, with_ext, num_descs;
+	int cb32 = (device->version < IOAT_VER_3_3) ? 1 : 0;
 
 	dev_dbg(to_dev(chan), "%s\n", __func__);
 	/* the engine requires at least two sources (we provide
@@ -662,7 +1087,7 @@ __ioat3_prep_pq_lock(struct dma_chan *c, enum sum_check_flags *result,
 	 * order.
 	 */
 	if (likely(num_descs) &&
-	    ioat2_check_space_lock(ioat, num_descs+1) == 0)
+	    ioat2_check_space_lock(ioat, num_descs + cb32) == 0)
 		idx = ioat->head;
 	else
 		return NULL;
@@ -700,6 +1125,9 @@ __ioat3_prep_pq_lock(struct dma_chan *c, enum sum_check_flags *result,
 		pq->q_addr = dst[1] + offset;
 		pq->ctl = 0;
 		pq->ctl_f.op = op;
+		/* we turn on descriptor write back error status */
+		if (device->cap & IOAT_CAP_DWBES)
+			pq->ctl_f.wb_en = result ? 1 : 0;
 		pq->ctl_f.src_cnt = src_cnt_to_hw(s);
 		pq->ctl_f.p_disable = !!(flags & DMA_PREP_PQ_DISABLE_P);
 		pq->ctl_f.q_disable = !!(flags & DMA_PREP_PQ_DISABLE_Q);
@@ -716,26 +1144,140 @@ __ioat3_prep_pq_lock(struct dma_chan *c, enum sum_check_flags *result,
 	pq->ctl_f.fence = !!(flags & DMA_PREP_FENCE);
 	dump_pq_desc_dbg(ioat, desc, ext);
 
-	/* completion descriptor carries interrupt bit */
-	compl_desc = ioat2_get_ring_ent(ioat, idx + i);
-	compl_desc->txd.flags = flags & DMA_PREP_INTERRUPT;
-	hw = compl_desc->hw;
-	hw->ctl = 0;
-	hw->ctl_f.null = 1;
-	hw->ctl_f.int_en = !!(flags & DMA_PREP_INTERRUPT);
-	hw->ctl_f.compl_write = 1;
-	hw->size = NULL_DESC_BUFFER_SIZE;
-	dump_desc_dbg(ioat, compl_desc);
+	if (!cb32) {
+		pq->ctl_f.int_en = !!(flags & DMA_PREP_INTERRUPT);
+		pq->ctl_f.compl_write = 1;
+		compl_desc = desc;
+	} else {
+		/* completion descriptor carries interrupt bit */
+		compl_desc = ioat2_get_ring_ent(ioat, idx + i);
+		compl_desc->txd.flags = flags & DMA_PREP_INTERRUPT;
+		hw = compl_desc->hw;
+		hw->ctl = 0;
+		hw->ctl_f.null = 1;
+		hw->ctl_f.int_en = !!(flags & DMA_PREP_INTERRUPT);
+		hw->ctl_f.compl_write = 1;
+		hw->size = NULL_DESC_BUFFER_SIZE;
+		dump_desc_dbg(ioat, compl_desc);
+	}
+
 
 	/* we leave the channel locked to ensure in order submission */
 	return &compl_desc->txd;
 }
 
 static struct dma_async_tx_descriptor *
+__ioat3_prep_pq16_lock(struct dma_chan *c, enum sum_check_flags *result,
+		       const dma_addr_t *dst, const dma_addr_t *src,
+		       unsigned int src_cnt, const unsigned char *scf,
+		       size_t len, unsigned long flags)
+{
+	struct ioat2_dma_chan *ioat = to_ioat2_chan(c);
+	struct ioat_chan_common *chan = &ioat->base;
+	struct ioatdma_device *device = chan->device;
+	struct ioat_ring_ent *desc;
+	size_t total_len = len;
+	struct ioat_pq_descriptor *pq;
+	u32 offset = 0;
+	u8 op;
+	int i, s, idx, num_descs;
+
+	/* this function only handles src_cnt 9 - 16 */
+	BUG_ON(src_cnt < 9);
+
+	/* this function is only called with 9-16 sources */
+	op = result ? IOAT_OP_PQ_VAL_16S : IOAT_OP_PQ_16S;
+
+	dev_dbg(to_dev(chan), "%s\n", __func__);
+
+	num_descs = ioat2_xferlen_to_descs(ioat, len);
+
+	/*
+	 * 16 source pq is only available on cb3.3 and has no completion
+	 * write hw bug.
+	 */
+	if (num_descs && ioat2_check_space_lock(ioat, num_descs) == 0)
+		idx = ioat->head;
+	else
+		return NULL;
+
+	i = 0;
+
+	do {
+		struct ioat_raw_descriptor *descs[4];
+		size_t xfer_size = min_t(size_t, len, 1 << ioat->xfercap_log);
+
+		desc = ioat2_get_ring_ent(ioat, idx + i);
+		pq = desc->pq;
+
+		descs[0] = (struct ioat_raw_descriptor *) pq;
+
+		desc->sed = ioat3_alloc_sed(device,
+					    sed_get_pq16_pool_idx(src_cnt));
+		if (!desc->sed) {
+			dev_err(to_dev(chan),
+				"%s: no free sed entries\n", __func__);
+			return NULL;
+		}
+
+		pq->sed_addr = desc->sed->dma;
+		desc->sed->parent = desc;
+
+		descs[1] = (struct ioat_raw_descriptor *)desc->sed->hw;
+		descs[2] = (void *)descs[1] + 64;
+
+		for (s = 0; s < src_cnt; s++)
+			pq16_set_src(descs, src[s], offset, scf[s], s);
+
+		/* see the comment for dma_maxpq in include/linux/dmaengine.h */
+		if (dmaf_p_disabled_continue(flags))
+			pq16_set_src(descs, dst[1], offset, 1, s++);
+		else if (dmaf_continue(flags)) {
+			pq16_set_src(descs, dst[0], offset, 0, s++);
+			pq16_set_src(descs, dst[1], offset, 1, s++);
+			pq16_set_src(descs, dst[1], offset, 0, s++);
+		}
+
+		pq->size = xfer_size;
+		pq->p_addr = dst[0] + offset;
+		pq->q_addr = dst[1] + offset;
+		pq->ctl = 0;
+		pq->ctl_f.op = op;
+		pq->ctl_f.src_cnt = src16_cnt_to_hw(s);
+		/* we turn on descriptor write back error status */
+		if (device->cap & IOAT_CAP_DWBES)
+			pq->ctl_f.wb_en = result ? 1 : 0;
+		pq->ctl_f.p_disable = !!(flags & DMA_PREP_PQ_DISABLE_P);
+		pq->ctl_f.q_disable = !!(flags & DMA_PREP_PQ_DISABLE_Q);
+
+		len -= xfer_size;
+		offset += xfer_size;
+	} while (++i < num_descs);
+
+	/* last pq descriptor carries the unmap parameters and fence bit */
+	desc->txd.flags = flags;
+	desc->len = total_len;
+	if (result)
+		desc->result = result;
+	pq->ctl_f.fence = !!(flags & DMA_PREP_FENCE);
+
+	/* with cb3.3 we should be able to do completion w/o a null desc */
+	pq->ctl_f.int_en = !!(flags & DMA_PREP_INTERRUPT);
+	pq->ctl_f.compl_write = 1;
+
+	dump_pq16_desc_dbg(ioat, desc);
+
+	/* we leave the channel locked to ensure in order submission */
+	return &desc->txd;
+}
+
+static struct dma_async_tx_descriptor *
 ioat3_prep_pq(struct dma_chan *chan, dma_addr_t *dst, dma_addr_t *src,
 	      unsigned int src_cnt, const unsigned char *scf, size_t len,
 	      unsigned long flags)
 {
+	struct dma_device *dma = chan->device;
+
 	/* specify valid address for disabled result */
 	if (flags & DMA_PREP_PQ_DISABLE_P)
 		dst[0] = dst[1];
@@ -755,11 +1297,20 @@ ioat3_prep_pq(struct dma_chan *chan, dma_addr_t *dst, dma_addr_t *src,
 		single_source_coef[0] = scf[0];
 		single_source_coef[1] = 0;
 
-		return __ioat3_prep_pq_lock(chan, NULL, dst, single_source, 2,
-					    single_source_coef, len, flags);
-	} else
-		return __ioat3_prep_pq_lock(chan, NULL, dst, src, src_cnt, scf,
-					    len, flags);
+		return (src_cnt > 8) && (dma->max_pq > 8) ?
+			__ioat3_prep_pq16_lock(chan, NULL, dst, single_source,
+					       2, single_source_coef, len,
+					       flags) :
+			__ioat3_prep_pq_lock(chan, NULL, dst, single_source, 2,
+					     single_source_coef, len, flags);
+
+	} else {
+		return (src_cnt > 8) && (dma->max_pq > 8) ?
+			__ioat3_prep_pq16_lock(chan, NULL, dst, src, src_cnt,
+					       scf, len, flags) :
+			__ioat3_prep_pq_lock(chan, NULL, dst, src, src_cnt,
+					     scf, len, flags);
+	}
 }
 
 struct dma_async_tx_descriptor *
@@ -767,6 +1318,8 @@ ioat3_prep_pq_val(struct dma_chan *chan, dma_addr_t *pq, dma_addr_t *src,
 		  unsigned int src_cnt, const unsigned char *scf, size_t len,
 		  enum sum_check_flags *pqres, unsigned long flags)
 {
+	struct dma_device *dma = chan->device;
+
 	/* specify valid address for disabled result */
 	if (flags & DMA_PREP_PQ_DISABLE_P)
 		pq[0] = pq[1];
@@ -778,14 +1331,18 @@ ioat3_prep_pq_val(struct dma_chan *chan, dma_addr_t *pq, dma_addr_t *src,
 	 */
 	*pqres = 0;
 
-	return __ioat3_prep_pq_lock(chan, pqres, pq, src, src_cnt, scf, len,
-				    flags);
+	return (src_cnt > 8) && (dma->max_pq > 8) ?
+		__ioat3_prep_pq16_lock(chan, pqres, pq, src, src_cnt, scf, len,
+				       flags) :
+		__ioat3_prep_pq_lock(chan, pqres, pq, src, src_cnt, scf, len,
+				     flags);
 }
 
 static struct dma_async_tx_descriptor *
 ioat3_prep_pqxor(struct dma_chan *chan, dma_addr_t dst, dma_addr_t *src,
 		 unsigned int src_cnt, size_t len, unsigned long flags)
 {
+	struct dma_device *dma = chan->device;
 	unsigned char scf[src_cnt];
 	dma_addr_t pq[2];
 
@@ -794,8 +1351,11 @@ ioat3_prep_pqxor(struct dma_chan *chan, dma_addr_t dst, dma_addr_t *src,
 	flags |= DMA_PREP_PQ_DISABLE_Q;
 	pq[1] = dst; /* specify valid address for disabled result */
 
-	return __ioat3_prep_pq_lock(chan, NULL, pq, src, src_cnt, scf, len,
-				    flags);
+	return (src_cnt > 8) && (dma->max_pq > 8) ?
+		__ioat3_prep_pq16_lock(chan, NULL, pq, src, src_cnt, scf, len,
+				       flags) :
+		__ioat3_prep_pq_lock(chan, NULL, pq, src, src_cnt, scf, len,
+				     flags);
 }
 
 struct dma_async_tx_descriptor *
@@ -803,6 +1363,7 @@ ioat3_prep_pqxor_val(struct dma_chan *chan, dma_addr_t *src,
 		     unsigned int src_cnt, size_t len,
 		     enum sum_check_flags *result, unsigned long flags)
 {
+	struct dma_device *dma = chan->device;
 	unsigned char scf[src_cnt];
 	dma_addr_t pq[2];
 
@@ -816,8 +1377,12 @@ ioat3_prep_pqxor_val(struct dma_chan *chan, dma_addr_t *src,
 	flags |= DMA_PREP_PQ_DISABLE_Q;
 	pq[1] = pq[0]; /* specify valid address for disabled result */
 
-	return __ioat3_prep_pq_lock(chan, result, pq, &src[1], src_cnt - 1, scf,
-				    len, flags);
+
+	return (src_cnt > 8) && (dma->max_pq > 8) ?
+		__ioat3_prep_pq16_lock(chan, result, pq, &src[1], src_cnt - 1,
+				       scf, len, flags) :
+		__ioat3_prep_pq_lock(chan, result, pq, &src[1], src_cnt - 1,
+				     scf, len, flags);
 }
 
 static struct dma_async_tx_descriptor *
@@ -1167,6 +1732,56 @@ static int ioat3_dma_self_test(struct ioatdma_device *device)
 	return 0;
 }
 
+static int ioat3_irq_reinit(struct ioatdma_device *device)
+{
+	int msixcnt = device->common.chancnt;
+	struct pci_dev *pdev = device->pdev;
+	int i;
+	struct msix_entry *msix;
+	struct ioat_chan_common *chan;
+	int err = 0;
+
+	switch (device->irq_mode) {
+	case IOAT_MSIX:
+
+		for (i = 0; i < msixcnt; i++) {
+			msix = &device->msix_entries[i];
+			chan = ioat_chan_by_index(device, i);
+			devm_free_irq(&pdev->dev, msix->vector, chan);
+		}
+
+		pci_disable_msix(pdev);
+		break;
+
+	case IOAT_MSIX_SINGLE:
+		msix = &device->msix_entries[0];
+		chan = ioat_chan_by_index(device, 0);
+		devm_free_irq(&pdev->dev, msix->vector, chan);
+		pci_disable_msix(pdev);
+		break;
+
+	case IOAT_MSI:
+		chan = ioat_chan_by_index(device, 0);
+		devm_free_irq(&pdev->dev, pdev->irq, chan);
+		pci_disable_msi(pdev);
+		break;
+
+	case IOAT_INTX:
+		chan = ioat_chan_by_index(device, 0);
+		devm_free_irq(&pdev->dev, pdev->irq, chan);
+		break;
+
+	default:
+		return 0;
+	}
+
+	device->irq_mode = IOAT_NOIRQ;
+
+	err = ioat_dma_setup_interrupts(device);
+
+	return err;
+}
+
 static int ioat3_reset_hw(struct ioat_chan_common *chan)
 {
 	/* throw away whatever the channel was doing and get it
@@ -1183,80 +1798,65 @@ static int ioat3_reset_hw(struct ioat_chan_common *chan)
 	chanerr = readl(chan->reg_base + IOAT_CHANERR_OFFSET);
 	writel(chanerr, chan->reg_base + IOAT_CHANERR_OFFSET);
 
-	/* clear any pending errors */
-	err = pci_read_config_dword(pdev, IOAT_PCI_CHANERR_INT_OFFSET, &chanerr);
+	if (device->version < IOAT_VER_3_3) {
+		/* clear any pending errors */
+		err = pci_read_config_dword(pdev,
+				IOAT_PCI_CHANERR_INT_OFFSET, &chanerr);
+		if (err) {
+			dev_err(&pdev->dev,
+				"channel error register unreachable\n");
+			return err;
+		}
+		pci_write_config_dword(pdev,
+				IOAT_PCI_CHANERR_INT_OFFSET, chanerr);
+
+		/* Clear DMAUNCERRSTS Cfg-Reg Parity Error status bit
+		 * (workaround for spurious config parity error after restart)
+		 */
+		pci_read_config_word(pdev, IOAT_PCI_DEVICE_ID_OFFSET, &dev_id);
+		if (dev_id == PCI_DEVICE_ID_INTEL_IOAT_TBG0) {
+			pci_write_config_dword(pdev,
+					       IOAT_PCI_DMAUNCERRSTS_OFFSET,
+					       0x10);
+		}
+	}
+
+	err = ioat2_reset_sync(chan, msecs_to_jiffies(200));
 	if (err) {
-		dev_err(&pdev->dev, "channel error register unreachable\n");
+		dev_err(&pdev->dev, "Failed to reset!\n");
 		return err;
 	}
-	pci_write_config_dword(pdev, IOAT_PCI_CHANERR_INT_OFFSET, chanerr);
 
-	/* Clear DMAUNCERRSTS Cfg-Reg Parity Error status bit
-	 * (workaround for spurious config parity error after restart)
-	 */
-	pci_read_config_word(pdev, IOAT_PCI_DEVICE_ID_OFFSET, &dev_id);
-	if (dev_id == PCI_DEVICE_ID_INTEL_IOAT_TBG0)
-		pci_write_config_dword(pdev, IOAT_PCI_DMAUNCERRSTS_OFFSET, 0x10);
+	if (device->irq_mode != IOAT_NOIRQ && is_bwd_ioat(pdev))
+		err = ioat3_irq_reinit(device);
 
-	return ioat2_reset_sync(chan, msecs_to_jiffies(200));
+	return err;
 }
 
-static bool is_jf_ioat(struct pci_dev *pdev)
+static void ioat3_intr_quirk(struct ioatdma_device *device)
 {
-	switch (pdev->device) {
-	case PCI_DEVICE_ID_INTEL_IOAT_JSF0:
-	case PCI_DEVICE_ID_INTEL_IOAT_JSF1:
-	case PCI_DEVICE_ID_INTEL_IOAT_JSF2:
-	case PCI_DEVICE_ID_INTEL_IOAT_JSF3:
-	case PCI_DEVICE_ID_INTEL_IOAT_JSF4:
-	case PCI_DEVICE_ID_INTEL_IOAT_JSF5:
-	case PCI_DEVICE_ID_INTEL_IOAT_JSF6:
-	case PCI_DEVICE_ID_INTEL_IOAT_JSF7:
-	case PCI_DEVICE_ID_INTEL_IOAT_JSF8:
-	case PCI_DEVICE_ID_INTEL_IOAT_JSF9:
-		return true;
-	default:
-		return false;
-	}
-}
+	struct dma_device *dma;
+	struct dma_chan *c;
+	struct ioat_chan_common *chan;
+	u32 errmask;
 
-static bool is_snb_ioat(struct pci_dev *pdev)
-{
-	switch (pdev->device) {
-	case PCI_DEVICE_ID_INTEL_IOAT_SNB0:
-	case PCI_DEVICE_ID_INTEL_IOAT_SNB1:
-	case PCI_DEVICE_ID_INTEL_IOAT_SNB2:
-	case PCI_DEVICE_ID_INTEL_IOAT_SNB3:
-	case PCI_DEVICE_ID_INTEL_IOAT_SNB4:
-	case PCI_DEVICE_ID_INTEL_IOAT_SNB5:
-	case PCI_DEVICE_ID_INTEL_IOAT_SNB6:
-	case PCI_DEVICE_ID_INTEL_IOAT_SNB7:
-	case PCI_DEVICE_ID_INTEL_IOAT_SNB8:
-	case PCI_DEVICE_ID_INTEL_IOAT_SNB9:
-		return true;
-	default:
-		return false;
-	}
-}
+	dma = &device->common;
 
-static bool is_ivb_ioat(struct pci_dev *pdev)
-{
-	switch (pdev->device) {
-	case PCI_DEVICE_ID_INTEL_IOAT_IVB0:
-	case PCI_DEVICE_ID_INTEL_IOAT_IVB1:
-	case PCI_DEVICE_ID_INTEL_IOAT_IVB2:
-	case PCI_DEVICE_ID_INTEL_IOAT_IVB3:
-	case PCI_DEVICE_ID_INTEL_IOAT_IVB4:
-	case PCI_DEVICE_ID_INTEL_IOAT_IVB5:
-	case PCI_DEVICE_ID_INTEL_IOAT_IVB6:
-	case PCI_DEVICE_ID_INTEL_IOAT_IVB7:
-	case PCI_DEVICE_ID_INTEL_IOAT_IVB8:
-	case PCI_DEVICE_ID_INTEL_IOAT_IVB9:
-		return true;
-	default:
-		return false;
+	/*
+	 * if we have descriptor write back error status, we mask the
+	 * error interrupts
+	 */
+	if (device->cap & IOAT_CAP_DWBES) {
+		list_for_each_entry(c, &dma->channels, device_node) {
+			chan = to_chan_common(c);
+			errmask = readl(chan->reg_base +
+					IOAT_CHANERR_MASK_OFFSET);
+			errmask |= IOAT_CHANERR_XOR_P_OR_CRC_ERR |
+				   IOAT_CHANERR_XOR_Q_ERR;
+			writel(errmask, chan->reg_base +
+					IOAT_CHANERR_MASK_OFFSET);
+		}
 	}
-
 }
 
 int ioat3_dma_probe(struct ioatdma_device *device, int dca)
@@ -1268,30 +1868,33 @@ int ioat3_dma_probe(struct ioatdma_device *device, int dca)
 	struct ioat_chan_common *chan;
 	bool is_raid_device = false;
 	int err;
-	u32 cap;
 
 	device->enumerate_channels = ioat2_enumerate_channels;
 	device->reset_hw = ioat3_reset_hw;
 	device->self_test = ioat3_dma_self_test;
+	device->intr_quirk = ioat3_intr_quirk;
 	dma = &device->common;
 	dma->device_prep_dma_memcpy = ioat2_dma_prep_memcpy_lock;
 	dma->device_issue_pending = ioat2_issue_pending;
 	dma->device_alloc_chan_resources = ioat2_alloc_chan_resources;
 	dma->device_free_chan_resources = ioat2_free_chan_resources;
 
-	if (is_jf_ioat(pdev) || is_snb_ioat(pdev) || is_ivb_ioat(pdev))
+	if (is_xeon_cb32(pdev))
 		dma->copy_align = 6;
 
 	dma_cap_set(DMA_INTERRUPT, dma->cap_mask);
 	dma->device_prep_dma_interrupt = ioat3_prep_interrupt_lock;
 
-	cap = readl(device->reg_base + IOAT_DMA_CAP_OFFSET);
+	device->cap = readl(device->reg_base + IOAT_DMA_CAP_OFFSET);
+
+	if (is_bwd_noraid(pdev))
+		device->cap &= ~(IOAT_CAP_XOR | IOAT_CAP_PQ | IOAT_CAP_RAID16SS);
 
 	/* dca is incompatible with raid operations */
-	if (dca_en && (cap & (IOAT_CAP_XOR|IOAT_CAP_PQ)))
-		cap &= ~(IOAT_CAP_XOR|IOAT_CAP_PQ);
+	if (dca_en && (device->cap & (IOAT_CAP_XOR|IOAT_CAP_PQ)))
+		device->cap &= ~(IOAT_CAP_XOR|IOAT_CAP_PQ);
 
-	if (cap & IOAT_CAP_XOR) {
+	if (device->cap & IOAT_CAP_XOR) {
 		is_raid_device = true;
 		dma->max_xor = 8;
 		dma->xor_align = 6;
@@ -1302,53 +1905,86 @@ int ioat3_dma_probe(struct ioatdma_device *device, int dca)
 		dma_cap_set(DMA_XOR_VAL, dma->cap_mask);
 		dma->device_prep_dma_xor_val = ioat3_prep_xor_val;
 	}
-	if (cap & IOAT_CAP_PQ) {
+
+	if (device->cap & IOAT_CAP_PQ) {
 		is_raid_device = true;
-		dma_set_maxpq(dma, 8, 0);
-		dma->pq_align = 6;
 
-		dma_cap_set(DMA_PQ, dma->cap_mask);
 		dma->device_prep_dma_pq = ioat3_prep_pq;
-
-		dma_cap_set(DMA_PQ_VAL, dma->cap_mask);
 		dma->device_prep_dma_pq_val = ioat3_prep_pq_val;
+		dma_cap_set(DMA_PQ, dma->cap_mask);
+		dma_cap_set(DMA_PQ_VAL, dma->cap_mask);
 
-		if (!(cap & IOAT_CAP_XOR)) {
-			dma->max_xor = 8;
-			dma->xor_align = 6;
+		if (device->cap & IOAT_CAP_RAID16SS) {
+			dma_set_maxpq(dma, 16, 0);
+			dma->pq_align = 0;
+		} else {
+			dma_set_maxpq(dma, 8, 0);
+			if (is_xeon_cb32(pdev))
+				dma->pq_align = 6;
+			else
+				dma->pq_align = 0;
+		}
 
-			dma_cap_set(DMA_XOR, dma->cap_mask);
+		if (!(device->cap & IOAT_CAP_XOR)) {
 			dma->device_prep_dma_xor = ioat3_prep_pqxor;
-
-			dma_cap_set(DMA_XOR_VAL, dma->cap_mask);
 			dma->device_prep_dma_xor_val = ioat3_prep_pqxor_val;
+			dma_cap_set(DMA_XOR, dma->cap_mask);
+			dma_cap_set(DMA_XOR_VAL, dma->cap_mask);
+
+			if (device->cap & IOAT_CAP_RAID16SS) {
+				dma->max_xor = 16;
+				dma->xor_align = 0;
+			} else {
+				dma->max_xor = 8;
+				if (is_xeon_cb32(pdev))
+					dma->xor_align = 6;
+				else
+					dma->xor_align = 0;
+			}
 		}
 	}
-	if (is_raid_device && (cap & IOAT_CAP_FILL_BLOCK)) {
+
+	if (is_raid_device && (device->cap & IOAT_CAP_FILL_BLOCK)) {
 		dma_cap_set(DMA_MEMSET, dma->cap_mask);
 		dma->device_prep_dma_memset = ioat3_prep_memset_lock;
 	}
 
 
-	if (is_raid_device) {
-		dma->device_tx_status = ioat3_tx_status;
-		device->cleanup_fn = ioat3_cleanup_event;
-		device->timer_fn = ioat3_timer_event;
-	} else {
-		dma->device_tx_status = ioat_dma_tx_status;
-		device->cleanup_fn = ioat2_cleanup_event;
-		device->timer_fn = ioat2_timer_event;
+	dma->device_tx_status = ioat3_tx_status;
+	device->cleanup_fn = ioat3_cleanup_event;
+	device->timer_fn = ioat3_timer_event;
+
+	if (is_xeon_cb32(pdev)) {
+		dma_cap_clear(DMA_XOR_VAL, dma->cap_mask);
+		dma->device_prep_dma_xor_val = NULL;
+
+		dma_cap_clear(DMA_PQ_VAL, dma->cap_mask);
+		dma->device_prep_dma_pq_val = NULL;
 	}
 
-	#ifdef CONFIG_ASYNC_TX_DISABLE_PQ_VAL_DMA
-	dma_cap_clear(DMA_PQ_VAL, dma->cap_mask);
-	dma->device_prep_dma_pq_val = NULL;
-	#endif
+	/* starting with CB3.3 super extended descriptors are supported */
+	if (device->cap & IOAT_CAP_RAID16SS) {
+		char pool_name[14];
+		int i;
+
+		/* allocate sw descriptor pool for SED */
+		device->sed_pool = kmem_cache_create("ioat_sed",
+				sizeof(struct ioat_sed_ent), 0, 0, NULL);
+		if (!device->sed_pool)
+			return -ENOMEM;
+
+		for (i = 0; i < MAX_SED_POOLS; i++) {
+			snprintf(pool_name, 14, "ioat_hw%d_sed", i);
 
-	#ifdef CONFIG_ASYNC_TX_DISABLE_XOR_VAL_DMA
-	dma_cap_clear(DMA_XOR_VAL, dma->cap_mask);
-	dma->device_prep_dma_xor_val = NULL;
-	#endif
+			/* allocate SED DMA pool */
+			device->sed_hw_pool[i] = dma_pool_create(pool_name,
+					&pdev->dev,
+					SED_SIZE * (i + 1), 64, 0);
+			if (!device->sed_hw_pool[i])
+				goto sed_pool_cleanup;
+
+		}
+	}
 
 	err = ioat_probe(device);
 	if (err)
@@ -1371,4 +2007,28 @@ int ioat3_dma_probe(struct ioatdma_device *device, int dca)
 		device->dca = ioat3_dca_init(pdev, device->reg_base);
 
 	return 0;
+
+sed_pool_cleanup:
+	if (device->sed_pool) {
+		int i;
+		kmem_cache_destroy(device->sed_pool);
+
+		for (i = 0; i < MAX_SED_POOLS; i++)
+			if (device->sed_hw_pool[i])
+				dma_pool_destroy(device->sed_hw_pool[i]);
+	}
+
+	return -ENOMEM;
+}
+
+void ioat3_dma_remove(struct ioatdma_device *device)
+{
+	if (device->sed_pool) {
+		int i;
+		kmem_cache_destroy(device->sed_pool);
+
+		for (i = 0; i < MAX_SED_POOLS; i++)
+			if (device->sed_hw_pool[i])
+				dma_pool_destroy(device->sed_hw_pool[i]);
+	}
 }
diff --git a/drivers/dma/ioat/hw.h b/drivers/dma/ioat/hw.h
index 7cb74c62c719..5ee57d402a6e 100644
--- a/drivers/dma/ioat/hw.h
+++ b/drivers/dma/ioat/hw.h
@@ -30,11 +30,6 @@
 #define IOAT_PCI_DID_SCNB       0x65FF
 #define IOAT_PCI_DID_SNB        0x402F
 
-#define IOAT_VER_1_2            0x12    /* Version 1.2 */
-#define IOAT_VER_2_0            0x20    /* Version 2.0 */
-#define IOAT_VER_3_0            0x30    /* Version 3.0 */
-#define IOAT_VER_3_2            0x32    /* Version 3.2 */
-
 #define PCI_DEVICE_ID_INTEL_IOAT_IVB0	0x0e20
 #define PCI_DEVICE_ID_INTEL_IOAT_IVB1	0x0e21
 #define PCI_DEVICE_ID_INTEL_IOAT_IVB2	0x0e22
@@ -46,6 +41,29 @@
 #define PCI_DEVICE_ID_INTEL_IOAT_IVB8	0x0e2e
 #define PCI_DEVICE_ID_INTEL_IOAT_IVB9	0x0e2f
 
+#define PCI_DEVICE_ID_INTEL_IOAT_HSW0	0x2f20
+#define PCI_DEVICE_ID_INTEL_IOAT_HSW1	0x2f21
+#define PCI_DEVICE_ID_INTEL_IOAT_HSW2	0x2f22
+#define PCI_DEVICE_ID_INTEL_IOAT_HSW3	0x2f23
+#define PCI_DEVICE_ID_INTEL_IOAT_HSW4	0x2f24
+#define PCI_DEVICE_ID_INTEL_IOAT_HSW5	0x2f25
+#define PCI_DEVICE_ID_INTEL_IOAT_HSW6	0x2f26
+#define PCI_DEVICE_ID_INTEL_IOAT_HSW7	0x2f27
+#define PCI_DEVICE_ID_INTEL_IOAT_HSW8	0x2f2e
+#define PCI_DEVICE_ID_INTEL_IOAT_HSW9	0x2f2f
+
+#define PCI_DEVICE_ID_INTEL_IOAT_BWD0	0x0C50
+#define PCI_DEVICE_ID_INTEL_IOAT_BWD1	0x0C51
+#define PCI_DEVICE_ID_INTEL_IOAT_BWD2	0x0C52
+#define PCI_DEVICE_ID_INTEL_IOAT_BWD3	0x0C53
+
+#define IOAT_VER_1_2            0x12    /* Version 1.2 */
+#define IOAT_VER_2_0            0x20    /* Version 2.0 */
+#define IOAT_VER_3_0            0x30    /* Version 3.0 */
+#define IOAT_VER_3_2            0x32    /* Version 3.2 */
+#define IOAT_VER_3_3            0x33    /* Version 3.3 */
+
+
 int system_has_dca_enabled(struct pci_dev *pdev);
 
 struct ioat_dma_descriptor {
@@ -147,7 +165,17 @@ struct ioat_xor_ext_descriptor {
 };
 
 struct ioat_pq_descriptor {
-	uint32_t	size;
+	union {
+		uint32_t	size;
+		uint32_t	dwbes;
+		struct {
+			unsigned int rsvd:25;
+			unsigned int p_val_err:1;
+			unsigned int q_val_err:1;
+			unsigned int rsvd1:4;
+			unsigned int wbes:1;
+		} dwbes_f;
+	};
 	union {
 		uint32_t ctl;
 		struct {
@@ -162,9 +190,14 @@ struct ioat_pq_descriptor {
 			unsigned int hint:1;
 			unsigned int p_disable:1;
 			unsigned int q_disable:1;
-			unsigned int rsvd:11;
+			unsigned int rsvd2:2;
+			unsigned int wb_en:1;
+			unsigned int prl_en:1;
+			unsigned int rsvd3:7;
 			#define IOAT_OP_PQ 0x89
 			#define IOAT_OP_PQ_VAL 0x8a
+			#define IOAT_OP_PQ_16S 0xa0
+			#define IOAT_OP_PQ_VAL_16S 0xa1
 			unsigned int op:8;
 		} ctl_f;
 	};
@@ -172,7 +205,10 @@ struct ioat_pq_descriptor {
 	uint64_t	p_addr;
 	uint64_t	next;
 	uint64_t	src_addr2;
-	uint64_t	src_addr3;
+	union {
+		uint64_t	src_addr3;
+		uint64_t	sed_addr;
+	};
 	uint8_t		coef[8];
 	uint64_t	q_addr;
 };
@@ -221,4 +257,40 @@ struct ioat_pq_update_descriptor {
 struct ioat_raw_descriptor {
 	uint64_t	field[8];
 };
+
+struct ioat_pq16a_descriptor {
+	uint8_t coef[8];
+	uint64_t src_addr3;
+	uint64_t src_addr4;
+	uint64_t src_addr5;
+	uint64_t src_addr6;
+	uint64_t src_addr7;
+	uint64_t src_addr8;
+	uint64_t src_addr9;
+};
+
+struct ioat_pq16b_descriptor {
+	uint64_t src_addr10;
+	uint64_t src_addr11;
+	uint64_t src_addr12;
+	uint64_t src_addr13;
+	uint64_t src_addr14;
+	uint64_t src_addr15;
+	uint64_t src_addr16;
+	uint64_t rsvd;
+};
+
+union ioat_sed_pq_descriptor {
+	struct ioat_pq16a_descriptor a;
+	struct ioat_pq16b_descriptor b;
+};
+
+#define SED_SIZE	64
+
+struct ioat_sed_raw_descriptor {
+	uint64_t	a[8];
+	uint64_t	b[8];
+	uint64_t	c[8];
+};
+
 #endif
diff --git a/drivers/dma/ioat/pci.c b/drivers/dma/ioat/pci.c
index 71c7ecd80fac..2c8d560e6334 100644
--- a/drivers/dma/ioat/pci.c
+++ b/drivers/dma/ioat/pci.c
@@ -94,6 +94,23 @@ static struct pci_device_id ioat_pci_tbl[] = {
 	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB8) },
 	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB9) },
 
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_HSW0) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_HSW1) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_HSW2) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_HSW3) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_HSW4) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_HSW5) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_HSW6) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_HSW7) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_HSW8) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_HSW9) },
+
+	/* I/OAT v3.3 platforms */
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_BWD0) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_BWD1) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_BWD2) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_BWD3) },
+
 	{ 0, }
 };
 MODULE_DEVICE_TABLE(pci, ioat_pci_tbl);
@@ -190,6 +207,9 @@ static void ioat_remove(struct pci_dev *pdev)
 	if (!device)
 		return;
 
+	if (device->version >= IOAT_VER_3_0)
+		ioat3_dma_remove(device);
+
 	dev_err(&pdev->dev, "Removing dma and dca services\n");
 	if (device->dca) {
 		unregister_dca_provider(device->dca, &pdev->dev);
diff --git a/drivers/dma/ioat/registers.h b/drivers/dma/ioat/registers.h
index 1391798542b6..2f1cfa0f1f47 100644
--- a/drivers/dma/ioat/registers.h
+++ b/drivers/dma/ioat/registers.h
@@ -79,6 +79,8 @@
 #define IOAT_CAP_APIC				0x00000080
 #define IOAT_CAP_XOR				0x00000100
 #define IOAT_CAP_PQ				0x00000200
+#define IOAT_CAP_DWBES				0x00002000
+#define IOAT_CAP_RAID16SS			0x00020000
 
 #define IOAT_CHANNEL_MMIO_SIZE			0x80	/* Each Channel MMIO space is this size */
 
@@ -93,6 +95,8 @@
 #define IOAT_CHANCTRL_ERR_COMPLETION_EN		0x0004
 #define IOAT_CHANCTRL_INT_REARM			0x0001
 #define IOAT_CHANCTRL_RUN			(IOAT_CHANCTRL_INT_REARM |\
+						 IOAT_CHANCTRL_ERR_INT_EN |\
+						 IOAT_CHANCTRL_ERR_COMPLETION_EN |\
 						 IOAT_CHANCTRL_ANY_ERR_ABORT_EN)
 
 #define IOAT_DMA_COMP_OFFSET			0x02	/* 16-bit DMA channel compatibility */
diff --git a/drivers/dma/ipu/ipu_idmac.c b/drivers/dma/ipu/ipu_idmac.c
index 8c61d17a86bf..d39c2cd0795d 100644
--- a/drivers/dma/ipu/ipu_idmac.c
+++ b/drivers/dma/ipu/ipu_idmac.c
@@ -1642,7 +1642,7 @@ static int __init ipu_idmac_init(struct ipu *ipu)
 	return dma_async_device_register(&idmac->dma);
 }
 
-static void __exit ipu_idmac_exit(struct ipu *ipu)
+static void ipu_idmac_exit(struct ipu *ipu)
 {
 	int i;
 	struct idmac *idmac = &ipu->idmac;
@@ -1756,7 +1756,7 @@ err_noirq:
 	return ret;
 }
 
-static int __exit ipu_remove(struct platform_device *pdev)
+static int ipu_remove(struct platform_device *pdev)
 {
 	struct ipu *ipu = platform_get_drvdata(pdev);
 
@@ -1781,7 +1781,7 @@ static struct platform_driver ipu_platform_driver = {
 		.name	= "ipu-core",
 		.owner	= THIS_MODULE,
 	},
-	.remove		= __exit_p(ipu_remove),
+	.remove		= ipu_remove,
 };
 
 static int __init ipu_init(void)
diff --git a/drivers/dma/mxs-dma.c b/drivers/dma/mxs-dma.c
index 8f6d30d37c45..b48a79c28845 100644
--- a/drivers/dma/mxs-dma.c
+++ b/drivers/dma/mxs-dma.c
@@ -27,6 +27,7 @@
 #include <linux/stmp_device.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
+#include <linux/of_dma.h>
 
 #include <asm/irq.h>
 
@@ -139,6 +140,8 @@ struct mxs_dma_engine {
 	struct dma_device		dma_device;
 	struct device_dma_parameters	dma_parms;
 	struct mxs_dma_chan		mxs_chans[MXS_DMA_CHANNELS];
+	struct platform_device		*pdev;
+	unsigned int			nr_channels;
 };
 
 struct mxs_dma_type {
@@ -350,10 +353,8 @@ static int mxs_dma_alloc_chan_resources(struct dma_chan *chan)
 	struct mxs_dma_engine *mxs_dma = mxs_chan->mxs_dma;
 	int ret;
 
-	if (!data)
-		return -EINVAL;
-
-	mxs_chan->chan_irq = data->chan_irq;
+	if (data)
+		mxs_chan->chan_irq = data->chan_irq;
 
 	mxs_chan->ccw = dma_alloc_coherent(mxs_dma->dma_device.dev,
 				CCW_BLOCK_SIZE, &mxs_chan->ccw_phys,
@@ -665,8 +666,55 @@ err_out:
 	return ret;
 }
 
+struct mxs_dma_filter_param {
+	struct device_node *of_node;
+	unsigned int chan_id;
+};
+
+static bool mxs_dma_filter_fn(struct dma_chan *chan, void *fn_param)
+{
+	struct mxs_dma_filter_param *param = fn_param;
+	struct mxs_dma_chan *mxs_chan = to_mxs_dma_chan(chan);
+	struct mxs_dma_engine *mxs_dma = mxs_chan->mxs_dma;
+	int chan_irq;
+
+	if (mxs_dma->dma_device.dev->of_node != param->of_node)
+		return false;
+
+	if (chan->chan_id != param->chan_id)
+		return false;
+
+	chan_irq = platform_get_irq(mxs_dma->pdev, param->chan_id);
+	if (chan_irq < 0)
+		return false;
+
+	mxs_chan->chan_irq = chan_irq;
+
+	return true;
+}
+
+struct dma_chan *mxs_dma_xlate(struct of_phandle_args *dma_spec,
+			       struct of_dma *ofdma)
+{
+	struct mxs_dma_engine *mxs_dma = ofdma->of_dma_data;
+	dma_cap_mask_t mask = mxs_dma->dma_device.cap_mask;
+	struct mxs_dma_filter_param param;
+
+	if (dma_spec->args_count != 1)
+		return NULL;
+
+	param.of_node = ofdma->of_node;
+	param.chan_id = dma_spec->args[0];
+
+	if (param.chan_id >= mxs_dma->nr_channels)
+		return NULL;
+
+	return dma_request_channel(mask, mxs_dma_filter_fn, &param);
+}
+
 static int __init mxs_dma_probe(struct platform_device *pdev)
 {
+	struct device_node *np = pdev->dev.of_node;
 	const struct platform_device_id *id_entry;
 	const struct of_device_id *of_id;
 	const struct mxs_dma_type *dma_type;
@@ -674,10 +722,16 @@ static int __init mxs_dma_probe(struct platform_device *pdev)
 	struct resource *iores;
 	int ret, i;
 
-	mxs_dma = kzalloc(sizeof(*mxs_dma), GFP_KERNEL);
+	mxs_dma = devm_kzalloc(&pdev->dev, sizeof(*mxs_dma), GFP_KERNEL);
 	if (!mxs_dma)
 		return -ENOMEM;
 
+	ret = of_property_read_u32(np, "dma-channels", &mxs_dma->nr_channels);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to read dma-channels\n");
+		return ret;
+	}
+
 	of_id = of_match_device(mxs_dma_dt_ids, &pdev->dev);
 	if (of_id)
 		id_entry = of_id->data;
@@ -689,24 +743,13 @@ static int __init mxs_dma_probe(struct platform_device *pdev)
 	mxs_dma->dev_id = dma_type->id;
 
 	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mxs_dma->base = devm_ioremap_resource(&pdev->dev, iores);
+	if (IS_ERR(mxs_dma->base))
+		return PTR_ERR(mxs_dma->base);
 
-	if (!request_mem_region(iores->start, resource_size(iores),
-				pdev->name)) {
-		ret = -EBUSY;
-		goto err_request_region;
-	}
-
-	mxs_dma->base = ioremap(iores->start, resource_size(iores));
-	if (!mxs_dma->base) {
-		ret = -ENOMEM;
-		goto err_ioremap;
-	}
-
-	mxs_dma->clk = clk_get(&pdev->dev, NULL);
-	if (IS_ERR(mxs_dma->clk)) {
-		ret = PTR_ERR(mxs_dma->clk);
-		goto err_clk;
-	}
+	mxs_dma->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(mxs_dma->clk))
+		return PTR_ERR(mxs_dma->clk);
 
 	dma_cap_set(DMA_SLAVE, mxs_dma->dma_device.cap_mask);
 	dma_cap_set(DMA_CYCLIC, mxs_dma->dma_device.cap_mask);
@@ -732,8 +775,9 @@ static int __init mxs_dma_probe(struct platform_device *pdev)
 
 	ret = mxs_dma_init(mxs_dma);
 	if (ret)
-		goto err_init;
+		return ret;
 
+	mxs_dma->pdev = pdev;
 	mxs_dma->dma_device.dev = &pdev->dev;
 
 	/* mxs_dma gets 65535 bytes maximum sg size */
@@ -751,22 +795,19 @@ static int __init mxs_dma_probe(struct platform_device *pdev)
 	ret = dma_async_device_register(&mxs_dma->dma_device);
 	if (ret) {
 		dev_err(mxs_dma->dma_device.dev, "unable to register\n");
-		goto err_init;
+		return ret;
+	}
+
+	ret = of_dma_controller_register(np, mxs_dma_xlate, mxs_dma);
+	if (ret) {
+		dev_err(mxs_dma->dma_device.dev,
+			"failed to register controller\n");
+		dma_async_device_unregister(&mxs_dma->dma_device);
 	}
 
 	dev_info(mxs_dma->dma_device.dev, "initialized\n");
 
 	return 0;
-
-err_init:
-	clk_put(mxs_dma->clk);
-err_clk:
-	iounmap(mxs_dma->base);
-err_ioremap:
-	release_mem_region(iores->start, resource_size(iores));
-err_request_region:
-	kfree(mxs_dma);
-	return ret;
 }
 
 static struct platform_driver mxs_dma_driver = {
diff --git a/drivers/dma/of-dma.c b/drivers/dma/of-dma.c
index 69d04d28b1ef..7aa0864cd487 100644
--- a/drivers/dma/of-dma.c
+++ b/drivers/dma/of-dma.c
@@ -13,43 +13,31 @@
 #include <linux/device.h>
 #include <linux/err.h>
 #include <linux/module.h>
-#include <linux/rculist.h>
+#include <linux/mutex.h>
 #include <linux/slab.h>
 #include <linux/of.h>
 #include <linux/of_dma.h>
 
 static LIST_HEAD(of_dma_list);
-static DEFINE_SPINLOCK(of_dma_lock);
+static DEFINE_MUTEX(of_dma_lock);
 
 /**
- * of_dma_get_controller - Get a DMA controller in DT DMA helpers list
+ * of_dma_find_controller - Get a DMA controller in DT DMA helpers list
  * @dma_spec:	pointer to DMA specifier as found in the device tree
  *
  * Finds a DMA controller with matching device node and number for dma cells
- * in a list of registered DMA controllers. If a match is found the use_count
- * variable is increased and a valid pointer to the DMA data stored is retuned.
- * A NULL pointer is returned if no match is found.
+ * in a list of registered DMA controllers. If a match is found a valid pointer
+ * to the DMA data stored is retuned. A NULL pointer is returned if no match is
+ * found.
  */
-static struct of_dma *of_dma_get_controller(struct of_phandle_args *dma_spec)
+static struct of_dma *of_dma_find_controller(struct of_phandle_args *dma_spec)
 {
 	struct of_dma *ofdma;
 
-	spin_lock(&of_dma_lock);
-
-	if (list_empty(&of_dma_list)) {
-		spin_unlock(&of_dma_lock);
-		return NULL;
-	}
-
 	list_for_each_entry(ofdma, &of_dma_list, of_dma_controllers)
 		if ((ofdma->of_node == dma_spec->np) &&
-		    (ofdma->of_dma_nbcells == dma_spec->args_count)) {
-			ofdma->use_count++;
-			spin_unlock(&of_dma_lock);
+		    (ofdma->of_dma_nbcells == dma_spec->args_count))
 			return ofdma;
-		}
-
-	spin_unlock(&of_dma_lock);
 
 	pr_debug("%s: can't find DMA controller %s\n", __func__,
 		 dma_spec->np->full_name);
@@ -58,22 +46,6 @@ static struct of_dma *of_dma_get_controller(struct of_phandle_args *dma_spec)
 }
 
 /**
- * of_dma_put_controller - Decrement use count for a registered DMA controller
- * @of_dma:	pointer to DMA controller data
- *
- * Decrements the use_count variable in the DMA data structure. This function
- * should be called only when a valid pointer is returned from
- * of_dma_get_controller() and no further accesses to data referenced by that
- * pointer are needed.
- */
-static void of_dma_put_controller(struct of_dma *ofdma)
-{
-	spin_lock(&of_dma_lock);
-	ofdma->use_count--;
-	spin_unlock(&of_dma_lock);
-}
-
-/**
  * of_dma_controller_register - Register a DMA controller to DT DMA helpers
  * @np:			device node of DMA controller
  * @of_dma_xlate:	translation function which converts a phandle
@@ -93,6 +65,7 @@ int of_dma_controller_register(struct device_node *np,
 {
 	struct of_dma	*ofdma;
 	int		nbcells;
+	const __be32	*prop;
 
 	if (!np || !of_dma_xlate) {
 		pr_err("%s: not enough information provided\n", __func__);
@@ -103,8 +76,11 @@ int of_dma_controller_register(struct device_node *np,
 	if (!ofdma)
 		return -ENOMEM;
 
-	nbcells = be32_to_cpup(of_get_property(np, "#dma-cells", NULL));
-	if (!nbcells) {
+	prop = of_get_property(np, "#dma-cells", NULL);
+	if (prop)
+		nbcells = be32_to_cpup(prop);
+
+	if (!prop || !nbcells) {
 		pr_err("%s: #dma-cells property is missing or invalid\n",
 		       __func__);
 		kfree(ofdma);
@@ -115,12 +91,11 @@ int of_dma_controller_register(struct device_node *np,
 	ofdma->of_dma_nbcells = nbcells;
 	ofdma->of_dma_xlate = of_dma_xlate;
 	ofdma->of_dma_data = data;
-	ofdma->use_count = 0;
 
 	/* Now queue of_dma controller structure in list */
-	spin_lock(&of_dma_lock);
+	mutex_lock(&of_dma_lock);
 	list_add_tail(&ofdma->of_dma_controllers, &of_dma_list);
-	spin_unlock(&of_dma_lock);
+	mutex_unlock(&of_dma_lock);
 
 	return 0;
 }
@@ -132,32 +107,20 @@ EXPORT_SYMBOL_GPL(of_dma_controller_register);
  *
  * Memory allocated by of_dma_controller_register() is freed here.
  */
-int of_dma_controller_free(struct device_node *np)
+void of_dma_controller_free(struct device_node *np)
 {
 	struct of_dma *ofdma;
 
-	spin_lock(&of_dma_lock);
-
-	if (list_empty(&of_dma_list)) {
-		spin_unlock(&of_dma_lock);
-		return -ENODEV;
-	}
+	mutex_lock(&of_dma_lock);
 
 	list_for_each_entry(ofdma, &of_dma_list, of_dma_controllers)
 		if (ofdma->of_node == np) {
-			if (ofdma->use_count) {
-				spin_unlock(&of_dma_lock);
-				return -EBUSY;
-			}
-
 			list_del(&ofdma->of_dma_controllers);
-			spin_unlock(&of_dma_lock);
 			kfree(ofdma);
-			return 0;
+			break;
 		}
 
-	spin_unlock(&of_dma_lock);
-	return -ENODEV;
+	mutex_unlock(&of_dma_lock);
 }
 EXPORT_SYMBOL_GPL(of_dma_controller_free);
 
@@ -172,8 +135,8 @@ EXPORT_SYMBOL_GPL(of_dma_controller_free);
  * specifiers, matches the name provided. Returns 0 if the name matches and
  * a valid pointer to the DMA specifier is found. Otherwise returns -ENODEV.
  */
-static int of_dma_match_channel(struct device_node *np, char *name, int index,
-				struct of_phandle_args *dma_spec)
+static int of_dma_match_channel(struct device_node *np, const char *name,
+				int index, struct of_phandle_args *dma_spec)
 {
 	const char *s;
 
@@ -198,7 +161,7 @@ static int of_dma_match_channel(struct device_node *np, char *name, int index,
  * Returns pointer to appropriate dma channel on success or NULL on error.
  */
 struct dma_chan *of_dma_request_slave_channel(struct device_node *np,
-					      char *name)
+					      const char *name)
 {
 	struct of_phandle_args	dma_spec;
 	struct of_dma		*ofdma;
@@ -220,14 +183,15 @@ struct dma_chan *of_dma_request_slave_channel(struct device_node *np,
 		if (of_dma_match_channel(np, name, i, &dma_spec))
 			continue;
 
-		ofdma = of_dma_get_controller(&dma_spec);
-
-		if (!ofdma)
-			continue;
+		mutex_lock(&of_dma_lock);
+		ofdma = of_dma_find_controller(&dma_spec);
 
-		chan = ofdma->of_dma_xlate(&dma_spec, ofdma);
+		if (ofdma)
+			chan = ofdma->of_dma_xlate(&dma_spec, ofdma);
+		else
+			chan = NULL;
 
-		of_dma_put_controller(ofdma);
+		mutex_unlock(&of_dma_lock);
 
 		of_node_put(dma_spec.np);
 
diff --git a/drivers/dma/omap-dma.c b/drivers/dma/omap-dma.c
index 08b43bf37158..ec3fc4fd9160 100644
--- a/drivers/dma/omap-dma.c
+++ b/drivers/dma/omap-dma.c
@@ -16,6 +16,8 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
+#include <linux/of_dma.h>
+#include <linux/of_device.h>
 
 #include "virt-dma.h"
 
@@ -67,6 +69,10 @@ static const unsigned es_bytes[] = {
 	[OMAP_DMA_DATA_TYPE_S32] = 4,
 };
 
+static struct of_dma_filter_info omap_dma_info = {
+	.filter_fn = omap_dma_filter_fn,
+};
+
 static inline struct omap_dmadev *to_omap_dma_dev(struct dma_device *d)
 {
 	return container_of(d, struct omap_dmadev, ddev);
@@ -629,8 +635,22 @@ static int omap_dma_probe(struct platform_device *pdev)
 		pr_warn("OMAP-DMA: failed to register slave DMA engine device: %d\n",
 			rc);
 		omap_dma_free(od);
-	} else {
-		platform_set_drvdata(pdev, od);
+		return rc;
+	}
+
+	platform_set_drvdata(pdev, od);
+
+	if (pdev->dev.of_node) {
+		omap_dma_info.dma_cap = od->ddev.cap_mask;
+
+		/* Device-tree DMA controller registration */
+		rc = of_dma_controller_register(pdev->dev.of_node,
+				of_dma_simple_xlate, &omap_dma_info);
+		if (rc) {
+			pr_warn("OMAP-DMA: failed to register DMA controller\n");
+			dma_async_device_unregister(&od->ddev);
+			omap_dma_free(od);
+		}
 	}
 
 	dev_info(&pdev->dev, "OMAP DMA engine driver\n");
@@ -642,18 +662,32 @@ static int omap_dma_remove(struct platform_device *pdev)
 {
 	struct omap_dmadev *od = platform_get_drvdata(pdev);
 
+	if (pdev->dev.of_node)
+		of_dma_controller_free(pdev->dev.of_node);
+
 	dma_async_device_unregister(&od->ddev);
 	omap_dma_free(od);
 
 	return 0;
 }
 
+static const struct of_device_id omap_dma_match[] = {
+	{ .compatible = "ti,omap2420-sdma", },
+	{ .compatible = "ti,omap2430-sdma", },
+	{ .compatible = "ti,omap3430-sdma", },
+	{ .compatible = "ti,omap3630-sdma", },
+	{ .compatible = "ti,omap4430-sdma", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, omap_dma_match);
+
 static struct platform_driver omap_dma_driver = {
 	.probe	= omap_dma_probe,
 	.remove	= omap_dma_remove,
 	.driver = {
 		.name = "omap-dma-engine",
 		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(omap_dma_match),
 	},
 };
 
diff --git a/drivers/dma/pch_dma.c b/drivers/dma/pch_dma.c
index d01faeb0f27c..ce3dc3e9688c 100644
--- a/drivers/dma/pch_dma.c
+++ b/drivers/dma/pch_dma.c
@@ -476,7 +476,7 @@ static struct pch_dma_desc *pdc_desc_get(struct pch_dma_chan *pd_chan)
 	dev_dbg(chan2dev(&pd_chan->chan), "scanned %d descriptors\n", i);
 
 	if (!ret) {
-		ret = pdc_alloc_desc(&pd_chan->chan, GFP_NOIO);
+		ret = pdc_alloc_desc(&pd_chan->chan, GFP_ATOMIC);
 		if (ret) {
 			spin_lock(&pd_chan->lock);
 			pd_chan->descs_allocated++;
diff --git a/drivers/dma/pl330.c b/drivers/dma/pl330.c
index 5dbc5946c4c3..a17553f7c028 100644
--- a/drivers/dma/pl330.c
+++ b/drivers/dma/pl330.c
@@ -26,6 +26,7 @@
 #include <linux/scatterlist.h>
 #include <linux/of.h>
 #include <linux/of_dma.h>
+#include <linux/err.h>
 
 #include "dmaengine.h"
 #define PL330_MAX_CHAN		8
@@ -2288,13 +2289,12 @@ static inline void fill_queue(struct dma_pl330_chan *pch)
 
 		/* If already submitted */
 		if (desc->status == BUSY)
-			break;
+			continue;
 
 		ret = pl330_submit_req(pch->pl330_chid,
 						&desc->req);
 		if (!ret) {
 			desc->status = BUSY;
-			break;
 		} else if (ret == -EAGAIN) {
 			/* QFull or DMAC Dying */
 			break;
@@ -2904,9 +2904,9 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 	pi->mcbufsz = pdat ? pdat->mcbuf_sz : 0;
 
 	res = &adev->res;
-	pi->base = devm_request_and_ioremap(&adev->dev, res);
-	if (!pi->base)
-		return -ENXIO;
+	pi->base = devm_ioremap_resource(&adev->dev, res);
+	if (IS_ERR(pi->base))
+		return PTR_ERR(pi->base);
 
 	amba_set_drvdata(adev, pdmac);
 
diff --git a/drivers/dma/sh/Kconfig b/drivers/dma/sh/Kconfig
new file mode 100644
index 000000000000..5c1dee20c13e
--- /dev/null
+++ b/drivers/dma/sh/Kconfig
@@ -0,0 +1,24 @@
+#
+# DMA engine configuration for sh
+#
+
+config SH_DMAE_BASE
+	bool "Renesas SuperH DMA Engine support"
+	depends on (SUPERH && SH_DMA) || (ARM && ARCH_SHMOBILE)
+	depends on !SH_DMA_API
+	default y
+	select DMA_ENGINE
+	help
+	  Enable support for the Renesas SuperH DMA controllers.
+
+config SH_DMAE
+	tristate "Renesas SuperH DMAC support"
+	depends on SH_DMAE_BASE
+	help
+	  Enable support for the Renesas SuperH DMA controllers.
+
+config SUDMAC
+	tristate "Renesas SUDMAC support"
+	depends on SH_DMAE_BASE
+	help
+	  Enable support for the Renesas SUDMAC controllers.
diff --git a/drivers/dma/sh/Makefile b/drivers/dma/sh/Makefile
index 54ae9572b0ac..c07ca4612e46 100644
--- a/drivers/dma/sh/Makefile
+++ b/drivers/dma/sh/Makefile
@@ -1,2 +1,3 @@
-obj-$(CONFIG_SH_DMAE) += shdma-base.o
+obj-$(CONFIG_SH_DMAE_BASE) += shdma-base.o
 obj-$(CONFIG_SH_DMAE) += shdma.o
+obj-$(CONFIG_SUDMAC) += sudmac.o
diff --git a/drivers/dma/sh/sudmac.c b/drivers/dma/sh/sudmac.c
new file mode 100644
index 000000000000..e7c94bbddb53
--- /dev/null
+++ b/drivers/dma/sh/sudmac.c
@@ -0,0 +1,428 @@
+/*
+ * Renesas SUDMAC support
+ *
+ * Copyright (C) 2013 Renesas Solutions Corp.
+ *
+ * based on drivers/dma/sh/shdma.c:
+ * Copyright (C) 2011-2012 Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ * Copyright (C) 2009 Nobuhiro Iwamatsu <iwamatsu.nobuhiro@renesas.com>
+ * Copyright (C) 2009 Renesas Solutions, Inc. All rights reserved.
+ * Copyright (C) 2007 Freescale Semiconductor, Inc. All rights reserved.
+ *
+ * This is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/dmaengine.h>
+#include <linux/platform_device.h>
+#include <linux/sudmac.h>
+
+struct sudmac_chan {
+	struct shdma_chan shdma_chan;
+	void __iomem *base;
+	char dev_id[16];	/* unique name per DMAC of channel */
+
+	u32 offset;		/* for CFG, BA, BBC, CA, CBC, DEN */
+	u32 cfg;
+	u32 dint_end_bit;
+};
+
+struct sudmac_device {
+	struct shdma_dev shdma_dev;
+	struct sudmac_pdata *pdata;
+	void __iomem *chan_reg;
+};
+
+struct sudmac_regs {
+	u32 base_addr;
+	u32 base_byte_count;
+};
+
+struct sudmac_desc {
+	struct sudmac_regs hw;
+	struct shdma_desc shdma_desc;
+};
+
+#define to_chan(schan) container_of(schan, struct sudmac_chan, shdma_chan)
+#define to_desc(sdesc) container_of(sdesc, struct sudmac_desc, shdma_desc)
+#define to_sdev(sc) container_of(sc->shdma_chan.dma_chan.device, \
+				 struct sudmac_device, shdma_dev.dma_dev)
+
+/* SUDMAC register */
+#define SUDMAC_CH0CFG		0x00
+#define SUDMAC_CH0BA		0x10
+#define SUDMAC_CH0BBC		0x18
+#define SUDMAC_CH0CA		0x20
+#define SUDMAC_CH0CBC		0x28
+#define SUDMAC_CH0DEN		0x30
+#define SUDMAC_DSTSCLR		0x38
+#define SUDMAC_DBUFCTRL		0x3C
+#define SUDMAC_DINTCTRL		0x40
+#define SUDMAC_DINTSTS		0x44
+#define SUDMAC_DINTSTSCLR	0x48
+#define SUDMAC_CH0SHCTRL	0x50
+
+/* Definitions for the sudmac_channel.config */
+#define SUDMAC_SENDBUFM	0x1000 /* b12: Transmit Buffer Mode */
+#define SUDMAC_RCVENDM	0x0100 /* b8: Receive Data Transfer End Mode */
+#define SUDMAC_LBA_WAIT	0x0030 /* b5-4: Local Bus Access Wait */
+
+/* Definitions for the sudmac_channel.dint_end_bit */
+#define SUDMAC_CH1ENDE	0x0002 /* b1: Ch1 DMA Transfer End Int Enable */
+#define SUDMAC_CH0ENDE	0x0001 /* b0: Ch0 DMA Transfer End Int Enable */
+
+#define SUDMAC_DRV_NAME "sudmac"
+
+static void sudmac_writel(struct sudmac_chan *sc, u32 data, u32 reg)
+{
+	iowrite32(data, sc->base + reg);
+}
+
+static u32 sudmac_readl(struct sudmac_chan *sc, u32 reg)
+{
+	return ioread32(sc->base + reg);
+}
+
+static bool sudmac_is_busy(struct sudmac_chan *sc)
+{
+	u32 den = sudmac_readl(sc, SUDMAC_CH0DEN + sc->offset);
+
+	if (den)
+		return true; /* working */
+
+	return false; /* waiting */
+}
+
+static void sudmac_set_reg(struct sudmac_chan *sc, struct sudmac_regs *hw,
+			   struct shdma_desc *sdesc)
+{
+	sudmac_writel(sc, sc->cfg, SUDMAC_CH0CFG + sc->offset);
+	sudmac_writel(sc, hw->base_addr, SUDMAC_CH0BA + sc->offset);
+	sudmac_writel(sc, hw->base_byte_count, SUDMAC_CH0BBC + sc->offset);
+}
+
+static void sudmac_start(struct sudmac_chan *sc)
+{
+	u32 dintctrl = sudmac_readl(sc, SUDMAC_DINTCTRL);
+
+	sudmac_writel(sc, dintctrl | sc->dint_end_bit, SUDMAC_DINTCTRL);
+	sudmac_writel(sc, 1, SUDMAC_CH0DEN + sc->offset);
+}
+
+static void sudmac_start_xfer(struct shdma_chan *schan,
+			      struct shdma_desc *sdesc)
+{
+	struct sudmac_chan *sc = to_chan(schan);
+	struct sudmac_desc *sd = to_desc(sdesc);
+
+	sudmac_set_reg(sc, &sd->hw, sdesc);
+	sudmac_start(sc);
+}
+
+static bool sudmac_channel_busy(struct shdma_chan *schan)
+{
+	struct sudmac_chan *sc = to_chan(schan);
+
+	return sudmac_is_busy(sc);
+}
+
+static void sudmac_setup_xfer(struct shdma_chan *schan, int slave_id)
+{
+}
+
+static const struct sudmac_slave_config *sudmac_find_slave(
+	struct sudmac_chan *sc, int slave_id)
+{
+	struct sudmac_device *sdev = to_sdev(sc);
+	struct sudmac_pdata *pdata = sdev->pdata;
+	const struct sudmac_slave_config *cfg;
+	int i;
+
+	for (i = 0, cfg = pdata->slave; i < pdata->slave_num; i++, cfg++)
+		if (cfg->slave_id == slave_id)
+			return cfg;
+
+	return NULL;
+}
+
+static int sudmac_set_slave(struct shdma_chan *schan, int slave_id, bool try)
+{
+	struct sudmac_chan *sc = to_chan(schan);
+	const struct sudmac_slave_config *cfg = sudmac_find_slave(sc, slave_id);
+
+	if (!cfg)
+		return -ENODEV;
+
+	return 0;
+}
+
+static inline void sudmac_dma_halt(struct sudmac_chan *sc)
+{
+	u32 dintctrl = sudmac_readl(sc, SUDMAC_DINTCTRL);
+
+	sudmac_writel(sc, 0, SUDMAC_CH0DEN + sc->offset);
+	sudmac_writel(sc, dintctrl & ~sc->dint_end_bit, SUDMAC_DINTCTRL);
+	sudmac_writel(sc, sc->dint_end_bit, SUDMAC_DINTSTSCLR);
+}
+
+static int sudmac_desc_setup(struct shdma_chan *schan,
+			     struct shdma_desc *sdesc,
+			     dma_addr_t src, dma_addr_t dst, size_t *len)
+{
+	struct sudmac_chan *sc = to_chan(schan);
+	struct sudmac_desc *sd = to_desc(sdesc);
+
+	dev_dbg(sc->shdma_chan.dev, "%s: src=%x, dst=%x, len=%d\n",
+		__func__, src, dst, *len);
+
+	if (*len > schan->max_xfer_len)
+		*len = schan->max_xfer_len;
+
+	if (dst)
+		sd->hw.base_addr = dst;
+	else if (src)
+		sd->hw.base_addr = src;
+	sd->hw.base_byte_count = *len;
+
+	return 0;
+}
+
+static void sudmac_halt(struct shdma_chan *schan)
+{
+	struct sudmac_chan *sc = to_chan(schan);
+
+	sudmac_dma_halt(sc);
+}
+
+static bool sudmac_chan_irq(struct shdma_chan *schan, int irq)
+{
+	struct sudmac_chan *sc = to_chan(schan);
+	u32 dintsts = sudmac_readl(sc, SUDMAC_DINTSTS);
+
+	if (!(dintsts & sc->dint_end_bit))
+		return false;
+
+	/* DMA stop */
+	sudmac_dma_halt(sc);
+
+	return true;
+}
+
+static size_t sudmac_get_partial(struct shdma_chan *schan,
+				 struct shdma_desc *sdesc)
+{
+	struct sudmac_chan *sc = to_chan(schan);
+	struct sudmac_desc *sd = to_desc(sdesc);
+	u32 current_byte_count = sudmac_readl(sc, SUDMAC_CH0CBC + sc->offset);
+
+	return sd->hw.base_byte_count - current_byte_count;
+}
+
+static bool sudmac_desc_completed(struct shdma_chan *schan,
+				  struct shdma_desc *sdesc)
+{
+	struct sudmac_chan *sc = to_chan(schan);
+	struct sudmac_desc *sd = to_desc(sdesc);
+	u32 current_addr = sudmac_readl(sc, SUDMAC_CH0CA + sc->offset);
+
+	return sd->hw.base_addr + sd->hw.base_byte_count == current_addr;
+}
+
+static int sudmac_chan_probe(struct sudmac_device *su_dev, int id, int irq,
+			     unsigned long flags)
+{
+	struct shdma_dev *sdev = &su_dev->shdma_dev;
+	struct platform_device *pdev = to_platform_device(sdev->dma_dev.dev);
+	struct sudmac_chan *sc;
+	struct shdma_chan *schan;
+	int err;
+
+	sc = devm_kzalloc(&pdev->dev, sizeof(struct sudmac_chan), GFP_KERNEL);
+	if (!sc) {
+		dev_err(sdev->dma_dev.dev,
+			"No free memory for allocating dma channels!\n");
+		return -ENOMEM;
+	}
+
+	schan = &sc->shdma_chan;
+	schan->max_xfer_len = 64 * 1024 * 1024 - 1;
+
+	shdma_chan_probe(sdev, schan, id);
+
+	sc->base = su_dev->chan_reg;
+
+	/* get platform_data */
+	sc->offset = su_dev->pdata->channel->offset;
+	if (su_dev->pdata->channel->config & SUDMAC_TX_BUFFER_MODE)
+		sc->cfg |= SUDMAC_SENDBUFM;
+	if (su_dev->pdata->channel->config & SUDMAC_RX_END_MODE)
+		sc->cfg |= SUDMAC_RCVENDM;
+	sc->cfg |= (su_dev->pdata->channel->wait << 4) & SUDMAC_LBA_WAIT;
+
+	if (su_dev->pdata->channel->dint_end_bit & SUDMAC_DMA_BIT_CH0)
+		sc->dint_end_bit |= SUDMAC_CH0ENDE;
+	if (su_dev->pdata->channel->dint_end_bit & SUDMAC_DMA_BIT_CH1)
+		sc->dint_end_bit |= SUDMAC_CH1ENDE;
+
+	/* set up channel irq */
+	if (pdev->id >= 0)
+		snprintf(sc->dev_id, sizeof(sc->dev_id), "sudmac%d.%d",
+			 pdev->id, id);
+	else
+		snprintf(sc->dev_id, sizeof(sc->dev_id), "sudmac%d", id);
+
+	err = shdma_request_irq(schan, irq, flags, sc->dev_id);
+	if (err) {
+		dev_err(sdev->dma_dev.dev,
+			"DMA channel %d request_irq failed %d\n", id, err);
+		goto err_no_irq;
+	}
+
+	return 0;
+
+err_no_irq:
+	/* remove from dmaengine device node */
+	shdma_chan_remove(schan);
+	return err;
+}
+
+static void sudmac_chan_remove(struct sudmac_device *su_dev)
+{
+	struct dma_device *dma_dev = &su_dev->shdma_dev.dma_dev;
+	struct shdma_chan *schan;
+	int i;
+
+	shdma_for_each_chan(schan, &su_dev->shdma_dev, i) {
+		struct sudmac_chan *sc = to_chan(schan);
+
+		BUG_ON(!schan);
+
+		shdma_free_irq(&sc->shdma_chan);
+		shdma_chan_remove(schan);
+	}
+	dma_dev->chancnt = 0;
+}
+
+static dma_addr_t sudmac_slave_addr(struct shdma_chan *schan)
+{
+	/* SUDMAC doesn't need the address */
+	return 0;
+}
+
+static struct shdma_desc *sudmac_embedded_desc(void *buf, int i)
+{
+	return &((struct sudmac_desc *)buf)[i].shdma_desc;
+}
+
+static const struct shdma_ops sudmac_shdma_ops = {
+	.desc_completed = sudmac_desc_completed,
+	.halt_channel = sudmac_halt,
+	.channel_busy = sudmac_channel_busy,
+	.slave_addr = sudmac_slave_addr,
+	.desc_setup = sudmac_desc_setup,
+	.set_slave = sudmac_set_slave,
+	.setup_xfer = sudmac_setup_xfer,
+	.start_xfer = sudmac_start_xfer,
+	.embedded_desc = sudmac_embedded_desc,
+	.chan_irq = sudmac_chan_irq,
+	.get_partial = sudmac_get_partial,
+};
+
+static int sudmac_probe(struct platform_device *pdev)
+{
+	struct sudmac_pdata *pdata = pdev->dev.platform_data;
+	int err, i;
+	struct sudmac_device *su_dev;
+	struct dma_device *dma_dev;
+	struct resource *chan, *irq_res;
+
+	/* get platform data */
+	if (!pdata)
+		return -ENODEV;
+
+	chan = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq_res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!chan || !irq_res)
+		return -ENODEV;
+
+	err = -ENOMEM;
+	su_dev = devm_kzalloc(&pdev->dev, sizeof(struct sudmac_device),
+			      GFP_KERNEL);
+	if (!su_dev) {
+		dev_err(&pdev->dev, "Not enough memory\n");
+		return err;
+	}
+
+	dma_dev = &su_dev->shdma_dev.dma_dev;
+
+	su_dev->chan_reg = devm_request_and_ioremap(&pdev->dev, chan);
+	if (!su_dev->chan_reg)
+		return err;
+
+	dma_cap_set(DMA_SLAVE, dma_dev->cap_mask);
+
+	su_dev->shdma_dev.ops = &sudmac_shdma_ops;
+	su_dev->shdma_dev.desc_size = sizeof(struct sudmac_desc);
+	err = shdma_init(&pdev->dev, &su_dev->shdma_dev, pdata->channel_num);
+	if (err < 0)
+		return err;
+
+	/* platform data */
+	su_dev->pdata = pdev->dev.platform_data;
+
+	platform_set_drvdata(pdev, su_dev);
+
+	/* Create DMA Channel */
+	for (i = 0; i < pdata->channel_num; i++) {
+		err = sudmac_chan_probe(su_dev, i, irq_res->start, IRQF_SHARED);
+		if (err)
+			goto chan_probe_err;
+	}
+
+	err = dma_async_device_register(&su_dev->shdma_dev.dma_dev);
+	if (err < 0)
+		goto chan_probe_err;
+
+	return err;
+
+chan_probe_err:
+	sudmac_chan_remove(su_dev);
+
+	platform_set_drvdata(pdev, NULL);
+	shdma_cleanup(&su_dev->shdma_dev);
+
+	return err;
+}
+
+static int sudmac_remove(struct platform_device *pdev)
+{
+	struct sudmac_device *su_dev = platform_get_drvdata(pdev);
+	struct dma_device *dma_dev = &su_dev->shdma_dev.dma_dev;
+
+	dma_async_device_unregister(dma_dev);
+	sudmac_chan_remove(su_dev);
+	shdma_cleanup(&su_dev->shdma_dev);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver sudmac_driver = {
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= SUDMAC_DRV_NAME,
+	},
+	.probe		= sudmac_probe,
+	.remove		= sudmac_remove,
+};
+module_platform_driver(sudmac_driver);
+
+MODULE_AUTHOR("Yoshihiro Shimoda");
+MODULE_DESCRIPTION("Renesas SUDMAC driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:" SUDMAC_DRV_NAME);
diff --git a/drivers/dma/sirf-dma.c b/drivers/dma/sirf-dma.c
index 1d627e2391f4..1765a0a2736d 100644
--- a/drivers/dma/sirf-dma.c
+++ b/drivers/dma/sirf-dma.c
@@ -16,6 +16,7 @@
 #include <linux/of_address.h>
 #include <linux/of_device.h>
 #include <linux/of_platform.h>
+#include <linux/clk.h>
 #include <linux/sirfsoc_dma.h>
 
 #include "dmaengine.h"
@@ -78,6 +79,7 @@ struct sirfsoc_dma {
 	struct sirfsoc_dma_chan		channels[SIRFSOC_DMA_CHANNELS];
 	void __iomem			*base;
 	int				irq;
+	struct clk			*clk;
 	bool				is_marco;
 };
 
@@ -639,6 +641,12 @@ static int sirfsoc_dma_probe(struct platform_device *op)
 		return -EINVAL;
 	}
 
+	sdma->clk = devm_clk_get(dev, NULL);
+	if (IS_ERR(sdma->clk)) {
+		dev_err(dev, "failed to get a clock.\n");
+		return PTR_ERR(sdma->clk);
+	}
+
 	ret = of_address_to_resource(dn, 0, &res);
 	if (ret) {
 		dev_err(dev, "Error parsing memory region!\n");
@@ -698,6 +706,8 @@ static int sirfsoc_dma_probe(struct platform_device *op)
 
 	tasklet_init(&sdma->tasklet, sirfsoc_dma_tasklet, (unsigned long)sdma);
 
+	clk_prepare_enable(sdma->clk);
+
 	/* Register DMA engine */
 	dev_set_drvdata(dev, sdma);
 	ret = dma_async_device_register(dma);
@@ -720,6 +730,7 @@ static int sirfsoc_dma_remove(struct platform_device *op)
 	struct device *dev = &op->dev;
 	struct sirfsoc_dma *sdma = dev_get_drvdata(dev);
 
+	clk_disable_unprepare(sdma->clk);
 	dma_async_device_unregister(&sdma->dma);
 	free_irq(sdma->irq, sdma);
 	irq_dispose_mapping(sdma->irq);
@@ -742,7 +753,18 @@ static struct platform_driver sirfsoc_dma_driver = {
 	},
 };
 
-module_platform_driver(sirfsoc_dma_driver);
+static __init int sirfsoc_dma_init(void)
+{
+	return platform_driver_register(&sirfsoc_dma_driver);
+}
+
+static void __exit sirfsoc_dma_exit(void)
+{
+	platform_driver_unregister(&sirfsoc_dma_driver);
+}
+
+subsys_initcall(sirfsoc_dma_init);
+module_exit(sirfsoc_dma_exit);
 
 MODULE_AUTHOR("Rongjun Ying <rongjun.ying@csr.com>, "
 	"Barry Song <baohua.song@csr.com>");
diff --git a/drivers/dma/tegra20-apb-dma.c b/drivers/dma/tegra20-apb-dma.c
index fcee27eae1f6..ce193409ebd3 100644
--- a/drivers/dma/tegra20-apb-dma.c
+++ b/drivers/dma/tegra20-apb-dma.c
@@ -30,6 +30,7 @@
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
+#include <linux/pm.h>
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
 #include <linux/clk/tegra.h>
@@ -199,6 +200,7 @@ struct tegra_dma_channel {
 
 	/* Channel-slave specific configuration */
 	struct dma_slave_config dma_sconfig;
+	struct tegra_dma_channel_regs	channel_reg;
 };
 
 /* tegra_dma: Tegra DMA specific information */
@@ -1213,7 +1215,6 @@ static const struct tegra_dma_chip_data tegra20_dma_chip_data = {
 	.support_channel_pause	= false,
 };
 
-#if defined(CONFIG_OF)
 /* Tegra30 specific DMA controller information */
 static const struct tegra_dma_chip_data tegra30_dma_chip_data = {
 	.nr_channels		= 32,
@@ -1243,7 +1244,6 @@ static const struct of_device_id tegra_dma_of_match[] = {
 	},
 };
 MODULE_DEVICE_TABLE(of, tegra_dma_of_match);
-#endif
 
 static int tegra_dma_probe(struct platform_device *pdev)
 {
@@ -1252,20 +1252,14 @@ static int tegra_dma_probe(struct platform_device *pdev)
 	int ret;
 	int i;
 	const struct tegra_dma_chip_data *cdata = NULL;
+	const struct of_device_id *match;
 
-	if (pdev->dev.of_node) {
-		const struct of_device_id *match;
-		match = of_match_device(of_match_ptr(tegra_dma_of_match),
-					&pdev->dev);
-		if (!match) {
-			dev_err(&pdev->dev, "Error: No device match found\n");
-			return -ENODEV;
-		}
-		cdata = match->data;
-	} else {
-		/* If no device tree then fallback to tegra20 */
-		cdata = &tegra20_dma_chip_data;
+	match = of_match_device(tegra_dma_of_match, &pdev->dev);
+	if (!match) {
+		dev_err(&pdev->dev, "Error: No device match found\n");
+		return -ENODEV;
 	}
+	cdata = match->data;
 
 	tdma = devm_kzalloc(&pdev->dev, sizeof(*tdma) + cdata->nr_channels *
 			sizeof(struct tegra_dma_channel), GFP_KERNEL);
@@ -1448,11 +1442,74 @@ static int tegra_dma_runtime_resume(struct device *dev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int tegra_dma_pm_suspend(struct device *dev)
+{
+	struct tegra_dma *tdma = dev_get_drvdata(dev);
+	int i;
+	int ret;
+
+	/* Enable clock before accessing register */
+	ret = tegra_dma_runtime_resume(dev);
+	if (ret < 0)
+		return ret;
+
+	tdma->reg_gen = tdma_read(tdma, TEGRA_APBDMA_GENERAL);
+	for (i = 0; i < tdma->chip_data->nr_channels; i++) {
+		struct tegra_dma_channel *tdc = &tdma->channels[i];
+		struct tegra_dma_channel_regs *ch_reg = &tdc->channel_reg;
+
+		ch_reg->csr = tdc_read(tdc, TEGRA_APBDMA_CHAN_CSR);
+		ch_reg->ahb_ptr = tdc_read(tdc, TEGRA_APBDMA_CHAN_AHBPTR);
+		ch_reg->apb_ptr = tdc_read(tdc, TEGRA_APBDMA_CHAN_APBPTR);
+		ch_reg->ahb_seq = tdc_read(tdc, TEGRA_APBDMA_CHAN_AHBSEQ);
+		ch_reg->apb_seq = tdc_read(tdc, TEGRA_APBDMA_CHAN_APBSEQ);
+	}
+
+	/* Disable clock */
+	tegra_dma_runtime_suspend(dev);
+	return 0;
+}
+
+static int tegra_dma_pm_resume(struct device *dev)
+{
+	struct tegra_dma *tdma = dev_get_drvdata(dev);
+	int i;
+	int ret;
+
+	/* Enable clock before accessing register */
+	ret = tegra_dma_runtime_resume(dev);
+	if (ret < 0)
+		return ret;
+
+	tdma_write(tdma, TEGRA_APBDMA_GENERAL, tdma->reg_gen);
+	tdma_write(tdma, TEGRA_APBDMA_CONTROL, 0);
+	tdma_write(tdma, TEGRA_APBDMA_IRQ_MASK_SET, 0xFFFFFFFFul);
+
+	for (i = 0; i < tdma->chip_data->nr_channels; i++) {
+		struct tegra_dma_channel *tdc = &tdma->channels[i];
+		struct tegra_dma_channel_regs *ch_reg = &tdc->channel_reg;
+
+		tdc_write(tdc, TEGRA_APBDMA_CHAN_APBSEQ, ch_reg->apb_seq);
+		tdc_write(tdc, TEGRA_APBDMA_CHAN_APBPTR, ch_reg->apb_ptr);
+		tdc_write(tdc, TEGRA_APBDMA_CHAN_AHBSEQ, ch_reg->ahb_seq);
+		tdc_write(tdc, TEGRA_APBDMA_CHAN_AHBPTR, ch_reg->ahb_ptr);
+		tdc_write(tdc, TEGRA_APBDMA_CHAN_CSR,
+			(ch_reg->csr & ~TEGRA_APBDMA_CSR_ENB));
+	}
+
+	/* Disable clock */
+	tegra_dma_runtime_suspend(dev);
+	return 0;
+}
+#endif
+
 static const struct dev_pm_ops tegra_dma_dev_pm_ops = {
 #ifdef CONFIG_PM_RUNTIME
 	.runtime_suspend = tegra_dma_runtime_suspend,
 	.runtime_resume = tegra_dma_runtime_resume,
 #endif
+	SET_SYSTEM_SLEEP_PM_OPS(tegra_dma_pm_suspend, tegra_dma_pm_resume)
 };
 
 static struct platform_driver tegra_dmac_driver = {
@@ -1460,7 +1517,7 @@ static struct platform_driver tegra_dmac_driver = {
 		.name	= "tegra-apbdma",
 		.owner = THIS_MODULE,
 		.pm	= &tegra_dma_dev_pm_ops,
-		.of_match_table = of_match_ptr(tegra_dma_of_match),
+		.of_match_table = tegra_dma_of_match,
 	},
 	.probe		= tegra_dma_probe,
 	.remove		= tegra_dma_remove,
diff --git a/drivers/dma/timb_dma.c b/drivers/dma/timb_dma.c
index 952f823901a6..26107ba6edb3 100644
--- a/drivers/dma/timb_dma.c
+++ b/drivers/dma/timb_dma.c
@@ -823,7 +823,7 @@ static struct platform_driver td_driver = {
 		.owner  = THIS_MODULE,
 	},
 	.probe	= td_probe,
-	.remove	= __exit_p(td_remove),
+	.remove	= td_remove,
 };
 
 module_platform_driver(td_driver);
diff --git a/drivers/dma/txx9dmac.c b/drivers/dma/txx9dmac.c
index 913f55c76c99..a59fb4841d4c 100644
--- a/drivers/dma/txx9dmac.c
+++ b/drivers/dma/txx9dmac.c
@@ -1190,7 +1190,7 @@ static int __init txx9dmac_chan_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int __exit txx9dmac_chan_remove(struct platform_device *pdev)
+static int txx9dmac_chan_remove(struct platform_device *pdev)
 {
 	struct txx9dmac_chan *dc = platform_get_drvdata(pdev);
 
@@ -1252,7 +1252,7 @@ static int __init txx9dmac_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int __exit txx9dmac_remove(struct platform_device *pdev)
+static int txx9dmac_remove(struct platform_device *pdev)
 {
 	struct txx9dmac_dev *ddev = platform_get_drvdata(pdev);
 
@@ -1299,14 +1299,14 @@ static const struct dev_pm_ops txx9dmac_dev_pm_ops = {
 };
 
 static struct platform_driver txx9dmac_chan_driver = {
-	.remove		= __exit_p(txx9dmac_chan_remove),
+	.remove		= txx9dmac_chan_remove,
 	.driver = {
 		.name	= "txx9dmac-chan",
 	},
 };
 
 static struct platform_driver txx9dmac_driver = {
-	.remove		= __exit_p(txx9dmac_remove),
+	.remove		= txx9dmac_remove,
 	.shutdown	= txx9dmac_shutdown,
 	.driver = {
 		.name	= "txx9dmac",
diff --git a/drivers/edac/edac_mc_sysfs.c b/drivers/edac/edac_mc_sysfs.c
index 5899a76eec3b..67610a6ebf87 100644
--- a/drivers/edac/edac_mc_sysfs.c
+++ b/drivers/edac/edac_mc_sysfs.c
@@ -87,7 +87,7 @@ static struct device *mci_pdev;
 /*
  * various constants for Memory Controllers
  */
-static const char *mem_types[] = {
+static const char * const mem_types[] = {
 	[MEM_EMPTY] = "Empty",
 	[MEM_RESERVED] = "Reserved",
 	[MEM_UNKNOWN] = "Unknown",
@@ -107,7 +107,7 @@ static const char *mem_types[] = {
 	[MEM_RDDR3] = "Registered-DDR3"
 };
 
-static const char *dev_types[] = {
+static const char * const dev_types[] = {
 	[DEV_UNKNOWN] = "Unknown",
 	[DEV_X1] = "x1",
 	[DEV_X2] = "x2",
@@ -118,7 +118,7 @@ static const char *dev_types[] = {
 	[DEV_X64] = "x64"
 };
 
-static const char *edac_caps[] = {
+static const char * const edac_caps[] = {
 	[EDAC_UNKNOWN] = "Unknown",
 	[EDAC_NONE] = "None",
 	[EDAC_RESERVED] = "Reserved",
@@ -327,17 +327,17 @@ static struct device_attribute *dynamic_csrow_dimm_attr[] = {
 };
 
 /* possible dynamic channel ce_count attribute files */
-DEVICE_CHANNEL(ch0_ce_count, S_IRUGO | S_IWUSR,
+DEVICE_CHANNEL(ch0_ce_count, S_IRUGO,
 		   channel_ce_count_show, NULL, 0);
-DEVICE_CHANNEL(ch1_ce_count, S_IRUGO | S_IWUSR,
+DEVICE_CHANNEL(ch1_ce_count, S_IRUGO,
 		   channel_ce_count_show, NULL, 1);
-DEVICE_CHANNEL(ch2_ce_count, S_IRUGO | S_IWUSR,
+DEVICE_CHANNEL(ch2_ce_count, S_IRUGO,
 		   channel_ce_count_show, NULL, 2);
-DEVICE_CHANNEL(ch3_ce_count, S_IRUGO | S_IWUSR,
+DEVICE_CHANNEL(ch3_ce_count, S_IRUGO,
 		   channel_ce_count_show, NULL, 3);
-DEVICE_CHANNEL(ch4_ce_count, S_IRUGO | S_IWUSR,
+DEVICE_CHANNEL(ch4_ce_count, S_IRUGO,
 		   channel_ce_count_show, NULL, 4);
-DEVICE_CHANNEL(ch5_ce_count, S_IRUGO | S_IWUSR,
+DEVICE_CHANNEL(ch5_ce_count, S_IRUGO,
 		   channel_ce_count_show, NULL, 5);
 
 /* Total possible dynamic ce_count attribute file table */
diff --git a/drivers/edac/i7300_edac.c b/drivers/edac/i7300_edac.c
index 087c27bc5d42..9004c64b169e 100644
--- a/drivers/edac/i7300_edac.c
+++ b/drivers/edac/i7300_edac.c
@@ -750,15 +750,23 @@ static int i7300_init_csrows(struct mem_ctl_info *mci)
 	struct i7300_dimm_info *dinfo;
 	int rc = -ENODEV;
 	int mtr;
-	int ch, branch, slot, channel;
+	int ch, branch, slot, channel, max_channel, max_branch;
 	struct dimm_info *dimm;
 
 	pvt = mci->pvt_info;
 
 	edac_dbg(2, "Memory Technology Registers:\n");
 
+	if (IS_SINGLE_MODE(pvt->mc_settings_a)) {
+		max_branch = 1;
+		max_channel = 1;
+	} else {
+		max_branch = MAX_BRANCHES;
+		max_channel = MAX_CH_PER_BRANCH;
+	}
+
 	/* Get the AMB present registers for the four channels */
-	for (branch = 0; branch < MAX_BRANCHES; branch++) {
+	for (branch = 0; branch < max_branch; branch++) {
 		/* Read and dump branch 0's MTRs */
 		channel = to_channel(0, branch);
 		pci_read_config_word(pvt->pci_dev_2x_0_fbd_branch[branch],
@@ -767,6 +775,9 @@ static int i7300_init_csrows(struct mem_ctl_info *mci)
 		edac_dbg(2, "\t\tAMB-present CH%d = 0x%x:\n",
 			 channel, pvt->ambpresent[channel]);
 
+		if (max_channel == 1)
+			continue;
+
 		channel = to_channel(1, branch);
 		pci_read_config_word(pvt->pci_dev_2x_0_fbd_branch[branch],
 				     AMBPRESENT_1,
@@ -778,11 +789,11 @@ static int i7300_init_csrows(struct mem_ctl_info *mci)
 	/* Get the set of MTR[0-7] regs by each branch */
 	for (slot = 0; slot < MAX_SLOTS; slot++) {
 		int where = mtr_regs[slot];
-		for (branch = 0; branch < MAX_BRANCHES; branch++) {
+		for (branch = 0; branch < max_branch; branch++) {
 			pci_read_config_word(pvt->pci_dev_2x_0_fbd_branch[branch],
 					where,
 					&pvt->mtr[slot][branch]);
-			for (ch = 0; ch < MAX_CH_PER_BRANCH; ch++) {
+			for (ch = 0; ch < max_channel; ch++) {
 				int channel = to_channel(ch, branch);
 
 				dimm = EDAC_DIMM_PTR(mci->layers, mci->dimms,
diff --git a/drivers/edac/sb_edac.c b/drivers/edac/sb_edac.c
index 57244f995614..e04462b60756 100644
--- a/drivers/edac/sb_edac.c
+++ b/drivers/edac/sb_edac.c
@@ -331,30 +331,31 @@ struct sbridge_pvt {
 	u64			tolm, tohm;
 };
 
-#define PCI_DESCR(device, function, device_id)	\
-	.dev = (device),			\
-	.func = (function),			\
-	.dev_id = (device_id)
+#define PCI_DESCR(device, function, device_id, opt)	\
+	.dev = (device),				\
+	.func = (function),				\
+	.dev_id = (device_id),				\
+	.optional = opt
 
 static const struct pci_id_descr pci_dev_descr_sbridge[] = {
 		/* Processor Home Agent */
-	{ PCI_DESCR(14, 0, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_HA0)		},
+	{ PCI_DESCR(14, 0, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_HA0, 0)	},
 
 		/* Memory controller */
-	{ PCI_DESCR(15, 0, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TA)		},
-	{ PCI_DESCR(15, 1, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_RAS)		},
-	{ PCI_DESCR(15, 2, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD0)	},
-	{ PCI_DESCR(15, 3, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD1)	},
-	{ PCI_DESCR(15, 4, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD2)	},
-	{ PCI_DESCR(15, 5, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD3)	},
-	{ PCI_DESCR(17, 0, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_DDRIO)	},
+	{ PCI_DESCR(15, 0, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TA, 0)	},
+	{ PCI_DESCR(15, 1, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_RAS, 0)	},
+	{ PCI_DESCR(15, 2, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD0, 0)	},
+	{ PCI_DESCR(15, 3, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD1, 0)	},
+	{ PCI_DESCR(15, 4, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD2, 0)	},
+	{ PCI_DESCR(15, 5, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD3, 0)	},
+	{ PCI_DESCR(17, 0, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_DDRIO, 1)	},
 
 		/* System Address Decoder */
-	{ PCI_DESCR(12, 6, PCI_DEVICE_ID_INTEL_SBRIDGE_SAD0)		},
-	{ PCI_DESCR(12, 7, PCI_DEVICE_ID_INTEL_SBRIDGE_SAD1)		},
+	{ PCI_DESCR(12, 6, PCI_DEVICE_ID_INTEL_SBRIDGE_SAD0, 0)		},
+	{ PCI_DESCR(12, 7, PCI_DEVICE_ID_INTEL_SBRIDGE_SAD1, 0)		},
 
 		/* Broadcast Registers */
-	{ PCI_DESCR(13, 6, PCI_DEVICE_ID_INTEL_SBRIDGE_BR)		},
+	{ PCI_DESCR(13, 6, PCI_DEVICE_ID_INTEL_SBRIDGE_BR, 0)		},
 };
 
 #define PCI_ID_TABLE_ENTRY(A) { .descr=A, .n_devs = ARRAY_SIZE(A) }
@@ -556,14 +557,19 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 		pvt->is_close_pg = false;
 	}
 
-	pci_read_config_dword(pvt->pci_ddrio, RANK_CFG_A, &reg);
-	if (IS_RDIMM_ENABLED(reg)) {
-		/* FIXME: Can also be LRDIMM */
-		edac_dbg(0, "Memory is registered\n");
-		mtype = MEM_RDDR3;
+	if (pvt->pci_ddrio) {
+		pci_read_config_dword(pvt->pci_ddrio, RANK_CFG_A, &reg);
+		if (IS_RDIMM_ENABLED(reg)) {
+			/* FIXME: Can also be LRDIMM */
+			edac_dbg(0, "Memory is registered\n");
+			mtype = MEM_RDDR3;
+		} else {
+			edac_dbg(0, "Memory is unregistered\n");
+			mtype = MEM_DDR3;
+		}
 	} else {
-		edac_dbg(0, "Memory is unregistered\n");
-		mtype = MEM_DDR3;
+		edac_dbg(0, "Cannot determine memory type\n");
+		mtype = MEM_UNKNOWN;
 	}
 
 	/* On all supported DDR3 DIMM types, there are 8 banks available */
@@ -1303,8 +1309,7 @@ static int mci_bind_devs(struct mem_ctl_info *mci,
 
 	/* Check if everything were registered */
 	if (!pvt->pci_sad0 || !pvt->pci_sad1 || !pvt->pci_ha0 ||
-	    !pvt-> pci_tad || !pvt->pci_ras  || !pvt->pci_ta ||
-	    !pvt->pci_ddrio)
+	    !pvt-> pci_tad || !pvt->pci_ras  || !pvt->pci_ta)
 		goto enodev;
 
 	for (i = 0; i < NUM_CHANNELS; i++) {
diff --git a/drivers/extcon/Kconfig b/drivers/extcon/Kconfig
index 5168a1324a65..3297301a42d4 100644
--- a/drivers/extcon/Kconfig
+++ b/drivers/extcon/Kconfig
@@ -16,7 +16,7 @@ comment "Extcon Device Drivers"
 
 config EXTCON_GPIO
 	tristate "GPIO extcon support"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  Say Y here to enable GPIO based extcon support. Note that GPIO
 	  extcon supports single state per extcon instance.
diff --git a/drivers/firewire/Kconfig b/drivers/firewire/Kconfig
index 7224533e8ca6..7a701a58bbf0 100644
--- a/drivers/firewire/Kconfig
+++ b/drivers/firewire/Kconfig
@@ -47,9 +47,9 @@ config FIREWIRE_NET
 	tristate "IP networking over 1394"
 	depends on FIREWIRE && INET
 	help
-	  This enables IPv4 over IEEE 1394, providing IP connectivity with
-	  other implementations of RFC 2734 as found on several operating
-	  systems.  Multicast support is currently limited.
+	  This enables IPv4/IPv6 over IEEE 1394, providing IP connectivity
+	  with other implementations of RFC 2734/3146 as found on several
+	  operating systems.  Multicast support is currently limited.
 
 	  To compile this driver as a module, say M here:  The module will be
 	  called firewire-net.
diff --git a/drivers/firewire/core-cdev.c b/drivers/firewire/core-cdev.c
index 27ac423ab25e..7ef316fdc4d9 100644
--- a/drivers/firewire/core-cdev.c
+++ b/drivers/firewire/core-cdev.c
@@ -389,10 +389,8 @@ static void queue_bus_reset_event(struct client *client)
 	struct bus_reset_event *e;
 
 	e = kzalloc(sizeof(*e), GFP_KERNEL);
-	if (e == NULL) {
-		fw_notice(client->device->card, "out of memory when allocating event\n");
+	if (e == NULL)
 		return;
-	}
 
 	fill_bus_reset_event(&e->reset, client);
 
@@ -693,10 +691,9 @@ static void handle_request(struct fw_card *card, struct fw_request *request,
 
 	r = kmalloc(sizeof(*r), GFP_ATOMIC);
 	e = kmalloc(sizeof(*e), GFP_ATOMIC);
-	if (r == NULL || e == NULL) {
-		fw_notice(card, "out of memory when allocating event\n");
+	if (r == NULL || e == NULL)
 		goto failed;
-	}
+
 	r->card    = card;
 	r->request = request;
 	r->data    = payload;
@@ -930,10 +927,9 @@ static void iso_callback(struct fw_iso_context *context, u32 cycle,
 	struct iso_interrupt_event *e;
 
 	e = kmalloc(sizeof(*e) + header_length, GFP_ATOMIC);
-	if (e == NULL) {
-		fw_notice(context->card, "out of memory when allocating event\n");
+	if (e == NULL)
 		return;
-	}
+
 	e->interrupt.type      = FW_CDEV_EVENT_ISO_INTERRUPT;
 	e->interrupt.closure   = client->iso_closure;
 	e->interrupt.cycle     = cycle;
@@ -950,10 +946,9 @@ static void iso_mc_callback(struct fw_iso_context *context,
 	struct iso_interrupt_mc_event *e;
 
 	e = kmalloc(sizeof(*e), GFP_ATOMIC);
-	if (e == NULL) {
-		fw_notice(context->card, "out of memory when allocating event\n");
+	if (e == NULL)
 		return;
-	}
+
 	e->interrupt.type      = FW_CDEV_EVENT_ISO_INTERRUPT_MULTICHANNEL;
 	e->interrupt.closure   = client->iso_closure;
 	e->interrupt.completed = fw_iso_buffer_lookup(&client->buffer,
@@ -1366,8 +1361,7 @@ static int init_iso_resource(struct client *client,
 	int ret;
 
 	if ((request->channels == 0 && request->bandwidth == 0) ||
-	    request->bandwidth > BANDWIDTH_AVAILABLE_INITIAL ||
-	    request->bandwidth < 0)
+	    request->bandwidth > BANDWIDTH_AVAILABLE_INITIAL)
 		return -EINVAL;
 
 	r  = kmalloc(sizeof(*r), GFP_KERNEL);
@@ -1582,10 +1576,9 @@ void fw_cdev_handle_phy_packet(struct fw_card *card, struct fw_packet *p)
 
 	list_for_each_entry(client, &card->phy_receiver_list, phy_receiver_link) {
 		e = kmalloc(sizeof(*e) + 8, GFP_ATOMIC);
-		if (e == NULL) {
-			fw_notice(card, "out of memory when allocating event\n");
+		if (e == NULL)
 			break;
-		}
+
 		e->phy_packet.closure	= client->phy_receiver_closure;
 		e->phy_packet.type	= FW_CDEV_EVENT_PHY_PACKET_RECEIVED;
 		e->phy_packet.rcode	= RCODE_COMPLETE;
diff --git a/drivers/firewire/core-device.c b/drivers/firewire/core-device.c
index 03ce7d980c6a..664a6ff0a823 100644
--- a/drivers/firewire/core-device.c
+++ b/drivers/firewire/core-device.c
@@ -692,10 +692,8 @@ static void create_units(struct fw_device *device)
 		 * match the drivers id_tables against it.
 		 */
 		unit = kzalloc(sizeof(*unit), GFP_KERNEL);
-		if (unit == NULL) {
-			fw_err(device->card, "out of memory for unit\n");
+		if (unit == NULL)
 			continue;
-		}
 
 		unit->directory = ci.p + value - 1;
 		unit->device.bus = &fw_bus_type;
diff --git a/drivers/firewire/net.c b/drivers/firewire/net.c
index 2b27bff2591a..815b0fcbe918 100644
--- a/drivers/firewire/net.c
+++ b/drivers/firewire/net.c
@@ -1,5 +1,6 @@
 /*
  * IPv4 over IEEE 1394, per RFC 2734
+ * IPv6 over IEEE 1394, per RFC 3146
  *
  * Copyright (C) 2009 Jay Fenlason <fenlason@redhat.com>
  *
@@ -28,6 +29,7 @@
 
 #include <asm/unaligned.h>
 #include <net/arp.h>
+#include <net/firewire.h>
 
 /* rx limits */
 #define FWNET_MAX_FRAGMENTS		30 /* arbitrary, > TX queue depth */
@@ -45,6 +47,7 @@
 
 #define IANA_SPECIFIER_ID		0x00005eU
 #define RFC2734_SW_VERSION		0x000001U
+#define RFC3146_SW_VERSION		0x000002U
 
 #define IEEE1394_GASP_HDR_SIZE	8
 
@@ -57,32 +60,10 @@
 #define RFC2374_HDR_LASTFRAG	2	/* last fragment	*/
 #define RFC2374_HDR_INTFRAG	3	/* interior fragment	*/
 
-#define RFC2734_HW_ADDR_LEN	16
-
-struct rfc2734_arp {
-	__be16 hw_type;		/* 0x0018	*/
-	__be16 proto_type;	/* 0x0806       */
-	u8 hw_addr_len;		/* 16		*/
-	u8 ip_addr_len;		/* 4		*/
-	__be16 opcode;		/* ARP Opcode	*/
-	/* Above is exactly the same format as struct arphdr */
-
-	__be64 s_uniq_id;	/* Sender's 64bit EUI			*/
-	u8 max_rec;		/* Sender's max packet size		*/
-	u8 sspd;		/* Sender's max speed			*/
-	__be16 fifo_hi;		/* hi 16bits of sender's FIFO addr	*/
-	__be32 fifo_lo;		/* lo 32bits of sender's FIFO addr	*/
-	__be32 sip;		/* Sender's IP Address			*/
-	__be32 tip;		/* IP Address of requested hw addr	*/
-} __packed;
-
-/* This header format is specific to this driver implementation. */
-#define FWNET_ALEN	8
-#define FWNET_HLEN	10
-struct fwnet_header {
-	u8 h_dest[FWNET_ALEN];	/* destination address */
-	__be16 h_proto;		/* packet type ID field */
-} __packed;
+static bool fwnet_hwaddr_is_multicast(u8 *ha)
+{
+	return !!(*ha & 1);
+}
 
 /* IPv4 and IPv6 encapsulation header */
 struct rfc2734_header {
@@ -191,8 +172,6 @@ struct fwnet_peer {
 	struct list_head peer_link;
 	struct fwnet_device *dev;
 	u64 guid;
-	u64 fifo;
-	__be32 ip;
 
 	/* guarded by dev->lock */
 	struct list_head pd_list; /* received partial datagrams */
@@ -222,6 +201,15 @@ struct fwnet_packet_task {
 };
 
 /*
+ * Get fifo address embedded in hwaddr
+ */
+static __u64 fwnet_hwaddr_fifo(union fwnet_hwaddr *ha)
+{
+	return (u64)get_unaligned_be16(&ha->uc.fifo_hi) << 32
+	       | get_unaligned_be32(&ha->uc.fifo_lo);
+}
+
+/*
  * saddr == NULL means use device source address.
  * daddr == NULL means leave destination address (eg unresolved arp).
  */
@@ -368,10 +356,8 @@ static struct fwnet_fragment_info *fwnet_frag_new(
 	}
 
 	new = kmalloc(sizeof(*new), GFP_ATOMIC);
-	if (!new) {
-		dev_err(&pd->skb->dev->dev, "out of memory\n");
+	if (!new)
 		return NULL;
-	}
 
 	new->offset = offset;
 	new->len = len;
@@ -414,8 +400,6 @@ fail_w_fi:
 fail_w_new:
 	kfree(new);
 fail:
-	dev_err(&net->dev, "out of memory\n");
-
 	return NULL;
 }
 
@@ -513,10 +497,20 @@ static int fwnet_finish_incoming_packet(struct net_device *net,
 					bool is_broadcast, u16 ether_type)
 {
 	struct fwnet_device *dev;
-	static const __be64 broadcast_hw = cpu_to_be64(~0ULL);
 	int status;
 	__be64 guid;
 
+	switch (ether_type) {
+	case ETH_P_ARP:
+	case ETH_P_IP:
+#if IS_ENABLED(CONFIG_IPV6)
+	case ETH_P_IPV6:
+#endif
+		break;
+	default:
+		goto err;
+	}
+
 	dev = netdev_priv(net);
 	/* Write metadata, and then pass to the receive level */
 	skb->dev = net;
@@ -524,92 +518,11 @@ static int fwnet_finish_incoming_packet(struct net_device *net,
 
 	/*
 	 * Parse the encapsulation header. This actually does the job of
-	 * converting to an ethernet frame header, as well as arp
-	 * conversion if needed. ARP conversion is easier in this
-	 * direction, since we are using ethernet as our backend.
+	 * converting to an ethernet-like pseudo frame header.
 	 */
-	/*
-	 * If this is an ARP packet, convert it. First, we want to make
-	 * use of some of the fields, since they tell us a little bit
-	 * about the sending machine.
-	 */
-	if (ether_type == ETH_P_ARP) {
-		struct rfc2734_arp *arp1394;
-		struct arphdr *arp;
-		unsigned char *arp_ptr;
-		u64 fifo_addr;
-		u64 peer_guid;
-		unsigned sspd;
-		u16 max_payload;
-		struct fwnet_peer *peer;
-		unsigned long flags;
-
-		arp1394   = (struct rfc2734_arp *)skb->data;
-		arp       = (struct arphdr *)skb->data;
-		arp_ptr   = (unsigned char *)(arp + 1);
-		peer_guid = get_unaligned_be64(&arp1394->s_uniq_id);
-		fifo_addr = (u64)get_unaligned_be16(&arp1394->fifo_hi) << 32
-				| get_unaligned_be32(&arp1394->fifo_lo);
-
-		sspd = arp1394->sspd;
-		/* Sanity check.  OS X 10.3 PPC reportedly sends 131. */
-		if (sspd > SCODE_3200) {
-			dev_notice(&net->dev, "sspd %x out of range\n", sspd);
-			sspd = SCODE_3200;
-		}
-		max_payload = fwnet_max_payload(arp1394->max_rec, sspd);
-
-		spin_lock_irqsave(&dev->lock, flags);
-		peer = fwnet_peer_find_by_guid(dev, peer_guid);
-		if (peer) {
-			peer->fifo = fifo_addr;
-
-			if (peer->speed > sspd)
-				peer->speed = sspd;
-			if (peer->max_payload > max_payload)
-				peer->max_payload = max_payload;
-
-			peer->ip = arp1394->sip;
-		}
-		spin_unlock_irqrestore(&dev->lock, flags);
-
-		if (!peer) {
-			dev_notice(&net->dev,
-				   "no peer for ARP packet from %016llx\n",
-				   (unsigned long long)peer_guid);
-			goto no_peer;
-		}
-
-		/*
-		 * Now that we're done with the 1394 specific stuff, we'll
-		 * need to alter some of the data.  Believe it or not, all
-		 * that needs to be done is sender_IP_address needs to be
-		 * moved, the destination hardware address get stuffed
-		 * in and the hardware address length set to 8.
-		 *
-		 * IMPORTANT: The code below overwrites 1394 specific data
-		 * needed above so keep the munging of the data for the
-		 * higher level IP stack last.
-		 */
-
-		arp->ar_hln = 8;
-		/* skip over sender unique id */
-		arp_ptr += arp->ar_hln;
-		/* move sender IP addr */
-		put_unaligned(arp1394->sip, (u32 *)arp_ptr);
-		/* skip over sender IP addr */
-		arp_ptr += arp->ar_pln;
-
-		if (arp->ar_op == htons(ARPOP_REQUEST))
-			memset(arp_ptr, 0, sizeof(u64));
-		else
-			memcpy(arp_ptr, net->dev_addr, sizeof(u64));
-	}
-
-	/* Now add the ethernet header. */
 	guid = cpu_to_be64(dev->card->guid);
 	if (dev_hard_header(skb, net, ether_type,
-			   is_broadcast ? &broadcast_hw : &guid,
+			   is_broadcast ? net->broadcast : net->dev_addr,
 			   NULL, skb->len) >= 0) {
 		struct fwnet_header *eth;
 		u16 *rawp;
@@ -618,7 +531,7 @@ static int fwnet_finish_incoming_packet(struct net_device *net,
 		skb_reset_mac_header(skb);
 		skb_pull(skb, sizeof(*eth));
 		eth = (struct fwnet_header *)skb_mac_header(skb);
-		if (*eth->h_dest & 1) {
+		if (fwnet_hwaddr_is_multicast(eth->h_dest)) {
 			if (memcmp(eth->h_dest, net->broadcast,
 				   net->addr_len) == 0)
 				skb->pkt_type = PACKET_BROADCAST;
@@ -630,7 +543,7 @@ static int fwnet_finish_incoming_packet(struct net_device *net,
 			if (memcmp(eth->h_dest, net->dev_addr, net->addr_len))
 				skb->pkt_type = PACKET_OTHERHOST;
 		}
-		if (ntohs(eth->h_proto) >= 1536) {
+		if (ntohs(eth->h_proto) >= ETH_P_802_3_MIN) {
 			protocol = eth->h_proto;
 		} else {
 			rawp = (u16 *)skb->data;
@@ -652,7 +565,7 @@ static int fwnet_finish_incoming_packet(struct net_device *net,
 
 	return 0;
 
- no_peer:
+ err:
 	net->stats.rx_errors++;
 	net->stats.rx_dropped++;
 
@@ -692,7 +605,6 @@ static int fwnet_incoming_packet(struct fwnet_device *dev, __be32 *buf, int len,
 
 		skb = dev_alloc_skb(len + LL_RESERVED_SPACE(net));
 		if (unlikely(!skb)) {
-			dev_err(&net->dev, "out of memory\n");
 			net->stats.rx_dropped++;
 
 			return -ENOMEM;
@@ -856,7 +768,12 @@ static void fwnet_receive_broadcast(struct fw_iso_context *context,
 	ver = be32_to_cpu(buf_ptr[1]) & 0xffffff;
 	source_node_id = be32_to_cpu(buf_ptr[0]) >> 16;
 
-	if (specifier_id == IANA_SPECIFIER_ID && ver == RFC2734_SW_VERSION) {
+	if (specifier_id == IANA_SPECIFIER_ID &&
+	    (ver == RFC2734_SW_VERSION
+#if IS_ENABLED(CONFIG_IPV6)
+	     || ver == RFC3146_SW_VERSION
+#endif
+	    )) {
 		buf_ptr += 2;
 		length -= IEEE1394_GASP_HDR_SIZE;
 		fwnet_incoming_packet(dev, buf_ptr, length, source_node_id,
@@ -1059,16 +976,27 @@ static int fwnet_send_packet(struct fwnet_packet_task *ptask)
 		u8 *p;
 		int generation;
 		int node_id;
+		unsigned int sw_version;
 
 		/* ptask->generation may not have been set yet */
 		generation = dev->card->generation;
 		smp_rmb();
 		node_id = dev->card->node_id;
 
+		switch (ptask->skb->protocol) {
+		default:
+			sw_version = RFC2734_SW_VERSION;
+			break;
+#if IS_ENABLED(CONFIG_IPV6)
+		case htons(ETH_P_IPV6):
+			sw_version = RFC3146_SW_VERSION;
+#endif
+		}
+
 		p = skb_push(ptask->skb, IEEE1394_GASP_HDR_SIZE);
 		put_unaligned_be32(node_id << 16 | IANA_SPECIFIER_ID >> 8, p);
 		put_unaligned_be32((IANA_SPECIFIER_ID & 0xff) << 24
-						| RFC2734_SW_VERSION, &p[4]);
+						| sw_version, &p[4]);
 
 		/* We should not transmit if broadcast_channel.valid == 0. */
 		fw_send_request(dev->card, &ptask->transaction,
@@ -1116,6 +1044,62 @@ static int fwnet_send_packet(struct fwnet_packet_task *ptask)
 	return 0;
 }
 
+static void fwnet_fifo_stop(struct fwnet_device *dev)
+{
+	if (dev->local_fifo == FWNET_NO_FIFO_ADDR)
+		return;
+
+	fw_core_remove_address_handler(&dev->handler);
+	dev->local_fifo = FWNET_NO_FIFO_ADDR;
+}
+
+static int fwnet_fifo_start(struct fwnet_device *dev)
+{
+	int retval;
+
+	if (dev->local_fifo != FWNET_NO_FIFO_ADDR)
+		return 0;
+
+	dev->handler.length = 4096;
+	dev->handler.address_callback = fwnet_receive_packet;
+	dev->handler.callback_data = dev;
+
+	retval = fw_core_add_address_handler(&dev->handler,
+					     &fw_high_memory_region);
+	if (retval < 0)
+		return retval;
+
+	dev->local_fifo = dev->handler.offset;
+
+	return 0;
+}
+
+static void __fwnet_broadcast_stop(struct fwnet_device *dev)
+{
+	unsigned u;
+
+	if (dev->broadcast_state != FWNET_BROADCAST_ERROR) {
+		for (u = 0; u < FWNET_ISO_PAGE_COUNT; u++)
+			kunmap(dev->broadcast_rcv_buffer.pages[u]);
+		fw_iso_buffer_destroy(&dev->broadcast_rcv_buffer, dev->card);
+	}
+	if (dev->broadcast_rcv_context) {
+		fw_iso_context_destroy(dev->broadcast_rcv_context);
+		dev->broadcast_rcv_context = NULL;
+	}
+	kfree(dev->broadcast_rcv_buffer_ptrs);
+	dev->broadcast_rcv_buffer_ptrs = NULL;
+	dev->broadcast_state = FWNET_BROADCAST_ERROR;
+}
+
+static void fwnet_broadcast_stop(struct fwnet_device *dev)
+{
+	if (dev->broadcast_state == FWNET_BROADCAST_ERROR)
+		return;
+	fw_iso_context_stop(dev->broadcast_rcv_context);
+	__fwnet_broadcast_stop(dev);
+}
+
 static int fwnet_broadcast_start(struct fwnet_device *dev)
 {
 	struct fw_iso_context *context;
@@ -1124,60 +1108,47 @@ static int fwnet_broadcast_start(struct fwnet_device *dev)
 	unsigned max_receive;
 	struct fw_iso_packet packet;
 	unsigned long offset;
+	void **ptrptr;
 	unsigned u;
 
-	if (dev->local_fifo == FWNET_NO_FIFO_ADDR) {
-		dev->handler.length = 4096;
-		dev->handler.address_callback = fwnet_receive_packet;
-		dev->handler.callback_data = dev;
-
-		retval = fw_core_add_address_handler(&dev->handler,
-					&fw_high_memory_region);
-		if (retval < 0)
-			goto failed_initial;
-
-		dev->local_fifo = dev->handler.offset;
-	}
+	if (dev->broadcast_state != FWNET_BROADCAST_ERROR)
+		return 0;
 
 	max_receive = 1U << (dev->card->max_receive + 1);
 	num_packets = (FWNET_ISO_PAGE_COUNT * PAGE_SIZE) / max_receive;
 
-	if (!dev->broadcast_rcv_context) {
-		void **ptrptr;
-
-		context = fw_iso_context_create(dev->card,
-		    FW_ISO_CONTEXT_RECEIVE, IEEE1394_BROADCAST_CHANNEL,
-		    dev->card->link_speed, 8, fwnet_receive_broadcast, dev);
-		if (IS_ERR(context)) {
-			retval = PTR_ERR(context);
-			goto failed_context_create;
-		}
+	ptrptr = kmalloc(sizeof(void *) * num_packets, GFP_KERNEL);
+	if (!ptrptr) {
+		retval = -ENOMEM;
+		goto failed;
+	}
+	dev->broadcast_rcv_buffer_ptrs = ptrptr;
+
+	context = fw_iso_context_create(dev->card, FW_ISO_CONTEXT_RECEIVE,
+					IEEE1394_BROADCAST_CHANNEL,
+					dev->card->link_speed, 8,
+					fwnet_receive_broadcast, dev);
+	if (IS_ERR(context)) {
+		retval = PTR_ERR(context);
+		goto failed;
+	}
 
-		retval = fw_iso_buffer_init(&dev->broadcast_rcv_buffer,
-		    dev->card, FWNET_ISO_PAGE_COUNT, DMA_FROM_DEVICE);
-		if (retval < 0)
-			goto failed_buffer_init;
+	retval = fw_iso_buffer_init(&dev->broadcast_rcv_buffer, dev->card,
+				    FWNET_ISO_PAGE_COUNT, DMA_FROM_DEVICE);
+	if (retval < 0)
+		goto failed;
 
-		ptrptr = kmalloc(sizeof(void *) * num_packets, GFP_KERNEL);
-		if (!ptrptr) {
-			retval = -ENOMEM;
-			goto failed_ptrs_alloc;
-		}
+	dev->broadcast_state = FWNET_BROADCAST_STOPPED;
 
-		dev->broadcast_rcv_buffer_ptrs = ptrptr;
-		for (u = 0; u < FWNET_ISO_PAGE_COUNT; u++) {
-			void *ptr;
-			unsigned v;
+	for (u = 0; u < FWNET_ISO_PAGE_COUNT; u++) {
+		void *ptr;
+		unsigned v;
 
-			ptr = kmap(dev->broadcast_rcv_buffer.pages[u]);
-			for (v = 0; v < num_packets / FWNET_ISO_PAGE_COUNT; v++)
-				*ptrptr++ = (void *)
-						((char *)ptr + v * max_receive);
-		}
-		dev->broadcast_rcv_context = context;
-	} else {
-		context = dev->broadcast_rcv_context;
+		ptr = kmap(dev->broadcast_rcv_buffer.pages[u]);
+		for (v = 0; v < num_packets / FWNET_ISO_PAGE_COUNT; v++)
+			*ptrptr++ = (void *) ((char *)ptr + v * max_receive);
 	}
+	dev->broadcast_rcv_context = context;
 
 	packet.payload_length = max_receive;
 	packet.interrupt = 1;
@@ -1191,7 +1162,7 @@ static int fwnet_broadcast_start(struct fwnet_device *dev)
 		retval = fw_iso_context_queue(context, &packet,
 				&dev->broadcast_rcv_buffer, offset);
 		if (retval < 0)
-			goto failed_rcv_queue;
+			goto failed;
 
 		offset += max_receive;
 	}
@@ -1201,7 +1172,7 @@ static int fwnet_broadcast_start(struct fwnet_device *dev)
 	retval = fw_iso_context_start(context, -1, 0,
 			FW_ISO_CONTEXT_MATCH_ALL_TAGS); /* ??? sync */
 	if (retval < 0)
-		goto failed_rcv_queue;
+		goto failed;
 
 	/* FIXME: adjust it according to the min. speed of all known peers? */
 	dev->broadcast_xmt_max_payload = IEEE1394_MAX_PAYLOAD_S100
@@ -1210,19 +1181,8 @@ static int fwnet_broadcast_start(struct fwnet_device *dev)
 
 	return 0;
 
- failed_rcv_queue:
-	kfree(dev->broadcast_rcv_buffer_ptrs);
-	dev->broadcast_rcv_buffer_ptrs = NULL;
- failed_ptrs_alloc:
-	fw_iso_buffer_destroy(&dev->broadcast_rcv_buffer, dev->card);
- failed_buffer_init:
-	fw_iso_context_destroy(context);
-	dev->broadcast_rcv_context = NULL;
- failed_context_create:
-	fw_core_remove_address_handler(&dev->handler);
- failed_initial:
-	dev->local_fifo = FWNET_NO_FIFO_ADDR;
-
+ failed:
+	__fwnet_broadcast_stop(dev);
 	return retval;
 }
 
@@ -1240,11 +1200,10 @@ static int fwnet_open(struct net_device *net)
 	struct fwnet_device *dev = netdev_priv(net);
 	int ret;
 
-	if (dev->broadcast_state == FWNET_BROADCAST_ERROR) {
-		ret = fwnet_broadcast_start(dev);
-		if (ret)
-			return ret;
-	}
+	ret = fwnet_broadcast_start(dev);
+	if (ret)
+		return ret;
+
 	netif_start_queue(net);
 
 	spin_lock_irq(&dev->lock);
@@ -1257,9 +1216,10 @@ static int fwnet_open(struct net_device *net)
 /* ifdown */
 static int fwnet_stop(struct net_device *net)
 {
-	netif_stop_queue(net);
+	struct fwnet_device *dev = netdev_priv(net);
 
-	/* Deallocate iso context for use by other applications? */
+	netif_stop_queue(net);
+	fwnet_broadcast_stop(dev);
 
 	return 0;
 }
@@ -1299,19 +1259,27 @@ static netdev_tx_t fwnet_tx(struct sk_buff *skb, struct net_device *net)
 	 * We might need to rebuild the header on tx failure.
 	 */
 	memcpy(&hdr_buf, skb->data, sizeof(hdr_buf));
-	skb_pull(skb, sizeof(hdr_buf));
-
 	proto = hdr_buf.h_proto;
+
+	switch (proto) {
+	case htons(ETH_P_ARP):
+	case htons(ETH_P_IP):
+#if IS_ENABLED(CONFIG_IPV6)
+	case htons(ETH_P_IPV6):
+#endif
+		break;
+	default:
+		goto fail;
+	}
+
+	skb_pull(skb, sizeof(hdr_buf));
 	dg_size = skb->len;
 
 	/*
 	 * Set the transmission type for the packet.  ARP packets and IP
 	 * broadcast packets are sent via GASP.
 	 */
-	if (memcmp(hdr_buf.h_dest, net->broadcast, FWNET_ALEN) == 0
-	    || proto == htons(ETH_P_ARP)
-	    || (proto == htons(ETH_P_IP)
-		&& IN_MULTICAST(ntohl(ip_hdr(skb)->daddr)))) {
+	if (fwnet_hwaddr_is_multicast(hdr_buf.h_dest)) {
 		max_payload        = dev->broadcast_xmt_max_payload;
 		datagram_label_ptr = &dev->broadcast_xmt_datagramlabel;
 
@@ -1320,11 +1288,12 @@ static netdev_tx_t fwnet_tx(struct sk_buff *skb, struct net_device *net)
 		ptask->dest_node   = IEEE1394_ALL_NODES;
 		ptask->speed       = SCODE_100;
 	} else {
-		__be64 guid = get_unaligned((__be64 *)hdr_buf.h_dest);
+		union fwnet_hwaddr *ha = (union fwnet_hwaddr *)hdr_buf.h_dest;
+		__be64 guid = get_unaligned(&ha->uc.uniq_id);
 		u8 generation;
 
 		peer = fwnet_peer_find_by_guid(dev, be64_to_cpu(guid));
-		if (!peer || peer->fifo == FWNET_NO_FIFO_ADDR)
+		if (!peer)
 			goto fail;
 
 		generation         = peer->generation;
@@ -1332,32 +1301,12 @@ static netdev_tx_t fwnet_tx(struct sk_buff *skb, struct net_device *net)
 		max_payload        = peer->max_payload;
 		datagram_label_ptr = &peer->datagram_label;
 
-		ptask->fifo_addr   = peer->fifo;
+		ptask->fifo_addr   = fwnet_hwaddr_fifo(ha);
 		ptask->generation  = generation;
 		ptask->dest_node   = dest_node;
 		ptask->speed       = peer->speed;
 	}
 
-	/* If this is an ARP packet, convert it */
-	if (proto == htons(ETH_P_ARP)) {
-		struct arphdr *arp = (struct arphdr *)skb->data;
-		unsigned char *arp_ptr = (unsigned char *)(arp + 1);
-		struct rfc2734_arp *arp1394 = (struct rfc2734_arp *)skb->data;
-		__be32 ipaddr;
-
-		ipaddr = get_unaligned((__be32 *)(arp_ptr + FWNET_ALEN));
-
-		arp1394->hw_addr_len    = RFC2734_HW_ADDR_LEN;
-		arp1394->max_rec        = dev->card->max_receive;
-		arp1394->sspd		= dev->card->link_speed;
-
-		put_unaligned_be16(dev->local_fifo >> 32,
-				   &arp1394->fifo_hi);
-		put_unaligned_be32(dev->local_fifo & 0xffffffff,
-				   &arp1394->fifo_lo);
-		put_unaligned(ipaddr, &arp1394->sip);
-	}
-
 	ptask->hdr.w0 = 0;
 	ptask->hdr.w1 = 0;
 	ptask->skb = skb;
@@ -1472,8 +1421,6 @@ static int fwnet_add_peer(struct fwnet_device *dev,
 
 	peer->dev = dev;
 	peer->guid = (u64)device->config_rom[3] << 32 | device->config_rom[4];
-	peer->fifo = FWNET_NO_FIFO_ADDR;
-	peer->ip = 0;
 	INIT_LIST_HEAD(&peer->pd_list);
 	peer->pdg_size = 0;
 	peer->datagram_label = 0;
@@ -1503,6 +1450,7 @@ static int fwnet_probe(struct device *_dev)
 	struct fwnet_device *dev;
 	unsigned max_mtu;
 	int ret;
+	union fwnet_hwaddr *ha;
 
 	mutex_lock(&fwnet_device_mutex);
 
@@ -1533,6 +1481,11 @@ static int fwnet_probe(struct device *_dev)
 	dev->card = card;
 	dev->netdev = net;
 
+	ret = fwnet_fifo_start(dev);
+	if (ret < 0)
+		goto out;
+	dev->local_fifo = dev->handler.offset;
+
 	/*
 	 * Use the RFC 2734 default 1500 octets or the maximum payload
 	 * as initial MTU
@@ -1542,24 +1495,31 @@ static int fwnet_probe(struct device *_dev)
 	net->mtu = min(1500U, max_mtu);
 
 	/* Set our hardware address while we're at it */
-	put_unaligned_be64(card->guid, net->dev_addr);
-	put_unaligned_be64(~0ULL, net->broadcast);
+	ha = (union fwnet_hwaddr *)net->dev_addr;
+	put_unaligned_be64(card->guid, &ha->uc.uniq_id);
+	ha->uc.max_rec = dev->card->max_receive;
+	ha->uc.sspd = dev->card->link_speed;
+	put_unaligned_be16(dev->local_fifo >> 32, &ha->uc.fifo_hi);
+	put_unaligned_be32(dev->local_fifo & 0xffffffff, &ha->uc.fifo_lo);
+
+	memset(net->broadcast, -1, net->addr_len);
+
 	ret = register_netdev(net);
 	if (ret)
 		goto out;
 
 	list_add_tail(&dev->dev_link, &fwnet_device_list);
-	dev_notice(&net->dev, "IPv4 over IEEE 1394 on card %s\n",
+	dev_notice(&net->dev, "IP over IEEE 1394 on card %s\n",
 		   dev_name(card->device));
  have_dev:
 	ret = fwnet_add_peer(dev, unit, device);
 	if (ret && allocated_netdev) {
 		unregister_netdev(net);
 		list_del(&dev->dev_link);
-	}
  out:
-	if (ret && allocated_netdev)
+		fwnet_fifo_stop(dev);
 		free_netdev(net);
+	}
 
 	mutex_unlock(&fwnet_device_mutex);
 
@@ -1592,22 +1552,14 @@ static int fwnet_remove(struct device *_dev)
 	mutex_lock(&fwnet_device_mutex);
 
 	net = dev->netdev;
-	if (net && peer->ip)
-		arp_invalidate(net, peer->ip);
 
 	fwnet_remove_peer(peer, dev);
 
 	if (list_empty(&dev->peer_list)) {
 		unregister_netdev(net);
 
-		if (dev->local_fifo != FWNET_NO_FIFO_ADDR)
-			fw_core_remove_address_handler(&dev->handler);
-		if (dev->broadcast_rcv_context) {
-			fw_iso_context_stop(dev->broadcast_rcv_context);
-			fw_iso_buffer_destroy(&dev->broadcast_rcv_buffer,
-					      dev->card);
-			fw_iso_context_destroy(dev->broadcast_rcv_context);
-		}
+		fwnet_fifo_stop(dev);
+
 		for (i = 0; dev->queued_datagrams && i < 5; i++)
 			ssleep(1);
 		WARN_ON(dev->queued_datagrams);
@@ -1646,6 +1598,14 @@ static const struct ieee1394_device_id fwnet_id_table[] = {
 		.specifier_id = IANA_SPECIFIER_ID,
 		.version      = RFC2734_SW_VERSION,
 	},
+#if IS_ENABLED(CONFIG_IPV6)
+	{
+		.match_flags  = IEEE1394_MATCH_SPECIFIER_ID |
+				IEEE1394_MATCH_VERSION,
+		.specifier_id = IANA_SPECIFIER_ID,
+		.version      = RFC3146_SW_VERSION,
+	},
+#endif
 	{ }
 };
 
@@ -1683,6 +1643,30 @@ static struct fw_descriptor rfc2374_unit_directory = {
 	.data   = rfc2374_unit_directory_data
 };
 
+#if IS_ENABLED(CONFIG_IPV6)
+static const u32 rfc3146_unit_directory_data[] = {
+	0x00040000,	/* directory_length		*/
+	0x1200005e,	/* unit_specifier_id: IANA	*/
+	0x81000003,	/* textual descriptor offset	*/
+	0x13000002,	/* unit_sw_version: RFC 3146	*/
+	0x81000005,	/* textual descriptor offset	*/
+	0x00030000,	/* descriptor_length		*/
+	0x00000000,	/* text				*/
+	0x00000000,	/* minimal ASCII, en		*/
+	0x49414e41,	/* I A N A			*/
+	0x00030000,	/* descriptor_length		*/
+	0x00000000,	/* text				*/
+	0x00000000,	/* minimal ASCII, en		*/
+	0x49507636,	/* I P v 6			*/
+};
+
+static struct fw_descriptor rfc3146_unit_directory = {
+	.length = ARRAY_SIZE(rfc3146_unit_directory_data),
+	.key    = (CSR_DIRECTORY | CSR_UNIT) << 24,
+	.data   = rfc3146_unit_directory_data
+};
+#endif
+
 static int __init fwnet_init(void)
 {
 	int err;
@@ -1691,11 +1675,17 @@ static int __init fwnet_init(void)
 	if (err)
 		return err;
 
+#if IS_ENABLED(CONFIG_IPV6)
+	err = fw_core_add_descriptor(&rfc3146_unit_directory);
+	if (err)
+		goto out;
+#endif
+
 	fwnet_packet_task_cache = kmem_cache_create("packet_task",
 			sizeof(struct fwnet_packet_task), 0, 0, NULL);
 	if (!fwnet_packet_task_cache) {
 		err = -ENOMEM;
-		goto out;
+		goto out2;
 	}
 
 	err = driver_register(&fwnet_driver.driver);
@@ -1703,7 +1693,11 @@ static int __init fwnet_init(void)
 		return 0;
 
 	kmem_cache_destroy(fwnet_packet_task_cache);
+out2:
+#if IS_ENABLED(CONFIG_IPV6)
+	fw_core_remove_descriptor(&rfc3146_unit_directory);
 out:
+#endif
 	fw_core_remove_descriptor(&rfc2374_unit_directory);
 
 	return err;
@@ -1714,11 +1708,14 @@ static void __exit fwnet_cleanup(void)
 {
 	driver_unregister(&fwnet_driver.driver);
 	kmem_cache_destroy(fwnet_packet_task_cache);
+#if IS_ENABLED(CONFIG_IPV6)
+	fw_core_remove_descriptor(&rfc3146_unit_directory);
+#endif
 	fw_core_remove_descriptor(&rfc2374_unit_directory);
 }
 module_exit(fwnet_cleanup);
 
 MODULE_AUTHOR("Jay Fenlason <fenlason@redhat.com>");
-MODULE_DESCRIPTION("IPv4 over IEEE1394 as per RFC 2734");
+MODULE_DESCRIPTION("IP over IEEE1394 as per RFC 2734/3146");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(ieee1394, fwnet_id_table);
diff --git a/drivers/firewire/ohci.c b/drivers/firewire/ohci.c
index 45912e6e0ac2..9e1db6490b9a 100644
--- a/drivers/firewire/ohci.c
+++ b/drivers/firewire/ohci.c
@@ -54,6 +54,10 @@
 #include "core.h"
 #include "ohci.h"
 
+#define ohci_info(ohci, f, args...)	dev_info(ohci->card.device, f, ##args)
+#define ohci_notice(ohci, f, args...)	dev_notice(ohci->card.device, f, ##args)
+#define ohci_err(ohci, f, args...)	dev_err(ohci->card.device, f, ##args)
+
 #define DESCRIPTOR_OUTPUT_MORE		0
 #define DESCRIPTOR_OUTPUT_LAST		(1 << 12)
 #define DESCRIPTOR_INPUT_MORE		(2 << 12)
@@ -68,6 +72,8 @@
 #define DESCRIPTOR_BRANCH_ALWAYS	(3 << 2)
 #define DESCRIPTOR_WAIT			(3 << 0)
 
+#define DESCRIPTOR_CMD			(0xf << 12)
+
 struct descriptor {
 	__le16 req_count;
 	__le16 control;
@@ -149,10 +155,11 @@ struct context {
 	struct descriptor *last;
 
 	/*
-	 * The last descriptor in the DMA program.  It contains the branch
+	 * The last descriptor block in the DMA program. It contains the branch
 	 * address that must be updated upon appending a new descriptor.
 	 */
 	struct descriptor *prev;
+	int prev_z;
 
 	descriptor_callback_t callback;
 
@@ -270,7 +277,9 @@ static char ohci_driver_name[] = KBUILD_MODNAME;
 #define PCI_DEVICE_ID_TI_TSB12LV22	0x8009
 #define PCI_DEVICE_ID_TI_TSB12LV26	0x8020
 #define PCI_DEVICE_ID_TI_TSB82AA2	0x8025
+#define PCI_DEVICE_ID_VIA_VT630X	0x3044
 #define PCI_VENDOR_ID_PINNACLE_SYSTEMS	0x11bd
+#define PCI_REV_ID_VIA_VT6306		0x46
 
 #define QUIRK_CYCLE_TIMER		1
 #define QUIRK_RESET_PACKET		2
@@ -278,6 +287,8 @@ static char ohci_driver_name[] = KBUILD_MODNAME;
 #define QUIRK_NO_1394A			8
 #define QUIRK_NO_MSI			16
 #define QUIRK_TI_SLLZ059		32
+#define QUIRK_IR_WAKE			64
+#define QUIRK_PHY_LCTRL_TIMEOUT		128
 
 /* In case of multiple matches in ohci_quirks[], only the first one is used. */
 static const struct {
@@ -290,7 +301,10 @@ static const struct {
 		QUIRK_BE_HEADERS},
 
 	{PCI_VENDOR_ID_ATT, PCI_DEVICE_ID_AGERE_FW643, 6,
-		QUIRK_NO_MSI},
+		QUIRK_PHY_LCTRL_TIMEOUT | QUIRK_NO_MSI},
+
+	{PCI_VENDOR_ID_ATT, PCI_ANY_ID, PCI_ANY_ID,
+		QUIRK_PHY_LCTRL_TIMEOUT},
 
 	{PCI_VENDOR_ID_CREATIVE, PCI_DEVICE_ID_CREATIVE_SB1394, PCI_ANY_ID,
 		QUIRK_RESET_PACKET},
@@ -319,6 +333,9 @@ static const struct {
 	{PCI_VENDOR_ID_TI, PCI_ANY_ID, PCI_ANY_ID,
 		QUIRK_RESET_PACKET},
 
+	{PCI_VENDOR_ID_VIA, PCI_DEVICE_ID_VIA_VT630X, PCI_REV_ID_VIA_VT6306,
+		QUIRK_CYCLE_TIMER | QUIRK_IR_WAKE},
+
 	{PCI_VENDOR_ID_VIA, PCI_ANY_ID, PCI_ANY_ID,
 		QUIRK_CYCLE_TIMER | QUIRK_NO_MSI},
 };
@@ -333,6 +350,8 @@ MODULE_PARM_DESC(quirks, "Chip quirks (default = 0"
 	", no 1394a enhancements = "	__stringify(QUIRK_NO_1394A)
 	", disable MSI = "		__stringify(QUIRK_NO_MSI)
 	", TI SLLZ059 erratum = "	__stringify(QUIRK_TI_SLLZ059)
+	", IR wake unreliable = "	__stringify(QUIRK_IR_WAKE)
+	", phy LCtrl timeout = "	__stringify(QUIRK_PHY_LCTRL_TIMEOUT)
 	")");
 
 #define OHCI_PARAM_DEBUG_AT_AR		1
@@ -359,8 +378,7 @@ static void log_irqs(struct fw_ohci *ohci, u32 evt)
 	    !(evt & OHCI1394_busReset))
 		return;
 
-	dev_notice(ohci->card.device,
-	    "IRQ %08x%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n", evt,
+	ohci_notice(ohci, "IRQ %08x%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n", evt,
 	    evt & OHCI1394_selfIDComplete	? " selfID"		: "",
 	    evt & OHCI1394_RQPkt		? " AR_req"		: "",
 	    evt & OHCI1394_RSPkt		? " AR_resp"		: "",
@@ -406,21 +424,19 @@ static void log_selfids(struct fw_ohci *ohci, int generation, int self_id_count)
 	if (likely(!(param_debug & OHCI_PARAM_DEBUG_SELFIDS)))
 		return;
 
-	dev_notice(ohci->card.device,
-		   "%d selfIDs, generation %d, local node ID %04x\n",
-		   self_id_count, generation, ohci->node_id);
+	ohci_notice(ohci, "%d selfIDs, generation %d, local node ID %04x\n",
+		    self_id_count, generation, ohci->node_id);
 
 	for (s = ohci->self_id_buffer; self_id_count--; ++s)
 		if ((*s & 1 << 23) == 0)
-			dev_notice(ohci->card.device,
-			    "selfID 0: %08x, phy %d [%c%c%c] "
-			    "%s gc=%d %s %s%s%s\n",
+			ohci_notice(ohci,
+			    "selfID 0: %08x, phy %d [%c%c%c] %s gc=%d %s %s%s%s\n",
 			    *s, *s >> 24 & 63, _p(s, 6), _p(s, 4), _p(s, 2),
 			    speed[*s >> 14 & 3], *s >> 16 & 63,
 			    power[*s >> 8 & 7], *s >> 22 & 1 ? "L" : "",
 			    *s >> 11 & 1 ? "c" : "", *s & 2 ? "i" : "");
 		else
-			dev_notice(ohci->card.device,
+			ohci_notice(ohci,
 			    "selfID n: %08x, phy %d [%c%c%c%c%c%c%c%c]\n",
 			    *s, *s >> 24 & 63,
 			    _p(s, 16), _p(s, 14), _p(s, 12), _p(s, 10),
@@ -470,9 +486,8 @@ static void log_ar_at_event(struct fw_ohci *ohci,
 			evt = 0x1f;
 
 	if (evt == OHCI1394_evt_bus_reset) {
-		dev_notice(ohci->card.device,
-			   "A%c evt_bus_reset, generation %d\n",
-			   dir, (header[2] >> 16) & 0xff);
+		ohci_notice(ohci, "A%c evt_bus_reset, generation %d\n",
+			    dir, (header[2] >> 16) & 0xff);
 		return;
 	}
 
@@ -491,32 +506,26 @@ static void log_ar_at_event(struct fw_ohci *ohci,
 
 	switch (tcode) {
 	case 0xa:
-		dev_notice(ohci->card.device,
-			   "A%c %s, %s\n",
-			   dir, evts[evt], tcodes[tcode]);
+		ohci_notice(ohci, "A%c %s, %s\n",
+			    dir, evts[evt], tcodes[tcode]);
 		break;
 	case 0xe:
-		dev_notice(ohci->card.device,
-			   "A%c %s, PHY %08x %08x\n",
-			   dir, evts[evt], header[1], header[2]);
+		ohci_notice(ohci, "A%c %s, PHY %08x %08x\n",
+			    dir, evts[evt], header[1], header[2]);
 		break;
 	case 0x0: case 0x1: case 0x4: case 0x5: case 0x9:
-		dev_notice(ohci->card.device,
-			   "A%c spd %x tl %02x, "
-			   "%04x -> %04x, %s, "
-			   "%s, %04x%08x%s\n",
-			   dir, speed, header[0] >> 10 & 0x3f,
-			   header[1] >> 16, header[0] >> 16, evts[evt],
-			   tcodes[tcode], header[1] & 0xffff, header[2], specific);
+		ohci_notice(ohci,
+			    "A%c spd %x tl %02x, %04x -> %04x, %s, %s, %04x%08x%s\n",
+			    dir, speed, header[0] >> 10 & 0x3f,
+			    header[1] >> 16, header[0] >> 16, evts[evt],
+			    tcodes[tcode], header[1] & 0xffff, header[2], specific);
 		break;
 	default:
-		dev_notice(ohci->card.device,
-			   "A%c spd %x tl %02x, "
-			   "%04x -> %04x, %s, "
-			   "%s%s\n",
-			   dir, speed, header[0] >> 10 & 0x3f,
-			   header[1] >> 16, header[0] >> 16, evts[evt],
-			   tcodes[tcode], specific);
+		ohci_notice(ohci,
+			    "A%c spd %x tl %02x, %04x -> %04x, %s, %s%s\n",
+			    dir, speed, header[0] >> 10 & 0x3f,
+			    header[1] >> 16, header[0] >> 16, evts[evt],
+			    tcodes[tcode], specific);
 	}
 }
 
@@ -563,7 +572,8 @@ static int read_phy_reg(struct fw_ohci *ohci, int addr)
 		if (i >= 3)
 			msleep(1);
 	}
-	dev_err(ohci->card.device, "failed to read phy reg\n");
+	ohci_err(ohci, "failed to read phy reg %d\n", addr);
+	dump_stack();
 
 	return -EBUSY;
 }
@@ -585,7 +595,8 @@ static int write_phy_reg(const struct fw_ohci *ohci, int addr, u32 val)
 		if (i >= 3)
 			msleep(1);
 	}
-	dev_err(ohci->card.device, "failed to write phy reg\n");
+	ohci_err(ohci, "failed to write phy reg %d, val %u\n", addr, val);
+	dump_stack();
 
 	return -EBUSY;
 }
@@ -690,8 +701,7 @@ static void ar_context_abort(struct ar_context *ctx, const char *error_msg)
 		reg_write(ohci, CONTROL_CLEAR(ctx->regs), CONTEXT_RUN);
 		flush_writes(ohci);
 
-		dev_err(ohci->card.device, "AR error: %s; DMA stopped\n",
-			error_msg);
+		ohci_err(ohci, "AR error: %s; DMA stopped\n", error_msg);
 	}
 	/* FIXME: restart? */
 }
@@ -1157,6 +1167,7 @@ static int context_init(struct context *ctx, struct fw_ohci *ohci,
 	ctx->buffer_tail->used += sizeof(*ctx->buffer_tail->buffer);
 	ctx->last = ctx->buffer_tail->buffer;
 	ctx->prev = ctx->buffer_tail->buffer;
+	ctx->prev_z = 1;
 
 	return 0;
 }
@@ -1221,14 +1232,35 @@ static void context_append(struct context *ctx,
 {
 	dma_addr_t d_bus;
 	struct descriptor_buffer *desc = ctx->buffer_tail;
+	struct descriptor *d_branch;
 
 	d_bus = desc->buffer_bus + (d - desc->buffer) * sizeof(*d);
 
 	desc->used += (z + extra) * sizeof(*d);
 
 	wmb(); /* finish init of new descriptors before branch_address update */
-	ctx->prev->branch_address = cpu_to_le32(d_bus | z);
-	ctx->prev = find_branch_descriptor(d, z);
+
+	d_branch = find_branch_descriptor(ctx->prev, ctx->prev_z);
+	d_branch->branch_address = cpu_to_le32(d_bus | z);
+
+	/*
+	 * VT6306 incorrectly checks only the single descriptor at the
+	 * CommandPtr when the wake bit is written, so if it's a
+	 * multi-descriptor block starting with an INPUT_MORE, put a copy of
+	 * the branch address in the first descriptor.
+	 *
+	 * Not doing this for transmit contexts since not sure how it interacts
+	 * with skip addresses.
+	 */
+	if (unlikely(ctx->ohci->quirks & QUIRK_IR_WAKE) &&
+	    d_branch != ctx->prev &&
+	    (ctx->prev->control & cpu_to_le16(DESCRIPTOR_CMD)) ==
+	     cpu_to_le16(DESCRIPTOR_INPUT_MORE)) {
+		ctx->prev->branch_address = cpu_to_le32(d_bus | z);
+	}
+
+	ctx->prev = d;
+	ctx->prev_z = z;
 }
 
 static void context_stop(struct context *ctx)
@@ -1248,7 +1280,7 @@ static void context_stop(struct context *ctx)
 		if (i)
 			udelay(10);
 	}
-	dev_err(ohci->card.device, "DMA context still active (0x%08x)\n", reg);
+	ohci_err(ohci, "DMA context still active (0x%08x)\n", reg);
 }
 
 struct driver_data {
@@ -1557,7 +1589,7 @@ static void handle_local_lock(struct fw_ohci *ohci,
 			goto out;
 		}
 
-	dev_err(ohci->card.device, "swap not done (CSR lock timeout)\n");
+	ohci_err(ohci, "swap not done (CSR lock timeout)\n");
 	fw_fill_response(&response, packet->header, RCODE_BUSY, NULL, 0);
 
  out:
@@ -1632,8 +1664,7 @@ static void detect_dead_context(struct fw_ohci *ohci,
 
 	ctl = reg_read(ohci, CONTROL_SET(regs));
 	if (ctl & CONTEXT_DEAD)
-		dev_err(ohci->card.device,
-			"DMA context %s has stopped, error code: %s\n",
+		ohci_err(ohci, "DMA context %s has stopped, error code: %s\n",
 			name, evts[ctl & 0x1f]);
 }
 
@@ -1815,8 +1846,8 @@ static int find_and_insert_self_id(struct fw_ohci *ohci, int self_id_count)
 
 	reg = reg_read(ohci, OHCI1394_NodeID);
 	if (!(reg & OHCI1394_NodeID_idValid)) {
-		dev_notice(ohci->card.device,
-			   "node ID not valid, new bus reset in progress\n");
+		ohci_notice(ohci,
+			    "node ID not valid, new bus reset in progress\n");
 		return -EBUSY;
 	}
 	self_id |= ((reg & 0x3f) << 24); /* phy ID */
@@ -1863,12 +1894,12 @@ static void bus_reset_work(struct work_struct *work)
 
 	reg = reg_read(ohci, OHCI1394_NodeID);
 	if (!(reg & OHCI1394_NodeID_idValid)) {
-		dev_notice(ohci->card.device,
-			   "node ID not valid, new bus reset in progress\n");
+		ohci_notice(ohci,
+			    "node ID not valid, new bus reset in progress\n");
 		return;
 	}
 	if ((reg & OHCI1394_NodeID_nodeNumber) == 63) {
-		dev_notice(ohci->card.device, "malconfigured bus\n");
+		ohci_notice(ohci, "malconfigured bus\n");
 		return;
 	}
 	ohci->node_id = reg & (OHCI1394_NodeID_busNumber |
@@ -1882,7 +1913,7 @@ static void bus_reset_work(struct work_struct *work)
 
 	reg = reg_read(ohci, OHCI1394_SelfIDCount);
 	if (reg & OHCI1394_SelfIDCount_selfIDError) {
-		dev_notice(ohci->card.device, "inconsistent self IDs\n");
+		ohci_notice(ohci, "self ID receive error\n");
 		return;
 	}
 	/*
@@ -1894,7 +1925,7 @@ static void bus_reset_work(struct work_struct *work)
 	self_id_count = (reg >> 3) & 0xff;
 
 	if (self_id_count > 252) {
-		dev_notice(ohci->card.device, "inconsistent self IDs\n");
+		ohci_notice(ohci, "bad selfIDSize (%08x)\n", reg);
 		return;
 	}
 
@@ -1902,7 +1933,10 @@ static void bus_reset_work(struct work_struct *work)
 	rmb();
 
 	for (i = 1, j = 0; j < self_id_count; i += 2, j++) {
-		if (ohci->self_id_cpu[i] != ~ohci->self_id_cpu[i + 1]) {
+		u32 id  = cond_le32_to_cpu(ohci->self_id_cpu[i]);
+		u32 id2 = cond_le32_to_cpu(ohci->self_id_cpu[i + 1]);
+
+		if (id != ~id2) {
 			/*
 			 * If the invalid data looks like a cycle start packet,
 			 * it's likely to be the result of the cycle master
@@ -1910,33 +1944,30 @@ static void bus_reset_work(struct work_struct *work)
 			 * so far are valid and should be processed so that the
 			 * bus manager can then correct the gap count.
 			 */
-			if (cond_le32_to_cpu(ohci->self_id_cpu[i])
-							== 0xffff008f) {
-				dev_notice(ohci->card.device,
-					   "ignoring spurious self IDs\n");
+			if (id == 0xffff008f) {
+				ohci_notice(ohci, "ignoring spurious self IDs\n");
 				self_id_count = j;
 				break;
-			} else {
-				dev_notice(ohci->card.device,
-					   "inconsistent self IDs\n");
-				return;
 			}
+
+			ohci_notice(ohci, "bad self ID %d/%d (%08x != ~%08x)\n",
+				    j, self_id_count, id, id2);
+			return;
 		}
-		ohci->self_id_buffer[j] =
-				cond_le32_to_cpu(ohci->self_id_cpu[i]);
+		ohci->self_id_buffer[j] = id;
 	}
 
 	if (ohci->quirks & QUIRK_TI_SLLZ059) {
 		self_id_count = find_and_insert_self_id(ohci, self_id_count);
 		if (self_id_count < 0) {
-			dev_notice(ohci->card.device,
-				   "could not construct local self ID\n");
+			ohci_notice(ohci,
+				    "could not construct local self ID\n");
 			return;
 		}
 	}
 
 	if (self_id_count == 0) {
-		dev_notice(ohci->card.device, "inconsistent self IDs\n");
+		ohci_notice(ohci, "no self IDs\n");
 		return;
 	}
 	rmb();
@@ -1957,8 +1988,7 @@ static void bus_reset_work(struct work_struct *work)
 
 	new_generation = (reg_read(ohci, OHCI1394_SelfIDCount) >> 16) & 0xff;
 	if (new_generation != generation) {
-		dev_notice(ohci->card.device,
-			   "new bus reset, discarding self ids\n");
+		ohci_notice(ohci, "new bus reset, discarding self ids\n");
 		return;
 	}
 
@@ -2096,7 +2126,7 @@ static irqreturn_t irq_handler(int irq, void *data)
 	}
 
 	if (unlikely(event & OHCI1394_regAccessFail))
-		dev_err(ohci->card.device, "register access failure\n");
+		ohci_err(ohci, "register access failure\n");
 
 	if (unlikely(event & OHCI1394_postedWriteErr)) {
 		reg_read(ohci, OHCI1394_PostedWriteAddressHi);
@@ -2104,13 +2134,12 @@ static irqreturn_t irq_handler(int irq, void *data)
 		reg_write(ohci, OHCI1394_IntEventClear,
 			  OHCI1394_postedWriteErr);
 		if (printk_ratelimit())
-			dev_err(ohci->card.device, "PCI posted write error\n");
+			ohci_err(ohci, "PCI posted write error\n");
 	}
 
 	if (unlikely(event & OHCI1394_cycleTooLong)) {
 		if (printk_ratelimit())
-			dev_notice(ohci->card.device,
-				   "isochronous cycle too long\n");
+			ohci_notice(ohci, "isochronous cycle too long\n");
 		reg_write(ohci, OHCI1394_LinkControlSet,
 			  OHCI1394_LinkControl_cycleMaster);
 	}
@@ -2123,8 +2152,7 @@ static irqreturn_t irq_handler(int irq, void *data)
 		 * them at least two cycles later.  (FIXME?)
 		 */
 		if (printk_ratelimit())
-			dev_notice(ohci->card.device,
-				   "isochronous cycle inconsistent\n");
+			ohci_notice(ohci, "isochronous cycle inconsistent\n");
 	}
 
 	if (unlikely(event & OHCI1394_unrecoverableError))
@@ -2246,12 +2274,11 @@ static int ohci_enable(struct fw_card *card,
 		       const __be32 *config_rom, size_t length)
 {
 	struct fw_ohci *ohci = fw_ohci(card);
-	struct pci_dev *dev = to_pci_dev(card->device);
 	u32 lps, version, irqs;
 	int i, ret;
 
 	if (software_reset(ohci)) {
-		dev_err(card->device, "failed to reset ohci card\n");
+		ohci_err(ohci, "failed to reset ohci card\n");
 		return -EBUSY;
 	}
 
@@ -2262,20 +2289,31 @@ static int ohci_enable(struct fw_card *card,
 	 * will lock up the machine.  Wait 50msec to make sure we have
 	 * full link enabled.  However, with some cards (well, at least
 	 * a JMicron PCIe card), we have to try again sometimes.
+	 *
+	 * TI TSB82AA2 + TSB81BA3(A) cards signal LPS enabled early but
+	 * cannot actually use the phy at that time.  These need tens of
+	 * millisecods pause between LPS write and first phy access too.
+	 *
+	 * But do not wait for 50msec on Agere/LSI cards.  Their phy
+	 * arbitration state machine may time out during such a long wait.
 	 */
+
 	reg_write(ohci, OHCI1394_HCControlSet,
 		  OHCI1394_HCControl_LPS |
 		  OHCI1394_HCControl_postedWriteEnable);
 	flush_writes(ohci);
 
-	for (lps = 0, i = 0; !lps && i < 3; i++) {
+	if (!(ohci->quirks & QUIRK_PHY_LCTRL_TIMEOUT))
 		msleep(50);
+
+	for (lps = 0, i = 0; !lps && i < 150; i++) {
+		msleep(1);
 		lps = reg_read(ohci, OHCI1394_HCControlSet) &
 		      OHCI1394_HCControl_LPS;
 	}
 
 	if (!lps) {
-		dev_err(card->device, "failed to set Link Power Status\n");
+		ohci_err(ohci, "failed to set Link Power Status\n");
 		return -EIO;
 	}
 
@@ -2284,7 +2322,7 @@ static int ohci_enable(struct fw_card *card,
 		if (ret < 0)
 			return ret;
 		if (ret)
-			dev_notice(card->device, "local TSB41BA3D phy\n");
+			ohci_notice(ohci, "local TSB41BA3D phy\n");
 		else
 			ohci->quirks &= ~QUIRK_TI_SLLZ059;
 	}
@@ -2382,24 +2420,6 @@ static int ohci_enable(struct fw_card *card,
 
 	reg_write(ohci, OHCI1394_AsReqFilterHiSet, 0x80000000);
 
-	if (!(ohci->quirks & QUIRK_NO_MSI))
-		pci_enable_msi(dev);
-	if (request_irq(dev->irq, irq_handler,
-			pci_dev_msi_enabled(dev) ? 0 : IRQF_SHARED,
-			ohci_driver_name, ohci)) {
-		dev_err(card->device, "failed to allocate interrupt %d\n",
-			dev->irq);
-		pci_disable_msi(dev);
-
-		if (config_rom) {
-			dma_free_coherent(ohci->card.device, CONFIG_ROM_SIZE,
-					  ohci->next_config_rom,
-					  ohci->next_config_rom_bus);
-			ohci->next_config_rom = NULL;
-		}
-		return -EIO;
-	}
-
 	irqs =	OHCI1394_reqTxComplete | OHCI1394_respTxComplete |
 		OHCI1394_RQPkt | OHCI1394_RSPkt |
 		OHCI1394_isochTx | OHCI1394_isochRx |
@@ -3578,20 +3598,20 @@ static int pci_probe(struct pci_dev *dev,
 
 	if (!(pci_resource_flags(dev, 0) & IORESOURCE_MEM) ||
 	    pci_resource_len(dev, 0) < OHCI1394_REGISTER_SIZE) {
-		dev_err(&dev->dev, "invalid MMIO resource\n");
+		ohci_err(ohci, "invalid MMIO resource\n");
 		err = -ENXIO;
 		goto fail_disable;
 	}
 
 	err = pci_request_region(dev, 0, ohci_driver_name);
 	if (err) {
-		dev_err(&dev->dev, "MMIO resource unavailable\n");
+		ohci_err(ohci, "MMIO resource unavailable\n");
 		goto fail_disable;
 	}
 
 	ohci->registers = pci_iomap(dev, 0, OHCI1394_REGISTER_SIZE);
 	if (ohci->registers == NULL) {
-		dev_err(&dev->dev, "failed to remap registers\n");
+		ohci_err(ohci, "failed to remap registers\n");
 		err = -ENXIO;
 		goto fail_iomem;
 	}
@@ -3675,19 +3695,33 @@ static int pci_probe(struct pci_dev *dev,
 	guid = ((u64) reg_read(ohci, OHCI1394_GUIDHi) << 32) |
 		reg_read(ohci, OHCI1394_GUIDLo);
 
+	if (!(ohci->quirks & QUIRK_NO_MSI))
+		pci_enable_msi(dev);
+	if (request_irq(dev->irq, irq_handler,
+			pci_dev_msi_enabled(dev) ? 0 : IRQF_SHARED,
+			ohci_driver_name, ohci)) {
+		ohci_err(ohci, "failed to allocate interrupt %d\n", dev->irq);
+		err = -EIO;
+		goto fail_msi;
+	}
+
 	err = fw_card_add(&ohci->card, max_receive, link_speed, guid);
 	if (err)
-		goto fail_contexts;
+		goto fail_irq;
 
 	version = reg_read(ohci, OHCI1394_Version) & 0x00ff00ff;
-	dev_notice(&dev->dev,
-		  "added OHCI v%x.%x device as card %d, "
-		  "%d IR + %d IT contexts, quirks 0x%x\n",
-		  version >> 16, version & 0xff, ohci->card.index,
-		  ohci->n_ir, ohci->n_it, ohci->quirks);
+	ohci_notice(ohci,
+		    "added OHCI v%x.%x device as card %d, "
+		    "%d IR + %d IT contexts, quirks 0x%x\n",
+		    version >> 16, version & 0xff, ohci->card.index,
+		    ohci->n_ir, ohci->n_it, ohci->quirks);
 
 	return 0;
 
+ fail_irq:
+	free_irq(dev->irq, ohci);
+ fail_msi:
+	pci_disable_msi(dev);
  fail_contexts:
 	kfree(ohci->ir_context_list);
 	kfree(ohci->it_context_list);
@@ -3711,19 +3745,21 @@ static int pci_probe(struct pci_dev *dev,
 	kfree(ohci);
 	pmac_ohci_off(dev);
  fail:
-	if (err == -ENOMEM)
-		dev_err(&dev->dev, "out of memory\n");
-
 	return err;
 }
 
 static void pci_remove(struct pci_dev *dev)
 {
-	struct fw_ohci *ohci;
+	struct fw_ohci *ohci = pci_get_drvdata(dev);
 
-	ohci = pci_get_drvdata(dev);
-	reg_write(ohci, OHCI1394_IntMaskClear, ~0);
-	flush_writes(ohci);
+	/*
+	 * If the removal is happening from the suspend state, LPS won't be
+	 * enabled and host registers (eg., IntMaskClear) won't be accessible.
+	 */
+	if (reg_read(ohci, OHCI1394_HCControlSet) & OHCI1394_HCControl_LPS) {
+		reg_write(ohci, OHCI1394_IntMaskClear, ~0);
+		flush_writes(ohci);
+	}
 	cancel_work_sync(&ohci->bus_reset_work);
 	fw_core_remove_card(&ohci->card);
 
@@ -3766,16 +3802,14 @@ static int pci_suspend(struct pci_dev *dev, pm_message_t state)
 	int err;
 
 	software_reset(ohci);
-	free_irq(dev->irq, ohci);
-	pci_disable_msi(dev);
 	err = pci_save_state(dev);
 	if (err) {
-		dev_err(&dev->dev, "pci_save_state failed\n");
+		ohci_err(ohci, "pci_save_state failed\n");
 		return err;
 	}
 	err = pci_set_power_state(dev, pci_choose_state(dev, state));
 	if (err)
-		dev_err(&dev->dev, "pci_set_power_state failed with %d\n", err);
+		ohci_err(ohci, "pci_set_power_state failed with %d\n", err);
 	pmac_ohci_off(dev);
 
 	return 0;
@@ -3791,7 +3825,7 @@ static int pci_resume(struct pci_dev *dev)
 	pci_restore_state(dev);
 	err = pci_enable_device(dev);
 	if (err) {
-		dev_err(&dev->dev, "pci_enable_device failed\n");
+		ohci_err(ohci, "pci_enable_device failed\n");
 		return err;
 	}
 
@@ -3837,6 +3871,4 @@ MODULE_DESCRIPTION("Driver for PCI OHCI IEEE1394 controllers");
 MODULE_LICENSE("GPL");
 
 /* Provide a module alias so root-on-sbp2 initrds don't break. */
-#ifndef CONFIG_IEEE1394_OHCI1394_MODULE
 MODULE_ALIAS("ohci1394");
-#endif
diff --git a/drivers/firewire/sbp2.c b/drivers/firewire/sbp2.c
index 1162d6b3bf85..47674b913843 100644
--- a/drivers/firewire/sbp2.c
+++ b/drivers/firewire/sbp2.c
@@ -1144,8 +1144,8 @@ static int sbp2_probe(struct device *dev)
 		return -ENODEV;
 
 	if (dma_get_max_seg_size(device->card->device) > SBP2_MAX_SEG_SIZE)
-		BUG_ON(dma_set_max_seg_size(device->card->device,
-					    SBP2_MAX_SEG_SIZE));
+		WARN_ON(dma_set_max_seg_size(device->card->device,
+					     SBP2_MAX_SEG_SIZE));
 
 	shost = scsi_host_alloc(&scsi_driver_template, sizeof(*tgt));
 	if (shost == NULL)
@@ -1475,10 +1475,8 @@ static int sbp2_scsi_queuecommand(struct Scsi_Host *shost,
 	}
 
 	orb = kzalloc(sizeof(*orb), GFP_ATOMIC);
-	if (orb == NULL) {
-		dev_notice(lu_dev(lu), "failed to alloc ORB\n");
+	if (orb == NULL)
 		return SCSI_MLQUEUE_HOST_BUSY;
-	}
 
 	/* Initialize rcode to something not RCODE_COMPLETE. */
 	orb->base.rcode = -1;
@@ -1636,9 +1634,7 @@ MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(ieee1394, sbp2_id_table);
 
 /* Provide a module alias so root-on-sbp2 initrds don't break. */
-#ifndef CONFIG_IEEE1394_SBP2_MODULE
 MODULE_ALIAS("sbp2");
-#endif
 
 static int __init sbp2_init(void)
 {
diff --git a/drivers/firmware/Kconfig b/drivers/firmware/Kconfig
index 3e532002e4d1..93876302fb2e 100644
--- a/drivers/firmware/Kconfig
+++ b/drivers/firmware/Kconfig
@@ -36,42 +36,6 @@ config FIRMWARE_MEMMAP
 
       See also Documentation/ABI/testing/sysfs-firmware-memmap.
 
-config EFI_VARS
-	tristate "EFI Variable Support via sysfs"
-	depends on EFI
-	select UCS2_STRING
-	default n
-	help
-	  If you say Y here, you are able to get EFI (Extensible Firmware
-	  Interface) variable information via sysfs.  You may read,
-	  write, create, and destroy EFI variables through this interface.
-
-	  Note that using this driver in concert with efibootmgr requires
-	  at least test release version 0.5.0-test3 or later, which is
-	  available from Matt Domsch's website located at:
-	  <http://linux.dell.com/efibootmgr/testing/efibootmgr-0.5.0-test3.tar.gz>
-
-	  Subsequent efibootmgr releases may be found at:
-	  <http://linux.dell.com/efibootmgr>
-
-config EFI_VARS_PSTORE
-	bool "Register efivars backend for pstore"
-	depends on EFI_VARS && PSTORE
-	default y
-	help
-	  Say Y here to enable use efivars as a backend to pstore. This
-	  will allow writing console messages, crash dumps, or anything
-	  else supported by pstore to EFI variables.
-
-config EFI_VARS_PSTORE_DEFAULT_DISABLE
-	bool "Disable using efivars as a pstore backend by default"
-	depends on EFI_VARS_PSTORE
-	default n
-	help
-	  Saying Y here will disable the use of efivars as a storage
-	  backend for pstore by default. This setting can be overridden
-	  using the efivars module's pstore_disable parameter.
-
 config EFI_PCDP
 	bool "Console device selection via EFI PCDP or HCDP table"
 	depends on ACPI && EFI && IA64
@@ -165,5 +129,6 @@ config ISCSI_IBFT
 	  Otherwise, say N.
 
 source "drivers/firmware/google/Kconfig"
+source "drivers/firmware/efi/Kconfig"
 
 endmenu
diff --git a/drivers/firmware/Makefile b/drivers/firmware/Makefile
index 5a7e27399729..299fad6b5867 100644
--- a/drivers/firmware/Makefile
+++ b/drivers/firmware/Makefile
@@ -4,7 +4,6 @@
 obj-$(CONFIG_DMI)		+= dmi_scan.o
 obj-$(CONFIG_DMI_SYSFS)		+= dmi-sysfs.o
 obj-$(CONFIG_EDD)		+= edd.o
-obj-$(CONFIG_EFI_VARS)		+= efivars.o
 obj-$(CONFIG_EFI_PCDP)		+= pcdp.o
 obj-$(CONFIG_DELL_RBU)          += dell_rbu.o
 obj-$(CONFIG_DCDBAS)		+= dcdbas.o
@@ -14,3 +13,4 @@ obj-$(CONFIG_ISCSI_IBFT)	+= iscsi_ibft.o
 obj-$(CONFIG_FIRMWARE_MEMMAP)	+= memmap.o
 
 obj-$(CONFIG_GOOGLE_FIRMWARE)	+= google/
+obj-$(CONFIG_EFI)		+= efi/
diff --git a/drivers/firmware/dmi_scan.c b/drivers/firmware/dmi_scan.c
index 4cd392dbf115..b95159b33c39 100644
--- a/drivers/firmware/dmi_scan.c
+++ b/drivers/firmware/dmi_scan.c
@@ -22,6 +22,9 @@ static u16 __initdata dmi_ver;
  */
 static int dmi_initialized;
 
+/* DMI system identification string used during boot */
+static char dmi_ids_string[128] __initdata;
+
 static const char * __init dmi_string_nosave(const struct dmi_header *dm, u8 s)
 {
 	const u8 *bp = ((u8 *) dm) + dm->length;
@@ -376,99 +379,103 @@ static void __init dmi_decode(const struct dmi_header *dm, void *dummy)
 	}
 }
 
-static void __init print_filtered(const char *info)
+static int __init print_filtered(char *buf, size_t len, const char *info)
 {
+	int c = 0;
 	const char *p;
 
 	if (!info)
-		return;
+		return c;
 
 	for (p = info; *p; p++)
 		if (isprint(*p))
-			printk(KERN_CONT "%c", *p);
+			c += scnprintf(buf + c, len - c, "%c", *p);
 		else
-			printk(KERN_CONT "\\x%02x", *p & 0xff);
+			c += scnprintf(buf + c, len - c, "\\x%02x", *p & 0xff);
+	return c;
 }
 
-static void __init dmi_dump_ids(void)
+static void __init dmi_format_ids(char *buf, size_t len)
 {
+	int c = 0;
 	const char *board;	/* Board Name is optional */
 
-	printk(KERN_DEBUG "DMI: ");
-	print_filtered(dmi_get_system_info(DMI_SYS_VENDOR));
-	printk(KERN_CONT " ");
-	print_filtered(dmi_get_system_info(DMI_PRODUCT_NAME));
+	c += print_filtered(buf + c, len - c,
+			    dmi_get_system_info(DMI_SYS_VENDOR));
+	c += scnprintf(buf + c, len - c, " ");
+	c += print_filtered(buf + c, len - c,
+			    dmi_get_system_info(DMI_PRODUCT_NAME));
+
 	board = dmi_get_system_info(DMI_BOARD_NAME);
 	if (board) {
-		printk(KERN_CONT "/");
-		print_filtered(board);
+		c += scnprintf(buf + c, len - c, "/");
+		c += print_filtered(buf + c, len - c, board);
 	}
-	printk(KERN_CONT ", BIOS ");
-	print_filtered(dmi_get_system_info(DMI_BIOS_VERSION));
-	printk(KERN_CONT " ");
-	print_filtered(dmi_get_system_info(DMI_BIOS_DATE));
-	printk(KERN_CONT "\n");
+	c += scnprintf(buf + c, len - c, ", BIOS ");
+	c += print_filtered(buf + c, len - c,
+			    dmi_get_system_info(DMI_BIOS_VERSION));
+	c += scnprintf(buf + c, len - c, " ");
+	c += print_filtered(buf + c, len - c,
+			    dmi_get_system_info(DMI_BIOS_DATE));
 }
 
-static int __init dmi_present(const char __iomem *p)
+static int __init dmi_present(const u8 *buf)
 {
-	u8 buf[15];
+	int smbios_ver;
+
+	if (memcmp(buf, "_SM_", 4) == 0 &&
+	    buf[5] < 32 && dmi_checksum(buf, buf[5])) {
+		smbios_ver = (buf[6] << 8) + buf[7];
+
+		/* Some BIOS report weird SMBIOS version, fix that up */
+		switch (smbios_ver) {
+		case 0x021F:
+		case 0x0221:
+			pr_debug("SMBIOS version fixup(2.%d->2.%d)\n",
+				 smbios_ver & 0xFF, 3);
+			smbios_ver = 0x0203;
+			break;
+		case 0x0233:
+			pr_debug("SMBIOS version fixup(2.%d->2.%d)\n", 51, 6);
+			smbios_ver = 0x0206;
+			break;
+		}
+	} else {
+		smbios_ver = 0;
+	}
 
-	memcpy_fromio(buf, p, 15);
-	if (dmi_checksum(buf, 15)) {
+	buf += 16;
+
+	if (memcmp(buf, "_DMI_", 5) == 0 && dmi_checksum(buf, 15)) {
 		dmi_num = (buf[13] << 8) | buf[12];
 		dmi_len = (buf[7] << 8) | buf[6];
 		dmi_base = (buf[11] << 24) | (buf[10] << 16) |
 			(buf[9] << 8) | buf[8];
 
 		if (dmi_walk_early(dmi_decode) == 0) {
-			if (dmi_ver)
+			if (smbios_ver) {
+				dmi_ver = smbios_ver;
 				pr_info("SMBIOS %d.%d present.\n",
 				       dmi_ver >> 8, dmi_ver & 0xFF);
-			else {
+			} else {
 				dmi_ver = (buf[14] & 0xF0) << 4 |
 					   (buf[14] & 0x0F);
 				pr_info("Legacy DMI %d.%d present.\n",
 				       dmi_ver >> 8, dmi_ver & 0xFF);
 			}
-			dmi_dump_ids();
+			dmi_format_ids(dmi_ids_string, sizeof(dmi_ids_string));
+			printk(KERN_DEBUG "DMI: %s\n", dmi_ids_string);
 			return 0;
 		}
 	}
-	dmi_ver = 0;
-	return 1;
-}
-
-static int __init smbios_present(const char __iomem *p)
-{
-	u8 buf[32];
 
-	memcpy_fromio(buf, p, 32);
-	if ((buf[5] < 32) && dmi_checksum(buf, buf[5])) {
-		dmi_ver = (buf[6] << 8) + buf[7];
-
-		/* Some BIOS report weird SMBIOS version, fix that up */
-		switch (dmi_ver) {
-		case 0x021F:
-		case 0x0221:
-			pr_debug("SMBIOS version fixup(2.%d->2.%d)\n",
-			       dmi_ver & 0xFF, 3);
-			dmi_ver = 0x0203;
-			break;
-		case 0x0233:
-			pr_debug("SMBIOS version fixup(2.%d->2.%d)\n", 51, 6);
-			dmi_ver = 0x0206;
-			break;
-		}
-		return memcmp(p + 16, "_DMI_", 5) || dmi_present(p + 16);
-	}
 	return 1;
 }
 
 void __init dmi_scan_machine(void)
 {
 	char __iomem *p, *q;
-	int rc;
+	char buf[32];
 
 	if (efi_enabled(EFI_CONFIG_TABLES)) {
 		if (efi.smbios == EFI_INVALID_TABLE_ADDR)
@@ -481,10 +488,10 @@ void __init dmi_scan_machine(void)
 		p = dmi_ioremap(efi.smbios, 32);
 		if (p == NULL)
 			goto error;
-
-		rc = smbios_present(p);
+		memcpy_fromio(buf, p, 32);
 		dmi_iounmap(p, 32);
-		if (!rc) {
+
+		if (!dmi_present(buf)) {
 			dmi_available = 1;
 			goto out;
 		}
@@ -499,18 +506,15 @@ void __init dmi_scan_machine(void)
 		if (p == NULL)
 			goto error;
 
+		memset(buf, 0, 16);
 		for (q = p; q < p + 0x10000; q += 16) {
-			if (memcmp(q, "_SM_", 4) == 0 && q - p <= 0xFFE0)
-				rc = smbios_present(q);
-			else if (memcmp(q, "_DMI_", 5) == 0)
-				rc = dmi_present(q);
-			else
-				continue;
-			if (!rc) {
+			memcpy_fromio(buf + 16, q, 16);
+			if (!dmi_present(buf)) {
 				dmi_available = 1;
 				dmi_iounmap(p, 0x10000);
 				goto out;
 			}
+			memcpy(buf, buf + 16, 16);
 		}
 		dmi_iounmap(p, 0x10000);
 	}
@@ -521,6 +525,19 @@ void __init dmi_scan_machine(void)
 }
 
 /**
+ * dmi_set_dump_stack_arch_desc - set arch description for dump_stack()
+ *
+ * Invoke dump_stack_set_arch_desc() with DMI system information so that
+ * DMI identifiers are printed out on task dumps.  Arch boot code should
+ * call this function after dmi_scan_machine() if it wants to print out DMI
+ * identifiers on task dumps.
+ */
+void __init dmi_set_dump_stack_arch_desc(void)
+{
+	dump_stack_set_arch_desc("%s", dmi_ids_string);
+}
+
+/**
  *	dmi_matches - check if dmi_system_id structure matches system DMI data
  *	@dmi: pointer to the dmi_system_id structure to check
  */
diff --git a/drivers/firmware/efi/Kconfig b/drivers/firmware/efi/Kconfig
new file mode 100644
index 000000000000..b0fc7c79dfbb
--- /dev/null
+++ b/drivers/firmware/efi/Kconfig
@@ -0,0 +1,39 @@
+menu "EFI (Extensible Firmware Interface) Support"
+	depends on EFI
+
+config EFI_VARS
+	tristate "EFI Variable Support via sysfs"
+	depends on EFI
+	default n
+	help
+	  If you say Y here, you are able to get EFI (Extensible Firmware
+	  Interface) variable information via sysfs.  You may read,
+	  write, create, and destroy EFI variables through this interface.
+
+	  Note that using this driver in concert with efibootmgr requires
+	  at least test release version 0.5.0-test3 or later, which is
+	  available from Matt Domsch's website located at:
+	  <http://linux.dell.com/efibootmgr/testing/efibootmgr-0.5.0-test3.tar.gz>
+
+	  Subsequent efibootmgr releases may be found at:
+	  <http://linux.dell.com/efibootmgr>
+
+config EFI_VARS_PSTORE
+	tristate "Register efivars backend for pstore"
+	depends on EFI_VARS && PSTORE
+	default y
+	help
+	  Say Y here to enable use efivars as a backend to pstore. This
+	  will allow writing console messages, crash dumps, or anything
+	  else supported by pstore to EFI variables.
+
+config EFI_VARS_PSTORE_DEFAULT_DISABLE
+	bool "Disable using efivars as a pstore backend by default"
+	depends on EFI_VARS_PSTORE
+	default n
+	help
+	  Saying Y here will disable the use of efivars as a storage
+	  backend for pstore by default. This setting can be overridden
+	  using the efivars module's pstore_disable parameter.
+
+endmenu
diff --git a/drivers/firmware/efi/Makefile b/drivers/firmware/efi/Makefile
new file mode 100644
index 000000000000..99245ab5a79c
--- /dev/null
+++ b/drivers/firmware/efi/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for linux kernel
+#
+obj-y					+= efi.o vars.o
+obj-$(CONFIG_EFI_VARS)			+= efivars.o
+obj-$(CONFIG_EFI_VARS_PSTORE)		+= efi-pstore.o
diff --git a/drivers/firmware/efi/efi-pstore.c b/drivers/firmware/efi/efi-pstore.c
new file mode 100644
index 000000000000..202d2c85ba2e
--- /dev/null
+++ b/drivers/firmware/efi/efi-pstore.c
@@ -0,0 +1,252 @@
+#include <linux/efi.h>
+#include <linux/module.h>
+#include <linux/pstore.h>
+#include <linux/slab.h>
+#include <linux/ucs2_string.h>
+
+#define DUMP_NAME_LEN 52
+
+static bool efivars_pstore_disable =
+	IS_ENABLED(CONFIG_EFI_VARS_PSTORE_DEFAULT_DISABLE);
+
+module_param_named(pstore_disable, efivars_pstore_disable, bool, 0644);
+
+#define PSTORE_EFI_ATTRIBUTES \
+	(EFI_VARIABLE_NON_VOLATILE | \
+	 EFI_VARIABLE_BOOTSERVICE_ACCESS | \
+	 EFI_VARIABLE_RUNTIME_ACCESS)
+
+static int efi_pstore_open(struct pstore_info *psi)
+{
+	efivar_entry_iter_begin();
+	psi->data = NULL;
+	return 0;
+}
+
+static int efi_pstore_close(struct pstore_info *psi)
+{
+	efivar_entry_iter_end();
+	psi->data = NULL;
+	return 0;
+}
+
+struct pstore_read_data {
+	u64 *id;
+	enum pstore_type_id *type;
+	int *count;
+	struct timespec *timespec;
+	char **buf;
+};
+
+static int efi_pstore_read_func(struct efivar_entry *entry, void *data)
+{
+	efi_guid_t vendor = LINUX_EFI_CRASH_GUID;
+	struct pstore_read_data *cb_data = data;
+	char name[DUMP_NAME_LEN];
+	int i;
+	int cnt;
+	unsigned int part;
+	unsigned long time, size;
+
+	if (efi_guidcmp(entry->var.VendorGuid, vendor))
+		return 0;
+
+	for (i = 0; i < DUMP_NAME_LEN; i++)
+		name[i] = entry->var.VariableName[i];
+
+	if (sscanf(name, "dump-type%u-%u-%d-%lu",
+		   cb_data->type, &part, &cnt, &time) == 4) {
+		*cb_data->id = part;
+		*cb_data->count = cnt;
+		cb_data->timespec->tv_sec = time;
+		cb_data->timespec->tv_nsec = 0;
+	} else if (sscanf(name, "dump-type%u-%u-%lu",
+			  cb_data->type, &part, &time) == 3) {
+		/*
+		 * Check if an old format,
+		 * which doesn't support holding
+		 * multiple logs, remains.
+		 */
+		*cb_data->id = part;
+		*cb_data->count = 0;
+		cb_data->timespec->tv_sec = time;
+		cb_data->timespec->tv_nsec = 0;
+	} else
+		return 0;
+
+	entry->var.DataSize = 1024;
+	__efivar_entry_get(entry, &entry->var.Attributes,
+			   &entry->var.DataSize, entry->var.Data);
+	size = entry->var.DataSize;
+
+	*cb_data->buf = kmalloc(size, GFP_KERNEL);
+	if (*cb_data->buf == NULL)
+		return -ENOMEM;
+	memcpy(*cb_data->buf, entry->var.Data, size);
+	return size;
+}
+
+static ssize_t efi_pstore_read(u64 *id, enum pstore_type_id *type,
+			       int *count, struct timespec *timespec,
+			       char **buf, struct pstore_info *psi)
+{
+	struct pstore_read_data data;
+
+	data.id = id;
+	data.type = type;
+	data.count = count;
+	data.timespec = timespec;
+	data.buf = buf;
+
+	return __efivar_entry_iter(efi_pstore_read_func, &efivar_sysfs_list, &data,
+				   (struct efivar_entry **)&psi->data);
+}
+
+static int efi_pstore_write(enum pstore_type_id type,
+		enum kmsg_dump_reason reason, u64 *id,
+		unsigned int part, int count, size_t size,
+		struct pstore_info *psi)
+{
+	char name[DUMP_NAME_LEN];
+	efi_char16_t efi_name[DUMP_NAME_LEN];
+	efi_guid_t vendor = LINUX_EFI_CRASH_GUID;
+	int i, ret = 0;
+
+	sprintf(name, "dump-type%u-%u-%d-%lu", type, part, count,
+		get_seconds());
+
+	for (i = 0; i < DUMP_NAME_LEN; i++)
+		efi_name[i] = name[i];
+
+	efivar_entry_set_safe(efi_name, vendor, PSTORE_EFI_ATTRIBUTES,
+			      !pstore_cannot_block_path(reason),
+			      size, psi->buf);
+
+	if (reason == KMSG_DUMP_OOPS)
+		efivar_run_worker();
+
+	*id = part;
+	return ret;
+};
+
+struct pstore_erase_data {
+	u64 id;
+	enum pstore_type_id type;
+	int count;
+	struct timespec time;
+	efi_char16_t *name;
+};
+
+/*
+ * Clean up an entry with the same name
+ */
+static int efi_pstore_erase_func(struct efivar_entry *entry, void *data)
+{
+	struct pstore_erase_data *ed = data;
+	efi_guid_t vendor = LINUX_EFI_CRASH_GUID;
+	efi_char16_t efi_name_old[DUMP_NAME_LEN];
+	efi_char16_t *efi_name = ed->name;
+	unsigned long ucs2_len = ucs2_strlen(ed->name);
+	char name_old[DUMP_NAME_LEN];
+	int i;
+
+	if (efi_guidcmp(entry->var.VendorGuid, vendor))
+		return 0;
+
+	if (ucs2_strncmp(entry->var.VariableName,
+			  efi_name, (size_t)ucs2_len)) {
+		/*
+		 * Check if an old format, which doesn't support
+		 * holding multiple logs, remains.
+		 */
+		sprintf(name_old, "dump-type%u-%u-%lu", ed->type,
+			(unsigned int)ed->id, ed->time.tv_sec);
+
+		for (i = 0; i < DUMP_NAME_LEN; i++)
+			efi_name_old[i] = name_old[i];
+
+		if (ucs2_strncmp(entry->var.VariableName, efi_name_old,
+				  ucs2_strlen(efi_name_old)))
+			return 0;
+	}
+
+	/* found */
+	__efivar_entry_delete(entry);
+	list_del(&entry->list);
+
+	return 1;
+}
+
+static int efi_pstore_erase(enum pstore_type_id type, u64 id, int count,
+			    struct timespec time, struct pstore_info *psi)
+{
+	struct pstore_erase_data edata;
+	struct efivar_entry *entry = NULL;
+	char name[DUMP_NAME_LEN];
+	efi_char16_t efi_name[DUMP_NAME_LEN];
+	int found, i;
+
+	sprintf(name, "dump-type%u-%u-%d-%lu", type, (unsigned int)id, count,
+		time.tv_sec);
+
+	for (i = 0; i < DUMP_NAME_LEN; i++)
+		efi_name[i] = name[i];
+
+	edata.id = id;
+	edata.type = type;
+	edata.count = count;
+	edata.time = time;
+	edata.name = efi_name;
+
+	efivar_entry_iter_begin();
+	found = __efivar_entry_iter(efi_pstore_erase_func, &efivar_sysfs_list, &edata, &entry);
+	efivar_entry_iter_end();
+
+	if (found)
+		efivar_unregister(entry);
+
+	return 0;
+}
+
+static struct pstore_info efi_pstore_info = {
+	.owner		= THIS_MODULE,
+	.name		= "efi",
+	.open		= efi_pstore_open,
+	.close		= efi_pstore_close,
+	.read		= efi_pstore_read,
+	.write		= efi_pstore_write,
+	.erase		= efi_pstore_erase,
+};
+
+static __init int efivars_pstore_init(void)
+{
+	if (!efi_enabled(EFI_RUNTIME_SERVICES))
+		return 0;
+
+	if (!efivars_kobject())
+		return 0;
+
+	if (efivars_pstore_disable)
+		return 0;
+
+	efi_pstore_info.buf = kmalloc(4096, GFP_KERNEL);
+	if (!efi_pstore_info.buf)
+		return -ENOMEM;
+
+	efi_pstore_info.bufsize = 1024;
+	spin_lock_init(&efi_pstore_info.buf_lock);
+
+	pstore_register(&efi_pstore_info);
+
+	return 0;
+}
+
+static __exit void efivars_pstore_exit(void)
+{
+}
+
+module_init(efivars_pstore_init);
+module_exit(efivars_pstore_exit);
+
+MODULE_DESCRIPTION("EFI variable backend for pstore");
+MODULE_LICENSE("GPL");
diff --git a/drivers/firmware/efi/efi.c b/drivers/firmware/efi/efi.c
new file mode 100644
index 000000000000..5145fa344ad5
--- /dev/null
+++ b/drivers/firmware/efi/efi.c
@@ -0,0 +1,134 @@
+/*
+ * efi.c - EFI subsystem
+ *
+ * Copyright (C) 2001,2003,2004 Dell <Matt_Domsch@dell.com>
+ * Copyright (C) 2004 Intel Corporation <matthew.e.tolentino@intel.com>
+ * Copyright (C) 2013 Tom Gundersen <teg@jklm.no>
+ *
+ * This code registers /sys/firmware/efi{,/efivars} when EFI is supported,
+ * allowing the efivarfs to be mounted or the efivars module to be loaded.
+ * The existance of /sys/firmware/efi may also be used by userspace to
+ * determine that the system supports EFI.
+ *
+ * This file is released under the GPLv2.
+ */
+
+#include <linux/kobject.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/efi.h>
+
+static struct kobject *efi_kobj;
+static struct kobject *efivars_kobj;
+
+/*
+ * Let's not leave out systab information that snuck into
+ * the efivars driver
+ */
+static ssize_t systab_show(struct kobject *kobj,
+			   struct kobj_attribute *attr, char *buf)
+{
+	char *str = buf;
+
+	if (!kobj || !buf)
+		return -EINVAL;
+
+	if (efi.mps != EFI_INVALID_TABLE_ADDR)
+		str += sprintf(str, "MPS=0x%lx\n", efi.mps);
+	if (efi.acpi20 != EFI_INVALID_TABLE_ADDR)
+		str += sprintf(str, "ACPI20=0x%lx\n", efi.acpi20);
+	if (efi.acpi != EFI_INVALID_TABLE_ADDR)
+		str += sprintf(str, "ACPI=0x%lx\n", efi.acpi);
+	if (efi.smbios != EFI_INVALID_TABLE_ADDR)
+		str += sprintf(str, "SMBIOS=0x%lx\n", efi.smbios);
+	if (efi.hcdp != EFI_INVALID_TABLE_ADDR)
+		str += sprintf(str, "HCDP=0x%lx\n", efi.hcdp);
+	if (efi.boot_info != EFI_INVALID_TABLE_ADDR)
+		str += sprintf(str, "BOOTINFO=0x%lx\n", efi.boot_info);
+	if (efi.uga != EFI_INVALID_TABLE_ADDR)
+		str += sprintf(str, "UGA=0x%lx\n", efi.uga);
+
+	return str - buf;
+}
+
+static struct kobj_attribute efi_attr_systab =
+			__ATTR(systab, 0400, systab_show, NULL);
+
+static struct attribute *efi_subsys_attrs[] = {
+	&efi_attr_systab.attr,
+	NULL,	/* maybe more in the future? */
+};
+
+static struct attribute_group efi_subsys_attr_group = {
+	.attrs = efi_subsys_attrs,
+};
+
+static struct efivars generic_efivars;
+static struct efivar_operations generic_ops;
+
+static int generic_ops_register(void)
+{
+	generic_ops.get_variable = efi.get_variable;
+	generic_ops.set_variable = efi.set_variable;
+	generic_ops.get_next_variable = efi.get_next_variable;
+	generic_ops.query_variable_store = efi_query_variable_store;
+
+	return efivars_register(&generic_efivars, &generic_ops, efi_kobj);
+}
+
+static void generic_ops_unregister(void)
+{
+	efivars_unregister(&generic_efivars);
+}
+
+/*
+ * We register the efi subsystem with the firmware subsystem and the
+ * efivars subsystem with the efi subsystem, if the system was booted with
+ * EFI.
+ */
+static int __init efisubsys_init(void)
+{
+	int error;
+
+	if (!efi_enabled(EFI_BOOT))
+		return 0;
+
+	/* We register the efi directory at /sys/firmware/efi */
+	efi_kobj = kobject_create_and_add("efi", firmware_kobj);
+	if (!efi_kobj) {
+		pr_err("efi: Firmware registration failed.\n");
+		return -ENOMEM;
+	}
+
+	error = generic_ops_register();
+	if (error)
+		goto err_put;
+
+	error = sysfs_create_group(efi_kobj, &efi_subsys_attr_group);
+	if (error) {
+		pr_err("efi: Sysfs attribute export failed with error %d.\n",
+		       error);
+		goto err_unregister;
+	}
+
+	/* and the standard mountpoint for efivarfs */
+	efivars_kobj = kobject_create_and_add("efivars", efi_kobj);
+	if (!efivars_kobj) {
+		pr_err("efivars: Subsystem registration failed.\n");
+		error = -ENOMEM;
+		goto err_remove_group;
+	}
+
+	return 0;
+
+err_remove_group:
+	sysfs_remove_group(efi_kobj, &efi_subsys_attr_group);
+err_unregister:
+	generic_ops_unregister();
+err_put:
+	kobject_put(efi_kobj);
+	return error;
+}
+
+subsys_initcall(efisubsys_init);
diff --git a/drivers/firmware/efi/efivars.c b/drivers/firmware/efi/efivars.c
new file mode 100644
index 000000000000..b623c599e572
--- /dev/null
+++ b/drivers/firmware/efi/efivars.c
@@ -0,0 +1,617 @@
+/*
+ * Originally from efivars.c,
+ *
+ * Copyright (C) 2001,2003,2004 Dell <Matt_Domsch@dell.com>
+ * Copyright (C) 2004 Intel Corporation <matthew.e.tolentino@intel.com>
+ *
+ * This code takes all variables accessible from EFI runtime and
+ *  exports them via sysfs
+ *
+ *  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
+ *
+ * Changelog:
+ *
+ *  17 May 2004 - Matt Domsch <Matt_Domsch@dell.com>
+ *   remove check for efi_enabled in exit
+ *   add MODULE_VERSION
+ *
+ *  26 Apr 2004 - Matt Domsch <Matt_Domsch@dell.com>
+ *   minor bug fixes
+ *
+ *  21 Apr 2004 - Matt Tolentino <matthew.e.tolentino@intel.com)
+ *   converted driver to export variable information via sysfs
+ *   and moved to drivers/firmware directory
+ *   bumped revision number to v0.07 to reflect conversion & move
+ *
+ *  10 Dec 2002 - Matt Domsch <Matt_Domsch@dell.com>
+ *   fix locking per Peter Chubb's findings
+ *
+ *  25 Mar 2002 - Matt Domsch <Matt_Domsch@dell.com>
+ *   move uuid_unparse() to include/asm-ia64/efi.h:efi_guid_unparse()
+ *
+ *  12 Feb 2002 - Matt Domsch <Matt_Domsch@dell.com>
+ *   use list_for_each_safe when deleting vars.
+ *   remove ifdef CONFIG_SMP around include <linux/smp.h>
+ *   v0.04 release to linux-ia64@linuxia64.org
+ *
+ *  20 April 2001 - Matt Domsch <Matt_Domsch@dell.com>
+ *   Moved vars from /proc/efi to /proc/efi/vars, and made
+ *   efi.c own the /proc/efi directory.
+ *   v0.03 release to linux-ia64@linuxia64.org
+ *
+ *  26 March 2001 - Matt Domsch <Matt_Domsch@dell.com>
+ *   At the request of Stephane, moved ownership of /proc/efi
+ *   to efi.c, and now efivars lives under /proc/efi/vars.
+ *
+ *  12 March 2001 - Matt Domsch <Matt_Domsch@dell.com>
+ *   Feedback received from Stephane Eranian incorporated.
+ *   efivar_write() checks copy_from_user() return value.
+ *   efivar_read/write() returns proper errno.
+ *   v0.02 release to linux-ia64@linuxia64.org
+ *
+ *  26 February 2001 - Matt Domsch <Matt_Domsch@dell.com>
+ *   v0.01 release to linux-ia64@linuxia64.org
+ */
+
+#include <linux/efi.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/ucs2_string.h>
+
+#define EFIVARS_VERSION "0.08"
+#define EFIVARS_DATE "2004-May-17"
+
+MODULE_AUTHOR("Matt Domsch <Matt_Domsch@Dell.com>");
+MODULE_DESCRIPTION("sysfs interface to EFI Variables");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(EFIVARS_VERSION);
+
+LIST_HEAD(efivar_sysfs_list);
+EXPORT_SYMBOL_GPL(efivar_sysfs_list);
+
+static struct kset *efivars_kset;
+
+static struct bin_attribute *efivars_new_var;
+static struct bin_attribute *efivars_del_var;
+
+struct efivar_attribute {
+	struct attribute attr;
+	ssize_t (*show) (struct efivar_entry *entry, char *buf);
+	ssize_t (*store)(struct efivar_entry *entry, const char *buf, size_t count);
+};
+
+#define EFIVAR_ATTR(_name, _mode, _show, _store) \
+struct efivar_attribute efivar_attr_##_name = { \
+	.attr = {.name = __stringify(_name), .mode = _mode}, \
+	.show = _show, \
+	.store = _store, \
+};
+
+#define to_efivar_attr(_attr) container_of(_attr, struct efivar_attribute, attr)
+#define to_efivar_entry(obj)  container_of(obj, struct efivar_entry, kobj)
+
+/*
+ * Prototype for sysfs creation function
+ */
+static int
+efivar_create_sysfs_entry(struct efivar_entry *new_var);
+
+static ssize_t
+efivar_guid_read(struct efivar_entry *entry, char *buf)
+{
+	struct efi_variable *var = &entry->var;
+	char *str = buf;
+
+	if (!entry || !buf)
+		return 0;
+
+	efi_guid_unparse(&var->VendorGuid, str);
+	str += strlen(str);
+	str += sprintf(str, "\n");
+
+	return str - buf;
+}
+
+static ssize_t
+efivar_attr_read(struct efivar_entry *entry, char *buf)
+{
+	struct efi_variable *var = &entry->var;
+	char *str = buf;
+
+	if (!entry || !buf)
+		return -EINVAL;
+
+	var->DataSize = 1024;
+	if (efivar_entry_get(entry, &var->Attributes, &var->DataSize, var->Data))
+		return -EIO;
+
+	if (var->Attributes & EFI_VARIABLE_NON_VOLATILE)
+		str += sprintf(str, "EFI_VARIABLE_NON_VOLATILE\n");
+	if (var->Attributes & EFI_VARIABLE_BOOTSERVICE_ACCESS)
+		str += sprintf(str, "EFI_VARIABLE_BOOTSERVICE_ACCESS\n");
+	if (var->Attributes & EFI_VARIABLE_RUNTIME_ACCESS)
+		str += sprintf(str, "EFI_VARIABLE_RUNTIME_ACCESS\n");
+	if (var->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD)
+		str += sprintf(str, "EFI_VARIABLE_HARDWARE_ERROR_RECORD\n");
+	if (var->Attributes & EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS)
+		str += sprintf(str,
+			"EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS\n");
+	if (var->Attributes &
+			EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS)
+		str += sprintf(str,
+			"EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS\n");
+	if (var->Attributes & EFI_VARIABLE_APPEND_WRITE)
+		str += sprintf(str, "EFI_VARIABLE_APPEND_WRITE\n");
+	return str - buf;
+}
+
+static ssize_t
+efivar_size_read(struct efivar_entry *entry, char *buf)
+{
+	struct efi_variable *var = &entry->var;
+	char *str = buf;
+
+	if (!entry || !buf)
+		return -EINVAL;
+
+	var->DataSize = 1024;
+	if (efivar_entry_get(entry, &var->Attributes, &var->DataSize, var->Data))
+		return -EIO;
+
+	str += sprintf(str, "0x%lx\n", var->DataSize);
+	return str - buf;
+}
+
+static ssize_t
+efivar_data_read(struct efivar_entry *entry, char *buf)
+{
+	struct efi_variable *var = &entry->var;
+
+	if (!entry || !buf)
+		return -EINVAL;
+
+	var->DataSize = 1024;
+	if (efivar_entry_get(entry, &var->Attributes, &var->DataSize, var->Data))
+		return -EIO;
+
+	memcpy(buf, var->Data, var->DataSize);
+	return var->DataSize;
+}
+/*
+ * We allow each variable to be edited via rewriting the
+ * entire efi variable structure.
+ */
+static ssize_t
+efivar_store_raw(struct efivar_entry *entry, const char *buf, size_t count)
+{
+	struct efi_variable *new_var, *var = &entry->var;
+	int err;
+
+	if (count != sizeof(struct efi_variable))
+		return -EINVAL;
+
+	new_var = (struct efi_variable *)buf;
+	/*
+	 * If only updating the variable data, then the name
+	 * and guid should remain the same
+	 */
+	if (memcmp(new_var->VariableName, var->VariableName, sizeof(var->VariableName)) ||
+		efi_guidcmp(new_var->VendorGuid, var->VendorGuid)) {
+		printk(KERN_ERR "efivars: Cannot edit the wrong variable!\n");
+		return -EINVAL;
+	}
+
+	if ((new_var->DataSize <= 0) || (new_var->Attributes == 0)){
+		printk(KERN_ERR "efivars: DataSize & Attributes must be valid!\n");
+		return -EINVAL;
+	}
+
+	if ((new_var->Attributes & ~EFI_VARIABLE_MASK) != 0 ||
+	    efivar_validate(new_var, new_var->Data, new_var->DataSize) == false) {
+		printk(KERN_ERR "efivars: Malformed variable content\n");
+		return -EINVAL;
+	}
+
+	memcpy(&entry->var, new_var, count);
+
+	err = efivar_entry_set(entry, new_var->Attributes,
+			       new_var->DataSize, new_var->Data, false);
+	if (err) {
+		printk(KERN_WARNING "efivars: set_variable() failed: status=%d\n", err);
+		return -EIO;
+	}
+
+	return count;
+}
+
+static ssize_t
+efivar_show_raw(struct efivar_entry *entry, char *buf)
+{
+	struct efi_variable *var = &entry->var;
+
+	if (!entry || !buf)
+		return 0;
+
+	var->DataSize = 1024;
+	if (efivar_entry_get(entry, &entry->var.Attributes,
+			     &entry->var.DataSize, entry->var.Data))
+		return -EIO;
+
+	memcpy(buf, var, sizeof(*var));
+
+	return sizeof(*var);
+}
+
+/*
+ * Generic read/write functions that call the specific functions of
+ * the attributes...
+ */
+static ssize_t efivar_attr_show(struct kobject *kobj, struct attribute *attr,
+				char *buf)
+{
+	struct efivar_entry *var = to_efivar_entry(kobj);
+	struct efivar_attribute *efivar_attr = to_efivar_attr(attr);
+	ssize_t ret = -EIO;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EACCES;
+
+	if (efivar_attr->show) {
+		ret = efivar_attr->show(var, buf);
+	}
+	return ret;
+}
+
+static ssize_t efivar_attr_store(struct kobject *kobj, struct attribute *attr,
+				const char *buf, size_t count)
+{
+	struct efivar_entry *var = to_efivar_entry(kobj);
+	struct efivar_attribute *efivar_attr = to_efivar_attr(attr);
+	ssize_t ret = -EIO;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EACCES;
+
+	if (efivar_attr->store)
+		ret = efivar_attr->store(var, buf, count);
+
+	return ret;
+}
+
+static const struct sysfs_ops efivar_attr_ops = {
+	.show = efivar_attr_show,
+	.store = efivar_attr_store,
+};
+
+static void efivar_release(struct kobject *kobj)
+{
+	struct efivar_entry *var = container_of(kobj, struct efivar_entry, kobj);
+	kfree(var);
+}
+
+static EFIVAR_ATTR(guid, 0400, efivar_guid_read, NULL);
+static EFIVAR_ATTR(attributes, 0400, efivar_attr_read, NULL);
+static EFIVAR_ATTR(size, 0400, efivar_size_read, NULL);
+static EFIVAR_ATTR(data, 0400, efivar_data_read, NULL);
+static EFIVAR_ATTR(raw_var, 0600, efivar_show_raw, efivar_store_raw);
+
+static struct attribute *def_attrs[] = {
+	&efivar_attr_guid.attr,
+	&efivar_attr_size.attr,
+	&efivar_attr_attributes.attr,
+	&efivar_attr_data.attr,
+	&efivar_attr_raw_var.attr,
+	NULL,
+};
+
+static struct kobj_type efivar_ktype = {
+	.release = efivar_release,
+	.sysfs_ops = &efivar_attr_ops,
+	.default_attrs = def_attrs,
+};
+
+static ssize_t efivar_create(struct file *filp, struct kobject *kobj,
+			     struct bin_attribute *bin_attr,
+			     char *buf, loff_t pos, size_t count)
+{
+	struct efi_variable *new_var = (struct efi_variable *)buf;
+	struct efivar_entry *new_entry;
+	int err;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EACCES;
+
+	if ((new_var->Attributes & ~EFI_VARIABLE_MASK) != 0 ||
+	    efivar_validate(new_var, new_var->Data, new_var->DataSize) == false) {
+		printk(KERN_ERR "efivars: Malformed variable content\n");
+		return -EINVAL;
+	}
+
+	new_entry = kzalloc(sizeof(*new_entry), GFP_KERNEL);
+	if (!new_entry)
+		return -ENOMEM;
+
+	memcpy(&new_entry->var, new_var, sizeof(*new_var));
+
+	err = efivar_entry_set(new_entry, new_var->Attributes, new_var->DataSize,
+			       new_var->Data, &efivar_sysfs_list);
+	if (err) {
+		if (err == -EEXIST)
+			err = -EINVAL;
+		goto out;
+	}
+
+	if (efivar_create_sysfs_entry(new_entry)) {
+		printk(KERN_WARNING "efivars: failed to create sysfs entry.\n");
+		kfree(new_entry);
+	}
+	return count;
+
+out:
+	kfree(new_entry);
+	return err;
+}
+
+static ssize_t efivar_delete(struct file *filp, struct kobject *kobj,
+			     struct bin_attribute *bin_attr,
+			     char *buf, loff_t pos, size_t count)
+{
+	struct efi_variable *del_var = (struct efi_variable *)buf;
+	struct efivar_entry *entry;
+	int err = 0;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EACCES;
+
+	efivar_entry_iter_begin();
+	entry = efivar_entry_find(del_var->VariableName, del_var->VendorGuid,
+				  &efivar_sysfs_list, true);
+	if (!entry)
+		err = -EINVAL;
+	else if (__efivar_entry_delete(entry))
+		err = -EIO;
+
+	efivar_entry_iter_end();
+
+	if (err)
+		return err;
+
+	efivar_unregister(entry);
+
+	/* It's dead Jim.... */
+	return count;
+}
+
+/**
+ * efivar_create_sysfs_entry - create a new entry in sysfs
+ * @new_var: efivar entry to create
+ *
+ * Returns 1 on failure, 0 on success
+ */
+static int
+efivar_create_sysfs_entry(struct efivar_entry *new_var)
+{
+	int i, short_name_size;
+	char *short_name;
+	unsigned long variable_name_size;
+	efi_char16_t *variable_name;
+
+	variable_name = new_var->var.VariableName;
+	variable_name_size = ucs2_strlen(variable_name) * sizeof(efi_char16_t);
+
+	/*
+	 * Length of the variable bytes in ASCII, plus the '-' separator,
+	 * plus the GUID, plus trailing NUL
+	 */
+	short_name_size = variable_name_size / sizeof(efi_char16_t)
+				+ 1 + EFI_VARIABLE_GUID_LEN + 1;
+
+	short_name = kzalloc(short_name_size, GFP_KERNEL);
+
+	if (!short_name)
+		return 1;
+
+	/* Convert Unicode to normal chars (assume top bits are 0),
+	   ala UTF-8 */
+	for (i=0; i < (int)(variable_name_size / sizeof(efi_char16_t)); i++) {
+		short_name[i] = variable_name[i] & 0xFF;
+	}
+	/* This is ugly, but necessary to separate one vendor's
+	   private variables from another's.         */
+
+	*(short_name + strlen(short_name)) = '-';
+	efi_guid_unparse(&new_var->var.VendorGuid,
+			 short_name + strlen(short_name));
+
+	new_var->kobj.kset = efivars_kset;
+
+	i = kobject_init_and_add(&new_var->kobj, &efivar_ktype,
+				   NULL, "%s", short_name);
+	kfree(short_name);
+	if (i)
+		return 1;
+
+	kobject_uevent(&new_var->kobj, KOBJ_ADD);
+	efivar_entry_add(new_var, &efivar_sysfs_list);
+
+	return 0;
+}
+
+static int
+create_efivars_bin_attributes(void)
+{
+	struct bin_attribute *attr;
+	int error;
+
+	/* new_var */
+	attr = kzalloc(sizeof(*attr), GFP_KERNEL);
+	if (!attr)
+		return -ENOMEM;
+
+	attr->attr.name = "new_var";
+	attr->attr.mode = 0200;
+	attr->write = efivar_create;
+	efivars_new_var = attr;
+
+	/* del_var */
+	attr = kzalloc(sizeof(*attr), GFP_KERNEL);
+	if (!attr) {
+		error = -ENOMEM;
+		goto out_free;
+	}
+	attr->attr.name = "del_var";
+	attr->attr.mode = 0200;
+	attr->write = efivar_delete;
+	efivars_del_var = attr;
+
+	sysfs_bin_attr_init(efivars_new_var);
+	sysfs_bin_attr_init(efivars_del_var);
+
+	/* Register */
+	error = sysfs_create_bin_file(&efivars_kset->kobj, efivars_new_var);
+	if (error) {
+		printk(KERN_ERR "efivars: unable to create new_var sysfs file"
+			" due to error %d\n", error);
+		goto out_free;
+	}
+
+	error = sysfs_create_bin_file(&efivars_kset->kobj, efivars_del_var);
+	if (error) {
+		printk(KERN_ERR "efivars: unable to create del_var sysfs file"
+			" due to error %d\n", error);
+		sysfs_remove_bin_file(&efivars_kset->kobj, efivars_new_var);
+		goto out_free;
+	}
+
+	return 0;
+out_free:
+	kfree(efivars_del_var);
+	efivars_del_var = NULL;
+	kfree(efivars_new_var);
+	efivars_new_var = NULL;
+	return error;
+}
+
+static int efivar_update_sysfs_entry(efi_char16_t *name, efi_guid_t vendor,
+				     unsigned long name_size, void *data)
+{
+	struct efivar_entry *entry = data;
+
+	if (efivar_entry_find(name, vendor, &efivar_sysfs_list, false))
+		return 0;
+
+	memcpy(entry->var.VariableName, name, name_size);
+	memcpy(&(entry->var.VendorGuid), &vendor, sizeof(efi_guid_t));
+
+	return 1;
+}
+
+static void efivar_update_sysfs_entries(struct work_struct *work)
+{
+	struct efivar_entry *entry;
+	int err;
+
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
+	if (!entry)
+		return;
+
+	/* Add new sysfs entries */
+	while (1) {
+		memset(entry, 0, sizeof(*entry));
+
+		err = efivar_init(efivar_update_sysfs_entry, entry,
+				  true, false, &efivar_sysfs_list);
+		if (!err)
+			break;
+
+		efivar_create_sysfs_entry(entry);
+	}
+
+	kfree(entry);
+}
+
+static int efivars_sysfs_callback(efi_char16_t *name, efi_guid_t vendor,
+				  unsigned long name_size, void *data)
+{
+	struct efivar_entry *entry;
+
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
+	if (!entry)
+		return -ENOMEM;
+
+	memcpy(entry->var.VariableName, name, name_size);
+	memcpy(&(entry->var.VendorGuid), &vendor, sizeof(efi_guid_t));
+
+	efivar_create_sysfs_entry(entry);
+
+	return 0;
+}
+
+static int efivar_sysfs_destroy(struct efivar_entry *entry, void *data)
+{
+	efivar_entry_remove(entry);
+	efivar_unregister(entry);
+	return 0;
+}
+
+void efivars_sysfs_exit(void)
+{
+	/* Remove all entries and destroy */
+	__efivar_entry_iter(efivar_sysfs_destroy, &efivar_sysfs_list, NULL, NULL);
+
+	if (efivars_new_var)
+		sysfs_remove_bin_file(&efivars_kset->kobj, efivars_new_var);
+	if (efivars_del_var)
+		sysfs_remove_bin_file(&efivars_kset->kobj, efivars_del_var);
+	kfree(efivars_new_var);
+	kfree(efivars_del_var);
+	kset_unregister(efivars_kset);
+}
+
+int efivars_sysfs_init(void)
+{
+	struct kobject *parent_kobj = efivars_kobject();
+	int error = 0;
+
+	/* No efivars has been registered yet */
+	if (!parent_kobj)
+		return 0;
+
+	printk(KERN_INFO "EFI Variables Facility v%s %s\n", EFIVARS_VERSION,
+	       EFIVARS_DATE);
+
+	efivars_kset = kset_create_and_add("vars", NULL, parent_kobj);
+	if (!efivars_kset) {
+		printk(KERN_ERR "efivars: Subsystem registration failed.\n");
+		return -ENOMEM;
+	}
+
+	efivar_init(efivars_sysfs_callback, NULL, false,
+		    true, &efivar_sysfs_list);
+
+	error = create_efivars_bin_attributes();
+	if (error) {
+		efivars_sysfs_exit();
+		return error;
+	}
+
+	INIT_WORK(&efivar_work, efivar_update_sysfs_entries);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(efivars_sysfs_init);
+
+module_init(efivars_sysfs_init);
+module_exit(efivars_sysfs_exit);
diff --git a/drivers/firmware/efi/vars.c b/drivers/firmware/efi/vars.c
new file mode 100644
index 000000000000..391c67b182d9
--- /dev/null
+++ b/drivers/firmware/efi/vars.c
@@ -0,0 +1,1041 @@
+/*
+ * Originally from efivars.c
+ *
+ * Copyright (C) 2001,2003,2004 Dell <Matt_Domsch@dell.com>
+ * Copyright (C) 2004 Intel Corporation <matthew.e.tolentino@intel.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/capability.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/smp.h>
+#include <linux/efi.h>
+#include <linux/sysfs.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/ucs2_string.h>
+
+/* Private pointer to registered efivars */
+static struct efivars *__efivars;
+
+static bool efivar_wq_enabled = true;
+DECLARE_WORK(efivar_work, NULL);
+EXPORT_SYMBOL_GPL(efivar_work);
+
+static bool
+validate_device_path(struct efi_variable *var, int match, u8 *buffer,
+		     unsigned long len)
+{
+	struct efi_generic_dev_path *node;
+	int offset = 0;
+
+	node = (struct efi_generic_dev_path *)buffer;
+
+	if (len < sizeof(*node))
+		return false;
+
+	while (offset <= len - sizeof(*node) &&
+	       node->length >= sizeof(*node) &&
+		node->length <= len - offset) {
+		offset += node->length;
+
+		if ((node->type == EFI_DEV_END_PATH ||
+		     node->type == EFI_DEV_END_PATH2) &&
+		    node->sub_type == EFI_DEV_END_ENTIRE)
+			return true;
+
+		node = (struct efi_generic_dev_path *)(buffer + offset);
+	}
+
+	/*
+	 * If we're here then either node->length pointed past the end
+	 * of the buffer or we reached the end of the buffer without
+	 * finding a device path end node.
+	 */
+	return false;
+}
+
+static bool
+validate_boot_order(struct efi_variable *var, int match, u8 *buffer,
+		    unsigned long len)
+{
+	/* An array of 16-bit integers */
+	if ((len % 2) != 0)
+		return false;
+
+	return true;
+}
+
+static bool
+validate_load_option(struct efi_variable *var, int match, u8 *buffer,
+		     unsigned long len)
+{
+	u16 filepathlength;
+	int i, desclength = 0, namelen;
+
+	namelen = ucs2_strnlen(var->VariableName, sizeof(var->VariableName));
+
+	/* Either "Boot" or "Driver" followed by four digits of hex */
+	for (i = match; i < match+4; i++) {
+		if (var->VariableName[i] > 127 ||
+		    hex_to_bin(var->VariableName[i] & 0xff) < 0)
+			return true;
+	}
+
+	/* Reject it if there's 4 digits of hex and then further content */
+	if (namelen > match + 4)
+		return false;
+
+	/* A valid entry must be at least 8 bytes */
+	if (len < 8)
+		return false;
+
+	filepathlength = buffer[4] | buffer[5] << 8;
+
+	/*
+	 * There's no stored length for the description, so it has to be
+	 * found by hand
+	 */
+	desclength = ucs2_strsize((efi_char16_t *)(buffer + 6), len - 6) + 2;
+
+	/* Each boot entry must have a descriptor */
+	if (!desclength)
+		return false;
+
+	/*
+	 * If the sum of the length of the description, the claimed filepath
+	 * length and the original header are greater than the length of the
+	 * variable, it's malformed
+	 */
+	if ((desclength + filepathlength + 6) > len)
+		return false;
+
+	/*
+	 * And, finally, check the filepath
+	 */
+	return validate_device_path(var, match, buffer + desclength + 6,
+				    filepathlength);
+}
+
+static bool
+validate_uint16(struct efi_variable *var, int match, u8 *buffer,
+		unsigned long len)
+{
+	/* A single 16-bit integer */
+	if (len != 2)
+		return false;
+
+	return true;
+}
+
+static bool
+validate_ascii_string(struct efi_variable *var, int match, u8 *buffer,
+		      unsigned long len)
+{
+	int i;
+
+	for (i = 0; i < len; i++) {
+		if (buffer[i] > 127)
+			return false;
+
+		if (buffer[i] == 0)
+			return true;
+	}
+
+	return false;
+}
+
+struct variable_validate {
+	char *name;
+	bool (*validate)(struct efi_variable *var, int match, u8 *data,
+			 unsigned long len);
+};
+
+static const struct variable_validate variable_validate[] = {
+	{ "BootNext", validate_uint16 },
+	{ "BootOrder", validate_boot_order },
+	{ "DriverOrder", validate_boot_order },
+	{ "Boot*", validate_load_option },
+	{ "Driver*", validate_load_option },
+	{ "ConIn", validate_device_path },
+	{ "ConInDev", validate_device_path },
+	{ "ConOut", validate_device_path },
+	{ "ConOutDev", validate_device_path },
+	{ "ErrOut", validate_device_path },
+	{ "ErrOutDev", validate_device_path },
+	{ "Timeout", validate_uint16 },
+	{ "Lang", validate_ascii_string },
+	{ "PlatformLang", validate_ascii_string },
+	{ "", NULL },
+};
+
+bool
+efivar_validate(struct efi_variable *var, u8 *data, unsigned long len)
+{
+	int i;
+	u16 *unicode_name = var->VariableName;
+
+	for (i = 0; variable_validate[i].validate != NULL; i++) {
+		const char *name = variable_validate[i].name;
+		int match;
+
+		for (match = 0; ; match++) {
+			char c = name[match];
+			u16 u = unicode_name[match];
+
+			/* All special variables are plain ascii */
+			if (u > 127)
+				return true;
+
+			/* Wildcard in the matching name means we've matched */
+			if (c == '*')
+				return variable_validate[i].validate(var,
+							     match, data, len);
+
+			/* Case sensitive match */
+			if (c != u)
+				break;
+
+			/* Reached the end of the string while matching */
+			if (!c)
+				return variable_validate[i].validate(var,
+							     match, data, len);
+		}
+	}
+
+	return true;
+}
+EXPORT_SYMBOL_GPL(efivar_validate);
+
+static efi_status_t
+check_var_size(u32 attributes, unsigned long size)
+{
+	const struct efivar_operations *fops = __efivars->ops;
+
+	if (!fops->query_variable_store)
+		return EFI_UNSUPPORTED;
+
+	return fops->query_variable_store(attributes, size);
+}
+
+static int efi_status_to_err(efi_status_t status)
+{
+	int err;
+
+	switch (status) {
+	case EFI_SUCCESS:
+		err = 0;
+		break;
+	case EFI_INVALID_PARAMETER:
+		err = -EINVAL;
+		break;
+	case EFI_OUT_OF_RESOURCES:
+		err = -ENOSPC;
+		break;
+	case EFI_DEVICE_ERROR:
+		err = -EIO;
+		break;
+	case EFI_WRITE_PROTECTED:
+		err = -EROFS;
+		break;
+	case EFI_SECURITY_VIOLATION:
+		err = -EACCES;
+		break;
+	case EFI_NOT_FOUND:
+		err = -ENOENT;
+		break;
+	default:
+		err = -EINVAL;
+	}
+
+	return err;
+}
+
+static bool variable_is_present(efi_char16_t *variable_name, efi_guid_t *vendor,
+				struct list_head *head)
+{
+	struct efivar_entry *entry, *n;
+	unsigned long strsize1, strsize2;
+	bool found = false;
+
+	strsize1 = ucs2_strsize(variable_name, 1024);
+	list_for_each_entry_safe(entry, n, head, list) {
+		strsize2 = ucs2_strsize(entry->var.VariableName, 1024);
+		if (strsize1 == strsize2 &&
+			!memcmp(variable_name, &(entry->var.VariableName),
+				strsize2) &&
+			!efi_guidcmp(entry->var.VendorGuid,
+				*vendor)) {
+			found = true;
+			break;
+		}
+	}
+	return found;
+}
+
+/*
+ * Returns the size of variable_name, in bytes, including the
+ * terminating NULL character, or variable_name_size if no NULL
+ * character is found among the first variable_name_size bytes.
+ */
+static unsigned long var_name_strnsize(efi_char16_t *variable_name,
+				       unsigned long variable_name_size)
+{
+	unsigned long len;
+	efi_char16_t c;
+
+	/*
+	 * The variable name is, by definition, a NULL-terminated
+	 * string, so make absolutely sure that variable_name_size is
+	 * the value we expect it to be. If not, return the real size.
+	 */
+	for (len = 2; len <= variable_name_size; len += sizeof(c)) {
+		c = variable_name[(len / sizeof(c)) - 1];
+		if (!c)
+			break;
+	}
+
+	return min(len, variable_name_size);
+}
+
+/*
+ * Print a warning when duplicate EFI variables are encountered and
+ * disable the sysfs workqueue since the firmware is buggy.
+ */
+static void dup_variable_bug(efi_char16_t *s16, efi_guid_t *vendor_guid,
+			     unsigned long len16)
+{
+	size_t i, len8 = len16 / sizeof(efi_char16_t);
+	char *s8;
+
+	/*
+	 * Disable the workqueue since the algorithm it uses for
+	 * detecting new variables won't work with this buggy
+	 * implementation of GetNextVariableName().
+	 */
+	efivar_wq_enabled = false;
+
+	s8 = kzalloc(len8, GFP_KERNEL);
+	if (!s8)
+		return;
+
+	for (i = 0; i < len8; i++)
+		s8[i] = s16[i];
+
+	printk(KERN_WARNING "efivars: duplicate variable: %s-%pUl\n",
+	       s8, vendor_guid);
+	kfree(s8);
+}
+
+/**
+ * efivar_init - build the initial list of EFI variables
+ * @func: callback function to invoke for every variable
+ * @data: function-specific data to pass to @func
+ * @atomic: do we need to execute the @func-loop atomically?
+ * @duplicates: error if we encounter duplicates on @head?
+ * @head: initialised head of variable list
+ *
+ * Get every EFI variable from the firmware and invoke @func. @func
+ * should call efivar_entry_add() to build the list of variables.
+ *
+ * Returns 0 on success, or a kernel error code on failure.
+ */
+int efivar_init(int (*func)(efi_char16_t *, efi_guid_t, unsigned long, void *),
+		void *data, bool atomic, bool duplicates,
+		struct list_head *head)
+{
+	const struct efivar_operations *ops = __efivars->ops;
+	unsigned long variable_name_size = 1024;
+	efi_char16_t *variable_name;
+	efi_status_t status;
+	efi_guid_t vendor_guid;
+	int err = 0;
+
+	variable_name = kzalloc(variable_name_size, GFP_KERNEL);
+	if (!variable_name) {
+		printk(KERN_ERR "efivars: Memory allocation failed.\n");
+		return -ENOMEM;
+	}
+
+	spin_lock_irq(&__efivars->lock);
+
+	/*
+	 * Per EFI spec, the maximum storage allocated for both
+	 * the variable name and variable data is 1024 bytes.
+	 */
+
+	do {
+		variable_name_size = 1024;
+
+		status = ops->get_next_variable(&variable_name_size,
+						variable_name,
+						&vendor_guid);
+		switch (status) {
+		case EFI_SUCCESS:
+			if (!atomic)
+				spin_unlock_irq(&__efivars->lock);
+
+			variable_name_size = var_name_strnsize(variable_name,
+							       variable_name_size);
+
+			/*
+			 * Some firmware implementations return the
+			 * same variable name on multiple calls to
+			 * get_next_variable(). Terminate the loop
+			 * immediately as there is no guarantee that
+			 * we'll ever see a different variable name,
+			 * and may end up looping here forever.
+			 */
+			if (duplicates &&
+			    variable_is_present(variable_name, &vendor_guid, head)) {
+				dup_variable_bug(variable_name, &vendor_guid,
+						 variable_name_size);
+				if (!atomic)
+					spin_lock_irq(&__efivars->lock);
+
+				status = EFI_NOT_FOUND;
+				break;
+			}
+
+			err = func(variable_name, vendor_guid, variable_name_size, data);
+			if (err)
+				status = EFI_NOT_FOUND;
+
+			if (!atomic)
+				spin_lock_irq(&__efivars->lock);
+
+			break;
+		case EFI_NOT_FOUND:
+			break;
+		default:
+			printk(KERN_WARNING "efivars: get_next_variable: status=%lx\n",
+				status);
+			status = EFI_NOT_FOUND;
+			break;
+		}
+
+	} while (status != EFI_NOT_FOUND);
+
+	spin_unlock_irq(&__efivars->lock);
+
+	kfree(variable_name);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(efivar_init);
+
+/**
+ * efivar_entry_add - add entry to variable list
+ * @entry: entry to add to list
+ * @head: list head
+ */
+void efivar_entry_add(struct efivar_entry *entry, struct list_head *head)
+{
+	spin_lock_irq(&__efivars->lock);
+	list_add(&entry->list, head);
+	spin_unlock_irq(&__efivars->lock);
+}
+EXPORT_SYMBOL_GPL(efivar_entry_add);
+
+/**
+ * efivar_entry_remove - remove entry from variable list
+ * @entry: entry to remove from list
+ */
+void efivar_entry_remove(struct efivar_entry *entry)
+{
+	spin_lock_irq(&__efivars->lock);
+	list_del(&entry->list);
+	spin_unlock_irq(&__efivars->lock);
+}
+EXPORT_SYMBOL_GPL(efivar_entry_remove);
+
+/*
+ * efivar_entry_list_del_unlock - remove entry from variable list
+ * @entry: entry to remove
+ *
+ * Remove @entry from the variable list and release the list lock.
+ *
+ * NOTE: slightly weird locking semantics here - we expect to be
+ * called with the efivars lock already held, and we release it before
+ * returning. This is because this function is usually called after
+ * set_variable() while the lock is still held.
+ */
+static void efivar_entry_list_del_unlock(struct efivar_entry *entry)
+{
+	WARN_ON(!spin_is_locked(&__efivars->lock));
+
+	list_del(&entry->list);
+	spin_unlock_irq(&__efivars->lock);
+}
+
+/**
+ * __efivar_entry_delete - delete an EFI variable
+ * @entry: entry containing EFI variable to delete
+ *
+ * Delete the variable from the firmware but leave @entry on the
+ * variable list.
+ *
+ * This function differs from efivar_entry_delete() because it does
+ * not remove @entry from the variable list. Also, it is safe to be
+ * called from within a efivar_entry_iter_begin() and
+ * efivar_entry_iter_end() region, unlike efivar_entry_delete().
+ *
+ * Returns 0 on success, or a converted EFI status code if
+ * set_variable() fails.
+ */
+int __efivar_entry_delete(struct efivar_entry *entry)
+{
+	const struct efivar_operations *ops = __efivars->ops;
+	efi_status_t status;
+
+	WARN_ON(!spin_is_locked(&__efivars->lock));
+
+	status = ops->set_variable(entry->var.VariableName,
+				   &entry->var.VendorGuid,
+				   0, 0, NULL);
+
+	return efi_status_to_err(status);
+}
+EXPORT_SYMBOL_GPL(__efivar_entry_delete);
+
+/**
+ * efivar_entry_delete - delete variable and remove entry from list
+ * @entry: entry containing variable to delete
+ *
+ * Delete the variable from the firmware and remove @entry from the
+ * variable list. It is the caller's responsibility to free @entry
+ * once we return.
+ *
+ * Returns 0 on success, or a converted EFI status code if
+ * set_variable() fails.
+ */
+int efivar_entry_delete(struct efivar_entry *entry)
+{
+	const struct efivar_operations *ops = __efivars->ops;
+	efi_status_t status;
+
+	spin_lock_irq(&__efivars->lock);
+	status = ops->set_variable(entry->var.VariableName,
+				   &entry->var.VendorGuid,
+				   0, 0, NULL);
+	if (!(status == EFI_SUCCESS || status == EFI_NOT_FOUND)) {
+		spin_unlock_irq(&__efivars->lock);
+		return efi_status_to_err(status);
+	}
+
+	efivar_entry_list_del_unlock(entry);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(efivar_entry_delete);
+
+/**
+ * efivar_entry_set - call set_variable()
+ * @entry: entry containing the EFI variable to write
+ * @attributes: variable attributes
+ * @size: size of @data buffer
+ * @data: buffer containing variable data
+ * @head: head of variable list
+ *
+ * Calls set_variable() for an EFI variable. If creating a new EFI
+ * variable, this function is usually followed by efivar_entry_add().
+ *
+ * Before writing the variable, the remaining EFI variable storage
+ * space is checked to ensure there is enough room available.
+ *
+ * If @head is not NULL a lookup is performed to determine whether
+ * the entry is already on the list.
+ *
+ * Returns 0 on success, -EEXIST if a lookup is performed and the entry
+ * already exists on the list, or a converted EFI status code if
+ * set_variable() fails.
+ */
+int efivar_entry_set(struct efivar_entry *entry, u32 attributes,
+		     unsigned long size, void *data, struct list_head *head)
+{
+	const struct efivar_operations *ops = __efivars->ops;
+	efi_status_t status;
+	efi_char16_t *name = entry->var.VariableName;
+	efi_guid_t vendor = entry->var.VendorGuid;
+
+	spin_lock_irq(&__efivars->lock);
+
+	if (head && efivar_entry_find(name, vendor, head, false)) {
+		spin_unlock_irq(&__efivars->lock);
+		return -EEXIST;
+	}
+
+	status = check_var_size(attributes, size + ucs2_strsize(name, 1024));
+	if (status == EFI_SUCCESS || status == EFI_UNSUPPORTED)
+		status = ops->set_variable(name, &vendor,
+					   attributes, size, data);
+
+	spin_unlock_irq(&__efivars->lock);
+
+	return efi_status_to_err(status);
+
+}
+EXPORT_SYMBOL_GPL(efivar_entry_set);
+
+/**
+ * efivar_entry_set_safe - call set_variable() if enough space in firmware
+ * @name: buffer containing the variable name
+ * @vendor: variable vendor guid
+ * @attributes: variable attributes
+ * @block: can we block in this context?
+ * @size: size of @data buffer
+ * @data: buffer containing variable data
+ *
+ * Ensures there is enough free storage in the firmware for this variable, and
+ * if so, calls set_variable(). If creating a new EFI variable, this function
+ * is usually followed by efivar_entry_add().
+ *
+ * Returns 0 on success, -ENOSPC if the firmware does not have enough
+ * space for set_variable() to succeed, or a converted EFI status code
+ * if set_variable() fails.
+ */
+int efivar_entry_set_safe(efi_char16_t *name, efi_guid_t vendor, u32 attributes,
+			  bool block, unsigned long size, void *data)
+{
+	const struct efivar_operations *ops = __efivars->ops;
+	unsigned long flags;
+	efi_status_t status;
+
+	if (!ops->query_variable_store)
+		return -ENOSYS;
+
+	if (!block) {
+		if (!spin_trylock_irqsave(&__efivars->lock, flags))
+			return -EBUSY;
+	} else {
+		spin_lock_irqsave(&__efivars->lock, flags);
+	}
+
+	status = check_var_size(attributes, size + ucs2_strsize(name, 1024));
+	if (status != EFI_SUCCESS) {
+		spin_unlock_irqrestore(&__efivars->lock, flags);
+		return -ENOSPC;
+	}
+
+	status = ops->set_variable(name, &vendor, attributes, size, data);
+
+	spin_unlock_irqrestore(&__efivars->lock, flags);
+
+	return efi_status_to_err(status);
+}
+EXPORT_SYMBOL_GPL(efivar_entry_set_safe);
+
+/**
+ * efivar_entry_find - search for an entry
+ * @name: the EFI variable name
+ * @guid: the EFI variable vendor's guid
+ * @head: head of the variable list
+ * @remove: should we remove the entry from the list?
+ *
+ * Search for an entry on the variable list that has the EFI variable
+ * name @name and vendor guid @guid. If an entry is found on the list
+ * and @remove is true, the entry is removed from the list.
+ *
+ * The caller MUST call efivar_entry_iter_begin() and
+ * efivar_entry_iter_end() before and after the invocation of this
+ * function, respectively.
+ *
+ * Returns the entry if found on the list, %NULL otherwise.
+ */
+struct efivar_entry *efivar_entry_find(efi_char16_t *name, efi_guid_t guid,
+				       struct list_head *head, bool remove)
+{
+	struct efivar_entry *entry, *n;
+	int strsize1, strsize2;
+	bool found = false;
+
+	WARN_ON(!spin_is_locked(&__efivars->lock));
+
+	list_for_each_entry_safe(entry, n, head, list) {
+		strsize1 = ucs2_strsize(name, 1024);
+		strsize2 = ucs2_strsize(entry->var.VariableName, 1024);
+		if (strsize1 == strsize2 &&
+		    !memcmp(name, &(entry->var.VariableName), strsize1) &&
+		    !efi_guidcmp(guid, entry->var.VendorGuid)) {
+			found = true;
+			break;
+		}
+	}
+
+	if (!found)
+		return NULL;
+
+	if (remove)
+		list_del(&entry->list);
+
+	return entry;
+}
+EXPORT_SYMBOL_GPL(efivar_entry_find);
+
+/**
+ * efivar_entry_size - obtain the size of a variable
+ * @entry: entry for this variable
+ * @size: location to store the variable's size
+ */
+int efivar_entry_size(struct efivar_entry *entry, unsigned long *size)
+{
+	const struct efivar_operations *ops = __efivars->ops;
+	efi_status_t status;
+
+	*size = 0;
+
+	spin_lock_irq(&__efivars->lock);
+	status = ops->get_variable(entry->var.VariableName,
+				   &entry->var.VendorGuid, NULL, size, NULL);
+	spin_unlock_irq(&__efivars->lock);
+
+	if (status != EFI_BUFFER_TOO_SMALL)
+		return efi_status_to_err(status);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(efivar_entry_size);
+
+/**
+ * __efivar_entry_get - call get_variable()
+ * @entry: read data for this variable
+ * @attributes: variable attributes
+ * @size: size of @data buffer
+ * @data: buffer to store variable data
+ *
+ * The caller MUST call efivar_entry_iter_begin() and
+ * efivar_entry_iter_end() before and after the invocation of this
+ * function, respectively.
+ */
+int __efivar_entry_get(struct efivar_entry *entry, u32 *attributes,
+		       unsigned long *size, void *data)
+{
+	const struct efivar_operations *ops = __efivars->ops;
+	efi_status_t status;
+
+	WARN_ON(!spin_is_locked(&__efivars->lock));
+
+	status = ops->get_variable(entry->var.VariableName,
+				   &entry->var.VendorGuid,
+				   attributes, size, data);
+
+	return efi_status_to_err(status);
+}
+EXPORT_SYMBOL_GPL(__efivar_entry_get);
+
+/**
+ * efivar_entry_get - call get_variable()
+ * @entry: read data for this variable
+ * @attributes: variable attributes
+ * @size: size of @data buffer
+ * @data: buffer to store variable data
+ */
+int efivar_entry_get(struct efivar_entry *entry, u32 *attributes,
+		     unsigned long *size, void *data)
+{
+	const struct efivar_operations *ops = __efivars->ops;
+	efi_status_t status;
+
+	spin_lock_irq(&__efivars->lock);
+	status = ops->get_variable(entry->var.VariableName,
+				   &entry->var.VendorGuid,
+				   attributes, size, data);
+	spin_unlock_irq(&__efivars->lock);
+
+	return efi_status_to_err(status);
+}
+EXPORT_SYMBOL_GPL(efivar_entry_get);
+
+/**
+ * efivar_entry_set_get_size - call set_variable() and get new size (atomic)
+ * @entry: entry containing variable to set and get
+ * @attributes: attributes of variable to be written
+ * @size: size of data buffer
+ * @data: buffer containing data to write
+ * @set: did the set_variable() call succeed?
+ *
+ * This is a pretty special (complex) function. See efivarfs_file_write().
+ *
+ * Atomically call set_variable() for @entry and if the call is
+ * successful, return the new size of the variable from get_variable()
+ * in @size. The success of set_variable() is indicated by @set.
+ *
+ * Returns 0 on success, -EINVAL if the variable data is invalid,
+ * -ENOSPC if the firmware does not have enough available space, or a
+ * converted EFI status code if either of set_variable() or
+ * get_variable() fail.
+ *
+ * If the EFI variable does not exist when calling set_variable()
+ * (EFI_NOT_FOUND), @entry is removed from the variable list.
+ */
+int efivar_entry_set_get_size(struct efivar_entry *entry, u32 attributes,
+			      unsigned long *size, void *data, bool *set)
+{
+	const struct efivar_operations *ops = __efivars->ops;
+	efi_char16_t *name = entry->var.VariableName;
+	efi_guid_t *vendor = &entry->var.VendorGuid;
+	efi_status_t status;
+	int err;
+
+	*set = false;
+
+	if (efivar_validate(&entry->var, data, *size) == false)
+		return -EINVAL;
+
+	/*
+	 * The lock here protects the get_variable call, the conditional
+	 * set_variable call, and removal of the variable from the efivars
+	 * list (in the case of an authenticated delete).
+	 */
+	spin_lock_irq(&__efivars->lock);
+
+	/*
+	 * Ensure that the available space hasn't shrunk below the safe level
+	 */
+	status = check_var_size(attributes, *size + ucs2_strsize(name, 1024));
+	if (status != EFI_SUCCESS) {
+		if (status != EFI_UNSUPPORTED) {
+			err = efi_status_to_err(status);
+			goto out;
+		}
+
+		if (*size > 65536) {
+			err = -ENOSPC;
+			goto out;
+		}
+	}
+
+	status = ops->set_variable(name, vendor, attributes, *size, data);
+	if (status != EFI_SUCCESS) {
+		err = efi_status_to_err(status);
+		goto out;
+	}
+
+	*set = true;
+
+	/*
+	 * Writing to the variable may have caused a change in size (which
+	 * could either be an append or an overwrite), or the variable to be
+	 * deleted. Perform a GetVariable() so we can tell what actually
+	 * happened.
+	 */
+	*size = 0;
+	status = ops->get_variable(entry->var.VariableName,
+				   &entry->var.VendorGuid,
+				   NULL, size, NULL);
+
+	if (status == EFI_NOT_FOUND)
+		efivar_entry_list_del_unlock(entry);
+	else
+		spin_unlock_irq(&__efivars->lock);
+
+	if (status && status != EFI_BUFFER_TOO_SMALL)
+		return efi_status_to_err(status);
+
+	return 0;
+
+out:
+	spin_unlock_irq(&__efivars->lock);
+	return err;
+
+}
+EXPORT_SYMBOL_GPL(efivar_entry_set_get_size);
+
+/**
+ * efivar_entry_iter_begin - begin iterating the variable list
+ *
+ * Lock the variable list to prevent entry insertion and removal until
+ * efivar_entry_iter_end() is called. This function is usually used in
+ * conjunction with __efivar_entry_iter() or efivar_entry_iter().
+ */
+void efivar_entry_iter_begin(void)
+{
+	spin_lock_irq(&__efivars->lock);
+}
+EXPORT_SYMBOL_GPL(efivar_entry_iter_begin);
+
+/**
+ * efivar_entry_iter_end - finish iterating the variable list
+ *
+ * Unlock the variable list and allow modifications to the list again.
+ */
+void efivar_entry_iter_end(void)
+{
+	spin_unlock_irq(&__efivars->lock);
+}
+EXPORT_SYMBOL_GPL(efivar_entry_iter_end);
+
+/**
+ * __efivar_entry_iter - iterate over variable list
+ * @func: callback function
+ * @head: head of the variable list
+ * @data: function-specific data to pass to callback
+ * @prev: entry to begin iterating from
+ *
+ * Iterate over the list of EFI variables and call @func with every
+ * entry on the list. It is safe for @func to remove entries in the
+ * list via efivar_entry_delete().
+ *
+ * You MUST call efivar_enter_iter_begin() before this function, and
+ * efivar_entry_iter_end() afterwards.
+ *
+ * It is possible to begin iteration from an arbitrary entry within
+ * the list by passing @prev. @prev is updated on return to point to
+ * the last entry passed to @func. To begin iterating from the
+ * beginning of the list @prev must be %NULL.
+ *
+ * The restrictions for @func are the same as documented for
+ * efivar_entry_iter().
+ */
+int __efivar_entry_iter(int (*func)(struct efivar_entry *, void *),
+			struct list_head *head, void *data,
+			struct efivar_entry **prev)
+{
+	struct efivar_entry *entry, *n;
+	int err = 0;
+
+	if (!prev || !*prev) {
+		list_for_each_entry_safe(entry, n, head, list) {
+			err = func(entry, data);
+			if (err)
+				break;
+		}
+
+		if (prev)
+			*prev = entry;
+
+		return err;
+	}
+
+
+	list_for_each_entry_safe_continue((*prev), n, head, list) {
+		err = func(*prev, data);
+		if (err)
+			break;
+	}
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(__efivar_entry_iter);
+
+/**
+ * efivar_entry_iter - iterate over variable list
+ * @func: callback function
+ * @head: head of variable list
+ * @data: function-specific data to pass to callback
+ *
+ * Iterate over the list of EFI variables and call @func with every
+ * entry on the list. It is safe for @func to remove entries in the
+ * list via efivar_entry_delete() while iterating.
+ *
+ * Some notes for the callback function:
+ *  - a non-zero return value indicates an error and terminates the loop
+ *  - @func is called from atomic context
+ */
+int efivar_entry_iter(int (*func)(struct efivar_entry *, void *),
+		      struct list_head *head, void *data)
+{
+	int err = 0;
+
+	efivar_entry_iter_begin();
+	err = __efivar_entry_iter(func, head, data, NULL);
+	efivar_entry_iter_end();
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(efivar_entry_iter);
+
+/**
+ * efivars_kobject - get the kobject for the registered efivars
+ *
+ * If efivars_register() has not been called we return NULL,
+ * otherwise return the kobject used at registration time.
+ */
+struct kobject *efivars_kobject(void)
+{
+	if (!__efivars)
+		return NULL;
+
+	return __efivars->kobject;
+}
+EXPORT_SYMBOL_GPL(efivars_kobject);
+
+/**
+ * efivar_run_worker - schedule the efivar worker thread
+ */
+void efivar_run_worker(void)
+{
+	if (efivar_wq_enabled)
+		schedule_work(&efivar_work);
+}
+EXPORT_SYMBOL_GPL(efivar_run_worker);
+
+/**
+ * efivars_register - register an efivars
+ * @efivars: efivars to register
+ * @ops: efivars operations
+ * @kobject: @efivars-specific kobject
+ *
+ * Only a single efivars can be registered at any time.
+ */
+int efivars_register(struct efivars *efivars,
+		     const struct efivar_operations *ops,
+		     struct kobject *kobject)
+{
+	spin_lock_init(&efivars->lock);
+	efivars->ops = ops;
+	efivars->kobject = kobject;
+
+	__efivars = efivars;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(efivars_register);
+
+/**
+ * efivars_unregister - unregister an efivars
+ * @efivars: efivars to unregister
+ *
+ * The caller must have already removed every entry from the list,
+ * failure to do so is an error.
+ */
+int efivars_unregister(struct efivars *efivars)
+{
+	int rv;
+
+	if (!__efivars) {
+		printk(KERN_ERR "efivars not registered\n");
+		rv = -EINVAL;
+		goto out;
+	}
+
+	if (__efivars != efivars) {
+		rv = -EINVAL;
+		goto out;
+	}
+
+	__efivars = NULL;
+
+	rv = 0;
+out:
+	return rv;
+}
+EXPORT_SYMBOL_GPL(efivars_unregister);
diff --git a/drivers/firmware/efivars.c b/drivers/firmware/efivars.c
deleted file mode 100644
index f4baa11d3644..000000000000
--- a/drivers/firmware/efivars.c
+++ /dev/null
@@ -1,2117 +0,0 @@
-/*
- * EFI Variables - efivars.c
- *
- * Copyright (C) 2001,2003,2004 Dell <Matt_Domsch@dell.com>
- * Copyright (C) 2004 Intel Corporation <matthew.e.tolentino@intel.com>
- *
- * This code takes all variables accessible from EFI runtime and
- *  exports them via sysfs
- *
- *  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
- *
- * Changelog:
- *
- *  17 May 2004 - Matt Domsch <Matt_Domsch@dell.com>
- *   remove check for efi_enabled in exit
- *   add MODULE_VERSION
- *
- *  26 Apr 2004 - Matt Domsch <Matt_Domsch@dell.com>
- *   minor bug fixes
- *
- *  21 Apr 2004 - Matt Tolentino <matthew.e.tolentino@intel.com)
- *   converted driver to export variable information via sysfs
- *   and moved to drivers/firmware directory
- *   bumped revision number to v0.07 to reflect conversion & move
- *
- *  10 Dec 2002 - Matt Domsch <Matt_Domsch@dell.com>
- *   fix locking per Peter Chubb's findings
- *
- *  25 Mar 2002 - Matt Domsch <Matt_Domsch@dell.com>
- *   move uuid_unparse() to include/asm-ia64/efi.h:efi_guid_unparse()
- *
- *  12 Feb 2002 - Matt Domsch <Matt_Domsch@dell.com>
- *   use list_for_each_safe when deleting vars.
- *   remove ifdef CONFIG_SMP around include <linux/smp.h>
- *   v0.04 release to linux-ia64@linuxia64.org
- *
- *  20 April 2001 - Matt Domsch <Matt_Domsch@dell.com>
- *   Moved vars from /proc/efi to /proc/efi/vars, and made
- *   efi.c own the /proc/efi directory.
- *   v0.03 release to linux-ia64@linuxia64.org
- *
- *  26 March 2001 - Matt Domsch <Matt_Domsch@dell.com>
- *   At the request of Stephane, moved ownership of /proc/efi
- *   to efi.c, and now efivars lives under /proc/efi/vars.
- *
- *  12 March 2001 - Matt Domsch <Matt_Domsch@dell.com>
- *   Feedback received from Stephane Eranian incorporated.
- *   efivar_write() checks copy_from_user() return value.
- *   efivar_read/write() returns proper errno.
- *   v0.02 release to linux-ia64@linuxia64.org
- *
- *  26 February 2001 - Matt Domsch <Matt_Domsch@dell.com>
- *   v0.01 release to linux-ia64@linuxia64.org
- */
-
-#include <linux/capability.h>
-#include <linux/types.h>
-#include <linux/errno.h>
-#include <linux/init.h>
-#include <linux/mm.h>
-#include <linux/module.h>
-#include <linux/string.h>
-#include <linux/smp.h>
-#include <linux/efi.h>
-#include <linux/sysfs.h>
-#include <linux/kobject.h>
-#include <linux/device.h>
-#include <linux/slab.h>
-#include <linux/pstore.h>
-#include <linux/ctype.h>
-#include <linux/ucs2_string.h>
-
-#include <linux/fs.h>
-#include <linux/ramfs.h>
-#include <linux/pagemap.h>
-
-#include <asm/uaccess.h>
-
-#define EFIVARS_VERSION "0.08"
-#define EFIVARS_DATE "2004-May-17"
-
-MODULE_AUTHOR("Matt Domsch <Matt_Domsch@Dell.com>");
-MODULE_DESCRIPTION("sysfs interface to EFI Variables");
-MODULE_LICENSE("GPL");
-MODULE_VERSION(EFIVARS_VERSION);
-
-#define DUMP_NAME_LEN 52
-
-/*
- * Length of a GUID string (strlen("aaaaaaaa-bbbb-cccc-dddd-eeeeeeeeeeee"))
- * not including trailing NUL
- */
-#define GUID_LEN 36
-
-static bool efivars_pstore_disable =
-	IS_ENABLED(CONFIG_EFI_VARS_PSTORE_DEFAULT_DISABLE);
-
-module_param_named(pstore_disable, efivars_pstore_disable, bool, 0644);
-
-/*
- * The maximum size of VariableName + Data = 1024
- * Therefore, it's reasonable to save that much
- * space in each part of the structure,
- * and we use a page for reading/writing.
- */
-
-struct efi_variable {
-	efi_char16_t  VariableName[1024/sizeof(efi_char16_t)];
-	efi_guid_t    VendorGuid;
-	unsigned long DataSize;
-	__u8          Data[1024];
-	efi_status_t  Status;
-	__u32         Attributes;
-} __attribute__((packed));
-
-struct efivar_entry {
-	struct efivars *efivars;
-	struct efi_variable var;
-	struct list_head list;
-	struct kobject kobj;
-};
-
-struct efivar_attribute {
-	struct attribute attr;
-	ssize_t (*show) (struct efivar_entry *entry, char *buf);
-	ssize_t (*store)(struct efivar_entry *entry, const char *buf, size_t count);
-};
-
-static struct efivars __efivars;
-static struct efivar_operations ops;
-
-#define PSTORE_EFI_ATTRIBUTES \
-	(EFI_VARIABLE_NON_VOLATILE | \
-	 EFI_VARIABLE_BOOTSERVICE_ACCESS | \
-	 EFI_VARIABLE_RUNTIME_ACCESS)
-
-#define EFIVAR_ATTR(_name, _mode, _show, _store) \
-struct efivar_attribute efivar_attr_##_name = { \
-	.attr = {.name = __stringify(_name), .mode = _mode}, \
-	.show = _show, \
-	.store = _store, \
-};
-
-#define to_efivar_attr(_attr) container_of(_attr, struct efivar_attribute, attr)
-#define to_efivar_entry(obj)  container_of(obj, struct efivar_entry, kobj)
-
-/*
- * Prototype for sysfs creation function
- */
-static int
-efivar_create_sysfs_entry(struct efivars *efivars,
-			  unsigned long variable_name_size,
-			  efi_char16_t *variable_name,
-			  efi_guid_t *vendor_guid);
-
-/*
- * Prototype for workqueue functions updating sysfs entry
- */
-
-static void efivar_update_sysfs_entries(struct work_struct *);
-static DECLARE_WORK(efivar_work, efivar_update_sysfs_entries);
-static bool efivar_wq_enabled = true;
-
-static bool
-validate_device_path(struct efi_variable *var, int match, u8 *buffer,
-		     unsigned long len)
-{
-	struct efi_generic_dev_path *node;
-	int offset = 0;
-
-	node = (struct efi_generic_dev_path *)buffer;
-
-	if (len < sizeof(*node))
-		return false;
-
-	while (offset <= len - sizeof(*node) &&
-	       node->length >= sizeof(*node) &&
-		node->length <= len - offset) {
-		offset += node->length;
-
-		if ((node->type == EFI_DEV_END_PATH ||
-		     node->type == EFI_DEV_END_PATH2) &&
-		    node->sub_type == EFI_DEV_END_ENTIRE)
-			return true;
-
-		node = (struct efi_generic_dev_path *)(buffer + offset);
-	}
-
-	/*
-	 * If we're here then either node->length pointed past the end
-	 * of the buffer or we reached the end of the buffer without
-	 * finding a device path end node.
-	 */
-	return false;
-}
-
-static bool
-validate_boot_order(struct efi_variable *var, int match, u8 *buffer,
-		    unsigned long len)
-{
-	/* An array of 16-bit integers */
-	if ((len % 2) != 0)
-		return false;
-
-	return true;
-}
-
-static bool
-validate_load_option(struct efi_variable *var, int match, u8 *buffer,
-		     unsigned long len)
-{
-	u16 filepathlength;
-	int i, desclength = 0, namelen;
-
-	namelen = ucs2_strnlen(var->VariableName, sizeof(var->VariableName));
-
-	/* Either "Boot" or "Driver" followed by four digits of hex */
-	for (i = match; i < match+4; i++) {
-		if (var->VariableName[i] > 127 ||
-		    hex_to_bin(var->VariableName[i] & 0xff) < 0)
-			return true;
-	}
-
-	/* Reject it if there's 4 digits of hex and then further content */
-	if (namelen > match + 4)
-		return false;
-
-	/* A valid entry must be at least 8 bytes */
-	if (len < 8)
-		return false;
-
-	filepathlength = buffer[4] | buffer[5] << 8;
-
-	/*
-	 * There's no stored length for the description, so it has to be
-	 * found by hand
-	 */
-	desclength = ucs2_strsize((efi_char16_t *)(buffer + 6), len - 6) + 2;
-
-	/* Each boot entry must have a descriptor */
-	if (!desclength)
-		return false;
-
-	/*
-	 * If the sum of the length of the description, the claimed filepath
-	 * length and the original header are greater than the length of the
-	 * variable, it's malformed
-	 */
-	if ((desclength + filepathlength + 6) > len)
-		return false;
-
-	/*
-	 * And, finally, check the filepath
-	 */
-	return validate_device_path(var, match, buffer + desclength + 6,
-				    filepathlength);
-}
-
-static bool
-validate_uint16(struct efi_variable *var, int match, u8 *buffer,
-		unsigned long len)
-{
-	/* A single 16-bit integer */
-	if (len != 2)
-		return false;
-
-	return true;
-}
-
-static bool
-validate_ascii_string(struct efi_variable *var, int match, u8 *buffer,
-		      unsigned long len)
-{
-	int i;
-
-	for (i = 0; i < len; i++) {
-		if (buffer[i] > 127)
-			return false;
-
-		if (buffer[i] == 0)
-			return true;
-	}
-
-	return false;
-}
-
-struct variable_validate {
-	char *name;
-	bool (*validate)(struct efi_variable *var, int match, u8 *data,
-			 unsigned long len);
-};
-
-static const struct variable_validate variable_validate[] = {
-	{ "BootNext", validate_uint16 },
-	{ "BootOrder", validate_boot_order },
-	{ "DriverOrder", validate_boot_order },
-	{ "Boot*", validate_load_option },
-	{ "Driver*", validate_load_option },
-	{ "ConIn", validate_device_path },
-	{ "ConInDev", validate_device_path },
-	{ "ConOut", validate_device_path },
-	{ "ConOutDev", validate_device_path },
-	{ "ErrOut", validate_device_path },
-	{ "ErrOutDev", validate_device_path },
-	{ "Timeout", validate_uint16 },
-	{ "Lang", validate_ascii_string },
-	{ "PlatformLang", validate_ascii_string },
-	{ "", NULL },
-};
-
-static bool
-validate_var(struct efi_variable *var, u8 *data, unsigned long len)
-{
-	int i;
-	u16 *unicode_name = var->VariableName;
-
-	for (i = 0; variable_validate[i].validate != NULL; i++) {
-		const char *name = variable_validate[i].name;
-		int match;
-
-		for (match = 0; ; match++) {
-			char c = name[match];
-			u16 u = unicode_name[match];
-
-			/* All special variables are plain ascii */
-			if (u > 127)
-				return true;
-
-			/* Wildcard in the matching name means we've matched */
-			if (c == '*')
-				return variable_validate[i].validate(var,
-							     match, data, len);
-
-			/* Case sensitive match */
-			if (c != u)
-				break;
-
-			/* Reached the end of the string while matching */
-			if (!c)
-				return variable_validate[i].validate(var,
-							     match, data, len);
-		}
-	}
-
-	return true;
-}
-
-static efi_status_t
-get_var_data_locked(struct efivars *efivars, struct efi_variable *var)
-{
-	efi_status_t status;
-
-	var->DataSize = 1024;
-	status = efivars->ops->get_variable(var->VariableName,
-					    &var->VendorGuid,
-					    &var->Attributes,
-					    &var->DataSize,
-					    var->Data);
-	return status;
-}
-
-static efi_status_t
-get_var_data(struct efivars *efivars, struct efi_variable *var)
-{
-	efi_status_t status;
-	unsigned long flags;
-
-	spin_lock_irqsave(&efivars->lock, flags);
-	status = get_var_data_locked(efivars, var);
-	spin_unlock_irqrestore(&efivars->lock, flags);
-
-	if (status != EFI_SUCCESS) {
-		printk(KERN_WARNING "efivars: get_variable() failed 0x%lx!\n",
-			status);
-	}
-	return status;
-}
-
-static efi_status_t
-check_var_size_locked(struct efivars *efivars, u32 attributes,
-			unsigned long size)
-{
-	const struct efivar_operations *fops = efivars->ops;
-
-	if (!efivars->ops->query_variable_store)
-		return EFI_UNSUPPORTED;
-
-	return fops->query_variable_store(attributes, size);
-}
-
-
-static efi_status_t
-check_var_size(struct efivars *efivars, u32 attributes, unsigned long size)
-{
-	efi_status_t status;
-	unsigned long flags;
-
-	spin_lock_irqsave(&efivars->lock, flags);
-	status = check_var_size_locked(efivars, attributes, size);
-	spin_unlock_irqrestore(&efivars->lock, flags);
-
-	return status;
-}
-
-static ssize_t
-efivar_guid_read(struct efivar_entry *entry, char *buf)
-{
-	struct efi_variable *var = &entry->var;
-	char *str = buf;
-
-	if (!entry || !buf)
-		return 0;
-
-	efi_guid_unparse(&var->VendorGuid, str);
-	str += strlen(str);
-	str += sprintf(str, "\n");
-
-	return str - buf;
-}
-
-static ssize_t
-efivar_attr_read(struct efivar_entry *entry, char *buf)
-{
-	struct efi_variable *var = &entry->var;
-	char *str = buf;
-	efi_status_t status;
-
-	if (!entry || !buf)
-		return -EINVAL;
-
-	status = get_var_data(entry->efivars, var);
-	if (status != EFI_SUCCESS)
-		return -EIO;
-
-	if (var->Attributes & EFI_VARIABLE_NON_VOLATILE)
-		str += sprintf(str, "EFI_VARIABLE_NON_VOLATILE\n");
-	if (var->Attributes & EFI_VARIABLE_BOOTSERVICE_ACCESS)
-		str += sprintf(str, "EFI_VARIABLE_BOOTSERVICE_ACCESS\n");
-	if (var->Attributes & EFI_VARIABLE_RUNTIME_ACCESS)
-		str += sprintf(str, "EFI_VARIABLE_RUNTIME_ACCESS\n");
-	if (var->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD)
-		str += sprintf(str, "EFI_VARIABLE_HARDWARE_ERROR_RECORD\n");
-	if (var->Attributes & EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS)
-		str += sprintf(str,
-			"EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS\n");
-	if (var->Attributes &
-			EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS)
-		str += sprintf(str,
-			"EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS\n");
-	if (var->Attributes & EFI_VARIABLE_APPEND_WRITE)
-		str += sprintf(str, "EFI_VARIABLE_APPEND_WRITE\n");
-	return str - buf;
-}
-
-static ssize_t
-efivar_size_read(struct efivar_entry *entry, char *buf)
-{
-	struct efi_variable *var = &entry->var;
-	char *str = buf;
-	efi_status_t status;
-
-	if (!entry || !buf)
-		return -EINVAL;
-
-	status = get_var_data(entry->efivars, var);
-	if (status != EFI_SUCCESS)
-		return -EIO;
-
-	str += sprintf(str, "0x%lx\n", var->DataSize);
-	return str - buf;
-}
-
-static ssize_t
-efivar_data_read(struct efivar_entry *entry, char *buf)
-{
-	struct efi_variable *var = &entry->var;
-	efi_status_t status;
-
-	if (!entry || !buf)
-		return -EINVAL;
-
-	status = get_var_data(entry->efivars, var);
-	if (status != EFI_SUCCESS)
-		return -EIO;
-
-	memcpy(buf, var->Data, var->DataSize);
-	return var->DataSize;
-}
-/*
- * We allow each variable to be edited via rewriting the
- * entire efi variable structure.
- */
-static ssize_t
-efivar_store_raw(struct efivar_entry *entry, const char *buf, size_t count)
-{
-	struct efi_variable *new_var, *var = &entry->var;
-	struct efivars *efivars = entry->efivars;
-	efi_status_t status = EFI_NOT_FOUND;
-
-	if (count != sizeof(struct efi_variable))
-		return -EINVAL;
-
-	new_var = (struct efi_variable *)buf;
-	/*
-	 * If only updating the variable data, then the name
-	 * and guid should remain the same
-	 */
-	if (memcmp(new_var->VariableName, var->VariableName, sizeof(var->VariableName)) ||
-		efi_guidcmp(new_var->VendorGuid, var->VendorGuid)) {
-		printk(KERN_ERR "efivars: Cannot edit the wrong variable!\n");
-		return -EINVAL;
-	}
-
-	if ((new_var->DataSize <= 0) || (new_var->Attributes == 0)){
-		printk(KERN_ERR "efivars: DataSize & Attributes must be valid!\n");
-		return -EINVAL;
-	}
-
-	if ((new_var->Attributes & ~EFI_VARIABLE_MASK) != 0 ||
-	    validate_var(new_var, new_var->Data, new_var->DataSize) == false) {
-		printk(KERN_ERR "efivars: Malformed variable content\n");
-		return -EINVAL;
-	}
-
-	spin_lock_irq(&efivars->lock);
-
-	status = check_var_size_locked(efivars, new_var->Attributes,
-	       new_var->DataSize + ucs2_strsize(new_var->VariableName, 1024));
-
-	if (status == EFI_SUCCESS || status == EFI_UNSUPPORTED)
-		status = efivars->ops->set_variable(new_var->VariableName,
-						    &new_var->VendorGuid,
-						    new_var->Attributes,
-						    new_var->DataSize,
-						    new_var->Data);
-
-	spin_unlock_irq(&efivars->lock);
-
-	if (status != EFI_SUCCESS) {
-		printk(KERN_WARNING "efivars: set_variable() failed: status=%lx\n",
-			status);
-		return -EIO;
-	}
-
-	memcpy(&entry->var, new_var, count);
-	return count;
-}
-
-static ssize_t
-efivar_show_raw(struct efivar_entry *entry, char *buf)
-{
-	struct efi_variable *var = &entry->var;
-	efi_status_t status;
-
-	if (!entry || !buf)
-		return 0;
-
-	status = get_var_data(entry->efivars, var);
-	if (status != EFI_SUCCESS)
-		return -EIO;
-
-	memcpy(buf, var, sizeof(*var));
-	return sizeof(*var);
-}
-
-/*
- * Generic read/write functions that call the specific functions of
- * the attributes...
- */
-static ssize_t efivar_attr_show(struct kobject *kobj, struct attribute *attr,
-				char *buf)
-{
-	struct efivar_entry *var = to_efivar_entry(kobj);
-	struct efivar_attribute *efivar_attr = to_efivar_attr(attr);
-	ssize_t ret = -EIO;
-
-	if (!capable(CAP_SYS_ADMIN))
-		return -EACCES;
-
-	if (efivar_attr->show) {
-		ret = efivar_attr->show(var, buf);
-	}
-	return ret;
-}
-
-static ssize_t efivar_attr_store(struct kobject *kobj, struct attribute *attr,
-				const char *buf, size_t count)
-{
-	struct efivar_entry *var = to_efivar_entry(kobj);
-	struct efivar_attribute *efivar_attr = to_efivar_attr(attr);
-	ssize_t ret = -EIO;
-
-	if (!capable(CAP_SYS_ADMIN))
-		return -EACCES;
-
-	if (efivar_attr->store)
-		ret = efivar_attr->store(var, buf, count);
-
-	return ret;
-}
-
-static const struct sysfs_ops efivar_attr_ops = {
-	.show = efivar_attr_show,
-	.store = efivar_attr_store,
-};
-
-static void efivar_release(struct kobject *kobj)
-{
-	struct efivar_entry *var = container_of(kobj, struct efivar_entry, kobj);
-	kfree(var);
-}
-
-static EFIVAR_ATTR(guid, 0400, efivar_guid_read, NULL);
-static EFIVAR_ATTR(attributes, 0400, efivar_attr_read, NULL);
-static EFIVAR_ATTR(size, 0400, efivar_size_read, NULL);
-static EFIVAR_ATTR(data, 0400, efivar_data_read, NULL);
-static EFIVAR_ATTR(raw_var, 0600, efivar_show_raw, efivar_store_raw);
-
-static struct attribute *def_attrs[] = {
-	&efivar_attr_guid.attr,
-	&efivar_attr_size.attr,
-	&efivar_attr_attributes.attr,
-	&efivar_attr_data.attr,
-	&efivar_attr_raw_var.attr,
-	NULL,
-};
-
-static struct kobj_type efivar_ktype = {
-	.release = efivar_release,
-	.sysfs_ops = &efivar_attr_ops,
-	.default_attrs = def_attrs,
-};
-
-static inline void
-efivar_unregister(struct efivar_entry *var)
-{
-	kobject_put(&var->kobj);
-}
-
-static int efivarfs_file_open(struct inode *inode, struct file *file)
-{
-	file->private_data = inode->i_private;
-	return 0;
-}
-
-static int efi_status_to_err(efi_status_t status)
-{
-	int err;
-
-	switch (status) {
-	case EFI_INVALID_PARAMETER:
-		err = -EINVAL;
-		break;
-	case EFI_OUT_OF_RESOURCES:
-		err = -ENOSPC;
-		break;
-	case EFI_DEVICE_ERROR:
-		err = -EIO;
-		break;
-	case EFI_WRITE_PROTECTED:
-		err = -EROFS;
-		break;
-	case EFI_SECURITY_VIOLATION:
-		err = -EACCES;
-		break;
-	case EFI_NOT_FOUND:
-		err = -EIO;
-		break;
-	default:
-		err = -EINVAL;
-	}
-
-	return err;
-}
-
-static ssize_t efivarfs_file_write(struct file *file,
-		const char __user *userbuf, size_t count, loff_t *ppos)
-{
-	struct efivar_entry *var = file->private_data;
-	struct efivars *efivars;
-	efi_status_t status;
-	void *data;
-	u32 attributes;
-	struct inode *inode = file->f_mapping->host;
-	unsigned long datasize = count - sizeof(attributes);
-	unsigned long newdatasize, varsize;
-	ssize_t bytes = 0;
-
-	if (count < sizeof(attributes))
-		return -EINVAL;
-
-	if (copy_from_user(&attributes, userbuf, sizeof(attributes)))
-		return -EFAULT;
-
-	if (attributes & ~(EFI_VARIABLE_MASK))
-		return -EINVAL;
-
-	efivars = var->efivars;
-
-	/*
-	 * Ensure that the user can't allocate arbitrarily large
-	 * amounts of memory. Pick a default size of 64K if
-	 * QueryVariableInfo() isn't supported by the firmware.
-	 */
-
-	varsize = datasize + ucs2_strsize(var->var.VariableName, 1024);
-	status = check_var_size(efivars, attributes, varsize);
-
-	if (status != EFI_SUCCESS) {
-		if (status != EFI_UNSUPPORTED)
-			return efi_status_to_err(status);
-
-		if (datasize > 65536)
-			return -ENOSPC;
-	}
-
-	data = kmalloc(datasize, GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
-
-	if (copy_from_user(data, userbuf + sizeof(attributes), datasize)) {
-		bytes = -EFAULT;
-		goto out;
-	}
-
-	if (validate_var(&var->var, data, datasize) == false) {
-		bytes = -EINVAL;
-		goto out;
-	}
-
-	/*
-	 * The lock here protects the get_variable call, the conditional
-	 * set_variable call, and removal of the variable from the efivars
-	 * list (in the case of an authenticated delete).
-	 */
-	spin_lock_irq(&efivars->lock);
-
-	/*
-	 * Ensure that the available space hasn't shrunk below the safe level
-	 */
-
-	status = check_var_size_locked(efivars, attributes, varsize);
-
-	if (status != EFI_SUCCESS && status != EFI_UNSUPPORTED) {
-		spin_unlock_irq(&efivars->lock);
-		kfree(data);
-
-		return efi_status_to_err(status);
-	}
-
-	status = efivars->ops->set_variable(var->var.VariableName,
-					    &var->var.VendorGuid,
-					    attributes, datasize,
-					    data);
-
-	if (status != EFI_SUCCESS) {
-		spin_unlock_irq(&efivars->lock);
-		kfree(data);
-
-		return efi_status_to_err(status);
-	}
-
-	bytes = count;
-
-	/*
-	 * Writing to the variable may have caused a change in size (which
-	 * could either be an append or an overwrite), or the variable to be
-	 * deleted. Perform a GetVariable() so we can tell what actually
-	 * happened.
-	 */
-	newdatasize = 0;
-	status = efivars->ops->get_variable(var->var.VariableName,
-					    &var->var.VendorGuid,
-					    NULL, &newdatasize,
-					    NULL);
-
-	if (status == EFI_BUFFER_TOO_SMALL) {
-		spin_unlock_irq(&efivars->lock);
-		mutex_lock(&inode->i_mutex);
-		i_size_write(inode, newdatasize + sizeof(attributes));
-		mutex_unlock(&inode->i_mutex);
-
-	} else if (status == EFI_NOT_FOUND) {
-		list_del(&var->list);
-		spin_unlock_irq(&efivars->lock);
-		efivar_unregister(var);
-		drop_nlink(inode);
-		d_delete(file->f_dentry);
-		dput(file->f_dentry);
-
-	} else {
-		spin_unlock_irq(&efivars->lock);
-		pr_warn("efivarfs: inconsistent EFI variable implementation? "
-				"status = %lx\n", status);
-	}
-
-out:
-	kfree(data);
-
-	return bytes;
-}
-
-static ssize_t efivarfs_file_read(struct file *file, char __user *userbuf,
-		size_t count, loff_t *ppos)
-{
-	struct efivar_entry *var = file->private_data;
-	struct efivars *efivars = var->efivars;
-	efi_status_t status;
-	unsigned long datasize = 0;
-	u32 attributes;
-	void *data;
-	ssize_t size = 0;
-
-	spin_lock_irq(&efivars->lock);
-	status = efivars->ops->get_variable(var->var.VariableName,
-					    &var->var.VendorGuid,
-					    &attributes, &datasize, NULL);
-	spin_unlock_irq(&efivars->lock);
-
-	if (status != EFI_BUFFER_TOO_SMALL)
-		return efi_status_to_err(status);
-
-	data = kmalloc(datasize + sizeof(attributes), GFP_KERNEL);
-
-	if (!data)
-		return -ENOMEM;
-
-	spin_lock_irq(&efivars->lock);
-	status = efivars->ops->get_variable(var->var.VariableName,
-					    &var->var.VendorGuid,
-					    &attributes, &datasize,
-					    (data + sizeof(attributes)));
-	spin_unlock_irq(&efivars->lock);
-
-	if (status != EFI_SUCCESS) {
-		size = efi_status_to_err(status);
-		goto out_free;
-	}
-
-	memcpy(data, &attributes, sizeof(attributes));
-	size = simple_read_from_buffer(userbuf, count, ppos,
-				       data, datasize + sizeof(attributes));
-out_free:
-	kfree(data);
-
-	return size;
-}
-
-static void efivarfs_evict_inode(struct inode *inode)
-{
-	clear_inode(inode);
-}
-
-static const struct super_operations efivarfs_ops = {
-	.statfs = simple_statfs,
-	.drop_inode = generic_delete_inode,
-	.evict_inode = efivarfs_evict_inode,
-	.show_options = generic_show_options,
-};
-
-static struct super_block *efivarfs_sb;
-
-static const struct inode_operations efivarfs_dir_inode_operations;
-
-static const struct file_operations efivarfs_file_operations = {
-	.open	= efivarfs_file_open,
-	.read	= efivarfs_file_read,
-	.write	= efivarfs_file_write,
-	.llseek	= no_llseek,
-};
-
-static struct inode *efivarfs_get_inode(struct super_block *sb,
-				const struct inode *dir, int mode, dev_t dev)
-{
-	struct inode *inode = new_inode(sb);
-
-	if (inode) {
-		inode->i_ino = get_next_ino();
-		inode->i_mode = mode;
-		inode->i_atime = inode->i_mtime = inode->i_ctime = CURRENT_TIME;
-		switch (mode & S_IFMT) {
-		case S_IFREG:
-			inode->i_fop = &efivarfs_file_operations;
-			break;
-		case S_IFDIR:
-			inode->i_op = &efivarfs_dir_inode_operations;
-			inode->i_fop = &simple_dir_operations;
-			inc_nlink(inode);
-			break;
-		}
-	}
-	return inode;
-}
-
-/*
- * Return true if 'str' is a valid efivarfs filename of the form,
- *
- *	VariableName-12345678-1234-1234-1234-1234567891bc
- */
-static bool efivarfs_valid_name(const char *str, int len)
-{
-	static const char dashes[GUID_LEN] = {
-		[8] = 1, [13] = 1, [18] = 1, [23] = 1
-	};
-	const char *s = str + len - GUID_LEN;
-	int i;
-
-	/*
-	 * We need a GUID, plus at least one letter for the variable name,
-	 * plus the '-' separator
-	 */
-	if (len < GUID_LEN + 2)
-		return false;
-
-	/* GUID must be preceded by a '-' */
-	if (*(s - 1) != '-')
-		return false;
-
-	/*
-	 * Validate that 's' is of the correct format, e.g.
-	 *
-	 *	12345678-1234-1234-1234-123456789abc
-	 */
-	for (i = 0; i < GUID_LEN; i++) {
-		if (dashes[i]) {
-			if (*s++ != '-')
-				return false;
-		} else {
-			if (!isxdigit(*s++))
-				return false;
-		}
-	}
-
-	return true;
-}
-
-static void efivarfs_hex_to_guid(const char *str, efi_guid_t *guid)
-{
-	guid->b[0] = hex_to_bin(str[6]) << 4 | hex_to_bin(str[7]);
-	guid->b[1] = hex_to_bin(str[4]) << 4 | hex_to_bin(str[5]);
-	guid->b[2] = hex_to_bin(str[2]) << 4 | hex_to_bin(str[3]);
-	guid->b[3] = hex_to_bin(str[0]) << 4 | hex_to_bin(str[1]);
-	guid->b[4] = hex_to_bin(str[11]) << 4 | hex_to_bin(str[12]);
-	guid->b[5] = hex_to_bin(str[9]) << 4 | hex_to_bin(str[10]);
-	guid->b[6] = hex_to_bin(str[16]) << 4 | hex_to_bin(str[17]);
-	guid->b[7] = hex_to_bin(str[14]) << 4 | hex_to_bin(str[15]);
-	guid->b[8] = hex_to_bin(str[19]) << 4 | hex_to_bin(str[20]);
-	guid->b[9] = hex_to_bin(str[21]) << 4 | hex_to_bin(str[22]);
-	guid->b[10] = hex_to_bin(str[24]) << 4 | hex_to_bin(str[25]);
-	guid->b[11] = hex_to_bin(str[26]) << 4 | hex_to_bin(str[27]);
-	guid->b[12] = hex_to_bin(str[28]) << 4 | hex_to_bin(str[29]);
-	guid->b[13] = hex_to_bin(str[30]) << 4 | hex_to_bin(str[31]);
-	guid->b[14] = hex_to_bin(str[32]) << 4 | hex_to_bin(str[33]);
-	guid->b[15] = hex_to_bin(str[34]) << 4 | hex_to_bin(str[35]);
-}
-
-static int efivarfs_create(struct inode *dir, struct dentry *dentry,
-			  umode_t mode, bool excl)
-{
-	struct inode *inode;
-	struct efivars *efivars = &__efivars;
-	struct efivar_entry *var;
-	int namelen, i = 0, err = 0;
-
-	if (!efivarfs_valid_name(dentry->d_name.name, dentry->d_name.len))
-		return -EINVAL;
-
-	inode = efivarfs_get_inode(dir->i_sb, dir, mode, 0);
-	if (!inode)
-		return -ENOMEM;
-
-	var = kzalloc(sizeof(struct efivar_entry), GFP_KERNEL);
-	if (!var) {
-		err = -ENOMEM;
-		goto out;
-	}
-
-	/* length of the variable name itself: remove GUID and separator */
-	namelen = dentry->d_name.len - GUID_LEN - 1;
-
-	efivarfs_hex_to_guid(dentry->d_name.name + namelen + 1,
-			&var->var.VendorGuid);
-
-	for (i = 0; i < namelen; i++)
-		var->var.VariableName[i] = dentry->d_name.name[i];
-
-	var->var.VariableName[i] = '\0';
-
-	inode->i_private = var;
-	var->efivars = efivars;
-	var->kobj.kset = efivars->kset;
-
-	err = kobject_init_and_add(&var->kobj, &efivar_ktype, NULL, "%s",
-			     dentry->d_name.name);
-	if (err)
-		goto out;
-
-	kobject_uevent(&var->kobj, KOBJ_ADD);
-	spin_lock_irq(&efivars->lock);
-	list_add(&var->list, &efivars->list);
-	spin_unlock_irq(&efivars->lock);
-	d_instantiate(dentry, inode);
-	dget(dentry);
-out:
-	if (err) {
-		kfree(var);
-		iput(inode);
-	}
-	return err;
-}
-
-static int efivarfs_unlink(struct inode *dir, struct dentry *dentry)
-{
-	struct efivar_entry *var = dentry->d_inode->i_private;
-	struct efivars *efivars = var->efivars;
-	efi_status_t status;
-
-	spin_lock_irq(&efivars->lock);
-
-	status = efivars->ops->set_variable(var->var.VariableName,
-					    &var->var.VendorGuid,
-					    0, 0, NULL);
-
-	if (status == EFI_SUCCESS || status == EFI_NOT_FOUND) {
-		list_del(&var->list);
-		spin_unlock_irq(&efivars->lock);
-		efivar_unregister(var);
-		drop_nlink(dentry->d_inode);
-		dput(dentry);
-		return 0;
-	}
-
-	spin_unlock_irq(&efivars->lock);
-	return -EINVAL;
-};
-
-/*
- * Compare two efivarfs file names.
- *
- * An efivarfs filename is composed of two parts,
- *
- *	1. A case-sensitive variable name
- *	2. A case-insensitive GUID
- *
- * So we need to perform a case-sensitive match on part 1 and a
- * case-insensitive match on part 2.
- */
-static int efivarfs_d_compare(const struct dentry *parent, const struct inode *pinode,
-			      const struct dentry *dentry, const struct inode *inode,
-			      unsigned int len, const char *str,
-			      const struct qstr *name)
-{
-	int guid = len - GUID_LEN;
-
-	if (name->len != len)
-		return 1;
-
-	/* Case-sensitive compare for the variable name */
-	if (memcmp(str, name->name, guid))
-		return 1;
-
-	/* Case-insensitive compare for the GUID */
-	return strncasecmp(name->name + guid, str + guid, GUID_LEN);
-}
-
-static int efivarfs_d_hash(const struct dentry *dentry,
-			   const struct inode *inode, struct qstr *qstr)
-{
-	unsigned long hash = init_name_hash();
-	const unsigned char *s = qstr->name;
-	unsigned int len = qstr->len;
-
-	if (!efivarfs_valid_name(s, len))
-		return -EINVAL;
-
-	while (len-- > GUID_LEN)
-		hash = partial_name_hash(*s++, hash);
-
-	/* GUID is case-insensitive. */
-	while (len--)
-		hash = partial_name_hash(tolower(*s++), hash);
-
-	qstr->hash = end_name_hash(hash);
-	return 0;
-}
-
-/*
- * Retaining negative dentries for an in-memory filesystem just wastes
- * memory and lookup time: arrange for them to be deleted immediately.
- */
-static int efivarfs_delete_dentry(const struct dentry *dentry)
-{
-	return 1;
-}
-
-static struct dentry_operations efivarfs_d_ops = {
-	.d_compare = efivarfs_d_compare,
-	.d_hash = efivarfs_d_hash,
-	.d_delete = efivarfs_delete_dentry,
-};
-
-static struct dentry *efivarfs_alloc_dentry(struct dentry *parent, char *name)
-{
-	struct dentry *d;
-	struct qstr q;
-	int err;
-
-	q.name = name;
-	q.len = strlen(name);
-
-	err = efivarfs_d_hash(NULL, NULL, &q);
-	if (err)
-		return ERR_PTR(err);
-
-	d = d_alloc(parent, &q);
-	if (d)
-		return d;
-
-	return ERR_PTR(-ENOMEM);
-}
-
-static int efivarfs_fill_super(struct super_block *sb, void *data, int silent)
-{
-	struct inode *inode = NULL;
-	struct dentry *root;
-	struct efivar_entry *entry, *n;
-	struct efivars *efivars = &__efivars;
-	char *name;
-	int err = -ENOMEM;
-
-	efivarfs_sb = sb;
-
-	sb->s_maxbytes          = MAX_LFS_FILESIZE;
-	sb->s_blocksize         = PAGE_CACHE_SIZE;
-	sb->s_blocksize_bits    = PAGE_CACHE_SHIFT;
-	sb->s_magic             = EFIVARFS_MAGIC;
-	sb->s_op                = &efivarfs_ops;
-	sb->s_d_op		= &efivarfs_d_ops;
-	sb->s_time_gran         = 1;
-
-	inode = efivarfs_get_inode(sb, NULL, S_IFDIR | 0755, 0);
-	if (!inode)
-		return -ENOMEM;
-	inode->i_op = &efivarfs_dir_inode_operations;
-
-	root = d_make_root(inode);
-	sb->s_root = root;
-	if (!root)
-		return -ENOMEM;
-
-	list_for_each_entry_safe(entry, n, &efivars->list, list) {
-		struct dentry *dentry, *root = efivarfs_sb->s_root;
-		unsigned long size = 0;
-		int len, i;
-
-		inode = NULL;
-
-		len = ucs2_strlen(entry->var.VariableName);
-
-		/* name, plus '-', plus GUID, plus NUL*/
-		name = kmalloc(len + 1 + GUID_LEN + 1, GFP_ATOMIC);
-		if (!name)
-			goto fail;
-
-		for (i = 0; i < len; i++)
-			name[i] = entry->var.VariableName[i] & 0xFF;
-
-		name[len] = '-';
-
-		efi_guid_unparse(&entry->var.VendorGuid, name + len + 1);
-
-		name[len+GUID_LEN+1] = '\0';
-
-		inode = efivarfs_get_inode(efivarfs_sb, root->d_inode,
-					  S_IFREG | 0644, 0);
-		if (!inode)
-			goto fail_name;
-
-		dentry = efivarfs_alloc_dentry(root, name);
-		if (IS_ERR(dentry)) {
-			err = PTR_ERR(dentry);
-			goto fail_inode;
-		}
-
-		/* copied by the above to local storage in the dentry. */
-		kfree(name);
-
-		spin_lock_irq(&efivars->lock);
-		efivars->ops->get_variable(entry->var.VariableName,
-					   &entry->var.VendorGuid,
-					   &entry->var.Attributes,
-					   &size,
-					   NULL);
-		spin_unlock_irq(&efivars->lock);
-
-		mutex_lock(&inode->i_mutex);
-		inode->i_private = entry;
-		i_size_write(inode, size + sizeof(entry->var.Attributes));
-		mutex_unlock(&inode->i_mutex);
-		d_add(dentry, inode);
-	}
-
-	return 0;
-
-fail_inode:
-	iput(inode);
-fail_name:
-	kfree(name);
-fail:
-	return err;
-}
-
-static struct dentry *efivarfs_mount(struct file_system_type *fs_type,
-				    int flags, const char *dev_name, void *data)
-{
-	return mount_single(fs_type, flags, data, efivarfs_fill_super);
-}
-
-static void efivarfs_kill_sb(struct super_block *sb)
-{
-	kill_litter_super(sb);
-	efivarfs_sb = NULL;
-}
-
-static struct file_system_type efivarfs_type = {
-	.name    = "efivarfs",
-	.mount   = efivarfs_mount,
-	.kill_sb = efivarfs_kill_sb,
-};
-MODULE_ALIAS_FS("efivarfs");
-
-/*
- * Handle negative dentry.
- */
-static struct dentry *efivarfs_lookup(struct inode *dir, struct dentry *dentry,
-				      unsigned int flags)
-{
-	if (dentry->d_name.len > NAME_MAX)
-		return ERR_PTR(-ENAMETOOLONG);
-	d_add(dentry, NULL);
-	return NULL;
-}
-
-static const struct inode_operations efivarfs_dir_inode_operations = {
-	.lookup = efivarfs_lookup,
-	.unlink = efivarfs_unlink,
-	.create = efivarfs_create,
-};
-
-#ifdef CONFIG_EFI_VARS_PSTORE
-
-static int efi_pstore_open(struct pstore_info *psi)
-{
-	struct efivars *efivars = psi->data;
-
-	spin_lock_irq(&efivars->lock);
-	efivars->walk_entry = list_first_entry(&efivars->list,
-					       struct efivar_entry, list);
-	return 0;
-}
-
-static int efi_pstore_close(struct pstore_info *psi)
-{
-	struct efivars *efivars = psi->data;
-
-	spin_unlock_irq(&efivars->lock);
-	return 0;
-}
-
-static ssize_t efi_pstore_read(u64 *id, enum pstore_type_id *type,
-			       int *count, struct timespec *timespec,
-			       char **buf, struct pstore_info *psi)
-{
-	efi_guid_t vendor = LINUX_EFI_CRASH_GUID;
-	struct efivars *efivars = psi->data;
-	char name[DUMP_NAME_LEN];
-	int i;
-	int cnt;
-	unsigned int part, size;
-	unsigned long time;
-
-	while (&efivars->walk_entry->list != &efivars->list) {
-		if (!efi_guidcmp(efivars->walk_entry->var.VendorGuid,
-				 vendor)) {
-			for (i = 0; i < DUMP_NAME_LEN; i++) {
-				name[i] = efivars->walk_entry->var.VariableName[i];
-			}
-			if (sscanf(name, "dump-type%u-%u-%d-%lu",
-				   type, &part, &cnt, &time) == 4) {
-				*id = part;
-				*count = cnt;
-				timespec->tv_sec = time;
-				timespec->tv_nsec = 0;
-			} else if (sscanf(name, "dump-type%u-%u-%lu",
-				   type, &part, &time) == 3) {
-				/*
-				 * Check if an old format,
-				 * which doesn't support holding
-				 * multiple logs, remains.
-				 */
-				*id = part;
-				*count = 0;
-				timespec->tv_sec = time;
-				timespec->tv_nsec = 0;
-			} else {
-				efivars->walk_entry = list_entry(
-						efivars->walk_entry->list.next,
-						struct efivar_entry, list);
-				continue;
-			}
-
-			get_var_data_locked(efivars, &efivars->walk_entry->var);
-			size = efivars->walk_entry->var.DataSize;
-			*buf = kmalloc(size, GFP_KERNEL);
-			if (*buf == NULL)
-				return -ENOMEM;
-			memcpy(*buf, efivars->walk_entry->var.Data,
-			       size);
-			efivars->walk_entry = list_entry(
-					efivars->walk_entry->list.next,
-					struct efivar_entry, list);
-			return size;
-		}
-		efivars->walk_entry = list_entry(efivars->walk_entry->list.next,
-						 struct efivar_entry, list);
-	}
-	return 0;
-}
-
-static int efi_pstore_write(enum pstore_type_id type,
-		enum kmsg_dump_reason reason, u64 *id,
-		unsigned int part, int count, size_t size,
-		struct pstore_info *psi)
-{
-	char name[DUMP_NAME_LEN];
-	efi_char16_t efi_name[DUMP_NAME_LEN];
-	efi_guid_t vendor = LINUX_EFI_CRASH_GUID;
-	struct efivars *efivars = psi->data;
-	int i, ret = 0;
-	efi_status_t status = EFI_NOT_FOUND;
-	unsigned long flags;
-
-	if (pstore_cannot_block_path(reason)) {
-		/*
-		 * If the lock is taken by another cpu in non-blocking path,
-		 * this driver returns without entering firmware to avoid
-		 * hanging up.
-		 */
-		if (!spin_trylock_irqsave(&efivars->lock, flags))
-			return -EBUSY;
-	} else
-		spin_lock_irqsave(&efivars->lock, flags);
-
-	/*
-	 * Check if there is a space enough to log.
-	 * size: a size of logging data
-	 * DUMP_NAME_LEN * 2: a maximum size of variable name
-	 */
-
-	status = check_var_size_locked(efivars, PSTORE_EFI_ATTRIBUTES,
-					 size + DUMP_NAME_LEN * 2);
-
-	if (status) {
-		spin_unlock_irqrestore(&efivars->lock, flags);
-		*id = part;
-		return -ENOSPC;
-	}
-
-	sprintf(name, "dump-type%u-%u-%d-%lu", type, part, count,
-		get_seconds());
-
-	for (i = 0; i < DUMP_NAME_LEN; i++)
-		efi_name[i] = name[i];
-
-	efivars->ops->set_variable(efi_name, &vendor, PSTORE_EFI_ATTRIBUTES,
-				   size, psi->buf);
-
-	spin_unlock_irqrestore(&efivars->lock, flags);
-
-	if (reason == KMSG_DUMP_OOPS && efivar_wq_enabled)
-		schedule_work(&efivar_work);
-
-	*id = part;
-	return ret;
-};
-
-static int efi_pstore_erase(enum pstore_type_id type, u64 id, int count,
-			    struct timespec time, struct pstore_info *psi)
-{
-	char name[DUMP_NAME_LEN];
-	efi_char16_t efi_name[DUMP_NAME_LEN];
-	char name_old[DUMP_NAME_LEN];
-	efi_char16_t efi_name_old[DUMP_NAME_LEN];
-	efi_guid_t vendor = LINUX_EFI_CRASH_GUID;
-	struct efivars *efivars = psi->data;
-	struct efivar_entry *entry, *found = NULL;
-	int i;
-
-	sprintf(name, "dump-type%u-%u-%d-%lu", type, (unsigned int)id, count,
-		time.tv_sec);
-
-	spin_lock_irq(&efivars->lock);
-
-	for (i = 0; i < DUMP_NAME_LEN; i++)
-		efi_name[i] = name[i];
-
-	/*
-	 * Clean up an entry with the same name
-	 */
-
-	list_for_each_entry(entry, &efivars->list, list) {
-		get_var_data_locked(efivars, &entry->var);
-
-		if (efi_guidcmp(entry->var.VendorGuid, vendor))
-			continue;
-		if (ucs2_strncmp(entry->var.VariableName, efi_name,
-				  ucs2_strlen(efi_name))) {
-			/*
-			 * Check if an old format,
-			 * which doesn't support holding
-			 * multiple logs, remains.
-			 */
-			sprintf(name_old, "dump-type%u-%u-%lu", type,
-				(unsigned int)id, time.tv_sec);
-
-			for (i = 0; i < DUMP_NAME_LEN; i++)
-				efi_name_old[i] = name_old[i];
-
-			if (ucs2_strncmp(entry->var.VariableName, efi_name_old,
-					  ucs2_strlen(efi_name_old)))
-				continue;
-		}
-
-		/* found */
-		found = entry;
-		efivars->ops->set_variable(entry->var.VariableName,
-					   &entry->var.VendorGuid,
-					   PSTORE_EFI_ATTRIBUTES,
-					   0, NULL);
-		break;
-	}
-
-	if (found)
-		list_del(&found->list);
-
-	spin_unlock_irq(&efivars->lock);
-
-	if (found)
-		efivar_unregister(found);
-
-	return 0;
-}
-
-static struct pstore_info efi_pstore_info = {
-	.owner		= THIS_MODULE,
-	.name		= "efi",
-	.open		= efi_pstore_open,
-	.close		= efi_pstore_close,
-	.read		= efi_pstore_read,
-	.write		= efi_pstore_write,
-	.erase		= efi_pstore_erase,
-};
-
-static void efivar_pstore_register(struct efivars *efivars)
-{
-	efivars->efi_pstore_info = efi_pstore_info;
-	efivars->efi_pstore_info.buf = kmalloc(4096, GFP_KERNEL);
-	if (efivars->efi_pstore_info.buf) {
-		efivars->efi_pstore_info.bufsize = 1024;
-		efivars->efi_pstore_info.data = efivars;
-		spin_lock_init(&efivars->efi_pstore_info.buf_lock);
-		pstore_register(&efivars->efi_pstore_info);
-	}
-}
-#else
-static void efivar_pstore_register(struct efivars *efivars)
-{
-	return;
-}
-#endif
-
-static ssize_t efivar_create(struct file *filp, struct kobject *kobj,
-			     struct bin_attribute *bin_attr,
-			     char *buf, loff_t pos, size_t count)
-{
-	struct efi_variable *new_var = (struct efi_variable *)buf;
-	struct efivars *efivars = bin_attr->private;
-	struct efivar_entry *search_efivar, *n;
-	unsigned long strsize1, strsize2;
-	efi_status_t status = EFI_NOT_FOUND;
-	int found = 0;
-
-	if (!capable(CAP_SYS_ADMIN))
-		return -EACCES;
-
-	if ((new_var->Attributes & ~EFI_VARIABLE_MASK) != 0 ||
-	    validate_var(new_var, new_var->Data, new_var->DataSize) == false) {
-		printk(KERN_ERR "efivars: Malformed variable content\n");
-		return -EINVAL;
-	}
-
-	spin_lock_irq(&efivars->lock);
-
-	/*
-	 * Does this variable already exist?
-	 */
-	list_for_each_entry_safe(search_efivar, n, &efivars->list, list) {
-		strsize1 = ucs2_strsize(search_efivar->var.VariableName, 1024);
-		strsize2 = ucs2_strsize(new_var->VariableName, 1024);
-		if (strsize1 == strsize2 &&
-			!memcmp(&(search_efivar->var.VariableName),
-				new_var->VariableName, strsize1) &&
-			!efi_guidcmp(search_efivar->var.VendorGuid,
-				new_var->VendorGuid)) {
-			found = 1;
-			break;
-		}
-	}
-	if (found) {
-		spin_unlock_irq(&efivars->lock);
-		return -EINVAL;
-	}
-
-	status = check_var_size_locked(efivars, new_var->Attributes,
-	       new_var->DataSize + ucs2_strsize(new_var->VariableName, 1024));
-
-	if (status && status != EFI_UNSUPPORTED) {
-		spin_unlock_irq(&efivars->lock);
-		return efi_status_to_err(status);
-	}
-
-	/* now *really* create the variable via EFI */
-	status = efivars->ops->set_variable(new_var->VariableName,
-					    &new_var->VendorGuid,
-					    new_var->Attributes,
-					    new_var->DataSize,
-					    new_var->Data);
-
-	if (status != EFI_SUCCESS) {
-		printk(KERN_WARNING "efivars: set_variable() failed: status=%lx\n",
-			status);
-		spin_unlock_irq(&efivars->lock);
-		return -EIO;
-	}
-	spin_unlock_irq(&efivars->lock);
-
-	/* Create the entry in sysfs.  Locking is not required here */
-	status = efivar_create_sysfs_entry(efivars,
-					   ucs2_strsize(new_var->VariableName,
-							 1024),
-					   new_var->VariableName,
-					   &new_var->VendorGuid);
-	if (status) {
-		printk(KERN_WARNING "efivars: variable created, but sysfs entry wasn't.\n");
-	}
-	return count;
-}
-
-static ssize_t efivar_delete(struct file *filp, struct kobject *kobj,
-			     struct bin_attribute *bin_attr,
-			     char *buf, loff_t pos, size_t count)
-{
-	struct efi_variable *del_var = (struct efi_variable *)buf;
-	struct efivars *efivars = bin_attr->private;
-	struct efivar_entry *search_efivar, *n;
-	unsigned long strsize1, strsize2;
-	efi_status_t status = EFI_NOT_FOUND;
-	int found = 0;
-
-	if (!capable(CAP_SYS_ADMIN))
-		return -EACCES;
-
-	spin_lock_irq(&efivars->lock);
-
-	/*
-	 * Does this variable already exist?
-	 */
-	list_for_each_entry_safe(search_efivar, n, &efivars->list, list) {
-		strsize1 = ucs2_strsize(search_efivar->var.VariableName, 1024);
-		strsize2 = ucs2_strsize(del_var->VariableName, 1024);
-		if (strsize1 == strsize2 &&
-			!memcmp(&(search_efivar->var.VariableName),
-				del_var->VariableName, strsize1) &&
-			!efi_guidcmp(search_efivar->var.VendorGuid,
-				del_var->VendorGuid)) {
-			found = 1;
-			break;
-		}
-	}
-	if (!found) {
-		spin_unlock_irq(&efivars->lock);
-		return -EINVAL;
-	}
-	/* force the Attributes/DataSize to 0 to ensure deletion */
-	del_var->Attributes = 0;
-	del_var->DataSize = 0;
-
-	status = efivars->ops->set_variable(del_var->VariableName,
-					    &del_var->VendorGuid,
-					    del_var->Attributes,
-					    del_var->DataSize,
-					    del_var->Data);
-
-	if (status != EFI_SUCCESS) {
-		printk(KERN_WARNING "efivars: set_variable() failed: status=%lx\n",
-			status);
-		spin_unlock_irq(&efivars->lock);
-		return -EIO;
-	}
-	list_del(&search_efivar->list);
-	/* We need to release this lock before unregistering. */
-	spin_unlock_irq(&efivars->lock);
-	efivar_unregister(search_efivar);
-
-	/* It's dead Jim.... */
-	return count;
-}
-
-static bool variable_is_present(struct efivars *efivars,
-				efi_char16_t *variable_name,
-				efi_guid_t *vendor)
-{
-	struct efivar_entry *entry, *n;
-	unsigned long strsize1, strsize2;
-	bool found = false;
-
-	strsize1 = ucs2_strsize(variable_name, 1024);
-	list_for_each_entry_safe(entry, n, &efivars->list, list) {
-		strsize2 = ucs2_strsize(entry->var.VariableName, 1024);
-		if (strsize1 == strsize2 &&
-			!memcmp(variable_name, &(entry->var.VariableName),
-				strsize2) &&
-			!efi_guidcmp(entry->var.VendorGuid,
-				*vendor)) {
-			found = true;
-			break;
-		}
-	}
-	return found;
-}
-
-/*
- * Returns the size of variable_name, in bytes, including the
- * terminating NULL character, or variable_name_size if no NULL
- * character is found among the first variable_name_size bytes.
- */
-static unsigned long var_name_strnsize(efi_char16_t *variable_name,
-				       unsigned long variable_name_size)
-{
-	unsigned long len;
-	efi_char16_t c;
-
-	/*
-	 * The variable name is, by definition, a NULL-terminated
-	 * string, so make absolutely sure that variable_name_size is
-	 * the value we expect it to be. If not, return the real size.
-	 */
-	for (len = 2; len <= variable_name_size; len += sizeof(c)) {
-		c = variable_name[(len / sizeof(c)) - 1];
-		if (!c)
-			break;
-	}
-
-	return min(len, variable_name_size);
-}
-
-static void efivar_update_sysfs_entries(struct work_struct *work)
-{
-	struct efivars *efivars = &__efivars;
-	efi_guid_t vendor;
-	efi_char16_t *variable_name;
-	unsigned long variable_name_size = 1024;
-	efi_status_t status = EFI_NOT_FOUND;
-	bool found;
-
-	/* Add new sysfs entries */
-	while (1) {
-		variable_name = kzalloc(variable_name_size, GFP_KERNEL);
-		if (!variable_name) {
-			pr_err("efivars: Memory allocation failed.\n");
-			return;
-		}
-
-		spin_lock_irq(&efivars->lock);
-		found = false;
-		while (1) {
-			variable_name_size = 1024;
-			status = efivars->ops->get_next_variable(
-							&variable_name_size,
-							variable_name,
-							&vendor);
-			if (status != EFI_SUCCESS) {
-				break;
-			} else {
-				if (!variable_is_present(efivars,
-				    variable_name, &vendor)) {
-					found = true;
-					break;
-				}
-			}
-		}
-		spin_unlock_irq(&efivars->lock);
-
-		if (!found) {
-			kfree(variable_name);
-			break;
-		} else {
-			variable_name_size = var_name_strnsize(variable_name,
-							       variable_name_size);
-			efivar_create_sysfs_entry(efivars,
-						  variable_name_size,
-						  variable_name, &vendor);
-		}
-	}
-}
-
-/*
- * Let's not leave out systab information that snuck into
- * the efivars driver
- */
-static ssize_t systab_show(struct kobject *kobj,
-			   struct kobj_attribute *attr, char *buf)
-{
-	char *str = buf;
-
-	if (!kobj || !buf)
-		return -EINVAL;
-
-	if (efi.mps != EFI_INVALID_TABLE_ADDR)
-		str += sprintf(str, "MPS=0x%lx\n", efi.mps);
-	if (efi.acpi20 != EFI_INVALID_TABLE_ADDR)
-		str += sprintf(str, "ACPI20=0x%lx\n", efi.acpi20);
-	if (efi.acpi != EFI_INVALID_TABLE_ADDR)
-		str += sprintf(str, "ACPI=0x%lx\n", efi.acpi);
-	if (efi.smbios != EFI_INVALID_TABLE_ADDR)
-		str += sprintf(str, "SMBIOS=0x%lx\n", efi.smbios);
-	if (efi.hcdp != EFI_INVALID_TABLE_ADDR)
-		str += sprintf(str, "HCDP=0x%lx\n", efi.hcdp);
-	if (efi.boot_info != EFI_INVALID_TABLE_ADDR)
-		str += sprintf(str, "BOOTINFO=0x%lx\n", efi.boot_info);
-	if (efi.uga != EFI_INVALID_TABLE_ADDR)
-		str += sprintf(str, "UGA=0x%lx\n", efi.uga);
-
-	return str - buf;
-}
-
-static struct kobj_attribute efi_attr_systab =
-			__ATTR(systab, 0400, systab_show, NULL);
-
-static struct attribute *efi_subsys_attrs[] = {
-	&efi_attr_systab.attr,
-	NULL,	/* maybe more in the future? */
-};
-
-static struct attribute_group efi_subsys_attr_group = {
-	.attrs = efi_subsys_attrs,
-};
-
-static struct kobject *efi_kobj;
-
-/*
- * efivar_create_sysfs_entry()
- * Requires:
- *    variable_name_size = number of bytes required to hold
- *                         variable_name (not counting the NULL
- *                         character at the end.
- *    efivars->lock is not held on entry or exit.
- * Returns 1 on failure, 0 on success
- */
-static int
-efivar_create_sysfs_entry(struct efivars *efivars,
-			  unsigned long variable_name_size,
-			  efi_char16_t *variable_name,
-			  efi_guid_t *vendor_guid)
-{
-	int i, short_name_size;
-	char *short_name;
-	struct efivar_entry *new_efivar;
-
-	/*
-	 * Length of the variable bytes in ASCII, plus the '-' separator,
-	 * plus the GUID, plus trailing NUL
-	 */
-	short_name_size = variable_name_size / sizeof(efi_char16_t)
-				+ 1 + GUID_LEN + 1;
-
-	short_name = kzalloc(short_name_size, GFP_KERNEL);
-	new_efivar = kzalloc(sizeof(struct efivar_entry), GFP_KERNEL);
-
-	if (!short_name || !new_efivar)  {
-		kfree(short_name);
-		kfree(new_efivar);
-		return 1;
-	}
-
-	new_efivar->efivars = efivars;
-	memcpy(new_efivar->var.VariableName, variable_name,
-		variable_name_size);
-	memcpy(&(new_efivar->var.VendorGuid), vendor_guid, sizeof(efi_guid_t));
-
-	/* Convert Unicode to normal chars (assume top bits are 0),
-	   ala UTF-8 */
-	for (i=0; i < (int)(variable_name_size / sizeof(efi_char16_t)); i++) {
-		short_name[i] = variable_name[i] & 0xFF;
-	}
-	/* This is ugly, but necessary to separate one vendor's
-	   private variables from another's.         */
-
-	*(short_name + strlen(short_name)) = '-';
-	efi_guid_unparse(vendor_guid, short_name + strlen(short_name));
-
-	new_efivar->kobj.kset = efivars->kset;
-	i = kobject_init_and_add(&new_efivar->kobj, &efivar_ktype, NULL,
-				 "%s", short_name);
-	if (i) {
-		kfree(short_name);
-		kfree(new_efivar);
-		return 1;
-	}
-
-	kobject_uevent(&new_efivar->kobj, KOBJ_ADD);
-	kfree(short_name);
-	short_name = NULL;
-
-	spin_lock_irq(&efivars->lock);
-	list_add(&new_efivar->list, &efivars->list);
-	spin_unlock_irq(&efivars->lock);
-
-	return 0;
-}
-
-static int
-create_efivars_bin_attributes(struct efivars *efivars)
-{
-	struct bin_attribute *attr;
-	int error;
-
-	/* new_var */
-	attr = kzalloc(sizeof(*attr), GFP_KERNEL);
-	if (!attr)
-		return -ENOMEM;
-
-	attr->attr.name = "new_var";
-	attr->attr.mode = 0200;
-	attr->write = efivar_create;
-	attr->private = efivars;
-	efivars->new_var = attr;
-
-	/* del_var */
-	attr = kzalloc(sizeof(*attr), GFP_KERNEL);
-	if (!attr) {
-		error = -ENOMEM;
-		goto out_free;
-	}
-	attr->attr.name = "del_var";
-	attr->attr.mode = 0200;
-	attr->write = efivar_delete;
-	attr->private = efivars;
-	efivars->del_var = attr;
-
-	sysfs_bin_attr_init(efivars->new_var);
-	sysfs_bin_attr_init(efivars->del_var);
-
-	/* Register */
-	error = sysfs_create_bin_file(&efivars->kset->kobj,
-				      efivars->new_var);
-	if (error) {
-		printk(KERN_ERR "efivars: unable to create new_var sysfs file"
-			" due to error %d\n", error);
-		goto out_free;
-	}
-	error = sysfs_create_bin_file(&efivars->kset->kobj,
-				      efivars->del_var);
-	if (error) {
-		printk(KERN_ERR "efivars: unable to create del_var sysfs file"
-			" due to error %d\n", error);
-		sysfs_remove_bin_file(&efivars->kset->kobj,
-				      efivars->new_var);
-		goto out_free;
-	}
-
-	return 0;
-out_free:
-	kfree(efivars->del_var);
-	efivars->del_var = NULL;
-	kfree(efivars->new_var);
-	efivars->new_var = NULL;
-	return error;
-}
-
-void unregister_efivars(struct efivars *efivars)
-{
-	struct efivar_entry *entry, *n;
-
-	list_for_each_entry_safe(entry, n, &efivars->list, list) {
-		spin_lock_irq(&efivars->lock);
-		list_del(&entry->list);
-		spin_unlock_irq(&efivars->lock);
-		efivar_unregister(entry);
-	}
-	if (efivars->new_var)
-		sysfs_remove_bin_file(&efivars->kset->kobj, efivars->new_var);
-	if (efivars->del_var)
-		sysfs_remove_bin_file(&efivars->kset->kobj, efivars->del_var);
-	kfree(efivars->new_var);
-	kfree(efivars->del_var);
-	kobject_put(efivars->kobject);
-	kset_unregister(efivars->kset);
-}
-EXPORT_SYMBOL_GPL(unregister_efivars);
-
-/*
- * Print a warning when duplicate EFI variables are encountered and
- * disable the sysfs workqueue since the firmware is buggy.
- */
-static void dup_variable_bug(efi_char16_t *s16, efi_guid_t *vendor_guid,
-			     unsigned long len16)
-{
-	size_t i, len8 = len16 / sizeof(efi_char16_t);
-	char *s8;
-
-	/*
-	 * Disable the workqueue since the algorithm it uses for
-	 * detecting new variables won't work with this buggy
-	 * implementation of GetNextVariableName().
-	 */
-	efivar_wq_enabled = false;
-
-	s8 = kzalloc(len8, GFP_KERNEL);
-	if (!s8)
-		return;
-
-	for (i = 0; i < len8; i++)
-		s8[i] = s16[i];
-
-	printk(KERN_WARNING "efivars: duplicate variable: %s-%pUl\n",
-	       s8, vendor_guid);
-	kfree(s8);
-}
-
-int register_efivars(struct efivars *efivars,
-		     const struct efivar_operations *ops,
-		     struct kobject *parent_kobj)
-{
-	efi_status_t status = EFI_NOT_FOUND;
-	efi_guid_t vendor_guid;
-	efi_char16_t *variable_name;
-	unsigned long variable_name_size = 1024;
-	int error = 0;
-
-	variable_name = kzalloc(variable_name_size, GFP_KERNEL);
-	if (!variable_name) {
-		printk(KERN_ERR "efivars: Memory allocation failed.\n");
-		return -ENOMEM;
-	}
-
-	spin_lock_init(&efivars->lock);
-	INIT_LIST_HEAD(&efivars->list);
-	efivars->ops = ops;
-
-	efivars->kset = kset_create_and_add("vars", NULL, parent_kobj);
-	if (!efivars->kset) {
-		printk(KERN_ERR "efivars: Subsystem registration failed.\n");
-		error = -ENOMEM;
-		goto out;
-	}
-
-	efivars->kobject = kobject_create_and_add("efivars", parent_kobj);
-	if (!efivars->kobject) {
-		pr_err("efivars: Subsystem registration failed.\n");
-		error = -ENOMEM;
-		kset_unregister(efivars->kset);
-		goto out;
-	}
-
-	/*
-	 * Per EFI spec, the maximum storage allocated for both
-	 * the variable name and variable data is 1024 bytes.
-	 */
-
-	do {
-		variable_name_size = 1024;
-
-		status = ops->get_next_variable(&variable_name_size,
-						variable_name,
-						&vendor_guid);
-		switch (status) {
-		case EFI_SUCCESS:
-			variable_name_size = var_name_strnsize(variable_name,
-							       variable_name_size);
-
-			/*
-			 * Some firmware implementations return the
-			 * same variable name on multiple calls to
-			 * get_next_variable(). Terminate the loop
-			 * immediately as there is no guarantee that
-			 * we'll ever see a different variable name,
-			 * and may end up looping here forever.
-			 */
-			if (variable_is_present(efivars, variable_name,
-						&vendor_guid)) {
-				dup_variable_bug(variable_name, &vendor_guid,
-						 variable_name_size);
-				status = EFI_NOT_FOUND;
-				break;
-			}
-
-			efivar_create_sysfs_entry(efivars,
-						  variable_name_size,
-						  variable_name,
-						  &vendor_guid);
-			break;
-		case EFI_NOT_FOUND:
-			break;
-		default:
-			printk(KERN_WARNING "efivars: get_next_variable: status=%lx\n",
-				status);
-			status = EFI_NOT_FOUND;
-			break;
-		}
-	} while (status != EFI_NOT_FOUND);
-
-	error = create_efivars_bin_attributes(efivars);
-	if (error)
-		unregister_efivars(efivars);
-
-	if (!efivars_pstore_disable)
-		efivar_pstore_register(efivars);
-
-	register_filesystem(&efivarfs_type);
-
-out:
-	kfree(variable_name);
-
-	return error;
-}
-EXPORT_SYMBOL_GPL(register_efivars);
-
-/*
- * For now we register the efi subsystem with the firmware subsystem
- * and the vars subsystem with the efi subsystem.  In the future, it
- * might make sense to split off the efi subsystem into its own
- * driver, but for now only efivars will register with it, so just
- * include it here.
- */
-
-static int __init
-efivars_init(void)
-{
-	int error = 0;
-
-	printk(KERN_INFO "EFI Variables Facility v%s %s\n", EFIVARS_VERSION,
-	       EFIVARS_DATE);
-
-	if (!efi_enabled(EFI_RUNTIME_SERVICES))
-		return 0;
-
-	/* For now we'll register the efi directory at /sys/firmware/efi */
-	efi_kobj = kobject_create_and_add("efi", firmware_kobj);
-	if (!efi_kobj) {
-		printk(KERN_ERR "efivars: Firmware registration failed.\n");
-		return -ENOMEM;
-	}
-
-	ops.get_variable = efi.get_variable;
-	ops.set_variable = efi.set_variable;
-	ops.get_next_variable = efi.get_next_variable;
-	ops.query_variable_store = efi_query_variable_store;
-
-	error = register_efivars(&__efivars, &ops, efi_kobj);
-	if (error)
-		goto err_put;
-
-	/* Don't forget the systab entry */
-	error = sysfs_create_group(efi_kobj, &efi_subsys_attr_group);
-	if (error) {
-		printk(KERN_ERR
-		       "efivars: Sysfs attribute export failed with error %d.\n",
-		       error);
-		goto err_unregister;
-	}
-
-	return 0;
-
-err_unregister:
-	unregister_efivars(&__efivars);
-err_put:
-	kobject_put(efi_kobj);
-	return error;
-}
-
-static void __exit
-efivars_exit(void)
-{
-	cancel_work_sync(&efivar_work);
-
-	if (efi_enabled(EFI_RUNTIME_SERVICES)) {
-		unregister_efivars(&__efivars);
-		kobject_put(efi_kobj);
-	}
-}
-
-module_init(efivars_init);
-module_exit(efivars_exit);
-
diff --git a/drivers/firmware/google/gsmi.c b/drivers/firmware/google/gsmi.c
index 91ddf0f7a1b9..acba0b9f4406 100644
--- a/drivers/firmware/google/gsmi.c
+++ b/drivers/firmware/google/gsmi.c
@@ -28,6 +28,7 @@
 #include <linux/reboot.h>
 #include <linux/efi.h>
 #include <linux/module.h>
+#include <linux/ucs2_string.h>
 
 #define GSMI_SHUTDOWN_CLEAN	0	/* Clean Shutdown */
 /* TODO(mikew@google.com): Tie in HARDLOCKUP_DETECTOR with NMIWDT */
@@ -288,17 +289,6 @@ static int gsmi_exec(u8 func, u8 sub)
 	return rc;
 }
 
-/* Return the number of unicode characters in data */
-static size_t
-utf16_strlen(efi_char16_t *data, unsigned long maxlength)
-{
-	unsigned long length = 0;
-
-	while (*data++ != 0 && length < maxlength)
-		length++;
-	return length;
-}
-
 static efi_status_t gsmi_get_variable(efi_char16_t *name,
 				      efi_guid_t *vendor, u32 *attr,
 				      unsigned long *data_size,
@@ -311,7 +301,7 @@ static efi_status_t gsmi_get_variable(efi_char16_t *name,
 	};
 	efi_status_t ret = EFI_SUCCESS;
 	unsigned long flags;
-	size_t name_len = utf16_strlen(name, GSMI_BUF_SIZE / 2);
+	size_t name_len = ucs2_strnlen(name, GSMI_BUF_SIZE / 2);
 	int rc;
 
 	if (name_len >= GSMI_BUF_SIZE / 2)
@@ -380,7 +370,7 @@ static efi_status_t gsmi_get_next_variable(unsigned long *name_size,
 		return EFI_BAD_BUFFER_SIZE;
 
 	/* Let's make sure the thing is at least null-terminated */
-	if (utf16_strlen(name, GSMI_BUF_SIZE / 2) == GSMI_BUF_SIZE / 2)
+	if (ucs2_strnlen(name, GSMI_BUF_SIZE / 2) == GSMI_BUF_SIZE / 2)
 		return EFI_INVALID_PARAMETER;
 
 	spin_lock_irqsave(&gsmi_dev.lock, flags);
@@ -408,7 +398,7 @@ static efi_status_t gsmi_get_next_variable(unsigned long *name_size,
 
 		/* Copy the name back */
 		memcpy(name, gsmi_dev.name_buf->start, GSMI_BUF_SIZE);
-		*name_size = utf16_strlen(name, GSMI_BUF_SIZE / 2) * 2;
+		*name_size = ucs2_strnlen(name, GSMI_BUF_SIZE / 2) * 2;
 
 		/* copy guid to return buffer */
 		memcpy(vendor, &param.guid, sizeof(param.guid));
@@ -434,7 +424,7 @@ static efi_status_t gsmi_set_variable(efi_char16_t *name,
 			      EFI_VARIABLE_BOOTSERVICE_ACCESS |
 			      EFI_VARIABLE_RUNTIME_ACCESS,
 	};
-	size_t name_len = utf16_strlen(name, GSMI_BUF_SIZE / 2);
+	size_t name_len = ucs2_strnlen(name, GSMI_BUF_SIZE / 2);
 	efi_status_t ret = EFI_SUCCESS;
 	int rc;
 	unsigned long flags;
@@ -893,12 +883,19 @@ static __init int gsmi_init(void)
 		goto out_remove_bin_file;
 	}
 
-	ret = register_efivars(&efivars, &efivar_ops, gsmi_kobj);
+	ret = efivars_register(&efivars, &efivar_ops, gsmi_kobj);
 	if (ret) {
 		printk(KERN_INFO "gsmi: Failed to register efivars\n");
 		goto out_remove_sysfs_files;
 	}
 
+	ret = efivars_sysfs_init();
+	if (ret) {
+		printk(KERN_INFO "gsmi: Failed to create efivars files\n");
+		efivars_unregister(&efivars);
+		goto out_remove_sysfs_files;
+	}
+
 	register_reboot_notifier(&gsmi_reboot_notifier);
 	register_die_notifier(&gsmi_die_notifier);
 	atomic_notifier_chain_register(&panic_notifier_list,
@@ -930,7 +927,7 @@ static void __exit gsmi_exit(void)
 	unregister_die_notifier(&gsmi_die_notifier);
 	atomic_notifier_chain_unregister(&panic_notifier_list,
 					 &gsmi_panic_notifier);
-	unregister_efivars(&efivars);
+	efivars_unregister(&efivars);
 
 	sysfs_remove_files(gsmi_kobj, gsmi_attrs);
 	sysfs_remove_bin_file(gsmi_kobj, &eventlog_bin_attr);
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index 93aaadf99f28..87d567089f13 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -38,7 +38,6 @@ config GPIO_DEVRES
 menuconfig GPIOLIB
 	bool "GPIO Support"
 	depends on ARCH_WANT_OPTIONAL_GPIOLIB || ARCH_REQUIRE_GPIOLIB
-	select GENERIC_GPIO
 	help
 	  This enables GPIO support through the generic GPIO library.
 	  You only need to enable this, if you also want to enable
@@ -204,6 +203,12 @@ config GPIO_PXA
 	help
 	  Say yes here to support the PXA GPIO device
 
+config GPIO_RCAR
+	tristate "Renesas R-Car GPIO"
+	depends on ARM
+	help
+	  Say yes here to support GPIO on Renesas R-Car SoCs.
+
 config GPIO_SPEAR_SPICS
 	bool "ST SPEAr13xx SPI Chip Select as GPIO support"
 	depends on PLAT_SPEAR
@@ -227,12 +232,6 @@ config GPIO_TS5500
 	  blocks of the TS-5500: DIO1, DIO2 and the LCD port, and the TS-5600
 	  LCD port.
 
-config GPIO_VT8500
-	bool "VIA/Wondermedia SoC GPIO Support"
-	depends on ARCH_VT8500
-	help
-	  Say yes here to support the VT8500/WM8505/WM8650 GPIO controller.
-
 config GPIO_XILINX
 	bool "Xilinx GPIO support"
 	depends on PPC_OF || MICROBLAZE
@@ -303,12 +302,21 @@ config GPIO_GE_FPGA
 
 config GPIO_LYNXPOINT
 	bool "Intel Lynxpoint GPIO support"
-	depends on ACPI
+	depends on ACPI && X86
 	select IRQ_DOMAIN
 	help
 	  driver for GPIO functionality on Intel Lynxpoint PCH chipset
 	  Requires ACPI device enumeration code to set up a platform device.
 
+config GPIO_GRGPIO
+	tristate "Aeroflex Gaisler GRGPIO support"
+	depends on OF
+	select GPIO_GENERIC
+	select IRQ_DOMAIN
+	help
+	  Select this to support Aeroflex Gaisler GRGPIO cores from the GRLIB
+	  VHDL IP core library.
+
 comment "I2C GPIO expanders:"
 
 config GPIO_ARIZONA
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 22e07bc9fcb5..0cb2d656ad16 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -25,6 +25,7 @@ obj-$(CONFIG_ARCH_DAVINCI)	+= gpio-davinci.o
 obj-$(CONFIG_GPIO_EM)		+= gpio-em.o
 obj-$(CONFIG_GPIO_EP93XX)	+= gpio-ep93xx.o
 obj-$(CONFIG_GPIO_GE_FPGA)	+= gpio-ge.o
+obj-$(CONFIG_GPIO_GRGPIO)	+= gpio-grgpio.o
 obj-$(CONFIG_GPIO_ICH)		+= gpio-ich.o
 obj-$(CONFIG_GPIO_IT8761E)	+= gpio-it8761e.o
 obj-$(CONFIG_GPIO_JANZ_TTL)	+= gpio-janz-ttl.o
@@ -57,6 +58,7 @@ obj-$(CONFIG_GPIO_PL061)	+= gpio-pl061.o
 obj-$(CONFIG_GPIO_PXA)		+= gpio-pxa.o
 obj-$(CONFIG_GPIO_RC5T583)	+= gpio-rc5t583.o
 obj-$(CONFIG_GPIO_RDC321X)	+= gpio-rdc321x.o
+obj-$(CONFIG_GPIO_RCAR)		+= gpio-rcar.o
 obj-$(CONFIG_PLAT_SAMSUNG)	+= gpio-samsung.o
 obj-$(CONFIG_ARCH_SA1100)	+= gpio-sa1100.o
 obj-$(CONFIG_GPIO_SCH)		+= gpio-sch.o
@@ -80,7 +82,6 @@ obj-$(CONFIG_GPIO_TWL6040)	+= gpio-twl6040.o
 obj-$(CONFIG_GPIO_UCB1400)	+= gpio-ucb1400.o
 obj-$(CONFIG_GPIO_VIPERBOARD)	+= gpio-viperboard.o
 obj-$(CONFIG_GPIO_VR41XX)	+= gpio-vr41xx.o
-obj-$(CONFIG_GPIO_VT8500)	+= gpio-vt8500.o
 obj-$(CONFIG_GPIO_VX855)	+= gpio-vx855.o
 obj-$(CONFIG_GPIO_WM831X)	+= gpio-wm831x.o
 obj-$(CONFIG_GPIO_WM8350)	+= gpio-wm8350.o
diff --git a/drivers/gpio/gpio-74x164.c b/drivers/gpio/gpio-74x164.c
index 464be961f605..721607904d0a 100644
--- a/drivers/gpio/gpio-74x164.c
+++ b/drivers/gpio/gpio-74x164.c
@@ -137,7 +137,7 @@ static int gen_74x164_probe(struct spi_device *spi)
 
 	mutex_init(&chip->lock);
 
-	dev_set_drvdata(&spi->dev, chip);
+	spi_set_drvdata(spi, chip);
 
 	chip->spi = spi;
 
@@ -176,7 +176,7 @@ static int gen_74x164_probe(struct spi_device *spi)
 	return ret;
 
 exit_destroy:
-	dev_set_drvdata(&spi->dev, NULL);
+	spi_set_drvdata(spi, NULL);
 	mutex_destroy(&chip->lock);
 	return ret;
 }
@@ -186,11 +186,11 @@ static int gen_74x164_remove(struct spi_device *spi)
 	struct gen_74x164_chip *chip;
 	int ret;
 
-	chip = dev_get_drvdata(&spi->dev);
+	chip = spi_get_drvdata(spi);
 	if (chip == NULL)
 		return -ENODEV;
 
-	dev_set_drvdata(&spi->dev, NULL);
+	spi_set_drvdata(spi, NULL);
 
 	ret = gpiochip_remove(&chip->gpio_chip);
 	if (!ret)
diff --git a/drivers/gpio/gpio-adp5520.c b/drivers/gpio/gpio-adp5520.c
index 8afa95f831b1..f33f78dcadaa 100644
--- a/drivers/gpio/gpio-adp5520.c
+++ b/drivers/gpio/gpio-adp5520.c
@@ -105,7 +105,7 @@ static int adp5520_gpio_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
 	if (dev == NULL) {
 		dev_err(&pdev->dev, "failed to alloc memory\n");
 		return -ENOMEM;
@@ -163,7 +163,6 @@ static int adp5520_gpio_probe(struct platform_device *pdev)
 	return 0;
 
 err:
-	kfree(dev);
 	return ret;
 }
 
@@ -180,7 +179,6 @@ static int adp5520_gpio_remove(struct platform_device *pdev)
 		return ret;
 	}
 
-	kfree(dev);
 	return 0;
 }
 
diff --git a/drivers/gpio/gpio-em.c b/drivers/gpio/gpio-em.c
index deca78f99316..5cba855638bf 100644
--- a/drivers/gpio/gpio-em.c
+++ b/drivers/gpio/gpio-em.c
@@ -231,10 +231,12 @@ static int em_gio_irq_domain_map(struct irq_domain *h, unsigned int virq,
 
 static struct irq_domain_ops em_gio_irq_domain_ops = {
 	.map	= em_gio_irq_domain_map,
+	.xlate	= irq_domain_xlate_twocell,
 };
 
 static int em_gio_probe(struct platform_device *pdev)
 {
+	struct gpio_em_config pdata_dt;
 	struct gpio_em_config *pdata = pdev->dev.platform_data;
 	struct em_gio_priv *p;
 	struct resource *io[2], *irq[2];
@@ -243,7 +245,7 @@ static int em_gio_probe(struct platform_device *pdev)
 	const char *name = dev_name(&pdev->dev);
 	int ret;
 
-	p = kzalloc(sizeof(*p), GFP_KERNEL);
+	p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
 	if (!p) {
 		dev_err(&pdev->dev, "failed to allocate driver data\n");
 		ret = -ENOMEM;
@@ -259,24 +261,45 @@ static int em_gio_probe(struct platform_device *pdev)
 	irq[0] = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	irq[1] = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
 
-	if (!io[0] || !io[1] || !irq[0] || !irq[1] || !pdata) {
-		dev_err(&pdev->dev, "missing IRQ, IOMEM or configuration\n");
+	if (!io[0] || !io[1] || !irq[0] || !irq[1]) {
+		dev_err(&pdev->dev, "missing IRQ or IOMEM\n");
 		ret = -EINVAL;
-		goto err1;
+		goto err0;
 	}
 
-	p->base0 = ioremap_nocache(io[0]->start, resource_size(io[0]));
+	p->base0 = devm_ioremap_nocache(&pdev->dev, io[0]->start,
+					resource_size(io[0]));
 	if (!p->base0) {
 		dev_err(&pdev->dev, "failed to remap low I/O memory\n");
 		ret = -ENXIO;
-		goto err1;
+		goto err0;
 	}
 
-	p->base1 = ioremap_nocache(io[1]->start, resource_size(io[1]));
+	p->base1 = devm_ioremap_nocache(&pdev->dev, io[1]->start,
+				   resource_size(io[1]));
 	if (!p->base1) {
 		dev_err(&pdev->dev, "failed to remap high I/O memory\n");
 		ret = -ENXIO;
-		goto err2;
+		goto err0;
+	}
+
+	if (!pdata) {
+		memset(&pdata_dt, 0, sizeof(pdata_dt));
+		pdata = &pdata_dt;
+
+		if (of_property_read_u32(pdev->dev.of_node, "ngpios",
+					 &pdata->number_of_pins)) {
+			dev_err(&pdev->dev, "Missing ngpios OF property\n");
+			ret = -EINVAL;
+			goto err0;
+		}
+
+		ret = of_alias_get_id(pdev->dev.of_node, "gpio");
+		if (ret < 0) {
+			dev_err(&pdev->dev, "Couldn't get OF id\n");
+			goto err0;
+		}
+		pdata->gpio_base = ret * 32; /* 32 GPIOs per instance */
 	}
 
 	gpio_chip = &p->gpio_chip;
@@ -306,40 +329,32 @@ static int em_gio_probe(struct platform_device *pdev)
 	if (!p->irq_domain) {
 		ret = -ENXIO;
 		dev_err(&pdev->dev, "cannot initialize irq domain\n");
-		goto err3;
+		goto err0;
 	}
 
-	if (request_irq(irq[0]->start, em_gio_irq_handler, 0, name, p)) {
+	if (devm_request_irq(&pdev->dev, irq[0]->start,
+			     em_gio_irq_handler, 0, name, p)) {
 		dev_err(&pdev->dev, "failed to request low IRQ\n");
 		ret = -ENOENT;
-		goto err4;
+		goto err1;
 	}
 
-	if (request_irq(irq[1]->start, em_gio_irq_handler, 0, name, p)) {
+	if (devm_request_irq(&pdev->dev, irq[1]->start,
+			     em_gio_irq_handler, 0, name, p)) {
 		dev_err(&pdev->dev, "failed to request high IRQ\n");
 		ret = -ENOENT;
-		goto err5;
+		goto err1;
 	}
 
 	ret = gpiochip_add(gpio_chip);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to add GPIO controller\n");
-		goto err6;
+		goto err1;
 	}
 	return 0;
 
-err6:
-	free_irq(irq[1]->start, pdev);
-err5:
-	free_irq(irq[0]->start, pdev);
-err4:
-	irq_domain_remove(p->irq_domain);
-err3:
-	iounmap(p->base1);
-err2:
-	iounmap(p->base0);
 err1:
-	kfree(p);
+	irq_domain_remove(p->irq_domain);
 err0:
 	return ret;
 }
@@ -347,34 +362,43 @@ err0:
 static int em_gio_remove(struct platform_device *pdev)
 {
 	struct em_gio_priv *p = platform_get_drvdata(pdev);
-	struct resource *irq[2];
 	int ret;
 
 	ret = gpiochip_remove(&p->gpio_chip);
 	if (ret)
 		return ret;
 
-	irq[0] = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	irq[1] = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
-
-	free_irq(irq[1]->start, pdev);
-	free_irq(irq[0]->start, pdev);
 	irq_domain_remove(p->irq_domain);
-	iounmap(p->base1);
-	iounmap(p->base0);
-	kfree(p);
 	return 0;
 }
 
+static const struct of_device_id em_gio_dt_ids[] = {
+	{ .compatible = "renesas,em-gio", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, em_gio_dt_ids);
+
 static struct platform_driver em_gio_device_driver = {
 	.probe		= em_gio_probe,
 	.remove		= em_gio_remove,
 	.driver		= {
 		.name	= "em_gio",
+		.of_match_table = em_gio_dt_ids,
+		.owner		= THIS_MODULE,
 	}
 };
 
-module_platform_driver(em_gio_device_driver);
+static int __init em_gio_init(void)
+{
+	return platform_driver_register(&em_gio_device_driver);
+}
+postcore_initcall(em_gio_init);
+
+static void __exit em_gio_exit(void)
+{
+	platform_driver_unregister(&em_gio_device_driver);
+}
+module_exit(em_gio_exit);
 
 MODULE_AUTHOR("Magnus Damm");
 MODULE_DESCRIPTION("Renesas Emma Mobile GIO Driver");
diff --git a/drivers/gpio/gpio-generic.c b/drivers/gpio/gpio-generic.c
index 05fcc0f247ca..d2196bf73847 100644
--- a/drivers/gpio/gpio-generic.c
+++ b/drivers/gpio/gpio-generic.c
@@ -104,6 +104,26 @@ static unsigned long bgpio_read64(void __iomem *reg)
 }
 #endif /* BITS_PER_LONG >= 64 */
 
+static void bgpio_write16be(void __iomem *reg, unsigned long data)
+{
+	iowrite16be(data, reg);
+}
+
+static unsigned long bgpio_read16be(void __iomem *reg)
+{
+	return ioread16be(reg);
+}
+
+static void bgpio_write32be(void __iomem *reg, unsigned long data)
+{
+	iowrite32be(data, reg);
+}
+
+static unsigned long bgpio_read32be(void __iomem *reg)
+{
+	return ioread32be(reg);
+}
+
 static unsigned long bgpio_pin2mask(struct bgpio_chip *bgc, unsigned int pin)
 {
 	return 1 << pin;
@@ -249,7 +269,8 @@ static int bgpio_dir_out_inv(struct gpio_chip *gc, unsigned int gpio, int val)
 
 static int bgpio_setup_accessors(struct device *dev,
 				 struct bgpio_chip *bgc,
-				 bool be)
+				 bool bit_be,
+				 bool byte_be)
 {
 
 	switch (bgc->bits) {
@@ -258,17 +279,33 @@ static int bgpio_setup_accessors(struct device *dev,
 		bgc->write_reg	= bgpio_write8;
 		break;
 	case 16:
-		bgc->read_reg	= bgpio_read16;
-		bgc->write_reg	= bgpio_write16;
+		if (byte_be) {
+			bgc->read_reg	= bgpio_read16be;
+			bgc->write_reg	= bgpio_write16be;
+		} else {
+			bgc->read_reg	= bgpio_read16;
+			bgc->write_reg	= bgpio_write16;
+		}
 		break;
 	case 32:
-		bgc->read_reg	= bgpio_read32;
-		bgc->write_reg	= bgpio_write32;
+		if (byte_be) {
+			bgc->read_reg	= bgpio_read32be;
+			bgc->write_reg	= bgpio_write32be;
+		} else {
+			bgc->read_reg	= bgpio_read32;
+			bgc->write_reg	= bgpio_write32;
+		}
 		break;
 #if BITS_PER_LONG >= 64
 	case 64:
-		bgc->read_reg	= bgpio_read64;
-		bgc->write_reg	= bgpio_write64;
+		if (byte_be) {
+			dev_err(dev,
+				"64 bit big endian byte order unsupported\n");
+			return -EINVAL;
+		} else {
+			bgc->read_reg	= bgpio_read64;
+			bgc->write_reg	= bgpio_write64;
+		}
 		break;
 #endif /* BITS_PER_LONG >= 64 */
 	default:
@@ -276,7 +313,7 @@ static int bgpio_setup_accessors(struct device *dev,
 		return -EINVAL;
 	}
 
-	bgc->pin2mask = be ? bgpio_pin2mask_be : bgpio_pin2mask;
+	bgc->pin2mask = bit_be ? bgpio_pin2mask_be : bgpio_pin2mask;
 
 	return 0;
 }
@@ -353,11 +390,7 @@ static int bgpio_setup_direction(struct bgpio_chip *bgc,
 
 int bgpio_remove(struct bgpio_chip *bgc)
 {
-	int err = gpiochip_remove(&bgc->gc);
-
-	kfree(bgc);
-
-	return err;
+	return gpiochip_remove(&bgc->gc);
 }
 EXPORT_SYMBOL_GPL(bgpio_remove);
 
@@ -385,7 +418,8 @@ int bgpio_init(struct bgpio_chip *bgc, struct device *dev,
 	if (ret)
 		return ret;
 
-	ret = bgpio_setup_accessors(dev, bgc, flags & BGPIOF_BIG_ENDIAN);
+	ret = bgpio_setup_accessors(dev, bgc, flags & BGPIOF_BIG_ENDIAN,
+				    flags & BGPIOF_BIG_ENDIAN_BYTE_ORDER);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpio/gpio-grgpio.c b/drivers/gpio/gpio-grgpio.c
new file mode 100644
index 000000000000..8e08b8647655
--- /dev/null
+++ b/drivers/gpio/gpio-grgpio.c
@@ -0,0 +1,505 @@
+/*
+ * Driver for Aeroflex Gaisler GRGPIO General Purpose I/O cores.
+ *
+ * 2013 (c) Aeroflex Gaisler AB
+ *
+ * This driver supports the GRGPIO GPIO core available in the GRLIB VHDL
+ * IP core library.
+ *
+ * Full documentation of the GRGPIO core can be found here:
+ * http://www.gaisler.com/products/grlib/grip.pdf
+ *
+ * See "Documentation/devicetree/bindings/gpio/gpio-grgpio.txt" for
+ * information on open firmware properties.
+ *
+ * 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.
+ *
+ * Contributors: Andreas Larsson <andreas@gaisler.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/spinlock.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/of_platform.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/basic_mmio_gpio.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+
+#define GRGPIO_MAX_NGPIO 32
+
+#define GRGPIO_DATA		0x00
+#define GRGPIO_OUTPUT		0x04
+#define GRGPIO_DIR		0x08
+#define GRGPIO_IMASK		0x0c
+#define GRGPIO_IPOL		0x10
+#define GRGPIO_IEDGE		0x14
+#define GRGPIO_BYPASS		0x18
+#define GRGPIO_IMAP_BASE	0x20
+
+/* Structure for an irq of the core - called an underlying irq */
+struct grgpio_uirq {
+	u8 refcnt; /* Reference counter to manage requesting/freeing of uirq */
+	u8 uirq; /* Underlying irq of the gpio driver */
+};
+
+/*
+ * Structure for an irq of a gpio line handed out by this driver. The index is
+ * used to map to the corresponding underlying irq.
+ */
+struct grgpio_lirq {
+	s8 index; /* Index into struct grgpio_priv's uirqs, or -1 */
+	u8 irq; /* irq for the gpio line */
+};
+
+struct grgpio_priv {
+	struct bgpio_chip bgc;
+	void __iomem *regs;
+	struct device *dev;
+
+	u32 imask; /* irq mask shadow register */
+
+	/*
+	 * The grgpio core can have multiple "underlying" irqs. The gpio lines
+	 * can be mapped to any one or none of these underlying irqs
+	 * independently of each other. This driver sets up an irq domain and
+	 * hands out separate irqs to each gpio line
+	 */
+	struct irq_domain *domain;
+
+	/*
+	 * This array contains information on each underlying irq, each
+	 * irq of the grgpio core itself.
+	 */
+	struct grgpio_uirq uirqs[GRGPIO_MAX_NGPIO];
+
+	/*
+	 * This array contains information for each gpio line on the irqs
+	 * obtains from this driver. An index value of -1 for a certain gpio
+	 * line indicates that the line has no irq. Otherwise the index connects
+	 * the irq to the underlying irq by pointing into the uirqs array.
+	 */
+	struct grgpio_lirq lirqs[GRGPIO_MAX_NGPIO];
+};
+
+static inline struct grgpio_priv *grgpio_gc_to_priv(struct gpio_chip *gc)
+{
+	struct bgpio_chip *bgc = to_bgpio_chip(gc);
+
+	return container_of(bgc, struct grgpio_priv, bgc);
+}
+
+static void grgpio_set_imask(struct grgpio_priv *priv, unsigned int offset,
+			     int val)
+{
+	struct bgpio_chip *bgc = &priv->bgc;
+	unsigned long mask = bgc->pin2mask(bgc, offset);
+	unsigned long flags;
+
+	spin_lock_irqsave(&bgc->lock, flags);
+
+	if (val)
+		priv->imask |= mask;
+	else
+		priv->imask &= ~mask;
+	bgc->write_reg(priv->regs + GRGPIO_IMASK, priv->imask);
+
+	spin_unlock_irqrestore(&bgc->lock, flags);
+}
+
+static int grgpio_to_irq(struct gpio_chip *gc, unsigned offset)
+{
+	struct grgpio_priv *priv = grgpio_gc_to_priv(gc);
+
+	if (offset > gc->ngpio)
+		return -ENXIO;
+
+	if (priv->lirqs[offset].index < 0)
+		return -ENXIO;
+
+	return irq_create_mapping(priv->domain, offset);
+}
+
+/* -------------------- IRQ chip functions -------------------- */
+
+static int grgpio_irq_set_type(struct irq_data *d, unsigned int type)
+{
+	struct grgpio_priv *priv = irq_data_get_irq_chip_data(d);
+	unsigned long flags;
+	u32 mask = BIT(d->hwirq);
+	u32 ipol;
+	u32 iedge;
+	u32 pol;
+	u32 edge;
+
+	switch (type) {
+	case IRQ_TYPE_LEVEL_LOW:
+		pol = 0;
+		edge = 0;
+		break;
+	case IRQ_TYPE_LEVEL_HIGH:
+		pol = mask;
+		edge = 0;
+		break;
+	case IRQ_TYPE_EDGE_FALLING:
+		pol = 0;
+		edge = mask;
+		break;
+	case IRQ_TYPE_EDGE_RISING:
+		pol = mask;
+		edge = mask;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&priv->bgc.lock, flags);
+
+	ipol = priv->bgc.read_reg(priv->regs + GRGPIO_IPOL) & ~mask;
+	iedge = priv->bgc.read_reg(priv->regs + GRGPIO_IEDGE) & ~mask;
+
+	priv->bgc.write_reg(priv->regs + GRGPIO_IPOL, ipol | pol);
+	priv->bgc.write_reg(priv->regs + GRGPIO_IEDGE, iedge | edge);
+
+	spin_unlock_irqrestore(&priv->bgc.lock, flags);
+
+	return 0;
+}
+
+static void grgpio_irq_mask(struct irq_data *d)
+{
+	struct grgpio_priv *priv = irq_data_get_irq_chip_data(d);
+	int offset = d->hwirq;
+
+	grgpio_set_imask(priv, offset, 0);
+}
+
+static void grgpio_irq_unmask(struct irq_data *d)
+{
+	struct grgpio_priv *priv = irq_data_get_irq_chip_data(d);
+	int offset = d->hwirq;
+
+	grgpio_set_imask(priv, offset, 1);
+}
+
+static struct irq_chip grgpio_irq_chip = {
+	.name			= "grgpio",
+	.irq_mask		= grgpio_irq_mask,
+	.irq_unmask		= grgpio_irq_unmask,
+	.irq_set_type		= grgpio_irq_set_type,
+};
+
+static irqreturn_t grgpio_irq_handler(int irq, void *dev)
+{
+	struct grgpio_priv *priv = dev;
+	int ngpio = priv->bgc.gc.ngpio;
+	unsigned long flags;
+	int i;
+	int match = 0;
+
+	spin_lock_irqsave(&priv->bgc.lock, flags);
+
+	/*
+	 * For each gpio line, call its interrupt handler if it its underlying
+	 * irq matches the current irq that is handled.
+	 */
+	for (i = 0; i < ngpio; i++) {
+		struct grgpio_lirq *lirq = &priv->lirqs[i];
+
+		if (priv->imask & BIT(i) && lirq->index >= 0 &&
+		    priv->uirqs[lirq->index].uirq == irq) {
+			generic_handle_irq(lirq->irq);
+			match = 1;
+		}
+	}
+
+	spin_unlock_irqrestore(&priv->bgc.lock, flags);
+
+	if (!match)
+		dev_warn(priv->dev, "No gpio line matched irq %d\n", irq);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * This function will be called as a consequence of the call to
+ * irq_create_mapping in grgpio_to_irq
+ */
+int grgpio_irq_map(struct irq_domain *d, unsigned int irq,
+		   irq_hw_number_t hwirq)
+{
+	struct grgpio_priv *priv = d->host_data;
+	struct grgpio_lirq *lirq;
+	struct grgpio_uirq *uirq;
+	unsigned long flags;
+	int offset = hwirq;
+	int ret = 0;
+
+	if (!priv)
+		return -EINVAL;
+
+	lirq = &priv->lirqs[offset];
+	if (lirq->index < 0)
+		return -EINVAL;
+
+	dev_dbg(priv->dev, "Mapping irq %d for gpio line %d\n",
+		irq, offset);
+
+	spin_lock_irqsave(&priv->bgc.lock, flags);
+
+	/* Request underlying irq if not already requested */
+	lirq->irq = irq;
+	uirq = &priv->uirqs[lirq->index];
+	if (uirq->refcnt == 0) {
+		ret = request_irq(uirq->uirq, grgpio_irq_handler, 0,
+				  dev_name(priv->dev), priv);
+		if (ret) {
+			dev_err(priv->dev,
+				"Could not request underlying irq %d\n",
+				uirq->uirq);
+
+			spin_unlock_irqrestore(&priv->bgc.lock, flags);
+
+			return ret;
+		}
+	}
+	uirq->refcnt++;
+
+	spin_unlock_irqrestore(&priv->bgc.lock, flags);
+
+	/* Setup irq  */
+	irq_set_chip_data(irq, priv);
+	irq_set_chip_and_handler(irq, &grgpio_irq_chip,
+				 handle_simple_irq);
+	irq_clear_status_flags(irq, IRQ_NOREQUEST);
+#ifdef CONFIG_ARM
+	set_irq_flags(irq, IRQF_VALID);
+#else
+	irq_set_noprobe(irq);
+#endif
+
+	return ret;
+}
+
+void grgpio_irq_unmap(struct irq_domain *d, unsigned int irq)
+{
+	struct grgpio_priv *priv = d->host_data;
+	int index;
+	struct grgpio_lirq *lirq;
+	struct grgpio_uirq *uirq;
+	unsigned long flags;
+	int ngpio = priv->bgc.gc.ngpio;
+	int i;
+
+#ifdef CONFIG_ARM
+	set_irq_flags(irq, 0);
+#endif
+	irq_set_chip_and_handler(irq, NULL, NULL);
+	irq_set_chip_data(irq, NULL);
+
+	spin_lock_irqsave(&priv->bgc.lock, flags);
+
+	/* Free underlying irq if last user unmapped */
+	index = -1;
+	for (i = 0; i < ngpio; i++) {
+		lirq = &priv->lirqs[i];
+		if (lirq->irq == irq) {
+			grgpio_set_imask(priv, i, 0);
+			lirq->irq = 0;
+			index = lirq->index;
+			break;
+		}
+	}
+	WARN_ON(index < 0);
+
+	if (index >= 0) {
+		uirq = &priv->uirqs[lirq->index];
+		uirq->refcnt--;
+		if (uirq->refcnt == 0)
+			free_irq(uirq->uirq, priv);
+	}
+
+	spin_unlock_irqrestore(&priv->bgc.lock, flags);
+}
+
+static struct irq_domain_ops grgpio_irq_domain_ops = {
+	.map	= grgpio_irq_map,
+	.unmap	= grgpio_irq_unmap,
+};
+
+/* ------------------------------------------------------------ */
+
+static int grgpio_probe(struct platform_device *ofdev)
+{
+	struct device_node *np = ofdev->dev.of_node;
+	void  __iomem *regs;
+	struct gpio_chip *gc;
+	struct bgpio_chip *bgc;
+	struct grgpio_priv *priv;
+	struct resource *res;
+	int err;
+	u32 prop;
+	s32 *irqmap;
+	int size;
+	int i;
+
+	priv = devm_kzalloc(&ofdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	res = platform_get_resource(ofdev, IORESOURCE_MEM, 0);
+	regs = devm_ioremap_resource(&ofdev->dev, res);
+	if (IS_ERR(regs))
+		return PTR_ERR(regs);
+
+	bgc = &priv->bgc;
+	err = bgpio_init(bgc, &ofdev->dev, 4, regs + GRGPIO_DATA,
+			 regs + GRGPIO_OUTPUT, NULL, regs + GRGPIO_DIR, NULL,
+			 BGPIOF_BIG_ENDIAN_BYTE_ORDER);
+	if (err) {
+		dev_err(&ofdev->dev, "bgpio_init() failed\n");
+		return err;
+	}
+
+	priv->regs = regs;
+	priv->imask = bgc->read_reg(regs + GRGPIO_IMASK);
+	priv->dev = &ofdev->dev;
+
+	gc = &bgc->gc;
+	gc->of_node = np;
+	gc->owner = THIS_MODULE;
+	gc->to_irq = grgpio_to_irq;
+	gc->label = np->full_name;
+	gc->base = -1;
+
+	err = of_property_read_u32(np, "nbits", &prop);
+	if (err || prop <= 0 || prop > GRGPIO_MAX_NGPIO) {
+		gc->ngpio = GRGPIO_MAX_NGPIO;
+		dev_dbg(&ofdev->dev,
+			"No or invalid nbits property: assume %d\n", gc->ngpio);
+	} else {
+		gc->ngpio = prop;
+	}
+
+	/*
+	 * The irqmap contains the index values indicating which underlying irq,
+	 * if anyone, is connected to that line
+	 */
+	irqmap = (s32 *)of_get_property(np, "irqmap", &size);
+	if (irqmap) {
+		if (size < gc->ngpio) {
+			dev_err(&ofdev->dev,
+				"irqmap shorter than ngpio (%d < %d)\n",
+				size, gc->ngpio);
+			return -EINVAL;
+		}
+
+		priv->domain = irq_domain_add_linear(np, gc->ngpio,
+						     &grgpio_irq_domain_ops,
+						     priv);
+		if (!priv->domain) {
+			dev_err(&ofdev->dev, "Could not add irq domain\n");
+			return -EINVAL;
+		}
+
+		for (i = 0; i < gc->ngpio; i++) {
+			struct grgpio_lirq *lirq;
+			int ret;
+
+			lirq = &priv->lirqs[i];
+			lirq->index = irqmap[i];
+
+			if (lirq->index < 0)
+				continue;
+
+			ret = platform_get_irq(ofdev, lirq->index);
+			if (ret <= 0) {
+				/*
+				 * Continue without irq functionality for that
+				 * gpio line
+				 */
+				dev_err(priv->dev,
+					"Failed to get irq for offset %d\n", i);
+				continue;
+			}
+			priv->uirqs[lirq->index].uirq = ret;
+		}
+	}
+
+	platform_set_drvdata(ofdev, priv);
+
+	err = gpiochip_add(gc);
+	if (err) {
+		dev_err(&ofdev->dev, "Could not add gpiochip\n");
+		return err;
+	}
+
+	dev_info(&ofdev->dev, "regs=0x%p, base=%d, ngpio=%d, irqs=%s\n",
+		 priv->regs, gc->base, gc->ngpio, priv->domain ? "on" : "off");
+
+	return 0;
+}
+
+static int grgpio_remove(struct platform_device *ofdev)
+{
+	struct grgpio_priv *priv = platform_get_drvdata(ofdev);
+	unsigned long flags;
+	int i;
+	int ret = 0;
+
+	spin_lock_irqsave(&priv->bgc.lock, flags);
+
+	if (priv->domain) {
+		for (i = 0; i < GRGPIO_MAX_NGPIO; i++) {
+			if (priv->uirqs[i].refcnt != 0) {
+				ret = -EBUSY;
+				goto out;
+			}
+		}
+	}
+
+	ret = gpiochip_remove(&priv->bgc.gc);
+	if (ret)
+		goto out;
+
+	if (priv->domain)
+		irq_domain_remove(priv->domain);
+
+out:
+	spin_unlock_irqrestore(&priv->bgc.lock, flags);
+
+	return ret;
+}
+
+static struct of_device_id grgpio_match[] = {
+	{.name = "GAISLER_GPIO"},
+	{.name = "01_01a"},
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, grgpio_match);
+
+static struct platform_driver grgpio_driver = {
+	.driver = {
+		.name = "grgpio",
+		.owner = THIS_MODULE,
+		.of_match_table = grgpio_match,
+	},
+	.probe = grgpio_probe,
+	.remove = grgpio_remove,
+};
+module_platform_driver(grgpio_driver);
+
+MODULE_AUTHOR("Aeroflex Gaisler AB.");
+MODULE_DESCRIPTION("Driver for Aeroflex Gaisler GRGPIO");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/gpio-ich.c b/drivers/gpio/gpio-ich.c
index de3c317bd3e2..e16d932fd444 100644
--- a/drivers/gpio/gpio-ich.c
+++ b/drivers/gpio/gpio-ich.c
@@ -130,14 +130,11 @@ static int ichx_read_bit(int reg, unsigned nr)
 
 static bool ichx_gpio_check_available(struct gpio_chip *gpio, unsigned nr)
 {
-	return ichx_priv.use_gpio & (1 << (nr / 32));
+	return !!(ichx_priv.use_gpio & (1 << (nr / 32)));
 }
 
 static int ichx_gpio_direction_input(struct gpio_chip *gpio, unsigned nr)
 {
-	if (!ichx_gpio_check_available(gpio, nr))
-		return -ENXIO;
-
 	/*
 	 * Try setting pin as an input and verify it worked since many pins
 	 * are output-only.
@@ -151,9 +148,6 @@ static int ichx_gpio_direction_input(struct gpio_chip *gpio, unsigned nr)
 static int ichx_gpio_direction_output(struct gpio_chip *gpio, unsigned nr,
 					int val)
 {
-	if (!ichx_gpio_check_available(gpio, nr))
-		return -ENXIO;
-
 	/* Set GPIO output value. */
 	ichx_write_bit(GPIO_LVL, nr, val, 0);
 
@@ -169,9 +163,6 @@ static int ichx_gpio_direction_output(struct gpio_chip *gpio, unsigned nr,
 
 static int ichx_gpio_get(struct gpio_chip *chip, unsigned nr)
 {
-	if (!ichx_gpio_check_available(chip, nr))
-		return -ENXIO;
-
 	return ichx_read_bit(GPIO_LVL, nr);
 }
 
@@ -180,9 +171,6 @@ static int ich6_gpio_get(struct gpio_chip *chip, unsigned nr)
 	unsigned long flags;
 	u32 data;
 
-	if (!ichx_gpio_check_available(chip, nr))
-		return -ENXIO;
-
 	/*
 	 * GPI 0 - 15 need to be read from the power management registers on
 	 * a ICH6/3100 bridge.
@@ -207,6 +195,9 @@ static int ich6_gpio_get(struct gpio_chip *chip, unsigned nr)
 
 static int ichx_gpio_request(struct gpio_chip *chip, unsigned nr)
 {
+	if (!ichx_gpio_check_available(chip, nr))
+		return -ENXIO;
+
 	/*
 	 * Note we assume the BIOS properly set a bridge's USE value.  Some
 	 * chips (eg Intel 3100) have bogus USE values though, so first see if
diff --git a/drivers/gpio/gpio-lpc32xx.c b/drivers/gpio/gpio-lpc32xx.c
index 36d7dee07b28..90a80eb688a9 100644
--- a/drivers/gpio/gpio-lpc32xx.c
+++ b/drivers/gpio/gpio-lpc32xx.c
@@ -255,7 +255,7 @@ static int __get_gpo_state_p3(struct lpc32xx_gpio_chip *group,
 }
 
 /*
- * GENERIC_GPIO primitives.
+ * GPIO primitives.
  */
 static int lpc32xx_gpio_dir_input_p012(struct gpio_chip *chip,
 	unsigned pin)
@@ -533,7 +533,7 @@ static int lpc32xx_of_xlate(struct gpio_chip *gc,
 {
 	/* Is this the correct bank? */
 	u32 bank = gpiospec->args[0];
-	if ((bank > ARRAY_SIZE(lpc32xx_gpiochip) ||
+	if ((bank >= ARRAY_SIZE(lpc32xx_gpiochip) ||
 	    (gc != &lpc32xx_gpiochip[bank].chip)))
 		return -EINVAL;
 
diff --git a/drivers/gpio/gpio-lynxpoint.c b/drivers/gpio/gpio-lynxpoint.c
index 3472b05ac512..86c17de87692 100644
--- a/drivers/gpio/gpio-lynxpoint.c
+++ b/drivers/gpio/gpio-lynxpoint.c
@@ -32,6 +32,7 @@
 #include <linux/acpi.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
+#include <linux/io.h>
 
 /* LynxPoint chipset has support for 94 gpio pins */
 
diff --git a/drivers/gpio/gpio-max7300.c b/drivers/gpio/gpio-max7300.c
index 4b6b9a04e326..40ab6dfb6021 100644
--- a/drivers/gpio/gpio-max7300.c
+++ b/drivers/gpio/gpio-max7300.c
@@ -41,7 +41,7 @@ static int max7300_probe(struct i2c_client *client,
 			I2C_FUNC_SMBUS_BYTE_DATA))
 		return -EIO;
 
-	ts = kzalloc(sizeof(struct max7301), GFP_KERNEL);
+	ts = devm_kzalloc(&client->dev, sizeof(struct max7301), GFP_KERNEL);
 	if (!ts)
 		return -ENOMEM;
 
@@ -50,8 +50,6 @@ static int max7300_probe(struct i2c_client *client,
 	ts->dev = &client->dev;
 
 	ret = __max730x_probe(ts);
-	if (ret)
-		kfree(ts);
 	return ret;
 }
 
diff --git a/drivers/gpio/gpio-max7301.c b/drivers/gpio/gpio-max7301.c
index c6c535c1310e..3b16ab701630 100644
--- a/drivers/gpio/gpio-max7301.c
+++ b/drivers/gpio/gpio-max7301.c
@@ -56,12 +56,13 @@ static int max7301_probe(struct spi_device *spi)
 	int ret;
 
 	/* bits_per_word cannot be configured in platform data */
-	spi->bits_per_word = 16;
+	if (spi->dev.platform_data)
+		spi->bits_per_word = 16;
 	ret = spi_setup(spi);
 	if (ret < 0)
 		return ret;
 
-	ts = kzalloc(sizeof(struct max7301), GFP_KERNEL);
+	ts = devm_kzalloc(&spi->dev, sizeof(struct max7301), GFP_KERNEL);
 	if (!ts)
 		return -ENOMEM;
 
@@ -70,8 +71,6 @@ static int max7301_probe(struct spi_device *spi)
 	ts->dev = &spi->dev;
 
 	ret = __max730x_probe(ts);
-	if (ret)
-		kfree(ts);
 	return ret;
 }
 
diff --git a/drivers/gpio/gpio-max732x.c b/drivers/gpio/gpio-max732x.c
index 1e0467ce4c37..d4b51b163b03 100644
--- a/drivers/gpio/gpio-max732x.c
+++ b/drivers/gpio/gpio-max732x.c
@@ -589,7 +589,8 @@ static int max732x_probe(struct i2c_client *client,
 		return -EINVAL;
 	}
 
-	chip = kzalloc(sizeof(struct max732x_chip), GFP_KERNEL);
+	chip = devm_kzalloc(&client->dev, sizeof(struct max732x_chip),
+			GFP_KERNEL);
 	if (chip == NULL)
 		return -ENOMEM;
 	chip->client = client;
@@ -647,7 +648,6 @@ static int max732x_probe(struct i2c_client *client,
 
 out_failed:
 	max732x_irq_teardown(chip);
-	kfree(chip);
 	return ret;
 }
 
@@ -680,7 +680,6 @@ static int max732x_remove(struct i2c_client *client)
 	if (chip->client_dummy)
 		i2c_unregister_device(chip->client_dummy);
 
-	kfree(chip);
 	return 0;
 }
 
diff --git a/drivers/gpio/gpio-mc33880.c b/drivers/gpio/gpio-mc33880.c
index 6a8fdc26ae6a..63a7a1bfb2d9 100644
--- a/drivers/gpio/gpio-mc33880.c
+++ b/drivers/gpio/gpio-mc33880.c
@@ -101,13 +101,13 @@ static int mc33880_probe(struct spi_device *spi)
 	if (ret < 0)
 		return ret;
 
-	mc = kzalloc(sizeof(struct mc33880), GFP_KERNEL);
+	mc = devm_kzalloc(&spi->dev, sizeof(struct mc33880), GFP_KERNEL);
 	if (!mc)
 		return -ENOMEM;
 
 	mutex_init(&mc->lock);
 
-	dev_set_drvdata(&spi->dev, mc);
+	spi_set_drvdata(spi, mc);
 
 	mc->spi = spi;
 
@@ -130,7 +130,8 @@ static int mc33880_probe(struct spi_device *spi)
 		ret = mc33880_write_config(mc);
 
 	if (ret) {
-		printk(KERN_ERR "Failed writing to " DRIVER_NAME ": %d\n", ret);
+		dev_err(&spi->dev, "Failed writing to " DRIVER_NAME ": %d\n",
+			ret);
 		goto exit_destroy;
 	}
 
@@ -141,9 +142,8 @@ static int mc33880_probe(struct spi_device *spi)
 	return ret;
 
 exit_destroy:
-	dev_set_drvdata(&spi->dev, NULL);
+	spi_set_drvdata(spi, NULL);
 	mutex_destroy(&mc->lock);
-	kfree(mc);
 	return ret;
 }
 
@@ -152,17 +152,16 @@ static int mc33880_remove(struct spi_device *spi)
 	struct mc33880 *mc;
 	int ret;
 
-	mc = dev_get_drvdata(&spi->dev);
+	mc = spi_get_drvdata(spi);
 	if (mc == NULL)
 		return -ENODEV;
 
-	dev_set_drvdata(&spi->dev, NULL);
+	spi_set_drvdata(spi, NULL);
 
 	ret = gpiochip_remove(&mc->chip);
-	if (!ret) {
+	if (!ret)
 		mutex_destroy(&mc->lock);
-		kfree(mc);
-	} else
+	else
 		dev_err(&spi->dev, "Failed to remove the GPIO controller: %d\n",
 			ret);
 
diff --git a/drivers/gpio/gpio-mcp23s08.c b/drivers/gpio/gpio-mcp23s08.c
index 3cea0ea79e80..6a4470b84488 100644
--- a/drivers/gpio/gpio-mcp23s08.c
+++ b/drivers/gpio/gpio-mcp23s08.c
@@ -12,6 +12,8 @@
 #include <linux/spi/mcp23s08.h>
 #include <linux/slab.h>
 #include <asm/byteorder.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 
 /**
  * MCP types supported by driver
@@ -383,6 +385,10 @@ static int mcp23s08_probe_one(struct mcp23s08 *mcp, struct device *dev,
 	mcp->chip.direction_output = mcp23s08_direction_output;
 	mcp->chip.set = mcp23s08_set;
 	mcp->chip.dbg_show = mcp23s08_dbg_show;
+#ifdef CONFIG_OF
+	mcp->chip.of_gpio_n_cells = 2;
+	mcp->chip.of_node = dev->of_node;
+#endif
 
 	switch (type) {
 #ifdef CONFIG_SPI_MASTER
@@ -473,6 +479,35 @@ fail:
 
 /*----------------------------------------------------------------------*/
 
+#ifdef CONFIG_OF
+#ifdef CONFIG_SPI_MASTER
+static struct of_device_id mcp23s08_spi_of_match[] = {
+	{
+		.compatible = "mcp,mcp23s08", .data = (void *) MCP_TYPE_S08,
+	},
+	{
+		.compatible = "mcp,mcp23s17", .data = (void *) MCP_TYPE_S17,
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, mcp23s08_spi_of_match);
+#endif
+
+#if IS_ENABLED(CONFIG_I2C)
+static struct of_device_id mcp23s08_i2c_of_match[] = {
+	{
+		.compatible = "mcp,mcp23008", .data = (void *) MCP_TYPE_008,
+	},
+	{
+		.compatible = "mcp,mcp23017", .data = (void *) MCP_TYPE_017,
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, mcp23s08_i2c_of_match);
+#endif
+#endif /* CONFIG_OF */
+
+
 #if IS_ENABLED(CONFIG_I2C)
 
 static int mcp230xx_probe(struct i2c_client *client,
@@ -480,12 +515,23 @@ static int mcp230xx_probe(struct i2c_client *client,
 {
 	struct mcp23s08_platform_data *pdata;
 	struct mcp23s08 *mcp;
-	int status;
-
-	pdata = client->dev.platform_data;
-	if (!pdata || !gpio_is_valid(pdata->base)) {
-		dev_dbg(&client->dev, "invalid or missing platform data\n");
-		return -EINVAL;
+	int status, base, pullups;
+	const struct of_device_id *match;
+
+	match = of_match_device(of_match_ptr(mcp23s08_i2c_of_match),
+					&client->dev);
+	if (match) {
+		base = -1;
+		pullups = 0;
+	} else {
+		pdata = client->dev.platform_data;
+		if (!pdata || !gpio_is_valid(pdata->base)) {
+			dev_dbg(&client->dev,
+					"invalid or missing platform data\n");
+			return -EINVAL;
+		}
+		base = pdata->base;
+		pullups = pdata->chip[0].pullups;
 	}
 
 	mcp = kzalloc(sizeof *mcp, GFP_KERNEL);
@@ -493,8 +539,7 @@ static int mcp230xx_probe(struct i2c_client *client,
 		return -ENOMEM;
 
 	status = mcp23s08_probe_one(mcp, &client->dev, client, client->addr,
-				    id->driver_data, pdata->base,
-				    pdata->chip[0].pullups);
+				    id->driver_data, base, pullups);
 	if (status)
 		goto fail;
 
@@ -531,6 +576,7 @@ static struct i2c_driver mcp230xx_driver = {
 	.driver = {
 		.name	= "mcp230xx",
 		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(mcp23s08_i2c_of_match),
 	},
 	.probe		= mcp230xx_probe,
 	.remove		= mcp230xx_remove,
@@ -565,28 +611,55 @@ static int mcp23s08_probe(struct spi_device *spi)
 	unsigned			chips = 0;
 	struct mcp23s08_driver_data	*data;
 	int				status, type;
-	unsigned			base;
-
-	type = spi_get_device_id(spi)->driver_data;
-
-	pdata = spi->dev.platform_data;
-	if (!pdata || !gpio_is_valid(pdata->base)) {
-		dev_dbg(&spi->dev, "invalid or missing platform data\n");
-		return -EINVAL;
-	}
+	unsigned			base = -1,
+					ngpio = 0,
+					pullups[ARRAY_SIZE(pdata->chip)];
+	const struct			of_device_id *match;
+	u32				spi_present_mask = 0;
+
+	match = of_match_device(of_match_ptr(mcp23s08_spi_of_match), &spi->dev);
+	if (match) {
+		type = (int)match->data;
+		status = of_property_read_u32(spi->dev.of_node,
+				"mcp,spi-present-mask", &spi_present_mask);
+		if (status) {
+			dev_err(&spi->dev, "DT has no spi-present-mask\n");
+			return -ENODEV;
+		}
+		if ((spi_present_mask <= 0) || (spi_present_mask >= 256)) {
+			dev_err(&spi->dev, "invalid spi-present-mask\n");
+			return -ENODEV;
+		}
 
-	for (addr = 0; addr < ARRAY_SIZE(pdata->chip); addr++) {
-		if (!pdata->chip[addr].is_present)
-			continue;
-		chips++;
-		if ((type == MCP_TYPE_S08) && (addr > 3)) {
-			dev_err(&spi->dev,
-				"mcp23s08 only supports address 0..3\n");
+		for (addr = 0; addr < ARRAY_SIZE(pdata->chip); addr++)
+			pullups[addr] = 0;
+	} else {
+		type = spi_get_device_id(spi)->driver_data;
+		pdata = spi->dev.platform_data;
+		if (!pdata || !gpio_is_valid(pdata->base)) {
+			dev_dbg(&spi->dev,
+					"invalid or missing platform data\n");
 			return -EINVAL;
 		}
+
+		for (addr = 0; addr < ARRAY_SIZE(pdata->chip); addr++) {
+			if (!pdata->chip[addr].is_present)
+				continue;
+			chips++;
+			if ((type == MCP_TYPE_S08) && (addr > 3)) {
+				dev_err(&spi->dev,
+					"mcp23s08 only supports address 0..3\n");
+				return -EINVAL;
+			}
+			spi_present_mask |= 1 << addr;
+			pullups[addr] = pdata->chip[addr].pullups;
+		}
+
+		if (!chips)
+			return -ENODEV;
+
+		base = pdata->base;
 	}
-	if (!chips)
-		return -ENODEV;
 
 	data = kzalloc(sizeof *data + chips * sizeof(struct mcp23s08),
 			GFP_KERNEL);
@@ -594,21 +667,22 @@ static int mcp23s08_probe(struct spi_device *spi)
 		return -ENOMEM;
 	spi_set_drvdata(spi, data);
 
-	base = pdata->base;
 	for (addr = 0; addr < ARRAY_SIZE(pdata->chip); addr++) {
-		if (!pdata->chip[addr].is_present)
+		if (!(spi_present_mask & (1 << addr)))
 			continue;
 		chips--;
 		data->mcp[addr] = &data->chip[chips];
 		status = mcp23s08_probe_one(data->mcp[addr], &spi->dev, spi,
 					    0x40 | (addr << 1), type, base,
-					    pdata->chip[addr].pullups);
+					    pullups[addr]);
 		if (status < 0)
 			goto fail;
 
-		base += (type == MCP_TYPE_S17) ? 16 : 8;
+		if (base != -1)
+			base += (type == MCP_TYPE_S17) ? 16 : 8;
+		ngpio += (type == MCP_TYPE_S17) ? 16 : 8;
 	}
-	data->ngpio = base - pdata->base;
+	data->ngpio = ngpio;
 
 	/* NOTE:  these chips have a relatively sane IRQ framework, with
 	 * per-signal masking and level/edge triggering.  It's not yet
@@ -668,6 +742,7 @@ static struct spi_driver mcp23s08_driver = {
 	.driver = {
 		.name	= "mcp23s08",
 		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(mcp23s08_spi_of_match),
 	},
 };
 
diff --git a/drivers/gpio/gpio-msm-v1.c b/drivers/gpio/gpio-msm-v1.c
index 52a4d4286eba..c798585a3fe5 100644
--- a/drivers/gpio/gpio-msm-v1.c
+++ b/drivers/gpio/gpio-msm-v1.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (C) 2007 Google, Inc.
- * Copyright (c) 2009-2011, Code Aurora Forum. All rights reserved.
+ * Copyright (c) 2009-2012, The Linux Foundation. All rights reserved.
  *
  * This software is licensed under the terms of the GNU General Public
  * License version 2, as published by the Free Software Foundation, and
@@ -19,9 +19,10 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 #include <linux/module.h>
-#include <mach/cpu.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+
 #include <mach/msm_gpiomux.h>
-#include <mach/msm_iomap.h>
 
 /* see 80-VA736-2 Rev C pp 695-751
 **
@@ -34,10 +35,10 @@
 ** macros.
 */
 
-#define MSM_GPIO1_REG(off) (MSM_GPIO1_BASE + (off))
-#define MSM_GPIO2_REG(off) (MSM_GPIO2_BASE + 0x400 + (off))
-#define MSM_GPIO1_SHADOW_REG(off) (MSM_GPIO1_BASE + 0x800 + (off))
-#define MSM_GPIO2_SHADOW_REG(off) (MSM_GPIO2_BASE + 0xC00 + (off))
+#define MSM_GPIO1_REG(off) (off)
+#define MSM_GPIO2_REG(off) (off)
+#define MSM_GPIO1_SHADOW_REG(off) (off)
+#define MSM_GPIO2_SHADOW_REG(off) (off)
 
 /*
  * MSM7X00 registers
@@ -276,16 +277,14 @@
 
 #define MSM_GPIO_BANK(soc, bank, first, last)				\
 	{								\
-		.regs = {						\
-			.out =         soc##_GPIO_OUT_##bank,		\
-			.in =          soc##_GPIO_IN_##bank,		\
-			.int_status =  soc##_GPIO_INT_STATUS_##bank,	\
-			.int_clear =   soc##_GPIO_INT_CLEAR_##bank,	\
-			.int_en =      soc##_GPIO_INT_EN_##bank,	\
-			.int_edge =    soc##_GPIO_INT_EDGE_##bank,	\
-			.int_pos =     soc##_GPIO_INT_POS_##bank,	\
-			.oe =          soc##_GPIO_OE_##bank,		\
-		},							\
+		.regs[MSM_GPIO_OUT] =         soc##_GPIO_OUT_##bank,	\
+		.regs[MSM_GPIO_IN] =          soc##_GPIO_IN_##bank,	\
+		.regs[MSM_GPIO_INT_STATUS] =  soc##_GPIO_INT_STATUS_##bank, \
+		.regs[MSM_GPIO_INT_CLEAR] =   soc##_GPIO_INT_CLEAR_##bank, \
+		.regs[MSM_GPIO_INT_EN] =      soc##_GPIO_INT_EN_##bank,	\
+		.regs[MSM_GPIO_INT_EDGE] =    soc##_GPIO_INT_EDGE_##bank, \
+		.regs[MSM_GPIO_INT_POS] =     soc##_GPIO_INT_POS_##bank, \
+		.regs[MSM_GPIO_OE] =          soc##_GPIO_OE_##bank,	\
 		.chip = {						\
 			.base = (first),				\
 			.ngpio = (last) - (first) + 1,			\
@@ -301,39 +300,57 @@
 
 #define MSM_GPIO_BROKEN_INT_CLEAR 1
 
-struct msm_gpio_regs {
-	void __iomem *out;
-	void __iomem *in;
-	void __iomem *int_status;
-	void __iomem *int_clear;
-	void __iomem *int_en;
-	void __iomem *int_edge;
-	void __iomem *int_pos;
-	void __iomem *oe;
+enum msm_gpio_reg {
+	MSM_GPIO_IN,
+	MSM_GPIO_OUT,
+	MSM_GPIO_INT_STATUS,
+	MSM_GPIO_INT_CLEAR,
+	MSM_GPIO_INT_EN,
+	MSM_GPIO_INT_EDGE,
+	MSM_GPIO_INT_POS,
+	MSM_GPIO_OE,
+	MSM_GPIO_REG_NR
 };
 
 struct msm_gpio_chip {
 	spinlock_t		lock;
 	struct gpio_chip	chip;
-	struct msm_gpio_regs	regs;
+	unsigned long		regs[MSM_GPIO_REG_NR];
 #if MSM_GPIO_BROKEN_INT_CLEAR
 	unsigned                int_status_copy;
 #endif
 	unsigned int            both_edge_detect;
 	unsigned int            int_enable[2]; /* 0: awake, 1: sleep */
+	void __iomem		*base;
+};
+
+struct msm_gpio_initdata {
+	struct msm_gpio_chip *chips;
+	int count;
 };
 
+static void msm_gpio_writel(struct msm_gpio_chip *chip, u32 val,
+			    enum msm_gpio_reg reg)
+{
+	writel(val, chip->base + chip->regs[reg]);
+}
+
+static u32 msm_gpio_readl(struct msm_gpio_chip *chip, enum msm_gpio_reg reg)
+{
+	return readl(chip->base + chip->regs[reg]);
+}
+
 static int msm_gpio_write(struct msm_gpio_chip *msm_chip,
 			  unsigned offset, unsigned on)
 {
 	unsigned mask = BIT(offset);
 	unsigned val;
 
-	val = readl(msm_chip->regs.out);
+	val = msm_gpio_readl(msm_chip, MSM_GPIO_OUT);
 	if (on)
-		writel(val | mask, msm_chip->regs.out);
+		msm_gpio_writel(msm_chip, val | mask, MSM_GPIO_OUT);
 	else
-		writel(val & ~mask, msm_chip->regs.out);
+		msm_gpio_writel(msm_chip, val & ~mask, MSM_GPIO_OUT);
 	return 0;
 }
 
@@ -342,13 +359,13 @@ static void msm_gpio_update_both_edge_detect(struct msm_gpio_chip *msm_chip)
 	int loop_limit = 100;
 	unsigned pol, val, val2, intstat;
 	do {
-		val = readl(msm_chip->regs.in);
-		pol = readl(msm_chip->regs.int_pos);
+		val = msm_gpio_readl(msm_chip, MSM_GPIO_IN);
+		pol = msm_gpio_readl(msm_chip, MSM_GPIO_INT_POS);
 		pol = (pol & ~msm_chip->both_edge_detect) |
 		      (~val & msm_chip->both_edge_detect);
-		writel(pol, msm_chip->regs.int_pos);
-		intstat = readl(msm_chip->regs.int_status);
-		val2 = readl(msm_chip->regs.in);
+		msm_gpio_writel(msm_chip, pol, MSM_GPIO_INT_POS);
+		intstat = msm_gpio_readl(msm_chip, MSM_GPIO_INT_STATUS);
+		val2 = msm_gpio_readl(msm_chip, MSM_GPIO_IN);
 		if (((val ^ val2) & msm_chip->both_edge_detect & ~intstat) == 0)
 			return;
 	} while (loop_limit-- > 0);
@@ -365,10 +382,11 @@ static int msm_gpio_clear_detect_status(struct msm_gpio_chip *msm_chip,
 	/* Save interrupts that already triggered before we loose them. */
 	/* Any interrupt that triggers between the read of int_status */
 	/* and the write to int_clear will still be lost though. */
-	msm_chip->int_status_copy |= readl(msm_chip->regs.int_status);
+	msm_chip->int_status_copy |=
+		msm_gpio_readl(msm_chip, MSM_GPIO_INT_STATUS);
 	msm_chip->int_status_copy &= ~bit;
 #endif
-	writel(bit, msm_chip->regs.int_clear);
+	msm_gpio_writel(msm_chip, bit, MSM_GPIO_INT_CLEAR);
 	msm_gpio_update_both_edge_detect(msm_chip);
 	return 0;
 }
@@ -377,10 +395,12 @@ static int msm_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
 	struct msm_gpio_chip *msm_chip;
 	unsigned long irq_flags;
+	u32 val;
 
 	msm_chip = container_of(chip, struct msm_gpio_chip, chip);
 	spin_lock_irqsave(&msm_chip->lock, irq_flags);
-	writel(readl(msm_chip->regs.oe) & ~BIT(offset), msm_chip->regs.oe);
+	val = msm_gpio_readl(msm_chip, MSM_GPIO_OE) & ~BIT(offset);
+	msm_gpio_writel(msm_chip, val, MSM_GPIO_OE);
 	spin_unlock_irqrestore(&msm_chip->lock, irq_flags);
 	return 0;
 }
@@ -390,11 +410,13 @@ msm_gpio_direction_output(struct gpio_chip *chip, unsigned offset, int value)
 {
 	struct msm_gpio_chip *msm_chip;
 	unsigned long irq_flags;
+	u32 val;
 
 	msm_chip = container_of(chip, struct msm_gpio_chip, chip);
 	spin_lock_irqsave(&msm_chip->lock, irq_flags);
 	msm_gpio_write(msm_chip, offset, value);
-	writel(readl(msm_chip->regs.oe) | BIT(offset), msm_chip->regs.oe);
+	val = msm_gpio_readl(msm_chip, MSM_GPIO_OE) | BIT(offset);
+	msm_gpio_writel(msm_chip, val, MSM_GPIO_OE);
 	spin_unlock_irqrestore(&msm_chip->lock, irq_flags);
 	return 0;
 }
@@ -404,7 +426,7 @@ static int msm_gpio_get(struct gpio_chip *chip, unsigned offset)
 	struct msm_gpio_chip *msm_chip;
 
 	msm_chip = container_of(chip, struct msm_gpio_chip, chip);
-	return (readl(msm_chip->regs.in) & (1U << offset)) ? 1 : 0;
+	return (msm_gpio_readl(msm_chip, MSM_GPIO_IN) & (1U << offset)) ? 1 : 0;
 }
 
 static void msm_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
@@ -450,6 +472,11 @@ static struct msm_gpio_chip msm_gpio_chips_msm7x01[] = {
 	MSM_GPIO_BANK(MSM7X00, 5, 107, 121),
 };
 
+static struct msm_gpio_initdata msm_gpio_7x01_init = {
+	.chips = msm_gpio_chips_msm7x01,
+	.count = ARRAY_SIZE(msm_gpio_chips_msm7x01),
+};
+
 static struct msm_gpio_chip msm_gpio_chips_msm7x30[] = {
 	MSM_GPIO_BANK(MSM7X30, 0,   0,  15),
 	MSM_GPIO_BANK(MSM7X30, 1,  16,  43),
@@ -461,6 +488,11 @@ static struct msm_gpio_chip msm_gpio_chips_msm7x30[] = {
 	MSM_GPIO_BANK(MSM7X30, 7, 151, 181),
 };
 
+static struct msm_gpio_initdata msm_gpio_7x30_init = {
+	.chips = msm_gpio_chips_msm7x30,
+	.count = ARRAY_SIZE(msm_gpio_chips_msm7x30),
+};
+
 static struct msm_gpio_chip msm_gpio_chips_qsd8x50[] = {
 	MSM_GPIO_BANK(QSD8X50, 0,   0,  15),
 	MSM_GPIO_BANK(QSD8X50, 1,  16,  42),
@@ -472,6 +504,11 @@ static struct msm_gpio_chip msm_gpio_chips_qsd8x50[] = {
 	MSM_GPIO_BANK(QSD8X50, 7, 153, 164),
 };
 
+static struct msm_gpio_initdata msm_gpio_8x50_init = {
+	.chips = msm_gpio_chips_qsd8x50,
+	.count = ARRAY_SIZE(msm_gpio_chips_qsd8x50),
+};
+
 static void msm_gpio_irq_ack(struct irq_data *d)
 {
 	unsigned long irq_flags;
@@ -490,10 +527,10 @@ static void msm_gpio_irq_mask(struct irq_data *d)
 
 	spin_lock_irqsave(&msm_chip->lock, irq_flags);
 	/* level triggered interrupts are also latched */
-	if (!(readl(msm_chip->regs.int_edge) & BIT(offset)))
+	if (!(msm_gpio_readl(msm_chip, MSM_GPIO_INT_EDGE) & BIT(offset)))
 		msm_gpio_clear_detect_status(msm_chip, offset);
 	msm_chip->int_enable[0] &= ~BIT(offset);
-	writel(msm_chip->int_enable[0], msm_chip->regs.int_en);
+	msm_gpio_writel(msm_chip, msm_chip->int_enable[0], MSM_GPIO_INT_EN);
 	spin_unlock_irqrestore(&msm_chip->lock, irq_flags);
 }
 
@@ -505,10 +542,10 @@ static void msm_gpio_irq_unmask(struct irq_data *d)
 
 	spin_lock_irqsave(&msm_chip->lock, irq_flags);
 	/* level triggered interrupts are also latched */
-	if (!(readl(msm_chip->regs.int_edge) & BIT(offset)))
+	if (!(msm_gpio_readl(msm_chip, MSM_GPIO_INT_EDGE) & BIT(offset)))
 		msm_gpio_clear_detect_status(msm_chip, offset);
 	msm_chip->int_enable[0] |= BIT(offset);
-	writel(msm_chip->int_enable[0], msm_chip->regs.int_en);
+	msm_gpio_writel(msm_chip, msm_chip->int_enable[0], MSM_GPIO_INT_EN);
 	spin_unlock_irqrestore(&msm_chip->lock, irq_flags);
 }
 
@@ -537,12 +574,12 @@ static int msm_gpio_irq_set_type(struct irq_data *d, unsigned int flow_type)
 	unsigned val, mask = BIT(offset);
 
 	spin_lock_irqsave(&msm_chip->lock, irq_flags);
-	val = readl(msm_chip->regs.int_edge);
+	val = msm_gpio_readl(msm_chip, MSM_GPIO_INT_EDGE);
 	if (flow_type & IRQ_TYPE_EDGE_BOTH) {
-		writel(val | mask, msm_chip->regs.int_edge);
+		msm_gpio_writel(msm_chip, val | mask, MSM_GPIO_INT_EDGE);
 		__irq_set_handler_locked(d->irq, handle_edge_irq);
 	} else {
-		writel(val & ~mask, msm_chip->regs.int_edge);
+		msm_gpio_writel(msm_chip, val & ~mask, MSM_GPIO_INT_EDGE);
 		__irq_set_handler_locked(d->irq, handle_level_irq);
 	}
 	if ((flow_type & IRQ_TYPE_EDGE_BOTH) == IRQ_TYPE_EDGE_BOTH) {
@@ -550,11 +587,12 @@ static int msm_gpio_irq_set_type(struct irq_data *d, unsigned int flow_type)
 		msm_gpio_update_both_edge_detect(msm_chip);
 	} else {
 		msm_chip->both_edge_detect &= ~mask;
-		val = readl(msm_chip->regs.int_pos);
+		val = msm_gpio_readl(msm_chip, MSM_GPIO_INT_POS);
 		if (flow_type & (IRQF_TRIGGER_RISING | IRQF_TRIGGER_HIGH))
-			writel(val | mask, msm_chip->regs.int_pos);
+			val |= mask;
 		else
-			writel(val & ~mask, msm_chip->regs.int_pos);
+			val &= ~mask;
+		msm_gpio_writel(msm_chip, val, MSM_GPIO_INT_POS);
 	}
 	spin_unlock_irqrestore(&msm_chip->lock, irq_flags);
 	return 0;
@@ -567,7 +605,7 @@ static void msm_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 
 	for (i = 0; i < msm_gpio_count; i++) {
 		struct msm_gpio_chip *msm_chip = &msm_gpio_chips[i];
-		val = readl(msm_chip->regs.int_status);
+		val = msm_gpio_readl(msm_chip, MSM_GPIO_INT_STATUS);
 		val &= msm_chip->int_enable[0];
 		while (val) {
 			mask = val & -val;
@@ -592,22 +630,36 @@ static struct irq_chip msm_gpio_irq_chip = {
 	.irq_set_type  = msm_gpio_irq_set_type,
 };
 
-static int __init msm_init_gpio(void)
+static int __devinit gpio_msm_v1_probe(struct platform_device *pdev)
 {
 	int i, j = 0;
-
-	if (cpu_is_msm7x01()) {
-		msm_gpio_chips = msm_gpio_chips_msm7x01;
-		msm_gpio_count = ARRAY_SIZE(msm_gpio_chips_msm7x01);
-	} else if (cpu_is_msm7x30()) {
-		msm_gpio_chips = msm_gpio_chips_msm7x30;
-		msm_gpio_count = ARRAY_SIZE(msm_gpio_chips_msm7x30);
-	} else if (cpu_is_qsd8x50()) {
-		msm_gpio_chips = msm_gpio_chips_qsd8x50;
-		msm_gpio_count = ARRAY_SIZE(msm_gpio_chips_qsd8x50);
-	} else {
-		return 0;
-	}
+	const struct platform_device_id *dev_id = platform_get_device_id(pdev);
+	struct msm_gpio_initdata *data;
+	int irq1, irq2;
+	struct resource *res;
+	void __iomem *base1, __iomem *base2;
+
+	data = (struct msm_gpio_initdata *)dev_id->driver_data;
+	msm_gpio_chips = data->chips;
+	msm_gpio_count = data->count;
+
+	irq1 = platform_get_irq(pdev, 0);
+	if (irq1 < 0)
+		return irq1;
+
+	irq2 = platform_get_irq(pdev, 1);
+	if (irq2 < 0)
+		return irq2;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base1 = devm_request_and_ioremap(&pdev->dev, res);
+	if (!base1)
+		return -EADDRNOTAVAIL;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	base2 = devm_request_and_ioremap(&pdev->dev, res);
+	if (!base2)
+		return -EADDRNOTAVAIL;
 
 	for (i = FIRST_GPIO_IRQ; i < FIRST_GPIO_IRQ + NR_GPIO_IRQS; i++) {
 		if (i - FIRST_GPIO_IRQ >=
@@ -621,16 +673,42 @@ static int __init msm_init_gpio(void)
 	}
 
 	for (i = 0; i < msm_gpio_count; i++) {
+		if (i == 1)
+			msm_gpio_chips[i].base = base2;
+		else
+			msm_gpio_chips[i].base = base1;
 		spin_lock_init(&msm_gpio_chips[i].lock);
-		writel(0, msm_gpio_chips[i].regs.int_en);
+		msm_gpio_writel(&msm_gpio_chips[i], 0, MSM_GPIO_INT_EN);
 		gpiochip_add(&msm_gpio_chips[i].chip);
 	}
 
-	irq_set_chained_handler(INT_GPIO_GROUP1, msm_gpio_irq_handler);
-	irq_set_chained_handler(INT_GPIO_GROUP2, msm_gpio_irq_handler);
-	irq_set_irq_wake(INT_GPIO_GROUP1, 1);
-	irq_set_irq_wake(INT_GPIO_GROUP2, 2);
+	irq_set_chained_handler(irq1, msm_gpio_irq_handler);
+	irq_set_chained_handler(irq2, msm_gpio_irq_handler);
+	irq_set_irq_wake(irq1, 1);
+	irq_set_irq_wake(irq2, 2);
 	return 0;
 }
 
-postcore_initcall(msm_init_gpio);
+static struct platform_device_id gpio_msm_v1_device_ids[] = {
+	{ "gpio-msm-7201", (unsigned long)&msm_gpio_7x01_init },
+	{ "gpio-msm-7x30", (unsigned long)&msm_gpio_7x30_init },
+	{ "gpio-msm-8x50", (unsigned long)&msm_gpio_8x50_init },
+	{ }
+};
+MODULE_DEVICE_TABLE(platform, gpio_msm_v1_device_ids);
+
+static struct platform_driver gpio_msm_v1_driver = {
+	.driver = {
+		.name = "gpio-msm-v1",
+		.owner = THIS_MODULE,
+	},
+	.probe = gpio_msm_v1_probe,
+	.id_table = gpio_msm_v1_device_ids,
+};
+
+static int __init gpio_msm_v1_init(void)
+{
+	return platform_driver_register(&gpio_msm_v1_driver);
+}
+postcore_initcall(gpio_msm_v1_init);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpio/gpio-msm-v2.c b/drivers/gpio/gpio-msm-v2.c
index 55a7e7769af6..dd2eddeb1e0c 100644
--- a/drivers/gpio/gpio-msm-v2.c
+++ b/drivers/gpio/gpio-msm-v2.c
@@ -23,13 +23,12 @@
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
+#include <linux/irqchip/chained_irq.h>
 #include <linux/irq.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/spinlock.h>
 
-#include <asm/mach/irq.h>
-
 #include <mach/msm_gpiomux.h>
 #include <mach/msm_iomap.h>
 
diff --git a/drivers/gpio/gpio-mvebu.c b/drivers/gpio/gpio-mvebu.c
index 61a6fde6c089..bf69a7eff370 100644
--- a/drivers/gpio/gpio-mvebu.c
+++ b/drivers/gpio/gpio-mvebu.c
@@ -117,7 +117,7 @@ static inline void __iomem *mvebu_gpioreg_edge_cause(struct mvebu_gpio_chip *mvc
 {
 	int cpu;
 
-	switch(mvchip->soc_variant) {
+	switch (mvchip->soc_variant) {
 	case MVEBU_GPIO_SOC_VARIANT_ORION:
 	case MVEBU_GPIO_SOC_VARIANT_MV78200:
 		return mvchip->membase + GPIO_EDGE_CAUSE_OFF;
@@ -133,7 +133,7 @@ static inline void __iomem *mvebu_gpioreg_edge_mask(struct mvebu_gpio_chip *mvch
 {
 	int cpu;
 
-	switch(mvchip->soc_variant) {
+	switch (mvchip->soc_variant) {
 	case MVEBU_GPIO_SOC_VARIANT_ORION:
 		return mvchip->membase + GPIO_EDGE_MASK_OFF;
 	case MVEBU_GPIO_SOC_VARIANT_MV78200:
@@ -151,7 +151,7 @@ static void __iomem *mvebu_gpioreg_level_mask(struct mvebu_gpio_chip *mvchip)
 {
 	int cpu;
 
-	switch(mvchip->soc_variant) {
+	switch (mvchip->soc_variant) {
 	case MVEBU_GPIO_SOC_VARIANT_ORION:
 		return mvchip->membase + GPIO_LEVEL_MASK_OFF;
 	case MVEBU_GPIO_SOC_VARIANT_MV78200:
@@ -401,7 +401,7 @@ static int mvebu_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 	/*
 	 * Configure interrupt polarity.
 	 */
-	switch(type) {
+	switch (type) {
 	case IRQ_TYPE_EDGE_RISING:
 	case IRQ_TYPE_LEVEL_HIGH:
 		u = readl_relaxed(mvebu_gpioreg_in_pol(mvchip));
@@ -470,18 +470,76 @@ static void mvebu_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 	}
 }
 
+#ifdef CONFIG_DEBUG_FS
+#include <linux/seq_file.h>
+
+static void mvebu_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
+{
+	struct mvebu_gpio_chip *mvchip =
+		container_of(chip, struct mvebu_gpio_chip, chip);
+	u32 out, io_conf, blink, in_pol, data_in, cause, edg_msk, lvl_msk;
+	int i;
+
+	out	= readl_relaxed(mvebu_gpioreg_out(mvchip));
+	io_conf	= readl_relaxed(mvebu_gpioreg_io_conf(mvchip));
+	blink	= readl_relaxed(mvebu_gpioreg_blink(mvchip));
+	in_pol	= readl_relaxed(mvebu_gpioreg_in_pol(mvchip));
+	data_in	= readl_relaxed(mvebu_gpioreg_data_in(mvchip));
+	cause	= readl_relaxed(mvebu_gpioreg_edge_cause(mvchip));
+	edg_msk	= readl_relaxed(mvebu_gpioreg_edge_mask(mvchip));
+	lvl_msk	= readl_relaxed(mvebu_gpioreg_level_mask(mvchip));
+
+	for (i = 0; i < chip->ngpio; i++) {
+		const char *label;
+		u32 msk;
+		bool is_out;
+
+		label = gpiochip_is_requested(chip, i);
+		if (!label)
+			continue;
+
+		msk = 1 << i;
+		is_out = !(io_conf & msk);
+
+		seq_printf(s, " gpio-%-3d (%-20.20s)", chip->base + i, label);
+
+		if (is_out) {
+			seq_printf(s, " out %s %s\n",
+				   out & msk ? "hi" : "lo",
+				   blink & msk ? "(blink )" : "");
+			continue;
+		}
+
+		seq_printf(s, " in  %s (act %s) - IRQ",
+			   (data_in ^ in_pol) & msk  ? "hi" : "lo",
+			   in_pol & msk ? "lo" : "hi");
+		if (!((edg_msk | lvl_msk) & msk)) {
+			seq_printf(s, " disabled\n");
+			continue;
+		}
+		if (edg_msk & msk)
+			seq_printf(s, " edge ");
+		if (lvl_msk & msk)
+			seq_printf(s, " level");
+		seq_printf(s, " (%s)\n", cause & msk ? "pending" : "clear  ");
+	}
+}
+#else
+#define mvebu_gpio_dbg_show NULL
+#endif
+
 static struct of_device_id mvebu_gpio_of_match[] = {
 	{
 		.compatible = "marvell,orion-gpio",
-		.data       = (void*) MVEBU_GPIO_SOC_VARIANT_ORION,
+		.data       = (void *) MVEBU_GPIO_SOC_VARIANT_ORION,
 	},
 	{
 		.compatible = "marvell,mv78200-gpio",
-		.data       = (void*) MVEBU_GPIO_SOC_VARIANT_MV78200,
+		.data       = (void *) MVEBU_GPIO_SOC_VARIANT_MV78200,
 	},
 	{
 		.compatible = "marvell,armadaxp-gpio",
-		.data       = (void*) MVEBU_GPIO_SOC_VARIANT_ARMADAXP,
+		.data       = (void *) MVEBU_GPIO_SOC_VARIANT_ARMADAXP,
 	},
 	{
 		/* sentinel */
@@ -509,13 +567,13 @@ static int mvebu_gpio_probe(struct platform_device *pdev)
 		soc_variant = MVEBU_GPIO_SOC_VARIANT_ORION;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (! res) {
+	if (!res) {
 		dev_err(&pdev->dev, "Cannot get memory resource\n");
 		return -ENODEV;
 	}
 
 	mvchip = devm_kzalloc(&pdev->dev, sizeof(struct mvebu_gpio_chip), GFP_KERNEL);
-	if (! mvchip){
+	if (!mvchip) {
 		dev_err(&pdev->dev, "Cannot allocate memory\n");
 		return -ENOMEM;
 	}
@@ -550,6 +608,7 @@ static int mvebu_gpio_probe(struct platform_device *pdev)
 	mvchip->chip.ngpio = ngpios;
 	mvchip->chip.can_sleep = 0;
 	mvchip->chip.of_node = np;
+	mvchip->chip.dbg_show = mvebu_gpio_dbg_show;
 
 	spin_lock_init(&mvchip->lock);
 	mvchip->membase = devm_ioremap_resource(&pdev->dev, res);
@@ -560,21 +619,21 @@ static int mvebu_gpio_probe(struct platform_device *pdev)
 	 * per-CPU registers */
 	if (soc_variant == MVEBU_GPIO_SOC_VARIANT_ARMADAXP) {
 		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-		if (! res) {
+		if (!res) {
 			dev_err(&pdev->dev, "Cannot get memory resource\n");
 			return -ENODEV;
 		}
 
 		mvchip->percpu_membase = devm_ioremap_resource(&pdev->dev,
 							       res);
-		if (IS_ERR(mvchip->percpu_membase)) 
+		if (IS_ERR(mvchip->percpu_membase))
 			return PTR_ERR(mvchip->percpu_membase);
 	}
 
 	/*
 	 * Mask and clear GPIO interrupts.
 	 */
-	switch(soc_variant) {
+	switch (soc_variant) {
 	case MVEBU_GPIO_SOC_VARIANT_ORION:
 		writel_relaxed(0, mvchip->membase + GPIO_EDGE_CAUSE_OFF);
 		writel_relaxed(0, mvchip->membase + GPIO_EDGE_MASK_OFF);
@@ -632,7 +691,7 @@ static int mvebu_gpio_probe(struct platform_device *pdev)
 
 	gc = irq_alloc_generic_chip("mvebu_gpio_irq", 2, mvchip->irqbase,
 				    mvchip->membase, handle_level_irq);
-	if (! gc) {
+	if (!gc) {
 		dev_err(&pdev->dev, "Cannot allocate generic irq_chip\n");
 		return -ENOMEM;
 	}
diff --git a/drivers/gpio/gpio-mxc.c b/drivers/gpio/gpio-mxc.c
index 7877335c4cc8..7176743915d3 100644
--- a/drivers/gpio/gpio-mxc.c
+++ b/drivers/gpio/gpio-mxc.c
@@ -24,6 +24,7 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 #include <linux/irqdomain.h>
+#include <linux/irqchip/chained_irq.h>
 #include <linux/gpio.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
@@ -32,7 +33,6 @@
 #include <linux/of_device.h>
 #include <linux/module.h>
 #include <asm-generic/bug.h>
-#include <asm/mach/irq.h>
 
 enum mxc_gpio_hwtype {
 	IMX1_GPIO,	/* runs on i.mx1 */
diff --git a/drivers/gpio/gpio-omap.c b/drivers/gpio/gpio-omap.c
index 159f5c57eb45..2050891d9c65 100644
--- a/drivers/gpio/gpio-omap.c
+++ b/drivers/gpio/gpio-omap.c
@@ -25,11 +25,10 @@
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/irqdomain.h>
+#include <linux/irqchip/chained_irq.h>
 #include <linux/gpio.h>
 #include <linux/platform_data/gpio-omap.h>
 
-#include <asm/mach/irq.h>
-
 #define OFF_MODE	1
 
 static LIST_HEAD(omap_gpio_list);
@@ -53,7 +52,6 @@ struct gpio_bank {
 	struct list_head node;
 	void __iomem *base;
 	u16 irq;
-	int irq_base;
 	struct irq_domain *domain;
 	u32 non_wakeup_gpios;
 	u32 enabled_non_wakeup_gpios;
@@ -89,7 +87,14 @@ struct gpio_bank {
 
 static int irq_to_gpio(struct gpio_bank *bank, unsigned int gpio_irq)
 {
-	return gpio_irq - bank->irq_base + bank->chip.base;
+	return bank->chip.base + gpio_irq;
+}
+
+static int omap_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
+{
+	struct gpio_bank *bank = container_of(chip, struct gpio_bank, chip);
+
+	return irq_find_mapping(bank->domain, offset);
 }
 
 static void _set_gpio_direction(struct gpio_bank *bank, int gpio, int is_input)
@@ -421,13 +426,16 @@ static int gpio_irq_type(struct irq_data *d, unsigned type)
 	int retval;
 	unsigned long flags;
 
+	if (WARN_ON(!bank->mod_usage))
+		return -EINVAL;
+
 #ifdef CONFIG_ARCH_OMAP1
 	if (d->irq > IH_MPUIO_BASE)
 		gpio = OMAP_MPUIO(d->irq - IH_MPUIO_BASE);
 #endif
 
 	if (!gpio)
-		gpio = irq_to_gpio(bank, d->irq);
+		gpio = irq_to_gpio(bank, d->hwirq);
 
 	if (type & ~IRQ_TYPE_SENSE_MASK)
 		return -EINVAL;
@@ -580,7 +588,7 @@ static void _reset_gpio(struct gpio_bank *bank, int gpio)
 static int gpio_wake_enable(struct irq_data *d, unsigned int enable)
 {
 	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
-	unsigned int gpio = irq_to_gpio(bank, d->irq);
+	unsigned int gpio = irq_to_gpio(bank, d->hwirq);
 
 	return _set_gpio_wakeup(bank, gpio, enable);
 }
@@ -680,7 +688,7 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 {
 	void __iomem *isr_reg = NULL;
 	u32 isr;
-	unsigned int gpio_irq, gpio_index;
+	unsigned int bit;
 	struct gpio_bank *bank;
 	int unmasked = 0;
 	struct irq_chip *chip = irq_desc_get_chip(desc);
@@ -694,7 +702,7 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 	if (WARN_ON(!isr_reg))
 		goto exit;
 
-	while(1) {
+	while (1) {
 		u32 isr_saved, level_mask = 0;
 		u32 enabled;
 
@@ -721,14 +729,9 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 		if (!isr)
 			break;
 
-		gpio_irq = bank->irq_base;
-		for (; isr != 0; isr >>= 1, gpio_irq++) {
-			int gpio = irq_to_gpio(bank, gpio_irq);
-
-			if (!(isr & 1))
-				continue;
-
-			gpio_index = GPIO_INDEX(bank, gpio);
+		while (isr) {
+			bit = __ffs(isr);
+			isr &= ~(1 << bit);
 
 			/*
 			 * Some chips can't respond to both rising and falling
@@ -737,10 +740,10 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 			 * to respond to the IRQ for the opposite direction.
 			 * This will be indicated in the bank toggle_mask.
 			 */
-			if (bank->toggle_mask & (1 << gpio_index))
-				_toggle_gpio_edge_triggering(bank, gpio_index);
+			if (bank->toggle_mask & (1 << bit))
+				_toggle_gpio_edge_triggering(bank, bit);
 
-			generic_handle_irq(gpio_irq);
+			generic_handle_irq(irq_find_mapping(bank->domain, bit));
 		}
 	}
 	/* if bank has any level sensitive GPIO pin interrupt
@@ -756,7 +759,7 @@ exit:
 static void gpio_irq_shutdown(struct irq_data *d)
 {
 	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
-	unsigned int gpio = irq_to_gpio(bank, d->irq);
+	unsigned int gpio = irq_to_gpio(bank, d->hwirq);
 	unsigned long flags;
 
 	spin_lock_irqsave(&bank->lock, flags);
@@ -767,7 +770,7 @@ static void gpio_irq_shutdown(struct irq_data *d)
 static void gpio_ack_irq(struct irq_data *d)
 {
 	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
-	unsigned int gpio = irq_to_gpio(bank, d->irq);
+	unsigned int gpio = irq_to_gpio(bank, d->hwirq);
 
 	_clear_gpio_irqstatus(bank, gpio);
 }
@@ -775,7 +778,7 @@ static void gpio_ack_irq(struct irq_data *d)
 static void gpio_mask_irq(struct irq_data *d)
 {
 	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
-	unsigned int gpio = irq_to_gpio(bank, d->irq);
+	unsigned int gpio = irq_to_gpio(bank, d->hwirq);
 	unsigned long flags;
 
 	spin_lock_irqsave(&bank->lock, flags);
@@ -787,7 +790,7 @@ static void gpio_mask_irq(struct irq_data *d)
 static void gpio_unmask_irq(struct irq_data *d)
 {
 	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
-	unsigned int gpio = irq_to_gpio(bank, d->irq);
+	unsigned int gpio = irq_to_gpio(bank, d->hwirq);
 	unsigned int irq_mask = GPIO_BIT(bank, gpio);
 	u32 trigger = irqd_get_trigger_type(d);
 	unsigned long flags;
@@ -953,14 +956,6 @@ static void gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 	spin_unlock_irqrestore(&bank->lock, flags);
 }
 
-static int gpio_2irq(struct gpio_chip *chip, unsigned offset)
-{
-	struct gpio_bank *bank;
-
-	bank = container_of(chip, struct gpio_bank, chip);
-	return bank->irq_base + offset;
-}
-
 /*---------------------------------------------------------------------*/
 
 static void __init omap_gpio_show_rev(struct gpio_bank *bank)
@@ -1057,7 +1052,7 @@ static void omap_gpio_chip_init(struct gpio_bank *bank)
 	bank->chip.direction_output = gpio_output;
 	bank->chip.set_debounce = gpio_debounce;
 	bank->chip.set = gpio_set;
-	bank->chip.to_irq = gpio_2irq;
+	bank->chip.to_irq = omap_gpio_to_irq;
 	if (bank->is_mpuio) {
 		bank->chip.label = "mpuio";
 		if (bank->regs->wkup_en)
@@ -1072,15 +1067,16 @@ static void omap_gpio_chip_init(struct gpio_bank *bank)
 
 	gpiochip_add(&bank->chip);
 
-	for (j = bank->irq_base; j < bank->irq_base + bank->width; j++) {
-		irq_set_lockdep_class(j, &gpio_lock_class);
-		irq_set_chip_data(j, bank);
+	for (j = 0; j < bank->width; j++) {
+		int irq = irq_create_mapping(bank->domain, j);
+		irq_set_lockdep_class(irq, &gpio_lock_class);
+		irq_set_chip_data(irq, bank);
 		if (bank->is_mpuio) {
-			omap_mpuio_alloc_gc(bank, j, bank->width);
+			omap_mpuio_alloc_gc(bank, irq, bank->width);
 		} else {
-			irq_set_chip(j, &gpio_irq_chip);
-			irq_set_handler(j, handle_simple_irq);
-			set_irq_flags(j, IRQF_VALID);
+			irq_set_chip_and_handler(irq, &gpio_irq_chip,
+						 handle_simple_irq);
+			set_irq_flags(irq, IRQF_VALID);
 		}
 	}
 	irq_set_chained_handler(bank->irq, gpio_irq_handler);
@@ -1097,7 +1093,6 @@ static int omap_gpio_probe(struct platform_device *pdev)
 	const struct omap_gpio_platform_data *pdata;
 	struct resource *res;
 	struct gpio_bank *bank;
-	int ret = 0;
 
 	match = of_match_device(of_match_ptr(omap_gpio_match), dev);
 
@@ -1124,20 +1119,22 @@ static int omap_gpio_probe(struct platform_device *pdev)
 	bank->width = pdata->bank_width;
 	bank->is_mpuio = pdata->is_mpuio;
 	bank->non_wakeup_gpios = pdata->non_wakeup_gpios;
-	bank->loses_context = pdata->loses_context;
 	bank->regs = pdata->regs;
 #ifdef CONFIG_OF_GPIO
 	bank->chip.of_node = of_node_get(node);
 #endif
-
-	bank->irq_base = irq_alloc_descs(-1, 0, bank->width, 0);
-	if (bank->irq_base < 0) {
-		dev_err(dev, "Couldn't allocate IRQ numbers\n");
-		return -ENODEV;
+	if (node) {
+		if (!of_property_read_bool(node, "ti,gpio-always-on"))
+			bank->loses_context = true;
+	} else {
+		bank->loses_context = pdata->loses_context;
 	}
 
-	bank->domain = irq_domain_add_legacy(node, bank->width, bank->irq_base,
-					     0, &irq_domain_simple_ops, NULL);
+
+	bank->domain = irq_domain_add_linear(node, bank->width,
+					     &irq_domain_simple_ops, NULL);
+	if (!bank->domain)
+		return -ENODEV;
 
 	if (bank->regs->set_dataout && bank->regs->clr_dataout)
 		bank->set_dataout = _set_gpio_dataout_reg;
@@ -1150,18 +1147,21 @@ static int omap_gpio_probe(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (unlikely(!res)) {
 		dev_err(dev, "Invalid mem resource\n");
+		irq_domain_remove(bank->domain);
 		return -ENODEV;
 	}
 
 	if (!devm_request_mem_region(dev, res->start, resource_size(res),
 				     pdev->name)) {
 		dev_err(dev, "Region already claimed\n");
+		irq_domain_remove(bank->domain);
 		return -EBUSY;
 	}
 
 	bank->base = devm_ioremap(dev, res->start, resource_size(res));
 	if (!bank->base) {
 		dev_err(dev, "Could not ioremap\n");
+		irq_domain_remove(bank->domain);
 		return -ENOMEM;
 	}
 
@@ -1185,7 +1185,7 @@ static int omap_gpio_probe(struct platform_device *pdev)
 
 	list_add_tail(&bank->node, &omap_gpio_list);
 
-	return ret;
+	return 0;
 }
 
 #ifdef CONFIG_ARCH_OMAP2PLUS
@@ -1263,9 +1263,9 @@ static int omap_gpio_runtime_resume(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct gpio_bank *bank = platform_get_drvdata(pdev);
-	int context_lost_cnt_after;
 	u32 l = 0, gen, gen0, gen1;
 	unsigned long flags;
+	int c;
 
 	spin_lock_irqsave(&bank->lock, flags);
 	_gpio_dbck_enable(bank);
@@ -1281,14 +1281,17 @@ static int omap_gpio_runtime_resume(struct device *dev)
 	__raw_writel(bank->context.risingdetect,
 		     bank->base + bank->regs->risingdetect);
 
-	if (bank->get_context_loss_count) {
-		context_lost_cnt_after =
-			bank->get_context_loss_count(bank->dev);
-		if (context_lost_cnt_after != bank->context_loss_count) {
+	if (bank->loses_context) {
+		if (!bank->get_context_loss_count) {
 			omap_gpio_restore_context(bank);
 		} else {
-			spin_unlock_irqrestore(&bank->lock, flags);
-			return 0;
+			c = bank->get_context_loss_count(bank->dev);
+			if (c != bank->context_loss_count) {
+				omap_gpio_restore_context(bank);
+			} else {
+				spin_unlock_irqrestore(&bank->lock, flags);
+				return 0;
+			}
 		}
 	}
 
@@ -1297,10 +1300,6 @@ static int omap_gpio_runtime_resume(struct device *dev)
 		return 0;
 	}
 
-	__raw_writel(bank->context.fallingdetect,
-			bank->base + bank->regs->fallingdetect);
-	__raw_writel(bank->context.risingdetect,
-			bank->base + bank->regs->risingdetect);
 	l = __raw_readl(bank->base + bank->regs->datain);
 
 	/*
diff --git a/drivers/gpio/gpio-pca953x.c b/drivers/gpio/gpio-pca953x.c
index 9391cf16e990..426c51dd420c 100644
--- a/drivers/gpio/gpio-pca953x.c
+++ b/drivers/gpio/gpio-pca953x.c
@@ -146,8 +146,7 @@ static int pca953x_write_regs(struct pca953x_chip *chip, int reg, u8 *val)
 		ret = i2c_smbus_write_i2c_block_data(chip->client,
 					(reg << bank_shift) | REG_ADDR_AI,
 					NBANK(chip), val);
-	}
-	else {
+	} else {
 		switch (chip->chip_type) {
 		case PCA953X_TYPE:
 			ret = i2c_smbus_write_word_data(chip->client,
diff --git a/drivers/gpio/gpio-pcf857x.c b/drivers/gpio/gpio-pcf857x.c
index a19b7457a726..e8faf53f3875 100644
--- a/drivers/gpio/gpio-pcf857x.c
+++ b/drivers/gpio/gpio-pcf857x.c
@@ -45,6 +45,7 @@ static const struct i2c_device_id pcf857x_id[] = {
 	{ "pca9675", 16 },
 	{ "max7328", 8 },
 	{ "max7329", 8 },
+	{ "tca9554", 8 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, pcf857x_id);
@@ -267,7 +268,7 @@ static int pcf857x_probe(struct i2c_client *client,
 	}
 
 	/* Allocate, initialize, and register this gpio_chip. */
-	gpio = kzalloc(sizeof *gpio, GFP_KERNEL);
+	gpio = devm_kzalloc(&client->dev, sizeof(*gpio), GFP_KERNEL);
 	if (!gpio)
 		return -ENOMEM;
 
@@ -390,7 +391,6 @@ fail:
 	if (pdata && client->irq)
 		pcf857x_irq_domain_cleanup(gpio);
 
-	kfree(gpio);
 	return status;
 }
 
@@ -415,9 +415,7 @@ static int pcf857x_remove(struct i2c_client *client)
 		pcf857x_irq_domain_cleanup(gpio);
 
 	status = gpiochip_remove(&gpio->chip);
-	if (status == 0)
-		kfree(gpio);
-	else
+	if (status)
 		dev_err(&client->dev, "%s --> %d\n", "remove", status);
 	return status;
 }
diff --git a/drivers/gpio/gpio-pl061.c b/drivers/gpio/gpio-pl061.c
index d7008dfdd6f0..6a4bd0dae0ce 100644
--- a/drivers/gpio/gpio-pl061.c
+++ b/drivers/gpio/gpio-pl061.c
@@ -16,6 +16,7 @@
 #include <linux/ioport.h>
 #include <linux/irq.h>
 #include <linux/irqdomain.h>
+#include <linux/irqchip/chained_irq.h>
 #include <linux/bitops.h>
 #include <linux/workqueue.h>
 #include <linux/gpio.h>
@@ -25,7 +26,6 @@
 #include <linux/slab.h>
 #include <linux/pinctrl/consumer.h>
 #include <linux/pm.h>
-#include <asm/mach/irq.h>
 
 #define GPIODIR 0x400
 #define GPIOIS  0x404
diff --git a/drivers/gpio/gpio-pxa.c b/drivers/gpio/gpio-pxa.c
index 8325f580c0f1..df2199dd1499 100644
--- a/drivers/gpio/gpio-pxa.c
+++ b/drivers/gpio/gpio-pxa.c
@@ -19,6 +19,7 @@
 #include <linux/init.h>
 #include <linux/irq.h>
 #include <linux/irqdomain.h>
+#include <linux/irqchip/chained_irq.h>
 #include <linux/io.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
@@ -26,8 +27,6 @@
 #include <linux/syscore_ops.h>
 #include <linux/slab.h>
 
-#include <asm/mach/irq.h>
-
 #include <mach/irqs.h>
 
 /*
@@ -86,20 +85,61 @@ struct pxa_gpio_chip {
 #endif
 };
 
-enum {
+enum pxa_gpio_type {
 	PXA25X_GPIO = 0,
 	PXA26X_GPIO,
 	PXA27X_GPIO,
 	PXA3XX_GPIO,
 	PXA93X_GPIO,
 	MMP_GPIO = 0x10,
+	MMP2_GPIO,
+};
+
+struct pxa_gpio_id {
+	enum pxa_gpio_type	type;
+	int			gpio_nums;
 };
 
 static DEFINE_SPINLOCK(gpio_lock);
 static struct pxa_gpio_chip *pxa_gpio_chips;
-static int gpio_type;
+static enum pxa_gpio_type gpio_type;
 static void __iomem *gpio_reg_base;
 
+static struct pxa_gpio_id pxa25x_id = {
+	.type		= PXA25X_GPIO,
+	.gpio_nums	= 85,
+};
+
+static struct pxa_gpio_id pxa26x_id = {
+	.type		= PXA26X_GPIO,
+	.gpio_nums	= 90,
+};
+
+static struct pxa_gpio_id pxa27x_id = {
+	.type		= PXA27X_GPIO,
+	.gpio_nums	= 121,
+};
+
+static struct pxa_gpio_id pxa3xx_id = {
+	.type		= PXA3XX_GPIO,
+	.gpio_nums	= 128,
+};
+
+static struct pxa_gpio_id pxa93x_id = {
+	.type		= PXA93X_GPIO,
+	.gpio_nums	= 192,
+};
+
+static struct pxa_gpio_id mmp_id = {
+	.type		= MMP_GPIO,
+	.gpio_nums	= 128,
+};
+
+static struct pxa_gpio_id mmp2_id = {
+	.type		= MMP2_GPIO,
+	.gpio_nums	= 192,
+};
+
 #define for_each_gpio_chip(i, c)			\
 	for (i = 0, c = &pxa_gpio_chips[0]; i <= pxa_last_gpio; i += 32, c++)
 
@@ -432,47 +472,39 @@ static struct irq_chip pxa_muxed_gpio_chip = {
 	.irq_set_wake	= pxa_gpio_set_wake,
 };
 
-static int pxa_gpio_nums(void)
+static int pxa_gpio_nums(struct platform_device *pdev)
 {
+	const struct platform_device_id *id = platform_get_device_id(pdev);
+	struct pxa_gpio_id *pxa_id = (struct pxa_gpio_id *)id->driver_data;
 	int count = 0;
 
-#ifdef CONFIG_ARCH_PXA
-	if (cpu_is_pxa25x()) {
-#ifdef CONFIG_CPU_PXA26x
-		count = 89;
-		gpio_type = PXA26X_GPIO;
-#elif defined(CONFIG_PXA25x)
-		count = 84;
-		gpio_type = PXA26X_GPIO;
-#endif /* CONFIG_CPU_PXA26x */
-	} else if (cpu_is_pxa27x()) {
-		count = 120;
-		gpio_type = PXA27X_GPIO;
-	} else if (cpu_is_pxa93x()) {
-		count = 191;
-		gpio_type = PXA93X_GPIO;
-	} else if (cpu_is_pxa3xx()) {
-		count = 127;
-		gpio_type = PXA3XX_GPIO;
-	}
-#endif /* CONFIG_ARCH_PXA */
-
-#ifdef CONFIG_ARCH_MMP
-	if (cpu_is_pxa168() || cpu_is_pxa910()) {
-		count = 127;
-		gpio_type = MMP_GPIO;
-	} else if (cpu_is_mmp2()) {
-		count = 191;
-		gpio_type = MMP_GPIO;
+	switch (pxa_id->type) {
+	case PXA25X_GPIO:
+	case PXA26X_GPIO:
+	case PXA27X_GPIO:
+	case PXA3XX_GPIO:
+	case PXA93X_GPIO:
+	case MMP_GPIO:
+	case MMP2_GPIO:
+		gpio_type = pxa_id->type;
+		count = pxa_id->gpio_nums - 1;
+		break;
+	default:
+		count = -EINVAL;
+		break;
 	}
-#endif /* CONFIG_ARCH_MMP */
 	return count;
 }
 
 #ifdef CONFIG_OF
 static struct of_device_id pxa_gpio_dt_ids[] = {
-	{ .compatible = "mrvl,pxa-gpio" },
-	{ .compatible = "mrvl,mmp-gpio", .data = (void *)MMP_GPIO },
+	{ .compatible = "intel,pxa25x-gpio",	.data = &pxa25x_id, },
+	{ .compatible = "intel,pxa26x-gpio",	.data = &pxa26x_id, },
+	{ .compatible = "intel,pxa27x-gpio",	.data = &pxa27x_id, },
+	{ .compatible = "intel,pxa3xx-gpio",	.data = &pxa3xx_id, },
+	{ .compatible = "marvell,pxa93x-gpio",	.data = &pxa93x_id, },
+	{ .compatible = "marvell,mmp-gpio",	.data = &mmp_id, },
+	{ .compatible = "marvell,mmp2-gpio",	.data = &mmp2_id, },
 	{}
 };
 
@@ -492,16 +524,18 @@ const struct irq_domain_ops pxa_irq_domain_ops = {
 
 static int pxa_gpio_probe_dt(struct platform_device *pdev)
 {
-	int ret, nr_banks, nr_gpios;
+	int ret, nr_gpios;
 	struct device_node *prev, *next, *np = pdev->dev.of_node;
 	const struct of_device_id *of_id =
 				of_match_device(pxa_gpio_dt_ids, &pdev->dev);
+	const struct pxa_gpio_id *gpio_id;
 
-	if (!of_id) {
+	if (!of_id || !of_id->data) {
 		dev_err(&pdev->dev, "Failed to find gpio controller\n");
 		return -EFAULT;
 	}
-	gpio_type = (int)of_id->data;
+	gpio_id = of_id->data;
+	gpio_type = gpio_id->type;
 
 	next = of_get_next_child(np, NULL);
 	prev = next;
@@ -510,14 +544,8 @@ static int pxa_gpio_probe_dt(struct platform_device *pdev)
 		ret = -EINVAL;
 		goto err;
 	}
-	for (nr_banks = 1; ; nr_banks++) {
-		next = of_get_next_child(np, prev);
-		if (!next)
-			break;
-		prev = next;
-	}
 	of_node_put(prev);
-	nr_gpios = nr_banks << 5;
+	nr_gpios = gpio_id->gpio_nums;
 	pxa_last_gpio = nr_gpios - 1;
 
 	irq_base = irq_alloc_descs(-1, 0, nr_gpios, 0);
@@ -546,19 +574,18 @@ static int pxa_gpio_probe(struct platform_device *pdev)
 	int gpio, irq, ret, use_of = 0;
 	int irq0 = 0, irq1 = 0, irq_mux, gpio_offset = 0;
 
-	ret = pxa_gpio_probe_dt(pdev);
-	if (ret < 0) {
-		pxa_last_gpio = pxa_gpio_nums();
-#ifdef CONFIG_ARCH_PXA
-		if (gpio_is_pxa_type(gpio_type))
-			irq_base = PXA_GPIO_TO_IRQ(0);
-#endif
-#ifdef CONFIG_ARCH_MMP
-		if (gpio_is_mmp_type(gpio_type))
-			irq_base = MMP_GPIO_TO_IRQ(0);
-#endif
+	info = dev_get_platdata(&pdev->dev);
+	if (info) {
+		irq_base = info->irq_base;
+		if (irq_base <= 0)
+			return -EINVAL;
+		pxa_last_gpio = pxa_gpio_nums(pdev);
 	} else {
+		irq_base = 0;
 		use_of = 1;
+		ret = pxa_gpio_probe_dt(pdev);
+		if (ret < 0)
+			return -EINVAL;
 	}
 
 	if (!pxa_last_gpio)
@@ -595,14 +622,13 @@ static int pxa_gpio_probe(struct platform_device *pdev)
 	}
 
 	/* Initialize GPIO chips */
-	info = dev_get_platdata(&pdev->dev);
 	pxa_init_gpio_chip(pxa_last_gpio, info ? info->gpio_set_wake : NULL);
 
 	/* clear all GPIO edge detects */
 	for_each_gpio_chip(gpio, c) {
 		writel_relaxed(0, c->regbase + GFER_OFFSET);
 		writel_relaxed(0, c->regbase + GRER_OFFSET);
-		writel_relaxed(~0,c->regbase + GEDR_OFFSET);
+		writel_relaxed(~0, c->regbase + GEDR_OFFSET);
 		/* unmask GPIO edge detect for AP side */
 		if (gpio_is_mmp_type(gpio_type))
 			writel_relaxed(~0, c->regbase + ED_MASK_OFFSET);
@@ -635,12 +661,24 @@ static int pxa_gpio_probe(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct platform_device_id gpio_id_table[] = {
+	{ "pxa25x-gpio",	(unsigned long)&pxa25x_id },
+	{ "pxa26x-gpio",	(unsigned long)&pxa26x_id },
+	{ "pxa27x-gpio",	(unsigned long)&pxa27x_id },
+	{ "pxa3xx-gpio",	(unsigned long)&pxa3xx_id },
+	{ "pxa93x-gpio",	(unsigned long)&pxa93x_id },
+	{ "mmp-gpio",		(unsigned long)&mmp_id },
+	{ "mmp2-gpio",		(unsigned long)&mmp2_id },
+	{ },
+};
+
 static struct platform_driver pxa_gpio_driver = {
 	.probe		= pxa_gpio_probe,
 	.driver		= {
 		.name	= "pxa-gpio",
 		.of_match_table = of_match_ptr(pxa_gpio_dt_ids),
 	},
+	.id_table	= gpio_id_table,
 };
 
 static int __init pxa_gpio_init(void)
@@ -674,7 +712,7 @@ static void pxa_gpio_resume(void)
 
 	for_each_gpio_chip(gpio, c) {
 		/* restore level with set/clear */
-		writel_relaxed( c->saved_gplr, c->regbase + GPSR_OFFSET);
+		writel_relaxed(c->saved_gplr, c->regbase + GPSR_OFFSET);
 		writel_relaxed(~c->saved_gplr, c->regbase + GPCR_OFFSET);
 
 		writel_relaxed(c->saved_grer, c->regbase + GRER_OFFSET);
diff --git a/drivers/gpio/gpio-rcar.c b/drivers/gpio/gpio-rcar.c
new file mode 100644
index 000000000000..b4ca450947b8
--- /dev/null
+++ b/drivers/gpio/gpio-rcar.c
@@ -0,0 +1,396 @@
+/*
+ * Renesas R-Car GPIO Support
+ *
+ *  Copyright (C) 2013 Magnus Damm
+ *
+ * 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.
+ */
+
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/module.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_data/gpio-rcar.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+
+struct gpio_rcar_priv {
+	void __iomem *base;
+	spinlock_t lock;
+	struct gpio_rcar_config config;
+	struct platform_device *pdev;
+	struct gpio_chip gpio_chip;
+	struct irq_chip irq_chip;
+	struct irq_domain *irq_domain;
+};
+
+#define IOINTSEL 0x00
+#define INOUTSEL 0x04
+#define OUTDT 0x08
+#define INDT 0x0c
+#define INTDT 0x10
+#define INTCLR 0x14
+#define INTMSK 0x18
+#define MSKCLR 0x1c
+#define POSNEG 0x20
+#define EDGLEVEL 0x24
+#define FILONOFF 0x28
+
+static inline u32 gpio_rcar_read(struct gpio_rcar_priv *p, int offs)
+{
+	return ioread32(p->base + offs);
+}
+
+static inline void gpio_rcar_write(struct gpio_rcar_priv *p, int offs,
+				   u32 value)
+{
+	iowrite32(value, p->base + offs);
+}
+
+static void gpio_rcar_modify_bit(struct gpio_rcar_priv *p, int offs,
+				 int bit, bool value)
+{
+	u32 tmp = gpio_rcar_read(p, offs);
+
+	if (value)
+		tmp |= BIT(bit);
+	else
+		tmp &= ~BIT(bit);
+
+	gpio_rcar_write(p, offs, tmp);
+}
+
+static void gpio_rcar_irq_disable(struct irq_data *d)
+{
+	struct gpio_rcar_priv *p = irq_data_get_irq_chip_data(d);
+
+	gpio_rcar_write(p, INTMSK, ~BIT(irqd_to_hwirq(d)));
+}
+
+static void gpio_rcar_irq_enable(struct irq_data *d)
+{
+	struct gpio_rcar_priv *p = irq_data_get_irq_chip_data(d);
+
+	gpio_rcar_write(p, MSKCLR, BIT(irqd_to_hwirq(d)));
+}
+
+static void gpio_rcar_config_interrupt_input_mode(struct gpio_rcar_priv *p,
+						  unsigned int hwirq,
+						  bool active_high_rising_edge,
+						  bool level_trigger)
+{
+	unsigned long flags;
+
+	/* follow steps in the GPIO documentation for
+	 * "Setting Edge-Sensitive Interrupt Input Mode" and
+	 * "Setting Level-Sensitive Interrupt Input Mode"
+	 */
+
+	spin_lock_irqsave(&p->lock, flags);
+
+	/* Configure postive or negative logic in POSNEG */
+	gpio_rcar_modify_bit(p, POSNEG, hwirq, !active_high_rising_edge);
+
+	/* Configure edge or level trigger in EDGLEVEL */
+	gpio_rcar_modify_bit(p, EDGLEVEL, hwirq, !level_trigger);
+
+	/* Select "Interrupt Input Mode" in IOINTSEL */
+	gpio_rcar_modify_bit(p, IOINTSEL, hwirq, true);
+
+	/* Write INTCLR in case of edge trigger */
+	if (!level_trigger)
+		gpio_rcar_write(p, INTCLR, BIT(hwirq));
+
+	spin_unlock_irqrestore(&p->lock, flags);
+}
+
+static int gpio_rcar_irq_set_type(struct irq_data *d, unsigned int type)
+{
+	struct gpio_rcar_priv *p = irq_data_get_irq_chip_data(d);
+	unsigned int hwirq = irqd_to_hwirq(d);
+
+	dev_dbg(&p->pdev->dev, "sense irq = %d, type = %d\n", hwirq, type);
+
+	switch (type & IRQ_TYPE_SENSE_MASK) {
+	case IRQ_TYPE_LEVEL_HIGH:
+		gpio_rcar_config_interrupt_input_mode(p, hwirq, true, true);
+		break;
+	case IRQ_TYPE_LEVEL_LOW:
+		gpio_rcar_config_interrupt_input_mode(p, hwirq, false, true);
+		break;
+	case IRQ_TYPE_EDGE_RISING:
+		gpio_rcar_config_interrupt_input_mode(p, hwirq, true, false);
+		break;
+	case IRQ_TYPE_EDGE_FALLING:
+		gpio_rcar_config_interrupt_input_mode(p, hwirq, false, false);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static irqreturn_t gpio_rcar_irq_handler(int irq, void *dev_id)
+{
+	struct gpio_rcar_priv *p = dev_id;
+	u32 pending;
+	unsigned int offset, irqs_handled = 0;
+
+	while ((pending = gpio_rcar_read(p, INTDT))) {
+		offset = __ffs(pending);
+		gpio_rcar_write(p, INTCLR, BIT(offset));
+		generic_handle_irq(irq_find_mapping(p->irq_domain, offset));
+		irqs_handled++;
+	}
+
+	return irqs_handled ? IRQ_HANDLED : IRQ_NONE;
+}
+
+static inline struct gpio_rcar_priv *gpio_to_priv(struct gpio_chip *chip)
+{
+	return container_of(chip, struct gpio_rcar_priv, gpio_chip);
+}
+
+static void gpio_rcar_config_general_input_output_mode(struct gpio_chip *chip,
+						       unsigned int gpio,
+						       bool output)
+{
+	struct gpio_rcar_priv *p = gpio_to_priv(chip);
+	unsigned long flags;
+
+	/* follow steps in the GPIO documentation for
+	 * "Setting General Output Mode" and
+	 * "Setting General Input Mode"
+	 */
+
+	spin_lock_irqsave(&p->lock, flags);
+
+	/* Configure postive logic in POSNEG */
+	gpio_rcar_modify_bit(p, POSNEG, gpio, false);
+
+	/* Select "General Input/Output Mode" in IOINTSEL */
+	gpio_rcar_modify_bit(p, IOINTSEL, gpio, false);
+
+	/* Select Input Mode or Output Mode in INOUTSEL */
+	gpio_rcar_modify_bit(p, INOUTSEL, gpio, output);
+
+	spin_unlock_irqrestore(&p->lock, flags);
+}
+
+static int gpio_rcar_request(struct gpio_chip *chip, unsigned offset)
+{
+	return pinctrl_request_gpio(chip->base + offset);
+}
+
+static void gpio_rcar_free(struct gpio_chip *chip, unsigned offset)
+{
+	pinctrl_free_gpio(chip->base + offset);
+
+	/* Set the GPIO as an input to ensure that the next GPIO request won't
+	 * drive the GPIO pin as an output.
+	 */
+	gpio_rcar_config_general_input_output_mode(chip, offset, false);
+}
+
+static int gpio_rcar_direction_input(struct gpio_chip *chip, unsigned offset)
+{
+	gpio_rcar_config_general_input_output_mode(chip, offset, false);
+	return 0;
+}
+
+static int gpio_rcar_get(struct gpio_chip *chip, unsigned offset)
+{
+	return (int)(gpio_rcar_read(gpio_to_priv(chip), INDT) & BIT(offset));
+}
+
+static void gpio_rcar_set(struct gpio_chip *chip, unsigned offset, int value)
+{
+	struct gpio_rcar_priv *p = gpio_to_priv(chip);
+	unsigned long flags;
+
+	spin_lock_irqsave(&p->lock, flags);
+	gpio_rcar_modify_bit(p, OUTDT, offset, value);
+	spin_unlock_irqrestore(&p->lock, flags);
+}
+
+static int gpio_rcar_direction_output(struct gpio_chip *chip, unsigned offset,
+				      int value)
+{
+	/* write GPIO value to output before selecting output mode of pin */
+	gpio_rcar_set(chip, offset, value);
+	gpio_rcar_config_general_input_output_mode(chip, offset, true);
+	return 0;
+}
+
+static int gpio_rcar_to_irq(struct gpio_chip *chip, unsigned offset)
+{
+	return irq_create_mapping(gpio_to_priv(chip)->irq_domain, offset);
+}
+
+static int gpio_rcar_irq_domain_map(struct irq_domain *h, unsigned int virq,
+				 irq_hw_number_t hw)
+{
+	struct gpio_rcar_priv *p = h->host_data;
+
+	dev_dbg(&p->pdev->dev, "map hw irq = %d, virq = %d\n", (int)hw, virq);
+
+	irq_set_chip_data(virq, h->host_data);
+	irq_set_chip_and_handler(virq, &p->irq_chip, handle_level_irq);
+	set_irq_flags(virq, IRQF_VALID); /* kill me now */
+	return 0;
+}
+
+static struct irq_domain_ops gpio_rcar_irq_domain_ops = {
+	.map	= gpio_rcar_irq_domain_map,
+};
+
+static int gpio_rcar_probe(struct platform_device *pdev)
+{
+	struct gpio_rcar_config *pdata = pdev->dev.platform_data;
+	struct gpio_rcar_priv *p;
+	struct resource *io, *irq;
+	struct gpio_chip *gpio_chip;
+	struct irq_chip *irq_chip;
+	const char *name = dev_name(&pdev->dev);
+	int ret;
+
+	p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
+	if (!p) {
+		dev_err(&pdev->dev, "failed to allocate driver data\n");
+		ret = -ENOMEM;
+		goto err0;
+	}
+
+	/* deal with driver instance configuration */
+	if (pdata)
+		p->config = *pdata;
+
+	p->pdev = pdev;
+	platform_set_drvdata(pdev, p);
+	spin_lock_init(&p->lock);
+
+	io = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+
+	if (!io || !irq) {
+		dev_err(&pdev->dev, "missing IRQ or IOMEM\n");
+		ret = -EINVAL;
+		goto err0;
+	}
+
+	p->base = devm_ioremap_nocache(&pdev->dev, io->start,
+				       resource_size(io));
+	if (!p->base) {
+		dev_err(&pdev->dev, "failed to remap I/O memory\n");
+		ret = -ENXIO;
+		goto err0;
+	}
+
+	gpio_chip = &p->gpio_chip;
+	gpio_chip->request = gpio_rcar_request;
+	gpio_chip->free = gpio_rcar_free;
+	gpio_chip->direction_input = gpio_rcar_direction_input;
+	gpio_chip->get = gpio_rcar_get;
+	gpio_chip->direction_output = gpio_rcar_direction_output;
+	gpio_chip->set = gpio_rcar_set;
+	gpio_chip->to_irq = gpio_rcar_to_irq;
+	gpio_chip->label = name;
+	gpio_chip->owner = THIS_MODULE;
+	gpio_chip->base = p->config.gpio_base;
+	gpio_chip->ngpio = p->config.number_of_pins;
+
+	irq_chip = &p->irq_chip;
+	irq_chip->name = name;
+	irq_chip->irq_mask = gpio_rcar_irq_disable;
+	irq_chip->irq_unmask = gpio_rcar_irq_enable;
+	irq_chip->irq_enable = gpio_rcar_irq_enable;
+	irq_chip->irq_disable = gpio_rcar_irq_disable;
+	irq_chip->irq_set_type = gpio_rcar_irq_set_type;
+	irq_chip->flags	= IRQCHIP_SKIP_SET_WAKE | IRQCHIP_SET_TYPE_MASKED;
+
+	p->irq_domain = irq_domain_add_simple(pdev->dev.of_node,
+					      p->config.number_of_pins,
+					      p->config.irq_base,
+					      &gpio_rcar_irq_domain_ops, p);
+	if (!p->irq_domain) {
+		ret = -ENXIO;
+		dev_err(&pdev->dev, "cannot initialize irq domain\n");
+		goto err1;
+	}
+
+	if (devm_request_irq(&pdev->dev, irq->start,
+			     gpio_rcar_irq_handler, 0, name, p)) {
+		dev_err(&pdev->dev, "failed to request IRQ\n");
+		ret = -ENOENT;
+		goto err1;
+	}
+
+	ret = gpiochip_add(gpio_chip);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to add GPIO controller\n");
+		goto err1;
+	}
+
+	dev_info(&pdev->dev, "driving %d GPIOs\n", p->config.number_of_pins);
+
+	/* warn in case of mismatch if irq base is specified */
+	if (p->config.irq_base) {
+		ret = irq_find_mapping(p->irq_domain, 0);
+		if (p->config.irq_base != ret)
+			dev_warn(&pdev->dev, "irq base mismatch (%u/%u)\n",
+				 p->config.irq_base, ret);
+	}
+
+	ret = gpiochip_add_pin_range(gpio_chip, p->config.pctl_name, 0,
+				     gpio_chip->base, gpio_chip->ngpio);
+	if (ret < 0)
+		dev_warn(&pdev->dev, "failed to add pin range\n");
+
+	return 0;
+
+err1:
+	irq_domain_remove(p->irq_domain);
+err0:
+	return ret;
+}
+
+static int gpio_rcar_remove(struct platform_device *pdev)
+{
+	struct gpio_rcar_priv *p = platform_get_drvdata(pdev);
+	int ret;
+
+	ret = gpiochip_remove(&p->gpio_chip);
+	if (ret)
+		return ret;
+
+	irq_domain_remove(p->irq_domain);
+	return 0;
+}
+
+static struct platform_driver gpio_rcar_device_driver = {
+	.probe		= gpio_rcar_probe,
+	.remove		= gpio_rcar_remove,
+	.driver		= {
+		.name	= "gpio_rcar",
+	}
+};
+
+module_platform_driver(gpio_rcar_device_driver);
+
+MODULE_AUTHOR("Magnus Damm");
+MODULE_DESCRIPTION("Renesas R-Car GPIO Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpio/gpio-samsung.c b/drivers/gpio/gpio-samsung.c
index b3643ff007e4..b22ca7933745 100644
--- a/drivers/gpio/gpio-samsung.c
+++ b/drivers/gpio/gpio-samsung.c
@@ -1122,8 +1122,12 @@ int samsung_gpiolib_to_irq(struct gpio_chip *chip, unsigned int offset)
 #ifdef CONFIG_PLAT_S3C24XX
 static int s3c24xx_gpiolib_fbank_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	if (offset < 4)
-		return IRQ_EINT0 + offset;
+	if (offset < 4) {
+		if (soc_is_s3c2412())
+			return IRQ_EINT0_2412 + offset;
+		else
+			return IRQ_EINT0 + offset;
+	}
 
 	if (offset < 8)
 		return IRQ_EINT4 + offset - 4;
@@ -3024,7 +3028,9 @@ static __init int samsung_gpiolib_init(void)
 	static const struct of_device_id exynos_pinctrl_ids[] = {
 		{ .compatible = "samsung,exynos4210-pinctrl", },
 		{ .compatible = "samsung,exynos4x12-pinctrl", },
+		{ .compatible = "samsung,exynos5250-pinctrl", },
 		{ .compatible = "samsung,exynos5440-pinctrl", },
+		{ }
 	};
 	for_each_matching_node(pctrl_np, exynos_pinctrl_ids)
 		if (pctrl_np && of_device_is_available(pctrl_np))
diff --git a/drivers/gpio/gpio-sch.c b/drivers/gpio/gpio-sch.c
index edae963f4625..1e4de16ceb41 100644
--- a/drivers/gpio/gpio-sch.c
+++ b/drivers/gpio/gpio-sch.c
@@ -125,13 +125,17 @@ static int sch_gpio_resume_direction_in(struct gpio_chip *gc,
 					unsigned gpio_num)
 {
 	u8 curr_dirs;
+	unsigned short offset, bit;
 
 	spin_lock(&gpio_lock);
 
-	curr_dirs = inb(gpio_ba + RGIO);
+	offset = RGIO + gpio_num / 8;
+	bit = gpio_num % 8;
+
+	curr_dirs = inb(gpio_ba + offset);
 
-	if (!(curr_dirs & (1 << gpio_num)))
-		outb(curr_dirs | (1 << gpio_num) , gpio_ba + RGIO);
+	if (!(curr_dirs & (1 << bit)))
+		outb(curr_dirs | (1 << bit), gpio_ba + offset);
 
 	spin_unlock(&gpio_lock);
 	return 0;
@@ -139,22 +143,31 @@ static int sch_gpio_resume_direction_in(struct gpio_chip *gc,
 
 static int sch_gpio_resume_get(struct gpio_chip *gc, unsigned gpio_num)
 {
-	return !!(inb(gpio_ba + RGLV) & (1 << gpio_num));
+	unsigned short offset, bit;
+
+	offset = RGLV + gpio_num / 8;
+	bit = gpio_num % 8;
+
+	return !!(inb(gpio_ba + offset) & (1 << bit));
 }
 
 static void sch_gpio_resume_set(struct gpio_chip *gc,
 				unsigned gpio_num, int val)
 {
 	u8 curr_vals;
+	unsigned short offset, bit;
 
 	spin_lock(&gpio_lock);
 
-	curr_vals = inb(gpio_ba + RGLV);
+	offset = RGLV + gpio_num / 8;
+	bit = gpio_num % 8;
+
+	curr_vals = inb(gpio_ba + offset);
 
 	if (val)
-		outb(curr_vals | (1 << gpio_num), gpio_ba + RGLV);
+		outb(curr_vals | (1 << bit), gpio_ba + offset);
 	else
-		outb((curr_vals & ~(1 << gpio_num)), gpio_ba + RGLV);
+		outb((curr_vals & ~(1 << bit)), gpio_ba + offset);
 
 	spin_unlock(&gpio_lock);
 }
@@ -163,14 +176,18 @@ static int sch_gpio_resume_direction_out(struct gpio_chip *gc,
 					unsigned gpio_num, int val)
 {
 	u8 curr_dirs;
+	unsigned short offset, bit;
 
 	sch_gpio_resume_set(gc, gpio_num, val);
 
+	offset = RGIO + gpio_num / 8;
+	bit = gpio_num % 8;
+
 	spin_lock(&gpio_lock);
 
-	curr_dirs = inb(gpio_ba + RGIO);
-	if (curr_dirs & (1 << gpio_num))
-		outb(curr_dirs & ~(1 << gpio_num), gpio_ba + RGIO);
+	curr_dirs = inb(gpio_ba + offset);
+	if (curr_dirs & (1 << bit))
+		outb(curr_dirs & ~(1 << bit), gpio_ba + offset);
 
 	spin_unlock(&gpio_lock);
 	return 0;
@@ -204,45 +221,41 @@ static int sch_gpio_probe(struct platform_device *pdev)
 	gpio_ba = res->start;
 
 	switch (id) {
-		case PCI_DEVICE_ID_INTEL_SCH_LPC:
-			sch_gpio_core.base = 0;
-			sch_gpio_core.ngpio = 10;
-
-			sch_gpio_resume.base = 10;
-			sch_gpio_resume.ngpio = 4;
-
-			/*
-			 * GPIO[6:0] enabled by default
-			 * GPIO7 is configured by the CMC as SLPIOVR
-			 * Enable GPIO[9:8] core powered gpios explicitly
-			 */
-			outb(0x3, gpio_ba + CGEN + 1);
-			/*
-			 * SUS_GPIO[2:0] enabled by default
-			 * Enable SUS_GPIO3 resume powered gpio explicitly
-			 */
-			outb(0x8, gpio_ba + RGEN);
-			break;
-
-		case PCI_DEVICE_ID_INTEL_ITC_LPC:
-			sch_gpio_core.base = 0;
-			sch_gpio_core.ngpio = 5;
-
-			sch_gpio_resume.base = 5;
-			sch_gpio_resume.ngpio = 9;
-			break;
-
-		case PCI_DEVICE_ID_INTEL_CENTERTON_ILB:
-			sch_gpio_core.base = 0;
-			sch_gpio_core.ngpio = 21;
-
-			sch_gpio_resume.base = 21;
-			sch_gpio_resume.ngpio = 9;
-			break;
-
-		default:
-			err = -ENODEV;
-			goto err_sch_gpio_core;
+	case PCI_DEVICE_ID_INTEL_SCH_LPC:
+		sch_gpio_core.base = 0;
+		sch_gpio_core.ngpio = 10;
+		sch_gpio_resume.base = 10;
+		sch_gpio_resume.ngpio = 4;
+		/*
+		 * GPIO[6:0] enabled by default
+		 * GPIO7 is configured by the CMC as SLPIOVR
+		 * Enable GPIO[9:8] core powered gpios explicitly
+		 */
+		outb(0x3, gpio_ba + CGEN + 1);
+		/*
+		 * SUS_GPIO[2:0] enabled by default
+		 * Enable SUS_GPIO3 resume powered gpio explicitly
+		 */
+		outb(0x8, gpio_ba + RGEN);
+		break;
+
+	case PCI_DEVICE_ID_INTEL_ITC_LPC:
+		sch_gpio_core.base = 0;
+		sch_gpio_core.ngpio = 5;
+		sch_gpio_resume.base = 5;
+		sch_gpio_resume.ngpio = 9;
+		break;
+
+	case PCI_DEVICE_ID_INTEL_CENTERTON_ILB:
+		sch_gpio_core.base = 0;
+		sch_gpio_core.ngpio = 21;
+		sch_gpio_resume.base = 21;
+		sch_gpio_resume.ngpio = 9;
+		break;
+
+	default:
+		err = -ENODEV;
+		goto err_sch_gpio_core;
 	}
 
 	sch_gpio_core.dev = &pdev->dev;
diff --git a/drivers/gpio/gpio-stp-xway.c b/drivers/gpio/gpio-stp-xway.c
index c20e05151212..04882a911b65 100644
--- a/drivers/gpio/gpio-stp-xway.c
+++ b/drivers/gpio/gpio-stp-xway.c
@@ -217,7 +217,7 @@ static int xway_stp_probe(struct platform_device *pdev)
 	chip->virt = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(chip->virt))
 		return PTR_ERR(chip->virt);
-	
+
 	chip->gc.dev = &pdev->dev;
 	chip->gc.label = "stp-xway";
 	chip->gc.direction_output = xway_stp_dir_out;
diff --git a/drivers/gpio/gpio-tc3589x.c b/drivers/gpio/gpio-tc3589x.c
index c0595bbf3268..d34d80dfb083 100644
--- a/drivers/gpio/gpio-tc3589x.c
+++ b/drivers/gpio/gpio-tc3589x.c
@@ -282,9 +282,9 @@ static void tc3589x_gpio_irq_unmap(struct irq_domain *d, unsigned int virq)
 }
 
 static struct irq_domain_ops tc3589x_irq_ops = {
-        .map    = tc3589x_gpio_irq_map,
-        .unmap  = tc3589x_gpio_irq_unmap,
-        .xlate  = irq_domain_xlate_twocell,
+	.map    = tc3589x_gpio_irq_map,
+	.unmap  = tc3589x_gpio_irq_unmap,
+	.xlate  = irq_domain_xlate_twocell,
 };
 
 static int tc3589x_gpio_irq_init(struct tc3589x_gpio *tc3589x_gpio,
@@ -344,7 +344,7 @@ static int tc3589x_gpio_probe(struct platform_device *pdev)
 	tc3589x_gpio->chip.base = (pdata) ? pdata->gpio_base : -1;
 
 #ifdef CONFIG_OF_GPIO
-        tc3589x_gpio->chip.of_node = np;
+	tc3589x_gpio->chip.of_node = np;
 #endif
 
 	tc3589x_gpio->irq_base = tc3589x->irq_base ?
diff --git a/drivers/gpio/gpio-tegra.c b/drivers/gpio/gpio-tegra.c
index 414ad912232f..da4cb5b0cb87 100644
--- a/drivers/gpio/gpio-tegra.c
+++ b/drivers/gpio/gpio-tegra.c
@@ -27,11 +27,10 @@
 #include <linux/platform_device.h>
 #include <linux/module.h>
 #include <linux/irqdomain.h>
+#include <linux/irqchip/chained_irq.h>
 #include <linux/pinctrl/consumer.h>
 #include <linux/pm.h>
 
-#include <asm/mach/irq.h>
-
 #define GPIO_BANK(x)		((x) >> 5)
 #define GPIO_PORT(x)		(((x) >> 3) & 0x3)
 #define GPIO_BIT(x)		((x) & 0x7)
@@ -72,6 +71,7 @@ struct tegra_gpio_bank {
 	u32 oe[4];
 	u32 int_enb[4];
 	u32 int_lvl[4];
+	u32 wake_enb[4];
 #endif
 };
 
@@ -333,15 +333,31 @@ static int tegra_gpio_suspend(struct device *dev)
 			bank->oe[p] = tegra_gpio_readl(GPIO_OE(gpio));
 			bank->int_enb[p] = tegra_gpio_readl(GPIO_INT_ENB(gpio));
 			bank->int_lvl[p] = tegra_gpio_readl(GPIO_INT_LVL(gpio));
+
+			/* Enable gpio irq for wake up source */
+			tegra_gpio_writel(bank->wake_enb[p],
+					  GPIO_INT_ENB(gpio));
 		}
 	}
 	local_irq_restore(flags);
 	return 0;
 }
 
-static int tegra_gpio_wake_enable(struct irq_data *d, unsigned int enable)
+static int tegra_gpio_irq_set_wake(struct irq_data *d, unsigned int enable)
 {
 	struct tegra_gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	int gpio = d->hwirq;
+	u32 port, bit, mask;
+
+	port = GPIO_PORT(gpio);
+	bit = GPIO_BIT(gpio);
+	mask = BIT(bit);
+
+	if (enable)
+		bank->wake_enb[port] |= mask;
+	else
+		bank->wake_enb[port] &= ~mask;
+
 	return irq_set_irq_wake(bank->irq, enable);
 }
 #endif
@@ -353,7 +369,7 @@ static struct irq_chip tegra_gpio_irq_chip = {
 	.irq_unmask	= tegra_gpio_irq_unmask,
 	.irq_set_type	= tegra_gpio_irq_set_type,
 #ifdef CONFIG_PM_SLEEP
-	.irq_set_wake	= tegra_gpio_wake_enable,
+	.irq_set_wake	= tegra_gpio_irq_set_wake,
 #endif
 };
 
@@ -398,10 +414,11 @@ static int tegra_gpio_probe(struct platform_device *pdev)
 	int j;
 
 	match = of_match_device(tegra_gpio_of_match, &pdev->dev);
-	if (match)
-		config = (struct tegra_gpio_soc_config *)match->data;
-	else
-		config = &tegra20_gpio_config;
+	if (!match) {
+		dev_err(&pdev->dev, "Error: No device match found\n");
+		return -ENODEV;
+	}
+	config = (struct tegra_gpio_soc_config *)match->data;
 
 	tegra_gpio_bank_stride = config->bank_stride;
 	tegra_gpio_upper_offset = config->upper_offset;
@@ -462,9 +479,7 @@ static int tegra_gpio_probe(struct platform_device *pdev)
 		}
 	}
 
-#ifdef CONFIG_OF_GPIO
 	tegra_gpio_chip.of_node = pdev->dev.of_node;
-#endif
 
 	gpiochip_add(&tegra_gpio_chip);
 
diff --git a/drivers/gpio/gpio-timberdale.c b/drivers/gpio/gpio-timberdale.c
index 702cca9284f1..43774058b693 100644
--- a/drivers/gpio/gpio-timberdale.c
+++ b/drivers/gpio/gpio-timberdale.c
@@ -167,8 +167,7 @@ static int timbgpio_irq_type(struct irq_data *d, unsigned trigger)
 		if (ver < 3) {
 			ret = -EINVAL;
 			goto out;
-		}
-		else {
+		} else {
 			flr |= 1 << offset;
 			bflr |= 1 << offset;
 		}
diff --git a/drivers/gpio/gpio-tps65910.c b/drivers/gpio/gpio-tps65910.c
index 5083825a0348..06146219d9d2 100644
--- a/drivers/gpio/gpio-tps65910.c
+++ b/drivers/gpio/gpio-tps65910.c
@@ -133,7 +133,7 @@ static int tps65910_gpio_probe(struct platform_device *pdev)
 	tps65910_gpio->gpio_chip.owner = THIS_MODULE;
 	tps65910_gpio->gpio_chip.label = tps65910->i2c_client->name;
 
-	switch(tps65910_chip_id(tps65910)) {
+	switch (tps65910_chip_id(tps65910)) {
 	case TPS65910:
 		tps65910_gpio->gpio_chip.ngpio = TPS65910_NUM_GPIO;
 		break;
diff --git a/drivers/gpio/gpio-ucb1400.c b/drivers/gpio/gpio-ucb1400.c
index 26405efe0f9f..6d0feb234d3c 100644
--- a/drivers/gpio/gpio-ucb1400.c
+++ b/drivers/gpio/gpio-ucb1400.c
@@ -12,8 +12,6 @@
 #include <linux/module.h>
 #include <linux/ucb1400.h>
 
-struct ucb1400_gpio_data *ucbdata;
-
 static int ucb1400_gpio_dir_in(struct gpio_chip *gc, unsigned off)
 {
 	struct ucb1400_gpio *gpio;
@@ -50,7 +48,7 @@ static int ucb1400_gpio_probe(struct platform_device *dev)
 	struct ucb1400_gpio *ucb = dev->dev.platform_data;
 	int err = 0;
 
-	if (!(ucbdata && ucbdata->gpio_offset)) {
+	if (!(ucb && ucb->gpio_offset)) {
 		err = -EINVAL;
 		goto err;
 	}
@@ -58,7 +56,7 @@ static int ucb1400_gpio_probe(struct platform_device *dev)
 	platform_set_drvdata(dev, ucb);
 
 	ucb->gc.label = "ucb1400_gpio";
-	ucb->gc.base = ucbdata->gpio_offset;
+	ucb->gc.base = ucb->gpio_offset;
 	ucb->gc.ngpio = 10;
 	ucb->gc.owner = THIS_MODULE;
 
@@ -72,8 +70,8 @@ static int ucb1400_gpio_probe(struct platform_device *dev)
 	if (err)
 		goto err;
 
-	if (ucbdata && ucbdata->gpio_setup)
-		err = ucbdata->gpio_setup(&dev->dev, ucb->gc.ngpio);
+	if (ucb && ucb->gpio_setup)
+		err = ucb->gpio_setup(&dev->dev, ucb->gc.ngpio);
 
 err:
 	return err;
@@ -85,8 +83,8 @@ static int ucb1400_gpio_remove(struct platform_device *dev)
 	int err = 0;
 	struct ucb1400_gpio *ucb = platform_get_drvdata(dev);
 
-	if (ucbdata && ucbdata->gpio_teardown) {
-		err = ucbdata->gpio_teardown(&dev->dev, ucb->gc.ngpio);
+	if (ucb && ucb->gpio_teardown) {
+		err = ucb->gpio_teardown(&dev->dev, ucb->gc.ngpio);
 		if (err)
 			return err;
 	}
@@ -103,11 +101,6 @@ static struct platform_driver ucb1400_gpio_driver = {
 	},
 };
 
-void __init ucb1400_gpio_set_data(struct ucb1400_gpio_data *data)
-{
-	ucbdata = data;
-}
-
 module_platform_driver(ucb1400_gpio_driver);
 
 MODULE_DESCRIPTION("Philips UCB1400 GPIO driver");
diff --git a/drivers/gpio/gpio-viperboard.c b/drivers/gpio/gpio-viperboard.c
index 59d72391de26..095ab14cea4d 100644
--- a/drivers/gpio/gpio-viperboard.c
+++ b/drivers/gpio/gpio-viperboard.c
@@ -380,10 +380,6 @@ static int vprbrd_gpiob_direction_output(struct gpio_chip *chip,
 	struct vprbrd *vb = gpio->vb;
 
 	gpio->gpiob_out |= (1 << offset);
-	if (value)
-		gpio->gpiob_val |= (1 << offset);
-	else
-		gpio->gpiob_val &= ~(1 << offset);
 
 	mutex_lock(&vb->lock);
 
diff --git a/drivers/gpio/gpio-vt8500.c b/drivers/gpio/gpio-vt8500.c
deleted file mode 100644
index 81683ca35ac1..000000000000
--- a/drivers/gpio/gpio-vt8500.c
+++ /dev/null
@@ -1,355 +0,0 @@
-/* drivers/gpio/gpio-vt8500.c
- *
- * Copyright (C) 2012 Tony Prisk <linux@prisktech.co.nz>
- * Based on arch/arm/mach-vt8500/gpio.c:
- * - Copyright (C) 2010 Alexey Charkov <alchark@gmail.com>
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * 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.
- *
- */
-
-#include <linux/module.h>
-#include <linux/err.h>
-#include <linux/io.h>
-#include <linux/gpio.h>
-#include <linux/platform_device.h>
-#include <linux/bitops.h>
-#include <linux/of.h>
-#include <linux/of_address.h>
-#include <linux/of_irq.h>
-#include <linux/of_device.h>
-
-/*
-	We handle GPIOs by bank, each bank containing up to 32 GPIOs covered
-	by one set of registers (although not all may be valid).
-
-	Because different SoC's have different register offsets, we pass the
-	register offsets as data in vt8500_gpio_dt_ids[].
-
-	A value of NO_REG is used to indicate that this register is not
-	supported. Only used for ->en at the moment.
-*/
-
-#define NO_REG	0xFFFF
-
-/*
- * struct vt8500_gpio_bank_regoffsets
- * @en: offset to enable register of the bank
- * @dir: offset to direction register of the bank
- * @data_out: offset to the data out register of the bank
- * @data_in: offset to the data in register of the bank
- * @ngpio: highest valid pin in this bank
- */
-
-struct vt8500_gpio_bank_regoffsets {
-	unsigned int	en;
-	unsigned int	dir;
-	unsigned int	data_out;
-	unsigned int	data_in;
-	unsigned char	ngpio;
-};
-
-struct vt8500_gpio_data {
-	unsigned int				num_banks;
-	struct vt8500_gpio_bank_regoffsets	banks[];
-};
-
-#define VT8500_BANK(__en, __dir, __out, __in, __ngpio)		\
-{								\
-	.en = __en,						\
-	.dir = __dir,						\
-	.data_out = __out,					\
-	.data_in = __in,					\
-	.ngpio = __ngpio,					\
-}
-
-static struct vt8500_gpio_data vt8500_data = {
-	.num_banks	= 7,
-	.banks	= {
-		VT8500_BANK(NO_REG, 0x3C, 0x5C, 0x7C, 9),
-		VT8500_BANK(0x00, 0x20, 0x40, 0x60, 26),
-		VT8500_BANK(0x04, 0x24, 0x44, 0x64, 28),
-		VT8500_BANK(0x08, 0x28, 0x48, 0x68, 31),
-		VT8500_BANK(0x0C, 0x2C, 0x4C, 0x6C, 19),
-		VT8500_BANK(0x10, 0x30, 0x50, 0x70, 19),
-		VT8500_BANK(0x14, 0x34, 0x54, 0x74, 23),
-	},
-};
-
-static struct vt8500_gpio_data wm8505_data = {
-	.num_banks	= 10,
-	.banks	= {
-		VT8500_BANK(0x64, 0x8C, 0xB4, 0xDC, 22),
-		VT8500_BANK(0x40, 0x68, 0x90, 0xB8, 8),
-		VT8500_BANK(0x44, 0x6C, 0x94, 0xBC, 32),
-		VT8500_BANK(0x48, 0x70, 0x98, 0xC0, 6),
-		VT8500_BANK(0x4C, 0x74, 0x9C, 0xC4, 16),
-		VT8500_BANK(0x50, 0x78, 0xA0, 0xC8, 25),
-		VT8500_BANK(0x54, 0x7C, 0xA4, 0xCC, 5),
-		VT8500_BANK(0x58, 0x80, 0xA8, 0xD0, 5),
-		VT8500_BANK(0x5C, 0x84, 0xAC, 0xD4, 12),
-		VT8500_BANK(0x60, 0x88, 0xB0, 0xD8, 16),
-		VT8500_BANK(0x500, 0x504, 0x508, 0x50C, 6),
-	},
-};
-
-/*
- * No information about which bits are valid so we just make
- * them all available until its figured out.
- */
-static struct vt8500_gpio_data wm8650_data = {
-	.num_banks	= 9,
-	.banks	= {
-		VT8500_BANK(0x40, 0x80, 0xC0, 0x00, 32),
-		VT8500_BANK(0x44, 0x84, 0xC4, 0x04, 32),
-		VT8500_BANK(0x48, 0x88, 0xC8, 0x08, 32),
-		VT8500_BANK(0x4C, 0x8C, 0xCC, 0x0C, 32),
-		VT8500_BANK(0x50, 0x90, 0xD0, 0x10, 32),
-		VT8500_BANK(0x54, 0x94, 0xD4, 0x14, 32),
-		VT8500_BANK(0x58, 0x98, 0xD8, 0x18, 32),
-		VT8500_BANK(0x5C, 0x9C, 0xDC, 0x1C, 32),
-		VT8500_BANK(0x7C, 0xBC, 0xFC, 0x3C, 32),
-		VT8500_BANK(0x500, 0x504, 0x508, 0x50C, 6),
-	},
-};
-
-struct vt8500_gpio_chip {
-	struct gpio_chip		chip;
-
-	const struct vt8500_gpio_bank_regoffsets *regs;
-	void __iomem	*base;
-};
-
-struct vt8500_data {
-	struct vt8500_gpio_chip *chip;
-	void __iomem *iobase;
-	int num_banks;
-};
-
-
-#define to_vt8500(__chip) container_of(__chip, struct vt8500_gpio_chip, chip)
-
-static int vt8500_gpio_request(struct gpio_chip *chip, unsigned offset)
-{
-	u32 val;
-	struct vt8500_gpio_chip *vt8500_chip = to_vt8500(chip);
-
-	if (vt8500_chip->regs->en == NO_REG)
-		return 0;
-
-	val = readl_relaxed(vt8500_chip->base + vt8500_chip->regs->en);
-	val |= BIT(offset);
-	writel_relaxed(val, vt8500_chip->base + vt8500_chip->regs->en);
-
-	return 0;
-}
-
-static void vt8500_gpio_free(struct gpio_chip *chip, unsigned offset)
-{
-	struct vt8500_gpio_chip *vt8500_chip = to_vt8500(chip);
-	u32 val;
-
-	if (vt8500_chip->regs->en == NO_REG)
-		return;
-
-	val = readl_relaxed(vt8500_chip->base + vt8500_chip->regs->en);
-	val &= ~BIT(offset);
-	writel_relaxed(val, vt8500_chip->base + vt8500_chip->regs->en);
-}
-
-static int vt8500_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
-{
-	struct vt8500_gpio_chip *vt8500_chip = to_vt8500(chip);
-
-	u32 val = readl_relaxed(vt8500_chip->base + vt8500_chip->regs->dir);
-	val &= ~BIT(offset);
-	writel_relaxed(val, vt8500_chip->base + vt8500_chip->regs->dir);
-
-	return 0;
-}
-
-static int vt8500_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
-								int value)
-{
-	struct vt8500_gpio_chip *vt8500_chip = to_vt8500(chip);
-
-	u32 val = readl_relaxed(vt8500_chip->base + vt8500_chip->regs->dir);
-	val |= BIT(offset);
-	writel_relaxed(val, vt8500_chip->base + vt8500_chip->regs->dir);
-
-	if (value) {
-		val = readl_relaxed(vt8500_chip->base +
-						vt8500_chip->regs->data_out);
-		val |= BIT(offset);
-		writel_relaxed(val, vt8500_chip->base +
-						vt8500_chip->regs->data_out);
-	}
-	return 0;
-}
-
-static int vt8500_gpio_get_value(struct gpio_chip *chip, unsigned offset)
-{
-	struct vt8500_gpio_chip *vt8500_chip = to_vt8500(chip);
-
-	return (readl_relaxed(vt8500_chip->base + vt8500_chip->regs->data_in) >>
-								offset) & 1;
-}
-
-static void vt8500_gpio_set_value(struct gpio_chip *chip, unsigned offset,
-								int value)
-{
-	struct vt8500_gpio_chip *vt8500_chip = to_vt8500(chip);
-
-	u32 val = readl_relaxed(vt8500_chip->base +
-						vt8500_chip->regs->data_out);
-	if (value)
-		val |= BIT(offset);
-	else
-		val &= ~BIT(offset);
-
-	writel_relaxed(val, vt8500_chip->base + vt8500_chip->regs->data_out);
-}
-
-static int vt8500_of_xlate(struct gpio_chip *gc,
-			    const struct of_phandle_args *gpiospec, u32 *flags)
-{
-	/* bank if specificed in gpiospec->args[0] */
-	if (flags)
-		*flags = gpiospec->args[2];
-
-	return gpiospec->args[1];
-}
-
-static int vt8500_add_chips(struct platform_device *pdev, void __iomem *base,
-				const struct vt8500_gpio_data *data)
-{
-	struct vt8500_data *priv;
-	struct vt8500_gpio_chip *vtchip;
-	struct gpio_chip *chip;
-	int i;
-	int pin_cnt = 0;
-
-	priv = devm_kzalloc(&pdev->dev, sizeof(struct vt8500_data), GFP_KERNEL);
-	if (!priv) {
-		dev_err(&pdev->dev, "failed to allocate memory\n");
-		return -ENOMEM;
-	}
-
-	priv->chip = devm_kzalloc(&pdev->dev,
-			sizeof(struct vt8500_gpio_chip) * data->num_banks,
-			GFP_KERNEL);
-	if (!priv->chip) {
-		dev_err(&pdev->dev, "failed to allocate chip memory\n");
-		return -ENOMEM;
-	}
-
-	priv->iobase = base;
-	priv->num_banks = data->num_banks;
-	platform_set_drvdata(pdev, priv);
-
-	vtchip = priv->chip;
-
-	for (i = 0; i < data->num_banks; i++) {
-		vtchip[i].base = base;
-		vtchip[i].regs = &data->banks[i];
-
-		chip = &vtchip[i].chip;
-
-		chip->of_xlate = vt8500_of_xlate;
-		chip->of_gpio_n_cells = 3;
-		chip->of_node = pdev->dev.of_node;
-
-		chip->request = vt8500_gpio_request;
-		chip->free = vt8500_gpio_free;
-		chip->direction_input = vt8500_gpio_direction_input;
-		chip->direction_output = vt8500_gpio_direction_output;
-		chip->get = vt8500_gpio_get_value;
-		chip->set = vt8500_gpio_set_value;
-		chip->can_sleep = 0;
-		chip->base = pin_cnt;
-		chip->ngpio = data->banks[i].ngpio;
-
-		pin_cnt += data->banks[i].ngpio;
-
-		gpiochip_add(chip);
-	}
-	return 0;
-}
-
-static struct of_device_id vt8500_gpio_dt_ids[] = {
-	{ .compatible = "via,vt8500-gpio", .data = &vt8500_data, },
-	{ .compatible = "wm,wm8505-gpio", .data = &wm8505_data, },
-	{ .compatible = "wm,wm8650-gpio", .data = &wm8650_data, },
-	{ /* Sentinel */ },
-};
-
-static int vt8500_gpio_probe(struct platform_device *pdev)
-{
-	int ret;
-	void __iomem *gpio_base;
-	struct resource *res;
-	const struct of_device_id *of_id =
-				of_match_device(vt8500_gpio_dt_ids, &pdev->dev);
-
-	if (!of_id) {
-		dev_err(&pdev->dev, "No matching driver data\n");
-		return -ENODEV;
-	}
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "Unable to get IO resource\n");
-		return -ENODEV;
-	}
-
-	gpio_base = devm_request_and_ioremap(&pdev->dev, res);
-	if (!gpio_base) {
-		dev_err(&pdev->dev, "Unable to map GPIO registers\n");
-		return -ENOMEM;
-	}
-
-	ret = vt8500_add_chips(pdev, gpio_base, of_id->data);
-
-	return ret;
-}
-
-static int vt8500_gpio_remove(struct platform_device *pdev)
-{
-	int i;
-	int ret;
-	struct vt8500_data *priv = platform_get_drvdata(pdev);
-	struct vt8500_gpio_chip *vtchip = priv->chip;
-
-	for (i = 0; i < priv->num_banks; i++) {
-		ret = gpiochip_remove(&vtchip[i].chip);
-		if (ret)
-			dev_warn(&pdev->dev, "gpiochip_remove returned %d\n",
-				 ret);
-	}
-
-	return 0;
-}
-
-static struct platform_driver vt8500_gpio_driver = {
-	.probe		= vt8500_gpio_probe,
-	.remove		= vt8500_gpio_remove,
-	.driver		= {
-		.name	= "vt8500-gpio",
-		.owner	= THIS_MODULE,
-		.of_match_table = vt8500_gpio_dt_ids,
-	},
-};
-
-module_platform_driver(vt8500_gpio_driver);
-
-MODULE_DESCRIPTION("VT8500 GPIO Driver");
-MODULE_AUTHOR("Tony Prisk <linux@prisktech.co.nz>");
-MODULE_LICENSE("GPL v2");
-MODULE_DEVICE_TABLE(of, vt8500_gpio_dt_ids);
diff --git a/drivers/gpio/gpiolib-acpi.c b/drivers/gpio/gpiolib-acpi.c
index a063eb04b6ce..5c1ef2b3ef18 100644
--- a/drivers/gpio/gpiolib-acpi.c
+++ b/drivers/gpio/gpiolib-acpi.c
@@ -17,6 +17,13 @@
 #include <linux/acpi.h>
 #include <linux/interrupt.h>
 
+struct acpi_gpio_evt_pin {
+	struct list_head node;
+	acpi_handle *evt_handle;
+	unsigned int pin;
+	unsigned int irq;
+};
+
 static int acpi_gpiochip_find(struct gpio_chip *gc, void *data)
 {
 	if (!gc->dev)
@@ -54,7 +61,6 @@ int acpi_get_gpio(char *path, int pin)
 }
 EXPORT_SYMBOL_GPL(acpi_get_gpio);
 
-
 static irqreturn_t acpi_gpio_irq_handler(int irq, void *data)
 {
 	acpi_handle handle = data;
@@ -64,6 +70,27 @@ static irqreturn_t acpi_gpio_irq_handler(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
+static irqreturn_t acpi_gpio_irq_handler_evt(int irq, void *data)
+{
+	struct acpi_gpio_evt_pin *evt_pin = data;
+	struct acpi_object_list args;
+	union acpi_object arg;
+
+	arg.type = ACPI_TYPE_INTEGER;
+	arg.integer.value = evt_pin->pin;
+	args.count = 1;
+	args.pointer = &arg;
+
+	acpi_evaluate_object(evt_pin->evt_handle, NULL, &args, NULL);
+
+	return IRQ_HANDLED;
+}
+
+static void acpi_gpio_evt_dh(acpi_handle handle, void *data)
+{
+	/* The address of this function is used as a key. */
+}
+
 /**
  * acpi_gpiochip_request_interrupts() - Register isr for gpio chip ACPI events
  * @chip:      gpio chip
@@ -73,15 +100,13 @@ static irqreturn_t acpi_gpio_irq_handler(int irq, void *data)
  * chip's interrupt handler. acpi_gpiochip_request_interrupts finds out which
  * gpio pins have acpi event methods and assigns interrupt handlers that calls
  * the acpi event methods for those pins.
- *
- * Interrupts are automatically freed on driver detach
  */
-
 void acpi_gpiochip_request_interrupts(struct gpio_chip *chip)
 {
 	struct acpi_buffer buf = {ACPI_ALLOCATE_BUFFER, NULL};
 	struct acpi_resource *res;
-	acpi_handle handle, ev_handle;
+	acpi_handle handle, evt_handle;
+	struct list_head *evt_pins = NULL;
 	acpi_status status;
 	unsigned int pin;
 	int irq, ret;
@@ -98,13 +123,30 @@ void acpi_gpiochip_request_interrupts(struct gpio_chip *chip)
 	if (ACPI_FAILURE(status))
 		return;
 
-	/* If a gpio interrupt has an acpi event handler method, then
-	 * set up an interrupt handler that calls the acpi event handler
-	 */
+	status = acpi_get_handle(handle, "_EVT", &evt_handle);
+	if (ACPI_SUCCESS(status)) {
+		evt_pins = kzalloc(sizeof(*evt_pins), GFP_KERNEL);
+		if (evt_pins) {
+			INIT_LIST_HEAD(evt_pins);
+			status = acpi_attach_data(handle, acpi_gpio_evt_dh,
+						  evt_pins);
+			if (ACPI_FAILURE(status)) {
+				kfree(evt_pins);
+				evt_pins = NULL;
+			}
+		}
+	}
 
+	/*
+	 * If a GPIO interrupt has an ACPI event handler method, or _EVT is
+	 * present, set up an interrupt handler that calls the ACPI event
+	 * handler.
+	 */
 	for (res = buf.pointer;
 	     res && (res->type != ACPI_RESOURCE_TYPE_END_TAG);
 	     res = ACPI_NEXT_RESOURCE(res)) {
+		irq_handler_t handler = NULL;
+		void *data;
 
 		if (res->type != ACPI_RESOURCE_TYPE_GPIO ||
 		    res->data.gpio.connection_type !=
@@ -115,23 +157,42 @@ void acpi_gpiochip_request_interrupts(struct gpio_chip *chip)
 		if (pin > chip->ngpio)
 			continue;
 
-		sprintf(ev_name, "_%c%02X",
-		res->data.gpio.triggering ? 'E' : 'L', pin);
-
-		status = acpi_get_handle(handle, ev_name, &ev_handle);
-		if (ACPI_FAILURE(status))
-			continue;
-
 		irq = chip->to_irq(chip, pin);
 		if (irq < 0)
 			continue;
 
+		if (pin <= 255) {
+			acpi_handle ev_handle;
+
+			sprintf(ev_name, "_%c%02X",
+				res->data.gpio.triggering ? 'E' : 'L', pin);
+			status = acpi_get_handle(handle, ev_name, &ev_handle);
+			if (ACPI_SUCCESS(status)) {
+				handler = acpi_gpio_irq_handler;
+				data = ev_handle;
+			}
+		}
+		if (!handler && evt_pins) {
+			struct acpi_gpio_evt_pin *evt_pin;
+
+			evt_pin = kzalloc(sizeof(*evt_pin), GFP_KERNEL);
+			if (!evt_pin)
+				continue;
+
+			list_add_tail(&evt_pin->node, evt_pins);
+			evt_pin->evt_handle = evt_handle;
+			evt_pin->pin = pin;
+			evt_pin->irq = irq;
+			handler = acpi_gpio_irq_handler_evt;
+			data = evt_pin;
+		}
+		if (!handler)
+			continue;
+
 		/* Assume BIOS sets the triggering, so no flags */
-		ret = devm_request_threaded_irq(chip->dev, irq, NULL,
-					  acpi_gpio_irq_handler,
-					  0,
-					  "GPIO-signaled-ACPI-event",
-					  ev_handle);
+		ret = devm_request_threaded_irq(chip->dev, irq, NULL, handler,
+						0, "GPIO-signaled-ACPI-event",
+						data);
 		if (ret)
 			dev_err(chip->dev,
 				"Failed to request IRQ %d ACPI event handler\n",
@@ -139,3 +200,119 @@ void acpi_gpiochip_request_interrupts(struct gpio_chip *chip)
 	}
 }
 EXPORT_SYMBOL(acpi_gpiochip_request_interrupts);
+
+struct acpi_gpio_lookup {
+	struct acpi_gpio_info info;
+	int index;
+	int gpio;
+	int n;
+};
+
+static int acpi_find_gpio(struct acpi_resource *ares, void *data)
+{
+	struct acpi_gpio_lookup *lookup = data;
+
+	if (ares->type != ACPI_RESOURCE_TYPE_GPIO)
+		return 1;
+
+	if (lookup->n++ == lookup->index && lookup->gpio < 0) {
+		const struct acpi_resource_gpio *agpio = &ares->data.gpio;
+
+		lookup->gpio = acpi_get_gpio(agpio->resource_source.string_ptr,
+					     agpio->pin_table[0]);
+		lookup->info.gpioint =
+			agpio->connection_type == ACPI_RESOURCE_GPIO_TYPE_INT;
+	}
+
+	return 1;
+}
+
+/**
+ * acpi_get_gpio_by_index() - get a GPIO number from device resources
+ * @dev: pointer to a device to get GPIO from
+ * @index: index of GpioIo/GpioInt resource (starting from %0)
+ * @info: info pointer to fill in (optional)
+ *
+ * Function goes through ACPI resources for @dev and based on @index looks
+ * up a GpioIo/GpioInt resource, translates it to the Linux GPIO number,
+ * and returns it. @index matches GpioIo/GpioInt resources only so if there
+ * are total %3 GPIO resources, the index goes from %0 to %2.
+ *
+ * If the GPIO cannot be translated or there is an error, negative errno is
+ * returned.
+ *
+ * Note: if the GPIO resource has multiple entries in the pin list, this
+ * function only returns the first.
+ */
+int acpi_get_gpio_by_index(struct device *dev, int index,
+			   struct acpi_gpio_info *info)
+{
+	struct acpi_gpio_lookup lookup;
+	struct list_head resource_list;
+	struct acpi_device *adev;
+	acpi_handle handle;
+	int ret;
+
+	if (!dev)
+		return -EINVAL;
+
+	handle = ACPI_HANDLE(dev);
+	if (!handle || acpi_bus_get_device(handle, &adev))
+		return -ENODEV;
+
+	memset(&lookup, 0, sizeof(lookup));
+	lookup.index = index;
+	lookup.gpio = -ENODEV;
+
+	INIT_LIST_HEAD(&resource_list);
+	ret = acpi_dev_get_resources(adev, &resource_list, acpi_find_gpio,
+				     &lookup);
+	if (ret < 0)
+		return ret;
+
+	acpi_dev_free_resource_list(&resource_list);
+
+	if (lookup.gpio >= 0 && info)
+		*info = lookup.info;
+
+	return lookup.gpio;
+}
+EXPORT_SYMBOL_GPL(acpi_get_gpio_by_index);
+
+/**
+ * acpi_gpiochip_free_interrupts() - Free GPIO _EVT ACPI event interrupts.
+ * @chip:      gpio chip
+ *
+ * Free interrupts associated with the _EVT method for the given GPIO chip.
+ *
+ * The remaining ACPI event interrupts associated with the chip are freed
+ * automatically.
+ */
+void acpi_gpiochip_free_interrupts(struct gpio_chip *chip)
+{
+	acpi_handle handle;
+	acpi_status status;
+	struct list_head *evt_pins;
+	struct acpi_gpio_evt_pin *evt_pin, *ep;
+
+	if (!chip->dev || !chip->to_irq)
+		return;
+
+	handle = ACPI_HANDLE(chip->dev);
+	if (!handle)
+		return;
+
+	status = acpi_get_data(handle, acpi_gpio_evt_dh, (void **)&evt_pins);
+	if (ACPI_FAILURE(status))
+		return;
+
+	list_for_each_entry_safe_reverse(evt_pin, ep, evt_pins, node) {
+		devm_free_irq(chip->dev, evt_pin->irq, evt_pin);
+		list_del(&evt_pin->node);
+		kfree(evt_pin);
+	}
+
+	acpi_detach_data(handle, acpi_gpio_evt_dh);
+	kfree(evt_pins);
+}
+EXPORT_SYMBOL(acpi_gpiochip_free_interrupts);
diff --git a/drivers/gpio/gpiolib-of.c b/drivers/gpio/gpiolib-of.c
index 465f4ca57e80..665f9530c950 100644
--- a/drivers/gpio/gpiolib-of.c
+++ b/drivers/gpio/gpiolib-of.c
@@ -61,7 +61,7 @@ static int of_gpiochip_find_and_xlate(struct gpio_chip *gc, void *data)
  * in flags for the GPIO.
  */
 int of_get_named_gpio_flags(struct device_node *np, const char *propname,
-                           int index, enum of_gpio_flags *flags)
+			   int index, enum of_gpio_flags *flags)
 {
 	/* Return -EPROBE_DEFER to support probe() functions to be called
 	 * later when the GPIO actually becomes available
diff --git a/drivers/gpu/Makefile b/drivers/gpu/Makefile
index 30879df3daea..d8a22c2a579d 100644
--- a/drivers/gpu/Makefile
+++ b/drivers/gpu/Makefile
@@ -1 +1,2 @@
 obj-y			+= drm/ vga/
+obj-$(CONFIG_TEGRA_HOST1X)	+= host1x/
diff --git a/drivers/gpu/drm/Kconfig b/drivers/gpu/drm/Kconfig
index 1e82882da9de..b16c50ee769c 100644
--- a/drivers/gpu/drm/Kconfig
+++ b/drivers/gpu/drm/Kconfig
@@ -215,8 +215,8 @@ source "drivers/gpu/drm/cirrus/Kconfig"
 
 source "drivers/gpu/drm/shmobile/Kconfig"
 
-source "drivers/gpu/drm/tegra/Kconfig"
-
 source "drivers/gpu/drm/omapdrm/Kconfig"
 
 source "drivers/gpu/drm/tilcdc/Kconfig"
+
+source "drivers/gpu/drm/qxl/Kconfig"
diff --git a/drivers/gpu/drm/Makefile b/drivers/gpu/drm/Makefile
index 0d59b24f8d23..1c9f24396002 100644
--- a/drivers/gpu/drm/Makefile
+++ b/drivers/gpu/drm/Makefile
@@ -49,7 +49,7 @@ obj-$(CONFIG_DRM_GMA500) += gma500/
 obj-$(CONFIG_DRM_UDL) += udl/
 obj-$(CONFIG_DRM_AST) += ast/
 obj-$(CONFIG_DRM_SHMOBILE) +=shmobile/
-obj-$(CONFIG_DRM_TEGRA) += tegra/
 obj-$(CONFIG_DRM_OMAP)	+= omapdrm/
 obj-$(CONFIG_DRM_TILCDC)	+= tilcdc/
+obj-$(CONFIG_DRM_QXL) += qxl/
 obj-y			+= i2c/
diff --git a/drivers/gpu/drm/ast/ast_drv.h b/drivers/gpu/drm/ast/ast_drv.h
index 528429252f0f..02e52d543e4b 100644
--- a/drivers/gpu/drm/ast/ast_drv.h
+++ b/drivers/gpu/drm/ast/ast_drv.h
@@ -241,6 +241,8 @@ struct ast_fbdev {
 	void *sysram;
 	int size;
 	struct ttm_bo_kmap_obj mapping;
+	int x1, y1, x2, y2; /* dirty rect */
+	spinlock_t dirty_lock;
 };
 
 #define to_ast_crtc(x) container_of(x, struct ast_crtc, base)
diff --git a/drivers/gpu/drm/ast/ast_fb.c b/drivers/gpu/drm/ast/ast_fb.c
index 34931fe7d2c5..fbc0823cfa18 100644
--- a/drivers/gpu/drm/ast/ast_fb.c
+++ b/drivers/gpu/drm/ast/ast_fb.c
@@ -53,16 +53,52 @@ static void ast_dirty_update(struct ast_fbdev *afbdev,
 	int bpp = (afbdev->afb.base.bits_per_pixel + 7)/8;
 	int ret;
 	bool unmap = false;
+	bool store_for_later = false;
+	int x2, y2;
+	unsigned long flags;
 
 	obj = afbdev->afb.obj;
 	bo = gem_to_ast_bo(obj);
 
+	/*
+	 * try and reserve the BO, if we fail with busy
+	 * then the BO is being moved and we should
+	 * store up the damage until later.
+	 */
 	ret = ast_bo_reserve(bo, true);
 	if (ret) {
-		DRM_ERROR("failed to reserve fb bo\n");
+		if (ret != -EBUSY)
+			return;
+
+		store_for_later = true;
+	}
+
+	x2 = x + width - 1;
+	y2 = y + height - 1;
+	spin_lock_irqsave(&afbdev->dirty_lock, flags);
+
+	if (afbdev->y1 < y)
+		y = afbdev->y1;
+	if (afbdev->y2 > y2)
+		y2 = afbdev->y2;
+	if (afbdev->x1 < x)
+		x = afbdev->x1;
+	if (afbdev->x2 > x2)
+		x2 = afbdev->x2;
+
+	if (store_for_later) {
+		afbdev->x1 = x;
+		afbdev->x2 = x2;
+		afbdev->y1 = y;
+		afbdev->y2 = y2;
+		spin_unlock_irqrestore(&afbdev->dirty_lock, flags);
 		return;
 	}
 
+	afbdev->x1 = afbdev->y1 = INT_MAX;
+	afbdev->x2 = afbdev->y2 = 0;
+	spin_unlock_irqrestore(&afbdev->dirty_lock, flags);
+
 	if (!bo->kmap.virtual) {
 		ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &bo->kmap);
 		if (ret) {
@@ -72,10 +108,10 @@ static void ast_dirty_update(struct ast_fbdev *afbdev,
 		}
 		unmap = true;
 	}
-	for (i = y; i < y + height; i++) {
+	for (i = y; i <= y2; i++) {
 		/* assume equal stride for now */
 		src_offset = dst_offset = i * afbdev->afb.base.pitches[0] + (x * bpp);
-		memcpy_toio(bo->kmap.virtual + src_offset, afbdev->sysram + src_offset, width * bpp);
+		memcpy_toio(bo->kmap.virtual + src_offset, afbdev->sysram + src_offset, (x2 - x + 1) * bpp);
 
 	}
 	if (unmap)
@@ -292,6 +328,7 @@ int ast_fbdev_init(struct drm_device *dev)
 
 	ast->fbdev = afbdev;
 	afbdev->helper.funcs = &ast_fb_helper_funcs;
+	spin_lock_init(&afbdev->dirty_lock);
 	ret = drm_fb_helper_init(dev, &afbdev->helper,
 				 1, 1);
 	if (ret) {
diff --git a/drivers/gpu/drm/ast/ast_ttm.c b/drivers/gpu/drm/ast/ast_ttm.c
index 3602731a6112..09da3393c527 100644
--- a/drivers/gpu/drm/ast/ast_ttm.c
+++ b/drivers/gpu/drm/ast/ast_ttm.c
@@ -316,7 +316,7 @@ int ast_bo_reserve(struct ast_bo *bo, bool no_wait)
 
 	ret = ttm_bo_reserve(&bo->bo, true, no_wait, false, 0);
 	if (ret) {
-		if (ret != -ERESTARTSYS)
+		if (ret != -ERESTARTSYS && ret != -EBUSY)
 			DRM_ERROR("reserve failed %p\n", bo);
 		return ret;
 	}
diff --git a/drivers/gpu/drm/cirrus/cirrus_drv.h b/drivers/gpu/drm/cirrus/cirrus_drv.h
index 6e0cc724e5a2..7ca059596887 100644
--- a/drivers/gpu/drm/cirrus/cirrus_drv.h
+++ b/drivers/gpu/drm/cirrus/cirrus_drv.h
@@ -154,6 +154,8 @@ struct cirrus_fbdev {
 	struct list_head fbdev_list;
 	void *sysram;
 	int size;
+	int x1, y1, x2, y2; /* dirty rect */
+	spinlock_t dirty_lock;
 };
 
 struct cirrus_bo {
diff --git a/drivers/gpu/drm/cirrus/cirrus_fbdev.c b/drivers/gpu/drm/cirrus/cirrus_fbdev.c
index e25afccaf85b..3541b567bbd8 100644
--- a/drivers/gpu/drm/cirrus/cirrus_fbdev.c
+++ b/drivers/gpu/drm/cirrus/cirrus_fbdev.c
@@ -27,16 +27,51 @@ static void cirrus_dirty_update(struct cirrus_fbdev *afbdev,
 	int bpp = (afbdev->gfb.base.bits_per_pixel + 7)/8;
 	int ret;
 	bool unmap = false;
+	bool store_for_later = false;
+	int x2, y2;
+	unsigned long flags;
 
 	obj = afbdev->gfb.obj;
 	bo = gem_to_cirrus_bo(obj);
 
+	/*
+	 * try and reserve the BO, if we fail with busy
+	 * then the BO is being moved and we should
+	 * store up the damage until later.
+	 */
 	ret = cirrus_bo_reserve(bo, true);
 	if (ret) {
-		DRM_ERROR("failed to reserve fb bo\n");
+		if (ret != -EBUSY)
+			return;
+		store_for_later = true;
+	}
+
+	x2 = x + width - 1;
+	y2 = y + height - 1;
+	spin_lock_irqsave(&afbdev->dirty_lock, flags);
+
+	if (afbdev->y1 < y)
+		y = afbdev->y1;
+	if (afbdev->y2 > y2)
+		y2 = afbdev->y2;
+	if (afbdev->x1 < x)
+		x = afbdev->x1;
+	if (afbdev->x2 > x2)
+		x2 = afbdev->x2;
+
+	if (store_for_later) {
+		afbdev->x1 = x;
+		afbdev->x2 = x2;
+		afbdev->y1 = y;
+		afbdev->y2 = y2;
+		spin_unlock_irqrestore(&afbdev->dirty_lock, flags);
 		return;
 	}
 
+	afbdev->x1 = afbdev->y1 = INT_MAX;
+	afbdev->x2 = afbdev->y2 = 0;
+	spin_unlock_irqrestore(&afbdev->dirty_lock, flags);
+
 	if (!bo->kmap.virtual) {
 		ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &bo->kmap);
 		if (ret) {
@@ -268,6 +303,7 @@ int cirrus_fbdev_init(struct cirrus_device *cdev)
 
 	cdev->mode_info.gfbdev = gfbdev;
 	gfbdev->helper.funcs = &cirrus_fb_helper_funcs;
+	spin_lock_init(&gfbdev->dirty_lock);
 
 	ret = drm_fb_helper_init(cdev->dev, &gfbdev->helper,
 				 cdev->num_crtc, CIRRUSFB_CONN_LIMIT);
diff --git a/drivers/gpu/drm/cirrus/cirrus_ttm.c b/drivers/gpu/drm/cirrus/cirrus_ttm.c
index 1413a26e4905..2ed8cfc740c9 100644
--- a/drivers/gpu/drm/cirrus/cirrus_ttm.c
+++ b/drivers/gpu/drm/cirrus/cirrus_ttm.c
@@ -321,7 +321,7 @@ int cirrus_bo_reserve(struct cirrus_bo *bo, bool no_wait)
 
 	ret = ttm_bo_reserve(&bo->bo, true, no_wait, false, 0);
 	if (ret) {
-		if (ret != -ERESTARTSYS)
+		if (ret != -ERESTARTSYS && ret != -EBUSY)
 			DRM_ERROR("reserve failed %p\n", bo);
 		return ret;
 	}
diff --git a/drivers/gpu/drm/drm_cache.c b/drivers/gpu/drm/drm_cache.c
index a575cb2e6bdb..bb8f58012189 100644
--- a/drivers/gpu/drm/drm_cache.c
+++ b/drivers/gpu/drm/drm_cache.c
@@ -105,12 +105,11 @@ drm_clflush_sg(struct sg_table *st)
 {
 #if defined(CONFIG_X86)
 	if (cpu_has_clflush) {
-		struct scatterlist *sg;
-		int i;
+		struct sg_page_iter sg_iter;
 
 		mb();
-		for_each_sg(st->sgl, sg, st->nents, i)
-			drm_clflush_page(sg_page(sg));
+		for_each_sg_page(st->sgl, &sg_iter, st->nents, 0)
+			drm_clflush_page(sg_page_iter_page(&sg_iter));
 		mb();
 
 		return;
diff --git a/drivers/gpu/drm/drm_crtc.c b/drivers/gpu/drm/drm_crtc.c
index dd64a06dc5b4..3a8f7e6db295 100644
--- a/drivers/gpu/drm/drm_crtc.c
+++ b/drivers/gpu/drm/drm_crtc.c
@@ -178,9 +178,6 @@ static struct drm_prop_enum_list drm_dirty_info_enum_list[] = {
 	{ DRM_MODE_DIRTY_ANNOTATE, "Annotate" },
 };
 
-DRM_ENUM_NAME_FN(drm_get_dirty_info_name,
-		 drm_dirty_info_enum_list)
-
 struct drm_conn_prop_enum_list {
 	int type;
 	char *name;
@@ -412,7 +409,7 @@ struct drm_framebuffer *drm_framebuffer_lookup(struct drm_device *dev,
 	mutex_lock(&dev->mode_config.fb_lock);
 	fb = __drm_framebuffer_lookup(dev, id);
 	if (fb)
-		kref_get(&fb->refcount);
+		drm_framebuffer_reference(fb);
 	mutex_unlock(&dev->mode_config.fb_lock);
 
 	return fb;
@@ -706,7 +703,6 @@ int drm_connector_init(struct drm_device *dev,
 	connector->connector_type = connector_type;
 	connector->connector_type_id =
 		++drm_connector_enum_list[connector_type].count; /* TODO */
-	INIT_LIST_HEAD(&connector->user_modes);
 	INIT_LIST_HEAD(&connector->probed_modes);
 	INIT_LIST_HEAD(&connector->modes);
 	connector->edid_blob_ptr = NULL;
@@ -747,9 +743,6 @@ void drm_connector_cleanup(struct drm_connector *connector)
 	list_for_each_entry_safe(mode, t, &connector->modes, head)
 		drm_mode_remove(connector, mode);
 
-	list_for_each_entry_safe(mode, t, &connector->user_modes, head)
-		drm_mode_remove(connector, mode);
-
 	drm_mode_object_put(dev, &connector->base);
 	list_del(&connector->head);
 	dev->mode_config.num_connector--;
@@ -1120,45 +1113,7 @@ int drm_mode_create_dirty_info_property(struct drm_device *dev)
 }
 EXPORT_SYMBOL(drm_mode_create_dirty_info_property);
 
-/**
- * drm_mode_config_init - initialize DRM mode_configuration structure
- * @dev: DRM device
- *
- * Initialize @dev's mode_config structure, used for tracking the graphics
- * configuration of @dev.
- *
- * Since this initializes the modeset locks, no locking is possible. Which is no
- * problem, since this should happen single threaded at init time. It is the
- * driver's problem to ensure this guarantee.
- *
- */
-void drm_mode_config_init(struct drm_device *dev)
-{
-	mutex_init(&dev->mode_config.mutex);
-	mutex_init(&dev->mode_config.idr_mutex);
-	mutex_init(&dev->mode_config.fb_lock);
-	INIT_LIST_HEAD(&dev->mode_config.fb_list);
-	INIT_LIST_HEAD(&dev->mode_config.crtc_list);
-	INIT_LIST_HEAD(&dev->mode_config.connector_list);
-	INIT_LIST_HEAD(&dev->mode_config.encoder_list);
-	INIT_LIST_HEAD(&dev->mode_config.property_list);
-	INIT_LIST_HEAD(&dev->mode_config.property_blob_list);
-	INIT_LIST_HEAD(&dev->mode_config.plane_list);
-	idr_init(&dev->mode_config.crtc_idr);
-
-	drm_modeset_lock_all(dev);
-	drm_mode_create_standard_connector_properties(dev);
-	drm_modeset_unlock_all(dev);
-
-	/* Just to be sure */
-	dev->mode_config.num_fb = 0;
-	dev->mode_config.num_connector = 0;
-	dev->mode_config.num_crtc = 0;
-	dev->mode_config.num_encoder = 0;
-}
-EXPORT_SYMBOL(drm_mode_config_init);
-
-int drm_mode_group_init(struct drm_device *dev, struct drm_mode_group *group)
+static int drm_mode_group_init(struct drm_device *dev, struct drm_mode_group *group)
 {
 	uint32_t total_objects = 0;
 
@@ -1203,69 +1158,6 @@ int drm_mode_group_init_legacy_group(struct drm_device *dev,
 EXPORT_SYMBOL(drm_mode_group_init_legacy_group);
 
 /**
- * drm_mode_config_cleanup - free up DRM mode_config info
- * @dev: DRM device
- *
- * Free up all the connectors and CRTCs associated with this DRM device, then
- * free up the framebuffers and associated buffer objects.
- *
- * Note that since this /should/ happen single-threaded at driver/device
- * teardown time, no locking is required. It's the driver's job to ensure that
- * this guarantee actually holds true.
- *
- * FIXME: cleanup any dangling user buffer objects too
- */
-void drm_mode_config_cleanup(struct drm_device *dev)
-{
-	struct drm_connector *connector, *ot;
-	struct drm_crtc *crtc, *ct;
-	struct drm_encoder *encoder, *enct;
-	struct drm_framebuffer *fb, *fbt;
-	struct drm_property *property, *pt;
-	struct drm_plane *plane, *plt;
-
-	list_for_each_entry_safe(encoder, enct, &dev->mode_config.encoder_list,
-				 head) {
-		encoder->funcs->destroy(encoder);
-	}
-
-	list_for_each_entry_safe(connector, ot,
-				 &dev->mode_config.connector_list, head) {
-		connector->funcs->destroy(connector);
-	}
-
-	list_for_each_entry_safe(property, pt, &dev->mode_config.property_list,
-				 head) {
-		drm_property_destroy(dev, property);
-	}
-
-	/*
-	 * Single-threaded teardown context, so it's not required to grab the
-	 * fb_lock to protect against concurrent fb_list access. Contrary, it
-	 * would actually deadlock with the drm_framebuffer_cleanup function.
-	 *
-	 * Also, if there are any framebuffers left, that's a driver leak now,
-	 * so politely WARN about this.
-	 */
-	WARN_ON(!list_empty(&dev->mode_config.fb_list));
-	list_for_each_entry_safe(fb, fbt, &dev->mode_config.fb_list, head) {
-		drm_framebuffer_remove(fb);
-	}
-
-	list_for_each_entry_safe(plane, plt, &dev->mode_config.plane_list,
-				 head) {
-		plane->funcs->destroy(plane);
-	}
-
-	list_for_each_entry_safe(crtc, ct, &dev->mode_config.crtc_list, head) {
-		crtc->funcs->destroy(crtc);
-	}
-
-	idr_destroy(&dev->mode_config.crtc_idr);
-}
-EXPORT_SYMBOL(drm_mode_config_cleanup);
-
-/**
  * drm_crtc_convert_to_umode - convert a drm_display_mode into a modeinfo
  * @out: drm_mode_modeinfo struct to return to the user
  * @in: drm_display_mode to use
@@ -2717,192 +2609,6 @@ void drm_fb_release(struct drm_file *priv)
 	mutex_unlock(&priv->fbs_lock);
 }
 
-/**
- * drm_mode_attachmode - add a mode to the user mode list
- * @dev: DRM device
- * @connector: connector to add the mode to
- * @mode: mode to add
- *
- * Add @mode to @connector's user mode list.
- */
-static void drm_mode_attachmode(struct drm_device *dev,
-				struct drm_connector *connector,
-				struct drm_display_mode *mode)
-{
-	list_add_tail(&mode->head, &connector->user_modes);
-}
-
-int drm_mode_attachmode_crtc(struct drm_device *dev, struct drm_crtc *crtc,
-			     const struct drm_display_mode *mode)
-{
-	struct drm_connector *connector;
-	int ret = 0;
-	struct drm_display_mode *dup_mode, *next;
-	LIST_HEAD(list);
-
-	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-		if (!connector->encoder)
-			continue;
-		if (connector->encoder->crtc == crtc) {
-			dup_mode = drm_mode_duplicate(dev, mode);
-			if (!dup_mode) {
-				ret = -ENOMEM;
-				goto out;
-			}
-			list_add_tail(&dup_mode->head, &list);
-		}
-	}
-
-	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-		if (!connector->encoder)
-			continue;
-		if (connector->encoder->crtc == crtc)
-			list_move_tail(list.next, &connector->user_modes);
-	}
-
-	WARN_ON(!list_empty(&list));
-
- out:
-	list_for_each_entry_safe(dup_mode, next, &list, head)
-		drm_mode_destroy(dev, dup_mode);
-
-	return ret;
-}
-EXPORT_SYMBOL(drm_mode_attachmode_crtc);
-
-static int drm_mode_detachmode(struct drm_device *dev,
-			       struct drm_connector *connector,
-			       struct drm_display_mode *mode)
-{
-	int found = 0;
-	int ret = 0;
-	struct drm_display_mode *match_mode, *t;
-
-	list_for_each_entry_safe(match_mode, t, &connector->user_modes, head) {
-		if (drm_mode_equal(match_mode, mode)) {
-			list_del(&match_mode->head);
-			drm_mode_destroy(dev, match_mode);
-			found = 1;
-			break;
-		}
-	}
-
-	if (!found)
-		ret = -EINVAL;
-
-	return ret;
-}
-
-int drm_mode_detachmode_crtc(struct drm_device *dev, struct drm_display_mode *mode)
-{
-	struct drm_connector *connector;
-
-	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-		drm_mode_detachmode(dev, connector, mode);
-	}
-	return 0;
-}
-EXPORT_SYMBOL(drm_mode_detachmode_crtc);
-
-/**
- * drm_fb_attachmode - Attach a user mode to an connector
- * @dev: drm device for the ioctl
- * @data: data pointer for the ioctl
- * @file_priv: drm file for the ioctl call
- *
- * This attaches a user specified mode to an connector.
- * Called by the user via ioctl.
- *
- * RETURNS:
- * Zero on success, errno on failure.
- */
-int drm_mode_attachmode_ioctl(struct drm_device *dev,
-			      void *data, struct drm_file *file_priv)
-{
-	struct drm_mode_mode_cmd *mode_cmd = data;
-	struct drm_connector *connector;
-	struct drm_display_mode *mode;
-	struct drm_mode_object *obj;
-	struct drm_mode_modeinfo *umode = &mode_cmd->mode;
-	int ret;
-
-	if (!drm_core_check_feature(dev, DRIVER_MODESET))
-		return -EINVAL;
-
-	drm_modeset_lock_all(dev);
-
-	obj = drm_mode_object_find(dev, mode_cmd->connector_id, DRM_MODE_OBJECT_CONNECTOR);
-	if (!obj) {
-		ret = -EINVAL;
-		goto out;
-	}
-	connector = obj_to_connector(obj);
-
-	mode = drm_mode_create(dev);
-	if (!mode) {
-		ret = -ENOMEM;
-		goto out;
-	}
-
-	ret = drm_crtc_convert_umode(mode, umode);
-	if (ret) {
-		DRM_DEBUG_KMS("Invalid mode\n");
-		drm_mode_destroy(dev, mode);
-		goto out;
-	}
-
-	drm_mode_attachmode(dev, connector, mode);
-out:
-	drm_modeset_unlock_all(dev);
-	return ret;
-}
-
-
-/**
- * drm_fb_detachmode - Detach a user specified mode from an connector
- * @dev: drm device for the ioctl
- * @data: data pointer for the ioctl
- * @file_priv: drm file for the ioctl call
- *
- * Called by the user via ioctl.
- *
- * RETURNS:
- * Zero on success, errno on failure.
- */
-int drm_mode_detachmode_ioctl(struct drm_device *dev,
-			      void *data, struct drm_file *file_priv)
-{
-	struct drm_mode_object *obj;
-	struct drm_mode_mode_cmd *mode_cmd = data;
-	struct drm_connector *connector;
-	struct drm_display_mode mode;
-	struct drm_mode_modeinfo *umode = &mode_cmd->mode;
-	int ret;
-
-	if (!drm_core_check_feature(dev, DRIVER_MODESET))
-		return -EINVAL;
-
-	drm_modeset_lock_all(dev);
-
-	obj = drm_mode_object_find(dev, mode_cmd->connector_id, DRM_MODE_OBJECT_CONNECTOR);
-	if (!obj) {
-		ret = -EINVAL;
-		goto out;
-	}
-	connector = obj_to_connector(obj);
-
-	ret = drm_crtc_convert_umode(&mode, umode);
-	if (ret) {
-		DRM_DEBUG_KMS("Invalid mode\n");
-		goto out;
-	}
-
-	ret = drm_mode_detachmode(dev, connector, &mode);
-out:
-	drm_modeset_unlock_all(dev);
-	return ret;
-}
-
 struct drm_property *drm_property_create(struct drm_device *dev, int flags,
 					 const char *name, int num_values)
 {
@@ -3739,6 +3445,12 @@ int drm_mode_page_flip_ioctl(struct drm_device *dev,
 		goto out;
 	}
 
+	if (crtc->fb->pixel_format != fb->pixel_format) {
+		DRM_DEBUG_KMS("Page flip is not allowed to change frame buffer format.\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
 	if (page_flip->flags & DRM_MODE_PAGE_FLIP_EVENT) {
 		ret = -ENOMEM;
 		spin_lock_irqsave(&dev->event_lock, flags);
@@ -4064,3 +3776,110 @@ int drm_format_vert_chroma_subsampling(uint32_t format)
 	}
 }
 EXPORT_SYMBOL(drm_format_vert_chroma_subsampling);
+
+/**
+ * drm_mode_config_init - initialize DRM mode_configuration structure
+ * @dev: DRM device
+ *
+ * Initialize @dev's mode_config structure, used for tracking the graphics
+ * configuration of @dev.
+ *
+ * Since this initializes the modeset locks, no locking is possible. Which is no
+ * problem, since this should happen single threaded at init time. It is the
+ * driver's problem to ensure this guarantee.
+ *
+ */
+void drm_mode_config_init(struct drm_device *dev)
+{
+	mutex_init(&dev->mode_config.mutex);
+	mutex_init(&dev->mode_config.idr_mutex);
+	mutex_init(&dev->mode_config.fb_lock);
+	INIT_LIST_HEAD(&dev->mode_config.fb_list);
+	INIT_LIST_HEAD(&dev->mode_config.crtc_list);
+	INIT_LIST_HEAD(&dev->mode_config.connector_list);
+	INIT_LIST_HEAD(&dev->mode_config.encoder_list);
+	INIT_LIST_HEAD(&dev->mode_config.property_list);
+	INIT_LIST_HEAD(&dev->mode_config.property_blob_list);
+	INIT_LIST_HEAD(&dev->mode_config.plane_list);
+	idr_init(&dev->mode_config.crtc_idr);
+
+	drm_modeset_lock_all(dev);
+	drm_mode_create_standard_connector_properties(dev);
+	drm_modeset_unlock_all(dev);
+
+	/* Just to be sure */
+	dev->mode_config.num_fb = 0;
+	dev->mode_config.num_connector = 0;
+	dev->mode_config.num_crtc = 0;
+	dev->mode_config.num_encoder = 0;
+}
+EXPORT_SYMBOL(drm_mode_config_init);
+
+/**
+ * drm_mode_config_cleanup - free up DRM mode_config info
+ * @dev: DRM device
+ *
+ * Free up all the connectors and CRTCs associated with this DRM device, then
+ * free up the framebuffers and associated buffer objects.
+ *
+ * Note that since this /should/ happen single-threaded at driver/device
+ * teardown time, no locking is required. It's the driver's job to ensure that
+ * this guarantee actually holds true.
+ *
+ * FIXME: cleanup any dangling user buffer objects too
+ */
+void drm_mode_config_cleanup(struct drm_device *dev)
+{
+	struct drm_connector *connector, *ot;
+	struct drm_crtc *crtc, *ct;
+	struct drm_encoder *encoder, *enct;
+	struct drm_framebuffer *fb, *fbt;
+	struct drm_property *property, *pt;
+	struct drm_property_blob *blob, *bt;
+	struct drm_plane *plane, *plt;
+
+	list_for_each_entry_safe(encoder, enct, &dev->mode_config.encoder_list,
+				 head) {
+		encoder->funcs->destroy(encoder);
+	}
+
+	list_for_each_entry_safe(connector, ot,
+				 &dev->mode_config.connector_list, head) {
+		connector->funcs->destroy(connector);
+	}
+
+	list_for_each_entry_safe(property, pt, &dev->mode_config.property_list,
+				 head) {
+		drm_property_destroy(dev, property);
+	}
+
+	list_for_each_entry_safe(blob, bt, &dev->mode_config.property_blob_list,
+				 head) {
+		drm_property_destroy_blob(dev, blob);
+	}
+
+	/*
+	 * Single-threaded teardown context, so it's not required to grab the
+	 * fb_lock to protect against concurrent fb_list access. Contrary, it
+	 * would actually deadlock with the drm_framebuffer_cleanup function.
+	 *
+	 * Also, if there are any framebuffers left, that's a driver leak now,
+	 * so politely WARN about this.
+	 */
+	WARN_ON(!list_empty(&dev->mode_config.fb_list));
+	list_for_each_entry_safe(fb, fbt, &dev->mode_config.fb_list, head) {
+		drm_framebuffer_remove(fb);
+	}
+
+	list_for_each_entry_safe(plane, plt, &dev->mode_config.plane_list,
+				 head) {
+		plane->funcs->destroy(plane);
+	}
+
+	list_for_each_entry_safe(crtc, ct, &dev->mode_config.crtc_list, head) {
+		crtc->funcs->destroy(crtc);
+	}
+
+	idr_destroy(&dev->mode_config.crtc_idr);
+}
+EXPORT_SYMBOL(drm_mode_config_cleanup);
diff --git a/drivers/gpu/drm/drm_crtc_helper.c b/drivers/gpu/drm/drm_crtc_helper.c
index 7b2d378b2576..e974f9309b72 100644
--- a/drivers/gpu/drm/drm_crtc_helper.c
+++ b/drivers/gpu/drm/drm_crtc_helper.c
@@ -648,6 +648,9 @@ int drm_crtc_helper_set_config(struct drm_mode_set *set)
 		} else if (set->fb->bits_per_pixel !=
 			   set->crtc->fb->bits_per_pixel) {
 			mode_changed = true;
+		} else if (set->fb->pixel_format !=
+			   set->crtc->fb->pixel_format) {
+			mode_changed = true;
 		} else
 			fb_changed = true;
 	}
diff --git a/drivers/gpu/drm/drm_drv.c b/drivers/gpu/drm/drm_drv.c
index 25f91cd23e60..8d4f29075af5 100644
--- a/drivers/gpu/drm/drm_drv.c
+++ b/drivers/gpu/drm/drm_drv.c
@@ -60,7 +60,7 @@ static int drm_version(struct drm_device *dev, void *data,
 	[DRM_IOCTL_NR(ioctl)] = {.cmd = ioctl, .func = _func, .flags = _flags, .cmd_drv = 0}
 
 /** Ioctl table */
-static struct drm_ioctl_desc drm_ioctls[] = {
+static const struct drm_ioctl_desc drm_ioctls[] = {
 	DRM_IOCTL_DEF(DRM_IOCTL_VERSION, drm_version, DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_GET_UNIQUE, drm_getunique, 0),
 	DRM_IOCTL_DEF(DRM_IOCTL_GET_MAGIC, drm_getmagic, 0),
@@ -150,8 +150,8 @@ static struct drm_ioctl_desc drm_ioctls[] = {
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_SETGAMMA, drm_mode_gamma_set_ioctl, DRM_MASTER|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETENCODER, drm_mode_getencoder, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETCONNECTOR, drm_mode_getconnector, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_ATTACHMODE, drm_mode_attachmode_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DETACHMODE, drm_mode_detachmode_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_ATTACHMODE, drm_noop, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DETACHMODE, drm_noop, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETPROPERTY, drm_mode_getproperty_ioctl, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_SETPROPERTY, drm_mode_connector_property_set_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETPROPBLOB, drm_mode_getblob_ioctl, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
@@ -375,7 +375,7 @@ long drm_ioctl(struct file *filp,
 {
 	struct drm_file *file_priv = filp->private_data;
 	struct drm_device *dev;
-	struct drm_ioctl_desc *ioctl;
+	const struct drm_ioctl_desc *ioctl;
 	drm_ioctl_t *func;
 	unsigned int nr = DRM_IOCTL_NR(cmd);
 	int retcode = -EINVAL;
@@ -408,6 +408,7 @@ long drm_ioctl(struct file *filp,
 		usize = asize = _IOC_SIZE(cmd);
 		if (drv_size > asize)
 			asize = drv_size;
+		cmd = ioctl->cmd_drv;
 	}
 	else if ((nr >= DRM_COMMAND_END) || (nr < DRM_COMMAND_BASE)) {
 		ioctl = &drm_ioctls[nr];
diff --git a/drivers/gpu/drm/drm_edid.c b/drivers/gpu/drm/drm_edid.c
index e2acfdbf7d3c..9e62bbedb5ad 100644
--- a/drivers/gpu/drm/drm_edid.c
+++ b/drivers/gpu/drm/drm_edid.c
@@ -587,284 +587,348 @@ static const struct drm_display_mode edid_cea_modes[] = {
 	/* 1 - 640x480@60Hz */
 	{ DRM_MODE("640x480", DRM_MODE_TYPE_DRIVER, 25175, 640, 656,
 		   752, 800, 0, 480, 490, 492, 525, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 60, },
 	/* 2 - 720x480@60Hz */
 	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 27000, 720, 736,
 		   798, 858, 0, 480, 489, 495, 525, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 60, },
 	/* 3 - 720x480@60Hz */
 	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 27000, 720, 736,
 		   798, 858, 0, 480, 489, 495, 525, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 60, },
 	/* 4 - 1280x720@60Hz */
 	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 74250, 1280, 1390,
 		   1430, 1650, 0, 720, 725, 730, 750, 0,
-		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC),
+	  .vrefresh = 60, },
 	/* 5 - 1920x1080i@60Hz */
 	{ DRM_MODE("1920x1080i", DRM_MODE_TYPE_DRIVER, 74250, 1920, 2008,
 		   2052, 2200, 0, 1080, 1084, 1094, 1125, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
+			DRM_MODE_FLAG_INTERLACE),
+	  .vrefresh = 60, },
 	/* 6 - 1440x480i@60Hz */
 	{ DRM_MODE("1440x480i", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
 		   1602, 1716, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 60, },
 	/* 7 - 1440x480i@60Hz */
 	{ DRM_MODE("1440x480i", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
 		   1602, 1716, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 60, },
 	/* 8 - 1440x240@60Hz */
 	{ DRM_MODE("1440x240", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
 		   1602, 1716, 0, 240, 244, 247, 262, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 60, },
 	/* 9 - 1440x240@60Hz */
 	{ DRM_MODE("1440x240", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
 		   1602, 1716, 0, 240, 244, 247, 262, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 60, },
 	/* 10 - 2880x480i@60Hz */
 	{ DRM_MODE("2880x480i", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2956,
 		   3204, 3432, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
+			DRM_MODE_FLAG_INTERLACE),
+	  .vrefresh = 60, },
 	/* 11 - 2880x480i@60Hz */
 	{ DRM_MODE("2880x480i", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2956,
 		   3204, 3432, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
+			DRM_MODE_FLAG_INTERLACE),
+	  .vrefresh = 60, },
 	/* 12 - 2880x240@60Hz */
 	{ DRM_MODE("2880x240", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2956,
 		   3204, 3432, 0, 240, 244, 247, 262, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 60, },
 	/* 13 - 2880x240@60Hz */
 	{ DRM_MODE("2880x240", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2956,
 		   3204, 3432, 0, 240, 244, 247, 262, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 60, },
 	/* 14 - 1440x480@60Hz */
 	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1472,
 		   1596, 1716, 0, 480, 489, 495, 525, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 60, },
 	/* 15 - 1440x480@60Hz */
 	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1472,
 		   1596, 1716, 0, 480, 489, 495, 525, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 60, },
 	/* 16 - 1920x1080@60Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 148500, 1920, 2008,
 		   2052, 2200, 0, 1080, 1084, 1089, 1125, 0,
-		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC),
+	  .vrefresh = 60, },
 	/* 17 - 720x576@50Hz */
 	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 27000, 720, 732,
 		   796, 864, 0, 576, 581, 586, 625, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 50, },
 	/* 18 - 720x576@50Hz */
 	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 27000, 720, 732,
 		   796, 864, 0, 576, 581, 586, 625, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 50, },
 	/* 19 - 1280x720@50Hz */
 	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 74250, 1280, 1720,
 		   1760, 1980, 0, 720, 725, 730, 750, 0,
-		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC),
+	  .vrefresh = 50, },
 	/* 20 - 1920x1080i@50Hz */
 	{ DRM_MODE("1920x1080i", DRM_MODE_TYPE_DRIVER, 74250, 1920, 2448,
 		   2492, 2640, 0, 1080, 1084, 1094, 1125, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
+			DRM_MODE_FLAG_INTERLACE),
+	  .vrefresh = 50, },
 	/* 21 - 1440x576i@50Hz */
 	{ DRM_MODE("1440x576i", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
 		   1590, 1728, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 50, },
 	/* 22 - 1440x576i@50Hz */
 	{ DRM_MODE("1440x576i", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
 		   1590, 1728, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 50, },
 	/* 23 - 1440x288@50Hz */
 	{ DRM_MODE("1440x288", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
 		   1590, 1728, 0, 288, 290, 293, 312, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 50, },
 	/* 24 - 1440x288@50Hz */
 	{ DRM_MODE("1440x288", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
 		   1590, 1728, 0, 288, 290, 293, 312, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 50, },
 	/* 25 - 2880x576i@50Hz */
 	{ DRM_MODE("2880x576i", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2928,
 		   3180, 3456, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
+			DRM_MODE_FLAG_INTERLACE),
+	  .vrefresh = 50, },
 	/* 26 - 2880x576i@50Hz */
 	{ DRM_MODE("2880x576i", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2928,
 		   3180, 3456, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
+			DRM_MODE_FLAG_INTERLACE),
+	  .vrefresh = 50, },
 	/* 27 - 2880x288@50Hz */
 	{ DRM_MODE("2880x288", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2928,
 		   3180, 3456, 0, 288, 290, 293, 312, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 50, },
 	/* 28 - 2880x288@50Hz */
 	{ DRM_MODE("2880x288", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2928,
 		   3180, 3456, 0, 288, 290, 293, 312, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 50, },
 	/* 29 - 1440x576@50Hz */
 	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1464,
 		   1592, 1728, 0, 576, 581, 586, 625, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 50, },
 	/* 30 - 1440x576@50Hz */
 	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1464,
 		   1592, 1728, 0, 576, 581, 586, 625, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 50, },
 	/* 31 - 1920x1080@50Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 148500, 1920, 2448,
 		   2492, 2640, 0, 1080, 1084, 1089, 1125, 0,
-		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC),
+	  .vrefresh = 50, },
 	/* 32 - 1920x1080@24Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 74250, 1920, 2558,
 		   2602, 2750, 0, 1080, 1084, 1089, 1125, 0,
-		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC),
+	  .vrefresh = 24, },
 	/* 33 - 1920x1080@25Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 74250, 1920, 2448,
 		   2492, 2640, 0, 1080, 1084, 1089, 1125, 0,
-		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC),
+	  .vrefresh = 25, },
 	/* 34 - 1920x1080@30Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 74250, 1920, 2008,
 		   2052, 2200, 0, 1080, 1084, 1089, 1125, 0,
-		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC),
+	  .vrefresh = 30, },
 	/* 35 - 2880x480@60Hz */
 	{ DRM_MODE("2880x480", DRM_MODE_TYPE_DRIVER, 108000, 2880, 2944,
 		   3192, 3432, 0, 480, 489, 495, 525, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 60, },
 	/* 36 - 2880x480@60Hz */
 	{ DRM_MODE("2880x480", DRM_MODE_TYPE_DRIVER, 108000, 2880, 2944,
 		   3192, 3432, 0, 480, 489, 495, 525, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 60, },
 	/* 37 - 2880x576@50Hz */
 	{ DRM_MODE("2880x576", DRM_MODE_TYPE_DRIVER, 108000, 2880, 2928,
 		   3184, 3456, 0, 576, 581, 586, 625, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 50, },
 	/* 38 - 2880x576@50Hz */
 	{ DRM_MODE("2880x576", DRM_MODE_TYPE_DRIVER, 108000, 2880, 2928,
 		   3184, 3456, 0, 576, 581, 586, 625, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 50, },
 	/* 39 - 1920x1080i@50Hz */
 	{ DRM_MODE("1920x1080i", DRM_MODE_TYPE_DRIVER, 72000, 1920, 1952,
 		   2120, 2304, 0, 1080, 1126, 1136, 1250, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
+			DRM_MODE_FLAG_INTERLACE),
+	  .vrefresh = 50, },
 	/* 40 - 1920x1080i@100Hz */
 	{ DRM_MODE("1920x1080i", DRM_MODE_TYPE_DRIVER, 148500, 1920, 2448,
 		   2492, 2640, 0, 1080, 1084, 1094, 1125, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
+			DRM_MODE_FLAG_INTERLACE),
+	  .vrefresh = 100, },
 	/* 41 - 1280x720@100Hz */
 	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 148500, 1280, 1720,
 		   1760, 1980, 0, 720, 725, 730, 750, 0,
-		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC),
+	  .vrefresh = 100, },
 	/* 42 - 720x576@100Hz */
 	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 54000, 720, 732,
 		   796, 864, 0, 576, 581, 586, 625, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 100, },
 	/* 43 - 720x576@100Hz */
 	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 54000, 720, 732,
 		   796, 864, 0, 576, 581, 586, 625, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 100, },
 	/* 44 - 1440x576i@100Hz */
 	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1464,
 		   1590, 1728, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 100, },
 	/* 45 - 1440x576i@100Hz */
 	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1464,
 		   1590, 1728, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 100, },
 	/* 46 - 1920x1080i@120Hz */
 	{ DRM_MODE("1920x1080i", DRM_MODE_TYPE_DRIVER, 148500, 1920, 2008,
 		   2052, 2200, 0, 1080, 1084, 1094, 1125, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC |
-			DRM_MODE_FLAG_INTERLACE) },
+			DRM_MODE_FLAG_INTERLACE),
+	  .vrefresh = 120, },
 	/* 47 - 1280x720@120Hz */
 	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 148500, 1280, 1390,
 		   1430, 1650, 0, 720, 725, 730, 750, 0,
-		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC),
+	  .vrefresh = 120, },
 	/* 48 - 720x480@120Hz */
 	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 54000, 720, 736,
 		   798, 858, 0, 480, 489, 495, 525, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 120, },
 	/* 49 - 720x480@120Hz */
 	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 54000, 720, 736,
 		   798, 858, 0, 480, 489, 495, 525, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 120, },
 	/* 50 - 1440x480i@120Hz */
 	{ DRM_MODE("1440x480i", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1478,
 		   1602, 1716, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 120, },
 	/* 51 - 1440x480i@120Hz */
 	{ DRM_MODE("1440x480i", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1478,
 		   1602, 1716, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 120, },
 	/* 52 - 720x576@200Hz */
 	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 108000, 720, 732,
 		   796, 864, 0, 576, 581, 586, 625, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 200, },
 	/* 53 - 720x576@200Hz */
 	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 108000, 720, 732,
 		   796, 864, 0, 576, 581, 586, 625, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 200, },
 	/* 54 - 1440x576i@200Hz */
 	{ DRM_MODE("1440x576i", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1464,
 		   1590, 1728, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 200, },
 	/* 55 - 1440x576i@200Hz */
 	{ DRM_MODE("1440x576i", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1464,
 		   1590, 1728, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 200, },
 	/* 56 - 720x480@240Hz */
 	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 108000, 720, 736,
 		   798, 858, 0, 480, 489, 495, 525, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 240, },
 	/* 57 - 720x480@240Hz */
 	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 108000, 720, 736,
 		   798, 858, 0, 480, 489, 495, 525, 0,
-		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
+	  .vrefresh = 240, },
 	/* 58 - 1440x480i@240 */
 	{ DRM_MODE("1440x480i", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1478,
 		   1602, 1716, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 240, },
 	/* 59 - 1440x480i@240 */
 	{ DRM_MODE("1440x480i", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1478,
 		   1602, 1716, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK) },
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
+	  .vrefresh = 240, },
 	/* 60 - 1280x720@24Hz */
 	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 59400, 1280, 3040,
 		   3080, 3300, 0, 720, 725, 730, 750, 0,
-		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC),
+	  .vrefresh = 24, },
 	/* 61 - 1280x720@25Hz */
 	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 74250, 1280, 3700,
 		   3740, 3960, 0, 720, 725, 730, 750, 0,
-		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC),
+	  .vrefresh = 25, },
 	/* 62 - 1280x720@30Hz */
 	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 74250, 1280, 3040,
 		   3080, 3300, 0, 720, 725, 730, 750, 0,
-		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC),
+	  .vrefresh = 30, },
 	/* 63 - 1920x1080@120Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 297000, 1920, 2008,
 		   2052, 2200, 0, 1080, 1084, 1089, 1125, 0,
-		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC),
+	 .vrefresh = 120, },
 	/* 64 - 1920x1080@100Hz */
 	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 297000, 1920, 2448,
 		   2492, 2640, 0, 1080, 1084, 1094, 1125, 0,
-		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC),
+	 .vrefresh = 100, },
 };
 
 /*** DDC fetch and block validation ***/
@@ -2266,13 +2330,34 @@ EXPORT_SYMBOL(drm_find_cea_extension);
  */
 u8 drm_match_cea_mode(const struct drm_display_mode *to_match)
 {
-	struct drm_display_mode *cea_mode;
 	u8 mode;
 
+	if (!to_match->clock)
+		return 0;
+
 	for (mode = 0; mode < ARRAY_SIZE(edid_cea_modes); mode++) {
-		cea_mode = (struct drm_display_mode *)&edid_cea_modes[mode];
+		const struct drm_display_mode *cea_mode = &edid_cea_modes[mode];
+		unsigned int clock1, clock2;
+
+		clock1 = clock2 = cea_mode->clock;
 
-		if (drm_mode_equal(to_match, cea_mode))
+		/* Check both 60Hz and 59.94Hz */
+		if (cea_mode->vrefresh % 6 == 0) {
+			/*
+			 * edid_cea_modes contains the 59.94Hz
+			 * variant for 240 and 480 line modes,
+			 * and the 60Hz variant otherwise.
+			 */
+			if (cea_mode->vdisplay == 240 ||
+			    cea_mode->vdisplay == 480)
+				clock1 = clock1 * 1001 / 1000;
+			else
+				clock2 = DIV_ROUND_UP(clock2 * 1000, 1001);
+		}
+
+		if ((KHZ2PICOS(to_match->clock) == KHZ2PICOS(clock1) ||
+		     KHZ2PICOS(to_match->clock) == KHZ2PICOS(clock2)) &&
+		    drm_mode_equal_no_clocks(to_match, cea_mode))
 			return mode + 1;
 	}
 	return 0;
@@ -2294,6 +2379,7 @@ do_cea_modes (struct drm_connector *connector, u8 *db, u8 len)
 			newmode = drm_mode_duplicate(dev,
 						     &edid_cea_modes[cea_mode]);
 			if (newmode) {
+				newmode->vrefresh = 0;
 				drm_mode_probed_add(connector, newmode);
 				modes++;
 			}
@@ -2511,6 +2597,65 @@ void drm_edid_to_eld(struct drm_connector *connector, struct edid *edid)
 EXPORT_SYMBOL(drm_edid_to_eld);
 
 /**
+ * drm_edid_to_sad - extracts SADs from EDID
+ * @edid: EDID to parse
+ * @sads: pointer that will be set to the extracted SADs
+ *
+ * Looks for CEA EDID block and extracts SADs (Short Audio Descriptors) from it.
+ * Note: returned pointer needs to be kfreed
+ *
+ * Return number of found SADs or negative number on error.
+ */
+int drm_edid_to_sad(struct edid *edid, struct cea_sad **sads)
+{
+	int count = 0;
+	int i, start, end, dbl;
+	u8 *cea;
+
+	cea = drm_find_cea_extension(edid);
+	if (!cea) {
+		DRM_DEBUG_KMS("SAD: no CEA Extension found\n");
+		return -ENOENT;
+	}
+
+	if (cea_revision(cea) < 3) {
+		DRM_DEBUG_KMS("SAD: wrong CEA revision\n");
+		return -ENOTSUPP;
+	}
+
+	if (cea_db_offsets(cea, &start, &end)) {
+		DRM_DEBUG_KMS("SAD: invalid data block offsets\n");
+		return -EPROTO;
+	}
+
+	for_each_cea_db(cea, i, start, end) {
+		u8 *db = &cea[i];
+
+		if (cea_db_tag(db) == AUDIO_BLOCK) {
+			int j;
+			dbl = cea_db_payload_len(db);
+
+			count = dbl / 3; /* SAD is 3B */
+			*sads = kcalloc(count, sizeof(**sads), GFP_KERNEL);
+			if (!*sads)
+				return -ENOMEM;
+			for (j = 0; j < count; j++) {
+				u8 *sad = &db[1 + j * 3];
+
+				(*sads)[j].format = (sad[0] & 0x78) >> 3;
+				(*sads)[j].channels = sad[0] & 0x7;
+				(*sads)[j].freq = sad[1] & 0x7F;
+				(*sads)[j].byte2 = sad[2];
+			}
+			break;
+		}
+	}
+
+	return count;
+}
+EXPORT_SYMBOL(drm_edid_to_sad);
+
+/**
  * drm_av_sync_delay - HDMI/DP sink audio-video sync delay in millisecond
  * @connector: connector associated with the HDMI/DP sink
  * @mode: the display mode
diff --git a/drivers/gpu/drm/drm_edid_load.c b/drivers/gpu/drm/drm_edid_load.c
index 38d3943f72de..fa445dd4dc00 100644
--- a/drivers/gpu/drm/drm_edid_load.c
+++ b/drivers/gpu/drm/drm_edid_load.c
@@ -31,10 +31,11 @@ module_param_string(edid_firmware, edid_firmware, sizeof(edid_firmware), 0644);
 MODULE_PARM_DESC(edid_firmware, "Do not probe monitor, use specified EDID blob "
 	"from built-in data or /lib/firmware instead. ");
 
-#define GENERIC_EDIDS 4
+#define GENERIC_EDIDS 5
 static char *generic_edid_name[GENERIC_EDIDS] = {
 	"edid/1024x768.bin",
 	"edid/1280x1024.bin",
+	"edid/1600x1200.bin",
 	"edid/1680x1050.bin",
 	"edid/1920x1080.bin",
 };
@@ -79,6 +80,24 @@ static u8 generic_edid[GENERIC_EDIDS][128] = {
 	{
 	0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00,
 	0x31, 0xd8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x05, 0x16, 0x01, 0x03, 0x6d, 0x37, 0x29, 0x78,
+	0xea, 0x5e, 0xc0, 0xa4, 0x59, 0x4a, 0x98, 0x25,
+	0x20, 0x50, 0x54, 0x00, 0x00, 0x00, 0xa9, 0x40,
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x48, 0x3f,
+	0x40, 0x30, 0x62, 0xb0, 0x32, 0x40, 0x40, 0xc0,
+	0x13, 0x00, 0x2b, 0xa0, 0x21, 0x00, 0x00, 0x1e,
+	0x00, 0x00, 0x00, 0xff, 0x00, 0x4c, 0x69, 0x6e,
+	0x75, 0x78, 0x20, 0x23, 0x30, 0x0a, 0x20, 0x20,
+	0x20, 0x20, 0x00, 0x00, 0x00, 0xfd, 0x00, 0x3b,
+	0x3d, 0x4a, 0x4c, 0x11, 0x00, 0x0a, 0x20, 0x20,
+	0x20, 0x20, 0x20, 0x20, 0x00, 0x00, 0x00, 0xfc,
+	0x00, 0x4c, 0x69, 0x6e, 0x75, 0x78, 0x20, 0x55,
+	0x58, 0x47, 0x41, 0x0a, 0x20, 0x20, 0x00, 0x9d,
+	},
+	{
+	0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00,
+	0x31, 0xd8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	0x05, 0x16, 0x01, 0x03, 0x6d, 0x2b, 0x1b, 0x78,
 	0xea, 0x5e, 0xc0, 0xa4, 0x59, 0x4a, 0x98, 0x25,
 	0x20, 0x50, 0x54, 0x00, 0x00, 0x00, 0xb3, 0x00,
diff --git a/drivers/gpu/drm/drm_fb_helper.c b/drivers/gpu/drm/drm_fb_helper.c
index 892ff9f95975..b78cbe74dadf 100644
--- a/drivers/gpu/drm/drm_fb_helper.c
+++ b/drivers/gpu/drm/drm_fb_helper.c
@@ -1398,7 +1398,7 @@ static void drm_setup_crtcs(struct drm_fb_helper *fb_helper)
 	struct drm_mode_set *modeset;
 	bool *enabled;
 	int width, height;
-	int i, ret;
+	int i;
 
 	DRM_DEBUG_KMS("\n");
 
@@ -1419,16 +1419,23 @@ static void drm_setup_crtcs(struct drm_fb_helper *fb_helper)
 
 	drm_enable_connectors(fb_helper, enabled);
 
-	ret = drm_target_cloned(fb_helper, modes, enabled, width, height);
-	if (!ret) {
-		ret = drm_target_preferred(fb_helper, modes, enabled, width, height);
-		if (!ret)
+	if (!(fb_helper->funcs->initial_config &&
+	      fb_helper->funcs->initial_config(fb_helper, crtcs, modes,
+					       enabled, width, height))) {
+		memset(modes, 0, dev->mode_config.num_connector*sizeof(modes[0]));
+		memset(crtcs, 0, dev->mode_config.num_connector*sizeof(crtcs[0]));
+
+		if (!drm_target_cloned(fb_helper,
+				       modes, enabled, width, height) &&
+		    !drm_target_preferred(fb_helper,
+					  modes, enabled, width, height))
 			DRM_ERROR("Unable to find initial modes\n");
-	}
 
-	DRM_DEBUG_KMS("picking CRTCs for %dx%d config\n", width, height);
+		DRM_DEBUG_KMS("picking CRTCs for %dx%d config\n",
+			      width, height);
 
-	drm_pick_crtcs(fb_helper, crtcs, modes, 0, width, height);
+		drm_pick_crtcs(fb_helper, crtcs, modes, 0, width, height);
+	}
 
 	/* need to set the modesets up here for use later */
 	/* fill out the connector<->crtc mappings into the modesets */
diff --git a/drivers/gpu/drm/drm_gem.c b/drivers/gpu/drm/drm_gem.c
index af779ae19ebf..cf919e36e8ae 100644
--- a/drivers/gpu/drm/drm_gem.c
+++ b/drivers/gpu/drm/drm_gem.c
@@ -205,11 +205,11 @@ static void
 drm_gem_remove_prime_handles(struct drm_gem_object *obj, struct drm_file *filp)
 {
 	if (obj->import_attach) {
-		drm_prime_remove_imported_buf_handle(&filp->prime,
+		drm_prime_remove_buf_handle(&filp->prime,
 				obj->import_attach->dmabuf);
 	}
 	if (obj->export_dma_buf) {
-		drm_prime_remove_imported_buf_handle(&filp->prime,
+		drm_prime_remove_buf_handle(&filp->prime,
 				obj->export_dma_buf);
 	}
 }
diff --git a/drivers/gpu/drm/drm_modes.c b/drivers/gpu/drm/drm_modes.c
index f83f0719922e..faa79df02648 100644
--- a/drivers/gpu/drm/drm_modes.c
+++ b/drivers/gpu/drm/drm_modes.c
@@ -848,6 +848,26 @@ bool drm_mode_equal(const struct drm_display_mode *mode1, const struct drm_displ
 	} else if (mode1->clock != mode2->clock)
 		return false;
 
+	return drm_mode_equal_no_clocks(mode1, mode2);
+}
+EXPORT_SYMBOL(drm_mode_equal);
+
+/**
+ * drm_mode_equal_no_clocks - test modes for equality
+ * @mode1: first mode
+ * @mode2: second mode
+ *
+ * LOCKING:
+ * None.
+ *
+ * Check to see if @mode1 and @mode2 are equivalent, but
+ * don't check the pixel clocks.
+ *
+ * RETURNS:
+ * True if the modes are equal, false otherwise.
+ */
+bool drm_mode_equal_no_clocks(const struct drm_display_mode *mode1, const struct drm_display_mode *mode2)
+{
 	if (mode1->hdisplay == mode2->hdisplay &&
 	    mode1->hsync_start == mode2->hsync_start &&
 	    mode1->hsync_end == mode2->hsync_end &&
@@ -863,7 +883,7 @@ bool drm_mode_equal(const struct drm_display_mode *mode1, const struct drm_displ
 
 	return false;
 }
-EXPORT_SYMBOL(drm_mode_equal);
+EXPORT_SYMBOL(drm_mode_equal_no_clocks);
 
 /**
  * drm_mode_validate_size - make sure modes adhere to size constraints
diff --git a/drivers/gpu/drm/drm_pci.c b/drivers/gpu/drm/drm_pci.c
index bd719e936e13..14194b6ef644 100644
--- a/drivers/gpu/drm/drm_pci.c
+++ b/drivers/gpu/drm/drm_pci.c
@@ -152,7 +152,7 @@ static const char *drm_pci_get_name(struct drm_device *dev)
 	return pdriver->name;
 }
 
-int drm_pci_set_busid(struct drm_device *dev, struct drm_master *master)
+static int drm_pci_set_busid(struct drm_device *dev, struct drm_master *master)
 {
 	int len, ret;
 	struct pci_driver *pdriver = dev->driver->kdriver.pci;
@@ -194,9 +194,9 @@ err:
 	return ret;
 }
 
-int drm_pci_set_unique(struct drm_device *dev,
-		       struct drm_master *master,
-		       struct drm_unique *u)
+static int drm_pci_set_unique(struct drm_device *dev,
+			      struct drm_master *master,
+			      struct drm_unique *u)
 {
 	int domain, bus, slot, func, ret;
 	const char *bus_name;
@@ -266,7 +266,7 @@ static int drm_pci_irq_by_busid(struct drm_device *dev, struct drm_irq_busid *p)
 	return 0;
 }
 
-int drm_pci_agp_init(struct drm_device *dev)
+static int drm_pci_agp_init(struct drm_device *dev)
 {
 	if (drm_core_has_AGP(dev)) {
 		if (drm_pci_device_is_agp(dev))
diff --git a/drivers/gpu/drm/drm_prime.c b/drivers/gpu/drm/drm_prime.c
index 366910ddcfcb..dcde35231e25 100644
--- a/drivers/gpu/drm/drm_prime.c
+++ b/drivers/gpu/drm/drm_prime.c
@@ -62,6 +62,7 @@ struct drm_prime_member {
 	struct dma_buf *dma_buf;
 	uint32_t handle;
 };
+static int drm_prime_add_buf_handle(struct drm_prime_file_private *prime_fpriv, struct dma_buf *dma_buf, uint32_t handle);
 
 static struct sg_table *drm_gem_map_dma_buf(struct dma_buf_attachment *attach,
 		enum dma_data_direction dir)
@@ -200,7 +201,8 @@ int drm_gem_prime_handle_to_fd(struct drm_device *dev,
 {
 	struct drm_gem_object *obj;
 	void *buf;
-	int ret;
+	int ret = 0;
+	struct dma_buf *dmabuf;
 
 	obj = drm_gem_object_lookup(dev, file_priv, handle);
 	if (!obj)
@@ -209,43 +211,44 @@ int drm_gem_prime_handle_to_fd(struct drm_device *dev,
 	mutex_lock(&file_priv->prime.lock);
 	/* re-export the original imported object */
 	if (obj->import_attach) {
-		get_dma_buf(obj->import_attach->dmabuf);
-		*prime_fd = dma_buf_fd(obj->import_attach->dmabuf, flags);
-		drm_gem_object_unreference_unlocked(obj);
-		mutex_unlock(&file_priv->prime.lock);
-		return 0;
+		dmabuf = obj->import_attach->dmabuf;
+		goto out_have_obj;
 	}
 
 	if (obj->export_dma_buf) {
-		get_dma_buf(obj->export_dma_buf);
-		*prime_fd = dma_buf_fd(obj->export_dma_buf, flags);
-		drm_gem_object_unreference_unlocked(obj);
-	} else {
-		buf = dev->driver->gem_prime_export(dev, obj, flags);
-		if (IS_ERR(buf)) {
-			/* normally the created dma-buf takes ownership of the ref,
-			 * but if that fails then drop the ref
-			 */
-			drm_gem_object_unreference_unlocked(obj);
-			mutex_unlock(&file_priv->prime.lock);
-			return PTR_ERR(buf);
-		}
-		obj->export_dma_buf = buf;
-		*prime_fd = dma_buf_fd(buf, flags);
+		dmabuf = obj->export_dma_buf;
+		goto out_have_obj;
+	}
+
+	buf = dev->driver->gem_prime_export(dev, obj, flags);
+	if (IS_ERR(buf)) {
+		/* normally the created dma-buf takes ownership of the ref,
+		 * but if that fails then drop the ref
+		 */
+		ret = PTR_ERR(buf);
+		goto out;
 	}
+	obj->export_dma_buf = buf;
+
 	/* if we've exported this buffer the cheat and add it to the import list
 	 * so we get the correct handle back
 	 */
-	ret = drm_prime_add_imported_buf_handle(&file_priv->prime,
-			obj->export_dma_buf, handle);
-	if (ret) {
-		drm_gem_object_unreference_unlocked(obj);
-		mutex_unlock(&file_priv->prime.lock);
-		return ret;
-	}
+	ret = drm_prime_add_buf_handle(&file_priv->prime,
+				       obj->export_dma_buf, handle);
+	if (ret)
+		goto out;
 
+	*prime_fd = dma_buf_fd(buf, flags);
 	mutex_unlock(&file_priv->prime.lock);
 	return 0;
+
+out_have_obj:
+	get_dma_buf(dmabuf);
+	*prime_fd = dma_buf_fd(dmabuf, flags);
+out:
+	drm_gem_object_unreference_unlocked(obj);
+	mutex_unlock(&file_priv->prime.lock);
+	return ret;
 }
 EXPORT_SYMBOL(drm_gem_prime_handle_to_fd);
 
@@ -268,7 +271,6 @@ struct drm_gem_object *drm_gem_prime_import(struct drm_device *dev,
 			 * refcount on gem itself instead of f_count of dmabuf.
 			 */
 			drm_gem_object_reference(obj);
-			dma_buf_put(dma_buf);
 			return obj;
 		}
 	}
@@ -277,6 +279,8 @@ struct drm_gem_object *drm_gem_prime_import(struct drm_device *dev,
 	if (IS_ERR(attach))
 		return ERR_PTR(PTR_ERR(attach));
 
+	get_dma_buf(dma_buf);
+
 	sgt = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
 	if (IS_ERR_OR_NULL(sgt)) {
 		ret = PTR_ERR(sgt);
@@ -297,6 +301,8 @@ fail_unmap:
 	dma_buf_unmap_attachment(attach, sgt, DMA_BIDIRECTIONAL);
 fail_detach:
 	dma_buf_detach(dma_buf, attach);
+	dma_buf_put(dma_buf);
+
 	return ERR_PTR(ret);
 }
 EXPORT_SYMBOL(drm_gem_prime_import);
@@ -314,7 +320,7 @@ int drm_gem_prime_fd_to_handle(struct drm_device *dev,
 
 	mutex_lock(&file_priv->prime.lock);
 
-	ret = drm_prime_lookup_imported_buf_handle(&file_priv->prime,
+	ret = drm_prime_lookup_buf_handle(&file_priv->prime,
 			dma_buf, handle);
 	if (!ret) {
 		ret = 0;
@@ -333,12 +339,15 @@ int drm_gem_prime_fd_to_handle(struct drm_device *dev,
 	if (ret)
 		goto out_put;
 
-	ret = drm_prime_add_imported_buf_handle(&file_priv->prime,
+	ret = drm_prime_add_buf_handle(&file_priv->prime,
 			dma_buf, *handle);
 	if (ret)
 		goto fail;
 
 	mutex_unlock(&file_priv->prime.lock);
+
+	dma_buf_put(dma_buf);
+
 	return 0;
 
 fail:
@@ -401,21 +410,17 @@ int drm_prime_fd_to_handle_ioctl(struct drm_device *dev, void *data,
 struct sg_table *drm_prime_pages_to_sg(struct page **pages, int nr_pages)
 {
 	struct sg_table *sg = NULL;
-	struct scatterlist *iter;
-	int i;
 	int ret;
 
 	sg = kmalloc(sizeof(struct sg_table), GFP_KERNEL);
 	if (!sg)
 		goto out;
 
-	ret = sg_alloc_table(sg, nr_pages, GFP_KERNEL);
+	ret = sg_alloc_table_from_pages(sg, pages, nr_pages, 0,
+				nr_pages << PAGE_SHIFT, GFP_KERNEL);
 	if (ret)
 		goto out;
 
-	for_each_sg(sg->sgl, iter, nr_pages, i)
-		sg_set_page(iter, pages[i], PAGE_SIZE, 0);
-
 	return sg;
 out:
 	kfree(sg);
@@ -483,15 +488,12 @@ EXPORT_SYMBOL(drm_prime_init_file_private);
 
 void drm_prime_destroy_file_private(struct drm_prime_file_private *prime_fpriv)
 {
-	struct drm_prime_member *member, *safe;
-	list_for_each_entry_safe(member, safe, &prime_fpriv->head, entry) {
-		list_del(&member->entry);
-		kfree(member);
-	}
+	/* by now drm_gem_release should've made sure the list is empty */
+	WARN_ON(!list_empty(&prime_fpriv->head));
 }
 EXPORT_SYMBOL(drm_prime_destroy_file_private);
 
-int drm_prime_add_imported_buf_handle(struct drm_prime_file_private *prime_fpriv, struct dma_buf *dma_buf, uint32_t handle)
+static int drm_prime_add_buf_handle(struct drm_prime_file_private *prime_fpriv, struct dma_buf *dma_buf, uint32_t handle)
 {
 	struct drm_prime_member *member;
 
@@ -499,14 +501,14 @@ int drm_prime_add_imported_buf_handle(struct drm_prime_file_private *prime_fpriv
 	if (!member)
 		return -ENOMEM;
 
+	get_dma_buf(dma_buf);
 	member->dma_buf = dma_buf;
 	member->handle = handle;
 	list_add(&member->entry, &prime_fpriv->head);
 	return 0;
 }
-EXPORT_SYMBOL(drm_prime_add_imported_buf_handle);
 
-int drm_prime_lookup_imported_buf_handle(struct drm_prime_file_private *prime_fpriv, struct dma_buf *dma_buf, uint32_t *handle)
+int drm_prime_lookup_buf_handle(struct drm_prime_file_private *prime_fpriv, struct dma_buf *dma_buf, uint32_t *handle)
 {
 	struct drm_prime_member *member;
 
@@ -518,19 +520,20 @@ int drm_prime_lookup_imported_buf_handle(struct drm_prime_file_private *prime_fp
 	}
 	return -ENOENT;
 }
-EXPORT_SYMBOL(drm_prime_lookup_imported_buf_handle);
+EXPORT_SYMBOL(drm_prime_lookup_buf_handle);
 
-void drm_prime_remove_imported_buf_handle(struct drm_prime_file_private *prime_fpriv, struct dma_buf *dma_buf)
+void drm_prime_remove_buf_handle(struct drm_prime_file_private *prime_fpriv, struct dma_buf *dma_buf)
 {
 	struct drm_prime_member *member, *safe;
 
 	mutex_lock(&prime_fpriv->lock);
 	list_for_each_entry_safe(member, safe, &prime_fpriv->head, entry) {
 		if (member->dma_buf == dma_buf) {
+			dma_buf_put(dma_buf);
 			list_del(&member->entry);
 			kfree(member);
 		}
 	}
 	mutex_unlock(&prime_fpriv->lock);
 }
-EXPORT_SYMBOL(drm_prime_remove_imported_buf_handle);
+EXPORT_SYMBOL(drm_prime_remove_buf_handle);
diff --git a/drivers/gpu/drm/drm_proc.c b/drivers/gpu/drm/drm_proc.c
index ff5456b7df72..d7f2324b4fb1 100644
--- a/drivers/gpu/drm/drm_proc.c
+++ b/drivers/gpu/drm/drm_proc.c
@@ -49,7 +49,7 @@
 /**
  * Proc file list.
  */
-static struct drm_info_list drm_proc_list[] = {
+static const struct drm_info_list drm_proc_list[] = {
 	{"name", drm_name_info, 0},
 	{"vm", drm_vm_info, 0},
 	{"clients", drm_clients_info, 0},
@@ -63,7 +63,7 @@ static struct drm_info_list drm_proc_list[] = {
 
 static int drm_proc_open(struct inode *inode, struct file *file)
 {
-	struct drm_info_node* node = PDE(inode)->data;
+	struct drm_info_node* node = PDE_DATA(inode);
 
 	return single_open(file, node->info_ent->show, node);
 }
@@ -89,13 +89,13 @@ static const struct file_operations drm_proc_fops = {
  * Create a given set of proc files represented by an array of
  * gdm_proc_lists in the given root directory.
  */
-static int drm_proc_create_files(struct drm_info_list *files, int count,
+static int drm_proc_create_files(const struct drm_info_list *files, int count,
 			  struct proc_dir_entry *root, struct drm_minor *minor)
 {
 	struct drm_device *dev = minor->dev;
 	struct proc_dir_entry *ent;
 	struct drm_info_node *tmp;
-	int i, ret;
+	int i;
 
 	for (i = 0; i < count; i++) {
 		u32 features = files[i].driver_features;
@@ -105,10 +105,9 @@ static int drm_proc_create_files(struct drm_info_list *files, int count,
 			continue;
 
 		tmp = kmalloc(sizeof(struct drm_info_node), GFP_KERNEL);
-		if (tmp == NULL) {
-			ret = -1;
-			goto fail;
-		}
+		if (!tmp)
+			return -1;
+
 		tmp->minor = minor;
 		tmp->info_ent = &files[i];
 		list_add(&tmp->list, &minor->proc_nodes.list);
@@ -116,28 +115,20 @@ static int drm_proc_create_files(struct drm_info_list *files, int count,
 		ent = proc_create_data(files[i].name, S_IRUGO, root,
 				       &drm_proc_fops, tmp);
 		if (!ent) {
-			DRM_ERROR("Cannot create /proc/dri/%s/%s\n",
-				  root->name, files[i].name);
+			DRM_ERROR("Cannot create /proc/dri/%u/%s\n",
+				  minor->index, files[i].name);
 			list_del(&tmp->list);
 			kfree(tmp);
-			ret = -1;
-			goto fail;
+			return -1;
 		}
-
 	}
 	return 0;
-
-fail:
-	for (i = 0; i < count; i++)
-		remove_proc_entry(drm_proc_list[i].name, minor->proc_root);
-	return ret;
 }
 
 /**
  * Initialize the DRI proc filesystem for a device
  *
  * \param dev DRM device
- * \param minor device minor number
  * \param root DRI proc dir entry.
  * \param dev_root resulting DRI device proc dir entry.
  * \return root entry pointer on success, or NULL on failure.
@@ -146,14 +137,13 @@ fail:
  * "/proc/dri/%minor%/", and each entry in proc_list as
  * "/proc/dri/%minor%/%name%".
  */
-int drm_proc_init(struct drm_minor *minor, int minor_id,
-		  struct proc_dir_entry *root)
+int drm_proc_init(struct drm_minor *minor, struct proc_dir_entry *root)
 {
-	char name[64];
+	char name[12];
 	int ret;
 
 	INIT_LIST_HEAD(&minor->proc_nodes.list);
-	sprintf(name, "%d", minor_id);
+	sprintf(name, "%u", minor->index);
 	minor->proc_root = proc_mkdir(name, root);
 	if (!minor->proc_root) {
 		DRM_ERROR("Cannot create /proc/dri/%s\n", name);
@@ -163,7 +153,7 @@ int drm_proc_init(struct drm_minor *minor, int minor_id,
 	ret = drm_proc_create_files(drm_proc_list, DRM_PROC_ENTRIES,
 				    minor->proc_root, minor);
 	if (ret) {
-		remove_proc_entry(name, root);
+		remove_proc_subtree(name, root);
 		minor->proc_root = NULL;
 		DRM_ERROR("Failed to create core drm proc files\n");
 		return ret;
@@ -172,7 +162,7 @@ int drm_proc_init(struct drm_minor *minor, int minor_id,
 	return 0;
 }
 
-static int drm_proc_remove_files(struct drm_info_list *files, int count,
+static int drm_proc_remove_files(const struct drm_info_list *files, int count,
 			  struct drm_minor *minor)
 {
 	struct list_head *pos, *q;
@@ -213,8 +203,7 @@ int drm_proc_cleanup(struct drm_minor *minor, struct proc_dir_entry *root)
 	drm_proc_remove_files(drm_proc_list, DRM_PROC_ENTRIES, minor);
 
 	sprintf(name, "%d", minor->index);
-	remove_proc_entry(name, root);
-
+	remove_proc_subtree(name, root);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/drm_stub.c b/drivers/gpu/drm/drm_stub.c
index 7d30802a018f..16f3ec579b3b 100644
--- a/drivers/gpu/drm/drm_stub.c
+++ b/drivers/gpu/drm/drm_stub.c
@@ -352,7 +352,7 @@ int drm_get_minor(struct drm_device *dev, struct drm_minor **minor, int type)
 	idr_replace(&drm_minors_idr, new_minor, minor_id);
 
 	if (type == DRM_MINOR_LEGACY) {
-		ret = drm_proc_init(new_minor, minor_id, drm_proc_root);
+		ret = drm_proc_init(new_minor, drm_proc_root);
 		if (ret) {
 			DRM_ERROR("DRM: Failed to initialize /proc/dri.\n");
 			goto err_mem;
diff --git a/drivers/gpu/drm/drm_vm.c b/drivers/gpu/drm/drm_vm.c
index db7bd292410b..1d4f7c9fe661 100644
--- a/drivers/gpu/drm/drm_vm.c
+++ b/drivers/gpu/drm/drm_vm.c
@@ -422,6 +422,7 @@ void drm_vm_open_locked(struct drm_device *dev,
 		list_add(&vma_entry->head, &dev->vmalist);
 	}
 }
+EXPORT_SYMBOL_GPL(drm_vm_open_locked);
 
 static void drm_vm_open(struct vm_area_struct *vma)
 {
diff --git a/drivers/gpu/drm/exynos/Kconfig b/drivers/gpu/drm/exynos/Kconfig
index 046bcda36abe..772c62a6e2ac 100644
--- a/drivers/gpu/drm/exynos/Kconfig
+++ b/drivers/gpu/drm/exynos/Kconfig
@@ -24,7 +24,9 @@ config DRM_EXYNOS_DMABUF
 
 config DRM_EXYNOS_FIMD
 	bool "Exynos DRM FIMD"
-	depends on DRM_EXYNOS && !FB_S3C && !ARCH_MULTIPLATFORM
+	depends on OF && DRM_EXYNOS && !FB_S3C && !ARCH_MULTIPLATFORM
+	select FB_MODE_HELPERS
+	select VIDEOMODE_HELPERS
 	help
 	  Choose this option if you want to use Exynos FIMD for DRM.
 
@@ -54,7 +56,7 @@ config DRM_EXYNOS_IPP
 
 config DRM_EXYNOS_FIMC
 	bool "Exynos DRM FIMC"
-	depends on DRM_EXYNOS_IPP
+	depends on DRM_EXYNOS_IPP && MFD_SYSCON && OF
 	help
 	  Choose this option if you want to use Exynos FIMC for DRM.
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_connector.c b/drivers/gpu/drm/exynos/exynos_drm_connector.c
index 4c5b6859c9ea..8bcc13ac9f73 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_connector.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_connector.c
@@ -124,7 +124,7 @@ static int exynos_drm_connector_get_modes(struct drm_connector *connector)
 		}
 
 		count = drm_add_edid_modes(connector, edid);
-		if (count < 0) {
+		if (!count) {
 			DRM_ERROR("Add edid modes failed %d\n", count);
 			goto out;
 		}
diff --git a/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c b/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c
index ba0a3aa78547..ff7f2a886a34 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c
@@ -235,7 +235,6 @@ struct drm_gem_object *exynos_dmabuf_prime_import(struct drm_device *drm_dev,
 			 * refcount on gem itself instead of f_count of dmabuf.
 			 */
 			drm_gem_object_reference(obj);
-			dma_buf_put(dma_buf);
 			return obj;
 		}
 	}
@@ -244,6 +243,7 @@ struct drm_gem_object *exynos_dmabuf_prime_import(struct drm_device *drm_dev,
 	if (IS_ERR(attach))
 		return ERR_PTR(-EINVAL);
 
+	get_dma_buf(dma_buf);
 
 	sgt = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
 	if (IS_ERR_OR_NULL(sgt)) {
@@ -298,6 +298,8 @@ err_unmap_attach:
 	dma_buf_unmap_attachment(attach, sgt, DMA_BIDIRECTIONAL);
 err_buf_detach:
 	dma_buf_detach(dma_buf, attach);
+	dma_buf_put(dma_buf);
+
 	return ERR_PTR(ret);
 }
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.c b/drivers/gpu/drm/exynos/exynos_drm_drv.c
index 3da5c2d214d8..ba6d995e4375 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.c
@@ -380,6 +380,10 @@ static int __init exynos_drm_init(void)
 	ret = platform_driver_register(&ipp_driver);
 	if (ret < 0)
 		goto out_ipp;
+
+	ret = exynos_platform_device_ipp_register();
+	if (ret < 0)
+		goto out_ipp_dev;
 #endif
 
 	ret = platform_driver_register(&exynos_drm_platform_driver);
@@ -388,7 +392,7 @@ static int __init exynos_drm_init(void)
 
 	exynos_drm_pdev = platform_device_register_simple("exynos-drm", -1,
 				NULL, 0);
-	if (IS_ERR_OR_NULL(exynos_drm_pdev)) {
+	if (IS_ERR(exynos_drm_pdev)) {
 		ret = PTR_ERR(exynos_drm_pdev);
 		goto out;
 	}
@@ -400,6 +404,8 @@ out:
 
 out_drm:
 #ifdef CONFIG_DRM_EXYNOS_IPP
+	exynos_platform_device_ipp_unregister();
+out_ipp_dev:
 	platform_driver_unregister(&ipp_driver);
 out_ipp:
 #endif
@@ -456,6 +462,7 @@ static void __exit exynos_drm_exit(void)
 	platform_driver_unregister(&exynos_drm_platform_driver);
 
 #ifdef CONFIG_DRM_EXYNOS_IPP
+	exynos_platform_device_ipp_unregister();
 	platform_driver_unregister(&ipp_driver);
 #endif
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.h b/drivers/gpu/drm/exynos/exynos_drm_drv.h
index 4606fac7241a..680a7c1b9dea 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.h
@@ -322,13 +322,23 @@ void exynos_drm_subdrv_close(struct drm_device *dev, struct drm_file *file);
  * this function registers exynos drm hdmi platform device. It ensures only one
  * instance of the device is created.
  */
-extern int exynos_platform_device_hdmi_register(void);
+int exynos_platform_device_hdmi_register(void);
 
 /*
  * this function unregisters exynos drm hdmi platform device if it exists.
  */
 void exynos_platform_device_hdmi_unregister(void);
 
+/*
+ * this function registers exynos drm ipp platform device.
+ */
+int exynos_platform_device_ipp_register(void);
+
+/*
+ * this function unregisters exynos drm ipp platform device if it exists.
+ */
+void exynos_platform_device_ipp_unregister(void);
+
 extern struct platform_driver fimd_driver;
 extern struct platform_driver hdmi_driver;
 extern struct platform_driver mixer_driver;
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fimc.c b/drivers/gpu/drm/exynos/exynos_drm_fimc.c
index 411f69b76e84..773f583fa964 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fimc.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fimc.c
@@ -12,11 +12,12 @@
  *
  */
 #include <linux/kernel.h>
+#include <linux/mfd/syscon.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/regmap.h>
 #include <linux/clk.h>
 #include <linux/pm_runtime.h>
-#include <plat/map-base.h>
 
 #include <drm/drmP.h>
 #include <drm/exynos_drm.h>
@@ -76,6 +77,27 @@ enum fimc_wb {
 	FIMC_WB_B,
 };
 
+enum {
+	FIMC_CLK_LCLK,
+	FIMC_CLK_GATE,
+	FIMC_CLK_WB_A,
+	FIMC_CLK_WB_B,
+	FIMC_CLK_MUX,
+	FIMC_CLK_PARENT,
+	FIMC_CLKS_MAX
+};
+
+static const char * const fimc_clock_names[] = {
+	[FIMC_CLK_LCLK]   = "sclk_fimc",
+	[FIMC_CLK_GATE]   = "fimc",
+	[FIMC_CLK_WB_A]   = "pxl_async0",
+	[FIMC_CLK_WB_B]   = "pxl_async1",
+	[FIMC_CLK_MUX]    = "mux",
+	[FIMC_CLK_PARENT] = "parent",
+};
+
+#define FIMC_DEFAULT_LCLK_FREQUENCY 133000000UL
+
 /*
  * A structure of scaler.
  *
@@ -119,28 +141,16 @@ struct fimc_capability {
 };
 
 /*
- * A structure of fimc driver data.
- *
- * @parent_clk: name of parent clock.
- */
-struct fimc_driverdata {
-	char	*parent_clk;
-};
-
-/*
  * A structure of fimc context.
  *
  * @ippdrv: prepare initialization using ippdrv.
  * @regs_res: register resources.
  * @regs: memory mapped io registers.
  * @lock: locking of operations.
- * @sclk_fimc_clk: fimc source clock.
- * @fimc_clk: fimc clock.
- * @wb_clk: writeback a clock.
- * @wb_b_clk: writeback b clock.
+ * @clocks: fimc clocks.
+ * @clk_frequency: LCLK clock frequency.
+ * @sysreg: handle to SYSREG block regmap.
  * @sc: scaler infomations.
- * @odr: ordering of YUV.
- * @ver: fimc version.
  * @pol: porarity of writeback.
  * @id: fimc id.
  * @irq: irq number.
@@ -151,12 +161,10 @@ struct fimc_context {
 	struct resource	*regs_res;
 	void __iomem	*regs;
 	struct mutex	lock;
-	struct clk	*sclk_fimc_clk;
-	struct clk	*fimc_clk;
-	struct clk	*wb_clk;
-	struct clk	*wb_b_clk;
+	struct clk	*clocks[FIMC_CLKS_MAX];
+	u32		clk_frequency;
+	struct regmap	*sysreg;
 	struct fimc_scaler	sc;
-	struct fimc_driverdata	*ddata;
 	struct exynos_drm_ipp_pol	pol;
 	int	id;
 	int	irq;
@@ -200,17 +208,13 @@ static void fimc_sw_reset(struct fimc_context *ctx)
 	fimc_write(0x0, EXYNOS_CIFCNTSEQ);
 }
 
-static void fimc_set_camblk_fimd0_wb(struct fimc_context *ctx)
+static int fimc_set_camblk_fimd0_wb(struct fimc_context *ctx)
 {
-	u32 camblk_cfg;
-
 	DRM_DEBUG_KMS("%s\n", __func__);
 
-	camblk_cfg = readl(SYSREG_CAMERA_BLK);
-	camblk_cfg &= ~(SYSREG_FIMD0WB_DEST_MASK);
-	camblk_cfg |= ctx->id << (SYSREG_FIMD0WB_DEST_SHIFT);
-
-	writel(camblk_cfg, SYSREG_CAMERA_BLK);
+	return regmap_update_bits(ctx->sysreg, SYSREG_CAMERA_BLK,
+				  SYSREG_FIMD0WB_DEST_MASK,
+				  ctx->id << SYSREG_FIMD0WB_DEST_SHIFT);
 }
 
 static void fimc_set_type_ctrl(struct fimc_context *ctx, enum fimc_wb wb)
@@ -1301,14 +1305,12 @@ static int fimc_clk_ctrl(struct fimc_context *ctx, bool enable)
 	DRM_DEBUG_KMS("%s:enable[%d]\n", __func__, enable);
 
 	if (enable) {
-		clk_enable(ctx->sclk_fimc_clk);
-		clk_enable(ctx->fimc_clk);
-		clk_enable(ctx->wb_clk);
+		clk_prepare_enable(ctx->clocks[FIMC_CLK_GATE]);
+		clk_prepare_enable(ctx->clocks[FIMC_CLK_WB_A]);
 		ctx->suspended = false;
 	} else {
-		clk_disable(ctx->sclk_fimc_clk);
-		clk_disable(ctx->fimc_clk);
-		clk_disable(ctx->wb_clk);
+		clk_disable_unprepare(ctx->clocks[FIMC_CLK_GATE]);
+		clk_disable_unprepare(ctx->clocks[FIMC_CLK_WB_A]);
 		ctx->suspended = true;
 	}
 
@@ -1613,7 +1615,11 @@ static int fimc_ippdrv_start(struct device *dev, enum drm_exynos_ipp_cmd cmd)
 		fimc_handle_lastend(ctx, true);
 
 		/* setup FIMD */
-		fimc_set_camblk_fimd0_wb(ctx);
+		ret = fimc_set_camblk_fimd0_wb(ctx);
+		if (ret < 0) {
+			dev_err(dev, "camblk setup failed.\n");
+			return ret;
+		}
 
 		set_wb.enable = 1;
 		set_wb.refresh = property->refresh_rate;
@@ -1713,76 +1719,118 @@ static void fimc_ippdrv_stop(struct device *dev, enum drm_exynos_ipp_cmd cmd)
 	fimc_write(cfg, EXYNOS_CIGCTRL);
 }
 
+static void fimc_put_clocks(struct fimc_context *ctx)
+{
+	int i;
+
+	for (i = 0; i < FIMC_CLKS_MAX; i++) {
+		if (IS_ERR(ctx->clocks[i]))
+			continue;
+		clk_put(ctx->clocks[i]);
+		ctx->clocks[i] = ERR_PTR(-EINVAL);
+	}
+}
+
+static int fimc_setup_clocks(struct fimc_context *ctx)
+{
+	struct device *fimc_dev = ctx->ippdrv.dev;
+	struct device *dev;
+	int ret, i;
+
+	for (i = 0; i < FIMC_CLKS_MAX; i++)
+		ctx->clocks[i] = ERR_PTR(-EINVAL);
+
+	for (i = 0; i < FIMC_CLKS_MAX; i++) {
+		if (i == FIMC_CLK_WB_A || i == FIMC_CLK_WB_B)
+			dev = fimc_dev->parent;
+		else
+			dev = fimc_dev;
+
+		ctx->clocks[i] = clk_get(dev, fimc_clock_names[i]);
+		if (IS_ERR(ctx->clocks[i])) {
+			if (i >= FIMC_CLK_MUX)
+				break;
+			ret = PTR_ERR(ctx->clocks[i]);
+			dev_err(fimc_dev, "failed to get clock: %s\n",
+						fimc_clock_names[i]);
+			goto e_clk_free;
+		}
+	}
+
+	/* Optional FIMC LCLK parent clock setting */
+	if (!IS_ERR(ctx->clocks[FIMC_CLK_PARENT])) {
+		ret = clk_set_parent(ctx->clocks[FIMC_CLK_MUX],
+				     ctx->clocks[FIMC_CLK_PARENT]);
+		if (ret < 0) {
+			dev_err(fimc_dev, "failed to set parent.\n");
+			goto e_clk_free;
+		}
+	}
+
+	ret = clk_set_rate(ctx->clocks[FIMC_CLK_LCLK], ctx->clk_frequency);
+	if (ret < 0)
+		goto e_clk_free;
+
+	ret = clk_prepare_enable(ctx->clocks[FIMC_CLK_LCLK]);
+	if (!ret)
+		return ret;
+e_clk_free:
+	fimc_put_clocks(ctx);
+	return ret;
+}
+
+static int fimc_parse_dt(struct fimc_context *ctx)
+{
+	struct device_node *node = ctx->ippdrv.dev->of_node;
+
+	/* Handle only devices that support the LCD Writeback data path */
+	if (!of_property_read_bool(node, "samsung,lcd-wb"))
+		return -ENODEV;
+
+	if (of_property_read_u32(node, "clock-frequency",
+					&ctx->clk_frequency))
+		ctx->clk_frequency = FIMC_DEFAULT_LCLK_FREQUENCY;
+
+	ctx->id = of_alias_get_id(node, "fimc");
+
+	if (ctx->id < 0) {
+		dev_err(ctx->ippdrv.dev, "failed to get node alias id.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static int fimc_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct fimc_context *ctx;
-	struct clk	*parent_clk;
 	struct resource *res;
 	struct exynos_drm_ippdrv *ippdrv;
-	struct exynos_drm_fimc_pdata *pdata;
-	struct fimc_driverdata *ddata;
 	int ret;
 
-	pdata = pdev->dev.platform_data;
-	if (!pdata) {
-		dev_err(dev, "no platform data specified.\n");
-		return -EINVAL;
+	if (!dev->of_node) {
+		dev_err(dev, "device tree node not found.\n");
+		return -ENODEV;
 	}
 
 	ctx = devm_kzalloc(dev, sizeof(*ctx), GFP_KERNEL);
 	if (!ctx)
 		return -ENOMEM;
 
-	ddata = (struct fimc_driverdata *)
-		platform_get_device_id(pdev)->driver_data;
-
-	/* clock control */
-	ctx->sclk_fimc_clk = devm_clk_get(dev, "sclk_fimc");
-	if (IS_ERR(ctx->sclk_fimc_clk)) {
-		dev_err(dev, "failed to get src fimc clock.\n");
-		return PTR_ERR(ctx->sclk_fimc_clk);
-	}
-	clk_enable(ctx->sclk_fimc_clk);
-
-	ctx->fimc_clk = devm_clk_get(dev, "fimc");
-	if (IS_ERR(ctx->fimc_clk)) {
-		dev_err(dev, "failed to get fimc clock.\n");
-		clk_disable(ctx->sclk_fimc_clk);
-		return PTR_ERR(ctx->fimc_clk);
-	}
-
-	ctx->wb_clk = devm_clk_get(dev, "pxl_async0");
-	if (IS_ERR(ctx->wb_clk)) {
-		dev_err(dev, "failed to get writeback a clock.\n");
-		clk_disable(ctx->sclk_fimc_clk);
-		return PTR_ERR(ctx->wb_clk);
-	}
-
-	ctx->wb_b_clk = devm_clk_get(dev, "pxl_async1");
-	if (IS_ERR(ctx->wb_b_clk)) {
-		dev_err(dev, "failed to get writeback b clock.\n");
-		clk_disable(ctx->sclk_fimc_clk);
-		return PTR_ERR(ctx->wb_b_clk);
-	}
+	ctx->ippdrv.dev = dev;
 
-	parent_clk = devm_clk_get(dev, ddata->parent_clk);
-
-	if (IS_ERR(parent_clk)) {
-		dev_err(dev, "failed to get parent clock.\n");
-		clk_disable(ctx->sclk_fimc_clk);
-		return PTR_ERR(parent_clk);
-	}
+	ret = fimc_parse_dt(ctx);
+	if (ret < 0)
+		return ret;
 
-	if (clk_set_parent(ctx->sclk_fimc_clk, parent_clk)) {
-		dev_err(dev, "failed to set parent.\n");
-		clk_disable(ctx->sclk_fimc_clk);
-		return -EINVAL;
+	ctx->sysreg = syscon_regmap_lookup_by_phandle(dev->of_node,
+						"samsung,sysreg");
+	if (IS_ERR(ctx->sysreg)) {
+		dev_err(dev, "syscon regmap lookup failed.\n");
+		return PTR_ERR(ctx->sysreg);
 	}
 
-	devm_clk_put(dev, parent_clk);
-	clk_set_rate(ctx->sclk_fimc_clk, pdata->clk_rate);
-
 	/* resource memory */
 	ctx->regs_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	ctx->regs = devm_ioremap_resource(dev, ctx->regs_res);
@@ -1804,13 +1852,11 @@ static int fimc_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	/* context initailization */
-	ctx->id = pdev->id;
-	ctx->pol = pdata->pol;
-	ctx->ddata = ddata;
+	ret = fimc_setup_clocks(ctx);
+	if (ret < 0)
+		goto err_free_irq;
 
 	ippdrv = &ctx->ippdrv;
-	ippdrv->dev = dev;
 	ippdrv->ops[EXYNOS_DRM_OPS_SRC] = &fimc_src_ops;
 	ippdrv->ops[EXYNOS_DRM_OPS_DST] = &fimc_dst_ops;
 	ippdrv->check_property = fimc_ippdrv_check_property;
@@ -1820,7 +1866,7 @@ static int fimc_probe(struct platform_device *pdev)
 	ret = fimc_init_prop_list(ippdrv);
 	if (ret < 0) {
 		dev_err(dev, "failed to init property list.\n");
-		goto err_get_irq;
+		goto err_put_clk;
 	}
 
 	DRM_DEBUG_KMS("%s:id[%d]ippdrv[0x%x]\n", __func__, ctx->id,
@@ -1835,17 +1881,18 @@ static int fimc_probe(struct platform_device *pdev)
 	ret = exynos_drm_ippdrv_register(ippdrv);
 	if (ret < 0) {
 		dev_err(dev, "failed to register drm fimc device.\n");
-		goto err_ippdrv_register;
+		goto err_pm_dis;
 	}
 
 	dev_info(&pdev->dev, "drm fimc registered successfully.\n");
 
 	return 0;
 
-err_ippdrv_register:
-	devm_kfree(dev, ippdrv->prop_list);
+err_pm_dis:
 	pm_runtime_disable(dev);
-err_get_irq:
+err_put_clk:
+	fimc_put_clocks(ctx);
+err_free_irq:
 	free_irq(ctx->irq, ctx);
 
 	return ret;
@@ -1857,10 +1904,10 @@ static int fimc_remove(struct platform_device *pdev)
 	struct fimc_context *ctx = get_fimc_context(dev);
 	struct exynos_drm_ippdrv *ippdrv = &ctx->ippdrv;
 
-	devm_kfree(dev, ippdrv->prop_list);
 	exynos_drm_ippdrv_unregister(ippdrv);
 	mutex_destroy(&ctx->lock);
 
+	fimc_put_clocks(ctx);
 	pm_runtime_set_suspended(dev);
 	pm_runtime_disable(dev);
 
@@ -1915,36 +1962,22 @@ static int fimc_runtime_resume(struct device *dev)
 }
 #endif
 
-static struct fimc_driverdata exynos4210_fimc_data = {
-	.parent_clk = "mout_mpll",
-};
-
-static struct fimc_driverdata exynos4410_fimc_data = {
-	.parent_clk = "mout_mpll_user",
-};
-
-static struct platform_device_id fimc_driver_ids[] = {
-	{
-		.name		= "exynos4210-fimc",
-		.driver_data	= (unsigned long)&exynos4210_fimc_data,
-	}, {
-		.name		= "exynos4412-fimc",
-		.driver_data	= (unsigned long)&exynos4410_fimc_data,
-	},
-	{},
-};
-MODULE_DEVICE_TABLE(platform, fimc_driver_ids);
-
 static const struct dev_pm_ops fimc_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(fimc_suspend, fimc_resume)
 	SET_RUNTIME_PM_OPS(fimc_runtime_suspend, fimc_runtime_resume, NULL)
 };
 
+static const struct of_device_id fimc_of_match[] = {
+	{ .compatible = "samsung,exynos4210-fimc" },
+	{ .compatible = "samsung,exynos4212-fimc" },
+	{ },
+};
+
 struct platform_driver fimc_driver = {
 	.probe		= fimc_probe,
 	.remove		= fimc_remove,
-	.id_table	= fimc_driver_ids,
 	.driver		= {
+		.of_match_table = fimc_of_match,
 		.name	= "exynos-drm-fimc",
 		.owner	= THIS_MODULE,
 		.pm	= &fimc_pm_ops,
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fimd.c b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
index 98cc14725ba9..746b282b343a 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fimd.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
@@ -20,6 +20,7 @@
 #include <linux/of_device.h>
 #include <linux/pm_runtime.h>
 
+#include <video/of_display_timing.h>
 #include <video/samsung_fimd.h>
 #include <drm/exynos_drm.h>
 
@@ -800,18 +801,18 @@ static int fimd_clock(struct fimd_context *ctx, bool enable)
 	if (enable) {
 		int ret;
 
-		ret = clk_enable(ctx->bus_clk);
+		ret = clk_prepare_enable(ctx->bus_clk);
 		if (ret < 0)
 			return ret;
 
-		ret = clk_enable(ctx->lcd_clk);
+		ret = clk_prepare_enable(ctx->lcd_clk);
 		if  (ret < 0) {
-			clk_disable(ctx->bus_clk);
+			clk_disable_unprepare(ctx->bus_clk);
 			return ret;
 		}
 	} else {
-		clk_disable(ctx->lcd_clk);
-		clk_disable(ctx->bus_clk);
+		clk_disable_unprepare(ctx->lcd_clk);
+		clk_disable_unprepare(ctx->bus_clk);
 	}
 
 	return 0;
@@ -884,10 +885,25 @@ static int fimd_probe(struct platform_device *pdev)
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	pdata = pdev->dev.platform_data;
-	if (!pdata) {
-		dev_err(dev, "no platform data specified\n");
-		return -EINVAL;
+	if (pdev->dev.of_node) {
+		pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+		if (!pdata) {
+			DRM_ERROR("memory allocation for pdata failed\n");
+			return -ENOMEM;
+		}
+
+		ret = of_get_fb_videomode(dev->of_node, &pdata->panel.timing,
+					OF_USE_NATIVE_MODE);
+		if (ret) {
+			DRM_ERROR("failed: of_get_fb_videomode() : %d\n", ret);
+			return ret;
+		}
+	} else {
+		pdata = pdev->dev.platform_data;
+		if (!pdata) {
+			DRM_ERROR("no platform data specified\n");
+			return -EINVAL;
+		}
 	}
 
 	panel = &pdata->panel;
@@ -918,7 +934,7 @@ static int fimd_probe(struct platform_device *pdev)
 	if (IS_ERR(ctx->regs))
 		return PTR_ERR(ctx->regs);
 
-	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	res = platform_get_resource_byname(pdev, IORESOURCE_IRQ, "vsync");
 	if (!res) {
 		dev_err(dev, "irq request failed.\n");
 		return -ENXIO;
@@ -980,9 +996,6 @@ static int fimd_remove(struct platform_device *pdev)
 	if (ctx->suspended)
 		goto out;
 
-	clk_disable(ctx->lcd_clk);
-	clk_disable(ctx->bus_clk);
-
 	pm_runtime_set_suspended(dev);
 	pm_runtime_put_sync(dev);
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.c b/drivers/gpu/drm/exynos/exynos_drm_gem.c
index 0e6fe000578c..cf4543ffa079 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.c
@@ -682,7 +682,8 @@ int exynos_drm_gem_dumb_create(struct drm_file *file_priv,
 	args->pitch = args->width * ((args->bpp + 7) / 8);
 	args->size = args->pitch * args->height;
 
-	exynos_gem_obj = exynos_drm_gem_create(dev, args->flags, args->size);
+	exynos_gem_obj = exynos_drm_gem_create(dev, EXYNOS_BO_CONTIG |
+						EXYNOS_BO_WC, args->size);
 	if (IS_ERR(exynos_gem_obj))
 		return PTR_ERR(exynos_gem_obj);
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_hdmi.c b/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
index 7c27df03c9ff..ba2f0f1aa05f 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
@@ -51,21 +51,27 @@ struct drm_hdmi_context {
 
 int exynos_platform_device_hdmi_register(void)
 {
+	struct platform_device *pdev;
+
 	if (exynos_drm_hdmi_pdev)
 		return -EEXIST;
 
-	exynos_drm_hdmi_pdev = platform_device_register_simple(
+	pdev = platform_device_register_simple(
 			"exynos-drm-hdmi", -1, NULL, 0);
-	if (IS_ERR_OR_NULL(exynos_drm_hdmi_pdev))
-		return PTR_ERR(exynos_drm_hdmi_pdev);
+	if (IS_ERR(pdev))
+		return PTR_ERR(pdev);
+
+	exynos_drm_hdmi_pdev = pdev;
 
 	return 0;
 }
 
 void exynos_platform_device_hdmi_unregister(void)
 {
-	if (exynos_drm_hdmi_pdev)
+	if (exynos_drm_hdmi_pdev) {
 		platform_device_unregister(exynos_drm_hdmi_pdev);
+		exynos_drm_hdmi_pdev = NULL;
+	}
 }
 
 void exynos_hdmi_drv_attach(struct exynos_drm_hdmi_context *ctx)
@@ -205,13 +211,45 @@ static void drm_hdmi_mode_fixup(struct device *subdrv_dev,
 				const struct drm_display_mode *mode,
 				struct drm_display_mode *adjusted_mode)
 {
-	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+	struct drm_display_mode *m;
+	int mode_ok;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	if (hdmi_ops && hdmi_ops->mode_fixup)
-		hdmi_ops->mode_fixup(ctx->hdmi_ctx->ctx, connector, mode,
-				     adjusted_mode);
+	drm_mode_set_crtcinfo(adjusted_mode, 0);
+
+	mode_ok = drm_hdmi_check_timing(subdrv_dev, adjusted_mode);
+
+	/* just return if user desired mode exists. */
+	if (mode_ok == 0)
+		return;
+
+	/*
+	 * otherwise, find the most suitable mode among modes and change it
+	 * to adjusted_mode.
+	 */
+	list_for_each_entry(m, &connector->modes, head) {
+		mode_ok = drm_hdmi_check_timing(subdrv_dev, m);
+
+		if (mode_ok == 0) {
+			struct drm_mode_object base;
+			struct list_head head;
+
+			DRM_INFO("desired mode doesn't exist so\n");
+			DRM_INFO("use the most suitable mode among modes.\n");
+
+			DRM_DEBUG_KMS("Adjusted Mode: [%d]x[%d] [%d]Hz\n",
+				m->hdisplay, m->vdisplay, m->vrefresh);
+
+			/* preserve display mode header while copying. */
+			head = adjusted_mode->head;
+			base = adjusted_mode->base;
+			memcpy(adjusted_mode, m, sizeof(*m));
+			adjusted_mode->head = head;
+			adjusted_mode->base = base;
+			break;
+		}
+	}
 }
 
 static void drm_hdmi_mode_set(struct device *subdrv_dev, void *mode)
diff --git a/drivers/gpu/drm/exynos/exynos_drm_hdmi.h b/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
index b7faa3662307..6b709440df4c 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
@@ -36,9 +36,6 @@ struct exynos_hdmi_ops {
 	int (*power_on)(void *ctx, int mode);
 
 	/* manager */
-	void (*mode_fixup)(void *ctx, struct drm_connector *connector,
-				const struct drm_display_mode *mode,
-				struct drm_display_mode *adjusted_mode);
 	void (*mode_set)(void *ctx, void *mode);
 	void (*get_max_resol)(void *ctx, unsigned int *width,
 				unsigned int *height);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_ipp.c b/drivers/gpu/drm/exynos/exynos_drm_ipp.c
index 1adce07ecb5b..29d2ad314490 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_ipp.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_ipp.c
@@ -47,6 +47,9 @@
 #define get_ipp_context(dev)	platform_get_drvdata(to_platform_device(dev))
 #define ipp_is_m2m_cmd(c)	(c == IPP_CMD_M2M)
 
+/* platform device pointer for ipp device. */
+static struct platform_device *exynos_drm_ipp_pdev;
+
 /*
  * A structure of event.
  *
@@ -102,6 +105,30 @@ static LIST_HEAD(exynos_drm_ippdrv_list);
 static DEFINE_MUTEX(exynos_drm_ippdrv_lock);
 static BLOCKING_NOTIFIER_HEAD(exynos_drm_ippnb_list);
 
+int exynos_platform_device_ipp_register(void)
+{
+	struct platform_device *pdev;
+
+	if (exynos_drm_ipp_pdev)
+		return -EEXIST;
+
+	pdev = platform_device_register_simple("exynos-drm-ipp", -1, NULL, 0);
+	if (IS_ERR(pdev))
+		return PTR_ERR(pdev);
+
+	exynos_drm_ipp_pdev = pdev;
+
+	return 0;
+}
+
+void exynos_platform_device_ipp_unregister(void)
+{
+	if (exynos_drm_ipp_pdev) {
+		platform_device_unregister(exynos_drm_ipp_pdev);
+		exynos_drm_ipp_pdev = NULL;
+	}
+}
+
 int exynos_drm_ippdrv_register(struct exynos_drm_ippdrv *ippdrv)
 {
 	DRM_DEBUG_KMS("%s\n", __func__);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_rotator.c b/drivers/gpu/drm/exynos/exynos_drm_rotator.c
index a40b9fb60240..947f09f15ad1 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_rotator.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_rotator.c
@@ -674,7 +674,7 @@ static int rotator_probe(struct platform_device *pdev)
 	}
 
 	rot->clock = devm_clk_get(dev, "rotator");
-	if (IS_ERR_OR_NULL(rot->clock)) {
+	if (IS_ERR(rot->clock)) {
 		dev_err(dev, "failed to get clock\n");
 		ret = PTR_ERR(rot->clock);
 		goto err_clk_get;
diff --git a/drivers/gpu/drm/exynos/exynos_hdmi.c b/drivers/gpu/drm/exynos/exynos_hdmi.c
index 2c5f266154ad..bbfc3840080c 100644
--- a/drivers/gpu/drm/exynos/exynos_hdmi.c
+++ b/drivers/gpu/drm/exynos/exynos_hdmi.c
@@ -108,7 +108,20 @@ struct hdmi_tg_regs {
 	u8 tg_3d[1];
 };
 
-struct hdmi_core_regs {
+struct hdmi_v13_core_regs {
+	u8 h_blank[2];
+	u8 v_blank[3];
+	u8 h_v_line[3];
+	u8 vsync_pol[1];
+	u8 int_pro_mode[1];
+	u8 v_blank_f[3];
+	u8 h_sync_gen[3];
+	u8 v_sync_gen1[3];
+	u8 v_sync_gen2[3];
+	u8 v_sync_gen3[3];
+};
+
+struct hdmi_v14_core_regs {
 	u8 h_blank[2];
 	u8 v2_blank[2];
 	u8 v1_blank[2];
@@ -147,11 +160,23 @@ struct hdmi_core_regs {
 	u8 vact_space_6[2];
 };
 
+struct hdmi_v13_conf {
+	struct hdmi_v13_core_regs core;
+	struct hdmi_tg_regs tg;
+};
+
 struct hdmi_v14_conf {
-	int pixel_clock;
-	struct hdmi_core_regs core;
+	struct hdmi_v14_core_regs core;
 	struct hdmi_tg_regs tg;
+};
+
+struct hdmi_conf_regs {
+	int pixel_clock;
 	int cea_video_id;
+	union {
+		struct hdmi_v13_conf v13_conf;
+		struct hdmi_v14_conf v14_conf;
+	} conf;
 };
 
 struct hdmi_context {
@@ -169,9 +194,8 @@ struct hdmi_context {
 	struct i2c_client		*ddc_port;
 	struct i2c_client		*hdmiphy_port;
 
-	/* current hdmiphy conf index */
-	int cur_conf;
-	struct hdmi_v14_conf		mode_conf;
+	/* current hdmiphy conf regs */
+	struct hdmi_conf_regs		mode_conf;
 
 	struct hdmi_resources		res;
 
@@ -180,292 +204,60 @@ struct hdmi_context {
 	enum hdmi_type			type;
 };
 
-/* HDMI Version 1.3 */
-static const u8 hdmiphy_v13_conf27[32] = {
-	0x01, 0x05, 0x00, 0xD8, 0x10, 0x1C, 0x30, 0x40,
-	0x6B, 0x10, 0x02, 0x51, 0xDF, 0xF2, 0x54, 0x87,
-	0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
-	0x22, 0x40, 0xE3, 0x26, 0x00, 0x00, 0x00, 0x00,
-};
-
-static const u8 hdmiphy_v13_conf27_027[32] = {
-	0x01, 0x05, 0x00, 0xD4, 0x10, 0x9C, 0x09, 0x64,
-	0x6B, 0x10, 0x02, 0x51, 0xDF, 0xF2, 0x54, 0x87,
-	0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
-	0x22, 0x40, 0xE3, 0x26, 0x00, 0x00, 0x00, 0x00,
-};
-
-static const u8 hdmiphy_v13_conf74_175[32] = {
-	0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xef, 0x5B,
-	0x6D, 0x10, 0x01, 0x51, 0xef, 0xF3, 0x54, 0xb9,
-	0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
-	0x22, 0x40, 0xa5, 0x26, 0x01, 0x00, 0x00, 0x00,
-};
-
-static const u8 hdmiphy_v13_conf74_25[32] = {
-	0x01, 0x05, 0x00, 0xd8, 0x10, 0x9c, 0xf8, 0x40,
-	0x6a, 0x10, 0x01, 0x51, 0xff, 0xf1, 0x54, 0xba,
-	0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xe0,
-	0x22, 0x40, 0xa4, 0x26, 0x01, 0x00, 0x00, 0x00,
-};
-
-static const u8 hdmiphy_v13_conf148_5[32] = {
-	0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xf8, 0x40,
-	0x6A, 0x18, 0x00, 0x51, 0xff, 0xF1, 0x54, 0xba,
-	0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xE0,
-	0x22, 0x40, 0xa4, 0x26, 0x02, 0x00, 0x00, 0x00,
-};
-
-struct hdmi_v13_tg_regs {
-	u8 cmd;
-	u8 h_fsz_l;
-	u8 h_fsz_h;
-	u8 hact_st_l;
-	u8 hact_st_h;
-	u8 hact_sz_l;
-	u8 hact_sz_h;
-	u8 v_fsz_l;
-	u8 v_fsz_h;
-	u8 vsync_l;
-	u8 vsync_h;
-	u8 vsync2_l;
-	u8 vsync2_h;
-	u8 vact_st_l;
-	u8 vact_st_h;
-	u8 vact_sz_l;
-	u8 vact_sz_h;
-	u8 field_chg_l;
-	u8 field_chg_h;
-	u8 vact_st2_l;
-	u8 vact_st2_h;
-	u8 vsync_top_hdmi_l;
-	u8 vsync_top_hdmi_h;
-	u8 vsync_bot_hdmi_l;
-	u8 vsync_bot_hdmi_h;
-	u8 field_top_hdmi_l;
-	u8 field_top_hdmi_h;
-	u8 field_bot_hdmi_l;
-	u8 field_bot_hdmi_h;
-};
-
-struct hdmi_v13_core_regs {
-	u8 h_blank[2];
-	u8 v_blank[3];
-	u8 h_v_line[3];
-	u8 vsync_pol[1];
-	u8 int_pro_mode[1];
-	u8 v_blank_f[3];
-	u8 h_sync_gen[3];
-	u8 v_sync_gen1[3];
-	u8 v_sync_gen2[3];
-	u8 v_sync_gen3[3];
-};
-
-struct hdmi_v13_preset_conf {
-	struct hdmi_v13_core_regs core;
-	struct hdmi_v13_tg_regs tg;
-};
-
-struct hdmi_v13_conf {
-	int width;
-	int height;
-	int vrefresh;
-	bool interlace;
-	int cea_video_id;
-	const u8 *hdmiphy_data;
-	const struct hdmi_v13_preset_conf *conf;
-};
-
-static const struct hdmi_v13_preset_conf hdmi_v13_conf_480p = {
-	.core = {
-		.h_blank = {0x8a, 0x00},
-		.v_blank = {0x0d, 0x6a, 0x01},
-		.h_v_line = {0x0d, 0xa2, 0x35},
-		.vsync_pol = {0x01},
-		.int_pro_mode = {0x00},
-		.v_blank_f = {0x00, 0x00, 0x00},
-		.h_sync_gen = {0x0e, 0x30, 0x11},
-		.v_sync_gen1 = {0x0f, 0x90, 0x00},
-		/* other don't care */
-	},
-	.tg = {
-		0x00, /* cmd */
-		0x5a, 0x03, /* h_fsz */
-		0x8a, 0x00, 0xd0, 0x02, /* hact */
-		0x0d, 0x02, /* v_fsz */
-		0x01, 0x00, 0x33, 0x02, /* vsync */
-		0x2d, 0x00, 0xe0, 0x01, /* vact */
-		0x33, 0x02, /* field_chg */
-		0x49, 0x02, /* vact_st2 */
-		0x01, 0x00, 0x33, 0x02, /* vsync top/bot */
-		0x01, 0x00, 0x33, 0x02, /* field top/bot */
-	},
-};
-
-static const struct hdmi_v13_preset_conf hdmi_v13_conf_720p60 = {
-	.core = {
-		.h_blank = {0x72, 0x01},
-		.v_blank = {0xee, 0xf2, 0x00},
-		.h_v_line = {0xee, 0x22, 0x67},
-		.vsync_pol = {0x00},
-		.int_pro_mode = {0x00},
-		.v_blank_f = {0x00, 0x00, 0x00}, /* don't care */
-		.h_sync_gen = {0x6c, 0x50, 0x02},
-		.v_sync_gen1 = {0x0a, 0x50, 0x00},
-		.v_sync_gen2 = {0x01, 0x10, 0x00},
-		.v_sync_gen3 = {0x01, 0x10, 0x00},
-		/* other don't care */
-	},
-	.tg = {
-		0x00, /* cmd */
-		0x72, 0x06, /* h_fsz */
-		0x71, 0x01, 0x01, 0x05, /* hact */
-		0xee, 0x02, /* v_fsz */
-		0x01, 0x00, 0x33, 0x02, /* vsync */
-		0x1e, 0x00, 0xd0, 0x02, /* vact */
-		0x33, 0x02, /* field_chg */
-		0x49, 0x02, /* vact_st2 */
-		0x01, 0x00, 0x01, 0x00, /* vsync top/bot */
-		0x01, 0x00, 0x33, 0x02, /* field top/bot */
-	},
-};
-
-static const struct hdmi_v13_preset_conf hdmi_v13_conf_1080i50 = {
-	.core = {
-		.h_blank = {0xd0, 0x02},
-		.v_blank = {0x32, 0xB2, 0x00},
-		.h_v_line = {0x65, 0x04, 0xa5},
-		.vsync_pol = {0x00},
-		.int_pro_mode = {0x01},
-		.v_blank_f = {0x49, 0x2A, 0x23},
-		.h_sync_gen = {0x0E, 0xEA, 0x08},
-		.v_sync_gen1 = {0x07, 0x20, 0x00},
-		.v_sync_gen2 = {0x39, 0x42, 0x23},
-		.v_sync_gen3 = {0x38, 0x87, 0x73},
-		/* other don't care */
-	},
-	.tg = {
-		0x00, /* cmd */
-		0x50, 0x0A, /* h_fsz */
-		0xCF, 0x02, 0x81, 0x07, /* hact */
-		0x65, 0x04, /* v_fsz */
-		0x01, 0x00, 0x33, 0x02, /* vsync */
-		0x16, 0x00, 0x1c, 0x02, /* vact */
-		0x33, 0x02, /* field_chg */
-		0x49, 0x02, /* vact_st2 */
-		0x01, 0x00, 0x33, 0x02, /* vsync top/bot */
-		0x01, 0x00, 0x33, 0x02, /* field top/bot */
-	},
+struct hdmiphy_config {
+	int pixel_clock;
+	u8 conf[32];
 };
 
-static const struct hdmi_v13_preset_conf hdmi_v13_conf_1080p50 = {
-	.core = {
-		.h_blank = {0xd0, 0x02},
-		.v_blank = {0x65, 0x6c, 0x01},
-		.h_v_line = {0x65, 0x04, 0xa5},
-		.vsync_pol = {0x00},
-		.int_pro_mode = {0x00},
-		.v_blank_f = {0x00, 0x00, 0x00}, /* don't care */
-		.h_sync_gen = {0x0e, 0xea, 0x08},
-		.v_sync_gen1 = {0x09, 0x40, 0x00},
-		.v_sync_gen2 = {0x01, 0x10, 0x00},
-		.v_sync_gen3 = {0x01, 0x10, 0x00},
-		/* other don't care */
-	},
-	.tg = {
-		0x00, /* cmd */
-		0x50, 0x0A, /* h_fsz */
-		0xCF, 0x02, 0x81, 0x07, /* hact */
-		0x65, 0x04, /* v_fsz */
-		0x01, 0x00, 0x33, 0x02, /* vsync */
-		0x2d, 0x00, 0x38, 0x04, /* vact */
-		0x33, 0x02, /* field_chg */
-		0x48, 0x02, /* vact_st2 */
-		0x01, 0x00, 0x01, 0x00, /* vsync top/bot */
-		0x01, 0x00, 0x33, 0x02, /* field top/bot */
+/* list of phy config settings */
+static const struct hdmiphy_config hdmiphy_v13_configs[] = {
+	{
+		.pixel_clock = 27000000,
+		.conf = {
+			0x01, 0x05, 0x00, 0xD8, 0x10, 0x1C, 0x30, 0x40,
+			0x6B, 0x10, 0x02, 0x51, 0xDF, 0xF2, 0x54, 0x87,
+			0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+			0x22, 0x40, 0xE3, 0x26, 0x00, 0x00, 0x00, 0x00,
+		},
 	},
-};
-
-static const struct hdmi_v13_preset_conf hdmi_v13_conf_1080i60 = {
-	.core = {
-		.h_blank = {0x18, 0x01},
-		.v_blank = {0x32, 0xB2, 0x00},
-		.h_v_line = {0x65, 0x84, 0x89},
-		.vsync_pol = {0x00},
-		.int_pro_mode = {0x01},
-		.v_blank_f = {0x49, 0x2A, 0x23},
-		.h_sync_gen = {0x56, 0x08, 0x02},
-		.v_sync_gen1 = {0x07, 0x20, 0x00},
-		.v_sync_gen2 = {0x39, 0x42, 0x23},
-		.v_sync_gen3 = {0xa4, 0x44, 0x4a},
-		/* other don't care */
+	{
+		.pixel_clock = 27027000,
+		.conf = {
+			0x01, 0x05, 0x00, 0xD4, 0x10, 0x9C, 0x09, 0x64,
+			0x6B, 0x10, 0x02, 0x51, 0xDF, 0xF2, 0x54, 0x87,
+			0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+			0x22, 0x40, 0xE3, 0x26, 0x00, 0x00, 0x00, 0x00,
+		},
 	},
-	.tg = {
-		0x00, /* cmd */
-		0x98, 0x08, /* h_fsz */
-		0x17, 0x01, 0x81, 0x07, /* hact */
-		0x65, 0x04, /* v_fsz */
-		0x01, 0x00, 0x33, 0x02, /* vsync */
-		0x16, 0x00, 0x1c, 0x02, /* vact */
-		0x33, 0x02, /* field_chg */
-		0x49, 0x02, /* vact_st2 */
-		0x01, 0x00, 0x33, 0x02, /* vsync top/bot */
-		0x01, 0x00, 0x33, 0x02, /* field top/bot */
+	{
+		.pixel_clock = 74176000,
+		.conf = {
+			0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xef, 0x5B,
+			0x6D, 0x10, 0x01, 0x51, 0xef, 0xF3, 0x54, 0xb9,
+			0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+			0x22, 0x40, 0xa5, 0x26, 0x01, 0x00, 0x00, 0x00,
+		},
 	},
-};
-
-static const struct hdmi_v13_preset_conf hdmi_v13_conf_1080p60 = {
-	.core = {
-		.h_blank = {0x18, 0x01},
-		.v_blank = {0x65, 0x6c, 0x01},
-		.h_v_line = {0x65, 0x84, 0x89},
-		.vsync_pol = {0x00},
-		.int_pro_mode = {0x00},
-		.v_blank_f = {0x00, 0x00, 0x00}, /* don't care */
-		.h_sync_gen = {0x56, 0x08, 0x02},
-		.v_sync_gen1 = {0x09, 0x40, 0x00},
-		.v_sync_gen2 = {0x01, 0x10, 0x00},
-		.v_sync_gen3 = {0x01, 0x10, 0x00},
-		/* other don't care */
+	{
+		.pixel_clock = 74250000,
+		.conf = {
+			0x01, 0x05, 0x00, 0xd8, 0x10, 0x9c, 0xf8, 0x40,
+			0x6a, 0x10, 0x01, 0x51, 0xff, 0xf1, 0x54, 0xba,
+			0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xe0,
+			0x22, 0x40, 0xa4, 0x26, 0x01, 0x00, 0x00, 0x00,
+		},
 	},
-	.tg = {
-		0x00, /* cmd */
-		0x98, 0x08, /* h_fsz */
-		0x17, 0x01, 0x81, 0x07, /* hact */
-		0x65, 0x04, /* v_fsz */
-		0x01, 0x00, 0x33, 0x02, /* vsync */
-		0x2d, 0x00, 0x38, 0x04, /* vact */
-		0x33, 0x02, /* field_chg */
-		0x48, 0x02, /* vact_st2 */
-		0x01, 0x00, 0x01, 0x00, /* vsync top/bot */
-		0x01, 0x00, 0x33, 0x02, /* field top/bot */
+	{
+		.pixel_clock = 148500000,
+		.conf = {
+			0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xf8, 0x40,
+			0x6A, 0x18, 0x00, 0x51, 0xff, 0xF1, 0x54, 0xba,
+			0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xE0,
+			0x22, 0x40, 0xa4, 0x26, 0x02, 0x00, 0x00, 0x00,
+		},
 	},
 };
 
-static const struct hdmi_v13_conf hdmi_v13_confs[] = {
-	{ 1280, 720, 60, false, 4, hdmiphy_v13_conf74_25,
-			&hdmi_v13_conf_720p60 },
-	{ 1280, 720, 50, false, 19, hdmiphy_v13_conf74_25,
-			&hdmi_v13_conf_720p60 },
-	{ 720, 480, 60, false, 3, hdmiphy_v13_conf27_027,
-			&hdmi_v13_conf_480p },
-	{ 1920, 1080, 50, true, 20, hdmiphy_v13_conf74_25,
-			&hdmi_v13_conf_1080i50 },
-	{ 1920, 1080, 50, false, 31, hdmiphy_v13_conf148_5,
-			&hdmi_v13_conf_1080p50 },
-	{ 1920, 1080, 60, true, 5, hdmiphy_v13_conf74_25,
-			&hdmi_v13_conf_1080i60 },
-	{ 1920, 1080, 60, false, 16, hdmiphy_v13_conf148_5,
-			&hdmi_v13_conf_1080p60 },
-};
-
-/* HDMI Version 1.4 */
-struct hdmiphy_config {
-	int pixel_clock;
-	u8 conf[32];
-};
-
-/* list of all required phy config settings */
 static const struct hdmiphy_config hdmiphy_v14_configs[] = {
 	{
 		.pixel_clock = 25200000,
@@ -873,22 +665,6 @@ static void hdmi_regs_dump(struct hdmi_context *hdata, char *prefix)
 		hdmi_v14_regs_dump(hdata, prefix);
 }
 
-static int hdmi_v13_conf_index(struct drm_display_mode *mode)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(hdmi_v13_confs); ++i)
-		if (hdmi_v13_confs[i].width == mode->hdisplay &&
-				hdmi_v13_confs[i].height == mode->vdisplay &&
-				hdmi_v13_confs[i].vrefresh == mode->vrefresh &&
-				hdmi_v13_confs[i].interlace ==
-				((mode->flags & DRM_MODE_FLAG_INTERLACE) ?
-				 true : false))
-			return i;
-
-	return -EINVAL;
-}
-
 static u8 hdmi_chksum(struct hdmi_context *hdata,
 			u32 start, u8 len, u32 hdr_sum)
 {
@@ -943,11 +719,7 @@ static void hdmi_reg_infoframe(struct hdmi_context *hdata,
 		hdmi_reg_writeb(hdata, HDMI_AVI_BYTE(2), aspect_ratio |
 				AVI_SAME_AS_PIC_ASPECT_RATIO);
 
-		if (hdata->type == HDMI_TYPE13)
-			vic = hdmi_v13_confs[hdata->cur_conf].cea_video_id;
-		else
-			vic = hdata->mode_conf.cea_video_id;
-
+		vic = hdata->mode_conf.cea_video_id;
 		hdmi_reg_writeb(hdata, HDMI_AVI_BYTE(4), vic);
 
 		chksum = hdmi_chksum(hdata, HDMI_AVI_BYTE(1),
@@ -1000,63 +772,34 @@ static struct edid *hdmi_get_edid(void *ctx, struct drm_connector *connector)
 	return raw_edid;
 }
 
-static int hdmi_v13_check_timing(struct fb_videomode *check_timing)
+static int hdmi_find_phy_conf(struct hdmi_context *hdata, u32 pixel_clock)
 {
-	int i;
+	const struct hdmiphy_config *confs;
+	int count, i;
 
-	DRM_DEBUG_KMS("valid mode : xres=%d, yres=%d, refresh=%d, intl=%d\n",
-			check_timing->xres, check_timing->yres,
-			check_timing->refresh, (check_timing->vmode &
-			FB_VMODE_INTERLACED) ? true : false);
-
-	for (i = 0; i < ARRAY_SIZE(hdmi_v13_confs); ++i)
-		if (hdmi_v13_confs[i].width == check_timing->xres &&
-			hdmi_v13_confs[i].height == check_timing->yres &&
-			hdmi_v13_confs[i].vrefresh == check_timing->refresh &&
-			hdmi_v13_confs[i].interlace ==
-			((check_timing->vmode & FB_VMODE_INTERLACED) ?
-			 true : false))
-				return 0;
-
-	/* TODO */
-
-	return -EINVAL;
-}
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
 
-static int hdmi_v14_find_phy_conf(int pixel_clock)
-{
-	int i;
+	if (hdata->type == HDMI_TYPE13) {
+		confs = hdmiphy_v13_configs;
+		count = ARRAY_SIZE(hdmiphy_v13_configs);
+	} else if (hdata->type == HDMI_TYPE14) {
+		confs = hdmiphy_v14_configs;
+		count = ARRAY_SIZE(hdmiphy_v14_configs);
+	} else
+		return -EINVAL;
 
-	for (i = 0; i < ARRAY_SIZE(hdmiphy_v14_configs); i++) {
-		if (hdmiphy_v14_configs[i].pixel_clock == pixel_clock)
+	for (i = 0; i < count; i++)
+		if (confs[i].pixel_clock == pixel_clock)
 			return i;
-	}
 
 	DRM_DEBUG_KMS("Could not find phy config for %d\n", pixel_clock);
 	return -EINVAL;
 }
 
-static int hdmi_v14_check_timing(struct fb_videomode *check_timing)
-{
-	int i;
-
-	DRM_DEBUG_KMS("mode: xres=%d, yres=%d, refresh=%d, clock=%d, intl=%d\n",
-			check_timing->xres, check_timing->yres,
-			check_timing->refresh, check_timing->pixclock,
-			(check_timing->vmode & FB_VMODE_INTERLACED) ?
-			true : false);
-
-	for (i = 0; i < ARRAY_SIZE(hdmiphy_v14_configs); i++)
-		if (hdmiphy_v14_configs[i].pixel_clock ==
-			check_timing->pixclock)
-			return 0;
-
-	return -EINVAL;
-}
-
 static int hdmi_check_timing(void *ctx, struct fb_videomode *timing)
 {
 	struct hdmi_context *hdata = ctx;
+	int ret;
 
 	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
 
@@ -1064,10 +807,10 @@ static int hdmi_check_timing(void *ctx, struct fb_videomode *timing)
 			timing->yres, timing->refresh,
 			timing->vmode);
 
-	if (hdata->type == HDMI_TYPE13)
-		return hdmi_v13_check_timing(timing);
-	else
-		return hdmi_v14_check_timing(timing);
+	ret = hdmi_find_phy_conf(hdata, timing->pixclock);
+	if (ret < 0)
+		return ret;
+	return 0;
 }
 
 static void hdmi_set_acr(u32 freq, u8 *acr)
@@ -1301,10 +1044,9 @@ static void hdmi_conf_init(struct hdmi_context *hdata)
 
 static void hdmi_v13_timing_apply(struct hdmi_context *hdata)
 {
-	const struct hdmi_v13_preset_conf *conf =
-		hdmi_v13_confs[hdata->cur_conf].conf;
-	const struct hdmi_v13_core_regs *core = &conf->core;
-	const struct hdmi_v13_tg_regs *tg = &conf->tg;
+	const struct hdmi_tg_regs *tg = &hdata->mode_conf.conf.v13_conf.tg;
+	const struct hdmi_v13_core_regs *core =
+		&hdata->mode_conf.conf.v13_conf.core;
 	int tries;
 
 	/* setting core registers */
@@ -1334,34 +1076,34 @@ static void hdmi_v13_timing_apply(struct hdmi_context *hdata)
 	hdmi_reg_writeb(hdata, HDMI_V13_V_SYNC_GEN_3_1, core->v_sync_gen3[1]);
 	hdmi_reg_writeb(hdata, HDMI_V13_V_SYNC_GEN_3_2, core->v_sync_gen3[2]);
 	/* Timing generator registers */
-	hdmi_reg_writeb(hdata, HDMI_TG_H_FSZ_L, tg->h_fsz_l);
-	hdmi_reg_writeb(hdata, HDMI_TG_H_FSZ_H, tg->h_fsz_h);
-	hdmi_reg_writeb(hdata, HDMI_TG_HACT_ST_L, tg->hact_st_l);
-	hdmi_reg_writeb(hdata, HDMI_TG_HACT_ST_H, tg->hact_st_h);
-	hdmi_reg_writeb(hdata, HDMI_TG_HACT_SZ_L, tg->hact_sz_l);
-	hdmi_reg_writeb(hdata, HDMI_TG_HACT_SZ_H, tg->hact_sz_h);
-	hdmi_reg_writeb(hdata, HDMI_TG_V_FSZ_L, tg->v_fsz_l);
-	hdmi_reg_writeb(hdata, HDMI_TG_V_FSZ_H, tg->v_fsz_h);
-	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_L, tg->vsync_l);
-	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_H, tg->vsync_h);
-	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC2_L, tg->vsync2_l);
-	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC2_H, tg->vsync2_h);
-	hdmi_reg_writeb(hdata, HDMI_TG_VACT_ST_L, tg->vact_st_l);
-	hdmi_reg_writeb(hdata, HDMI_TG_VACT_ST_H, tg->vact_st_h);
-	hdmi_reg_writeb(hdata, HDMI_TG_VACT_SZ_L, tg->vact_sz_l);
-	hdmi_reg_writeb(hdata, HDMI_TG_VACT_SZ_H, tg->vact_sz_h);
-	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_CHG_L, tg->field_chg_l);
-	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_CHG_H, tg->field_chg_h);
-	hdmi_reg_writeb(hdata, HDMI_TG_VACT_ST2_L, tg->vact_st2_l);
-	hdmi_reg_writeb(hdata, HDMI_TG_VACT_ST2_H, tg->vact_st2_h);
-	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_TOP_HDMI_L, tg->vsync_top_hdmi_l);
-	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_TOP_HDMI_H, tg->vsync_top_hdmi_h);
-	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_BOT_HDMI_L, tg->vsync_bot_hdmi_l);
-	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_BOT_HDMI_H, tg->vsync_bot_hdmi_h);
-	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_TOP_HDMI_L, tg->field_top_hdmi_l);
-	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_TOP_HDMI_H, tg->field_top_hdmi_h);
-	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_BOT_HDMI_L, tg->field_bot_hdmi_l);
-	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_BOT_HDMI_H, tg->field_bot_hdmi_h);
+	hdmi_reg_writeb(hdata, HDMI_TG_H_FSZ_L, tg->h_fsz[0]);
+	hdmi_reg_writeb(hdata, HDMI_TG_H_FSZ_H, tg->h_fsz[1]);
+	hdmi_reg_writeb(hdata, HDMI_TG_HACT_ST_L, tg->hact_st[0]);
+	hdmi_reg_writeb(hdata, HDMI_TG_HACT_ST_H, tg->hact_st[1]);
+	hdmi_reg_writeb(hdata, HDMI_TG_HACT_SZ_L, tg->hact_sz[0]);
+	hdmi_reg_writeb(hdata, HDMI_TG_HACT_SZ_H, tg->hact_sz[1]);
+	hdmi_reg_writeb(hdata, HDMI_TG_V_FSZ_L, tg->v_fsz[0]);
+	hdmi_reg_writeb(hdata, HDMI_TG_V_FSZ_H, tg->v_fsz[1]);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_L, tg->vsync[0]);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_H, tg->vsync[1]);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC2_L, tg->vsync2[0]);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC2_H, tg->vsync2[1]);
+	hdmi_reg_writeb(hdata, HDMI_TG_VACT_ST_L, tg->vact_st[0]);
+	hdmi_reg_writeb(hdata, HDMI_TG_VACT_ST_H, tg->vact_st[1]);
+	hdmi_reg_writeb(hdata, HDMI_TG_VACT_SZ_L, tg->vact_sz[0]);
+	hdmi_reg_writeb(hdata, HDMI_TG_VACT_SZ_H, tg->vact_sz[1]);
+	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_CHG_L, tg->field_chg[0]);
+	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_CHG_H, tg->field_chg[1]);
+	hdmi_reg_writeb(hdata, HDMI_TG_VACT_ST2_L, tg->vact_st2[0]);
+	hdmi_reg_writeb(hdata, HDMI_TG_VACT_ST2_H, tg->vact_st2[1]);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_TOP_HDMI_L, tg->vsync_top_hdmi[0]);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_TOP_HDMI_H, tg->vsync_top_hdmi[1]);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_BOT_HDMI_L, tg->vsync_bot_hdmi[0]);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_BOT_HDMI_H, tg->vsync_bot_hdmi[1]);
+	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_TOP_HDMI_L, tg->field_top_hdmi[0]);
+	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_TOP_HDMI_H, tg->field_top_hdmi[1]);
+	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_BOT_HDMI_L, tg->field_bot_hdmi[0]);
+	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_BOT_HDMI_H, tg->field_bot_hdmi[1]);
 
 	/* waiting for HDMIPHY's PLL to get to steady state */
 	for (tries = 100; tries; --tries) {
@@ -1391,8 +1133,9 @@ static void hdmi_v13_timing_apply(struct hdmi_context *hdata)
 
 static void hdmi_v14_timing_apply(struct hdmi_context *hdata)
 {
-	struct hdmi_core_regs *core = &hdata->mode_conf.core;
-	struct hdmi_tg_regs *tg = &hdata->mode_conf.tg;
+	const struct hdmi_tg_regs *tg = &hdata->mode_conf.conf.v14_conf.tg;
+	const struct hdmi_v14_core_regs *core =
+		&hdata->mode_conf.conf.v14_conf.core;
 	int tries;
 
 	/* setting core registers */
@@ -1624,17 +1367,16 @@ static void hdmiphy_conf_apply(struct hdmi_context *hdata)
 	}
 
 	/* pixel clock */
-	if (hdata->type == HDMI_TYPE13) {
-		hdmiphy_data = hdmi_v13_confs[hdata->cur_conf].hdmiphy_data;
-	} else {
-		i = hdmi_v14_find_phy_conf(hdata->mode_conf.pixel_clock);
-		if (i < 0) {
-			DRM_ERROR("failed to find hdmiphy conf\n");
-			return;
-		}
+	i = hdmi_find_phy_conf(hdata, hdata->mode_conf.pixel_clock);
+	if (i < 0) {
+		DRM_ERROR("failed to find hdmiphy conf\n");
+		return;
+	}
 
+	if (hdata->type == HDMI_TYPE13)
+		hdmiphy_data = hdmiphy_v13_configs[i].conf;
+	else
 		hdmiphy_data = hdmiphy_v14_configs[i].conf;
-	}
 
 	memcpy(buffer, hdmiphy_data, 32);
 	ret = i2c_master_send(hdata->hdmiphy_port, buffer, 32);
@@ -1687,75 +1429,121 @@ static void hdmi_conf_apply(struct hdmi_context *hdata)
 	hdmi_regs_dump(hdata, "start");
 }
 
-static void hdmi_mode_fixup(void *ctx, struct drm_connector *connector,
-				const struct drm_display_mode *mode,
-				struct drm_display_mode *adjusted_mode)
+static void hdmi_set_reg(u8 *reg_pair, int num_bytes, u32 value)
 {
-	struct drm_display_mode *m;
-	struct hdmi_context *hdata = ctx;
-	int index;
+	int i;
+	BUG_ON(num_bytes > 4);
+	for (i = 0; i < num_bytes; i++)
+		reg_pair[i] = (value >> (8 * i)) & 0xff;
+}
 
-	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+static void hdmi_v13_mode_set(struct hdmi_context *hdata,
+			struct drm_display_mode *m)
+{
+	struct hdmi_v13_core_regs *core = &hdata->mode_conf.conf.v13_conf.core;
+	struct hdmi_tg_regs *tg = &hdata->mode_conf.conf.v13_conf.tg;
+	unsigned int val;
 
-	drm_mode_set_crtcinfo(adjusted_mode, 0);
+	hdata->mode_conf.cea_video_id =
+		drm_match_cea_mode((struct drm_display_mode *)m);
+	hdata->mode_conf.pixel_clock = m->clock * 1000;
 
-	if (hdata->type == HDMI_TYPE13)
-		index = hdmi_v13_conf_index(adjusted_mode);
-	else
-		index = hdmi_v14_find_phy_conf(adjusted_mode->clock * 1000);
+	hdmi_set_reg(core->h_blank, 2, m->htotal - m->hdisplay);
+	hdmi_set_reg(core->h_v_line, 3, (m->htotal << 12) | m->vtotal);
 
-	/* just return if user desired mode exists. */
-	if (index >= 0)
-		return;
+	val = (m->flags & DRM_MODE_FLAG_NVSYNC) ? 1 : 0;
+	hdmi_set_reg(core->vsync_pol, 1, val);
+
+	val = (m->flags & DRM_MODE_FLAG_INTERLACE) ? 1 : 0;
+	hdmi_set_reg(core->int_pro_mode, 1, val);
+
+	val = (m->hsync_start - m->hdisplay - 2);
+	val |= ((m->hsync_end - m->hdisplay - 2) << 10);
+	val |= ((m->flags & DRM_MODE_FLAG_NHSYNC)  ? 1 : 0)<<20;
+	hdmi_set_reg(core->h_sync_gen, 3, val);
 
 	/*
-	 * otherwise, find the most suitable mode among modes and change it
-	 * to adjusted_mode.
+	 * Quirk requirement for exynos HDMI IP design,
+	 * 2 pixels less than the actual calculation for hsync_start
+	 * and end.
 	 */
-	list_for_each_entry(m, &connector->modes, head) {
-		if (hdata->type == HDMI_TYPE13)
-			index = hdmi_v13_conf_index(m);
-		else
-			index = hdmi_v14_find_phy_conf(m->clock * 1000);
-
-		if (index >= 0) {
-			struct drm_mode_object base;
-			struct list_head head;
-
-			DRM_INFO("desired mode doesn't exist so\n");
-			DRM_INFO("use the most suitable mode among modes.\n");
-
-			DRM_DEBUG_KMS("Adjusted Mode: [%d]x[%d] [%d]Hz\n",
-				m->hdisplay, m->vdisplay, m->vrefresh);
-
-			/* preserve display mode header while copying. */
-			head = adjusted_mode->head;
-			base = adjusted_mode->base;
-			memcpy(adjusted_mode, m, sizeof(*m));
-			adjusted_mode->head = head;
-			adjusted_mode->base = base;
-			break;
-		}
+
+	/* Following values & calculations differ for different type of modes */
+	if (m->flags & DRM_MODE_FLAG_INTERLACE) {
+		/* Interlaced Mode */
+		val = ((m->vsync_end - m->vdisplay) / 2);
+		val |= ((m->vsync_start - m->vdisplay) / 2) << 12;
+		hdmi_set_reg(core->v_sync_gen1, 3, val);
+
+		val = m->vtotal / 2;
+		val |= ((m->vtotal - m->vdisplay) / 2) << 11;
+		hdmi_set_reg(core->v_blank, 3, val);
+
+		val = (m->vtotal +
+			((m->vsync_end - m->vsync_start) * 4) + 5) / 2;
+		val |= m->vtotal << 11;
+		hdmi_set_reg(core->v_blank_f, 3, val);
+
+		val = ((m->vtotal / 2) + 7);
+		val |= ((m->vtotal / 2) + 2) << 12;
+		hdmi_set_reg(core->v_sync_gen2, 3, val);
+
+		val = ((m->htotal / 2) + (m->hsync_start - m->hdisplay));
+		val |= ((m->htotal / 2) +
+			(m->hsync_start - m->hdisplay)) << 12;
+		hdmi_set_reg(core->v_sync_gen3, 3, val);
+
+		hdmi_set_reg(tg->vact_st, 2, (m->vtotal - m->vdisplay) / 2);
+		hdmi_set_reg(tg->vact_sz, 2, m->vdisplay / 2);
+
+		hdmi_set_reg(tg->vact_st2, 2, 0x249);/* Reset value + 1*/
+	} else {
+		/* Progressive Mode */
+
+		val = m->vtotal;
+		val |= (m->vtotal - m->vdisplay) << 11;
+		hdmi_set_reg(core->v_blank, 3, val);
+
+		hdmi_set_reg(core->v_blank_f, 3, 0);
+
+		val = (m->vsync_end - m->vdisplay);
+		val |= ((m->vsync_start - m->vdisplay) << 12);
+		hdmi_set_reg(core->v_sync_gen1, 3, val);
+
+		hdmi_set_reg(core->v_sync_gen2, 3, 0x1001);/* Reset value  */
+		hdmi_set_reg(core->v_sync_gen3, 3, 0x1001);/* Reset value  */
+		hdmi_set_reg(tg->vact_st, 2, m->vtotal - m->vdisplay);
+		hdmi_set_reg(tg->vact_sz, 2, m->vdisplay);
+		hdmi_set_reg(tg->vact_st2, 2, 0x248); /* Reset value */
 	}
-}
 
-static void hdmi_set_reg(u8 *reg_pair, int num_bytes, u32 value)
-{
-	int i;
-	BUG_ON(num_bytes > 4);
-	for (i = 0; i < num_bytes; i++)
-		reg_pair[i] = (value >> (8 * i)) & 0xff;
+	/* Timing generator registers */
+	hdmi_set_reg(tg->cmd, 1, 0x0);
+	hdmi_set_reg(tg->h_fsz, 2, m->htotal);
+	hdmi_set_reg(tg->hact_st, 2, m->htotal - m->hdisplay);
+	hdmi_set_reg(tg->hact_sz, 2, m->hdisplay);
+	hdmi_set_reg(tg->v_fsz, 2, m->vtotal);
+	hdmi_set_reg(tg->vsync, 2, 0x1);
+	hdmi_set_reg(tg->vsync2, 2, 0x233); /* Reset value */
+	hdmi_set_reg(tg->field_chg, 2, 0x233); /* Reset value */
+	hdmi_set_reg(tg->vsync_top_hdmi, 2, 0x1); /* Reset value */
+	hdmi_set_reg(tg->vsync_bot_hdmi, 2, 0x233); /* Reset value */
+	hdmi_set_reg(tg->field_top_hdmi, 2, 0x1); /* Reset value */
+	hdmi_set_reg(tg->field_bot_hdmi, 2, 0x233); /* Reset value */
+	hdmi_set_reg(tg->tg_3d, 1, 0x0); /* Not used */
 }
 
 static void hdmi_v14_mode_set(struct hdmi_context *hdata,
 			struct drm_display_mode *m)
 {
-	struct hdmi_core_regs *core = &hdata->mode_conf.core;
-	struct hdmi_tg_regs *tg = &hdata->mode_conf.tg;
-
-	hdata->mode_conf.cea_video_id = drm_match_cea_mode(m);
+	struct hdmi_tg_regs *tg = &hdata->mode_conf.conf.v14_conf.tg;
+	struct hdmi_v14_core_regs *core =
+		&hdata->mode_conf.conf.v14_conf.core;
 
+	hdata->mode_conf.cea_video_id =
+		drm_match_cea_mode((struct drm_display_mode *)m);
 	hdata->mode_conf.pixel_clock = m->clock * 1000;
+
 	hdmi_set_reg(core->h_blank, 2, m->htotal - m->hdisplay);
 	hdmi_set_reg(core->v_line, 2, m->vtotal);
 	hdmi_set_reg(core->h_line, 2, m->htotal);
@@ -1852,25 +1640,22 @@ static void hdmi_v14_mode_set(struct hdmi_context *hdata,
 	hdmi_set_reg(tg->field_top_hdmi, 2, 0x1); /* Reset value */
 	hdmi_set_reg(tg->field_bot_hdmi, 2, 0x233); /* Reset value */
 	hdmi_set_reg(tg->tg_3d, 1, 0x0);
-
 }
 
 static void hdmi_mode_set(void *ctx, void *mode)
 {
 	struct hdmi_context *hdata = ctx;
-	int conf_idx;
+	struct drm_display_mode *m = mode;
 
-	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+	DRM_DEBUG_KMS("[%s]: xres=%d, yres=%d, refresh=%d, intl=%s\n",
+		__func__, m->hdisplay, m->vdisplay,
+		m->vrefresh, (m->flags & DRM_MODE_FLAG_INTERLACE) ?
+		"INTERLACED" : "PROGERESSIVE");
 
-	if (hdata->type == HDMI_TYPE13) {
-		conf_idx = hdmi_v13_conf_index(mode);
-		if (conf_idx >= 0)
-			hdata->cur_conf = conf_idx;
-		else
-			DRM_DEBUG_KMS("not supported mode\n");
-	} else {
+	if (hdata->type == HDMI_TYPE13)
+		hdmi_v13_mode_set(hdata, mode);
+	else
 		hdmi_v14_mode_set(hdata, mode);
-	}
 }
 
 static void hdmi_get_max_resol(void *ctx, unsigned int *width,
@@ -1983,7 +1768,6 @@ static struct exynos_hdmi_ops hdmi_ops = {
 	.check_timing	= hdmi_check_timing,
 
 	/* manager */
-	.mode_fixup	= hdmi_mode_fixup,
 	.mode_set	= hdmi_mode_set,
 	.get_max_resol	= hdmi_get_max_resol,
 	.commit		= hdmi_commit,
@@ -2023,27 +1807,27 @@ static int hdmi_resources_init(struct hdmi_context *hdata)
 
 	/* get clocks, power */
 	res->hdmi = devm_clk_get(dev, "hdmi");
-	if (IS_ERR_OR_NULL(res->hdmi)) {
+	if (IS_ERR(res->hdmi)) {
 		DRM_ERROR("failed to get clock 'hdmi'\n");
 		goto fail;
 	}
 	res->sclk_hdmi = devm_clk_get(dev, "sclk_hdmi");
-	if (IS_ERR_OR_NULL(res->sclk_hdmi)) {
+	if (IS_ERR(res->sclk_hdmi)) {
 		DRM_ERROR("failed to get clock 'sclk_hdmi'\n");
 		goto fail;
 	}
 	res->sclk_pixel = devm_clk_get(dev, "sclk_pixel");
-	if (IS_ERR_OR_NULL(res->sclk_pixel)) {
+	if (IS_ERR(res->sclk_pixel)) {
 		DRM_ERROR("failed to get clock 'sclk_pixel'\n");
 		goto fail;
 	}
 	res->sclk_hdmiphy = devm_clk_get(dev, "sclk_hdmiphy");
-	if (IS_ERR_OR_NULL(res->sclk_hdmiphy)) {
+	if (IS_ERR(res->sclk_hdmiphy)) {
 		DRM_ERROR("failed to get clock 'sclk_hdmiphy'\n");
 		goto fail;
 	}
 	res->hdmiphy = devm_clk_get(dev, "hdmiphy");
-	if (IS_ERR_OR_NULL(res->hdmiphy)) {
+	if (IS_ERR(res->hdmiphy)) {
 		DRM_ERROR("failed to get clock 'hdmiphy'\n");
 		goto fail;
 	}
diff --git a/drivers/gpu/drm/exynos/exynos_mixer.c b/drivers/gpu/drm/exynos/exynos_mixer.c
index 2f4f72f07047..ec3e376b7e01 100644
--- a/drivers/gpu/drm/exynos/exynos_mixer.c
+++ b/drivers/gpu/drm/exynos/exynos_mixer.c
@@ -643,12 +643,14 @@ static void mixer_win_reset(struct mixer_context *ctx)
 	/* setting graphical layers */
 	val  = MXR_GRP_CFG_COLOR_KEY_DISABLE; /* no blank key */
 	val |= MXR_GRP_CFG_WIN_BLEND_EN;
-	val |= MXR_GRP_CFG_BLEND_PRE_MUL;
-	val |= MXR_GRP_CFG_PIXEL_BLEND_EN;
 	val |= MXR_GRP_CFG_ALPHA_VAL(0xff); /* non-transparent alpha */
 
-	/* the same configuration for both layers */
+	/* Don't blend layer 0 onto the mixer background */
 	mixer_reg_write(res, MXR_GRAPHIC_CFG(0), val);
+
+	/* Blend layer 1 into layer 0 */
+	val |= MXR_GRP_CFG_BLEND_PRE_MUL;
+	val |= MXR_GRP_CFG_PIXEL_BLEND_EN;
 	mixer_reg_write(res, MXR_GRAPHIC_CFG(1), val);
 
 	/* setting video layers */
@@ -820,7 +822,6 @@ static void mixer_win_disable(void *ctx, int win)
 
 static int mixer_check_timing(void *ctx, struct fb_videomode *timing)
 {
-	struct mixer_context *mixer_ctx = ctx;
 	u32 w, h;
 
 	w = timing->xres;
@@ -831,9 +832,6 @@ static int mixer_check_timing(void *ctx, struct fb_videomode *timing)
 		timing->refresh, (timing->vmode &
 		FB_VMODE_INTERLACED) ? true : false);
 
-	if (mixer_ctx->mxr_ver == MXR_VER_0_0_0_16)
-		return 0;
-
 	if ((w >= 464 && w <= 720 && h >= 261 && h <= 576) ||
 		(w >= 1024 && w <= 1280 && h >= 576 && h <= 720) ||
 		(w >= 1664 && w <= 1920 && h >= 936 && h <= 1080))
@@ -1047,13 +1045,13 @@ static int mixer_resources_init(struct exynos_drm_hdmi_context *ctx,
 	spin_lock_init(&mixer_res->reg_slock);
 
 	mixer_res->mixer = devm_clk_get(dev, "mixer");
-	if (IS_ERR_OR_NULL(mixer_res->mixer)) {
+	if (IS_ERR(mixer_res->mixer)) {
 		dev_err(dev, "failed to get clock 'mixer'\n");
 		return -ENODEV;
 	}
 
 	mixer_res->sclk_hdmi = devm_clk_get(dev, "sclk_hdmi");
-	if (IS_ERR_OR_NULL(mixer_res->sclk_hdmi)) {
+	if (IS_ERR(mixer_res->sclk_hdmi)) {
 		dev_err(dev, "failed to get clock 'sclk_hdmi'\n");
 		return -ENODEV;
 	}
@@ -1096,17 +1094,17 @@ static int vp_resources_init(struct exynos_drm_hdmi_context *ctx,
 	struct resource *res;
 
 	mixer_res->vp = devm_clk_get(dev, "vp");
-	if (IS_ERR_OR_NULL(mixer_res->vp)) {
+	if (IS_ERR(mixer_res->vp)) {
 		dev_err(dev, "failed to get clock 'vp'\n");
 		return -ENODEV;
 	}
 	mixer_res->sclk_mixer = devm_clk_get(dev, "sclk_mixer");
-	if (IS_ERR_OR_NULL(mixer_res->sclk_mixer)) {
+	if (IS_ERR(mixer_res->sclk_mixer)) {
 		dev_err(dev, "failed to get clock 'sclk_mixer'\n");
 		return -ENODEV;
 	}
 	mixer_res->sclk_dac = devm_clk_get(dev, "sclk_dac");
-	if (IS_ERR_OR_NULL(mixer_res->sclk_dac)) {
+	if (IS_ERR(mixer_res->sclk_dac)) {
 		dev_err(dev, "failed to get clock 'sclk_dac'\n");
 		return -ENODEV;
 	}
diff --git a/drivers/gpu/drm/exynos/regs-fimc.h b/drivers/gpu/drm/exynos/regs-fimc.h
index b4f9ca1fd851..30496134a3d0 100644
--- a/drivers/gpu/drm/exynos/regs-fimc.h
+++ b/drivers/gpu/drm/exynos/regs-fimc.h
@@ -661,9 +661,8 @@
 #define EXYNOS_CLKSRC_SCLK				(1 << 1)
 
 /* SYSREG for FIMC writeback */
-#define SYSREG_CAMERA_BLK			(S3C_VA_SYS + 0x0218)
-#define SYSREG_ISP_BLK				(S3C_VA_SYS + 0x020c)
-#define SYSREG_FIMD0WB_DEST_MASK	(0x3 << 23)
-#define SYSREG_FIMD0WB_DEST_SHIFT	23
+#define SYSREG_CAMERA_BLK			(0x0218)
+#define SYSREG_FIMD0WB_DEST_MASK		(0x3 << 23)
+#define SYSREG_FIMD0WB_DEST_SHIFT		23
 
 #endif /* EXYNOS_REGS_FIMC_H */
diff --git a/drivers/gpu/drm/gma500/Kconfig b/drivers/gpu/drm/gma500/Kconfig
index 1188f0fe7e4f..1f6e2dfaaeae 100644
--- a/drivers/gpu/drm/gma500/Kconfig
+++ b/drivers/gpu/drm/gma500/Kconfig
@@ -2,10 +2,15 @@ config DRM_GMA500
 	tristate "Intel GMA5/600 KMS Framebuffer"
 	depends on DRM && PCI && X86
 	select FB_CFB_COPYAREA
-        select FB_CFB_FILLRECT
-        select FB_CFB_IMAGEBLIT
-        select DRM_KMS_HELPER
-        select DRM_TTM
+	select FB_CFB_FILLRECT
+	select FB_CFB_IMAGEBLIT
+	select DRM_KMS_HELPER
+	select DRM_TTM
+	# GMA500 depends on ACPI_VIDEO when ACPI is enabled, just like i915
+	select ACPI_VIDEO if ACPI
+	select BACKLIGHT_CLASS_DEVICE if ACPI
+	select VIDEO_OUTPUT_CONTROL if ACPI
+	select INPUT if ACPI
 	help
 	  Say yes for an experimental 2D KMS framebuffer driver for the
 	  Intel GMA500 ('Poulsbo') and other Intel IMG based graphics
diff --git a/drivers/gpu/drm/gma500/cdv_intel_crt.c b/drivers/gpu/drm/gma500/cdv_intel_crt.c
index 8c175345d85c..7b8386fc3024 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_crt.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_crt.c
@@ -276,6 +276,7 @@ void cdv_intel_crt_init(struct drm_device *dev,
 		goto failed_connector;
 
 	connector = &psb_intel_connector->base;
+	connector->polled = DRM_CONNECTOR_POLL_HPD;
 	drm_connector_init(dev, connector,
 		&cdv_intel_crt_connector_funcs, DRM_MODE_CONNECTOR_VGA);
 
diff --git a/drivers/gpu/drm/gma500/cdv_intel_hdmi.c b/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
index e223b500022e..464153d9d2df 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
@@ -319,6 +319,7 @@ void cdv_hdmi_init(struct drm_device *dev,
 		goto err_priv;
 
 	connector = &psb_intel_connector->base;
+	connector->polled = DRM_CONNECTOR_POLL_HPD;
 	encoder = &psb_intel_encoder->base;
 	drm_connector_init(dev, connector,
 			   &cdv_hdmi_connector_funcs,
diff --git a/drivers/gpu/drm/gma500/framebuffer.c b/drivers/gpu/drm/gma500/framebuffer.c
index 2590cac84257..1534e220097a 100644
--- a/drivers/gpu/drm/gma500/framebuffer.c
+++ b/drivers/gpu/drm/gma500/framebuffer.c
@@ -431,7 +431,7 @@ static int psbfb_create(struct psb_fbdev *fbdev,
 	fbdev->psb_fb_helper.fbdev = info;
 
 	drm_fb_helper_fill_fix(info, fb->pitches[0], fb->depth);
-	strcpy(info->fix.id, "psbfb");
+	strcpy(info->fix.id, "psbdrmfb");
 
 	info->flags = FBINFO_DEFAULT;
 	if (dev_priv->ops->accel_2d && pitch_lines > 8)	/* 2D engine */
@@ -772,8 +772,8 @@ void psb_modeset_init(struct drm_device *dev)
 	for (i = 0; i < dev_priv->num_pipe; i++)
 		psb_intel_crtc_init(dev, i, mode_dev);
 
-	dev->mode_config.max_width = 2048;
-	dev->mode_config.max_height = 2048;
+	dev->mode_config.max_width = 4096;
+	dev->mode_config.max_height = 4096;
 
 	psb_setup_outputs(dev);
 
diff --git a/drivers/gpu/drm/gma500/gtt.c b/drivers/gpu/drm/gma500/gtt.c
index 054e26e769ec..1f82183536a3 100644
--- a/drivers/gpu/drm/gma500/gtt.c
+++ b/drivers/gpu/drm/gma500/gtt.c
@@ -80,7 +80,8 @@ static u32 __iomem *psb_gtt_entry(struct drm_device *dev, struct gtt_range *r)
  *	the GTT. This is protected via the gtt mutex which the caller
  *	must hold.
  */
-static int psb_gtt_insert(struct drm_device *dev, struct gtt_range *r)
+static int psb_gtt_insert(struct drm_device *dev, struct gtt_range *r,
+			  int resume)
 {
 	u32 __iomem *gtt_slot;
 	u32 pte;
@@ -97,8 +98,10 @@ static int psb_gtt_insert(struct drm_device *dev, struct gtt_range *r)
 	gtt_slot = psb_gtt_entry(dev, r);
 	pages = r->pages;
 
-	/* Make sure changes are visible to the GPU */
-	set_pages_array_wc(pages, r->npage);
+	if (!resume) {
+		/* Make sure changes are visible to the GPU */
+		set_pages_array_wc(pages, r->npage);
+	}
 
 	/* Write our page entries into the GTT itself */
 	for (i = r->roll; i < r->npage; i++) {
@@ -269,7 +272,7 @@ int psb_gtt_pin(struct gtt_range *gt)
 		ret = psb_gtt_attach_pages(gt);
 		if (ret < 0)
 			goto out;
-		ret = psb_gtt_insert(dev, gt);
+		ret = psb_gtt_insert(dev, gt, 0);
 		if (ret < 0) {
 			psb_gtt_detach_pages(gt);
 			goto out;
@@ -421,9 +424,11 @@ int psb_gtt_init(struct drm_device *dev, int resume)
 	int ret = 0;
 	uint32_t pte;
 
-	mutex_init(&dev_priv->gtt_mutex);
+	if (!resume) {
+		mutex_init(&dev_priv->gtt_mutex);
+		psb_gtt_alloc(dev);
+	}
 
-	psb_gtt_alloc(dev);
 	pg = &dev_priv->gtt;
 
 	/* Enable the GTT */
@@ -505,7 +510,8 @@ int psb_gtt_init(struct drm_device *dev, int resume)
 	/*
 	 *	Map the GTT and the stolen memory area
 	 */
-	dev_priv->gtt_map = ioremap_nocache(pg->gtt_phys_start,
+	if (!resume)
+		dev_priv->gtt_map = ioremap_nocache(pg->gtt_phys_start,
 						gtt_pages << PAGE_SHIFT);
 	if (!dev_priv->gtt_map) {
 		dev_err(dev->dev, "Failure to map gtt.\n");
@@ -513,7 +519,9 @@ int psb_gtt_init(struct drm_device *dev, int resume)
 		goto out_err;
 	}
 
-	dev_priv->vram_addr = ioremap_wc(dev_priv->stolen_base, stolen_size);
+	if (!resume)
+		dev_priv->vram_addr = ioremap_wc(dev_priv->stolen_base,
+						 stolen_size);
 	if (!dev_priv->vram_addr) {
 		dev_err(dev->dev, "Failure to map stolen base.\n");
 		ret = -ENOMEM;
@@ -549,3 +557,31 @@ out_err:
 	psb_gtt_takedown(dev);
 	return ret;
 }
+
+int psb_gtt_restore(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct resource *r = dev_priv->gtt_mem->child;
+	struct gtt_range *range;
+	unsigned int restored = 0, total = 0, size = 0;
+
+	/* On resume, the gtt_mutex is already initialized */
+	mutex_lock(&dev_priv->gtt_mutex);
+	psb_gtt_init(dev, 1);
+
+	while (r != NULL) {
+		range = container_of(r, struct gtt_range, resource);
+		if (range->pages) {
+			psb_gtt_insert(dev, range, 1);
+			size += range->resource.end - range->resource.start;
+			restored++;
+		}
+		r = r->sibling;
+		total++;
+	}
+	mutex_unlock(&dev_priv->gtt_mutex);
+	DRM_DEBUG_DRIVER("Restored %u of %u gtt ranges (%u KB)", restored,
+			 total, (size / 1024));
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/gma500/gtt.h b/drivers/gpu/drm/gma500/gtt.h
index aa1742387f5a..6191d10acf33 100644
--- a/drivers/gpu/drm/gma500/gtt.h
+++ b/drivers/gpu/drm/gma500/gtt.h
@@ -60,5 +60,5 @@ extern int psb_gtt_pin(struct gtt_range *gt);
 extern void psb_gtt_unpin(struct gtt_range *gt);
 extern void psb_gtt_roll(struct drm_device *dev,
 					struct gtt_range *gt, int roll);
-
+extern int psb_gtt_restore(struct drm_device *dev);
 #endif
diff --git a/drivers/gpu/drm/gma500/intel_bios.c b/drivers/gpu/drm/gma500/intel_bios.c
index 403fffb03abd..d3497348c4d5 100644
--- a/drivers/gpu/drm/gma500/intel_bios.c
+++ b/drivers/gpu/drm/gma500/intel_bios.c
@@ -218,12 +218,11 @@ static void parse_backlight_data(struct drm_psb_private *dev_priv,
 	bl_start = find_section(bdb, BDB_LVDS_BACKLIGHT);
 	vbt_lvds_bl = (struct bdb_lvds_backlight *)(bl_start + 1) + p_type;
 
-	lvds_bl = kzalloc(sizeof(*vbt_lvds_bl), GFP_KERNEL);
+	lvds_bl = kmemdup(vbt_lvds_bl, sizeof(*vbt_lvds_bl), GFP_KERNEL);
 	if (!lvds_bl) {
 		dev_err(dev_priv->dev->dev, "out of memory for backlight data\n");
 		return;
 	}
-	memcpy(lvds_bl, vbt_lvds_bl, sizeof(*vbt_lvds_bl));
 	dev_priv->lvds_bl = lvds_bl;
 }
 
diff --git a/drivers/gpu/drm/gma500/intel_bios.h b/drivers/gpu/drm/gma500/intel_bios.h
index c6267c98c9e7..978ae4b25e82 100644
--- a/drivers/gpu/drm/gma500/intel_bios.h
+++ b/drivers/gpu/drm/gma500/intel_bios.h
@@ -19,8 +19,8 @@
  *
  */
 
-#ifndef _I830_BIOS_H_
-#define _I830_BIOS_H_
+#ifndef _INTEL_BIOS_H_
+#define _INTEL_BIOS_H_
 
 #include <drm/drmP.h>
 #include <drm/drm_dp_helper.h>
@@ -618,4 +618,4 @@ extern void psb_intel_destroy_bios(struct drm_device *dev);
 #define		PORT_IDPC	8
 #define		PORT_IDPD	9
 
-#endif /* _I830_BIOS_H_ */
+#endif /* _INTEL_BIOS_H_ */
diff --git a/drivers/gpu/drm/gma500/mdfld_dsi_output.c b/drivers/gpu/drm/gma500/mdfld_dsi_output.c
index 2d4ab48f07a2..3abf8315f57c 100644
--- a/drivers/gpu/drm/gma500/mdfld_dsi_output.c
+++ b/drivers/gpu/drm/gma500/mdfld_dsi_output.c
@@ -92,8 +92,8 @@ void mdfld_dsi_brightness_init(struct mdfld_dsi_config *dsi_config, int pipe)
 {
 	struct mdfld_dsi_pkg_sender *sender =
 				mdfld_dsi_get_pkg_sender(dsi_config);
-	struct drm_device *dev = sender->dev;
-	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct drm_device *dev;
+	struct drm_psb_private *dev_priv;
 	u32 gen_ctrl_val;
 
 	if (!sender) {
@@ -101,6 +101,9 @@ void mdfld_dsi_brightness_init(struct mdfld_dsi_config *dsi_config, int pipe)
 		return;
 	}
 
+	dev = sender->dev;
+	dev_priv = dev->dev_private;
+
 	/* Set default display backlight value to 85% (0xd8)*/
 	mdfld_dsi_send_mcs_short(sender, write_display_brightness, 0xd8, 1,
 				true);
diff --git a/drivers/gpu/drm/gma500/oaktrail_hdmi_i2c.c b/drivers/gpu/drm/gma500/oaktrail_hdmi_i2c.c
index 88627e3ba1e3..1eb86c79523e 100644
--- a/drivers/gpu/drm/gma500/oaktrail_hdmi_i2c.c
+++ b/drivers/gpu/drm/gma500/oaktrail_hdmi_i2c.c
@@ -319,8 +319,7 @@ void oaktrail_hdmi_i2c_exit(struct pci_dev *dev)
 	struct hdmi_i2c_dev *i2c_dev;
 
 	hdmi_dev = pci_get_drvdata(dev);
-	if (i2c_del_adapter(&oaktrail_hdmi_i2c_adapter))
-		DRM_DEBUG_DRIVER("Failed to delete hdmi-i2c adapter\n");
+	i2c_del_adapter(&oaktrail_hdmi_i2c_adapter);
 
 	i2c_dev = hdmi_dev->i2c_dev;
 	kfree(i2c_dev);
diff --git a/drivers/gpu/drm/gma500/power.c b/drivers/gpu/drm/gma500/power.c
index 889b854751da..b6b135fcd59c 100644
--- a/drivers/gpu/drm/gma500/power.c
+++ b/drivers/gpu/drm/gma500/power.c
@@ -110,6 +110,8 @@ static void gma_resume_display(struct pci_dev *pdev)
 	PSB_WVDC32(dev_priv->pge_ctl | _PSB_PGETBL_ENABLED, PSB_PGETBL_CTL);
 	pci_write_config_word(pdev, PSB_GMCH_CTRL,
 			dev_priv->gmch_ctrl | _PSB_GMCH_ENABLED);
+
+	psb_gtt_restore(dev); /* Rebuild our GTT mappings */
 	dev_priv->ops->restore_regs(dev);
 }
 
@@ -313,3 +315,18 @@ int psb_runtime_idle(struct device *dev)
 	else
 		return 1;
 }
+
+int gma_power_thaw(struct device *_dev)
+{
+	return gma_power_resume(_dev);
+}
+
+int gma_power_freeze(struct device *_dev)
+{
+	return gma_power_suspend(_dev);
+}
+
+int gma_power_restore(struct device *_dev)
+{
+	return gma_power_resume(_dev);
+}
diff --git a/drivers/gpu/drm/gma500/power.h b/drivers/gpu/drm/gma500/power.h
index 1969d2ecb328..56d8708bd41c 100644
--- a/drivers/gpu/drm/gma500/power.h
+++ b/drivers/gpu/drm/gma500/power.h
@@ -41,6 +41,9 @@ void gma_power_uninit(struct drm_device *dev);
  */
 int gma_power_suspend(struct device *dev);
 int gma_power_resume(struct device *dev);
+int gma_power_thaw(struct device *dev);
+int gma_power_freeze(struct device *dev);
+int gma_power_restore(struct device *_dev);
 
 /*
  * These are the functions the driver should use to wrap all hw access
diff --git a/drivers/gpu/drm/gma500/psb_drv.c b/drivers/gpu/drm/gma500/psb_drv.c
index 111e3df9c5de..bddea5807442 100644
--- a/drivers/gpu/drm/gma500/psb_drv.c
+++ b/drivers/gpu/drm/gma500/psb_drv.c
@@ -601,6 +601,9 @@ static void psb_remove(struct pci_dev *pdev)
 static const struct dev_pm_ops psb_pm_ops = {
 	.resume = gma_power_resume,
 	.suspend = gma_power_suspend,
+	.thaw = gma_power_thaw,
+	.freeze = gma_power_freeze,
+	.restore = gma_power_restore,
 	.runtime_suspend = psb_runtime_suspend,
 	.runtime_resume = psb_runtime_resume,
 	.runtime_idle = psb_runtime_idle,
diff --git a/drivers/gpu/drm/gma500/psb_drv.h b/drivers/gpu/drm/gma500/psb_drv.h
index a7fd6c48b793..6053b8abcd12 100644
--- a/drivers/gpu/drm/gma500/psb_drv.h
+++ b/drivers/gpu/drm/gma500/psb_drv.h
@@ -876,7 +876,6 @@ extern const struct psb_ops cdv_chip_ops;
 #define PSB_D_MSVDX   (1 << 9)
 #define PSB_D_TOPAZ   (1 << 10)
 
-extern int drm_psb_no_fb;
 extern int drm_idle_check_interval;
 
 /*
diff --git a/drivers/gpu/drm/gma500/psb_intel_display.c b/drivers/gpu/drm/gma500/psb_intel_display.c
index 9edb1902a096..6e8f42b61ff6 100644
--- a/drivers/gpu/drm/gma500/psb_intel_display.c
+++ b/drivers/gpu/drm/gma500/psb_intel_display.c
@@ -50,119 +50,41 @@ struct psb_intel_p2_t {
 	int p2_slow, p2_fast;
 };
 
-#define INTEL_P2_NUM		      2
-
 struct psb_intel_limit_t {
 	struct psb_intel_range_t dot, vco, n, m, m1, m2, p, p1;
 	struct psb_intel_p2_t p2;
 };
 
-#define I8XX_DOT_MIN		  25000
-#define I8XX_DOT_MAX		 350000
-#define I8XX_VCO_MIN		 930000
-#define I8XX_VCO_MAX		1400000
-#define I8XX_N_MIN		      3
-#define I8XX_N_MAX		     16
-#define I8XX_M_MIN		     96
-#define I8XX_M_MAX		    140
-#define I8XX_M1_MIN		     18
-#define I8XX_M1_MAX		     26
-#define I8XX_M2_MIN		      6
-#define I8XX_M2_MAX		     16
-#define I8XX_P_MIN		      4
-#define I8XX_P_MAX		    128
-#define I8XX_P1_MIN		      2
-#define I8XX_P1_MAX		     33
-#define I8XX_P1_LVDS_MIN	      1
-#define I8XX_P1_LVDS_MAX	      6
-#define I8XX_P2_SLOW		      4
-#define I8XX_P2_FAST		      2
-#define I8XX_P2_LVDS_SLOW	      14
-#define I8XX_P2_LVDS_FAST	      14	/* No fast option */
-#define I8XX_P2_SLOW_LIMIT	 165000
-
-#define I9XX_DOT_MIN		  20000
-#define I9XX_DOT_MAX		 400000
-#define I9XX_VCO_MIN		1400000
-#define I9XX_VCO_MAX		2800000
-#define I9XX_N_MIN		      1
-#define I9XX_N_MAX		      6
-#define I9XX_M_MIN		     70
-#define I9XX_M_MAX		    120
-#define I9XX_M1_MIN		      8
-#define I9XX_M1_MAX		     18
-#define I9XX_M2_MIN		      3
-#define I9XX_M2_MAX		      7
-#define I9XX_P_SDVO_DAC_MIN	      5
-#define I9XX_P_SDVO_DAC_MAX	     80
-#define I9XX_P_LVDS_MIN		      7
-#define I9XX_P_LVDS_MAX		     98
-#define I9XX_P1_MIN		      1
-#define I9XX_P1_MAX		      8
-#define I9XX_P2_SDVO_DAC_SLOW		     10
-#define I9XX_P2_SDVO_DAC_FAST		      5
-#define I9XX_P2_SDVO_DAC_SLOW_LIMIT	 200000
-#define I9XX_P2_LVDS_SLOW		     14
-#define I9XX_P2_LVDS_FAST		      7
-#define I9XX_P2_LVDS_SLOW_LIMIT		 112000
-
-#define INTEL_LIMIT_I8XX_DVO_DAC    0
-#define INTEL_LIMIT_I8XX_LVDS	    1
-#define INTEL_LIMIT_I9XX_SDVO_DAC   2
-#define INTEL_LIMIT_I9XX_LVDS	    3
+#define INTEL_LIMIT_I9XX_SDVO_DAC   0
+#define INTEL_LIMIT_I9XX_LVDS	    1
 
 static const struct psb_intel_limit_t psb_intel_limits[] = {
-	{			/* INTEL_LIMIT_I8XX_DVO_DAC */
-	 .dot = {.min = I8XX_DOT_MIN, .max = I8XX_DOT_MAX},
-	 .vco = {.min = I8XX_VCO_MIN, .max = I8XX_VCO_MAX},
-	 .n = {.min = I8XX_N_MIN, .max = I8XX_N_MAX},
-	 .m = {.min = I8XX_M_MIN, .max = I8XX_M_MAX},
-	 .m1 = {.min = I8XX_M1_MIN, .max = I8XX_M1_MAX},
-	 .m2 = {.min = I8XX_M2_MIN, .max = I8XX_M2_MAX},
-	 .p = {.min = I8XX_P_MIN, .max = I8XX_P_MAX},
-	 .p1 = {.min = I8XX_P1_MIN, .max = I8XX_P1_MAX},
-	 .p2 = {.dot_limit = I8XX_P2_SLOW_LIMIT,
-		.p2_slow = I8XX_P2_SLOW, .p2_fast = I8XX_P2_FAST},
-	 },
-	{			/* INTEL_LIMIT_I8XX_LVDS */
-	 .dot = {.min = I8XX_DOT_MIN, .max = I8XX_DOT_MAX},
-	 .vco = {.min = I8XX_VCO_MIN, .max = I8XX_VCO_MAX},
-	 .n = {.min = I8XX_N_MIN, .max = I8XX_N_MAX},
-	 .m = {.min = I8XX_M_MIN, .max = I8XX_M_MAX},
-	 .m1 = {.min = I8XX_M1_MIN, .max = I8XX_M1_MAX},
-	 .m2 = {.min = I8XX_M2_MIN, .max = I8XX_M2_MAX},
-	 .p = {.min = I8XX_P_MIN, .max = I8XX_P_MAX},
-	 .p1 = {.min = I8XX_P1_LVDS_MIN, .max = I8XX_P1_LVDS_MAX},
-	 .p2 = {.dot_limit = I8XX_P2_SLOW_LIMIT,
-		.p2_slow = I8XX_P2_LVDS_SLOW, .p2_fast = I8XX_P2_LVDS_FAST},
-	 },
 	{			/* INTEL_LIMIT_I9XX_SDVO_DAC */
-	 .dot = {.min = I9XX_DOT_MIN, .max = I9XX_DOT_MAX},
-	 .vco = {.min = I9XX_VCO_MIN, .max = I9XX_VCO_MAX},
-	 .n = {.min = I9XX_N_MIN, .max = I9XX_N_MAX},
-	 .m = {.min = I9XX_M_MIN, .max = I9XX_M_MAX},
-	 .m1 = {.min = I9XX_M1_MIN, .max = I9XX_M1_MAX},
-	 .m2 = {.min = I9XX_M2_MIN, .max = I9XX_M2_MAX},
-	 .p = {.min = I9XX_P_SDVO_DAC_MIN, .max = I9XX_P_SDVO_DAC_MAX},
-	 .p1 = {.min = I9XX_P1_MIN, .max = I9XX_P1_MAX},
-	 .p2 = {.dot_limit = I9XX_P2_SDVO_DAC_SLOW_LIMIT,
-		.p2_slow = I9XX_P2_SDVO_DAC_SLOW, .p2_fast =
-		I9XX_P2_SDVO_DAC_FAST},
+	 .dot = {.min = 20000, .max = 400000},
+	 .vco = {.min = 1400000, .max = 2800000},
+	 .n = {.min = 1, .max = 6},
+	 .m = {.min = 70, .max = 120},
+	 .m1 = {.min = 8, .max = 18},
+	 .m2 = {.min = 3, .max = 7},
+	 .p = {.min = 5, .max = 80},
+	 .p1 = {.min = 1, .max = 8},
+	 .p2 = {.dot_limit = 200000,
+		.p2_slow = 10, .p2_fast = 5},
 	 },
 	{			/* INTEL_LIMIT_I9XX_LVDS */
-	 .dot = {.min = I9XX_DOT_MIN, .max = I9XX_DOT_MAX},
-	 .vco = {.min = I9XX_VCO_MIN, .max = I9XX_VCO_MAX},
-	 .n = {.min = I9XX_N_MIN, .max = I9XX_N_MAX},
-	 .m = {.min = I9XX_M_MIN, .max = I9XX_M_MAX},
-	 .m1 = {.min = I9XX_M1_MIN, .max = I9XX_M1_MAX},
-	 .m2 = {.min = I9XX_M2_MIN, .max = I9XX_M2_MAX},
-	 .p = {.min = I9XX_P_LVDS_MIN, .max = I9XX_P_LVDS_MAX},
-	 .p1 = {.min = I9XX_P1_MIN, .max = I9XX_P1_MAX},
+	 .dot = {.min = 20000, .max = 400000},
+	 .vco = {.min = 1400000, .max = 2800000},
+	 .n = {.min = 1, .max = 6},
+	 .m = {.min = 70, .max = 120},
+	 .m1 = {.min = 8, .max = 18},
+	 .m2 = {.min = 3, .max = 7},
+	 .p = {.min = 7, .max = 98},
+	 .p1 = {.min = 1, .max = 8},
 	 /* The single-channel range is 25-112Mhz, and dual-channel
 	  * is 80-224Mhz.  Prefer single channel as much as possible.
 	  */
-	 .p2 = {.dot_limit = I9XX_P2_LVDS_SLOW_LIMIT,
-		.p2_slow = I9XX_P2_LVDS_SLOW, .p2_fast = I9XX_P2_LVDS_FAST},
+	 .p2 = {.dot_limit = 112000,
+		.p2_slow = 14, .p2_fast = 7},
 	 },
 };
 
@@ -177,9 +99,7 @@ static const struct psb_intel_limit_t *psb_intel_limit(struct drm_crtc *crtc)
 	return limit;
 }
 
-/** Derive the pixel clock for the given refclk and divisors for 8xx chips. */
-
-static void i8xx_clock(int refclk, struct psb_intel_clock_t *clock)
+static void psb_intel_clock(int refclk, struct psb_intel_clock_t *clock)
 {
 	clock->m = 5 * (clock->m1 + 2) + (clock->m2 + 2);
 	clock->p = clock->p1 * clock->p2;
@@ -187,22 +107,6 @@ static void i8xx_clock(int refclk, struct psb_intel_clock_t *clock)
 	clock->dot = clock->vco / clock->p;
 }
 
-/** Derive the pixel clock for the given refclk and divisors for 9xx chips. */
-
-static void i9xx_clock(int refclk, struct psb_intel_clock_t *clock)
-{
-	clock->m = 5 * (clock->m1 + 2) + (clock->m2 + 2);
-	clock->p = clock->p1 * clock->p2;
-	clock->vco = refclk * clock->m / (clock->n + 2);
-	clock->dot = clock->vco / clock->p;
-}
-
-static void psb_intel_clock(struct drm_device *dev, int refclk,
-			struct psb_intel_clock_t *clock)
-{
-	return i9xx_clock(refclk, clock);
-}
-
 /**
  * Returns whether any output on the specified pipe is of the specified type
  */
@@ -308,7 +212,7 @@ static bool psb_intel_find_best_PLL(struct drm_crtc *crtc, int target,
 				     clock.p1++) {
 					int this_err;
 
-					psb_intel_clock(dev, refclk, &clock);
+					psb_intel_clock(refclk, &clock);
 
 					if (!psb_intel_PLL_is_valid
 					    (crtc, &clock))
@@ -1068,7 +972,7 @@ static int psb_intel_crtc_cursor_move(struct drm_crtc *crtc, int x, int y)
 	return 0;
 }
 
-void psb_intel_crtc_gamma_set(struct drm_crtc *crtc, u16 *red,
+static void psb_intel_crtc_gamma_set(struct drm_crtc *crtc, u16 *red,
 			 u16 *green, u16 *blue, uint32_t type, uint32_t size)
 {
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
@@ -1149,9 +1053,9 @@ static int psb_intel_crtc_clock_get(struct drm_device *dev,
 		if ((dpll & PLL_REF_INPUT_MASK) ==
 		    PLLB_REF_INPUT_SPREADSPECTRUMIN) {
 			/* XXX: might not be 66MHz */
-			i8xx_clock(66000, &clock);
+			psb_intel_clock(66000, &clock);
 		} else
-			i8xx_clock(48000, &clock);
+			psb_intel_clock(48000, &clock);
 	} else {
 		if (dpll & PLL_P1_DIVIDE_BY_TWO)
 			clock.p1 = 2;
@@ -1166,7 +1070,7 @@ static int psb_intel_crtc_clock_get(struct drm_device *dev,
 		else
 			clock.p2 = 2;
 
-		i8xx_clock(48000, &clock);
+		psb_intel_clock(48000, &clock);
 	}
 
 	/* XXX: It would be nice to validate the clocks, but we can't reuse
@@ -1225,7 +1129,7 @@ struct drm_display_mode *psb_intel_crtc_mode_get(struct drm_device *dev,
 	return mode;
 }
 
-void psb_intel_crtc_destroy(struct drm_crtc *crtc)
+static void psb_intel_crtc_destroy(struct drm_crtc *crtc)
 {
 	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
 	struct gtt_range *gt;
diff --git a/drivers/gpu/drm/gma500/psb_intel_display.h b/drivers/gpu/drm/gma500/psb_intel_display.h
index 535b49a5e409..3724b971e91c 100644
--- a/drivers/gpu/drm/gma500/psb_intel_display.h
+++ b/drivers/gpu/drm/gma500/psb_intel_display.h
@@ -21,8 +21,5 @@
 #define _INTEL_DISPLAY_H_
 
 bool psb_intel_pipe_has_type(struct drm_crtc *crtc, int type);
-void psb_intel_crtc_gamma_set(struct drm_crtc *crtc, u16 *red,
-			 u16 *green, u16 *blue, uint32_t type, uint32_t size);
-void psb_intel_crtc_destroy(struct drm_crtc *crtc);
 
 #endif
diff --git a/drivers/gpu/drm/gma500/psb_intel_drv.h b/drivers/gpu/drm/gma500/psb_intel_drv.h
index 90f2d11e686b..4dcae421a58d 100644
--- a/drivers/gpu/drm/gma500/psb_intel_drv.h
+++ b/drivers/gpu/drm/gma500/psb_intel_drv.h
@@ -32,9 +32,6 @@
 /* maximum connectors per crtcs in the mode set */
 #define INTELFB_CONN_LIMIT 4
 
-#define INTEL_I2C_BUS_DVO 1
-#define INTEL_I2C_BUS_SDVO 2
-
 /* Intel Pipe Clone Bit */
 #define INTEL_HDMIB_CLONE_BIT 1
 #define INTEL_HDMIC_CLONE_BIT 2
@@ -68,11 +65,6 @@
 #define INTEL_OUTPUT_DISPLAYPORT 9
 #define INTEL_OUTPUT_EDP 10
 
-#define INTEL_DVO_CHIP_NONE 0
-#define INTEL_DVO_CHIP_LVDS 1
-#define INTEL_DVO_CHIP_TMDS 2
-#define INTEL_DVO_CHIP_TVOUT 4
-
 #define INTEL_MODE_PIXEL_MULTIPLIER_SHIFT (0x0)
 #define INTEL_MODE_PIXEL_MULTIPLIER_MASK (0xf << INTEL_MODE_PIXEL_MULTIPLIER_SHIFT)
 
diff --git a/drivers/gpu/drm/gma500/psb_intel_reg.h b/drivers/gpu/drm/gma500/psb_intel_reg.h
index d914719c4b60..0be30e4d146d 100644
--- a/drivers/gpu/drm/gma500/psb_intel_reg.h
+++ b/drivers/gpu/drm/gma500/psb_intel_reg.h
@@ -493,7 +493,6 @@
 #define PIPEACONF_DISABLE		0
 #define PIPEACONF_DOUBLE_WIDE		(1 << 30)
 #define PIPECONF_ACTIVE			(1 << 30)
-#define I965_PIPECONF_ACTIVE		(1 << 30)
 #define PIPECONF_DSIPLL_LOCK		(1 << 29)
 #define PIPEACONF_SINGLE_WIDE		0
 #define PIPEACONF_PIPE_UNLOCKED		0
diff --git a/drivers/gpu/drm/gma500/psb_intel_sdvo.c b/drivers/gpu/drm/gma500/psb_intel_sdvo.c
index a4cc777ab7a6..19e36603b23b 100644
--- a/drivers/gpu/drm/gma500/psb_intel_sdvo.c
+++ b/drivers/gpu/drm/gma500/psb_intel_sdvo.c
@@ -134,6 +134,9 @@ struct psb_intel_sdvo {
 
 	/* Input timings for adjusted_mode */
 	struct psb_intel_sdvo_dtd input_dtd;
+
+	/* Saved SDVO output states */
+	uint32_t saveSDVO; /* Can be SDVOB or SDVOC depending on sdvo_reg */
 };
 
 struct psb_intel_sdvo_connector {
@@ -1830,6 +1833,34 @@ done:
 #undef CHECK_PROPERTY
 }
 
+static void psb_intel_sdvo_save(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+	struct psb_intel_sdvo *sdvo =
+				to_psb_intel_sdvo(&psb_intel_encoder->base);
+
+	sdvo->saveSDVO = REG_READ(sdvo->sdvo_reg);
+}
+
+static void psb_intel_sdvo_restore(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_encoder *encoder =
+				&psb_intel_attached_encoder(connector)->base;
+	struct psb_intel_sdvo *sdvo = to_psb_intel_sdvo(encoder);
+	struct drm_crtc *crtc = encoder->crtc;
+
+	REG_WRITE(sdvo->sdvo_reg, sdvo->saveSDVO);
+
+	/* Force a full mode set on the crtc. We're supposed to have the
+	   mode_config lock already. */
+	if (connector->status == connector_status_connected)
+		drm_crtc_helper_set_mode(crtc, &crtc->mode, crtc->x, crtc->y,
+					 NULL);
+}
+
 static const struct drm_encoder_helper_funcs psb_intel_sdvo_helper_funcs = {
 	.dpms = psb_intel_sdvo_dpms,
 	.mode_fixup = psb_intel_sdvo_mode_fixup,
@@ -1840,6 +1871,8 @@ static const struct drm_encoder_helper_funcs psb_intel_sdvo_helper_funcs = {
 
 static const struct drm_connector_funcs psb_intel_sdvo_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
+	.save = psb_intel_sdvo_save,
+	.restore = psb_intel_sdvo_restore,
 	.detect = psb_intel_sdvo_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = psb_intel_sdvo_set_property,
diff --git a/drivers/gpu/drm/gma500/psb_irq.c b/drivers/gpu/drm/gma500/psb_irq.c
index 8652cdf3f03f..029eccf30137 100644
--- a/drivers/gpu/drm/gma500/psb_irq.c
+++ b/drivers/gpu/drm/gma500/psb_irq.c
@@ -211,7 +211,7 @@ irqreturn_t psb_irq_handler(DRM_IRQ_ARGS)
 
 	vdc_stat = PSB_RVDC32(PSB_INT_IDENTITY_R);
 
-	if (vdc_stat & _PSB_PIPE_EVENT_FLAG)
+	if (vdc_stat & (_PSB_PIPE_EVENT_FLAG|_PSB_IRQ_ASLE))
 		dsp_int = 1;
 
 	/* FIXME: Handle Medfield
diff --git a/drivers/gpu/drm/gma500/psb_irq.h b/drivers/gpu/drm/gma500/psb_irq.h
index 603045bee58a..debb7f190c06 100644
--- a/drivers/gpu/drm/gma500/psb_irq.h
+++ b/drivers/gpu/drm/gma500/psb_irq.h
@@ -21,8 +21,8 @@
  *
  **************************************************************************/
 
-#ifndef _SYSIRQ_H_
-#define _SYSIRQ_H_
+#ifndef _PSB_IRQ_H_
+#define _PSB_IRQ_H_
 
 #include <drm/drmP.h>
 
@@ -44,4 +44,4 @@ u32  psb_get_vblank_counter(struct drm_device *dev, int pipe);
 
 int mdfld_enable_te(struct drm_device *dev, int pipe);
 void mdfld_disable_te(struct drm_device *dev, int pipe);
-#endif /* _SYSIRQ_H_ */
+#endif /* _PSB_IRQ_H_ */
diff --git a/drivers/gpu/drm/i915/i915_debugfs.c b/drivers/gpu/drm/i915/i915_debugfs.c
index 7299ea45dd03..e913d325d5b8 100644
--- a/drivers/gpu/drm/i915/i915_debugfs.c
+++ b/drivers/gpu/drm/i915/i915_debugfs.c
@@ -772,6 +772,23 @@ static int i915_error_state(struct seq_file *m, void *unused)
 				}
 			}
 		}
+
+		obj = error->ring[i].ctx;
+		if (obj) {
+			seq_printf(m, "%s --- HW Context = 0x%08x\n",
+				   dev_priv->ring[i].name,
+				   obj->gtt_offset);
+			offset = 0;
+			for (elt = 0; elt < PAGE_SIZE/16; elt += 4) {
+				seq_printf(m, "[%04x] %08x %08x %08x %08x\n",
+					   offset,
+					   obj->pages[0][elt],
+					   obj->pages[0][elt+1],
+					   obj->pages[0][elt+2],
+					   obj->pages[0][elt+3]);
+					offset += 16;
+			}
+		}
 	}
 
 	if (error->overlay)
@@ -849,76 +866,42 @@ static const struct file_operations i915_error_state_fops = {
 	.release = i915_error_state_release,
 };
 
-static ssize_t
-i915_next_seqno_read(struct file *filp,
-		 char __user *ubuf,
-		 size_t max,
-		 loff_t *ppos)
+static int
+i915_next_seqno_get(void *data, u64 *val)
 {
-	struct drm_device *dev = filp->private_data;
+	struct drm_device *dev = data;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	char buf[80];
-	int len;
 	int ret;
 
 	ret = mutex_lock_interruptible(&dev->struct_mutex);
 	if (ret)
 		return ret;
 
-	len = snprintf(buf, sizeof(buf),
-		       "next_seqno :  0x%x\n",
-		       dev_priv->next_seqno);
-
+	*val = dev_priv->next_seqno;
 	mutex_unlock(&dev->struct_mutex);
 
-	if (len > sizeof(buf))
-		len = sizeof(buf);
-
-	return simple_read_from_buffer(ubuf, max, ppos, buf, len);
+	return 0;
 }
 
-static ssize_t
-i915_next_seqno_write(struct file *filp,
-		      const char __user *ubuf,
-		      size_t cnt,
-		      loff_t *ppos)
-{
-	struct drm_device *dev = filp->private_data;
-	char buf[20];
-	u32 val = 1;
+static int
+i915_next_seqno_set(void *data, u64 val)
+{
+	struct drm_device *dev = data;
 	int ret;
 
-	if (cnt > 0) {
-		if (cnt > sizeof(buf) - 1)
-			return -EINVAL;
-
-		if (copy_from_user(buf, ubuf, cnt))
-			return -EFAULT;
-		buf[cnt] = 0;
-
-		ret = kstrtouint(buf, 0, &val);
-		if (ret < 0)
-			return ret;
-	}
-
 	ret = mutex_lock_interruptible(&dev->struct_mutex);
 	if (ret)
 		return ret;
 
 	ret = i915_gem_set_seqno(dev, val);
-
 	mutex_unlock(&dev->struct_mutex);
 
-	return ret ?: cnt;
+	return ret;
 }
 
-static const struct file_operations i915_next_seqno_fops = {
-	.owner = THIS_MODULE,
-	.open = simple_open,
-	.read = i915_next_seqno_read,
-	.write = i915_next_seqno_write,
-	.llseek = default_llseek,
-};
+DEFINE_SIMPLE_ATTRIBUTE(i915_next_seqno_fops,
+			i915_next_seqno_get, i915_next_seqno_set,
+			"0x%llx\n");
 
 static int i915_rstdby_delays(struct seq_file *m, void *unused)
 {
@@ -1023,6 +1006,9 @@ static int i915_cur_delayinfo(struct seq_file *m, void *unused)
 		max_freq = rp_state_cap & 0xff;
 		seq_printf(m, "Max non-overclocked (RP0) frequency: %dMHz\n",
 			   max_freq * GT_FREQUENCY_MULTIPLIER);
+
+		seq_printf(m, "Max overclocked frequency: %dMHz\n",
+			   dev_priv->rps.hw_max * GT_FREQUENCY_MULTIPLIER);
 	} else {
 		seq_printf(m, "no P-state info available\n");
 	}
@@ -1371,7 +1357,7 @@ static int i915_ring_freq_table(struct seq_file *m, void *unused)
 	if (ret)
 		return ret;
 
-	seq_printf(m, "GPU freq (MHz)\tEffective CPU freq (MHz)\n");
+	seq_printf(m, "GPU freq (MHz)\tEffective CPU freq (MHz)\tEffective Ring freq (MHz)\n");
 
 	for (gpu_freq = dev_priv->rps.min_delay;
 	     gpu_freq <= dev_priv->rps.max_delay;
@@ -1380,7 +1366,10 @@ static int i915_ring_freq_table(struct seq_file *m, void *unused)
 		sandybridge_pcode_read(dev_priv,
 				       GEN6_PCODE_READ_MIN_FREQ_TABLE,
 				       &ia_freq);
-		seq_printf(m, "%d\t\t%d\n", gpu_freq * GT_FREQUENCY_MULTIPLIER, ia_freq * 100);
+		seq_printf(m, "%d\t\t%d\t\t\t\t%d\n",
+			   gpu_freq * GT_FREQUENCY_MULTIPLIER,
+			   ((ia_freq >> 0) & 0xff) * 100,
+			   ((ia_freq >> 8) & 0xff) * 100);
 	}
 
 	mutex_unlock(&dev_priv->rps.hw_lock);
@@ -1680,105 +1669,51 @@ static int i915_dpio_info(struct seq_file *m, void *data)
 	return 0;
 }
 
-static ssize_t
-i915_wedged_read(struct file *filp,
-		 char __user *ubuf,
-		 size_t max,
-		 loff_t *ppos)
+static int
+i915_wedged_get(void *data, u64 *val)
 {
-	struct drm_device *dev = filp->private_data;
+	struct drm_device *dev = data;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	char buf[80];
-	int len;
 
-	len = snprintf(buf, sizeof(buf),
-		       "wedged :  %d\n",
-		       atomic_read(&dev_priv->gpu_error.reset_counter));
+	*val = atomic_read(&dev_priv->gpu_error.reset_counter);
 
-	if (len > sizeof(buf))
-		len = sizeof(buf);
-
-	return simple_read_from_buffer(ubuf, max, ppos, buf, len);
+	return 0;
 }
 
-static ssize_t
-i915_wedged_write(struct file *filp,
-		  const char __user *ubuf,
-		  size_t cnt,
-		  loff_t *ppos)
+static int
+i915_wedged_set(void *data, u64 val)
 {
-	struct drm_device *dev = filp->private_data;
-	char buf[20];
-	int val = 1;
-
-	if (cnt > 0) {
-		if (cnt > sizeof(buf) - 1)
-			return -EINVAL;
+	struct drm_device *dev = data;
 
-		if (copy_from_user(buf, ubuf, cnt))
-			return -EFAULT;
-		buf[cnt] = 0;
-
-		val = simple_strtoul(buf, NULL, 0);
-	}
-
-	DRM_INFO("Manually setting wedged to %d\n", val);
+	DRM_INFO("Manually setting wedged to %llu\n", val);
 	i915_handle_error(dev, val);
 
-	return cnt;
+	return 0;
 }
 
-static const struct file_operations i915_wedged_fops = {
-	.owner = THIS_MODULE,
-	.open = simple_open,
-	.read = i915_wedged_read,
-	.write = i915_wedged_write,
-	.llseek = default_llseek,
-};
+DEFINE_SIMPLE_ATTRIBUTE(i915_wedged_fops,
+			i915_wedged_get, i915_wedged_set,
+			"%llu\n");
 
-static ssize_t
-i915_ring_stop_read(struct file *filp,
-		    char __user *ubuf,
-		    size_t max,
-		    loff_t *ppos)
+static int
+i915_ring_stop_get(void *data, u64 *val)
 {
-	struct drm_device *dev = filp->private_data;
+	struct drm_device *dev = data;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	char buf[20];
-	int len;
 
-	len = snprintf(buf, sizeof(buf),
-		       "0x%08x\n", dev_priv->gpu_error.stop_rings);
+	*val = dev_priv->gpu_error.stop_rings;
 
-	if (len > sizeof(buf))
-		len = sizeof(buf);
-
-	return simple_read_from_buffer(ubuf, max, ppos, buf, len);
+	return 0;
 }
 
-static ssize_t
-i915_ring_stop_write(struct file *filp,
-		     const char __user *ubuf,
-		     size_t cnt,
-		     loff_t *ppos)
+static int
+i915_ring_stop_set(void *data, u64 val)
 {
-	struct drm_device *dev = filp->private_data;
+	struct drm_device *dev = data;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	char buf[20];
-	int val = 0, ret;
-
-	if (cnt > 0) {
-		if (cnt > sizeof(buf) - 1)
-			return -EINVAL;
-
-		if (copy_from_user(buf, ubuf, cnt))
-			return -EFAULT;
-		buf[cnt] = 0;
-
-		val = simple_strtoul(buf, NULL, 0);
-	}
+	int ret;
 
-	DRM_DEBUG_DRIVER("Stopping rings 0x%08x\n", val);
+	DRM_DEBUG_DRIVER("Stopping rings 0x%08llx\n", val);
 
 	ret = mutex_lock_interruptible(&dev->struct_mutex);
 	if (ret)
@@ -1787,16 +1722,12 @@ i915_ring_stop_write(struct file *filp,
 	dev_priv->gpu_error.stop_rings = val;
 	mutex_unlock(&dev->struct_mutex);
 
-	return cnt;
+	return 0;
 }
 
-static const struct file_operations i915_ring_stop_fops = {
-	.owner = THIS_MODULE,
-	.open = simple_open,
-	.read = i915_ring_stop_read,
-	.write = i915_ring_stop_write,
-	.llseek = default_llseek,
-};
+DEFINE_SIMPLE_ATTRIBUTE(i915_ring_stop_fops,
+			i915_ring_stop_get, i915_ring_stop_set,
+			"0x%08llx\n");
 
 #define DROP_UNBOUND 0x1
 #define DROP_BOUND 0x2
@@ -1806,46 +1737,23 @@ static const struct file_operations i915_ring_stop_fops = {
 		  DROP_BOUND | \
 		  DROP_RETIRE | \
 		  DROP_ACTIVE)
-static ssize_t
-i915_drop_caches_read(struct file *filp,
-		      char __user *ubuf,
-		      size_t max,
-		      loff_t *ppos)
+static int
+i915_drop_caches_get(void *data, u64 *val)
 {
-	char buf[20];
-	int len;
+	*val = DROP_ALL;
 
-	len = snprintf(buf, sizeof(buf), "0x%08x\n", DROP_ALL);
-	if (len > sizeof(buf))
-		len = sizeof(buf);
-
-	return simple_read_from_buffer(ubuf, max, ppos, buf, len);
+	return 0;
 }
 
-static ssize_t
-i915_drop_caches_write(struct file *filp,
-		       const char __user *ubuf,
-		       size_t cnt,
-		       loff_t *ppos)
+static int
+i915_drop_caches_set(void *data, u64 val)
 {
-	struct drm_device *dev = filp->private_data;
+	struct drm_device *dev = data;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_i915_gem_object *obj, *next;
-	char buf[20];
-	int val = 0, ret;
-
-	if (cnt > 0) {
-		if (cnt > sizeof(buf) - 1)
-			return -EINVAL;
-
-		if (copy_from_user(buf, ubuf, cnt))
-			return -EFAULT;
-		buf[cnt] = 0;
-
-		val = simple_strtoul(buf, NULL, 0);
-	}
+	int ret;
 
-	DRM_DEBUG_DRIVER("Dropping caches: 0x%08x\n", val);
+	DRM_DEBUG_DRIVER("Dropping caches: 0x%08llx\n", val);
 
 	/* No need to check and wait for gpu resets, only libdrm auto-restarts
 	 * on ioctls on -EAGAIN. */
@@ -1883,27 +1791,19 @@ i915_drop_caches_write(struct file *filp,
 unlock:
 	mutex_unlock(&dev->struct_mutex);
 
-	return ret ?: cnt;
+	return ret;
 }
 
-static const struct file_operations i915_drop_caches_fops = {
-	.owner = THIS_MODULE,
-	.open = simple_open,
-	.read = i915_drop_caches_read,
-	.write = i915_drop_caches_write,
-	.llseek = default_llseek,
-};
+DEFINE_SIMPLE_ATTRIBUTE(i915_drop_caches_fops,
+			i915_drop_caches_get, i915_drop_caches_set,
+			"0x%08llx\n");
 
-static ssize_t
-i915_max_freq_read(struct file *filp,
-		   char __user *ubuf,
-		   size_t max,
-		   loff_t *ppos)
+static int
+i915_max_freq_get(void *data, u64 *val)
 {
-	struct drm_device *dev = filp->private_data;
+	struct drm_device *dev = data;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	char buf[80];
-	int len, ret;
+	int ret;
 
 	if (!(IS_GEN6(dev) || IS_GEN7(dev)))
 		return -ENODEV;
@@ -1912,42 +1812,23 @@ i915_max_freq_read(struct file *filp,
 	if (ret)
 		return ret;
 
-	len = snprintf(buf, sizeof(buf),
-		       "max freq: %d\n", dev_priv->rps.max_delay * GT_FREQUENCY_MULTIPLIER);
+	*val = dev_priv->rps.max_delay * GT_FREQUENCY_MULTIPLIER;
 	mutex_unlock(&dev_priv->rps.hw_lock);
 
-	if (len > sizeof(buf))
-		len = sizeof(buf);
-
-	return simple_read_from_buffer(ubuf, max, ppos, buf, len);
+	return 0;
 }
 
-static ssize_t
-i915_max_freq_write(struct file *filp,
-		  const char __user *ubuf,
-		  size_t cnt,
-		  loff_t *ppos)
+static int
+i915_max_freq_set(void *data, u64 val)
 {
-	struct drm_device *dev = filp->private_data;
+	struct drm_device *dev = data;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	char buf[20];
-	int val = 1, ret;
+	int ret;
 
 	if (!(IS_GEN6(dev) || IS_GEN7(dev)))
 		return -ENODEV;
 
-	if (cnt > 0) {
-		if (cnt > sizeof(buf) - 1)
-			return -EINVAL;
-
-		if (copy_from_user(buf, ubuf, cnt))
-			return -EFAULT;
-		buf[cnt] = 0;
-
-		val = simple_strtoul(buf, NULL, 0);
-	}
-
-	DRM_DEBUG_DRIVER("Manually setting max freq to %d\n", val);
+	DRM_DEBUG_DRIVER("Manually setting max freq to %llu\n", val);
 
 	ret = mutex_lock_interruptible(&dev_priv->rps.hw_lock);
 	if (ret)
@@ -1956,30 +1837,24 @@ i915_max_freq_write(struct file *filp,
 	/*
 	 * Turbo will still be enabled, but won't go above the set value.
 	 */
-	dev_priv->rps.max_delay = val / GT_FREQUENCY_MULTIPLIER;
-
-	gen6_set_rps(dev, val / GT_FREQUENCY_MULTIPLIER);
+	do_div(val, GT_FREQUENCY_MULTIPLIER);
+	dev_priv->rps.max_delay = val;
+	gen6_set_rps(dev, val);
 	mutex_unlock(&dev_priv->rps.hw_lock);
 
-	return cnt;
+	return 0;
 }
 
-static const struct file_operations i915_max_freq_fops = {
-	.owner = THIS_MODULE,
-	.open = simple_open,
-	.read = i915_max_freq_read,
-	.write = i915_max_freq_write,
-	.llseek = default_llseek,
-};
+DEFINE_SIMPLE_ATTRIBUTE(i915_max_freq_fops,
+			i915_max_freq_get, i915_max_freq_set,
+			"%llu\n");
 
-static ssize_t
-i915_min_freq_read(struct file *filp, char __user *ubuf, size_t max,
-		   loff_t *ppos)
+static int
+i915_min_freq_get(void *data, u64 *val)
 {
-	struct drm_device *dev = filp->private_data;
+	struct drm_device *dev = data;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	char buf[80];
-	int len, ret;
+	int ret;
 
 	if (!(IS_GEN6(dev) || IS_GEN7(dev)))
 		return -ENODEV;
@@ -1988,40 +1863,23 @@ i915_min_freq_read(struct file *filp, char __user *ubuf, size_t max,
 	if (ret)
 		return ret;
 
-	len = snprintf(buf, sizeof(buf),
-		       "min freq: %d\n", dev_priv->rps.min_delay * GT_FREQUENCY_MULTIPLIER);
+	*val = dev_priv->rps.min_delay * GT_FREQUENCY_MULTIPLIER;
 	mutex_unlock(&dev_priv->rps.hw_lock);
 
-	if (len > sizeof(buf))
-		len = sizeof(buf);
-
-	return simple_read_from_buffer(ubuf, max, ppos, buf, len);
+	return 0;
 }
 
-static ssize_t
-i915_min_freq_write(struct file *filp, const char __user *ubuf, size_t cnt,
-		    loff_t *ppos)
+static int
+i915_min_freq_set(void *data, u64 val)
 {
-	struct drm_device *dev = filp->private_data;
+	struct drm_device *dev = data;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	char buf[20];
-	int val = 1, ret;
+	int ret;
 
 	if (!(IS_GEN6(dev) || IS_GEN7(dev)))
 		return -ENODEV;
 
-	if (cnt > 0) {
-		if (cnt > sizeof(buf) - 1)
-			return -EINVAL;
-
-		if (copy_from_user(buf, ubuf, cnt))
-			return -EFAULT;
-		buf[cnt] = 0;
-
-		val = simple_strtoul(buf, NULL, 0);
-	}
-
-	DRM_DEBUG_DRIVER("Manually setting min freq to %d\n", val);
+	DRM_DEBUG_DRIVER("Manually setting min freq to %llu\n", val);
 
 	ret = mutex_lock_interruptible(&dev_priv->rps.hw_lock);
 	if (ret)
@@ -2030,33 +1888,25 @@ i915_min_freq_write(struct file *filp, const char __user *ubuf, size_t cnt,
 	/*
 	 * Turbo will still be enabled, but won't go below the set value.
 	 */
-	dev_priv->rps.min_delay = val / GT_FREQUENCY_MULTIPLIER;
-
-	gen6_set_rps(dev, val / GT_FREQUENCY_MULTIPLIER);
+	do_div(val, GT_FREQUENCY_MULTIPLIER);
+	dev_priv->rps.min_delay = val;
+	gen6_set_rps(dev, val);
 	mutex_unlock(&dev_priv->rps.hw_lock);
 
-	return cnt;
+	return 0;
 }
 
-static const struct file_operations i915_min_freq_fops = {
-	.owner = THIS_MODULE,
-	.open = simple_open,
-	.read = i915_min_freq_read,
-	.write = i915_min_freq_write,
-	.llseek = default_llseek,
-};
+DEFINE_SIMPLE_ATTRIBUTE(i915_min_freq_fops,
+			i915_min_freq_get, i915_min_freq_set,
+			"%llu\n");
 
-static ssize_t
-i915_cache_sharing_read(struct file *filp,
-		   char __user *ubuf,
-		   size_t max,
-		   loff_t *ppos)
+static int
+i915_cache_sharing_get(void *data, u64 *val)
 {
-	struct drm_device *dev = filp->private_data;
+	struct drm_device *dev = data;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	char buf[80];
 	u32 snpcr;
-	int len, ret;
+	int ret;
 
 	if (!(IS_GEN6(dev) || IS_GEN7(dev)))
 		return -ENODEV;
@@ -2068,46 +1918,25 @@ i915_cache_sharing_read(struct file *filp,
 	snpcr = I915_READ(GEN6_MBCUNIT_SNPCR);
 	mutex_unlock(&dev_priv->dev->struct_mutex);
 
-	len = snprintf(buf, sizeof(buf),
-		       "%d\n", (snpcr & GEN6_MBC_SNPCR_MASK) >>
-		       GEN6_MBC_SNPCR_SHIFT);
+	*val = (snpcr & GEN6_MBC_SNPCR_MASK) >> GEN6_MBC_SNPCR_SHIFT;
 
-	if (len > sizeof(buf))
-		len = sizeof(buf);
-
-	return simple_read_from_buffer(ubuf, max, ppos, buf, len);
+	return 0;
 }
 
-static ssize_t
-i915_cache_sharing_write(struct file *filp,
-		  const char __user *ubuf,
-		  size_t cnt,
-		  loff_t *ppos)
+static int
+i915_cache_sharing_set(void *data, u64 val)
 {
-	struct drm_device *dev = filp->private_data;
+	struct drm_device *dev = data;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	char buf[20];
 	u32 snpcr;
-	int val = 1;
 
 	if (!(IS_GEN6(dev) || IS_GEN7(dev)))
 		return -ENODEV;
 
-	if (cnt > 0) {
-		if (cnt > sizeof(buf) - 1)
-			return -EINVAL;
-
-		if (copy_from_user(buf, ubuf, cnt))
-			return -EFAULT;
-		buf[cnt] = 0;
-
-		val = simple_strtoul(buf, NULL, 0);
-	}
-
-	if (val < 0 || val > 3)
+	if (val > 3)
 		return -EINVAL;
 
-	DRM_DEBUG_DRIVER("Manually setting uncore sharing to %d\n", val);
+	DRM_DEBUG_DRIVER("Manually setting uncore sharing to %llu\n", val);
 
 	/* Update the cache sharing policy here as well */
 	snpcr = I915_READ(GEN6_MBCUNIT_SNPCR);
@@ -2115,16 +1944,12 @@ i915_cache_sharing_write(struct file *filp,
 	snpcr |= (val << GEN6_MBC_SNPCR_SHIFT);
 	I915_WRITE(GEN6_MBCUNIT_SNPCR, snpcr);
 
-	return cnt;
+	return 0;
 }
 
-static const struct file_operations i915_cache_sharing_fops = {
-	.owner = THIS_MODULE,
-	.open = simple_open,
-	.read = i915_cache_sharing_read,
-	.write = i915_cache_sharing_write,
-	.llseek = default_llseek,
-};
+DEFINE_SIMPLE_ATTRIBUTE(i915_cache_sharing_fops,
+			i915_cache_sharing_get, i915_cache_sharing_set,
+			"%llu\n");
 
 /* As the drm_debugfs_init() routines are called before dev->dev_private is
  * allocated we need to hook into the minor for release. */
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index 4fa6beb14c77..3b315ba85a3e 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -1322,6 +1322,10 @@ static int i915_load_modeset_init(struct drm_device *dev)
 	/* Always safe in the mode setting case. */
 	/* FIXME: do pre/post-mode set stuff in core KMS code */
 	dev->vblank_disable_allowed = 1;
+	if (INTEL_INFO(dev)->num_pipes == 0) {
+		dev_priv->mm.suspended = 0;
+		return 0;
+	}
 
 	ret = intel_fbdev_init(dev);
 	if (ret)
@@ -1453,6 +1457,22 @@ static void i915_dump_device_info(struct drm_i915_private *dev_priv)
 }
 
 /**
+ * intel_early_sanitize_regs - clean up BIOS state
+ * @dev: DRM device
+ *
+ * This function must be called before we do any I915_READ or I915_WRITE. Its
+ * purpose is to clean up any state left by the BIOS that may affect us when
+ * reading and/or writing registers.
+ */
+static void intel_early_sanitize_regs(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (IS_HASWELL(dev))
+		I915_WRITE_NOTRACE(FPGA_DBG, FPGA_DBG_RM_NOCLAIM);
+}
+
+/**
  * i915_driver_load - setup chip and create an initial config
  * @dev: DRM device
  * @flags: startup flags
@@ -1498,6 +1518,28 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 		goto free_priv;
 	}
 
+	mmio_bar = IS_GEN2(dev) ? 1 : 0;
+	/* Before gen4, the registers and the GTT are behind different BARs.
+	 * However, from gen4 onwards, the registers and the GTT are shared
+	 * in the same BAR, so we want to restrict this ioremap from
+	 * clobbering the GTT which we want ioremap_wc instead. Fortunately,
+	 * the register BAR remains the same size for all the earlier
+	 * generations up to Ironlake.
+	 */
+	if (info->gen < 5)
+		mmio_size = 512*1024;
+	else
+		mmio_size = 2*1024*1024;
+
+	dev_priv->regs = pci_iomap(dev->pdev, mmio_bar, mmio_size);
+	if (!dev_priv->regs) {
+		DRM_ERROR("failed to map registers\n");
+		ret = -EIO;
+		goto put_bridge;
+	}
+
+	intel_early_sanitize_regs(dev);
+
 	ret = i915_gem_gtt_init(dev);
 	if (ret)
 		goto put_bridge;
@@ -1522,26 +1564,6 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 	if (IS_BROADWATER(dev) || IS_CRESTLINE(dev))
 		dma_set_coherent_mask(&dev->pdev->dev, DMA_BIT_MASK(32));
 
-	mmio_bar = IS_GEN2(dev) ? 1 : 0;
-	/* Before gen4, the registers and the GTT are behind different BARs.
-	 * However, from gen4 onwards, the registers and the GTT are shared
-	 * in the same BAR, so we want to restrict this ioremap from
-	 * clobbering the GTT which we want ioremap_wc instead. Fortunately,
-	 * the register BAR remains the same size for all the earlier
-	 * generations up to Ironlake.
-	 */
-	if (info->gen < 5)
-		mmio_size = 512*1024;
-	else
-		mmio_size = 2*1024*1024;
-
-	dev_priv->regs = pci_iomap(dev->pdev, mmio_bar, mmio_size);
-	if (!dev_priv->regs) {
-		DRM_ERROR("failed to map registers\n");
-		ret = -EIO;
-		goto put_gmch;
-	}
-
 	aperture_size = dev_priv->gtt.mappable_end;
 
 	dev_priv->gtt.mappable =
@@ -1612,16 +1634,15 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 	mutex_init(&dev_priv->rps.hw_lock);
 	mutex_init(&dev_priv->modeset_restore_lock);
 
-	if (IS_IVYBRIDGE(dev) || IS_HASWELL(dev))
-		dev_priv->num_pipe = 3;
-	else if (IS_MOBILE(dev) || !IS_GEN2(dev))
-		dev_priv->num_pipe = 2;
-	else
-		dev_priv->num_pipe = 1;
+	dev_priv->num_plane = 1;
+	if (IS_VALLEYVIEW(dev))
+		dev_priv->num_plane = 2;
 
-	ret = drm_vblank_init(dev, dev_priv->num_pipe);
-	if (ret)
-		goto out_gem_unload;
+	if (INTEL_INFO(dev)->num_pipes) {
+		ret = drm_vblank_init(dev, INTEL_INFO(dev)->num_pipes);
+		if (ret)
+			goto out_gem_unload;
+	}
 
 	/* Start out suspended */
 	dev_priv->mm.suspended = 1;
@@ -1636,9 +1657,11 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 
 	i915_setup_sysfs(dev);
 
-	/* Must be done after probing outputs */
-	intel_opregion_init(dev);
-	acpi_video_register();
+	if (INTEL_INFO(dev)->num_pipes) {
+		/* Must be done after probing outputs */
+		intel_opregion_init(dev);
+		acpi_video_register();
+	}
 
 	if (IS_GEN5(dev))
 		intel_gpu_ips_init(dev_priv);
@@ -1663,10 +1686,9 @@ out_mtrrfree:
 		dev_priv->mm.gtt_mtrr = -1;
 	}
 	io_mapping_free(dev_priv->gtt.mappable);
+	dev_priv->gtt.gtt_remove(dev);
 out_rmmap:
 	pci_iounmap(dev->pdev, dev_priv->regs);
-put_gmch:
-	dev_priv->gtt.gtt_remove(dev);
 put_bridge:
 	pci_dev_put(dev_priv->bridge_dev);
 free_priv:
diff --git a/drivers/gpu/drm/i915/i915_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index e9b57893db2b..9ebe895c17d6 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -121,9 +121,7 @@ MODULE_PARM_DESC(i915_enable_ppgtt,
 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)");
+		"Enable preliminary hardware support. (default: false)");
 
 int i915_disable_power_well __read_mostly = 0;
 module_param_named(disable_power_well, i915_disable_power_well, int, 0600);
@@ -142,75 +140,85 @@ extern int intel_agp_enabled;
 	.subdevice = PCI_ANY_ID,		\
 	.driver_data = (unsigned long) info }
 
+#define INTEL_QUANTA_VGA_DEVICE(info) {		\
+	.class = PCI_BASE_CLASS_DISPLAY << 16,	\
+	.class_mask = 0xff0000,			\
+	.vendor = 0x8086,			\
+	.device = 0x16a,			\
+	.subvendor = 0x152d,			\
+	.subdevice = 0x8990,			\
+	.driver_data = (unsigned long) info }
+
+
 static const struct intel_device_info intel_i830_info = {
-	.gen = 2, .is_mobile = 1, .cursor_needs_physical = 1,
+	.gen = 2, .is_mobile = 1, .cursor_needs_physical = 1, .num_pipes = 2,
 	.has_overlay = 1, .overlay_needs_physical = 1,
 };
 
 static const struct intel_device_info intel_845g_info = {
-	.gen = 2,
+	.gen = 2, .num_pipes = 1,
 	.has_overlay = 1, .overlay_needs_physical = 1,
 };
 
 static const struct intel_device_info intel_i85x_info = {
-	.gen = 2, .is_i85x = 1, .is_mobile = 1,
+	.gen = 2, .is_i85x = 1, .is_mobile = 1, .num_pipes = 2,
 	.cursor_needs_physical = 1,
 	.has_overlay = 1, .overlay_needs_physical = 1,
 };
 
 static const struct intel_device_info intel_i865g_info = {
-	.gen = 2,
+	.gen = 2, .num_pipes = 1,
 	.has_overlay = 1, .overlay_needs_physical = 1,
 };
 
 static const struct intel_device_info intel_i915g_info = {
-	.gen = 3, .is_i915g = 1, .cursor_needs_physical = 1,
+	.gen = 3, .is_i915g = 1, .cursor_needs_physical = 1, .num_pipes = 2,
 	.has_overlay = 1, .overlay_needs_physical = 1,
 };
 static const struct intel_device_info intel_i915gm_info = {
-	.gen = 3, .is_mobile = 1,
+	.gen = 3, .is_mobile = 1, .num_pipes = 2,
 	.cursor_needs_physical = 1,
 	.has_overlay = 1, .overlay_needs_physical = 1,
 	.supports_tv = 1,
 };
 static const struct intel_device_info intel_i945g_info = {
-	.gen = 3, .has_hotplug = 1, .cursor_needs_physical = 1,
+	.gen = 3, .has_hotplug = 1, .cursor_needs_physical = 1, .num_pipes = 2,
 	.has_overlay = 1, .overlay_needs_physical = 1,
 };
 static const struct intel_device_info intel_i945gm_info = {
-	.gen = 3, .is_i945gm = 1, .is_mobile = 1,
+	.gen = 3, .is_i945gm = 1, .is_mobile = 1, .num_pipes = 2,
 	.has_hotplug = 1, .cursor_needs_physical = 1,
 	.has_overlay = 1, .overlay_needs_physical = 1,
 	.supports_tv = 1,
 };
 
 static const struct intel_device_info intel_i965g_info = {
-	.gen = 4, .is_broadwater = 1,
+	.gen = 4, .is_broadwater = 1, .num_pipes = 2,
 	.has_hotplug = 1,
 	.has_overlay = 1,
 };
 
 static const struct intel_device_info intel_i965gm_info = {
-	.gen = 4, .is_crestline = 1,
+	.gen = 4, .is_crestline = 1, .num_pipes = 2,
 	.is_mobile = 1, .has_fbc = 1, .has_hotplug = 1,
 	.has_overlay = 1,
 	.supports_tv = 1,
 };
 
 static const struct intel_device_info intel_g33_info = {
-	.gen = 3, .is_g33 = 1,
+	.gen = 3, .is_g33 = 1, .num_pipes = 2,
 	.need_gfx_hws = 1, .has_hotplug = 1,
 	.has_overlay = 1,
 };
 
 static const struct intel_device_info intel_g45_info = {
-	.gen = 4, .is_g4x = 1, .need_gfx_hws = 1,
+	.gen = 4, .is_g4x = 1, .need_gfx_hws = 1, .num_pipes = 2,
 	.has_pipe_cxsr = 1, .has_hotplug = 1,
 	.has_bsd_ring = 1,
 };
 
 static const struct intel_device_info intel_gm45_info = {
-	.gen = 4, .is_g4x = 1,
+	.gen = 4, .is_g4x = 1, .num_pipes = 2,
 	.is_mobile = 1, .need_gfx_hws = 1, .has_fbc = 1,
 	.has_pipe_cxsr = 1, .has_hotplug = 1,
 	.supports_tv = 1,
@@ -218,26 +226,26 @@ static const struct intel_device_info intel_gm45_info = {
 };
 
 static const struct intel_device_info intel_pineview_info = {
-	.gen = 3, .is_g33 = 1, .is_pineview = 1, .is_mobile = 1,
+	.gen = 3, .is_g33 = 1, .is_pineview = 1, .is_mobile = 1, .num_pipes = 2,
 	.need_gfx_hws = 1, .has_hotplug = 1,
 	.has_overlay = 1,
 };
 
 static const struct intel_device_info intel_ironlake_d_info = {
-	.gen = 5,
+	.gen = 5, .num_pipes = 2,
 	.need_gfx_hws = 1, .has_hotplug = 1,
 	.has_bsd_ring = 1,
 };
 
 static const struct intel_device_info intel_ironlake_m_info = {
-	.gen = 5, .is_mobile = 1,
+	.gen = 5, .is_mobile = 1, .num_pipes = 2,
 	.need_gfx_hws = 1, .has_hotplug = 1,
 	.has_fbc = 1,
 	.has_bsd_ring = 1,
 };
 
 static const struct intel_device_info intel_sandybridge_d_info = {
-	.gen = 6,
+	.gen = 6, .num_pipes = 2,
 	.need_gfx_hws = 1, .has_hotplug = 1,
 	.has_bsd_ring = 1,
 	.has_blt_ring = 1,
@@ -246,7 +254,7 @@ static const struct intel_device_info intel_sandybridge_d_info = {
 };
 
 static const struct intel_device_info intel_sandybridge_m_info = {
-	.gen = 6, .is_mobile = 1,
+	.gen = 6, .is_mobile = 1, .num_pipes = 2,
 	.need_gfx_hws = 1, .has_hotplug = 1,
 	.has_fbc = 1,
 	.has_bsd_ring = 1,
@@ -255,61 +263,57 @@ static const struct intel_device_info intel_sandybridge_m_info = {
 	.has_force_wake = 1,
 };
 
+#define GEN7_FEATURES  \
+	.gen = 7, .num_pipes = 3, \
+	.need_gfx_hws = 1, .has_hotplug = 1, \
+	.has_bsd_ring = 1, \
+	.has_blt_ring = 1, \
+	.has_llc = 1, \
+	.has_force_wake = 1
+
 static const struct intel_device_info intel_ivybridge_d_info = {
-	.is_ivybridge = 1, .gen = 7,
-	.need_gfx_hws = 1, .has_hotplug = 1,
-	.has_bsd_ring = 1,
-	.has_blt_ring = 1,
-	.has_llc = 1,
-	.has_force_wake = 1,
+	GEN7_FEATURES,
+	.is_ivybridge = 1,
 };
 
 static const struct intel_device_info intel_ivybridge_m_info = {
-	.is_ivybridge = 1, .gen = 7, .is_mobile = 1,
-	.need_gfx_hws = 1, .has_hotplug = 1,
-	.has_fbc = 0,	/* FBC is not enabled on Ivybridge mobile yet */
-	.has_bsd_ring = 1,
-	.has_blt_ring = 1,
-	.has_llc = 1,
-	.has_force_wake = 1,
+	GEN7_FEATURES,
+	.is_ivybridge = 1,
+	.is_mobile = 1,
+};
+
+static const struct intel_device_info intel_ivybridge_q_info = {
+	GEN7_FEATURES,
+	.is_ivybridge = 1,
+	.num_pipes = 0, /* legal, last one wins */
 };
 
 static const struct intel_device_info intel_valleyview_m_info = {
-	.gen = 7, .is_mobile = 1,
-	.need_gfx_hws = 1, .has_hotplug = 1,
-	.has_fbc = 0,
-	.has_bsd_ring = 1,
-	.has_blt_ring = 1,
+	GEN7_FEATURES,
+	.is_mobile = 1,
+	.num_pipes = 2,
 	.is_valleyview = 1,
 	.display_mmio_offset = VLV_DISPLAY_BASE,
+	.has_llc = 0, /* legal, last one wins */
 };
 
 static const struct intel_device_info intel_valleyview_d_info = {
-	.gen = 7,
-	.need_gfx_hws = 1, .has_hotplug = 1,
-	.has_fbc = 0,
-	.has_bsd_ring = 1,
-	.has_blt_ring = 1,
+	GEN7_FEATURES,
+	.num_pipes = 2,
 	.is_valleyview = 1,
 	.display_mmio_offset = VLV_DISPLAY_BASE,
+	.has_llc = 0, /* legal, last one wins */
 };
 
 static const struct intel_device_info intel_haswell_d_info = {
-	.is_haswell = 1, .gen = 7,
-	.need_gfx_hws = 1, .has_hotplug = 1,
-	.has_bsd_ring = 1,
-	.has_blt_ring = 1,
-	.has_llc = 1,
-	.has_force_wake = 1,
+	GEN7_FEATURES,
+	.is_haswell = 1,
 };
 
 static const struct intel_device_info intel_haswell_m_info = {
-	.is_haswell = 1, .gen = 7, .is_mobile = 1,
-	.need_gfx_hws = 1, .has_hotplug = 1,
-	.has_bsd_ring = 1,
-	.has_blt_ring = 1,
-	.has_llc = 1,
-	.has_force_wake = 1,
+	GEN7_FEATURES,
+	.is_haswell = 1,
+	.is_mobile = 1,
 };
 
 static const struct pci_device_id pciidlist[] = {		/* aka */
@@ -356,6 +360,7 @@ static const struct pci_device_id pciidlist[] = {		/* aka */
 	INTEL_VGA_DEVICE(0x0152, &intel_ivybridge_d_info), /* GT1 desktop */
 	INTEL_VGA_DEVICE(0x0162, &intel_ivybridge_d_info), /* GT2 desktop */
 	INTEL_VGA_DEVICE(0x015a, &intel_ivybridge_d_info), /* GT1 server */
+	INTEL_QUANTA_VGA_DEVICE(&intel_ivybridge_q_info), /* Quanta transcode */
 	INTEL_VGA_DEVICE(0x016a, &intel_ivybridge_d_info), /* GT2 server */
 	INTEL_VGA_DEVICE(0x0402, &intel_haswell_d_info), /* GT1 desktop */
 	INTEL_VGA_DEVICE(0x0412, &intel_haswell_d_info), /* GT2 desktop */
@@ -394,6 +399,9 @@ static const struct pci_device_id pciidlist[] = {		/* aka */
 	INTEL_VGA_DEVICE(0x0D16, &intel_haswell_m_info), /* CRW GT2 mobile */
 	INTEL_VGA_DEVICE(0x0D26, &intel_haswell_m_info), /* CRW GT2 mobile */
 	INTEL_VGA_DEVICE(0x0f30, &intel_valleyview_m_info),
+	INTEL_VGA_DEVICE(0x0f31, &intel_valleyview_m_info),
+	INTEL_VGA_DEVICE(0x0f32, &intel_valleyview_m_info),
+	INTEL_VGA_DEVICE(0x0f33, &intel_valleyview_m_info),
 	INTEL_VGA_DEVICE(0x0157, &intel_valleyview_m_info),
 	INTEL_VGA_DEVICE(0x0155, &intel_valleyview_d_info),
 	{0, 0, 0}
@@ -408,6 +416,15 @@ void intel_detect_pch(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct pci_dev *pch;
 
+	/* In all current cases, num_pipes is equivalent to the PCH_NOP setting
+	 * (which really amounts to a PCH but no South Display).
+	 */
+	if (INTEL_INFO(dev)->num_pipes == 0) {
+		dev_priv->pch_type = PCH_NOP;
+		dev_priv->num_pch_pll = 0;
+		return;
+	}
+
 	/*
 	 * The reason to probe ISA bridge instead of Dev31:Fun0 is to
 	 * make graphics device passthrough work easy for VMM, that only
@@ -442,11 +459,13 @@ void intel_detect_pch(struct drm_device *dev)
 				dev_priv->num_pch_pll = 0;
 				DRM_DEBUG_KMS("Found LynxPoint PCH\n");
 				WARN_ON(!IS_HASWELL(dev));
+				WARN_ON(IS_ULT(dev));
 			} else if (id == INTEL_PCH_LPT_LP_DEVICE_ID_TYPE) {
 				dev_priv->pch_type = PCH_LPT;
 				dev_priv->num_pch_pll = 0;
 				DRM_DEBUG_KMS("Found LynxPoint LP PCH\n");
 				WARN_ON(!IS_HASWELL(dev));
+				WARN_ON(!IS_ULT(dev));
 			}
 			BUG_ON(dev_priv->num_pch_pll > I915_NUM_PLLS);
 		}
@@ -474,6 +493,7 @@ bool i915_semaphore_is_enabled(struct drm_device *dev)
 static int i915_drm_freeze(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc;
 
 	/* ignore lid events during suspend */
 	mutex_lock(&dev_priv->modeset_restore_lock);
@@ -497,10 +517,14 @@ static int i915_drm_freeze(struct drm_device *dev)
 
 		cancel_delayed_work_sync(&dev_priv->rps.delayed_resume_work);
 
-		intel_modeset_disable(dev);
-
 		drm_irq_uninstall(dev);
 		dev_priv->enable_hotplug_processing = false;
+		/*
+		 * Disable CRTCs directly since we want to preserve sw state
+		 * for _thaw.
+		 */
+		list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
+			dev_priv->display.crtc_disable(crtc);
 	}
 
 	i915_save_state(dev);
@@ -556,6 +580,24 @@ void intel_console_resume(struct work_struct *work)
 	console_unlock();
 }
 
+static void intel_resume_hotplug(struct drm_device *dev)
+{
+	struct drm_mode_config *mode_config = &dev->mode_config;
+	struct intel_encoder *encoder;
+
+	mutex_lock(&mode_config->mutex);
+	DRM_DEBUG_KMS("running encoder hotplug functions\n");
+
+	list_for_each_entry(encoder, &mode_config->encoder_list, base.head)
+		if (encoder->hot_plug)
+			encoder->hot_plug(encoder);
+
+	mutex_unlock(&mode_config->mutex);
+
+	/* Just fire off a uevent and let userspace tell us what to do */
+	drm_helper_hpd_irq_event(dev);
+}
+
 static int __i915_drm_thaw(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -578,7 +620,10 @@ static int __i915_drm_thaw(struct drm_device *dev)
 		drm_irq_install(dev);
 
 		intel_modeset_init_hw(dev);
-		intel_modeset_setup_hw_state(dev, false);
+
+		drm_modeset_lock_all(dev);
+		intel_modeset_setup_hw_state(dev, true);
+		drm_modeset_unlock_all(dev);
 
 		/*
 		 * ... but also need to make sure that hotplug processing
@@ -588,6 +633,8 @@ static int __i915_drm_thaw(struct drm_device *dev)
 		 * */
 		intel_hpd_init(dev);
 		dev_priv->enable_hotplug_processing = true;
+		/* Config may have changed between suspend and resume */
+		intel_resume_hotplug(dev);
 	}
 
 	intel_opregion_init(dev);
@@ -732,6 +779,7 @@ static int ironlake_do_reset(struct drm_device *dev)
 	int ret;
 
 	gdrst = I915_READ(MCHBAR_MIRROR_BASE + ILK_GDSR);
+	gdrst &= ~GRDOM_MASK;
 	I915_WRITE(MCHBAR_MIRROR_BASE + ILK_GDSR,
 		   gdrst | GRDOM_RENDER | GRDOM_RESET_ENABLE);
 	ret = wait_for(I915_READ(MCHBAR_MIRROR_BASE + ILK_GDSR) & 0x1, 500);
@@ -740,6 +788,7 @@ static int ironlake_do_reset(struct drm_device *dev)
 
 	/* We can't reset render&media without also resetting display ... */
 	gdrst = I915_READ(MCHBAR_MIRROR_BASE + ILK_GDSR);
+	gdrst &= ~GRDOM_MASK;
 	I915_WRITE(MCHBAR_MIRROR_BASE + ILK_GDSR,
 		   gdrst | GRDOM_MEDIA | GRDOM_RESET_ENABLE);
 	return wait_for(I915_READ(MCHBAR_MIRROR_BASE + ILK_GDSR) & 0x1, 500);
@@ -803,7 +852,7 @@ int intel_gpu_reset(struct drm_device *dev)
 
 	/* Also reset the gpu hangman. */
 	if (dev_priv->gpu_error.stop_rings) {
-		DRM_DEBUG("Simulated gpu hang, resetting stop_rings\n");
+		DRM_INFO("Simulated gpu hang, resetting stop_rings\n");
 		dev_priv->gpu_error.stop_rings = 0;
 		if (ret == -ENODEV) {
 			DRM_ERROR("Reset not implemented, but ignoring "
@@ -882,7 +931,11 @@ int i915_reset(struct drm_device *dev)
 			ring->init(ring);
 
 		i915_gem_context_init(dev);
-		i915_gem_init_ppgtt(dev);
+		if (dev_priv->mm.aliasing_ppgtt) {
+			ret = dev_priv->mm.aliasing_ppgtt->enable(dev);
+			if (ret)
+				i915_gem_cleanup_aliasing_ppgtt(dev);
+		}
 
 		/*
 		 * It would make sense to re-init all the other hw state, at
@@ -1147,6 +1200,27 @@ ilk_dummy_write(struct drm_i915_private *dev_priv)
 	I915_WRITE_NOTRACE(MI_MODE, 0);
 }
 
+static void
+hsw_unclaimed_reg_clear(struct drm_i915_private *dev_priv, u32 reg)
+{
+	if (IS_HASWELL(dev_priv->dev) &&
+	    (I915_READ_NOTRACE(FPGA_DBG) & FPGA_DBG_RM_NOCLAIM)) {
+		DRM_ERROR("Unknown unclaimed register before writing to %x\n",
+			  reg);
+		I915_WRITE_NOTRACE(FPGA_DBG, FPGA_DBG_RM_NOCLAIM);
+	}
+}
+
+static void
+hsw_unclaimed_reg_check(struct drm_i915_private *dev_priv, u32 reg)
+{
+	if (IS_HASWELL(dev_priv->dev) &&
+	    (I915_READ_NOTRACE(FPGA_DBG) & FPGA_DBG_RM_NOCLAIM)) {
+		DRM_ERROR("Unclaimed write to %x\n", reg);
+		I915_WRITE_NOTRACE(FPGA_DBG, FPGA_DBG_RM_NOCLAIM);
+	}
+}
+
 #define __i915_read(x, y) \
 u##x i915_read##x(struct drm_i915_private *dev_priv, u32 reg) { \
 	u##x val = 0; \
@@ -1183,18 +1257,12 @@ void i915_write##x(struct drm_i915_private *dev_priv, u32 reg, u##x val) { \
 	} \
 	if (IS_GEN5(dev_priv->dev)) \
 		ilk_dummy_write(dev_priv); \
-	if (IS_HASWELL(dev_priv->dev) && (I915_READ_NOTRACE(GEN7_ERR_INT) & ERR_INT_MMIO_UNCLAIMED)) { \
-		DRM_ERROR("Unknown unclaimed register before writing to %x\n", reg); \
-		I915_WRITE_NOTRACE(GEN7_ERR_INT, ERR_INT_MMIO_UNCLAIMED); \
-	} \
+	hsw_unclaimed_reg_clear(dev_priv, reg); \
 	write##y(val, dev_priv->regs + reg); \
 	if (unlikely(__fifo_ret)) { \
 		gen6_gt_check_fifodbg(dev_priv); \
 	} \
-	if (IS_HASWELL(dev_priv->dev) && (I915_READ_NOTRACE(GEN7_ERR_INT) & ERR_INT_MMIO_UNCLAIMED)) { \
-		DRM_ERROR("Unclaimed write to %x\n", reg); \
-		writel(ERR_INT_MMIO_UNCLAIMED, dev_priv->regs + GEN7_ERR_INT);	\
-	} \
+	hsw_unclaimed_reg_check(dev_priv, reg); \
 }
 __i915_write(8, b)
 __i915_write(16, w)
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 01769e2a9953..d5dcf7fe1ee9 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -86,6 +86,19 @@ enum port {
 };
 #define port_name(p) ((p) + 'A')
 
+enum hpd_pin {
+	HPD_NONE = 0,
+	HPD_PORT_A = HPD_NONE, /* PORT_A is internal */
+	HPD_TV = HPD_NONE,     /* TV is known to be unreliable */
+	HPD_CRT,
+	HPD_SDVO_B,
+	HPD_SDVO_C,
+	HPD_PORT_B,
+	HPD_PORT_C,
+	HPD_PORT_D,
+	HPD_NUM_PINS
+};
+
 #define I915_GEM_GPU_DOMAINS \
 	(I915_GEM_DOMAIN_RENDER | \
 	 I915_GEM_DOMAIN_SAMPLER | \
@@ -93,7 +106,7 @@ enum port {
 	 I915_GEM_DOMAIN_INSTRUCTION | \
 	 I915_GEM_DOMAIN_VERTEX)
 
-#define for_each_pipe(p) for ((p) = 0; (p) < dev_priv->num_pipe; (p)++)
+#define for_each_pipe(p) for ((p) = 0; (p) < INTEL_INFO(dev)->num_pipes; (p)++)
 
 #define for_each_encoder_on_crtc(dev, __crtc, intel_encoder) \
 	list_for_each_entry((intel_encoder), &(dev)->mode_config.encoder_list, base.head) \
@@ -182,9 +195,9 @@ struct drm_i915_master_private {
 	struct _drm_i915_sarea *sarea_priv;
 };
 #define I915_FENCE_REG_NONE -1
-#define I915_MAX_NUM_FENCES 16
-/* 16 fences + sign bit for FENCE_REG_NONE */
-#define I915_MAX_NUM_FENCE_BITS 5
+#define I915_MAX_NUM_FENCES 32
+/* 32 fences + sign bit for FENCE_REG_NONE */
+#define I915_MAX_NUM_FENCE_BITS 6
 
 struct drm_i915_fence_reg {
 	struct list_head lru_list;
@@ -243,7 +256,7 @@ struct drm_i915_error_state {
 			int page_count;
 			u32 gtt_offset;
 			u32 *pages[0];
-		} *ringbuffer, *batchbuffer;
+		} *ringbuffer, *batchbuffer, *ctx;
 		struct drm_i915_error_request {
 			long jiffies;
 			u32 seqno;
@@ -271,6 +284,9 @@ struct drm_i915_error_state {
 	struct intel_display_error_state *display;
 };
 
+struct intel_crtc_config;
+struct intel_crtc;
+
 struct drm_i915_display_funcs {
 	bool (*fbc_enabled)(struct drm_device *dev);
 	void (*enable_fbc)(struct drm_crtc *crtc, unsigned long interval);
@@ -283,9 +299,11 @@ struct drm_i915_display_funcs {
 	void (*update_linetime_wm)(struct drm_device *dev, int pipe,
 				 struct drm_display_mode *mode);
 	void (*modeset_global_resources)(struct drm_device *dev);
+	/* Returns the active state of the crtc, and if the crtc is active,
+	 * fills out the pipe-config with the hw state. */
+	bool (*get_pipe_config)(struct intel_crtc *,
+				struct intel_crtc_config *);
 	int (*crtc_mode_set)(struct drm_crtc *crtc,
-			     struct drm_display_mode *mode,
-			     struct drm_display_mode *adjusted_mode,
 			     int x, int y,
 			     struct drm_framebuffer *old_fb);
 	void (*crtc_enable)(struct drm_crtc *crtc);
@@ -341,6 +359,7 @@ struct drm_i915_gt_funcs {
 
 struct intel_device_info {
 	u32 display_mmio_offset;
+	u8 num_pipes:3;
 	u8 gen;
 	u8 is_mobile:1;
 	u8 is_i85x:1;
@@ -430,6 +449,7 @@ struct i915_hw_ppgtt {
 			       struct sg_table *st,
 			       unsigned int pg_start,
 			       enum i915_cache_level cache_level);
+	int (*enable)(struct drm_device *dev);
 	void (*cleanup)(struct i915_hw_ppgtt *ppgtt);
 };
 
@@ -460,6 +480,7 @@ enum intel_pch {
 	PCH_IBX,	/* Ibexpeak PCH */
 	PCH_CPT,	/* Cougarpoint PCH */
 	PCH_LPT,	/* Lynxpoint PCH */
+	PCH_NOP,
 };
 
 enum intel_sbi_destination {
@@ -647,6 +668,7 @@ struct intel_gen6_power_mgmt {
 	u8 cur_delay;
 	u8 min_delay;
 	u8 max_delay;
+	u8 hw_max;
 
 	struct delayed_work delayed_resume_work;
 
@@ -905,16 +927,24 @@ typedef struct drm_i915_private {
 	struct mutex dpio_lock;
 
 	/** Cached value of IMR to avoid reads in updating the bitfield */
-	u32 pipestat[2];
 	u32 irq_mask;
 	u32 gt_irq_mask;
 
-	u32 hotplug_supported_mask;
 	struct work_struct hotplug_work;
 	bool enable_hotplug_processing;
+	struct {
+		unsigned long hpd_last_jiffies;
+		int hpd_cnt;
+		enum {
+			HPD_ENABLED = 0,
+			HPD_DISABLED = 1,
+			HPD_MARK_DISABLED = 2
+		} hpd_mark;
+	} hpd_stats[HPD_NUM_PINS];
+	struct timer_list hotplug_reenable_timer;
 
-	int num_pipe;
 	int num_pch_pll;
+	int num_plane;
 
 	unsigned long cfb_size;
 	unsigned int cfb_fb;
@@ -928,9 +958,14 @@ typedef struct drm_i915_private {
 	struct intel_overlay *overlay;
 	unsigned int sprite_scaling_enabled;
 
+	/* backlight */
+	struct {
+		int level;
+		bool enabled;
+		struct backlight_device *device;
+	} backlight;
+
 	/* LVDS info */
-	int backlight_level;  /* restore backlight to this value */
-	bool backlight_enabled;
 	struct drm_display_mode *lfp_lvds_vbt_mode; /* if any */
 	struct drm_display_mode *sdvo_lvds_vbt_mode; /* if any */
 
@@ -941,6 +976,7 @@ typedef struct drm_i915_private {
 	unsigned int int_crt_support:1;
 	unsigned int lvds_use_ssc:1;
 	unsigned int display_clock_mode:1;
+	unsigned int fdi_rx_polarity_inverted:1;
 	int lvds_ssc_freq;
 	unsigned int bios_lvds_val; /* initial [PCH_]LVDS reg val in VBIOS */
 	struct {
@@ -1032,8 +1068,6 @@ typedef struct drm_i915_private {
 	 */
 	struct work_struct console_resume_work;
 
-	struct backlight_device *backlight;
-
 	struct drm_property *broadcast_rgb_property;
 	struct drm_property *force_audio_property;
 
@@ -1340,6 +1374,7 @@ struct drm_i915_file_private {
 #define HAS_PIPE_CONTROL(dev) (INTEL_INFO(dev)->gen >= 5)
 
 #define HAS_DDI(dev)		(IS_HASWELL(dev))
+#define HAS_POWER_WELL(dev)	(IS_HASWELL(dev))
 
 #define INTEL_PCH_DEVICE_ID_MASK		0xff00
 #define INTEL_PCH_IBX_DEVICE_ID_TYPE		0x3b00
@@ -1352,6 +1387,7 @@ struct drm_i915_file_private {
 #define HAS_PCH_LPT(dev) (INTEL_PCH_TYPE(dev) == PCH_LPT)
 #define HAS_PCH_CPT(dev) (INTEL_PCH_TYPE(dev) == PCH_CPT)
 #define HAS_PCH_IBX(dev) (INTEL_PCH_TYPE(dev) == PCH_IBX)
+#define HAS_PCH_NOP(dev) (INTEL_PCH_TYPE(dev) == PCH_NOP)
 #define HAS_PCH_SPLIT(dev) (INTEL_PCH_TYPE(dev) != PCH_NONE)
 
 #define HAS_FORCE_WAKE(dev) (INTEL_INFO(dev)->has_force_wake)
@@ -1529,17 +1565,12 @@ void i915_gem_lastclose(struct drm_device *dev);
 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;
-	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);
+	struct sg_page_iter sg_iter;
+
+	for_each_sg_page(obj->pages->sgl, &sg_iter, obj->pages->nents, n)
+		return sg_page_iter_page(&sg_iter);
+
+	return NULL;
 }
 static inline void i915_gem_object_pin_pages(struct drm_i915_gem_object *obj)
 {
@@ -1624,7 +1655,6 @@ int __must_check i915_gem_init(struct drm_device *dev);
 int __must_check i915_gem_init_hw(struct drm_device *dev);
 void i915_gem_l3_remap(struct drm_device *dev);
 void i915_gem_init_swizzling(struct drm_device *dev);
-void i915_gem_init_ppgtt(struct drm_device *dev);
 void i915_gem_cleanup_ringbuffer(struct drm_device *dev);
 int __must_check i915_gpu_idle(struct drm_device *dev);
 int __must_check i915_gem_idle(struct drm_device *dev);
@@ -1718,6 +1748,11 @@ void i915_gem_stolen_cleanup_compression(struct drm_device *dev);
 void i915_gem_cleanup_stolen(struct drm_device *dev);
 struct drm_i915_gem_object *
 i915_gem_object_create_stolen(struct drm_device *dev, u32 size);
+struct drm_i915_gem_object *
+i915_gem_object_create_stolen_for_preallocated(struct drm_device *dev,
+					       u32 stolen_offset,
+					       u32 gtt_offset,
+					       u32 size);
 void i915_gem_object_release_stolen(struct drm_i915_gem_object *obj);
 
 /* i915_gem_tiling.c */
@@ -1848,6 +1883,8 @@ int __gen6_gt_wait_for_fifo(struct drm_i915_private *dev_priv);
 
 int sandybridge_pcode_read(struct drm_i915_private *dev_priv, u8 mbox, u32 *val);
 int sandybridge_pcode_write(struct drm_i915_private *dev_priv, u8 mbox, u32 val);
+int valleyview_punit_read(struct drm_i915_private *dev_priv, u8 addr, u32 *val);
+int valleyview_punit_write(struct drm_i915_private *dev_priv, u8 addr, u32 val);
 
 #define __i915_read(x, y) \
 	u##x i915_read##x(struct drm_i915_private *dev_priv, u32 reg);
@@ -1901,4 +1938,9 @@ static inline uint32_t i915_vgacntrl_reg(struct drm_device *dev)
 		return VGACNTRL;
 }
 
+static inline void __user *to_user_ptr(u64 address)
+{
+	return (void __user *)(uintptr_t)address;
+}
+
 #endif
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index 0e207e6e0df8..6be940effefd 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -411,10 +411,9 @@ i915_gem_shmem_pread(struct drm_device *dev,
 	int obj_do_bit17_swizzling, page_do_bit17_swizzling;
 	int prefaulted = 0;
 	int needs_clflush = 0;
-	struct scatterlist *sg;
-	int i;
+	struct sg_page_iter sg_iter;
 
-	user_data = (char __user *) (uintptr_t) args->data_ptr;
+	user_data = to_user_ptr(args->data_ptr);
 	remain = args->size;
 
 	obj_do_bit17_swizzling = i915_gem_object_needs_bit17_swizzle(obj);
@@ -441,11 +440,9 @@ i915_gem_shmem_pread(struct drm_device *dev,
 
 	offset = args->offset;
 
-	for_each_sg(obj->pages->sgl, sg, obj->pages->nents, i) {
-		struct page *page;
-
-		if (i < offset >> PAGE_SHIFT)
-			continue;
+	for_each_sg_page(obj->pages->sgl, &sg_iter, obj->pages->nents,
+			 offset >> PAGE_SHIFT) {
+		struct page *page = sg_page_iter_page(&sg_iter);
 
 		if (remain <= 0)
 			break;
@@ -460,7 +457,6 @@ i915_gem_shmem_pread(struct drm_device *dev,
 		if ((shmem_page_offset + page_length) > PAGE_SIZE)
 			page_length = PAGE_SIZE - shmem_page_offset;
 
-		page = sg_page(sg);
 		page_do_bit17_swizzling = obj_do_bit17_swizzling &&
 			(page_to_phys(page) & (1 << 17)) != 0;
 
@@ -522,7 +518,7 @@ i915_gem_pread_ioctl(struct drm_device *dev, void *data,
 		return 0;
 
 	if (!access_ok(VERIFY_WRITE,
-		       (char __user *)(uintptr_t)args->data_ptr,
+		       to_user_ptr(args->data_ptr),
 		       args->size))
 		return -EFAULT;
 
@@ -613,7 +609,7 @@ i915_gem_gtt_pwrite_fast(struct drm_device *dev,
 	if (ret)
 		goto out_unpin;
 
-	user_data = (char __user *) (uintptr_t) args->data_ptr;
+	user_data = to_user_ptr(args->data_ptr);
 	remain = args->size;
 
 	offset = obj->gtt_offset + args->offset;
@@ -732,10 +728,9 @@ i915_gem_shmem_pwrite(struct drm_device *dev,
 	int hit_slowpath = 0;
 	int needs_clflush_after = 0;
 	int needs_clflush_before = 0;
-	int i;
-	struct scatterlist *sg;
+	struct sg_page_iter sg_iter;
 
-	user_data = (char __user *) (uintptr_t) args->data_ptr;
+	user_data = to_user_ptr(args->data_ptr);
 	remain = args->size;
 
 	obj_do_bit17_swizzling = i915_gem_object_needs_bit17_swizzle(obj);
@@ -768,13 +763,11 @@ i915_gem_shmem_pwrite(struct drm_device *dev,
 	offset = args->offset;
 	obj->dirty = 1;
 
-	for_each_sg(obj->pages->sgl, sg, obj->pages->nents, i) {
-		struct page *page;
+	for_each_sg_page(obj->pages->sgl, &sg_iter, obj->pages->nents,
+			 offset >> PAGE_SHIFT) {
+		struct page *page = sg_page_iter_page(&sg_iter);
 		int partial_cacheline_write;
 
-		if (i < offset >> PAGE_SHIFT)
-			continue;
-
 		if (remain <= 0)
 			break;
 
@@ -796,7 +789,6 @@ i915_gem_shmem_pwrite(struct drm_device *dev,
 			((shmem_page_offset | page_length)
 				& (boot_cpu_data.x86_clflush_size - 1));
 
-		page = sg_page(sg);
 		page_do_bit17_swizzling = obj_do_bit17_swizzling &&
 			(page_to_phys(page) & (1 << 17)) != 0;
 
@@ -867,11 +859,11 @@ i915_gem_pwrite_ioctl(struct drm_device *dev, void *data,
 		return 0;
 
 	if (!access_ok(VERIFY_READ,
-		       (char __user *)(uintptr_t)args->data_ptr,
+		       to_user_ptr(args->data_ptr),
 		       args->size))
 		return -EFAULT;
 
-	ret = fault_in_multipages_readable((char __user *)(uintptr_t)args->data_ptr,
+	ret = fault_in_multipages_readable(to_user_ptr(args->data_ptr),
 					   args->size);
 	if (ret)
 		return -EFAULT;
@@ -1633,9 +1625,8 @@ i915_gem_object_is_purgeable(struct drm_i915_gem_object *obj)
 static void
 i915_gem_object_put_pages_gtt(struct drm_i915_gem_object *obj)
 {
-	int page_count = obj->base.size / PAGE_SIZE;
-	struct scatterlist *sg;
-	int ret, i;
+	struct sg_page_iter sg_iter;
+	int ret;
 
 	BUG_ON(obj->madv == __I915_MADV_PURGED);
 
@@ -1655,8 +1646,8 @@ i915_gem_object_put_pages_gtt(struct drm_i915_gem_object *obj)
 	if (obj->madv == I915_MADV_DONTNEED)
 		obj->dirty = 0;
 
-	for_each_sg(obj->pages->sgl, sg, page_count, i) {
-		struct page *page = sg_page(sg);
+	for_each_sg_page(obj->pages->sgl, &sg_iter, obj->pages->nents, 0) {
+		struct page *page = sg_page_iter_page(&sg_iter);
 
 		if (obj->dirty)
 			set_page_dirty(page);
@@ -1757,7 +1748,9 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
 	struct address_space *mapping;
 	struct sg_table *st;
 	struct scatterlist *sg;
+	struct sg_page_iter sg_iter;
 	struct page *page;
+	unsigned long last_pfn = 0;	/* suppress gcc warning */
 	gfp_t gfp;
 
 	/* Assert that the object is not currently in any GPU domain. As it
@@ -1787,7 +1780,9 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
 	gfp = mapping_gfp_mask(mapping);
 	gfp |= __GFP_NORETRY | __GFP_NOWARN | __GFP_NO_KSWAPD;
 	gfp &= ~(__GFP_IO | __GFP_WAIT);
-	for_each_sg(st->sgl, sg, page_count, i) {
+	sg = st->sgl;
+	st->nents = 0;
+	for (i = 0; i < page_count; i++) {
 		page = shmem_read_mapping_page_gfp(mapping, i, gfp);
 		if (IS_ERR(page)) {
 			i915_gem_purge(dev_priv, page_count);
@@ -1810,9 +1805,18 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
 			gfp &= ~(__GFP_IO | __GFP_WAIT);
 		}
 
-		sg_set_page(sg, page, PAGE_SIZE, 0);
+		if (!i || page_to_pfn(page) != last_pfn + 1) {
+			if (i)
+				sg = sg_next(sg);
+			st->nents++;
+			sg_set_page(sg, page, PAGE_SIZE, 0);
+		} else {
+			sg->length += PAGE_SIZE;
+		}
+		last_pfn = page_to_pfn(page);
 	}
 
+	sg_mark_end(sg);
 	obj->pages = st;
 
 	if (i915_gem_object_needs_bit17_swizzle(obj))
@@ -1821,8 +1825,9 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
 	return 0;
 
 err_pages:
-	for_each_sg(st->sgl, sg, i, page_count)
-		page_cache_release(sg_page(sg));
+	sg_mark_end(sg);
+	for_each_sg_page(st->sgl, &sg_iter, st->nents, 0)
+		page_cache_release(sg_page_iter_page(&sg_iter));
 	sg_free_table(st);
 	kfree(st);
 	return PTR_ERR(page);
@@ -2123,11 +2128,11 @@ static void i915_gem_reset_fences(struct drm_device *dev)
 	for (i = 0; i < dev_priv->num_fence_regs; i++) {
 		struct drm_i915_fence_reg *reg = &dev_priv->fence_regs[i];
 
-		i915_gem_write_fence(dev, i, NULL);
-
 		if (reg->obj)
 			i915_gem_object_fence_lost(reg->obj);
 
+		i915_gem_write_fence(dev, i, NULL);
+
 		reg->pin_count = 0;
 		reg->obj = NULL;
 		INIT_LIST_HEAD(&reg->lru_list);
@@ -2678,17 +2683,35 @@ static inline int fence_number(struct drm_i915_private *dev_priv,
 	return fence - dev_priv->fence_regs;
 }
 
+static void i915_gem_write_fence__ipi(void *data)
+{
+	wbinvd();
+}
+
 static void i915_gem_object_update_fence(struct drm_i915_gem_object *obj,
 					 struct drm_i915_fence_reg *fence,
 					 bool enable)
 {
-	struct drm_i915_private *dev_priv = obj->base.dev->dev_private;
-	int reg = fence_number(dev_priv, fence);
-
-	i915_gem_write_fence(obj->base.dev, reg, enable ? obj : NULL);
+	struct drm_device *dev = obj->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int fence_reg = fence_number(dev_priv, fence);
+
+	/* In order to fully serialize access to the fenced region and
+	 * the update to the fence register we need to take extreme
+	 * measures on SNB+. In theory, the write to the fence register
+	 * flushes all memory transactions before, and coupled with the
+	 * mb() placed around the register write we serialise all memory
+	 * operations with respect to the changes in the tiler. Yet, on
+	 * SNB+ we need to take a step further and emit an explicit wbinvd()
+	 * on each processor in order to manually flush all memory
+	 * transactions before updating the fence register.
+	 */
+	if (HAS_LLC(obj->base.dev))
+		on_each_cpu(i915_gem_write_fence__ipi, NULL, 1);
+	i915_gem_write_fence(dev, fence_reg, enable ? obj : NULL);
 
 	if (enable) {
-		obj->fence_reg = reg;
+		obj->fence_reg = fence_reg;
 		fence->obj = obj;
 		list_move_tail(&fence->lru_list, &dev_priv->mm.fence_list);
 	} else {
@@ -2717,6 +2740,7 @@ int
 i915_gem_object_put_fence(struct drm_i915_gem_object *obj)
 {
 	struct drm_i915_private *dev_priv = obj->base.dev->dev_private;
+	struct drm_i915_fence_reg *fence;
 	int ret;
 
 	ret = i915_gem_object_wait_fence(obj);
@@ -2726,10 +2750,10 @@ i915_gem_object_put_fence(struct drm_i915_gem_object *obj)
 	if (obj->fence_reg == I915_FENCE_REG_NONE)
 		return 0;
 
-	i915_gem_object_update_fence(obj,
-				     &dev_priv->fence_regs[obj->fence_reg],
-				     false);
+	fence = &dev_priv->fence_regs[obj->fence_reg];
+
 	i915_gem_object_fence_lost(obj);
+	i915_gem_object_update_fence(obj, fence, false);
 
 	return 0;
 }
@@ -3986,6 +4010,12 @@ i915_gem_init_hw(struct drm_device *dev)
 	if (IS_HASWELL(dev) && (I915_READ(0x120010) == 1))
 		I915_WRITE(0x9008, I915_READ(0x9008) | 0xf0000);
 
+	if (HAS_PCH_NOP(dev)) {
+		u32 temp = I915_READ(GEN7_MSG_CTL);
+		temp &= ~(WAIT_FOR_PCH_FLR_ACK | WAIT_FOR_PCH_RESET_ACK);
+		I915_WRITE(GEN7_MSG_CTL, temp);
+	}
+
 	i915_gem_l3_remap(dev);
 
 	i915_gem_init_swizzling(dev);
@@ -3999,7 +4029,13 @@ i915_gem_init_hw(struct drm_device *dev)
 	 * contexts before PPGTT.
 	 */
 	i915_gem_context_init(dev);
-	i915_gem_init_ppgtt(dev);
+	if (dev_priv->mm.aliasing_ppgtt) {
+		ret = dev_priv->mm.aliasing_ppgtt->enable(dev);
+		if (ret) {
+			i915_gem_cleanup_aliasing_ppgtt(dev);
+			DRM_INFO("PPGTT enable failed. This is not fatal, but unexpected\n");
+		}
+	}
 
 	return 0;
 }
@@ -4010,7 +4046,16 @@ int i915_gem_init(struct drm_device *dev)
 	int ret;
 
 	mutex_lock(&dev->struct_mutex);
+
+	if (IS_VALLEYVIEW(dev)) {
+		/* VLVA0 (potential hack), BIOS isn't actually waking us */
+		I915_WRITE(VLV_GTLC_WAKE_CTRL, 1);
+		if (wait_for((I915_READ(VLV_GTLC_PW_STATUS) & 1) == 1, 10))
+			DRM_DEBUG_DRIVER("allow wake ack timed out\n");
+	}
+
 	i915_gem_init_global_gtt(dev);
+
 	ret = i915_gem_init_hw(dev);
 	mutex_unlock(&dev->struct_mutex);
 	if (ret) {
@@ -4145,7 +4190,9 @@ i915_gem_load(struct drm_device *dev)
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		dev_priv->fence_reg_start = 3;
 
-	if (INTEL_INFO(dev)->gen >= 4 || IS_I945G(dev) || IS_I945GM(dev) || IS_G33(dev))
+	if (INTEL_INFO(dev)->gen >= 7 && !IS_VALLEYVIEW(dev))
+		dev_priv->num_fence_regs = 32;
+	else if (INTEL_INFO(dev)->gen >= 4 || IS_I945G(dev) || IS_I945GM(dev) || IS_G33(dev))
 		dev_priv->num_fence_regs = 16;
 	else
 		dev_priv->num_fence_regs = 8;
@@ -4327,7 +4374,7 @@ i915_gem_phys_pwrite(struct drm_device *dev,
 		     struct drm_file *file_priv)
 {
 	void *vaddr = obj->phys_obj->handle->vaddr + args->offset;
-	char __user *user_data = (char __user *) (uintptr_t) args->data_ptr;
+	char __user *user_data = to_user_ptr(args->data_ptr);
 
 	if (__copy_from_user_inatomic_nocache(vaddr, user_data, args->size)) {
 		unsigned long unwritten;
diff --git a/drivers/gpu/drm/i915/i915_gem_context.c b/drivers/gpu/drm/i915/i915_gem_context.c
index 94d873a6cffb..a1e8ecb6adf6 100644
--- a/drivers/gpu/drm/i915/i915_gem_context.c
+++ b/drivers/gpu/drm/i915/i915_gem_context.c
@@ -152,6 +152,13 @@ create_hw_context(struct drm_device *dev,
 		return ERR_PTR(-ENOMEM);
 	}
 
+	if (INTEL_INFO(dev)->gen >= 7) {
+		ret = i915_gem_object_set_cache_level(ctx->obj,
+						      I915_CACHE_LLC_MLC);
+		if (ret)
+			goto err_out;
+	}
+
 	/* The ring associated with the context object is handled by the normal
 	 * object tracking code. We give an initial ring value simple to pass an
 	 * assertion in the context switch code.
diff --git a/drivers/gpu/drm/i915/i915_gem_dmabuf.c b/drivers/gpu/drm/i915/i915_gem_dmabuf.c
index 6a5af6828624..dc53a527126b 100644
--- a/drivers/gpu/drm/i915/i915_gem_dmabuf.c
+++ b/drivers/gpu/drm/i915/i915_gem_dmabuf.c
@@ -62,7 +62,7 @@ static struct sg_table *i915_gem_map_dma_buf(struct dma_buf_attachment *attachme
 	src = obj->pages->sgl;
 	dst = st->sgl;
 	for (i = 0; i < obj->pages->nents; i++) {
-		sg_set_page(dst, sg_page(src), PAGE_SIZE, 0);
+		sg_set_page(dst, sg_page(src), src->length, 0);
 		dst = sg_next(dst);
 		src = sg_next(src);
 	}
@@ -105,7 +105,7 @@ static void *i915_gem_dmabuf_vmap(struct dma_buf *dma_buf)
 {
 	struct drm_i915_gem_object *obj = dma_buf->priv;
 	struct drm_device *dev = obj->base.dev;
-	struct scatterlist *sg;
+	struct sg_page_iter sg_iter;
 	struct page **pages;
 	int ret, i;
 
@@ -124,14 +124,15 @@ static void *i915_gem_dmabuf_vmap(struct dma_buf *dma_buf)
 
 	ret = -ENOMEM;
 
-	pages = drm_malloc_ab(obj->pages->nents, sizeof(struct page *));
+	pages = drm_malloc_ab(obj->base.size >> PAGE_SHIFT, sizeof(*pages));
 	if (pages == NULL)
 		goto error;
 
-	for_each_sg(obj->pages->sgl, sg, obj->pages->nents, i)
-		pages[i] = sg_page(sg);
+	i = 0;
+	for_each_sg_page(obj->pages->sgl, &sg_iter, obj->pages->nents, 0)
+		pages[i++] = sg_page_iter_page(&sg_iter);
 
-	obj->dma_buf_vmapping = vmap(pages, obj->pages->nents, 0, PAGE_KERNEL);
+	obj->dma_buf_vmapping = vmap(pages, i, 0, PAGE_KERNEL);
 	drm_free_large(pages);
 
 	if (!obj->dma_buf_vmapping)
@@ -271,7 +272,6 @@ struct drm_gem_object *i915_gem_prime_import(struct drm_device *dev,
 			 * refcount on gem itself instead of f_count of dmabuf.
 			 */
 			drm_gem_object_reference(&obj->base);
-			dma_buf_put(dma_buf);
 			return &obj->base;
 		}
 	}
@@ -281,6 +281,8 @@ struct drm_gem_object *i915_gem_prime_import(struct drm_device *dev,
 	if (IS_ERR(attach))
 		return ERR_CAST(attach);
 
+	get_dma_buf(dma_buf);
+
 	obj = i915_gem_object_alloc(dev);
 	if (obj == NULL) {
 		ret = -ENOMEM;
@@ -300,5 +302,7 @@ struct drm_gem_object *i915_gem_prime_import(struct drm_device *dev,
 
 fail_detach:
 	dma_buf_detach(dma_buf, attach);
+	dma_buf_put(dma_buf);
+
 	return ERR_PTR(ret);
 }
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index 9a48e1a2d417..117ce3813681 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -305,7 +305,7 @@ i915_gem_execbuffer_relocate_object(struct drm_i915_gem_object *obj,
 	struct drm_i915_gem_exec_object2 *entry = obj->exec_entry;
 	int remain, ret;
 
-	user_relocs = (void __user *)(uintptr_t)entry->relocs_ptr;
+	user_relocs = to_user_ptr(entry->relocs_ptr);
 
 	remain = entry->relocation_count;
 	while (remain) {
@@ -359,8 +359,7 @@ i915_gem_execbuffer_relocate_object_slow(struct drm_i915_gem_object *obj,
 }
 
 static int
-i915_gem_execbuffer_relocate(struct drm_device *dev,
-			     struct eb_objects *eb)
+i915_gem_execbuffer_relocate(struct eb_objects *eb)
 {
 	struct drm_i915_gem_object *obj;
 	int ret = 0;
@@ -475,7 +474,6 @@ i915_gem_execbuffer_unreserve_object(struct drm_i915_gem_object *obj)
 
 static int
 i915_gem_execbuffer_reserve(struct intel_ring_buffer *ring,
-			    struct drm_file *file,
 			    struct list_head *objects,
 			    bool *need_relocs)
 {
@@ -618,7 +616,7 @@ i915_gem_execbuffer_relocate_slow(struct drm_device *dev,
 		u64 invalid_offset = (u64)-1;
 		int j;
 
-		user_relocs = (void __user *)(uintptr_t)exec[i].relocs_ptr;
+		user_relocs = to_user_ptr(exec[i].relocs_ptr);
 
 		if (copy_from_user(reloc+total, user_relocs,
 				   exec[i].relocation_count * sizeof(*reloc))) {
@@ -663,7 +661,7 @@ i915_gem_execbuffer_relocate_slow(struct drm_device *dev,
 		goto err;
 
 	need_relocs = (args->flags & I915_EXEC_NO_RELOC) == 0;
-	ret = i915_gem_execbuffer_reserve(ring, file, &eb->objects, &need_relocs);
+	ret = i915_gem_execbuffer_reserve(ring, &eb->objects, &need_relocs);
 	if (ret)
 		goto err;
 
@@ -736,7 +734,7 @@ validate_exec_list(struct drm_i915_gem_exec_object2 *exec,
 	int relocs_max = INT_MAX / sizeof(struct drm_i915_gem_relocation_entry);
 
 	for (i = 0; i < count; i++) {
-		char __user *ptr = (char __user *)(uintptr_t)exec[i].relocs_ptr;
+		char __user *ptr = to_user_ptr(exec[i].relocs_ptr);
 		int length; /* limited by fault_in_pages_readable() */
 
 		if (exec[i].flags & __EXEC_OBJECT_UNKNOWN_FLAGS)
@@ -752,7 +750,11 @@ validate_exec_list(struct drm_i915_gem_exec_object2 *exec,
 
 		length = exec[i].relocation_count *
 			sizeof(struct drm_i915_gem_relocation_entry);
-		/* we may also need to update the presumed offsets */
+		/*
+		 * We must check that the entire relocation array is safe
+		 * to read, but since we may need to update the presumed
+		 * offsets during execution, check for full write access.
+		 */
 		if (!access_ok(VERIFY_WRITE, ptr, length))
 			return -EFAULT;
 
@@ -949,9 +951,8 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 		}
 
 		if (copy_from_user(cliprects,
-				     (struct drm_clip_rect __user *)(uintptr_t)
-				     args->cliprects_ptr,
-				     sizeof(*cliprects)*args->num_cliprects)) {
+				   to_user_ptr(args->cliprects_ptr),
+				   sizeof(*cliprects)*args->num_cliprects)) {
 			ret = -EFAULT;
 			goto pre_mutex_err;
 		}
@@ -986,13 +987,13 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 
 	/* Move the objects en-masse into the GTT, evicting if necessary. */
 	need_relocs = (args->flags & I915_EXEC_NO_RELOC) == 0;
-	ret = i915_gem_execbuffer_reserve(ring, file, &eb->objects, &need_relocs);
+	ret = i915_gem_execbuffer_reserve(ring, &eb->objects, &need_relocs);
 	if (ret)
 		goto err;
 
 	/* The objects are in their final locations, apply the relocations. */
 	if (need_relocs)
-		ret = i915_gem_execbuffer_relocate(dev, eb);
+		ret = i915_gem_execbuffer_relocate(eb);
 	if (ret) {
 		if (ret == -EFAULT) {
 			ret = i915_gem_execbuffer_relocate_slow(dev, args, file, ring,
@@ -1115,7 +1116,7 @@ i915_gem_execbuffer(struct drm_device *dev, void *data,
 		return -ENOMEM;
 	}
 	ret = copy_from_user(exec_list,
-			     (void __user *)(uintptr_t)args->buffers_ptr,
+			     to_user_ptr(args->buffers_ptr),
 			     sizeof(*exec_list) * args->buffer_count);
 	if (ret != 0) {
 		DRM_DEBUG("copy %d exec entries failed %d\n",
@@ -1154,7 +1155,7 @@ i915_gem_execbuffer(struct drm_device *dev, void *data,
 		for (i = 0; i < args->buffer_count; i++)
 			exec_list[i].offset = exec2_list[i].offset;
 		/* ... and back out to userspace */
-		ret = copy_to_user((void __user *)(uintptr_t)args->buffers_ptr,
+		ret = copy_to_user(to_user_ptr(args->buffers_ptr),
 				   exec_list,
 				   sizeof(*exec_list) * args->buffer_count);
 		if (ret) {
@@ -1195,8 +1196,7 @@ i915_gem_execbuffer2(struct drm_device *dev, void *data,
 		return -ENOMEM;
 	}
 	ret = copy_from_user(exec2_list,
-			     (struct drm_i915_relocation_entry __user *)
-			     (uintptr_t) args->buffers_ptr,
+			     to_user_ptr(args->buffers_ptr),
 			     sizeof(*exec2_list) * args->buffer_count);
 	if (ret != 0) {
 		DRM_DEBUG("copy %d exec entries failed %d\n",
@@ -1208,7 +1208,7 @@ i915_gem_execbuffer2(struct drm_device *dev, void *data,
 	ret = i915_gem_do_execbuffer(dev, data, file, args, exec2_list);
 	if (!ret) {
 		/* Copy the new buffer offsets back to the user's exec list. */
-		ret = copy_to_user((void __user *)(uintptr_t)args->buffers_ptr,
+		ret = copy_to_user(to_user_ptr(args->buffers_ptr),
 				   exec2_list,
 				   sizeof(*exec2_list) * args->buffer_count);
 		if (ret) {
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.c b/drivers/gpu/drm/i915/i915_gem_gtt.c
index 926a1e2dd234..dca614de71b6 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.c
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.c
@@ -28,7 +28,7 @@
 #include "i915_trace.h"
 #include "intel_drv.h"
 
-typedef uint32_t gtt_pte_t;
+typedef uint32_t gen6_gtt_pte_t;
 
 /* PPGTT stuff */
 #define GEN6_GTT_ADDR_ENCODE(addr)	((addr) | (((addr) >> 28) & 0xff0))
@@ -44,11 +44,11 @@ typedef uint32_t gtt_pte_t;
 #define GEN6_PTE_CACHE_LLC_MLC		(3 << 1)
 #define GEN6_PTE_ADDR_ENCODE(addr)	GEN6_GTT_ADDR_ENCODE(addr)
 
-static inline gtt_pte_t gen6_pte_encode(struct drm_device *dev,
-					dma_addr_t addr,
-					enum i915_cache_level level)
+static inline gen6_gtt_pte_t gen6_pte_encode(struct drm_device *dev,
+					     dma_addr_t addr,
+					     enum i915_cache_level level)
 {
-	gtt_pte_t pte = GEN6_PTE_VALID;
+	gen6_gtt_pte_t pte = GEN6_PTE_VALID;
 	pte |= GEN6_PTE_ADDR_ENCODE(addr);
 
 	switch (level) {
@@ -72,18 +72,85 @@ static inline gtt_pte_t gen6_pte_encode(struct drm_device *dev,
 		BUG();
 	}
 
-
 	return pte;
 }
 
+static int gen6_ppgtt_enable(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	uint32_t pd_offset;
+	struct intel_ring_buffer *ring;
+	struct i915_hw_ppgtt *ppgtt = dev_priv->mm.aliasing_ppgtt;
+	gen6_gtt_pte_t __iomem *pd_addr;
+	uint32_t pd_entry;
+	int i;
+
+	pd_addr = (gen6_gtt_pte_t __iomem*)dev_priv->gtt.gsm +
+		ppgtt->pd_offset / sizeof(gen6_gtt_pte_t);
+	for (i = 0; i < ppgtt->num_pd_entries; i++) {
+		dma_addr_t pt_addr;
+
+		pt_addr = ppgtt->pt_dma_addr[i];
+		pd_entry = GEN6_PDE_ADDR_ENCODE(pt_addr);
+		pd_entry |= GEN6_PDE_VALID;
+
+		writel(pd_entry, pd_addr + i);
+	}
+	readl(pd_addr);
+
+	pd_offset = ppgtt->pd_offset;
+	pd_offset /= 64; /* in cachelines, */
+	pd_offset <<= 16;
+
+	if (INTEL_INFO(dev)->gen == 6) {
+		uint32_t ecochk, gab_ctl, ecobits;
+
+		ecobits = I915_READ(GAC_ECO_BITS);
+		I915_WRITE(GAC_ECO_BITS, ecobits | ECOBITS_SNB_BIT |
+					 ECOBITS_PPGTT_CACHE64B);
+
+		gab_ctl = I915_READ(GAB_CTL);
+		I915_WRITE(GAB_CTL, gab_ctl | GAB_CTL_CONT_AFTER_PAGEFAULT);
+
+		ecochk = I915_READ(GAM_ECOCHK);
+		I915_WRITE(GAM_ECOCHK, ecochk | ECOCHK_SNB_BIT |
+				       ECOCHK_PPGTT_CACHE64B);
+		I915_WRITE(GFX_MODE, _MASKED_BIT_ENABLE(GFX_PPGTT_ENABLE));
+	} else if (INTEL_INFO(dev)->gen >= 7) {
+		uint32_t ecochk, ecobits;
+
+		ecobits = I915_READ(GAC_ECO_BITS);
+		I915_WRITE(GAC_ECO_BITS, ecobits | ECOBITS_PPGTT_CACHE64B);
+
+		ecochk = I915_READ(GAM_ECOCHK);
+		if (IS_HASWELL(dev)) {
+			ecochk |= ECOCHK_PPGTT_WB_HSW;
+		} else {
+			ecochk |= ECOCHK_PPGTT_LLC_IVB;
+			ecochk &= ~ECOCHK_PPGTT_GFDT_IVB;
+		}
+		I915_WRITE(GAM_ECOCHK, ecochk);
+		/* GFX_MODE is per-ring on gen7+ */
+	}
+
+	for_each_ring(ring, dev_priv, i) {
+		if (INTEL_INFO(dev)->gen >= 7)
+			I915_WRITE(RING_MODE_GEN7(ring),
+				   _MASKED_BIT_ENABLE(GFX_PPGTT_ENABLE));
+
+		I915_WRITE(RING_PP_DIR_DCLV(ring), PP_DIR_DCLV_2G);
+		I915_WRITE(RING_PP_DIR_BASE(ring), pd_offset);
+	}
+	return 0;
+}
+
 /* PPGTT support for Sandybdrige/Gen6 and later */
 static void gen6_ppgtt_clear_range(struct i915_hw_ppgtt *ppgtt,
 				   unsigned first_entry,
 				   unsigned num_entries)
 {
-	gtt_pte_t *pt_vaddr;
-	gtt_pte_t scratch_pte;
-	unsigned act_pd = first_entry / I915_PPGTT_PT_ENTRIES;
+	gen6_gtt_pte_t *pt_vaddr, scratch_pte;
+	unsigned act_pt = first_entry / I915_PPGTT_PT_ENTRIES;
 	unsigned first_pte = first_entry % I915_PPGTT_PT_ENTRIES;
 	unsigned last_pte, i;
 
@@ -96,7 +163,7 @@ static void gen6_ppgtt_clear_range(struct i915_hw_ppgtt *ppgtt,
 		if (last_pte > I915_PPGTT_PT_ENTRIES)
 			last_pte = I915_PPGTT_PT_ENTRIES;
 
-		pt_vaddr = kmap_atomic(ppgtt->pt_pages[act_pd]);
+		pt_vaddr = kmap_atomic(ppgtt->pt_pages[act_pt]);
 
 		for (i = first_pte; i < last_pte; i++)
 			pt_vaddr[i] = scratch_pte;
@@ -105,7 +172,7 @@ static void gen6_ppgtt_clear_range(struct i915_hw_ppgtt *ppgtt,
 
 		num_entries -= last_pte - first_pte;
 		first_pte = 0;
-		act_pd++;
+		act_pt++;
 	}
 }
 
@@ -114,43 +181,27 @@ static void gen6_ppgtt_insert_entries(struct i915_hw_ppgtt *ppgtt,
 				      unsigned first_entry,
 				      enum i915_cache_level cache_level)
 {
-	gtt_pte_t *pt_vaddr;
-	unsigned act_pd = first_entry / I915_PPGTT_PT_ENTRIES;
-	unsigned first_pte = first_entry % I915_PPGTT_PT_ENTRIES;
-	unsigned i, j, m, segment_len;
-	dma_addr_t page_addr;
-	struct scatterlist *sg;
-
-	/* init sg walking */
-	sg = pages->sgl;
-	i = 0;
-	segment_len = sg_dma_len(sg) >> PAGE_SHIFT;
-	m = 0;
-
-	while (i < pages->nents) {
-		pt_vaddr = kmap_atomic(ppgtt->pt_pages[act_pd]);
-
-		for (j = first_pte; j < I915_PPGTT_PT_ENTRIES; j++) {
-			page_addr = sg_dma_address(sg) + (m << PAGE_SHIFT);
-			pt_vaddr[j] = gen6_pte_encode(ppgtt->dev, page_addr,
-						      cache_level);
-
-			/* grab the next page */
-			if (++m == segment_len) {
-				if (++i == pages->nents)
-					break;
-
-				sg = sg_next(sg);
-				segment_len = sg_dma_len(sg) >> PAGE_SHIFT;
-				m = 0;
-			}
-		}
+	gen6_gtt_pte_t *pt_vaddr;
+	unsigned act_pt = first_entry / I915_PPGTT_PT_ENTRIES;
+	unsigned act_pte = first_entry % I915_PPGTT_PT_ENTRIES;
+	struct sg_page_iter sg_iter;
 
-		kunmap_atomic(pt_vaddr);
+	pt_vaddr = kmap_atomic(ppgtt->pt_pages[act_pt]);
+	for_each_sg_page(pages->sgl, &sg_iter, pages->nents, 0) {
+		dma_addr_t page_addr;
 
-		first_pte = 0;
-		act_pd++;
+		page_addr = sg_page_iter_dma_address(&sg_iter);
+		pt_vaddr[act_pte] = gen6_pte_encode(ppgtt->dev, page_addr,
+						    cache_level);
+		if (++act_pte == I915_PPGTT_PT_ENTRIES) {
+			kunmap_atomic(pt_vaddr);
+			act_pt++;
+			pt_vaddr = kmap_atomic(ppgtt->pt_pages[act_pt]);
+			act_pte = 0;
+
+		}
 	}
+	kunmap_atomic(pt_vaddr);
 }
 
 static void gen6_ppgtt_cleanup(struct i915_hw_ppgtt *ppgtt)
@@ -182,10 +233,10 @@ static int gen6_ppgtt_init(struct i915_hw_ppgtt *ppgtt)
 	/* ppgtt PDEs reside in the global gtt pagetable, which has 512*1024
 	 * entries. For aliasing ppgtt support we just steal them at the end for
 	 * now. */
-	first_pd_entry_in_global_pt =
-		gtt_total_entries(dev_priv->gtt) - I915_PPGTT_PD_ENTRIES;
+       first_pd_entry_in_global_pt = gtt_total_entries(dev_priv->gtt);
 
 	ppgtt->num_pd_entries = I915_PPGTT_PD_ENTRIES;
+	ppgtt->enable = gen6_ppgtt_enable;
 	ppgtt->clear_range = gen6_ppgtt_clear_range;
 	ppgtt->insert_entries = gen6_ppgtt_insert_entries;
 	ppgtt->cleanup = gen6_ppgtt_cleanup;
@@ -219,12 +270,10 @@ static int gen6_ppgtt_init(struct i915_hw_ppgtt *ppgtt)
 		ppgtt->pt_dma_addr[i] = pt_addr;
 	}
 
-	ppgtt->scratch_page_dma_addr = dev_priv->gtt.scratch_page_dma;
-
 	ppgtt->clear_range(ppgtt, 0,
 			   ppgtt->num_pd_entries*I915_PPGTT_PT_ENTRIES);
 
-	ppgtt->pd_offset = (first_pd_entry_in_global_pt)*sizeof(gtt_pte_t);
+	ppgtt->pd_offset = first_pd_entry_in_global_pt * sizeof(gen6_gtt_pte_t);
 
 	return 0;
 
@@ -256,8 +305,13 @@ static int i915_gem_init_aliasing_ppgtt(struct drm_device *dev)
 		return -ENOMEM;
 
 	ppgtt->dev = dev;
+	ppgtt->scratch_page_dma_addr = dev_priv->gtt.scratch_page_dma;
+
+	if (INTEL_INFO(dev)->gen < 8)
+		ret = gen6_ppgtt_init(ppgtt);
+	else
+		BUG();
 
-	ret = gen6_ppgtt_init(ppgtt);
 	if (ret)
 		kfree(ppgtt);
 	else
@@ -275,6 +329,7 @@ void i915_gem_cleanup_aliasing_ppgtt(struct drm_device *dev)
 		return;
 
 	ppgtt->cleanup(ppgtt);
+	dev_priv->mm.aliasing_ppgtt = NULL;
 }
 
 void i915_ppgtt_bind_object(struct i915_hw_ppgtt *ppgtt,
@@ -294,64 +349,6 @@ void i915_ppgtt_unbind_object(struct i915_hw_ppgtt *ppgtt,
 			   obj->base.size >> PAGE_SHIFT);
 }
 
-void i915_gem_init_ppgtt(struct drm_device *dev)
-{
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	uint32_t pd_offset;
-	struct intel_ring_buffer *ring;
-	struct i915_hw_ppgtt *ppgtt = dev_priv->mm.aliasing_ppgtt;
-	gtt_pte_t __iomem *pd_addr;
-	uint32_t pd_entry;
-	int i;
-
-	if (!dev_priv->mm.aliasing_ppgtt)
-		return;
-
-
-	pd_addr = (gtt_pte_t __iomem*)dev_priv->gtt.gsm + ppgtt->pd_offset/sizeof(gtt_pte_t);
-	for (i = 0; i < ppgtt->num_pd_entries; i++) {
-		dma_addr_t pt_addr;
-
-		pt_addr = ppgtt->pt_dma_addr[i];
-		pd_entry = GEN6_PDE_ADDR_ENCODE(pt_addr);
-		pd_entry |= GEN6_PDE_VALID;
-
-		writel(pd_entry, pd_addr + i);
-	}
-	readl(pd_addr);
-
-	pd_offset = ppgtt->pd_offset;
-	pd_offset /= 64; /* in cachelines, */
-	pd_offset <<= 16;
-
-	if (INTEL_INFO(dev)->gen == 6) {
-		uint32_t ecochk, gab_ctl, ecobits;
-
-		ecobits = I915_READ(GAC_ECO_BITS);
-		I915_WRITE(GAC_ECO_BITS, ecobits | ECOBITS_PPGTT_CACHE64B);
-
-		gab_ctl = I915_READ(GAB_CTL);
-		I915_WRITE(GAB_CTL, gab_ctl | GAB_CTL_CONT_AFTER_PAGEFAULT);
-
-		ecochk = I915_READ(GAM_ECOCHK);
-		I915_WRITE(GAM_ECOCHK, ecochk | ECOCHK_SNB_BIT |
-				       ECOCHK_PPGTT_CACHE64B);
-		I915_WRITE(GFX_MODE, _MASKED_BIT_ENABLE(GFX_PPGTT_ENABLE));
-	} else if (INTEL_INFO(dev)->gen >= 7) {
-		I915_WRITE(GAM_ECOCHK, ECOCHK_PPGTT_CACHE64B);
-		/* GFX_MODE is per-ring on gen7+ */
-	}
-
-	for_each_ring(ring, dev_priv, i) {
-		if (INTEL_INFO(dev)->gen >= 7)
-			I915_WRITE(RING_MODE_GEN7(ring),
-				   _MASKED_BIT_ENABLE(GFX_PPGTT_ENABLE));
-
-		I915_WRITE(RING_PP_DIR_DCLV(ring), PP_DIR_DCLV_2G);
-		I915_WRITE(RING_PP_DIR_BASE(ring), pd_offset);
-	}
-}
-
 extern int intel_iommu_gfx_mapped;
 /* Certain Gen5 chipsets require require idling the GPU before
  * unmapping anything from the GTT when VT-d is enabled.
@@ -432,21 +429,16 @@ static void gen6_ggtt_insert_entries(struct drm_device *dev,
 				     enum i915_cache_level level)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct scatterlist *sg = st->sgl;
-	gtt_pte_t __iomem *gtt_entries =
-		(gtt_pte_t __iomem *)dev_priv->gtt.gsm + first_entry;
-	int unused, i = 0;
-	unsigned int len, m = 0;
+	gen6_gtt_pte_t __iomem *gtt_entries =
+		(gen6_gtt_pte_t __iomem *)dev_priv->gtt.gsm + first_entry;
+	int i = 0;
+	struct sg_page_iter sg_iter;
 	dma_addr_t addr;
 
-	for_each_sg(st->sgl, sg, st->nents, unused) {
-		len = sg_dma_len(sg) >> PAGE_SHIFT;
-		for (m = 0; m < len; m++) {
-			addr = sg_dma_address(sg) + (m << PAGE_SHIFT);
-			iowrite32(gen6_pte_encode(dev, addr, level),
-				  &gtt_entries[i]);
-			i++;
-		}
+	for_each_sg_page(st->sgl, &sg_iter, st->nents, 0) {
+		addr = sg_page_iter_dma_address(&sg_iter);
+		iowrite32(gen6_pte_encode(dev, addr, level), &gtt_entries[i]);
+		i++;
 	}
 
 	/* XXX: This serves as a posting read to make sure that the PTE has
@@ -472,8 +464,8 @@ static void gen6_ggtt_clear_range(struct drm_device *dev,
 				  unsigned int num_entries)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	gtt_pte_t scratch_pte;
-	gtt_pte_t __iomem *gtt_base = (gtt_pte_t __iomem *) dev_priv->gtt.gsm + first_entry;
+	gen6_gtt_pte_t scratch_pte, __iomem *gtt_base =
+		(gen6_gtt_pte_t __iomem *) dev_priv->gtt.gsm + first_entry;
 	const int max_entries = gtt_total_entries(dev_priv->gtt) - first_entry;
 	int i;
 
@@ -647,9 +639,12 @@ void i915_gem_init_global_gtt(struct drm_device *dev)
 
 	if (intel_enable_ppgtt(dev) && HAS_ALIASING_PPGTT(dev)) {
 		int ret;
-		/* PPGTT pdes are stolen from global gtt ptes, so shrink the
-		 * aperture accordingly when using aliasing ppgtt. */
-		gtt_size -= I915_PPGTT_PD_ENTRIES*PAGE_SIZE;
+
+		if (INTEL_INFO(dev)->gen <= 7) {
+			/* PPGTT pdes are stolen from global gtt ptes, so shrink the
+			 * aperture accordingly when using aliasing ppgtt. */
+			gtt_size -= I915_PPGTT_PD_ENTRIES*PAGE_SIZE;
+		}
 
 		i915_gem_setup_global_gtt(dev, 0, mappable_size, gtt_size);
 
@@ -752,15 +747,17 @@ static int gen6_gmch_probe(struct drm_device *dev,
 	pci_read_config_word(dev->pdev, SNB_GMCH_CTRL, &snb_gmch_ctl);
 	gtt_size = gen6_get_total_gtt_size(snb_gmch_ctl);
 
-	if (IS_GEN7(dev))
+	if (IS_GEN7(dev) && !IS_VALLEYVIEW(dev))
 		*stolen = gen7_get_stolen_size(snb_gmch_ctl);
 	else
 		*stolen = gen6_get_stolen_size(snb_gmch_ctl);
 
-	*gtt_total = (gtt_size / sizeof(gtt_pte_t)) << PAGE_SHIFT;
+	*gtt_total = (gtt_size / sizeof(gen6_gtt_pte_t)) << PAGE_SHIFT;
+
+	/* For Modern GENs the PTEs and register space are split in the BAR */
+	gtt_bus_addr = pci_resource_start(dev->pdev, 0) +
+		(pci_resource_len(dev->pdev, 0) / 2);
 
-	/* For GEN6+ the PTEs for the ggtt live at 2MB + BAR0 */
-	gtt_bus_addr = pci_resource_start(dev->pdev, 0) + (2<<20);
 	dev_priv->gtt.gsm = ioremap_wc(gtt_bus_addr, gtt_size);
 	if (!dev_priv->gtt.gsm) {
 		DRM_ERROR("Failed to map the gtt page table\n");
@@ -817,7 +814,6 @@ int i915_gem_gtt_init(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct i915_gtt *gtt = &dev_priv->gtt;
-	unsigned long gtt_size;
 	int ret;
 
 	if (INTEL_INFO(dev)->gen <= 5) {
@@ -835,8 +831,6 @@ int i915_gem_gtt_init(struct drm_device *dev)
 	if (ret)
 		return ret;
 
-	gtt_size = (dev_priv->gtt.total >> PAGE_SHIFT) * sizeof(gtt_pte_t);
-
 	/* GMADR is the PCI mmio aperture into the global GTT. */
 	DRM_INFO("Memory usable by graphics device = %zdM\n",
 		 dev_priv->gtt.total >> 20);
diff --git a/drivers/gpu/drm/i915/i915_gem_stolen.c b/drivers/gpu/drm/i915/i915_gem_stolen.c
index 69d97cbac13c..130d1db27e28 100644
--- a/drivers/gpu/drm/i915/i915_gem_stolen.c
+++ b/drivers/gpu/drm/i915/i915_gem_stolen.c
@@ -312,6 +312,71 @@ i915_gem_object_create_stolen(struct drm_device *dev, u32 size)
 	return NULL;
 }
 
+struct drm_i915_gem_object *
+i915_gem_object_create_stolen_for_preallocated(struct drm_device *dev,
+					       u32 stolen_offset,
+					       u32 gtt_offset,
+					       u32 size)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_gem_object *obj;
+	struct drm_mm_node *stolen;
+
+	if (dev_priv->mm.stolen_base == 0)
+		return NULL;
+
+	DRM_DEBUG_KMS("creating preallocated stolen object: stolen_offset=%x, gtt_offset=%x, size=%x\n",
+			stolen_offset, gtt_offset, size);
+
+	/* KISS and expect everything to be page-aligned */
+	BUG_ON(stolen_offset & 4095);
+	BUG_ON(gtt_offset & 4095);
+	BUG_ON(size & 4095);
+
+	if (WARN_ON(size == 0))
+		return NULL;
+
+	stolen = drm_mm_create_block(&dev_priv->mm.stolen,
+				     stolen_offset, size,
+				     false);
+	if (stolen == NULL) {
+		DRM_DEBUG_KMS("failed to allocate stolen space\n");
+		return NULL;
+	}
+
+	obj = _i915_gem_object_create_stolen(dev, stolen);
+	if (obj == NULL) {
+		DRM_DEBUG_KMS("failed to allocate stolen object\n");
+		drm_mm_put_block(stolen);
+		return NULL;
+	}
+
+	/* To simplify the initialisation sequence between KMS and GTT,
+	 * we allow construction of the stolen object prior to
+	 * setting up the GTT space. The actual reservation will occur
+	 * later.
+	 */
+	if (drm_mm_initialized(&dev_priv->mm.gtt_space)) {
+		obj->gtt_space = drm_mm_create_block(&dev_priv->mm.gtt_space,
+						     gtt_offset, size,
+						     false);
+		if (obj->gtt_space == NULL) {
+			DRM_DEBUG_KMS("failed to allocate stolen GTT space\n");
+			drm_gem_object_unreference(&obj->base);
+			return NULL;
+		}
+	} else
+		obj->gtt_space = I915_GTT_RESERVED;
+
+	obj->gtt_offset = gtt_offset;
+	obj->has_global_gtt_mapping = 1;
+
+	list_add_tail(&obj->gtt_list, &dev_priv->mm.bound_list);
+	list_add_tail(&obj->mm_list, &dev_priv->mm.inactive_list);
+
+	return obj;
+}
+
 void
 i915_gem_object_release_stolen(struct drm_i915_gem_object *obj)
 {
diff --git a/drivers/gpu/drm/i915/i915_gem_tiling.c b/drivers/gpu/drm/i915/i915_gem_tiling.c
index abcba2f5a788..537545be69db 100644
--- a/drivers/gpu/drm/i915/i915_gem_tiling.c
+++ b/drivers/gpu/drm/i915/i915_gem_tiling.c
@@ -217,9 +217,12 @@ i915_tiling_ok(struct drm_device *dev, int stride, int size, int tiling_mode)
 		tile_width = 512;
 
 	/* check maximum stride & object size */
-	if (INTEL_INFO(dev)->gen >= 4) {
-		/* i965 stores the end address of the gtt mapping in the fence
-		 * reg, so dont bother to check the size */
+	/* i965+ stores the end address of the gtt mapping in the fence
+	 * reg, so dont bother to check the size */
+	if (INTEL_INFO(dev)->gen >= 7) {
+		if (stride / 128 > GEN7_FENCE_MAX_PITCH_VAL)
+			return false;
+	} else if (INTEL_INFO(dev)->gen >= 4) {
 		if (stride / 128 > I965_FENCE_MAX_PITCH_VAL)
 			return false;
 	} else {
@@ -235,6 +238,9 @@ i915_tiling_ok(struct drm_device *dev, int stride, int size, int tiling_mode)
 		}
 	}
 
+	if (stride < tile_width)
+		return false;
+
 	/* 965+ just needs multiples of tile width */
 	if (INTEL_INFO(dev)->gen >= 4) {
 		if (stride & (tile_width - 1))
@@ -243,9 +249,6 @@ i915_tiling_ok(struct drm_device *dev, int stride, int size, int tiling_mode)
 	}
 
 	/* Pre-965 needs power of two tile widths */
-	if (stride < tile_width)
-		return false;
-
 	if (stride & (stride - 1))
 		return false;
 
@@ -473,28 +476,29 @@ i915_gem_swizzle_page(struct page *page)
 void
 i915_gem_object_do_bit_17_swizzle(struct drm_i915_gem_object *obj)
 {
-	struct scatterlist *sg;
-	int page_count = obj->base.size >> PAGE_SHIFT;
+	struct sg_page_iter sg_iter;
 	int i;
 
 	if (obj->bit_17 == NULL)
 		return;
 
-	for_each_sg(obj->pages->sgl, sg, page_count, i) {
-		struct page *page = sg_page(sg);
+	i = 0;
+	for_each_sg_page(obj->pages->sgl, &sg_iter, obj->pages->nents, 0) {
+		struct page *page = sg_page_iter_page(&sg_iter);
 		char new_bit_17 = page_to_phys(page) >> 17;
 		if ((new_bit_17 & 0x1) !=
 		    (test_bit(i, obj->bit_17) != 0)) {
 			i915_gem_swizzle_page(page);
 			set_page_dirty(page);
 		}
+		i++;
 	}
 }
 
 void
 i915_gem_object_save_bit_17_swizzle(struct drm_i915_gem_object *obj)
 {
-	struct scatterlist *sg;
+	struct sg_page_iter sg_iter;
 	int page_count = obj->base.size >> PAGE_SHIFT;
 	int i;
 
@@ -508,11 +512,12 @@ i915_gem_object_save_bit_17_swizzle(struct drm_i915_gem_object *obj)
 		}
 	}
 
-	for_each_sg(obj->pages->sgl, sg, page_count, i) {
-		struct page *page = sg_page(sg);
-		if (page_to_phys(page) & (1 << 17))
+	i = 0;
+	for_each_sg_page(obj->pages->sgl, &sg_iter, obj->pages->nents, 0) {
+		if (page_to_phys(sg_page_iter_page(&sg_iter)) & (1 << 17))
 			__set_bit(i, obj->bit_17);
 		else
 			__clear_bit(i, obj->bit_17);
+		i++;
 	}
 }
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index 3c7bb0410b51..0aa2ef0d2ae0 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -36,6 +36,61 @@
 #include "i915_trace.h"
 #include "intel_drv.h"
 
+static const u32 hpd_ibx[] = {
+	[HPD_CRT] = SDE_CRT_HOTPLUG,
+	[HPD_SDVO_B] = SDE_SDVOB_HOTPLUG,
+	[HPD_PORT_B] = SDE_PORTB_HOTPLUG,
+	[HPD_PORT_C] = SDE_PORTC_HOTPLUG,
+	[HPD_PORT_D] = SDE_PORTD_HOTPLUG
+};
+
+static const u32 hpd_cpt[] = {
+	[HPD_CRT] = SDE_CRT_HOTPLUG_CPT,
+	[HPD_SDVO_B] = SDE_SDVOB_HOTPLUG_CPT,
+	[HPD_PORT_B] = SDE_PORTB_HOTPLUG_CPT,
+	[HPD_PORT_C] = SDE_PORTC_HOTPLUG_CPT,
+	[HPD_PORT_D] = SDE_PORTD_HOTPLUG_CPT
+};
+
+static const u32 hpd_mask_i915[] = {
+	[HPD_CRT] = CRT_HOTPLUG_INT_EN,
+	[HPD_SDVO_B] = SDVOB_HOTPLUG_INT_EN,
+	[HPD_SDVO_C] = SDVOC_HOTPLUG_INT_EN,
+	[HPD_PORT_B] = PORTB_HOTPLUG_INT_EN,
+	[HPD_PORT_C] = PORTC_HOTPLUG_INT_EN,
+	[HPD_PORT_D] = PORTD_HOTPLUG_INT_EN
+};
+
+static const u32 hpd_status_gen4[] = {
+	[HPD_CRT] = CRT_HOTPLUG_INT_STATUS,
+	[HPD_SDVO_B] = SDVOB_HOTPLUG_INT_STATUS_G4X,
+	[HPD_SDVO_C] = SDVOC_HOTPLUG_INT_STATUS_G4X,
+	[HPD_PORT_B] = PORTB_HOTPLUG_INT_STATUS,
+	[HPD_PORT_C] = PORTC_HOTPLUG_INT_STATUS,
+	[HPD_PORT_D] = PORTD_HOTPLUG_INT_STATUS
+};
+
+static const u32 hpd_status_i965[] = {
+	 [HPD_CRT] = CRT_HOTPLUG_INT_STATUS,
+	 [HPD_SDVO_B] = SDVOB_HOTPLUG_INT_STATUS_I965,
+	 [HPD_SDVO_C] = SDVOC_HOTPLUG_INT_STATUS_I965,
+	 [HPD_PORT_B] = PORTB_HOTPLUG_INT_STATUS,
+	 [HPD_PORT_C] = PORTC_HOTPLUG_INT_STATUS,
+	 [HPD_PORT_D] = PORTD_HOTPLUG_INT_STATUS
+};
+
+static const u32 hpd_status_i915[] = { /* i915 and valleyview are the same */
+	[HPD_CRT] = CRT_HOTPLUG_INT_STATUS,
+	[HPD_SDVO_B] = SDVOB_HOTPLUG_INT_STATUS_I915,
+	[HPD_SDVO_C] = SDVOC_HOTPLUG_INT_STATUS_I915,
+	[HPD_PORT_B] = PORTB_HOTPLUG_INT_STATUS,
+	[HPD_PORT_C] = PORTC_HOTPLUG_INT_STATUS,
+	[HPD_PORT_D] = PORTD_HOTPLUG_INT_STATUS
+};
+
+static void ibx_hpd_irq_setup(struct drm_device *dev);
+static void i915_hpd_irq_setup(struct drm_device *dev);
+
 /* For display hotplug interrupt */
 static void
 ironlake_enable_display_irq(drm_i915_private_t *dev_priv, u32 mask)
@@ -47,7 +102,7 @@ ironlake_enable_display_irq(drm_i915_private_t *dev_priv, u32 mask)
 	}
 }
 
-static inline void
+static void
 ironlake_disable_display_irq(drm_i915_private_t *dev_priv, u32 mask)
 {
 	if ((dev_priv->irq_mask & mask) != mask) {
@@ -60,26 +115,30 @@ ironlake_disable_display_irq(drm_i915_private_t *dev_priv, u32 mask)
 void
 i915_enable_pipestat(drm_i915_private_t *dev_priv, int pipe, u32 mask)
 {
-	if ((dev_priv->pipestat[pipe] & mask) != mask) {
-		u32 reg = PIPESTAT(pipe);
+	u32 reg = PIPESTAT(pipe);
+	u32 pipestat = I915_READ(reg) & 0x7fff0000;
 
-		dev_priv->pipestat[pipe] |= mask;
-		/* Enable the interrupt, clear any pending status */
-		I915_WRITE(reg, dev_priv->pipestat[pipe] | (mask >> 16));
-		POSTING_READ(reg);
-	}
+	if ((pipestat & mask) == mask)
+		return;
+
+	/* Enable the interrupt, clear any pending status */
+	pipestat |= mask | (mask >> 16);
+	I915_WRITE(reg, pipestat);
+	POSTING_READ(reg);
 }
 
 void
 i915_disable_pipestat(drm_i915_private_t *dev_priv, int pipe, u32 mask)
 {
-	if ((dev_priv->pipestat[pipe] & mask) != 0) {
-		u32 reg = PIPESTAT(pipe);
+	u32 reg = PIPESTAT(pipe);
+	u32 pipestat = I915_READ(reg) & 0x7fff0000;
 
-		dev_priv->pipestat[pipe] &= ~mask;
-		I915_WRITE(reg, dev_priv->pipestat[pipe]);
-		POSTING_READ(reg);
-	}
+	if ((pipestat & mask) == 0)
+		return;
+
+	pipestat &= ~mask;
+	I915_WRITE(reg, pipestat);
+	POSTING_READ(reg);
 }
 
 /**
@@ -250,10 +309,9 @@ static int i915_get_vblank_timestamp(struct drm_device *dev, int pipe,
 			      struct timeval *vblank_time,
 			      unsigned flags)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_crtc *crtc;
 
-	if (pipe < 0 || pipe >= dev_priv->num_pipe) {
+	if (pipe < 0 || pipe >= INTEL_INFO(dev)->num_pipes) {
 		DRM_ERROR("Invalid crtc %d\n", pipe);
 		return -EINVAL;
 	}
@@ -279,13 +337,19 @@ static int i915_get_vblank_timestamp(struct drm_device *dev, int pipe,
 /*
  * Handle hotplug events outside the interrupt handler proper.
  */
+#define I915_REENABLE_HOTPLUG_DELAY (2*60*1000)
+
 static void i915_hotplug_work_func(struct work_struct *work)
 {
 	drm_i915_private_t *dev_priv = container_of(work, drm_i915_private_t,
 						    hotplug_work);
 	struct drm_device *dev = dev_priv->dev;
 	struct drm_mode_config *mode_config = &dev->mode_config;
-	struct intel_encoder *encoder;
+	struct intel_connector *intel_connector;
+	struct intel_encoder *intel_encoder;
+	struct drm_connector *connector;
+	unsigned long irqflags;
+	bool hpd_disabled = false;
 
 	/* HPD irq before everything is fully set up. */
 	if (!dev_priv->enable_hotplug_processing)
@@ -294,9 +358,36 @@ static void i915_hotplug_work_func(struct work_struct *work)
 	mutex_lock(&mode_config->mutex);
 	DRM_DEBUG_KMS("running encoder hotplug functions\n");
 
-	list_for_each_entry(encoder, &mode_config->encoder_list, base.head)
-		if (encoder->hot_plug)
-			encoder->hot_plug(encoder);
+	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	list_for_each_entry(connector, &mode_config->connector_list, head) {
+		intel_connector = to_intel_connector(connector);
+		intel_encoder = intel_connector->encoder;
+		if (intel_encoder->hpd_pin > HPD_NONE &&
+		    dev_priv->hpd_stats[intel_encoder->hpd_pin].hpd_mark == HPD_MARK_DISABLED &&
+		    connector->polled == DRM_CONNECTOR_POLL_HPD) {
+			DRM_INFO("HPD interrupt storm detected on connector %s: "
+				 "switching from hotplug detection to polling\n",
+				drm_get_connector_name(connector));
+			dev_priv->hpd_stats[intel_encoder->hpd_pin].hpd_mark = HPD_DISABLED;
+			connector->polled = DRM_CONNECTOR_POLL_CONNECT
+				| DRM_CONNECTOR_POLL_DISCONNECT;
+			hpd_disabled = true;
+		}
+	}
+	 /* if there were no outputs to poll, poll was disabled,
+	  * therefore make sure it's enabled when disabling HPD on
+	  * some connectors */
+	if (hpd_disabled) {
+		drm_kms_helper_poll_enable(dev);
+		mod_timer(&dev_priv->hotplug_reenable_timer,
+			  jiffies + msecs_to_jiffies(I915_REENABLE_HOTPLUG_DELAY));
+	}
+
+	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+
+	list_for_each_entry(intel_encoder, &mode_config->encoder_list, base.head)
+		if (intel_encoder->hot_plug)
+			intel_encoder->hot_plug(intel_encoder);
 
 	mutex_unlock(&mode_config->mutex);
 
@@ -525,6 +616,45 @@ static void gen6_queue_rps_work(struct drm_i915_private *dev_priv,
 	queue_work(dev_priv->wq, &dev_priv->rps.work);
 }
 
+#define HPD_STORM_DETECT_PERIOD 1000
+#define HPD_STORM_THRESHOLD 5
+
+static inline bool hotplug_irq_storm_detect(struct drm_device *dev,
+					    u32 hotplug_trigger,
+					    const u32 *hpd)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	unsigned long irqflags;
+	int i;
+	bool ret = false;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+
+	for (i = 1; i < HPD_NUM_PINS; i++) {
+
+		if (!(hpd[i] & hotplug_trigger) ||
+		    dev_priv->hpd_stats[i].hpd_mark != HPD_ENABLED)
+			continue;
+
+		if (!time_in_range(jiffies, dev_priv->hpd_stats[i].hpd_last_jiffies,
+				   dev_priv->hpd_stats[i].hpd_last_jiffies
+				   + msecs_to_jiffies(HPD_STORM_DETECT_PERIOD))) {
+			dev_priv->hpd_stats[i].hpd_last_jiffies = jiffies;
+			dev_priv->hpd_stats[i].hpd_cnt = 0;
+		} else if (dev_priv->hpd_stats[i].hpd_cnt > HPD_STORM_THRESHOLD) {
+			dev_priv->hpd_stats[i].hpd_mark = HPD_MARK_DISABLED;
+			DRM_DEBUG_KMS("HPD interrupt storm detected on PIN %d\n", i);
+			ret = true;
+		} else {
+			dev_priv->hpd_stats[i].hpd_cnt++;
+		}
+	}
+
+	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+
+	return ret;
+}
+
 static void gmbus_irq_handler(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = (drm_i915_private_t *) dev->dev_private;
@@ -593,13 +723,16 @@ static irqreturn_t valleyview_irq_handler(int irq, void *arg)
 		/* Consume port.  Then clear IIR or we'll miss events */
 		if (iir & I915_DISPLAY_PORT_INTERRUPT) {
 			u32 hotplug_status = I915_READ(PORT_HOTPLUG_STAT);
+			u32 hotplug_trigger = hotplug_status & HOTPLUG_INT_STATUS_I915;
 
 			DRM_DEBUG_DRIVER("hotplug event received, stat 0x%08x\n",
 					 hotplug_status);
-			if (hotplug_status & dev_priv->hotplug_supported_mask)
+			if (hotplug_trigger) {
+				if (hotplug_irq_storm_detect(dev, hotplug_trigger, hpd_status_i915))
+					i915_hpd_irq_setup(dev);
 				queue_work(dev_priv->wq,
 					   &dev_priv->hotplug_work);
-
+			}
 			I915_WRITE(PORT_HOTPLUG_STAT, hotplug_status);
 			I915_READ(PORT_HOTPLUG_STAT);
 		}
@@ -623,10 +756,13 @@ static void ibx_irq_handler(struct drm_device *dev, u32 pch_iir)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
 	int pipe;
+	u32 hotplug_trigger = pch_iir & SDE_HOTPLUG_MASK;
 
-	if (pch_iir & SDE_HOTPLUG_MASK)
+	if (hotplug_trigger) {
+		if (hotplug_irq_storm_detect(dev, hotplug_trigger, hpd_ibx))
+			ibx_hpd_irq_setup(dev);
 		queue_work(dev_priv->wq, &dev_priv->hotplug_work);
-
+	}
 	if (pch_iir & SDE_AUDIO_POWER_MASK)
 		DRM_DEBUG_DRIVER("PCH audio power change on port %d\n",
 				 (pch_iir & SDE_AUDIO_POWER_MASK) >>
@@ -669,10 +805,13 @@ static void cpt_irq_handler(struct drm_device *dev, u32 pch_iir)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
 	int pipe;
+	u32 hotplug_trigger = pch_iir & SDE_HOTPLUG_MASK_CPT;
 
-	if (pch_iir & SDE_HOTPLUG_MASK_CPT)
+	if (hotplug_trigger) {
+		if (hotplug_irq_storm_detect(dev, hotplug_trigger, hpd_cpt))
+			ibx_hpd_irq_setup(dev);
 		queue_work(dev_priv->wq, &dev_priv->hotplug_work);
-
+	}
 	if (pch_iir & SDE_AUDIO_POWER_MASK_CPT)
 		DRM_DEBUG_DRIVER("PCH audio power change on port %d\n",
 				 (pch_iir & SDE_AUDIO_POWER_MASK_CPT) >>
@@ -701,7 +840,7 @@ static irqreturn_t ivybridge_irq_handler(int irq, void *arg)
 {
 	struct drm_device *dev = (struct drm_device *) arg;
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-	u32 de_iir, gt_iir, de_ier, pm_iir, sde_ier;
+	u32 de_iir, gt_iir, de_ier, pm_iir, sde_ier = 0;
 	irqreturn_t ret = IRQ_NONE;
 	int i;
 
@@ -716,9 +855,11 @@ static irqreturn_t ivybridge_irq_handler(int irq, void *arg)
 	 * able to process them after we restore SDEIER (as soon as we restore
 	 * it, we'll get an interrupt if SDEIIR still has something to process
 	 * due to its back queue). */
-	sde_ier = I915_READ(SDEIER);
-	I915_WRITE(SDEIER, 0);
-	POSTING_READ(SDEIER);
+	if (!HAS_PCH_NOP(dev)) {
+		sde_ier = I915_READ(SDEIER);
+		I915_WRITE(SDEIER, 0);
+		POSTING_READ(SDEIER);
+	}
 
 	gt_iir = I915_READ(GTIIR);
 	if (gt_iir) {
@@ -745,7 +886,7 @@ static irqreturn_t ivybridge_irq_handler(int irq, void *arg)
 		}
 
 		/* check event from PCH */
-		if (de_iir & DE_PCH_EVENT_IVB) {
+		if (!HAS_PCH_NOP(dev) && (de_iir & DE_PCH_EVENT_IVB)) {
 			u32 pch_iir = I915_READ(SDEIIR);
 
 			cpt_irq_handler(dev, pch_iir);
@@ -768,8 +909,10 @@ static irqreturn_t ivybridge_irq_handler(int irq, void *arg)
 
 	I915_WRITE(DEIER, de_ier);
 	POSTING_READ(DEIER);
-	I915_WRITE(SDEIER, sde_ier);
-	POSTING_READ(SDEIER);
+	if (!HAS_PCH_NOP(dev)) {
+		I915_WRITE(SDEIER, sde_ier);
+		POSTING_READ(SDEIER);
+	}
 
 	return ret;
 }
@@ -937,6 +1080,8 @@ static void i915_error_work_func(struct work_struct *work)
 		for_each_ring(ring, dev_priv, i)
 			wake_up_all(&ring->irq_queue);
 
+		intel_display_handle_reset(dev);
+
 		wake_up_all(&dev_priv->gpu_error.reset_queue);
 	}
 }
@@ -972,24 +1117,23 @@ static void i915_get_extra_instdone(struct drm_device *dev,
 
 #ifdef CONFIG_DEBUG_FS
 static struct drm_i915_error_object *
-i915_error_object_create(struct drm_i915_private *dev_priv,
-			 struct drm_i915_gem_object *src)
+i915_error_object_create_sized(struct drm_i915_private *dev_priv,
+			       struct drm_i915_gem_object *src,
+			       const int num_pages)
 {
 	struct drm_i915_error_object *dst;
-	int i, count;
+	int i;
 	u32 reloc_offset;
 
 	if (src == NULL || src->pages == NULL)
 		return NULL;
 
-	count = src->base.size / PAGE_SIZE;
-
-	dst = kmalloc(sizeof(*dst) + count * sizeof(u32 *), GFP_ATOMIC);
+	dst = kmalloc(sizeof(*dst) + num_pages * sizeof(u32 *), GFP_ATOMIC);
 	if (dst == NULL)
 		return NULL;
 
 	reloc_offset = src->gtt_offset;
-	for (i = 0; i < count; i++) {
+	for (i = 0; i < num_pages; i++) {
 		unsigned long flags;
 		void *d;
 
@@ -1039,7 +1183,7 @@ i915_error_object_create(struct drm_i915_private *dev_priv,
 
 		reloc_offset += PAGE_SIZE;
 	}
-	dst->page_count = count;
+	dst->page_count = num_pages;
 	dst->gtt_offset = src->gtt_offset;
 
 	return dst;
@@ -1050,6 +1194,9 @@ unwind:
 	kfree(dst);
 	return NULL;
 }
+#define i915_error_object_create(dev_priv, src) \
+	i915_error_object_create_sized((dev_priv), (src), \
+				       (src)->base.size>>PAGE_SHIFT)
 
 static void
 i915_error_object_free(struct drm_i915_error_object *obj)
@@ -1148,7 +1295,7 @@ static void i915_gem_record_fences(struct drm_device *dev,
 	switch (INTEL_INFO(dev)->gen) {
 	case 7:
 	case 6:
-		for (i = 0; i < 16; i++)
+		for (i = 0; i < dev_priv->num_fence_regs; i++)
 			error->fence[i] = I915_READ64(FENCE_REG_SANDYBRIDGE_0 + (i * 8));
 		break;
 	case 5:
@@ -1256,6 +1403,26 @@ static void i915_record_ring_state(struct drm_device *dev,
 	error->cpu_ring_tail[ring->id] = ring->tail;
 }
 
+
+static void i915_gem_record_active_context(struct intel_ring_buffer *ring,
+					   struct drm_i915_error_state *error,
+					   struct drm_i915_error_ring *ering)
+{
+	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+	struct drm_i915_gem_object *obj;
+
+	/* Currently render ring is the only HW context user */
+	if (ring->id != RCS || !error->ccid)
+		return;
+
+	list_for_each_entry(obj, &dev_priv->mm.bound_list, gtt_list) {
+		if ((error->ccid & PAGE_MASK) == obj->gtt_offset) {
+			ering->ctx = i915_error_object_create_sized(dev_priv,
+								    obj, 1);
+		}
+	}
+}
+
 static void i915_gem_record_rings(struct drm_device *dev,
 				  struct drm_i915_error_state *error)
 {
@@ -1273,6 +1440,9 @@ static void i915_gem_record_rings(struct drm_device *dev,
 		error->ring[i].ringbuffer =
 			i915_error_object_create(dev_priv, ring->obj);
 
+
+		i915_gem_record_active_context(ring, error, &error->ring[i]);
+
 		count = 0;
 		list_for_each_entry(request, &ring->request_list, list)
 			count++;
@@ -1328,14 +1498,15 @@ static void i915_capture_error_state(struct drm_device *dev)
 		return;
 	}
 
-	DRM_INFO("capturing error event; look for more information in"
+	DRM_INFO("capturing error event; look for more information in "
 		 "/sys/kernel/debug/dri/%d/i915_error_state\n",
 		 dev->primary->index);
 
 	kref_init(&error->ref);
 	error->eir = I915_READ(EIR);
 	error->pgtbl_er = I915_READ(PGTBL_ER);
-	error->ccid = I915_READ(CCID);
+	if (HAS_HW_CONTEXTS(dev))
+		error->ccid = I915_READ(CCID);
 
 	if (HAS_PCH_SPLIT(dev))
 		error->ier = I915_READ(DEIER) | I915_READ(GTIER);
@@ -1356,8 +1527,9 @@ static void i915_capture_error_state(struct drm_device *dev)
 	else if (INTEL_INFO(dev)->gen == 6)
 		error->forcewake = I915_READ(FORCEWAKE);
 
-	for_each_pipe(pipe)
-		error->pipestat[pipe] = I915_READ(PIPESTAT(pipe));
+	if (!HAS_PCH_SPLIT(dev))
+		for_each_pipe(pipe)
+			error->pipestat[pipe] = I915_READ(PIPESTAT(pipe));
 
 	if (INTEL_INFO(dev)->gen >= 6) {
 		error->error = I915_READ(ERROR_GEN6);
@@ -1567,7 +1739,7 @@ void i915_handle_error(struct drm_device *dev, bool wedged)
 	queue_work(dev_priv->wq, &dev_priv->gpu_error.work);
 }
 
-static void i915_pageflip_stall_check(struct drm_device *dev, int pipe)
+static void __always_unused i915_pageflip_stall_check(struct drm_device *dev, int pipe)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pipe];
@@ -1777,6 +1949,37 @@ static bool i915_hangcheck_ring_idle(struct intel_ring_buffer *ring, bool *err)
 	return false;
 }
 
+static bool semaphore_passed(struct intel_ring_buffer *ring)
+{
+	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+	u32 acthd = intel_ring_get_active_head(ring) & HEAD_ADDR;
+	struct intel_ring_buffer *signaller;
+	u32 cmd, ipehr, acthd_min;
+
+	ipehr = I915_READ(RING_IPEHR(ring->mmio_base));
+	if ((ipehr & ~(0x3 << 16)) !=
+	    (MI_SEMAPHORE_MBOX | MI_SEMAPHORE_COMPARE | MI_SEMAPHORE_REGISTER))
+		return false;
+
+	/* ACTHD is likely pointing to the dword after the actual command,
+	 * so scan backwards until we find the MBOX.
+	 */
+	acthd_min = max((int)acthd - 3 * 4, 0);
+	do {
+		cmd = ioread32(ring->virtual_start + acthd);
+		if (cmd == ipehr)
+			break;
+
+		acthd -= 4;
+		if (acthd < acthd_min)
+			return false;
+	} while (1);
+
+	signaller = &dev_priv->ring[(ring->id + (((ipehr >> 17) & 1) + 1)) % 3];
+	return i915_seqno_passed(signaller->get_seqno(signaller, false),
+				 ioread32(ring->virtual_start+acthd+4)+1);
+}
+
 static bool kick_ring(struct intel_ring_buffer *ring)
 {
 	struct drm_device *dev = ring->dev;
@@ -1788,6 +1991,15 @@ static bool kick_ring(struct intel_ring_buffer *ring)
 		I915_WRITE_CTL(ring, tmp);
 		return true;
 	}
+
+	if (INTEL_INFO(dev)->gen >= 6 &&
+	    tmp & RING_WAIT_SEMAPHORE &&
+	    semaphore_passed(ring)) {
+		DRM_ERROR("Kicking stuck semaphore on %s\n",
+			  ring->name);
+		I915_WRITE_CTL(ring, tmp);
+		return true;
+	}
 	return false;
 }
 
@@ -1901,9 +2113,18 @@ static void ironlake_irq_preinstall(struct drm_device *dev)
 	I915_WRITE(GTIER, 0x0);
 	POSTING_READ(GTIER);
 
+	if (HAS_PCH_NOP(dev))
+		return;
+
 	/* south display irq */
 	I915_WRITE(SDEIMR, 0xffffffff);
-	I915_WRITE(SDEIER, 0x0);
+	/*
+	 * SDEIER is also touched by the interrupt handler to work around missed
+	 * PCH interrupts. Hence we can't update it after the interrupt handler
+	 * is enabled - instead we unconditionally enable all PCH interrupt
+	 * sources here, but then only unmask them as needed with SDEIMR.
+	 */
+	I915_WRITE(SDEIER, 0xffffffff);
 	POSTING_READ(SDEIER);
 }
 
@@ -1939,18 +2160,34 @@ static void valleyview_irq_preinstall(struct drm_device *dev)
 	POSTING_READ(VLV_IER);
 }
 
-/*
- * Enable digital hotplug on the PCH, and configure the DP short pulse
- * duration to 2ms (which is the minimum in the Display Port spec)
- *
- * This register is the same on all known PCH chips.
- */
-
-static void ibx_enable_hotplug(struct drm_device *dev)
+static void ibx_hpd_irq_setup(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-	u32	hotplug;
+	struct drm_mode_config *mode_config = &dev->mode_config;
+	struct intel_encoder *intel_encoder;
+	u32 mask = ~I915_READ(SDEIMR);
+	u32 hotplug;
+
+	if (HAS_PCH_IBX(dev)) {
+		mask &= ~SDE_HOTPLUG_MASK;
+		list_for_each_entry(intel_encoder, &mode_config->encoder_list, base.head)
+			if (dev_priv->hpd_stats[intel_encoder->hpd_pin].hpd_mark == HPD_ENABLED)
+				mask |= hpd_ibx[intel_encoder->hpd_pin];
+	} else {
+		mask &= ~SDE_HOTPLUG_MASK_CPT;
+		list_for_each_entry(intel_encoder, &mode_config->encoder_list, base.head)
+			if (dev_priv->hpd_stats[intel_encoder->hpd_pin].hpd_mark == HPD_ENABLED)
+				mask |= hpd_cpt[intel_encoder->hpd_pin];
+	}
 
+	I915_WRITE(SDEIMR, ~mask);
+
+	/*
+	 * Enable digital hotplug on the PCH, and configure the DP short pulse
+	 * duration to 2ms (which is the minimum in the Display Port spec)
+	 *
+	 * This register is the same on all known PCH chips.
+	 */
 	hotplug = I915_READ(PCH_PORT_HOTPLUG);
 	hotplug &= ~(PORTD_PULSE_DURATION_MASK|PORTC_PULSE_DURATION_MASK|PORTB_PULSE_DURATION_MASK);
 	hotplug |= PORTD_HOTPLUG_ENABLE | PORTD_PULSE_DURATION_2ms;
@@ -1965,20 +2202,15 @@ static void ibx_irq_postinstall(struct drm_device *dev)
 	u32 mask;
 
 	if (HAS_PCH_IBX(dev))
-		mask = SDE_HOTPLUG_MASK |
-		       SDE_GMBUS |
-		       SDE_AUX_MASK;
+		mask = SDE_GMBUS | SDE_AUX_MASK;
 	else
-		mask = SDE_HOTPLUG_MASK_CPT |
-		       SDE_GMBUS_CPT |
-		       SDE_AUX_MASK_CPT;
+		mask = SDE_GMBUS_CPT | SDE_AUX_MASK_CPT;
+
+	if (HAS_PCH_NOP(dev))
+		return;
 
 	I915_WRITE(SDEIIR, I915_READ(SDEIIR));
 	I915_WRITE(SDEIMR, ~mask);
-	I915_WRITE(SDEIER, mask);
-	POSTING_READ(SDEIER);
-
-	ibx_enable_hotplug(dev);
 }
 
 static int ironlake_irq_postinstall(struct drm_device *dev)
@@ -2089,9 +2321,6 @@ static int valleyview_irq_postinstall(struct drm_device *dev)
 		I915_DISPLAY_PIPE_A_VBLANK_INTERRUPT |
 		I915_DISPLAY_PIPE_B_VBLANK_INTERRUPT;
 
-	dev_priv->pipestat[0] = 0;
-	dev_priv->pipestat[1] = 0;
-
 	/* Hack for broken MSIs on VLV */
 	pci_write_config_dword(dev_priv->dev->pdev, 0x94, 0xfee00000);
 	pci_read_config_word(dev->pdev, 0x98, &msid);
@@ -2135,30 +2364,6 @@ static int valleyview_irq_postinstall(struct drm_device *dev)
 	return 0;
 }
 
-static void valleyview_hpd_irq_setup(struct drm_device *dev)
-{
-	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-	u32 hotplug_en = I915_READ(PORT_HOTPLUG_EN);
-
-	/* Note HDMI and DP share bits */
-	if (dev_priv->hotplug_supported_mask & PORTB_HOTPLUG_INT_STATUS)
-		hotplug_en |= PORTB_HOTPLUG_INT_EN;
-	if (dev_priv->hotplug_supported_mask & PORTC_HOTPLUG_INT_STATUS)
-		hotplug_en |= PORTC_HOTPLUG_INT_EN;
-	if (dev_priv->hotplug_supported_mask & PORTD_HOTPLUG_INT_STATUS)
-		hotplug_en |= PORTD_HOTPLUG_INT_EN;
-	if (dev_priv->hotplug_supported_mask & SDVOC_HOTPLUG_INT_STATUS_I915)
-		hotplug_en |= SDVOC_HOTPLUG_INT_EN;
-	if (dev_priv->hotplug_supported_mask & SDVOB_HOTPLUG_INT_STATUS_I915)
-		hotplug_en |= SDVOB_HOTPLUG_INT_EN;
-	if (dev_priv->hotplug_supported_mask & CRT_HOTPLUG_INT_STATUS) {
-		hotplug_en |= CRT_HOTPLUG_INT_EN;
-		hotplug_en |= CRT_HOTPLUG_VOLTAGE_COMPARE_50;
-	}
-
-	I915_WRITE(PORT_HOTPLUG_EN, hotplug_en);
-}
-
 static void valleyview_irq_uninstall(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
@@ -2167,6 +2372,8 @@ static void valleyview_irq_uninstall(struct drm_device *dev)
 	if (!dev_priv)
 		return;
 
+	del_timer_sync(&dev_priv->hotplug_reenable_timer);
+
 	for_each_pipe(pipe)
 		I915_WRITE(PIPESTAT(pipe), 0xffff);
 
@@ -2188,6 +2395,8 @@ static void ironlake_irq_uninstall(struct drm_device *dev)
 	if (!dev_priv)
 		return;
 
+	del_timer_sync(&dev_priv->hotplug_reenable_timer);
+
 	I915_WRITE(HWSTAM, 0xffffffff);
 
 	I915_WRITE(DEIMR, 0xffffffff);
@@ -2198,6 +2407,9 @@ static void ironlake_irq_uninstall(struct drm_device *dev)
 	I915_WRITE(GTIER, 0x0);
 	I915_WRITE(GTIIR, I915_READ(GTIIR));
 
+	if (HAS_PCH_NOP(dev))
+		return;
+
 	I915_WRITE(SDEIMR, 0xffffffff);
 	I915_WRITE(SDEIER, 0x0);
 	I915_WRITE(SDEIIR, I915_READ(SDEIIR));
@@ -2221,9 +2433,6 @@ static int i8xx_irq_postinstall(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
 
-	dev_priv->pipestat[0] = 0;
-	dev_priv->pipestat[1] = 0;
-
 	I915_WRITE16(EMR,
 		     ~(I915_ERROR_PAGE_TABLE | I915_ERROR_MEMORY_REFRESH));
 
@@ -2246,6 +2455,37 @@ static int i8xx_irq_postinstall(struct drm_device *dev)
 	return 0;
 }
 
+/*
+ * Returns true when a page flip has completed.
+ */
+static bool i8xx_handle_vblank(struct drm_device *dev,
+			       int pipe, u16 iir)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	u16 flip_pending = DISPLAY_PLANE_FLIP_PENDING(pipe);
+
+	if (!drm_handle_vblank(dev, pipe))
+		return false;
+
+	if ((iir & flip_pending) == 0)
+		return false;
+
+	intel_prepare_page_flip(dev, pipe);
+
+	/* We detect FlipDone by looking for the change in PendingFlip from '1'
+	 * to '0' on the following vblank, i.e. IIR has the Pendingflip
+	 * asserted following the MI_DISPLAY_FLIP, but ISR is deasserted, hence
+	 * the flip is completed (no longer pending). Since this doesn't raise
+	 * an interrupt per se, we watch for the change at vblank.
+	 */
+	if (I915_READ16(ISR) & flip_pending)
+		return false;
+
+	intel_finish_page_flip(dev, pipe);
+
+	return true;
+}
+
 static irqreturn_t i8xx_irq_handler(int irq, void *arg)
 {
 	struct drm_device *dev = (struct drm_device *) arg;
@@ -2301,22 +2541,12 @@ static irqreturn_t i8xx_irq_handler(int irq, void *arg)
 			notify_ring(dev, &dev_priv->ring[RCS]);
 
 		if (pipe_stats[0] & PIPE_VBLANK_INTERRUPT_STATUS &&
-		    drm_handle_vblank(dev, 0)) {
-			if (iir & I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT) {
-				intel_prepare_page_flip(dev, 0);
-				intel_finish_page_flip(dev, 0);
-				flip_mask &= ~I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT;
-			}
-		}
+		    i8xx_handle_vblank(dev, 0, iir))
+			flip_mask &= ~DISPLAY_PLANE_FLIP_PENDING(0);
 
 		if (pipe_stats[1] & PIPE_VBLANK_INTERRUPT_STATUS &&
-		    drm_handle_vblank(dev, 1)) {
-			if (iir & I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT) {
-				intel_prepare_page_flip(dev, 1);
-				intel_finish_page_flip(dev, 1);
-				flip_mask &= ~I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT;
-			}
-		}
+		    i8xx_handle_vblank(dev, 1, iir))
+			flip_mask &= ~DISPLAY_PLANE_FLIP_PENDING(1);
 
 		iir = new_iir;
 	}
@@ -2364,9 +2594,6 @@ static int i915_irq_postinstall(struct drm_device *dev)
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
 	u32 enable_mask;
 
-	dev_priv->pipestat[0] = 0;
-	dev_priv->pipestat[1] = 0;
-
 	I915_WRITE(EMR, ~(I915_ERROR_PAGE_TABLE | I915_ERROR_MEMORY_REFRESH));
 
 	/* Unmask the interrupts that we always want on. */
@@ -2404,33 +2631,35 @@ static int i915_irq_postinstall(struct drm_device *dev)
 	return 0;
 }
 
-static void i915_hpd_irq_setup(struct drm_device *dev)
+/*
+ * Returns true when a page flip has completed.
+ */
+static bool i915_handle_vblank(struct drm_device *dev,
+			       int plane, int pipe, u32 iir)
 {
-	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-	u32 hotplug_en;
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	u32 flip_pending = DISPLAY_PLANE_FLIP_PENDING(plane);
 
-	if (I915_HAS_HOTPLUG(dev)) {
-		hotplug_en = I915_READ(PORT_HOTPLUG_EN);
+	if (!drm_handle_vblank(dev, pipe))
+		return false;
 
-		if (dev_priv->hotplug_supported_mask & PORTB_HOTPLUG_INT_STATUS)
-			hotplug_en |= PORTB_HOTPLUG_INT_EN;
-		if (dev_priv->hotplug_supported_mask & PORTC_HOTPLUG_INT_STATUS)
-			hotplug_en |= PORTC_HOTPLUG_INT_EN;
-		if (dev_priv->hotplug_supported_mask & PORTD_HOTPLUG_INT_STATUS)
-			hotplug_en |= PORTD_HOTPLUG_INT_EN;
-		if (dev_priv->hotplug_supported_mask & SDVOC_HOTPLUG_INT_STATUS_I915)
-			hotplug_en |= SDVOC_HOTPLUG_INT_EN;
-		if (dev_priv->hotplug_supported_mask & SDVOB_HOTPLUG_INT_STATUS_I915)
-			hotplug_en |= SDVOB_HOTPLUG_INT_EN;
-		if (dev_priv->hotplug_supported_mask & CRT_HOTPLUG_INT_STATUS) {
-			hotplug_en |= CRT_HOTPLUG_INT_EN;
-			hotplug_en |= CRT_HOTPLUG_VOLTAGE_COMPARE_50;
-		}
+	if ((iir & flip_pending) == 0)
+		return false;
 
-		/* Ignore TV since it's buggy */
+	intel_prepare_page_flip(dev, plane);
 
-		I915_WRITE(PORT_HOTPLUG_EN, hotplug_en);
-	}
+	/* We detect FlipDone by looking for the change in PendingFlip from '1'
+	 * to '0' on the following vblank, i.e. IIR has the Pendingflip
+	 * asserted following the MI_DISPLAY_FLIP, but ISR is deasserted, hence
+	 * the flip is completed (no longer pending). Since this doesn't raise
+	 * an interrupt per se, we watch for the change at vblank.
+	 */
+	if (I915_READ(ISR) & flip_pending)
+		return false;
+
+	intel_finish_page_flip(dev, pipe);
+
+	return true;
 }
 
 static irqreturn_t i915_irq_handler(int irq, void *arg)
@@ -2442,10 +2671,6 @@ static irqreturn_t i915_irq_handler(int irq, void *arg)
 	u32 flip_mask =
 		I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
 		I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT;
-	u32 flip[2] = {
-		I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT,
-		I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT
-	};
 	int pipe, ret = IRQ_NONE;
 
 	atomic_inc(&dev_priv->irq_received);
@@ -2486,13 +2711,16 @@ static irqreturn_t i915_irq_handler(int irq, void *arg)
 		if ((I915_HAS_HOTPLUG(dev)) &&
 		    (iir & I915_DISPLAY_PORT_INTERRUPT)) {
 			u32 hotplug_status = I915_READ(PORT_HOTPLUG_STAT);
+			u32 hotplug_trigger = hotplug_status & HOTPLUG_INT_STATUS_I915;
 
 			DRM_DEBUG_DRIVER("hotplug event received, stat 0x%08x\n",
 				  hotplug_status);
-			if (hotplug_status & dev_priv->hotplug_supported_mask)
+			if (hotplug_trigger) {
+				if (hotplug_irq_storm_detect(dev, hotplug_trigger, hpd_status_i915))
+					i915_hpd_irq_setup(dev);
 				queue_work(dev_priv->wq,
 					   &dev_priv->hotplug_work);
-
+			}
 			I915_WRITE(PORT_HOTPLUG_STAT, hotplug_status);
 			POSTING_READ(PORT_HOTPLUG_STAT);
 		}
@@ -2507,14 +2735,10 @@ static irqreturn_t i915_irq_handler(int irq, void *arg)
 			int plane = pipe;
 			if (IS_MOBILE(dev))
 				plane = !plane;
+
 			if (pipe_stats[pipe] & PIPE_VBLANK_INTERRUPT_STATUS &&
-			    drm_handle_vblank(dev, pipe)) {
-				if (iir & flip[plane]) {
-					intel_prepare_page_flip(dev, plane);
-					intel_finish_page_flip(dev, pipe);
-					flip_mask &= ~flip[plane];
-				}
-			}
+			    i915_handle_vblank(dev, plane, pipe, iir))
+				flip_mask &= ~DISPLAY_PLANE_FLIP_PENDING(plane);
 
 			if (pipe_stats[pipe] & PIPE_LEGACY_BLC_EVENT_STATUS)
 				blc_event = true;
@@ -2552,6 +2776,8 @@ static void i915_irq_uninstall(struct drm_device * dev)
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
 	int pipe;
 
+	del_timer_sync(&dev_priv->hotplug_reenable_timer);
+
 	if (I915_HAS_HOTPLUG(dev)) {
 		I915_WRITE(PORT_HOTPLUG_EN, 0);
 		I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
@@ -2603,13 +2829,13 @@ static int i965_irq_postinstall(struct drm_device *dev)
 			       I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT);
 
 	enable_mask = ~dev_priv->irq_mask;
+	enable_mask &= ~(I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
+			 I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT);
 	enable_mask |= I915_USER_INTERRUPT;
 
 	if (IS_G4X(dev))
 		enable_mask |= I915_BSD_USER_INTERRUPT;
 
-	dev_priv->pipestat[0] = 0;
-	dev_priv->pipestat[1] = 0;
 	i915_enable_pipestat(dev_priv, 0, PIPE_GMBUS_EVENT_ENABLE);
 
 	/*
@@ -2639,45 +2865,33 @@ static int i965_irq_postinstall(struct drm_device *dev)
 	return 0;
 }
 
-static void i965_hpd_irq_setup(struct drm_device *dev)
+static void i915_hpd_irq_setup(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	struct drm_mode_config *mode_config = &dev->mode_config;
+	struct intel_encoder *intel_encoder;
 	u32 hotplug_en;
 
-	/* Note HDMI and DP share hotplug bits */
-	hotplug_en = 0;
-	if (dev_priv->hotplug_supported_mask & PORTB_HOTPLUG_INT_STATUS)
-		hotplug_en |= PORTB_HOTPLUG_INT_EN;
-	if (dev_priv->hotplug_supported_mask & PORTC_HOTPLUG_INT_STATUS)
-		hotplug_en |= PORTC_HOTPLUG_INT_EN;
-	if (dev_priv->hotplug_supported_mask & PORTD_HOTPLUG_INT_STATUS)
-		hotplug_en |= PORTD_HOTPLUG_INT_EN;
-	if (IS_G4X(dev)) {
-		if (dev_priv->hotplug_supported_mask & SDVOC_HOTPLUG_INT_STATUS_G4X)
-			hotplug_en |= SDVOC_HOTPLUG_INT_EN;
-		if (dev_priv->hotplug_supported_mask & SDVOB_HOTPLUG_INT_STATUS_G4X)
-			hotplug_en |= SDVOB_HOTPLUG_INT_EN;
-	} else {
-		if (dev_priv->hotplug_supported_mask & SDVOC_HOTPLUG_INT_STATUS_I965)
-			hotplug_en |= SDVOC_HOTPLUG_INT_EN;
-		if (dev_priv->hotplug_supported_mask & SDVOB_HOTPLUG_INT_STATUS_I965)
-			hotplug_en |= SDVOB_HOTPLUG_INT_EN;
-	}
-	if (dev_priv->hotplug_supported_mask & CRT_HOTPLUG_INT_STATUS) {
-		hotplug_en |= CRT_HOTPLUG_INT_EN;
-
+	if (I915_HAS_HOTPLUG(dev)) {
+		hotplug_en = I915_READ(PORT_HOTPLUG_EN);
+		hotplug_en &= ~HOTPLUG_INT_EN_MASK;
+		/* Note HDMI and DP share hotplug bits */
+		/* enable bits are the same for all generations */
+		list_for_each_entry(intel_encoder, &mode_config->encoder_list, base.head)
+			if (dev_priv->hpd_stats[intel_encoder->hpd_pin].hpd_mark == HPD_ENABLED)
+				hotplug_en |= hpd_mask_i915[intel_encoder->hpd_pin];
 		/* Programming the CRT detection parameters tends
 		   to generate a spurious hotplug event about three
 		   seconds later.  So just do it once.
-		   */
+		*/
 		if (IS_G4X(dev))
 			hotplug_en |= CRT_HOTPLUG_ACTIVATION_PERIOD_64;
+		hotplug_en &= ~CRT_HOTPLUG_VOLTAGE_COMPARE_MASK;
 		hotplug_en |= CRT_HOTPLUG_VOLTAGE_COMPARE_50;
-	}
-
-	/* Ignore TV since it's buggy */
 
-	I915_WRITE(PORT_HOTPLUG_EN, hotplug_en);
+		/* Ignore TV since it's buggy */
+		I915_WRITE(PORT_HOTPLUG_EN, hotplug_en);
+	}
 }
 
 static irqreturn_t i965_irq_handler(int irq, void *arg)
@@ -2689,6 +2903,9 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 	unsigned long irqflags;
 	int irq_received;
 	int ret = IRQ_NONE, pipe;
+	u32 flip_mask =
+		I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
+		I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT;
 
 	atomic_inc(&dev_priv->irq_received);
 
@@ -2697,7 +2914,7 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 	for (;;) {
 		bool blc_event = false;
 
-		irq_received = iir != 0;
+		irq_received = (iir & ~flip_mask) != 0;
 
 		/* Can't rely on pipestat interrupt bit in iir as it might
 		 * have been cleared after the pipestat interrupt was received.
@@ -2733,18 +2950,24 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 		/* Consume port.  Then clear IIR or we'll miss events */
 		if (iir & I915_DISPLAY_PORT_INTERRUPT) {
 			u32 hotplug_status = I915_READ(PORT_HOTPLUG_STAT);
+			u32 hotplug_trigger = hotplug_status & (IS_G4X(dev) ?
+								  HOTPLUG_INT_STATUS_G4X :
+								  HOTPLUG_INT_STATUS_I965);
 
 			DRM_DEBUG_DRIVER("hotplug event received, stat 0x%08x\n",
 				  hotplug_status);
-			if (hotplug_status & dev_priv->hotplug_supported_mask)
+			if (hotplug_trigger) {
+				if (hotplug_irq_storm_detect(dev, hotplug_trigger,
+							    IS_G4X(dev) ? hpd_status_gen4 : hpd_status_i965))
+					i915_hpd_irq_setup(dev);
 				queue_work(dev_priv->wq,
 					   &dev_priv->hotplug_work);
-
+			}
 			I915_WRITE(PORT_HOTPLUG_STAT, hotplug_status);
 			I915_READ(PORT_HOTPLUG_STAT);
 		}
 
-		I915_WRITE(IIR, iir);
+		I915_WRITE(IIR, iir & ~flip_mask);
 		new_iir = I915_READ(IIR); /* Flush posted writes */
 
 		if (iir & I915_USER_INTERRUPT)
@@ -2752,18 +2975,10 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 		if (iir & I915_BSD_USER_INTERRUPT)
 			notify_ring(dev, &dev_priv->ring[VCS]);
 
-		if (iir & I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT)
-			intel_prepare_page_flip(dev, 0);
-
-		if (iir & I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT)
-			intel_prepare_page_flip(dev, 1);
-
 		for_each_pipe(pipe) {
 			if (pipe_stats[pipe] & PIPE_START_VBLANK_INTERRUPT_STATUS &&
-			    drm_handle_vblank(dev, pipe)) {
-				i915_pageflip_stall_check(dev, pipe);
-				intel_finish_page_flip(dev, pipe);
-			}
+			    i915_handle_vblank(dev, pipe, pipe, iir))
+				flip_mask &= ~DISPLAY_PLANE_FLIP_PENDING(pipe);
 
 			if (pipe_stats[pipe] & PIPE_LEGACY_BLC_EVENT_STATUS)
 				blc_event = true;
@@ -2807,6 +3022,8 @@ static void i965_irq_uninstall(struct drm_device * dev)
 	if (!dev_priv)
 		return;
 
+	del_timer_sync(&dev_priv->hotplug_reenable_timer);
+
 	I915_WRITE(PORT_HOTPLUG_EN, 0);
 	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
 
@@ -2822,6 +3039,41 @@ static void i965_irq_uninstall(struct drm_device * dev)
 	I915_WRITE(IIR, I915_READ(IIR));
 }
 
+static void i915_reenable_hotplug_timer_func(unsigned long data)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *)data;
+	struct drm_device *dev = dev_priv->dev;
+	struct drm_mode_config *mode_config = &dev->mode_config;
+	unsigned long irqflags;
+	int i;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
+	for (i = (HPD_NONE + 1); i < HPD_NUM_PINS; i++) {
+		struct drm_connector *connector;
+
+		if (dev_priv->hpd_stats[i].hpd_mark != HPD_DISABLED)
+			continue;
+
+		dev_priv->hpd_stats[i].hpd_mark = HPD_ENABLED;
+
+		list_for_each_entry(connector, &mode_config->connector_list, head) {
+			struct intel_connector *intel_connector = to_intel_connector(connector);
+
+			if (intel_connector->encoder->hpd_pin == i) {
+				if (connector->polled != intel_connector->polled)
+					DRM_DEBUG_DRIVER("Reenabling HPD on connector %s\n",
+							 drm_get_connector_name(connector));
+				connector->polled = intel_connector->polled;
+				if (!connector->polled)
+					connector->polled = DRM_CONNECTOR_POLL_HPD;
+			}
+		}
+	}
+	if (dev_priv->display.hpd_irq_setup)
+		dev_priv->display.hpd_irq_setup(dev);
+	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+}
+
 void intel_irq_init(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -2834,6 +3086,8 @@ void intel_irq_init(struct drm_device *dev)
 	setup_timer(&dev_priv->gpu_error.hangcheck_timer,
 		    i915_hangcheck_elapsed,
 		    (unsigned long) dev);
+	setup_timer(&dev_priv->hotplug_reenable_timer, i915_reenable_hotplug_timer_func,
+		    (unsigned long) dev_priv);
 
 	pm_qos_add_request(&dev_priv->pm_qos, PM_QOS_CPU_DMA_LATENCY, PM_QOS_DEFAULT_VALUE);
 
@@ -2857,7 +3111,7 @@ void intel_irq_init(struct drm_device *dev)
 		dev->driver->irq_uninstall = valleyview_irq_uninstall;
 		dev->driver->enable_vblank = valleyview_enable_vblank;
 		dev->driver->disable_vblank = valleyview_disable_vblank;
-		dev_priv->display.hpd_irq_setup = valleyview_hpd_irq_setup;
+		dev_priv->display.hpd_irq_setup = i915_hpd_irq_setup;
 	} else if (IS_IVYBRIDGE(dev) || IS_HASWELL(dev)) {
 		/* Share pre & uninstall handlers with ILK/SNB */
 		dev->driver->irq_handler = ivybridge_irq_handler;
@@ -2866,6 +3120,7 @@ void intel_irq_init(struct drm_device *dev)
 		dev->driver->irq_uninstall = ironlake_irq_uninstall;
 		dev->driver->enable_vblank = ivybridge_enable_vblank;
 		dev->driver->disable_vblank = ivybridge_disable_vblank;
+		dev_priv->display.hpd_irq_setup = ibx_hpd_irq_setup;
 	} else if (HAS_PCH_SPLIT(dev)) {
 		dev->driver->irq_handler = ironlake_irq_handler;
 		dev->driver->irq_preinstall = ironlake_irq_preinstall;
@@ -2873,6 +3128,7 @@ void intel_irq_init(struct drm_device *dev)
 		dev->driver->irq_uninstall = ironlake_irq_uninstall;
 		dev->driver->enable_vblank = ironlake_enable_vblank;
 		dev->driver->disable_vblank = ironlake_disable_vblank;
+		dev_priv->display.hpd_irq_setup = ibx_hpd_irq_setup;
 	} else {
 		if (INTEL_INFO(dev)->gen == 2) {
 			dev->driver->irq_preinstall = i8xx_irq_preinstall;
@@ -2890,7 +3146,7 @@ void intel_irq_init(struct drm_device *dev)
 			dev->driver->irq_postinstall = i965_irq_postinstall;
 			dev->driver->irq_uninstall = i965_irq_uninstall;
 			dev->driver->irq_handler = i965_irq_handler;
-			dev_priv->display.hpd_irq_setup = i965_hpd_irq_setup;
+			dev_priv->display.hpd_irq_setup = i915_hpd_irq_setup;
 		}
 		dev->driver->enable_vblank = i915_enable_vblank;
 		dev->driver->disable_vblank = i915_disable_vblank;
@@ -2900,7 +3156,20 @@ void intel_irq_init(struct drm_device *dev)
 void intel_hpd_init(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_mode_config *mode_config = &dev->mode_config;
+	struct drm_connector *connector;
+	int i;
 
+	for (i = 1; i < HPD_NUM_PINS; i++) {
+		dev_priv->hpd_stats[i].hpd_cnt = 0;
+		dev_priv->hpd_stats[i].hpd_mark = HPD_ENABLED;
+	}
+	list_for_each_entry(connector, &mode_config->connector_list, head) {
+		struct intel_connector *intel_connector = to_intel_connector(connector);
+		connector->polled = intel_connector->polled;
+		if (!connector->polled && I915_HAS_HOTPLUG(dev) && intel_connector->encoder->hpd_pin > HPD_NONE)
+			connector->polled = DRM_CONNECTOR_POLL_HPD;
+	}
 	if (dev_priv->display.hpd_irq_setup)
 		dev_priv->display.hpd_irq_setup(dev);
 }
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index 848992f67d56..83f9c26e1adb 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -91,6 +91,7 @@
 #define  GRDOM_FULL	(0<<2)
 #define  GRDOM_RENDER	(1<<2)
 #define  GRDOM_MEDIA	(3<<2)
+#define  GRDOM_MASK	(3<<2)
 #define  GRDOM_RESET_ENABLE (1<<0)
 
 #define GEN6_MBCUNIT_SNPCR	0x900c /* for LLC config */
@@ -121,10 +122,17 @@
 
 #define GAM_ECOCHK			0x4090
 #define   ECOCHK_SNB_BIT		(1<<10)
+#define   HSW_ECOCHK_ARB_PRIO_SOL	(1<<6)
 #define   ECOCHK_PPGTT_CACHE64B		(0x3<<3)
 #define   ECOCHK_PPGTT_CACHE4B		(0x0<<3)
+#define   ECOCHK_PPGTT_GFDT_IVB		(0x1<<4)
+#define   ECOCHK_PPGTT_LLC_IVB		(0x1<<3)
+#define   ECOCHK_PPGTT_UC_HSW		(0x1<<3)
+#define   ECOCHK_PPGTT_WT_HSW		(0x2<<3)
+#define   ECOCHK_PPGTT_WB_HSW		(0x3<<3)
 
 #define GAC_ECO_BITS			0x14090
+#define   ECOBITS_SNB_BIT		(1<<13)
 #define   ECOBITS_PPGTT_CACHE64B	(3<<8)
 #define   ECOBITS_PPGTT_CACHE4B		(0<<8)
 
@@ -422,6 +430,7 @@
 
 #define FENCE_REG_SANDYBRIDGE_0		0x100000
 #define   SANDYBRIDGE_FENCE_PITCH_SHIFT	32
+#define   GEN7_FENCE_MAX_PITCH_VAL	0x0800
 
 /* control register for cpu gtt access */
 #define TILECTL				0x101000
@@ -522,6 +531,9 @@
 #define GEN7_ERR_INT	0x44040
 #define   ERR_INT_MMIO_UNCLAIMED (1<<13)
 
+#define FPGA_DBG		0x42300
+#define   FPGA_DBG_RM_NOCLAIM	(1<<31)
+
 #define DERRMR		0x44050
 
 /* GM45+ chicken bits -- debug workaround bits that may be required
@@ -591,6 +603,7 @@
 #define   I915_USER_INTERRUPT				(1<<1)
 #define   I915_ASLE_INTERRUPT				(1<<0)
 #define   I915_BSD_USER_INTERRUPT                      (1<<25)
+#define   DISPLAY_PLANE_FLIP_PENDING(plane) (1<<(11-(plane))) /* A and B only */
 #define EIR		0x020b0
 #define EMR		0x020b4
 #define ESR		0x020b8
@@ -1197,6 +1210,9 @@
 
 #define MCHBAR_MIRROR_BASE_SNB	0x140000
 
+/* Memory controller frequency in MCHBAR for Haswell (possible SNB+) */
+#define DCLK 0x5e04
+
 /** 915-945 and GM965 MCH register controlling DRAM channel access */
 #define DCC			0x10200
 #define DCC_ADDRESSING_MODE_SINGLE_CHANNEL		(0 << 0)
@@ -1637,6 +1653,12 @@
 #define   SDVOC_HOTPLUG_INT_EN			(1 << 25)
 #define   TV_HOTPLUG_INT_EN			(1 << 18)
 #define   CRT_HOTPLUG_INT_EN			(1 << 9)
+#define HOTPLUG_INT_EN_MASK			(PORTB_HOTPLUG_INT_EN | \
+						 PORTC_HOTPLUG_INT_EN | \
+						 PORTD_HOTPLUG_INT_EN | \
+						 SDVOC_HOTPLUG_INT_EN | \
+						 SDVOB_HOTPLUG_INT_EN | \
+						 CRT_HOTPLUG_INT_EN)
 #define   CRT_HOTPLUG_FORCE_DETECT		(1 << 3)
 #define CRT_HOTPLUG_ACTIVATION_PERIOD_32	(0 << 8)
 /* must use period 64 on GM45 according to docs */
@@ -1675,43 +1697,84 @@
 #define   SDVOB_HOTPLUG_INT_STATUS_I965		(3 << 2)
 #define   SDVOC_HOTPLUG_INT_STATUS_I915		(1 << 7)
 #define   SDVOB_HOTPLUG_INT_STATUS_I915		(1 << 6)
-
-/* SDVO port control */
-#define SDVOB			0x61140
-#define SDVOC			0x61160
-#define   SDVO_ENABLE		(1 << 31)
-#define   SDVO_PIPE_B_SELECT	(1 << 30)
-#define   SDVO_STALL_SELECT	(1 << 29)
-#define   SDVO_INTERRUPT_ENABLE	(1 << 26)
+#define   HOTPLUG_INT_STATUS_G4X		(CRT_HOTPLUG_INT_STATUS | \
+						 SDVOB_HOTPLUG_INT_STATUS_G4X | \
+						 SDVOC_HOTPLUG_INT_STATUS_G4X | \
+						 PORTB_HOTPLUG_INT_STATUS | \
+						 PORTC_HOTPLUG_INT_STATUS | \
+						 PORTD_HOTPLUG_INT_STATUS)
+
+#define HOTPLUG_INT_STATUS_I965			(CRT_HOTPLUG_INT_STATUS | \
+						 SDVOB_HOTPLUG_INT_STATUS_I965 | \
+						 SDVOC_HOTPLUG_INT_STATUS_I965 | \
+						 PORTB_HOTPLUG_INT_STATUS | \
+						 PORTC_HOTPLUG_INT_STATUS | \
+						 PORTD_HOTPLUG_INT_STATUS)
+
+#define HOTPLUG_INT_STATUS_I915			(CRT_HOTPLUG_INT_STATUS | \
+						 SDVOB_HOTPLUG_INT_STATUS_I915 | \
+						 SDVOC_HOTPLUG_INT_STATUS_I915 | \
+						 PORTB_HOTPLUG_INT_STATUS | \
+						 PORTC_HOTPLUG_INT_STATUS | \
+						 PORTD_HOTPLUG_INT_STATUS)
+
+/* SDVO and HDMI port control.
+ * The same register may be used for SDVO or HDMI */
+#define GEN3_SDVOB	0x61140
+#define GEN3_SDVOC	0x61160
+#define GEN4_HDMIB	GEN3_SDVOB
+#define GEN4_HDMIC	GEN3_SDVOC
+#define PCH_SDVOB	0xe1140
+#define PCH_HDMIB	PCH_SDVOB
+#define PCH_HDMIC	0xe1150
+#define PCH_HDMID	0xe1160
+
+/* Gen 3 SDVO bits: */
+#define   SDVO_ENABLE				(1 << 31)
+#define   SDVO_PIPE_SEL(pipe)			((pipe) << 30)
+#define   SDVO_PIPE_SEL_MASK			(1 << 30)
+#define   SDVO_PIPE_B_SELECT			(1 << 30)
+#define   SDVO_STALL_SELECT			(1 << 29)
+#define   SDVO_INTERRUPT_ENABLE			(1 << 26)
 /**
  * 915G/GM SDVO pixel multiplier.
- *
  * Programmed value is multiplier - 1, up to 5x.
- *
  * \sa DPLL_MD_UDI_MULTIPLIER_MASK
  */
-#define   SDVO_PORT_MULTIPLY_MASK	(7 << 23)
+#define   SDVO_PORT_MULTIPLY_MASK		(7 << 23)
 #define   SDVO_PORT_MULTIPLY_SHIFT		23
-#define   SDVO_PHASE_SELECT_MASK	(15 << 19)
-#define   SDVO_PHASE_SELECT_DEFAULT	(6 << 19)
-#define   SDVO_CLOCK_OUTPUT_INVERT	(1 << 18)
-#define   SDVOC_GANG_MODE		(1 << 16)
-#define   SDVO_ENCODING_SDVO		(0x0 << 10)
-#define   SDVO_ENCODING_HDMI		(0x2 << 10)
-/** Requird for HDMI operation */
-#define   SDVO_NULL_PACKETS_DURING_VSYNC (1 << 9)
-#define   SDVO_COLOR_RANGE_16_235	(1 << 8)
-#define   SDVO_BORDER_ENABLE		(1 << 7)
-#define   SDVO_AUDIO_ENABLE		(1 << 6)
-/** New with 965, default is to be set */
-#define   SDVO_VSYNC_ACTIVE_HIGH	(1 << 4)
-/** New with 965, default is to be set */
-#define   SDVO_HSYNC_ACTIVE_HIGH	(1 << 3)
-#define   SDVOB_PCIE_CONCURRENCY	(1 << 3)
-#define   SDVO_DETECTED			(1 << 2)
+#define   SDVO_PHASE_SELECT_MASK		(15 << 19)
+#define   SDVO_PHASE_SELECT_DEFAULT		(6 << 19)
+#define   SDVO_CLOCK_OUTPUT_INVERT		(1 << 18)
+#define   SDVOC_GANG_MODE			(1 << 16) /* Port C only */
+#define   SDVO_BORDER_ENABLE			(1 << 7) /* SDVO only */
+#define   SDVOB_PCIE_CONCURRENCY		(1 << 3) /* Port B only */
+#define   SDVO_DETECTED				(1 << 2)
 /* Bits to be preserved when writing */
-#define   SDVOB_PRESERVE_MASK ((1 << 17) | (1 << 16) | (1 << 14) | (1 << 26))
-#define   SDVOC_PRESERVE_MASK ((1 << 17) | (1 << 26))
+#define   SDVOB_PRESERVE_MASK ((1 << 17) | (1 << 16) | (1 << 14) | \
+			       SDVO_INTERRUPT_ENABLE)
+#define   SDVOC_PRESERVE_MASK ((1 << 17) | SDVO_INTERRUPT_ENABLE)
+
+/* Gen 4 SDVO/HDMI bits: */
+#define   SDVO_COLOR_FORMAT_8bpc		(0 << 26)
+#define   SDVO_ENCODING_SDVO			(0 << 10)
+#define   SDVO_ENCODING_HDMI			(2 << 10)
+#define   HDMI_MODE_SELECT_HDMI			(1 << 9) /* HDMI only */
+#define   HDMI_MODE_SELECT_DVI			(0 << 9) /* HDMI only */
+#define   HDMI_COLOR_RANGE_16_235		(1 << 8) /* HDMI only */
+#define   SDVO_AUDIO_ENABLE			(1 << 6)
+/* VSYNC/HSYNC bits new with 965, default is to be set */
+#define   SDVO_VSYNC_ACTIVE_HIGH		(1 << 4)
+#define   SDVO_HSYNC_ACTIVE_HIGH		(1 << 3)
+
+/* Gen 5 (IBX) SDVO/HDMI bits: */
+#define   HDMI_COLOR_FORMAT_12bpc		(3 << 26) /* HDMI only */
+#define   SDVOB_HOTPLUG_ENABLE			(1 << 23) /* SDVO only */
+
+/* Gen 6 (CPT) SDVO/HDMI bits: */
+#define   SDVO_PIPE_SEL_CPT(pipe)		((pipe) << 29)
+#define   SDVO_PIPE_SEL_MASK_CPT		(3 << 29)
+
 
 /* DVO port control */
 #define DVOA			0x61120
@@ -1898,7 +1961,7 @@
 #define PFIT_AUTO_RATIOS (dev_priv->info->display_mmio_offset + 0x61238)
 
 /* Backlight control */
-#define BLC_PWM_CTL2		0x61250 /* 965+ only */
+#define BLC_PWM_CTL2	(dev_priv->info->display_mmio_offset + 0x61250) /* 965+ only */
 #define   BLM_PWM_ENABLE		(1 << 31)
 #define   BLM_COMBINATION_MODE		(1 << 30) /* gen4 only */
 #define   BLM_PIPE_SELECT		(1 << 29)
@@ -1917,7 +1980,7 @@
 #define   BLM_PHASE_IN_COUNT_MASK	(0xff << 8)
 #define   BLM_PHASE_IN_INCR_SHIFT	(0)
 #define   BLM_PHASE_IN_INCR_MASK	(0xff << 0)
-#define BLC_PWM_CTL		0x61254
+#define BLC_PWM_CTL	(dev_priv->info->display_mmio_offset + 0x61254)
 /*
  * This is the most significant 15 bits of the number of backlight cycles in a
  * complete cycle of the modulated backlight control.
@@ -1939,7 +2002,7 @@
 #define   BACKLIGHT_DUTY_CYCLE_MASK_PNV		(0xfffe)
 #define   BLM_POLARITY_PNV			(1 << 0) /* pnv only */
 
-#define BLC_HIST_CTL		0x61260
+#define BLC_HIST_CTL	(dev_priv->info->display_mmio_offset + 0x61260)
 
 /* New registers for PCH-split platforms. Safe where new bits show up, the
  * register layout machtes with gen4 BLC_PWM_CTL[12]. */
@@ -2589,14 +2652,14 @@
 #define _PIPEB_GMCH_DATA_M			0x71050
 
 /* Transfer unit size for display port - 1, default is 0x3f (for TU size 64) */
-#define   PIPE_GMCH_DATA_M_TU_SIZE_MASK		(0x3f << 25)
-#define   PIPE_GMCH_DATA_M_TU_SIZE_SHIFT	25
+#define  TU_SIZE(x)             (((x)-1) << 25) /* default size 64 */
+#define  TU_SIZE_MASK           (0x3f << 25)
 
-#define   PIPE_GMCH_DATA_M_MASK			(0xffffff)
+#define  DATA_LINK_M_N_MASK	(0xffffff)
+#define  DATA_LINK_N_MAX	(0x800000)
 
 #define _PIPEA_GMCH_DATA_N			0x70054
 #define _PIPEB_GMCH_DATA_N			0x71054
-#define   PIPE_GMCH_DATA_N_MASK			(0xffffff)
 
 /*
  * Computing Link M and N values for the Display Port link
@@ -2611,11 +2674,9 @@
 
 #define _PIPEA_DP_LINK_M				0x70060
 #define _PIPEB_DP_LINK_M				0x71060
-#define   PIPEA_DP_LINK_M_MASK			(0xffffff)
 
 #define _PIPEA_DP_LINK_N				0x70064
 #define _PIPEB_DP_LINK_N				0x71064
-#define   PIPEA_DP_LINK_N_MASK			(0xffffff)
 
 #define PIPE_GMCH_DATA_M(pipe) _PIPE(pipe, _PIPEA_GMCH_DATA_M, _PIPEB_GMCH_DATA_M)
 #define PIPE_GMCH_DATA_N(pipe) _PIPE(pipe, _PIPEA_GMCH_DATA_N, _PIPEB_GMCH_DATA_N)
@@ -2776,6 +2837,8 @@
 #define   DSPFW_HPLL_CURSOR_SHIFT	16
 #define   DSPFW_HPLL_CURSOR_MASK	(0x3f<<16)
 #define   DSPFW_HPLL_SR_MASK		(0x1ff)
+#define DSPFW4			(dev_priv->info->display_mmio_offset + 0x70070)
+#define DSPFW7			(dev_priv->info->display_mmio_offset + 0x7007c)
 
 /* drain latency register values*/
 #define DRAIN_LATENCY_PRECISION_32	32
@@ -3233,6 +3296,63 @@
 #define SPRGAMC(pipe) _PIPE(pipe, _SPRA_GAMC, _SPRB_GAMC)
 #define SPRSURFLIVE(pipe) _PIPE(pipe, _SPRA_SURFLIVE, _SPRB_SURFLIVE)
 
+#define _SPACNTR		0x72180
+#define   SP_ENABLE			(1<<31)
+#define   SP_GEAMMA_ENABLE		(1<<30)
+#define   SP_PIXFORMAT_MASK		(0xf<<26)
+#define   SP_FORMAT_YUV422		(0<<26)
+#define   SP_FORMAT_BGR565		(5<<26)
+#define   SP_FORMAT_BGRX8888		(6<<26)
+#define   SP_FORMAT_BGRA8888		(7<<26)
+#define   SP_FORMAT_RGBX1010102		(8<<26)
+#define   SP_FORMAT_RGBA1010102		(9<<26)
+#define   SP_FORMAT_RGBX8888		(0xe<<26)
+#define   SP_FORMAT_RGBA8888		(0xf<<26)
+#define   SP_SOURCE_KEY			(1<<22)
+#define   SP_YUV_BYTE_ORDER_MASK	(3<<16)
+#define   SP_YUV_ORDER_YUYV		(0<<16)
+#define   SP_YUV_ORDER_UYVY		(1<<16)
+#define   SP_YUV_ORDER_YVYU		(2<<16)
+#define   SP_YUV_ORDER_VYUY		(3<<16)
+#define   SP_TILED			(1<<10)
+#define _SPALINOFF		0x72184
+#define _SPASTRIDE		0x72188
+#define _SPAPOS			0x7218c
+#define _SPASIZE		0x72190
+#define _SPAKEYMINVAL		0x72194
+#define _SPAKEYMSK		0x72198
+#define _SPASURF		0x7219c
+#define _SPAKEYMAXVAL		0x721a0
+#define _SPATILEOFF		0x721a4
+#define _SPACONSTALPHA		0x721a8
+#define _SPAGAMC		0x721f4
+
+#define _SPBCNTR		0x72280
+#define _SPBLINOFF		0x72284
+#define _SPBSTRIDE		0x72288
+#define _SPBPOS			0x7228c
+#define _SPBSIZE		0x72290
+#define _SPBKEYMINVAL		0x72294
+#define _SPBKEYMSK		0x72298
+#define _SPBSURF		0x7229c
+#define _SPBKEYMAXVAL		0x722a0
+#define _SPBTILEOFF		0x722a4
+#define _SPBCONSTALPHA		0x722a8
+#define _SPBGAMC		0x722f4
+
+#define SPCNTR(pipe, plane) _PIPE(pipe * 2 + plane, _SPACNTR, _SPBCNTR)
+#define SPLINOFF(pipe, plane) _PIPE(pipe * 2 + plane, _SPALINOFF, _SPBLINOFF)
+#define SPSTRIDE(pipe, plane) _PIPE(pipe * 2 + plane, _SPASTRIDE, _SPBSTRIDE)
+#define SPPOS(pipe, plane) _PIPE(pipe * 2 + plane, _SPAPOS, _SPBPOS)
+#define SPSIZE(pipe, plane) _PIPE(pipe * 2 + plane, _SPASIZE, _SPBSIZE)
+#define SPKEYMINVAL(pipe, plane) _PIPE(pipe * 2 + plane, _SPAKEYMINVAL, _SPBKEYMINVAL)
+#define SPKEYMSK(pipe, plane) _PIPE(pipe * 2 + plane, _SPAKEYMSK, _SPBKEYMSK)
+#define SPSURF(pipe, plane) _PIPE(pipe * 2 + plane, _SPASURF, _SPBSURF)
+#define SPKEYMAXVAL(pipe, plane) _PIPE(pipe * 2 + plane, _SPAKEYMAXVAL, _SPBKEYMAXVAL)
+#define SPTILEOFF(pipe, plane) _PIPE(pipe * 2 + plane, _SPATILEOFF, _SPBTILEOFF)
+#define SPCONSTALPHA(pipe, plane) _PIPE(pipe * 2 + plane, _SPACONSTALPHA, _SPBCONSTALPHA)
+#define SPGAMC(pipe, plane) _PIPE(pipe * 2 + plane, _SPAGAMC, _SPBGAMC)
+
 /* VBIOS regs */
 #define VGACNTRL		0x71400
 # define VGA_DISP_DISABLE			(1 << 31)
@@ -3282,8 +3402,6 @@
 
 
 #define _PIPEA_DATA_M1           (dev_priv->info->display_mmio_offset + 0x60030)
-#define  TU_SIZE(x)             (((x)-1) << 25) /* default size 64 */
-#define  TU_SIZE_MASK           0x7e000000
 #define  PIPE_DATA_M1_OFFSET    0
 #define _PIPEA_DATA_N1           (dev_priv->info->display_mmio_offset + 0x60034)
 #define  PIPE_DATA_N1_OFFSET    0
@@ -3456,6 +3574,9 @@
 #define DISP_ARB_CTL	0x45000
 #define  DISP_TILE_SURFACE_SWIZZLING	(1<<13)
 #define  DISP_FBC_WM_DIS		(1<<15)
+#define GEN7_MSG_CTL	0x45010
+#define  WAIT_FOR_PCH_RESET_ACK		(1<<1)
+#define  WAIT_FOR_PCH_FLR_ACK		(1<<0)
 
 /* GEN7 chicken */
 #define GEN7_COMMON_SLICE_CHICKEN1		0x7010
@@ -3508,7 +3629,11 @@
 #define SDE_PORTC_HOTPLUG       (1 << 9)
 #define SDE_PORTB_HOTPLUG       (1 << 8)
 #define SDE_SDVOB_HOTPLUG       (1 << 6)
-#define SDE_HOTPLUG_MASK	(0xf << 8)
+#define SDE_HOTPLUG_MASK        (SDE_CRT_HOTPLUG | \
+				 SDE_SDVOB_HOTPLUG |	\
+				 SDE_PORTB_HOTPLUG |	\
+				 SDE_PORTC_HOTPLUG |	\
+				 SDE_PORTD_HOTPLUG)
 #define SDE_TRANSB_CRC_DONE	(1 << 5)
 #define SDE_TRANSB_CRC_ERR	(1 << 4)
 #define SDE_TRANSB_FIFO_UNDER	(1 << 3)
@@ -3531,7 +3656,9 @@
 #define SDE_PORTC_HOTPLUG_CPT	(1 << 22)
 #define SDE_PORTB_HOTPLUG_CPT	(1 << 21)
 #define SDE_CRT_HOTPLUG_CPT	(1 << 19)
+#define SDE_SDVOB_HOTPLUG_CPT	(1 << 18)
 #define SDE_HOTPLUG_MASK_CPT	(SDE_CRT_HOTPLUG_CPT |		\
+				 SDE_SDVOB_HOTPLUG_CPT |	\
 				 SDE_PORTD_HOTPLUG_CPT |	\
 				 SDE_PORTC_HOTPLUG_CPT |	\
 				 SDE_PORTB_HOTPLUG_CPT)
@@ -3754,14 +3881,16 @@
 #define HSW_VIDEO_DIP_VSC_ECC_B		0x61344
 #define HSW_VIDEO_DIP_GCP_B		0x61210
 
-#define HSW_TVIDEO_DIP_CTL(pipe) \
-	 _PIPE(pipe, HSW_VIDEO_DIP_CTL_A, HSW_VIDEO_DIP_CTL_B)
-#define HSW_TVIDEO_DIP_AVI_DATA(pipe) \
-	 _PIPE(pipe, HSW_VIDEO_DIP_AVI_DATA_A, HSW_VIDEO_DIP_AVI_DATA_B)
-#define HSW_TVIDEO_DIP_SPD_DATA(pipe) \
-	 _PIPE(pipe, HSW_VIDEO_DIP_SPD_DATA_A, HSW_VIDEO_DIP_SPD_DATA_B)
-#define HSW_TVIDEO_DIP_GCP(pipe) \
-	_PIPE(pipe, HSW_VIDEO_DIP_GCP_A, HSW_VIDEO_DIP_GCP_B)
+#define HSW_TVIDEO_DIP_CTL(trans) \
+	 _TRANSCODER(trans, HSW_VIDEO_DIP_CTL_A, HSW_VIDEO_DIP_CTL_B)
+#define HSW_TVIDEO_DIP_AVI_DATA(trans) \
+	 _TRANSCODER(trans, HSW_VIDEO_DIP_AVI_DATA_A, HSW_VIDEO_DIP_AVI_DATA_B)
+#define HSW_TVIDEO_DIP_SPD_DATA(trans) \
+	 _TRANSCODER(trans, HSW_VIDEO_DIP_SPD_DATA_A, HSW_VIDEO_DIP_SPD_DATA_B)
+#define HSW_TVIDEO_DIP_GCP(trans) \
+	_TRANSCODER(trans, HSW_VIDEO_DIP_GCP_A, HSW_VIDEO_DIP_GCP_B)
+#define HSW_TVIDEO_DIP_VSC_DATA(trans) \
+	 _TRANSCODER(trans, HSW_VIDEO_DIP_VSC_DATA_A, HSW_VIDEO_DIP_VSC_DATA_B)
 
 #define _TRANS_HTOTAL_B          0xe1000
 #define _TRANS_HBLANK_B          0xe1004
@@ -3826,8 +3955,11 @@
 #define _TRANSA_CHICKEN2	 0xf0064
 #define _TRANSB_CHICKEN2	 0xf1064
 #define TRANS_CHICKEN2(pipe) _PIPE(pipe, _TRANSA_CHICKEN2, _TRANSB_CHICKEN2)
-#define  TRANS_CHICKEN2_TIMING_OVERRIDE		(1<<31)
-
+#define  TRANS_CHICKEN2_TIMING_OVERRIDE			(1<<31)
+#define  TRANS_CHICKEN2_FDI_POLARITY_REVERSED		(1<<29)
+#define  TRANS_CHICKEN2_FRAME_START_DELAY_MASK		(3<<27)
+#define  TRANS_CHICKEN2_DISABLE_DEEP_COLOR_COUNTER	(1<<26)
+#define  TRANS_CHICKEN2_DISABLE_DEEP_COLOR_MODESWITCH	(1<<25)
 
 #define SOUTH_CHICKEN1		0xc2000
 #define  FDIA_PHASE_SYNC_SHIFT_OVR	19
@@ -3976,34 +4108,6 @@
 #define FDI_PLL_CTL_1           0xfe000
 #define FDI_PLL_CTL_2           0xfe004
 
-/* or SDVOB */
-#define HDMIB   0xe1140
-#define  PORT_ENABLE    (1 << 31)
-#define  TRANSCODER(pipe)       ((pipe) << 30)
-#define  TRANSCODER_CPT(pipe)   ((pipe) << 29)
-#define  TRANSCODER_MASK        (1 << 30)
-#define  TRANSCODER_MASK_CPT    (3 << 29)
-#define  COLOR_FORMAT_8bpc      (0)
-#define  COLOR_FORMAT_12bpc     (3 << 26)
-#define  SDVOB_HOTPLUG_ENABLE   (1 << 23)
-#define  SDVO_ENCODING          (0)
-#define  TMDS_ENCODING          (2 << 10)
-#define  NULL_PACKET_VSYNC_ENABLE       (1 << 9)
-/* CPT */
-#define  HDMI_MODE_SELECT	(1 << 9)
-#define  DVI_MODE_SELECT	(0)
-#define  SDVOB_BORDER_ENABLE    (1 << 7)
-#define  AUDIO_ENABLE           (1 << 6)
-#define  VSYNC_ACTIVE_HIGH      (1 << 4)
-#define  HSYNC_ACTIVE_HIGH      (1 << 3)
-#define  PORT_DETECTED          (1 << 2)
-
-/* PCH SDVOB multiplex with HDMIB */
-#define PCH_SDVOB	HDMIB
-
-#define HDMIC   0xe1150
-#define HDMID   0xe1160
-
 #define PCH_LVDS	0xe1180
 #define  LVDS_DETECTED	(1 << 1)
 
@@ -4020,6 +4124,15 @@
 #define PIPEB_PP_OFF_DELAYS     (VLV_DISPLAY_BASE + 0x6130c)
 #define PIPEB_PP_DIVISOR        (VLV_DISPLAY_BASE + 0x61310)
 
+#define VLV_PIPE_PP_STATUS(pipe) _PIPE(pipe, PIPEA_PP_STATUS, PIPEB_PP_STATUS)
+#define VLV_PIPE_PP_CONTROL(pipe) _PIPE(pipe, PIPEA_PP_CONTROL, PIPEB_PP_CONTROL)
+#define VLV_PIPE_PP_ON_DELAYS(pipe) \
+		_PIPE(pipe, PIPEA_PP_ON_DELAYS, PIPEB_PP_ON_DELAYS)
+#define VLV_PIPE_PP_OFF_DELAYS(pipe) \
+		_PIPE(pipe, PIPEA_PP_OFF_DELAYS, PIPEB_PP_OFF_DELAYS)
+#define VLV_PIPE_PP_DIVISOR(pipe) \
+		_PIPE(pipe, PIPEA_PP_DIVISOR, PIPEB_PP_DIVISOR)
+
 #define PCH_PP_STATUS		0xc7200
 #define PCH_PP_CONTROL		0xc7204
 #define  PANEL_UNLOCK_REGS	(0xabcd << 16)
@@ -4149,8 +4262,12 @@
 #define  FORCEWAKE				0xA18C
 #define  FORCEWAKE_VLV				0x1300b0
 #define  FORCEWAKE_ACK_VLV			0x1300b4
+#define  FORCEWAKE_MEDIA_VLV			0x1300b8
+#define  FORCEWAKE_ACK_MEDIA_VLV		0x1300bc
 #define  FORCEWAKE_ACK_HSW			0x130044
 #define  FORCEWAKE_ACK				0x130090
+#define  VLV_GTLC_WAKE_CTRL			0x130090
+#define  VLV_GTLC_PW_STATUS			0x130094
 #define  FORCEWAKE_MT				0xa188 /* multi-threaded */
 #define   FORCEWAKE_KERNEL			0x1
 #define   FORCEWAKE_USER			0x2
@@ -4184,6 +4301,7 @@
 #define GEN6_RPNSWREQ				0xA008
 #define   GEN6_TURBO_DISABLE			(1<<31)
 #define   GEN6_FREQUENCY(x)			((x)<<25)
+#define   HSW_FREQUENCY(x)			((x)<<24)
 #define   GEN6_OFFSET(x)			((x)<<19)
 #define   GEN6_AGGRESSIVE_TURBO			(0<<15)
 #define GEN6_RC_VIDEO_FREQ			0xA00C
@@ -4274,6 +4392,21 @@
 #define   GEN6_DECODE_RC6_VID(vids)		(((vids) * 5) + 245)
 #define GEN6_PCODE_DATA				0x138128
 #define   GEN6_PCODE_FREQ_IA_RATIO_SHIFT	8
+#define   GEN6_PCODE_FREQ_RING_RATIO_SHIFT	16
+
+#define VLV_IOSF_DOORBELL_REQ			0x182100
+#define   IOSF_DEVFN_SHIFT			24
+#define   IOSF_OPCODE_SHIFT			16
+#define   IOSF_PORT_SHIFT			8
+#define   IOSF_BYTE_ENABLES_SHIFT		4
+#define   IOSF_BAR_SHIFT			1
+#define   IOSF_SB_BUSY				(1<<0)
+#define   IOSF_PORT_PUNIT			0x4
+#define VLV_IOSF_DATA				0x182104
+#define VLV_IOSF_ADDR				0x182108
+
+#define PUNIT_OPCODE_REG_READ			6
+#define PUNIT_OPCODE_REG_WRITE			7
 
 #define GEN6_GT_CORE_STATUS		0x138060
 #define   GEN6_CORE_CPD_STATE_MASK	(7<<4)
diff --git a/drivers/gpu/drm/i915/i915_suspend.c b/drivers/gpu/drm/i915/i915_suspend.c
index 2135f21ea458..41f0fdecfbdc 100644
--- a/drivers/gpu/drm/i915/i915_suspend.c
+++ b/drivers/gpu/drm/i915/i915_suspend.c
@@ -209,7 +209,8 @@ static void i915_save_display(struct drm_device *dev)
 		dev_priv->regfile.saveBLC_PWM_CTL2 = I915_READ(BLC_PWM_PCH_CTL2);
 		dev_priv->regfile.saveBLC_CPU_PWM_CTL = I915_READ(BLC_PWM_CPU_CTL);
 		dev_priv->regfile.saveBLC_CPU_PWM_CTL2 = I915_READ(BLC_PWM_CPU_CTL2);
-		dev_priv->regfile.saveLVDS = I915_READ(PCH_LVDS);
+		if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev))
+			dev_priv->regfile.saveLVDS = I915_READ(PCH_LVDS);
 	} else {
 		dev_priv->regfile.savePP_CONTROL = I915_READ(PP_CONTROL);
 		dev_priv->regfile.savePFIT_PGM_RATIOS = I915_READ(PFIT_PGM_RATIOS);
@@ -255,6 +256,7 @@ static void i915_save_display(struct drm_device *dev)
 static void i915_restore_display(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 mask = 0xffffffff;
 
 	/* Display arbitration */
 	if (INTEL_INFO(dev)->gen <= 4)
@@ -267,10 +269,13 @@ static void i915_restore_display(struct drm_device *dev)
 	if (INTEL_INFO(dev)->gen >= 4 && !HAS_PCH_SPLIT(dev))
 		I915_WRITE(BLC_PWM_CTL2, dev_priv->regfile.saveBLC_PWM_CTL2);
 
-	if (HAS_PCH_SPLIT(dev)) {
-		I915_WRITE(PCH_LVDS, dev_priv->regfile.saveLVDS);
-	} else if (IS_MOBILE(dev) && !IS_I830(dev))
-		I915_WRITE(LVDS, dev_priv->regfile.saveLVDS);
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		mask = ~LVDS_PORT_EN;
+
+	if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev))
+		I915_WRITE(PCH_LVDS, dev_priv->regfile.saveLVDS & mask);
+	else if (INTEL_INFO(dev)->gen <= 4 && IS_MOBILE(dev) && !IS_I830(dev))
+		I915_WRITE(LVDS, dev_priv->regfile.saveLVDS & mask);
 
 	if (!IS_I830(dev) && !IS_845G(dev) && !HAS_PCH_SPLIT(dev))
 		I915_WRITE(PFIT_CONTROL, dev_priv->regfile.savePFIT_CONTROL);
diff --git a/drivers/gpu/drm/i915/i915_sysfs.c b/drivers/gpu/drm/i915/i915_sysfs.c
index 9462081b1e60..d5e1890678f9 100644
--- a/drivers/gpu/drm/i915/i915_sysfs.c
+++ b/drivers/gpu/drm/i915/i915_sysfs.c
@@ -49,7 +49,7 @@ static ssize_t
 show_rc6_mask(struct device *kdev, struct device_attribute *attr, char *buf)
 {
 	struct drm_minor *dminor = container_of(kdev, struct drm_minor, kdev);
-	return snprintf(buf, PAGE_SIZE, "%x", intel_enable_rc6(dminor->dev));
+	return snprintf(buf, PAGE_SIZE, "%x\n", intel_enable_rc6(dminor->dev));
 }
 
 static ssize_t
@@ -57,7 +57,7 @@ show_rc6_ms(struct device *kdev, struct device_attribute *attr, char *buf)
 {
 	struct drm_minor *dminor = container_of(kdev, struct drm_minor, kdev);
 	u32 rc6_residency = calc_residency(dminor->dev, GEN6_GT_GFX_RC6);
-	return snprintf(buf, PAGE_SIZE, "%u", rc6_residency);
+	return snprintf(buf, PAGE_SIZE, "%u\n", rc6_residency);
 }
 
 static ssize_t
@@ -65,7 +65,7 @@ show_rc6p_ms(struct device *kdev, struct device_attribute *attr, char *buf)
 {
 	struct drm_minor *dminor = container_of(kdev, struct drm_minor, kdev);
 	u32 rc6p_residency = calc_residency(dminor->dev, GEN6_GT_GFX_RC6p);
-	return snprintf(buf, PAGE_SIZE, "%u", rc6p_residency);
+	return snprintf(buf, PAGE_SIZE, "%u\n", rc6p_residency);
 }
 
 static ssize_t
@@ -73,7 +73,7 @@ show_rc6pp_ms(struct device *kdev, struct device_attribute *attr, char *buf)
 {
 	struct drm_minor *dminor = container_of(kdev, struct drm_minor, kdev);
 	u32 rc6pp_residency = calc_residency(dminor->dev, GEN6_GT_GFX_RC6pp);
-	return snprintf(buf, PAGE_SIZE, "%u", rc6pp_residency);
+	return snprintf(buf, PAGE_SIZE, "%u\n", rc6pp_residency);
 }
 
 static DEVICE_ATTR(rc6_enable, S_IRUGO, show_rc6_mask, NULL);
@@ -215,7 +215,7 @@ static ssize_t gt_cur_freq_mhz_show(struct device *kdev,
 	ret = dev_priv->rps.cur_delay * GT_FREQUENCY_MULTIPLIER;
 	mutex_unlock(&dev_priv->rps.hw_lock);
 
-	return snprintf(buf, PAGE_SIZE, "%d", ret);
+	return snprintf(buf, PAGE_SIZE, "%d\n", ret);
 }
 
 static ssize_t gt_max_freq_mhz_show(struct device *kdev, struct device_attribute *attr, char *buf)
@@ -229,7 +229,7 @@ static ssize_t gt_max_freq_mhz_show(struct device *kdev, struct device_attribute
 	ret = dev_priv->rps.max_delay * GT_FREQUENCY_MULTIPLIER;
 	mutex_unlock(&dev_priv->rps.hw_lock);
 
-	return snprintf(buf, PAGE_SIZE, "%d", ret);
+	return snprintf(buf, PAGE_SIZE, "%d\n", ret);
 }
 
 static ssize_t gt_max_freq_mhz_store(struct device *kdev,
@@ -239,7 +239,7 @@ static ssize_t gt_max_freq_mhz_store(struct device *kdev,
 	struct drm_minor *minor = container_of(kdev, struct drm_minor, kdev);
 	struct drm_device *dev = minor->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 val, rp_state_cap, hw_max, hw_min;
+	u32 val, rp_state_cap, hw_max, hw_min, non_oc_max;
 	ssize_t ret;
 
 	ret = kstrtou32(buf, 0, &val);
@@ -251,7 +251,8 @@ static ssize_t gt_max_freq_mhz_store(struct device *kdev,
 	mutex_lock(&dev_priv->rps.hw_lock);
 
 	rp_state_cap = I915_READ(GEN6_RP_STATE_CAP);
-	hw_max = (rp_state_cap & 0xff);
+	hw_max = dev_priv->rps.hw_max;
+	non_oc_max = (rp_state_cap & 0xff);
 	hw_min = ((rp_state_cap & 0xff0000) >> 16);
 
 	if (val < hw_min || val > hw_max || val < dev_priv->rps.min_delay) {
@@ -259,6 +260,10 @@ static ssize_t gt_max_freq_mhz_store(struct device *kdev,
 		return -EINVAL;
 	}
 
+	if (val > non_oc_max)
+		DRM_DEBUG("User requested overclocking to %d\n",
+			  val * GT_FREQUENCY_MULTIPLIER);
+
 	if (dev_priv->rps.cur_delay > val)
 		gen6_set_rps(dev_priv->dev, val);
 
@@ -280,7 +285,7 @@ static ssize_t gt_min_freq_mhz_show(struct device *kdev, struct device_attribute
 	ret = dev_priv->rps.min_delay * GT_FREQUENCY_MULTIPLIER;
 	mutex_unlock(&dev_priv->rps.hw_lock);
 
-	return snprintf(buf, PAGE_SIZE, "%d", ret);
+	return snprintf(buf, PAGE_SIZE, "%d\n", ret);
 }
 
 static ssize_t gt_min_freq_mhz_store(struct device *kdev,
@@ -302,7 +307,7 @@ static ssize_t gt_min_freq_mhz_store(struct device *kdev,
 	mutex_lock(&dev_priv->rps.hw_lock);
 
 	rp_state_cap = I915_READ(GEN6_RP_STATE_CAP);
-	hw_max = (rp_state_cap & 0xff);
+	hw_max = dev_priv->rps.hw_max;
 	hw_min = ((rp_state_cap & 0xff0000) >> 16);
 
 	if (val < hw_min || val > hw_max || val > dev_priv->rps.max_delay) {
@@ -355,7 +360,7 @@ static ssize_t gt_rp_mhz_show(struct device *kdev, struct device_attribute *attr
 	} else {
 		BUG();
 	}
-	return snprintf(buf, PAGE_SIZE, "%d", val);
+	return snprintf(buf, PAGE_SIZE, "%d\n", val);
 }
 
 static const struct attribute *gen6_attrs[] = {
diff --git a/drivers/gpu/drm/i915/intel_bios.c b/drivers/gpu/drm/i915/intel_bios.c
index 55ffba1f5818..95070b2124c6 100644
--- a/drivers/gpu/drm/i915/intel_bios.c
+++ b/drivers/gpu/drm/i915/intel_bios.c
@@ -351,12 +351,14 @@ parse_general_features(struct drm_i915_private *dev_priv,
 		dev_priv->lvds_ssc_freq =
 			intel_bios_ssc_frequency(dev, general->ssc_freq);
 		dev_priv->display_clock_mode = general->display_clock_mode;
-		DRM_DEBUG_KMS("BDB_GENERAL_FEATURES int_tv_support %d int_crt_support %d lvds_use_ssc %d lvds_ssc_freq %d display_clock_mode %d\n",
+		dev_priv->fdi_rx_polarity_inverted = general->fdi_rx_polarity_inverted;
+		DRM_DEBUG_KMS("BDB_GENERAL_FEATURES int_tv_support %d int_crt_support %d lvds_use_ssc %d lvds_ssc_freq %d display_clock_mode %d fdi_rx_polarity_inverted %d\n",
 			      dev_priv->int_tv_support,
 			      dev_priv->int_crt_support,
 			      dev_priv->lvds_use_ssc,
 			      dev_priv->lvds_ssc_freq,
-			      dev_priv->display_clock_mode);
+			      dev_priv->display_clock_mode,
+			      dev_priv->fdi_rx_polarity_inverted);
 	}
 }
 
@@ -692,6 +694,9 @@ intel_parse_bios(struct drm_device *dev)
 	struct bdb_header *bdb = NULL;
 	u8 __iomem *bios = NULL;
 
+	if (HAS_PCH_NOP(dev))
+		return -ENODEV;
+
 	init_vbt_defaults(dev_priv);
 
 	/* XXX Should this validation be moved to intel_opregion.c? */
diff --git a/drivers/gpu/drm/i915/intel_bios.h b/drivers/gpu/drm/i915/intel_bios.h
index 36e57f934373..e088d6f0956a 100644
--- a/drivers/gpu/drm/i915/intel_bios.h
+++ b/drivers/gpu/drm/i915/intel_bios.h
@@ -127,7 +127,9 @@ struct bdb_general_features {
         /* bits 3 */
 	u8 disable_smooth_vision:1;
 	u8 single_dvi:1;
-	u8 rsvd9:6; /* finish byte */
+	u8 rsvd9:1;
+	u8 fdi_rx_polarity_inverted:1;
+	u8 rsvd10:4; /* finish byte */
 
         /* bits 4 */
 	u8 legacy_monitor_detect;
diff --git a/drivers/gpu/drm/i915/intel_crt.c b/drivers/gpu/drm/i915/intel_crt.c
index 1ce45a0a2d3e..58b4a53715cd 100644
--- a/drivers/gpu/drm/i915/intel_crt.c
+++ b/drivers/gpu/drm/i915/intel_crt.c
@@ -199,10 +199,14 @@ static int intel_crt_mode_valid(struct drm_connector *connector,
 	return MODE_OK;
 }
 
-static bool intel_crt_mode_fixup(struct drm_encoder *encoder,
-				 const struct drm_display_mode *mode,
-				 struct drm_display_mode *adjusted_mode)
+static bool intel_crt_compute_config(struct intel_encoder *encoder,
+				     struct intel_crtc_config *pipe_config)
 {
+	struct drm_device *dev = encoder->base.dev;
+
+	if (HAS_PCH_SPLIT(dev))
+		pipe_config->has_pch_encoder = true;
+
 	return true;
 }
 
@@ -676,7 +680,6 @@ static void intel_crt_reset(struct drm_connector *connector)
  */
 
 static const struct drm_encoder_helper_funcs crt_encoder_funcs = {
-	.mode_fixup = intel_crt_mode_fixup,
 	.mode_set = intel_crt_mode_set,
 };
 
@@ -768,8 +771,11 @@ void intel_crt_init(struct drm_device *dev)
 	else
 		crt->adpa_reg = ADPA;
 
+	crt->base.compute_config = intel_crt_compute_config;
 	crt->base.disable = intel_disable_crt;
 	crt->base.enable = intel_enable_crt;
+	if (I915_HAS_HOTPLUG(dev))
+		crt->base.hpd_pin = HPD_CRT;
 	if (HAS_DDI(dev))
 		crt->base.get_hw_state = intel_ddi_get_hw_state;
 	else
@@ -781,18 +787,14 @@ void intel_crt_init(struct drm_device *dev)
 
 	drm_sysfs_connector_add(connector);
 
-	if (I915_HAS_HOTPLUG(dev))
-		connector->polled = DRM_CONNECTOR_POLL_HPD;
-	else
-		connector->polled = DRM_CONNECTOR_POLL_CONNECT;
+	if (!I915_HAS_HOTPLUG(dev))
+		intel_connector->polled = DRM_CONNECTOR_POLL_CONNECT;
 
 	/*
 	 * Configure the automatic hotplug detection stuff
 	 */
 	crt->force_hotplug_required = 0;
 
-	dev_priv->hotplug_supported_mask |= CRT_HOTPLUG_INT_STATUS;
-
 	/*
 	 * TODO: find a proper way to discover whether we need to set the the
 	 * polarity and link reversal bits or not, instead of relying on the
diff --git a/drivers/gpu/drm/i915/intel_ddi.c b/drivers/gpu/drm/i915/intel_ddi.c
index 8d0bac3c35d7..26a0a570f92e 100644
--- a/drivers/gpu/drm/i915/intel_ddi.c
+++ b/drivers/gpu/drm/i915/intel_ddi.c
@@ -898,6 +898,9 @@ bool intel_ddi_pll_mode_set(struct drm_crtc *crtc, int clock)
 			plls->spll_refcount++;
 			reg = SPLL_CTL;
 			intel_crtc->ddi_pll_sel = PORT_CLK_SEL_SPLL;
+		} else {
+			DRM_ERROR("SPLL already in use\n");
+			return false;
 		}
 
 		WARN(I915_READ(reg) & SPLL_PLL_ENABLE,
@@ -921,14 +924,14 @@ void intel_ddi_set_pipe_settings(struct drm_crtc *crtc)
 	struct drm_i915_private *dev_priv = crtc->dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_encoder *intel_encoder = intel_ddi_get_crtc_encoder(crtc);
-	enum transcoder cpu_transcoder = intel_crtc->cpu_transcoder;
+	enum transcoder cpu_transcoder = intel_crtc->config.cpu_transcoder;
 	int type = intel_encoder->type;
 	uint32_t temp;
 
 	if (type == INTEL_OUTPUT_DISPLAYPORT || type == INTEL_OUTPUT_EDP) {
 
 		temp = TRANS_MSA_SYNC_CLK;
-		switch (intel_crtc->bpp) {
+		switch (intel_crtc->config.pipe_bpp) {
 		case 18:
 			temp |= TRANS_MSA_6_BPC;
 			break;
@@ -942,22 +945,20 @@ void intel_ddi_set_pipe_settings(struct drm_crtc *crtc)
 			temp |= TRANS_MSA_12_BPC;
 			break;
 		default:
-			temp |= TRANS_MSA_8_BPC;
-			WARN(1, "%d bpp unsupported by DDI function\n",
-			     intel_crtc->bpp);
+			BUG();
 		}
 		I915_WRITE(TRANS_MSA_MISC(cpu_transcoder), temp);
 	}
 }
 
-void intel_ddi_enable_pipe_func(struct drm_crtc *crtc)
+void intel_ddi_enable_transcoder_func(struct drm_crtc *crtc)
 {
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_encoder *intel_encoder = intel_ddi_get_crtc_encoder(crtc);
 	struct drm_encoder *encoder = &intel_encoder->base;
 	struct drm_i915_private *dev_priv = crtc->dev->dev_private;
 	enum pipe pipe = intel_crtc->pipe;
-	enum transcoder cpu_transcoder = intel_crtc->cpu_transcoder;
+	enum transcoder cpu_transcoder = intel_crtc->config.cpu_transcoder;
 	enum port port = intel_ddi_get_encoder_port(intel_encoder);
 	int type = intel_encoder->type;
 	uint32_t temp;
@@ -966,7 +967,7 @@ void intel_ddi_enable_pipe_func(struct drm_crtc *crtc)
 	temp = TRANS_DDI_FUNC_ENABLE;
 	temp |= TRANS_DDI_SELECT_PORT(port);
 
-	switch (intel_crtc->bpp) {
+	switch (intel_crtc->config.pipe_bpp) {
 	case 18:
 		temp |= TRANS_DDI_BPC_6;
 		break;
@@ -980,8 +981,7 @@ void intel_ddi_enable_pipe_func(struct drm_crtc *crtc)
 		temp |= TRANS_DDI_BPC_12;
 		break;
 	default:
-		WARN(1, "%d bpp unsupported by transcoder DDI function\n",
-		     intel_crtc->bpp);
+		BUG();
 	}
 
 	if (crtc->mode.flags & DRM_MODE_FLAG_PVSYNC)
@@ -1150,14 +1150,14 @@ bool intel_ddi_get_hw_state(struct intel_encoder *encoder,
 
 	DRM_DEBUG_KMS("No pipe for ddi port %i found\n", port);
 
-	return true;
+	return false;
 }
 
 static uint32_t intel_ddi_get_crtc_pll(struct drm_i915_private *dev_priv,
 				       enum pipe pipe)
 {
 	uint32_t temp, ret;
-	enum port port;
+	enum port port = I915_MAX_PORTS;
 	enum transcoder cpu_transcoder = intel_pipe_to_cpu_transcoder(dev_priv,
 								      pipe);
 	int i;
@@ -1173,10 +1173,16 @@ static uint32_t intel_ddi_get_crtc_pll(struct drm_i915_private *dev_priv,
 				port = i;
 	}
 
-	ret = I915_READ(PORT_CLK_SEL(port));
-
-	DRM_DEBUG_KMS("Pipe %c connected to port %c using clock 0x%08x\n",
-		      pipe_name(pipe), port_name(port), ret);
+	if (port == I915_MAX_PORTS) {
+		WARN(1, "Pipe %c enabled on an unknown port\n",
+		     pipe_name(pipe));
+		ret = PORT_CLK_SEL_NONE;
+	} else {
+		ret = I915_READ(PORT_CLK_SEL(port));
+		DRM_DEBUG_KMS("Pipe %c connected to port %c using clock "
+			      "0x%08x\n", pipe_name(pipe), port_name(port),
+			      ret);
+	}
 
 	return ret;
 }
@@ -1217,7 +1223,7 @@ void intel_ddi_enable_pipe_clock(struct intel_crtc *intel_crtc)
 	struct drm_i915_private *dev_priv = crtc->dev->dev_private;
 	struct intel_encoder *intel_encoder = intel_ddi_get_crtc_encoder(crtc);
 	enum port port = intel_ddi_get_encoder_port(intel_encoder);
-	enum transcoder cpu_transcoder = intel_crtc->cpu_transcoder;
+	enum transcoder cpu_transcoder = intel_crtc->config.cpu_transcoder;
 
 	if (cpu_transcoder != TRANSCODER_EDP)
 		I915_WRITE(TRANS_CLK_SEL(cpu_transcoder),
@@ -1227,7 +1233,7 @@ void intel_ddi_enable_pipe_clock(struct intel_crtc *intel_crtc)
 void intel_ddi_disable_pipe_clock(struct intel_crtc *intel_crtc)
 {
 	struct drm_i915_private *dev_priv = intel_crtc->base.dev->dev_private;
-	enum transcoder cpu_transcoder = intel_crtc->cpu_transcoder;
+	enum transcoder cpu_transcoder = intel_crtc->config.cpu_transcoder;
 
 	if (cpu_transcoder != TRANSCODER_EDP)
 		I915_WRITE(TRANS_CLK_SEL(cpu_transcoder),
@@ -1341,15 +1347,15 @@ static void intel_disable_ddi(struct intel_encoder *intel_encoder)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	uint32_t tmp;
 
+	tmp = I915_READ(HSW_AUD_PIN_ELD_CP_VLD);
+	tmp &= ~((AUDIO_OUTPUT_ENABLE_A | AUDIO_ELD_VALID_A) << (pipe * 4));
+	I915_WRITE(HSW_AUD_PIN_ELD_CP_VLD, tmp);
+
 	if (type == INTEL_OUTPUT_EDP) {
 		struct intel_dp *intel_dp = enc_to_intel_dp(encoder);
 
 		ironlake_edp_backlight_off(intel_dp);
 	}
-
-	tmp = I915_READ(HSW_AUD_PIN_ELD_CP_VLD);
-	tmp &= ~((AUDIO_OUTPUT_ENABLE_A | AUDIO_ELD_VALID_A) << (pipe * 4));
-	I915_WRITE(HSW_AUD_PIN_ELD_CP_VLD, tmp);
 }
 
 int intel_ddi_get_cdclk_freq(struct drm_i915_private *dev_priv)
@@ -1467,19 +1473,17 @@ static void intel_ddi_destroy(struct drm_encoder *encoder)
 	intel_dp_encoder_destroy(encoder);
 }
 
-static bool intel_ddi_mode_fixup(struct drm_encoder *encoder,
-				 const struct drm_display_mode *mode,
-				 struct drm_display_mode *adjusted_mode)
+static bool intel_ddi_compute_config(struct intel_encoder *encoder,
+				     struct intel_crtc_config *pipe_config)
 {
-	struct intel_encoder *intel_encoder = to_intel_encoder(encoder);
-	int type = intel_encoder->type;
+	int type = encoder->type;
 
-	WARN(type == INTEL_OUTPUT_UNKNOWN, "mode_fixup() on unknown output!\n");
+	WARN(type == INTEL_OUTPUT_UNKNOWN, "compute_config() on unknown output!\n");
 
 	if (type == INTEL_OUTPUT_HDMI)
-		return intel_hdmi_mode_fixup(encoder, mode, adjusted_mode);
+		return intel_hdmi_compute_config(encoder, pipe_config);
 	else
-		return intel_dp_mode_fixup(encoder, mode, adjusted_mode);
+		return intel_dp_compute_config(encoder, pipe_config);
 }
 
 static const struct drm_encoder_funcs intel_ddi_funcs = {
@@ -1487,7 +1491,6 @@ static const struct drm_encoder_funcs intel_ddi_funcs = {
 };
 
 static const struct drm_encoder_helper_funcs intel_ddi_helper_funcs = {
-	.mode_fixup = intel_ddi_mode_fixup,
 	.mode_set = intel_ddi_mode_set,
 };
 
@@ -1527,6 +1530,7 @@ void intel_ddi_init(struct drm_device *dev, enum port port)
 			 DRM_MODE_ENCODER_TMDS);
 	drm_encoder_helper_add(encoder, &intel_ddi_helper_funcs);
 
+	intel_encoder->compute_config = intel_ddi_compute_config;
 	intel_encoder->enable = intel_enable_ddi;
 	intel_encoder->pre_enable = intel_ddi_pre_enable;
 	intel_encoder->disable = intel_disable_ddi;
@@ -1537,9 +1541,7 @@ void intel_ddi_init(struct drm_device *dev, enum port port)
 	intel_dig_port->port_reversal = I915_READ(DDI_BUF_CTL(port)) &
 					DDI_BUF_PORT_REVERSAL;
 	if (hdmi_connector)
-		intel_dig_port->hdmi.sdvox_reg = DDI_BUF_CTL(port);
-	else
-		intel_dig_port->hdmi.sdvox_reg = 0;
+		intel_dig_port->hdmi.hdmi_reg = DDI_BUF_CTL(port);
 	intel_dig_port->dp.output_reg = DDI_BUF_CTL(port);
 
 	intel_encoder->type = INTEL_OUTPUT_UNKNOWN;
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index b20d50192fcc..efe829919755 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -71,8 +71,24 @@ typedef struct intel_limit intel_limit_t;
 struct intel_limit {
 	intel_range_t   dot, vco, n, m, m1, m2, p, p1;
 	intel_p2_t	    p2;
-	bool (* find_pll)(const intel_limit_t *, struct drm_crtc *,
-			int, int, intel_clock_t *, intel_clock_t *);
+	/**
+	 * find_pll() - Find the best values for the PLL
+	 * @limit: limits for the PLL
+	 * @crtc: current CRTC
+	 * @target: target frequency in kHz
+	 * @refclk: reference clock frequency in kHz
+	 * @match_clock: if provided, @best_clock P divider must
+	 *               match the P divider from @match_clock
+	 *               used for LVDS downclocking
+	 * @best_clock: best PLL values found
+	 *
+	 * Returns true on success, false on failure.
+	 */
+	bool (*find_pll)(const intel_limit_t *limit,
+			 struct drm_crtc *crtc,
+			 int target, int refclk,
+			 intel_clock_t *match_clock,
+			 intel_clock_t *best_clock);
 };
 
 /* FDI */
@@ -471,7 +487,6 @@ static const intel_limit_t *intel_ironlake_limit(struct drm_crtc *crtc,
 
 	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
 		if (intel_is_dual_link_lvds(dev)) {
-			/* LVDS dual channel */
 			if (refclk == 100000)
 				limit = &intel_limits_ironlake_dual_lvds_100m;
 			else
@@ -498,10 +513,8 @@ static const intel_limit_t *intel_g4x_limit(struct drm_crtc *crtc)
 
 	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
 		if (intel_is_dual_link_lvds(dev))
-			/* LVDS with dual channel */
 			limit = &intel_limits_g4x_dual_channel_lvds;
 		else
-			/* LVDS with dual channel */
 			limit = &intel_limits_g4x_single_channel_lvds;
 	} else if (intel_pipe_has_type(crtc, INTEL_OUTPUT_HDMI) ||
 		   intel_pipe_has_type(crtc, INTEL_OUTPUT_ANALOG)) {
@@ -879,7 +892,7 @@ enum transcoder intel_pipe_to_cpu_transcoder(struct drm_i915_private *dev_priv,
 	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pipe];
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 
-	return intel_crtc->cpu_transcoder;
+	return intel_crtc->config.cpu_transcoder;
 }
 
 static void ironlake_wait_for_vblank(struct drm_device *dev, int pipe)
@@ -1214,8 +1227,8 @@ void assert_pipe(struct drm_i915_private *dev_priv,
 	if (pipe == PIPE_A && dev_priv->quirks & QUIRK_PIPEA_FORCE)
 		state = true;
 
-	if (IS_HASWELL(dev_priv->dev) && cpu_transcoder != TRANSCODER_EDP &&
-	    !(I915_READ(HSW_PWR_WELL_DRIVER) & HSW_PWR_WELL_ENABLE)) {
+	if (!intel_using_power_well(dev_priv->dev) &&
+	    cpu_transcoder != TRANSCODER_EDP) {
 		cur_state = false;
 	} else {
 		reg = PIPECONF(cpu_transcoder);
@@ -1254,7 +1267,7 @@ static void assert_planes_disabled(struct drm_i915_private *dev_priv,
 	int cur_pipe;
 
 	/* Planes are fixed to pipes on ILK+ */
-	if (HAS_PCH_SPLIT(dev_priv->dev)) {
+	if (HAS_PCH_SPLIT(dev_priv->dev) || IS_VALLEYVIEW(dev_priv->dev)) {
 		reg = DSPCNTR(pipe);
 		val = I915_READ(reg);
 		WARN((val & DISPLAY_PLANE_ENABLE),
@@ -1275,6 +1288,25 @@ static void assert_planes_disabled(struct drm_i915_private *dev_priv,
 	}
 }
 
+static void assert_sprites_disabled(struct drm_i915_private *dev_priv,
+				    enum pipe pipe)
+{
+	int reg, i;
+	u32 val;
+
+	if (!IS_VALLEYVIEW(dev_priv->dev))
+		return;
+
+	/* Need to check both planes against the pipe */
+	for (i = 0; i < dev_priv->num_plane; i++) {
+		reg = SPCNTR(pipe, i);
+		val = I915_READ(reg);
+		WARN((val & SP_ENABLE),
+		     "sprite %d assertion failure, should be off on pipe %c but is still active\n",
+		     pipe * 2 + i, pipe_name(pipe));
+	}
+}
+
 static void assert_pch_refclk_enabled(struct drm_i915_private *dev_priv)
 {
 	u32 val;
@@ -1327,14 +1359,14 @@ static bool dp_pipe_enabled(struct drm_i915_private *dev_priv,
 static bool hdmi_pipe_enabled(struct drm_i915_private *dev_priv,
 			      enum pipe pipe, u32 val)
 {
-	if ((val & PORT_ENABLE) == 0)
+	if ((val & SDVO_ENABLE) == 0)
 		return false;
 
 	if (HAS_PCH_CPT(dev_priv->dev)) {
-		if ((val & PORT_TRANS_SEL_MASK) != PORT_TRANS_SEL_CPT(pipe))
+		if ((val & SDVO_PIPE_SEL_MASK_CPT) != SDVO_PIPE_SEL_CPT(pipe))
 			return false;
 	} else {
-		if ((val & TRANSCODER_MASK) != TRANSCODER(pipe))
+		if ((val & SDVO_PIPE_SEL_MASK) != SDVO_PIPE_SEL(pipe))
 			return false;
 	}
 	return true;
@@ -1392,7 +1424,7 @@ static void assert_pch_hdmi_disabled(struct drm_i915_private *dev_priv,
 	     "PCH HDMI (0x%08x) enabled on transcoder %c, should be disabled\n",
 	     reg, pipe_name(pipe));
 
-	WARN(HAS_PCH_IBX(dev_priv->dev) && (val & PORT_ENABLE) == 0
+	WARN(HAS_PCH_IBX(dev_priv->dev) && (val & SDVO_ENABLE) == 0
 	     && (val & SDVO_PIPE_B_SELECT),
 	     "IBX PCH hdmi port still using transcoder B\n");
 }
@@ -1419,9 +1451,9 @@ static void assert_pch_ports_disabled(struct drm_i915_private *dev_priv,
 	     "PCH LVDS enabled on transcoder %c, should be disabled\n",
 	     pipe_name(pipe));
 
-	assert_pch_hdmi_disabled(dev_priv, pipe, HDMIB);
-	assert_pch_hdmi_disabled(dev_priv, pipe, HDMIC);
-	assert_pch_hdmi_disabled(dev_priv, pipe, HDMID);
+	assert_pch_hdmi_disabled(dev_priv, pipe, PCH_HDMIB);
+	assert_pch_hdmi_disabled(dev_priv, pipe, PCH_HDMIC);
+	assert_pch_hdmi_disabled(dev_priv, pipe, PCH_HDMID);
 }
 
 /**
@@ -1859,6 +1891,7 @@ static void intel_disable_pipe(struct drm_i915_private *dev_priv,
 	 * or we might hang the display.
 	 */
 	assert_planes_disabled(dev_priv, pipe);
+	assert_sprites_disabled(dev_priv, pipe);
 
 	/* Don't disable pipe A or pipe A PLLs if needed */
 	if (pipe == PIPE_A && (dev_priv->quirks & QUIRK_PIPEA_FORCE))
@@ -1937,6 +1970,15 @@ static void intel_disable_plane(struct drm_i915_private *dev_priv,
 	intel_wait_for_vblank(dev_priv->dev, pipe);
 }
 
+static bool need_vtd_wa(struct drm_device *dev)
+{
+#ifdef CONFIG_INTEL_IOMMU
+	if (INTEL_INFO(dev)->gen >= 6 && intel_iommu_gfx_mapped)
+		return true;
+#endif
+	return false;
+}
+
 int
 intel_pin_and_fence_fb_obj(struct drm_device *dev,
 			   struct drm_i915_gem_object *obj,
@@ -1960,13 +2002,23 @@ intel_pin_and_fence_fb_obj(struct drm_device *dev,
 		alignment = 0;
 		break;
 	case I915_TILING_Y:
-		/* FIXME: Is this true? */
-		DRM_ERROR("Y tiled not allowed for scan out buffers\n");
+		/* Despite that we check this in framebuffer_init userspace can
+		 * screw us over and change the tiling after the fact. Only
+		 * pinned buffers can't change their tiling. */
+		DRM_DEBUG_DRIVER("Y tiled not allowed for scan out buffers\n");
 		return -EINVAL;
 	default:
 		BUG();
 	}
 
+	/* Note that the w/a also requires 64 PTE of padding following the
+	 * bo. We currently fill all unused PTE with the shadow page and so
+	 * we should always have valid PTE following the scanout preventing
+	 * the VT-d warning.
+	 */
+	if (need_vtd_wa(dev) && alignment < 256 * 1024)
+		alignment = 256 * 1024;
+
 	dev_priv->mm.interruptible = false;
 	ret = i915_gem_object_pin_to_display_plane(obj, alignment, pipelined);
 	if (ret)
@@ -2083,8 +2135,7 @@ static int i9xx_update_plane(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 		dspcntr |= DISPPLANE_RGBX101010;
 		break;
 	default:
-		DRM_ERROR("Unknown pixel format 0x%08x\n", fb->pixel_format);
-		return -EINVAL;
+		BUG();
 	}
 
 	if (INTEL_INFO(dev)->gen >= 4) {
@@ -2177,8 +2228,7 @@ static int ironlake_update_plane(struct drm_crtc *crtc,
 		dspcntr |= DISPPLANE_RGBX101010;
 		break;
 	default:
-		DRM_ERROR("Unknown pixel format 0x%08x\n", fb->pixel_format);
-		return -EINVAL;
+		BUG();
 	}
 
 	if (obj->tiling_mode != I915_TILING_NONE)
@@ -2229,6 +2279,44 @@ intel_pipe_set_base_atomic(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 	return dev_priv->display.update_plane(crtc, fb, x, y);
 }
 
+void intel_display_handle_reset(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc;
+
+	/*
+	 * Flips in the rings have been nuked by the reset,
+	 * so complete all pending flips so that user space
+	 * will get its events and not get stuck.
+	 *
+	 * Also update the base address of all primary
+	 * planes to the the last fb to make sure we're
+	 * showing the correct fb after a reset.
+	 *
+	 * Need to make two loops over the crtcs so that we
+	 * don't try to grab a crtc mutex before the
+	 * pending_flip_queue really got woken up.
+	 */
+
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+		struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+		enum plane plane = intel_crtc->plane;
+
+		intel_prepare_page_flip(dev, plane);
+		intel_finish_page_flip_plane(dev, plane);
+	}
+
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+		struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+
+		mutex_lock(&crtc->mutex);
+		if (intel_crtc->active)
+			dev_priv->display.update_plane(crtc, crtc->fb,
+						       crtc->x, crtc->y);
+		mutex_unlock(&crtc->mutex);
+	}
+}
+
 static int
 intel_finish_fb(struct drm_framebuffer *old_fb)
 {
@@ -2295,10 +2383,10 @@ intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 		return 0;
 	}
 
-	if(intel_crtc->plane > dev_priv->num_pipe) {
+	if (intel_crtc->plane > INTEL_INFO(dev)->num_pipes) {
 		DRM_ERROR("no plane for crtc: plane %d, num_pipes %d\n",
 				intel_crtc->plane,
-				dev_priv->num_pipe);
+				INTEL_INFO(dev)->num_pipes);
 		return -EINVAL;
 	}
 
@@ -2312,9 +2400,6 @@ intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 		return ret;
 	}
 
-	if (crtc->fb)
-		intel_finish_fb(crtc->fb);
-
 	ret = dev_priv->display.update_plane(crtc, fb, x, y);
 	if (ret) {
 		intel_unpin_fb_obj(to_intel_framebuffer(fb)->obj);
@@ -2912,32 +2997,6 @@ static void intel_crtc_wait_for_pending_flips(struct drm_crtc *crtc)
 	mutex_unlock(&dev->struct_mutex);
 }
 
-static bool ironlake_crtc_driving_pch(struct drm_crtc *crtc)
-{
-	struct drm_device *dev = crtc->dev;
-	struct intel_encoder *intel_encoder;
-
-	/*
-	 * If there's a non-PCH eDP on this crtc, it must be DP_A, and that
-	 * must be driven by its own crtc; no sharing is possible.
-	 */
-	for_each_encoder_on_crtc(dev, crtc, intel_encoder) {
-		switch (intel_encoder->type) {
-		case INTEL_OUTPUT_EDP:
-			if (!intel_encoder_is_pch_edp(&intel_encoder->base))
-				return false;
-			continue;
-		}
-	}
-
-	return true;
-}
-
-static bool haswell_crtc_driving_pch(struct drm_crtc *crtc)
-{
-	return intel_pipe_has_type(crtc, INTEL_OUTPUT_ANALOG);
-}
-
 /* Program iCLKIP clock to the desired frequency */
 static void lpt_program_iclkip(struct drm_crtc *crtc)
 {
@@ -3144,7 +3203,7 @@ static void lpt_pch_enable(struct drm_crtc *crtc)
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	enum transcoder cpu_transcoder = intel_crtc->cpu_transcoder;
+	enum transcoder cpu_transcoder = intel_crtc->config.cpu_transcoder;
 
 	assert_transcoder_disabled(dev_priv, TRANSCODER_A);
 
@@ -3273,7 +3332,6 @@ static void ironlake_crtc_enable(struct drm_crtc *crtc)
 	int pipe = intel_crtc->pipe;
 	int plane = intel_crtc->plane;
 	u32 temp;
-	bool is_pch_port;
 
 	WARN_ON(!crtc->enabled);
 
@@ -3289,9 +3347,8 @@ static void ironlake_crtc_enable(struct drm_crtc *crtc)
 			I915_WRITE(PCH_LVDS, temp | LVDS_PORT_EN);
 	}
 
-	is_pch_port = ironlake_crtc_driving_pch(crtc);
 
-	if (is_pch_port) {
+	if (intel_crtc->config.has_pch_encoder) {
 		/* Note: FDI PLL enabling _must_ be done before we enable the
 		 * cpu pipes, hence this is separate from all the other fdi/pch
 		 * enabling. */
@@ -3328,10 +3385,11 @@ static void ironlake_crtc_enable(struct drm_crtc *crtc)
 	 */
 	intel_crtc_load_lut(crtc);
 
-	intel_enable_pipe(dev_priv, pipe, is_pch_port);
+	intel_enable_pipe(dev_priv, pipe,
+			  intel_crtc->config.has_pch_encoder);
 	intel_enable_plane(dev_priv, plane, pipe);
 
-	if (is_pch_port)
+	if (intel_crtc->config.has_pch_encoder)
 		ironlake_pch_enable(crtc);
 
 	mutex_lock(&dev->struct_mutex);
@@ -3365,7 +3423,6 @@ static void haswell_crtc_enable(struct drm_crtc *crtc)
 	struct intel_encoder *encoder;
 	int pipe = intel_crtc->pipe;
 	int plane = intel_crtc->plane;
-	bool is_pch_port;
 
 	WARN_ON(!crtc->enabled);
 
@@ -3375,9 +3432,7 @@ static void haswell_crtc_enable(struct drm_crtc *crtc)
 	intel_crtc->active = true;
 	intel_update_watermarks(dev);
 
-	is_pch_port = haswell_crtc_driving_pch(crtc);
-
-	if (is_pch_port)
+	if (intel_crtc->config.has_pch_encoder)
 		dev_priv->display.fdi_link_train(crtc);
 
 	for_each_encoder_on_crtc(dev, crtc, encoder)
@@ -3406,12 +3461,13 @@ static void haswell_crtc_enable(struct drm_crtc *crtc)
 	intel_crtc_load_lut(crtc);
 
 	intel_ddi_set_pipe_settings(crtc);
-	intel_ddi_enable_pipe_func(crtc);
+	intel_ddi_enable_transcoder_func(crtc);
 
-	intel_enable_pipe(dev_priv, pipe, is_pch_port);
+	intel_enable_pipe(dev_priv, pipe,
+			  intel_crtc->config.has_pch_encoder);
 	intel_enable_plane(dev_priv, plane, pipe);
 
-	if (is_pch_port)
+	if (intel_crtc->config.has_pch_encoder)
 		lpt_pch_enable(crtc);
 
 	mutex_lock(&dev->struct_mutex);
@@ -3522,14 +3578,11 @@ static void haswell_crtc_disable(struct drm_crtc *crtc)
 	struct intel_encoder *encoder;
 	int pipe = intel_crtc->pipe;
 	int plane = intel_crtc->plane;
-	enum transcoder cpu_transcoder = intel_crtc->cpu_transcoder;
-	bool is_pch_port;
+	enum transcoder cpu_transcoder = intel_crtc->config.cpu_transcoder;
 
 	if (!intel_crtc->active)
 		return;
 
-	is_pch_port = haswell_crtc_driving_pch(crtc);
-
 	for_each_encoder_on_crtc(dev, crtc, encoder)
 		encoder->disable(encoder);
 
@@ -3546,9 +3599,13 @@ static void haswell_crtc_disable(struct drm_crtc *crtc)
 
 	intel_ddi_disable_transcoder_func(dev_priv, cpu_transcoder);
 
-	/* Disable PF */
-	I915_WRITE(PF_CTL(pipe), 0);
-	I915_WRITE(PF_WIN_SZ(pipe), 0);
+	/* XXX: Once we have proper panel fitter state tracking implemented with
+	 * hardware state read/check support we should switch to only disable
+	 * the panel fitter when we know it's used. */
+	if (intel_using_power_well(dev)) {
+		I915_WRITE(PF_CTL(pipe), 0);
+		I915_WRITE(PF_WIN_SZ(pipe), 0);
+	}
 
 	intel_ddi_disable_pipe_clock(intel_crtc);
 
@@ -3556,7 +3613,7 @@ static void haswell_crtc_disable(struct drm_crtc *crtc)
 		if (encoder->post_disable)
 			encoder->post_disable(encoder);
 
-	if (is_pch_port) {
+	if (intel_crtc->config.has_pch_encoder) {
 		lpt_disable_pch_transcoder(dev_priv);
 		intel_ddi_fdi_disable(crtc);
 	}
@@ -3581,7 +3638,7 @@ static void haswell_crtc_off(struct drm_crtc *crtc)
 
 	/* Stop saying we're using TRANSCODER_EDP because some other CRTC might
 	 * start using it. */
-	intel_crtc->cpu_transcoder = (enum transcoder) intel_crtc->pipe;
+	intel_crtc->config.cpu_transcoder = (enum transcoder) intel_crtc->pipe;
 
 	intel_ddi_put_crtc_pll(crtc);
 }
@@ -3667,6 +3724,26 @@ static void i9xx_crtc_enable(struct drm_crtc *crtc)
 		encoder->enable(encoder);
 }
 
+static void i9xx_pfit_disable(struct intel_crtc *crtc)
+{
+	struct drm_device *dev = crtc->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	enum pipe pipe;
+	uint32_t pctl = I915_READ(PFIT_CONTROL);
+
+	assert_pipe_disabled(dev_priv, crtc->pipe);
+
+	if (INTEL_INFO(dev)->gen >= 4)
+		pipe = (pctl & PFIT_PIPE_MASK) >> PFIT_PIPE_SHIFT;
+	else
+		pipe = PIPE_B;
+
+	if (pipe == crtc->pipe) {
+		DRM_DEBUG_DRIVER("disabling pfit, current: 0x%08x\n", pctl);
+		I915_WRITE(PFIT_CONTROL, 0);
+	}
+}
+
 static void i9xx_crtc_disable(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
@@ -3675,8 +3752,6 @@ static void i9xx_crtc_disable(struct drm_crtc *crtc)
 	struct intel_encoder *encoder;
 	int pipe = intel_crtc->pipe;
 	int plane = intel_crtc->plane;
-	u32 pctl;
-
 
 	if (!intel_crtc->active)
 		return;
@@ -3696,11 +3771,7 @@ static void i9xx_crtc_disable(struct drm_crtc *crtc)
 	intel_disable_plane(dev_priv, plane, pipe);
 	intel_disable_pipe(dev_priv, pipe);
 
-	/* Disable pannel fitter if it is on this pipe. */
-	pctl = I915_READ(PFIT_CONTROL);
-	if ((pctl & PFIT_ENABLE) &&
-	    ((pctl & PFIT_PIPE_MASK) >> PFIT_PIPE_SHIFT) == pipe)
-		I915_WRITE(PFIT_CONTROL, 0);
+	i9xx_pfit_disable(intel_crtc);
 
 	intel_disable_pll(dev_priv, pipe);
 
@@ -3906,22 +3977,23 @@ bool intel_connector_get_hw_state(struct intel_connector *connector)
 	return encoder->get_hw_state(encoder, &pipe);
 }
 
-static bool intel_crtc_mode_fixup(struct drm_crtc *crtc,
-				  const struct drm_display_mode *mode,
-				  struct drm_display_mode *adjusted_mode)
+static bool intel_crtc_compute_config(struct drm_crtc *crtc,
+				      struct intel_crtc_config *pipe_config)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_display_mode *adjusted_mode = &pipe_config->adjusted_mode;
 
 	if (HAS_PCH_SPLIT(dev)) {
 		/* FDI link clock is fixed at 2.7G */
-		if (mode->clock * 3 > IRONLAKE_FDI_FREQ * 4)
+		if (pipe_config->requested_mode.clock * 3
+		    > IRONLAKE_FDI_FREQ * 4)
 			return false;
 	}
 
 	/* All interlaced capable intel hw wants timings in frames. Note though
 	 * that intel_lvds_mode_fixup does some funny tricks with the crtc
 	 * timings, so we need to be careful not to clobber these.*/
-	if (!(adjusted_mode->private_flags & INTEL_MODE_CRTC_TIMINGS_SET))
+	if (!pipe_config->timings_set)
 		drm_mode_set_crtcinfo(adjusted_mode, 0);
 
 	/* WaPruneModeWithIncorrectHsyncOffset: Cantiga+ cannot handle modes
@@ -3931,6 +4003,14 @@ static bool intel_crtc_mode_fixup(struct drm_crtc *crtc,
 		adjusted_mode->hsync_start == adjusted_mode->hdisplay)
 		return false;
 
+	if ((IS_G4X(dev) || IS_VALLEYVIEW(dev)) && pipe_config->pipe_bpp > 10*3) {
+		pipe_config->pipe_bpp = 10*3; /* 12bpc is gen5+ */
+	} else if (INTEL_INFO(dev)->gen <= 4 && pipe_config->pipe_bpp > 8*3) {
+		/* only a 8bpc pipe, with 6bpc dither through the panel fitter
+		 * for lvds. */
+		pipe_config->pipe_bpp = 8*3;
+	}
+
 	return true;
 }
 
@@ -4004,26 +4084,36 @@ static int i830_get_display_clock_speed(struct drm_device *dev)
 }
 
 static void
-intel_reduce_ratio(uint32_t *num, uint32_t *den)
+intel_reduce_m_n_ratio(uint32_t *num, uint32_t *den)
 {
-	while (*num > 0xffffff || *den > 0xffffff) {
+	while (*num > DATA_LINK_M_N_MASK ||
+	       *den > DATA_LINK_M_N_MASK) {
 		*num >>= 1;
 		*den >>= 1;
 	}
 }
 
+static void compute_m_n(unsigned int m, unsigned int n,
+			uint32_t *ret_m, uint32_t *ret_n)
+{
+	*ret_n = min_t(unsigned int, roundup_pow_of_two(n), DATA_LINK_N_MAX);
+	*ret_m = div_u64((uint64_t) m * *ret_n, n);
+	intel_reduce_m_n_ratio(ret_m, ret_n);
+}
+
 void
 intel_link_compute_m_n(int bits_per_pixel, int nlanes,
 		       int pixel_clock, int link_clock,
 		       struct intel_link_m_n *m_n)
 {
 	m_n->tu = 64;
-	m_n->gmch_m = bits_per_pixel * pixel_clock;
-	m_n->gmch_n = link_clock * nlanes * 8;
-	intel_reduce_ratio(&m_n->gmch_m, &m_n->gmch_n);
-	m_n->link_m = pixel_clock;
-	m_n->link_n = link_clock;
-	intel_reduce_ratio(&m_n->link_m, &m_n->link_n);
+
+	compute_m_n(bits_per_pixel * pixel_clock,
+		    link_clock * nlanes * 8,
+		    &m_n->gmch_m, &m_n->gmch_n);
+
+	compute_m_n(pixel_clock, link_clock,
+		    &m_n->link_m, &m_n->link_n);
 }
 
 static inline bool intel_panel_use_ssc(struct drm_i915_private *dev_priv)
@@ -4034,142 +4124,6 @@ static inline bool intel_panel_use_ssc(struct drm_i915_private *dev_priv)
 		&& !(dev_priv->quirks & QUIRK_LVDS_SSC_DISABLE);
 }
 
-/**
- * intel_choose_pipe_bpp_dither - figure out what color depth the pipe should send
- * @crtc: CRTC structure
- * @mode: requested mode
- *
- * A pipe may be connected to one or more outputs.  Based on the depth of the
- * attached framebuffer, choose a good color depth to use on the pipe.
- *
- * If possible, match the pipe depth to the fb depth.  In some cases, this
- * isn't ideal, because the connected output supports a lesser or restricted
- * set of depths.  Resolve that here:
- *    LVDS typically supports only 6bpc, so clamp down in that case
- *    HDMI supports only 8bpc or 12bpc, so clamp to 8bpc with dither for 10bpc
- *    Displays may support a restricted set as well, check EDID and clamp as
- *      appropriate.
- *    DP may want to dither down to 6bpc to fit larger modes
- *
- * RETURNS:
- * Dithering requirement (i.e. false if display bpc and pipe bpc match,
- * true if they don't match).
- */
-static bool intel_choose_pipe_bpp_dither(struct drm_crtc *crtc,
-					 struct drm_framebuffer *fb,
-					 unsigned int *pipe_bpp,
-					 struct drm_display_mode *mode)
-{
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_connector *connector;
-	struct intel_encoder *intel_encoder;
-	unsigned int display_bpc = UINT_MAX, bpc;
-
-	/* Walk the encoders & connectors on this crtc, get min bpc */
-	for_each_encoder_on_crtc(dev, crtc, intel_encoder) {
-
-		if (intel_encoder->type == INTEL_OUTPUT_LVDS) {
-			unsigned int lvds_bpc;
-
-			if ((I915_READ(PCH_LVDS) & LVDS_A3_POWER_MASK) ==
-			    LVDS_A3_POWER_UP)
-				lvds_bpc = 8;
-			else
-				lvds_bpc = 6;
-
-			if (lvds_bpc < display_bpc) {
-				DRM_DEBUG_KMS("clamping display bpc (was %d) to LVDS (%d)\n", display_bpc, lvds_bpc);
-				display_bpc = lvds_bpc;
-			}
-			continue;
-		}
-
-		/* Not one of the known troublemakers, check the EDID */
-		list_for_each_entry(connector, &dev->mode_config.connector_list,
-				    head) {
-			if (connector->encoder != &intel_encoder->base)
-				continue;
-
-			/* Don't use an invalid EDID bpc value */
-			if (connector->display_info.bpc &&
-			    connector->display_info.bpc < display_bpc) {
-				DRM_DEBUG_KMS("clamping display bpc (was %d) to EDID reported max of %d\n", display_bpc, connector->display_info.bpc);
-				display_bpc = connector->display_info.bpc;
-			}
-		}
-
-		if (intel_encoder->type == INTEL_OUTPUT_EDP) {
-			/* Use VBT settings if we have an eDP panel */
-			unsigned int edp_bpc = dev_priv->edp.bpp / 3;
-
-			if (edp_bpc && edp_bpc < display_bpc) {
-				DRM_DEBUG_KMS("clamping display bpc (was %d) to eDP (%d)\n", display_bpc, edp_bpc);
-				display_bpc = edp_bpc;
-			}
-			continue;
-		}
-
-		/*
-		 * HDMI is either 12 or 8, so if the display lets 10bpc sneak
-		 * through, clamp it down.  (Note: >12bpc will be caught below.)
-		 */
-		if (intel_encoder->type == INTEL_OUTPUT_HDMI) {
-			if (display_bpc > 8 && display_bpc < 12) {
-				DRM_DEBUG_KMS("forcing bpc to 12 for HDMI\n");
-				display_bpc = 12;
-			} else {
-				DRM_DEBUG_KMS("forcing bpc to 8 for HDMI\n");
-				display_bpc = 8;
-			}
-		}
-	}
-
-	if (mode->private_flags & INTEL_MODE_DP_FORCE_6BPC) {
-		DRM_DEBUG_KMS("Dithering DP to 6bpc\n");
-		display_bpc = 6;
-	}
-
-	/*
-	 * We could just drive the pipe at the highest bpc all the time and
-	 * enable dithering as needed, but that costs bandwidth.  So choose
-	 * the minimum value that expresses the full color range of the fb but
-	 * also stays within the max display bpc discovered above.
-	 */
-
-	switch (fb->depth) {
-	case 8:
-		bpc = 8; /* since we go through a colormap */
-		break;
-	case 15:
-	case 16:
-		bpc = 6; /* min is 18bpp */
-		break;
-	case 24:
-		bpc = 8;
-		break;
-	case 30:
-		bpc = 10;
-		break;
-	case 48:
-		bpc = 12;
-		break;
-	default:
-		DRM_DEBUG("unsupported depth, assuming 24 bits\n");
-		bpc = min((unsigned int)8, display_bpc);
-		break;
-	}
-
-	display_bpc = min(display_bpc, bpc);
-
-	DRM_DEBUG_KMS("setting pipe bpc to %d (max display bpc %d)\n",
-		      bpc, display_bpc);
-
-	*pipe_bpp = display_bpc * 3;
-
-	return display_bpc != bpc;
-}
-
 static int vlv_get_refclk(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
@@ -4214,37 +4168,38 @@ static int i9xx_get_refclk(struct drm_crtc *crtc, int num_connectors)
 	return refclk;
 }
 
-static void i9xx_adjust_sdvo_tv_clock(struct drm_display_mode *adjusted_mode,
-				      intel_clock_t *clock)
+static void i9xx_adjust_sdvo_tv_clock(struct intel_crtc *crtc)
 {
+	unsigned dotclock = crtc->config.adjusted_mode.clock;
+	struct dpll *clock = &crtc->config.dpll;
+
 	/* SDVO TV has fixed PLL values depend on its clock range,
 	   this mirrors vbios setting. */
-	if (adjusted_mode->clock >= 100000
-	    && adjusted_mode->clock < 140500) {
+	if (dotclock >= 100000 && dotclock < 140500) {
 		clock->p1 = 2;
 		clock->p2 = 10;
 		clock->n = 3;
 		clock->m1 = 16;
 		clock->m2 = 8;
-	} else if (adjusted_mode->clock >= 140500
-		   && adjusted_mode->clock <= 200000) {
+	} else if (dotclock >= 140500 && dotclock <= 200000) {
 		clock->p1 = 1;
 		clock->p2 = 10;
 		clock->n = 6;
 		clock->m1 = 12;
 		clock->m2 = 8;
 	}
+
+	crtc->config.clock_set = true;
 }
 
-static void i9xx_update_pll_dividers(struct drm_crtc *crtc,
-				     intel_clock_t *clock,
+static void i9xx_update_pll_dividers(struct intel_crtc *crtc,
 				     intel_clock_t *reduced_clock)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int pipe = intel_crtc->pipe;
+	int pipe = crtc->pipe;
 	u32 fp, fp2 = 0;
+	struct dpll *clock = &crtc->config.dpll;
 
 	if (IS_PINEVIEW(dev)) {
 		fp = (1 << clock->n) << 16 | clock->m1 << 8 | clock->m2;
@@ -4260,26 +4215,29 @@ static void i9xx_update_pll_dividers(struct drm_crtc *crtc,
 
 	I915_WRITE(FP0(pipe), fp);
 
-	intel_crtc->lowfreq_avail = false;
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS) &&
+	crtc->lowfreq_avail = false;
+	if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_LVDS) &&
 	    reduced_clock && i915_powersave) {
 		I915_WRITE(FP1(pipe), fp2);
-		intel_crtc->lowfreq_avail = true;
+		crtc->lowfreq_avail = true;
 	} else {
 		I915_WRITE(FP1(pipe), fp);
 	}
 }
 
-static void vlv_update_pll(struct drm_crtc *crtc,
-			   struct drm_display_mode *mode,
-			   struct drm_display_mode *adjusted_mode,
-			   intel_clock_t *clock, intel_clock_t *reduced_clock,
-			   int num_connectors)
+static void intel_dp_set_m_n(struct intel_crtc *crtc)
 {
-	struct drm_device *dev = crtc->dev;
+	if (crtc->config.has_pch_encoder)
+		intel_pch_transcoder_set_m_n(crtc, &crtc->config.dp_m_n);
+	else
+		intel_cpu_transcoder_set_m_n(crtc, &crtc->config.dp_m_n);
+}
+
+static void vlv_update_pll(struct intel_crtc *crtc)
+{
+	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int pipe = intel_crtc->pipe;
+	int pipe = crtc->pipe;
 	u32 dpll, mdiv, pdiv;
 	u32 bestn, bestm1, bestm2, bestp1, bestp2;
 	bool is_sdvo;
@@ -4287,8 +4245,8 @@ static void vlv_update_pll(struct drm_crtc *crtc,
 
 	mutex_lock(&dev_priv->dpio_lock);
 
-	is_sdvo = intel_pipe_has_type(crtc, INTEL_OUTPUT_SDVO) ||
-		intel_pipe_has_type(crtc, INTEL_OUTPUT_HDMI);
+	is_sdvo = intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_SDVO) ||
+		intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_HDMI);
 
 	dpll = DPLL_VGA_MODE_DIS;
 	dpll |= DPLL_EXT_BUFFER_ENABLE_VLV;
@@ -4298,11 +4256,11 @@ static void vlv_update_pll(struct drm_crtc *crtc,
 	I915_WRITE(DPLL(pipe), dpll);
 	POSTING_READ(DPLL(pipe));
 
-	bestn = clock->n;
-	bestm1 = clock->m1;
-	bestm2 = clock->m2;
-	bestp1 = clock->p1;
-	bestp2 = clock->p2;
+	bestn = crtc->config.dpll.n;
+	bestm1 = crtc->config.dpll.m1;
+	bestm2 = crtc->config.dpll.m2;
+	bestp1 = crtc->config.dpll.p1;
+	bestp2 = crtc->config.dpll.p2;
 
 	/*
 	 * In Valleyview PLL and program lane counter registers are exposed
@@ -4334,8 +4292,8 @@ static void vlv_update_pll(struct drm_crtc *crtc,
 
 	intel_dpio_write(dev_priv, DPIO_FASTCLK_DISABLE, 0x620);
 
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT))
-		intel_dp_set_m_n(crtc, mode, adjusted_mode);
+	if (crtc->config.has_dp_encoder)
+		intel_dp_set_m_n(crtc);
 
 	I915_WRITE(DPLL(pipe), dpll);
 
@@ -4345,26 +4303,25 @@ static void vlv_update_pll(struct drm_crtc *crtc,
 
 	temp = 0;
 	if (is_sdvo) {
-		temp = intel_mode_get_pixel_multiplier(adjusted_mode);
-		if (temp > 1)
-			temp = (temp - 1) << DPLL_MD_UDI_MULTIPLIER_SHIFT;
-		else
-			temp = 0;
+		temp = 0;
+		if (crtc->config.pixel_multiplier > 1) {
+			temp = (crtc->config.pixel_multiplier - 1)
+				<< DPLL_MD_UDI_MULTIPLIER_SHIFT;
+		}
 	}
 	I915_WRITE(DPLL_MD(pipe), temp);
 	POSTING_READ(DPLL_MD(pipe));
 
 	/* Now program lane control registers */
-	if(intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT)
-			|| intel_pipe_has_type(crtc, INTEL_OUTPUT_HDMI))
-	{
+	if(intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_DISPLAYPORT)
+	   || intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_HDMI)) {
 		temp = 0x1000C4;
 		if(pipe == 1)
 			temp |= (1 << 21);
 		intel_dpio_write(dev_priv, DPIO_DATA_CHANNEL1, temp);
 	}
-	if(intel_pipe_has_type(crtc,INTEL_OUTPUT_EDP))
-	{
+
+	if(intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_EDP)) {
 		temp = 0x1000C4;
 		if(pipe == 1)
 			temp |= (1 << 21);
@@ -4374,40 +4331,39 @@ static void vlv_update_pll(struct drm_crtc *crtc,
 	mutex_unlock(&dev_priv->dpio_lock);
 }
 
-static void i9xx_update_pll(struct drm_crtc *crtc,
-			    struct drm_display_mode *mode,
-			    struct drm_display_mode *adjusted_mode,
-			    intel_clock_t *clock, intel_clock_t *reduced_clock,
+static void i9xx_update_pll(struct intel_crtc *crtc,
+			    intel_clock_t *reduced_clock,
 			    int num_connectors)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_encoder *encoder;
-	int pipe = intel_crtc->pipe;
+	int pipe = crtc->pipe;
 	u32 dpll;
 	bool is_sdvo;
+	struct dpll *clock = &crtc->config.dpll;
 
-	i9xx_update_pll_dividers(crtc, clock, reduced_clock);
+	i9xx_update_pll_dividers(crtc, reduced_clock);
 
-	is_sdvo = intel_pipe_has_type(crtc, INTEL_OUTPUT_SDVO) ||
-		intel_pipe_has_type(crtc, INTEL_OUTPUT_HDMI);
+	is_sdvo = intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_SDVO) ||
+		intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_HDMI);
 
 	dpll = DPLL_VGA_MODE_DIS;
 
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS))
+	if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_LVDS))
 		dpll |= DPLLB_MODE_LVDS;
 	else
 		dpll |= DPLLB_MODE_DAC_SERIAL;
+
 	if (is_sdvo) {
-		int pixel_multiplier = intel_mode_get_pixel_multiplier(adjusted_mode);
-		if (pixel_multiplier > 1) {
-			if (IS_I945G(dev) || IS_I945GM(dev) || IS_G33(dev))
-				dpll |= (pixel_multiplier - 1) << SDVO_MULTIPLIER_SHIFT_HIRES;
+		if ((crtc->config.pixel_multiplier > 1) &&
+		    (IS_I945G(dev) || IS_I945GM(dev) || IS_G33(dev))) {
+			dpll |= (crtc->config.pixel_multiplier - 1)
+				<< SDVO_MULTIPLIER_SHIFT_HIRES;
 		}
 		dpll |= DPLL_DVO_HIGH_SPEED;
 	}
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT))
+	if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_DISPLAYPORT))
 		dpll |= DPLL_DVO_HIGH_SPEED;
 
 	/* compute bitmask from p1 value */
@@ -4435,13 +4391,13 @@ static void i9xx_update_pll(struct drm_crtc *crtc,
 	if (INTEL_INFO(dev)->gen >= 4)
 		dpll |= (6 << PLL_LOAD_PULSE_PHASE_SHIFT);
 
-	if (is_sdvo && intel_pipe_has_type(crtc, INTEL_OUTPUT_TVOUT))
+	if (is_sdvo && intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_TVOUT))
 		dpll |= PLL_REF_INPUT_TVCLKINBC;
-	else if (intel_pipe_has_type(crtc, INTEL_OUTPUT_TVOUT))
+	else if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_TVOUT))
 		/* XXX: just matching BIOS for now */
 		/*	dpll |= PLL_REF_INPUT_TVCLKINBC; */
 		dpll |= 3;
-	else if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS) &&
+	else if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_LVDS) &&
 		 intel_panel_use_ssc(dev_priv) && num_connectors < 2)
 		dpll |= PLLB_REF_INPUT_SPREADSPECTRUMIN;
 	else
@@ -4452,12 +4408,12 @@ static void i9xx_update_pll(struct drm_crtc *crtc,
 	POSTING_READ(DPLL(pipe));
 	udelay(150);
 
-	for_each_encoder_on_crtc(dev, crtc, encoder)
+	for_each_encoder_on_crtc(dev, &crtc->base, encoder)
 		if (encoder->pre_pll_enable)
 			encoder->pre_pll_enable(encoder);
 
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT))
-		intel_dp_set_m_n(crtc, mode, adjusted_mode);
+	if (crtc->config.has_dp_encoder)
+		intel_dp_set_m_n(crtc);
 
 	I915_WRITE(DPLL(pipe), dpll);
 
@@ -4468,11 +4424,11 @@ static void i9xx_update_pll(struct drm_crtc *crtc,
 	if (INTEL_INFO(dev)->gen >= 4) {
 		u32 temp = 0;
 		if (is_sdvo) {
-			temp = intel_mode_get_pixel_multiplier(adjusted_mode);
-			if (temp > 1)
-				temp = (temp - 1) << DPLL_MD_UDI_MULTIPLIER_SHIFT;
-			else
-				temp = 0;
+			temp = 0;
+			if (crtc->config.pixel_multiplier > 1) {
+				temp = (crtc->config.pixel_multiplier - 1)
+					<< DPLL_MD_UDI_MULTIPLIER_SHIFT;
+			}
 		}
 		I915_WRITE(DPLL_MD(pipe), temp);
 	} else {
@@ -4485,23 +4441,23 @@ static void i9xx_update_pll(struct drm_crtc *crtc,
 	}
 }
 
-static void i8xx_update_pll(struct drm_crtc *crtc,
+static void i8xx_update_pll(struct intel_crtc *crtc,
 			    struct drm_display_mode *adjusted_mode,
-			    intel_clock_t *clock, intel_clock_t *reduced_clock,
+			    intel_clock_t *reduced_clock,
 			    int num_connectors)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_encoder *encoder;
-	int pipe = intel_crtc->pipe;
+	int pipe = crtc->pipe;
 	u32 dpll;
+	struct dpll *clock = &crtc->config.dpll;
 
-	i9xx_update_pll_dividers(crtc, clock, reduced_clock);
+	i9xx_update_pll_dividers(crtc, reduced_clock);
 
 	dpll = DPLL_VGA_MODE_DIS;
 
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
+	if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_LVDS)) {
 		dpll |= (1 << (clock->p1 - 1)) << DPLL_FPA01_P1_POST_DIV_SHIFT;
 	} else {
 		if (clock->p1 == 2)
@@ -4512,11 +4468,7 @@ static void i8xx_update_pll(struct drm_crtc *crtc,
 			dpll |= PLL_P2_DIVIDE_BY_4;
 	}
 
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_TVOUT))
-		/* XXX: just matching BIOS for now */
-		/*	dpll |= PLL_REF_INPUT_TVCLKINBC; */
-		dpll |= 3;
-	else if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS) &&
+	if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_LVDS) &&
 		 intel_panel_use_ssc(dev_priv) && num_connectors < 2)
 		dpll |= PLLB_REF_INPUT_SPREADSPECTRUMIN;
 	else
@@ -4527,7 +4479,7 @@ static void i8xx_update_pll(struct drm_crtc *crtc,
 	POSTING_READ(DPLL(pipe));
 	udelay(150);
 
-	for_each_encoder_on_crtc(dev, crtc, encoder)
+	for_each_encoder_on_crtc(dev, &crtc->base, encoder)
 		if (encoder->pre_pll_enable)
 			encoder->pre_pll_enable(encoder);
 
@@ -4552,7 +4504,7 @@ static void intel_set_pipe_timings(struct intel_crtc *intel_crtc,
 	struct drm_device *dev = intel_crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	enum pipe pipe = intel_crtc->pipe;
-	enum transcoder cpu_transcoder = intel_crtc->cpu_transcoder;
+	enum transcoder cpu_transcoder = intel_crtc->config.cpu_transcoder;
 	uint32_t vsyncshift;
 
 	if (!IS_GEN2(dev) && adjusted_mode->flags & DRM_MODE_FLAG_INTERLACE) {
@@ -4603,22 +4555,92 @@ static void intel_set_pipe_timings(struct intel_crtc *intel_crtc,
 		   ((mode->hdisplay - 1) << 16) | (mode->vdisplay - 1));
 }
 
+static void i9xx_set_pipeconf(struct intel_crtc *intel_crtc)
+{
+	struct drm_device *dev = intel_crtc->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t pipeconf;
+
+	pipeconf = I915_READ(PIPECONF(intel_crtc->pipe));
+
+	if (intel_crtc->pipe == 0 && INTEL_INFO(dev)->gen < 4) {
+		/* Enable pixel doubling when the dot clock is > 90% of the (display)
+		 * core speed.
+		 *
+		 * XXX: No double-wide on 915GM pipe B. Is that the only reason for the
+		 * pipe == 0 check?
+		 */
+		if (intel_crtc->config.requested_mode.clock >
+		    dev_priv->display.get_display_clock_speed(dev) * 9 / 10)
+			pipeconf |= PIPECONF_DOUBLE_WIDE;
+		else
+			pipeconf &= ~PIPECONF_DOUBLE_WIDE;
+	}
+
+	/* default to 8bpc */
+	pipeconf &= ~(PIPECONF_BPC_MASK | PIPECONF_DITHER_EN);
+	if (intel_crtc->config.has_dp_encoder) {
+		if (intel_crtc->config.dither) {
+			pipeconf |= PIPECONF_6BPC |
+				    PIPECONF_DITHER_EN |
+				    PIPECONF_DITHER_TYPE_SP;
+		}
+	}
+
+	if (IS_VALLEYVIEW(dev) && intel_pipe_has_type(&intel_crtc->base,
+						      INTEL_OUTPUT_EDP)) {
+		if (intel_crtc->config.dither) {
+			pipeconf |= PIPECONF_6BPC |
+					PIPECONF_ENABLE |
+					I965_PIPECONF_ACTIVE;
+		}
+	}
+
+	if (HAS_PIPE_CXSR(dev)) {
+		if (intel_crtc->lowfreq_avail) {
+			DRM_DEBUG_KMS("enabling CxSR downclocking\n");
+			pipeconf |= PIPECONF_CXSR_DOWNCLOCK;
+		} else {
+			DRM_DEBUG_KMS("disabling CxSR downclocking\n");
+			pipeconf &= ~PIPECONF_CXSR_DOWNCLOCK;
+		}
+	}
+
+	pipeconf &= ~PIPECONF_INTERLACE_MASK;
+	if (!IS_GEN2(dev) &&
+	    intel_crtc->config.adjusted_mode.flags & DRM_MODE_FLAG_INTERLACE)
+		pipeconf |= PIPECONF_INTERLACE_W_FIELD_INDICATION;
+	else
+		pipeconf |= PIPECONF_PROGRESSIVE;
+
+	if (IS_VALLEYVIEW(dev)) {
+		if (intel_crtc->config.limited_color_range)
+			pipeconf |= PIPECONF_COLOR_RANGE_SELECT;
+		else
+			pipeconf &= ~PIPECONF_COLOR_RANGE_SELECT;
+	}
+
+	I915_WRITE(PIPECONF(intel_crtc->pipe), pipeconf);
+	POSTING_READ(PIPECONF(intel_crtc->pipe));
+}
+
 static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
-			      struct drm_display_mode *mode,
-			      struct drm_display_mode *adjusted_mode,
 			      int x, int y,
 			      struct drm_framebuffer *fb)
 {
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct drm_display_mode *adjusted_mode =
+		&intel_crtc->config.adjusted_mode;
+	struct drm_display_mode *mode = &intel_crtc->config.requested_mode;
 	int pipe = intel_crtc->pipe;
 	int plane = intel_crtc->plane;
 	int refclk, num_connectors = 0;
 	intel_clock_t clock, reduced_clock;
-	u32 dspcntr, pipeconf;
+	u32 dspcntr;
 	bool ok, has_reduced_clock = false, is_sdvo = false;
-	bool is_lvds = false, is_tv = false, is_dp = false;
+	bool is_lvds = false, is_tv = false;
 	struct intel_encoder *encoder;
 	const intel_limit_t *limit;
 	int ret;
@@ -4637,9 +4659,6 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 		case INTEL_OUTPUT_TVOUT:
 			is_tv = true;
 			break;
-		case INTEL_OUTPUT_DISPLAYPORT:
-			is_dp = true;
-			break;
 		}
 
 		num_connectors++;
@@ -4676,86 +4695,42 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 						    &clock,
 						    &reduced_clock);
 	}
+	/* Compat-code for transition, will disappear. */
+	if (!intel_crtc->config.clock_set) {
+		intel_crtc->config.dpll.n = clock.n;
+		intel_crtc->config.dpll.m1 = clock.m1;
+		intel_crtc->config.dpll.m2 = clock.m2;
+		intel_crtc->config.dpll.p1 = clock.p1;
+		intel_crtc->config.dpll.p2 = clock.p2;
+	}
 
 	if (is_sdvo && is_tv)
-		i9xx_adjust_sdvo_tv_clock(adjusted_mode, &clock);
+		i9xx_adjust_sdvo_tv_clock(intel_crtc);
 
 	if (IS_GEN2(dev))
-		i8xx_update_pll(crtc, adjusted_mode, &clock,
+		i8xx_update_pll(intel_crtc, adjusted_mode,
 				has_reduced_clock ? &reduced_clock : NULL,
 				num_connectors);
 	else if (IS_VALLEYVIEW(dev))
-		vlv_update_pll(crtc, mode, adjusted_mode, &clock,
-				has_reduced_clock ? &reduced_clock : NULL,
-				num_connectors);
+		vlv_update_pll(intel_crtc);
 	else
-		i9xx_update_pll(crtc, mode, adjusted_mode, &clock,
+		i9xx_update_pll(intel_crtc,
 				has_reduced_clock ? &reduced_clock : NULL,
 				num_connectors);
 
-	/* setup pipeconf */
-	pipeconf = I915_READ(PIPECONF(pipe));
-
 	/* Set up the display plane register */
 	dspcntr = DISPPLANE_GAMMA_ENABLE;
 
-	if (pipe == 0)
-		dspcntr &= ~DISPPLANE_SEL_PIPE_MASK;
-	else
-		dspcntr |= DISPPLANE_SEL_PIPE_B;
-
-	if (pipe == 0 && INTEL_INFO(dev)->gen < 4) {
-		/* Enable pixel doubling when the dot clock is > 90% of the (display)
-		 * core speed.
-		 *
-		 * XXX: No double-wide on 915GM pipe B. Is that the only reason for the
-		 * pipe == 0 check?
-		 */
-		if (mode->clock >
-		    dev_priv->display.get_display_clock_speed(dev) * 9 / 10)
-			pipeconf |= PIPECONF_DOUBLE_WIDE;
+	if (!IS_VALLEYVIEW(dev)) {
+		if (pipe == 0)
+			dspcntr &= ~DISPPLANE_SEL_PIPE_MASK;
 		else
-			pipeconf &= ~PIPECONF_DOUBLE_WIDE;
-	}
-
-	/* default to 8bpc */
-	pipeconf &= ~(PIPECONF_BPC_MASK | PIPECONF_DITHER_EN);
-	if (is_dp) {
-		if (adjusted_mode->private_flags & INTEL_MODE_DP_FORCE_6BPC) {
-			pipeconf |= PIPECONF_6BPC |
-				    PIPECONF_DITHER_EN |
-				    PIPECONF_DITHER_TYPE_SP;
-		}
-	}
-
-	if (IS_VALLEYVIEW(dev) && intel_pipe_has_type(crtc, INTEL_OUTPUT_EDP)) {
-		if (adjusted_mode->private_flags & INTEL_MODE_DP_FORCE_6BPC) {
-			pipeconf |= PIPECONF_6BPC |
-					PIPECONF_ENABLE |
-					I965_PIPECONF_ACTIVE;
-		}
+			dspcntr |= DISPPLANE_SEL_PIPE_B;
 	}
 
 	DRM_DEBUG_KMS("Mode for pipe %c:\n", pipe == 0 ? 'A' : 'B');
 	drm_mode_debug_printmodeline(mode);
 
-	if (HAS_PIPE_CXSR(dev)) {
-		if (intel_crtc->lowfreq_avail) {
-			DRM_DEBUG_KMS("enabling CxSR downclocking\n");
-			pipeconf |= PIPECONF_CXSR_DOWNCLOCK;
-		} else {
-			DRM_DEBUG_KMS("disabling CxSR downclocking\n");
-			pipeconf &= ~PIPECONF_CXSR_DOWNCLOCK;
-		}
-	}
-
-	pipeconf &= ~PIPECONF_INTERLACE_MASK;
-	if (!IS_GEN2(dev) &&
-	    adjusted_mode->flags & DRM_MODE_FLAG_INTERLACE)
-		pipeconf |= PIPECONF_INTERLACE_W_FIELD_INDICATION;
-	else
-		pipeconf |= PIPECONF_PROGRESSIVE;
-
 	intel_set_pipe_timings(intel_crtc, mode, adjusted_mode);
 
 	/* pipesrc and dspsize control the size that is scaled from,
@@ -4766,8 +4741,8 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 		   (mode->hdisplay - 1));
 	I915_WRITE(DSPPOS(plane), 0);
 
-	I915_WRITE(PIPECONF(pipe), pipeconf);
-	POSTING_READ(PIPECONF(pipe));
+	i9xx_set_pipeconf(intel_crtc);
+
 	intel_enable_pipe(dev_priv, pipe, false);
 
 	intel_wait_for_vblank(dev, pipe);
@@ -4782,12 +4757,26 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 	return ret;
 }
 
+static bool i9xx_get_pipe_config(struct intel_crtc *crtc,
+				 struct intel_crtc_config *pipe_config)
+{
+	struct drm_device *dev = crtc->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t tmp;
+
+	tmp = I915_READ(PIPECONF(crtc->pipe));
+	if (!(tmp & PIPECONF_ENABLE))
+		return false;
+
+	return true;
+}
+
 static void ironlake_init_pch_refclk(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_mode_config *mode_config = &dev->mode_config;
 	struct intel_encoder *encoder;
-	u32 temp;
+	u32 val, final;
 	bool has_lvds = false;
 	bool has_cpu_edp = false;
 	bool has_pch_edp = false;
@@ -4830,70 +4819,109 @@ static void ironlake_init_pch_refclk(struct drm_device *dev)
 	 * PCH B stepping, previous chipset stepping should be
 	 * ignoring this setting.
 	 */
-	temp = I915_READ(PCH_DREF_CONTROL);
+	val = I915_READ(PCH_DREF_CONTROL);
+
+	/* As we must carefully and slowly disable/enable each source in turn,
+	 * compute the final state we want first and check if we need to
+	 * make any changes at all.
+	 */
+	final = val;
+	final &= ~DREF_NONSPREAD_SOURCE_MASK;
+	if (has_ck505)
+		final |= DREF_NONSPREAD_CK505_ENABLE;
+	else
+		final |= DREF_NONSPREAD_SOURCE_ENABLE;
+
+	final &= ~DREF_SSC_SOURCE_MASK;
+	final &= ~DREF_CPU_SOURCE_OUTPUT_MASK;
+	final &= ~DREF_SSC1_ENABLE;
+
+	if (has_panel) {
+		final |= DREF_SSC_SOURCE_ENABLE;
+
+		if (intel_panel_use_ssc(dev_priv) && can_ssc)
+			final |= DREF_SSC1_ENABLE;
+
+		if (has_cpu_edp) {
+			if (intel_panel_use_ssc(dev_priv) && can_ssc)
+				final |= DREF_CPU_SOURCE_OUTPUT_DOWNSPREAD;
+			else
+				final |= DREF_CPU_SOURCE_OUTPUT_NONSPREAD;
+		} else
+			final |= DREF_CPU_SOURCE_OUTPUT_DISABLE;
+	} else {
+		final |= DREF_SSC_SOURCE_DISABLE;
+		final |= DREF_CPU_SOURCE_OUTPUT_DISABLE;
+	}
+
+	if (final == val)
+		return;
+
 	/* Always enable nonspread source */
-	temp &= ~DREF_NONSPREAD_SOURCE_MASK;
+	val &= ~DREF_NONSPREAD_SOURCE_MASK;
 
 	if (has_ck505)
-		temp |= DREF_NONSPREAD_CK505_ENABLE;
+		val |= DREF_NONSPREAD_CK505_ENABLE;
 	else
-		temp |= DREF_NONSPREAD_SOURCE_ENABLE;
+		val |= DREF_NONSPREAD_SOURCE_ENABLE;
 
 	if (has_panel) {
-		temp &= ~DREF_SSC_SOURCE_MASK;
-		temp |= DREF_SSC_SOURCE_ENABLE;
+		val &= ~DREF_SSC_SOURCE_MASK;
+		val |= DREF_SSC_SOURCE_ENABLE;
 
 		/* SSC must be turned on before enabling the CPU output  */
 		if (intel_panel_use_ssc(dev_priv) && can_ssc) {
 			DRM_DEBUG_KMS("Using SSC on panel\n");
-			temp |= DREF_SSC1_ENABLE;
+			val |= DREF_SSC1_ENABLE;
 		} else
-			temp &= ~DREF_SSC1_ENABLE;
+			val &= ~DREF_SSC1_ENABLE;
 
 		/* Get SSC going before enabling the outputs */
-		I915_WRITE(PCH_DREF_CONTROL, temp);
+		I915_WRITE(PCH_DREF_CONTROL, val);
 		POSTING_READ(PCH_DREF_CONTROL);
 		udelay(200);
 
-		temp &= ~DREF_CPU_SOURCE_OUTPUT_MASK;
+		val &= ~DREF_CPU_SOURCE_OUTPUT_MASK;
 
 		/* Enable CPU source on CPU attached eDP */
 		if (has_cpu_edp) {
 			if (intel_panel_use_ssc(dev_priv) && can_ssc) {
 				DRM_DEBUG_KMS("Using SSC on eDP\n");
-				temp |= DREF_CPU_SOURCE_OUTPUT_DOWNSPREAD;
+				val |= DREF_CPU_SOURCE_OUTPUT_DOWNSPREAD;
 			}
 			else
-				temp |= DREF_CPU_SOURCE_OUTPUT_NONSPREAD;
+				val |= DREF_CPU_SOURCE_OUTPUT_NONSPREAD;
 		} else
-			temp |= DREF_CPU_SOURCE_OUTPUT_DISABLE;
+			val |= DREF_CPU_SOURCE_OUTPUT_DISABLE;
 
-		I915_WRITE(PCH_DREF_CONTROL, temp);
+		I915_WRITE(PCH_DREF_CONTROL, val);
 		POSTING_READ(PCH_DREF_CONTROL);
 		udelay(200);
 	} else {
 		DRM_DEBUG_KMS("Disabling SSC entirely\n");
 
-		temp &= ~DREF_CPU_SOURCE_OUTPUT_MASK;
+		val &= ~DREF_CPU_SOURCE_OUTPUT_MASK;
 
 		/* Turn off CPU output */
-		temp |= DREF_CPU_SOURCE_OUTPUT_DISABLE;
+		val |= DREF_CPU_SOURCE_OUTPUT_DISABLE;
 
-		I915_WRITE(PCH_DREF_CONTROL, temp);
+		I915_WRITE(PCH_DREF_CONTROL, val);
 		POSTING_READ(PCH_DREF_CONTROL);
 		udelay(200);
 
 		/* Turn off the SSC source */
-		temp &= ~DREF_SSC_SOURCE_MASK;
-		temp |= DREF_SSC_SOURCE_DISABLE;
+		val &= ~DREF_SSC_SOURCE_MASK;
+		val |= DREF_SSC_SOURCE_DISABLE;
 
 		/* Turn off SSC1 */
-		temp &= ~ DREF_SSC1_ENABLE;
+		val &= ~DREF_SSC1_ENABLE;
 
-		I915_WRITE(PCH_DREF_CONTROL, temp);
+		I915_WRITE(PCH_DREF_CONTROL, val);
 		POSTING_READ(PCH_DREF_CONTROL);
 		udelay(200);
 	}
+
+	BUG_ON(val != final);
 }
 
 /* Sequence to enable CLKOUT_DP for FDI usage and configure PCH FDI I/O. */
@@ -4958,13 +4986,6 @@ static void lpt_init_pch_refclk(struct drm_device *dev)
 	tmp |= (0x12 << 24);
 	intel_sbi_write(dev_priv, 0x8008, tmp, SBI_MPHY);
 
-	if (!is_sdv) {
-		tmp = intel_sbi_read(dev_priv, 0x808C, SBI_MPHY);
-		tmp &= ~(0x3 << 6);
-		tmp |= (1 << 6) | (1 << 0);
-		intel_sbi_write(dev_priv, 0x808C, tmp, SBI_MPHY);
-	}
-
 	if (is_sdv) {
 		tmp = intel_sbi_read(dev_priv, 0x800C, SBI_MPHY);
 		tmp |= 0x7FFF;
@@ -5118,7 +5139,7 @@ static void ironlake_set_pipeconf(struct drm_crtc *crtc,
 	val = I915_READ(PIPECONF(pipe));
 
 	val &= ~PIPECONF_BPC_MASK;
-	switch (intel_crtc->bpp) {
+	switch (intel_crtc->config.pipe_bpp) {
 	case 18:
 		val |= PIPECONF_6BPC;
 		break;
@@ -5146,7 +5167,7 @@ static void ironlake_set_pipeconf(struct drm_crtc *crtc,
 	else
 		val |= PIPECONF_PROGRESSIVE;
 
-	if (adjusted_mode->private_flags & INTEL_MODE_LIMITED_COLOR_RANGE)
+	if (intel_crtc->config.limited_color_range)
 		val |= PIPECONF_COLOR_RANGE_SELECT;
 	else
 		val &= ~PIPECONF_COLOR_RANGE_SELECT;
@@ -5162,8 +5183,7 @@ static void ironlake_set_pipeconf(struct drm_crtc *crtc,
  * is supported, but eventually this should handle various
  * RGB<->YCbCr scenarios as well.
  */
-static void intel_set_pipe_csc(struct drm_crtc *crtc,
-			       const struct drm_display_mode *adjusted_mode)
+static void intel_set_pipe_csc(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -5178,7 +5198,7 @@ static void intel_set_pipe_csc(struct drm_crtc *crtc,
 	 * consideration.
 	 */
 
-	if (adjusted_mode->private_flags & INTEL_MODE_LIMITED_COLOR_RANGE)
+	if (intel_crtc->config.limited_color_range)
 		coeff = ((235 - 16) * (1 << 12) / 255) & 0xff8; /* 0.xxx... */
 
 	/*
@@ -5202,7 +5222,7 @@ static void intel_set_pipe_csc(struct drm_crtc *crtc,
 	if (INTEL_INFO(dev)->gen > 6) {
 		uint16_t postoff = 0;
 
-		if (adjusted_mode->private_flags & INTEL_MODE_LIMITED_COLOR_RANGE)
+		if (intel_crtc->config.limited_color_range)
 			postoff = (16 * (1 << 13) / 255) & 0x1fff;
 
 		I915_WRITE(PIPE_CSC_POSTOFF_HI(pipe), postoff);
@@ -5213,7 +5233,7 @@ static void intel_set_pipe_csc(struct drm_crtc *crtc,
 	} else {
 		uint32_t mode = CSC_MODE_YUV_TO_RGB;
 
-		if (adjusted_mode->private_flags & INTEL_MODE_LIMITED_COLOR_RANGE)
+		if (intel_crtc->config.limited_color_range)
 			mode |= CSC_BLACK_SCREEN_OFFSET;
 
 		I915_WRITE(PIPE_CSC_MODE(pipe), mode);
@@ -5226,7 +5246,7 @@ static void haswell_set_pipeconf(struct drm_crtc *crtc,
 {
 	struct drm_i915_private *dev_priv = crtc->dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	enum transcoder cpu_transcoder = intel_crtc->cpu_transcoder;
+	enum transcoder cpu_transcoder = intel_crtc->config.cpu_transcoder;
 	uint32_t val;
 
 	val = I915_READ(PIPECONF(cpu_transcoder));
@@ -5303,7 +5323,7 @@ static bool ironlake_compute_clocks(struct drm_crtc *crtc,
 	}
 
 	if (is_sdvo && is_tv)
-		i9xx_adjust_sdvo_tv_clock(adjusted_mode, clock);
+		i9xx_adjust_sdvo_tv_clock(to_intel_crtc(crtc));
 
 	return true;
 }
@@ -5344,7 +5364,7 @@ static bool ironlake_check_fdi_lanes(struct intel_crtc *intel_crtc)
 		return false;
 	}
 
-	if (dev_priv->num_pipe == 2)
+	if (INTEL_INFO(dev)->num_pipes == 2)
 		return true;
 
 	switch (intel_crtc->pipe) {
@@ -5401,87 +5421,87 @@ int ironlake_get_lanes_required(int target_clock, int link_bw, int bpp)
 	return bps / (link_bw * 8) + 1;
 }
 
-static void ironlake_set_m_n(struct drm_crtc *crtc,
-			     struct drm_display_mode *mode,
-			     struct drm_display_mode *adjusted_mode)
+void intel_pch_transcoder_set_m_n(struct intel_crtc *crtc,
+				  struct intel_link_m_n *m_n)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	enum transcoder cpu_transcoder = intel_crtc->cpu_transcoder;
-	struct intel_encoder *intel_encoder, *edp_encoder = NULL;
-	struct intel_link_m_n m_n = {0};
-	int target_clock, pixel_multiplier, lane, link_bw;
-	bool is_dp = false, is_cpu_edp = false;
+	int pipe = crtc->pipe;
 
-	for_each_encoder_on_crtc(dev, crtc, intel_encoder) {
-		switch (intel_encoder->type) {
-		case INTEL_OUTPUT_DISPLAYPORT:
-			is_dp = true;
-			break;
-		case INTEL_OUTPUT_EDP:
-			is_dp = true;
-			if (!intel_encoder_is_pch_edp(&intel_encoder->base))
-				is_cpu_edp = true;
-			edp_encoder = intel_encoder;
-			break;
-		}
-	}
+	I915_WRITE(TRANSDATA_M1(pipe), TU_SIZE(m_n->tu) | m_n->gmch_m);
+	I915_WRITE(TRANSDATA_N1(pipe), m_n->gmch_n);
+	I915_WRITE(TRANSDPLINK_M1(pipe), m_n->link_m);
+	I915_WRITE(TRANSDPLINK_N1(pipe), m_n->link_n);
+}
+
+void intel_cpu_transcoder_set_m_n(struct intel_crtc *crtc,
+				  struct intel_link_m_n *m_n)
+{
+	struct drm_device *dev = crtc->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int pipe = crtc->pipe;
+	enum transcoder transcoder = crtc->config.cpu_transcoder;
 
-	/* FDI link */
-	pixel_multiplier = intel_mode_get_pixel_multiplier(adjusted_mode);
-	lane = 0;
-	/* CPU eDP doesn't require FDI link, so just set DP M/N
-	   according to current link config */
-	if (is_cpu_edp) {
-		intel_edp_link_config(edp_encoder, &lane, &link_bw);
+	if (INTEL_INFO(dev)->gen >= 5) {
+		I915_WRITE(PIPE_DATA_M1(transcoder), TU_SIZE(m_n->tu) | m_n->gmch_m);
+		I915_WRITE(PIPE_DATA_N1(transcoder), m_n->gmch_n);
+		I915_WRITE(PIPE_LINK_M1(transcoder), m_n->link_m);
+		I915_WRITE(PIPE_LINK_N1(transcoder), m_n->link_n);
 	} else {
-		/* FDI is a binary signal running at ~2.7GHz, encoding
-		 * each output octet as 10 bits. The actual frequency
-		 * is stored as a divider into a 100MHz clock, and the
-		 * mode pixel clock is stored in units of 1KHz.
-		 * Hence the bw of each lane in terms of the mode signal
-		 * is:
-		 */
-		link_bw = intel_fdi_link_freq(dev) * MHz(100)/KHz(1)/10;
+		I915_WRITE(PIPE_GMCH_DATA_M(pipe), TU_SIZE(m_n->tu) | m_n->gmch_m);
+		I915_WRITE(PIPE_GMCH_DATA_N(pipe), m_n->gmch_n);
+		I915_WRITE(PIPE_DP_LINK_M(pipe), m_n->link_m);
+		I915_WRITE(PIPE_DP_LINK_N(pipe), m_n->link_n);
 	}
+}
+
+static void ironlake_fdi_set_m_n(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct drm_display_mode *adjusted_mode =
+		&intel_crtc->config.adjusted_mode;
+	struct intel_link_m_n m_n = {0};
+	int target_clock, lane, link_bw;
+
+	/* FDI is a binary signal running at ~2.7GHz, encoding
+	 * each output octet as 10 bits. The actual frequency
+	 * is stored as a divider into a 100MHz clock, and the
+	 * mode pixel clock is stored in units of 1KHz.
+	 * Hence the bw of each lane in terms of the mode signal
+	 * is:
+	 */
+	link_bw = intel_fdi_link_freq(dev) * MHz(100)/KHz(1)/10;
 
-	/* [e]DP over FDI requires target mode clock instead of link clock. */
-	if (edp_encoder)
-		target_clock = intel_edp_target_clock(edp_encoder, mode);
-	else if (is_dp)
-		target_clock = mode->clock;
+	if (intel_crtc->config.pixel_target_clock)
+		target_clock = intel_crtc->config.pixel_target_clock;
 	else
 		target_clock = adjusted_mode->clock;
 
-	if (!lane)
-		lane = ironlake_get_lanes_required(target_clock, link_bw,
-						   intel_crtc->bpp);
+	lane = ironlake_get_lanes_required(target_clock, link_bw,
+					   intel_crtc->config.pipe_bpp);
 
 	intel_crtc->fdi_lanes = lane;
 
-	if (pixel_multiplier > 1)
-		link_bw *= pixel_multiplier;
-	intel_link_compute_m_n(intel_crtc->bpp, lane, target_clock, link_bw, &m_n);
+	if (intel_crtc->config.pixel_multiplier > 1)
+		link_bw *= intel_crtc->config.pixel_multiplier;
+	intel_link_compute_m_n(intel_crtc->config.pipe_bpp, lane, target_clock,
+			       link_bw, &m_n);
 
-	I915_WRITE(PIPE_DATA_M1(cpu_transcoder), TU_SIZE(m_n.tu) | m_n.gmch_m);
-	I915_WRITE(PIPE_DATA_N1(cpu_transcoder), m_n.gmch_n);
-	I915_WRITE(PIPE_LINK_M1(cpu_transcoder), m_n.link_m);
-	I915_WRITE(PIPE_LINK_N1(cpu_transcoder), m_n.link_n);
+	intel_cpu_transcoder_set_m_n(intel_crtc, &m_n);
 }
 
 static uint32_t ironlake_compute_dpll(struct intel_crtc *intel_crtc,
-				      struct drm_display_mode *adjusted_mode,
-				      intel_clock_t *clock, u32 fp)
+				      intel_clock_t *clock, u32 *fp,
+				      intel_clock_t *reduced_clock, u32 *fp2)
 {
 	struct drm_crtc *crtc = &intel_crtc->base;
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_encoder *intel_encoder;
 	uint32_t dpll;
-	int factor, pixel_multiplier, num_connectors = 0;
+	int factor, num_connectors = 0;
 	bool is_lvds = false, is_sdvo = false, is_tv = false;
-	bool is_dp = false, is_cpu_edp = false;
 
 	for_each_encoder_on_crtc(dev, crtc, intel_encoder) {
 		switch (intel_encoder->type) {
@@ -5497,14 +5517,6 @@ static uint32_t ironlake_compute_dpll(struct intel_crtc *intel_crtc,
 		case INTEL_OUTPUT_TVOUT:
 			is_tv = true;
 			break;
-		case INTEL_OUTPUT_DISPLAYPORT:
-			is_dp = true;
-			break;
-		case INTEL_OUTPUT_EDP:
-			is_dp = true;
-			if (!intel_encoder_is_pch_edp(&intel_encoder->base))
-				is_cpu_edp = true;
-			break;
 		}
 
 		num_connectors++;
@@ -5515,13 +5527,16 @@ static uint32_t ironlake_compute_dpll(struct intel_crtc *intel_crtc,
 	if (is_lvds) {
 		if ((intel_panel_use_ssc(dev_priv) &&
 		     dev_priv->lvds_ssc_freq == 100) ||
-		    intel_is_dual_link_lvds(dev))
+		    (HAS_PCH_IBX(dev) && intel_is_dual_link_lvds(dev)))
 			factor = 25;
 	} else if (is_sdvo && is_tv)
 		factor = 20;
 
 	if (clock->m < factor * clock->n)
-		fp |= FP_CB_TUNE;
+		*fp |= FP_CB_TUNE;
+
+	if (fp2 && (reduced_clock->m < factor * reduced_clock->n))
+		*fp2 |= FP_CB_TUNE;
 
 	dpll = 0;
 
@@ -5530,13 +5545,14 @@ static uint32_t ironlake_compute_dpll(struct intel_crtc *intel_crtc,
 	else
 		dpll |= DPLLB_MODE_DAC_SERIAL;
 	if (is_sdvo) {
-		pixel_multiplier = intel_mode_get_pixel_multiplier(adjusted_mode);
-		if (pixel_multiplier > 1) {
-			dpll |= (pixel_multiplier - 1) << PLL_REF_SDVO_HDMI_MULTIPLIER_SHIFT;
+		if (intel_crtc->config.pixel_multiplier > 1) {
+			dpll |= (intel_crtc->config.pixel_multiplier - 1)
+				<< PLL_REF_SDVO_HDMI_MULTIPLIER_SHIFT;
 		}
 		dpll |= DPLL_DVO_HIGH_SPEED;
 	}
-	if (is_dp && !is_cpu_edp)
+	if (intel_crtc->config.has_dp_encoder &&
+	    intel_crtc->config.has_pch_encoder)
 		dpll |= DPLL_DVO_HIGH_SPEED;
 
 	/* compute bitmask from p1 value */
@@ -5574,21 +5590,22 @@ static uint32_t ironlake_compute_dpll(struct intel_crtc *intel_crtc,
 }
 
 static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
-				  struct drm_display_mode *mode,
-				  struct drm_display_mode *adjusted_mode,
 				  int x, int y,
 				  struct drm_framebuffer *fb)
 {
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct drm_display_mode *adjusted_mode =
+		&intel_crtc->config.adjusted_mode;
+	struct drm_display_mode *mode = &intel_crtc->config.requested_mode;
 	int pipe = intel_crtc->pipe;
 	int plane = intel_crtc->plane;
 	int num_connectors = 0;
 	intel_clock_t clock, reduced_clock;
 	u32 dpll, fp = 0, fp2 = 0;
 	bool ok, has_reduced_clock = false;
-	bool is_lvds = false, is_dp = false, is_cpu_edp = false;
+	bool is_lvds = false;
 	struct intel_encoder *encoder;
 	int ret;
 	bool dither, fdi_config_ok;
@@ -5598,14 +5615,6 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 		case INTEL_OUTPUT_LVDS:
 			is_lvds = true;
 			break;
-		case INTEL_OUTPUT_DISPLAYPORT:
-			is_dp = true;
-			break;
-		case INTEL_OUTPUT_EDP:
-			is_dp = true;
-			if (!intel_encoder_is_pch_edp(&encoder->base))
-				is_cpu_edp = true;
-			break;
 		}
 
 		num_connectors++;
@@ -5614,19 +5623,28 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 	WARN(!(HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev)),
 	     "Unexpected PCH type %d\n", INTEL_PCH_TYPE(dev));
 
+	intel_crtc->config.cpu_transcoder = pipe;
+
 	ok = ironlake_compute_clocks(crtc, adjusted_mode, &clock,
 				     &has_reduced_clock, &reduced_clock);
 	if (!ok) {
 		DRM_ERROR("Couldn't find PLL settings for mode!\n");
 		return -EINVAL;
 	}
+	/* Compat-code for transition, will disappear. */
+	if (!intel_crtc->config.clock_set) {
+		intel_crtc->config.dpll.n = clock.n;
+		intel_crtc->config.dpll.m1 = clock.m1;
+		intel_crtc->config.dpll.m2 = clock.m2;
+		intel_crtc->config.dpll.p1 = clock.p1;
+		intel_crtc->config.dpll.p2 = clock.p2;
+	}
 
 	/* Ensure that the cursor is valid for the new mode before changing... */
 	intel_crtc_update_cursor(crtc, true);
 
 	/* determine panel color depth */
-	dither = intel_choose_pipe_bpp_dither(crtc, fb, &intel_crtc->bpp,
-					      adjusted_mode);
+	dither = intel_crtc->config.dither;
 	if (is_lvds && dev_priv->lvds_dither)
 		dither = true;
 
@@ -5635,13 +5653,14 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 		fp2 = reduced_clock.n << 16 | reduced_clock.m1 << 8 |
 			reduced_clock.m2;
 
-	dpll = ironlake_compute_dpll(intel_crtc, adjusted_mode, &clock, fp);
+	dpll = ironlake_compute_dpll(intel_crtc, &clock, &fp, &reduced_clock,
+				     has_reduced_clock ? &fp2 : NULL);
 
 	DRM_DEBUG_KMS("Mode for pipe %d:\n", pipe);
 	drm_mode_debug_printmodeline(mode);
 
 	/* CPU eDP is the only output that doesn't need a PCH PLL of its own. */
-	if (!is_cpu_edp) {
+	if (intel_crtc->config.has_pch_encoder) {
 		struct intel_pch_pll *pll;
 
 		pll = intel_get_pch_pll(intel_crtc, dpll, fp);
@@ -5653,8 +5672,8 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 	} else
 		intel_put_pch_pll(intel_crtc);
 
-	if (is_dp && !is_cpu_edp)
-		intel_dp_set_m_n(crtc, mode, adjusted_mode);
+	if (intel_crtc->config.has_dp_encoder)
+		intel_dp_set_m_n(intel_crtc);
 
 	for_each_encoder_on_crtc(dev, crtc, encoder)
 		if (encoder->pre_pll_enable)
@@ -5689,7 +5708,9 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 
 	/* Note, this also computes intel_crtc->fdi_lanes which is used below in
 	 * ironlake_check_fdi_lanes. */
-	ironlake_set_m_n(crtc, mode, adjusted_mode);
+	intel_crtc->fdi_lanes = 0;
+	if (intel_crtc->config.has_pch_encoder)
+		ironlake_fdi_set_m_n(crtc);
 
 	fdi_config_ok = ironlake_check_fdi_lanes(intel_crtc);
 
@@ -5710,6 +5731,23 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 	return fdi_config_ok ? ret : -EINVAL;
 }
 
+static bool ironlake_get_pipe_config(struct intel_crtc *crtc,
+				     struct intel_crtc_config *pipe_config)
+{
+	struct drm_device *dev = crtc->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t tmp;
+
+	tmp = I915_READ(PIPECONF(crtc->pipe));
+	if (!(tmp & PIPECONF_ENABLE))
+		return false;
+
+	if (I915_READ(TRANSCONF(crtc->pipe)) & TRANS_ENABLE)
+		pipe_config->has_pch_encoder = true;
+
+	return true;
+}
+
 static void haswell_modeset_global_resources(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -5740,29 +5778,26 @@ static void haswell_modeset_global_resources(struct drm_device *dev)
 }
 
 static int haswell_crtc_mode_set(struct drm_crtc *crtc,
-				 struct drm_display_mode *mode,
-				 struct drm_display_mode *adjusted_mode,
 				 int x, int y,
 				 struct drm_framebuffer *fb)
 {
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct drm_display_mode *adjusted_mode =
+		&intel_crtc->config.adjusted_mode;
+	struct drm_display_mode *mode = &intel_crtc->config.requested_mode;
 	int pipe = intel_crtc->pipe;
 	int plane = intel_crtc->plane;
 	int num_connectors = 0;
-	bool is_dp = false, is_cpu_edp = false;
+	bool is_cpu_edp = false;
 	struct intel_encoder *encoder;
 	int ret;
 	bool dither;
 
 	for_each_encoder_on_crtc(dev, crtc, encoder) {
 		switch (encoder->type) {
-		case INTEL_OUTPUT_DISPLAYPORT:
-			is_dp = true;
-			break;
 		case INTEL_OUTPUT_EDP:
-			is_dp = true;
 			if (!intel_encoder_is_pch_edp(&encoder->base))
 				is_cpu_edp = true;
 			break;
@@ -5772,9 +5807,9 @@ static int haswell_crtc_mode_set(struct drm_crtc *crtc,
 	}
 
 	if (is_cpu_edp)
-		intel_crtc->cpu_transcoder = TRANSCODER_EDP;
+		intel_crtc->config.cpu_transcoder = TRANSCODER_EDP;
 	else
-		intel_crtc->cpu_transcoder = pipe;
+		intel_crtc->config.cpu_transcoder = pipe;
 
 	/* We are not sure yet this won't happen. */
 	WARN(!HAS_PCH_LPT(dev), "Unexpected PCH type %d\n",
@@ -5783,7 +5818,7 @@ static int haswell_crtc_mode_set(struct drm_crtc *crtc,
 	WARN(num_connectors != 1, "%d connectors attached to pipe %c\n",
 	     num_connectors, pipe_name(pipe));
 
-	WARN_ON(I915_READ(PIPECONF(intel_crtc->cpu_transcoder)) &
+	WARN_ON(I915_READ(PIPECONF(intel_crtc->config.cpu_transcoder)) &
 		(PIPECONF_ENABLE | I965_PIPECONF_ACTIVE));
 
 	WARN_ON(I915_READ(DSPCNTR(plane)) & DISPLAY_PLANE_ENABLE);
@@ -5795,25 +5830,24 @@ static int haswell_crtc_mode_set(struct drm_crtc *crtc,
 	intel_crtc_update_cursor(crtc, true);
 
 	/* determine panel color depth */
-	dither = intel_choose_pipe_bpp_dither(crtc, fb, &intel_crtc->bpp,
-					      adjusted_mode);
+	dither = intel_crtc->config.dither;
 
 	DRM_DEBUG_KMS("Mode for pipe %d:\n", pipe);
 	drm_mode_debug_printmodeline(mode);
 
-	if (is_dp && !is_cpu_edp)
-		intel_dp_set_m_n(crtc, mode, adjusted_mode);
+	if (intel_crtc->config.has_dp_encoder)
+		intel_dp_set_m_n(intel_crtc);
 
 	intel_crtc->lowfreq_avail = false;
 
 	intel_set_pipe_timings(intel_crtc, mode, adjusted_mode);
 
-	if (!is_dp || is_cpu_edp)
-		ironlake_set_m_n(crtc, mode, adjusted_mode);
+	if (intel_crtc->config.has_pch_encoder)
+		ironlake_fdi_set_m_n(crtc);
 
 	haswell_set_pipeconf(crtc, adjusted_mode, dither);
 
-	intel_set_pipe_csc(crtc, adjusted_mode);
+	intel_set_pipe_csc(crtc);
 
 	/* Set up the display plane register */
 	I915_WRITE(DSPCNTR(plane), DISPPLANE_GAMMA_ENABLE | DISPPLANE_PIPE_CSC_ENABLE);
@@ -5828,9 +5862,32 @@ static int haswell_crtc_mode_set(struct drm_crtc *crtc,
 	return ret;
 }
 
+static bool haswell_get_pipe_config(struct intel_crtc *crtc,
+				    struct intel_crtc_config *pipe_config)
+{
+	struct drm_device *dev = crtc->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t tmp;
+
+	tmp = I915_READ(PIPECONF(crtc->config.cpu_transcoder));
+	if (!(tmp & PIPECONF_ENABLE))
+		return false;
+
+	/*
+	 * aswell has only FDI/PCH transcoder A. It is which is connected to
+	 * DDI E. So just check whether this pipe is wired to DDI E and whether
+	 * the PCH transcoder is on.
+	 */
+	tmp = I915_READ(TRANS_DDI_FUNC_CTL(crtc->pipe));
+	if ((tmp & TRANS_DDI_PORT_MASK) == TRANS_DDI_SELECT_PORT(PORT_E) &&
+	    I915_READ(TRANSCONF(PIPE_A)) & TRANS_ENABLE)
+		pipe_config->has_pch_encoder = true;
+
+
+	return true;
+}
+
 static int intel_crtc_mode_set(struct drm_crtc *crtc,
-			       struct drm_display_mode *mode,
-			       struct drm_display_mode *adjusted_mode,
 			       int x, int y,
 			       struct drm_framebuffer *fb)
 {
@@ -5839,13 +5896,16 @@ static int intel_crtc_mode_set(struct drm_crtc *crtc,
 	struct drm_encoder_helper_funcs *encoder_funcs;
 	struct intel_encoder *encoder;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct drm_display_mode *adjusted_mode =
+		&intel_crtc->config.adjusted_mode;
+	struct drm_display_mode *mode = &intel_crtc->config.requested_mode;
 	int pipe = intel_crtc->pipe;
 	int ret;
 
 	drm_vblank_pre_modeset(dev, pipe);
 
-	ret = dev_priv->display.crtc_mode_set(crtc, mode, adjusted_mode,
-					      x, y, fb);
+	ret = dev_priv->display.crtc_mode_set(crtc, x, y, fb);
+
 	drm_vblank_post_modeset(dev, pipe);
 
 	if (ret != 0)
@@ -5856,8 +5916,12 @@ static int intel_crtc_mode_set(struct drm_crtc *crtc,
 			encoder->base.base.id,
 			drm_get_encoder_name(&encoder->base),
 			mode->base.id, mode->name);
-		encoder_funcs = encoder->base.helper_private;
-		encoder_funcs->mode_set(&encoder->base, mode, adjusted_mode);
+		if (encoder->mode_set) {
+			encoder->mode_set(encoder);
+		} else {
+			encoder_funcs = encoder->base.helper_private;
+			encoder_funcs->mode_set(&encoder->base, mode, adjusted_mode);
+		}
 	}
 
 	return 0;
@@ -6325,13 +6389,24 @@ static int intel_crtc_cursor_set(struct drm_crtc *crtc,
 	/* we only need to pin inside GTT if cursor is non-phy */
 	mutex_lock(&dev->struct_mutex);
 	if (!dev_priv->info->cursor_needs_physical) {
+		unsigned alignment;
+
 		if (obj->tiling_mode) {
 			DRM_ERROR("cursor cannot be tiled\n");
 			ret = -EINVAL;
 			goto fail_locked;
 		}
 
-		ret = i915_gem_object_pin_to_display_plane(obj, 0, NULL);
+		/* Note that the w/a also requires 2 PTE of padding following
+		 * the bo. We currently fill all unused PTE with the shadow
+		 * page and so we should always have valid PTE following the
+		 * cursor preventing the VT-d warning.
+		 */
+		alignment = 0;
+		if (need_vtd_wa(dev))
+			alignment = 64*1024;
+
+		ret = i915_gem_object_pin_to_display_plane(obj, alignment, NULL);
 		if (ret) {
 			DRM_ERROR("failed to move cursor bo into the GTT\n");
 			goto fail_locked;
@@ -6436,20 +6511,6 @@ static void intel_crtc_gamma_set(struct drm_crtc *crtc, u16 *red, u16 *green,
 	intel_crtc_load_lut(crtc);
 }
 
-/**
- * Get a pipe with a simple mode set on it for doing load-based monitor
- * detection.
- *
- * It will be up to the load-detect code to adjust the pipe as appropriate for
- * its requirements.  The pipe will be connected to no other encoders.
- *
- * Currently this code will only succeed if there is a pipe with no encoders
- * configured for it.  In the future, it could choose to temporarily disable
- * some outputs to free up a pipe for its use.
- *
- * \return crtc, or NULL if no pipes are available.
- */
-
 /* VESA 640x480x72Hz mode to set on the pipe */
 static struct drm_display_mode load_detect_mode = {
 	DRM_MODE("640x480", DRM_MODE_TYPE_DEFAULT, 31500, 640, 664,
@@ -6776,7 +6837,7 @@ struct drm_display_mode *intel_crtc_mode_get(struct drm_device *dev,
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	enum transcoder cpu_transcoder = intel_crtc->cpu_transcoder;
+	enum transcoder cpu_transcoder = intel_crtc->config.cpu_transcoder;
 	struct drm_display_mode *mode;
 	int htot = I915_READ(HTOTAL(cpu_transcoder));
 	int hsync = I915_READ(HSYNC(cpu_transcoder));
@@ -6954,7 +7015,6 @@ static void do_intel_finish_page_flip(struct drm_device *dev,
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_unpin_work *work;
-	struct drm_i915_gem_object *obj;
 	unsigned long flags;
 
 	/* Ignore early vblank irqs */
@@ -6984,8 +7044,6 @@ static void do_intel_finish_page_flip(struct drm_device *dev,
 
 	spin_unlock_irqrestore(&dev->event_lock, flags);
 
-	obj = work->old_fb_obj;
-
 	wake_up_all(&dev_priv->pending_flip_queue);
 
 	queue_work(dev_priv->wq, &work->work);
@@ -7473,19 +7531,93 @@ static void intel_modeset_commit_output_state(struct drm_device *dev)
 	}
 }
 
-static struct drm_display_mode *
-intel_modeset_adjusted_mode(struct drm_crtc *crtc,
-			    struct drm_display_mode *mode)
+static int
+pipe_config_set_bpp(struct drm_crtc *crtc,
+		    struct drm_framebuffer *fb,
+		    struct intel_crtc_config *pipe_config)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_connector *connector;
+	int bpp;
+
+	switch (fb->pixel_format) {
+	case DRM_FORMAT_C8:
+		bpp = 8*3; /* since we go through a colormap */
+		break;
+	case DRM_FORMAT_XRGB1555:
+	case DRM_FORMAT_ARGB1555:
+		/* checked in intel_framebuffer_init already */
+		if (WARN_ON(INTEL_INFO(dev)->gen > 3))
+			return -EINVAL;
+	case DRM_FORMAT_RGB565:
+		bpp = 6*3; /* min is 18bpp */
+		break;
+	case DRM_FORMAT_XBGR8888:
+	case DRM_FORMAT_ABGR8888:
+		/* checked in intel_framebuffer_init already */
+		if (WARN_ON(INTEL_INFO(dev)->gen < 4))
+			return -EINVAL;
+	case DRM_FORMAT_XRGB8888:
+	case DRM_FORMAT_ARGB8888:
+		bpp = 8*3;
+		break;
+	case DRM_FORMAT_XRGB2101010:
+	case DRM_FORMAT_ARGB2101010:
+	case DRM_FORMAT_XBGR2101010:
+	case DRM_FORMAT_ABGR2101010:
+		/* checked in intel_framebuffer_init already */
+		if (WARN_ON(INTEL_INFO(dev)->gen < 4))
+			return -EINVAL;
+		bpp = 10*3;
+		break;
+	/* TODO: gen4+ supports 16 bpc floating point, too. */
+	default:
+		DRM_DEBUG_KMS("unsupported depth\n");
+		return -EINVAL;
+	}
+
+	pipe_config->pipe_bpp = bpp;
+
+	/* Clamp display bpp to EDID value */
+	list_for_each_entry(connector, &dev->mode_config.connector_list,
+			    head) {
+		if (connector->encoder && connector->encoder->crtc != crtc)
+			continue;
+
+		/* Don't use an invalid EDID bpc value */
+		if (connector->display_info.bpc &&
+		    connector->display_info.bpc * 3 < bpp) {
+			DRM_DEBUG_KMS("clamping display bpp (was %d) to EDID reported max of %d\n",
+				      bpp, connector->display_info.bpc*3);
+			pipe_config->pipe_bpp = connector->display_info.bpc*3;
+		}
+	}
+
+	return bpp;
+}
+
+static struct intel_crtc_config *
+intel_modeset_pipe_config(struct drm_crtc *crtc,
+			  struct drm_framebuffer *fb,
+			  struct drm_display_mode *mode)
 {
 	struct drm_device *dev = crtc->dev;
-	struct drm_display_mode *adjusted_mode;
 	struct drm_encoder_helper_funcs *encoder_funcs;
 	struct intel_encoder *encoder;
+	struct intel_crtc_config *pipe_config;
+	int plane_bpp;
 
-	adjusted_mode = drm_mode_duplicate(dev, mode);
-	if (!adjusted_mode)
+	pipe_config = kzalloc(sizeof(*pipe_config), GFP_KERNEL);
+	if (!pipe_config)
 		return ERR_PTR(-ENOMEM);
 
+	drm_mode_copy(&pipe_config->adjusted_mode, mode);
+	drm_mode_copy(&pipe_config->requested_mode, mode);
+
+	plane_bpp = pipe_config_set_bpp(crtc, fb, pipe_config);
+	if (plane_bpp < 0)
+		goto fail;
+
 	/* Pass our mode to the connectors and the CRTC to give them a chance to
 	 * adjust it according to limitations or connector properties, and also
 	 * a chance to reject the mode entirely.
@@ -7495,23 +7627,38 @@ intel_modeset_adjusted_mode(struct drm_crtc *crtc,
 
 		if (&encoder->new_crtc->base != crtc)
 			continue;
+
+		if (encoder->compute_config) {
+			if (!(encoder->compute_config(encoder, pipe_config))) {
+				DRM_DEBUG_KMS("Encoder config failure\n");
+				goto fail;
+			}
+
+			continue;
+		}
+
 		encoder_funcs = encoder->base.helper_private;
-		if (!(encoder_funcs->mode_fixup(&encoder->base, mode,
-						adjusted_mode))) {
+		if (!(encoder_funcs->mode_fixup(&encoder->base,
+						&pipe_config->requested_mode,
+						&pipe_config->adjusted_mode))) {
 			DRM_DEBUG_KMS("Encoder fixup failed\n");
 			goto fail;
 		}
 	}
 
-	if (!(intel_crtc_mode_fixup(crtc, mode, adjusted_mode))) {
+	if (!(intel_crtc_compute_config(crtc, pipe_config))) {
 		DRM_DEBUG_KMS("CRTC fixup failed\n");
 		goto fail;
 	}
 	DRM_DEBUG_KMS("[CRTC:%d]\n", crtc->base.id);
 
-	return adjusted_mode;
+	pipe_config->dither = pipe_config->pipe_bpp != plane_bpp;
+	DRM_DEBUG_KMS("plane bpp: %i, pipe bpp: %i, dithering: %i\n",
+		      plane_bpp, pipe_config->pipe_bpp, pipe_config->dither);
+
+	return pipe_config;
 fail:
-	drm_mode_destroy(dev, adjusted_mode);
+	kfree(pipe_config);
 	return ERR_PTR(-EINVAL);
 }
 
@@ -7589,22 +7736,25 @@ intel_modeset_affected_pipes(struct drm_crtc *crtc, unsigned *modeset_pipes,
 	if (crtc->enabled)
 		*prepare_pipes |= 1 << intel_crtc->pipe;
 
-	/* We only support modeset on one single crtc, hence we need to do that
-	 * only for the passed in crtc iff we change anything else than just
-	 * disable crtcs.
-	 *
-	 * This is actually not true, to be fully compatible with the old crtc
-	 * helper we automatically disable _any_ output (i.e. doesn't need to be
-	 * connected to the crtc we're modesetting on) if it's disconnected.
-	 * Which is a rather nutty api (since changed the output configuration
-	 * without userspace's explicit request can lead to confusion), but
-	 * alas. Hence we currently need to modeset on all pipes we prepare. */
+	/*
+	 * For simplicity do a full modeset on any pipe where the output routing
+	 * changed. We could be more clever, but that would require us to be
+	 * more careful with calling the relevant encoder->mode_set functions.
+	 */
 	if (*prepare_pipes)
 		*modeset_pipes = *prepare_pipes;
 
 	/* ... and mask these out. */
 	*modeset_pipes &= ~(*disable_pipes);
 	*prepare_pipes &= ~(*disable_pipes);
+
+	/*
+	 * HACK: We don't (yet) fully support global modesets. intel_set_config
+	 * obies this rule, but the modeset restore mode of
+	 * intel_modeset_setup_hw_state does not.
+	 */
+	*modeset_pipes &= 1 << intel_crtc->pipe;
+	*prepare_pipes &= 1 << intel_crtc->pipe;
 }
 
 static bool intel_crtc_in_use(struct drm_crtc *crtc)
@@ -7673,12 +7823,29 @@ intel_modeset_update_state(struct drm_device *dev, unsigned prepare_pipes)
 			    base.head) \
 		if (mask & (1 <<(intel_crtc)->pipe)) \
 
+static bool
+intel_pipe_config_compare(struct intel_crtc_config *current_config,
+			  struct intel_crtc_config *pipe_config)
+{
+	if (current_config->has_pch_encoder != pipe_config->has_pch_encoder) {
+		DRM_ERROR("mismatch in has_pch_encoder "
+			  "(expected %i, found %i)\n",
+			  current_config->has_pch_encoder,
+			  pipe_config->has_pch_encoder);
+		return false;
+	}
+
+	return true;
+}
+
 void
 intel_modeset_check_state(struct drm_device *dev)
 {
+	drm_i915_private_t *dev_priv = dev->dev_private;
 	struct intel_crtc *crtc;
 	struct intel_encoder *encoder;
 	struct intel_connector *connector;
+	struct intel_crtc_config pipe_config;
 
 	list_for_each_entry(connector, &dev->mode_config.connector_list,
 			    base.head) {
@@ -7767,17 +7934,27 @@ intel_modeset_check_state(struct drm_device *dev)
 		     "crtc's computed enabled state doesn't match tracked enabled state "
 		     "(expected %i, found %i)\n", enabled, crtc->base.enabled);
 
-		assert_pipe(dev->dev_private, crtc->pipe, crtc->active);
+		memset(&pipe_config, 0, sizeof(pipe_config));
+		active = dev_priv->display.get_pipe_config(crtc,
+							   &pipe_config);
+		WARN(crtc->active != active,
+		     "crtc active state doesn't match with hw state "
+		     "(expected %i, found %i)\n", crtc->active, active);
+
+		WARN(active &&
+		     !intel_pipe_config_compare(&crtc->config, &pipe_config),
+		     "pipe state doesn't match!\n");
 	}
 }
 
-int intel_set_mode(struct drm_crtc *crtc,
-		   struct drm_display_mode *mode,
-		   int x, int y, struct drm_framebuffer *fb)
+static int __intel_set_mode(struct drm_crtc *crtc,
+			    struct drm_display_mode *mode,
+			    int x, int y, struct drm_framebuffer *fb)
 {
 	struct drm_device *dev = crtc->dev;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	struct drm_display_mode *adjusted_mode, *saved_mode, *saved_hwmode;
+	struct drm_display_mode *saved_mode, *saved_hwmode;
+	struct intel_crtc_config *pipe_config = NULL;
 	struct intel_crtc *intel_crtc;
 	unsigned disable_pipes, prepare_pipes, modeset_pipes;
 	int ret = 0;
@@ -7790,12 +7967,6 @@ int intel_set_mode(struct drm_crtc *crtc,
 	intel_modeset_affected_pipes(crtc, &modeset_pipes,
 				     &prepare_pipes, &disable_pipes);
 
-	DRM_DEBUG_KMS("set mode pipe masks: modeset: %x, prepare: %x, disable: %x\n",
-		      modeset_pipes, prepare_pipes, disable_pipes);
-
-	for_each_intel_crtc_masked(dev, disable_pipes, intel_crtc)
-		intel_crtc_disable(&intel_crtc->base);
-
 	*saved_hwmode = crtc->hwmode;
 	*saved_mode = crtc->mode;
 
@@ -7804,15 +7975,22 @@ int intel_set_mode(struct drm_crtc *crtc,
 	 * Hence simply check whether any bit is set in modeset_pipes in all the
 	 * pieces of code that are not yet converted to deal with mutliple crtcs
 	 * changing their mode at the same time. */
-	adjusted_mode = NULL;
 	if (modeset_pipes) {
-		adjusted_mode = intel_modeset_adjusted_mode(crtc, mode);
-		if (IS_ERR(adjusted_mode)) {
-			ret = PTR_ERR(adjusted_mode);
+		pipe_config = intel_modeset_pipe_config(crtc, fb, mode);
+		if (IS_ERR(pipe_config)) {
+			ret = PTR_ERR(pipe_config);
+			pipe_config = NULL;
+
 			goto out;
 		}
 	}
 
+	DRM_DEBUG_KMS("set mode pipe masks: modeset: %x, prepare: %x, disable: %x\n",
+		      modeset_pipes, prepare_pipes, disable_pipes);
+
+	for_each_intel_crtc_masked(dev, disable_pipes, intel_crtc)
+		intel_crtc_disable(&intel_crtc->base);
+
 	for_each_intel_crtc_masked(dev, prepare_pipes, intel_crtc) {
 		if (intel_crtc->base.enabled)
 			dev_priv->display.crtc_disable(&intel_crtc->base);
@@ -7821,8 +7999,14 @@ int intel_set_mode(struct drm_crtc *crtc,
 	/* crtc->mode is already used by the ->mode_set callbacks, hence we need
 	 * to set it here already despite that we pass it down the callchain.
 	 */
-	if (modeset_pipes)
+	if (modeset_pipes) {
+		enum transcoder tmp = to_intel_crtc(crtc)->config.cpu_transcoder;
 		crtc->mode = *mode;
+		/* mode_set/enable/disable functions rely on a correct pipe
+		 * config. */
+		to_intel_crtc(crtc)->config = *pipe_config;
+		to_intel_crtc(crtc)->config.cpu_transcoder = tmp;
+	}
 
 	/* Only after disabling all output pipelines that will be changed can we
 	 * update the the output configuration. */
@@ -7836,7 +8020,6 @@ int intel_set_mode(struct drm_crtc *crtc,
 	 */
 	for_each_intel_crtc_masked(dev, modeset_pipes, intel_crtc) {
 		ret = intel_crtc_mode_set(&intel_crtc->base,
-					  mode, adjusted_mode,
 					  x, y, fb);
 		if (ret)
 			goto done;
@@ -7848,7 +8031,7 @@ int intel_set_mode(struct drm_crtc *crtc,
 
 	if (modeset_pipes) {
 		/* Store real post-adjustment hardware mode. */
-		crtc->hwmode = *adjusted_mode;
+		crtc->hwmode = pipe_config->adjusted_mode;
 
 		/* Calculate and store various constants which
 		 * are later needed by vblank and swap-completion
@@ -7859,19 +8042,31 @@ int intel_set_mode(struct drm_crtc *crtc,
 
 	/* FIXME: add subpixel order */
 done:
-	drm_mode_destroy(dev, adjusted_mode);
 	if (ret && crtc->enabled) {
 		crtc->hwmode = *saved_hwmode;
 		crtc->mode = *saved_mode;
-	} else {
-		intel_modeset_check_state(dev);
 	}
 
 out:
+	kfree(pipe_config);
 	kfree(saved_mode);
 	return ret;
 }
 
+int intel_set_mode(struct drm_crtc *crtc,
+		     struct drm_display_mode *mode,
+		     int x, int y, struct drm_framebuffer *fb)
+{
+	int ret;
+
+	ret = __intel_set_mode(crtc, mode, x, y, fb);
+
+	if (ret == 0)
+		intel_modeset_check_state(crtc->dev);
+
+	return ret;
+}
+
 void intel_crtc_restore_mode(struct drm_crtc *crtc)
 {
 	intel_set_mode(crtc, &crtc->mode, crtc->x, crtc->y, crtc->fb);
@@ -7959,10 +8154,8 @@ intel_set_config_compute_mode_changes(struct drm_mode_set *set,
 			config->mode_changed = true;
 		} else if (set->fb == NULL) {
 			config->mode_changed = true;
-		} else if (set->fb->depth != set->crtc->fb->depth) {
-			config->mode_changed = true;
-		} else if (set->fb->bits_per_pixel !=
-			   set->crtc->fb->bits_per_pixel) {
+		} else if (set->fb->pixel_format !=
+			   set->crtc->fb->pixel_format) {
 			config->mode_changed = true;
 		} else
 			config->fb_changed = true;
@@ -8145,6 +8338,8 @@ static int intel_crtc_set_config(struct drm_mode_set *set)
 			goto fail;
 		}
 	} else if (config->fb_changed) {
+		intel_crtc_wait_for_pending_flips(set->crtc);
+
 		ret = intel_pipe_set_base(set->crtc,
 					  set->x, set->y, set->fb);
 	}
@@ -8221,7 +8416,7 @@ static void intel_crtc_init(struct drm_device *dev, int pipe)
 	/* Swap pipes & planes for FBC on pre-965 */
 	intel_crtc->pipe = pipe;
 	intel_crtc->plane = pipe;
-	intel_crtc->cpu_transcoder = pipe;
+	intel_crtc->config.cpu_transcoder = pipe;
 	if (IS_MOBILE(dev) && IS_GEN3(dev)) {
 		DRM_DEBUG_KMS("swapping pipes & planes for FBC\n");
 		intel_crtc->plane = !pipe;
@@ -8232,8 +8427,6 @@ static void intel_crtc_init(struct drm_device *dev, int pipe)
 	dev_priv->plane_to_crtc_mapping[intel_crtc->plane] = &intel_crtc->base;
 	dev_priv->pipe_to_crtc_mapping[intel_crtc->pipe] = &intel_crtc->base;
 
-	intel_crtc->bpp = 24; /* default for pre-Ironlake */
-
 	drm_crtc_helper_add(&intel_crtc->base, &intel_helper_funcs);
 }
 
@@ -8314,7 +8507,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 		I915_WRITE(PFIT_CONTROL, 0);
 	}
 
-	if (!(HAS_DDI(dev) && (I915_READ(DDI_BUF_CTL(PORT_A)) & DDI_A_4_LANES)))
+	if (!IS_ULT(dev))
 		intel_crt_init(dev);
 
 	if (HAS_DDI(dev)) {
@@ -8343,20 +8536,20 @@ static void intel_setup_outputs(struct drm_device *dev)
 		if (has_edp_a(dev))
 			intel_dp_init(dev, DP_A, PORT_A);
 
-		if (I915_READ(HDMIB) & PORT_DETECTED) {
+		if (I915_READ(PCH_HDMIB) & SDVO_DETECTED) {
 			/* PCH SDVOB multiplex with HDMIB */
 			found = intel_sdvo_init(dev, PCH_SDVOB, true);
 			if (!found)
-				intel_hdmi_init(dev, HDMIB, PORT_B);
+				intel_hdmi_init(dev, PCH_HDMIB, PORT_B);
 			if (!found && (I915_READ(PCH_DP_B) & DP_DETECTED))
 				intel_dp_init(dev, PCH_DP_B, PORT_B);
 		}
 
-		if (I915_READ(HDMIC) & PORT_DETECTED)
-			intel_hdmi_init(dev, HDMIC, PORT_C);
+		if (I915_READ(PCH_HDMIC) & SDVO_DETECTED)
+			intel_hdmi_init(dev, PCH_HDMIC, PORT_C);
 
-		if (!dpd_is_edp && I915_READ(HDMID) & PORT_DETECTED)
-			intel_hdmi_init(dev, HDMID, PORT_D);
+		if (!dpd_is_edp && I915_READ(PCH_HDMID) & SDVO_DETECTED)
+			intel_hdmi_init(dev, PCH_HDMID, PORT_D);
 
 		if (I915_READ(PCH_DP_C) & DP_DETECTED)
 			intel_dp_init(dev, PCH_DP_C, PORT_C);
@@ -8368,24 +8561,21 @@ static void intel_setup_outputs(struct drm_device *dev)
 		if (I915_READ(VLV_DISPLAY_BASE + DP_C) & DP_DETECTED)
 			intel_dp_init(dev, VLV_DISPLAY_BASE + DP_C, PORT_C);
 
-		if (I915_READ(VLV_DISPLAY_BASE + SDVOB) & PORT_DETECTED) {
-			intel_hdmi_init(dev, VLV_DISPLAY_BASE + SDVOB, PORT_B);
+		if (I915_READ(VLV_DISPLAY_BASE + GEN4_HDMIB) & SDVO_DETECTED) {
+			intel_hdmi_init(dev, VLV_DISPLAY_BASE + GEN4_HDMIB,
+					PORT_B);
 			if (I915_READ(VLV_DISPLAY_BASE + DP_B) & DP_DETECTED)
 				intel_dp_init(dev, VLV_DISPLAY_BASE + DP_B, PORT_B);
 		}
-
-		if (I915_READ(VLV_DISPLAY_BASE + SDVOC) & PORT_DETECTED)
-			intel_hdmi_init(dev, VLV_DISPLAY_BASE + SDVOC, PORT_C);
-
 	} else if (SUPPORTS_DIGITAL_OUTPUTS(dev)) {
 		bool found = false;
 
-		if (I915_READ(SDVOB) & SDVO_DETECTED) {
+		if (I915_READ(GEN3_SDVOB) & SDVO_DETECTED) {
 			DRM_DEBUG_KMS("probing SDVOB\n");
-			found = intel_sdvo_init(dev, SDVOB, true);
+			found = intel_sdvo_init(dev, GEN3_SDVOB, true);
 			if (!found && SUPPORTS_INTEGRATED_HDMI(dev)) {
 				DRM_DEBUG_KMS("probing HDMI on SDVOB\n");
-				intel_hdmi_init(dev, SDVOB, PORT_B);
+				intel_hdmi_init(dev, GEN4_HDMIB, PORT_B);
 			}
 
 			if (!found && SUPPORTS_INTEGRATED_DP(dev)) {
@@ -8396,16 +8586,16 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 		/* Before G4X SDVOC doesn't have its own detect register */
 
-		if (I915_READ(SDVOB) & SDVO_DETECTED) {
+		if (I915_READ(GEN3_SDVOB) & SDVO_DETECTED) {
 			DRM_DEBUG_KMS("probing SDVOC\n");
-			found = intel_sdvo_init(dev, SDVOC, false);
+			found = intel_sdvo_init(dev, GEN3_SDVOC, false);
 		}
 
-		if (!found && (I915_READ(SDVOC) & SDVO_DETECTED)) {
+		if (!found && (I915_READ(GEN3_SDVOC) & SDVO_DETECTED)) {
 
 			if (SUPPORTS_INTEGRATED_HDMI(dev)) {
 				DRM_DEBUG_KMS("probing HDMI on SDVOC\n");
-				intel_hdmi_init(dev, SDVOC, PORT_C);
+				intel_hdmi_init(dev, GEN4_HDMIC, PORT_C);
 			}
 			if (SUPPORTS_INTEGRATED_DP(dev)) {
 				DRM_DEBUG_KMS("probing DP_C\n");
@@ -8572,20 +8762,22 @@ static void intel_init_display(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	/* We always want a DPMS function */
 	if (HAS_DDI(dev)) {
+		dev_priv->display.get_pipe_config = haswell_get_pipe_config;
 		dev_priv->display.crtc_mode_set = haswell_crtc_mode_set;
 		dev_priv->display.crtc_enable = haswell_crtc_enable;
 		dev_priv->display.crtc_disable = haswell_crtc_disable;
 		dev_priv->display.off = haswell_crtc_off;
 		dev_priv->display.update_plane = ironlake_update_plane;
 	} else if (HAS_PCH_SPLIT(dev)) {
+		dev_priv->display.get_pipe_config = ironlake_get_pipe_config;
 		dev_priv->display.crtc_mode_set = ironlake_crtc_mode_set;
 		dev_priv->display.crtc_enable = ironlake_crtc_enable;
 		dev_priv->display.crtc_disable = ironlake_crtc_disable;
 		dev_priv->display.off = ironlake_crtc_off;
 		dev_priv->display.update_plane = ironlake_update_plane;
 	} else {
+		dev_priv->display.get_pipe_config = i9xx_get_pipe_config;
 		dev_priv->display.crtc_mode_set = i9xx_crtc_mode_set;
 		dev_priv->display.crtc_enable = i9xx_crtc_enable;
 		dev_priv->display.crtc_disable = i9xx_crtc_disable;
@@ -8828,7 +9020,7 @@ void intel_modeset_init_hw(struct drm_device *dev)
 void intel_modeset_init(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int i, ret;
+	int i, j, ret;
 
 	drm_mode_config_init(dev);
 
@@ -8844,6 +9036,9 @@ void intel_modeset_init(struct drm_device *dev)
 
 	intel_init_pm(dev);
 
+	if (INTEL_INFO(dev)->num_pipes == 0)
+		return;
+
 	intel_init_display(dev);
 
 	if (IS_GEN2(dev)) {
@@ -8859,13 +9054,17 @@ void intel_modeset_init(struct drm_device *dev)
 	dev->mode_config.fb_base = dev_priv->gtt.mappable_base;
 
 	DRM_DEBUG_KMS("%d display pipe%s available.\n",
-		      dev_priv->num_pipe, dev_priv->num_pipe > 1 ? "s" : "");
+		      INTEL_INFO(dev)->num_pipes,
+		      INTEL_INFO(dev)->num_pipes > 1 ? "s" : "");
 
-	for (i = 0; i < dev_priv->num_pipe; i++) {
+	for (i = 0; i < INTEL_INFO(dev)->num_pipes; i++) {
 		intel_crtc_init(dev, i);
-		ret = intel_plane_init(dev, i);
-		if (ret)
-			DRM_DEBUG_KMS("plane %d init failed: %d\n", i, ret);
+		for (j = 0; j < dev_priv->num_plane; j++) {
+			ret = intel_plane_init(dev, i, j);
+			if (ret)
+				DRM_DEBUG_KMS("pipe %d plane %d init failed: %d\n",
+					      i, j, ret);
+		}
 	}
 
 	intel_cpu_pll_init(dev);
@@ -8918,10 +9117,11 @@ 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;
+	struct drm_device *dev = crtc->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 reg, val;
 
-	if (dev_priv->num_pipe == 1)
+	if (INTEL_INFO(dev)->num_pipes == 1)
 		return true;
 
 	reg = DSPCNTR(!crtc->plane);
@@ -8941,7 +9141,7 @@ static void intel_sanitize_crtc(struct intel_crtc *crtc)
 	u32 reg;
 
 	/* Clear any frame start delays used for debugging left by the BIOS */
-	reg = PIPECONF(crtc->cpu_transcoder);
+	reg = PIPECONF(crtc->config.cpu_transcoder);
 	I915_WRITE(reg, I915_READ(reg) & ~PIPECONF_FRAME_START_DELAY_MASK);
 
 	/* We need to sanitize the plane -> pipe mapping first because this will
@@ -9077,6 +9277,7 @@ void intel_modeset_setup_hw_state(struct drm_device *dev,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	enum pipe pipe;
 	u32 tmp;
+	struct drm_plane *plane;
 	struct intel_crtc *crtc;
 	struct intel_encoder *encoder;
 	struct intel_connector *connector;
@@ -9096,24 +9297,32 @@ void intel_modeset_setup_hw_state(struct drm_device *dev,
 			case TRANS_DDI_EDP_INPUT_C_ONOFF:
 				pipe = PIPE_C;
 				break;
+			default:
+				/* A bogus value has been programmed, disable
+				 * the transcoder */
+				WARN(1, "Bogus eDP source %08x\n", tmp);
+				intel_ddi_disable_transcoder_func(dev_priv,
+						TRANSCODER_EDP);
+				goto setup_pipes;
 			}
 
 			crtc = to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
-			crtc->cpu_transcoder = TRANSCODER_EDP;
+			crtc->config.cpu_transcoder = TRANSCODER_EDP;
 
 			DRM_DEBUG_KMS("Pipe %c using transcoder EDP\n",
 				      pipe_name(pipe));
 		}
 	}
 
-	for_each_pipe(pipe) {
-		crtc = to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
+setup_pipes:
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list,
+			    base.head) {
+		enum transcoder tmp = crtc->config.cpu_transcoder;
+		memset(&crtc->config, 0, sizeof(crtc->config));
+		crtc->config.cpu_transcoder = tmp;
 
-		tmp = I915_READ(PIPECONF(crtc->cpu_transcoder));
-		if (tmp & PIPECONF_ENABLE)
-			crtc->active = true;
-		else
-			crtc->active = false;
+		crtc->active = dev_priv->display.get_pipe_config(crtc,
+								 &crtc->config);
 
 		crtc->base.enabled = crtc->active;
 
@@ -9172,9 +9381,19 @@ void intel_modeset_setup_hw_state(struct drm_device *dev,
 	}
 
 	if (force_restore) {
+		/*
+		 * We need to use raw interfaces for restoring state to avoid
+		 * checking (bogus) intermediate states.
+		 */
 		for_each_pipe(pipe) {
-			intel_crtc_restore_mode(dev_priv->pipe_to_crtc_mapping[pipe]);
+			struct drm_crtc *crtc =
+				dev_priv->pipe_to_crtc_mapping[pipe];
+
+			__intel_set_mode(crtc, &crtc->mode, crtc->x, crtc->y,
+					 crtc->fb);
 		}
+		list_for_each_entry(plane, &dev->mode_config.plane_list, head)
+			intel_plane_restore(plane);
 
 		i915_redisable_vga(dev);
 	} else {
@@ -9236,6 +9455,9 @@ void intel_modeset_cleanup(struct drm_device *dev)
 	/* flush any delayed tasks or pending work */
 	flush_scheduled_work();
 
+	/* destroy backlight, if any, before the connectors */
+	intel_panel_destroy_backlight(dev);
+
 	drm_mode_config_cleanup(dev);
 
 	intel_cleanup_overlay(dev);
@@ -9323,15 +9545,24 @@ intel_display_capture_error_state(struct drm_device *dev)
 	for_each_pipe(i) {
 		cpu_transcoder = intel_pipe_to_cpu_transcoder(dev_priv, i);
 
-		error->cursor[i].control = I915_READ(CURCNTR(i));
-		error->cursor[i].position = I915_READ(CURPOS(i));
-		error->cursor[i].base = I915_READ(CURBASE(i));
+		if (INTEL_INFO(dev)->gen <= 6 || IS_VALLEYVIEW(dev)) {
+			error->cursor[i].control = I915_READ(CURCNTR(i));
+			error->cursor[i].position = I915_READ(CURPOS(i));
+			error->cursor[i].base = I915_READ(CURBASE(i));
+		} else {
+			error->cursor[i].control = I915_READ(CURCNTR_IVB(i));
+			error->cursor[i].position = I915_READ(CURPOS_IVB(i));
+			error->cursor[i].base = I915_READ(CURBASE_IVB(i));
+		}
 
 		error->plane[i].control = I915_READ(DSPCNTR(i));
 		error->plane[i].stride = I915_READ(DSPSTRIDE(i));
-		error->plane[i].size = I915_READ(DSPSIZE(i));
-		error->plane[i].pos = I915_READ(DSPPOS(i));
-		error->plane[i].addr = I915_READ(DSPADDR(i));
+		if (INTEL_INFO(dev)->gen <= 3) {
+			error->plane[i].size = I915_READ(DSPSIZE(i));
+			error->plane[i].pos = I915_READ(DSPPOS(i));
+		}
+		if (INTEL_INFO(dev)->gen <= 7 && !IS_HASWELL(dev))
+			error->plane[i].addr = I915_READ(DSPADDR(i));
 		if (INTEL_INFO(dev)->gen >= 4) {
 			error->plane[i].surface = I915_READ(DSPSURF(i));
 			error->plane[i].tile_offset = I915_READ(DSPTILEOFF(i));
@@ -9355,10 +9586,9 @@ intel_display_print_error_state(struct seq_file *m,
 				struct drm_device *dev,
 				struct intel_display_error_state *error)
 {
-	drm_i915_private_t *dev_priv = dev->dev_private;
 	int i;
 
-	seq_printf(m, "Num Pipes: %d\n", dev_priv->num_pipe);
+	seq_printf(m, "Num Pipes: %d\n", INTEL_INFO(dev)->num_pipes);
 	for_each_pipe(i) {
 		seq_printf(m, "Pipe [%d]:\n", i);
 		seq_printf(m, "  CONF: %08x\n", error->pipe[i].conf);
@@ -9373,9 +9603,12 @@ intel_display_print_error_state(struct seq_file *m,
 		seq_printf(m, "Plane [%d]:\n", i);
 		seq_printf(m, "  CNTR: %08x\n", error->plane[i].control);
 		seq_printf(m, "  STRIDE: %08x\n", error->plane[i].stride);
-		seq_printf(m, "  SIZE: %08x\n", error->plane[i].size);
-		seq_printf(m, "  POS: %08x\n", error->plane[i].pos);
-		seq_printf(m, "  ADDR: %08x\n", error->plane[i].addr);
+		if (INTEL_INFO(dev)->gen <= 3) {
+			seq_printf(m, "  SIZE: %08x\n", error->plane[i].size);
+			seq_printf(m, "  POS: %08x\n", error->plane[i].pos);
+		}
+		if (INTEL_INFO(dev)->gen <= 7 && !IS_HASWELL(dev))
+			seq_printf(m, "  ADDR: %08x\n", error->plane[i].addr);
 		if (INTEL_INFO(dev)->gen >= 4) {
 			seq_printf(m, "  SURF: %08x\n", error->plane[i].surface);
 			seq_printf(m, "  TILEOFF: %08x\n", error->plane[i].tile_offset);
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index c3f5bd8a5077..fb2fbc1e08b9 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -109,29 +109,6 @@ bool intel_encoder_is_pch_edp(struct drm_encoder *encoder)
 
 static void intel_dp_link_down(struct intel_dp *intel_dp);
 
-void
-intel_edp_link_config(struct intel_encoder *intel_encoder,
-		       int *lane_num, int *link_bw)
-{
-	struct intel_dp *intel_dp = enc_to_intel_dp(&intel_encoder->base);
-
-	*lane_num = intel_dp->lane_count;
-	*link_bw = drm_dp_bw_code_to_link_rate(intel_dp->link_bw);
-}
-
-int
-intel_edp_target_clock(struct intel_encoder *intel_encoder,
-		       struct drm_display_mode *mode)
-{
-	struct intel_dp *intel_dp = enc_to_intel_dp(&intel_encoder->base);
-	struct intel_connector *intel_connector = intel_dp->attached_connector;
-
-	if (intel_connector->panel.fixed_mode)
-		return intel_connector->panel.fixed_mode->clock;
-	else
-		return mode->clock;
-}
-
 static int
 intel_dp_max_link_bw(struct intel_dp *intel_dp)
 {
@@ -177,34 +154,6 @@ intel_dp_max_data_rate(int max_link_clock, int max_lanes)
 	return (max_link_clock * max_lanes * 8) / 10;
 }
 
-static bool
-intel_dp_adjust_dithering(struct intel_dp *intel_dp,
-			  struct drm_display_mode *mode,
-			  bool adjust_mode)
-{
-	int max_link_clock =
-		drm_dp_bw_code_to_link_rate(intel_dp_max_link_bw(intel_dp));
-	int max_lanes = drm_dp_max_lane_count(intel_dp->dpcd);
-	int max_rate, mode_rate;
-
-	mode_rate = intel_dp_link_required(mode->clock, 24);
-	max_rate = intel_dp_max_data_rate(max_link_clock, max_lanes);
-
-	if (mode_rate > max_rate) {
-		mode_rate = intel_dp_link_required(mode->clock, 18);
-		if (mode_rate > max_rate)
-			return false;
-
-		if (adjust_mode)
-			mode->private_flags
-				|= INTEL_MODE_DP_FORCE_6BPC;
-
-		return true;
-	}
-
-	return true;
-}
-
 static int
 intel_dp_mode_valid(struct drm_connector *connector,
 		    struct drm_display_mode *mode)
@@ -212,6 +161,8 @@ intel_dp_mode_valid(struct drm_connector *connector,
 	struct intel_dp *intel_dp = intel_attached_dp(connector);
 	struct intel_connector *intel_connector = to_intel_connector(connector);
 	struct drm_display_mode *fixed_mode = intel_connector->panel.fixed_mode;
+	int target_clock = mode->clock;
+	int max_rate, mode_rate, max_lanes, max_link_clock;
 
 	if (is_edp(intel_dp) && fixed_mode) {
 		if (mode->hdisplay > fixed_mode->hdisplay)
@@ -219,9 +170,17 @@ intel_dp_mode_valid(struct drm_connector *connector,
 
 		if (mode->vdisplay > fixed_mode->vdisplay)
 			return MODE_PANEL;
+
+		target_clock = fixed_mode->clock;
 	}
 
-	if (!intel_dp_adjust_dithering(intel_dp, mode, false))
+	max_link_clock = drm_dp_bw_code_to_link_rate(intel_dp_max_link_bw(intel_dp));
+	max_lanes = drm_dp_max_lane_count(intel_dp->dpcd);
+
+	max_rate = intel_dp_max_data_rate(max_link_clock, max_lanes);
+	mode_rate = intel_dp_link_required(target_clock, 18);
+
+	if (mode_rate > max_rate)
 		return MODE_CLOCK_HIGH;
 
 	if (mode->clock < 10000)
@@ -294,16 +253,20 @@ static bool ironlake_edp_have_panel_power(struct intel_dp *intel_dp)
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 pp_stat_reg;
 
-	return (I915_READ(PCH_PP_STATUS) & PP_ON) != 0;
+	pp_stat_reg = IS_VALLEYVIEW(dev) ? PIPEA_PP_STATUS : PCH_PP_STATUS;
+	return (I915_READ(pp_stat_reg) & PP_ON) != 0;
 }
 
 static bool ironlake_edp_have_panel_vdd(struct intel_dp *intel_dp)
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 pp_ctrl_reg;
 
-	return (I915_READ(PCH_PP_CONTROL) & EDP_FORCE_VDD) != 0;
+	pp_ctrl_reg = IS_VALLEYVIEW(dev) ? PIPEA_PP_CONTROL : PCH_PP_CONTROL;
+	return (I915_READ(pp_ctrl_reg) & EDP_FORCE_VDD) != 0;
 }
 
 static void
@@ -311,14 +274,19 @@ intel_dp_check_edp(struct intel_dp *intel_dp)
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 pp_stat_reg, pp_ctrl_reg;
 
 	if (!is_edp(intel_dp))
 		return;
+
+	pp_stat_reg = IS_VALLEYVIEW(dev) ? PIPEA_PP_STATUS : PCH_PP_STATUS;
+	pp_ctrl_reg = IS_VALLEYVIEW(dev) ? PIPEA_PP_CONTROL : PCH_PP_CONTROL;
+
 	if (!ironlake_edp_have_panel_power(intel_dp) && !ironlake_edp_have_panel_vdd(intel_dp)) {
 		WARN(1, "eDP powered off while attempting aux channel communication.\n");
 		DRM_DEBUG_KMS("Status 0x%08x Control 0x%08x\n",
-			      I915_READ(PCH_PP_STATUS),
-			      I915_READ(PCH_PP_CONTROL));
+				I915_READ(pp_stat_reg),
+				I915_READ(pp_ctrl_reg));
 	}
 }
 
@@ -328,29 +296,10 @@ intel_dp_aux_wait_done(struct intel_dp *intel_dp, bool has_aux_irq)
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	struct drm_device *dev = intel_dig_port->base.base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t ch_ctl = intel_dp->output_reg + 0x10;
+	uint32_t ch_ctl = intel_dp->aux_ch_ctl_reg;
 	uint32_t status;
 	bool done;
 
-	if (IS_HASWELL(dev)) {
-		switch (intel_dig_port->port) {
-		case PORT_A:
-			ch_ctl = DPA_AUX_CH_CTL;
-			break;
-		case PORT_B:
-			ch_ctl = PCH_DPB_AUX_CH_CTL;
-			break;
-		case PORT_C:
-			ch_ctl = PCH_DPC_AUX_CH_CTL;
-			break;
-		case PORT_D:
-			ch_ctl = PCH_DPD_AUX_CH_CTL;
-			break;
-		default:
-			BUG();
-		}
-	}
-
 #define C (((status = I915_READ_NOTRACE(ch_ctl)) & DP_AUX_CH_CTL_SEND_BUSY) == 0)
 	if (has_aux_irq)
 		done = wait_event_timeout(dev_priv->gmbus_wait_queue, C,
@@ -370,11 +319,10 @@ intel_dp_aux_ch(struct intel_dp *intel_dp,
 		uint8_t *send, int send_bytes,
 		uint8_t *recv, int recv_size)
 {
-	uint32_t output_reg = intel_dp->output_reg;
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	struct drm_device *dev = intel_dig_port->base.base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t ch_ctl = output_reg + 0x10;
+	uint32_t ch_ctl = intel_dp->aux_ch_ctl_reg;
 	uint32_t ch_data = ch_ctl + 4;
 	int i, ret, recv_bytes;
 	uint32_t status;
@@ -388,29 +336,6 @@ intel_dp_aux_ch(struct intel_dp *intel_dp,
 	 */
 	pm_qos_update_request(&dev_priv->pm_qos, 0);
 
-	if (IS_HASWELL(dev)) {
-		switch (intel_dig_port->port) {
-		case PORT_A:
-			ch_ctl = DPA_AUX_CH_CTL;
-			ch_data = DPA_AUX_CH_DATA1;
-			break;
-		case PORT_B:
-			ch_ctl = PCH_DPB_AUX_CH_CTL;
-			ch_data = PCH_DPB_AUX_CH_DATA1;
-			break;
-		case PORT_C:
-			ch_ctl = PCH_DPC_AUX_CH_CTL;
-			ch_data = PCH_DPC_AUX_CH_DATA1;
-			break;
-		case PORT_D:
-			ch_ctl = PCH_DPD_AUX_CH_CTL;
-			ch_data = PCH_DPD_AUX_CH_DATA1;
-			break;
-		default:
-			BUG();
-		}
-	}
-
 	intel_dp_check_edp(intel_dp);
 	/* The clock divider is based off the hrawclk,
 	 * and would like to run at 2MHz. So, take the
@@ -428,10 +353,14 @@ intel_dp_aux_ch(struct intel_dp *intel_dp,
 			aux_clock_divider = 200; /* SNB & IVB eDP input clock at 400Mhz */
 		else
 			aux_clock_divider = 225; /* eDP input clock at 450Mhz */
-	} else if (HAS_PCH_SPLIT(dev))
+	} else if (dev_priv->pch_id == INTEL_PCH_LPT_DEVICE_ID_TYPE) {
+		/* Workaround for non-ULT HSW */
+		aux_clock_divider = 74;
+	} else if (HAS_PCH_SPLIT(dev)) {
 		aux_clock_divider = DIV_ROUND_UP(intel_pch_rawclk(dev), 2);
-	else
+	} else {
 		aux_clock_divider = intel_hrawclk(dev) / 2;
+	}
 
 	if (IS_GEN6(dev))
 		precharge = 3;
@@ -732,18 +661,26 @@ intel_dp_i2c_init(struct intel_dp *intel_dp,
 }
 
 bool
-intel_dp_mode_fixup(struct drm_encoder *encoder,
-		    const struct drm_display_mode *mode,
-		    struct drm_display_mode *adjusted_mode)
+intel_dp_compute_config(struct intel_encoder *encoder,
+			struct intel_crtc_config *pipe_config)
 {
-	struct drm_device *dev = encoder->dev;
-	struct intel_dp *intel_dp = enc_to_intel_dp(encoder);
+	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_display_mode *adjusted_mode = &pipe_config->adjusted_mode;
+	struct drm_display_mode *mode = &pipe_config->requested_mode;
+	struct intel_dp *intel_dp = enc_to_intel_dp(&encoder->base);
 	struct intel_connector *intel_connector = intel_dp->attached_connector;
 	int lane_count, clock;
 	int max_lane_count = drm_dp_max_lane_count(intel_dp->dpcd);
 	int max_clock = intel_dp_max_link_bw(intel_dp) == DP_LINK_BW_2_7 ? 1 : 0;
 	int bpp, mode_rate;
 	static int bws[2] = { DP_LINK_BW_1_62, DP_LINK_BW_2_7 };
+	int target_clock, link_avail, link_clock;
+
+	if (HAS_PCH_SPLIT(dev) && !HAS_DDI(dev) && !is_cpu_edp(intel_dp))
+		pipe_config->has_pch_encoder = true;
+
+	pipe_config->has_dp_encoder = true;
 
 	if (is_edp(intel_dp) && intel_connector->panel.fixed_mode) {
 		intel_fixed_panel_mode(intel_connector->panel.fixed_mode,
@@ -752,6 +689,8 @@ intel_dp_mode_fixup(struct drm_encoder *encoder,
 					intel_connector->panel.fitting_mode,
 					mode, adjusted_mode);
 	}
+	/* We need to take the panel's fixed mode into account. */
+	target_clock = adjusted_mode->clock;
 
 	if (adjusted_mode->flags & DRM_MODE_FLAG_DBLCLK)
 		return false;
@@ -760,11 +699,28 @@ intel_dp_mode_fixup(struct drm_encoder *encoder,
 		      "max bw %02x pixel clock %iKHz\n",
 		      max_lane_count, bws[max_clock], adjusted_mode->clock);
 
-	if (!intel_dp_adjust_dithering(intel_dp, adjusted_mode, true))
-		return false;
+	/* Walk through all bpp values. Luckily they're all nicely spaced with 2
+	 * bpc in between. */
+	bpp = min_t(int, 8*3, pipe_config->pipe_bpp);
+	for (; bpp >= 6*3; bpp -= 2*3) {
+		mode_rate = intel_dp_link_required(target_clock, bpp);
+
+		for (clock = 0; clock <= max_clock; clock++) {
+			for (lane_count = 1; lane_count <= max_lane_count; lane_count <<= 1) {
+				link_clock = drm_dp_bw_code_to_link_rate(bws[clock]);
+				link_avail = intel_dp_max_data_rate(link_clock,
+								    lane_count);
+
+				if (mode_rate <= link_avail) {
+					goto found;
+				}
+			}
+		}
+	}
 
-	bpp = adjusted_mode->private_flags & INTEL_MODE_DP_FORCE_6BPC ? 18 : 24;
+	return false;
 
+found:
 	if (intel_dp->color_range_auto) {
 		/*
 		 * See:
@@ -778,104 +734,38 @@ intel_dp_mode_fixup(struct drm_encoder *encoder,
 	}
 
 	if (intel_dp->color_range)
-		adjusted_mode->private_flags |= INTEL_MODE_LIMITED_COLOR_RANGE;
-
-	mode_rate = intel_dp_link_required(adjusted_mode->clock, bpp);
-
-	for (clock = 0; clock <= max_clock; clock++) {
-		for (lane_count = 1; lane_count <= max_lane_count; lane_count <<= 1) {
-			int link_bw_clock =
-				drm_dp_bw_code_to_link_rate(bws[clock]);
-			int link_avail = intel_dp_max_data_rate(link_bw_clock,
-								lane_count);
-
-			if (mode_rate <= link_avail) {
-				intel_dp->link_bw = bws[clock];
-				intel_dp->lane_count = lane_count;
-				adjusted_mode->clock = link_bw_clock;
-				DRM_DEBUG_KMS("DP link bw %02x lane "
-						"count %d clock %d bpp %d\n",
-				       intel_dp->link_bw, intel_dp->lane_count,
-				       adjusted_mode->clock, bpp);
-				DRM_DEBUG_KMS("DP link bw required %i available %i\n",
-					      mode_rate, link_avail);
-				return true;
-			}
-		}
-	}
-
-	return false;
-}
+		pipe_config->limited_color_range = true;
 
-void
-intel_dp_set_m_n(struct drm_crtc *crtc, struct drm_display_mode *mode,
-		 struct drm_display_mode *adjusted_mode)
-{
-	struct drm_device *dev = crtc->dev;
-	struct intel_encoder *intel_encoder;
-	struct intel_dp *intel_dp;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int lane_count = 4;
-	struct intel_link_m_n m_n;
-	int pipe = intel_crtc->pipe;
-	enum transcoder cpu_transcoder = intel_crtc->cpu_transcoder;
-	int target_clock;
+	intel_dp->link_bw = bws[clock];
+	intel_dp->lane_count = lane_count;
+	adjusted_mode->clock = drm_dp_bw_code_to_link_rate(intel_dp->link_bw);
+	pipe_config->pixel_target_clock = target_clock;
 
-	/*
-	 * Find the lane count in the intel_encoder private
-	 */
-	for_each_encoder_on_crtc(dev, crtc, intel_encoder) {
-		intel_dp = enc_to_intel_dp(&intel_encoder->base);
+	DRM_DEBUG_KMS("DP link bw %02x lane count %d clock %d bpp %d\n",
+		      intel_dp->link_bw, intel_dp->lane_count,
+		      adjusted_mode->clock, bpp);
+	DRM_DEBUG_KMS("DP link bw required %i available %i\n",
+		      mode_rate, link_avail);
 
-		if (intel_encoder->type == INTEL_OUTPUT_DISPLAYPORT ||
-		    intel_encoder->type == INTEL_OUTPUT_EDP)
-		{
-			lane_count = intel_dp->lane_count;
-			break;
-		}
-	}
-
-	target_clock = mode->clock;
-	for_each_encoder_on_crtc(dev, crtc, intel_encoder) {
-		if (intel_encoder->type == INTEL_OUTPUT_EDP) {
-			target_clock = intel_edp_target_clock(intel_encoder,
-							      mode);
-			break;
-		}
-	}
+	intel_link_compute_m_n(bpp, lane_count,
+			       target_clock, adjusted_mode->clock,
+			       &pipe_config->dp_m_n);
 
 	/*
-	 * Compute the GMCH and Link ratios. The '3' here is
-	 * the number of bytes_per_pixel post-LUT, which we always
-	 * set up for 8-bits of R/G/B, or 3 bytes total.
+	 * XXX: We have a strange regression where using the vbt edp bpp value
+	 * for the link bw computation results in black screens, the panel only
+	 * works when we do the computation at the usual 24bpp (but still
+	 * requires us to use 18bpp). Until that's fully debugged, stay
+	 * bug-for-bug compatible with the old code.
 	 */
-	intel_link_compute_m_n(intel_crtc->bpp, lane_count,
-			       target_clock, adjusted_mode->clock, &m_n);
-
-	if (IS_HASWELL(dev)) {
-		I915_WRITE(PIPE_DATA_M1(cpu_transcoder),
-			   TU_SIZE(m_n.tu) | m_n.gmch_m);
-		I915_WRITE(PIPE_DATA_N1(cpu_transcoder), m_n.gmch_n);
-		I915_WRITE(PIPE_LINK_M1(cpu_transcoder), m_n.link_m);
-		I915_WRITE(PIPE_LINK_N1(cpu_transcoder), m_n.link_n);
-	} else if (HAS_PCH_SPLIT(dev)) {
-		I915_WRITE(TRANSDATA_M1(pipe), TU_SIZE(m_n.tu) | m_n.gmch_m);
-		I915_WRITE(TRANSDATA_N1(pipe), m_n.gmch_n);
-		I915_WRITE(TRANSDPLINK_M1(pipe), m_n.link_m);
-		I915_WRITE(TRANSDPLINK_N1(pipe), m_n.link_n);
-	} else if (IS_VALLEYVIEW(dev)) {
-		I915_WRITE(PIPE_DATA_M1(pipe), TU_SIZE(m_n.tu) | m_n.gmch_m);
-		I915_WRITE(PIPE_DATA_N1(pipe), m_n.gmch_n);
-		I915_WRITE(PIPE_LINK_M1(pipe), m_n.link_m);
-		I915_WRITE(PIPE_LINK_N1(pipe), m_n.link_n);
-	} else {
-		I915_WRITE(PIPE_GMCH_DATA_M(pipe),
-			   TU_SIZE(m_n.tu) | m_n.gmch_m);
-		I915_WRITE(PIPE_GMCH_DATA_N(pipe), m_n.gmch_n);
-		I915_WRITE(PIPE_DP_LINK_M(pipe), m_n.link_m);
-		I915_WRITE(PIPE_DP_LINK_N(pipe), m_n.link_n);
+	if (is_edp(intel_dp) && dev_priv->edp.bpp) {
+		DRM_DEBUG_KMS("clamping display bpc (was %d) to eDP (%d)\n",
+			      bpp, dev_priv->edp.bpp);
+		bpp = min_t(int, bpp, dev_priv->edp.bpp);
 	}
+	pipe_config->pipe_bpp = bpp;
+
+	return true;
 }
 
 void intel_dp_init_link_config(struct intel_dp *intel_dp)
@@ -994,7 +884,7 @@ intel_dp_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode,
 		else
 			intel_dp->DP |= DP_PLL_FREQ_270MHZ;
 	} else if (!HAS_PCH_CPT(dev) || is_cpu_edp(intel_dp)) {
-		if (!HAS_PCH_SPLIT(dev))
+		if (!HAS_PCH_SPLIT(dev) && !IS_VALLEYVIEW(dev))
 			intel_dp->DP |= intel_dp->color_range;
 
 		if (adjusted_mode->flags & DRM_MODE_FLAG_PHSYNC)
@@ -1009,7 +899,7 @@ intel_dp_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode,
 		if (intel_crtc->pipe == 1)
 			intel_dp->DP |= DP_PIPEB_SELECT;
 
-		if (is_cpu_edp(intel_dp)) {
+		if (is_cpu_edp(intel_dp) && !IS_VALLEYVIEW(dev)) {
 			/* don't miss out required setting for eDP */
 			if (adjusted_mode->clock < 200000)
 				intel_dp->DP |= DP_PLL_FREQ_160MHZ;
@@ -1020,7 +910,7 @@ intel_dp_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode,
 		intel_dp->DP |= DP_LINK_TRAIN_OFF_CPT;
 	}
 
-	if (is_cpu_edp(intel_dp))
+	if (is_cpu_edp(intel_dp) && !IS_VALLEYVIEW(dev))
 		ironlake_set_pll_edp(crtc, adjusted_mode->clock);
 }
 
@@ -1039,16 +929,20 @@ static void ironlake_wait_panel_status(struct intel_dp *intel_dp,
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 pp_stat_reg, pp_ctrl_reg;
+
+	pp_stat_reg = IS_VALLEYVIEW(dev) ? PIPEA_PP_STATUS : PCH_PP_STATUS;
+	pp_ctrl_reg = IS_VALLEYVIEW(dev) ? PIPEA_PP_CONTROL : PCH_PP_CONTROL;
 
 	DRM_DEBUG_KMS("mask %08x value %08x status %08x control %08x\n",
-		      mask, value,
-		      I915_READ(PCH_PP_STATUS),
-		      I915_READ(PCH_PP_CONTROL));
+			mask, value,
+			I915_READ(pp_stat_reg),
+			I915_READ(pp_ctrl_reg));
 
-	if (_wait_for((I915_READ(PCH_PP_STATUS) & mask) == value, 5000, 10)) {
+	if (_wait_for((I915_READ(pp_stat_reg) & mask) == value, 5000, 10)) {
 		DRM_ERROR("Panel status timeout: status %08x control %08x\n",
-			  I915_READ(PCH_PP_STATUS),
-			  I915_READ(PCH_PP_CONTROL));
+				I915_READ(pp_stat_reg),
+				I915_READ(pp_ctrl_reg));
 	}
 }
 
@@ -1075,9 +969,15 @@ static void ironlake_wait_panel_power_cycle(struct intel_dp *intel_dp)
  * is locked
  */
 
-static  u32 ironlake_get_pp_control(struct drm_i915_private *dev_priv)
+static  u32 ironlake_get_pp_control(struct intel_dp *intel_dp)
 {
-	u32	control = I915_READ(PCH_PP_CONTROL);
+	struct drm_device *dev = intel_dp_to_dev(intel_dp);
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 control;
+	u32 pp_ctrl_reg;
+
+	pp_ctrl_reg = IS_VALLEYVIEW(dev) ? PIPEA_PP_CONTROL : PCH_PP_CONTROL;
+	control = I915_READ(pp_ctrl_reg);
 
 	control &= ~PANEL_UNLOCK_MASK;
 	control |= PANEL_UNLOCK_REGS;
@@ -1089,6 +989,7 @@ void ironlake_edp_panel_vdd_on(struct intel_dp *intel_dp)
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 pp;
+	u32 pp_stat_reg, pp_ctrl_reg;
 
 	if (!is_edp(intel_dp))
 		return;
@@ -1107,13 +1008,16 @@ void ironlake_edp_panel_vdd_on(struct intel_dp *intel_dp)
 	if (!ironlake_edp_have_panel_power(intel_dp))
 		ironlake_wait_panel_power_cycle(intel_dp);
 
-	pp = ironlake_get_pp_control(dev_priv);
+	pp = ironlake_get_pp_control(intel_dp);
 	pp |= EDP_FORCE_VDD;
-	I915_WRITE(PCH_PP_CONTROL, pp);
-	POSTING_READ(PCH_PP_CONTROL);
-	DRM_DEBUG_KMS("PCH_PP_STATUS: 0x%08x PCH_PP_CONTROL: 0x%08x\n",
-		      I915_READ(PCH_PP_STATUS), I915_READ(PCH_PP_CONTROL));
 
+	pp_stat_reg = IS_VALLEYVIEW(dev) ? PIPEA_PP_STATUS : PCH_PP_STATUS;
+	pp_ctrl_reg = IS_VALLEYVIEW(dev) ? PIPEA_PP_CONTROL : PCH_PP_CONTROL;
+
+	I915_WRITE(pp_ctrl_reg, pp);
+	POSTING_READ(pp_ctrl_reg);
+	DRM_DEBUG_KMS("PP_STATUS: 0x%08x PP_CONTROL: 0x%08x\n",
+			I915_READ(pp_stat_reg), I915_READ(pp_ctrl_reg));
 	/*
 	 * If the panel wasn't on, delay before accessing aux channel
 	 */
@@ -1128,19 +1032,23 @@ static void ironlake_panel_vdd_off_sync(struct intel_dp *intel_dp)
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 pp;
+	u32 pp_stat_reg, pp_ctrl_reg;
 
 	WARN_ON(!mutex_is_locked(&dev->mode_config.mutex));
 
 	if (!intel_dp->want_panel_vdd && ironlake_edp_have_panel_vdd(intel_dp)) {
-		pp = ironlake_get_pp_control(dev_priv);
+		pp = ironlake_get_pp_control(intel_dp);
 		pp &= ~EDP_FORCE_VDD;
-		I915_WRITE(PCH_PP_CONTROL, pp);
-		POSTING_READ(PCH_PP_CONTROL);
 
-		/* Make sure sequencer is idle before allowing subsequent activity */
-		DRM_DEBUG_KMS("PCH_PP_STATUS: 0x%08x PCH_PP_CONTROL: 0x%08x\n",
-			      I915_READ(PCH_PP_STATUS), I915_READ(PCH_PP_CONTROL));
+		pp_stat_reg = IS_VALLEYVIEW(dev) ? PIPEA_PP_STATUS : PCH_PP_STATUS;
+		pp_ctrl_reg = IS_VALLEYVIEW(dev) ? PIPEA_PP_CONTROL : PCH_PP_CONTROL;
+
+		I915_WRITE(pp_ctrl_reg, pp);
+		POSTING_READ(pp_ctrl_reg);
 
+		/* Make sure sequencer is idle before allowing subsequent activity */
+		DRM_DEBUG_KMS("PP_STATUS: 0x%08x PP_CONTROL: 0x%08x\n",
+		I915_READ(pp_stat_reg), I915_READ(pp_ctrl_reg));
 		msleep(intel_dp->panel_power_down_delay);
 	}
 }
@@ -1184,6 +1092,7 @@ void ironlake_edp_panel_on(struct intel_dp *intel_dp)
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 pp;
+	u32 pp_ctrl_reg;
 
 	if (!is_edp(intel_dp))
 		return;
@@ -1197,7 +1106,7 @@ void ironlake_edp_panel_on(struct intel_dp *intel_dp)
 
 	ironlake_wait_panel_power_cycle(intel_dp);
 
-	pp = ironlake_get_pp_control(dev_priv);
+	pp = ironlake_get_pp_control(intel_dp);
 	if (IS_GEN5(dev)) {
 		/* ILK workaround: disable reset around power sequence */
 		pp &= ~PANEL_POWER_RESET;
@@ -1209,8 +1118,10 @@ void ironlake_edp_panel_on(struct intel_dp *intel_dp)
 	if (!IS_GEN5(dev))
 		pp |= PANEL_POWER_RESET;
 
-	I915_WRITE(PCH_PP_CONTROL, pp);
-	POSTING_READ(PCH_PP_CONTROL);
+	pp_ctrl_reg = IS_VALLEYVIEW(dev) ? PIPEA_PP_CONTROL : PCH_PP_CONTROL;
+
+	I915_WRITE(pp_ctrl_reg, pp);
+	POSTING_READ(pp_ctrl_reg);
 
 	ironlake_wait_panel_on(intel_dp);
 
@@ -1226,6 +1137,7 @@ void ironlake_edp_panel_off(struct intel_dp *intel_dp)
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 pp;
+	u32 pp_ctrl_reg;
 
 	if (!is_edp(intel_dp))
 		return;
@@ -1234,12 +1146,15 @@ void ironlake_edp_panel_off(struct intel_dp *intel_dp)
 
 	WARN(!intel_dp->want_panel_vdd, "Need VDD to turn off panel\n");
 
-	pp = ironlake_get_pp_control(dev_priv);
+	pp = ironlake_get_pp_control(intel_dp);
 	/* We need to switch off panel power _and_ force vdd, for otherwise some
 	 * panels get very unhappy and cease to work. */
 	pp &= ~(POWER_TARGET_ON | EDP_FORCE_VDD | PANEL_POWER_RESET | EDP_BLC_ENABLE);
-	I915_WRITE(PCH_PP_CONTROL, pp);
-	POSTING_READ(PCH_PP_CONTROL);
+
+	pp_ctrl_reg = IS_VALLEYVIEW(dev) ? PIPEA_PP_CONTROL : PCH_PP_CONTROL;
+
+	I915_WRITE(pp_ctrl_reg, pp);
+	POSTING_READ(pp_ctrl_reg);
 
 	intel_dp->want_panel_vdd = false;
 
@@ -1253,6 +1168,7 @@ void ironlake_edp_backlight_on(struct intel_dp *intel_dp)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int pipe = to_intel_crtc(intel_dig_port->base.base.crtc)->pipe;
 	u32 pp;
+	u32 pp_ctrl_reg;
 
 	if (!is_edp(intel_dp))
 		return;
@@ -1265,10 +1181,13 @@ void ironlake_edp_backlight_on(struct intel_dp *intel_dp)
 	 * allowing it to appear.
 	 */
 	msleep(intel_dp->backlight_on_delay);
-	pp = ironlake_get_pp_control(dev_priv);
+	pp = ironlake_get_pp_control(intel_dp);
 	pp |= EDP_BLC_ENABLE;
-	I915_WRITE(PCH_PP_CONTROL, pp);
-	POSTING_READ(PCH_PP_CONTROL);
+
+	pp_ctrl_reg = IS_VALLEYVIEW(dev) ? PIPEA_PP_CONTROL : PCH_PP_CONTROL;
+
+	I915_WRITE(pp_ctrl_reg, pp);
+	POSTING_READ(pp_ctrl_reg);
 
 	intel_panel_enable_backlight(dev, pipe);
 }
@@ -1278,6 +1197,7 @@ void ironlake_edp_backlight_off(struct intel_dp *intel_dp)
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 pp;
+	u32 pp_ctrl_reg;
 
 	if (!is_edp(intel_dp))
 		return;
@@ -1285,10 +1205,13 @@ void ironlake_edp_backlight_off(struct intel_dp *intel_dp)
 	intel_panel_disable_backlight(dev);
 
 	DRM_DEBUG_KMS("\n");
-	pp = ironlake_get_pp_control(dev_priv);
+	pp = ironlake_get_pp_control(intel_dp);
 	pp &= ~EDP_BLC_ENABLE;
-	I915_WRITE(PCH_PP_CONTROL, pp);
-	POSTING_READ(PCH_PP_CONTROL);
+
+	pp_ctrl_reg = IS_VALLEYVIEW(dev) ? PIPEA_PP_CONTROL : PCH_PP_CONTROL;
+
+	I915_WRITE(pp_ctrl_reg, pp);
+	POSTING_READ(pp_ctrl_reg);
 	msleep(intel_dp->backlight_off_delay);
 }
 
@@ -1384,7 +1307,7 @@ static bool intel_dp_get_hw_state(struct intel_encoder *encoder,
 	if (!(tmp & DP_PORT_EN))
 		return false;
 
-	if (is_cpu_edp(intel_dp) && IS_GEN7(dev)) {
+	if (is_cpu_edp(intel_dp) && IS_GEN7(dev) && !IS_VALLEYVIEW(dev)) {
 		*pipe = PORT_TO_PIPE_CPT(tmp);
 	} else if (!HAS_PCH_CPT(dev) || is_cpu_edp(intel_dp)) {
 		*pipe = PORT_TO_PIPE(tmp);
@@ -1441,10 +1364,12 @@ static void intel_disable_dp(struct intel_encoder *encoder)
 static void intel_post_disable_dp(struct intel_encoder *encoder)
 {
 	struct intel_dp *intel_dp = enc_to_intel_dp(&encoder->base);
+	struct drm_device *dev = encoder->base.dev;
 
 	if (is_cpu_edp(intel_dp)) {
 		intel_dp_link_down(intel_dp);
-		ironlake_edp_pll_off(intel_dp);
+		if (!IS_VALLEYVIEW(dev))
+			ironlake_edp_pll_off(intel_dp);
 	}
 }
 
@@ -1470,8 +1395,9 @@ static void intel_enable_dp(struct intel_encoder *encoder)
 static void intel_pre_enable_dp(struct intel_encoder *encoder)
 {
 	struct intel_dp *intel_dp = enc_to_intel_dp(&encoder->base);
+	struct drm_device *dev = encoder->base.dev;
 
-	if (is_cpu_edp(intel_dp))
+	if (is_cpu_edp(intel_dp) && !IS_VALLEYVIEW(dev))
 		ironlake_edp_pll_on(intel_dp);
 }
 
@@ -1548,7 +1474,7 @@ intel_dp_pre_emphasis_max(struct intel_dp *intel_dp, uint8_t voltage_swing)
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 
-	if (IS_HASWELL(dev)) {
+	if (HAS_DDI(dev)) {
 		switch (voltage_swing & DP_TRAIN_VOLTAGE_SWING_MASK) {
 		case DP_TRAIN_VOLTAGE_SWING_400:
 			return DP_TRAIN_PRE_EMPHASIS_9_5;
@@ -1756,7 +1682,7 @@ intel_dp_set_signal_levels(struct intel_dp *intel_dp, uint32_t *DP)
 	uint32_t signal_levels, mask;
 	uint8_t train_set = intel_dp->train_set[0];
 
-	if (IS_HASWELL(dev)) {
+	if (HAS_DDI(dev)) {
 		signal_levels = intel_hsw_signal_levels(train_set);
 		mask = DDI_BUF_EMP_MASK;
 	} else if (IS_GEN7(dev) && is_cpu_edp(intel_dp) && !IS_VALLEYVIEW(dev)) {
@@ -1787,7 +1713,7 @@ intel_dp_set_link_train(struct intel_dp *intel_dp,
 	int ret;
 	uint32_t temp;
 
-	if (IS_HASWELL(dev)) {
+	if (HAS_DDI(dev)) {
 		temp = I915_READ(DP_TP_CTL(port));
 
 		if (dp_train_pat & DP_LINK_SCRAMBLING_DISABLE)
@@ -2311,6 +2237,16 @@ g4x_dp_detect(struct intel_dp *intel_dp)
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	uint32_t bit;
 
+	/* Can't disconnect eDP, but you can close the lid... */
+	if (is_edp(intel_dp)) {
+		enum drm_connector_status status;
+
+		status = intel_panel_detect(dev);
+		if (status == connector_status_unknown)
+			status = connector_status_connected;
+		return status;
+	}
+
 	switch (intel_dig_port->port) {
 	case PORT_B:
 		bit = PORTB_HOTPLUG_LIVE_STATUS;
@@ -2492,6 +2428,9 @@ intel_dp_set_property(struct drm_connector *connector,
 	}
 
 	if (property == dev_priv->broadcast_rgb_property) {
+		bool old_auto = intel_dp->color_range_auto;
+		uint32_t old_range = intel_dp->color_range;
+
 		switch (val) {
 		case INTEL_BROADCAST_RGB_AUTO:
 			intel_dp->color_range_auto = true;
@@ -2507,6 +2446,11 @@ intel_dp_set_property(struct drm_connector *connector,
 		default:
 			return -EINVAL;
 		}
+
+		if (old_auto == intel_dp->color_range_auto &&
+		    old_range == intel_dp->color_range)
+			return 0;
+
 		goto done;
 	}
 
@@ -2538,17 +2482,14 @@ done:
 static void
 intel_dp_destroy(struct drm_connector *connector)
 {
-	struct drm_device *dev = connector->dev;
 	struct intel_dp *intel_dp = intel_attached_dp(connector);
 	struct intel_connector *intel_connector = to_intel_connector(connector);
 
 	if (!IS_ERR_OR_NULL(intel_connector->edid))
 		kfree(intel_connector->edid);
 
-	if (is_edp(intel_dp)) {
-		intel_panel_destroy_backlight(dev);
+	if (is_edp(intel_dp))
 		intel_panel_fini(&intel_connector->panel);
-	}
 
 	drm_sysfs_connector_remove(connector);
 	drm_connector_cleanup(connector);
@@ -2573,7 +2514,6 @@ void intel_dp_encoder_destroy(struct drm_encoder *encoder)
 }
 
 static const struct drm_encoder_helper_funcs intel_dp_helper_funcs = {
-	.mode_fixup = intel_dp_mode_fixup,
 	.mode_set = intel_dp_mode_set,
 };
 
@@ -2669,15 +2609,28 @@ intel_dp_init_panel_power_sequencer(struct drm_device *dev,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct edp_power_seq cur, vbt, spec, final;
 	u32 pp_on, pp_off, pp_div, pp;
+	int pp_control_reg, pp_on_reg, pp_off_reg, pp_div_reg;
+
+	if (HAS_PCH_SPLIT(dev)) {
+		pp_control_reg = PCH_PP_CONTROL;
+		pp_on_reg = PCH_PP_ON_DELAYS;
+		pp_off_reg = PCH_PP_OFF_DELAYS;
+		pp_div_reg = PCH_PP_DIVISOR;
+	} else {
+		pp_control_reg = PIPEA_PP_CONTROL;
+		pp_on_reg = PIPEA_PP_ON_DELAYS;
+		pp_off_reg = PIPEA_PP_OFF_DELAYS;
+		pp_div_reg = PIPEA_PP_DIVISOR;
+	}
 
 	/* Workaround: Need to write PP_CONTROL with the unlock key as
 	 * the very first thing. */
-	pp = ironlake_get_pp_control(dev_priv);
-	I915_WRITE(PCH_PP_CONTROL, pp);
+	pp = ironlake_get_pp_control(intel_dp);
+	I915_WRITE(pp_control_reg, pp);
 
-	pp_on = I915_READ(PCH_PP_ON_DELAYS);
-	pp_off = I915_READ(PCH_PP_OFF_DELAYS);
-	pp_div = I915_READ(PCH_PP_DIVISOR);
+	pp_on = I915_READ(pp_on_reg);
+	pp_off = I915_READ(pp_off_reg);
+	pp_div = I915_READ(pp_div_reg);
 
 	/* Pull timing values out of registers */
 	cur.t1_t3 = (pp_on & PANEL_POWER_UP_DELAY_MASK) >>
@@ -2752,7 +2705,22 @@ intel_dp_init_panel_power_sequencer_registers(struct drm_device *dev,
 					      struct edp_power_seq *seq)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 pp_on, pp_off, pp_div;
+	u32 pp_on, pp_off, pp_div, port_sel = 0;
+	int div = HAS_PCH_SPLIT(dev) ? intel_pch_rawclk(dev) : intel_hrawclk(dev);
+	int pp_on_reg, pp_off_reg, pp_div_reg;
+
+	if (HAS_PCH_SPLIT(dev)) {
+		pp_on_reg = PCH_PP_ON_DELAYS;
+		pp_off_reg = PCH_PP_OFF_DELAYS;
+		pp_div_reg = PCH_PP_DIVISOR;
+	} else {
+		pp_on_reg = PIPEA_PP_ON_DELAYS;
+		pp_off_reg = PIPEA_PP_OFF_DELAYS;
+		pp_div_reg = PIPEA_PP_DIVISOR;
+	}
+
+	if (IS_VALLEYVIEW(dev))
+		port_sel = I915_READ(pp_on_reg) & 0xc0000000;
 
 	/* And finally store the new values in the power sequencer. */
 	pp_on = (seq->t1_t3 << PANEL_POWER_UP_DELAY_SHIFT) |
@@ -2761,8 +2729,7 @@ intel_dp_init_panel_power_sequencer_registers(struct drm_device *dev,
 		 (seq->t10 << PANEL_POWER_DOWN_DELAY_SHIFT);
 	/* Compute the divisor for the pp clock, simply match the Bspec
 	 * formula. */
-	pp_div = ((100 * intel_pch_rawclk(dev))/2 - 1)
-			<< PP_REFERENCE_DIVIDER_SHIFT;
+	pp_div = ((100 * div)/2 - 1) << PP_REFERENCE_DIVIDER_SHIFT;
 	pp_div |= (DIV_ROUND_UP(seq->t11_t12, 1000)
 			<< PANEL_POWER_CYCLE_DELAY_SHIFT);
 
@@ -2770,19 +2737,21 @@ intel_dp_init_panel_power_sequencer_registers(struct drm_device *dev,
 	 * power sequencer any more. */
 	if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev)) {
 		if (is_cpu_edp(intel_dp))
-			pp_on |= PANEL_POWER_PORT_DP_A;
+			port_sel = PANEL_POWER_PORT_DP_A;
 		else
-			pp_on |= PANEL_POWER_PORT_DP_D;
+			port_sel = PANEL_POWER_PORT_DP_D;
 	}
 
-	I915_WRITE(PCH_PP_ON_DELAYS, pp_on);
-	I915_WRITE(PCH_PP_OFF_DELAYS, pp_off);
-	I915_WRITE(PCH_PP_DIVISOR, pp_div);
+	pp_on |= port_sel;
+
+	I915_WRITE(pp_on_reg, pp_on);
+	I915_WRITE(pp_off_reg, pp_off);
+	I915_WRITE(pp_div_reg, pp_div);
 
 	DRM_DEBUG_KMS("panel power sequencer register settings: PP_ON %#x, PP_OFF %#x, PP_DIV %#x\n",
-		      I915_READ(PCH_PP_ON_DELAYS),
-		      I915_READ(PCH_PP_OFF_DELAYS),
-		      I915_READ(PCH_PP_DIVISOR));
+		      I915_READ(pp_on_reg),
+		      I915_READ(pp_off_reg),
+		      I915_READ(pp_div_reg));
 }
 
 void
@@ -2829,7 +2798,6 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	drm_connector_init(dev, connector, &intel_dp_connector_funcs, type);
 	drm_connector_helper_add(connector, &intel_dp_connector_helper_funcs);
 
-	connector->polled = DRM_CONNECTOR_POLL_HPD;
 	connector->interlace_allowed = true;
 	connector->doublescan_allowed = 0;
 
@@ -2844,27 +2812,46 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	else
 		intel_connector->get_hw_state = intel_connector_get_hw_state;
 
+	intel_dp->aux_ch_ctl_reg = intel_dp->output_reg + 0x10;
+	if (HAS_DDI(dev)) {
+		switch (intel_dig_port->port) {
+		case PORT_A:
+			intel_dp->aux_ch_ctl_reg = DPA_AUX_CH_CTL;
+			break;
+		case PORT_B:
+			intel_dp->aux_ch_ctl_reg = PCH_DPB_AUX_CH_CTL;
+			break;
+		case PORT_C:
+			intel_dp->aux_ch_ctl_reg = PCH_DPC_AUX_CH_CTL;
+			break;
+		case PORT_D:
+			intel_dp->aux_ch_ctl_reg = PCH_DPD_AUX_CH_CTL;
+			break;
+		default:
+			BUG();
+		}
+	}
 
 	/* Set up the DDC bus. */
 	switch (port) {
 	case PORT_A:
+		intel_encoder->hpd_pin = HPD_PORT_A;
 		name = "DPDDC-A";
 		break;
 	case PORT_B:
-		dev_priv->hotplug_supported_mask |= PORTB_HOTPLUG_INT_STATUS;
+		intel_encoder->hpd_pin = HPD_PORT_B;
 		name = "DPDDC-B";
 		break;
 	case PORT_C:
-		dev_priv->hotplug_supported_mask |= PORTC_HOTPLUG_INT_STATUS;
+		intel_encoder->hpd_pin = HPD_PORT_C;
 		name = "DPDDC-C";
 		break;
 	case PORT_D:
-		dev_priv->hotplug_supported_mask |= PORTD_HOTPLUG_INT_STATUS;
+		intel_encoder->hpd_pin = HPD_PORT_D;
 		name = "DPDDC-D";
 		break;
 	default:
-		WARN(1, "Invalid port %c\n", port_name(port));
-		break;
+		BUG();
 	}
 
 	if (is_edp(intel_dp))
@@ -2974,6 +2961,7 @@ intel_dp_init(struct drm_device *dev, int output_reg, enum port port)
 			 DRM_MODE_ENCODER_TMDS);
 	drm_encoder_helper_add(&intel_encoder->base, &intel_dp_helper_funcs);
 
+	intel_encoder->compute_config = intel_dp_compute_config;
 	intel_encoder->enable = intel_enable_dp;
 	intel_encoder->pre_enable = intel_pre_enable_dp;
 	intel_encoder->disable = intel_disable_dp;
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index 07ebac6fe8ca..b5b6d19e6dd3 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -33,12 +33,21 @@
 #include <drm/drm_fb_helper.h>
 #include <drm/drm_dp_helper.h>
 
+/**
+ * _wait_for - magic (register) wait macro
+ *
+ * Does the right thing for modeset paths when run under kdgb or similar atomic
+ * contexts. Note that it's important that we check the condition again after
+ * having timed out, since the timeout could be due to preemption or similar and
+ * we've never had a chance to check the condition before the timeout.
+ */
 #define _wait_for(COND, MS, W) ({ \
-	unsigned long timeout__ = jiffies + msecs_to_jiffies(MS);	\
+	unsigned long timeout__ = jiffies + msecs_to_jiffies(MS) + 1;	\
 	int ret__ = 0;							\
 	while (!(COND)) {						\
 		if (time_after(jiffies, timeout__)) {			\
-			ret__ = -ETIMEDOUT;				\
+			if (!(COND))					\
+				ret__ = -ETIMEDOUT;			\
 			break;						\
 		}							\
 		if (W && drm_can_sleep())  {				\
@@ -50,21 +59,10 @@
 	ret__;								\
 })
 
-#define wait_for_atomic_us(COND, US) ({ \
-	unsigned long timeout__ = jiffies + usecs_to_jiffies(US);	\
-	int ret__ = 0;							\
-	while (!(COND)) {						\
-		if (time_after(jiffies, timeout__)) {			\
-			ret__ = -ETIMEDOUT;				\
-			break;						\
-		}							\
-		cpu_relax();						\
-	}								\
-	ret__;								\
-})
-
 #define wait_for(COND, MS) _wait_for(COND, MS, 1)
 #define wait_for_atomic(COND, MS) _wait_for(COND, MS, 0)
+#define wait_for_atomic_us(COND, US) _wait_for((COND), \
+					       DIV_ROUND_UP((US), 1000), 0)
 
 #define KHz(x) (1000*x)
 #define MHz(x) KHz(1000*x)
@@ -101,34 +99,6 @@
 #define INTEL_DVO_CHIP_TMDS 2
 #define INTEL_DVO_CHIP_TVOUT 4
 
-/* drm_display_mode->private_flags */
-#define INTEL_MODE_PIXEL_MULTIPLIER_SHIFT (0x0)
-#define INTEL_MODE_PIXEL_MULTIPLIER_MASK (0xf << INTEL_MODE_PIXEL_MULTIPLIER_SHIFT)
-#define INTEL_MODE_DP_FORCE_6BPC (0x10)
-/* This flag must be set by the encoder's mode_fixup if it changes the crtc
- * timings in the mode to prevent the crtc fixup from overwriting them.
- * Currently only lvds needs that. */
-#define INTEL_MODE_CRTC_TIMINGS_SET (0x20)
-/*
- * Set when limited 16-235 (as opposed to full 0-255) RGB color range is
- * to be used.
- */
-#define INTEL_MODE_LIMITED_COLOR_RANGE (0x40)
-
-static inline void
-intel_mode_set_pixel_multiplier(struct drm_display_mode *mode,
-				int multiplier)
-{
-	mode->clock *= multiplier;
-	mode->private_flags |= multiplier;
-}
-
-static inline int
-intel_mode_get_pixel_multiplier(const struct drm_display_mode *mode)
-{
-	return (mode->private_flags & INTEL_MODE_PIXEL_MULTIPLIER_MASK) >> INTEL_MODE_PIXEL_MULTIPLIER_SHIFT;
-}
-
 struct intel_framebuffer {
 	struct drm_framebuffer base;
 	struct drm_i915_gem_object *obj;
@@ -158,9 +128,12 @@ struct intel_encoder {
 	bool cloneable;
 	bool connectors_active;
 	void (*hot_plug)(struct intel_encoder *);
+	bool (*compute_config)(struct intel_encoder *,
+			       struct intel_crtc_config *);
 	void (*pre_pll_enable)(struct intel_encoder *);
 	void (*pre_enable)(struct intel_encoder *);
 	void (*enable)(struct intel_encoder *);
+	void (*mode_set)(struct intel_encoder *intel_encoder);
 	void (*disable)(struct intel_encoder *);
 	void (*post_disable)(struct intel_encoder *);
 	/* Read out the current hw state of this connector, returning true if
@@ -168,6 +141,7 @@ struct intel_encoder {
 	 * it is connected to in the pipe parameter. */
 	bool (*get_hw_state)(struct intel_encoder *, enum pipe *pipe);
 	int crtc_mask;
+	enum hpd_pin hpd_pin;
 };
 
 struct intel_panel {
@@ -197,13 +171,65 @@ struct intel_connector {
 
 	/* Cached EDID for eDP and LVDS. May hold ERR_PTR for invalid EDID. */
 	struct edid *edid;
+
+	/* since POLL and HPD connectors may use the same HPD line keep the native
+	   state of connector->polled in case hotplug storm detection changes it */
+	u8 polled;
+};
+
+struct intel_crtc_config {
+	struct drm_display_mode requested_mode;
+	struct drm_display_mode adjusted_mode;
+	/* This flag must be set by the encoder's compute_config callback if it
+	 * changes the crtc timings in the mode to prevent the crtc fixup from
+	 * overwriting them.  Currently only lvds needs that. */
+	bool timings_set;
+	/* Whether to set up the PCH/FDI. Note that we never allow sharing
+	 * between pch encoders and cpu encoders. */
+	bool has_pch_encoder;
+
+	/* CPU Transcoder for the pipe. Currently this can only differ from the
+	 * pipe on Haswell (where we have a special eDP transcoder). */
+	enum transcoder cpu_transcoder;
+
+	/*
+	 * Use reduced/limited/broadcast rbg range, compressing from the full
+	 * range fed into the crtcs.
+	 */
+	bool limited_color_range;
+
+	/* DP has a bunch of special case unfortunately, so mark the pipe
+	 * accordingly. */
+	bool has_dp_encoder;
+	bool dither;
+
+	/* Controls for the clock computation, to override various stages. */
+	bool clock_set;
+
+	/* Settings for the intel dpll used on pretty much everything but
+	 * haswell. */
+	struct dpll {
+		unsigned n;
+		unsigned m1, m2;
+		unsigned p1, p2;
+	} dpll;
+
+	int pipe_bpp;
+	struct intel_link_m_n dp_m_n;
+	/**
+	 * This is currently used by DP and HDMI encoders since those can have a
+	 * target pixel clock != the port link clock (which is currently stored
+	 * in adjusted_mode->clock).
+	 */
+	int pixel_target_clock;
+	/* Used by SDVO (and if we ever fix it, HDMI). */
+	unsigned pixel_multiplier;
 };
 
 struct intel_crtc {
 	struct drm_crtc base;
 	enum pipe pipe;
 	enum plane plane;
-	enum transcoder cpu_transcoder;
 	u8 lut_r[256], lut_g[256], lut_b[256];
 	/*
 	 * Whether the crtc and the connected output pipeline is active. Implies
@@ -230,7 +256,8 @@ struct intel_crtc {
 	int16_t cursor_x, cursor_y;
 	int16_t cursor_width, cursor_height;
 	bool cursor_visible;
-	unsigned int bpp;
+
+	struct intel_crtc_config config;
 
 	/* We can share PLLs across outputs if the timings match */
 	struct intel_pch_pll *pch_pll;
@@ -242,11 +269,16 @@ struct intel_crtc {
 
 struct intel_plane {
 	struct drm_plane base;
+	int plane;
 	enum pipe pipe;
 	struct drm_i915_gem_object *obj;
 	bool can_scale;
 	int max_downscale;
 	u32 lut_r[1024], lut_g[1024], lut_b[1024];
+	int crtc_x, crtc_y;
+	unsigned int crtc_w, crtc_h;
+	uint32_t src_x, src_y;
+	uint32_t src_w, src_h;
 	void (*update_plane)(struct drm_plane *plane,
 			     struct drm_framebuffer *fb,
 			     struct drm_i915_gem_object *obj,
@@ -347,7 +379,7 @@ struct dip_infoframe {
 } __attribute__((packed));
 
 struct intel_hdmi {
-	u32 sdvox_reg;
+	u32 hdmi_reg;
 	int ddc_bus;
 	uint32_t color_range;
 	bool color_range_auto;
@@ -366,6 +398,7 @@ struct intel_hdmi {
 
 struct intel_dp {
 	uint32_t output_reg;
+	uint32_t aux_ch_ctl_reg;
 	uint32_t DP;
 	uint8_t  link_configuration[DP_LINK_CONFIGURATION_SIZE];
 	bool has_audio;
@@ -443,13 +476,12 @@ extern void intel_attach_broadcast_rgb_property(struct drm_connector *connector)
 
 extern void intel_crt_init(struct drm_device *dev);
 extern void intel_hdmi_init(struct drm_device *dev,
-			    int sdvox_reg, enum port port);
+			    int hdmi_reg, enum port port);
 extern void intel_hdmi_init_connector(struct intel_digital_port *intel_dig_port,
 				      struct intel_connector *intel_connector);
 extern struct intel_hdmi *enc_to_intel_hdmi(struct drm_encoder *encoder);
-extern bool intel_hdmi_mode_fixup(struct drm_encoder *encoder,
-				  const struct drm_display_mode *mode,
-				  struct drm_display_mode *adjusted_mode);
+extern bool intel_hdmi_compute_config(struct intel_encoder *encoder,
+				      struct intel_crtc_config *pipe_config);
 extern void intel_dip_infoframe_csum(struct dip_infoframe *avi_if);
 extern bool intel_sdvo_init(struct drm_device *dev, uint32_t sdvo_reg,
 			    bool is_sdvob);
@@ -464,18 +496,14 @@ extern void intel_dp_init(struct drm_device *dev, int output_reg,
 			  enum port port);
 extern void intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 				    struct intel_connector *intel_connector);
-void
-intel_dp_set_m_n(struct drm_crtc *crtc, struct drm_display_mode *mode,
-		 struct drm_display_mode *adjusted_mode);
 extern void intel_dp_init_link_config(struct intel_dp *intel_dp);
 extern void intel_dp_start_link_train(struct intel_dp *intel_dp);
 extern void intel_dp_complete_link_train(struct intel_dp *intel_dp);
 extern void intel_dp_sink_dpms(struct intel_dp *intel_dp, int mode);
 extern void intel_dp_encoder_destroy(struct drm_encoder *encoder);
 extern void intel_dp_check_link_status(struct intel_dp *intel_dp);
-extern bool intel_dp_mode_fixup(struct drm_encoder *encoder,
-				const struct drm_display_mode *mode,
-				struct drm_display_mode *adjusted_mode);
+extern bool intel_dp_compute_config(struct intel_encoder *encoder,
+				    struct intel_crtc_config *pipe_config);
 extern bool intel_dpd_is_edp(struct drm_device *dev);
 extern void ironlake_edp_backlight_on(struct intel_dp *intel_dp);
 extern void ironlake_edp_backlight_off(struct intel_dp *intel_dp);
@@ -483,11 +511,8 @@ extern void ironlake_edp_panel_on(struct intel_dp *intel_dp);
 extern void ironlake_edp_panel_off(struct intel_dp *intel_dp);
 extern void ironlake_edp_panel_vdd_on(struct intel_dp *intel_dp);
 extern void ironlake_edp_panel_vdd_off(struct intel_dp *intel_dp, bool sync);
-extern void intel_edp_link_config(struct intel_encoder *, int *, int *);
-extern int intel_edp_target_clock(struct intel_encoder *,
-				  struct drm_display_mode *mode);
 extern bool intel_encoder_is_pch_edp(struct drm_encoder *encoder);
-extern int intel_plane_init(struct drm_device *dev, enum pipe pipe);
+extern int intel_plane_init(struct drm_device *dev, enum pipe pipe, int plane);
 extern void intel_flush_display_plane(struct drm_i915_private *dev_priv,
 				      enum plane plane);
 
@@ -531,6 +556,7 @@ extern bool intel_encoder_check_is_cloned(struct intel_encoder *encoder);
 extern void intel_connector_dpms(struct drm_connector *, int mode);
 extern bool intel_connector_get_hw_state(struct intel_connector *connector);
 extern void intel_modeset_check_state(struct drm_device *dev);
+extern void intel_plane_restore(struct drm_plane *plane);
 
 
 static inline struct intel_encoder *intel_attached_encoder(struct drm_connector *connector)
@@ -636,6 +662,10 @@ extern void intel_init_clock_gating(struct drm_device *dev);
 extern void intel_write_eld(struct drm_encoder *encoder,
 			    struct drm_display_mode *mode);
 extern void intel_cpt_verify_modeset(struct drm_device *dev, int pipe);
+extern void intel_cpu_transcoder_set_m_n(struct intel_crtc *crtc,
+					 struct intel_link_m_n *m_n);
+extern void intel_pch_transcoder_set_m_n(struct intel_crtc *crtc,
+					 struct intel_link_m_n *m_n);
 extern void intel_prepare_ddi(struct drm_device *dev);
 extern void hsw_fdi_link_train(struct drm_crtc *crtc);
 extern void intel_ddi_init(struct drm_device *dev, enum port port);
@@ -670,6 +700,7 @@ extern void intel_update_fbc(struct drm_device *dev);
 extern void intel_gpu_ips_init(struct drm_i915_private *dev_priv);
 extern void intel_gpu_ips_teardown(void);
 
+extern bool intel_using_power_well(struct drm_device *dev);
 extern void intel_init_power_well(struct drm_device *dev);
 extern void intel_set_power_well(struct drm_device *dev, bool enable);
 extern void intel_enable_gt_powersave(struct drm_device *dev);
@@ -681,7 +712,7 @@ extern bool intel_ddi_get_hw_state(struct intel_encoder *encoder,
 				   enum pipe *pipe);
 extern int intel_ddi_get_cdclk_freq(struct drm_i915_private *dev_priv);
 extern void intel_ddi_pll_init(struct drm_device *dev);
-extern void intel_ddi_enable_pipe_func(struct drm_crtc *crtc);
+extern void intel_ddi_enable_transcoder_func(struct drm_crtc *crtc);
 extern void intel_ddi_disable_transcoder_func(struct drm_i915_private *dev_priv,
 					      enum transcoder cpu_transcoder);
 extern void intel_ddi_enable_pipe_clock(struct intel_crtc *intel_crtc);
@@ -695,4 +726,6 @@ extern bool
 intel_ddi_connector_get_hw_state(struct intel_connector *intel_connector);
 extern void intel_ddi_fdi_disable(struct drm_crtc *crtc);
 
+extern void intel_display_handle_reset(struct drm_device *dev);
+
 #endif /* __INTEL_DRV_H__ */
diff --git a/drivers/gpu/drm/i915/intel_dvo.c b/drivers/gpu/drm/i915/intel_dvo.c
index 00e70dbe82da..cc70b16d5d42 100644
--- a/drivers/gpu/drm/i915/intel_dvo.c
+++ b/drivers/gpu/drm/i915/intel_dvo.c
@@ -448,6 +448,7 @@ void intel_dvo_init(struct drm_device *dev)
 		const struct intel_dvo_device *dvo = &intel_dvo_devices[i];
 		struct i2c_adapter *i2c;
 		int gpio;
+		bool dvoinit;
 
 		/* Allow the I2C driver info to specify the GPIO to be used in
 		 * special cases, but otherwise default to what's defined
@@ -467,7 +468,17 @@ void intel_dvo_init(struct drm_device *dev)
 		i2c = intel_gmbus_get_adapter(dev_priv, gpio);
 
 		intel_dvo->dev = *dvo;
-		if (!dvo->dev_ops->init(&intel_dvo->dev, i2c))
+
+		/* GMBUS NAK handling seems to be unstable, hence let the
+		 * transmitter detection run in bit banging mode for now.
+		 */
+		intel_gmbus_force_bit(i2c, true);
+
+		dvoinit = dvo->dev_ops->init(&intel_dvo->dev, i2c);
+
+		intel_gmbus_force_bit(i2c, false);
+
+		if (!dvoinit)
 			continue;
 
 		intel_encoder->type = INTEL_OUTPUT_DVO;
diff --git a/drivers/gpu/drm/i915/intel_fb.c b/drivers/gpu/drm/i915/intel_fb.c
index 981bdce3634e..0e19e575a1b4 100644
--- a/drivers/gpu/drm/i915/intel_fb.c
+++ b/drivers/gpu/drm/i915/intel_fb.c
@@ -150,7 +150,8 @@ static int intelfb_create(struct drm_fb_helper *helper,
 	}
 	info->screen_size = size;
 
-//	memset(info->screen_base, 0, size);
+	/* This driver doesn't need a VT switch to restore the mode on resume */
+	info->skip_vt_switch = true;
 
 	drm_fb_helper_fill_fix(info, fb->pitches[0], fb->depth);
 	drm_fb_helper_fill_var(info, &ifbdev->helper, sizes->fb_width, sizes->fb_height);
@@ -227,7 +228,7 @@ int intel_fbdev_init(struct drm_device *dev)
 	ifbdev->helper.funcs = &intel_fb_helper_funcs;
 
 	ret = drm_fb_helper_init(dev, &ifbdev->helper,
-				 dev_priv->num_pipe,
+				 INTEL_INFO(dev)->num_pipes,
 				 INTELFB_CONN_LIMIT);
 	if (ret) {
 		kfree(ifbdev);
@@ -282,6 +283,9 @@ void intel_fb_restore_mode(struct drm_device *dev)
 	struct drm_mode_config *config = &dev->mode_config;
 	struct drm_plane *plane;
 
+	if (INTEL_INFO(dev)->num_pipes == 0)
+		return;
+
 	drm_modeset_lock_all(dev);
 
 	ret = drm_fb_helper_restore_fbdev_mode(&dev_priv->fbdev->helper);
diff --git a/drivers/gpu/drm/i915/intel_hdmi.c b/drivers/gpu/drm/i915/intel_hdmi.c
index fa8ec4a26041..a9057930f2b2 100644
--- a/drivers/gpu/drm/i915/intel_hdmi.c
+++ b/drivers/gpu/drm/i915/intel_hdmi.c
@@ -50,7 +50,7 @@ assert_hdmi_port_disabled(struct intel_hdmi *intel_hdmi)
 
 	enabled_bits = HAS_DDI(dev) ? DDI_BUF_CTL_ENABLE : SDVO_ENABLE;
 
-	WARN(I915_READ(intel_hdmi->sdvox_reg) & enabled_bits,
+	WARN(I915_READ(intel_hdmi->hdmi_reg) & enabled_bits,
 	     "HDMI port enabled, expecting disabled\n");
 }
 
@@ -120,13 +120,14 @@ static u32 hsw_infoframe_enable(struct dip_infoframe *frame)
 	}
 }
 
-static u32 hsw_infoframe_data_reg(struct dip_infoframe *frame, enum pipe pipe)
+static u32 hsw_infoframe_data_reg(struct dip_infoframe *frame,
+				  enum transcoder cpu_transcoder)
 {
 	switch (frame->type) {
 	case DIP_TYPE_AVI:
-		return HSW_TVIDEO_DIP_AVI_DATA(pipe);
+		return HSW_TVIDEO_DIP_AVI_DATA(cpu_transcoder);
 	case DIP_TYPE_SPD:
-		return HSW_TVIDEO_DIP_SPD_DATA(pipe);
+		return HSW_TVIDEO_DIP_SPD_DATA(cpu_transcoder);
 	default:
 		DRM_DEBUG_DRIVER("unknown info frame type %d\n", frame->type);
 		return 0;
@@ -293,8 +294,8 @@ static void hsw_write_infoframe(struct drm_encoder *encoder,
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
-	u32 ctl_reg = HSW_TVIDEO_DIP_CTL(intel_crtc->pipe);
-	u32 data_reg = hsw_infoframe_data_reg(frame, intel_crtc->pipe);
+	u32 ctl_reg = HSW_TVIDEO_DIP_CTL(intel_crtc->config.cpu_transcoder);
+	u32 data_reg = hsw_infoframe_data_reg(frame, intel_crtc->config.cpu_transcoder);
 	unsigned int i, len = DIP_HEADER_SIZE + frame->len;
 	u32 val = I915_READ(ctl_reg);
 
@@ -332,6 +333,7 @@ static void intel_hdmi_set_avi_infoframe(struct drm_encoder *encoder,
 					 struct drm_display_mode *adjusted_mode)
 {
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
 	struct dip_infoframe avi_if = {
 		.type = DIP_TYPE_AVI,
 		.ver = DIP_VERSION_AVI,
@@ -342,7 +344,7 @@ static void intel_hdmi_set_avi_infoframe(struct drm_encoder *encoder,
 		avi_if.body.avi.YQ_CN_PR |= DIP_AVI_PR_2;
 
 	if (intel_hdmi->rgb_quant_range_selectable) {
-		if (adjusted_mode->private_flags & INTEL_MODE_LIMITED_COLOR_RANGE)
+		if (intel_crtc->config.limited_color_range)
 			avi_if.body.avi.ITC_EC_Q_SC |= DIP_AVI_RGB_QUANT_RANGE_LIMITED;
 		else
 			avi_if.body.avi.ITC_EC_Q_SC |= DIP_AVI_RGB_QUANT_RANGE_FULL;
@@ -568,7 +570,7 @@ static void hsw_set_infoframes(struct drm_encoder *encoder,
 	struct drm_i915_private *dev_priv = encoder->dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
-	u32 reg = HSW_TVIDEO_DIP_CTL(intel_crtc->pipe);
+	u32 reg = HSW_TVIDEO_DIP_CTL(intel_crtc->config.cpu_transcoder);
 	u32 val = I915_READ(reg);
 
 	assert_hdmi_port_disabled(intel_hdmi);
@@ -597,40 +599,40 @@ static void intel_hdmi_mode_set(struct drm_encoder *encoder,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
-	u32 sdvox;
+	u32 hdmi_val;
 
-	sdvox = SDVO_ENCODING_HDMI;
-	if (!HAS_PCH_SPLIT(dev))
-		sdvox |= intel_hdmi->color_range;
+	hdmi_val = SDVO_ENCODING_HDMI;
+	if (!HAS_PCH_SPLIT(dev) && !IS_VALLEYVIEW(dev))
+		hdmi_val |= intel_hdmi->color_range;
 	if (adjusted_mode->flags & DRM_MODE_FLAG_PVSYNC)
-		sdvox |= SDVO_VSYNC_ACTIVE_HIGH;
+		hdmi_val |= SDVO_VSYNC_ACTIVE_HIGH;
 	if (adjusted_mode->flags & DRM_MODE_FLAG_PHSYNC)
-		sdvox |= SDVO_HSYNC_ACTIVE_HIGH;
+		hdmi_val |= SDVO_HSYNC_ACTIVE_HIGH;
 
-	if (intel_crtc->bpp > 24)
-		sdvox |= COLOR_FORMAT_12bpc;
+	if (intel_crtc->config.pipe_bpp > 24)
+		hdmi_val |= HDMI_COLOR_FORMAT_12bpc;
 	else
-		sdvox |= COLOR_FORMAT_8bpc;
+		hdmi_val |= SDVO_COLOR_FORMAT_8bpc;
 
 	/* Required on CPT */
 	if (intel_hdmi->has_hdmi_sink && HAS_PCH_CPT(dev))
-		sdvox |= HDMI_MODE_SELECT;
+		hdmi_val |= HDMI_MODE_SELECT_HDMI;
 
 	if (intel_hdmi->has_audio) {
 		DRM_DEBUG_DRIVER("Enabling HDMI audio on pipe %c\n",
 				 pipe_name(intel_crtc->pipe));
-		sdvox |= SDVO_AUDIO_ENABLE;
-		sdvox |= SDVO_NULL_PACKETS_DURING_VSYNC;
+		hdmi_val |= SDVO_AUDIO_ENABLE;
+		hdmi_val |= HDMI_MODE_SELECT_HDMI;
 		intel_write_eld(encoder, adjusted_mode);
 	}
 
 	if (HAS_PCH_CPT(dev))
-		sdvox |= PORT_TRANS_SEL_CPT(intel_crtc->pipe);
-	else if (intel_crtc->pipe == PIPE_B)
-		sdvox |= SDVO_PIPE_B_SELECT;
+		hdmi_val |= SDVO_PIPE_SEL_CPT(intel_crtc->pipe);
+	else
+		hdmi_val |= SDVO_PIPE_SEL(intel_crtc->pipe);
 
-	I915_WRITE(intel_hdmi->sdvox_reg, sdvox);
-	POSTING_READ(intel_hdmi->sdvox_reg);
+	I915_WRITE(intel_hdmi->hdmi_reg, hdmi_val);
+	POSTING_READ(intel_hdmi->hdmi_reg);
 
 	intel_hdmi->set_infoframes(encoder, adjusted_mode);
 }
@@ -643,7 +645,7 @@ static bool intel_hdmi_get_hw_state(struct intel_encoder *encoder,
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(&encoder->base);
 	u32 tmp;
 
-	tmp = I915_READ(intel_hdmi->sdvox_reg);
+	tmp = I915_READ(intel_hdmi->hdmi_reg);
 
 	if (!(tmp & SDVO_ENABLE))
 		return false;
@@ -660,6 +662,7 @@ static void intel_enable_hdmi(struct intel_encoder *encoder)
 {
 	struct drm_device *dev = encoder->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->base.crtc);
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(&encoder->base);
 	u32 temp;
 	u32 enable_bits = SDVO_ENABLE;
@@ -667,38 +670,32 @@ static void intel_enable_hdmi(struct intel_encoder *encoder)
 	if (intel_hdmi->has_audio)
 		enable_bits |= SDVO_AUDIO_ENABLE;
 
-	temp = I915_READ(intel_hdmi->sdvox_reg);
+	temp = I915_READ(intel_hdmi->hdmi_reg);
 
 	/* HW workaround for IBX, we need to move the port to transcoder A
-	 * before disabling it. */
-	if (HAS_PCH_IBX(dev)) {
-		struct drm_crtc *crtc = encoder->base.crtc;
-		int pipe = crtc ? to_intel_crtc(crtc)->pipe : -1;
-
-		/* Restore the transcoder select bit. */
-		if (pipe == PIPE_B)
-			enable_bits |= SDVO_PIPE_B_SELECT;
-	}
+	 * before disabling it, so restore the transcoder select bit here. */
+	if (HAS_PCH_IBX(dev))
+		enable_bits |= SDVO_PIPE_SEL(intel_crtc->pipe);
 
 	/* HW workaround, need to toggle enable bit off and on for 12bpc, but
 	 * we do this anyway which shows more stable in testing.
 	 */
 	if (HAS_PCH_SPLIT(dev)) {
-		I915_WRITE(intel_hdmi->sdvox_reg, temp & ~SDVO_ENABLE);
-		POSTING_READ(intel_hdmi->sdvox_reg);
+		I915_WRITE(intel_hdmi->hdmi_reg, temp & ~SDVO_ENABLE);
+		POSTING_READ(intel_hdmi->hdmi_reg);
 	}
 
 	temp |= enable_bits;
 
-	I915_WRITE(intel_hdmi->sdvox_reg, temp);
-	POSTING_READ(intel_hdmi->sdvox_reg);
+	I915_WRITE(intel_hdmi->hdmi_reg, temp);
+	POSTING_READ(intel_hdmi->hdmi_reg);
 
 	/* HW workaround, need to write this twice for issue that may result
 	 * in first write getting masked.
 	 */
 	if (HAS_PCH_SPLIT(dev)) {
-		I915_WRITE(intel_hdmi->sdvox_reg, temp);
-		POSTING_READ(intel_hdmi->sdvox_reg);
+		I915_WRITE(intel_hdmi->hdmi_reg, temp);
+		POSTING_READ(intel_hdmi->hdmi_reg);
 	}
 }
 
@@ -710,7 +707,7 @@ static void intel_disable_hdmi(struct intel_encoder *encoder)
 	u32 temp;
 	u32 enable_bits = SDVO_ENABLE | SDVO_AUDIO_ENABLE;
 
-	temp = I915_READ(intel_hdmi->sdvox_reg);
+	temp = I915_READ(intel_hdmi->hdmi_reg);
 
 	/* HW workaround for IBX, we need to move the port to transcoder A
 	 * before disabling it. */
@@ -720,12 +717,12 @@ static void intel_disable_hdmi(struct intel_encoder *encoder)
 
 		if (temp & SDVO_PIPE_B_SELECT) {
 			temp &= ~SDVO_PIPE_B_SELECT;
-			I915_WRITE(intel_hdmi->sdvox_reg, temp);
-			POSTING_READ(intel_hdmi->sdvox_reg);
+			I915_WRITE(intel_hdmi->hdmi_reg, temp);
+			POSTING_READ(intel_hdmi->hdmi_reg);
 
 			/* Again we need to write this twice. */
-			I915_WRITE(intel_hdmi->sdvox_reg, temp);
-			POSTING_READ(intel_hdmi->sdvox_reg);
+			I915_WRITE(intel_hdmi->hdmi_reg, temp);
+			POSTING_READ(intel_hdmi->hdmi_reg);
 
 			/* Transcoder selection bits only update
 			 * effectively on vblank. */
@@ -740,21 +737,21 @@ static void intel_disable_hdmi(struct intel_encoder *encoder)
 	 * we do this anyway which shows more stable in testing.
 	 */
 	if (HAS_PCH_SPLIT(dev)) {
-		I915_WRITE(intel_hdmi->sdvox_reg, temp & ~SDVO_ENABLE);
-		POSTING_READ(intel_hdmi->sdvox_reg);
+		I915_WRITE(intel_hdmi->hdmi_reg, temp & ~SDVO_ENABLE);
+		POSTING_READ(intel_hdmi->hdmi_reg);
 	}
 
 	temp &= ~enable_bits;
 
-	I915_WRITE(intel_hdmi->sdvox_reg, temp);
-	POSTING_READ(intel_hdmi->sdvox_reg);
+	I915_WRITE(intel_hdmi->hdmi_reg, temp);
+	POSTING_READ(intel_hdmi->hdmi_reg);
 
 	/* HW workaround, need to write this twice for issue that may result
 	 * in first write getting masked.
 	 */
 	if (HAS_PCH_SPLIT(dev)) {
-		I915_WRITE(intel_hdmi->sdvox_reg, temp);
-		POSTING_READ(intel_hdmi->sdvox_reg);
+		I915_WRITE(intel_hdmi->hdmi_reg, temp);
+		POSTING_READ(intel_hdmi->hdmi_reg);
 	}
 }
 
@@ -772,23 +769,40 @@ static int intel_hdmi_mode_valid(struct drm_connector *connector,
 	return MODE_OK;
 }
 
-bool intel_hdmi_mode_fixup(struct drm_encoder *encoder,
-			   const struct drm_display_mode *mode,
-			   struct drm_display_mode *adjusted_mode)
+bool intel_hdmi_compute_config(struct intel_encoder *encoder,
+			       struct intel_crtc_config *pipe_config)
 {
-	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
+	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(&encoder->base);
+	struct drm_device *dev = encoder->base.dev;
+	struct drm_display_mode *adjusted_mode = &pipe_config->adjusted_mode;
 
 	if (intel_hdmi->color_range_auto) {
 		/* See CEA-861-E - 5.1 Default Encoding Parameters */
 		if (intel_hdmi->has_hdmi_sink &&
 		    drm_match_cea_mode(adjusted_mode) > 1)
-			intel_hdmi->color_range = SDVO_COLOR_RANGE_16_235;
+			intel_hdmi->color_range = HDMI_COLOR_RANGE_16_235;
 		else
 			intel_hdmi->color_range = 0;
 	}
 
 	if (intel_hdmi->color_range)
-		adjusted_mode->private_flags |= INTEL_MODE_LIMITED_COLOR_RANGE;
+		pipe_config->limited_color_range = true;
+
+	if (HAS_PCH_SPLIT(dev) && !HAS_DDI(dev))
+		pipe_config->has_pch_encoder = true;
+
+	/*
+	 * HDMI is either 12 or 8, so if the display lets 10bpc sneak
+	 * through, clamp it down. Note that g4x/vlv don't support 12bpc hdmi
+	 * outputs.
+	 */
+	if (pipe_config->pipe_bpp > 8*3 && HAS_PCH_SPLIT(dev)) {
+		DRM_DEBUG_KMS("forcing bpc to 12 for HDMI\n");
+		pipe_config->pipe_bpp = 12*3;
+	} else {
+		DRM_DEBUG_KMS("forcing bpc to 8 for HDMI\n");
+		pipe_config->pipe_bpp = 8*3;
+	}
 
 	return true;
 }
@@ -906,6 +920,9 @@ intel_hdmi_set_property(struct drm_connector *connector,
 	}
 
 	if (property == dev_priv->broadcast_rgb_property) {
+		bool old_auto = intel_hdmi->color_range_auto;
+		uint32_t old_range = intel_hdmi->color_range;
+
 		switch (val) {
 		case INTEL_BROADCAST_RGB_AUTO:
 			intel_hdmi->color_range_auto = true;
@@ -916,11 +933,16 @@ intel_hdmi_set_property(struct drm_connector *connector,
 			break;
 		case INTEL_BROADCAST_RGB_LIMITED:
 			intel_hdmi->color_range_auto = false;
-			intel_hdmi->color_range = SDVO_COLOR_RANGE_16_235;
+			intel_hdmi->color_range = HDMI_COLOR_RANGE_16_235;
 			break;
 		default:
 			return -EINVAL;
 		}
+
+		if (old_auto == intel_hdmi->color_range_auto &&
+		    old_range == intel_hdmi->color_range)
+			return 0;
+
 		goto done;
 	}
 
@@ -941,7 +963,6 @@ static void intel_hdmi_destroy(struct drm_connector *connector)
 }
 
 static const struct drm_encoder_helper_funcs intel_hdmi_helper_funcs = {
-	.mode_fixup = intel_hdmi_mode_fixup,
 	.mode_set = intel_hdmi_mode_set,
 };
 
@@ -985,36 +1006,36 @@ void intel_hdmi_init_connector(struct intel_digital_port *intel_dig_port,
 			   DRM_MODE_CONNECTOR_HDMIA);
 	drm_connector_helper_add(connector, &intel_hdmi_connector_helper_funcs);
 
-	connector->polled = DRM_CONNECTOR_POLL_HPD;
 	connector->interlace_allowed = 1;
 	connector->doublescan_allowed = 0;
 
 	switch (port) {
 	case PORT_B:
 		intel_hdmi->ddc_bus = GMBUS_PORT_DPB;
-		dev_priv->hotplug_supported_mask |= PORTB_HOTPLUG_INT_STATUS;
+		intel_encoder->hpd_pin = HPD_PORT_B;
 		break;
 	case PORT_C:
 		intel_hdmi->ddc_bus = GMBUS_PORT_DPC;
-		dev_priv->hotplug_supported_mask |= PORTC_HOTPLUG_INT_STATUS;
+		intel_encoder->hpd_pin = HPD_PORT_C;
 		break;
 	case PORT_D:
 		intel_hdmi->ddc_bus = GMBUS_PORT_DPD;
-		dev_priv->hotplug_supported_mask |= PORTD_HOTPLUG_INT_STATUS;
+		intel_encoder->hpd_pin = HPD_PORT_D;
 		break;
 	case PORT_A:
+		intel_encoder->hpd_pin = HPD_PORT_A;
 		/* Internal port only for eDP. */
 	default:
 		BUG();
 	}
 
-	if (!HAS_PCH_SPLIT(dev)) {
-		intel_hdmi->write_infoframe = g4x_write_infoframe;
-		intel_hdmi->set_infoframes = g4x_set_infoframes;
-	} else if (IS_VALLEYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev)) {
 		intel_hdmi->write_infoframe = vlv_write_infoframe;
 		intel_hdmi->set_infoframes = vlv_set_infoframes;
-	} else if (IS_HASWELL(dev)) {
+	} else if (!HAS_PCH_SPLIT(dev)) {
+		intel_hdmi->write_infoframe = g4x_write_infoframe;
+		intel_hdmi->set_infoframes = g4x_set_infoframes;
+	} else if (HAS_DDI(dev)) {
 		intel_hdmi->write_infoframe = hsw_write_infoframe;
 		intel_hdmi->set_infoframes = hsw_set_infoframes;
 	} else if (HAS_PCH_IBX(dev)) {
@@ -1045,7 +1066,7 @@ void intel_hdmi_init_connector(struct intel_digital_port *intel_dig_port,
 	}
 }
 
-void intel_hdmi_init(struct drm_device *dev, int sdvox_reg, enum port port)
+void intel_hdmi_init(struct drm_device *dev, int hdmi_reg, enum port port)
 {
 	struct intel_digital_port *intel_dig_port;
 	struct intel_encoder *intel_encoder;
@@ -1069,6 +1090,7 @@ void intel_hdmi_init(struct drm_device *dev, int sdvox_reg, enum port port)
 			 DRM_MODE_ENCODER_TMDS);
 	drm_encoder_helper_add(&intel_encoder->base, &intel_hdmi_helper_funcs);
 
+	intel_encoder->compute_config = intel_hdmi_compute_config;
 	intel_encoder->enable = intel_enable_hdmi;
 	intel_encoder->disable = intel_disable_hdmi;
 	intel_encoder->get_hw_state = intel_hdmi_get_hw_state;
@@ -1078,7 +1100,7 @@ void intel_hdmi_init(struct drm_device *dev, int sdvox_reg, enum port port)
 	intel_encoder->cloneable = false;
 
 	intel_dig_port->port = port;
-	intel_dig_port->hdmi.sdvox_reg = sdvox_reg;
+	intel_dig_port->hdmi.hdmi_reg = hdmi_reg;
 	intel_dig_port->dp.output_reg = 0;
 
 	intel_hdmi_init_connector(intel_dig_port, intel_connector);
diff --git a/drivers/gpu/drm/i915/intel_i2c.c b/drivers/gpu/drm/i915/intel_i2c.c
index ef4744e1bf0b..5d245031e391 100644
--- a/drivers/gpu/drm/i915/intel_i2c.c
+++ b/drivers/gpu/drm/i915/intel_i2c.c
@@ -522,7 +522,9 @@ int intel_setup_gmbus(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret, i;
 
-	if (HAS_PCH_SPLIT(dev))
+	if (HAS_PCH_NOP(dev))
+		return 0;
+	else if (HAS_PCH_SPLIT(dev))
 		dev_priv->gpio_mmio_base = PCH_GPIOA - GPIOA;
 	else if (IS_VALLEYVIEW(dev))
 		dev_priv->gpio_mmio_base = VLV_DISPLAY_BASE;
diff --git a/drivers/gpu/drm/i915/intel_lvds.c b/drivers/gpu/drm/i915/intel_lvds.c
index 3d1d97488cc9..f36f1baabd5a 100644
--- a/drivers/gpu/drm/i915/intel_lvds.c
+++ b/drivers/gpu/drm/i915/intel_lvds.c
@@ -261,8 +261,6 @@ centre_horizontally(struct drm_display_mode *mode,
 
 	mode->crtc_hsync_start = mode->crtc_hblank_start + sync_pos;
 	mode->crtc_hsync_end = mode->crtc_hsync_start + sync_width;
-
-	mode->private_flags |= INTEL_MODE_CRTC_TIMINGS_SET;
 }
 
 static void
@@ -284,8 +282,6 @@ centre_vertically(struct drm_display_mode *mode,
 
 	mode->crtc_vsync_start = mode->crtc_vblank_start + sync_pos;
 	mode->crtc_vsync_end = mode->crtc_vsync_start + sync_width;
-
-	mode->private_flags |= INTEL_MODE_CRTC_TIMINGS_SET;
 }
 
 static inline u32 panel_fitter_scaling(u32 source, u32 target)
@@ -301,17 +297,20 @@ static inline u32 panel_fitter_scaling(u32 source, u32 target)
 	return (FACTOR * ratio + FACTOR/2) / FACTOR;
 }
 
-static bool intel_lvds_mode_fixup(struct drm_encoder *encoder,
-				  const struct drm_display_mode *mode,
-				  struct drm_display_mode *adjusted_mode)
+static bool intel_lvds_compute_config(struct intel_encoder *intel_encoder,
+				      struct intel_crtc_config *pipe_config)
 {
-	struct drm_device *dev = encoder->dev;
+	struct drm_device *dev = intel_encoder->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_lvds_encoder *lvds_encoder = to_lvds_encoder(encoder);
+	struct intel_lvds_encoder *lvds_encoder =
+		to_lvds_encoder(&intel_encoder->base);
 	struct intel_connector *intel_connector =
 		&lvds_encoder->attached_connector->base;
+	struct drm_display_mode *adjusted_mode = &pipe_config->adjusted_mode;
+	struct drm_display_mode *mode = &pipe_config->requested_mode;
 	struct intel_crtc *intel_crtc = lvds_encoder->base.new_crtc;
 	u32 pfit_control = 0, pfit_pgm_ratios = 0, border = 0;
+	unsigned int lvds_bpp;
 	int pipe;
 
 	/* Should never happen!! */
@@ -323,6 +322,17 @@ static bool intel_lvds_mode_fixup(struct drm_encoder *encoder,
 	if (intel_encoder_check_is_cloned(&lvds_encoder->base))
 		return false;
 
+	if ((I915_READ(lvds_encoder->reg) & LVDS_A3_POWER_MASK) ==
+	    LVDS_A3_POWER_UP)
+		lvds_bpp = 8*3;
+	else
+		lvds_bpp = 6*3;
+
+	if (lvds_bpp != pipe_config->pipe_bpp) {
+		DRM_DEBUG_KMS("forcing display bpp (was %d) to LVDS (%d)\n",
+			      pipe_config->pipe_bpp, lvds_bpp);
+		pipe_config->pipe_bpp = lvds_bpp;
+	}
 	/*
 	 * We have timings from the BIOS for the panel, put them in
 	 * to the adjusted mode.  The CRTC will be set up for this mode,
@@ -333,6 +343,8 @@ static bool intel_lvds_mode_fixup(struct drm_encoder *encoder,
 			       adjusted_mode);
 
 	if (HAS_PCH_SPLIT(dev)) {
+		pipe_config->has_pch_encoder = true;
+
 		intel_pch_panel_fitting(dev,
 					intel_connector->panel.fitting_mode,
 					mode, adjusted_mode);
@@ -359,6 +371,7 @@ static bool intel_lvds_mode_fixup(struct drm_encoder *encoder,
 		I915_WRITE(BCLRPAT(pipe), 0);
 
 	drm_mode_set_crtcinfo(adjusted_mode, 0);
+	pipe_config->timings_set = true;
 
 	switch (intel_connector->panel.fitting_mode) {
 	case DRM_MODE_SCALE_CENTER:
@@ -618,7 +631,6 @@ static void intel_lvds_destroy(struct drm_connector *connector)
 	if (!IS_ERR_OR_NULL(lvds_connector->base.edid))
 		kfree(lvds_connector->base.edid);
 
-	intel_panel_destroy_backlight(connector->dev);
 	intel_panel_fini(&lvds_connector->base.panel);
 
 	drm_sysfs_connector_remove(connector);
@@ -661,7 +673,6 @@ static int intel_lvds_set_property(struct drm_connector *connector,
 }
 
 static const struct drm_encoder_helper_funcs intel_lvds_helper_funcs = {
-	.mode_fixup = intel_lvds_mode_fixup,
 	.mode_set = intel_lvds_mode_set,
 };
 
@@ -850,6 +861,14 @@ static const struct dmi_system_id intel_no_lvds[] = {
 			DMI_MATCH(DMI_PRODUCT_NAME, "X7SPA-H"),
 		},
 	},
+	{
+		.callback = intel_no_lvds_dmi_callback,
+		.ident = "Fujitsu Esprimo Q900",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "ESPRIMO Q900"),
+		},
+	},
 
 	{ }	/* terminating entry */
 };
@@ -1019,12 +1038,15 @@ static bool intel_lvds_supported(struct drm_device *dev)
 {
 	/* With the introduction of the PCH we gained a dedicated
 	 * LVDS presence pin, use it. */
-	if (HAS_PCH_SPLIT(dev))
+	if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev))
 		return true;
 
 	/* Otherwise LVDS was only attached to mobile products,
 	 * except for the inglorious 830gm */
-	return IS_MOBILE(dev) && !IS_I830(dev);
+	if (INTEL_INFO(dev)->gen <= 4 && IS_MOBILE(dev) && !IS_I830(dev))
+		return true;
+
+	return false;
 }
 
 /**
@@ -1102,6 +1124,7 @@ bool intel_lvds_init(struct drm_device *dev)
 	intel_encoder->enable = intel_enable_lvds;
 	intel_encoder->pre_enable = intel_pre_enable_lvds;
 	intel_encoder->pre_pll_enable = intel_pre_pll_enable_lvds;
+	intel_encoder->compute_config = intel_lvds_compute_config;
 	intel_encoder->disable = intel_disable_lvds;
 	intel_encoder->get_hw_state = intel_lvds_get_hw_state;
 	intel_connector->get_hw_state = intel_connector_get_hw_state;
diff --git a/drivers/gpu/drm/i915/intel_opregion.c b/drivers/gpu/drm/i915/intel_opregion.c
index 4d338740f2cb..a8117e614009 100644
--- a/drivers/gpu/drm/i915/intel_opregion.c
+++ b/drivers/gpu/drm/i915/intel_opregion.c
@@ -350,11 +350,11 @@ static void intel_didl_outputs(struct drm_device *dev)
 	if (!handle || acpi_bus_get_device(handle, &acpi_dev))
 		return;
 
-	if (acpi_is_video_device(acpi_dev))
+	if (acpi_is_video_device(handle))
 		acpi_video_bus = acpi_dev;
 	else {
 		list_for_each_entry(acpi_cdev, &acpi_dev->children, node) {
-			if (acpi_is_video_device(acpi_cdev)) {
+			if (acpi_is_video_device(acpi_cdev->handle)) {
 				acpi_video_bus = acpi_cdev;
 				break;
 			}
diff --git a/drivers/gpu/drm/i915/intel_panel.c b/drivers/gpu/drm/i915/intel_panel.c
index bee8cb6108a7..eb5e6e95f3c7 100644
--- a/drivers/gpu/drm/i915/intel_panel.c
+++ b/drivers/gpu/drm/i915/intel_panel.c
@@ -286,8 +286,11 @@ void intel_panel_set_backlight(struct drm_device *dev, u32 level)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	dev_priv->backlight_level = level;
-	if (dev_priv->backlight_enabled)
+	dev_priv->backlight.level = level;
+	if (dev_priv->backlight.device)
+		dev_priv->backlight.device->props.brightness = level;
+
+	if (dev_priv->backlight.enabled)
 		intel_panel_actually_set_backlight(dev, level);
 }
 
@@ -295,7 +298,7 @@ void intel_panel_disable_backlight(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	dev_priv->backlight_enabled = false;
+	dev_priv->backlight.enabled = false;
 	intel_panel_actually_set_backlight(dev, 0);
 
 	if (INTEL_INFO(dev)->gen >= 4) {
@@ -318,8 +321,12 @@ void intel_panel_enable_backlight(struct drm_device *dev,
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	if (dev_priv->backlight_level == 0)
-		dev_priv->backlight_level = intel_panel_get_max_backlight(dev);
+	if (dev_priv->backlight.level == 0) {
+		dev_priv->backlight.level = intel_panel_get_max_backlight(dev);
+		if (dev_priv->backlight.device)
+			dev_priv->backlight.device->props.brightness =
+				dev_priv->backlight.level;
+	}
 
 	if (INTEL_INFO(dev)->gen >= 4) {
 		uint32_t reg, tmp;
@@ -335,7 +342,7 @@ void intel_panel_enable_backlight(struct drm_device *dev,
 		if (tmp & BLM_PWM_ENABLE)
 			goto set_level;
 
-		if (dev_priv->num_pipe == 3)
+		if (INTEL_INFO(dev)->num_pipes == 3)
 			tmp &= ~BLM_PIPE_SELECT_IVB;
 		else
 			tmp &= ~BLM_PIPE_SELECT;
@@ -360,16 +367,16 @@ set_level:
 	 * BLC_PWM_CPU_CTL may be cleared to zero automatically when these
 	 * registers are set.
 	 */
-	dev_priv->backlight_enabled = true;
-	intel_panel_actually_set_backlight(dev, dev_priv->backlight_level);
+	dev_priv->backlight.enabled = true;
+	intel_panel_actually_set_backlight(dev, dev_priv->backlight.level);
 }
 
 static void intel_panel_init_backlight(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	dev_priv->backlight_level = intel_panel_get_backlight(dev);
-	dev_priv->backlight_enabled = dev_priv->backlight_level != 0;
+	dev_priv->backlight.level = intel_panel_get_backlight(dev);
+	dev_priv->backlight.enabled = dev_priv->backlight.level != 0;
 }
 
 enum drm_connector_status
@@ -405,8 +412,7 @@ static int intel_panel_update_status(struct backlight_device *bd)
 static int intel_panel_get_brightness(struct backlight_device *bd)
 {
 	struct drm_device *dev = bl_get_data(bd);
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	return dev_priv->backlight_level;
+	return intel_panel_get_backlight(dev);
 }
 
 static const struct backlight_ops intel_panel_bl_ops = {
@@ -422,33 +428,38 @@ int intel_panel_setup_backlight(struct drm_connector *connector)
 
 	intel_panel_init_backlight(dev);
 
+	if (WARN_ON(dev_priv->backlight.device))
+		return -ENODEV;
+
 	memset(&props, 0, sizeof(props));
 	props.type = BACKLIGHT_RAW;
+	props.brightness = dev_priv->backlight.level;
 	props.max_brightness = _intel_panel_get_max_backlight(dev);
 	if (props.max_brightness == 0) {
 		DRM_DEBUG_DRIVER("Failed to get maximum backlight value\n");
 		return -ENODEV;
 	}
-	dev_priv->backlight =
+	dev_priv->backlight.device =
 		backlight_device_register("intel_backlight",
 					  &connector->kdev, dev,
 					  &intel_panel_bl_ops, &props);
 
-	if (IS_ERR(dev_priv->backlight)) {
+	if (IS_ERR(dev_priv->backlight.device)) {
 		DRM_ERROR("Failed to register backlight: %ld\n",
-			  PTR_ERR(dev_priv->backlight));
-		dev_priv->backlight = NULL;
+			  PTR_ERR(dev_priv->backlight.device));
+		dev_priv->backlight.device = NULL;
 		return -ENODEV;
 	}
-	dev_priv->backlight->props.brightness = intel_panel_get_backlight(dev);
 	return 0;
 }
 
 void intel_panel_destroy_backlight(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	if (dev_priv->backlight)
-		backlight_device_unregister(dev_priv->backlight);
+	if (dev_priv->backlight.device) {
+		backlight_device_unregister(dev_priv->backlight.device);
+		dev_priv->backlight.device = NULL;
+	}
 }
 #else
 int intel_panel_setup_backlight(struct drm_connector *connector)
diff --git a/drivers/gpu/drm/i915/intel_pm.c b/drivers/gpu/drm/i915/intel_pm.c
index adca00783e61..de3b0dc5658b 100644
--- a/drivers/gpu/drm/i915/intel_pm.c
+++ b/drivers/gpu/drm/i915/intel_pm.c
@@ -2460,10 +2460,14 @@ void gen6_set_rps(struct drm_device *dev, u8 val)
 	if (val == dev_priv->rps.cur_delay)
 		return;
 
-	I915_WRITE(GEN6_RPNSWREQ,
-		   GEN6_FREQUENCY(val) |
-		   GEN6_OFFSET(0) |
-		   GEN6_AGGRESSIVE_TURBO);
+	if (IS_HASWELL(dev))
+		I915_WRITE(GEN6_RPNSWREQ,
+			   HSW_FREQUENCY(val));
+	else
+		I915_WRITE(GEN6_RPNSWREQ,
+			   GEN6_FREQUENCY(val) |
+			   GEN6_OFFSET(0) |
+			   GEN6_AGGRESSIVE_TURBO);
 
 	/* Make sure we continue to get interrupts
 	 * until we hit the minimum or maximum frequencies.
@@ -2554,8 +2558,8 @@ static void gen6_enable_rps(struct drm_device *dev)
 	rp_state_cap = I915_READ(GEN6_RP_STATE_CAP);
 	gt_perf_status = I915_READ(GEN6_GT_PERF_STATUS);
 
-	/* In units of 100MHz */
-	dev_priv->rps.max_delay = rp_state_cap & 0xff;
+	/* In units of 50MHz */
+	dev_priv->rps.hw_max = dev_priv->rps.max_delay = rp_state_cap & 0xff;
 	dev_priv->rps.min_delay = (rp_state_cap & 0xff0000) >> 16;
 	dev_priv->rps.cur_delay = 0;
 
@@ -2601,12 +2605,19 @@ static void gen6_enable_rps(struct drm_device *dev)
 		   GEN6_RC_CTL_EI_MODE(1) |
 		   GEN6_RC_CTL_HW_ENABLE);
 
-	I915_WRITE(GEN6_RPNSWREQ,
-		   GEN6_FREQUENCY(10) |
-		   GEN6_OFFSET(0) |
-		   GEN6_AGGRESSIVE_TURBO);
-	I915_WRITE(GEN6_RC_VIDEO_FREQ,
-		   GEN6_FREQUENCY(12));
+	if (IS_HASWELL(dev)) {
+		I915_WRITE(GEN6_RPNSWREQ,
+			   HSW_FREQUENCY(10));
+		I915_WRITE(GEN6_RC_VIDEO_FREQ,
+			   HSW_FREQUENCY(12));
+	} else {
+		I915_WRITE(GEN6_RPNSWREQ,
+			   GEN6_FREQUENCY(10) |
+			   GEN6_OFFSET(0) |
+			   GEN6_AGGRESSIVE_TURBO);
+		I915_WRITE(GEN6_RC_VIDEO_FREQ,
+			   GEN6_FREQUENCY(12));
+	}
 
 	I915_WRITE(GEN6_RP_DOWN_TIMEOUT, 1000000);
 	I915_WRITE(GEN6_RP_INTERRUPT_LIMITS,
@@ -2631,9 +2642,11 @@ static void gen6_enable_rps(struct drm_device *dev)
 	if (!ret) {
 		pcu_mbox = 0;
 		ret = sandybridge_pcode_read(dev_priv, GEN6_READ_OC_PARAMS, &pcu_mbox);
-		if (ret && pcu_mbox & (1<<31)) { /* OC supported */
-			dev_priv->rps.max_delay = pcu_mbox & 0xff;
-			DRM_DEBUG_DRIVER("overclocking supported, adjusting frequency max to %dMHz\n", pcu_mbox * 50);
+		if (!ret && (pcu_mbox & (1<<31))) { /* OC supported */
+			DRM_DEBUG_DRIVER("Overclocking supported. Max: %dMHz, Overclock max: %dMHz\n",
+					 (dev_priv->rps.max_delay & 0xff) * 50,
+					 (pcu_mbox & 0xff) * 50);
+			dev_priv->rps.hw_max = pcu_mbox & 0xff;
 		}
 	} else {
 		DRM_DEBUG_DRIVER("Failed to set the min frequency\n");
@@ -2671,8 +2684,8 @@ static void gen6_update_ring_freq(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int min_freq = 15;
-	int gpu_freq;
-	unsigned int ia_freq, max_ia_freq;
+	unsigned int gpu_freq;
+	unsigned int max_ia_freq, min_ring_freq;
 	int scaling_factor = 180;
 
 	WARN_ON(!mutex_is_locked(&dev_priv->rps.hw_lock));
@@ -2688,6 +2701,10 @@ static void gen6_update_ring_freq(struct drm_device *dev)
 	/* Convert from kHz to MHz */
 	max_ia_freq /= 1000;
 
+	min_ring_freq = I915_READ(MCHBAR_MIRROR_BASE_SNB + DCLK);
+	/* convert DDR frequency from units of 133.3MHz to bandwidth */
+	min_ring_freq = (2 * 4 * min_ring_freq + 2) / 3;
+
 	/*
 	 * For each potential GPU frequency, load a ring frequency we'd like
 	 * to use for memory access.  We do this by specifying the IA frequency
@@ -2696,21 +2713,32 @@ static void gen6_update_ring_freq(struct drm_device *dev)
 	for (gpu_freq = dev_priv->rps.max_delay; gpu_freq >= dev_priv->rps.min_delay;
 	     gpu_freq--) {
 		int diff = dev_priv->rps.max_delay - gpu_freq;
-
-		/*
-		 * For GPU frequencies less than 750MHz, just use the lowest
-		 * ring freq.
-		 */
-		if (gpu_freq < min_freq)
-			ia_freq = 800;
-		else
-			ia_freq = max_ia_freq - ((diff * scaling_factor) / 2);
-		ia_freq = DIV_ROUND_CLOSEST(ia_freq, 100);
-		ia_freq <<= GEN6_PCODE_FREQ_IA_RATIO_SHIFT;
+		unsigned int ia_freq = 0, ring_freq = 0;
+
+		if (IS_HASWELL(dev)) {
+			ring_freq = (gpu_freq * 5 + 3) / 4;
+			ring_freq = max(min_ring_freq, ring_freq);
+			/* leave ia_freq as the default, chosen by cpufreq */
+		} else {
+			/* On older processors, there is no separate ring
+			 * clock domain, so in order to boost the bandwidth
+			 * of the ring, we need to upclock the CPU (ia_freq).
+			 *
+			 * For GPU frequencies less than 750MHz,
+			 * just use the lowest ring freq.
+			 */
+			if (gpu_freq < min_freq)
+				ia_freq = 800;
+			else
+				ia_freq = max_ia_freq - ((diff * scaling_factor) / 2);
+			ia_freq = DIV_ROUND_CLOSEST(ia_freq, 100);
+		}
 
 		sandybridge_pcode_write(dev_priv,
 					GEN6_PCODE_WRITE_MIN_FREQ_TABLE,
-					ia_freq | gpu_freq);
+					ia_freq << GEN6_PCODE_FREQ_IA_RATIO_SHIFT |
+					ring_freq << GEN6_PCODE_FREQ_RING_RATIO_SHIFT |
+					gpu_freq);
 	}
 }
 
@@ -2821,7 +2849,7 @@ static void ironlake_enable_rc6(struct drm_device *dev)
 	ret = intel_ring_idle(ring);
 	dev_priv->mm.interruptible = was_interruptible;
 	if (ret) {
-		DRM_ERROR("failed to enable ironlake power power savings\n");
+		DRM_ERROR("failed to enable ironlake power savings\n");
 		ironlake_teardown_rc6(dev);
 		return;
 	}
@@ -3562,6 +3590,7 @@ static void cpt_init_clock_gating(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int pipe;
+	uint32_t val;
 
 	/*
 	 * On Ibex Peak and Cougar Point, we need to disable clock
@@ -3574,8 +3603,17 @@ static void cpt_init_clock_gating(struct drm_device *dev)
 	/* The below fixes the weird display corruption, a few pixels shifted
 	 * downward, on (only) LVDS of some HP laptops with IVY.
 	 */
-	for_each_pipe(pipe)
-		I915_WRITE(TRANS_CHICKEN2(pipe), TRANS_CHICKEN2_TIMING_OVERRIDE);
+	for_each_pipe(pipe) {
+		val = I915_READ(TRANS_CHICKEN2(pipe));
+		val |= TRANS_CHICKEN2_TIMING_OVERRIDE;
+		val &= ~TRANS_CHICKEN2_FDI_POLARITY_REVERSED;
+		if (dev_priv->fdi_rx_polarity_inverted)
+			val |= TRANS_CHICKEN2_FDI_POLARITY_REVERSED;
+		val &= ~TRANS_CHICKEN2_FRAME_START_DELAY_MASK;
+		val &= ~TRANS_CHICKEN2_DISABLE_DEEP_COLOR_COUNTER;
+		val &= ~TRANS_CHICKEN2_DISABLE_DEEP_COLOR_MODESWITCH;
+		I915_WRITE(TRANS_CHICKEN2(pipe), val);
+	}
 	/* WADP0ClockGatingDisable */
 	for_each_pipe(pipe) {
 		I915_WRITE(TRANS_CHICKEN1(pipe),
@@ -3768,6 +3806,9 @@ static void haswell_init_clock_gating(struct drm_device *dev)
 	I915_WRITE(GEN6_MBCTL, I915_READ(GEN6_MBCTL) |
 		   GEN6_MBCTL_ENABLE_BOOT_FETCH);
 
+	/* WaSwitchSolVfFArbitrationPriority */
+	I915_WRITE(GAM_ECOCHK, I915_READ(GAM_ECOCHK) | HSW_ECOCHK_ARB_PRIO_SOL);
+
 	/* XXX: This is a workaround for early silicon revisions and should be
 	 * removed later.
 	 */
@@ -3874,7 +3915,8 @@ static void ivybridge_init_clock_gating(struct drm_device *dev)
 	snpcr |= GEN6_MBC_SNPCR_MED;
 	I915_WRITE(GEN6_MBCUNIT_SNPCR, snpcr);
 
-	cpt_init_clock_gating(dev);
+	if (!HAS_PCH_NOP(dev))
+		cpt_init_clock_gating(dev);
 
 	gen6_check_mch_setup(dev);
 }
@@ -3899,8 +3941,10 @@ static void valleyview_init_clock_gating(struct drm_device *dev)
 		   CHICKEN3_DGMG_REQ_OUT_FIX_DISABLE |
 		   CHICKEN3_DGMG_DONE_FIX_DISABLE);
 
+	/* WaDisablePSDDualDispatchEnable */
 	I915_WRITE(GEN7_HALF_SLICE_CHICKEN1,
-		   _MASKED_BIT_ENABLE(GEN7_PSD_SINGLE_PORT_DISPATCH_ENABLE));
+		   _MASKED_BIT_ENABLE(GEN7_MAX_PS_THREAD_DEP |
+				      GEN7_PSD_SINGLE_PORT_DISPATCH_ENABLE));
 
 	/* Apply the WaDisableRHWOOptimizationForRenderHang workaround. */
 	I915_WRITE(GEN7_COMMON_SLICE_CHICKEN1,
@@ -3968,24 +4012,20 @@ static void valleyview_init_clock_gating(struct drm_device *dev)
 		   _MASKED_BIT_ENABLE(PIXEL_SUBSPAN_COLLECT_OPT_DISABLE));
 
 	/*
-	 * On ValleyView, the GUnit needs to signal the GT
-	 * when flip and other events complete.  So enable
-	 * all the GUnit->GT interrupts here
-	 */
-	I915_WRITE(VLV_DPFLIPSTAT, PIPEB_LINE_COMPARE_INT_EN |
-		   PIPEB_HLINE_INT_EN | PIPEB_VBLANK_INT_EN |
-		   SPRITED_FLIPDONE_INT_EN | SPRITEC_FLIPDONE_INT_EN |
-		   PLANEB_FLIPDONE_INT_EN | PIPEA_LINE_COMPARE_INT_EN |
-		   PIPEA_HLINE_INT_EN | PIPEA_VBLANK_INT_EN |
-		   SPRITEB_FLIPDONE_INT_EN | SPRITEA_FLIPDONE_INT_EN |
-		   PLANEA_FLIPDONE_INT_EN);
-
-	/*
 	 * WaDisableVLVClockGating_VBIIssue
 	 * Disable clock gating on th GCFG unit to prevent a delay
 	 * in the reporting of vblank events.
 	 */
-	I915_WRITE(VLV_GUNIT_CLOCK_GATE, GCFG_DIS);
+	I915_WRITE(VLV_GUNIT_CLOCK_GATE, 0xffffffff);
+
+	/* Conservative clock gating settings for now */
+	I915_WRITE(0x9400, 0xffffffff);
+	I915_WRITE(0x9404, 0xffffffff);
+	I915_WRITE(0x9408, 0xffffffff);
+	I915_WRITE(0x940c, 0xffffffff);
+	I915_WRITE(0x9410, 0xffffffff);
+	I915_WRITE(0x9414, 0xffffffff);
+	I915_WRITE(0x9418, 0xffffffff);
 }
 
 static void g4x_init_clock_gating(struct drm_device *dev)
@@ -4070,13 +4110,29 @@ void intel_init_clock_gating(struct drm_device *dev)
 	dev_priv->display.init_clock_gating(dev);
 }
 
+/**
+ * We should only use the power well if we explicitly asked the hardware to
+ * enable it, so check if it's enabled and also check if we've requested it to
+ * be enabled.
+ */
+bool intel_using_power_well(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (IS_HASWELL(dev))
+		return I915_READ(HSW_PWR_WELL_DRIVER) ==
+		       (HSW_PWR_WELL_ENABLE | HSW_PWR_WELL_STATE);
+	else
+		return true;
+}
+
 void intel_set_power_well(struct drm_device *dev, bool enable)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	bool is_enabled, enable_requested;
 	uint32_t tmp;
 
-	if (!IS_HASWELL(dev))
+	if (!HAS_POWER_WELL(dev))
 		return;
 
 	if (!i915_disable_power_well && !enable)
@@ -4114,7 +4170,7 @@ void intel_init_power_well(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	if (!IS_HASWELL(dev))
+	if (!HAS_POWER_WELL(dev))
 		return;
 
 	/* For now, we need the power well to be always enabled. */
@@ -4176,7 +4232,6 @@ void intel_init_pm(struct drm_device *dev)
 			}
 			dev_priv->display.init_clock_gating = gen6_init_clock_gating;
 		} else if (IS_IVYBRIDGE(dev)) {
-			/* FIXME: detect B0+ stepping and use auto training */
 			if (SNB_READ_WM0_LATENCY()) {
 				dev_priv->display.update_wm = ivybridge_update_wm;
 				dev_priv->display.update_sprite_wm = sandybridge_update_sprite_wm;
@@ -4274,21 +4329,14 @@ static void __gen6_gt_force_wake_reset(struct drm_i915_private *dev_priv)
 
 static void __gen6_gt_force_wake_get(struct drm_i915_private *dev_priv)
 {
-	u32 forcewake_ack;
-
-	if (IS_HASWELL(dev_priv->dev))
-		forcewake_ack = FORCEWAKE_ACK_HSW;
-	else
-		forcewake_ack = FORCEWAKE_ACK;
-
-	if (wait_for_atomic((I915_READ_NOTRACE(forcewake_ack) & 1) == 0,
+	if (wait_for_atomic((I915_READ_NOTRACE(FORCEWAKE_ACK) & 1) == 0,
 			    FORCEWAKE_ACK_TIMEOUT_MS))
 		DRM_ERROR("Timed out waiting for forcewake old ack to clear.\n");
 
-	I915_WRITE_NOTRACE(FORCEWAKE, FORCEWAKE_KERNEL);
+	I915_WRITE_NOTRACE(FORCEWAKE, 1);
 	POSTING_READ(ECOBUS); /* something from same cacheline, but !FORCEWAKE */
 
-	if (wait_for_atomic((I915_READ_NOTRACE(forcewake_ack) & 1),
+	if (wait_for_atomic((I915_READ_NOTRACE(FORCEWAKE_ACK) & 1),
 			    FORCEWAKE_ACK_TIMEOUT_MS))
 		DRM_ERROR("Timed out waiting for forcewake to ack request.\n");
 
@@ -4311,7 +4359,7 @@ static void __gen6_gt_force_wake_mt_get(struct drm_i915_private *dev_priv)
 	else
 		forcewake_ack = FORCEWAKE_MT_ACK;
 
-	if (wait_for_atomic((I915_READ_NOTRACE(forcewake_ack) & 1) == 0,
+	if (wait_for_atomic((I915_READ_NOTRACE(forcewake_ack) & FORCEWAKE_KERNEL) == 0,
 			    FORCEWAKE_ACK_TIMEOUT_MS))
 		DRM_ERROR("Timed out waiting for forcewake old ack to clear.\n");
 
@@ -4319,7 +4367,7 @@ static void __gen6_gt_force_wake_mt_get(struct drm_i915_private *dev_priv)
 	/* something from same cacheline, but !FORCEWAKE_MT */
 	POSTING_READ(ECOBUS);
 
-	if (wait_for_atomic((I915_READ_NOTRACE(forcewake_ack) & 1),
+	if (wait_for_atomic((I915_READ_NOTRACE(forcewake_ack) & FORCEWAKE_KERNEL),
 			    FORCEWAKE_ACK_TIMEOUT_MS))
 		DRM_ERROR("Timed out waiting for forcewake to ack request.\n");
 
@@ -4409,15 +4457,22 @@ static void vlv_force_wake_reset(struct drm_i915_private *dev_priv)
 
 static void vlv_force_wake_get(struct drm_i915_private *dev_priv)
 {
-	if (wait_for_atomic((I915_READ_NOTRACE(FORCEWAKE_ACK_VLV) & 1) == 0,
+	if (wait_for_atomic((I915_READ_NOTRACE(FORCEWAKE_ACK_VLV) & FORCEWAKE_KERNEL) == 0,
 			    FORCEWAKE_ACK_TIMEOUT_MS))
 		DRM_ERROR("Timed out waiting for forcewake old ack to clear.\n");
 
 	I915_WRITE_NOTRACE(FORCEWAKE_VLV, _MASKED_BIT_ENABLE(FORCEWAKE_KERNEL));
+	I915_WRITE_NOTRACE(FORCEWAKE_MEDIA_VLV,
+			   _MASKED_BIT_ENABLE(FORCEWAKE_KERNEL));
 
-	if (wait_for_atomic((I915_READ_NOTRACE(FORCEWAKE_ACK_VLV) & 1),
+	if (wait_for_atomic((I915_READ_NOTRACE(FORCEWAKE_ACK_VLV) & FORCEWAKE_KERNEL),
 			    FORCEWAKE_ACK_TIMEOUT_MS))
-		DRM_ERROR("Timed out waiting for forcewake to ack request.\n");
+		DRM_ERROR("Timed out waiting for GT to ack forcewake request.\n");
+
+	if (wait_for_atomic((I915_READ_NOTRACE(FORCEWAKE_ACK_MEDIA_VLV) &
+			     FORCEWAKE_KERNEL),
+			    FORCEWAKE_ACK_TIMEOUT_MS))
+		DRM_ERROR("Timed out waiting for media to ack forcewake request.\n");
 
 	__gen6_gt_wait_for_thread_c0(dev_priv);
 }
@@ -4425,8 +4480,9 @@ static void vlv_force_wake_get(struct drm_i915_private *dev_priv)
 static void vlv_force_wake_put(struct drm_i915_private *dev_priv)
 {
 	I915_WRITE_NOTRACE(FORCEWAKE_VLV, _MASKED_BIT_DISABLE(FORCEWAKE_KERNEL));
-	/* something from same cacheline, but !FORCEWAKE_VLV */
-	POSTING_READ(FORCEWAKE_ACK_VLV);
+	I915_WRITE_NOTRACE(FORCEWAKE_MEDIA_VLV,
+			   _MASKED_BIT_DISABLE(FORCEWAKE_KERNEL));
+	/* The below doubles as a POSTING_READ */
 	gen6_gt_check_fifodbg(dev_priv);
 }
 
@@ -4511,3 +4567,56 @@ int sandybridge_pcode_write(struct drm_i915_private *dev_priv, u8 mbox, u32 val)
 
 	return 0;
 }
+
+static int vlv_punit_rw(struct drm_i915_private *dev_priv, u8 opcode,
+			u8 addr, u32 *val)
+{
+	u32 cmd, devfn, port, be, bar;
+
+	bar = 0;
+	be = 0xf;
+	port = IOSF_PORT_PUNIT;
+	devfn = PCI_DEVFN(2, 0);
+
+	cmd = (devfn << IOSF_DEVFN_SHIFT) | (opcode << IOSF_OPCODE_SHIFT) |
+		(port << IOSF_PORT_SHIFT) | (be << IOSF_BYTE_ENABLES_SHIFT) |
+		(bar << IOSF_BAR_SHIFT);
+
+	WARN_ON(!mutex_is_locked(&dev_priv->rps.hw_lock));
+
+	if (I915_READ(VLV_IOSF_DOORBELL_REQ) & IOSF_SB_BUSY) {
+		DRM_DEBUG_DRIVER("warning: pcode (%s) mailbox access failed\n",
+				 opcode == PUNIT_OPCODE_REG_READ ?
+				 "read" : "write");
+		return -EAGAIN;
+	}
+
+	I915_WRITE(VLV_IOSF_ADDR, addr);
+	if (opcode == PUNIT_OPCODE_REG_WRITE)
+		I915_WRITE(VLV_IOSF_DATA, *val);
+	I915_WRITE(VLV_IOSF_DOORBELL_REQ, cmd);
+
+	if (wait_for((I915_READ(VLV_IOSF_DOORBELL_REQ) & IOSF_SB_BUSY) == 0,
+		     500)) {
+		DRM_ERROR("timeout waiting for pcode %s (%d) to finish\n",
+			  opcode == PUNIT_OPCODE_REG_READ ? "read" : "write",
+			  addr);
+		return -ETIMEDOUT;
+	}
+
+	if (opcode == PUNIT_OPCODE_REG_READ)
+		*val = I915_READ(VLV_IOSF_DATA);
+	I915_WRITE(VLV_IOSF_DATA, 0);
+
+	return 0;
+}
+
+int valleyview_punit_read(struct drm_i915_private *dev_priv, u8 addr, u32 *val)
+{
+	return vlv_punit_rw(dev_priv, PUNIT_OPCODE_REG_READ, addr, val);
+}
+
+int valleyview_punit_write(struct drm_i915_private *dev_priv, u8 addr, u32 val)
+{
+	return vlv_punit_rw(dev_priv, PUNIT_OPCODE_REG_WRITE, addr, &val);
+}
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index 78413ec623c9..d15428404b9a 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -246,11 +246,11 @@ static void intel_sdvo_write_sdvox(struct intel_sdvo *intel_sdvo, u32 val)
 		return;
 	}
 
-	if (intel_sdvo->sdvo_reg == SDVOB) {
-		cval = I915_READ(SDVOC);
-	} else {
-		bval = I915_READ(SDVOB);
-	}
+	if (intel_sdvo->sdvo_reg == GEN3_SDVOB)
+		cval = I915_READ(GEN3_SDVOC);
+	else
+		bval = I915_READ(GEN3_SDVOB);
+
 	/*
 	 * Write the registers twice for luck. Sometimes,
 	 * writing them only once doesn't appear to 'stick'.
@@ -258,10 +258,10 @@ static void intel_sdvo_write_sdvox(struct intel_sdvo *intel_sdvo, u32 val)
 	 */
 	for (i = 0; i < 2; i++)
 	{
-		I915_WRITE(SDVOB, bval);
-		I915_READ(SDVOB);
-		I915_WRITE(SDVOC, cval);
-		I915_READ(SDVOC);
+		I915_WRITE(GEN3_SDVOB, bval);
+		I915_READ(GEN3_SDVOB);
+		I915_WRITE(GEN3_SDVOC, cval);
+		I915_READ(GEN3_SDVOC);
 	}
 }
 
@@ -788,7 +788,6 @@ static void intel_sdvo_get_dtd_from_mode(struct intel_sdvo_dtd *dtd,
 	v_sync_offset = mode->vsync_start - mode->vdisplay;
 
 	mode_clock = mode->clock;
-	mode_clock /= intel_mode_get_pixel_multiplier(mode) ?: 1;
 	mode_clock /= 10;
 	dtd->part1.clock = mode_clock;
 
@@ -957,14 +956,17 @@ static bool intel_sdvo_set_avi_infoframe(struct intel_sdvo *intel_sdvo,
 		.len = DIP_LEN_AVI,
 	};
 	uint8_t sdvo_data[4 + sizeof(avi_if.body.avi)];
+	struct intel_crtc *intel_crtc = to_intel_crtc(intel_sdvo->base.base.crtc);
 
 	if (intel_sdvo->rgb_quant_range_selectable) {
-		if (adjusted_mode->private_flags & INTEL_MODE_LIMITED_COLOR_RANGE)
+		if (intel_crtc->config.limited_color_range)
 			avi_if.body.avi.ITC_EC_Q_SC |= DIP_AVI_RGB_QUANT_RANGE_LIMITED;
 		else
 			avi_if.body.avi.ITC_EC_Q_SC |= DIP_AVI_RGB_QUANT_RANGE_FULL;
 	}
 
+	avi_if.body.avi.VIC = drm_match_cea_mode(adjusted_mode);
+
 	intel_dip_infoframe_csum(&avi_if);
 
 	/* sdvo spec says that the ecc is handled by the hw, and it looks like
@@ -1039,12 +1041,18 @@ intel_sdvo_get_preferred_input_mode(struct intel_sdvo *intel_sdvo,
 	return true;
 }
 
-static bool intel_sdvo_mode_fixup(struct drm_encoder *encoder,
-				  const struct drm_display_mode *mode,
-				  struct drm_display_mode *adjusted_mode)
+static bool intel_sdvo_compute_config(struct intel_encoder *encoder,
+				      struct intel_crtc_config *pipe_config)
 {
-	struct intel_sdvo *intel_sdvo = to_intel_sdvo(encoder);
-	int multiplier;
+	struct intel_sdvo *intel_sdvo = to_intel_sdvo(&encoder->base);
+	struct drm_display_mode *adjusted_mode = &pipe_config->adjusted_mode;
+	struct drm_display_mode *mode = &pipe_config->requested_mode;
+
+	DRM_DEBUG_KMS("forcing bpc to 8 for SDVO\n");
+	pipe_config->pipe_bpp = 8*3;
+
+	if (HAS_PCH_SPLIT(encoder->base.dev))
+		pipe_config->has_pch_encoder = true;
 
 	/* We need to construct preferred input timings based on our
 	 * output timings.  To do that, we have to set the output
@@ -1071,37 +1079,40 @@ static bool intel_sdvo_mode_fixup(struct drm_encoder *encoder,
 	/* Make the CRTC code factor in the SDVO pixel multiplier.  The
 	 * SDVO device will factor out the multiplier during mode_set.
 	 */
-	multiplier = intel_sdvo_get_pixel_multiplier(adjusted_mode);
-	intel_mode_set_pixel_multiplier(adjusted_mode, multiplier);
+	pipe_config->pixel_multiplier =
+		intel_sdvo_get_pixel_multiplier(adjusted_mode);
+	adjusted_mode->clock *= pipe_config->pixel_multiplier;
 
 	if (intel_sdvo->color_range_auto) {
 		/* See CEA-861-E - 5.1 Default Encoding Parameters */
+		/* FIXME: This bit is only valid when using TMDS encoding and 8
+		 * bit per color mode. */
 		if (intel_sdvo->has_hdmi_monitor &&
 		    drm_match_cea_mode(adjusted_mode) > 1)
-			intel_sdvo->color_range = SDVO_COLOR_RANGE_16_235;
+			intel_sdvo->color_range = HDMI_COLOR_RANGE_16_235;
 		else
 			intel_sdvo->color_range = 0;
 	}
 
 	if (intel_sdvo->color_range)
-		adjusted_mode->private_flags |= INTEL_MODE_LIMITED_COLOR_RANGE;
+		pipe_config->limited_color_range = true;
 
 	return true;
 }
 
-static void intel_sdvo_mode_set(struct drm_encoder *encoder,
-				struct drm_display_mode *mode,
-				struct drm_display_mode *adjusted_mode)
+static void intel_sdvo_mode_set(struct intel_encoder *intel_encoder)
 {
-	struct drm_device *dev = encoder->dev;
+	struct drm_device *dev = intel_encoder->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc = encoder->crtc;
+	struct drm_crtc *crtc = intel_encoder->base.crtc;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	struct intel_sdvo *intel_sdvo = to_intel_sdvo(encoder);
+	struct drm_display_mode *adjusted_mode =
+		&intel_crtc->config.adjusted_mode;
+	struct drm_display_mode *mode = &intel_crtc->config.requested_mode;
+	struct intel_sdvo *intel_sdvo = to_intel_sdvo(&intel_encoder->base);
 	u32 sdvox;
 	struct intel_sdvo_in_out_map in_out;
 	struct intel_sdvo_dtd input_dtd, output_dtd;
-	int pixel_multiplier = intel_mode_get_pixel_multiplier(adjusted_mode);
 	int rate;
 
 	if (!mode)
@@ -1161,7 +1172,7 @@ static void intel_sdvo_mode_set(struct drm_encoder *encoder,
 		DRM_INFO("Setting input timings on %s failed\n",
 			 SDVO_NAME(intel_sdvo));
 
-	switch (pixel_multiplier) {
+	switch (intel_crtc->config.pixel_multiplier) {
 	default:
 	case 1: rate = SDVO_CLOCK_RATE_MULT_1X; break;
 	case 2: rate = SDVO_CLOCK_RATE_MULT_2X; break;
@@ -1182,10 +1193,10 @@ static void intel_sdvo_mode_set(struct drm_encoder *encoder,
 	} else {
 		sdvox = I915_READ(intel_sdvo->sdvo_reg);
 		switch (intel_sdvo->sdvo_reg) {
-		case SDVOB:
+		case GEN3_SDVOB:
 			sdvox &= SDVOB_PRESERVE_MASK;
 			break;
-		case SDVOC:
+		case GEN3_SDVOC:
 			sdvox &= SDVOC_PRESERVE_MASK;
 			break;
 		}
@@ -1193,9 +1204,9 @@ static void intel_sdvo_mode_set(struct drm_encoder *encoder,
 	}
 
 	if (INTEL_PCH_TYPE(dev) >= PCH_CPT)
-		sdvox |= TRANSCODER_CPT(intel_crtc->pipe);
+		sdvox |= SDVO_PIPE_SEL_CPT(intel_crtc->pipe);
 	else
-		sdvox |= TRANSCODER(intel_crtc->pipe);
+		sdvox |= SDVO_PIPE_SEL(intel_crtc->pipe);
 
 	if (intel_sdvo->has_hdmi_audio)
 		sdvox |= SDVO_AUDIO_ENABLE;
@@ -1205,7 +1216,8 @@ static void intel_sdvo_mode_set(struct drm_encoder *encoder,
 	} else if (IS_I945G(dev) || IS_I945GM(dev) || IS_G33(dev)) {
 		/* done in crtc_mode_set as it lives inside the dpll register */
 	} else {
-		sdvox |= (pixel_multiplier - 1) << SDVO_PORT_MULTIPLY_SHIFT;
+		sdvox |= (intel_crtc->config.pixel_multiplier - 1)
+			<< SDVO_PORT_MULTIPLY_SHIFT;
 	}
 
 	if (input_dtd.part2.sdvo_flags & SDVO_NEED_TO_STALL &&
@@ -1235,11 +1247,13 @@ static bool intel_sdvo_get_hw_state(struct intel_encoder *encoder,
 	struct drm_device *dev = encoder->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_sdvo *intel_sdvo = to_intel_sdvo(&encoder->base);
+	u16 active_outputs;
 	u32 tmp;
 
 	tmp = I915_READ(intel_sdvo->sdvo_reg);
+	intel_sdvo_get_active_outputs(intel_sdvo, &active_outputs);
 
-	if (!(tmp & SDVO_ENABLE))
+	if (!(tmp & SDVO_ENABLE) && (active_outputs == 0))
 		return false;
 
 	if (HAS_PCH_CPT(dev))
@@ -1305,15 +1319,9 @@ static void intel_enable_sdvo(struct intel_encoder *encoder)
 	temp = I915_READ(intel_sdvo->sdvo_reg);
 	if ((temp & SDVO_ENABLE) == 0) {
 		/* HW workaround for IBX, we need to move the port
-		 * to transcoder A before disabling it. */
-		if (HAS_PCH_IBX(dev)) {
-			struct drm_crtc *crtc = encoder->base.crtc;
-			int pipe = crtc ? to_intel_crtc(crtc)->pipe : -1;
-
-			/* Restore the transcoder select bit. */
-			if (pipe == PIPE_B)
-				temp |= SDVO_PIPE_B_SELECT;
-		}
+		 * to transcoder A before disabling it, so restore it here. */
+		if (HAS_PCH_IBX(dev))
+			temp |= SDVO_PIPE_SEL(intel_crtc->pipe);
 
 		intel_sdvo_write_sdvox(intel_sdvo, temp | SDVO_ENABLE);
 	}
@@ -1922,6 +1930,9 @@ intel_sdvo_set_property(struct drm_connector *connector,
 	}
 
 	if (property == dev_priv->broadcast_rgb_property) {
+		bool old_auto = intel_sdvo->color_range_auto;
+		uint32_t old_range = intel_sdvo->color_range;
+
 		switch (val) {
 		case INTEL_BROADCAST_RGB_AUTO:
 			intel_sdvo->color_range_auto = true;
@@ -1932,11 +1943,18 @@ intel_sdvo_set_property(struct drm_connector *connector,
 			break;
 		case INTEL_BROADCAST_RGB_LIMITED:
 			intel_sdvo->color_range_auto = false;
-			intel_sdvo->color_range = SDVO_COLOR_RANGE_16_235;
+			/* FIXME: this bit is only valid when using TMDS
+			 * encoding and 8 bit per color mode. */
+			intel_sdvo->color_range = HDMI_COLOR_RANGE_16_235;
 			break;
 		default:
 			return -EINVAL;
 		}
+
+		if (old_auto == intel_sdvo->color_range_auto &&
+		    old_range == intel_sdvo->color_range)
+			return 0;
+
 		goto done;
 	}
 
@@ -2040,11 +2058,6 @@ done:
 #undef CHECK_PROPERTY
 }
 
-static const struct drm_encoder_helper_funcs intel_sdvo_helper_funcs = {
-	.mode_fixup = intel_sdvo_mode_fixup,
-	.mode_set = intel_sdvo_mode_set,
-};
-
 static const struct drm_connector_funcs intel_sdvo_connector_funcs = {
 	.dpms = intel_sdvo_dpms,
 	.detect = intel_sdvo_detect,
@@ -2269,7 +2282,6 @@ intel_sdvo_dvi_init(struct intel_sdvo *intel_sdvo, int device)
 	connector = &intel_connector->base;
 	if (intel_sdvo_get_hotplug_support(intel_sdvo) &
 		intel_sdvo_connector->output_flag) {
-		connector->polled = DRM_CONNECTOR_POLL_HPD;
 		intel_sdvo->hotplug_active |= intel_sdvo_connector->output_flag;
 		/* Some SDVO devices have one-shot hotplug interrupts.
 		 * Ensure that they get re-enabled when an interrupt happens.
@@ -2277,7 +2289,7 @@ intel_sdvo_dvi_init(struct intel_sdvo *intel_sdvo, int device)
 		intel_encoder->hot_plug = intel_sdvo_enable_hotplug;
 		intel_sdvo_enable_hotplug(intel_encoder);
 	} else {
-		connector->polled = DRM_CONNECTOR_POLL_CONNECT | DRM_CONNECTOR_POLL_DISCONNECT;
+		intel_connector->polled = DRM_CONNECTOR_POLL_CONNECT | DRM_CONNECTOR_POLL_DISCONNECT;
 	}
 	encoder->encoder_type = DRM_MODE_ENCODER_TMDS;
 	connector->connector_type = DRM_MODE_CONNECTOR_DVID;
@@ -2346,7 +2358,7 @@ intel_sdvo_analog_init(struct intel_sdvo *intel_sdvo, int device)
 
 	intel_connector = &intel_sdvo_connector->base;
 	connector = &intel_connector->base;
-	connector->polled = DRM_CONNECTOR_POLL_CONNECT;
+	intel_connector->polled = DRM_CONNECTOR_POLL_CONNECT;
 	encoder->encoder_type = DRM_MODE_ENCODER_DAC;
 	connector->connector_type = DRM_MODE_CONNECTOR_VGA;
 
@@ -2739,7 +2751,6 @@ bool intel_sdvo_init(struct drm_device *dev, uint32_t sdvo_reg, bool is_sdvob)
 	struct intel_sdvo *intel_sdvo;
 	u32 hotplug_mask;
 	int i;
-
 	intel_sdvo = kzalloc(sizeof(struct intel_sdvo), GFP_KERNEL);
 	if (!intel_sdvo)
 		return false;
@@ -2779,9 +2790,15 @@ bool intel_sdvo_init(struct drm_device *dev, uint32_t sdvo_reg, bool is_sdvob)
 			SDVOB_HOTPLUG_INT_STATUS_I915 : SDVOC_HOTPLUG_INT_STATUS_I915;
 	}
 
-	drm_encoder_helper_add(&intel_encoder->base, &intel_sdvo_helper_funcs);
+	/* Only enable the hotplug irq if we need it, to work around noisy
+	 * hotplug lines.
+	 */
+	if (intel_sdvo->hotplug_active)
+		intel_encoder->hpd_pin = HPD_SDVO_B ? HPD_SDVO_B : HPD_SDVO_C;
 
+	intel_encoder->compute_config = intel_sdvo_compute_config;
 	intel_encoder->disable = intel_disable_sdvo;
+	intel_encoder->mode_set = intel_sdvo_mode_set;
 	intel_encoder->enable = intel_enable_sdvo;
 	intel_encoder->get_hw_state = intel_sdvo_get_hw_state;
 
@@ -2807,12 +2824,6 @@ bool intel_sdvo_init(struct drm_device *dev, uint32_t sdvo_reg, bool is_sdvob)
 	 */
 	intel_sdvo->base.cloneable = false;
 
-	/* Only enable the hotplug irq if we need it, to work around noisy
-	 * hotplug lines.
-	 */
-	if (intel_sdvo->hotplug_active)
-		dev_priv->hotplug_supported_mask |= hotplug_mask;
-
 	intel_sdvo_select_ddc_bus(dev_priv, intel_sdvo, sdvo_reg);
 
 	/* Set the input timing to the screen. Assume always input 0. */
diff --git a/drivers/gpu/drm/i915/intel_sprite.c b/drivers/gpu/drm/i915/intel_sprite.c
index 1b6eb76beb7c..c7d25c5dd4e6 100644
--- a/drivers/gpu/drm/i915/intel_sprite.c
+++ b/drivers/gpu/drm/i915/intel_sprite.c
@@ -37,6 +37,174 @@
 #include "i915_drv.h"
 
 static void
+vlv_update_plane(struct drm_plane *dplane, struct drm_framebuffer *fb,
+		 struct drm_i915_gem_object *obj, int crtc_x, int crtc_y,
+		 unsigned int crtc_w, unsigned int crtc_h,
+		 uint32_t x, uint32_t y,
+		 uint32_t src_w, uint32_t src_h)
+{
+	struct drm_device *dev = dplane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane = to_intel_plane(dplane);
+	int pipe = intel_plane->pipe;
+	int plane = intel_plane->plane;
+	u32 sprctl;
+	unsigned long sprsurf_offset, linear_offset;
+	int pixel_size = drm_format_plane_cpp(fb->pixel_format, 0);
+
+	sprctl = I915_READ(SPCNTR(pipe, plane));
+
+	/* Mask out pixel format bits in case we change it */
+	sprctl &= ~SP_PIXFORMAT_MASK;
+	sprctl &= ~SP_YUV_BYTE_ORDER_MASK;
+	sprctl &= ~SP_TILED;
+
+	switch (fb->pixel_format) {
+	case DRM_FORMAT_YUYV:
+		sprctl |= SP_FORMAT_YUV422 | SP_YUV_ORDER_YUYV;
+		break;
+	case DRM_FORMAT_YVYU:
+		sprctl |= SP_FORMAT_YUV422 | SP_YUV_ORDER_YVYU;
+		break;
+	case DRM_FORMAT_UYVY:
+		sprctl |= SP_FORMAT_YUV422 | SP_YUV_ORDER_UYVY;
+		break;
+	case DRM_FORMAT_VYUY:
+		sprctl |= SP_FORMAT_YUV422 | SP_YUV_ORDER_VYUY;
+		break;
+	case DRM_FORMAT_RGB565:
+		sprctl |= SP_FORMAT_BGR565;
+		break;
+	case DRM_FORMAT_XRGB8888:
+		sprctl |= SP_FORMAT_BGRX8888;
+		break;
+	case DRM_FORMAT_ARGB8888:
+		sprctl |= SP_FORMAT_BGRA8888;
+		break;
+	case DRM_FORMAT_XBGR2101010:
+		sprctl |= SP_FORMAT_RGBX1010102;
+		break;
+	case DRM_FORMAT_ABGR2101010:
+		sprctl |= SP_FORMAT_RGBA1010102;
+		break;
+	case DRM_FORMAT_XBGR8888:
+		sprctl |= SP_FORMAT_RGBX8888;
+		break;
+	case DRM_FORMAT_ABGR8888:
+		sprctl |= SP_FORMAT_RGBA8888;
+		break;
+	default:
+		/*
+		 * If we get here one of the upper layers failed to filter
+		 * out the unsupported plane formats
+		 */
+		BUG();
+		break;
+	}
+
+	if (obj->tiling_mode != I915_TILING_NONE)
+		sprctl |= SP_TILED;
+
+	sprctl |= SP_ENABLE;
+
+	/* Sizes are 0 based */
+	src_w--;
+	src_h--;
+	crtc_w--;
+	crtc_h--;
+
+	intel_update_sprite_watermarks(dev, pipe, crtc_w, pixel_size);
+
+	I915_WRITE(SPSTRIDE(pipe, plane), fb->pitches[0]);
+	I915_WRITE(SPPOS(pipe, plane), (crtc_y << 16) | crtc_x);
+
+	linear_offset = y * fb->pitches[0] + x * pixel_size;
+	sprsurf_offset = intel_gen4_compute_page_offset(&x, &y,
+							obj->tiling_mode,
+							pixel_size,
+							fb->pitches[0]);
+	linear_offset -= sprsurf_offset;
+
+	if (obj->tiling_mode != I915_TILING_NONE)
+		I915_WRITE(SPTILEOFF(pipe, plane), (y << 16) | x);
+	else
+		I915_WRITE(SPLINOFF(pipe, plane), linear_offset);
+
+	I915_WRITE(SPSIZE(pipe, plane), (crtc_h << 16) | crtc_w);
+	I915_WRITE(SPCNTR(pipe, plane), sprctl);
+	I915_MODIFY_DISPBASE(SPSURF(pipe, plane), obj->gtt_offset +
+			     sprsurf_offset);
+	POSTING_READ(SPSURF(pipe, plane));
+}
+
+static void
+vlv_disable_plane(struct drm_plane *dplane)
+{
+	struct drm_device *dev = dplane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane = to_intel_plane(dplane);
+	int pipe = intel_plane->pipe;
+	int plane = intel_plane->plane;
+
+	I915_WRITE(SPCNTR(pipe, plane), I915_READ(SPCNTR(pipe, plane)) &
+		   ~SP_ENABLE);
+	/* Activate double buffered register update */
+	I915_MODIFY_DISPBASE(SPSURF(pipe, plane), 0);
+	POSTING_READ(SPSURF(pipe, plane));
+}
+
+static int
+vlv_update_colorkey(struct drm_plane *dplane,
+		    struct drm_intel_sprite_colorkey *key)
+{
+	struct drm_device *dev = dplane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane = to_intel_plane(dplane);
+	int pipe = intel_plane->pipe;
+	int plane = intel_plane->plane;
+	u32 sprctl;
+
+	if (key->flags & I915_SET_COLORKEY_DESTINATION)
+		return -EINVAL;
+
+	I915_WRITE(SPKEYMINVAL(pipe, plane), key->min_value);
+	I915_WRITE(SPKEYMAXVAL(pipe, plane), key->max_value);
+	I915_WRITE(SPKEYMSK(pipe, plane), key->channel_mask);
+
+	sprctl = I915_READ(SPCNTR(pipe, plane));
+	sprctl &= ~SP_SOURCE_KEY;
+	if (key->flags & I915_SET_COLORKEY_SOURCE)
+		sprctl |= SP_SOURCE_KEY;
+	I915_WRITE(SPCNTR(pipe, plane), sprctl);
+
+	POSTING_READ(SPKEYMSK(pipe, plane));
+
+	return 0;
+}
+
+static void
+vlv_get_colorkey(struct drm_plane *dplane,
+		 struct drm_intel_sprite_colorkey *key)
+{
+	struct drm_device *dev = dplane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane = to_intel_plane(dplane);
+	int pipe = intel_plane->pipe;
+	int plane = intel_plane->plane;
+	u32 sprctl;
+
+	key->min_value = I915_READ(SPKEYMINVAL(pipe, plane));
+	key->max_value = I915_READ(SPKEYMAXVAL(pipe, plane));
+	key->channel_mask = I915_READ(SPKEYMSK(pipe, plane));
+
+	sprctl = I915_READ(SPCNTR(pipe, plane));
+	if (sprctl & SP_SOURCE_KEY)
+		key->flags = I915_SET_COLORKEY_SOURCE;
+	else
+		key->flags = I915_SET_COLORKEY_NONE;
+}
+
+static void
 ivb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 		 struct drm_i915_gem_object *obj, int crtc_x, int crtc_y,
 		 unsigned int crtc_w, unsigned int crtc_h,
@@ -441,6 +609,15 @@ intel_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 
 	old_obj = intel_plane->obj;
 
+	intel_plane->crtc_x = crtc_x;
+	intel_plane->crtc_y = crtc_y;
+	intel_plane->crtc_w = crtc_w;
+	intel_plane->crtc_h = crtc_h;
+	intel_plane->src_x = src_x;
+	intel_plane->src_y = src_y;
+	intel_plane->src_w = src_w;
+	intel_plane->src_h = src_h;
+
 	src_w = src_w >> 16;
 	src_h = src_h >> 16;
 
@@ -513,6 +690,11 @@ intel_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 
 	mutex_lock(&dev->struct_mutex);
 
+	/* Note that this will apply the VT-d workaround for scanouts,
+	 * which is more restrictive than required for sprites. (The
+	 * primary plane requires 256KiB alignment with 64 PTE padding,
+	 * the sprite planes only require 128KiB alignment and 32 PTE padding.
+	 */
 	ret = intel_pin_and_fence_fb_obj(dev, obj, NULL);
 	if (ret)
 		goto out_unlock;
@@ -568,6 +750,8 @@ intel_disable_plane(struct drm_plane *plane)
 	if (!intel_plane->obj)
 		goto out;
 
+	intel_wait_for_vblank(dev, intel_plane->pipe);
+
 	mutex_lock(&dev->struct_mutex);
 	intel_unpin_fb_obj(intel_plane->obj);
 	intel_plane->obj = NULL;
@@ -647,6 +831,20 @@ out_unlock:
 	return ret;
 }
 
+void intel_plane_restore(struct drm_plane *plane)
+{
+	struct intel_plane *intel_plane = to_intel_plane(plane);
+
+	if (!plane->crtc || !plane->fb)
+		return;
+
+	intel_update_plane(plane, plane->crtc, plane->fb,
+			   intel_plane->crtc_x, intel_plane->crtc_y,
+			   intel_plane->crtc_w, intel_plane->crtc_h,
+			   intel_plane->src_x, intel_plane->src_y,
+			   intel_plane->src_w, intel_plane->src_h);
+}
+
 static const struct drm_plane_funcs intel_plane_funcs = {
 	.update_plane = intel_update_plane,
 	.disable_plane = intel_disable_plane,
@@ -670,8 +868,22 @@ static uint32_t snb_plane_formats[] = {
 	DRM_FORMAT_VYUY,
 };
 
+static uint32_t vlv_plane_formats[] = {
+	DRM_FORMAT_RGB565,
+	DRM_FORMAT_ABGR8888,
+	DRM_FORMAT_ARGB8888,
+	DRM_FORMAT_XBGR8888,
+	DRM_FORMAT_XRGB8888,
+	DRM_FORMAT_XBGR2101010,
+	DRM_FORMAT_ABGR2101010,
+	DRM_FORMAT_YUYV,
+	DRM_FORMAT_YVYU,
+	DRM_FORMAT_UYVY,
+	DRM_FORMAT_VYUY,
+};
+
 int
-intel_plane_init(struct drm_device *dev, enum pipe pipe)
+intel_plane_init(struct drm_device *dev, enum pipe pipe, int plane)
 {
 	struct intel_plane *intel_plane;
 	unsigned long possible_crtcs;
@@ -710,14 +922,26 @@ intel_plane_init(struct drm_device *dev, enum pipe pipe)
 			intel_plane->can_scale = false;
 		else
 			intel_plane->can_scale = true;
-		intel_plane->max_downscale = 2;
-		intel_plane->update_plane = ivb_update_plane;
-		intel_plane->disable_plane = ivb_disable_plane;
-		intel_plane->update_colorkey = ivb_update_colorkey;
-		intel_plane->get_colorkey = ivb_get_colorkey;
-
-		plane_formats = snb_plane_formats;
-		num_plane_formats = ARRAY_SIZE(snb_plane_formats);
+
+		if (IS_VALLEYVIEW(dev)) {
+			intel_plane->max_downscale = 1;
+			intel_plane->update_plane = vlv_update_plane;
+			intel_plane->disable_plane = vlv_disable_plane;
+			intel_plane->update_colorkey = vlv_update_colorkey;
+			intel_plane->get_colorkey = vlv_get_colorkey;
+
+			plane_formats = vlv_plane_formats;
+			num_plane_formats = ARRAY_SIZE(vlv_plane_formats);
+		} else {
+			intel_plane->max_downscale = 2;
+			intel_plane->update_plane = ivb_update_plane;
+			intel_plane->disable_plane = ivb_disable_plane;
+			intel_plane->update_colorkey = ivb_update_colorkey;
+			intel_plane->get_colorkey = ivb_get_colorkey;
+
+			plane_formats = snb_plane_formats;
+			num_plane_formats = ARRAY_SIZE(snb_plane_formats);
+		}
 		break;
 
 	default:
@@ -726,6 +950,7 @@ intel_plane_init(struct drm_device *dev, enum pipe pipe)
 	}
 
 	intel_plane->pipe = pipe;
+	intel_plane->plane = plane;
 	possible_crtcs = (1 << pipe);
 	ret = drm_plane_init(dev, &intel_plane->base, possible_crtcs,
 			     &intel_plane_funcs,
diff --git a/drivers/gpu/drm/i915/intel_tv.c b/drivers/gpu/drm/i915/intel_tv.c
index d808421c1c80..b945bc54207a 100644
--- a/drivers/gpu/drm/i915/intel_tv.c
+++ b/drivers/gpu/drm/i915/intel_tv.c
@@ -905,11 +905,10 @@ intel_tv_mode_valid(struct drm_connector *connector,
 
 
 static bool
-intel_tv_mode_fixup(struct drm_encoder *encoder,
-		    const struct drm_display_mode *mode,
-		    struct drm_display_mode *adjusted_mode)
+intel_tv_compute_config(struct intel_encoder *encoder,
+			struct intel_crtc_config *pipe_config)
 {
-	struct intel_tv *intel_tv = enc_to_intel_tv(encoder);
+	struct intel_tv *intel_tv = enc_to_intel_tv(&encoder->base);
 	const struct tv_mode *tv_mode = intel_tv_mode_find(intel_tv);
 
 	if (!tv_mode)
@@ -918,7 +917,10 @@ intel_tv_mode_fixup(struct drm_encoder *encoder,
 	if (intel_encoder_check_is_cloned(&intel_tv->base))
 		return false;
 
-	adjusted_mode->clock = tv_mode->clock;
+	pipe_config->adjusted_mode.clock = tv_mode->clock;
+	DRM_DEBUG_KMS("forcing bpc to 8 for TV\n");
+	pipe_config->pipe_bpp = 8*3;
+
 	return true;
 }
 
@@ -1485,7 +1487,6 @@ out:
 }
 
 static const struct drm_encoder_helper_funcs intel_tv_helper_funcs = {
-	.mode_fixup = intel_tv_mode_fixup,
 	.mode_set = intel_tv_mode_set,
 };
 
@@ -1612,7 +1613,7 @@ intel_tv_init(struct drm_device *dev)
 	 *
 	 * More recent chipsets favour HDMI rather than integrated S-Video.
 	 */
-	connector->polled = DRM_CONNECTOR_POLL_CONNECT;
+	intel_connector->polled = DRM_CONNECTOR_POLL_CONNECT;
 
 	drm_connector_init(dev, connector, &intel_tv_connector_funcs,
 			   DRM_MODE_CONNECTOR_SVIDEO);
@@ -1620,6 +1621,7 @@ intel_tv_init(struct drm_device *dev)
 	drm_encoder_init(dev, &intel_encoder->base, &intel_tv_enc_funcs,
 			 DRM_MODE_ENCODER_TVDAC);
 
+	intel_encoder->compute_config = intel_tv_compute_config;
 	intel_encoder->enable = intel_enable_tv;
 	intel_encoder->disable = intel_disable_tv;
 	intel_encoder->get_hw_state = intel_tv_get_hw_state;
diff --git a/drivers/gpu/drm/mgag200/mgag200_drv.h b/drivers/gpu/drm/mgag200/mgag200_drv.h
index 4d932c46725d..bf29b2f4d68d 100644
--- a/drivers/gpu/drm/mgag200/mgag200_drv.h
+++ b/drivers/gpu/drm/mgag200/mgag200_drv.h
@@ -115,6 +115,8 @@ struct mga_fbdev {
 	void *sysram;
 	int size;
 	struct ttm_bo_kmap_obj mapping;
+	int x1, y1, x2, y2; /* dirty rect */
+	spinlock_t dirty_lock;
 };
 
 struct mga_crtc {
@@ -215,7 +217,7 @@ mgag200_bo(struct ttm_buffer_object *bo)
 {
 	return container_of(bo, struct mgag200_bo, bo);
 }
-				/* mga_crtc.c */
+				/* mgag200_crtc.c */
 void mga_crtc_fb_gamma_set(struct drm_crtc *crtc, u16 red, u16 green,
 			     u16 blue, int regno);
 void mga_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
@@ -225,7 +227,7 @@ void mga_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
 int mgag200_modeset_init(struct mga_device *mdev);
 void mgag200_modeset_fini(struct mga_device *mdev);
 
-				/* mga_fbdev.c */
+				/* mgag200_fb.c */
 int mgag200_fbdev_init(struct mga_device *mdev);
 void mgag200_fbdev_fini(struct mga_device *mdev);
 
@@ -254,7 +256,7 @@ mgag200_dumb_mmap_offset(struct drm_file *file,
 			 struct drm_device *dev,
 			 uint32_t handle,
 			 uint64_t *offset);
-				/* mga_i2c.c */
+				/* mgag200_i2c.c */
 struct mga_i2c_chan *mgag200_i2c_create(struct drm_device *dev);
 void mgag200_i2c_destroy(struct mga_i2c_chan *i2c);
 
diff --git a/drivers/gpu/drm/mgag200/mgag200_fb.c b/drivers/gpu/drm/mgag200/mgag200_fb.c
index d2253f639481..5da824ce9ba1 100644
--- a/drivers/gpu/drm/mgag200/mgag200_fb.c
+++ b/drivers/gpu/drm/mgag200/mgag200_fb.c
@@ -29,16 +29,52 @@ static void mga_dirty_update(struct mga_fbdev *mfbdev,
 	int bpp = (mfbdev->mfb.base.bits_per_pixel + 7)/8;
 	int ret;
 	bool unmap = false;
+	bool store_for_later = false;
+	int x2, y2;
+	unsigned long flags;
 
 	obj = mfbdev->mfb.obj;
 	bo = gem_to_mga_bo(obj);
 
+	/*
+	 * try and reserve the BO, if we fail with busy
+	 * then the BO is being moved and we should
+	 * store up the damage until later.
+	 */
 	ret = mgag200_bo_reserve(bo, true);
 	if (ret) {
-		DRM_ERROR("failed to reserve fb bo\n");
+		if (ret != -EBUSY)
+			return;
+
+		store_for_later = true;
+	}
+
+	x2 = x + width - 1;
+	y2 = y + height - 1;
+	spin_lock_irqsave(&mfbdev->dirty_lock, flags);
+
+	if (mfbdev->y1 < y)
+		y = mfbdev->y1;
+	if (mfbdev->y2 > y2)
+		y2 = mfbdev->y2;
+	if (mfbdev->x1 < x)
+		x = mfbdev->x1;
+	if (mfbdev->x2 > x2)
+		x2 = mfbdev->x2;
+
+	if (store_for_later) {
+		mfbdev->x1 = x;
+		mfbdev->x2 = x2;
+		mfbdev->y1 = y;
+		mfbdev->y2 = y2;
+		spin_unlock_irqrestore(&mfbdev->dirty_lock, flags);
 		return;
 	}
 
+	mfbdev->x1 = mfbdev->y1 = INT_MAX;
+	mfbdev->x2 = mfbdev->y2 = 0;
+	spin_unlock_irqrestore(&mfbdev->dirty_lock, flags);
+
 	if (!bo->kmap.virtual) {
 		ret = ttm_bo_kmap(&bo->bo, 0, bo->bo.num_pages, &bo->kmap);
 		if (ret) {
@@ -48,10 +84,10 @@ static void mga_dirty_update(struct mga_fbdev *mfbdev,
 		}
 		unmap = true;
 	}
-	for (i = y; i < y + height; i++) {
+	for (i = y; i <= y2; i++) {
 		/* assume equal stride for now */
 		src_offset = dst_offset = i * mfbdev->mfb.base.pitches[0] + (x * bpp);
-		memcpy_toio(bo->kmap.virtual + src_offset, mfbdev->sysram + src_offset, width * bpp);
+		memcpy_toio(bo->kmap.virtual + src_offset, mfbdev->sysram + src_offset, (x2 - x + 1) * bpp);
 
 	}
 	if (unmap)
@@ -105,12 +141,9 @@ static int mgag200fb_create_object(struct mga_fbdev *afbdev,
 				   struct drm_gem_object **gobj_p)
 {
 	struct drm_device *dev = afbdev->helper.dev;
-	u32 bpp, depth;
 	u32 size;
 	struct drm_gem_object *gobj;
-
 	int ret = 0;
-	drm_fb_get_bpp_depth(mode_cmd->pixel_format, &depth, &bpp);
 
 	size = mode_cmd->pitches[0] * mode_cmd->height;
 	ret = mgag200_gem_create(dev, size, true, &gobj);
@@ -249,19 +282,19 @@ int mgag200_fbdev_init(struct mga_device *mdev)
 	struct mga_fbdev *mfbdev;
 	int ret;
 
-	mfbdev = kzalloc(sizeof(struct mga_fbdev), GFP_KERNEL);
+	mfbdev = devm_kzalloc(mdev->dev->dev, sizeof(struct mga_fbdev), GFP_KERNEL);
 	if (!mfbdev)
 		return -ENOMEM;
 
 	mdev->mfbdev = mfbdev;
 	mfbdev->helper.funcs = &mga_fb_helper_funcs;
+	spin_lock_init(&mfbdev->dirty_lock);
 
 	ret = drm_fb_helper_init(mdev->dev, &mfbdev->helper,
 				 mdev->num_crtc, MGAG200FB_CONN_LIMIT);
-	if (ret) {
-		kfree(mfbdev);
+	if (ret)
 		return ret;
-	}
+
 	drm_fb_helper_single_add_all_connectors(&mfbdev->helper);
 
 	/* disable all the possible outputs/crtcs before entering KMS mode */
@@ -278,6 +311,4 @@ void mgag200_fbdev_fini(struct mga_device *mdev)
 		return;
 
 	mga_fbdev_destroy(mdev->dev, mdev->mfbdev);
-	kfree(mdev->mfbdev);
-	mdev->mfbdev = NULL;
 }
diff --git a/drivers/gpu/drm/mgag200/mgag200_main.c b/drivers/gpu/drm/mgag200/mgag200_main.c
index 64297c72464f..99059237da38 100644
--- a/drivers/gpu/drm/mgag200/mgag200_main.c
+++ b/drivers/gpu/drm/mgag200/mgag200_main.c
@@ -76,15 +76,6 @@ static const struct drm_mode_config_funcs mga_mode_funcs = {
 	.fb_create = mgag200_user_framebuffer_create,
 };
 
-/* Unmap the framebuffer from the core and release the memory */
-static void mga_vram_fini(struct mga_device *mdev)
-{
-	pci_iounmap(mdev->dev->pdev, mdev->rmmio);
-	mdev->rmmio = NULL;
-	if (mdev->mc.vram_base)
-		release_mem_region(mdev->mc.vram_base, mdev->mc.vram_window);
-}
-
 static int mga_probe_vram(struct mga_device *mdev, void __iomem *mem)
 {
 	int offset;
@@ -140,7 +131,7 @@ static int mga_vram_init(struct mga_device *mdev)
 	remove_conflicting_framebuffers(aper, "mgafb", true);
 	kfree(aper);
 
-	if (!request_mem_region(mdev->mc.vram_base, mdev->mc.vram_window,
+	if (!devm_request_mem_region(mdev->dev->dev, mdev->mc.vram_base, mdev->mc.vram_window,
 				"mgadrmfb_vram")) {
 		DRM_ERROR("can't reserve VRAM\n");
 		return -ENXIO;
@@ -173,13 +164,13 @@ static int mgag200_device_init(struct drm_device *dev,
 	mdev->rmmio_base = pci_resource_start(mdev->dev->pdev, 1);
 	mdev->rmmio_size = pci_resource_len(mdev->dev->pdev, 1);
 
-	if (!request_mem_region(mdev->rmmio_base, mdev->rmmio_size,
+	if (!devm_request_mem_region(mdev->dev->dev, mdev->rmmio_base, mdev->rmmio_size,
 				"mgadrmfb_mmio")) {
 		DRM_ERROR("can't reserve mmio registers\n");
 		return -ENOMEM;
 	}
 
-	mdev->rmmio = pci_iomap(dev->pdev, 1, 0);
+	mdev->rmmio = pcim_iomap(dev->pdev, 1, 0);
 	if (mdev->rmmio == NULL)
 		return -ENOMEM;
 
@@ -188,10 +179,8 @@ static int mgag200_device_init(struct drm_device *dev,
 		mdev->reg_1e24 = RREG32(0x1e24);
 
 	ret = mga_vram_init(mdev);
-	if (ret) {
-		release_mem_region(mdev->rmmio_base, mdev->rmmio_size);
+	if (ret)
 		return ret;
-	}
 
 	mdev->bpp_shifts[0] = 0;
 	mdev->bpp_shifts[1] = 1;
@@ -200,12 +189,6 @@ static int mgag200_device_init(struct drm_device *dev,
 	return 0;
 }
 
-void mgag200_device_fini(struct mga_device *mdev)
-{
-	release_mem_region(mdev->rmmio_base, mdev->rmmio_size);
-	mga_vram_fini(mdev);
-}
-
 /*
  * Functions here will be called by the core once it's bound the driver to
  * a PCI device
@@ -217,7 +200,7 @@ int mgag200_driver_load(struct drm_device *dev, unsigned long flags)
 	struct mga_device *mdev;
 	int r;
 
-	mdev = kzalloc(sizeof(struct mga_device), GFP_KERNEL);
+	mdev = devm_kzalloc(dev->dev, sizeof(struct mga_device), GFP_KERNEL);
 	if (mdev == NULL)
 		return -ENOMEM;
 	dev->dev_private = (void *)mdev;
@@ -234,8 +217,6 @@ int mgag200_driver_load(struct drm_device *dev, unsigned long flags)
 
 	drm_mode_config_init(dev);
 	dev->mode_config.funcs = (void *)&mga_mode_funcs;
-	dev->mode_config.min_width = 0;
-	dev->mode_config.min_height = 0;
 	dev->mode_config.preferred_depth = 24;
 	dev->mode_config.prefer_shadow = 1;
 
@@ -258,8 +239,6 @@ int mgag200_driver_unload(struct drm_device *dev)
 	mgag200_fbdev_fini(mdev);
 	drm_mode_config_cleanup(dev);
 	mgag200_mm_fini(mdev);
-	mgag200_device_fini(mdev);
-	kfree(mdev);
 	dev->dev_private = NULL;
 	return 0;
 }
diff --git a/drivers/gpu/drm/mgag200/mgag200_mode.c b/drivers/gpu/drm/mgag200/mgag200_mode.c
index 78d8e919509f..f9889658329b 100644
--- a/drivers/gpu/drm/mgag200/mgag200_mode.c
+++ b/drivers/gpu/drm/mgag200/mgag200_mode.c
@@ -1254,9 +1254,8 @@ static const struct drm_crtc_helper_funcs mga_helper_funcs = {
 };
 
 /* CRTC setup */
-static void mga_crtc_init(struct drm_device *dev)
+static void mga_crtc_init(struct mga_device *mdev)
 {
-	struct mga_device *mdev = dev->dev_private;
 	struct mga_crtc *mga_crtc;
 	int i;
 
@@ -1267,7 +1266,7 @@ static void mga_crtc_init(struct drm_device *dev)
 	if (mga_crtc == NULL)
 		return;
 
-	drm_crtc_init(dev, &mga_crtc->base, &mga_crtc_funcs);
+	drm_crtc_init(mdev->dev, &mga_crtc->base, &mga_crtc_funcs);
 
 	drm_mode_crtc_set_gamma_size(&mga_crtc->base, MGAG200_LUT_SIZE);
 	mdev->mode_info.crtc = mga_crtc;
@@ -1522,7 +1521,7 @@ int mgag200_modeset_init(struct mga_device *mdev)
 
 	mdev->dev->mode_config.fb_base = mdev->mc.vram_base;
 
-	mga_crtc_init(mdev->dev);
+	mga_crtc_init(mdev);
 
 	encoder = mga_encoder_init(mdev->dev);
 	if (!encoder) {
diff --git a/drivers/gpu/drm/mgag200/mgag200_ttm.c b/drivers/gpu/drm/mgag200/mgag200_ttm.c
index 8fc9d9201945..401c9891d3a8 100644
--- a/drivers/gpu/drm/mgag200/mgag200_ttm.c
+++ b/drivers/gpu/drm/mgag200/mgag200_ttm.c
@@ -315,8 +315,8 @@ int mgag200_bo_reserve(struct mgag200_bo *bo, bool no_wait)
 
 	ret = ttm_bo_reserve(&bo->bo, true, no_wait, false, 0);
 	if (ret) {
-		if (ret != -ERESTARTSYS)
-			DRM_ERROR("reserve failed %p\n", bo);
+		if (ret != -ERESTARTSYS && ret != -EBUSY)
+			DRM_ERROR("reserve failed %p %d\n", bo, ret);
 		return ret;
 	}
 	return 0;
diff --git a/drivers/gpu/drm/nouveau/Makefile b/drivers/gpu/drm/nouveau/Makefile
index 90f9140eeefd..998e8b4444f3 100644
--- a/drivers/gpu/drm/nouveau/Makefile
+++ b/drivers/gpu/drm/nouveau/Makefile
@@ -53,15 +53,6 @@ nouveau-y += core/subdev/clock/nva3.o
 nouveau-y += core/subdev/clock/nvc0.o
 nouveau-y += core/subdev/clock/pllnv04.o
 nouveau-y += core/subdev/clock/pllnva3.o
-nouveau-y += core/subdev/device/base.o
-nouveau-y += core/subdev/device/nv04.o
-nouveau-y += core/subdev/device/nv10.o
-nouveau-y += core/subdev/device/nv20.o
-nouveau-y += core/subdev/device/nv30.o
-nouveau-y += core/subdev/device/nv40.o
-nouveau-y += core/subdev/device/nv50.o
-nouveau-y += core/subdev/device/nvc0.o
-nouveau-y += core/subdev/device/nve0.o
 nouveau-y += core/subdev/devinit/base.o
 nouveau-y += core/subdev/devinit/nv04.o
 nouveau-y += core/subdev/devinit/nv05.o
@@ -126,6 +117,7 @@ nouveau-y += core/subdev/therm/ic.o
 nouveau-y += core/subdev/therm/temp.o
 nouveau-y += core/subdev/therm/nv40.o
 nouveau-y += core/subdev/therm/nv50.o
+nouveau-y += core/subdev/therm/nv84.o
 nouveau-y += core/subdev/therm/nva3.o
 nouveau-y += core/subdev/therm/nvd0.o
 nouveau-y += core/subdev/timer/base.o
@@ -150,6 +142,15 @@ nouveau-y += core/engine/copy/nvc0.o
 nouveau-y += core/engine/copy/nve0.o
 nouveau-y += core/engine/crypt/nv84.o
 nouveau-y += core/engine/crypt/nv98.o
+nouveau-y += core/engine/device/base.o
+nouveau-y += core/engine/device/nv04.o
+nouveau-y += core/engine/device/nv10.o
+nouveau-y += core/engine/device/nv20.o
+nouveau-y += core/engine/device/nv30.o
+nouveau-y += core/engine/device/nv40.o
+nouveau-y += core/engine/device/nv50.o
+nouveau-y += core/engine/device/nvc0.o
+nouveau-y += core/engine/device/nve0.o
 nouveau-y += core/engine/disp/base.o
 nouveau-y += core/engine/disp/nv04.o
 nouveau-y += core/engine/disp/nv50.o
@@ -159,6 +160,7 @@ nouveau-y += core/engine/disp/nva0.o
 nouveau-y += core/engine/disp/nva3.o
 nouveau-y += core/engine/disp/nvd0.o
 nouveau-y += core/engine/disp/nve0.o
+nouveau-y += core/engine/disp/nvf0.o
 nouveau-y += core/engine/disp/dacnv50.o
 nouveau-y += core/engine/disp/dport.o
 nouveau-y += core/engine/disp/hdanva3.o
@@ -212,7 +214,7 @@ nouveau-y += core/engine/vp/nve0.o
 
 # drm/core
 nouveau-y += nouveau_drm.o nouveau_chan.o nouveau_dma.o nouveau_fence.o
-nouveau-y += nouveau_irq.o nouveau_vga.o nouveau_agp.o
+nouveau-y += nouveau_vga.o nouveau_agp.o
 nouveau-y += nouveau_ttm.o nouveau_sgdma.o nouveau_bo.o nouveau_gem.o
 nouveau-y += nouveau_prime.o nouveau_abi16.o
 nouveau-y += nv04_fence.o nv10_fence.o nv17_fence.o
@@ -224,9 +226,7 @@ nouveau-y += nouveau_connector.o nouveau_dp.o
 nouveau-y += nv04_fbcon.o nv50_fbcon.o nvc0_fbcon.o
 
 # drm/kms/nv04:nv50
-nouveau-y += nouveau_hw.o nouveau_calc.o
-nouveau-y += nv04_dac.o nv04_dfp.o nv04_tv.o nv17_tv.o nv17_tv_modes.o
-nouveau-y += nv04_crtc.o nv04_display.o nv04_cursor.o
+include $(src)/dispnv04/Makefile
 
 # drm/kms/nv50-
 nouveau-y += nv50_display.o
diff --git a/drivers/gpu/drm/nouveau/core/core/client.c b/drivers/gpu/drm/nouveau/core/core/client.c
index 295c22165eac..9079c0ac58e6 100644
--- a/drivers/gpu/drm/nouveau/core/core/client.c
+++ b/drivers/gpu/drm/nouveau/core/core/client.c
@@ -27,7 +27,7 @@
 #include <core/handle.h>
 #include <core/option.h>
 
-#include <subdev/device.h>
+#include <engine/device.h>
 
 static void
 nouveau_client_dtor(struct nouveau_object *object)
@@ -58,8 +58,9 @@ nouveau_client_create_(const char *name, u64 devname, const char *cfg,
 		return -ENODEV;
 
 	ret = nouveau_namedb_create_(NULL, NULL, &nouveau_client_oclass,
-				     NV_CLIENT_CLASS, nouveau_device_sclass,
-				     0, length, pobject);
+				     NV_CLIENT_CLASS, NULL,
+				     (1ULL << NVDEV_ENGINE_DEVICE),
+				     length, pobject);
 	client = *pobject;
 	if (ret)
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/core/engine.c b/drivers/gpu/drm/nouveau/core/core/engine.c
index 09b3bd502fd0..c8bed4a26833 100644
--- a/drivers/gpu/drm/nouveau/core/core/engine.c
+++ b/drivers/gpu/drm/nouveau/core/core/engine.c
@@ -33,7 +33,6 @@ nouveau_engine_create_(struct nouveau_object *parent,
 		       const char *iname, const char *fname,
 		       int length, void **pobject)
 {
-	struct nouveau_device *device = nv_device(parent);
 	struct nouveau_engine *engine;
 	int ret;
 
@@ -43,7 +42,8 @@ nouveau_engine_create_(struct nouveau_object *parent,
 	if (ret)
 		return ret;
 
-	if (!nouveau_boolopt(device->cfgopt, iname, enable)) {
+	if ( parent &&
+	    !nouveau_boolopt(nv_device(parent)->cfgopt, iname, enable)) {
 		if (!enable)
 			nv_warn(engine, "disabled, %s=1 to enable\n", iname);
 		return -ENODEV;
diff --git a/drivers/gpu/drm/nouveau/core/core/event.c b/drivers/gpu/drm/nouveau/core/core/event.c
index 6d01e0f0fc8a..7eb81c1b6fab 100644
--- a/drivers/gpu/drm/nouveau/core/core/event.c
+++ b/drivers/gpu/drm/nouveau/core/core/event.c
@@ -27,8 +27,10 @@ static void
 nouveau_event_put_locked(struct nouveau_event *event, int index,
 			 struct nouveau_eventh *handler)
 {
-	if (!--event->index[index].refs)
-		event->disable(event, index);
+	if (!--event->index[index].refs) {
+		if (event->disable)
+			event->disable(event, index);
+	}
 	list_del(&handler->head);
 }
 
@@ -53,8 +55,10 @@ nouveau_event_get(struct nouveau_event *event, int index,
 	spin_lock_irqsave(&event->lock, flags);
 	if (index < event->index_nr) {
 		list_add(&handler->head, &event->index[index].list);
-		if (!event->index[index].refs++)
-			event->enable(event, index);
+		if (!event->index[index].refs++) {
+			if (event->enable)
+				event->enable(event, index);
+		}
 	}
 	spin_unlock_irqrestore(&event->lock, flags);
 }
diff --git a/drivers/gpu/drm/nouveau/core/core/object.c b/drivers/gpu/drm/nouveau/core/core/object.c
index 3b2e7b6304d3..7f48e288215f 100644
--- a/drivers/gpu/drm/nouveau/core/core/object.c
+++ b/drivers/gpu/drm/nouveau/core/core/object.c
@@ -136,26 +136,30 @@ nouveau_object_ctor(struct nouveau_object *parent,
 		    struct nouveau_object **pobject)
 {
 	struct nouveau_ofuncs *ofuncs = oclass->ofuncs;
+	struct nouveau_object *object = NULL;
 	int ret;
 
-	*pobject = NULL;
-
-	ret = ofuncs->ctor(parent, engine, oclass, data, size, pobject);
+	ret = ofuncs->ctor(parent, engine, oclass, data, size, &object);
+	*pobject = object;
 	if (ret < 0) {
 		if (ret != -ENODEV) {
 			nv_error(parent, "failed to create 0x%08x, %d\n",
 				 oclass->handle, ret);
 		}
 
-		if (*pobject) {
-			ofuncs->dtor(*pobject);
+		if (object) {
+			ofuncs->dtor(object);
 			*pobject = NULL;
 		}
 
 		return ret;
 	}
 
-	nv_debug(*pobject, "created\n");
+	if (ret == 0) {
+		nv_debug(object, "created\n");
+		atomic_set(&object->refcount, 1);
+	}
+
 	return 0;
 }
 
@@ -327,6 +331,7 @@ nouveau_object_inc(struct nouveau_object *object)
 	}
 
 	ret = nv_ofuncs(object)->init(object);
+	atomic_set(&object->usecount, 1);
 	if (ret) {
 		nv_error(object, "init failed, %d\n", ret);
 		goto fail_self;
@@ -357,6 +362,7 @@ nouveau_object_decf(struct nouveau_object *object)
 	nv_trace(object, "stopping...\n");
 
 	ret = nv_ofuncs(object)->fini(object, false);
+	atomic_set(&object->usecount, 0);
 	if (ret)
 		nv_warn(object, "failed fini, %d\n", ret);
 
@@ -381,6 +387,7 @@ nouveau_object_decs(struct nouveau_object *object)
 	nv_trace(object, "suspending...\n");
 
 	ret = nv_ofuncs(object)->fini(object, true);
+	atomic_set(&object->usecount, 0);
 	if (ret) {
 		nv_error(object, "failed suspend, %d\n", ret);
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/core/parent.c b/drivers/gpu/drm/nouveau/core/core/parent.c
index db7c54943102..313380ce632d 100644
--- a/drivers/gpu/drm/nouveau/core/core/parent.c
+++ b/drivers/gpu/drm/nouveau/core/core/parent.c
@@ -24,6 +24,7 @@
 
 #include <core/object.h>
 #include <core/parent.h>
+#include <core/client.h>
 
 int
 nouveau_parent_sclass(struct nouveau_object *parent, u16 handle,
@@ -50,7 +51,12 @@ nouveau_parent_sclass(struct nouveau_object *parent, u16 handle,
 	while (mask) {
 		int i = ffsll(mask) - 1;
 
-		if ((engine = nouveau_engine(parent, i))) {
+		if (nv_iclass(parent, NV_CLIENT_CLASS))
+			engine = nv_engine(nv_client(parent)->device);
+		else
+			engine = nouveau_engine(parent, i);
+
+		if (engine) {
 			oclass = engine->sclass;
 			while (oclass->ofuncs) {
 				if ((oclass->handle & 0xffff) == handle) {
diff --git a/drivers/gpu/drm/nouveau/core/subdev/device/base.c b/drivers/gpu/drm/nouveau/core/engine/device/base.c
index 3937ced5c753..4c72571655ad 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/device/base.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/base.c
@@ -29,7 +29,7 @@
 
 #include <core/class.h>
 
-#include <subdev/device.h>
+#include <engine/device.h>
 
 static DEFINE_MUTEX(nv_devices_mutex);
 static LIST_HEAD(nv_devices);
@@ -55,7 +55,6 @@ nouveau_device_find(u64 name)
 struct nouveau_devobj {
 	struct nouveau_parent base;
 	struct nouveau_object *subdev[NVDEV_SUBDEV_NR];
-	bool created;
 };
 
 static const u64 disable_map[] = {
@@ -173,7 +172,8 @@ nouveau_devobj_ctor(struct nouveau_object *parent,
 			case 0xa0: device->card_type = NV_50; break;
 			case 0xc0: device->card_type = NV_C0; break;
 			case 0xd0: device->card_type = NV_D0; break;
-			case 0xe0: device->card_type = NV_E0; break;
+			case 0xe0:
+			case 0xf0: device->card_type = NV_E0; break;
 			default:
 				break;
 			}
@@ -238,26 +238,24 @@ nouveau_devobj_ctor(struct nouveau_object *parent,
 	}
 
 	/* ensure requested subsystems are available for use */
-	for (i = 0, c = 0; i < NVDEV_SUBDEV_NR; i++) {
+	for (i = 1, c = 1; i < NVDEV_SUBDEV_NR; i++) {
 		if (!(oclass = device->oclass[i]) || (disable & (1ULL << i)))
 			continue;
 
-		if (!device->subdev[i]) {
-			ret = nouveau_object_ctor(nv_object(device), NULL,
-						  oclass, NULL, i,
-						  &devobj->subdev[i]);
-			if (ret == -ENODEV)
-				continue;
-			if (ret)
-				return ret;
-
-			if (nv_iclass(devobj->subdev[i], NV_ENGINE_CLASS))
-				nouveau_subdev_reset(devobj->subdev[i]);
-		} else {
+		if (device->subdev[i]) {
 			nouveau_object_ref(device->subdev[i],
 					  &devobj->subdev[i]);
+			continue;
 		}
 
+		ret = nouveau_object_ctor(nv_object(device), NULL,
+					  oclass, NULL, i,
+					  &devobj->subdev[i]);
+		if (ret == -ENODEV)
+			continue;
+		if (ret)
+			return ret;
+
 		/* note: can't init *any* subdevs until devinit has been run
 		 * due to not knowing exactly what the vbios init tables will
 		 * mess with.  devinit also can't be run until all of its
@@ -273,6 +271,10 @@ nouveau_devobj_ctor(struct nouveau_object *parent,
 				ret = nouveau_object_inc(subdev);
 				if (ret)
 					return ret;
+				atomic_dec(&nv_object(device)->usecount);
+			} else
+			if (subdev) {
+				nouveau_subdev_reset(subdev);
 			}
 		}
 	}
@@ -292,74 +294,6 @@ nouveau_devobj_dtor(struct nouveau_object *object)
 	nouveau_parent_destroy(&devobj->base);
 }
 
-static int
-nouveau_devobj_init(struct nouveau_object *object)
-{
-	struct nouveau_devobj *devobj = (void *)object;
-	struct nouveau_object *subdev;
-	int ret, i;
-
-	ret = nouveau_parent_init(&devobj->base);
-	if (ret)
-		return ret;
-
-	for (i = 0; devobj->created && i < NVDEV_SUBDEV_NR; i++) {
-		if ((subdev = devobj->subdev[i])) {
-			if (!nv_iclass(subdev, NV_ENGINE_CLASS)) {
-				ret = nouveau_object_inc(subdev);
-				if (ret)
-					goto fail;
-			}
-		}
-	}
-
-	devobj->created = true;
-	return 0;
-
-fail:
-	for (--i; i >= 0; i--) {
-		if ((subdev = devobj->subdev[i])) {
-			if (!nv_iclass(subdev, NV_ENGINE_CLASS))
-				nouveau_object_dec(subdev, false);
-		}
-	}
-
-	return ret;
-}
-
-static int
-nouveau_devobj_fini(struct nouveau_object *object, bool suspend)
-{
-	struct nouveau_devobj *devobj = (void *)object;
-	struct nouveau_object *subdev;
-	int ret, i;
-
-	for (i = NVDEV_SUBDEV_NR - 1; i >= 0; i--) {
-		if ((subdev = devobj->subdev[i])) {
-			if (!nv_iclass(subdev, NV_ENGINE_CLASS)) {
-				ret = nouveau_object_dec(subdev, suspend);
-				if (ret && suspend)
-					goto fail;
-			}
-		}
-	}
-
-	ret = nouveau_parent_fini(&devobj->base, suspend);
-fail:
-	for (; ret && suspend && i < NVDEV_SUBDEV_NR; i++) {
-		if ((subdev = devobj->subdev[i])) {
-			if (!nv_iclass(subdev, NV_ENGINE_CLASS)) {
-				ret = nouveau_object_inc(subdev);
-				if (ret) {
-					/* XXX */
-				}
-			}
-		}
-	}
-
-	return ret;
-}
-
 static u8
 nouveau_devobj_rd08(struct nouveau_object *object, u64 addr)
 {
@@ -400,8 +334,8 @@ static struct nouveau_ofuncs
 nouveau_devobj_ofuncs = {
 	.ctor = nouveau_devobj_ctor,
 	.dtor = nouveau_devobj_dtor,
-	.init = nouveau_devobj_init,
-	.fini = nouveau_devobj_fini,
+	.init = _nouveau_parent_init,
+	.fini = _nouveau_parent_fini,
 	.rd08 = nouveau_devobj_rd08,
 	.rd16 = nouveau_devobj_rd16,
 	.rd32 = nouveau_devobj_rd32,
@@ -413,12 +347,76 @@ nouveau_devobj_ofuncs = {
 /******************************************************************************
  * nouveau_device: engine functions
  *****************************************************************************/
-struct nouveau_oclass
+static struct nouveau_oclass
 nouveau_device_sclass[] = {
 	{ 0x0080, &nouveau_devobj_ofuncs },
 	{}
 };
 
+static int
+nouveau_device_fini(struct nouveau_object *object, bool suspend)
+{
+	struct nouveau_device *device = (void *)object;
+	struct nouveau_object *subdev;
+	int ret, i;
+
+	for (i = NVDEV_SUBDEV_NR - 1; i >= 0; i--) {
+		if ((subdev = device->subdev[i])) {
+			if (!nv_iclass(subdev, NV_ENGINE_CLASS)) {
+				ret = nouveau_object_dec(subdev, suspend);
+				if (ret && suspend)
+					goto fail;
+			}
+		}
+	}
+
+	ret = 0;
+fail:
+	for (; ret && i < NVDEV_SUBDEV_NR; i++) {
+		if ((subdev = device->subdev[i])) {
+			if (!nv_iclass(subdev, NV_ENGINE_CLASS)) {
+				ret = nouveau_object_inc(subdev);
+				if (ret) {
+					/* XXX */
+				}
+			}
+		}
+	}
+
+	return ret;
+}
+
+static int
+nouveau_device_init(struct nouveau_object *object)
+{
+	struct nouveau_device *device = (void *)object;
+	struct nouveau_object *subdev;
+	int ret, i;
+
+	for (i = 0; i < NVDEV_SUBDEV_NR; i++) {
+		if ((subdev = device->subdev[i])) {
+			if (!nv_iclass(subdev, NV_ENGINE_CLASS)) {
+				ret = nouveau_object_inc(subdev);
+				if (ret)
+					goto fail;
+			} else {
+				nouveau_subdev_reset(subdev);
+			}
+		}
+	}
+
+	ret = 0;
+fail:
+	for (--i; ret && i >= 0; i--) {
+		if ((subdev = device->subdev[i])) {
+			if (!nv_iclass(subdev, NV_ENGINE_CLASS))
+				nouveau_object_dec(subdev, false);
+		}
+	}
+
+	return ret;
+}
+
 static void
 nouveau_device_dtor(struct nouveau_object *object)
 {
@@ -428,17 +426,19 @@ nouveau_device_dtor(struct nouveau_object *object)
 	list_del(&device->head);
 	mutex_unlock(&nv_devices_mutex);
 
-	if (device->base.mmio)
-		iounmap(device->base.mmio);
+	if (nv_subdev(device)->mmio)
+		iounmap(nv_subdev(device)->mmio);
 
-	nouveau_subdev_destroy(&device->base);
+	nouveau_engine_destroy(&device->base);
 }
 
 static struct nouveau_oclass
 nouveau_device_oclass = {
-	.handle = NV_SUBDEV(DEVICE, 0x00),
+	.handle = NV_ENGINE(DEVICE, 0x00),
 	.ofuncs = &(struct nouveau_ofuncs) {
 		.dtor = nouveau_device_dtor,
+		.init = nouveau_device_init,
+		.fini = nouveau_device_fini,
 	},
 };
 
@@ -456,13 +456,12 @@ nouveau_device_create_(struct pci_dev *pdev, u64 name, const char *sname,
 			goto done;
 	}
 
-	ret = nouveau_subdev_create_(NULL, NULL, &nouveau_device_oclass, 0,
+	ret = nouveau_engine_create_(NULL, NULL, &nouveau_device_oclass, true,
 				     "DEVICE", "device", length, pobject);
 	device = *pobject;
 	if (ret)
 		goto done;
 
-	atomic_set(&nv_object(device)->usecount, 2);
 	device->pdev = pdev;
 	device->handle = name;
 	device->cfgopt = cfg;
@@ -470,6 +469,7 @@ nouveau_device_create_(struct pci_dev *pdev, u64 name, const char *sname,
 	device->name = sname;
 
 	nv_subdev(device)->debug = nouveau_dbgopt(device->dbgopt, "DEVICE");
+	nv_engine(device)->sclass = nouveau_device_sclass;
 	list_add(&device->head, &nv_devices);
 done:
 	mutex_unlock(&nv_devices_mutex);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/device/nv04.c b/drivers/gpu/drm/nouveau/core/engine/device/nv04.c
index 473c5c03d3c9..a0284cf09c0f 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/device/nv04.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/nv04.c
@@ -22,7 +22,6 @@
  * Authors: Ben Skeggs
  */
 
-#include <subdev/device.h>
 #include <subdev/bios.h>
 #include <subdev/bus.h>
 #include <subdev/i2c.h>
@@ -34,6 +33,7 @@
 #include <subdev/instmem.h>
 #include <subdev/vm.h>
 
+#include <engine/device.h>
 #include <engine/dmaobj.h>
 #include <engine/fifo.h>
 #include <engine/software.h>
diff --git a/drivers/gpu/drm/nouveau/core/subdev/device/nv10.c b/drivers/gpu/drm/nouveau/core/engine/device/nv10.c
index d0774f5bebe1..1b7809a095c3 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/device/nv10.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/nv10.c
@@ -22,7 +22,6 @@
  * Authors: Ben Skeggs
  */
 
-#include <subdev/device.h>
 #include <subdev/bios.h>
 #include <subdev/bus.h>
 #include <subdev/gpio.h>
@@ -35,6 +34,7 @@
 #include <subdev/instmem.h>
 #include <subdev/vm.h>
 
+#include <engine/device.h>
 #include <engine/dmaobj.h>
 #include <engine/fifo.h>
 #include <engine/software.h>
diff --git a/drivers/gpu/drm/nouveau/core/subdev/device/nv20.c b/drivers/gpu/drm/nouveau/core/engine/device/nv20.c
index ab920e0dc45b..12a4005fa619 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/device/nv20.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/nv20.c
@@ -22,7 +22,6 @@
  * Authors: Ben Skeggs
  */
 
-#include <subdev/device.h>
 #include <subdev/bios.h>
 #include <subdev/bus.h>
 #include <subdev/gpio.h>
@@ -36,6 +35,7 @@
 #include <subdev/instmem.h>
 #include <subdev/vm.h>
 
+#include <engine/device.h>
 #include <engine/dmaobj.h>
 #include <engine/fifo.h>
 #include <engine/software.h>
diff --git a/drivers/gpu/drm/nouveau/core/subdev/device/nv30.c b/drivers/gpu/drm/nouveau/core/engine/device/nv30.c
index 5f2110261b04..cef0f1ea4c21 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/device/nv30.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/nv30.c
@@ -22,7 +22,6 @@
  * Authors: Ben Skeggs
  */
 
-#include <subdev/device.h>
 #include <subdev/bios.h>
 #include <subdev/bus.h>
 #include <subdev/gpio.h>
@@ -35,6 +34,7 @@
 #include <subdev/instmem.h>
 #include <subdev/vm.h>
 
+#include <engine/device.h>
 #include <engine/dmaobj.h>
 #include <engine/fifo.h>
 #include <engine/software.h>
diff --git a/drivers/gpu/drm/nouveau/core/subdev/device/nv40.c b/drivers/gpu/drm/nouveau/core/engine/device/nv40.c
index f3d55efe9ac9..1719cb0ee595 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/device/nv40.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/nv40.c
@@ -22,7 +22,6 @@
  * Authors: Ben Skeggs
  */
 
-#include <subdev/device.h>
 #include <subdev/bios.h>
 #include <subdev/bus.h>
 #include <subdev/vm.h>
@@ -37,6 +36,7 @@
 #include <subdev/instmem.h>
 #include <subdev/vm.h>
 
+#include <engine/device.h>
 #include <engine/dmaobj.h>
 #include <engine/fifo.h>
 #include <engine/software.h>
diff --git a/drivers/gpu/drm/nouveau/core/subdev/device/nv50.c b/drivers/gpu/drm/nouveau/core/engine/device/nv50.c
index 5ed2fa51ddc2..5e8c3de75593 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/device/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/nv50.c
@@ -22,7 +22,6 @@
  * Authors: Ben Skeggs
  */
 
-#include <subdev/device.h>
 #include <subdev/bios.h>
 #include <subdev/bus.h>
 #include <subdev/gpio.h>
@@ -38,6 +37,7 @@
 #include <subdev/vm.h>
 #include <subdev/bar.h>
 
+#include <engine/device.h>
 #include <engine/dmaobj.h>
 #include <engine/fifo.h>
 #include <engine/software.h>
@@ -83,7 +83,7 @@ nv50_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_GPIO   ] = &nv50_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] = &nv50_i2c_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nv50_clock_oclass;
-		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv50_therm_oclass;
+		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
 		device->oclass[NVDEV_SUBDEV_DEVINIT] = &nv50_devinit_oclass;
 		device->oclass[NVDEV_SUBDEV_MC     ] = &nv50_mc_oclass;
@@ -109,7 +109,7 @@ nv50_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_GPIO   ] = &nv50_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] = &nv50_i2c_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nv50_clock_oclass;
-		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv50_therm_oclass;
+		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
 		device->oclass[NVDEV_SUBDEV_DEVINIT] = &nv50_devinit_oclass;
 		device->oclass[NVDEV_SUBDEV_MC     ] = &nv50_mc_oclass;
@@ -135,7 +135,7 @@ nv50_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_GPIO   ] = &nv50_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] = &nv50_i2c_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nv50_clock_oclass;
-		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv50_therm_oclass;
+		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
 		device->oclass[NVDEV_SUBDEV_DEVINIT] = &nv50_devinit_oclass;
 		device->oclass[NVDEV_SUBDEV_MC     ] = &nv50_mc_oclass;
@@ -161,7 +161,7 @@ nv50_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_GPIO   ] = &nv50_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] = &nv94_i2c_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nv50_clock_oclass;
-		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv50_therm_oclass;
+		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
 		device->oclass[NVDEV_SUBDEV_DEVINIT] = &nv50_devinit_oclass;
 		device->oclass[NVDEV_SUBDEV_MC     ] = &nv50_mc_oclass;
@@ -187,7 +187,7 @@ nv50_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_GPIO   ] = &nv50_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] = &nv94_i2c_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nv50_clock_oclass;
-		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv50_therm_oclass;
+		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
 		device->oclass[NVDEV_SUBDEV_DEVINIT] = &nv50_devinit_oclass;
 		device->oclass[NVDEV_SUBDEV_MC     ] = &nv50_mc_oclass;
@@ -213,7 +213,7 @@ nv50_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_GPIO   ] = &nv50_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] = &nv94_i2c_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nv50_clock_oclass;
-		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv50_therm_oclass;
+		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
 		device->oclass[NVDEV_SUBDEV_DEVINIT] = &nv50_devinit_oclass;
 		device->oclass[NVDEV_SUBDEV_MC     ] = &nv98_mc_oclass;
@@ -239,7 +239,7 @@ nv50_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_GPIO   ] = &nv50_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] = &nv50_i2c_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nv50_clock_oclass;
-		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv50_therm_oclass;
+		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
 		device->oclass[NVDEV_SUBDEV_DEVINIT] = &nv50_devinit_oclass;
 		device->oclass[NVDEV_SUBDEV_MC     ] = &nv98_mc_oclass;
@@ -265,7 +265,7 @@ nv50_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_GPIO   ] = &nv50_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] = &nv94_i2c_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nv50_clock_oclass;
-		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv50_therm_oclass;
+		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
 		device->oclass[NVDEV_SUBDEV_DEVINIT] = &nv50_devinit_oclass;
 		device->oclass[NVDEV_SUBDEV_MC     ] = &nv98_mc_oclass;
@@ -291,7 +291,7 @@ nv50_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_GPIO   ] = &nv50_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] = &nv94_i2c_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nv50_clock_oclass;
-		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv50_therm_oclass;
+		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
 		device->oclass[NVDEV_SUBDEV_DEVINIT] = &nv50_devinit_oclass;
 		device->oclass[NVDEV_SUBDEV_MC     ] = &nv98_mc_oclass;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/device/nvc0.c b/drivers/gpu/drm/nouveau/core/engine/device/nvc0.c
index 4393eb4d6564..955af122c3a6 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/device/nvc0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/nvc0.c
@@ -22,7 +22,6 @@
  * Authors: Ben Skeggs
  */
 
-#include <subdev/device.h>
 #include <subdev/bios.h>
 #include <subdev/bus.h>
 #include <subdev/gpio.h>
@@ -40,6 +39,7 @@
 #include <subdev/vm.h>
 #include <subdev/bar.h>
 
+#include <engine/device.h>
 #include <engine/dmaobj.h>
 #include <engine/fifo.h>
 #include <engine/software.h>
@@ -285,6 +285,34 @@ nvc0_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_ENGINE_COPY0  ] = &nvc0_copy0_oclass;
 		device->oclass[NVDEV_ENGINE_DISP   ] = &nvd0_disp_oclass;
 		break;
+	case 0xd7:
+		device->cname = "GF117";
+		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] = &nvd0_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_I2C    ] = &nvd0_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nvc0_clock_oclass;
+		device->oclass[NVDEV_SUBDEV_THERM  ] = &nvd0_therm_oclass;
+		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
+		device->oclass[NVDEV_SUBDEV_DEVINIT] = &nv50_devinit_oclass;
+		device->oclass[NVDEV_SUBDEV_MC     ] = &nvc0_mc_oclass;
+		device->oclass[NVDEV_SUBDEV_BUS    ] = &nvc0_bus_oclass;
+		device->oclass[NVDEV_SUBDEV_TIMER  ] = &nv04_timer_oclass;
+		device->oclass[NVDEV_SUBDEV_FB     ] = &nvc0_fb_oclass;
+		device->oclass[NVDEV_SUBDEV_LTCG   ] = &nvc0_ltcg_oclass;
+		device->oclass[NVDEV_SUBDEV_IBUS   ] = &nvc0_ibus_oclass;
+		device->oclass[NVDEV_SUBDEV_INSTMEM] = &nv50_instmem_oclass;
+		device->oclass[NVDEV_SUBDEV_VM     ] = &nvc0_vmmgr_oclass;
+		device->oclass[NVDEV_SUBDEV_BAR    ] = &nvc0_bar_oclass;
+		device->oclass[NVDEV_ENGINE_DMAOBJ ] = &nvd0_dmaeng_oclass;
+		device->oclass[NVDEV_ENGINE_FIFO   ] = &nvc0_fifo_oclass;
+		device->oclass[NVDEV_ENGINE_SW     ] = &nvc0_software_oclass;
+		device->oclass[NVDEV_ENGINE_GR     ] = &nvc0_graph_oclass;
+		device->oclass[NVDEV_ENGINE_VP     ] = &nvc0_vp_oclass;
+		device->oclass[NVDEV_ENGINE_BSP    ] = &nvc0_bsp_oclass;
+		device->oclass[NVDEV_ENGINE_PPP    ] = &nvc0_ppp_oclass;
+		device->oclass[NVDEV_ENGINE_COPY0  ] = &nvc0_copy0_oclass;
+		device->oclass[NVDEV_ENGINE_DISP   ] = &nvd0_disp_oclass;
+		break;
 	default:
 		nv_fatal(device, "unknown Fermi chipset\n");
 		return -EINVAL;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/device/nve0.c b/drivers/gpu/drm/nouveau/core/engine/device/nve0.c
index 5c12391619fd..a354e409cdff 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/device/nve0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/nve0.c
@@ -22,7 +22,6 @@
  * Authors: Ben Skeggs
  */
 
-#include <subdev/device.h>
 #include <subdev/bios.h>
 #include <subdev/bus.h>
 #include <subdev/gpio.h>
@@ -40,6 +39,7 @@
 #include <subdev/vm.h>
 #include <subdev/bar.h>
 
+#include <engine/device.h>
 #include <engine/dmaobj.h>
 #include <engine/fifo.h>
 #include <engine/software.h>
@@ -141,6 +141,40 @@ nve0_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_ENGINE_VP     ] = &nve0_vp_oclass;
 		device->oclass[NVDEV_ENGINE_PPP    ] = &nvc0_ppp_oclass;
 		break;
+	case 0xf0:
+		device->cname = "GK110";
+		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] = &nve0_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_I2C    ] = &nvd0_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nvc0_clock_oclass;
+		device->oclass[NVDEV_SUBDEV_THERM  ] = &nvd0_therm_oclass;
+		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
+		device->oclass[NVDEV_SUBDEV_DEVINIT] = &nv50_devinit_oclass;
+		device->oclass[NVDEV_SUBDEV_MC     ] = &nvc0_mc_oclass;
+		device->oclass[NVDEV_SUBDEV_BUS    ] = &nvc0_bus_oclass;
+		device->oclass[NVDEV_SUBDEV_TIMER  ] = &nv04_timer_oclass;
+		device->oclass[NVDEV_SUBDEV_FB     ] = &nvc0_fb_oclass;
+		device->oclass[NVDEV_SUBDEV_LTCG   ] = &nvc0_ltcg_oclass;
+		device->oclass[NVDEV_SUBDEV_IBUS   ] = &nve0_ibus_oclass;
+		device->oclass[NVDEV_SUBDEV_INSTMEM] = &nv50_instmem_oclass;
+		device->oclass[NVDEV_SUBDEV_VM     ] = &nvc0_vmmgr_oclass;
+		device->oclass[NVDEV_SUBDEV_BAR    ] = &nvc0_bar_oclass;
+		device->oclass[NVDEV_ENGINE_DMAOBJ ] = &nvd0_dmaeng_oclass;
+#if 0
+		device->oclass[NVDEV_ENGINE_FIFO   ] = &nve0_fifo_oclass;
+		device->oclass[NVDEV_ENGINE_SW     ] = &nvc0_software_oclass;
+		device->oclass[NVDEV_ENGINE_GR     ] = &nve0_graph_oclass;
+#endif
+		device->oclass[NVDEV_ENGINE_DISP   ] = &nvf0_disp_oclass;
+#if 0
+		device->oclass[NVDEV_ENGINE_COPY0  ] = &nve0_copy0_oclass;
+		device->oclass[NVDEV_ENGINE_COPY1  ] = &nve0_copy1_oclass;
+		device->oclass[NVDEV_ENGINE_COPY2  ] = &nve0_copy2_oclass;
+		device->oclass[NVDEV_ENGINE_BSP    ] = &nve0_bsp_oclass;
+		device->oclass[NVDEV_ENGINE_VP     ] = &nve0_vp_oclass;
+		device->oclass[NVDEV_ENGINE_PPP    ] = &nvc0_ppp_oclass;
+#endif
+		break;
 	default:
 		nv_fatal(device, "unknown Kepler chipset\n");
 		return -EINVAL;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/dport.c b/drivers/gpu/drm/nouveau/core/engine/disp/dport.c
index fa27b02ff829..31cc8fe8e7f0 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/dport.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/dport.c
@@ -191,7 +191,7 @@ dp_link_train_cr(struct dp_state *dp)
 static int
 dp_link_train_eq(struct dp_state *dp)
 {
-	bool eq_done, cr_done = true;
+	bool eq_done = false, cr_done = true;
 	int tries = 0, i;
 
 	dp_set_training_pattern(dp, 2);
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nv50.c b/drivers/gpu/drm/nouveau/core/engine/disp/nv50.c
index 02e369f80449..6a38402fa56c 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nv50.c
@@ -572,7 +572,8 @@ nv50_disp_base_ctor(struct nouveau_object *parent,
 	priv->base.vblank->priv = priv;
 	priv->base.vblank->enable = nv50_disp_base_vblank_enable;
 	priv->base.vblank->disable = nv50_disp_base_vblank_disable;
-	return nouveau_ramht_new(parent, parent, 0x1000, 0, &base->ramht);
+	return nouveau_ramht_new(nv_object(base), nv_object(base), 0x1000, 0,
+				&base->ramht);
 }
 
 static void
@@ -719,7 +720,7 @@ nv50_disp_data_ctor(struct nouveau_object *parent,
 	if (nv_mclass(parent) != NV_DEVICE_CLASS) {
 		atomic_inc(&parent->refcount);
 		*pobject = parent;
-		return 0;
+		return 1;
 	}
 
 	/* allocate display hardware to client */
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c b/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c
index 788dd34ccb54..019eacd8a68f 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c
@@ -473,7 +473,8 @@ nvd0_disp_base_ctor(struct nouveau_object *parent,
 	priv->base.vblank->enable = nvd0_disp_base_vblank_enable;
 	priv->base.vblank->disable = nvd0_disp_base_vblank_disable;
 
-	return nouveau_ramht_new(parent, parent, 0x1000, 0, &base->ramht);
+	return nouveau_ramht_new(nv_object(base), nv_object(base), 0x1000, 0,
+				&base->ramht);
 }
 
 static void
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nvf0.c b/drivers/gpu/drm/nouveau/core/engine/disp/nvf0.c
new file mode 100644
index 000000000000..a488c36e40f9
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nvf0.c
@@ -0,0 +1,89 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include <engine/software.h>
+#include <engine/disp.h>
+
+#include <core/class.h>
+
+#include "nv50.h"
+
+static struct nouveau_oclass
+nvf0_disp_sclass[] = {
+	{ NVF0_DISP_MAST_CLASS, &nvd0_disp_mast_ofuncs },
+	{ NVF0_DISP_SYNC_CLASS, &nvd0_disp_sync_ofuncs },
+	{ NVF0_DISP_OVLY_CLASS, &nvd0_disp_ovly_ofuncs },
+	{ NVF0_DISP_OIMM_CLASS, &nvd0_disp_oimm_ofuncs },
+	{ NVF0_DISP_CURS_CLASS, &nvd0_disp_curs_ofuncs },
+	{}
+};
+
+static struct nouveau_oclass
+nvf0_disp_base_oclass[] = {
+	{ NVF0_DISP_CLASS, &nvd0_disp_base_ofuncs, nva3_disp_base_omthds },
+	{}
+};
+
+static int
+nvf0_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
+	       struct nouveau_oclass *oclass, void *data, u32 size,
+	       struct nouveau_object **pobject)
+{
+	struct nv50_disp_priv *priv;
+	int heads = nv_rd32(parent, 0x022448);
+	int ret;
+
+	ret = nouveau_disp_create(parent, engine, oclass, heads,
+				  "PDISP", "display", &priv);
+	*pobject = nv_object(priv);
+	if (ret)
+		return ret;
+
+	nv_engine(priv)->sclass = nvf0_disp_base_oclass;
+	nv_engine(priv)->cclass = &nv50_disp_cclass;
+	nv_subdev(priv)->intr = nvd0_disp_intr;
+	INIT_WORK(&priv->supervisor, nvd0_disp_intr_supervisor);
+	priv->sclass = nvf0_disp_sclass;
+	priv->head.nr = heads;
+	priv->dac.nr = 3;
+	priv->sor.nr = 4;
+	priv->dac.power = nv50_dac_power;
+	priv->dac.sense = nv50_dac_sense;
+	priv->sor.power = nv50_sor_power;
+	priv->sor.hda_eld = nvd0_hda_eld;
+	priv->sor.hdmi = nvd0_hdmi_ctrl;
+	priv->sor.dp = &nvd0_sor_dp_func;
+	return 0;
+}
+
+struct nouveau_oclass
+nvf0_disp_oclass = {
+	.handle = NV_ENGINE(DISP, 0x92),
+	.ofuncs = &(struct nouveau_ofuncs) {
+		.ctor = nvf0_disp_ctor,
+		.dtor = _nouveau_disp_dtor,
+		.init = _nouveau_disp_init,
+		.fini = _nouveau_disp_fini,
+	},
+};
diff --git a/drivers/gpu/drm/nouveau/core/engine/dmaobj/nvd0.c b/drivers/gpu/drm/nouveau/core/engine/dmaobj/nvd0.c
index d1528752980c..944e73ac485c 100644
--- a/drivers/gpu/drm/nouveau/core/engine/dmaobj/nvd0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/dmaobj/nvd0.c
@@ -50,6 +50,9 @@ nvd0_dmaobj_bind(struct nouveau_dmaeng *dmaeng,
 		case NVE0_DISP_MAST_CLASS:
 		case NVE0_DISP_SYNC_CLASS:
 		case NVE0_DISP_OVLY_CLASS:
+		case NVF0_DISP_MAST_CLASS:
+		case NVF0_DISP_SYNC_CLASS:
+		case NVF0_DISP_OVLY_CLASS:
 			break;
 		default:
 			return -EINVAL;
diff --git a/drivers/gpu/drm/nouveau/core/engine/fifo/base.c b/drivers/gpu/drm/nouveau/core/engine/fifo/base.c
index 7341ebe131fa..d3ec436d9cb5 100644
--- a/drivers/gpu/drm/nouveau/core/engine/fifo/base.c
+++ b/drivers/gpu/drm/nouveau/core/engine/fifo/base.c
@@ -91,6 +91,8 @@ nouveau_fifo_channel_create_(struct nouveau_object *parent,
 	if (!chan->user)
 		return -EFAULT;
 
+	nouveau_event_trigger(priv->cevent, 0);
+
 	chan->size = size;
 	return 0;
 }
@@ -167,6 +169,7 @@ nouveau_fifo_destroy(struct nouveau_fifo *priv)
 {
 	kfree(priv->channel);
 	nouveau_event_destroy(&priv->uevent);
+	nouveau_event_destroy(&priv->cevent);
 	nouveau_engine_destroy(&priv->base);
 }
 
@@ -191,6 +194,10 @@ nouveau_fifo_create_(struct nouveau_object *parent,
 	if (!priv->channel)
 		return -ENOMEM;
 
+	ret = nouveau_event_create(1, &priv->cevent);
+	if (ret)
+		return ret;
+
 	ret = nouveau_event_create(1, &priv->uevent);
 	if (ret)
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/engine/fifo/nv50.c b/drivers/gpu/drm/nouveau/core/engine/fifo/nv50.c
index 840af6172788..ddaeb5572903 100644
--- a/drivers/gpu/drm/nouveau/core/engine/fifo/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/engine/fifo/nv50.c
@@ -210,7 +210,8 @@ nv50_fifo_chan_ctor_dma(struct nouveau_object *parent,
 	nv_parent(chan)->object_attach = nv50_fifo_object_attach;
 	nv_parent(chan)->object_detach = nv50_fifo_object_detach;
 
-	ret = nouveau_ramht_new(parent, parent, 0x8000, 16, &chan->ramht);
+	ret = nouveau_ramht_new(nv_object(chan), nv_object(chan), 0x8000, 16,
+				&chan->ramht);
 	if (ret)
 		return ret;
 
@@ -263,7 +264,8 @@ nv50_fifo_chan_ctor_ind(struct nouveau_object *parent,
 	nv_parent(chan)->object_attach = nv50_fifo_object_attach;
 	nv_parent(chan)->object_detach = nv50_fifo_object_detach;
 
-	ret = nouveau_ramht_new(parent, parent, 0x8000, 16, &chan->ramht);
+	ret = nouveau_ramht_new(nv_object(chan), nv_object(chan), 0x8000, 16,
+			       &chan->ramht);
 	if (ret)
 		return ret;
 
@@ -373,17 +375,17 @@ nv50_fifo_context_ctor(struct nouveau_object *parent,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, nv_object(base), 0x0200, 0x1000,
-				 NVOBJ_FLAG_ZERO_ALLOC, &base->ramfc);
+	ret = nouveau_gpuobj_new(nv_object(base), nv_object(base), 0x0200,
+				 0x1000, NVOBJ_FLAG_ZERO_ALLOC, &base->ramfc);
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, nv_object(base), 0x1200, 0,
+	ret = nouveau_gpuobj_new(nv_object(base), nv_object(base), 0x1200, 0,
 				 NVOBJ_FLAG_ZERO_ALLOC, &base->eng);
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, nv_object(base), 0x4000, 0, 0,
+	ret = nouveau_gpuobj_new(nv_object(base), nv_object(base), 0x4000, 0, 0,
 				&base->pgd);
 	if (ret)
 		return ret;
@@ -437,12 +439,12 @@ nv50_fifo_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 128 * 4, 0x1000, 0,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 128 * 4, 0x1000, 0,
 				&priv->playlist[0]);
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 128 * 4, 0x1000, 0,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 128 * 4, 0x1000, 0,
 				&priv->playlist[1]);
 	if (ret)
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/engine/fifo/nv84.c b/drivers/gpu/drm/nouveau/core/engine/fifo/nv84.c
index 094000e87871..35b94bd18808 100644
--- a/drivers/gpu/drm/nouveau/core/engine/fifo/nv84.c
+++ b/drivers/gpu/drm/nouveau/core/engine/fifo/nv84.c
@@ -180,7 +180,8 @@ nv84_fifo_chan_ctor_dma(struct nouveau_object *parent,
 	if (ret)
 		return ret;
 
-	ret = nouveau_ramht_new(parent, parent, 0x8000, 16, &chan->ramht);
+	ret = nouveau_ramht_new(nv_object(chan), nv_object(chan), 0x8000, 16,
+			       &chan->ramht);
 	if (ret)
 		return ret;
 
@@ -242,7 +243,8 @@ nv84_fifo_chan_ctor_ind(struct nouveau_object *parent,
 	if (ret)
 		return ret;
 
-	ret = nouveau_ramht_new(parent, parent, 0x8000, 16, &chan->ramht);
+	ret = nouveau_ramht_new(nv_object(chan), nv_object(chan), 0x8000, 16,
+			       &chan->ramht);
 	if (ret)
 		return ret;
 
@@ -336,12 +338,12 @@ nv84_fifo_context_ctor(struct nouveau_object *parent,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, nv_object(base), 0x0200, 0,
+	ret = nouveau_gpuobj_new(nv_object(base), nv_object(base), 0x0200, 0,
 				 NVOBJ_FLAG_ZERO_ALLOC, &base->eng);
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, nv_object(base), 0x4000, 0,
+	ret = nouveau_gpuobj_new(nv_object(base), nv_object(base), 0x4000, 0,
 				 0, &base->pgd);
 	if (ret)
 		return ret;
@@ -350,13 +352,13 @@ nv84_fifo_context_ctor(struct nouveau_object *parent,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, nv_object(base), 0x1000, 0x400,
-				 NVOBJ_FLAG_ZERO_ALLOC, &base->cache);
+	ret = nouveau_gpuobj_new(nv_object(base), nv_object(base), 0x1000,
+				 0x400, NVOBJ_FLAG_ZERO_ALLOC, &base->cache);
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, nv_object(base), 0x0100, 0x100,
-				 NVOBJ_FLAG_ZERO_ALLOC, &base->ramfc);
+	ret = nouveau_gpuobj_new(nv_object(base), nv_object(base), 0x0100,
+				 0x100, NVOBJ_FLAG_ZERO_ALLOC, &base->ramfc);
 	if (ret)
 		return ret;
 
@@ -407,12 +409,12 @@ nv84_fifo_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 128 * 4, 0x1000, 0,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 128 * 4, 0x1000, 0,
 				&priv->playlist[0]);
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 128 * 4, 0x1000, 0,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 128 * 4, 0x1000, 0,
 				&priv->playlist[1]);
 	if (ret)
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/engine/fifo/nvc0.c b/drivers/gpu/drm/nouveau/core/engine/fifo/nvc0.c
index 4f226afb5591..4d4a6b905370 100644
--- a/drivers/gpu/drm/nouveau/core/engine/fifo/nvc0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/fifo/nvc0.c
@@ -292,7 +292,8 @@ nvc0_fifo_context_ctor(struct nouveau_object *parent,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 0x10000, 0x1000, 0, &base->pgd);
+	ret = nouveau_gpuobj_new(nv_object(base), NULL, 0x10000, 0x1000, 0,
+				&base->pgd);
 	if (ret)
 		return ret;
 
@@ -623,17 +624,17 @@ nvc0_fifo_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 0x1000, 0x1000, 0,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x1000, 0x1000, 0,
 				&priv->playlist[0]);
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 0x1000, 0x1000, 0,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x1000, 0x1000, 0,
 				&priv->playlist[1]);
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 128 * 0x1000, 0x1000, 0,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 128 * 0x1000, 0x1000, 0,
 				&priv->user.mem);
 	if (ret)
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/engine/fifo/nve0.c b/drivers/gpu/drm/nouveau/core/engine/fifo/nve0.c
index 4419e40d88e9..9151919fb831 100644
--- a/drivers/gpu/drm/nouveau/core/engine/fifo/nve0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/fifo/nve0.c
@@ -96,7 +96,7 @@ nve0_fifo_playlist_update(struct nve0_fifo_priv *priv, u32 engine)
 
 	cur = engn->playlist[engn->cur_playlist];
 	if (unlikely(cur == NULL)) {
-		int ret = nouveau_gpuobj_new(nv_object(priv)->parent, NULL,
+		int ret = nouveau_gpuobj_new(nv_object(priv), NULL,
 					     0x8000, 0x1000, 0, &cur);
 		if (ret) {
 			nv_error(priv, "playlist alloc failed\n");
@@ -333,7 +333,8 @@ nve0_fifo_context_ctor(struct nouveau_object *parent,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 0x10000, 0x1000, 0, &base->pgd);
+	ret = nouveau_gpuobj_new(nv_object(base), NULL, 0x10000, 0x1000, 0,
+				&base->pgd);
 	if (ret)
 		return ret;
 
@@ -595,7 +596,7 @@ nve0_fifo_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 4096 * 0x200, 0x1000,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 4096 * 0x200, 0x1000,
 				 NVOBJ_FLAG_ZERO_ALLOC, &priv->user.mem);
 	if (ret)
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/ctxnvc0.c b/drivers/gpu/drm/nouveau/core/engine/graph/ctxnvc0.c
index 0b7951a85943..4cc6269d4077 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/ctxnvc0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/ctxnvc0.c
@@ -36,7 +36,6 @@ int
 nvc0_grctx_init(struct nvc0_graph_priv *priv, struct nvc0_grctx *info)
 {
 	struct nouveau_bar *bar = nouveau_bar(priv);
-	struct nouveau_object *parent = nv_object(priv);
 	struct nouveau_gpuobj *chan;
 	u32 size = (0x80000 + priv->size + 4095) & ~4095;
 	int ret, i;
@@ -44,7 +43,7 @@ nvc0_grctx_init(struct nvc0_graph_priv *priv, struct nvc0_grctx *info)
 	/* allocate memory to for a "channel", which we'll use to generate
 	 * the default context values
 	 */
-	ret = nouveau_gpuobj_new(parent, NULL, size, 0x1000,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, size, 0x1000,
 				 NVOBJ_FLAG_ZERO_ALLOC, &info->chan);
 	chan = info->chan;
 	if (ret) {
@@ -1399,7 +1398,7 @@ nvc0_grctx_generate_90c0(struct nvc0_graph_priv *priv)
 {
 	int i;
 
-	for (i = 0; nv_device(priv)->chipset == 0xd9 && i < 4; i++) {
+	for (i = 0; nv_device(priv)->chipset >= 0xd0 && i < 4; i++) {
 		nv_mthd(priv, 0x90c0, 0x2700 + (i * 0x40), 0x00000000);
 		nv_mthd(priv, 0x90c0, 0x2720 + (i * 0x40), 0x00000000);
 		nv_mthd(priv, 0x90c0, 0x2704 + (i * 0x40), 0x00000000);
@@ -1415,7 +1414,7 @@ nvc0_grctx_generate_90c0(struct nvc0_graph_priv *priv)
 	nv_mthd(priv, 0x90c0, 0x27ac, 0x00000000);
 	nv_mthd(priv, 0x90c0, 0x27cc, 0x00000000);
 	nv_mthd(priv, 0x90c0, 0x27ec, 0x00000000);
-	for (i = 0; nv_device(priv)->chipset == 0xd9 && i < 4; i++) {
+	for (i = 0; nv_device(priv)->chipset >= 0xd0 && i < 4; i++) {
 		nv_mthd(priv, 0x90c0, 0x2710 + (i * 0x40), 0x00014000);
 		nv_mthd(priv, 0x90c0, 0x2730 + (i * 0x40), 0x00014000);
 		nv_mthd(priv, 0x90c0, 0x2714 + (i * 0x40), 0x00000040);
@@ -1615,7 +1614,7 @@ static void
 nvc0_grctx_generate_shaders(struct nvc0_graph_priv *priv)
 {
 
-	if (nv_device(priv)->chipset == 0xd9) {
+	if (nv_device(priv)->chipset >= 0xd0) {
 		nv_wr32(priv, 0x405800, 0x0f8000bf);
 		nv_wr32(priv, 0x405830, 0x02180218);
 		nv_wr32(priv, 0x405834, 0x08000000);
@@ -1658,10 +1657,10 @@ nvc0_grctx_generate_unk64xx(struct nvc0_graph_priv *priv)
 	nv_wr32(priv, 0x4064ac, 0x00003fff);
 	nv_wr32(priv, 0x4064b4, 0x00000000);
 	nv_wr32(priv, 0x4064b8, 0x00000000);
-	if (nv_device(priv)->chipset == 0xd9)
+	if (nv_device(priv)->chipset >= 0xd0)
 		nv_wr32(priv, 0x4064bc, 0x00000000);
 	if (nv_device(priv)->chipset == 0xc1 ||
-	    nv_device(priv)->chipset == 0xd9) {
+	    nv_device(priv)->chipset >= 0xd0) {
 		nv_wr32(priv, 0x4064c0, 0x80140078);
 		nv_wr32(priv, 0x4064c4, 0x0086ffff);
 	}
@@ -1701,7 +1700,7 @@ nvc0_grctx_generate_rop(struct nvc0_graph_priv *priv)
 	/* ROPC_BROADCAST */
 	nv_wr32(priv, 0x408800, 0x02802a3c);
 	nv_wr32(priv, 0x408804, 0x00000040);
-	if (chipset == 0xd9) {
+	if (chipset >= 0xd0) {
 		nv_wr32(priv, 0x408808, 0x1043e005);
 		nv_wr32(priv, 0x408900, 0x3080b801);
 		nv_wr32(priv, 0x408904, 0x1043e005);
@@ -1735,7 +1734,7 @@ nvc0_grctx_generate_gpc(struct nvc0_graph_priv *priv)
 	nv_wr32(priv, 0x418408, 0x00000000);
 	nv_wr32(priv, 0x41840c, 0x00001008);
 	nv_wr32(priv, 0x418410, 0x0fff0fff);
-	nv_wr32(priv, 0x418414, chipset != 0xd9 ? 0x00200fff : 0x02200fff);
+	nv_wr32(priv, 0x418414, chipset < 0xd0 ? 0x00200fff : 0x02200fff);
 	nv_wr32(priv, 0x418450, 0x00000000);
 	nv_wr32(priv, 0x418454, 0x00000000);
 	nv_wr32(priv, 0x418458, 0x00000000);
@@ -1750,14 +1749,14 @@ nvc0_grctx_generate_gpc(struct nvc0_graph_priv *priv)
 	nv_wr32(priv, 0x418700, 0x00000002);
 	nv_wr32(priv, 0x418704, 0x00000080);
 	nv_wr32(priv, 0x418708, 0x00000000);
-	nv_wr32(priv, 0x41870c, chipset != 0xd9 ? 0x07c80000 : 0x00000000);
+	nv_wr32(priv, 0x41870c, chipset < 0xd0 ? 0x07c80000 : 0x00000000);
 	nv_wr32(priv, 0x418710, 0x00000000);
-	nv_wr32(priv, 0x418800, chipset != 0xd9 ? 0x0006860a : 0x7006860a);
+	nv_wr32(priv, 0x418800, chipset < 0xd0 ? 0x0006860a : 0x7006860a);
 	nv_wr32(priv, 0x418808, 0x00000000);
 	nv_wr32(priv, 0x41880c, 0x00000000);
 	nv_wr32(priv, 0x418810, 0x00000000);
 	nv_wr32(priv, 0x418828, 0x00008442);
-	if (chipset == 0xc1 || chipset == 0xd9)
+	if (chipset == 0xc1 || chipset >= 0xd0)
 		nv_wr32(priv, 0x418830, 0x10000001);
 	else
 		nv_wr32(priv, 0x418830, 0x00000001);
@@ -1768,7 +1767,7 @@ nvc0_grctx_generate_gpc(struct nvc0_graph_priv *priv)
 	nv_wr32(priv, 0x4188f0, 0x00000000);
 	nv_wr32(priv, 0x4188f4, 0x00000000);
 	nv_wr32(priv, 0x4188f8, 0x00000000);
-	if (chipset == 0xd9)
+	if (chipset >= 0xd0)
 		nv_wr32(priv, 0x4188fc, 0x20100008);
 	else if (chipset == 0xc1)
 		nv_wr32(priv, 0x4188fc, 0x00100018);
@@ -1787,7 +1786,7 @@ nvc0_grctx_generate_gpc(struct nvc0_graph_priv *priv)
 		nv_wr32(priv, 0x418a14 + (i * 0x20), 0x00000000);
 		nv_wr32(priv, 0x418a18 + (i * 0x20), 0x00000000);
 	}
-	nv_wr32(priv, 0x418b00, chipset != 0xd9 ? 0x00000000 : 0x00000006);
+	nv_wr32(priv, 0x418b00, chipset < 0xd0 ? 0x00000000 : 0x00000006);
 	nv_wr32(priv, 0x418b08, 0x0a418820);
 	nv_wr32(priv, 0x418b0c, 0x062080e6);
 	nv_wr32(priv, 0x418b10, 0x020398a4);
@@ -1804,7 +1803,7 @@ nvc0_grctx_generate_gpc(struct nvc0_graph_priv *priv)
 	nv_wr32(priv, 0x418c24, 0x00000000);
 	nv_wr32(priv, 0x418c28, 0x00000000);
 	nv_wr32(priv, 0x418c2c, 0x00000000);
-	if (chipset == 0xc1 || chipset == 0xd9)
+	if (chipset == 0xc1 || chipset >= 0xd0)
 		nv_wr32(priv, 0x418c6c, 0x00000001);
 	nv_wr32(priv, 0x418c80, 0x20200004);
 	nv_wr32(priv, 0x418c8c, 0x00000001);
@@ -1823,7 +1822,7 @@ nvc0_grctx_generate_tp(struct nvc0_graph_priv *priv)
 	nv_wr32(priv, 0x419818, 0x00000000);
 	nv_wr32(priv, 0x41983c, 0x00038bc7);
 	nv_wr32(priv, 0x419848, 0x00000000);
-	if (chipset == 0xc1 || chipset == 0xd9)
+	if (chipset == 0xc1 || chipset >= 0xd0)
 		nv_wr32(priv, 0x419864, 0x00000129);
 	else
 		nv_wr32(priv, 0x419864, 0x0000012a);
@@ -1836,7 +1835,7 @@ nvc0_grctx_generate_tp(struct nvc0_graph_priv *priv)
 	nv_wr32(priv, 0x419a14, 0x00000200);
 	nv_wr32(priv, 0x419a1c, 0x00000000);
 	nv_wr32(priv, 0x419a20, 0x00000800);
-	if (chipset == 0xd9)
+	if (chipset >= 0xd0)
 		nv_wr32(priv, 0x00419ac4, 0x0017f440);
 	else if (chipset != 0xc0 && chipset != 0xc8)
 		nv_wr32(priv, 0x00419ac4, 0x0007f440);
@@ -1847,16 +1846,16 @@ nvc0_grctx_generate_tp(struct nvc0_graph_priv *priv)
 	nv_wr32(priv, 0x419b10, 0x0a418820);
 	nv_wr32(priv, 0x419b14, 0x000000e6);
 	nv_wr32(priv, 0x419bd0, 0x00900103);
-	if (chipset == 0xc1 || chipset == 0xd9)
+	if (chipset == 0xc1 || chipset >= 0xd0)
 		nv_wr32(priv, 0x419be0, 0x00400001);
 	else
 		nv_wr32(priv, 0x419be0, 0x00000001);
 	nv_wr32(priv, 0x419be4, 0x00000000);
-	nv_wr32(priv, 0x419c00, chipset != 0xd9 ? 0x00000002 : 0x0000000a);
+	nv_wr32(priv, 0x419c00, chipset < 0xd0 ? 0x00000002 : 0x0000000a);
 	nv_wr32(priv, 0x419c04, 0x00000006);
 	nv_wr32(priv, 0x419c08, 0x00000002);
 	nv_wr32(priv, 0x419c20, 0x00000000);
-	if (nv_device(priv)->chipset == 0xd9) {
+	if (nv_device(priv)->chipset >= 0xd0) {
 		nv_wr32(priv, 0x419c24, 0x00084210);
 		nv_wr32(priv, 0x419c28, 0x3cf3cf3c);
 		nv_wr32(priv, 0x419cb0, 0x00020048);
@@ -1868,12 +1867,12 @@ nvc0_grctx_generate_tp(struct nvc0_graph_priv *priv)
 	}
 	nv_wr32(priv, 0x419ce8, 0x00000000);
 	nv_wr32(priv, 0x419cf4, 0x00000183);
-	if (chipset == 0xc1 || chipset == 0xd9)
+	if (chipset == 0xc1 || chipset >= 0xd0)
 		nv_wr32(priv, 0x419d20, 0x12180000);
 	else
 		nv_wr32(priv, 0x419d20, 0x02180000);
 	nv_wr32(priv, 0x419d24, 0x00001fff);
-	if (chipset == 0xc1 || chipset == 0xd9)
+	if (chipset == 0xc1 || chipset >= 0xd0)
 		nv_wr32(priv, 0x419d44, 0x02180218);
 	nv_wr32(priv, 0x419e04, 0x00000000);
 	nv_wr32(priv, 0x419e08, 0x00000000);
@@ -2210,7 +2209,7 @@ nvc0_grctx_generate(struct nvc0_graph_priv *priv)
 	nv_icmd(priv, 0x00000215, 0x00000040);
 	nv_icmd(priv, 0x00000216, 0x00000040);
 	nv_icmd(priv, 0x00000217, 0x00000040);
-	if (nv_device(priv)->chipset == 0xd9) {
+	if (nv_device(priv)->chipset >= 0xd0) {
 		for (i = 0x0400; i <= 0x0417; i++)
 			nv_icmd(priv, i, 0x00000040);
 	}
@@ -2222,7 +2221,7 @@ nvc0_grctx_generate(struct nvc0_graph_priv *priv)
 	nv_icmd(priv, 0x0000021d, 0x0000c080);
 	nv_icmd(priv, 0x0000021e, 0x0000c080);
 	nv_icmd(priv, 0x0000021f, 0x0000c080);
-	if (nv_device(priv)->chipset == 0xd9) {
+	if (nv_device(priv)->chipset >= 0xd0) {
 		for (i = 0x0440; i <= 0x0457; i++)
 			nv_icmd(priv, i, 0x0000c080);
 	}
@@ -2789,7 +2788,7 @@ nvc0_grctx_generate(struct nvc0_graph_priv *priv)
 	nv_icmd(priv, 0x00000585, 0x0000003f);
 	nv_icmd(priv, 0x00000576, 0x00000003);
 	if (nv_device(priv)->chipset == 0xc1 ||
-	    nv_device(priv)->chipset == 0xd9)
+	    nv_device(priv)->chipset >= 0xd0)
 		nv_icmd(priv, 0x0000057b, 0x00000059);
 	nv_icmd(priv, 0x00000586, 0x00000040);
 	nv_icmd(priv, 0x00000582, 0x00000080);
@@ -2891,7 +2890,7 @@ nvc0_grctx_generate(struct nvc0_graph_priv *priv)
 	nv_icmd(priv, 0x00000957, 0x00000003);
 	nv_icmd(priv, 0x0000095e, 0x20164010);
 	nv_icmd(priv, 0x0000095f, 0x00000020);
-	if (nv_device(priv)->chipset == 0xd9)
+	if (nv_device(priv)->chipset >= 0xd0)
 		nv_icmd(priv, 0x0000097d, 0x00000020);
 	nv_icmd(priv, 0x00000683, 0x00000006);
 	nv_icmd(priv, 0x00000685, 0x003fffff);
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/ctxnve0.c b/drivers/gpu/drm/nouveau/core/engine/graph/ctxnve0.c
index 6d8c63931ee6..ae27dae3fe38 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/ctxnve0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/ctxnve0.c
@@ -2772,10 +2772,15 @@ nve0_grctx_generate(struct nvc0_graph_priv *priv)
 	for (i = 0; i < 8; i++)
 		nv_wr32(priv, 0x4064d0 + (i * 0x04), 0x00000000);
 
-	nv_wr32(priv, 0x405b00, 0x201);
-	nv_wr32(priv, 0x408850, 0x2);
-	nv_wr32(priv, 0x408958, 0x2);
-	nv_wr32(priv, 0x419f78, 0xa);
+	nv_wr32(priv, 0x405b00, (priv->tpc_total << 8) | priv->gpc_nr);
+	if (priv->gpc_nr == 1) {
+		nv_mask(priv, 0x408850, 0x0000000f, priv->tpc_nr[0]);
+		nv_mask(priv, 0x408958, 0x0000000f, priv->tpc_nr[0]);
+	} else {
+		nv_mask(priv, 0x408850, 0x0000000f, priv->gpc_nr);
+		nv_mask(priv, 0x408958, 0x0000000f, priv->gpc_nr);
+	}
+	nv_mask(priv, 0x419f78, 0x00000001, 0x00000000);
 
 	nve0_grctx_generate_icmd(priv);
 	nve0_grctx_generate_a097(priv);
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/fuc/gpcnvc0.fuc b/drivers/gpu/drm/nouveau/core/engine/graph/fuc/gpcnvc0.fuc
index b86cc60dcd56..f7055af0f2a6 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/fuc/gpcnvc0.fuc
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/fuc/gpcnvc0.fuc
@@ -87,6 +87,11 @@ chipsets:
 .b16 #nvd9_gpc_mmio_tail
 .b16 #nvd9_tpc_mmio_head
 .b16 #nvd9_tpc_mmio_tail
+.b8  0xd7 0 0 0
+.b16 #nvd9_gpc_mmio_head
+.b16 #nvd9_gpc_mmio_tail
+.b16 #nvd9_tpc_mmio_head
+.b16 #nvd9_tpc_mmio_tail
 .b8  0 0 0 0
 
 // GPC mmio lists
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/fuc/hubnvc0.fuc b/drivers/gpu/drm/nouveau/core/engine/graph/fuc/hubnvc0.fuc
index 0bcfa4d447e5..7fbdebb2bafb 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/fuc/hubnvc0.fuc
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/fuc/hubnvc0.fuc
@@ -62,6 +62,9 @@ chipsets:
 .b8  0xd9 0 0 0
 .b16 #nvd9_hub_mmio_head
 .b16 #nvd9_hub_mmio_tail
+.b8  0xd7 0 0 0
+.b16 #nvd9_hub_mmio_head
+.b16 #nvd9_hub_mmio_tail
 .b8  0 0 0 0
 
 nvc0_hub_mmio_head:
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/nv20.c b/drivers/gpu/drm/nouveau/core/engine/graph/nv20.c
index 0607b9801748..b24559315903 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/nv20.c
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/nv20.c
@@ -254,7 +254,7 @@ nv20_graph_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 32 * 4, 16,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 32 * 4, 16,
 				 NVOBJ_FLAG_ZERO_ALLOC, &priv->ctxtab);
 	if (ret)
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/nv25.c b/drivers/gpu/drm/nouveau/core/engine/graph/nv25.c
index b2b650dd8b28..7a80d005a974 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/nv25.c
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/nv25.c
@@ -142,7 +142,7 @@ nv25_graph_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 32 * 4, 16,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 32 * 4, 16,
 				 NVOBJ_FLAG_ZERO_ALLOC, &priv->ctxtab);
 	if (ret)
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/nv2a.c b/drivers/gpu/drm/nouveau/core/engine/graph/nv2a.c
index 700462fa0ae0..3e1f32ee43d4 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/nv2a.c
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/nv2a.c
@@ -109,7 +109,7 @@ nv2a_graph_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 32 * 4, 16,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 32 * 4, 16,
 				 NVOBJ_FLAG_ZERO_ALLOC, &priv->ctxtab);
 	if (ret)
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/nv30.c b/drivers/gpu/drm/nouveau/core/engine/graph/nv30.c
index cedadaa92d3f..e451db32e92a 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/nv30.c
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/nv30.c
@@ -143,7 +143,7 @@ nv30_graph_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 32 * 4, 16,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 32 * 4, 16,
 				 NVOBJ_FLAG_ZERO_ALLOC, &priv->ctxtab);
 	if (ret)
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/nv34.c b/drivers/gpu/drm/nouveau/core/engine/graph/nv34.c
index 273f6320027b..9385ac7b44a4 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/nv34.c
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/nv34.c
@@ -143,7 +143,7 @@ nv34_graph_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 32 * 4, 16,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 32 * 4, 16,
 				 NVOBJ_FLAG_ZERO_ALLOC, &priv->ctxtab);
 	if (ret)
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/nv35.c b/drivers/gpu/drm/nouveau/core/engine/graph/nv35.c
index f40ee2116ee1..9ce84b73f86a 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/nv35.c
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/nv35.c
@@ -141,7 +141,7 @@ nv35_graph_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 32 * 4, 16,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 32 * 4, 16,
 				 NVOBJ_FLAG_ZERO_ALLOC, &priv->ctxtab);
 	if (ret)
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/nv40.c b/drivers/gpu/drm/nouveau/core/engine/graph/nv40.c
index 17049d5c723d..193a5de1b482 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/nv40.c
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/nv40.c
@@ -46,6 +46,14 @@ struct nv40_graph_chan {
 	struct nouveau_graph_chan base;
 };
 
+static u64
+nv40_graph_units(struct nouveau_graph *graph)
+{
+	struct nv40_graph_priv *priv = (void *)graph;
+
+	return nv_rd32(priv, 0x1540);
+}
+
 /*******************************************************************************
  * Graphics object classes
  ******************************************************************************/
@@ -359,6 +367,8 @@ nv40_graph_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	else
 		nv_engine(priv)->sclass = nv40_graph_sclass;
 	nv_engine(priv)->tile_prog = nv40_graph_tile_prog;
+
+	priv->base.units = nv40_graph_units;
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/nv50.c b/drivers/gpu/drm/nouveau/core/engine/graph/nv50.c
index f2b1a7a124f2..1ac36110ca19 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/nv50.c
@@ -48,6 +48,14 @@ struct nv50_graph_chan {
 	struct nouveau_graph_chan base;
 };
 
+static u64
+nv50_graph_units(struct nouveau_graph *graph)
+{
+	struct nv50_graph_priv *priv = (void *)graph;
+
+	return nv_rd32(priv, 0x1540);
+}
+
 /*******************************************************************************
  * Graphics object classes
  ******************************************************************************/
@@ -819,6 +827,8 @@ nv50_graph_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	nv_subdev(priv)->intr = nv50_graph_intr;
 	nv_engine(priv)->cclass = &nv50_graph_cclass;
 
+	priv->base.units = nv50_graph_units;
+
 	switch (nv_device(priv)->chipset) {
 	case 0x50:
 		nv_engine(priv)->sclass = nv50_graph_sclass;
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/nvc0.c b/drivers/gpu/drm/nouveau/core/engine/graph/nvc0.c
index 0de0dd724aff..f9b9d82c287f 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/nvc0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/nvc0.c
@@ -60,6 +60,19 @@ nvc8_graph_sclass[] = {
 	{}
 };
 
+u64
+nvc0_graph_units(struct nouveau_graph *graph)
+{
+	struct nvc0_graph_priv *priv = (void *)graph;
+	u64 cfg;
+
+	cfg  = (u32)priv->gpc_nr;
+	cfg |= (u32)priv->tpc_total << 8;
+	cfg |= (u64)priv->rop_nr << 32;
+
+	return cfg;
+}
+
 /*******************************************************************************
  * PGRAPH context
  ******************************************************************************/
@@ -89,7 +102,8 @@ nvc0_graph_context_ctor(struct nouveau_object *parent,
 	 * fuc to modify some per-context register settings on first load
 	 * of the context.
 	 */
-	ret = nouveau_gpuobj_new(parent, NULL, 0x1000, 0x100, 0, &chan->mmio);
+	ret = nouveau_gpuobj_new(nv_object(chan), NULL, 0x1000, 0x100, 0,
+				&chan->mmio);
 	if (ret)
 		return ret;
 
@@ -101,8 +115,8 @@ nvc0_graph_context_ctor(struct nouveau_object *parent,
 
 	/* allocate buffers referenced by mmio list */
 	for (i = 0; data->size && i < ARRAY_SIZE(priv->mmio_data); i++) {
-		ret = nouveau_gpuobj_new(parent, NULL, data->size, data->align,
-					 0, &chan->data[i].mem);
+		ret = nouveau_gpuobj_new(nv_object(chan), NULL, data->size,
+					 data->align, 0, &chan->data[i].mem);
 		if (ret)
 			return ret;
 
@@ -518,9 +532,10 @@ nvc0_graph_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 {
 	struct nouveau_device *device = nv_device(parent);
 	struct nvc0_graph_priv *priv;
+	bool enable = device->chipset != 0xd7;
 	int ret, i;
 
-	ret = nouveau_graph_create(parent, engine, oclass, true, &priv);
+	ret = nouveau_graph_create(parent, engine, oclass, enable, &priv);
 	*pobject = nv_object(priv);
 	if (ret)
 		return ret;
@@ -529,6 +544,8 @@ nvc0_graph_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	nv_subdev(priv)->intr = nvc0_graph_intr;
 	nv_engine(priv)->cclass = &nvc0_graph_cclass;
 
+	priv->base.units = nvc0_graph_units;
+
 	if (nouveau_boolopt(device->cfgopt, "NvGrUseFW", false)) {
 		nv_info(priv, "using external firmware\n");
 		if (nvc0_graph_ctor_fw(priv, "fuc409c", &priv->fuc409c) ||
@@ -551,11 +568,13 @@ nvc0_graph_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 		break;
 	}
 
-	ret = nouveau_gpuobj_new(parent, NULL, 0x1000, 256, 0, &priv->unk4188b4);
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x1000, 256, 0,
+				&priv->unk4188b4);
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 0x1000, 256, 0, &priv->unk4188b8);
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x1000, 256, 0,
+				&priv->unk4188b8);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/nvc0.h b/drivers/gpu/drm/nouveau/core/engine/graph/nvc0.h
index a1e78de46456..c870dad0f670 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/nvc0.h
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/nvc0.h
@@ -118,6 +118,7 @@ nvc0_graph_class(void *obj)
 		return 0x9197;
 	case 0xc8:
 	case 0xd9:
+	case 0xd7:
 		return 0x9297;
 	case 0xe4:
 	case 0xe7:
@@ -169,4 +170,6 @@ int  nvc0_graph_context_ctor(struct nouveau_object *, struct nouveau_object *,
 			     struct nouveau_object **);
 void nvc0_graph_context_dtor(struct nouveau_object *);
 
+u64 nvc0_graph_units(struct nouveau_graph *);
+
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/nve0.c b/drivers/gpu/drm/nouveau/core/engine/graph/nve0.c
index 4857f913efdd..678c16f63055 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/nve0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/nve0.c
@@ -77,11 +77,207 @@ nve0_graph_ctxctl_isr(struct nvc0_graph_priv *priv)
 	nv_wr32(priv, 0x409c20, ustat);
 }
 
+static const struct nouveau_enum nve0_mp_warp_error[] = {
+	{ 0x00, "NO_ERROR" },
+	{ 0x01, "STACK_MISMATCH" },
+	{ 0x05, "MISALIGNED_PC" },
+	{ 0x08, "MISALIGNED_GPR" },
+	{ 0x09, "INVALID_OPCODE" },
+	{ 0x0d, "GPR_OUT_OF_BOUNDS" },
+	{ 0x0e, "MEM_OUT_OF_BOUNDS" },
+	{ 0x0f, "UNALIGNED_MEM_ACCESS" },
+	{ 0x11, "INVALID_PARAM" },
+	{}
+};
+
+static const struct nouveau_enum nve0_mp_global_error[] = {
+	{ 2, "MULTIPLE_WARP_ERRORS" },
+	{ 3, "OUT_OF_STACK_SPACE" },
+	{}
+};
+
+static const struct nouveau_enum nve0_gpc_rop_error[] = {
+	{ 1, "RT_PITCH_OVERRUN" },
+	{ 4, "RT_WIDTH_OVERRUN" },
+	{ 5, "RT_HEIGHT_OVERRUN" },
+	{ 7, "ZETA_STORAGE_TYPE_MISMATCH" },
+	{ 8, "RT_STORAGE_TYPE_MISMATCH" },
+	{ 10, "RT_LINEAR_MISMATCH" },
+	{}
+};
+
+static const struct nouveau_enum nve0_sked_error[] = {
+	{ 7, "CONSTANT_BUFFER_SIZE" },
+	{ 9, "LOCAL_MEMORY_SIZE_POS" },
+	{ 10, "LOCAL_MEMORY_SIZE_NEG" },
+	{ 11, "WARP_CSTACK_SIZE" },
+	{ 12, "TOTAL_TEMP_SIZE" },
+	{ 13, "REGISTER_COUNT" },
+	{ 18, "TOTAL_THREADS" },
+	{ 20, "PROGRAM_OFFSET" },
+	{ 21, "SHARED_MEMORY_SIZE" },
+	{ 25, "SHARED_CONFIG_TOO_SMALL" },
+	{ 26, "TOTAL_REGISTER_COUNT" },
+	{}
+};
+
+static void
+nve0_graph_mp_trap(struct nvc0_graph_priv *priv, int gpc, int tp)
+{
+	int i;
+	u32 werr = nv_rd32(priv, TPC_UNIT(gpc, tp, 0x648));
+	u32 gerr = nv_rd32(priv, TPC_UNIT(gpc, tp, 0x650));
+
+	nv_error(priv, "GPC%i/TP%i/MP trap:", gpc, tp);
+
+	for (i = 0; i <= 31; ++i) {
+		if (!(gerr & (1 << i)))
+			continue;
+		pr_cont(" ");
+		nouveau_enum_print(nve0_mp_global_error, i);
+	}
+	if (werr) {
+		pr_cont(" ");
+		nouveau_enum_print(nve0_mp_warp_error, werr & 0xffff);
+	}
+	pr_cont("\n");
+
+	/* disable MP trap to avoid spam */
+	nv_mask(priv, TPC_UNIT(gpc, tp, 0x50c), 0x2, 0x0);
+
+	/* TODO: figure out how to resume after an MP trap */
+}
+
+static void
+nve0_graph_tp_trap(struct nvc0_graph_priv *priv, int gpc, int tp)
+{
+	u32 stat = nv_rd32(priv, TPC_UNIT(gpc, tp, 0x508));
+
+	if (stat & 0x1) {
+		u32 trap = nv_rd32(priv, TPC_UNIT(gpc, tp, 0x224));
+		nv_error(priv, "GPC%i/TP%i/TEX trap: %08x\n",
+			 gpc, tp, trap);
+
+		nv_wr32(priv, TPC_UNIT(gpc, tp, 0x224), 0xc0000000);
+		stat &= ~0x1;
+	}
+
+	if (stat & 0x2) {
+		nve0_graph_mp_trap(priv, gpc, tp);
+		stat &= ~0x2;
+	}
+
+	if (stat & 0x4) {
+		u32 trap = nv_rd32(priv, TPC_UNIT(gpc, tp, 0x084));
+		nv_error(priv, "GPC%i/TP%i/POLY trap: %08x\n",
+			 gpc, tp, trap);
+
+		nv_wr32(priv, TPC_UNIT(gpc, tp, 0x084), 0xc0000000);
+		stat &= ~0x4;
+	}
+
+	if (stat & 0x8) {
+		u32 trap = nv_rd32(priv, TPC_UNIT(gpc, tp, 0x48c));
+		nv_error(priv, "GPC%i/TP%i/L1C trap: %08x\n",
+			 gpc, tp, trap);
+
+		nv_wr32(priv, TPC_UNIT(gpc, tp, 0x48c), 0xc0000000);
+		stat &= ~0x8;
+	}
+
+	if (stat) {
+		nv_error(priv, "GPC%i/TP%i: unknown stat %08x\n",
+			 gpc, tp, stat);
+	}
+}
+
+static void
+nve0_graph_gpc_trap(struct nvc0_graph_priv *priv)
+{
+	const u32 mask = nv_rd32(priv, 0x400118);
+	int gpc;
+
+	for (gpc = 0; gpc < 4; ++gpc) {
+		u32 stat;
+		int tp;
+
+		if (!(mask & (1 << gpc)))
+			continue;
+		stat = nv_rd32(priv, GPC_UNIT(gpc, 0x2c90));
+
+		if (stat & 0x0001) {
+			u32 trap[4];
+			int i;
+
+			trap[0] = nv_rd32(priv, GPC_UNIT(gpc, 0x0420));
+			trap[1] = nv_rd32(priv, GPC_UNIT(gpc, 0x0434));
+			trap[2] = nv_rd32(priv, GPC_UNIT(gpc, 0x0438));
+			trap[3] = nv_rd32(priv, GPC_UNIT(gpc, 0x043c));
+
+			nv_error(priv, "GPC%i/PROP trap:", gpc);
+			for (i = 0; i <= 29; ++i) {
+				if (!(trap[0] & (1 << i)))
+					continue;
+				pr_cont(" ");
+				nouveau_enum_print(nve0_gpc_rop_error, i);
+			}
+			pr_cont("\n");
+
+			nv_error(priv, "x = %u, y = %u, "
+				 "format = %x, storage type = %x\n",
+				 trap[1] & 0xffff,
+				 trap[1] >> 16,
+				 (trap[2] >> 8) & 0x3f,
+				 trap[3] & 0xff);
+
+			nv_wr32(priv, GPC_UNIT(gpc, 0x0420), 0xc0000000);
+			stat &= ~0x0001;
+		}
+
+		if (stat & 0x0002) {
+			u32 trap = nv_rd32(priv, GPC_UNIT(gpc, 0x0900));
+			nv_error(priv, "GPC%i/ZCULL trap: %08x\n", gpc,
+				 trap);
+			nv_wr32(priv, GPC_UNIT(gpc, 0x0900), 0xc0000000);
+			stat &= ~0x0002;
+		}
+
+		if (stat & 0x0004) {
+			u32 trap = nv_rd32(priv, GPC_UNIT(gpc, 0x1028));
+			nv_error(priv, "GPC%i/CCACHE trap: %08x\n", gpc,
+				 trap);
+			nv_wr32(priv, GPC_UNIT(gpc, 0x1028), 0xc0000000);
+			stat &= ~0x0004;
+		}
+
+		if (stat & 0x0008) {
+			u32 trap = nv_rd32(priv, GPC_UNIT(gpc, 0x0824));
+			nv_error(priv, "GPC%i/ESETUP trap %08x\n", gpc,
+				 trap);
+			nv_wr32(priv, GPC_UNIT(gpc, 0x0824), 0xc0000000);
+			stat &= ~0x0008;
+		}
+
+		for (tp = 0; tp < 8; ++tp) {
+			if (stat & (1 << (16 + tp)))
+				nve0_graph_tp_trap(priv, gpc, tp);
+		}
+		stat &= ~0xff0000;
+
+		if (stat) {
+			nv_error(priv, "GPC%i: unknown stat %08x\n",
+				 gpc, stat);
+		}
+	}
+}
+
+
 static void
 nve0_graph_trap_isr(struct nvc0_graph_priv *priv, int chid, u64 inst,
 		struct nouveau_object *engctx)
 {
 	u32 trap = nv_rd32(priv, 0x400108);
+	int i;
 	int rop;
 
 	if (trap & 0x00000001) {
@@ -102,6 +298,32 @@ nve0_graph_trap_isr(struct nvc0_graph_priv *priv, int chid, u64 inst,
 		trap &= ~0x00000010;
 	}
 
+	if (trap & 0x00000100) {
+		u32 stat = nv_rd32(priv, 0x407020);
+		nv_error(priv, "SKED ch %d [0x%010llx %s]:",
+			 chid, inst, nouveau_client_name(engctx));
+
+		for (i = 0; i <= 29; ++i) {
+			if (!(stat & (1 << i)))
+				continue;
+			pr_cont(" ");
+			nouveau_enum_print(nve0_sked_error, i);
+		}
+		pr_cont("\n");
+
+		if (stat & 0x3fffffff)
+			nv_wr32(priv, 0x407020, 0x40000000);
+		nv_wr32(priv, 0x400108, 0x00000100);
+		trap &= ~0x00000100;
+	}
+
+	if (trap & 0x01000000) {
+		nv_error(priv, "GPC ch %d [0x%010llx %s]:\n",
+			 chid, inst, nouveau_client_name(engctx));
+		nve0_graph_gpc_trap(priv);
+		trap &= ~0x01000000;
+	}
+
 	if (trap & 0x02000000) {
 		for (rop = 0; rop < priv->rop_nr; rop++) {
 			u32 statz = nv_rd32(priv, ROP_UNIT(rop, 0x070));
@@ -217,6 +439,8 @@ nve0_graph_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	nv_engine(priv)->cclass = &nve0_graph_cclass;
 	nv_engine(priv)->sclass = nve0_graph_sclass;
 
+	priv->base.units = nvc0_graph_units;
+
 	if (nouveau_boolopt(device->cfgopt, "NvGrUseFW", false)) {
 		nv_info(priv, "using external firmware\n");
 		if (nvc0_graph_ctor_fw(priv, "fuc409c", &priv->fuc409c) ||
@@ -227,11 +451,13 @@ nve0_graph_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 		priv->firmware = true;
 	}
 
-	ret = nouveau_gpuobj_new(parent, NULL, 0x1000, 256, 0, &priv->unk4188b4);
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x1000, 256, 0,
+				&priv->unk4188b4);
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 0x1000, 256, 0, &priv->unk4188b8);
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x1000, 256, 0,
+				&priv->unk4188b8);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpu/drm/nouveau/core/engine/software/nvc0.c b/drivers/gpu/drm/nouveau/core/engine/software/nvc0.c
index a523eaad47e3..d698e710ddd4 100644
--- a/drivers/gpu/drm/nouveau/core/engine/software/nvc0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/software/nvc0.c
@@ -94,6 +94,32 @@ nvc0_software_mthd_flip(struct nouveau_object *object, u32 mthd,
 	return -EINVAL;
 }
 
+static int
+nvc0_software_mthd_mp_control(struct nouveau_object *object, u32 mthd,
+                              void *args, u32 size)
+{
+	struct nvc0_software_chan *chan = (void *)nv_engctx(object->parent);
+	struct nvc0_software_priv *priv = (void *)nv_object(chan)->engine;
+	u32 data = *(u32 *)args;
+
+	switch (mthd) {
+	case 0x600:
+		nv_wr32(priv, 0x419e00, data); /* MP.PM_UNK000 */
+		break;
+	case 0x644:
+		if (data & ~0x1ffffe)
+			return -EINVAL;
+		nv_wr32(priv, 0x419e44, data); /* MP.TRAP_WARP_ERROR_EN */
+		break;
+	case 0x6ac:
+		nv_wr32(priv, 0x419eac, data); /* MP.PM_UNK0AC */
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
 static struct nouveau_omthds
 nvc0_software_omthds[] = {
 	{ 0x0400, 0x0400, nvc0_software_mthd_vblsem_offset },
@@ -101,6 +127,9 @@ nvc0_software_omthds[] = {
 	{ 0x0408, 0x0408, nvc0_software_mthd_vblsem_value },
 	{ 0x040c, 0x040c, nvc0_software_mthd_vblsem_release },
 	{ 0x0500, 0x0500, nvc0_software_mthd_flip },
+	{ 0x0600, 0x0600, nvc0_software_mthd_mp_control },
+	{ 0x0644, 0x0644, nvc0_software_mthd_mp_control },
+	{ 0x06ac, 0x06ac, nvc0_software_mthd_mp_control },
 	{}
 };
 
diff --git a/drivers/gpu/drm/nouveau/core/include/core/class.h b/drivers/gpu/drm/nouveau/core/include/core/class.h
index 92d3ab11d962..0a393f7f055f 100644
--- a/drivers/gpu/drm/nouveau/core/include/core/class.h
+++ b/drivers/gpu/drm/nouveau/core/include/core/class.h
@@ -169,6 +169,7 @@ struct nv04_display_class {
  * 8570: NVA3_DISP
  * 9070: NVD0_DISP
  * 9170: NVE0_DISP
+ * 9270: NVF0_DISP
  */
 
 #define NV50_DISP_CLASS                                              0x00005070
@@ -178,6 +179,7 @@ struct nv04_display_class {
 #define NVA3_DISP_CLASS                                              0x00008570
 #define NVD0_DISP_CLASS                                              0x00009070
 #define NVE0_DISP_CLASS                                              0x00009170
+#define NVF0_DISP_CLASS                                              0x00009270
 
 #define NV50_DISP_SOR_MTHD                                           0x00010000
 #define NV50_DISP_SOR_MTHD_TYPE                                      0x0000f000
@@ -246,6 +248,7 @@ struct nv50_display_class {
  * 857a: NVA3_DISP_CURS
  * 907a: NVD0_DISP_CURS
  * 917a: NVE0_DISP_CURS
+ * 927a: NVF0_DISP_CURS
  */
 
 #define NV50_DISP_CURS_CLASS                                         0x0000507a
@@ -255,6 +258,7 @@ struct nv50_display_class {
 #define NVA3_DISP_CURS_CLASS                                         0x0000857a
 #define NVD0_DISP_CURS_CLASS                                         0x0000907a
 #define NVE0_DISP_CURS_CLASS                                         0x0000917a
+#define NVF0_DISP_CURS_CLASS                                         0x0000927a
 
 struct nv50_display_curs_class {
 	u32 head;
@@ -267,6 +271,7 @@ struct nv50_display_curs_class {
  * 857b: NVA3_DISP_OIMM
  * 907b: NVD0_DISP_OIMM
  * 917b: NVE0_DISP_OIMM
+ * 927b: NVE0_DISP_OIMM
  */
 
 #define NV50_DISP_OIMM_CLASS                                         0x0000507b
@@ -276,6 +281,7 @@ struct nv50_display_curs_class {
 #define NVA3_DISP_OIMM_CLASS                                         0x0000857b
 #define NVD0_DISP_OIMM_CLASS                                         0x0000907b
 #define NVE0_DISP_OIMM_CLASS                                         0x0000917b
+#define NVF0_DISP_OIMM_CLASS                                         0x0000927b
 
 struct nv50_display_oimm_class {
 	u32 head;
@@ -288,6 +294,7 @@ struct nv50_display_oimm_class {
  * 857c: NVA3_DISP_SYNC
  * 907c: NVD0_DISP_SYNC
  * 917c: NVE0_DISP_SYNC
+ * 927c: NVF0_DISP_SYNC
  */
 
 #define NV50_DISP_SYNC_CLASS                                         0x0000507c
@@ -297,6 +304,7 @@ struct nv50_display_oimm_class {
 #define NVA3_DISP_SYNC_CLASS                                         0x0000857c
 #define NVD0_DISP_SYNC_CLASS                                         0x0000907c
 #define NVE0_DISP_SYNC_CLASS                                         0x0000917c
+#define NVF0_DISP_SYNC_CLASS                                         0x0000927c
 
 struct nv50_display_sync_class {
 	u32 pushbuf;
@@ -310,6 +318,7 @@ struct nv50_display_sync_class {
  * 857d: NVA3_DISP_MAST
  * 907d: NVD0_DISP_MAST
  * 917d: NVE0_DISP_MAST
+ * 927d: NVF0_DISP_MAST
  */
 
 #define NV50_DISP_MAST_CLASS                                         0x0000507d
@@ -319,6 +328,7 @@ struct nv50_display_sync_class {
 #define NVA3_DISP_MAST_CLASS                                         0x0000857d
 #define NVD0_DISP_MAST_CLASS                                         0x0000907d
 #define NVE0_DISP_MAST_CLASS                                         0x0000917d
+#define NVF0_DISP_MAST_CLASS                                         0x0000927d
 
 struct nv50_display_mast_class {
 	u32 pushbuf;
@@ -331,6 +341,7 @@ struct nv50_display_mast_class {
  * 857e: NVA3_DISP_OVLY
  * 907e: NVD0_DISP_OVLY
  * 917e: NVE0_DISP_OVLY
+ * 927e: NVF0_DISP_OVLY
  */
 
 #define NV50_DISP_OVLY_CLASS                                         0x0000507e
@@ -340,6 +351,7 @@ struct nv50_display_mast_class {
 #define NVA3_DISP_OVLY_CLASS                                         0x0000857e
 #define NVD0_DISP_OVLY_CLASS                                         0x0000907e
 #define NVE0_DISP_OVLY_CLASS                                         0x0000917e
+#define NVF0_DISP_OVLY_CLASS                                         0x0000927e
 
 struct nv50_display_ovly_class {
 	u32 pushbuf;
diff --git a/drivers/gpu/drm/nouveau/core/include/core/device.h b/drivers/gpu/drm/nouveau/core/include/core/device.h
index d351a4e5819c..05840f3eee98 100644
--- a/drivers/gpu/drm/nouveau/core/include/core/device.h
+++ b/drivers/gpu/drm/nouveau/core/include/core/device.h
@@ -6,7 +6,7 @@
 #include <core/engine.h>
 
 enum nv_subdev_type {
-	NVDEV_SUBDEV_DEVICE,
+	NVDEV_ENGINE_DEVICE,
 	NVDEV_SUBDEV_VBIOS,
 
 	/* All subdevs from DEVINIT to DEVINIT_LAST will be created before
@@ -57,7 +57,7 @@ enum nv_subdev_type {
 };
 
 struct nouveau_device {
-	struct nouveau_subdev base;
+	struct nouveau_engine base;
 	struct list_head head;
 
 	struct pci_dev *pdev;
@@ -99,7 +99,7 @@ nv_device(void *obj)
 
 #if CONFIG_NOUVEAU_DEBUG >= NV_DBG_PARANOIA
 	if (unlikely(!nv_iclass(device, NV_SUBDEV_CLASS) ||
-		     (nv_hclass(device) & 0xff) != NVDEV_SUBDEV_DEVICE)) {
+		     (nv_hclass(device) & 0xff) != NVDEV_ENGINE_DEVICE)) {
 		nv_assert("BAD CAST -> NvDevice, 0x%08x 0x%08x",
 			  nv_hclass(object), nv_hclass(device));
 	}
diff --git a/drivers/gpu/drm/nouveau/core/include/core/parent.h b/drivers/gpu/drm/nouveau/core/include/core/parent.h
index 31cd852c96df..9f5ea900ff00 100644
--- a/drivers/gpu/drm/nouveau/core/include/core/parent.h
+++ b/drivers/gpu/drm/nouveau/core/include/core/parent.h
@@ -51,8 +51,8 @@ int  nouveau_parent_create_(struct nouveau_object *, struct nouveau_object *,
 void nouveau_parent_destroy(struct nouveau_parent *);
 
 void _nouveau_parent_dtor(struct nouveau_object *);
-#define _nouveau_parent_init _nouveau_object_init
-#define _nouveau_parent_fini _nouveau_object_fini
+#define _nouveau_parent_init nouveau_object_init
+#define _nouveau_parent_fini nouveau_object_fini
 
 int nouveau_parent_sclass(struct nouveau_object *, u16 handle,
 			  struct nouveau_object **pengine,
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/device.h b/drivers/gpu/drm/nouveau/core/include/engine/device.h
index c9e4c4afa50e..b3dd2c4c2f1e 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/device.h
+++ b/drivers/gpu/drm/nouveau/core/include/engine/device.h
@@ -18,7 +18,6 @@ int nv50_identify(struct nouveau_device *);
 int nvc0_identify(struct nouveau_device *);
 int nve0_identify(struct nouveau_device *);
 
-extern struct nouveau_oclass nouveau_device_sclass[];
 struct nouveau_device *nouveau_device_find(u64 name);
 
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/include/engine/disp.h b/drivers/gpu/drm/nouveau/core/include/engine/disp.h
index 28da6772c095..4b21fabfbddb 100644
--- a/drivers/gpu/drm/nouveau/core/include/engine/disp.h
+++ b/drivers/gpu/drm/nouveau/core/include/engine/disp.h
@@ -44,5 +44,6 @@ extern struct nouveau_oclass nv94_disp_oclass;
 extern struct nouveau_oclass nva3_disp_oclass;
 extern struct nouveau_oclass nvd0_disp_oclass;
 extern struct nouveau_oclass nve0_disp_oclass;
+extern struct nouveau_oclass nvf0_disp_oclass;
 
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/include/engine/fifo.h b/drivers/gpu/drm/nouveau/core/include/engine/fifo.h
index b46c197709f3..633c2f806482 100644
--- a/drivers/gpu/drm/nouveau/core/include/engine/fifo.h
+++ b/drivers/gpu/drm/nouveau/core/include/engine/fifo.h
@@ -65,7 +65,8 @@ struct nouveau_fifo_base {
 struct nouveau_fifo {
 	struct nouveau_engine base;
 
-	struct nouveau_event *uevent;
+	struct nouveau_event *cevent; /* channel creation event */
+	struct nouveau_event *uevent; /* async user trigger */
 
 	struct nouveau_object **channel;
 	spinlock_t lock;
diff --git a/drivers/gpu/drm/nouveau/core/include/engine/graph.h b/drivers/gpu/drm/nouveau/core/include/engine/graph.h
index 6943b40d0817..5d392439f2ac 100644
--- a/drivers/gpu/drm/nouveau/core/include/engine/graph.h
+++ b/drivers/gpu/drm/nouveau/core/include/engine/graph.h
@@ -26,6 +26,10 @@ struct nouveau_graph_chan {
 
 struct nouveau_graph {
 	struct nouveau_engine base;
+
+	/* Returns chipset-specific counts of units packed into an u64.
+	 */
+	u64 (*units)(struct nouveau_graph *);
 };
 
 static inline struct nouveau_graph *
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/ltcg.h b/drivers/gpu/drm/nouveau/core/include/subdev/ltcg.h
index f351f63bc654..a1985ed3d58d 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/ltcg.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/ltcg.h
@@ -4,8 +4,15 @@
 #include <core/subdev.h>
 #include <core/device.h>
 
+struct nouveau_mm_node;
+
 struct nouveau_ltcg {
 	struct nouveau_subdev base;
+
+	int  (*tags_alloc)(struct nouveau_ltcg *, u32 count,
+	                   struct nouveau_mm_node **);
+	void (*tags_free)(struct nouveau_ltcg *, struct nouveau_mm_node **);
+	void (*tags_clear)(struct nouveau_ltcg *, u32 first, u32 count);
 };
 
 static inline struct nouveau_ltcg *
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/mc.h b/drivers/gpu/drm/nouveau/core/include/subdev/mc.h
index fded97cea500..d5502267c30f 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/mc.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/mc.h
@@ -21,18 +21,22 @@ nouveau_mc(void *obj)
 }
 
 #define nouveau_mc_create(p,e,o,d)                                             \
-	nouveau_subdev_create_((p), (e), (o), 0, "PMC", "master",              \
-			       sizeof(**d), (void **)d)
-#define nouveau_mc_destroy(p)                                                  \
-	nouveau_subdev_destroy(&(p)->base)
-#define nouveau_mc_init(p)                                                     \
-	nouveau_subdev_init(&(p)->base)
-#define nouveau_mc_fini(p,s)                                                   \
-	nouveau_subdev_fini(&(p)->base, (s))
-
-#define _nouveau_mc_dtor _nouveau_subdev_dtor
-#define _nouveau_mc_init _nouveau_subdev_init
-#define _nouveau_mc_fini _nouveau_subdev_fini
+	nouveau_mc_create_((p), (e), (o), sizeof(**d), (void **)d)
+#define nouveau_mc_destroy(p) ({                                               \
+	struct nouveau_mc *pmc = (p); _nouveau_mc_dtor(nv_object(pmc));        \
+})
+#define nouveau_mc_init(p) ({                                                  \
+	struct nouveau_mc *pmc = (p); _nouveau_mc_init(nv_object(pmc));        \
+})
+#define nouveau_mc_fini(p,s) ({                                                \
+	struct nouveau_mc *pmc = (p); _nouveau_mc_fini(nv_object(pmc), (s));   \
+})
+
+int  nouveau_mc_create_(struct nouveau_object *, struct nouveau_object *,
+			struct nouveau_oclass *, int, void **);
+void _nouveau_mc_dtor(struct nouveau_object *);
+int  _nouveau_mc_init(struct nouveau_object *);
+int  _nouveau_mc_fini(struct nouveau_object *, bool);
 
 extern struct nouveau_oclass nv04_mc_oclass;
 extern struct nouveau_oclass nv44_mc_oclass;
@@ -40,8 +44,6 @@ extern struct nouveau_oclass nv50_mc_oclass;
 extern struct nouveau_oclass nv98_mc_oclass;
 extern struct nouveau_oclass nvc0_mc_oclass;
 
-void nouveau_mc_intr(struct nouveau_subdev *);
-
 extern const struct nouveau_mc_intr nv04_mc_intr[];
 int nv04_mc_init(struct nouveau_object *);
 int nv50_mc_init(struct nouveau_object *);
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/therm.h b/drivers/gpu/drm/nouveau/core/include/subdev/therm.h
index 0b20fc0d19c1..c075998d82e6 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/therm.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/therm.h
@@ -73,6 +73,7 @@ int  _nouveau_therm_fini(struct nouveau_object *, bool);
 
 extern struct nouveau_oclass nv40_therm_oclass;
 extern struct nouveau_oclass nv50_therm_oclass;
+extern struct nouveau_oclass nv84_therm_oclass;
 extern struct nouveau_oclass nva3_therm_oclass;
 extern struct nouveau_oclass nvd0_therm_oclass;
 
diff --git a/drivers/gpu/drm/nouveau/core/os.h b/drivers/gpu/drm/nouveau/core/os.h
index eb496033b55c..3bd9be2ab37f 100644
--- a/drivers/gpu/drm/nouveau/core/os.h
+++ b/drivers/gpu/drm/nouveau/core/os.h
@@ -17,6 +17,7 @@
 #include <linux/acpi.h>
 #include <linux/dmi.h>
 #include <linux/reboot.h>
+#include <linux/interrupt.h>
 
 #include <asm/unaligned.h>
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bar/nv50.c b/drivers/gpu/drm/nouveau/core/subdev/bar/nv50.c
index c3acf5b70d9e..649f1ced1fe0 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bar/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bar/nv50.c
@@ -122,18 +122,20 @@ nv50_bar_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 0x20000, 0, NVOBJ_FLAG_HEAP,
-				&priv->mem);
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x20000, 0,
+				 NVOBJ_FLAG_HEAP, &priv->mem);
 	heap = nv_object(priv->mem);
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, heap, (device->chipset == 0x50) ?
-				 0x1400 : 0x0200, 0, 0, &priv->pad);
+	ret = nouveau_gpuobj_new(nv_object(priv), heap,
+				(device->chipset == 0x50) ? 0x1400 : 0x0200,
+				 0, 0, &priv->pad);
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, heap, 0x4000, 0, 0, &priv->pgd);
+	ret = nouveau_gpuobj_new(nv_object(priv), heap, 0x4000, 0,
+				 0, &priv->pgd);
 	if (ret)
 		return ret;
 
@@ -145,9 +147,9 @@ nv50_bar_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, heap, ((limit-- - start) >> 12) * 8,
-				 0x1000, NVOBJ_FLAG_ZERO_ALLOC,
-				 &vm->pgt[0].obj[0]);
+	ret = nouveau_gpuobj_new(nv_object(priv), heap,
+				 ((limit-- - start) >> 12) * 8, 0x1000,
+				 NVOBJ_FLAG_ZERO_ALLOC, &vm->pgt[0].obj[0]);
 	vm->pgt[0].refcount[0] = 1;
 	if (ret)
 		return ret;
@@ -157,7 +159,7 @@ nv50_bar_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, heap, 24, 16, 0, &priv->bar3);
+	ret = nouveau_gpuobj_new(nv_object(priv), heap, 24, 16, 0, &priv->bar3);
 	if (ret)
 		return ret;
 
@@ -182,7 +184,7 @@ nv50_bar_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, heap, 24, 16, 0, &priv->bar1);
+	ret = nouveau_gpuobj_new(nv_object(priv), heap, 24, 16, 0, &priv->bar1);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bar/nvc0.c b/drivers/gpu/drm/nouveau/core/subdev/bar/nvc0.c
index 77a6fb725d3f..f8a44956dec1 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bar/nvc0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bar/nvc0.c
@@ -101,12 +101,14 @@ nvc0_bar_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 		return ret;
 
 	/* BAR3 */
-	ret = nouveau_gpuobj_new(parent, NULL, 0x1000, 0, 0, &priv->bar[0].mem);
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x1000, 0, 0,
+				&priv->bar[0].mem);
 	mem = priv->bar[0].mem;
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 0x8000, 0, 0, &priv->bar[0].pgd);
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x8000, 0, 0,
+				&priv->bar[0].pgd);
 	if (ret)
 		return ret;
 
@@ -114,7 +116,7 @@ nvc0_bar_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL,
 				 (pci_resource_len(pdev, 3) >> 12) * 8,
 				 0x1000, NVOBJ_FLAG_ZERO_ALLOC,
 				 &vm->pgt[0].obj[0]);
@@ -133,12 +135,14 @@ nvc0_bar_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	nv_wo32(mem, 0x020c, upper_32_bits(pci_resource_len(pdev, 3) - 1));
 
 	/* BAR1 */
-	ret = nouveau_gpuobj_new(parent, NULL, 0x1000, 0, 0, &priv->bar[1].mem);
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x1000, 0, 0,
+				&priv->bar[1].mem);
 	mem = priv->bar[1].mem;
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL, 0x8000, 0, 0, &priv->bar[1].pgd);
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x8000, 0, 0,
+				&priv->bar[1].pgd);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/init.c b/drivers/gpu/drm/nouveau/core/subdev/bios/init.c
index 9c41b58d57e2..c300b5e7b670 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/init.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/init.c
@@ -64,27 +64,33 @@ init_exec_force(struct nvbios_init *init, bool exec)
 static inline int
 init_or(struct nvbios_init *init)
 {
-	if (init->outp)
-		return ffs(init->outp->or) - 1;
-	error("script needs OR!!\n");
+	if (init_exec(init)) {
+		if (init->outp)
+			return ffs(init->outp->or) - 1;
+		error("script needs OR!!\n");
+	}
 	return 0;
 }
 
 static inline int
 init_link(struct nvbios_init *init)
 {
-	if (init->outp)
-		return !(init->outp->sorconf.link & 1);
-	error("script needs OR link\n");
+	if (init_exec(init)) {
+		if (init->outp)
+			return !(init->outp->sorconf.link & 1);
+		error("script needs OR link\n");
+	}
 	return 0;
 }
 
 static inline int
 init_crtc(struct nvbios_init *init)
 {
-	if (init->crtc >= 0)
-		return init->crtc;
-	error("script needs crtc\n");
+	if (init_exec(init)) {
+		if (init->crtc >= 0)
+			return init->crtc;
+		error("script needs crtc\n");
+	}
 	return 0;
 }
 
@@ -92,16 +98,21 @@ static u8
 init_conn(struct nvbios_init *init)
 {
 	struct nouveau_bios *bios = init->bios;
+	u8  ver, len;
+	u16 conn;
 
-	if (init->outp) {
-		u8  ver, len;
-		u16 conn = dcb_conn(bios, init->outp->connector, &ver, &len);
-		if (conn)
-			return nv_ro08(bios, conn);
+	if (init_exec(init)) {
+		if (init->outp) {
+			conn = init->outp->connector;
+			conn = dcb_conn(bios, conn, &ver, &len);
+			if (conn)
+				return nv_ro08(bios, conn);
+		}
+
+		error("script needs connector type\n");
 	}
 
-	error("script needs connector type\n");
-	return 0x00;
+	return 0xff;
 }
 
 static inline u32
@@ -227,7 +238,8 @@ init_i2c(struct nvbios_init *init, int index)
 	} else
 	if (index < 0) {
 		if (!init->outp) {
-			error("script needs output for i2c\n");
+			if (init_exec(init))
+				error("script needs output for i2c\n");
 			return NULL;
 		}
 
@@ -544,7 +556,8 @@ init_tmds_reg(struct nvbios_init *init, u8 tmds)
 			return 0x6808b0 + dacoffset;
 		}
 
-		error("tmds opcodes need dcb\n");
+		if (init_exec(init))
+			error("tmds opcodes need dcb\n");
 	} else {
 		if (tmds < ARRAY_SIZE(pramdac_table))
 			return pramdac_table[tmds];
@@ -792,7 +805,8 @@ init_dp_condition(struct nvbios_init *init)
 			break;
 		}
 
-		warn("script needs dp output table data\n");
+		if (init_exec(init))
+			warn("script needs dp output table data\n");
 		break;
 	case 5:
 		if (!(init_rdauxr(init, 0x0d) & 1))
@@ -816,7 +830,7 @@ init_io_mask_or(struct nvbios_init *init)
 	u8    or = init_or(init);
 	u8  data;
 
-	trace("IO_MASK_OR\t0x03d4[0x%02x] &= ~(1 << 0x%02x)", index, or);
+	trace("IO_MASK_OR\t0x03d4[0x%02x] &= ~(1 << 0x%02x)\n", index, or);
 	init->offset += 2;
 
 	data = init_rdvgai(init, 0x03d4, index);
@@ -835,7 +849,7 @@ init_io_or(struct nvbios_init *init)
 	u8    or = init_or(init);
 	u8  data;
 
-	trace("IO_OR\t0x03d4[0x%02x] |= (1 << 0x%02x)", index, or);
+	trace("IO_OR\t0x03d4[0x%02x] |= (1 << 0x%02x)\n", index, or);
 	init->offset += 2;
 
 	data = init_rdvgai(init, 0x03d4, index);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/nvc0.c b/drivers/gpu/drm/nouveau/core/subdev/fb/nvc0.c
index 7606ed15b6fa..86ad59203c8b 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/nvc0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/nvc0.c
@@ -23,6 +23,7 @@
  */
 
 #include <subdev/fb.h>
+#include <subdev/ltcg.h>
 #include <subdev/bios.h>
 
 struct nvc0_fb_priv {
@@ -31,34 +32,14 @@ struct nvc0_fb_priv {
 	dma_addr_t r100c10;
 };
 
-/* 0 = unsupported
- * 1 = non-compressed
- * 3 = compressed
- */
-static const u8 types[256] = {
-	1, 1, 3, 3, 3, 3, 0, 3, 3, 3, 3, 0, 0, 0, 0, 0,
-	0, 1, 0, 0, 0, 0, 0, 3, 3, 3, 3, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 3, 3, 3, 3, 3,
-	3, 3, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 3, 3, 3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 3, 3, 3, 3, 0, 1, 1, 1, 1, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 3, 3, 3, 3, 1, 1, 1, 1, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3,
-	3, 3, 3, 1, 0, 0, 0, 0, 0, 0, 0, 0, 3, 3, 3, 3,
-	3, 3, 0, 0, 0, 0, 0, 0, 3, 0, 0, 3, 0, 3, 0, 3,
-	3, 0, 3, 3, 3, 3, 3, 0, 0, 3, 0, 3, 0, 3, 3, 0,
-	3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 0, 1, 1, 0
-};
+extern const u8 nvc0_pte_storage_type_map[256];
+
 
 static bool
 nvc0_fb_memtype_valid(struct nouveau_fb *pfb, u32 tile_flags)
 {
 	u8 memtype = (tile_flags & 0x0000ff00) >> 8;
-	return likely((types[memtype] == 1));
+	return likely((nvc0_pte_storage_type_map[memtype] != 0xff));
 }
 
 static int
@@ -130,6 +111,7 @@ nvc0_fb_vram_new(struct nouveau_fb *pfb, u64 size, u32 align, u32 ncmin,
 	int type = (memtype & 0x0ff);
 	int back = (memtype & 0x800);
 	int ret;
+	const bool comp = nvc0_pte_storage_type_map[type] != type;
 
 	size  >>= 12;
 	align >>= 12;
@@ -142,10 +124,22 @@ nvc0_fb_vram_new(struct nouveau_fb *pfb, u64 size, u32 align, u32 ncmin,
 		return -ENOMEM;
 
 	INIT_LIST_HEAD(&mem->regions);
-	mem->memtype = type;
 	mem->size = size;
 
 	mutex_lock(&pfb->base.mutex);
+	if (comp) {
+		struct nouveau_ltcg *ltcg = nouveau_ltcg(pfb->base.base.parent);
+
+		/* compression only works with lpages */
+		if (align == (1 << (17 - 12))) {
+			int n = size >> 5;
+			ltcg->tags_alloc(ltcg, n, &mem->tag);
+		}
+		if (unlikely(!mem->tag))
+			type = nvc0_pte_storage_type_map[type];
+	}
+	mem->memtype = type;
+
 	do {
 		if (back)
 			ret = nouveau_mm_tail(mm, 1, size, ncmin, align, &r);
@@ -168,6 +162,17 @@ nvc0_fb_vram_new(struct nouveau_fb *pfb, u64 size, u32 align, u32 ncmin,
 	return 0;
 }
 
+static void
+nvc0_fb_vram_del(struct nouveau_fb *pfb, struct nouveau_mem **pmem)
+{
+	struct nouveau_ltcg *ltcg = nouveau_ltcg(pfb->base.base.parent);
+
+	if ((*pmem)->tag)
+		ltcg->tags_free(ltcg, &(*pmem)->tag);
+
+	nv50_fb_vram_del(pfb, pmem);
+}
+
 static int
 nvc0_fb_init(struct nouveau_object *object)
 {
@@ -178,7 +183,8 @@ nvc0_fb_init(struct nouveau_object *object)
 	if (ret)
 		return ret;
 
-	nv_wr32(priv, 0x100c10, priv->r100c10 >> 8);
+	if (priv->r100c10_page)
+		nv_wr32(priv, 0x100c10, priv->r100c10 >> 8);
 	return 0;
 }
 
@@ -214,16 +220,16 @@ nvc0_fb_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	priv->base.memtype_valid = nvc0_fb_memtype_valid;
 	priv->base.ram.init = nvc0_fb_vram_init;
 	priv->base.ram.get = nvc0_fb_vram_new;
-	priv->base.ram.put = nv50_fb_vram_del;
+	priv->base.ram.put = nvc0_fb_vram_del;
 
 	priv->r100c10_page = alloc_page(GFP_KERNEL | __GFP_ZERO);
-	if (!priv->r100c10_page)
-		return -ENOMEM;
-
-	priv->r100c10 = pci_map_page(device->pdev, priv->r100c10_page, 0,
-				     PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
-	if (pci_dma_mapping_error(device->pdev, priv->r100c10))
-		return -EFAULT;
+	if (priv->r100c10_page) {
+		priv->r100c10 = pci_map_page(device->pdev, priv->r100c10_page,
+					     0, PAGE_SIZE,
+					     PCI_DMA_BIDIRECTIONAL);
+		if (pci_dma_mapping_error(device->pdev, priv->r100c10))
+			return -EFAULT;
+	}
 
 	return nouveau_fb_preinit(&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 2e98e8a3f1aa..8ae2625415e1 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/i2c/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/i2c/base.c
@@ -140,12 +140,8 @@ nouveau_i2c_port_create_(struct nouveau_object *parent,
 	}
 
 	/* drop port's i2c subdev refcount, i2c handles this itself */
-	if (ret == 0) {
+	if (ret == 0)
 		list_add_tail(&port->head, &i2c->ports);
-		atomic_dec(&parent->refcount);
-		atomic_dec(&engine->refcount);
-	}
-
 	return ret;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/instmem/nv04.c b/drivers/gpu/drm/nouveau/core/subdev/instmem/nv04.c
index f5bbd3834116..795393d7b2f5 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/instmem/nv04.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/instmem/nv04.c
@@ -93,7 +93,6 @@ nv04_instmem_alloc(struct nouveau_instmem *imem, struct nouveau_object *parent,
 		   u32 size, u32 align, struct nouveau_object **pobject)
 {
 	struct nouveau_object *engine = nv_object(imem);
-	struct nv04_instmem_priv *priv = (void *)(imem);
 	int ret;
 
 	ret = nouveau_object_ctor(parent, engine, &nv04_instobj_oclass,
@@ -101,14 +100,6 @@ nv04_instmem_alloc(struct nouveau_instmem *imem, struct nouveau_object *parent,
 	if (ret)
 		return ret;
 
-	/* INSTMEM itself creates objects to reserve (and preserve across
-	 * suspend/resume) various fixed data locations, each one of these
-	 * takes a reference on INSTMEM itself, causing it to never be
-	 * freed.  We drop all the self-references here to avoid this.
-	 */
-	if (unlikely(!priv->created))
-		atomic_dec(&engine->refcount);
-
 	return 0;
 }
 
@@ -134,27 +125,28 @@ nv04_instmem_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 		return ret;
 
 	/* 0x00000-0x10000: reserve for probable vbios image */
-	ret = nouveau_gpuobj_new(parent, NULL, 0x10000, 0, 0, &priv->vbios);
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x10000, 0, 0,
+				&priv->vbios);
 	if (ret)
 		return ret;
 
 	/* 0x10000-0x18000: reserve for RAMHT */
-	ret = nouveau_ramht_new(parent, NULL, 0x08000, 0, &priv->ramht);
+	ret = nouveau_ramht_new(nv_object(priv), NULL, 0x08000, 0, &priv->ramht);
 	if (ret)
 		return ret;
 
 	/* 0x18000-0x18800: reserve for RAMFC (enough for 32 nv30 channels) */
-	ret = nouveau_gpuobj_new(parent, NULL, 0x00800, 0,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x00800, 0,
 				 NVOBJ_FLAG_ZERO_ALLOC, &priv->ramfc);
 	if (ret)
 		return ret;
 
 	/* 0x18800-0x18a00: reserve for RAMRO */
-	ret = nouveau_gpuobj_new(parent, NULL, 0x00200, 0, 0, &priv->ramro);
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x00200, 0, 0,
+				&priv->ramro);
 	if (ret)
 		return ret;
 
-	priv->created = true;
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/instmem/nv04.h b/drivers/gpu/drm/nouveau/core/subdev/instmem/nv04.h
index 7983d8d9b358..b15b61310236 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/instmem/nv04.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/instmem/nv04.h
@@ -9,7 +9,6 @@
 
 struct nv04_instmem_priv {
 	struct nouveau_instmem base;
-	bool created;
 
 	void __iomem *iomem;
 	struct nouveau_mm heap;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/instmem/nv40.c b/drivers/gpu/drm/nouveau/core/subdev/instmem/nv40.c
index da64253201ef..716bf41bc3c1 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/instmem/nv40.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/instmem/nv40.c
@@ -82,31 +82,33 @@ nv40_instmem_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 		return ret;
 
 	/* 0x00000-0x10000: reserve for probable vbios image */
-	ret = nouveau_gpuobj_new(parent, NULL, 0x10000, 0, 0, &priv->vbios);
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x10000, 0, 0,
+				&priv->vbios);
 	if (ret)
 		return ret;
 
 	/* 0x10000-0x18000: reserve for RAMHT */
-	ret = nouveau_ramht_new(parent, NULL, 0x08000, 0, &priv->ramht);
+	ret = nouveau_ramht_new(nv_object(priv), NULL, 0x08000, 0,
+			       &priv->ramht);
 	if (ret)
 		return ret;
 
 	/* 0x18000-0x18200: reserve for RAMRO
 	 * 0x18200-0x20000: padding
 	 */
-	ret = nouveau_gpuobj_new(parent, NULL, 0x08000, 0, 0, &priv->ramro);
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x08000, 0, 0,
+				&priv->ramro);
 	if (ret)
 		return ret;
 
 	/* 0x20000-0x21000: reserve for RAMFC
 	 * 0x21000-0x40000: padding and some unknown crap
 	 */
-	ret = nouveau_gpuobj_new(parent, NULL, 0x20000, 0,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL, 0x20000, 0,
 				 NVOBJ_FLAG_ZERO_ALLOC, &priv->ramfc);
 	if (ret)
 		return ret;
 
-	priv->created = true;
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/ltcg/nvc0.c b/drivers/gpu/drm/nouveau/core/subdev/ltcg/nvc0.c
index 078a2b9d6bd6..e4940fb166e8 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/ltcg/nvc0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/ltcg/nvc0.c
@@ -23,10 +23,17 @@
  */
 
 #include <subdev/ltcg.h>
+#include <subdev/fb.h>
+#include <subdev/timer.h>
 
 struct nvc0_ltcg_priv {
 	struct nouveau_ltcg base;
+	u32 part_nr;
+	u32 part_mask;
 	u32 subp_nr;
+	struct nouveau_mm tags;
+	u32 num_tags;
+	struct nouveau_mm_node *tag_ram;
 };
 
 static void
@@ -62,11 +69,104 @@ nvc0_ltcg_intr(struct nouveau_subdev *subdev)
 }
 
 static int
+nvc0_ltcg_tags_alloc(struct nouveau_ltcg *ltcg, u32 n,
+		     struct nouveau_mm_node **pnode)
+{
+	struct nvc0_ltcg_priv *priv = (struct nvc0_ltcg_priv *)ltcg;
+	int ret;
+
+	ret = nouveau_mm_head(&priv->tags, 1, n, n, 1, pnode);
+	if (ret)
+		*pnode = NULL;
+
+	return ret;
+}
+
+static void
+nvc0_ltcg_tags_free(struct nouveau_ltcg *ltcg, struct nouveau_mm_node **pnode)
+{
+	struct nvc0_ltcg_priv *priv = (struct nvc0_ltcg_priv *)ltcg;
+
+	nouveau_mm_free(&priv->tags, pnode);
+}
+
+static void
+nvc0_ltcg_tags_clear(struct nouveau_ltcg *ltcg, u32 first, u32 count)
+{
+	struct nvc0_ltcg_priv *priv = (struct nvc0_ltcg_priv *)ltcg;
+	u32 last = first + count - 1;
+	int p, i;
+
+	BUG_ON((first > last) || (last >= priv->num_tags));
+
+	nv_wr32(priv, 0x17e8cc, first);
+	nv_wr32(priv, 0x17e8d0, last);
+	nv_wr32(priv, 0x17e8c8, 0x4); /* trigger clear */
+
+	/* wait until it's finished with clearing */
+	for (p = 0; p < priv->part_nr; ++p) {
+		if (!(priv->part_mask & (1 << p)))
+			continue;
+		for (i = 0; i < priv->subp_nr; ++i)
+			nv_wait(priv, 0x1410c8 + p * 0x2000 + i * 0x400, ~0, 0);
+	}
+}
+
+/* TODO: Figure out tag memory details and drop the over-cautious allocation.
+ */
+static int
+nvc0_ltcg_init_tag_ram(struct nouveau_fb *pfb, struct nvc0_ltcg_priv *priv)
+{
+	u32 tag_size, tag_margin, tag_align;
+	int ret;
+
+	nv_wr32(priv, 0x17e8d8, priv->part_nr);
+
+	/* tags for 1/4 of VRAM should be enough (8192/4 per GiB of VRAM) */
+	priv->num_tags = (pfb->ram.size >> 17) / 4;
+	if (priv->num_tags > (1 << 17))
+		priv->num_tags = 1 << 17; /* we have 17 bits in PTE */
+	priv->num_tags = (priv->num_tags + 63) & ~63; /* round up to 64 */
+
+	tag_align = priv->part_nr * 0x800;
+	tag_margin = (tag_align < 0x6000) ? 0x6000 : tag_align;
+
+	/* 4 part 4 sub: 0x2000 bytes for 56 tags */
+	/* 3 part 4 sub: 0x6000 bytes for 168 tags */
+	/*
+	 * About 147 bytes per tag. Let's be safe and allocate x2, which makes
+	 * 0x4980 bytes for 64 tags, and round up to 0x6000 bytes for 64 tags.
+	 *
+	 * For 4 GiB of memory we'll have 8192 tags which makes 3 MiB, < 0.1 %.
+	 */
+	tag_size  = (priv->num_tags / 64) * 0x6000 + tag_margin;
+	tag_size += tag_align;
+	tag_size  = (tag_size + 0xfff) >> 12; /* round up */
+
+	ret = nouveau_mm_tail(&pfb->vram, 0, tag_size, tag_size, 1,
+	                      &priv->tag_ram);
+	if (ret) {
+		priv->num_tags = 0;
+	} else {
+		u64 tag_base = (priv->tag_ram->offset << 12) + tag_margin;
+
+		tag_base += tag_align - 1;
+		ret = do_div(tag_base, tag_align);
+
+		nv_wr32(priv, 0x17e8d4, tag_base);
+	}
+	ret = nouveau_mm_init(&priv->tags, 0, priv->num_tags, 1);
+
+	return ret;
+}
+
+static int
 nvc0_ltcg_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	       struct nouveau_oclass *oclass, void *data, u32 size,
 	       struct nouveau_object **pobject)
 {
 	struct nvc0_ltcg_priv *priv;
+	struct nouveau_fb *pfb = nouveau_fb(parent);
 	int ret;
 
 	ret = nouveau_ltcg_create(parent, engine, oclass, &priv);
@@ -74,19 +174,44 @@ nvc0_ltcg_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	priv->subp_nr = nv_rd32(priv, 0x17e8dc) >> 24;
+	priv->part_nr = nv_rd32(priv, 0x022438);
+	priv->part_mask = nv_rd32(priv, 0x022554);
+
+	priv->subp_nr = nv_rd32(priv, 0x17e8dc) >> 28;
+
 	nv_mask(priv, 0x17e820, 0x00100000, 0x00000000); /* INTR_EN &= ~0x10 */
 
+	ret = nvc0_ltcg_init_tag_ram(pfb, priv);
+	if (ret)
+		return ret;
+
+	priv->base.tags_alloc = nvc0_ltcg_tags_alloc;
+	priv->base.tags_free  = nvc0_ltcg_tags_free;
+	priv->base.tags_clear = nvc0_ltcg_tags_clear;
+
 	nv_subdev(priv)->intr = nvc0_ltcg_intr;
 	return 0;
 }
 
+static void
+nvc0_ltcg_dtor(struct nouveau_object *object)
+{
+	struct nouveau_ltcg *ltcg = (struct nouveau_ltcg *)object;
+	struct nvc0_ltcg_priv *priv = (struct nvc0_ltcg_priv *)ltcg;
+	struct nouveau_fb *pfb = nouveau_fb(ltcg->base.base.parent);
+
+	nouveau_mm_fini(&priv->tags);
+	nouveau_mm_free(&pfb->vram, &priv->tag_ram);
+
+	nouveau_ltcg_destroy(ltcg);
+}
+
 struct nouveau_oclass
 nvc0_ltcg_oclass = {
 	.handle = NV_SUBDEV(LTCG, 0xc0),
 	.ofuncs = &(struct nouveau_ofuncs) {
 		.ctor = nvc0_ltcg_ctor,
-		.dtor = _nouveau_ltcg_dtor,
+		.dtor = nvc0_ltcg_dtor,
 		.init = _nouveau_ltcg_init,
 		.fini = _nouveau_ltcg_fini,
 	},
diff --git a/drivers/gpu/drm/nouveau/core/subdev/mc/base.c b/drivers/gpu/drm/nouveau/core/subdev/mc/base.c
index 8379aafa6e1b..1c0330b8c9a4 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/mc/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/mc/base.c
@@ -24,10 +24,10 @@
 
 #include <subdev/mc.h>
 
-void
-nouveau_mc_intr(struct nouveau_subdev *subdev)
+static irqreturn_t
+nouveau_mc_intr(int irq, void *arg)
 {
-	struct nouveau_mc *pmc = nouveau_mc(subdev);
+	struct nouveau_mc *pmc = arg;
 	const struct nouveau_mc_intr *map = pmc->intr_map;
 	struct nouveau_subdev *unit;
 	u32 stat, intr;
@@ -35,7 +35,7 @@ nouveau_mc_intr(struct nouveau_subdev *subdev)
 	intr = stat = nv_rd32(pmc, 0x000100);
 	while (stat && map->stat) {
 		if (stat & map->stat) {
-			unit = nouveau_subdev(subdev, map->unit);
+			unit = nouveau_subdev(pmc, map->unit);
 			if (unit && unit->intr)
 				unit->intr(unit);
 			intr &= ~map->stat;
@@ -46,4 +46,56 @@ nouveau_mc_intr(struct nouveau_subdev *subdev)
 	if (intr) {
 		nv_error(pmc, "unknown intr 0x%08x\n", stat);
 	}
+
+	return stat ? IRQ_HANDLED : IRQ_NONE;
+}
+
+int
+_nouveau_mc_fini(struct nouveau_object *object, bool suspend)
+{
+	struct nouveau_mc *pmc = (void *)object;
+	nv_wr32(pmc, 0x000140, 0x00000000);
+	return nouveau_subdev_fini(&pmc->base, suspend);
+}
+
+int
+_nouveau_mc_init(struct nouveau_object *object)
+{
+	struct nouveau_mc *pmc = (void *)object;
+	int ret = nouveau_subdev_init(&pmc->base);
+	if (ret)
+		return ret;
+	nv_wr32(pmc, 0x000140, 0x00000001);
+	return 0;
+}
+
+void
+_nouveau_mc_dtor(struct nouveau_object *object)
+{
+	struct nouveau_device *device = nv_device(object);
+	struct nouveau_mc *pmc = (void *)object;
+	free_irq(device->pdev->irq, pmc);
+	nouveau_subdev_destroy(&pmc->base);
+}
+
+int
+nouveau_mc_create_(struct nouveau_object *parent, struct nouveau_object *engine,
+		   struct nouveau_oclass *oclass, int length, void **pobject)
+{
+	struct nouveau_device *device = nv_device(parent);
+	struct nouveau_mc *pmc;
+	int ret;
+
+	ret = nouveau_subdev_create_(parent, engine, oclass, 0, "PMC",
+				     "master", length, pobject);
+	pmc = *pobject;
+	if (ret)
+		return ret;
+
+	ret = request_irq(device->pdev->irq, nouveau_mc_intr,
+			  IRQF_SHARED, "nouveau", pmc);
+	if (ret < 0)
+		return ret;
+
+	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/core/subdev/mc/nv04.c b/drivers/gpu/drm/nouveau/core/subdev/mc/nv04.c
index 89da8fa7ea0f..8c769715227b 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/mc/nv04.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/mc/nv04.c
@@ -55,7 +55,6 @@ nv04_mc_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	nv_subdev(priv)->intr = nouveau_mc_intr;
 	priv->base.intr_map = nv04_mc_intr;
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/core/subdev/mc/nv44.c b/drivers/gpu/drm/nouveau/core/subdev/mc/nv44.c
index 397d868359ad..51919371810f 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/mc/nv44.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/mc/nv44.c
@@ -41,7 +41,6 @@ nv44_mc_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	nv_subdev(priv)->intr = nouveau_mc_intr;
 	priv->base.intr_map = nv04_mc_intr;
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/core/subdev/mc/nv50.c b/drivers/gpu/drm/nouveau/core/subdev/mc/nv50.c
index 5965add6daee..d796924f9930 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/mc/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/mc/nv50.c
@@ -57,7 +57,6 @@ nv50_mc_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	nv_subdev(priv)->intr = nouveau_mc_intr;
 	priv->base.intr_map = nv50_mc_intr;
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/core/subdev/mc/nv98.c b/drivers/gpu/drm/nouveau/core/subdev/mc/nv98.c
index 3a80b29dce0f..e82fd21b5041 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/mc/nv98.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/mc/nv98.c
@@ -59,7 +59,6 @@ nv98_mc_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	nv_subdev(priv)->intr = nouveau_mc_intr;
 	priv->base.intr_map = nv98_mc_intr;
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/core/subdev/mc/nvc0.c b/drivers/gpu/drm/nouveau/core/subdev/mc/nvc0.c
index 42bbf72023a8..737bd4b682e1 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/mc/nvc0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/mc/nvc0.c
@@ -61,7 +61,6 @@ nvc0_mc_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	nv_subdev(priv)->intr = nouveau_mc_intr;
 	priv->base.intr_map = nvc0_mc_intr;
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/nv40.c b/drivers/gpu/drm/nouveau/core/subdev/therm/nv40.c
index a70d1b7e397b..002e51b3af93 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/nv40.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/nv40.c
@@ -165,7 +165,7 @@ nv40_fan_pwm_set(struct nouveau_therm *therm, int line, u32 divs, u32 duty)
 	return 0;
 }
 
-static void
+void
 nv40_therm_intr(struct nouveau_subdev *subdev)
 {
 	struct nouveau_therm *therm = nouveau_therm(subdev);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/nv50.c b/drivers/gpu/drm/nouveau/core/subdev/therm/nv50.c
index 86632cbd65ce..8cf7597a2182 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/nv50.c
@@ -118,145 +118,36 @@ nv50_fan_pwm_clock(struct nouveau_therm *therm)
 	return pwm_clock;
 }
 
-int
-nv50_temp_get(struct nouveau_therm *therm)
-{
-	return nv_rd32(therm, 0x20400);
-}
-
-static void
-nv50_therm_program_alarms(struct nouveau_therm *therm)
-{
-	struct nouveau_therm_priv *priv = (void *)therm;
-	struct nvbios_therm_sensor *sensor = &priv->bios_sensor;
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->sensor.alarm_program_lock, flags);
-
-	/* enable RISING and FALLING IRQs for shutdown, THRS 0, 1, 2 and 4 */
-	nv_wr32(therm, 0x20000, 0x000003ff);
-
-	/* shutdown: The computer should be shutdown when reached */
-	nv_wr32(therm, 0x20484, sensor->thrs_shutdown.hysteresis);
-	nv_wr32(therm, 0x20480, sensor->thrs_shutdown.temp);
-
-	/* THRS_1 : fan boost*/
-	nv_wr32(therm, 0x204c4, sensor->thrs_fan_boost.temp);
-
-	/* THRS_2 : critical */
-	nv_wr32(therm, 0x204c0, sensor->thrs_critical.temp);
-
-	/* THRS_4 : down clock */
-	nv_wr32(therm, 0x20414, sensor->thrs_down_clock.temp);
-	spin_unlock_irqrestore(&priv->sensor.alarm_program_lock, flags);
-
-	nv_info(therm,
-		"Programmed thresholds [ %d(%d), %d(%d), %d(%d), %d(%d) ]\n",
-		sensor->thrs_fan_boost.temp, sensor->thrs_fan_boost.hysteresis,
-		sensor->thrs_down_clock.temp,
-		sensor->thrs_down_clock.hysteresis,
-		sensor->thrs_critical.temp, sensor->thrs_critical.hysteresis,
-		sensor->thrs_shutdown.temp, sensor->thrs_shutdown.hysteresis);
-
-}
-
-/* must be called with alarm_program_lock taken ! */
 static void
-nv50_therm_threshold_hyst_emulation(struct nouveau_therm *therm,
-				   uint32_t thrs_reg, u8 status_bit,
-				   const struct nvbios_therm_threshold *thrs,
-				   enum nouveau_therm_thrs thrs_name)
+nv50_sensor_setup(struct nouveau_therm *therm)
 {
-	enum nouveau_therm_thrs_direction direction;
-	enum nouveau_therm_thrs_state prev_state, new_state;
-	int temp, cur;
-
-	prev_state = nouveau_therm_sensor_get_threshold_state(therm, thrs_name);
-	temp = nv_rd32(therm, thrs_reg);
-
-	/* program the next threshold */
-	if (temp == thrs->temp) {
-		nv_wr32(therm, thrs_reg, thrs->temp - thrs->hysteresis);
-		new_state = NOUVEAU_THERM_THRS_HIGHER;
-	} else {
-		nv_wr32(therm, thrs_reg, thrs->temp);
-		new_state = NOUVEAU_THERM_THRS_LOWER;
-	}
-
-	/* fix the state (in case someone reprogrammed the alarms) */
-	cur = therm->temp_get(therm);
-	if (new_state == NOUVEAU_THERM_THRS_LOWER && cur > thrs->temp)
-		new_state = NOUVEAU_THERM_THRS_HIGHER;
-	else if (new_state == NOUVEAU_THERM_THRS_HIGHER &&
-		cur < thrs->temp - thrs->hysteresis)
-		new_state = NOUVEAU_THERM_THRS_LOWER;
-	nouveau_therm_sensor_set_threshold_state(therm, thrs_name, new_state);
-
-	/* find the direction */
-	if (prev_state < new_state)
-		direction = NOUVEAU_THERM_THRS_RISING;
-	else if (prev_state > new_state)
-		direction = NOUVEAU_THERM_THRS_FALLING;
-	else
-		return;
-
-	/* advertise a change in direction */
-	nouveau_therm_sensor_event(therm, thrs_name, direction);
+	nv_mask(therm, 0x20010, 0x40000000, 0x0);
+	mdelay(20); /* wait for the temperature to stabilize */
 }
 
-static void
-nv50_therm_intr(struct nouveau_subdev *subdev)
+static int
+nv50_temp_get(struct nouveau_therm *therm)
 {
-	struct nouveau_therm *therm = nouveau_therm(subdev);
 	struct nouveau_therm_priv *priv = (void *)therm;
 	struct nvbios_therm_sensor *sensor = &priv->bios_sensor;
-	unsigned long flags;
-	uint32_t intr;
-
-	spin_lock_irqsave(&priv->sensor.alarm_program_lock, flags);
-
-	intr = nv_rd32(therm, 0x20100);
-
-	/* THRS_4: downclock */
-	if (intr & 0x002) {
-		nv50_therm_threshold_hyst_emulation(therm, 0x20414, 24,
-						  &sensor->thrs_down_clock,
-						  NOUVEAU_THERM_THRS_DOWNCLOCK);
-		intr &= ~0x002;
-	}
+	int core_temp;
 
-	/* shutdown */
-	if (intr & 0x004) {
-		nv50_therm_threshold_hyst_emulation(therm, 0x20480, 20,
-						   &sensor->thrs_shutdown,
-						   NOUVEAU_THERM_THRS_SHUTDOWN);
-		intr &= ~0x004;
-	}
-
-	/* THRS_1 : fan boost */
-	if (intr & 0x008) {
-		nv50_therm_threshold_hyst_emulation(therm, 0x204c4, 21,
-						   &sensor->thrs_fan_boost,
-						   NOUVEAU_THERM_THRS_FANBOOST);
-		intr &= ~0x008;
-	}
+	core_temp = nv_rd32(therm, 0x20014) & 0x3fff;
 
-	/* THRS_2 : critical */
-	if (intr & 0x010) {
-		nv50_therm_threshold_hyst_emulation(therm, 0x204c0, 22,
-						   &sensor->thrs_critical,
-						   NOUVEAU_THERM_THRS_CRITICAL);
-		intr &= ~0x010;
-	}
+	/* if the slope or the offset is unset, do no use the sensor */
+	if (!sensor->slope_div || !sensor->slope_mult ||
+	    !sensor->offset_num || !sensor->offset_den)
+	    return -ENODEV;
 
-	if (intr)
-		nv_error(therm, "unhandled intr 0x%08x\n", intr);
+	core_temp = core_temp * sensor->slope_mult / sensor->slope_div;
+	core_temp = core_temp + sensor->offset_num / sensor->offset_den;
+	core_temp = core_temp + sensor->offset_constant - 8;
 
-	/* ACK everything */
-	nv_wr32(therm, 0x20100, 0xffffffff);
-	nv_wr32(therm, 0x1100, 0x10000); /* PBUS */
+	/* reserve negative temperatures for errors */
+	if (core_temp < 0)
+		core_temp = 0;
 
-	spin_unlock_irqrestore(&priv->sensor.alarm_program_lock, flags);
+	return core_temp;
 }
 
 static int
@@ -278,33 +169,29 @@ nv50_therm_ctor(struct nouveau_object *parent,
 	priv->base.base.pwm_set = nv50_fan_pwm_set;
 	priv->base.base.pwm_clock = nv50_fan_pwm_clock;
 	priv->base.base.temp_get = nv50_temp_get;
-	priv->base.sensor.program_alarms = nv50_therm_program_alarms;
-	nv_subdev(priv)->intr = nv50_therm_intr;
-
-	/* init the thresholds */
-	nouveau_therm_sensor_set_threshold_state(&priv->base.base,
-						 NOUVEAU_THERM_THRS_SHUTDOWN,
-						 NOUVEAU_THERM_THRS_LOWER);
-	nouveau_therm_sensor_set_threshold_state(&priv->base.base,
-						 NOUVEAU_THERM_THRS_FANBOOST,
-						 NOUVEAU_THERM_THRS_LOWER);
-	nouveau_therm_sensor_set_threshold_state(&priv->base.base,
-						 NOUVEAU_THERM_THRS_CRITICAL,
-						 NOUVEAU_THERM_THRS_LOWER);
-	nouveau_therm_sensor_set_threshold_state(&priv->base.base,
-						 NOUVEAU_THERM_THRS_DOWNCLOCK,
-						 NOUVEAU_THERM_THRS_LOWER);
+	priv->base.sensor.program_alarms = nouveau_therm_program_alarms_polling;
+	nv_subdev(priv)->intr = nv40_therm_intr;
 
 	return nouveau_therm_preinit(&priv->base.base);
 }
 
+static int
+nv50_therm_init(struct nouveau_object *object)
+{
+	struct nouveau_therm *therm = (void *)object;
+
+	nv50_sensor_setup(therm);
+
+	return _nouveau_therm_init(object);
+}
+
 struct nouveau_oclass
 nv50_therm_oclass = {
 	.handle = NV_SUBDEV(THERM, 0x50),
 	.ofuncs = &(struct nouveau_ofuncs) {
 		.ctor = nv50_therm_ctor,
 		.dtor = _nouveau_therm_dtor,
-		.init = _nouveau_therm_init,
+		.init = nv50_therm_init,
 		.fini = _nouveau_therm_fini,
 	},
 };
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/nv84.c b/drivers/gpu/drm/nouveau/core/subdev/therm/nv84.c
new file mode 100644
index 000000000000..42ba633ccff7
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/nv84.c
@@ -0,0 +1,221 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ * 	    Martin Peres
+ */
+
+#include "priv.h"
+
+struct nv84_therm_priv {
+	struct nouveau_therm_priv base;
+};
+
+int
+nv84_temp_get(struct nouveau_therm *therm)
+{
+	return nv_rd32(therm, 0x20400);
+}
+
+static void
+nv84_therm_program_alarms(struct nouveau_therm *therm)
+{
+	struct nouveau_therm_priv *priv = (void *)therm;
+	struct nvbios_therm_sensor *sensor = &priv->bios_sensor;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->sensor.alarm_program_lock, flags);
+
+	/* enable RISING and FALLING IRQs for shutdown, THRS 0, 1, 2 and 4 */
+	nv_wr32(therm, 0x20000, 0x000003ff);
+
+	/* shutdown: The computer should be shutdown when reached */
+	nv_wr32(therm, 0x20484, sensor->thrs_shutdown.hysteresis);
+	nv_wr32(therm, 0x20480, sensor->thrs_shutdown.temp);
+
+	/* THRS_1 : fan boost*/
+	nv_wr32(therm, 0x204c4, sensor->thrs_fan_boost.temp);
+
+	/* THRS_2 : critical */
+	nv_wr32(therm, 0x204c0, sensor->thrs_critical.temp);
+
+	/* THRS_4 : down clock */
+	nv_wr32(therm, 0x20414, sensor->thrs_down_clock.temp);
+	spin_unlock_irqrestore(&priv->sensor.alarm_program_lock, flags);
+
+	nv_debug(therm,
+		 "Programmed thresholds [ %d(%d), %d(%d), %d(%d), %d(%d) ]\n",
+		 sensor->thrs_fan_boost.temp, sensor->thrs_fan_boost.hysteresis,
+		 sensor->thrs_down_clock.temp,
+		 sensor->thrs_down_clock.hysteresis,
+		 sensor->thrs_critical.temp, sensor->thrs_critical.hysteresis,
+		 sensor->thrs_shutdown.temp, sensor->thrs_shutdown.hysteresis);
+
+}
+
+/* must be called with alarm_program_lock taken ! */
+static void
+nv84_therm_threshold_hyst_emulation(struct nouveau_therm *therm,
+				   uint32_t thrs_reg, u8 status_bit,
+				   const struct nvbios_therm_threshold *thrs,
+				   enum nouveau_therm_thrs thrs_name)
+{
+	enum nouveau_therm_thrs_direction direction;
+	enum nouveau_therm_thrs_state prev_state, new_state;
+	int temp, cur;
+
+	prev_state = nouveau_therm_sensor_get_threshold_state(therm, thrs_name);
+	temp = nv_rd32(therm, thrs_reg);
+
+	/* program the next threshold */
+	if (temp == thrs->temp) {
+		nv_wr32(therm, thrs_reg, thrs->temp - thrs->hysteresis);
+		new_state = NOUVEAU_THERM_THRS_HIGHER;
+	} else {
+		nv_wr32(therm, thrs_reg, thrs->temp);
+		new_state = NOUVEAU_THERM_THRS_LOWER;
+	}
+
+	/* fix the state (in case someone reprogrammed the alarms) */
+	cur = therm->temp_get(therm);
+	if (new_state == NOUVEAU_THERM_THRS_LOWER && cur > thrs->temp)
+		new_state = NOUVEAU_THERM_THRS_HIGHER;
+	else if (new_state == NOUVEAU_THERM_THRS_HIGHER &&
+		cur < thrs->temp - thrs->hysteresis)
+		new_state = NOUVEAU_THERM_THRS_LOWER;
+	nouveau_therm_sensor_set_threshold_state(therm, thrs_name, new_state);
+
+	/* find the direction */
+	if (prev_state < new_state)
+		direction = NOUVEAU_THERM_THRS_RISING;
+	else if (prev_state > new_state)
+		direction = NOUVEAU_THERM_THRS_FALLING;
+	else
+		return;
+
+	/* advertise a change in direction */
+	nouveau_therm_sensor_event(therm, thrs_name, direction);
+}
+
+static void
+nv84_therm_intr(struct nouveau_subdev *subdev)
+{
+	struct nouveau_therm *therm = nouveau_therm(subdev);
+	struct nouveau_therm_priv *priv = (void *)therm;
+	struct nvbios_therm_sensor *sensor = &priv->bios_sensor;
+	unsigned long flags;
+	uint32_t intr;
+
+	spin_lock_irqsave(&priv->sensor.alarm_program_lock, flags);
+
+	intr = nv_rd32(therm, 0x20100);
+
+	/* THRS_4: downclock */
+	if (intr & 0x002) {
+		nv84_therm_threshold_hyst_emulation(therm, 0x20414, 24,
+						  &sensor->thrs_down_clock,
+						  NOUVEAU_THERM_THRS_DOWNCLOCK);
+		intr &= ~0x002;
+	}
+
+	/* shutdown */
+	if (intr & 0x004) {
+		nv84_therm_threshold_hyst_emulation(therm, 0x20480, 20,
+						   &sensor->thrs_shutdown,
+						   NOUVEAU_THERM_THRS_SHUTDOWN);
+		intr &= ~0x004;
+	}
+
+	/* THRS_1 : fan boost */
+	if (intr & 0x008) {
+		nv84_therm_threshold_hyst_emulation(therm, 0x204c4, 21,
+						   &sensor->thrs_fan_boost,
+						   NOUVEAU_THERM_THRS_FANBOOST);
+		intr &= ~0x008;
+	}
+
+	/* THRS_2 : critical */
+	if (intr & 0x010) {
+		nv84_therm_threshold_hyst_emulation(therm, 0x204c0, 22,
+						   &sensor->thrs_critical,
+						   NOUVEAU_THERM_THRS_CRITICAL);
+		intr &= ~0x010;
+	}
+
+	if (intr)
+		nv_error(therm, "unhandled intr 0x%08x\n", intr);
+
+	/* ACK everything */
+	nv_wr32(therm, 0x20100, 0xffffffff);
+	nv_wr32(therm, 0x1100, 0x10000); /* PBUS */
+
+	spin_unlock_irqrestore(&priv->sensor.alarm_program_lock, flags);
+}
+
+static int
+nv84_therm_ctor(struct nouveau_object *parent,
+		struct nouveau_object *engine,
+		struct nouveau_oclass *oclass, void *data, u32 size,
+		struct nouveau_object **pobject)
+{
+	struct nv84_therm_priv *priv;
+	int ret;
+
+	ret = nouveau_therm_create(parent, engine, oclass, &priv);
+	*pobject = nv_object(priv);
+	if (ret)
+		return ret;
+
+	priv->base.base.pwm_ctrl = nv50_fan_pwm_ctrl;
+	priv->base.base.pwm_get = nv50_fan_pwm_get;
+	priv->base.base.pwm_set = nv50_fan_pwm_set;
+	priv->base.base.pwm_clock = nv50_fan_pwm_clock;
+	priv->base.base.temp_get = nv84_temp_get;
+	priv->base.sensor.program_alarms = nv84_therm_program_alarms;
+	nv_subdev(priv)->intr = nv84_therm_intr;
+
+	/* init the thresholds */
+	nouveau_therm_sensor_set_threshold_state(&priv->base.base,
+						 NOUVEAU_THERM_THRS_SHUTDOWN,
+						 NOUVEAU_THERM_THRS_LOWER);
+	nouveau_therm_sensor_set_threshold_state(&priv->base.base,
+						 NOUVEAU_THERM_THRS_FANBOOST,
+						 NOUVEAU_THERM_THRS_LOWER);
+	nouveau_therm_sensor_set_threshold_state(&priv->base.base,
+						 NOUVEAU_THERM_THRS_CRITICAL,
+						 NOUVEAU_THERM_THRS_LOWER);
+	nouveau_therm_sensor_set_threshold_state(&priv->base.base,
+						 NOUVEAU_THERM_THRS_DOWNCLOCK,
+						 NOUVEAU_THERM_THRS_LOWER);
+
+	return nouveau_therm_preinit(&priv->base.base);
+}
+
+struct nouveau_oclass
+nv84_therm_oclass = {
+	.handle = NV_SUBDEV(THERM, 0x84),
+	.ofuncs = &(struct nouveau_ofuncs) {
+		.ctor = nv84_therm_ctor,
+		.dtor = _nouveau_therm_dtor,
+		.init = _nouveau_therm_init,
+		.fini = _nouveau_therm_fini,
+	},
+};
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/nva3.c b/drivers/gpu/drm/nouveau/core/subdev/therm/nva3.c
index 2dcc5437116a..d11a7c400813 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/nva3.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/nva3.c
@@ -81,7 +81,7 @@ nva3_therm_ctor(struct nouveau_object *parent,
 	priv->base.base.pwm_get = nv50_fan_pwm_get;
 	priv->base.base.pwm_set = nv50_fan_pwm_set;
 	priv->base.base.pwm_clock = nv50_fan_pwm_clock;
-	priv->base.base.temp_get = nv50_temp_get;
+	priv->base.base.temp_get = nv84_temp_get;
 	priv->base.base.fan_sense = nva3_therm_fan_sense;
 	priv->base.sensor.program_alarms = nouveau_therm_program_alarms_polling;
 	return nouveau_therm_preinit(&priv->base.base);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c b/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c
index d7d30ee8332e..54c28bdc4204 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c
@@ -135,7 +135,7 @@ nvd0_therm_ctor(struct nouveau_object *parent,
 	priv->base.base.pwm_get = nvd0_fan_pwm_get;
 	priv->base.base.pwm_set = nvd0_fan_pwm_set;
 	priv->base.base.pwm_clock = nvd0_fan_pwm_clock;
-	priv->base.base.temp_get = nv50_temp_get;
+	priv->base.base.temp_get = nv84_temp_get;
 	priv->base.base.fan_sense = nva3_therm_fan_sense;
 	priv->base.sensor.program_alarms = nouveau_therm_program_alarms_polling;
 	return nouveau_therm_preinit(&priv->base.base);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/priv.h b/drivers/gpu/drm/nouveau/core/subdev/therm/priv.h
index 438d9824b774..15ca64e481f1 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/priv.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/priv.h
@@ -134,11 +134,12 @@ void nouveau_therm_sensor_event(struct nouveau_therm *therm,
 			        enum nouveau_therm_thrs_direction dir);
 void nouveau_therm_program_alarms_polling(struct nouveau_therm *therm);
 
+void nv40_therm_intr(struct nouveau_subdev *);
 int nv50_fan_pwm_ctrl(struct nouveau_therm *, int, bool);
 int nv50_fan_pwm_get(struct nouveau_therm *, int, u32 *, u32 *);
 int nv50_fan_pwm_set(struct nouveau_therm *, int, u32, u32);
 int nv50_fan_pwm_clock(struct nouveau_therm *);
-int nv50_temp_get(struct nouveau_therm *therm);
+int nv84_temp_get(struct nouveau_therm *therm);
 
 int nva3_therm_fan_sense(struct nouveau_therm *);
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/temp.c b/drivers/gpu/drm/nouveau/core/subdev/therm/temp.c
index 470f6a47b656..dde746c78c8a 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/temp.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/temp.c
@@ -205,13 +205,13 @@ nouveau_therm_program_alarms_polling(struct nouveau_therm *therm)
 	struct nouveau_therm_priv *priv = (void *)therm;
 	struct nvbios_therm_sensor *sensor = &priv->bios_sensor;
 
-	nv_info(therm,
-		"programmed thresholds [ %d(%d), %d(%d), %d(%d), %d(%d) ]\n",
-		sensor->thrs_fan_boost.temp, sensor->thrs_fan_boost.hysteresis,
-		sensor->thrs_down_clock.temp,
-		sensor->thrs_down_clock.hysteresis,
-		sensor->thrs_critical.temp, sensor->thrs_critical.hysteresis,
-		sensor->thrs_shutdown.temp, sensor->thrs_shutdown.hysteresis);
+	nv_debug(therm,
+		 "programmed thresholds [ %d(%d), %d(%d), %d(%d), %d(%d) ]\n",
+		 sensor->thrs_fan_boost.temp, sensor->thrs_fan_boost.hysteresis,
+		 sensor->thrs_down_clock.temp,
+		 sensor->thrs_down_clock.hysteresis,
+		 sensor->thrs_critical.temp, sensor->thrs_critical.hysteresis,
+		 sensor->thrs_shutdown.temp, sensor->thrs_shutdown.hysteresis);
 
 	alarm_timer_callback(&priv->sensor.therm_poll_alarm);
 }
diff --git a/drivers/gpu/drm/nouveau/core/subdev/timer/nv04.c b/drivers/gpu/drm/nouveau/core/subdev/timer/nv04.c
index 8e1bae4f12e8..9469b8275675 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/timer/nv04.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/timer/nv04.c
@@ -96,11 +96,16 @@ nv04_timer_alarm(struct nouveau_timer *ptimer, u64 time,
 
 	/* append new alarm to list, in soonest-alarm-first order */
 	spin_lock_irqsave(&priv->lock, flags);
-	list_for_each_entry(list, &priv->alarms, head) {
-		if (list->timestamp > alarm->timestamp)
-			break;
+	if (!time) {
+		if (!list_empty(&alarm->head))
+			list_del(&alarm->head);
+	} else {
+		list_for_each_entry(list, &priv->alarms, head) {
+			if (list->timestamp > alarm->timestamp)
+				break;
+		}
+		list_add_tail(&alarm->head, &list->head);
 	}
-	list_add_tail(&alarm->head, &list->head);
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	/* process pending alarms */
diff --git a/drivers/gpu/drm/nouveau/core/subdev/vm/nv04.c b/drivers/gpu/drm/nouveau/core/subdev/vm/nv04.c
index 6adbbc9cc361..ed45437167f2 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/vm/nv04.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/vm/nv04.c
@@ -110,7 +110,7 @@ nv04_vmmgr_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL,
 				 (NV04_PDMA_SIZE / NV04_PDMA_PAGE) * 4 +
 				 8, 16, NVOBJ_FLAG_ZERO_ALLOC,
 				 &priv->vm->pgt[0].obj[0]);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/vm/nv41.c b/drivers/gpu/drm/nouveau/core/subdev/vm/nv41.c
index 9474cfca6e4c..064c76262876 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/vm/nv41.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/vm/nv41.c
@@ -119,7 +119,7 @@ nv41_vmmgr_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL,
 				(NV41_GART_SIZE / NV41_GART_PAGE) * 4,
 				 16, NVOBJ_FLAG_ZERO_ALLOC,
 				 &priv->vm->pgt[0].obj[0]);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/vm/nv44.c b/drivers/gpu/drm/nouveau/core/subdev/vm/nv44.c
index aa8131436e3d..fae1f67d5948 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/vm/nv44.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/vm/nv44.c
@@ -196,7 +196,7 @@ nv44_vmmgr_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_gpuobj_new(parent, NULL,
+	ret = nouveau_gpuobj_new(nv_object(priv), NULL,
 				(NV44_GART_SIZE / NV44_GART_PAGE) * 4,
 				 512 * 1024, NVOBJ_FLAG_ZERO_ALLOC,
 				 &priv->vm->pgt[0].obj[0]);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/vm/nvc0.c b/drivers/gpu/drm/nouveau/core/subdev/vm/nvc0.c
index 30c61e6c2017..4c3b0a23b9d6 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/vm/nvc0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/vm/nvc0.c
@@ -28,12 +28,54 @@
 #include <subdev/timer.h>
 #include <subdev/fb.h>
 #include <subdev/vm.h>
+#include <subdev/ltcg.h>
 
 struct nvc0_vmmgr_priv {
 	struct nouveau_vmmgr base;
 	spinlock_t lock;
 };
 
+
+/* Map from compressed to corresponding uncompressed storage type.
+ * The value 0xff represents an invalid storage type.
+ */
+const u8 nvc0_pte_storage_type_map[256] =
+{
+	0x00, 0x01, 0x01, 0x01, 0x01, 0x01, 0xff, 0x01, /* 0x00 */
+	0x01, 0x01, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff,
+	0xff, 0x11, 0xff, 0xff, 0xff, 0xff, 0xff, 0x11, /* 0x10 */
+	0x11, 0x11, 0x11, 0xff, 0xff, 0xff, 0xff, 0xff,
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x26, 0x27, /* 0x20 */
+	0x28, 0x29, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, /* 0x30 */
+	0xff, 0xff, 0x26, 0x27, 0x28, 0x29, 0x26, 0x27,
+	0x28, 0x29, 0xff, 0xff, 0xff, 0xff, 0x46, 0xff, /* 0x40 */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	0xff, 0x46, 0x46, 0x46, 0x46, 0xff, 0xff, 0xff, /* 0x50 */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, /* 0x60 */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, /* 0x70 */
+	0xff, 0xff, 0xff, 0x7b, 0xff, 0xff, 0xff, 0xff,
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x7b, 0x7b, /* 0x80 */
+	0x7b, 0x7b, 0xff, 0x8b, 0x8c, 0x8d, 0x8e, 0xff,
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, /* 0x90 */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	0xff, 0xff, 0xff, 0x8b, 0x8c, 0x8d, 0x8e, 0xa7, /* 0xa0 */
+	0xa8, 0xa9, 0xaa, 0xff, 0xff, 0xff, 0xff, 0xff,
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, /* 0xb0 */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xa7,
+	0xa8, 0xa9, 0xaa, 0xc3, 0xff, 0xff, 0xff, 0xff, /* 0xc0 */
+	0xff, 0xff, 0xff, 0xff, 0xfe, 0xfe, 0xc3, 0xc3,
+	0xc3, 0xc3, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, /* 0xd0 */
+	0xfe, 0xff, 0xff, 0xfe, 0xff, 0xfe, 0xff, 0xfe,
+	0xfe, 0xff, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xff, /* 0xe0 */
+	0xff, 0xfe, 0xff, 0xfe, 0xff, 0xfe, 0xfe, 0xff,
+	0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, /* 0xf0 */
+	0xfe, 0xfe, 0xfe, 0xfe, 0xff, 0xfd, 0xfe, 0xff
+};
+
+
 static void
 nvc0_vm_map_pgt(struct nouveau_gpuobj *pgd, u32 index,
 		struct nouveau_gpuobj *pgt[2])
@@ -68,10 +110,20 @@ static void
 nvc0_vm_map(struct nouveau_vma *vma, struct nouveau_gpuobj *pgt,
 	    struct nouveau_mem *mem, u32 pte, u32 cnt, u64 phys, u64 delta)
 {
-	u32 next = 1 << (vma->node->type - 8);
+	u64 next = 1 << (vma->node->type - 8);
 
 	phys  = nvc0_vm_addr(vma, phys, mem->memtype, 0);
 	pte <<= 3;
+
+	if (mem->tag) {
+		struct nouveau_ltcg *ltcg =
+			nouveau_ltcg(vma->vm->vmm->base.base.parent);
+		u32 tag = mem->tag->offset + (delta >> 17);
+		phys |= (u64)tag << (32 + 12);
+		next |= (u64)1   << (32 + 12);
+		ltcg->tags_clear(ltcg, tag, cnt);
+	}
+
 	while (cnt--) {
 		nv_wo32(pgt, pte + 0, lower_32_bits(phys));
 		nv_wo32(pgt, pte + 4, upper_32_bits(phys));
@@ -85,10 +137,12 @@ nvc0_vm_map_sg(struct nouveau_vma *vma, struct nouveau_gpuobj *pgt,
 	       struct nouveau_mem *mem, u32 pte, u32 cnt, dma_addr_t *list)
 {
 	u32 target = (vma->access & NV_MEM_ACCESS_NOSNOOP) ? 7 : 5;
+	/* compressed storage types are invalid for system memory */
+	u32 memtype = nvc0_pte_storage_type_map[mem->memtype & 0xff];
 
 	pte <<= 3;
 	while (cnt--) {
-		u64 phys = nvc0_vm_addr(vma, *list++, mem->memtype, target);
+		u64 phys = nvc0_vm_addr(vma, *list++, memtype, target);
 		nv_wo32(pgt, pte + 0, lower_32_bits(phys));
 		nv_wo32(pgt, pte + 4, upper_32_bits(phys));
 		pte += 8;
diff --git a/drivers/gpu/drm/nouveau/dispnv04/Makefile b/drivers/gpu/drm/nouveau/dispnv04/Makefile
new file mode 100644
index 000000000000..ea3f5b8a0f95
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/dispnv04/Makefile
@@ -0,0 +1,10 @@
+nouveau-y += dispnv04/arb.o
+nouveau-y += dispnv04/crtc.o
+nouveau-y += dispnv04/cursor.o
+nouveau-y += dispnv04/dac.o
+nouveau-y += dispnv04/dfp.o
+nouveau-y += dispnv04/disp.o
+nouveau-y += dispnv04/hw.o
+nouveau-y += dispnv04/tvmodesnv17.o
+nouveau-y += dispnv04/tvnv04.o
+nouveau-y += dispnv04/tvnv17.o
diff --git a/drivers/gpu/drm/nouveau/nouveau_calc.c b/drivers/gpu/drm/nouveau/dispnv04/arb.c
index 6da576445b3d..2e70462883e8 100644
--- a/drivers/gpu/drm/nouveau/nouveau_calc.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/arb.c
@@ -25,7 +25,7 @@
 
 #include "nouveau_drm.h"
 #include "nouveau_reg.h"
-#include "nouveau_hw.h"
+#include "hw.h"
 
 /****************************************************************************\
 *                                                                            *
diff --git a/drivers/gpu/drm/nouveau/nv04_crtc.c b/drivers/gpu/drm/nouveau/dispnv04/crtc.c
index 6578cd28c556..0782bd2f1e04 100644
--- a/drivers/gpu/drm/nouveau/nv04_crtc.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/crtc.c
@@ -33,10 +33,10 @@
 #include "nouveau_encoder.h"
 #include "nouveau_connector.h"
 #include "nouveau_crtc.h"
-#include "nouveau_hw.h"
+#include "hw.h"
 #include "nvreg.h"
 #include "nouveau_fbcon.h"
-#include "nv04_display.h"
+#include "disp.h"
 
 #include <subdev/bios/pll.h>
 #include <subdev/clock.h>
@@ -1070,4 +1070,3 @@ nv04_crtc_create(struct drm_device *dev, int crtc_num)
 
 	return 0;
 }
-
diff --git a/drivers/gpu/drm/nouveau/nv04_cursor.c b/drivers/gpu/drm/nouveau/dispnv04/cursor.c
index fe86f0de348f..a810303169de 100644
--- a/drivers/gpu/drm/nouveau/nv04_cursor.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/cursor.c
@@ -3,7 +3,7 @@
 #include "nouveau_drm.h"
 #include "nouveau_reg.h"
 #include "nouveau_crtc.h"
-#include "nouveau_hw.h"
+#include "hw.h"
 
 static void
 nv04_cursor_show(struct nouveau_crtc *nv_crtc, bool update)
@@ -68,4 +68,3 @@ nv04_cursor_init(struct nouveau_crtc *crtc)
 	crtc->cursor.show = nv04_cursor_show;
 	return 0;
 }
-
diff --git a/drivers/gpu/drm/nouveau/nv04_dac.c b/drivers/gpu/drm/nouveau/dispnv04/dac.c
index 64f7020fb605..434b920f6bd4 100644
--- a/drivers/gpu/drm/nouveau/nv04_dac.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/dac.c
@@ -31,7 +31,7 @@
 #include "nouveau_encoder.h"
 #include "nouveau_connector.h"
 #include "nouveau_crtc.h"
-#include "nouveau_hw.h"
+#include "hw.h"
 #include "nvreg.h"
 
 #include <subdev/bios/gpio.h>
diff --git a/drivers/gpu/drm/nouveau/nv04_dfp.c b/drivers/gpu/drm/nouveau/dispnv04/dfp.c
index 7e24cdf1cb39..93dd23ff0093 100644
--- a/drivers/gpu/drm/nouveau/nv04_dfp.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/dfp.c
@@ -32,7 +32,7 @@
 #include "nouveau_encoder.h"
 #include "nouveau_connector.h"
 #include "nouveau_crtc.h"
-#include "nouveau_hw.h"
+#include "hw.h"
 #include "nvreg.h"
 
 #include <drm/i2c/sil164.h>
diff --git a/drivers/gpu/drm/nouveau/nv04_display.c b/drivers/gpu/drm/nouveau/dispnv04/disp.c
index ad48444c385c..4908d3fd0486 100644
--- a/drivers/gpu/drm/nouveau/nv04_display.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/disp.c
@@ -30,7 +30,7 @@
 
 #include "nouveau_drm.h"
 #include "nouveau_reg.h"
-#include "nouveau_hw.h"
+#include "hw.h"
 #include "nouveau_encoder.h"
 #include "nouveau_connector.h"
 
diff --git a/drivers/gpu/drm/nouveau/nv04_display.h b/drivers/gpu/drm/nouveau/dispnv04/disp.h
index a0a031dad13f..a0a031dad13f 100644
--- a/drivers/gpu/drm/nouveau/nv04_display.h
+++ b/drivers/gpu/drm/nouveau/dispnv04/disp.h
diff --git a/drivers/gpu/drm/nouveau/nouveau_hw.c b/drivers/gpu/drm/nouveau/dispnv04/hw.c
index 617a06ffdb46..973056b86207 100644
--- a/drivers/gpu/drm/nouveau/nouveau_hw.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/hw.c
@@ -24,7 +24,7 @@
 
 #include <drm/drmP.h>
 #include "nouveau_drm.h"
-#include "nouveau_hw.h"
+#include "hw.h"
 
 #include <subdev/bios/pll.h>
 #include <subdev/clock.h>
diff --git a/drivers/gpu/drm/nouveau/nouveau_hw.h b/drivers/gpu/drm/nouveau/dispnv04/hw.h
index 7dff1021fab4..eeb70d912d99 100644
--- a/drivers/gpu/drm/nouveau/nouveau_hw.h
+++ b/drivers/gpu/drm/nouveau/dispnv04/hw.h
@@ -24,7 +24,8 @@
 #define __NOUVEAU_HW_H__
 
 #include <drm/drmP.h>
-#include "nv04_display.h"
+#include "disp.h"
+#include "nvreg.h"
 
 #include <subdev/bios/pll.h>
 
diff --git a/drivers/gpu/drm/nouveau/nvreg.h b/drivers/gpu/drm/nouveau/dispnv04/nvreg.h
index bbfb1a68fb11..bbfb1a68fb11 100644
--- a/drivers/gpu/drm/nouveau/nvreg.h
+++ b/drivers/gpu/drm/nouveau/dispnv04/nvreg.h
diff --git a/drivers/gpu/drm/nouveau/nv17_tv_modes.c b/drivers/gpu/drm/nouveau/dispnv04/tvmodesnv17.c
index 1cdfe2a5875d..08c6f5e50610 100644
--- a/drivers/gpu/drm/nouveau/nv17_tv_modes.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/tvmodesnv17.c
@@ -29,8 +29,8 @@
 #include "nouveau_drm.h"
 #include "nouveau_encoder.h"
 #include "nouveau_crtc.h"
-#include "nouveau_hw.h"
-#include "nv17_tv.h"
+#include "hw.h"
+#include "tvnv17.h"
 
 char *nv17_tv_norm_names[NUM_TV_NORMS] = {
 	[TV_NORM_PAL] = "PAL",
diff --git a/drivers/gpu/drm/nouveau/nv04_tv.c b/drivers/gpu/drm/nouveau/dispnv04/tvnv04.c
index 4a69ccdef9b4..bf13db4e8631 100644
--- a/drivers/gpu/drm/nouveau/nv04_tv.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/tvnv04.c
@@ -30,7 +30,7 @@
 #include "nouveau_encoder.h"
 #include "nouveau_connector.h"
 #include "nouveau_crtc.h"
-#include "nouveau_hw.h"
+#include "hw.h"
 #include <drm/drm_crtc_helper.h>
 
 #include <drm/i2c/ch7006.h>
diff --git a/drivers/gpu/drm/nouveau/nv17_tv.c b/drivers/gpu/drm/nouveau/dispnv04/tvnv17.c
index 977e42be2050..acef48f4a4ea 100644
--- a/drivers/gpu/drm/nouveau/nv17_tv.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/tvnv17.c
@@ -31,8 +31,8 @@
 #include "nouveau_encoder.h"
 #include "nouveau_connector.h"
 #include "nouveau_crtc.h"
-#include "nouveau_hw.h"
-#include "nv17_tv.h"
+#include "hw.h"
+#include "tvnv17.h"
 
 #include <core/device.h>
 
diff --git a/drivers/gpu/drm/nouveau/nv17_tv.h b/drivers/gpu/drm/nouveau/dispnv04/tvnv17.h
index 7b331543a41b..7b331543a41b 100644
--- a/drivers/gpu/drm/nouveau/nv17_tv.h
+++ b/drivers/gpu/drm/nouveau/dispnv04/tvnv17.h
diff --git a/drivers/gpu/drm/nouveau/nouveau_abi16.c b/drivers/gpu/drm/nouveau/nouveau_abi16.c
index 5eb3e0da7c6e..1c4c6c9161ac 100644
--- a/drivers/gpu/drm/nouveau/nouveau_abi16.c
+++ b/drivers/gpu/drm/nouveau/nouveau_abi16.c
@@ -30,6 +30,7 @@
 #include <subdev/fb.h>
 #include <subdev/timer.h>
 #include <subdev/instmem.h>
+#include <engine/graph.h>
 
 #include "nouveau_drm.h"
 #include "nouveau_dma.h"
@@ -168,6 +169,7 @@ nouveau_abi16_ioctl_getparam(ABI16_IOCTL_ARGS)
 	struct nouveau_drm *drm = nouveau_drm(dev);
 	struct nouveau_device *device = nv_device(drm->device);
 	struct nouveau_timer *ptimer = nouveau_timer(device);
+	struct nouveau_graph *graph = (void *)nouveau_engine(device, NVDEV_ENGINE_GR);
 	struct drm_nouveau_getparam *getparam = data;
 
 	switch (getparam->param) {
@@ -208,14 +210,8 @@ nouveau_abi16_ioctl_getparam(ABI16_IOCTL_ARGS)
 		getparam->value = 1;
 		break;
 	case NOUVEAU_GETPARAM_GRAPH_UNITS:
-		/* NV40 and NV50 versions are quite different, but register
-		 * address is the same. User is supposed to know the card
-		 * family anyway... */
-		if (device->chipset >= 0x40) {
-			getparam->value = nv_rd32(device, 0x001540);
-			break;
-		}
-		/* FALLTHRU */
+		getparam->value = graph->units ? graph->units(graph) : 0;
+		break;
 	default:
 		nv_debug(device, "unknown parameter %lld\n", getparam->param);
 		return -EINVAL;
diff --git a/drivers/gpu/drm/nouveau/nouveau_backlight.c b/drivers/gpu/drm/nouveau/nouveau_backlight.c
index 5d940302d2aa..2ffad2176b7f 100644
--- a/drivers/gpu/drm/nouveau/nouveau_backlight.c
+++ b/drivers/gpu/drm/nouveau/nouveau_backlight.c
@@ -239,6 +239,9 @@ nouveau_backlight_init(struct drm_device *dev)
 		case NV_40:
 			return nv40_backlight_init(connector);
 		case NV_50:
+		case NV_C0:
+		case NV_D0:
+		case NV_E0:
 			return nv50_backlight_init(connector);
 		default:
 			break;
diff --git a/drivers/gpu/drm/nouveau/nouveau_bios.c b/drivers/gpu/drm/nouveau/nouveau_bios.c
index 50a6dd02f7c5..6aa2137e093a 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bios.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bios.c
@@ -28,7 +28,7 @@
 
 #include "nouveau_drm.h"
 #include "nouveau_reg.h"
-#include "nouveau_hw.h"
+#include "dispnv04/hw.h"
 #include "nouveau_encoder.h"
 
 #include <linux/io-mapping.h>
diff --git a/drivers/gpu/drm/nouveau/nouveau_bios.h b/drivers/gpu/drm/nouveau/nouveau_bios.h
index 7ccd28f11adf..0067586eb015 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bios.h
+++ b/drivers/gpu/drm/nouveau/nouveau_bios.h
@@ -24,8 +24,6 @@
 #ifndef __NOUVEAU_DISPBIOS_H__
 #define __NOUVEAU_DISPBIOS_H__
 
-#include "nvreg.h"
-
 #define DCB_MAX_NUM_ENTRIES 16
 #define DCB_MAX_NUM_I2C_ENTRIES 16
 #define DCB_MAX_NUM_GPIO_ENTRIES 32
diff --git a/drivers/gpu/drm/nouveau/nouveau_connector.c b/drivers/gpu/drm/nouveau/nouveau_connector.c
index 4dd7ae2ac6c6..4da776f344d7 100644
--- a/drivers/gpu/drm/nouveau/nouveau_connector.c
+++ b/drivers/gpu/drm/nouveau/nouveau_connector.c
@@ -32,7 +32,7 @@
 
 #include "nouveau_reg.h"
 #include "nouveau_drm.h"
-#include "nouveau_hw.h"
+#include "dispnv04/hw.h"
 #include "nouveau_acpi.h"
 
 #include "nouveau_display.h"
diff --git a/drivers/gpu/drm/nouveau/nouveau_display.c b/drivers/gpu/drm/nouveau/nouveau_display.c
index 4610c3a29bbe..7bf22d4a3d96 100644
--- a/drivers/gpu/drm/nouveau/nouveau_display.c
+++ b/drivers/gpu/drm/nouveau/nouveau_display.c
@@ -28,7 +28,7 @@
 #include <drm/drm_crtc_helper.h>
 
 #include "nouveau_fbcon.h"
-#include "nouveau_hw.h"
+#include "dispnv04/hw.h"
 #include "nouveau_crtc.h"
 #include "nouveau_dma.h"
 #include "nouveau_gem.h"
diff --git a/drivers/gpu/drm/nouveau/nouveau_drm.c b/drivers/gpu/drm/nouveau/nouveau_drm.c
index c95decf543e9..46c152ff0a80 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_drm.c
@@ -31,13 +31,13 @@
 #include <core/gpuobj.h>
 #include <core/class.h>
 
-#include <subdev/device.h>
-#include <subdev/vm.h>
-
+#include <engine/device.h>
 #include <engine/disp.h>
+#include <engine/fifo.h>
+
+#include <subdev/vm.h>
 
 #include "nouveau_drm.h"
-#include "nouveau_irq.h"
 #include "nouveau_dma.h"
 #include "nouveau_ttm.h"
 #include "nouveau_gem.h"
@@ -165,7 +165,7 @@ nouveau_accel_init(struct nouveau_drm *drm)
 	u32 arg0, arg1;
 	int ret;
 
-	if (nouveau_noaccel)
+	if (nouveau_noaccel || !nouveau_fifo(device) /*XXX*/)
 		return;
 
 	/* initialise synchronisation routines */
@@ -365,10 +365,6 @@ nouveau_drm_load(struct drm_device *dev, unsigned long flags)
 	if (ret)
 		goto fail_bios;
 
-	ret = nouveau_irq_init(dev);
-	if (ret)
-		goto fail_irq;
-
 	ret = nouveau_display_create(dev);
 	if (ret)
 		goto fail_dispctor;
@@ -388,8 +384,6 @@ nouveau_drm_load(struct drm_device *dev, unsigned long flags)
 fail_dispinit:
 	nouveau_display_destroy(dev);
 fail_dispctor:
-	nouveau_irq_fini(dev);
-fail_irq:
 	nouveau_bios_takedown(dev);
 fail_bios:
 	nouveau_ttm_fini(drm);
@@ -415,7 +409,6 @@ nouveau_drm_unload(struct drm_device *dev)
 		nouveau_display_fini(dev);
 	nouveau_display_destroy(dev);
 
-	nouveau_irq_fini(dev);
 	nouveau_bios_takedown(dev);
 
 	nouveau_ttm_fini(drm);
@@ -533,7 +526,6 @@ nouveau_do_resume(struct drm_device *dev)
 		nouveau_fence(drm)->resume(drm);
 
 	nouveau_run_vbios_init(dev);
-	nouveau_irq_postinstall(dev);
 	nouveau_pm_resume(dev);
 
 	if (dev->mode_config.num_crtc) {
@@ -669,8 +661,7 @@ static struct drm_driver
 driver = {
 	.driver_features =
 		DRIVER_USE_AGP | DRIVER_PCI_DMA | DRIVER_SG |
-		DRIVER_HAVE_IRQ | DRIVER_IRQ_SHARED | DRIVER_GEM |
-		DRIVER_MODESET | DRIVER_PRIME,
+		DRIVER_GEM | DRIVER_MODESET | DRIVER_PRIME,
 
 	.load = nouveau_drm_load,
 	.unload = nouveau_drm_unload,
@@ -684,11 +675,6 @@ driver = {
 	.debugfs_cleanup = nouveau_debugfs_takedown,
 #endif
 
-	.irq_preinstall = nouveau_irq_preinstall,
-	.irq_postinstall = nouveau_irq_postinstall,
-	.irq_uninstall = nouveau_irq_uninstall,
-	.irq_handler = nouveau_irq_handler,
-
 	.get_vblank_counter = drm_vblank_count,
 	.enable_vblank = nouveau_drm_vblank_enable,
 	.disable_vblank = nouveau_drm_vblank_disable,
diff --git a/drivers/gpu/drm/nouveau/nouveau_drm.h b/drivers/gpu/drm/nouveau/nouveau_drm.h
index 9c39bafbef2c..f2b30f89dee0 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drm.h
+++ b/drivers/gpu/drm/nouveau/nouveau_drm.h
@@ -10,7 +10,18 @@
 
 #define DRIVER_MAJOR		1
 #define DRIVER_MINOR		1
-#define DRIVER_PATCHLEVEL	0
+#define DRIVER_PATCHLEVEL	1
+
+/*
+ * 1.1.1:
+ * 	- added support for tiled system memory buffer objects
+ *      - added support for NOUVEAU_GETPARAM_GRAPH_UNITS on [nvc0,nve0].
+ *      - added support for compressed memory storage types on [nvc0,nve0].
+ *      - added support for software methods 0x600,0x644,0x6ac on nvc0
+ *        to control registers on the MPs to enable performance counters,
+ *        and to control the warp error enable mask (OpenGL requires out of
+ *        bounds access to local memory to be silently ignored / return 0).
+ */
 
 #include <core/client.h>
 #include <core/event.h>
diff --git a/drivers/gpu/drm/nouveau/nouveau_encoder.h b/drivers/gpu/drm/nouveau/nouveau_encoder.h
index e24341229d5e..24660c0f713d 100644
--- a/drivers/gpu/drm/nouveau/nouveau_encoder.h
+++ b/drivers/gpu/drm/nouveau/nouveau_encoder.h
@@ -30,7 +30,7 @@
 #include <subdev/bios/dcb.h>
 
 #include <drm/drm_encoder_slave.h>
-#include "nv04_display.h"
+#include "dispnv04/disp.h"
 
 #define NV_DPMS_CLEARED 0x80
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_irq.c b/drivers/gpu/drm/nouveau/nouveau_irq.c
deleted file mode 100644
index 1303680affd3..000000000000
--- a/drivers/gpu/drm/nouveau/nouveau_irq.c
+++ /dev/null
@@ -1,76 +0,0 @@
-/*
- * Copyright 2012 Red Hat Inc.
- *
- * Permission is hereby granted, free of charge, to any person obtaining a
- * copy of this software and associated documentation files (the "Software"),
- * to deal in the Software without restriction, including without limitation
- * the rights to use, copy, modify, merge, publish, distribute, sublicense,
- * and/or sell copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
- * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- *
- * Authors: Ben Skeggs
- */
-
-#include <subdev/mc.h>
-
-#include "nouveau_drm.h"
-#include "nouveau_irq.h"
-#include "nv50_display.h"
-
-void
-nouveau_irq_preinstall(struct drm_device *dev)
-{
-	nv_wr32(nouveau_dev(dev), 0x000140, 0x00000000);
-}
-
-int
-nouveau_irq_postinstall(struct drm_device *dev)
-{
-	nv_wr32(nouveau_dev(dev), 0x000140, 0x00000001);
-	return 0;
-}
-
-void
-nouveau_irq_uninstall(struct drm_device *dev)
-{
-	nv_wr32(nouveau_dev(dev), 0x000140, 0x00000000);
-}
-
-irqreturn_t
-nouveau_irq_handler(DRM_IRQ_ARGS)
-{
-	struct drm_device *dev = arg;
-	struct nouveau_device *device = nouveau_dev(dev);
-	struct nouveau_mc *pmc = nouveau_mc(device);
-	u32 stat;
-
-	stat = nv_rd32(device, 0x000100);
-	if (stat == 0 || stat == ~0)
-		return IRQ_NONE;
-
-	nv_subdev(pmc)->intr(nv_subdev(pmc));
-	return IRQ_HANDLED;
-}
-
-int
-nouveau_irq_init(struct drm_device *dev)
-{
-	return drm_irq_install(dev);
-}
-
-void
-nouveau_irq_fini(struct drm_device *dev)
-{
-	drm_irq_uninstall(dev);
-}
diff --git a/drivers/gpu/drm/nouveau/nouveau_irq.h b/drivers/gpu/drm/nouveau/nouveau_irq.h
deleted file mode 100644
index 06714ad857bb..000000000000
--- a/drivers/gpu/drm/nouveau/nouveau_irq.h
+++ /dev/null
@@ -1,11 +0,0 @@
-#ifndef __NOUVEAU_IRQ_H__
-#define __NOUVEAU_IRQ_H__
-
-extern int         nouveau_irq_init(struct drm_device *);
-extern void        nouveau_irq_fini(struct drm_device *);
-extern irqreturn_t nouveau_irq_handler(DRM_IRQ_ARGS);
-extern void        nouveau_irq_preinstall(struct drm_device *);
-extern int         nouveau_irq_postinstall(struct drm_device *);
-extern void        nouveau_irq_uninstall(struct drm_device *);
-
-#endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_ttm.c b/drivers/gpu/drm/nouveau/nouveau_ttm.c
index 9be9cb58e19b..f19a15a3bc03 100644
--- a/drivers/gpu/drm/nouveau/nouveau_ttm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_ttm.c
@@ -35,14 +35,16 @@
 static int
 nouveau_vram_manager_init(struct ttm_mem_type_manager *man, unsigned long psize)
 {
-	/* nothing to do */
+	struct nouveau_drm *drm = nouveau_bdev(man->bdev);
+	struct nouveau_fb *pfb = nouveau_fb(drm->device);
+	man->priv = pfb;
 	return 0;
 }
 
 static int
 nouveau_vram_manager_fini(struct ttm_mem_type_manager *man)
 {
-	/* nothing to do */
+	man->priv = NULL;
 	return 0;
 }
 
@@ -104,7 +106,8 @@ nouveau_vram_manager_new(struct ttm_mem_type_manager *man,
 static void
 nouveau_vram_manager_debug(struct ttm_mem_type_manager *man, const char *prefix)
 {
-	struct nouveau_mm *mm = man->priv;
+	struct nouveau_fb *pfb = man->priv;
+	struct nouveau_mm *mm = &pfb->vram;
 	struct nouveau_mm_node *r;
 	u32 total = 0, free = 0;
 
@@ -161,6 +164,8 @@ nouveau_gart_manager_new(struct ttm_mem_type_manager *man,
 			 struct ttm_placement *placement,
 			 struct ttm_mem_reg *mem)
 {
+	struct nouveau_drm *drm = nouveau_bdev(bo->bdev);
+	struct nouveau_bo *nvbo = nouveau_bo(bo);
 	struct nouveau_mem *node;
 
 	if (unlikely((mem->num_pages << PAGE_SHIFT) >= 512 * 1024 * 1024))
@@ -171,6 +176,20 @@ nouveau_gart_manager_new(struct ttm_mem_type_manager *man,
 		return -ENOMEM;
 	node->page_shift = 12;
 
+	switch (nv_device(drm->device)->card_type) {
+	case NV_50:
+		if (nv_device(drm->device)->chipset != 0x50)
+			node->memtype = (nvbo->tile_flags & 0x7f00) >> 8;
+		break;
+	case NV_C0:
+	case NV_D0:
+	case NV_E0:
+		node->memtype = (nvbo->tile_flags & 0xff00) >> 8;
+		break;
+	default:
+		break;
+	}
+
 	mem->mm_node = node;
 	mem->start   = 0;
 	return 0;
diff --git a/drivers/gpu/drm/nouveau/nv04_pm.c b/drivers/gpu/drm/nouveau/nv04_pm.c
index 2a0cc9d0614a..27afc0ea28b0 100644
--- a/drivers/gpu/drm/nouveau/nv04_pm.c
+++ b/drivers/gpu/drm/nouveau/nv04_pm.c
@@ -25,7 +25,7 @@
 #include <drm/drmP.h>
 #include "nouveau_drm.h"
 #include "nouveau_reg.h"
-#include "nouveau_hw.h"
+#include "dispnv04/hw.h"
 #include "nouveau_pm.h"
 
 #include <subdev/bios/pll.h>
diff --git a/drivers/gpu/drm/nouveau/nv40_pm.c b/drivers/gpu/drm/nouveau/nv40_pm.c
index 3382064c7f33..3af5bcd0b203 100644
--- a/drivers/gpu/drm/nouveau/nv40_pm.c
+++ b/drivers/gpu/drm/nouveau/nv40_pm.c
@@ -26,7 +26,7 @@
 #include "nouveau_drm.h"
 #include "nouveau_bios.h"
 #include "nouveau_pm.h"
-#include "nouveau_hw.h"
+#include "dispnv04/hw.h"
 
 #include <subdev/bios/pll.h>
 #include <subdev/clock.h>
diff --git a/drivers/gpu/drm/nouveau/nv50_display.c b/drivers/gpu/drm/nouveau/nv50_display.c
index 1ddc03e51bf4..ebf0a683305e 100644
--- a/drivers/gpu/drm/nouveau/nv50_display.c
+++ b/drivers/gpu/drm/nouveau/nv50_display.c
@@ -2174,6 +2174,7 @@ int
 nv50_display_create(struct drm_device *dev)
 {
 	static const u16 oclass[] = {
+		NVF0_DISP_CLASS,
 		NVE0_DISP_CLASS,
 		NVD0_DISP_CLASS,
 		NVA3_DISP_CLASS,
diff --git a/drivers/gpu/drm/nouveau/nv50_pm.c b/drivers/gpu/drm/nouveau/nv50_pm.c
index 8bd5d2781baf..69620e39c90c 100644
--- a/drivers/gpu/drm/nouveau/nv50_pm.c
+++ b/drivers/gpu/drm/nouveau/nv50_pm.c
@@ -25,7 +25,7 @@
 #include <drm/drmP.h>
 #include "nouveau_drm.h"
 #include "nouveau_bios.h"
-#include "nouveau_hw.h"
+#include "dispnv04/hw.h"
 #include "nouveau_pm.h"
 #include "nouveau_hwsq.h"
 
diff --git a/drivers/gpu/drm/omapdrm/omap_connector.c b/drivers/gpu/drm/omapdrm/omap_connector.c
index c451c41a7a7d..912759daf562 100644
--- a/drivers/gpu/drm/omapdrm/omap_connector.c
+++ b/drivers/gpu/drm/omapdrm/omap_connector.c
@@ -110,6 +110,11 @@ static enum drm_connector_status omap_connector_detect(
 			ret = connector_status_connected;
 		else
 			ret = connector_status_disconnected;
+	} else if (dssdev->type == OMAP_DISPLAY_TYPE_DPI ||
+			dssdev->type == OMAP_DISPLAY_TYPE_DBI ||
+			dssdev->type == OMAP_DISPLAY_TYPE_SDI ||
+			dssdev->type == OMAP_DISPLAY_TYPE_DSI) {
+		ret = connector_status_connected;
 	} else {
 		ret = connector_status_unknown;
 	}
@@ -189,12 +194,30 @@ static int omap_connector_mode_valid(struct drm_connector *connector,
 	struct omap_video_timings timings = {0};
 	struct drm_device *dev = connector->dev;
 	struct drm_display_mode *new_mode;
-	int ret = MODE_BAD;
+	int r, ret = MODE_BAD;
 
 	copy_timings_drm_to_omap(&timings, mode);
 	mode->vrefresh = drm_mode_vrefresh(mode);
 
-	if (!dssdrv->check_timings(dssdev, &timings)) {
+	/*
+	 * if the panel driver doesn't have a check_timings, it's most likely
+	 * a fixed resolution panel, check if the timings match with the
+	 * panel's timings
+	 */
+	if (dssdrv->check_timings) {
+		r = dssdrv->check_timings(dssdev, &timings);
+	} else {
+		struct omap_video_timings t = {0};
+
+		dssdrv->get_timings(dssdev, &t);
+
+		if (memcmp(&timings, &t, sizeof(struct omap_video_timings)))
+			r = -EINVAL;
+		else
+			r = 0;
+	}
+
+	if (!r) {
 		/* check if vrefresh is still valid */
 		new_mode = drm_mode_duplicate(dev, mode);
 		new_mode->clock = timings.pixel_clock;
diff --git a/drivers/gpu/drm/omapdrm/omap_crtc.c b/drivers/gpu/drm/omapdrm/omap_crtc.c
index bec66a490b8f..79b200aee18a 100644
--- a/drivers/gpu/drm/omapdrm/omap_crtc.c
+++ b/drivers/gpu/drm/omapdrm/omap_crtc.c
@@ -74,6 +74,13 @@ struct omap_crtc {
 	struct work_struct page_flip_work;
 };
 
+uint32_t pipe2vbl(struct drm_crtc *crtc)
+{
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+
+	return dispc_mgr_get_vsync_irq(omap_crtc->channel);
+}
+
 /*
  * Manager-ops, callbacks from output when they need to configure
  * the upstream part of the video pipe.
@@ -613,7 +620,13 @@ struct drm_crtc *omap_crtc_init(struct drm_device *dev,
 	omap_crtc->apply.pre_apply  = omap_crtc_pre_apply;
 	omap_crtc->apply.post_apply = omap_crtc_post_apply;
 
-	omap_crtc->apply_irq.irqmask = pipe2vbl(id);
+	omap_crtc->channel = channel;
+	omap_crtc->plane = plane;
+	omap_crtc->plane->crtc = crtc;
+	omap_crtc->name = channel_names[channel];
+	omap_crtc->pipe = id;
+
+	omap_crtc->apply_irq.irqmask = pipe2vbl(crtc);
 	omap_crtc->apply_irq.irq = omap_crtc_apply_irq;
 
 	omap_crtc->error_irq.irqmask =
@@ -621,12 +634,6 @@ struct drm_crtc *omap_crtc_init(struct drm_device *dev,
 	omap_crtc->error_irq.irq = omap_crtc_error_irq;
 	omap_irq_register(dev, &omap_crtc->error_irq);
 
-	omap_crtc->channel = channel;
-	omap_crtc->plane = plane;
-	omap_crtc->plane->crtc = crtc;
-	omap_crtc->name = channel_names[channel];
-	omap_crtc->pipe = id;
-
 	/* temporary: */
 	omap_crtc->mgr.id = channel;
 
diff --git a/drivers/gpu/drm/omapdrm/omap_drv.c b/drivers/gpu/drm/omapdrm/omap_drv.c
index 079c54c6f94c..9c53c25e5201 100644
--- a/drivers/gpu/drm/omapdrm/omap_drv.c
+++ b/drivers/gpu/drm/omapdrm/omap_drv.c
@@ -74,54 +74,53 @@ static int get_connector_type(struct omap_dss_device *dssdev)
 	}
 }
 
+static bool channel_used(struct drm_device *dev, enum omap_channel channel)
+{
+	struct omap_drm_private *priv = dev->dev_private;
+	int i;
+
+	for (i = 0; i < priv->num_crtcs; i++) {
+		struct drm_crtc *crtc = priv->crtcs[i];
+
+		if (omap_crtc_channel(crtc) == channel)
+			return true;
+	}
+
+	return false;
+}
+
 static int omap_modeset_init(struct drm_device *dev)
 {
 	struct omap_drm_private *priv = dev->dev_private;
 	struct omap_dss_device *dssdev = NULL;
 	int num_ovls = dss_feat_get_num_ovls();
-	int id;
+	int num_mgrs = dss_feat_get_num_mgrs();
+	int num_crtcs;
+	int i, id = 0;
 
 	drm_mode_config_init(dev);
 
 	omap_drm_irq_install(dev);
 
 	/*
-	 * Create private planes and CRTCs for the last NUM_CRTCs overlay
-	 * plus manager:
+	 * We usually don't want to create a CRTC for each manager, at least
+	 * not until we have a way to expose private planes to userspace.
+	 * Otherwise there would not be enough video pipes left for drm planes.
+	 * We use the num_crtc argument to limit the number of crtcs we create.
 	 */
-	for (id = 0; id < min(num_crtc, num_ovls); id++) {
-		struct drm_plane *plane;
-		struct drm_crtc *crtc;
-
-		plane = omap_plane_init(dev, id, true);
-		crtc = omap_crtc_init(dev, plane, pipe2chan(id), id);
+	num_crtcs = min3(num_crtc, num_mgrs, num_ovls);
 
-		BUG_ON(priv->num_crtcs >= ARRAY_SIZE(priv->crtcs));
-		priv->crtcs[id] = crtc;
-		priv->num_crtcs++;
-
-		priv->planes[id] = plane;
-		priv->num_planes++;
-	}
-
-	/*
-	 * Create normal planes for the remaining overlays:
-	 */
-	for (; id < num_ovls; id++) {
-		struct drm_plane *plane = omap_plane_init(dev, id, false);
-
-		BUG_ON(priv->num_planes >= ARRAY_SIZE(priv->planes));
-		priv->planes[priv->num_planes++] = plane;
-	}
+	dssdev = NULL;
 
 	for_each_dss_dev(dssdev) {
 		struct drm_connector *connector;
 		struct drm_encoder *encoder;
+		enum omap_channel channel;
 
 		if (!dssdev->driver) {
 			dev_warn(dev->dev, "%s has no driver.. skipping it\n",
 					dssdev->name);
-			return 0;
+			continue;
 		}
 
 		if (!(dssdev->driver->get_timings ||
@@ -129,7 +128,7 @@ static int omap_modeset_init(struct drm_device *dev)
 			dev_warn(dev->dev, "%s driver does not support "
 				"get_timings or read_edid.. skipping it!\n",
 				dssdev->name);
-			return 0;
+			continue;
 		}
 
 		encoder = omap_encoder_init(dev, dssdev);
@@ -157,16 +156,118 @@ static int omap_modeset_init(struct drm_device *dev)
 
 		drm_mode_connector_attach_encoder(connector, encoder);
 
+		/*
+		 * if we have reached the limit of the crtcs we are allowed to
+		 * create, let's not try to look for a crtc for this
+		 * panel/encoder and onwards, we will, of course, populate the
+		 * the possible_crtcs field for all the encoders with the final
+		 * set of crtcs we create
+		 */
+		if (id == num_crtcs)
+			continue;
+
+		/*
+		 * get the recommended DISPC channel for this encoder. For now,
+		 * we only try to get create a crtc out of the recommended, the
+		 * other possible channels to which the encoder can connect are
+		 * not considered.
+		 */
+		channel = dssdev->output->dispc_channel;
+
+		/*
+		 * if this channel hasn't already been taken by a previously
+		 * allocated crtc, we create a new crtc for it
+		 */
+		if (!channel_used(dev, channel)) {
+			struct drm_plane *plane;
+			struct drm_crtc *crtc;
+
+			plane = omap_plane_init(dev, id, true);
+			crtc = omap_crtc_init(dev, plane, channel, id);
+
+			BUG_ON(priv->num_crtcs >= ARRAY_SIZE(priv->crtcs));
+			priv->crtcs[id] = crtc;
+			priv->num_crtcs++;
+
+			priv->planes[id] = plane;
+			priv->num_planes++;
+
+			id++;
+		}
+	}
+
+	/*
+	 * we have allocated crtcs according to the need of the panels/encoders,
+	 * adding more crtcs here if needed
+	 */
+	for (; id < num_crtcs; id++) {
+
+		/* find a free manager for this crtc */
+		for (i = 0; i < num_mgrs; i++) {
+			if (!channel_used(dev, i)) {
+				struct drm_plane *plane;
+				struct drm_crtc *crtc;
+
+				plane = omap_plane_init(dev, id, true);
+				crtc = omap_crtc_init(dev, plane, i, id);
+
+				BUG_ON(priv->num_crtcs >=
+					ARRAY_SIZE(priv->crtcs));
+
+				priv->crtcs[id] = crtc;
+				priv->num_crtcs++;
+
+				priv->planes[id] = plane;
+				priv->num_planes++;
+
+				break;
+			} else {
+				continue;
+			}
+		}
+
+		if (i == num_mgrs) {
+			/* this shouldn't really happen */
+			dev_err(dev->dev, "no managers left for crtc\n");
+			return -ENOMEM;
+		}
+	}
+
+	/*
+	 * Create normal planes for the remaining overlays:
+	 */
+	for (; id < num_ovls; id++) {
+		struct drm_plane *plane = omap_plane_init(dev, id, false);
+
+		BUG_ON(priv->num_planes >= ARRAY_SIZE(priv->planes));
+		priv->planes[priv->num_planes++] = plane;
+	}
+
+	for (i = 0; i < priv->num_encoders; i++) {
+		struct drm_encoder *encoder = priv->encoders[i];
+		struct omap_dss_device *dssdev =
+					omap_encoder_get_dssdev(encoder);
+
 		/* figure out which crtc's we can connect the encoder to: */
 		encoder->possible_crtcs = 0;
 		for (id = 0; id < priv->num_crtcs; id++) {
-			enum omap_dss_output_id supported_outputs =
-					dss_feat_get_supported_outputs(pipe2chan(id));
+			struct drm_crtc *crtc = priv->crtcs[id];
+			enum omap_channel crtc_channel;
+			enum omap_dss_output_id supported_outputs;
+
+			crtc_channel = omap_crtc_channel(crtc);
+			supported_outputs =
+				dss_feat_get_supported_outputs(crtc_channel);
+
 			if (supported_outputs & dssdev->output->id)
 				encoder->possible_crtcs |= (1 << id);
 		}
 	}
 
+	DBG("registered %d planes, %d crtcs, %d encoders and %d connectors\n",
+		priv->num_planes, priv->num_crtcs, priv->num_encoders,
+		priv->num_connectors);
+
 	dev->mode_config.min_width = 32;
 	dev->mode_config.min_height = 32;
 
@@ -303,7 +404,7 @@ static int ioctl_gem_info(struct drm_device *dev, void *data,
 	return ret;
 }
 
-struct drm_ioctl_desc ioctls[DRM_COMMAND_END - DRM_COMMAND_BASE] = {
+static struct drm_ioctl_desc ioctls[DRM_COMMAND_END - DRM_COMMAND_BASE] = {
 	DRM_IOCTL_DEF_DRV(OMAP_GET_PARAM, ioctl_get_param, DRM_UNLOCKED|DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(OMAP_SET_PARAM, ioctl_set_param, DRM_UNLOCKED|DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
 	DRM_IOCTL_DEF_DRV(OMAP_GEM_NEW, ioctl_gem_new, DRM_UNLOCKED|DRM_AUTH),
@@ -567,7 +668,7 @@ static const struct dev_pm_ops omapdrm_pm_ops = {
 };
 #endif
 
-struct platform_driver pdev = {
+static struct platform_driver pdev = {
 		.driver = {
 			.name = DRIVER_NAME,
 			.owner = THIS_MODULE,
diff --git a/drivers/gpu/drm/omapdrm/omap_drv.h b/drivers/gpu/drm/omapdrm/omap_drv.h
index d4f997bb4ac0..215a20dd340c 100644
--- a/drivers/gpu/drm/omapdrm/omap_drv.h
+++ b/drivers/gpu/drm/omapdrm/omap_drv.h
@@ -139,8 +139,8 @@ void omap_gem_describe_objects(struct list_head *list, struct seq_file *m);
 int omap_gem_resume(struct device *dev);
 #endif
 
-int omap_irq_enable_vblank(struct drm_device *dev, int crtc);
-void omap_irq_disable_vblank(struct drm_device *dev, int crtc);
+int omap_irq_enable_vblank(struct drm_device *dev, int crtc_id);
+void omap_irq_disable_vblank(struct drm_device *dev, int crtc_id);
 irqreturn_t omap_irq_handler(DRM_IRQ_ARGS);
 void omap_irq_preinstall(struct drm_device *dev);
 int omap_irq_postinstall(struct drm_device *dev);
@@ -271,39 +271,9 @@ static inline int align_pitch(int pitch, int width, int bpp)
 	return ALIGN(pitch, 8 * bytespp);
 }
 
-static inline enum omap_channel pipe2chan(int pipe)
-{
-	int num_mgrs = dss_feat_get_num_mgrs();
-
-	/*
-	 * We usually don't want to create a CRTC for each manager,
-	 * at least not until we have a way to expose private planes
-	 * to userspace.  Otherwise there would not be enough video
-	 * pipes left for drm planes.  The higher #'d managers tend
-	 * to have more features so start in reverse order.
-	 */
-	return num_mgrs - pipe - 1;
-}
-
 /* map crtc to vblank mask */
-static inline uint32_t pipe2vbl(int crtc)
-{
-	enum omap_channel channel = pipe2chan(crtc);
-	return dispc_mgr_get_vsync_irq(channel);
-}
-
-static inline int crtc2pipe(struct drm_device *dev, struct drm_crtc *crtc)
-{
-	struct omap_drm_private *priv = dev->dev_private;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(priv->crtcs); i++)
-		if (priv->crtcs[i] == crtc)
-			return i;
-
-	BUG();  /* bogus CRTC ptr */
-	return -1;
-}
+uint32_t pipe2vbl(struct drm_crtc *crtc);
+struct omap_dss_device *omap_encoder_get_dssdev(struct drm_encoder *encoder);
 
 /* should these be made into common util helpers?
  */
diff --git a/drivers/gpu/drm/omapdrm/omap_encoder.c b/drivers/gpu/drm/omapdrm/omap_encoder.c
index 21d126d0317e..c29451ba65da 100644
--- a/drivers/gpu/drm/omapdrm/omap_encoder.c
+++ b/drivers/gpu/drm/omapdrm/omap_encoder.c
@@ -41,6 +41,13 @@ struct omap_encoder {
 	struct omap_dss_device *dssdev;
 };
 
+struct omap_dss_device *omap_encoder_get_dssdev(struct drm_encoder *encoder)
+{
+	struct omap_encoder *omap_encoder = to_omap_encoder(encoder);
+
+	return omap_encoder->dssdev;
+}
+
 static void omap_encoder_destroy(struct drm_encoder *encoder)
 {
 	struct omap_encoder *omap_encoder = to_omap_encoder(encoder);
@@ -128,13 +135,26 @@ int omap_encoder_update(struct drm_encoder *encoder,
 
 	dssdev->output->manager = mgr;
 
-	ret = dssdrv->check_timings(dssdev, timings);
+	if (dssdrv->check_timings) {
+		ret = dssdrv->check_timings(dssdev, timings);
+	} else {
+		struct omap_video_timings t = {0};
+
+		dssdrv->get_timings(dssdev, &t);
+
+		if (memcmp(timings, &t, sizeof(struct omap_video_timings)))
+			ret = -EINVAL;
+		else
+			ret = 0;
+	}
+
 	if (ret) {
 		dev_err(dev->dev, "could not set timings: %d\n", ret);
 		return ret;
 	}
 
-	dssdrv->set_timings(dssdev, timings);
+	if (dssdrv->set_timings)
+		dssdrv->set_timings(dssdev, timings);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/omapdrm/omap_gem_dmabuf.c b/drivers/gpu/drm/omapdrm/omap_gem_dmabuf.c
index ac74d1bc67bf..be7cd97a0db0 100644
--- a/drivers/gpu/drm/omapdrm/omap_gem_dmabuf.c
+++ b/drivers/gpu/drm/omapdrm/omap_gem_dmabuf.c
@@ -178,7 +178,7 @@ out_unlock:
 	return omap_gem_mmap_obj(obj, vma);
 }
 
-struct dma_buf_ops omap_dmabuf_ops = {
+static struct dma_buf_ops omap_dmabuf_ops = {
 		.map_dma_buf = omap_gem_map_dma_buf,
 		.unmap_dma_buf = omap_gem_unmap_dma_buf,
 		.release = omap_gem_dmabuf_release,
@@ -212,7 +212,6 @@ struct drm_gem_object *omap_gem_prime_import(struct drm_device *dev,
 			 * refcount on gem itself instead of f_count of dmabuf.
 			 */
 			drm_gem_object_reference(obj);
-			dma_buf_put(buffer);
 			return obj;
 		}
 	}
diff --git a/drivers/gpu/drm/omapdrm/omap_irq.c b/drivers/gpu/drm/omapdrm/omap_irq.c
index e01303ee00c3..9263db117ff8 100644
--- a/drivers/gpu/drm/omapdrm/omap_irq.c
+++ b/drivers/gpu/drm/omapdrm/omap_irq.c
@@ -130,12 +130,13 @@ int omap_irq_wait(struct drm_device *dev, struct omap_irq_wait *wait,
  * Zero on success, appropriate errno if the given @crtc's vblank
  * interrupt cannot be enabled.
  */
-int omap_irq_enable_vblank(struct drm_device *dev, int crtc)
+int omap_irq_enable_vblank(struct drm_device *dev, int crtc_id)
 {
 	struct omap_drm_private *priv = dev->dev_private;
+	struct drm_crtc *crtc = priv->crtcs[crtc_id];
 	unsigned long flags;
 
-	DBG("dev=%p, crtc=%d", dev, crtc);
+	DBG("dev=%p, crtc=%d", dev, crtc_id);
 
 	dispc_runtime_get();
 	spin_lock_irqsave(&list_lock, flags);
@@ -156,12 +157,13 @@ int omap_irq_enable_vblank(struct drm_device *dev, int crtc)
  * a hardware vblank counter, this routine should be a no-op, since
  * interrupts will have to stay on to keep the count accurate.
  */
-void omap_irq_disable_vblank(struct drm_device *dev, int crtc)
+void omap_irq_disable_vblank(struct drm_device *dev, int crtc_id)
 {
 	struct omap_drm_private *priv = dev->dev_private;
+	struct drm_crtc *crtc = priv->crtcs[crtc_id];
 	unsigned long flags;
 
-	DBG("dev=%p, crtc=%d", dev, crtc);
+	DBG("dev=%p, crtc=%d", dev, crtc_id);
 
 	dispc_runtime_get();
 	spin_lock_irqsave(&list_lock, flags);
@@ -186,9 +188,12 @@ irqreturn_t omap_irq_handler(DRM_IRQ_ARGS)
 
 	VERB("irqs: %08x", irqstatus);
 
-	for (id = 0; id < priv->num_crtcs; id++)
-		if (irqstatus & pipe2vbl(id))
+	for (id = 0; id < priv->num_crtcs; id++) {
+		struct drm_crtc *crtc = priv->crtcs[id];
+
+		if (irqstatus & pipe2vbl(crtc))
 			drm_handle_vblank(dev, id);
+	}
 
 	spin_lock_irqsave(&list_lock, flags);
 	list_for_each_entry_safe(handler, n, &priv->irq_list, node) {
diff --git a/drivers/gpu/drm/omapdrm/omap_plane.c b/drivers/gpu/drm/omapdrm/omap_plane.c
index 2882cda6ea19..8d225d7ff4e3 100644
--- a/drivers/gpu/drm/omapdrm/omap_plane.c
+++ b/drivers/gpu/drm/omapdrm/omap_plane.c
@@ -247,6 +247,12 @@ static int omap_plane_update(struct drm_plane *plane,
 {
 	struct omap_plane *omap_plane = to_omap_plane(plane);
 	omap_plane->enabled = true;
+
+	if (plane->fb)
+		drm_framebuffer_unreference(plane->fb);
+
+	drm_framebuffer_reference(fb);
+
 	return omap_plane_mode_set(plane, crtc, fb,
 			crtc_x, crtc_y, crtc_w, crtc_h,
 			src_x, src_y, src_w, src_h,
diff --git a/drivers/gpu/drm/qxl/Kconfig b/drivers/gpu/drm/qxl/Kconfig
new file mode 100644
index 000000000000..2f1a57e11140
--- /dev/null
+++ b/drivers/gpu/drm/qxl/Kconfig
@@ -0,0 +1,10 @@
+config DRM_QXL
+	tristate "QXL virtual GPU"
+	depends on DRM && PCI
+	select FB_SYS_FILLRECT
+	select FB_SYS_COPYAREA
+	select FB_SYS_IMAGEBLIT
+        select DRM_KMS_HELPER
+        select DRM_TTM
+	help
+		QXL virtual GPU for Spice virtualization desktop integration. Do not enable this driver unless your distro ships a corresponding X.org QXL driver that can handle kernel modesetting.
diff --git a/drivers/gpu/drm/qxl/Makefile b/drivers/gpu/drm/qxl/Makefile
new file mode 100644
index 000000000000..ea046ba691d2
--- /dev/null
+++ b/drivers/gpu/drm/qxl/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for the drm device driver.  This driver provides support for the
+# Direct Rendering Infrastructure (DRI) in XFree86 4.1.0 and higher.
+
+ccflags-y := -Iinclude/drm
+
+qxl-y := qxl_drv.o qxl_kms.o qxl_display.o qxl_ttm.o qxl_fb.o qxl_object.o qxl_gem.o qxl_cmd.o qxl_image.o qxl_draw.o qxl_debugfs.o qxl_irq.o qxl_dumb.o qxl_ioctl.o qxl_fence.o qxl_release.o
+
+obj-$(CONFIG_DRM_QXL)+= qxl.o
diff --git a/drivers/gpu/drm/qxl/qxl_cmd.c b/drivers/gpu/drm/qxl/qxl_cmd.c
new file mode 100644
index 000000000000..08b0823c93d5
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_cmd.c
@@ -0,0 +1,685 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Dave Airlie
+ *          Alon Levy
+ */
+
+/* QXL cmd/ring handling */
+
+#include "qxl_drv.h"
+#include "qxl_object.h"
+
+static int qxl_reap_surface_id(struct qxl_device *qdev, int max_to_reap);
+
+struct ring {
+	struct qxl_ring_header      header;
+	uint8_t                     elements[0];
+};
+
+struct qxl_ring {
+	struct ring	       *ring;
+	int			element_size;
+	int			n_elements;
+	int			prod_notify;
+	wait_queue_head_t      *push_event;
+	spinlock_t             lock;
+};
+
+void qxl_ring_free(struct qxl_ring *ring)
+{
+	kfree(ring);
+}
+
+struct qxl_ring *
+qxl_ring_create(struct qxl_ring_header *header,
+		int element_size,
+		int n_elements,
+		int prod_notify,
+		bool set_prod_notify,
+		wait_queue_head_t *push_event)
+{
+	struct qxl_ring *ring;
+
+	ring = kmalloc(sizeof(*ring), GFP_KERNEL);
+	if (!ring)
+		return NULL;
+
+	ring->ring = (struct ring *)header;
+	ring->element_size = element_size;
+	ring->n_elements = n_elements;
+	ring->prod_notify = prod_notify;
+	ring->push_event = push_event;
+	if (set_prod_notify)
+		header->notify_on_prod = ring->n_elements;
+	spin_lock_init(&ring->lock);
+	return ring;
+}
+
+static int qxl_check_header(struct qxl_ring *ring)
+{
+	int ret;
+	struct qxl_ring_header *header = &(ring->ring->header);
+	unsigned long flags;
+	spin_lock_irqsave(&ring->lock, flags);
+	ret = header->prod - header->cons < header->num_items;
+	if (ret == 0)
+		header->notify_on_cons = header->cons + 1;
+	spin_unlock_irqrestore(&ring->lock, flags);
+	return ret;
+}
+
+static int qxl_check_idle(struct qxl_ring *ring)
+{
+	int ret;
+	struct qxl_ring_header *header = &(ring->ring->header);
+	unsigned long flags;
+	spin_lock_irqsave(&ring->lock, flags);
+	ret = header->prod == header->cons;
+	spin_unlock_irqrestore(&ring->lock, flags);
+	return ret;
+}
+
+int qxl_ring_push(struct qxl_ring *ring,
+		  const void *new_elt, bool interruptible)
+{
+	struct qxl_ring_header *header = &(ring->ring->header);
+	uint8_t *elt;
+	int idx, ret;
+	unsigned long flags;
+	spin_lock_irqsave(&ring->lock, flags);
+	if (header->prod - header->cons == header->num_items) {
+		header->notify_on_cons = header->cons + 1;
+		mb();
+		spin_unlock_irqrestore(&ring->lock, flags);
+		if (!drm_can_sleep()) {
+			while (!qxl_check_header(ring))
+				udelay(1);
+		} else {
+			if (interruptible) {
+				ret = wait_event_interruptible(*ring->push_event,
+							       qxl_check_header(ring));
+				if (ret)
+					return ret;
+			} else {
+				wait_event(*ring->push_event,
+					   qxl_check_header(ring));
+			}
+
+		}
+		spin_lock_irqsave(&ring->lock, flags);
+	}
+
+	idx = header->prod & (ring->n_elements - 1);
+	elt = ring->ring->elements + idx * ring->element_size;
+
+	memcpy((void *)elt, new_elt, ring->element_size);
+
+	header->prod++;
+
+	mb();
+
+	if (header->prod == header->notify_on_prod)
+		outb(0, ring->prod_notify);
+
+	spin_unlock_irqrestore(&ring->lock, flags);
+	return 0;
+}
+
+static bool qxl_ring_pop(struct qxl_ring *ring,
+			 void *element)
+{
+	volatile struct qxl_ring_header *header = &(ring->ring->header);
+	volatile uint8_t *ring_elt;
+	int idx;
+	unsigned long flags;
+	spin_lock_irqsave(&ring->lock, flags);
+	if (header->cons == header->prod) {
+		header->notify_on_prod = header->cons + 1;
+		spin_unlock_irqrestore(&ring->lock, flags);
+		return false;
+	}
+
+	idx = header->cons & (ring->n_elements - 1);
+	ring_elt = ring->ring->elements + idx * ring->element_size;
+
+	memcpy(element, (void *)ring_elt, ring->element_size);
+
+	header->cons++;
+
+	spin_unlock_irqrestore(&ring->lock, flags);
+	return true;
+}
+
+int
+qxl_push_command_ring_release(struct qxl_device *qdev, struct qxl_release *release,
+			      uint32_t type, bool interruptible)
+{
+	struct qxl_command cmd;
+
+	cmd.type = type;
+	cmd.data = qxl_bo_physical_address(qdev, release->bos[0], release->release_offset);
+
+	return qxl_ring_push(qdev->command_ring, &cmd, interruptible);
+}
+
+int
+qxl_push_cursor_ring_release(struct qxl_device *qdev, struct qxl_release *release,
+			     uint32_t type, bool interruptible)
+{
+	struct qxl_command cmd;
+
+	cmd.type = type;
+	cmd.data = qxl_bo_physical_address(qdev, release->bos[0], release->release_offset);
+
+	return qxl_ring_push(qdev->cursor_ring, &cmd, interruptible);
+}
+
+bool qxl_queue_garbage_collect(struct qxl_device *qdev, bool flush)
+{
+	if (!qxl_check_idle(qdev->release_ring)) {
+		queue_work(qdev->gc_queue, &qdev->gc_work);
+		if (flush)
+			flush_work(&qdev->gc_work);
+		return true;
+	}
+	return false;
+}
+
+int qxl_garbage_collect(struct qxl_device *qdev)
+{
+	struct qxl_release *release;
+	uint64_t id, next_id;
+	int i = 0;
+	int ret;
+	union qxl_release_info *info;
+
+	while (qxl_ring_pop(qdev->release_ring, &id)) {
+		QXL_INFO(qdev, "popped %lld\n", id);
+		while (id) {
+			release = qxl_release_from_id_locked(qdev, id);
+			if (release == NULL)
+				break;
+
+			ret = qxl_release_reserve(qdev, release, false);
+			if (ret) {
+				qxl_io_log(qdev, "failed to reserve release on garbage collect %lld\n", id);
+				DRM_ERROR("failed to reserve release %lld\n", id);
+			}
+
+			info = qxl_release_map(qdev, release);
+			next_id = info->next;
+			qxl_release_unmap(qdev, release, info);
+
+			qxl_release_unreserve(qdev, release);
+			QXL_INFO(qdev, "popped %lld, next %lld\n", id,
+				next_id);
+
+			switch (release->type) {
+			case QXL_RELEASE_DRAWABLE:
+			case QXL_RELEASE_SURFACE_CMD:
+			case QXL_RELEASE_CURSOR_CMD:
+				break;
+			default:
+				DRM_ERROR("unexpected release type\n");
+				break;
+			}
+			id = next_id;
+
+			qxl_release_free(qdev, release);
+			++i;
+		}
+	}
+
+	QXL_INFO(qdev, "%s: %lld\n", __func__, i);
+
+	return i;
+}
+
+int qxl_alloc_bo_reserved(struct qxl_device *qdev, unsigned long size,
+			  struct qxl_bo **_bo)
+{
+	struct qxl_bo *bo;
+	int ret;
+
+	ret = qxl_bo_create(qdev, size, false /* not kernel - device */,
+			    QXL_GEM_DOMAIN_VRAM, NULL, &bo);
+	if (ret) {
+		DRM_ERROR("failed to allocate VRAM BO\n");
+		return ret;
+	}
+	ret = qxl_bo_reserve(bo, false);
+	if (unlikely(ret != 0))
+		goto out_unref;
+
+	*_bo = bo;
+	return 0;
+out_unref:
+	qxl_bo_unref(&bo);
+	return 0;
+}
+
+static int wait_for_io_cmd_user(struct qxl_device *qdev, uint8_t val, long port)
+{
+	int irq_num;
+	long addr = qdev->io_base + port;
+	int ret;
+
+	mutex_lock(&qdev->async_io_mutex);
+	irq_num = atomic_read(&qdev->irq_received_io_cmd);
+
+
+	if (qdev->last_sent_io_cmd > irq_num) {
+		ret = wait_event_interruptible(qdev->io_cmd_event,
+					       atomic_read(&qdev->irq_received_io_cmd) > irq_num);
+		if (ret)
+			goto out;
+		irq_num = atomic_read(&qdev->irq_received_io_cmd);
+	}
+	outb(val, addr);
+	qdev->last_sent_io_cmd = irq_num + 1;
+	ret = wait_event_interruptible(qdev->io_cmd_event,
+				       atomic_read(&qdev->irq_received_io_cmd) > irq_num);
+out:
+	mutex_unlock(&qdev->async_io_mutex);
+	return ret;
+}
+
+static void wait_for_io_cmd(struct qxl_device *qdev, uint8_t val, long port)
+{
+	int ret;
+
+restart:
+	ret = wait_for_io_cmd_user(qdev, val, port);
+	if (ret == -ERESTARTSYS)
+		goto restart;
+}
+
+int qxl_io_update_area(struct qxl_device *qdev, struct qxl_bo *surf,
+			const struct qxl_rect *area)
+{
+	int surface_id;
+	uint32_t surface_width, surface_height;
+	int ret;
+
+	if (!surf->hw_surf_alloc)
+		DRM_ERROR("got io update area with no hw surface\n");
+
+	if (surf->is_primary)
+		surface_id = 0;
+	else
+		surface_id = surf->surface_id;
+	surface_width = surf->surf.width;
+	surface_height = surf->surf.height;
+
+	if (area->left < 0 || area->top < 0 ||
+	    area->right > surface_width || area->bottom > surface_height) {
+		qxl_io_log(qdev, "%s: not doing area update for "
+			   "%d, (%d,%d,%d,%d) (%d,%d)\n", __func__, surface_id, area->left,
+			   area->top, area->right, area->bottom, surface_width, surface_height);
+		return -EINVAL;
+	}
+	mutex_lock(&qdev->update_area_mutex);
+	qdev->ram_header->update_area = *area;
+	qdev->ram_header->update_surface = surface_id;
+	ret = wait_for_io_cmd_user(qdev, 0, QXL_IO_UPDATE_AREA_ASYNC);
+	mutex_unlock(&qdev->update_area_mutex);
+	return ret;
+}
+
+void qxl_io_notify_oom(struct qxl_device *qdev)
+{
+	outb(0, qdev->io_base + QXL_IO_NOTIFY_OOM);
+}
+
+void qxl_io_flush_release(struct qxl_device *qdev)
+{
+	outb(0, qdev->io_base + QXL_IO_FLUSH_RELEASE);
+}
+
+void qxl_io_flush_surfaces(struct qxl_device *qdev)
+{
+	wait_for_io_cmd(qdev, 0, QXL_IO_FLUSH_SURFACES_ASYNC);
+}
+
+
+void qxl_io_destroy_primary(struct qxl_device *qdev)
+{
+	wait_for_io_cmd(qdev, 0, QXL_IO_DESTROY_PRIMARY_ASYNC);
+}
+
+void qxl_io_create_primary(struct qxl_device *qdev, unsigned width,
+			   unsigned height, unsigned offset, struct qxl_bo *bo)
+{
+	struct qxl_surface_create *create;
+
+	QXL_INFO(qdev, "%s: qdev %p, ram_header %p\n", __func__, qdev,
+		 qdev->ram_header);
+	create = &qdev->ram_header->create_surface;
+	create->format = bo->surf.format;
+	create->width = width;
+	create->height = height;
+	create->stride = bo->surf.stride;
+	create->mem = qxl_bo_physical_address(qdev, bo, offset);
+
+	QXL_INFO(qdev, "%s: mem = %llx, from %p\n", __func__, create->mem,
+		 bo->kptr);
+
+	create->flags = QXL_SURF_FLAG_KEEP_DATA;
+	create->type = QXL_SURF_TYPE_PRIMARY;
+
+	wait_for_io_cmd(qdev, 0, QXL_IO_CREATE_PRIMARY_ASYNC);
+}
+
+void qxl_io_memslot_add(struct qxl_device *qdev, uint8_t id)
+{
+	QXL_INFO(qdev, "qxl_memslot_add %d\n", id);
+	wait_for_io_cmd(qdev, id, QXL_IO_MEMSLOT_ADD_ASYNC);
+}
+
+void qxl_io_log(struct qxl_device *qdev, const char *fmt, ...)
+{
+	va_list args;
+
+	va_start(args, fmt);
+	vsnprintf(qdev->ram_header->log_buf, QXL_LOG_BUF_SIZE, fmt, args);
+	va_end(args);
+	/*
+	 * DO not do a DRM output here - this will call printk, which will
+	 * call back into qxl for rendering (qxl_fb)
+	 */
+	outb(0, qdev->io_base + QXL_IO_LOG);
+}
+
+void qxl_io_reset(struct qxl_device *qdev)
+{
+	outb(0, qdev->io_base + QXL_IO_RESET);
+}
+
+void qxl_io_monitors_config(struct qxl_device *qdev)
+{
+	qxl_io_log(qdev, "%s: %d [%dx%d+%d+%d]\n", __func__,
+		   qdev->monitors_config ?
+		   qdev->monitors_config->count : -1,
+		   qdev->monitors_config && qdev->monitors_config->count ?
+		   qdev->monitors_config->heads[0].width : -1,
+		   qdev->monitors_config && qdev->monitors_config->count ?
+		   qdev->monitors_config->heads[0].height : -1,
+		   qdev->monitors_config && qdev->monitors_config->count ?
+		   qdev->monitors_config->heads[0].x : -1,
+		   qdev->monitors_config && qdev->monitors_config->count ?
+		   qdev->monitors_config->heads[0].y : -1
+		   );
+
+	wait_for_io_cmd(qdev, 0, QXL_IO_MONITORS_CONFIG_ASYNC);
+}
+
+int qxl_surface_id_alloc(struct qxl_device *qdev,
+		      struct qxl_bo *surf)
+{
+	uint32_t handle;
+	int idr_ret;
+	int count = 0;
+again:
+	idr_preload(GFP_ATOMIC);
+	spin_lock(&qdev->surf_id_idr_lock);
+	idr_ret = idr_alloc(&qdev->surf_id_idr, NULL, 1, 0, GFP_NOWAIT);
+	spin_unlock(&qdev->surf_id_idr_lock);
+	idr_preload_end();
+	if (idr_ret < 0)
+		return idr_ret;
+	handle = idr_ret;
+
+	if (handle >= qdev->rom->n_surfaces) {
+		count++;
+		spin_lock(&qdev->surf_id_idr_lock);
+		idr_remove(&qdev->surf_id_idr, handle);
+		spin_unlock(&qdev->surf_id_idr_lock);
+		qxl_reap_surface_id(qdev, 2);
+		goto again;
+	}
+	surf->surface_id = handle;
+
+	spin_lock(&qdev->surf_id_idr_lock);
+	qdev->last_alloced_surf_id = handle;
+	spin_unlock(&qdev->surf_id_idr_lock);
+	return 0;
+}
+
+void qxl_surface_id_dealloc(struct qxl_device *qdev,
+			    uint32_t surface_id)
+{
+	spin_lock(&qdev->surf_id_idr_lock);
+	idr_remove(&qdev->surf_id_idr, surface_id);
+	spin_unlock(&qdev->surf_id_idr_lock);
+}
+
+int qxl_hw_surface_alloc(struct qxl_device *qdev,
+			 struct qxl_bo *surf,
+			 struct ttm_mem_reg *new_mem)
+{
+	struct qxl_surface_cmd *cmd;
+	struct qxl_release *release;
+	int ret;
+
+	if (surf->hw_surf_alloc)
+		return 0;
+
+	ret = qxl_alloc_surface_release_reserved(qdev, QXL_SURFACE_CMD_CREATE,
+						 NULL,
+						 &release);
+	if (ret)
+		return ret;
+
+	cmd = (struct qxl_surface_cmd *)qxl_release_map(qdev, release);
+	cmd->type = QXL_SURFACE_CMD_CREATE;
+	cmd->u.surface_create.format = surf->surf.format;
+	cmd->u.surface_create.width = surf->surf.width;
+	cmd->u.surface_create.height = surf->surf.height;
+	cmd->u.surface_create.stride = surf->surf.stride;
+	if (new_mem) {
+		int slot_id = surf->type == QXL_GEM_DOMAIN_VRAM ? qdev->main_mem_slot : qdev->surfaces_mem_slot;
+		struct qxl_memslot *slot = &(qdev->mem_slots[slot_id]);
+
+		/* TODO - need to hold one of the locks to read tbo.offset */
+		cmd->u.surface_create.data = slot->high_bits;
+
+		cmd->u.surface_create.data |= (new_mem->start << PAGE_SHIFT) + surf->tbo.bdev->man[new_mem->mem_type].gpu_offset;
+	} else
+		cmd->u.surface_create.data = qxl_bo_physical_address(qdev, surf, 0);
+	cmd->surface_id = surf->surface_id;
+	qxl_release_unmap(qdev, release, &cmd->release_info);
+
+	surf->surf_create = release;
+
+	/* no need to add a release to the fence for this bo,
+	   since it is only released when we ask to destroy the surface
+	   and it would never signal otherwise */
+	qxl_fence_releaseable(qdev, release);
+
+	qxl_push_command_ring_release(qdev, release, QXL_CMD_SURFACE, false);
+
+	qxl_release_unreserve(qdev, release);
+
+	surf->hw_surf_alloc = true;
+	spin_lock(&qdev->surf_id_idr_lock);
+	idr_replace(&qdev->surf_id_idr, surf, surf->surface_id);
+	spin_unlock(&qdev->surf_id_idr_lock);
+	return 0;
+}
+
+int qxl_hw_surface_dealloc(struct qxl_device *qdev,
+			   struct qxl_bo *surf)
+{
+	struct qxl_surface_cmd *cmd;
+	struct qxl_release *release;
+	int ret;
+	int id;
+
+	if (!surf->hw_surf_alloc)
+		return 0;
+
+	ret = qxl_alloc_surface_release_reserved(qdev, QXL_SURFACE_CMD_DESTROY,
+						 surf->surf_create,
+						 &release);
+	if (ret)
+		return ret;
+
+	surf->surf_create = NULL;
+	/* remove the surface from the idr, but not the surface id yet */
+	spin_lock(&qdev->surf_id_idr_lock);
+	idr_replace(&qdev->surf_id_idr, NULL, surf->surface_id);
+	spin_unlock(&qdev->surf_id_idr_lock);
+	surf->hw_surf_alloc = false;
+
+	id = surf->surface_id;
+	surf->surface_id = 0;
+
+	release->surface_release_id = id;
+	cmd = (struct qxl_surface_cmd *)qxl_release_map(qdev, release);
+	cmd->type = QXL_SURFACE_CMD_DESTROY;
+	cmd->surface_id = id;
+	qxl_release_unmap(qdev, release, &cmd->release_info);
+
+	qxl_fence_releaseable(qdev, release);
+
+	qxl_push_command_ring_release(qdev, release, QXL_CMD_SURFACE, false);
+
+	qxl_release_unreserve(qdev, release);
+
+
+	return 0;
+}
+
+int qxl_update_surface(struct qxl_device *qdev, struct qxl_bo *surf)
+{
+	struct qxl_rect rect;
+	int ret;
+
+	/* if we are evicting, we need to make sure the surface is up
+	   to date */
+	rect.left = 0;
+	rect.right = surf->surf.width;
+	rect.top = 0;
+	rect.bottom = surf->surf.height;
+retry:
+	ret = qxl_io_update_area(qdev, surf, &rect);
+	if (ret == -ERESTARTSYS)
+		goto retry;
+	return ret;
+}
+
+static void qxl_surface_evict_locked(struct qxl_device *qdev, struct qxl_bo *surf, bool do_update_area)
+{
+	/* no need to update area if we are just freeing the surface normally */
+	if (do_update_area)
+		qxl_update_surface(qdev, surf);
+
+	/* nuke the surface id at the hw */
+	qxl_hw_surface_dealloc(qdev, surf);
+}
+
+void qxl_surface_evict(struct qxl_device *qdev, struct qxl_bo *surf, bool do_update_area)
+{
+	mutex_lock(&qdev->surf_evict_mutex);
+	qxl_surface_evict_locked(qdev, surf, do_update_area);
+	mutex_unlock(&qdev->surf_evict_mutex);
+}
+
+static int qxl_reap_surf(struct qxl_device *qdev, struct qxl_bo *surf, bool stall)
+{
+	int ret;
+
+	ret = qxl_bo_reserve(surf, false);
+	if (ret == -EBUSY)
+		return -EBUSY;
+
+	if (surf->fence.num_active_releases > 0 && stall == false) {
+		qxl_bo_unreserve(surf);
+		return -EBUSY;
+	}
+
+	if (stall)
+		mutex_unlock(&qdev->surf_evict_mutex);
+
+	spin_lock(&surf->tbo.bdev->fence_lock);
+	ret = ttm_bo_wait(&surf->tbo, true, true, !stall);
+	spin_unlock(&surf->tbo.bdev->fence_lock);
+
+	if (stall)
+		mutex_lock(&qdev->surf_evict_mutex);
+	if (ret == -EBUSY) {
+		qxl_bo_unreserve(surf);
+		return -EBUSY;
+	}
+
+	qxl_surface_evict_locked(qdev, surf, true);
+	qxl_bo_unreserve(surf);
+	return 0;
+}
+
+static int qxl_reap_surface_id(struct qxl_device *qdev, int max_to_reap)
+{
+	int num_reaped = 0;
+	int i, ret;
+	bool stall = false;
+	int start = 0;
+
+	mutex_lock(&qdev->surf_evict_mutex);
+again:
+
+	spin_lock(&qdev->surf_id_idr_lock);
+	start = qdev->last_alloced_surf_id + 1;
+	spin_unlock(&qdev->surf_id_idr_lock);
+
+	for (i = start; i < start + qdev->rom->n_surfaces; i++) {
+		void *objptr;
+		int surfid = i % qdev->rom->n_surfaces;
+
+		/* this avoids the case where the objects is in the
+		   idr but has been evicted half way - its makes
+		   the idr lookup atomic with the eviction */
+		spin_lock(&qdev->surf_id_idr_lock);
+		objptr = idr_find(&qdev->surf_id_idr, surfid);
+		spin_unlock(&qdev->surf_id_idr_lock);
+
+		if (!objptr)
+			continue;
+
+		ret = qxl_reap_surf(qdev, objptr, stall);
+		if (ret == 0)
+			num_reaped++;
+		if (num_reaped >= max_to_reap)
+			break;
+	}
+	if (num_reaped == 0 && stall == false) {
+		stall = true;
+		goto again;
+	}
+
+	mutex_unlock(&qdev->surf_evict_mutex);
+	if (num_reaped) {
+		usleep_range(500, 1000);
+		qxl_queue_garbage_collect(qdev, true);
+	}
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/qxl/qxl_debugfs.c b/drivers/gpu/drm/qxl/qxl_debugfs.c
new file mode 100644
index 000000000000..c3c2bbdc6674
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_debugfs.c
@@ -0,0 +1,141 @@
+/*
+ * Copyright (C) 2009 Red Hat <bskeggs@redhat.com>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial
+ * portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE COPYRIGHT OWNER(S) AND/OR ITS SUPPLIERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+/*
+ * Authors:
+ *  Alon Levy <alevy@redhat.com>
+ */
+
+#include <linux/debugfs.h>
+
+#include "drmP.h"
+#include "qxl_drv.h"
+#include "qxl_object.h"
+
+
+#if defined(CONFIG_DEBUG_FS)
+static int
+qxl_debugfs_irq_received(struct seq_file *m, void *data)
+{
+	struct drm_info_node *node = (struct drm_info_node *) m->private;
+	struct qxl_device *qdev = node->minor->dev->dev_private;
+
+	seq_printf(m, "%d\n", atomic_read(&qdev->irq_received));
+	seq_printf(m, "%d\n", atomic_read(&qdev->irq_received_display));
+	seq_printf(m, "%d\n", atomic_read(&qdev->irq_received_cursor));
+	seq_printf(m, "%d\n", atomic_read(&qdev->irq_received_io_cmd));
+	seq_printf(m, "%d\n", qdev->irq_received_error);
+	return 0;
+}
+
+static int
+qxl_debugfs_buffers_info(struct seq_file *m, void *data)
+{
+	struct drm_info_node *node = (struct drm_info_node *) m->private;
+	struct qxl_device *qdev = node->minor->dev->dev_private;
+	struct qxl_bo *bo;
+
+	list_for_each_entry(bo, &qdev->gem.objects, list) {
+		seq_printf(m, "size %ld, pc %d, sync obj %p, num releases %d\n",
+			   (unsigned long)bo->gem_base.size, bo->pin_count,
+			   bo->tbo.sync_obj, bo->fence.num_active_releases);
+	}
+	return 0;
+}
+
+static struct drm_info_list qxl_debugfs_list[] = {
+	{ "irq_received", qxl_debugfs_irq_received, 0, NULL },
+	{ "qxl_buffers", qxl_debugfs_buffers_info, 0, NULL },
+};
+#define QXL_DEBUGFS_ENTRIES ARRAY_SIZE(qxl_debugfs_list)
+#endif
+
+int
+qxl_debugfs_init(struct drm_minor *minor)
+{
+#if defined(CONFIG_DEBUG_FS)
+	drm_debugfs_create_files(qxl_debugfs_list, QXL_DEBUGFS_ENTRIES,
+				 minor->debugfs_root, minor);
+#endif
+	return 0;
+}
+
+void
+qxl_debugfs_takedown(struct drm_minor *minor)
+{
+#if defined(CONFIG_DEBUG_FS)
+	drm_debugfs_remove_files(qxl_debugfs_list, QXL_DEBUGFS_ENTRIES,
+				 minor);
+#endif
+}
+
+int qxl_debugfs_add_files(struct qxl_device *qdev,
+			  struct drm_info_list *files,
+			  unsigned nfiles)
+{
+	unsigned i;
+
+	for (i = 0; i < qdev->debugfs_count; i++) {
+		if (qdev->debugfs[i].files == files) {
+			/* Already registered */
+			return 0;
+		}
+	}
+
+	i = qdev->debugfs_count + 1;
+	if (i > QXL_DEBUGFS_MAX_COMPONENTS) {
+		DRM_ERROR("Reached maximum number of debugfs components.\n");
+		DRM_ERROR("Report so we increase QXL_DEBUGFS_MAX_COMPONENTS.\n");
+		return -EINVAL;
+	}
+	qdev->debugfs[qdev->debugfs_count].files = files;
+	qdev->debugfs[qdev->debugfs_count].num_files = nfiles;
+	qdev->debugfs_count = i;
+#if defined(CONFIG_DEBUG_FS)
+	drm_debugfs_create_files(files, nfiles,
+				 qdev->ddev->control->debugfs_root,
+				 qdev->ddev->control);
+	drm_debugfs_create_files(files, nfiles,
+				 qdev->ddev->primary->debugfs_root,
+				 qdev->ddev->primary);
+#endif
+	return 0;
+}
+
+void qxl_debugfs_remove_files(struct qxl_device *qdev)
+{
+#if defined(CONFIG_DEBUG_FS)
+	unsigned i;
+
+	for (i = 0; i < qdev->debugfs_count; i++) {
+		drm_debugfs_remove_files(qdev->debugfs[i].files,
+					 qdev->debugfs[i].num_files,
+					 qdev->ddev->control);
+		drm_debugfs_remove_files(qdev->debugfs[i].files,
+					 qdev->debugfs[i].num_files,
+					 qdev->ddev->primary);
+	}
+#endif
+}
diff --git a/drivers/gpu/drm/qxl/qxl_dev.h b/drivers/gpu/drm/qxl/qxl_dev.h
new file mode 100644
index 000000000000..94c5aec71920
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_dev.h
@@ -0,0 +1,879 @@
+/*
+   Copyright (C) 2009 Red Hat, Inc.
+
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are
+   met:
+
+       * Redistributions of source code must retain the above copyright
+	 notice, this list of conditions and the following disclaimer.
+       * Redistributions in binary form must reproduce the above copyright
+	 notice, this list of conditions and the following disclaimer in
+	 the documentation and/or other materials provided with the
+	 distribution.
+       * Neither the name of the copyright holder nor the names of its
+	 contributors may be used to endorse or promote products derived
+	 from this software without specific prior written permission.
+
+   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDER AND CONTRIBUTORS "AS
+   IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+   TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+   PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+   HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#ifndef H_QXL_DEV
+#define H_QXL_DEV
+
+#include <linux/types.h>
+
+/*
+ * from spice-protocol
+ * Release 0.10.0
+ */
+
+/* enums.h */
+
+enum SpiceImageType {
+	SPICE_IMAGE_TYPE_BITMAP,
+	SPICE_IMAGE_TYPE_QUIC,
+	SPICE_IMAGE_TYPE_RESERVED,
+	SPICE_IMAGE_TYPE_LZ_PLT = 100,
+	SPICE_IMAGE_TYPE_LZ_RGB,
+	SPICE_IMAGE_TYPE_GLZ_RGB,
+	SPICE_IMAGE_TYPE_FROM_CACHE,
+	SPICE_IMAGE_TYPE_SURFACE,
+	SPICE_IMAGE_TYPE_JPEG,
+	SPICE_IMAGE_TYPE_FROM_CACHE_LOSSLESS,
+	SPICE_IMAGE_TYPE_ZLIB_GLZ_RGB,
+	SPICE_IMAGE_TYPE_JPEG_ALPHA,
+
+	SPICE_IMAGE_TYPE_ENUM_END
+};
+
+enum SpiceBitmapFmt {
+	SPICE_BITMAP_FMT_INVALID,
+	SPICE_BITMAP_FMT_1BIT_LE,
+	SPICE_BITMAP_FMT_1BIT_BE,
+	SPICE_BITMAP_FMT_4BIT_LE,
+	SPICE_BITMAP_FMT_4BIT_BE,
+	SPICE_BITMAP_FMT_8BIT,
+	SPICE_BITMAP_FMT_16BIT,
+	SPICE_BITMAP_FMT_24BIT,
+	SPICE_BITMAP_FMT_32BIT,
+	SPICE_BITMAP_FMT_RGBA,
+
+	SPICE_BITMAP_FMT_ENUM_END
+};
+
+enum SpiceSurfaceFmt {
+	SPICE_SURFACE_FMT_INVALID,
+	SPICE_SURFACE_FMT_1_A,
+	SPICE_SURFACE_FMT_8_A = 8,
+	SPICE_SURFACE_FMT_16_555 = 16,
+	SPICE_SURFACE_FMT_32_xRGB = 32,
+	SPICE_SURFACE_FMT_16_565 = 80,
+	SPICE_SURFACE_FMT_32_ARGB = 96,
+
+	SPICE_SURFACE_FMT_ENUM_END
+};
+
+enum SpiceClipType {
+	SPICE_CLIP_TYPE_NONE,
+	SPICE_CLIP_TYPE_RECTS,
+
+	SPICE_CLIP_TYPE_ENUM_END
+};
+
+enum SpiceRopd {
+	SPICE_ROPD_INVERS_SRC = (1 << 0),
+	SPICE_ROPD_INVERS_BRUSH = (1 << 1),
+	SPICE_ROPD_INVERS_DEST = (1 << 2),
+	SPICE_ROPD_OP_PUT = (1 << 3),
+	SPICE_ROPD_OP_OR = (1 << 4),
+	SPICE_ROPD_OP_AND = (1 << 5),
+	SPICE_ROPD_OP_XOR = (1 << 6),
+	SPICE_ROPD_OP_BLACKNESS = (1 << 7),
+	SPICE_ROPD_OP_WHITENESS = (1 << 8),
+	SPICE_ROPD_OP_INVERS = (1 << 9),
+	SPICE_ROPD_INVERS_RES = (1 << 10),
+
+	SPICE_ROPD_MASK = 0x7ff
+};
+
+enum SpiceBrushType {
+	SPICE_BRUSH_TYPE_NONE,
+	SPICE_BRUSH_TYPE_SOLID,
+	SPICE_BRUSH_TYPE_PATTERN,
+
+	SPICE_BRUSH_TYPE_ENUM_END
+};
+
+enum SpiceCursorType {
+	SPICE_CURSOR_TYPE_ALPHA,
+	SPICE_CURSOR_TYPE_MONO,
+	SPICE_CURSOR_TYPE_COLOR4,
+	SPICE_CURSOR_TYPE_COLOR8,
+	SPICE_CURSOR_TYPE_COLOR16,
+	SPICE_CURSOR_TYPE_COLOR24,
+	SPICE_CURSOR_TYPE_COLOR32,
+
+	SPICE_CURSOR_TYPE_ENUM_END
+};
+
+/* qxl_dev.h */
+
+#pragma pack(push, 1)
+
+#define REDHAT_PCI_VENDOR_ID 0x1b36
+
+/* 0x100-0x11f reserved for spice, 0x1ff used for unstable work */
+#define QXL_DEVICE_ID_STABLE 0x0100
+
+enum {
+	QXL_REVISION_STABLE_V04 = 0x01,
+	QXL_REVISION_STABLE_V06 = 0x02,
+	QXL_REVISION_STABLE_V10 = 0x03,
+	QXL_REVISION_STABLE_V12 = 0x04,
+};
+
+#define QXL_DEVICE_ID_DEVEL 0x01ff
+#define QXL_REVISION_DEVEL 0x01
+
+#define QXL_ROM_MAGIC (*(uint32_t *)"QXRO")
+#define QXL_RAM_MAGIC (*(uint32_t *)"QXRA")
+
+enum {
+	QXL_RAM_RANGE_INDEX,
+	QXL_VRAM_RANGE_INDEX,
+	QXL_ROM_RANGE_INDEX,
+	QXL_IO_RANGE_INDEX,
+
+	QXL_PCI_RANGES
+};
+
+/* qxl-1 compat: append only */
+enum {
+	QXL_IO_NOTIFY_CMD,
+	QXL_IO_NOTIFY_CURSOR,
+	QXL_IO_UPDATE_AREA,
+	QXL_IO_UPDATE_IRQ,
+	QXL_IO_NOTIFY_OOM,
+	QXL_IO_RESET,
+	QXL_IO_SET_MODE,                  /* qxl-1 */
+	QXL_IO_LOG,
+	/* appended for qxl-2 */
+	QXL_IO_MEMSLOT_ADD,
+	QXL_IO_MEMSLOT_DEL,
+	QXL_IO_DETACH_PRIMARY,
+	QXL_IO_ATTACH_PRIMARY,
+	QXL_IO_CREATE_PRIMARY,
+	QXL_IO_DESTROY_PRIMARY,
+	QXL_IO_DESTROY_SURFACE_WAIT,
+	QXL_IO_DESTROY_ALL_SURFACES,
+	/* appended for qxl-3 */
+	QXL_IO_UPDATE_AREA_ASYNC,
+	QXL_IO_MEMSLOT_ADD_ASYNC,
+	QXL_IO_CREATE_PRIMARY_ASYNC,
+	QXL_IO_DESTROY_PRIMARY_ASYNC,
+	QXL_IO_DESTROY_SURFACE_ASYNC,
+	QXL_IO_DESTROY_ALL_SURFACES_ASYNC,
+	QXL_IO_FLUSH_SURFACES_ASYNC,
+	QXL_IO_FLUSH_RELEASE,
+	/* appended for qxl-4 */
+	QXL_IO_MONITORS_CONFIG_ASYNC,
+
+	QXL_IO_RANGE_SIZE
+};
+
+typedef uint64_t QXLPHYSICAL;
+typedef int32_t QXLFIXED; /* fixed 28.4 */
+
+struct qxl_point_fix {
+	QXLFIXED x;
+	QXLFIXED y;
+};
+
+struct qxl_point {
+	int32_t x;
+	int32_t y;
+};
+
+struct qxl_point_1_6 {
+	int16_t x;
+	int16_t y;
+};
+
+struct qxl_rect {
+	int32_t top;
+	int32_t left;
+	int32_t bottom;
+	int32_t right;
+};
+
+struct qxl_urect {
+	uint32_t top;
+	uint32_t left;
+	uint32_t bottom;
+	uint32_t right;
+};
+
+/* qxl-1 compat: append only */
+struct qxl_rom {
+	uint32_t magic;
+	uint32_t id;
+	uint32_t update_id;
+	uint32_t compression_level;
+	uint32_t log_level;
+	uint32_t mode;			  /* qxl-1 */
+	uint32_t modes_offset;
+	uint32_t num_io_pages;
+	uint32_t pages_offset;		  /* qxl-1 */
+	uint32_t draw_area_offset;	  /* qxl-1 */
+	uint32_t surface0_area_size;	  /* qxl-1 name: draw_area_size */
+	uint32_t ram_header_offset;
+	uint32_t mm_clock;
+	/* appended for qxl-2 */
+	uint32_t n_surfaces;
+	uint64_t flags;
+	uint8_t slots_start;
+	uint8_t slots_end;
+	uint8_t slot_gen_bits;
+	uint8_t slot_id_bits;
+	uint8_t slot_generation;
+	/* appended for qxl-4 */
+	uint8_t client_present;
+	uint8_t client_capabilities[58];
+	uint32_t client_monitors_config_crc;
+	struct {
+		uint16_t count;
+	uint16_t padding;
+		struct qxl_urect heads[64];
+	} client_monitors_config;
+};
+
+/* qxl-1 compat: fixed */
+struct qxl_mode {
+	uint32_t id;
+	uint32_t x_res;
+	uint32_t y_res;
+	uint32_t bits;
+	uint32_t stride;
+	uint32_t x_mili;
+	uint32_t y_mili;
+	uint32_t orientation;
+};
+
+/* qxl-1 compat: fixed */
+struct qxl_modes {
+	uint32_t n_modes;
+	struct qxl_mode modes[0];
+};
+
+/* qxl-1 compat: append only */
+enum qxl_cmd_type {
+	QXL_CMD_NOP,
+	QXL_CMD_DRAW,
+	QXL_CMD_UPDATE,
+	QXL_CMD_CURSOR,
+	QXL_CMD_MESSAGE,
+	QXL_CMD_SURFACE,
+};
+
+/* qxl-1 compat: fixed */
+struct qxl_command {
+	QXLPHYSICAL data;
+	uint32_t type;
+	uint32_t padding;
+};
+
+#define QXL_COMMAND_FLAG_COMPAT		(1<<0)
+#define QXL_COMMAND_FLAG_COMPAT_16BPP	(2<<0)
+
+struct qxl_command_ext {
+	struct qxl_command cmd;
+	uint32_t group_id;
+	uint32_t flags;
+};
+
+struct qxl_mem_slot {
+	uint64_t mem_start;
+	uint64_t mem_end;
+};
+
+#define QXL_SURF_TYPE_PRIMARY	   0
+
+#define QXL_SURF_FLAG_KEEP_DATA	   (1 << 0)
+
+struct qxl_surface_create {
+	uint32_t width;
+	uint32_t height;
+	int32_t stride;
+	uint32_t format;
+	uint32_t position;
+	uint32_t mouse_mode;
+	uint32_t flags;
+	uint32_t type;
+	QXLPHYSICAL mem;
+};
+
+#define QXL_COMMAND_RING_SIZE 32
+#define QXL_CURSOR_RING_SIZE 32
+#define QXL_RELEASE_RING_SIZE 8
+
+#define QXL_LOG_BUF_SIZE 4096
+
+#define QXL_INTERRUPT_DISPLAY (1 << 0)
+#define QXL_INTERRUPT_CURSOR (1 << 1)
+#define QXL_INTERRUPT_IO_CMD (1 << 2)
+#define QXL_INTERRUPT_ERROR  (1 << 3)
+#define QXL_INTERRUPT_CLIENT (1 << 4)
+#define QXL_INTERRUPT_CLIENT_MONITORS_CONFIG  (1 << 5)
+
+struct qxl_ring_header {
+	uint32_t num_items;
+	uint32_t prod;
+	uint32_t notify_on_prod;
+	uint32_t cons;
+	uint32_t notify_on_cons;
+};
+
+/* qxl-1 compat: append only */
+struct qxl_ram_header {
+	uint32_t magic;
+	uint32_t int_pending;
+	uint32_t int_mask;
+	uint8_t log_buf[QXL_LOG_BUF_SIZE];
+	struct qxl_ring_header  cmd_ring_hdr;
+	struct qxl_command	cmd_ring[QXL_COMMAND_RING_SIZE];
+	struct qxl_ring_header  cursor_ring_hdr;
+	struct qxl_command	cursor_ring[QXL_CURSOR_RING_SIZE];
+	struct qxl_ring_header  release_ring_hdr;
+	uint64_t		release_ring[QXL_RELEASE_RING_SIZE];
+	struct qxl_rect update_area;
+	/* appended for qxl-2 */
+	uint32_t update_surface;
+	struct qxl_mem_slot mem_slot;
+	struct qxl_surface_create create_surface;
+	uint64_t flags;
+
+	/* appended for qxl-4 */
+
+	/* used by QXL_IO_MONITORS_CONFIG_ASYNC */
+	QXLPHYSICAL monitors_config;
+	uint8_t guest_capabilities[64];
+};
+
+union qxl_release_info {
+	uint64_t id;	  /* in  */
+	uint64_t next;	  /* out */
+};
+
+struct qxl_release_info_ext {
+	union qxl_release_info *info;
+	uint32_t group_id;
+};
+
+struct qxl_data_chunk {
+	uint32_t data_size;
+	QXLPHYSICAL prev_chunk;
+	QXLPHYSICAL next_chunk;
+	uint8_t data[0];
+};
+
+struct qxl_message {
+	union qxl_release_info release_info;
+	uint8_t data[0];
+};
+
+struct qxl_compat_update_cmd {
+	union qxl_release_info release_info;
+	struct qxl_rect area;
+	uint32_t update_id;
+};
+
+struct qxl_update_cmd {
+	union qxl_release_info release_info;
+	struct qxl_rect area;
+	uint32_t update_id;
+	uint32_t surface_id;
+};
+
+struct qxl_cursor_header {
+	uint64_t unique;
+	uint16_t type;
+	uint16_t width;
+	uint16_t height;
+	uint16_t hot_spot_x;
+	uint16_t hot_spot_y;
+};
+
+struct qxl_cursor {
+	struct qxl_cursor_header header;
+	uint32_t data_size;
+	struct qxl_data_chunk chunk;
+};
+
+enum {
+	QXL_CURSOR_SET,
+	QXL_CURSOR_MOVE,
+	QXL_CURSOR_HIDE,
+	QXL_CURSOR_TRAIL,
+};
+
+#define QXL_CURSOR_DEVICE_DATA_SIZE 128
+
+struct qxl_cursor_cmd {
+	union qxl_release_info release_info;
+	uint8_t type;
+	union {
+		struct {
+			struct qxl_point_1_6 position;
+			uint8_t visible;
+			QXLPHYSICAL shape;
+		} set;
+		struct {
+			uint16_t length;
+			uint16_t frequency;
+		} trail;
+		struct qxl_point_1_6 position;
+	} u;
+	/* todo: dynamic size from rom */
+	uint8_t device_data[QXL_CURSOR_DEVICE_DATA_SIZE];
+};
+
+enum {
+	QXL_DRAW_NOP,
+	QXL_DRAW_FILL,
+	QXL_DRAW_OPAQUE,
+	QXL_DRAW_COPY,
+	QXL_COPY_BITS,
+	QXL_DRAW_BLEND,
+	QXL_DRAW_BLACKNESS,
+	QXL_DRAW_WHITENESS,
+	QXL_DRAW_INVERS,
+	QXL_DRAW_ROP3,
+	QXL_DRAW_STROKE,
+	QXL_DRAW_TEXT,
+	QXL_DRAW_TRANSPARENT,
+	QXL_DRAW_ALPHA_BLEND,
+	QXL_DRAW_COMPOSITE
+};
+
+struct qxl_raster_glyph {
+	struct qxl_point render_pos;
+	struct qxl_point glyph_origin;
+	uint16_t width;
+	uint16_t height;
+	uint8_t data[0];
+};
+
+struct qxl_string {
+	uint32_t data_size;
+	uint16_t length;
+	uint16_t flags;
+	struct qxl_data_chunk chunk;
+};
+
+struct qxl_copy_bits {
+	struct qxl_point src_pos;
+};
+
+enum qxl_effect_type {
+	QXL_EFFECT_BLEND = 0,
+	QXL_EFFECT_OPAQUE = 1,
+	QXL_EFFECT_REVERT_ON_DUP = 2,
+	QXL_EFFECT_BLACKNESS_ON_DUP = 3,
+	QXL_EFFECT_WHITENESS_ON_DUP = 4,
+	QXL_EFFECT_NOP_ON_DUP = 5,
+	QXL_EFFECT_NOP = 6,
+	QXL_EFFECT_OPAQUE_BRUSH = 7
+};
+
+struct qxl_pattern {
+	QXLPHYSICAL pat;
+	struct qxl_point pos;
+};
+
+struct qxl_brush {
+	uint32_t type;
+	union {
+		uint32_t color;
+		struct qxl_pattern pattern;
+	} u;
+};
+
+struct qxl_q_mask {
+	uint8_t flags;
+	struct qxl_point pos;
+	QXLPHYSICAL bitmap;
+};
+
+struct qxl_fill {
+	struct qxl_brush brush;
+	uint16_t rop_descriptor;
+	struct qxl_q_mask mask;
+};
+
+struct qxl_opaque {
+	QXLPHYSICAL src_bitmap;
+	struct qxl_rect src_area;
+	struct qxl_brush brush;
+	uint16_t rop_descriptor;
+	uint8_t scale_mode;
+	struct qxl_q_mask mask;
+};
+
+struct qxl_copy {
+	QXLPHYSICAL src_bitmap;
+	struct qxl_rect src_area;
+	uint16_t rop_descriptor;
+	uint8_t scale_mode;
+	struct qxl_q_mask mask;
+};
+
+struct qxl_transparent {
+	QXLPHYSICAL src_bitmap;
+	struct qxl_rect src_area;
+	uint32_t src_color;
+	uint32_t true_color;
+};
+
+struct qxl_alpha_blend {
+	uint16_t alpha_flags;
+	uint8_t alpha;
+	QXLPHYSICAL src_bitmap;
+	struct qxl_rect src_area;
+};
+
+struct qxl_compat_alpha_blend {
+	uint8_t alpha;
+	QXLPHYSICAL src_bitmap;
+	struct qxl_rect src_area;
+};
+
+struct qxl_rop_3 {
+	QXLPHYSICAL src_bitmap;
+	struct qxl_rect src_area;
+	struct qxl_brush brush;
+	uint8_t rop3;
+	uint8_t scale_mode;
+	struct qxl_q_mask mask;
+};
+
+struct qxl_line_attr {
+	uint8_t flags;
+	uint8_t join_style;
+	uint8_t end_style;
+	uint8_t style_nseg;
+	QXLFIXED width;
+	QXLFIXED miter_limit;
+	QXLPHYSICAL style;
+};
+
+struct qxl_stroke {
+	QXLPHYSICAL path;
+	struct qxl_line_attr attr;
+	struct qxl_brush brush;
+	uint16_t fore_mode;
+	uint16_t back_mode;
+};
+
+struct qxl_text {
+	QXLPHYSICAL str;
+	struct qxl_rect back_area;
+	struct qxl_brush fore_brush;
+	struct qxl_brush back_brush;
+	uint16_t fore_mode;
+	uint16_t back_mode;
+};
+
+struct qxl_mask {
+	struct qxl_q_mask mask;
+};
+
+struct qxl_clip {
+	uint32_t type;
+	QXLPHYSICAL data;
+};
+
+enum qxl_operator {
+	QXL_OP_CLEAR			 = 0x00,
+	QXL_OP_SOURCE			 = 0x01,
+	QXL_OP_DST			 = 0x02,
+	QXL_OP_OVER			 = 0x03,
+	QXL_OP_OVER_REVERSE		 = 0x04,
+	QXL_OP_IN			 = 0x05,
+	QXL_OP_IN_REVERSE		 = 0x06,
+	QXL_OP_OUT			 = 0x07,
+	QXL_OP_OUT_REVERSE		 = 0x08,
+	QXL_OP_ATOP			 = 0x09,
+	QXL_OP_ATOP_REVERSE		 = 0x0a,
+	QXL_OP_XOR			 = 0x0b,
+	QXL_OP_ADD			 = 0x0c,
+	QXL_OP_SATURATE			 = 0x0d,
+	/* Note the jump here from 0x0d to 0x30 */
+	QXL_OP_MULTIPLY			 = 0x30,
+	QXL_OP_SCREEN			 = 0x31,
+	QXL_OP_OVERLAY			 = 0x32,
+	QXL_OP_DARKEN			 = 0x33,
+	QXL_OP_LIGHTEN			 = 0x34,
+	QXL_OP_COLOR_DODGE		 = 0x35,
+	QXL_OP_COLOR_BURN		 = 0x36,
+	QXL_OP_HARD_LIGHT		 = 0x37,
+	QXL_OP_SOFT_LIGHT		 = 0x38,
+	QXL_OP_DIFFERENCE		 = 0x39,
+	QXL_OP_EXCLUSION		 = 0x3a,
+	QXL_OP_HSL_HUE			 = 0x3b,
+	QXL_OP_HSL_SATURATION		 = 0x3c,
+	QXL_OP_HSL_COLOR		 = 0x3d,
+	QXL_OP_HSL_LUMINOSITY		 = 0x3e
+};
+
+struct qxl_transform {
+	uint32_t	t00;
+	uint32_t	t01;
+	uint32_t	t02;
+	uint32_t	t10;
+	uint32_t	t11;
+	uint32_t	t12;
+};
+
+/* The flags field has the following bit fields:
+ *
+ *     operator:		[  0 -  7 ]
+ *     src_filter:		[  8 - 10 ]
+ *     mask_filter:		[ 11 - 13 ]
+ *     src_repeat:		[ 14 - 15 ]
+ *     mask_repeat:		[ 16 - 17 ]
+ *     component_alpha:		[ 18 - 18 ]
+ *     reserved:		[ 19 - 31 ]
+ *
+ * The repeat and filter values are those of pixman:
+ *		REPEAT_NONE =		0
+ *              REPEAT_NORMAL =		1
+ *		REPEAT_PAD =		2
+ *		REPEAT_REFLECT =	3
+ *
+ * The filter values are:
+ *		FILTER_NEAREST =	0
+ *		FILTER_BILINEAR	=	1
+ */
+struct qxl_composite {
+	uint32_t		flags;
+
+	QXLPHYSICAL			src;
+	QXLPHYSICAL			src_transform;	/* May be NULL */
+	QXLPHYSICAL			mask;		/* May be NULL */
+	QXLPHYSICAL			mask_transform;	/* May be NULL */
+	struct qxl_point_1_6	src_origin;
+	struct qxl_point_1_6	mask_origin;
+};
+
+struct qxl_compat_drawable {
+	union qxl_release_info release_info;
+	uint8_t effect;
+	uint8_t type;
+	uint16_t bitmap_offset;
+	struct qxl_rect bitmap_area;
+	struct qxl_rect bbox;
+	struct qxl_clip clip;
+	uint32_t mm_time;
+	union {
+		struct qxl_fill fill;
+		struct qxl_opaque opaque;
+		struct qxl_copy copy;
+		struct qxl_transparent transparent;
+		struct qxl_compat_alpha_blend alpha_blend;
+		struct qxl_copy_bits copy_bits;
+		struct qxl_copy blend;
+		struct qxl_rop_3 rop3;
+		struct qxl_stroke stroke;
+		struct qxl_text text;
+		struct qxl_mask blackness;
+		struct qxl_mask invers;
+		struct qxl_mask whiteness;
+	} u;
+};
+
+struct qxl_drawable {
+	union qxl_release_info release_info;
+	uint32_t surface_id;
+	uint8_t effect;
+	uint8_t type;
+	uint8_t self_bitmap;
+	struct qxl_rect self_bitmap_area;
+	struct qxl_rect bbox;
+	struct qxl_clip clip;
+	uint32_t mm_time;
+	int32_t surfaces_dest[3];
+	struct qxl_rect surfaces_rects[3];
+	union {
+		struct qxl_fill fill;
+		struct qxl_opaque opaque;
+		struct qxl_copy copy;
+		struct qxl_transparent transparent;
+		struct qxl_alpha_blend alpha_blend;
+		struct qxl_copy_bits copy_bits;
+		struct qxl_copy blend;
+		struct qxl_rop_3 rop3;
+		struct qxl_stroke stroke;
+		struct qxl_text text;
+		struct qxl_mask blackness;
+		struct qxl_mask invers;
+		struct qxl_mask whiteness;
+		struct qxl_composite composite;
+	} u;
+};
+
+enum qxl_surface_cmd_type {
+	QXL_SURFACE_CMD_CREATE,
+	QXL_SURFACE_CMD_DESTROY,
+};
+
+struct qxl_surface {
+	uint32_t format;
+	uint32_t width;
+	uint32_t height;
+	int32_t stride;
+	QXLPHYSICAL data;
+};
+
+struct qxl_surface_cmd {
+	union qxl_release_info release_info;
+	uint32_t surface_id;
+	uint8_t type;
+	uint32_t flags;
+	union {
+		struct qxl_surface surface_create;
+	} u;
+};
+
+struct qxl_clip_rects {
+	uint32_t num_rects;
+	struct qxl_data_chunk chunk;
+};
+
+enum {
+	QXL_PATH_BEGIN = (1 << 0),
+	QXL_PATH_END = (1 << 1),
+	QXL_PATH_CLOSE = (1 << 3),
+	QXL_PATH_BEZIER = (1 << 4),
+};
+
+struct qxl_path_seg {
+	uint32_t flags;
+	uint32_t count;
+	struct qxl_point_fix points[0];
+};
+
+struct qxl_path {
+	uint32_t data_size;
+	struct qxl_data_chunk chunk;
+};
+
+enum {
+	QXL_IMAGE_GROUP_DRIVER,
+	QXL_IMAGE_GROUP_DEVICE,
+	QXL_IMAGE_GROUP_RED,
+	QXL_IMAGE_GROUP_DRIVER_DONT_CACHE,
+};
+
+struct qxl_image_id {
+	uint32_t group;
+	uint32_t unique;
+};
+
+union qxl_image_id_union {
+	struct qxl_image_id id;
+	uint64_t value;
+};
+
+enum qxl_image_flags {
+	QXL_IMAGE_CACHE = (1 << 0),
+	QXL_IMAGE_HIGH_BITS_SET = (1 << 1),
+};
+
+enum qxl_bitmap_flags {
+	QXL_BITMAP_DIRECT = (1 << 0),
+	QXL_BITMAP_UNSTABLE = (1 << 1),
+	QXL_BITMAP_TOP_DOWN = (1 << 2), /* == SPICE_BITMAP_FLAGS_TOP_DOWN */
+};
+
+#define QXL_SET_IMAGE_ID(image, _group, _unique) {              \
+	(image)->descriptor.id = (((uint64_t)_unique) << 32) | _group;	\
+}
+
+struct qxl_image_descriptor {
+	uint64_t id;
+	uint8_t type;
+	uint8_t flags;
+	uint32_t width;
+	uint32_t height;
+};
+
+struct qxl_palette {
+	uint64_t unique;
+	uint16_t num_ents;
+	uint32_t ents[0];
+};
+
+struct qxl_bitmap {
+	uint8_t format;
+	uint8_t flags;
+	uint32_t x;
+	uint32_t y;
+	uint32_t stride;
+	QXLPHYSICAL palette;
+	QXLPHYSICAL data; /* data[0] ? */
+};
+
+struct qxl_surface_id {
+	uint32_t surface_id;
+};
+
+struct qxl_encoder_data {
+	uint32_t data_size;
+	uint8_t data[0];
+};
+
+struct qxl_image {
+	struct qxl_image_descriptor descriptor;
+	union { /* variable length */
+		struct qxl_bitmap bitmap;
+		struct qxl_encoder_data quic;
+		struct qxl_surface_id surface_image;
+	} u;
+};
+
+/* A QXLHead is a single monitor output backed by a QXLSurface.
+ * x and y offsets are unsigned since they are used in relation to
+ * the given surface, not the same as the x, y coordinates in the guest
+ * screen reference frame. */
+struct qxl_head {
+	uint32_t id;
+	uint32_t surface_id;
+	uint32_t width;
+	uint32_t height;
+	uint32_t x;
+	uint32_t y;
+	uint32_t flags;
+};
+
+struct qxl_monitors_config {
+	uint16_t count;
+	uint16_t max_allowed; /* If it is 0 no fixed limit is given by the
+				 driver */
+	struct qxl_head heads[0];
+};
+
+#pragma pack(pop)
+
+#endif /* _H_QXL_DEV */
diff --git a/drivers/gpu/drm/qxl/qxl_display.c b/drivers/gpu/drm/qxl/qxl_display.c
new file mode 100644
index 000000000000..fcfd4436ceed
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_display.c
@@ -0,0 +1,982 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Dave Airlie
+ *          Alon Levy
+ */
+
+
+#include "linux/crc32.h"
+
+#include "qxl_drv.h"
+#include "qxl_object.h"
+#include "drm_crtc_helper.h"
+
+static void qxl_crtc_set_to_mode(struct qxl_device *qdev,
+				 struct drm_connector *connector,
+				 struct qxl_head *head)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_display_mode *mode, *t;
+	int width = head->width;
+	int height = head->height;
+
+	if (width < 320 || height < 240) {
+		qxl_io_log(qdev, "%s: bad head: %dx%d", width, height);
+		width = 1024;
+		height = 768;
+	}
+	if (width * height * 4 > 16*1024*1024) {
+		width = 1024;
+		height = 768;
+	}
+	/* TODO: go over regular modes and removed preferred? */
+	list_for_each_entry_safe(mode, t, &connector->probed_modes, head)
+		drm_mode_remove(connector, mode);
+	mode = drm_cvt_mode(dev, width, height, 60, false, false, false);
+	mode->type |= DRM_MODE_TYPE_PREFERRED;
+	mode->status = MODE_OK;
+	drm_mode_probed_add(connector, mode);
+	qxl_io_log(qdev, "%s: %d x %d\n", __func__, width, height);
+}
+
+void qxl_crtc_set_from_monitors_config(struct qxl_device *qdev)
+{
+	struct drm_connector *connector;
+	int i;
+	struct drm_device *dev = qdev->ddev;
+
+	i = 0;
+	qxl_io_log(qdev, "%s: %d, %d\n", __func__,
+		   dev->mode_config.num_connector,
+		   qdev->monitors_config->count);
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
+		if (i > qdev->monitors_config->count) {
+			/* crtc will be reported as disabled */
+			continue;
+		}
+		qxl_crtc_set_to_mode(qdev, connector,
+				     &qdev->monitors_config->heads[i]);
+		++i;
+	}
+}
+
+void qxl_alloc_client_monitors_config(struct qxl_device *qdev, unsigned count)
+{
+	if (qdev->client_monitors_config &&
+	    count > qdev->client_monitors_config->count) {
+		kfree(qdev->client_monitors_config);
+		qdev->client_monitors_config = NULL;
+	}
+	if (!qdev->client_monitors_config) {
+		qdev->client_monitors_config = kzalloc(
+				sizeof(struct qxl_monitors_config) +
+				sizeof(struct qxl_head) * count, GFP_KERNEL);
+		if (!qdev->client_monitors_config) {
+			qxl_io_log(qdev,
+				   "%s: allocation failure for %u heads\n",
+				   __func__, count);
+			return;
+		}
+	}
+	qdev->client_monitors_config->count = count;
+}
+
+static int qxl_display_copy_rom_client_monitors_config(struct qxl_device *qdev)
+{
+	int i;
+	int num_monitors;
+	uint32_t crc;
+
+	BUG_ON(!qdev->monitors_config);
+	num_monitors = qdev->rom->client_monitors_config.count;
+	crc = crc32(0, (const uint8_t *)&qdev->rom->client_monitors_config,
+		  sizeof(qdev->rom->client_monitors_config));
+	if (crc != qdev->rom->client_monitors_config_crc) {
+		qxl_io_log(qdev, "crc mismatch: have %X (%d) != %X\n", crc,
+			   sizeof(qdev->rom->client_monitors_config),
+			   qdev->rom->client_monitors_config_crc);
+		return 1;
+	}
+	if (num_monitors > qdev->monitors_config->max_allowed) {
+		DRM_INFO("client monitors list will be truncated: %d < %d\n",
+			 qdev->monitors_config->max_allowed, num_monitors);
+		num_monitors = qdev->monitors_config->max_allowed;
+	} else {
+		num_monitors = qdev->rom->client_monitors_config.count;
+	}
+	qxl_alloc_client_monitors_config(qdev, num_monitors);
+	/* we copy max from the client but it isn't used */
+	qdev->client_monitors_config->max_allowed =
+				qdev->monitors_config->max_allowed;
+	for (i = 0 ; i < qdev->client_monitors_config->count ; ++i) {
+		struct qxl_urect *c_rect =
+			&qdev->rom->client_monitors_config.heads[i];
+		struct qxl_head *client_head =
+			&qdev->client_monitors_config->heads[i];
+		struct qxl_head *head = &qdev->monitors_config->heads[i];
+		client_head->x = head->x = c_rect->left;
+		client_head->y = head->y = c_rect->top;
+		client_head->width = head->width =
+						c_rect->right - c_rect->left;
+		client_head->height = head->height =
+						c_rect->bottom - c_rect->top;
+		client_head->surface_id = head->surface_id = 0;
+		client_head->id = head->id = i;
+		client_head->flags = head->flags = 0;
+		QXL_DEBUG(qdev, "read %dx%d+%d+%d\n", head->width, head->height,
+			  head->x, head->y);
+	}
+	return 0;
+}
+
+void qxl_display_read_client_monitors_config(struct qxl_device *qdev)
+{
+
+	while (qxl_display_copy_rom_client_monitors_config(qdev)) {
+		qxl_io_log(qdev, "failed crc check for client_monitors_config,"
+				 " retrying\n");
+	}
+	qxl_crtc_set_from_monitors_config(qdev);
+	/* fire off a uevent and let userspace tell us what to do */
+	qxl_io_log(qdev, "calling drm_sysfs_hotplug_event\n");
+	drm_sysfs_hotplug_event(qdev->ddev);
+}
+
+static int qxl_add_monitors_config_modes(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct qxl_device *qdev = dev->dev_private;
+	struct qxl_output *output = drm_connector_to_qxl_output(connector);
+	int h = output->index;
+	struct drm_display_mode *mode = NULL;
+	struct qxl_head *head;
+
+	if (!qdev->monitors_config)
+		return 0;
+	head = &qdev->monitors_config->heads[h];
+
+	mode = drm_cvt_mode(dev, head->width, head->height, 60, false, false,
+			    false);
+	mode->type |= DRM_MODE_TYPE_PREFERRED;
+	drm_mode_probed_add(connector, mode);
+	return 1;
+}
+
+static int qxl_add_common_modes(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_display_mode *mode = NULL;
+	int i;
+	struct mode_size {
+		int w;
+		int h;
+	} common_modes[] = {
+		{ 640,  480},
+		{ 720,  480},
+		{ 800,  600},
+		{ 848,  480},
+		{1024,  768},
+		{1152,  768},
+		{1280,  720},
+		{1280,  800},
+		{1280,  854},
+		{1280,  960},
+		{1280, 1024},
+		{1440,  900},
+		{1400, 1050},
+		{1680, 1050},
+		{1600, 1200},
+		{1920, 1080},
+		{1920, 1200}
+	};
+
+	for (i = 0; i < ARRAY_SIZE(common_modes); i++) {
+		if (common_modes[i].w < 320 || common_modes[i].h < 200)
+			continue;
+
+		mode = drm_cvt_mode(dev, common_modes[i].w, common_modes[i].h,
+				    60, false, false, false);
+		if (common_modes[i].w == 1024 && common_modes[i].h == 768)
+			mode->type |= DRM_MODE_TYPE_PREFERRED;
+		drm_mode_probed_add(connector, mode);
+	}
+	return i - 1;
+}
+
+static void qxl_crtc_gamma_set(struct drm_crtc *crtc, u16 *red, u16 *green,
+			       u16 *blue, uint32_t start, uint32_t size)
+{
+	/* TODO */
+}
+
+static void qxl_crtc_destroy(struct drm_crtc *crtc)
+{
+	struct qxl_crtc *qxl_crtc = to_qxl_crtc(crtc);
+
+	drm_crtc_cleanup(crtc);
+	kfree(qxl_crtc);
+}
+
+static void
+qxl_hide_cursor(struct qxl_device *qdev)
+{
+	struct qxl_release *release;
+	struct qxl_cursor_cmd *cmd;
+	int ret;
+
+	ret = qxl_alloc_release_reserved(qdev, sizeof(*cmd), QXL_RELEASE_CURSOR_CMD,
+					 &release, NULL);
+
+	cmd = (struct qxl_cursor_cmd *)qxl_release_map(qdev, release);
+	cmd->type = QXL_CURSOR_HIDE;
+	qxl_release_unmap(qdev, release, &cmd->release_info);
+
+	qxl_fence_releaseable(qdev, release);
+	qxl_push_cursor_ring_release(qdev, release, QXL_CMD_CURSOR, false);
+	qxl_release_unreserve(qdev, release);
+}
+
+static int qxl_crtc_cursor_set(struct drm_crtc *crtc,
+			       struct drm_file *file_priv,
+			       uint32_t handle,
+			       uint32_t width,
+			       uint32_t height)
+{
+	struct drm_device *dev = crtc->dev;
+	struct qxl_device *qdev = dev->dev_private;
+	struct qxl_crtc *qcrtc = to_qxl_crtc(crtc);
+	struct drm_gem_object *obj;
+	struct qxl_cursor *cursor;
+	struct qxl_cursor_cmd *cmd;
+	struct qxl_bo *cursor_bo, *user_bo;
+	struct qxl_release *release;
+	void *user_ptr;
+
+	int size = 64*64*4;
+	int ret = 0;
+	if (!handle) {
+		qxl_hide_cursor(qdev);
+		return 0;
+	}
+
+	obj = drm_gem_object_lookup(crtc->dev, file_priv, handle);
+	if (!obj) {
+		DRM_ERROR("cannot find cursor object\n");
+		return -ENOENT;
+	}
+
+	user_bo = gem_to_qxl_bo(obj);
+
+	ret = qxl_bo_reserve(user_bo, false);
+	if (ret)
+		goto out_unref;
+
+	ret = qxl_bo_pin(user_bo, QXL_GEM_DOMAIN_CPU, NULL);
+	if (ret)
+		goto out_unreserve;
+
+	ret = qxl_bo_kmap(user_bo, &user_ptr);
+	if (ret)
+		goto out_unpin;
+
+	ret = qxl_alloc_release_reserved(qdev, sizeof(*cmd),
+					 QXL_RELEASE_CURSOR_CMD,
+					 &release, NULL);
+	if (ret)
+		goto out_kunmap;
+	ret = qxl_alloc_bo_reserved(qdev, sizeof(struct qxl_cursor) + size,
+				    &cursor_bo);
+	if (ret)
+		goto out_free_release;
+	ret = qxl_bo_kmap(cursor_bo, (void **)&cursor);
+	if (ret)
+		goto out_free_bo;
+
+	cursor->header.unique = 0;
+	cursor->header.type = SPICE_CURSOR_TYPE_ALPHA;
+	cursor->header.width = 64;
+	cursor->header.height = 64;
+	cursor->header.hot_spot_x = 0;
+	cursor->header.hot_spot_y = 0;
+	cursor->data_size = size;
+	cursor->chunk.next_chunk = 0;
+	cursor->chunk.prev_chunk = 0;
+	cursor->chunk.data_size = size;
+
+	memcpy(cursor->chunk.data, user_ptr, size);
+
+	qxl_bo_kunmap(cursor_bo);
+
+	/* finish with the userspace bo */
+	qxl_bo_kunmap(user_bo);
+	qxl_bo_unpin(user_bo);
+	qxl_bo_unreserve(user_bo);
+	drm_gem_object_unreference_unlocked(obj);
+
+	cmd = (struct qxl_cursor_cmd *)qxl_release_map(qdev, release);
+	cmd->type = QXL_CURSOR_SET;
+	cmd->u.set.position.x = qcrtc->cur_x;
+	cmd->u.set.position.y = qcrtc->cur_y;
+
+	cmd->u.set.shape = qxl_bo_physical_address(qdev, cursor_bo, 0);
+	qxl_release_add_res(qdev, release, cursor_bo);
+
+	cmd->u.set.visible = 1;
+	qxl_release_unmap(qdev, release, &cmd->release_info);
+
+	qxl_fence_releaseable(qdev, release);
+	qxl_push_cursor_ring_release(qdev, release, QXL_CMD_CURSOR, false);
+	qxl_release_unreserve(qdev, release);
+
+	qxl_bo_unreserve(cursor_bo);
+	qxl_bo_unref(&cursor_bo);
+
+	return ret;
+out_free_bo:
+	qxl_bo_unref(&cursor_bo);
+out_free_release:
+	qxl_release_unreserve(qdev, release);
+	qxl_release_free(qdev, release);
+out_kunmap:
+	qxl_bo_kunmap(user_bo);
+out_unpin:
+	qxl_bo_unpin(user_bo);
+out_unreserve:
+	qxl_bo_unreserve(user_bo);
+out_unref:
+	drm_gem_object_unreference_unlocked(obj);
+	return ret;
+}
+
+static int qxl_crtc_cursor_move(struct drm_crtc *crtc,
+				int x, int y)
+{
+	struct drm_device *dev = crtc->dev;
+	struct qxl_device *qdev = dev->dev_private;
+	struct qxl_crtc *qcrtc = to_qxl_crtc(crtc);
+	struct qxl_release *release;
+	struct qxl_cursor_cmd *cmd;
+	int ret;
+
+	ret = qxl_alloc_release_reserved(qdev, sizeof(*cmd), QXL_RELEASE_CURSOR_CMD,
+				   &release, NULL);
+
+	qcrtc->cur_x = x;
+	qcrtc->cur_y = y;
+
+	cmd = (struct qxl_cursor_cmd *)qxl_release_map(qdev, release);
+	cmd->type = QXL_CURSOR_MOVE;
+	cmd->u.position.x = qcrtc->cur_x;
+	cmd->u.position.y = qcrtc->cur_y;
+	qxl_release_unmap(qdev, release, &cmd->release_info);
+
+	qxl_fence_releaseable(qdev, release);
+	qxl_push_cursor_ring_release(qdev, release, QXL_CMD_CURSOR, false);
+	qxl_release_unreserve(qdev, release);
+	return 0;
+}
+
+
+static const struct drm_crtc_funcs qxl_crtc_funcs = {
+	.cursor_set = qxl_crtc_cursor_set,
+	.cursor_move = qxl_crtc_cursor_move,
+	.gamma_set = qxl_crtc_gamma_set,
+	.set_config = drm_crtc_helper_set_config,
+	.destroy = qxl_crtc_destroy,
+};
+
+static void qxl_user_framebuffer_destroy(struct drm_framebuffer *fb)
+{
+	struct qxl_framebuffer *qxl_fb = to_qxl_framebuffer(fb);
+
+	if (qxl_fb->obj)
+		drm_gem_object_unreference_unlocked(qxl_fb->obj);
+	drm_framebuffer_cleanup(fb);
+	kfree(qxl_fb);
+}
+
+static int qxl_framebuffer_surface_dirty(struct drm_framebuffer *fb,
+					 struct drm_file *file_priv,
+					 unsigned flags, unsigned color,
+					 struct drm_clip_rect *clips,
+					 unsigned num_clips)
+{
+	/* TODO: vmwgfx where this was cribbed from had locking. Why? */
+	struct qxl_framebuffer *qxl_fb = to_qxl_framebuffer(fb);
+	struct qxl_device *qdev = qxl_fb->base.dev->dev_private;
+	struct drm_clip_rect norect;
+	struct qxl_bo *qobj;
+	int inc = 1;
+
+	qobj = gem_to_qxl_bo(qxl_fb->obj);
+	if (qxl_fb != qdev->active_user_framebuffer) {
+		DRM_INFO("%s: qxl_fb 0x%p != qdev->active_user_framebuffer 0x%p\n",
+			__func__, qxl_fb, qdev->active_user_framebuffer);
+	}
+	if (!num_clips) {
+		num_clips = 1;
+		clips = &norect;
+		norect.x1 = norect.y1 = 0;
+		norect.x2 = fb->width;
+		norect.y2 = fb->height;
+	} else if (flags & DRM_MODE_FB_DIRTY_ANNOTATE_COPY) {
+		num_clips /= 2;
+		inc = 2; /* skip source rects */
+	}
+
+	qxl_draw_dirty_fb(qdev, qxl_fb, qobj, flags, color,
+			  clips, num_clips, inc);
+	return 0;
+}
+
+static const struct drm_framebuffer_funcs qxl_fb_funcs = {
+	.destroy = qxl_user_framebuffer_destroy,
+	.dirty = qxl_framebuffer_surface_dirty,
+/*	TODO?
+ *	.create_handle = qxl_user_framebuffer_create_handle, */
+};
+
+int
+qxl_framebuffer_init(struct drm_device *dev,
+		     struct qxl_framebuffer *qfb,
+		     struct drm_mode_fb_cmd2 *mode_cmd,
+		     struct drm_gem_object *obj)
+{
+	int ret;
+
+	qfb->obj = obj;
+	ret = drm_framebuffer_init(dev, &qfb->base, &qxl_fb_funcs);
+	if (ret) {
+		qfb->obj = NULL;
+		return ret;
+	}
+	drm_helper_mode_fill_fb_struct(&qfb->base, mode_cmd);
+	return 0;
+}
+
+static void qxl_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+}
+
+static bool qxl_crtc_mode_fixup(struct drm_crtc *crtc,
+				  const struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	struct drm_device *dev = crtc->dev;
+	struct qxl_device *qdev = dev->dev_private;
+
+	qxl_io_log(qdev, "%s: (%d,%d) => (%d,%d)\n",
+		   __func__,
+		   mode->hdisplay, mode->vdisplay,
+		   adjusted_mode->hdisplay,
+		   adjusted_mode->vdisplay);
+	return true;
+}
+
+void
+qxl_send_monitors_config(struct qxl_device *qdev)
+{
+	int i;
+
+	BUG_ON(!qdev->ram_header->monitors_config);
+
+	if (qdev->monitors_config->count == 0) {
+		qxl_io_log(qdev, "%s: 0 monitors??\n", __func__);
+		return;
+	}
+	for (i = 0 ; i < qdev->monitors_config->count ; ++i) {
+		struct qxl_head *head = &qdev->monitors_config->heads[i];
+
+		if (head->y > 8192 || head->y < head->x ||
+		    head->width > 8192 || head->height > 8192) {
+			DRM_ERROR("head %d wrong: %dx%d+%d+%d\n",
+				  i, head->width, head->height,
+				  head->x, head->y);
+			return;
+		}
+	}
+	qxl_io_monitors_config(qdev);
+}
+
+static void qxl_monitors_config_set_single(struct qxl_device *qdev,
+					   unsigned x, unsigned y,
+					   unsigned width, unsigned height)
+{
+	DRM_DEBUG("%dx%d+%d+%d\n", width, height, x, y);
+	qdev->monitors_config->count = 1;
+	qdev->monitors_config->heads[0].x = x;
+	qdev->monitors_config->heads[0].y = y;
+	qdev->monitors_config->heads[0].width = width;
+	qdev->monitors_config->heads[0].height = height;
+}
+
+static int qxl_crtc_mode_set(struct drm_crtc *crtc,
+			       struct drm_display_mode *mode,
+			       struct drm_display_mode *adjusted_mode,
+			       int x, int y,
+			       struct drm_framebuffer *old_fb)
+{
+	struct drm_device *dev = crtc->dev;
+	struct qxl_device *qdev = dev->dev_private;
+	struct qxl_mode *m = (void *)mode->private;
+	struct qxl_framebuffer *qfb;
+	struct qxl_bo *bo, *old_bo = NULL;
+	uint32_t width, height, base_offset;
+	bool recreate_primary = false;
+	int ret;
+
+	if (!crtc->fb) {
+		DRM_DEBUG_KMS("No FB bound\n");
+		return 0;
+	}
+
+	if (old_fb) {
+		qfb = to_qxl_framebuffer(old_fb);
+		old_bo = gem_to_qxl_bo(qfb->obj);
+	}
+	qfb = to_qxl_framebuffer(crtc->fb);
+	bo = gem_to_qxl_bo(qfb->obj);
+	if (!m)
+		/* and do we care? */
+		DRM_DEBUG("%dx%d: not a native mode\n", x, y);
+	else
+		DRM_DEBUG("%dx%d: qxl id %d\n",
+			  mode->hdisplay, mode->vdisplay, m->id);
+	DRM_DEBUG("+%d+%d (%d,%d) => (%d,%d)\n",
+		  x, y,
+		  mode->hdisplay, mode->vdisplay,
+		  adjusted_mode->hdisplay,
+		  adjusted_mode->vdisplay);
+
+	recreate_primary = true;
+
+	width = mode->hdisplay;
+	height = mode->vdisplay;
+	base_offset = 0;
+
+	ret = qxl_bo_reserve(bo, false);
+	if (ret != 0)
+		return ret;
+	ret = qxl_bo_pin(bo, bo->type, NULL);
+	if (ret != 0) {
+		qxl_bo_unreserve(bo);
+		return -EINVAL;
+	}
+	qxl_bo_unreserve(bo);
+	if (recreate_primary) {
+		qxl_io_destroy_primary(qdev);
+		qxl_io_log(qdev,
+			   "recreate primary: %dx%d (was %dx%d,%d,%d)\n",
+			   width, height, bo->surf.width,
+			   bo->surf.height, bo->surf.stride, bo->surf.format);
+		qxl_io_create_primary(qdev, width, height, base_offset, bo);
+		bo->is_primary = true;
+	}
+
+	if (old_bo && old_bo != bo) {
+		old_bo->is_primary = false;
+		ret = qxl_bo_reserve(old_bo, false);
+		qxl_bo_unpin(old_bo);
+		qxl_bo_unreserve(old_bo);
+	}
+
+	if (qdev->monitors_config->count == 0) {
+		qxl_monitors_config_set_single(qdev, x, y,
+					       mode->hdisplay,
+					       mode->vdisplay);
+	}
+	qdev->mode_set = true;
+	return 0;
+}
+
+static void qxl_crtc_prepare(struct drm_crtc *crtc)
+{
+	DRM_DEBUG("current: %dx%d+%d+%d (%d).\n",
+		  crtc->mode.hdisplay, crtc->mode.vdisplay,
+		  crtc->x, crtc->y, crtc->enabled);
+}
+
+static void qxl_crtc_commit(struct drm_crtc *crtc)
+{
+	DRM_DEBUG("\n");
+}
+
+static void qxl_crtc_load_lut(struct drm_crtc *crtc)
+{
+	DRM_DEBUG("\n");
+}
+
+static const struct drm_crtc_helper_funcs qxl_crtc_helper_funcs = {
+	.dpms = qxl_crtc_dpms,
+	.mode_fixup = qxl_crtc_mode_fixup,
+	.mode_set = qxl_crtc_mode_set,
+	.prepare = qxl_crtc_prepare,
+	.commit = qxl_crtc_commit,
+	.load_lut = qxl_crtc_load_lut,
+};
+
+static int qdev_crtc_init(struct drm_device *dev, int num_crtc)
+{
+	struct qxl_crtc *qxl_crtc;
+
+	qxl_crtc = kzalloc(sizeof(struct qxl_crtc), GFP_KERNEL);
+	if (!qxl_crtc)
+		return -ENOMEM;
+
+	drm_crtc_init(dev, &qxl_crtc->base, &qxl_crtc_funcs);
+
+	drm_mode_crtc_set_gamma_size(&qxl_crtc->base, 256);
+	drm_crtc_helper_add(&qxl_crtc->base, &qxl_crtc_helper_funcs);
+	return 0;
+}
+
+static void qxl_enc_dpms(struct drm_encoder *encoder, int mode)
+{
+	DRM_DEBUG("\n");
+}
+
+static bool qxl_enc_mode_fixup(struct drm_encoder *encoder,
+			       const struct drm_display_mode *mode,
+			       struct drm_display_mode *adjusted_mode)
+{
+	DRM_DEBUG("\n");
+	return true;
+}
+
+static void qxl_enc_prepare(struct drm_encoder *encoder)
+{
+	DRM_DEBUG("\n");
+}
+
+static void qxl_write_monitors_config_for_encoder(struct qxl_device *qdev,
+		struct drm_encoder *encoder)
+{
+	int i;
+	struct qxl_head *head;
+	struct drm_display_mode *mode;
+
+	BUG_ON(!encoder);
+	/* TODO: ugly, do better */
+	for (i = 0 ; (encoder->possible_crtcs != (1 << i)) && i < 32; ++i)
+		;
+	if (encoder->possible_crtcs != (1 << i)) {
+		DRM_ERROR("encoder has wrong possible_crtcs: %x\n",
+			  encoder->possible_crtcs);
+		return;
+	}
+	if (!qdev->monitors_config ||
+	    qdev->monitors_config->max_allowed <= i) {
+		DRM_ERROR(
+		"head number too large or missing monitors config: %p, %d",
+		qdev->monitors_config,
+		qdev->monitors_config ?
+			qdev->monitors_config->max_allowed : -1);
+		return;
+	}
+	if (!encoder->crtc) {
+		DRM_ERROR("missing crtc on encoder %p\n", encoder);
+		return;
+	}
+	if (i != 0)
+		DRM_DEBUG("missing for multiple monitors: no head holes\n");
+	head = &qdev->monitors_config->heads[i];
+	head->id = i;
+	head->surface_id = 0;
+	if (encoder->crtc->enabled) {
+		mode = &encoder->crtc->mode;
+		head->width = mode->hdisplay;
+		head->height = mode->vdisplay;
+		head->x = encoder->crtc->x;
+		head->y = encoder->crtc->y;
+		if (qdev->monitors_config->count < i + 1)
+			qdev->monitors_config->count = i + 1;
+	} else {
+		head->width = 0;
+		head->height = 0;
+		head->x = 0;
+		head->y = 0;
+	}
+	DRM_DEBUG("setting head %d to +%d+%d %dx%d\n",
+		  i, head->x, head->y, head->width, head->height);
+	head->flags = 0;
+	/* TODO - somewhere else to call this for multiple monitors
+	 * (config_commit?) */
+	qxl_send_monitors_config(qdev);
+}
+
+static void qxl_enc_commit(struct drm_encoder *encoder)
+{
+	struct qxl_device *qdev = encoder->dev->dev_private;
+
+	qxl_write_monitors_config_for_encoder(qdev, encoder);
+	DRM_DEBUG("\n");
+}
+
+static void qxl_enc_mode_set(struct drm_encoder *encoder,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
+{
+	DRM_DEBUG("\n");
+}
+
+static int qxl_conn_get_modes(struct drm_connector *connector)
+{
+	int ret = 0;
+	struct qxl_device *qdev = connector->dev->dev_private;
+
+	DRM_DEBUG_KMS("monitors_config=%p\n", qdev->monitors_config);
+	/* TODO: what should we do here? only show the configured modes for the
+	 * device, or allow the full list, or both? */
+	if (qdev->monitors_config && qdev->monitors_config->count) {
+		ret = qxl_add_monitors_config_modes(connector);
+		if (ret < 0)
+			return ret;
+	}
+	ret += qxl_add_common_modes(connector);
+	return ret;
+}
+
+static int qxl_conn_mode_valid(struct drm_connector *connector,
+			       struct drm_display_mode *mode)
+{
+	/* TODO: is this called for user defined modes? (xrandr --add-mode)
+	 * TODO: check that the mode fits in the framebuffer */
+	DRM_DEBUG("%s: %dx%d status=%d\n", mode->name, mode->hdisplay,
+		  mode->vdisplay, mode->status);
+	return MODE_OK;
+}
+
+static struct drm_encoder *qxl_best_encoder(struct drm_connector *connector)
+{
+	struct qxl_output *qxl_output =
+		drm_connector_to_qxl_output(connector);
+
+	DRM_DEBUG("\n");
+	return &qxl_output->enc;
+}
+
+
+static const struct drm_encoder_helper_funcs qxl_enc_helper_funcs = {
+	.dpms = qxl_enc_dpms,
+	.mode_fixup = qxl_enc_mode_fixup,
+	.prepare = qxl_enc_prepare,
+	.mode_set = qxl_enc_mode_set,
+	.commit = qxl_enc_commit,
+};
+
+static const struct drm_connector_helper_funcs qxl_connector_helper_funcs = {
+	.get_modes = qxl_conn_get_modes,
+	.mode_valid = qxl_conn_mode_valid,
+	.best_encoder = qxl_best_encoder,
+};
+
+static void qxl_conn_save(struct drm_connector *connector)
+{
+	DRM_DEBUG("\n");
+}
+
+static void qxl_conn_restore(struct drm_connector *connector)
+{
+	DRM_DEBUG("\n");
+}
+
+static enum drm_connector_status qxl_conn_detect(
+			struct drm_connector *connector,
+			bool force)
+{
+	struct qxl_output *output =
+		drm_connector_to_qxl_output(connector);
+	struct drm_device *ddev = connector->dev;
+	struct qxl_device *qdev = ddev->dev_private;
+	int connected;
+
+	/* The first monitor is always connected */
+	connected = (output->index == 0) ||
+		    (qdev->monitors_config &&
+		     qdev->monitors_config->count > output->index);
+
+	DRM_DEBUG("\n");
+	return connected ? connector_status_connected
+			 : connector_status_disconnected;
+}
+
+static int qxl_conn_set_property(struct drm_connector *connector,
+				   struct drm_property *property,
+				   uint64_t value)
+{
+	DRM_DEBUG("\n");
+	return 0;
+}
+
+static void qxl_conn_destroy(struct drm_connector *connector)
+{
+	struct qxl_output *qxl_output =
+		drm_connector_to_qxl_output(connector);
+
+	drm_sysfs_connector_remove(connector);
+	drm_connector_cleanup(connector);
+	kfree(qxl_output);
+}
+
+static const struct drm_connector_funcs qxl_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.save = qxl_conn_save,
+	.restore = qxl_conn_restore,
+	.detect = qxl_conn_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.set_property = qxl_conn_set_property,
+	.destroy = qxl_conn_destroy,
+};
+
+static void qxl_enc_destroy(struct drm_encoder *encoder)
+{
+	drm_encoder_cleanup(encoder);
+}
+
+static const struct drm_encoder_funcs qxl_enc_funcs = {
+	.destroy = qxl_enc_destroy,
+};
+
+static int qdev_output_init(struct drm_device *dev, int num_output)
+{
+	struct qxl_output *qxl_output;
+	struct drm_connector *connector;
+	struct drm_encoder *encoder;
+
+	qxl_output = kzalloc(sizeof(struct qxl_output), GFP_KERNEL);
+	if (!qxl_output)
+		return -ENOMEM;
+
+	qxl_output->index = num_output;
+
+	connector = &qxl_output->base;
+	encoder = &qxl_output->enc;
+	drm_connector_init(dev, &qxl_output->base,
+			   &qxl_connector_funcs, DRM_MODE_CONNECTOR_VIRTUAL);
+
+	drm_encoder_init(dev, &qxl_output->enc, &qxl_enc_funcs,
+			 DRM_MODE_ENCODER_VIRTUAL);
+
+	encoder->possible_crtcs = 1 << num_output;
+	drm_mode_connector_attach_encoder(&qxl_output->base,
+					  &qxl_output->enc);
+	drm_encoder_helper_add(encoder, &qxl_enc_helper_funcs);
+	drm_connector_helper_add(connector, &qxl_connector_helper_funcs);
+
+	drm_sysfs_connector_add(connector);
+	return 0;
+}
+
+static struct drm_framebuffer *
+qxl_user_framebuffer_create(struct drm_device *dev,
+			    struct drm_file *file_priv,
+			    struct drm_mode_fb_cmd2 *mode_cmd)
+{
+	struct drm_gem_object *obj;
+	struct qxl_framebuffer *qxl_fb;
+	struct qxl_device *qdev = dev->dev_private;
+	int ret;
+
+	obj = drm_gem_object_lookup(dev, file_priv, mode_cmd->handles[0]);
+
+	qxl_fb = kzalloc(sizeof(*qxl_fb), GFP_KERNEL);
+	if (qxl_fb == NULL)
+		return NULL;
+
+	ret = qxl_framebuffer_init(dev, qxl_fb, mode_cmd, obj);
+	if (ret) {
+		kfree(qxl_fb);
+		drm_gem_object_unreference_unlocked(obj);
+		return NULL;
+	}
+
+	if (qdev->active_user_framebuffer) {
+		DRM_INFO("%s: active_user_framebuffer %p -> %p\n",
+			 __func__,
+			 qdev->active_user_framebuffer, qxl_fb);
+	}
+	qdev->active_user_framebuffer = qxl_fb;
+
+	return &qxl_fb->base;
+}
+
+static const struct drm_mode_config_funcs qxl_mode_funcs = {
+	.fb_create = qxl_user_framebuffer_create,
+};
+
+int qxl_modeset_init(struct qxl_device *qdev)
+{
+	int i;
+	int ret;
+	struct drm_gem_object *gobj;
+	int max_allowed = QXL_NUM_OUTPUTS;
+	int monitors_config_size = sizeof(struct qxl_monitors_config) +
+				   max_allowed * sizeof(struct qxl_head);
+
+	drm_mode_config_init(qdev->ddev);
+	ret = qxl_gem_object_create(qdev, monitors_config_size, 0,
+				    QXL_GEM_DOMAIN_VRAM,
+				    false, false, NULL, &gobj);
+	if (ret) {
+		DRM_ERROR("%s: failed to create gem ret=%d\n", __func__, ret);
+		return -ENOMEM;
+	}
+	qdev->monitors_config_bo = gem_to_qxl_bo(gobj);
+	qxl_bo_kmap(qdev->monitors_config_bo, NULL);
+	qdev->monitors_config = qdev->monitors_config_bo->kptr;
+	qdev->ram_header->monitors_config =
+		qxl_bo_physical_address(qdev, qdev->monitors_config_bo, 0);
+
+	memset(qdev->monitors_config, 0, monitors_config_size);
+	qdev->monitors_config->max_allowed = max_allowed;
+
+	qdev->ddev->mode_config.funcs = (void *)&qxl_mode_funcs;
+
+	/* modes will be validated against the framebuffer size */
+	qdev->ddev->mode_config.min_width = 320;
+	qdev->ddev->mode_config.min_height = 200;
+	qdev->ddev->mode_config.max_width = 8192;
+	qdev->ddev->mode_config.max_height = 8192;
+
+	qdev->ddev->mode_config.fb_base = qdev->vram_base;
+	for (i = 0 ; i < QXL_NUM_OUTPUTS; ++i) {
+		qdev_crtc_init(qdev->ddev, i);
+		qdev_output_init(qdev->ddev, i);
+	}
+
+	qdev->mode_info.mode_config_initialized = true;
+
+	/* primary surface must be created by this point, to allow
+	 * issuing command queue commands and having them read by
+	 * spice server. */
+	qxl_fbdev_init(qdev);
+	return 0;
+}
+
+void qxl_modeset_fini(struct qxl_device *qdev)
+{
+	qxl_fbdev_fini(qdev);
+	if (qdev->mode_info.mode_config_initialized) {
+		drm_mode_config_cleanup(qdev->ddev);
+		qdev->mode_info.mode_config_initialized = false;
+	}
+}
diff --git a/drivers/gpu/drm/qxl/qxl_draw.c b/drivers/gpu/drm/qxl/qxl_draw.c
new file mode 100644
index 000000000000..3c8c3dbf9378
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_draw.c
@@ -0,0 +1,390 @@
+/*
+ * Copyright 2011 Red Hat, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * on the rights to use, copy, modify, merge, publish, distribute, sub
+ * license, and/or sell copies of the Software, and to permit persons to whom
+ * the Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "qxl_drv.h"
+#include "qxl_object.h"
+
+/* returns a pointer to the already allocated qxl_rect array inside
+ * the qxl_clip_rects. This is *not* the same as the memory allocated
+ * on the device, it is offset to qxl_clip_rects.chunk.data */
+static struct qxl_rect *drawable_set_clipping(struct qxl_device *qdev,
+					      struct qxl_drawable *drawable,
+					      unsigned num_clips,
+					      struct qxl_bo **clips_bo,
+					      struct qxl_release *release)
+{
+	struct qxl_clip_rects *dev_clips;
+	int ret;
+	int size = sizeof(*dev_clips) + sizeof(struct qxl_rect) * num_clips;
+	ret = qxl_alloc_bo_reserved(qdev, size, clips_bo);
+	if (ret)
+		return NULL;
+
+	ret = qxl_bo_kmap(*clips_bo, (void **)&dev_clips);
+	if (ret) {
+		qxl_bo_unref(clips_bo);
+		return NULL;
+	}
+	dev_clips->num_rects = num_clips;
+	dev_clips->chunk.next_chunk = 0;
+	dev_clips->chunk.prev_chunk = 0;
+	dev_clips->chunk.data_size = sizeof(struct qxl_rect) * num_clips;
+	return (struct qxl_rect *)dev_clips->chunk.data;
+}
+
+static int
+make_drawable(struct qxl_device *qdev, int surface, uint8_t type,
+	      const struct qxl_rect *rect,
+	      struct qxl_release **release)
+{
+	struct qxl_drawable *drawable;
+	int i, ret;
+
+	ret = qxl_alloc_release_reserved(qdev, sizeof(*drawable),
+					 QXL_RELEASE_DRAWABLE, release,
+					 NULL);
+	if (ret)
+		return ret;
+
+	drawable = (struct qxl_drawable *)qxl_release_map(qdev, *release);
+	drawable->type = type;
+
+	drawable->surface_id = surface;		/* Only primary for now */
+	drawable->effect = QXL_EFFECT_OPAQUE;
+	drawable->self_bitmap = 0;
+	drawable->self_bitmap_area.top = 0;
+	drawable->self_bitmap_area.left = 0;
+	drawable->self_bitmap_area.bottom = 0;
+	drawable->self_bitmap_area.right = 0;
+	/* FIXME: add clipping */
+	drawable->clip.type = SPICE_CLIP_TYPE_NONE;
+
+	/*
+	 * surfaces_dest[i] should apparently be filled out with the
+	 * surfaces that we depend on, and surface_rects should be
+	 * filled with the rectangles of those surfaces that we
+	 * are going to use.
+	 */
+	for (i = 0; i < 3; ++i)
+		drawable->surfaces_dest[i] = -1;
+
+	if (rect)
+		drawable->bbox = *rect;
+
+	drawable->mm_time = qdev->rom->mm_clock;
+	qxl_release_unmap(qdev, *release, &drawable->release_info);
+	return 0;
+}
+
+static int qxl_palette_create_1bit(struct qxl_bo **palette_bo,
+				   const struct qxl_fb_image *qxl_fb_image)
+{
+	struct qxl_device *qdev = qxl_fb_image->qdev;
+	const struct fb_image *fb_image = &qxl_fb_image->fb_image;
+	uint32_t visual = qxl_fb_image->visual;
+	const uint32_t *pseudo_palette = qxl_fb_image->pseudo_palette;
+	struct qxl_palette *pal;
+	int ret;
+	uint32_t fgcolor, bgcolor;
+	static uint64_t unique; /* we make no attempt to actually set this
+				 * correctly globaly, since that would require
+				 * tracking all of our palettes. */
+
+	ret = qxl_alloc_bo_reserved(qdev,
+				    sizeof(struct qxl_palette) + sizeof(uint32_t) * 2,
+				    palette_bo);
+
+	ret = qxl_bo_kmap(*palette_bo, (void **)&pal);
+	pal->num_ents = 2;
+	pal->unique = unique++;
+	if (visual == FB_VISUAL_TRUECOLOR || visual == FB_VISUAL_DIRECTCOLOR) {
+		/* NB: this is the only used branch currently. */
+		fgcolor = pseudo_palette[fb_image->fg_color];
+		bgcolor = pseudo_palette[fb_image->bg_color];
+	} else {
+		fgcolor = fb_image->fg_color;
+		bgcolor = fb_image->bg_color;
+	}
+	pal->ents[0] = bgcolor;
+	pal->ents[1] = fgcolor;
+	qxl_bo_kunmap(*palette_bo);
+	return 0;
+}
+
+void qxl_draw_opaque_fb(const struct qxl_fb_image *qxl_fb_image,
+			int stride /* filled in if 0 */)
+{
+	struct qxl_device *qdev = qxl_fb_image->qdev;
+	struct qxl_drawable *drawable;
+	struct qxl_rect rect;
+	const struct fb_image *fb_image = &qxl_fb_image->fb_image;
+	int x = fb_image->dx;
+	int y = fb_image->dy;
+	int width = fb_image->width;
+	int height = fb_image->height;
+	const char *src = fb_image->data;
+	int depth = fb_image->depth;
+	struct qxl_release *release;
+	struct qxl_bo *image_bo;
+	struct qxl_image *image;
+	int ret;
+
+	if (stride == 0)
+		stride = depth * width / 8;
+
+	rect.left = x;
+	rect.right = x + width;
+	rect.top = y;
+	rect.bottom = y + height;
+
+	ret = make_drawable(qdev, 0, QXL_DRAW_COPY, &rect, &release);
+	if (ret)
+		return;
+
+	ret = qxl_image_create(qdev, release, &image_bo,
+			       (const uint8_t *)src, 0, 0,
+			       width, height, depth, stride);
+	if (ret) {
+		qxl_release_unreserve(qdev, release);
+		qxl_release_free(qdev, release);
+		return;
+	}
+
+	if (depth == 1) {
+		struct qxl_bo *palette_bo;
+		void *ptr;
+		ret = qxl_palette_create_1bit(&palette_bo, qxl_fb_image);
+		qxl_release_add_res(qdev, release, palette_bo);
+
+		ptr = qxl_bo_kmap_atomic_page(qdev, image_bo, 0);
+		image = ptr;
+		image->u.bitmap.palette =
+			qxl_bo_physical_address(qdev, palette_bo, 0);
+		qxl_bo_kunmap_atomic_page(qdev, image_bo, ptr);
+		qxl_bo_unreserve(palette_bo);
+		qxl_bo_unref(&palette_bo);
+	}
+
+	drawable = (struct qxl_drawable *)qxl_release_map(qdev, release);
+
+	drawable->u.copy.src_area.top = 0;
+	drawable->u.copy.src_area.bottom = height;
+	drawable->u.copy.src_area.left = 0;
+	drawable->u.copy.src_area.right = width;
+
+	drawable->u.copy.rop_descriptor = SPICE_ROPD_OP_PUT;
+	drawable->u.copy.scale_mode = 0;
+	drawable->u.copy.mask.flags = 0;
+	drawable->u.copy.mask.pos.x = 0;
+	drawable->u.copy.mask.pos.y = 0;
+	drawable->u.copy.mask.bitmap = 0;
+
+	drawable->u.copy.src_bitmap =
+		qxl_bo_physical_address(qdev, image_bo, 0);
+	qxl_release_unmap(qdev, release, &drawable->release_info);
+
+	qxl_release_add_res(qdev, release, image_bo);
+	qxl_bo_unreserve(image_bo);
+	qxl_bo_unref(&image_bo);
+
+	qxl_fence_releaseable(qdev, release);
+	qxl_push_command_ring_release(qdev, release, QXL_CMD_DRAW, false);
+	qxl_release_unreserve(qdev, release);
+}
+
+/* push a draw command using the given clipping rectangles as
+ * the sources from the shadow framebuffer.
+ *
+ * Right now implementing with a single draw and a clip list. Clip
+ * lists are known to be a problem performance wise, this can be solved
+ * by treating them differently in the server.
+ */
+void qxl_draw_dirty_fb(struct qxl_device *qdev,
+		       struct qxl_framebuffer *qxl_fb,
+		       struct qxl_bo *bo,
+		       unsigned flags, unsigned color,
+		       struct drm_clip_rect *clips,
+		       unsigned num_clips, int inc)
+{
+	/*
+	 * TODO: if flags & DRM_MODE_FB_DIRTY_ANNOTATE_FILL then we should
+	 * send a fill command instead, much cheaper.
+	 *
+	 * See include/drm/drm_mode.h
+	 */
+	struct drm_clip_rect *clips_ptr;
+	int i;
+	int left, right, top, bottom;
+	int width, height;
+	struct qxl_drawable *drawable;
+	struct qxl_rect drawable_rect;
+	struct qxl_rect *rects;
+	int stride = qxl_fb->base.pitches[0];
+	/* depth is not actually interesting, we don't mask with it */
+	int depth = qxl_fb->base.bits_per_pixel;
+	uint8_t *surface_base;
+	struct qxl_release *release;
+	struct qxl_bo *image_bo;
+	struct qxl_bo *clips_bo;
+	int ret;
+
+	left = clips->x1;
+	right = clips->x2;
+	top = clips->y1;
+	bottom = clips->y2;
+
+	/* skip the first clip rect */
+	for (i = 1, clips_ptr = clips + inc;
+	     i < num_clips; i++, clips_ptr += inc) {
+		left = min_t(int, left, (int)clips_ptr->x1);
+		right = max_t(int, right, (int)clips_ptr->x2);
+		top = min_t(int, top, (int)clips_ptr->y1);
+		bottom = max_t(int, bottom, (int)clips_ptr->y2);
+	}
+
+	width = right - left;
+	height = bottom - top;
+	drawable_rect.left = left;
+	drawable_rect.right = right;
+	drawable_rect.top = top;
+	drawable_rect.bottom = bottom;
+	ret = make_drawable(qdev, 0, QXL_DRAW_COPY, &drawable_rect,
+			    &release);
+	if (ret)
+		return;
+
+	ret = qxl_bo_kmap(bo, (void **)&surface_base);
+	if (ret)
+		goto out_unref;
+
+	ret = qxl_image_create(qdev, release, &image_bo, surface_base,
+			       left, top, width, height, depth, stride);
+	qxl_bo_kunmap(bo);
+	if (ret)
+		goto out_unref;
+
+	rects = drawable_set_clipping(qdev, drawable, num_clips, &clips_bo, release);
+	if (!rects) {
+		qxl_bo_unref(&image_bo);
+		goto out_unref;
+	}
+	drawable = (struct qxl_drawable *)qxl_release_map(qdev, release);
+
+	drawable->clip.type = SPICE_CLIP_TYPE_RECTS;
+	drawable->clip.data = qxl_bo_physical_address(qdev,
+						      clips_bo, 0);
+	qxl_release_add_res(qdev, release, clips_bo);
+
+	drawable->u.copy.src_area.top = 0;
+	drawable->u.copy.src_area.bottom = height;
+	drawable->u.copy.src_area.left = 0;
+	drawable->u.copy.src_area.right = width;
+
+	drawable->u.copy.rop_descriptor = SPICE_ROPD_OP_PUT;
+	drawable->u.copy.scale_mode = 0;
+	drawable->u.copy.mask.flags = 0;
+	drawable->u.copy.mask.pos.x = 0;
+	drawable->u.copy.mask.pos.y = 0;
+	drawable->u.copy.mask.bitmap = 0;
+
+	drawable->u.copy.src_bitmap = qxl_bo_physical_address(qdev, image_bo, 0);
+	qxl_release_unmap(qdev, release, &drawable->release_info);
+	qxl_release_add_res(qdev, release, image_bo);
+	qxl_bo_unreserve(image_bo);
+	qxl_bo_unref(&image_bo);
+	clips_ptr = clips;
+	for (i = 0; i < num_clips; i++, clips_ptr += inc) {
+		rects[i].left   = clips_ptr->x1;
+		rects[i].right  = clips_ptr->x2;
+		rects[i].top    = clips_ptr->y1;
+		rects[i].bottom = clips_ptr->y2;
+	}
+	qxl_bo_kunmap(clips_bo);
+	qxl_bo_unreserve(clips_bo);
+	qxl_bo_unref(&clips_bo);
+
+	qxl_fence_releaseable(qdev, release);
+	qxl_push_command_ring_release(qdev, release, QXL_CMD_DRAW, false);
+	qxl_release_unreserve(qdev, release);
+	return;
+
+out_unref:
+	qxl_release_unreserve(qdev, release);
+	qxl_release_free(qdev, release);
+}
+
+void qxl_draw_copyarea(struct qxl_device *qdev,
+		       u32 width, u32 height,
+		       u32 sx, u32 sy,
+		       u32 dx, u32 dy)
+{
+	struct qxl_drawable *drawable;
+	struct qxl_rect rect;
+	struct qxl_release *release;
+	int ret;
+
+	rect.left = dx;
+	rect.top = dy;
+	rect.right = dx + width;
+	rect.bottom = dy + height;
+	ret = make_drawable(qdev, 0, QXL_COPY_BITS, &rect, &release);
+	if (ret)
+		return;
+
+	drawable = (struct qxl_drawable *)qxl_release_map(qdev, release);
+	drawable->u.copy_bits.src_pos.x = sx;
+	drawable->u.copy_bits.src_pos.y = sy;
+
+	qxl_release_unmap(qdev, release, &drawable->release_info);
+	qxl_fence_releaseable(qdev, release);
+	qxl_push_command_ring_release(qdev, release, QXL_CMD_DRAW, false);
+	qxl_release_unreserve(qdev, release);
+}
+
+void qxl_draw_fill(struct qxl_draw_fill *qxl_draw_fill_rec)
+{
+	struct qxl_device *qdev = qxl_draw_fill_rec->qdev;
+	struct qxl_rect rect = qxl_draw_fill_rec->rect;
+	uint32_t color = qxl_draw_fill_rec->color;
+	uint16_t rop = qxl_draw_fill_rec->rop;
+	struct qxl_drawable *drawable;
+	struct qxl_release *release;
+	int ret;
+
+	ret = make_drawable(qdev, 0, QXL_DRAW_FILL, &rect, &release);
+	if (ret)
+		return;
+
+	drawable = (struct qxl_drawable *)qxl_release_map(qdev, release);
+	drawable->u.fill.brush.type = SPICE_BRUSH_TYPE_SOLID;
+	drawable->u.fill.brush.u.color = color;
+	drawable->u.fill.rop_descriptor = rop;
+	drawable->u.fill.mask.flags = 0;
+	drawable->u.fill.mask.pos.x = 0;
+	drawable->u.fill.mask.pos.y = 0;
+	drawable->u.fill.mask.bitmap = 0;
+
+	qxl_release_unmap(qdev, release, &drawable->release_info);
+	qxl_fence_releaseable(qdev, release);
+	qxl_push_command_ring_release(qdev, release, QXL_CMD_DRAW, false);
+	qxl_release_unreserve(qdev, release);
+}
diff --git a/drivers/gpu/drm/qxl/qxl_drv.c b/drivers/gpu/drm/qxl/qxl_drv.c
new file mode 100644
index 000000000000..aa291d8a98a2
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_drv.c
@@ -0,0 +1,145 @@
+/* vim: set ts=8 sw=8 tw=78 ai noexpandtab */
+/* qxl_drv.c -- QXL driver -*- linux-c -*-
+ *
+ * Copyright 2011 Red Hat, Inc.
+ * All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * VA LINUX SYSTEMS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors:
+ *    Dave Airlie <airlie@redhat.com>
+ *    Alon Levy <alevy@redhat.com>
+ */
+
+#include <linux/module.h>
+#include <linux/console.h>
+
+#include "drmP.h"
+#include "drm/drm.h"
+
+#include "qxl_drv.h"
+
+extern int qxl_max_ioctls;
+static DEFINE_PCI_DEVICE_TABLE(pciidlist) = {
+	{ 0x1b36, 0x100, PCI_ANY_ID, PCI_ANY_ID, PCI_CLASS_DISPLAY_VGA << 8,
+	  0xffff00, 0 },
+	{ 0x1b36, 0x100, PCI_ANY_ID, PCI_ANY_ID, PCI_CLASS_DISPLAY_OTHER << 8,
+	  0xffff00, 0 },
+	{ 0, 0, 0 },
+};
+MODULE_DEVICE_TABLE(pci, pciidlist);
+
+static int qxl_modeset = -1;
+
+MODULE_PARM_DESC(modeset, "Disable/Enable modesetting");
+module_param_named(modeset, qxl_modeset, int, 0400);
+
+static struct drm_driver qxl_driver;
+static struct pci_driver qxl_pci_driver;
+
+static int
+qxl_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	if (pdev->revision < 4) {
+		DRM_ERROR("qxl too old, doesn't support client_monitors_config,"
+			  " use xf86-video-qxl in user mode");
+		return -EINVAL; /* TODO: ENODEV ? */
+	}
+	return drm_get_pci_dev(pdev, ent, &qxl_driver);
+}
+
+static void
+qxl_pci_remove(struct pci_dev *pdev)
+{
+	struct drm_device *dev = pci_get_drvdata(pdev);
+
+	drm_put_dev(dev);
+}
+
+static struct pci_driver qxl_pci_driver = {
+	 .name = DRIVER_NAME,
+	 .id_table = pciidlist,
+	 .probe = qxl_pci_probe,
+	 .remove = qxl_pci_remove,
+};
+
+static const struct file_operations qxl_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.mmap = qxl_mmap,
+};
+
+static struct drm_driver qxl_driver = {
+	.driver_features = DRIVER_GEM | DRIVER_MODESET |
+			   DRIVER_HAVE_IRQ | DRIVER_IRQ_SHARED,
+	.dev_priv_size = 0,
+	.load = qxl_driver_load,
+	.unload = qxl_driver_unload,
+
+	.dumb_create = qxl_mode_dumb_create,
+	.dumb_map_offset = qxl_mode_dumb_mmap,
+	.dumb_destroy = qxl_mode_dumb_destroy,
+#if defined(CONFIG_DEBUG_FS)
+	.debugfs_init = qxl_debugfs_init,
+	.debugfs_cleanup = qxl_debugfs_takedown,
+#endif
+	.gem_init_object = qxl_gem_object_init,
+	.gem_free_object = qxl_gem_object_free,
+	.gem_open_object = qxl_gem_object_open,
+	.gem_close_object = qxl_gem_object_close,
+	.fops = &qxl_fops,
+	.ioctls = qxl_ioctls,
+	.irq_handler = qxl_irq_handler,
+	.name = DRIVER_NAME,
+	.desc = DRIVER_DESC,
+	.date = DRIVER_DATE,
+	.major = 0,
+	.minor = 1,
+	.patchlevel = 0,
+};
+
+static int __init qxl_init(void)
+{
+#ifdef CONFIG_VGA_CONSOLE
+	if (vgacon_text_force() && qxl_modeset == -1)
+		return -EINVAL;
+#endif
+
+	if (qxl_modeset == 0)
+		return -EINVAL;
+	qxl_driver.num_ioctls = qxl_max_ioctls;
+	return drm_pci_init(&qxl_driver, &qxl_pci_driver);
+}
+
+static void __exit qxl_exit(void)
+{
+	drm_pci_exit(&qxl_driver, &qxl_pci_driver);
+}
+
+module_init(qxl_init);
+module_exit(qxl_exit);
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL and additional rights");
diff --git a/drivers/gpu/drm/qxl/qxl_drv.h b/drivers/gpu/drm/qxl/qxl_drv.h
new file mode 100644
index 000000000000..52b582c211da
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_drv.h
@@ -0,0 +1,566 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Dave Airlie
+ *          Alon Levy
+ */
+
+
+#ifndef QXL_DRV_H
+#define QXL_DRV_H
+
+/*
+ * Definitions taken from spice-protocol, plus kernel driver specific bits.
+ */
+
+#include <linux/workqueue.h>
+#include <linux/firmware.h>
+#include <linux/platform_device.h>
+
+#include "drmP.h"
+#include "drm_crtc.h"
+#include <ttm/ttm_bo_api.h>
+#include <ttm/ttm_bo_driver.h>
+#include <ttm/ttm_placement.h>
+#include <ttm/ttm_module.h>
+
+#include <drm/qxl_drm.h>
+#include "qxl_dev.h"
+
+#define DRIVER_AUTHOR		"Dave Airlie"
+
+#define DRIVER_NAME		"qxl"
+#define DRIVER_DESC		"RH QXL"
+#define DRIVER_DATE		"20120117"
+
+#define DRIVER_MAJOR 0
+#define DRIVER_MINOR 1
+#define DRIVER_PATCHLEVEL 0
+
+#define QXL_NUM_OUTPUTS 1
+
+#define QXL_DEBUGFS_MAX_COMPONENTS		32
+
+extern int qxl_log_level;
+
+enum {
+	QXL_INFO_LEVEL = 1,
+	QXL_DEBUG_LEVEL = 2,
+};
+
+#define QXL_INFO(qdev, fmt, ...) do { \
+		if (qxl_log_level >= QXL_INFO_LEVEL) {	\
+			qxl_io_log(qdev, fmt, __VA_ARGS__); \
+		}	\
+	} while (0)
+#define QXL_DEBUG(qdev, fmt, ...) do { \
+		if (qxl_log_level >= QXL_DEBUG_LEVEL) {	\
+			qxl_io_log(qdev, fmt, __VA_ARGS__); \
+		}	\
+	} while (0)
+#define QXL_INFO_ONCE(qdev, fmt, ...) do { \
+		static int done;		\
+		if (!done) {			\
+			done = 1;			\
+			QXL_INFO(qdev, fmt, __VA_ARGS__);	\
+		}						\
+	} while (0)
+
+#define DRM_FILE_OFFSET 0x100000000ULL
+#define DRM_FILE_PAGE_OFFSET (DRM_FILE_OFFSET >> PAGE_SHIFT)
+
+#define QXL_INTERRUPT_MASK (\
+	QXL_INTERRUPT_DISPLAY |\
+	QXL_INTERRUPT_CURSOR |\
+	QXL_INTERRUPT_IO_CMD |\
+	QXL_INTERRUPT_CLIENT_MONITORS_CONFIG)
+
+struct qxl_fence {
+	struct qxl_device *qdev;
+	uint32_t num_active_releases;
+	uint32_t *release_ids;
+	struct radix_tree_root tree;
+};
+
+struct qxl_bo {
+	/* Protected by gem.mutex */
+	struct list_head		list;
+	/* Protected by tbo.reserved */
+	u32				placements[3];
+	struct ttm_placement		placement;
+	struct ttm_buffer_object	tbo;
+	struct ttm_bo_kmap_obj		kmap;
+	unsigned			pin_count;
+	void				*kptr;
+	int                             type;
+	/* Constant after initialization */
+	struct drm_gem_object		gem_base;
+	bool is_primary; /* is this now a primary surface */
+	bool hw_surf_alloc;
+	struct qxl_surface surf;
+	uint32_t surface_id;
+	struct qxl_fence fence; /* per bo fence  - list of releases */
+	struct qxl_release *surf_create;
+	atomic_t reserve_count;
+};
+#define gem_to_qxl_bo(gobj) container_of((gobj), struct qxl_bo, gem_base)
+
+struct qxl_gem {
+	struct mutex		mutex;
+	struct list_head	objects;
+};
+
+struct qxl_bo_list {
+	struct list_head lhead;
+	struct qxl_bo *bo;
+};
+
+struct qxl_reloc_list {
+	struct list_head bos;
+};
+
+struct qxl_crtc {
+	struct drm_crtc base;
+	int cur_x;
+	int cur_y;
+};
+
+struct qxl_output {
+	int index;
+	struct drm_connector base;
+	struct drm_encoder enc;
+};
+
+struct qxl_framebuffer {
+	struct drm_framebuffer base;
+	struct drm_gem_object *obj;
+};
+
+#define to_qxl_crtc(x) container_of(x, struct qxl_crtc, base)
+#define drm_connector_to_qxl_output(x) container_of(x, struct qxl_output, base)
+#define drm_encoder_to_qxl_output(x) container_of(x, struct qxl_output, base)
+#define to_qxl_framebuffer(x) container_of(x, struct qxl_framebuffer, base)
+
+struct qxl_mman {
+	struct ttm_bo_global_ref        bo_global_ref;
+	struct drm_global_reference	mem_global_ref;
+	bool				mem_global_referenced;
+	struct ttm_bo_device		bdev;
+};
+
+struct qxl_mode_info {
+	int num_modes;
+	struct qxl_mode *modes;
+	bool mode_config_initialized;
+
+	/* pointer to fbdev info structure */
+	struct qxl_fbdev *qfbdev;
+};
+
+
+struct qxl_memslot {
+	uint8_t		generation;
+	uint64_t	start_phys_addr;
+	uint64_t	end_phys_addr;
+	uint64_t	high_bits;
+};
+
+enum {
+	QXL_RELEASE_DRAWABLE,
+	QXL_RELEASE_SURFACE_CMD,
+	QXL_RELEASE_CURSOR_CMD,
+};
+
+/* drm_ prefix to differentiate from qxl_release_info in
+ * spice-protocol/qxl_dev.h */
+#define QXL_MAX_RES 96
+struct qxl_release {
+	int id;
+	int type;
+	int bo_count;
+	uint32_t release_offset;
+	uint32_t surface_release_id;
+	struct qxl_bo *bos[QXL_MAX_RES];
+};
+
+struct qxl_fb_image {
+	struct qxl_device *qdev;
+	uint32_t pseudo_palette[16];
+	struct fb_image fb_image;
+	uint32_t visual;
+};
+
+struct qxl_draw_fill {
+	struct qxl_device *qdev;
+	struct qxl_rect rect;
+	uint32_t color;
+	uint16_t rop;
+};
+
+/*
+ * Debugfs
+ */
+struct qxl_debugfs {
+	struct drm_info_list	*files;
+	unsigned		num_files;
+};
+
+int qxl_debugfs_add_files(struct qxl_device *rdev,
+			     struct drm_info_list *files,
+			     unsigned nfiles);
+int qxl_debugfs_fence_init(struct qxl_device *rdev);
+void qxl_debugfs_remove_files(struct qxl_device *qdev);
+
+struct qxl_device;
+
+struct qxl_device {
+	struct device			*dev;
+	struct drm_device		*ddev;
+	struct pci_dev			*pdev;
+	unsigned long flags;
+
+	resource_size_t vram_base, vram_size;
+	resource_size_t surfaceram_base, surfaceram_size;
+	resource_size_t rom_base, rom_size;
+	struct qxl_rom *rom;
+
+	struct qxl_mode *modes;
+	struct qxl_bo *monitors_config_bo;
+	struct qxl_monitors_config *monitors_config;
+
+	/* last received client_monitors_config */
+	struct qxl_monitors_config *client_monitors_config;
+
+	int io_base;
+	void *ram;
+	struct qxl_mman		mman;
+	struct qxl_gem		gem;
+	struct qxl_mode_info mode_info;
+
+	/*
+	 * last created framebuffer with fb_create
+	 * only used by debugfs dumbppm
+	 */
+	struct qxl_framebuffer *active_user_framebuffer;
+
+	struct fb_info			*fbdev_info;
+	struct qxl_framebuffer	*fbdev_qfb;
+	void *ram_physical;
+
+	struct qxl_ring *release_ring;
+	struct qxl_ring *command_ring;
+	struct qxl_ring *cursor_ring;
+
+	struct qxl_ram_header *ram_header;
+	bool mode_set;
+
+	bool primary_created;
+
+	struct qxl_memslot	*mem_slots;
+	uint8_t		n_mem_slots;
+
+	uint8_t		main_mem_slot;
+	uint8_t		surfaces_mem_slot;
+	uint8_t		slot_id_bits;
+	uint8_t		slot_gen_bits;
+	uint64_t	va_slot_mask;
+
+	struct idr	release_idr;
+	spinlock_t release_idr_lock;
+	struct mutex	async_io_mutex;
+	unsigned int last_sent_io_cmd;
+
+	/* interrupt handling */
+	atomic_t irq_received;
+	atomic_t irq_received_display;
+	atomic_t irq_received_cursor;
+	atomic_t irq_received_io_cmd;
+	unsigned irq_received_error;
+	wait_queue_head_t display_event;
+	wait_queue_head_t cursor_event;
+	wait_queue_head_t io_cmd_event;
+	struct work_struct client_monitors_config_work;
+
+	/* debugfs */
+	struct qxl_debugfs	debugfs[QXL_DEBUGFS_MAX_COMPONENTS];
+	unsigned		debugfs_count;
+
+	struct mutex		update_area_mutex;
+
+	struct idr	surf_id_idr;
+	spinlock_t surf_id_idr_lock;
+	int last_alloced_surf_id;
+
+	struct mutex surf_evict_mutex;
+	struct io_mapping *vram_mapping;
+	struct io_mapping *surface_mapping;
+
+	/* */
+	struct mutex release_mutex;
+	struct qxl_bo *current_release_bo[3];
+	int current_release_bo_offset[3];
+
+	struct workqueue_struct *gc_queue;
+	struct work_struct gc_work;
+
+};
+
+/* forward declaration for QXL_INFO_IO */
+void qxl_io_log(struct qxl_device *qdev, const char *fmt, ...);
+
+extern struct drm_ioctl_desc qxl_ioctls[];
+extern int qxl_max_ioctl;
+
+int qxl_driver_load(struct drm_device *dev, unsigned long flags);
+int qxl_driver_unload(struct drm_device *dev);
+
+int qxl_modeset_init(struct qxl_device *qdev);
+void qxl_modeset_fini(struct qxl_device *qdev);
+
+int qxl_bo_init(struct qxl_device *qdev);
+void qxl_bo_fini(struct qxl_device *qdev);
+
+struct qxl_ring *qxl_ring_create(struct qxl_ring_header *header,
+				 int element_size,
+				 int n_elements,
+				 int prod_notify,
+				 bool set_prod_notify,
+				 wait_queue_head_t *push_event);
+void qxl_ring_free(struct qxl_ring *ring);
+
+static inline void *
+qxl_fb_virtual_address(struct qxl_device *qdev, unsigned long physical)
+{
+	QXL_INFO(qdev, "not implemented (%lu)\n", physical);
+	return 0;
+}
+
+static inline uint64_t
+qxl_bo_physical_address(struct qxl_device *qdev, struct qxl_bo *bo,
+			unsigned long offset)
+{
+	int slot_id = bo->type == QXL_GEM_DOMAIN_VRAM ? qdev->main_mem_slot : qdev->surfaces_mem_slot;
+	struct qxl_memslot *slot = &(qdev->mem_slots[slot_id]);
+
+	/* TODO - need to hold one of the locks to read tbo.offset */
+	return slot->high_bits | (bo->tbo.offset + offset);
+}
+
+/* qxl_fb.c */
+#define QXLFB_CONN_LIMIT 1
+
+int qxl_fbdev_init(struct qxl_device *qdev);
+void qxl_fbdev_fini(struct qxl_device *qdev);
+int qxl_get_handle_for_primary_fb(struct qxl_device *qdev,
+				  struct drm_file *file_priv,
+				  uint32_t *handle);
+
+/* qxl_display.c */
+int
+qxl_framebuffer_init(struct drm_device *dev,
+		     struct qxl_framebuffer *rfb,
+		     struct drm_mode_fb_cmd2 *mode_cmd,
+		     struct drm_gem_object *obj);
+void qxl_display_read_client_monitors_config(struct qxl_device *qdev);
+void qxl_send_monitors_config(struct qxl_device *qdev);
+
+/* used by qxl_debugfs only */
+void qxl_crtc_set_from_monitors_config(struct qxl_device *qdev);
+void qxl_alloc_client_monitors_config(struct qxl_device *qdev, unsigned count);
+
+/* qxl_gem.c */
+int qxl_gem_init(struct qxl_device *qdev);
+void qxl_gem_fini(struct qxl_device *qdev);
+int qxl_gem_object_create(struct qxl_device *qdev, int size,
+			  int alignment, int initial_domain,
+			  bool discardable, bool kernel,
+			  struct qxl_surface *surf,
+			  struct drm_gem_object **obj);
+int qxl_gem_object_pin(struct drm_gem_object *obj, uint32_t pin_domain,
+			  uint64_t *gpu_addr);
+void qxl_gem_object_unpin(struct drm_gem_object *obj);
+int qxl_gem_object_create_with_handle(struct qxl_device *qdev,
+				      struct drm_file *file_priv,
+				      u32 domain,
+				      size_t size,
+				      struct qxl_surface *surf,
+				      struct qxl_bo **qobj,
+				      uint32_t *handle);
+int qxl_gem_object_init(struct drm_gem_object *obj);
+void qxl_gem_object_free(struct drm_gem_object *gobj);
+int qxl_gem_object_open(struct drm_gem_object *obj, struct drm_file *file_priv);
+void qxl_gem_object_close(struct drm_gem_object *obj,
+			  struct drm_file *file_priv);
+void qxl_bo_force_delete(struct qxl_device *qdev);
+int qxl_bo_kmap(struct qxl_bo *bo, void **ptr);
+
+/* qxl_dumb.c */
+int qxl_mode_dumb_create(struct drm_file *file_priv,
+			 struct drm_device *dev,
+			 struct drm_mode_create_dumb *args);
+int qxl_mode_dumb_destroy(struct drm_file *file_priv,
+			  struct drm_device *dev,
+			  uint32_t handle);
+int qxl_mode_dumb_mmap(struct drm_file *filp,
+		       struct drm_device *dev,
+		       uint32_t handle, uint64_t *offset_p);
+
+
+/* qxl ttm */
+int qxl_ttm_init(struct qxl_device *qdev);
+void qxl_ttm_fini(struct qxl_device *qdev);
+int qxl_mmap(struct file *filp, struct vm_area_struct *vma);
+
+/* qxl image */
+
+int qxl_image_create(struct qxl_device *qdev,
+		     struct qxl_release *release,
+		     struct qxl_bo **image_bo,
+		     const uint8_t *data,
+		     int x, int y, int width, int height,
+		     int depth, int stride);
+void qxl_update_screen(struct qxl_device *qxl);
+
+/* qxl io operations (qxl_cmd.c) */
+
+void qxl_io_create_primary(struct qxl_device *qdev,
+			   unsigned width, unsigned height, unsigned offset,
+			   struct qxl_bo *bo);
+void qxl_io_destroy_primary(struct qxl_device *qdev);
+void qxl_io_memslot_add(struct qxl_device *qdev, uint8_t id);
+void qxl_io_notify_oom(struct qxl_device *qdev);
+
+int qxl_io_update_area(struct qxl_device *qdev, struct qxl_bo *surf,
+		       const struct qxl_rect *area);
+
+void qxl_io_reset(struct qxl_device *qdev);
+void qxl_io_monitors_config(struct qxl_device *qdev);
+int qxl_ring_push(struct qxl_ring *ring, const void *new_elt, bool interruptible);
+void qxl_io_flush_release(struct qxl_device *qdev);
+void qxl_io_flush_surfaces(struct qxl_device *qdev);
+
+int qxl_release_reserve(struct qxl_device *qdev,
+			struct qxl_release *release, bool no_wait);
+void qxl_release_unreserve(struct qxl_device *qdev,
+			   struct qxl_release *release);
+union qxl_release_info *qxl_release_map(struct qxl_device *qdev,
+					struct qxl_release *release);
+void qxl_release_unmap(struct qxl_device *qdev,
+		       struct qxl_release *release,
+		       union qxl_release_info *info);
+/*
+ * qxl_bo_add_resource.
+ *
+ */
+void qxl_bo_add_resource(struct qxl_bo *main_bo, struct qxl_bo *resource);
+
+int qxl_alloc_surface_release_reserved(struct qxl_device *qdev,
+				       enum qxl_surface_cmd_type surface_cmd_type,
+				       struct qxl_release *create_rel,
+				       struct qxl_release **release);
+int qxl_alloc_release_reserved(struct qxl_device *qdev, unsigned long size,
+			       int type, struct qxl_release **release,
+			       struct qxl_bo **rbo);
+int qxl_fence_releaseable(struct qxl_device *qdev,
+			  struct qxl_release *release);
+int
+qxl_push_command_ring_release(struct qxl_device *qdev, struct qxl_release *release,
+			      uint32_t type, bool interruptible);
+int
+qxl_push_cursor_ring_release(struct qxl_device *qdev, struct qxl_release *release,
+			     uint32_t type, bool interruptible);
+int qxl_alloc_bo_reserved(struct qxl_device *qdev, unsigned long size,
+			  struct qxl_bo **_bo);
+/* qxl drawing commands */
+
+void qxl_draw_opaque_fb(const struct qxl_fb_image *qxl_fb_image,
+			int stride /* filled in if 0 */);
+
+void qxl_draw_dirty_fb(struct qxl_device *qdev,
+		       struct qxl_framebuffer *qxl_fb,
+		       struct qxl_bo *bo,
+		       unsigned flags, unsigned color,
+		       struct drm_clip_rect *clips,
+		       unsigned num_clips, int inc);
+
+void qxl_draw_fill(struct qxl_draw_fill *qxl_draw_fill_rec);
+
+void qxl_draw_copyarea(struct qxl_device *qdev,
+		       u32 width, u32 height,
+		       u32 sx, u32 sy,
+		       u32 dx, u32 dy);
+
+uint64_t
+qxl_release_alloc(struct qxl_device *qdev, int type,
+		  struct qxl_release **ret);
+
+void qxl_release_free(struct qxl_device *qdev,
+		      struct qxl_release *release);
+void qxl_release_add_res(struct qxl_device *qdev,
+			 struct qxl_release *release,
+			 struct qxl_bo *bo);
+/* used by qxl_debugfs_release */
+struct qxl_release *qxl_release_from_id_locked(struct qxl_device *qdev,
+						   uint64_t id);
+
+bool qxl_queue_garbage_collect(struct qxl_device *qdev, bool flush);
+int qxl_garbage_collect(struct qxl_device *qdev);
+
+/* debugfs */
+
+int qxl_debugfs_init(struct drm_minor *minor);
+void qxl_debugfs_takedown(struct drm_minor *minor);
+
+/* qxl_irq.c */
+int qxl_irq_init(struct qxl_device *qdev);
+irqreturn_t qxl_irq_handler(DRM_IRQ_ARGS);
+
+/* qxl_fb.c */
+int qxl_fb_init(struct qxl_device *qdev);
+
+int qxl_debugfs_add_files(struct qxl_device *qdev,
+			  struct drm_info_list *files,
+			  unsigned nfiles);
+
+int qxl_surface_id_alloc(struct qxl_device *qdev,
+			 struct qxl_bo *surf);
+void qxl_surface_id_dealloc(struct qxl_device *qdev,
+			    uint32_t surface_id);
+int qxl_hw_surface_alloc(struct qxl_device *qdev,
+			 struct qxl_bo *surf,
+			 struct ttm_mem_reg *mem);
+int qxl_hw_surface_dealloc(struct qxl_device *qdev,
+			   struct qxl_bo *surf);
+
+int qxl_bo_check_id(struct qxl_device *qdev, struct qxl_bo *bo);
+
+struct qxl_drv_surface *
+qxl_surface_lookup(struct drm_device *dev, int surface_id);
+void qxl_surface_evict(struct qxl_device *qdev, struct qxl_bo *surf, bool freeing);
+int qxl_update_surface(struct qxl_device *qdev, struct qxl_bo *surf);
+
+/* qxl_fence.c */
+int qxl_fence_add_release(struct qxl_fence *qfence, uint32_t rel_id);
+int qxl_fence_remove_release(struct qxl_fence *qfence, uint32_t rel_id);
+int qxl_fence_init(struct qxl_device *qdev, struct qxl_fence *qfence);
+void qxl_fence_fini(struct qxl_fence *qfence);
+
+#endif
diff --git a/drivers/gpu/drm/qxl/qxl_dumb.c b/drivers/gpu/drm/qxl/qxl_dumb.c
new file mode 100644
index 000000000000..847c4ee798f7
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_dumb.c
@@ -0,0 +1,93 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Dave Airlie
+ *          Alon Levy
+ */
+
+#include "qxl_drv.h"
+#include "qxl_object.h"
+
+/* dumb ioctls implementation */
+
+int qxl_mode_dumb_create(struct drm_file *file_priv,
+			    struct drm_device *dev,
+			    struct drm_mode_create_dumb *args)
+{
+	struct qxl_device *qdev = dev->dev_private;
+	struct qxl_bo *qobj;
+	uint32_t handle;
+	int r;
+	struct qxl_surface surf;
+	uint32_t pitch, format;
+	pitch = args->width * ((args->bpp + 1) / 8);
+	args->size = pitch * args->height;
+	args->size = ALIGN(args->size, PAGE_SIZE);
+
+	switch (args->bpp) {
+	case 16:
+		format = SPICE_SURFACE_FMT_16_565;
+		break;
+	case 32:
+		format = SPICE_SURFACE_FMT_32_xRGB;
+		break;
+	default:
+		return -EINVAL;
+	}
+	  
+	surf.width = args->width;
+	surf.height = args->height;
+	surf.stride = pitch;
+	surf.format = format;
+	r = qxl_gem_object_create_with_handle(qdev, file_priv,
+					      QXL_GEM_DOMAIN_VRAM,
+					      args->size, &surf, &qobj,
+					      &handle);
+	if (r)
+		return r;
+	args->pitch = pitch;
+	args->handle = handle;
+	return 0;
+}
+
+int qxl_mode_dumb_destroy(struct drm_file *file_priv,
+			     struct drm_device *dev,
+			     uint32_t handle)
+{
+	return drm_gem_handle_delete(file_priv, handle);
+}
+
+int qxl_mode_dumb_mmap(struct drm_file *file_priv,
+		       struct drm_device *dev,
+		       uint32_t handle, uint64_t *offset_p)
+{
+	struct drm_gem_object *gobj;
+	struct qxl_bo *qobj;
+
+	BUG_ON(!offset_p);
+	gobj = drm_gem_object_lookup(dev, file_priv, handle);
+	if (gobj == NULL)
+		return -ENOENT;
+	qobj = gem_to_qxl_bo(gobj);
+	*offset_p = qxl_bo_mmap_offset(qobj);
+	drm_gem_object_unreference_unlocked(gobj);
+	return 0;
+}
diff --git a/drivers/gpu/drm/qxl/qxl_fb.c b/drivers/gpu/drm/qxl/qxl_fb.c
new file mode 100644
index 000000000000..b3c51275df5c
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_fb.c
@@ -0,0 +1,567 @@
+/*
+ * Copyright © 2013 Red Hat
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * Authors:
+ *     David Airlie
+ */
+#include <linux/module.h>
+#include <linux/fb.h>
+
+#include "drmP.h"
+#include "drm/drm.h"
+#include "drm/drm_crtc.h"
+#include "drm/drm_crtc_helper.h"
+#include "qxl_drv.h"
+
+#include "qxl_object.h"
+#include "drm_fb_helper.h"
+
+#define QXL_DIRTY_DELAY (HZ / 30)
+
+struct qxl_fbdev {
+	struct drm_fb_helper helper;
+	struct qxl_framebuffer	qfb;
+	struct list_head	fbdev_list;
+	struct qxl_device	*qdev;
+
+	void *shadow;
+	int size;
+
+	/* dirty memory logging */
+	struct {
+		spinlock_t lock;
+		bool active;
+		unsigned x1;
+		unsigned y1;
+		unsigned x2;
+		unsigned y2;
+	} dirty;
+};
+
+static void qxl_fb_image_init(struct qxl_fb_image *qxl_fb_image,
+			      struct qxl_device *qdev, struct fb_info *info,
+			      const struct fb_image *image)
+{
+	qxl_fb_image->qdev = qdev;
+	if (info) {
+		qxl_fb_image->visual = info->fix.visual;
+		if (qxl_fb_image->visual == FB_VISUAL_TRUECOLOR ||
+		    qxl_fb_image->visual == FB_VISUAL_DIRECTCOLOR)
+			memcpy(&qxl_fb_image->pseudo_palette,
+			       info->pseudo_palette,
+			       sizeof(qxl_fb_image->pseudo_palette));
+	} else {
+		 /* fallback */
+		if (image->depth == 1)
+			qxl_fb_image->visual = FB_VISUAL_MONO10;
+		else
+			qxl_fb_image->visual = FB_VISUAL_DIRECTCOLOR;
+	}
+	if (image) {
+		memcpy(&qxl_fb_image->fb_image, image,
+		       sizeof(qxl_fb_image->fb_image));
+	}
+}
+
+static void qxl_fb_dirty_flush(struct fb_info *info)
+{
+	struct qxl_fbdev *qfbdev = info->par;
+	struct qxl_device *qdev = qfbdev->qdev;
+	struct qxl_fb_image qxl_fb_image;
+	struct fb_image *image = &qxl_fb_image.fb_image;
+	u32 x1, x2, y1, y2;
+
+	/* TODO: hard coding 32 bpp */
+	int stride = qfbdev->qfb.base.pitches[0] * 4;
+
+	x1 = qfbdev->dirty.x1;
+	x2 = qfbdev->dirty.x2;
+	y1 = qfbdev->dirty.y1;
+	y2 = qfbdev->dirty.y2;
+	/*
+	 * we are using a shadow draw buffer, at qdev->surface0_shadow
+	 */
+	qxl_io_log(qdev, "dirty x[%d, %d], y[%d, %d]", x1, x2, y1, y2);
+	image->dx = x1;
+	image->dy = y1;
+	image->width = x2 - x1;
+	image->height = y2 - y1;
+	image->fg_color = 0xffffffff; /* unused, just to avoid uninitialized
+					 warnings */
+	image->bg_color = 0;
+	image->depth = 32;	     /* TODO: take from somewhere? */
+	image->cmap.start = 0;
+	image->cmap.len = 0;
+	image->cmap.red = NULL;
+	image->cmap.green = NULL;
+	image->cmap.blue = NULL;
+	image->cmap.transp = NULL;
+	image->data = qfbdev->shadow + (x1 * 4) + (stride * y1);
+
+	qxl_fb_image_init(&qxl_fb_image, qdev, info, NULL);
+	qxl_draw_opaque_fb(&qxl_fb_image, stride);
+	qfbdev->dirty.x1 = 0;
+	qfbdev->dirty.x2 = 0;
+	qfbdev->dirty.y1 = 0;
+	qfbdev->dirty.y2 = 0;
+}
+
+static void qxl_deferred_io(struct fb_info *info,
+			    struct list_head *pagelist)
+{
+	struct qxl_fbdev *qfbdev = info->par;
+	unsigned long start, end, min, max;
+	struct page *page;
+	int y1, y2;
+
+	min = ULONG_MAX;
+	max = 0;
+	list_for_each_entry(page, pagelist, lru) {
+		start = page->index << PAGE_SHIFT;
+		end = start + PAGE_SIZE - 1;
+		min = min(min, start);
+		max = max(max, end);
+	}
+
+	if (min < max) {
+		y1 = min / info->fix.line_length;
+		y2 = (max / info->fix.line_length) + 1;
+
+		/* TODO: add spin lock? */
+		/* spin_lock_irqsave(&qfbdev->dirty.lock, flags); */
+		qfbdev->dirty.x1 = 0;
+		qfbdev->dirty.y1 = y1;
+		qfbdev->dirty.x2 = info->var.xres;
+		qfbdev->dirty.y2 = y2;
+		/* spin_unlock_irqrestore(&qfbdev->dirty.lock, flags); */
+	}
+
+	qxl_fb_dirty_flush(info);
+};
+
+
+static struct fb_deferred_io qxl_defio = {
+	.delay		= QXL_DIRTY_DELAY,
+	.deferred_io	= qxl_deferred_io,
+};
+
+static void qxl_fb_fillrect(struct fb_info *info,
+			    const struct fb_fillrect *fb_rect)
+{
+	struct qxl_fbdev *qfbdev = info->par;
+	struct qxl_device *qdev = qfbdev->qdev;
+	struct qxl_rect rect;
+	uint32_t color;
+	int x = fb_rect->dx;
+	int y = fb_rect->dy;
+	int width = fb_rect->width;
+	int height = fb_rect->height;
+	uint16_t rop;
+	struct qxl_draw_fill qxl_draw_fill_rec;
+
+	if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
+	    info->fix.visual == FB_VISUAL_DIRECTCOLOR)
+		color = ((u32 *) (info->pseudo_palette))[fb_rect->color];
+	else
+		color = fb_rect->color;
+	rect.left = x;
+	rect.right = x + width;
+	rect.top = y;
+	rect.bottom = y + height;
+	switch (fb_rect->rop) {
+	case ROP_XOR:
+		rop = SPICE_ROPD_OP_XOR;
+		break;
+	case ROP_COPY:
+		rop = SPICE_ROPD_OP_PUT;
+		break;
+	default:
+		pr_err("qxl_fb_fillrect(): unknown rop, "
+		       "defaulting to SPICE_ROPD_OP_PUT\n");
+		rop = SPICE_ROPD_OP_PUT;
+	}
+	qxl_draw_fill_rec.qdev = qdev;
+	qxl_draw_fill_rec.rect = rect;
+	qxl_draw_fill_rec.color = color;
+	qxl_draw_fill_rec.rop = rop;
+	if (!drm_can_sleep()) {
+		qxl_io_log(qdev,
+			"%s: TODO use RCU, mysterious locks with spin_lock\n",
+			__func__);
+		return;
+	}
+	qxl_draw_fill(&qxl_draw_fill_rec);
+}
+
+static void qxl_fb_copyarea(struct fb_info *info,
+			    const struct fb_copyarea *region)
+{
+	struct qxl_fbdev *qfbdev = info->par;
+
+	qxl_draw_copyarea(qfbdev->qdev,
+			  region->width, region->height,
+			  region->sx, region->sy,
+			  region->dx, region->dy);
+}
+
+static void qxl_fb_imageblit_safe(struct qxl_fb_image *qxl_fb_image)
+{
+	qxl_draw_opaque_fb(qxl_fb_image, 0);
+}
+
+static void qxl_fb_imageblit(struct fb_info *info,
+			     const struct fb_image *image)
+{
+	struct qxl_fbdev *qfbdev = info->par;
+	struct qxl_device *qdev = qfbdev->qdev;
+	struct qxl_fb_image qxl_fb_image;
+
+	if (!drm_can_sleep()) {
+		/* we cannot do any ttm_bo allocation since that will fail on
+		 * ioremap_wc..__get_vm_area_node, so queue the work item
+		 * instead This can happen from printk inside an interrupt
+		 * context, i.e.: smp_apic_timer_interrupt..check_cpu_stall */
+		qxl_io_log(qdev,
+			"%s: TODO use RCU, mysterious locks with spin_lock\n",
+			   __func__);
+		return;
+	}
+
+	/* ensure proper order of rendering operations - TODO: must do this
+	 * for everything. */
+	qxl_fb_image_init(&qxl_fb_image, qfbdev->qdev, info, image);
+	qxl_fb_imageblit_safe(&qxl_fb_image);
+}
+
+int qxl_fb_init(struct qxl_device *qdev)
+{
+	return 0;
+}
+
+static struct fb_ops qxlfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = drm_fb_helper_check_var,
+	.fb_set_par = drm_fb_helper_set_par, /* TODO: copy vmwgfx */
+	.fb_fillrect = qxl_fb_fillrect,
+	.fb_copyarea = qxl_fb_copyarea,
+	.fb_imageblit = qxl_fb_imageblit,
+	.fb_pan_display = drm_fb_helper_pan_display,
+	.fb_blank = drm_fb_helper_blank,
+	.fb_setcmap = drm_fb_helper_setcmap,
+	.fb_debug_enter = drm_fb_helper_debug_enter,
+	.fb_debug_leave = drm_fb_helper_debug_leave,
+};
+
+static void qxlfb_destroy_pinned_object(struct drm_gem_object *gobj)
+{
+	struct qxl_bo *qbo = gem_to_qxl_bo(gobj);
+	int ret;
+
+	ret = qxl_bo_reserve(qbo, false);
+	if (likely(ret == 0)) {
+		qxl_bo_kunmap(qbo);
+		qxl_bo_unpin(qbo);
+		qxl_bo_unreserve(qbo);
+	}
+	drm_gem_object_unreference_unlocked(gobj);
+}
+
+int qxl_get_handle_for_primary_fb(struct qxl_device *qdev,
+				  struct drm_file *file_priv,
+				  uint32_t *handle)
+{
+	int r;
+	struct drm_gem_object *gobj = qdev->fbdev_qfb->obj;
+
+	BUG_ON(!gobj);
+	/* drm_get_handle_create adds a reference - good */
+	r = drm_gem_handle_create(file_priv, gobj, handle);
+	if (r)
+		return r;
+	return 0;
+}
+
+static int qxlfb_create_pinned_object(struct qxl_fbdev *qfbdev,
+				      struct drm_mode_fb_cmd2 *mode_cmd,
+				      struct drm_gem_object **gobj_p)
+{
+	struct qxl_device *qdev = qfbdev->qdev;
+	struct drm_gem_object *gobj = NULL;
+	struct qxl_bo *qbo = NULL;
+	int ret;
+	int aligned_size, size;
+	int height = mode_cmd->height;
+	int bpp;
+	int depth;
+
+	drm_fb_get_bpp_depth(mode_cmd->pixel_format, &bpp, &depth);
+
+	size = mode_cmd->pitches[0] * height;
+	aligned_size = ALIGN(size, PAGE_SIZE);
+	/* TODO: unallocate and reallocate surface0 for real. Hack to just
+	 * have a large enough surface0 for 1024x768 Xorg 32bpp mode */
+	ret = qxl_gem_object_create(qdev, aligned_size, 0,
+				    QXL_GEM_DOMAIN_SURFACE,
+				    false, /* is discardable */
+				    false, /* is kernel (false means device) */
+				    NULL,
+				    &gobj);
+	if (ret) {
+		pr_err("failed to allocate framebuffer (%d)\n",
+		       aligned_size);
+		return -ENOMEM;
+	}
+	qbo = gem_to_qxl_bo(gobj);
+
+	qbo->surf.width = mode_cmd->width;
+	qbo->surf.height = mode_cmd->height;
+	qbo->surf.stride = mode_cmd->pitches[0];
+	qbo->surf.format = SPICE_SURFACE_FMT_32_xRGB;
+	ret = qxl_bo_reserve(qbo, false);
+	if (unlikely(ret != 0))
+		goto out_unref;
+	ret = qxl_bo_pin(qbo, QXL_GEM_DOMAIN_SURFACE, NULL);
+	if (ret) {
+		qxl_bo_unreserve(qbo);
+		goto out_unref;
+	}
+	ret = qxl_bo_kmap(qbo, NULL);
+	qxl_bo_unreserve(qbo); /* unreserve, will be mmaped */
+	if (ret)
+		goto out_unref;
+
+	*gobj_p = gobj;
+	return 0;
+out_unref:
+	qxlfb_destroy_pinned_object(gobj);
+	*gobj_p = NULL;
+	return ret;
+}
+
+static int qxlfb_create(struct qxl_fbdev *qfbdev,
+			struct drm_fb_helper_surface_size *sizes)
+{
+	struct qxl_device *qdev = qfbdev->qdev;
+	struct fb_info *info;
+	struct drm_framebuffer *fb = NULL;
+	struct drm_mode_fb_cmd2 mode_cmd;
+	struct drm_gem_object *gobj = NULL;
+	struct qxl_bo *qbo = NULL;
+	struct device *device = &qdev->pdev->dev;
+	int ret;
+	int size;
+	int bpp = sizes->surface_bpp;
+	int depth = sizes->surface_depth;
+	void *shadow;
+
+	mode_cmd.width = sizes->surface_width;
+	mode_cmd.height = sizes->surface_height;
+
+	mode_cmd.pitches[0] = ALIGN(mode_cmd.width * ((bpp + 1) / 8), 64);
+	mode_cmd.pixel_format = drm_mode_legacy_fb_format(bpp, depth);
+
+	ret = qxlfb_create_pinned_object(qfbdev, &mode_cmd, &gobj);
+	qbo = gem_to_qxl_bo(gobj);
+	QXL_INFO(qdev, "%s: %dx%d %d\n", __func__, mode_cmd.width,
+		 mode_cmd.height, mode_cmd.pitches[0]);
+
+	shadow = vmalloc(mode_cmd.pitches[0] * mode_cmd.height);
+	/* TODO: what's the usual response to memory allocation errors? */
+	BUG_ON(!shadow);
+	QXL_INFO(qdev,
+	"surface0 at gpu offset %lld, mmap_offset %lld (virt %p, shadow %p)\n",
+		 qxl_bo_gpu_offset(qbo),
+		 qxl_bo_mmap_offset(qbo),
+		 qbo->kptr,
+		 shadow);
+	size = mode_cmd.pitches[0] * mode_cmd.height;
+
+	info = framebuffer_alloc(0, device);
+	if (info == NULL) {
+		ret = -ENOMEM;
+		goto out_unref;
+	}
+
+	info->par = qfbdev;
+
+	qxl_framebuffer_init(qdev->ddev, &qfbdev->qfb, &mode_cmd, gobj);
+
+	fb = &qfbdev->qfb.base;
+
+	/* setup helper with fb data */
+	qfbdev->helper.fb = fb;
+	qfbdev->helper.fbdev = info;
+	qfbdev->shadow = shadow;
+	strcpy(info->fix.id, "qxldrmfb");
+
+	drm_fb_helper_fill_fix(info, fb->pitches[0], fb->depth);
+
+	info->flags = FBINFO_DEFAULT | FBINFO_HWACCEL_COPYAREA | FBINFO_HWACCEL_FILLRECT;
+	info->fbops = &qxlfb_ops;
+
+	/*
+	 * TODO: using gobj->size in various places in this function. Not sure
+	 * what the difference between the different sizes is.
+	 */
+	info->fix.smem_start = qdev->vram_base; /* TODO - correct? */
+	info->fix.smem_len = gobj->size;
+	info->screen_base = qfbdev->shadow;
+	info->screen_size = gobj->size;
+
+	drm_fb_helper_fill_var(info, &qfbdev->helper, sizes->fb_width,
+			       sizes->fb_height);
+
+	/* setup aperture base/size for vesafb takeover */
+	info->apertures = alloc_apertures(1);
+	if (!info->apertures) {
+		ret = -ENOMEM;
+		goto out_unref;
+	}
+	info->apertures->ranges[0].base = qdev->ddev->mode_config.fb_base;
+	info->apertures->ranges[0].size = qdev->vram_size;
+
+	info->fix.mmio_start = 0;
+	info->fix.mmio_len = 0;
+
+	if (info->screen_base == NULL) {
+		ret = -ENOSPC;
+		goto out_unref;
+	}
+
+	ret = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (ret) {
+		ret = -ENOMEM;
+		goto out_unref;
+	}
+
+	info->fbdefio = &qxl_defio;
+	fb_deferred_io_init(info);
+
+	qdev->fbdev_info = info;
+	qdev->fbdev_qfb = &qfbdev->qfb;
+	DRM_INFO("fb mappable at 0x%lX, size %lu\n",  info->fix.smem_start, (unsigned long)info->screen_size);
+	DRM_INFO("fb: depth %d, pitch %d, width %d, height %d\n", fb->depth, fb->pitches[0], fb->width, fb->height);
+	return 0;
+
+out_unref:
+	if (qbo) {
+		ret = qxl_bo_reserve(qbo, false);
+		if (likely(ret == 0)) {
+			qxl_bo_kunmap(qbo);
+			qxl_bo_unpin(qbo);
+			qxl_bo_unreserve(qbo);
+		}
+	}
+	if (fb && ret) {
+		drm_gem_object_unreference(gobj);
+		drm_framebuffer_cleanup(fb);
+		kfree(fb);
+	}
+	drm_gem_object_unreference(gobj);
+	return ret;
+}
+
+static int qxl_fb_find_or_create_single(
+		struct drm_fb_helper *helper,
+		struct drm_fb_helper_surface_size *sizes)
+{
+	struct qxl_fbdev *qfbdev = (struct qxl_fbdev *)helper;
+	int new_fb = 0;
+	int ret;
+
+	if (!helper->fb) {
+		ret = qxlfb_create(qfbdev, sizes);
+		if (ret)
+			return ret;
+		new_fb = 1;
+	}
+	return new_fb;
+}
+
+static int qxl_fbdev_destroy(struct drm_device *dev, struct qxl_fbdev *qfbdev)
+{
+	struct fb_info *info;
+	struct qxl_framebuffer *qfb = &qfbdev->qfb;
+
+	if (qfbdev->helper.fbdev) {
+		info = qfbdev->helper.fbdev;
+
+		unregister_framebuffer(info);
+		framebuffer_release(info);
+	}
+	if (qfb->obj) {
+		qxlfb_destroy_pinned_object(qfb->obj);
+		qfb->obj = NULL;
+	}
+	drm_fb_helper_fini(&qfbdev->helper);
+	vfree(qfbdev->shadow);
+	drm_framebuffer_cleanup(&qfb->base);
+
+	return 0;
+}
+
+static struct drm_fb_helper_funcs qxl_fb_helper_funcs = {
+	/* TODO
+	.gamma_set = qxl_crtc_fb_gamma_set,
+	.gamma_get = qxl_crtc_fb_gamma_get,
+	*/
+	.fb_probe = qxl_fb_find_or_create_single,
+};
+
+int qxl_fbdev_init(struct qxl_device *qdev)
+{
+	struct qxl_fbdev *qfbdev;
+	int bpp_sel = 32; /* TODO: parameter from somewhere? */
+	int ret;
+
+	qfbdev = kzalloc(sizeof(struct qxl_fbdev), GFP_KERNEL);
+	if (!qfbdev)
+		return -ENOMEM;
+
+	qfbdev->qdev = qdev;
+	qdev->mode_info.qfbdev = qfbdev;
+	qfbdev->helper.funcs = &qxl_fb_helper_funcs;
+
+	ret = drm_fb_helper_init(qdev->ddev, &qfbdev->helper,
+				 1 /* num_crtc - QXL supports just 1 */,
+				 QXLFB_CONN_LIMIT);
+	if (ret) {
+		kfree(qfbdev);
+		return ret;
+	}
+
+	drm_fb_helper_single_add_all_connectors(&qfbdev->helper);
+	drm_fb_helper_initial_config(&qfbdev->helper, bpp_sel);
+	return 0;
+}
+
+void qxl_fbdev_fini(struct qxl_device *qdev)
+{
+	if (!qdev->mode_info.qfbdev)
+		return;
+
+	qxl_fbdev_destroy(qdev->ddev, qdev->mode_info.qfbdev);
+	kfree(qdev->mode_info.qfbdev);
+	qdev->mode_info.qfbdev = NULL;
+}
+
+
diff --git a/drivers/gpu/drm/qxl/qxl_fence.c b/drivers/gpu/drm/qxl/qxl_fence.c
new file mode 100644
index 000000000000..63c6715ad385
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_fence.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Dave Airlie
+ *          Alon Levy
+ */
+
+
+#include "qxl_drv.h"
+
+/* QXL fencing-
+
+   When we submit operations to the GPU we pass a release reference to the GPU
+   with them, the release reference is then added to the release ring when
+   the GPU is finished with that particular operation and has removed it from
+   its tree.
+
+   So we have can have multiple outstanding non linear fences per object.
+
+   From a TTM POV we only care if the object has any outstanding releases on
+   it.
+
+   we wait until all outstanding releases are processeed.
+
+   sync object is just a list of release ids that represent that fence on
+   that buffer.
+
+   we just add new releases onto the sync object attached to the object.
+
+   This currently uses a radix tree to store the list of release ids.
+
+   For some reason every so often qxl hw fails to release, things go wrong.
+*/
+
+
+int qxl_fence_add_release(struct qxl_fence *qfence, uint32_t rel_id)
+{
+	struct qxl_bo *bo = container_of(qfence, struct qxl_bo, fence);
+
+	spin_lock(&bo->tbo.bdev->fence_lock);
+	radix_tree_insert(&qfence->tree, rel_id, qfence);
+	qfence->num_active_releases++;
+	spin_unlock(&bo->tbo.bdev->fence_lock);
+	return 0;
+}
+
+int qxl_fence_remove_release(struct qxl_fence *qfence, uint32_t rel_id)
+{
+	void *ret;
+	int retval = 0;
+	struct qxl_bo *bo = container_of(qfence, struct qxl_bo, fence);
+
+	spin_lock(&bo->tbo.bdev->fence_lock);
+
+	ret = radix_tree_delete(&qfence->tree, rel_id);
+	if (ret == qfence)
+		qfence->num_active_releases--;
+	else {
+		DRM_DEBUG("didn't find fence in radix tree for %d\n", rel_id);
+		retval = -ENOENT;
+	}
+	spin_unlock(&bo->tbo.bdev->fence_lock);
+	return retval;
+}
+
+
+int qxl_fence_init(struct qxl_device *qdev, struct qxl_fence *qfence)
+{
+	qfence->qdev = qdev;
+	qfence->num_active_releases = 0;
+	INIT_RADIX_TREE(&qfence->tree, GFP_ATOMIC);
+	return 0;
+}
+
+void qxl_fence_fini(struct qxl_fence *qfence)
+{
+	kfree(qfence->release_ids);
+	qfence->num_active_releases = 0;
+}
diff --git a/drivers/gpu/drm/qxl/qxl_gem.c b/drivers/gpu/drm/qxl/qxl_gem.c
new file mode 100644
index 000000000000..a235693aabba
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_gem.c
@@ -0,0 +1,149 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Dave Airlie
+ *          Alon Levy
+ */
+
+#include "drmP.h"
+#include "drm/drm.h"
+#include "qxl_drv.h"
+#include "qxl_object.h"
+
+int qxl_gem_object_init(struct drm_gem_object *obj)
+{
+	/* we do nothings here */
+	return 0;
+}
+
+void qxl_gem_object_free(struct drm_gem_object *gobj)
+{
+	struct qxl_bo *qobj = gem_to_qxl_bo(gobj);
+
+	if (qobj)
+		qxl_bo_unref(&qobj);
+}
+
+int qxl_gem_object_create(struct qxl_device *qdev, int size,
+			  int alignment, int initial_domain,
+			  bool discardable, bool kernel,
+			  struct qxl_surface *surf,
+			  struct drm_gem_object **obj)
+{
+	struct qxl_bo *qbo;
+	int r;
+
+	*obj = NULL;
+	/* At least align on page size */
+	if (alignment < PAGE_SIZE)
+		alignment = PAGE_SIZE;
+	r = qxl_bo_create(qdev, size, kernel, initial_domain, surf, &qbo);
+	if (r) {
+		if (r != -ERESTARTSYS)
+			DRM_ERROR(
+			"Failed to allocate GEM object (%d, %d, %u, %d)\n",
+				  size, initial_domain, alignment, r);
+		return r;
+	}
+	*obj = &qbo->gem_base;
+
+	mutex_lock(&qdev->gem.mutex);
+	list_add_tail(&qbo->list, &qdev->gem.objects);
+	mutex_unlock(&qdev->gem.mutex);
+
+	return 0;
+}
+
+int qxl_gem_object_create_with_handle(struct qxl_device *qdev,
+				      struct drm_file *file_priv,
+				      u32 domain,
+				      size_t size,
+				      struct qxl_surface *surf,
+				      struct qxl_bo **qobj,
+				      uint32_t *handle)
+{
+	struct drm_gem_object *gobj;
+	int r;
+
+	BUG_ON(!qobj);
+	BUG_ON(!handle);
+
+	r = qxl_gem_object_create(qdev, size, 0,
+				  domain,
+				  false, false, surf,
+				  &gobj);
+	if (r)
+		return -ENOMEM;
+	r = drm_gem_handle_create(file_priv, gobj, handle);
+	if (r)
+		return r;
+	/* drop reference from allocate - handle holds it now */
+	*qobj = gem_to_qxl_bo(gobj);
+	drm_gem_object_unreference_unlocked(gobj);
+	return 0;
+}
+
+int qxl_gem_object_pin(struct drm_gem_object *obj, uint32_t pin_domain,
+			  uint64_t *gpu_addr)
+{
+	struct qxl_bo *qobj = obj->driver_private;
+	int r;
+
+	r = qxl_bo_reserve(qobj, false);
+	if (unlikely(r != 0))
+		return r;
+	r = qxl_bo_pin(qobj, pin_domain, gpu_addr);
+	qxl_bo_unreserve(qobj);
+	return r;
+}
+
+void qxl_gem_object_unpin(struct drm_gem_object *obj)
+{
+	struct qxl_bo *qobj = obj->driver_private;
+	int r;
+
+	r = qxl_bo_reserve(qobj, false);
+	if (likely(r == 0)) {
+		qxl_bo_unpin(qobj);
+		qxl_bo_unreserve(qobj);
+	}
+}
+
+int qxl_gem_object_open(struct drm_gem_object *obj, struct drm_file *file_priv)
+{
+	return 0;
+}
+
+void qxl_gem_object_close(struct drm_gem_object *obj,
+			  struct drm_file *file_priv)
+{
+}
+
+int qxl_gem_init(struct qxl_device *qdev)
+{
+	INIT_LIST_HEAD(&qdev->gem.objects);
+	return 0;
+}
+
+void qxl_gem_fini(struct qxl_device *qdev)
+{
+	qxl_bo_force_delete(qdev);
+}
diff --git a/drivers/gpu/drm/qxl/qxl_image.c b/drivers/gpu/drm/qxl/qxl_image.c
new file mode 100644
index 000000000000..cf856206996b
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_image.c
@@ -0,0 +1,176 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Dave Airlie
+ *          Alon Levy
+ */
+
+#include <linux/gfp.h>
+#include <linux/slab.h>
+
+#include "qxl_drv.h"
+#include "qxl_object.h"
+
+static int
+qxl_image_create_helper(struct qxl_device *qdev,
+			struct qxl_release *release,
+			struct qxl_bo **image_bo,
+			const uint8_t *data,
+			int width, int height,
+			int depth, unsigned int hash,
+			int stride)
+{
+	struct qxl_image *image;
+	struct qxl_data_chunk *chunk;
+	int i;
+	int chunk_stride;
+	int linesize = width * depth / 8;
+	struct qxl_bo *chunk_bo;
+	int ret;
+	void *ptr;
+	/* Chunk */
+	/* FIXME: Check integer overflow */
+	/* TODO: variable number of chunks */
+	chunk_stride = stride; /* TODO: should use linesize, but it renders
+				  wrong (check the bitmaps are sent correctly
+				  first) */
+	ret = qxl_alloc_bo_reserved(qdev, sizeof(*chunk) + height * chunk_stride,
+				    &chunk_bo);
+	
+	ptr = qxl_bo_kmap_atomic_page(qdev, chunk_bo, 0);
+	chunk = ptr;
+	chunk->data_size = height * chunk_stride;
+	chunk->prev_chunk = 0;
+	chunk->next_chunk = 0;
+	qxl_bo_kunmap_atomic_page(qdev, chunk_bo, ptr);
+
+	{
+		void *k_data, *i_data;
+		int remain;
+		int page;
+		int size;
+		if (stride == linesize && chunk_stride == stride) {
+			remain = linesize * height;
+			page = 0;
+			i_data = (void *)data;
+
+			while (remain > 0) {
+				ptr = qxl_bo_kmap_atomic_page(qdev, chunk_bo, page << PAGE_SHIFT);
+
+				if (page == 0) {
+					chunk = ptr;
+					k_data = chunk->data;
+					size = PAGE_SIZE - offsetof(struct qxl_data_chunk, data);
+				} else {
+					k_data = ptr;
+					size = PAGE_SIZE;
+				}
+				size = min(size, remain);
+
+				memcpy(k_data, i_data, size);
+
+				qxl_bo_kunmap_atomic_page(qdev, chunk_bo, ptr);
+				i_data += size;
+				remain -= size;
+				page++;
+			}
+		} else {
+			unsigned page_base, page_offset, out_offset;
+			for (i = 0 ; i < height ; ++i) {
+				i_data = (void *)data + i * stride;
+				remain = linesize;
+				out_offset = offsetof(struct qxl_data_chunk, data) + i * chunk_stride;
+
+				while (remain > 0) {
+					page_base = out_offset & PAGE_MASK;
+					page_offset = offset_in_page(out_offset);
+					
+					size = min((int)(PAGE_SIZE - page_offset), remain);
+
+					ptr = qxl_bo_kmap_atomic_page(qdev, chunk_bo, page_base);
+					k_data = ptr + page_offset;
+					memcpy(k_data, i_data, size);
+					qxl_bo_kunmap_atomic_page(qdev, chunk_bo, ptr);
+					remain -= size;
+					i_data += size;
+					out_offset += size;
+				}
+			}
+		}
+	}
+
+
+	qxl_bo_kunmap(chunk_bo);
+
+	/* Image */
+	ret = qxl_alloc_bo_reserved(qdev, sizeof(*image), image_bo);
+
+	ptr = qxl_bo_kmap_atomic_page(qdev, *image_bo, 0);
+	image = ptr;
+
+	image->descriptor.id = 0;
+	image->descriptor.type = SPICE_IMAGE_TYPE_BITMAP;
+
+	image->descriptor.flags = 0;
+	image->descriptor.width = width;
+	image->descriptor.height = height;
+
+	switch (depth) {
+	case 1:
+		/* TODO: BE? check by arch? */
+		image->u.bitmap.format = SPICE_BITMAP_FMT_1BIT_BE;
+		break;
+	case 24:
+		image->u.bitmap.format = SPICE_BITMAP_FMT_24BIT;
+		break;
+	case 32:
+		image->u.bitmap.format = SPICE_BITMAP_FMT_32BIT;
+		break;
+	default:
+		DRM_ERROR("unsupported image bit depth\n");
+		return -EINVAL; /* TODO: cleanup */
+	}
+	image->u.bitmap.flags = QXL_BITMAP_TOP_DOWN;
+	image->u.bitmap.x = width;
+	image->u.bitmap.y = height;
+	image->u.bitmap.stride = chunk_stride;
+	image->u.bitmap.palette = 0;
+	image->u.bitmap.data = qxl_bo_physical_address(qdev, chunk_bo, 0);
+	qxl_release_add_res(qdev, release, chunk_bo);
+	qxl_bo_unreserve(chunk_bo);
+	qxl_bo_unref(&chunk_bo);
+
+	qxl_bo_kunmap_atomic_page(qdev, *image_bo, ptr);
+
+	return 0;
+}
+
+int qxl_image_create(struct qxl_device *qdev,
+		     struct qxl_release *release,
+		     struct qxl_bo **image_bo,
+		     const uint8_t *data,
+		     int x, int y, int width, int height,
+		     int depth, int stride)
+{
+	data += y * stride + x * (depth / 8);
+	return qxl_image_create_helper(qdev, release, image_bo, data,
+				       width, height, depth, 0, stride);
+}
diff --git a/drivers/gpu/drm/qxl/qxl_ioctl.c b/drivers/gpu/drm/qxl/qxl_ioctl.c
new file mode 100644
index 000000000000..04b64f9cbfdb
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_ioctl.c
@@ -0,0 +1,411 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Dave Airlie
+ *          Alon Levy
+ */
+
+#include "qxl_drv.h"
+#include "qxl_object.h"
+
+/*
+ * TODO: allocating a new gem(in qxl_bo) for each request.
+ * This is wasteful since bo's are page aligned.
+ */
+static int qxl_alloc_ioctl(struct drm_device *dev, void *data,
+			   struct drm_file *file_priv)
+{
+	struct qxl_device *qdev = dev->dev_private;
+	struct drm_qxl_alloc *qxl_alloc = data;
+	int ret;
+	struct qxl_bo *qobj;
+	uint32_t handle;
+	u32 domain = QXL_GEM_DOMAIN_VRAM;
+
+	if (qxl_alloc->size == 0) {
+		DRM_ERROR("invalid size %d\n", qxl_alloc->size);
+		return -EINVAL;
+	}
+	ret = qxl_gem_object_create_with_handle(qdev, file_priv,
+						domain,
+						qxl_alloc->size,
+						NULL,
+						&qobj, &handle);
+	if (ret) {
+		DRM_ERROR("%s: failed to create gem ret=%d\n",
+			  __func__, ret);
+		return -ENOMEM;
+	}
+	qxl_alloc->handle = handle;
+	return 0;
+}
+
+static int qxl_map_ioctl(struct drm_device *dev, void *data,
+			 struct drm_file *file_priv)
+{
+	struct qxl_device *qdev = dev->dev_private;
+	struct drm_qxl_map *qxl_map = data;
+
+	return qxl_mode_dumb_mmap(file_priv, qdev->ddev, qxl_map->handle,
+				  &qxl_map->offset);
+}
+
+/*
+ * dst must be validated, i.e. whole bo on vram/surfacesram (right now all bo's
+ * are on vram).
+ * *(dst + dst_off) = qxl_bo_physical_address(src, src_off)
+ */
+static void
+apply_reloc(struct qxl_device *qdev, struct qxl_bo *dst, uint64_t dst_off,
+	    struct qxl_bo *src, uint64_t src_off)
+{
+	void *reloc_page;
+
+	reloc_page = qxl_bo_kmap_atomic_page(qdev, dst, dst_off & PAGE_MASK);
+	*(uint64_t *)(reloc_page + (dst_off & ~PAGE_MASK)) = qxl_bo_physical_address(qdev,
+								     src, src_off);
+	qxl_bo_kunmap_atomic_page(qdev, dst, reloc_page);
+}
+
+static void
+apply_surf_reloc(struct qxl_device *qdev, struct qxl_bo *dst, uint64_t dst_off,
+		 struct qxl_bo *src)
+{
+	uint32_t id = 0;
+	void *reloc_page;
+
+	if (src && !src->is_primary)
+		id = src->surface_id;
+
+	reloc_page = qxl_bo_kmap_atomic_page(qdev, dst, dst_off & PAGE_MASK);
+	*(uint32_t *)(reloc_page + (dst_off & ~PAGE_MASK)) = id;
+	qxl_bo_kunmap_atomic_page(qdev, dst, reloc_page);
+}
+
+/* return holding the reference to this object */
+static struct qxl_bo *qxlhw_handle_to_bo(struct qxl_device *qdev,
+					 struct drm_file *file_priv, uint64_t handle,
+					 struct qxl_reloc_list *reloc_list)
+{
+	struct drm_gem_object *gobj;
+	struct qxl_bo *qobj;
+	int ret;
+
+	gobj = drm_gem_object_lookup(qdev->ddev, file_priv, handle);
+	if (!gobj) {
+		DRM_ERROR("bad bo handle %lld\n", handle);
+		return NULL;
+	}
+	qobj = gem_to_qxl_bo(gobj);
+
+	ret = qxl_bo_list_add(reloc_list, qobj);
+	if (ret)
+		return NULL;
+
+	return qobj;
+}
+
+/*
+ * Usage of execbuffer:
+ * Relocations need to take into account the full QXLDrawable size.
+ * However, the command as passed from user space must *not* contain the initial
+ * QXLReleaseInfo struct (first XXX bytes)
+ */
+static int qxl_execbuffer_ioctl(struct drm_device *dev, void *data,
+				struct drm_file *file_priv)
+{
+	struct qxl_device *qdev = dev->dev_private;
+	struct drm_qxl_execbuffer *execbuffer = data;
+	struct drm_qxl_command user_cmd;
+	int cmd_num;
+	struct qxl_bo *reloc_src_bo;
+	struct qxl_bo *reloc_dst_bo;
+	struct drm_qxl_reloc reloc;
+	void *fb_cmd;
+	int i, ret;
+	struct qxl_reloc_list reloc_list;
+	int unwritten;
+	uint32_t reloc_dst_offset;
+	INIT_LIST_HEAD(&reloc_list.bos);
+
+	for (cmd_num = 0; cmd_num < execbuffer->commands_num; ++cmd_num) {
+		struct qxl_release *release;
+		struct qxl_bo *cmd_bo;
+		int release_type;
+		struct drm_qxl_command *commands =
+			(struct drm_qxl_command *)execbuffer->commands;
+
+		if (DRM_COPY_FROM_USER(&user_cmd, &commands[cmd_num],
+				       sizeof(user_cmd)))
+			return -EFAULT;
+		switch (user_cmd.type) {
+		case QXL_CMD_DRAW:
+			release_type = QXL_RELEASE_DRAWABLE;
+			break;
+		case QXL_CMD_SURFACE:
+		case QXL_CMD_CURSOR:
+		default:
+			DRM_DEBUG("Only draw commands in execbuffers\n");
+			return -EINVAL;
+			break;
+		}
+
+		if (user_cmd.command_size > PAGE_SIZE - sizeof(union qxl_release_info))
+			return -EINVAL;
+
+		ret = qxl_alloc_release_reserved(qdev,
+						 sizeof(union qxl_release_info) +
+						 user_cmd.command_size,
+						 release_type,
+						 &release,
+						 &cmd_bo);
+		if (ret)
+			return ret;
+
+		/* TODO copy slow path code from i915 */
+		fb_cmd = qxl_bo_kmap_atomic_page(qdev, cmd_bo, (release->release_offset & PAGE_SIZE));
+		unwritten = __copy_from_user_inatomic_nocache(fb_cmd + sizeof(union qxl_release_info) + (release->release_offset & ~PAGE_SIZE), (void *)(unsigned long)user_cmd.command, user_cmd.command_size);
+		qxl_bo_kunmap_atomic_page(qdev, cmd_bo, fb_cmd);
+		if (unwritten) {
+			DRM_ERROR("got unwritten %d\n", unwritten);
+			qxl_release_unreserve(qdev, release);
+			qxl_release_free(qdev, release);
+			return -EFAULT;
+		}
+
+		for (i = 0 ; i < user_cmd.relocs_num; ++i) {
+			if (DRM_COPY_FROM_USER(&reloc,
+					       &((struct drm_qxl_reloc *)user_cmd.relocs)[i],
+					       sizeof(reloc))) {
+				qxl_bo_list_unreserve(&reloc_list, true);
+				qxl_release_unreserve(qdev, release);
+				qxl_release_free(qdev, release);
+				return -EFAULT;
+			}
+
+			/* add the bos to the list of bos to validate -
+			   need to validate first then process relocs? */
+			if (reloc.dst_handle) {
+				reloc_dst_bo = qxlhw_handle_to_bo(qdev, file_priv,
+								  reloc.dst_handle, &reloc_list);
+				if (!reloc_dst_bo) {
+					qxl_bo_list_unreserve(&reloc_list, true);
+					qxl_release_unreserve(qdev, release);
+					qxl_release_free(qdev, release);
+					return -EINVAL;
+				}
+				reloc_dst_offset = 0;
+			} else {
+				reloc_dst_bo = cmd_bo;
+				reloc_dst_offset = release->release_offset;
+			}
+
+			/* reserve and validate the reloc dst bo */
+			if (reloc.reloc_type == QXL_RELOC_TYPE_BO || reloc.src_handle > 0) {
+				reloc_src_bo =
+					qxlhw_handle_to_bo(qdev, file_priv,
+							   reloc.src_handle, &reloc_list);
+				if (!reloc_src_bo) {
+					if (reloc_dst_bo != cmd_bo)
+						drm_gem_object_unreference_unlocked(&reloc_dst_bo->gem_base);
+					qxl_bo_list_unreserve(&reloc_list, true);
+					qxl_release_unreserve(qdev, release);
+					qxl_release_free(qdev, release);
+					return -EINVAL;
+				}
+			} else
+				reloc_src_bo = NULL;
+			if (reloc.reloc_type == QXL_RELOC_TYPE_BO) {
+				apply_reloc(qdev, reloc_dst_bo, reloc_dst_offset + reloc.dst_offset,
+					    reloc_src_bo, reloc.src_offset);
+			} else if (reloc.reloc_type == QXL_RELOC_TYPE_SURF) {
+				apply_surf_reloc(qdev, reloc_dst_bo, reloc_dst_offset + reloc.dst_offset, reloc_src_bo);
+			} else {
+				DRM_ERROR("unknown reloc type %d\n", reloc.reloc_type);
+				return -EINVAL;
+			}
+
+			if (reloc_src_bo && reloc_src_bo != cmd_bo) {
+				qxl_release_add_res(qdev, release, reloc_src_bo);
+				drm_gem_object_unreference_unlocked(&reloc_src_bo->gem_base);
+			}
+
+			if (reloc_dst_bo != cmd_bo)
+				drm_gem_object_unreference_unlocked(&reloc_dst_bo->gem_base);
+		}
+		qxl_fence_releaseable(qdev, release);
+
+		ret = qxl_push_command_ring_release(qdev, release, user_cmd.type, true);
+		if (ret == -ERESTARTSYS) {
+			qxl_release_unreserve(qdev, release);
+			qxl_release_free(qdev, release);
+			qxl_bo_list_unreserve(&reloc_list, true);
+			return ret;
+		}
+		qxl_release_unreserve(qdev, release);
+	}
+	qxl_bo_list_unreserve(&reloc_list, 0);
+	return 0;
+}
+
+static int qxl_update_area_ioctl(struct drm_device *dev, void *data,
+				 struct drm_file *file)
+{
+	struct qxl_device *qdev = dev->dev_private;
+	struct drm_qxl_update_area *update_area = data;
+	struct qxl_rect area = {.left = update_area->left,
+				.top = update_area->top,
+				.right = update_area->right,
+				.bottom = update_area->bottom};
+	int ret;
+	struct drm_gem_object *gobj = NULL;
+	struct qxl_bo *qobj = NULL;
+
+	if (update_area->left >= update_area->right ||
+	    update_area->top >= update_area->bottom)
+		return -EINVAL;
+
+	gobj = drm_gem_object_lookup(dev, file, update_area->handle);
+	if (gobj == NULL)
+		return -ENOENT;
+
+	qobj = gem_to_qxl_bo(gobj);
+
+	ret = qxl_bo_reserve(qobj, false);
+	if (ret)
+		goto out;
+
+	if (!qobj->pin_count) {
+		ret = ttm_bo_validate(&qobj->tbo, &qobj->placement,
+				      true, false);
+		if (unlikely(ret))
+			goto out;
+	}
+
+	ret = qxl_bo_check_id(qdev, qobj);
+	if (ret)
+		goto out2;
+	if (!qobj->surface_id)
+		DRM_ERROR("got update area for surface with no id %d\n", update_area->handle);
+	ret = qxl_io_update_area(qdev, qobj, &area);
+
+out2:
+	qxl_bo_unreserve(qobj);
+
+out:
+	drm_gem_object_unreference_unlocked(gobj);
+	return ret;
+}
+
+static int qxl_getparam_ioctl(struct drm_device *dev, void *data,
+		       struct drm_file *file_priv)
+{
+	struct qxl_device *qdev = dev->dev_private;
+	struct drm_qxl_getparam *param = data;
+
+	switch (param->param) {
+	case QXL_PARAM_NUM_SURFACES:
+		param->value = qdev->rom->n_surfaces;
+		break;
+	case QXL_PARAM_MAX_RELOCS:
+		param->value = QXL_MAX_RES;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int qxl_clientcap_ioctl(struct drm_device *dev, void *data,
+				  struct drm_file *file_priv)
+{
+	struct qxl_device *qdev = dev->dev_private;
+	struct drm_qxl_clientcap *param = data;
+	int byte, idx;
+
+	byte = param->index / 8;
+	idx = param->index % 8;
+
+	if (qdev->pdev->revision < 4)
+		return -ENOSYS;
+
+	if (byte >= 58)
+		return -ENOSYS;
+
+	if (qdev->rom->client_capabilities[byte] & (1 << idx))
+		return 0;
+	return -ENOSYS;
+}
+
+static int qxl_alloc_surf_ioctl(struct drm_device *dev, void *data,
+				struct drm_file *file)
+{
+	struct qxl_device *qdev = dev->dev_private;
+	struct drm_qxl_alloc_surf *param = data;
+	struct qxl_bo *qobj;
+	int handle;
+	int ret;
+	int size, actual_stride;
+	struct qxl_surface surf;
+
+	/* work out size allocate bo with handle */
+	actual_stride = param->stride < 0 ? -param->stride : param->stride;
+	size = actual_stride * param->height + actual_stride;
+
+	surf.format = param->format;
+	surf.width = param->width;
+	surf.height = param->height;
+	surf.stride = param->stride;
+	surf.data = 0;
+
+	ret = qxl_gem_object_create_with_handle(qdev, file,
+						QXL_GEM_DOMAIN_SURFACE,
+						size,
+						&surf,
+						&qobj, &handle);
+	if (ret) {
+		DRM_ERROR("%s: failed to create gem ret=%d\n",
+			  __func__, ret);
+		return -ENOMEM;
+	} else
+		param->handle = handle;
+	return ret;
+}
+
+struct drm_ioctl_desc qxl_ioctls[] = {
+	DRM_IOCTL_DEF_DRV(QXL_ALLOC, qxl_alloc_ioctl, DRM_AUTH|DRM_UNLOCKED),
+
+	DRM_IOCTL_DEF_DRV(QXL_MAP, qxl_map_ioctl, DRM_AUTH|DRM_UNLOCKED),
+
+	DRM_IOCTL_DEF_DRV(QXL_EXECBUFFER, qxl_execbuffer_ioctl,
+							DRM_AUTH|DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(QXL_UPDATE_AREA, qxl_update_area_ioctl,
+							DRM_AUTH|DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(QXL_GETPARAM, qxl_getparam_ioctl,
+							DRM_AUTH|DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(QXL_CLIENTCAP, qxl_clientcap_ioctl,
+							DRM_AUTH|DRM_UNLOCKED),
+
+	DRM_IOCTL_DEF_DRV(QXL_ALLOC_SURF, qxl_alloc_surf_ioctl,
+			  DRM_AUTH|DRM_UNLOCKED),
+};
+
+int qxl_max_ioctls = DRM_ARRAY_SIZE(qxl_ioctls);
diff --git a/drivers/gpu/drm/qxl/qxl_irq.c b/drivers/gpu/drm/qxl/qxl_irq.c
new file mode 100644
index 000000000000..21393dc4700a
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_irq.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Dave Airlie
+ *          Alon Levy
+ */
+
+#include "qxl_drv.h"
+
+irqreturn_t qxl_irq_handler(DRM_IRQ_ARGS)
+{
+	struct drm_device *dev = (struct drm_device *) arg;
+	struct qxl_device *qdev = (struct qxl_device *)dev->dev_private;
+	uint32_t pending;
+
+	pending = xchg(&qdev->ram_header->int_pending, 0);
+
+	atomic_inc(&qdev->irq_received);
+
+	if (pending & QXL_INTERRUPT_DISPLAY) {
+		atomic_inc(&qdev->irq_received_display);
+		wake_up_all(&qdev->display_event);
+		qxl_queue_garbage_collect(qdev, false);
+	}
+	if (pending & QXL_INTERRUPT_CURSOR) {
+		atomic_inc(&qdev->irq_received_cursor);
+		wake_up_all(&qdev->cursor_event);
+	}
+	if (pending & QXL_INTERRUPT_IO_CMD) {
+		atomic_inc(&qdev->irq_received_io_cmd);
+		wake_up_all(&qdev->io_cmd_event);
+	}
+	if (pending & QXL_INTERRUPT_ERROR) {
+		/* TODO: log it, reset device (only way to exit this condition)
+		 * (do it a certain number of times, afterwards admit defeat,
+		 * to avoid endless loops).
+		 */
+		qdev->irq_received_error++;
+		qxl_io_log(qdev, "%s: driver is in bug mode.\n", __func__);
+	}
+	if (pending & QXL_INTERRUPT_CLIENT_MONITORS_CONFIG) {
+		qxl_io_log(qdev, "QXL_INTERRUPT_CLIENT_MONITORS_CONFIG\n");
+		schedule_work(&qdev->client_monitors_config_work);
+	}
+	qdev->ram_header->int_mask = QXL_INTERRUPT_MASK;
+	outb(0, qdev->io_base + QXL_IO_UPDATE_IRQ);
+	return IRQ_HANDLED;
+}
+
+static void qxl_client_monitors_config_work_func(struct work_struct *work)
+{
+	struct qxl_device *qdev = container_of(work, struct qxl_device,
+					       client_monitors_config_work);
+
+	qxl_display_read_client_monitors_config(qdev);
+}
+
+int qxl_irq_init(struct qxl_device *qdev)
+{
+	int ret;
+
+	init_waitqueue_head(&qdev->display_event);
+	init_waitqueue_head(&qdev->cursor_event);
+	init_waitqueue_head(&qdev->io_cmd_event);
+	INIT_WORK(&qdev->client_monitors_config_work,
+		  qxl_client_monitors_config_work_func);
+	atomic_set(&qdev->irq_received, 0);
+	atomic_set(&qdev->irq_received_display, 0);
+	atomic_set(&qdev->irq_received_cursor, 0);
+	atomic_set(&qdev->irq_received_io_cmd, 0);
+	qdev->irq_received_error = 0;
+	ret = drm_irq_install(qdev->ddev);
+	qdev->ram_header->int_mask = QXL_INTERRUPT_MASK;
+	if (unlikely(ret != 0)) {
+		DRM_ERROR("Failed installing irq: %d\n", ret);
+		return 1;
+	}
+	return 0;
+}
diff --git a/drivers/gpu/drm/qxl/qxl_kms.c b/drivers/gpu/drm/qxl/qxl_kms.c
new file mode 100644
index 000000000000..85127ed24cfd
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_kms.c
@@ -0,0 +1,302 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Dave Airlie
+ *          Alon Levy
+ */
+
+#include "qxl_drv.h"
+#include "qxl_object.h"
+
+#include <linux/io-mapping.h>
+
+int qxl_log_level;
+
+static void qxl_dump_mode(struct qxl_device *qdev, void *p)
+{
+	struct qxl_mode *m = p;
+	DRM_DEBUG_KMS("%d: %dx%d %d bits, stride %d, %dmm x %dmm, orientation %d\n",
+		      m->id, m->x_res, m->y_res, m->bits, m->stride, m->x_mili,
+		      m->y_mili, m->orientation);
+}
+
+static bool qxl_check_device(struct qxl_device *qdev)
+{
+	struct qxl_rom *rom = qdev->rom;
+	int mode_offset;
+	int i;
+
+	if (rom->magic != 0x4f525851) {
+		DRM_ERROR("bad rom signature %x\n", rom->magic);
+		return false;
+	}
+
+	DRM_INFO("Device Version %d.%d\n", rom->id, rom->update_id);
+	DRM_INFO("Compression level %d log level %d\n", rom->compression_level,
+		 rom->log_level);
+	DRM_INFO("Currently using mode #%d, list at 0x%x\n",
+		 rom->mode, rom->modes_offset);
+	DRM_INFO("%d io pages at offset 0x%x\n",
+		 rom->num_io_pages, rom->pages_offset);
+	DRM_INFO("%d byte draw area at offset 0x%x\n",
+		 rom->surface0_area_size, rom->draw_area_offset);
+
+	qdev->vram_size = rom->surface0_area_size;
+	DRM_INFO("RAM header offset: 0x%x\n", rom->ram_header_offset);
+
+	mode_offset = rom->modes_offset / 4;
+	qdev->mode_info.num_modes = ((u32 *)rom)[mode_offset];
+	DRM_INFO("rom modes offset 0x%x for %d modes\n", rom->modes_offset,
+		 qdev->mode_info.num_modes);
+	qdev->mode_info.modes = (void *)((uint32_t *)rom + mode_offset + 1);
+	for (i = 0; i < qdev->mode_info.num_modes; i++)
+		qxl_dump_mode(qdev, qdev->mode_info.modes + i);
+	return true;
+}
+
+static uint8_t setup_slot(struct qxl_device *qdev, uint8_t slot_index_offset,
+	unsigned long start_phys_addr, unsigned long end_phys_addr)
+{
+	uint64_t high_bits;
+	struct qxl_memslot *slot;
+	uint8_t slot_index;
+	struct qxl_ram_header *ram_header = qdev->ram_header;
+
+	slot_index = qdev->rom->slots_start + slot_index_offset;
+	slot = &qdev->mem_slots[slot_index];
+	slot->start_phys_addr = start_phys_addr;
+	slot->end_phys_addr = end_phys_addr;
+	ram_header->mem_slot.mem_start = slot->start_phys_addr;
+	ram_header->mem_slot.mem_end = slot->end_phys_addr;
+	qxl_io_memslot_add(qdev, slot_index);
+	slot->generation = qdev->rom->slot_generation;
+	high_bits = slot_index << qdev->slot_gen_bits;
+	high_bits |= slot->generation;
+	high_bits <<= (64 - (qdev->slot_gen_bits + qdev->slot_id_bits));
+	slot->high_bits = high_bits;
+	return slot_index;
+}
+
+static void qxl_gc_work(struct work_struct *work)
+{
+	struct qxl_device *qdev = container_of(work, struct qxl_device, gc_work);
+	qxl_garbage_collect(qdev);
+}
+
+int qxl_device_init(struct qxl_device *qdev,
+		    struct drm_device *ddev,
+		    struct pci_dev *pdev,
+		    unsigned long flags)
+{
+	int r;
+
+	qdev->dev = &pdev->dev;
+	qdev->ddev = ddev;
+	qdev->pdev = pdev;
+	qdev->flags = flags;
+
+	mutex_init(&qdev->gem.mutex);
+	mutex_init(&qdev->update_area_mutex);
+	mutex_init(&qdev->release_mutex);
+	mutex_init(&qdev->surf_evict_mutex);
+	INIT_LIST_HEAD(&qdev->gem.objects);
+
+	qdev->rom_base = pci_resource_start(pdev, 2);
+	qdev->rom_size = pci_resource_len(pdev, 2);
+	qdev->vram_base = pci_resource_start(pdev, 0);
+	qdev->surfaceram_base = pci_resource_start(pdev, 1);
+	qdev->surfaceram_size = pci_resource_len(pdev, 1);
+	qdev->io_base = pci_resource_start(pdev, 3);
+
+	qdev->vram_mapping = io_mapping_create_wc(qdev->vram_base, pci_resource_len(pdev, 0));
+	qdev->surface_mapping = io_mapping_create_wc(qdev->surfaceram_base, qdev->surfaceram_size);
+	DRM_DEBUG_KMS("qxl: vram %p-%p(%dM %dk), surface %p-%p(%dM %dk)\n",
+		 (void *)qdev->vram_base, (void *)pci_resource_end(pdev, 0),
+		 (int)pci_resource_len(pdev, 0) / 1024 / 1024,
+		 (int)pci_resource_len(pdev, 0) / 1024,
+		 (void *)qdev->surfaceram_base,
+		 (void *)pci_resource_end(pdev, 1),
+		 (int)qdev->surfaceram_size / 1024 / 1024,
+		 (int)qdev->surfaceram_size / 1024);
+
+	qdev->rom = ioremap(qdev->rom_base, qdev->rom_size);
+	if (!qdev->rom) {
+		pr_err("Unable to ioremap ROM\n");
+		return -ENOMEM;
+	}
+
+	qxl_check_device(qdev);
+
+	r = qxl_bo_init(qdev);
+	if (r) {
+		DRM_ERROR("bo init failed %d\n", r);
+		return r;
+	}
+
+	qdev->ram_header = ioremap(qdev->vram_base +
+				   qdev->rom->ram_header_offset,
+				   sizeof(*qdev->ram_header));
+
+	qdev->command_ring = qxl_ring_create(&(qdev->ram_header->cmd_ring_hdr),
+					     sizeof(struct qxl_command),
+					     QXL_COMMAND_RING_SIZE,
+					     qdev->io_base + QXL_IO_NOTIFY_CMD,
+					     false,
+					     &qdev->display_event);
+
+	qdev->cursor_ring = qxl_ring_create(
+				&(qdev->ram_header->cursor_ring_hdr),
+				sizeof(struct qxl_command),
+				QXL_CURSOR_RING_SIZE,
+				qdev->io_base + QXL_IO_NOTIFY_CMD,
+				false,
+				&qdev->cursor_event);
+
+	qdev->release_ring = qxl_ring_create(
+				&(qdev->ram_header->release_ring_hdr),
+				sizeof(uint64_t),
+				QXL_RELEASE_RING_SIZE, 0, true,
+				NULL);
+
+	/* TODO - slot initialization should happen on reset. where is our
+	 * reset handler? */
+	qdev->n_mem_slots = qdev->rom->slots_end;
+	qdev->slot_gen_bits = qdev->rom->slot_gen_bits;
+	qdev->slot_id_bits = qdev->rom->slot_id_bits;
+	qdev->va_slot_mask =
+		(~(uint64_t)0) >> (qdev->slot_id_bits + qdev->slot_gen_bits);
+
+	qdev->mem_slots =
+		kmalloc(qdev->n_mem_slots * sizeof(struct qxl_memslot),
+			GFP_KERNEL);
+
+	idr_init(&qdev->release_idr);
+	spin_lock_init(&qdev->release_idr_lock);
+
+	idr_init(&qdev->surf_id_idr);
+	spin_lock_init(&qdev->surf_id_idr_lock);
+
+	mutex_init(&qdev->async_io_mutex);
+
+	/* reset the device into a known state - no memslots, no primary
+	 * created, no surfaces. */
+	qxl_io_reset(qdev);
+
+	/* must initialize irq before first async io - slot creation */
+	r = qxl_irq_init(qdev);
+	if (r)
+		return r;
+
+	/*
+	 * Note that virtual is surface0. We rely on the single ioremap done
+	 * before.
+	 */
+	qdev->main_mem_slot = setup_slot(qdev, 0,
+		(unsigned long)qdev->vram_base,
+		(unsigned long)qdev->vram_base + qdev->rom->ram_header_offset);
+	qdev->surfaces_mem_slot = setup_slot(qdev, 1,
+		(unsigned long)qdev->surfaceram_base,
+		(unsigned long)qdev->surfaceram_base + qdev->surfaceram_size);
+	DRM_INFO("main mem slot %d [%lx,%x)\n",
+		qdev->main_mem_slot,
+		(unsigned long)qdev->vram_base, qdev->rom->ram_header_offset);
+
+
+	qdev->gc_queue = create_singlethread_workqueue("qxl_gc");
+	INIT_WORK(&qdev->gc_work, qxl_gc_work);
+
+	r = qxl_fb_init(qdev);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static void qxl_device_fini(struct qxl_device *qdev)
+{
+	if (qdev->current_release_bo[0])
+		qxl_bo_unref(&qdev->current_release_bo[0]);
+	if (qdev->current_release_bo[1])
+		qxl_bo_unref(&qdev->current_release_bo[1]);
+	flush_workqueue(qdev->gc_queue);
+	destroy_workqueue(qdev->gc_queue);
+	qdev->gc_queue = NULL;
+
+	qxl_ring_free(qdev->command_ring);
+	qxl_ring_free(qdev->cursor_ring);
+	qxl_ring_free(qdev->release_ring);
+	qxl_bo_fini(qdev);
+	io_mapping_free(qdev->surface_mapping);
+	io_mapping_free(qdev->vram_mapping);
+	iounmap(qdev->ram_header);
+	iounmap(qdev->rom);
+	qdev->rom = NULL;
+	qdev->mode_info.modes = NULL;
+	qdev->mode_info.num_modes = 0;
+	qxl_debugfs_remove_files(qdev);
+}
+
+int qxl_driver_unload(struct drm_device *dev)
+{
+	struct qxl_device *qdev = dev->dev_private;
+
+	if (qdev == NULL)
+		return 0;
+	qxl_modeset_fini(qdev);
+	qxl_device_fini(qdev);
+
+	kfree(qdev);
+	dev->dev_private = NULL;
+	return 0;
+}
+
+int qxl_driver_load(struct drm_device *dev, unsigned long flags)
+{
+	struct qxl_device *qdev;
+	int r;
+
+	/* require kms */
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		return -ENODEV;
+
+	qdev = kzalloc(sizeof(struct qxl_device), GFP_KERNEL);
+	if (qdev == NULL)
+		return -ENOMEM;
+
+	dev->dev_private = qdev;
+
+	r = qxl_device_init(qdev, dev, dev->pdev, flags);
+	if (r)
+		goto out;
+
+	r = qxl_modeset_init(qdev);
+	if (r) {
+		qxl_driver_unload(dev);
+		goto out;
+	}
+
+	return 0;
+out:
+	kfree(qdev);
+	return r;
+}
+
+
diff --git a/drivers/gpu/drm/qxl/qxl_object.c b/drivers/gpu/drm/qxl/qxl_object.c
new file mode 100644
index 000000000000..d9b12e7bc6e1
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_object.c
@@ -0,0 +1,365 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Dave Airlie
+ *          Alon Levy
+ */
+
+#include "qxl_drv.h"
+#include "qxl_object.h"
+
+#include <linux/io-mapping.h>
+static void qxl_ttm_bo_destroy(struct ttm_buffer_object *tbo)
+{
+	struct qxl_bo *bo;
+	struct qxl_device *qdev;
+
+	bo = container_of(tbo, struct qxl_bo, tbo);
+	qdev = (struct qxl_device *)bo->gem_base.dev->dev_private;
+
+	qxl_surface_evict(qdev, bo, false);
+	qxl_fence_fini(&bo->fence);
+	mutex_lock(&qdev->gem.mutex);
+	list_del_init(&bo->list);
+	mutex_unlock(&qdev->gem.mutex);
+	drm_gem_object_release(&bo->gem_base);
+	kfree(bo);
+}
+
+bool qxl_ttm_bo_is_qxl_bo(struct ttm_buffer_object *bo)
+{
+	if (bo->destroy == &qxl_ttm_bo_destroy)
+		return true;
+	return false;
+}
+
+void qxl_ttm_placement_from_domain(struct qxl_bo *qbo, u32 domain)
+{
+	u32 c = 0;
+
+	qbo->placement.fpfn = 0;
+	qbo->placement.lpfn = 0;
+	qbo->placement.placement = qbo->placements;
+	qbo->placement.busy_placement = qbo->placements;
+	if (domain == QXL_GEM_DOMAIN_VRAM)
+		qbo->placements[c++] = TTM_PL_FLAG_CACHED | TTM_PL_FLAG_VRAM;
+	if (domain == QXL_GEM_DOMAIN_SURFACE)
+		qbo->placements[c++] = TTM_PL_FLAG_CACHED | TTM_PL_FLAG_PRIV0;
+	if (domain == QXL_GEM_DOMAIN_CPU)
+		qbo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+	if (!c)
+		qbo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+	qbo->placement.num_placement = c;
+	qbo->placement.num_busy_placement = c;
+}
+
+
+int qxl_bo_create(struct qxl_device *qdev,
+		  unsigned long size, bool kernel, u32 domain,
+		  struct qxl_surface *surf,
+		  struct qxl_bo **bo_ptr)
+{
+	struct qxl_bo *bo;
+	enum ttm_bo_type type;
+	int r;
+
+	if (unlikely(qdev->mman.bdev.dev_mapping == NULL))
+		qdev->mman.bdev.dev_mapping = qdev->ddev->dev_mapping;
+	if (kernel)
+		type = ttm_bo_type_kernel;
+	else
+		type = ttm_bo_type_device;
+	*bo_ptr = NULL;
+	bo = kzalloc(sizeof(struct qxl_bo), GFP_KERNEL);
+	if (bo == NULL)
+		return -ENOMEM;
+	size = roundup(size, PAGE_SIZE);
+	r = drm_gem_object_init(qdev->ddev, &bo->gem_base, size);
+	if (unlikely(r)) {
+		kfree(bo);
+		return r;
+	}
+	bo->gem_base.driver_private = NULL;
+	bo->type = domain;
+	bo->pin_count = 0;
+	bo->surface_id = 0;
+	qxl_fence_init(qdev, &bo->fence);
+	INIT_LIST_HEAD(&bo->list);
+	atomic_set(&bo->reserve_count, 0);
+	if (surf)
+		bo->surf = *surf;
+
+	qxl_ttm_placement_from_domain(bo, domain);
+
+	r = ttm_bo_init(&qdev->mman.bdev, &bo->tbo, size, type,
+			&bo->placement, 0, !kernel, NULL, size,
+			NULL, &qxl_ttm_bo_destroy);
+	if (unlikely(r != 0)) {
+		if (r != -ERESTARTSYS)
+			dev_err(qdev->dev,
+				"object_init failed for (%lu, 0x%08X)\n",
+				size, domain);
+		return r;
+	}
+	*bo_ptr = bo;
+	return 0;
+}
+
+int qxl_bo_kmap(struct qxl_bo *bo, void **ptr)
+{
+	bool is_iomem;
+	int r;
+
+	if (bo->kptr) {
+		if (ptr)
+			*ptr = bo->kptr;
+		return 0;
+	}
+	r = ttm_bo_kmap(&bo->tbo, 0, bo->tbo.num_pages, &bo->kmap);
+	if (r)
+		return r;
+	bo->kptr = ttm_kmap_obj_virtual(&bo->kmap, &is_iomem);
+	if (ptr)
+		*ptr = bo->kptr;
+	return 0;
+}
+
+void *qxl_bo_kmap_atomic_page(struct qxl_device *qdev,
+			      struct qxl_bo *bo, int page_offset)
+{
+	struct ttm_mem_type_manager *man = &bo->tbo.bdev->man[bo->tbo.mem.mem_type];
+	void *rptr;
+	int ret;
+	struct io_mapping *map;
+
+	if (bo->tbo.mem.mem_type == TTM_PL_VRAM)
+		map = qdev->vram_mapping;
+	else if (bo->tbo.mem.mem_type == TTM_PL_PRIV0)
+		map = qdev->surface_mapping;
+	else
+		goto fallback;
+
+	(void) ttm_mem_io_lock(man, false);
+	ret = ttm_mem_io_reserve(bo->tbo.bdev, &bo->tbo.mem);
+	ttm_mem_io_unlock(man);
+
+	return io_mapping_map_atomic_wc(map, bo->tbo.mem.bus.offset + page_offset);
+fallback:
+	if (bo->kptr) {
+		rptr = bo->kptr + (page_offset * PAGE_SIZE);
+		return rptr;
+	}
+
+	ret = qxl_bo_kmap(bo, &rptr);
+	if (ret)
+		return NULL;
+
+	rptr += page_offset * PAGE_SIZE;
+	return rptr;
+}
+
+void qxl_bo_kunmap(struct qxl_bo *bo)
+{
+	if (bo->kptr == NULL)
+		return;
+	bo->kptr = NULL;
+	ttm_bo_kunmap(&bo->kmap);
+}
+
+void qxl_bo_kunmap_atomic_page(struct qxl_device *qdev,
+			       struct qxl_bo *bo, void *pmap)
+{
+	struct ttm_mem_type_manager *man = &bo->tbo.bdev->man[bo->tbo.mem.mem_type];
+	struct io_mapping *map;
+
+	if (bo->tbo.mem.mem_type == TTM_PL_VRAM)
+		map = qdev->vram_mapping;
+	else if (bo->tbo.mem.mem_type == TTM_PL_PRIV0)
+		map = qdev->surface_mapping;
+	else
+		goto fallback;
+
+	io_mapping_unmap_atomic(pmap);
+
+	(void) ttm_mem_io_lock(man, false);
+	ttm_mem_io_free(bo->tbo.bdev, &bo->tbo.mem);
+	ttm_mem_io_unlock(man);
+	return ;
+ fallback:
+	qxl_bo_kunmap(bo);
+}
+
+void qxl_bo_unref(struct qxl_bo **bo)
+{
+	struct ttm_buffer_object *tbo;
+
+	if ((*bo) == NULL)
+		return;
+	tbo = &((*bo)->tbo);
+	ttm_bo_unref(&tbo);
+	if (tbo == NULL)
+		*bo = NULL;
+}
+
+struct qxl_bo *qxl_bo_ref(struct qxl_bo *bo)
+{
+	ttm_bo_reference(&bo->tbo);
+	return bo;
+}
+
+int qxl_bo_pin(struct qxl_bo *bo, u32 domain, u64 *gpu_addr)
+{
+	struct qxl_device *qdev = (struct qxl_device *)bo->gem_base.dev->dev_private;
+	int r, i;
+
+	if (bo->pin_count) {
+		bo->pin_count++;
+		if (gpu_addr)
+			*gpu_addr = qxl_bo_gpu_offset(bo);
+		return 0;
+	}
+	qxl_ttm_placement_from_domain(bo, domain);
+	for (i = 0; i < bo->placement.num_placement; i++)
+		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+	r = ttm_bo_validate(&bo->tbo, &bo->placement, false, false);
+	if (likely(r == 0)) {
+		bo->pin_count = 1;
+		if (gpu_addr != NULL)
+			*gpu_addr = qxl_bo_gpu_offset(bo);
+	}
+	if (unlikely(r != 0))
+		dev_err(qdev->dev, "%p pin failed\n", bo);
+	return r;
+}
+
+int qxl_bo_unpin(struct qxl_bo *bo)
+{
+	struct qxl_device *qdev = (struct qxl_device *)bo->gem_base.dev->dev_private;
+	int r, i;
+
+	if (!bo->pin_count) {
+		dev_warn(qdev->dev, "%p unpin not necessary\n", bo);
+		return 0;
+	}
+	bo->pin_count--;
+	if (bo->pin_count)
+		return 0;
+	for (i = 0; i < bo->placement.num_placement; i++)
+		bo->placements[i] &= ~TTM_PL_FLAG_NO_EVICT;
+	r = ttm_bo_validate(&bo->tbo, &bo->placement, false, false);
+	if (unlikely(r != 0))
+		dev_err(qdev->dev, "%p validate failed for unpin\n", bo);
+	return r;
+}
+
+void qxl_bo_force_delete(struct qxl_device *qdev)
+{
+	struct qxl_bo *bo, *n;
+
+	if (list_empty(&qdev->gem.objects))
+		return;
+	dev_err(qdev->dev, "Userspace still has active objects !\n");
+	list_for_each_entry_safe(bo, n, &qdev->gem.objects, list) {
+		mutex_lock(&qdev->ddev->struct_mutex);
+		dev_err(qdev->dev, "%p %p %lu %lu force free\n",
+			&bo->gem_base, bo, (unsigned long)bo->gem_base.size,
+			*((unsigned long *)&bo->gem_base.refcount));
+		mutex_lock(&qdev->gem.mutex);
+		list_del_init(&bo->list);
+		mutex_unlock(&qdev->gem.mutex);
+		/* this should unref the ttm bo */
+		drm_gem_object_unreference(&bo->gem_base);
+		mutex_unlock(&qdev->ddev->struct_mutex);
+	}
+}
+
+int qxl_bo_init(struct qxl_device *qdev)
+{
+	return qxl_ttm_init(qdev);
+}
+
+void qxl_bo_fini(struct qxl_device *qdev)
+{
+	qxl_ttm_fini(qdev);
+}
+
+int qxl_bo_check_id(struct qxl_device *qdev, struct qxl_bo *bo)
+{
+	int ret;
+	if (bo->type == QXL_GEM_DOMAIN_SURFACE && bo->surface_id == 0) {
+		/* allocate a surface id for this surface now */
+		ret = qxl_surface_id_alloc(qdev, bo);
+		if (ret)
+			return ret;
+
+		ret = qxl_hw_surface_alloc(qdev, bo, NULL);
+		if (ret)
+			return ret;
+	}
+	return 0;
+}
+
+void qxl_bo_list_unreserve(struct qxl_reloc_list *reloc_list, bool failed)
+{
+	struct qxl_bo_list *entry, *sf;
+
+	list_for_each_entry_safe(entry, sf, &reloc_list->bos, lhead) {
+		qxl_bo_unreserve(entry->bo);
+		list_del(&entry->lhead);
+		kfree(entry);
+	}
+}
+
+int qxl_bo_list_add(struct qxl_reloc_list *reloc_list, struct qxl_bo *bo)
+{
+	struct qxl_bo_list *entry;
+	int ret;
+
+	list_for_each_entry(entry, &reloc_list->bos, lhead) {
+		if (entry->bo == bo)
+			return 0;
+	}
+
+	entry = kmalloc(sizeof(struct qxl_bo_list), GFP_KERNEL);
+	if (!entry)
+		return -ENOMEM;
+
+	entry->bo = bo;
+	list_add(&entry->lhead, &reloc_list->bos);
+
+	ret = qxl_bo_reserve(bo, false);
+	if (ret)
+		return ret;
+
+	if (!bo->pin_count) {
+		qxl_ttm_placement_from_domain(bo, bo->type);
+		ret = ttm_bo_validate(&bo->tbo, &bo->placement,
+				      true, false);
+		if (ret)
+			return ret;
+	}
+
+	/* allocate a surface for reserved + validated buffers */
+	ret = qxl_bo_check_id(bo->gem_base.dev->dev_private, bo);
+	if (ret)
+		return ret;
+	return 0;
+}
diff --git a/drivers/gpu/drm/qxl/qxl_object.h b/drivers/gpu/drm/qxl/qxl_object.h
new file mode 100644
index 000000000000..b4fd89fbd8b7
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_object.h
@@ -0,0 +1,112 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Dave Airlie
+ *          Alon Levy
+ */
+#ifndef QXL_OBJECT_H
+#define QXL_OBJECT_H
+
+#include "qxl_drv.h"
+
+static inline int qxl_bo_reserve(struct qxl_bo *bo, bool no_wait)
+{
+	int r;
+
+	r = ttm_bo_reserve(&bo->tbo, true, no_wait, false, 0);
+	if (unlikely(r != 0)) {
+		if (r != -ERESTARTSYS) {
+			struct qxl_device *qdev = (struct qxl_device *)bo->gem_base.dev->dev_private;
+			dev_err(qdev->dev, "%p reserve failed\n", bo);
+		}
+		return r;
+	}
+	return 0;
+}
+
+static inline void qxl_bo_unreserve(struct qxl_bo *bo)
+{
+	ttm_bo_unreserve(&bo->tbo);
+}
+
+static inline u64 qxl_bo_gpu_offset(struct qxl_bo *bo)
+{
+	return bo->tbo.offset;
+}
+
+static inline unsigned long qxl_bo_size(struct qxl_bo *bo)
+{
+	return bo->tbo.num_pages << PAGE_SHIFT;
+}
+
+static inline bool qxl_bo_is_reserved(struct qxl_bo *bo)
+{
+	return !!atomic_read(&bo->tbo.reserved);
+}
+
+static inline u64 qxl_bo_mmap_offset(struct qxl_bo *bo)
+{
+	return bo->tbo.addr_space_offset;
+}
+
+static inline int qxl_bo_wait(struct qxl_bo *bo, u32 *mem_type,
+			      bool no_wait)
+{
+	int r;
+
+	r = ttm_bo_reserve(&bo->tbo, true, no_wait, false, 0);
+	if (unlikely(r != 0)) {
+		if (r != -ERESTARTSYS) {
+			struct qxl_device *qdev = (struct qxl_device *)bo->gem_base.dev->dev_private;
+			dev_err(qdev->dev, "%p reserve failed for wait\n",
+				bo);
+		}
+		return r;
+	}
+	spin_lock(&bo->tbo.bdev->fence_lock);
+	if (mem_type)
+		*mem_type = bo->tbo.mem.mem_type;
+	if (bo->tbo.sync_obj)
+		r = ttm_bo_wait(&bo->tbo, true, true, no_wait);
+	spin_unlock(&bo->tbo.bdev->fence_lock);
+	ttm_bo_unreserve(&bo->tbo);
+	return r;
+}
+
+extern int qxl_bo_create(struct qxl_device *qdev,
+			 unsigned long size,
+			 bool kernel, u32 domain,
+			 struct qxl_surface *surf,
+			 struct qxl_bo **bo_ptr);
+extern int qxl_bo_kmap(struct qxl_bo *bo, void **ptr);
+extern void qxl_bo_kunmap(struct qxl_bo *bo);
+void *qxl_bo_kmap_atomic_page(struct qxl_device *qdev, struct qxl_bo *bo, int page_offset);
+void qxl_bo_kunmap_atomic_page(struct qxl_device *qdev, struct qxl_bo *bo, void *map);
+extern struct qxl_bo *qxl_bo_ref(struct qxl_bo *bo);
+extern void qxl_bo_unref(struct qxl_bo **bo);
+extern int qxl_bo_pin(struct qxl_bo *bo, u32 domain, u64 *gpu_addr);
+extern int qxl_bo_unpin(struct qxl_bo *bo);
+extern void qxl_ttm_placement_from_domain(struct qxl_bo *qbo, u32 domain);
+extern bool qxl_ttm_bo_is_qxl_bo(struct ttm_buffer_object *bo);
+
+extern int qxl_bo_list_add(struct qxl_reloc_list *reloc_list, struct qxl_bo *bo);
+extern void qxl_bo_list_unreserve(struct qxl_reloc_list *reloc_list, bool failed);
+#endif
diff --git a/drivers/gpu/drm/qxl/qxl_release.c b/drivers/gpu/drm/qxl/qxl_release.c
new file mode 100644
index 000000000000..b443d6751d5f
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_release.c
@@ -0,0 +1,304 @@
+/*
+ * Copyright 2011 Red Hat, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * on the rights to use, copy, modify, merge, publish, distribute, sub
+ * license, and/or sell copies of the Software, and to permit persons to whom
+ * the Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#include "qxl_drv.h"
+#include "qxl_object.h"
+
+/*
+ * drawable cmd cache - allocate a bunch of VRAM pages, suballocate
+ * into 256 byte chunks for now - gives 16 cmds per page.
+ *
+ * use an ida to index into the chunks?
+ */
+/* manage releaseables */
+/* stack them 16 high for now -drawable object is 191 */
+#define RELEASE_SIZE 256
+#define RELEASES_PER_BO (4096 / RELEASE_SIZE)
+/* put an alloc/dealloc surface cmd into one bo and round up to 128 */
+#define SURFACE_RELEASE_SIZE 128
+#define SURFACE_RELEASES_PER_BO (4096 / SURFACE_RELEASE_SIZE)
+
+static const int release_size_per_bo[] = { RELEASE_SIZE, SURFACE_RELEASE_SIZE, RELEASE_SIZE };
+static const int releases_per_bo[] = { RELEASES_PER_BO, SURFACE_RELEASES_PER_BO, RELEASES_PER_BO };
+uint64_t
+qxl_release_alloc(struct qxl_device *qdev, int type,
+		  struct qxl_release **ret)
+{
+	struct qxl_release *release;
+	int handle;
+	size_t size = sizeof(*release);
+	int idr_ret;
+
+	release = kmalloc(size, GFP_KERNEL);
+	if (!release) {
+		DRM_ERROR("Out of memory\n");
+		return 0;
+	}
+	release->type = type;
+	release->bo_count = 0;
+	release->release_offset = 0;
+	release->surface_release_id = 0;
+
+	idr_preload(GFP_KERNEL);
+	spin_lock(&qdev->release_idr_lock);
+	idr_ret = idr_alloc(&qdev->release_idr, release, 1, 0, GFP_NOWAIT);
+	spin_unlock(&qdev->release_idr_lock);
+	idr_preload_end();
+	handle = idr_ret;
+	if (idr_ret < 0)
+		goto release_fail;
+	*ret = release;
+	QXL_INFO(qdev, "allocated release %lld\n", handle);
+	release->id = handle;
+release_fail:
+
+	return handle;
+}
+
+void
+qxl_release_free(struct qxl_device *qdev,
+		 struct qxl_release *release)
+{
+	int i;
+
+	QXL_INFO(qdev, "release %d, type %d, %d bos\n", release->id,
+		 release->type, release->bo_count);
+
+	if (release->surface_release_id)
+		qxl_surface_id_dealloc(qdev, release->surface_release_id);
+
+	for (i = 0 ; i < release->bo_count; ++i) {
+		QXL_INFO(qdev, "release %llx\n",
+			release->bos[i]->tbo.addr_space_offset
+						- DRM_FILE_OFFSET);
+		qxl_fence_remove_release(&release->bos[i]->fence, release->id);
+		qxl_bo_unref(&release->bos[i]);
+	}
+	spin_lock(&qdev->release_idr_lock);
+	idr_remove(&qdev->release_idr, release->id);
+	spin_unlock(&qdev->release_idr_lock);
+	kfree(release);
+}
+
+void
+qxl_release_add_res(struct qxl_device *qdev, struct qxl_release *release,
+		    struct qxl_bo *bo)
+{
+	int i;
+	for (i = 0; i < release->bo_count; i++)
+		if (release->bos[i] == bo)
+			return;
+
+	if (release->bo_count >= QXL_MAX_RES) {
+		DRM_ERROR("exceeded max resource on a qxl_release item\n");
+		return;
+	}
+	release->bos[release->bo_count++] = qxl_bo_ref(bo);
+}
+
+static int qxl_release_bo_alloc(struct qxl_device *qdev,
+				struct qxl_bo **bo)
+{
+	int ret;
+	ret = qxl_bo_create(qdev, PAGE_SIZE, false, QXL_GEM_DOMAIN_VRAM, NULL,
+			    bo);
+	return ret;
+}
+
+int qxl_release_reserve(struct qxl_device *qdev,
+			struct qxl_release *release, bool no_wait)
+{
+	int ret;
+	if (atomic_inc_return(&release->bos[0]->reserve_count) == 1) {
+		ret = qxl_bo_reserve(release->bos[0], no_wait);
+		if (ret)
+			return ret;
+	}
+	return 0;
+}
+
+void qxl_release_unreserve(struct qxl_device *qdev,
+			  struct qxl_release *release)
+{
+	if (atomic_dec_and_test(&release->bos[0]->reserve_count))
+		qxl_bo_unreserve(release->bos[0]);
+}
+
+int qxl_alloc_surface_release_reserved(struct qxl_device *qdev,
+				       enum qxl_surface_cmd_type surface_cmd_type,
+				       struct qxl_release *create_rel,
+				       struct qxl_release **release)
+{
+	int ret;
+
+	if (surface_cmd_type == QXL_SURFACE_CMD_DESTROY && create_rel) {
+		int idr_ret;
+		struct qxl_bo *bo;
+		union qxl_release_info *info;
+
+		/* stash the release after the create command */
+		idr_ret = qxl_release_alloc(qdev, QXL_RELEASE_SURFACE_CMD, release);
+		bo = qxl_bo_ref(create_rel->bos[0]);
+
+		(*release)->release_offset = create_rel->release_offset + 64;
+
+		qxl_release_add_res(qdev, *release, bo);
+
+		ret = qxl_release_reserve(qdev, *release, false);
+		if (ret) {
+			DRM_ERROR("release reserve failed\n");
+			goto out_unref;
+		}
+		info = qxl_release_map(qdev, *release);
+		info->id = idr_ret;
+		qxl_release_unmap(qdev, *release, info);
+
+
+out_unref:
+		qxl_bo_unref(&bo);
+		return ret;
+	}
+
+	return qxl_alloc_release_reserved(qdev, sizeof(struct qxl_surface_cmd),
+					 QXL_RELEASE_SURFACE_CMD, release, NULL);
+}
+
+int qxl_alloc_release_reserved(struct qxl_device *qdev, unsigned long size,
+				       int type, struct qxl_release **release,
+				       struct qxl_bo **rbo)
+{
+	struct qxl_bo *bo;
+	int idr_ret;
+	int ret;
+	union qxl_release_info *info;
+	int cur_idx;
+
+	if (type == QXL_RELEASE_DRAWABLE)
+		cur_idx = 0;
+	else if (type == QXL_RELEASE_SURFACE_CMD)
+		cur_idx = 1;
+	else if (type == QXL_RELEASE_CURSOR_CMD)
+		cur_idx = 2;
+	else {
+		DRM_ERROR("got illegal type: %d\n", type);
+		return -EINVAL;
+	}
+
+	idr_ret = qxl_release_alloc(qdev, type, release);
+
+	mutex_lock(&qdev->release_mutex);
+	if (qdev->current_release_bo_offset[cur_idx] + 1 >= releases_per_bo[cur_idx]) {
+		qxl_bo_unref(&qdev->current_release_bo[cur_idx]);
+		qdev->current_release_bo_offset[cur_idx] = 0;
+		qdev->current_release_bo[cur_idx] = NULL;
+	}
+	if (!qdev->current_release_bo[cur_idx]) {
+		ret = qxl_release_bo_alloc(qdev, &qdev->current_release_bo[cur_idx]);
+		if (ret) {
+			mutex_unlock(&qdev->release_mutex);
+			return ret;
+		}
+
+		/* pin releases bo's they are too messy to evict */
+		ret = qxl_bo_reserve(qdev->current_release_bo[cur_idx], false);
+		qxl_bo_pin(qdev->current_release_bo[cur_idx], QXL_GEM_DOMAIN_VRAM, NULL);
+		qxl_bo_unreserve(qdev->current_release_bo[cur_idx]);
+	}
+
+	bo = qxl_bo_ref(qdev->current_release_bo[cur_idx]);
+
+	(*release)->release_offset = qdev->current_release_bo_offset[cur_idx] * release_size_per_bo[cur_idx];
+	qdev->current_release_bo_offset[cur_idx]++;
+
+	if (rbo)
+		*rbo = bo;
+
+	qxl_release_add_res(qdev, *release, bo);
+
+	ret = qxl_release_reserve(qdev, *release, false);
+	mutex_unlock(&qdev->release_mutex);
+	if (ret)
+		goto out_unref;
+
+	info = qxl_release_map(qdev, *release);
+	info->id = idr_ret;
+	qxl_release_unmap(qdev, *release, info);
+
+out_unref:
+	qxl_bo_unref(&bo);
+	return ret;
+}
+
+int qxl_fence_releaseable(struct qxl_device *qdev,
+			  struct qxl_release *release)
+{
+	int i, ret;
+	for (i = 0; i < release->bo_count; i++) {
+		if (!release->bos[i]->tbo.sync_obj)
+			release->bos[i]->tbo.sync_obj = &release->bos[i]->fence;
+		ret = qxl_fence_add_release(&release->bos[i]->fence, release->id);
+		if (ret)
+			return ret;
+	}
+	return 0;
+}
+
+struct qxl_release *qxl_release_from_id_locked(struct qxl_device *qdev,
+						   uint64_t id)
+{
+	struct qxl_release *release;
+
+	spin_lock(&qdev->release_idr_lock);
+	release = idr_find(&qdev->release_idr, id);
+	spin_unlock(&qdev->release_idr_lock);
+	if (!release) {
+		DRM_ERROR("failed to find id in release_idr\n");
+		return NULL;
+	}
+	if (release->bo_count < 1) {
+		DRM_ERROR("read a released resource with 0 bos\n");
+		return NULL;
+	}
+	return release;
+}
+
+union qxl_release_info *qxl_release_map(struct qxl_device *qdev,
+					struct qxl_release *release)
+{
+	void *ptr;
+	union qxl_release_info *info;
+	struct qxl_bo *bo = release->bos[0];
+
+	ptr = qxl_bo_kmap_atomic_page(qdev, bo, release->release_offset & PAGE_SIZE);
+	info = ptr + (release->release_offset & ~PAGE_SIZE);
+	return info;
+}
+
+void qxl_release_unmap(struct qxl_device *qdev,
+		       struct qxl_release *release,
+		       union qxl_release_info *info)
+{
+	struct qxl_bo *bo = release->bos[0];
+	void *ptr;
+
+	ptr = ((void *)info) - (release->release_offset & ~PAGE_SIZE);
+	qxl_bo_kunmap_atomic_page(qdev, bo, ptr);
+}
diff --git a/drivers/gpu/drm/qxl/qxl_ttm.c b/drivers/gpu/drm/qxl/qxl_ttm.c
new file mode 100644
index 000000000000..489cb8cece4d
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_ttm.c
@@ -0,0 +1,581 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Dave Airlie
+ *          Alon Levy
+ */
+
+#include <ttm/ttm_bo_api.h>
+#include <ttm/ttm_bo_driver.h>
+#include <ttm/ttm_placement.h>
+#include <ttm/ttm_page_alloc.h>
+#include <ttm/ttm_module.h>
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include <drm/qxl_drm.h>
+#include "qxl_drv.h"
+#include "qxl_object.h"
+
+#include <linux/delay.h>
+static int qxl_ttm_debugfs_init(struct qxl_device *qdev);
+
+static struct qxl_device *qxl_get_qdev(struct ttm_bo_device *bdev)
+{
+	struct qxl_mman *mman;
+	struct qxl_device *qdev;
+
+	mman = container_of(bdev, struct qxl_mman, bdev);
+	qdev = container_of(mman, struct qxl_device, mman);
+	return qdev;
+}
+
+static int qxl_ttm_mem_global_init(struct drm_global_reference *ref)
+{
+	return ttm_mem_global_init(ref->object);
+}
+
+static void qxl_ttm_mem_global_release(struct drm_global_reference *ref)
+{
+	ttm_mem_global_release(ref->object);
+}
+
+static int qxl_ttm_global_init(struct qxl_device *qdev)
+{
+	struct drm_global_reference *global_ref;
+	int r;
+
+	qdev->mman.mem_global_referenced = false;
+	global_ref = &qdev->mman.mem_global_ref;
+	global_ref->global_type = DRM_GLOBAL_TTM_MEM;
+	global_ref->size = sizeof(struct ttm_mem_global);
+	global_ref->init = &qxl_ttm_mem_global_init;
+	global_ref->release = &qxl_ttm_mem_global_release;
+
+	r = drm_global_item_ref(global_ref);
+	if (r != 0) {
+		DRM_ERROR("Failed setting up TTM memory accounting "
+			  "subsystem.\n");
+		return r;
+	}
+
+	qdev->mman.bo_global_ref.mem_glob =
+		qdev->mman.mem_global_ref.object;
+	global_ref = &qdev->mman.bo_global_ref.ref;
+	global_ref->global_type = DRM_GLOBAL_TTM_BO;
+	global_ref->size = sizeof(struct ttm_bo_global);
+	global_ref->init = &ttm_bo_global_init;
+	global_ref->release = &ttm_bo_global_release;
+	r = drm_global_item_ref(global_ref);
+	if (r != 0) {
+		DRM_ERROR("Failed setting up TTM BO subsystem.\n");
+		drm_global_item_unref(&qdev->mman.mem_global_ref);
+		return r;
+	}
+
+	qdev->mman.mem_global_referenced = true;
+	return 0;
+}
+
+static void qxl_ttm_global_fini(struct qxl_device *qdev)
+{
+	if (qdev->mman.mem_global_referenced) {
+		drm_global_item_unref(&qdev->mman.bo_global_ref.ref);
+		drm_global_item_unref(&qdev->mman.mem_global_ref);
+		qdev->mman.mem_global_referenced = false;
+	}
+}
+
+static struct vm_operations_struct qxl_ttm_vm_ops;
+static const struct vm_operations_struct *ttm_vm_ops;
+
+static int qxl_ttm_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
+{
+	struct ttm_buffer_object *bo;
+	struct qxl_device *qdev;
+	int r;
+
+	bo = (struct ttm_buffer_object *)vma->vm_private_data;
+	if (bo == NULL)
+		return VM_FAULT_NOPAGE;
+	qdev = qxl_get_qdev(bo->bdev);
+	r = ttm_vm_ops->fault(vma, vmf);
+	return r;
+}
+
+int qxl_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct drm_file *file_priv;
+	struct qxl_device *qdev;
+	int r;
+
+	if (unlikely(vma->vm_pgoff < DRM_FILE_PAGE_OFFSET)) {
+		pr_info("%s: vma->vm_pgoff (%ld) < DRM_FILE_PAGE_OFFSET\n",
+			__func__, vma->vm_pgoff);
+		return drm_mmap(filp, vma);
+	}
+
+	file_priv = filp->private_data;
+	qdev = file_priv->minor->dev->dev_private;
+	if (qdev == NULL) {
+		DRM_ERROR(
+		 "filp->private_data->minor->dev->dev_private == NULL\n");
+		return -EINVAL;
+	}
+	QXL_INFO(qdev, "%s: filp->private_data = 0x%p, vma->vm_pgoff = %lx\n",
+		 __func__, filp->private_data, vma->vm_pgoff);
+
+	r = ttm_bo_mmap(filp, vma, &qdev->mman.bdev);
+	if (unlikely(r != 0))
+		return r;
+	if (unlikely(ttm_vm_ops == NULL)) {
+		ttm_vm_ops = vma->vm_ops;
+		qxl_ttm_vm_ops = *ttm_vm_ops;
+		qxl_ttm_vm_ops.fault = &qxl_ttm_fault;
+	}
+	vma->vm_ops = &qxl_ttm_vm_ops;
+	return 0;
+}
+
+static int qxl_invalidate_caches(struct ttm_bo_device *bdev, uint32_t flags)
+{
+	return 0;
+}
+
+static int qxl_init_mem_type(struct ttm_bo_device *bdev, uint32_t type,
+			     struct ttm_mem_type_manager *man)
+{
+	struct qxl_device *qdev;
+
+	qdev = qxl_get_qdev(bdev);
+
+	switch (type) {
+	case TTM_PL_SYSTEM:
+		/* System memory */
+		man->flags = TTM_MEMTYPE_FLAG_MAPPABLE;
+		man->available_caching = TTM_PL_MASK_CACHING;
+		man->default_caching = TTM_PL_FLAG_CACHED;
+		break;
+	case TTM_PL_VRAM:
+	case TTM_PL_PRIV0:
+		/* "On-card" video ram */
+		man->func = &ttm_bo_manager_func;
+		man->gpu_offset = 0;
+		man->flags = TTM_MEMTYPE_FLAG_FIXED |
+			     TTM_MEMTYPE_FLAG_MAPPABLE;
+		man->available_caching = TTM_PL_MASK_CACHING;
+		man->default_caching = TTM_PL_FLAG_CACHED;
+		break;
+	default:
+		DRM_ERROR("Unsupported memory type %u\n", (unsigned)type);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void qxl_evict_flags(struct ttm_buffer_object *bo,
+				struct ttm_placement *placement)
+{
+	struct qxl_bo *qbo;
+	static u32 placements = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+
+	if (!qxl_ttm_bo_is_qxl_bo(bo)) {
+		placement->fpfn = 0;
+		placement->lpfn = 0;
+		placement->placement = &placements;
+		placement->busy_placement = &placements;
+		placement->num_placement = 1;
+		placement->num_busy_placement = 1;
+		return;
+	}
+	qbo = container_of(bo, struct qxl_bo, tbo);
+	qxl_ttm_placement_from_domain(qbo, QXL_GEM_DOMAIN_CPU);
+	*placement = qbo->placement;
+}
+
+static int qxl_verify_access(struct ttm_buffer_object *bo, struct file *filp)
+{
+	return 0;
+}
+
+static int qxl_ttm_io_mem_reserve(struct ttm_bo_device *bdev,
+				  struct ttm_mem_reg *mem)
+{
+	struct ttm_mem_type_manager *man = &bdev->man[mem->mem_type];
+	struct qxl_device *qdev = qxl_get_qdev(bdev);
+
+	mem->bus.addr = NULL;
+	mem->bus.offset = 0;
+	mem->bus.size = mem->num_pages << PAGE_SHIFT;
+	mem->bus.base = 0;
+	mem->bus.is_iomem = false;
+	if (!(man->flags & TTM_MEMTYPE_FLAG_MAPPABLE))
+		return -EINVAL;
+	switch (mem->mem_type) {
+	case TTM_PL_SYSTEM:
+		/* system memory */
+		return 0;
+	case TTM_PL_VRAM:
+		mem->bus.is_iomem = true;
+		mem->bus.base = qdev->vram_base;
+		mem->bus.offset = mem->start << PAGE_SHIFT;
+		break;
+	case TTM_PL_PRIV0:
+		mem->bus.is_iomem = true;
+		mem->bus.base = qdev->surfaceram_base;
+		mem->bus.offset = mem->start << PAGE_SHIFT;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void qxl_ttm_io_mem_free(struct ttm_bo_device *bdev,
+				struct ttm_mem_reg *mem)
+{
+}
+
+/*
+ * TTM backend functions.
+ */
+struct qxl_ttm_tt {
+	struct ttm_dma_tt		ttm;
+	struct qxl_device		*qdev;
+	u64				offset;
+};
+
+static int qxl_ttm_backend_bind(struct ttm_tt *ttm,
+				struct ttm_mem_reg *bo_mem)
+{
+	struct qxl_ttm_tt *gtt = (void *)ttm;
+
+	gtt->offset = (unsigned long)(bo_mem->start << PAGE_SHIFT);
+	if (!ttm->num_pages) {
+		WARN(1, "nothing to bind %lu pages for mreg %p back %p!\n",
+		     ttm->num_pages, bo_mem, ttm);
+	}
+	/* Not implemented */
+	return -1;
+}
+
+static int qxl_ttm_backend_unbind(struct ttm_tt *ttm)
+{
+	/* Not implemented */
+	return -1;
+}
+
+static void qxl_ttm_backend_destroy(struct ttm_tt *ttm)
+{
+	struct qxl_ttm_tt *gtt = (void *)ttm;
+
+	ttm_dma_tt_fini(&gtt->ttm);
+	kfree(gtt);
+}
+
+static struct ttm_backend_func qxl_backend_func = {
+	.bind = &qxl_ttm_backend_bind,
+	.unbind = &qxl_ttm_backend_unbind,
+	.destroy = &qxl_ttm_backend_destroy,
+};
+
+static int qxl_ttm_tt_populate(struct ttm_tt *ttm)
+{
+	int r;
+
+	if (ttm->state != tt_unpopulated)
+		return 0;
+
+	r = ttm_pool_populate(ttm);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static void qxl_ttm_tt_unpopulate(struct ttm_tt *ttm)
+{
+	ttm_pool_unpopulate(ttm);
+}
+
+static struct ttm_tt *qxl_ttm_tt_create(struct ttm_bo_device *bdev,
+					unsigned long size, uint32_t page_flags,
+					struct page *dummy_read_page)
+{
+	struct qxl_device *qdev;
+	struct qxl_ttm_tt *gtt;
+
+	qdev = qxl_get_qdev(bdev);
+	gtt = kzalloc(sizeof(struct qxl_ttm_tt), GFP_KERNEL);
+	if (gtt == NULL)
+		return NULL;
+	gtt->ttm.ttm.func = &qxl_backend_func;
+	gtt->qdev = qdev;
+	if (ttm_dma_tt_init(&gtt->ttm, bdev, size, page_flags,
+			    dummy_read_page)) {
+		kfree(gtt);
+		return NULL;
+	}
+	return &gtt->ttm.ttm;
+}
+
+static void qxl_move_null(struct ttm_buffer_object *bo,
+			     struct ttm_mem_reg *new_mem)
+{
+	struct ttm_mem_reg *old_mem = &bo->mem;
+
+	BUG_ON(old_mem->mm_node != NULL);
+	*old_mem = *new_mem;
+	new_mem->mm_node = NULL;
+}
+
+static int qxl_bo_move(struct ttm_buffer_object *bo,
+		       bool evict, bool interruptible,
+		       bool no_wait_gpu,
+		       struct ttm_mem_reg *new_mem)
+{
+	struct ttm_mem_reg *old_mem = &bo->mem;
+	if (old_mem->mem_type == TTM_PL_SYSTEM && bo->ttm == NULL) {
+		qxl_move_null(bo, new_mem);
+		return 0;
+	}
+	return ttm_bo_move_memcpy(bo, evict, no_wait_gpu, new_mem);
+}
+
+
+static int qxl_sync_obj_wait(void *sync_obj,
+			     bool lazy, bool interruptible)
+{
+	struct qxl_fence *qfence = (struct qxl_fence *)sync_obj;
+	int count = 0, sc = 0;
+	struct qxl_bo *bo = container_of(qfence, struct qxl_bo, fence);
+
+	if (qfence->num_active_releases == 0)
+		return 0;
+
+retry:
+	if (sc == 0) {
+		if (bo->type == QXL_GEM_DOMAIN_SURFACE)
+			qxl_update_surface(qfence->qdev, bo);
+	} else if (sc >= 1) {
+		qxl_io_notify_oom(qfence->qdev);
+	}
+
+	sc++;
+
+	for (count = 0; count < 10; count++) {
+		bool ret;
+		ret = qxl_queue_garbage_collect(qfence->qdev, true);
+		if (ret == false)
+			break;
+
+		if (qfence->num_active_releases == 0)
+			return 0;
+	}
+
+	if (qfence->num_active_releases) {
+		bool have_drawable_releases = false;
+		void **slot;
+		struct radix_tree_iter iter;
+		int release_id;
+
+		radix_tree_for_each_slot(slot, &qfence->tree, &iter, 0) {
+			struct qxl_release *release;
+
+			release_id = iter.index;
+			release = qxl_release_from_id_locked(qfence->qdev, release_id);
+			if (release == NULL)
+				continue;
+
+			if (release->type == QXL_RELEASE_DRAWABLE)
+				have_drawable_releases = true;
+		}
+
+		qxl_queue_garbage_collect(qfence->qdev, true);
+
+		if (have_drawable_releases || sc < 4) {
+			if (sc > 2)
+				/* back off */
+				usleep_range(500, 1000);
+			if (have_drawable_releases && sc > 300) {
+				WARN(1, "sync obj %d still has outstanding releases %d %d %d %ld %d\n", sc, bo->surface_id, bo->is_primary, bo->pin_count, (unsigned long)bo->gem_base.size, qfence->num_active_releases);
+				return -EBUSY;
+			}
+			goto retry;
+		}
+	}
+	return 0;
+}
+
+static int qxl_sync_obj_flush(void *sync_obj)
+{
+	return 0;
+}
+
+static void qxl_sync_obj_unref(void **sync_obj)
+{
+}
+
+static void *qxl_sync_obj_ref(void *sync_obj)
+{
+	return sync_obj;
+}
+
+static bool qxl_sync_obj_signaled(void *sync_obj)
+{
+	struct qxl_fence *qfence = (struct qxl_fence *)sync_obj;
+	return (qfence->num_active_releases == 0);
+}
+
+static void qxl_bo_move_notify(struct ttm_buffer_object *bo,
+			       struct ttm_mem_reg *new_mem)
+{
+	struct qxl_bo *qbo;
+	struct qxl_device *qdev;
+
+	if (!qxl_ttm_bo_is_qxl_bo(bo))
+		return;
+	qbo = container_of(bo, struct qxl_bo, tbo);
+	qdev = qbo->gem_base.dev->dev_private;
+
+	if (bo->mem.mem_type == TTM_PL_PRIV0 && qbo->surface_id)
+		qxl_surface_evict(qdev, qbo, new_mem ? true : false);
+}
+
+static struct ttm_bo_driver qxl_bo_driver = {
+	.ttm_tt_create = &qxl_ttm_tt_create,
+	.ttm_tt_populate = &qxl_ttm_tt_populate,
+	.ttm_tt_unpopulate = &qxl_ttm_tt_unpopulate,
+	.invalidate_caches = &qxl_invalidate_caches,
+	.init_mem_type = &qxl_init_mem_type,
+	.evict_flags = &qxl_evict_flags,
+	.move = &qxl_bo_move,
+	.verify_access = &qxl_verify_access,
+	.io_mem_reserve = &qxl_ttm_io_mem_reserve,
+	.io_mem_free = &qxl_ttm_io_mem_free,
+	.sync_obj_signaled = &qxl_sync_obj_signaled,
+	.sync_obj_wait = &qxl_sync_obj_wait,
+	.sync_obj_flush = &qxl_sync_obj_flush,
+	.sync_obj_unref = &qxl_sync_obj_unref,
+	.sync_obj_ref = &qxl_sync_obj_ref,
+	.move_notify = &qxl_bo_move_notify,
+};
+
+
+
+int qxl_ttm_init(struct qxl_device *qdev)
+{
+	int r;
+	int num_io_pages; /* != rom->num_io_pages, we include surface0 */
+
+	r = qxl_ttm_global_init(qdev);
+	if (r)
+		return r;
+	/* No others user of address space so set it to 0 */
+	r = ttm_bo_device_init(&qdev->mman.bdev,
+			       qdev->mman.bo_global_ref.ref.object,
+			       &qxl_bo_driver, DRM_FILE_PAGE_OFFSET, 0);
+	if (r) {
+		DRM_ERROR("failed initializing buffer object driver(%d).\n", r);
+		return r;
+	}
+	/* NOTE: this includes the framebuffer (aka surface 0) */
+	num_io_pages = qdev->rom->ram_header_offset / PAGE_SIZE;
+	r = ttm_bo_init_mm(&qdev->mman.bdev, TTM_PL_VRAM,
+			   num_io_pages);
+	if (r) {
+		DRM_ERROR("Failed initializing VRAM heap.\n");
+		return r;
+	}
+	r = ttm_bo_init_mm(&qdev->mman.bdev, TTM_PL_PRIV0,
+			   qdev->surfaceram_size / PAGE_SIZE);
+	if (r) {
+		DRM_ERROR("Failed initializing Surfaces heap.\n");
+		return r;
+	}
+	DRM_INFO("qxl: %uM of VRAM memory size\n",
+		 (unsigned)qdev->vram_size / (1024 * 1024));
+	DRM_INFO("qxl: %luM of IO pages memory ready (VRAM domain)\n",
+		 ((unsigned)num_io_pages * PAGE_SIZE) / (1024 * 1024));
+	if (unlikely(qdev->mman.bdev.dev_mapping == NULL))
+		qdev->mman.bdev.dev_mapping = qdev->ddev->dev_mapping;
+	r = qxl_ttm_debugfs_init(qdev);
+	if (r) {
+		DRM_ERROR("Failed to init debugfs\n");
+		return r;
+	}
+	return 0;
+}
+
+void qxl_ttm_fini(struct qxl_device *qdev)
+{
+	ttm_bo_clean_mm(&qdev->mman.bdev, TTM_PL_VRAM);
+	ttm_bo_clean_mm(&qdev->mman.bdev, TTM_PL_PRIV0);
+	ttm_bo_device_release(&qdev->mman.bdev);
+	qxl_ttm_global_fini(qdev);
+	DRM_INFO("qxl: ttm finalized\n");
+}
+
+
+#define QXL_DEBUGFS_MEM_TYPES 2
+
+#if defined(CONFIG_DEBUG_FS)
+static int qxl_mm_dump_table(struct seq_file *m, void *data)
+{
+	struct drm_info_node *node = (struct drm_info_node *)m->private;
+	struct drm_mm *mm = (struct drm_mm *)node->info_ent->data;
+	struct drm_device *dev = node->minor->dev;
+	struct qxl_device *rdev = dev->dev_private;
+	int ret;
+	struct ttm_bo_global *glob = rdev->mman.bdev.glob;
+
+	spin_lock(&glob->lru_lock);
+	ret = drm_mm_dump_table(m, mm);
+	spin_unlock(&glob->lru_lock);
+	return ret;
+}
+#endif
+
+static int qxl_ttm_debugfs_init(struct qxl_device *qdev)
+{
+#if defined(CONFIG_DEBUG_FS)
+	static struct drm_info_list qxl_mem_types_list[QXL_DEBUGFS_MEM_TYPES];
+	static char qxl_mem_types_names[QXL_DEBUGFS_MEM_TYPES][32];
+	unsigned i;
+
+	for (i = 0; i < QXL_DEBUGFS_MEM_TYPES; i++) {
+		if (i == 0)
+			sprintf(qxl_mem_types_names[i], "qxl_mem_mm");
+		else
+			sprintf(qxl_mem_types_names[i], "qxl_surf_mm");
+		qxl_mem_types_list[i].name = qxl_mem_types_names[i];
+		qxl_mem_types_list[i].show = &qxl_mm_dump_table;
+		qxl_mem_types_list[i].driver_features = 0;
+		if (i == 0)
+			qxl_mem_types_list[i].data = qdev->mman.bdev.man[TTM_PL_VRAM].priv;
+		else
+			qxl_mem_types_list[i].data = qdev->mman.bdev.man[TTM_PL_PRIV0].priv;
+
+	}
+	return qxl_debugfs_add_files(qdev, qxl_mem_types_list, i);
+#else
+	return 0;
+#endif
+}
diff --git a/drivers/gpu/drm/radeon/Makefile b/drivers/gpu/drm/radeon/Makefile
index bf172522ea68..86c5e3611892 100644
--- a/drivers/gpu/drm/radeon/Makefile
+++ b/drivers/gpu/drm/radeon/Makefile
@@ -76,7 +76,7 @@ radeon-y += radeon_device.o radeon_asic.o radeon_kms.o \
 	evergreen.o evergreen_cs.o evergreen_blit_shaders.o evergreen_blit_kms.o \
 	evergreen_hdmi.o radeon_trace_points.o ni.o cayman_blit_shaders.o \
 	atombios_encoders.o radeon_semaphore.o radeon_sa.o atombios_i2c.o si.o \
-	si_blit_shaders.o radeon_prime.o
+	si_blit_shaders.o radeon_prime.o radeon_uvd.o
 
 radeon-$(CONFIG_COMPAT) += radeon_ioc32.o
 radeon-$(CONFIG_VGA_SWITCHEROO) += radeon_atpx_handler.o
diff --git a/drivers/gpu/drm/radeon/atom.c b/drivers/gpu/drm/radeon/atom.c
index 46a9c3772850..fb441a790f3d 100644
--- a/drivers/gpu/drm/radeon/atom.c
+++ b/drivers/gpu/drm/radeon/atom.c
@@ -1394,10 +1394,10 @@ int atom_allocate_fb_scratch(struct atom_context *ctx)
 		firmware_usage = (struct _ATOM_VRAM_USAGE_BY_FIRMWARE *)(ctx->bios + data_offset);
 
 		DRM_DEBUG("atom firmware requested %08x %dkb\n",
-			  firmware_usage->asFirmwareVramReserveInfo[0].ulStartAddrUsedByFirmware,
-			  firmware_usage->asFirmwareVramReserveInfo[0].usFirmwareUseInKb);
+			  le32_to_cpu(firmware_usage->asFirmwareVramReserveInfo[0].ulStartAddrUsedByFirmware),
+			  le16_to_cpu(firmware_usage->asFirmwareVramReserveInfo[0].usFirmwareUseInKb));
 
-		usage_bytes = firmware_usage->asFirmwareVramReserveInfo[0].usFirmwareUseInKb * 1024;
+		usage_bytes = le16_to_cpu(firmware_usage->asFirmwareVramReserveInfo[0].usFirmwareUseInKb) * 1024;
 	}
 	ctx->scratch_size_bytes = 0;
 	if (usage_bytes == 0)
diff --git a/drivers/gpu/drm/radeon/atombios.h b/drivers/gpu/drm/radeon/atombios.h
index 4b04ba3828e8..0ee573743de9 100644
--- a/drivers/gpu/drm/radeon/atombios.h
+++ b/drivers/gpu/drm/radeon/atombios.h
@@ -458,6 +458,7 @@ typedef struct _COMPUTE_MEMORY_ENGINE_PLL_PARAMETERS_V3
   union
   {
     ATOM_COMPUTE_CLOCK_FREQ  ulClock;         //Input Parameter
+    ULONG ulClockParams;                      //ULONG access for BE
     ATOM_S_MPLL_FB_DIVIDER   ulFbDiv;         //Output Parameter
   };
   UCHAR   ucRefDiv;                           //Output Parameter      
@@ -490,6 +491,7 @@ typedef struct _COMPUTE_MEMORY_ENGINE_PLL_PARAMETERS_V5
   union
   {
     ATOM_COMPUTE_CLOCK_FREQ  ulClock;         //Input Parameter
+    ULONG ulClockParams;                      //ULONG access for BE
     ATOM_S_MPLL_FB_DIVIDER   ulFbDiv;         //Output Parameter
   };
   UCHAR   ucRefDiv;                           //Output Parameter      
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index 21a892c6ab9c..6d6fdb3ba0d0 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -557,6 +557,9 @@ static u32 atombios_adjust_pll(struct drm_crtc *crtc,
 		/* use frac fb div on APUs */
 		if (ASIC_IS_DCE41(rdev) || ASIC_IS_DCE61(rdev))
 			radeon_crtc->pll_flags |= RADEON_PLL_USE_FRAC_FB_DIV;
+		/* use frac fb div on RS780/RS880 */
+		if ((rdev->family == CHIP_RS780) || (rdev->family == CHIP_RS880))
+			radeon_crtc->pll_flags |= RADEON_PLL_USE_FRAC_FB_DIV;
 		if (ASIC_IS_DCE32(rdev) && mode->clock > 165000)
 			radeon_crtc->pll_flags |= RADEON_PLL_USE_FRAC_FB_DIV;
 	} else {
diff --git a/drivers/gpu/drm/radeon/atombios_encoders.c b/drivers/gpu/drm/radeon/atombios_encoders.c
index 4552d4aff317..44a7da66e081 100644
--- a/drivers/gpu/drm/radeon/atombios_encoders.c
+++ b/drivers/gpu/drm/radeon/atombios_encoders.c
@@ -2150,13 +2150,10 @@ radeon_atom_encoder_mode_set(struct drm_encoder *encoder,
 	atombios_apply_encoder_quirks(encoder, adjusted_mode);
 
 	if (atombios_get_encoder_mode(encoder) == ATOM_ENCODER_MODE_HDMI) {
-		r600_hdmi_enable(encoder);
-		if (ASIC_IS_DCE6(rdev))
-			; /* TODO (use pointers instead of if-s?) */
-		else if (ASIC_IS_DCE4(rdev))
-			evergreen_hdmi_setmode(encoder, adjusted_mode);
-		else
-			r600_hdmi_setmode(encoder, adjusted_mode);
+		if (rdev->asic->display.hdmi_enable)
+			radeon_hdmi_enable(rdev, encoder, true);
+		if (rdev->asic->display.hdmi_setmode)
+			radeon_hdmi_setmode(rdev, encoder, adjusted_mode);
 	}
 }
 
@@ -2413,8 +2410,10 @@ static void radeon_atom_encoder_disable(struct drm_encoder *encoder)
 
 disable_done:
 	if (radeon_encoder_is_digital(encoder)) {
-		if (atombios_get_encoder_mode(encoder) == ATOM_ENCODER_MODE_HDMI)
-			r600_hdmi_disable(encoder);
+		if (atombios_get_encoder_mode(encoder) == ATOM_ENCODER_MODE_HDMI) {
+			if (rdev->asic->display.hdmi_enable)
+				radeon_hdmi_enable(rdev, encoder, false);
+		}
 		dig = radeon_encoder->enc_priv;
 		dig->dig_encoder = -1;
 	}
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index 305a657bf215..105bafb6c29d 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -53,6 +53,864 @@ void evergreen_pcie_gen2_enable(struct radeon_device *rdev);
 extern void cayman_cp_int_cntl_setup(struct radeon_device *rdev,
 				     int ring, u32 cp_int_cntl);
 
+static const u32 evergreen_golden_registers[] =
+{
+	0x3f90, 0xffff0000, 0xff000000,
+	0x9148, 0xffff0000, 0xff000000,
+	0x3f94, 0xffff0000, 0xff000000,
+	0x914c, 0xffff0000, 0xff000000,
+	0x9b7c, 0xffffffff, 0x00000000,
+	0x8a14, 0xffffffff, 0x00000007,
+	0x8b10, 0xffffffff, 0x00000000,
+	0x960c, 0xffffffff, 0x54763210,
+	0x88c4, 0xffffffff, 0x000000c2,
+	0x88d4, 0xffffffff, 0x00000010,
+	0x8974, 0xffffffff, 0x00000000,
+	0xc78, 0x00000080, 0x00000080,
+	0x5eb4, 0xffffffff, 0x00000002,
+	0x5e78, 0xffffffff, 0x001000f0,
+	0x6104, 0x01000300, 0x00000000,
+	0x5bc0, 0x00300000, 0x00000000,
+	0x7030, 0xffffffff, 0x00000011,
+	0x7c30, 0xffffffff, 0x00000011,
+	0x10830, 0xffffffff, 0x00000011,
+	0x11430, 0xffffffff, 0x00000011,
+	0x12030, 0xffffffff, 0x00000011,
+	0x12c30, 0xffffffff, 0x00000011,
+	0xd02c, 0xffffffff, 0x08421000,
+	0x240c, 0xffffffff, 0x00000380,
+	0x8b24, 0xffffffff, 0x00ff0fff,
+	0x28a4c, 0x06000000, 0x06000000,
+	0x10c, 0x00000001, 0x00000001,
+	0x8d00, 0xffffffff, 0x100e4848,
+	0x8d04, 0xffffffff, 0x00164745,
+	0x8c00, 0xffffffff, 0xe4000003,
+	0x8c04, 0xffffffff, 0x40600060,
+	0x8c08, 0xffffffff, 0x001c001c,
+	0x8cf0, 0xffffffff, 0x08e00620,
+	0x8c20, 0xffffffff, 0x00800080,
+	0x8c24, 0xffffffff, 0x00800080,
+	0x8c18, 0xffffffff, 0x20202078,
+	0x8c1c, 0xffffffff, 0x00001010,
+	0x28350, 0xffffffff, 0x00000000,
+	0xa008, 0xffffffff, 0x00010000,
+	0x5cc, 0xffffffff, 0x00000001,
+	0x9508, 0xffffffff, 0x00000002,
+	0x913c, 0x0000000f, 0x0000000a
+};
+
+static const u32 evergreen_golden_registers2[] =
+{
+	0x2f4c, 0xffffffff, 0x00000000,
+	0x54f4, 0xffffffff, 0x00000000,
+	0x54f0, 0xffffffff, 0x00000000,
+	0x5498, 0xffffffff, 0x00000000,
+	0x549c, 0xffffffff, 0x00000000,
+	0x5494, 0xffffffff, 0x00000000,
+	0x53cc, 0xffffffff, 0x00000000,
+	0x53c8, 0xffffffff, 0x00000000,
+	0x53c4, 0xffffffff, 0x00000000,
+	0x53c0, 0xffffffff, 0x00000000,
+	0x53bc, 0xffffffff, 0x00000000,
+	0x53b8, 0xffffffff, 0x00000000,
+	0x53b4, 0xffffffff, 0x00000000,
+	0x53b0, 0xffffffff, 0x00000000
+};
+
+static const u32 cypress_mgcg_init[] =
+{
+	0x802c, 0xffffffff, 0xc0000000,
+	0x5448, 0xffffffff, 0x00000100,
+	0x55e4, 0xffffffff, 0x00000100,
+	0x160c, 0xffffffff, 0x00000100,
+	0x5644, 0xffffffff, 0x00000100,
+	0xc164, 0xffffffff, 0x00000100,
+	0x8a18, 0xffffffff, 0x00000100,
+	0x897c, 0xffffffff, 0x06000100,
+	0x8b28, 0xffffffff, 0x00000100,
+	0x9144, 0xffffffff, 0x00000100,
+	0x9a60, 0xffffffff, 0x00000100,
+	0x9868, 0xffffffff, 0x00000100,
+	0x8d58, 0xffffffff, 0x00000100,
+	0x9510, 0xffffffff, 0x00000100,
+	0x949c, 0xffffffff, 0x00000100,
+	0x9654, 0xffffffff, 0x00000100,
+	0x9030, 0xffffffff, 0x00000100,
+	0x9034, 0xffffffff, 0x00000100,
+	0x9038, 0xffffffff, 0x00000100,
+	0x903c, 0xffffffff, 0x00000100,
+	0x9040, 0xffffffff, 0x00000100,
+	0xa200, 0xffffffff, 0x00000100,
+	0xa204, 0xffffffff, 0x00000100,
+	0xa208, 0xffffffff, 0x00000100,
+	0xa20c, 0xffffffff, 0x00000100,
+	0x971c, 0xffffffff, 0x00000100,
+	0x977c, 0xffffffff, 0x00000100,
+	0x3f80, 0xffffffff, 0x00000100,
+	0xa210, 0xffffffff, 0x00000100,
+	0xa214, 0xffffffff, 0x00000100,
+	0x4d8, 0xffffffff, 0x00000100,
+	0x9784, 0xffffffff, 0x00000100,
+	0x9698, 0xffffffff, 0x00000100,
+	0x4d4, 0xffffffff, 0x00000200,
+	0x30cc, 0xffffffff, 0x00000100,
+	0xd0c0, 0xffffffff, 0xff000100,
+	0x802c, 0xffffffff, 0x40000000,
+	0x915c, 0xffffffff, 0x00010000,
+	0x9160, 0xffffffff, 0x00030002,
+	0x9178, 0xffffffff, 0x00070000,
+	0x917c, 0xffffffff, 0x00030002,
+	0x9180, 0xffffffff, 0x00050004,
+	0x918c, 0xffffffff, 0x00010006,
+	0x9190, 0xffffffff, 0x00090008,
+	0x9194, 0xffffffff, 0x00070000,
+	0x9198, 0xffffffff, 0x00030002,
+	0x919c, 0xffffffff, 0x00050004,
+	0x91a8, 0xffffffff, 0x00010006,
+	0x91ac, 0xffffffff, 0x00090008,
+	0x91b0, 0xffffffff, 0x00070000,
+	0x91b4, 0xffffffff, 0x00030002,
+	0x91b8, 0xffffffff, 0x00050004,
+	0x91c4, 0xffffffff, 0x00010006,
+	0x91c8, 0xffffffff, 0x00090008,
+	0x91cc, 0xffffffff, 0x00070000,
+	0x91d0, 0xffffffff, 0x00030002,
+	0x91d4, 0xffffffff, 0x00050004,
+	0x91e0, 0xffffffff, 0x00010006,
+	0x91e4, 0xffffffff, 0x00090008,
+	0x91e8, 0xffffffff, 0x00000000,
+	0x91ec, 0xffffffff, 0x00070000,
+	0x91f0, 0xffffffff, 0x00030002,
+	0x91f4, 0xffffffff, 0x00050004,
+	0x9200, 0xffffffff, 0x00010006,
+	0x9204, 0xffffffff, 0x00090008,
+	0x9208, 0xffffffff, 0x00070000,
+	0x920c, 0xffffffff, 0x00030002,
+	0x9210, 0xffffffff, 0x00050004,
+	0x921c, 0xffffffff, 0x00010006,
+	0x9220, 0xffffffff, 0x00090008,
+	0x9224, 0xffffffff, 0x00070000,
+	0x9228, 0xffffffff, 0x00030002,
+	0x922c, 0xffffffff, 0x00050004,
+	0x9238, 0xffffffff, 0x00010006,
+	0x923c, 0xffffffff, 0x00090008,
+	0x9240, 0xffffffff, 0x00070000,
+	0x9244, 0xffffffff, 0x00030002,
+	0x9248, 0xffffffff, 0x00050004,
+	0x9254, 0xffffffff, 0x00010006,
+	0x9258, 0xffffffff, 0x00090008,
+	0x925c, 0xffffffff, 0x00070000,
+	0x9260, 0xffffffff, 0x00030002,
+	0x9264, 0xffffffff, 0x00050004,
+	0x9270, 0xffffffff, 0x00010006,
+	0x9274, 0xffffffff, 0x00090008,
+	0x9278, 0xffffffff, 0x00070000,
+	0x927c, 0xffffffff, 0x00030002,
+	0x9280, 0xffffffff, 0x00050004,
+	0x928c, 0xffffffff, 0x00010006,
+	0x9290, 0xffffffff, 0x00090008,
+	0x9294, 0xffffffff, 0x00000000,
+	0x929c, 0xffffffff, 0x00000001,
+	0x802c, 0xffffffff, 0x40010000,
+	0x915c, 0xffffffff, 0x00010000,
+	0x9160, 0xffffffff, 0x00030002,
+	0x9178, 0xffffffff, 0x00070000,
+	0x917c, 0xffffffff, 0x00030002,
+	0x9180, 0xffffffff, 0x00050004,
+	0x918c, 0xffffffff, 0x00010006,
+	0x9190, 0xffffffff, 0x00090008,
+	0x9194, 0xffffffff, 0x00070000,
+	0x9198, 0xffffffff, 0x00030002,
+	0x919c, 0xffffffff, 0x00050004,
+	0x91a8, 0xffffffff, 0x00010006,
+	0x91ac, 0xffffffff, 0x00090008,
+	0x91b0, 0xffffffff, 0x00070000,
+	0x91b4, 0xffffffff, 0x00030002,
+	0x91b8, 0xffffffff, 0x00050004,
+	0x91c4, 0xffffffff, 0x00010006,
+	0x91c8, 0xffffffff, 0x00090008,
+	0x91cc, 0xffffffff, 0x00070000,
+	0x91d0, 0xffffffff, 0x00030002,
+	0x91d4, 0xffffffff, 0x00050004,
+	0x91e0, 0xffffffff, 0x00010006,
+	0x91e4, 0xffffffff, 0x00090008,
+	0x91e8, 0xffffffff, 0x00000000,
+	0x91ec, 0xffffffff, 0x00070000,
+	0x91f0, 0xffffffff, 0x00030002,
+	0x91f4, 0xffffffff, 0x00050004,
+	0x9200, 0xffffffff, 0x00010006,
+	0x9204, 0xffffffff, 0x00090008,
+	0x9208, 0xffffffff, 0x00070000,
+	0x920c, 0xffffffff, 0x00030002,
+	0x9210, 0xffffffff, 0x00050004,
+	0x921c, 0xffffffff, 0x00010006,
+	0x9220, 0xffffffff, 0x00090008,
+	0x9224, 0xffffffff, 0x00070000,
+	0x9228, 0xffffffff, 0x00030002,
+	0x922c, 0xffffffff, 0x00050004,
+	0x9238, 0xffffffff, 0x00010006,
+	0x923c, 0xffffffff, 0x00090008,
+	0x9240, 0xffffffff, 0x00070000,
+	0x9244, 0xffffffff, 0x00030002,
+	0x9248, 0xffffffff, 0x00050004,
+	0x9254, 0xffffffff, 0x00010006,
+	0x9258, 0xffffffff, 0x00090008,
+	0x925c, 0xffffffff, 0x00070000,
+	0x9260, 0xffffffff, 0x00030002,
+	0x9264, 0xffffffff, 0x00050004,
+	0x9270, 0xffffffff, 0x00010006,
+	0x9274, 0xffffffff, 0x00090008,
+	0x9278, 0xffffffff, 0x00070000,
+	0x927c, 0xffffffff, 0x00030002,
+	0x9280, 0xffffffff, 0x00050004,
+	0x928c, 0xffffffff, 0x00010006,
+	0x9290, 0xffffffff, 0x00090008,
+	0x9294, 0xffffffff, 0x00000000,
+	0x929c, 0xffffffff, 0x00000001,
+	0x802c, 0xffffffff, 0xc0000000
+};
+
+static const u32 redwood_mgcg_init[] =
+{
+	0x802c, 0xffffffff, 0xc0000000,
+	0x5448, 0xffffffff, 0x00000100,
+	0x55e4, 0xffffffff, 0x00000100,
+	0x160c, 0xffffffff, 0x00000100,
+	0x5644, 0xffffffff, 0x00000100,
+	0xc164, 0xffffffff, 0x00000100,
+	0x8a18, 0xffffffff, 0x00000100,
+	0x897c, 0xffffffff, 0x06000100,
+	0x8b28, 0xffffffff, 0x00000100,
+	0x9144, 0xffffffff, 0x00000100,
+	0x9a60, 0xffffffff, 0x00000100,
+	0x9868, 0xffffffff, 0x00000100,
+	0x8d58, 0xffffffff, 0x00000100,
+	0x9510, 0xffffffff, 0x00000100,
+	0x949c, 0xffffffff, 0x00000100,
+	0x9654, 0xffffffff, 0x00000100,
+	0x9030, 0xffffffff, 0x00000100,
+	0x9034, 0xffffffff, 0x00000100,
+	0x9038, 0xffffffff, 0x00000100,
+	0x903c, 0xffffffff, 0x00000100,
+	0x9040, 0xffffffff, 0x00000100,
+	0xa200, 0xffffffff, 0x00000100,
+	0xa204, 0xffffffff, 0x00000100,
+	0xa208, 0xffffffff, 0x00000100,
+	0xa20c, 0xffffffff, 0x00000100,
+	0x971c, 0xffffffff, 0x00000100,
+	0x977c, 0xffffffff, 0x00000100,
+	0x3f80, 0xffffffff, 0x00000100,
+	0xa210, 0xffffffff, 0x00000100,
+	0xa214, 0xffffffff, 0x00000100,
+	0x4d8, 0xffffffff, 0x00000100,
+	0x9784, 0xffffffff, 0x00000100,
+	0x9698, 0xffffffff, 0x00000100,
+	0x4d4, 0xffffffff, 0x00000200,
+	0x30cc, 0xffffffff, 0x00000100,
+	0xd0c0, 0xffffffff, 0xff000100,
+	0x802c, 0xffffffff, 0x40000000,
+	0x915c, 0xffffffff, 0x00010000,
+	0x9160, 0xffffffff, 0x00030002,
+	0x9178, 0xffffffff, 0x00070000,
+	0x917c, 0xffffffff, 0x00030002,
+	0x9180, 0xffffffff, 0x00050004,
+	0x918c, 0xffffffff, 0x00010006,
+	0x9190, 0xffffffff, 0x00090008,
+	0x9194, 0xffffffff, 0x00070000,
+	0x9198, 0xffffffff, 0x00030002,
+	0x919c, 0xffffffff, 0x00050004,
+	0x91a8, 0xffffffff, 0x00010006,
+	0x91ac, 0xffffffff, 0x00090008,
+	0x91b0, 0xffffffff, 0x00070000,
+	0x91b4, 0xffffffff, 0x00030002,
+	0x91b8, 0xffffffff, 0x00050004,
+	0x91c4, 0xffffffff, 0x00010006,
+	0x91c8, 0xffffffff, 0x00090008,
+	0x91cc, 0xffffffff, 0x00070000,
+	0x91d0, 0xffffffff, 0x00030002,
+	0x91d4, 0xffffffff, 0x00050004,
+	0x91e0, 0xffffffff, 0x00010006,
+	0x91e4, 0xffffffff, 0x00090008,
+	0x91e8, 0xffffffff, 0x00000000,
+	0x91ec, 0xffffffff, 0x00070000,
+	0x91f0, 0xffffffff, 0x00030002,
+	0x91f4, 0xffffffff, 0x00050004,
+	0x9200, 0xffffffff, 0x00010006,
+	0x9204, 0xffffffff, 0x00090008,
+	0x9294, 0xffffffff, 0x00000000,
+	0x929c, 0xffffffff, 0x00000001,
+	0x802c, 0xffffffff, 0xc0000000
+};
+
+static const u32 cedar_golden_registers[] =
+{
+	0x3f90, 0xffff0000, 0xff000000,
+	0x9148, 0xffff0000, 0xff000000,
+	0x3f94, 0xffff0000, 0xff000000,
+	0x914c, 0xffff0000, 0xff000000,
+	0x9b7c, 0xffffffff, 0x00000000,
+	0x8a14, 0xffffffff, 0x00000007,
+	0x8b10, 0xffffffff, 0x00000000,
+	0x960c, 0xffffffff, 0x54763210,
+	0x88c4, 0xffffffff, 0x000000c2,
+	0x88d4, 0xffffffff, 0x00000000,
+	0x8974, 0xffffffff, 0x00000000,
+	0xc78, 0x00000080, 0x00000080,
+	0x5eb4, 0xffffffff, 0x00000002,
+	0x5e78, 0xffffffff, 0x001000f0,
+	0x6104, 0x01000300, 0x00000000,
+	0x5bc0, 0x00300000, 0x00000000,
+	0x7030, 0xffffffff, 0x00000011,
+	0x7c30, 0xffffffff, 0x00000011,
+	0x10830, 0xffffffff, 0x00000011,
+	0x11430, 0xffffffff, 0x00000011,
+	0xd02c, 0xffffffff, 0x08421000,
+	0x240c, 0xffffffff, 0x00000380,
+	0x8b24, 0xffffffff, 0x00ff0fff,
+	0x28a4c, 0x06000000, 0x06000000,
+	0x10c, 0x00000001, 0x00000001,
+	0x8d00, 0xffffffff, 0x100e4848,
+	0x8d04, 0xffffffff, 0x00164745,
+	0x8c00, 0xffffffff, 0xe4000003,
+	0x8c04, 0xffffffff, 0x40600060,
+	0x8c08, 0xffffffff, 0x001c001c,
+	0x8cf0, 0xffffffff, 0x08e00410,
+	0x8c20, 0xffffffff, 0x00800080,
+	0x8c24, 0xffffffff, 0x00800080,
+	0x8c18, 0xffffffff, 0x20202078,
+	0x8c1c, 0xffffffff, 0x00001010,
+	0x28350, 0xffffffff, 0x00000000,
+	0xa008, 0xffffffff, 0x00010000,
+	0x5cc, 0xffffffff, 0x00000001,
+	0x9508, 0xffffffff, 0x00000002
+};
+
+static const u32 cedar_mgcg_init[] =
+{
+	0x802c, 0xffffffff, 0xc0000000,
+	0x5448, 0xffffffff, 0x00000100,
+	0x55e4, 0xffffffff, 0x00000100,
+	0x160c, 0xffffffff, 0x00000100,
+	0x5644, 0xffffffff, 0x00000100,
+	0xc164, 0xffffffff, 0x00000100,
+	0x8a18, 0xffffffff, 0x00000100,
+	0x897c, 0xffffffff, 0x06000100,
+	0x8b28, 0xffffffff, 0x00000100,
+	0x9144, 0xffffffff, 0x00000100,
+	0x9a60, 0xffffffff, 0x00000100,
+	0x9868, 0xffffffff, 0x00000100,
+	0x8d58, 0xffffffff, 0x00000100,
+	0x9510, 0xffffffff, 0x00000100,
+	0x949c, 0xffffffff, 0x00000100,
+	0x9654, 0xffffffff, 0x00000100,
+	0x9030, 0xffffffff, 0x00000100,
+	0x9034, 0xffffffff, 0x00000100,
+	0x9038, 0xffffffff, 0x00000100,
+	0x903c, 0xffffffff, 0x00000100,
+	0x9040, 0xffffffff, 0x00000100,
+	0xa200, 0xffffffff, 0x00000100,
+	0xa204, 0xffffffff, 0x00000100,
+	0xa208, 0xffffffff, 0x00000100,
+	0xa20c, 0xffffffff, 0x00000100,
+	0x971c, 0xffffffff, 0x00000100,
+	0x977c, 0xffffffff, 0x00000100,
+	0x3f80, 0xffffffff, 0x00000100,
+	0xa210, 0xffffffff, 0x00000100,
+	0xa214, 0xffffffff, 0x00000100,
+	0x4d8, 0xffffffff, 0x00000100,
+	0x9784, 0xffffffff, 0x00000100,
+	0x9698, 0xffffffff, 0x00000100,
+	0x4d4, 0xffffffff, 0x00000200,
+	0x30cc, 0xffffffff, 0x00000100,
+	0xd0c0, 0xffffffff, 0xff000100,
+	0x802c, 0xffffffff, 0x40000000,
+	0x915c, 0xffffffff, 0x00010000,
+	0x9178, 0xffffffff, 0x00050000,
+	0x917c, 0xffffffff, 0x00030002,
+	0x918c, 0xffffffff, 0x00010004,
+	0x9190, 0xffffffff, 0x00070006,
+	0x9194, 0xffffffff, 0x00050000,
+	0x9198, 0xffffffff, 0x00030002,
+	0x91a8, 0xffffffff, 0x00010004,
+	0x91ac, 0xffffffff, 0x00070006,
+	0x91e8, 0xffffffff, 0x00000000,
+	0x9294, 0xffffffff, 0x00000000,
+	0x929c, 0xffffffff, 0x00000001,
+	0x802c, 0xffffffff, 0xc0000000
+};
+
+static const u32 juniper_mgcg_init[] =
+{
+	0x802c, 0xffffffff, 0xc0000000,
+	0x5448, 0xffffffff, 0x00000100,
+	0x55e4, 0xffffffff, 0x00000100,
+	0x160c, 0xffffffff, 0x00000100,
+	0x5644, 0xffffffff, 0x00000100,
+	0xc164, 0xffffffff, 0x00000100,
+	0x8a18, 0xffffffff, 0x00000100,
+	0x897c, 0xffffffff, 0x06000100,
+	0x8b28, 0xffffffff, 0x00000100,
+	0x9144, 0xffffffff, 0x00000100,
+	0x9a60, 0xffffffff, 0x00000100,
+	0x9868, 0xffffffff, 0x00000100,
+	0x8d58, 0xffffffff, 0x00000100,
+	0x9510, 0xffffffff, 0x00000100,
+	0x949c, 0xffffffff, 0x00000100,
+	0x9654, 0xffffffff, 0x00000100,
+	0x9030, 0xffffffff, 0x00000100,
+	0x9034, 0xffffffff, 0x00000100,
+	0x9038, 0xffffffff, 0x00000100,
+	0x903c, 0xffffffff, 0x00000100,
+	0x9040, 0xffffffff, 0x00000100,
+	0xa200, 0xffffffff, 0x00000100,
+	0xa204, 0xffffffff, 0x00000100,
+	0xa208, 0xffffffff, 0x00000100,
+	0xa20c, 0xffffffff, 0x00000100,
+	0x971c, 0xffffffff, 0x00000100,
+	0xd0c0, 0xffffffff, 0xff000100,
+	0x802c, 0xffffffff, 0x40000000,
+	0x915c, 0xffffffff, 0x00010000,
+	0x9160, 0xffffffff, 0x00030002,
+	0x9178, 0xffffffff, 0x00070000,
+	0x917c, 0xffffffff, 0x00030002,
+	0x9180, 0xffffffff, 0x00050004,
+	0x918c, 0xffffffff, 0x00010006,
+	0x9190, 0xffffffff, 0x00090008,
+	0x9194, 0xffffffff, 0x00070000,
+	0x9198, 0xffffffff, 0x00030002,
+	0x919c, 0xffffffff, 0x00050004,
+	0x91a8, 0xffffffff, 0x00010006,
+	0x91ac, 0xffffffff, 0x00090008,
+	0x91b0, 0xffffffff, 0x00070000,
+	0x91b4, 0xffffffff, 0x00030002,
+	0x91b8, 0xffffffff, 0x00050004,
+	0x91c4, 0xffffffff, 0x00010006,
+	0x91c8, 0xffffffff, 0x00090008,
+	0x91cc, 0xffffffff, 0x00070000,
+	0x91d0, 0xffffffff, 0x00030002,
+	0x91d4, 0xffffffff, 0x00050004,
+	0x91e0, 0xffffffff, 0x00010006,
+	0x91e4, 0xffffffff, 0x00090008,
+	0x91e8, 0xffffffff, 0x00000000,
+	0x91ec, 0xffffffff, 0x00070000,
+	0x91f0, 0xffffffff, 0x00030002,
+	0x91f4, 0xffffffff, 0x00050004,
+	0x9200, 0xffffffff, 0x00010006,
+	0x9204, 0xffffffff, 0x00090008,
+	0x9208, 0xffffffff, 0x00070000,
+	0x920c, 0xffffffff, 0x00030002,
+	0x9210, 0xffffffff, 0x00050004,
+	0x921c, 0xffffffff, 0x00010006,
+	0x9220, 0xffffffff, 0x00090008,
+	0x9224, 0xffffffff, 0x00070000,
+	0x9228, 0xffffffff, 0x00030002,
+	0x922c, 0xffffffff, 0x00050004,
+	0x9238, 0xffffffff, 0x00010006,
+	0x923c, 0xffffffff, 0x00090008,
+	0x9240, 0xffffffff, 0x00070000,
+	0x9244, 0xffffffff, 0x00030002,
+	0x9248, 0xffffffff, 0x00050004,
+	0x9254, 0xffffffff, 0x00010006,
+	0x9258, 0xffffffff, 0x00090008,
+	0x925c, 0xffffffff, 0x00070000,
+	0x9260, 0xffffffff, 0x00030002,
+	0x9264, 0xffffffff, 0x00050004,
+	0x9270, 0xffffffff, 0x00010006,
+	0x9274, 0xffffffff, 0x00090008,
+	0x9278, 0xffffffff, 0x00070000,
+	0x927c, 0xffffffff, 0x00030002,
+	0x9280, 0xffffffff, 0x00050004,
+	0x928c, 0xffffffff, 0x00010006,
+	0x9290, 0xffffffff, 0x00090008,
+	0x9294, 0xffffffff, 0x00000000,
+	0x929c, 0xffffffff, 0x00000001,
+	0x802c, 0xffffffff, 0xc0000000,
+	0x977c, 0xffffffff, 0x00000100,
+	0x3f80, 0xffffffff, 0x00000100,
+	0xa210, 0xffffffff, 0x00000100,
+	0xa214, 0xffffffff, 0x00000100,
+	0x4d8, 0xffffffff, 0x00000100,
+	0x9784, 0xffffffff, 0x00000100,
+	0x9698, 0xffffffff, 0x00000100,
+	0x4d4, 0xffffffff, 0x00000200,
+	0x30cc, 0xffffffff, 0x00000100,
+	0x802c, 0xffffffff, 0xc0000000
+};
+
+static const u32 supersumo_golden_registers[] =
+{
+	0x5eb4, 0xffffffff, 0x00000002,
+	0x5cc, 0xffffffff, 0x00000001,
+	0x7030, 0xffffffff, 0x00000011,
+	0x7c30, 0xffffffff, 0x00000011,
+	0x6104, 0x01000300, 0x00000000,
+	0x5bc0, 0x00300000, 0x00000000,
+	0x8c04, 0xffffffff, 0x40600060,
+	0x8c08, 0xffffffff, 0x001c001c,
+	0x8c20, 0xffffffff, 0x00800080,
+	0x8c24, 0xffffffff, 0x00800080,
+	0x8c18, 0xffffffff, 0x20202078,
+	0x8c1c, 0xffffffff, 0x00001010,
+	0x918c, 0xffffffff, 0x00010006,
+	0x91a8, 0xffffffff, 0x00010006,
+	0x91c4, 0xffffffff, 0x00010006,
+	0x91e0, 0xffffffff, 0x00010006,
+	0x9200, 0xffffffff, 0x00010006,
+	0x9150, 0xffffffff, 0x6e944040,
+	0x917c, 0xffffffff, 0x00030002,
+	0x9180, 0xffffffff, 0x00050004,
+	0x9198, 0xffffffff, 0x00030002,
+	0x919c, 0xffffffff, 0x00050004,
+	0x91b4, 0xffffffff, 0x00030002,
+	0x91b8, 0xffffffff, 0x00050004,
+	0x91d0, 0xffffffff, 0x00030002,
+	0x91d4, 0xffffffff, 0x00050004,
+	0x91f0, 0xffffffff, 0x00030002,
+	0x91f4, 0xffffffff, 0x00050004,
+	0x915c, 0xffffffff, 0x00010000,
+	0x9160, 0xffffffff, 0x00030002,
+	0x3f90, 0xffff0000, 0xff000000,
+	0x9178, 0xffffffff, 0x00070000,
+	0x9194, 0xffffffff, 0x00070000,
+	0x91b0, 0xffffffff, 0x00070000,
+	0x91cc, 0xffffffff, 0x00070000,
+	0x91ec, 0xffffffff, 0x00070000,
+	0x9148, 0xffff0000, 0xff000000,
+	0x9190, 0xffffffff, 0x00090008,
+	0x91ac, 0xffffffff, 0x00090008,
+	0x91c8, 0xffffffff, 0x00090008,
+	0x91e4, 0xffffffff, 0x00090008,
+	0x9204, 0xffffffff, 0x00090008,
+	0x3f94, 0xffff0000, 0xff000000,
+	0x914c, 0xffff0000, 0xff000000,
+	0x929c, 0xffffffff, 0x00000001,
+	0x8a18, 0xffffffff, 0x00000100,
+	0x8b28, 0xffffffff, 0x00000100,
+	0x9144, 0xffffffff, 0x00000100,
+	0x5644, 0xffffffff, 0x00000100,
+	0x9b7c, 0xffffffff, 0x00000000,
+	0x8030, 0xffffffff, 0x0000100a,
+	0x8a14, 0xffffffff, 0x00000007,
+	0x8b24, 0xffffffff, 0x00ff0fff,
+	0x8b10, 0xffffffff, 0x00000000,
+	0x28a4c, 0x06000000, 0x06000000,
+	0x4d8, 0xffffffff, 0x00000100,
+	0x913c, 0xffff000f, 0x0100000a,
+	0x960c, 0xffffffff, 0x54763210,
+	0x88c4, 0xffffffff, 0x000000c2,
+	0x88d4, 0xffffffff, 0x00000010,
+	0x8974, 0xffffffff, 0x00000000,
+	0xc78, 0x00000080, 0x00000080,
+	0x5e78, 0xffffffff, 0x001000f0,
+	0xd02c, 0xffffffff, 0x08421000,
+	0xa008, 0xffffffff, 0x00010000,
+	0x8d00, 0xffffffff, 0x100e4848,
+	0x8d04, 0xffffffff, 0x00164745,
+	0x8c00, 0xffffffff, 0xe4000003,
+	0x8cf0, 0x1fffffff, 0x08e00620,
+	0x28350, 0xffffffff, 0x00000000,
+	0x9508, 0xffffffff, 0x00000002
+};
+
+static const u32 sumo_golden_registers[] =
+{
+	0x900c, 0x00ffffff, 0x0017071f,
+	0x8c18, 0xffffffff, 0x10101060,
+	0x8c1c, 0xffffffff, 0x00001010,
+	0x8c30, 0x0000000f, 0x00000005,
+	0x9688, 0x0000000f, 0x00000007
+};
+
+static const u32 wrestler_golden_registers[] =
+{
+	0x5eb4, 0xffffffff, 0x00000002,
+	0x5cc, 0xffffffff, 0x00000001,
+	0x7030, 0xffffffff, 0x00000011,
+	0x7c30, 0xffffffff, 0x00000011,
+	0x6104, 0x01000300, 0x00000000,
+	0x5bc0, 0x00300000, 0x00000000,
+	0x918c, 0xffffffff, 0x00010006,
+	0x91a8, 0xffffffff, 0x00010006,
+	0x9150, 0xffffffff, 0x6e944040,
+	0x917c, 0xffffffff, 0x00030002,
+	0x9198, 0xffffffff, 0x00030002,
+	0x915c, 0xffffffff, 0x00010000,
+	0x3f90, 0xffff0000, 0xff000000,
+	0x9178, 0xffffffff, 0x00070000,
+	0x9194, 0xffffffff, 0x00070000,
+	0x9148, 0xffff0000, 0xff000000,
+	0x9190, 0xffffffff, 0x00090008,
+	0x91ac, 0xffffffff, 0x00090008,
+	0x3f94, 0xffff0000, 0xff000000,
+	0x914c, 0xffff0000, 0xff000000,
+	0x929c, 0xffffffff, 0x00000001,
+	0x8a18, 0xffffffff, 0x00000100,
+	0x8b28, 0xffffffff, 0x00000100,
+	0x9144, 0xffffffff, 0x00000100,
+	0x9b7c, 0xffffffff, 0x00000000,
+	0x8030, 0xffffffff, 0x0000100a,
+	0x8a14, 0xffffffff, 0x00000001,
+	0x8b24, 0xffffffff, 0x00ff0fff,
+	0x8b10, 0xffffffff, 0x00000000,
+	0x28a4c, 0x06000000, 0x06000000,
+	0x4d8, 0xffffffff, 0x00000100,
+	0x913c, 0xffff000f, 0x0100000a,
+	0x960c, 0xffffffff, 0x54763210,
+	0x88c4, 0xffffffff, 0x000000c2,
+	0x88d4, 0xffffffff, 0x00000010,
+	0x8974, 0xffffffff, 0x00000000,
+	0xc78, 0x00000080, 0x00000080,
+	0x5e78, 0xffffffff, 0x001000f0,
+	0xd02c, 0xffffffff, 0x08421000,
+	0xa008, 0xffffffff, 0x00010000,
+	0x8d00, 0xffffffff, 0x100e4848,
+	0x8d04, 0xffffffff, 0x00164745,
+	0x8c00, 0xffffffff, 0xe4000003,
+	0x8cf0, 0x1fffffff, 0x08e00410,
+	0x28350, 0xffffffff, 0x00000000,
+	0x9508, 0xffffffff, 0x00000002,
+	0x900c, 0xffffffff, 0x0017071f,
+	0x8c18, 0xffffffff, 0x10101060,
+	0x8c1c, 0xffffffff, 0x00001010
+};
+
+static const u32 barts_golden_registers[] =
+{
+	0x5eb4, 0xffffffff, 0x00000002,
+	0x5e78, 0x8f311ff1, 0x001000f0,
+	0x3f90, 0xffff0000, 0xff000000,
+	0x9148, 0xffff0000, 0xff000000,
+	0x3f94, 0xffff0000, 0xff000000,
+	0x914c, 0xffff0000, 0xff000000,
+	0xc78, 0x00000080, 0x00000080,
+	0xbd4, 0x70073777, 0x00010001,
+	0xd02c, 0xbfffff1f, 0x08421000,
+	0xd0b8, 0x03773777, 0x02011003,
+	0x5bc0, 0x00200000, 0x50100000,
+	0x98f8, 0x33773777, 0x02011003,
+	0x98fc, 0xffffffff, 0x76543210,
+	0x7030, 0x31000311, 0x00000011,
+	0x2f48, 0x00000007, 0x02011003,
+	0x6b28, 0x00000010, 0x00000012,
+	0x7728, 0x00000010, 0x00000012,
+	0x10328, 0x00000010, 0x00000012,
+	0x10f28, 0x00000010, 0x00000012,
+	0x11b28, 0x00000010, 0x00000012,
+	0x12728, 0x00000010, 0x00000012,
+	0x240c, 0x000007ff, 0x00000380,
+	0x8a14, 0xf000001f, 0x00000007,
+	0x8b24, 0x3fff3fff, 0x00ff0fff,
+	0x8b10, 0x0000ff0f, 0x00000000,
+	0x28a4c, 0x07ffffff, 0x06000000,
+	0x10c, 0x00000001, 0x00010003,
+	0xa02c, 0xffffffff, 0x0000009b,
+	0x913c, 0x0000000f, 0x0100000a,
+	0x8d00, 0xffff7f7f, 0x100e4848,
+	0x8d04, 0x00ffffff, 0x00164745,
+	0x8c00, 0xfffc0003, 0xe4000003,
+	0x8c04, 0xf8ff00ff, 0x40600060,
+	0x8c08, 0x00ff00ff, 0x001c001c,
+	0x8cf0, 0x1fff1fff, 0x08e00620,
+	0x8c20, 0x0fff0fff, 0x00800080,
+	0x8c24, 0x0fff0fff, 0x00800080,
+	0x8c18, 0xffffffff, 0x20202078,
+	0x8c1c, 0x0000ffff, 0x00001010,
+	0x28350, 0x00000f01, 0x00000000,
+	0x9508, 0x3700001f, 0x00000002,
+	0x960c, 0xffffffff, 0x54763210,
+	0x88c4, 0x001f3ae3, 0x000000c2,
+	0x88d4, 0x0000001f, 0x00000010,
+	0x8974, 0xffffffff, 0x00000000
+};
+
+static const u32 turks_golden_registers[] =
+{
+	0x5eb4, 0xffffffff, 0x00000002,
+	0x5e78, 0x8f311ff1, 0x001000f0,
+	0x8c8, 0x00003000, 0x00001070,
+	0x8cc, 0x000fffff, 0x00040035,
+	0x3f90, 0xffff0000, 0xfff00000,
+	0x9148, 0xffff0000, 0xfff00000,
+	0x3f94, 0xffff0000, 0xfff00000,
+	0x914c, 0xffff0000, 0xfff00000,
+	0xc78, 0x00000080, 0x00000080,
+	0xbd4, 0x00073007, 0x00010002,
+	0xd02c, 0xbfffff1f, 0x08421000,
+	0xd0b8, 0x03773777, 0x02010002,
+	0x5bc0, 0x00200000, 0x50100000,
+	0x98f8, 0x33773777, 0x00010002,
+	0x98fc, 0xffffffff, 0x33221100,
+	0x7030, 0x31000311, 0x00000011,
+	0x2f48, 0x33773777, 0x00010002,
+	0x6b28, 0x00000010, 0x00000012,
+	0x7728, 0x00000010, 0x00000012,
+	0x10328, 0x00000010, 0x00000012,
+	0x10f28, 0x00000010, 0x00000012,
+	0x11b28, 0x00000010, 0x00000012,
+	0x12728, 0x00000010, 0x00000012,
+	0x240c, 0x000007ff, 0x00000380,
+	0x8a14, 0xf000001f, 0x00000007,
+	0x8b24, 0x3fff3fff, 0x00ff0fff,
+	0x8b10, 0x0000ff0f, 0x00000000,
+	0x28a4c, 0x07ffffff, 0x06000000,
+	0x10c, 0x00000001, 0x00010003,
+	0xa02c, 0xffffffff, 0x0000009b,
+	0x913c, 0x0000000f, 0x0100000a,
+	0x8d00, 0xffff7f7f, 0x100e4848,
+	0x8d04, 0x00ffffff, 0x00164745,
+	0x8c00, 0xfffc0003, 0xe4000003,
+	0x8c04, 0xf8ff00ff, 0x40600060,
+	0x8c08, 0x00ff00ff, 0x001c001c,
+	0x8cf0, 0x1fff1fff, 0x08e00410,
+	0x8c20, 0x0fff0fff, 0x00800080,
+	0x8c24, 0x0fff0fff, 0x00800080,
+	0x8c18, 0xffffffff, 0x20202078,
+	0x8c1c, 0x0000ffff, 0x00001010,
+	0x28350, 0x00000f01, 0x00000000,
+	0x9508, 0x3700001f, 0x00000002,
+	0x960c, 0xffffffff, 0x54763210,
+	0x88c4, 0x001f3ae3, 0x000000c2,
+	0x88d4, 0x0000001f, 0x00000010,
+	0x8974, 0xffffffff, 0x00000000
+};
+
+static const u32 caicos_golden_registers[] =
+{
+	0x5eb4, 0xffffffff, 0x00000002,
+	0x5e78, 0x8f311ff1, 0x001000f0,
+	0x8c8, 0x00003420, 0x00001450,
+	0x8cc, 0x000fffff, 0x00040035,
+	0x3f90, 0xffff0000, 0xfffc0000,
+	0x9148, 0xffff0000, 0xfffc0000,
+	0x3f94, 0xffff0000, 0xfffc0000,
+	0x914c, 0xffff0000, 0xfffc0000,
+	0xc78, 0x00000080, 0x00000080,
+	0xbd4, 0x00073007, 0x00010001,
+	0xd02c, 0xbfffff1f, 0x08421000,
+	0xd0b8, 0x03773777, 0x02010001,
+	0x5bc0, 0x00200000, 0x50100000,
+	0x98f8, 0x33773777, 0x02010001,
+	0x98fc, 0xffffffff, 0x33221100,
+	0x7030, 0x31000311, 0x00000011,
+	0x2f48, 0x33773777, 0x02010001,
+	0x6b28, 0x00000010, 0x00000012,
+	0x7728, 0x00000010, 0x00000012,
+	0x10328, 0x00000010, 0x00000012,
+	0x10f28, 0x00000010, 0x00000012,
+	0x11b28, 0x00000010, 0x00000012,
+	0x12728, 0x00000010, 0x00000012,
+	0x240c, 0x000007ff, 0x00000380,
+	0x8a14, 0xf000001f, 0x00000001,
+	0x8b24, 0x3fff3fff, 0x00ff0fff,
+	0x8b10, 0x0000ff0f, 0x00000000,
+	0x28a4c, 0x07ffffff, 0x06000000,
+	0x10c, 0x00000001, 0x00010003,
+	0xa02c, 0xffffffff, 0x0000009b,
+	0x913c, 0x0000000f, 0x0100000a,
+	0x8d00, 0xffff7f7f, 0x100e4848,
+	0x8d04, 0x00ffffff, 0x00164745,
+	0x8c00, 0xfffc0003, 0xe4000003,
+	0x8c04, 0xf8ff00ff, 0x40600060,
+	0x8c08, 0x00ff00ff, 0x001c001c,
+	0x8cf0, 0x1fff1fff, 0x08e00410,
+	0x8c20, 0x0fff0fff, 0x00800080,
+	0x8c24, 0x0fff0fff, 0x00800080,
+	0x8c18, 0xffffffff, 0x20202078,
+	0x8c1c, 0x0000ffff, 0x00001010,
+	0x28350, 0x00000f01, 0x00000000,
+	0x9508, 0x3700001f, 0x00000002,
+	0x960c, 0xffffffff, 0x54763210,
+	0x88c4, 0x001f3ae3, 0x000000c2,
+	0x88d4, 0x0000001f, 0x00000010,
+	0x8974, 0xffffffff, 0x00000000
+};
+
+static void evergreen_init_golden_registers(struct radeon_device *rdev)
+{
+	switch (rdev->family) {
+	case CHIP_CYPRESS:
+	case CHIP_HEMLOCK:
+		radeon_program_register_sequence(rdev,
+						 evergreen_golden_registers,
+						 (const u32)ARRAY_SIZE(evergreen_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 evergreen_golden_registers2,
+						 (const u32)ARRAY_SIZE(evergreen_golden_registers2));
+		radeon_program_register_sequence(rdev,
+						 cypress_mgcg_init,
+						 (const u32)ARRAY_SIZE(cypress_mgcg_init));
+		break;
+	case CHIP_JUNIPER:
+		radeon_program_register_sequence(rdev,
+						 evergreen_golden_registers,
+						 (const u32)ARRAY_SIZE(evergreen_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 evergreen_golden_registers2,
+						 (const u32)ARRAY_SIZE(evergreen_golden_registers2));
+		radeon_program_register_sequence(rdev,
+						 juniper_mgcg_init,
+						 (const u32)ARRAY_SIZE(juniper_mgcg_init));
+		break;
+	case CHIP_REDWOOD:
+		radeon_program_register_sequence(rdev,
+						 evergreen_golden_registers,
+						 (const u32)ARRAY_SIZE(evergreen_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 evergreen_golden_registers2,
+						 (const u32)ARRAY_SIZE(evergreen_golden_registers2));
+		radeon_program_register_sequence(rdev,
+						 redwood_mgcg_init,
+						 (const u32)ARRAY_SIZE(redwood_mgcg_init));
+		break;
+	case CHIP_CEDAR:
+		radeon_program_register_sequence(rdev,
+						 cedar_golden_registers,
+						 (const u32)ARRAY_SIZE(cedar_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 evergreen_golden_registers2,
+						 (const u32)ARRAY_SIZE(evergreen_golden_registers2));
+		radeon_program_register_sequence(rdev,
+						 cedar_mgcg_init,
+						 (const u32)ARRAY_SIZE(cedar_mgcg_init));
+		break;
+	case CHIP_PALM:
+		radeon_program_register_sequence(rdev,
+						 wrestler_golden_registers,
+						 (const u32)ARRAY_SIZE(wrestler_golden_registers));
+		break;
+	case CHIP_SUMO:
+		radeon_program_register_sequence(rdev,
+						 supersumo_golden_registers,
+						 (const u32)ARRAY_SIZE(supersumo_golden_registers));
+		break;
+	case CHIP_SUMO2:
+		radeon_program_register_sequence(rdev,
+						 supersumo_golden_registers,
+						 (const u32)ARRAY_SIZE(supersumo_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 sumo_golden_registers,
+						 (const u32)ARRAY_SIZE(sumo_golden_registers));
+		break;
+	case CHIP_BARTS:
+		radeon_program_register_sequence(rdev,
+						 barts_golden_registers,
+						 (const u32)ARRAY_SIZE(barts_golden_registers));
+		break;
+	case CHIP_TURKS:
+		radeon_program_register_sequence(rdev,
+						 turks_golden_registers,
+						 (const u32)ARRAY_SIZE(turks_golden_registers));
+		break;
+	case CHIP_CAICOS:
+		radeon_program_register_sequence(rdev,
+						 caicos_golden_registers,
+						 (const u32)ARRAY_SIZE(caicos_golden_registers));
+		break;
+	default:
+		break;
+	}
+}
+
 void evergreen_tiling_fields(unsigned tiling_flags, unsigned *bankw,
 			     unsigned *bankh, unsigned *mtaspect,
 			     unsigned *tile_split)
@@ -84,6 +942,142 @@ void evergreen_tiling_fields(unsigned tiling_flags, unsigned *bankw,
 	}
 }
 
+static int sumo_set_uvd_clock(struct radeon_device *rdev, u32 clock,
+			      u32 cntl_reg, u32 status_reg)
+{
+	int r, i;
+	struct atom_clock_dividers dividers;
+
+        r = radeon_atom_get_clock_dividers(rdev, COMPUTE_ENGINE_PLL_PARAM,
+					   clock, false, &dividers);
+	if (r)
+		return r;
+
+	WREG32_P(cntl_reg, dividers.post_div, ~(DCLK_DIR_CNTL_EN|DCLK_DIVIDER_MASK));
+
+	for (i = 0; i < 100; i++) {
+		if (RREG32(status_reg) & DCLK_STATUS)
+			break;
+		mdelay(10);
+	}
+	if (i == 100)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+int sumo_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk)
+{
+	int r = 0;
+	u32 cg_scratch = RREG32(CG_SCRATCH1);
+
+	r = sumo_set_uvd_clock(rdev, vclk, CG_VCLK_CNTL, CG_VCLK_STATUS);
+	if (r)
+		goto done;
+	cg_scratch &= 0xffff0000;
+	cg_scratch |= vclk / 100; /* Mhz */
+
+	r = sumo_set_uvd_clock(rdev, dclk, CG_DCLK_CNTL, CG_DCLK_STATUS);
+	if (r)
+		goto done;
+	cg_scratch &= 0x0000ffff;
+	cg_scratch |= (dclk / 100) << 16; /* Mhz */
+
+done:
+	WREG32(CG_SCRATCH1, cg_scratch);
+
+	return r;
+}
+
+int evergreen_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk)
+{
+	/* start off with something large */
+	unsigned fb_div = 0, vclk_div = 0, dclk_div = 0;
+	int r;
+
+	/* bypass vclk and dclk with bclk */
+	WREG32_P(CG_UPLL_FUNC_CNTL_2,
+		VCLK_SRC_SEL(1) | DCLK_SRC_SEL(1),
+		~(VCLK_SRC_SEL_MASK | DCLK_SRC_SEL_MASK));
+
+	/* put PLL in bypass mode */
+	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_BYPASS_EN_MASK, ~UPLL_BYPASS_EN_MASK);
+
+	if (!vclk || !dclk) {
+		/* keep the Bypass mode, put PLL to sleep */
+		WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_SLEEP_MASK, ~UPLL_SLEEP_MASK);
+		return 0;
+	}
+
+	r = radeon_uvd_calc_upll_dividers(rdev, vclk, dclk, 125000, 250000,
+					  16384, 0x03FFFFFF, 0, 128, 5,
+					  &fb_div, &vclk_div, &dclk_div);
+	if (r)
+		return r;
+
+	/* set VCO_MODE to 1 */
+	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_VCO_MODE_MASK, ~UPLL_VCO_MODE_MASK);
+
+	/* toggle UPLL_SLEEP to 1 then back to 0 */
+	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_SLEEP_MASK, ~UPLL_SLEEP_MASK);
+	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_SLEEP_MASK);
+
+	/* deassert UPLL_RESET */
+	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_RESET_MASK);
+
+	mdelay(1);
+
+	r = radeon_uvd_send_upll_ctlreq(rdev, CG_UPLL_FUNC_CNTL);
+	if (r)
+		return r;
+
+	/* assert UPLL_RESET again */
+	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_RESET_MASK, ~UPLL_RESET_MASK);
+
+	/* disable spread spectrum. */
+	WREG32_P(CG_UPLL_SPREAD_SPECTRUM, 0, ~SSEN_MASK);
+
+	/* set feedback divider */
+	WREG32_P(CG_UPLL_FUNC_CNTL_3, UPLL_FB_DIV(fb_div), ~UPLL_FB_DIV_MASK);
+
+	/* set ref divider to 0 */
+	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_REF_DIV_MASK);
+
+	if (fb_div < 307200)
+		WREG32_P(CG_UPLL_FUNC_CNTL_4, 0, ~UPLL_SPARE_ISPARE9);
+	else
+		WREG32_P(CG_UPLL_FUNC_CNTL_4, UPLL_SPARE_ISPARE9, ~UPLL_SPARE_ISPARE9);
+
+	/* set PDIV_A and PDIV_B */
+	WREG32_P(CG_UPLL_FUNC_CNTL_2,
+		UPLL_PDIV_A(vclk_div) | UPLL_PDIV_B(dclk_div),
+		~(UPLL_PDIV_A_MASK | UPLL_PDIV_B_MASK));
+
+	/* give the PLL some time to settle */
+	mdelay(15);
+
+	/* deassert PLL_RESET */
+	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_RESET_MASK);
+
+	mdelay(15);
+
+	/* switch from bypass mode to normal mode */
+	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_BYPASS_EN_MASK);
+
+	r = radeon_uvd_send_upll_ctlreq(rdev, CG_UPLL_FUNC_CNTL);
+	if (r)
+		return r;
+
+	/* switch VCLK and DCLK selection */
+	WREG32_P(CG_UPLL_FUNC_CNTL_2,
+		VCLK_SRC_SEL(2) | DCLK_SRC_SEL(2),
+		~(VCLK_SRC_SEL_MASK | DCLK_SRC_SEL_MASK));
+
+	mdelay(100);
+
+	return 0;
+}
+
 void evergreen_fix_pci_max_read_req_size(struct radeon_device *rdev)
 {
 	u16 ctl, v;
@@ -105,6 +1099,27 @@ void evergreen_fix_pci_max_read_req_size(struct radeon_device *rdev)
 	}
 }
 
+static bool dce4_is_in_vblank(struct radeon_device *rdev, int crtc)
+{
+	if (RREG32(EVERGREEN_CRTC_STATUS + crtc_offsets[crtc]) & EVERGREEN_CRTC_V_BLANK)
+		return true;
+	else
+		return false;
+}
+
+static bool dce4_is_counter_moving(struct radeon_device *rdev, int crtc)
+{
+	u32 pos1, pos2;
+
+	pos1 = RREG32(EVERGREEN_CRTC_STATUS_POSITION + crtc_offsets[crtc]);
+	pos2 = RREG32(EVERGREEN_CRTC_STATUS_POSITION + crtc_offsets[crtc]);
+
+	if (pos1 != pos2)
+		return true;
+	else
+		return false;
+}
+
 /**
  * dce4_wait_for_vblank - vblank wait asic callback.
  *
@@ -115,21 +1130,28 @@ void evergreen_fix_pci_max_read_req_size(struct radeon_device *rdev)
  */
 void dce4_wait_for_vblank(struct radeon_device *rdev, int crtc)
 {
-	int i;
+	unsigned i = 0;
 
 	if (crtc >= rdev->num_crtc)
 		return;
 
-	if (RREG32(EVERGREEN_CRTC_CONTROL + crtc_offsets[crtc]) & EVERGREEN_CRTC_MASTER_EN) {
-		for (i = 0; i < rdev->usec_timeout; i++) {
-			if (!(RREG32(EVERGREEN_CRTC_STATUS + crtc_offsets[crtc]) & EVERGREEN_CRTC_V_BLANK))
+	if (!(RREG32(EVERGREEN_CRTC_CONTROL + crtc_offsets[crtc]) & EVERGREEN_CRTC_MASTER_EN))
+		return;
+
+	/* depending on when we hit vblank, we may be close to active; if so,
+	 * wait for another frame.
+	 */
+	while (dce4_is_in_vblank(rdev, crtc)) {
+		if (i++ % 100 == 0) {
+			if (!dce4_is_counter_moving(rdev, crtc))
 				break;
-			udelay(1);
 		}
-		for (i = 0; i < rdev->usec_timeout; i++) {
-			if (RREG32(EVERGREEN_CRTC_STATUS + crtc_offsets[crtc]) & EVERGREEN_CRTC_V_BLANK)
+	}
+
+	while (!dce4_is_in_vblank(rdev, crtc)) {
+		if (i++ % 100 == 0) {
+			if (!dce4_is_counter_moving(rdev, crtc))
 				break;
-			udelay(1);
 		}
 	}
 }
@@ -608,6 +1630,16 @@ void evergreen_hpd_init(struct radeon_device *rdev)
 
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
 		struct radeon_connector *radeon_connector = to_radeon_connector(connector);
+
+		if (connector->connector_type == DRM_MODE_CONNECTOR_eDP ||
+		    connector->connector_type == DRM_MODE_CONNECTOR_LVDS) {
+			/* don't try to enable hpd on eDP or LVDS avoid breaking the
+			 * aux dp channel on imac and help (but not completely fix)
+			 * https://bugzilla.redhat.com/show_bug.cgi?id=726143
+			 * also avoid interrupt storms during dpms.
+			 */
+			continue;
+		}
 		switch (radeon_connector->hpd.hpd) {
 		case RADEON_HPD_1:
 			WREG32(DC_HPD1_CONTROL, tmp);
@@ -1325,17 +2357,16 @@ void evergreen_mc_stop(struct radeon_device *rdev, struct evergreen_mc_save *sav
 				tmp = RREG32(EVERGREEN_CRTC_BLANK_CONTROL + crtc_offsets[i]);
 				if (!(tmp & EVERGREEN_CRTC_BLANK_DATA_EN)) {
 					radeon_wait_for_vblank(rdev, i);
-					tmp |= EVERGREEN_CRTC_BLANK_DATA_EN;
 					WREG32(EVERGREEN_CRTC_UPDATE_LOCK + crtc_offsets[i], 1);
+					tmp |= EVERGREEN_CRTC_BLANK_DATA_EN;
 					WREG32(EVERGREEN_CRTC_BLANK_CONTROL + crtc_offsets[i], tmp);
-					WREG32(EVERGREEN_CRTC_UPDATE_LOCK + crtc_offsets[i], 0);
 				}
 			} else {
 				tmp = RREG32(EVERGREEN_CRTC_CONTROL + crtc_offsets[i]);
 				if (!(tmp & EVERGREEN_CRTC_DISP_READ_REQUEST_DISABLE)) {
 					radeon_wait_for_vblank(rdev, i);
-					tmp |= EVERGREEN_CRTC_DISP_READ_REQUEST_DISABLE;
 					WREG32(EVERGREEN_CRTC_UPDATE_LOCK + crtc_offsets[i], 1);
+					tmp |= EVERGREEN_CRTC_DISP_READ_REQUEST_DISABLE;
 					WREG32(EVERGREEN_CRTC_CONTROL + crtc_offsets[i], tmp);
 					WREG32(EVERGREEN_CRTC_UPDATE_LOCK + crtc_offsets[i], 0);
 				}
@@ -1347,6 +2378,15 @@ void evergreen_mc_stop(struct radeon_device *rdev, struct evergreen_mc_save *sav
 					break;
 				udelay(1);
 			}
+
+			/* XXX this is a hack to avoid strange behavior with EFI on certain systems */
+			WREG32(EVERGREEN_CRTC_UPDATE_LOCK + crtc_offsets[i], 1);
+			tmp = RREG32(EVERGREEN_CRTC_CONTROL + crtc_offsets[i]);
+			tmp &= ~EVERGREEN_CRTC_MASTER_EN;
+			WREG32(EVERGREEN_CRTC_CONTROL + crtc_offsets[i], tmp);
+			WREG32(EVERGREEN_CRTC_UPDATE_LOCK + crtc_offsets[i], 0);
+			save->crtc_enabled[i] = false;
+			/* ***** */
 		} else {
 			save->crtc_enabled[i] = false;
 		}
@@ -1364,6 +2404,22 @@ void evergreen_mc_stop(struct radeon_device *rdev, struct evergreen_mc_save *sav
 	}
 	/* wait for the MC to settle */
 	udelay(100);
+
+	/* lock double buffered regs */
+	for (i = 0; i < rdev->num_crtc; i++) {
+		if (save->crtc_enabled[i]) {
+			tmp = RREG32(EVERGREEN_GRPH_UPDATE + crtc_offsets[i]);
+			if (!(tmp & EVERGREEN_GRPH_UPDATE_LOCK)) {
+				tmp |= EVERGREEN_GRPH_UPDATE_LOCK;
+				WREG32(EVERGREEN_GRPH_UPDATE + crtc_offsets[i], tmp);
+			}
+			tmp = RREG32(EVERGREEN_MASTER_UPDATE_LOCK + crtc_offsets[i]);
+			if (!(tmp & 1)) {
+				tmp |= 1;
+				WREG32(EVERGREEN_MASTER_UPDATE_LOCK + crtc_offsets[i], tmp);
+			}
+		}
+	}
 }
 
 void evergreen_mc_resume(struct radeon_device *rdev, struct evergreen_mc_save *save)
@@ -1385,6 +2441,33 @@ void evergreen_mc_resume(struct radeon_device *rdev, struct evergreen_mc_save *s
 	WREG32(EVERGREEN_VGA_MEMORY_BASE_ADDRESS_HIGH, upper_32_bits(rdev->mc.vram_start));
 	WREG32(EVERGREEN_VGA_MEMORY_BASE_ADDRESS, (u32)rdev->mc.vram_start);
 
+	/* unlock regs and wait for update */
+	for (i = 0; i < rdev->num_crtc; i++) {
+		if (save->crtc_enabled[i]) {
+			tmp = RREG32(EVERGREEN_MASTER_UPDATE_MODE + crtc_offsets[i]);
+			if ((tmp & 0x3) != 0) {
+				tmp &= ~0x3;
+				WREG32(EVERGREEN_MASTER_UPDATE_MODE + crtc_offsets[i], tmp);
+			}
+			tmp = RREG32(EVERGREEN_GRPH_UPDATE + crtc_offsets[i]);
+			if (tmp & EVERGREEN_GRPH_UPDATE_LOCK) {
+				tmp &= ~EVERGREEN_GRPH_UPDATE_LOCK;
+				WREG32(EVERGREEN_GRPH_UPDATE + crtc_offsets[i], tmp);
+			}
+			tmp = RREG32(EVERGREEN_MASTER_UPDATE_LOCK + crtc_offsets[i]);
+			if (tmp & 1) {
+				tmp &= ~1;
+				WREG32(EVERGREEN_MASTER_UPDATE_LOCK + crtc_offsets[i], tmp);
+			}
+			for (j = 0; j < rdev->usec_timeout; j++) {
+				tmp = RREG32(EVERGREEN_GRPH_UPDATE + crtc_offsets[i]);
+				if ((tmp & EVERGREEN_GRPH_SURFACE_UPDATE_PENDING) == 0)
+					break;
+				udelay(1);
+			}
+		}
+	}
+
 	/* unblackout the MC */
 	tmp = RREG32(MC_SHARED_BLACKOUT_CNTL);
 	tmp &= ~BLACKOUT_MODE_MASK;
@@ -2050,6 +3133,14 @@ static void evergreen_gpu_init(struct radeon_device *rdev)
 	}
 	/* enabled rb are just the one not disabled :) */
 	disabled_rb_mask = tmp;
+	tmp = 0;
+	for (i = 0; i < rdev->config.evergreen.max_backends; i++)
+		tmp |= (1 << i);
+	/* if all the backends are disabled, fix it up here */
+	if ((disabled_rb_mask & tmp) == tmp) {
+		for (i = 0; i < rdev->config.evergreen.max_backends; i++)
+			disabled_rb_mask &= ~(1 << i);
+	}
 
 	WREG32(GRBM_GFX_INDEX, INSTANCE_BROADCAST_WRITES | SE_BROADCAST_WRITES);
 	WREG32(RLC_GFX_INDEX, INSTANCE_BROADCAST_WRITES | SE_BROADCAST_WRITES);
@@ -2058,6 +3149,9 @@ static void evergreen_gpu_init(struct radeon_device *rdev)
 	WREG32(DMIF_ADDR_CONFIG, gb_addr_config);
 	WREG32(HDP_ADDR_CONFIG, gb_addr_config);
 	WREG32(DMA_TILING_CONFIG, gb_addr_config);
+	WREG32(UVD_UDEC_ADDR_CONFIG, gb_addr_config);
+	WREG32(UVD_UDEC_DB_ADDR_CONFIG, gb_addr_config);
+	WREG32(UVD_UDEC_DBW_ADDR_CONFIG, gb_addr_config);
 
 	if ((rdev->config.evergreen.max_backends == 1) &&
 	    (rdev->flags & RADEON_IS_IGP)) {
@@ -3360,6 +4454,9 @@ restart_ih:
 				DRM_ERROR("Unhandled interrupt: %d %d\n", src_id, src_data);
 				break;
 			}
+		case 124: /* UVD */
+			DRM_DEBUG("IH: UVD int: 0x%08x\n", src_data);
+			radeon_fence_process(rdev, R600_RING_TYPE_UVD_INDEX);
 			break;
 		case 146:
 		case 147:
@@ -3571,7 +4668,7 @@ int evergreen_copy_dma(struct radeon_device *rdev,
 
 static int evergreen_startup(struct radeon_device *rdev)
 {
-	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
+	struct radeon_ring *ring;
 	int r;
 
 	/* enable pcie gen2 link */
@@ -3638,6 +4735,17 @@ static int evergreen_startup(struct radeon_device *rdev)
 		return r;
 	}
 
+	r = rv770_uvd_resume(rdev);
+	if (!r) {
+		r = radeon_fence_driver_start_ring(rdev,
+						   R600_RING_TYPE_UVD_INDEX);
+		if (r)
+			dev_err(rdev->dev, "UVD fences init error (%d).\n", r);
+	}
+
+	if (r)
+		rdev->ring[R600_RING_TYPE_UVD_INDEX].ring_size = 0;
+
 	/* Enable IRQ */
 	r = r600_irq_init(rdev);
 	if (r) {
@@ -3647,6 +4755,7 @@ static int evergreen_startup(struct radeon_device *rdev)
 	}
 	evergreen_irq_set(rdev);
 
+	ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	r = radeon_ring_init(rdev, ring, ring->ring_size, RADEON_WB_CP_RPTR_OFFSET,
 			     R600_CP_RB_RPTR, R600_CP_RB_WPTR,
 			     0, 0xfffff, RADEON_CP_PACKET2);
@@ -3670,6 +4779,19 @@ static int evergreen_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	ring = &rdev->ring[R600_RING_TYPE_UVD_INDEX];
+	if (ring->ring_size) {
+		r = radeon_ring_init(rdev, ring, ring->ring_size,
+				     R600_WB_UVD_RPTR_OFFSET,
+				     UVD_RBC_RB_RPTR, UVD_RBC_RB_WPTR,
+				     0, 0xfffff, RADEON_CP_PACKET2);
+		if (!r)
+			r = r600_uvd_init(rdev);
+
+		if (r)
+			DRM_ERROR("radeon: error initializing UVD (%d).\n", r);
+	}
+
 	r = radeon_ib_pool_init(rdev);
 	if (r) {
 		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
@@ -3701,6 +4823,9 @@ int evergreen_resume(struct radeon_device *rdev)
 	/* post card */
 	atom_asic_init(rdev->mode_info.atom_context);
 
+	/* init golden registers */
+	evergreen_init_golden_registers(rdev);
+
 	rdev->accel_working = true;
 	r = evergreen_startup(rdev);
 	if (r) {
@@ -3716,8 +4841,10 @@ int evergreen_resume(struct radeon_device *rdev)
 int evergreen_suspend(struct radeon_device *rdev)
 {
 	r600_audio_fini(rdev);
+	radeon_uvd_suspend(rdev);
 	r700_cp_stop(rdev);
 	r600_dma_stop(rdev);
+	r600_uvd_rbc_stop(rdev);
 	evergreen_irq_suspend(rdev);
 	radeon_wb_disable(rdev);
 	evergreen_pcie_gart_disable(rdev);
@@ -3762,6 +4889,8 @@ int evergreen_init(struct radeon_device *rdev)
 		DRM_INFO("GPU not posted. posting now...\n");
 		atom_asic_init(rdev->mode_info.atom_context);
 	}
+	/* init golden registers */
+	evergreen_init_golden_registers(rdev);
 	/* Initialize scratch registers */
 	r600_scratch_init(rdev);
 	/* Initialize surface registers */
@@ -3797,6 +4926,13 @@ int evergreen_init(struct radeon_device *rdev)
 	rdev->ring[R600_RING_TYPE_DMA_INDEX].ring_obj = NULL;
 	r600_ring_init(rdev, &rdev->ring[R600_RING_TYPE_DMA_INDEX], 64 * 1024);
 
+	r = radeon_uvd_init(rdev);
+	if (!r) {
+		rdev->ring[R600_RING_TYPE_UVD_INDEX].ring_obj = NULL;
+		r600_ring_init(rdev, &rdev->ring[R600_RING_TYPE_UVD_INDEX],
+			       4096);
+	}
+
 	rdev->ih.ring_obj = NULL;
 	r600_ih_ring_init(rdev, 64 * 1024);
 
@@ -3843,6 +4979,7 @@ void evergreen_fini(struct radeon_device *rdev)
 	radeon_ib_pool_fini(rdev);
 	radeon_irq_kms_fini(rdev);
 	evergreen_pcie_gart_fini(rdev);
+	radeon_uvd_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
 	radeon_fence_driver_fini(rdev);
@@ -3878,7 +5015,7 @@ 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);
+	speed_cntl = RREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL);
 	if (speed_cntl & LC_CURRENT_DATA_RATE) {
 		DRM_INFO("PCIE gen 2 link speeds already enabled\n");
 		return;
@@ -3889,33 +5026,33 @@ void evergreen_pcie_gen2_enable(struct radeon_device *rdev)
 	if ((speed_cntl & LC_OTHER_SIDE_EVER_SENT_GEN2) ||
 	    (speed_cntl & LC_OTHER_SIDE_SUPPORTS_GEN2)) {
 
-		link_width_cntl = RREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL);
+		link_width_cntl = RREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL);
 		link_width_cntl &= ~LC_UPCONFIGURE_DIS;
-		WREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
 
-		speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
+		speed_cntl = RREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL);
 		speed_cntl &= ~LC_TARGET_LINK_SPEED_OVERRIDE_EN;
-		WREG32_PCIE_P(PCIE_LC_SPEED_CNTL, speed_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL, speed_cntl);
 
-		speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
+		speed_cntl = RREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL);
 		speed_cntl |= LC_CLR_FAILED_SPD_CHANGE_CNT;
-		WREG32_PCIE_P(PCIE_LC_SPEED_CNTL, speed_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL, speed_cntl);
 
-		speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
+		speed_cntl = RREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL);
 		speed_cntl &= ~LC_CLR_FAILED_SPD_CHANGE_CNT;
-		WREG32_PCIE_P(PCIE_LC_SPEED_CNTL, speed_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL, speed_cntl);
 
-		speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
+		speed_cntl = RREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL);
 		speed_cntl |= LC_GEN2_EN_STRAP;
-		WREG32_PCIE_P(PCIE_LC_SPEED_CNTL, speed_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL, speed_cntl);
 
 	} else {
-		link_width_cntl = RREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL);
+		link_width_cntl = RREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL);
 		/* XXX: only disable it if gen1 bridge vendor == 0x111d or 0x1106 */
 		if (1)
 			link_width_cntl |= LC_UPCONFIGURE_DIS;
 		else
 			link_width_cntl &= ~LC_UPCONFIGURE_DIS;
-		WREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
 	}
 }
diff --git a/drivers/gpu/drm/radeon/evergreen_hdmi.c b/drivers/gpu/drm/radeon/evergreen_hdmi.c
index 4fdecc2b4040..b4ab8ceb1654 100644
--- a/drivers/gpu/drm/radeon/evergreen_hdmi.c
+++ b/drivers/gpu/drm/radeon/evergreen_hdmi.c
@@ -54,6 +54,68 @@ static void evergreen_hdmi_update_ACR(struct drm_encoder *encoder, uint32_t cloc
 	WREG32(HDMI_ACR_48_1 + offset, acr.n_48khz);
 }
 
+static void evergreen_hdmi_write_sad_regs(struct drm_encoder *encoder)
+{
+	struct radeon_device *rdev = encoder->dev->dev_private;
+	struct drm_connector *connector;
+	struct radeon_connector *radeon_connector = NULL;
+	struct cea_sad *sads;
+	int i, sad_count;
+
+	static const u16 eld_reg_to_type[][2] = {
+		{ AZ_F0_CODEC_PIN0_CONTROL_AUDIO_DESCRIPTOR0, HDMI_AUDIO_CODING_TYPE_PCM },
+		{ AZ_F0_CODEC_PIN0_CONTROL_AUDIO_DESCRIPTOR1, HDMI_AUDIO_CODING_TYPE_AC3 },
+		{ AZ_F0_CODEC_PIN0_CONTROL_AUDIO_DESCRIPTOR2, HDMI_AUDIO_CODING_TYPE_MPEG1 },
+		{ AZ_F0_CODEC_PIN0_CONTROL_AUDIO_DESCRIPTOR3, HDMI_AUDIO_CODING_TYPE_MP3 },
+		{ AZ_F0_CODEC_PIN0_CONTROL_AUDIO_DESCRIPTOR4, HDMI_AUDIO_CODING_TYPE_MPEG2 },
+		{ AZ_F0_CODEC_PIN0_CONTROL_AUDIO_DESCRIPTOR5, HDMI_AUDIO_CODING_TYPE_AAC_LC },
+		{ AZ_F0_CODEC_PIN0_CONTROL_AUDIO_DESCRIPTOR6, HDMI_AUDIO_CODING_TYPE_DTS },
+		{ AZ_F0_CODEC_PIN0_CONTROL_AUDIO_DESCRIPTOR7, HDMI_AUDIO_CODING_TYPE_ATRAC },
+		{ AZ_F0_CODEC_PIN0_CONTROL_AUDIO_DESCRIPTOR9, HDMI_AUDIO_CODING_TYPE_EAC3 },
+		{ AZ_F0_CODEC_PIN0_CONTROL_AUDIO_DESCRIPTOR10, HDMI_AUDIO_CODING_TYPE_DTS_HD },
+		{ AZ_F0_CODEC_PIN0_CONTROL_AUDIO_DESCRIPTOR11, HDMI_AUDIO_CODING_TYPE_MLP },
+		{ AZ_F0_CODEC_PIN0_CONTROL_AUDIO_DESCRIPTOR13, HDMI_AUDIO_CODING_TYPE_WMA_PRO },
+	};
+
+	list_for_each_entry(connector, &encoder->dev->mode_config.connector_list, head) {
+		if (connector->encoder == encoder)
+			radeon_connector = to_radeon_connector(connector);
+	}
+
+	if (!radeon_connector) {
+		DRM_ERROR("Couldn't find encoder's connector\n");
+		return;
+	}
+
+	sad_count = drm_edid_to_sad(radeon_connector->edid, &sads);
+	if (sad_count < 0) {
+		DRM_ERROR("Couldn't read SADs: %d\n", sad_count);
+		return;
+	}
+	BUG_ON(!sads);
+
+	for (i = 0; i < ARRAY_SIZE(eld_reg_to_type); i++) {
+		u32 value = 0;
+		int j;
+
+		for (j = 0; j < sad_count; j++) {
+			struct cea_sad *sad = &sads[j];
+
+			if (sad->format == eld_reg_to_type[i][1]) {
+				value = MAX_CHANNELS(sad->channels) |
+					DESCRIPTOR_BYTE_2(sad->byte2) |
+					SUPPORTED_FREQUENCIES(sad->freq);
+				if (sad->format == HDMI_AUDIO_CODING_TYPE_PCM)
+					value |= SUPPORTED_FREQUENCIES_STEREO(sad->freq);
+				break;
+			}
+		}
+		WREG32(eld_reg_to_type[i][0], value);
+	}
+
+	kfree(sads);
+}
+
 /*
  * build a HDMI Video Info Frame
  */
@@ -85,6 +147,30 @@ static void evergreen_hdmi_update_avi_infoframe(struct drm_encoder *encoder,
 		frame[0xC] | (frame[0xD] << 8));
 }
 
+static void evergreen_audio_set_dto(struct drm_encoder *encoder, u32 clock)
+{
+	struct drm_device *dev = encoder->dev;
+	struct radeon_device *rdev = dev->dev_private;
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	struct radeon_crtc *radeon_crtc = to_radeon_crtc(encoder->crtc);
+	u32 base_rate = 48000;
+
+	if (!dig || !dig->afmt)
+		return;
+
+	/* XXX: properly calculate this */
+	/* XXX two dtos; generally use dto0 for hdmi */
+	/* Express [24MHz / target pixel clock] as an exact rational
+	 * number (coefficient of two integer numbers.  DCCG_AUDIO_DTOx_PHASE
+	 * is the numerator, DCCG_AUDIO_DTOx_MODULE is the denominator
+	 */
+	WREG32(DCCG_AUDIO_DTO0_PHASE, (base_rate*50) & 0xffffff);
+	WREG32(DCCG_AUDIO_DTO0_MODULE, (clock*100) & 0xffffff);
+	WREG32(DCCG_AUDIO_DTO_SOURCE, DCCG_AUDIO_DTO0_SOURCE_SEL(radeon_crtc->crtc_id));
+}
+
+
 /*
  * update the info frames with the data from the current display mode
  */
@@ -104,33 +190,19 @@ void evergreen_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode
 		return;
 	offset = dig->afmt->offset;
 
-	r600_audio_set_clock(encoder, mode->clock);
+	evergreen_audio_set_dto(encoder, mode->clock);
 
 	WREG32(HDMI_VBI_PACKET_CONTROL + offset,
 	       HDMI_NULL_SEND); /* send null packets when required */
 
 	WREG32(AFMT_AUDIO_CRC_CONTROL + offset, 0x1000);
 
-	WREG32(HDMI_AUDIO_PACKET_CONTROL + offset,
-	       HDMI_AUDIO_DELAY_EN(1) | /* set the default audio delay */
-	       HDMI_AUDIO_PACKETS_PER_LINE(3)); /* should be suffient for all audio modes and small enough for all hblanks */
-
-	WREG32(AFMT_AUDIO_PACKET_CONTROL + offset,
-	       AFMT_AUDIO_SAMPLE_SEND | /* send audio packets */
-	       AFMT_60958_CS_UPDATE); /* allow 60958 channel status fields to be updated */
-
-	WREG32(HDMI_ACR_PACKET_CONTROL + offset,
-	       HDMI_ACR_AUTO_SEND | /* allow hw to sent ACR packets when required */
-	       HDMI_ACR_SOURCE); /* select SW CTS value */
-
 	WREG32(HDMI_VBI_PACKET_CONTROL + offset,
 	       HDMI_NULL_SEND | /* send null packets when required */
 	       HDMI_GC_SEND | /* send general control packets */
 	       HDMI_GC_CONT); /* send general control packets every frame */
 
 	WREG32(HDMI_INFOFRAME_CONTROL0 + offset,
-	       HDMI_AVI_INFO_SEND | /* enable AVI info frames */
-	       HDMI_AVI_INFO_CONT | /* send AVI info frames every frame/field */
 	       HDMI_AUDIO_INFO_SEND | /* enable audio info frames (frames won't be set until audio is enabled) */
 	       HDMI_AUDIO_INFO_CONT); /* required for audio info values to be updated */
 
@@ -138,11 +210,47 @@ void evergreen_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode
 	       AFMT_AUDIO_INFO_UPDATE); /* required for audio info values to be updated */
 
 	WREG32(HDMI_INFOFRAME_CONTROL1 + offset,
-	       HDMI_AVI_INFO_LINE(2) | /* anything other than 0 */
 	       HDMI_AUDIO_INFO_LINE(2)); /* anything other than 0 */
 
 	WREG32(HDMI_GC + offset, 0); /* unset HDMI_GC_AVMUTE */
 
+	WREG32(HDMI_AUDIO_PACKET_CONTROL + offset,
+	       HDMI_AUDIO_DELAY_EN(1) | /* set the default audio delay */
+	       HDMI_AUDIO_PACKETS_PER_LINE(3)); /* should be suffient for all audio modes and small enough for all hblanks */
+
+	WREG32(AFMT_AUDIO_PACKET_CONTROL + offset,
+	       AFMT_60958_CS_UPDATE); /* allow 60958 channel status fields to be updated */
+
+	/* fglrx clears sth in AFMT_AUDIO_PACKET_CONTROL2 here */
+
+	WREG32(HDMI_ACR_PACKET_CONTROL + offset,
+	       HDMI_ACR_AUTO_SEND | /* allow hw to sent ACR packets when required */
+	       HDMI_ACR_SOURCE); /* select SW CTS value */
+
+	evergreen_hdmi_update_ACR(encoder, mode->clock);
+
+	WREG32(AFMT_60958_0 + offset,
+	       AFMT_60958_CS_CHANNEL_NUMBER_L(1));
+
+	WREG32(AFMT_60958_1 + offset,
+	       AFMT_60958_CS_CHANNEL_NUMBER_R(2));
+
+	WREG32(AFMT_60958_2 + offset,
+	       AFMT_60958_CS_CHANNEL_NUMBER_2(3) |
+	       AFMT_60958_CS_CHANNEL_NUMBER_3(4) |
+	       AFMT_60958_CS_CHANNEL_NUMBER_4(5) |
+	       AFMT_60958_CS_CHANNEL_NUMBER_5(6) |
+	       AFMT_60958_CS_CHANNEL_NUMBER_6(7) |
+	       AFMT_60958_CS_CHANNEL_NUMBER_7(8));
+
+	/* fglrx sets 0x0001005f | (x & 0x00fc0000) in 0x5f78 here */
+
+	WREG32(AFMT_AUDIO_PACKET_CONTROL2 + offset,
+	       AFMT_AUDIO_CHANNEL_ENABLE(0xff));
+
+	/* fglrx sets 0x40 in 0x5f80 here */
+	evergreen_hdmi_write_sad_regs(encoder);
+
 	err = drm_hdmi_avi_infoframe_from_display_mode(&frame, mode);
 	if (err < 0) {
 		DRM_ERROR("failed to setup AVI infoframe: %zd\n", err);
@@ -156,7 +264,17 @@ void evergreen_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode
 	}
 
 	evergreen_hdmi_update_avi_infoframe(encoder, buffer, sizeof(buffer));
-	evergreen_hdmi_update_ACR(encoder, mode->clock);
+
+	WREG32_OR(HDMI_INFOFRAME_CONTROL0 + offset,
+		  HDMI_AVI_INFO_SEND | /* enable AVI info frames */
+		  HDMI_AVI_INFO_CONT); /* required for audio info values to be updated */
+
+	WREG32_P(HDMI_INFOFRAME_CONTROL1 + offset,
+		 HDMI_AVI_INFO_LINE(2), /* anything other than 0 */
+		 ~HDMI_AVI_INFO_LINE_MASK);
+
+	WREG32_OR(AFMT_AUDIO_PACKET_CONTROL + offset,
+		  AFMT_AUDIO_SAMPLE_SEND); /* send audio packets */
 
 	/* it's unknown what these bits do excatly, but it's indeed quite useful for debugging */
 	WREG32(AFMT_RAMP_CONTROL0 + offset, 0x00FFFFFF);
@@ -164,3 +282,20 @@ void evergreen_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode
 	WREG32(AFMT_RAMP_CONTROL2 + offset, 0x00000001);
 	WREG32(AFMT_RAMP_CONTROL3 + offset, 0x00000001);
 }
+
+void evergreen_hdmi_enable(struct drm_encoder *encoder, bool enable)
+{
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+
+	/* Silent, r600_hdmi_enable will raise WARN for us */
+	if (enable && dig->afmt->enabled)
+		return;
+	if (!enable && !dig->afmt->enabled)
+		return;
+
+	dig->afmt->enabled = enable;
+
+	DRM_DEBUG("%sabling HDMI interface @ 0x%04X for encoder 0x%x\n",
+		  enable ? "En" : "Dis", dig->afmt->offset, radeon_encoder->encoder_id);
+}
diff --git a/drivers/gpu/drm/radeon/evergreen_reg.h b/drivers/gpu/drm/radeon/evergreen_reg.h
index f585be16e2d5..881aba23c477 100644
--- a/drivers/gpu/drm/radeon/evergreen_reg.h
+++ b/drivers/gpu/drm/radeon/evergreen_reg.h
@@ -226,6 +226,8 @@
 #define EVERGREEN_CRTC_STATUS_HV_COUNT                  0x6ea0
 #define EVERGREEN_MASTER_UPDATE_MODE                    0x6ef8
 #define EVERGREEN_CRTC_UPDATE_LOCK                      0x6ed4
+#define EVERGREEN_MASTER_UPDATE_LOCK                    0x6ef4
+#define EVERGREEN_MASTER_UPDATE_MODE                    0x6ef8
 
 #define EVERGREEN_DC_GPIO_HPD_MASK                      0x64b0
 #define EVERGREEN_DC_GPIO_HPD_A                         0x64b4
diff --git a/drivers/gpu/drm/radeon/evergreend.h b/drivers/gpu/drm/radeon/evergreend.h
index 982d25ad9af3..75c05631146d 100644
--- a/drivers/gpu/drm/radeon/evergreend.h
+++ b/drivers/gpu/drm/radeon/evergreend.h
@@ -53,6 +53,43 @@
 #define RCU_IND_INDEX           			0x100
 #define RCU_IND_DATA            			0x104
 
+/* discrete uvd clocks */
+#define CG_UPLL_FUNC_CNTL				0x718
+#	define UPLL_RESET_MASK				0x00000001
+#	define UPLL_SLEEP_MASK				0x00000002
+#	define UPLL_BYPASS_EN_MASK			0x00000004
+#	define UPLL_CTLREQ_MASK				0x00000008
+#	define UPLL_REF_DIV_MASK			0x003F0000
+#	define UPLL_VCO_MODE_MASK			0x00000200
+#	define UPLL_CTLACK_MASK				0x40000000
+#	define UPLL_CTLACK2_MASK			0x80000000
+#define CG_UPLL_FUNC_CNTL_2				0x71c
+#	define UPLL_PDIV_A(x)				((x) << 0)
+#	define UPLL_PDIV_A_MASK				0x0000007F
+#	define UPLL_PDIV_B(x)				((x) << 8)
+#	define UPLL_PDIV_B_MASK				0x00007F00
+#	define VCLK_SRC_SEL(x)				((x) << 20)
+#	define VCLK_SRC_SEL_MASK			0x01F00000
+#	define DCLK_SRC_SEL(x)				((x) << 25)
+#	define DCLK_SRC_SEL_MASK			0x3E000000
+#define CG_UPLL_FUNC_CNTL_3				0x720
+#	define UPLL_FB_DIV(x)				((x) << 0)
+#	define UPLL_FB_DIV_MASK				0x01FFFFFF
+#define CG_UPLL_FUNC_CNTL_4				0x854
+#	define UPLL_SPARE_ISPARE9			0x00020000
+#define CG_UPLL_SPREAD_SPECTRUM				0x79c
+#	define SSEN_MASK				0x00000001
+
+/* fusion uvd clocks */
+#define CG_DCLK_CNTL                                    0x610
+#       define DCLK_DIVIDER_MASK                        0x7f
+#       define DCLK_DIR_CNTL_EN                         (1 << 8)
+#define CG_DCLK_STATUS                                  0x614
+#       define DCLK_STATUS                              (1 << 0)
+#define CG_VCLK_CNTL                                    0x618
+#define CG_VCLK_STATUS                                  0x61c
+#define	CG_SCRATCH1					0x820
+
 #define GRBM_GFX_INDEX          			0x802C
 #define		INSTANCE_INDEX(x)			((x) << 0)
 #define		SE_INDEX(x)     			((x) << 16)
@@ -197,6 +234,7 @@
 #       define HDMI_MPEG_INFO_CONT           (1 << 9)
 #define HDMI_INFOFRAME_CONTROL1              0x7048
 #       define HDMI_AVI_INFO_LINE(x)         (((x) & 0x3f) << 0)
+#       define HDMI_AVI_INFO_LINE_MASK       (0x3f << 0)
 #       define HDMI_AUDIO_INFO_LINE(x)       (((x) & 0x3f) << 8)
 #       define HDMI_MPEG_INFO_LINE(x)        (((x) & 0x3f) << 16)
 #define HDMI_GENERIC_PACKET_CONTROL          0x704c
@@ -992,6 +1030,16 @@
 #       define TARGET_LINK_SPEED_MASK                     (0xf << 0)
 #       define SELECTABLE_DEEMPHASIS                      (1 << 6)
 
+
+/*
+ * UVD
+ */
+#define UVD_UDEC_ADDR_CONFIG				0xef4c
+#define UVD_UDEC_DB_ADDR_CONFIG				0xef50
+#define UVD_UDEC_DBW_ADDR_CONFIG			0xef54
+#define UVD_RBC_RB_RPTR					0xf690
+#define UVD_RBC_RB_WPTR					0xf694
+
 /*
  * PM4
  */
diff --git a/drivers/gpu/drm/radeon/ni.c b/drivers/gpu/drm/radeon/ni.c
index 27769e724b6d..7969c0c8ec20 100644
--- a/drivers/gpu/drm/radeon/ni.c
+++ b/drivers/gpu/drm/radeon/ni.c
@@ -78,6 +78,282 @@ MODULE_FIRMWARE("radeon/ARUBA_pfp.bin");
 MODULE_FIRMWARE("radeon/ARUBA_me.bin");
 MODULE_FIRMWARE("radeon/ARUBA_rlc.bin");
 
+
+static const u32 cayman_golden_registers2[] =
+{
+	0x3e5c, 0xffffffff, 0x00000000,
+	0x3e48, 0xffffffff, 0x00000000,
+	0x3e4c, 0xffffffff, 0x00000000,
+	0x3e64, 0xffffffff, 0x00000000,
+	0x3e50, 0xffffffff, 0x00000000,
+	0x3e60, 0xffffffff, 0x00000000
+};
+
+static const u32 cayman_golden_registers[] =
+{
+	0x5eb4, 0xffffffff, 0x00000002,
+	0x5e78, 0x8f311ff1, 0x001000f0,
+	0x3f90, 0xffff0000, 0xff000000,
+	0x9148, 0xffff0000, 0xff000000,
+	0x3f94, 0xffff0000, 0xff000000,
+	0x914c, 0xffff0000, 0xff000000,
+	0xc78, 0x00000080, 0x00000080,
+	0xbd4, 0x70073777, 0x00011003,
+	0xd02c, 0xbfffff1f, 0x08421000,
+	0xd0b8, 0x73773777, 0x02011003,
+	0x5bc0, 0x00200000, 0x50100000,
+	0x98f8, 0x33773777, 0x02011003,
+	0x98fc, 0xffffffff, 0x76541032,
+	0x7030, 0x31000311, 0x00000011,
+	0x2f48, 0x33773777, 0x42010001,
+	0x6b28, 0x00000010, 0x00000012,
+	0x7728, 0x00000010, 0x00000012,
+	0x10328, 0x00000010, 0x00000012,
+	0x10f28, 0x00000010, 0x00000012,
+	0x11b28, 0x00000010, 0x00000012,
+	0x12728, 0x00000010, 0x00000012,
+	0x240c, 0x000007ff, 0x00000000,
+	0x8a14, 0xf000001f, 0x00000007,
+	0x8b24, 0x3fff3fff, 0x00ff0fff,
+	0x8b10, 0x0000ff0f, 0x00000000,
+	0x28a4c, 0x07ffffff, 0x06000000,
+	0x10c, 0x00000001, 0x00010003,
+	0xa02c, 0xffffffff, 0x0000009b,
+	0x913c, 0x0000010f, 0x01000100,
+	0x8c04, 0xf8ff00ff, 0x40600060,
+	0x28350, 0x00000f01, 0x00000000,
+	0x9508, 0x3700001f, 0x00000002,
+	0x960c, 0xffffffff, 0x54763210,
+	0x88c4, 0x001f3ae3, 0x00000082,
+	0x88d0, 0xffffffff, 0x0f40df40,
+	0x88d4, 0x0000001f, 0x00000010,
+	0x8974, 0xffffffff, 0x00000000
+};
+
+static const u32 dvst_golden_registers2[] =
+{
+	0x8f8, 0xffffffff, 0,
+	0x8fc, 0x00380000, 0,
+	0x8f8, 0xffffffff, 1,
+	0x8fc, 0x0e000000, 0
+};
+
+static const u32 dvst_golden_registers[] =
+{
+	0x690, 0x3fff3fff, 0x20c00033,
+	0x918c, 0x0fff0fff, 0x00010006,
+	0x91a8, 0x0fff0fff, 0x00010006,
+	0x9150, 0xffffdfff, 0x6e944040,
+	0x917c, 0x0fff0fff, 0x00030002,
+	0x9198, 0x0fff0fff, 0x00030002,
+	0x915c, 0x0fff0fff, 0x00010000,
+	0x3f90, 0xffff0001, 0xff000000,
+	0x9178, 0x0fff0fff, 0x00070000,
+	0x9194, 0x0fff0fff, 0x00070000,
+	0x9148, 0xffff0001, 0xff000000,
+	0x9190, 0x0fff0fff, 0x00090008,
+	0x91ac, 0x0fff0fff, 0x00090008,
+	0x3f94, 0xffff0000, 0xff000000,
+	0x914c, 0xffff0000, 0xff000000,
+	0x929c, 0x00000fff, 0x00000001,
+	0x55e4, 0xff607fff, 0xfc000100,
+	0x8a18, 0xff000fff, 0x00000100,
+	0x8b28, 0xff000fff, 0x00000100,
+	0x9144, 0xfffc0fff, 0x00000100,
+	0x6ed8, 0x00010101, 0x00010000,
+	0x9830, 0xffffffff, 0x00000000,
+	0x9834, 0xf00fffff, 0x00000400,
+	0x9838, 0xfffffffe, 0x00000000,
+	0xd0c0, 0xff000fff, 0x00000100,
+	0xd02c, 0xbfffff1f, 0x08421000,
+	0xd0b8, 0x73773777, 0x12010001,
+	0x5bb0, 0x000000f0, 0x00000070,
+	0x98f8, 0x73773777, 0x12010001,
+	0x98fc, 0xffffffff, 0x00000010,
+	0x9b7c, 0x00ff0000, 0x00fc0000,
+	0x8030, 0x00001f0f, 0x0000100a,
+	0x2f48, 0x73773777, 0x12010001,
+	0x2408, 0x00030000, 0x000c007f,
+	0x8a14, 0xf000003f, 0x00000007,
+	0x8b24, 0x3fff3fff, 0x00ff0fff,
+	0x8b10, 0x0000ff0f, 0x00000000,
+	0x28a4c, 0x07ffffff, 0x06000000,
+	0x4d8, 0x00000fff, 0x00000100,
+	0xa008, 0xffffffff, 0x00010000,
+	0x913c, 0xffff03ff, 0x01000100,
+	0x8c00, 0x000000ff, 0x00000003,
+	0x8c04, 0xf8ff00ff, 0x40600060,
+	0x8cf0, 0x1fff1fff, 0x08e00410,
+	0x28350, 0x00000f01, 0x00000000,
+	0x9508, 0xf700071f, 0x00000002,
+	0x960c, 0xffffffff, 0x54763210,
+	0x20ef8, 0x01ff01ff, 0x00000002,
+	0x20e98, 0xfffffbff, 0x00200000,
+	0x2015c, 0xffffffff, 0x00000f40,
+	0x88c4, 0x001f3ae3, 0x00000082,
+	0x8978, 0x3fffffff, 0x04050140,
+	0x88d4, 0x0000001f, 0x00000010,
+	0x8974, 0xffffffff, 0x00000000
+};
+
+static const u32 scrapper_golden_registers[] =
+{
+	0x690, 0x3fff3fff, 0x20c00033,
+	0x918c, 0x0fff0fff, 0x00010006,
+	0x918c, 0x0fff0fff, 0x00010006,
+	0x91a8, 0x0fff0fff, 0x00010006,
+	0x91a8, 0x0fff0fff, 0x00010006,
+	0x9150, 0xffffdfff, 0x6e944040,
+	0x9150, 0xffffdfff, 0x6e944040,
+	0x917c, 0x0fff0fff, 0x00030002,
+	0x917c, 0x0fff0fff, 0x00030002,
+	0x9198, 0x0fff0fff, 0x00030002,
+	0x9198, 0x0fff0fff, 0x00030002,
+	0x915c, 0x0fff0fff, 0x00010000,
+	0x915c, 0x0fff0fff, 0x00010000,
+	0x3f90, 0xffff0001, 0xff000000,
+	0x3f90, 0xffff0001, 0xff000000,
+	0x9178, 0x0fff0fff, 0x00070000,
+	0x9178, 0x0fff0fff, 0x00070000,
+	0x9194, 0x0fff0fff, 0x00070000,
+	0x9194, 0x0fff0fff, 0x00070000,
+	0x9148, 0xffff0001, 0xff000000,
+	0x9148, 0xffff0001, 0xff000000,
+	0x9190, 0x0fff0fff, 0x00090008,
+	0x9190, 0x0fff0fff, 0x00090008,
+	0x91ac, 0x0fff0fff, 0x00090008,
+	0x91ac, 0x0fff0fff, 0x00090008,
+	0x3f94, 0xffff0000, 0xff000000,
+	0x3f94, 0xffff0000, 0xff000000,
+	0x914c, 0xffff0000, 0xff000000,
+	0x914c, 0xffff0000, 0xff000000,
+	0x929c, 0x00000fff, 0x00000001,
+	0x929c, 0x00000fff, 0x00000001,
+	0x55e4, 0xff607fff, 0xfc000100,
+	0x8a18, 0xff000fff, 0x00000100,
+	0x8a18, 0xff000fff, 0x00000100,
+	0x8b28, 0xff000fff, 0x00000100,
+	0x8b28, 0xff000fff, 0x00000100,
+	0x9144, 0xfffc0fff, 0x00000100,
+	0x9144, 0xfffc0fff, 0x00000100,
+	0x6ed8, 0x00010101, 0x00010000,
+	0x9830, 0xffffffff, 0x00000000,
+	0x9830, 0xffffffff, 0x00000000,
+	0x9834, 0xf00fffff, 0x00000400,
+	0x9834, 0xf00fffff, 0x00000400,
+	0x9838, 0xfffffffe, 0x00000000,
+	0x9838, 0xfffffffe, 0x00000000,
+	0xd0c0, 0xff000fff, 0x00000100,
+	0xd02c, 0xbfffff1f, 0x08421000,
+	0xd02c, 0xbfffff1f, 0x08421000,
+	0xd0b8, 0x73773777, 0x12010001,
+	0xd0b8, 0x73773777, 0x12010001,
+	0x5bb0, 0x000000f0, 0x00000070,
+	0x98f8, 0x73773777, 0x12010001,
+	0x98f8, 0x73773777, 0x12010001,
+	0x98fc, 0xffffffff, 0x00000010,
+	0x98fc, 0xffffffff, 0x00000010,
+	0x9b7c, 0x00ff0000, 0x00fc0000,
+	0x9b7c, 0x00ff0000, 0x00fc0000,
+	0x8030, 0x00001f0f, 0x0000100a,
+	0x8030, 0x00001f0f, 0x0000100a,
+	0x2f48, 0x73773777, 0x12010001,
+	0x2f48, 0x73773777, 0x12010001,
+	0x2408, 0x00030000, 0x000c007f,
+	0x8a14, 0xf000003f, 0x00000007,
+	0x8a14, 0xf000003f, 0x00000007,
+	0x8b24, 0x3fff3fff, 0x00ff0fff,
+	0x8b24, 0x3fff3fff, 0x00ff0fff,
+	0x8b10, 0x0000ff0f, 0x00000000,
+	0x8b10, 0x0000ff0f, 0x00000000,
+	0x28a4c, 0x07ffffff, 0x06000000,
+	0x28a4c, 0x07ffffff, 0x06000000,
+	0x4d8, 0x00000fff, 0x00000100,
+	0x4d8, 0x00000fff, 0x00000100,
+	0xa008, 0xffffffff, 0x00010000,
+	0xa008, 0xffffffff, 0x00010000,
+	0x913c, 0xffff03ff, 0x01000100,
+	0x913c, 0xffff03ff, 0x01000100,
+	0x90e8, 0x001fffff, 0x010400c0,
+	0x8c00, 0x000000ff, 0x00000003,
+	0x8c00, 0x000000ff, 0x00000003,
+	0x8c04, 0xf8ff00ff, 0x40600060,
+	0x8c04, 0xf8ff00ff, 0x40600060,
+	0x8c30, 0x0000000f, 0x00040005,
+	0x8cf0, 0x1fff1fff, 0x08e00410,
+	0x8cf0, 0x1fff1fff, 0x08e00410,
+	0x900c, 0x00ffffff, 0x0017071f,
+	0x28350, 0x00000f01, 0x00000000,
+	0x28350, 0x00000f01, 0x00000000,
+	0x9508, 0xf700071f, 0x00000002,
+	0x9508, 0xf700071f, 0x00000002,
+	0x9688, 0x00300000, 0x0017000f,
+	0x960c, 0xffffffff, 0x54763210,
+	0x960c, 0xffffffff, 0x54763210,
+	0x20ef8, 0x01ff01ff, 0x00000002,
+	0x20e98, 0xfffffbff, 0x00200000,
+	0x2015c, 0xffffffff, 0x00000f40,
+	0x88c4, 0x001f3ae3, 0x00000082,
+	0x88c4, 0x001f3ae3, 0x00000082,
+	0x8978, 0x3fffffff, 0x04050140,
+	0x8978, 0x3fffffff, 0x04050140,
+	0x88d4, 0x0000001f, 0x00000010,
+	0x88d4, 0x0000001f, 0x00000010,
+	0x8974, 0xffffffff, 0x00000000,
+	0x8974, 0xffffffff, 0x00000000
+};
+
+static void ni_init_golden_registers(struct radeon_device *rdev)
+{
+	switch (rdev->family) {
+	case CHIP_CAYMAN:
+		radeon_program_register_sequence(rdev,
+						 cayman_golden_registers,
+						 (const u32)ARRAY_SIZE(cayman_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 cayman_golden_registers2,
+						 (const u32)ARRAY_SIZE(cayman_golden_registers2));
+		break;
+	case CHIP_ARUBA:
+		if ((rdev->pdev->device == 0x9900) ||
+		    (rdev->pdev->device == 0x9901) ||
+		    (rdev->pdev->device == 0x9903) ||
+		    (rdev->pdev->device == 0x9904) ||
+		    (rdev->pdev->device == 0x9905) ||
+		    (rdev->pdev->device == 0x9906) ||
+		    (rdev->pdev->device == 0x9907) ||
+		    (rdev->pdev->device == 0x9908) ||
+		    (rdev->pdev->device == 0x9909) ||
+		    (rdev->pdev->device == 0x990A) ||
+		    (rdev->pdev->device == 0x990B) ||
+		    (rdev->pdev->device == 0x990C) ||
+		    (rdev->pdev->device == 0x990D) ||
+		    (rdev->pdev->device == 0x990E) ||
+		    (rdev->pdev->device == 0x990F) ||
+		    (rdev->pdev->device == 0x9910) ||
+		    (rdev->pdev->device == 0x9913) ||
+		    (rdev->pdev->device == 0x9917) ||
+		    (rdev->pdev->device == 0x9918)) {
+			radeon_program_register_sequence(rdev,
+							 dvst_golden_registers,
+							 (const u32)ARRAY_SIZE(dvst_golden_registers));
+			radeon_program_register_sequence(rdev,
+							 dvst_golden_registers2,
+							 (const u32)ARRAY_SIZE(dvst_golden_registers2));
+		} else {
+			radeon_program_register_sequence(rdev,
+							 scrapper_golden_registers,
+							 (const u32)ARRAY_SIZE(scrapper_golden_registers));
+			radeon_program_register_sequence(rdev,
+							 dvst_golden_registers2,
+							 (const u32)ARRAY_SIZE(dvst_golden_registers2));
+		}
+		break;
+	default:
+		break;
+	}
+}
+
 #define BTC_IO_MC_REGS_SIZE 29
 
 static const u32 barts_io_mc_regs[BTC_IO_MC_REGS_SIZE][2] = {
@@ -473,7 +749,8 @@ static void cayman_gpu_init(struct radeon_device *rdev)
 		    (rdev->pdev->device == 0x990F) ||
 		    (rdev->pdev->device == 0x9910) ||
 		    (rdev->pdev->device == 0x9917) ||
-		    (rdev->pdev->device == 0x9999)) {
+		    (rdev->pdev->device == 0x9999) ||
+		    (rdev->pdev->device == 0x999C)) {
 			rdev->config.cayman.max_simds_per_se = 6;
 			rdev->config.cayman.max_backends_per_se = 2;
 		} else if ((rdev->pdev->device == 0x9903) ||
@@ -482,7 +759,8 @@ static void cayman_gpu_init(struct radeon_device *rdev)
 			   (rdev->pdev->device == 0x990D) ||
 			   (rdev->pdev->device == 0x990E) ||
 			   (rdev->pdev->device == 0x9913) ||
-			   (rdev->pdev->device == 0x9918)) {
+			   (rdev->pdev->device == 0x9918) ||
+			   (rdev->pdev->device == 0x999D)) {
 			rdev->config.cayman.max_simds_per_se = 4;
 			rdev->config.cayman.max_backends_per_se = 2;
 		} else if ((rdev->pdev->device == 0x9919) ||
@@ -615,15 +893,28 @@ static void cayman_gpu_init(struct radeon_device *rdev)
 	}
 	/* enabled rb are just the one not disabled :) */
 	disabled_rb_mask = tmp;
+	tmp = 0;
+	for (i = 0; i < (rdev->config.cayman.max_backends_per_se * rdev->config.cayman.max_shader_engines); i++)
+		tmp |= (1 << i);
+	/* if all the backends are disabled, fix it up here */
+	if ((disabled_rb_mask & tmp) == tmp) {
+		for (i = 0; i < (rdev->config.cayman.max_backends_per_se * rdev->config.cayman.max_shader_engines); i++)
+			disabled_rb_mask &= ~(1 << i);
+	}
 
 	WREG32(GRBM_GFX_INDEX, INSTANCE_BROADCAST_WRITES | SE_BROADCAST_WRITES);
 	WREG32(RLC_GFX_INDEX, INSTANCE_BROADCAST_WRITES | SE_BROADCAST_WRITES);
 
 	WREG32(GB_ADDR_CONFIG, gb_addr_config);
 	WREG32(DMIF_ADDR_CONFIG, gb_addr_config);
+	if (ASIC_IS_DCE6(rdev))
+		WREG32(DMIF_ADDR_CALC, gb_addr_config);
 	WREG32(HDP_ADDR_CONFIG, gb_addr_config);
 	WREG32(DMA_TILING_CONFIG + DMA0_REGISTER_OFFSET, gb_addr_config);
 	WREG32(DMA_TILING_CONFIG + DMA1_REGISTER_OFFSET, gb_addr_config);
+	WREG32(UVD_UDEC_ADDR_CONFIG, gb_addr_config);
+	WREG32(UVD_UDEC_DB_ADDR_CONFIG, gb_addr_config);
+	WREG32(UVD_UDEC_DBW_ADDR_CONFIG, gb_addr_config);
 
 	if ((rdev->config.cayman.max_backends_per_se == 1) &&
 	    (rdev->flags & RADEON_IS_IGP)) {
@@ -931,6 +1222,23 @@ void cayman_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 	radeon_ring_write(ring, 10); /* poll interval */
 }
 
+void cayman_uvd_semaphore_emit(struct radeon_device *rdev,
+			       struct radeon_ring *ring,
+			       struct radeon_semaphore *semaphore,
+			       bool emit_wait)
+{
+	uint64_t addr = semaphore->gpu_addr;
+
+	radeon_ring_write(ring, PACKET0(UVD_SEMA_ADDR_LOW, 0));
+	radeon_ring_write(ring, (addr >> 3) & 0x000FFFFF);
+
+	radeon_ring_write(ring, PACKET0(UVD_SEMA_ADDR_HIGH, 0));
+	radeon_ring_write(ring, (addr >> 23) & 0x000FFFFF);
+
+	radeon_ring_write(ring, PACKET0(UVD_SEMA_CMD, 0));
+	radeon_ring_write(ring, 0x80 | (emit_wait ? 1 : 0));
+}
+
 static void cayman_cp_enable(struct radeon_device *rdev, bool enable)
 {
 	if (enable)
@@ -1682,6 +1990,16 @@ static int cayman_startup(struct radeon_device *rdev)
 		return r;
 	}
 
+	r = rv770_uvd_resume(rdev);
+	if (!r) {
+		r = radeon_fence_driver_start_ring(rdev,
+						   R600_RING_TYPE_UVD_INDEX);
+		if (r)
+			dev_err(rdev->dev, "UVD fences init error (%d).\n", r);
+	}
+	if (r)
+		rdev->ring[R600_RING_TYPE_UVD_INDEX].ring_size = 0;
+
 	r = radeon_fence_driver_start_ring(rdev, CAYMAN_RING_TYPE_CP1_INDEX);
 	if (r) {
 		dev_err(rdev->dev, "failed initializing CP fences (%d).\n", r);
@@ -1748,6 +2066,18 @@ static int cayman_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	ring = &rdev->ring[R600_RING_TYPE_UVD_INDEX];
+	if (ring->ring_size) {
+		r = radeon_ring_init(rdev, ring, ring->ring_size,
+				     R600_WB_UVD_RPTR_OFFSET,
+				     UVD_RBC_RB_RPTR, UVD_RBC_RB_WPTR,
+				     0, 0xfffff, RADEON_CP_PACKET2);
+		if (!r)
+			r = r600_uvd_init(rdev);
+		if (r)
+			DRM_ERROR("radeon: failed initializing UVD (%d).\n", r);
+	}
+
 	r = radeon_ib_pool_init(rdev);
 	if (r) {
 		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
@@ -1778,6 +2108,9 @@ int cayman_resume(struct radeon_device *rdev)
 	/* post card */
 	atom_asic_init(rdev->mode_info.atom_context);
 
+	/* init golden registers */
+	ni_init_golden_registers(rdev);
+
 	rdev->accel_working = true;
 	r = cayman_startup(rdev);
 	if (r) {
@@ -1794,6 +2127,8 @@ int cayman_suspend(struct radeon_device *rdev)
 	radeon_vm_manager_fini(rdev);
 	cayman_cp_enable(rdev, false);
 	cayman_dma_stop(rdev);
+	r600_uvd_rbc_stop(rdev);
+	radeon_uvd_suspend(rdev);
 	evergreen_irq_suspend(rdev);
 	radeon_wb_disable(rdev);
 	cayman_pcie_gart_disable(rdev);
@@ -1834,6 +2169,8 @@ int cayman_init(struct radeon_device *rdev)
 		DRM_INFO("GPU not posted. posting now...\n");
 		atom_asic_init(rdev->mode_info.atom_context);
 	}
+	/* init golden registers */
+	ni_init_golden_registers(rdev);
 	/* Initialize scratch registers */
 	r600_scratch_init(rdev);
 	/* Initialize surface registers */
@@ -1868,6 +2205,13 @@ int cayman_init(struct radeon_device *rdev)
 	ring->ring_obj = NULL;
 	r600_ring_init(rdev, ring, 64 * 1024);
 
+	r = radeon_uvd_init(rdev);
+	if (!r) {
+		ring = &rdev->ring[R600_RING_TYPE_UVD_INDEX];
+		ring->ring_obj = NULL;
+		r600_ring_init(rdev, ring, 4096);
+	}
+
 	rdev->ih.ring_obj = NULL;
 	r600_ih_ring_init(rdev, 64 * 1024);
 
@@ -1919,6 +2263,7 @@ void cayman_fini(struct radeon_device *rdev)
 	radeon_vm_manager_fini(rdev);
 	radeon_ib_pool_fini(rdev);
 	radeon_irq_kms_fini(rdev);
+	radeon_uvd_fini(rdev);
 	cayman_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
@@ -2017,28 +2362,57 @@ void cayman_vm_set_page(struct radeon_device *rdev,
 			}
 		}
 	} else {
-		while (count) {
-			ndw = count * 2;
-			if (ndw > 0xFFFFE)
-				ndw = 0xFFFFE;
+		if ((flags & RADEON_VM_PAGE_SYSTEM) ||
+		    (count == 1)) {
+			while (count) {
+				ndw = count * 2;
+				if (ndw > 0xFFFFE)
+					ndw = 0xFFFFE;
+
+				/* for non-physically contiguous pages (system) */
+				ib->ptr[ib->length_dw++] = DMA_PACKET(DMA_PACKET_WRITE, 0, 0, ndw);
+				ib->ptr[ib->length_dw++] = pe;
+				ib->ptr[ib->length_dw++] = upper_32_bits(pe) & 0xff;
+				for (; ndw > 0; ndw -= 2, --count, pe += 8) {
+					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;
+					ib->ptr[ib->length_dw++] = value;
+					ib->ptr[ib->length_dw++] = upper_32_bits(value);
+				}
+			}
+			while (ib->length_dw & 0x7)
+				ib->ptr[ib->length_dw++] = DMA_PACKET(DMA_PACKET_NOP, 0, 0, 0);
+		} else {
+			while (count) {
+				ndw = count * 2;
+				if (ndw > 0xFFFFE)
+					ndw = 0xFFFFE;
 
-			/* for non-physically contiguous pages (system) */
-			ib->ptr[ib->length_dw++] = DMA_PACKET(DMA_PACKET_WRITE, 0, 0, ndw);
-			ib->ptr[ib->length_dw++] = pe;
-			ib->ptr[ib->length_dw++] = upper_32_bits(pe) & 0xff;
-			for (; ndw > 0; ndw -= 2, --count, pe += 8) {
-				if (flags & RADEON_VM_PAGE_SYSTEM) {
-					value = radeon_vm_map_gart(rdev, addr);
-					value &= 0xFFFFFFFFFFFFF000ULL;
-				} else if (flags & RADEON_VM_PAGE_VALID) {
+				if (flags & RADEON_VM_PAGE_VALID)
 					value = addr;
-				} else {
+				else
 					value = 0;
-				}
-				addr += incr;
-				value |= r600_flags;
-				ib->ptr[ib->length_dw++] = value;
+				/* for physically contiguous pages (vram) */
+				ib->ptr[ib->length_dw++] = DMA_PTE_PDE_PACKET(ndw);
+				ib->ptr[ib->length_dw++] = pe; /* dst addr */
+				ib->ptr[ib->length_dw++] = upper_32_bits(pe) & 0xff;
+				ib->ptr[ib->length_dw++] = r600_flags; /* mask */
+				ib->ptr[ib->length_dw++] = 0;
+				ib->ptr[ib->length_dw++] = value; /* value */
 				ib->ptr[ib->length_dw++] = upper_32_bits(value);
+				ib->ptr[ib->length_dw++] = incr; /* increment size */
+				ib->ptr[ib->length_dw++] = 0;
+				pe += ndw * 4;
+				addr += (ndw / 2) * incr;
+				count -= ndw / 2;
 			}
 		}
 		while (ib->length_dw & 0x7)
diff --git a/drivers/gpu/drm/radeon/nid.h b/drivers/gpu/drm/radeon/nid.h
index 079dee202a9e..e226faf16fea 100644
--- a/drivers/gpu/drm/radeon/nid.h
+++ b/drivers/gpu/drm/radeon/nid.h
@@ -45,6 +45,10 @@
 #define ARUBA_GB_ADDR_CONFIG_GOLDEN        0x12010001
 
 #define DMIF_ADDR_CONFIG  				0xBD4
+
+/* DCE6 only */
+#define DMIF_ADDR_CALC  				0xC00
+
 #define	SRBM_GFX_CNTL				        0x0E44
 #define		RINGID(x)					(((x) & 0x3) << 0)
 #define		VMID(x)						(((x) & 0x7) << 0)
@@ -486,6 +490,18 @@
 #       define CACHE_FLUSH_AND_INV_EVENT                        (0x16 << 0)
 
 /*
+ * UVD
+ */
+#define UVD_SEMA_ADDR_LOW				0xEF00
+#define UVD_SEMA_ADDR_HIGH				0xEF04
+#define UVD_SEMA_CMD					0xEF08
+#define UVD_UDEC_ADDR_CONFIG				0xEF4C
+#define UVD_UDEC_DB_ADDR_CONFIG				0xEF50
+#define UVD_UDEC_DBW_ADDR_CONFIG			0xEF54
+#define UVD_RBC_RB_RPTR					0xF690
+#define UVD_RBC_RB_WPTR					0xF694
+
+/*
  * PM4
  */
 #define PACKET0(reg, n)	((RADEON_PACKET_TYPE0 << 30) |			\
@@ -668,6 +684,11 @@
 					 (((vmid) & 0xF) << 20) |	\
 					 (((n) & 0xFFFFF) << 0))
 
+#define DMA_PTE_PDE_PACKET(n)		((2 << 28) |			\
+					 (1 << 26) |			\
+					 (1 << 21) |			\
+					 (((n) & 0xFFFFF) << 0))
+
 /* async DMA Packet types */
 #define	DMA_PACKET_WRITE				  0x2
 #define	DMA_PACKET_COPY					  0x3
diff --git a/drivers/gpu/drm/radeon/r100.c b/drivers/gpu/drm/radeon/r100.c
index 9db58530be37..4973bff37fec 100644
--- a/drivers/gpu/drm/radeon/r100.c
+++ b/drivers/gpu/drm/radeon/r100.c
@@ -69,6 +69,38 @@ MODULE_FIRMWARE(FIRMWARE_R520);
  * and others in some cases.
  */
 
+static bool r100_is_in_vblank(struct radeon_device *rdev, int crtc)
+{
+	if (crtc == 0) {
+		if (RREG32(RADEON_CRTC_STATUS) & RADEON_CRTC_VBLANK_CUR)
+			return true;
+		else
+			return false;
+	} else {
+		if (RREG32(RADEON_CRTC2_STATUS) & RADEON_CRTC2_VBLANK_CUR)
+			return true;
+		else
+			return false;
+	}
+}
+
+static bool r100_is_counter_moving(struct radeon_device *rdev, int crtc)
+{
+	u32 vline1, vline2;
+
+	if (crtc == 0) {
+		vline1 = (RREG32(RADEON_CRTC_VLINE_CRNT_VLINE) >> 16) & RADEON_CRTC_V_TOTAL;
+		vline2 = (RREG32(RADEON_CRTC_VLINE_CRNT_VLINE) >> 16) & RADEON_CRTC_V_TOTAL;
+	} else {
+		vline1 = (RREG32(RADEON_CRTC2_VLINE_CRNT_VLINE) >> 16) & RADEON_CRTC_V_TOTAL;
+		vline2 = (RREG32(RADEON_CRTC2_VLINE_CRNT_VLINE) >> 16) & RADEON_CRTC_V_TOTAL;
+	}
+	if (vline1 != vline2)
+		return true;
+	else
+		return false;
+}
+
 /**
  * r100_wait_for_vblank - vblank wait asic callback.
  *
@@ -79,36 +111,33 @@ MODULE_FIRMWARE(FIRMWARE_R520);
  */
 void r100_wait_for_vblank(struct radeon_device *rdev, int crtc)
 {
-	int i;
+	unsigned i = 0;
 
 	if (crtc >= rdev->num_crtc)
 		return;
 
 	if (crtc == 0) {
-		if (RREG32(RADEON_CRTC_GEN_CNTL) & RADEON_CRTC_EN) {
-			for (i = 0; i < rdev->usec_timeout; i++) {
-				if (!(RREG32(RADEON_CRTC_STATUS) & RADEON_CRTC_VBLANK_CUR))
-					break;
-				udelay(1);
-			}
-			for (i = 0; i < rdev->usec_timeout; i++) {
-				if (RREG32(RADEON_CRTC_STATUS) & RADEON_CRTC_VBLANK_CUR)
-					break;
-				udelay(1);
-			}
-		}
+		if (!(RREG32(RADEON_CRTC_GEN_CNTL) & RADEON_CRTC_EN))
+			return;
 	} else {
-		if (RREG32(RADEON_CRTC2_GEN_CNTL) & RADEON_CRTC2_EN) {
-			for (i = 0; i < rdev->usec_timeout; i++) {
-				if (!(RREG32(RADEON_CRTC2_STATUS) & RADEON_CRTC2_VBLANK_CUR))
-					break;
-				udelay(1);
-			}
-			for (i = 0; i < rdev->usec_timeout; i++) {
-				if (RREG32(RADEON_CRTC2_STATUS) & RADEON_CRTC2_VBLANK_CUR)
-					break;
-				udelay(1);
-			}
+		if (!(RREG32(RADEON_CRTC2_GEN_CNTL) & RADEON_CRTC2_EN))
+			return;
+	}
+
+	/* depending on when we hit vblank, we may be close to active; if so,
+	 * wait for another frame.
+	 */
+	while (r100_is_in_vblank(rdev, crtc)) {
+		if (i++ % 100 == 0) {
+			if (!r100_is_counter_moving(rdev, crtc))
+				break;
+		}
+	}
+
+	while (!r100_is_in_vblank(rdev, crtc)) {
+		if (i++ % 100 == 0) {
+			if (!r100_is_counter_moving(rdev, crtc))
+				break;
 		}
 	}
 }
diff --git a/drivers/gpu/drm/radeon/r500_reg.h b/drivers/gpu/drm/radeon/r500_reg.h
index c0dc8d3ba0bb..1dd0d32993d5 100644
--- a/drivers/gpu/drm/radeon/r500_reg.h
+++ b/drivers/gpu/drm/radeon/r500_reg.h
@@ -358,7 +358,9 @@
 #define AVIVO_D1CRTC_STATUS_HV_COUNT                            0x60ac
 #define AVIVO_D1CRTC_STEREO_CONTROL                             0x60c4
 
+#define AVIVO_D1MODE_MASTER_UPDATE_LOCK                         0x60e0
 #define AVIVO_D1MODE_MASTER_UPDATE_MODE                         0x60e4
+#define AVIVO_D1CRTC_UPDATE_LOCK                                0x60e8
 
 /* master controls */
 #define AVIVO_DC_CRTC_MASTER_EN                                 0x60f8
diff --git a/drivers/gpu/drm/radeon/r600.c b/drivers/gpu/drm/radeon/r600.c
index 0740db3fcd22..1a08008c978b 100644
--- a/drivers/gpu/drm/radeon/r600.c
+++ b/drivers/gpu/drm/radeon/r600.c
@@ -1145,7 +1145,7 @@ static void r600_vram_gtt_location(struct radeon_device *rdev, struct radeon_mc
 	}
 	if (rdev->flags & RADEON_IS_AGP) {
 		size_bf = mc->gtt_start;
-		size_af = 0xFFFFFFFF - mc->gtt_end;
+		size_af = mc->mc_mask - mc->gtt_end;
 		if (size_bf > size_af) {
 			if (mc->mc_vram_size > size_bf) {
 				dev_warn(rdev->dev, "limiting VRAM\n");
@@ -2552,6 +2552,193 @@ void r600_dma_fini(struct radeon_device *rdev)
 }
 
 /*
+ * UVD
+ */
+int r600_uvd_rbc_start(struct radeon_device *rdev)
+{
+	struct radeon_ring *ring = &rdev->ring[R600_RING_TYPE_UVD_INDEX];
+	uint64_t rptr_addr;
+	uint32_t rb_bufsz, tmp;
+	int r;
+
+	rptr_addr = rdev->wb.gpu_addr + R600_WB_UVD_RPTR_OFFSET;
+
+	if (upper_32_bits(rptr_addr) != upper_32_bits(ring->gpu_addr)) {
+		DRM_ERROR("UVD ring and rptr not in the same 4GB segment!\n");
+		return -EINVAL;
+	}
+
+	/* force RBC into idle state */
+	WREG32(UVD_RBC_RB_CNTL, 0x11010101);
+
+	/* Set the write pointer delay */
+	WREG32(UVD_RBC_RB_WPTR_CNTL, 0);
+
+	/* set the wb address */
+	WREG32(UVD_RBC_RB_RPTR_ADDR, rptr_addr >> 2);
+
+	/* programm the 4GB memory segment for rptr and ring buffer */
+	WREG32(UVD_LMI_EXT40_ADDR, upper_32_bits(rptr_addr) |
+				   (0x7 << 16) | (0x1 << 31));
+
+	/* Initialize the ring buffer's read and write pointers */
+	WREG32(UVD_RBC_RB_RPTR, 0x0);
+
+	ring->wptr = ring->rptr = RREG32(UVD_RBC_RB_RPTR);
+	WREG32(UVD_RBC_RB_WPTR, ring->wptr);
+
+	/* set the ring address */
+	WREG32(UVD_RBC_RB_BASE, ring->gpu_addr);
+
+	/* Set ring buffer size */
+	rb_bufsz = drm_order(ring->ring_size);
+	rb_bufsz = (0x1 << 8) | rb_bufsz;
+	WREG32(UVD_RBC_RB_CNTL, rb_bufsz);
+
+	ring->ready = true;
+	r = radeon_ring_test(rdev, R600_RING_TYPE_UVD_INDEX, ring);
+	if (r) {
+		ring->ready = false;
+		return r;
+	}
+
+	r = radeon_ring_lock(rdev, ring, 10);
+	if (r) {
+		DRM_ERROR("radeon: ring failed to lock UVD ring (%d).\n", r);
+		return r;
+	}
+
+	tmp = PACKET0(UVD_SEMA_WAIT_FAULT_TIMEOUT_CNTL, 0);
+	radeon_ring_write(ring, tmp);
+	radeon_ring_write(ring, 0xFFFFF);
+
+	tmp = PACKET0(UVD_SEMA_WAIT_INCOMPLETE_TIMEOUT_CNTL, 0);
+	radeon_ring_write(ring, tmp);
+	radeon_ring_write(ring, 0xFFFFF);
+
+	tmp = PACKET0(UVD_SEMA_SIGNAL_INCOMPLETE_TIMEOUT_CNTL, 0);
+	radeon_ring_write(ring, tmp);
+	radeon_ring_write(ring, 0xFFFFF);
+
+	/* Clear timeout status bits */
+	radeon_ring_write(ring, PACKET0(UVD_SEMA_TIMEOUT_STATUS, 0));
+	radeon_ring_write(ring, 0x8);
+
+	radeon_ring_write(ring, PACKET0(UVD_SEMA_CNTL, 0));
+	radeon_ring_write(ring, 3);
+
+	radeon_ring_unlock_commit(rdev, ring);
+
+	return 0;
+}
+
+void r600_uvd_rbc_stop(struct radeon_device *rdev)
+{
+	struct radeon_ring *ring = &rdev->ring[R600_RING_TYPE_UVD_INDEX];
+
+	/* force RBC into idle state */
+	WREG32(UVD_RBC_RB_CNTL, 0x11010101);
+	ring->ready = false;
+}
+
+int r600_uvd_init(struct radeon_device *rdev)
+{
+	int i, j, r;
+
+	/* raise clocks while booting up the VCPU */
+	radeon_set_uvd_clocks(rdev, 53300, 40000);
+
+	/* disable clock gating */
+	WREG32(UVD_CGC_GATE, 0);
+
+	/* disable interupt */
+	WREG32_P(UVD_MASTINT_EN, 0, ~(1 << 1));
+
+	/* put LMI, VCPU, RBC etc... into reset */
+	WREG32(UVD_SOFT_RESET, LMI_SOFT_RESET | VCPU_SOFT_RESET |
+	       LBSI_SOFT_RESET | RBC_SOFT_RESET | CSM_SOFT_RESET |
+	       CXW_SOFT_RESET | TAP_SOFT_RESET | LMI_UMC_SOFT_RESET);
+	mdelay(5);
+
+	/* take UVD block out of reset */
+	WREG32_P(SRBM_SOFT_RESET, 0, ~SOFT_RESET_UVD);
+	mdelay(5);
+
+	/* initialize UVD memory controller */
+	WREG32(UVD_LMI_CTRL, 0x40 | (1 << 8) | (1 << 13) |
+			     (1 << 21) | (1 << 9) | (1 << 20));
+
+	/* disable byte swapping */
+	WREG32(UVD_LMI_SWAP_CNTL, 0);
+	WREG32(UVD_MP_SWAP_CNTL, 0);
+
+	WREG32(UVD_MPC_SET_MUXA0, 0x40c2040);
+	WREG32(UVD_MPC_SET_MUXA1, 0x0);
+	WREG32(UVD_MPC_SET_MUXB0, 0x40c2040);
+	WREG32(UVD_MPC_SET_MUXB1, 0x0);
+	WREG32(UVD_MPC_SET_ALU, 0);
+	WREG32(UVD_MPC_SET_MUX, 0x88);
+
+	/* Stall UMC */
+	WREG32_P(UVD_LMI_CTRL2, 1 << 8, ~(1 << 8));
+	WREG32_P(UVD_RB_ARB_CTRL, 1 << 3, ~(1 << 3));
+
+	/* take all subblocks out of reset, except VCPU */
+	WREG32(UVD_SOFT_RESET, VCPU_SOFT_RESET);
+	mdelay(5);
+
+	/* enable VCPU clock */
+	WREG32(UVD_VCPU_CNTL,  1 << 9);
+
+	/* enable UMC */
+	WREG32_P(UVD_LMI_CTRL2, 0, ~(1 << 8));
+
+	/* boot up the VCPU */
+	WREG32(UVD_SOFT_RESET, 0);
+	mdelay(10);
+
+	WREG32_P(UVD_RB_ARB_CTRL, 0, ~(1 << 3));
+
+	for (i = 0; i < 10; ++i) {
+		uint32_t status;
+		for (j = 0; j < 100; ++j) {
+			status = RREG32(UVD_STATUS);
+			if (status & 2)
+				break;
+			mdelay(10);
+		}
+		r = 0;
+		if (status & 2)
+			break;
+
+		DRM_ERROR("UVD not responding, trying to reset the VCPU!!!\n");
+		WREG32_P(UVD_SOFT_RESET, VCPU_SOFT_RESET, ~VCPU_SOFT_RESET);
+		mdelay(10);
+		WREG32_P(UVD_SOFT_RESET, 0, ~VCPU_SOFT_RESET);
+		mdelay(10);
+		r = -1;
+	}
+
+	if (r) {
+		DRM_ERROR("UVD not responding, giving up!!!\n");
+		radeon_set_uvd_clocks(rdev, 0, 0);
+		return r;
+	}
+
+	/* enable interupt */
+	WREG32_P(UVD_MASTINT_EN, 3<<1, ~(3 << 1));
+
+	r = r600_uvd_rbc_start(rdev);
+	if (!r)
+		DRM_INFO("UVD initialized successfully.\n");
+
+	/* lower clocks again */
+	radeon_set_uvd_clocks(rdev, 0, 0);
+
+	return r;
+}
+
+/*
  * GPU scratch registers helpers function.
  */
 void r600_scratch_init(struct radeon_device *rdev)
@@ -2660,6 +2847,40 @@ int r600_dma_ring_test(struct radeon_device *rdev,
 	return r;
 }
 
+int r600_uvd_ring_test(struct radeon_device *rdev, struct radeon_ring *ring)
+{
+	uint32_t tmp = 0;
+	unsigned i;
+	int r;
+
+	WREG32(UVD_CONTEXT_ID, 0xCAFEDEAD);
+	r = radeon_ring_lock(rdev, ring, 3);
+	if (r) {
+		DRM_ERROR("radeon: cp failed to lock ring %d (%d).\n",
+			  ring->idx, r);
+		return r;
+	}
+	radeon_ring_write(ring, PACKET0(UVD_CONTEXT_ID, 0));
+	radeon_ring_write(ring, 0xDEADBEEF);
+	radeon_ring_unlock_commit(rdev, ring);
+	for (i = 0; i < rdev->usec_timeout; i++) {
+		tmp = RREG32(UVD_CONTEXT_ID);
+		if (tmp == 0xDEADBEEF)
+			break;
+		DRM_UDELAY(1);
+	}
+
+	if (i < rdev->usec_timeout) {
+		DRM_INFO("ring test on %d succeeded in %d usecs\n",
+			 ring->idx, i);
+	} else {
+		DRM_ERROR("radeon: ring %d test failed (0x%08X)\n",
+			  ring->idx, tmp);
+		r = -EINVAL;
+	}
+	return r;
+}
+
 /*
  * CP fences/semaphores
  */
@@ -2711,6 +2932,30 @@ void r600_fence_ring_emit(struct radeon_device *rdev,
 	}
 }
 
+void r600_uvd_fence_emit(struct radeon_device *rdev,
+			 struct radeon_fence *fence)
+{
+	struct radeon_ring *ring = &rdev->ring[fence->ring];
+	uint32_t addr = rdev->fence_drv[fence->ring].gpu_addr;
+
+	radeon_ring_write(ring, PACKET0(UVD_CONTEXT_ID, 0));
+	radeon_ring_write(ring, fence->seq);
+	radeon_ring_write(ring, PACKET0(UVD_GPCOM_VCPU_DATA0, 0));
+	radeon_ring_write(ring, addr & 0xffffffff);
+	radeon_ring_write(ring, PACKET0(UVD_GPCOM_VCPU_DATA1, 0));
+	radeon_ring_write(ring, upper_32_bits(addr) & 0xff);
+	radeon_ring_write(ring, PACKET0(UVD_GPCOM_VCPU_CMD, 0));
+	radeon_ring_write(ring, 0);
+
+	radeon_ring_write(ring, PACKET0(UVD_GPCOM_VCPU_DATA0, 0));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, PACKET0(UVD_GPCOM_VCPU_DATA1, 0));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, PACKET0(UVD_GPCOM_VCPU_CMD, 0));
+	radeon_ring_write(ring, 2);
+	return;
+}
+
 void r600_semaphore_ring_emit(struct radeon_device *rdev,
 			      struct radeon_ring *ring,
 			      struct radeon_semaphore *semaphore,
@@ -2780,6 +3025,23 @@ void r600_dma_semaphore_ring_emit(struct radeon_device *rdev,
 	radeon_ring_write(ring, upper_32_bits(addr) & 0xff);
 }
 
+void r600_uvd_semaphore_emit(struct radeon_device *rdev,
+			     struct radeon_ring *ring,
+			     struct radeon_semaphore *semaphore,
+			     bool emit_wait)
+{
+	uint64_t addr = semaphore->gpu_addr;
+
+	radeon_ring_write(ring, PACKET0(UVD_SEMA_ADDR_LOW, 0));
+	radeon_ring_write(ring, (addr >> 3) & 0x000FFFFF);
+
+	radeon_ring_write(ring, PACKET0(UVD_SEMA_ADDR_HIGH, 0));
+	radeon_ring_write(ring, (addr >> 23) & 0x000FFFFF);
+
+	radeon_ring_write(ring, PACKET0(UVD_SEMA_CMD, 0));
+	radeon_ring_write(ring, emit_wait ? 1 : 0);
+}
+
 int r600_copy_blit(struct radeon_device *rdev,
 		   uint64_t src_offset,
 		   uint64_t dst_offset,
@@ -3183,6 +3445,16 @@ void r600_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 	radeon_ring_write(ring, ib->length_dw);
 }
 
+void r600_uvd_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
+{
+	struct radeon_ring *ring = &rdev->ring[ib->ring];
+
+	radeon_ring_write(ring, PACKET0(UVD_RBC_IB_BASE, 0));
+	radeon_ring_write(ring, ib->gpu_addr);
+	radeon_ring_write(ring, PACKET0(UVD_RBC_IB_SIZE, 0));
+	radeon_ring_write(ring, ib->length_dw);
+}
+
 int r600_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	struct radeon_ib ib;
@@ -3300,6 +3572,41 @@ int r600_dma_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	return r;
 }
 
+int r600_uvd_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
+{
+	struct radeon_fence *fence = NULL;
+	int r;
+
+	r = radeon_set_uvd_clocks(rdev, 53300, 40000);
+	if (r) {
+		DRM_ERROR("radeon: failed to raise UVD clocks (%d).\n", r);
+		return r;
+	}
+
+	r = radeon_uvd_get_create_msg(rdev, ring->idx, 1, NULL);
+	if (r) {
+		DRM_ERROR("radeon: failed to get create msg (%d).\n", r);
+		goto error;
+	}
+
+	r = radeon_uvd_get_destroy_msg(rdev, ring->idx, 1, &fence);
+	if (r) {
+		DRM_ERROR("radeon: failed to get destroy ib (%d).\n", r);
+		goto error;
+	}
+
+	r = radeon_fence_wait(fence, false);
+	if (r) {
+		DRM_ERROR("radeon: fence wait failed (%d).\n", r);
+		goto error;
+	}
+	DRM_INFO("ib test on ring %d succeeded\n",  ring->idx);
+error:
+	radeon_fence_unref(&fence);
+	radeon_set_uvd_clocks(rdev, 0, 0);
+	return r;
+}
+
 /**
  * r600_dma_ring_ib_execute - Schedule an IB on the DMA engine
  *
@@ -4232,7 +4539,7 @@ void r600_ioctl_wait_idle(struct radeon_device *rdev, struct radeon_bo *bo)
 
 void r600_set_pcie_lanes(struct radeon_device *rdev, int lanes)
 {
-	u32 link_width_cntl, mask, target_reg;
+	u32 link_width_cntl, mask;
 
 	if (rdev->flags & RADEON_IS_IGP)
 		return;
@@ -4244,7 +4551,7 @@ void r600_set_pcie_lanes(struct radeon_device *rdev, int lanes)
 	if (ASIC_IS_X2(rdev))
 		return;
 
-	/* FIXME wait for idle */
+	radeon_gui_idle(rdev);
 
 	switch (lanes) {
 	case 0:
@@ -4263,53 +4570,24 @@ void r600_set_pcie_lanes(struct radeon_device *rdev, int lanes)
 		mask = RADEON_PCIE_LC_LINK_WIDTH_X8;
 		break;
 	case 12:
+		/* not actually supported */
 		mask = RADEON_PCIE_LC_LINK_WIDTH_X12;
 		break;
 	case 16:
-	default:
 		mask = RADEON_PCIE_LC_LINK_WIDTH_X16;
 		break;
-	}
-
-	link_width_cntl = RREG32_PCIE_P(RADEON_PCIE_LC_LINK_WIDTH_CNTL);
-
-	if ((link_width_cntl & RADEON_PCIE_LC_LINK_WIDTH_RD_MASK) ==
-	    (mask << RADEON_PCIE_LC_LINK_WIDTH_RD_SHIFT))
-		return;
-
-	if (link_width_cntl & R600_PCIE_LC_UPCONFIGURE_DIS)
+	default:
+		DRM_ERROR("invalid pcie lane request: %d\n", lanes);
 		return;
+	}
 
-	link_width_cntl &= ~(RADEON_PCIE_LC_LINK_WIDTH_MASK |
-			     RADEON_PCIE_LC_RECONFIG_NOW |
-			     R600_PCIE_LC_RENEGOTIATE_EN |
-			     R600_PCIE_LC_RECONFIG_ARC_MISSING_ESCAPE);
-	link_width_cntl |= mask;
-
-	WREG32_PCIE_P(RADEON_PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
-
-        /* some northbridges can renegotiate the link rather than requiring                                  
-         * a complete re-config.                                                                             
-         * e.g., AMD 780/790 northbridges (pci ids: 0x5956, 0x5957, 0x5958, etc.)                            
-         */
-        if (link_width_cntl & R600_PCIE_LC_RENEGOTIATION_SUPPORT)
-		link_width_cntl |= R600_PCIE_LC_RENEGOTIATE_EN | R600_PCIE_LC_UPCONFIGURE_SUPPORT;
-        else
-		link_width_cntl |= R600_PCIE_LC_RECONFIG_ARC_MISSING_ESCAPE;
-
-	WREG32_PCIE_P(RADEON_PCIE_LC_LINK_WIDTH_CNTL, (link_width_cntl |
-						       RADEON_PCIE_LC_RECONFIG_NOW));
-
-        if (rdev->family >= CHIP_RV770)
-		target_reg = R700_TARGET_AND_CURRENT_PROFILE_INDEX;
-        else
-		target_reg = R600_TARGET_AND_CURRENT_PROFILE_INDEX;
-
-        /* wait for lane set to complete */
-        link_width_cntl = RREG32(target_reg);
-        while (link_width_cntl == 0xffffffff)
-		link_width_cntl = RREG32(target_reg);
+	link_width_cntl = RREG32_PCIE_PORT(RADEON_PCIE_LC_LINK_WIDTH_CNTL);
+	link_width_cntl &= ~RADEON_PCIE_LC_LINK_WIDTH_MASK;
+	link_width_cntl |= mask << RADEON_PCIE_LC_LINK_WIDTH_SHIFT;
+	link_width_cntl |= (RADEON_PCIE_LC_RECONFIG_NOW |
+			    R600_PCIE_LC_RECONFIG_ARC_MISSING_ESCAPE);
 
+	WREG32_PCIE_PORT(RADEON_PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
 }
 
 int r600_get_pcie_lanes(struct radeon_device *rdev)
@@ -4326,13 +4604,11 @@ int r600_get_pcie_lanes(struct radeon_device *rdev)
 	if (ASIC_IS_X2(rdev))
 		return 0;
 
-	/* FIXME wait for idle */
+	radeon_gui_idle(rdev);
 
-	link_width_cntl = RREG32_PCIE_P(RADEON_PCIE_LC_LINK_WIDTH_CNTL);
+	link_width_cntl = RREG32_PCIE_PORT(RADEON_PCIE_LC_LINK_WIDTH_CNTL);
 
 	switch ((link_width_cntl & RADEON_PCIE_LC_LINK_WIDTH_RD_MASK) >> RADEON_PCIE_LC_LINK_WIDTH_RD_SHIFT) {
-	case RADEON_PCIE_LC_LINK_WIDTH_X0:
-		return 0;
 	case RADEON_PCIE_LC_LINK_WIDTH_X1:
 		return 1;
 	case RADEON_PCIE_LC_LINK_WIDTH_X2:
@@ -4341,6 +4617,10 @@ int r600_get_pcie_lanes(struct radeon_device *rdev)
 		return 4;
 	case RADEON_PCIE_LC_LINK_WIDTH_X8:
 		return 8;
+	case RADEON_PCIE_LC_LINK_WIDTH_X12:
+		/* not actually supported */
+		return 12;
+	case RADEON_PCIE_LC_LINK_WIDTH_X0:
 	case RADEON_PCIE_LC_LINK_WIDTH_X16:
 	default:
 		return 16;
@@ -4378,7 +4658,7 @@ 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);
+	speed_cntl = RREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL);
 	if (speed_cntl & LC_CURRENT_DATA_RATE) {
 		DRM_INFO("PCIE gen 2 link speeds already enabled\n");
 		return;
@@ -4391,23 +4671,23 @@ static void r600_pcie_gen2_enable(struct radeon_device *rdev)
 	    (rdev->family == CHIP_RV620) ||
 	    (rdev->family == CHIP_RV635)) {
 		/* advertise upconfig capability */
-		link_width_cntl = RREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL);
+		link_width_cntl = RREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL);
 		link_width_cntl &= ~LC_UPCONFIGURE_DIS;
-		WREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
-		link_width_cntl = RREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL);
+		WREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
+		link_width_cntl = RREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL);
 		if (link_width_cntl & LC_RENEGOTIATION_SUPPORT) {
 			lanes = (link_width_cntl & LC_LINK_WIDTH_RD_MASK) >> LC_LINK_WIDTH_RD_SHIFT;
 			link_width_cntl &= ~(LC_LINK_WIDTH_MASK |
 					     LC_RECONFIG_ARC_MISSING_ESCAPE);
 			link_width_cntl |= lanes | LC_RECONFIG_NOW | LC_RENEGOTIATE_EN;
-			WREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
+			WREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
 		} else {
 			link_width_cntl |= LC_UPCONFIGURE_DIS;
-			WREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
+			WREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
 		}
 	}
 
-	speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
+	speed_cntl = RREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL);
 	if ((speed_cntl & LC_OTHER_SIDE_EVER_SENT_GEN2) &&
 	    (speed_cntl & LC_OTHER_SIDE_SUPPORTS_GEN2)) {
 
@@ -4428,7 +4708,7 @@ static void r600_pcie_gen2_enable(struct radeon_device *rdev)
 		speed_cntl &= ~LC_VOLTAGE_TIMER_SEL_MASK;
 		speed_cntl &= ~LC_FORCE_DIS_HW_SPEED_CHANGE;
 		speed_cntl |= LC_FORCE_EN_HW_SPEED_CHANGE;
-		WREG32_PCIE_P(PCIE_LC_SPEED_CNTL, speed_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL, speed_cntl);
 
 		tmp = RREG32(0x541c);
 		WREG32(0x541c, tmp | 0x8);
@@ -4442,27 +4722,27 @@ static void r600_pcie_gen2_enable(struct radeon_device *rdev)
 		if ((rdev->family == CHIP_RV670) ||
 		    (rdev->family == CHIP_RV620) ||
 		    (rdev->family == CHIP_RV635)) {
-			training_cntl = RREG32_PCIE_P(PCIE_LC_TRAINING_CNTL);
+			training_cntl = RREG32_PCIE_PORT(PCIE_LC_TRAINING_CNTL);
 			training_cntl &= ~LC_POINT_7_PLUS_EN;
-			WREG32_PCIE_P(PCIE_LC_TRAINING_CNTL, training_cntl);
+			WREG32_PCIE_PORT(PCIE_LC_TRAINING_CNTL, training_cntl);
 		} else {
-			speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
+			speed_cntl = RREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL);
 			speed_cntl &= ~LC_TARGET_LINK_SPEED_OVERRIDE_EN;
-			WREG32_PCIE_P(PCIE_LC_SPEED_CNTL, speed_cntl);
+			WREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL, speed_cntl);
 		}
 
-		speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
+		speed_cntl = RREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL);
 		speed_cntl |= LC_GEN2_EN_STRAP;
-		WREG32_PCIE_P(PCIE_LC_SPEED_CNTL, speed_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL, speed_cntl);
 
 	} else {
-		link_width_cntl = RREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL);
+		link_width_cntl = RREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL);
 		/* XXX: only disable it if gen1 bridge vendor == 0x111d or 0x1106 */
 		if (1)
 			link_width_cntl |= LC_UPCONFIGURE_DIS;
 		else
 			link_width_cntl &= ~LC_UPCONFIGURE_DIS;
-		WREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
 	}
 }
 
diff --git a/drivers/gpu/drm/radeon/r600_audio.c b/drivers/gpu/drm/radeon/r600_audio.c
index cb03fe22b0ab..c92eb86a8e55 100644
--- a/drivers/gpu/drm/radeon/r600_audio.c
+++ b/drivers/gpu/drm/radeon/r600_audio.c
@@ -57,10 +57,7 @@ static bool radeon_dig_encoder(struct drm_encoder *encoder)
  */
 static int r600_audio_chipset_supported(struct radeon_device *rdev)
 {
-	return (rdev->family >= CHIP_R600 && !ASIC_IS_DCE6(rdev))
-		|| rdev->family == CHIP_RS600
-		|| rdev->family == CHIP_RS690
-		|| rdev->family == CHIP_RS740;
+	return ASIC_IS_DCE2(rdev) && !ASIC_IS_DCE6(rdev);
 }
 
 struct r600_audio r600_audio_status(struct radeon_device *rdev)
@@ -184,65 +181,6 @@ int r600_audio_init(struct radeon_device *rdev)
 }
 
 /*
- * atach the audio codec to the clock source of the encoder
- */
-void r600_audio_set_clock(struct drm_encoder *encoder, int clock)
-{
-	struct drm_device *dev = encoder->dev;
-	struct radeon_device *rdev = dev->dev_private;
-	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
-	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
-	struct radeon_crtc *radeon_crtc = to_radeon_crtc(encoder->crtc);
-	int base_rate = 48000;
-
-	switch (radeon_encoder->encoder_id) {
-	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_TMDS1:
-	case ENCODER_OBJECT_ID_INTERNAL_LVTM1:
-		WREG32_P(R600_AUDIO_TIMING, 0, ~0x301);
-		break;
-	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY:
-	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY1:
-	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY2:
-	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_LVTMA:
-		WREG32_P(R600_AUDIO_TIMING, 0x100, ~0x301);
-		break;
-	default:
-		dev_err(rdev->dev, "Unsupported encoder type 0x%02X\n",
-			  radeon_encoder->encoder_id);
-		return;
-	}
-
-	if (ASIC_IS_DCE4(rdev)) {
-		/* TODO: other PLLs? */
-		WREG32(EVERGREEN_AUDIO_PLL1_MUL, base_rate * 10);
-		WREG32(EVERGREEN_AUDIO_PLL1_DIV, clock * 10);
-		WREG32(EVERGREEN_AUDIO_PLL1_UNK, 0x00000071);
-
-		/* Select DTO source */
-		WREG32(0x5ac, radeon_crtc->crtc_id);
-	} else {
-		switch (dig->dig_encoder) {
-		case 0:
-			WREG32(R600_AUDIO_PLL1_MUL, base_rate * 50);
-			WREG32(R600_AUDIO_PLL1_DIV, clock * 100);
-			WREG32(R600_AUDIO_CLK_SRCSEL, 0);
-			break;
-
-		case 1:
-			WREG32(R600_AUDIO_PLL2_MUL, base_rate * 50);
-			WREG32(R600_AUDIO_PLL2_DIV, clock * 100);
-			WREG32(R600_AUDIO_CLK_SRCSEL, 1);
-			break;
-		default:
-			dev_err(rdev->dev,
-				"Unsupported DIG on encoder 0x%02X\n",
-				radeon_encoder->encoder_id);
-			return;
-		}
-	}
-}
-
-/*
  * release the audio timer
  * TODO: How to do this correctly on SMP systems?
  */
diff --git a/drivers/gpu/drm/radeon/r600_hdmi.c b/drivers/gpu/drm/radeon/r600_hdmi.c
index 21ecc0e12dc4..47f180a79352 100644
--- a/drivers/gpu/drm/radeon/r600_hdmi.c
+++ b/drivers/gpu/drm/radeon/r600_hdmi.c
@@ -226,6 +226,39 @@ static void r600_hdmi_audio_workaround(struct drm_encoder *encoder)
 		 value, ~HDMI0_AUDIO_TEST_EN);
 }
 
+void r600_audio_set_dto(struct drm_encoder *encoder, u32 clock)
+{
+	struct drm_device *dev = encoder->dev;
+	struct radeon_device *rdev = dev->dev_private;
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
+	u32 base_rate = 48000;
+
+	if (!dig || !dig->afmt)
+		return;
+
+	/* there are two DTOs selected by DCCG_AUDIO_DTO_SELECT.
+	 * doesn't matter which one you use.  Just use the first one.
+	 */
+	/* XXX: properly calculate this */
+	/* XXX two dtos; generally use dto0 for hdmi */
+	/* Express [24MHz / target pixel clock] as an exact rational
+	 * number (coefficient of two integer numbers.  DCCG_AUDIO_DTOx_PHASE
+	 * is the numerator, DCCG_AUDIO_DTOx_MODULE is the denominator
+	 */
+	if (ASIC_IS_DCE3(rdev)) {
+		/* according to the reg specs, this should DCE3.2 only, but in
+		 * practice it seems to cover DCE3.0 as well.
+		 */
+		WREG32(DCCG_AUDIO_DTO0_PHASE, base_rate * 50);
+		WREG32(DCCG_AUDIO_DTO0_MODULE, clock * 100);
+		WREG32(DCCG_AUDIO_DTO_SELECT, 0); /* select DTO0 */
+	} else {
+		/* according to the reg specs, this should be DCE2.0 and DCE3.0 */
+		WREG32(AUDIO_DTO, AUDIO_DTO_PHASE(base_rate * 50) |
+		       AUDIO_DTO_MODULE(clock * 100));
+	}
+}
 
 /*
  * update the info frames with the data from the current display mode
@@ -246,7 +279,7 @@ void r600_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mod
 		return;
 	offset = dig->afmt->offset;
 
-	r600_audio_set_clock(encoder, mode->clock);
+	r600_audio_set_dto(encoder, mode->clock);
 
 	WREG32(HDMI0_VBI_PACKET_CONTROL + offset,
 	       HDMI0_NULL_SEND); /* send null packets when required */
@@ -415,114 +448,73 @@ void r600_hdmi_update_audio_settings(struct drm_encoder *encoder)
 /*
  * enable the HDMI engine
  */
-void r600_hdmi_enable(struct drm_encoder *encoder)
+void r600_hdmi_enable(struct drm_encoder *encoder, bool enable)
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
 	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
-	uint32_t offset;
-	u32 hdmi;
-
-	if (ASIC_IS_DCE6(rdev))
-		return;
+	u32 hdmi = HDMI0_ERROR_ACK;
 
 	/* Silent, r600_hdmi_enable will raise WARN for us */
-	if (dig->afmt->enabled)
+	if (enable && dig->afmt->enabled)
+		return;
+	if (!enable && !dig->afmt->enabled)
 		return;
-	offset = dig->afmt->offset;
 
 	/* Older chipsets require setting HDMI and routing manually */
-	if (rdev->family >= CHIP_R600 && !ASIC_IS_DCE3(rdev)) {
-		hdmi = HDMI0_ERROR_ACK | HDMI0_ENABLE;
+	if (!ASIC_IS_DCE3(rdev)) {
+		if (enable)
+			hdmi |= HDMI0_ENABLE;
 		switch (radeon_encoder->encoder_id) {
 		case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_TMDS1:
-			WREG32_P(AVIVO_TMDSA_CNTL, AVIVO_TMDSA_CNTL_HDMI_EN,
-				 ~AVIVO_TMDSA_CNTL_HDMI_EN);
-			hdmi |= HDMI0_STREAM(HDMI0_STREAM_TMDSA);
+			if (enable) {
+				WREG32_OR(AVIVO_TMDSA_CNTL, AVIVO_TMDSA_CNTL_HDMI_EN);
+				hdmi |= HDMI0_STREAM(HDMI0_STREAM_TMDSA);
+			} else {
+				WREG32_AND(AVIVO_TMDSA_CNTL, ~AVIVO_TMDSA_CNTL_HDMI_EN);
+			}
 			break;
 		case ENCODER_OBJECT_ID_INTERNAL_LVTM1:
-			WREG32_P(AVIVO_LVTMA_CNTL, AVIVO_LVTMA_CNTL_HDMI_EN,
-				 ~AVIVO_LVTMA_CNTL_HDMI_EN);
-			hdmi |= HDMI0_STREAM(HDMI0_STREAM_LVTMA);
+			if (enable) {
+				WREG32_OR(AVIVO_LVTMA_CNTL, AVIVO_LVTMA_CNTL_HDMI_EN);
+				hdmi |= HDMI0_STREAM(HDMI0_STREAM_LVTMA);
+			} else {
+				WREG32_AND(AVIVO_LVTMA_CNTL, ~AVIVO_LVTMA_CNTL_HDMI_EN);
+			}
 			break;
 		case ENCODER_OBJECT_ID_INTERNAL_DDI:
-			WREG32_P(DDIA_CNTL, DDIA_HDMI_EN, ~DDIA_HDMI_EN);
-			hdmi |= HDMI0_STREAM(HDMI0_STREAM_DDIA);
+			if (enable) {
+				WREG32_OR(DDIA_CNTL, DDIA_HDMI_EN);
+				hdmi |= HDMI0_STREAM(HDMI0_STREAM_DDIA);
+			} else {
+				WREG32_AND(DDIA_CNTL, ~DDIA_HDMI_EN);
+			}
 			break;
 		case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DVO1:
-			hdmi |= HDMI0_STREAM(HDMI0_STREAM_DVOA);
+			if (enable)
+				hdmi |= HDMI0_STREAM(HDMI0_STREAM_DVOA);
 			break;
 		default:
 			dev_err(rdev->dev, "Invalid encoder for HDMI: 0x%X\n",
 				radeon_encoder->encoder_id);
 			break;
 		}
-		WREG32(HDMI0_CONTROL + offset, hdmi);
+		WREG32(HDMI0_CONTROL + dig->afmt->offset, hdmi);
 	}
 
 	if (rdev->irq.installed) {
 		/* if irq is available use it */
-		radeon_irq_kms_enable_afmt(rdev, dig->afmt->id);
+		/* XXX: shouldn't need this on any asics.  Double check DCE2/3 */
+		if (enable)
+			radeon_irq_kms_enable_afmt(rdev, dig->afmt->id);
+		else
+			radeon_irq_kms_disable_afmt(rdev, dig->afmt->id);
 	}
 
-	dig->afmt->enabled = true;
+	dig->afmt->enabled = enable;
 
-	DRM_DEBUG("Enabling HDMI interface @ 0x%04X for encoder 0x%x\n",
-		  offset, radeon_encoder->encoder_id);
+	DRM_DEBUG("%sabling HDMI interface @ 0x%04X for encoder 0x%x\n",
+		  enable ? "En" : "Dis", dig->afmt->offset, radeon_encoder->encoder_id);
 }
 
-/*
- * disable the HDMI engine
- */
-void r600_hdmi_disable(struct drm_encoder *encoder)
-{
-	struct drm_device *dev = encoder->dev;
-	struct radeon_device *rdev = dev->dev_private;
-	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
-	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
-	uint32_t offset;
-
-	if (ASIC_IS_DCE6(rdev))
-		return;
-
-	/* Called for ATOM_ENCODER_MODE_HDMI only */
-	if (!dig || !dig->afmt) {
-		return;
-	}
-	if (!dig->afmt->enabled)
-		return;
-	offset = dig->afmt->offset;
-
-	DRM_DEBUG("Disabling HDMI interface @ 0x%04X for encoder 0x%x\n",
-		  offset, radeon_encoder->encoder_id);
-
-	/* disable irq */
-	radeon_irq_kms_disable_afmt(rdev, dig->afmt->id);
-
-	/* Older chipsets not handled by AtomBIOS */
-	if (rdev->family >= CHIP_R600 && !ASIC_IS_DCE3(rdev)) {
-		switch (radeon_encoder->encoder_id) {
-		case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_TMDS1:
-			WREG32_P(AVIVO_TMDSA_CNTL, 0,
-				 ~AVIVO_TMDSA_CNTL_HDMI_EN);
-			break;
-		case ENCODER_OBJECT_ID_INTERNAL_LVTM1:
-			WREG32_P(AVIVO_LVTMA_CNTL, 0,
-				 ~AVIVO_LVTMA_CNTL_HDMI_EN);
-			break;
-		case ENCODER_OBJECT_ID_INTERNAL_DDI:
-			WREG32_P(DDIA_CNTL, 0, ~DDIA_HDMI_EN);
-			break;
-		case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DVO1:
-			break;
-		default:
-			dev_err(rdev->dev, "Invalid encoder for HDMI: 0x%X\n",
-				radeon_encoder->encoder_id);
-			break;
-		}
-		WREG32(HDMI0_CONTROL + offset, HDMI0_ERROR_ACK);
-	}
-
-	dig->afmt->enabled = false;
-}
diff --git a/drivers/gpu/drm/radeon/r600d.h b/drivers/gpu/drm/radeon/r600d.h
index a42ba11a3bed..acb146c06973 100644
--- a/drivers/gpu/drm/radeon/r600d.h
+++ b/drivers/gpu/drm/radeon/r600d.h
@@ -691,6 +691,7 @@
 #define SRBM_SOFT_RESET                                   0xe60
 #       define SOFT_RESET_DMA                             (1 << 12)
 #       define SOFT_RESET_RLC                             (1 << 13)
+#       define SOFT_RESET_UVD                             (1 << 18)
 #       define RV770_SOFT_RESET_DMA                       (1 << 20)
 
 #define CP_INT_CNTL                                       0xc124
@@ -909,7 +910,12 @@
 #       define TARGET_LINK_SPEED_MASK                     (0xf << 0)
 #       define SELECTABLE_DEEMPHASIS                      (1 << 6)
 
-/* Audio clocks */
+/* Audio clocks DCE 2.0/3.0 */
+#define AUDIO_DTO                         0x7340
+#       define AUDIO_DTO_PHASE(x)         (((x) & 0xffff) << 0)
+#       define AUDIO_DTO_MODULE(x)        (((x) & 0xffff) << 16)
+
+/* Audio clocks DCE 3.2 */
 #define DCCG_AUDIO_DTO0_PHASE             0x0514
 #define DCCG_AUDIO_DTO0_MODULE            0x0518
 #define DCCG_AUDIO_DTO0_LOAD              0x051c
@@ -1143,6 +1149,70 @@
 #       define AFMT_AZ_AUDIO_ENABLE_CHG_ACK  (1 << 30)
 
 /*
+ * UVD
+ */
+#define UVD_SEMA_ADDR_LOW				0xef00
+#define UVD_SEMA_ADDR_HIGH				0xef04
+#define UVD_SEMA_CMD					0xef08
+
+#define UVD_GPCOM_VCPU_CMD				0xef0c
+#define UVD_GPCOM_VCPU_DATA0				0xef10
+#define UVD_GPCOM_VCPU_DATA1				0xef14
+#define UVD_ENGINE_CNTL					0xef18
+
+#define UVD_SEMA_CNTL					0xf400
+#define UVD_RB_ARB_CTRL					0xf480
+
+#define UVD_LMI_EXT40_ADDR				0xf498
+#define UVD_CGC_GATE					0xf4a8
+#define UVD_LMI_CTRL2					0xf4f4
+#define UVD_MASTINT_EN					0xf500
+#define UVD_LMI_ADDR_EXT				0xf594
+#define UVD_LMI_CTRL					0xf598
+#define UVD_LMI_SWAP_CNTL				0xf5b4
+#define UVD_MP_SWAP_CNTL				0xf5bC
+#define UVD_MPC_CNTL					0xf5dC
+#define UVD_MPC_SET_MUXA0				0xf5e4
+#define UVD_MPC_SET_MUXA1				0xf5e8
+#define UVD_MPC_SET_MUXB0				0xf5eC
+#define UVD_MPC_SET_MUXB1				0xf5f0
+#define UVD_MPC_SET_MUX					0xf5f4
+#define UVD_MPC_SET_ALU					0xf5f8
+
+#define UVD_VCPU_CNTL					0xf660
+#define UVD_SOFT_RESET					0xf680
+#define		RBC_SOFT_RESET					(1<<0)
+#define		LBSI_SOFT_RESET					(1<<1)
+#define		LMI_SOFT_RESET					(1<<2)
+#define		VCPU_SOFT_RESET					(1<<3)
+#define		CSM_SOFT_RESET					(1<<5)
+#define		CXW_SOFT_RESET					(1<<6)
+#define		TAP_SOFT_RESET					(1<<7)
+#define		LMI_UMC_SOFT_RESET				(1<<13)
+#define UVD_RBC_IB_BASE					0xf684
+#define UVD_RBC_IB_SIZE					0xf688
+#define UVD_RBC_RB_BASE					0xf68c
+#define UVD_RBC_RB_RPTR					0xf690
+#define UVD_RBC_RB_WPTR					0xf694
+#define UVD_RBC_RB_WPTR_CNTL				0xf698
+
+#define UVD_STATUS					0xf6bc
+
+#define UVD_SEMA_TIMEOUT_STATUS				0xf6c0
+#define UVD_SEMA_WAIT_INCOMPLETE_TIMEOUT_CNTL		0xf6c4
+#define UVD_SEMA_WAIT_FAULT_TIMEOUT_CNTL		0xf6c8
+#define UVD_SEMA_SIGNAL_INCOMPLETE_TIMEOUT_CNTL		0xf6cc
+
+#define UVD_RBC_RB_CNTL					0xf6a4
+#define UVD_RBC_RB_RPTR_ADDR				0xf6a8
+
+#define UVD_CONTEXT_ID					0xf6f4
+
+#	define UPLL_CTLREQ_MASK				0x00000008
+#	define UPLL_CTLACK_MASK				0x40000000
+#	define UPLL_CTLACK2_MASK			0x80000000
+
+/*
  * PM4
  */
 #define PACKET0(reg, n)	((RADEON_PACKET_TYPE0 << 30) |			\
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index 8263af3fd832..1442ce765d48 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -95,6 +95,7 @@ extern int radeon_hw_i2c;
 extern int radeon_pcie_gen2;
 extern int radeon_msi;
 extern int radeon_lockup_timeout;
+extern int radeon_fastfb;
 
 /*
  * Copy from radeon_drv.h so we don't have to include both and have conflicting
@@ -109,24 +110,27 @@ extern int radeon_lockup_timeout;
 #define RADEON_BIOS_NUM_SCRATCH			8
 
 /* max number of rings */
-#define RADEON_NUM_RINGS			5
+#define RADEON_NUM_RINGS			6
 
 /* fence seq are set to this number when signaled */
 #define RADEON_FENCE_SIGNALED_SEQ		0LL
 
 /* internal ring indices */
 /* r1xx+ has gfx CP ring */
-#define RADEON_RING_TYPE_GFX_INDEX		0
+#define RADEON_RING_TYPE_GFX_INDEX	0
 
 /* cayman has 2 compute CP rings */
-#define CAYMAN_RING_TYPE_CP1_INDEX		1
-#define CAYMAN_RING_TYPE_CP2_INDEX		2
+#define CAYMAN_RING_TYPE_CP1_INDEX	1
+#define CAYMAN_RING_TYPE_CP2_INDEX	2
 
 /* R600+ has an async dma ring */
 #define R600_RING_TYPE_DMA_INDEX		3
 /* cayman add a second async dma ring */
 #define CAYMAN_RING_TYPE_DMA1_INDEX		4
 
+/* R600+ */
+#define R600_RING_TYPE_UVD_INDEX	5
+
 /* hardcode those limit for now */
 #define RADEON_VA_IB_OFFSET			(1 << 20)
 #define RADEON_VA_RESERVED_SIZE			(8 << 20)
@@ -202,6 +206,11 @@ void radeon_pm_suspend(struct radeon_device *rdev);
 void radeon_pm_resume(struct radeon_device *rdev);
 void radeon_combios_get_power_modes(struct radeon_device *rdev);
 void radeon_atombios_get_power_modes(struct radeon_device *rdev);
+int radeon_atom_get_clock_dividers(struct radeon_device *rdev,
+				   u8 clock_type,
+				   u32 clock,
+				   bool strobe_mode,
+				   struct atom_clock_dividers *dividers);
 void radeon_atom_set_voltage(struct radeon_device *rdev, u16 voltage_level, u8 voltage_type);
 void rs690_pm_info(struct radeon_device *rdev);
 extern int rv6xx_get_temp(struct radeon_device *rdev);
@@ -349,7 +358,8 @@ struct radeon_bo {
 	struct radeon_device		*rdev;
 	struct drm_gem_object		gem_base;
 
-	struct ttm_bo_kmap_obj dma_buf_vmap;
+	struct ttm_bo_kmap_obj		dma_buf_vmap;
+	pid_t				pid;
 };
 #define gem_to_radeon_bo(gobj) container_of((gobj), struct radeon_bo, gem_base)
 
@@ -357,11 +367,14 @@ struct radeon_bo_list {
 	struct ttm_validate_buffer tv;
 	struct radeon_bo	*bo;
 	uint64_t		gpu_offset;
-	unsigned		rdomain;
-	unsigned		wdomain;
+	bool			written;
+	unsigned		domain;
+	unsigned		alt_domain;
 	u32			tiling_flags;
 };
 
+int radeon_gem_debugfs_init(struct radeon_device *rdev);
+
 /* sub-allocation manager, it has to be protected by another lock.
  * By conception this is an helper for other part of the driver
  * like the indirect buffer or semaphore, which both have their
@@ -517,6 +530,7 @@ struct radeon_mc {
 	bool			vram_is_ddr;
 	bool			igp_sideport_enabled;
 	u64                     gtt_base_align;
+	u64                     mc_mask;
 };
 
 bool radeon_combios_sideport_present(struct radeon_device *rdev);
@@ -918,6 +932,7 @@ struct radeon_wb {
 #define R600_WB_DMA_RPTR_OFFSET   1792
 #define R600_WB_IH_WPTR_OFFSET   2048
 #define CAYMAN_WB_DMA1_RPTR_OFFSET   2304
+#define R600_WB_UVD_RPTR_OFFSET  2560
 #define R600_WB_EVENT_OFFSET     3072
 
 /**
@@ -1118,6 +1133,46 @@ struct radeon_pm {
 int radeon_pm_get_type_index(struct radeon_device *rdev,
 			     enum radeon_pm_state_type ps_type,
 			     int instance);
+/*
+ * UVD
+ */
+#define RADEON_MAX_UVD_HANDLES	10
+#define RADEON_UVD_STACK_SIZE	(1024*1024)
+#define RADEON_UVD_HEAP_SIZE	(1024*1024)
+
+struct radeon_uvd {
+	struct radeon_bo	*vcpu_bo;
+	void			*cpu_addr;
+	uint64_t		gpu_addr;
+	atomic_t		handles[RADEON_MAX_UVD_HANDLES];
+	struct drm_file		*filp[RADEON_MAX_UVD_HANDLES];
+	struct delayed_work	idle_work;
+};
+
+int radeon_uvd_init(struct radeon_device *rdev);
+void radeon_uvd_fini(struct radeon_device *rdev);
+int radeon_uvd_suspend(struct radeon_device *rdev);
+int radeon_uvd_resume(struct radeon_device *rdev);
+int radeon_uvd_get_create_msg(struct radeon_device *rdev, int ring,
+			      uint32_t handle, struct radeon_fence **fence);
+int radeon_uvd_get_destroy_msg(struct radeon_device *rdev, int ring,
+			       uint32_t handle, struct radeon_fence **fence);
+void radeon_uvd_force_into_uvd_segment(struct radeon_bo *rbo);
+void radeon_uvd_free_handles(struct radeon_device *rdev,
+			     struct drm_file *filp);
+int radeon_uvd_cs_parse(struct radeon_cs_parser *parser);
+void radeon_uvd_note_usage(struct radeon_device *rdev);
+int radeon_uvd_calc_upll_dividers(struct radeon_device *rdev,
+				  unsigned vclk, unsigned dclk,
+				  unsigned vco_min, unsigned vco_max,
+				  unsigned fb_factor, unsigned fb_mask,
+				  unsigned pd_min, unsigned pd_max,
+				  unsigned pd_even,
+				  unsigned *optimal_fb_div,
+				  unsigned *optimal_vclk_div,
+				  unsigned *optimal_dclk_div);
+int radeon_uvd_send_upll_ctlreq(struct radeon_device *rdev,
+                                unsigned cg_upll_func_cntl);
 
 struct r600_audio {
 	int			channels;
@@ -1229,6 +1284,9 @@ struct radeon_asic {
 		void (*set_backlight_level)(struct radeon_encoder *radeon_encoder, u8 level);
 		/* get backlight level */
 		u8 (*get_backlight_level)(struct radeon_encoder *radeon_encoder);
+		/* audio callbacks */
+		void (*hdmi_enable)(struct drm_encoder *encoder, bool enable);
+		void (*hdmi_setmode)(struct drm_encoder *encoder, struct drm_display_mode *mode);
 	} display;
 	/* copy functions for bo handling */
 	struct {
@@ -1281,6 +1339,7 @@ struct radeon_asic {
 		int (*get_pcie_lanes)(struct radeon_device *rdev);
 		void (*set_pcie_lanes)(struct radeon_device *rdev, int lanes);
 		void (*set_clock_gating)(struct radeon_device *rdev, int enable);
+		int (*set_uvd_clocks)(struct radeon_device *rdev, u32 vclk, u32 dclk);
 	} pm;
 	/* pageflipping */
 	struct {
@@ -1443,6 +1502,7 @@ struct si_asic {
 	unsigned multi_gpu_tile_size;
 
 	unsigned tile_config;
+	uint32_t tile_mode_array[32];
 };
 
 union radeon_asic_config {
@@ -1608,6 +1668,7 @@ struct radeon_device {
 	struct radeon_asic		*asic;
 	struct radeon_gem		gem;
 	struct radeon_pm		pm;
+	struct radeon_uvd		uvd;
 	uint32_t			bios_scratch[RADEON_BIOS_NUM_SCRATCH];
 	struct radeon_wb		wb;
 	struct radeon_dummy_page	dummy_page;
@@ -1615,12 +1676,14 @@ struct radeon_device {
 	bool				suspend;
 	bool				need_dma32;
 	bool				accel_working;
+	bool				fastfb_working; /* IGP feature*/
 	struct radeon_surface_reg surface_regs[RADEON_GEM_MAX_SURFACES];
 	const struct firmware *me_fw;	/* all family ME firmware */
 	const struct firmware *pfp_fw;	/* r6/700 PFP firmware */
 	const struct firmware *rlc_fw;	/* r6/700 RLC firmware */
 	const struct firmware *mc_fw;	/* NI MC firmware */
 	const struct firmware *ce_fw;	/* SI CE firmware */
+	const struct firmware *uvd_fw;	/* UVD firmware */
 	struct r600_blit r600_blit;
 	struct r600_vram_scratch vram_scratch;
 	int msi_enabled; /* msi enabled */
@@ -1688,8 +1751,8 @@ void r100_io_wreg(struct radeon_device *rdev, u32 reg, u32 v);
 #define WREG32_MC(reg, v) rdev->mc_wreg(rdev, (reg), (v))
 #define RREG32_PCIE(reg) rv370_pcie_rreg(rdev, (reg))
 #define WREG32_PCIE(reg, v) rv370_pcie_wreg(rdev, (reg), (v))
-#define RREG32_PCIE_P(reg) rdev->pciep_rreg(rdev, (reg))
-#define WREG32_PCIE_P(reg, v) rdev->pciep_wreg(rdev, (reg), (v))
+#define RREG32_PCIE_PORT(reg) rdev->pciep_rreg(rdev, (reg))
+#define WREG32_PCIE_PORT(reg, v) rdev->pciep_wreg(rdev, (reg), (v))
 #define WREG32_P(reg, val, mask)				\
 	do {							\
 		uint32_t tmp_ = RREG32(reg);			\
@@ -1697,6 +1760,8 @@ void r100_io_wreg(struct radeon_device *rdev, u32 reg, u32 v);
 		tmp_ |= ((val) & ~(mask));			\
 		WREG32(reg, tmp_);				\
 	} while (0)
+#define WREG32_AND(reg, and) WREG32_P(reg, 0, and)
+#define WREG32_OR(reg, or) WREG32_P(reg, or, ~or)
 #define WREG32_PLL_P(reg, val, mask)				\
 	do {							\
 		uint32_t tmp_ = RREG32_PLL(reg);		\
@@ -1830,6 +1895,8 @@ void radeon_ring_write(struct radeon_ring *ring, uint32_t v);
 #define radeon_get_vblank_counter(rdev, crtc) (rdev)->asic->display.get_vblank_counter((rdev), (crtc))
 #define radeon_set_backlight_level(rdev, e, l) (rdev)->asic->display.set_backlight_level((e), (l))
 #define radeon_get_backlight_level(rdev, e) (rdev)->asic->display.get_backlight_level((e))
+#define radeon_hdmi_enable(rdev, e, b) (rdev)->asic->display.hdmi_enable((e), (b))
+#define radeon_hdmi_setmode(rdev, e, m) (rdev)->asic->display.hdmi_setmode((e), (m))
 #define radeon_fence_ring_emit(rdev, r, fence) (rdev)->asic->ring[(r)].emit_fence((rdev), (fence))
 #define radeon_semaphore_ring_emit(rdev, r, cp, semaphore, emit_wait) (rdev)->asic->ring[(r)].emit_semaphore((rdev), (cp), (semaphore), (emit_wait))
 #define radeon_copy_blit(rdev, s, d, np, f) (rdev)->asic->copy.blit((rdev), (s), (d), (np), (f))
@@ -1845,6 +1912,7 @@ void radeon_ring_write(struct radeon_ring *ring, uint32_t v);
 #define radeon_get_pcie_lanes(rdev) (rdev)->asic->pm.get_pcie_lanes((rdev))
 #define radeon_set_pcie_lanes(rdev, l) (rdev)->asic->pm.set_pcie_lanes((rdev), (l))
 #define radeon_set_clock_gating(rdev, e) (rdev)->asic->pm.set_clock_gating((rdev), (e))
+#define radeon_set_uvd_clocks(rdev, v, d) (rdev)->asic->pm.set_uvd_clocks((rdev), (v), (d))
 #define radeon_set_surface_reg(rdev, r, f, p, o, s) ((rdev)->asic->surface.set_reg((rdev), (r), (f), (p), (o), (s)))
 #define radeon_clear_surface_reg(rdev, r) ((rdev)->asic->surface.clear_reg((rdev), (r)))
 #define radeon_bandwidth_update(rdev) (rdev)->asic->display.bandwidth_update((rdev))
@@ -1892,6 +1960,9 @@ extern void radeon_gtt_location(struct radeon_device *rdev, struct radeon_mc *mc
 extern int radeon_resume_kms(struct drm_device *dev);
 extern int radeon_suspend_kms(struct drm_device *dev, pm_message_t state);
 extern void radeon_ttm_set_active_vram_size(struct radeon_device *rdev, u64 size);
+extern void radeon_program_register_sequence(struct radeon_device *rdev,
+					     const u32 *registers,
+					     const u32 array_size);
 
 /*
  * vm
@@ -1964,9 +2035,6 @@ struct radeon_hdmi_acr {
 
 extern struct radeon_hdmi_acr r600_hdmi_acr(uint32_t clock);
 
-extern void r600_hdmi_enable(struct drm_encoder *encoder);
-extern void r600_hdmi_disable(struct drm_encoder *encoder);
-extern void r600_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mode);
 extern u32 r6xx_remap_render_backend(struct radeon_device *rdev,
 				     u32 tiling_pipe_num,
 				     u32 max_rb_num,
@@ -1977,8 +2045,6 @@ extern u32 r6xx_remap_render_backend(struct radeon_device *rdev,
  * evergreen functions used by radeon_encoder.c
  */
 
-extern void evergreen_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mode);
-
 extern int ni_init_microcode(struct radeon_device *rdev);
 extern int ni_mc_load_microcode(struct radeon_device *rdev);
 
diff --git a/drivers/gpu/drm/radeon/radeon_asic.c b/drivers/gpu/drm/radeon/radeon_asic.c
index aba0a893ea98..6417132c50cf 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.c
+++ b/drivers/gpu/drm/radeon/radeon_asic.c
@@ -656,6 +656,8 @@ static struct radeon_asic rs600_asic = {
 		.wait_for_vblank = &avivo_wait_for_vblank,
 		.set_backlight_level = &atombios_set_backlight_level,
 		.get_backlight_level = &atombios_get_backlight_level,
+		.hdmi_enable = &r600_hdmi_enable,
+		.hdmi_setmode = &r600_hdmi_setmode,
 	},
 	.copy = {
 		.blit = &r100_copy_blit,
@@ -732,6 +734,8 @@ static struct radeon_asic rs690_asic = {
 		.wait_for_vblank = &avivo_wait_for_vblank,
 		.set_backlight_level = &atombios_set_backlight_level,
 		.get_backlight_level = &atombios_get_backlight_level,
+		.hdmi_enable = &r600_hdmi_enable,
+		.hdmi_setmode = &r600_hdmi_setmode,
 	},
 	.copy = {
 		.blit = &r100_copy_blit,
@@ -970,6 +974,8 @@ static struct radeon_asic r600_asic = {
 		.wait_for_vblank = &avivo_wait_for_vblank,
 		.set_backlight_level = &atombios_set_backlight_level,
 		.get_backlight_level = &atombios_get_backlight_level,
+		.hdmi_enable = &r600_hdmi_enable,
+		.hdmi_setmode = &r600_hdmi_setmode,
 	},
 	.copy = {
 		.blit = &r600_copy_blit,
@@ -1056,6 +1062,8 @@ static struct radeon_asic rs780_asic = {
 		.wait_for_vblank = &avivo_wait_for_vblank,
 		.set_backlight_level = &atombios_set_backlight_level,
 		.get_backlight_level = &atombios_get_backlight_level,
+		.hdmi_enable = &r600_hdmi_enable,
+		.hdmi_setmode = &r600_hdmi_setmode,
 	},
 	.copy = {
 		.blit = &r600_copy_blit,
@@ -1130,6 +1138,15 @@ static struct radeon_asic rv770_asic = {
 			.ring_test = &r600_dma_ring_test,
 			.ib_test = &r600_dma_ib_test,
 			.is_lockup = &r600_dma_is_lockup,
+		},
+		[R600_RING_TYPE_UVD_INDEX] = {
+			.ib_execute = &r600_uvd_ib_execute,
+			.emit_fence = &r600_uvd_fence_emit,
+			.emit_semaphore = &r600_uvd_semaphore_emit,
+			.cs_parse = &radeon_uvd_cs_parse,
+			.ring_test = &r600_uvd_ring_test,
+			.ib_test = &r600_uvd_ib_test,
+			.is_lockup = &radeon_ring_test_lockup,
 		}
 	},
 	.irq = {
@@ -1142,6 +1159,8 @@ static struct radeon_asic rv770_asic = {
 		.wait_for_vblank = &avivo_wait_for_vblank,
 		.set_backlight_level = &atombios_set_backlight_level,
 		.get_backlight_level = &atombios_get_backlight_level,
+		.hdmi_enable = &r600_hdmi_enable,
+		.hdmi_setmode = &r600_hdmi_setmode,
 	},
 	.copy = {
 		.blit = &r600_copy_blit,
@@ -1174,6 +1193,7 @@ static struct radeon_asic rv770_asic = {
 		.get_pcie_lanes = &r600_get_pcie_lanes,
 		.set_pcie_lanes = &r600_set_pcie_lanes,
 		.set_clock_gating = &radeon_atom_set_clock_gating,
+		.set_uvd_clocks = &rv770_set_uvd_clocks,
 	},
 	.pflip = {
 		.pre_page_flip = &rs600_pre_page_flip,
@@ -1216,6 +1236,15 @@ static struct radeon_asic evergreen_asic = {
 			.ring_test = &r600_dma_ring_test,
 			.ib_test = &r600_dma_ib_test,
 			.is_lockup = &evergreen_dma_is_lockup,
+		},
+		[R600_RING_TYPE_UVD_INDEX] = {
+			.ib_execute = &r600_uvd_ib_execute,
+			.emit_fence = &r600_uvd_fence_emit,
+			.emit_semaphore = &r600_uvd_semaphore_emit,
+			.cs_parse = &radeon_uvd_cs_parse,
+			.ring_test = &r600_uvd_ring_test,
+			.ib_test = &r600_uvd_ib_test,
+			.is_lockup = &radeon_ring_test_lockup,
 		}
 	},
 	.irq = {
@@ -1228,6 +1257,8 @@ static struct radeon_asic evergreen_asic = {
 		.wait_for_vblank = &dce4_wait_for_vblank,
 		.set_backlight_level = &atombios_set_backlight_level,
 		.get_backlight_level = &atombios_get_backlight_level,
+		.hdmi_enable = &evergreen_hdmi_enable,
+		.hdmi_setmode = &evergreen_hdmi_setmode,
 	},
 	.copy = {
 		.blit = &r600_copy_blit,
@@ -1260,6 +1291,7 @@ static struct radeon_asic evergreen_asic = {
 		.get_pcie_lanes = &r600_get_pcie_lanes,
 		.set_pcie_lanes = &r600_set_pcie_lanes,
 		.set_clock_gating = NULL,
+		.set_uvd_clocks = &evergreen_set_uvd_clocks,
 	},
 	.pflip = {
 		.pre_page_flip = &evergreen_pre_page_flip,
@@ -1302,6 +1334,15 @@ static struct radeon_asic sumo_asic = {
 			.ring_test = &r600_dma_ring_test,
 			.ib_test = &r600_dma_ib_test,
 			.is_lockup = &evergreen_dma_is_lockup,
+		},
+		[R600_RING_TYPE_UVD_INDEX] = {
+			.ib_execute = &r600_uvd_ib_execute,
+			.emit_fence = &r600_uvd_fence_emit,
+			.emit_semaphore = &r600_uvd_semaphore_emit,
+			.cs_parse = &radeon_uvd_cs_parse,
+			.ring_test = &r600_uvd_ring_test,
+			.ib_test = &r600_uvd_ib_test,
+			.is_lockup = &radeon_ring_test_lockup,
 		}
 	},
 	.irq = {
@@ -1314,6 +1355,8 @@ static struct radeon_asic sumo_asic = {
 		.wait_for_vblank = &dce4_wait_for_vblank,
 		.set_backlight_level = &atombios_set_backlight_level,
 		.get_backlight_level = &atombios_get_backlight_level,
+		.hdmi_enable = &evergreen_hdmi_enable,
+		.hdmi_setmode = &evergreen_hdmi_setmode,
 	},
 	.copy = {
 		.blit = &r600_copy_blit,
@@ -1346,6 +1389,7 @@ static struct radeon_asic sumo_asic = {
 		.get_pcie_lanes = NULL,
 		.set_pcie_lanes = NULL,
 		.set_clock_gating = NULL,
+		.set_uvd_clocks = &sumo_set_uvd_clocks,
 	},
 	.pflip = {
 		.pre_page_flip = &evergreen_pre_page_flip,
@@ -1388,6 +1432,15 @@ static struct radeon_asic btc_asic = {
 			.ring_test = &r600_dma_ring_test,
 			.ib_test = &r600_dma_ib_test,
 			.is_lockup = &evergreen_dma_is_lockup,
+		},
+		[R600_RING_TYPE_UVD_INDEX] = {
+			.ib_execute = &r600_uvd_ib_execute,
+			.emit_fence = &r600_uvd_fence_emit,
+			.emit_semaphore = &r600_uvd_semaphore_emit,
+			.cs_parse = &radeon_uvd_cs_parse,
+			.ring_test = &r600_uvd_ring_test,
+			.ib_test = &r600_uvd_ib_test,
+			.is_lockup = &radeon_ring_test_lockup,
 		}
 	},
 	.irq = {
@@ -1400,6 +1453,8 @@ static struct radeon_asic btc_asic = {
 		.wait_for_vblank = &dce4_wait_for_vblank,
 		.set_backlight_level = &atombios_set_backlight_level,
 		.get_backlight_level = &atombios_get_backlight_level,
+		.hdmi_enable = &evergreen_hdmi_enable,
+		.hdmi_setmode = &evergreen_hdmi_setmode,
 	},
 	.copy = {
 		.blit = &r600_copy_blit,
@@ -1429,9 +1484,10 @@ static struct radeon_asic btc_asic = {
 		.set_engine_clock = &radeon_atom_set_engine_clock,
 		.get_memory_clock = &radeon_atom_get_memory_clock,
 		.set_memory_clock = &radeon_atom_set_memory_clock,
-		.get_pcie_lanes = NULL,
-		.set_pcie_lanes = NULL,
+		.get_pcie_lanes = &r600_get_pcie_lanes,
+		.set_pcie_lanes = &r600_set_pcie_lanes,
 		.set_clock_gating = NULL,
+		.set_uvd_clocks = &evergreen_set_uvd_clocks,
 	},
 	.pflip = {
 		.pre_page_flip = &evergreen_pre_page_flip,
@@ -1517,6 +1573,15 @@ static struct radeon_asic cayman_asic = {
 			.ib_test = &r600_dma_ib_test,
 			.is_lockup = &cayman_dma_is_lockup,
 			.vm_flush = &cayman_dma_vm_flush,
+		},
+		[R600_RING_TYPE_UVD_INDEX] = {
+			.ib_execute = &r600_uvd_ib_execute,
+			.emit_fence = &r600_uvd_fence_emit,
+			.emit_semaphore = &cayman_uvd_semaphore_emit,
+			.cs_parse = &radeon_uvd_cs_parse,
+			.ring_test = &r600_uvd_ring_test,
+			.ib_test = &r600_uvd_ib_test,
+			.is_lockup = &radeon_ring_test_lockup,
 		}
 	},
 	.irq = {
@@ -1529,6 +1594,8 @@ static struct radeon_asic cayman_asic = {
 		.wait_for_vblank = &dce4_wait_for_vblank,
 		.set_backlight_level = &atombios_set_backlight_level,
 		.get_backlight_level = &atombios_get_backlight_level,
+		.hdmi_enable = &evergreen_hdmi_enable,
+		.hdmi_setmode = &evergreen_hdmi_setmode,
 	},
 	.copy = {
 		.blit = &r600_copy_blit,
@@ -1558,9 +1625,10 @@ static struct radeon_asic cayman_asic = {
 		.set_engine_clock = &radeon_atom_set_engine_clock,
 		.get_memory_clock = &radeon_atom_get_memory_clock,
 		.set_memory_clock = &radeon_atom_set_memory_clock,
-		.get_pcie_lanes = NULL,
-		.set_pcie_lanes = NULL,
+		.get_pcie_lanes = &r600_get_pcie_lanes,
+		.set_pcie_lanes = &r600_set_pcie_lanes,
 		.set_clock_gating = NULL,
+		.set_uvd_clocks = &evergreen_set_uvd_clocks,
 	},
 	.pflip = {
 		.pre_page_flip = &evergreen_pre_page_flip,
@@ -1646,6 +1714,15 @@ static struct radeon_asic trinity_asic = {
 			.ib_test = &r600_dma_ib_test,
 			.is_lockup = &cayman_dma_is_lockup,
 			.vm_flush = &cayman_dma_vm_flush,
+		},
+		[R600_RING_TYPE_UVD_INDEX] = {
+			.ib_execute = &r600_uvd_ib_execute,
+			.emit_fence = &r600_uvd_fence_emit,
+			.emit_semaphore = &cayman_uvd_semaphore_emit,
+			.cs_parse = &radeon_uvd_cs_parse,
+			.ring_test = &r600_uvd_ring_test,
+			.ib_test = &r600_uvd_ib_test,
+			.is_lockup = &radeon_ring_test_lockup,
 		}
 	},
 	.irq = {
@@ -1690,6 +1767,7 @@ static struct radeon_asic trinity_asic = {
 		.get_pcie_lanes = NULL,
 		.set_pcie_lanes = NULL,
 		.set_clock_gating = NULL,
+		.set_uvd_clocks = &sumo_set_uvd_clocks,
 	},
 	.pflip = {
 		.pre_page_flip = &evergreen_pre_page_flip,
@@ -1775,6 +1853,15 @@ static struct radeon_asic si_asic = {
 			.ib_test = &r600_dma_ib_test,
 			.is_lockup = &si_dma_is_lockup,
 			.vm_flush = &si_dma_vm_flush,
+		},
+		[R600_RING_TYPE_UVD_INDEX] = {
+			.ib_execute = &r600_uvd_ib_execute,
+			.emit_fence = &r600_uvd_fence_emit,
+			.emit_semaphore = &cayman_uvd_semaphore_emit,
+			.cs_parse = &radeon_uvd_cs_parse,
+			.ring_test = &r600_uvd_ring_test,
+			.ib_test = &r600_uvd_ib_test,
+			.is_lockup = &radeon_ring_test_lockup,
 		}
 	},
 	.irq = {
@@ -1816,9 +1903,10 @@ static struct radeon_asic si_asic = {
 		.set_engine_clock = &radeon_atom_set_engine_clock,
 		.get_memory_clock = &radeon_atom_get_memory_clock,
 		.set_memory_clock = &radeon_atom_set_memory_clock,
-		.get_pcie_lanes = NULL,
-		.set_pcie_lanes = NULL,
+		.get_pcie_lanes = &r600_get_pcie_lanes,
+		.set_pcie_lanes = &r600_set_pcie_lanes,
 		.set_clock_gating = NULL,
+		.set_uvd_clocks = &si_set_uvd_clocks,
 	},
 	.pflip = {
 		.pre_page_flip = &evergreen_pre_page_flip,
diff --git a/drivers/gpu/drm/radeon/radeon_asic.h b/drivers/gpu/drm/radeon/radeon_asic.h
index 3535f73ad3e2..2c87365d345f 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.h
+++ b/drivers/gpu/drm/radeon/radeon_asic.h
@@ -330,6 +330,7 @@ int r600_dma_ib_test(struct radeon_device *rdev, struct radeon_ring *ring);
 void r600_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib);
 int r600_ring_test(struct radeon_device *rdev, struct radeon_ring *cp);
 int r600_dma_ring_test(struct radeon_device *rdev, struct radeon_ring *cp);
+int r600_uvd_ring_test(struct radeon_device *rdev, struct radeon_ring *ring);
 int r600_copy_blit(struct radeon_device *rdev,
 		   uint64_t src_offset, uint64_t dst_offset,
 		   unsigned num_gpu_pages, struct radeon_fence **fence);
@@ -373,11 +374,12 @@ void r600_disable_interrupts(struct radeon_device *rdev);
 void r600_rlc_stop(struct radeon_device *rdev);
 /* r600 audio */
 int r600_audio_init(struct radeon_device *rdev);
-void r600_audio_set_clock(struct drm_encoder *encoder, int clock);
 struct r600_audio r600_audio_status(struct radeon_device *rdev);
 void r600_audio_fini(struct radeon_device *rdev);
 int r600_hdmi_buffer_status_changed(struct drm_encoder *encoder);
 void r600_hdmi_update_audio_settings(struct drm_encoder *encoder);
+void r600_hdmi_enable(struct drm_encoder *encoder, bool enable);
+void r600_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mode);
 /* r600 blit */
 int r600_blit_prepare_copy(struct radeon_device *rdev, unsigned num_gpu_pages,
 			   struct radeon_fence **fence, struct radeon_sa_bo **vb,
@@ -392,6 +394,19 @@ int r600_mc_wait_for_idle(struct radeon_device *rdev);
 u32 r600_get_xclk(struct radeon_device *rdev);
 uint64_t r600_get_gpu_clock_counter(struct radeon_device *rdev);
 
+/* uvd */
+int r600_uvd_init(struct radeon_device *rdev);
+int r600_uvd_rbc_start(struct radeon_device *rdev);
+void r600_uvd_rbc_stop(struct radeon_device *rdev);
+int r600_uvd_ib_test(struct radeon_device *rdev, struct radeon_ring *ring);
+void r600_uvd_fence_emit(struct radeon_device *rdev,
+			 struct radeon_fence *fence);
+void r600_uvd_semaphore_emit(struct radeon_device *rdev,
+			     struct radeon_ring *ring,
+			     struct radeon_semaphore *semaphore,
+			     bool emit_wait);
+void r600_uvd_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib);
+
 /*
  * rv770,rv730,rv710,rv740
  */
@@ -409,6 +424,8 @@ int rv770_copy_dma(struct radeon_device *rdev,
 		  unsigned num_gpu_pages,
 		   struct radeon_fence **fence);
 u32 rv770_get_xclk(struct radeon_device *rdev);
+int rv770_uvd_resume(struct radeon_device *rdev);
+int rv770_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk);
 
 /*
  * evergreen
@@ -444,6 +461,8 @@ extern void evergreen_pm_prepare(struct radeon_device *rdev);
 extern void evergreen_pm_finish(struct radeon_device *rdev);
 extern void sumo_pm_init_profile(struct radeon_device *rdev);
 extern void btc_pm_init_profile(struct radeon_device *rdev);
+int sumo_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk);
+int evergreen_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk);
 extern void evergreen_pre_page_flip(struct radeon_device *rdev, int crtc);
 extern u32 evergreen_page_flip(struct radeon_device *rdev, int crtc, u64 crtc_base);
 extern void evergreen_post_page_flip(struct radeon_device *rdev, int crtc);
@@ -459,12 +478,18 @@ int evergreen_copy_dma(struct radeon_device *rdev,
 		       uint64_t src_offset, uint64_t dst_offset,
 		       unsigned num_gpu_pages,
 		       struct radeon_fence **fence);
+void evergreen_hdmi_enable(struct drm_encoder *encoder, bool enable);
+void evergreen_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mode);
 
 /*
  * cayman
  */
 void cayman_fence_ring_emit(struct radeon_device *rdev,
 			    struct radeon_fence *fence);
+void cayman_uvd_semaphore_emit(struct radeon_device *rdev,
+			       struct radeon_ring *ring,
+			       struct radeon_semaphore *semaphore,
+			       bool emit_wait);
 void cayman_pcie_gart_tlb_flush(struct radeon_device *rdev);
 int cayman_init(struct radeon_device *rdev);
 void cayman_fini(struct radeon_device *rdev);
@@ -524,5 +549,6 @@ int si_copy_dma(struct radeon_device *rdev,
 void si_dma_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm);
 u32 si_get_xclk(struct radeon_device *rdev);
 uint64_t si_get_gpu_clock_counter(struct radeon_device *rdev);
+int si_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk);
 
 #endif
diff --git a/drivers/gpu/drm/radeon/radeon_atombios.c b/drivers/gpu/drm/radeon/radeon_atombios.c
index f22eb5713528..dea6f63c9724 100644
--- a/drivers/gpu/drm/radeon/radeon_atombios.c
+++ b/drivers/gpu/drm/radeon/radeon_atombios.c
@@ -2028,6 +2028,8 @@ static int radeon_atombios_parse_power_table_1_3(struct radeon_device *rdev)
 	num_modes = power_info->info.ucNumOfPowerModeEntries;
 	if (num_modes > ATOM_MAX_NUMBEROF_POWER_BLOCK)
 		num_modes = ATOM_MAX_NUMBEROF_POWER_BLOCK;
+	if (num_modes == 0)
+		return state_index;
 	rdev->pm.power_state = kzalloc(sizeof(struct radeon_power_state) * num_modes, GFP_KERNEL);
 	if (!rdev->pm.power_state)
 		return state_index;
@@ -2307,7 +2309,7 @@ static void radeon_atombios_parse_pplib_non_clock_info(struct radeon_device *rde
 		rdev->pm.default_power_state_index = state_index;
 		rdev->pm.power_state[state_index].default_clock_mode =
 			&rdev->pm.power_state[state_index].clock_info[mode_index - 1];
-		if (ASIC_IS_DCE5(rdev) && !(rdev->flags & RADEON_IS_IGP)) {
+		if ((rdev->family >= CHIP_BARTS) && !(rdev->flags & RADEON_IS_IGP)) {
 			/* NI chips post without MC ucode, so default clocks are strobe mode only */
 			rdev->pm.default_sclk = rdev->pm.power_state[state_index].clock_info[0].sclk;
 			rdev->pm.default_mclk = rdev->pm.power_state[state_index].clock_info[0].mclk;
@@ -2345,7 +2347,7 @@ static bool radeon_atombios_parse_pplib_clock_info(struct radeon_device *rdev,
 			sclk |= clock_info->rs780.ucLowEngineClockHigh << 16;
 			rdev->pm.power_state[state_index].clock_info[mode_index].sclk = sclk;
 		}
-	} else if (ASIC_IS_DCE6(rdev)) {
+	} else if (rdev->family >= CHIP_TAHITI) {
 		sclk = le16_to_cpu(clock_info->si.usEngineClockLow);
 		sclk |= clock_info->si.ucEngineClockHigh << 16;
 		mclk = le16_to_cpu(clock_info->si.usMemoryClockLow);
@@ -2358,7 +2360,7 @@ static bool radeon_atombios_parse_pplib_clock_info(struct radeon_device *rdev,
 			le16_to_cpu(clock_info->si.usVDDC);
 		rdev->pm.power_state[state_index].clock_info[mode_index].voltage.vddci =
 			le16_to_cpu(clock_info->si.usVDDCI);
-	} else if (ASIC_IS_DCE4(rdev)) {
+	} else if (rdev->family >= CHIP_CEDAR) {
 		sclk = le16_to_cpu(clock_info->evergreen.usEngineClockLow);
 		sclk |= clock_info->evergreen.ucEngineClockHigh << 16;
 		mclk = le16_to_cpu(clock_info->evergreen.usMemoryClockLow);
@@ -2432,6 +2434,8 @@ static int radeon_atombios_parse_power_table_4_5(struct radeon_device *rdev)
 	power_info = (union power_info *)(mode_info->atom_context->bios + data_offset);
 
 	radeon_atombios_add_pplib_thermal_controller(rdev, &power_info->pplib.sThermalController);
+	if (power_info->pplib.ucNumStates == 0)
+		return state_index;
 	rdev->pm.power_state = kzalloc(sizeof(struct radeon_power_state) *
 				       power_info->pplib.ucNumStates, GFP_KERNEL);
 	if (!rdev->pm.power_state)
@@ -2514,6 +2518,7 @@ static int radeon_atombios_parse_power_table_6(struct radeon_device *rdev)
 	int index = GetIndexIntoMasterTable(DATA, PowerPlayInfo);
         u16 data_offset;
 	u8 frev, crev;
+	u8 *power_state_offset;
 
 	if (!atom_parse_data_header(mode_info->atom_context, index, NULL,
 				   &frev, &crev, &data_offset))
@@ -2530,15 +2535,17 @@ static int radeon_atombios_parse_power_table_6(struct radeon_device *rdev)
 	non_clock_info_array = (struct _NonClockInfoArray *)
 		(mode_info->atom_context->bios + data_offset +
 		 le16_to_cpu(power_info->pplib.usNonClockInfoArrayOffset));
+	if (state_array->ucNumEntries == 0)
+		return state_index;
 	rdev->pm.power_state = kzalloc(sizeof(struct radeon_power_state) *
 				       state_array->ucNumEntries, GFP_KERNEL);
 	if (!rdev->pm.power_state)
 		return state_index;
+	power_state_offset = (u8 *)state_array->states;
 	for (i = 0; i < state_array->ucNumEntries; i++) {
 		mode_index = 0;
-		power_state = (union pplib_power_state *)&state_array->states[i];
-		/* XXX this might be an inagua bug... */
-		non_clock_array_index = i; /* power_state->v2.nonClockInfoIndex */
+		power_state = (union pplib_power_state *)power_state_offset;
+		non_clock_array_index = power_state->v2.nonClockInfoIndex;
 		non_clock_info = (struct _ATOM_PPLIB_NONCLOCK_INFO *)
 			&non_clock_info_array->nonClockInfo[non_clock_array_index];
 		rdev->pm.power_state[i].clock_info = kzalloc(sizeof(struct radeon_pm_clock_info) *
@@ -2550,9 +2557,6 @@ static int radeon_atombios_parse_power_table_6(struct radeon_device *rdev)
 		if (power_state->v2.ucNumDPMLevels) {
 			for (j = 0; j < power_state->v2.ucNumDPMLevels; j++) {
 				clock_array_index = power_state->v2.clockInfoIndex[j];
-				/* XXX this might be an inagua bug... */
-				if (clock_array_index >= clock_info_array->ucNumEntries)
-					continue;
 				clock_info = (union pplib_clock_info *)
 					&clock_info_array->clockInfo[clock_array_index * clock_info_array->ucEntrySize];
 				valid = radeon_atombios_parse_pplib_clock_info(rdev,
@@ -2574,6 +2578,7 @@ static int radeon_atombios_parse_power_table_6(struct radeon_device *rdev)
 								   non_clock_info);
 			state_index++;
 		}
+		power_state_offset += 2 + power_state->v2.ucNumDPMLevels;
 	}
 	/* if multiple clock modes, mark the lowest as no display */
 	for (i = 0; i < state_index; i++) {
@@ -2620,7 +2625,9 @@ void radeon_atombios_get_power_modes(struct radeon_device *rdev)
 		default:
 			break;
 		}
-	} else {
+	}
+
+	if (state_index == 0) {
 		rdev->pm.power_state = kzalloc(sizeof(struct radeon_power_state), GFP_KERNEL);
 		if (rdev->pm.power_state) {
 			rdev->pm.power_state[0].clock_info =
@@ -2654,6 +2661,111 @@ void radeon_atombios_get_power_modes(struct radeon_device *rdev)
 		rdev->pm.current_vddc = 0;
 }
 
+union get_clock_dividers {
+	struct _COMPUTE_MEMORY_ENGINE_PLL_PARAMETERS v1;
+	struct _COMPUTE_MEMORY_ENGINE_PLL_PARAMETERS_V2 v2;
+	struct _COMPUTE_MEMORY_ENGINE_PLL_PARAMETERS_V3 v3;
+	struct _COMPUTE_MEMORY_ENGINE_PLL_PARAMETERS_V4 v4;
+	struct _COMPUTE_MEMORY_ENGINE_PLL_PARAMETERS_V5 v5;
+};
+
+int radeon_atom_get_clock_dividers(struct radeon_device *rdev,
+				   u8 clock_type,
+				   u32 clock,
+				   bool strobe_mode,
+				   struct atom_clock_dividers *dividers)
+{
+	union get_clock_dividers args;
+	int index = GetIndexIntoMasterTable(COMMAND, ComputeMemoryEnginePLL);
+	u8 frev, crev;
+
+	memset(&args, 0, sizeof(args));
+	memset(dividers, 0, sizeof(struct atom_clock_dividers));
+
+	if (!atom_parse_cmd_header(rdev->mode_info.atom_context, index, &frev, &crev))
+		return -EINVAL;
+
+	switch (crev) {
+	case 1:
+		/* r4xx, r5xx */
+		args.v1.ucAction = clock_type;
+		args.v1.ulClock = cpu_to_le32(clock);	/* 10 khz */
+
+		atom_execute_table(rdev->mode_info.atom_context, index, (uint32_t *)&args);
+
+		dividers->post_div = args.v1.ucPostDiv;
+		dividers->fb_div = args.v1.ucFbDiv;
+		dividers->enable_post_div = true;
+		break;
+	case 2:
+	case 3:
+		/* r6xx, r7xx, evergreen, ni */
+		if (rdev->family <= CHIP_RV770) {
+			args.v2.ucAction = clock_type;
+			args.v2.ulClock = cpu_to_le32(clock);	/* 10 khz */
+
+			atom_execute_table(rdev->mode_info.atom_context, index, (uint32_t *)&args);
+
+			dividers->post_div = args.v2.ucPostDiv;
+			dividers->fb_div = le16_to_cpu(args.v2.usFbDiv);
+			dividers->ref_div = args.v2.ucAction;
+			if (rdev->family == CHIP_RV770) {
+				dividers->enable_post_div = (le32_to_cpu(args.v2.ulClock) & (1 << 24)) ?
+					true : false;
+				dividers->vco_mode = (le32_to_cpu(args.v2.ulClock) & (1 << 25)) ? 1 : 0;
+			} else
+				dividers->enable_post_div = (dividers->fb_div & 1) ? true : false;
+		} else {
+			if (clock_type == COMPUTE_ENGINE_PLL_PARAM) {
+				args.v3.ulClockParams = cpu_to_le32((clock_type << 24) | clock);
+
+				atom_execute_table(rdev->mode_info.atom_context, index, (uint32_t *)&args);
+
+				dividers->post_div = args.v3.ucPostDiv;
+				dividers->enable_post_div = (args.v3.ucCntlFlag &
+							     ATOM_PLL_CNTL_FLAG_PLL_POST_DIV_EN) ? true : false;
+				dividers->enable_dithen = (args.v3.ucCntlFlag &
+							   ATOM_PLL_CNTL_FLAG_FRACTION_DISABLE) ? false : true;
+				dividers->fb_div = le16_to_cpu(args.v3.ulFbDiv.usFbDiv);
+				dividers->frac_fb_div = le16_to_cpu(args.v3.ulFbDiv.usFbDivFrac);
+				dividers->ref_div = args.v3.ucRefDiv;
+				dividers->vco_mode = (args.v3.ucCntlFlag &
+						      ATOM_PLL_CNTL_FLAG_MPLL_VCO_MODE) ? 1 : 0;
+			} else {
+				args.v5.ulClockParams = cpu_to_le32((clock_type << 24) | clock);
+				if (strobe_mode)
+					args.v5.ucInputFlag = ATOM_PLL_INPUT_FLAG_PLL_STROBE_MODE_EN;
+
+				atom_execute_table(rdev->mode_info.atom_context, index, (uint32_t *)&args);
+
+				dividers->post_div = args.v5.ucPostDiv;
+				dividers->enable_post_div = (args.v5.ucCntlFlag &
+							     ATOM_PLL_CNTL_FLAG_PLL_POST_DIV_EN) ? true : false;
+				dividers->enable_dithen = (args.v5.ucCntlFlag &
+							   ATOM_PLL_CNTL_FLAG_FRACTION_DISABLE) ? false : true;
+				dividers->whole_fb_div = le16_to_cpu(args.v5.ulFbDiv.usFbDiv);
+				dividers->frac_fb_div = le16_to_cpu(args.v5.ulFbDiv.usFbDivFrac);
+				dividers->ref_div = args.v5.ucRefDiv;
+				dividers->vco_mode = (args.v5.ucCntlFlag &
+						      ATOM_PLL_CNTL_FLAG_MPLL_VCO_MODE) ? 1 : 0;
+			}
+		}
+		break;
+	case 4:
+		/* fusion */
+		args.v4.ulClock = cpu_to_le32(clock);	/* 10 khz */
+
+		atom_execute_table(rdev->mode_info.atom_context, index, (uint32_t *)&args);
+
+		dividers->post_div = args.v4.ucPostDiv;
+		dividers->real_clock = le32_to_cpu(args.v4.ulClock);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
 void radeon_atom_set_clock_gating(struct radeon_device *rdev, int enable)
 {
 	DYNAMIC_CLOCK_GATING_PS_ALLOCATION args;
diff --git a/drivers/gpu/drm/radeon/radeon_cs.c b/drivers/gpu/drm/radeon/radeon_cs.c
index 70d38241b083..7e265a58141f 100644
--- a/drivers/gpu/drm/radeon/radeon_cs.c
+++ b/drivers/gpu/drm/radeon/radeon_cs.c
@@ -63,30 +63,50 @@ static int radeon_cs_parser_relocs(struct radeon_cs_parser *p)
 				break;
 			}
 		}
-		if (!duplicate) {
-			p->relocs[i].gobj = drm_gem_object_lookup(ddev,
-								  p->filp,
-								  r->handle);
-			if (p->relocs[i].gobj == NULL) {
-				DRM_ERROR("gem object lookup failed 0x%x\n",
-					  r->handle);
-				return -ENOENT;
-			}
-			p->relocs_ptr[i] = &p->relocs[i];
-			p->relocs[i].robj = gem_to_radeon_bo(p->relocs[i].gobj);
-			p->relocs[i].lobj.bo = p->relocs[i].robj;
-			p->relocs[i].lobj.wdomain = r->write_domain;
-			p->relocs[i].lobj.rdomain = r->read_domains;
-			p->relocs[i].lobj.tv.bo = &p->relocs[i].robj->tbo;
-			p->relocs[i].handle = r->handle;
-			p->relocs[i].flags = r->flags;
-			radeon_bo_list_add_object(&p->relocs[i].lobj,
-						  &p->validated);
-
-		} else
+		if (duplicate) {
 			p->relocs[i].handle = 0;
+			continue;
+		}
+
+		p->relocs[i].gobj = drm_gem_object_lookup(ddev, p->filp,
+							  r->handle);
+		if (p->relocs[i].gobj == NULL) {
+			DRM_ERROR("gem object lookup failed 0x%x\n",
+				  r->handle);
+			return -ENOENT;
+		}
+		p->relocs_ptr[i] = &p->relocs[i];
+		p->relocs[i].robj = gem_to_radeon_bo(p->relocs[i].gobj);
+		p->relocs[i].lobj.bo = p->relocs[i].robj;
+		p->relocs[i].lobj.written = !!r->write_domain;
+
+		/* the first reloc of an UVD job is the
+		   msg and that must be in VRAM */
+		if (p->ring == R600_RING_TYPE_UVD_INDEX && i == 0) {
+			/* TODO: is this still needed for NI+ ? */
+			p->relocs[i].lobj.domain =
+				RADEON_GEM_DOMAIN_VRAM;
+
+			p->relocs[i].lobj.alt_domain =
+				RADEON_GEM_DOMAIN_VRAM;
+
+		} else {
+			uint32_t domain = r->write_domain ?
+				r->write_domain : r->read_domains;
+
+			p->relocs[i].lobj.domain = domain;
+			if (domain == RADEON_GEM_DOMAIN_VRAM)
+				domain |= RADEON_GEM_DOMAIN_GTT;
+			p->relocs[i].lobj.alt_domain = domain;
+		}
+
+		p->relocs[i].lobj.tv.bo = &p->relocs[i].robj->tbo;
+		p->relocs[i].handle = r->handle;
+
+		radeon_bo_list_add_object(&p->relocs[i].lobj,
+					  &p->validated);
 	}
-	return radeon_bo_list_validate(&p->validated);
+	return radeon_bo_list_validate(&p->validated, p->ring);
 }
 
 static int radeon_cs_get_ring(struct radeon_cs_parser *p, u32 ring, s32 priority)
@@ -121,6 +141,9 @@ static int radeon_cs_get_ring(struct radeon_cs_parser *p, u32 ring, s32 priority
 			return -EINVAL;
 		}
 		break;
+	case RADEON_CS_RING_UVD:
+		p->ring = R600_RING_TYPE_UVD_INDEX;
+		break;
 	}
 	return 0;
 }
@@ -241,15 +264,15 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 			return -EINVAL;
 		}
 
-		/* we only support VM on SI+ */
-		if ((p->rdev->family >= CHIP_TAHITI) &&
-		    ((p->cs_flags & RADEON_CS_USE_VM) == 0)) {
-			DRM_ERROR("VM required on SI+!\n");
+		if (radeon_cs_get_ring(p, ring, priority))
 			return -EINVAL;
-		}
 
-		if (radeon_cs_get_ring(p, ring, priority))
+		/* we only support VM on some SI+ rings */
+		if ((p->rdev->asic->ring[p->ring].cs_parse == NULL) &&
+		   ((p->cs_flags & RADEON_CS_USE_VM) == 0)) {
+			DRM_ERROR("Ring %d requires VM!\n", p->ring);
 			return -EINVAL;
+		}
 	}
 
 	/* deal with non-vm */
@@ -526,6 +549,10 @@ int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 		r = radeon_cs_handle_lockup(rdev, r);
 		return r;
 	}
+
+	if (parser.ring == R600_RING_TYPE_UVD_INDEX)
+		radeon_uvd_note_usage(rdev);
+
 	r = radeon_cs_ib_chunk(rdev, &parser);
 	if (r) {
 		goto out;
diff --git a/drivers/gpu/drm/radeon/radeon_device.c b/drivers/gpu/drm/radeon/radeon_device.c
index 44b8034a400d..a8f608903989 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -98,6 +98,42 @@ static const char radeon_family_name[][16] = {
 };
 
 /**
+ * radeon_program_register_sequence - program an array of registers.
+ *
+ * @rdev: radeon_device pointer
+ * @registers: pointer to the register array
+ * @array_size: size of the register array
+ *
+ * Programs an array or registers with and and or masks.
+ * This is a helper for setting golden registers.
+ */
+void radeon_program_register_sequence(struct radeon_device *rdev,
+				      const u32 *registers,
+				      const u32 array_size)
+{
+	u32 tmp, reg, and_mask, or_mask;
+	int i;
+
+	if (array_size % 3)
+		return;
+
+	for (i = 0; i < array_size; i +=3) {
+		reg = registers[i + 0];
+		and_mask = registers[i + 1];
+		or_mask = registers[i + 2];
+
+		if (and_mask == 0xffffffff) {
+			tmp = or_mask;
+		} else {
+			tmp = RREG32(reg);
+			tmp &= ~and_mask;
+			tmp |= or_mask;
+		}
+		WREG32(reg, tmp);
+	}
+}
+
+/**
  * radeon_surface_init - Clear GPU surface registers.
  *
  * @rdev: radeon_device pointer
@@ -359,7 +395,7 @@ void radeon_vram_location(struct radeon_device *rdev, struct radeon_mc *mc, u64
 	uint64_t limit = (uint64_t)radeon_vram_limit << 20;
 
 	mc->vram_start = base;
-	if (mc->mc_vram_size > (0xFFFFFFFF - base + 1)) {
+	if (mc->mc_vram_size > (rdev->mc.mc_mask - base + 1)) {
 		dev_warn(rdev->dev, "limiting VRAM to PCI aperture size\n");
 		mc->real_vram_size = mc->aper_size;
 		mc->mc_vram_size = mc->aper_size;
@@ -394,7 +430,7 @@ void radeon_gtt_location(struct radeon_device *rdev, struct radeon_mc *mc)
 {
 	u64 size_af, size_bf;
 
-	size_af = ((0xFFFFFFFF - mc->vram_end) + mc->gtt_base_align) & ~mc->gtt_base_align;
+	size_af = ((rdev->mc.mc_mask - mc->vram_end) + mc->gtt_base_align) & ~mc->gtt_base_align;
 	size_bf = mc->vram_start & ~mc->gtt_base_align;
 	if (size_bf > size_af) {
 		if (mc->gtt_size > size_bf) {
@@ -1068,6 +1104,17 @@ int radeon_device_init(struct radeon_device *rdev,
 		radeon_agp_disable(rdev);
 	}
 
+	/* Set the internal MC address mask
+	 * This is the max address of the GPU's
+	 * internal address space.
+	 */
+	if (rdev->family >= CHIP_CAYMAN)
+		rdev->mc.mc_mask = 0xffffffffffULL; /* 40 bit MC */
+	else if (rdev->family >= CHIP_CEDAR)
+		rdev->mc.mc_mask = 0xfffffffffULL; /* 36 bit MC */
+	else
+		rdev->mc.mc_mask = 0xffffffffULL; /* 32 bit MC */
+
 	/* set DMA mask + need_dma32 flags.
 	 * PCIE - can handle 40-bits.
 	 * IGP - can handle 40-bits
@@ -1131,6 +1178,11 @@ int radeon_device_init(struct radeon_device *rdev,
 	if (r)
 		DRM_ERROR("ib ring test failed (%d).\n", r);
 
+	r = radeon_gem_debugfs_init(rdev);
+	if (r) {
+		DRM_ERROR("registering gem debugfs failed (%d).\n", r);
+	}
+
 	if (rdev->flags & RADEON_IS_AGP && !rdev->accel_working) {
 		/* Acceleration not working on AGP card try again
 		 * with fallback to PCI or PCIE GART
diff --git a/drivers/gpu/drm/radeon/radeon_drv.c b/drivers/gpu/drm/radeon/radeon_drv.c
index 66a7f0fd9620..d33f484ace48 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.c
+++ b/drivers/gpu/drm/radeon/radeon_drv.c
@@ -71,9 +71,12 @@
  *   2.28.0 - r600-eg: Add MEM_WRITE packet support
  *   2.29.0 - R500 FP16 color clear registers
  *   2.30.0 - fix for FMASK texturing
+ *   2.31.0 - Add fastfb support for rs690
+ *   2.32.0 - new info request for rings working
+ *   2.33.0 - Add SI tiling mode array query
  */
 #define KMS_DRIVER_MAJOR	2
-#define KMS_DRIVER_MINOR	30
+#define KMS_DRIVER_MINOR	33
 #define KMS_DRIVER_PATCHLEVEL	0
 int radeon_driver_load_kms(struct drm_device *dev, unsigned long flags);
 int radeon_driver_unload_kms(struct drm_device *dev);
@@ -160,6 +163,7 @@ int radeon_hw_i2c = 0;
 int radeon_pcie_gen2 = -1;
 int radeon_msi = -1;
 int radeon_lockup_timeout = 10000;
+int radeon_fastfb = 0;
 
 MODULE_PARM_DESC(no_wb, "Disable AGP writeback for scratch registers");
 module_param_named(no_wb, radeon_no_wb, int, 0444);
@@ -212,6 +216,9 @@ module_param_named(msi, radeon_msi, int, 0444);
 MODULE_PARM_DESC(lockup_timeout, "GPU lockup timeout in ms (defaul 10000 = 10 seconds, 0 = disable)");
 module_param_named(lockup_timeout, radeon_lockup_timeout, int, 0444);
 
+MODULE_PARM_DESC(fastfb, "Direct FB access for IGP chips (0 = disable, 1 = enable)");
+module_param_named(fastfb, radeon_fastfb, int, 0444);
+
 static struct pci_device_id pciidlist[] = {
 	radeon_PCI_IDS
 };
diff --git a/drivers/gpu/drm/radeon/radeon_fence.c b/drivers/gpu/drm/radeon/radeon_fence.c
index 34356252567a..5b937dfe6f65 100644
--- a/drivers/gpu/drm/radeon/radeon_fence.c
+++ b/drivers/gpu/drm/radeon/radeon_fence.c
@@ -31,9 +31,9 @@
 #include <linux/seq_file.h>
 #include <linux/atomic.h>
 #include <linux/wait.h>
-#include <linux/list.h>
 #include <linux/kref.h>
 #include <linux/slab.h>
+#include <linux/firmware.h>
 #include <drm/drmP.h>
 #include "radeon_reg.h"
 #include "radeon.h"
@@ -768,7 +768,19 @@ int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring)
 	radeon_scratch_free(rdev, rdev->fence_drv[ring].scratch_reg);
 	if (rdev->wb.use_event || !radeon_ring_supports_scratch_reg(rdev, &rdev->ring[ring])) {
 		rdev->fence_drv[ring].scratch_reg = 0;
-		index = R600_WB_EVENT_OFFSET + ring * 4;
+		if (ring != R600_RING_TYPE_UVD_INDEX) {
+			index = R600_WB_EVENT_OFFSET + ring * 4;
+			rdev->fence_drv[ring].cpu_addr = &rdev->wb.wb[index/4];
+			rdev->fence_drv[ring].gpu_addr = rdev->wb.gpu_addr +
+							 index;
+
+		} else {
+			/* put fence directly behind firmware */
+			index = ALIGN(rdev->uvd_fw->size, 8);
+			rdev->fence_drv[ring].cpu_addr = rdev->uvd.cpu_addr + index;
+			rdev->fence_drv[ring].gpu_addr = rdev->uvd.gpu_addr + index;
+		}
+
 	} else {
 		r = radeon_scratch_get(rdev, &rdev->fence_drv[ring].scratch_reg);
 		if (r) {
@@ -778,9 +790,9 @@ int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring)
 		index = RADEON_WB_SCRATCH_OFFSET +
 			rdev->fence_drv[ring].scratch_reg -
 			rdev->scratch.reg_base;
+		rdev->fence_drv[ring].cpu_addr = &rdev->wb.wb[index/4];
+		rdev->fence_drv[ring].gpu_addr = rdev->wb.gpu_addr + index;
 	}
-	rdev->fence_drv[ring].cpu_addr = &rdev->wb.wb[index/4];
-	rdev->fence_drv[ring].gpu_addr = rdev->wb.gpu_addr + index;
 	radeon_fence_write(rdev, atomic64_read(&rdev->fence_drv[ring].last_seq), ring);
 	rdev->fence_drv[ring].initialized = true;
 	dev_info(rdev->dev, "fence driver on ring %d use gpu addr 0x%016llx and cpu addr 0x%p\n",
diff --git a/drivers/gpu/drm/radeon/radeon_gem.c b/drivers/gpu/drm/radeon/radeon_gem.c
index fe5c1f6b7957..aa796031ab65 100644
--- a/drivers/gpu/drm/radeon/radeon_gem.c
+++ b/drivers/gpu/drm/radeon/radeon_gem.c
@@ -84,6 +84,7 @@ retry:
 		return r;
 	}
 	*obj = &robj->gem_base;
+	robj->pid = task_pid_nr(current);
 
 	mutex_lock(&rdev->gem.mutex);
 	list_add_tail(&robj->list, &rdev->gem.objects);
@@ -575,3 +576,52 @@ int radeon_mode_dumb_destroy(struct drm_file *file_priv,
 {
 	return drm_gem_handle_delete(file_priv, handle);
 }
+
+#if defined(CONFIG_DEBUG_FS)
+static int radeon_debugfs_gem_info(struct seq_file *m, void *data)
+{
+	struct drm_info_node *node = (struct drm_info_node *)m->private;
+	struct drm_device *dev = node->minor->dev;
+	struct radeon_device *rdev = dev->dev_private;
+	struct radeon_bo *rbo;
+	unsigned i = 0;
+
+	mutex_lock(&rdev->gem.mutex);
+	list_for_each_entry(rbo, &rdev->gem.objects, list) {
+		unsigned domain;
+		const char *placement;
+
+		domain = radeon_mem_type_to_domain(rbo->tbo.mem.mem_type);
+		switch (domain) {
+		case RADEON_GEM_DOMAIN_VRAM:
+			placement = "VRAM";
+			break;
+		case RADEON_GEM_DOMAIN_GTT:
+			placement = " GTT";
+			break;
+		case RADEON_GEM_DOMAIN_CPU:
+		default:
+			placement = " CPU";
+			break;
+		}
+		seq_printf(m, "bo[0x%08x] %8ldkB %8ldMB %s pid %8ld\n",
+			   i, radeon_bo_size(rbo) >> 10, radeon_bo_size(rbo) >> 20,
+			   placement, (unsigned long)rbo->pid);
+		i++;
+	}
+	mutex_unlock(&rdev->gem.mutex);
+	return 0;
+}
+
+static struct drm_info_list radeon_debugfs_gem_list[] = {
+	{"radeon_gem_info", &radeon_debugfs_gem_info, 0, NULL},
+};
+#endif
+
+int radeon_gem_debugfs_init(struct radeon_device *rdev)
+{
+#if defined(CONFIG_DEBUG_FS)
+	return radeon_debugfs_add_files(rdev, radeon_debugfs_gem_list, 1);
+#endif
+	return 0;
+}
diff --git a/drivers/gpu/drm/radeon/radeon_kms.c b/drivers/gpu/drm/radeon/radeon_kms.c
index c75cb2c6ba71..4f2d4f4c1dab 100644
--- a/drivers/gpu/drm/radeon/radeon_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_kms.c
@@ -50,9 +50,13 @@ int radeon_driver_unload_kms(struct drm_device *dev)
 
 	if (rdev == NULL)
 		return 0;
+	if (rdev->rmmio == NULL)
+		goto done_free;
 	radeon_acpi_fini(rdev);
 	radeon_modeset_fini(rdev);
 	radeon_device_fini(rdev);
+
+done_free:
 	kfree(rdev);
 	dev->dev_private = NULL;
 	return 0;
@@ -176,80 +180,65 @@ int radeon_info_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 	struct radeon_device *rdev = dev->dev_private;
 	struct drm_radeon_info *info = data;
 	struct radeon_mode_info *minfo = &rdev->mode_info;
-	uint32_t value, *value_ptr;
-	uint64_t value64, *value_ptr64;
+	uint32_t *value, value_tmp, *value_ptr, value_size;
+	uint64_t value64;
 	struct drm_crtc *crtc;
 	int i, found;
 
-	/* TIMESTAMP is a 64-bit value, needs special handling. */
-	if (info->request == RADEON_INFO_TIMESTAMP) {
-		if (rdev->family >= CHIP_R600) {
-			value_ptr64 = (uint64_t*)((unsigned long)info->value);
-			value64 = radeon_get_gpu_clock_counter(rdev);
-
-			if (DRM_COPY_TO_USER(value_ptr64, &value64, sizeof(value64))) {
-				DRM_ERROR("copy_to_user %s:%u\n", __func__, __LINE__);
-				return -EFAULT;
-			}
-			return 0;
-		} else {
-			DRM_DEBUG_KMS("timestamp is r6xx+ only!\n");
-			return -EINVAL;
-		}
-	}
-
 	value_ptr = (uint32_t *)((unsigned long)info->value);
-	if (DRM_COPY_FROM_USER(&value, value_ptr, sizeof(value))) {
-		DRM_ERROR("copy_from_user %s:%u\n", __func__, __LINE__);
-		return -EFAULT;
-	}
+	value = &value_tmp;
+	value_size = sizeof(uint32_t);
 
 	switch (info->request) {
 	case RADEON_INFO_DEVICE_ID:
-		value = dev->pci_device;
+		*value = dev->pci_device;
 		break;
 	case RADEON_INFO_NUM_GB_PIPES:
-		value = rdev->num_gb_pipes;
+		*value = rdev->num_gb_pipes;
 		break;
 	case RADEON_INFO_NUM_Z_PIPES:
-		value = rdev->num_z_pipes;
+		*value = rdev->num_z_pipes;
 		break;
 	case RADEON_INFO_ACCEL_WORKING:
 		/* xf86-video-ati 6.13.0 relies on this being false for evergreen */
 		if ((rdev->family >= CHIP_CEDAR) && (rdev->family <= CHIP_HEMLOCK))
-			value = false;
+			*value = false;
 		else
-			value = rdev->accel_working;
+			*value = rdev->accel_working;
 		break;
 	case RADEON_INFO_CRTC_FROM_ID:
+		if (DRM_COPY_FROM_USER(value, value_ptr, sizeof(uint32_t))) {
+			DRM_ERROR("copy_from_user %s:%u\n", __func__, __LINE__);
+			return -EFAULT;
+		}
 		for (i = 0, found = 0; i < rdev->num_crtc; i++) {
 			crtc = (struct drm_crtc *)minfo->crtcs[i];
-			if (crtc && crtc->base.id == value) {
+			if (crtc && crtc->base.id == *value) {
 				struct radeon_crtc *radeon_crtc = to_radeon_crtc(crtc);
-				value = radeon_crtc->crtc_id;
+				*value = radeon_crtc->crtc_id;
 				found = 1;
 				break;
 			}
 		}
 		if (!found) {
-			DRM_DEBUG_KMS("unknown crtc id %d\n", value);
+			DRM_DEBUG_KMS("unknown crtc id %d\n", *value);
 			return -EINVAL;
 		}
 		break;
 	case RADEON_INFO_ACCEL_WORKING2:
-		value = rdev->accel_working;
+		*value = rdev->accel_working;
 		break;
 	case RADEON_INFO_TILING_CONFIG:
 		if (rdev->family >= CHIP_TAHITI)
-			value = rdev->config.si.tile_config;
+			*value = rdev->config.si.tile_config;
 		else if (rdev->family >= CHIP_CAYMAN)
-			value = rdev->config.cayman.tile_config;
+			*value = rdev->config.cayman.tile_config;
 		else if (rdev->family >= CHIP_CEDAR)
-			value = rdev->config.evergreen.tile_config;
+			*value = rdev->config.evergreen.tile_config;
 		else if (rdev->family >= CHIP_RV770)
-			value = rdev->config.rv770.tile_config;
+			*value = rdev->config.rv770.tile_config;
 		else if (rdev->family >= CHIP_R600)
-			value = rdev->config.r600.tile_config;
+			*value = rdev->config.r600.tile_config;
 		else {
 			DRM_DEBUG_KMS("tiling config is r6xx+ only!\n");
 			return -EINVAL;
@@ -262,73 +251,81 @@ int radeon_info_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 		 *
 		 * When returning, the value is 1 if filp owns hyper-z access,
 		 * 0 otherwise. */
-		if (value >= 2) {
-			DRM_DEBUG_KMS("WANT_HYPERZ: invalid value %d\n", value);
+		if (DRM_COPY_FROM_USER(value, value_ptr, sizeof(uint32_t))) {
+			DRM_ERROR("copy_from_user %s:%u\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+		if (*value >= 2) {
+			DRM_DEBUG_KMS("WANT_HYPERZ: invalid value %d\n", *value);
 			return -EINVAL;
 		}
-		radeon_set_filp_rights(dev, &rdev->hyperz_filp, filp, &value);
+		radeon_set_filp_rights(dev, &rdev->hyperz_filp, filp, value);
 		break;
 	case RADEON_INFO_WANT_CMASK:
 		/* The same logic as Hyper-Z. */
-		if (value >= 2) {
-			DRM_DEBUG_KMS("WANT_CMASK: invalid value %d\n", value);
+		if (DRM_COPY_FROM_USER(value, value_ptr, sizeof(uint32_t))) {
+			DRM_ERROR("copy_from_user %s:%u\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+		if (*value >= 2) {
+			DRM_DEBUG_KMS("WANT_CMASK: invalid value %d\n", *value);
 			return -EINVAL;
 		}
-		radeon_set_filp_rights(dev, &rdev->cmask_filp, filp, &value);
+		radeon_set_filp_rights(dev, &rdev->cmask_filp, filp, value);
 		break;
 	case RADEON_INFO_CLOCK_CRYSTAL_FREQ:
 		/* return clock value in KHz */
 		if (rdev->asic->get_xclk)
-			value = radeon_get_xclk(rdev) * 10;
+			*value = radeon_get_xclk(rdev) * 10;
 		else
-			value = rdev->clock.spll.reference_freq * 10;
+			*value = rdev->clock.spll.reference_freq * 10;
 		break;
 	case RADEON_INFO_NUM_BACKENDS:
 		if (rdev->family >= CHIP_TAHITI)
-			value = rdev->config.si.max_backends_per_se *
+			*value = rdev->config.si.max_backends_per_se *
 				rdev->config.si.max_shader_engines;
 		else if (rdev->family >= CHIP_CAYMAN)
-			value = rdev->config.cayman.max_backends_per_se *
+			*value = rdev->config.cayman.max_backends_per_se *
 				rdev->config.cayman.max_shader_engines;
 		else if (rdev->family >= CHIP_CEDAR)
-			value = rdev->config.evergreen.max_backends;
+			*value = rdev->config.evergreen.max_backends;
 		else if (rdev->family >= CHIP_RV770)
-			value = rdev->config.rv770.max_backends;
+			*value = rdev->config.rv770.max_backends;
 		else if (rdev->family >= CHIP_R600)
-			value = rdev->config.r600.max_backends;
+			*value = rdev->config.r600.max_backends;
 		else {
 			return -EINVAL;
 		}
 		break;
 	case RADEON_INFO_NUM_TILE_PIPES:
 		if (rdev->family >= CHIP_TAHITI)
-			value = rdev->config.si.max_tile_pipes;
+			*value = rdev->config.si.max_tile_pipes;
 		else if (rdev->family >= CHIP_CAYMAN)
-			value = rdev->config.cayman.max_tile_pipes;
+			*value = rdev->config.cayman.max_tile_pipes;
 		else if (rdev->family >= CHIP_CEDAR)
-			value = rdev->config.evergreen.max_tile_pipes;
+			*value = rdev->config.evergreen.max_tile_pipes;
 		else if (rdev->family >= CHIP_RV770)
-			value = rdev->config.rv770.max_tile_pipes;
+			*value = rdev->config.rv770.max_tile_pipes;
 		else if (rdev->family >= CHIP_R600)
-			value = rdev->config.r600.max_tile_pipes;
+			*value = rdev->config.r600.max_tile_pipes;
 		else {
 			return -EINVAL;
 		}
 		break;
 	case RADEON_INFO_FUSION_GART_WORKING:
-		value = 1;
+		*value = 1;
 		break;
 	case RADEON_INFO_BACKEND_MAP:
 		if (rdev->family >= CHIP_TAHITI)
-			value = rdev->config.si.backend_map;
+			*value = rdev->config.si.backend_map;
 		else if (rdev->family >= CHIP_CAYMAN)
-			value = rdev->config.cayman.backend_map;
+			*value = rdev->config.cayman.backend_map;
 		else if (rdev->family >= CHIP_CEDAR)
-			value = rdev->config.evergreen.backend_map;
+			*value = rdev->config.evergreen.backend_map;
 		else if (rdev->family >= CHIP_RV770)
-			value = rdev->config.rv770.backend_map;
+			*value = rdev->config.rv770.backend_map;
 		else if (rdev->family >= CHIP_R600)
-			value = rdev->config.r600.backend_map;
+			*value = rdev->config.r600.backend_map;
 		else {
 			return -EINVAL;
 		}
@@ -337,50 +334,91 @@ int radeon_info_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 		/* this is where we report if vm is supported or not */
 		if (rdev->family < CHIP_CAYMAN)
 			return -EINVAL;
-		value = RADEON_VA_RESERVED_SIZE;
+		*value = RADEON_VA_RESERVED_SIZE;
 		break;
 	case RADEON_INFO_IB_VM_MAX_SIZE:
 		/* this is where we report if vm is supported or not */
 		if (rdev->family < CHIP_CAYMAN)
 			return -EINVAL;
-		value = RADEON_IB_VM_MAX_SIZE;
+		*value = RADEON_IB_VM_MAX_SIZE;
 		break;
 	case RADEON_INFO_MAX_PIPES:
 		if (rdev->family >= CHIP_TAHITI)
-			value = rdev->config.si.max_cu_per_sh;
+			*value = rdev->config.si.max_cu_per_sh;
 		else if (rdev->family >= CHIP_CAYMAN)
-			value = rdev->config.cayman.max_pipes_per_simd;
+			*value = rdev->config.cayman.max_pipes_per_simd;
 		else if (rdev->family >= CHIP_CEDAR)
-			value = rdev->config.evergreen.max_pipes;
+			*value = rdev->config.evergreen.max_pipes;
 		else if (rdev->family >= CHIP_RV770)
-			value = rdev->config.rv770.max_pipes;
+			*value = rdev->config.rv770.max_pipes;
 		else if (rdev->family >= CHIP_R600)
-			value = rdev->config.r600.max_pipes;
+			*value = rdev->config.r600.max_pipes;
 		else {
 			return -EINVAL;
 		}
 		break;
+	case RADEON_INFO_TIMESTAMP:
+		if (rdev->family < CHIP_R600) {
+			DRM_DEBUG_KMS("timestamp is r6xx+ only!\n");
+			return -EINVAL;
+		}
+		value = (uint32_t*)&value64;
+		value_size = sizeof(uint64_t);
+		value64 = radeon_get_gpu_clock_counter(rdev);
+		break;
 	case RADEON_INFO_MAX_SE:
 		if (rdev->family >= CHIP_TAHITI)
-			value = rdev->config.si.max_shader_engines;
+			*value = rdev->config.si.max_shader_engines;
 		else if (rdev->family >= CHIP_CAYMAN)
-			value = rdev->config.cayman.max_shader_engines;
+			*value = rdev->config.cayman.max_shader_engines;
 		else if (rdev->family >= CHIP_CEDAR)
-			value = rdev->config.evergreen.num_ses;
+			*value = rdev->config.evergreen.num_ses;
 		else
-			value = 1;
+			*value = 1;
 		break;
 	case RADEON_INFO_MAX_SH_PER_SE:
 		if (rdev->family >= CHIP_TAHITI)
-			value = rdev->config.si.max_sh_per_se;
+			*value = rdev->config.si.max_sh_per_se;
 		else
 			return -EINVAL;
 		break;
+	case RADEON_INFO_FASTFB_WORKING:
+		*value = rdev->fastfb_working;
+		break;
+	case RADEON_INFO_RING_WORKING:
+		if (DRM_COPY_FROM_USER(value, value_ptr, sizeof(uint32_t))) {
+			DRM_ERROR("copy_from_user %s:%u\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+		switch (*value) {
+		case RADEON_CS_RING_GFX:
+		case RADEON_CS_RING_COMPUTE:
+			*value = rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ready;
+			break;
+		case RADEON_CS_RING_DMA:
+			*value = rdev->ring[R600_RING_TYPE_DMA_INDEX].ready;
+			*value |= rdev->ring[CAYMAN_RING_TYPE_DMA1_INDEX].ready;
+			break;
+		case RADEON_CS_RING_UVD:
+			*value = rdev->ring[R600_RING_TYPE_UVD_INDEX].ready;
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	case RADEON_INFO_SI_TILE_MODE_ARRAY:
+		if (rdev->family < CHIP_TAHITI) {
+			DRM_DEBUG_KMS("tile mode array is si only!\n");
+			return -EINVAL;
+		}
+		value = rdev->config.si.tile_mode_array;
+		value_size = sizeof(uint32_t)*32;
+		break;
 	default:
 		DRM_DEBUG_KMS("Invalid request %d\n", info->request);
 		return -EINVAL;
 	}
-	if (DRM_COPY_TO_USER(value_ptr, &value, sizeof(uint32_t))) {
+	if (DRM_COPY_TO_USER(value_ptr, (char*)value, value_size)) {
 		DRM_ERROR("copy_to_user %s:%u\n", __func__, __LINE__);
 		return -EFAULT;
 	}
@@ -513,6 +551,7 @@ void radeon_driver_preclose_kms(struct drm_device *dev,
 		rdev->hyperz_filp = NULL;
 	if (rdev->cmask_filp == file_priv)
 		rdev->cmask_filp = NULL;
+	radeon_uvd_free_handles(rdev, file_priv);
 }
 
 /*
diff --git a/drivers/gpu/drm/radeon/radeon_mode.h b/drivers/gpu/drm/radeon/radeon_mode.h
index 4003f5a68c09..44e579e75fd0 100644
--- a/drivers/gpu/drm/radeon/radeon_mode.h
+++ b/drivers/gpu/drm/radeon/radeon_mode.h
@@ -492,6 +492,29 @@ struct radeon_framebuffer {
 #define ENCODER_MODE_IS_DP(em) (((em) == ATOM_ENCODER_MODE_DP) || \
 				((em) == ATOM_ENCODER_MODE_DP_MST))
 
+struct atom_clock_dividers {
+	u32 post_div;
+	union {
+		struct {
+#ifdef __BIG_ENDIAN
+			u32 reserved : 6;
+			u32 whole_fb_div : 12;
+			u32 frac_fb_div : 14;
+#else
+			u32 frac_fb_div : 14;
+			u32 whole_fb_div : 12;
+			u32 reserved : 6;
+#endif
+		};
+		u32 fb_div;
+	};
+	u32 ref_div;
+	bool enable_post_div;
+	bool enable_dithen;
+	u32 vco_mode;
+	u32 real_clock;
+};
+
 extern enum radeon_tv_std
 radeon_combios_get_tv_info(struct radeon_device *rdev);
 extern enum radeon_tv_std
diff --git a/drivers/gpu/drm/radeon/radeon_object.c b/drivers/gpu/drm/radeon/radeon_object.c
index d3aface2d12d..1424ccde2377 100644
--- a/drivers/gpu/drm/radeon/radeon_object.c
+++ b/drivers/gpu/drm/radeon/radeon_object.c
@@ -321,8 +321,10 @@ void radeon_bo_force_delete(struct radeon_device *rdev)
 int radeon_bo_init(struct radeon_device *rdev)
 {
 	/* Add an MTRR for the VRAM */
-	rdev->mc.vram_mtrr = mtrr_add(rdev->mc.aper_base, rdev->mc.aper_size,
+	if (!rdev->fastfb_working) {
+		rdev->mc.vram_mtrr = mtrr_add(rdev->mc.aper_base, rdev->mc.aper_size,
 			MTRR_TYPE_WRCOMB, 1);
+	}
 	DRM_INFO("Detected VRAM RAM=%lluM, BAR=%lluM\n",
 		rdev->mc.mc_vram_size >> 20,
 		(unsigned long long)rdev->mc.aper_size >> 20);
@@ -339,14 +341,14 @@ void radeon_bo_fini(struct radeon_device *rdev)
 void radeon_bo_list_add_object(struct radeon_bo_list *lobj,
 				struct list_head *head)
 {
-	if (lobj->wdomain) {
+	if (lobj->written) {
 		list_add(&lobj->tv.head, head);
 	} else {
 		list_add_tail(&lobj->tv.head, head);
 	}
 }
 
-int radeon_bo_list_validate(struct list_head *head)
+int radeon_bo_list_validate(struct list_head *head, int ring)
 {
 	struct radeon_bo_list *lobj;
 	struct radeon_bo *bo;
@@ -360,15 +362,17 @@ int radeon_bo_list_validate(struct list_head *head)
 	list_for_each_entry(lobj, head, tv.head) {
 		bo = lobj->bo;
 		if (!bo->pin_count) {
-			domain = lobj->wdomain ? lobj->wdomain : lobj->rdomain;
+			domain = lobj->domain;
 			
 		retry:
 			radeon_ttm_placement_from_domain(bo, domain);
+			if (ring == R600_RING_TYPE_UVD_INDEX)
+				radeon_uvd_force_into_uvd_segment(bo);
 			r = ttm_bo_validate(&bo->tbo, &bo->placement,
 						true, false);
 			if (unlikely(r)) {
-				if (r != -ERESTARTSYS && domain == RADEON_GEM_DOMAIN_VRAM) {
-					domain |= RADEON_GEM_DOMAIN_GTT;
+				if (r != -ERESTARTSYS && domain != lobj->alt_domain) {
+					domain = lobj->alt_domain;
 					goto retry;
 				}
 				return r;
diff --git a/drivers/gpu/drm/radeon/radeon_object.h b/drivers/gpu/drm/radeon/radeon_object.h
index 5fc86b03043b..e2cb80a96b51 100644
--- a/drivers/gpu/drm/radeon/radeon_object.h
+++ b/drivers/gpu/drm/radeon/radeon_object.h
@@ -128,7 +128,7 @@ extern int radeon_bo_init(struct radeon_device *rdev);
 extern void radeon_bo_fini(struct radeon_device *rdev);
 extern void radeon_bo_list_add_object(struct radeon_bo_list *lobj,
 				struct list_head *head);
-extern int radeon_bo_list_validate(struct list_head *head);
+extern int radeon_bo_list_validate(struct list_head *head, int ring);
 extern int radeon_bo_fbdev_mmap(struct radeon_bo *bo,
 				struct vm_area_struct *vma);
 extern int radeon_bo_set_tiling_flags(struct radeon_bo *bo,
diff --git a/drivers/gpu/drm/radeon/radeon_pm.c b/drivers/gpu/drm/radeon/radeon_pm.c
index 338fd6a74e87..788c64cb4b47 100644
--- a/drivers/gpu/drm/radeon/radeon_pm.c
+++ b/drivers/gpu/drm/radeon/radeon_pm.c
@@ -843,7 +843,11 @@ static int radeon_debugfs_pm_info(struct seq_file *m, void *data)
 	struct radeon_device *rdev = dev->dev_private;
 
 	seq_printf(m, "default engine clock: %u0 kHz\n", rdev->pm.default_sclk);
-	seq_printf(m, "current engine clock: %u0 kHz\n", radeon_get_engine_clock(rdev));
+	/* radeon_get_engine_clock is not reliable on APUs so just print the current clock */
+	if ((rdev->family >= CHIP_PALM) && (rdev->flags & RADEON_IS_IGP))
+		seq_printf(m, "current engine clock: %u0 kHz\n", rdev->pm.current_sclk);
+	else
+		seq_printf(m, "current engine clock: %u0 kHz\n", radeon_get_engine_clock(rdev));
 	seq_printf(m, "default memory clock: %u0 kHz\n", rdev->pm.default_mclk);
 	if (rdev->asic->pm.get_memory_clock)
 		seq_printf(m, "current memory clock: %u0 kHz\n", radeon_get_memory_clock(rdev));
diff --git a/drivers/gpu/drm/radeon/radeon_ring.c b/drivers/gpu/drm/radeon/radeon_ring.c
index 8d58e268ff6d..e17faa7cf732 100644
--- a/drivers/gpu/drm/radeon/radeon_ring.c
+++ b/drivers/gpu/drm/radeon/radeon_ring.c
@@ -180,7 +180,8 @@ int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib,
 		radeon_semaphore_free(rdev, &ib->semaphore, NULL);
 	}
 	/* if we can't remember our last VM flush then flush now! */
-	if (ib->vm && !ib->vm->last_flush) {
+	/* XXX figure out why we have to flush for every IB */
+	if (ib->vm /*&& !ib->vm->last_flush*/) {
 		radeon_ring_vm_flush(rdev, ib->ring, ib->vm);
 	}
 	if (const_ib) {
@@ -368,7 +369,7 @@ void radeon_ring_free_size(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	u32 rptr;
 
-	if (rdev->wb.enabled)
+	if (rdev->wb.enabled && ring != &rdev->ring[R600_RING_TYPE_UVD_INDEX])
 		rptr = le32_to_cpu(rdev->wb.wb[ring->rptr_offs/4]);
 	else
 		rptr = RREG32(ring->rptr_reg);
@@ -821,18 +822,20 @@ static int radeon_debugfs_ring_info(struct seq_file *m, void *data)
 	return 0;
 }
 
-static int radeon_ring_type_gfx_index = RADEON_RING_TYPE_GFX_INDEX;
-static int cayman_ring_type_cp1_index = CAYMAN_RING_TYPE_CP1_INDEX;
-static int cayman_ring_type_cp2_index = CAYMAN_RING_TYPE_CP2_INDEX;
-static int radeon_ring_type_dma1_index = R600_RING_TYPE_DMA_INDEX;
-static int radeon_ring_type_dma2_index = CAYMAN_RING_TYPE_DMA1_INDEX;
+static int radeon_gfx_index = RADEON_RING_TYPE_GFX_INDEX;
+static int cayman_cp1_index = CAYMAN_RING_TYPE_CP1_INDEX;
+static int cayman_cp2_index = CAYMAN_RING_TYPE_CP2_INDEX;
+static int radeon_dma1_index = R600_RING_TYPE_DMA_INDEX;
+static int radeon_dma2_index = CAYMAN_RING_TYPE_DMA1_INDEX;
+static int r600_uvd_index = R600_RING_TYPE_UVD_INDEX;
 
 static struct drm_info_list radeon_debugfs_ring_info_list[] = {
-	{"radeon_ring_gfx", radeon_debugfs_ring_info, 0, &radeon_ring_type_gfx_index},
-	{"radeon_ring_cp1", radeon_debugfs_ring_info, 0, &cayman_ring_type_cp1_index},
-	{"radeon_ring_cp2", radeon_debugfs_ring_info, 0, &cayman_ring_type_cp2_index},
-	{"radeon_ring_dma1", radeon_debugfs_ring_info, 0, &radeon_ring_type_dma1_index},
-	{"radeon_ring_dma2", radeon_debugfs_ring_info, 0, &radeon_ring_type_dma2_index},
+	{"radeon_ring_gfx", radeon_debugfs_ring_info, 0, &radeon_gfx_index},
+	{"radeon_ring_cp1", radeon_debugfs_ring_info, 0, &cayman_cp1_index},
+	{"radeon_ring_cp2", radeon_debugfs_ring_info, 0, &cayman_cp2_index},
+	{"radeon_ring_dma1", radeon_debugfs_ring_info, 0, &radeon_dma1_index},
+	{"radeon_ring_dma2", radeon_debugfs_ring_info, 0, &radeon_dma2_index},
+	{"radeon_ring_uvd", radeon_debugfs_ring_info, 0, &r600_uvd_index},
 };
 
 static int radeon_debugfs_sa_info(struct seq_file *m, void *data)
diff --git a/drivers/gpu/drm/radeon/radeon_sa.c b/drivers/gpu/drm/radeon/radeon_sa.c
index cb800995d4f9..0abe5a9431bb 100644
--- a/drivers/gpu/drm/radeon/radeon_sa.c
+++ b/drivers/gpu/drm/radeon/radeon_sa.c
@@ -64,7 +64,7 @@ int radeon_sa_bo_manager_init(struct radeon_device *rdev,
 	}
 
 	r = radeon_bo_create(rdev, size, RADEON_GPU_PAGE_SIZE, true,
-			     RADEON_GEM_DOMAIN_CPU, NULL, &sa_manager->bo);
+			     domain, NULL, &sa_manager->bo);
 	if (r) {
 		dev_err(rdev->dev, "(%d) failed to allocate bo for manager\n", r);
 		return r;
diff --git a/drivers/gpu/drm/radeon/radeon_test.c b/drivers/gpu/drm/radeon/radeon_test.c
index fda09c9ea689..bbed4af8d0bc 100644
--- a/drivers/gpu/drm/radeon/radeon_test.c
+++ b/drivers/gpu/drm/radeon/radeon_test.c
@@ -252,6 +252,36 @@ void radeon_test_moves(struct radeon_device *rdev)
 		radeon_do_test_moves(rdev, RADEON_TEST_COPY_BLIT);
 }
 
+static int radeon_test_create_and_emit_fence(struct radeon_device *rdev,
+					     struct radeon_ring *ring,
+					     struct radeon_fence **fence)
+{
+	int r;
+
+	if (ring->idx == R600_RING_TYPE_UVD_INDEX) {
+		r = radeon_uvd_get_create_msg(rdev, ring->idx, 1, NULL);
+		if (r) {
+			DRM_ERROR("Failed to get dummy create msg\n");
+			return r;
+		}
+
+		r = radeon_uvd_get_destroy_msg(rdev, ring->idx, 1, fence);
+		if (r) {
+			DRM_ERROR("Failed to get dummy destroy msg\n");
+			return r;
+		}
+	} else {
+		r = radeon_ring_lock(rdev, ring, 64);
+		if (r) {
+			DRM_ERROR("Failed to lock ring A %d\n", ring->idx);
+			return r;
+		}
+		radeon_fence_emit(rdev, fence, ring->idx);
+		radeon_ring_unlock_commit(rdev, ring);
+	}
+	return 0;
+}
+
 void radeon_test_ring_sync(struct radeon_device *rdev,
 			   struct radeon_ring *ringA,
 			   struct radeon_ring *ringB)
@@ -272,21 +302,24 @@ void radeon_test_ring_sync(struct radeon_device *rdev,
 		goto out_cleanup;
 	}
 	radeon_semaphore_emit_wait(rdev, ringA->idx, semaphore);
-	r = radeon_fence_emit(rdev, &fence1, ringA->idx);
-	if (r) {
-		DRM_ERROR("Failed to emit fence 1\n");
-		radeon_ring_unlock_undo(rdev, ringA);
+	radeon_ring_unlock_commit(rdev, ringA);
+
+	r = radeon_test_create_and_emit_fence(rdev, ringA, &fence1);
+	if (r)
 		goto out_cleanup;
-	}
-	radeon_semaphore_emit_wait(rdev, ringA->idx, semaphore);
-	r = radeon_fence_emit(rdev, &fence2, ringA->idx);
+
+	r = radeon_ring_lock(rdev, ringA, 64);
 	if (r) {
-		DRM_ERROR("Failed to emit fence 2\n");
-		radeon_ring_unlock_undo(rdev, ringA);
+		DRM_ERROR("Failed to lock ring A %d\n", ringA->idx);
 		goto out_cleanup;
 	}
+	radeon_semaphore_emit_wait(rdev, ringA->idx, semaphore);
 	radeon_ring_unlock_commit(rdev, ringA);
 
+	r = radeon_test_create_and_emit_fence(rdev, ringA, &fence2);
+	if (r)
+		goto out_cleanup;
+
 	mdelay(1000);
 
 	if (radeon_fence_signaled(fence1)) {
@@ -364,27 +397,22 @@ static void radeon_test_ring_sync2(struct radeon_device *rdev,
 		goto out_cleanup;
 	}
 	radeon_semaphore_emit_wait(rdev, ringA->idx, semaphore);
-	r = radeon_fence_emit(rdev, &fenceA, ringA->idx);
-	if (r) {
-		DRM_ERROR("Failed to emit sync fence 1\n");
-		radeon_ring_unlock_undo(rdev, ringA);
-		goto out_cleanup;
-	}
 	radeon_ring_unlock_commit(rdev, ringA);
 
+	r = radeon_test_create_and_emit_fence(rdev, ringA, &fenceA);
+	if (r)
+		goto out_cleanup;
+
 	r = radeon_ring_lock(rdev, ringB, 64);
 	if (r) {
 		DRM_ERROR("Failed to lock ring B %d\n", ringB->idx);
 		goto out_cleanup;
 	}
 	radeon_semaphore_emit_wait(rdev, ringB->idx, semaphore);
-	r = radeon_fence_emit(rdev, &fenceB, ringB->idx);
-	if (r) {
-		DRM_ERROR("Failed to create sync fence 2\n");
-		radeon_ring_unlock_undo(rdev, ringB);
-		goto out_cleanup;
-	}
 	radeon_ring_unlock_commit(rdev, ringB);
+	r = radeon_test_create_and_emit_fence(rdev, ringB, &fenceB);
+	if (r)
+		goto out_cleanup;
 
 	mdelay(1000);
 
@@ -393,7 +421,7 @@ static void radeon_test_ring_sync2(struct radeon_device *rdev,
 		goto out_cleanup;
 	}
 	if (radeon_fence_signaled(fenceB)) {
-		DRM_ERROR("Fence A signaled without waiting for semaphore.\n");
+		DRM_ERROR("Fence B signaled without waiting for semaphore.\n");
 		goto out_cleanup;
 	}
 
diff --git a/drivers/gpu/drm/radeon/radeon_uvd.c b/drivers/gpu/drm/radeon/radeon_uvd.c
new file mode 100644
index 000000000000..906e5c0ca3b9
--- /dev/null
+++ b/drivers/gpu/drm/radeon/radeon_uvd.c
@@ -0,0 +1,831 @@
+/*
+ * Copyright 2011 Advanced Micro Devices, Inc.
+ * All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+/*
+ * Authors:
+ *    Christian König <deathsimple@vodafone.de>
+ */
+
+#include <linux/firmware.h>
+#include <linux/module.h>
+#include <drm/drmP.h>
+#include <drm/drm.h>
+
+#include "radeon.h"
+#include "r600d.h"
+
+/* 1 second timeout */
+#define UVD_IDLE_TIMEOUT_MS	1000
+
+/* Firmware Names */
+#define FIRMWARE_RV710		"radeon/RV710_uvd.bin"
+#define FIRMWARE_CYPRESS	"radeon/CYPRESS_uvd.bin"
+#define FIRMWARE_SUMO		"radeon/SUMO_uvd.bin"
+#define FIRMWARE_TAHITI		"radeon/TAHITI_uvd.bin"
+
+MODULE_FIRMWARE(FIRMWARE_RV710);
+MODULE_FIRMWARE(FIRMWARE_CYPRESS);
+MODULE_FIRMWARE(FIRMWARE_SUMO);
+MODULE_FIRMWARE(FIRMWARE_TAHITI);
+
+static void radeon_uvd_idle_work_handler(struct work_struct *work);
+
+int radeon_uvd_init(struct radeon_device *rdev)
+{
+	struct platform_device *pdev;
+	unsigned long bo_size;
+	const char *fw_name;
+	int i, r;
+
+	INIT_DELAYED_WORK(&rdev->uvd.idle_work, radeon_uvd_idle_work_handler);
+
+	pdev = platform_device_register_simple("radeon_uvd", 0, NULL, 0);
+	r = IS_ERR(pdev);
+	if (r) {
+		dev_err(rdev->dev, "radeon_uvd: Failed to register firmware\n");
+		return -EINVAL;
+	}
+
+	switch (rdev->family) {
+	case CHIP_RV710:
+	case CHIP_RV730:
+	case CHIP_RV740:
+		fw_name = FIRMWARE_RV710;
+		break;
+
+	case CHIP_CYPRESS:
+	case CHIP_HEMLOCK:
+	case CHIP_JUNIPER:
+	case CHIP_REDWOOD:
+	case CHIP_CEDAR:
+		fw_name = FIRMWARE_CYPRESS;
+		break;
+
+	case CHIP_SUMO:
+	case CHIP_SUMO2:
+	case CHIP_PALM:
+	case CHIP_CAYMAN:
+	case CHIP_BARTS:
+	case CHIP_TURKS:
+	case CHIP_CAICOS:
+		fw_name = FIRMWARE_SUMO;
+		break;
+
+	case CHIP_TAHITI:
+	case CHIP_VERDE:
+	case CHIP_PITCAIRN:
+	case CHIP_ARUBA:
+		fw_name = FIRMWARE_TAHITI;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	r = request_firmware(&rdev->uvd_fw, fw_name, &pdev->dev);
+	if (r) {
+		dev_err(rdev->dev, "radeon_uvd: Can't load firmware \"%s\"\n",
+			fw_name);
+		platform_device_unregister(pdev);
+		return r;
+	}
+
+	platform_device_unregister(pdev);
+
+	bo_size = RADEON_GPU_PAGE_ALIGN(rdev->uvd_fw->size + 8) +
+		  RADEON_UVD_STACK_SIZE + RADEON_UVD_HEAP_SIZE;
+	r = radeon_bo_create(rdev, bo_size, PAGE_SIZE, true,
+			     RADEON_GEM_DOMAIN_VRAM, NULL, &rdev->uvd.vcpu_bo);
+	if (r) {
+		dev_err(rdev->dev, "(%d) failed to allocate UVD bo\n", r);
+		return r;
+	}
+
+	r = radeon_uvd_resume(rdev);
+	if (r)
+		return r;
+
+	memset(rdev->uvd.cpu_addr, 0, bo_size);
+	memcpy(rdev->uvd.cpu_addr, rdev->uvd_fw->data, rdev->uvd_fw->size);
+
+	r = radeon_uvd_suspend(rdev);
+	if (r)
+		return r;
+
+	for (i = 0; i < RADEON_MAX_UVD_HANDLES; ++i) {
+		atomic_set(&rdev->uvd.handles[i], 0);
+		rdev->uvd.filp[i] = NULL;
+	}
+
+	return 0;
+}
+
+void radeon_uvd_fini(struct radeon_device *rdev)
+{
+	radeon_uvd_suspend(rdev);
+	radeon_bo_unref(&rdev->uvd.vcpu_bo);
+}
+
+int radeon_uvd_suspend(struct radeon_device *rdev)
+{
+	int r;
+
+	if (rdev->uvd.vcpu_bo == NULL)
+		return 0;
+
+	r = radeon_bo_reserve(rdev->uvd.vcpu_bo, false);
+	if (!r) {
+		radeon_bo_kunmap(rdev->uvd.vcpu_bo);
+		radeon_bo_unpin(rdev->uvd.vcpu_bo);
+		radeon_bo_unreserve(rdev->uvd.vcpu_bo);
+	}
+	return r;
+}
+
+int radeon_uvd_resume(struct radeon_device *rdev)
+{
+	int r;
+
+	if (rdev->uvd.vcpu_bo == NULL)
+		return -EINVAL;
+
+	r = radeon_bo_reserve(rdev->uvd.vcpu_bo, false);
+	if (r) {
+		radeon_bo_unref(&rdev->uvd.vcpu_bo);
+		dev_err(rdev->dev, "(%d) failed to reserve UVD bo\n", r);
+		return r;
+	}
+
+	r = radeon_bo_pin(rdev->uvd.vcpu_bo, RADEON_GEM_DOMAIN_VRAM,
+			  &rdev->uvd.gpu_addr);
+	if (r) {
+		radeon_bo_unreserve(rdev->uvd.vcpu_bo);
+		radeon_bo_unref(&rdev->uvd.vcpu_bo);
+		dev_err(rdev->dev, "(%d) UVD bo pin failed\n", r);
+		return r;
+	}
+
+	r = radeon_bo_kmap(rdev->uvd.vcpu_bo, &rdev->uvd.cpu_addr);
+	if (r) {
+		dev_err(rdev->dev, "(%d) UVD map failed\n", r);
+		return r;
+	}
+
+	radeon_bo_unreserve(rdev->uvd.vcpu_bo);
+
+	return 0;
+}
+
+void radeon_uvd_force_into_uvd_segment(struct radeon_bo *rbo)
+{
+	rbo->placement.fpfn = 0 >> PAGE_SHIFT;
+	rbo->placement.lpfn = (256 * 1024 * 1024) >> PAGE_SHIFT;
+}
+
+void radeon_uvd_free_handles(struct radeon_device *rdev, struct drm_file *filp)
+{
+	int i, r;
+	for (i = 0; i < RADEON_MAX_UVD_HANDLES; ++i) {
+		if (rdev->uvd.filp[i] == filp) {
+			uint32_t handle = atomic_read(&rdev->uvd.handles[i]);
+			struct radeon_fence *fence;
+
+			r = radeon_uvd_get_destroy_msg(rdev,
+				R600_RING_TYPE_UVD_INDEX, handle, &fence);
+			if (r) {
+				DRM_ERROR("Error destroying UVD (%d)!\n", r);
+				continue;
+			}
+
+			radeon_fence_wait(fence, false);
+			radeon_fence_unref(&fence);
+
+			rdev->uvd.filp[i] = NULL;
+			atomic_set(&rdev->uvd.handles[i], 0);
+		}
+	}
+}
+
+static int radeon_uvd_cs_msg_decode(uint32_t *msg, unsigned buf_sizes[])
+{
+	unsigned stream_type = msg[4];
+	unsigned width = msg[6];
+	unsigned height = msg[7];
+	unsigned dpb_size = msg[9];
+	unsigned pitch = msg[28];
+
+	unsigned width_in_mb = width / 16;
+	unsigned height_in_mb = ALIGN(height / 16, 2);
+
+	unsigned image_size, tmp, min_dpb_size;
+
+	image_size = width * height;
+	image_size += image_size / 2;
+	image_size = ALIGN(image_size, 1024);
+
+	switch (stream_type) {
+	case 0: /* H264 */
+
+		/* reference picture buffer */
+		min_dpb_size = image_size * 17;
+
+		/* macroblock context buffer */
+		min_dpb_size += width_in_mb * height_in_mb * 17 * 192;
+
+		/* IT surface buffer */
+		min_dpb_size += width_in_mb * height_in_mb * 32;
+		break;
+
+	case 1: /* VC1 */
+
+		/* reference picture buffer */
+		min_dpb_size = image_size * 3;
+
+		/* CONTEXT_BUFFER */
+		min_dpb_size += width_in_mb * height_in_mb * 128;
+
+		/* IT surface buffer */
+		min_dpb_size += width_in_mb * 64;
+
+		/* DB surface buffer */
+		min_dpb_size += width_in_mb * 128;
+
+		/* BP */
+		tmp = max(width_in_mb, height_in_mb);
+		min_dpb_size += ALIGN(tmp * 7 * 16, 64);
+		break;
+
+	case 3: /* MPEG2 */
+
+		/* reference picture buffer */
+		min_dpb_size = image_size * 3;
+		break;
+
+	case 4: /* MPEG4 */
+
+		/* reference picture buffer */
+		min_dpb_size = image_size * 3;
+
+		/* CM */
+		min_dpb_size += width_in_mb * height_in_mb * 64;
+
+		/* IT surface buffer */
+		min_dpb_size += ALIGN(width_in_mb * height_in_mb * 32, 64);
+		break;
+
+	default:
+		DRM_ERROR("UVD codec not handled %d!\n", stream_type);
+		return -EINVAL;
+	}
+
+	if (width > pitch) {
+		DRM_ERROR("Invalid UVD decoding target pitch!\n");
+		return -EINVAL;
+	}
+
+	if (dpb_size < min_dpb_size) {
+		DRM_ERROR("Invalid dpb_size in UVD message (%d / %d)!\n",
+			  dpb_size, min_dpb_size);
+		return -EINVAL;
+	}
+
+	buf_sizes[0x1] = dpb_size;
+	buf_sizes[0x2] = image_size;
+	return 0;
+}
+
+static int radeon_uvd_cs_msg(struct radeon_cs_parser *p, struct radeon_bo *bo,
+			     unsigned offset, unsigned buf_sizes[])
+{
+	int32_t *msg, msg_type, handle;
+	void *ptr;
+
+	int i, r;
+
+	if (offset & 0x3F) {
+		DRM_ERROR("UVD messages must be 64 byte aligned!\n");
+		return -EINVAL;
+	}
+
+	r = radeon_bo_kmap(bo, &ptr);
+	if (r)
+		return r;
+
+	msg = ptr + offset;
+
+	msg_type = msg[1];
+	handle = msg[2];
+
+	if (handle == 0) {
+		DRM_ERROR("Invalid UVD handle!\n");
+		return -EINVAL;
+	}
+
+	if (msg_type == 1) {
+		/* it's a decode msg, calc buffer sizes */
+		r = radeon_uvd_cs_msg_decode(msg, buf_sizes);
+		radeon_bo_kunmap(bo);
+		if (r)
+			return r;
+
+	} else if (msg_type == 2) {
+		/* it's a destroy msg, free the handle */
+		for (i = 0; i < RADEON_MAX_UVD_HANDLES; ++i)
+			atomic_cmpxchg(&p->rdev->uvd.handles[i], handle, 0);
+		radeon_bo_kunmap(bo);
+		return 0;
+	} else {
+		/* it's a create msg, no special handling needed */
+		radeon_bo_kunmap(bo);
+	}
+
+	/* create or decode, validate the handle */
+	for (i = 0; i < RADEON_MAX_UVD_HANDLES; ++i) {
+		if (atomic_read(&p->rdev->uvd.handles[i]) == handle)
+			return 0;
+	}
+
+	/* handle not found try to alloc a new one */
+	for (i = 0; i < RADEON_MAX_UVD_HANDLES; ++i) {
+		if (!atomic_cmpxchg(&p->rdev->uvd.handles[i], 0, handle)) {
+			p->rdev->uvd.filp[i] = p->filp;
+			return 0;
+		}
+	}
+
+	DRM_ERROR("No more free UVD handles!\n");
+	return -EINVAL;
+}
+
+static int radeon_uvd_cs_reloc(struct radeon_cs_parser *p,
+			       int data0, int data1,
+			       unsigned buf_sizes[])
+{
+	struct radeon_cs_chunk *relocs_chunk;
+	struct radeon_cs_reloc *reloc;
+	unsigned idx, cmd, offset;
+	uint64_t start, end;
+	int r;
+
+	relocs_chunk = &p->chunks[p->chunk_relocs_idx];
+	offset = radeon_get_ib_value(p, data0);
+	idx = radeon_get_ib_value(p, data1);
+	if (idx >= relocs_chunk->length_dw) {
+		DRM_ERROR("Relocs at %d after relocations chunk end %d !\n",
+			  idx, relocs_chunk->length_dw);
+		return -EINVAL;
+	}
+
+	reloc = p->relocs_ptr[(idx / 4)];
+	start = reloc->lobj.gpu_offset;
+	end = start + radeon_bo_size(reloc->robj);
+	start += offset;
+
+	p->ib.ptr[data0] = start & 0xFFFFFFFF;
+	p->ib.ptr[data1] = start >> 32;
+
+	cmd = radeon_get_ib_value(p, p->idx) >> 1;
+
+	if (cmd < 0x4) {
+		if ((end - start) < buf_sizes[cmd]) {
+			DRM_ERROR("buffer to small (%d / %d)!\n",
+				  (unsigned)(end - start), buf_sizes[cmd]);
+			return -EINVAL;
+		}
+
+	} else if (cmd != 0x100) {
+		DRM_ERROR("invalid UVD command %X!\n", cmd);
+		return -EINVAL;
+	}
+
+	if ((start >> 28) != (end >> 28)) {
+		DRM_ERROR("reloc %LX-%LX crossing 256MB boundary!\n",
+			  start, end);
+		return -EINVAL;
+	}
+
+	/* TODO: is this still necessary on NI+ ? */
+	if ((cmd == 0 || cmd == 0x3) &&
+	    (start >> 28) != (p->rdev->uvd.gpu_addr >> 28)) {
+		DRM_ERROR("msg/fb buffer %LX-%LX out of 256MB segment!\n",
+			  start, end);
+		return -EINVAL;
+	}
+
+	if (cmd == 0) {
+		r = radeon_uvd_cs_msg(p, reloc->robj, offset, buf_sizes);
+		if (r)
+			return r;
+	}
+
+	return 0;
+}
+
+static int radeon_uvd_cs_reg(struct radeon_cs_parser *p,
+			     struct radeon_cs_packet *pkt,
+			     int *data0, int *data1,
+			     unsigned buf_sizes[])
+{
+	int i, r;
+
+	p->idx++;
+	for (i = 0; i <= pkt->count; ++i) {
+		switch (pkt->reg + i*4) {
+		case UVD_GPCOM_VCPU_DATA0:
+			*data0 = p->idx;
+			break;
+		case UVD_GPCOM_VCPU_DATA1:
+			*data1 = p->idx;
+			break;
+		case UVD_GPCOM_VCPU_CMD:
+			r = radeon_uvd_cs_reloc(p, *data0, *data1, buf_sizes);
+			if (r)
+				return r;
+			break;
+		case UVD_ENGINE_CNTL:
+			break;
+		default:
+			DRM_ERROR("Invalid reg 0x%X!\n",
+				  pkt->reg + i*4);
+			return -EINVAL;
+		}
+		p->idx++;
+	}
+	return 0;
+}
+
+int radeon_uvd_cs_parse(struct radeon_cs_parser *p)
+{
+	struct radeon_cs_packet pkt;
+	int r, data0 = 0, data1 = 0;
+
+	/* minimum buffer sizes */
+	unsigned buf_sizes[] = {
+		[0x00000000]	=	2048,
+		[0x00000001]	=	32 * 1024 * 1024,
+		[0x00000002]	=	2048 * 1152 * 3,
+		[0x00000003]	=	2048,
+	};
+
+	if (p->chunks[p->chunk_ib_idx].length_dw % 16) {
+		DRM_ERROR("UVD IB length (%d) not 16 dwords aligned!\n",
+			  p->chunks[p->chunk_ib_idx].length_dw);
+		return -EINVAL;
+	}
+
+	if (p->chunk_relocs_idx == -1) {
+		DRM_ERROR("No relocation chunk !\n");
+		return -EINVAL;
+	}
+
+
+	do {
+		r = radeon_cs_packet_parse(p, &pkt, p->idx);
+		if (r)
+			return r;
+		switch (pkt.type) {
+		case RADEON_PACKET_TYPE0:
+			r = radeon_uvd_cs_reg(p, &pkt, &data0,
+					      &data1, buf_sizes);
+			if (r)
+				return r;
+			break;
+		case RADEON_PACKET_TYPE2:
+			p->idx += pkt.count + 2;
+			break;
+		default:
+			DRM_ERROR("Unknown packet type %d !\n", pkt.type);
+			return -EINVAL;
+		}
+	} while (p->idx < p->chunks[p->chunk_ib_idx].length_dw);
+	return 0;
+}
+
+static int radeon_uvd_send_msg(struct radeon_device *rdev,
+			       int ring, struct radeon_bo *bo,
+			       struct radeon_fence **fence)
+{
+	struct ttm_validate_buffer tv;
+	struct list_head head;
+	struct radeon_ib ib;
+	uint64_t addr;
+	int i, r;
+
+	memset(&tv, 0, sizeof(tv));
+	tv.bo = &bo->tbo;
+
+	INIT_LIST_HEAD(&head);
+	list_add(&tv.head, &head);
+
+	r = ttm_eu_reserve_buffers(&head);
+	if (r)
+		return r;
+
+	radeon_ttm_placement_from_domain(bo, RADEON_GEM_DOMAIN_VRAM);
+	radeon_uvd_force_into_uvd_segment(bo);
+
+	r = ttm_bo_validate(&bo->tbo, &bo->placement, true, false);
+	if (r) {
+		ttm_eu_backoff_reservation(&head);
+		return r;
+	}
+
+	r = radeon_ib_get(rdev, ring, &ib, NULL, 16);
+	if (r) {
+		ttm_eu_backoff_reservation(&head);
+		return r;
+	}
+
+	addr = radeon_bo_gpu_offset(bo);
+	ib.ptr[0] = PACKET0(UVD_GPCOM_VCPU_DATA0, 0);
+	ib.ptr[1] = addr;
+	ib.ptr[2] = PACKET0(UVD_GPCOM_VCPU_DATA1, 0);
+	ib.ptr[3] = addr >> 32;
+	ib.ptr[4] = PACKET0(UVD_GPCOM_VCPU_CMD, 0);
+	ib.ptr[5] = 0;
+	for (i = 6; i < 16; ++i)
+		ib.ptr[i] = PACKET2(0);
+	ib.length_dw = 16;
+
+	r = radeon_ib_schedule(rdev, &ib, NULL);
+	if (r) {
+		ttm_eu_backoff_reservation(&head);
+		return r;
+	}
+	ttm_eu_fence_buffer_objects(&head, ib.fence);
+
+	if (fence)
+		*fence = radeon_fence_ref(ib.fence);
+
+	radeon_ib_free(rdev, &ib);
+	radeon_bo_unref(&bo);
+	return 0;
+}
+
+/* multiple fence commands without any stream commands in between can
+   crash the vcpu so just try to emmit a dummy create/destroy msg to
+   avoid this */
+int radeon_uvd_get_create_msg(struct radeon_device *rdev, int ring,
+			      uint32_t handle, struct radeon_fence **fence)
+{
+	struct radeon_bo *bo;
+	uint32_t *msg;
+	int r, i;
+
+	r = radeon_bo_create(rdev, 1024, PAGE_SIZE, true,
+			     RADEON_GEM_DOMAIN_VRAM, NULL, &bo);
+	if (r)
+		return r;
+
+	r = radeon_bo_reserve(bo, false);
+	if (r) {
+		radeon_bo_unref(&bo);
+		return r;
+	}
+
+	r = radeon_bo_kmap(bo, (void **)&msg);
+	if (r) {
+		radeon_bo_unreserve(bo);
+		radeon_bo_unref(&bo);
+		return r;
+	}
+
+	/* stitch together an UVD create msg */
+	msg[0] = 0x00000de4;
+	msg[1] = 0x00000000;
+	msg[2] = handle;
+	msg[3] = 0x00000000;
+	msg[4] = 0x00000000;
+	msg[5] = 0x00000000;
+	msg[6] = 0x00000000;
+	msg[7] = 0x00000780;
+	msg[8] = 0x00000440;
+	msg[9] = 0x00000000;
+	msg[10] = 0x01b37000;
+	for (i = 11; i < 1024; ++i)
+		msg[i] = 0x0;
+
+	radeon_bo_kunmap(bo);
+	radeon_bo_unreserve(bo);
+
+	return radeon_uvd_send_msg(rdev, ring, bo, fence);
+}
+
+int radeon_uvd_get_destroy_msg(struct radeon_device *rdev, int ring,
+			       uint32_t handle, struct radeon_fence **fence)
+{
+	struct radeon_bo *bo;
+	uint32_t *msg;
+	int r, i;
+
+	r = radeon_bo_create(rdev, 1024, PAGE_SIZE, true,
+			     RADEON_GEM_DOMAIN_VRAM, NULL, &bo);
+	if (r)
+		return r;
+
+	r = radeon_bo_reserve(bo, false);
+	if (r) {
+		radeon_bo_unref(&bo);
+		return r;
+	}
+
+	r = radeon_bo_kmap(bo, (void **)&msg);
+	if (r) {
+		radeon_bo_unreserve(bo);
+		radeon_bo_unref(&bo);
+		return r;
+	}
+
+	/* stitch together an UVD destroy msg */
+	msg[0] = 0x00000de4;
+	msg[1] = 0x00000002;
+	msg[2] = handle;
+	msg[3] = 0x00000000;
+	for (i = 4; i < 1024; ++i)
+		msg[i] = 0x0;
+
+	radeon_bo_kunmap(bo);
+	radeon_bo_unreserve(bo);
+
+	return radeon_uvd_send_msg(rdev, ring, bo, fence);
+}
+
+static void radeon_uvd_idle_work_handler(struct work_struct *work)
+{
+	struct radeon_device *rdev =
+		container_of(work, struct radeon_device, uvd.idle_work.work);
+
+	if (radeon_fence_count_emitted(rdev, R600_RING_TYPE_UVD_INDEX) == 0)
+		radeon_set_uvd_clocks(rdev, 0, 0);
+	else
+		schedule_delayed_work(&rdev->uvd.idle_work,
+				      msecs_to_jiffies(UVD_IDLE_TIMEOUT_MS));
+}
+
+void radeon_uvd_note_usage(struct radeon_device *rdev)
+{
+	bool set_clocks = !cancel_delayed_work_sync(&rdev->uvd.idle_work);
+	set_clocks &= schedule_delayed_work(&rdev->uvd.idle_work,
+					    msecs_to_jiffies(UVD_IDLE_TIMEOUT_MS));
+	if (set_clocks)
+		radeon_set_uvd_clocks(rdev, 53300, 40000);
+}
+
+static unsigned radeon_uvd_calc_upll_post_div(unsigned vco_freq,
+					      unsigned target_freq,
+					      unsigned pd_min,
+					      unsigned pd_even)
+{
+	unsigned post_div = vco_freq / target_freq;
+
+	/* adjust to post divider minimum value */
+	if (post_div < pd_min)
+		post_div = pd_min;
+
+	/* we alway need a frequency less than or equal the target */
+	if ((vco_freq / post_div) > target_freq)
+		post_div += 1;
+
+	/* post dividers above a certain value must be even */
+	if (post_div > pd_even && post_div % 2)
+		post_div += 1;
+
+	return post_div;
+}
+
+/**
+ * radeon_uvd_calc_upll_dividers - calc UPLL clock dividers
+ *
+ * @rdev: radeon_device pointer
+ * @vclk: wanted VCLK
+ * @dclk: wanted DCLK
+ * @vco_min: minimum VCO frequency
+ * @vco_max: maximum VCO frequency
+ * @fb_factor: factor to multiply vco freq with
+ * @fb_mask: limit and bitmask for feedback divider
+ * @pd_min: post divider minimum
+ * @pd_max: post divider maximum
+ * @pd_even: post divider must be even above this value
+ * @optimal_fb_div: resulting feedback divider
+ * @optimal_vclk_div: resulting vclk post divider
+ * @optimal_dclk_div: resulting dclk post divider
+ *
+ * Calculate dividers for UVDs UPLL (R6xx-SI, except APUs).
+ * Returns zero on success -EINVAL on error.
+ */
+int radeon_uvd_calc_upll_dividers(struct radeon_device *rdev,
+				  unsigned vclk, unsigned dclk,
+				  unsigned vco_min, unsigned vco_max,
+				  unsigned fb_factor, unsigned fb_mask,
+				  unsigned pd_min, unsigned pd_max,
+				  unsigned pd_even,
+				  unsigned *optimal_fb_div,
+				  unsigned *optimal_vclk_div,
+				  unsigned *optimal_dclk_div)
+{
+	unsigned vco_freq, ref_freq = rdev->clock.spll.reference_freq;
+
+	/* start off with something large */
+	unsigned optimal_score = ~0;
+
+	/* loop through vco from low to high */
+	vco_min = max(max(vco_min, vclk), dclk);
+	for (vco_freq = vco_min; vco_freq <= vco_max; vco_freq += 100) {
+
+		uint64_t fb_div = (uint64_t)vco_freq * fb_factor;
+		unsigned vclk_div, dclk_div, score;
+
+		do_div(fb_div, ref_freq);
+
+		/* fb div out of range ? */
+		if (fb_div > fb_mask)
+			break; /* it can oly get worse */
+
+		fb_div &= fb_mask;
+
+		/* calc vclk divider with current vco freq */
+		vclk_div = radeon_uvd_calc_upll_post_div(vco_freq, vclk,
+							 pd_min, pd_even);
+		if (vclk_div > pd_max)
+			break; /* vco is too big, it has to stop */
+
+		/* calc dclk divider with current vco freq */
+		dclk_div = radeon_uvd_calc_upll_post_div(vco_freq, dclk,
+							 pd_min, pd_even);
+		if (vclk_div > pd_max)
+			break; /* vco is too big, it has to stop */
+
+		/* calc score with current vco freq */
+		score = vclk - (vco_freq / vclk_div) + dclk - (vco_freq / dclk_div);
+
+		/* determine if this vco setting is better than current optimal settings */
+		if (score < optimal_score) {
+			*optimal_fb_div = fb_div;
+			*optimal_vclk_div = vclk_div;
+			*optimal_dclk_div = dclk_div;
+			optimal_score = score;
+			if (optimal_score == 0)
+				break; /* it can't get better than this */
+		}
+	}
+
+	/* did we found a valid setup ? */
+	if (optimal_score == ~0)
+		return -EINVAL;
+
+	return 0;
+}
+
+int radeon_uvd_send_upll_ctlreq(struct radeon_device *rdev,
+				unsigned cg_upll_func_cntl)
+{
+	unsigned i;
+
+	/* make sure UPLL_CTLREQ is deasserted */
+	WREG32_P(cg_upll_func_cntl, 0, ~UPLL_CTLREQ_MASK);
+
+	mdelay(10);
+
+	/* assert UPLL_CTLREQ */
+	WREG32_P(cg_upll_func_cntl, UPLL_CTLREQ_MASK, ~UPLL_CTLREQ_MASK);
+
+	/* wait for CTLACK and CTLACK2 to get asserted */
+	for (i = 0; i < 100; ++i) {
+		uint32_t mask = UPLL_CTLACK_MASK | UPLL_CTLACK2_MASK;
+		if ((RREG32(cg_upll_func_cntl) & mask) == mask)
+			break;
+		mdelay(10);
+	}
+
+	/* deassert UPLL_CTLREQ */
+	WREG32_P(cg_upll_func_cntl, 0, ~UPLL_CTLREQ_MASK);
+
+	if (i == 100) {
+		DRM_ERROR("Timeout setting UVD clocks!\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/radeon/rs600.c b/drivers/gpu/drm/radeon/rs600.c
index 5a0fc74c2ba6..46fa1b07c560 100644
--- a/drivers/gpu/drm/radeon/rs600.c
+++ b/drivers/gpu/drm/radeon/rs600.c
@@ -52,23 +52,59 @@ static const u32 crtc_offsets[2] =
 	AVIVO_D2CRTC_H_TOTAL - AVIVO_D1CRTC_H_TOTAL
 };
 
+static bool avivo_is_in_vblank(struct radeon_device *rdev, int crtc)
+{
+	if (RREG32(AVIVO_D1CRTC_STATUS + crtc_offsets[crtc]) & AVIVO_D1CRTC_V_BLANK)
+		return true;
+	else
+		return false;
+}
+
+static bool avivo_is_counter_moving(struct radeon_device *rdev, int crtc)
+{
+	u32 pos1, pos2;
+
+	pos1 = RREG32(AVIVO_D1CRTC_STATUS_POSITION + crtc_offsets[crtc]);
+	pos2 = RREG32(AVIVO_D1CRTC_STATUS_POSITION + crtc_offsets[crtc]);
+
+	if (pos1 != pos2)
+		return true;
+	else
+		return false;
+}
+
+/**
+ * avivo_wait_for_vblank - vblank wait asic callback.
+ *
+ * @rdev: radeon_device pointer
+ * @crtc: crtc to wait for vblank on
+ *
+ * Wait for vblank on the requested crtc (r5xx-r7xx).
+ */
 void avivo_wait_for_vblank(struct radeon_device *rdev, int crtc)
 {
-	int i;
+	unsigned i = 0;
 
 	if (crtc >= rdev->num_crtc)
 		return;
 
-	if (RREG32(AVIVO_D1CRTC_CONTROL + crtc_offsets[crtc]) & AVIVO_CRTC_EN) {
-		for (i = 0; i < rdev->usec_timeout; i++) {
-			if (!(RREG32(AVIVO_D1CRTC_STATUS + crtc_offsets[crtc]) & AVIVO_D1CRTC_V_BLANK))
+	if (!(RREG32(AVIVO_D1CRTC_CONTROL + crtc_offsets[crtc]) & AVIVO_CRTC_EN))
+		return;
+
+	/* depending on when we hit vblank, we may be close to active; if so,
+	 * wait for another frame.
+	 */
+	while (avivo_is_in_vblank(rdev, crtc)) {
+		if (i++ % 100 == 0) {
+			if (!avivo_is_counter_moving(rdev, crtc))
 				break;
-			udelay(1);
 		}
-		for (i = 0; i < rdev->usec_timeout; i++) {
-			if (RREG32(AVIVO_D1CRTC_STATUS + crtc_offsets[crtc]) & AVIVO_D1CRTC_V_BLANK)
+	}
+
+	while (!avivo_is_in_vblank(rdev, crtc)) {
+		if (i++ % 100 == 0) {
+			if (!avivo_is_counter_moving(rdev, crtc))
 				break;
-			udelay(1);
 		}
 	}
 }
diff --git a/drivers/gpu/drm/radeon/rs690.c b/drivers/gpu/drm/radeon/rs690.c
index 5706d2ac75ab..ab4c86cfd552 100644
--- a/drivers/gpu/drm/radeon/rs690.c
+++ b/drivers/gpu/drm/radeon/rs690.c
@@ -148,6 +148,8 @@ void rs690_pm_info(struct radeon_device *rdev)
 static void rs690_mc_init(struct radeon_device *rdev)
 {
 	u64 base;
+	uint32_t h_addr, l_addr;
+	unsigned long long k8_addr;
 
 	rs400_gart_adjust_size(rdev);
 	rdev->mc.vram_is_ddr = true;
@@ -160,6 +162,27 @@ static void rs690_mc_init(struct radeon_device *rdev)
 	base = RREG32_MC(R_000100_MCCFG_FB_LOCATION);
 	base = G_000100_MC_FB_START(base) << 16;
 	rdev->mc.igp_sideport_enabled = radeon_atombios_sideport_present(rdev);
+
+	/* Use K8 direct mapping for fast fb access. */ 
+	rdev->fastfb_working = false;
+	h_addr = G_00005F_K8_ADDR_EXT(RREG32_MC(R_00005F_MC_MISC_UMA_CNTL));
+	l_addr = RREG32_MC(R_00001E_K8_FB_LOCATION);
+	k8_addr = ((unsigned long long)h_addr) << 32 | l_addr;
+#if defined(CONFIG_X86_32) && !defined(CONFIG_X86_PAE)
+	if (k8_addr + rdev->mc.visible_vram_size < 0x100000000ULL)	
+#endif
+	{
+		/* FastFB shall be used with UMA memory. Here it is simply disabled when sideport 
+		 * memory is present.
+		 */
+		if (rdev->mc.igp_sideport_enabled == false && radeon_fastfb == 1) {
+			DRM_INFO("Direct mapping: aper base at 0x%llx, replaced by direct mapping base 0x%llx.\n", 
+					(unsigned long long)rdev->mc.aper_base, k8_addr);
+			rdev->mc.aper_base = (resource_size_t)k8_addr;
+			rdev->fastfb_working = true;
+		}
+	}  
+
 	rs690_pm_info(rdev);
 	radeon_vram_location(rdev, &rdev->mc, base);
 	rdev->mc.gtt_base_align = rdev->mc.gtt_size - 1;
diff --git a/drivers/gpu/drm/radeon/rs690d.h b/drivers/gpu/drm/radeon/rs690d.h
index 36e6398a98ae..8af3ccf20cc0 100644
--- a/drivers/gpu/drm/radeon/rs690d.h
+++ b/drivers/gpu/drm/radeon/rs690d.h
@@ -29,6 +29,9 @@
 #define __RS690D_H__
 
 /* Registers */
+#define R_00001E_K8_FB_LOCATION                      0x00001E
+#define R_00005F_MC_MISC_UMA_CNTL                    0x00005F
+#define   G_00005F_K8_ADDR_EXT(x)                      (((x) >> 0) & 0xFF)
 #define R_000078_MC_INDEX                            0x000078
 #define   S_000078_MC_IND_ADDR(x)                      (((x) & 0x1FF) << 0)
 #define   G_000078_MC_IND_ADDR(x)                      (((x) >> 0) & 0x1FF)
diff --git a/drivers/gpu/drm/radeon/rv515.c b/drivers/gpu/drm/radeon/rv515.c
index 435ed3551364..ffcba730c57c 100644
--- a/drivers/gpu/drm/radeon/rv515.c
+++ b/drivers/gpu/drm/radeon/rv515.c
@@ -303,8 +303,10 @@ void rv515_mc_stop(struct radeon_device *rdev, struct rv515_mc_save *save)
 			tmp = RREG32(AVIVO_D1CRTC_CONTROL + crtc_offsets[i]);
 			if (!(tmp & AVIVO_CRTC_DISP_READ_REQUEST_DISABLE)) {
 				radeon_wait_for_vblank(rdev, i);
+				WREG32(AVIVO_D1CRTC_UPDATE_LOCK + crtc_offsets[i], 1);
 				tmp |= AVIVO_CRTC_DISP_READ_REQUEST_DISABLE;
 				WREG32(AVIVO_D1CRTC_CONTROL + crtc_offsets[i], tmp);
+				WREG32(AVIVO_D1CRTC_UPDATE_LOCK + crtc_offsets[i], 0);
 			}
 			/* wait for the next frame */
 			frame_count = radeon_get_vblank_counter(rdev, i);
@@ -313,6 +315,15 @@ void rv515_mc_stop(struct radeon_device *rdev, struct rv515_mc_save *save)
 					break;
 				udelay(1);
 			}
+
+			/* XXX this is a hack to avoid strange behavior with EFI on certain systems */
+			WREG32(AVIVO_D1CRTC_UPDATE_LOCK + crtc_offsets[i], 1);
+			tmp = RREG32(AVIVO_D1CRTC_CONTROL + crtc_offsets[i]);
+			tmp &= ~AVIVO_CRTC_EN;
+			WREG32(AVIVO_D1CRTC_CONTROL + crtc_offsets[i], tmp);
+			WREG32(AVIVO_D1CRTC_UPDATE_LOCK + crtc_offsets[i], 0);
+			save->crtc_enabled[i] = false;
+			/* ***** */
 		} else {
 			save->crtc_enabled[i] = false;
 		}
@@ -338,6 +349,22 @@ void rv515_mc_stop(struct radeon_device *rdev, struct rv515_mc_save *save)
 	}
 	/* wait for the MC to settle */
 	udelay(100);
+
+	/* lock double buffered regs */
+	for (i = 0; i < rdev->num_crtc; i++) {
+		if (save->crtc_enabled[i]) {
+			tmp = RREG32(AVIVO_D1GRPH_UPDATE + crtc_offsets[i]);
+			if (!(tmp & AVIVO_D1GRPH_UPDATE_LOCK)) {
+				tmp |= AVIVO_D1GRPH_UPDATE_LOCK;
+				WREG32(AVIVO_D1GRPH_UPDATE + crtc_offsets[i], tmp);
+			}
+			tmp = RREG32(AVIVO_D1MODE_MASTER_UPDATE_LOCK + crtc_offsets[i]);
+			if (!(tmp & 1)) {
+				tmp |= 1;
+				WREG32(AVIVO_D1MODE_MASTER_UPDATE_LOCK + crtc_offsets[i], tmp);
+			}
+		}
+	}
 }
 
 void rv515_mc_resume(struct radeon_device *rdev, struct rv515_mc_save *save)
@@ -348,7 +375,7 @@ void rv515_mc_resume(struct radeon_device *rdev, struct rv515_mc_save *save)
 	/* update crtc base addresses */
 	for (i = 0; i < rdev->num_crtc; i++) {
 		if (rdev->family >= CHIP_RV770) {
-			if (i == 1) {
+			if (i == 0) {
 				WREG32(R700_D1GRPH_PRIMARY_SURFACE_ADDRESS_HIGH,
 				       upper_32_bits(rdev->mc.vram_start));
 				WREG32(R700_D1GRPH_SECONDARY_SURFACE_ADDRESS_HIGH,
@@ -367,6 +394,33 @@ void rv515_mc_resume(struct radeon_device *rdev, struct rv515_mc_save *save)
 	}
 	WREG32(R_000310_VGA_MEMORY_BASE_ADDRESS, (u32)rdev->mc.vram_start);
 
+	/* unlock regs and wait for update */
+	for (i = 0; i < rdev->num_crtc; i++) {
+		if (save->crtc_enabled[i]) {
+			tmp = RREG32(AVIVO_D1MODE_MASTER_UPDATE_MODE + crtc_offsets[i]);
+			if ((tmp & 0x3) != 0) {
+				tmp &= ~0x3;
+				WREG32(AVIVO_D1MODE_MASTER_UPDATE_MODE + crtc_offsets[i], tmp);
+			}
+			tmp = RREG32(AVIVO_D1GRPH_UPDATE + crtc_offsets[i]);
+			if (tmp & AVIVO_D1GRPH_UPDATE_LOCK) {
+				tmp &= ~AVIVO_D1GRPH_UPDATE_LOCK;
+				WREG32(AVIVO_D1GRPH_UPDATE + crtc_offsets[i], tmp);
+			}
+			tmp = RREG32(AVIVO_D1MODE_MASTER_UPDATE_LOCK + crtc_offsets[i]);
+			if (tmp & 1) {
+				tmp &= ~1;
+				WREG32(AVIVO_D1MODE_MASTER_UPDATE_LOCK + crtc_offsets[i], tmp);
+			}
+			for (j = 0; j < rdev->usec_timeout; j++) {
+				tmp = RREG32(AVIVO_D1GRPH_UPDATE + crtc_offsets[i]);
+				if ((tmp & AVIVO_D1GRPH_SURFACE_UPDATE_PENDING) == 0)
+					break;
+				udelay(1);
+			}
+		}
+	}
+
 	if (rdev->family >= CHIP_R600) {
 		/* unblackout the MC */
 		if (rdev->family >= CHIP_RV770)
diff --git a/drivers/gpu/drm/radeon/rv770.c b/drivers/gpu/drm/radeon/rv770.c
index d63fe1d0f53f..83f612a9500b 100644
--- a/drivers/gpu/drm/radeon/rv770.c
+++ b/drivers/gpu/drm/radeon/rv770.c
@@ -42,6 +42,739 @@
 static void rv770_gpu_init(struct radeon_device *rdev);
 void rv770_fini(struct radeon_device *rdev);
 static void rv770_pcie_gen2_enable(struct radeon_device *rdev);
+int evergreen_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk);
+
+int rv770_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk)
+{
+	unsigned fb_div = 0, vclk_div = 0, dclk_div = 0;
+	int r;
+
+	/* RV740 uses evergreen uvd clk programming */
+	if (rdev->family == CHIP_RV740)
+		return evergreen_set_uvd_clocks(rdev, vclk, dclk);
+
+	/* bypass vclk and dclk with bclk */
+	WREG32_P(CG_UPLL_FUNC_CNTL_2,
+		 VCLK_SRC_SEL(1) | DCLK_SRC_SEL(1),
+		 ~(VCLK_SRC_SEL_MASK | DCLK_SRC_SEL_MASK));
+
+	if (!vclk || !dclk) {
+		/* keep the Bypass mode, put PLL to sleep */
+		WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_SLEEP_MASK, ~UPLL_SLEEP_MASK);
+		return 0;
+	}
+
+	r = radeon_uvd_calc_upll_dividers(rdev, vclk, dclk, 50000, 160000,
+					  43663, 0x03FFFFFE, 1, 30, ~0,
+					  &fb_div, &vclk_div, &dclk_div);
+	if (r)
+		return r;
+
+	fb_div |= 1;
+	vclk_div -= 1;
+	dclk_div -= 1;
+
+	/* set UPLL_FB_DIV to 0x50000 */
+	WREG32_P(CG_UPLL_FUNC_CNTL_3, UPLL_FB_DIV(0x50000), ~UPLL_FB_DIV_MASK);
+
+	/* deassert UPLL_RESET and UPLL_SLEEP */
+	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~(UPLL_RESET_MASK | UPLL_SLEEP_MASK));
+
+	/* assert BYPASS EN and FB_DIV[0] <- ??? why? */
+	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_BYPASS_EN_MASK, ~UPLL_BYPASS_EN_MASK);
+	WREG32_P(CG_UPLL_FUNC_CNTL_3, UPLL_FB_DIV(1), ~UPLL_FB_DIV(1));
+
+	r = radeon_uvd_send_upll_ctlreq(rdev, CG_UPLL_FUNC_CNTL);
+	if (r)
+		return r;
+
+	/* assert PLL_RESET */
+	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_RESET_MASK, ~UPLL_RESET_MASK);
+
+	/* set the required FB_DIV, REF_DIV, Post divder values */
+	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_REF_DIV(1), ~UPLL_REF_DIV_MASK);
+	WREG32_P(CG_UPLL_FUNC_CNTL_2,
+		 UPLL_SW_HILEN(vclk_div >> 1) |
+		 UPLL_SW_LOLEN((vclk_div >> 1) + (vclk_div & 1)) |
+		 UPLL_SW_HILEN2(dclk_div >> 1) |
+		 UPLL_SW_LOLEN2((dclk_div >> 1) + (dclk_div & 1)),
+		 ~UPLL_SW_MASK);
+
+	WREG32_P(CG_UPLL_FUNC_CNTL_3, UPLL_FB_DIV(fb_div),
+		 ~UPLL_FB_DIV_MASK);
+
+	/* give the PLL some time to settle */
+	mdelay(15);
+
+	/* deassert PLL_RESET */
+	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_RESET_MASK);
+
+	mdelay(15);
+
+	/* deassert BYPASS EN and FB_DIV[0] <- ??? why? */
+	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_BYPASS_EN_MASK);
+	WREG32_P(CG_UPLL_FUNC_CNTL_3, 0, ~UPLL_FB_DIV(1));
+
+	r = radeon_uvd_send_upll_ctlreq(rdev, CG_UPLL_FUNC_CNTL);
+	if (r)
+		return r;
+
+	/* switch VCLK and DCLK selection */
+	WREG32_P(CG_UPLL_FUNC_CNTL_2,
+		 VCLK_SRC_SEL(2) | DCLK_SRC_SEL(2),
+		 ~(VCLK_SRC_SEL_MASK | DCLK_SRC_SEL_MASK));
+
+	mdelay(100);
+
+	return 0;
+}
+
+static const u32 r7xx_golden_registers[] =
+{
+	0x8d00, 0xffffffff, 0x0e0e0074,
+	0x8d04, 0xffffffff, 0x013a2b34,
+	0x9508, 0xffffffff, 0x00000002,
+	0x8b20, 0xffffffff, 0,
+	0x88c4, 0xffffffff, 0x000000c2,
+	0x28350, 0xffffffff, 0,
+	0x9058, 0xffffffff, 0x0fffc40f,
+	0x240c, 0xffffffff, 0x00000380,
+	0x733c, 0xffffffff, 0x00000002,
+	0x2650, 0x00040000, 0,
+	0x20bc, 0x00040000, 0,
+	0x7300, 0xffffffff, 0x001000f0
+};
+
+static const u32 r7xx_golden_dyn_gpr_registers[] =
+{
+	0x8db0, 0xffffffff, 0x98989898,
+	0x8db4, 0xffffffff, 0x98989898,
+	0x8db8, 0xffffffff, 0x98989898,
+	0x8dbc, 0xffffffff, 0x98989898,
+	0x8dc0, 0xffffffff, 0x98989898,
+	0x8dc4, 0xffffffff, 0x98989898,
+	0x8dc8, 0xffffffff, 0x98989898,
+	0x8dcc, 0xffffffff, 0x98989898,
+	0x88c4, 0xffffffff, 0x00000082
+};
+
+static const u32 rv770_golden_registers[] =
+{
+	0x562c, 0xffffffff, 0,
+	0x3f90, 0xffffffff, 0,
+	0x9148, 0xffffffff, 0,
+	0x3f94, 0xffffffff, 0,
+	0x914c, 0xffffffff, 0,
+	0x9698, 0x18000000, 0x18000000
+};
+
+static const u32 rv770ce_golden_registers[] =
+{
+	0x562c, 0xffffffff, 0,
+	0x3f90, 0xffffffff, 0x00cc0000,
+	0x9148, 0xffffffff, 0x00cc0000,
+	0x3f94, 0xffffffff, 0x00cc0000,
+	0x914c, 0xffffffff, 0x00cc0000,
+	0x9b7c, 0xffffffff, 0x00fa0000,
+	0x3f8c, 0xffffffff, 0x00fa0000,
+	0x9698, 0x18000000, 0x18000000
+};
+
+static const u32 rv770_mgcg_init[] =
+{
+	0x8bcc, 0xffffffff, 0x130300f9,
+	0x5448, 0xffffffff, 0x100,
+	0x55e4, 0xffffffff, 0x100,
+	0x160c, 0xffffffff, 0x100,
+	0x5644, 0xffffffff, 0x100,
+	0xc164, 0xffffffff, 0x100,
+	0x8a18, 0xffffffff, 0x100,
+	0x897c, 0xffffffff, 0x8000100,
+	0x8b28, 0xffffffff, 0x3c000100,
+	0x9144, 0xffffffff, 0x100,
+	0x9a1c, 0xffffffff, 0x10000,
+	0x9a50, 0xffffffff, 0x100,
+	0x9a1c, 0xffffffff, 0x10001,
+	0x9a50, 0xffffffff, 0x100,
+	0x9a1c, 0xffffffff, 0x10002,
+	0x9a50, 0xffffffff, 0x100,
+	0x9a1c, 0xffffffff, 0x10003,
+	0x9a50, 0xffffffff, 0x100,
+	0x9a1c, 0xffffffff, 0x0,
+	0x9870, 0xffffffff, 0x100,
+	0x8d58, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x0,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x1,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x2,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x3,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x4,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x5,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x6,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x7,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x8,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x9,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x8000,
+	0x9490, 0xffffffff, 0x0,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x1,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x2,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x3,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x4,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x5,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x6,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x7,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x8,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x9,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x8000,
+	0x9604, 0xffffffff, 0x0,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x1,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x2,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x3,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x4,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x5,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x6,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x7,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x8,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x9,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x80000000,
+	0x9030, 0xffffffff, 0x100,
+	0x9034, 0xffffffff, 0x100,
+	0x9038, 0xffffffff, 0x100,
+	0x903c, 0xffffffff, 0x100,
+	0x9040, 0xffffffff, 0x100,
+	0xa200, 0xffffffff, 0x100,
+	0xa204, 0xffffffff, 0x100,
+	0xa208, 0xffffffff, 0x100,
+	0xa20c, 0xffffffff, 0x100,
+	0x971c, 0xffffffff, 0x100,
+	0x915c, 0xffffffff, 0x00020001,
+	0x9160, 0xffffffff, 0x00040003,
+	0x916c, 0xffffffff, 0x00060005,
+	0x9170, 0xffffffff, 0x00080007,
+	0x9174, 0xffffffff, 0x000a0009,
+	0x9178, 0xffffffff, 0x000c000b,
+	0x917c, 0xffffffff, 0x000e000d,
+	0x9180, 0xffffffff, 0x0010000f,
+	0x918c, 0xffffffff, 0x00120011,
+	0x9190, 0xffffffff, 0x00140013,
+	0x9194, 0xffffffff, 0x00020001,
+	0x9198, 0xffffffff, 0x00040003,
+	0x919c, 0xffffffff, 0x00060005,
+	0x91a8, 0xffffffff, 0x00080007,
+	0x91ac, 0xffffffff, 0x000a0009,
+	0x91b0, 0xffffffff, 0x000c000b,
+	0x91b4, 0xffffffff, 0x000e000d,
+	0x91b8, 0xffffffff, 0x0010000f,
+	0x91c4, 0xffffffff, 0x00120011,
+	0x91c8, 0xffffffff, 0x00140013,
+	0x91cc, 0xffffffff, 0x00020001,
+	0x91d0, 0xffffffff, 0x00040003,
+	0x91d4, 0xffffffff, 0x00060005,
+	0x91e0, 0xffffffff, 0x00080007,
+	0x91e4, 0xffffffff, 0x000a0009,
+	0x91e8, 0xffffffff, 0x000c000b,
+	0x91ec, 0xffffffff, 0x00020001,
+	0x91f0, 0xffffffff, 0x00040003,
+	0x91f4, 0xffffffff, 0x00060005,
+	0x9200, 0xffffffff, 0x00080007,
+	0x9204, 0xffffffff, 0x000a0009,
+	0x9208, 0xffffffff, 0x000c000b,
+	0x920c, 0xffffffff, 0x000e000d,
+	0x9210, 0xffffffff, 0x0010000f,
+	0x921c, 0xffffffff, 0x00120011,
+	0x9220, 0xffffffff, 0x00140013,
+	0x9224, 0xffffffff, 0x00020001,
+	0x9228, 0xffffffff, 0x00040003,
+	0x922c, 0xffffffff, 0x00060005,
+	0x9238, 0xffffffff, 0x00080007,
+	0x923c, 0xffffffff, 0x000a0009,
+	0x9240, 0xffffffff, 0x000c000b,
+	0x9244, 0xffffffff, 0x000e000d,
+	0x9248, 0xffffffff, 0x0010000f,
+	0x9254, 0xffffffff, 0x00120011,
+	0x9258, 0xffffffff, 0x00140013,
+	0x925c, 0xffffffff, 0x00020001,
+	0x9260, 0xffffffff, 0x00040003,
+	0x9264, 0xffffffff, 0x00060005,
+	0x9270, 0xffffffff, 0x00080007,
+	0x9274, 0xffffffff, 0x000a0009,
+	0x9278, 0xffffffff, 0x000c000b,
+	0x927c, 0xffffffff, 0x000e000d,
+	0x9280, 0xffffffff, 0x0010000f,
+	0x928c, 0xffffffff, 0x00120011,
+	0x9290, 0xffffffff, 0x00140013,
+	0x9294, 0xffffffff, 0x00020001,
+	0x929c, 0xffffffff, 0x00040003,
+	0x92a0, 0xffffffff, 0x00060005,
+	0x92a4, 0xffffffff, 0x00080007
+};
+
+static const u32 rv710_golden_registers[] =
+{
+	0x3f90, 0x00ff0000, 0x00fc0000,
+	0x9148, 0x00ff0000, 0x00fc0000,
+	0x3f94, 0x00ff0000, 0x00fc0000,
+	0x914c, 0x00ff0000, 0x00fc0000,
+	0xb4c, 0x00000020, 0x00000020,
+	0xa180, 0xffffffff, 0x00003f3f
+};
+
+static const u32 rv710_mgcg_init[] =
+{
+	0x8bcc, 0xffffffff, 0x13030040,
+	0x5448, 0xffffffff, 0x100,
+	0x55e4, 0xffffffff, 0x100,
+	0x160c, 0xffffffff, 0x100,
+	0x5644, 0xffffffff, 0x100,
+	0xc164, 0xffffffff, 0x100,
+	0x8a18, 0xffffffff, 0x100,
+	0x897c, 0xffffffff, 0x8000100,
+	0x8b28, 0xffffffff, 0x3c000100,
+	0x9144, 0xffffffff, 0x100,
+	0x9a1c, 0xffffffff, 0x10000,
+	0x9a50, 0xffffffff, 0x100,
+	0x9a1c, 0xffffffff, 0x0,
+	0x9870, 0xffffffff, 0x100,
+	0x8d58, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x0,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x1,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x8000,
+	0x9490, 0xffffffff, 0x0,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x1,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x8000,
+	0x9604, 0xffffffff, 0x0,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x1,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x80000000,
+	0x9030, 0xffffffff, 0x100,
+	0x9034, 0xffffffff, 0x100,
+	0x9038, 0xffffffff, 0x100,
+	0x903c, 0xffffffff, 0x100,
+	0x9040, 0xffffffff, 0x100,
+	0xa200, 0xffffffff, 0x100,
+	0xa204, 0xffffffff, 0x100,
+	0xa208, 0xffffffff, 0x100,
+	0xa20c, 0xffffffff, 0x100,
+	0x971c, 0xffffffff, 0x100,
+	0x915c, 0xffffffff, 0x00020001,
+	0x9174, 0xffffffff, 0x00000003,
+	0x9178, 0xffffffff, 0x00050001,
+	0x917c, 0xffffffff, 0x00030002,
+	0x918c, 0xffffffff, 0x00000004,
+	0x9190, 0xffffffff, 0x00070006,
+	0x9194, 0xffffffff, 0x00050001,
+	0x9198, 0xffffffff, 0x00030002,
+	0x91a8, 0xffffffff, 0x00000004,
+	0x91ac, 0xffffffff, 0x00070006,
+	0x91e8, 0xffffffff, 0x00000001,
+	0x9294, 0xffffffff, 0x00000001,
+	0x929c, 0xffffffff, 0x00000002,
+	0x92a0, 0xffffffff, 0x00040003,
+	0x9150, 0xffffffff, 0x4d940000
+};
+
+static const u32 rv730_golden_registers[] =
+{
+	0x3f90, 0x00ff0000, 0x00f00000,
+	0x9148, 0x00ff0000, 0x00f00000,
+	0x3f94, 0x00ff0000, 0x00f00000,
+	0x914c, 0x00ff0000, 0x00f00000,
+	0x900c, 0xffffffff, 0x003b033f,
+	0xb4c, 0x00000020, 0x00000020,
+	0xa180, 0xffffffff, 0x00003f3f
+};
+
+static const u32 rv730_mgcg_init[] =
+{
+	0x8bcc, 0xffffffff, 0x130300f9,
+	0x5448, 0xffffffff, 0x100,
+	0x55e4, 0xffffffff, 0x100,
+	0x160c, 0xffffffff, 0x100,
+	0x5644, 0xffffffff, 0x100,
+	0xc164, 0xffffffff, 0x100,
+	0x8a18, 0xffffffff, 0x100,
+	0x897c, 0xffffffff, 0x8000100,
+	0x8b28, 0xffffffff, 0x3c000100,
+	0x9144, 0xffffffff, 0x100,
+	0x9a1c, 0xffffffff, 0x10000,
+	0x9a50, 0xffffffff, 0x100,
+	0x9a1c, 0xffffffff, 0x10001,
+	0x9a50, 0xffffffff, 0x100,
+	0x9a1c, 0xffffffff, 0x0,
+	0x9870, 0xffffffff, 0x100,
+	0x8d58, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x0,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x1,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x2,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x3,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x4,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x5,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x6,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x7,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x8000,
+	0x9490, 0xffffffff, 0x0,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x1,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x2,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x3,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x4,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x5,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x6,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x7,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x8000,
+	0x9604, 0xffffffff, 0x0,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x1,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x2,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x3,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x4,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x5,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x6,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x7,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x80000000,
+	0x9030, 0xffffffff, 0x100,
+	0x9034, 0xffffffff, 0x100,
+	0x9038, 0xffffffff, 0x100,
+	0x903c, 0xffffffff, 0x100,
+	0x9040, 0xffffffff, 0x100,
+	0xa200, 0xffffffff, 0x100,
+	0xa204, 0xffffffff, 0x100,
+	0xa208, 0xffffffff, 0x100,
+	0xa20c, 0xffffffff, 0x100,
+	0x971c, 0xffffffff, 0x100,
+	0x915c, 0xffffffff, 0x00020001,
+	0x916c, 0xffffffff, 0x00040003,
+	0x9170, 0xffffffff, 0x00000005,
+	0x9178, 0xffffffff, 0x00050001,
+	0x917c, 0xffffffff, 0x00030002,
+	0x918c, 0xffffffff, 0x00000004,
+	0x9190, 0xffffffff, 0x00070006,
+	0x9194, 0xffffffff, 0x00050001,
+	0x9198, 0xffffffff, 0x00030002,
+	0x91a8, 0xffffffff, 0x00000004,
+	0x91ac, 0xffffffff, 0x00070006,
+	0x91b0, 0xffffffff, 0x00050001,
+	0x91b4, 0xffffffff, 0x00030002,
+	0x91c4, 0xffffffff, 0x00000004,
+	0x91c8, 0xffffffff, 0x00070006,
+	0x91cc, 0xffffffff, 0x00050001,
+	0x91d0, 0xffffffff, 0x00030002,
+	0x91e0, 0xffffffff, 0x00000004,
+	0x91e4, 0xffffffff, 0x00070006,
+	0x91e8, 0xffffffff, 0x00000001,
+	0x91ec, 0xffffffff, 0x00050001,
+	0x91f0, 0xffffffff, 0x00030002,
+	0x9200, 0xffffffff, 0x00000004,
+	0x9204, 0xffffffff, 0x00070006,
+	0x9208, 0xffffffff, 0x00050001,
+	0x920c, 0xffffffff, 0x00030002,
+	0x921c, 0xffffffff, 0x00000004,
+	0x9220, 0xffffffff, 0x00070006,
+	0x9224, 0xffffffff, 0x00050001,
+	0x9228, 0xffffffff, 0x00030002,
+	0x9238, 0xffffffff, 0x00000004,
+	0x923c, 0xffffffff, 0x00070006,
+	0x9240, 0xffffffff, 0x00050001,
+	0x9244, 0xffffffff, 0x00030002,
+	0x9254, 0xffffffff, 0x00000004,
+	0x9258, 0xffffffff, 0x00070006,
+	0x9294, 0xffffffff, 0x00000001,
+	0x929c, 0xffffffff, 0x00000002,
+	0x92a0, 0xffffffff, 0x00040003,
+	0x92a4, 0xffffffff, 0x00000005
+};
+
+static const u32 rv740_golden_registers[] =
+{
+	0x88c4, 0xffffffff, 0x00000082,
+	0x28a50, 0xfffffffc, 0x00000004,
+	0x2650, 0x00040000, 0,
+	0x20bc, 0x00040000, 0,
+	0x733c, 0xffffffff, 0x00000002,
+	0x7300, 0xffffffff, 0x001000f0,
+	0x3f90, 0x00ff0000, 0,
+	0x9148, 0x00ff0000, 0,
+	0x3f94, 0x00ff0000, 0,
+	0x914c, 0x00ff0000, 0,
+	0x240c, 0xffffffff, 0x00000380,
+	0x8a14, 0x00000007, 0x00000007,
+	0x8b24, 0xffffffff, 0x00ff0fff,
+	0x28a4c, 0xffffffff, 0x00004000,
+	0xa180, 0xffffffff, 0x00003f3f,
+	0x8d00, 0xffffffff, 0x0e0e003a,
+	0x8d04, 0xffffffff, 0x013a0e2a,
+	0x8c00, 0xffffffff, 0xe400000f,
+	0x8db0, 0xffffffff, 0x98989898,
+	0x8db4, 0xffffffff, 0x98989898,
+	0x8db8, 0xffffffff, 0x98989898,
+	0x8dbc, 0xffffffff, 0x98989898,
+	0x8dc0, 0xffffffff, 0x98989898,
+	0x8dc4, 0xffffffff, 0x98989898,
+	0x8dc8, 0xffffffff, 0x98989898,
+	0x8dcc, 0xffffffff, 0x98989898,
+	0x9058, 0xffffffff, 0x0fffc40f,
+	0x900c, 0xffffffff, 0x003b033f,
+	0x28350, 0xffffffff, 0,
+	0x8cf0, 0x1fffffff, 0x08e00420,
+	0x9508, 0xffffffff, 0x00000002,
+	0x88c4, 0xffffffff, 0x000000c2,
+	0x9698, 0x18000000, 0x18000000
+};
+
+static const u32 rv740_mgcg_init[] =
+{
+	0x8bcc, 0xffffffff, 0x13030100,
+	0x5448, 0xffffffff, 0x100,
+	0x55e4, 0xffffffff, 0x100,
+	0x160c, 0xffffffff, 0x100,
+	0x5644, 0xffffffff, 0x100,
+	0xc164, 0xffffffff, 0x100,
+	0x8a18, 0xffffffff, 0x100,
+	0x897c, 0xffffffff, 0x100,
+	0x8b28, 0xffffffff, 0x100,
+	0x9144, 0xffffffff, 0x100,
+	0x9a1c, 0xffffffff, 0x10000,
+	0x9a50, 0xffffffff, 0x100,
+	0x9a1c, 0xffffffff, 0x10001,
+	0x9a50, 0xffffffff, 0x100,
+	0x9a1c, 0xffffffff, 0x10002,
+	0x9a50, 0xffffffff, 0x100,
+	0x9a1c, 0xffffffff, 0x10003,
+	0x9a50, 0xffffffff, 0x100,
+	0x9a1c, 0xffffffff, 0x0,
+	0x9870, 0xffffffff, 0x100,
+	0x8d58, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x0,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x1,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x2,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x3,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x4,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x5,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x6,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x7,
+	0x9510, 0xffffffff, 0x100,
+	0x9500, 0xffffffff, 0x8000,
+	0x9490, 0xffffffff, 0x0,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x1,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x2,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x3,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x4,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x5,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x6,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x7,
+	0x949c, 0xffffffff, 0x100,
+	0x9490, 0xffffffff, 0x8000,
+	0x9604, 0xffffffff, 0x0,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x1,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x2,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x3,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x4,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x5,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x6,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x7,
+	0x9654, 0xffffffff, 0x100,
+	0x9604, 0xffffffff, 0x80000000,
+	0x9030, 0xffffffff, 0x100,
+	0x9034, 0xffffffff, 0x100,
+	0x9038, 0xffffffff, 0x100,
+	0x903c, 0xffffffff, 0x100,
+	0x9040, 0xffffffff, 0x100,
+	0xa200, 0xffffffff, 0x100,
+	0xa204, 0xffffffff, 0x100,
+	0xa208, 0xffffffff, 0x100,
+	0xa20c, 0xffffffff, 0x100,
+	0x971c, 0xffffffff, 0x100,
+	0x915c, 0xffffffff, 0x00020001,
+	0x9160, 0xffffffff, 0x00040003,
+	0x916c, 0xffffffff, 0x00060005,
+	0x9170, 0xffffffff, 0x00080007,
+	0x9174, 0xffffffff, 0x000a0009,
+	0x9178, 0xffffffff, 0x000c000b,
+	0x917c, 0xffffffff, 0x000e000d,
+	0x9180, 0xffffffff, 0x0010000f,
+	0x918c, 0xffffffff, 0x00120011,
+	0x9190, 0xffffffff, 0x00140013,
+	0x9194, 0xffffffff, 0x00020001,
+	0x9198, 0xffffffff, 0x00040003,
+	0x919c, 0xffffffff, 0x00060005,
+	0x91a8, 0xffffffff, 0x00080007,
+	0x91ac, 0xffffffff, 0x000a0009,
+	0x91b0, 0xffffffff, 0x000c000b,
+	0x91b4, 0xffffffff, 0x000e000d,
+	0x91b8, 0xffffffff, 0x0010000f,
+	0x91c4, 0xffffffff, 0x00120011,
+	0x91c8, 0xffffffff, 0x00140013,
+	0x91cc, 0xffffffff, 0x00020001,
+	0x91d0, 0xffffffff, 0x00040003,
+	0x91d4, 0xffffffff, 0x00060005,
+	0x91e0, 0xffffffff, 0x00080007,
+	0x91e4, 0xffffffff, 0x000a0009,
+	0x91e8, 0xffffffff, 0x000c000b,
+	0x91ec, 0xffffffff, 0x00020001,
+	0x91f0, 0xffffffff, 0x00040003,
+	0x91f4, 0xffffffff, 0x00060005,
+	0x9200, 0xffffffff, 0x00080007,
+	0x9204, 0xffffffff, 0x000a0009,
+	0x9208, 0xffffffff, 0x000c000b,
+	0x920c, 0xffffffff, 0x000e000d,
+	0x9210, 0xffffffff, 0x0010000f,
+	0x921c, 0xffffffff, 0x00120011,
+	0x9220, 0xffffffff, 0x00140013,
+	0x9224, 0xffffffff, 0x00020001,
+	0x9228, 0xffffffff, 0x00040003,
+	0x922c, 0xffffffff, 0x00060005,
+	0x9238, 0xffffffff, 0x00080007,
+	0x923c, 0xffffffff, 0x000a0009,
+	0x9240, 0xffffffff, 0x000c000b,
+	0x9244, 0xffffffff, 0x000e000d,
+	0x9248, 0xffffffff, 0x0010000f,
+	0x9254, 0xffffffff, 0x00120011,
+	0x9258, 0xffffffff, 0x00140013,
+	0x9294, 0xffffffff, 0x00020001,
+	0x929c, 0xffffffff, 0x00040003,
+	0x92a0, 0xffffffff, 0x00060005,
+	0x92a4, 0xffffffff, 0x00080007
+};
+
+static void rv770_init_golden_registers(struct radeon_device *rdev)
+{
+	switch (rdev->family) {
+	case CHIP_RV770:
+		radeon_program_register_sequence(rdev,
+						 r7xx_golden_registers,
+						 (const u32)ARRAY_SIZE(r7xx_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 r7xx_golden_dyn_gpr_registers,
+						 (const u32)ARRAY_SIZE(r7xx_golden_dyn_gpr_registers));
+		if (rdev->pdev->device == 0x994e)
+			radeon_program_register_sequence(rdev,
+							 rv770ce_golden_registers,
+							 (const u32)ARRAY_SIZE(rv770ce_golden_registers));
+		else
+			radeon_program_register_sequence(rdev,
+							 rv770_golden_registers,
+							 (const u32)ARRAY_SIZE(rv770_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 rv770_mgcg_init,
+						 (const u32)ARRAY_SIZE(rv770_mgcg_init));
+		break;
+	case CHIP_RV730:
+		radeon_program_register_sequence(rdev,
+						 r7xx_golden_registers,
+						 (const u32)ARRAY_SIZE(r7xx_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 r7xx_golden_dyn_gpr_registers,
+						 (const u32)ARRAY_SIZE(r7xx_golden_dyn_gpr_registers));
+		radeon_program_register_sequence(rdev,
+						 rv730_golden_registers,
+						 (const u32)ARRAY_SIZE(rv770_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 rv730_mgcg_init,
+						 (const u32)ARRAY_SIZE(rv770_mgcg_init));
+		break;
+	case CHIP_RV710:
+		radeon_program_register_sequence(rdev,
+						 r7xx_golden_registers,
+						 (const u32)ARRAY_SIZE(r7xx_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 r7xx_golden_dyn_gpr_registers,
+						 (const u32)ARRAY_SIZE(r7xx_golden_dyn_gpr_registers));
+		radeon_program_register_sequence(rdev,
+						 rv710_golden_registers,
+						 (const u32)ARRAY_SIZE(rv770_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 rv710_mgcg_init,
+						 (const u32)ARRAY_SIZE(rv770_mgcg_init));
+		break;
+	case CHIP_RV740:
+		radeon_program_register_sequence(rdev,
+						 rv740_golden_registers,
+						 (const u32)ARRAY_SIZE(rv770_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 rv740_mgcg_init,
+						 (const u32)ARRAY_SIZE(rv770_mgcg_init));
+		break;
+	default:
+		break;
+	}
+}
 
 #define PCIE_BUS_CLK                10000
 #define TCLK                        (PCIE_BUS_CLK / 10)
@@ -68,6 +801,105 @@ u32 rv770_get_xclk(struct radeon_device *rdev)
 	return reference_clock;
 }
 
+int rv770_uvd_resume(struct radeon_device *rdev)
+{
+	uint64_t addr;
+	uint32_t chip_id, size;
+	int r;
+
+	r = radeon_uvd_resume(rdev);
+	if (r)
+		return r;
+
+	/* programm the VCPU memory controller bits 0-27 */
+	addr = rdev->uvd.gpu_addr >> 3;
+	size = RADEON_GPU_PAGE_ALIGN(rdev->uvd_fw->size + 4) >> 3;
+	WREG32(UVD_VCPU_CACHE_OFFSET0, addr);
+	WREG32(UVD_VCPU_CACHE_SIZE0, size);
+
+	addr += size;
+	size = RADEON_UVD_STACK_SIZE >> 3;
+	WREG32(UVD_VCPU_CACHE_OFFSET1, addr);
+	WREG32(UVD_VCPU_CACHE_SIZE1, size);
+
+	addr += size;
+	size = RADEON_UVD_HEAP_SIZE >> 3;
+	WREG32(UVD_VCPU_CACHE_OFFSET2, addr);
+	WREG32(UVD_VCPU_CACHE_SIZE2, size);
+
+	/* bits 28-31 */
+	addr = (rdev->uvd.gpu_addr >> 28) & 0xF;
+	WREG32(UVD_LMI_ADDR_EXT, (addr << 12) | (addr << 0));
+
+	/* bits 32-39 */
+	addr = (rdev->uvd.gpu_addr >> 32) & 0xFF;
+	WREG32(UVD_LMI_EXT40_ADDR, addr | (0x9 << 16) | (0x1 << 31));
+
+	/* tell firmware which hardware it is running on */
+	switch (rdev->family) {
+	default:
+		return -EINVAL;
+	case CHIP_RV710:
+		chip_id = 0x01000005;
+		break;
+	case CHIP_RV730:
+		chip_id = 0x01000006;
+		break;
+	case CHIP_RV740:
+		chip_id = 0x01000007;
+		break;
+	case CHIP_CYPRESS:
+	case CHIP_HEMLOCK:
+		chip_id = 0x01000008;
+		break;
+	case CHIP_JUNIPER:
+		chip_id = 0x01000009;
+		break;
+	case CHIP_REDWOOD:
+		chip_id = 0x0100000a;
+		break;
+	case CHIP_CEDAR:
+		chip_id = 0x0100000b;
+		break;
+	case CHIP_SUMO:
+		chip_id = 0x0100000c;
+		break;
+	case CHIP_SUMO2:
+		chip_id = 0x0100000d;
+		break;
+	case CHIP_PALM:
+		chip_id = 0x0100000e;
+		break;
+	case CHIP_CAYMAN:
+		chip_id = 0x0100000f;
+		break;
+	case CHIP_BARTS:
+		chip_id = 0x01000010;
+		break;
+	case CHIP_TURKS:
+		chip_id = 0x01000011;
+		break;
+	case CHIP_CAICOS:
+		chip_id = 0x01000012;
+		break;
+	case CHIP_TAHITI:
+		chip_id = 0x01000014;
+		break;
+	case CHIP_VERDE:
+		chip_id = 0x01000015;
+		break;
+	case CHIP_PITCAIRN:
+		chip_id = 0x01000016;
+		break;
+	case CHIP_ARUBA:
+		chip_id = 0x01000017;
+		break;
+	}
+	WREG32(UVD_VCPU_CHIP_ID, chip_id);
+
+	return 0;
+}
+
 u32 rv770_page_flip(struct radeon_device *rdev, int crtc_id, u64 crtc_base)
 {
 	struct radeon_crtc *radeon_crtc = rdev->mode_info.crtcs[crtc_id];
@@ -611,6 +1443,11 @@ static void rv770_gpu_init(struct radeon_device *rdev)
 	WREG32(HDP_TILING_CONFIG, (gb_tiling_config & 0xffff));
 	WREG32(DMA_TILING_CONFIG, (gb_tiling_config & 0xffff));
 	WREG32(DMA_TILING_CONFIG2, (gb_tiling_config & 0xffff));
+	if (rdev->family == CHIP_RV730) {
+		WREG32(UVD_UDEC_DB_TILING_CONFIG, (gb_tiling_config & 0xffff));
+		WREG32(UVD_UDEC_DBW_TILING_CONFIG, (gb_tiling_config & 0xffff));
+		WREG32(UVD_UDEC_TILING_CONFIG, (gb_tiling_config & 0xffff));
+	}
 
 	WREG32(CGTS_SYS_TCC_DISABLE, 0);
 	WREG32(CGTS_TCC_DISABLE, 0);
@@ -840,7 +1677,7 @@ void r700_vram_gtt_location(struct radeon_device *rdev, struct radeon_mc *mc)
 	}
 	if (rdev->flags & RADEON_IS_AGP) {
 		size_bf = mc->gtt_start;
-		size_af = 0xFFFFFFFF - mc->gtt_end;
+		size_af = mc->mc_mask - mc->gtt_end;
 		if (size_bf > size_af) {
 			if (mc->mc_vram_size > size_bf) {
 				dev_warn(rdev->dev, "limiting VRAM\n");
@@ -1040,6 +1877,17 @@ static int rv770_startup(struct radeon_device *rdev)
 		return r;
 	}
 
+	r = rv770_uvd_resume(rdev);
+	if (!r) {
+		r = radeon_fence_driver_start_ring(rdev,
+						   R600_RING_TYPE_UVD_INDEX);
+		if (r)
+			dev_err(rdev->dev, "UVD fences init error (%d).\n", r);
+	}
+
+	if (r)
+		rdev->ring[R600_RING_TYPE_UVD_INDEX].ring_size = 0;
+
 	/* Enable IRQ */
 	r = r600_irq_init(rdev);
 	if (r) {
@@ -1074,6 +1922,19 @@ static int rv770_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	ring = &rdev->ring[R600_RING_TYPE_UVD_INDEX];
+	if (ring->ring_size) {
+		r = radeon_ring_init(rdev, ring, ring->ring_size,
+				     R600_WB_UVD_RPTR_OFFSET,
+				     UVD_RBC_RB_RPTR, UVD_RBC_RB_WPTR,
+				     0, 0xfffff, RADEON_CP_PACKET2);
+		if (!r)
+			r = r600_uvd_init(rdev);
+
+		if (r)
+			DRM_ERROR("radeon: failed initializing UVD (%d).\n", r);
+	}
+
 	r = radeon_ib_pool_init(rdev);
 	if (r) {
 		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
@@ -1100,6 +1961,9 @@ int rv770_resume(struct radeon_device *rdev)
 	/* post card */
 	atom_asic_init(rdev->mode_info.atom_context);
 
+	/* init golden registers */
+	rv770_init_golden_registers(rdev);
+
 	rdev->accel_working = true;
 	r = rv770_startup(rdev);
 	if (r) {
@@ -1115,6 +1979,7 @@ int rv770_resume(struct radeon_device *rdev)
 int rv770_suspend(struct radeon_device *rdev)
 {
 	r600_audio_fini(rdev);
+	radeon_uvd_suspend(rdev);
 	r700_cp_stop(rdev);
 	r600_dma_stop(rdev);
 	r600_irq_suspend(rdev);
@@ -1156,6 +2021,8 @@ int rv770_init(struct radeon_device *rdev)
 		DRM_INFO("GPU not posted. posting now...\n");
 		atom_asic_init(rdev->mode_info.atom_context);
 	}
+	/* init golden registers */
+	rv770_init_golden_registers(rdev);
 	/* Initialize scratch registers */
 	r600_scratch_init(rdev);
 	/* Initialize surface registers */
@@ -1190,6 +2057,13 @@ int rv770_init(struct radeon_device *rdev)
 	rdev->ring[R600_RING_TYPE_DMA_INDEX].ring_obj = NULL;
 	r600_ring_init(rdev, &rdev->ring[R600_RING_TYPE_DMA_INDEX], 64 * 1024);
 
+	r = radeon_uvd_init(rdev);
+	if (!r) {
+		rdev->ring[R600_RING_TYPE_UVD_INDEX].ring_obj = NULL;
+		r600_ring_init(rdev, &rdev->ring[R600_RING_TYPE_UVD_INDEX],
+			       4096);
+	}
+
 	rdev->ih.ring_obj = NULL;
 	r600_ih_ring_init(rdev, 64 * 1024);
 
@@ -1224,6 +2098,7 @@ void rv770_fini(struct radeon_device *rdev)
 	radeon_ib_pool_fini(rdev);
 	radeon_irq_kms_fini(rdev);
 	rv770_pcie_gart_fini(rdev);
+	radeon_uvd_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
 	radeon_fence_driver_fini(rdev);
@@ -1264,23 +2139,23 @@ static void rv770_pcie_gen2_enable(struct radeon_device *rdev)
 	DRM_INFO("enabling PCIE gen 2 link speeds, disable with radeon.pcie_gen2=0\n");
 
 	/* advertise upconfig capability */
-	link_width_cntl = RREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL);
+	link_width_cntl = RREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL);
 	link_width_cntl &= ~LC_UPCONFIGURE_DIS;
-	WREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
-	link_width_cntl = RREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL);
+	WREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
+	link_width_cntl = RREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL);
 	if (link_width_cntl & LC_RENEGOTIATION_SUPPORT) {
 		lanes = (link_width_cntl & LC_LINK_WIDTH_RD_MASK) >> LC_LINK_WIDTH_RD_SHIFT;
 		link_width_cntl &= ~(LC_LINK_WIDTH_MASK |
 				     LC_RECONFIG_ARC_MISSING_ESCAPE);
 		link_width_cntl |= lanes | LC_RECONFIG_NOW |
 			LC_RENEGOTIATE_EN | LC_UPCONFIGURE_SUPPORT;
-		WREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
 	} else {
 		link_width_cntl |= LC_UPCONFIGURE_DIS;
-		WREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
 	}
 
-	speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
+	speed_cntl = RREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL);
 	if ((speed_cntl & LC_OTHER_SIDE_EVER_SENT_GEN2) &&
 	    (speed_cntl & LC_OTHER_SIDE_SUPPORTS_GEN2)) {
 
@@ -1293,29 +2168,29 @@ static void rv770_pcie_gen2_enable(struct radeon_device *rdev)
 		WREG16(0x4088, link_cntl2);
 		WREG32(MM_CFGREGS_CNTL, 0);
 
-		speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
+		speed_cntl = RREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL);
 		speed_cntl &= ~LC_TARGET_LINK_SPEED_OVERRIDE_EN;
-		WREG32_PCIE_P(PCIE_LC_SPEED_CNTL, speed_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL, speed_cntl);
 
-		speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
+		speed_cntl = RREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL);
 		speed_cntl |= LC_CLR_FAILED_SPD_CHANGE_CNT;
-		WREG32_PCIE_P(PCIE_LC_SPEED_CNTL, speed_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL, speed_cntl);
 
-		speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
+		speed_cntl = RREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL);
 		speed_cntl &= ~LC_CLR_FAILED_SPD_CHANGE_CNT;
-		WREG32_PCIE_P(PCIE_LC_SPEED_CNTL, speed_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL, speed_cntl);
 
-		speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
+		speed_cntl = RREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL);
 		speed_cntl |= LC_GEN2_EN_STRAP;
-		WREG32_PCIE_P(PCIE_LC_SPEED_CNTL, speed_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_SPEED_CNTL, speed_cntl);
 
 	} else {
-		link_width_cntl = RREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL);
+		link_width_cntl = RREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL);
 		/* XXX: only disable it if gen1 bridge vendor == 0x111d or 0x1106 */
 		if (1)
 			link_width_cntl |= LC_UPCONFIGURE_DIS;
 		else
 			link_width_cntl &= ~LC_UPCONFIGURE_DIS;
-		WREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
+		WREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL, link_width_cntl);
 	}
 }
diff --git a/drivers/gpu/drm/radeon/rv770d.h b/drivers/gpu/drm/radeon/rv770d.h
index c55f950a4af7..85b16266f748 100644
--- a/drivers/gpu/drm/radeon/rv770d.h
+++ b/drivers/gpu/drm/radeon/rv770d.h
@@ -38,6 +38,30 @@
 #define R7XX_MAX_PIPES             8
 #define R7XX_MAX_PIPES_MASK        0xff
 
+/* discrete uvd clocks */
+#define CG_UPLL_FUNC_CNTL				0x718
+#	define UPLL_RESET_MASK				0x00000001
+#	define UPLL_SLEEP_MASK				0x00000002
+#	define UPLL_BYPASS_EN_MASK			0x00000004
+#	define UPLL_CTLREQ_MASK				0x00000008
+#	define UPLL_REF_DIV(x)				((x) << 16)
+#	define UPLL_REF_DIV_MASK			0x003F0000
+#	define UPLL_CTLACK_MASK				0x40000000
+#	define UPLL_CTLACK2_MASK			0x80000000
+#define CG_UPLL_FUNC_CNTL_2				0x71c
+#	define UPLL_SW_HILEN(x)				((x) << 0)
+#	define UPLL_SW_LOLEN(x)				((x) << 4)
+#	define UPLL_SW_HILEN2(x)			((x) << 8)
+#	define UPLL_SW_LOLEN2(x)			((x) << 12)
+#	define UPLL_SW_MASK				0x0000FFFF
+#	define VCLK_SRC_SEL(x)				((x) << 20)
+#	define VCLK_SRC_SEL_MASK			0x01F00000
+#	define DCLK_SRC_SEL(x)				((x) << 25)
+#	define DCLK_SRC_SEL_MASK			0x3E000000
+#define CG_UPLL_FUNC_CNTL_3				0x720
+#	define UPLL_FB_DIV(x)				((x) << 0)
+#	define UPLL_FB_DIV_MASK				0x01FFFFFF
+
 /* Registers */
 #define	CB_COLOR0_BASE					0x28040
 #define	CB_COLOR1_BASE					0x28044
@@ -112,6 +136,11 @@
 #define DMA_TILING_CONFIG                               0x3ec8
 #define DMA_TILING_CONFIG2                              0xd0b8
 
+/* RV730 only */
+#define UVD_UDEC_TILING_CONFIG                          0xef40
+#define UVD_UDEC_DB_TILING_CONFIG                       0xef44
+#define UVD_UDEC_DBW_TILING_CONFIG                      0xef48
+
 #define	GC_USER_SHADER_PIPE_CONFIG			0x8954
 #define		INACTIVE_QD_PIPES(x)				((x) << 8)
 #define		INACTIVE_QD_PIPES_MASK				0x0000FF00
@@ -671,4 +700,18 @@
 #       define TARGET_LINK_SPEED_MASK                     (0xf << 0)
 #       define SELECTABLE_DEEMPHASIS                      (1 << 6)
 
+/* UVD */
+#define UVD_LMI_EXT40_ADDR				0xf498
+#define UVD_VCPU_CHIP_ID				0xf4d4
+#define UVD_VCPU_CACHE_OFFSET0				0xf4d8
+#define UVD_VCPU_CACHE_SIZE0				0xf4dc
+#define UVD_VCPU_CACHE_OFFSET1				0xf4e0
+#define UVD_VCPU_CACHE_SIZE1				0xf4e4
+#define UVD_VCPU_CACHE_OFFSET2				0xf4e8
+#define UVD_VCPU_CACHE_SIZE2				0xf4ec
+#define UVD_LMI_ADDR_EXT				0xf594
+
+#define UVD_RBC_RB_RPTR					0xf690
+#define UVD_RBC_RB_WPTR					0xf694
+
 #endif
diff --git a/drivers/gpu/drm/radeon/si.c b/drivers/gpu/drm/radeon/si.c
index bafbe3216952..f0b6c2f87c4d 100644
--- a/drivers/gpu/drm/radeon/si.c
+++ b/drivers/gpu/drm/radeon/si.c
@@ -70,6 +70,794 @@ extern u32 evergreen_get_number_of_dram_channels(struct radeon_device *rdev);
 extern void evergreen_print_gpu_status_regs(struct radeon_device *rdev);
 extern bool evergreen_is_display_hung(struct radeon_device *rdev);
 
+static const u32 tahiti_golden_rlc_registers[] =
+{
+	0xc424, 0xffffffff, 0x00601005,
+	0xc47c, 0xffffffff, 0x10104040,
+	0xc488, 0xffffffff, 0x0100000a,
+	0xc314, 0xffffffff, 0x00000800,
+	0xc30c, 0xffffffff, 0x800000f4,
+	0xf4a8, 0xffffffff, 0x00000000
+};
+
+static const u32 tahiti_golden_registers[] =
+{
+	0x9a10, 0x00010000, 0x00018208,
+	0x9830, 0xffffffff, 0x00000000,
+	0x9834, 0xf00fffff, 0x00000400,
+	0x9838, 0x0002021c, 0x00020200,
+	0xc78, 0x00000080, 0x00000000,
+	0xd030, 0x000300c0, 0x00800040,
+	0xd830, 0x000300c0, 0x00800040,
+	0x5bb0, 0x000000f0, 0x00000070,
+	0x5bc0, 0x00200000, 0x50100000,
+	0x7030, 0x31000311, 0x00000011,
+	0x277c, 0x00000003, 0x000007ff,
+	0x240c, 0x000007ff, 0x00000000,
+	0x8a14, 0xf000001f, 0x00000007,
+	0x8b24, 0xffffffff, 0x00ffffff,
+	0x8b10, 0x0000ff0f, 0x00000000,
+	0x28a4c, 0x07ffffff, 0x4e000000,
+	0x28350, 0x3f3f3fff, 0x2a00126a,
+	0x30, 0x000000ff, 0x0040,
+	0x34, 0x00000040, 0x00004040,
+	0x9100, 0x07ffffff, 0x03000000,
+	0x8e88, 0x01ff1f3f, 0x00000000,
+	0x8e84, 0x01ff1f3f, 0x00000000,
+	0x9060, 0x0000007f, 0x00000020,
+	0x9508, 0x00010000, 0x00010000,
+	0xac14, 0x00000200, 0x000002fb,
+	0xac10, 0xffffffff, 0x0000543b,
+	0xac0c, 0xffffffff, 0xa9210876,
+	0x88d0, 0xffffffff, 0x000fff40,
+	0x88d4, 0x0000001f, 0x00000010,
+	0x1410, 0x20000000, 0x20fffed8,
+	0x15c0, 0x000c0fc0, 0x000c0400
+};
+
+static const u32 tahiti_golden_registers2[] =
+{
+	0xc64, 0x00000001, 0x00000001
+};
+
+static const u32 pitcairn_golden_rlc_registers[] =
+{
+	0xc424, 0xffffffff, 0x00601004,
+	0xc47c, 0xffffffff, 0x10102020,
+	0xc488, 0xffffffff, 0x01000020,
+	0xc314, 0xffffffff, 0x00000800,
+	0xc30c, 0xffffffff, 0x800000a4
+};
+
+static const u32 pitcairn_golden_registers[] =
+{
+	0x9a10, 0x00010000, 0x00018208,
+	0x9830, 0xffffffff, 0x00000000,
+	0x9834, 0xf00fffff, 0x00000400,
+	0x9838, 0x0002021c, 0x00020200,
+	0xc78, 0x00000080, 0x00000000,
+	0xd030, 0x000300c0, 0x00800040,
+	0xd830, 0x000300c0, 0x00800040,
+	0x5bb0, 0x000000f0, 0x00000070,
+	0x5bc0, 0x00200000, 0x50100000,
+	0x7030, 0x31000311, 0x00000011,
+	0x2ae4, 0x00073ffe, 0x000022a2,
+	0x240c, 0x000007ff, 0x00000000,
+	0x8a14, 0xf000001f, 0x00000007,
+	0x8b24, 0xffffffff, 0x00ffffff,
+	0x8b10, 0x0000ff0f, 0x00000000,
+	0x28a4c, 0x07ffffff, 0x4e000000,
+	0x28350, 0x3f3f3fff, 0x2a00126a,
+	0x30, 0x000000ff, 0x0040,
+	0x34, 0x00000040, 0x00004040,
+	0x9100, 0x07ffffff, 0x03000000,
+	0x9060, 0x0000007f, 0x00000020,
+	0x9508, 0x00010000, 0x00010000,
+	0xac14, 0x000003ff, 0x000000f7,
+	0xac10, 0xffffffff, 0x00000000,
+	0xac0c, 0xffffffff, 0x32761054,
+	0x88d4, 0x0000001f, 0x00000010,
+	0x15c0, 0x000c0fc0, 0x000c0400
+};
+
+static const u32 verde_golden_rlc_registers[] =
+{
+	0xc424, 0xffffffff, 0x033f1005,
+	0xc47c, 0xffffffff, 0x10808020,
+	0xc488, 0xffffffff, 0x00800008,
+	0xc314, 0xffffffff, 0x00001000,
+	0xc30c, 0xffffffff, 0x80010014
+};
+
+static const u32 verde_golden_registers[] =
+{
+	0x9a10, 0x00010000, 0x00018208,
+	0x9830, 0xffffffff, 0x00000000,
+	0x9834, 0xf00fffff, 0x00000400,
+	0x9838, 0x0002021c, 0x00020200,
+	0xc78, 0x00000080, 0x00000000,
+	0xd030, 0x000300c0, 0x00800040,
+	0xd030, 0x000300c0, 0x00800040,
+	0xd830, 0x000300c0, 0x00800040,
+	0xd830, 0x000300c0, 0x00800040,
+	0x5bb0, 0x000000f0, 0x00000070,
+	0x5bc0, 0x00200000, 0x50100000,
+	0x7030, 0x31000311, 0x00000011,
+	0x2ae4, 0x00073ffe, 0x000022a2,
+	0x2ae4, 0x00073ffe, 0x000022a2,
+	0x2ae4, 0x00073ffe, 0x000022a2,
+	0x240c, 0x000007ff, 0x00000000,
+	0x240c, 0x000007ff, 0x00000000,
+	0x240c, 0x000007ff, 0x00000000,
+	0x8a14, 0xf000001f, 0x00000007,
+	0x8a14, 0xf000001f, 0x00000007,
+	0x8a14, 0xf000001f, 0x00000007,
+	0x8b24, 0xffffffff, 0x00ffffff,
+	0x8b10, 0x0000ff0f, 0x00000000,
+	0x28a4c, 0x07ffffff, 0x4e000000,
+	0x28350, 0x3f3f3fff, 0x0000124a,
+	0x28350, 0x3f3f3fff, 0x0000124a,
+	0x28350, 0x3f3f3fff, 0x0000124a,
+	0x30, 0x000000ff, 0x0040,
+	0x34, 0x00000040, 0x00004040,
+	0x9100, 0x07ffffff, 0x03000000,
+	0x9100, 0x07ffffff, 0x03000000,
+	0x8e88, 0x01ff1f3f, 0x00000000,
+	0x8e88, 0x01ff1f3f, 0x00000000,
+	0x8e88, 0x01ff1f3f, 0x00000000,
+	0x8e84, 0x01ff1f3f, 0x00000000,
+	0x8e84, 0x01ff1f3f, 0x00000000,
+	0x8e84, 0x01ff1f3f, 0x00000000,
+	0x9060, 0x0000007f, 0x00000020,
+	0x9508, 0x00010000, 0x00010000,
+	0xac14, 0x000003ff, 0x00000003,
+	0xac14, 0x000003ff, 0x00000003,
+	0xac14, 0x000003ff, 0x00000003,
+	0xac10, 0xffffffff, 0x00000000,
+	0xac10, 0xffffffff, 0x00000000,
+	0xac10, 0xffffffff, 0x00000000,
+	0xac0c, 0xffffffff, 0x00001032,
+	0xac0c, 0xffffffff, 0x00001032,
+	0xac0c, 0xffffffff, 0x00001032,
+	0x88d4, 0x0000001f, 0x00000010,
+	0x88d4, 0x0000001f, 0x00000010,
+	0x88d4, 0x0000001f, 0x00000010,
+	0x15c0, 0x000c0fc0, 0x000c0400
+};
+
+static const u32 oland_golden_rlc_registers[] =
+{
+	0xc424, 0xffffffff, 0x00601005,
+	0xc47c, 0xffffffff, 0x10104040,
+	0xc488, 0xffffffff, 0x0100000a,
+	0xc314, 0xffffffff, 0x00000800,
+	0xc30c, 0xffffffff, 0x800000f4
+};
+
+static const u32 oland_golden_registers[] =
+{
+	0x9a10, 0x00010000, 0x00018208,
+	0x9830, 0xffffffff, 0x00000000,
+	0x9834, 0xf00fffff, 0x00000400,
+	0x9838, 0x0002021c, 0x00020200,
+	0xc78, 0x00000080, 0x00000000,
+	0xd030, 0x000300c0, 0x00800040,
+	0xd830, 0x000300c0, 0x00800040,
+	0x5bb0, 0x000000f0, 0x00000070,
+	0x5bc0, 0x00200000, 0x50100000,
+	0x7030, 0x31000311, 0x00000011,
+	0x2ae4, 0x00073ffe, 0x000022a2,
+	0x240c, 0x000007ff, 0x00000000,
+	0x8a14, 0xf000001f, 0x00000007,
+	0x8b24, 0xffffffff, 0x00ffffff,
+	0x8b10, 0x0000ff0f, 0x00000000,
+	0x28a4c, 0x07ffffff, 0x4e000000,
+	0x28350, 0x3f3f3fff, 0x00000082,
+	0x30, 0x000000ff, 0x0040,
+	0x34, 0x00000040, 0x00004040,
+	0x9100, 0x07ffffff, 0x03000000,
+	0x9060, 0x0000007f, 0x00000020,
+	0x9508, 0x00010000, 0x00010000,
+	0xac14, 0x000003ff, 0x000000f3,
+	0xac10, 0xffffffff, 0x00000000,
+	0xac0c, 0xffffffff, 0x00003210,
+	0x88d4, 0x0000001f, 0x00000010,
+	0x15c0, 0x000c0fc0, 0x000c0400
+};
+
+static const u32 tahiti_mgcg_cgcg_init[] =
+{
+	0xc400, 0xffffffff, 0xfffffffc,
+	0x802c, 0xffffffff, 0xe0000000,
+	0x9a60, 0xffffffff, 0x00000100,
+	0x92a4, 0xffffffff, 0x00000100,
+	0xc164, 0xffffffff, 0x00000100,
+	0x9774, 0xffffffff, 0x00000100,
+	0x8984, 0xffffffff, 0x06000100,
+	0x8a18, 0xffffffff, 0x00000100,
+	0x92a0, 0xffffffff, 0x00000100,
+	0xc380, 0xffffffff, 0x00000100,
+	0x8b28, 0xffffffff, 0x00000100,
+	0x9144, 0xffffffff, 0x00000100,
+	0x8d88, 0xffffffff, 0x00000100,
+	0x8d8c, 0xffffffff, 0x00000100,
+	0x9030, 0xffffffff, 0x00000100,
+	0x9034, 0xffffffff, 0x00000100,
+	0x9038, 0xffffffff, 0x00000100,
+	0x903c, 0xffffffff, 0x00000100,
+	0xad80, 0xffffffff, 0x00000100,
+	0xac54, 0xffffffff, 0x00000100,
+	0x897c, 0xffffffff, 0x06000100,
+	0x9868, 0xffffffff, 0x00000100,
+	0x9510, 0xffffffff, 0x00000100,
+	0xaf04, 0xffffffff, 0x00000100,
+	0xae04, 0xffffffff, 0x00000100,
+	0x949c, 0xffffffff, 0x00000100,
+	0x802c, 0xffffffff, 0xe0000000,
+	0x9160, 0xffffffff, 0x00010000,
+	0x9164, 0xffffffff, 0x00030002,
+	0x9168, 0xffffffff, 0x00040007,
+	0x916c, 0xffffffff, 0x00060005,
+	0x9170, 0xffffffff, 0x00090008,
+	0x9174, 0xffffffff, 0x00020001,
+	0x9178, 0xffffffff, 0x00040003,
+	0x917c, 0xffffffff, 0x00000007,
+	0x9180, 0xffffffff, 0x00060005,
+	0x9184, 0xffffffff, 0x00090008,
+	0x9188, 0xffffffff, 0x00030002,
+	0x918c, 0xffffffff, 0x00050004,
+	0x9190, 0xffffffff, 0x00000008,
+	0x9194, 0xffffffff, 0x00070006,
+	0x9198, 0xffffffff, 0x000a0009,
+	0x919c, 0xffffffff, 0x00040003,
+	0x91a0, 0xffffffff, 0x00060005,
+	0x91a4, 0xffffffff, 0x00000009,
+	0x91a8, 0xffffffff, 0x00080007,
+	0x91ac, 0xffffffff, 0x000b000a,
+	0x91b0, 0xffffffff, 0x00050004,
+	0x91b4, 0xffffffff, 0x00070006,
+	0x91b8, 0xffffffff, 0x0008000b,
+	0x91bc, 0xffffffff, 0x000a0009,
+	0x91c0, 0xffffffff, 0x000d000c,
+	0x91c4, 0xffffffff, 0x00060005,
+	0x91c8, 0xffffffff, 0x00080007,
+	0x91cc, 0xffffffff, 0x0000000b,
+	0x91d0, 0xffffffff, 0x000a0009,
+	0x91d4, 0xffffffff, 0x000d000c,
+	0x91d8, 0xffffffff, 0x00070006,
+	0x91dc, 0xffffffff, 0x00090008,
+	0x91e0, 0xffffffff, 0x0000000c,
+	0x91e4, 0xffffffff, 0x000b000a,
+	0x91e8, 0xffffffff, 0x000e000d,
+	0x91ec, 0xffffffff, 0x00080007,
+	0x91f0, 0xffffffff, 0x000a0009,
+	0x91f4, 0xffffffff, 0x0000000d,
+	0x91f8, 0xffffffff, 0x000c000b,
+	0x91fc, 0xffffffff, 0x000f000e,
+	0x9200, 0xffffffff, 0x00090008,
+	0x9204, 0xffffffff, 0x000b000a,
+	0x9208, 0xffffffff, 0x000c000f,
+	0x920c, 0xffffffff, 0x000e000d,
+	0x9210, 0xffffffff, 0x00110010,
+	0x9214, 0xffffffff, 0x000a0009,
+	0x9218, 0xffffffff, 0x000c000b,
+	0x921c, 0xffffffff, 0x0000000f,
+	0x9220, 0xffffffff, 0x000e000d,
+	0x9224, 0xffffffff, 0x00110010,
+	0x9228, 0xffffffff, 0x000b000a,
+	0x922c, 0xffffffff, 0x000d000c,
+	0x9230, 0xffffffff, 0x00000010,
+	0x9234, 0xffffffff, 0x000f000e,
+	0x9238, 0xffffffff, 0x00120011,
+	0x923c, 0xffffffff, 0x000c000b,
+	0x9240, 0xffffffff, 0x000e000d,
+	0x9244, 0xffffffff, 0x00000011,
+	0x9248, 0xffffffff, 0x0010000f,
+	0x924c, 0xffffffff, 0x00130012,
+	0x9250, 0xffffffff, 0x000d000c,
+	0x9254, 0xffffffff, 0x000f000e,
+	0x9258, 0xffffffff, 0x00100013,
+	0x925c, 0xffffffff, 0x00120011,
+	0x9260, 0xffffffff, 0x00150014,
+	0x9264, 0xffffffff, 0x000e000d,
+	0x9268, 0xffffffff, 0x0010000f,
+	0x926c, 0xffffffff, 0x00000013,
+	0x9270, 0xffffffff, 0x00120011,
+	0x9274, 0xffffffff, 0x00150014,
+	0x9278, 0xffffffff, 0x000f000e,
+	0x927c, 0xffffffff, 0x00110010,
+	0x9280, 0xffffffff, 0x00000014,
+	0x9284, 0xffffffff, 0x00130012,
+	0x9288, 0xffffffff, 0x00160015,
+	0x928c, 0xffffffff, 0x0010000f,
+	0x9290, 0xffffffff, 0x00120011,
+	0x9294, 0xffffffff, 0x00000015,
+	0x9298, 0xffffffff, 0x00140013,
+	0x929c, 0xffffffff, 0x00170016,
+	0x9150, 0xffffffff, 0x96940200,
+	0x8708, 0xffffffff, 0x00900100,
+	0xc478, 0xffffffff, 0x00000080,
+	0xc404, 0xffffffff, 0x0020003f,
+	0x30, 0xffffffff, 0x0000001c,
+	0x34, 0x000f0000, 0x000f0000,
+	0x160c, 0xffffffff, 0x00000100,
+	0x1024, 0xffffffff, 0x00000100,
+	0x102c, 0x00000101, 0x00000000,
+	0x20a8, 0xffffffff, 0x00000104,
+	0x264c, 0x000c0000, 0x000c0000,
+	0x2648, 0x000c0000, 0x000c0000,
+	0x55e4, 0xff000fff, 0x00000100,
+	0x55e8, 0x00000001, 0x00000001,
+	0x2f50, 0x00000001, 0x00000001,
+	0x30cc, 0xc0000fff, 0x00000104,
+	0xc1e4, 0x00000001, 0x00000001,
+	0xd0c0, 0xfffffff0, 0x00000100,
+	0xd8c0, 0xfffffff0, 0x00000100
+};
+
+static const u32 pitcairn_mgcg_cgcg_init[] =
+{
+	0xc400, 0xffffffff, 0xfffffffc,
+	0x802c, 0xffffffff, 0xe0000000,
+	0x9a60, 0xffffffff, 0x00000100,
+	0x92a4, 0xffffffff, 0x00000100,
+	0xc164, 0xffffffff, 0x00000100,
+	0x9774, 0xffffffff, 0x00000100,
+	0x8984, 0xffffffff, 0x06000100,
+	0x8a18, 0xffffffff, 0x00000100,
+	0x92a0, 0xffffffff, 0x00000100,
+	0xc380, 0xffffffff, 0x00000100,
+	0x8b28, 0xffffffff, 0x00000100,
+	0x9144, 0xffffffff, 0x00000100,
+	0x8d88, 0xffffffff, 0x00000100,
+	0x8d8c, 0xffffffff, 0x00000100,
+	0x9030, 0xffffffff, 0x00000100,
+	0x9034, 0xffffffff, 0x00000100,
+	0x9038, 0xffffffff, 0x00000100,
+	0x903c, 0xffffffff, 0x00000100,
+	0xad80, 0xffffffff, 0x00000100,
+	0xac54, 0xffffffff, 0x00000100,
+	0x897c, 0xffffffff, 0x06000100,
+	0x9868, 0xffffffff, 0x00000100,
+	0x9510, 0xffffffff, 0x00000100,
+	0xaf04, 0xffffffff, 0x00000100,
+	0xae04, 0xffffffff, 0x00000100,
+	0x949c, 0xffffffff, 0x00000100,
+	0x802c, 0xffffffff, 0xe0000000,
+	0x9160, 0xffffffff, 0x00010000,
+	0x9164, 0xffffffff, 0x00030002,
+	0x9168, 0xffffffff, 0x00040007,
+	0x916c, 0xffffffff, 0x00060005,
+	0x9170, 0xffffffff, 0x00090008,
+	0x9174, 0xffffffff, 0x00020001,
+	0x9178, 0xffffffff, 0x00040003,
+	0x917c, 0xffffffff, 0x00000007,
+	0x9180, 0xffffffff, 0x00060005,
+	0x9184, 0xffffffff, 0x00090008,
+	0x9188, 0xffffffff, 0x00030002,
+	0x918c, 0xffffffff, 0x00050004,
+	0x9190, 0xffffffff, 0x00000008,
+	0x9194, 0xffffffff, 0x00070006,
+	0x9198, 0xffffffff, 0x000a0009,
+	0x919c, 0xffffffff, 0x00040003,
+	0x91a0, 0xffffffff, 0x00060005,
+	0x91a4, 0xffffffff, 0x00000009,
+	0x91a8, 0xffffffff, 0x00080007,
+	0x91ac, 0xffffffff, 0x000b000a,
+	0x91b0, 0xffffffff, 0x00050004,
+	0x91b4, 0xffffffff, 0x00070006,
+	0x91b8, 0xffffffff, 0x0008000b,
+	0x91bc, 0xffffffff, 0x000a0009,
+	0x91c0, 0xffffffff, 0x000d000c,
+	0x9200, 0xffffffff, 0x00090008,
+	0x9204, 0xffffffff, 0x000b000a,
+	0x9208, 0xffffffff, 0x000c000f,
+	0x920c, 0xffffffff, 0x000e000d,
+	0x9210, 0xffffffff, 0x00110010,
+	0x9214, 0xffffffff, 0x000a0009,
+	0x9218, 0xffffffff, 0x000c000b,
+	0x921c, 0xffffffff, 0x0000000f,
+	0x9220, 0xffffffff, 0x000e000d,
+	0x9224, 0xffffffff, 0x00110010,
+	0x9228, 0xffffffff, 0x000b000a,
+	0x922c, 0xffffffff, 0x000d000c,
+	0x9230, 0xffffffff, 0x00000010,
+	0x9234, 0xffffffff, 0x000f000e,
+	0x9238, 0xffffffff, 0x00120011,
+	0x923c, 0xffffffff, 0x000c000b,
+	0x9240, 0xffffffff, 0x000e000d,
+	0x9244, 0xffffffff, 0x00000011,
+	0x9248, 0xffffffff, 0x0010000f,
+	0x924c, 0xffffffff, 0x00130012,
+	0x9250, 0xffffffff, 0x000d000c,
+	0x9254, 0xffffffff, 0x000f000e,
+	0x9258, 0xffffffff, 0x00100013,
+	0x925c, 0xffffffff, 0x00120011,
+	0x9260, 0xffffffff, 0x00150014,
+	0x9150, 0xffffffff, 0x96940200,
+	0x8708, 0xffffffff, 0x00900100,
+	0xc478, 0xffffffff, 0x00000080,
+	0xc404, 0xffffffff, 0x0020003f,
+	0x30, 0xffffffff, 0x0000001c,
+	0x34, 0x000f0000, 0x000f0000,
+	0x160c, 0xffffffff, 0x00000100,
+	0x1024, 0xffffffff, 0x00000100,
+	0x102c, 0x00000101, 0x00000000,
+	0x20a8, 0xffffffff, 0x00000104,
+	0x55e4, 0xff000fff, 0x00000100,
+	0x55e8, 0x00000001, 0x00000001,
+	0x2f50, 0x00000001, 0x00000001,
+	0x30cc, 0xc0000fff, 0x00000104,
+	0xc1e4, 0x00000001, 0x00000001,
+	0xd0c0, 0xfffffff0, 0x00000100,
+	0xd8c0, 0xfffffff0, 0x00000100
+};
+
+static const u32 verde_mgcg_cgcg_init[] =
+{
+	0xc400, 0xffffffff, 0xfffffffc,
+	0x802c, 0xffffffff, 0xe0000000,
+	0x9a60, 0xffffffff, 0x00000100,
+	0x92a4, 0xffffffff, 0x00000100,
+	0xc164, 0xffffffff, 0x00000100,
+	0x9774, 0xffffffff, 0x00000100,
+	0x8984, 0xffffffff, 0x06000100,
+	0x8a18, 0xffffffff, 0x00000100,
+	0x92a0, 0xffffffff, 0x00000100,
+	0xc380, 0xffffffff, 0x00000100,
+	0x8b28, 0xffffffff, 0x00000100,
+	0x9144, 0xffffffff, 0x00000100,
+	0x8d88, 0xffffffff, 0x00000100,
+	0x8d8c, 0xffffffff, 0x00000100,
+	0x9030, 0xffffffff, 0x00000100,
+	0x9034, 0xffffffff, 0x00000100,
+	0x9038, 0xffffffff, 0x00000100,
+	0x903c, 0xffffffff, 0x00000100,
+	0xad80, 0xffffffff, 0x00000100,
+	0xac54, 0xffffffff, 0x00000100,
+	0x897c, 0xffffffff, 0x06000100,
+	0x9868, 0xffffffff, 0x00000100,
+	0x9510, 0xffffffff, 0x00000100,
+	0xaf04, 0xffffffff, 0x00000100,
+	0xae04, 0xffffffff, 0x00000100,
+	0x949c, 0xffffffff, 0x00000100,
+	0x802c, 0xffffffff, 0xe0000000,
+	0x9160, 0xffffffff, 0x00010000,
+	0x9164, 0xffffffff, 0x00030002,
+	0x9168, 0xffffffff, 0x00040007,
+	0x916c, 0xffffffff, 0x00060005,
+	0x9170, 0xffffffff, 0x00090008,
+	0x9174, 0xffffffff, 0x00020001,
+	0x9178, 0xffffffff, 0x00040003,
+	0x917c, 0xffffffff, 0x00000007,
+	0x9180, 0xffffffff, 0x00060005,
+	0x9184, 0xffffffff, 0x00090008,
+	0x9188, 0xffffffff, 0x00030002,
+	0x918c, 0xffffffff, 0x00050004,
+	0x9190, 0xffffffff, 0x00000008,
+	0x9194, 0xffffffff, 0x00070006,
+	0x9198, 0xffffffff, 0x000a0009,
+	0x919c, 0xffffffff, 0x00040003,
+	0x91a0, 0xffffffff, 0x00060005,
+	0x91a4, 0xffffffff, 0x00000009,
+	0x91a8, 0xffffffff, 0x00080007,
+	0x91ac, 0xffffffff, 0x000b000a,
+	0x91b0, 0xffffffff, 0x00050004,
+	0x91b4, 0xffffffff, 0x00070006,
+	0x91b8, 0xffffffff, 0x0008000b,
+	0x91bc, 0xffffffff, 0x000a0009,
+	0x91c0, 0xffffffff, 0x000d000c,
+	0x9200, 0xffffffff, 0x00090008,
+	0x9204, 0xffffffff, 0x000b000a,
+	0x9208, 0xffffffff, 0x000c000f,
+	0x920c, 0xffffffff, 0x000e000d,
+	0x9210, 0xffffffff, 0x00110010,
+	0x9214, 0xffffffff, 0x000a0009,
+	0x9218, 0xffffffff, 0x000c000b,
+	0x921c, 0xffffffff, 0x0000000f,
+	0x9220, 0xffffffff, 0x000e000d,
+	0x9224, 0xffffffff, 0x00110010,
+	0x9228, 0xffffffff, 0x000b000a,
+	0x922c, 0xffffffff, 0x000d000c,
+	0x9230, 0xffffffff, 0x00000010,
+	0x9234, 0xffffffff, 0x000f000e,
+	0x9238, 0xffffffff, 0x00120011,
+	0x923c, 0xffffffff, 0x000c000b,
+	0x9240, 0xffffffff, 0x000e000d,
+	0x9244, 0xffffffff, 0x00000011,
+	0x9248, 0xffffffff, 0x0010000f,
+	0x924c, 0xffffffff, 0x00130012,
+	0x9250, 0xffffffff, 0x000d000c,
+	0x9254, 0xffffffff, 0x000f000e,
+	0x9258, 0xffffffff, 0x00100013,
+	0x925c, 0xffffffff, 0x00120011,
+	0x9260, 0xffffffff, 0x00150014,
+	0x9150, 0xffffffff, 0x96940200,
+	0x8708, 0xffffffff, 0x00900100,
+	0xc478, 0xffffffff, 0x00000080,
+	0xc404, 0xffffffff, 0x0020003f,
+	0x30, 0xffffffff, 0x0000001c,
+	0x34, 0x000f0000, 0x000f0000,
+	0x160c, 0xffffffff, 0x00000100,
+	0x1024, 0xffffffff, 0x00000100,
+	0x102c, 0x00000101, 0x00000000,
+	0x20a8, 0xffffffff, 0x00000104,
+	0x264c, 0x000c0000, 0x000c0000,
+	0x2648, 0x000c0000, 0x000c0000,
+	0x55e4, 0xff000fff, 0x00000100,
+	0x55e8, 0x00000001, 0x00000001,
+	0x2f50, 0x00000001, 0x00000001,
+	0x30cc, 0xc0000fff, 0x00000104,
+	0xc1e4, 0x00000001, 0x00000001,
+	0xd0c0, 0xfffffff0, 0x00000100,
+	0xd8c0, 0xfffffff0, 0x00000100
+};
+
+static const u32 oland_mgcg_cgcg_init[] =
+{
+	0xc400, 0xffffffff, 0xfffffffc,
+	0x802c, 0xffffffff, 0xe0000000,
+	0x9a60, 0xffffffff, 0x00000100,
+	0x92a4, 0xffffffff, 0x00000100,
+	0xc164, 0xffffffff, 0x00000100,
+	0x9774, 0xffffffff, 0x00000100,
+	0x8984, 0xffffffff, 0x06000100,
+	0x8a18, 0xffffffff, 0x00000100,
+	0x92a0, 0xffffffff, 0x00000100,
+	0xc380, 0xffffffff, 0x00000100,
+	0x8b28, 0xffffffff, 0x00000100,
+	0x9144, 0xffffffff, 0x00000100,
+	0x8d88, 0xffffffff, 0x00000100,
+	0x8d8c, 0xffffffff, 0x00000100,
+	0x9030, 0xffffffff, 0x00000100,
+	0x9034, 0xffffffff, 0x00000100,
+	0x9038, 0xffffffff, 0x00000100,
+	0x903c, 0xffffffff, 0x00000100,
+	0xad80, 0xffffffff, 0x00000100,
+	0xac54, 0xffffffff, 0x00000100,
+	0x897c, 0xffffffff, 0x06000100,
+	0x9868, 0xffffffff, 0x00000100,
+	0x9510, 0xffffffff, 0x00000100,
+	0xaf04, 0xffffffff, 0x00000100,
+	0xae04, 0xffffffff, 0x00000100,
+	0x949c, 0xffffffff, 0x00000100,
+	0x802c, 0xffffffff, 0xe0000000,
+	0x9160, 0xffffffff, 0x00010000,
+	0x9164, 0xffffffff, 0x00030002,
+	0x9168, 0xffffffff, 0x00040007,
+	0x916c, 0xffffffff, 0x00060005,
+	0x9170, 0xffffffff, 0x00090008,
+	0x9174, 0xffffffff, 0x00020001,
+	0x9178, 0xffffffff, 0x00040003,
+	0x917c, 0xffffffff, 0x00000007,
+	0x9180, 0xffffffff, 0x00060005,
+	0x9184, 0xffffffff, 0x00090008,
+	0x9188, 0xffffffff, 0x00030002,
+	0x918c, 0xffffffff, 0x00050004,
+	0x9190, 0xffffffff, 0x00000008,
+	0x9194, 0xffffffff, 0x00070006,
+	0x9198, 0xffffffff, 0x000a0009,
+	0x919c, 0xffffffff, 0x00040003,
+	0x91a0, 0xffffffff, 0x00060005,
+	0x91a4, 0xffffffff, 0x00000009,
+	0x91a8, 0xffffffff, 0x00080007,
+	0x91ac, 0xffffffff, 0x000b000a,
+	0x91b0, 0xffffffff, 0x00050004,
+	0x91b4, 0xffffffff, 0x00070006,
+	0x91b8, 0xffffffff, 0x0008000b,
+	0x91bc, 0xffffffff, 0x000a0009,
+	0x91c0, 0xffffffff, 0x000d000c,
+	0x91c4, 0xffffffff, 0x00060005,
+	0x91c8, 0xffffffff, 0x00080007,
+	0x91cc, 0xffffffff, 0x0000000b,
+	0x91d0, 0xffffffff, 0x000a0009,
+	0x91d4, 0xffffffff, 0x000d000c,
+	0x9150, 0xffffffff, 0x96940200,
+	0x8708, 0xffffffff, 0x00900100,
+	0xc478, 0xffffffff, 0x00000080,
+	0xc404, 0xffffffff, 0x0020003f,
+	0x30, 0xffffffff, 0x0000001c,
+	0x34, 0x000f0000, 0x000f0000,
+	0x160c, 0xffffffff, 0x00000100,
+	0x1024, 0xffffffff, 0x00000100,
+	0x102c, 0x00000101, 0x00000000,
+	0x20a8, 0xffffffff, 0x00000104,
+	0x264c, 0x000c0000, 0x000c0000,
+	0x2648, 0x000c0000, 0x000c0000,
+	0x55e4, 0xff000fff, 0x00000100,
+	0x55e8, 0x00000001, 0x00000001,
+	0x2f50, 0x00000001, 0x00000001,
+	0x30cc, 0xc0000fff, 0x00000104,
+	0xc1e4, 0x00000001, 0x00000001,
+	0xd0c0, 0xfffffff0, 0x00000100,
+	0xd8c0, 0xfffffff0, 0x00000100
+};
+
+static u32 verde_pg_init[] =
+{
+	0x353c, 0xffffffff, 0x40000,
+	0x3538, 0xffffffff, 0x200010ff,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x7007,
+	0x3538, 0xffffffff, 0x300010ff,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x400000,
+	0x3538, 0xffffffff, 0x100010ff,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x120200,
+	0x3538, 0xffffffff, 0x500010ff,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x1e1e16,
+	0x3538, 0xffffffff, 0x600010ff,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x171f1e,
+	0x3538, 0xffffffff, 0x700010ff,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x353c, 0xffffffff, 0x0,
+	0x3538, 0xffffffff, 0x9ff,
+	0x3500, 0xffffffff, 0x0,
+	0x3504, 0xffffffff, 0x10000800,
+	0x3504, 0xffffffff, 0xf,
+	0x3504, 0xffffffff, 0xf,
+	0x3500, 0xffffffff, 0x4,
+	0x3504, 0xffffffff, 0x1000051e,
+	0x3504, 0xffffffff, 0xffff,
+	0x3504, 0xffffffff, 0xffff,
+	0x3500, 0xffffffff, 0x8,
+	0x3504, 0xffffffff, 0x80500,
+	0x3500, 0xffffffff, 0x12,
+	0x3504, 0xffffffff, 0x9050c,
+	0x3500, 0xffffffff, 0x1d,
+	0x3504, 0xffffffff, 0xb052c,
+	0x3500, 0xffffffff, 0x2a,
+	0x3504, 0xffffffff, 0x1053e,
+	0x3500, 0xffffffff, 0x2d,
+	0x3504, 0xffffffff, 0x10546,
+	0x3500, 0xffffffff, 0x30,
+	0x3504, 0xffffffff, 0xa054e,
+	0x3500, 0xffffffff, 0x3c,
+	0x3504, 0xffffffff, 0x1055f,
+	0x3500, 0xffffffff, 0x3f,
+	0x3504, 0xffffffff, 0x10567,
+	0x3500, 0xffffffff, 0x42,
+	0x3504, 0xffffffff, 0x1056f,
+	0x3500, 0xffffffff, 0x45,
+	0x3504, 0xffffffff, 0x10572,
+	0x3500, 0xffffffff, 0x48,
+	0x3504, 0xffffffff, 0x20575,
+	0x3500, 0xffffffff, 0x4c,
+	0x3504, 0xffffffff, 0x190801,
+	0x3500, 0xffffffff, 0x67,
+	0x3504, 0xffffffff, 0x1082a,
+	0x3500, 0xffffffff, 0x6a,
+	0x3504, 0xffffffff, 0x1b082d,
+	0x3500, 0xffffffff, 0x87,
+	0x3504, 0xffffffff, 0x310851,
+	0x3500, 0xffffffff, 0xba,
+	0x3504, 0xffffffff, 0x891,
+	0x3500, 0xffffffff, 0xbc,
+	0x3504, 0xffffffff, 0x893,
+	0x3500, 0xffffffff, 0xbe,
+	0x3504, 0xffffffff, 0x20895,
+	0x3500, 0xffffffff, 0xc2,
+	0x3504, 0xffffffff, 0x20899,
+	0x3500, 0xffffffff, 0xc6,
+	0x3504, 0xffffffff, 0x2089d,
+	0x3500, 0xffffffff, 0xca,
+	0x3504, 0xffffffff, 0x8a1,
+	0x3500, 0xffffffff, 0xcc,
+	0x3504, 0xffffffff, 0x8a3,
+	0x3500, 0xffffffff, 0xce,
+	0x3504, 0xffffffff, 0x308a5,
+	0x3500, 0xffffffff, 0xd3,
+	0x3504, 0xffffffff, 0x6d08cd,
+	0x3500, 0xffffffff, 0x142,
+	0x3504, 0xffffffff, 0x2000095a,
+	0x3504, 0xffffffff, 0x1,
+	0x3500, 0xffffffff, 0x144,
+	0x3504, 0xffffffff, 0x301f095b,
+	0x3500, 0xffffffff, 0x165,
+	0x3504, 0xffffffff, 0xc094d,
+	0x3500, 0xffffffff, 0x173,
+	0x3504, 0xffffffff, 0xf096d,
+	0x3500, 0xffffffff, 0x184,
+	0x3504, 0xffffffff, 0x15097f,
+	0x3500, 0xffffffff, 0x19b,
+	0x3504, 0xffffffff, 0xc0998,
+	0x3500, 0xffffffff, 0x1a9,
+	0x3504, 0xffffffff, 0x409a7,
+	0x3500, 0xffffffff, 0x1af,
+	0x3504, 0xffffffff, 0xcdc,
+	0x3500, 0xffffffff, 0x1b1,
+	0x3504, 0xffffffff, 0x800,
+	0x3508, 0xffffffff, 0x6c9b2000,
+	0x3510, 0xfc00, 0x2000,
+	0x3544, 0xffffffff, 0xfc0,
+	0x28d4, 0x00000100, 0x100
+};
+
+static void si_init_golden_registers(struct radeon_device *rdev)
+{
+	switch (rdev->family) {
+	case CHIP_TAHITI:
+		radeon_program_register_sequence(rdev,
+						 tahiti_golden_registers,
+						 (const u32)ARRAY_SIZE(tahiti_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 tahiti_golden_rlc_registers,
+						 (const u32)ARRAY_SIZE(tahiti_golden_rlc_registers));
+		radeon_program_register_sequence(rdev,
+						 tahiti_mgcg_cgcg_init,
+						 (const u32)ARRAY_SIZE(tahiti_mgcg_cgcg_init));
+		radeon_program_register_sequence(rdev,
+						 tahiti_golden_registers2,
+						 (const u32)ARRAY_SIZE(tahiti_golden_registers2));
+		break;
+	case CHIP_PITCAIRN:
+		radeon_program_register_sequence(rdev,
+						 pitcairn_golden_registers,
+						 (const u32)ARRAY_SIZE(pitcairn_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 pitcairn_golden_rlc_registers,
+						 (const u32)ARRAY_SIZE(pitcairn_golden_rlc_registers));
+		radeon_program_register_sequence(rdev,
+						 pitcairn_mgcg_cgcg_init,
+						 (const u32)ARRAY_SIZE(pitcairn_mgcg_cgcg_init));
+		break;
+	case CHIP_VERDE:
+		radeon_program_register_sequence(rdev,
+						 verde_golden_registers,
+						 (const u32)ARRAY_SIZE(verde_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 verde_golden_rlc_registers,
+						 (const u32)ARRAY_SIZE(verde_golden_rlc_registers));
+		radeon_program_register_sequence(rdev,
+						 verde_mgcg_cgcg_init,
+						 (const u32)ARRAY_SIZE(verde_mgcg_cgcg_init));
+		radeon_program_register_sequence(rdev,
+						 verde_pg_init,
+						 (const u32)ARRAY_SIZE(verde_pg_init));
+		break;
+	case CHIP_OLAND:
+		radeon_program_register_sequence(rdev,
+						 oland_golden_registers,
+						 (const u32)ARRAY_SIZE(oland_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 oland_golden_rlc_registers,
+						 (const u32)ARRAY_SIZE(oland_golden_rlc_registers));
+		radeon_program_register_sequence(rdev,
+						 oland_mgcg_cgcg_init,
+						 (const u32)ARRAY_SIZE(oland_mgcg_cgcg_init));
+		break;
+	default:
+		break;
+	}
+}
+
 #define PCIE_BUS_CLK                10000
 #define TCLK                        (PCIE_BUS_CLK / 10)
 
@@ -1211,6 +1999,7 @@ static void si_tiling_mode_table_init(struct radeon_device *rdev)
 				gb_tile_moden = 0;
 				break;
 			}
+			rdev->config.si.tile_mode_array[reg_offset] = gb_tile_moden;
 			WREG32(GB_TILE_MODE0 + (reg_offset * 4), gb_tile_moden);
 		}
 	} else if ((rdev->family == CHIP_VERDE) ||
@@ -1451,6 +2240,7 @@ static void si_tiling_mode_table_init(struct radeon_device *rdev)
 				gb_tile_moden = 0;
 				break;
 			}
+			rdev->config.si.tile_mode_array[reg_offset] = gb_tile_moden;
 			WREG32(GB_TILE_MODE0 + (reg_offset * 4), gb_tile_moden);
 		}
 	} else
@@ -1463,7 +2253,7 @@ static void si_select_se_sh(struct radeon_device *rdev,
 	u32 data = INSTANCE_BROADCAST_WRITES;
 
 	if ((se_num == 0xffffffff) && (sh_num == 0xffffffff))
-		data = SH_BROADCAST_WRITES | SE_BROADCAST_WRITES;
+		data |= SH_BROADCAST_WRITES | SE_BROADCAST_WRITES;
 	else if (se_num == 0xffffffff)
 		data |= SE_BROADCAST_WRITES | SH_INDEX(sh_num);
 	else if (sh_num == 0xffffffff)
@@ -1765,9 +2555,13 @@ static void si_gpu_init(struct radeon_device *rdev)
 
 	WREG32(GB_ADDR_CONFIG, gb_addr_config);
 	WREG32(DMIF_ADDR_CONFIG, gb_addr_config);
+	WREG32(DMIF_ADDR_CALC, gb_addr_config);
 	WREG32(HDP_ADDR_CONFIG, gb_addr_config);
 	WREG32(DMA_TILING_CONFIG + DMA0_REGISTER_OFFSET, gb_addr_config);
 	WREG32(DMA_TILING_CONFIG + DMA1_REGISTER_OFFSET, gb_addr_config);
+	WREG32(UVD_UDEC_ADDR_CONFIG, gb_addr_config);
+	WREG32(UVD_UDEC_DB_ADDR_CONFIG, gb_addr_config);
+	WREG32(UVD_UDEC_DBW_ADDR_CONFIG, gb_addr_config);
 
 	si_tiling_mode_table_init(rdev);
 
@@ -2538,46 +3332,6 @@ static void si_mc_program(struct radeon_device *rdev)
 	rv515_vga_render_disable(rdev);
 }
 
-/* SI MC address space is 40 bits */
-static void si_vram_location(struct radeon_device *rdev,
-			     struct radeon_mc *mc, u64 base)
-{
-	mc->vram_start = base;
-	if (mc->mc_vram_size > (0xFFFFFFFFFFULL - base + 1)) {
-		dev_warn(rdev->dev, "limiting VRAM to PCI aperture size\n");
-		mc->real_vram_size = mc->aper_size;
-		mc->mc_vram_size = mc->aper_size;
-	}
-	mc->vram_end = mc->vram_start + mc->mc_vram_size - 1;
-	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);
-}
-
-static void si_gtt_location(struct radeon_device *rdev, struct radeon_mc *mc)
-{
-	u64 size_af, size_bf;
-
-	size_af = ((0xFFFFFFFFFFULL - mc->vram_end) + mc->gtt_base_align) & ~mc->gtt_base_align;
-	size_bf = mc->vram_start & ~mc->gtt_base_align;
-	if (size_bf > size_af) {
-		if (mc->gtt_size > size_bf) {
-			dev_warn(rdev->dev, "limiting GTT\n");
-			mc->gtt_size = size_bf;
-		}
-		mc->gtt_start = (mc->vram_start & ~mc->gtt_base_align) - mc->gtt_size;
-	} else {
-		if (mc->gtt_size > size_af) {
-			dev_warn(rdev->dev, "limiting GTT\n");
-			mc->gtt_size = size_af;
-		}
-		mc->gtt_start = (mc->vram_end + 1 + mc->gtt_base_align) & ~mc->gtt_base_align;
-	}
-	mc->gtt_end = mc->gtt_start + mc->gtt_size - 1;
-	dev_info(rdev->dev, "GTT: %lluM 0x%016llX - 0x%016llX\n",
-			mc->gtt_size >> 20, mc->gtt_start, mc->gtt_end);
-}
-
 static void si_vram_gtt_location(struct radeon_device *rdev,
 				 struct radeon_mc *mc)
 {
@@ -2587,9 +3341,9 @@ static void si_vram_gtt_location(struct radeon_device *rdev,
 		mc->real_vram_size = 0xFFC0000000ULL;
 		mc->mc_vram_size = 0xFFC0000000ULL;
 	}
-	si_vram_location(rdev, &rdev->mc, 0);
+	radeon_vram_location(rdev, &rdev->mc, 0);
 	rdev->mc.gtt_base_align = 0;
-	si_gtt_location(rdev, mc);
+	radeon_gtt_location(rdev, mc);
 }
 
 static int si_mc_init(struct radeon_device *rdev)
@@ -4322,14 +5076,6 @@ static int si_startup(struct radeon_device *rdev)
 		return r;
 	si_gpu_init(rdev);
 
-#if 0
-	r = evergreen_blit_init(rdev);
-	if (r) {
-		r600_blit_fini(rdev);
-		rdev->asic->copy = NULL;
-		dev_warn(rdev->dev, "failed blitter (%d) falling back to memcpy\n", r);
-	}
-#endif
 	/* allocate rlc buffers */
 	r = si_rlc_init(rdev);
 	if (r) {
@@ -4372,6 +5118,16 @@ static int si_startup(struct radeon_device *rdev)
 		return r;
 	}
 
+	r = rv770_uvd_resume(rdev);
+	if (!r) {
+		r = radeon_fence_driver_start_ring(rdev,
+						   R600_RING_TYPE_UVD_INDEX);
+		if (r)
+			dev_err(rdev->dev, "UVD fences init error (%d).\n", r);
+	}
+	if (r)
+		rdev->ring[R600_RING_TYPE_UVD_INDEX].ring_size = 0;
+
 	/* Enable IRQ */
 	r = si_irq_init(rdev);
 	if (r) {
@@ -4429,6 +5185,18 @@ static int si_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	ring = &rdev->ring[R600_RING_TYPE_UVD_INDEX];
+	if (ring->ring_size) {
+		r = radeon_ring_init(rdev, ring, ring->ring_size,
+				     R600_WB_UVD_RPTR_OFFSET,
+				     UVD_RBC_RB_RPTR, UVD_RBC_RB_WPTR,
+				     0, 0xfffff, RADEON_CP_PACKET2);
+		if (!r)
+			r = r600_uvd_init(rdev);
+		if (r)
+			DRM_ERROR("radeon: failed initializing UVD (%d).\n", r);
+	}
+
 	r = radeon_ib_pool_init(rdev);
 	if (r) {
 		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
@@ -4455,6 +5223,9 @@ int si_resume(struct radeon_device *rdev)
 	/* post card */
 	atom_asic_init(rdev->mode_info.atom_context);
 
+	/* init golden registers */
+	si_init_golden_registers(rdev);
+
 	rdev->accel_working = true;
 	r = si_startup(rdev);
 	if (r) {
@@ -4472,6 +5243,8 @@ int si_suspend(struct radeon_device *rdev)
 	radeon_vm_manager_fini(rdev);
 	si_cp_enable(rdev, false);
 	cayman_dma_stop(rdev);
+	r600_uvd_rbc_stop(rdev);
+	radeon_uvd_suspend(rdev);
 	si_irq_suspend(rdev);
 	radeon_wb_disable(rdev);
 	si_pcie_gart_disable(rdev);
@@ -4512,6 +5285,8 @@ int si_init(struct radeon_device *rdev)
 		DRM_INFO("GPU not posted. posting now...\n");
 		atom_asic_init(rdev->mode_info.atom_context);
 	}
+	/* init golden registers */
+	si_init_golden_registers(rdev);
 	/* Initialize scratch registers */
 	si_scratch_init(rdev);
 	/* Initialize surface registers */
@@ -4557,6 +5332,13 @@ int si_init(struct radeon_device *rdev)
 	ring->ring_obj = NULL;
 	r600_ring_init(rdev, ring, 64 * 1024);
 
+	r = radeon_uvd_init(rdev);
+	if (!r) {
+		ring = &rdev->ring[R600_RING_TYPE_UVD_INDEX];
+		ring->ring_obj = NULL;
+		r600_ring_init(rdev, ring, 4096);
+	}
+
 	rdev->ih.ring_obj = NULL;
 	r600_ih_ring_init(rdev, 64 * 1024);
 
@@ -4594,9 +5376,6 @@ int si_init(struct radeon_device *rdev)
 
 void si_fini(struct radeon_device *rdev)
 {
-#if 0
-	r600_blit_fini(rdev);
-#endif
 	si_cp_fini(rdev);
 	cayman_dma_fini(rdev);
 	si_irq_fini(rdev);
@@ -4605,6 +5384,7 @@ void si_fini(struct radeon_device *rdev)
 	radeon_vm_manager_fini(rdev);
 	radeon_ib_pool_fini(rdev);
 	radeon_irq_kms_fini(rdev);
+	radeon_uvd_fini(rdev);
 	si_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
@@ -4634,3 +5414,94 @@ uint64_t si_get_gpu_clock_counter(struct radeon_device *rdev)
 	mutex_unlock(&rdev->gpu_clock_mutex);
 	return clock;
 }
+
+int si_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk)
+{
+	unsigned fb_div = 0, vclk_div = 0, dclk_div = 0;
+	int r;
+
+	/* bypass vclk and dclk with bclk */
+	WREG32_P(CG_UPLL_FUNC_CNTL_2,
+		VCLK_SRC_SEL(1) | DCLK_SRC_SEL(1),
+		~(VCLK_SRC_SEL_MASK | DCLK_SRC_SEL_MASK));
+
+	/* put PLL in bypass mode */
+	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_BYPASS_EN_MASK, ~UPLL_BYPASS_EN_MASK);
+
+	if (!vclk || !dclk) {
+		/* keep the Bypass mode, put PLL to sleep */
+		WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_SLEEP_MASK, ~UPLL_SLEEP_MASK);
+		return 0;
+	}
+
+	r = radeon_uvd_calc_upll_dividers(rdev, vclk, dclk, 125000, 250000,
+					  16384, 0x03FFFFFF, 0, 128, 5,
+					  &fb_div, &vclk_div, &dclk_div);
+	if (r)
+		return r;
+
+	/* set RESET_ANTI_MUX to 0 */
+	WREG32_P(CG_UPLL_FUNC_CNTL_5, 0, ~RESET_ANTI_MUX_MASK);
+
+	/* set VCO_MODE to 1 */
+	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_VCO_MODE_MASK, ~UPLL_VCO_MODE_MASK);
+
+	/* toggle UPLL_SLEEP to 1 then back to 0 */
+	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_SLEEP_MASK, ~UPLL_SLEEP_MASK);
+	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_SLEEP_MASK);
+
+	/* deassert UPLL_RESET */
+	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_RESET_MASK);
+
+	mdelay(1);
+
+	r = radeon_uvd_send_upll_ctlreq(rdev, CG_UPLL_FUNC_CNTL);
+	if (r)
+		return r;
+
+	/* assert UPLL_RESET again */
+	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_RESET_MASK, ~UPLL_RESET_MASK);
+
+	/* disable spread spectrum. */
+	WREG32_P(CG_UPLL_SPREAD_SPECTRUM, 0, ~SSEN_MASK);
+
+	/* set feedback divider */
+	WREG32_P(CG_UPLL_FUNC_CNTL_3, UPLL_FB_DIV(fb_div), ~UPLL_FB_DIV_MASK);
+
+	/* set ref divider to 0 */
+	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_REF_DIV_MASK);
+
+	if (fb_div < 307200)
+		WREG32_P(CG_UPLL_FUNC_CNTL_4, 0, ~UPLL_SPARE_ISPARE9);
+	else
+		WREG32_P(CG_UPLL_FUNC_CNTL_4, UPLL_SPARE_ISPARE9, ~UPLL_SPARE_ISPARE9);
+
+	/* set PDIV_A and PDIV_B */
+	WREG32_P(CG_UPLL_FUNC_CNTL_2,
+		UPLL_PDIV_A(vclk_div) | UPLL_PDIV_B(dclk_div),
+		~(UPLL_PDIV_A_MASK | UPLL_PDIV_B_MASK));
+
+	/* give the PLL some time to settle */
+	mdelay(15);
+
+	/* deassert PLL_RESET */
+	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_RESET_MASK);
+
+	mdelay(15);
+
+	/* switch from bypass mode to normal mode */
+	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_BYPASS_EN_MASK);
+
+	r = radeon_uvd_send_upll_ctlreq(rdev, CG_UPLL_FUNC_CNTL);
+	if (r)
+		return r;
+
+	/* switch VCLK and DCLK selection */
+	WREG32_P(CG_UPLL_FUNC_CNTL_2,
+		VCLK_SRC_SEL(2) | DCLK_SRC_SEL(2),
+		~(VCLK_SRC_SEL_MASK | DCLK_SRC_SEL_MASK));
+
+	mdelay(100);
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/radeon/sid.h b/drivers/gpu/drm/radeon/sid.h
index 23fc08fc8e7f..222877ba6cf5 100644
--- a/drivers/gpu/drm/radeon/sid.h
+++ b/drivers/gpu/drm/radeon/sid.h
@@ -29,6 +29,35 @@
 #define TAHITI_GB_ADDR_CONFIG_GOLDEN        0x12011003
 #define VERDE_GB_ADDR_CONFIG_GOLDEN         0x12010002
 
+/* discrete uvd clocks */
+#define	CG_UPLL_FUNC_CNTL				0x634
+#	define UPLL_RESET_MASK				0x00000001
+#	define UPLL_SLEEP_MASK				0x00000002
+#	define UPLL_BYPASS_EN_MASK			0x00000004
+#	define UPLL_CTLREQ_MASK				0x00000008
+#	define UPLL_VCO_MODE_MASK			0x00000600
+#	define UPLL_REF_DIV_MASK			0x003F0000
+#	define UPLL_CTLACK_MASK				0x40000000
+#	define UPLL_CTLACK2_MASK			0x80000000
+#define	CG_UPLL_FUNC_CNTL_2				0x638
+#	define UPLL_PDIV_A(x)				((x) << 0)
+#	define UPLL_PDIV_A_MASK				0x0000007F
+#	define UPLL_PDIV_B(x)				((x) << 8)
+#	define UPLL_PDIV_B_MASK				0x00007F00
+#	define VCLK_SRC_SEL(x)				((x) << 20)
+#	define VCLK_SRC_SEL_MASK			0x01F00000
+#	define DCLK_SRC_SEL(x)				((x) << 25)
+#	define DCLK_SRC_SEL_MASK			0x3E000000
+#define	CG_UPLL_FUNC_CNTL_3				0x63C
+#	define UPLL_FB_DIV(x)				((x) << 0)
+#	define UPLL_FB_DIV_MASK				0x01FFFFFF
+#define	CG_UPLL_FUNC_CNTL_4                             0x644
+#	define UPLL_SPARE_ISPARE9			0x00020000
+#define	CG_UPLL_FUNC_CNTL_5				0x648
+#	define RESET_ANTI_MUX_MASK			0x00000200
+#define	CG_UPLL_SPREAD_SPECTRUM				0x650
+#	define SSEN_MASK				0x00000001
+
 #define	CG_MULT_THERMAL_STATUS					0x714
 #define		ASIC_MAX_TEMP(x)				((x) << 0)
 #define		ASIC_MAX_TEMP_MASK				0x000001ff
@@ -65,6 +94,8 @@
 
 #define DMIF_ADDR_CONFIG  				0xBD4
 
+#define DMIF_ADDR_CALC  				0xC00
+
 #define	SRBM_STATUS				        0xE50
 #define		GRBM_RQ_PENDING 			(1 << 5)
 #define		VMC_BUSY 				(1 << 8)
@@ -798,6 +829,15 @@
 #       define THREAD_TRACE_FINISH                      (55 << 0)
 
 /*
+ * UVD
+ */
+#define UVD_UDEC_ADDR_CONFIG				0xEF4C
+#define UVD_UDEC_DB_ADDR_CONFIG				0xEF50
+#define UVD_UDEC_DBW_ADDR_CONFIG			0xEF54
+#define UVD_RBC_RB_RPTR					0xF690
+#define UVD_RBC_RB_WPTR					0xF694
+
+/*
  * PM4
  */
 #define PACKET0(reg, n)	((RADEON_PACKET_TYPE0 << 30) |			\
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_crtc.c b/drivers/gpu/drm/shmobile/shmob_drm_crtc.c
index d917a411ca85..7dff49ed66e7 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_crtc.c
+++ b/drivers/gpu/drm/shmobile/shmob_drm_crtc.c
@@ -494,10 +494,10 @@ static int shmob_drm_crtc_page_flip(struct drm_crtc *crtc,
 
 	if (event) {
 		event->pipe = 0;
+		drm_vblank_get(dev, 0);
 		spin_lock_irqsave(&dev->event_lock, flags);
 		scrtc->event = event;
 		spin_unlock_irqrestore(&dev->event_lock, flags);
-		drm_vblank_get(dev, 0);
 	}
 
 	return 0;
diff --git a/drivers/gpu/drm/tegra/Makefile b/drivers/gpu/drm/tegra/Makefile
deleted file mode 100644
index 80f73d1315d0..000000000000
--- a/drivers/gpu/drm/tegra/Makefile
+++ /dev/null
@@ -1,7 +0,0 @@
-ccflags-y := -Iinclude/drm
-ccflags-$(CONFIG_DRM_TEGRA_DEBUG) += -DDEBUG
-
-tegra-drm-y := drm.o fb.o dc.o host1x.o
-tegra-drm-y += output.o rgb.o hdmi.o
-
-obj-$(CONFIG_DRM_TEGRA) += tegra-drm.o
diff --git a/drivers/gpu/drm/tegra/drm.c b/drivers/gpu/drm/tegra/drm.c
deleted file mode 100644
index 9d452df5bcad..000000000000
--- a/drivers/gpu/drm/tegra/drm.c
+++ /dev/null
@@ -1,217 +0,0 @@
-/*
- * Copyright (C) 2012 Avionic Design GmbH
- * Copyright (C) 2012 NVIDIA CORPORATION.  All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#include <linux/module.h>
-#include <linux/of_address.h>
-#include <linux/of_platform.h>
-
-#include <linux/dma-mapping.h>
-#include <asm/dma-iommu.h>
-
-#include "drm.h"
-
-#define DRIVER_NAME "tegra"
-#define DRIVER_DESC "NVIDIA Tegra graphics"
-#define DRIVER_DATE "20120330"
-#define DRIVER_MAJOR 0
-#define DRIVER_MINOR 0
-#define DRIVER_PATCHLEVEL 0
-
-static int tegra_drm_load(struct drm_device *drm, unsigned long flags)
-{
-	struct device *dev = drm->dev;
-	struct host1x *host1x;
-	int err;
-
-	host1x = dev_get_drvdata(dev);
-	drm->dev_private = host1x;
-	host1x->drm = drm;
-
-	drm_mode_config_init(drm);
-
-	err = host1x_drm_init(host1x, drm);
-	if (err < 0)
-		return err;
-
-	err = drm_vblank_init(drm, drm->mode_config.num_crtc);
-	if (err < 0)
-		return err;
-
-	err = tegra_drm_fb_init(drm);
-	if (err < 0)
-		return err;
-
-	drm_kms_helper_poll_init(drm);
-
-	return 0;
-}
-
-static int tegra_drm_unload(struct drm_device *drm)
-{
-	drm_kms_helper_poll_fini(drm);
-	tegra_drm_fb_exit(drm);
-
-	drm_mode_config_cleanup(drm);
-
-	return 0;
-}
-
-static int tegra_drm_open(struct drm_device *drm, struct drm_file *filp)
-{
-	return 0;
-}
-
-static void tegra_drm_lastclose(struct drm_device *drm)
-{
-	struct host1x *host1x = drm->dev_private;
-
-	drm_fbdev_cma_restore_mode(host1x->fbdev);
-}
-
-static struct drm_ioctl_desc tegra_drm_ioctls[] = {
-};
-
-static const struct file_operations tegra_drm_fops = {
-	.owner = THIS_MODULE,
-	.open = drm_open,
-	.release = drm_release,
-	.unlocked_ioctl = drm_ioctl,
-	.mmap = drm_gem_cma_mmap,
-	.poll = drm_poll,
-	.fasync = drm_fasync,
-	.read = drm_read,
-#ifdef CONFIG_COMPAT
-	.compat_ioctl = drm_compat_ioctl,
-#endif
-	.llseek = noop_llseek,
-};
-
-static struct drm_crtc *tegra_crtc_from_pipe(struct drm_device *drm, int pipe)
-{
-	struct drm_crtc *crtc;
-
-	list_for_each_entry(crtc, &drm->mode_config.crtc_list, head) {
-		struct tegra_dc *dc = to_tegra_dc(crtc);
-
-		if (dc->pipe == pipe)
-			return crtc;
-	}
-
-	return NULL;
-}
-
-static u32 tegra_drm_get_vblank_counter(struct drm_device *dev, int crtc)
-{
-	/* TODO: implement real hardware counter using syncpoints */
-	return drm_vblank_count(dev, crtc);
-}
-
-static int tegra_drm_enable_vblank(struct drm_device *drm, int pipe)
-{
-	struct drm_crtc *crtc = tegra_crtc_from_pipe(drm, pipe);
-	struct tegra_dc *dc = to_tegra_dc(crtc);
-
-	if (!crtc)
-		return -ENODEV;
-
-	tegra_dc_enable_vblank(dc);
-
-	return 0;
-}
-
-static void tegra_drm_disable_vblank(struct drm_device *drm, int pipe)
-{
-	struct drm_crtc *crtc = tegra_crtc_from_pipe(drm, pipe);
-	struct tegra_dc *dc = to_tegra_dc(crtc);
-
-	if (crtc)
-		tegra_dc_disable_vblank(dc);
-}
-
-static void tegra_drm_preclose(struct drm_device *drm, struct drm_file *file)
-{
-	struct drm_crtc *crtc;
-
-	list_for_each_entry(crtc, &drm->mode_config.crtc_list, head)
-		tegra_dc_cancel_page_flip(crtc, file);
-}
-
-#ifdef CONFIG_DEBUG_FS
-static int tegra_debugfs_framebuffers(struct seq_file *s, void *data)
-{
-	struct drm_info_node *node = (struct drm_info_node *)s->private;
-	struct drm_device *drm = node->minor->dev;
-	struct drm_framebuffer *fb;
-
-	mutex_lock(&drm->mode_config.fb_lock);
-
-	list_for_each_entry(fb, &drm->mode_config.fb_list, head) {
-		seq_printf(s, "%3d: user size: %d x %d, depth %d, %d bpp, refcount %d\n",
-			   fb->base.id, fb->width, fb->height, fb->depth,
-			   fb->bits_per_pixel,
-			   atomic_read(&fb->refcount.refcount));
-	}
-
-	mutex_unlock(&drm->mode_config.fb_lock);
-
-	return 0;
-}
-
-static struct drm_info_list tegra_debugfs_list[] = {
-	{ "framebuffers", tegra_debugfs_framebuffers, 0 },
-};
-
-static int tegra_debugfs_init(struct drm_minor *minor)
-{
-	return drm_debugfs_create_files(tegra_debugfs_list,
-					ARRAY_SIZE(tegra_debugfs_list),
-					minor->debugfs_root, minor);
-}
-
-static void tegra_debugfs_cleanup(struct drm_minor *minor)
-{
-	drm_debugfs_remove_files(tegra_debugfs_list,
-				 ARRAY_SIZE(tegra_debugfs_list), minor);
-}
-#endif
-
-struct drm_driver tegra_drm_driver = {
-	.driver_features = DRIVER_BUS_PLATFORM | DRIVER_MODESET | DRIVER_GEM,
-	.load = tegra_drm_load,
-	.unload = tegra_drm_unload,
-	.open = tegra_drm_open,
-	.preclose = tegra_drm_preclose,
-	.lastclose = tegra_drm_lastclose,
-
-	.get_vblank_counter = tegra_drm_get_vblank_counter,
-	.enable_vblank = tegra_drm_enable_vblank,
-	.disable_vblank = tegra_drm_disable_vblank,
-
-#if defined(CONFIG_DEBUG_FS)
-	.debugfs_init = tegra_debugfs_init,
-	.debugfs_cleanup = tegra_debugfs_cleanup,
-#endif
-
-	.gem_free_object = drm_gem_cma_free_object,
-	.gem_vm_ops = &drm_gem_cma_vm_ops,
-	.dumb_create = drm_gem_cma_dumb_create,
-	.dumb_map_offset = drm_gem_cma_dumb_map_offset,
-	.dumb_destroy = drm_gem_cma_dumb_destroy,
-
-	.ioctls = tegra_drm_ioctls,
-	.num_ioctls = ARRAY_SIZE(tegra_drm_ioctls),
-	.fops = &tegra_drm_fops,
-
-	.name = DRIVER_NAME,
-	.desc = DRIVER_DESC,
-	.date = DRIVER_DATE,
-	.major = DRIVER_MAJOR,
-	.minor = DRIVER_MINOR,
-	.patchlevel = DRIVER_PATCHLEVEL,
-};
diff --git a/drivers/gpu/drm/tegra/fb.c b/drivers/gpu/drm/tegra/fb.c
deleted file mode 100644
index 03914953cb1c..000000000000
--- a/drivers/gpu/drm/tegra/fb.c
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * Copyright (C) 2012 Avionic Design GmbH
- * Copyright (C) 2012 NVIDIA CORPORATION.  All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#include "drm.h"
-
-static void tegra_drm_fb_output_poll_changed(struct drm_device *drm)
-{
-	struct host1x *host1x = drm->dev_private;
-
-	drm_fbdev_cma_hotplug_event(host1x->fbdev);
-}
-
-static const struct drm_mode_config_funcs tegra_drm_mode_funcs = {
-	.fb_create = drm_fb_cma_create,
-	.output_poll_changed = tegra_drm_fb_output_poll_changed,
-};
-
-int tegra_drm_fb_init(struct drm_device *drm)
-{
-	struct host1x *host1x = drm->dev_private;
-	struct drm_fbdev_cma *fbdev;
-
-	drm->mode_config.min_width = 0;
-	drm->mode_config.min_height = 0;
-
-	drm->mode_config.max_width = 4096;
-	drm->mode_config.max_height = 4096;
-
-	drm->mode_config.funcs = &tegra_drm_mode_funcs;
-
-	fbdev = drm_fbdev_cma_init(drm, 32, drm->mode_config.num_crtc,
-				   drm->mode_config.num_connector);
-	if (IS_ERR(fbdev))
-		return PTR_ERR(fbdev);
-
-	host1x->fbdev = fbdev;
-
-	return 0;
-}
-
-void tegra_drm_fb_exit(struct drm_device *drm)
-{
-	struct host1x *host1x = drm->dev_private;
-
-	drm_fbdev_cma_fini(host1x->fbdev);
-}
diff --git a/drivers/gpu/drm/tegra/host1x.c b/drivers/gpu/drm/tegra/host1x.c
deleted file mode 100644
index 92e25a7e00ea..000000000000
--- a/drivers/gpu/drm/tegra/host1x.c
+++ /dev/null
@@ -1,327 +0,0 @@
-/*
- * Copyright (C) 2012 Avionic Design GmbH
- * Copyright (C) 2012 NVIDIA CORPORATION.  All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#include <linux/clk.h>
-#include <linux/err.h>
-#include <linux/module.h>
-#include <linux/of.h>
-#include <linux/platform_device.h>
-
-#include "drm.h"
-
-struct host1x_drm_client {
-	struct host1x_client *client;
-	struct device_node *np;
-	struct list_head list;
-};
-
-static int host1x_add_drm_client(struct host1x *host1x, struct device_node *np)
-{
-	struct host1x_drm_client *client;
-
-	client = kzalloc(sizeof(*client), GFP_KERNEL);
-	if (!client)
-		return -ENOMEM;
-
-	INIT_LIST_HEAD(&client->list);
-	client->np = of_node_get(np);
-
-	list_add_tail(&client->list, &host1x->drm_clients);
-
-	return 0;
-}
-
-static int host1x_activate_drm_client(struct host1x *host1x,
-				      struct host1x_drm_client *drm,
-				      struct host1x_client *client)
-{
-	mutex_lock(&host1x->drm_clients_lock);
-	list_del_init(&drm->list);
-	list_add_tail(&drm->list, &host1x->drm_active);
-	drm->client = client;
-	mutex_unlock(&host1x->drm_clients_lock);
-
-	return 0;
-}
-
-static int host1x_remove_drm_client(struct host1x *host1x,
-				    struct host1x_drm_client *client)
-{
-	mutex_lock(&host1x->drm_clients_lock);
-	list_del_init(&client->list);
-	mutex_unlock(&host1x->drm_clients_lock);
-
-	of_node_put(client->np);
-	kfree(client);
-
-	return 0;
-}
-
-static int host1x_parse_dt(struct host1x *host1x)
-{
-	static const char * const compat[] = {
-		"nvidia,tegra20-dc",
-		"nvidia,tegra20-hdmi",
-		"nvidia,tegra30-dc",
-		"nvidia,tegra30-hdmi",
-	};
-	unsigned int i;
-	int err;
-
-	for (i = 0; i < ARRAY_SIZE(compat); i++) {
-		struct device_node *np;
-
-		for_each_child_of_node(host1x->dev->of_node, np) {
-			if (of_device_is_compatible(np, compat[i]) &&
-			    of_device_is_available(np)) {
-				err = host1x_add_drm_client(host1x, np);
-				if (err < 0)
-					return err;
-			}
-		}
-	}
-
-	return 0;
-}
-
-static int tegra_host1x_probe(struct platform_device *pdev)
-{
-	struct host1x *host1x;
-	struct resource *regs;
-	int err;
-
-	host1x = devm_kzalloc(&pdev->dev, sizeof(*host1x), GFP_KERNEL);
-	if (!host1x)
-		return -ENOMEM;
-
-	mutex_init(&host1x->drm_clients_lock);
-	INIT_LIST_HEAD(&host1x->drm_clients);
-	INIT_LIST_HEAD(&host1x->drm_active);
-	mutex_init(&host1x->clients_lock);
-	INIT_LIST_HEAD(&host1x->clients);
-	host1x->dev = &pdev->dev;
-
-	err = host1x_parse_dt(host1x);
-	if (err < 0) {
-		dev_err(&pdev->dev, "failed to parse DT: %d\n", err);
-		return err;
-	}
-
-	host1x->clk = devm_clk_get(&pdev->dev, NULL);
-	if (IS_ERR(host1x->clk))
-		return PTR_ERR(host1x->clk);
-
-	err = clk_prepare_enable(host1x->clk);
-	if (err < 0)
-		return err;
-
-	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!regs) {
-		err = -ENXIO;
-		goto err;
-	}
-
-	err = platform_get_irq(pdev, 0);
-	if (err < 0)
-		goto err;
-
-	host1x->syncpt = err;
-
-	err = platform_get_irq(pdev, 1);
-	if (err < 0)
-		goto err;
-
-	host1x->irq = err;
-
-	host1x->regs = devm_ioremap_resource(&pdev->dev, regs);
-	if (IS_ERR(host1x->regs)) {
-		err = PTR_ERR(host1x->regs);
-		goto err;
-	}
-
-	platform_set_drvdata(pdev, host1x);
-
-	return 0;
-
-err:
-	clk_disable_unprepare(host1x->clk);
-	return err;
-}
-
-static int tegra_host1x_remove(struct platform_device *pdev)
-{
-	struct host1x *host1x = platform_get_drvdata(pdev);
-
-	clk_disable_unprepare(host1x->clk);
-
-	return 0;
-}
-
-int host1x_drm_init(struct host1x *host1x, struct drm_device *drm)
-{
-	struct host1x_client *client;
-
-	mutex_lock(&host1x->clients_lock);
-
-	list_for_each_entry(client, &host1x->clients, list) {
-		if (client->ops && client->ops->drm_init) {
-			int err = client->ops->drm_init(client, drm);
-			if (err < 0) {
-				dev_err(host1x->dev,
-					"DRM setup failed for %s: %d\n",
-					dev_name(client->dev), err);
-				return err;
-			}
-		}
-	}
-
-	mutex_unlock(&host1x->clients_lock);
-
-	return 0;
-}
-
-int host1x_drm_exit(struct host1x *host1x)
-{
-	struct platform_device *pdev = to_platform_device(host1x->dev);
-	struct host1x_client *client;
-
-	if (!host1x->drm)
-		return 0;
-
-	mutex_lock(&host1x->clients_lock);
-
-	list_for_each_entry_reverse(client, &host1x->clients, list) {
-		if (client->ops && client->ops->drm_exit) {
-			int err = client->ops->drm_exit(client);
-			if (err < 0) {
-				dev_err(host1x->dev,
-					"DRM cleanup failed for %s: %d\n",
-					dev_name(client->dev), err);
-				return err;
-			}
-		}
-	}
-
-	mutex_unlock(&host1x->clients_lock);
-
-	drm_platform_exit(&tegra_drm_driver, pdev);
-	host1x->drm = NULL;
-
-	return 0;
-}
-
-int host1x_register_client(struct host1x *host1x, struct host1x_client *client)
-{
-	struct host1x_drm_client *drm, *tmp;
-	int err;
-
-	mutex_lock(&host1x->clients_lock);
-	list_add_tail(&client->list, &host1x->clients);
-	mutex_unlock(&host1x->clients_lock);
-
-	list_for_each_entry_safe(drm, tmp, &host1x->drm_clients, list)
-		if (drm->np == client->dev->of_node)
-			host1x_activate_drm_client(host1x, drm, client);
-
-	if (list_empty(&host1x->drm_clients)) {
-		struct platform_device *pdev = to_platform_device(host1x->dev);
-
-		err = drm_platform_init(&tegra_drm_driver, pdev);
-		if (err < 0) {
-			dev_err(host1x->dev, "drm_platform_init(): %d\n", err);
-			return err;
-		}
-	}
-
-	client->host1x = host1x;
-
-	return 0;
-}
-
-int host1x_unregister_client(struct host1x *host1x,
-			     struct host1x_client *client)
-{
-	struct host1x_drm_client *drm, *tmp;
-	int err;
-
-	list_for_each_entry_safe(drm, tmp, &host1x->drm_active, list) {
-		if (drm->client == client) {
-			err = host1x_drm_exit(host1x);
-			if (err < 0) {
-				dev_err(host1x->dev, "host1x_drm_exit(): %d\n",
-					err);
-				return err;
-			}
-
-			host1x_remove_drm_client(host1x, drm);
-			break;
-		}
-	}
-
-	mutex_lock(&host1x->clients_lock);
-	list_del_init(&client->list);
-	mutex_unlock(&host1x->clients_lock);
-
-	return 0;
-}
-
-static struct of_device_id tegra_host1x_of_match[] = {
-	{ .compatible = "nvidia,tegra30-host1x", },
-	{ .compatible = "nvidia,tegra20-host1x", },
-	{ },
-};
-MODULE_DEVICE_TABLE(of, tegra_host1x_of_match);
-
-struct platform_driver tegra_host1x_driver = {
-	.driver = {
-		.name = "tegra-host1x",
-		.owner = THIS_MODULE,
-		.of_match_table = tegra_host1x_of_match,
-	},
-	.probe = tegra_host1x_probe,
-	.remove = tegra_host1x_remove,
-};
-
-static int __init tegra_host1x_init(void)
-{
-	int err;
-
-	err = platform_driver_register(&tegra_host1x_driver);
-	if (err < 0)
-		return err;
-
-	err = platform_driver_register(&tegra_dc_driver);
-	if (err < 0)
-		goto unregister_host1x;
-
-	err = platform_driver_register(&tegra_hdmi_driver);
-	if (err < 0)
-		goto unregister_dc;
-
-	return 0;
-
-unregister_dc:
-	platform_driver_unregister(&tegra_dc_driver);
-unregister_host1x:
-	platform_driver_unregister(&tegra_host1x_driver);
-	return err;
-}
-module_init(tegra_host1x_init);
-
-static void __exit tegra_host1x_exit(void)
-{
-	platform_driver_unregister(&tegra_hdmi_driver);
-	platform_driver_unregister(&tegra_dc_driver);
-	platform_driver_unregister(&tegra_host1x_driver);
-}
-module_exit(tegra_host1x_exit);
-
-MODULE_AUTHOR("Thierry Reding <thierry.reding@avionic-design.de>");
-MODULE_DESCRIPTION("NVIDIA Tegra DRM driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/drm/tilcdc/Makefile b/drivers/gpu/drm/tilcdc/Makefile
index deda656b10e7..7d2eefe94bf7 100644
--- a/drivers/gpu/drm/tilcdc/Makefile
+++ b/drivers/gpu/drm/tilcdc/Makefile
@@ -1,4 +1,7 @@
-ccflags-y := -Iinclude/drm -Werror
+ccflags-y := -Iinclude/drm
+ifeq (, $(findstring -W,$(EXTRA_CFLAGS)))
+	ccflags-y += -Werror
+endif
 
 tilcdc-y := \
 	tilcdc_crtc.o \
diff --git a/drivers/gpu/drm/tilcdc/tilcdc_drv.c b/drivers/gpu/drm/tilcdc/tilcdc_drv.c
index c5b592dc1970..2b5461bcd9fb 100644
--- a/drivers/gpu/drm/tilcdc/tilcdc_drv.c
+++ b/drivers/gpu/drm/tilcdc/tilcdc_drv.c
@@ -75,7 +75,7 @@ static int modeset_init(struct drm_device *dev)
 		mod->funcs->modeset_init(mod, dev);
 	}
 
-	if ((priv->num_encoders = 0) || (priv->num_connectors == 0)) {
+	if ((priv->num_encoders == 0) || (priv->num_connectors == 0)) {
 		/* oh nos! */
 		dev_err(dev->dev, "no encoders/connectors found\n");
 		return -ENXIO;
@@ -299,11 +299,10 @@ static int tilcdc_irq_postinstall(struct drm_device *dev)
 	struct tilcdc_drm_private *priv = dev->dev_private;
 
 	/* enable FIFO underflow irq: */
-	if (priv->rev == 1) {
+	if (priv->rev == 1)
 		tilcdc_set(dev, LCDC_RASTER_CTRL_REG, LCDC_V1_UNDERFLOW_INT_ENA);
-	} else {
+	else
 		tilcdc_set(dev, LCDC_INT_ENABLE_SET_REG, LCDC_V2_UNDERFLOW_INT_ENA);
-	}
 
 	return 0;
 }
@@ -363,7 +362,7 @@ static const struct {
 	uint8_t  rev;
 	uint8_t  save;
 	uint32_t reg;
-} registers[] = 	{
+} registers[] =		{
 #define REG(rev, save, reg) { #reg, rev, save, reg }
 		/* exists in revision 1: */
 		REG(1, false, LCDC_PID_REG),
diff --git a/drivers/gpu/drm/tilcdc/tilcdc_panel.c b/drivers/gpu/drm/tilcdc/tilcdc_panel.c
index 90ee49786372..09176654fddb 100644
--- a/drivers/gpu/drm/tilcdc/tilcdc_panel.c
+++ b/drivers/gpu/drm/tilcdc/tilcdc_panel.c
@@ -305,7 +305,7 @@ static const struct tilcdc_module_ops panel_module_ops = {
  */
 
 /* maybe move this somewhere common if it is needed by other outputs? */
-static struct tilcdc_panel_info * of_get_panel_info(struct device_node *np)
+static struct tilcdc_panel_info *of_get_panel_info(struct device_node *np)
 {
 	struct device_node *info_np;
 	struct tilcdc_panel_info *info;
@@ -413,7 +413,6 @@ static struct of_device_id panel_of_match[] = {
 		{ .compatible = "ti,tilcdc,panel", },
 		{ },
 };
-MODULE_DEVICE_TABLE(of, panel_of_match);
 
 struct platform_driver panel_driver = {
 	.probe = panel_probe,
diff --git a/drivers/gpu/drm/tilcdc/tilcdc_slave.c b/drivers/gpu/drm/tilcdc/tilcdc_slave.c
index 568dc1c08e6c..db1d2fc9dfb5 100644
--- a/drivers/gpu/drm/tilcdc/tilcdc_slave.c
+++ b/drivers/gpu/drm/tilcdc/tilcdc_slave.c
@@ -353,7 +353,6 @@ static struct of_device_id slave_of_match[] = {
 		{ .compatible = "ti,tilcdc,slave", },
 		{ },
 };
-MODULE_DEVICE_TABLE(of, slave_of_match);
 
 struct platform_driver slave_driver = {
 	.probe = slave_probe,
diff --git a/drivers/gpu/drm/tilcdc/tilcdc_tfp410.c b/drivers/gpu/drm/tilcdc/tilcdc_tfp410.c
index 58d487ba2414..a36788fbcd98 100644
--- a/drivers/gpu/drm/tilcdc/tilcdc_tfp410.c
+++ b/drivers/gpu/drm/tilcdc/tilcdc_tfp410.c
@@ -396,7 +396,6 @@ static struct of_device_id tfp410_of_match[] = {
 		{ .compatible = "ti,tilcdc,tfp410", },
 		{ },
 };
-MODULE_DEVICE_TABLE(of, tfp410_of_match);
 
 struct platform_driver tfp410_driver = {
 	.probe = tfp410_probe,
diff --git a/drivers/gpu/drm/ttm/ttm_bo_util.c b/drivers/gpu/drm/ttm/ttm_bo_util.c
index 8be35c809c7b..af894584dd90 100644
--- a/drivers/gpu/drm/ttm/ttm_bo_util.c
+++ b/drivers/gpu/drm/ttm/ttm_bo_util.c
@@ -86,6 +86,7 @@ int ttm_mem_io_lock(struct ttm_mem_type_manager *man, bool interruptible)
 	mutex_lock(&man->io_reserve_mutex);
 	return 0;
 }
+EXPORT_SYMBOL(ttm_mem_io_lock);
 
 void ttm_mem_io_unlock(struct ttm_mem_type_manager *man)
 {
@@ -94,6 +95,7 @@ void ttm_mem_io_unlock(struct ttm_mem_type_manager *man)
 
 	mutex_unlock(&man->io_reserve_mutex);
 }
+EXPORT_SYMBOL(ttm_mem_io_unlock);
 
 static int ttm_mem_io_evict(struct ttm_mem_type_manager *man)
 {
@@ -111,8 +113,9 @@ static int ttm_mem_io_evict(struct ttm_mem_type_manager *man)
 	return 0;
 }
 
-static int ttm_mem_io_reserve(struct ttm_bo_device *bdev,
-			      struct ttm_mem_reg *mem)
+
+int ttm_mem_io_reserve(struct ttm_bo_device *bdev,
+		       struct ttm_mem_reg *mem)
 {
 	struct ttm_mem_type_manager *man = &bdev->man[mem->mem_type];
 	int ret = 0;
@@ -134,9 +137,10 @@ retry:
 	}
 	return ret;
 }
+EXPORT_SYMBOL(ttm_mem_io_reserve);
 
-static void ttm_mem_io_free(struct ttm_bo_device *bdev,
-			    struct ttm_mem_reg *mem)
+void ttm_mem_io_free(struct ttm_bo_device *bdev,
+		     struct ttm_mem_reg *mem)
 {
 	struct ttm_mem_type_manager *man = &bdev->man[mem->mem_type];
 
@@ -149,6 +153,7 @@ static void ttm_mem_io_free(struct ttm_bo_device *bdev,
 		bdev->driver->io_mem_free(bdev, mem);
 
 }
+EXPORT_SYMBOL(ttm_mem_io_free);
 
 int ttm_mem_io_reserve_vm(struct ttm_buffer_object *bo)
 {
diff --git a/drivers/gpu/drm/ttm/ttm_bo_vm.c b/drivers/gpu/drm/ttm/ttm_bo_vm.c
index 74705f329d99..3df9f16b041c 100644
--- a/drivers/gpu/drm/ttm/ttm_bo_vm.c
+++ b/drivers/gpu/drm/ttm/ttm_bo_vm.c
@@ -147,7 +147,7 @@ static int ttm_bo_vm_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 
 	page_offset = ((address - vma->vm_start) >> PAGE_SHIFT) +
 	    bo->vm_node->start - vma->vm_pgoff;
-	page_last = ((vma->vm_end - vma->vm_start) >> PAGE_SHIFT) +
+	page_last = vma_pages(vma) +
 	    bo->vm_node->start - vma->vm_pgoff;
 
 	if (unlikely(page_offset >= bo->num_pages)) {
@@ -258,7 +258,7 @@ int ttm_bo_mmap(struct file *filp, struct vm_area_struct *vma,
 
 	read_lock(&bdev->vm_lock);
 	bo = ttm_bo_vm_lookup_rb(bdev, vma->vm_pgoff,
-				 (vma->vm_end - vma->vm_start) >> PAGE_SHIFT);
+				 vma_pages(vma));
 	if (likely(bo != NULL) && !kref_get_unless_zero(&bo->kref))
 		bo = NULL;
 	read_unlock(&bdev->vm_lock);
diff --git a/drivers/gpu/drm/udl/udl_fb.c b/drivers/gpu/drm/udl/udl_fb.c
index 9f4be3d4a02e..dc0c065f8d39 100644
--- a/drivers/gpu/drm/udl/udl_fb.c
+++ b/drivers/gpu/drm/udl/udl_fb.c
@@ -482,7 +482,7 @@ static int udlfb_create(struct drm_fb_helper *helper,
 	struct udl_fbdev *ufbdev = (struct udl_fbdev *)helper;
 	struct drm_device *dev = ufbdev->helper.dev;
 	struct fb_info *info;
-	struct device *device = &dev->usbdev->dev;
+	struct device *device = dev->dev;
 	struct drm_framebuffer *fb;
 	struct drm_mode_fb_cmd2 mode_cmd;
 	struct udl_gem_object *obj;
diff --git a/drivers/gpu/drm/udl/udl_gem.c b/drivers/gpu/drm/udl/udl_gem.c
index 3816270ba49b..ef034fa3e6f5 100644
--- a/drivers/gpu/drm/udl/udl_gem.c
+++ b/drivers/gpu/drm/udl/udl_gem.c
@@ -303,6 +303,8 @@ struct drm_gem_object *udl_gem_prime_import(struct drm_device *dev,
 	if (IS_ERR(attach))
 		return ERR_CAST(attach);
 
+	get_dma_buf(dma_buf);
+
 	sg = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
 	if (IS_ERR(sg)) {
 		ret = PTR_ERR(sg);
@@ -322,5 +324,7 @@ fail_unmap:
 	dma_buf_unmap_attachment(attach, sg, DMA_BIDIRECTIONAL);
 fail_detach:
 	dma_buf_detach(dma_buf, attach);
+	dma_buf_put(dma_buf);
+
 	return ERR_PTR(ret);
 }
diff --git a/drivers/gpu/host1x/Kconfig b/drivers/gpu/host1x/Kconfig
new file mode 100644
index 000000000000..ccfd42b23606
--- /dev/null
+++ b/drivers/gpu/host1x/Kconfig
@@ -0,0 +1,24 @@
+config TEGRA_HOST1X
+	tristate "NVIDIA Tegra host1x driver"
+	depends on ARCH_TEGRA || ARCH_MULTIPLATFORM
+	help
+	  Driver for the NVIDIA Tegra host1x hardware.
+
+	  The Tegra host1x module is the DMA engine for register access to
+	  Tegra's graphics- and multimedia-related modules. The modules served
+	  by host1x are referred to as clients. host1x includes some other
+	  functionality, such as synchronization.
+
+if TEGRA_HOST1X
+
+config TEGRA_HOST1X_FIREWALL
+	bool "Enable HOST1X security firewall"
+	default y
+	help
+	  Say yes if kernel should protect command streams from tampering.
+
+	  If unsure, choose Y.
+
+source "drivers/gpu/host1x/drm/Kconfig"
+
+endif
diff --git a/drivers/gpu/host1x/Makefile b/drivers/gpu/host1x/Makefile
new file mode 100644
index 000000000000..3b037b6e0298
--- /dev/null
+++ b/drivers/gpu/host1x/Makefile
@@ -0,0 +1,20 @@
+ccflags-y = -Idrivers/gpu/host1x
+
+host1x-y = \
+	syncpt.o \
+	dev.o \
+	intr.o \
+	cdma.o \
+	channel.o \
+	job.o \
+	debug.o \
+	hw/host1x01.o
+
+ccflags-y += -Iinclude/drm
+ccflags-$(CONFIG_DRM_TEGRA_DEBUG) += -DDEBUG
+
+host1x-$(CONFIG_DRM_TEGRA) += drm/drm.o drm/fb.o drm/dc.o
+host1x-$(CONFIG_DRM_TEGRA) += drm/output.o drm/rgb.o drm/hdmi.o
+host1x-$(CONFIG_DRM_TEGRA) += drm/gem.o
+host1x-$(CONFIG_DRM_TEGRA) += drm/gr2d.o
+obj-$(CONFIG_TEGRA_HOST1X) += host1x.o
diff --git a/drivers/gpu/host1x/cdma.c b/drivers/gpu/host1x/cdma.c
new file mode 100644
index 000000000000..de72172d3b5f
--- /dev/null
+++ b/drivers/gpu/host1x/cdma.c
@@ -0,0 +1,491 @@
+/*
+ * Tegra host1x Command DMA
+ *
+ * Copyright (c) 2010-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 <asm/cacheflush.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/kfifo.h>
+#include <linux/slab.h>
+#include <trace/events/host1x.h>
+
+#include "cdma.h"
+#include "channel.h"
+#include "dev.h"
+#include "debug.h"
+#include "host1x_bo.h"
+#include "job.h"
+
+/*
+ * push_buffer
+ *
+ * The push buffer is a circular array of words to be fetched by command DMA.
+ * Note that it works slightly differently to the sync queue; fence == pos
+ * means that the push buffer is full, not empty.
+ */
+
+#define HOST1X_PUSHBUFFER_SLOTS	512
+
+/*
+ * Clean up push buffer resources
+ */
+static void host1x_pushbuffer_destroy(struct push_buffer *pb)
+{
+	struct host1x_cdma *cdma = pb_to_cdma(pb);
+	struct host1x *host1x = cdma_to_host1x(cdma);
+
+	if (pb->phys != 0)
+		dma_free_writecombine(host1x->dev, pb->size_bytes + 4,
+				      pb->mapped, pb->phys);
+
+	pb->mapped = NULL;
+	pb->phys = 0;
+}
+
+/*
+ * Init push buffer resources
+ */
+static int host1x_pushbuffer_init(struct push_buffer *pb)
+{
+	struct host1x_cdma *cdma = pb_to_cdma(pb);
+	struct host1x *host1x = cdma_to_host1x(cdma);
+
+	pb->mapped = NULL;
+	pb->phys = 0;
+	pb->size_bytes = HOST1X_PUSHBUFFER_SLOTS * 8;
+
+	/* initialize buffer pointers */
+	pb->fence = pb->size_bytes - 8;
+	pb->pos = 0;
+
+	/* allocate and map pushbuffer memory */
+	pb->mapped = dma_alloc_writecombine(host1x->dev, pb->size_bytes + 4,
+					    &pb->phys, GFP_KERNEL);
+	if (!pb->mapped)
+		goto fail;
+
+	host1x_hw_pushbuffer_init(host1x, pb);
+
+	return 0;
+
+fail:
+	host1x_pushbuffer_destroy(pb);
+	return -ENOMEM;
+}
+
+/*
+ * Push two words to the push buffer
+ * Caller must ensure push buffer is not full
+ */
+static void host1x_pushbuffer_push(struct push_buffer *pb, u32 op1, u32 op2)
+{
+	u32 pos = pb->pos;
+	u32 *p = (u32 *)((u32)pb->mapped + pos);
+	WARN_ON(pos == pb->fence);
+	*(p++) = op1;
+	*(p++) = op2;
+	pb->pos = (pos + 8) & (pb->size_bytes - 1);
+}
+
+/*
+ * Pop a number of two word slots from the push buffer
+ * Caller must ensure push buffer is not empty
+ */
+static void host1x_pushbuffer_pop(struct push_buffer *pb, unsigned int slots)
+{
+	/* Advance the next write position */
+	pb->fence = (pb->fence + slots * 8) & (pb->size_bytes - 1);
+}
+
+/*
+ * Return the number of two word slots free in the push buffer
+ */
+static u32 host1x_pushbuffer_space(struct push_buffer *pb)
+{
+	return ((pb->fence - pb->pos) & (pb->size_bytes - 1)) / 8;
+}
+
+/*
+ * Sleep (if necessary) until the requested event happens
+ *   - CDMA_EVENT_SYNC_QUEUE_EMPTY : sync queue is completely empty.
+ *     - Returns 1
+ *   - CDMA_EVENT_PUSH_BUFFER_SPACE : there is space in the push buffer
+ *     - Return the amount of space (> 0)
+ * Must be called with the cdma lock held.
+ */
+unsigned int host1x_cdma_wait_locked(struct host1x_cdma *cdma,
+				     enum cdma_event event)
+{
+	for (;;) {
+		unsigned int space;
+
+		if (event == CDMA_EVENT_SYNC_QUEUE_EMPTY)
+			space = list_empty(&cdma->sync_queue) ? 1 : 0;
+		else if (event == CDMA_EVENT_PUSH_BUFFER_SPACE) {
+			struct push_buffer *pb = &cdma->push_buffer;
+			space = host1x_pushbuffer_space(pb);
+		} else {
+			WARN_ON(1);
+			return -EINVAL;
+		}
+
+		if (space)
+			return space;
+
+		trace_host1x_wait_cdma(dev_name(cdma_to_channel(cdma)->dev),
+				       event);
+
+		/* If somebody has managed to already start waiting, yield */
+		if (cdma->event != CDMA_EVENT_NONE) {
+			mutex_unlock(&cdma->lock);
+			schedule();
+			mutex_lock(&cdma->lock);
+			continue;
+		}
+		cdma->event = event;
+
+		mutex_unlock(&cdma->lock);
+		down(&cdma->sem);
+		mutex_lock(&cdma->lock);
+	}
+	return 0;
+}
+
+/*
+ * Start timer that tracks the time spent by the job.
+ * Must be called with the cdma lock held.
+ */
+static void cdma_start_timer_locked(struct host1x_cdma *cdma,
+				    struct host1x_job *job)
+{
+	struct host1x *host = cdma_to_host1x(cdma);
+
+	if (cdma->timeout.client) {
+		/* timer already started */
+		return;
+	}
+
+	cdma->timeout.client = job->client;
+	cdma->timeout.syncpt = host1x_syncpt_get(host, job->syncpt_id);
+	cdma->timeout.syncpt_val = job->syncpt_end;
+	cdma->timeout.start_ktime = ktime_get();
+
+	schedule_delayed_work(&cdma->timeout.wq,
+			      msecs_to_jiffies(job->timeout));
+}
+
+/*
+ * Stop timer when a buffer submission completes.
+ * Must be called with the cdma lock held.
+ */
+static void stop_cdma_timer_locked(struct host1x_cdma *cdma)
+{
+	cancel_delayed_work(&cdma->timeout.wq);
+	cdma->timeout.client = 0;
+}
+
+/*
+ * For all sync queue entries that have already finished according to the
+ * current sync point registers:
+ *  - unpin & unref their mems
+ *  - pop their push buffer slots
+ *  - remove them from the sync queue
+ * This is normally called from the host code's worker thread, but can be
+ * called manually if necessary.
+ * Must be called with the cdma lock held.
+ */
+static void update_cdma_locked(struct host1x_cdma *cdma)
+{
+	bool signal = false;
+	struct host1x *host1x = cdma_to_host1x(cdma);
+	struct host1x_job *job, *n;
+
+	/* If CDMA is stopped, queue is cleared and we can return */
+	if (!cdma->running)
+		return;
+
+	/*
+	 * Walk the sync queue, reading the sync point registers as necessary,
+	 * to consume as many sync queue entries as possible without blocking
+	 */
+	list_for_each_entry_safe(job, n, &cdma->sync_queue, list) {
+		struct host1x_syncpt *sp =
+			host1x_syncpt_get(host1x, job->syncpt_id);
+
+		/* Check whether this syncpt has completed, and bail if not */
+		if (!host1x_syncpt_is_expired(sp, job->syncpt_end)) {
+			/* Start timer on next pending syncpt */
+			if (job->timeout)
+				cdma_start_timer_locked(cdma, job);
+			break;
+		}
+
+		/* Cancel timeout, when a buffer completes */
+		if (cdma->timeout.client)
+			stop_cdma_timer_locked(cdma);
+
+		/* Unpin the memory */
+		host1x_job_unpin(job);
+
+		/* Pop push buffer slots */
+		if (job->num_slots) {
+			struct push_buffer *pb = &cdma->push_buffer;
+			host1x_pushbuffer_pop(pb, job->num_slots);
+			if (cdma->event == CDMA_EVENT_PUSH_BUFFER_SPACE)
+				signal = true;
+		}
+
+		list_del(&job->list);
+		host1x_job_put(job);
+	}
+
+	if (cdma->event == CDMA_EVENT_SYNC_QUEUE_EMPTY &&
+	    list_empty(&cdma->sync_queue))
+		signal = true;
+
+	if (signal) {
+		cdma->event = CDMA_EVENT_NONE;
+		up(&cdma->sem);
+	}
+}
+
+void host1x_cdma_update_sync_queue(struct host1x_cdma *cdma,
+				   struct device *dev)
+{
+	u32 restart_addr;
+	u32 syncpt_incrs;
+	struct host1x_job *job = NULL;
+	u32 syncpt_val;
+	struct host1x *host1x = cdma_to_host1x(cdma);
+
+	syncpt_val = host1x_syncpt_load(cdma->timeout.syncpt);
+
+	dev_dbg(dev, "%s: starting cleanup (thresh %d)\n",
+		__func__, syncpt_val);
+
+	/*
+	 * Move the sync_queue read pointer to the first entry that hasn't
+	 * completed based on the current HW syncpt value. It's likely there
+	 * won't be any (i.e. we're still at the head), but covers the case
+	 * where a syncpt incr happens just prior/during the teardown.
+	 */
+
+	dev_dbg(dev, "%s: skip completed buffers still in sync_queue\n",
+		__func__);
+
+	list_for_each_entry(job, &cdma->sync_queue, list) {
+		if (syncpt_val < job->syncpt_end)
+			break;
+
+		host1x_job_dump(dev, job);
+	}
+
+	/*
+	 * Walk the sync_queue, first incrementing with the CPU syncpts that
+	 * are partially executed (the first buffer) or fully skipped while
+	 * still in the current context (slots are also NOP-ed).
+	 *
+	 * At the point contexts are interleaved, syncpt increments must be
+	 * done inline with the pushbuffer from a GATHER buffer to maintain
+	 * the order (slots are modified to be a GATHER of syncpt incrs).
+	 *
+	 * Note: save in restart_addr the location where the timed out buffer
+	 * started in the PB, so we can start the refetch from there (with the
+	 * modified NOP-ed PB slots). This lets things appear to have completed
+	 * properly for this buffer and resources are freed.
+	 */
+
+	dev_dbg(dev, "%s: perform CPU incr on pending same ctx buffers\n",
+		__func__);
+
+	if (!list_empty(&cdma->sync_queue))
+		restart_addr = job->first_get;
+	else
+		restart_addr = cdma->last_pos;
+
+	/* do CPU increments as long as this context continues */
+	list_for_each_entry_from(job, &cdma->sync_queue, list) {
+		/* different context, gets us out of this loop */
+		if (job->client != cdma->timeout.client)
+			break;
+
+		/* won't need a timeout when replayed */
+		job->timeout = 0;
+
+		syncpt_incrs = job->syncpt_end - syncpt_val;
+		dev_dbg(dev, "%s: CPU incr (%d)\n", __func__, syncpt_incrs);
+
+		host1x_job_dump(dev, job);
+
+		/* safe to use CPU to incr syncpts */
+		host1x_hw_cdma_timeout_cpu_incr(host1x, cdma, job->first_get,
+						syncpt_incrs, job->syncpt_end,
+						job->num_slots);
+
+		syncpt_val += syncpt_incrs;
+	}
+
+	/* The following sumbits from the same client may be dependent on the
+	 * failed submit and therefore they may fail. Force a small timeout
+	 * to make the queue cleanup faster */
+
+	list_for_each_entry_from(job, &cdma->sync_queue, list)
+		if (job->client == cdma->timeout.client)
+			job->timeout = min_t(unsigned int, job->timeout, 500);
+
+	dev_dbg(dev, "%s: finished sync_queue modification\n", __func__);
+
+	/* roll back DMAGET and start up channel again */
+	host1x_hw_cdma_resume(host1x, cdma, restart_addr);
+}
+
+/*
+ * Create a cdma
+ */
+int host1x_cdma_init(struct host1x_cdma *cdma)
+{
+	int err;
+
+	mutex_init(&cdma->lock);
+	sema_init(&cdma->sem, 0);
+
+	INIT_LIST_HEAD(&cdma->sync_queue);
+
+	cdma->event = CDMA_EVENT_NONE;
+	cdma->running = false;
+	cdma->torndown = false;
+
+	err = host1x_pushbuffer_init(&cdma->push_buffer);
+	if (err)
+		return err;
+	return 0;
+}
+
+/*
+ * Destroy a cdma
+ */
+int host1x_cdma_deinit(struct host1x_cdma *cdma)
+{
+	struct push_buffer *pb = &cdma->push_buffer;
+	struct host1x *host1x = cdma_to_host1x(cdma);
+
+	if (cdma->running) {
+		pr_warn("%s: CDMA still running\n", __func__);
+		return -EBUSY;
+	}
+
+	host1x_pushbuffer_destroy(pb);
+	host1x_hw_cdma_timeout_destroy(host1x, cdma);
+
+	return 0;
+}
+
+/*
+ * Begin a cdma submit
+ */
+int host1x_cdma_begin(struct host1x_cdma *cdma, struct host1x_job *job)
+{
+	struct host1x *host1x = cdma_to_host1x(cdma);
+
+	mutex_lock(&cdma->lock);
+
+	if (job->timeout) {
+		/* init state on first submit with timeout value */
+		if (!cdma->timeout.initialized) {
+			int err;
+			err = host1x_hw_cdma_timeout_init(host1x, cdma,
+							  job->syncpt_id);
+			if (err) {
+				mutex_unlock(&cdma->lock);
+				return err;
+			}
+		}
+	}
+	if (!cdma->running)
+		host1x_hw_cdma_start(host1x, cdma);
+
+	cdma->slots_free = 0;
+	cdma->slots_used = 0;
+	cdma->first_get = cdma->push_buffer.pos;
+
+	trace_host1x_cdma_begin(dev_name(job->channel->dev));
+	return 0;
+}
+
+/*
+ * Push two words into a push buffer slot
+ * Blocks as necessary if the push buffer is full.
+ */
+void host1x_cdma_push(struct host1x_cdma *cdma, u32 op1, u32 op2)
+{
+	struct host1x *host1x = cdma_to_host1x(cdma);
+	struct push_buffer *pb = &cdma->push_buffer;
+	u32 slots_free = cdma->slots_free;
+
+	if (host1x_debug_trace_cmdbuf)
+		trace_host1x_cdma_push(dev_name(cdma_to_channel(cdma)->dev),
+				       op1, op2);
+
+	if (slots_free == 0) {
+		host1x_hw_cdma_flush(host1x, cdma);
+		slots_free = host1x_cdma_wait_locked(cdma,
+						CDMA_EVENT_PUSH_BUFFER_SPACE);
+	}
+	cdma->slots_free = slots_free - 1;
+	cdma->slots_used++;
+	host1x_pushbuffer_push(pb, op1, op2);
+}
+
+/*
+ * End a cdma submit
+ * Kick off DMA, add job to the sync queue, and a number of slots to be freed
+ * from the pushbuffer. The handles for a submit must all be pinned at the same
+ * time, but they can be unpinned in smaller chunks.
+ */
+void host1x_cdma_end(struct host1x_cdma *cdma,
+		     struct host1x_job *job)
+{
+	struct host1x *host1x = cdma_to_host1x(cdma);
+	bool idle = list_empty(&cdma->sync_queue);
+
+	host1x_hw_cdma_flush(host1x, cdma);
+
+	job->first_get = cdma->first_get;
+	job->num_slots = cdma->slots_used;
+	host1x_job_get(job);
+	list_add_tail(&job->list, &cdma->sync_queue);
+
+	/* start timer on idle -> active transitions */
+	if (job->timeout && idle)
+		cdma_start_timer_locked(cdma, job);
+
+	trace_host1x_cdma_end(dev_name(job->channel->dev));
+	mutex_unlock(&cdma->lock);
+}
+
+/*
+ * Update cdma state according to current sync point values
+ */
+void host1x_cdma_update(struct host1x_cdma *cdma)
+{
+	mutex_lock(&cdma->lock);
+	update_cdma_locked(cdma);
+	mutex_unlock(&cdma->lock);
+}
diff --git a/drivers/gpu/host1x/cdma.h b/drivers/gpu/host1x/cdma.h
new file mode 100644
index 000000000000..313c4b784348
--- /dev/null
+++ b/drivers/gpu/host1x/cdma.h
@@ -0,0 +1,100 @@
+/*
+ * Tegra host1x Command DMA
+ *
+ * Copyright (c) 2010-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __HOST1X_CDMA_H
+#define __HOST1X_CDMA_H
+
+#include <linux/sched.h>
+#include <linux/semaphore.h>
+#include <linux/list.h>
+
+struct host1x_syncpt;
+struct host1x_userctx_timeout;
+struct host1x_job;
+
+/*
+ * cdma
+ *
+ * This is in charge of a host command DMA channel.
+ * Sends ops to a push buffer, and takes responsibility for unpinning
+ * (& possibly freeing) of memory after those ops have completed.
+ * Producer:
+ *	begin
+ *		push - send ops to the push buffer
+ *	end - start command DMA and enqueue handles to be unpinned
+ * Consumer:
+ *	update - call to update sync queue and push buffer, unpin memory
+ */
+
+struct push_buffer {
+	u32 *mapped;			/* mapped pushbuffer memory */
+	dma_addr_t phys;		/* physical address of pushbuffer */
+	u32 fence;			/* index we've written */
+	u32 pos;			/* index to write to */
+	u32 size_bytes;
+};
+
+struct buffer_timeout {
+	struct delayed_work wq;		/* work queue */
+	bool initialized;		/* timer one-time setup flag */
+	struct host1x_syncpt *syncpt;	/* buffer completion syncpt */
+	u32 syncpt_val;			/* syncpt value when completed */
+	ktime_t start_ktime;		/* starting time */
+	/* context timeout information */
+	int client;
+};
+
+enum cdma_event {
+	CDMA_EVENT_NONE,		/* not waiting for any event */
+	CDMA_EVENT_SYNC_QUEUE_EMPTY,	/* wait for empty sync queue */
+	CDMA_EVENT_PUSH_BUFFER_SPACE	/* wait for space in push buffer */
+};
+
+struct host1x_cdma {
+	struct mutex lock;		/* controls access to shared state */
+	struct semaphore sem;		/* signalled when event occurs */
+	enum cdma_event event;		/* event that sem is waiting for */
+	unsigned int slots_used;	/* pb slots used in current submit */
+	unsigned int slots_free;	/* pb slots free in current submit */
+	unsigned int first_get;		/* DMAGET value, where submit begins */
+	unsigned int last_pos;		/* last value written to DMAPUT */
+	struct push_buffer push_buffer;	/* channel's push buffer */
+	struct list_head sync_queue;	/* job queue */
+	struct buffer_timeout timeout;	/* channel's timeout state/wq */
+	bool running;
+	bool torndown;
+};
+
+#define cdma_to_channel(cdma) container_of(cdma, struct host1x_channel, cdma)
+#define cdma_to_host1x(cdma) dev_get_drvdata(cdma_to_channel(cdma)->dev->parent)
+#define pb_to_cdma(pb) container_of(pb, struct host1x_cdma, push_buffer)
+
+int host1x_cdma_init(struct host1x_cdma *cdma);
+int host1x_cdma_deinit(struct host1x_cdma *cdma);
+void host1x_cdma_stop(struct host1x_cdma *cdma);
+int host1x_cdma_begin(struct host1x_cdma *cdma, struct host1x_job *job);
+void host1x_cdma_push(struct host1x_cdma *cdma, u32 op1, u32 op2);
+void host1x_cdma_end(struct host1x_cdma *cdma, struct host1x_job *job);
+void host1x_cdma_update(struct host1x_cdma *cdma);
+void host1x_cdma_peek(struct host1x_cdma *cdma, u32 dmaget, int slot,
+		      u32 *out);
+unsigned int host1x_cdma_wait_locked(struct host1x_cdma *cdma,
+				     enum cdma_event event);
+void host1x_cdma_update_sync_queue(struct host1x_cdma *cdma,
+				   struct device *dev);
+#endif
diff --git a/drivers/gpu/host1x/channel.c b/drivers/gpu/host1x/channel.c
new file mode 100644
index 000000000000..83ea51b9f0fc
--- /dev/null
+++ b/drivers/gpu/host1x/channel.c
@@ -0,0 +1,126 @@
+/*
+ * Tegra host1x Channel
+ *
+ * Copyright (c) 2010-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/slab.h>
+#include <linux/module.h>
+
+#include "channel.h"
+#include "dev.h"
+#include "job.h"
+
+/* Constructor for the host1x device list */
+int host1x_channel_list_init(struct host1x *host)
+{
+	INIT_LIST_HEAD(&host->chlist.list);
+	mutex_init(&host->chlist_mutex);
+
+	if (host->info->nb_channels > BITS_PER_LONG) {
+		WARN(1, "host1x hardware has more channels than supported by the driver\n");
+		return -ENOSYS;
+	}
+
+	return 0;
+}
+
+int host1x_job_submit(struct host1x_job *job)
+{
+	struct host1x *host = dev_get_drvdata(job->channel->dev->parent);
+
+	return host1x_hw_channel_submit(host, job);
+}
+
+struct host1x_channel *host1x_channel_get(struct host1x_channel *channel)
+{
+	int err = 0;
+
+	mutex_lock(&channel->reflock);
+
+	if (channel->refcount == 0)
+		err = host1x_cdma_init(&channel->cdma);
+
+	if (!err)
+		channel->refcount++;
+
+	mutex_unlock(&channel->reflock);
+
+	return err ? NULL : channel;
+}
+
+void host1x_channel_put(struct host1x_channel *channel)
+{
+	mutex_lock(&channel->reflock);
+
+	if (channel->refcount == 1) {
+		struct host1x *host = dev_get_drvdata(channel->dev->parent);
+
+		host1x_hw_cdma_stop(host, &channel->cdma);
+		host1x_cdma_deinit(&channel->cdma);
+	}
+
+	channel->refcount--;
+
+	mutex_unlock(&channel->reflock);
+}
+
+struct host1x_channel *host1x_channel_request(struct device *dev)
+{
+	struct host1x *host = dev_get_drvdata(dev->parent);
+	int max_channels = host->info->nb_channels;
+	struct host1x_channel *channel = NULL;
+	int index, err;
+
+	mutex_lock(&host->chlist_mutex);
+
+	index = find_first_zero_bit(&host->allocated_channels, max_channels);
+	if (index >= max_channels)
+		goto fail;
+
+	channel = kzalloc(sizeof(*channel), GFP_KERNEL);
+	if (!channel)
+		goto fail;
+
+	err = host1x_hw_channel_init(host, channel, index);
+	if (err < 0)
+		goto fail;
+
+	/* Link device to host1x_channel */
+	channel->dev = dev;
+
+	/* Add to channel list */
+	list_add_tail(&channel->list, &host->chlist.list);
+
+	host->allocated_channels |= BIT(index);
+
+	mutex_unlock(&host->chlist_mutex);
+	return channel;
+
+fail:
+	dev_err(dev, "failed to init channel\n");
+	kfree(channel);
+	mutex_unlock(&host->chlist_mutex);
+	return NULL;
+}
+
+void host1x_channel_free(struct host1x_channel *channel)
+{
+	struct host1x *host = dev_get_drvdata(channel->dev->parent);
+
+	host->allocated_channels &= ~BIT(channel->id);
+	list_del(&channel->list);
+	kfree(channel);
+}
diff --git a/drivers/gpu/host1x/channel.h b/drivers/gpu/host1x/channel.h
new file mode 100644
index 000000000000..48723b8eea42
--- /dev/null
+++ b/drivers/gpu/host1x/channel.h
@@ -0,0 +1,52 @@
+/*
+ * Tegra host1x Channel
+ *
+ * Copyright (c) 2010-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __HOST1X_CHANNEL_H
+#define __HOST1X_CHANNEL_H
+
+#include <linux/io.h>
+
+#include "cdma.h"
+
+struct host1x;
+
+struct host1x_channel {
+	struct list_head list;
+
+	unsigned int refcount;
+	unsigned int id;
+	struct mutex reflock;
+	struct mutex submitlock;
+	void __iomem *regs;
+	struct device *dev;
+	struct host1x_cdma cdma;
+};
+
+/* channel list operations */
+int host1x_channel_list_init(struct host1x *host);
+
+struct host1x_channel *host1x_channel_request(struct device *dev);
+void host1x_channel_free(struct host1x_channel *channel);
+struct host1x_channel *host1x_channel_get(struct host1x_channel *channel);
+void host1x_channel_put(struct host1x_channel *channel);
+int host1x_job_submit(struct host1x_job *job);
+
+#define host1x_for_each_channel(host, channel)				\
+	list_for_each_entry(channel, &host->chlist.list, list)
+
+#endif
diff --git a/drivers/gpu/host1x/debug.c b/drivers/gpu/host1x/debug.c
new file mode 100644
index 000000000000..3ec7d77de24d
--- /dev/null
+++ b/drivers/gpu/host1x/debug.c
@@ -0,0 +1,210 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ *
+ * Copyright (C) 2011-2013 NVIDIA Corporation
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+
+#include <linux/io.h>
+
+#include "dev.h"
+#include "debug.h"
+#include "channel.h"
+
+unsigned int host1x_debug_trace_cmdbuf;
+
+static pid_t host1x_debug_force_timeout_pid;
+static u32 host1x_debug_force_timeout_val;
+static u32 host1x_debug_force_timeout_channel;
+
+void host1x_debug_output(struct output *o, const char *fmt, ...)
+{
+	va_list args;
+	int len;
+
+	va_start(args, fmt);
+	len = vsnprintf(o->buf, sizeof(o->buf), fmt, args);
+	va_end(args);
+	o->fn(o->ctx, o->buf, len);
+}
+
+static int show_channels(struct host1x_channel *ch, void *data, bool show_fifo)
+{
+	struct host1x *m = dev_get_drvdata(ch->dev->parent);
+	struct output *o = data;
+
+	mutex_lock(&ch->reflock);
+	if (ch->refcount) {
+		mutex_lock(&ch->cdma.lock);
+		if (show_fifo)
+			host1x_hw_show_channel_fifo(m, ch, o);
+		host1x_hw_show_channel_cdma(m, ch, o);
+		mutex_unlock(&ch->cdma.lock);
+	}
+	mutex_unlock(&ch->reflock);
+
+	return 0;
+}
+
+static void show_syncpts(struct host1x *m, struct output *o)
+{
+	int i;
+	host1x_debug_output(o, "---- syncpts ----\n");
+	for (i = 0; i < host1x_syncpt_nb_pts(m); i++) {
+		u32 max = host1x_syncpt_read_max(m->syncpt + i);
+		u32 min = host1x_syncpt_load(m->syncpt + i);
+		if (!min && !max)
+			continue;
+		host1x_debug_output(o, "id %d (%s) min %d max %d\n",
+				    i, m->syncpt[i].name, min, max);
+	}
+
+	for (i = 0; i < host1x_syncpt_nb_bases(m); i++) {
+		u32 base_val;
+		base_val = host1x_syncpt_load_wait_base(m->syncpt + i);
+		if (base_val)
+			host1x_debug_output(o, "waitbase id %d val %d\n", i,
+					    base_val);
+	}
+
+	host1x_debug_output(o, "\n");
+}
+
+static void show_all(struct host1x *m, struct output *o)
+{
+	struct host1x_channel *ch;
+
+	host1x_hw_show_mlocks(m, o);
+	show_syncpts(m, o);
+	host1x_debug_output(o, "---- channels ----\n");
+
+	host1x_for_each_channel(m, ch)
+		show_channels(ch, o, true);
+}
+
+#ifdef CONFIG_DEBUG_FS
+static void show_all_no_fifo(struct host1x *host1x, struct output *o)
+{
+	struct host1x_channel *ch;
+
+	host1x_hw_show_mlocks(host1x, o);
+	show_syncpts(host1x, o);
+	host1x_debug_output(o, "---- channels ----\n");
+
+	host1x_for_each_channel(host1x, ch)
+		show_channels(ch, o, false);
+}
+
+static int host1x_debug_show_all(struct seq_file *s, void *unused)
+{
+	struct output o = {
+		.fn = write_to_seqfile,
+		.ctx = s
+	};
+	show_all(s->private, &o);
+	return 0;
+}
+
+static int host1x_debug_show(struct seq_file *s, void *unused)
+{
+	struct output o = {
+		.fn = write_to_seqfile,
+		.ctx = s
+	};
+	show_all_no_fifo(s->private, &o);
+	return 0;
+}
+
+static int host1x_debug_open_all(struct inode *inode, struct file *file)
+{
+	return single_open(file, host1x_debug_show_all, inode->i_private);
+}
+
+static const struct file_operations host1x_debug_all_fops = {
+	.open		= host1x_debug_open_all,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int host1x_debug_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, host1x_debug_show, inode->i_private);
+}
+
+static const struct file_operations host1x_debug_fops = {
+	.open		= host1x_debug_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+void host1x_debug_init(struct host1x *host1x)
+{
+	struct dentry *de = debugfs_create_dir("tegra-host1x", NULL);
+
+	if (!de)
+		return;
+
+	/* Store the created entry */
+	host1x->debugfs = de;
+
+	debugfs_create_file("status", S_IRUGO, de, host1x, &host1x_debug_fops);
+	debugfs_create_file("status_all", S_IRUGO, de, host1x,
+			    &host1x_debug_all_fops);
+
+	debugfs_create_u32("trace_cmdbuf", S_IRUGO|S_IWUSR, de,
+			   &host1x_debug_trace_cmdbuf);
+
+	host1x_hw_debug_init(host1x, de);
+
+	debugfs_create_u32("force_timeout_pid", S_IRUGO|S_IWUSR, de,
+			   &host1x_debug_force_timeout_pid);
+	debugfs_create_u32("force_timeout_val", S_IRUGO|S_IWUSR, de,
+			   &host1x_debug_force_timeout_val);
+	debugfs_create_u32("force_timeout_channel", S_IRUGO|S_IWUSR, de,
+			   &host1x_debug_force_timeout_channel);
+}
+
+void host1x_debug_deinit(struct host1x *host1x)
+{
+	debugfs_remove_recursive(host1x->debugfs);
+}
+#else
+void host1x_debug_init(struct host1x *host1x)
+{
+}
+void host1x_debug_deinit(struct host1x *host1x)
+{
+}
+#endif
+
+void host1x_debug_dump(struct host1x *host1x)
+{
+	struct output o = {
+		.fn = write_to_printk
+	};
+	show_all(host1x, &o);
+}
+
+void host1x_debug_dump_syncpts(struct host1x *host1x)
+{
+	struct output o = {
+		.fn = write_to_printk
+	};
+	show_syncpts(host1x, &o);
+}
diff --git a/drivers/gpu/host1x/debug.h b/drivers/gpu/host1x/debug.h
new file mode 100644
index 000000000000..4595b2e0799f
--- /dev/null
+++ b/drivers/gpu/host1x/debug.h
@@ -0,0 +1,51 @@
+/*
+ * Tegra host1x Debug
+ *
+ * Copyright (c) 2011-2013 NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef __HOST1X_DEBUG_H
+#define __HOST1X_DEBUG_H
+
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+
+struct host1x;
+
+struct output {
+	void (*fn)(void *ctx, const char *str, size_t len);
+	void *ctx;
+	char buf[256];
+};
+
+static inline void write_to_seqfile(void *ctx, const char *str, size_t len)
+{
+	seq_write((struct seq_file *)ctx, str, len);
+}
+
+static inline void write_to_printk(void *ctx, const char *str, size_t len)
+{
+	pr_info("%s", str);
+}
+
+void __printf(2, 3) host1x_debug_output(struct output *o, const char *fmt, ...);
+
+extern unsigned int host1x_debug_trace_cmdbuf;
+
+void host1x_debug_init(struct host1x *host1x);
+void host1x_debug_deinit(struct host1x *host1x);
+void host1x_debug_dump(struct host1x *host1x);
+void host1x_debug_dump_syncpts(struct host1x *host1x);
+
+#endif
diff --git a/drivers/gpu/host1x/dev.c b/drivers/gpu/host1x/dev.c
new file mode 100644
index 000000000000..28e28a23d444
--- /dev/null
+++ b/drivers/gpu/host1x/dev.c
@@ -0,0 +1,246 @@
+/*
+ * Tegra host1x driver
+ *
+ * Copyright (c) 2010-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+
+#define CREATE_TRACE_POINTS
+#include <trace/events/host1x.h>
+
+#include "dev.h"
+#include "intr.h"
+#include "channel.h"
+#include "debug.h"
+#include "hw/host1x01.h"
+#include "host1x_client.h"
+
+void host1x_set_drm_data(struct device *dev, void *data)
+{
+	struct host1x *host1x = dev_get_drvdata(dev);
+	host1x->drm_data = data;
+}
+
+void *host1x_get_drm_data(struct device *dev)
+{
+	struct host1x *host1x = dev_get_drvdata(dev);
+	return host1x->drm_data;
+}
+
+void host1x_sync_writel(struct host1x *host1x, u32 v, u32 r)
+{
+	void __iomem *sync_regs = host1x->regs + host1x->info->sync_offset;
+
+	writel(v, sync_regs + r);
+}
+
+u32 host1x_sync_readl(struct host1x *host1x, u32 r)
+{
+	void __iomem *sync_regs = host1x->regs + host1x->info->sync_offset;
+
+	return readl(sync_regs + r);
+}
+
+void host1x_ch_writel(struct host1x_channel *ch, u32 v, u32 r)
+{
+	writel(v, ch->regs + r);
+}
+
+u32 host1x_ch_readl(struct host1x_channel *ch, u32 r)
+{
+	return readl(ch->regs + r);
+}
+
+static const struct host1x_info host1x01_info = {
+	.nb_channels	= 8,
+	.nb_pts		= 32,
+	.nb_mlocks	= 16,
+	.nb_bases	= 8,
+	.init		= host1x01_init,
+	.sync_offset	= 0x3000,
+};
+
+static struct of_device_id host1x_of_match[] = {
+	{ .compatible = "nvidia,tegra30-host1x", .data = &host1x01_info, },
+	{ .compatible = "nvidia,tegra20-host1x", .data = &host1x01_info, },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, host1x_of_match);
+
+static int host1x_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *id;
+	struct host1x *host;
+	struct resource *regs;
+	int syncpt_irq;
+	int err;
+
+	id = of_match_device(host1x_of_match, &pdev->dev);
+	if (!id)
+		return -EINVAL;
+
+	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!regs) {
+		dev_err(&pdev->dev, "failed to get registers\n");
+		return -ENXIO;
+	}
+
+	syncpt_irq = platform_get_irq(pdev, 0);
+	if (syncpt_irq < 0) {
+		dev_err(&pdev->dev, "failed to get IRQ\n");
+		return -ENXIO;
+	}
+
+	host = devm_kzalloc(&pdev->dev, sizeof(*host), GFP_KERNEL);
+	if (!host)
+		return -ENOMEM;
+
+	host->dev = &pdev->dev;
+	host->info = id->data;
+
+	/* set common host1x device data */
+	platform_set_drvdata(pdev, host);
+
+	host->regs = devm_ioremap_resource(&pdev->dev, regs);
+	if (IS_ERR(host->regs))
+		return PTR_ERR(host->regs);
+
+	if (host->info->init) {
+		err = host->info->init(host);
+		if (err)
+			return err;
+	}
+
+	host->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(host->clk)) {
+		dev_err(&pdev->dev, "failed to get clock\n");
+		err = PTR_ERR(host->clk);
+		return err;
+	}
+
+	err = host1x_channel_list_init(host);
+	if (err) {
+		dev_err(&pdev->dev, "failed to initialize channel list\n");
+		return err;
+	}
+
+	err = clk_prepare_enable(host->clk);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to enable clock\n");
+		return err;
+	}
+
+	err = host1x_syncpt_init(host);
+	if (err) {
+		dev_err(&pdev->dev, "failed to initialize syncpts\n");
+		return err;
+	}
+
+	err = host1x_intr_init(host, syncpt_irq);
+	if (err) {
+		dev_err(&pdev->dev, "failed to initialize interrupts\n");
+		goto fail_deinit_syncpt;
+	}
+
+	host1x_debug_init(host);
+
+	host1x_drm_alloc(pdev);
+
+	return 0;
+
+fail_deinit_syncpt:
+	host1x_syncpt_deinit(host);
+	return err;
+}
+
+static int __exit host1x_remove(struct platform_device *pdev)
+{
+	struct host1x *host = platform_get_drvdata(pdev);
+
+	host1x_intr_deinit(host);
+	host1x_syncpt_deinit(host);
+	clk_disable_unprepare(host->clk);
+
+	return 0;
+}
+
+static struct platform_driver tegra_host1x_driver = {
+	.probe = host1x_probe,
+	.remove = __exit_p(host1x_remove),
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "tegra-host1x",
+		.of_match_table = host1x_of_match,
+	},
+};
+
+static int __init tegra_host1x_init(void)
+{
+	int err;
+
+	err = platform_driver_register(&tegra_host1x_driver);
+	if (err < 0)
+		return err;
+
+#ifdef CONFIG_DRM_TEGRA
+	err = platform_driver_register(&tegra_dc_driver);
+	if (err < 0)
+		goto unregister_host1x;
+
+	err = platform_driver_register(&tegra_hdmi_driver);
+	if (err < 0)
+		goto unregister_dc;
+
+	err = platform_driver_register(&tegra_gr2d_driver);
+	if (err < 0)
+		goto unregister_hdmi;
+#endif
+
+	return 0;
+
+#ifdef CONFIG_DRM_TEGRA
+unregister_hdmi:
+	platform_driver_unregister(&tegra_hdmi_driver);
+unregister_dc:
+	platform_driver_unregister(&tegra_dc_driver);
+unregister_host1x:
+	platform_driver_unregister(&tegra_host1x_driver);
+	return err;
+#endif
+}
+module_init(tegra_host1x_init);
+
+static void __exit tegra_host1x_exit(void)
+{
+#ifdef CONFIG_DRM_TEGRA
+	platform_driver_unregister(&tegra_gr2d_driver);
+	platform_driver_unregister(&tegra_hdmi_driver);
+	platform_driver_unregister(&tegra_dc_driver);
+#endif
+	platform_driver_unregister(&tegra_host1x_driver);
+}
+module_exit(tegra_host1x_exit);
+
+MODULE_AUTHOR("Thierry Reding <thierry.reding@avionic-design.de>");
+MODULE_AUTHOR("Terje Bergstrom <tbergstrom@nvidia.com>");
+MODULE_DESCRIPTION("Host1x driver for Tegra products");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/host1x/dev.h b/drivers/gpu/host1x/dev.h
new file mode 100644
index 000000000000..a1607d6e135b
--- /dev/null
+++ b/drivers/gpu/host1x/dev.h
@@ -0,0 +1,308 @@
+/*
+ * Copyright (c) 2012-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef HOST1X_DEV_H
+#define HOST1X_DEV_H
+
+#include <linux/platform_device.h>
+#include <linux/device.h>
+
+#include "channel.h"
+#include "syncpt.h"
+#include "intr.h"
+#include "cdma.h"
+#include "job.h"
+
+struct host1x_syncpt;
+struct host1x_channel;
+struct host1x_cdma;
+struct host1x_job;
+struct push_buffer;
+struct output;
+struct dentry;
+
+struct host1x_channel_ops {
+	int (*init)(struct host1x_channel *channel, struct host1x *host,
+		    unsigned int id);
+	int (*submit)(struct host1x_job *job);
+};
+
+struct host1x_cdma_ops {
+	void (*start)(struct host1x_cdma *cdma);
+	void (*stop)(struct host1x_cdma *cdma);
+	void (*flush)(struct  host1x_cdma *cdma);
+	int (*timeout_init)(struct host1x_cdma *cdma, u32 syncpt_id);
+	void (*timeout_destroy)(struct host1x_cdma *cdma);
+	void (*freeze)(struct host1x_cdma *cdma);
+	void (*resume)(struct host1x_cdma *cdma, u32 getptr);
+	void (*timeout_cpu_incr)(struct host1x_cdma *cdma, u32 getptr,
+				 u32 syncpt_incrs, u32 syncval, u32 nr_slots);
+};
+
+struct host1x_pushbuffer_ops {
+	void (*init)(struct push_buffer *pb);
+};
+
+struct host1x_debug_ops {
+	void (*debug_init)(struct dentry *de);
+	void (*show_channel_cdma)(struct host1x *host,
+				  struct host1x_channel *ch,
+				  struct output *o);
+	void (*show_channel_fifo)(struct host1x *host,
+				  struct host1x_channel *ch,
+				  struct output *o);
+	void (*show_mlocks)(struct host1x *host, struct output *output);
+
+};
+
+struct host1x_syncpt_ops {
+	void (*restore)(struct host1x_syncpt *syncpt);
+	void (*restore_wait_base)(struct host1x_syncpt *syncpt);
+	void (*load_wait_base)(struct host1x_syncpt *syncpt);
+	u32 (*load)(struct host1x_syncpt *syncpt);
+	void (*cpu_incr)(struct host1x_syncpt *syncpt);
+	int (*patch_wait)(struct host1x_syncpt *syncpt, void *patch_addr);
+};
+
+struct host1x_intr_ops {
+	int (*init_host_sync)(struct host1x *host, u32 cpm,
+		void (*syncpt_thresh_work)(struct work_struct *work));
+	void (*set_syncpt_threshold)(
+		struct host1x *host, u32 id, u32 thresh);
+	void (*enable_syncpt_intr)(struct host1x *host, u32 id);
+	void (*disable_syncpt_intr)(struct host1x *host, u32 id);
+	void (*disable_all_syncpt_intrs)(struct host1x *host);
+	int (*free_syncpt_irq)(struct host1x *host);
+};
+
+struct host1x_info {
+	int	nb_channels;		/* host1x: num channels supported */
+	int	nb_pts;			/* host1x: num syncpoints supported */
+	int	nb_bases;		/* host1x: num syncpoints supported */
+	int	nb_mlocks;		/* host1x: number of mlocks */
+	int	(*init)(struct host1x *); /* initialize per SoC ops */
+	int	sync_offset;
+};
+
+struct host1x {
+	const struct host1x_info *info;
+
+	void __iomem *regs;
+	struct host1x_syncpt *syncpt;
+	struct device *dev;
+	struct clk *clk;
+
+	struct mutex intr_mutex;
+	struct workqueue_struct *intr_wq;
+	int intr_syncpt_irq;
+
+	const struct host1x_syncpt_ops *syncpt_op;
+	const struct host1x_intr_ops *intr_op;
+	const struct host1x_channel_ops *channel_op;
+	const struct host1x_cdma_ops *cdma_op;
+	const struct host1x_pushbuffer_ops *cdma_pb_op;
+	const struct host1x_debug_ops *debug_op;
+
+	struct host1x_syncpt *nop_sp;
+
+	struct mutex chlist_mutex;
+	struct host1x_channel chlist;
+	unsigned long allocated_channels;
+	unsigned int num_allocated_channels;
+
+	struct dentry *debugfs;
+
+	void *drm_data;
+};
+
+void host1x_sync_writel(struct host1x *host1x, u32 r, u32 v);
+u32 host1x_sync_readl(struct host1x *host1x, u32 r);
+void host1x_ch_writel(struct host1x_channel *ch, u32 r, u32 v);
+u32 host1x_ch_readl(struct host1x_channel *ch, u32 r);
+
+static inline void host1x_hw_syncpt_restore(struct host1x *host,
+					    struct host1x_syncpt *sp)
+{
+	host->syncpt_op->restore(sp);
+}
+
+static inline void host1x_hw_syncpt_restore_wait_base(struct host1x *host,
+						      struct host1x_syncpt *sp)
+{
+	host->syncpt_op->restore_wait_base(sp);
+}
+
+static inline void host1x_hw_syncpt_load_wait_base(struct host1x *host,
+						   struct host1x_syncpt *sp)
+{
+	host->syncpt_op->load_wait_base(sp);
+}
+
+static inline u32 host1x_hw_syncpt_load(struct host1x *host,
+					struct host1x_syncpt *sp)
+{
+	return host->syncpt_op->load(sp);
+}
+
+static inline void host1x_hw_syncpt_cpu_incr(struct host1x *host,
+					     struct host1x_syncpt *sp)
+{
+	host->syncpt_op->cpu_incr(sp);
+}
+
+static inline int host1x_hw_syncpt_patch_wait(struct host1x *host,
+					      struct host1x_syncpt *sp,
+					      void *patch_addr)
+{
+	return host->syncpt_op->patch_wait(sp, patch_addr);
+}
+
+static inline int host1x_hw_intr_init_host_sync(struct host1x *host, u32 cpm,
+			void (*syncpt_thresh_work)(struct work_struct *))
+{
+	return host->intr_op->init_host_sync(host, cpm, syncpt_thresh_work);
+}
+
+static inline void host1x_hw_intr_set_syncpt_threshold(struct host1x *host,
+						       u32 id, u32 thresh)
+{
+	host->intr_op->set_syncpt_threshold(host, id, thresh);
+}
+
+static inline void host1x_hw_intr_enable_syncpt_intr(struct host1x *host,
+						     u32 id)
+{
+	host->intr_op->enable_syncpt_intr(host, id);
+}
+
+static inline void host1x_hw_intr_disable_syncpt_intr(struct host1x *host,
+						      u32 id)
+{
+	host->intr_op->disable_syncpt_intr(host, id);
+}
+
+static inline void host1x_hw_intr_disable_all_syncpt_intrs(struct host1x *host)
+{
+	host->intr_op->disable_all_syncpt_intrs(host);
+}
+
+static inline int host1x_hw_intr_free_syncpt_irq(struct host1x *host)
+{
+	return host->intr_op->free_syncpt_irq(host);
+}
+
+static inline int host1x_hw_channel_init(struct host1x *host,
+					 struct host1x_channel *channel,
+					 int chid)
+{
+	return host->channel_op->init(channel, host, chid);
+}
+
+static inline int host1x_hw_channel_submit(struct host1x *host,
+					   struct host1x_job *job)
+{
+	return host->channel_op->submit(job);
+}
+
+static inline void host1x_hw_cdma_start(struct host1x *host,
+					struct host1x_cdma *cdma)
+{
+	host->cdma_op->start(cdma);
+}
+
+static inline void host1x_hw_cdma_stop(struct host1x *host,
+				       struct host1x_cdma *cdma)
+{
+	host->cdma_op->stop(cdma);
+}
+
+static inline void host1x_hw_cdma_flush(struct host1x *host,
+					struct host1x_cdma *cdma)
+{
+	host->cdma_op->flush(cdma);
+}
+
+static inline int host1x_hw_cdma_timeout_init(struct host1x *host,
+					      struct host1x_cdma *cdma,
+					      u32 syncpt_id)
+{
+	return host->cdma_op->timeout_init(cdma, syncpt_id);
+}
+
+static inline void host1x_hw_cdma_timeout_destroy(struct host1x *host,
+						  struct host1x_cdma *cdma)
+{
+	host->cdma_op->timeout_destroy(cdma);
+}
+
+static inline void host1x_hw_cdma_freeze(struct host1x *host,
+					 struct host1x_cdma *cdma)
+{
+	host->cdma_op->freeze(cdma);
+}
+
+static inline void host1x_hw_cdma_resume(struct host1x *host,
+					 struct host1x_cdma *cdma, u32 getptr)
+{
+	host->cdma_op->resume(cdma, getptr);
+}
+
+static inline void host1x_hw_cdma_timeout_cpu_incr(struct host1x *host,
+						   struct host1x_cdma *cdma,
+						   u32 getptr,
+						   u32 syncpt_incrs,
+						   u32 syncval, u32 nr_slots)
+{
+	host->cdma_op->timeout_cpu_incr(cdma, getptr, syncpt_incrs, syncval,
+					nr_slots);
+}
+
+static inline void host1x_hw_pushbuffer_init(struct host1x *host,
+					     struct push_buffer *pb)
+{
+	host->cdma_pb_op->init(pb);
+}
+
+static inline void host1x_hw_debug_init(struct host1x *host, struct dentry *de)
+{
+	if (host->debug_op && host->debug_op->debug_init)
+		host->debug_op->debug_init(de);
+}
+
+static inline void host1x_hw_show_channel_cdma(struct host1x *host,
+					       struct host1x_channel *channel,
+					       struct output *o)
+{
+	host->debug_op->show_channel_cdma(host, channel, o);
+}
+
+static inline void host1x_hw_show_channel_fifo(struct host1x *host,
+					       struct host1x_channel *channel,
+					       struct output *o)
+{
+	host->debug_op->show_channel_fifo(host, channel, o);
+}
+
+static inline void host1x_hw_show_mlocks(struct host1x *host, struct output *o)
+{
+	host->debug_op->show_mlocks(host, o);
+}
+
+extern struct platform_driver tegra_hdmi_driver;
+extern struct platform_driver tegra_dc_driver;
+extern struct platform_driver tegra_gr2d_driver;
+
+#endif
diff --git a/drivers/gpu/drm/tegra/Kconfig b/drivers/gpu/host1x/drm/Kconfig
index be1daf7344d3..69853a4de40a 100644
--- a/drivers/gpu/drm/tegra/Kconfig
+++ b/drivers/gpu/host1x/drm/Kconfig
@@ -1,12 +1,10 @@
 config DRM_TEGRA
-	tristate "NVIDIA Tegra DRM"
-	depends on DRM && OF && ARCH_TEGRA
+	bool "NVIDIA Tegra DRM"
+	depends on DRM
 	select DRM_KMS_HELPER
-	select DRM_GEM_CMA_HELPER
-	select DRM_KMS_CMA_HELPER
-	select FB_CFB_FILLRECT
-	select FB_CFB_COPYAREA
-	select FB_CFB_IMAGEBLIT
+	select FB_SYS_FILLRECT
+	select FB_SYS_COPYAREA
+	select FB_SYS_IMAGEBLIT
 	help
 	  Choose this option if you have an NVIDIA Tegra SoC.
 
@@ -15,6 +13,14 @@ config DRM_TEGRA
 
 if DRM_TEGRA
 
+config DRM_TEGRA_STAGING
+	bool "Enable HOST1X interface"
+	depends on STAGING
+	help
+	  Say yes if HOST1X should be available for userspace DRM users.
+
+	  If unsure, choose N.
+
 config DRM_TEGRA_DEBUG
 	bool "NVIDIA Tegra DRM debug support"
 	help
diff --git a/drivers/gpu/drm/tegra/dc.c b/drivers/gpu/host1x/drm/dc.c
index de94707b9dbe..1e2060324f02 100644
--- a/drivers/gpu/drm/tegra/dc.c
+++ b/drivers/gpu/host1x/drm/dc.c
@@ -14,8 +14,10 @@
 #include <linux/platform_device.h>
 #include <linux/clk/tegra.h>
 
-#include "drm.h"
+#include "host1x_client.h"
 #include "dc.h"
+#include "drm.h"
+#include "gem.h"
 
 struct tegra_plane {
 	struct drm_plane base;
@@ -51,9 +53,9 @@ static int tegra_plane_update(struct drm_plane *plane, struct drm_crtc *crtc,
 	window.bits_per_pixel = fb->bits_per_pixel;
 
 	for (i = 0; i < drm_format_num_planes(fb->pixel_format); i++) {
-		struct drm_gem_cma_object *gem = drm_fb_cma_get_gem_obj(fb, i);
+		struct tegra_bo *bo = tegra_fb_get_plane(fb, i);
 
-		window.base[i] = gem->paddr + fb->offsets[i];
+		window.base[i] = bo->paddr + fb->offsets[i];
 
 		/*
 		 * Tegra doesn't support different strides for U and V planes
@@ -103,7 +105,9 @@ static const struct drm_plane_funcs tegra_plane_funcs = {
 };
 
 static const uint32_t plane_formats[] = {
+	DRM_FORMAT_XBGR8888,
 	DRM_FORMAT_XRGB8888,
+	DRM_FORMAT_RGB565,
 	DRM_FORMAT_UYVY,
 	DRM_FORMAT_YUV420,
 	DRM_FORMAT_YUV422,
@@ -136,7 +140,7 @@ static int tegra_dc_add_planes(struct drm_device *drm, struct tegra_dc *dc)
 static int tegra_dc_set_base(struct tegra_dc *dc, int x, int y,
 			     struct drm_framebuffer *fb)
 {
-	struct drm_gem_cma_object *gem = drm_fb_cma_get_gem_obj(fb, 0);
+	struct tegra_bo *bo = tegra_fb_get_plane(fb, 0);
 	unsigned long value;
 
 	tegra_dc_writel(dc, WINDOW_A_SELECT, DC_CMD_DISPLAY_WINDOW_HEADER);
@@ -144,7 +148,7 @@ static int tegra_dc_set_base(struct tegra_dc *dc, int x, int y,
 	value = fb->offsets[0] + y * fb->pitches[0] +
 		x * fb->bits_per_pixel / 8;
 
-	tegra_dc_writel(dc, gem->paddr + value, DC_WINBUF_START_ADDR);
+	tegra_dc_writel(dc, bo->paddr + value, DC_WINBUF_START_ADDR);
 	tegra_dc_writel(dc, fb->pitches[0], DC_WIN_LINE_STRIDE);
 
 	value = GENERAL_UPDATE | WIN_A_UPDATE;
@@ -186,20 +190,20 @@ static void tegra_dc_finish_page_flip(struct tegra_dc *dc)
 {
 	struct drm_device *drm = dc->base.dev;
 	struct drm_crtc *crtc = &dc->base;
-	struct drm_gem_cma_object *gem;
 	unsigned long flags, base;
+	struct tegra_bo *bo;
 
 	if (!dc->event)
 		return;
 
-	gem = drm_fb_cma_get_gem_obj(crtc->fb, 0);
+	bo = tegra_fb_get_plane(crtc->fb, 0);
 
 	/* check if new start address has been latched */
 	tegra_dc_writel(dc, READ_MUX, DC_CMD_STATE_ACCESS);
 	base = tegra_dc_readl(dc, DC_WINBUF_START_ADDR);
 	tegra_dc_writel(dc, 0, DC_CMD_STATE_ACCESS);
 
-	if (base == gem->paddr + crtc->fb->offsets[0]) {
+	if (base == bo->paddr + crtc->fb->offsets[0]) {
 		spin_lock_irqsave(&drm->event_lock, flags);
 		drm_send_vblank_event(drm, dc->pipe, dc->event);
 		drm_vblank_put(drm, dc->pipe);
@@ -541,6 +545,9 @@ int tegra_dc_setup_window(struct tegra_dc *dc, unsigned int index,
 unsigned int tegra_dc_format(uint32_t format)
 {
 	switch (format) {
+	case DRM_FORMAT_XBGR8888:
+		return WIN_COLOR_DEPTH_R8G8B8A8;
+
 	case DRM_FORMAT_XRGB8888:
 		return WIN_COLOR_DEPTH_B8G8R8A8;
 
@@ -569,7 +576,7 @@ static int tegra_crtc_mode_set(struct drm_crtc *crtc,
 			       struct drm_display_mode *adjusted,
 			       int x, int y, struct drm_framebuffer *old_fb)
 {
-	struct drm_gem_cma_object *gem = drm_fb_cma_get_gem_obj(crtc->fb, 0);
+	struct tegra_bo *bo = tegra_fb_get_plane(crtc->fb, 0);
 	struct tegra_dc *dc = to_tegra_dc(crtc);
 	struct tegra_dc_window window;
 	unsigned long div, value;
@@ -616,7 +623,7 @@ static int tegra_crtc_mode_set(struct drm_crtc *crtc,
 	window.format = tegra_dc_format(crtc->fb->pixel_format);
 	window.bits_per_pixel = crtc->fb->bits_per_pixel;
 	window.stride[0] = crtc->fb->pitches[0];
-	window.base[0] = gem->paddr;
+	window.base[0] = bo->paddr;
 
 	err = tegra_dc_setup_window(dc, 0, &window);
 	if (err < 0)
@@ -1097,7 +1104,7 @@ static const struct host1x_client_ops dc_client_ops = {
 
 static int tegra_dc_probe(struct platform_device *pdev)
 {
-	struct host1x *host1x = dev_get_drvdata(pdev->dev.parent);
+	struct host1x_drm *host1x = host1x_get_drm_data(pdev->dev.parent);
 	struct resource *regs;
 	struct tegra_dc *dc;
 	int err;
@@ -1160,7 +1167,7 @@ static int tegra_dc_probe(struct platform_device *pdev)
 
 static int tegra_dc_remove(struct platform_device *pdev)
 {
-	struct host1x *host1x = dev_get_drvdata(pdev->dev.parent);
+	struct host1x_drm *host1x = host1x_get_drm_data(pdev->dev.parent);
 	struct tegra_dc *dc = platform_get_drvdata(pdev);
 	int err;
 
diff --git a/drivers/gpu/drm/tegra/dc.h b/drivers/gpu/host1x/drm/dc.h
index 79eaec9aac77..79eaec9aac77 100644
--- a/drivers/gpu/drm/tegra/dc.h
+++ b/drivers/gpu/host1x/drm/dc.h
diff --git a/drivers/gpu/host1x/drm/drm.c b/drivers/gpu/host1x/drm/drm.c
new file mode 100644
index 000000000000..2b561c9118c6
--- /dev/null
+++ b/drivers/gpu/host1x/drm/drm.c
@@ -0,0 +1,640 @@
+/*
+ * Copyright (C) 2012 Avionic Design GmbH
+ * Copyright (C) 2012-2013 NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+
+#include <linux/dma-mapping.h>
+#include <asm/dma-iommu.h>
+
+#include <drm/drm.h>
+#include <drm/drmP.h>
+
+#include "host1x_client.h"
+#include "dev.h"
+#include "drm.h"
+#include "gem.h"
+#include "syncpt.h"
+
+#define DRIVER_NAME "tegra"
+#define DRIVER_DESC "NVIDIA Tegra graphics"
+#define DRIVER_DATE "20120330"
+#define DRIVER_MAJOR 0
+#define DRIVER_MINOR 0
+#define DRIVER_PATCHLEVEL 0
+
+struct host1x_drm_client {
+	struct host1x_client *client;
+	struct device_node *np;
+	struct list_head list;
+};
+
+static int host1x_add_drm_client(struct host1x_drm *host1x,
+				 struct device_node *np)
+{
+	struct host1x_drm_client *client;
+
+	client = kzalloc(sizeof(*client), GFP_KERNEL);
+	if (!client)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&client->list);
+	client->np = of_node_get(np);
+
+	list_add_tail(&client->list, &host1x->drm_clients);
+
+	return 0;
+}
+
+static int host1x_activate_drm_client(struct host1x_drm *host1x,
+				      struct host1x_drm_client *drm,
+				      struct host1x_client *client)
+{
+	mutex_lock(&host1x->drm_clients_lock);
+	list_del_init(&drm->list);
+	list_add_tail(&drm->list, &host1x->drm_active);
+	drm->client = client;
+	mutex_unlock(&host1x->drm_clients_lock);
+
+	return 0;
+}
+
+static int host1x_remove_drm_client(struct host1x_drm *host1x,
+				    struct host1x_drm_client *client)
+{
+	mutex_lock(&host1x->drm_clients_lock);
+	list_del_init(&client->list);
+	mutex_unlock(&host1x->drm_clients_lock);
+
+	of_node_put(client->np);
+	kfree(client);
+
+	return 0;
+}
+
+static int host1x_parse_dt(struct host1x_drm *host1x)
+{
+	static const char * const compat[] = {
+		"nvidia,tegra20-dc",
+		"nvidia,tegra20-hdmi",
+		"nvidia,tegra20-gr2d",
+		"nvidia,tegra30-dc",
+		"nvidia,tegra30-hdmi",
+		"nvidia,tegra30-gr2d",
+	};
+	unsigned int i;
+	int err;
+
+	for (i = 0; i < ARRAY_SIZE(compat); i++) {
+		struct device_node *np;
+
+		for_each_child_of_node(host1x->dev->of_node, np) {
+			if (of_device_is_compatible(np, compat[i]) &&
+			    of_device_is_available(np)) {
+				err = host1x_add_drm_client(host1x, np);
+				if (err < 0)
+					return err;
+			}
+		}
+	}
+
+	return 0;
+}
+
+int host1x_drm_alloc(struct platform_device *pdev)
+{
+	struct host1x_drm *host1x;
+	int err;
+
+	host1x = devm_kzalloc(&pdev->dev, sizeof(*host1x), GFP_KERNEL);
+	if (!host1x)
+		return -ENOMEM;
+
+	mutex_init(&host1x->drm_clients_lock);
+	INIT_LIST_HEAD(&host1x->drm_clients);
+	INIT_LIST_HEAD(&host1x->drm_active);
+	mutex_init(&host1x->clients_lock);
+	INIT_LIST_HEAD(&host1x->clients);
+	host1x->dev = &pdev->dev;
+
+	err = host1x_parse_dt(host1x);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to parse DT: %d\n", err);
+		return err;
+	}
+
+	host1x_set_drm_data(&pdev->dev, host1x);
+
+	return 0;
+}
+
+int host1x_drm_init(struct host1x_drm *host1x, struct drm_device *drm)
+{
+	struct host1x_client *client;
+
+	mutex_lock(&host1x->clients_lock);
+
+	list_for_each_entry(client, &host1x->clients, list) {
+		if (client->ops && client->ops->drm_init) {
+			int err = client->ops->drm_init(client, drm);
+			if (err < 0) {
+				dev_err(host1x->dev,
+					"DRM setup failed for %s: %d\n",
+					dev_name(client->dev), err);
+				return err;
+			}
+		}
+	}
+
+	mutex_unlock(&host1x->clients_lock);
+
+	return 0;
+}
+
+int host1x_drm_exit(struct host1x_drm *host1x)
+{
+	struct platform_device *pdev = to_platform_device(host1x->dev);
+	struct host1x_client *client;
+
+	if (!host1x->drm)
+		return 0;
+
+	mutex_lock(&host1x->clients_lock);
+
+	list_for_each_entry_reverse(client, &host1x->clients, list) {
+		if (client->ops && client->ops->drm_exit) {
+			int err = client->ops->drm_exit(client);
+			if (err < 0) {
+				dev_err(host1x->dev,
+					"DRM cleanup failed for %s: %d\n",
+					dev_name(client->dev), err);
+				return err;
+			}
+		}
+	}
+
+	mutex_unlock(&host1x->clients_lock);
+
+	drm_platform_exit(&tegra_drm_driver, pdev);
+	host1x->drm = NULL;
+
+	return 0;
+}
+
+int host1x_register_client(struct host1x_drm *host1x,
+			   struct host1x_client *client)
+{
+	struct host1x_drm_client *drm, *tmp;
+	int err;
+
+	mutex_lock(&host1x->clients_lock);
+	list_add_tail(&client->list, &host1x->clients);
+	mutex_unlock(&host1x->clients_lock);
+
+	list_for_each_entry_safe(drm, tmp, &host1x->drm_clients, list)
+		if (drm->np == client->dev->of_node)
+			host1x_activate_drm_client(host1x, drm, client);
+
+	if (list_empty(&host1x->drm_clients)) {
+		struct platform_device *pdev = to_platform_device(host1x->dev);
+
+		err = drm_platform_init(&tegra_drm_driver, pdev);
+		if (err < 0) {
+			dev_err(host1x->dev, "drm_platform_init(): %d\n", err);
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+int host1x_unregister_client(struct host1x_drm *host1x,
+			     struct host1x_client *client)
+{
+	struct host1x_drm_client *drm, *tmp;
+	int err;
+
+	list_for_each_entry_safe(drm, tmp, &host1x->drm_active, list) {
+		if (drm->client == client) {
+			err = host1x_drm_exit(host1x);
+			if (err < 0) {
+				dev_err(host1x->dev, "host1x_drm_exit(): %d\n",
+					err);
+				return err;
+			}
+
+			host1x_remove_drm_client(host1x, drm);
+			break;
+		}
+	}
+
+	mutex_lock(&host1x->clients_lock);
+	list_del_init(&client->list);
+	mutex_unlock(&host1x->clients_lock);
+
+	return 0;
+}
+
+static int tegra_drm_load(struct drm_device *drm, unsigned long flags)
+{
+	struct host1x_drm *host1x;
+	int err;
+
+	host1x = host1x_get_drm_data(drm->dev);
+	drm->dev_private = host1x;
+	host1x->drm = drm;
+
+	drm_mode_config_init(drm);
+
+	err = host1x_drm_init(host1x, drm);
+	if (err < 0)
+		return err;
+
+	err = drm_vblank_init(drm, drm->mode_config.num_crtc);
+	if (err < 0)
+		return err;
+
+	err = tegra_drm_fb_init(drm);
+	if (err < 0)
+		return err;
+
+	drm_kms_helper_poll_init(drm);
+
+	return 0;
+}
+
+static int tegra_drm_unload(struct drm_device *drm)
+{
+	drm_kms_helper_poll_fini(drm);
+	tegra_drm_fb_exit(drm);
+
+	drm_mode_config_cleanup(drm);
+
+	return 0;
+}
+
+static int tegra_drm_open(struct drm_device *drm, struct drm_file *filp)
+{
+	struct host1x_drm_file *fpriv;
+
+	fpriv = kzalloc(sizeof(*fpriv), GFP_KERNEL);
+	if (!fpriv)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&fpriv->contexts);
+	filp->driver_priv = fpriv;
+
+	return 0;
+}
+
+static void host1x_drm_context_free(struct host1x_drm_context *context)
+{
+	context->client->ops->close_channel(context);
+	kfree(context);
+}
+
+static void tegra_drm_lastclose(struct drm_device *drm)
+{
+	struct host1x_drm *host1x = drm->dev_private;
+
+	tegra_fbdev_restore_mode(host1x->fbdev);
+}
+
+#ifdef CONFIG_DRM_TEGRA_STAGING
+static bool host1x_drm_file_owns_context(struct host1x_drm_file *file,
+					 struct host1x_drm_context *context)
+{
+	struct host1x_drm_context *ctx;
+
+	list_for_each_entry(ctx, &file->contexts, list)
+		if (ctx == context)
+			return true;
+
+	return false;
+}
+
+static int tegra_gem_create(struct drm_device *drm, void *data,
+			    struct drm_file *file)
+{
+	struct drm_tegra_gem_create *args = data;
+	struct tegra_bo *bo;
+
+	bo = tegra_bo_create_with_handle(file, drm, args->size,
+					 &args->handle);
+	if (IS_ERR(bo))
+		return PTR_ERR(bo);
+
+	return 0;
+}
+
+static int tegra_gem_mmap(struct drm_device *drm, void *data,
+			  struct drm_file *file)
+{
+	struct drm_tegra_gem_mmap *args = data;
+	struct drm_gem_object *gem;
+	struct tegra_bo *bo;
+
+	gem = drm_gem_object_lookup(drm, file, args->handle);
+	if (!gem)
+		return -EINVAL;
+
+	bo = to_tegra_bo(gem);
+
+	args->offset = tegra_bo_get_mmap_offset(bo);
+
+	drm_gem_object_unreference(gem);
+
+	return 0;
+}
+
+static int tegra_syncpt_read(struct drm_device *drm, void *data,
+			     struct drm_file *file)
+{
+	struct drm_tegra_syncpt_read *args = data;
+	struct host1x *host = dev_get_drvdata(drm->dev);
+	struct host1x_syncpt *sp = host1x_syncpt_get(host, args->id);
+
+	if (!sp)
+		return -EINVAL;
+
+	args->value = host1x_syncpt_read_min(sp);
+	return 0;
+}
+
+static int tegra_syncpt_incr(struct drm_device *drm, void *data,
+			     struct drm_file *file)
+{
+	struct drm_tegra_syncpt_incr *args = data;
+	struct host1x *host = dev_get_drvdata(drm->dev);
+	struct host1x_syncpt *sp = host1x_syncpt_get(host, args->id);
+
+	if (!sp)
+		return -EINVAL;
+
+	host1x_syncpt_incr(sp);
+	return 0;
+}
+
+static int tegra_syncpt_wait(struct drm_device *drm, void *data,
+			     struct drm_file *file)
+{
+	struct drm_tegra_syncpt_wait *args = data;
+	struct host1x *host = dev_get_drvdata(drm->dev);
+	struct host1x_syncpt *sp = host1x_syncpt_get(host, args->id);
+
+	if (!sp)
+		return -EINVAL;
+
+	return host1x_syncpt_wait(sp, args->thresh, args->timeout,
+				  &args->value);
+}
+
+static int tegra_open_channel(struct drm_device *drm, void *data,
+			      struct drm_file *file)
+{
+	struct drm_tegra_open_channel *args = data;
+	struct host1x_client *client;
+	struct host1x_drm_context *context;
+	struct host1x_drm_file *fpriv = file->driver_priv;
+	struct host1x_drm *host1x = drm->dev_private;
+	int err = -ENODEV;
+
+	context = kzalloc(sizeof(*context), GFP_KERNEL);
+	if (!context)
+		return -ENOMEM;
+
+	list_for_each_entry(client, &host1x->clients, list)
+		if (client->class == args->client) {
+			err = client->ops->open_channel(client, context);
+			if (err)
+				break;
+
+			context->client = client;
+			list_add(&context->list, &fpriv->contexts);
+			args->context = (uintptr_t)context;
+			return 0;
+		}
+
+	kfree(context);
+	return err;
+}
+
+static int tegra_close_channel(struct drm_device *drm, void *data,
+			       struct drm_file *file)
+{
+	struct drm_tegra_close_channel *args = data;
+	struct host1x_drm_file *fpriv = file->driver_priv;
+	struct host1x_drm_context *context =
+		(struct host1x_drm_context *)(uintptr_t)args->context;
+
+	if (!host1x_drm_file_owns_context(fpriv, context))
+		return -EINVAL;
+
+	list_del(&context->list);
+	host1x_drm_context_free(context);
+
+	return 0;
+}
+
+static int tegra_get_syncpt(struct drm_device *drm, void *data,
+			    struct drm_file *file)
+{
+	struct drm_tegra_get_syncpt *args = data;
+	struct host1x_drm_file *fpriv = file->driver_priv;
+	struct host1x_drm_context *context =
+		(struct host1x_drm_context *)(uintptr_t)args->context;
+	struct host1x_syncpt *syncpt;
+
+	if (!host1x_drm_file_owns_context(fpriv, context))
+		return -ENODEV;
+
+	if (args->index >= context->client->num_syncpts)
+		return -EINVAL;
+
+	syncpt = context->client->syncpts[args->index];
+	args->id = host1x_syncpt_id(syncpt);
+
+	return 0;
+}
+
+static int tegra_submit(struct drm_device *drm, void *data,
+			struct drm_file *file)
+{
+	struct drm_tegra_submit *args = data;
+	struct host1x_drm_file *fpriv = file->driver_priv;
+	struct host1x_drm_context *context =
+		(struct host1x_drm_context *)(uintptr_t)args->context;
+
+	if (!host1x_drm_file_owns_context(fpriv, context))
+		return -ENODEV;
+
+	return context->client->ops->submit(context, args, drm, file);
+}
+#endif
+
+static struct drm_ioctl_desc tegra_drm_ioctls[] = {
+#ifdef CONFIG_DRM_TEGRA_STAGING
+	DRM_IOCTL_DEF_DRV(TEGRA_GEM_CREATE, tegra_gem_create, DRM_UNLOCKED | DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(TEGRA_GEM_MMAP, tegra_gem_mmap, DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(TEGRA_SYNCPT_READ, tegra_syncpt_read, DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(TEGRA_SYNCPT_INCR, tegra_syncpt_incr, DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(TEGRA_SYNCPT_WAIT, tegra_syncpt_wait, DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(TEGRA_OPEN_CHANNEL, tegra_open_channel, DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(TEGRA_CLOSE_CHANNEL, tegra_close_channel, DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(TEGRA_GET_SYNCPT, tegra_get_syncpt, DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(TEGRA_SUBMIT, tegra_submit, DRM_UNLOCKED),
+#endif
+};
+
+static const struct file_operations tegra_drm_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = tegra_drm_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.read = drm_read,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl = drm_compat_ioctl,
+#endif
+	.llseek = noop_llseek,
+};
+
+static struct drm_crtc *tegra_crtc_from_pipe(struct drm_device *drm, int pipe)
+{
+	struct drm_crtc *crtc;
+
+	list_for_each_entry(crtc, &drm->mode_config.crtc_list, head) {
+		struct tegra_dc *dc = to_tegra_dc(crtc);
+
+		if (dc->pipe == pipe)
+			return crtc;
+	}
+
+	return NULL;
+}
+
+static u32 tegra_drm_get_vblank_counter(struct drm_device *dev, int crtc)
+{
+	/* TODO: implement real hardware counter using syncpoints */
+	return drm_vblank_count(dev, crtc);
+}
+
+static int tegra_drm_enable_vblank(struct drm_device *drm, int pipe)
+{
+	struct drm_crtc *crtc = tegra_crtc_from_pipe(drm, pipe);
+	struct tegra_dc *dc = to_tegra_dc(crtc);
+
+	if (!crtc)
+		return -ENODEV;
+
+	tegra_dc_enable_vblank(dc);
+
+	return 0;
+}
+
+static void tegra_drm_disable_vblank(struct drm_device *drm, int pipe)
+{
+	struct drm_crtc *crtc = tegra_crtc_from_pipe(drm, pipe);
+	struct tegra_dc *dc = to_tegra_dc(crtc);
+
+	if (crtc)
+		tegra_dc_disable_vblank(dc);
+}
+
+static void tegra_drm_preclose(struct drm_device *drm, struct drm_file *file)
+{
+	struct host1x_drm_file *fpriv = file->driver_priv;
+	struct host1x_drm_context *context, *tmp;
+	struct drm_crtc *crtc;
+
+	list_for_each_entry(crtc, &drm->mode_config.crtc_list, head)
+		tegra_dc_cancel_page_flip(crtc, file);
+
+	list_for_each_entry_safe(context, tmp, &fpriv->contexts, list)
+		host1x_drm_context_free(context);
+
+	kfree(fpriv);
+}
+
+#ifdef CONFIG_DEBUG_FS
+static int tegra_debugfs_framebuffers(struct seq_file *s, void *data)
+{
+	struct drm_info_node *node = (struct drm_info_node *)s->private;
+	struct drm_device *drm = node->minor->dev;
+	struct drm_framebuffer *fb;
+
+	mutex_lock(&drm->mode_config.fb_lock);
+
+	list_for_each_entry(fb, &drm->mode_config.fb_list, head) {
+		seq_printf(s, "%3d: user size: %d x %d, depth %d, %d bpp, refcount %d\n",
+			   fb->base.id, fb->width, fb->height, fb->depth,
+			   fb->bits_per_pixel,
+			   atomic_read(&fb->refcount.refcount));
+	}
+
+	mutex_unlock(&drm->mode_config.fb_lock);
+
+	return 0;
+}
+
+static struct drm_info_list tegra_debugfs_list[] = {
+	{ "framebuffers", tegra_debugfs_framebuffers, 0 },
+};
+
+static int tegra_debugfs_init(struct drm_minor *minor)
+{
+	return drm_debugfs_create_files(tegra_debugfs_list,
+					ARRAY_SIZE(tegra_debugfs_list),
+					minor->debugfs_root, minor);
+}
+
+static void tegra_debugfs_cleanup(struct drm_minor *minor)
+{
+	drm_debugfs_remove_files(tegra_debugfs_list,
+				 ARRAY_SIZE(tegra_debugfs_list), minor);
+}
+#endif
+
+struct drm_driver tegra_drm_driver = {
+	.driver_features = DRIVER_BUS_PLATFORM | DRIVER_MODESET | DRIVER_GEM,
+	.load = tegra_drm_load,
+	.unload = tegra_drm_unload,
+	.open = tegra_drm_open,
+	.preclose = tegra_drm_preclose,
+	.lastclose = tegra_drm_lastclose,
+
+	.get_vblank_counter = tegra_drm_get_vblank_counter,
+	.enable_vblank = tegra_drm_enable_vblank,
+	.disable_vblank = tegra_drm_disable_vblank,
+
+#if defined(CONFIG_DEBUG_FS)
+	.debugfs_init = tegra_debugfs_init,
+	.debugfs_cleanup = tegra_debugfs_cleanup,
+#endif
+
+	.gem_free_object = tegra_bo_free_object,
+	.gem_vm_ops = &tegra_bo_vm_ops,
+	.dumb_create = tegra_bo_dumb_create,
+	.dumb_map_offset = tegra_bo_dumb_map_offset,
+	.dumb_destroy = tegra_bo_dumb_destroy,
+
+	.ioctls = tegra_drm_ioctls,
+	.num_ioctls = ARRAY_SIZE(tegra_drm_ioctls),
+	.fops = &tegra_drm_fops,
+
+	.name = DRIVER_NAME,
+	.desc = DRIVER_DESC,
+	.date = DRIVER_DATE,
+	.major = DRIVER_MAJOR,
+	.minor = DRIVER_MINOR,
+	.patchlevel = DRIVER_PATCHLEVEL,
+};
diff --git a/drivers/gpu/drm/tegra/drm.h b/drivers/gpu/host1x/drm/drm.h
index 6dd75a2600eb..02ce020f2575 100644
--- a/drivers/gpu/drm/tegra/drm.h
+++ b/drivers/gpu/host1x/drm/drm.h
@@ -1,24 +1,36 @@
 /*
  * Copyright (C) 2012 Avionic Design GmbH
- * Copyright (C) 2012 NVIDIA CORPORATION.  All rights reserved.
+ * Copyright (C) 2012-2013 NVIDIA CORPORATION.  All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
 
-#ifndef TEGRA_DRM_H
-#define TEGRA_DRM_H 1
+#ifndef HOST1X_DRM_H
+#define HOST1X_DRM_H 1
 
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
 #include <drm/drm_edid.h>
 #include <drm/drm_fb_helper.h>
-#include <drm/drm_gem_cma_helper.h>
-#include <drm/drm_fb_cma_helper.h>
 #include <drm/drm_fixed.h>
+#include <uapi/drm/tegra_drm.h>
 
-struct host1x {
+#include "host1x.h"
+
+struct tegra_fb {
+	struct drm_framebuffer base;
+	struct tegra_bo **planes;
+	unsigned int num_planes;
+};
+
+struct tegra_fbdev {
+	struct drm_fb_helper base;
+	struct tegra_fb *fb;
+};
+
+struct host1x_drm {
 	struct drm_device *drm;
 	struct device *dev;
 	void __iomem *regs;
@@ -33,31 +45,53 @@ struct host1x {
 	struct mutex clients_lock;
 	struct list_head clients;
 
-	struct drm_fbdev_cma *fbdev;
+	struct tegra_fbdev *fbdev;
 };
 
 struct host1x_client;
 
+struct host1x_drm_context {
+	struct host1x_client *client;
+	struct host1x_channel *channel;
+	struct list_head list;
+};
+
 struct host1x_client_ops {
 	int (*drm_init)(struct host1x_client *client, struct drm_device *drm);
 	int (*drm_exit)(struct host1x_client *client);
+	int (*open_channel)(struct host1x_client *client,
+			    struct host1x_drm_context *context);
+	void (*close_channel)(struct host1x_drm_context *context);
+	int (*submit)(struct host1x_drm_context *context,
+		      struct drm_tegra_submit *args, struct drm_device *drm,
+		      struct drm_file *file);
+};
+
+struct host1x_drm_file {
+	struct list_head contexts;
 };
 
 struct host1x_client {
-	struct host1x *host1x;
+	struct host1x_drm *host1x;
 	struct device *dev;
 
 	const struct host1x_client_ops *ops;
 
+	enum host1x_class class;
+	struct host1x_channel *channel;
+
+	struct host1x_syncpt **syncpts;
+	unsigned int num_syncpts;
+
 	struct list_head list;
 };
 
-extern int host1x_drm_init(struct host1x *host1x, struct drm_device *drm);
-extern int host1x_drm_exit(struct host1x *host1x);
+extern int host1x_drm_init(struct host1x_drm *host1x, struct drm_device *drm);
+extern int host1x_drm_exit(struct host1x_drm *host1x);
 
-extern int host1x_register_client(struct host1x *host1x,
+extern int host1x_register_client(struct host1x_drm *host1x,
 				  struct host1x_client *client);
-extern int host1x_unregister_client(struct host1x *host1x,
+extern int host1x_unregister_client(struct host1x_drm *host1x,
 				    struct host1x_client *client);
 
 struct tegra_output;
@@ -66,7 +100,7 @@ struct tegra_dc {
 	struct host1x_client client;
 	spinlock_t lock;
 
-	struct host1x *host1x;
+	struct host1x_drm *host1x;
 	struct device *dev;
 
 	struct drm_crtc base;
@@ -226,12 +260,12 @@ extern int tegra_output_init(struct drm_device *drm, struct tegra_output *output
 extern int tegra_output_exit(struct tegra_output *output);
 
 /* from fb.c */
+struct tegra_bo *tegra_fb_get_plane(struct drm_framebuffer *framebuffer,
+				    unsigned int index);
 extern int tegra_drm_fb_init(struct drm_device *drm);
 extern void tegra_drm_fb_exit(struct drm_device *drm);
+extern void tegra_fbdev_restore_mode(struct tegra_fbdev *fbdev);
 
-extern struct platform_driver tegra_host1x_driver;
-extern struct platform_driver tegra_hdmi_driver;
-extern struct platform_driver tegra_dc_driver;
 extern struct drm_driver tegra_drm_driver;
 
-#endif /* TEGRA_DRM_H */
+#endif /* HOST1X_DRM_H */
diff --git a/drivers/gpu/host1x/drm/fb.c b/drivers/gpu/host1x/drm/fb.c
new file mode 100644
index 000000000000..979a3e32b78b
--- /dev/null
+++ b/drivers/gpu/host1x/drm/fb.c
@@ -0,0 +1,374 @@
+/*
+ * Copyright (C) 2012-2013 Avionic Design GmbH
+ * Copyright (C) 2012 NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Based on the KMS/FB CMA helpers
+ *   Copyright (C) 2012 Analog Device Inc.
+ *
+ * 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 "drm.h"
+#include "gem.h"
+
+static inline struct tegra_fb *to_tegra_fb(struct drm_framebuffer *fb)
+{
+	return container_of(fb, struct tegra_fb, base);
+}
+
+static inline struct tegra_fbdev *to_tegra_fbdev(struct drm_fb_helper *helper)
+{
+	return container_of(helper, struct tegra_fbdev, base);
+}
+
+struct tegra_bo *tegra_fb_get_plane(struct drm_framebuffer *framebuffer,
+				    unsigned int index)
+{
+	struct tegra_fb *fb = to_tegra_fb(framebuffer);
+
+	if (index >= drm_format_num_planes(framebuffer->pixel_format))
+		return NULL;
+
+	return fb->planes[index];
+}
+
+static void tegra_fb_destroy(struct drm_framebuffer *framebuffer)
+{
+	struct tegra_fb *fb = to_tegra_fb(framebuffer);
+	unsigned int i;
+
+	for (i = 0; i < fb->num_planes; i++) {
+		struct tegra_bo *bo = fb->planes[i];
+
+		if (bo)
+			drm_gem_object_unreference_unlocked(&bo->gem);
+	}
+
+	drm_framebuffer_cleanup(framebuffer);
+	kfree(fb->planes);
+	kfree(fb);
+}
+
+static int tegra_fb_create_handle(struct drm_framebuffer *framebuffer,
+				  struct drm_file *file, unsigned int *handle)
+{
+	struct tegra_fb *fb = to_tegra_fb(framebuffer);
+
+	return drm_gem_handle_create(file, &fb->planes[0]->gem, handle);
+}
+
+static struct drm_framebuffer_funcs tegra_fb_funcs = {
+	.destroy = tegra_fb_destroy,
+	.create_handle = tegra_fb_create_handle,
+};
+
+static struct tegra_fb *tegra_fb_alloc(struct drm_device *drm,
+				       struct drm_mode_fb_cmd2 *mode_cmd,
+				       struct tegra_bo **planes,
+				       unsigned int num_planes)
+{
+	struct tegra_fb *fb;
+	unsigned int i;
+	int err;
+
+	fb = kzalloc(sizeof(*fb), GFP_KERNEL);
+	if (!fb)
+		return ERR_PTR(-ENOMEM);
+
+	fb->planes = kzalloc(num_planes * sizeof(*planes), GFP_KERNEL);
+	if (!fb->planes)
+		return ERR_PTR(-ENOMEM);
+
+	fb->num_planes = num_planes;
+
+	drm_helper_mode_fill_fb_struct(&fb->base, mode_cmd);
+
+	for (i = 0; i < fb->num_planes; i++)
+		fb->planes[i] = planes[i];
+
+	err = drm_framebuffer_init(drm, &fb->base, &tegra_fb_funcs);
+	if (err < 0) {
+		dev_err(drm->dev, "failed to initialize framebuffer: %d\n",
+			err);
+		kfree(fb->planes);
+		kfree(fb);
+		return ERR_PTR(err);
+	}
+
+	return fb;
+}
+
+static struct drm_framebuffer *tegra_fb_create(struct drm_device *drm,
+					       struct drm_file *file,
+					       struct drm_mode_fb_cmd2 *cmd)
+{
+	unsigned int hsub, vsub, i;
+	struct tegra_bo *planes[4];
+	struct drm_gem_object *gem;
+	struct tegra_fb *fb;
+	int err;
+
+	hsub = drm_format_horz_chroma_subsampling(cmd->pixel_format);
+	vsub = drm_format_vert_chroma_subsampling(cmd->pixel_format);
+
+	for (i = 0; i < drm_format_num_planes(cmd->pixel_format); i++) {
+		unsigned int width = cmd->width / (i ? hsub : 1);
+		unsigned int height = cmd->height / (i ? vsub : 1);
+		unsigned int size, bpp;
+
+		gem = drm_gem_object_lookup(drm, file, cmd->handles[i]);
+		if (!gem) {
+			err = -ENXIO;
+			goto unreference;
+		}
+
+		bpp = drm_format_plane_cpp(cmd->pixel_format, i);
+
+		size = (height - 1) * cmd->pitches[i] +
+		       width * bpp + cmd->offsets[i];
+
+		if (gem->size < size) {
+			err = -EINVAL;
+			goto unreference;
+		}
+
+		planes[i] = to_tegra_bo(gem);
+	}
+
+	fb = tegra_fb_alloc(drm, cmd, planes, i);
+	if (IS_ERR(fb)) {
+		err = PTR_ERR(fb);
+		goto unreference;
+	}
+
+	return &fb->base;
+
+unreference:
+	while (i--)
+		drm_gem_object_unreference_unlocked(&planes[i]->gem);
+
+	return ERR_PTR(err);
+}
+
+static struct fb_ops tegra_fb_ops = {
+	.owner = THIS_MODULE,
+	.fb_fillrect = sys_fillrect,
+	.fb_copyarea = sys_copyarea,
+	.fb_imageblit = sys_imageblit,
+	.fb_check_var = drm_fb_helper_check_var,
+	.fb_set_par = drm_fb_helper_set_par,
+	.fb_blank = drm_fb_helper_blank,
+	.fb_pan_display = drm_fb_helper_pan_display,
+	.fb_setcmap = drm_fb_helper_setcmap,
+};
+
+static int tegra_fbdev_probe(struct drm_fb_helper *helper,
+			     struct drm_fb_helper_surface_size *sizes)
+{
+	struct tegra_fbdev *fbdev = to_tegra_fbdev(helper);
+	struct drm_device *drm = helper->dev;
+	struct drm_mode_fb_cmd2 cmd = { 0 };
+	unsigned int bytes_per_pixel;
+	struct drm_framebuffer *fb;
+	unsigned long offset;
+	struct fb_info *info;
+	struct tegra_bo *bo;
+	size_t size;
+	int err;
+
+	bytes_per_pixel = DIV_ROUND_UP(sizes->surface_bpp, 8);
+
+	cmd.width = sizes->surface_width;
+	cmd.height = sizes->surface_height;
+	cmd.pitches[0] = sizes->surface_width * bytes_per_pixel;
+	cmd.pixel_format = drm_mode_legacy_fb_format(sizes->surface_bpp,
+						     sizes->surface_depth);
+
+	size = cmd.pitches[0] * cmd.height;
+
+	bo = tegra_bo_create(drm, size);
+	if (IS_ERR(bo))
+		return PTR_ERR(bo);
+
+	info = framebuffer_alloc(0, drm->dev);
+	if (!info) {
+		dev_err(drm->dev, "failed to allocate framebuffer info\n");
+		tegra_bo_free_object(&bo->gem);
+		return -ENOMEM;
+	}
+
+	fbdev->fb = tegra_fb_alloc(drm, &cmd, &bo, 1);
+	if (IS_ERR(fbdev->fb)) {
+		dev_err(drm->dev, "failed to allocate DRM framebuffer\n");
+		err = PTR_ERR(fbdev->fb);
+		goto release;
+	}
+
+	fb = &fbdev->fb->base;
+	helper->fb = fb;
+	helper->fbdev = info;
+
+	info->par = helper;
+	info->flags = FBINFO_FLAG_DEFAULT;
+	info->fbops = &tegra_fb_ops;
+
+	err = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (err < 0) {
+		dev_err(drm->dev, "failed to allocate color map: %d\n", err);
+		goto destroy;
+	}
+
+	drm_fb_helper_fill_fix(info, fb->pitches[0], fb->depth);
+	drm_fb_helper_fill_var(info, helper, fb->width, fb->height);
+
+	offset = info->var.xoffset * bytes_per_pixel +
+		 info->var.yoffset * fb->pitches[0];
+
+	drm->mode_config.fb_base = (resource_size_t)bo->paddr;
+	info->screen_base = bo->vaddr + offset;
+	info->screen_size = size;
+	info->fix.smem_start = (unsigned long)(bo->paddr + offset);
+	info->fix.smem_len = size;
+
+	return 0;
+
+destroy:
+	drm_framebuffer_unregister_private(fb);
+	tegra_fb_destroy(fb);
+release:
+	framebuffer_release(info);
+	return err;
+}
+
+static struct drm_fb_helper_funcs tegra_fb_helper_funcs = {
+	.fb_probe = tegra_fbdev_probe,
+};
+
+static struct tegra_fbdev *tegra_fbdev_create(struct drm_device *drm,
+					      unsigned int preferred_bpp,
+					      unsigned int num_crtc,
+					      unsigned int max_connectors)
+{
+	struct drm_fb_helper *helper;
+	struct tegra_fbdev *fbdev;
+	int err;
+
+	fbdev = kzalloc(sizeof(*fbdev), GFP_KERNEL);
+	if (!fbdev) {
+		dev_err(drm->dev, "failed to allocate DRM fbdev\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	fbdev->base.funcs = &tegra_fb_helper_funcs;
+	helper = &fbdev->base;
+
+	err = drm_fb_helper_init(drm, &fbdev->base, num_crtc, max_connectors);
+	if (err < 0) {
+		dev_err(drm->dev, "failed to initialize DRM FB helper\n");
+		goto free;
+	}
+
+	err = drm_fb_helper_single_add_all_connectors(&fbdev->base);
+	if (err < 0) {
+		dev_err(drm->dev, "failed to add connectors\n");
+		goto fini;
+	}
+
+	drm_helper_disable_unused_functions(drm);
+
+	err = drm_fb_helper_initial_config(&fbdev->base, preferred_bpp);
+	if (err < 0) {
+		dev_err(drm->dev, "failed to set initial configuration\n");
+		goto fini;
+	}
+
+	return fbdev;
+
+fini:
+	drm_fb_helper_fini(&fbdev->base);
+free:
+	kfree(fbdev);
+	return ERR_PTR(err);
+}
+
+static void tegra_fbdev_free(struct tegra_fbdev *fbdev)
+{
+	struct fb_info *info = fbdev->base.fbdev;
+
+	if (info) {
+		int err;
+
+		err = unregister_framebuffer(info);
+		if (err < 0)
+			DRM_DEBUG_KMS("failed to unregister framebuffer\n");
+
+		if (info->cmap.len)
+			fb_dealloc_cmap(&info->cmap);
+
+		framebuffer_release(info);
+	}
+
+	if (fbdev->fb) {
+		drm_framebuffer_unregister_private(&fbdev->fb->base);
+		tegra_fb_destroy(&fbdev->fb->base);
+	}
+
+	drm_fb_helper_fini(&fbdev->base);
+	kfree(fbdev);
+}
+
+static void tegra_fb_output_poll_changed(struct drm_device *drm)
+{
+	struct host1x_drm *host1x = drm->dev_private;
+
+	if (host1x->fbdev)
+		drm_fb_helper_hotplug_event(&host1x->fbdev->base);
+}
+
+static const struct drm_mode_config_funcs tegra_drm_mode_funcs = {
+	.fb_create = tegra_fb_create,
+	.output_poll_changed = tegra_fb_output_poll_changed,
+};
+
+int tegra_drm_fb_init(struct drm_device *drm)
+{
+	struct host1x_drm *host1x = drm->dev_private;
+	struct tegra_fbdev *fbdev;
+
+	drm->mode_config.min_width = 0;
+	drm->mode_config.min_height = 0;
+
+	drm->mode_config.max_width = 4096;
+	drm->mode_config.max_height = 4096;
+
+	drm->mode_config.funcs = &tegra_drm_mode_funcs;
+
+	fbdev = tegra_fbdev_create(drm, 32, drm->mode_config.num_crtc,
+				   drm->mode_config.num_connector);
+	if (IS_ERR(fbdev))
+		return PTR_ERR(fbdev);
+
+	host1x->fbdev = fbdev;
+
+	return 0;
+}
+
+void tegra_drm_fb_exit(struct drm_device *drm)
+{
+	struct host1x_drm *host1x = drm->dev_private;
+
+	tegra_fbdev_free(host1x->fbdev);
+}
+
+void tegra_fbdev_restore_mode(struct tegra_fbdev *fbdev)
+{
+	if (fbdev) {
+		drm_modeset_lock_all(fbdev->base.dev);
+		drm_fb_helper_restore_fbdev_mode(&fbdev->base);
+		drm_modeset_unlock_all(fbdev->base.dev);
+	}
+}
diff --git a/drivers/gpu/host1x/drm/gem.c b/drivers/gpu/host1x/drm/gem.c
new file mode 100644
index 000000000000..c5e9a9b494c2
--- /dev/null
+++ b/drivers/gpu/host1x/drm/gem.c
@@ -0,0 +1,270 @@
+/*
+ * NVIDIA Tegra DRM GEM helper functions
+ *
+ * Copyright (C) 2012 Sascha Hauer, Pengutronix
+ * Copyright (C) 2013 NVIDIA CORPORATION, All rights reserved.
+ *
+ * Based on the GEM/CMA helpers
+ *
+ * Copyright (c) 2011 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.
+ * 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.
+ */
+
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/export.h>
+#include <linux/dma-mapping.h>
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+
+#include "gem.h"
+
+static inline struct tegra_bo *host1x_to_drm_bo(struct host1x_bo *bo)
+{
+	return container_of(bo, struct tegra_bo, base);
+}
+
+static void tegra_bo_put(struct host1x_bo *bo)
+{
+	struct tegra_bo *obj = host1x_to_drm_bo(bo);
+	struct drm_device *drm = obj->gem.dev;
+
+	mutex_lock(&drm->struct_mutex);
+	drm_gem_object_unreference(&obj->gem);
+	mutex_unlock(&drm->struct_mutex);
+}
+
+static dma_addr_t tegra_bo_pin(struct host1x_bo *bo, struct sg_table **sgt)
+{
+	struct tegra_bo *obj = host1x_to_drm_bo(bo);
+
+	return obj->paddr;
+}
+
+static void tegra_bo_unpin(struct host1x_bo *bo, struct sg_table *sgt)
+{
+}
+
+static void *tegra_bo_mmap(struct host1x_bo *bo)
+{
+	struct tegra_bo *obj = host1x_to_drm_bo(bo);
+
+	return obj->vaddr;
+}
+
+static void tegra_bo_munmap(struct host1x_bo *bo, void *addr)
+{
+}
+
+static void *tegra_bo_kmap(struct host1x_bo *bo, unsigned int page)
+{
+	struct tegra_bo *obj = host1x_to_drm_bo(bo);
+
+	return obj->vaddr + page * PAGE_SIZE;
+}
+
+static void tegra_bo_kunmap(struct host1x_bo *bo, unsigned int page,
+			    void *addr)
+{
+}
+
+static struct host1x_bo *tegra_bo_get(struct host1x_bo *bo)
+{
+	struct tegra_bo *obj = host1x_to_drm_bo(bo);
+	struct drm_device *drm = obj->gem.dev;
+
+	mutex_lock(&drm->struct_mutex);
+	drm_gem_object_reference(&obj->gem);
+	mutex_unlock(&drm->struct_mutex);
+
+	return bo;
+}
+
+const struct host1x_bo_ops tegra_bo_ops = {
+	.get = tegra_bo_get,
+	.put = tegra_bo_put,
+	.pin = tegra_bo_pin,
+	.unpin = tegra_bo_unpin,
+	.mmap = tegra_bo_mmap,
+	.munmap = tegra_bo_munmap,
+	.kmap = tegra_bo_kmap,
+	.kunmap = tegra_bo_kunmap,
+};
+
+static void tegra_bo_destroy(struct drm_device *drm, struct tegra_bo *bo)
+{
+	dma_free_writecombine(drm->dev, bo->gem.size, bo->vaddr, bo->paddr);
+}
+
+unsigned int tegra_bo_get_mmap_offset(struct tegra_bo *bo)
+{
+	return (unsigned int)bo->gem.map_list.hash.key << PAGE_SHIFT;
+}
+
+struct tegra_bo *tegra_bo_create(struct drm_device *drm, unsigned int size)
+{
+	struct tegra_bo *bo;
+	int err;
+
+	bo = kzalloc(sizeof(*bo), GFP_KERNEL);
+	if (!bo)
+		return ERR_PTR(-ENOMEM);
+
+	host1x_bo_init(&bo->base, &tegra_bo_ops);
+	size = round_up(size, PAGE_SIZE);
+
+	bo->vaddr = dma_alloc_writecombine(drm->dev, size, &bo->paddr,
+					   GFP_KERNEL | __GFP_NOWARN);
+	if (!bo->vaddr) {
+		dev_err(drm->dev, "failed to allocate buffer with size %u\n",
+			size);
+		err = -ENOMEM;
+		goto err_dma;
+	}
+
+	err = drm_gem_object_init(drm, &bo->gem, size);
+	if (err)
+		goto err_init;
+
+	err = drm_gem_create_mmap_offset(&bo->gem);
+	if (err)
+		goto err_mmap;
+
+	return bo;
+
+err_mmap:
+	drm_gem_object_release(&bo->gem);
+err_init:
+	tegra_bo_destroy(drm, bo);
+err_dma:
+	kfree(bo);
+
+	return ERR_PTR(err);
+
+}
+
+struct tegra_bo *tegra_bo_create_with_handle(struct drm_file *file,
+					    struct drm_device *drm,
+					    unsigned int size,
+					    unsigned int *handle)
+{
+	struct tegra_bo *bo;
+	int ret;
+
+	bo = tegra_bo_create(drm, size);
+	if (IS_ERR(bo))
+		return bo;
+
+	ret = drm_gem_handle_create(file, &bo->gem, handle);
+	if (ret)
+		goto err;
+
+	drm_gem_object_unreference_unlocked(&bo->gem);
+
+	return bo;
+
+err:
+	tegra_bo_free_object(&bo->gem);
+	return ERR_PTR(ret);
+}
+
+void tegra_bo_free_object(struct drm_gem_object *gem)
+{
+	struct tegra_bo *bo = to_tegra_bo(gem);
+
+	if (gem->map_list.map)
+		drm_gem_free_mmap_offset(gem);
+
+	drm_gem_object_release(gem);
+	tegra_bo_destroy(gem->dev, bo);
+
+	kfree(bo);
+}
+
+int tegra_bo_dumb_create(struct drm_file *file, struct drm_device *drm,
+			 struct drm_mode_create_dumb *args)
+{
+	int min_pitch = DIV_ROUND_UP(args->width * args->bpp, 8);
+	struct tegra_bo *bo;
+
+	if (args->pitch < min_pitch)
+		args->pitch = min_pitch;
+
+	if (args->size < args->pitch * args->height)
+		args->size = args->pitch * args->height;
+
+	bo = tegra_bo_create_with_handle(file, drm, args->size,
+					    &args->handle);
+	if (IS_ERR(bo))
+		return PTR_ERR(bo);
+
+	return 0;
+}
+
+int tegra_bo_dumb_map_offset(struct drm_file *file, struct drm_device *drm,
+			     uint32_t handle, uint64_t *offset)
+{
+	struct drm_gem_object *gem;
+	struct tegra_bo *bo;
+
+	mutex_lock(&drm->struct_mutex);
+
+	gem = drm_gem_object_lookup(drm, file, handle);
+	if (!gem) {
+		dev_err(drm->dev, "failed to lookup GEM object\n");
+		mutex_unlock(&drm->struct_mutex);
+		return -EINVAL;
+	}
+
+	bo = to_tegra_bo(gem);
+
+	*offset = tegra_bo_get_mmap_offset(bo);
+
+	drm_gem_object_unreference(gem);
+
+	mutex_unlock(&drm->struct_mutex);
+
+	return 0;
+}
+
+const struct vm_operations_struct tegra_bo_vm_ops = {
+	.open = drm_gem_vm_open,
+	.close = drm_gem_vm_close,
+};
+
+int tegra_drm_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct drm_gem_object *gem;
+	struct tegra_bo *bo;
+	int ret;
+
+	ret = drm_gem_mmap(file, vma);
+	if (ret)
+		return ret;
+
+	gem = vma->vm_private_data;
+	bo = to_tegra_bo(gem);
+
+	ret = remap_pfn_range(vma, vma->vm_start, bo->paddr >> PAGE_SHIFT,
+			      vma->vm_end - vma->vm_start, vma->vm_page_prot);
+	if (ret)
+		drm_gem_vm_close(vma);
+
+	return ret;
+}
+
+int tegra_bo_dumb_destroy(struct drm_file *file, struct drm_device *drm,
+			  unsigned int handle)
+{
+	return drm_gem_handle_delete(file, handle);
+}
diff --git a/drivers/gpu/host1x/drm/gem.h b/drivers/gpu/host1x/drm/gem.h
new file mode 100644
index 000000000000..34de2b486eb7
--- /dev/null
+++ b/drivers/gpu/host1x/drm/gem.h
@@ -0,0 +1,59 @@
+/*
+ * Tegra host1x GEM implementation
+ *
+ * Copyright (c) 2012-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __HOST1X_GEM_H
+#define __HOST1X_GEM_H
+
+#include <drm/drm.h>
+#include <drm/drmP.h>
+
+#include "host1x_bo.h"
+
+struct tegra_bo {
+	struct drm_gem_object gem;
+	struct host1x_bo base;
+	dma_addr_t paddr;
+	void *vaddr;
+};
+
+static inline struct tegra_bo *to_tegra_bo(struct drm_gem_object *gem)
+{
+	return container_of(gem, struct tegra_bo, gem);
+}
+
+extern const struct host1x_bo_ops tegra_bo_ops;
+
+struct tegra_bo *tegra_bo_create(struct drm_device *drm, unsigned int size);
+struct tegra_bo *tegra_bo_create_with_handle(struct drm_file *file,
+					    struct drm_device *drm,
+					    unsigned int size,
+					    unsigned int *handle);
+void tegra_bo_free_object(struct drm_gem_object *gem);
+unsigned int tegra_bo_get_mmap_offset(struct tegra_bo *bo);
+int tegra_bo_dumb_create(struct drm_file *file, struct drm_device *drm,
+			 struct drm_mode_create_dumb *args);
+int tegra_bo_dumb_map_offset(struct drm_file *file, struct drm_device *drm,
+			     uint32_t handle, uint64_t *offset);
+int tegra_bo_dumb_destroy(struct drm_file *file, struct drm_device *drm,
+			  unsigned int handle);
+
+int tegra_drm_mmap(struct file *file, struct vm_area_struct *vma);
+
+extern const struct vm_operations_struct tegra_bo_vm_ops;
+
+#endif
diff --git a/drivers/gpu/host1x/drm/gr2d.c b/drivers/gpu/host1x/drm/gr2d.c
new file mode 100644
index 000000000000..6a45ae090ee7
--- /dev/null
+++ b/drivers/gpu/host1x/drm/gr2d.c
@@ -0,0 +1,339 @@
+/*
+ * drivers/video/tegra/host/gr2d/gr2d.c
+ *
+ * Tegra Graphics 2D
+ *
+ * Copyright (c) 2012-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/export.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/clk.h>
+
+#include "channel.h"
+#include "drm.h"
+#include "gem.h"
+#include "job.h"
+#include "host1x.h"
+#include "host1x_bo.h"
+#include "host1x_client.h"
+#include "syncpt.h"
+
+struct gr2d {
+	struct host1x_client client;
+	struct clk *clk;
+	struct host1x_channel *channel;
+	unsigned long *addr_regs;
+};
+
+static inline struct gr2d *to_gr2d(struct host1x_client *client)
+{
+	return container_of(client, struct gr2d, client);
+}
+
+static int gr2d_is_addr_reg(struct device *dev, u32 class, u32 reg);
+
+static int gr2d_client_init(struct host1x_client *client,
+			    struct drm_device *drm)
+{
+	return 0;
+}
+
+static int gr2d_client_exit(struct host1x_client *client)
+{
+	return 0;
+}
+
+static int gr2d_open_channel(struct host1x_client *client,
+			     struct host1x_drm_context *context)
+{
+	struct gr2d *gr2d = to_gr2d(client);
+
+	context->channel = host1x_channel_get(gr2d->channel);
+
+	if (!context->channel)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void gr2d_close_channel(struct host1x_drm_context *context)
+{
+	host1x_channel_put(context->channel);
+}
+
+static struct host1x_bo *host1x_bo_lookup(struct drm_device *drm,
+					  struct drm_file *file,
+					  u32 handle)
+{
+	struct drm_gem_object *gem;
+	struct tegra_bo *bo;
+
+	gem = drm_gem_object_lookup(drm, file, handle);
+	if (!gem)
+		return 0;
+
+	mutex_lock(&drm->struct_mutex);
+	drm_gem_object_unreference(gem);
+	mutex_unlock(&drm->struct_mutex);
+
+	bo = to_tegra_bo(gem);
+	return &bo->base;
+}
+
+static int gr2d_submit(struct host1x_drm_context *context,
+		       struct drm_tegra_submit *args, struct drm_device *drm,
+		       struct drm_file *file)
+{
+	struct host1x_job *job;
+	unsigned int num_cmdbufs = args->num_cmdbufs;
+	unsigned int num_relocs = args->num_relocs;
+	unsigned int num_waitchks = args->num_waitchks;
+	struct drm_tegra_cmdbuf __user *cmdbufs =
+		(void * __user)(uintptr_t)args->cmdbufs;
+	struct drm_tegra_reloc __user *relocs =
+		(void * __user)(uintptr_t)args->relocs;
+	struct drm_tegra_waitchk __user *waitchks =
+		(void * __user)(uintptr_t)args->waitchks;
+	struct drm_tegra_syncpt syncpt;
+	int err;
+
+	/* We don't yet support other than one syncpt_incr struct per submit */
+	if (args->num_syncpts != 1)
+		return -EINVAL;
+
+	job = host1x_job_alloc(context->channel, args->num_cmdbufs,
+			       args->num_relocs, args->num_waitchks);
+	if (!job)
+		return -ENOMEM;
+
+	job->num_relocs = args->num_relocs;
+	job->num_waitchk = args->num_waitchks;
+	job->client = (u32)args->context;
+	job->class = context->client->class;
+	job->serialize = true;
+
+	while (num_cmdbufs) {
+		struct drm_tegra_cmdbuf cmdbuf;
+		struct host1x_bo *bo;
+
+		err = copy_from_user(&cmdbuf, cmdbufs, sizeof(cmdbuf));
+		if (err)
+			goto fail;
+
+		bo = host1x_bo_lookup(drm, file, cmdbuf.handle);
+		if (!bo)
+			goto fail;
+
+		host1x_job_add_gather(job, bo, cmdbuf.words, cmdbuf.offset);
+		num_cmdbufs--;
+		cmdbufs++;
+	}
+
+	err = copy_from_user(job->relocarray, relocs,
+			     sizeof(*relocs) * num_relocs);
+	if (err)
+		goto fail;
+
+	while (num_relocs--) {
+		struct host1x_reloc *reloc = &job->relocarray[num_relocs];
+		struct host1x_bo *cmdbuf, *target;
+
+		cmdbuf = host1x_bo_lookup(drm, file, (u32)reloc->cmdbuf);
+		target = host1x_bo_lookup(drm, file, (u32)reloc->target);
+
+		reloc->cmdbuf = cmdbuf;
+		reloc->target = target;
+
+		if (!reloc->target || !reloc->cmdbuf)
+			goto fail;
+	}
+
+	err = copy_from_user(job->waitchk, waitchks,
+			     sizeof(*waitchks) * num_waitchks);
+	if (err)
+		goto fail;
+
+	err = copy_from_user(&syncpt, (void * __user)(uintptr_t)args->syncpts,
+			     sizeof(syncpt));
+	if (err)
+		goto fail;
+
+	job->syncpt_id = syncpt.id;
+	job->syncpt_incrs = syncpt.incrs;
+	job->timeout = 10000;
+	job->is_addr_reg = gr2d_is_addr_reg;
+
+	if (args->timeout && args->timeout < 10000)
+		job->timeout = args->timeout;
+
+	err = host1x_job_pin(job, context->client->dev);
+	if (err)
+		goto fail;
+
+	err = host1x_job_submit(job);
+	if (err)
+		goto fail_submit;
+
+	args->fence = job->syncpt_end;
+
+	host1x_job_put(job);
+	return 0;
+
+fail_submit:
+	host1x_job_unpin(job);
+fail:
+	host1x_job_put(job);
+	return err;
+}
+
+static struct host1x_client_ops gr2d_client_ops = {
+	.drm_init = gr2d_client_init,
+	.drm_exit = gr2d_client_exit,
+	.open_channel = gr2d_open_channel,
+	.close_channel = gr2d_close_channel,
+	.submit = gr2d_submit,
+};
+
+static void gr2d_init_addr_reg_map(struct device *dev, struct gr2d *gr2d)
+{
+	const u32 gr2d_addr_regs[] = {0x1a, 0x1b, 0x26, 0x2b, 0x2c, 0x2d, 0x31,
+				      0x32, 0x48, 0x49, 0x4a, 0x4b, 0x4c};
+	unsigned long *bitmap;
+	int i;
+
+	bitmap = devm_kzalloc(dev, DIV_ROUND_UP(256, BITS_PER_BYTE),
+			      GFP_KERNEL);
+
+	for (i = 0; i < ARRAY_SIZE(gr2d_addr_regs); ++i) {
+		u32 reg = gr2d_addr_regs[i];
+		bitmap[BIT_WORD(reg)] |= BIT_MASK(reg);
+	}
+
+	gr2d->addr_regs = bitmap;
+}
+
+static int gr2d_is_addr_reg(struct device *dev, u32 class, u32 reg)
+{
+	struct gr2d *gr2d = dev_get_drvdata(dev);
+
+	switch (class) {
+	case HOST1X_CLASS_HOST1X:
+		return reg == 0x2b;
+	case HOST1X_CLASS_GR2D:
+	case HOST1X_CLASS_GR2D_SB:
+		reg &= 0xff;
+		if (gr2d->addr_regs[BIT_WORD(reg)] & BIT_MASK(reg))
+			return 1;
+	default:
+		return 0;
+	}
+}
+
+static const struct of_device_id gr2d_match[] = {
+	{ .compatible = "nvidia,tegra30-gr2d" },
+	{ .compatible = "nvidia,tegra20-gr2d" },
+	{ },
+};
+
+static int gr2d_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct host1x_drm *host1x = host1x_get_drm_data(dev->parent);
+	int err;
+	struct gr2d *gr2d = NULL;
+	struct host1x_syncpt **syncpts;
+
+	gr2d = devm_kzalloc(dev, sizeof(*gr2d), GFP_KERNEL);
+	if (!gr2d)
+		return -ENOMEM;
+
+	syncpts = devm_kzalloc(dev, sizeof(*syncpts), GFP_KERNEL);
+	if (!syncpts)
+		return -ENOMEM;
+
+	gr2d->clk = devm_clk_get(dev, NULL);
+	if (IS_ERR(gr2d->clk)) {
+		dev_err(dev, "cannot get clock\n");
+		return PTR_ERR(gr2d->clk);
+	}
+
+	err = clk_prepare_enable(gr2d->clk);
+	if (err) {
+		dev_err(dev, "cannot turn on clock\n");
+		return err;
+	}
+
+	gr2d->channel = host1x_channel_request(dev);
+	if (!gr2d->channel)
+		return -ENOMEM;
+
+	*syncpts = host1x_syncpt_request(dev, 0);
+	if (!(*syncpts)) {
+		host1x_channel_free(gr2d->channel);
+		return -ENOMEM;
+	}
+
+	gr2d->client.ops = &gr2d_client_ops;
+	gr2d->client.dev = dev;
+	gr2d->client.class = HOST1X_CLASS_GR2D;
+	gr2d->client.syncpts = syncpts;
+	gr2d->client.num_syncpts = 1;
+
+	err = host1x_register_client(host1x, &gr2d->client);
+	if (err < 0) {
+		dev_err(dev, "failed to register host1x client: %d\n", err);
+		return err;
+	}
+
+	gr2d_init_addr_reg_map(dev, gr2d);
+
+	platform_set_drvdata(pdev, gr2d);
+
+	return 0;
+}
+
+static int __exit gr2d_remove(struct platform_device *pdev)
+{
+	struct host1x_drm *host1x = host1x_get_drm_data(pdev->dev.parent);
+	struct gr2d *gr2d = platform_get_drvdata(pdev);
+	unsigned int i;
+	int err;
+
+	err = host1x_unregister_client(host1x, &gr2d->client);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to unregister client: %d\n", err);
+		return err;
+	}
+
+	for (i = 0; i < gr2d->client.num_syncpts; i++)
+		host1x_syncpt_free(gr2d->client.syncpts[i]);
+
+	host1x_channel_free(gr2d->channel);
+	clk_disable_unprepare(gr2d->clk);
+
+	return 0;
+}
+
+struct platform_driver tegra_gr2d_driver = {
+	.probe = gr2d_probe,
+	.remove = __exit_p(gr2d_remove),
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "gr2d",
+		.of_match_table = gr2d_match,
+	}
+};
diff --git a/drivers/gpu/drm/tegra/hdmi.c b/drivers/gpu/host1x/drm/hdmi.c
index bb747f6cd1a4..01097da09f7f 100644
--- a/drivers/gpu/drm/tegra/hdmi.c
+++ b/drivers/gpu/host1x/drm/hdmi.c
@@ -22,6 +22,7 @@
 #include "hdmi.h"
 #include "drm.h"
 #include "dc.h"
+#include "host1x_client.h"
 
 struct tegra_hdmi {
 	struct host1x_client client;
@@ -1189,7 +1190,7 @@ static const struct host1x_client_ops hdmi_client_ops = {
 
 static int tegra_hdmi_probe(struct platform_device *pdev)
 {
-	struct host1x *host1x = dev_get_drvdata(pdev->dev.parent);
+	struct host1x_drm *host1x = host1x_get_drm_data(pdev->dev.parent);
 	struct tegra_hdmi *hdmi;
 	struct resource *regs;
 	int err;
@@ -1278,7 +1279,7 @@ static int tegra_hdmi_probe(struct platform_device *pdev)
 
 static int tegra_hdmi_remove(struct platform_device *pdev)
 {
-	struct host1x *host1x = dev_get_drvdata(pdev->dev.parent);
+	struct host1x_drm *host1x = host1x_get_drm_data(pdev->dev.parent);
 	struct tegra_hdmi *hdmi = platform_get_drvdata(pdev);
 	int err;
 
diff --git a/drivers/gpu/drm/tegra/hdmi.h b/drivers/gpu/host1x/drm/hdmi.h
index 52ac36e08ccb..52ac36e08ccb 100644
--- a/drivers/gpu/drm/tegra/hdmi.h
+++ b/drivers/gpu/host1x/drm/hdmi.h
diff --git a/drivers/gpu/drm/tegra/output.c b/drivers/gpu/host1x/drm/output.c
index 8140fc6c34d8..8140fc6c34d8 100644
--- a/drivers/gpu/drm/tegra/output.c
+++ b/drivers/gpu/host1x/drm/output.c
diff --git a/drivers/gpu/drm/tegra/rgb.c b/drivers/gpu/host1x/drm/rgb.c
index ed4416f20260..ed4416f20260 100644
--- a/drivers/gpu/drm/tegra/rgb.c
+++ b/drivers/gpu/host1x/drm/rgb.c
diff --git a/drivers/gpu/host1x/host1x.h b/drivers/gpu/host1x/host1x.h
new file mode 100644
index 000000000000..a2bc1e65e972
--- /dev/null
+++ b/drivers/gpu/host1x/host1x.h
@@ -0,0 +1,30 @@
+/*
+ * Tegra host1x driver
+ *
+ * Copyright (c) 2009-2013, NVIDIA Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __LINUX_HOST1X_H
+#define __LINUX_HOST1X_H
+
+enum host1x_class {
+	HOST1X_CLASS_HOST1X	= 0x1,
+	HOST1X_CLASS_GR2D	= 0x51,
+	HOST1X_CLASS_GR2D_SB    = 0x52
+};
+
+#endif
diff --git a/drivers/gpu/host1x/host1x_bo.h b/drivers/gpu/host1x/host1x_bo.h
new file mode 100644
index 000000000000..4c1f10bd773d
--- /dev/null
+++ b/drivers/gpu/host1x/host1x_bo.h
@@ -0,0 +1,87 @@
+/*
+ * Tegra host1x Memory Management Abstraction header
+ *
+ * Copyright (c) 2012-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _HOST1X_BO_H
+#define _HOST1X_BO_H
+
+struct host1x_bo;
+
+struct host1x_bo_ops {
+	struct host1x_bo *(*get)(struct host1x_bo *bo);
+	void (*put)(struct host1x_bo *bo);
+	dma_addr_t (*pin)(struct host1x_bo *bo, struct sg_table **sgt);
+	void (*unpin)(struct host1x_bo *bo, struct sg_table *sgt);
+	void *(*mmap)(struct host1x_bo *bo);
+	void (*munmap)(struct host1x_bo *bo, void *addr);
+	void *(*kmap)(struct host1x_bo *bo, unsigned int pagenum);
+	void (*kunmap)(struct host1x_bo *bo, unsigned int pagenum, void *addr);
+};
+
+struct host1x_bo {
+	const struct host1x_bo_ops *ops;
+};
+
+static inline void host1x_bo_init(struct host1x_bo *bo,
+				  const struct host1x_bo_ops *ops)
+{
+	bo->ops = ops;
+}
+
+static inline struct host1x_bo *host1x_bo_get(struct host1x_bo *bo)
+{
+	return bo->ops->get(bo);
+}
+
+static inline void host1x_bo_put(struct host1x_bo *bo)
+{
+	bo->ops->put(bo);
+}
+
+static inline dma_addr_t host1x_bo_pin(struct host1x_bo *bo,
+				       struct sg_table **sgt)
+{
+	return bo->ops->pin(bo, sgt);
+}
+
+static inline void host1x_bo_unpin(struct host1x_bo *bo, struct sg_table *sgt)
+{
+	bo->ops->unpin(bo, sgt);
+}
+
+static inline void *host1x_bo_mmap(struct host1x_bo *bo)
+{
+	return bo->ops->mmap(bo);
+}
+
+static inline void host1x_bo_munmap(struct host1x_bo *bo, void *addr)
+{
+	bo->ops->munmap(bo, addr);
+}
+
+static inline void *host1x_bo_kmap(struct host1x_bo *bo, unsigned int pagenum)
+{
+	return bo->ops->kmap(bo, pagenum);
+}
+
+static inline void host1x_bo_kunmap(struct host1x_bo *bo,
+				    unsigned int pagenum, void *addr)
+{
+	bo->ops->kunmap(bo, pagenum, addr);
+}
+
+#endif
diff --git a/drivers/gpu/host1x/host1x_client.h b/drivers/gpu/host1x/host1x_client.h
new file mode 100644
index 000000000000..9b85f10f4a44
--- /dev/null
+++ b/drivers/gpu/host1x/host1x_client.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef HOST1X_CLIENT_H
+#define HOST1X_CLIENT_H
+
+struct device;
+struct platform_device;
+
+#ifdef CONFIG_DRM_TEGRA
+int host1x_drm_alloc(struct platform_device *pdev);
+#else
+static inline int host1x_drm_alloc(struct platform_device *pdev)
+{
+	return 0;
+}
+#endif
+
+void host1x_set_drm_data(struct device *dev, void *data);
+void *host1x_get_drm_data(struct device *dev);
+
+#endif
diff --git a/drivers/gpu/host1x/hw/Makefile b/drivers/gpu/host1x/hw/Makefile
new file mode 100644
index 000000000000..9b50863a2236
--- /dev/null
+++ b/drivers/gpu/host1x/hw/Makefile
@@ -0,0 +1,6 @@
+ccflags-y = -Idrivers/gpu/host1x
+
+host1x-hw-objs  = \
+	host1x01.o
+
+obj-$(CONFIG_TEGRA_HOST1X) += host1x-hw.o
diff --git a/drivers/gpu/host1x/hw/cdma_hw.c b/drivers/gpu/host1x/hw/cdma_hw.c
new file mode 100644
index 000000000000..590b69d91dab
--- /dev/null
+++ b/drivers/gpu/host1x/hw/cdma_hw.c
@@ -0,0 +1,326 @@
+/*
+ * Tegra host1x Command DMA
+ *
+ * Copyright (c) 2010-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/slab.h>
+#include <linux/scatterlist.h>
+#include <linux/dma-mapping.h>
+
+#include "cdma.h"
+#include "channel.h"
+#include "dev.h"
+#include "debug.h"
+
+/*
+ * Put the restart at the end of pushbuffer memor
+ */
+static void push_buffer_init(struct push_buffer *pb)
+{
+	*(pb->mapped + (pb->size_bytes >> 2)) = host1x_opcode_restart(0);
+}
+
+/*
+ * Increment timedout buffer's syncpt via CPU.
+ */
+static void cdma_timeout_cpu_incr(struct host1x_cdma *cdma, u32 getptr,
+				u32 syncpt_incrs, u32 syncval, u32 nr_slots)
+{
+	struct host1x *host1x = cdma_to_host1x(cdma);
+	struct push_buffer *pb = &cdma->push_buffer;
+	u32 i;
+
+	for (i = 0; i < syncpt_incrs; i++)
+		host1x_syncpt_cpu_incr(cdma->timeout.syncpt);
+
+	/* after CPU incr, ensure shadow is up to date */
+	host1x_syncpt_load(cdma->timeout.syncpt);
+
+	/* NOP all the PB slots */
+	while (nr_slots--) {
+		u32 *p = (u32 *)((u32)pb->mapped + getptr);
+		*(p++) = HOST1X_OPCODE_NOP;
+		*(p++) = HOST1X_OPCODE_NOP;
+		dev_dbg(host1x->dev, "%s: NOP at 0x%x\n", __func__,
+			pb->phys + getptr);
+		getptr = (getptr + 8) & (pb->size_bytes - 1);
+	}
+	wmb();
+}
+
+/*
+ * Start channel DMA
+ */
+static void cdma_start(struct host1x_cdma *cdma)
+{
+	struct host1x_channel *ch = cdma_to_channel(cdma);
+
+	if (cdma->running)
+		return;
+
+	cdma->last_pos = cdma->push_buffer.pos;
+
+	host1x_ch_writel(ch, HOST1X_CHANNEL_DMACTRL_DMASTOP,
+			 HOST1X_CHANNEL_DMACTRL);
+
+	/* set base, put and end pointer */
+	host1x_ch_writel(ch, cdma->push_buffer.phys, HOST1X_CHANNEL_DMASTART);
+	host1x_ch_writel(ch, cdma->push_buffer.pos, HOST1X_CHANNEL_DMAPUT);
+	host1x_ch_writel(ch, cdma->push_buffer.phys +
+			 cdma->push_buffer.size_bytes + 4,
+			 HOST1X_CHANNEL_DMAEND);
+
+	/* reset GET */
+	host1x_ch_writel(ch, HOST1X_CHANNEL_DMACTRL_DMASTOP |
+			 HOST1X_CHANNEL_DMACTRL_DMAGETRST |
+			 HOST1X_CHANNEL_DMACTRL_DMAINITGET,
+			 HOST1X_CHANNEL_DMACTRL);
+
+	/* start the command DMA */
+	host1x_ch_writel(ch, 0, HOST1X_CHANNEL_DMACTRL);
+
+	cdma->running = true;
+}
+
+/*
+ * Similar to cdma_start(), but rather than starting from an idle
+ * state (where DMA GET is set to DMA PUT), on a timeout we restore
+ * DMA GET from an explicit value (so DMA may again be pending).
+ */
+static void cdma_timeout_restart(struct host1x_cdma *cdma, u32 getptr)
+{
+	struct host1x *host1x = cdma_to_host1x(cdma);
+	struct host1x_channel *ch = cdma_to_channel(cdma);
+
+	if (cdma->running)
+		return;
+
+	cdma->last_pos = cdma->push_buffer.pos;
+
+	host1x_ch_writel(ch, HOST1X_CHANNEL_DMACTRL_DMASTOP,
+			 HOST1X_CHANNEL_DMACTRL);
+
+	/* set base, end pointer (all of memory) */
+	host1x_ch_writel(ch, cdma->push_buffer.phys, HOST1X_CHANNEL_DMASTART);
+	host1x_ch_writel(ch, cdma->push_buffer.phys +
+			 cdma->push_buffer.size_bytes,
+			 HOST1X_CHANNEL_DMAEND);
+
+	/* set GET, by loading the value in PUT (then reset GET) */
+	host1x_ch_writel(ch, getptr, HOST1X_CHANNEL_DMAPUT);
+	host1x_ch_writel(ch, HOST1X_CHANNEL_DMACTRL_DMASTOP |
+			 HOST1X_CHANNEL_DMACTRL_DMAGETRST |
+			 HOST1X_CHANNEL_DMACTRL_DMAINITGET,
+			 HOST1X_CHANNEL_DMACTRL);
+
+	dev_dbg(host1x->dev,
+		"%s: DMA GET 0x%x, PUT HW 0x%x / shadow 0x%x\n", __func__,
+		host1x_ch_readl(ch, HOST1X_CHANNEL_DMAGET),
+		host1x_ch_readl(ch, HOST1X_CHANNEL_DMAPUT),
+		cdma->last_pos);
+
+	/* deassert GET reset and set PUT */
+	host1x_ch_writel(ch, HOST1X_CHANNEL_DMACTRL_DMASTOP,
+			 HOST1X_CHANNEL_DMACTRL);
+	host1x_ch_writel(ch, cdma->push_buffer.pos, HOST1X_CHANNEL_DMAPUT);
+
+	/* start the command DMA */
+	host1x_ch_writel(ch, 0, HOST1X_CHANNEL_DMACTRL);
+
+	cdma->running = true;
+}
+
+/*
+ * Kick channel DMA into action by writing its PUT offset (if it has changed)
+ */
+static void cdma_flush(struct host1x_cdma *cdma)
+{
+	struct host1x_channel *ch = cdma_to_channel(cdma);
+
+	if (cdma->push_buffer.pos != cdma->last_pos) {
+		host1x_ch_writel(ch, cdma->push_buffer.pos,
+				 HOST1X_CHANNEL_DMAPUT);
+		cdma->last_pos = cdma->push_buffer.pos;
+	}
+}
+
+static void cdma_stop(struct host1x_cdma *cdma)
+{
+	struct host1x_channel *ch = cdma_to_channel(cdma);
+
+	mutex_lock(&cdma->lock);
+	if (cdma->running) {
+		host1x_cdma_wait_locked(cdma, CDMA_EVENT_SYNC_QUEUE_EMPTY);
+		host1x_ch_writel(ch, HOST1X_CHANNEL_DMACTRL_DMASTOP,
+				 HOST1X_CHANNEL_DMACTRL);
+		cdma->running = false;
+	}
+	mutex_unlock(&cdma->lock);
+}
+
+/*
+ * Stops both channel's command processor and CDMA immediately.
+ * Also, tears down the channel and resets corresponding module.
+ */
+static void cdma_freeze(struct host1x_cdma *cdma)
+{
+	struct host1x *host = cdma_to_host1x(cdma);
+	struct host1x_channel *ch = cdma_to_channel(cdma);
+	u32 cmdproc_stop;
+
+	if (cdma->torndown && !cdma->running) {
+		dev_warn(host->dev, "Already torn down\n");
+		return;
+	}
+
+	dev_dbg(host->dev, "freezing channel (id %d)\n", ch->id);
+
+	cmdproc_stop = host1x_sync_readl(host, HOST1X_SYNC_CMDPROC_STOP);
+	cmdproc_stop |= BIT(ch->id);
+	host1x_sync_writel(host, cmdproc_stop, HOST1X_SYNC_CMDPROC_STOP);
+
+	dev_dbg(host->dev, "%s: DMA GET 0x%x, PUT HW 0x%x / shadow 0x%x\n",
+		__func__, host1x_ch_readl(ch, HOST1X_CHANNEL_DMAGET),
+		host1x_ch_readl(ch, HOST1X_CHANNEL_DMAPUT),
+		cdma->last_pos);
+
+	host1x_ch_writel(ch, HOST1X_CHANNEL_DMACTRL_DMASTOP,
+			 HOST1X_CHANNEL_DMACTRL);
+
+	host1x_sync_writel(host, BIT(ch->id), HOST1X_SYNC_CH_TEARDOWN);
+
+	cdma->running = false;
+	cdma->torndown = true;
+}
+
+static void cdma_resume(struct host1x_cdma *cdma, u32 getptr)
+{
+	struct host1x *host1x = cdma_to_host1x(cdma);
+	struct host1x_channel *ch = cdma_to_channel(cdma);
+	u32 cmdproc_stop;
+
+	dev_dbg(host1x->dev,
+		"resuming channel (id %d, DMAGET restart = 0x%x)\n",
+		ch->id, getptr);
+
+	cmdproc_stop = host1x_sync_readl(host1x, HOST1X_SYNC_CMDPROC_STOP);
+	cmdproc_stop &= ~(BIT(ch->id));
+	host1x_sync_writel(host1x, cmdproc_stop, HOST1X_SYNC_CMDPROC_STOP);
+
+	cdma->torndown = false;
+	cdma_timeout_restart(cdma, getptr);
+}
+
+/*
+ * If this timeout fires, it indicates the current sync_queue entry has
+ * exceeded its TTL and the userctx should be timed out and remaining
+ * submits already issued cleaned up (future submits return an error).
+ */
+static void cdma_timeout_handler(struct work_struct *work)
+{
+	struct host1x_cdma *cdma;
+	struct host1x *host1x;
+	struct host1x_channel *ch;
+
+	u32 syncpt_val;
+
+	u32 prev_cmdproc, cmdproc_stop;
+
+	cdma = container_of(to_delayed_work(work), struct host1x_cdma,
+			    timeout.wq);
+	host1x = cdma_to_host1x(cdma);
+	ch = cdma_to_channel(cdma);
+
+	host1x_debug_dump(cdma_to_host1x(cdma));
+
+	mutex_lock(&cdma->lock);
+
+	if (!cdma->timeout.client) {
+		dev_dbg(host1x->dev,
+			"cdma_timeout: expired, but has no clientid\n");
+		mutex_unlock(&cdma->lock);
+		return;
+	}
+
+	/* stop processing to get a clean snapshot */
+	prev_cmdproc = host1x_sync_readl(host1x, HOST1X_SYNC_CMDPROC_STOP);
+	cmdproc_stop = prev_cmdproc | BIT(ch->id);
+	host1x_sync_writel(host1x, cmdproc_stop, HOST1X_SYNC_CMDPROC_STOP);
+
+	dev_dbg(host1x->dev, "cdma_timeout: cmdproc was 0x%x is 0x%x\n",
+		prev_cmdproc, cmdproc_stop);
+
+	syncpt_val = host1x_syncpt_load(cdma->timeout.syncpt);
+
+	/* has buffer actually completed? */
+	if ((s32)(syncpt_val - cdma->timeout.syncpt_val) >= 0) {
+		dev_dbg(host1x->dev,
+			"cdma_timeout: expired, but buffer had completed\n");
+		/* restore */
+		cmdproc_stop = prev_cmdproc & ~(BIT(ch->id));
+		host1x_sync_writel(host1x, cmdproc_stop,
+				   HOST1X_SYNC_CMDPROC_STOP);
+		mutex_unlock(&cdma->lock);
+		return;
+	}
+
+	dev_warn(host1x->dev, "%s: timeout: %d (%s), HW thresh %d, done %d\n",
+		__func__, cdma->timeout.syncpt->id, cdma->timeout.syncpt->name,
+		syncpt_val, cdma->timeout.syncpt_val);
+
+	/* stop HW, resetting channel/module */
+	host1x_hw_cdma_freeze(host1x, cdma);
+
+	host1x_cdma_update_sync_queue(cdma, ch->dev);
+	mutex_unlock(&cdma->lock);
+}
+
+/*
+ * Init timeout resources
+ */
+static int cdma_timeout_init(struct host1x_cdma *cdma, u32 syncpt_id)
+{
+	INIT_DELAYED_WORK(&cdma->timeout.wq, cdma_timeout_handler);
+	cdma->timeout.initialized = true;
+
+	return 0;
+}
+
+/*
+ * Clean up timeout resources
+ */
+static void cdma_timeout_destroy(struct host1x_cdma *cdma)
+{
+	if (cdma->timeout.initialized)
+		cancel_delayed_work(&cdma->timeout.wq);
+	cdma->timeout.initialized = false;
+}
+
+static const struct host1x_cdma_ops host1x_cdma_ops = {
+	.start = cdma_start,
+	.stop = cdma_stop,
+	.flush = cdma_flush,
+
+	.timeout_init = cdma_timeout_init,
+	.timeout_destroy = cdma_timeout_destroy,
+	.freeze = cdma_freeze,
+	.resume = cdma_resume,
+	.timeout_cpu_incr = cdma_timeout_cpu_incr,
+};
+
+static const struct host1x_pushbuffer_ops host1x_pushbuffer_ops = {
+	.init = push_buffer_init,
+};
diff --git a/drivers/gpu/host1x/hw/channel_hw.c b/drivers/gpu/host1x/hw/channel_hw.c
new file mode 100644
index 000000000000..ee199623e365
--- /dev/null
+++ b/drivers/gpu/host1x/hw/channel_hw.c
@@ -0,0 +1,168 @@
+/*
+ * Tegra host1x Channel
+ *
+ * Copyright (c) 2010-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/slab.h>
+#include <trace/events/host1x.h>
+
+#include "host1x.h"
+#include "host1x_bo.h"
+#include "channel.h"
+#include "dev.h"
+#include "intr.h"
+#include "job.h"
+
+#define HOST1X_CHANNEL_SIZE 16384
+#define TRACE_MAX_LENGTH 128U
+
+static void trace_write_gather(struct host1x_cdma *cdma, struct host1x_bo *bo,
+			       u32 offset, u32 words)
+{
+	void *mem = NULL;
+
+	if (host1x_debug_trace_cmdbuf)
+		mem = host1x_bo_mmap(bo);
+
+	if (mem) {
+		u32 i;
+		/*
+		 * Write in batches of 128 as there seems to be a limit
+		 * of how much you can output to ftrace at once.
+		 */
+		for (i = 0; i < words; i += TRACE_MAX_LENGTH) {
+			trace_host1x_cdma_push_gather(
+				dev_name(cdma_to_channel(cdma)->dev),
+				(u32)bo, min(words - i, TRACE_MAX_LENGTH),
+				offset + i * sizeof(u32), mem);
+		}
+		host1x_bo_munmap(bo, mem);
+	}
+}
+
+static void submit_gathers(struct host1x_job *job)
+{
+	struct host1x_cdma *cdma = &job->channel->cdma;
+	unsigned int i;
+
+	for (i = 0; i < job->num_gathers; i++) {
+		struct host1x_job_gather *g = &job->gathers[i];
+		u32 op1 = host1x_opcode_gather(g->words);
+		u32 op2 = g->base + g->offset;
+		trace_write_gather(cdma, g->bo, g->offset, op1 & 0xffff);
+		host1x_cdma_push(cdma, op1, op2);
+	}
+}
+
+static int channel_submit(struct host1x_job *job)
+{
+	struct host1x_channel *ch = job->channel;
+	struct host1x_syncpt *sp;
+	u32 user_syncpt_incrs = job->syncpt_incrs;
+	u32 prev_max = 0;
+	u32 syncval;
+	int err;
+	struct host1x_waitlist *completed_waiter = NULL;
+	struct host1x *host = dev_get_drvdata(ch->dev->parent);
+
+	sp = host->syncpt + job->syncpt_id;
+	trace_host1x_channel_submit(dev_name(ch->dev),
+				    job->num_gathers, job->num_relocs,
+				    job->num_waitchk, job->syncpt_id,
+				    job->syncpt_incrs);
+
+	/* before error checks, return current max */
+	prev_max = job->syncpt_end = host1x_syncpt_read_max(sp);
+
+	/* get submit lock */
+	err = mutex_lock_interruptible(&ch->submitlock);
+	if (err)
+		goto error;
+
+	completed_waiter = kzalloc(sizeof(*completed_waiter), GFP_KERNEL);
+	if (!completed_waiter) {
+		mutex_unlock(&ch->submitlock);
+		err = -ENOMEM;
+		goto error;
+	}
+
+	/* begin a CDMA submit */
+	err = host1x_cdma_begin(&ch->cdma, job);
+	if (err) {
+		mutex_unlock(&ch->submitlock);
+		goto error;
+	}
+
+	if (job->serialize) {
+		/*
+		 * Force serialization by inserting a host wait for the
+		 * previous job to finish before this one can commence.
+		 */
+		host1x_cdma_push(&ch->cdma,
+				 host1x_opcode_setclass(HOST1X_CLASS_HOST1X,
+					host1x_uclass_wait_syncpt_r(), 1),
+				 host1x_class_host_wait_syncpt(job->syncpt_id,
+					host1x_syncpt_read_max(sp)));
+	}
+
+	syncval = host1x_syncpt_incr_max(sp, user_syncpt_incrs);
+
+	job->syncpt_end = syncval;
+
+	/* add a setclass for modules that require it */
+	if (job->class)
+		host1x_cdma_push(&ch->cdma,
+				 host1x_opcode_setclass(job->class, 0, 0),
+				 HOST1X_OPCODE_NOP);
+
+	submit_gathers(job);
+
+	/* end CDMA submit & stash pinned hMems into sync queue */
+	host1x_cdma_end(&ch->cdma, job);
+
+	trace_host1x_channel_submitted(dev_name(ch->dev), prev_max, syncval);
+
+	/* schedule a submit complete interrupt */
+	err = host1x_intr_add_action(host, job->syncpt_id, syncval,
+				     HOST1X_INTR_ACTION_SUBMIT_COMPLETE, ch,
+				     completed_waiter, NULL);
+	completed_waiter = NULL;
+	WARN(err, "Failed to set submit complete interrupt");
+
+	mutex_unlock(&ch->submitlock);
+
+	return 0;
+
+error:
+	kfree(completed_waiter);
+	return err;
+}
+
+static int host1x_channel_init(struct host1x_channel *ch, struct host1x *dev,
+			       unsigned int index)
+{
+	ch->id = index;
+	mutex_init(&ch->reflock);
+	mutex_init(&ch->submitlock);
+
+	ch->regs = dev->regs + index * HOST1X_CHANNEL_SIZE;
+	return 0;
+}
+
+static const struct host1x_channel_ops host1x_channel_ops = {
+	.init = host1x_channel_init,
+	.submit = channel_submit,
+};
diff --git a/drivers/gpu/host1x/hw/debug_hw.c b/drivers/gpu/host1x/hw/debug_hw.c
new file mode 100644
index 000000000000..334c038052f5
--- /dev/null
+++ b/drivers/gpu/host1x/hw/debug_hw.c
@@ -0,0 +1,322 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ *
+ * Copyright (C) 2011-2013 NVIDIA Corporation
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/mm.h>
+#include <linux/scatterlist.h>
+
+#include <linux/io.h>
+
+#include "dev.h"
+#include "debug.h"
+#include "cdma.h"
+#include "channel.h"
+#include "host1x_bo.h"
+
+#define HOST1X_DEBUG_MAX_PAGE_OFFSET 102400
+
+enum {
+	HOST1X_OPCODE_SETCLASS	= 0x00,
+	HOST1X_OPCODE_INCR	= 0x01,
+	HOST1X_OPCODE_NONINCR	= 0x02,
+	HOST1X_OPCODE_MASK	= 0x03,
+	HOST1X_OPCODE_IMM	= 0x04,
+	HOST1X_OPCODE_RESTART	= 0x05,
+	HOST1X_OPCODE_GATHER	= 0x06,
+	HOST1X_OPCODE_EXTEND	= 0x0e,
+};
+
+enum {
+	HOST1X_OPCODE_EXTEND_ACQUIRE_MLOCK	= 0x00,
+	HOST1X_OPCODE_EXTEND_RELEASE_MLOCK	= 0x01,
+};
+
+static unsigned int show_channel_command(struct output *o, u32 val)
+{
+	unsigned mask;
+	unsigned subop;
+
+	switch (val >> 28) {
+	case HOST1X_OPCODE_SETCLASS:
+		mask = val & 0x3f;
+		if (mask) {
+			host1x_debug_output(o, "SETCL(class=%03x, offset=%03x, mask=%02x, [",
+					    val >> 6 & 0x3ff,
+					    val >> 16 & 0xfff, mask);
+			return hweight8(mask);
+		} else {
+			host1x_debug_output(o, "SETCL(class=%03x)\n",
+					    val >> 6 & 0x3ff);
+			return 0;
+		}
+
+	case HOST1X_OPCODE_INCR:
+		host1x_debug_output(o, "INCR(offset=%03x, [",
+				    val >> 16 & 0xfff);
+		return val & 0xffff;
+
+	case HOST1X_OPCODE_NONINCR:
+		host1x_debug_output(o, "NONINCR(offset=%03x, [",
+				    val >> 16 & 0xfff);
+		return val & 0xffff;
+
+	case HOST1X_OPCODE_MASK:
+		mask = val & 0xffff;
+		host1x_debug_output(o, "MASK(offset=%03x, mask=%03x, [",
+				    val >> 16 & 0xfff, mask);
+		return hweight16(mask);
+
+	case HOST1X_OPCODE_IMM:
+		host1x_debug_output(o, "IMM(offset=%03x, data=%03x)\n",
+				    val >> 16 & 0xfff, val & 0xffff);
+		return 0;
+
+	case HOST1X_OPCODE_RESTART:
+		host1x_debug_output(o, "RESTART(offset=%08x)\n", val << 4);
+		return 0;
+
+	case HOST1X_OPCODE_GATHER:
+		host1x_debug_output(o, "GATHER(offset=%03x, insert=%d, type=%d, count=%04x, addr=[",
+				    val >> 16 & 0xfff, val >> 15 & 0x1,
+				    val >> 14 & 0x1, val & 0x3fff);
+		return 1;
+
+	case HOST1X_OPCODE_EXTEND:
+		subop = val >> 24 & 0xf;
+		if (subop == HOST1X_OPCODE_EXTEND_ACQUIRE_MLOCK)
+			host1x_debug_output(o, "ACQUIRE_MLOCK(index=%d)\n",
+					    val & 0xff);
+		else if (subop == HOST1X_OPCODE_EXTEND_RELEASE_MLOCK)
+			host1x_debug_output(o, "RELEASE_MLOCK(index=%d)\n",
+					    val & 0xff);
+		else
+			host1x_debug_output(o, "EXTEND_UNKNOWN(%08x)\n", val);
+		return 0;
+
+	default:
+		return 0;
+	}
+}
+
+static void show_gather(struct output *o, phys_addr_t phys_addr,
+			unsigned int words, struct host1x_cdma *cdma,
+			phys_addr_t pin_addr, u32 *map_addr)
+{
+	/* Map dmaget cursor to corresponding mem handle */
+	u32 offset = phys_addr - pin_addr;
+	unsigned int data_count = 0, i;
+
+	/*
+	 * Sometimes we're given different hardware address to the same
+	 * page - in these cases the offset will get an invalid number and
+	 * we just have to bail out.
+	 */
+	if (offset > HOST1X_DEBUG_MAX_PAGE_OFFSET) {
+		host1x_debug_output(o, "[address mismatch]\n");
+		return;
+	}
+
+	for (i = 0; i < words; i++) {
+		u32 addr = phys_addr + i * 4;
+		u32 val = *(map_addr + offset / 4 + i);
+
+		if (!data_count) {
+			host1x_debug_output(o, "%08x: %08x:", addr, val);
+			data_count = show_channel_command(o, val);
+		} else {
+			host1x_debug_output(o, "%08x%s", val,
+					    data_count > 0 ? ", " : "])\n");
+			data_count--;
+		}
+	}
+}
+
+static void show_channel_gathers(struct output *o, struct host1x_cdma *cdma)
+{
+	struct host1x_job *job;
+
+	list_for_each_entry(job, &cdma->sync_queue, list) {
+		int i;
+		host1x_debug_output(o, "\n%p: JOB, syncpt_id=%d, syncpt_val=%d, first_get=%08x, timeout=%d num_slots=%d, num_handles=%d\n",
+				    job, job->syncpt_id, job->syncpt_end,
+				    job->first_get, job->timeout,
+				    job->num_slots, job->num_unpins);
+
+		for (i = 0; i < job->num_gathers; i++) {
+			struct host1x_job_gather *g = &job->gathers[i];
+			u32 *mapped;
+
+			if (job->gather_copy_mapped)
+				mapped = (u32 *)job->gather_copy_mapped;
+			else
+				mapped = host1x_bo_mmap(g->bo);
+
+			if (!mapped) {
+				host1x_debug_output(o, "[could not mmap]\n");
+				continue;
+			}
+
+			host1x_debug_output(o, "    GATHER at %08x+%04x, %d words\n",
+					    g->base, g->offset, g->words);
+
+			show_gather(o, g->base + g->offset, g->words, cdma,
+				    g->base, mapped);
+
+			if (!job->gather_copy_mapped)
+				host1x_bo_munmap(g->bo, mapped);
+		}
+	}
+}
+
+static void host1x_debug_show_channel_cdma(struct host1x *host,
+					   struct host1x_channel *ch,
+					   struct output *o)
+{
+	struct host1x_cdma *cdma = &ch->cdma;
+	u32 dmaput, dmaget, dmactrl;
+	u32 cbstat, cbread;
+	u32 val, base, baseval;
+
+	dmaput = host1x_ch_readl(ch, HOST1X_CHANNEL_DMAPUT);
+	dmaget = host1x_ch_readl(ch, HOST1X_CHANNEL_DMAGET);
+	dmactrl = host1x_ch_readl(ch, HOST1X_CHANNEL_DMACTRL);
+	cbread = host1x_sync_readl(host, HOST1X_SYNC_CBREAD(ch->id));
+	cbstat = host1x_sync_readl(host, HOST1X_SYNC_CBSTAT(ch->id));
+
+	host1x_debug_output(o, "%d-%s: ", ch->id, dev_name(ch->dev));
+
+	if (HOST1X_CHANNEL_DMACTRL_DMASTOP_V(dmactrl) ||
+	    !ch->cdma.push_buffer.mapped) {
+		host1x_debug_output(o, "inactive\n\n");
+		return;
+	}
+
+	if (HOST1X_SYNC_CBSTAT_CBCLASS_V(cbstat) == HOST1X_CLASS_HOST1X &&
+	    HOST1X_SYNC_CBSTAT_CBOFFSET_V(cbstat) ==
+	    HOST1X_UCLASS_WAIT_SYNCPT)
+		host1x_debug_output(o, "waiting on syncpt %d val %d\n",
+				    cbread >> 24, cbread & 0xffffff);
+	else if (HOST1X_SYNC_CBSTAT_CBCLASS_V(cbstat) ==
+	   HOST1X_CLASS_HOST1X &&
+	   HOST1X_SYNC_CBSTAT_CBOFFSET_V(cbstat) ==
+	   HOST1X_UCLASS_WAIT_SYNCPT_BASE) {
+
+		base = (cbread >> 16) & 0xff;
+		baseval =
+			host1x_sync_readl(host, HOST1X_SYNC_SYNCPT_BASE(base));
+		val = cbread & 0xffff;
+		host1x_debug_output(o, "waiting on syncpt %d val %d (base %d = %d; offset = %d)\n",
+				    cbread >> 24, baseval + val, base,
+				    baseval, val);
+	} else
+		host1x_debug_output(o, "active class %02x, offset %04x, val %08x\n",
+				    HOST1X_SYNC_CBSTAT_CBCLASS_V(cbstat),
+				    HOST1X_SYNC_CBSTAT_CBOFFSET_V(cbstat),
+				    cbread);
+
+	host1x_debug_output(o, "DMAPUT %08x, DMAGET %08x, DMACTL %08x\n",
+			    dmaput, dmaget, dmactrl);
+	host1x_debug_output(o, "CBREAD %08x, CBSTAT %08x\n", cbread, cbstat);
+
+	show_channel_gathers(o, cdma);
+	host1x_debug_output(o, "\n");
+}
+
+static void host1x_debug_show_channel_fifo(struct host1x *host,
+					   struct host1x_channel *ch,
+					   struct output *o)
+{
+	u32 val, rd_ptr, wr_ptr, start, end;
+	unsigned int data_count = 0;
+
+	host1x_debug_output(o, "%d: fifo:\n", ch->id);
+
+	val = host1x_ch_readl(ch, HOST1X_CHANNEL_FIFOSTAT);
+	host1x_debug_output(o, "FIFOSTAT %08x\n", val);
+	if (HOST1X_CHANNEL_FIFOSTAT_CFEMPTY_V(val)) {
+		host1x_debug_output(o, "[empty]\n");
+		return;
+	}
+
+	host1x_sync_writel(host, 0x0, HOST1X_SYNC_CFPEEK_CTRL);
+	host1x_sync_writel(host, HOST1X_SYNC_CFPEEK_CTRL_ENA_F(1) |
+			   HOST1X_SYNC_CFPEEK_CTRL_CHANNR_F(ch->id),
+			   HOST1X_SYNC_CFPEEK_CTRL);
+
+	val = host1x_sync_readl(host, HOST1X_SYNC_CFPEEK_PTRS);
+	rd_ptr = HOST1X_SYNC_CFPEEK_PTRS_CF_RD_PTR_V(val);
+	wr_ptr = HOST1X_SYNC_CFPEEK_PTRS_CF_WR_PTR_V(val);
+
+	val = host1x_sync_readl(host, HOST1X_SYNC_CF_SETUP(ch->id));
+	start = HOST1X_SYNC_CF_SETUP_BASE_V(val);
+	end = HOST1X_SYNC_CF_SETUP_LIMIT_V(val);
+
+	do {
+		host1x_sync_writel(host, 0x0, HOST1X_SYNC_CFPEEK_CTRL);
+		host1x_sync_writel(host, HOST1X_SYNC_CFPEEK_CTRL_ENA_F(1) |
+				   HOST1X_SYNC_CFPEEK_CTRL_CHANNR_F(ch->id) |
+				   HOST1X_SYNC_CFPEEK_CTRL_ADDR_F(rd_ptr),
+				   HOST1X_SYNC_CFPEEK_CTRL);
+		val = host1x_sync_readl(host, HOST1X_SYNC_CFPEEK_READ);
+
+		if (!data_count) {
+			host1x_debug_output(o, "%08x:", val);
+			data_count = show_channel_command(o, val);
+		} else {
+			host1x_debug_output(o, "%08x%s", val,
+					    data_count > 0 ? ", " : "])\n");
+			data_count--;
+		}
+
+		if (rd_ptr == end)
+			rd_ptr = start;
+		else
+			rd_ptr++;
+	} while (rd_ptr != wr_ptr);
+
+	if (data_count)
+		host1x_debug_output(o, ", ...])\n");
+	host1x_debug_output(o, "\n");
+
+	host1x_sync_writel(host, 0x0, HOST1X_SYNC_CFPEEK_CTRL);
+}
+
+static void host1x_debug_show_mlocks(struct host1x *host, struct output *o)
+{
+	int i;
+
+	host1x_debug_output(o, "---- mlocks ----\n");
+	for (i = 0; i < host1x_syncpt_nb_mlocks(host); i++) {
+		u32 owner =
+			host1x_sync_readl(host, HOST1X_SYNC_MLOCK_OWNER(i));
+		if (HOST1X_SYNC_MLOCK_OWNER_CH_OWNS_V(owner))
+			host1x_debug_output(o, "%d: locked by channel %d\n",
+				i, HOST1X_SYNC_MLOCK_OWNER_CHID_F(owner));
+		else if (HOST1X_SYNC_MLOCK_OWNER_CPU_OWNS_V(owner))
+			host1x_debug_output(o, "%d: locked by cpu\n", i);
+		else
+			host1x_debug_output(o, "%d: unlocked\n", i);
+	}
+	host1x_debug_output(o, "\n");
+}
+
+static const struct host1x_debug_ops host1x_debug_ops = {
+	.show_channel_cdma = host1x_debug_show_channel_cdma,
+	.show_channel_fifo = host1x_debug_show_channel_fifo,
+	.show_mlocks = host1x_debug_show_mlocks,
+};
diff --git a/drivers/gpu/host1x/hw/host1x01.c b/drivers/gpu/host1x/hw/host1x01.c
new file mode 100644
index 000000000000..a14e91cd1e58
--- /dev/null
+++ b/drivers/gpu/host1x/hw/host1x01.c
@@ -0,0 +1,42 @@
+/*
+ * Host1x init for T20 and T30 Architecture Chips
+ *
+ * Copyright (c) 2011-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 hw specification */
+#include "hw/host1x01.h"
+#include "hw/host1x01_hardware.h"
+
+/* include code */
+#include "hw/cdma_hw.c"
+#include "hw/channel_hw.c"
+#include "hw/debug_hw.c"
+#include "hw/intr_hw.c"
+#include "hw/syncpt_hw.c"
+
+#include "dev.h"
+
+int host1x01_init(struct host1x *host)
+{
+	host->channel_op = &host1x_channel_ops;
+	host->cdma_op = &host1x_cdma_ops;
+	host->cdma_pb_op = &host1x_pushbuffer_ops;
+	host->syncpt_op = &host1x_syncpt_ops;
+	host->intr_op = &host1x_intr_ops;
+	host->debug_op = &host1x_debug_ops;
+
+	return 0;
+}
diff --git a/drivers/gpu/host1x/hw/host1x01.h b/drivers/gpu/host1x/hw/host1x01.h
new file mode 100644
index 000000000000..2706b6743250
--- /dev/null
+++ b/drivers/gpu/host1x/hw/host1x01.h
@@ -0,0 +1,25 @@
+/*
+ * Host1x init for T20 and T30 Architecture Chips
+ *
+ * Copyright (c) 2011-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef HOST1X_HOST1X01_H
+#define HOST1X_HOST1X01_H
+
+struct host1x;
+
+int host1x01_init(struct host1x *host);
+
+#endif /* HOST1X_HOST1X01_H_ */
diff --git a/drivers/gpu/host1x/hw/host1x01_hardware.h b/drivers/gpu/host1x/hw/host1x01_hardware.h
new file mode 100644
index 000000000000..5f0fb866efa8
--- /dev/null
+++ b/drivers/gpu/host1x/hw/host1x01_hardware.h
@@ -0,0 +1,143 @@
+/*
+ * Tegra host1x Register Offsets for Tegra20 and Tegra30
+ *
+ * Copyright (c) 2010-2013 NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __HOST1X_HOST1X01_HARDWARE_H
+#define __HOST1X_HOST1X01_HARDWARE_H
+
+#include <linux/types.h>
+#include <linux/bitops.h>
+
+#include "hw_host1x01_channel.h"
+#include "hw_host1x01_sync.h"
+#include "hw_host1x01_uclass.h"
+
+static inline u32 host1x_class_host_wait_syncpt(
+	unsigned indx, unsigned threshold)
+{
+	return host1x_uclass_wait_syncpt_indx_f(indx)
+		| host1x_uclass_wait_syncpt_thresh_f(threshold);
+}
+
+static inline u32 host1x_class_host_load_syncpt_base(
+	unsigned indx, unsigned threshold)
+{
+	return host1x_uclass_load_syncpt_base_base_indx_f(indx)
+		| host1x_uclass_load_syncpt_base_value_f(threshold);
+}
+
+static inline u32 host1x_class_host_wait_syncpt_base(
+	unsigned indx, unsigned base_indx, unsigned offset)
+{
+	return host1x_uclass_wait_syncpt_base_indx_f(indx)
+		| host1x_uclass_wait_syncpt_base_base_indx_f(base_indx)
+		| host1x_uclass_wait_syncpt_base_offset_f(offset);
+}
+
+static inline u32 host1x_class_host_incr_syncpt_base(
+	unsigned base_indx, unsigned offset)
+{
+	return host1x_uclass_incr_syncpt_base_base_indx_f(base_indx)
+		| host1x_uclass_incr_syncpt_base_offset_f(offset);
+}
+
+static inline u32 host1x_class_host_incr_syncpt(
+	unsigned cond, unsigned indx)
+{
+	return host1x_uclass_incr_syncpt_cond_f(cond)
+		| host1x_uclass_incr_syncpt_indx_f(indx);
+}
+
+static inline u32 host1x_class_host_indoff_reg_write(
+	unsigned mod_id, unsigned offset, bool auto_inc)
+{
+	u32 v = host1x_uclass_indoff_indbe_f(0xf)
+		| host1x_uclass_indoff_indmodid_f(mod_id)
+		| host1x_uclass_indoff_indroffset_f(offset);
+	if (auto_inc)
+		v |= host1x_uclass_indoff_autoinc_f(1);
+	return v;
+}
+
+static inline u32 host1x_class_host_indoff_reg_read(
+	unsigned mod_id, unsigned offset, bool auto_inc)
+{
+	u32 v = host1x_uclass_indoff_indmodid_f(mod_id)
+		| host1x_uclass_indoff_indroffset_f(offset)
+		| host1x_uclass_indoff_rwn_read_v();
+	if (auto_inc)
+		v |= host1x_uclass_indoff_autoinc_f(1);
+	return v;
+}
+
+
+/* cdma opcodes */
+static inline u32 host1x_opcode_setclass(
+	unsigned class_id, unsigned offset, unsigned mask)
+{
+	return (0 << 28) | (offset << 16) | (class_id << 6) | mask;
+}
+
+static inline u32 host1x_opcode_incr(unsigned offset, unsigned count)
+{
+	return (1 << 28) | (offset << 16) | count;
+}
+
+static inline u32 host1x_opcode_nonincr(unsigned offset, unsigned count)
+{
+	return (2 << 28) | (offset << 16) | count;
+}
+
+static inline u32 host1x_opcode_mask(unsigned offset, unsigned mask)
+{
+	return (3 << 28) | (offset << 16) | mask;
+}
+
+static inline u32 host1x_opcode_imm(unsigned offset, unsigned value)
+{
+	return (4 << 28) | (offset << 16) | value;
+}
+
+static inline u32 host1x_opcode_imm_incr_syncpt(unsigned cond, unsigned indx)
+{
+	return host1x_opcode_imm(host1x_uclass_incr_syncpt_r(),
+		host1x_class_host_incr_syncpt(cond, indx));
+}
+
+static inline u32 host1x_opcode_restart(unsigned address)
+{
+	return (5 << 28) | (address >> 4);
+}
+
+static inline u32 host1x_opcode_gather(unsigned count)
+{
+	return (6 << 28) | count;
+}
+
+static inline u32 host1x_opcode_gather_nonincr(unsigned offset,	unsigned count)
+{
+	return (6 << 28) | (offset << 16) | BIT(15) | count;
+}
+
+static inline u32 host1x_opcode_gather_incr(unsigned offset, unsigned count)
+{
+	return (6 << 28) | (offset << 16) | BIT(15) | BIT(14) | count;
+}
+
+#define HOST1X_OPCODE_NOP host1x_opcode_nonincr(0, 0)
+
+#endif
diff --git a/drivers/gpu/host1x/hw/hw_host1x01_channel.h b/drivers/gpu/host1x/hw/hw_host1x01_channel.h
new file mode 100644
index 000000000000..b4bc7ca4e051
--- /dev/null
+++ b/drivers/gpu/host1x/hw/hw_host1x01_channel.h
@@ -0,0 +1,120 @@
+/*
+ * Copyright (c) 2012-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/>.
+ *
+ */
+
+ /*
+  * Function naming determines intended use:
+  *
+  *     <x>_r(void) : Returns the offset for register <x>.
+  *
+  *     <x>_w(void) : Returns the word offset for word (4 byte) element <x>.
+  *
+  *     <x>_<y>_s(void) : Returns size of field <y> of register <x> in bits.
+  *
+  *     <x>_<y>_f(u32 v) : Returns a value based on 'v' which has been shifted
+  *         and masked to place it at field <y> of register <x>.  This value
+  *         can be |'d with others to produce a full register value for
+  *         register <x>.
+  *
+  *     <x>_<y>_m(void) : Returns a mask for field <y> of register <x>.  This
+  *         value can be ~'d and then &'d to clear the value of field <y> for
+  *         register <x>.
+  *
+  *     <x>_<y>_<z>_f(void) : Returns the constant value <z> after being shifted
+  *         to place it at field <y> of register <x>.  This value can be |'d
+  *         with others to produce a full register value for <x>.
+  *
+  *     <x>_<y>_v(u32 r) : Returns the value of field <y> from a full register
+  *         <x> value 'r' after being shifted to place its LSB at bit 0.
+  *         This value is suitable for direct comparison with other unshifted
+  *         values appropriate for use in field <y> of register <x>.
+  *
+  *     <x>_<y>_<z>_v(void) : Returns the constant value for <z> defined for
+  *         field <y> of register <x>.  This value is suitable for direct
+  *         comparison with unshifted values appropriate for use in field <y>
+  *         of register <x>.
+  */
+
+#ifndef __hw_host1x_channel_host1x_h__
+#define __hw_host1x_channel_host1x_h__
+
+static inline u32 host1x_channel_fifostat_r(void)
+{
+	return 0x0;
+}
+#define HOST1X_CHANNEL_FIFOSTAT \
+	host1x_channel_fifostat_r()
+static inline u32 host1x_channel_fifostat_cfempty_v(u32 r)
+{
+	return (r >> 10) & 0x1;
+}
+#define HOST1X_CHANNEL_FIFOSTAT_CFEMPTY_V(r) \
+	host1x_channel_fifostat_cfempty_v(r)
+static inline u32 host1x_channel_dmastart_r(void)
+{
+	return 0x14;
+}
+#define HOST1X_CHANNEL_DMASTART \
+	host1x_channel_dmastart_r()
+static inline u32 host1x_channel_dmaput_r(void)
+{
+	return 0x18;
+}
+#define HOST1X_CHANNEL_DMAPUT \
+	host1x_channel_dmaput_r()
+static inline u32 host1x_channel_dmaget_r(void)
+{
+	return 0x1c;
+}
+#define HOST1X_CHANNEL_DMAGET \
+	host1x_channel_dmaget_r()
+static inline u32 host1x_channel_dmaend_r(void)
+{
+	return 0x20;
+}
+#define HOST1X_CHANNEL_DMAEND \
+	host1x_channel_dmaend_r()
+static inline u32 host1x_channel_dmactrl_r(void)
+{
+	return 0x24;
+}
+#define HOST1X_CHANNEL_DMACTRL \
+	host1x_channel_dmactrl_r()
+static inline u32 host1x_channel_dmactrl_dmastop(void)
+{
+	return 1 << 0;
+}
+#define HOST1X_CHANNEL_DMACTRL_DMASTOP \
+	host1x_channel_dmactrl_dmastop()
+static inline u32 host1x_channel_dmactrl_dmastop_v(u32 r)
+{
+	return (r >> 0) & 0x1;
+}
+#define HOST1X_CHANNEL_DMACTRL_DMASTOP_V(r) \
+	host1x_channel_dmactrl_dmastop_v(r)
+static inline u32 host1x_channel_dmactrl_dmagetrst(void)
+{
+	return 1 << 1;
+}
+#define HOST1X_CHANNEL_DMACTRL_DMAGETRST \
+	host1x_channel_dmactrl_dmagetrst()
+static inline u32 host1x_channel_dmactrl_dmainitget(void)
+{
+	return 1 << 2;
+}
+#define HOST1X_CHANNEL_DMACTRL_DMAINITGET \
+	host1x_channel_dmactrl_dmainitget()
+#endif
diff --git a/drivers/gpu/host1x/hw/hw_host1x01_sync.h b/drivers/gpu/host1x/hw/hw_host1x01_sync.h
new file mode 100644
index 000000000000..ac704e579977
--- /dev/null
+++ b/drivers/gpu/host1x/hw/hw_host1x01_sync.h
@@ -0,0 +1,243 @@
+/*
+ * Copyright (c) 2012-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/>.
+ *
+ */
+
+ /*
+  * Function naming determines intended use:
+  *
+  *     <x>_r(void) : Returns the offset for register <x>.
+  *
+  *     <x>_w(void) : Returns the word offset for word (4 byte) element <x>.
+  *
+  *     <x>_<y>_s(void) : Returns size of field <y> of register <x> in bits.
+  *
+  *     <x>_<y>_f(u32 v) : Returns a value based on 'v' which has been shifted
+  *         and masked to place it at field <y> of register <x>.  This value
+  *         can be |'d with others to produce a full register value for
+  *         register <x>.
+  *
+  *     <x>_<y>_m(void) : Returns a mask for field <y> of register <x>.  This
+  *         value can be ~'d and then &'d to clear the value of field <y> for
+  *         register <x>.
+  *
+  *     <x>_<y>_<z>_f(void) : Returns the constant value <z> after being shifted
+  *         to place it at field <y> of register <x>.  This value can be |'d
+  *         with others to produce a full register value for <x>.
+  *
+  *     <x>_<y>_v(u32 r) : Returns the value of field <y> from a full register
+  *         <x> value 'r' after being shifted to place its LSB at bit 0.
+  *         This value is suitable for direct comparison with other unshifted
+  *         values appropriate for use in field <y> of register <x>.
+  *
+  *     <x>_<y>_<z>_v(void) : Returns the constant value for <z> defined for
+  *         field <y> of register <x>.  This value is suitable for direct
+  *         comparison with unshifted values appropriate for use in field <y>
+  *         of register <x>.
+  */
+
+#ifndef __hw_host1x01_sync_h__
+#define __hw_host1x01_sync_h__
+
+#define REGISTER_STRIDE	4
+
+static inline u32 host1x_sync_syncpt_r(unsigned int id)
+{
+	return 0x400 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_SYNCPT(id) \
+	host1x_sync_syncpt_r(id)
+static inline u32 host1x_sync_syncpt_thresh_cpu0_int_status_r(unsigned int id)
+{
+	return 0x40 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_SYNCPT_THRESH_CPU0_INT_STATUS(id) \
+	host1x_sync_syncpt_thresh_cpu0_int_status_r(id)
+static inline u32 host1x_sync_syncpt_thresh_int_disable_r(unsigned int id)
+{
+	return 0x60 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_SYNCPT_THRESH_INT_DISABLE(id) \
+	host1x_sync_syncpt_thresh_int_disable_r(id)
+static inline u32 host1x_sync_syncpt_thresh_int_enable_cpu0_r(unsigned int id)
+{
+	return 0x68 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_SYNCPT_THRESH_INT_ENABLE_CPU0(id) \
+	host1x_sync_syncpt_thresh_int_enable_cpu0_r(id)
+static inline u32 host1x_sync_cf_setup_r(unsigned int channel)
+{
+	return 0x80 + channel * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_CF_SETUP(channel) \
+	host1x_sync_cf_setup_r(channel)
+static inline u32 host1x_sync_cf_setup_base_v(u32 r)
+{
+	return (r >> 0) & 0x1ff;
+}
+#define HOST1X_SYNC_CF_SETUP_BASE_V(r) \
+	host1x_sync_cf_setup_base_v(r)
+static inline u32 host1x_sync_cf_setup_limit_v(u32 r)
+{
+	return (r >> 16) & 0x1ff;
+}
+#define HOST1X_SYNC_CF_SETUP_LIMIT_V(r) \
+	host1x_sync_cf_setup_limit_v(r)
+static inline u32 host1x_sync_cmdproc_stop_r(void)
+{
+	return 0xac;
+}
+#define HOST1X_SYNC_CMDPROC_STOP \
+	host1x_sync_cmdproc_stop_r()
+static inline u32 host1x_sync_ch_teardown_r(void)
+{
+	return 0xb0;
+}
+#define HOST1X_SYNC_CH_TEARDOWN \
+	host1x_sync_ch_teardown_r()
+static inline u32 host1x_sync_usec_clk_r(void)
+{
+	return 0x1a4;
+}
+#define HOST1X_SYNC_USEC_CLK \
+	host1x_sync_usec_clk_r()
+static inline u32 host1x_sync_ctxsw_timeout_cfg_r(void)
+{
+	return 0x1a8;
+}
+#define HOST1X_SYNC_CTXSW_TIMEOUT_CFG \
+	host1x_sync_ctxsw_timeout_cfg_r()
+static inline u32 host1x_sync_ip_busy_timeout_r(void)
+{
+	return 0x1bc;
+}
+#define HOST1X_SYNC_IP_BUSY_TIMEOUT \
+	host1x_sync_ip_busy_timeout_r()
+static inline u32 host1x_sync_mlock_owner_r(unsigned int id)
+{
+	return 0x340 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_MLOCK_OWNER(id) \
+	host1x_sync_mlock_owner_r(id)
+static inline u32 host1x_sync_mlock_owner_chid_f(u32 v)
+{
+	return (v & 0xf) << 8;
+}
+#define HOST1X_SYNC_MLOCK_OWNER_CHID_F(v) \
+	host1x_sync_mlock_owner_chid_f(v)
+static inline u32 host1x_sync_mlock_owner_cpu_owns_v(u32 r)
+{
+	return (r >> 1) & 0x1;
+}
+#define HOST1X_SYNC_MLOCK_OWNER_CPU_OWNS_V(r) \
+	host1x_sync_mlock_owner_cpu_owns_v(r)
+static inline u32 host1x_sync_mlock_owner_ch_owns_v(u32 r)
+{
+	return (r >> 0) & 0x1;
+}
+#define HOST1X_SYNC_MLOCK_OWNER_CH_OWNS_V(r) \
+	host1x_sync_mlock_owner_ch_owns_v(r)
+static inline u32 host1x_sync_syncpt_int_thresh_r(unsigned int id)
+{
+	return 0x500 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_SYNCPT_INT_THRESH(id) \
+	host1x_sync_syncpt_int_thresh_r(id)
+static inline u32 host1x_sync_syncpt_base_r(unsigned int id)
+{
+	return 0x600 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_SYNCPT_BASE(id) \
+	host1x_sync_syncpt_base_r(id)
+static inline u32 host1x_sync_syncpt_cpu_incr_r(unsigned int id)
+{
+	return 0x700 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_SYNCPT_CPU_INCR(id) \
+	host1x_sync_syncpt_cpu_incr_r(id)
+static inline u32 host1x_sync_cbread_r(unsigned int channel)
+{
+	return 0x720 + channel * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_CBREAD(channel) \
+	host1x_sync_cbread_r(channel)
+static inline u32 host1x_sync_cfpeek_ctrl_r(void)
+{
+	return 0x74c;
+}
+#define HOST1X_SYNC_CFPEEK_CTRL \
+	host1x_sync_cfpeek_ctrl_r()
+static inline u32 host1x_sync_cfpeek_ctrl_addr_f(u32 v)
+{
+	return (v & 0x1ff) << 0;
+}
+#define HOST1X_SYNC_CFPEEK_CTRL_ADDR_F(v) \
+	host1x_sync_cfpeek_ctrl_addr_f(v)
+static inline u32 host1x_sync_cfpeek_ctrl_channr_f(u32 v)
+{
+	return (v & 0x7) << 16;
+}
+#define HOST1X_SYNC_CFPEEK_CTRL_CHANNR_F(v) \
+	host1x_sync_cfpeek_ctrl_channr_f(v)
+static inline u32 host1x_sync_cfpeek_ctrl_ena_f(u32 v)
+{
+	return (v & 0x1) << 31;
+}
+#define HOST1X_SYNC_CFPEEK_CTRL_ENA_F(v) \
+	host1x_sync_cfpeek_ctrl_ena_f(v)
+static inline u32 host1x_sync_cfpeek_read_r(void)
+{
+	return 0x750;
+}
+#define HOST1X_SYNC_CFPEEK_READ \
+	host1x_sync_cfpeek_read_r()
+static inline u32 host1x_sync_cfpeek_ptrs_r(void)
+{
+	return 0x754;
+}
+#define HOST1X_SYNC_CFPEEK_PTRS \
+	host1x_sync_cfpeek_ptrs_r()
+static inline u32 host1x_sync_cfpeek_ptrs_cf_rd_ptr_v(u32 r)
+{
+	return (r >> 0) & 0x1ff;
+}
+#define HOST1X_SYNC_CFPEEK_PTRS_CF_RD_PTR_V(r) \
+	host1x_sync_cfpeek_ptrs_cf_rd_ptr_v(r)
+static inline u32 host1x_sync_cfpeek_ptrs_cf_wr_ptr_v(u32 r)
+{
+	return (r >> 16) & 0x1ff;
+}
+#define HOST1X_SYNC_CFPEEK_PTRS_CF_WR_PTR_V(r) \
+	host1x_sync_cfpeek_ptrs_cf_wr_ptr_v(r)
+static inline u32 host1x_sync_cbstat_r(unsigned int channel)
+{
+	return 0x758 + channel * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_CBSTAT(channel) \
+	host1x_sync_cbstat_r(channel)
+static inline u32 host1x_sync_cbstat_cboffset_v(u32 r)
+{
+	return (r >> 0) & 0xffff;
+}
+#define HOST1X_SYNC_CBSTAT_CBOFFSET_V(r) \
+	host1x_sync_cbstat_cboffset_v(r)
+static inline u32 host1x_sync_cbstat_cbclass_v(u32 r)
+{
+	return (r >> 16) & 0x3ff;
+}
+#define HOST1X_SYNC_CBSTAT_CBCLASS_V(r) \
+	host1x_sync_cbstat_cbclass_v(r)
+
+#endif /* __hw_host1x01_sync_h__ */
diff --git a/drivers/gpu/host1x/hw/hw_host1x01_uclass.h b/drivers/gpu/host1x/hw/hw_host1x01_uclass.h
new file mode 100644
index 000000000000..42f3ce19ca32
--- /dev/null
+++ b/drivers/gpu/host1x/hw/hw_host1x01_uclass.h
@@ -0,0 +1,174 @@
+/*
+ * Copyright (c) 2012-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/>.
+ *
+ */
+
+ /*
+  * Function naming determines intended use:
+  *
+  *     <x>_r(void) : Returns the offset for register <x>.
+  *
+  *     <x>_w(void) : Returns the word offset for word (4 byte) element <x>.
+  *
+  *     <x>_<y>_s(void) : Returns size of field <y> of register <x> in bits.
+  *
+  *     <x>_<y>_f(u32 v) : Returns a value based on 'v' which has been shifted
+  *         and masked to place it at field <y> of register <x>.  This value
+  *         can be |'d with others to produce a full register value for
+  *         register <x>.
+  *
+  *     <x>_<y>_m(void) : Returns a mask for field <y> of register <x>.  This
+  *         value can be ~'d and then &'d to clear the value of field <y> for
+  *         register <x>.
+  *
+  *     <x>_<y>_<z>_f(void) : Returns the constant value <z> after being shifted
+  *         to place it at field <y> of register <x>.  This value can be |'d
+  *         with others to produce a full register value for <x>.
+  *
+  *     <x>_<y>_v(u32 r) : Returns the value of field <y> from a full register
+  *         <x> value 'r' after being shifted to place its LSB at bit 0.
+  *         This value is suitable for direct comparison with other unshifted
+  *         values appropriate for use in field <y> of register <x>.
+  *
+  *     <x>_<y>_<z>_v(void) : Returns the constant value for <z> defined for
+  *         field <y> of register <x>.  This value is suitable for direct
+  *         comparison with unshifted values appropriate for use in field <y>
+  *         of register <x>.
+  */
+
+#ifndef __hw_host1x_uclass_host1x_h__
+#define __hw_host1x_uclass_host1x_h__
+
+static inline u32 host1x_uclass_incr_syncpt_r(void)
+{
+	return 0x0;
+}
+#define HOST1X_UCLASS_INCR_SYNCPT \
+	host1x_uclass_incr_syncpt_r()
+static inline u32 host1x_uclass_incr_syncpt_cond_f(u32 v)
+{
+	return (v & 0xff) << 8;
+}
+#define HOST1X_UCLASS_INCR_SYNCPT_COND_F(v) \
+	host1x_uclass_incr_syncpt_cond_f(v)
+static inline u32 host1x_uclass_incr_syncpt_indx_f(u32 v)
+{
+	return (v & 0xff) << 0;
+}
+#define HOST1X_UCLASS_INCR_SYNCPT_INDX_F(v) \
+	host1x_uclass_incr_syncpt_indx_f(v)
+static inline u32 host1x_uclass_wait_syncpt_r(void)
+{
+	return 0x8;
+}
+#define HOST1X_UCLASS_WAIT_SYNCPT \
+	host1x_uclass_wait_syncpt_r()
+static inline u32 host1x_uclass_wait_syncpt_indx_f(u32 v)
+{
+	return (v & 0xff) << 24;
+}
+#define HOST1X_UCLASS_WAIT_SYNCPT_INDX_F(v) \
+	host1x_uclass_wait_syncpt_indx_f(v)
+static inline u32 host1x_uclass_wait_syncpt_thresh_f(u32 v)
+{
+	return (v & 0xffffff) << 0;
+}
+#define HOST1X_UCLASS_WAIT_SYNCPT_THRESH_F(v) \
+	host1x_uclass_wait_syncpt_thresh_f(v)
+static inline u32 host1x_uclass_wait_syncpt_base_r(void)
+{
+	return 0x9;
+}
+#define HOST1X_UCLASS_WAIT_SYNCPT_BASE \
+	host1x_uclass_wait_syncpt_base_r()
+static inline u32 host1x_uclass_wait_syncpt_base_indx_f(u32 v)
+{
+	return (v & 0xff) << 24;
+}
+#define HOST1X_UCLASS_WAIT_SYNCPT_BASE_INDX_F(v) \
+	host1x_uclass_wait_syncpt_base_indx_f(v)
+static inline u32 host1x_uclass_wait_syncpt_base_base_indx_f(u32 v)
+{
+	return (v & 0xff) << 16;
+}
+#define HOST1X_UCLASS_WAIT_SYNCPT_BASE_BASE_INDX_F(v) \
+	host1x_uclass_wait_syncpt_base_base_indx_f(v)
+static inline u32 host1x_uclass_wait_syncpt_base_offset_f(u32 v)
+{
+	return (v & 0xffff) << 0;
+}
+#define HOST1X_UCLASS_WAIT_SYNCPT_BASE_OFFSET_F(v) \
+	host1x_uclass_wait_syncpt_base_offset_f(v)
+static inline u32 host1x_uclass_load_syncpt_base_base_indx_f(u32 v)
+{
+	return (v & 0xff) << 24;
+}
+#define HOST1X_UCLASS_LOAD_SYNCPT_BASE_BASE_INDX_F(v) \
+	host1x_uclass_load_syncpt_base_base_indx_f(v)
+static inline u32 host1x_uclass_load_syncpt_base_value_f(u32 v)
+{
+	return (v & 0xffffff) << 0;
+}
+#define HOST1X_UCLASS_LOAD_SYNCPT_BASE_VALUE_F(v) \
+	host1x_uclass_load_syncpt_base_value_f(v)
+static inline u32 host1x_uclass_incr_syncpt_base_base_indx_f(u32 v)
+{
+	return (v & 0xff) << 24;
+}
+#define HOST1X_UCLASS_INCR_SYNCPT_BASE_BASE_INDX_F(v) \
+	host1x_uclass_incr_syncpt_base_base_indx_f(v)
+static inline u32 host1x_uclass_incr_syncpt_base_offset_f(u32 v)
+{
+	return (v & 0xffffff) << 0;
+}
+#define HOST1X_UCLASS_INCR_SYNCPT_BASE_OFFSET_F(v) \
+	host1x_uclass_incr_syncpt_base_offset_f(v)
+static inline u32 host1x_uclass_indoff_r(void)
+{
+	return 0x2d;
+}
+#define HOST1X_UCLASS_INDOFF \
+	host1x_uclass_indoff_r()
+static inline u32 host1x_uclass_indoff_indbe_f(u32 v)
+{
+	return (v & 0xf) << 28;
+}
+#define HOST1X_UCLASS_INDOFF_INDBE_F(v) \
+	host1x_uclass_indoff_indbe_f(v)
+static inline u32 host1x_uclass_indoff_autoinc_f(u32 v)
+{
+	return (v & 0x1) << 27;
+}
+#define HOST1X_UCLASS_INDOFF_AUTOINC_F(v) \
+	host1x_uclass_indoff_autoinc_f(v)
+static inline u32 host1x_uclass_indoff_indmodid_f(u32 v)
+{
+	return (v & 0xff) << 18;
+}
+#define HOST1X_UCLASS_INDOFF_INDMODID_F(v) \
+	host1x_uclass_indoff_indmodid_f(v)
+static inline u32 host1x_uclass_indoff_indroffset_f(u32 v)
+{
+	return (v & 0xffff) << 2;
+}
+#define HOST1X_UCLASS_INDOFF_INDROFFSET_F(v) \
+	host1x_uclass_indoff_indroffset_f(v)
+static inline u32 host1x_uclass_indoff_rwn_read_v(void)
+{
+	return 1;
+}
+#define HOST1X_UCLASS_INDOFF_INDROFFSET_F(v) \
+	host1x_uclass_indoff_indroffset_f(v)
+#endif
diff --git a/drivers/gpu/host1x/hw/intr_hw.c b/drivers/gpu/host1x/hw/intr_hw.c
new file mode 100644
index 000000000000..b592eef1efcb
--- /dev/null
+++ b/drivers/gpu/host1x/hw/intr_hw.c
@@ -0,0 +1,143 @@
+/*
+ * Tegra host1x Interrupt Management
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (c) 2010-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/interrupt.h>
+#include <linux/irq.h>
+#include <linux/io.h>
+#include <asm/mach/irq.h>
+
+#include "intr.h"
+#include "dev.h"
+
+/*
+ * Sync point threshold interrupt service function
+ * Handles sync point threshold triggers, in interrupt context
+ */
+static void host1x_intr_syncpt_handle(struct host1x_syncpt *syncpt)
+{
+	unsigned int id = syncpt->id;
+	struct host1x *host = syncpt->host;
+
+	host1x_sync_writel(host, BIT_MASK(id),
+		HOST1X_SYNC_SYNCPT_THRESH_INT_DISABLE(BIT_WORD(id)));
+	host1x_sync_writel(host, BIT_MASK(id),
+		HOST1X_SYNC_SYNCPT_THRESH_CPU0_INT_STATUS(BIT_WORD(id)));
+
+	queue_work(host->intr_wq, &syncpt->intr.work);
+}
+
+static irqreturn_t syncpt_thresh_isr(int irq, void *dev_id)
+{
+	struct host1x *host = dev_id;
+	unsigned long reg;
+	int i, id;
+
+	for (i = 0; i <= BIT_WORD(host->info->nb_pts); i++) {
+		reg = host1x_sync_readl(host,
+			HOST1X_SYNC_SYNCPT_THRESH_CPU0_INT_STATUS(i));
+		for_each_set_bit(id, &reg, BITS_PER_LONG) {
+			struct host1x_syncpt *syncpt =
+				host->syncpt + (i * BITS_PER_LONG + id);
+			host1x_intr_syncpt_handle(syncpt);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void _host1x_intr_disable_all_syncpt_intrs(struct host1x *host)
+{
+	u32 i;
+
+	for (i = 0; i <= BIT_WORD(host->info->nb_pts); ++i) {
+		host1x_sync_writel(host, 0xffffffffu,
+			HOST1X_SYNC_SYNCPT_THRESH_INT_DISABLE(i));
+		host1x_sync_writel(host, 0xffffffffu,
+			HOST1X_SYNC_SYNCPT_THRESH_CPU0_INT_STATUS(i));
+	}
+}
+
+static int _host1x_intr_init_host_sync(struct host1x *host, u32 cpm,
+	void (*syncpt_thresh_work)(struct work_struct *))
+{
+	int i, err;
+
+	host1x_hw_intr_disable_all_syncpt_intrs(host);
+
+	for (i = 0; i < host->info->nb_pts; i++)
+		INIT_WORK(&host->syncpt[i].intr.work, syncpt_thresh_work);
+
+	err = devm_request_irq(host->dev, host->intr_syncpt_irq,
+			       syncpt_thresh_isr, IRQF_SHARED,
+			       "host1x_syncpt", host);
+	if (IS_ERR_VALUE(err)) {
+		WARN_ON(1);
+		return err;
+	}
+
+	/* disable the ip_busy_timeout. this prevents write drops */
+	host1x_sync_writel(host, 0, HOST1X_SYNC_IP_BUSY_TIMEOUT);
+
+	/*
+	 * increase the auto-ack timout to the maximum value. 2d will hang
+	 * otherwise on Tegra2.
+	 */
+	host1x_sync_writel(host, 0xff, HOST1X_SYNC_CTXSW_TIMEOUT_CFG);
+
+	/* update host clocks per usec */
+	host1x_sync_writel(host, cpm, HOST1X_SYNC_USEC_CLK);
+
+	return 0;
+}
+
+static void _host1x_intr_set_syncpt_threshold(struct host1x *host,
+	u32 id, u32 thresh)
+{
+	host1x_sync_writel(host, thresh, HOST1X_SYNC_SYNCPT_INT_THRESH(id));
+}
+
+static void _host1x_intr_enable_syncpt_intr(struct host1x *host, u32 id)
+{
+	host1x_sync_writel(host, BIT_MASK(id),
+		HOST1X_SYNC_SYNCPT_THRESH_INT_ENABLE_CPU0(BIT_WORD(id)));
+}
+
+static void _host1x_intr_disable_syncpt_intr(struct host1x *host, u32 id)
+{
+	host1x_sync_writel(host, BIT_MASK(id),
+		HOST1X_SYNC_SYNCPT_THRESH_INT_DISABLE(BIT_WORD(id)));
+	host1x_sync_writel(host, BIT_MASK(id),
+		HOST1X_SYNC_SYNCPT_THRESH_CPU0_INT_STATUS(BIT_WORD(id)));
+}
+
+static int _host1x_free_syncpt_irq(struct host1x *host)
+{
+	devm_free_irq(host->dev, host->intr_syncpt_irq, host);
+	flush_workqueue(host->intr_wq);
+	return 0;
+}
+
+static const struct host1x_intr_ops host1x_intr_ops = {
+	.init_host_sync = _host1x_intr_init_host_sync,
+	.set_syncpt_threshold = _host1x_intr_set_syncpt_threshold,
+	.enable_syncpt_intr = _host1x_intr_enable_syncpt_intr,
+	.disable_syncpt_intr = _host1x_intr_disable_syncpt_intr,
+	.disable_all_syncpt_intrs = _host1x_intr_disable_all_syncpt_intrs,
+	.free_syncpt_irq = _host1x_free_syncpt_irq,
+};
diff --git a/drivers/gpu/host1x/hw/syncpt_hw.c b/drivers/gpu/host1x/hw/syncpt_hw.c
new file mode 100644
index 000000000000..61174990102a
--- /dev/null
+++ b/drivers/gpu/host1x/hw/syncpt_hw.c
@@ -0,0 +1,114 @@
+/*
+ * Tegra host1x Syncpoints
+ *
+ * Copyright (c) 2010-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/io.h>
+
+#include "dev.h"
+#include "syncpt.h"
+
+/*
+ * Write the current syncpoint value back to hw.
+ */
+static void syncpt_restore(struct host1x_syncpt *sp)
+{
+	struct host1x *host = sp->host;
+	int min = host1x_syncpt_read_min(sp);
+	host1x_sync_writel(host, min, HOST1X_SYNC_SYNCPT(sp->id));
+}
+
+/*
+ * Write the current waitbase value back to hw.
+ */
+static void syncpt_restore_wait_base(struct host1x_syncpt *sp)
+{
+	struct host1x *host = sp->host;
+	host1x_sync_writel(host, sp->base_val,
+			   HOST1X_SYNC_SYNCPT_BASE(sp->id));
+}
+
+/*
+ * Read waitbase value from hw.
+ */
+static void syncpt_read_wait_base(struct host1x_syncpt *sp)
+{
+	struct host1x *host = sp->host;
+	sp->base_val =
+		host1x_sync_readl(host, HOST1X_SYNC_SYNCPT_BASE(sp->id));
+}
+
+/*
+ * Updates the last value read from hardware.
+ */
+static u32 syncpt_load(struct host1x_syncpt *sp)
+{
+	struct host1x *host = sp->host;
+	u32 old, live;
+
+	/* Loop in case there's a race writing to min_val */
+	do {
+		old = host1x_syncpt_read_min(sp);
+		live = host1x_sync_readl(host, HOST1X_SYNC_SYNCPT(sp->id));
+	} while ((u32)atomic_cmpxchg(&sp->min_val, old, live) != old);
+
+	if (!host1x_syncpt_check_max(sp, live))
+		dev_err(host->dev, "%s failed: id=%u, min=%d, max=%d\n",
+			__func__, sp->id, host1x_syncpt_read_min(sp),
+			host1x_syncpt_read_max(sp));
+
+	return live;
+}
+
+/*
+ * Write a cpu syncpoint increment to the hardware, without touching
+ * the cache.
+ */
+static void syncpt_cpu_incr(struct host1x_syncpt *sp)
+{
+	struct host1x *host = sp->host;
+	u32 reg_offset = sp->id / 32;
+
+	if (!host1x_syncpt_client_managed(sp) &&
+	    host1x_syncpt_idle(sp)) {
+		dev_err(host->dev, "Trying to increment syncpoint id %d beyond max\n",
+			sp->id);
+		host1x_debug_dump(sp->host);
+		return;
+	}
+	host1x_sync_writel(host, BIT_MASK(sp->id),
+			   HOST1X_SYNC_SYNCPT_CPU_INCR(reg_offset));
+	wmb();
+}
+
+/* remove a wait pointed to by patch_addr */
+static int syncpt_patch_wait(struct host1x_syncpt *sp, void *patch_addr)
+{
+	u32 override = host1x_class_host_wait_syncpt(
+		HOST1X_SYNCPT_RESERVED, 0);
+
+	*((u32 *)patch_addr) = override;
+	return 0;
+}
+
+static const struct host1x_syncpt_ops host1x_syncpt_ops = {
+	.restore = syncpt_restore,
+	.restore_wait_base = syncpt_restore_wait_base,
+	.load_wait_base = syncpt_read_wait_base,
+	.load = syncpt_load,
+	.cpu_incr = syncpt_cpu_incr,
+	.patch_wait = syncpt_patch_wait,
+};
diff --git a/drivers/gpu/host1x/intr.c b/drivers/gpu/host1x/intr.c
new file mode 100644
index 000000000000..2491bf82e30c
--- /dev/null
+++ b/drivers/gpu/host1x/intr.c
@@ -0,0 +1,354 @@
+/*
+ * Tegra host1x Interrupt Management
+ *
+ * Copyright (c) 2010-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/clk.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+
+#include <trace/events/host1x.h>
+#include "channel.h"
+#include "dev.h"
+#include "intr.h"
+
+/* Wait list management */
+
+enum waitlist_state {
+	WLS_PENDING,
+	WLS_REMOVED,
+	WLS_CANCELLED,
+	WLS_HANDLED
+};
+
+static void waiter_release(struct kref *kref)
+{
+	kfree(container_of(kref, struct host1x_waitlist, refcount));
+}
+
+/*
+ * add a waiter to a waiter queue, sorted by threshold
+ * returns true if it was added at the head of the queue
+ */
+static bool add_waiter_to_queue(struct host1x_waitlist *waiter,
+				struct list_head *queue)
+{
+	struct host1x_waitlist *pos;
+	u32 thresh = waiter->thresh;
+
+	list_for_each_entry_reverse(pos, queue, list)
+		if ((s32)(pos->thresh - thresh) <= 0) {
+			list_add(&waiter->list, &pos->list);
+			return false;
+		}
+
+	list_add(&waiter->list, queue);
+	return true;
+}
+
+/*
+ * run through a waiter queue for a single sync point ID
+ * and gather all completed waiters into lists by actions
+ */
+static void remove_completed_waiters(struct list_head *head, u32 sync,
+			struct list_head completed[HOST1X_INTR_ACTION_COUNT])
+{
+	struct list_head *dest;
+	struct host1x_waitlist *waiter, *next, *prev;
+
+	list_for_each_entry_safe(waiter, next, head, list) {
+		if ((s32)(waiter->thresh - sync) > 0)
+			break;
+
+		dest = completed + waiter->action;
+
+		/* consolidate submit cleanups */
+		if (waiter->action == HOST1X_INTR_ACTION_SUBMIT_COMPLETE &&
+		    !list_empty(dest)) {
+			prev = list_entry(dest->prev,
+					  struct host1x_waitlist, list);
+			if (prev->data == waiter->data) {
+				prev->count++;
+				dest = NULL;
+			}
+		}
+
+		/* PENDING->REMOVED or CANCELLED->HANDLED */
+		if (atomic_inc_return(&waiter->state) == WLS_HANDLED || !dest) {
+			list_del(&waiter->list);
+			kref_put(&waiter->refcount, waiter_release);
+		} else
+			list_move_tail(&waiter->list, dest);
+	}
+}
+
+static void reset_threshold_interrupt(struct host1x *host,
+				      struct list_head *head,
+				      unsigned int id)
+{
+	u32 thresh =
+		list_first_entry(head, struct host1x_waitlist, list)->thresh;
+
+	host1x_hw_intr_set_syncpt_threshold(host, id, thresh);
+	host1x_hw_intr_enable_syncpt_intr(host, id);
+}
+
+static void action_submit_complete(struct host1x_waitlist *waiter)
+{
+	struct host1x_channel *channel = waiter->data;
+
+	host1x_cdma_update(&channel->cdma);
+
+	/*  Add nr_completed to trace */
+	trace_host1x_channel_submit_complete(dev_name(channel->dev),
+					     waiter->count, waiter->thresh);
+
+}
+
+static void action_wakeup(struct host1x_waitlist *waiter)
+{
+	wait_queue_head_t *wq = waiter->data;
+	wake_up(wq);
+}
+
+static void action_wakeup_interruptible(struct host1x_waitlist *waiter)
+{
+	wait_queue_head_t *wq = waiter->data;
+	wake_up_interruptible(wq);
+}
+
+typedef void (*action_handler)(struct host1x_waitlist *waiter);
+
+static action_handler action_handlers[HOST1X_INTR_ACTION_COUNT] = {
+	action_submit_complete,
+	action_wakeup,
+	action_wakeup_interruptible,
+};
+
+static void run_handlers(struct list_head completed[HOST1X_INTR_ACTION_COUNT])
+{
+	struct list_head *head = completed;
+	int i;
+
+	for (i = 0; i < HOST1X_INTR_ACTION_COUNT; ++i, ++head) {
+		action_handler handler = action_handlers[i];
+		struct host1x_waitlist *waiter, *next;
+
+		list_for_each_entry_safe(waiter, next, head, list) {
+			list_del(&waiter->list);
+			handler(waiter);
+			WARN_ON(atomic_xchg(&waiter->state, WLS_HANDLED) !=
+				WLS_REMOVED);
+			kref_put(&waiter->refcount, waiter_release);
+		}
+	}
+}
+
+/*
+ * Remove & handle all waiters that have completed for the given syncpt
+ */
+static int process_wait_list(struct host1x *host,
+			     struct host1x_syncpt *syncpt,
+			     u32 threshold)
+{
+	struct list_head completed[HOST1X_INTR_ACTION_COUNT];
+	unsigned int i;
+	int empty;
+
+	for (i = 0; i < HOST1X_INTR_ACTION_COUNT; ++i)
+		INIT_LIST_HEAD(completed + i);
+
+	spin_lock(&syncpt->intr.lock);
+
+	remove_completed_waiters(&syncpt->intr.wait_head, threshold,
+				 completed);
+
+	empty = list_empty(&syncpt->intr.wait_head);
+	if (empty)
+		host1x_hw_intr_disable_syncpt_intr(host, syncpt->id);
+	else
+		reset_threshold_interrupt(host, &syncpt->intr.wait_head,
+					  syncpt->id);
+
+	spin_unlock(&syncpt->intr.lock);
+
+	run_handlers(completed);
+
+	return empty;
+}
+
+/*
+ * Sync point threshold interrupt service thread function
+ * Handles sync point threshold triggers, in thread context
+ */
+
+static void syncpt_thresh_work(struct work_struct *work)
+{
+	struct host1x_syncpt_intr *syncpt_intr =
+		container_of(work, struct host1x_syncpt_intr, work);
+	struct host1x_syncpt *syncpt =
+		container_of(syncpt_intr, struct host1x_syncpt, intr);
+	unsigned int id = syncpt->id;
+	struct host1x *host = syncpt->host;
+
+	(void)process_wait_list(host, syncpt,
+				host1x_syncpt_load(host->syncpt + id));
+}
+
+int host1x_intr_add_action(struct host1x *host, u32 id, u32 thresh,
+			   enum host1x_intr_action action, void *data,
+			   struct host1x_waitlist *waiter, void **ref)
+{
+	struct host1x_syncpt *syncpt;
+	int queue_was_empty;
+
+	if (waiter == NULL) {
+		pr_warn("%s: NULL waiter\n", __func__);
+		return -EINVAL;
+	}
+
+	/* initialize a new waiter */
+	INIT_LIST_HEAD(&waiter->list);
+	kref_init(&waiter->refcount);
+	if (ref)
+		kref_get(&waiter->refcount);
+	waiter->thresh = thresh;
+	waiter->action = action;
+	atomic_set(&waiter->state, WLS_PENDING);
+	waiter->data = data;
+	waiter->count = 1;
+
+	syncpt = host->syncpt + id;
+
+	spin_lock(&syncpt->intr.lock);
+
+	queue_was_empty = list_empty(&syncpt->intr.wait_head);
+
+	if (add_waiter_to_queue(waiter, &syncpt->intr.wait_head)) {
+		/* added at head of list - new threshold value */
+		host1x_hw_intr_set_syncpt_threshold(host, id, thresh);
+
+		/* added as first waiter - enable interrupt */
+		if (queue_was_empty)
+			host1x_hw_intr_enable_syncpt_intr(host, id);
+	}
+
+	spin_unlock(&syncpt->intr.lock);
+
+	if (ref)
+		*ref = waiter;
+	return 0;
+}
+
+void host1x_intr_put_ref(struct host1x *host, u32 id, void *ref)
+{
+	struct host1x_waitlist *waiter = ref;
+	struct host1x_syncpt *syncpt;
+
+	while (atomic_cmpxchg(&waiter->state, WLS_PENDING, WLS_CANCELLED) ==
+	       WLS_REMOVED)
+		schedule();
+
+	syncpt = host->syncpt + id;
+	(void)process_wait_list(host, syncpt,
+				host1x_syncpt_load(host->syncpt + id));
+
+	kref_put(&waiter->refcount, waiter_release);
+}
+
+int host1x_intr_init(struct host1x *host, unsigned int irq_sync)
+{
+	unsigned int id;
+	u32 nb_pts = host1x_syncpt_nb_pts(host);
+
+	mutex_init(&host->intr_mutex);
+	host->intr_syncpt_irq = irq_sync;
+	host->intr_wq = create_workqueue("host_syncpt");
+	if (!host->intr_wq)
+		return -ENOMEM;
+
+	for (id = 0; id < nb_pts; ++id) {
+		struct host1x_syncpt *syncpt = host->syncpt + id;
+
+		spin_lock_init(&syncpt->intr.lock);
+		INIT_LIST_HEAD(&syncpt->intr.wait_head);
+		snprintf(syncpt->intr.thresh_irq_name,
+			 sizeof(syncpt->intr.thresh_irq_name),
+			 "host1x_sp_%02d", id);
+	}
+
+	host1x_intr_start(host);
+
+	return 0;
+}
+
+void host1x_intr_deinit(struct host1x *host)
+{
+	host1x_intr_stop(host);
+	destroy_workqueue(host->intr_wq);
+}
+
+void host1x_intr_start(struct host1x *host)
+{
+	u32 hz = clk_get_rate(host->clk);
+	int err;
+
+	mutex_lock(&host->intr_mutex);
+	err = host1x_hw_intr_init_host_sync(host, DIV_ROUND_UP(hz, 1000000),
+					    syncpt_thresh_work);
+	if (err) {
+		mutex_unlock(&host->intr_mutex);
+		return;
+	}
+	mutex_unlock(&host->intr_mutex);
+}
+
+void host1x_intr_stop(struct host1x *host)
+{
+	unsigned int id;
+	struct host1x_syncpt *syncpt = host->syncpt;
+	u32 nb_pts = host1x_syncpt_nb_pts(host);
+
+	mutex_lock(&host->intr_mutex);
+
+	host1x_hw_intr_disable_all_syncpt_intrs(host);
+
+	for (id = 0; id < nb_pts; ++id) {
+		struct host1x_waitlist *waiter, *next;
+
+		list_for_each_entry_safe(waiter, next,
+			&syncpt[id].intr.wait_head, list) {
+			if (atomic_cmpxchg(&waiter->state,
+			    WLS_CANCELLED, WLS_HANDLED) == WLS_CANCELLED) {
+				list_del(&waiter->list);
+				kref_put(&waiter->refcount, waiter_release);
+			}
+		}
+
+		if (!list_empty(&syncpt[id].intr.wait_head)) {
+			/* output diagnostics */
+			mutex_unlock(&host->intr_mutex);
+			pr_warn("%s cannot stop syncpt intr id=%d\n",
+				__func__, id);
+			return;
+		}
+	}
+
+	host1x_hw_intr_free_syncpt_irq(host);
+
+	mutex_unlock(&host->intr_mutex);
+}
diff --git a/drivers/gpu/host1x/intr.h b/drivers/gpu/host1x/intr.h
new file mode 100644
index 000000000000..2b8adf016a05
--- /dev/null
+++ b/drivers/gpu/host1x/intr.h
@@ -0,0 +1,102 @@
+/*
+ * Tegra host1x Interrupt Management
+ *
+ * Copyright (c) 2010-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __HOST1X_INTR_H
+#define __HOST1X_INTR_H
+
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+
+struct host1x;
+
+enum host1x_intr_action {
+	/*
+	 * Perform cleanup after a submit has completed.
+	 * 'data' points to a channel
+	 */
+	HOST1X_INTR_ACTION_SUBMIT_COMPLETE = 0,
+
+	/*
+	 * Wake up a  task.
+	 * 'data' points to a wait_queue_head_t
+	 */
+	HOST1X_INTR_ACTION_WAKEUP,
+
+	/*
+	 * Wake up a interruptible task.
+	 * 'data' points to a wait_queue_head_t
+	 */
+	HOST1X_INTR_ACTION_WAKEUP_INTERRUPTIBLE,
+
+	HOST1X_INTR_ACTION_COUNT
+};
+
+struct host1x_syncpt_intr {
+	spinlock_t lock;
+	struct list_head wait_head;
+	char thresh_irq_name[12];
+	struct work_struct work;
+};
+
+struct host1x_waitlist {
+	struct list_head list;
+	struct kref refcount;
+	u32 thresh;
+	enum host1x_intr_action action;
+	atomic_t state;
+	void *data;
+	int count;
+};
+
+/*
+ * Schedule an action to be taken when a sync point reaches the given threshold.
+ *
+ * @id the sync point
+ * @thresh the threshold
+ * @action the action to take
+ * @data a pointer to extra data depending on action, see above
+ * @waiter waiter structure - assumes ownership
+ * @ref must be passed if cancellation is possible, else NULL
+ *
+ * This is a non-blocking api.
+ */
+int host1x_intr_add_action(struct host1x *host, u32 id, u32 thresh,
+	enum host1x_intr_action action, void *data,
+	struct host1x_waitlist *waiter, void **ref);
+
+/*
+ * Unreference an action submitted to host1x_intr_add_action().
+ * You must call this if you passed non-NULL as ref.
+ * @ref the ref returned from host1x_intr_add_action()
+ */
+void host1x_intr_put_ref(struct host1x *host, u32 id, void *ref);
+
+/* Initialize host1x sync point interrupt */
+int host1x_intr_init(struct host1x *host, unsigned int irq_sync);
+
+/* Deinitialize host1x sync point interrupt */
+void host1x_intr_deinit(struct host1x *host);
+
+/* Enable host1x sync point interrupt */
+void host1x_intr_start(struct host1x *host);
+
+/* Disable host1x sync point interrupt */
+void host1x_intr_stop(struct host1x *host);
+
+irqreturn_t host1x_syncpt_thresh_fn(void *dev_id);
+#endif
diff --git a/drivers/gpu/host1x/job.c b/drivers/gpu/host1x/job.c
new file mode 100644
index 000000000000..f665d679031c
--- /dev/null
+++ b/drivers/gpu/host1x/job.c
@@ -0,0 +1,603 @@
+/*
+ * Tegra host1x Job
+ *
+ * Copyright (c) 2010-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/kref.h>
+#include <linux/module.h>
+#include <linux/scatterlist.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <trace/events/host1x.h>
+
+#include "channel.h"
+#include "dev.h"
+#include "host1x_bo.h"
+#include "job.h"
+#include "syncpt.h"
+
+struct host1x_job *host1x_job_alloc(struct host1x_channel *ch,
+				    u32 num_cmdbufs, u32 num_relocs,
+				    u32 num_waitchks)
+{
+	struct host1x_job *job = NULL;
+	unsigned int num_unpins = num_cmdbufs + num_relocs;
+	u64 total;
+	void *mem;
+
+	/* Check that we're not going to overflow */
+	total = sizeof(struct host1x_job) +
+		num_relocs * sizeof(struct host1x_reloc) +
+		num_unpins * sizeof(struct host1x_job_unpin_data) +
+		num_waitchks * sizeof(struct host1x_waitchk) +
+		num_cmdbufs * sizeof(struct host1x_job_gather) +
+		num_unpins * sizeof(dma_addr_t) +
+		num_unpins * sizeof(u32 *);
+	if (total > ULONG_MAX)
+		return NULL;
+
+	mem = job = kzalloc(total, GFP_KERNEL);
+	if (!job)
+		return NULL;
+
+	kref_init(&job->ref);
+	job->channel = ch;
+
+	/* Redistribute memory to the structs  */
+	mem += sizeof(struct host1x_job);
+	job->relocarray = num_relocs ? mem : NULL;
+	mem += num_relocs * sizeof(struct host1x_reloc);
+	job->unpins = num_unpins ? mem : NULL;
+	mem += num_unpins * sizeof(struct host1x_job_unpin_data);
+	job->waitchk = num_waitchks ? mem : NULL;
+	mem += num_waitchks * sizeof(struct host1x_waitchk);
+	job->gathers = num_cmdbufs ? mem : NULL;
+	mem += num_cmdbufs * sizeof(struct host1x_job_gather);
+	job->addr_phys = num_unpins ? mem : NULL;
+
+	job->reloc_addr_phys = job->addr_phys;
+	job->gather_addr_phys = &job->addr_phys[num_relocs];
+
+	return job;
+}
+
+struct host1x_job *host1x_job_get(struct host1x_job *job)
+{
+	kref_get(&job->ref);
+	return job;
+}
+
+static void job_free(struct kref *ref)
+{
+	struct host1x_job *job = container_of(ref, struct host1x_job, ref);
+
+	kfree(job);
+}
+
+void host1x_job_put(struct host1x_job *job)
+{
+	kref_put(&job->ref, job_free);
+}
+
+void host1x_job_add_gather(struct host1x_job *job, struct host1x_bo *bo,
+			   u32 words, u32 offset)
+{
+	struct host1x_job_gather *cur_gather = &job->gathers[job->num_gathers];
+
+	cur_gather->words = words;
+	cur_gather->bo = bo;
+	cur_gather->offset = offset;
+	job->num_gathers++;
+}
+
+/*
+ * NULL an already satisfied WAIT_SYNCPT host method, by patching its
+ * args in the command stream. The method data is changed to reference
+ * a reserved (never given out or incr) HOST1X_SYNCPT_RESERVED syncpt
+ * with a matching threshold value of 0, so is guaranteed to be popped
+ * by the host HW.
+ */
+static void host1x_syncpt_patch_offset(struct host1x_syncpt *sp,
+				       struct host1x_bo *h, u32 offset)
+{
+	void *patch_addr = NULL;
+
+	/* patch the wait */
+	patch_addr = host1x_bo_kmap(h, offset >> PAGE_SHIFT);
+	if (patch_addr) {
+		host1x_syncpt_patch_wait(sp,
+					 patch_addr + (offset & ~PAGE_MASK));
+		host1x_bo_kunmap(h, offset >> PAGE_SHIFT, patch_addr);
+	} else
+		pr_err("Could not map cmdbuf for wait check\n");
+}
+
+/*
+ * Check driver supplied waitchk structs for syncpt thresholds
+ * that have already been satisfied and NULL the comparison (to
+ * avoid a wrap condition in the HW).
+ */
+static int do_waitchks(struct host1x_job *job, struct host1x *host,
+		       struct host1x_bo *patch)
+{
+	int i;
+
+	/* compare syncpt vs wait threshold */
+	for (i = 0; i < job->num_waitchk; i++) {
+		struct host1x_waitchk *wait = &job->waitchk[i];
+		struct host1x_syncpt *sp =
+			host1x_syncpt_get(host, wait->syncpt_id);
+
+		/* validate syncpt id */
+		if (wait->syncpt_id > host1x_syncpt_nb_pts(host))
+			continue;
+
+		/* skip all other gathers */
+		if (patch != wait->bo)
+			continue;
+
+		trace_host1x_syncpt_wait_check(wait->bo, wait->offset,
+					       wait->syncpt_id, wait->thresh,
+					       host1x_syncpt_read_min(sp));
+
+		if (host1x_syncpt_is_expired(sp, wait->thresh)) {
+			dev_dbg(host->dev,
+				"drop WAIT id %d (%s) thresh 0x%x, min 0x%x\n",
+				wait->syncpt_id, sp->name, wait->thresh,
+				host1x_syncpt_read_min(sp));
+
+			host1x_syncpt_patch_offset(sp, patch, wait->offset);
+		}
+
+		wait->bo = NULL;
+	}
+
+	return 0;
+}
+
+static unsigned int pin_job(struct host1x_job *job)
+{
+	unsigned int i;
+
+	job->num_unpins = 0;
+
+	for (i = 0; i < job->num_relocs; i++) {
+		struct host1x_reloc *reloc = &job->relocarray[i];
+		struct sg_table *sgt;
+		dma_addr_t phys_addr;
+
+		reloc->target = host1x_bo_get(reloc->target);
+		if (!reloc->target)
+			goto unpin;
+
+		phys_addr = host1x_bo_pin(reloc->target, &sgt);
+		if (!phys_addr)
+			goto unpin;
+
+		job->addr_phys[job->num_unpins] = phys_addr;
+		job->unpins[job->num_unpins].bo = reloc->target;
+		job->unpins[job->num_unpins].sgt = sgt;
+		job->num_unpins++;
+	}
+
+	for (i = 0; i < job->num_gathers; i++) {
+		struct host1x_job_gather *g = &job->gathers[i];
+		struct sg_table *sgt;
+		dma_addr_t phys_addr;
+
+		g->bo = host1x_bo_get(g->bo);
+		if (!g->bo)
+			goto unpin;
+
+		phys_addr = host1x_bo_pin(g->bo, &sgt);
+		if (!phys_addr)
+			goto unpin;
+
+		job->addr_phys[job->num_unpins] = phys_addr;
+		job->unpins[job->num_unpins].bo = g->bo;
+		job->unpins[job->num_unpins].sgt = sgt;
+		job->num_unpins++;
+	}
+
+	return job->num_unpins;
+
+unpin:
+	host1x_job_unpin(job);
+	return 0;
+}
+
+static unsigned int do_relocs(struct host1x_job *job, struct host1x_bo *cmdbuf)
+{
+	int i = 0;
+	u32 last_page = ~0;
+	void *cmdbuf_page_addr = NULL;
+
+	/* pin & patch the relocs for one gather */
+	while (i < job->num_relocs) {
+		struct host1x_reloc *reloc = &job->relocarray[i];
+		u32 reloc_addr = (job->reloc_addr_phys[i] +
+			reloc->target_offset) >> reloc->shift;
+		u32 *target;
+
+		/* skip all other gathers */
+		if (!(reloc->cmdbuf && cmdbuf == reloc->cmdbuf)) {
+			i++;
+			continue;
+		}
+
+		if (last_page != reloc->cmdbuf_offset >> PAGE_SHIFT) {
+			if (cmdbuf_page_addr)
+				host1x_bo_kunmap(cmdbuf, last_page,
+						 cmdbuf_page_addr);
+
+			cmdbuf_page_addr = host1x_bo_kmap(cmdbuf,
+					reloc->cmdbuf_offset >> PAGE_SHIFT);
+			last_page = reloc->cmdbuf_offset >> PAGE_SHIFT;
+
+			if (unlikely(!cmdbuf_page_addr)) {
+				pr_err("Could not map cmdbuf for relocation\n");
+				return -ENOMEM;
+			}
+		}
+
+		target = cmdbuf_page_addr + (reloc->cmdbuf_offset & ~PAGE_MASK);
+		*target = reloc_addr;
+
+		/* mark this gather as handled */
+		reloc->cmdbuf = 0;
+	}
+
+	if (cmdbuf_page_addr)
+		host1x_bo_kunmap(cmdbuf, last_page, cmdbuf_page_addr);
+
+	return 0;
+}
+
+static int check_reloc(struct host1x_reloc *reloc, struct host1x_bo *cmdbuf,
+		       unsigned int offset)
+{
+	offset *= sizeof(u32);
+
+	if (reloc->cmdbuf != cmdbuf || reloc->cmdbuf_offset != offset)
+		return -EINVAL;
+
+	return 0;
+}
+
+struct host1x_firewall {
+	struct host1x_job *job;
+	struct device *dev;
+
+	unsigned int num_relocs;
+	struct host1x_reloc *reloc;
+
+	struct host1x_bo *cmdbuf_id;
+	unsigned int offset;
+
+	u32 words;
+	u32 class;
+	u32 reg;
+	u32 mask;
+	u32 count;
+};
+
+static int check_mask(struct host1x_firewall *fw)
+{
+	u32 mask = fw->mask;
+	u32 reg = fw->reg;
+
+	while (mask) {
+		if (fw->words == 0)
+			return -EINVAL;
+
+		if (mask & 1) {
+			if (fw->job->is_addr_reg(fw->dev, fw->class, reg)) {
+				bool bad_reloc = check_reloc(fw->reloc,
+							     fw->cmdbuf_id,
+							     fw->offset);
+				if (!fw->num_relocs || bad_reloc)
+					return -EINVAL;
+				fw->reloc++;
+				fw->num_relocs--;
+			}
+			fw->words--;
+			fw->offset++;
+		}
+		mask >>= 1;
+		reg++;
+	}
+
+	return 0;
+}
+
+static int check_incr(struct host1x_firewall *fw)
+{
+	u32 count = fw->count;
+	u32 reg = fw->reg;
+
+	while (fw) {
+		if (fw->words == 0)
+			return -EINVAL;
+
+		if (fw->job->is_addr_reg(fw->dev, fw->class, reg)) {
+			bool bad_reloc = check_reloc(fw->reloc, fw->cmdbuf_id,
+						     fw->offset);
+			if (!fw->num_relocs || bad_reloc)
+				return -EINVAL;
+			fw->reloc++;
+			fw->num_relocs--;
+		}
+		reg++;
+		fw->words--;
+		fw->offset++;
+		count--;
+	}
+
+	return 0;
+}
+
+static int check_nonincr(struct host1x_firewall *fw)
+{
+	int is_addr_reg = fw->job->is_addr_reg(fw->dev, fw->class, fw->reg);
+	u32 count = fw->count;
+
+	while (count) {
+		if (fw->words == 0)
+			return -EINVAL;
+
+		if (is_addr_reg) {
+			bool bad_reloc = check_reloc(fw->reloc, fw->cmdbuf_id,
+						     fw->offset);
+			if (!fw->num_relocs || bad_reloc)
+				return -EINVAL;
+			fw->reloc++;
+			fw->num_relocs--;
+		}
+		fw->words--;
+		fw->offset++;
+		count--;
+	}
+
+	return 0;
+}
+
+static int validate(struct host1x_job *job, struct device *dev,
+		    struct host1x_job_gather *g)
+{
+	u32 *cmdbuf_base;
+	int err = 0;
+	struct host1x_firewall fw;
+
+	fw.job = job;
+	fw.dev = dev;
+	fw.reloc = job->relocarray;
+	fw.num_relocs = job->num_relocs;
+	fw.cmdbuf_id = g->bo;
+
+	fw.offset = 0;
+	fw.class = 0;
+
+	if (!job->is_addr_reg)
+		return 0;
+
+	cmdbuf_base = host1x_bo_mmap(g->bo);
+	if (!cmdbuf_base)
+		return -ENOMEM;
+
+	fw.words = g->words;
+	while (fw.words && !err) {
+		u32 word = cmdbuf_base[fw.offset];
+		u32 opcode = (word & 0xf0000000) >> 28;
+
+		fw.mask = 0;
+		fw.reg = 0;
+		fw.count = 0;
+		fw.words--;
+		fw.offset++;
+
+		switch (opcode) {
+		case 0:
+			fw.class = word >> 6 & 0x3ff;
+			fw.mask = word & 0x3f;
+			fw.reg = word >> 16 & 0xfff;
+			err = check_mask(&fw);
+			if (err)
+				goto out;
+			break;
+		case 1:
+			fw.reg = word >> 16 & 0xfff;
+			fw.count = word & 0xffff;
+			err = check_incr(&fw);
+			if (err)
+				goto out;
+			break;
+
+		case 2:
+			fw.reg = word >> 16 & 0xfff;
+			fw.count = word & 0xffff;
+			err = check_nonincr(&fw);
+			if (err)
+				goto out;
+			break;
+
+		case 3:
+			fw.mask = word & 0xffff;
+			fw.reg = word >> 16 & 0xfff;
+			err = check_mask(&fw);
+			if (err)
+				goto out;
+			break;
+		case 4:
+		case 5:
+		case 14:
+			break;
+		default:
+			err = -EINVAL;
+			break;
+		}
+	}
+
+	/* No relocs should remain at this point */
+	if (fw.num_relocs)
+		err = -EINVAL;
+
+out:
+	host1x_bo_munmap(g->bo, cmdbuf_base);
+
+	return err;
+}
+
+static inline int copy_gathers(struct host1x_job *job, struct device *dev)
+{
+	size_t size = 0;
+	size_t offset = 0;
+	int i;
+
+	for (i = 0; i < job->num_gathers; i++) {
+		struct host1x_job_gather *g = &job->gathers[i];
+		size += g->words * sizeof(u32);
+	}
+
+	job->gather_copy_mapped = dma_alloc_writecombine(dev, size,
+							 &job->gather_copy,
+							 GFP_KERNEL);
+	if (!job->gather_copy_mapped) {
+		int err = PTR_ERR(job->gather_copy_mapped);
+		job->gather_copy_mapped = NULL;
+		return err;
+	}
+
+	job->gather_copy_size = size;
+
+	for (i = 0; i < job->num_gathers; i++) {
+		struct host1x_job_gather *g = &job->gathers[i];
+		void *gather;
+
+		gather = host1x_bo_mmap(g->bo);
+		memcpy(job->gather_copy_mapped + offset, gather + g->offset,
+		       g->words * sizeof(u32));
+		host1x_bo_munmap(g->bo, gather);
+
+		g->base = job->gather_copy;
+		g->offset = offset;
+		g->bo = NULL;
+
+		offset += g->words * sizeof(u32);
+	}
+
+	return 0;
+}
+
+int host1x_job_pin(struct host1x_job *job, struct device *dev)
+{
+	int err;
+	unsigned int i, j;
+	struct host1x *host = dev_get_drvdata(dev->parent);
+	DECLARE_BITMAP(waitchk_mask, host1x_syncpt_nb_pts(host));
+
+	bitmap_zero(waitchk_mask, host1x_syncpt_nb_pts(host));
+	for (i = 0; i < job->num_waitchk; i++) {
+		u32 syncpt_id = job->waitchk[i].syncpt_id;
+		if (syncpt_id < host1x_syncpt_nb_pts(host))
+			set_bit(syncpt_id, waitchk_mask);
+	}
+
+	/* get current syncpt values for waitchk */
+	for_each_set_bit(i, waitchk_mask, host1x_syncpt_nb_pts(host))
+		host1x_syncpt_load(host->syncpt + i);
+
+	/* pin memory */
+	err = pin_job(job);
+	if (!err)
+		goto out;
+
+	/* patch gathers */
+	for (i = 0; i < job->num_gathers; i++) {
+		struct host1x_job_gather *g = &job->gathers[i];
+
+		/* process each gather mem only once */
+		if (g->handled)
+			continue;
+
+		g->base = job->gather_addr_phys[i];
+
+		for (j = 0; j < job->num_gathers; j++)
+			if (job->gathers[j].bo == g->bo)
+				job->gathers[j].handled = true;
+
+		err = 0;
+
+		if (IS_ENABLED(CONFIG_TEGRA_HOST1X_FIREWALL))
+			err = validate(job, dev, g);
+
+		if (err)
+			dev_err(dev, "Job invalid (err=%d)\n", err);
+
+		if (!err)
+			err = do_relocs(job, g->bo);
+
+		if (!err)
+			err = do_waitchks(job, host, g->bo);
+
+		if (err)
+			break;
+	}
+
+	if (IS_ENABLED(CONFIG_TEGRA_HOST1X_FIREWALL) && !err) {
+		err = copy_gathers(job, dev);
+		if (err) {
+			host1x_job_unpin(job);
+			return err;
+		}
+	}
+
+out:
+	wmb();
+
+	return err;
+}
+
+void host1x_job_unpin(struct host1x_job *job)
+{
+	unsigned int i;
+
+	for (i = 0; i < job->num_unpins; i++) {
+		struct host1x_job_unpin_data *unpin = &job->unpins[i];
+		host1x_bo_unpin(unpin->bo, unpin->sgt);
+		host1x_bo_put(unpin->bo);
+	}
+	job->num_unpins = 0;
+
+	if (job->gather_copy_size)
+		dma_free_writecombine(job->channel->dev, job->gather_copy_size,
+				      job->gather_copy_mapped,
+				      job->gather_copy);
+}
+
+/*
+ * Debug routine used to dump job entries
+ */
+void host1x_job_dump(struct device *dev, struct host1x_job *job)
+{
+	dev_dbg(dev, "    SYNCPT_ID   %d\n", job->syncpt_id);
+	dev_dbg(dev, "    SYNCPT_VAL  %d\n", job->syncpt_end);
+	dev_dbg(dev, "    FIRST_GET   0x%x\n", job->first_get);
+	dev_dbg(dev, "    TIMEOUT     %d\n", job->timeout);
+	dev_dbg(dev, "    NUM_SLOTS   %d\n", job->num_slots);
+	dev_dbg(dev, "    NUM_HANDLES %d\n", job->num_unpins);
+}
diff --git a/drivers/gpu/host1x/job.h b/drivers/gpu/host1x/job.h
new file mode 100644
index 000000000000..fba45f20458e
--- /dev/null
+++ b/drivers/gpu/host1x/job.h
@@ -0,0 +1,162 @@
+/*
+ * Tegra host1x Job
+ *
+ * Copyright (c) 2011-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __HOST1X_JOB_H
+#define __HOST1X_JOB_H
+
+struct host1x_job_gather {
+	u32 words;
+	dma_addr_t base;
+	struct host1x_bo *bo;
+	int offset;
+	bool handled;
+};
+
+struct host1x_cmdbuf {
+	u32 handle;
+	u32 offset;
+	u32 words;
+	u32 pad;
+};
+
+struct host1x_reloc {
+	struct host1x_bo *cmdbuf;
+	u32 cmdbuf_offset;
+	struct host1x_bo *target;
+	u32 target_offset;
+	u32 shift;
+	u32 pad;
+};
+
+struct host1x_waitchk {
+	struct host1x_bo *bo;
+	u32 offset;
+	u32 syncpt_id;
+	u32 thresh;
+};
+
+struct host1x_job_unpin_data {
+	struct host1x_bo *bo;
+	struct sg_table *sgt;
+};
+
+/*
+ * Each submit is tracked as a host1x_job.
+ */
+struct host1x_job {
+	/* When refcount goes to zero, job can be freed */
+	struct kref ref;
+
+	/* List entry */
+	struct list_head list;
+
+	/* Channel where job is submitted to */
+	struct host1x_channel *channel;
+
+	u32 client;
+
+	/* Gathers and their memory */
+	struct host1x_job_gather *gathers;
+	unsigned int num_gathers;
+
+	/* Wait checks to be processed at submit time */
+	struct host1x_waitchk *waitchk;
+	unsigned int num_waitchk;
+	u32 waitchk_mask;
+
+	/* Array of handles to be pinned & unpinned */
+	struct host1x_reloc *relocarray;
+	unsigned int num_relocs;
+	struct host1x_job_unpin_data *unpins;
+	unsigned int num_unpins;
+
+	dma_addr_t *addr_phys;
+	dma_addr_t *gather_addr_phys;
+	dma_addr_t *reloc_addr_phys;
+
+	/* Sync point id, number of increments and end related to the submit */
+	u32 syncpt_id;
+	u32 syncpt_incrs;
+	u32 syncpt_end;
+
+	/* Maximum time to wait for this job */
+	unsigned int timeout;
+
+	/* Index and number of slots used in the push buffer */
+	unsigned int first_get;
+	unsigned int num_slots;
+
+	/* Copy of gathers */
+	size_t gather_copy_size;
+	dma_addr_t gather_copy;
+	u8 *gather_copy_mapped;
+
+	/* Check if register is marked as an address reg */
+	int (*is_addr_reg)(struct device *dev, u32 reg, u32 class);
+
+	/* Request a SETCLASS to this class */
+	u32 class;
+
+	/* Add a channel wait for previous ops to complete */
+	bool serialize;
+};
+/*
+ * Allocate memory for a job. Just enough memory will be allocated to
+ * accomodate the submit.
+ */
+struct host1x_job *host1x_job_alloc(struct host1x_channel *ch,
+				    u32 num_cmdbufs, u32 num_relocs,
+				    u32 num_waitchks);
+
+/*
+ * Add a gather to a job.
+ */
+void host1x_job_add_gather(struct host1x_job *job, struct host1x_bo *mem_id,
+			   u32 words, u32 offset);
+
+/*
+ * Increment reference going to host1x_job.
+ */
+struct host1x_job *host1x_job_get(struct host1x_job *job);
+
+/*
+ * Decrement reference job, free if goes to zero.
+ */
+void host1x_job_put(struct host1x_job *job);
+
+/*
+ * Pin memory related to job. This handles relocation of addresses to the
+ * host1x address space. Handles both the gather memory and any other memory
+ * referred to from the gather buffers.
+ *
+ * Handles also patching out host waits that would wait for an expired sync
+ * point value.
+ */
+int host1x_job_pin(struct host1x_job *job, struct device *dev);
+
+/*
+ * Unpin memory related to job.
+ */
+void host1x_job_unpin(struct host1x_job *job);
+
+/*
+ * Dump contents of job to debug output.
+ */
+void host1x_job_dump(struct device *dev, struct host1x_job *job);
+
+#endif
diff --git a/drivers/gpu/host1x/syncpt.c b/drivers/gpu/host1x/syncpt.c
new file mode 100644
index 000000000000..4b493453e805
--- /dev/null
+++ b/drivers/gpu/host1x/syncpt.c
@@ -0,0 +1,387 @@
+/*
+ * Tegra host1x Syncpoints
+ *
+ * Copyright (c) 2010-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+
+#include <trace/events/host1x.h>
+
+#include "syncpt.h"
+#include "dev.h"
+#include "intr.h"
+#include "debug.h"
+
+#define SYNCPT_CHECK_PERIOD (2 * HZ)
+#define MAX_STUCK_CHECK_COUNT 15
+
+static struct host1x_syncpt *_host1x_syncpt_alloc(struct host1x *host,
+						  struct device *dev,
+						  int client_managed)
+{
+	int i;
+	struct host1x_syncpt *sp = host->syncpt;
+	char *name;
+
+	for (i = 0; i < host->info->nb_pts && sp->name; i++, sp++)
+		;
+	if (sp->dev)
+		return NULL;
+
+	name = kasprintf(GFP_KERNEL, "%02d-%s", sp->id,
+			dev ? dev_name(dev) : NULL);
+	if (!name)
+		return NULL;
+
+	sp->dev = dev;
+	sp->name = name;
+	sp->client_managed = client_managed;
+
+	return sp;
+}
+
+u32 host1x_syncpt_id(struct host1x_syncpt *sp)
+{
+	return sp->id;
+}
+
+/*
+ * Updates the value sent to hardware.
+ */
+u32 host1x_syncpt_incr_max(struct host1x_syncpt *sp, u32 incrs)
+{
+	return (u32)atomic_add_return(incrs, &sp->max_val);
+}
+
+ /*
+ * Write cached syncpoint and waitbase values to hardware.
+ */
+void host1x_syncpt_restore(struct host1x *host)
+{
+	struct host1x_syncpt *sp_base = host->syncpt;
+	u32 i;
+
+	for (i = 0; i < host1x_syncpt_nb_pts(host); i++)
+		host1x_hw_syncpt_restore(host, sp_base + i);
+	for (i = 0; i < host1x_syncpt_nb_bases(host); i++)
+		host1x_hw_syncpt_restore_wait_base(host, sp_base + i);
+	wmb();
+}
+
+/*
+ * Update the cached syncpoint and waitbase values by reading them
+ * from the registers.
+  */
+void host1x_syncpt_save(struct host1x *host)
+{
+	struct host1x_syncpt *sp_base = host->syncpt;
+	u32 i;
+
+	for (i = 0; i < host1x_syncpt_nb_pts(host); i++) {
+		if (host1x_syncpt_client_managed(sp_base + i))
+			host1x_hw_syncpt_load(host, sp_base + i);
+		else
+			WARN_ON(!host1x_syncpt_idle(sp_base + i));
+	}
+
+	for (i = 0; i < host1x_syncpt_nb_bases(host); i++)
+		host1x_hw_syncpt_load_wait_base(host, sp_base + i);
+}
+
+/*
+ * Updates the cached syncpoint value by reading a new value from the hardware
+ * register
+ */
+u32 host1x_syncpt_load(struct host1x_syncpt *sp)
+{
+	u32 val;
+	val = host1x_hw_syncpt_load(sp->host, sp);
+	trace_host1x_syncpt_load_min(sp->id, val);
+
+	return val;
+}
+
+/*
+ * Get the current syncpoint base
+ */
+u32 host1x_syncpt_load_wait_base(struct host1x_syncpt *sp)
+{
+	u32 val;
+	host1x_hw_syncpt_load_wait_base(sp->host, sp);
+	val = sp->base_val;
+	return val;
+}
+
+/*
+ * Write a cpu syncpoint increment to the hardware, without touching
+ * the cache. Caller is responsible for host being powered.
+ */
+void host1x_syncpt_cpu_incr(struct host1x_syncpt *sp)
+{
+	host1x_hw_syncpt_cpu_incr(sp->host, sp);
+}
+
+/*
+ * Increment syncpoint value from cpu, updating cache
+ */
+void host1x_syncpt_incr(struct host1x_syncpt *sp)
+{
+	if (host1x_syncpt_client_managed(sp))
+		host1x_syncpt_incr_max(sp, 1);
+	host1x_syncpt_cpu_incr(sp);
+}
+
+/*
+ * Updated sync point form hardware, and returns true if syncpoint is expired,
+ * false if we may need to wait
+ */
+static bool syncpt_load_min_is_expired(struct host1x_syncpt *sp, u32 thresh)
+{
+	host1x_hw_syncpt_load(sp->host, sp);
+	return host1x_syncpt_is_expired(sp, thresh);
+}
+
+/*
+ * Main entrypoint for syncpoint value waits.
+ */
+int host1x_syncpt_wait(struct host1x_syncpt *sp, u32 thresh, long timeout,
+			u32 *value)
+{
+	DECLARE_WAIT_QUEUE_HEAD_ONSTACK(wq);
+	void *ref;
+	struct host1x_waitlist *waiter;
+	int err = 0, check_count = 0;
+	u32 val;
+
+	if (value)
+		*value = 0;
+
+	/* first check cache */
+	if (host1x_syncpt_is_expired(sp, thresh)) {
+		if (value)
+			*value = host1x_syncpt_load(sp);
+		return 0;
+	}
+
+	/* try to read from register */
+	val = host1x_hw_syncpt_load(sp->host, sp);
+	if (host1x_syncpt_is_expired(sp, thresh)) {
+		if (value)
+			*value = val;
+		goto done;
+	}
+
+	if (!timeout) {
+		err = -EAGAIN;
+		goto done;
+	}
+
+	/* allocate a waiter */
+	waiter = kzalloc(sizeof(*waiter), GFP_KERNEL);
+	if (!waiter) {
+		err = -ENOMEM;
+		goto done;
+	}
+
+	/* schedule a wakeup when the syncpoint value is reached */
+	err = host1x_intr_add_action(sp->host, sp->id, thresh,
+				     HOST1X_INTR_ACTION_WAKEUP_INTERRUPTIBLE,
+				     &wq, waiter, &ref);
+	if (err)
+		goto done;
+
+	err = -EAGAIN;
+	/* Caller-specified timeout may be impractically low */
+	if (timeout < 0)
+		timeout = LONG_MAX;
+
+	/* wait for the syncpoint, or timeout, or signal */
+	while (timeout) {
+		long check = min_t(long, SYNCPT_CHECK_PERIOD, timeout);
+		int remain = wait_event_interruptible_timeout(wq,
+				syncpt_load_min_is_expired(sp, thresh),
+				check);
+		if (remain > 0 || host1x_syncpt_is_expired(sp, thresh)) {
+			if (value)
+				*value = host1x_syncpt_load(sp);
+			err = 0;
+			break;
+		}
+		if (remain < 0) {
+			err = remain;
+			break;
+		}
+		timeout -= check;
+		if (timeout && check_count <= MAX_STUCK_CHECK_COUNT) {
+			dev_warn(sp->host->dev,
+				"%s: syncpoint id %d (%s) stuck waiting %d, timeout=%ld\n",
+				 current->comm, sp->id, sp->name,
+				 thresh, timeout);
+
+			host1x_debug_dump_syncpts(sp->host);
+			if (check_count == MAX_STUCK_CHECK_COUNT)
+				host1x_debug_dump(sp->host);
+			check_count++;
+		}
+	}
+	host1x_intr_put_ref(sp->host, sp->id, ref);
+
+done:
+	return err;
+}
+EXPORT_SYMBOL(host1x_syncpt_wait);
+
+/*
+ * Returns true if syncpoint is expired, false if we may need to wait
+ */
+bool host1x_syncpt_is_expired(struct host1x_syncpt *sp, u32 thresh)
+{
+	u32 current_val;
+	u32 future_val;
+	smp_rmb();
+	current_val = (u32)atomic_read(&sp->min_val);
+	future_val = (u32)atomic_read(&sp->max_val);
+
+	/* Note the use of unsigned arithmetic here (mod 1<<32).
+	 *
+	 * c = current_val = min_val	= the current value of the syncpoint.
+	 * t = thresh			= the value we are checking
+	 * f = future_val  = max_val	= the value c will reach when all
+	 *				  outstanding increments have completed.
+	 *
+	 * Note that c always chases f until it reaches f.
+	 *
+	 * Dtf = (f - t)
+	 * Dtc = (c - t)
+	 *
+	 *  Consider all cases:
+	 *
+	 *	A) .....c..t..f.....	Dtf < Dtc	need to wait
+	 *	B) .....c.....f..t..	Dtf > Dtc	expired
+	 *	C) ..t..c.....f.....	Dtf > Dtc	expired	   (Dct very large)
+	 *
+	 *  Any case where f==c: always expired (for any t).	Dtf == Dcf
+	 *  Any case where t==c: always expired (for any f).	Dtf >= Dtc (because Dtc==0)
+	 *  Any case where t==f!=c: always wait.		Dtf <  Dtc (because Dtf==0,
+	 *							Dtc!=0)
+	 *
+	 *  Other cases:
+	 *
+	 *	A) .....t..f..c.....	Dtf < Dtc	need to wait
+	 *	A) .....f..c..t.....	Dtf < Dtc	need to wait
+	 *	A) .....f..t..c.....	Dtf > Dtc	expired
+	 *
+	 *   So:
+	 *	   Dtf >= Dtc implies EXPIRED	(return true)
+	 *	   Dtf <  Dtc implies WAIT	(return false)
+	 *
+	 * Note: If t is expired then we *cannot* wait on it. We would wait
+	 * forever (hang the system).
+	 *
+	 * Note: do NOT get clever and remove the -thresh from both sides. It
+	 * is NOT the same.
+	 *
+	 * If future valueis zero, we have a client managed sync point. In that
+	 * case we do a direct comparison.
+	 */
+	if (!host1x_syncpt_client_managed(sp))
+		return future_val - thresh >= current_val - thresh;
+	else
+		return (s32)(current_val - thresh) >= 0;
+}
+
+/* remove a wait pointed to by patch_addr */
+int host1x_syncpt_patch_wait(struct host1x_syncpt *sp, void *patch_addr)
+{
+	return host1x_hw_syncpt_patch_wait(sp->host, sp, patch_addr);
+}
+
+int host1x_syncpt_init(struct host1x *host)
+{
+	struct host1x_syncpt *syncpt;
+	int i;
+
+	syncpt = devm_kzalloc(host->dev, sizeof(*syncpt) * host->info->nb_pts,
+		GFP_KERNEL);
+	if (!syncpt)
+		return -ENOMEM;
+
+	for (i = 0; i < host->info->nb_pts; ++i) {
+		syncpt[i].id = i;
+		syncpt[i].host = host;
+	}
+
+	host->syncpt = syncpt;
+
+	host1x_syncpt_restore(host);
+
+	/* Allocate sync point to use for clearing waits for expired fences */
+	host->nop_sp = _host1x_syncpt_alloc(host, NULL, 0);
+	if (!host->nop_sp)
+		return -ENOMEM;
+
+	return 0;
+}
+
+struct host1x_syncpt *host1x_syncpt_request(struct device *dev,
+					    int client_managed)
+{
+	struct host1x *host = dev_get_drvdata(dev->parent);
+	return _host1x_syncpt_alloc(host, dev, client_managed);
+}
+
+void host1x_syncpt_free(struct host1x_syncpt *sp)
+{
+	if (!sp)
+		return;
+
+	kfree(sp->name);
+	sp->dev = NULL;
+	sp->name = NULL;
+	sp->client_managed = 0;
+}
+
+void host1x_syncpt_deinit(struct host1x *host)
+{
+	int i;
+	struct host1x_syncpt *sp = host->syncpt;
+	for (i = 0; i < host->info->nb_pts; i++, sp++)
+		kfree(sp->name);
+}
+
+int host1x_syncpt_nb_pts(struct host1x *host)
+{
+	return host->info->nb_pts;
+}
+
+int host1x_syncpt_nb_bases(struct host1x *host)
+{
+	return host->info->nb_bases;
+}
+
+int host1x_syncpt_nb_mlocks(struct host1x *host)
+{
+	return host->info->nb_mlocks;
+}
+
+struct host1x_syncpt *host1x_syncpt_get(struct host1x *host, u32 id)
+{
+	if (host->info->nb_pts < id)
+		return NULL;
+	return host->syncpt + id;
+}
diff --git a/drivers/gpu/host1x/syncpt.h b/drivers/gpu/host1x/syncpt.h
new file mode 100644
index 000000000000..c99806130f2e
--- /dev/null
+++ b/drivers/gpu/host1x/syncpt.h
@@ -0,0 +1,165 @@
+/*
+ * Tegra host1x Syncpoints
+ *
+ * Copyright (c) 2010-2013, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __HOST1X_SYNCPT_H
+#define __HOST1X_SYNCPT_H
+
+#include <linux/atomic.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+
+#include "intr.h"
+
+struct host1x;
+
+/* Reserved for replacing an expired wait with a NOP */
+#define HOST1X_SYNCPT_RESERVED			0
+
+struct host1x_syncpt {
+	int id;
+	atomic_t min_val;
+	atomic_t max_val;
+	u32 base_val;
+	const char *name;
+	int client_managed;
+	struct host1x *host;
+	struct device *dev;
+
+	/* interrupt data */
+	struct host1x_syncpt_intr intr;
+};
+
+/* Initialize sync point array  */
+int host1x_syncpt_init(struct host1x *host);
+
+/*  Free sync point array */
+void host1x_syncpt_deinit(struct host1x *host);
+
+/*
+ * Read max. It indicates how many operations there are in queue, either in
+ * channel or in a software thread.
+ * */
+static inline u32 host1x_syncpt_read_max(struct host1x_syncpt *sp)
+{
+	smp_rmb();
+	return (u32)atomic_read(&sp->max_val);
+}
+
+/*
+ * Read min, which is a shadow of the current sync point value in hardware.
+ */
+static inline u32 host1x_syncpt_read_min(struct host1x_syncpt *sp)
+{
+	smp_rmb();
+	return (u32)atomic_read(&sp->min_val);
+}
+
+/* Return number of sync point supported. */
+int host1x_syncpt_nb_pts(struct host1x *host);
+
+/* Return number of wait bases supported. */
+int host1x_syncpt_nb_bases(struct host1x *host);
+
+/* Return number of mlocks supported. */
+int host1x_syncpt_nb_mlocks(struct host1x *host);
+
+/*
+ * Check sync point sanity. If max is larger than min, there have too many
+ * sync point increments.
+ *
+ * Client managed sync point are not tracked.
+ * */
+static inline bool host1x_syncpt_check_max(struct host1x_syncpt *sp, u32 real)
+{
+	u32 max;
+	if (sp->client_managed)
+		return true;
+	max = host1x_syncpt_read_max(sp);
+	return (s32)(max - real) >= 0;
+}
+
+/* Return true if sync point is client managed. */
+static inline int host1x_syncpt_client_managed(struct host1x_syncpt *sp)
+{
+	return sp->client_managed;
+}
+
+/*
+ * Returns true if syncpoint min == max, which means that there are no
+ * outstanding operations.
+ */
+static inline bool host1x_syncpt_idle(struct host1x_syncpt *sp)
+{
+	int min, max;
+	smp_rmb();
+	min = atomic_read(&sp->min_val);
+	max = atomic_read(&sp->max_val);
+	return (min == max);
+}
+
+/* Return pointer to struct denoting sync point id. */
+struct host1x_syncpt *host1x_syncpt_get(struct host1x *host, u32 id);
+
+/* Request incrementing a sync point. */
+void host1x_syncpt_cpu_incr(struct host1x_syncpt *sp);
+
+/* Load current value from hardware to the shadow register. */
+u32 host1x_syncpt_load(struct host1x_syncpt *sp);
+
+/* Check if the given syncpoint value has already passed */
+bool host1x_syncpt_is_expired(struct host1x_syncpt *sp, u32 thresh);
+
+/* Save host1x sync point state into shadow registers. */
+void host1x_syncpt_save(struct host1x *host);
+
+/* Reset host1x sync point state from shadow registers. */
+void host1x_syncpt_restore(struct host1x *host);
+
+/* Read current wait base value into shadow register and return it. */
+u32 host1x_syncpt_load_wait_base(struct host1x_syncpt *sp);
+
+/* Increment sync point and its max. */
+void host1x_syncpt_incr(struct host1x_syncpt *sp);
+
+/* Indicate future operations by incrementing the sync point max. */
+u32 host1x_syncpt_incr_max(struct host1x_syncpt *sp, u32 incrs);
+
+/* Wait until sync point reaches a threshold value, or a timeout. */
+int host1x_syncpt_wait(struct host1x_syncpt *sp, u32 thresh,
+			long timeout, u32 *value);
+
+/* Check if sync point id is valid. */
+static inline int host1x_syncpt_is_valid(struct host1x_syncpt *sp)
+{
+	return sp->id < host1x_syncpt_nb_pts(sp->host);
+}
+
+/* Patch a wait by replacing it with a wait for syncpt 0 value 0 */
+int host1x_syncpt_patch_wait(struct host1x_syncpt *sp, void *patch_addr);
+
+/* Return id of the sync point */
+u32 host1x_syncpt_id(struct host1x_syncpt *sp);
+
+/* Allocate a sync point for a device. */
+struct host1x_syncpt *host1x_syncpt_request(struct device *dev,
+		int client_managed);
+
+/* Free a sync point. */
+void host1x_syncpt_free(struct host1x_syncpt *sp);
+
+#endif
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index f61d98a62967..0428e8a74b19 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -39,6 +39,19 @@ config HWMON_DEBUG_CHIP
 
 comment "Native drivers"
 
+config SENSORS_AB8500
+	tristate "AB8500 thermal monitoring"
+	depends on AB8500_GPADC && AB8500_BM
+	default n
+	help
+	  If you say yes here you get support for the thermal sensor part
+	  of the AB8500 chip. The driver includes thermal management for
+	  AB8500 die and two GPADC channels. The GPADC channel are preferably
+	  used to access sensors outside the AB8500 chip.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called abx500-temp.
+
 config SENSORS_ABITUGURU
 	tristate "Abit uGuru (rev 1 & 2)"
 	depends on X86 && DMI
@@ -617,7 +630,7 @@ config SENSORS_LM75
 	  temperature sensor chip, with models including:
 
 		- Analog Devices ADT75
-		- Dallas Semiconductor DS75 and DS1775
+		- Dallas Semiconductor DS75, DS1775 and DS7505
 		- Maxim MAX6625 and MAX6626
 		- Microchip MCP980x
 		- National Semiconductor LM75, LM75A
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index c51b0dc35dc8..d17d3e64f9f4 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -19,6 +19,7 @@ obj-$(CONFIG_SENSORS_W83795)	+= w83795.o
 obj-$(CONFIG_SENSORS_W83781D)	+= w83781d.o
 obj-$(CONFIG_SENSORS_W83791D)	+= w83791d.o
 
+obj-$(CONFIG_SENSORS_AB8500)	+= abx500.o ab8500.o
 obj-$(CONFIG_SENSORS_ABITUGURU)	+= abituguru.o
 obj-$(CONFIG_SENSORS_ABITUGURU3)+= abituguru3.o
 obj-$(CONFIG_SENSORS_AD7314)	+= ad7314.o
diff --git a/drivers/hwmon/ab8500.c b/drivers/hwmon/ab8500.c
new file mode 100644
index 000000000000..d844dc806853
--- /dev/null
+++ b/drivers/hwmon/ab8500.c
@@ -0,0 +1,206 @@
+/*
+ * Copyright (C) ST-Ericsson 2010 - 2013
+ * Author: Martin Persson <martin.persson@stericsson.com>
+ *         Hongbo Zhang <hongbo.zhang@linaro.org>
+ * License Terms: GNU General Public License v2
+ *
+ * When the AB8500 thermal warning temperature is reached (threshold cannot
+ * be changed by SW), an interrupt is set, and if no further action is taken
+ * within a certain time frame, pm_power off will be called.
+ *
+ * When AB8500 thermal shutdown temperature is reached a hardware shutdown of
+ * the AB8500 will occur.
+ */
+
+#include <linux/err.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab8500-bm.h>
+#include <linux/mfd/abx500/ab8500-gpadc.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/power/ab8500.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include "abx500.h"
+
+#define DEFAULT_POWER_OFF_DELAY	(HZ * 10)
+#define THERMAL_VCC		1800
+#define PULL_UP_RESISTOR	47000
+/* Number of monitored sensors should not greater than NUM_SENSORS */
+#define NUM_MONITORED_SENSORS	4
+
+struct ab8500_gpadc_cfg {
+	const struct abx500_res_to_temp *temp_tbl;
+	int tbl_sz;
+	int vcc;
+	int r_up;
+};
+
+struct ab8500_temp {
+	struct ab8500_gpadc *gpadc;
+	struct ab8500_btemp *btemp;
+	struct delayed_work power_off_work;
+	struct ab8500_gpadc_cfg cfg;
+	struct abx500_temp *abx500_data;
+};
+
+/*
+ * The hardware connection is like this:
+ * VCC----[ R_up ]-----[ NTC ]----GND
+ * where R_up is pull-up resistance, and GPADC measures voltage on NTC.
+ * and res_to_temp table is strictly sorted by falling resistance values.
+ */
+static int ab8500_voltage_to_temp(struct ab8500_gpadc_cfg *cfg,
+		int v_ntc, int *temp)
+{
+	int r_ntc, i = 0, tbl_sz = cfg->tbl_sz;
+	const struct abx500_res_to_temp *tbl = cfg->temp_tbl;
+
+	if (cfg->vcc < 0 || v_ntc >= cfg->vcc)
+		return -EINVAL;
+
+	r_ntc = v_ntc * cfg->r_up / (cfg->vcc - v_ntc);
+	if (r_ntc > tbl[0].resist || r_ntc < tbl[tbl_sz - 1].resist)
+		return -EINVAL;
+
+	while (!(r_ntc <= tbl[i].resist && r_ntc > tbl[i + 1].resist) &&
+			i < tbl_sz - 2)
+		i++;
+
+	/* return milli-Celsius */
+	*temp = tbl[i].temp * 1000 + ((tbl[i + 1].temp - tbl[i].temp) * 1000 *
+		(r_ntc - tbl[i].resist)) / (tbl[i + 1].resist - tbl[i].resist);
+
+	return 0;
+}
+
+static int ab8500_read_sensor(struct abx500_temp *data, u8 sensor, int *temp)
+{
+	int voltage, ret;
+	struct ab8500_temp *ab8500_data = data->plat_data;
+
+	if (sensor == BAT_CTRL) {
+		*temp = ab8500_btemp_get_batctrl_temp(ab8500_data->btemp);
+	} else if (sensor == BTEMP_BALL) {
+		*temp = ab8500_btemp_get_temp(ab8500_data->btemp);
+	} else {
+		voltage = ab8500_gpadc_convert(ab8500_data->gpadc, sensor);
+		if (voltage < 0)
+			return voltage;
+
+		ret = ab8500_voltage_to_temp(&ab8500_data->cfg, voltage, temp);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static void ab8500_thermal_power_off(struct work_struct *work)
+{
+	struct ab8500_temp *ab8500_data = container_of(work,
+				struct ab8500_temp, power_off_work.work);
+	struct abx500_temp *abx500_data = ab8500_data->abx500_data;
+
+	dev_warn(&abx500_data->pdev->dev, "Power off due to critical temp\n");
+
+	pm_power_off();
+}
+
+static ssize_t ab8500_show_name(struct device *dev,
+		struct device_attribute *devattr, char *buf)
+{
+	return sprintf(buf, "ab8500\n");
+}
+
+static ssize_t ab8500_show_label(struct device *dev,
+		struct device_attribute *devattr, char *buf)
+{
+	char *label;
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int index = attr->index;
+
+	switch (index) {
+	case 1:
+		label = "ext_adc1";
+		break;
+	case 2:
+		label = "ext_adc2";
+		break;
+	case 3:
+		label = "bat_temp";
+		break;
+	case 4:
+		label = "bat_ctrl";
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return sprintf(buf, "%s\n", label);
+}
+
+static int ab8500_temp_irq_handler(int irq, struct abx500_temp *data)
+{
+	struct ab8500_temp *ab8500_data = data->plat_data;
+
+	dev_warn(&data->pdev->dev, "Power off in %d s\n",
+		 DEFAULT_POWER_OFF_DELAY / HZ);
+
+	schedule_delayed_work(&ab8500_data->power_off_work,
+		DEFAULT_POWER_OFF_DELAY);
+	return 0;
+}
+
+int abx500_hwmon_init(struct abx500_temp *data)
+{
+	struct ab8500_temp *ab8500_data;
+
+	ab8500_data = devm_kzalloc(&data->pdev->dev, sizeof(*ab8500_data),
+		GFP_KERNEL);
+	if (!ab8500_data)
+		return -ENOMEM;
+
+	ab8500_data->gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	if (IS_ERR(ab8500_data->gpadc))
+		return PTR_ERR(ab8500_data->gpadc);
+
+	ab8500_data->btemp = ab8500_btemp_get();
+	if (IS_ERR(ab8500_data->btemp))
+		return PTR_ERR(ab8500_data->btemp);
+
+	INIT_DELAYED_WORK(&ab8500_data->power_off_work,
+			  ab8500_thermal_power_off);
+
+	ab8500_data->cfg.vcc = THERMAL_VCC;
+	ab8500_data->cfg.r_up = PULL_UP_RESISTOR;
+	ab8500_data->cfg.temp_tbl = ab8500_temp_tbl_a_thermistor;
+	ab8500_data->cfg.tbl_sz = ab8500_temp_tbl_a_size;
+
+	data->plat_data = ab8500_data;
+
+	/*
+	 * ADC_AUX1 and ADC_AUX2, connected to external NTC
+	 * BTEMP_BALL and BAT_CTRL, fixed usage
+	 */
+	data->gpadc_addr[0] = ADC_AUX1;
+	data->gpadc_addr[1] = ADC_AUX2;
+	data->gpadc_addr[2] = BTEMP_BALL;
+	data->gpadc_addr[3] = BAT_CTRL;
+	data->monitored_sensors = NUM_MONITORED_SENSORS;
+
+	data->ops.read_sensor = ab8500_read_sensor;
+	data->ops.irq_handler = ab8500_temp_irq_handler;
+	data->ops.show_name = ab8500_show_name;
+	data->ops.show_label = ab8500_show_label;
+	data->ops.is_visible = NULL;
+
+	return 0;
+}
+EXPORT_SYMBOL(abx500_hwmon_init);
+
+MODULE_AUTHOR("Hongbo Zhang <hongbo.zhang@linaro.org>");
+MODULE_DESCRIPTION("AB8500 temperature driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/abx500.c b/drivers/hwmon/abx500.c
new file mode 100644
index 000000000000..eee1134274c8
--- /dev/null
+++ b/drivers/hwmon/abx500.c
@@ -0,0 +1,491 @@
+/*
+ * Copyright (C) ST-Ericsson 2010 - 2013
+ * Author: Martin Persson <martin.persson@stericsson.com>
+ *         Hongbo Zhang <hongbo.zhang@linaro.org>
+ * License Terms: GNU General Public License v2
+ *
+ * ABX500 does not provide auto ADC, so to monitor the required temperatures,
+ * a periodic work is used. It is more important to not wake up the CPU than
+ * to perform this job, hence the use of a deferred delay.
+ *
+ * A deferred delay for thermal monitor is considered safe because:
+ * If the chip gets too hot during a sleep state it's most likely due to
+ * external factors, such as the surrounding temperature. I.e. no SW decisions
+ * will make any difference.
+ */
+
+#include <linux/err.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/interrupt.h>
+#include <linux/jiffies.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/workqueue.h>
+#include "abx500.h"
+
+#define DEFAULT_MONITOR_DELAY	HZ
+#define DEFAULT_MAX_TEMP	130
+
+static inline void schedule_monitor(struct abx500_temp *data)
+{
+	data->work_active = true;
+	schedule_delayed_work(&data->work, DEFAULT_MONITOR_DELAY);
+}
+
+static void threshold_updated(struct abx500_temp *data)
+{
+	int i;
+	for (i = 0; i < data->monitored_sensors; i++)
+		if (data->max[i] != 0 || data->min[i] != 0) {
+			schedule_monitor(data);
+			return;
+		}
+
+	dev_dbg(&data->pdev->dev, "No active thresholds.\n");
+	cancel_delayed_work_sync(&data->work);
+	data->work_active = false;
+}
+
+static void gpadc_monitor(struct work_struct *work)
+{
+	int temp, i, ret;
+	char alarm_node[30];
+	bool updated_min_alarm, updated_max_alarm;
+	struct abx500_temp *data;
+
+	data = container_of(work, struct abx500_temp, work.work);
+	mutex_lock(&data->lock);
+
+	for (i = 0; i < data->monitored_sensors; i++) {
+		/* Thresholds are considered inactive if set to 0 */
+		if (data->max[i] == 0 && data->min[i] == 0)
+			continue;
+
+		if (data->max[i] < data->min[i])
+			continue;
+
+		ret = data->ops.read_sensor(data, data->gpadc_addr[i], &temp);
+		if (ret < 0) {
+			dev_err(&data->pdev->dev, "GPADC read failed\n");
+			continue;
+		}
+
+		updated_min_alarm = false;
+		updated_max_alarm = false;
+
+		if (data->min[i] != 0) {
+			if (temp < data->min[i]) {
+				if (data->min_alarm[i] == false) {
+					data->min_alarm[i] = true;
+					updated_min_alarm = true;
+				}
+			} else {
+				if (data->min_alarm[i] == true) {
+					data->min_alarm[i] = false;
+					updated_min_alarm = true;
+				}
+			}
+		}
+		if (data->max[i] != 0) {
+			if (temp > data->max[i]) {
+				if (data->max_alarm[i] == false) {
+					data->max_alarm[i] = true;
+					updated_max_alarm = true;
+				}
+			} else if (temp < data->max[i] - data->max_hyst[i]) {
+				if (data->max_alarm[i] == true) {
+					data->max_alarm[i] = false;
+					updated_max_alarm = true;
+				}
+			}
+		}
+
+		if (updated_min_alarm) {
+			ret = sprintf(alarm_node, "temp%d_min_alarm", i + 1);
+			sysfs_notify(&data->pdev->dev.kobj, NULL, alarm_node);
+		}
+		if (updated_max_alarm) {
+			ret = sprintf(alarm_node, "temp%d_max_alarm", i + 1);
+			sysfs_notify(&data->pdev->dev.kobj, NULL, alarm_node);
+		}
+	}
+
+	schedule_monitor(data);
+	mutex_unlock(&data->lock);
+}
+
+/* HWMON sysfs interfaces */
+static ssize_t show_name(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	/* Show chip name */
+	return data->ops.show_name(dev, devattr, buf);
+}
+
+static ssize_t show_label(struct device *dev,
+			  struct device_attribute *devattr, char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	/* Show each sensor label */
+	return data->ops.show_label(dev, devattr, buf);
+}
+
+static ssize_t show_input(struct device *dev,
+			  struct device_attribute *devattr, char *buf)
+{
+	int ret, temp;
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	u8 gpadc_addr = data->gpadc_addr[attr->index];
+
+	ret = data->ops.read_sensor(data, gpadc_addr, &temp);
+	if (ret < 0)
+		return ret;
+
+	return sprintf(buf, "%d\n", temp);
+}
+
+/* Set functions (RW nodes) */
+static ssize_t set_min(struct device *dev, struct device_attribute *devattr,
+		       const char *buf, size_t count)
+{
+	unsigned long val;
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = kstrtol(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	val = clamp_val(val, 0, DEFAULT_MAX_TEMP);
+
+	mutex_lock(&data->lock);
+	data->min[attr->index] = val;
+	threshold_updated(data);
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t set_max(struct device *dev, struct device_attribute *devattr,
+		       const char *buf, size_t count)
+{
+	unsigned long val;
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = kstrtol(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	val = clamp_val(val, 0, DEFAULT_MAX_TEMP);
+
+	mutex_lock(&data->lock);
+	data->max[attr->index] = val;
+	threshold_updated(data);
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t set_max_hyst(struct device *dev,
+			    struct device_attribute *devattr,
+			    const char *buf, size_t count)
+{
+	unsigned long val;
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = kstrtoul(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	val = clamp_val(val, 0, DEFAULT_MAX_TEMP);
+
+	mutex_lock(&data->lock);
+	data->max_hyst[attr->index] = val;
+	threshold_updated(data);
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+/* Show functions (RO nodes) */
+static ssize_t show_min(struct device *dev,
+			struct device_attribute *devattr, char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+
+	return sprintf(buf, "%ld\n", data->min[attr->index]);
+}
+
+static ssize_t show_max(struct device *dev,
+			struct device_attribute *devattr, char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+
+	return sprintf(buf, "%ld\n", data->max[attr->index]);
+}
+
+static ssize_t show_max_hyst(struct device *dev,
+			     struct device_attribute *devattr, char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+
+	return sprintf(buf, "%ld\n", data->max_hyst[attr->index]);
+}
+
+static ssize_t show_min_alarm(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+
+	return sprintf(buf, "%d\n", data->min_alarm[attr->index]);
+}
+
+static ssize_t show_max_alarm(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+
+	return sprintf(buf, "%d\n", data->max_alarm[attr->index]);
+}
+
+static umode_t abx500_attrs_visible(struct kobject *kobj,
+				   struct attribute *attr, int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct abx500_temp *data = dev_get_drvdata(dev);
+
+	if (data->ops.is_visible)
+		return data->ops.is_visible(attr, n);
+
+	return attr->mode;
+}
+
+/* Chip name, required by hwmon */
+static SENSOR_DEVICE_ATTR(name, S_IRUGO, show_name, NULL, 0);
+
+/* GPADC - SENSOR1 */
+static SENSOR_DEVICE_ATTR(temp1_label, S_IRUGO, show_label, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_input, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO, show_min, set_min, 0);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_max, set_max, 0);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO,
+			  show_max_hyst, set_max_hyst, 0);
+static SENSOR_DEVICE_ATTR(temp1_min_alarm, S_IRUGO, show_min_alarm, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_max_alarm, NULL, 0);
+
+/* GPADC - SENSOR2 */
+static SENSOR_DEVICE_ATTR(temp2_label, S_IRUGO, show_label, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_input, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp2_min, S_IWUSR | S_IRUGO, show_min, set_min, 1);
+static SENSOR_DEVICE_ATTR(temp2_max, S_IWUSR | S_IRUGO, show_max, set_max, 1);
+static SENSOR_DEVICE_ATTR(temp2_max_hyst, S_IWUSR | S_IRUGO,
+			  show_max_hyst, set_max_hyst, 1);
+static SENSOR_DEVICE_ATTR(temp2_min_alarm, S_IRUGO, show_min_alarm, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp2_max_alarm, S_IRUGO, show_max_alarm, NULL, 1);
+
+/* GPADC - SENSOR3 */
+static SENSOR_DEVICE_ATTR(temp3_label, S_IRUGO, show_label, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp3_input, S_IRUGO, show_input, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp3_min, S_IWUSR | S_IRUGO, show_min, set_min, 2);
+static SENSOR_DEVICE_ATTR(temp3_max, S_IWUSR | S_IRUGO, show_max, set_max, 2);
+static SENSOR_DEVICE_ATTR(temp3_max_hyst, S_IWUSR | S_IRUGO,
+			  show_max_hyst, set_max_hyst, 2);
+static SENSOR_DEVICE_ATTR(temp3_min_alarm, S_IRUGO, show_min_alarm, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp3_max_alarm, S_IRUGO, show_max_alarm, NULL, 2);
+
+/* GPADC - SENSOR4 */
+static SENSOR_DEVICE_ATTR(temp4_label, S_IRUGO, show_label, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp4_input, S_IRUGO, show_input, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp4_min, S_IWUSR | S_IRUGO, show_min, set_min, 3);
+static SENSOR_DEVICE_ATTR(temp4_max, S_IWUSR | S_IRUGO, show_max, set_max, 3);
+static SENSOR_DEVICE_ATTR(temp4_max_hyst, S_IWUSR | S_IRUGO,
+			  show_max_hyst, set_max_hyst, 3);
+static SENSOR_DEVICE_ATTR(temp4_min_alarm, S_IRUGO, show_min_alarm, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp4_max_alarm, S_IRUGO, show_max_alarm, NULL, 3);
+
+struct attribute *abx500_temp_attributes[] = {
+	&sensor_dev_attr_name.dev_attr.attr,
+
+	&sensor_dev_attr_temp1_label.dev_attr.attr,
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_min.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
+
+	&sensor_dev_attr_temp2_label.dev_attr.attr,
+	&sensor_dev_attr_temp2_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_min.dev_attr.attr,
+	&sensor_dev_attr_temp2_max.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp2_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_alarm.dev_attr.attr,
+
+	&sensor_dev_attr_temp3_label.dev_attr.attr,
+	&sensor_dev_attr_temp3_input.dev_attr.attr,
+	&sensor_dev_attr_temp3_min.dev_attr.attr,
+	&sensor_dev_attr_temp3_max.dev_attr.attr,
+	&sensor_dev_attr_temp3_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp3_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp3_max_alarm.dev_attr.attr,
+
+	&sensor_dev_attr_temp4_label.dev_attr.attr,
+	&sensor_dev_attr_temp4_input.dev_attr.attr,
+	&sensor_dev_attr_temp4_min.dev_attr.attr,
+	&sensor_dev_attr_temp4_max.dev_attr.attr,
+	&sensor_dev_attr_temp4_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp4_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp4_max_alarm.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group abx500_temp_group = {
+	.attrs = abx500_temp_attributes,
+	.is_visible = abx500_attrs_visible,
+};
+
+static irqreturn_t abx500_temp_irq_handler(int irq, void *irq_data)
+{
+	struct platform_device *pdev = irq_data;
+	struct abx500_temp *data = platform_get_drvdata(pdev);
+
+	data->ops.irq_handler(irq, data);
+	return IRQ_HANDLED;
+}
+
+static int setup_irqs(struct platform_device *pdev)
+{
+	int ret;
+	int irq = platform_get_irq_byname(pdev, "ABX500_TEMP_WARM");
+
+	if (irq < 0) {
+		dev_err(&pdev->dev, "Get irq by name failed\n");
+		return irq;
+	}
+
+	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
+		abx500_temp_irq_handler, IRQF_NO_SUSPEND, "abx500-temp", pdev);
+	if (ret < 0)
+		dev_err(&pdev->dev, "Request threaded irq failed (%d)\n", ret);
+
+	return ret;
+}
+
+static int abx500_temp_probe(struct platform_device *pdev)
+{
+	struct abx500_temp *data;
+	int err;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->pdev = pdev;
+	mutex_init(&data->lock);
+
+	/* Chip specific initialization */
+	err = abx500_hwmon_init(data);
+	if (err	< 0 || !data->ops.read_sensor || !data->ops.show_name ||
+			!data->ops.show_label)
+		return err;
+
+	INIT_DEFERRABLE_WORK(&data->work, gpadc_monitor);
+
+	platform_set_drvdata(pdev, data);
+
+	err = sysfs_create_group(&pdev->dev.kobj, &abx500_temp_group);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Create sysfs group failed (%d)\n", err);
+		return err;
+	}
+
+	data->hwmon_dev = hwmon_device_register(&pdev->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		err = PTR_ERR(data->hwmon_dev);
+		dev_err(&pdev->dev, "Class registration failed (%d)\n", err);
+		goto exit_sysfs_group;
+	}
+
+	if (data->ops.irq_handler) {
+		err = setup_irqs(pdev);
+		if (err < 0)
+			goto exit_hwmon_reg;
+	}
+	return 0;
+
+exit_hwmon_reg:
+	hwmon_device_unregister(data->hwmon_dev);
+exit_sysfs_group:
+	sysfs_remove_group(&pdev->dev.kobj, &abx500_temp_group);
+	return err;
+}
+
+static int abx500_temp_remove(struct platform_device *pdev)
+{
+	struct abx500_temp *data = platform_get_drvdata(pdev);
+
+	cancel_delayed_work_sync(&data->work);
+	hwmon_device_unregister(data->hwmon_dev);
+	sysfs_remove_group(&pdev->dev.kobj, &abx500_temp_group);
+
+	return 0;
+}
+
+static int abx500_temp_suspend(struct platform_device *pdev,
+			       pm_message_t state)
+{
+	struct abx500_temp *data = platform_get_drvdata(pdev);
+
+	if (data->work_active)
+		cancel_delayed_work_sync(&data->work);
+
+	return 0;
+}
+
+static int abx500_temp_resume(struct platform_device *pdev)
+{
+	struct abx500_temp *data = platform_get_drvdata(pdev);
+
+	if (data->work_active)
+		schedule_monitor(data);
+
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id abx500_temp_match[] = {
+	{ .compatible = "stericsson,abx500-temp" },
+	{},
+};
+#endif
+
+static struct platform_driver abx500_temp_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "abx500-temp",
+		.of_match_table = of_match_ptr(abx500_temp_match),
+	},
+	.suspend = abx500_temp_suspend,
+	.resume = abx500_temp_resume,
+	.probe = abx500_temp_probe,
+	.remove = abx500_temp_remove,
+};
+
+module_platform_driver(abx500_temp_driver);
+
+MODULE_AUTHOR("Martin Persson <martin.persson@stericsson.com>");
+MODULE_DESCRIPTION("ABX500 temperature driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/abx500.h b/drivers/hwmon/abx500.h
new file mode 100644
index 000000000000..9b295e684d0e
--- /dev/null
+++ b/drivers/hwmon/abx500.h
@@ -0,0 +1,69 @@
+/*
+ * Copyright (C) ST-Ericsson 2010 - 2013
+ * License terms: GNU General Public License v2
+ * Author: Martin Persson <martin.persson@stericsson.com>
+ *         Hongbo Zhang <hongbo.zhang@linaro.com>
+ */
+
+#ifndef _ABX500_H
+#define _ABX500_H
+
+#define NUM_SENSORS 5
+
+struct abx500_temp;
+
+/*
+ * struct abx500_temp_ops - abx500 chip specific ops
+ * @read_sensor: reads gpadc output
+ * @irq_handler: irq handler
+ * @show_name: hwmon device name
+ * @show_label: hwmon attribute label
+ * @is_visible: is attribute visible
+ */
+struct abx500_temp_ops {
+	int (*read_sensor)(struct abx500_temp *, u8, int *);
+	int (*irq_handler)(int, struct abx500_temp *);
+	ssize_t (*show_name)(struct device *,
+			struct device_attribute *, char *);
+	ssize_t (*show_label) (struct device *,
+			struct device_attribute *, char *);
+	int (*is_visible)(struct attribute *, int);
+};
+
+/*
+ * struct abx500_temp - representation of temp mon device
+ * @pdev: platform device
+ * @hwmon_dev: hwmon device
+ * @ops: abx500 chip specific ops
+ * @gpadc_addr: gpadc channel address
+ * @min: sensor temperature min value
+ * @max: sensor temperature max value
+ * @max_hyst: sensor temperature hysteresis value for max limit
+ * @min_alarm: sensor temperature min alarm
+ * @max_alarm: sensor temperature max alarm
+ * @work: delayed work scheduled to monitor temperature periodically
+ * @work_active: True if work is active
+ * @lock: mutex
+ * @monitored_sensors: number of monitored sensors
+ * @plat_data: private usage, usually points to platform specific data
+ */
+struct abx500_temp {
+	struct platform_device *pdev;
+	struct device *hwmon_dev;
+	struct abx500_temp_ops ops;
+	u8 gpadc_addr[NUM_SENSORS];
+	unsigned long min[NUM_SENSORS];
+	unsigned long max[NUM_SENSORS];
+	unsigned long max_hyst[NUM_SENSORS];
+	bool min_alarm[NUM_SENSORS];
+	bool max_alarm[NUM_SENSORS];
+	struct delayed_work work;
+	bool work_active;
+	struct mutex lock;
+	int monitored_sensors;
+	void *plat_data;
+};
+
+int abx500_hwmon_init(struct abx500_temp *data);
+
+#endif /* _ABX500_H */
diff --git a/drivers/hwmon/lm75.c b/drivers/hwmon/lm75.c
index 291edfff55bf..c03b490bba81 100644
--- a/drivers/hwmon/lm75.c
+++ b/drivers/hwmon/lm75.c
@@ -38,6 +38,7 @@ enum lm75_type {		/* keep sorted in alphabetical order */
 	adt75,
 	ds1775,
 	ds75,
+	ds7505,
 	lm75,
 	lm75a,
 	max6625,
@@ -71,9 +72,12 @@ struct lm75_data {
 	struct device		*hwmon_dev;
 	struct mutex		update_lock;
 	u8			orig_conf;
+	u8			resolution;	/* In bits, between 9 and 12 */
+	u8			resolution_limits;
 	char			valid;		/* !=0 if registers are valid */
 	unsigned long		last_updated;	/* In jiffies */
-	u16			temp[3];	/* Register values,
+	unsigned long		sample_time;	/* In jiffies */
+	s16			temp[3];	/* Register values,
 						   0 = input
 						   1 = max
 						   2 = hyst */
@@ -93,12 +97,15 @@ static ssize_t show_temp(struct device *dev, struct device_attribute *da,
 {
 	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
 	struct lm75_data *data = lm75_update_device(dev);
+	long temp;
 
 	if (IS_ERR(data))
 		return PTR_ERR(data);
 
-	return sprintf(buf, "%d\n",
-		       LM75_TEMP_FROM_REG(data->temp[attr->index]));
+	temp = ((data->temp[attr->index] >> (16 - data->resolution)) * 1000)
+	       >> (data->resolution - 8);
+
+	return sprintf(buf, "%ld\n", temp);
 }
 
 static ssize_t set_temp(struct device *dev, struct device_attribute *da,
@@ -110,13 +117,25 @@ static ssize_t set_temp(struct device *dev, struct device_attribute *da,
 	int nr = attr->index;
 	long temp;
 	int error;
+	u8 resolution;
 
 	error = kstrtol(buf, 10, &temp);
 	if (error)
 		return error;
 
+	/*
+	 * Resolution of limit registers is assumed to be the same as the
+	 * temperature input register resolution unless given explicitly.
+	 */
+	if (attr->index && data->resolution_limits)
+		resolution = data->resolution_limits;
+	else
+		resolution = data->resolution;
+
 	mutex_lock(&data->update_lock);
-	data->temp[nr] = LM75_TEMP_TO_REG(temp);
+	temp = clamp_val(temp, LM75_TEMP_MIN, LM75_TEMP_MAX);
+	data->temp[nr] = DIV_ROUND_CLOSEST(temp  << (resolution - 8),
+					   1000) << (16 - resolution);
 	lm75_write_value(client, LM75_REG_TEMP[nr], data->temp[nr]);
 	mutex_unlock(&data->update_lock);
 	return count;
@@ -151,6 +170,7 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	int status;
 	u8 set_mask, clr_mask;
 	int new;
+	enum lm75_type kind = id->driver_data;
 
 	if (!i2c_check_functionality(client->adapter,
 			I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA))
@@ -167,8 +187,65 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	 * Then tweak to be more precise when appropriate.
 	 */
 	set_mask = 0;
-	clr_mask = (1 << 0)			/* continuous conversions */
-		| (1 << 6) | (1 << 5);		/* 9-bit mode */
+	clr_mask = LM75_SHUTDOWN;		/* continuous conversions */
+
+	switch (kind) {
+	case adt75:
+		clr_mask |= 1 << 5;		/* not one-shot mode */
+		data->resolution = 12;
+		data->sample_time = HZ / 8;
+		break;
+	case ds1775:
+	case ds75:
+	case stds75:
+		clr_mask |= 3 << 5;
+		set_mask |= 2 << 5;		/* 11-bit mode */
+		data->resolution = 11;
+		data->sample_time = HZ;
+		break;
+	case ds7505:
+		set_mask |= 3 << 5;		/* 12-bit mode */
+		data->resolution = 12;
+		data->sample_time = HZ / 4;
+		break;
+	case lm75:
+	case lm75a:
+		data->resolution = 9;
+		data->sample_time = HZ / 2;
+		break;
+	case max6625:
+		data->resolution = 9;
+		data->sample_time = HZ / 4;
+		break;
+	case max6626:
+		data->resolution = 12;
+		data->resolution_limits = 9;
+		data->sample_time = HZ / 4;
+		break;
+	case tcn75:
+		data->resolution = 9;
+		data->sample_time = HZ / 8;
+		break;
+	case mcp980x:
+		data->resolution_limits = 9;
+		/* fall through */
+	case tmp100:
+	case tmp101:
+		set_mask |= 3 << 5;		/* 12-bit mode */
+		data->resolution = 12;
+		data->sample_time = HZ;
+		clr_mask |= 1 << 7;		/* not one-shot mode */
+		break;
+	case tmp105:
+	case tmp175:
+	case tmp275:
+	case tmp75:
+		set_mask |= 3 << 5;		/* 12-bit mode */
+		clr_mask |= 1 << 7;		/* not one-shot mode */
+		data->resolution = 12;
+		data->sample_time = HZ / 2;
+		break;
+	}
 
 	/* configure as specified */
 	status = lm75_read_value(client, LM75_REG_CONF);
@@ -218,6 +295,7 @@ static const struct i2c_device_id lm75_ids[] = {
 	{ "adt75", adt75, },
 	{ "ds1775", ds1775, },
 	{ "ds75", ds75, },
+	{ "ds7505", ds7505, },
 	{ "lm75", lm75, },
 	{ "lm75a", lm75a, },
 	{ "max6625", max6625, },
@@ -407,7 +485,7 @@ static struct lm75_data *lm75_update_device(struct device *dev)
 
 	mutex_lock(&data->update_lock);
 
-	if (time_after(jiffies, data->last_updated + HZ + HZ / 2)
+	if (time_after(jiffies, data->last_updated + data->sample_time)
 	    || !data->valid) {
 		int i;
 		dev_dbg(&client->dev, "Starting lm75 update\n");
diff --git a/drivers/hwspinlock/Kconfig b/drivers/hwspinlock/Kconfig
index c7c3128393d1..70637d23b1f9 100644
--- a/drivers/hwspinlock/Kconfig
+++ b/drivers/hwspinlock/Kconfig
@@ -10,7 +10,7 @@ menu "Hardware Spinlock drivers"
 
 config HWSPINLOCK_OMAP
 	tristate "OMAP Hardware Spinlock device"
-	depends on ARCH_OMAP4
+	depends on ARCH_OMAP4 || SOC_OMAP5
 	select HWSPINLOCK
 	help
 	  Say y here to support the OMAP Hardware Spinlock device (firstly
diff --git a/drivers/i2c/busses/Kconfig b/drivers/i2c/busses/Kconfig
index adfee98486b1..631736e2e7ed 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -363,7 +363,7 @@ config I2C_BLACKFIN_TWI_CLK_KHZ
 
 config I2C_CBUS_GPIO
 	tristate "CBUS I2C driver"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  Support for CBUS access using I2C API. Mostly relevant for Nokia
 	  Internet Tablets (770, N800 and N810).
@@ -436,7 +436,7 @@ config I2C_EG20T
 
 config I2C_GPIO
 	tristate "GPIO-based bitbanging I2C"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select I2C_ALGOBIT
 	help
 	  This is a very simple bitbanging I2C driver utilizing the
diff --git a/drivers/i2c/busses/i2c-amd756-s4882.c b/drivers/i2c/busses/i2c-amd756-s4882.c
index 378fcb5d5783..07f01ac853ff 100644
--- a/drivers/i2c/busses/i2c-amd756-s4882.c
+++ b/drivers/i2c/busses/i2c-amd756-s4882.c
@@ -169,11 +169,7 @@ static int __init amd756_s4882_init(void)
 	}
 
 	/* Unregister physical bus */
-	error = i2c_del_adapter(&amd756_smbus);
-	if (error) {
-		dev_err(&amd756_smbus.dev, "Physical bus removal failed\n");
-		goto ERROR0;
-	}
+	i2c_del_adapter(&amd756_smbus);
 
 	printk(KERN_INFO "Enabling SMBus multiplexing for Tyan S4882\n");
 	/* Define the 5 virtual adapters and algorithms structures */
diff --git a/drivers/i2c/busses/i2c-at91.c b/drivers/i2c/busses/i2c-at91.c
index 75195e3f5ddb..6bb839b688be 100644
--- a/drivers/i2c/busses/i2c-at91.c
+++ b/drivers/i2c/busses/i2c-at91.c
@@ -603,15 +603,18 @@ static const struct of_device_id atmel_twi_dt_ids[] = {
 	}
 };
 MODULE_DEVICE_TABLE(of, atmel_twi_dt_ids);
-#else
-#define atmel_twi_dt_ids NULL
 #endif
 
-static bool filter(struct dma_chan *chan, void *slave)
+static bool filter(struct dma_chan *chan, void *pdata)
 {
-	struct at_dma_slave *sl = slave;
+	struct at91_twi_pdata *sl_pdata = pdata;
+	struct at_dma_slave *sl;
+
+	if (!sl_pdata)
+		return false;
 
-	if (sl->dma_dev == chan->device->dev) {
+	sl = &sl_pdata->dma_slave;
+	if (sl && (sl->dma_dev == chan->device->dev)) {
 		chan->private = sl;
 		return true;
 	} else {
@@ -622,11 +625,10 @@ static bool filter(struct dma_chan *chan, void *slave)
 static int at91_twi_configure_dma(struct at91_twi_dev *dev, u32 phy_addr)
 {
 	int ret = 0;
-	struct at_dma_slave *sdata;
+	struct at91_twi_pdata *pdata = dev->pdata;
 	struct dma_slave_config slave_config;
 	struct at91_twi_dma *dma = &dev->dma;
-
-	sdata = &dev->pdata->dma_slave;
+	dma_cap_mask_t mask;
 
 	memset(&slave_config, 0, sizeof(slave_config));
 	slave_config.src_addr = (dma_addr_t)phy_addr + AT91_TWI_RHR;
@@ -637,25 +639,22 @@ static int at91_twi_configure_dma(struct at91_twi_dev *dev, u32 phy_addr)
 	slave_config.dst_maxburst = 1;
 	slave_config.device_fc = false;
 
-	if (sdata && sdata->dma_dev) {
-		dma_cap_mask_t mask;
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
 
-		dma_cap_zero(mask);
-		dma_cap_set(DMA_SLAVE, mask);
-		dma->chan_tx = dma_request_channel(mask, filter, sdata);
-		if (!dma->chan_tx) {
-			dev_err(dev->dev, "no DMA channel available for tx\n");
-			ret = -EBUSY;
-			goto error;
-		}
-		dma->chan_rx = dma_request_channel(mask, filter, sdata);
-		if (!dma->chan_rx) {
-			dev_err(dev->dev, "no DMA channel available for rx\n");
-			ret = -EBUSY;
-			goto error;
-		}
-	} else {
-		ret = -EINVAL;
+	dma->chan_tx = dma_request_slave_channel_compat(mask, filter, pdata,
+							dev->dev, "tx");
+	if (!dma->chan_tx) {
+		dev_err(dev->dev, "can't get a DMA channel for tx\n");
+		ret = -EBUSY;
+		goto error;
+	}
+
+	dma->chan_rx = dma_request_slave_channel_compat(mask, filter, pdata,
+							dev->dev, "rx");
+	if (!dma->chan_rx) {
+		dev_err(dev->dev, "can't get a DMA channel for rx\n");
+		ret = -EBUSY;
 		goto error;
 	}
 
@@ -785,12 +784,11 @@ static int at91_twi_probe(struct platform_device *pdev)
 static int at91_twi_remove(struct platform_device *pdev)
 {
 	struct at91_twi_dev *dev = platform_get_drvdata(pdev);
-	int rc;
 
-	rc = i2c_del_adapter(&dev->adapter);
+	i2c_del_adapter(&dev->adapter);
 	clk_disable_unprepare(dev->clk);
 
-	return rc;
+	return 0;
 }
 
 #ifdef CONFIG_PM
@@ -828,7 +826,7 @@ static struct platform_driver at91_twi_driver = {
 	.driver		= {
 		.name	= "at91_i2c",
 		.owner	= THIS_MODULE,
-		.of_match_table = atmel_twi_dt_ids,
+		.of_match_table = of_match_ptr(atmel_twi_dt_ids),
 		.pm	= at91_twi_pm_ops,
 	},
 };
diff --git a/drivers/i2c/busses/i2c-cbus-gpio.c b/drivers/i2c/busses/i2c-cbus-gpio.c
index 98386d659318..1be13ac11dc5 100644
--- a/drivers/i2c/busses/i2c-cbus-gpio.c
+++ b/drivers/i2c/busses/i2c-cbus-gpio.c
@@ -206,7 +206,9 @@ static int cbus_i2c_remove(struct platform_device *pdev)
 {
 	struct i2c_adapter *adapter = platform_get_drvdata(pdev);
 
-	return i2c_del_adapter(adapter);
+	i2c_del_adapter(adapter);
+
+	return 0;
 }
 
 static int cbus_i2c_probe(struct platform_device *pdev)
diff --git a/drivers/i2c/busses/i2c-davinci.c b/drivers/i2c/busses/i2c-davinci.c
index 7d1e590a7bb6..cf20e06a88e1 100644
--- a/drivers/i2c/busses/i2c-davinci.c
+++ b/drivers/i2c/busses/i2c-davinci.c
@@ -137,7 +137,7 @@ static inline u16 davinci_i2c_read_reg(struct davinci_i2c_dev *i2c_dev, int reg)
 }
 
 /* Generate a pulse on the i2c clock pin. */
-static void generic_i2c_clock_pulse(unsigned int scl_pin)
+static void davinci_i2c_clock_pulse(unsigned int scl_pin)
 {
 	u16 i;
 
@@ -155,7 +155,7 @@ static void generic_i2c_clock_pulse(unsigned int scl_pin)
 /* This routine does i2c bus recovery as specified in the
  * i2c protocol Rev. 03 section 3.16 titled "Bus clear"
  */
-static void i2c_recover_bus(struct davinci_i2c_dev *dev)
+static void davinci_i2c_recover_bus(struct davinci_i2c_dev *dev)
 {
 	u32 flag = 0;
 	struct davinci_i2c_platform_data *pdata = dev->pdata;
@@ -166,7 +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);
-	generic_i2c_clock_pulse(pdata->scl_pin);
+	davinci_i2c_clock_pulse(pdata->scl_pin);
 	/* Send STOP */
 	flag = davinci_i2c_read_reg(dev, DAVINCI_I2C_MDR_REG);
 	flag |= DAVINCI_I2C_MDR_STP;
@@ -289,7 +289,7 @@ static int i2c_davinci_wait_bus_not_busy(struct davinci_i2c_dev *dev,
 				return -ETIMEDOUT;
 			} else {
 				to_cnt = 0;
-				i2c_recover_bus(dev);
+				davinci_i2c_recover_bus(dev);
 				i2c_davinci_init(dev);
 			}
 		}
@@ -379,7 +379,7 @@ i2c_davinci_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg, int stop)
 						      dev->adapter.timeout);
 	if (r == 0) {
 		dev_err(dev->dev, "controller timed out\n");
-		i2c_recover_bus(dev);
+		davinci_i2c_recover_bus(dev);
 		i2c_davinci_init(dev);
 		dev->buf_len = 0;
 		return -ETIMEDOUT;
@@ -643,7 +643,7 @@ static int davinci_i2c_probe(struct platform_device *pdev)
 {
 	struct davinci_i2c_dev *dev;
 	struct i2c_adapter *adap;
-	struct resource *mem, *irq, *ioarea;
+	struct resource *mem, *irq;
 	int r;
 
 	/* NOTE: driver uses the static register mapping */
@@ -659,24 +659,18 @@ static int davinci_i2c_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	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 = kzalloc(sizeof(struct davinci_i2c_dev), GFP_KERNEL);
+	dev = devm_kzalloc(&pdev->dev, sizeof(struct davinci_i2c_dev),
+			GFP_KERNEL);
 	if (!dev) {
-		r = -ENOMEM;
-		goto err_release_region;
+		dev_err(&pdev->dev, "Memory allocation failed\n");
+		return -ENOMEM;
 	}
 
 	init_completion(&dev->cmd_complete);
 #ifdef CONFIG_CPU_FREQ
 	init_completion(&dev->xfr_complete);
 #endif
-	dev->dev = get_device(&pdev->dev);
+	dev->dev = &pdev->dev;
 	dev->irq = irq->start;
 	dev->pdata = dev->dev->platform_data;
 	platform_set_drvdata(pdev, dev);
@@ -686,10 +680,9 @@ static int davinci_i2c_probe(struct platform_device *pdev)
 
 		dev->pdata = devm_kzalloc(&pdev->dev,
 			sizeof(struct davinci_i2c_platform_data), GFP_KERNEL);
-		if (!dev->pdata) {
-			r = -ENOMEM;
-			goto err_free_mem;
-		}
+		if (!dev->pdata)
+			return -ENOMEM;
+
 		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",
@@ -699,22 +692,21 @@ static int davinci_i2c_probe(struct platform_device *pdev)
 		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;
-	}
+	dev->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(dev->clk))
+		return -ENODEV;
 	clk_prepare_enable(dev->clk);
 
-	dev->base = ioremap(mem->start, resource_size(mem));
-	if (!dev->base) {
-		r = -EBUSY;
-		goto err_mem_ioremap;
+	dev->base = devm_ioremap_resource(&pdev->dev, mem);
+	if (IS_ERR(dev->base)) {
+		r = PTR_ERR(dev->base);
+		goto err_unuse_clocks;
 	}
 
 	i2c_davinci_init(dev);
 
-	r = request_irq(dev->irq, i2c_davinci_isr, 0, pdev->name, dev);
+	r = devm_request_irq(&pdev->dev, dev->irq, i2c_davinci_isr, 0,
+			pdev->name, dev);
 	if (r) {
 		dev_err(&pdev->dev, "failure requesting irq %i\n", dev->irq);
 		goto err_unuse_clocks;
@@ -723,7 +715,7 @@ static int davinci_i2c_probe(struct platform_device *pdev)
 	r = i2c_davinci_cpufreq_register(dev);
 	if (r) {
 		dev_err(&pdev->dev, "failed to register cpufreq\n");
-		goto err_free_irq;
+		goto err_unuse_clocks;
 	}
 
 	adap = &dev->adapter;
@@ -740,50 +732,31 @@ static int davinci_i2c_probe(struct platform_device *pdev)
 	r = i2c_add_numbered_adapter(adap);
 	if (r) {
 		dev_err(&pdev->dev, "failure adding adapter\n");
-		goto err_free_irq;
+		goto err_unuse_clocks;
 	}
 	of_i2c_register_devices(adap);
 
 	return 0;
 
-err_free_irq:
-	free_irq(dev->irq, dev);
 err_unuse_clocks:
-	iounmap(dev->base);
-err_mem_ioremap:
 	clk_disable_unprepare(dev->clk);
-	clk_put(dev->clk);
 	dev->clk = NULL;
-err_free_mem:
-	put_device(&pdev->dev);
-	kfree(dev);
-err_release_region:
-	release_mem_region(mem->start, resource_size(mem));
-
 	return r;
 }
 
 static int davinci_i2c_remove(struct platform_device *pdev)
 {
 	struct davinci_i2c_dev *dev = platform_get_drvdata(pdev);
-	struct resource *mem;
 
 	i2c_davinci_cpufreq_deregister(dev);
 
 	i2c_del_adapter(&dev->adapter);
-	put_device(&pdev->dev);
 
 	clk_disable_unprepare(dev->clk);
-	clk_put(dev->clk);
 	dev->clk = NULL;
 
 	davinci_i2c_write_reg(dev, DAVINCI_I2C_MDR_REG, 0);
-	free_irq(dev->irq, 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-designware-core.c b/drivers/i2c/busses/i2c-designware-core.c
index 94fd81875409..21fbb340ad66 100644
--- a/drivers/i2c/busses/i2c-designware-core.c
+++ b/drivers/i2c/busses/i2c-designware-core.c
@@ -68,6 +68,7 @@
 #define DW_IC_TXFLR		0x74
 #define DW_IC_RXFLR		0x78
 #define DW_IC_TX_ABRT_SOURCE	0x80
+#define DW_IC_ENABLE_STATUS	0x9c
 #define DW_IC_COMP_PARAM_1	0xf4
 #define DW_IC_COMP_TYPE		0xfc
 #define DW_IC_COMP_TYPE_VALUE	0x44570140
@@ -248,6 +249,27 @@ static u32 i2c_dw_scl_lcnt(u32 ic_clk, u32 tLOW, u32 tf, int offset)
 	return ((ic_clk * (tLOW + tf) + 5000) / 10000) - 1 + offset;
 }
 
+static void __i2c_dw_enable(struct dw_i2c_dev *dev, bool enable)
+{
+	int timeout = 100;
+
+	do {
+		dw_writel(dev, enable, DW_IC_ENABLE);
+		if ((dw_readl(dev, DW_IC_ENABLE_STATUS) & 1) == enable)
+			return;
+
+		/*
+		 * Wait 10 times the signaling period of the highest I2C
+		 * transfer supported by the driver (for 400KHz this is
+		 * 25us) as described in the DesignWare I2C databook.
+		 */
+		usleep_range(25, 250);
+	} while (timeout--);
+
+	dev_warn(dev->dev, "timeout in %sabling adapter\n",
+		 enable ? "en" : "dis");
+}
+
 /**
  * i2c_dw_init() - initialize the designware i2c master hardware
  * @dev: device private data
@@ -278,7 +300,7 @@ int i2c_dw_init(struct dw_i2c_dev *dev)
 	}
 
 	/* Disable the adapter */
-	dw_writel(dev, 0, DW_IC_ENABLE);
+	__i2c_dw_enable(dev, false);
 
 	/* set standard and fast speed deviders for high/low periods */
 
@@ -333,7 +355,7 @@ static int i2c_dw_wait_bus_not_busy(struct dw_i2c_dev *dev)
 			return -ETIMEDOUT;
 		}
 		timeout--;
-		mdelay(1);
+		usleep_range(1000, 1100);
 	}
 
 	return 0;
@@ -345,7 +367,7 @@ static void i2c_dw_xfer_init(struct dw_i2c_dev *dev)
 	u32 ic_con;
 
 	/* Disable the adapter */
-	dw_writel(dev, 0, DW_IC_ENABLE);
+	__i2c_dw_enable(dev, false);
 
 	/* set the slave (target) address */
 	dw_writel(dev, msgs[dev->msg_write_idx].addr, DW_IC_TAR);
@@ -359,7 +381,7 @@ static void i2c_dw_xfer_init(struct dw_i2c_dev *dev)
 	dw_writel(dev, ic_con, DW_IC_CON);
 
 	/* Enable the adapter */
-	dw_writel(dev, 1, DW_IC_ENABLE);
+	__i2c_dw_enable(dev, true);
 
 	/* Enable interrupts */
 	dw_writel(dev, DW_IC_INTR_DEFAULT_MASK, DW_IC_INTR_MASK);
@@ -565,7 +587,7 @@ i2c_dw_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
 	/* no error */
 	if (likely(!dev->cmd_err)) {
 		/* Disable the adapter */
-		dw_writel(dev, 0, DW_IC_ENABLE);
+		__i2c_dw_enable(dev, false);
 		ret = num;
 		goto done;
 	}
@@ -578,7 +600,8 @@ i2c_dw_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
 	ret = -EIO;
 
 done:
-	pm_runtime_put(dev->dev);
+	pm_runtime_mark_last_busy(dev->dev);
+	pm_runtime_put_autosuspend(dev->dev);
 	mutex_unlock(&dev->lock);
 
 	return ret;
@@ -700,7 +723,7 @@ EXPORT_SYMBOL_GPL(i2c_dw_isr);
 void i2c_dw_enable(struct dw_i2c_dev *dev)
 {
        /* Enable the adapter */
-	dw_writel(dev, 1, DW_IC_ENABLE);
+	__i2c_dw_enable(dev, true);
 }
 EXPORT_SYMBOL_GPL(i2c_dw_enable);
 
@@ -713,7 +736,7 @@ EXPORT_SYMBOL_GPL(i2c_dw_is_enabled);
 void i2c_dw_disable(struct dw_i2c_dev *dev)
 {
 	/* Disable controller */
-	dw_writel(dev, 0, DW_IC_ENABLE);
+	__i2c_dw_enable(dev, false);
 
 	/* Disable all interupts */
 	dw_writel(dev, 0, DW_IC_INTR_MASK);
diff --git a/drivers/i2c/busses/i2c-designware-pcidrv.c b/drivers/i2c/busses/i2c-designware-pcidrv.c
index 7c5e383c350d..f6ed06c966ee 100644
--- a/drivers/i2c/busses/i2c-designware-pcidrv.c
+++ b/drivers/i2c/busses/i2c-designware-pcidrv.c
@@ -208,68 +208,45 @@ static u32 i2c_dw_get_clk_rate_khz(struct dw_i2c_dev *dev)
 }
 
 static int i2c_dw_pci_probe(struct pci_dev *pdev,
-const struct pci_device_id *id)
+			    const struct pci_device_id *id)
 {
 	struct dw_i2c_dev *dev;
 	struct i2c_adapter *adap;
-	unsigned long start, len;
-	void __iomem *base;
 	int r;
 	struct  dw_pci_controller *controller;
 
 	if (id->driver_data >= ARRAY_SIZE(dw_pci_controllers)) {
-		printk(KERN_ERR "dw_i2c_pci_probe: invalid driver data %ld\n",
+		dev_err(&pdev->dev, "%s: invalid driver data %ld\n", __func__,
 			id->driver_data);
 		return -EINVAL;
 	}
 
 	controller = &dw_pci_controllers[id->driver_data];
 
-	r = pci_enable_device(pdev);
+	r = pcim_enable_device(pdev);
 	if (r) {
 		dev_err(&pdev->dev, "Failed to enable I2C PCI device (%d)\n",
 			r);
-		goto exit;
+		return r;
 	}
 
-	/* Determine the address of the I2C area */
-	start = pci_resource_start(pdev, 0);
-	len = pci_resource_len(pdev, 0);
-	if (!start || len == 0) {
-		dev_err(&pdev->dev, "base address not set\n");
-		r = -ENODEV;
-		goto exit;
-	}
-
-	r = pci_request_region(pdev, 0, DRIVER_NAME);
+	r = pcim_iomap_regions(pdev, 1 << 0, pci_name(pdev));
 	if (r) {
-		dev_err(&pdev->dev, "failed to request I2C region "
-			"0x%lx-0x%lx\n", start,
-			(unsigned long)pci_resource_end(pdev, 0));
-		goto exit;
-	}
-
-	base = ioremap_nocache(start, len);
-	if (!base) {
 		dev_err(&pdev->dev, "I/O memory remapping failed\n");
-		r = -ENOMEM;
-		goto err_release_region;
+		return r;
 	}
 
-
-	dev = kzalloc(sizeof(struct dw_i2c_dev), GFP_KERNEL);
-	if (!dev) {
-		r = -ENOMEM;
-		goto err_release_region;
-	}
+	dev = devm_kzalloc(&pdev->dev, sizeof(struct dw_i2c_dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
 
 	init_completion(&dev->cmd_complete);
 	mutex_init(&dev->lock);
 	dev->clk = NULL;
 	dev->controller = controller;
 	dev->get_clk_rate_khz = i2c_dw_get_clk_rate_khz;
-	dev->base = base;
-	dev->dev = get_device(&pdev->dev);
+	dev->base = pcim_iomap_table(pdev)[0];
+	dev->dev = &pdev->dev;
 	dev->functionality =
 		I2C_FUNC_I2C |
 		I2C_FUNC_SMBUS_BYTE |
@@ -284,7 +261,7 @@ const struct pci_device_id *id)
 	dev->rx_fifo_depth = controller->rx_fifo_depth;
 	r = i2c_dw_init(dev);
 	if (r)
-		goto err_iounmap;
+		return r;
 
 	adap = &dev->adapter;
 	i2c_set_adapdata(adap, dev);
@@ -296,10 +273,11 @@ const struct pci_device_id *id)
 	snprintf(adap->name, sizeof(adap->name), "i2c-designware-pci-%d",
 		adap->nr);
 
-	r = request_irq(pdev->irq, i2c_dw_isr, IRQF_SHARED, adap->name, dev);
+	r = devm_request_irq(&pdev->dev, pdev->irq, i2c_dw_isr, IRQF_SHARED,
+			adap->name, dev);
 	if (r) {
 		dev_err(&pdev->dev, "failure requesting irq %i\n", dev->irq);
-		goto err_iounmap;
+		return r;
 	}
 
 	i2c_dw_disable_int(dev);
@@ -307,24 +285,14 @@ const struct pci_device_id *id)
 	r = i2c_add_numbered_adapter(adap);
 	if (r) {
 		dev_err(&pdev->dev, "failure adding adapter\n");
-		goto err_free_irq;
+		return r;
 	}
 
-	pm_runtime_put_noidle(&pdev->dev);
+	pm_runtime_set_autosuspend_delay(&pdev->dev, 1000);
+	pm_runtime_use_autosuspend(&pdev->dev);
 	pm_runtime_allow(&pdev->dev);
 
 	return 0;
-
-err_free_irq:
-	free_irq(pdev->irq, dev);
-err_iounmap:
-	iounmap(dev->base);
-	put_device(&pdev->dev);
-	kfree(dev);
-err_release_region:
-	pci_release_region(pdev, 0);
-exit:
-	return r;
 }
 
 static void i2c_dw_pci_remove(struct pci_dev *pdev)
@@ -336,11 +304,6 @@ static void i2c_dw_pci_remove(struct pci_dev *pdev)
 	pm_runtime_get_noresume(&pdev->dev);
 
 	i2c_del_adapter(&dev->adapter);
-	put_device(&pdev->dev);
-
-	free_irq(dev->irq, dev);
-	kfree(dev);
-	pci_release_region(pdev, 0);
 }
 
 /* work with hotplug and coldplug */
diff --git a/drivers/i2c/busses/i2c-designware-platdrv.c b/drivers/i2c/busses/i2c-designware-platdrv.c
index e3085c487ace..8ec91335d95a 100644
--- a/drivers/i2c/busses/i2c-designware-platdrv.c
+++ b/drivers/i2c/busses/i2c-designware-platdrv.c
@@ -56,20 +56,11 @@ static u32 i2c_dw_get_clk_rate_khz(struct dw_i2c_dev *dev)
 static int dw_i2c_acpi_configure(struct platform_device *pdev)
 {
 	struct dw_i2c_dev *dev = platform_get_drvdata(pdev);
-	struct acpi_device *adev;
-	int busno, ret;
 
 	if (!ACPI_HANDLE(&pdev->dev))
 		return -ENODEV;
 
-	ret = acpi_bus_get_device(ACPI_HANDLE(&pdev->dev), &adev);
-	if (ret)
-		return -ENODEV;
-
 	dev->adapter.nr = -1;
-	if (adev->pnp.unique_id && !kstrtoint(adev->pnp.unique_id, 0, &busno))
-		dev->adapter.nr = busno;
-
 	dev->tx_fifo_depth = 32;
 	dev->rx_fifo_depth = 32;
 	return 0;
@@ -92,7 +83,7 @@ static int dw_i2c_probe(struct platform_device *pdev)
 {
 	struct dw_i2c_dev *dev;
 	struct i2c_adapter *adap;
-	struct resource *mem, *ioarea;
+	struct resource *mem;
 	int irq, r;
 
 	/* NOTE: driver uses the static register mapping */
@@ -108,32 +99,25 @@ static int dw_i2c_probe(struct platform_device *pdev)
 		return irq; /* -ENXIO */
 	}
 
-	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 dw_i2c_dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
 
-	dev = kzalloc(sizeof(struct dw_i2c_dev), GFP_KERNEL);
-	if (!dev) {
-		r = -ENOMEM;
-		goto err_release_region;
-	}
+	dev->base = devm_ioremap_resource(&pdev->dev, mem);
+	if (IS_ERR(dev->base))
+		return PTR_ERR(dev->base);
 
 	init_completion(&dev->cmd_complete);
 	mutex_init(&dev->lock);
-	dev->dev = get_device(&pdev->dev);
+	dev->dev = &pdev->dev;
 	dev->irq = irq;
 	platform_set_drvdata(pdev, dev);
 
-	dev->clk = clk_get(&pdev->dev, NULL);
+	dev->clk = devm_clk_get(&pdev->dev, NULL);
 	dev->get_clk_rate_khz = i2c_dw_get_clk_rate_khz;
 
-	if (IS_ERR(dev->clk)) {
-		r = -ENODEV;
-		goto err_free_mem;
-	}
+	if (IS_ERR(dev->clk))
+		return PTR_ERR(dev->clk);
 	clk_prepare_enable(dev->clk);
 
 	dev->functionality =
@@ -146,13 +130,6 @@ static int dw_i2c_probe(struct platform_device *pdev)
 	dev->master_cfg =  DW_IC_CON_MASTER | DW_IC_CON_SLAVE_DISABLE |
 		DW_IC_CON_RESTART_EN | DW_IC_CON_SPEED_FAST;
 
-	dev->base = ioremap(mem->start, resource_size(mem));
-	if (dev->base == NULL) {
-		dev_err(&pdev->dev, "failure mapping io resources\n");
-		r = -EBUSY;
-		goto err_unuse_clocks;
-	}
-
 	/* Try first if we can configure the device from ACPI */
 	r = dw_i2c_acpi_configure(pdev);
 	if (r) {
@@ -164,13 +141,14 @@ static int dw_i2c_probe(struct platform_device *pdev)
 	}
 	r = i2c_dw_init(dev);
 	if (r)
-		goto err_iounmap;
+		return r;
 
 	i2c_dw_disable_int(dev);
-	r = request_irq(dev->irq, i2c_dw_isr, IRQF_SHARED, pdev->name, dev);
+	r = devm_request_irq(&pdev->dev, dev->irq, i2c_dw_isr, IRQF_SHARED,
+			pdev->name, dev);
 	if (r) {
 		dev_err(&pdev->dev, "failure requesting irq %i\n", dev->irq);
-		goto err_iounmap;
+		return r;
 	}
 
 	adap = &dev->adapter;
@@ -186,57 +164,32 @@ static int dw_i2c_probe(struct platform_device *pdev)
 	r = i2c_add_numbered_adapter(adap);
 	if (r) {
 		dev_err(&pdev->dev, "failure adding adapter\n");
-		goto err_free_irq;
+		return r;
 	}
 	of_i2c_register_devices(adap);
 	acpi_i2c_register_devices(adap);
 
+	pm_runtime_set_autosuspend_delay(&pdev->dev, 1000);
+	pm_runtime_use_autosuspend(&pdev->dev);
 	pm_runtime_set_active(&pdev->dev);
 	pm_runtime_enable(&pdev->dev);
-	pm_runtime_put(&pdev->dev);
 
 	return 0;
-
-err_free_irq:
-	free_irq(dev->irq, dev);
-err_iounmap:
-	iounmap(dev->base);
-err_unuse_clocks:
-	clk_disable_unprepare(dev->clk);
-	clk_put(dev->clk);
-	dev->clk = NULL;
-err_free_mem:
-	put_device(&pdev->dev);
-	kfree(dev);
-err_release_region:
-	release_mem_region(mem->start, resource_size(mem));
-
-	return r;
 }
 
 static int dw_i2c_remove(struct platform_device *pdev)
 {
 	struct dw_i2c_dev *dev = platform_get_drvdata(pdev);
-	struct resource *mem;
 
 	pm_runtime_get_sync(&pdev->dev);
 
 	i2c_del_adapter(&dev->adapter);
-	put_device(&pdev->dev);
-
-	clk_disable_unprepare(dev->clk);
-	clk_put(dev->clk);
-	dev->clk = NULL;
 
 	i2c_dw_disable(dev);
-	free_irq(dev->irq, dev);
-	kfree(dev);
 
 	pm_runtime_put(&pdev->dev);
 	pm_runtime_disable(&pdev->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-gpio.c b/drivers/i2c/busses/i2c-gpio.c
index f3fa4332bbdf..bc6e139c6e7f 100644
--- a/drivers/i2c/busses/i2c-gpio.c
+++ b/drivers/i2c/busses/i2c-gpio.c
@@ -85,23 +85,29 @@ static int i2c_gpio_getscl(void *data)
 	return gpio_get_value(pdata->scl_pin);
 }
 
-static int of_i2c_gpio_probe(struct device_node *np,
-			     struct i2c_gpio_platform_data *pdata)
+static int of_i2c_gpio_get_pins(struct device_node *np,
+				unsigned int *sda_pin, unsigned int *scl_pin)
 {
-	u32 reg;
-
 	if (of_gpio_count(np) < 2)
 		return -ENODEV;
 
-	pdata->sda_pin = of_get_gpio(np, 0);
-	pdata->scl_pin = of_get_gpio(np, 1);
+	*sda_pin = of_get_gpio(np, 0);
+	*scl_pin = of_get_gpio(np, 1);
 
-	if (!gpio_is_valid(pdata->sda_pin) || !gpio_is_valid(pdata->scl_pin)) {
+	if (!gpio_is_valid(*sda_pin) || !gpio_is_valid(*scl_pin)) {
 		pr_err("%s: invalid GPIO pins, sda=%d/scl=%d\n",
-		       np->full_name, pdata->sda_pin, pdata->scl_pin);
+		       np->full_name, *sda_pin, *scl_pin);
 		return -ENODEV;
 	}
 
+	return 0;
+}
+
+static void of_i2c_gpio_get_props(struct device_node *np,
+				  struct i2c_gpio_platform_data *pdata)
+{
+	u32 reg;
+
 	of_property_read_u32(np, "i2c-gpio,delay-us", &pdata->udelay);
 
 	if (!of_property_read_u32(np, "i2c-gpio,timeout-ms", &reg))
@@ -113,8 +119,6 @@ static int of_i2c_gpio_probe(struct device_node *np,
 		of_property_read_bool(np, "i2c-gpio,scl-open-drain");
 	pdata->scl_is_output_only =
 		of_property_read_bool(np, "i2c-gpio,scl-output-only");
-
-	return 0;
 }
 
 static int i2c_gpio_probe(struct platform_device *pdev)
@@ -123,31 +127,52 @@ static int i2c_gpio_probe(struct platform_device *pdev)
 	struct i2c_gpio_platform_data *pdata;
 	struct i2c_algo_bit_data *bit_data;
 	struct i2c_adapter *adap;
+	unsigned int sda_pin, scl_pin;
 	int ret;
 
-	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
-	adap = &priv->adap;
-	bit_data = &priv->bit_data;
-	pdata = &priv->pdata;
-
+	/* First get the GPIO pins; if it fails, we'll defer the probe. */
 	if (pdev->dev.of_node) {
-		ret = of_i2c_gpio_probe(pdev->dev.of_node, pdata);
+		ret = of_i2c_gpio_get_pins(pdev->dev.of_node,
+					   &sda_pin, &scl_pin);
 		if (ret)
 			return ret;
 	} else {
 		if (!pdev->dev.platform_data)
 			return -ENXIO;
-		memcpy(pdata, pdev->dev.platform_data, sizeof(*pdata));
+		pdata = pdev->dev.platform_data;
+		sda_pin = pdata->sda_pin;
+		scl_pin = pdata->scl_pin;
 	}
 
-	ret = gpio_request(pdata->sda_pin, "sda");
-	if (ret)
+	ret = gpio_request(sda_pin, "sda");
+	if (ret) {
+		if (ret == -EINVAL)
+			ret = -EPROBE_DEFER;	/* Try again later */
 		goto err_request_sda;
-	ret = gpio_request(pdata->scl_pin, "scl");
-	if (ret)
+	}
+	ret = gpio_request(scl_pin, "scl");
+	if (ret) {
+		if (ret == -EINVAL)
+			ret = -EPROBE_DEFER;	/* Try again later */
 		goto err_request_scl;
+	}
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		ret = -ENOMEM;
+		goto err_add_bus;
+	}
+	adap = &priv->adap;
+	bit_data = &priv->bit_data;
+	pdata = &priv->pdata;
+
+	if (pdev->dev.of_node) {
+		pdata->sda_pin = sda_pin;
+		pdata->scl_pin = scl_pin;
+		of_i2c_gpio_get_props(pdev->dev.of_node, pdata);
+	} else {
+		memcpy(pdata, pdev->dev.platform_data, sizeof(*pdata));
+	}
 
 	if (pdata->sda_is_open_drain) {
 		gpio_direction_output(pdata->sda_pin, 1);
@@ -211,9 +236,9 @@ static int i2c_gpio_probe(struct platform_device *pdev)
 	return 0;
 
 err_add_bus:
-	gpio_free(pdata->scl_pin);
+	gpio_free(scl_pin);
 err_request_scl:
-	gpio_free(pdata->sda_pin);
+	gpio_free(sda_pin);
 err_request_sda:
 	return ret;
 }
diff --git a/drivers/i2c/busses/i2c-intel-mid.c b/drivers/i2c/busses/i2c-intel-mid.c
index 323fa018ffd5..0fb659726ffc 100644
--- a/drivers/i2c/busses/i2c-intel-mid.c
+++ b/drivers/i2c/busses/i2c-intel-mid.c
@@ -1082,8 +1082,7 @@ static void intel_mid_i2c_remove(struct pci_dev *dev)
 {
 	struct intel_mid_i2c_private *mrst = pci_get_drvdata(dev);
 	intel_mid_i2c_disable(&mrst->adap);
-	if (i2c_del_adapter(&mrst->adap))
-		dev_err(&dev->dev, "Failed to delete i2c adapter");
+	i2c_del_adapter(&mrst->adap);
 
 	free_irq(dev->irq, mrst);
 	iounmap(mrst->base);
diff --git a/drivers/i2c/busses/i2c-ismt.c b/drivers/i2c/busses/i2c-ismt.c
index 130f02cc9d94..cd82eb44e4c4 100644
--- a/drivers/i2c/busses/i2c-ismt.c
+++ b/drivers/i2c/busses/i2c-ismt.c
@@ -183,7 +183,7 @@ struct ismt_priv {
 /**
  * ismt_ids - PCI device IDs supported by this driver
  */
-static const DEFINE_PCI_DEVICE_TABLE(ismt_ids) = {
+static DEFINE_PCI_DEVICE_TABLE(ismt_ids) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_S1200_SMT0) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_S1200_SMT1) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_AVOTON_SMT) },
diff --git a/drivers/i2c/busses/i2c-mv64xxx.c b/drivers/i2c/busses/i2c-mv64xxx.c
index 8b20ef8524ac..3bbd65d35a5e 100644
--- a/drivers/i2c/busses/i2c-mv64xxx.c
+++ b/drivers/i2c/busses/i2c-mv64xxx.c
@@ -701,9 +701,8 @@ static int
 mv64xxx_i2c_remove(struct platform_device *dev)
 {
 	struct mv64xxx_i2c_data		*drv_data = platform_get_drvdata(dev);
-	int	rc;
 
-	rc = i2c_del_adapter(&drv_data->adapter);
+	i2c_del_adapter(&drv_data->adapter);
 	free_irq(drv_data->irq, drv_data);
 	mv64xxx_i2c_unmap_regs(drv_data);
 #if defined(CONFIG_HAVE_CLK)
@@ -715,7 +714,7 @@ mv64xxx_i2c_remove(struct platform_device *dev)
 #endif
 	kfree(drv_data);
 
-	return rc;
+	return 0;
 }
 
 static const struct of_device_id mv64xxx_i2c_of_match_table[] = {
diff --git a/drivers/i2c/busses/i2c-mxs.c b/drivers/i2c/busses/i2c-mxs.c
index 120f24646696..2039f230482d 100644
--- a/drivers/i2c/busses/i2c-mxs.c
+++ b/drivers/i2c/busses/i2c-mxs.c
@@ -31,7 +31,6 @@
 #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"
 
@@ -56,6 +55,7 @@
 #define MXS_I2C_CTRL1_SET	(0x44)
 #define MXS_I2C_CTRL1_CLR	(0x48)
 
+#define MXS_I2C_CTRL1_CLR_GOT_A_NAK		0x10000000
 #define MXS_I2C_CTRL1_BUS_FREE_IRQ		0x80
 #define MXS_I2C_CTRL1_DATA_ENGINE_CMPLT_IRQ	0x40
 #define MXS_I2C_CTRL1_NO_SLAVE_ACK_IRQ		0x20
@@ -65,6 +65,10 @@
 #define MXS_I2C_CTRL1_SLAVE_STOP_IRQ		0x02
 #define MXS_I2C_CTRL1_SLAVE_IRQ			0x01
 
+#define MXS_I2C_STAT		(0x50)
+#define MXS_I2C_STAT_BUS_BUSY			0x00000800
+#define MXS_I2C_STAT_CLK_GEN_BUSY		0x00000400
+
 #define MXS_I2C_DATA		(0xa0)
 
 #define MXS_I2C_DEBUG0		(0xb0)
@@ -113,9 +117,7 @@ struct mxs_i2c_dev {
 	uint32_t timing1;
 
 	/* 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];
@@ -297,12 +299,10 @@ static int mxs_i2c_pio_wait_dmareq(struct mxs_i2c_dev *i2c)
 		cond_resched();
 	}
 
-	writel(MXS_I2C_DEBUG0_DMAREQ, i2c->regs + MXS_I2C_DEBUG0_CLR);
-
 	return 0;
 }
 
-static int mxs_i2c_pio_wait_cplt(struct mxs_i2c_dev *i2c)
+static int mxs_i2c_pio_wait_cplt(struct mxs_i2c_dev *i2c, int last)
 {
 	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
 
@@ -323,9 +323,50 @@ static int mxs_i2c_pio_wait_cplt(struct mxs_i2c_dev *i2c)
 	writel(MXS_I2C_CTRL1_DATA_ENGINE_CMPLT_IRQ,
 		i2c->regs + MXS_I2C_CTRL1_CLR);
 
+	/*
+	 * When ending a transfer with a stop, we have to wait for the bus to
+	 * go idle before we report the transfer as completed. Otherwise the
+	 * start of the next transfer may race with the end of the current one.
+	 */
+	while (last && (readl(i2c->regs + MXS_I2C_STAT) &
+			(MXS_I2C_STAT_BUS_BUSY | MXS_I2C_STAT_CLK_GEN_BUSY))) {
+		if (time_after(jiffies, timeout))
+			return -ETIMEDOUT;
+		cond_resched();
+	}
+
 	return 0;
 }
 
+static int mxs_i2c_pio_check_error_state(struct mxs_i2c_dev *i2c)
+{
+	u32 state;
+
+	state = readl(i2c->regs + MXS_I2C_CTRL1_CLR) & MXS_I2C_IRQ_MASK;
+
+	if (state & MXS_I2C_CTRL1_NO_SLAVE_ACK_IRQ)
+		i2c->cmd_err = -ENXIO;
+	else if (state & (MXS_I2C_CTRL1_EARLY_TERM_IRQ |
+			  MXS_I2C_CTRL1_MASTER_LOSS_IRQ |
+			  MXS_I2C_CTRL1_SLAVE_STOP_IRQ |
+			  MXS_I2C_CTRL1_SLAVE_IRQ))
+		i2c->cmd_err = -EIO;
+
+	return i2c->cmd_err;
+}
+
+static void mxs_i2c_pio_trigger_cmd(struct mxs_i2c_dev *i2c, u32 cmd)
+{
+	u32 reg;
+
+	writel(cmd, i2c->regs + MXS_I2C_CTRL0);
+
+	/* readback makes sure the write is latched into hardware */
+	reg = readl(i2c->regs + MXS_I2C_CTRL0);
+	reg |= MXS_I2C_CTRL0_RUN;
+	writel(reg, i2c->regs + MXS_I2C_CTRL0);
+}
+
 static int mxs_i2c_pio_setup_xfer(struct i2c_adapter *adap,
 			struct i2c_msg *msg, uint32_t flags)
 {
@@ -341,23 +382,26 @@ static int mxs_i2c_pio_setup_xfer(struct i2c_adapter *adap,
 		addr_data |= I2C_SMBUS_READ;
 
 		/* SELECT command. */
-		writel(MXS_I2C_CTRL0_RUN | MXS_CMD_I2C_SELECT,
-			i2c->regs + MXS_I2C_CTRL0);
+		mxs_i2c_pio_trigger_cmd(i2c, MXS_CMD_I2C_SELECT);
 
 		ret = mxs_i2c_pio_wait_dmareq(i2c);
 		if (ret)
 			return ret;
 
 		writel(addr_data, i2c->regs + MXS_I2C_DATA);
+		writel(MXS_I2C_DEBUG0_DMAREQ, i2c->regs + MXS_I2C_DEBUG0_CLR);
 
-		ret = mxs_i2c_pio_wait_cplt(i2c);
+		ret = mxs_i2c_pio_wait_cplt(i2c, 0);
 		if (ret)
 			return ret;
 
+		if (mxs_i2c_pio_check_error_state(i2c))
+			goto cleanup;
+
 		/* READ command. */
-		writel(MXS_I2C_CTRL0_RUN | MXS_CMD_I2C_READ | flags |
-			MXS_I2C_CTRL0_XFER_COUNT(msg->len),
-			i2c->regs + MXS_I2C_CTRL0);
+		mxs_i2c_pio_trigger_cmd(i2c,
+					MXS_CMD_I2C_READ | flags |
+					MXS_I2C_CTRL0_XFER_COUNT(msg->len));
 
 		for (i = 0; i < msg->len; i++) {
 			if ((i & 3) == 0) {
@@ -365,6 +409,8 @@ static int mxs_i2c_pio_setup_xfer(struct i2c_adapter *adap,
 				if (ret)
 					return ret;
 				data = readl(i2c->regs + MXS_I2C_DATA);
+				writel(MXS_I2C_DEBUG0_DMAREQ,
+				       i2c->regs + MXS_I2C_DEBUG0_CLR);
 			}
 			msg->buf[i] = data & 0xff;
 			data >>= 8;
@@ -373,9 +419,9 @@ static int mxs_i2c_pio_setup_xfer(struct i2c_adapter *adap,
 		addr_data |= I2C_SMBUS_WRITE;
 
 		/* WRITE command. */
-		writel(MXS_I2C_CTRL0_RUN | MXS_CMD_I2C_WRITE | flags |
-			MXS_I2C_CTRL0_XFER_COUNT(msg->len + 1),
-			i2c->regs + MXS_I2C_CTRL0);
+		mxs_i2c_pio_trigger_cmd(i2c,
+					MXS_CMD_I2C_WRITE | flags |
+					MXS_I2C_CTRL0_XFER_COUNT(msg->len + 1));
 
 		/*
 		 * The LSB of data buffer is the first byte blasted across
@@ -391,6 +437,8 @@ static int mxs_i2c_pio_setup_xfer(struct i2c_adapter *adap,
 				if (ret)
 					return ret;
 				writel(data, i2c->regs + MXS_I2C_DATA);
+				writel(MXS_I2C_DEBUG0_DMAREQ,
+				       i2c->regs + MXS_I2C_DEBUG0_CLR);
 			}
 		}
 
@@ -401,13 +449,19 @@ static int mxs_i2c_pio_setup_xfer(struct i2c_adapter *adap,
 			if (ret)
 				return ret;
 			writel(data, i2c->regs + MXS_I2C_DATA);
+			writel(MXS_I2C_DEBUG0_DMAREQ,
+			       i2c->regs + MXS_I2C_DEBUG0_CLR);
 		}
 	}
 
-	ret = mxs_i2c_pio_wait_cplt(i2c);
+	ret = mxs_i2c_pio_wait_cplt(i2c, flags & MXS_I2C_CTRL0_POST_SEND_STOP);
 	if (ret)
 		return ret;
 
+	/* make sure we capture any occurred error into cmd_err */
+	mxs_i2c_pio_check_error_state(i2c);
+
+cleanup:
 	/* Clear any dangling IRQs and re-enable interrupts. */
 	writel(MXS_I2C_IRQ_MASK, i2c->regs + MXS_I2C_CTRL1_CLR);
 	writel(MXS_I2C_IRQ_MASK << 8, i2c->regs + MXS_I2C_CTRL1_SET);
@@ -439,12 +493,12 @@ static int mxs_i2c_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg,
 	 * using PIO mode while longer transfers use DMA. The 8 byte border is
 	 * based on this empirical measurement and a lot of previous frobbing.
 	 */
+	i2c->cmd_err = 0;
 	if (msg->len < 8) {
 		ret = mxs_i2c_pio_setup_xfer(adap, msg, flags);
 		if (ret)
 			mxs_i2c_reset(i2c);
 	} else {
-		i2c->cmd_err = 0;
 		INIT_COMPLETION(i2c->cmd_complete);
 		ret = mxs_i2c_dma_setup_xfer(adap, msg, flags);
 		if (ret)
@@ -454,13 +508,19 @@ static int mxs_i2c_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg,
 						msecs_to_jiffies(1000));
 		if (ret == 0)
 			goto timeout;
+	}
 
-		if (i2c->cmd_err == -ENXIO)
-			mxs_i2c_reset(i2c);
-
-		ret = i2c->cmd_err;
+	if (i2c->cmd_err == -ENXIO) {
+		/*
+		 * If the transfer fails with a NAK from the slave the
+		 * controller halts until it gets told to return to idle state.
+		 */
+		writel(MXS_I2C_CTRL1_CLR_GOT_A_NAK,
+		       i2c->regs + MXS_I2C_CTRL1_SET);
 	}
 
+	ret = i2c->cmd_err;
+
 	dev_dbg(i2c->dev, "Done with err=%d\n", ret);
 
 	return ret;
@@ -518,21 +578,6 @@ 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 void mxs_i2c_derive_timing(struct mxs_i2c_dev *i2c, int speed)
 {
 	/* The I2C block clock run at 24MHz */
@@ -577,17 +622,6 @@ 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");
@@ -607,8 +641,7 @@ static int mxs_i2c_probe(struct platform_device *pdev)
 	struct pinctrl *pinctrl;
 	struct resource *res;
 	resource_size_t res_size;
-	int err, irq, dmairq;
-	dma_cap_mask_t mask;
+	int err, irq;
 
 	pinctrl = devm_pinctrl_get_select_default(dev);
 	if (IS_ERR(pinctrl))
@@ -620,9 +653,8 @@ static int mxs_i2c_probe(struct platform_device *pdev)
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	irq = platform_get_irq(pdev, 0);
-	dmairq = platform_get_irq(pdev, 1);
 
-	if (!res || irq < 0 || dmairq < 0)
+	if (!res || irq < 0)
 		return -ENOENT;
 
 	res_size = resource_size(res);
@@ -648,10 +680,7 @@ static int mxs_i2c_probe(struct platform_device *pdev)
 	}
 
 	/* 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);
+	i2c->dmach = dma_request_slave_channel(dev, "rx-tx");
 	if (!i2c->dmach) {
 		dev_err(dev, "Failed to request dma\n");
 		return -ENODEV;
@@ -686,11 +715,8 @@ static int mxs_i2c_probe(struct platform_device *pdev)
 static int mxs_i2c_remove(struct platform_device *pdev)
 {
 	struct mxs_i2c_dev *i2c = platform_get_drvdata(pdev);
-	int ret;
 
-	ret = i2c_del_adapter(&i2c->adapter);
-	if (ret)
-		return -EBUSY;
+	i2c_del_adapter(&i2c->adapter);
 
 	if (i2c->dmach)
 		dma_release_channel(i2c->dmach);
diff --git a/drivers/i2c/busses/i2c-nforce2-s4985.c b/drivers/i2c/busses/i2c-nforce2-s4985.c
index 29015eb9ca46..2ca268d6140b 100644
--- a/drivers/i2c/busses/i2c-nforce2-s4985.c
+++ b/drivers/i2c/busses/i2c-nforce2-s4985.c
@@ -164,11 +164,7 @@ static int __init nforce2_s4985_init(void)
 	}
 
 	/* Unregister physical bus */
-	error = i2c_del_adapter(nforce2_smbus);
-	if (error) {
-		dev_err(&nforce2_smbus->dev, "Physical bus removal failed\n");
-		goto ERROR0;
-	}
+	i2c_del_adapter(nforce2_smbus);
 
 	printk(KERN_INFO "Enabling SMBus multiplexing for Tyan S4985\n");
 	/* Define the 5 virtual adapters and algorithms structures */
diff --git a/drivers/i2c/busses/i2c-octeon.c b/drivers/i2c/busses/i2c-octeon.c
index 935585ef4d39..956fe320f313 100644
--- a/drivers/i2c/busses/i2c-octeon.c
+++ b/drivers/i2c/busses/i2c-octeon.c
@@ -183,7 +183,7 @@ static irqreturn_t octeon_i2c_isr(int irq, void *dev_id)
 	struct octeon_i2c *i2c = dev_id;
 
 	octeon_i2c_int_disable(i2c);
-	wake_up_interruptible(&i2c->queue);
+	wake_up(&i2c->queue);
 
 	return IRQ_HANDLED;
 }
@@ -206,9 +206,9 @@ static int octeon_i2c_wait(struct octeon_i2c *i2c)
 
 	octeon_i2c_int_enable(i2c);
 
-	result = wait_event_interruptible_timeout(i2c->queue,
-						  octeon_i2c_test_iflg(i2c),
-						  i2c->adap.timeout);
+	result = wait_event_timeout(i2c->queue,
+					octeon_i2c_test_iflg(i2c),
+					i2c->adap.timeout);
 
 	octeon_i2c_int_disable(i2c);
 
@@ -440,7 +440,7 @@ static struct i2c_adapter octeon_i2c_ops = {
 	.owner = THIS_MODULE,
 	.name = "OCTEON adapter",
 	.algo = &octeon_i2c_algo,
-	.timeout = 2,
+	.timeout = HZ / 50,
 };
 
 /**
diff --git a/drivers/i2c/busses/i2c-powermac.c b/drivers/i2c/busses/i2c-powermac.c
index da54e673449d..8dc90da1e6e6 100644
--- a/drivers/i2c/busses/i2c-powermac.c
+++ b/drivers/i2c/busses/i2c-powermac.c
@@ -213,14 +213,8 @@ static const struct i2c_algorithm i2c_powermac_algorithm = {
 static int i2c_powermac_remove(struct platform_device *dev)
 {
 	struct i2c_adapter	*adapter = platform_get_drvdata(dev);
-	int			rc;
-
-	rc = i2c_del_adapter(adapter);
-	/* We aren't that prepared to deal with this... */
-	if (rc)
-		printk(KERN_WARNING
-		       "i2c-powermac.c: Failed to remove bus %s !\n",
-		       adapter->name);
+
+	i2c_del_adapter(adapter);
 	memset(adapter, 0, sizeof(*adapter));
 
 	return 0;
diff --git a/drivers/i2c/busses/i2c-puv3.c b/drivers/i2c/busses/i2c-puv3.c
index 8acef657abf3..37a84c87c5fd 100644
--- a/drivers/i2c/busses/i2c-puv3.c
+++ b/drivers/i2c/busses/i2c-puv3.c
@@ -234,21 +234,15 @@ static int puv3_i2c_remove(struct platform_device *pdev)
 {
 	struct i2c_adapter *adapter = platform_get_drvdata(pdev);
 	struct resource *mem;
-	int rc;
 
-	rc = i2c_del_adapter(adapter);
-	if (rc) {
-		dev_err(&pdev->dev, "Adapter '%s' delete fail\n",
-				adapter->name);
-		return rc;
-	}
+	i2c_del_adapter(adapter);
 
 	put_device(&pdev->dev);
 
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	release_mem_region(mem->start, resource_size(mem));
 
-	return rc;
+	return 0;
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/i2c/busses/i2c-pxa.c b/drivers/i2c/busses/i2c-pxa.c
index 1e88e8d66c55..ea6d45d1dcd6 100644
--- a/drivers/i2c/busses/i2c-pxa.c
+++ b/drivers/i2c/busses/i2c-pxa.c
@@ -1053,16 +1053,13 @@ static int i2c_pxa_probe_dt(struct platform_device *pdev, struct pxa_i2c *i2c,
 	struct device_node *np = pdev->dev.of_node;
 	const struct of_device_id *of_id =
 			of_match_device(i2c_pxa_dt_ids, &pdev->dev);
-	int ret;
 
 	if (!of_id)
 		return 1;
-	ret = of_alias_get_id(np, "i2c");
-	if (ret < 0) {
-		dev_err(&pdev->dev, "failed to get alias id, errno %d\n", ret);
-		return ret;
-	}
-	pdev->id = ret;
+
+	/* For device tree we always use the dynamic or alias-assigned ID */
+	i2c->adap.nr = -1;
+
 	if (of_get_property(np, "mrvl,i2c-polling", NULL))
 		i2c->use_pio = 1;
 	if (of_get_property(np, "mrvl,i2c-fast-mode", NULL))
@@ -1100,6 +1097,9 @@ static int i2c_pxa_probe(struct platform_device *dev)
 		goto emalloc;
 	}
 
+	/* Default adapter num to device id; i2c_pxa_probe_dt can override. */
+	i2c->adap.nr = dev->id;
+
 	ret = i2c_pxa_probe_dt(dev, i2c, &i2c_type);
 	if (ret > 0)
 		ret = i2c_pxa_probe_pdata(dev, i2c, &i2c_type);
@@ -1124,9 +1124,7 @@ static int i2c_pxa_probe(struct platform_device *dev)
 	spin_lock_init(&i2c->lock);
 	init_waitqueue_head(&i2c->wait);
 
-	i2c->adap.nr = dev->id;
-	snprintf(i2c->adap.name, sizeof(i2c->adap.name), "pxa_i2c-i2c.%u",
-		 i2c->adap.nr);
+	strlcpy(i2c->adap.name, "pxa_i2c-i2c", sizeof(i2c->adap.name));
 
 	i2c->clk = clk_get(&dev->dev, NULL);
 	if (IS_ERR(i2c->clk)) {
@@ -1169,7 +1167,7 @@ static int i2c_pxa_probe(struct platform_device *dev)
 	} else {
 		i2c->adap.algo = &i2c_pxa_algorithm;
 		ret = request_irq(irq, i2c_pxa_handler, IRQF_SHARED,
-				  i2c->adap.name, i2c);
+				  dev_name(&dev->dev), i2c);
 		if (ret)
 			goto ereqirq;
 	}
diff --git a/drivers/i2c/busses/i2c-s3c2410.c b/drivers/i2c/busses/i2c-s3c2410.c
index f6b880ba1932..6e8ee92ab553 100644
--- a/drivers/i2c/busses/i2c-s3c2410.c
+++ b/drivers/i2c/busses/i2c-s3c2410.c
@@ -42,9 +42,46 @@
 
 #include <asm/irq.h>
 
-#include <plat/regs-iic.h>
 #include <linux/platform_data/i2c-s3c2410.h>
 
+/* see s3c2410x user guide, v1.1, section 9 (p447) for more info */
+
+#define S3C2410_IICCON			0x00
+#define S3C2410_IICSTAT			0x04
+#define S3C2410_IICADD			0x08
+#define S3C2410_IICDS			0x0C
+#define S3C2440_IICLC			0x10
+
+#define S3C2410_IICCON_ACKEN		(1 << 7)
+#define S3C2410_IICCON_TXDIV_16		(0 << 6)
+#define S3C2410_IICCON_TXDIV_512	(1 << 6)
+#define S3C2410_IICCON_IRQEN		(1 << 5)
+#define S3C2410_IICCON_IRQPEND		(1 << 4)
+#define S3C2410_IICCON_SCALE(x)		((x) & 0xf)
+#define S3C2410_IICCON_SCALEMASK	(0xf)
+
+#define S3C2410_IICSTAT_MASTER_RX	(2 << 6)
+#define S3C2410_IICSTAT_MASTER_TX	(3 << 6)
+#define S3C2410_IICSTAT_SLAVE_RX	(0 << 6)
+#define S3C2410_IICSTAT_SLAVE_TX	(1 << 6)
+#define S3C2410_IICSTAT_MODEMASK	(3 << 6)
+
+#define S3C2410_IICSTAT_START		(1 << 5)
+#define S3C2410_IICSTAT_BUSBUSY		(1 << 5)
+#define S3C2410_IICSTAT_TXRXEN		(1 << 4)
+#define S3C2410_IICSTAT_ARBITR		(1 << 3)
+#define S3C2410_IICSTAT_ASSLAVE		(1 << 2)
+#define S3C2410_IICSTAT_ADDR0		(1 << 1)
+#define S3C2410_IICSTAT_LASTBIT		(1 << 0)
+
+#define S3C2410_IICLC_SDA_DELAY0	(0 << 0)
+#define S3C2410_IICLC_SDA_DELAY5	(1 << 0)
+#define S3C2410_IICLC_SDA_DELAY10	(2 << 0)
+#define S3C2410_IICLC_SDA_DELAY15	(3 << 0)
+#define S3C2410_IICLC_SDA_DELAY_MASK	(3 << 0)
+
+#define S3C2410_IICLC_FILTER_ON		(1 << 2)
+
 /* Treat S3C2410 as baseline hardware, anything else is supported via quirks */
 #define QUIRK_S3C2440		(1 << 0)
 #define QUIRK_HDMIPHY		(1 << 1)
@@ -309,6 +346,12 @@ static inline int is_lastmsg(struct s3c24xx_i2c *i2c)
 
 static inline int is_msglast(struct s3c24xx_i2c *i2c)
 {
+	/* msg->len is always 1 for the first byte of smbus block read.
+	 * Actual length will be read from slave. More bytes will be
+	 * read according to the length then. */
+	if (i2c->msg->flags & I2C_M_RECV_LEN && i2c->msg->len == 1)
+		return 0;
+
 	return i2c->msg_ptr == i2c->msg->len-1;
 }
 
@@ -448,6 +491,9 @@ static int i2c_s3c_irq_nextbyte(struct s3c24xx_i2c *i2c, unsigned long iicstat)
 		byte = readb(i2c->regs + S3C2410_IICDS);
 		i2c->msg->buf[i2c->msg_ptr++] = byte;
 
+		/* Add actual length to read for smbus block read */
+		if (i2c->msg->flags & I2C_M_RECV_LEN && i2c->msg->len == 1)
+			i2c->msg->len += byte;
  prepare_read:
 		if (is_msglast(i2c)) {
 			/* last byte of buffer */
diff --git a/drivers/i2c/busses/i2c-tegra.c b/drivers/i2c/busses/i2c-tegra.c
index b714776b6ddd..b60ff90adc39 100644
--- a/drivers/i2c/busses/i2c-tegra.c
+++ b/drivers/i2c/busses/i2c-tegra.c
@@ -25,7 +25,6 @@
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/slab.h>
-#include <linux/i2c-tegra.h>
 #include <linux/of_i2c.h>
 #include <linux/of_device.h>
 #include <linux/module.h>
@@ -172,7 +171,7 @@ struct tegra_i2c_dev {
 	u8 *msg_buf;
 	size_t msg_buf_remaining;
 	int msg_read;
-	unsigned long bus_clk_rate;
+	u32 bus_clk_rate;
 	bool is_suspended;
 };
 
@@ -694,7 +693,6 @@ static const struct tegra_i2c_hw_feature tegra114_i2c_hw = {
 	.clk_divisor_std_fast_mode = 0x19,
 };
 
-#if defined(CONFIG_OF)
 /* Match table for of_platform binding */
 static const struct of_device_id tegra_i2c_of_match[] = {
 	{ .compatible = "nvidia,tegra114-i2c", .data = &tegra114_i2c_hw, },
@@ -704,16 +702,13 @@ static const struct of_device_id tegra_i2c_of_match[] = {
 	{},
 };
 MODULE_DEVICE_TABLE(of, tegra_i2c_of_match);
-#endif
 
 static int tegra_i2c_probe(struct platform_device *pdev)
 {
 	struct tegra_i2c_dev *i2c_dev;
-	struct tegra_i2c_platform_data *pdata = pdev->dev.platform_data;
 	struct resource *res;
 	struct clk *div_clk;
 	struct clk *fast_clk;
-	const unsigned int *prop;
 	void __iomem *base;
 	int irq;
 	int ret = 0;
@@ -754,23 +749,16 @@ static int tegra_i2c_probe(struct platform_device *pdev)
 	i2c_dev->cont_id = pdev->id;
 	i2c_dev->dev = &pdev->dev;
 
-	i2c_dev->bus_clk_rate = 100000; /* default clock rate */
-	if (pdata) {
-		i2c_dev->bus_clk_rate = pdata->bus_clk_rate;
-
-	} else if (i2c_dev->dev->of_node) {    /* if there is a device tree node ... */
-		prop = of_get_property(i2c_dev->dev->of_node,
-				"clock-frequency", NULL);
-		if (prop)
-			i2c_dev->bus_clk_rate = be32_to_cpup(prop);
-	}
+	ret = of_property_read_u32(i2c_dev->dev->of_node, "clock-frequency",
+					&i2c_dev->bus_clk_rate);
+	if (ret)
+		i2c_dev->bus_clk_rate = 100000; /* default clock rate */
 
 	i2c_dev->hw = &tegra20_i2c_hw;
 
 	if (pdev->dev.of_node) {
 		const struct of_device_id *match;
-		match = of_match_device(of_match_ptr(tegra_i2c_of_match),
-						&pdev->dev);
+		match = of_match_device(tegra_i2c_of_match, &pdev->dev);
 		i2c_dev->hw = match->data;
 		i2c_dev->is_dvc = of_device_is_compatible(pdev->dev.of_node,
 						"nvidia,tegra20-i2c-dvc");
@@ -876,7 +864,7 @@ static struct platform_driver tegra_i2c_driver = {
 	.driver  = {
 		.name  = "tegra-i2c",
 		.owner = THIS_MODULE,
-		.of_match_table = of_match_ptr(tegra_i2c_of_match),
+		.of_match_table = tegra_i2c_of_match,
 		.pm    = TEGRA_I2C_PM,
 	},
 };
diff --git a/drivers/i2c/busses/i2c-viperboard.c b/drivers/i2c/busses/i2c-viperboard.c
index f45c32c1ace6..c68450cd8d5f 100644
--- a/drivers/i2c/busses/i2c-viperboard.c
+++ b/drivers/i2c/busses/i2c-viperboard.c
@@ -421,11 +421,10 @@ error:
 static int vprbrd_i2c_remove(struct platform_device *pdev)
 {
 	struct vprbrd_i2c *vb_i2c = platform_get_drvdata(pdev);
-	int ret;
 
-	ret = i2c_del_adapter(&vb_i2c->i2c);
+	i2c_del_adapter(&vb_i2c->i2c);
 
-	return ret;
+	return 0;
 }
 
 static struct platform_driver vprbrd_i2c_driver = {
diff --git a/drivers/i2c/busses/i2c-xiic.c b/drivers/i2c/busses/i2c-xiic.c
index 332c720fb3fe..3d0f0520c1b4 100644
--- a/drivers/i2c/busses/i2c-xiic.c
+++ b/drivers/i2c/busses/i2c-xiic.c
@@ -312,10 +312,8 @@ static void xiic_fill_tx_fifo(struct xiic_i2c *i2c)
 			/* last message in transfer -> STOP */
 			data |= XIIC_TX_DYN_STOP_MASK;
 			dev_dbg(i2c->adap.dev.parent, "%s TX STOP\n", __func__);
-
-			xiic_setreg16(i2c, XIIC_DTR_REG_OFFSET, data);
-		} else
-			xiic_setreg8(i2c, XIIC_DTR_REG_OFFSET, data);
+		}
+		xiic_setreg16(i2c, XIIC_DTR_REG_OFFSET, data);
 	}
 }
 
diff --git a/drivers/i2c/i2c-core.c b/drivers/i2c/i2c-core.c
index 991d38daa87d..6b63cc7eb71e 100644
--- a/drivers/i2c/i2c-core.c
+++ b/drivers/i2c/i2c-core.c
@@ -27,7 +27,9 @@
 
 #include <linux/module.h>
 #include <linux/kernel.h>
+#include <linux/delay.h>
 #include <linux/errno.h>
+#include <linux/gpio.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
 #include <linux/init.h>
@@ -47,7 +49,7 @@
 
 /* core_lock protects i2c_adapter_idr, and guarantees
    that device detection, deletion of detected devices, and attach_adapter
-   and detach_adapter calls are serialized */
+   calls are serialized */
 static DEFINE_MUTEX(core_lock);
 static DEFINE_IDR(i2c_adapter_idr);
 
@@ -91,7 +93,6 @@ static int i2c_device_match(struct device *dev, struct device_driver *drv)
 	return 0;
 }
 
-#ifdef	CONFIG_HOTPLUG
 
 /* uevent helps with hotplug: modprobe -q $(MODALIAS) */
 static int i2c_device_uevent(struct device *dev, struct kobj_uevent_env *env)
@@ -105,9 +106,129 @@ static int i2c_device_uevent(struct device *dev, struct kobj_uevent_env *env)
 	return 0;
 }
 
-#else
-#define i2c_device_uevent	NULL
-#endif	/* CONFIG_HOTPLUG */
+/* i2c bus recovery routines */
+static int get_scl_gpio_value(struct i2c_adapter *adap)
+{
+	return gpio_get_value(adap->bus_recovery_info->scl_gpio);
+}
+
+static void set_scl_gpio_value(struct i2c_adapter *adap, int val)
+{
+	gpio_set_value(adap->bus_recovery_info->scl_gpio, val);
+}
+
+static int get_sda_gpio_value(struct i2c_adapter *adap)
+{
+	return gpio_get_value(adap->bus_recovery_info->sda_gpio);
+}
+
+static int i2c_get_gpios_for_recovery(struct i2c_adapter *adap)
+{
+	struct i2c_bus_recovery_info *bri = adap->bus_recovery_info;
+	struct device *dev = &adap->dev;
+	int ret = 0;
+
+	ret = gpio_request_one(bri->scl_gpio, GPIOF_OPEN_DRAIN |
+			GPIOF_OUT_INIT_HIGH, "i2c-scl");
+	if (ret) {
+		dev_warn(dev, "Can't get SCL gpio: %d\n", bri->scl_gpio);
+		return ret;
+	}
+
+	if (bri->get_sda) {
+		if (gpio_request_one(bri->sda_gpio, GPIOF_IN, "i2c-sda")) {
+			/* work without SDA polling */
+			dev_warn(dev, "Can't get SDA gpio: %d. Not using SDA polling\n",
+					bri->sda_gpio);
+			bri->get_sda = NULL;
+		}
+	}
+
+	return ret;
+}
+
+static void i2c_put_gpios_for_recovery(struct i2c_adapter *adap)
+{
+	struct i2c_bus_recovery_info *bri = adap->bus_recovery_info;
+
+	if (bri->get_sda)
+		gpio_free(bri->sda_gpio);
+
+	gpio_free(bri->scl_gpio);
+}
+
+/*
+ * We are generating clock pulses. ndelay() determines durating of clk pulses.
+ * We will generate clock with rate 100 KHz and so duration of both clock levels
+ * is: delay in ns = (10^6 / 100) / 2
+ */
+#define RECOVERY_NDELAY		5000
+#define RECOVERY_CLK_CNT	9
+
+static int i2c_generic_recovery(struct i2c_adapter *adap)
+{
+	struct i2c_bus_recovery_info *bri = adap->bus_recovery_info;
+	int i = 0, val = 1, ret = 0;
+
+	if (bri->prepare_recovery)
+		bri->prepare_recovery(bri);
+
+	/*
+	 * By this time SCL is high, as we need to give 9 falling-rising edges
+	 */
+	while (i++ < RECOVERY_CLK_CNT * 2) {
+		if (val) {
+			/* Break if SDA is high */
+			if (bri->get_sda && bri->get_sda(adap))
+					break;
+			/* SCL shouldn't be low here */
+			if (!bri->get_scl(adap)) {
+				dev_err(&adap->dev,
+					"SCL is stuck low, exit recovery\n");
+				ret = -EBUSY;
+				break;
+			}
+		}
+
+		val = !val;
+		bri->set_scl(adap, val);
+		ndelay(RECOVERY_NDELAY);
+	}
+
+	if (bri->unprepare_recovery)
+		bri->unprepare_recovery(bri);
+
+	return ret;
+}
+
+int i2c_generic_scl_recovery(struct i2c_adapter *adap)
+{
+	adap->bus_recovery_info->set_scl(adap, 1);
+	return i2c_generic_recovery(adap);
+}
+
+int i2c_generic_gpio_recovery(struct i2c_adapter *adap)
+{
+	int ret;
+
+	ret = i2c_get_gpios_for_recovery(adap);
+	if (ret)
+		return ret;
+
+	ret = i2c_generic_recovery(adap);
+	i2c_put_gpios_for_recovery(adap);
+
+	return ret;
+}
+
+int i2c_recover_bus(struct i2c_adapter *adap)
+{
+	if (!adap->bus_recovery_info)
+		return -EOPNOTSUPP;
+
+	dev_dbg(&adap->dev, "Trying i2c bus recovery\n");
+	return adap->bus_recovery_info->recover_bus(adap);
+}
 
 static int i2c_device_probe(struct device *dev)
 {
@@ -902,6 +1023,39 @@ static int i2c_register_adapter(struct i2c_adapter *adap)
 			 "Failed to create compatibility class link\n");
 #endif
 
+	/* bus recovery specific initialization */
+	if (adap->bus_recovery_info) {
+		struct i2c_bus_recovery_info *bri = adap->bus_recovery_info;
+
+		if (!bri->recover_bus) {
+			dev_err(&adap->dev, "No recover_bus() found, not using recovery\n");
+			adap->bus_recovery_info = NULL;
+			goto exit_recovery;
+		}
+
+		/* Generic GPIO recovery */
+		if (bri->recover_bus == i2c_generic_gpio_recovery) {
+			if (!gpio_is_valid(bri->scl_gpio)) {
+				dev_err(&adap->dev, "Invalid SCL gpio, not using recovery\n");
+				adap->bus_recovery_info = NULL;
+				goto exit_recovery;
+			}
+
+			if (gpio_is_valid(bri->sda_gpio))
+				bri->get_sda = get_sda_gpio_value;
+			else
+				bri->get_sda = NULL;
+
+			bri->get_scl = get_scl_gpio_value;
+			bri->set_scl = set_scl_gpio_value;
+		} else if (!bri->set_scl || !bri->get_scl) {
+			/* Generic SCL recovery */
+			dev_err(&adap->dev, "No {get|set}_gpio() found, not using recovery\n");
+			adap->bus_recovery_info = NULL;
+		}
+	}
+
+exit_recovery:
 	/* create pre-declared device nodes */
 	if (adap->nr < __i2c_first_dynamic_bus_num)
 		i2c_scan_static_board_info(adap);
@@ -921,13 +1075,35 @@ out_list:
 }
 
 /**
+ * __i2c_add_numbered_adapter - i2c_add_numbered_adapter where nr is never -1
+ * @adap: the adapter to register (with adap->nr initialized)
+ * Context: can sleep
+ *
+ * See i2c_add_numbered_adapter() for details.
+ */
+static int __i2c_add_numbered_adapter(struct i2c_adapter *adap)
+{
+	int	id;
+
+	mutex_lock(&core_lock);
+	id = idr_alloc(&i2c_adapter_idr, adap, adap->nr, adap->nr + 1,
+		       GFP_KERNEL);
+	mutex_unlock(&core_lock);
+	if (id < 0)
+		return id == -ENOSPC ? -EBUSY : id;
+
+	return i2c_register_adapter(adap);
+}
+
+/**
  * i2c_add_adapter - declare i2c adapter, use dynamic bus number
  * @adapter: the adapter to add
  * Context: can sleep
  *
  * This routine is used to declare an I2C adapter when its bus number
- * doesn't matter.  Examples: for I2C adapters dynamically added by
- * USB links or PCI plugin cards.
+ * doesn't matter or when its bus number is specified by an dt alias.
+ * Examples of bases when the bus number doesn't matter: I2C adapters
+ * dynamically added by USB links or PCI plugin cards.
  *
  * When this returns zero, a new bus number was allocated and stored
  * in adap->nr, and the specified adapter became available for clients.
@@ -935,8 +1111,17 @@ out_list:
  */
 int i2c_add_adapter(struct i2c_adapter *adapter)
 {
+	struct device *dev = &adapter->dev;
 	int id;
 
+	if (dev->of_node) {
+		id = of_alias_get_id(dev->of_node, "i2c");
+		if (id >= 0) {
+			adapter->nr = id;
+			return __i2c_add_numbered_adapter(adapter);
+		}
+	}
+
 	mutex_lock(&core_lock);
 	id = idr_alloc(&i2c_adapter_idr, adapter,
 		       __i2c_first_dynamic_bus_num, 0, GFP_KERNEL);
@@ -975,26 +1160,17 @@ EXPORT_SYMBOL(i2c_add_adapter);
  */
 int i2c_add_numbered_adapter(struct i2c_adapter *adap)
 {
-	int	id;
-
 	if (adap->nr == -1) /* -1 means dynamically assign bus id */
 		return i2c_add_adapter(adap);
 
-	mutex_lock(&core_lock);
-	id = idr_alloc(&i2c_adapter_idr, adap, adap->nr, adap->nr + 1,
-		       GFP_KERNEL);
-	mutex_unlock(&core_lock);
-	if (id < 0)
-		return id == -ENOSPC ? -EBUSY : id;
-	return i2c_register_adapter(adap);
+	return __i2c_add_numbered_adapter(adap);
 }
 EXPORT_SYMBOL_GPL(i2c_add_numbered_adapter);
 
-static int i2c_do_del_adapter(struct i2c_driver *driver,
+static void i2c_do_del_adapter(struct i2c_driver *driver,
 			      struct i2c_adapter *adapter)
 {
 	struct i2c_client *client, *_n;
-	int res;
 
 	/* Remove the devices we created ourselves as the result of hardware
 	 * probing (using a driver's detect method) */
@@ -1006,16 +1182,6 @@ static int i2c_do_del_adapter(struct i2c_driver *driver,
 			i2c_unregister_device(client);
 		}
 	}
-
-	if (!driver->detach_adapter)
-		return 0;
-	dev_warn(&adapter->dev, "%s: detach_adapter method is deprecated\n",
-		 driver->driver.name);
-	res = driver->detach_adapter(adapter);
-	if (res)
-		dev_err(&adapter->dev, "detach_adapter failed (%d) "
-			"for driver [%s]\n", res, driver->driver.name);
-	return res;
 }
 
 static int __unregister_client(struct device *dev, void *dummy)
@@ -1036,7 +1202,8 @@ static int __unregister_dummy(struct device *dev, void *dummy)
 
 static int __process_removed_adapter(struct device_driver *d, void *data)
 {
-	return i2c_do_del_adapter(to_i2c_driver(d), data);
+	i2c_do_del_adapter(to_i2c_driver(d), data);
+	return 0;
 }
 
 /**
@@ -1047,9 +1214,8 @@ static int __process_removed_adapter(struct device_driver *d, void *data)
  * This unregisters an I2C adapter which was previously registered
  * by @i2c_add_adapter or @i2c_add_numbered_adapter.
  */
-int i2c_del_adapter(struct i2c_adapter *adap)
+void i2c_del_adapter(struct i2c_adapter *adap)
 {
-	int res = 0;
 	struct i2c_adapter *found;
 	struct i2c_client *client, *next;
 
@@ -1060,16 +1226,14 @@ int i2c_del_adapter(struct i2c_adapter *adap)
 	if (found != adap) {
 		pr_debug("i2c-core: attempting to delete unregistered "
 			 "adapter [%s]\n", adap->name);
-		return -EINVAL;
+		return;
 	}
 
 	/* Tell drivers about this removal */
 	mutex_lock(&core_lock);
-	res = bus_for_each_drv(&i2c_bus_type, NULL, adap,
+	bus_for_each_drv(&i2c_bus_type, NULL, adap,
 			       __process_removed_adapter);
 	mutex_unlock(&core_lock);
-	if (res)
-		return res;
 
 	/* Remove devices instantiated from sysfs */
 	mutex_lock_nested(&adap->userspace_clients_lock,
@@ -1088,8 +1252,8 @@ int i2c_del_adapter(struct i2c_adapter *adap)
 	 * we can't remove the dummy devices during the first pass: they
 	 * could have been instantiated by real devices wishing to clean
 	 * them up properly, so we give them a chance to do that first. */
-	res = device_for_each_child(&adap->dev, NULL, __unregister_client);
-	res = device_for_each_child(&adap->dev, NULL, __unregister_dummy);
+	device_for_each_child(&adap->dev, NULL, __unregister_client);
+	device_for_each_child(&adap->dev, NULL, __unregister_dummy);
 
 #ifdef CONFIG_I2C_COMPAT
 	class_compat_remove_link(i2c_adapter_compat_class, &adap->dev,
@@ -1114,8 +1278,6 @@ int i2c_del_adapter(struct i2c_adapter *adap)
 	/* Clear the device structure in case this adapter is ever going to be
 	   added again */
 	memset(&adap->dev, 0, sizeof(adap->dev));
-
-	return 0;
 }
 EXPORT_SYMBOL(i2c_del_adapter);
 
@@ -1185,9 +1347,9 @@ EXPORT_SYMBOL(i2c_register_driver);
 
 static int __process_removed_driver(struct device *dev, void *data)
 {
-	if (dev->type != &i2c_adapter_type)
-		return 0;
-	return i2c_do_del_adapter(data, to_i2c_adapter(dev));
+	if (dev->type == &i2c_adapter_type)
+		i2c_do_del_adapter(data, to_i2c_adapter(dev));
+	return 0;
 }
 
 /**
diff --git a/drivers/i2c/i2c-mux.c b/drivers/i2c/i2c-mux.c
index d94e0ce78277..7409ebb33c47 100644
--- a/drivers/i2c/i2c-mux.c
+++ b/drivers/i2c/i2c-mux.c
@@ -191,17 +191,12 @@ struct i2c_adapter *i2c_add_mux_adapter(struct i2c_adapter *parent,
 }
 EXPORT_SYMBOL_GPL(i2c_add_mux_adapter);
 
-int i2c_del_mux_adapter(struct i2c_adapter *adap)
+void i2c_del_mux_adapter(struct i2c_adapter *adap)
 {
 	struct i2c_mux_priv *priv = adap->algo_data;
-	int ret;
 
-	ret = i2c_del_adapter(adap);
-	if (ret < 0)
-		return ret;
+	i2c_del_adapter(adap);
 	kfree(priv);
-
-	return 0;
 }
 EXPORT_SYMBOL_GPL(i2c_del_mux_adapter);
 
diff --git a/drivers/i2c/muxes/Kconfig b/drivers/i2c/muxes/Kconfig
index 0be5b83c08fa..f7f9865b8b89 100644
--- a/drivers/i2c/muxes/Kconfig
+++ b/drivers/i2c/muxes/Kconfig
@@ -5,9 +5,21 @@
 menu "Multiplexer I2C Chip support"
 	depends on I2C_MUX
 
+config I2C_ARB_GPIO_CHALLENGE
+	tristate "GPIO-based I2C arbitration"
+	depends on GPIOLIB && OF
+	help
+	  If you say yes to this option, support will be included for an
+	  I2C multimaster arbitration scheme using GPIOs and a challenge &
+	  response mechanism where masters have to claim the bus by asserting
+	  a GPIO.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called i2c-arb-gpio-challenge.
+
 config I2C_MUX_GPIO
 	tristate "GPIO-based I2C multiplexer"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  If you say yes to this option, support will be included for a
 	  GPIO based I2C multiplexer. This driver provides access to
diff --git a/drivers/i2c/muxes/Makefile b/drivers/i2c/muxes/Makefile
index 76da8692afff..465778b5d5dc 100644
--- a/drivers/i2c/muxes/Makefile
+++ b/drivers/i2c/muxes/Makefile
@@ -1,6 +1,8 @@
 #
 # Makefile for multiplexer I2C chip drivers.
 
+obj-$(CONFIG_I2C_ARB_GPIO_CHALLENGE)	+= i2c-arb-gpio-challenge.o
+
 obj-$(CONFIG_I2C_MUX_GPIO)	+= i2c-mux-gpio.o
 obj-$(CONFIG_I2C_MUX_PCA9541)	+= i2c-mux-pca9541.o
 obj-$(CONFIG_I2C_MUX_PCA954x)	+= i2c-mux-pca954x.o
diff --git a/drivers/i2c/muxes/i2c-arb-gpio-challenge.c b/drivers/i2c/muxes/i2c-arb-gpio-challenge.c
new file mode 100644
index 000000000000..210b6f7b9028
--- /dev/null
+++ b/drivers/i2c/muxes/i2c-arb-gpio-challenge.c
@@ -0,0 +1,251 @@
+/*
+ * GPIO-based I2C Arbitration Using a Challenge & Response Mechanism
+ *
+ * Copyright (C) 2012 Google, Inc
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/i2c-mux.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_i2c.h>
+#include <linux/of_gpio.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+
+/**
+ * struct i2c_arbitrator_data - Driver data for I2C arbitrator
+ *
+ * @parent: Parent adapter
+ * @child: Child bus
+ * @our_gpio: GPIO we'll use to claim.
+ * @our_gpio_release: 0 if active high; 1 if active low; AKA if the GPIO ==
+ *   this then consider it released.
+ * @their_gpio: GPIO that the other side will use to claim.
+ * @their_gpio_release: 0 if active high; 1 if active low; AKA if the GPIO ==
+ *   this then consider it released.
+ * @slew_delay_us: microseconds to wait for a GPIO to go high.
+ * @wait_retry_us: we'll attempt another claim after this many microseconds.
+ * @wait_free_us: we'll give up after this many microseconds.
+ */
+
+struct i2c_arbitrator_data {
+	struct i2c_adapter *parent;
+	struct i2c_adapter *child;
+	int our_gpio;
+	int our_gpio_release;
+	int their_gpio;
+	int their_gpio_release;
+	unsigned int slew_delay_us;
+	unsigned int wait_retry_us;
+	unsigned int wait_free_us;
+};
+
+
+/**
+ * i2c_arbitrator_select - claim the I2C bus
+ *
+ * Use the GPIO-based signalling protocol; return -EBUSY if we fail.
+ */
+static int i2c_arbitrator_select(struct i2c_adapter *adap, void *data, u32 chan)
+{
+	const struct i2c_arbitrator_data *arb = data;
+	unsigned long stop_retry, stop_time;
+
+	/* Start a round of trying to claim the bus */
+	stop_time = jiffies + usecs_to_jiffies(arb->wait_free_us) + 1;
+	do {
+		/* Indicate that we want to claim the bus */
+		gpio_set_value(arb->our_gpio, !arb->our_gpio_release);
+		udelay(arb->slew_delay_us);
+
+		/* Wait for the other master to release it */
+		stop_retry = jiffies + usecs_to_jiffies(arb->wait_retry_us) + 1;
+		while (time_before(jiffies, stop_retry)) {
+			int gpio_val = !!gpio_get_value(arb->their_gpio);
+
+			if (gpio_val == arb->their_gpio_release) {
+				/* We got it, so return */
+				return 0;
+			}
+
+			usleep_range(50, 200);
+		}
+
+		/* It didn't release, so give up, wait, and try again */
+		gpio_set_value(arb->our_gpio, arb->our_gpio_release);
+
+		usleep_range(arb->wait_retry_us, arb->wait_retry_us * 2);
+	} while (time_before(jiffies, stop_time));
+
+	/* Give up, release our claim */
+	gpio_set_value(arb->our_gpio, arb->our_gpio_release);
+	udelay(arb->slew_delay_us);
+	dev_err(&adap->dev, "Could not claim bus, timeout\n");
+	return -EBUSY;
+}
+
+/**
+ * i2c_arbitrator_deselect - release the I2C bus
+ *
+ * Release the I2C bus using the GPIO-based signalling protocol.
+ */
+static int i2c_arbitrator_deselect(struct i2c_adapter *adap, void *data,
+				   u32 chan)
+{
+	const struct i2c_arbitrator_data *arb = data;
+
+	/* Release the bus and wait for the other master to notice */
+	gpio_set_value(arb->our_gpio, arb->our_gpio_release);
+	udelay(arb->slew_delay_us);
+
+	return 0;
+}
+
+static int i2c_arbitrator_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct device_node *parent_np;
+	struct i2c_arbitrator_data *arb;
+	enum of_gpio_flags gpio_flags;
+	unsigned long out_init;
+	int ret;
+
+	/* We only support probing from device tree; no platform_data */
+	if (!np) {
+		dev_err(dev, "Cannot find device tree node\n");
+		return -ENODEV;
+	}
+	if (dev->platform_data) {
+		dev_err(dev, "Platform data is not supported\n");
+		return -EINVAL;
+	}
+
+	arb = devm_kzalloc(dev, sizeof(*arb), GFP_KERNEL);
+	if (!arb) {
+		dev_err(dev, "Cannot allocate i2c_arbitrator_data\n");
+		return -ENOMEM;
+	}
+	platform_set_drvdata(pdev, arb);
+
+	/* Request GPIOs */
+	ret = of_get_named_gpio_flags(np, "our-claim-gpio", 0, &gpio_flags);
+	if (!gpio_is_valid(ret)) {
+		if (ret != -EPROBE_DEFER)
+			dev_err(dev, "Error getting our-claim-gpio\n");
+		return ret;
+	}
+	arb->our_gpio = ret;
+	arb->our_gpio_release = !!(gpio_flags & OF_GPIO_ACTIVE_LOW);
+	out_init = (gpio_flags & OF_GPIO_ACTIVE_LOW) ?
+		GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
+	ret = devm_gpio_request_one(dev, arb->our_gpio, out_init,
+				    "our-claim-gpio");
+	if (ret) {
+		if (ret != -EPROBE_DEFER)
+			dev_err(dev, "Error requesting our-claim-gpio\n");
+		return ret;
+	}
+
+	ret = of_get_named_gpio_flags(np, "their-claim-gpios", 0, &gpio_flags);
+	if (!gpio_is_valid(ret)) {
+		if (ret != -EPROBE_DEFER)
+			dev_err(dev, "Error getting their-claim-gpio\n");
+		return ret;
+	}
+	arb->their_gpio = ret;
+	arb->their_gpio_release = !!(gpio_flags & OF_GPIO_ACTIVE_LOW);
+	ret = devm_gpio_request_one(dev, arb->their_gpio, GPIOF_IN,
+				    "their-claim-gpio");
+	if (ret) {
+		if (ret != -EPROBE_DEFER)
+			dev_err(dev, "Error requesting their-claim-gpio\n");
+		return ret;
+	}
+
+	/* At the moment we only support a single two master (us + 1 other) */
+	if (gpio_is_valid(of_get_named_gpio(np, "their-claim-gpios", 1))) {
+		dev_err(dev, "Only one other master is supported\n");
+		return -EINVAL;
+	}
+
+	/* Arbitration parameters */
+	if (of_property_read_u32(np, "slew-delay-us", &arb->slew_delay_us))
+		arb->slew_delay_us = 10;
+	if (of_property_read_u32(np, "wait-retry-us", &arb->wait_retry_us))
+		arb->wait_retry_us = 3000;
+	if (of_property_read_u32(np, "wait-free-us", &arb->wait_free_us))
+		arb->wait_free_us = 50000;
+
+	/* Find our parent */
+	parent_np = of_parse_phandle(np, "i2c-parent", 0);
+	if (!parent_np) {
+		dev_err(dev, "Cannot parse i2c-parent\n");
+		return -EINVAL;
+	}
+	arb->parent = of_find_i2c_adapter_by_node(parent_np);
+	if (!arb->parent) {
+		dev_err(dev, "Cannot find parent bus\n");
+		return -EINVAL;
+	}
+
+	/* Actually add the mux adapter */
+	arb->child = i2c_add_mux_adapter(arb->parent, dev, arb, 0, 0, 0,
+					 i2c_arbitrator_select,
+					 i2c_arbitrator_deselect);
+	if (!arb->child) {
+		dev_err(dev, "Failed to add adapter\n");
+		ret = -ENODEV;
+		i2c_put_adapter(arb->parent);
+	}
+
+	return ret;
+}
+
+static int i2c_arbitrator_remove(struct platform_device *pdev)
+{
+	struct i2c_arbitrator_data *arb = platform_get_drvdata(pdev);
+
+	i2c_del_mux_adapter(arb->child);
+	i2c_put_adapter(arb->parent);
+
+	return 0;
+}
+
+static const struct of_device_id i2c_arbitrator_of_match[] = {
+	{ .compatible = "i2c-arb-gpio-challenge", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, i2c_arbitrator_of_match);
+
+static struct platform_driver i2c_arbitrator_driver = {
+	.probe	= i2c_arbitrator_probe,
+	.remove	= i2c_arbitrator_remove,
+	.driver	= {
+		.owner	= THIS_MODULE,
+		.name	= "i2c-arb-gpio-challenge",
+		.of_match_table = of_match_ptr(i2c_arbitrator_of_match),
+	},
+};
+
+module_platform_driver(i2c_arbitrator_driver);
+
+MODULE_DESCRIPTION("GPIO-based I2C Arbitration");
+MODULE_AUTHOR("Doug Anderson <dianders@chromium.org>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:i2c-arb-gpio-challenge");
diff --git a/drivers/i2c/muxes/i2c-mux-gpio.c b/drivers/i2c/muxes/i2c-mux-gpio.c
index abc2e55aa243..5a0ce0081dce 100644
--- a/drivers/i2c/muxes/i2c-mux-gpio.c
+++ b/drivers/i2c/muxes/i2c-mux-gpio.c
@@ -201,10 +201,21 @@ static int i2c_mux_gpio_probe(struct platform_device *pdev)
 
 	for (i = 0; i < mux->data.n_gpios; i++) {
 		ret = gpio_request(gpio_base + mux->data.gpios[i], "i2c-mux-gpio");
-		if (ret)
+		if (ret) {
+			dev_err(&pdev->dev, "Failed to request GPIO %d\n",
+				mux->data.gpios[i]);
 			goto err_request_gpio;
-		gpio_direction_output(gpio_base + mux->data.gpios[i],
-				      initial_state & (1 << i));
+		}
+
+		ret = gpio_direction_output(gpio_base + mux->data.gpios[i],
+					    initial_state & (1 << i));
+		if (ret) {
+			dev_err(&pdev->dev,
+				"Failed to set direction of GPIO %d to output\n",
+				mux->data.gpios[i]);
+			i++;	/* gpio_request above succeeded, so must free */
+			goto err_request_gpio;
+		}
 	}
 
 	for (i = 0; i < mux->data.n_values; i++) {
diff --git a/drivers/i2c/muxes/i2c-mux-pca954x.c b/drivers/i2c/muxes/i2c-mux-pca954x.c
index 8e4387235b69..a531d801dbe4 100644
--- a/drivers/i2c/muxes/i2c-mux-pca954x.c
+++ b/drivers/i2c/muxes/i2c-mux-pca954x.c
@@ -262,13 +262,11 @@ static int pca954x_remove(struct i2c_client *client)
 {
 	struct pca954x *data = i2c_get_clientdata(client);
 	const struct chip_desc *chip = &chips[data->type];
-	int i, err;
+	int i;
 
 	for (i = 0; i < chip->nchans; ++i)
 		if (data->virt_adaps[i]) {
-			err = i2c_del_mux_adapter(data->virt_adaps[i]);
-			if (err)
-				return err;
+			i2c_del_mux_adapter(data->virt_adaps[i]);
 			data->virt_adaps[i] = NULL;
 		}
 
diff --git a/drivers/ide/ide-cd.c b/drivers/ide/ide-cd.c
index 8126824daccb..2ff620444930 100644
--- a/drivers/ide/ide-cd.c
+++ b/drivers/ide/ide-cd.c
@@ -1408,7 +1408,7 @@ static int idecd_capacity_proc_show(struct seq_file *m, void *v)
 
 static int idecd_capacity_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, idecd_capacity_proc_show, PDE(inode)->data);
+	return single_open(file, idecd_capacity_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations idecd_capacity_proc_fops = {
@@ -1606,7 +1606,7 @@ out:
 	return rc;
 }
 
-static int idecd_release(struct gendisk *disk, fmode_t mode)
+static void idecd_release(struct gendisk *disk, fmode_t mode)
 {
 	struct cdrom_info *info = ide_drv_g(disk, cdrom_info);
 
@@ -1615,8 +1615,6 @@ static int idecd_release(struct gendisk *disk, fmode_t mode)
 
 	ide_cd_put(info);
 	mutex_unlock(&ide_cd_mutex);
-
-	return 0;
 }
 
 static int idecd_set_spindown(struct cdrom_device_info *cdi, unsigned long arg)
diff --git a/drivers/ide/ide-disk_proc.c b/drivers/ide/ide-disk_proc.c
index 8b570a17bcd9..0d1fae6cba6d 100644
--- a/drivers/ide/ide-disk_proc.c
+++ b/drivers/ide/ide-disk_proc.c
@@ -53,7 +53,7 @@ static int idedisk_cache_proc_show(struct seq_file *m, void *v)
 
 static int idedisk_cache_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, idedisk_cache_proc_show, PDE(inode)->data);
+	return single_open(file, idedisk_cache_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations idedisk_cache_proc_fops = {
@@ -74,7 +74,7 @@ static int idedisk_capacity_proc_show(struct seq_file *m, void *v)
 
 static int idedisk_capacity_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, idedisk_capacity_proc_show, PDE(inode)->data);
+	return single_open(file, idedisk_capacity_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations idedisk_capacity_proc_fops = {
@@ -115,7 +115,7 @@ static int idedisk_sv_proc_show(struct seq_file *m, void *v)
 
 static int idedisk_sv_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, idedisk_sv_proc_show, PDE(inode)->data);
+	return single_open(file, idedisk_sv_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations idedisk_sv_proc_fops = {
@@ -133,7 +133,7 @@ static int idedisk_st_proc_show(struct seq_file *m, void *v)
 
 static int idedisk_st_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, idedisk_st_proc_show, PDE(inode)->data);
+	return single_open(file, idedisk_st_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations idedisk_st_proc_fops = {
diff --git a/drivers/ide/ide-floppy_proc.c b/drivers/ide/ide-floppy_proc.c
index 1600720f3e86..e7a25ea757df 100644
--- a/drivers/ide/ide-floppy_proc.c
+++ b/drivers/ide/ide-floppy_proc.c
@@ -15,7 +15,7 @@ static int idefloppy_capacity_proc_show(struct seq_file *m, void *v)
 
 static int idefloppy_capacity_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, idefloppy_capacity_proc_show, PDE(inode)->data);
+	return single_open(file, idefloppy_capacity_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations idefloppy_capacity_proc_fops = {
diff --git a/drivers/ide/ide-gd.c b/drivers/ide/ide-gd.c
index 70ea8763567d..de86631e767d 100644
--- a/drivers/ide/ide-gd.c
+++ b/drivers/ide/ide-gd.c
@@ -250,7 +250,7 @@ static int ide_gd_unlocked_open(struct block_device *bdev, fmode_t mode)
 }
 
 
-static int ide_gd_release(struct gendisk *disk, fmode_t mode)
+static void ide_gd_release(struct gendisk *disk, fmode_t mode)
 {
 	struct ide_disk_obj *idkp = ide_drv_g(disk, ide_disk_obj);
 	ide_drive_t *drive = idkp->drive;
@@ -270,8 +270,6 @@ static int ide_gd_release(struct gendisk *disk, fmode_t mode)
 
 	ide_disk_put(idkp);
 	mutex_unlock(&ide_gd_mutex);
-
-	return 0;
 }
 
 static int ide_gd_getgeo(struct block_device *bdev, struct hd_geometry *geo)
diff --git a/drivers/ide/ide-proc.c b/drivers/ide/ide-proc.c
index 2abcc4790f12..97c070077774 100644
--- a/drivers/ide/ide-proc.c
+++ b/drivers/ide/ide-proc.c
@@ -58,7 +58,7 @@ static int ide_imodel_proc_show(struct seq_file *m, void *v)
 
 static int ide_imodel_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, ide_imodel_proc_show, PDE(inode)->data);
+	return single_open(file, ide_imodel_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations ide_imodel_proc_fops = {
@@ -82,7 +82,7 @@ static int ide_mate_proc_show(struct seq_file *m, void *v)
 
 static int ide_mate_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, ide_mate_proc_show, PDE(inode)->data);
+	return single_open(file, ide_mate_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations ide_mate_proc_fops = {
@@ -103,7 +103,7 @@ static int ide_channel_proc_show(struct seq_file *m, void *v)
 
 static int ide_channel_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, ide_channel_proc_show, PDE(inode)->data);
+	return single_open(file, ide_channel_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations ide_channel_proc_fops = {
@@ -143,7 +143,7 @@ static int ide_identify_proc_show(struct seq_file *m, void *v)
 
 static int ide_identify_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, ide_identify_proc_show, PDE(inode)->data);
+	return single_open(file, ide_identify_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations ide_identify_proc_fops = {
@@ -325,7 +325,7 @@ static int ide_settings_proc_show(struct seq_file *m, void *v)
 
 static int ide_settings_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, ide_settings_proc_show, PDE(inode)->data);
+	return single_open(file, ide_settings_proc_show, PDE_DATA(inode));
 }
 
 #define MAX_LEN	30
@@ -333,7 +333,7 @@ static int ide_settings_proc_open(struct inode *inode, struct file *file)
 static ssize_t ide_settings_proc_write(struct file *file, const char __user *buffer,
 				       size_t count, loff_t *pos)
 {
-	ide_drive_t	*drive = (ide_drive_t *) PDE(file_inode(file))->data;
+	ide_drive_t	*drive = PDE_DATA(file_inode(file));
 	char		name[MAX_LEN + 1];
 	int		for_real = 0, mul_factor, div_factor;
 	unsigned long	n;
@@ -474,7 +474,7 @@ static int ide_geometry_proc_show(struct seq_file *m, void *v)
 
 static int ide_geometry_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, ide_geometry_proc_show, PDE(inode)->data);
+	return single_open(file, ide_geometry_proc_show, PDE_DATA(inode));
 }
 
 const struct file_operations ide_geometry_proc_fops = {
@@ -497,7 +497,7 @@ static int ide_dmodel_proc_show(struct seq_file *seq, void *v)
 
 static int ide_dmodel_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, ide_dmodel_proc_show, PDE(inode)->data);
+	return single_open(file, ide_dmodel_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations ide_dmodel_proc_fops = {
@@ -525,7 +525,7 @@ static int ide_driver_proc_show(struct seq_file *m, void *v)
 
 static int ide_driver_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, ide_driver_proc_show, PDE(inode)->data);
+	return single_open(file, ide_driver_proc_show, PDE_DATA(inode));
 }
 
 static int ide_replace_subdriver(ide_drive_t *drive, const char *driver)
@@ -558,7 +558,7 @@ static int ide_replace_subdriver(ide_drive_t *drive, const char *driver)
 static ssize_t ide_driver_proc_write(struct file *file, const char __user *buffer,
 				     size_t count, loff_t *pos)
 {
-	ide_drive_t	*drive = (ide_drive_t *) PDE(file_inode(file))->data;
+	ide_drive_t	*drive = PDE_DATA(file_inode(file));
 	char name[32];
 
 	if (!capable(CAP_SYS_ADMIN))
@@ -601,7 +601,7 @@ static int ide_media_proc_show(struct seq_file *m, void *v)
 
 static int ide_media_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, ide_media_proc_show, PDE(inode)->data);
+	return single_open(file, ide_media_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations ide_media_proc_fops = {
diff --git a/drivers/ide/ide-tape.c b/drivers/ide/ide-tape.c
index ce8237d36159..c6c574bd5f59 100644
--- a/drivers/ide/ide-tape.c
+++ b/drivers/ide/ide-tape.c
@@ -1847,7 +1847,7 @@ static int idetape_name_proc_show(struct seq_file *m, void *v)
 
 static int idetape_name_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, idetape_name_proc_show, PDE(inode)->data);
+	return single_open(file, idetape_name_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations idetape_name_proc_fops = {
@@ -1918,15 +1918,13 @@ static int idetape_open(struct block_device *bdev, fmode_t mode)
 	return 0;
 }
 
-static int idetape_release(struct gendisk *disk, fmode_t mode)
+static void idetape_release(struct gendisk *disk, fmode_t mode)
 {
 	struct ide_tape_obj *tape = ide_drv_g(disk, ide_tape_obj);
 
 	mutex_lock(&ide_tape_mutex);
 	ide_tape_put(tape);
 	mutex_unlock(&ide_tape_mutex);
-
-	return 0;
 }
 
 static int idetape_ioctl(struct block_device *bdev, fmode_t mode,
diff --git a/drivers/idle/intel_idle.c b/drivers/idle/intel_idle.c
index 1a38dd7dfe4e..0e8fab1913df 100644
--- a/drivers/idle/intel_idle.c
+++ b/drivers/idle/intel_idle.c
@@ -71,7 +71,6 @@
 static struct cpuidle_driver intel_idle_driver = {
 	.name = "intel_idle",
 	.owner = THIS_MODULE,
-	.en_core_tk_irqen = 1,
 };
 /* intel_idle.max_cstate=0 disables driver */
 static int max_cstate = CPUIDLE_STATE_MAX - 1;
@@ -339,7 +338,6 @@ static int intel_idle(struct cpuidle_device *dev,
 	if (!(lapic_timer_reliable_states & (1 << (cstate))))
 		clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &cpu);
 
-	stop_critical_timings();
 	if (!need_resched()) {
 
 		__monitor((void *)&current_thread_info()->flags, 0, 0);
@@ -348,8 +346,6 @@ static int intel_idle(struct cpuidle_device *dev,
 			__mwait(eax, ecx);
 	}
 
-	start_critical_timings();
-
 	if (!(lapic_timer_reliable_states & (1 << (cstate))))
 		clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &cpu);
 
diff --git a/drivers/infiniband/Kconfig b/drivers/infiniband/Kconfig
index a0f29c1d03bc..c85b56c28099 100644
--- a/drivers/infiniband/Kconfig
+++ b/drivers/infiniband/Kconfig
@@ -59,5 +59,6 @@ source "drivers/infiniband/ulp/srp/Kconfig"
 source "drivers/infiniband/ulp/srpt/Kconfig"
 
 source "drivers/infiniband/ulp/iser/Kconfig"
+source "drivers/infiniband/ulp/isert/Kconfig"
 
 endif # INFINIBAND
diff --git a/drivers/infiniband/Makefile b/drivers/infiniband/Makefile
index bf846a14b9d3..b126fefe0b1c 100644
--- a/drivers/infiniband/Makefile
+++ b/drivers/infiniband/Makefile
@@ -13,3 +13,4 @@ obj-$(CONFIG_INFINIBAND_IPOIB)		+= ulp/ipoib/
 obj-$(CONFIG_INFINIBAND_SRP)		+= ulp/srp/
 obj-$(CONFIG_INFINIBAND_SRPT)		+= ulp/srpt/
 obj-$(CONFIG_INFINIBAND_ISER)		+= ulp/iser/
+obj-$(CONFIG_INFINIBAND_ISERT)		+= ulp/isert/
diff --git a/drivers/infiniband/core/iwcm.c b/drivers/infiniband/core/iwcm.c
index 0bb99bb38809..c47c2034ca71 100644
--- a/drivers/infiniband/core/iwcm.c
+++ b/drivers/infiniband/core/iwcm.c
@@ -878,6 +878,8 @@ static void cm_work_handler(struct work_struct *_work)
 			}
 			return;
 		}
+		if (empty)
+			return;
 		spin_lock_irqsave(&cm_id_priv->lock, flags);
 	}
 	spin_unlock_irqrestore(&cm_id_priv->lock, flags);
diff --git a/drivers/infiniband/core/verbs.c b/drivers/infiniband/core/verbs.c
index a8fdd3381405..22192deb8828 100644
--- a/drivers/infiniband/core/verbs.c
+++ b/drivers/infiniband/core/verbs.c
@@ -348,7 +348,8 @@ static void __ib_shared_qp_event_handler(struct ib_event *event, void *context)
 	struct ib_qp *qp = context;
 
 	list_for_each_entry(event->element.qp, &qp->open_list, open_list)
-		event->element.qp->event_handler(event, event->element.qp->qp_context);
+		if (event->element.qp->event_handler)
+			event->element.qp->event_handler(event, event->element.qp->qp_context);
 }
 
 static void __ib_insert_xrcd_qp(struct ib_xrcd *xrcd, struct ib_qp *qp)
diff --git a/drivers/infiniband/hw/cxgb3/cxio_resource.c b/drivers/infiniband/hw/cxgb3/cxio_resource.c
index 31f9201b2980..c40088ecf9f3 100644
--- a/drivers/infiniband/hw/cxgb3/cxio_resource.c
+++ b/drivers/infiniband/hw/cxgb3/cxio_resource.c
@@ -62,13 +62,13 @@ static int __cxio_init_resource_fifo(struct kfifo *fifo,
 		kfifo_in(fifo, (unsigned char *) &entry, sizeof(u32));
 	if (random) {
 		j = 0;
-		random_bytes = random32();
+		random_bytes = prandom_u32();
 		for (i = 0; i < RANDOM_SIZE; i++)
 			rarray[i] = i + skip_low;
 		for (i = skip_low + RANDOM_SIZE; i < nr - skip_high; i++) {
 			if (j >= RANDOM_SIZE) {
 				j = 0;
-				random_bytes = random32();
+				random_bytes = prandom_u32();
 			}
 			idx = (random_bytes >> (j * 2)) & 0xF;
 			kfifo_in(fifo,
diff --git a/drivers/infiniband/hw/cxgb3/iwch_provider.c b/drivers/infiniband/hw/cxgb3/iwch_provider.c
index 9c12da0cbd32..e87f2201b220 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_provider.c
+++ b/drivers/infiniband/hw/cxgb3/iwch_provider.c
@@ -559,7 +559,7 @@ static int iwch_reregister_phys_mem(struct ib_mr *mr,
 	__be64 *page_list = NULL;
 	int shift = 0;
 	u64 total_size;
-	int npages;
+	int npages = 0;
 	int ret;
 
 	PDBG("%s ib_mr %p ib_pd %p\n", __func__, mr, pd);
diff --git a/drivers/infiniband/hw/cxgb4/cm.c b/drivers/infiniband/hw/cxgb4/cm.c
index a3fde52840ca..65c30ea8c1a1 100644
--- a/drivers/infiniband/hw/cxgb4/cm.c
+++ b/drivers/infiniband/hw/cxgb4/cm.c
@@ -511,12 +511,16 @@ static unsigned int select_ntuple(struct c4iw_dev *dev, struct dst_entry *dst,
 static int send_connect(struct c4iw_ep *ep)
 {
 	struct cpl_act_open_req *req;
+	struct cpl_t5_act_open_req *t5_req;
 	struct sk_buff *skb;
 	u64 opt0;
 	u32 opt2;
 	unsigned int mtu_idx;
 	int wscale;
-	int wrlen = roundup(sizeof *req, 16);
+	int size = is_t4(ep->com.dev->rdev.lldi.adapter_type) ?
+		sizeof(struct cpl_act_open_req) :
+		sizeof(struct cpl_t5_act_open_req);
+	int wrlen = roundup(size, 16);
 
 	PDBG("%s ep %p atid %u\n", __func__, ep, ep->atid);
 
@@ -552,17 +556,36 @@ static int send_connect(struct c4iw_ep *ep)
 		opt2 |= WND_SCALE_EN(1);
 	t4_set_arp_err_handler(skb, NULL, act_open_req_arp_failure);
 
-	req = (struct cpl_act_open_req *) skb_put(skb, wrlen);
-	INIT_TP_WR(req, 0);
-	OPCODE_TID(req) = cpu_to_be32(
-		MK_OPCODE_TID(CPL_ACT_OPEN_REQ, ((ep->rss_qid<<14)|ep->atid)));
-	req->local_port = ep->com.local_addr.sin_port;
-	req->peer_port = ep->com.remote_addr.sin_port;
-	req->local_ip = ep->com.local_addr.sin_addr.s_addr;
-	req->peer_ip = ep->com.remote_addr.sin_addr.s_addr;
-	req->opt0 = cpu_to_be64(opt0);
-	req->params = cpu_to_be32(select_ntuple(ep->com.dev, ep->dst, ep->l2t));
-	req->opt2 = cpu_to_be32(opt2);
+	if (is_t4(ep->com.dev->rdev.lldi.adapter_type)) {
+		req = (struct cpl_act_open_req *) skb_put(skb, wrlen);
+		INIT_TP_WR(req, 0);
+		OPCODE_TID(req) = cpu_to_be32(
+				MK_OPCODE_TID(CPL_ACT_OPEN_REQ,
+				((ep->rss_qid << 14) | ep->atid)));
+		req->local_port = ep->com.local_addr.sin_port;
+		req->peer_port = ep->com.remote_addr.sin_port;
+		req->local_ip = ep->com.local_addr.sin_addr.s_addr;
+		req->peer_ip = ep->com.remote_addr.sin_addr.s_addr;
+		req->opt0 = cpu_to_be64(opt0);
+		req->params = cpu_to_be32(select_ntuple(ep->com.dev,
+					ep->dst, ep->l2t));
+		req->opt2 = cpu_to_be32(opt2);
+	} else {
+		t5_req = (struct cpl_t5_act_open_req *) skb_put(skb, wrlen);
+		INIT_TP_WR(t5_req, 0);
+		OPCODE_TID(t5_req) = cpu_to_be32(
+					MK_OPCODE_TID(CPL_ACT_OPEN_REQ,
+					((ep->rss_qid << 14) | ep->atid)));
+		t5_req->local_port = ep->com.local_addr.sin_port;
+		t5_req->peer_port = ep->com.remote_addr.sin_port;
+		t5_req->local_ip = ep->com.local_addr.sin_addr.s_addr;
+		t5_req->peer_ip = ep->com.remote_addr.sin_addr.s_addr;
+		t5_req->opt0 = cpu_to_be64(opt0);
+		t5_req->params = cpu_to_be64(V_FILTER_TUPLE(
+				select_ntuple(ep->com.dev, ep->dst, ep->l2t)));
+		t5_req->opt2 = cpu_to_be32(opt2);
+	}
+
 	set_bit(ACT_OPEN_REQ, &ep->com.history);
 	return c4iw_l2t_send(&ep->com.dev->rdev, skb, ep->l2t);
 }
@@ -1676,9 +1699,9 @@ static int act_open_rpl(struct c4iw_dev *dev, struct sk_buff *skb)
 	case CPL_ERR_CONN_TIMEDOUT:
 		break;
 	case CPL_ERR_TCAM_FULL:
+		dev->rdev.stats.tcam_full++;
 		if (dev->rdev.lldi.enable_fw_ofld_conn) {
 			mutex_lock(&dev->rdev.stats.lock);
-			dev->rdev.stats.tcam_full++;
 			mutex_unlock(&dev->rdev.stats.lock);
 			send_fw_act_open_req(ep,
 					     GET_TID_TID(GET_AOPEN_ATID(
@@ -2875,12 +2898,14 @@ static int deferred_fw6_msg(struct c4iw_dev *dev, struct sk_buff *skb)
 static void build_cpl_pass_accept_req(struct sk_buff *skb, int stid , u8 tos)
 {
 	u32 l2info;
-	u16 vlantag, len, hdr_len;
+	u16 vlantag, len, hdr_len, eth_hdr_len;
 	u8 intf;
 	struct cpl_rx_pkt *cpl = cplhdr(skb);
 	struct cpl_pass_accept_req *req;
 	struct tcp_options_received tmp_opt;
+	struct c4iw_dev *dev;
 
+	dev = *((struct c4iw_dev **) (skb->cb + sizeof(void *)));
 	/* Store values from cpl_rx_pkt in temporary location. */
 	vlantag = (__force u16) cpl->vlan;
 	len = (__force u16) cpl->len;
@@ -2896,7 +2921,7 @@ static void build_cpl_pass_accept_req(struct sk_buff *skb, int stid , u8 tos)
 	 */
 	memset(&tmp_opt, 0, sizeof(tmp_opt));
 	tcp_clear_options(&tmp_opt);
-	tcp_parse_options(skb, &tmp_opt, NULL, 0, NULL);
+	tcp_parse_options(skb, &tmp_opt, 0, NULL);
 
 	req = (struct cpl_pass_accept_req *)__skb_push(skb, sizeof(*req));
 	memset(req, 0, sizeof(*req));
@@ -2904,14 +2929,16 @@ static void build_cpl_pass_accept_req(struct sk_buff *skb, int stid , u8 tos)
 			 V_SYN_MAC_IDX(G_RX_MACIDX(
 			 (__force int) htonl(l2info))) |
 			 F_SYN_XACT_MATCH);
+	eth_hdr_len = is_t4(dev->rdev.lldi.adapter_type) ?
+			    G_RX_ETHHDR_LEN((__force int) htonl(l2info)) :
+			    G_RX_T5_ETHHDR_LEN((__force int) htonl(l2info));
 	req->hdr_len = cpu_to_be32(V_SYN_RX_CHAN(G_RX_CHAN(
 					(__force int) htonl(l2info))) |
 				   V_TCP_HDR_LEN(G_RX_TCPHDR_LEN(
 					(__force int) htons(hdr_len))) |
 				   V_IP_HDR_LEN(G_RX_IPHDR_LEN(
 					(__force int) htons(hdr_len))) |
-				   V_ETH_HDR_LEN(G_RX_ETHHDR_LEN(
-					(__force int) htonl(l2info))));
+				   V_ETH_HDR_LEN(G_RX_ETHHDR_LEN(eth_hdr_len)));
 	req->vlan = (__force __be16) vlantag;
 	req->len = (__force __be16) len;
 	req->tos_stid = cpu_to_be32(PASS_OPEN_TID(stid) |
@@ -2999,7 +3026,7 @@ static int rx_pkt(struct c4iw_dev *dev, struct sk_buff *skb)
 	u16 window;
 	struct port_info *pi;
 	struct net_device *pdev;
-	u16 rss_qid;
+	u16 rss_qid, eth_hdr_len;
 	int step;
 	u32 tx_chan;
 	struct neighbour *neigh;
@@ -3028,7 +3055,10 @@ static int rx_pkt(struct c4iw_dev *dev, struct sk_buff *skb)
 		goto reject;
 	}
 
-	if (G_RX_ETHHDR_LEN(ntohl(cpl->l2info)) == ETH_HLEN) {
+	eth_hdr_len = is_t4(dev->rdev.lldi.adapter_type) ?
+			    G_RX_ETHHDR_LEN(htonl(cpl->l2info)) :
+			    G_RX_T5_ETHHDR_LEN(htonl(cpl->l2info));
+	if (eth_hdr_len == ETH_HLEN) {
 		eh = (struct ethhdr *)(req + 1);
 		iph = (struct iphdr *)(eh + 1);
 	} else {
diff --git a/drivers/infiniband/hw/cxgb4/device.c b/drivers/infiniband/hw/cxgb4/device.c
index 80069ad595c1..ae656016e1ae 100644
--- a/drivers/infiniband/hw/cxgb4/device.c
+++ b/drivers/infiniband/hw/cxgb4/device.c
@@ -41,10 +41,20 @@
 #define DRV_VERSION "0.1"
 
 MODULE_AUTHOR("Steve Wise");
-MODULE_DESCRIPTION("Chelsio T4 RDMA Driver");
+MODULE_DESCRIPTION("Chelsio T4/T5 RDMA Driver");
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_VERSION(DRV_VERSION);
 
+static int allow_db_fc_on_t5;
+module_param(allow_db_fc_on_t5, int, 0644);
+MODULE_PARM_DESC(allow_db_fc_on_t5,
+		 "Allow DB Flow Control on T5 (default = 0)");
+
+static int allow_db_coalescing_on_t5;
+module_param(allow_db_coalescing_on_t5, int, 0644);
+MODULE_PARM_DESC(allow_db_coalescing_on_t5,
+		 "Allow DB Coalescing on T5 (default = 0)");
+
 struct uld_ctx {
 	struct list_head entry;
 	struct cxgb4_lld_info lldi;
@@ -614,7 +624,7 @@ static int rdma_supported(const struct cxgb4_lld_info *infop)
 {
 	return infop->vr->stag.size > 0 && infop->vr->pbl.size > 0 &&
 	       infop->vr->rq.size > 0 && infop->vr->qp.size > 0 &&
-	       infop->vr->cq.size > 0 && infop->vr->ocq.size > 0;
+	       infop->vr->cq.size > 0;
 }
 
 static struct c4iw_dev *c4iw_alloc(const struct cxgb4_lld_info *infop)
@@ -627,6 +637,22 @@ static struct c4iw_dev *c4iw_alloc(const struct cxgb4_lld_info *infop)
 		       pci_name(infop->pdev));
 		return ERR_PTR(-ENOSYS);
 	}
+	if (!ocqp_supported(infop))
+		pr_info("%s: On-Chip Queues not supported on this device.\n",
+			pci_name(infop->pdev));
+
+	if (!is_t4(infop->adapter_type)) {
+		if (!allow_db_fc_on_t5) {
+			db_fc_threshold = 100000;
+			pr_info("DB Flow Control Disabled.\n");
+		}
+
+		if (!allow_db_coalescing_on_t5) {
+			db_coalescing_threshold = -1;
+			pr_info("DB Coalescing Disabled.\n");
+		}
+	}
+
 	devp = (struct c4iw_dev *)ib_alloc_device(sizeof(*devp));
 	if (!devp) {
 		printk(KERN_ERR MOD "Cannot allocate ib device\n");
@@ -678,8 +704,8 @@ static void *c4iw_uld_add(const struct cxgb4_lld_info *infop)
 	int i;
 
 	if (!vers_printed++)
-		printk(KERN_INFO MOD "Chelsio T4 RDMA Driver - version %s\n",
-		       DRV_VERSION);
+		pr_info("Chelsio T4/T5 RDMA Driver - version %s\n",
+			DRV_VERSION);
 
 	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
 	if (!ctx) {
diff --git a/drivers/infiniband/hw/cxgb4/id_table.c b/drivers/infiniband/hw/cxgb4/id_table.c
index f95e5df30db2..0161ae6ad629 100644
--- a/drivers/infiniband/hw/cxgb4/id_table.c
+++ b/drivers/infiniband/hw/cxgb4/id_table.c
@@ -54,7 +54,7 @@ u32 c4iw_id_alloc(struct c4iw_id_table *alloc)
 
 	if (obj < alloc->max) {
 		if (alloc->flags & C4IW_ID_TABLE_F_RANDOM)
-			alloc->last += random32() % RANDOM_SKIP;
+			alloc->last += prandom_u32() % RANDOM_SKIP;
 		else
 			alloc->last = obj + 1;
 		if (alloc->last >= alloc->max)
@@ -88,7 +88,7 @@ int c4iw_id_table_alloc(struct c4iw_id_table *alloc, u32 start, u32 num,
 	alloc->start = start;
 	alloc->flags = flags;
 	if (flags & C4IW_ID_TABLE_F_RANDOM)
-		alloc->last = random32() % RANDOM_SKIP;
+		alloc->last = prandom_u32() % RANDOM_SKIP;
 	else
 		alloc->last = 0;
 	alloc->max  = num;
diff --git a/drivers/infiniband/hw/cxgb4/iw_cxgb4.h b/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
index 7eec5e13fa8c..485183ad34cd 100644
--- a/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
+++ b/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
@@ -162,7 +162,7 @@ static inline int c4iw_num_stags(struct c4iw_rdev *rdev)
 	return min((int)T4_MAX_NUM_STAG, (int)(rdev->lldi.vr->stag.size >> 5));
 }
 
-#define C4IW_WR_TO (10*HZ)
+#define C4IW_WR_TO (30*HZ)
 
 struct c4iw_wr_wait {
 	struct completion completion;
@@ -369,7 +369,6 @@ struct c4iw_fr_page_list {
 	DEFINE_DMA_UNMAP_ADDR(mapping);
 	dma_addr_t dma_addr;
 	struct c4iw_dev *dev;
-	int size;
 };
 
 static inline struct c4iw_fr_page_list *to_c4iw_fr_page_list(
@@ -817,6 +816,15 @@ static inline int compute_wscale(int win)
 	return wscale;
 }
 
+static inline int ocqp_supported(const struct cxgb4_lld_info *infop)
+{
+#if defined(__i386__) || defined(__x86_64__) || defined(CONFIG_PPC64)
+	return infop->vr->ocq.size > 0;
+#else
+	return 0;
+#endif
+}
+
 u32 c4iw_id_alloc(struct c4iw_id_table *alloc);
 void c4iw_id_free(struct c4iw_id_table *alloc, u32 obj);
 int c4iw_id_table_alloc(struct c4iw_id_table *alloc, u32 start, u32 num,
@@ -930,6 +938,8 @@ extern struct cxgb4_client t4c_client;
 extern c4iw_handler_func c4iw_handlers[NUM_CPL_CMDS];
 extern int c4iw_max_read_depth;
 extern int db_fc_threshold;
+extern int db_coalescing_threshold;
+extern int use_dsgl;
 
 
 #endif
diff --git a/drivers/infiniband/hw/cxgb4/mem.c b/drivers/infiniband/hw/cxgb4/mem.c
index 903a92d6f91d..4cb8eb24497c 100644
--- a/drivers/infiniband/hw/cxgb4/mem.c
+++ b/drivers/infiniband/hw/cxgb4/mem.c
@@ -30,16 +30,76 @@
  * SOFTWARE.
  */
 
+#include <linux/module.h>
+#include <linux/moduleparam.h>
 #include <rdma/ib_umem.h>
 #include <linux/atomic.h>
 
 #include "iw_cxgb4.h"
 
+int use_dsgl = 1;
+module_param(use_dsgl, int, 0644);
+MODULE_PARM_DESC(use_dsgl, "Use DSGL for PBL/FastReg (default=1)");
+
 #define T4_ULPTX_MIN_IO 32
 #define C4IW_MAX_INLINE_SIZE 96
+#define T4_ULPTX_MAX_DMA 1024
+#define C4IW_INLINE_THRESHOLD 128
 
-static int write_adapter_mem(struct c4iw_rdev *rdev, u32 addr, u32 len,
-			     void *data)
+static int inline_threshold = C4IW_INLINE_THRESHOLD;
+module_param(inline_threshold, int, 0644);
+MODULE_PARM_DESC(inline_threshold, "inline vs dsgl threshold (default=128)");
+
+static int _c4iw_write_mem_dma_aligned(struct c4iw_rdev *rdev, u32 addr,
+				       u32 len, dma_addr_t data, int wait)
+{
+	struct sk_buff *skb;
+	struct ulp_mem_io *req;
+	struct ulptx_sgl *sgl;
+	u8 wr_len;
+	int ret = 0;
+	struct c4iw_wr_wait wr_wait;
+
+	addr &= 0x7FFFFFF;
+
+	if (wait)
+		c4iw_init_wr_wait(&wr_wait);
+	wr_len = roundup(sizeof(*req) + sizeof(*sgl), 16);
+
+	skb = alloc_skb(wr_len, GFP_KERNEL | __GFP_NOFAIL);
+	if (!skb)
+		return -ENOMEM;
+	set_wr_txq(skb, CPL_PRIORITY_CONTROL, 0);
+
+	req = (struct ulp_mem_io *)__skb_put(skb, wr_len);
+	memset(req, 0, wr_len);
+	INIT_ULPTX_WR(req, wr_len, 0, 0);
+	req->wr.wr_hi = cpu_to_be32(FW_WR_OP(FW_ULPTX_WR) |
+			(wait ? FW_WR_COMPL(1) : 0));
+	req->wr.wr_lo = wait ? (__force __be64)&wr_wait : 0;
+	req->wr.wr_mid = cpu_to_be32(FW_WR_LEN16(DIV_ROUND_UP(wr_len, 16)));
+	req->cmd = cpu_to_be32(ULPTX_CMD(ULP_TX_MEM_WRITE));
+	req->cmd |= cpu_to_be32(V_T5_ULP_MEMIO_ORDER(1));
+	req->dlen = cpu_to_be32(ULP_MEMIO_DATA_LEN(len>>5));
+	req->len16 = cpu_to_be32(DIV_ROUND_UP(wr_len-sizeof(req->wr), 16));
+	req->lock_addr = cpu_to_be32(ULP_MEMIO_ADDR(addr));
+
+	sgl = (struct ulptx_sgl *)(req + 1);
+	sgl->cmd_nsge = cpu_to_be32(ULPTX_CMD(ULP_TX_SC_DSGL) |
+				    ULPTX_NSGE(1));
+	sgl->len0 = cpu_to_be32(len);
+	sgl->addr0 = cpu_to_be64(data);
+
+	ret = c4iw_ofld_send(rdev, skb);
+	if (ret)
+		return ret;
+	if (wait)
+		ret = c4iw_wait_for_reply(rdev, &wr_wait, 0, 0, __func__);
+	return ret;
+}
+
+static int _c4iw_write_mem_inline(struct c4iw_rdev *rdev, u32 addr, u32 len,
+				  void *data)
 {
 	struct sk_buff *skb;
 	struct ulp_mem_io *req;
@@ -47,6 +107,12 @@ static int write_adapter_mem(struct c4iw_rdev *rdev, u32 addr, u32 len,
 	u8 wr_len, *to_dp, *from_dp;
 	int copy_len, num_wqe, i, ret = 0;
 	struct c4iw_wr_wait wr_wait;
+	__be32 cmd = cpu_to_be32(ULPTX_CMD(ULP_TX_MEM_WRITE));
+
+	if (is_t4(rdev->lldi.adapter_type))
+		cmd |= cpu_to_be32(ULP_MEMIO_ORDER(1));
+	else
+		cmd |= cpu_to_be32(V_T5_ULP_MEMIO_IMM(1));
 
 	addr &= 0x7FFFFFF;
 	PDBG("%s addr 0x%x len %u\n", __func__, addr, len);
@@ -77,7 +143,7 @@ static int write_adapter_mem(struct c4iw_rdev *rdev, u32 addr, u32 len,
 		req->wr.wr_mid = cpu_to_be32(
 				       FW_WR_LEN16(DIV_ROUND_UP(wr_len, 16)));
 
-		req->cmd = cpu_to_be32(ULPTX_CMD(ULP_TX_MEM_WRITE) | (1<<23));
+		req->cmd = cmd;
 		req->dlen = cpu_to_be32(ULP_MEMIO_DATA_LEN(
 				DIV_ROUND_UP(copy_len, T4_ULPTX_MIN_IO)));
 		req->len16 = cpu_to_be32(DIV_ROUND_UP(wr_len-sizeof(req->wr),
@@ -107,6 +173,67 @@ static int write_adapter_mem(struct c4iw_rdev *rdev, u32 addr, u32 len,
 	return ret;
 }
 
+int _c4iw_write_mem_dma(struct c4iw_rdev *rdev, u32 addr, u32 len, void *data)
+{
+	u32 remain = len;
+	u32 dmalen;
+	int ret = 0;
+	dma_addr_t daddr;
+	dma_addr_t save;
+
+	daddr = dma_map_single(&rdev->lldi.pdev->dev, data, len, DMA_TO_DEVICE);
+	if (dma_mapping_error(&rdev->lldi.pdev->dev, daddr))
+		return -1;
+	save = daddr;
+
+	while (remain > inline_threshold) {
+		if (remain < T4_ULPTX_MAX_DMA) {
+			if (remain & ~T4_ULPTX_MIN_IO)
+				dmalen = remain & ~(T4_ULPTX_MIN_IO-1);
+			else
+				dmalen = remain;
+		} else
+			dmalen = T4_ULPTX_MAX_DMA;
+		remain -= dmalen;
+		ret = _c4iw_write_mem_dma_aligned(rdev, addr, dmalen, daddr,
+						 !remain);
+		if (ret)
+			goto out;
+		addr += dmalen >> 5;
+		data += dmalen;
+		daddr += dmalen;
+	}
+	if (remain)
+		ret = _c4iw_write_mem_inline(rdev, addr, remain, data);
+out:
+	dma_unmap_single(&rdev->lldi.pdev->dev, save, len, DMA_TO_DEVICE);
+	return ret;
+}
+
+/*
+ * write len bytes of data into addr (32B aligned address)
+ * If data is NULL, clear len byte of memory to zero.
+ */
+static int write_adapter_mem(struct c4iw_rdev *rdev, u32 addr, u32 len,
+			     void *data)
+{
+	if (is_t5(rdev->lldi.adapter_type) && use_dsgl) {
+		if (len > inline_threshold) {
+			if (_c4iw_write_mem_dma(rdev, addr, len, data)) {
+				printk_ratelimited(KERN_WARNING
+						   "%s: dma map"
+						   " failure (non fatal)\n",
+						   pci_name(rdev->lldi.pdev));
+				return _c4iw_write_mem_inline(rdev, addr, len,
+							      data);
+			} else
+				return 0;
+		} else
+			return _c4iw_write_mem_inline(rdev, addr, len, data);
+	} else
+		return _c4iw_write_mem_inline(rdev, addr, len, data);
+}
+
 /*
  * Build and write a TPT entry.
  * IN: stag key, pdid, perm, bind_enabled, zbva, to, len, page_size,
@@ -760,19 +887,23 @@ struct ib_fast_reg_page_list *c4iw_alloc_fastreg_pbl(struct ib_device *device,
 	struct c4iw_fr_page_list *c4pl;
 	struct c4iw_dev *dev = to_c4iw_dev(device);
 	dma_addr_t dma_addr;
-	int size = sizeof *c4pl + page_list_len * sizeof(u64);
+	int pll_len = roundup(page_list_len * sizeof(u64), 32);
 
-	c4pl = dma_alloc_coherent(&dev->rdev.lldi.pdev->dev, size,
-				  &dma_addr, GFP_KERNEL);
+	c4pl = kmalloc(sizeof(*c4pl), GFP_KERNEL);
 	if (!c4pl)
 		return ERR_PTR(-ENOMEM);
 
+	c4pl->ibpl.page_list = dma_alloc_coherent(&dev->rdev.lldi.pdev->dev,
+						  pll_len, &dma_addr,
+						  GFP_KERNEL);
+	if (!c4pl->ibpl.page_list) {
+		kfree(c4pl);
+		return ERR_PTR(-ENOMEM);
+	}
 	dma_unmap_addr_set(c4pl, mapping, dma_addr);
 	c4pl->dma_addr = dma_addr;
 	c4pl->dev = dev;
-	c4pl->size = size;
-	c4pl->ibpl.page_list = (u64 *)(c4pl + 1);
-	c4pl->ibpl.max_page_list_len = page_list_len;
+	c4pl->ibpl.max_page_list_len = pll_len;
 
 	return &c4pl->ibpl;
 }
@@ -781,8 +912,10 @@ void c4iw_free_fastreg_pbl(struct ib_fast_reg_page_list *ibpl)
 {
 	struct c4iw_fr_page_list *c4pl = to_c4iw_fr_page_list(ibpl);
 
-	dma_free_coherent(&c4pl->dev->rdev.lldi.pdev->dev, c4pl->size,
-			  c4pl, dma_unmap_addr(c4pl, mapping));
+	dma_free_coherent(&c4pl->dev->rdev.lldi.pdev->dev,
+			  c4pl->ibpl.max_page_list_len,
+			  c4pl->ibpl.page_list, dma_unmap_addr(c4pl, mapping));
+	kfree(c4pl);
 }
 
 int c4iw_dereg_mr(struct ib_mr *ib_mr)
diff --git a/drivers/infiniband/hw/cxgb4/provider.c b/drivers/infiniband/hw/cxgb4/provider.c
index e084fdc6da7f..7e94c9a656a1 100644
--- a/drivers/infiniband/hw/cxgb4/provider.c
+++ b/drivers/infiniband/hw/cxgb4/provider.c
@@ -162,8 +162,14 @@ static int c4iw_mmap(struct ib_ucontext *context, struct vm_area_struct *vma)
 		 */
 		if (addr >= rdev->oc_mw_pa)
 			vma->vm_page_prot = t4_pgprot_wc(vma->vm_page_prot);
-		else
-			vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+		else {
+			if (is_t5(rdev->lldi.adapter_type))
+				vma->vm_page_prot =
+					t4_pgprot_wc(vma->vm_page_prot);
+			else
+				vma->vm_page_prot =
+					pgprot_noncached(vma->vm_page_prot);
+		}
 		ret = io_remap_pfn_range(vma, vma->vm_start,
 					 addr >> PAGE_SHIFT,
 					 len, vma->vm_page_prot);
@@ -263,7 +269,7 @@ static int c4iw_query_device(struct ib_device *ibdev,
 	dev = to_c4iw_dev(ibdev);
 	memset(props, 0, sizeof *props);
 	memcpy(&props->sys_image_guid, dev->rdev.lldi.ports[0]->dev_addr, 6);
-	props->hw_ver = dev->rdev.lldi.adapter_type;
+	props->hw_ver = CHELSIO_CHIP_RELEASE(dev->rdev.lldi.adapter_type);
 	props->fw_ver = dev->rdev.lldi.fw_vers;
 	props->device_cap_flags = dev->device_cap_flags;
 	props->page_size_cap = T4_PAGESIZE_MASK;
@@ -346,7 +352,8 @@ static ssize_t show_rev(struct device *dev, struct device_attribute *attr,
 	struct c4iw_dev *c4iw_dev = container_of(dev, struct c4iw_dev,
 						 ibdev.dev);
 	PDBG("%s dev 0x%p\n", __func__, dev);
-	return sprintf(buf, "%d\n", c4iw_dev->rdev.lldi.adapter_type);
+	return sprintf(buf, "%d\n",
+		       CHELSIO_CHIP_RELEASE(c4iw_dev->rdev.lldi.adapter_type));
 }
 
 static ssize_t show_fw_ver(struct device *dev, struct device_attribute *attr,
diff --git a/drivers/infiniband/hw/cxgb4/qp.c b/drivers/infiniband/hw/cxgb4/qp.c
index 70b1808a08f4..232040447e8a 100644
--- a/drivers/infiniband/hw/cxgb4/qp.c
+++ b/drivers/infiniband/hw/cxgb4/qp.c
@@ -42,10 +42,21 @@ static int ocqp_support = 1;
 module_param(ocqp_support, int, 0644);
 MODULE_PARM_DESC(ocqp_support, "Support on-chip SQs (default=1)");
 
-int db_fc_threshold = 2000;
+int db_fc_threshold = 1000;
 module_param(db_fc_threshold, int, 0644);
-MODULE_PARM_DESC(db_fc_threshold, "QP count/threshold that triggers automatic "
-		 "db flow control mode (default = 2000)");
+MODULE_PARM_DESC(db_fc_threshold,
+		 "QP count/threshold that triggers"
+		 " automatic db flow control mode (default = 1000)");
+
+int db_coalescing_threshold;
+module_param(db_coalescing_threshold, int, 0644);
+MODULE_PARM_DESC(db_coalescing_threshold,
+		 "QP count/threshold that triggers"
+		 " disabling db coalescing (default = 0)");
+
+static int max_fr_immd = T4_MAX_FR_IMMD;
+module_param(max_fr_immd, int, 0644);
+MODULE_PARM_DESC(max_fr_immd, "fastreg threshold for using DSGL instead of immedate");
 
 static void set_state(struct c4iw_qp *qhp, enum c4iw_qp_state state)
 {
@@ -76,7 +87,7 @@ static void dealloc_sq(struct c4iw_rdev *rdev, struct t4_sq *sq)
 
 static int alloc_oc_sq(struct c4iw_rdev *rdev, struct t4_sq *sq)
 {
-	if (!ocqp_support || !t4_ocqp_supported())
+	if (!ocqp_support || !ocqp_supported(&rdev->lldi))
 		return -ENOSYS;
 	sq->dma_addr = c4iw_ocqp_pool_alloc(rdev, sq->memsize);
 	if (!sq->dma_addr)
@@ -100,6 +111,16 @@ static int alloc_host_sq(struct c4iw_rdev *rdev, struct t4_sq *sq)
 	return 0;
 }
 
+static int alloc_sq(struct c4iw_rdev *rdev, struct t4_sq *sq, int user)
+{
+	int ret = -ENOSYS;
+	if (user)
+		ret = alloc_oc_sq(rdev, sq);
+	if (ret)
+		ret = alloc_host_sq(rdev, sq);
+	return ret;
+}
+
 static int destroy_qp(struct c4iw_rdev *rdev, struct t4_wq *wq,
 		      struct c4iw_dev_ucontext *uctx)
 {
@@ -129,7 +150,7 @@ static int create_qp(struct c4iw_rdev *rdev, struct t4_wq *wq,
 	int wr_len;
 	struct c4iw_wr_wait wr_wait;
 	struct sk_buff *skb;
-	int ret;
+	int ret = 0;
 	int eqsize;
 
 	wq->sq.qid = c4iw_get_qpid(rdev, uctx);
@@ -168,18 +189,9 @@ static int create_qp(struct c4iw_rdev *rdev, struct t4_wq *wq,
 		goto free_sw_rq;
 	}
 
-	if (user) {
-		ret = alloc_oc_sq(rdev, &wq->sq);
-		if (ret)
-			goto free_hwaddr;
-
-		ret = alloc_host_sq(rdev, &wq->sq);
-		if (ret)
-			goto free_sq;
-	} else
-		ret = alloc_host_sq(rdev, &wq->sq);
-		if (ret)
-			goto free_hwaddr;
+	ret = alloc_sq(rdev, &wq->sq, user);
+	if (ret)
+		goto free_hwaddr;
 	memset(wq->sq.queue, 0, wq->sq.memsize);
 	dma_unmap_addr_set(&wq->sq, mapping, wq->sq.dma_addr);
 
@@ -534,7 +546,7 @@ static int build_rdma_recv(struct c4iw_qp *qhp, union t4_recv_wr *wqe,
 }
 
 static int build_fastreg(struct t4_sq *sq, union t4_wr *wqe,
-			 struct ib_send_wr *wr, u8 *len16)
+			 struct ib_send_wr *wr, u8 *len16, u8 t5dev)
 {
 
 	struct fw_ri_immd *imdp;
@@ -556,28 +568,51 @@ static int build_fastreg(struct t4_sq *sq, union t4_wr *wqe,
 	wqe->fr.va_hi = cpu_to_be32(wr->wr.fast_reg.iova_start >> 32);
 	wqe->fr.va_lo_fbo = cpu_to_be32(wr->wr.fast_reg.iova_start &
 					0xffffffff);
-	WARN_ON(pbllen > T4_MAX_FR_IMMD);
-	imdp = (struct fw_ri_immd *)(&wqe->fr + 1);
-	imdp->op = FW_RI_DATA_IMMD;
-	imdp->r1 = 0;
-	imdp->r2 = 0;
-	imdp->immdlen = cpu_to_be32(pbllen);
-	p = (__be64 *)(imdp + 1);
-	rem = pbllen;
-	for (i = 0; i < wr->wr.fast_reg.page_list_len; i++) {
-		*p = cpu_to_be64((u64)wr->wr.fast_reg.page_list->page_list[i]);
-		rem -= sizeof *p;
-		if (++p == (__be64 *)&sq->queue[sq->size])
-			p = (__be64 *)sq->queue;
-	}
-	BUG_ON(rem < 0);
-	while (rem) {
-		*p = 0;
-		rem -= sizeof *p;
-		if (++p == (__be64 *)&sq->queue[sq->size])
-			p = (__be64 *)sq->queue;
+
+	if (t5dev && use_dsgl && (pbllen > max_fr_immd)) {
+		struct c4iw_fr_page_list *c4pl =
+			to_c4iw_fr_page_list(wr->wr.fast_reg.page_list);
+		struct fw_ri_dsgl *sglp;
+
+		for (i = 0; i < wr->wr.fast_reg.page_list_len; i++) {
+			wr->wr.fast_reg.page_list->page_list[i] = (__force u64)
+				cpu_to_be64((u64)
+				wr->wr.fast_reg.page_list->page_list[i]);
+		}
+
+		sglp = (struct fw_ri_dsgl *)(&wqe->fr + 1);
+		sglp->op = FW_RI_DATA_DSGL;
+		sglp->r1 = 0;
+		sglp->nsge = cpu_to_be16(1);
+		sglp->addr0 = cpu_to_be64(c4pl->dma_addr);
+		sglp->len0 = cpu_to_be32(pbllen);
+
+		*len16 = DIV_ROUND_UP(sizeof(wqe->fr) + sizeof(*sglp), 16);
+	} else {
+		imdp = (struct fw_ri_immd *)(&wqe->fr + 1);
+		imdp->op = FW_RI_DATA_IMMD;
+		imdp->r1 = 0;
+		imdp->r2 = 0;
+		imdp->immdlen = cpu_to_be32(pbllen);
+		p = (__be64 *)(imdp + 1);
+		rem = pbllen;
+		for (i = 0; i < wr->wr.fast_reg.page_list_len; i++) {
+			*p = cpu_to_be64(
+				(u64)wr->wr.fast_reg.page_list->page_list[i]);
+			rem -= sizeof(*p);
+			if (++p == (__be64 *)&sq->queue[sq->size])
+				p = (__be64 *)sq->queue;
+		}
+		BUG_ON(rem < 0);
+		while (rem) {
+			*p = 0;
+			rem -= sizeof(*p);
+			if (++p == (__be64 *)&sq->queue[sq->size])
+				p = (__be64 *)sq->queue;
+		}
+		*len16 = DIV_ROUND_UP(sizeof(wqe->fr) + sizeof(*imdp)
+				      + pbllen, 16);
 	}
-	*len16 = DIV_ROUND_UP(sizeof wqe->fr + sizeof *imdp + pbllen, 16);
 	return 0;
 }
 
@@ -678,7 +713,10 @@ int c4iw_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 		case IB_WR_FAST_REG_MR:
 			fw_opcode = FW_RI_FR_NSMR_WR;
 			swsqe->opcode = FW_RI_FAST_REGISTER;
-			err = build_fastreg(&qhp->wq.sq, wqe, wr, &len16);
+			err = build_fastreg(&qhp->wq.sq, wqe, wr, &len16,
+					    is_t5(
+					    qhp->rhp->rdev.lldi.adapter_type) ?
+					    1 : 0);
 			break;
 		case IB_WR_LOCAL_INV:
 			if (wr->send_flags & IB_SEND_FENCE)
@@ -1450,6 +1488,9 @@ int c4iw_destroy_qp(struct ib_qp *ib_qp)
 		rhp->db_state = NORMAL;
 		idr_for_each(&rhp->qpidr, enable_qp_db, NULL);
 	}
+	if (db_coalescing_threshold >= 0)
+		if (rhp->qpcnt <= db_coalescing_threshold)
+			cxgb4_enable_db_coalescing(rhp->rdev.lldi.ports[0]);
 	spin_unlock_irq(&rhp->lock);
 	atomic_dec(&qhp->refcnt);
 	wait_event(qhp->wait, !atomic_read(&qhp->refcnt));
@@ -1561,11 +1602,15 @@ struct ib_qp *c4iw_create_qp(struct ib_pd *pd, struct ib_qp_init_attr *attrs,
 	spin_lock_irq(&rhp->lock);
 	if (rhp->db_state != NORMAL)
 		t4_disable_wq_db(&qhp->wq);
-	if (++rhp->qpcnt > db_fc_threshold && rhp->db_state == NORMAL) {
+	rhp->qpcnt++;
+	if (rhp->qpcnt > db_fc_threshold && rhp->db_state == NORMAL) {
 		rhp->rdev.stats.db_state_transitions++;
 		rhp->db_state = FLOW_CONTROL;
 		idr_for_each(&rhp->qpidr, disable_qp_db, NULL);
 	}
+	if (db_coalescing_threshold >= 0)
+		if (rhp->qpcnt > db_coalescing_threshold)
+			cxgb4_disable_db_coalescing(rhp->rdev.lldi.ports[0]);
 	ret = insert_handle_nolock(rhp, &rhp->qpidr, qhp, qhp->wq.sq.qid);
 	spin_unlock_irq(&rhp->lock);
 	if (ret)
diff --git a/drivers/infiniband/hw/cxgb4/t4.h b/drivers/infiniband/hw/cxgb4/t4.h
index 16f26ab29302..ebcb03bd1b72 100644
--- a/drivers/infiniband/hw/cxgb4/t4.h
+++ b/drivers/infiniband/hw/cxgb4/t4.h
@@ -84,7 +84,7 @@ struct t4_status_page {
 			sizeof(struct fw_ri_isgl)) / sizeof(struct fw_ri_sge))
 #define T4_MAX_FR_IMMD ((T4_SQ_NUM_BYTES - sizeof(struct fw_ri_fr_nsmr_wr) - \
 			sizeof(struct fw_ri_immd)) & ~31UL)
-#define T4_MAX_FR_DEPTH (T4_MAX_FR_IMMD / sizeof(u64))
+#define T4_MAX_FR_DEPTH (1024 / sizeof(u64))
 
 #define T4_RQ_NUM_SLOTS 2
 #define T4_RQ_NUM_BYTES (T4_EQ_ENTRY_SIZE * T4_RQ_NUM_SLOTS)
@@ -280,15 +280,6 @@ static inline pgprot_t t4_pgprot_wc(pgprot_t prot)
 #endif
 }
 
-static inline int t4_ocqp_supported(void)
-{
-#if defined(__i386__) || defined(__x86_64__) || defined(CONFIG_PPC64)
-	return 1;
-#else
-	return 0;
-#endif
-}
-
 enum {
 	T4_SQ_ONCHIP = (1<<0),
 };
diff --git a/drivers/infiniband/hw/ipath/ipath_file_ops.c b/drivers/infiniband/hw/ipath/ipath_file_ops.c
index aed8afee56da..6d7f453b4d05 100644
--- a/drivers/infiniband/hw/ipath/ipath_file_ops.c
+++ b/drivers/infiniband/hw/ipath/ipath_file_ops.c
@@ -40,6 +40,7 @@
 #include <linux/slab.h>
 #include <linux/highmem.h>
 #include <linux/io.h>
+#include <linux/aio.h>
 #include <linux/jiffies.h>
 #include <linux/cpu.h>
 #include <asm/pgtable.h>
diff --git a/drivers/infiniband/hw/ipath/ipath_verbs.c b/drivers/infiniband/hw/ipath/ipath_verbs.c
index ea93870266eb..44ea9390417c 100644
--- a/drivers/infiniband/hw/ipath/ipath_verbs.c
+++ b/drivers/infiniband/hw/ipath/ipath_verbs.c
@@ -2187,7 +2187,8 @@ int ipath_register_ib_device(struct ipath_devdata *dd)
 	if (ret)
 		goto err_reg;
 
-	if (ipath_verbs_register_sysfs(dev))
+	ret = ipath_verbs_register_sysfs(dev);
+	if (ret)
 		goto err_class;
 
 	enable_timer(dd);
@@ -2327,15 +2328,15 @@ static int ipath_verbs_register_sysfs(struct ib_device *dev)
 	int i;
 	int ret;
 
-	for (i = 0; i < ARRAY_SIZE(ipath_class_attributes); ++i)
-		if (device_create_file(&dev->dev,
-				       ipath_class_attributes[i])) {
-			ret = 1;
+	for (i = 0; i < ARRAY_SIZE(ipath_class_attributes); ++i) {
+		ret = device_create_file(&dev->dev,
+				       ipath_class_attributes[i]);
+		if (ret)
 			goto bail;
-		}
-
-	ret = 0;
-
+	}
+	return 0;
 bail:
+	for (i = 0; i < ARRAY_SIZE(ipath_class_attributes); ++i)
+		device_remove_file(&dev->dev, ipath_class_attributes[i]);
 	return ret;
 }
diff --git a/drivers/infiniband/hw/mlx4/cq.c b/drivers/infiniband/hw/mlx4/cq.c
index ae67df35dd4d..d5e60f44ba5a 100644
--- a/drivers/infiniband/hw/mlx4/cq.c
+++ b/drivers/infiniband/hw/mlx4/cq.c
@@ -33,6 +33,7 @@
 
 #include <linux/mlx4/cq.h>
 #include <linux/mlx4/qp.h>
+#include <linux/mlx4/srq.h>
 #include <linux/slab.h>
 
 #include "mlx4_ib.h"
@@ -228,7 +229,7 @@ struct ib_cq *mlx4_ib_create_cq(struct ib_device *ibdev, int entries, int vector
 		vector = dev->eq_table[vector % ibdev->num_comp_vectors];
 
 	err = mlx4_cq_alloc(dev->dev, entries, &cq->buf.mtt, uar,
-			    cq->db.dma, &cq->mcq, vector, 0);
+			    cq->db.dma, &cq->mcq, vector, 0, 0);
 	if (err)
 		goto err_dbmap;
 
@@ -585,6 +586,7 @@ static int mlx4_ib_poll_one(struct mlx4_ib_cq *cq,
 	struct mlx4_qp *mqp;
 	struct mlx4_ib_wq *wq;
 	struct mlx4_ib_srq *srq;
+	struct mlx4_srq *msrq = NULL;
 	int is_send;
 	int is_error;
 	u32 g_mlpath_rqpn;
@@ -653,6 +655,20 @@ repoll:
 
 	wc->qp = &(*cur_qp)->ibqp;
 
+	if (wc->qp->qp_type == IB_QPT_XRC_TGT) {
+		u32 srq_num;
+		g_mlpath_rqpn = be32_to_cpu(cqe->g_mlpath_rqpn);
+		srq_num       = g_mlpath_rqpn & 0xffffff;
+		/* SRQ is also in the radix tree */
+		msrq = mlx4_srq_lookup(to_mdev(cq->ibcq.device)->dev,
+				       srq_num);
+		if (unlikely(!msrq)) {
+			pr_warn("CQ %06x with entry for unknown SRQN %06x\n",
+				cq->mcq.cqn, srq_num);
+			return -EINVAL;
+		}
+	}
+
 	if (is_send) {
 		wq = &(*cur_qp)->sq;
 		if (!(*cur_qp)->sq_signal_bits) {
@@ -666,6 +682,11 @@ repoll:
 		wqe_ctr = be16_to_cpu(cqe->wqe_index);
 		wc->wr_id = srq->wrid[wqe_ctr];
 		mlx4_ib_free_srq_wqe(srq, wqe_ctr);
+	} else if (msrq) {
+		srq = to_mibsrq(msrq);
+		wqe_ctr = be16_to_cpu(cqe->wqe_index);
+		wc->wr_id = srq->wrid[wqe_ctr];
+		mlx4_ib_free_srq_wqe(srq, wqe_ctr);
 	} else {
 		wq	  = &(*cur_qp)->rq;
 		tail	  = wq->tail & (wq->wqe_cnt - 1);
diff --git a/drivers/infiniband/hw/mlx4/mad.c b/drivers/infiniband/hw/mlx4/mad.c
index 934792c477bc..4d599cedbb0b 100644
--- a/drivers/infiniband/hw/mlx4/mad.c
+++ b/drivers/infiniband/hw/mlx4/mad.c
@@ -93,7 +93,7 @@ static void __propagate_pkey_ev(struct mlx4_ib_dev *dev, int port_num,
 __be64 mlx4_ib_gen_node_guid(void)
 {
 #define NODE_GUID_HI	((u64) (((u64)IB_OPENIB_OUI) << 40))
-	return cpu_to_be64(NODE_GUID_HI | random32());
+	return cpu_to_be64(NODE_GUID_HI | prandom_u32());
 }
 
 __be64 mlx4_ib_get_new_demux_tid(struct mlx4_ib_demux_ctx *ctx)
diff --git a/drivers/infiniband/hw/mlx4/qp.c b/drivers/infiniband/hw/mlx4/qp.c
index 35cced2a4da8..4f10af2905b5 100644
--- a/drivers/infiniband/hw/mlx4/qp.c
+++ b/drivers/infiniband/hw/mlx4/qp.c
@@ -1292,6 +1292,8 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 	if (cur_state == IB_QPS_RESET && new_state == IB_QPS_INIT) {
 		context->sq_size_stride |= !!qp->sq_no_prefetch << 7;
 		context->xrcd = cpu_to_be32((u32) qp->xrcdn);
+		if (ibqp->qp_type == IB_QPT_RAW_PACKET)
+			context->param3 |= cpu_to_be32(1 << 30);
 	}
 
 	if (qp->ibqp.uobject)
@@ -1458,6 +1460,10 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 		}
 	}
 
+	if (qp->ibqp.qp_type == IB_QPT_RAW_PACKET)
+		context->pri_path.ackto = (context->pri_path.ackto & 0xf8) |
+					MLX4_IB_LINK_TYPE_ETH;
+
 	if (cur_state == IB_QPS_RTS && new_state == IB_QPS_SQD	&&
 	    attr_mask & IB_QP_EN_SQD_ASYNC_NOTIFY && attr->en_sqd_async_notify)
 		sqd_event = 1;
diff --git a/drivers/infiniband/hw/nes/nes_hw.c b/drivers/infiniband/hw/nes/nes_hw.c
index 67647e264611..418004c93feb 100644
--- a/drivers/infiniband/hw/nes/nes_hw.c
+++ b/drivers/infiniband/hw/nes/nes_hw.c
@@ -2948,7 +2948,7 @@ void nes_nic_ce_handler(struct nes_device *nesdev, struct nes_hw_nic_cq *cq)
 					nes_debug(NES_DBG_CQ, "%s: Reporting stripped VLAN packet. Tag = 0x%04X\n",
 							nesvnic->netdev->name, vlan_tag);
 
-					__vlan_hwaccel_put_tag(rx_skb, vlan_tag);
+					__vlan_hwaccel_put_tag(rx_skb, htons(ETH_P_8021Q), vlan_tag);
 				}
 				if (nes_use_lro)
 					lro_receive_skb(&nesvnic->lro_mgr, rx_skb, NULL);
diff --git a/drivers/infiniband/hw/nes/nes_nic.c b/drivers/infiniband/hw/nes/nes_nic.c
index 85cf4d1ac442..49eb5111d2cd 100644
--- a/drivers/infiniband/hw/nes/nes_nic.c
+++ b/drivers/infiniband/hw/nes/nes_nic.c
@@ -1599,7 +1599,7 @@ static void nes_vlan_mode(struct net_device *netdev, struct nes_device *nesdev,
 
 	/* Enable/Disable VLAN Stripping */
 	u32temp = nes_read_indexed(nesdev, NES_IDX_PCIX_DIAG);
-	if (features & NETIF_F_HW_VLAN_RX)
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
 		u32temp &= 0xfdffffff;
 	else
 		u32temp	|= 0x02000000;
@@ -1614,10 +1614,10 @@ static netdev_features_t nes_fix_features(struct net_device *netdev, netdev_feat
 	 * Since there is no support for separate rx/tx vlan accel
 	 * enable/disable make sure tx flag is always in same state as rx.
 	 */
-	if (features & NETIF_F_HW_VLAN_RX)
-		features |= NETIF_F_HW_VLAN_TX;
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
+		features |= NETIF_F_HW_VLAN_CTAG_TX;
 	else
-		features &= ~NETIF_F_HW_VLAN_TX;
+		features &= ~NETIF_F_HW_VLAN_CTAG_TX;
 
 	return features;
 }
@@ -1628,7 +1628,7 @@ static int nes_set_features(struct net_device *netdev, netdev_features_t feature
 	struct nes_device *nesdev = nesvnic->nesdev;
 	u32 changed = netdev->features ^ features;
 
-	if (changed & NETIF_F_HW_VLAN_RX)
+	if (changed & NETIF_F_HW_VLAN_CTAG_RX)
 		nes_vlan_mode(netdev, nesdev, features);
 
 	return 0;
@@ -1706,11 +1706,11 @@ struct net_device *nes_netdev_init(struct nes_device *nesdev,
 	netdev->dev_addr[4] = (u8)(u64temp>>8);
 	netdev->dev_addr[5] = (u8)u64temp;
 
-	netdev->hw_features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_RXCSUM | NETIF_F_HW_VLAN_RX;
+	netdev->hw_features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_RXCSUM | NETIF_F_HW_VLAN_CTAG_RX;
 	if ((nesvnic->logical_port < 2) || (nesdev->nesadapter->hw_rev != NE020_REV))
 		netdev->hw_features |= NETIF_F_TSO;
 
-	netdev->features = netdev->hw_features | NETIF_F_HIGHDMA | NETIF_F_HW_VLAN_TX;
+	netdev->features = netdev->hw_features | NETIF_F_HIGHDMA | NETIF_F_HW_VLAN_CTAG_TX;
 	netdev->hw_features |= NETIF_F_LRO;
 
 	nes_debug(NES_DBG_INIT, "nesvnic = %p, reported features = 0x%lX, QPid = %d,"
diff --git a/drivers/infiniband/hw/qib/qib_file_ops.c b/drivers/infiniband/hw/qib/qib_file_ops.c
index 4f7aa301b3b1..b56c9428f3c5 100644
--- a/drivers/infiniband/hw/qib/qib_file_ops.c
+++ b/drivers/infiniband/hw/qib/qib_file_ops.c
@@ -39,7 +39,7 @@
 #include <linux/vmalloc.h>
 #include <linux/highmem.h>
 #include <linux/io.h>
-#include <linux/uio.h>
+#include <linux/aio.h>
 #include <linux/jiffies.h>
 #include <asm/pgtable.h>
 #include <linux/delay.h>
diff --git a/drivers/infiniband/hw/qib/qib_sysfs.c b/drivers/infiniband/hw/qib/qib_sysfs.c
index 034cc821de5c..3c8e4e3caca6 100644
--- a/drivers/infiniband/hw/qib/qib_sysfs.c
+++ b/drivers/infiniband/hw/qib/qib_sysfs.c
@@ -808,10 +808,14 @@ int qib_verbs_register_sysfs(struct qib_devdata *dd)
 	for (i = 0; i < ARRAY_SIZE(qib_attributes); ++i) {
 		ret = device_create_file(&dev->dev, qib_attributes[i]);
 		if (ret)
-			return ret;
+			goto bail;
 	}
 
 	return 0;
+bail:
+	for (i = 0; i < ARRAY_SIZE(qib_attributes); ++i)
+		device_remove_file(&dev->dev, qib_attributes[i]);
+	return ret;
 }
 
 /*
diff --git a/drivers/infiniband/hw/qib/qib_verbs.c b/drivers/infiniband/hw/qib/qib_verbs.c
index 7c0ab16a2fe2..904c384aa361 100644
--- a/drivers/infiniband/hw/qib/qib_verbs.c
+++ b/drivers/infiniband/hw/qib/qib_verbs.c
@@ -2234,7 +2234,8 @@ int qib_register_ib_device(struct qib_devdata *dd)
 	if (ret)
 		goto err_agents;
 
-	if (qib_verbs_register_sysfs(dd))
+	ret = qib_verbs_register_sysfs(dd);
+	if (ret)
 		goto err_class;
 
 	goto bail;
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_cm.c b/drivers/infiniband/ulp/ipoib/ipoib_cm.c
index 1ef880de3a41..3eceb61e3532 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_cm.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_cm.c
@@ -460,7 +460,7 @@ static int ipoib_cm_req_handler(struct ib_cm_id *cm_id, struct ib_cm_event *even
 		goto err_qp;
 	}
 
-	psn = random32() & 0xffffff;
+	psn = prandom_u32() & 0xffffff;
 	ret = ipoib_cm_modify_rx_qp(dev, cm_id, p->qp, psn);
 	if (ret)
 		goto err_modify;
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_main.c b/drivers/infiniband/ulp/ipoib/ipoib_main.c
index 8534afd04e7c..b6e049a3c7a8 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_main.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_main.c
@@ -730,7 +730,8 @@ static int ipoib_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		if ((header->proto != htons(ETH_P_IP)) &&
 		    (header->proto != htons(ETH_P_IPV6)) &&
 		    (header->proto != htons(ETH_P_ARP)) &&
-		    (header->proto != htons(ETH_P_RARP))) {
+		    (header->proto != htons(ETH_P_RARP)) &&
+		    (header->proto != htons(ETH_P_TIPC))) {
 			/* ethertype not supported by IPoIB */
 			++dev->stats.tx_dropped;
 			dev_kfree_skb_any(skb);
@@ -751,6 +752,7 @@ static int ipoib_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	switch (header->proto) {
 	case htons(ETH_P_IP):
 	case htons(ETH_P_IPV6):
+	case htons(ETH_P_TIPC):
 		neigh = ipoib_neigh_get(dev, cb->hwaddr);
 		if (unlikely(!neigh)) {
 			neigh_add_path(skb, cb->hwaddr, dev);
@@ -828,7 +830,7 @@ static int ipoib_hard_header(struct sk_buff *skb,
 	 */
 	memcpy(cb->hwaddr, daddr, INFINIBAND_ALEN);
 
-	return 0;
+	return sizeof *header;
 }
 
 static void ipoib_set_mcast_list(struct net_device *dev)
diff --git a/drivers/infiniband/ulp/iser/iscsi_iser.c b/drivers/infiniband/ulp/iser/iscsi_iser.c
index 0ab8c9cc3a78..f19b0998a53c 100644
--- a/drivers/infiniband/ulp/iser/iscsi_iser.c
+++ b/drivers/infiniband/ulp/iser/iscsi_iser.c
@@ -82,10 +82,10 @@ module_param_named(max_lun, iscsi_max_lun, uint, S_IRUGO);
 
 int iser_debug_level = 0;
 
-MODULE_DESCRIPTION("iSER (iSCSI Extensions for RDMA) Datamover "
-		   "v" DRV_VER " (" DRV_DATE ")");
+MODULE_DESCRIPTION("iSER (iSCSI Extensions for RDMA) Datamover");
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_AUTHOR("Alex Nezhinsky, Dan Bar Dov, Or Gerlitz");
+MODULE_VERSION(DRV_VER);
 
 module_param_named(debug_level, iser_debug_level, int, 0644);
 MODULE_PARM_DESC(debug_level, "Enable debug tracing if > 0 (default:disabled)");
@@ -370,8 +370,8 @@ iscsi_iser_conn_bind(struct iscsi_cls_session *cls_session,
 	/* binds the iSER connection retrieved from the previously
 	 * connected ep_handle to the iSCSI layer connection. exchanges
 	 * connection pointers */
-	iser_err("binding iscsi/iser conn %p %p to ib_conn %p\n",
-					conn, conn->dd_data, ib_conn);
+	iser_info("binding iscsi/iser conn %p %p to ib_conn %p\n",
+		  conn, conn->dd_data, ib_conn);
 	iser_conn = conn->dd_data;
 	ib_conn->iser_conn = iser_conn;
 	iser_conn->ib_conn  = ib_conn;
@@ -475,28 +475,28 @@ iscsi_iser_set_param(struct iscsi_cls_conn *cls_conn,
 	case ISCSI_PARAM_HDRDGST_EN:
 		sscanf(buf, "%d", &value);
 		if (value) {
-			printk(KERN_ERR "DataDigest wasn't negotiated to None");
+			iser_err("DataDigest wasn't negotiated to None");
 			return -EPROTO;
 		}
 		break;
 	case ISCSI_PARAM_DATADGST_EN:
 		sscanf(buf, "%d", &value);
 		if (value) {
-			printk(KERN_ERR "DataDigest wasn't negotiated to None");
+			iser_err("DataDigest wasn't negotiated to None");
 			return -EPROTO;
 		}
 		break;
 	case ISCSI_PARAM_IFMARKER_EN:
 		sscanf(buf, "%d", &value);
 		if (value) {
-			printk(KERN_ERR "IFMarker wasn't negotiated to No");
+			iser_err("IFMarker wasn't negotiated to No");
 			return -EPROTO;
 		}
 		break;
 	case ISCSI_PARAM_OFMARKER_EN:
 		sscanf(buf, "%d", &value);
 		if (value) {
-			printk(KERN_ERR "OFMarker wasn't negotiated to No");
+			iser_err("OFMarker wasn't negotiated to No");
 			return -EPROTO;
 		}
 		break;
@@ -596,7 +596,7 @@ iscsi_iser_ep_poll(struct iscsi_endpoint *ep, int timeout_ms)
 	     ib_conn->state == ISER_CONN_DOWN))
 		rc = -1;
 
-	iser_err("ib conn %p rc = %d\n", ib_conn, rc);
+	iser_info("ib conn %p rc = %d\n", ib_conn, rc);
 
 	if (rc > 0)
 		return 1; /* success, this is the equivalent of POLLOUT */
@@ -623,7 +623,7 @@ iscsi_iser_ep_disconnect(struct iscsi_endpoint *ep)
 		iscsi_suspend_tx(ib_conn->iser_conn->iscsi_conn);
 
 
-	iser_err("ib conn %p state %d\n",ib_conn, ib_conn->state);
+	iser_info("ib conn %p state %d\n", ib_conn, ib_conn->state);
 	iser_conn_terminate(ib_conn);
 }
 
@@ -682,7 +682,7 @@ static umode_t iser_attr_is_visible(int param_type, int param)
 
 static struct scsi_host_template iscsi_iser_sht = {
 	.module                 = THIS_MODULE,
-	.name                   = "iSCSI Initiator over iSER, v." DRV_VER,
+	.name                   = "iSCSI Initiator over iSER",
 	.queuecommand           = iscsi_queuecommand,
 	.change_queue_depth	= iscsi_change_queue_depth,
 	.sg_tablesize           = ISCSI_ISER_SG_TABLESIZE,
@@ -740,7 +740,7 @@ static int __init iser_init(void)
 	iser_dbg("Starting iSER datamover...\n");
 
 	if (iscsi_max_lun < 1) {
-		printk(KERN_ERR "Invalid max_lun value of %u\n", iscsi_max_lun);
+		iser_err("Invalid max_lun value of %u\n", iscsi_max_lun);
 		return -EINVAL;
 	}
 
diff --git a/drivers/infiniband/ulp/iser/iscsi_iser.h b/drivers/infiniband/ulp/iser/iscsi_iser.h
index 5babdb35bda7..06f578cde75b 100644
--- a/drivers/infiniband/ulp/iser/iscsi_iser.h
+++ b/drivers/infiniband/ulp/iser/iscsi_iser.h
@@ -42,6 +42,7 @@
 
 #include <linux/types.h>
 #include <linux/net.h>
+#include <linux/printk.h>
 #include <scsi/libiscsi.h>
 #include <scsi/scsi_transport_iscsi.h>
 
@@ -65,20 +66,26 @@
 
 #define DRV_NAME	"iser"
 #define PFX		DRV_NAME ": "
-#define DRV_VER		"0.1"
-#define DRV_DATE	"May 7th, 2006"
+#define DRV_VER		"1.1"
 
 #define iser_dbg(fmt, arg...)				\
 	do {						\
-		if (iser_debug_level > 1)		\
+		if (iser_debug_level > 2)		\
 			printk(KERN_DEBUG PFX "%s:" fmt,\
 				__func__ , ## arg);	\
 	} while (0)
 
 #define iser_warn(fmt, arg...)				\
 	do {						\
+		if (iser_debug_level > 1)		\
+			pr_warn(PFX "%s:" fmt,          \
+				__func__ , ## arg);	\
+	} while (0)
+
+#define iser_info(fmt, arg...)				\
+	do {						\
 		if (iser_debug_level > 0)		\
-			printk(KERN_DEBUG PFX "%s:" fmt,\
+			pr_info(PFX "%s:" fmt,          \
 				__func__ , ## arg);	\
 	} while (0)
 
@@ -133,6 +140,15 @@ struct iser_hdr {
 	__be64  read_va;
 } __attribute__((packed));
 
+
+#define ISER_ZBVA_NOT_SUPPORTED		0x80
+#define ISER_SEND_W_INV_NOT_SUPPORTED	0x40
+
+struct iser_cm_hdr {
+	u8      flags;
+	u8      rsvd[3];
+} __packed;
+
 /* Constant PDU lengths calculations */
 #define ISER_HEADERS_LEN  (sizeof(struct iser_hdr) + sizeof(struct iscsi_hdr))
 
diff --git a/drivers/infiniband/ulp/iser/iser_memory.c b/drivers/infiniband/ulp/iser/iser_memory.c
index be1edb04b085..68ebb7fe072a 100644
--- a/drivers/infiniband/ulp/iser/iser_memory.c
+++ b/drivers/infiniband/ulp/iser/iser_memory.c
@@ -416,8 +416,9 @@ int iser_reg_rdma_mem(struct iscsi_iser_task *iser_task,
 			for (i=0 ; i<ib_conn->page_vec->length ; i++)
 				iser_err("page_vec[%d] = 0x%llx\n", i,
 					 (unsigned long long) ib_conn->page_vec->pages[i]);
-			return err;
 		}
+		if (err)
+			return err;
 	}
 	return 0;
 }
diff --git a/drivers/infiniband/ulp/iser/iser_verbs.c b/drivers/infiniband/ulp/iser/iser_verbs.c
index 4debadc53106..5278916c3103 100644
--- a/drivers/infiniband/ulp/iser/iser_verbs.c
+++ b/drivers/infiniband/ulp/iser/iser_verbs.c
@@ -74,8 +74,9 @@ static int iser_create_device_ib_res(struct iser_device *device)
 	struct iser_cq_desc *cq_desc;
 
 	device->cqs_used = min(ISER_MAX_CQ, device->ib_device->num_comp_vectors);
-	iser_err("using %d CQs, device %s supports %d vectors\n", device->cqs_used,
-		 device->ib_device->name, device->ib_device->num_comp_vectors);
+	iser_info("using %d CQs, device %s supports %d vectors\n",
+		  device->cqs_used, device->ib_device->name,
+		  device->ib_device->num_comp_vectors);
 
 	device->cq_desc = kmalloc(sizeof(struct iser_cq_desc) * device->cqs_used,
 				  GFP_KERNEL);
@@ -262,7 +263,7 @@ static int iser_create_ib_conn_res(struct iser_conn *ib_conn)
 			min_index = index;
 	device->cq_active_qps[min_index]++;
 	mutex_unlock(&ig.connlist_mutex);
-	iser_err("cq index %d used for ib_conn %p\n", min_index, ib_conn);
+	iser_info("cq index %d used for ib_conn %p\n", min_index, ib_conn);
 
 	init_attr.event_handler = iser_qp_event_callback;
 	init_attr.qp_context	= (void *)ib_conn;
@@ -280,9 +281,9 @@ static int iser_create_ib_conn_res(struct iser_conn *ib_conn)
 		goto out_err;
 
 	ib_conn->qp = ib_conn->cma_id->qp;
-	iser_err("setting conn %p cma_id %p: fmr_pool %p qp %p\n",
-		 ib_conn, ib_conn->cma_id,
-		 ib_conn->fmr_pool, ib_conn->cma_id->qp);
+	iser_info("setting conn %p cma_id %p: fmr_pool %p qp %p\n",
+		  ib_conn, ib_conn->cma_id,
+		  ib_conn->fmr_pool, ib_conn->cma_id->qp);
 	return ret;
 
 out_err:
@@ -299,9 +300,9 @@ static int iser_free_ib_conn_res(struct iser_conn *ib_conn, int can_destroy_id)
 	int cq_index;
 	BUG_ON(ib_conn == NULL);
 
-	iser_err("freeing conn %p cma_id %p fmr pool %p qp %p\n",
-		 ib_conn, ib_conn->cma_id,
-		 ib_conn->fmr_pool, ib_conn->qp);
+	iser_info("freeing conn %p cma_id %p fmr pool %p qp %p\n",
+		  ib_conn, ib_conn->cma_id,
+		  ib_conn->fmr_pool, ib_conn->qp);
 
 	/* qp is created only once both addr & route are resolved */
 	if (ib_conn->fmr_pool != NULL)
@@ -379,7 +380,7 @@ static void iser_device_try_release(struct iser_device *device)
 {
 	mutex_lock(&ig.device_list_mutex);
 	device->refcount--;
-	iser_err("device %p refcount %d\n",device,device->refcount);
+	iser_info("device %p refcount %d\n", device, device->refcount);
 	if (!device->refcount) {
 		iser_free_device_ib_res(device);
 		list_del(&device->ig_list);
@@ -498,6 +499,7 @@ static int iser_route_handler(struct rdma_cm_id *cma_id)
 {
 	struct rdma_conn_param conn_param;
 	int    ret;
+	struct iser_cm_hdr req_hdr;
 
 	ret = iser_create_ib_conn_res((struct iser_conn *)cma_id->context);
 	if (ret)
@@ -509,6 +511,12 @@ static int iser_route_handler(struct rdma_cm_id *cma_id)
 	conn_param.retry_count	       = 7;
 	conn_param.rnr_retry_count     = 6;
 
+	memset(&req_hdr, 0, sizeof(req_hdr));
+	req_hdr.flags = (ISER_ZBVA_NOT_SUPPORTED |
+			ISER_SEND_W_INV_NOT_SUPPORTED);
+	conn_param.private_data		= (void *)&req_hdr;
+	conn_param.private_data_len	= sizeof(struct iser_cm_hdr);
+
 	ret = rdma_connect(cma_id, &conn_param);
 	if (ret) {
 		iser_err("failure connecting: %d\n", ret);
@@ -558,8 +566,8 @@ static int iser_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *eve
 {
 	int ret = 0;
 
-	iser_err("event %d status %d conn %p id %p\n",
-		event->event, event->status, cma_id->context, cma_id);
+	iser_info("event %d status %d conn %p id %p\n",
+		  event->event, event->status, cma_id->context, cma_id);
 
 	switch (event->event) {
 	case RDMA_CM_EVENT_ADDR_RESOLVED:
@@ -619,8 +627,8 @@ int iser_connect(struct iser_conn   *ib_conn,
 	/* the device is known only --after-- address resolution */
 	ib_conn->device = NULL;
 
-	iser_err("connecting to: %pI4, port 0x%x\n",
-		 &dst_addr->sin_addr, dst_addr->sin_port);
+	iser_info("connecting to: %pI4, port 0x%x\n",
+		  &dst_addr->sin_addr, dst_addr->sin_port);
 
 	ib_conn->state = ISER_CONN_PENDING;
 
diff --git a/drivers/infiniband/ulp/isert/Kconfig b/drivers/infiniband/ulp/isert/Kconfig
new file mode 100644
index 000000000000..ce3fd32167dc
--- /dev/null
+++ b/drivers/infiniband/ulp/isert/Kconfig
@@ -0,0 +1,5 @@
+config INFINIBAND_ISERT
+	tristate "iSCSI Extentions for RDMA (iSER) target support"
+	depends on INET && INFINIBAND_ADDR_TRANS && TARGET_CORE && ISCSI_TARGET
+	---help---
+	Support for iSCSI Extentions for RDMA (iSER) Target on Infiniband fabrics.
diff --git a/drivers/infiniband/ulp/isert/Makefile b/drivers/infiniband/ulp/isert/Makefile
new file mode 100644
index 000000000000..c8bf2421f5bc
--- /dev/null
+++ b/drivers/infiniband/ulp/isert/Makefile
@@ -0,0 +1,2 @@
+ccflags-y		:= -Idrivers/target -Idrivers/target/iscsi
+obj-$(CONFIG_INFINIBAND_ISERT)	+= ib_isert.o
diff --git a/drivers/infiniband/ulp/isert/ib_isert.c b/drivers/infiniband/ulp/isert/ib_isert.c
new file mode 100644
index 000000000000..41712f096515
--- /dev/null
+++ b/drivers/infiniband/ulp/isert/ib_isert.c
@@ -0,0 +1,2281 @@
+/*******************************************************************************
+ * This file contains iSCSI extentions for RDMA (iSER) Verbs
+ *
+ * (c) Copyright 2013 RisingTide Systems LLC.
+ *
+ * Nicholas A. Bellinger <nab@linux-iscsi.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.
+ ****************************************************************************/
+
+#include <linux/string.h>
+#include <linux/module.h>
+#include <linux/scatterlist.h>
+#include <linux/socket.h>
+#include <linux/in.h>
+#include <linux/in6.h>
+#include <rdma/ib_verbs.h>
+#include <rdma/rdma_cm.h>
+#include <target/target_core_base.h>
+#include <target/target_core_fabric.h>
+#include <target/iscsi/iscsi_transport.h>
+
+#include "isert_proto.h"
+#include "ib_isert.h"
+
+#define	ISERT_MAX_CONN		8
+#define ISER_MAX_RX_CQ_LEN	(ISERT_QP_MAX_RECV_DTOS * ISERT_MAX_CONN)
+#define ISER_MAX_TX_CQ_LEN	(ISERT_QP_MAX_REQ_DTOS  * ISERT_MAX_CONN)
+
+static DEFINE_MUTEX(device_list_mutex);
+static LIST_HEAD(device_list);
+static struct workqueue_struct *isert_rx_wq;
+static struct workqueue_struct *isert_comp_wq;
+static struct kmem_cache *isert_cmd_cache;
+
+static void
+isert_qp_event_callback(struct ib_event *e, void *context)
+{
+	struct isert_conn *isert_conn = (struct isert_conn *)context;
+
+	pr_err("isert_qp_event_callback event: %d\n", e->event);
+	switch (e->event) {
+	case IB_EVENT_COMM_EST:
+		rdma_notify(isert_conn->conn_cm_id, IB_EVENT_COMM_EST);
+		break;
+	case IB_EVENT_QP_LAST_WQE_REACHED:
+		pr_warn("Reached TX IB_EVENT_QP_LAST_WQE_REACHED:\n");
+		break;
+	default:
+		break;
+	}
+}
+
+static int
+isert_query_device(struct ib_device *ib_dev, struct ib_device_attr *devattr)
+{
+	int ret;
+
+	ret = ib_query_device(ib_dev, devattr);
+	if (ret) {
+		pr_err("ib_query_device() failed: %d\n", ret);
+		return ret;
+	}
+	pr_debug("devattr->max_sge: %d\n", devattr->max_sge);
+	pr_debug("devattr->max_sge_rd: %d\n", devattr->max_sge_rd);
+
+	return 0;
+}
+
+static int
+isert_conn_setup_qp(struct isert_conn *isert_conn, struct rdma_cm_id *cma_id)
+{
+	struct isert_device *device = isert_conn->conn_device;
+	struct ib_qp_init_attr attr;
+	struct ib_device_attr devattr;
+	int ret, index, min_index = 0;
+
+	memset(&devattr, 0, sizeof(struct ib_device_attr));
+	ret = isert_query_device(cma_id->device, &devattr);
+	if (ret)
+		return ret;
+
+	mutex_lock(&device_list_mutex);
+	for (index = 0; index < device->cqs_used; index++)
+		if (device->cq_active_qps[index] <
+		    device->cq_active_qps[min_index])
+			min_index = index;
+	device->cq_active_qps[min_index]++;
+	pr_debug("isert_conn_setup_qp: Using min_index: %d\n", min_index);
+	mutex_unlock(&device_list_mutex);
+
+	memset(&attr, 0, sizeof(struct ib_qp_init_attr));
+	attr.event_handler = isert_qp_event_callback;
+	attr.qp_context = isert_conn;
+	attr.send_cq = device->dev_tx_cq[min_index];
+	attr.recv_cq = device->dev_rx_cq[min_index];
+	attr.cap.max_send_wr = ISERT_QP_MAX_REQ_DTOS;
+	attr.cap.max_recv_wr = ISERT_QP_MAX_RECV_DTOS;
+	/*
+	 * FIXME: Use devattr.max_sge - 2 for max_send_sge as
+	 * work-around for RDMA_READ..
+	 */
+	attr.cap.max_send_sge = devattr.max_sge - 2;
+	isert_conn->max_sge = attr.cap.max_send_sge;
+
+	attr.cap.max_recv_sge = 1;
+	attr.sq_sig_type = IB_SIGNAL_REQ_WR;
+	attr.qp_type = IB_QPT_RC;
+
+	pr_debug("isert_conn_setup_qp cma_id->device: %p\n",
+		 cma_id->device);
+	pr_debug("isert_conn_setup_qp conn_pd->device: %p\n",
+		 isert_conn->conn_pd->device);
+
+	ret = rdma_create_qp(cma_id, isert_conn->conn_pd, &attr);
+	if (ret) {
+		pr_err("rdma_create_qp failed for cma_id %d\n", ret);
+		return ret;
+	}
+	isert_conn->conn_qp = cma_id->qp;
+	pr_debug("rdma_create_qp() returned success >>>>>>>>>>>>>>>>>>>>>>>>>.\n");
+
+	return 0;
+}
+
+static void
+isert_cq_event_callback(struct ib_event *e, void *context)
+{
+	pr_debug("isert_cq_event_callback event: %d\n", e->event);
+}
+
+static int
+isert_alloc_rx_descriptors(struct isert_conn *isert_conn)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct iser_rx_desc *rx_desc;
+	struct ib_sge *rx_sg;
+	u64 dma_addr;
+	int i, j;
+
+	isert_conn->conn_rx_descs = kzalloc(ISERT_QP_MAX_RECV_DTOS *
+				sizeof(struct iser_rx_desc), GFP_KERNEL);
+	if (!isert_conn->conn_rx_descs)
+		goto fail;
+
+	rx_desc = isert_conn->conn_rx_descs;
+
+	for (i = 0; i < ISERT_QP_MAX_RECV_DTOS; i++, rx_desc++)  {
+		dma_addr = ib_dma_map_single(ib_dev, (void *)rx_desc,
+					ISER_RX_PAYLOAD_SIZE, DMA_FROM_DEVICE);
+		if (ib_dma_mapping_error(ib_dev, dma_addr))
+			goto dma_map_fail;
+
+		rx_desc->dma_addr = dma_addr;
+
+		rx_sg = &rx_desc->rx_sg;
+		rx_sg->addr = rx_desc->dma_addr;
+		rx_sg->length = ISER_RX_PAYLOAD_SIZE;
+		rx_sg->lkey = isert_conn->conn_mr->lkey;
+	}
+
+	isert_conn->conn_rx_desc_head = 0;
+	return 0;
+
+dma_map_fail:
+	rx_desc = isert_conn->conn_rx_descs;
+	for (j = 0; j < i; j++, rx_desc++) {
+		ib_dma_unmap_single(ib_dev, rx_desc->dma_addr,
+				    ISER_RX_PAYLOAD_SIZE, DMA_FROM_DEVICE);
+	}
+	kfree(isert_conn->conn_rx_descs);
+	isert_conn->conn_rx_descs = NULL;
+fail:
+	return -ENOMEM;
+}
+
+static void
+isert_free_rx_descriptors(struct isert_conn *isert_conn)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct iser_rx_desc *rx_desc;
+	int i;
+
+	if (!isert_conn->conn_rx_descs)
+		return;
+
+	rx_desc = isert_conn->conn_rx_descs;
+	for (i = 0; i < ISERT_QP_MAX_RECV_DTOS; i++, rx_desc++)  {
+		ib_dma_unmap_single(ib_dev, rx_desc->dma_addr,
+				    ISER_RX_PAYLOAD_SIZE, DMA_FROM_DEVICE);
+	}
+
+	kfree(isert_conn->conn_rx_descs);
+	isert_conn->conn_rx_descs = NULL;
+}
+
+static void isert_cq_tx_callback(struct ib_cq *, void *);
+static void isert_cq_rx_callback(struct ib_cq *, void *);
+
+static int
+isert_create_device_ib_res(struct isert_device *device)
+{
+	struct ib_device *ib_dev = device->ib_device;
+	struct isert_cq_desc *cq_desc;
+	int ret = 0, i, j;
+
+	device->cqs_used = min_t(int, num_online_cpus(),
+				 device->ib_device->num_comp_vectors);
+	device->cqs_used = min(ISERT_MAX_CQ, device->cqs_used);
+	pr_debug("Using %d CQs, device %s supports %d vectors\n",
+		 device->cqs_used, device->ib_device->name,
+		 device->ib_device->num_comp_vectors);
+	device->cq_desc = kzalloc(sizeof(struct isert_cq_desc) *
+				device->cqs_used, GFP_KERNEL);
+	if (!device->cq_desc) {
+		pr_err("Unable to allocate device->cq_desc\n");
+		return -ENOMEM;
+	}
+	cq_desc = device->cq_desc;
+
+	device->dev_pd = ib_alloc_pd(ib_dev);
+	if (IS_ERR(device->dev_pd)) {
+		ret = PTR_ERR(device->dev_pd);
+		pr_err("ib_alloc_pd failed for dev_pd: %d\n", ret);
+		goto out_cq_desc;
+	}
+
+	for (i = 0; i < device->cqs_used; i++) {
+		cq_desc[i].device = device;
+		cq_desc[i].cq_index = i;
+
+		device->dev_rx_cq[i] = ib_create_cq(device->ib_device,
+						isert_cq_rx_callback,
+						isert_cq_event_callback,
+						(void *)&cq_desc[i],
+						ISER_MAX_RX_CQ_LEN, i);
+		if (IS_ERR(device->dev_rx_cq[i]))
+			goto out_cq;
+
+		device->dev_tx_cq[i] = ib_create_cq(device->ib_device,
+						isert_cq_tx_callback,
+						isert_cq_event_callback,
+						(void *)&cq_desc[i],
+						ISER_MAX_TX_CQ_LEN, i);
+		if (IS_ERR(device->dev_tx_cq[i]))
+			goto out_cq;
+
+		if (ib_req_notify_cq(device->dev_rx_cq[i], IB_CQ_NEXT_COMP))
+			goto out_cq;
+
+		if (ib_req_notify_cq(device->dev_tx_cq[i], IB_CQ_NEXT_COMP))
+			goto out_cq;
+	}
+
+	device->dev_mr = ib_get_dma_mr(device->dev_pd, IB_ACCESS_LOCAL_WRITE);
+	if (IS_ERR(device->dev_mr)) {
+		ret = PTR_ERR(device->dev_mr);
+		pr_err("ib_get_dma_mr failed for dev_mr: %d\n", ret);
+		goto out_cq;
+	}
+
+	return 0;
+
+out_cq:
+	for (j = 0; j < i; j++) {
+		cq_desc = &device->cq_desc[j];
+
+		if (device->dev_rx_cq[j]) {
+			cancel_work_sync(&cq_desc->cq_rx_work);
+			ib_destroy_cq(device->dev_rx_cq[j]);
+		}
+		if (device->dev_tx_cq[j]) {
+			cancel_work_sync(&cq_desc->cq_tx_work);
+			ib_destroy_cq(device->dev_tx_cq[j]);
+		}
+	}
+	ib_dealloc_pd(device->dev_pd);
+
+out_cq_desc:
+	kfree(device->cq_desc);
+
+	return ret;
+}
+
+static void
+isert_free_device_ib_res(struct isert_device *device)
+{
+	struct isert_cq_desc *cq_desc;
+	int i;
+
+	for (i = 0; i < device->cqs_used; i++) {
+		cq_desc = &device->cq_desc[i];
+
+		cancel_work_sync(&cq_desc->cq_rx_work);
+		cancel_work_sync(&cq_desc->cq_tx_work);
+		ib_destroy_cq(device->dev_rx_cq[i]);
+		ib_destroy_cq(device->dev_tx_cq[i]);
+		device->dev_rx_cq[i] = NULL;
+		device->dev_tx_cq[i] = NULL;
+	}
+
+	ib_dereg_mr(device->dev_mr);
+	ib_dealloc_pd(device->dev_pd);
+	kfree(device->cq_desc);
+}
+
+static void
+isert_device_try_release(struct isert_device *device)
+{
+	mutex_lock(&device_list_mutex);
+	device->refcount--;
+	if (!device->refcount) {
+		isert_free_device_ib_res(device);
+		list_del(&device->dev_node);
+		kfree(device);
+	}
+	mutex_unlock(&device_list_mutex);
+}
+
+static struct isert_device *
+isert_device_find_by_ib_dev(struct rdma_cm_id *cma_id)
+{
+	struct isert_device *device;
+	int ret;
+
+	mutex_lock(&device_list_mutex);
+	list_for_each_entry(device, &device_list, dev_node) {
+		if (device->ib_device->node_guid == cma_id->device->node_guid) {
+			device->refcount++;
+			mutex_unlock(&device_list_mutex);
+			return device;
+		}
+	}
+
+	device = kzalloc(sizeof(struct isert_device), GFP_KERNEL);
+	if (!device) {
+		mutex_unlock(&device_list_mutex);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	INIT_LIST_HEAD(&device->dev_node);
+
+	device->ib_device = cma_id->device;
+	ret = isert_create_device_ib_res(device);
+	if (ret) {
+		kfree(device);
+		mutex_unlock(&device_list_mutex);
+		return ERR_PTR(ret);
+	}
+
+	device->refcount++;
+	list_add_tail(&device->dev_node, &device_list);
+	mutex_unlock(&device_list_mutex);
+
+	return device;
+}
+
+static int
+isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
+{
+	struct iscsi_np *np = cma_id->context;
+	struct isert_np *isert_np = np->np_context;
+	struct isert_conn *isert_conn;
+	struct isert_device *device;
+	struct ib_device *ib_dev = cma_id->device;
+	int ret = 0;
+
+	pr_debug("Entering isert_connect_request cma_id: %p, context: %p\n",
+		 cma_id, cma_id->context);
+
+	isert_conn = kzalloc(sizeof(struct isert_conn), GFP_KERNEL);
+	if (!isert_conn) {
+		pr_err("Unable to allocate isert_conn\n");
+		return -ENOMEM;
+	}
+	isert_conn->state = ISER_CONN_INIT;
+	INIT_LIST_HEAD(&isert_conn->conn_accept_node);
+	init_completion(&isert_conn->conn_login_comp);
+	init_waitqueue_head(&isert_conn->conn_wait);
+	init_waitqueue_head(&isert_conn->conn_wait_comp_err);
+	kref_init(&isert_conn->conn_kref);
+	kref_get(&isert_conn->conn_kref);
+
+	cma_id->context = isert_conn;
+	isert_conn->conn_cm_id = cma_id;
+	isert_conn->responder_resources = event->param.conn.responder_resources;
+	isert_conn->initiator_depth = event->param.conn.initiator_depth;
+	pr_debug("Using responder_resources: %u initiator_depth: %u\n",
+		 isert_conn->responder_resources, isert_conn->initiator_depth);
+
+	isert_conn->login_buf = kzalloc(ISCSI_DEF_MAX_RECV_SEG_LEN +
+					ISER_RX_LOGIN_SIZE, GFP_KERNEL);
+	if (!isert_conn->login_buf) {
+		pr_err("Unable to allocate isert_conn->login_buf\n");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	isert_conn->login_req_buf = isert_conn->login_buf;
+	isert_conn->login_rsp_buf = isert_conn->login_buf +
+				    ISCSI_DEF_MAX_RECV_SEG_LEN;
+	pr_debug("Set login_buf: %p login_req_buf: %p login_rsp_buf: %p\n",
+		 isert_conn->login_buf, isert_conn->login_req_buf,
+		 isert_conn->login_rsp_buf);
+
+	isert_conn->login_req_dma = ib_dma_map_single(ib_dev,
+				(void *)isert_conn->login_req_buf,
+				ISCSI_DEF_MAX_RECV_SEG_LEN, DMA_FROM_DEVICE);
+
+	ret = ib_dma_mapping_error(ib_dev, isert_conn->login_req_dma);
+	if (ret) {
+		pr_err("ib_dma_mapping_error failed for login_req_dma: %d\n",
+		       ret);
+		isert_conn->login_req_dma = 0;
+		goto out_login_buf;
+	}
+
+	isert_conn->login_rsp_dma = ib_dma_map_single(ib_dev,
+					(void *)isert_conn->login_rsp_buf,
+					ISER_RX_LOGIN_SIZE, DMA_TO_DEVICE);
+
+	ret = ib_dma_mapping_error(ib_dev, isert_conn->login_rsp_dma);
+	if (ret) {
+		pr_err("ib_dma_mapping_error failed for login_rsp_dma: %d\n",
+		       ret);
+		isert_conn->login_rsp_dma = 0;
+		goto out_req_dma_map;
+	}
+
+	device = isert_device_find_by_ib_dev(cma_id);
+	if (IS_ERR(device)) {
+		ret = PTR_ERR(device);
+		goto out_rsp_dma_map;
+	}
+
+	isert_conn->conn_device = device;
+	isert_conn->conn_pd = device->dev_pd;
+	isert_conn->conn_mr = device->dev_mr;
+
+	ret = isert_conn_setup_qp(isert_conn, cma_id);
+	if (ret)
+		goto out_conn_dev;
+
+	mutex_lock(&isert_np->np_accept_mutex);
+	list_add_tail(&isert_np->np_accept_list, &isert_conn->conn_accept_node);
+	mutex_unlock(&isert_np->np_accept_mutex);
+
+	pr_debug("isert_connect_request() waking up np_accept_wq: %p\n", np);
+	wake_up(&isert_np->np_accept_wq);
+	return 0;
+
+out_conn_dev:
+	isert_device_try_release(device);
+out_rsp_dma_map:
+	ib_dma_unmap_single(ib_dev, isert_conn->login_rsp_dma,
+			    ISER_RX_LOGIN_SIZE, DMA_TO_DEVICE);
+out_req_dma_map:
+	ib_dma_unmap_single(ib_dev, isert_conn->login_req_dma,
+			    ISCSI_DEF_MAX_RECV_SEG_LEN, DMA_FROM_DEVICE);
+out_login_buf:
+	kfree(isert_conn->login_buf);
+out:
+	kfree(isert_conn);
+	return ret;
+}
+
+static void
+isert_connect_release(struct isert_conn *isert_conn)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct isert_device *device = isert_conn->conn_device;
+	int cq_index;
+
+	pr_debug("Entering isert_connect_release(): >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+
+	if (isert_conn->conn_qp) {
+		cq_index = ((struct isert_cq_desc *)
+			isert_conn->conn_qp->recv_cq->cq_context)->cq_index;
+		pr_debug("isert_connect_release: cq_index: %d\n", cq_index);
+		isert_conn->conn_device->cq_active_qps[cq_index]--;
+
+		rdma_destroy_qp(isert_conn->conn_cm_id);
+	}
+
+	isert_free_rx_descriptors(isert_conn);
+	rdma_destroy_id(isert_conn->conn_cm_id);
+
+	if (isert_conn->login_buf) {
+		ib_dma_unmap_single(ib_dev, isert_conn->login_rsp_dma,
+				    ISER_RX_LOGIN_SIZE, DMA_TO_DEVICE);
+		ib_dma_unmap_single(ib_dev, isert_conn->login_req_dma,
+				    ISCSI_DEF_MAX_RECV_SEG_LEN,
+				    DMA_FROM_DEVICE);
+		kfree(isert_conn->login_buf);
+	}
+	kfree(isert_conn);
+
+	if (device)
+		isert_device_try_release(device);
+
+	pr_debug("Leaving isert_connect_release >>>>>>>>>>>>\n");
+}
+
+static void
+isert_connected_handler(struct rdma_cm_id *cma_id)
+{
+	return;
+}
+
+static void
+isert_release_conn_kref(struct kref *kref)
+{
+	struct isert_conn *isert_conn = container_of(kref,
+				struct isert_conn, conn_kref);
+
+	pr_debug("Calling isert_connect_release for final kref %s/%d\n",
+		 current->comm, current->pid);
+
+	isert_connect_release(isert_conn);
+}
+
+static void
+isert_put_conn(struct isert_conn *isert_conn)
+{
+	kref_put(&isert_conn->conn_kref, isert_release_conn_kref);
+}
+
+static void
+isert_disconnect_work(struct work_struct *work)
+{
+	struct isert_conn *isert_conn = container_of(work,
+				struct isert_conn, conn_logout_work);
+
+	pr_debug("isert_disconnect_work(): >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+
+	isert_conn->state = ISER_CONN_DOWN;
+
+	if (isert_conn->post_recv_buf_count == 0 &&
+	    atomic_read(&isert_conn->post_send_buf_count) == 0) {
+		pr_debug("Calling wake_up(&isert_conn->conn_wait);\n");
+		wake_up(&isert_conn->conn_wait);
+	}
+
+	isert_put_conn(isert_conn);
+}
+
+static void
+isert_disconnected_handler(struct rdma_cm_id *cma_id)
+{
+	struct isert_conn *isert_conn = (struct isert_conn *)cma_id->context;
+
+	INIT_WORK(&isert_conn->conn_logout_work, isert_disconnect_work);
+	schedule_work(&isert_conn->conn_logout_work);
+}
+
+static int
+isert_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
+{
+	int ret = 0;
+
+	pr_debug("isert_cma_handler: event %d status %d conn %p id %p\n",
+		 event->event, event->status, cma_id->context, cma_id);
+
+	switch (event->event) {
+	case RDMA_CM_EVENT_CONNECT_REQUEST:
+		pr_debug("RDMA_CM_EVENT_CONNECT_REQUEST: >>>>>>>>>>>>>>>\n");
+		ret = isert_connect_request(cma_id, event);
+		break;
+	case RDMA_CM_EVENT_ESTABLISHED:
+		pr_debug("RDMA_CM_EVENT_ESTABLISHED >>>>>>>>>>>>>>\n");
+		isert_connected_handler(cma_id);
+		break;
+	case RDMA_CM_EVENT_DISCONNECTED:
+		pr_debug("RDMA_CM_EVENT_DISCONNECTED: >>>>>>>>>>>>>>\n");
+		isert_disconnected_handler(cma_id);
+		break;
+	case RDMA_CM_EVENT_DEVICE_REMOVAL:
+	case RDMA_CM_EVENT_ADDR_CHANGE:
+		break;
+	case RDMA_CM_EVENT_CONNECT_ERROR:
+	default:
+		pr_err("Unknown RDMA CMA event: %d\n", event->event);
+		break;
+	}
+
+	if (ret != 0) {
+		pr_err("isert_cma_handler failed RDMA_CM_EVENT: 0x%08x %d\n",
+		       event->event, ret);
+		dump_stack();
+	}
+
+	return ret;
+}
+
+static int
+isert_post_recv(struct isert_conn *isert_conn, u32 count)
+{
+	struct ib_recv_wr *rx_wr, *rx_wr_failed;
+	int i, ret;
+	unsigned int rx_head = isert_conn->conn_rx_desc_head;
+	struct iser_rx_desc *rx_desc;
+
+	for (rx_wr = isert_conn->conn_rx_wr, i = 0; i < count; i++, rx_wr++) {
+		rx_desc		= &isert_conn->conn_rx_descs[rx_head];
+		rx_wr->wr_id	= (unsigned long)rx_desc;
+		rx_wr->sg_list	= &rx_desc->rx_sg;
+		rx_wr->num_sge	= 1;
+		rx_wr->next	= rx_wr + 1;
+		rx_head = (rx_head + 1) & (ISERT_QP_MAX_RECV_DTOS - 1);
+	}
+
+	rx_wr--;
+	rx_wr->next = NULL; /* mark end of work requests list */
+
+	isert_conn->post_recv_buf_count += count;
+	ret = ib_post_recv(isert_conn->conn_qp, isert_conn->conn_rx_wr,
+				&rx_wr_failed);
+	if (ret) {
+		pr_err("ib_post_recv() failed with ret: %d\n", ret);
+		isert_conn->post_recv_buf_count -= count;
+	} else {
+		pr_debug("isert_post_recv(): Posted %d RX buffers\n", count);
+		isert_conn->conn_rx_desc_head = rx_head;
+	}
+	return ret;
+}
+
+static int
+isert_post_send(struct isert_conn *isert_conn, struct iser_tx_desc *tx_desc)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct ib_send_wr send_wr, *send_wr_failed;
+	int ret;
+
+	ib_dma_sync_single_for_device(ib_dev, tx_desc->dma_addr,
+				      ISER_HEADERS_LEN, DMA_TO_DEVICE);
+
+	send_wr.next	= NULL;
+	send_wr.wr_id	= (unsigned long)tx_desc;
+	send_wr.sg_list	= tx_desc->tx_sg;
+	send_wr.num_sge	= tx_desc->num_sge;
+	send_wr.opcode	= IB_WR_SEND;
+	send_wr.send_flags = IB_SEND_SIGNALED;
+
+	atomic_inc(&isert_conn->post_send_buf_count);
+
+	ret = ib_post_send(isert_conn->conn_qp, &send_wr, &send_wr_failed);
+	if (ret) {
+		pr_err("ib_post_send() failed, ret: %d\n", ret);
+		atomic_dec(&isert_conn->post_send_buf_count);
+	}
+
+	return ret;
+}
+
+static void
+isert_create_send_desc(struct isert_conn *isert_conn,
+		       struct isert_cmd *isert_cmd,
+		       struct iser_tx_desc *tx_desc)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+
+	ib_dma_sync_single_for_cpu(ib_dev, tx_desc->dma_addr,
+				   ISER_HEADERS_LEN, DMA_TO_DEVICE);
+
+	memset(&tx_desc->iser_header, 0, sizeof(struct iser_hdr));
+	tx_desc->iser_header.flags = ISER_VER;
+
+	tx_desc->num_sge = 1;
+	tx_desc->isert_cmd = isert_cmd;
+
+	if (tx_desc->tx_sg[0].lkey != isert_conn->conn_mr->lkey) {
+		tx_desc->tx_sg[0].lkey = isert_conn->conn_mr->lkey;
+		pr_debug("tx_desc %p lkey mismatch, fixing\n", tx_desc);
+	}
+}
+
+static int
+isert_init_tx_hdrs(struct isert_conn *isert_conn,
+		   struct iser_tx_desc *tx_desc)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	u64 dma_addr;
+
+	dma_addr = ib_dma_map_single(ib_dev, (void *)tx_desc,
+			ISER_HEADERS_LEN, DMA_TO_DEVICE);
+	if (ib_dma_mapping_error(ib_dev, dma_addr)) {
+		pr_err("ib_dma_mapping_error() failed\n");
+		return -ENOMEM;
+	}
+
+	tx_desc->dma_addr = dma_addr;
+	tx_desc->tx_sg[0].addr	= tx_desc->dma_addr;
+	tx_desc->tx_sg[0].length = ISER_HEADERS_LEN;
+	tx_desc->tx_sg[0].lkey = isert_conn->conn_mr->lkey;
+
+	pr_debug("isert_init_tx_hdrs: Setup tx_sg[0].addr: 0x%llx length: %u"
+		 " lkey: 0x%08x\n", tx_desc->tx_sg[0].addr,
+		 tx_desc->tx_sg[0].length, tx_desc->tx_sg[0].lkey);
+
+	return 0;
+}
+
+static void
+isert_init_send_wr(struct isert_cmd *isert_cmd, struct ib_send_wr *send_wr)
+{
+	isert_cmd->rdma_wr.iser_ib_op = ISER_IB_SEND;
+	send_wr->wr_id = (unsigned long)&isert_cmd->tx_desc;
+	send_wr->opcode = IB_WR_SEND;
+	send_wr->send_flags = IB_SEND_SIGNALED;
+	send_wr->sg_list = &isert_cmd->tx_desc.tx_sg[0];
+	send_wr->num_sge = isert_cmd->tx_desc.num_sge;
+}
+
+static int
+isert_rdma_post_recvl(struct isert_conn *isert_conn)
+{
+	struct ib_recv_wr rx_wr, *rx_wr_fail;
+	struct ib_sge sge;
+	int ret;
+
+	memset(&sge, 0, sizeof(struct ib_sge));
+	sge.addr = isert_conn->login_req_dma;
+	sge.length = ISER_RX_LOGIN_SIZE;
+	sge.lkey = isert_conn->conn_mr->lkey;
+
+	pr_debug("Setup sge: addr: %llx length: %d 0x%08x\n",
+		sge.addr, sge.length, sge.lkey);
+
+	memset(&rx_wr, 0, sizeof(struct ib_recv_wr));
+	rx_wr.wr_id = (unsigned long)isert_conn->login_req_buf;
+	rx_wr.sg_list = &sge;
+	rx_wr.num_sge = 1;
+
+	isert_conn->post_recv_buf_count++;
+	ret = ib_post_recv(isert_conn->conn_qp, &rx_wr, &rx_wr_fail);
+	if (ret) {
+		pr_err("ib_post_recv() failed: %d\n", ret);
+		isert_conn->post_recv_buf_count--;
+	}
+
+	pr_debug("ib_post_recv(): returned success >>>>>>>>>>>>>>>>>>>>>>>>\n");
+	return ret;
+}
+
+static int
+isert_put_login_tx(struct iscsi_conn *conn, struct iscsi_login *login,
+		   u32 length)
+{
+	struct isert_conn *isert_conn = conn->context;
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct iser_tx_desc *tx_desc = &isert_conn->conn_login_tx_desc;
+	int ret;
+
+	isert_create_send_desc(isert_conn, NULL, tx_desc);
+
+	memcpy(&tx_desc->iscsi_header, &login->rsp[0],
+	       sizeof(struct iscsi_hdr));
+
+	isert_init_tx_hdrs(isert_conn, tx_desc);
+
+	if (length > 0) {
+		struct ib_sge *tx_dsg = &tx_desc->tx_sg[1];
+
+		ib_dma_sync_single_for_cpu(ib_dev, isert_conn->login_rsp_dma,
+					   length, DMA_TO_DEVICE);
+
+		memcpy(isert_conn->login_rsp_buf, login->rsp_buf, length);
+
+		ib_dma_sync_single_for_device(ib_dev, isert_conn->login_rsp_dma,
+					      length, DMA_TO_DEVICE);
+
+		tx_dsg->addr	= isert_conn->login_rsp_dma;
+		tx_dsg->length	= length;
+		tx_dsg->lkey	= isert_conn->conn_mr->lkey;
+		tx_desc->num_sge = 2;
+	}
+	if (!login->login_failed) {
+		if (login->login_complete) {
+			ret = isert_alloc_rx_descriptors(isert_conn);
+			if (ret)
+				return ret;
+
+			ret = isert_post_recv(isert_conn, ISERT_MIN_POSTED_RX);
+			if (ret)
+				return ret;
+
+			isert_conn->state = ISER_CONN_UP;
+			goto post_send;
+		}
+
+		ret = isert_rdma_post_recvl(isert_conn);
+		if (ret)
+			return ret;
+	}
+post_send:
+	ret = isert_post_send(isert_conn, tx_desc);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static void
+isert_rx_login_req(struct iser_rx_desc *rx_desc, int rx_buflen,
+		   struct isert_conn *isert_conn)
+{
+	struct iscsi_conn *conn = isert_conn->conn;
+	struct iscsi_login *login = conn->conn_login;
+	int size;
+
+	if (!login) {
+		pr_err("conn->conn_login is NULL\n");
+		dump_stack();
+		return;
+	}
+
+	if (login->first_request) {
+		struct iscsi_login_req *login_req =
+			(struct iscsi_login_req *)&rx_desc->iscsi_header;
+		/*
+		 * Setup the initial iscsi_login values from the leading
+		 * login request PDU.
+		 */
+		login->leading_connection = (!login_req->tsih) ? 1 : 0;
+		login->current_stage =
+			(login_req->flags & ISCSI_FLAG_LOGIN_CURRENT_STAGE_MASK)
+			 >> 2;
+		login->version_min	= login_req->min_version;
+		login->version_max	= login_req->max_version;
+		memcpy(login->isid, login_req->isid, 6);
+		login->cmd_sn		= be32_to_cpu(login_req->cmdsn);
+		login->init_task_tag	= login_req->itt;
+		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);
+	}
+
+	memcpy(&login->req[0], (void *)&rx_desc->iscsi_header, ISCSI_HDR_LEN);
+
+	size = min(rx_buflen, MAX_KEY_VALUE_PAIRS);
+	pr_debug("Using login payload size: %d, rx_buflen: %d MAX_KEY_VALUE_PAIRS: %d\n",
+		 size, rx_buflen, MAX_KEY_VALUE_PAIRS);
+	memcpy(login->req_buf, &rx_desc->data[0], size);
+
+	complete(&isert_conn->conn_login_comp);
+}
+
+static void
+isert_release_cmd(struct iscsi_cmd *cmd)
+{
+	struct isert_cmd *isert_cmd = container_of(cmd, struct isert_cmd,
+						   iscsi_cmd);
+
+	pr_debug("Entering isert_release_cmd %p >>>>>>>>>>>>>>>.\n", isert_cmd);
+
+	kfree(cmd->buf_ptr);
+	kfree(cmd->tmr_req);
+
+	kmem_cache_free(isert_cmd_cache, isert_cmd);
+}
+
+static struct iscsi_cmd
+*isert_alloc_cmd(struct iscsi_conn *conn, gfp_t gfp)
+{
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct isert_cmd *isert_cmd;
+
+	isert_cmd = kmem_cache_zalloc(isert_cmd_cache, gfp);
+	if (!isert_cmd) {
+		pr_err("Unable to allocate isert_cmd\n");
+		return NULL;
+	}
+	isert_cmd->conn = isert_conn;
+	isert_cmd->iscsi_cmd.release_cmd = &isert_release_cmd;
+
+	return &isert_cmd->iscsi_cmd;
+}
+
+static int
+isert_handle_scsi_cmd(struct isert_conn *isert_conn,
+		      struct isert_cmd *isert_cmd, struct iser_rx_desc *rx_desc,
+		      unsigned char *buf)
+{
+	struct iscsi_cmd *cmd = &isert_cmd->iscsi_cmd;
+	struct iscsi_conn *conn = isert_conn->conn;
+	struct iscsi_scsi_req *hdr = (struct iscsi_scsi_req *)buf;
+	struct scatterlist *sg;
+	int imm_data, imm_data_len, unsol_data, sg_nents, rc;
+	bool dump_payload = false;
+
+	rc = iscsit_setup_scsi_cmd(conn, cmd, buf);
+	if (rc < 0)
+		return rc;
+
+	imm_data = cmd->immediate_data;
+	imm_data_len = cmd->first_burst_len;
+	unsol_data = cmd->unsolicited_data;
+
+	rc = iscsit_process_scsi_cmd(conn, cmd, hdr);
+	if (rc < 0) {
+		return 0;
+	} else if (rc > 0) {
+		dump_payload = true;
+		goto sequence_cmd;
+	}
+
+	if (!imm_data)
+		return 0;
+
+	sg = &cmd->se_cmd.t_data_sg[0];
+	sg_nents = max(1UL, DIV_ROUND_UP(imm_data_len, PAGE_SIZE));
+
+	pr_debug("Copying Immediate SG: %p sg_nents: %u from %p imm_data_len: %d\n",
+		 sg, sg_nents, &rx_desc->data[0], imm_data_len);
+
+	sg_copy_from_buffer(sg, sg_nents, &rx_desc->data[0], imm_data_len);
+
+	cmd->write_data_done += imm_data_len;
+
+	if (cmd->write_data_done == cmd->se_cmd.data_length) {
+		spin_lock_bh(&cmd->istate_lock);
+		cmd->cmd_flags |= ICF_GOT_LAST_DATAOUT;
+		cmd->i_state = ISTATE_RECEIVED_LAST_DATAOUT;
+		spin_unlock_bh(&cmd->istate_lock);
+	}
+
+sequence_cmd:
+	rc = iscsit_sequence_cmd(conn, cmd, hdr->cmdsn);
+
+	if (!rc && dump_payload == false && unsol_data)
+		iscsit_set_unsoliticed_dataout(cmd);
+
+	if (rc == CMDSN_ERROR_CANNOT_RECOVER)
+		return iscsit_add_reject_from_cmd(
+			   ISCSI_REASON_PROTOCOL_ERROR,
+			   1, 0, (unsigned char *)hdr, cmd);
+
+	return 0;
+}
+
+static int
+isert_handle_iscsi_dataout(struct isert_conn *isert_conn,
+			   struct iser_rx_desc *rx_desc, unsigned char *buf)
+{
+	struct scatterlist *sg_start;
+	struct iscsi_conn *conn = isert_conn->conn;
+	struct iscsi_cmd *cmd = NULL;
+	struct iscsi_data *hdr = (struct iscsi_data *)buf;
+	u32 unsol_data_len = ntoh24(hdr->dlength);
+	int rc, sg_nents, sg_off, page_off;
+
+	rc = iscsit_check_dataout_hdr(conn, buf, &cmd);
+	if (rc < 0)
+		return rc;
+	else if (!cmd)
+		return 0;
+	/*
+	 * FIXME: Unexpected unsolicited_data out
+	 */
+	if (!cmd->unsolicited_data) {
+		pr_err("Received unexpected solicited data payload\n");
+		dump_stack();
+		return -1;
+	}
+
+	pr_debug("Unsolicited DataOut unsol_data_len: %u, write_data_done: %u, data_length: %u\n",
+		 unsol_data_len, cmd->write_data_done, cmd->se_cmd.data_length);
+
+	sg_off = cmd->write_data_done / PAGE_SIZE;
+	sg_start = &cmd->se_cmd.t_data_sg[sg_off];
+	sg_nents = max(1UL, DIV_ROUND_UP(unsol_data_len, PAGE_SIZE));
+	page_off = cmd->write_data_done % PAGE_SIZE;
+	/*
+	 * FIXME: Non page-aligned unsolicited_data out
+	 */
+	if (page_off) {
+		pr_err("Received unexpected non-page aligned data payload\n");
+		dump_stack();
+		return -1;
+	}
+	pr_debug("Copying DataOut: sg_start: %p, sg_off: %u sg_nents: %u from %p %u\n",
+		 sg_start, sg_off, sg_nents, &rx_desc->data[0], unsol_data_len);
+
+	sg_copy_from_buffer(sg_start, sg_nents, &rx_desc->data[0],
+			    unsol_data_len);
+
+	rc = iscsit_check_dataout_payload(cmd, hdr, false);
+	if (rc < 0)
+		return rc;
+
+	return 0;
+}
+
+static int
+isert_rx_opcode(struct isert_conn *isert_conn, struct iser_rx_desc *rx_desc,
+		uint32_t read_stag, uint64_t read_va,
+		uint32_t write_stag, uint64_t write_va)
+{
+	struct iscsi_hdr *hdr = &rx_desc->iscsi_header;
+	struct iscsi_conn *conn = isert_conn->conn;
+	struct iscsi_cmd *cmd;
+	struct isert_cmd *isert_cmd;
+	int ret = -EINVAL;
+	u8 opcode = (hdr->opcode & ISCSI_OPCODE_MASK);
+
+	switch (opcode) {
+	case ISCSI_OP_SCSI_CMD:
+		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		if (!cmd)
+			break;
+
+		isert_cmd = container_of(cmd, struct isert_cmd, iscsi_cmd);
+		isert_cmd->read_stag = read_stag;
+		isert_cmd->read_va = read_va;
+		isert_cmd->write_stag = write_stag;
+		isert_cmd->write_va = write_va;
+
+		ret = isert_handle_scsi_cmd(isert_conn, isert_cmd,
+					rx_desc, (unsigned char *)hdr);
+		break;
+	case ISCSI_OP_NOOP_OUT:
+		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		if (!cmd)
+			break;
+
+		ret = iscsit_handle_nop_out(conn, cmd, (unsigned char *)hdr);
+		break;
+	case ISCSI_OP_SCSI_DATA_OUT:
+		ret = isert_handle_iscsi_dataout(isert_conn, rx_desc,
+						(unsigned char *)hdr);
+		break;
+	case ISCSI_OP_SCSI_TMFUNC:
+		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		if (!cmd)
+			break;
+
+		ret = iscsit_handle_task_mgt_cmd(conn, cmd,
+						(unsigned char *)hdr);
+		break;
+	case ISCSI_OP_LOGOUT:
+		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		if (!cmd)
+			break;
+
+		ret = iscsit_handle_logout_cmd(conn, cmd, (unsigned char *)hdr);
+		if (ret > 0)
+			wait_for_completion_timeout(&conn->conn_logout_comp,
+						    SECONDS_FOR_LOGOUT_COMP *
+						    HZ);
+		break;
+	default:
+		pr_err("Got unknown iSCSI OpCode: 0x%02x\n", opcode);
+		dump_stack();
+		break;
+	}
+
+	return ret;
+}
+
+static void
+isert_rx_do_work(struct iser_rx_desc *rx_desc, struct isert_conn *isert_conn)
+{
+	struct iser_hdr *iser_hdr = &rx_desc->iser_header;
+	uint64_t read_va = 0, write_va = 0;
+	uint32_t read_stag = 0, write_stag = 0;
+	int rc;
+
+	switch (iser_hdr->flags & 0xF0) {
+	case ISCSI_CTRL:
+		if (iser_hdr->flags & ISER_RSV) {
+			read_stag = be32_to_cpu(iser_hdr->read_stag);
+			read_va = be64_to_cpu(iser_hdr->read_va);
+			pr_debug("ISER_RSV: read_stag: 0x%08x read_va: 0x%16llx\n",
+				 read_stag, (unsigned long long)read_va);
+		}
+		if (iser_hdr->flags & ISER_WSV) {
+			write_stag = be32_to_cpu(iser_hdr->write_stag);
+			write_va = be64_to_cpu(iser_hdr->write_va);
+			pr_debug("ISER_WSV: write__stag: 0x%08x write_va: 0x%16llx\n",
+				 write_stag, (unsigned long long)write_va);
+		}
+
+		pr_debug("ISER ISCSI_CTRL PDU\n");
+		break;
+	case ISER_HELLO:
+		pr_err("iSER Hello message\n");
+		break;
+	default:
+		pr_warn("Unknown iSER hdr flags: 0x%02x\n", iser_hdr->flags);
+		break;
+	}
+
+	rc = isert_rx_opcode(isert_conn, rx_desc,
+			     read_stag, read_va, write_stag, write_va);
+}
+
+static void
+isert_rx_completion(struct iser_rx_desc *desc, struct isert_conn *isert_conn,
+		    unsigned long xfer_len)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct iscsi_hdr *hdr;
+	u64 rx_dma;
+	int rx_buflen, outstanding;
+
+	if ((char *)desc == isert_conn->login_req_buf) {
+		rx_dma = isert_conn->login_req_dma;
+		rx_buflen = ISER_RX_LOGIN_SIZE;
+		pr_debug("ISER login_buf: Using rx_dma: 0x%llx, rx_buflen: %d\n",
+			 rx_dma, rx_buflen);
+	} else {
+		rx_dma = desc->dma_addr;
+		rx_buflen = ISER_RX_PAYLOAD_SIZE;
+		pr_debug("ISER req_buf: Using rx_dma: 0x%llx, rx_buflen: %d\n",
+			 rx_dma, rx_buflen);
+	}
+
+	ib_dma_sync_single_for_cpu(ib_dev, rx_dma, rx_buflen, DMA_FROM_DEVICE);
+
+	hdr = &desc->iscsi_header;
+	pr_debug("iSCSI opcode: 0x%02x, ITT: 0x%08x, flags: 0x%02x dlen: %d\n",
+		 hdr->opcode, hdr->itt, hdr->flags,
+		 (int)(xfer_len - ISER_HEADERS_LEN));
+
+	if ((char *)desc == isert_conn->login_req_buf)
+		isert_rx_login_req(desc, xfer_len - ISER_HEADERS_LEN,
+				   isert_conn);
+	else
+		isert_rx_do_work(desc, isert_conn);
+
+	ib_dma_sync_single_for_device(ib_dev, rx_dma, rx_buflen,
+				      DMA_FROM_DEVICE);
+
+	isert_conn->post_recv_buf_count--;
+	pr_debug("iSERT: Decremented post_recv_buf_count: %d\n",
+		 isert_conn->post_recv_buf_count);
+
+	if ((char *)desc == isert_conn->login_req_buf)
+		return;
+
+	outstanding = isert_conn->post_recv_buf_count;
+	if (outstanding + ISERT_MIN_POSTED_RX <= ISERT_QP_MAX_RECV_DTOS) {
+		int err, count = min(ISERT_QP_MAX_RECV_DTOS - outstanding,
+				ISERT_MIN_POSTED_RX);
+		err = isert_post_recv(isert_conn, count);
+		if (err) {
+			pr_err("isert_post_recv() count: %d failed, %d\n",
+			       count, err);
+		}
+	}
+}
+
+static void
+isert_unmap_cmd(struct isert_cmd *isert_cmd, struct isert_conn *isert_conn)
+{
+	struct isert_rdma_wr *wr = &isert_cmd->rdma_wr;
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+
+	pr_debug("isert_unmap_cmd >>>>>>>>>>>>>>>>>>>>>>>\n");
+
+	if (wr->sge) {
+		ib_dma_unmap_sg(ib_dev, wr->sge, wr->num_sge, DMA_TO_DEVICE);
+		wr->sge = NULL;
+	}
+
+	kfree(wr->send_wr);
+	wr->send_wr = NULL;
+
+	kfree(isert_cmd->ib_sge);
+	isert_cmd->ib_sge = NULL;
+}
+
+static void
+isert_put_cmd(struct isert_cmd *isert_cmd)
+{
+	struct iscsi_cmd *cmd = &isert_cmd->iscsi_cmd;
+	struct isert_conn *isert_conn = isert_cmd->conn;
+	struct iscsi_conn *conn;
+
+	pr_debug("Entering isert_put_cmd: %p\n", isert_cmd);
+
+	switch (cmd->iscsi_opcode) {
+	case ISCSI_OP_SCSI_CMD:
+		conn = isert_conn->conn;
+
+		spin_lock_bh(&conn->cmd_lock);
+		if (!list_empty(&cmd->i_conn_node))
+			list_del(&cmd->i_conn_node);
+		spin_unlock_bh(&conn->cmd_lock);
+
+		if (cmd->data_direction == DMA_TO_DEVICE)
+			iscsit_stop_dataout_timer(cmd);
+
+		isert_unmap_cmd(isert_cmd, isert_conn);
+		/*
+		 * Fall-through
+		 */
+	case ISCSI_OP_SCSI_TMFUNC:
+		transport_generic_free_cmd(&cmd->se_cmd, 0);
+		break;
+	case ISCSI_OP_REJECT:
+	case ISCSI_OP_NOOP_OUT:
+		conn = isert_conn->conn;
+
+		spin_lock_bh(&conn->cmd_lock);
+		if (!list_empty(&cmd->i_conn_node))
+			list_del(&cmd->i_conn_node);
+		spin_unlock_bh(&conn->cmd_lock);
+
+		/*
+		 * Handle special case for REJECT when iscsi_add_reject*() has
+		 * overwritten the original iscsi_opcode assignment, and the
+		 * associated cmd->se_cmd needs to be released.
+		 */
+		if (cmd->se_cmd.se_tfo != NULL) {
+			transport_generic_free_cmd(&cmd->se_cmd, 0);
+			break;
+		}
+		/*
+		 * Fall-through
+		 */
+	default:
+		isert_release_cmd(cmd);
+		break;
+	}
+}
+
+static void
+isert_unmap_tx_desc(struct iser_tx_desc *tx_desc, struct ib_device *ib_dev)
+{
+	if (tx_desc->dma_addr != 0) {
+		pr_debug("Calling ib_dma_unmap_single for tx_desc->dma_addr\n");
+		ib_dma_unmap_single(ib_dev, tx_desc->dma_addr,
+				    ISER_HEADERS_LEN, DMA_TO_DEVICE);
+		tx_desc->dma_addr = 0;
+	}
+}
+
+static void
+isert_completion_put(struct iser_tx_desc *tx_desc, struct isert_cmd *isert_cmd,
+		     struct ib_device *ib_dev)
+{
+	if (isert_cmd->sense_buf_dma != 0) {
+		pr_debug("Calling ib_dma_unmap_single for isert_cmd->sense_buf_dma\n");
+		ib_dma_unmap_single(ib_dev, isert_cmd->sense_buf_dma,
+				    isert_cmd->sense_buf_len, DMA_TO_DEVICE);
+		isert_cmd->sense_buf_dma = 0;
+	}
+
+	isert_unmap_tx_desc(tx_desc, ib_dev);
+	isert_put_cmd(isert_cmd);
+}
+
+static void
+isert_completion_rdma_read(struct iser_tx_desc *tx_desc,
+			   struct isert_cmd *isert_cmd)
+{
+	struct isert_rdma_wr *wr = &isert_cmd->rdma_wr;
+	struct iscsi_cmd *cmd = &isert_cmd->iscsi_cmd;
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct ib_device *ib_dev = isert_cmd->conn->conn_cm_id->device;
+
+	iscsit_stop_dataout_timer(cmd);
+
+	if (wr->sge) {
+		pr_debug("isert_do_rdma_read_comp: Unmapping wr->sge from t_data_sg\n");
+		ib_dma_unmap_sg(ib_dev, wr->sge, wr->num_sge, DMA_TO_DEVICE);
+		wr->sge = NULL;
+	}
+
+	if (isert_cmd->ib_sge) {
+		pr_debug("isert_do_rdma_read_comp: Freeing isert_cmd->ib_sge\n");
+		kfree(isert_cmd->ib_sge);
+		isert_cmd->ib_sge = NULL;
+	}
+
+	cmd->write_data_done = se_cmd->data_length;
+
+	pr_debug("isert_do_rdma_read_comp, calling target_execute_cmd\n");
+	spin_lock_bh(&cmd->istate_lock);
+	cmd->cmd_flags |= ICF_GOT_LAST_DATAOUT;
+	cmd->i_state = ISTATE_RECEIVED_LAST_DATAOUT;
+	spin_unlock_bh(&cmd->istate_lock);
+
+	target_execute_cmd(se_cmd);
+}
+
+static void
+isert_do_control_comp(struct work_struct *work)
+{
+	struct isert_cmd *isert_cmd = container_of(work,
+			struct isert_cmd, comp_work);
+	struct isert_conn *isert_conn = isert_cmd->conn;
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct iscsi_cmd *cmd = &isert_cmd->iscsi_cmd;
+
+	switch (cmd->i_state) {
+	case ISTATE_SEND_TASKMGTRSP:
+		pr_debug("Calling iscsit_tmr_post_handler >>>>>>>>>>>>>>>>>\n");
+
+		atomic_dec(&isert_conn->post_send_buf_count);
+		iscsit_tmr_post_handler(cmd, cmd->conn);
+
+		cmd->i_state = ISTATE_SENT_STATUS;
+		isert_completion_put(&isert_cmd->tx_desc, isert_cmd, ib_dev);
+		break;
+	case ISTATE_SEND_REJECT:
+		pr_debug("Got isert_do_control_comp ISTATE_SEND_REJECT: >>>\n");
+		atomic_dec(&isert_conn->post_send_buf_count);
+
+		cmd->i_state = ISTATE_SENT_STATUS;
+		complete(&cmd->reject_comp);
+		isert_completion_put(&isert_cmd->tx_desc, isert_cmd, ib_dev);
+	case ISTATE_SEND_LOGOUTRSP:
+		pr_debug("Calling iscsit_logout_post_handler >>>>>>>>>>>>>>\n");
+		/*
+		 * Call atomic_dec(&isert_conn->post_send_buf_count)
+		 * from isert_free_conn()
+		 */
+		isert_conn->logout_posted = true;
+		iscsit_logout_post_handler(cmd, cmd->conn);
+		break;
+	default:
+		pr_err("Unknown do_control_comp i_state %d\n", cmd->i_state);
+		dump_stack();
+		break;
+	}
+}
+
+static void
+isert_response_completion(struct iser_tx_desc *tx_desc,
+			  struct isert_cmd *isert_cmd,
+			  struct isert_conn *isert_conn,
+			  struct ib_device *ib_dev)
+{
+	struct iscsi_cmd *cmd = &isert_cmd->iscsi_cmd;
+
+	if (cmd->i_state == ISTATE_SEND_TASKMGTRSP ||
+	    cmd->i_state == ISTATE_SEND_LOGOUTRSP) {
+		isert_unmap_tx_desc(tx_desc, ib_dev);
+
+		INIT_WORK(&isert_cmd->comp_work, isert_do_control_comp);
+		queue_work(isert_comp_wq, &isert_cmd->comp_work);
+		return;
+	}
+	atomic_dec(&isert_conn->post_send_buf_count);
+
+	cmd->i_state = ISTATE_SENT_STATUS;
+	isert_completion_put(tx_desc, isert_cmd, ib_dev);
+}
+
+static void
+isert_send_completion(struct iser_tx_desc *tx_desc,
+		      struct isert_conn *isert_conn)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct isert_cmd *isert_cmd = tx_desc->isert_cmd;
+	struct isert_rdma_wr *wr;
+
+	if (!isert_cmd) {
+		atomic_dec(&isert_conn->post_send_buf_count);
+		isert_unmap_tx_desc(tx_desc, ib_dev);
+		return;
+	}
+	wr = &isert_cmd->rdma_wr;
+
+	switch (wr->iser_ib_op) {
+	case ISER_IB_RECV:
+		pr_err("isert_send_completion: Got ISER_IB_RECV\n");
+		dump_stack();
+		break;
+	case ISER_IB_SEND:
+		pr_debug("isert_send_completion: Got ISER_IB_SEND\n");
+		isert_response_completion(tx_desc, isert_cmd,
+					  isert_conn, ib_dev);
+		break;
+	case ISER_IB_RDMA_WRITE:
+		pr_err("isert_send_completion: Got ISER_IB_RDMA_WRITE\n");
+		dump_stack();
+		break;
+	case ISER_IB_RDMA_READ:
+		pr_debug("isert_send_completion: Got ISER_IB_RDMA_READ:\n");
+
+		atomic_dec(&isert_conn->post_send_buf_count);
+		isert_completion_rdma_read(tx_desc, isert_cmd);
+		break;
+	default:
+		pr_err("Unknown wr->iser_ib_op: 0x%02x\n", wr->iser_ib_op);
+		dump_stack();
+		break;
+	}
+}
+
+static void
+isert_cq_comp_err(struct iser_tx_desc *tx_desc, struct isert_conn *isert_conn)
+{
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+
+	if (tx_desc) {
+		struct isert_cmd *isert_cmd = tx_desc->isert_cmd;
+
+		if (!isert_cmd)
+			isert_unmap_tx_desc(tx_desc, ib_dev);
+		else
+			isert_completion_put(tx_desc, isert_cmd, ib_dev);
+	}
+
+	if (isert_conn->post_recv_buf_count == 0 &&
+	    atomic_read(&isert_conn->post_send_buf_count) == 0) {
+		pr_debug("isert_cq_comp_err >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
+		pr_debug("Calling wake_up from isert_cq_comp_err\n");
+
+		isert_conn->state = ISER_CONN_TERMINATING;
+		wake_up(&isert_conn->conn_wait_comp_err);
+	}
+}
+
+static void
+isert_cq_tx_work(struct work_struct *work)
+{
+	struct isert_cq_desc *cq_desc = container_of(work,
+				struct isert_cq_desc, cq_tx_work);
+	struct isert_device *device = cq_desc->device;
+	int cq_index = cq_desc->cq_index;
+	struct ib_cq *tx_cq = device->dev_tx_cq[cq_index];
+	struct isert_conn *isert_conn;
+	struct iser_tx_desc *tx_desc;
+	struct ib_wc wc;
+
+	while (ib_poll_cq(tx_cq, 1, &wc) == 1) {
+		tx_desc = (struct iser_tx_desc *)(unsigned long)wc.wr_id;
+		isert_conn = wc.qp->qp_context;
+
+		if (wc.status == IB_WC_SUCCESS) {
+			isert_send_completion(tx_desc, isert_conn);
+		} else {
+			pr_debug("TX wc.status != IB_WC_SUCCESS >>>>>>>>>>>>>>\n");
+			pr_debug("TX wc.status: 0x%08x\n", wc.status);
+			atomic_dec(&isert_conn->post_send_buf_count);
+			isert_cq_comp_err(tx_desc, isert_conn);
+		}
+	}
+
+	ib_req_notify_cq(tx_cq, IB_CQ_NEXT_COMP);
+}
+
+static void
+isert_cq_tx_callback(struct ib_cq *cq, void *context)
+{
+	struct isert_cq_desc *cq_desc = (struct isert_cq_desc *)context;
+
+	INIT_WORK(&cq_desc->cq_tx_work, isert_cq_tx_work);
+	queue_work(isert_comp_wq, &cq_desc->cq_tx_work);
+}
+
+static void
+isert_cq_rx_work(struct work_struct *work)
+{
+	struct isert_cq_desc *cq_desc = container_of(work,
+			struct isert_cq_desc, cq_rx_work);
+	struct isert_device *device = cq_desc->device;
+	int cq_index = cq_desc->cq_index;
+	struct ib_cq *rx_cq = device->dev_rx_cq[cq_index];
+	struct isert_conn *isert_conn;
+	struct iser_rx_desc *rx_desc;
+	struct ib_wc wc;
+	unsigned long xfer_len;
+
+	while (ib_poll_cq(rx_cq, 1, &wc) == 1) {
+		rx_desc = (struct iser_rx_desc *)(unsigned long)wc.wr_id;
+		isert_conn = wc.qp->qp_context;
+
+		if (wc.status == IB_WC_SUCCESS) {
+			xfer_len = (unsigned long)wc.byte_len;
+			isert_rx_completion(rx_desc, isert_conn, xfer_len);
+		} else {
+			pr_debug("RX wc.status != IB_WC_SUCCESS >>>>>>>>>>>>>>\n");
+			if (wc.status != IB_WC_WR_FLUSH_ERR)
+				pr_debug("RX wc.status: 0x%08x\n", wc.status);
+
+			isert_conn->post_recv_buf_count--;
+			isert_cq_comp_err(NULL, isert_conn);
+		}
+	}
+
+	ib_req_notify_cq(rx_cq, IB_CQ_NEXT_COMP);
+}
+
+static void
+isert_cq_rx_callback(struct ib_cq *cq, void *context)
+{
+	struct isert_cq_desc *cq_desc = (struct isert_cq_desc *)context;
+
+	INIT_WORK(&cq_desc->cq_rx_work, isert_cq_rx_work);
+	queue_work(isert_rx_wq, &cq_desc->cq_rx_work);
+}
+
+static int
+isert_post_response(struct isert_conn *isert_conn, struct isert_cmd *isert_cmd)
+{
+	struct ib_send_wr *wr_failed;
+	int ret;
+
+	atomic_inc(&isert_conn->post_send_buf_count);
+
+	ret = ib_post_send(isert_conn->conn_qp, &isert_cmd->tx_desc.send_wr,
+			   &wr_failed);
+	if (ret) {
+		pr_err("ib_post_send failed with %d\n", ret);
+		atomic_dec(&isert_conn->post_send_buf_count);
+		return ret;
+	}
+	return ret;
+}
+
+static int
+isert_put_response(struct iscsi_conn *conn, struct iscsi_cmd *cmd)
+{
+	struct isert_cmd *isert_cmd = container_of(cmd,
+					struct isert_cmd, iscsi_cmd);
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct ib_send_wr *send_wr = &isert_cmd->tx_desc.send_wr;
+	struct iscsi_scsi_rsp *hdr = (struct iscsi_scsi_rsp *)
+				&isert_cmd->tx_desc.iscsi_header;
+
+	isert_create_send_desc(isert_conn, isert_cmd, &isert_cmd->tx_desc);
+	iscsit_build_rsp_pdu(cmd, conn, true, hdr);
+	isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
+	/*
+	 * Attach SENSE DATA payload to iSCSI Response PDU
+	 */
+	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))) {
+		struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+		struct ib_sge *tx_dsg = &isert_cmd->tx_desc.tx_sg[1];
+		u32 padding, sense_len;
+
+		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, (u32)cmd->se_cmd.scsi_sense_length);
+		sense_len = cmd->se_cmd.scsi_sense_length + padding;
+
+		isert_cmd->sense_buf_dma = ib_dma_map_single(ib_dev,
+				(void *)cmd->sense_buffer, sense_len,
+				DMA_TO_DEVICE);
+
+		isert_cmd->sense_buf_len = sense_len;
+		tx_dsg->addr	= isert_cmd->sense_buf_dma;
+		tx_dsg->length	= sense_len;
+		tx_dsg->lkey	= isert_conn->conn_mr->lkey;
+		isert_cmd->tx_desc.num_sge = 2;
+	}
+
+	isert_init_send_wr(isert_cmd, send_wr);
+
+	pr_debug("Posting SCSI Response IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+
+	return isert_post_response(isert_conn, isert_cmd);
+}
+
+static int
+isert_put_nopin(struct iscsi_cmd *cmd, struct iscsi_conn *conn,
+		bool nopout_response)
+{
+	struct isert_cmd *isert_cmd = container_of(cmd,
+				struct isert_cmd, iscsi_cmd);
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct ib_send_wr *send_wr = &isert_cmd->tx_desc.send_wr;
+
+	isert_create_send_desc(isert_conn, isert_cmd, &isert_cmd->tx_desc);
+	iscsit_build_nopin_rsp(cmd, conn, (struct iscsi_nopin *)
+			       &isert_cmd->tx_desc.iscsi_header,
+			       nopout_response);
+	isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
+	isert_init_send_wr(isert_cmd, send_wr);
+
+	pr_debug("Posting NOPIN Reponse IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+
+	return isert_post_response(isert_conn, isert_cmd);
+}
+
+static int
+isert_put_logout_rsp(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+{
+	struct isert_cmd *isert_cmd = container_of(cmd,
+				struct isert_cmd, iscsi_cmd);
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct ib_send_wr *send_wr = &isert_cmd->tx_desc.send_wr;
+
+	isert_create_send_desc(isert_conn, isert_cmd, &isert_cmd->tx_desc);
+	iscsit_build_logout_rsp(cmd, conn, (struct iscsi_logout_rsp *)
+				&isert_cmd->tx_desc.iscsi_header);
+	isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
+	isert_init_send_wr(isert_cmd, send_wr);
+
+	pr_debug("Posting Logout Response IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+
+	return isert_post_response(isert_conn, isert_cmd);
+}
+
+static int
+isert_put_tm_rsp(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+{
+	struct isert_cmd *isert_cmd = container_of(cmd,
+				struct isert_cmd, iscsi_cmd);
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct ib_send_wr *send_wr = &isert_cmd->tx_desc.send_wr;
+
+	isert_create_send_desc(isert_conn, isert_cmd, &isert_cmd->tx_desc);
+	iscsit_build_task_mgt_rsp(cmd, conn, (struct iscsi_tm_rsp *)
+				  &isert_cmd->tx_desc.iscsi_header);
+	isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
+	isert_init_send_wr(isert_cmd, send_wr);
+
+	pr_debug("Posting Task Management Response IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+
+	return isert_post_response(isert_conn, isert_cmd);
+}
+
+static int
+isert_put_reject(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+{
+	struct isert_cmd *isert_cmd = container_of(cmd,
+				struct isert_cmd, iscsi_cmd);
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct ib_send_wr *send_wr = &isert_cmd->tx_desc.send_wr;
+
+	isert_create_send_desc(isert_conn, isert_cmd, &isert_cmd->tx_desc);
+	iscsit_build_reject(cmd, conn, (struct iscsi_reject *)
+				&isert_cmd->tx_desc.iscsi_header);
+	isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
+	isert_init_send_wr(isert_cmd, send_wr);
+
+	pr_debug("Posting Reject IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
+
+	return isert_post_response(isert_conn, isert_cmd);
+}
+
+static int
+isert_build_rdma_wr(struct isert_conn *isert_conn, struct isert_cmd *isert_cmd,
+		    struct ib_sge *ib_sge, struct ib_send_wr *send_wr,
+		    u32 data_left, u32 offset)
+{
+	struct iscsi_cmd *cmd = &isert_cmd->iscsi_cmd;
+	struct scatterlist *sg_start, *tmp_sg;
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	u32 sg_off, page_off;
+	int i = 0, sg_nents;
+
+	sg_off = offset / PAGE_SIZE;
+	sg_start = &cmd->se_cmd.t_data_sg[sg_off];
+	sg_nents = min(cmd->se_cmd.t_data_nents - sg_off, isert_conn->max_sge);
+	page_off = offset % PAGE_SIZE;
+
+	send_wr->sg_list = ib_sge;
+	send_wr->num_sge = sg_nents;
+	send_wr->wr_id = (unsigned long)&isert_cmd->tx_desc;
+	/*
+	 * Perform mapping of TCM scatterlist memory ib_sge dma_addr.
+	 */
+	for_each_sg(sg_start, tmp_sg, sg_nents, i) {
+		pr_debug("ISER RDMA from SGL dma_addr: 0x%16llx dma_len: %u, page_off: %u\n",
+			 (unsigned long long)tmp_sg->dma_address,
+			 tmp_sg->length, page_off);
+
+		ib_sge->addr = ib_sg_dma_address(ib_dev, tmp_sg) + page_off;
+		ib_sge->length = min_t(u32, data_left,
+				ib_sg_dma_len(ib_dev, tmp_sg) - page_off);
+		ib_sge->lkey = isert_conn->conn_mr->lkey;
+
+		pr_debug("RDMA ib_sge: addr: 0x%16llx  length: %u\n",
+			 ib_sge->addr, ib_sge->length);
+		page_off = 0;
+		data_left -= ib_sge->length;
+		ib_sge++;
+		pr_debug("Incrementing ib_sge pointer to %p\n", ib_sge);
+	}
+
+	pr_debug("Set outgoing sg_list: %p num_sg: %u from TCM SGLs\n",
+		 send_wr->sg_list, send_wr->num_sge);
+
+	return sg_nents;
+}
+
+static int
+isert_put_datain(struct iscsi_conn *conn, struct iscsi_cmd *cmd)
+{
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct isert_cmd *isert_cmd = container_of(cmd,
+					struct isert_cmd, iscsi_cmd);
+	struct isert_rdma_wr *wr = &isert_cmd->rdma_wr;
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct ib_send_wr *wr_failed, *send_wr;
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct ib_sge *ib_sge;
+	struct scatterlist *sg;
+	u32 offset = 0, data_len, data_left, rdma_write_max;
+	int rc, ret = 0, count, sg_nents, i, ib_sge_cnt;
+
+	pr_debug("RDMA_WRITE: data_length: %u\n", se_cmd->data_length);
+
+	sg = &se_cmd->t_data_sg[0];
+	sg_nents = se_cmd->t_data_nents;
+
+	count = ib_dma_map_sg(ib_dev, sg, sg_nents, DMA_TO_DEVICE);
+	if (unlikely(!count)) {
+		pr_err("Unable to map put_datain SGs\n");
+		return -EINVAL;
+	}
+	wr->sge = sg;
+	wr->num_sge = sg_nents;
+	pr_debug("Mapped IB count: %u sg: %p sg_nents: %u for RDMA_WRITE\n",
+		 count, sg, sg_nents);
+
+	ib_sge = kzalloc(sizeof(struct ib_sge) * sg_nents, GFP_KERNEL);
+	if (!ib_sge) {
+		pr_warn("Unable to allocate datain ib_sge\n");
+		ret = -ENOMEM;
+		goto unmap_sg;
+	}
+	isert_cmd->ib_sge = ib_sge;
+
+	pr_debug("Allocated ib_sge: %p from t_data_ents: %d for RDMA_WRITE\n",
+		 ib_sge, se_cmd->t_data_nents);
+
+	wr->send_wr_num = DIV_ROUND_UP(sg_nents, isert_conn->max_sge);
+	wr->send_wr = kzalloc(sizeof(struct ib_send_wr) * wr->send_wr_num,
+				GFP_KERNEL);
+	if (!wr->send_wr) {
+		pr_err("Unable to allocate wr->send_wr\n");
+		ret = -ENOMEM;
+		goto unmap_sg;
+	}
+	pr_debug("Allocated wr->send_wr: %p wr->send_wr_num: %u\n",
+		 wr->send_wr, wr->send_wr_num);
+
+	iscsit_increment_maxcmdsn(cmd, conn->sess);
+	cmd->stat_sn = conn->stat_sn++;
+
+	wr->isert_cmd = isert_cmd;
+	rdma_write_max = isert_conn->max_sge * PAGE_SIZE;
+	data_left = se_cmd->data_length;
+
+	for (i = 0; i < wr->send_wr_num; i++) {
+		send_wr = &isert_cmd->rdma_wr.send_wr[i];
+		data_len = min(data_left, rdma_write_max);
+
+		send_wr->opcode = IB_WR_RDMA_WRITE;
+		send_wr->send_flags = 0;
+		send_wr->wr.rdma.remote_addr = isert_cmd->read_va + offset;
+		send_wr->wr.rdma.rkey = isert_cmd->read_stag;
+
+		ib_sge_cnt = isert_build_rdma_wr(isert_conn, isert_cmd, ib_sge,
+					send_wr, data_len, offset);
+		ib_sge += ib_sge_cnt;
+
+		if (i + 1 == wr->send_wr_num)
+			send_wr->next = &isert_cmd->tx_desc.send_wr;
+		else
+			send_wr->next = &wr->send_wr[i + 1];
+
+		offset += data_len;
+		data_left -= data_len;
+	}
+	/*
+	 * Build isert_conn->tx_desc for iSCSI response PDU and attach
+	 */
+	isert_create_send_desc(isert_conn, isert_cmd, &isert_cmd->tx_desc);
+	iscsit_build_rsp_pdu(cmd, conn, false, (struct iscsi_scsi_rsp *)
+			     &isert_cmd->tx_desc.iscsi_header);
+	isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
+	isert_init_send_wr(isert_cmd, &isert_cmd->tx_desc.send_wr);
+
+	atomic_inc(&isert_conn->post_send_buf_count);
+
+	rc = ib_post_send(isert_conn->conn_qp, wr->send_wr, &wr_failed);
+	if (rc) {
+		pr_warn("ib_post_send() failed for IB_WR_RDMA_WRITE\n");
+		atomic_dec(&isert_conn->post_send_buf_count);
+	}
+	pr_debug("Posted RDMA_WRITE + Response for iSER Data READ\n");
+	return 1;
+
+unmap_sg:
+	ib_dma_unmap_sg(ib_dev, sg, sg_nents, DMA_TO_DEVICE);
+	return ret;
+}
+
+static int
+isert_get_dataout(struct iscsi_conn *conn, struct iscsi_cmd *cmd, bool recovery)
+{
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct isert_cmd *isert_cmd = container_of(cmd,
+					struct isert_cmd, iscsi_cmd);
+	struct isert_rdma_wr *wr = &isert_cmd->rdma_wr;
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	struct ib_send_wr *wr_failed, *send_wr;
+	struct ib_sge *ib_sge;
+	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
+	struct scatterlist *sg_start;
+	u32 sg_off, sg_nents, page_off, va_offset = 0;
+	u32 offset = 0, data_len, data_left, rdma_write_max;
+	int rc, ret = 0, count, i, ib_sge_cnt;
+
+	pr_debug("RDMA_READ: data_length: %u write_data_done: %u\n",
+		 se_cmd->data_length, cmd->write_data_done);
+
+	sg_off = cmd->write_data_done / PAGE_SIZE;
+	sg_start = &cmd->se_cmd.t_data_sg[sg_off];
+	page_off = cmd->write_data_done % PAGE_SIZE;
+
+	pr_debug("RDMA_READ: sg_off: %d, sg_start: %p page_off: %d\n",
+		 sg_off, sg_start, page_off);
+
+	data_left = se_cmd->data_length - cmd->write_data_done;
+	sg_nents = se_cmd->t_data_nents - sg_off;
+
+	pr_debug("RDMA_READ: data_left: %d, sg_nents: %d\n",
+		 data_left, sg_nents);
+
+	count = ib_dma_map_sg(ib_dev, sg_start, sg_nents, DMA_FROM_DEVICE);
+	if (unlikely(!count)) {
+		pr_err("Unable to map get_dataout SGs\n");
+		return -EINVAL;
+	}
+	wr->sge = sg_start;
+	wr->num_sge = sg_nents;
+	pr_debug("Mapped IB count: %u sg_start: %p sg_nents: %u for RDMA_READ\n",
+		 count, sg_start, sg_nents);
+
+	ib_sge = kzalloc(sizeof(struct ib_sge) * sg_nents, GFP_KERNEL);
+	if (!ib_sge) {
+		pr_warn("Unable to allocate dataout ib_sge\n");
+		ret = -ENOMEM;
+		goto unmap_sg;
+	}
+	isert_cmd->ib_sge = ib_sge;
+
+	pr_debug("Using ib_sge: %p from sg_ents: %d for RDMA_READ\n",
+		 ib_sge, sg_nents);
+
+	wr->send_wr_num = DIV_ROUND_UP(sg_nents, isert_conn->max_sge);
+	wr->send_wr = kzalloc(sizeof(struct ib_send_wr) * wr->send_wr_num,
+				GFP_KERNEL);
+	if (!wr->send_wr) {
+		pr_debug("Unable to allocate wr->send_wr\n");
+		ret = -ENOMEM;
+		goto unmap_sg;
+	}
+	pr_debug("Allocated wr->send_wr: %p wr->send_wr_num: %u\n",
+		 wr->send_wr, wr->send_wr_num);
+
+	isert_cmd->tx_desc.isert_cmd = isert_cmd;
+
+	wr->iser_ib_op = ISER_IB_RDMA_READ;
+	wr->isert_cmd = isert_cmd;
+	rdma_write_max = isert_conn->max_sge * PAGE_SIZE;
+	offset = cmd->write_data_done;
+
+	for (i = 0; i < wr->send_wr_num; i++) {
+		send_wr = &isert_cmd->rdma_wr.send_wr[i];
+		data_len = min(data_left, rdma_write_max);
+
+		send_wr->opcode = IB_WR_RDMA_READ;
+		send_wr->wr.rdma.remote_addr = isert_cmd->write_va + va_offset;
+		send_wr->wr.rdma.rkey = isert_cmd->write_stag;
+
+		ib_sge_cnt = isert_build_rdma_wr(isert_conn, isert_cmd, ib_sge,
+					send_wr, data_len, offset);
+		ib_sge += ib_sge_cnt;
+
+		if (i + 1 == wr->send_wr_num)
+			send_wr->send_flags = IB_SEND_SIGNALED;
+		else
+			send_wr->next = &wr->send_wr[i + 1];
+
+		offset += data_len;
+		va_offset += data_len;
+		data_left -= data_len;
+	}
+
+	atomic_inc(&isert_conn->post_send_buf_count);
+
+	rc = ib_post_send(isert_conn->conn_qp, wr->send_wr, &wr_failed);
+	if (rc) {
+		pr_warn("ib_post_send() failed for IB_WR_RDMA_READ\n");
+		atomic_dec(&isert_conn->post_send_buf_count);
+	}
+	pr_debug("Posted RDMA_READ memory for ISER Data WRITE\n");
+	return 0;
+
+unmap_sg:
+	ib_dma_unmap_sg(ib_dev, sg_start, sg_nents, DMA_FROM_DEVICE);
+	return ret;
+}
+
+static int
+isert_immediate_queue(struct iscsi_conn *conn, struct iscsi_cmd *cmd, int state)
+{
+	int ret;
+
+	switch (state) {
+	case ISTATE_SEND_NOPIN_WANT_RESPONSE:
+		ret = isert_put_nopin(cmd, conn, false);
+		break;
+	default:
+		pr_err("Unknown immediate state: 0x%02x\n", state);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int
+isert_response_queue(struct iscsi_conn *conn, struct iscsi_cmd *cmd, int state)
+{
+	int ret;
+
+	switch (state) {
+	case ISTATE_SEND_LOGOUTRSP:
+		ret = isert_put_logout_rsp(cmd, conn);
+		if (!ret) {
+			pr_debug("Returning iSER Logout -EAGAIN\n");
+			ret = -EAGAIN;
+		}
+		break;
+	case ISTATE_SEND_NOPIN:
+		ret = isert_put_nopin(cmd, conn, true);
+		break;
+	case ISTATE_SEND_TASKMGTRSP:
+		ret = isert_put_tm_rsp(cmd, conn);
+		break;
+	case ISTATE_SEND_REJECT:
+		ret = isert_put_reject(cmd, conn);
+		break;
+	case ISTATE_SEND_STATUS:
+		/*
+		 * Special case for sending non GOOD SCSI status from TX thread
+		 * context during pre se_cmd excecution failure.
+		 */
+		ret = isert_put_response(conn, cmd);
+		break;
+	default:
+		pr_err("Unknown response state: 0x%02x\n", state);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int
+isert_setup_np(struct iscsi_np *np,
+	       struct __kernel_sockaddr_storage *ksockaddr)
+{
+	struct isert_np *isert_np;
+	struct rdma_cm_id *isert_lid;
+	struct sockaddr *sa;
+	int ret;
+
+	isert_np = kzalloc(sizeof(struct isert_np), GFP_KERNEL);
+	if (!isert_np) {
+		pr_err("Unable to allocate struct isert_np\n");
+		return -ENOMEM;
+	}
+	init_waitqueue_head(&isert_np->np_accept_wq);
+	mutex_init(&isert_np->np_accept_mutex);
+	INIT_LIST_HEAD(&isert_np->np_accept_list);
+	init_completion(&isert_np->np_login_comp);
+
+	sa = (struct sockaddr *)ksockaddr;
+	pr_debug("ksockaddr: %p, sa: %p\n", ksockaddr, sa);
+	/*
+	 * Setup the np->np_sockaddr from the passed sockaddr setup
+	 * in iscsi_target_configfs.c code..
+	 */
+	memcpy(&np->np_sockaddr, ksockaddr,
+	       sizeof(struct __kernel_sockaddr_storage));
+
+	isert_lid = rdma_create_id(isert_cma_handler, np, RDMA_PS_TCP,
+				IB_QPT_RC);
+	if (IS_ERR(isert_lid)) {
+		pr_err("rdma_create_id() for isert_listen_handler failed: %ld\n",
+		       PTR_ERR(isert_lid));
+		ret = PTR_ERR(isert_lid);
+		goto out;
+	}
+
+	ret = rdma_bind_addr(isert_lid, sa);
+	if (ret) {
+		pr_err("rdma_bind_addr() for isert_lid failed: %d\n", ret);
+		goto out_lid;
+	}
+
+	ret = rdma_listen(isert_lid, ISERT_RDMA_LISTEN_BACKLOG);
+	if (ret) {
+		pr_err("rdma_listen() for isert_lid failed: %d\n", ret);
+		goto out_lid;
+	}
+
+	isert_np->np_cm_id = isert_lid;
+	np->np_context = isert_np;
+	pr_debug("Setup isert_lid->context: %p\n", isert_lid->context);
+
+	return 0;
+
+out_lid:
+	rdma_destroy_id(isert_lid);
+out:
+	kfree(isert_np);
+	return ret;
+}
+
+static int
+isert_check_accept_queue(struct isert_np *isert_np)
+{
+	int empty;
+
+	mutex_lock(&isert_np->np_accept_mutex);
+	empty = list_empty(&isert_np->np_accept_list);
+	mutex_unlock(&isert_np->np_accept_mutex);
+
+	return empty;
+}
+
+static int
+isert_rdma_accept(struct isert_conn *isert_conn)
+{
+	struct rdma_cm_id *cm_id = isert_conn->conn_cm_id;
+	struct rdma_conn_param cp;
+	int ret;
+
+	memset(&cp, 0, sizeof(struct rdma_conn_param));
+	cp.responder_resources = isert_conn->responder_resources;
+	cp.initiator_depth = isert_conn->initiator_depth;
+	cp.retry_count = 7;
+	cp.rnr_retry_count = 7;
+
+	pr_debug("Before rdma_accept >>>>>>>>>>>>>>>>>>>>.\n");
+
+	ret = rdma_accept(cm_id, &cp);
+	if (ret) {
+		pr_err("rdma_accept() failed with: %d\n", ret);
+		return ret;
+	}
+
+	pr_debug("After rdma_accept >>>>>>>>>>>>>>>>>>>>>.\n");
+
+	return 0;
+}
+
+static int
+isert_get_login_rx(struct iscsi_conn *conn, struct iscsi_login *login)
+{
+	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
+	int ret;
+
+	pr_debug("isert_get_login_rx before conn_login_comp conn: %p\n", conn);
+
+	ret = wait_for_completion_interruptible(&isert_conn->conn_login_comp);
+	if (ret)
+		return ret;
+
+	pr_debug("isert_get_login_rx processing login->req: %p\n", login->req);
+	return 0;
+}
+
+static void
+isert_set_conn_info(struct iscsi_np *np, struct iscsi_conn *conn,
+		    struct isert_conn *isert_conn)
+{
+	struct rdma_cm_id *cm_id = isert_conn->conn_cm_id;
+	struct rdma_route *cm_route = &cm_id->route;
+	struct sockaddr_in *sock_in;
+	struct sockaddr_in6 *sock_in6;
+
+	conn->login_family = np->np_sockaddr.ss_family;
+
+	if (np->np_sockaddr.ss_family == AF_INET6) {
+		sock_in6 = (struct sockaddr_in6 *)&cm_route->addr.dst_addr;
+		snprintf(conn->login_ip, sizeof(conn->login_ip), "%pI6c",
+			 &sock_in6->sin6_addr.in6_u);
+		conn->login_port = ntohs(sock_in6->sin6_port);
+
+		sock_in6 = (struct sockaddr_in6 *)&cm_route->addr.src_addr;
+		snprintf(conn->local_ip, sizeof(conn->local_ip), "%pI6c",
+			 &sock_in6->sin6_addr.in6_u);
+		conn->local_port = ntohs(sock_in6->sin6_port);
+	} else {
+		sock_in = (struct sockaddr_in *)&cm_route->addr.dst_addr;
+		sprintf(conn->login_ip, "%pI4",
+			&sock_in->sin_addr.s_addr);
+		conn->login_port = ntohs(sock_in->sin_port);
+
+		sock_in = (struct sockaddr_in *)&cm_route->addr.src_addr;
+		sprintf(conn->local_ip, "%pI4",
+			&sock_in->sin_addr.s_addr);
+		conn->local_port = ntohs(sock_in->sin_port);
+	}
+}
+
+static int
+isert_accept_np(struct iscsi_np *np, struct iscsi_conn *conn)
+{
+	struct isert_np *isert_np = (struct isert_np *)np->np_context;
+	struct isert_conn *isert_conn;
+	int max_accept = 0, ret;
+
+accept_wait:
+	ret = wait_event_interruptible(isert_np->np_accept_wq,
+			!isert_check_accept_queue(isert_np) ||
+			np->np_thread_state == ISCSI_NP_THREAD_RESET);
+	if (max_accept > 5)
+		return -ENODEV;
+
+	spin_lock_bh(&np->np_thread_lock);
+	if (np->np_thread_state == ISCSI_NP_THREAD_RESET) {
+		spin_unlock_bh(&np->np_thread_lock);
+		pr_err("ISCSI_NP_THREAD_RESET for isert_accept_np\n");
+		return -ENODEV;
+	}
+	spin_unlock_bh(&np->np_thread_lock);
+
+	mutex_lock(&isert_np->np_accept_mutex);
+	if (list_empty(&isert_np->np_accept_list)) {
+		mutex_unlock(&isert_np->np_accept_mutex);
+		max_accept++;
+		goto accept_wait;
+	}
+	isert_conn = list_first_entry(&isert_np->np_accept_list,
+			struct isert_conn, conn_accept_node);
+	list_del_init(&isert_conn->conn_accept_node);
+	mutex_unlock(&isert_np->np_accept_mutex);
+
+	conn->context = isert_conn;
+	isert_conn->conn = conn;
+	max_accept = 0;
+
+	ret = isert_rdma_post_recvl(isert_conn);
+	if (ret)
+		return ret;
+
+	ret = isert_rdma_accept(isert_conn);
+	if (ret)
+		return ret;
+
+	isert_set_conn_info(np, conn, isert_conn);
+
+	pr_debug("Processing isert_accept_np: isert_conn: %p\n", isert_conn);
+	return 0;
+}
+
+static void
+isert_free_np(struct iscsi_np *np)
+{
+	struct isert_np *isert_np = (struct isert_np *)np->np_context;
+
+	rdma_destroy_id(isert_np->np_cm_id);
+
+	np->np_context = NULL;
+	kfree(isert_np);
+}
+
+static void isert_free_conn(struct iscsi_conn *conn)
+{
+	struct isert_conn *isert_conn = conn->context;
+
+	pr_debug("isert_free_conn: Starting \n");
+	/*
+	 * Decrement post_send_buf_count for special case when called
+	 * from isert_do_control_comp() -> iscsit_logout_post_handler()
+	 */
+	if (isert_conn->logout_posted)
+		atomic_dec(&isert_conn->post_send_buf_count);
+
+	if (isert_conn->conn_cm_id)
+		rdma_disconnect(isert_conn->conn_cm_id);
+	/*
+	 * Only wait for conn_wait_comp_err if the isert_conn made it
+	 * into full feature phase..
+	 */
+	if (isert_conn->state > ISER_CONN_INIT) {
+		pr_debug("isert_free_conn: Before wait_event comp_err %d\n",
+			 isert_conn->state);
+		wait_event(isert_conn->conn_wait_comp_err,
+			   isert_conn->state == ISER_CONN_TERMINATING);
+		pr_debug("isert_free_conn: After wait_event #1 >>>>>>>>>>>>\n");
+	}
+
+	pr_debug("isert_free_conn: wait_event conn_wait %d\n", isert_conn->state);
+	wait_event(isert_conn->conn_wait, isert_conn->state == ISER_CONN_DOWN);
+	pr_debug("isert_free_conn: After wait_event #2 >>>>>>>>>>>>>>>>>>>>\n");
+
+	isert_put_conn(isert_conn);
+}
+
+static struct iscsit_transport iser_target_transport = {
+	.name			= "IB/iSER",
+	.transport_type		= ISCSI_INFINIBAND,
+	.owner			= THIS_MODULE,
+	.iscsit_setup_np	= isert_setup_np,
+	.iscsit_accept_np	= isert_accept_np,
+	.iscsit_free_np		= isert_free_np,
+	.iscsit_free_conn	= isert_free_conn,
+	.iscsit_alloc_cmd	= isert_alloc_cmd,
+	.iscsit_get_login_rx	= isert_get_login_rx,
+	.iscsit_put_login_tx	= isert_put_login_tx,
+	.iscsit_immediate_queue	= isert_immediate_queue,
+	.iscsit_response_queue	= isert_response_queue,
+	.iscsit_get_dataout	= isert_get_dataout,
+	.iscsit_queue_data_in	= isert_put_datain,
+	.iscsit_queue_status	= isert_put_response,
+};
+
+static int __init isert_init(void)
+{
+	int ret;
+
+	isert_rx_wq = alloc_workqueue("isert_rx_wq", 0, 0);
+	if (!isert_rx_wq) {
+		pr_err("Unable to allocate isert_rx_wq\n");
+		return -ENOMEM;
+	}
+
+	isert_comp_wq = alloc_workqueue("isert_comp_wq", 0, 0);
+	if (!isert_comp_wq) {
+		pr_err("Unable to allocate isert_comp_wq\n");
+		ret = -ENOMEM;
+		goto destroy_rx_wq;
+	}
+
+	isert_cmd_cache = kmem_cache_create("isert_cmd_cache",
+			sizeof(struct isert_cmd), __alignof__(struct isert_cmd),
+			0, NULL);
+	if (!isert_cmd_cache) {
+		pr_err("Unable to create isert_cmd_cache\n");
+		ret = -ENOMEM;
+		goto destroy_tx_cq;
+	}
+
+	iscsit_register_transport(&iser_target_transport);
+	pr_debug("iSER_TARGET[0] - Loaded iser_target_transport\n");
+	return 0;
+
+destroy_tx_cq:
+	destroy_workqueue(isert_comp_wq);
+destroy_rx_wq:
+	destroy_workqueue(isert_rx_wq);
+	return ret;
+}
+
+static void __exit isert_exit(void)
+{
+	kmem_cache_destroy(isert_cmd_cache);
+	destroy_workqueue(isert_comp_wq);
+	destroy_workqueue(isert_rx_wq);
+	iscsit_unregister_transport(&iser_target_transport);
+	pr_debug("iSER_TARGET[0] - Released iser_target_transport\n");
+}
+
+MODULE_DESCRIPTION("iSER-Target for mainline target infrastructure");
+MODULE_VERSION("0.1");
+MODULE_AUTHOR("nab@Linux-iSCSI.org");
+MODULE_LICENSE("GPL");
+
+module_init(isert_init);
+module_exit(isert_exit);
diff --git a/drivers/infiniband/ulp/isert/ib_isert.h b/drivers/infiniband/ulp/isert/ib_isert.h
new file mode 100644
index 000000000000..b104f4c2cd38
--- /dev/null
+++ b/drivers/infiniband/ulp/isert/ib_isert.h
@@ -0,0 +1,138 @@
+#include <linux/socket.h>
+#include <linux/in.h>
+#include <linux/in6.h>
+#include <rdma/ib_verbs.h>
+#include <rdma/rdma_cm.h>
+
+#define ISERT_RDMA_LISTEN_BACKLOG	10
+
+enum isert_desc_type {
+	ISCSI_TX_CONTROL,
+	ISCSI_TX_DATAIN
+};
+
+enum iser_ib_op_code {
+	ISER_IB_RECV,
+	ISER_IB_SEND,
+	ISER_IB_RDMA_WRITE,
+	ISER_IB_RDMA_READ,
+};
+
+enum iser_conn_state {
+	ISER_CONN_INIT,
+	ISER_CONN_UP,
+	ISER_CONN_TERMINATING,
+	ISER_CONN_DOWN,
+};
+
+struct iser_rx_desc {
+	struct iser_hdr iser_header;
+	struct iscsi_hdr iscsi_header;
+	char		data[ISER_RECV_DATA_SEG_LEN];
+	u64		dma_addr;
+	struct ib_sge	rx_sg;
+	char		pad[ISER_RX_PAD_SIZE];
+} __packed;
+
+struct iser_tx_desc {
+	struct iser_hdr iser_header;
+	struct iscsi_hdr iscsi_header;
+	enum isert_desc_type type;
+	u64		dma_addr;
+	struct ib_sge	tx_sg[2];
+	int		num_sge;
+	struct isert_cmd *isert_cmd;
+	struct ib_send_wr send_wr;
+} __packed;
+
+struct isert_rdma_wr {
+	struct list_head	wr_list;
+	struct isert_cmd	*isert_cmd;
+	enum iser_ib_op_code	iser_ib_op;
+	struct ib_sge		*ib_sge;
+	int			num_sge;
+	struct scatterlist	*sge;
+	int			send_wr_num;
+	struct ib_send_wr	*send_wr;
+};
+
+struct isert_cmd {
+	uint32_t		read_stag;
+	uint32_t		write_stag;
+	uint64_t		read_va;
+	uint64_t		write_va;
+	u64			sense_buf_dma;
+	u32			sense_buf_len;
+	u32			read_va_off;
+	u32			write_va_off;
+	u32			rdma_wr_num;
+	struct isert_conn	*conn;
+	struct iscsi_cmd	iscsi_cmd;
+	struct ib_sge		*ib_sge;
+	struct iser_tx_desc	tx_desc;
+	struct isert_rdma_wr	rdma_wr;
+	struct work_struct	comp_work;
+};
+
+struct isert_device;
+
+struct isert_conn {
+	enum iser_conn_state	state;
+	bool			logout_posted;
+	int			post_recv_buf_count;
+	atomic_t		post_send_buf_count;
+	u32			responder_resources;
+	u32			initiator_depth;
+	u32			max_sge;
+	char			*login_buf;
+	char			*login_req_buf;
+	char			*login_rsp_buf;
+	u64			login_req_dma;
+	u64			login_rsp_dma;
+	unsigned int		conn_rx_desc_head;
+	struct iser_rx_desc	*conn_rx_descs;
+	struct ib_recv_wr	conn_rx_wr[ISERT_MIN_POSTED_RX];
+	struct iscsi_conn	*conn;
+	struct list_head	conn_accept_node;
+	struct completion	conn_login_comp;
+	struct iser_tx_desc	conn_login_tx_desc;
+	struct rdma_cm_id	*conn_cm_id;
+	struct ib_pd		*conn_pd;
+	struct ib_mr		*conn_mr;
+	struct ib_qp		*conn_qp;
+	struct isert_device	*conn_device;
+	struct work_struct	conn_logout_work;
+	wait_queue_head_t	conn_wait;
+	wait_queue_head_t	conn_wait_comp_err;
+	struct kref		conn_kref;
+};
+
+#define ISERT_MAX_CQ 64
+
+struct isert_cq_desc {
+	struct isert_device	*device;
+	int			cq_index;
+	struct work_struct	cq_rx_work;
+	struct work_struct	cq_tx_work;
+};
+
+struct isert_device {
+	int			cqs_used;
+	int			refcount;
+	int			cq_active_qps[ISERT_MAX_CQ];
+	struct ib_device	*ib_device;
+	struct ib_pd		*dev_pd;
+	struct ib_mr		*dev_mr;
+	struct ib_cq		*dev_rx_cq[ISERT_MAX_CQ];
+	struct ib_cq		*dev_tx_cq[ISERT_MAX_CQ];
+	struct isert_cq_desc	*cq_desc;
+	struct list_head	dev_node;
+};
+
+struct isert_np {
+	wait_queue_head_t	np_accept_wq;
+	struct rdma_cm_id	*np_cm_id;
+	struct mutex		np_accept_mutex;
+	struct list_head	np_accept_list;
+	struct completion	np_login_comp;
+};
diff --git a/drivers/infiniband/ulp/isert/isert_proto.h b/drivers/infiniband/ulp/isert/isert_proto.h
new file mode 100644
index 000000000000..4dccd313b777
--- /dev/null
+++ b/drivers/infiniband/ulp/isert/isert_proto.h
@@ -0,0 +1,47 @@
+/* From iscsi_iser.h */
+
+struct iser_hdr {
+	u8	flags;
+	u8	rsvd[3];
+	__be32	write_stag; /* write rkey */
+	__be64	write_va;
+	__be32	read_stag;  /* read rkey */
+	__be64	read_va;
+} __packed;
+
+/*Constant PDU lengths calculations */
+#define ISER_HEADERS_LEN  (sizeof(struct iser_hdr) + sizeof(struct iscsi_hdr))
+
+#define ISER_RECV_DATA_SEG_LEN  8192
+#define ISER_RX_PAYLOAD_SIZE    (ISER_HEADERS_LEN + ISER_RECV_DATA_SEG_LEN)
+#define ISER_RX_LOGIN_SIZE      (ISER_HEADERS_LEN + ISCSI_DEF_MAX_RECV_SEG_LEN)
+
+/* QP settings */
+/* Maximal bounds on received asynchronous PDUs */
+#define ISERT_MAX_TX_MISC_PDUS	4 /* NOOP_IN(2) , ASYNC_EVENT(2)   */
+
+#define ISERT_MAX_RX_MISC_PDUS	6 /* NOOP_OUT(2), TEXT(1),         *
+				   * SCSI_TMFUNC(2), LOGOUT(1) */
+
+#define ISCSI_DEF_XMIT_CMDS_MAX 128 /* from libiscsi.h, must be power of 2 */
+
+#define ISERT_QP_MAX_RECV_DTOS	(ISCSI_DEF_XMIT_CMDS_MAX)
+
+#define ISERT_MIN_POSTED_RX	(ISCSI_DEF_XMIT_CMDS_MAX >> 2)
+
+#define ISERT_INFLIGHT_DATAOUTS	8
+
+#define ISERT_QP_MAX_REQ_DTOS	(ISCSI_DEF_XMIT_CMDS_MAX *    \
+				(1 + ISERT_INFLIGHT_DATAOUTS) + \
+				ISERT_MAX_TX_MISC_PDUS	+ \
+				ISERT_MAX_RX_MISC_PDUS)
+
+#define ISER_RX_PAD_SIZE	(ISER_RECV_DATA_SEG_LEN + 4096 - \
+		(ISER_RX_PAYLOAD_SIZE + sizeof(u64) + sizeof(struct ib_sge)))
+
+#define ISER_VER	0x10
+#define ISER_WSV	0x08
+#define ISER_RSV	0x04
+#define ISCSI_CTRL	0x10
+#define ISER_HELLO	0x20
+#define ISER_HELLORPLY	0x30
diff --git a/drivers/infiniband/ulp/srpt/ib_srpt.c b/drivers/infiniband/ulp/srpt/ib_srpt.c
index c09d41b1a2ff..b08ca7a9f76b 100644
--- a/drivers/infiniband/ulp/srpt/ib_srpt.c
+++ b/drivers/infiniband/ulp/srpt/ib_srpt.c
@@ -1374,7 +1374,7 @@ static int srpt_abort_cmd(struct srpt_send_ioctx *ioctx)
 		target_put_sess_cmd(ioctx->ch->sess, &ioctx->cmd);
 		break;
 	default:
-		WARN_ON("ERROR: unexpected command state");
+		WARN(1, "Unexpected command state (%d)", state);
 		break;
 	}
 
diff --git a/drivers/input/input-mt.c b/drivers/input/input-mt.c
index 71db1930573f..d398f1321f14 100644
--- a/drivers/input/input-mt.c
+++ b/drivers/input/input-mt.c
@@ -79,6 +79,8 @@ int input_mt_init_slots(struct input_dev *dev, unsigned int num_slots,
 	}
 	if (flags & INPUT_MT_DIRECT)
 		__set_bit(INPUT_PROP_DIRECT, dev->propbit);
+	if (flags & INPUT_MT_SEMI_MT)
+		__set_bit(INPUT_PROP_SEMI_MT, dev->propbit);
 	if (flags & INPUT_MT_TRACK) {
 		unsigned int n2 = num_slots * num_slots;
 		mt->red = kcalloc(n2, sizeof(*mt->red), GFP_KERNEL);
@@ -246,6 +248,7 @@ void input_mt_sync_frame(struct input_dev *dev)
 {
 	struct input_mt *mt = dev->mt;
 	struct input_mt_slot *s;
+	bool use_count = false;
 
 	if (!mt)
 		return;
@@ -259,7 +262,10 @@ void input_mt_sync_frame(struct input_dev *dev)
 		}
 	}
 
-	input_mt_report_pointer_emulation(dev, (mt->flags & INPUT_MT_POINTER));
+	if ((mt->flags & INPUT_MT_POINTER) && !(mt->flags & INPUT_MT_SEMI_MT))
+		use_count = true;
+
+	input_mt_report_pointer_emulation(dev, use_count);
 
 	mt->frame++;
 }
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index ac0500667000..62a2c0e4cc99 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -175,7 +175,7 @@ config KEYBOARD_EP93XX
 
 config KEYBOARD_GPIO
 	tristate "GPIO Buttons"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  This driver implements support for buttons connected
 	  to GPIO pins of various CPUs (and some other chips).
@@ -190,7 +190,7 @@ config KEYBOARD_GPIO
 
 config KEYBOARD_GPIO_POLLED
 	tristate "Polled GPIO buttons"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select INPUT_POLLDEV
 	help
 	  This driver implements support for buttons connected
@@ -241,7 +241,7 @@ config KEYBOARD_TCA8418
 
 config KEYBOARD_MATRIX
 	tristate "GPIO driven matrix keypad support"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select INPUT_MATRIXKMAP
 	help
 	  Enable support for GPIO driven matrix keypad.
@@ -628,4 +628,16 @@ config KEYBOARD_W90P910
 	  To compile this driver as a module, choose M here: the
 	  module will be called w90p910_keypad.
 
+config KEYBOARD_CROS_EC
+	tristate "ChromeOS EC keyboard"
+	select INPUT_MATRIXKMAP
+	depends on MFD_CROS_EC
+	help
+	  Say Y here to enable the matrix keyboard used by ChromeOS devices
+	  and implemented on the ChromeOS EC. You must enable one bus option
+	  (MFD_CROS_EC_I2C or MFD_CROS_EC_SPI) to use this.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called cros_ec_keyb.
+
 endif
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index 49b16453d00e..0c43e8cf8d0e 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -11,6 +11,7 @@ obj-$(CONFIG_KEYBOARD_AMIGA)		+= amikbd.o
 obj-$(CONFIG_KEYBOARD_ATARI)		+= atakbd.o
 obj-$(CONFIG_KEYBOARD_ATKBD)		+= atkbd.o
 obj-$(CONFIG_KEYBOARD_BFIN)		+= bf54x-keys.o
+obj-$(CONFIG_KEYBOARD_CROS_EC)		+= cros_ec_keyb.o
 obj-$(CONFIG_KEYBOARD_DAVINCI)		+= davinci_keyscan.o
 obj-$(CONFIG_KEYBOARD_EP93XX)		+= ep93xx_keypad.o
 obj-$(CONFIG_KEYBOARD_GOLDFISH_EVENTS)	+= goldfish_events.o
diff --git a/drivers/input/keyboard/amikbd.c b/drivers/input/keyboard/amikbd.c
index 79172af164f2..ba0b36f7daea 100644
--- a/drivers/input/keyboard/amikbd.c
+++ b/drivers/input/keyboard/amikbd.c
@@ -260,18 +260,6 @@ static struct platform_driver amikbd_driver = {
 	},
 };
 
-static int __init amikbd_init(void)
-{
-	return platform_driver_probe(&amikbd_driver, amikbd_probe);
-}
-
-module_init(amikbd_init);
-
-static void __exit amikbd_exit(void)
-{
-	platform_driver_unregister(&amikbd_driver);
-}
-
-module_exit(amikbd_exit);
+module_platform_driver_probe(amikbd_driver, amikbd_probe);
 
 MODULE_ALIAS("platform:amiga-keyboard");
diff --git a/drivers/input/keyboard/cros_ec_keyb.c b/drivers/input/keyboard/cros_ec_keyb.c
new file mode 100644
index 000000000000..49557f27bfa6
--- /dev/null
+++ b/drivers/input/keyboard/cros_ec_keyb.c
@@ -0,0 +1,334 @@
+/*
+ * ChromeOS EC keyboard driver
+ *
+ * Copyright (C) 2012 Google, Inc
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ * This driver uses the Chrome OS EC byte-level message-based protocol for
+ * communicating the keyboard state (which keys are pressed) from a keyboard EC
+ * to the AP over some bus (such as i2c, lpc, spi).  The EC does debouncing,
+ * but everything else (including deghosting) is done here.  The main
+ * motivation for this is to keep the EC firmware as simple as possible, since
+ * it cannot be easily upgraded and EC flash/IRAM space is relatively
+ * expensive.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/kernel.h>
+#include <linux/notifier.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/input/matrix_keypad.h>
+#include <linux/mfd/cros_ec.h>
+#include <linux/mfd/cros_ec_commands.h>
+
+/*
+ * @rows: Number of rows in the keypad
+ * @cols: Number of columns in the keypad
+ * @row_shift: log2 or number of rows, rounded up
+ * @keymap_data: Matrix keymap data used to convert to keyscan values
+ * @ghost_filter: true to enable the matrix key-ghosting filter
+ * @dev: Device pointer
+ * @idev: Input device
+ * @ec: Top level ChromeOS device to use to talk to EC
+ * @event_notifier: interrupt event notifier for transport devices
+ */
+struct cros_ec_keyb {
+	unsigned int rows;
+	unsigned int cols;
+	int row_shift;
+	const struct matrix_keymap_data *keymap_data;
+	bool ghost_filter;
+
+	struct device *dev;
+	struct input_dev *idev;
+	struct cros_ec_device *ec;
+	struct notifier_block notifier;
+};
+
+
+static bool cros_ec_keyb_row_has_ghosting(struct cros_ec_keyb *ckdev,
+					  uint8_t *buf, int row)
+{
+	int pressed_in_row = 0;
+	int row_has_teeth = 0;
+	int col, mask;
+
+	mask = 1 << row;
+	for (col = 0; col < ckdev->cols; col++) {
+		if (buf[col] & mask) {
+			pressed_in_row++;
+			row_has_teeth |= buf[col] & ~mask;
+			if (pressed_in_row > 1 && row_has_teeth) {
+				/* ghosting */
+				dev_dbg(ckdev->dev,
+					"ghost found at: r%d c%d, pressed %d, teeth 0x%x\n",
+					row, col, pressed_in_row,
+					row_has_teeth);
+				return true;
+			}
+		}
+	}
+
+	return false;
+}
+
+/*
+ * Returns true when there is at least one combination of pressed keys that
+ * results in ghosting.
+ */
+static bool cros_ec_keyb_has_ghosting(struct cros_ec_keyb *ckdev, uint8_t *buf)
+{
+	int row;
+
+	/*
+	 * Ghosting happens if for any pressed key X there are other keys
+	 * pressed both in the same row and column of X as, for instance,
+	 * in the following diagram:
+	 *
+	 * . . Y . g .
+	 * . . . . . .
+	 * . . . . . .
+	 * . . X . Z .
+	 *
+	 * In this case only X, Y, and Z are pressed, but g appears to be
+	 * pressed too (see Wikipedia).
+	 *
+	 * We can detect ghosting in a single pass (*) over the keyboard state
+	 * by maintaining two arrays.  pressed_in_row counts how many pressed
+	 * keys we have found in a row.  row_has_teeth is true if any of the
+	 * pressed keys for this row has other pressed keys in its column.  If
+	 * at any point of the scan we find that a row has multiple pressed
+	 * keys, and at least one of them is at the intersection with a column
+	 * with multiple pressed keys, we're sure there is ghosting.
+	 * Conversely, if there is ghosting, we will detect such situation for
+	 * at least one key during the pass.
+	 *
+	 * (*) This looks linear in the number of keys, but it's not.  We can
+	 * cheat because the number of rows is small.
+	 */
+	for (row = 0; row < ckdev->rows; row++)
+		if (cros_ec_keyb_row_has_ghosting(ckdev, buf, row))
+			return true;
+
+	return false;
+}
+
+/*
+ * Compares the new keyboard state to the old one and produces key
+ * press/release events accordingly.  The keyboard state is 13 bytes (one byte
+ * per column)
+ */
+static void cros_ec_keyb_process(struct cros_ec_keyb *ckdev,
+			 uint8_t *kb_state, int len)
+{
+	struct input_dev *idev = ckdev->idev;
+	int col, row;
+	int new_state;
+	int num_cols;
+
+	num_cols = len;
+
+	if (ckdev->ghost_filter && cros_ec_keyb_has_ghosting(ckdev, kb_state)) {
+		/*
+		 * Simple-minded solution: ignore this state. The obvious
+		 * improvement is to only ignore changes to keys involved in
+		 * the ghosting, but process the other changes.
+		 */
+		dev_dbg(ckdev->dev, "ghosting found\n");
+		return;
+	}
+
+	for (col = 0; col < ckdev->cols; col++) {
+		for (row = 0; row < ckdev->rows; row++) {
+			int pos = MATRIX_SCAN_CODE(row, col, ckdev->row_shift);
+			const unsigned short *keycodes = idev->keycode;
+			int code;
+
+			code = keycodes[pos];
+			new_state = kb_state[col] & (1 << row);
+			if (!!new_state != test_bit(code, idev->key)) {
+				dev_dbg(ckdev->dev,
+					"changed: [r%d c%d]: byte %02x\n",
+					row, col, new_state);
+
+				input_report_key(idev, code, new_state);
+			}
+		}
+	}
+	input_sync(ckdev->idev);
+}
+
+static int cros_ec_keyb_open(struct input_dev *dev)
+{
+	struct cros_ec_keyb *ckdev = input_get_drvdata(dev);
+
+	return blocking_notifier_chain_register(&ckdev->ec->event_notifier,
+						&ckdev->notifier);
+}
+
+static void cros_ec_keyb_close(struct input_dev *dev)
+{
+	struct cros_ec_keyb *ckdev = input_get_drvdata(dev);
+
+	blocking_notifier_chain_unregister(&ckdev->ec->event_notifier,
+					   &ckdev->notifier);
+}
+
+static int cros_ec_keyb_get_state(struct cros_ec_keyb *ckdev, uint8_t *kb_state)
+{
+	return ckdev->ec->command_recv(ckdev->ec, EC_CMD_MKBP_STATE,
+					  kb_state, ckdev->cols);
+}
+
+static int cros_ec_keyb_work(struct notifier_block *nb,
+		     unsigned long state, void *_notify)
+{
+	int ret;
+	struct cros_ec_keyb *ckdev = container_of(nb, struct cros_ec_keyb,
+						    notifier);
+	uint8_t kb_state[ckdev->cols];
+
+	ret = cros_ec_keyb_get_state(ckdev, kb_state);
+	if (ret >= 0)
+		cros_ec_keyb_process(ckdev, kb_state, ret);
+
+	return NOTIFY_DONE;
+}
+
+/* Clear any keys in the buffer */
+static void cros_ec_keyb_clear_keyboard(struct cros_ec_keyb *ckdev)
+{
+	uint8_t old_state[ckdev->cols];
+	uint8_t new_state[ckdev->cols];
+	unsigned long duration;
+	int i, ret;
+
+	/*
+	 * Keep reading until we see that the scan state does not change.
+	 * That indicates that we are done.
+	 *
+	 * Assume that the EC keyscan buffer is at most 32 deep.
+	 */
+	duration = jiffies;
+	ret = cros_ec_keyb_get_state(ckdev, new_state);
+	for (i = 1; !ret && i < 32; i++) {
+		memcpy(old_state, new_state, sizeof(old_state));
+		ret = cros_ec_keyb_get_state(ckdev, new_state);
+		if (0 == memcmp(old_state, new_state, sizeof(old_state)))
+			break;
+	}
+	duration = jiffies - duration;
+	dev_info(ckdev->dev, "Discarded %d keyscan(s) in %dus\n", i,
+		jiffies_to_usecs(duration));
+}
+
+static int cros_ec_keyb_probe(struct platform_device *pdev)
+{
+	struct cros_ec_device *ec = dev_get_drvdata(pdev->dev.parent);
+	struct device *dev = ec->dev;
+	struct cros_ec_keyb *ckdev;
+	struct input_dev *idev;
+	struct device_node *np;
+	int err;
+
+	np = pdev->dev.of_node;
+	if (!np)
+		return -ENODEV;
+
+	ckdev = devm_kzalloc(&pdev->dev, sizeof(*ckdev), GFP_KERNEL);
+	if (!ckdev)
+		return -ENOMEM;
+	err = matrix_keypad_parse_of_params(&pdev->dev, &ckdev->rows,
+					    &ckdev->cols);
+	if (err)
+		return err;
+
+	idev = devm_input_allocate_device(&pdev->dev);
+	if (!idev)
+		return -ENOMEM;
+
+	ckdev->ec = ec;
+	ckdev->notifier.notifier_call = cros_ec_keyb_work;
+	ckdev->dev = dev;
+	dev_set_drvdata(&pdev->dev, ckdev);
+
+	idev->name = ec->ec_name;
+	idev->phys = ec->phys_name;
+	__set_bit(EV_REP, idev->evbit);
+
+	idev->id.bustype = BUS_VIRTUAL;
+	idev->id.version = 1;
+	idev->id.product = 0;
+	idev->dev.parent = &pdev->dev;
+	idev->open = cros_ec_keyb_open;
+	idev->close = cros_ec_keyb_close;
+
+	ckdev->ghost_filter = of_property_read_bool(np,
+					"google,needs-ghost-filter");
+
+	err = matrix_keypad_build_keymap(NULL, NULL, ckdev->rows, ckdev->cols,
+					 NULL, idev);
+	if (err) {
+		dev_err(dev, "cannot build key matrix\n");
+		return err;
+	}
+
+	ckdev->row_shift = get_count_order(ckdev->cols);
+
+	input_set_capability(idev, EV_MSC, MSC_SCAN);
+	input_set_drvdata(idev, ckdev);
+	ckdev->idev = idev;
+	err = input_register_device(ckdev->idev);
+	if (err) {
+		dev_err(dev, "cannot register input device\n");
+		return err;
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int cros_ec_keyb_resume(struct device *dev)
+{
+	struct cros_ec_keyb *ckdev = dev_get_drvdata(dev);
+
+	/*
+	 * When the EC is not a wake source, then it could not have caused the
+	 * resume, so we clear the EC's key scan buffer. If the EC was a
+	 * wake source (e.g. the lid is open and the user might press a key to
+	 * wake) then the key scan buffer should be preserved.
+	 */
+	if (ckdev->ec->was_wake_device)
+		cros_ec_keyb_clear_keyboard(ckdev);
+
+	return 0;
+}
+
+#endif
+
+static SIMPLE_DEV_PM_OPS(cros_ec_keyb_pm_ops, NULL, cros_ec_keyb_resume);
+
+static struct platform_driver cros_ec_keyb_driver = {
+	.probe = cros_ec_keyb_probe,
+	.driver = {
+		.name = "cros-ec-keyb",
+		.pm	= &cros_ec_keyb_pm_ops,
+	},
+};
+
+module_platform_driver(cros_ec_keyb_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ChromeOS EC keyboard driver");
+MODULE_ALIAS("platform:cros-ec-keyb");
diff --git a/drivers/input/keyboard/davinci_keyscan.c b/drivers/input/keyboard/davinci_keyscan.c
index 4e4e453ea15e..829753702b62 100644
--- a/drivers/input/keyboard/davinci_keyscan.c
+++ b/drivers/input/keyboard/davinci_keyscan.c
@@ -329,17 +329,7 @@ static struct platform_driver davinci_ks_driver = {
 	.remove	= davinci_ks_remove,
 };
 
-static int __init davinci_ks_init(void)
-{
-	return platform_driver_probe(&davinci_ks_driver, davinci_ks_probe);
-}
-module_init(davinci_ks_init);
-
-static void __exit davinci_ks_exit(void)
-{
-	platform_driver_unregister(&davinci_ks_driver);
-}
-module_exit(davinci_ks_exit);
+module_platform_driver_probe(davinci_ks_driver, davinci_ks_probe);
 
 MODULE_AUTHOR("Miguel Aguilar");
 MODULE_DESCRIPTION("Texas Instruments DaVinci Key Scan Driver");
diff --git a/drivers/input/keyboard/imx_keypad.c b/drivers/input/keyboard/imx_keypad.c
index 98f9113251d2..03c8cc5cb6c1 100644
--- a/drivers/input/keyboard/imx_keypad.c
+++ b/drivers/input/keyboard/imx_keypad.c
@@ -448,24 +448,17 @@ static int imx_keypad_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	res = request_mem_region(res->start, resource_size(res), pdev->name);
-	if (res == NULL) {
-		dev_err(&pdev->dev, "failed to request I/O memory\n");
-		return -EBUSY;
-	}
-
-	input_dev = input_allocate_device();
+	input_dev = devm_input_allocate_device(&pdev->dev);
 	if (!input_dev) {
 		dev_err(&pdev->dev, "failed to allocate the input device\n");
-		error = -ENOMEM;
-		goto failed_rel_mem;
+		return -ENOMEM;
 	}
 
-	keypad = kzalloc(sizeof(struct imx_keypad), GFP_KERNEL);
+	keypad = devm_kzalloc(&pdev->dev, sizeof(struct imx_keypad),
+			     GFP_KERNEL);
 	if (!keypad) {
 		dev_err(&pdev->dev, "not enough memory for driver data\n");
-		error = -ENOMEM;
-		goto failed_free_input;
+		return -ENOMEM;
 	}
 
 	keypad->input_dev = input_dev;
@@ -475,18 +468,14 @@ static int imx_keypad_probe(struct platform_device *pdev)
 	setup_timer(&keypad->check_matrix_timer,
 		    imx_keypad_check_for_events, (unsigned long) keypad);
 
-	keypad->mmio_base = ioremap(res->start, resource_size(res));
-	if (keypad->mmio_base == NULL) {
-		dev_err(&pdev->dev, "failed to remap I/O memory\n");
-		error = -ENOMEM;
-		goto failed_free_priv;
-	}
+	keypad->mmio_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(keypad->mmio_base))
+		return PTR_ERR(keypad->mmio_base);
 
-	keypad->clk = clk_get(&pdev->dev, NULL);
+	keypad->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(keypad->clk)) {
 		dev_err(&pdev->dev, "failed to get keypad clock\n");
-		error = PTR_ERR(keypad->clk);
-		goto failed_unmap;
+		return PTR_ERR(keypad->clk);
 	}
 
 	/* Init the Input device */
@@ -502,7 +491,7 @@ static int imx_keypad_probe(struct platform_device *pdev)
 					   keypad->keycodes, input_dev);
 	if (error) {
 		dev_err(&pdev->dev, "failed to build keymap\n");
-		goto failed_clock_put;
+		return error;
 	}
 
 	/* Search for rows and cols enabled */
@@ -527,61 +516,24 @@ static int imx_keypad_probe(struct platform_device *pdev)
 	imx_keypad_inhibit(keypad);
 	clk_disable_unprepare(keypad->clk);
 
-	error = request_irq(irq, imx_keypad_irq_handler, 0,
+	error = devm_request_irq(&pdev->dev, irq, imx_keypad_irq_handler, 0,
 			    pdev->name, keypad);
 	if (error) {
 		dev_err(&pdev->dev, "failed to request IRQ\n");
-		goto failed_clock_put;
+		return error;
 	}
 
 	/* Register the input device */
 	error = input_register_device(input_dev);
 	if (error) {
 		dev_err(&pdev->dev, "failed to register input device\n");
-		goto failed_free_irq;
+		return error;
 	}
 
 	platform_set_drvdata(pdev, keypad);
 	device_init_wakeup(&pdev->dev, 1);
 
 	return 0;
-
-failed_free_irq:
-	free_irq(irq, pdev);
-failed_clock_put:
-	clk_put(keypad->clk);
-failed_unmap:
-	iounmap(keypad->mmio_base);
-failed_free_priv:
-	kfree(keypad);
-failed_free_input:
-	input_free_device(input_dev);
-failed_rel_mem:
-	release_mem_region(res->start, resource_size(res));
-	return error;
-}
-
-static int imx_keypad_remove(struct platform_device *pdev)
-{
-	struct imx_keypad *keypad = platform_get_drvdata(pdev);
-	struct resource *res;
-
-	dev_dbg(&pdev->dev, ">%s\n", __func__);
-
-	platform_set_drvdata(pdev, NULL);
-
-	input_unregister_device(keypad->input_dev);
-
-	free_irq(keypad->irq, keypad);
-	clk_put(keypad->clk);
-
-	iounmap(keypad->mmio_base);
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	release_mem_region(res->start, resource_size(res));
-
-	kfree(keypad);
-
-	return 0;
 }
 
 #ifdef CONFIG_PM_SLEEP
@@ -640,7 +592,6 @@ static struct platform_driver imx_keypad_driver = {
 		.of_match_table = of_match_ptr(imx_keypad_of_match),
 	},
 	.probe		= imx_keypad_probe,
-	.remove		= imx_keypad_remove,
 };
 module_platform_driver(imx_keypad_driver);
 
diff --git a/drivers/input/keyboard/lpc32xx-keys.c b/drivers/input/keyboard/lpc32xx-keys.c
index 1b8add6cfb9d..42181435fe67 100644
--- a/drivers/input/keyboard/lpc32xx-keys.c
+++ b/drivers/input/keyboard/lpc32xx-keys.c
@@ -144,12 +144,13 @@ static int lpc32xx_parse_dt(struct device *dev,
 {
 	struct device_node *np = dev->of_node;
 	u32 rows = 0, columns = 0;
+	int err;
 
-	of_property_read_u32(np, "keypad,num-rows", &rows);
-	of_property_read_u32(np, "keypad,num-columns", &columns);
-	if (!rows || rows != columns) {
-		dev_err(dev,
-			"rows and columns must be specified and be equal!\n");
+	err = matrix_keypad_parse_of_params(dev, &rows, &columns);
+	if (err)
+		return err;
+	if (rows != columns) {
+		dev_err(dev, "rows and columns must be equal!\n");
 		return -EINVAL;
 	}
 
diff --git a/drivers/input/keyboard/nomadik-ske-keypad.c b/drivers/input/keyboard/nomadik-ske-keypad.c
index 0e6a8151fee3..c7d505cce72f 100644
--- a/drivers/input/keyboard/nomadik-ske-keypad.c
+++ b/drivers/input/keyboard/nomadik-ske-keypad.c
@@ -430,17 +430,7 @@ static struct platform_driver ske_keypad_driver = {
 	.remove = ske_keypad_remove,
 };
 
-static int __init ske_keypad_init(void)
-{
-	return platform_driver_probe(&ske_keypad_driver, ske_keypad_probe);
-}
-module_init(ske_keypad_init);
-
-static void __exit ske_keypad_exit(void)
-{
-	platform_driver_unregister(&ske_keypad_driver);
-}
-module_exit(ske_keypad_exit);
+module_platform_driver_probe(ske_keypad_driver, ske_keypad_probe);
 
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Naveen Kumar <naveen.gaddipati@stericsson.com> / Sundar Iyer <sundar.iyer@stericsson.com>");
diff --git a/drivers/input/keyboard/omap4-keypad.c b/drivers/input/keyboard/omap4-keypad.c
index e25b022692cd..1b289092f4e3 100644
--- a/drivers/input/keyboard/omap4-keypad.c
+++ b/drivers/input/keyboard/omap4-keypad.c
@@ -215,18 +215,12 @@ static int omap4_keypad_parse_dt(struct device *dev,
 				 struct omap4_keypad *keypad_data)
 {
 	struct device_node *np = dev->of_node;
+	int err;
 
-	if (!np) {
-		dev_err(dev, "missing DT data");
-		return -EINVAL;
-	}
-
-	of_property_read_u32(np, "keypad,num-rows", &keypad_data->rows);
-	of_property_read_u32(np, "keypad,num-columns", &keypad_data->cols);
-	if (!keypad_data->rows || !keypad_data->cols) {
-		dev_err(dev, "number of keypad rows/columns not specified\n");
-		return -EINVAL;
-	}
+	err = matrix_keypad_parse_of_params(dev, &keypad_data->rows,
+					    &keypad_data->cols);
+	if (err)
+		return err;
 
 	if (of_get_property(np, "linux,input-no-autorepeat", NULL))
 		keypad_data->no_autorepeat = true;
diff --git a/drivers/input/keyboard/tca8418_keypad.c b/drivers/input/keyboard/tca8418_keypad.c
index a34cc6714e5b..55c15304ddbc 100644
--- a/drivers/input/keyboard/tca8418_keypad.c
+++ b/drivers/input/keyboard/tca8418_keypad.c
@@ -288,8 +288,11 @@ static int tca8418_keypad_probe(struct i2c_client *client,
 		irq_is_gpio = pdata->irq_is_gpio;
 	} else {
 		struct device_node *np = dev->of_node;
-		of_property_read_u32(np, "keypad,num-rows", &rows);
-		of_property_read_u32(np, "keypad,num-columns", &cols);
+		int err;
+
+		err = matrix_keypad_parse_of_params(dev, &rows, &cols);
+		if (err)
+			return err;
 		rep = of_property_read_bool(np, "keypad,autorepeat");
 	}
 
diff --git a/drivers/input/keyboard/tegra-kbc.c b/drivers/input/keyboard/tegra-kbc.c
index 0e138ebcc768..b46142f78ef2 100644
--- a/drivers/input/keyboard/tegra-kbc.c
+++ b/drivers/input/keyboard/tegra-kbc.c
@@ -27,17 +27,19 @@
 #include <linux/io.h>
 #include <linux/interrupt.h>
 #include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/clk.h>
 #include <linux/slab.h>
 #include <linux/input/matrix_keypad.h>
 #include <linux/clk/tegra.h>
+#include <linux/err.h>
 
-#define KBC_MAX_GPIO	24
 #define KBC_MAX_KPENT	8
 
-#define KBC_MAX_ROW	16
-#define KBC_MAX_COL	8
-#define KBC_MAX_KEY	(KBC_MAX_ROW * KBC_MAX_COL)
+/* Maximum row/column supported by Tegra KBC yet  is 16x8 */
+#define KBC_MAX_GPIO	24
+/* Maximum keys supported by Tegra KBC yet is 16 x 8*/
+#define KBC_MAX_KEY	(16 * 8)
 
 #define KBC_MAX_DEBOUNCE_CNT	0x3ffu
 
@@ -80,6 +82,12 @@ enum tegra_pin_type {
 	PIN_CFG_ROW,
 };
 
+/* Tegra KBC hw support */
+struct tegra_kbc_hw_support {
+	int max_rows;
+	int max_columns;
+};
+
 struct tegra_kbc_pin_cfg {
 	enum tegra_pin_type type;
 	unsigned char num;
@@ -108,6 +116,9 @@ struct tegra_kbc {
 	u32 wakeup_key;
 	struct timer_list timer;
 	struct clk *clk;
+	const struct tegra_kbc_hw_support *hw_support;
+	int max_keys;
+	int num_rows_and_columns;
 };
 
 static void tegra_kbc_report_released_keys(struct input_dev *input,
@@ -204,11 +215,11 @@ static void tegra_kbc_report_keys(struct tegra_kbc *kbc)
 
 	/*
 	 * If the platform uses Fn keymaps, translate keys on a Fn keypress.
-	 * Function keycodes are KBC_MAX_KEY apart from the plain keycodes.
+	 * Function keycodes are max_keys apart from the plain keycodes.
 	 */
 	if (fn_keypress) {
 		for (i = 0; i < num_down; i++) {
-			scancodes[i] += KBC_MAX_KEY;
+			scancodes[i] += kbc->max_keys;
 			keycodes[i] = kbc->keycode[scancodes[i]];
 		}
 	}
@@ -315,7 +326,7 @@ static void tegra_kbc_setup_wakekeys(struct tegra_kbc *kbc, bool filter)
 	/* Either mask all keys or none. */
 	rst_val = (filter && !kbc->wakeup) ? ~0 : 0;
 
-	for (i = 0; i < KBC_MAX_ROW; i++)
+	for (i = 0; i < kbc->hw_support->max_rows; i++)
 		writel(rst_val, kbc->mmio + KBC_ROW0_MASK_0 + i * 4);
 }
 
@@ -452,7 +463,7 @@ static bool tegra_kbc_check_pin_cfg(const struct tegra_kbc *kbc,
 
 		switch (pin_cfg->type) {
 		case PIN_CFG_ROW:
-			if (pin_cfg->num >= KBC_MAX_ROW) {
+			if (pin_cfg->num >= kbc->hw_support->max_rows) {
 				dev_err(kbc->dev,
 					"pin_cfg[%d]: invalid row number %d\n",
 					i, pin_cfg->num);
@@ -462,7 +473,7 @@ static bool tegra_kbc_check_pin_cfg(const struct tegra_kbc *kbc,
 			break;
 
 		case PIN_CFG_COL:
-			if (pin_cfg->num >= KBC_MAX_COL) {
+			if (pin_cfg->num >= kbc->hw_support->max_columns) {
 				dev_err(kbc->dev,
 					"pin_cfg[%d]: invalid column number %d\n",
 					i, pin_cfg->num);
@@ -520,6 +531,18 @@ static int tegra_kbc_parse_dt(struct tegra_kbc *kbc)
 	}
 	num_cols = proplen / sizeof(u32);
 
+	if (num_rows > kbc->hw_support->max_rows) {
+		dev_err(kbc->dev,
+			"Number of rows is more than supported by hardware\n");
+		return -EINVAL;
+	}
+
+	if (num_cols > kbc->hw_support->max_columns) {
+		dev_err(kbc->dev,
+			"Number of cols is more than supported by hardware\n");
+		return -EINVAL;
+	}
+
 	if (!of_get_property(np, "linux,keymap", &proplen)) {
 		dev_err(kbc->dev, "property linux,keymap not found\n");
 		return -ENOENT;
@@ -532,7 +555,7 @@ static int tegra_kbc_parse_dt(struct tegra_kbc *kbc)
 	}
 
 	/* Set all pins as non-configured */
-	for (i = 0; i < KBC_MAX_GPIO; i++)
+	for (i = 0; i < kbc->num_rows_and_columns; i++)
 		kbc->pin_cfg[i].type = PIN_CFG_IGNORE;
 
 	ret = of_property_read_u32_array(np, "nvidia,kbc-row-pins",
@@ -562,6 +585,24 @@ static int tegra_kbc_parse_dt(struct tegra_kbc *kbc)
 	return 0;
 }
 
+static const struct tegra_kbc_hw_support tegra20_kbc_hw_support = {
+	.max_rows	= 16,
+	.max_columns	= 8,
+};
+
+static const struct tegra_kbc_hw_support tegra11_kbc_hw_support = {
+	.max_rows	= 11,
+	.max_columns	= 8,
+};
+
+static const struct of_device_id tegra_kbc_of_match[] = {
+	{ .compatible = "nvidia,tegra114-kbc", .data = &tegra11_kbc_hw_support},
+	{ .compatible = "nvidia,tegra30-kbc", .data = &tegra20_kbc_hw_support},
+	{ .compatible = "nvidia,tegra20-kbc", .data = &tegra20_kbc_hw_support},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, tegra_kbc_of_match);
+
 static int tegra_kbc_probe(struct platform_device *pdev)
 {
 	struct tegra_kbc *kbc;
@@ -570,7 +611,10 @@ static int tegra_kbc_probe(struct platform_device *pdev)
 	int num_rows = 0;
 	unsigned int debounce_cnt;
 	unsigned int scan_time_rows;
-	unsigned int keymap_rows = KBC_MAX_KEY;
+	unsigned int keymap_rows;
+	const struct of_device_id *match;
+
+	match = of_match_device(of_match_ptr(tegra_kbc_of_match), &pdev->dev);
 
 	kbc = devm_kzalloc(&pdev->dev, sizeof(*kbc), GFP_KERNEL);
 	if (!kbc) {
@@ -579,6 +623,12 @@ static int tegra_kbc_probe(struct platform_device *pdev)
 	}
 
 	kbc->dev = &pdev->dev;
+	kbc->hw_support = match->data;
+	kbc->max_keys = kbc->hw_support->max_rows *
+				kbc->hw_support->max_columns;
+	kbc->num_rows_and_columns = kbc->hw_support->max_rows +
+					kbc->hw_support->max_columns;
+	keymap_rows = kbc->max_keys;
 	spin_lock_init(&kbc->lock);
 
 	err = tegra_kbc_parse_dt(kbc);
@@ -608,11 +658,9 @@ static int tegra_kbc_probe(struct platform_device *pdev)
 
 	setup_timer(&kbc->timer, tegra_kbc_keypress_timer, (unsigned long)kbc);
 
-	kbc->mmio = devm_request_and_ioremap(&pdev->dev, res);
-	if (!kbc->mmio) {
-		dev_err(&pdev->dev, "Cannot request memregion/iomap address\n");
-		return -EBUSY;
-	}
+	kbc->mmio = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(kbc->mmio))
+		return PTR_ERR(kbc->mmio);
 
 	kbc->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(kbc->clk)) {
@@ -641,7 +689,8 @@ static int tegra_kbc_probe(struct platform_device *pdev)
 		keymap_rows *= 2;
 
 	err = matrix_keypad_build_keymap(kbc->keymap_data, NULL,
-					 keymap_rows, KBC_MAX_COL,
+					 keymap_rows,
+					 kbc->hw_support->max_columns,
 					 kbc->keycode, kbc->idev);
 	if (err) {
 		dev_err(&pdev->dev, "failed to setup keymap\n");
@@ -767,12 +816,6 @@ static int tegra_kbc_resume(struct device *dev)
 
 static SIMPLE_DEV_PM_OPS(tegra_kbc_pm_ops, tegra_kbc_suspend, tegra_kbc_resume);
 
-static const struct of_device_id tegra_kbc_of_match[] = {
-	{ .compatible = "nvidia,tegra20-kbc", },
-	{ },
-};
-MODULE_DEVICE_TABLE(of, tegra_kbc_of_match);
-
 static struct platform_driver tegra_kbc_driver = {
 	.probe		= tegra_kbc_probe,
 	.driver	= {
diff --git a/drivers/input/matrix-keymap.c b/drivers/input/matrix-keymap.c
index 3ae496ea5fe6..08b61f506db6 100644
--- a/drivers/input/matrix-keymap.c
+++ b/drivers/input/matrix-keymap.c
@@ -50,6 +50,26 @@ static bool matrix_keypad_map_key(struct input_dev *input_dev,
 }
 
 #ifdef CONFIG_OF
+int matrix_keypad_parse_of_params(struct device *dev,
+				  unsigned int *rows, unsigned int *cols)
+{
+	struct device_node *np = dev->of_node;
+
+	if (!np) {
+		dev_err(dev, "missing DT data");
+		return -EINVAL;
+	}
+	of_property_read_u32(np, "keypad,num-rows", rows);
+	of_property_read_u32(np, "keypad,num-columns", cols);
+	if (!*rows || !*cols) {
+		dev_err(dev, "number of keypad rows/columns not specified\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(matrix_keypad_parse_of_params);
+
 static int matrix_keypad_parse_of_keymap(const char *propname,
 					 unsigned int rows, unsigned int cols,
 					 struct input_dev *input_dev)
diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig
index 259ef31abb18..bb698e1f9e42 100644
--- a/drivers/input/misc/Kconfig
+++ b/drivers/input/misc/Kconfig
@@ -214,7 +214,7 @@ config INPUT_APANEL
 config INPUT_GP2A
 	tristate "Sharp GP2AP002A00F I2C Proximity/Opto sensor driver"
 	depends on I2C
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  Say Y here if you have a Sharp GP2AP002A00F proximity/als combo-chip
 	  hooked to an I2C bus.
@@ -224,7 +224,7 @@ config INPUT_GP2A
 
 config INPUT_GPIO_TILT_POLLED
 	tristate "Polled GPIO tilt switch"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select INPUT_POLLDEV
 	help
 	  This driver implements support for tilt switches connected
@@ -472,7 +472,7 @@ config INPUT_PWM_BEEPER
 
 config INPUT_GPIO_ROTARY_ENCODER
 	tristate "Rotary encoders connected to GPIO pins"
-	depends on GPIOLIB && GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  Say Y here to add support for rotary encoders connected to GPIO lines.
 	  Check file:Documentation/input/rotary-encoder.txt for more
@@ -484,7 +484,7 @@ config INPUT_GPIO_ROTARY_ENCODER
 config INPUT_RB532_BUTTON
 	tristate "Mikrotik Routerboard 532 button interface"
 	depends on MIKROTIK_RB532
-	depends on GPIOLIB && GENERIC_GPIO
+	depends on GPIOLIB
 	select INPUT_POLLDEV
 	help
 	  Say Y here if you want support for the S1 button built into
@@ -590,6 +590,16 @@ config INPUT_ADXL34X_SPI
 	  To compile this driver as a module, choose M here: the
 	  module will be called adxl34x-spi.
 
+config INPUT_IMS_PCU
+	tristate "IMS Passenger Control Unit driver"
+	depends on USB
+	depends on LEDS_CLASS
+	help
+	  Say Y here if you have system with IMS Rave Passenger Control Unit.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called ims_pcu.
+
 config INPUT_CMA3000
 	tristate "VTI CMA3000 Tri-axis accelerometer"
 	help
diff --git a/drivers/input/misc/Makefile b/drivers/input/misc/Makefile
index 1f1e1b109d9d..d7fc17f11d77 100644
--- a/drivers/input/misc/Makefile
+++ b/drivers/input/misc/Makefile
@@ -29,6 +29,7 @@ obj-$(CONFIG_INPUT_DM355EVM)		+= dm355evm_keys.o
 obj-$(CONFIG_INPUT_GP2A)		+= gp2ap002a00f.o
 obj-$(CONFIG_INPUT_GPIO_TILT_POLLED)	+= gpio_tilt_polled.o
 obj-$(CONFIG_HP_SDC_RTC)		+= hp_sdc_rtc.o
+obj-$(CONFIG_INPUT_IMS_PCU)		+= ims-pcu.o
 obj-$(CONFIG_INPUT_IXP4XX_BEEPER)	+= ixp4xx-beeper.o
 obj-$(CONFIG_INPUT_KEYSPAN_REMOTE)	+= keyspan_remote.o
 obj-$(CONFIG_INPUT_KXTJ9)		+= kxtj9.o
diff --git a/drivers/input/misc/ad714x-i2c.c b/drivers/input/misc/ad714x-i2c.c
index 29d2064c26f2..e0f522516ef5 100644
--- a/drivers/input/misc/ad714x-i2c.c
+++ b/drivers/input/misc/ad714x-i2c.c
@@ -13,7 +13,7 @@
 #include <linux/pm.h>
 #include "ad714x.h"
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int ad714x_i2c_suspend(struct device *dev)
 {
 	return ad714x_disable(i2c_get_clientdata(to_i2c_client(dev)));
diff --git a/drivers/input/misc/ad714x-spi.c b/drivers/input/misc/ad714x-spi.c
index bdccca42d138..61891486067c 100644
--- a/drivers/input/misc/ad714x-spi.c
+++ b/drivers/input/misc/ad714x-spi.c
@@ -16,7 +16,7 @@
 #define AD714x_SPI_CMD_PREFIX      0xE000   /* bits 15:11 */
 #define AD714x_SPI_READ            BIT(10)
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int ad714x_spi_suspend(struct device *dev)
 {
 	return ad714x_disable(spi_get_drvdata(to_spi_device(dev)));
diff --git a/drivers/input/misc/adxl34x-i2c.c b/drivers/input/misc/adxl34x-i2c.c
index 535dda48cace..416f47ddcc90 100644
--- a/drivers/input/misc/adxl34x-i2c.c
+++ b/drivers/input/misc/adxl34x-i2c.c
@@ -105,7 +105,7 @@ static int adxl34x_i2c_remove(struct i2c_client *client)
 	return adxl34x_remove(ac);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int adxl34x_i2c_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
diff --git a/drivers/input/misc/adxl34x-spi.c b/drivers/input/misc/adxl34x-spi.c
index ad5f40d37e48..76dc0679d3b1 100644
--- a/drivers/input/misc/adxl34x-spi.c
+++ b/drivers/input/misc/adxl34x-spi.c
@@ -89,16 +89,16 @@ static int adxl34x_spi_probe(struct spi_device *spi)
 
 static int adxl34x_spi_remove(struct spi_device *spi)
 {
-	struct adxl34x *ac = dev_get_drvdata(&spi->dev);
+	struct adxl34x *ac = spi_get_drvdata(spi);
 
 	return adxl34x_remove(ac);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int adxl34x_spi_suspend(struct device *dev)
 {
 	struct spi_device *spi = to_spi_device(dev);
-	struct adxl34x *ac = dev_get_drvdata(&spi->dev);
+	struct adxl34x *ac = spi_get_drvdata(spi);
 
 	adxl34x_suspend(ac);
 
@@ -108,7 +108,7 @@ static int adxl34x_spi_suspend(struct device *dev)
 static int adxl34x_spi_resume(struct device *dev)
 {
 	struct spi_device *spi = to_spi_device(dev);
-	struct adxl34x *ac = dev_get_drvdata(&spi->dev);
+	struct adxl34x *ac = spi_get_drvdata(spi);
 
 	adxl34x_resume(ac);
 
diff --git a/drivers/input/misc/hp_sdc_rtc.c b/drivers/input/misc/hp_sdc_rtc.c
index 2e3334b8f82d..86b822806e95 100644
--- a/drivers/input/misc/hp_sdc_rtc.c
+++ b/drivers/input/misc/hp_sdc_rtc.c
@@ -41,6 +41,7 @@
 #include <linux/time.h>
 #include <linux/miscdevice.h>
 #include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 #include <linux/poll.h>
 #include <linux/rtc.h>
 #include <linux/mutex.h>
@@ -74,9 +75,6 @@ static unsigned int hp_sdc_rtc_poll(struct file *file, poll_table *wait);
 static int hp_sdc_rtc_open(struct inode *inode, struct file *file);
 static int hp_sdc_rtc_fasync (int fd, struct file *filp, int on);
 
-static int hp_sdc_rtc_read_proc(char *page, char **start, off_t off,
-				int count, int *eof, void *data);
-
 static void hp_sdc_rtc_isr (int irq, void *dev_id, 
 			    uint8_t status, uint8_t data) 
 {
@@ -427,22 +425,19 @@ static int hp_sdc_rtc_fasync (int fd, struct file *filp, int on)
         return fasync_helper (fd, filp, on, &hp_sdc_rtc_async_queue);
 }
 
-static int hp_sdc_rtc_proc_output (char *buf)
+static int hp_sdc_rtc_proc_show(struct seq_file *m, void *v)
 {
 #define YN(bit) ("no")
 #define NY(bit) ("yes")
-        char *p;
         struct rtc_time tm;
 	struct timeval tv;
 
 	memset(&tm, 0, sizeof(struct rtc_time));
 
-	p = buf;
-
 	if (hp_sdc_rtc_read_bbrtc(&tm)) {
-		p += sprintf(p, "BBRTC\t\t: READ FAILED!\n");
+		seq_puts(m, "BBRTC\t\t: READ FAILED!\n");
 	} else {
-		p += sprintf(p,
+		seq_printf(m,
 			     "rtc_time\t: %02d:%02d:%02d\n"
 			     "rtc_date\t: %04d-%02d-%02d\n"
 			     "rtc_epoch\t: %04lu\n",
@@ -452,41 +447,41 @@ static int hp_sdc_rtc_proc_output (char *buf)
 	}
 
 	if (hp_sdc_rtc_read_rt(&tv)) {
-		p += sprintf(p, "i8042 rtc\t: READ FAILED!\n");
+		seq_puts(m, "i8042 rtc\t: READ FAILED!\n");
 	} else {
-		p += sprintf(p, "i8042 rtc\t: %ld.%02d seconds\n", 
+		seq_printf(m, "i8042 rtc\t: %ld.%02d seconds\n", 
 			     tv.tv_sec, (int)tv.tv_usec/1000);
 	}
 
 	if (hp_sdc_rtc_read_fhs(&tv)) {
-		p += sprintf(p, "handshake\t: READ FAILED!\n");
+		seq_puts(m, "handshake\t: READ FAILED!\n");
 	} else {
-        	p += sprintf(p, "handshake\t: %ld.%02d seconds\n", 
+        	seq_printf(m, "handshake\t: %ld.%02d seconds\n", 
 			     tv.tv_sec, (int)tv.tv_usec/1000);
 	}
 
 	if (hp_sdc_rtc_read_mt(&tv)) {
-		p += sprintf(p, "alarm\t\t: READ FAILED!\n");
+		seq_puts(m, "alarm\t\t: READ FAILED!\n");
 	} else {
-		p += sprintf(p, "alarm\t\t: %ld.%02d seconds\n", 
+		seq_printf(m, "alarm\t\t: %ld.%02d seconds\n", 
 			     tv.tv_sec, (int)tv.tv_usec/1000);
 	}
 
 	if (hp_sdc_rtc_read_dt(&tv)) {
-		p += sprintf(p, "delay\t\t: READ FAILED!\n");
+		seq_puts(m, "delay\t\t: READ FAILED!\n");
 	} else {
-		p += sprintf(p, "delay\t\t: %ld.%02d seconds\n", 
+		seq_printf(m, "delay\t\t: %ld.%02d seconds\n", 
 			     tv.tv_sec, (int)tv.tv_usec/1000);
 	}
 
 	if (hp_sdc_rtc_read_ct(&tv)) {
-		p += sprintf(p, "periodic\t: READ FAILED!\n");
+		seq_puts(m, "periodic\t: READ FAILED!\n");
 	} else {
-		p += sprintf(p, "periodic\t: %ld.%02d seconds\n", 
+		seq_printf(m, "periodic\t: %ld.%02d seconds\n", 
 			     tv.tv_sec, (int)tv.tv_usec/1000);
 	}
 
-        p += sprintf(p,
+        seq_printf(m,
                      "DST_enable\t: %s\n"
                      "BCD\t\t: %s\n"
                      "24hr\t\t: %s\n"
@@ -506,23 +501,23 @@ static int hp_sdc_rtc_proc_output (char *buf)
                      1UL,
                      1 ? "okay" : "dead");
 
-        return  p - buf;
+        return 0;
 #undef YN
 #undef NY
 }
 
-static int hp_sdc_rtc_read_proc(char *page, char **start, off_t off,
-                         int count, int *eof, void *data)
+static int hp_sdc_rtc_proc_open(struct inode *inode, struct file *file)
 {
-	int len = hp_sdc_rtc_proc_output (page);
-        if (len <= off+count) *eof = 1;
-        *start = page + off;
-        len -= off;
-        if (len>count) len = count;
-        if (len<0) len = 0;
-        return len;
+	return single_open(file, hp_sdc_rtc_proc_show, NULL);
 }
 
+static const struct file_operations hp_sdc_rtc_proc_fops = {
+	.open		= hp_sdc_rtc_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
 static int hp_sdc_rtc_ioctl(struct file *file, 
 			    unsigned int cmd, unsigned long arg)
 {
@@ -715,8 +710,7 @@ static int __init hp_sdc_rtc_init(void)
 	if (misc_register(&hp_sdc_rtc_dev) != 0)
 		printk(KERN_INFO "Could not register misc. dev for i8042 rtc\n");
 
-        create_proc_read_entry ("driver/rtc", 0, NULL,
-				hp_sdc_rtc_read_proc, NULL);
+        proc_create("driver/rtc", 0, NULL, &hp_sdc_rtc_proc_fops);
 
 	printk(KERN_INFO "HP i8042 SDC + MSM-58321 RTC support loaded "
 			 "(RTC v " RTC_VERSION ")\n");
diff --git a/drivers/input/misc/ims-pcu.c b/drivers/input/misc/ims-pcu.c
new file mode 100644
index 000000000000..e204f26b0011
--- /dev/null
+++ b/drivers/input/misc/ims-pcu.c
@@ -0,0 +1,1901 @@
+/*
+ * Driver for IMS Passenger Control Unit Devices
+ *
+ * Copyright (C) 2013 The IMS Company
+ *
+ * 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/completion.h>
+#include <linux/device.h>
+#include <linux/firmware.h>
+#include <linux/ihex.h>
+#include <linux/input.h>
+#include <linux/kernel.h>
+#include <linux/leds.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/usb/input.h>
+#include <linux/usb/cdc.h>
+#include <asm/unaligned.h>
+
+#define IMS_PCU_KEYMAP_LEN		32
+
+struct ims_pcu_buttons {
+	struct input_dev *input;
+	char name[32];
+	char phys[32];
+	unsigned short keymap[IMS_PCU_KEYMAP_LEN];
+};
+
+struct ims_pcu_gamepad {
+	struct input_dev *input;
+	char name[32];
+	char phys[32];
+};
+
+struct ims_pcu_backlight {
+	struct led_classdev cdev;
+	struct work_struct work;
+	enum led_brightness desired_brightness;
+	char name[32];
+};
+
+#define IMS_PCU_PART_NUMBER_LEN		15
+#define IMS_PCU_SERIAL_NUMBER_LEN	8
+#define IMS_PCU_DOM_LEN			8
+#define IMS_PCU_FW_VERSION_LEN		(9 + 1)
+#define IMS_PCU_BL_VERSION_LEN		(9 + 1)
+#define IMS_PCU_BL_RESET_REASON_LEN	(2 + 1)
+
+#define IMS_PCU_BUF_SIZE		128
+
+struct ims_pcu {
+	struct usb_device *udev;
+	struct device *dev; /* control interface's device, used for logging */
+
+	unsigned int device_no;
+
+	bool bootloader_mode;
+
+	char part_number[IMS_PCU_PART_NUMBER_LEN];
+	char serial_number[IMS_PCU_SERIAL_NUMBER_LEN];
+	char date_of_manufacturing[IMS_PCU_DOM_LEN];
+	char fw_version[IMS_PCU_FW_VERSION_LEN];
+	char bl_version[IMS_PCU_BL_VERSION_LEN];
+	char reset_reason[IMS_PCU_BL_RESET_REASON_LEN];
+	int update_firmware_status;
+
+	struct usb_interface *ctrl_intf;
+
+	struct usb_endpoint_descriptor *ep_ctrl;
+	struct urb *urb_ctrl;
+	u8 *urb_ctrl_buf;
+	dma_addr_t ctrl_dma;
+	size_t max_ctrl_size;
+
+	struct usb_interface *data_intf;
+
+	struct usb_endpoint_descriptor *ep_in;
+	struct urb *urb_in;
+	u8 *urb_in_buf;
+	dma_addr_t read_dma;
+	size_t max_in_size;
+
+	struct usb_endpoint_descriptor *ep_out;
+	u8 *urb_out_buf;
+	size_t max_out_size;
+
+	u8 read_buf[IMS_PCU_BUF_SIZE];
+	u8 read_pos;
+	u8 check_sum;
+	bool have_stx;
+	bool have_dle;
+
+	u8 cmd_buf[IMS_PCU_BUF_SIZE];
+	u8 ack_id;
+	u8 expected_response;
+	u8 cmd_buf_len;
+	struct completion cmd_done;
+	struct mutex cmd_mutex;
+
+	u32 fw_start_addr;
+	u32 fw_end_addr;
+	struct completion async_firmware_done;
+
+	struct ims_pcu_buttons buttons;
+	struct ims_pcu_gamepad *gamepad;
+	struct ims_pcu_backlight backlight;
+
+	bool setup_complete; /* Input and LED devices have been created */
+};
+
+
+/*********************************************************************
+ *             Buttons Input device support                          *
+ *********************************************************************/
+
+static const unsigned short ims_pcu_keymap_1[] = {
+	[1] = KEY_ATTENDANT_OFF,
+	[2] = KEY_ATTENDANT_ON,
+	[3] = KEY_LIGHTS_TOGGLE,
+	[4] = KEY_VOLUMEUP,
+	[5] = KEY_VOLUMEDOWN,
+	[6] = KEY_INFO,
+};
+
+static const unsigned short ims_pcu_keymap_2[] = {
+	[4] = KEY_VOLUMEUP,
+	[5] = KEY_VOLUMEDOWN,
+	[6] = KEY_INFO,
+};
+
+static const unsigned short ims_pcu_keymap_3[] = {
+	[1] = KEY_HOMEPAGE,
+	[2] = KEY_ATTENDANT_TOGGLE,
+	[3] = KEY_LIGHTS_TOGGLE,
+	[4] = KEY_VOLUMEUP,
+	[5] = KEY_VOLUMEDOWN,
+	[6] = KEY_DISPLAYTOGGLE,
+	[18] = KEY_PLAYPAUSE,
+};
+
+static const unsigned short ims_pcu_keymap_4[] = {
+	[1] = KEY_ATTENDANT_OFF,
+	[2] = KEY_ATTENDANT_ON,
+	[3] = KEY_LIGHTS_TOGGLE,
+	[4] = KEY_VOLUMEUP,
+	[5] = KEY_VOLUMEDOWN,
+	[6] = KEY_INFO,
+	[18] = KEY_PLAYPAUSE,
+};
+
+static const unsigned short ims_pcu_keymap_5[] = {
+	[1] = KEY_ATTENDANT_OFF,
+	[2] = KEY_ATTENDANT_ON,
+	[3] = KEY_LIGHTS_TOGGLE,
+};
+
+struct ims_pcu_device_info {
+	const unsigned short *keymap;
+	size_t keymap_len;
+	bool has_gamepad;
+};
+
+#define IMS_PCU_DEVINFO(_n, _gamepad)				\
+	[_n] = {						\
+		.keymap = ims_pcu_keymap_##_n,			\
+		.keymap_len = ARRAY_SIZE(ims_pcu_keymap_##_n),	\
+		.has_gamepad = _gamepad,			\
+	}
+
+static const struct ims_pcu_device_info ims_pcu_device_info[] = {
+	IMS_PCU_DEVINFO(1, true),
+	IMS_PCU_DEVINFO(2, true),
+	IMS_PCU_DEVINFO(3, true),
+	IMS_PCU_DEVINFO(4, true),
+	IMS_PCU_DEVINFO(5, false),
+};
+
+static void ims_pcu_buttons_report(struct ims_pcu *pcu, u32 data)
+{
+	struct ims_pcu_buttons *buttons = &pcu->buttons;
+	struct input_dev *input = buttons->input;
+	int i;
+
+	for (i = 0; i < 32; i++) {
+		unsigned short keycode = buttons->keymap[i];
+
+		if (keycode != KEY_RESERVED)
+			input_report_key(input, keycode, data & (1UL << i));
+	}
+
+	input_sync(input);
+}
+
+static int ims_pcu_setup_buttons(struct ims_pcu *pcu,
+				 const unsigned short *keymap,
+				 size_t keymap_len)
+{
+	struct ims_pcu_buttons *buttons = &pcu->buttons;
+	struct input_dev *input;
+	int i;
+	int error;
+
+	input = input_allocate_device();
+	if (!input) {
+		dev_err(pcu->dev,
+			"Not enough memory for input input device\n");
+		return -ENOMEM;
+	}
+
+	snprintf(buttons->name, sizeof(buttons->name),
+		 "IMS PCU#%d Button Interface", pcu->device_no);
+
+	usb_make_path(pcu->udev, buttons->phys, sizeof(buttons->phys));
+	strlcat(buttons->phys, "/input0", sizeof(buttons->phys));
+
+	memcpy(buttons->keymap, keymap, sizeof(*keymap) * keymap_len);
+
+	input->name = buttons->name;
+	input->phys = buttons->phys;
+	usb_to_input_id(pcu->udev, &input->id);
+	input->dev.parent = &pcu->ctrl_intf->dev;
+
+	input->keycode = buttons->keymap;
+	input->keycodemax = ARRAY_SIZE(buttons->keymap);
+	input->keycodesize = sizeof(buttons->keymap[0]);
+
+	__set_bit(EV_KEY, input->evbit);
+	for (i = 0; i < IMS_PCU_KEYMAP_LEN; i++)
+		__set_bit(buttons->keymap[i], input->keybit);
+	__clear_bit(KEY_RESERVED, input->keybit);
+
+	error = input_register_device(input);
+	if (error) {
+		dev_err(pcu->dev,
+			"Failed to register buttons input device: %d\n",
+			error);
+		input_free_device(input);
+		return error;
+	}
+
+	buttons->input = input;
+	return 0;
+}
+
+static void ims_pcu_destroy_buttons(struct ims_pcu *pcu)
+{
+	struct ims_pcu_buttons *buttons = &pcu->buttons;
+
+	input_unregister_device(buttons->input);
+}
+
+
+/*********************************************************************
+ *             Gamepad Input device support                          *
+ *********************************************************************/
+
+static void ims_pcu_gamepad_report(struct ims_pcu *pcu, u32 data)
+{
+	struct ims_pcu_gamepad *gamepad = pcu->gamepad;
+	struct input_dev *input = gamepad->input;
+	int x, y;
+
+	x = !!(data & (1 << 14)) - !!(data & (1 << 13));
+	y = !!(data & (1 << 12)) - !!(data & (1 << 11));
+
+	input_report_abs(input, ABS_X, x);
+	input_report_abs(input, ABS_Y, y);
+
+	input_report_key(input, BTN_A, data & (1 << 7));
+	input_report_key(input, BTN_B, data & (1 << 8));
+	input_report_key(input, BTN_X, data & (1 << 9));
+	input_report_key(input, BTN_Y, data & (1 << 10));
+	input_report_key(input, BTN_START, data & (1 << 15));
+	input_report_key(input, BTN_SELECT, data & (1 << 16));
+
+	input_sync(input);
+}
+
+static int ims_pcu_setup_gamepad(struct ims_pcu *pcu)
+{
+	struct ims_pcu_gamepad *gamepad;
+	struct input_dev *input;
+	int error;
+
+	gamepad = kzalloc(sizeof(struct ims_pcu_gamepad), GFP_KERNEL);
+	input = input_allocate_device();
+	if (!gamepad || !input) {
+		dev_err(pcu->dev,
+			"Not enough memory for gamepad device\n");
+		error = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	gamepad->input = input;
+
+	snprintf(gamepad->name, sizeof(gamepad->name),
+		 "IMS PCU#%d Gamepad Interface", pcu->device_no);
+
+	usb_make_path(pcu->udev, gamepad->phys, sizeof(gamepad->phys));
+	strlcat(gamepad->phys, "/input1", sizeof(gamepad->phys));
+
+	input->name = gamepad->name;
+	input->phys = gamepad->phys;
+	usb_to_input_id(pcu->udev, &input->id);
+	input->dev.parent = &pcu->ctrl_intf->dev;
+
+	__set_bit(EV_KEY, input->evbit);
+	__set_bit(BTN_A, input->keybit);
+	__set_bit(BTN_B, input->keybit);
+	__set_bit(BTN_X, input->keybit);
+	__set_bit(BTN_Y, input->keybit);
+	__set_bit(BTN_START, input->keybit);
+	__set_bit(BTN_SELECT, input->keybit);
+
+	__set_bit(EV_ABS, input->evbit);
+	input_set_abs_params(input, ABS_X, -1, 1, 0, 0);
+	input_set_abs_params(input, ABS_Y, -1, 1, 0, 0);
+
+	error = input_register_device(input);
+	if (error) {
+		dev_err(pcu->dev,
+			"Failed to register gamepad input device: %d\n",
+			error);
+		goto err_free_mem;
+	}
+
+	pcu->gamepad = gamepad;
+	return 0;
+
+err_free_mem:
+	input_free_device(input);
+	kfree(gamepad);
+	return -ENOMEM;
+}
+
+static void ims_pcu_destroy_gamepad(struct ims_pcu *pcu)
+{
+	struct ims_pcu_gamepad *gamepad = pcu->gamepad;
+
+	input_unregister_device(gamepad->input);
+	kfree(gamepad);
+}
+
+
+/*********************************************************************
+ *             PCU Communication protocol handling                   *
+ *********************************************************************/
+
+#define IMS_PCU_PROTOCOL_STX		0x02
+#define IMS_PCU_PROTOCOL_ETX		0x03
+#define IMS_PCU_PROTOCOL_DLE		0x10
+
+/* PCU commands */
+#define IMS_PCU_CMD_STATUS		0xa0
+#define IMS_PCU_CMD_PCU_RESET		0xa1
+#define IMS_PCU_CMD_RESET_REASON	0xa2
+#define IMS_PCU_CMD_SEND_BUTTONS	0xa3
+#define IMS_PCU_CMD_JUMP_TO_BTLDR	0xa4
+#define IMS_PCU_CMD_GET_INFO		0xa5
+#define IMS_PCU_CMD_SET_BRIGHTNESS	0xa6
+#define IMS_PCU_CMD_EEPROM		0xa7
+#define IMS_PCU_CMD_GET_FW_VERSION	0xa8
+#define IMS_PCU_CMD_GET_BL_VERSION	0xa9
+#define IMS_PCU_CMD_SET_INFO		0xab
+#define IMS_PCU_CMD_GET_BRIGHTNESS	0xac
+#define IMS_PCU_CMD_GET_DEVICE_ID	0xae
+#define IMS_PCU_CMD_SPECIAL_INFO	0xb0
+#define IMS_PCU_CMD_BOOTLOADER		0xb1	/* Pass data to bootloader */
+
+/* PCU responses */
+#define IMS_PCU_RSP_STATUS		0xc0
+#define IMS_PCU_RSP_PCU_RESET		0	/* Originally 0xc1 */
+#define IMS_PCU_RSP_RESET_REASON	0xc2
+#define IMS_PCU_RSP_SEND_BUTTONS	0xc3
+#define IMS_PCU_RSP_JUMP_TO_BTLDR	0	/* Originally 0xc4 */
+#define IMS_PCU_RSP_GET_INFO		0xc5
+#define IMS_PCU_RSP_SET_BRIGHTNESS	0xc6
+#define IMS_PCU_RSP_EEPROM		0xc7
+#define IMS_PCU_RSP_GET_FW_VERSION	0xc8
+#define IMS_PCU_RSP_GET_BL_VERSION	0xc9
+#define IMS_PCU_RSP_SET_INFO		0xcb
+#define IMS_PCU_RSP_GET_BRIGHTNESS	0xcc
+#define IMS_PCU_RSP_CMD_INVALID		0xcd
+#define IMS_PCU_RSP_GET_DEVICE_ID	0xce
+#define IMS_PCU_RSP_SPECIAL_INFO	0xd0
+#define IMS_PCU_RSP_BOOTLOADER		0xd1	/* Bootloader response */
+
+#define IMS_PCU_RSP_EVNT_BUTTONS	0xe0	/* Unsolicited, button state */
+#define IMS_PCU_GAMEPAD_MASK		0x0001ff80UL	/* Bits 7 through 16 */
+
+
+#define IMS_PCU_MIN_PACKET_LEN		3
+#define IMS_PCU_DATA_OFFSET		2
+
+#define IMS_PCU_CMD_WRITE_TIMEOUT	100 /* msec */
+#define IMS_PCU_CMD_RESPONSE_TIMEOUT	500 /* msec */
+
+static void ims_pcu_report_events(struct ims_pcu *pcu)
+{
+	u32 data = get_unaligned_be32(&pcu->read_buf[3]);
+
+	ims_pcu_buttons_report(pcu, data & ~IMS_PCU_GAMEPAD_MASK);
+	if (pcu->gamepad)
+		ims_pcu_gamepad_report(pcu, data);
+}
+
+static void ims_pcu_handle_response(struct ims_pcu *pcu)
+{
+	switch (pcu->read_buf[0]) {
+	case IMS_PCU_RSP_EVNT_BUTTONS:
+		if (likely(pcu->setup_complete))
+			ims_pcu_report_events(pcu);
+		break;
+
+	default:
+		/*
+		 * See if we got command completion.
+		 * If both the sequence and response code match save
+		 * the data and signal completion.
+		 */
+		if (pcu->read_buf[0] == pcu->expected_response &&
+		    pcu->read_buf[1] == pcu->ack_id - 1) {
+
+			memcpy(pcu->cmd_buf, pcu->read_buf, pcu->read_pos);
+			pcu->cmd_buf_len = pcu->read_pos;
+			complete(&pcu->cmd_done);
+		}
+		break;
+	}
+}
+
+static void ims_pcu_process_data(struct ims_pcu *pcu, struct urb *urb)
+{
+	int i;
+
+	for (i = 0; i < urb->actual_length; i++) {
+		u8 data = pcu->urb_in_buf[i];
+
+		/* Skip everything until we get Start Xmit */
+		if (!pcu->have_stx && data != IMS_PCU_PROTOCOL_STX)
+			continue;
+
+		if (pcu->have_dle) {
+			pcu->have_dle = false;
+			pcu->read_buf[pcu->read_pos++] = data;
+			pcu->check_sum += data;
+			continue;
+		}
+
+		switch (data) {
+		case IMS_PCU_PROTOCOL_STX:
+			if (pcu->have_stx)
+				dev_warn(pcu->dev,
+					 "Unexpected STX at byte %d, discarding old data\n",
+					 pcu->read_pos);
+			pcu->have_stx = true;
+			pcu->have_dle = false;
+			pcu->read_pos = 0;
+			pcu->check_sum = 0;
+			break;
+
+		case IMS_PCU_PROTOCOL_DLE:
+			pcu->have_dle = true;
+			break;
+
+		case IMS_PCU_PROTOCOL_ETX:
+			if (pcu->read_pos < IMS_PCU_MIN_PACKET_LEN) {
+				dev_warn(pcu->dev,
+					 "Short packet received (%d bytes), ignoring\n",
+					 pcu->read_pos);
+			} else if (pcu->check_sum != 0) {
+				dev_warn(pcu->dev,
+					 "Invalid checksum in packet (%d bytes), ignoring\n",
+					 pcu->read_pos);
+			} else {
+				ims_pcu_handle_response(pcu);
+			}
+
+			pcu->have_stx = false;
+			pcu->have_dle = false;
+			pcu->read_pos = 0;
+			break;
+
+		default:
+			pcu->read_buf[pcu->read_pos++] = data;
+			pcu->check_sum += data;
+			break;
+		}
+	}
+}
+
+static bool ims_pcu_byte_needs_escape(u8 byte)
+{
+	return byte == IMS_PCU_PROTOCOL_STX ||
+	       byte == IMS_PCU_PROTOCOL_ETX ||
+	       byte == IMS_PCU_PROTOCOL_DLE;
+}
+
+static int ims_pcu_send_cmd_chunk(struct ims_pcu *pcu,
+				  u8 command, int chunk, int len)
+{
+	int error;
+
+	error = usb_bulk_msg(pcu->udev,
+			     usb_sndbulkpipe(pcu->udev,
+					     pcu->ep_out->bEndpointAddress),
+			     pcu->urb_out_buf, len,
+			     NULL, IMS_PCU_CMD_WRITE_TIMEOUT);
+	if (error < 0) {
+		dev_dbg(pcu->dev,
+			"Sending 0x%02x command failed at chunk %d: %d\n",
+			command, chunk, error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int ims_pcu_send_command(struct ims_pcu *pcu,
+				u8 command, const u8 *data, int len)
+{
+	int count = 0;
+	int chunk = 0;
+	int delta;
+	int i;
+	int error;
+	u8 csum = 0;
+	u8 ack_id;
+
+	pcu->urb_out_buf[count++] = IMS_PCU_PROTOCOL_STX;
+
+	/* We know the command need not be escaped */
+	pcu->urb_out_buf[count++] = command;
+	csum += command;
+
+	ack_id = pcu->ack_id++;
+	if (ack_id == 0xff)
+		ack_id = pcu->ack_id++;
+
+	if (ims_pcu_byte_needs_escape(ack_id))
+		pcu->urb_out_buf[count++] = IMS_PCU_PROTOCOL_DLE;
+
+	pcu->urb_out_buf[count++] = ack_id;
+	csum += ack_id;
+
+	for (i = 0; i < len; i++) {
+
+		delta = ims_pcu_byte_needs_escape(data[i]) ? 2 : 1;
+		if (count + delta >= pcu->max_out_size) {
+			error = ims_pcu_send_cmd_chunk(pcu, command,
+						       ++chunk, count);
+			if (error)
+				return error;
+
+			count = 0;
+		}
+
+		if (delta == 2)
+			pcu->urb_out_buf[count++] = IMS_PCU_PROTOCOL_DLE;
+
+		pcu->urb_out_buf[count++] = data[i];
+		csum += data[i];
+	}
+
+	csum = 1 + ~csum;
+
+	delta = ims_pcu_byte_needs_escape(csum) ? 3 : 2;
+	if (count + delta >= pcu->max_out_size) {
+		error = ims_pcu_send_cmd_chunk(pcu, command, ++chunk, count);
+		if (error)
+			return error;
+
+		count = 0;
+	}
+
+	if (delta == 3)
+		pcu->urb_out_buf[count++] = IMS_PCU_PROTOCOL_DLE;
+
+	pcu->urb_out_buf[count++] = csum;
+	pcu->urb_out_buf[count++] = IMS_PCU_PROTOCOL_ETX;
+
+	return ims_pcu_send_cmd_chunk(pcu, command, ++chunk, count);
+}
+
+static int __ims_pcu_execute_command(struct ims_pcu *pcu,
+				     u8 command, const void *data, size_t len,
+				     u8 expected_response, int response_time)
+{
+	int error;
+
+	pcu->expected_response = expected_response;
+	init_completion(&pcu->cmd_done);
+
+	error = ims_pcu_send_command(pcu, command, data, len);
+	if (error)
+		return error;
+
+	if (expected_response &&
+	    !wait_for_completion_timeout(&pcu->cmd_done,
+					 msecs_to_jiffies(response_time))) {
+		dev_dbg(pcu->dev, "Command 0x%02x timed out\n", command);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+#define ims_pcu_execute_command(pcu, code, data, len)			\
+	__ims_pcu_execute_command(pcu,					\
+				  IMS_PCU_CMD_##code, data, len,	\
+				  IMS_PCU_RSP_##code,			\
+				  IMS_PCU_CMD_RESPONSE_TIMEOUT)
+
+#define ims_pcu_execute_query(pcu, code)				\
+	ims_pcu_execute_command(pcu, code, NULL, 0)
+
+/* Bootloader commands */
+#define IMS_PCU_BL_CMD_QUERY_DEVICE	0xa1
+#define IMS_PCU_BL_CMD_UNLOCK_CONFIG	0xa2
+#define IMS_PCU_BL_CMD_ERASE_APP	0xa3
+#define IMS_PCU_BL_CMD_PROGRAM_DEVICE	0xa4
+#define IMS_PCU_BL_CMD_PROGRAM_COMPLETE	0xa5
+#define IMS_PCU_BL_CMD_READ_APP		0xa6
+#define IMS_PCU_BL_CMD_RESET_DEVICE	0xa7
+#define IMS_PCU_BL_CMD_LAUNCH_APP	0xa8
+
+/* Bootloader commands */
+#define IMS_PCU_BL_RSP_QUERY_DEVICE	0xc1
+#define IMS_PCU_BL_RSP_UNLOCK_CONFIG	0xc2
+#define IMS_PCU_BL_RSP_ERASE_APP	0xc3
+#define IMS_PCU_BL_RSP_PROGRAM_DEVICE	0xc4
+#define IMS_PCU_BL_RSP_PROGRAM_COMPLETE	0xc5
+#define IMS_PCU_BL_RSP_READ_APP		0xc6
+#define IMS_PCU_BL_RSP_RESET_DEVICE	0	/* originally 0xa7 */
+#define IMS_PCU_BL_RSP_LAUNCH_APP	0	/* originally 0xa8 */
+
+#define IMS_PCU_BL_DATA_OFFSET		3
+
+static int __ims_pcu_execute_bl_command(struct ims_pcu *pcu,
+				        u8 command, const void *data, size_t len,
+				        u8 expected_response, int response_time)
+{
+	int error;
+
+	pcu->cmd_buf[0] = command;
+	if (data)
+		memcpy(&pcu->cmd_buf[1], data, len);
+
+	error = __ims_pcu_execute_command(pcu,
+				IMS_PCU_CMD_BOOTLOADER, pcu->cmd_buf, len + 1,
+				expected_response ? IMS_PCU_RSP_BOOTLOADER : 0,
+				response_time);
+	if (error) {
+		dev_err(pcu->dev,
+			"Failure when sending 0x%02x command to bootloader, error: %d\n",
+			pcu->cmd_buf[0], error);
+		return error;
+	}
+
+	if (expected_response && pcu->cmd_buf[2] != expected_response) {
+		dev_err(pcu->dev,
+			"Unexpected response from bootloader: 0x%02x, wanted 0x%02x\n",
+			pcu->cmd_buf[2], expected_response);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+#define ims_pcu_execute_bl_command(pcu, code, data, len, timeout)	\
+	__ims_pcu_execute_bl_command(pcu,				\
+				     IMS_PCU_BL_CMD_##code, data, len,	\
+				     IMS_PCU_BL_RSP_##code, timeout)	\
+
+#define IMS_PCU_INFO_PART_OFFSET	2
+#define IMS_PCU_INFO_DOM_OFFSET		17
+#define IMS_PCU_INFO_SERIAL_OFFSET	25
+
+#define IMS_PCU_SET_INFO_SIZE		31
+
+static int ims_pcu_get_info(struct ims_pcu *pcu)
+{
+	int error;
+
+	error = ims_pcu_execute_query(pcu, GET_INFO);
+	if (error) {
+		dev_err(pcu->dev,
+			"GET_INFO command failed, error: %d\n", error);
+		return error;
+	}
+
+	memcpy(pcu->part_number,
+	       &pcu->cmd_buf[IMS_PCU_INFO_PART_OFFSET],
+	       sizeof(pcu->part_number));
+	memcpy(pcu->date_of_manufacturing,
+	       &pcu->cmd_buf[IMS_PCU_INFO_DOM_OFFSET],
+	       sizeof(pcu->date_of_manufacturing));
+	memcpy(pcu->serial_number,
+	       &pcu->cmd_buf[IMS_PCU_INFO_SERIAL_OFFSET],
+	       sizeof(pcu->serial_number));
+
+	return 0;
+}
+
+static int ims_pcu_set_info(struct ims_pcu *pcu)
+{
+	int error;
+
+	memcpy(&pcu->cmd_buf[IMS_PCU_INFO_PART_OFFSET],
+	       pcu->part_number, sizeof(pcu->part_number));
+	memcpy(&pcu->cmd_buf[IMS_PCU_INFO_DOM_OFFSET],
+	       pcu->date_of_manufacturing, sizeof(pcu->date_of_manufacturing));
+	memcpy(&pcu->cmd_buf[IMS_PCU_INFO_SERIAL_OFFSET],
+	       pcu->serial_number, sizeof(pcu->serial_number));
+
+	error = ims_pcu_execute_command(pcu, SET_INFO,
+					&pcu->cmd_buf[IMS_PCU_DATA_OFFSET],
+					IMS_PCU_SET_INFO_SIZE);
+	if (error) {
+		dev_err(pcu->dev,
+			"Failed to update device information, error: %d\n",
+			error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int ims_pcu_switch_to_bootloader(struct ims_pcu *pcu)
+{
+	int error;
+
+	/* Execute jump to the bootoloader */
+	error = ims_pcu_execute_command(pcu, JUMP_TO_BTLDR, NULL, 0);
+	if (error) {
+		dev_err(pcu->dev,
+			"Failure when sending JUMP TO BOOLTLOADER command, error: %d\n",
+			error);
+		return error;
+	}
+
+	return 0;
+}
+
+/*********************************************************************
+ *             Firmware Update handling                              *
+ *********************************************************************/
+
+#define IMS_PCU_FIRMWARE_NAME	"imspcu.fw"
+
+struct ims_pcu_flash_fmt {
+	__le32 addr;
+	u8 len;
+	u8 data[];
+};
+
+static unsigned int ims_pcu_count_fw_records(const struct firmware *fw)
+{
+	const struct ihex_binrec *rec = (const struct ihex_binrec *)fw->data;
+	unsigned int count = 0;
+
+	while (rec) {
+		count++;
+		rec = ihex_next_binrec(rec);
+	}
+
+	return count;
+}
+
+static int ims_pcu_verify_block(struct ims_pcu *pcu,
+				u32 addr, u8 len, const u8 *data)
+{
+	struct ims_pcu_flash_fmt *fragment;
+	int error;
+
+	fragment = (void *)&pcu->cmd_buf[1];
+	put_unaligned_le32(addr, &fragment->addr);
+	fragment->len = len;
+
+	error = ims_pcu_execute_bl_command(pcu, READ_APP, NULL, 5,
+					IMS_PCU_CMD_RESPONSE_TIMEOUT);
+	if (error) {
+		dev_err(pcu->dev,
+			"Failed to retrieve block at 0x%08x, len %d, error: %d\n",
+			addr, len, error);
+		return error;
+	}
+
+	fragment = (void *)&pcu->cmd_buf[IMS_PCU_BL_DATA_OFFSET];
+	if (get_unaligned_le32(&fragment->addr) != addr ||
+	    fragment->len != len) {
+		dev_err(pcu->dev,
+			"Wrong block when retrieving 0x%08x (0x%08x), len %d (%d)\n",
+			addr, get_unaligned_le32(&fragment->addr),
+			len, fragment->len);
+		return -EINVAL;
+	}
+
+	if (memcmp(fragment->data, data, len)) {
+		dev_err(pcu->dev,
+			"Mismatch in block at 0x%08x, len %d\n",
+			addr, len);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ims_pcu_flash_firmware(struct ims_pcu *pcu,
+				  const struct firmware *fw,
+				  unsigned int n_fw_records)
+{
+	const struct ihex_binrec *rec = (const struct ihex_binrec *)fw->data;
+	struct ims_pcu_flash_fmt *fragment;
+	unsigned int count = 0;
+	u32 addr;
+	u8 len;
+	int error;
+
+	error = ims_pcu_execute_bl_command(pcu, ERASE_APP, NULL, 0, 2000);
+	if (error) {
+		dev_err(pcu->dev,
+			"Failed to erase application image, error: %d\n",
+			error);
+		return error;
+	}
+
+	while (rec) {
+		/*
+		 * The firmware format is messed up for some reason.
+		 * The address twice that of what is needed for some
+		 * reason and we end up overwriting half of the data
+		 * with the next record.
+		 */
+		addr = be32_to_cpu(rec->addr) / 2;
+		len = be16_to_cpu(rec->len);
+
+		fragment = (void *)&pcu->cmd_buf[1];
+		put_unaligned_le32(addr, &fragment->addr);
+		fragment->len = len;
+		memcpy(fragment->data, rec->data, len);
+
+		error = ims_pcu_execute_bl_command(pcu, PROGRAM_DEVICE,
+						NULL, len + 5,
+						IMS_PCU_CMD_RESPONSE_TIMEOUT);
+		if (error) {
+			dev_err(pcu->dev,
+				"Failed to write block at 0x%08x, len %d, error: %d\n",
+				addr, len, error);
+			return error;
+		}
+
+		if (addr >= pcu->fw_start_addr && addr < pcu->fw_end_addr) {
+			error = ims_pcu_verify_block(pcu, addr, len, rec->data);
+			if (error)
+				return error;
+		}
+
+		count++;
+		pcu->update_firmware_status = (count * 100) / n_fw_records;
+
+		rec = ihex_next_binrec(rec);
+	}
+
+	error = ims_pcu_execute_bl_command(pcu, PROGRAM_COMPLETE,
+					    NULL, 0, 2000);
+	if (error)
+		dev_err(pcu->dev,
+			"Failed to send PROGRAM_COMPLETE, error: %d\n",
+			error);
+
+	return 0;
+}
+
+static int ims_pcu_handle_firmware_update(struct ims_pcu *pcu,
+					  const struct firmware *fw)
+{
+	unsigned int n_fw_records;
+	int retval;
+
+	dev_info(pcu->dev, "Updating firmware %s, size: %zu\n",
+		 IMS_PCU_FIRMWARE_NAME, fw->size);
+
+	n_fw_records = ims_pcu_count_fw_records(fw);
+
+	retval = ims_pcu_flash_firmware(pcu, fw, n_fw_records);
+	if (retval)
+		goto out;
+
+	retval = ims_pcu_execute_bl_command(pcu, LAUNCH_APP, NULL, 0, 0);
+	if (retval)
+		dev_err(pcu->dev,
+			"Failed to start application image, error: %d\n",
+			retval);
+
+out:
+	pcu->update_firmware_status = retval;
+	sysfs_notify(&pcu->dev->kobj, NULL, "update_firmware_status");
+	return retval;
+}
+
+static void ims_pcu_process_async_firmware(const struct firmware *fw,
+					   void *context)
+{
+	struct ims_pcu *pcu = context;
+	int error;
+
+	if (!fw) {
+		dev_err(pcu->dev, "Failed to get firmware %s\n",
+			IMS_PCU_FIRMWARE_NAME);
+		goto out;
+	}
+
+	error = ihex_validate_fw(fw);
+	if (error) {
+		dev_err(pcu->dev, "Firmware %s is invalid\n",
+			IMS_PCU_FIRMWARE_NAME);
+		goto out;
+	}
+
+	mutex_lock(&pcu->cmd_mutex);
+	ims_pcu_handle_firmware_update(pcu, fw);
+	mutex_unlock(&pcu->cmd_mutex);
+
+	release_firmware(fw);
+
+out:
+	complete(&pcu->async_firmware_done);
+}
+
+/*********************************************************************
+ *             Backlight LED device support                          *
+ *********************************************************************/
+
+#define IMS_PCU_MAX_BRIGHTNESS		31998
+
+static void ims_pcu_backlight_work(struct work_struct *work)
+{
+	struct ims_pcu_backlight *backlight =
+			container_of(work, struct ims_pcu_backlight, work);
+	struct ims_pcu *pcu =
+			container_of(backlight, struct ims_pcu, backlight);
+	int desired_brightness = backlight->desired_brightness;
+	__le16 br_val = cpu_to_le16(desired_brightness);
+	int error;
+
+	mutex_lock(&pcu->cmd_mutex);
+
+	error = ims_pcu_execute_command(pcu, SET_BRIGHTNESS,
+					&br_val, sizeof(br_val));
+	if (error && error != -ENODEV)
+		dev_warn(pcu->dev,
+			 "Failed to set desired brightness %u, error: %d\n",
+			 desired_brightness, error);
+
+	mutex_unlock(&pcu->cmd_mutex);
+}
+
+static void ims_pcu_backlight_set_brightness(struct led_classdev *cdev,
+					     enum led_brightness value)
+{
+	struct ims_pcu_backlight *backlight =
+			container_of(cdev, struct ims_pcu_backlight, cdev);
+
+	backlight->desired_brightness = value;
+	schedule_work(&backlight->work);
+}
+
+static enum led_brightness
+ims_pcu_backlight_get_brightness(struct led_classdev *cdev)
+{
+	struct ims_pcu_backlight *backlight =
+			container_of(cdev, struct ims_pcu_backlight, cdev);
+	struct ims_pcu *pcu =
+			container_of(backlight, struct ims_pcu, backlight);
+	int brightness;
+	int error;
+
+	mutex_lock(&pcu->cmd_mutex);
+
+	error = ims_pcu_execute_query(pcu, GET_BRIGHTNESS);
+	if (error) {
+		dev_warn(pcu->dev,
+			 "Failed to get current brightness, error: %d\n",
+			 error);
+		/* Assume the LED is OFF */
+		brightness = LED_OFF;
+	} else {
+		brightness =
+			get_unaligned_le16(&pcu->cmd_buf[IMS_PCU_DATA_OFFSET]);
+	}
+
+	mutex_unlock(&pcu->cmd_mutex);
+
+	return brightness;
+}
+
+static int ims_pcu_setup_backlight(struct ims_pcu *pcu)
+{
+	struct ims_pcu_backlight *backlight = &pcu->backlight;
+	int error;
+
+	INIT_WORK(&backlight->work, ims_pcu_backlight_work);
+	snprintf(backlight->name, sizeof(backlight->name),
+		 "pcu%d::kbd_backlight", pcu->device_no);
+
+	backlight->cdev.name = backlight->name;
+	backlight->cdev.max_brightness = IMS_PCU_MAX_BRIGHTNESS;
+	backlight->cdev.brightness_get = ims_pcu_backlight_get_brightness;
+	backlight->cdev.brightness_set = ims_pcu_backlight_set_brightness;
+
+	error = led_classdev_register(pcu->dev, &backlight->cdev);
+	if (error) {
+		dev_err(pcu->dev,
+			"Failed to register backlight LED device, error: %d\n",
+			error);
+		return error;
+	}
+
+	return 0;
+}
+
+static void ims_pcu_destroy_backlight(struct ims_pcu *pcu)
+{
+	struct ims_pcu_backlight *backlight = &pcu->backlight;
+
+	led_classdev_unregister(&backlight->cdev);
+	cancel_work_sync(&backlight->work);
+}
+
+
+/*********************************************************************
+ *             Sysfs attributes handling                             *
+ *********************************************************************/
+
+struct ims_pcu_attribute {
+	struct device_attribute dattr;
+	size_t field_offset;
+	int field_length;
+};
+
+static ssize_t ims_pcu_attribute_show(struct device *dev,
+				      struct device_attribute *dattr,
+				      char *buf)
+{
+	struct usb_interface *intf = to_usb_interface(dev);
+	struct ims_pcu *pcu = usb_get_intfdata(intf);
+	struct ims_pcu_attribute *attr =
+			container_of(dattr, struct ims_pcu_attribute, dattr);
+	char *field = (char *)pcu + attr->field_offset;
+
+	return scnprintf(buf, PAGE_SIZE, "%.*s\n", attr->field_length, field);
+}
+
+static ssize_t ims_pcu_attribute_store(struct device *dev,
+				       struct device_attribute *dattr,
+				       const char *buf, size_t count)
+{
+
+	struct usb_interface *intf = to_usb_interface(dev);
+	struct ims_pcu *pcu = usb_get_intfdata(intf);
+	struct ims_pcu_attribute *attr =
+			container_of(dattr, struct ims_pcu_attribute, dattr);
+	char *field = (char *)pcu + attr->field_offset;
+	size_t data_len;
+	int error;
+
+	if (count > attr->field_length)
+		return -EINVAL;
+
+	data_len = strnlen(buf, attr->field_length);
+	if (data_len > attr->field_length)
+		return -EINVAL;
+
+	error = mutex_lock_interruptible(&pcu->cmd_mutex);
+	if (error)
+		return error;
+
+	memset(field, 0, attr->field_length);
+	memcpy(field, buf, data_len);
+
+	error = ims_pcu_set_info(pcu);
+
+	/*
+	 * Even if update failed, let's fetch the info again as we just
+	 * clobbered one of the fields.
+	 */
+	ims_pcu_get_info(pcu);
+
+	mutex_unlock(&pcu->cmd_mutex);
+
+	return error < 0 ? error : count;
+}
+
+#define IMS_PCU_ATTR(_field, _mode)					\
+struct ims_pcu_attribute ims_pcu_attr_##_field = {			\
+	.dattr = __ATTR(_field, _mode,					\
+			ims_pcu_attribute_show,				\
+			ims_pcu_attribute_store),			\
+	.field_offset = offsetof(struct ims_pcu, _field),		\
+	.field_length = sizeof(((struct ims_pcu *)NULL)->_field),	\
+}
+
+#define IMS_PCU_RO_ATTR(_field)						\
+		IMS_PCU_ATTR(_field, S_IRUGO)
+#define IMS_PCU_RW_ATTR(_field)						\
+		IMS_PCU_ATTR(_field, S_IRUGO | S_IWUSR)
+
+static IMS_PCU_RW_ATTR(part_number);
+static IMS_PCU_RW_ATTR(serial_number);
+static IMS_PCU_RW_ATTR(date_of_manufacturing);
+
+static IMS_PCU_RO_ATTR(fw_version);
+static IMS_PCU_RO_ATTR(bl_version);
+static IMS_PCU_RO_ATTR(reset_reason);
+
+static ssize_t ims_pcu_reset_device(struct device *dev,
+				    struct device_attribute *dattr,
+				    const char *buf, size_t count)
+{
+	static const u8 reset_byte = 1;
+	struct usb_interface *intf = to_usb_interface(dev);
+	struct ims_pcu *pcu = usb_get_intfdata(intf);
+	int value;
+	int error;
+
+	error = kstrtoint(buf, 0, &value);
+	if (error)
+		return error;
+
+	if (value != 1)
+		return -EINVAL;
+
+	dev_info(pcu->dev, "Attempting to reset device\n");
+
+	error = ims_pcu_execute_command(pcu, PCU_RESET, &reset_byte, 1);
+	if (error) {
+		dev_info(pcu->dev,
+			 "Failed to reset device, error: %d\n",
+			 error);
+		return error;
+	}
+
+	return count;
+}
+
+static DEVICE_ATTR(reset_device, S_IWUSR, NULL, ims_pcu_reset_device);
+
+static ssize_t ims_pcu_update_firmware_store(struct device *dev,
+					     struct device_attribute *dattr,
+					     const char *buf, size_t count)
+{
+	struct usb_interface *intf = to_usb_interface(dev);
+	struct ims_pcu *pcu = usb_get_intfdata(intf);
+	const struct firmware *fw = NULL;
+	int value;
+	int error;
+
+	error = kstrtoint(buf, 0, &value);
+	if (error)
+		return error;
+
+	if (value != 1)
+		return -EINVAL;
+
+	error = mutex_lock_interruptible(&pcu->cmd_mutex);
+	if (error)
+		return error;
+
+	error = request_ihex_firmware(&fw, IMS_PCU_FIRMWARE_NAME, pcu->dev);
+	if (error) {
+		dev_err(pcu->dev, "Failed to request firmware %s, error: %d\n",
+			IMS_PCU_FIRMWARE_NAME, error);
+		goto out;
+	}
+
+	/*
+	 * If we are already in bootloader mode we can proceed with
+	 * flashing the firmware.
+	 *
+	 * If we are in application mode, then we need to switch into
+	 * bootloader mode, which will cause the device to disconnect
+	 * and reconnect as different device.
+	 */
+	if (pcu->bootloader_mode)
+		error = ims_pcu_handle_firmware_update(pcu, fw);
+	else
+		error = ims_pcu_switch_to_bootloader(pcu);
+
+	release_firmware(fw);
+
+out:
+	mutex_unlock(&pcu->cmd_mutex);
+	return error ?: count;
+}
+
+static DEVICE_ATTR(update_firmware, S_IWUSR,
+		   NULL, ims_pcu_update_firmware_store);
+
+static ssize_t
+ims_pcu_update_firmware_status_show(struct device *dev,
+				    struct device_attribute *dattr,
+				    char *buf)
+{
+	struct usb_interface *intf = to_usb_interface(dev);
+	struct ims_pcu *pcu = usb_get_intfdata(intf);
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", pcu->update_firmware_status);
+}
+
+static DEVICE_ATTR(update_firmware_status, S_IRUGO,
+		   ims_pcu_update_firmware_status_show, NULL);
+
+static struct attribute *ims_pcu_attrs[] = {
+	&ims_pcu_attr_part_number.dattr.attr,
+	&ims_pcu_attr_serial_number.dattr.attr,
+	&ims_pcu_attr_date_of_manufacturing.dattr.attr,
+	&ims_pcu_attr_fw_version.dattr.attr,
+	&ims_pcu_attr_bl_version.dattr.attr,
+	&ims_pcu_attr_reset_reason.dattr.attr,
+	&dev_attr_reset_device.attr,
+	&dev_attr_update_firmware.attr,
+	&dev_attr_update_firmware_status.attr,
+	NULL
+};
+
+static umode_t ims_pcu_is_attr_visible(struct kobject *kobj,
+				       struct attribute *attr, int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct usb_interface *intf = to_usb_interface(dev);
+	struct ims_pcu *pcu = usb_get_intfdata(intf);
+	umode_t mode = attr->mode;
+
+	if (pcu->bootloader_mode) {
+		if (attr != &dev_attr_update_firmware_status.attr &&
+		    attr != &dev_attr_update_firmware.attr &&
+		    attr != &dev_attr_reset_device.attr) {
+			mode = 0;
+		}
+	} else {
+		if (attr == &dev_attr_update_firmware_status.attr)
+			mode = 0;
+	}
+
+	return mode;
+}
+
+static struct attribute_group ims_pcu_attr_group = {
+	.is_visible	= ims_pcu_is_attr_visible,
+	.attrs		= ims_pcu_attrs,
+};
+
+static void ims_pcu_irq(struct urb *urb)
+{
+	struct ims_pcu *pcu = urb->context;
+	int retval, status;
+
+	status = urb->status;
+
+	switch (status) {
+	case 0:
+		/* success */
+		break;
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+		/* this urb is terminated, clean up */
+		dev_dbg(pcu->dev, "%s - urb shutting down with status: %d\n",
+			__func__, status);
+		return;
+	default:
+		dev_dbg(pcu->dev, "%s - nonzero urb status received: %d\n",
+			__func__, status);
+		goto exit;
+	}
+
+	dev_dbg(pcu->dev, "%s: received %d: %*ph\n", __func__,
+		urb->actual_length, urb->actual_length, pcu->urb_in_buf);
+
+	if (urb == pcu->urb_in)
+		ims_pcu_process_data(pcu, urb);
+
+exit:
+	retval = usb_submit_urb(urb, GFP_ATOMIC);
+	if (retval && retval != -ENODEV)
+		dev_err(pcu->dev, "%s - usb_submit_urb failed with result %d\n",
+			__func__, retval);
+}
+
+static int ims_pcu_buffers_alloc(struct ims_pcu *pcu)
+{
+	int error;
+
+	pcu->urb_in_buf = usb_alloc_coherent(pcu->udev, pcu->max_in_size,
+					     GFP_KERNEL, &pcu->read_dma);
+	if (!pcu->urb_in_buf) {
+		dev_err(pcu->dev,
+			"Failed to allocate memory for read buffer\n");
+		return -ENOMEM;
+	}
+
+	pcu->urb_in = usb_alloc_urb(0, GFP_KERNEL);
+	if (!pcu->urb_in) {
+		dev_err(pcu->dev, "Failed to allocate input URB\n");
+		error = -ENOMEM;
+		goto err_free_urb_in_buf;
+	}
+
+	pcu->urb_in->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+	pcu->urb_in->transfer_dma = pcu->read_dma;
+
+	usb_fill_bulk_urb(pcu->urb_in, pcu->udev,
+			  usb_rcvbulkpipe(pcu->udev,
+					  pcu->ep_in->bEndpointAddress),
+			  pcu->urb_in_buf, pcu->max_in_size,
+			  ims_pcu_irq, pcu);
+
+	/*
+	 * We are using usb_bulk_msg() for sending so there is no point
+	 * in allocating memory with usb_alloc_coherent().
+	 */
+	pcu->urb_out_buf = kmalloc(pcu->max_out_size, GFP_KERNEL);
+	if (!pcu->urb_out_buf) {
+		dev_err(pcu->dev, "Failed to allocate memory for write buffer\n");
+		error = -ENOMEM;
+		goto err_free_in_urb;
+	}
+
+	pcu->urb_ctrl_buf = usb_alloc_coherent(pcu->udev, pcu->max_ctrl_size,
+					       GFP_KERNEL, &pcu->ctrl_dma);
+	if (!pcu->urb_ctrl_buf) {
+		dev_err(pcu->dev,
+			"Failed to allocate memory for read buffer\n");
+		goto err_free_urb_out_buf;
+	}
+
+	pcu->urb_ctrl = usb_alloc_urb(0, GFP_KERNEL);
+	if (!pcu->urb_ctrl) {
+		dev_err(pcu->dev, "Failed to allocate input URB\n");
+		error = -ENOMEM;
+		goto err_free_urb_ctrl_buf;
+	}
+
+	pcu->urb_ctrl->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+	pcu->urb_ctrl->transfer_dma = pcu->ctrl_dma;
+
+	usb_fill_int_urb(pcu->urb_ctrl, pcu->udev,
+			  usb_rcvintpipe(pcu->udev,
+					 pcu->ep_ctrl->bEndpointAddress),
+			  pcu->urb_ctrl_buf, pcu->max_ctrl_size,
+			  ims_pcu_irq, pcu, pcu->ep_ctrl->bInterval);
+
+	return 0;
+
+err_free_urb_ctrl_buf:
+	usb_free_coherent(pcu->udev, pcu->max_ctrl_size,
+			  pcu->urb_ctrl_buf, pcu->ctrl_dma);
+err_free_urb_out_buf:
+	kfree(pcu->urb_out_buf);
+err_free_in_urb:
+	usb_free_urb(pcu->urb_in);
+err_free_urb_in_buf:
+	usb_free_coherent(pcu->udev, pcu->max_in_size,
+			  pcu->urb_in_buf, pcu->read_dma);
+	return error;
+}
+
+static void ims_pcu_buffers_free(struct ims_pcu *pcu)
+{
+	usb_kill_urb(pcu->urb_in);
+	usb_free_urb(pcu->urb_in);
+
+	usb_free_coherent(pcu->udev, pcu->max_out_size,
+			  pcu->urb_in_buf, pcu->read_dma);
+
+	kfree(pcu->urb_out_buf);
+
+	usb_kill_urb(pcu->urb_ctrl);
+	usb_free_urb(pcu->urb_ctrl);
+
+	usb_free_coherent(pcu->udev, pcu->max_ctrl_size,
+			  pcu->urb_ctrl_buf, pcu->ctrl_dma);
+}
+
+static const struct usb_cdc_union_desc *
+ims_pcu_get_cdc_union_desc(struct usb_interface *intf)
+{
+	const void *buf = intf->altsetting->extra;
+	size_t buflen = intf->altsetting->extralen;
+	struct usb_cdc_union_desc *union_desc;
+
+	if (!buf) {
+		dev_err(&intf->dev, "Missing descriptor data\n");
+		return NULL;
+	}
+
+	if (!buflen) {
+		dev_err(&intf->dev, "Zero length descriptor\n");
+		return NULL;
+	}
+
+	while (buflen > 0) {
+		union_desc = (struct usb_cdc_union_desc *)buf;
+
+		if (union_desc->bDescriptorType == USB_DT_CS_INTERFACE &&
+		    union_desc->bDescriptorSubType == USB_CDC_UNION_TYPE) {
+			dev_dbg(&intf->dev, "Found union header\n");
+			return union_desc;
+		}
+
+		buflen -= union_desc->bLength;
+		buf += union_desc->bLength;
+	}
+
+	dev_err(&intf->dev, "Missing CDC union descriptor\n");
+	return NULL;
+}
+
+static int ims_pcu_parse_cdc_data(struct usb_interface *intf, struct ims_pcu *pcu)
+{
+	const struct usb_cdc_union_desc *union_desc;
+	struct usb_host_interface *alt;
+
+	union_desc = ims_pcu_get_cdc_union_desc(intf);
+	if (!union_desc)
+		return -EINVAL;
+
+	pcu->ctrl_intf = usb_ifnum_to_if(pcu->udev,
+					 union_desc->bMasterInterface0);
+
+	alt = pcu->ctrl_intf->cur_altsetting;
+	pcu->ep_ctrl = &alt->endpoint[0].desc;
+	pcu->max_ctrl_size = usb_endpoint_maxp(pcu->ep_ctrl);
+
+	pcu->data_intf = usb_ifnum_to_if(pcu->udev,
+					 union_desc->bSlaveInterface0);
+
+	alt = pcu->data_intf->cur_altsetting;
+	if (alt->desc.bNumEndpoints != 2) {
+		dev_err(pcu->dev,
+			"Incorrect number of endpoints on data interface (%d)\n",
+			alt->desc.bNumEndpoints);
+		return -EINVAL;
+	}
+
+	pcu->ep_out = &alt->endpoint[0].desc;
+	if (!usb_endpoint_is_bulk_out(pcu->ep_out)) {
+		dev_err(pcu->dev,
+			"First endpoint on data interface is not BULK OUT\n");
+		return -EINVAL;
+	}
+
+	pcu->max_out_size = usb_endpoint_maxp(pcu->ep_out);
+	if (pcu->max_out_size < 8) {
+		dev_err(pcu->dev,
+			"Max OUT packet size is too small (%zd)\n",
+			pcu->max_out_size);
+		return -EINVAL;
+	}
+
+	pcu->ep_in = &alt->endpoint[1].desc;
+	if (!usb_endpoint_is_bulk_in(pcu->ep_in)) {
+		dev_err(pcu->dev,
+			"Second endpoint on data interface is not BULK IN\n");
+		return -EINVAL;
+	}
+
+	pcu->max_in_size = usb_endpoint_maxp(pcu->ep_in);
+	if (pcu->max_in_size < 8) {
+		dev_err(pcu->dev,
+			"Max IN packet size is too small (%zd)\n",
+			pcu->max_in_size);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ims_pcu_start_io(struct ims_pcu *pcu)
+{
+	int error;
+
+	error = usb_submit_urb(pcu->urb_ctrl, GFP_KERNEL);
+	if (error) {
+		dev_err(pcu->dev,
+			"Failed to start control IO - usb_submit_urb failed with result: %d\n",
+			error);
+		return -EIO;
+	}
+
+	error = usb_submit_urb(pcu->urb_in, GFP_KERNEL);
+	if (error) {
+		dev_err(pcu->dev,
+			"Failed to start IO - usb_submit_urb failed with result: %d\n",
+			error);
+		usb_kill_urb(pcu->urb_ctrl);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static void ims_pcu_stop_io(struct ims_pcu *pcu)
+{
+	usb_kill_urb(pcu->urb_in);
+	usb_kill_urb(pcu->urb_ctrl);
+}
+
+static int ims_pcu_line_setup(struct ims_pcu *pcu)
+{
+	struct usb_host_interface *interface = pcu->ctrl_intf->cur_altsetting;
+	struct usb_cdc_line_coding *line = (void *)pcu->cmd_buf;
+	int error;
+
+	memset(line, 0, sizeof(*line));
+	line->dwDTERate = cpu_to_le32(57600);
+	line->bDataBits = 8;
+
+	error = usb_control_msg(pcu->udev, usb_sndctrlpipe(pcu->udev, 0),
+				USB_CDC_REQ_SET_LINE_CODING,
+				USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+				0, interface->desc.bInterfaceNumber,
+				line, sizeof(struct usb_cdc_line_coding),
+				5000);
+	if (error < 0) {
+		dev_err(pcu->dev, "Failed to set line coding, error: %d\n",
+			error);
+		return error;
+	}
+
+	error = usb_control_msg(pcu->udev, usb_sndctrlpipe(pcu->udev, 0),
+				USB_CDC_REQ_SET_CONTROL_LINE_STATE,
+				USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+				0x03, interface->desc.bInterfaceNumber,
+				NULL, 0, 5000);
+	if (error < 0) {
+		dev_err(pcu->dev, "Failed to set line state, error: %d\n",
+			error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int ims_pcu_get_device_info(struct ims_pcu *pcu)
+{
+	int error;
+
+	error = ims_pcu_get_info(pcu);
+	if (error)
+		return error;
+
+	error = ims_pcu_execute_query(pcu, GET_FW_VERSION);
+	if (error) {
+		dev_err(pcu->dev,
+			"GET_FW_VERSION command failed, error: %d\n", error);
+		return error;
+	}
+
+	snprintf(pcu->fw_version, sizeof(pcu->fw_version),
+		 "%02d%02d%02d%02d.%c%c",
+		 pcu->cmd_buf[2], pcu->cmd_buf[3], pcu->cmd_buf[4], pcu->cmd_buf[5],
+		 pcu->cmd_buf[6], pcu->cmd_buf[7]);
+
+	error = ims_pcu_execute_query(pcu, GET_BL_VERSION);
+	if (error) {
+		dev_err(pcu->dev,
+			"GET_BL_VERSION command failed, error: %d\n", error);
+		return error;
+	}
+
+	snprintf(pcu->bl_version, sizeof(pcu->bl_version),
+		 "%02d%02d%02d%02d.%c%c",
+		 pcu->cmd_buf[2], pcu->cmd_buf[3], pcu->cmd_buf[4], pcu->cmd_buf[5],
+		 pcu->cmd_buf[6], pcu->cmd_buf[7]);
+
+	error = ims_pcu_execute_query(pcu, RESET_REASON);
+	if (error) {
+		dev_err(pcu->dev,
+			"RESET_REASON command failed, error: %d\n", error);
+		return error;
+	}
+
+	snprintf(pcu->reset_reason, sizeof(pcu->reset_reason),
+		 "%02x", pcu->cmd_buf[IMS_PCU_DATA_OFFSET]);
+
+	dev_dbg(pcu->dev,
+		"P/N: %s, MD: %s, S/N: %s, FW: %s, BL: %s, RR: %s\n",
+		pcu->part_number,
+		pcu->date_of_manufacturing,
+		pcu->serial_number,
+		pcu->fw_version,
+		pcu->bl_version,
+		pcu->reset_reason);
+
+	return 0;
+}
+
+static int ims_pcu_identify_type(struct ims_pcu *pcu, u8 *device_id)
+{
+	int error;
+
+	error = ims_pcu_execute_query(pcu, GET_DEVICE_ID);
+	if (error) {
+		dev_err(pcu->dev,
+			"GET_DEVICE_ID command failed, error: %d\n", error);
+		return error;
+	}
+
+	*device_id = pcu->cmd_buf[IMS_PCU_DATA_OFFSET];
+	dev_dbg(pcu->dev, "Detected device ID: %d\n", *device_id);
+
+	return 0;
+}
+
+static int ims_pcu_init_application_mode(struct ims_pcu *pcu)
+{
+	static atomic_t device_no = ATOMIC_INIT(0);
+
+	const struct ims_pcu_device_info *info;
+	u8 device_id;
+	int error;
+
+	error = ims_pcu_get_device_info(pcu);
+	if (error) {
+		/* Device does not respond to basic queries, hopeless */
+		return error;
+	}
+
+	error = ims_pcu_identify_type(pcu, &device_id);
+	if (error) {
+		dev_err(pcu->dev,
+			"Failed to identify device, error: %d\n", error);
+		/*
+		 * Do not signal error, but do not create input nor
+		 * backlight devices either, let userspace figure this
+		 * out (flash a new firmware?).
+		 */
+		return 0;
+	}
+
+	if (device_id >= ARRAY_SIZE(ims_pcu_device_info) ||
+	    !ims_pcu_device_info[device_id].keymap) {
+		dev_err(pcu->dev, "Device ID %d is not valid\n", device_id);
+		/* Same as above, punt to userspace */
+		return 0;
+	}
+
+	/* Device appears to be operable, complete initialization */
+	pcu->device_no = atomic_inc_return(&device_no) - 1;
+
+	error = ims_pcu_setup_backlight(pcu);
+	if (error)
+		return error;
+
+	info = &ims_pcu_device_info[device_id];
+	error = ims_pcu_setup_buttons(pcu, info->keymap, info->keymap_len);
+	if (error)
+		goto err_destroy_backlight;
+
+	if (info->has_gamepad) {
+		error = ims_pcu_setup_gamepad(pcu);
+		if (error)
+			goto err_destroy_buttons;
+	}
+
+	pcu->setup_complete = true;
+
+	return 0;
+
+err_destroy_backlight:
+	ims_pcu_destroy_backlight(pcu);
+err_destroy_buttons:
+	ims_pcu_destroy_buttons(pcu);
+	return error;
+}
+
+static void ims_pcu_destroy_application_mode(struct ims_pcu *pcu)
+{
+	if (pcu->setup_complete) {
+		pcu->setup_complete = false;
+		mb(); /* make sure flag setting is not reordered */
+
+		if (pcu->gamepad)
+			ims_pcu_destroy_gamepad(pcu);
+		ims_pcu_destroy_buttons(pcu);
+		ims_pcu_destroy_backlight(pcu);
+	}
+}
+
+static int ims_pcu_init_bootloader_mode(struct ims_pcu *pcu)
+{
+	int error;
+
+	error = ims_pcu_execute_bl_command(pcu, QUERY_DEVICE, NULL, 0,
+					   IMS_PCU_CMD_RESPONSE_TIMEOUT);
+	if (error) {
+		dev_err(pcu->dev, "Bootloader does not respond, aborting\n");
+		return error;
+	}
+
+	pcu->fw_start_addr =
+		get_unaligned_le32(&pcu->cmd_buf[IMS_PCU_DATA_OFFSET + 11]);
+	pcu->fw_end_addr =
+		get_unaligned_le32(&pcu->cmd_buf[IMS_PCU_DATA_OFFSET + 15]);
+
+	dev_info(pcu->dev,
+		 "Device is in bootloader mode (addr 0x%08x-0x%08x), requesting firmware\n",
+		 pcu->fw_start_addr, pcu->fw_end_addr);
+
+	error = request_firmware_nowait(THIS_MODULE, true,
+					IMS_PCU_FIRMWARE_NAME,
+					pcu->dev, GFP_KERNEL, pcu,
+					ims_pcu_process_async_firmware);
+	if (error) {
+		/* This error is not fatal, let userspace have another chance */
+		complete(&pcu->async_firmware_done);
+	}
+
+	return 0;
+}
+
+static void ims_pcu_destroy_bootloader_mode(struct ims_pcu *pcu)
+{
+	/* Make sure our initial firmware request has completed */
+	wait_for_completion(&pcu->async_firmware_done);
+}
+
+#define IMS_PCU_APPLICATION_MODE	0
+#define IMS_PCU_BOOTLOADER_MODE		1
+
+static struct usb_driver ims_pcu_driver;
+
+static int ims_pcu_probe(struct usb_interface *intf,
+			 const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct ims_pcu *pcu;
+	int error;
+
+	pcu = kzalloc(sizeof(struct ims_pcu), GFP_KERNEL);
+	if (!pcu)
+		return -ENOMEM;
+
+	pcu->dev = &intf->dev;
+	pcu->udev = udev;
+	pcu->bootloader_mode = id->driver_info == IMS_PCU_BOOTLOADER_MODE;
+	mutex_init(&pcu->cmd_mutex);
+	init_completion(&pcu->cmd_done);
+	init_completion(&pcu->async_firmware_done);
+
+	error = ims_pcu_parse_cdc_data(intf, pcu);
+	if (error)
+		goto err_free_mem;
+
+	error = usb_driver_claim_interface(&ims_pcu_driver,
+					   pcu->data_intf, pcu);
+	if (error) {
+		dev_err(&intf->dev,
+			"Unable to claim corresponding data interface: %d\n",
+			error);
+		goto err_free_mem;
+	}
+
+	usb_set_intfdata(pcu->ctrl_intf, pcu);
+	usb_set_intfdata(pcu->data_intf, pcu);
+
+	error = ims_pcu_buffers_alloc(pcu);
+	if (error)
+		goto err_unclaim_intf;
+
+	error = ims_pcu_start_io(pcu);
+	if (error)
+		goto err_free_buffers;
+
+	error = ims_pcu_line_setup(pcu);
+	if (error)
+		goto err_stop_io;
+
+	error = sysfs_create_group(&intf->dev.kobj, &ims_pcu_attr_group);
+	if (error)
+		goto err_stop_io;
+
+	error = pcu->bootloader_mode ?
+			ims_pcu_init_bootloader_mode(pcu) :
+			ims_pcu_init_application_mode(pcu);
+	if (error)
+		goto err_remove_sysfs;
+
+	return 0;
+
+err_remove_sysfs:
+	sysfs_remove_group(&intf->dev.kobj, &ims_pcu_attr_group);
+err_stop_io:
+	ims_pcu_stop_io(pcu);
+err_free_buffers:
+	ims_pcu_buffers_free(pcu);
+err_unclaim_intf:
+	usb_driver_release_interface(&ims_pcu_driver, pcu->data_intf);
+err_free_mem:
+	kfree(pcu);
+	return error;
+}
+
+static void ims_pcu_disconnect(struct usb_interface *intf)
+{
+	struct ims_pcu *pcu = usb_get_intfdata(intf);
+	struct usb_host_interface *alt = intf->cur_altsetting;
+
+	usb_set_intfdata(intf, NULL);
+
+	/*
+	 * See if we are dealing with control or data interface. The cleanup
+	 * happens when we unbind primary (control) interface.
+	 */
+	if (alt->desc.bInterfaceClass != USB_CLASS_COMM)
+		return;
+
+	sysfs_remove_group(&intf->dev.kobj, &ims_pcu_attr_group);
+
+	ims_pcu_stop_io(pcu);
+
+	if (pcu->bootloader_mode)
+		ims_pcu_destroy_bootloader_mode(pcu);
+	else
+		ims_pcu_destroy_application_mode(pcu);
+
+	ims_pcu_buffers_free(pcu);
+	kfree(pcu);
+}
+
+#ifdef CONFIG_PM
+static int ims_pcu_suspend(struct usb_interface *intf,
+			   pm_message_t message)
+{
+	struct ims_pcu *pcu = usb_get_intfdata(intf);
+	struct usb_host_interface *alt = intf->cur_altsetting;
+
+	if (alt->desc.bInterfaceClass == USB_CLASS_COMM)
+		ims_pcu_stop_io(pcu);
+
+	return 0;
+}
+
+static int ims_pcu_resume(struct usb_interface *intf)
+{
+	struct ims_pcu *pcu = usb_get_intfdata(intf);
+	struct usb_host_interface *alt = intf->cur_altsetting;
+	int retval = 0;
+
+	if (alt->desc.bInterfaceClass == USB_CLASS_COMM) {
+		retval = ims_pcu_start_io(pcu);
+		if (retval == 0)
+			retval = ims_pcu_line_setup(pcu);
+	}
+
+	return retval;
+}
+#endif
+
+static const struct usb_device_id ims_pcu_id_table[] = {
+	{
+		USB_DEVICE_AND_INTERFACE_INFO(0x04d8, 0x0082,
+					USB_CLASS_COMM,
+					USB_CDC_SUBCLASS_ACM,
+					USB_CDC_ACM_PROTO_AT_V25TER),
+		.driver_info = IMS_PCU_APPLICATION_MODE,
+	},
+	{
+		USB_DEVICE_AND_INTERFACE_INFO(0x04d8, 0x0083,
+					USB_CLASS_COMM,
+					USB_CDC_SUBCLASS_ACM,
+					USB_CDC_ACM_PROTO_AT_V25TER),
+		.driver_info = IMS_PCU_BOOTLOADER_MODE,
+	},
+	{ }
+};
+
+static struct usb_driver ims_pcu_driver = {
+	.name			= "ims_pcu",
+	.id_table		= ims_pcu_id_table,
+	.probe			= ims_pcu_probe,
+	.disconnect		= ims_pcu_disconnect,
+#ifdef CONFIG_PM
+	.suspend		= ims_pcu_suspend,
+	.resume			= ims_pcu_resume,
+	.reset_resume		= ims_pcu_resume,
+#endif
+};
+
+module_usb_driver(ims_pcu_driver);
+
+MODULE_DESCRIPTION("IMS Passenger Control Unit driver");
+MODULE_AUTHOR("Dmitry Torokhov <dmitry.torokhov@gmail.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/misc/mma8450.c b/drivers/input/misc/mma8450.c
index 480557f14f23..f3309696d053 100644
--- a/drivers/input/misc/mma8450.c
+++ b/drivers/input/misc/mma8450.c
@@ -123,9 +123,9 @@ static void mma8450_poll(struct input_polled_dev *dev)
 	if (ret < 0)
 		return;
 
-	x = ((buf[1] << 4) & 0xff0) | (buf[0] & 0xf);
-	y = ((buf[3] << 4) & 0xff0) | (buf[2] & 0xf);
-	z = ((buf[5] << 4) & 0xff0) | (buf[4] & 0xf);
+	x = ((int)(s8)buf[1] << 4) | (buf[0] & 0xf);
+	y = ((int)(s8)buf[3] << 4) | (buf[2] & 0xf);
+	z = ((int)(s8)buf[5] << 4) | (buf[4] & 0xf);
 
 	input_report_abs(dev->input, ABS_X, x);
 	input_report_abs(dev->input, ABS_Y, y);
diff --git a/drivers/input/misc/twl4030-pwrbutton.c b/drivers/input/misc/twl4030-pwrbutton.c
index 27c2bc8aa890..b9a05fda03e4 100644
--- a/drivers/input/misc/twl4030-pwrbutton.c
+++ b/drivers/input/misc/twl4030-pwrbutton.c
@@ -114,18 +114,8 @@ static struct platform_driver twl4030_pwrbutton_driver = {
 	},
 };
 
-static int __init twl4030_pwrbutton_init(void)
-{
-	return platform_driver_probe(&twl4030_pwrbutton_driver,
+module_platform_driver_probe(twl4030_pwrbutton_driver,
 			twl4030_pwrbutton_probe);
-}
-module_init(twl4030_pwrbutton_init);
-
-static void __exit twl4030_pwrbutton_exit(void)
-{
-	platform_driver_unregister(&twl4030_pwrbutton_driver);
-}
-module_exit(twl4030_pwrbutton_exit);
 
 MODULE_ALIAS("platform:twl4030_pwrbutton");
 MODULE_DESCRIPTION("Triton2 Power Button");
diff --git a/drivers/input/mouse/Kconfig b/drivers/input/mouse/Kconfig
index 802bd6a72d73..effa9c5f2c5c 100644
--- a/drivers/input/mouse/Kconfig
+++ b/drivers/input/mouse/Kconfig
@@ -295,7 +295,7 @@ config MOUSE_VSXXXAA
 
 config MOUSE_GPIO
 	tristate "GPIO mouse"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select INPUT_POLLDEV
 	help
 	  This driver simulates a mouse on GPIO lines of various CPUs (and some
diff --git a/drivers/input/mouse/alps.c b/drivers/input/mouse/alps.c
index 0238e0e14335..7c5d72a6a26a 100644
--- a/drivers/input/mouse/alps.c
+++ b/drivers/input/mouse/alps.c
@@ -1013,8 +1013,8 @@ static int alps_rpt_cmd(struct psmouse *psmouse, int init_command,
 	if (ps2_command(ps2dev, param, PSMOUSE_CMD_GETINFO))
 		return -EIO;
 
-	psmouse_dbg(psmouse, "%2.2X report: %2.2x %2.2x %2.2x\n",
-		    repeated_command, param[0], param[1], param[2]);
+	psmouse_dbg(psmouse, "%2.2X report: %3ph\n",
+		    repeated_command, param);
 	return 0;
 }
 
@@ -1274,9 +1274,7 @@ static int alps_setup_trackstick_v3(struct psmouse *psmouse, int reg_base)
 		psmouse_warn(psmouse, "trackstick E7 report failed\n");
 		ret = -ENODEV;
 	} else {
-		psmouse_dbg(psmouse,
-			    "trackstick E7 report: %2.2x %2.2x %2.2x\n",
-			    param[0], param[1], param[2]);
+		psmouse_dbg(psmouse, "trackstick E7 report: %3ph\n", param);
 
 		/*
 		 * Not sure what this does, but it is absolutely
@@ -1323,6 +1321,7 @@ static int alps_hw_init_v3(struct psmouse *psmouse)
 	reg_val = alps_probe_trackstick_v3(psmouse, ALPS_REG_BASE_PINNACLE);
 	if (reg_val == -EIO)
 		goto error;
+
 	if (reg_val == 0 &&
 	    alps_setup_trackstick_v3(psmouse, ALPS_REG_BASE_PINNACLE) == -EIO)
 		goto error;
@@ -1676,8 +1675,7 @@ static int alps_identify(struct psmouse *psmouse, struct alps_data *priv)
 	}
 
 	psmouse_info(psmouse,
-		"Unknown ALPS touchpad: E7=%2.2x %2.2x %2.2x, EC=%2.2x %2.2x %2.2x\n",
-		e7[0], e7[1], e7[2], ec[0], ec[1], ec[2]);
+		     "Unknown ALPS touchpad: E7=%3ph, EC=%3ph\n", e7, ec);
 
 	return -EINVAL;
 }
diff --git a/drivers/input/mouse/amimouse.c b/drivers/input/mouse/amimouse.c
index 5fa99341a39d..b55d5af217a7 100644
--- a/drivers/input/mouse/amimouse.c
+++ b/drivers/input/mouse/amimouse.c
@@ -146,18 +146,6 @@ static struct platform_driver amimouse_driver = {
 	},
 };
 
-static int __init amimouse_init(void)
-{
-	return platform_driver_probe(&amimouse_driver, amimouse_probe);
-}
-
-module_init(amimouse_init);
-
-static void __exit amimouse_exit(void)
-{
-	platform_driver_unregister(&amimouse_driver);
-}
-
-module_exit(amimouse_exit);
+module_platform_driver_probe(amimouse_driver, amimouse_probe);
 
 MODULE_ALIAS("platform:amiga-mouse");
diff --git a/drivers/input/mouse/trackpoint.c b/drivers/input/mouse/trackpoint.c
index f3102494237d..ca843b6cf6bd 100644
--- a/drivers/input/mouse/trackpoint.c
+++ b/drivers/input/mouse/trackpoint.c
@@ -20,9 +20,34 @@
 #include "trackpoint.h"
 
 /*
+ * Power-on Reset: Resets all trackpoint parameters, including RAM values,
+ * to defaults.
+ * Returns zero on success, non-zero on failure.
+ */
+static int trackpoint_power_on_reset(struct ps2dev *ps2dev)
+{
+	unsigned char results[2];
+	int tries = 0;
+
+	/* Issue POR command, and repeat up to once if 0xFC00 received */
+	do {
+		if (ps2_command(ps2dev, NULL, MAKE_PS2_CMD(0, 0, TP_COMMAND)) ||
+		    ps2_command(ps2dev, results, MAKE_PS2_CMD(0, 2, TP_POR)))
+			return -1;
+	} while (results[0] == 0xFC && results[1] == 0x00 && ++tries < 2);
+
+	/* Check for success response -- 0xAA00 */
+	if (results[0] != 0xAA || results[1] != 0x00)
+		return -1;
+
+	return 0;
+}
+
+/*
  * Device IO: read, write and toggle bit
  */
-static int trackpoint_read(struct ps2dev *ps2dev, unsigned char loc, unsigned char *results)
+static int trackpoint_read(struct ps2dev *ps2dev,
+			   unsigned char loc, unsigned char *results)
 {
 	if (ps2_command(ps2dev, NULL, MAKE_PS2_CMD(0, 0, TP_COMMAND)) ||
 	    ps2_command(ps2dev, results, MAKE_PS2_CMD(0, 1, loc))) {
@@ -32,7 +57,8 @@ static int trackpoint_read(struct ps2dev *ps2dev, unsigned char loc, unsigned ch
 	return 0;
 }
 
-static int trackpoint_write(struct ps2dev *ps2dev, unsigned char loc, unsigned char val)
+static int trackpoint_write(struct ps2dev *ps2dev,
+			    unsigned char loc, unsigned char val)
 {
 	if (ps2_command(ps2dev, NULL, MAKE_PS2_CMD(0, 0, TP_COMMAND)) ||
 	    ps2_command(ps2dev, NULL, MAKE_PS2_CMD(0, 0, TP_WRITE_MEM)) ||
@@ -44,7 +70,8 @@ static int trackpoint_write(struct ps2dev *ps2dev, unsigned char loc, unsigned c
 	return 0;
 }
 
-static int trackpoint_toggle_bit(struct ps2dev *ps2dev, unsigned char loc, unsigned char mask)
+static int trackpoint_toggle_bit(struct ps2dev *ps2dev,
+				 unsigned char loc, unsigned char mask)
 {
 	/* Bad things will happen if the loc param isn't in this range */
 	if (loc < 0x20 || loc >= 0x2F)
@@ -60,6 +87,18 @@ static int trackpoint_toggle_bit(struct ps2dev *ps2dev, unsigned char loc, unsig
 	return 0;
 }
 
+static int trackpoint_update_bit(struct ps2dev *ps2dev, unsigned char loc,
+				 unsigned char mask, unsigned char value)
+{
+	int retval = 0;
+	unsigned char data;
+
+	trackpoint_read(ps2dev, loc, &data);
+	if (((data & mask) == mask) != !!value)
+		retval = trackpoint_toggle_bit(ps2dev, loc, mask);
+
+	return retval;
+}
 
 /*
  * Trackpoint-specific attributes
@@ -69,6 +108,7 @@ struct trackpoint_attr_data {
 	unsigned char command;
 	unsigned char mask;
 	unsigned char inverted;
+	unsigned char power_on_default;
 };
 
 static ssize_t trackpoint_show_int_attr(struct psmouse *psmouse, void *data, char *buf)
@@ -102,10 +142,11 @@ static ssize_t trackpoint_set_int_attr(struct psmouse *psmouse, void *data,
 	return count;
 }
 
-#define TRACKPOINT_INT_ATTR(_name, _command)					\
+#define TRACKPOINT_INT_ATTR(_name, _command, _default)				\
 	static struct trackpoint_attr_data trackpoint_attr_##_name = {		\
 		.field_offset = offsetof(struct trackpoint_data, _name),	\
 		.command = _command,						\
+		.power_on_default = _default,					\
 	};									\
 	PSMOUSE_DEFINE_ATTR(_name, S_IWUSR | S_IRUGO,				\
 			    &trackpoint_attr_##_name,				\
@@ -139,31 +180,60 @@ static ssize_t trackpoint_set_bit_attr(struct psmouse *psmouse, void *data,
 }
 
 
-#define TRACKPOINT_BIT_ATTR(_name, _command, _mask, _inv)				\
-	static struct trackpoint_attr_data trackpoint_attr_##_name = {		\
-		.field_offset	= offsetof(struct trackpoint_data, _name),	\
-		.command	= _command,					\
-		.mask		= _mask,					\
-		.inverted	= _inv,						\
-	};									\
-	PSMOUSE_DEFINE_ATTR(_name, S_IWUSR | S_IRUGO,				\
-			    &trackpoint_attr_##_name,				\
-			    trackpoint_show_int_attr, trackpoint_set_bit_attr)
-
-TRACKPOINT_INT_ATTR(sensitivity, TP_SENS);
-TRACKPOINT_INT_ATTR(speed, TP_SPEED);
-TRACKPOINT_INT_ATTR(inertia, TP_INERTIA);
-TRACKPOINT_INT_ATTR(reach, TP_REACH);
-TRACKPOINT_INT_ATTR(draghys, TP_DRAGHYS);
-TRACKPOINT_INT_ATTR(mindrag, TP_MINDRAG);
-TRACKPOINT_INT_ATTR(thresh, TP_THRESH);
-TRACKPOINT_INT_ATTR(upthresh, TP_UP_THRESH);
-TRACKPOINT_INT_ATTR(ztime, TP_Z_TIME);
-TRACKPOINT_INT_ATTR(jenks, TP_JENKS_CURV);
-
-TRACKPOINT_BIT_ATTR(press_to_select, TP_TOGGLE_PTSON, TP_MASK_PTSON, 0);
-TRACKPOINT_BIT_ATTR(skipback, TP_TOGGLE_SKIPBACK, TP_MASK_SKIPBACK, 0);
-TRACKPOINT_BIT_ATTR(ext_dev, TP_TOGGLE_EXT_DEV, TP_MASK_EXT_DEV, 1);
+#define TRACKPOINT_BIT_ATTR(_name, _command, _mask, _inv, _default)	\
+static struct trackpoint_attr_data trackpoint_attr_##_name = {		\
+	.field_offset		= offsetof(struct trackpoint_data,	\
+					   _name),			\
+	.command		= _command,				\
+	.mask			= _mask,				\
+	.inverted		= _inv,					\
+	.power_on_default	= _default,				\
+	};								\
+PSMOUSE_DEFINE_ATTR(_name, S_IWUSR | S_IRUGO,				\
+		    &trackpoint_attr_##_name,				\
+		    trackpoint_show_int_attr, trackpoint_set_bit_attr)
+
+#define TRACKPOINT_UPDATE_BIT(_psmouse, _tp, _name)			\
+do {									\
+	struct trackpoint_attr_data *_attr = &trackpoint_attr_##_name;	\
+									\
+	trackpoint_update_bit(&_psmouse->ps2dev,			\
+			_attr->command, _attr->mask, _tp->_name);	\
+} while (0)
+
+#define TRACKPOINT_UPDATE(_power_on, _psmouse, _tp, _name)		\
+do {									\
+	if (!_power_on ||						\
+	    _tp->_name != trackpoint_attr_##_name.power_on_default) {	\
+		if (!trackpoint_attr_##_name.mask)			\
+			trackpoint_write(&_psmouse->ps2dev,		\
+				 trackpoint_attr_##_name.command,	\
+				 _tp->_name);				\
+		else							\
+			TRACKPOINT_UPDATE_BIT(_psmouse, _tp, _name);	\
+	}								\
+} while (0)
+
+#define TRACKPOINT_SET_POWER_ON_DEFAULT(_tp, _name)				\
+	(_tp->_name = trackpoint_attr_##_name.power_on_default)
+
+TRACKPOINT_INT_ATTR(sensitivity, TP_SENS, TP_DEF_SENS);
+TRACKPOINT_INT_ATTR(speed, TP_SPEED, TP_DEF_SPEED);
+TRACKPOINT_INT_ATTR(inertia, TP_INERTIA, TP_DEF_INERTIA);
+TRACKPOINT_INT_ATTR(reach, TP_REACH, TP_DEF_REACH);
+TRACKPOINT_INT_ATTR(draghys, TP_DRAGHYS, TP_DEF_DRAGHYS);
+TRACKPOINT_INT_ATTR(mindrag, TP_MINDRAG, TP_DEF_MINDRAG);
+TRACKPOINT_INT_ATTR(thresh, TP_THRESH, TP_DEF_THRESH);
+TRACKPOINT_INT_ATTR(upthresh, TP_UP_THRESH, TP_DEF_UP_THRESH);
+TRACKPOINT_INT_ATTR(ztime, TP_Z_TIME, TP_DEF_Z_TIME);
+TRACKPOINT_INT_ATTR(jenks, TP_JENKS_CURV, TP_DEF_JENKS_CURV);
+
+TRACKPOINT_BIT_ATTR(press_to_select, TP_TOGGLE_PTSON, TP_MASK_PTSON, 0,
+		    TP_DEF_PTSON);
+TRACKPOINT_BIT_ATTR(skipback, TP_TOGGLE_SKIPBACK, TP_MASK_SKIPBACK, 0,
+		    TP_DEF_SKIPBACK);
+TRACKPOINT_BIT_ATTR(ext_dev, TP_TOGGLE_EXT_DEV, TP_MASK_EXT_DEV, 1,
+		    TP_DEF_EXT_DEV);
 
 static struct attribute *trackpoint_attrs[] = {
 	&psmouse_attr_sensitivity.dattr.attr,
@@ -202,73 +272,72 @@ static int trackpoint_start_protocol(struct psmouse *psmouse, unsigned char *fir
 	return 0;
 }
 
-static int trackpoint_sync(struct psmouse *psmouse)
+/*
+ * Write parameters to trackpad.
+ * in_power_on_state: Set to true if TP is in default / power-on state (ex. if
+ *		      power-on reset was run). If so, values will only be
+ *		      written to TP if they differ from power-on default.
+ */
+static int trackpoint_sync(struct psmouse *psmouse, bool in_power_on_state)
 {
 	struct trackpoint_data *tp = psmouse->private;
-	unsigned char toggle;
-
-	/* Disable features that may make device unusable with this driver */
-	trackpoint_read(&psmouse->ps2dev, TP_TOGGLE_TWOHAND, &toggle);
-	if (toggle & TP_MASK_TWOHAND)
-		trackpoint_toggle_bit(&psmouse->ps2dev, TP_TOGGLE_TWOHAND, TP_MASK_TWOHAND);
-
-	trackpoint_read(&psmouse->ps2dev, TP_TOGGLE_SOURCE_TAG, &toggle);
-	if (toggle & TP_MASK_SOURCE_TAG)
-		trackpoint_toggle_bit(&psmouse->ps2dev, TP_TOGGLE_SOURCE_TAG, TP_MASK_SOURCE_TAG);
-
-	trackpoint_read(&psmouse->ps2dev, TP_TOGGLE_MB, &toggle);
-	if (toggle & TP_MASK_MB)
-		trackpoint_toggle_bit(&psmouse->ps2dev, TP_TOGGLE_MB, TP_MASK_MB);
-
-	/* Push the config to the device */
-	trackpoint_write(&psmouse->ps2dev, TP_SENS, tp->sensitivity);
-	trackpoint_write(&psmouse->ps2dev, TP_INERTIA, tp->inertia);
-	trackpoint_write(&psmouse->ps2dev, TP_SPEED, tp->speed);
-
-	trackpoint_write(&psmouse->ps2dev, TP_REACH, tp->reach);
-	trackpoint_write(&psmouse->ps2dev, TP_DRAGHYS, tp->draghys);
-	trackpoint_write(&psmouse->ps2dev, TP_MINDRAG, tp->mindrag);
-
-	trackpoint_write(&psmouse->ps2dev, TP_THRESH, tp->thresh);
-	trackpoint_write(&psmouse->ps2dev, TP_UP_THRESH, tp->upthresh);
 
-	trackpoint_write(&psmouse->ps2dev, TP_Z_TIME, tp->ztime);
-	trackpoint_write(&psmouse->ps2dev, TP_JENKS_CURV, tp->jenks);
+	if (!in_power_on_state) {
+		/*
+		 * Disable features that may make device unusable
+		 * with this driver.
+		 */
+		trackpoint_update_bit(&psmouse->ps2dev, TP_TOGGLE_TWOHAND,
+				      TP_MASK_TWOHAND, TP_DEF_TWOHAND);
 
-	trackpoint_read(&psmouse->ps2dev, TP_TOGGLE_PTSON, &toggle);
-	if (((toggle & TP_MASK_PTSON) == TP_MASK_PTSON) != tp->press_to_select)
-		 trackpoint_toggle_bit(&psmouse->ps2dev, TP_TOGGLE_PTSON, TP_MASK_PTSON);
+		trackpoint_update_bit(&psmouse->ps2dev, TP_TOGGLE_SOURCE_TAG,
+				      TP_MASK_SOURCE_TAG, TP_DEF_SOURCE_TAG);
 
-	trackpoint_read(&psmouse->ps2dev, TP_TOGGLE_SKIPBACK, &toggle);
-	if (((toggle & TP_MASK_SKIPBACK) == TP_MASK_SKIPBACK) != tp->skipback)
-		trackpoint_toggle_bit(&psmouse->ps2dev, TP_TOGGLE_SKIPBACK, TP_MASK_SKIPBACK);
+		trackpoint_update_bit(&psmouse->ps2dev, TP_TOGGLE_MB,
+				      TP_MASK_MB, TP_DEF_MB);
+	}
 
-	trackpoint_read(&psmouse->ps2dev, TP_TOGGLE_EXT_DEV, &toggle);
-	if (((toggle & TP_MASK_EXT_DEV) == TP_MASK_EXT_DEV) != tp->ext_dev)
-		trackpoint_toggle_bit(&psmouse->ps2dev, TP_TOGGLE_EXT_DEV, TP_MASK_EXT_DEV);
+	/*
+	 * These properties can be changed in this driver. Only
+	 * configure them if the values are non-default or if the TP is in
+	 * an unknown state.
+	 */
+	TRACKPOINT_UPDATE(in_power_on_state, psmouse, tp, sensitivity);
+	TRACKPOINT_UPDATE(in_power_on_state, psmouse, tp, inertia);
+	TRACKPOINT_UPDATE(in_power_on_state, psmouse, tp, speed);
+	TRACKPOINT_UPDATE(in_power_on_state, psmouse, tp, reach);
+	TRACKPOINT_UPDATE(in_power_on_state, psmouse, tp, draghys);
+	TRACKPOINT_UPDATE(in_power_on_state, psmouse, tp, mindrag);
+	TRACKPOINT_UPDATE(in_power_on_state, psmouse, tp, thresh);
+	TRACKPOINT_UPDATE(in_power_on_state, psmouse, tp, upthresh);
+	TRACKPOINT_UPDATE(in_power_on_state, psmouse, tp, ztime);
+	TRACKPOINT_UPDATE(in_power_on_state, psmouse, tp, jenks);
+
+	/* toggles */
+	TRACKPOINT_UPDATE(in_power_on_state, psmouse, tp, press_to_select);
+	TRACKPOINT_UPDATE(in_power_on_state, psmouse, tp, skipback);
+	TRACKPOINT_UPDATE(in_power_on_state, psmouse, tp, ext_dev);
 
 	return 0;
 }
 
 static void trackpoint_defaults(struct trackpoint_data *tp)
 {
-	tp->press_to_select = TP_DEF_PTSON;
-	tp->sensitivity = TP_DEF_SENS;
-	tp->speed = TP_DEF_SPEED;
-	tp->reach = TP_DEF_REACH;
-
-	tp->draghys = TP_DEF_DRAGHYS;
-	tp->mindrag = TP_DEF_MINDRAG;
-
-	tp->thresh = TP_DEF_THRESH;
-	tp->upthresh = TP_DEF_UP_THRESH;
-
-	tp->ztime = TP_DEF_Z_TIME;
-	tp->jenks = TP_DEF_JENKS_CURV;
-
-	tp->inertia = TP_DEF_INERTIA;
-	tp->skipback = TP_DEF_SKIPBACK;
-	tp->ext_dev = TP_DEF_EXT_DEV;
+	TRACKPOINT_SET_POWER_ON_DEFAULT(tp, sensitivity);
+	TRACKPOINT_SET_POWER_ON_DEFAULT(tp, speed);
+	TRACKPOINT_SET_POWER_ON_DEFAULT(tp, reach);
+	TRACKPOINT_SET_POWER_ON_DEFAULT(tp, draghys);
+	TRACKPOINT_SET_POWER_ON_DEFAULT(tp, mindrag);
+	TRACKPOINT_SET_POWER_ON_DEFAULT(tp, thresh);
+	TRACKPOINT_SET_POWER_ON_DEFAULT(tp, upthresh);
+	TRACKPOINT_SET_POWER_ON_DEFAULT(tp, ztime);
+	TRACKPOINT_SET_POWER_ON_DEFAULT(tp, jenks);
+	TRACKPOINT_SET_POWER_ON_DEFAULT(tp, inertia);
+
+	/* toggles */
+	TRACKPOINT_SET_POWER_ON_DEFAULT(tp, press_to_select);
+	TRACKPOINT_SET_POWER_ON_DEFAULT(tp, skipback);
+	TRACKPOINT_SET_POWER_ON_DEFAULT(tp, ext_dev);
 }
 
 static void trackpoint_disconnect(struct psmouse *psmouse)
@@ -281,10 +350,13 @@ static void trackpoint_disconnect(struct psmouse *psmouse)
 
 static int trackpoint_reconnect(struct psmouse *psmouse)
 {
+	int reset_fail;
+
 	if (trackpoint_start_protocol(psmouse, NULL))
 		return -1;
 
-	if (trackpoint_sync(psmouse))
+	reset_fail = trackpoint_power_on_reset(&psmouse->ps2dev);
+	if (trackpoint_sync(psmouse, !reset_fail))
 		return -1;
 
 	return 0;
@@ -322,7 +394,12 @@ int trackpoint_detect(struct psmouse *psmouse, bool set_properties)
 		__set_bit(BTN_MIDDLE, psmouse->dev->keybit);
 
 	trackpoint_defaults(psmouse->private);
-	trackpoint_sync(psmouse);
+
+	error = trackpoint_power_on_reset(&psmouse->ps2dev);
+
+	/* Write defaults to TP only if reset fails. */
+	if (error)
+		trackpoint_sync(psmouse, false);
 
 	error = sysfs_create_group(&ps2dev->serio->dev.kobj, &trackpoint_attr_group);
 	if (error) {
diff --git a/drivers/input/mouse/trackpoint.h b/drivers/input/mouse/trackpoint.h
index e558a7096618..ecd0547964a5 100644
--- a/drivers/input/mouse/trackpoint.h
+++ b/drivers/input/mouse/trackpoint.h
@@ -126,6 +126,8 @@
 #define TP_DEF_PTSON		0x00
 #define TP_DEF_SKIPBACK		0x00
 #define TP_DEF_EXT_DEV		0x00	/* 0 means enabled */
+#define TP_DEF_TWOHAND		0x00
+#define TP_DEF_SOURCE_TAG	0x00
 
 #define MAKE_PS2_CMD(params, results, cmd) ((params<<12) | (results<<8) | (cmd))
 
@@ -136,9 +138,9 @@ struct trackpoint_data
 	unsigned char thresh, upthresh;
 	unsigned char ztime, jenks;
 
+	/* toggles */
 	unsigned char press_to_select;
 	unsigned char skipback;
-
 	unsigned char ext_dev;
 };
 
diff --git a/drivers/input/serio/Kconfig b/drivers/input/serio/Kconfig
index 3ec5ef2dd443..aebfe3ecb945 100644
--- a/drivers/input/serio/Kconfig
+++ b/drivers/input/serio/Kconfig
@@ -245,4 +245,14 @@ config SERIO_ARC_PS2
 	  To compile this driver as a module, choose M here; the module
 	  will be called arc_ps2.
 
+config SERIO_APBPS2
+	tristate "GRLIB APBPS2 PS/2 keyboard/mouse controller"
+	depends on OF
+	help
+	  Say Y here if you want support for GRLIB APBPS2 peripherals used
+	  to connect to PS/2 keyboard and/or mouse.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called apbps2.
+
 endif
diff --git a/drivers/input/serio/Makefile b/drivers/input/serio/Makefile
index 4b0c8f84f1c1..8edb36c2cdb4 100644
--- a/drivers/input/serio/Makefile
+++ b/drivers/input/serio/Makefile
@@ -26,3 +26,4 @@ obj-$(CONFIG_SERIO_AMS_DELTA)	+= ams_delta_serio.o
 obj-$(CONFIG_SERIO_XILINX_XPS_PS2)	+= xilinx_ps2.o
 obj-$(CONFIG_SERIO_ALTERA_PS2)	+= altera_ps2.o
 obj-$(CONFIG_SERIO_ARC_PS2)	+= arc_ps2.o
+obj-$(CONFIG_SERIO_APBPS2)	+= apbps2.o
diff --git a/drivers/input/serio/apbps2.c b/drivers/input/serio/apbps2.c
new file mode 100644
index 000000000000..17e01a807ddc
--- /dev/null
+++ b/drivers/input/serio/apbps2.c
@@ -0,0 +1,228 @@
+/*
+ * Copyright (C) 2013 Aeroflex Gaisler
+ *
+ * This driver supports the APBPS2 PS/2 core available in the GRLIB
+ * VHDL IP core library.
+ *
+ * Full documentation of the APBPS2 core can be found here:
+ * http://www.gaisler.com/products/grlib/grip.pdf
+ *
+ * See "Documentation/devicetree/bindings/input/ps2keyb-mouse-apbps2.txt" for
+ * information on open firmware properties.
+ *
+ * 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.
+ *
+ * Contributors: Daniel Hellstrom <daniel@gaisler.com>
+ */
+#include <linux/platform_device.h>
+#include <linux/of_device.h>
+#include <linux/module.h>
+#include <linux/serio.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/of_irq.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/kernel.h>
+#include <linux/io.h>
+
+struct apbps2_regs {
+	u32 __iomem data;	/* 0x00 */
+	u32 __iomem status;	/* 0x04 */
+	u32 __iomem ctrl;	/* 0x08 */
+	u32 __iomem reload;	/* 0x0c */
+};
+
+#define APBPS2_STATUS_DR	(1<<0)
+#define APBPS2_STATUS_PE	(1<<1)
+#define APBPS2_STATUS_FE	(1<<2)
+#define APBPS2_STATUS_KI	(1<<3)
+#define APBPS2_STATUS_RF	(1<<4)
+#define APBPS2_STATUS_TF	(1<<5)
+#define APBPS2_STATUS_TCNT	(0x1f<<22)
+#define APBPS2_STATUS_RCNT	(0x1f<<27)
+
+#define APBPS2_CTRL_RE		(1<<0)
+#define APBPS2_CTRL_TE		(1<<1)
+#define APBPS2_CTRL_RI		(1<<2)
+#define APBPS2_CTRL_TI		(1<<3)
+
+struct apbps2_priv {
+	struct serio		*io;
+	struct apbps2_regs	*regs;
+};
+
+static int apbps2_idx;
+
+static irqreturn_t apbps2_isr(int irq, void *dev_id)
+{
+	struct apbps2_priv *priv = dev_id;
+	unsigned long status, data, rxflags;
+	irqreturn_t ret = IRQ_NONE;
+
+	while ((status = ioread32be(&priv->regs->status)) & APBPS2_STATUS_DR) {
+		data = ioread32be(&priv->regs->data);
+		rxflags = (status & APBPS2_STATUS_PE) ? SERIO_PARITY : 0;
+		rxflags |= (status & APBPS2_STATUS_FE) ? SERIO_FRAME : 0;
+
+		/* clear error bits? */
+		if (rxflags)
+			iowrite32be(0, &priv->regs->status);
+
+		serio_interrupt(priv->io, data, rxflags);
+
+		ret = IRQ_HANDLED;
+	}
+
+	return ret;
+}
+
+static int apbps2_write(struct serio *io, unsigned char val)
+{
+	struct apbps2_priv *priv = io->port_data;
+	unsigned int tleft = 10000; /* timeout in 100ms */
+
+	/* delay until PS/2 controller has room for more chars */
+	while ((ioread32be(&priv->regs->status) & APBPS2_STATUS_TF) && tleft--)
+		udelay(10);
+
+	if ((ioread32be(&priv->regs->status) & APBPS2_STATUS_TF) == 0) {
+		iowrite32be(val, &priv->regs->data);
+
+		iowrite32be(APBPS2_CTRL_RE | APBPS2_CTRL_RI | APBPS2_CTRL_TE,
+				&priv->regs->ctrl);
+		return 0;
+	}
+
+	return -ETIMEDOUT;
+}
+
+static int apbps2_open(struct serio *io)
+{
+	struct apbps2_priv *priv = io->port_data;
+	int limit;
+	unsigned long tmp;
+
+	/* clear error flags */
+	iowrite32be(0, &priv->regs->status);
+
+	/* Clear old data if available (unlikely) */
+	limit = 1024;
+	while ((ioread32be(&priv->regs->status) & APBPS2_STATUS_DR) && --limit)
+		tmp = ioread32be(&priv->regs->data);
+
+	/* Enable reciever and it's interrupt */
+	iowrite32be(APBPS2_CTRL_RE | APBPS2_CTRL_RI, &priv->regs->ctrl);
+
+	return 0;
+}
+
+static void apbps2_close(struct serio *io)
+{
+	struct apbps2_priv *priv = io->port_data;
+
+	/* stop interrupts at PS/2 HW level */
+	iowrite32be(0, &priv->regs->ctrl);
+}
+
+/* Initialize one APBPS2 PS/2 core */
+static int apbps2_of_probe(struct platform_device *ofdev)
+{
+	struct apbps2_priv *priv;
+	int irq, err;
+	u32 freq_hz;
+	struct resource *res;
+
+	priv = devm_kzalloc(&ofdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(&ofdev->dev, "memory allocation failed\n");
+		return -ENOMEM;
+	}
+
+	/* Find Device Address */
+	res = platform_get_resource(ofdev, IORESOURCE_MEM, 0);
+	priv->regs = devm_ioremap_resource(&ofdev->dev, res);
+	if (IS_ERR(priv->regs))
+		return PTR_ERR(priv->regs);
+
+	/* Reset hardware, disable interrupt */
+	iowrite32be(0, &priv->regs->ctrl);
+
+	/* IRQ */
+	irq = irq_of_parse_and_map(ofdev->dev.of_node, 0);
+	err = devm_request_irq(&ofdev->dev, irq, apbps2_isr,
+				IRQF_SHARED, "apbps2", priv);
+	if (err) {
+		dev_err(&ofdev->dev, "request IRQ%d failed\n", irq);
+		return err;
+	}
+
+	/* Get core frequency */
+	if (of_property_read_u32(ofdev->dev.of_node, "freq", &freq_hz)) {
+		dev_err(&ofdev->dev, "unable to get core frequency\n");
+		return -EINVAL;
+	}
+
+	/* Set reload register to core freq in kHz/10 */
+	iowrite32be(freq_hz / 10000, &priv->regs->reload);
+
+	priv->io = kzalloc(sizeof(struct serio), GFP_KERNEL);
+	if (!priv->io)
+		return -ENOMEM;
+
+	priv->io->id.type = SERIO_8042;
+	priv->io->open = apbps2_open;
+	priv->io->close = apbps2_close;
+	priv->io->write = apbps2_write;
+	priv->io->port_data = priv;
+	strlcpy(priv->io->name, "APBPS2 PS/2", sizeof(priv->io->name));
+	snprintf(priv->io->phys, sizeof(priv->io->phys),
+		 "apbps2_%d", apbps2_idx++);
+
+	dev_info(&ofdev->dev, "irq = %d, base = 0x%p\n", irq, priv->regs);
+
+	serio_register_port(priv->io);
+
+	platform_set_drvdata(ofdev, priv);
+
+	return 0;
+}
+
+static int apbps2_of_remove(struct platform_device *of_dev)
+{
+	struct apbps2_priv *priv = platform_get_drvdata(of_dev);
+
+	serio_unregister_port(priv->io);
+
+	return 0;
+}
+
+static struct of_device_id apbps2_of_match[] = {
+	{ .name = "GAISLER_APBPS2", },
+	{ .name = "01_060", },
+	{}
+};
+
+MODULE_DEVICE_TABLE(of, apbps2_of_match);
+
+static struct platform_driver apbps2_of_driver = {
+	.driver = {
+		.name = "grlib-apbps2",
+		.owner = THIS_MODULE,
+		.of_match_table = apbps2_of_match,
+	},
+	.probe = apbps2_of_probe,
+	.remove = apbps2_of_remove,
+};
+
+module_platform_driver(apbps2_of_driver);
+
+MODULE_AUTHOR("Aeroflex Gaisler AB.");
+MODULE_DESCRIPTION("GRLIB APBPS2 PS/2 serial I/O");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/serio/arc_ps2.c b/drivers/input/serio/arc_ps2.c
index c52e3e589f72..3fb7727c8ea5 100644
--- a/drivers/input/serio/arc_ps2.c
+++ b/drivers/input/serio/arc_ps2.c
@@ -14,6 +14,7 @@
 #include <linux/input.h>
 #include <linux/serio.h>
 #include <linux/platform_device.h>
+#include <linux/of.h>
 #include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
@@ -259,10 +260,19 @@ static int arc_ps2_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_OF
+static const struct of_device_id arc_ps2_match[] = {
+	{ .compatible = "snps,arc_ps2" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, arc_ps2_match);
+#endif
+
 static struct platform_driver arc_ps2_driver = {
 	.driver	= {
-		.name	= "arc_ps2",
-		.owner	= THIS_MODULE,
+		.name		= "arc_ps2",
+		.owner		= THIS_MODULE,
+		.of_match_table	= of_match_ptr(arc_ps2_match),
 	},
 	.probe	= arc_ps2_probe,
 	.remove	= arc_ps2_remove,
diff --git a/drivers/input/serio/at32psif.c b/drivers/input/serio/at32psif.c
index 36e799c31f5e..190ce35af7df 100644
--- a/drivers/input/serio/at32psif.c
+++ b/drivers/input/serio/at32psif.c
@@ -359,18 +359,7 @@ static struct platform_driver psif_driver = {
 	},
 };
 
-static int __init psif_init(void)
-{
-	return platform_driver_probe(&psif_driver, psif_probe);
-}
-
-static void __exit psif_exit(void)
-{
-	platform_driver_unregister(&psif_driver);
-}
-
-module_init(psif_init);
-module_exit(psif_exit);
+module_platform_driver_probe(psif_driver, psif_probe);
 
 MODULE_AUTHOR("Hans-Christian Egtvedt <egtvedt@samfundet.no>");
 MODULE_DESCRIPTION("Atmel AVR32 PSIF PS/2 driver");
diff --git a/drivers/input/serio/q40kbd.c b/drivers/input/serio/q40kbd.c
index 70fe542839fb..436a3433f8e5 100644
--- a/drivers/input/serio/q40kbd.c
+++ b/drivers/input/serio/q40kbd.c
@@ -193,15 +193,4 @@ static struct platform_driver q40kbd_driver = {
 	.remove		= q40kbd_remove,
 };
 
-static int __init q40kbd_init(void)
-{
-	return platform_driver_probe(&q40kbd_driver, q40kbd_probe);
-}
-
-static void __exit q40kbd_exit(void)
-{
-	platform_driver_unregister(&q40kbd_driver);
-}
-
-module_init(q40kbd_init);
-module_exit(q40kbd_exit);
+module_platform_driver_probe(q40kbd_driver, q40kbd_probe);
diff --git a/drivers/input/touchscreen/ad7877.c b/drivers/input/touchscreen/ad7877.c
index 23fa829b869d..f3a174a83c82 100644
--- a/drivers/input/touchscreen/ad7877.c
+++ b/drivers/input/touchscreen/ad7877.c
@@ -273,7 +273,7 @@ static int ad7877_write(struct spi_device *spi, u16 reg, u16 val)
 
 static int ad7877_read_adc(struct spi_device *spi, unsigned command)
 {
-	struct ad7877 *ts = dev_get_drvdata(&spi->dev);
+	struct ad7877 *ts = spi_get_drvdata(spi);
 	struct ser_req *req;
 	int status;
 	int sample;
@@ -720,7 +720,7 @@ static int ad7877_probe(struct spi_device *spi)
 		goto err_free_mem;
 	}
 
-	dev_set_drvdata(&spi->dev, ts);
+	spi_set_drvdata(spi, ts);
 	ts->spi = spi;
 	ts->input = input_dev;
 
@@ -806,13 +806,13 @@ err_free_irq:
 err_free_mem:
 	input_free_device(input_dev);
 	kfree(ts);
-	dev_set_drvdata(&spi->dev, NULL);
+	spi_set_drvdata(spi, NULL);
 	return err;
 }
 
 static int ad7877_remove(struct spi_device *spi)
 {
-	struct ad7877 *ts = dev_get_drvdata(&spi->dev);
+	struct ad7877 *ts = spi_get_drvdata(spi);
 
 	sysfs_remove_group(&spi->dev.kobj, &ad7877_attr_group);
 
@@ -823,7 +823,7 @@ static int ad7877_remove(struct spi_device *spi)
 	kfree(ts);
 
 	dev_dbg(&spi->dev, "unregistered touchscreen\n");
-	dev_set_drvdata(&spi->dev, NULL);
+	spi_set_drvdata(spi, NULL);
 
 	return 0;
 }
diff --git a/drivers/input/touchscreen/ads7846.c b/drivers/input/touchscreen/ads7846.c
index 434c3df250ca..84ccf140c1bb 100644
--- a/drivers/input/touchscreen/ads7846.c
+++ b/drivers/input/touchscreen/ads7846.c
@@ -1245,7 +1245,7 @@ static int ads7846_probe(struct spi_device *spi)
 		goto err_free_mem;
 	}
 
-	dev_set_drvdata(&spi->dev, ts);
+	spi_set_drvdata(spi, ts);
 
 	ts->packet = packet;
 	ts->spi = spi;
@@ -1397,7 +1397,7 @@ static int ads7846_probe(struct spi_device *spi)
 
 static int ads7846_remove(struct spi_device *spi)
 {
-	struct ads7846 *ts = dev_get_drvdata(&spi->dev);
+	struct ads7846 *ts = spi_get_drvdata(spi);
 
 	device_init_wakeup(&spi->dev, false);
 
diff --git a/drivers/input/touchscreen/atmel-wm97xx.c b/drivers/input/touchscreen/atmel-wm97xx.c
index c5c2dbb93869..2c1e46b7e45b 100644
--- a/drivers/input/touchscreen/atmel-wm97xx.c
+++ b/drivers/input/touchscreen/atmel-wm97xx.c
@@ -432,17 +432,7 @@ static struct platform_driver atmel_wm97xx_driver = {
 	},
 };
 
-static int __init atmel_wm97xx_init(void)
-{
-	return platform_driver_probe(&atmel_wm97xx_driver, atmel_wm97xx_probe);
-}
-module_init(atmel_wm97xx_init);
-
-static void __exit atmel_wm97xx_exit(void)
-{
-	platform_driver_unregister(&atmel_wm97xx_driver);
-}
-module_exit(atmel_wm97xx_exit);
+module_platform_driver_probe(atmel_wm97xx_driver, atmel_wm97xx_probe);
 
 MODULE_AUTHOR("Hans-Christian Egtvedt <egtvedt@samfundet.no>");
 MODULE_DESCRIPTION("wm97xx continuous touch driver for Atmel AT91 and AVR32");
diff --git a/drivers/input/touchscreen/auo-pixcir-ts.c b/drivers/input/touchscreen/auo-pixcir-ts.c
index c6e19a96348e..d3f9f6b0f9b7 100644
--- a/drivers/input/touchscreen/auo-pixcir-ts.c
+++ b/drivers/input/touchscreen/auo-pixcir-ts.c
@@ -31,6 +31,8 @@
 #include <linux/delay.h>
 #include <linux/gpio.h>
 #include <linux/input/auo-pixcir-ts.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
 
 /*
  * Coordinate calculation:
@@ -111,6 +113,7 @@
 struct auo_pixcir_ts {
 	struct i2c_client	*client;
 	struct input_dev	*input;
+	const struct auo_pixcir_ts_platdata *pdata;
 	char			phys[32];
 
 	/* special handling for touch_indicate interupt mode */
@@ -132,7 +135,7 @@ static int auo_pixcir_collect_data(struct auo_pixcir_ts *ts,
 				   struct auo_point_t *point)
 {
 	struct i2c_client *client = ts->client;
-	const struct auo_pixcir_ts_platdata *pdata = client->dev.platform_data;
+	const struct auo_pixcir_ts_platdata *pdata = ts->pdata;
 	uint8_t raw_coord[8];
 	uint8_t raw_area[4];
 	int i, ret;
@@ -178,8 +181,7 @@ static int auo_pixcir_collect_data(struct auo_pixcir_ts *ts,
 static irqreturn_t auo_pixcir_interrupt(int irq, void *dev_id)
 {
 	struct auo_pixcir_ts *ts = dev_id;
-	struct i2c_client *client = ts->client;
-	const struct auo_pixcir_ts_platdata *pdata = client->dev.platform_data;
+	const struct auo_pixcir_ts_platdata *pdata = ts->pdata;
 	struct auo_point_t point[AUO_PIXCIR_REPORT_POINTS];
 	int i;
 	int ret;
@@ -290,7 +292,7 @@ static int auo_pixcir_int_config(struct auo_pixcir_ts *ts,
 					   int int_setting)
 {
 	struct i2c_client *client = ts->client;
-	struct auo_pixcir_ts_platdata *pdata = client->dev.platform_data;
+	const struct auo_pixcir_ts_platdata *pdata = ts->pdata;
 	int ret;
 
 	ret = i2c_smbus_read_byte_data(client, AUO_PIXCIR_REG_INT_SETTING);
@@ -479,53 +481,105 @@ unlock:
 }
 #endif
 
-static SIMPLE_DEV_PM_OPS(auo_pixcir_pm_ops, auo_pixcir_suspend,
-			 auo_pixcir_resume);
+static SIMPLE_DEV_PM_OPS(auo_pixcir_pm_ops,
+			 auo_pixcir_suspend, auo_pixcir_resume);
+
+#ifdef CONFIG_OF
+static struct auo_pixcir_ts_platdata *auo_pixcir_parse_dt(struct device *dev)
+{
+	struct auo_pixcir_ts_platdata *pdata;
+	struct device_node *np = dev->of_node;
+
+	if (!np)
+		return ERR_PTR(-ENOENT);
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata) {
+		dev_err(dev, "failed to allocate platform data\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	pdata->gpio_int = of_get_gpio(np, 0);
+	if (!gpio_is_valid(pdata->gpio_int)) {
+		dev_err(dev, "failed to get interrupt gpio\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	pdata->gpio_rst = of_get_gpio(np, 1);
+	if (!gpio_is_valid(pdata->gpio_rst)) {
+		dev_err(dev, "failed to get reset gpio\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	if (of_property_read_u32(np, "x-size", &pdata->x_max)) {
+		dev_err(dev, "failed to get x-size property\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	if (of_property_read_u32(np, "y-size", &pdata->y_max)) {
+		dev_err(dev, "failed to get y-size property\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	/* default to asserting the interrupt when the screen is touched */
+	pdata->int_setting = AUO_PIXCIR_INT_TOUCH_IND;
+
+	return pdata;
+}
+#else
+static struct auo_pixcir_ts_platdata *auo_pixcir_parse_dt(struct device *dev)
+{
+	return ERR_PTR(-EINVAL);
+}
+#endif
+
+static void auo_pixcir_reset(void *data)
+{
+	struct auo_pixcir_ts *ts = data;
+
+	gpio_set_value(ts->pdata->gpio_rst, 0);
+}
 
 static int auo_pixcir_probe(struct i2c_client *client,
-				      const struct i2c_device_id *id)
+			    const struct i2c_device_id *id)
 {
-	const struct auo_pixcir_ts_platdata *pdata = client->dev.platform_data;
+	const struct auo_pixcir_ts_platdata *pdata;
 	struct auo_pixcir_ts *ts;
 	struct input_dev *input_dev;
-	int ret;
-
-	if (!pdata)
-		return -EINVAL;
+	int version;
+	int error;
+
+	pdata = dev_get_platdata(&client->dev);
+	if (!pdata) {
+		pdata = auo_pixcir_parse_dt(&client->dev);
+		if (IS_ERR(pdata))
+			return PTR_ERR(pdata);
+	}
 
-	ts = kzalloc(sizeof(struct auo_pixcir_ts), GFP_KERNEL);
+	ts = devm_kzalloc(&client->dev,
+			  sizeof(struct auo_pixcir_ts), GFP_KERNEL);
 	if (!ts)
 		return -ENOMEM;
 
-	ret = gpio_request(pdata->gpio_int, "auo_pixcir_ts_int");
-	if (ret) {
-		dev_err(&client->dev, "request of gpio %d failed, %d\n",
-			pdata->gpio_int, ret);
-		goto err_gpio_int;
+	input_dev = devm_input_allocate_device(&client->dev);
+	if (!input_dev) {
+		dev_err(&client->dev, "could not allocate input device\n");
+		return -ENOMEM;
 	}
 
-	if (pdata->init_hw)
-		pdata->init_hw(client);
-
+	ts->pdata = pdata;
 	ts->client = client;
+	ts->input = input_dev;
 	ts->touch_ind_mode = 0;
+	ts->stopped = true;
 	init_waitqueue_head(&ts->wait);
 
 	snprintf(ts->phys, sizeof(ts->phys),
 		 "%s/input0", dev_name(&client->dev));
 
-	input_dev = input_allocate_device();
-	if (!input_dev) {
-		dev_err(&client->dev, "could not allocate input device\n");
-		goto err_input_alloc;
-	}
-
-	ts->input = input_dev;
-
 	input_dev->name = "AUO-Pixcir touchscreen";
 	input_dev->phys = ts->phys;
 	input_dev->id.bustype = BUS_I2C;
-	input_dev->dev.parent = &client->dev;
 
 	input_dev->open = auo_pixcir_input_open;
 	input_dev->close = auo_pixcir_input_close;
@@ -550,70 +604,70 @@ static int auo_pixcir_probe(struct i2c_client *client,
 			     AUO_PIXCIR_MAX_AREA, 0, 0);
 	input_set_abs_params(input_dev, ABS_MT_ORIENTATION, 0, 1, 0, 0);
 
-	ret = i2c_smbus_read_byte_data(client, AUO_PIXCIR_REG_VERSION);
-	if (ret < 0)
-		goto err_fw_vers;
-	dev_info(&client->dev, "firmware version 0x%X\n", ret);
-
-	ret = auo_pixcir_int_config(ts, pdata->int_setting);
-	if (ret)
-		goto err_fw_vers;
-
 	input_set_drvdata(ts->input, ts);
-	ts->stopped = true;
 
-	ret = request_threaded_irq(client->irq, NULL, auo_pixcir_interrupt,
-				   IRQF_TRIGGER_RISING | IRQF_ONESHOT,
-				   input_dev->name, ts);
-	if (ret) {
-		dev_err(&client->dev, "irq %d requested failed\n", client->irq);
-		goto err_fw_vers;
+	error = devm_gpio_request_one(&client->dev, pdata->gpio_int,
+				      GPIOF_DIR_IN, "auo_pixcir_ts_int");
+	if (error) {
+		dev_err(&client->dev, "request of gpio %d failed, %d\n",
+			pdata->gpio_int, error);
+		return error;
 	}
 
-	/* stop device and put it into deep sleep until it is opened */
-	ret = auo_pixcir_stop(ts);
-	if (ret < 0)
-		goto err_input_register;
-
-	ret = input_register_device(input_dev);
-	if (ret) {
-		dev_err(&client->dev, "could not register input device\n");
-		goto err_input_register;
+	error = devm_gpio_request_one(&client->dev, pdata->gpio_rst,
+				      GPIOF_DIR_OUT | GPIOF_INIT_HIGH,
+				      "auo_pixcir_ts_rst");
+	if (error) {
+		dev_err(&client->dev, "request of gpio %d failed, %d\n",
+			pdata->gpio_rst, error);
+		return error;
 	}
 
-	i2c_set_clientdata(client, ts);
-
-	return 0;
+	error = devm_add_action(&client->dev, auo_pixcir_reset, ts);
+	if (error) {
+		auo_pixcir_reset(ts);
+		dev_err(&client->dev, "failed to register reset action, %d\n",
+			error);
+		return error;
+	}
 
-err_input_register:
-	free_irq(client->irq, ts);
-err_fw_vers:
-	input_free_device(input_dev);
-err_input_alloc:
-	if (pdata->exit_hw)
-		pdata->exit_hw(client);
-	gpio_free(pdata->gpio_int);
-err_gpio_int:
-	kfree(ts);
+	msleep(200);
 
-	return ret;
-}
-
-static int auo_pixcir_remove(struct i2c_client *client)
-{
-	struct auo_pixcir_ts *ts = i2c_get_clientdata(client);
-	const struct auo_pixcir_ts_platdata *pdata = client->dev.platform_data;
+	version = i2c_smbus_read_byte_data(client, AUO_PIXCIR_REG_VERSION);
+	if (version < 0) {
+		error = version;
+		return error;
+	}
 
-	free_irq(client->irq, ts);
+	dev_info(&client->dev, "firmware version 0x%X\n", version);
 
-	input_unregister_device(ts->input);
+	error = auo_pixcir_int_config(ts, pdata->int_setting);
+	if (error)
+		return error;
 
-	if (pdata->exit_hw)
-		pdata->exit_hw(client);
+	error = devm_request_threaded_irq(&client->dev, client->irq,
+					  NULL, auo_pixcir_interrupt,
+					  IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					  input_dev->name, ts);
+	if (error) {
+		dev_err(&client->dev, "irq %d requested failed, %d\n",
+			client->irq, error);
+		return error;
+	}
 
-	gpio_free(pdata->gpio_int);
+	/* stop device and put it into deep sleep until it is opened */
+	error = auo_pixcir_stop(ts);
+	if (error)
+		return error;
+
+	error = input_register_device(input_dev);
+	if (error) {
+		dev_err(&client->dev, "could not register input device, %d\n",
+			error);
+		return error;
+	}
 
-	kfree(ts);
+	i2c_set_clientdata(client, ts);
 
 	return 0;
 }
@@ -624,14 +678,22 @@ static const struct i2c_device_id auo_pixcir_idtable[] = {
 };
 MODULE_DEVICE_TABLE(i2c, auo_pixcir_idtable);
 
+#ifdef CONFIG_OF
+static struct of_device_id auo_pixcir_ts_dt_idtable[] = {
+	{ .compatible = "auo,auo_pixcir_ts" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, auo_pixcir_ts_dt_idtable);
+#endif
+
 static struct i2c_driver auo_pixcir_driver = {
 	.driver = {
 		.owner	= THIS_MODULE,
 		.name	= "auo_pixcir_ts",
 		.pm	= &auo_pixcir_pm_ops,
+		.of_match_table	= of_match_ptr(auo_pixcir_ts_dt_idtable),
 	},
 	.probe		= auo_pixcir_probe,
-	.remove		= auo_pixcir_remove,
 	.id_table	= auo_pixcir_idtable,
 };
 
diff --git a/drivers/input/touchscreen/edt-ft5x06.c b/drivers/input/touchscreen/edt-ft5x06.c
index a9170157b442..83fa1b15a97f 100644
--- a/drivers/input/touchscreen/edt-ft5x06.c
+++ b/drivers/input/touchscreen/edt-ft5x06.c
@@ -440,8 +440,7 @@ static int edt_ft5x06_work_mode(struct edt_ft5x06_ts_data *tsdata)
 		return -EIO;
 	}
 
-	if (tsdata->raw_buffer)
-		kfree(tsdata->raw_buffer);
+	kfree(tsdata->raw_buffer);
 	tsdata->raw_buffer = NULL;
 
 	/* restore parameters */
diff --git a/drivers/input/touchscreen/eeti_ts.c b/drivers/input/touchscreen/eeti_ts.c
index 55255a940072..8fe5086c8d2e 100644
--- a/drivers/input/touchscreen/eeti_ts.c
+++ b/drivers/input/touchscreen/eeti_ts.c
@@ -206,8 +206,7 @@ static int eeti_ts_probe(struct i2c_client *client,
 	if (err < 0)
 		goto err1;
 
-	if (pdata)
-		priv->irq_active_high = pdata->irq_active_high;
+	priv->irq_active_high = pdata->irq_active_high;
 
 	irq_flags = priv->irq_active_high ?
 		IRQF_TRIGGER_RISING : IRQF_TRIGGER_FALLING;
diff --git a/drivers/input/touchscreen/mc13783_ts.c b/drivers/input/touchscreen/mc13783_ts.c
index 02103b6abb39..89308fe38752 100644
--- a/drivers/input/touchscreen/mc13783_ts.c
+++ b/drivers/input/touchscreen/mc13783_ts.c
@@ -250,17 +250,7 @@ static struct platform_driver mc13783_ts_driver = {
 	},
 };
 
-static int __init mc13783_ts_init(void)
-{
-	return platform_driver_probe(&mc13783_ts_driver, &mc13783_ts_probe);
-}
-module_init(mc13783_ts_init);
-
-static void __exit mc13783_ts_exit(void)
-{
-	platform_driver_unregister(&mc13783_ts_driver);
-}
-module_exit(mc13783_ts_exit);
+module_platform_driver_probe(mc13783_ts_driver, mc13783_ts_probe);
 
 MODULE_DESCRIPTION("MC13783 input touchscreen driver");
 MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de>");
diff --git a/drivers/input/touchscreen/st1232.c b/drivers/input/touchscreen/st1232.c
index d9d05e222428..1740a2496371 100644
--- a/drivers/input/touchscreen/st1232.c
+++ b/drivers/input/touchscreen/st1232.c
@@ -19,13 +19,16 @@
  */
 
 #include <linux/delay.h>
+#include <linux/gpio.h>
 #include <linux/i2c.h>
 #include <linux/input.h>
 #include <linux/interrupt.h>
 #include <linux/module.h>
+#include <linux/of_gpio.h>
 #include <linux/pm_qos.h>
 #include <linux/slab.h>
 #include <linux/types.h>
+#include <linux/platform_data/st1232_pdata.h>
 
 #define ST1232_TS_NAME	"st1232-ts"
 
@@ -48,6 +51,7 @@ struct st1232_ts_data {
 	struct input_dev *input_dev;
 	struct st1232_ts_finger finger[MAX_FINGERS];
 	struct dev_pm_qos_request low_latency_req;
+	int reset_gpio;
 };
 
 static int st1232_ts_read_data(struct st1232_ts_data *ts)
@@ -139,10 +143,17 @@ end:
 	return IRQ_HANDLED;
 }
 
+static void st1232_ts_power(struct st1232_ts_data *ts, bool poweron)
+{
+	if (gpio_is_valid(ts->reset_gpio))
+		gpio_direction_output(ts->reset_gpio, poweron);
+}
+
 static int st1232_ts_probe(struct i2c_client *client,
 					const struct i2c_device_id *id)
 {
 	struct st1232_ts_data *ts;
+	struct st1232_pdata *pdata = client->dev.platform_data;
 	struct input_dev *input_dev;
 	int error;
 
@@ -156,17 +167,36 @@ static int st1232_ts_probe(struct i2c_client *client,
 		return -EINVAL;
 	}
 
+	ts = devm_kzalloc(&client->dev, sizeof(*ts), GFP_KERNEL);
+	if (!ts)
+		return -ENOMEM;
 
-	ts = kzalloc(sizeof(struct st1232_ts_data), GFP_KERNEL);
-	input_dev = input_allocate_device();
-	if (!ts || !input_dev) {
-		error = -ENOMEM;
-		goto err_free_mem;
-	}
+	input_dev = devm_input_allocate_device(&client->dev);
+	if (!input_dev)
+		return -ENOMEM;
 
 	ts->client = client;
 	ts->input_dev = input_dev;
 
+	if (pdata)
+		ts->reset_gpio = pdata->reset_gpio;
+	else if (client->dev.of_node)
+		ts->reset_gpio = of_get_gpio(client->dev.of_node, 0);
+	else
+		ts->reset_gpio = -ENODEV;
+
+	if (gpio_is_valid(ts->reset_gpio)) {
+		error = devm_gpio_request(&client->dev, ts->reset_gpio, NULL);
+		if (error) {
+			dev_err(&client->dev,
+				"Unable to request GPIO pin %d.\n",
+				ts->reset_gpio);
+				return error;
+		}
+	}
+
+	st1232_ts_power(ts, true);
+
 	input_dev->name = "st1232-touchscreen";
 	input_dev->id.bustype = BUS_I2C;
 	input_dev->dev.parent = &client->dev;
@@ -179,31 +209,26 @@ static int st1232_ts_probe(struct i2c_client *client,
 	input_set_abs_params(input_dev, ABS_MT_POSITION_X, MIN_X, MAX_X, 0, 0);
 	input_set_abs_params(input_dev, ABS_MT_POSITION_Y, MIN_Y, MAX_Y, 0, 0);
 
-	error = request_threaded_irq(client->irq, NULL, st1232_ts_irq_handler,
-				     IRQF_ONESHOT, client->name, ts);
+	error = devm_request_threaded_irq(&client->dev, client->irq,
+					  NULL, st1232_ts_irq_handler,
+					  IRQF_ONESHOT,
+					  client->name, ts);
 	if (error) {
 		dev_err(&client->dev, "Failed to register interrupt\n");
-		goto err_free_mem;
+		return error;
 	}
 
 	error = input_register_device(ts->input_dev);
 	if (error) {
 		dev_err(&client->dev, "Unable to register %s input device\n",
 			input_dev->name);
-		goto err_free_irq;
+		return error;
 	}
 
 	i2c_set_clientdata(client, ts);
 	device_init_wakeup(&client->dev, 1);
 
 	return 0;
-
-err_free_irq:
-	free_irq(client->irq, ts);
-err_free_mem:
-	input_free_device(input_dev);
-	kfree(ts);
-	return error;
 }
 
 static int st1232_ts_remove(struct i2c_client *client)
@@ -211,9 +236,7 @@ static int st1232_ts_remove(struct i2c_client *client)
 	struct st1232_ts_data *ts = i2c_get_clientdata(client);
 
 	device_init_wakeup(&client->dev, 0);
-	free_irq(client->irq, ts);
-	input_unregister_device(ts->input_dev);
-	kfree(ts);
+	st1232_ts_power(ts, false);
 
 	return 0;
 }
@@ -222,11 +245,14 @@ static int st1232_ts_remove(struct i2c_client *client)
 static int st1232_ts_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
+	struct st1232_ts_data *ts = i2c_get_clientdata(client);
 
-	if (device_may_wakeup(&client->dev))
+	if (device_may_wakeup(&client->dev)) {
 		enable_irq_wake(client->irq);
-	else
+	} else {
 		disable_irq(client->irq);
+		st1232_ts_power(ts, false);
+	}
 
 	return 0;
 }
@@ -234,11 +260,14 @@ static int st1232_ts_suspend(struct device *dev)
 static int st1232_ts_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
+	struct st1232_ts_data *ts = i2c_get_clientdata(client);
 
-	if (device_may_wakeup(&client->dev))
+	if (device_may_wakeup(&client->dev)) {
 		disable_irq_wake(client->irq);
-	else
+	} else {
+		st1232_ts_power(ts, true);
 		enable_irq(client->irq);
+	}
 
 	return 0;
 }
diff --git a/drivers/input/touchscreen/wm9712.c b/drivers/input/touchscreen/wm9712.c
index 6e743e3dfda4..16b52115c27f 100644
--- a/drivers/input/touchscreen/wm9712.c
+++ b/drivers/input/touchscreen/wm9712.c
@@ -162,14 +162,14 @@ static void wm9712_phy_init(struct wm97xx *wm)
 	if (rpu) {
 		dig2 &= 0xffc0;
 		dig2 |= WM9712_RPU(rpu);
-		dev_dbg(wm->dev, "setting pen detect pull-up to %d Ohms",
+		dev_dbg(wm->dev, "setting pen detect pull-up to %d Ohms\n",
 			64000 / rpu);
 	}
 
 	/* WM9712 five wire */
 	if (five_wire) {
 		dig2 |= WM9712_45W;
-		dev_dbg(wm->dev, "setting 5-wire touchscreen mode.");
+		dev_dbg(wm->dev, "setting 5-wire touchscreen mode.\n");
 
 		if (pil) {
 			dev_warn(wm->dev, "pressure measurement is not "
@@ -182,21 +182,21 @@ static void wm9712_phy_init(struct wm97xx *wm)
 	if (pil == 2) {
 		dig2 |= WM9712_PIL;
 		dev_dbg(wm->dev,
-			"setting pressure measurement current to 400uA.");
+			"setting pressure measurement current to 400uA.\n");
 	} else if (pil)
 		dev_dbg(wm->dev,
-			"setting pressure measurement current to 200uA.");
+			"setting pressure measurement current to 200uA.\n");
 	if (!pil)
 		pressure = 0;
 
 	/* polling mode sample settling delay */
 	if (delay < 0 || delay > 15) {
-		dev_dbg(wm->dev, "supplied delay out of range.");
+		dev_dbg(wm->dev, "supplied delay out of range.\n");
 		delay = 4;
 	}
 	dig1 &= 0xff0f;
 	dig1 |= WM97XX_DELAY(delay);
-	dev_dbg(wm->dev, "setting adc sample delay to %d u Secs.",
+	dev_dbg(wm->dev, "setting adc sample delay to %d u Secs.\n",
 		delay_table[delay]);
 
 	/* mask */
@@ -285,7 +285,7 @@ static int wm9712_poll_sample(struct wm97xx *wm, int adcsel, int *sample)
 		if (is_pden(wm))
 			wm->pen_probably_down = 0;
 		else
-			dev_dbg(wm->dev, "adc sample timeout");
+			dev_dbg(wm->dev, "adc sample timeout\n");
 		return RC_PENUP;
 	}
 
@@ -295,15 +295,19 @@ static int wm9712_poll_sample(struct wm97xx *wm, int adcsel, int *sample)
 
 	/* check we have correct sample */
 	if ((*sample ^ adcsel) & WM97XX_ADCSEL_MASK) {
-		dev_dbg(wm->dev, "adc wrong sample, wanted %x got %x",
+		dev_dbg(wm->dev, "adc wrong sample, wanted %x got %x\n",
 			adcsel & WM97XX_ADCSEL_MASK,
 			*sample & WM97XX_ADCSEL_MASK);
-		return RC_PENUP;
+		return RC_AGAIN;
 	}
 
 	if (wants_pen && !(*sample & WM97XX_PEN_DOWN)) {
-		wm->pen_probably_down = 0;
-		return RC_PENUP;
+		/* Sometimes it reads a wrong value the first time. */
+		*sample = wm97xx_reg_read(wm, AC97_WM97XX_DIGITISER_RD);
+		if (!(*sample & WM97XX_PEN_DOWN)) {
+			wm->pen_probably_down = 0;
+			return RC_PENUP;
+		}
 	}
 
 	return RC_VALID;
@@ -345,7 +349,7 @@ static int wm9712_poll_coord(struct wm97xx *wm, struct wm97xx_data *data)
 		if (is_pden(wm))
 			wm->pen_probably_down = 0;
 		else
-			dev_dbg(wm->dev, "adc sample timeout");
+			dev_dbg(wm->dev, "adc sample timeout\n");
 		return RC_PENUP;
 	}
 
diff --git a/drivers/input/touchscreen/wm97xx-core.c b/drivers/input/touchscreen/wm97xx-core.c
index 5dbe73af2f8f..7e45c9f6e6b7 100644
--- a/drivers/input/touchscreen/wm97xx-core.c
+++ b/drivers/input/touchscreen/wm97xx-core.c
@@ -442,6 +442,16 @@ static int wm97xx_read_samples(struct wm97xx *wm)
 			"pen down: x=%x:%d, y=%x:%d, pressure=%x:%d\n",
 			data.x >> 12, data.x & 0xfff, data.y >> 12,
 			data.y & 0xfff, data.p >> 12, data.p & 0xfff);
+
+		if (abs_x[0] > (data.x & 0xfff) ||
+		    abs_x[1] < (data.x & 0xfff) ||
+		    abs_y[0] > (data.y & 0xfff) ||
+		    abs_y[1] < (data.y & 0xfff)) {
+			dev_dbg(wm->dev, "Measurement out of range, dropping it\n");
+			rc = RC_AGAIN;
+			goto out;
+		}
+
 		input_report_abs(wm->input_dev, ABS_X, data.x & 0xfff);
 		input_report_abs(wm->input_dev, ABS_Y, data.y & 0xfff);
 		input_report_abs(wm->input_dev, ABS_PRESSURE, data.p & 0xfff);
@@ -455,6 +465,7 @@ static int wm97xx_read_samples(struct wm97xx *wm)
 		wm->ts_reader_interval = wm->ts_reader_min_interval;
 	}
 
+out:
 	mutex_unlock(&wm->codec_mutex);
 	return rc;
 }
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index 830183737b0f..21d02b0d907c 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -46,6 +46,7 @@
 #include "amd_iommu_proto.h"
 #include "amd_iommu_types.h"
 #include "irq_remapping.h"
+#include "pci.h"
 
 #define CMD_SET_TYPE(cmd, t) ((cmd)->data[1] |= ((t) << 28))
 
@@ -263,12 +264,6 @@ static bool check_device(struct device *dev)
 	return true;
 }
 
-static void swap_pci_ref(struct pci_dev **from, struct pci_dev *to)
-{
-	pci_dev_put(*from);
-	*from = to;
-}
-
 static struct pci_bus *find_hosted_bus(struct pci_bus *bus)
 {
 	while (!bus->self) {
@@ -701,9 +696,6 @@ retry:
 static void iommu_poll_events(struct amd_iommu *iommu)
 {
 	u32 head, tail;
-	unsigned long flags;
-
-	spin_lock_irqsave(&iommu->lock, flags);
 
 	head = readl(iommu->mmio_base + MMIO_EVT_HEAD_OFFSET);
 	tail = readl(iommu->mmio_base + MMIO_EVT_TAIL_OFFSET);
@@ -714,8 +706,6 @@ static void iommu_poll_events(struct amd_iommu *iommu)
 	}
 
 	writel(head, iommu->mmio_base + MMIO_EVT_HEAD_OFFSET);
-
-	spin_unlock_irqrestore(&iommu->lock, flags);
 }
 
 static void iommu_handle_ppr_entry(struct amd_iommu *iommu, u64 *raw)
@@ -740,17 +730,11 @@ static void iommu_handle_ppr_entry(struct amd_iommu *iommu, u64 *raw)
 
 static void iommu_poll_ppr_log(struct amd_iommu *iommu)
 {
-	unsigned long flags;
 	u32 head, tail;
 
 	if (iommu->ppr_log == NULL)
 		return;
 
-	/* enable ppr interrupts again */
-	writel(MMIO_STATUS_PPR_INT_MASK, iommu->mmio_base + MMIO_STATUS_OFFSET);
-
-	spin_lock_irqsave(&iommu->lock, flags);
-
 	head = readl(iommu->mmio_base + MMIO_PPR_HEAD_OFFSET);
 	tail = readl(iommu->mmio_base + MMIO_PPR_TAIL_OFFSET);
 
@@ -786,34 +770,50 @@ static void iommu_poll_ppr_log(struct amd_iommu *iommu)
 		head = (head + PPR_ENTRY_SIZE) % PPR_LOG_SIZE;
 		writel(head, iommu->mmio_base + MMIO_PPR_HEAD_OFFSET);
 
-		/*
-		 * Release iommu->lock because ppr-handling might need to
-		 * re-acquire it
-		 */
-		spin_unlock_irqrestore(&iommu->lock, flags);
-
 		/* Handle PPR entry */
 		iommu_handle_ppr_entry(iommu, entry);
 
-		spin_lock_irqsave(&iommu->lock, flags);
-
 		/* Refresh ring-buffer information */
 		head = readl(iommu->mmio_base + MMIO_PPR_HEAD_OFFSET);
 		tail = readl(iommu->mmio_base + MMIO_PPR_TAIL_OFFSET);
 	}
-
-	spin_unlock_irqrestore(&iommu->lock, flags);
 }
 
 irqreturn_t amd_iommu_int_thread(int irq, void *data)
 {
-	struct amd_iommu *iommu;
+	struct amd_iommu *iommu = (struct amd_iommu *) data;
+	u32 status = readl(iommu->mmio_base + MMIO_STATUS_OFFSET);
 
-	for_each_iommu(iommu) {
-		iommu_poll_events(iommu);
-		iommu_poll_ppr_log(iommu);
-	}
+	while (status & (MMIO_STATUS_EVT_INT_MASK | MMIO_STATUS_PPR_INT_MASK)) {
+		/* Enable EVT and PPR interrupts again */
+		writel((MMIO_STATUS_EVT_INT_MASK | MMIO_STATUS_PPR_INT_MASK),
+			iommu->mmio_base + MMIO_STATUS_OFFSET);
 
+		if (status & MMIO_STATUS_EVT_INT_MASK) {
+			pr_devel("AMD-Vi: Processing IOMMU Event Log\n");
+			iommu_poll_events(iommu);
+		}
+
+		if (status & MMIO_STATUS_PPR_INT_MASK) {
+			pr_devel("AMD-Vi: Processing IOMMU PPR Log\n");
+			iommu_poll_ppr_log(iommu);
+		}
+
+		/*
+		 * Hardware bug: ERBT1312
+		 * When re-enabling interrupt (by writing 1
+		 * to clear the bit), the hardware might also try to set
+		 * the interrupt bit in the event status register.
+		 * In this scenario, the bit will be set, and disable
+		 * subsequent interrupts.
+		 *
+		 * Workaround: The IOMMU driver should read back the
+		 * status register and check if the interrupt bits are cleared.
+		 * If not, driver will need to go through the interrupt handler
+		 * again and re-clear the bits
+		 */
+		status = readl(iommu->mmio_base + MMIO_STATUS_OFFSET);
+	}
 	return IRQ_HANDLED;
 }
 
@@ -2839,24 +2839,6 @@ static void unmap_page(struct device *dev, dma_addr_t dma_addr, size_t size,
 }
 
 /*
- * This is a special map_sg function which is used if we should map a
- * device which is not handled by an AMD IOMMU in the system.
- */
-static int map_sg_no_iommu(struct device *dev, struct scatterlist *sglist,
-			   int nelems, int dir)
-{
-	struct scatterlist *s;
-	int i;
-
-	for_each_sg(sglist, s, nelems, i) {
-		s->dma_address = (dma_addr_t)sg_phys(s);
-		s->dma_length  = s->length;
-	}
-
-	return nelems;
-}
-
-/*
  * The exported map_sg function for dma_ops (handles scatter-gather
  * lists).
  */
@@ -2875,9 +2857,7 @@ static int map_sg(struct device *dev, struct scatterlist *sglist,
 	INC_STATS_COUNTER(cnt_map_sg);
 
 	domain = get_domain(dev);
-	if (PTR_ERR(domain) == -EINVAL)
-		return map_sg_no_iommu(dev, sglist, nelems, dir);
-	else if (IS_ERR(domain))
+	if (IS_ERR(domain))
 		return 0;
 
 	dma_mask = *dev->dma_mask;
@@ -3410,7 +3390,7 @@ static size_t amd_iommu_unmap(struct iommu_domain *dom, unsigned long iova,
 }
 
 static phys_addr_t amd_iommu_iova_to_phys(struct iommu_domain *dom,
-					  unsigned long iova)
+					  dma_addr_t iova)
 {
 	struct protection_domain *domain = dom->priv;
 	unsigned long offset_mask;
@@ -3947,6 +3927,9 @@ static struct irq_remap_table *get_irq_table(u16 devid, bool ioapic)
 	if (!table)
 		goto out;
 
+	/* Initialize table spin-lock */
+	spin_lock_init(&table->lock);
+
 	if (ioapic)
 		/* Keep the first 32 indexes free for IOAPIC interrupts */
 		table->min_index = 32;
@@ -4007,7 +3990,7 @@ static int alloc_irq_index(struct irq_cfg *cfg, u16 devid, int count)
 			c = 0;
 
 		if (c == count)	{
-			struct irq_2_iommu *irte_info;
+			struct irq_2_irte *irte_info;
 
 			for (; c != 0; --c)
 				table->table[index - c + 1] = IRTE_ALLOCATED;
@@ -4015,9 +3998,9 @@ static int alloc_irq_index(struct irq_cfg *cfg, u16 devid, int count)
 			index -= count - 1;
 
 			cfg->remapped	      = 1;
-			irte_info             = &cfg->irq_2_iommu;
-			irte_info->sub_handle = devid;
-			irte_info->irte_index = index;
+			irte_info             = &cfg->irq_2_irte;
+			irte_info->devid      = devid;
+			irte_info->index      = index;
 
 			goto out;
 		}
@@ -4098,7 +4081,7 @@ static int setup_ioapic_entry(int irq, struct IO_APIC_route_entry *entry,
 			      struct io_apic_irq_attr *attr)
 {
 	struct irq_remap_table *table;
-	struct irq_2_iommu *irte_info;
+	struct irq_2_irte *irte_info;
 	struct irq_cfg *cfg;
 	union irte irte;
 	int ioapic_id;
@@ -4110,7 +4093,7 @@ static int setup_ioapic_entry(int irq, struct IO_APIC_route_entry *entry,
 	if (!cfg)
 		return -EINVAL;
 
-	irte_info = &cfg->irq_2_iommu;
+	irte_info = &cfg->irq_2_irte;
 	ioapic_id = mpc_ioapic_id(attr->ioapic);
 	devid     = get_ioapic_devid(ioapic_id);
 
@@ -4125,8 +4108,8 @@ static int setup_ioapic_entry(int irq, struct IO_APIC_route_entry *entry,
 
 	/* Setup IRQ remapping info */
 	cfg->remapped	      = 1;
-	irte_info->sub_handle = devid;
-	irte_info->irte_index = index;
+	irte_info->devid      = devid;
+	irte_info->index      = index;
 
 	/* Setup IRTE for IOMMU */
 	irte.val		= 0;
@@ -4160,7 +4143,7 @@ static int setup_ioapic_entry(int irq, struct IO_APIC_route_entry *entry,
 static int set_affinity(struct irq_data *data, const struct cpumask *mask,
 			bool force)
 {
-	struct irq_2_iommu *irte_info;
+	struct irq_2_irte *irte_info;
 	unsigned int dest, irq;
 	struct irq_cfg *cfg;
 	union irte irte;
@@ -4171,12 +4154,12 @@ static int set_affinity(struct irq_data *data, const struct cpumask *mask,
 
 	cfg       = data->chip_data;
 	irq       = data->irq;
-	irte_info = &cfg->irq_2_iommu;
+	irte_info = &cfg->irq_2_irte;
 
 	if (!cpumask_intersects(mask, cpu_online_mask))
 		return -EINVAL;
 
-	if (get_irte(irte_info->sub_handle, irte_info->irte_index, &irte))
+	if (get_irte(irte_info->devid, irte_info->index, &irte))
 		return -EBUSY;
 
 	if (assign_irq_vector(irq, cfg, mask))
@@ -4192,7 +4175,7 @@ static int set_affinity(struct irq_data *data, const struct cpumask *mask,
 	irte.fields.vector      = cfg->vector;
 	irte.fields.destination = dest;
 
-	modify_irte(irte_info->sub_handle, irte_info->irte_index, irte);
+	modify_irte(irte_info->devid, irte_info->index, irte);
 
 	if (cfg->move_in_progress)
 		send_cleanup_vector(cfg);
@@ -4204,16 +4187,16 @@ static int set_affinity(struct irq_data *data, const struct cpumask *mask,
 
 static int free_irq(int irq)
 {
-	struct irq_2_iommu *irte_info;
+	struct irq_2_irte *irte_info;
 	struct irq_cfg *cfg;
 
 	cfg = irq_get_chip_data(irq);
 	if (!cfg)
 		return -EINVAL;
 
-	irte_info = &cfg->irq_2_iommu;
+	irte_info = &cfg->irq_2_irte;
 
-	free_irte(irte_info->sub_handle, irte_info->irte_index);
+	free_irte(irte_info->devid, irte_info->index);
 
 	return 0;
 }
@@ -4222,7 +4205,7 @@ static void compose_msi_msg(struct pci_dev *pdev,
 			    unsigned int irq, unsigned int dest,
 			    struct msi_msg *msg, u8 hpet_id)
 {
-	struct irq_2_iommu *irte_info;
+	struct irq_2_irte *irte_info;
 	struct irq_cfg *cfg;
 	union irte irte;
 
@@ -4230,7 +4213,7 @@ static void compose_msi_msg(struct pci_dev *pdev,
 	if (!cfg)
 		return;
 
-	irte_info = &cfg->irq_2_iommu;
+	irte_info = &cfg->irq_2_irte;
 
 	irte.val		= 0;
 	irte.fields.vector	= cfg->vector;
@@ -4239,11 +4222,11 @@ static void compose_msi_msg(struct pci_dev *pdev,
 	irte.fields.dm		= apic->irq_dest_mode;
 	irte.fields.valid	= 1;
 
-	modify_irte(irte_info->sub_handle, irte_info->irte_index, irte);
+	modify_irte(irte_info->devid, irte_info->index, irte);
 
 	msg->address_hi = MSI_ADDR_BASE_HI;
 	msg->address_lo = MSI_ADDR_BASE_LO;
-	msg->data       = irte_info->irte_index;
+	msg->data       = irte_info->index;
 }
 
 static int msi_alloc_irq(struct pci_dev *pdev, int irq, int nvec)
@@ -4268,7 +4251,7 @@ static int msi_alloc_irq(struct pci_dev *pdev, int irq, int nvec)
 static int msi_setup_irq(struct pci_dev *pdev, unsigned int irq,
 			 int index, int offset)
 {
-	struct irq_2_iommu *irte_info;
+	struct irq_2_irte *irte_info;
 	struct irq_cfg *cfg;
 	u16 devid;
 
@@ -4283,18 +4266,18 @@ static int msi_setup_irq(struct pci_dev *pdev, unsigned int irq,
 		return 0;
 
 	devid		= get_device_id(&pdev->dev);
-	irte_info	= &cfg->irq_2_iommu;
+	irte_info	= &cfg->irq_2_irte;
 
 	cfg->remapped	      = 1;
-	irte_info->sub_handle = devid;
-	irte_info->irte_index = index + offset;
+	irte_info->devid      = devid;
+	irte_info->index      = index + offset;
 
 	return 0;
 }
 
 static int setup_hpet_msi(unsigned int irq, unsigned int id)
 {
-	struct irq_2_iommu *irte_info;
+	struct irq_2_irte *irte_info;
 	struct irq_cfg *cfg;
 	int index, devid;
 
@@ -4302,7 +4285,7 @@ static int setup_hpet_msi(unsigned int irq, unsigned int id)
 	if (!cfg)
 		return -EINVAL;
 
-	irte_info = &cfg->irq_2_iommu;
+	irte_info = &cfg->irq_2_irte;
 	devid     = get_hpet_devid(id);
 	if (devid < 0)
 		return devid;
@@ -4312,8 +4295,8 @@ static int setup_hpet_msi(unsigned int irq, unsigned int id)
 		return index;
 
 	cfg->remapped	      = 1;
-	irte_info->sub_handle = devid;
-	irte_info->irte_index = index;
+	irte_info->devid      = devid;
+	irte_info->index      = index;
 
 	return 0;
 }
diff --git a/drivers/iommu/amd_iommu_init.c b/drivers/iommu/amd_iommu_init.c
index 2f46881256a2..bf51abb78dee 100644
--- a/drivers/iommu/amd_iommu_init.c
+++ b/drivers/iommu/amd_iommu_init.c
@@ -213,6 +213,14 @@ enum iommu_init_state {
 	IOMMU_INIT_ERROR,
 };
 
+/* Early ioapic and hpet maps from kernel command line */
+#define EARLY_MAP_SIZE		4
+static struct devid_map __initdata early_ioapic_map[EARLY_MAP_SIZE];
+static struct devid_map __initdata early_hpet_map[EARLY_MAP_SIZE];
+static int __initdata early_ioapic_map_size;
+static int __initdata early_hpet_map_size;
+static bool __initdata cmdline_maps;
+
 static enum iommu_init_state init_state = IOMMU_START_STATE;
 
 static int amd_iommu_enable_interrupts(void);
@@ -703,31 +711,66 @@ static void __init set_dev_entry_from_acpi(struct amd_iommu *iommu,
 	set_iommu_for_device(iommu, devid);
 }
 
-static int add_special_device(u8 type, u8 id, u16 devid)
+static int __init add_special_device(u8 type, u8 id, u16 devid, bool cmd_line)
 {
 	struct devid_map *entry;
 	struct list_head *list;
 
-	if (type != IVHD_SPECIAL_IOAPIC && type != IVHD_SPECIAL_HPET)
+	if (type == IVHD_SPECIAL_IOAPIC)
+		list = &ioapic_map;
+	else if (type == IVHD_SPECIAL_HPET)
+		list = &hpet_map;
+	else
 		return -EINVAL;
 
+	list_for_each_entry(entry, list, list) {
+		if (!(entry->id == id && entry->cmd_line))
+			continue;
+
+		pr_info("AMD-Vi: Command-line override present for %s id %d - ignoring\n",
+			type == IVHD_SPECIAL_IOAPIC ? "IOAPIC" : "HPET", id);
+
+		return 0;
+	}
+
 	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
 	if (!entry)
 		return -ENOMEM;
 
-	entry->id    = id;
-	entry->devid = devid;
-
-	if (type == IVHD_SPECIAL_IOAPIC)
-		list = &ioapic_map;
-	else
-		list = &hpet_map;
+	entry->id	= id;
+	entry->devid	= devid;
+	entry->cmd_line	= cmd_line;
 
 	list_add_tail(&entry->list, list);
 
 	return 0;
 }
 
+static int __init add_early_maps(void)
+{
+	int i, ret;
+
+	for (i = 0; i < early_ioapic_map_size; ++i) {
+		ret = add_special_device(IVHD_SPECIAL_IOAPIC,
+					 early_ioapic_map[i].id,
+					 early_ioapic_map[i].devid,
+					 early_ioapic_map[i].cmd_line);
+		if (ret)
+			return ret;
+	}
+
+	for (i = 0; i < early_hpet_map_size; ++i) {
+		ret = add_special_device(IVHD_SPECIAL_HPET,
+					 early_hpet_map[i].id,
+					 early_hpet_map[i].devid,
+					 early_hpet_map[i].cmd_line);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
 /*
  * Reads the device exclusion range from ACPI and initializes the IOMMU with
  * it
@@ -764,6 +807,12 @@ static int __init init_iommu_from_acpi(struct amd_iommu *iommu,
 	u32 dev_i, ext_flags = 0;
 	bool alias = false;
 	struct ivhd_entry *e;
+	int ret;
+
+
+	ret = add_early_maps();
+	if (ret)
+		return ret;
 
 	/*
 	 * First save the recommended feature enable bits from ACPI
@@ -929,7 +978,7 @@ static int __init init_iommu_from_acpi(struct amd_iommu *iommu,
 				    PCI_FUNC(devid));
 
 			set_dev_entry_from_acpi(iommu, devid, e->flags, 0);
-			ret = add_special_device(type, handle, devid);
+			ret = add_special_device(type, handle, devid, false);
 			if (ret)
 				return ret;
 			break;
@@ -1275,7 +1324,7 @@ static int iommu_setup_msi(struct amd_iommu *iommu)
 				 amd_iommu_int_handler,
 				 amd_iommu_int_thread,
 				 0, "AMD-Vi",
-				 iommu->dev);
+				 iommu);
 
 	if (r) {
 		pci_disable_msi(iommu->dev);
@@ -1638,18 +1687,28 @@ static void __init free_on_init_error(void)
 
 static bool __init check_ioapic_information(void)
 {
+	const char *fw_bug = FW_BUG;
 	bool ret, has_sb_ioapic;
 	int idx;
 
 	has_sb_ioapic = false;
 	ret           = false;
 
+	/*
+	 * If we have map overrides on the kernel command line the
+	 * messages in this function might not describe firmware bugs
+	 * anymore - so be careful
+	 */
+	if (cmdline_maps)
+		fw_bug = "";
+
 	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);
+			pr_err("%sAMD-Vi: IOAPIC[%d] not in IVRS table\n",
+				fw_bug, id);
 			ret = false;
 		} else if (devid == IOAPIC_SB_DEVID) {
 			has_sb_ioapic = true;
@@ -1666,11 +1725,11 @@ static bool __init check_ioapic_information(void)
 		 * 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");
+		pr_err("%sAMD-Vi: No southbridge IOAPIC found\n", fw_bug);
 	}
 
 	if (!ret)
-		pr_err("AMD-Vi: Disabling interrupt remapping due to BIOS Bug(s)\n");
+		pr_err("AMD-Vi: Disabling interrupt remapping\n");
 
 	return ret;
 }
@@ -1801,6 +1860,7 @@ static int __init early_amd_iommu_init(void)
 		 * Interrupt remapping enabled, create kmem_cache for the
 		 * remapping tables.
 		 */
+		ret = -ENOMEM;
 		amd_iommu_irq_cache = kmem_cache_create("irq_remap_cache",
 				MAX_IRQS_PER_TABLE * sizeof(u32),
 				IRQ_TABLE_ALIGNMENT,
@@ -2097,8 +2157,70 @@ static int __init parse_amd_iommu_options(char *str)
 	return 1;
 }
 
-__setup("amd_iommu_dump", parse_amd_iommu_dump);
-__setup("amd_iommu=", parse_amd_iommu_options);
+static int __init parse_ivrs_ioapic(char *str)
+{
+	unsigned int bus, dev, fn;
+	int ret, id, i;
+	u16 devid;
+
+	ret = sscanf(str, "[%d]=%x:%x.%x", &id, &bus, &dev, &fn);
+
+	if (ret != 4) {
+		pr_err("AMD-Vi: Invalid command line: ivrs_ioapic%s\n", str);
+		return 1;
+	}
+
+	if (early_ioapic_map_size == EARLY_MAP_SIZE) {
+		pr_err("AMD-Vi: Early IOAPIC map overflow - ignoring ivrs_ioapic%s\n",
+			str);
+		return 1;
+	}
+
+	devid = ((bus & 0xff) << 8) | ((dev & 0x1f) << 3) | (fn & 0x7);
+
+	cmdline_maps			= true;
+	i				= early_ioapic_map_size++;
+	early_ioapic_map[i].id		= id;
+	early_ioapic_map[i].devid	= devid;
+	early_ioapic_map[i].cmd_line	= true;
+
+	return 1;
+}
+
+static int __init parse_ivrs_hpet(char *str)
+{
+	unsigned int bus, dev, fn;
+	int ret, id, i;
+	u16 devid;
+
+	ret = sscanf(str, "[%d]=%x:%x.%x", &id, &bus, &dev, &fn);
+
+	if (ret != 4) {
+		pr_err("AMD-Vi: Invalid command line: ivrs_hpet%s\n", str);
+		return 1;
+	}
+
+	if (early_hpet_map_size == EARLY_MAP_SIZE) {
+		pr_err("AMD-Vi: Early HPET map overflow - ignoring ivrs_hpet%s\n",
+			str);
+		return 1;
+	}
+
+	devid = ((bus & 0xff) << 8) | ((dev & 0x1f) << 3) | (fn & 0x7);
+
+	cmdline_maps			= true;
+	i				= early_hpet_map_size++;
+	early_hpet_map[i].id		= id;
+	early_hpet_map[i].devid		= devid;
+	early_hpet_map[i].cmd_line	= true;
+
+	return 1;
+}
+
+__setup("amd_iommu_dump",	parse_amd_iommu_dump);
+__setup("amd_iommu=",		parse_amd_iommu_options);
+__setup("ivrs_ioapic",		parse_ivrs_ioapic);
+__setup("ivrs_hpet",		parse_ivrs_hpet);
 
 IOMMU_INIT_FINISH(amd_iommu_detect,
 		  gart_iommu_hole_init,
diff --git a/drivers/iommu/amd_iommu_types.h b/drivers/iommu/amd_iommu_types.h
index ec36cf63e0ca..0285a215df16 100644
--- a/drivers/iommu/amd_iommu_types.h
+++ b/drivers/iommu/amd_iommu_types.h
@@ -100,6 +100,7 @@
 #define PASID_MASK		0x000fffff
 
 /* MMIO status bits */
+#define MMIO_STATUS_EVT_INT_MASK	(1 << 1)
 #define MMIO_STATUS_COM_WAIT_INT_MASK	(1 << 2)
 #define MMIO_STATUS_PPR_INT_MASK	(1 << 6)
 
@@ -589,6 +590,7 @@ struct devid_map {
 	struct list_head list;
 	u8 id;
 	u16 devid;
+	bool cmd_line;
 };
 
 /* Map HPET and IOAPIC ids to the devid used by the IOMMU */
diff --git a/drivers/iommu/dmar.c b/drivers/iommu/dmar.c
index b8008f679bc7..a7967ceb79e6 100644
--- a/drivers/iommu/dmar.c
+++ b/drivers/iommu/dmar.c
@@ -646,7 +646,7 @@ out:
 int alloc_iommu(struct dmar_drhd_unit *drhd)
 {
 	struct intel_iommu *iommu;
-	u32 ver;
+	u32 ver, sts;
 	static int iommu_allocated = 0;
 	int agaw = 0;
 	int msagaw = 0;
@@ -696,6 +696,15 @@ int alloc_iommu(struct dmar_drhd_unit *drhd)
 		(unsigned long long)iommu->cap,
 		(unsigned long long)iommu->ecap);
 
+	/* Reflect status in gcmd */
+	sts = readl(iommu->reg + DMAR_GSTS_REG);
+	if (sts & DMA_GSTS_IRES)
+		iommu->gcmd |= DMA_GCMD_IRE;
+	if (sts & DMA_GSTS_TES)
+		iommu->gcmd |= DMA_GCMD_TE;
+	if (sts & DMA_GSTS_QIES)
+		iommu->gcmd |= DMA_GCMD_QIE;
+
 	raw_spin_lock_init(&iommu->register_lock);
 
 	drhd->iommu = iommu;
@@ -1205,7 +1214,7 @@ irqreturn_t dmar_fault(int irq, void *dev_id)
 
 	/* TBD: ignore advanced fault log currently */
 	if (!(fault_status & DMA_FSTS_PPF))
-		goto clear_rest;
+		goto unlock_exit;
 
 	fault_index = dma_fsts_fault_record_index(fault_status);
 	reg = cap_fault_reg_offset(iommu->cap);
@@ -1246,11 +1255,10 @@ irqreturn_t dmar_fault(int irq, void *dev_id)
 			fault_index = 0;
 		raw_spin_lock_irqsave(&iommu->register_lock, flag);
 	}
-clear_rest:
-	/* clear all the other faults */
-	fault_status = readl(iommu->reg + DMAR_FSTS_REG);
-	writel(fault_status, iommu->reg + DMAR_FSTS_REG);
 
+	writel(DMA_FSTS_PFO | DMA_FSTS_PPF, iommu->reg + DMAR_FSTS_REG);
+
+unlock_exit:
 	raw_spin_unlock_irqrestore(&iommu->register_lock, flag);
 	return IRQ_HANDLED;
 }
@@ -1298,6 +1306,7 @@ int __init enable_drhd_fault_handling(void)
 	for_each_drhd_unit(drhd) {
 		int ret;
 		struct intel_iommu *iommu = drhd->iommu;
+		u32 fault_status;
 		ret = dmar_set_interrupt(iommu);
 
 		if (ret) {
@@ -1310,6 +1319,8 @@ int __init enable_drhd_fault_handling(void)
 		 * Clear any previous faults.
 		 */
 		dmar_fault(iommu->irq, iommu);
+		fault_status = readl(iommu->reg + DMAR_FSTS_REG);
+		writel(fault_status, iommu->reg + DMAR_FSTS_REG);
 	}
 
 	return 0;
diff --git a/drivers/iommu/exynos-iommu.c b/drivers/iommu/exynos-iommu.c
index 238a3caa949a..3f32d64ab87a 100644
--- a/drivers/iommu/exynos-iommu.c
+++ b/drivers/iommu/exynos-iommu.c
@@ -1027,7 +1027,7 @@ done:
 }
 
 static phys_addr_t exynos_iommu_iova_to_phys(struct iommu_domain *domain,
-					  unsigned long iova)
+					  dma_addr_t iova)
 {
 	struct exynos_iommu_domain *priv = domain->priv;
 	unsigned long *entry;
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index 0099667a397e..b4f0e28dfa41 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -47,6 +47,7 @@
 #include <asm/iommu.h>
 
 #include "irq_remapping.h"
+#include "pci.h"
 
 #define ROOT_SIZE		VTD_PAGE_SIZE
 #define CONTEXT_SIZE		VTD_PAGE_SIZE
@@ -3665,6 +3666,7 @@ static struct notifier_block device_nb = {
 int __init intel_iommu_init(void)
 {
 	int ret = 0;
+	struct dmar_drhd_unit *drhd;
 
 	/* VT-d is required for a TXT/tboot launch, so enforce that */
 	force_on = tboot_force_iommu();
@@ -3675,6 +3677,20 @@ int __init intel_iommu_init(void)
 		return 	-ENODEV;
 	}
 
+	/*
+	 * Disable translation if already enabled prior to OS handover.
+	 */
+	for_each_drhd_unit(drhd) {
+		struct intel_iommu *iommu;
+
+		if (drhd->ignored)
+			continue;
+
+		iommu = drhd->iommu;
+		if (iommu->gcmd & DMA_GCMD_TE)
+			iommu_disable_translation(iommu);
+	}
+
 	if (dmar_dev_scope_init() < 0) {
 		if (force_on)
 			panic("tboot: Failed to initialize DMAR device scope\n");
@@ -4111,7 +4127,7 @@ static size_t intel_iommu_unmap(struct iommu_domain *domain,
 }
 
 static phys_addr_t intel_iommu_iova_to_phys(struct iommu_domain *domain,
-					    unsigned long iova)
+					    dma_addr_t iova)
 {
 	struct dmar_domain *dmar_domain = domain->priv;
 	struct dma_pte *pte;
@@ -4137,12 +4153,6 @@ static int intel_iommu_domain_has_cap(struct iommu_domain *domain,
 	return 0;
 }
 
-static void swap_pci_ref(struct pci_dev **from, struct pci_dev *to)
-{
-	pci_dev_put(*from);
-	*from = to;
-}
-
 #define REQ_ACS_FLAGS	(PCI_ACS_SV | PCI_ACS_RR | PCI_ACS_CR | PCI_ACS_UF)
 
 static int intel_iommu_add_device(struct device *dev)
diff --git a/drivers/iommu/intel_irq_remapping.c b/drivers/iommu/intel_irq_remapping.c
index f3b8f23b5d8f..5b19b2d6ec2d 100644
--- a/drivers/iommu/intel_irq_remapping.c
+++ b/drivers/iommu/intel_irq_remapping.c
@@ -524,6 +524,16 @@ static int __init intel_irq_remapping_supported(void)
 
 	if (disable_irq_remap)
 		return 0;
+	if (irq_remap_broken) {
+		WARN_TAINT(1, TAINT_FIRMWARE_WORKAROUND,
+			   "This system BIOS has enabled interrupt remapping\n"
+			   "on a chipset that contains an erratum making that\n"
+			   "feature unstable.  To maintain system stability\n"
+			   "interrupt remapping is being disabled.  Please\n"
+			   "contact your BIOS vendor for an update\n");
+		disable_irq_remap = 1;
+		return 0;
+	}
 
 	if (!dmar_ir_support())
 		return 0;
diff --git a/drivers/iommu/iommu.c b/drivers/iommu/iommu.c
index b972d430d92b..d8f98b14e2fe 100644
--- a/drivers/iommu/iommu.c
+++ b/drivers/iommu/iommu.c
@@ -204,6 +204,35 @@ again:
 }
 EXPORT_SYMBOL_GPL(iommu_group_alloc);
 
+struct iommu_group *iommu_group_get_by_id(int id)
+{
+	struct kobject *group_kobj;
+	struct iommu_group *group;
+	const char *name;
+
+	if (!iommu_group_kset)
+		return NULL;
+
+	name = kasprintf(GFP_KERNEL, "%d", id);
+	if (!name)
+		return NULL;
+
+	group_kobj = kset_find_obj(iommu_group_kset, name);
+	kfree(name);
+
+	if (!group_kobj)
+		return NULL;
+
+	group = container_of(group_kobj, struct iommu_group, kobj);
+	BUG_ON(group->id != id);
+
+	kobject_get(group->devices_kobj);
+	kobject_put(&group->kobj);
+
+	return group;
+}
+EXPORT_SYMBOL_GPL(iommu_group_get_by_id);
+
 /**
  * iommu_group_get_iommudata - retrieve iommu_data registered for a group
  * @group: the group
@@ -706,8 +735,7 @@ void iommu_detach_group(struct iommu_domain *domain, struct iommu_group *group)
 }
 EXPORT_SYMBOL_GPL(iommu_detach_group);
 
-phys_addr_t iommu_iova_to_phys(struct iommu_domain *domain,
-			       unsigned long iova)
+phys_addr_t iommu_iova_to_phys(struct iommu_domain *domain, dma_addr_t iova)
 {
 	if (unlikely(domain->ops->iova_to_phys == NULL))
 		return 0;
@@ -854,12 +882,13 @@ EXPORT_SYMBOL_GPL(iommu_unmap);
 
 
 int iommu_domain_window_enable(struct iommu_domain *domain, u32 wnd_nr,
-			       phys_addr_t paddr, u64 size)
+			       phys_addr_t paddr, u64 size, int prot)
 {
 	if (unlikely(domain->ops->domain_window_enable == NULL))
 		return -ENODEV;
 
-	return domain->ops->domain_window_enable(domain, wnd_nr, paddr, size);
+	return domain->ops->domain_window_enable(domain, wnd_nr, paddr, size,
+						 prot);
 }
 EXPORT_SYMBOL_GPL(iommu_domain_window_enable);
 
diff --git a/drivers/iommu/irq_remapping.c b/drivers/iommu/irq_remapping.c
index 7c11ff368d07..dcfea4e39be7 100644
--- a/drivers/iommu/irq_remapping.c
+++ b/drivers/iommu/irq_remapping.c
@@ -18,6 +18,7 @@
 int irq_remapping_enabled;
 
 int disable_irq_remap;
+int irq_remap_broken;
 int disable_sourceid_checking;
 int no_x2apic_optout;
 
@@ -210,6 +211,11 @@ void __init setup_irq_remapping_ops(void)
 #endif
 }
 
+void set_irq_remapping_broken(void)
+{
+	irq_remap_broken = 1;
+}
+
 int irq_remapping_supported(void)
 {
 	if (disable_irq_remap)
diff --git a/drivers/iommu/irq_remapping.h b/drivers/iommu/irq_remapping.h
index ecb637670405..90c4dae5a46b 100644
--- a/drivers/iommu/irq_remapping.h
+++ b/drivers/iommu/irq_remapping.h
@@ -32,6 +32,7 @@ struct pci_dev;
 struct msi_msg;
 
 extern int disable_irq_remap;
+extern int irq_remap_broken;
 extern int disable_sourceid_checking;
 extern int no_x2apic_optout;
 extern int irq_remapping_enabled;
@@ -89,6 +90,7 @@ extern struct irq_remap_ops amd_iommu_irq_ops;
 
 #define irq_remapping_enabled 0
 #define disable_irq_remap     1
+#define irq_remap_broken      0
 
 #endif /* CONFIG_IRQ_REMAP */
 
diff --git a/drivers/iommu/msm_iommu.c b/drivers/iommu/msm_iommu.c
index 6a8870a31668..8ab4f41090af 100644
--- a/drivers/iommu/msm_iommu.c
+++ b/drivers/iommu/msm_iommu.c
@@ -554,7 +554,7 @@ fail:
 }
 
 static phys_addr_t msm_iommu_iova_to_phys(struct iommu_domain *domain,
-					  unsigned long va)
+					  dma_addr_t va)
 {
 	struct msm_priv *priv;
 	struct msm_iommu_drvdata *iommu_drvdata;
diff --git a/drivers/iommu/omap-iommu.c b/drivers/iommu/omap-iommu.c
index 6ac02fa5910f..e02e5d71745b 100644
--- a/drivers/iommu/omap-iommu.c
+++ b/drivers/iommu/omap-iommu.c
@@ -1219,7 +1219,7 @@ static void omap_iommu_domain_destroy(struct iommu_domain *domain)
 }
 
 static phys_addr_t omap_iommu_iova_to_phys(struct iommu_domain *domain,
-					  unsigned long da)
+					  dma_addr_t da)
 {
 	struct omap_iommu_domain *omap_domain = domain->priv;
 	struct omap_iommu *oiommu = omap_domain->iommu_dev;
diff --git a/drivers/iommu/pci.h b/drivers/iommu/pci.h
new file mode 100644
index 000000000000..352d80ae7443
--- /dev/null
+++ b/drivers/iommu/pci.h
@@ -0,0 +1,29 @@
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ * Copyright (C) 2013 Red Hat, Inc.
+ * Copyright (C) 2013 Freescale Semiconductor, Inc.
+ *
+ */
+#ifndef __IOMMU_PCI_H
+#define __IOMMU_PCI_H
+
+/* Helper function for swapping pci device reference */
+static inline void swap_pci_ref(struct pci_dev **from, struct pci_dev *to)
+{
+	pci_dev_put(*from);
+	*from = to;
+}
+
+#endif  /* __IOMMU_PCI_H */
diff --git a/drivers/iommu/shmobile-iommu.c b/drivers/iommu/shmobile-iommu.c
index b6e8b57cf0a8..d572863dfccd 100644
--- a/drivers/iommu/shmobile-iommu.c
+++ b/drivers/iommu/shmobile-iommu.c
@@ -296,7 +296,7 @@ done:
 }
 
 static phys_addr_t shmobile_iommu_iova_to_phys(struct iommu_domain *domain,
-					       unsigned long iova)
+					       dma_addr_t iova)
 {
 	struct shmobile_iommu_domain *sh_domain = domain->priv;
 	uint32_t l1entry = 0, l2entry = 0;
diff --git a/drivers/iommu/tegra-gart.c b/drivers/iommu/tegra-gart.c
index 86437575f94d..108c0e9c24d9 100644
--- a/drivers/iommu/tegra-gart.c
+++ b/drivers/iommu/tegra-gart.c
@@ -279,7 +279,7 @@ static size_t gart_iommu_unmap(struct iommu_domain *domain, unsigned long iova,
 }
 
 static phys_addr_t gart_iommu_iova_to_phys(struct iommu_domain *domain,
-					   unsigned long iova)
+					   dma_addr_t iova)
 {
 	struct gart_device *gart = domain->priv;
 	unsigned long pte;
@@ -295,7 +295,8 @@ static phys_addr_t gart_iommu_iova_to_phys(struct iommu_domain *domain,
 
 	pa = (pte & GART_PAGE_MASK);
 	if (!pfn_valid(__phys_to_pfn(pa))) {
-		dev_err(gart->dev, "No entry for %08lx:%08x\n", iova, pa);
+		dev_err(gart->dev, "No entry for %08llx:%08x\n",
+			 (unsigned long long)iova, pa);
 		gart_dump_table(gart);
 		return -EINVAL;
 	}
diff --git a/drivers/iommu/tegra-smmu.c b/drivers/iommu/tegra-smmu.c
index b34e5fd7fd9e..f6f120e25409 100644
--- a/drivers/iommu/tegra-smmu.c
+++ b/drivers/iommu/tegra-smmu.c
@@ -757,7 +757,7 @@ static size_t smmu_iommu_unmap(struct iommu_domain *domain, unsigned long iova,
 }
 
 static phys_addr_t smmu_iommu_iova_to_phys(struct iommu_domain *domain,
-					   unsigned long iova)
+					   dma_addr_t iova)
 {
 	struct smmu_as *as = domain->priv;
 	unsigned long *pte;
@@ -772,7 +772,8 @@ static phys_addr_t smmu_iommu_iova_to_phys(struct iommu_domain *domain,
 	pfn = *pte & SMMU_PFN_MASK;
 	WARN_ON(!pfn_valid(pfn));
 	dev_dbg(as->smmu->dev,
-		"iova:%08lx pfn:%08lx asid:%d\n", iova, pfn, as->asid);
+		"iova:%08llx pfn:%08lx asid:%d\n", (unsigned long long)iova,
+		 pfn, as->asid);
 
 	spin_unlock_irqrestore(&as->lock, flags);
 	return PFN_PHYS(pfn);
diff --git a/drivers/irqchip/Kconfig b/drivers/irqchip/Kconfig
index a350969e5efe..4a33351c25dc 100644
--- a/drivers/irqchip/Kconfig
+++ b/drivers/irqchip/Kconfig
@@ -25,6 +25,14 @@ config ARM_VIC_NR
 	  The maximum number of VICs available in the system, for
 	  power management.
 
+config RENESAS_INTC_IRQPIN
+	bool
+	select IRQ_DOMAIN
+
+config RENESAS_IRQC
+	bool
+	select IRQ_DOMAIN
+
 config VERSATILE_FPGA_IRQ
 	bool
 	select IRQ_DOMAIN
diff --git a/drivers/irqchip/Makefile b/drivers/irqchip/Makefile
index 98e3b87bdf1b..cda4cb5f7327 100644
--- a/drivers/irqchip/Makefile
+++ b/drivers/irqchip/Makefile
@@ -2,10 +2,17 @@ obj-$(CONFIG_IRQCHIP)			+= irqchip.o
 
 obj-$(CONFIG_ARCH_BCM2835)		+= irq-bcm2835.o
 obj-$(CONFIG_ARCH_EXYNOS)		+= exynos-combiner.o
+obj-$(CONFIG_ARCH_MVEBU)		+= irq-armada-370-xp.o
+obj-$(CONFIG_ARCH_MXS)			+= irq-mxs.o
+obj-$(CONFIG_ARCH_S3C24XX)		+= irq-s3c24xx.o
 obj-$(CONFIG_METAG)			+= irq-metag-ext.o
 obj-$(CONFIG_METAG_PERFCOUNTER_IRQS)	+= irq-metag.o
-obj-$(CONFIG_ARCH_SUNXI)		+= irq-sunxi.o
+obj-$(CONFIG_ARCH_SUNXI)		+= irq-sun4i.o
 obj-$(CONFIG_ARCH_SPEAR3XX)		+= spear-shirq.o
 obj-$(CONFIG_ARM_GIC)			+= irq-gic.o
 obj-$(CONFIG_ARM_VIC)			+= irq-vic.o
+obj-$(CONFIG_SIRF_IRQ)			+= irq-sirfsoc.o
+obj-$(CONFIG_RENESAS_INTC_IRQPIN)	+= irq-renesas-intc-irqpin.o
+obj-$(CONFIG_RENESAS_IRQC)		+= irq-renesas-irqc.o
 obj-$(CONFIG_VERSATILE_FPGA_IRQ)	+= irq-versatile-fpga.o
+obj-$(CONFIG_ARCH_VT8500)		+= irq-vt8500.o
diff --git a/drivers/irqchip/exynos-combiner.c b/drivers/irqchip/exynos-combiner.c
index 04d86a9803f4..a9d2b2fa4afd 100644
--- a/drivers/irqchip/exynos-combiner.c
+++ b/drivers/irqchip/exynos-combiner.c
@@ -12,12 +12,16 @@
 #include <linux/export.h>
 #include <linux/init.h>
 #include <linux/io.h>
+#include <linux/slab.h>
 #include <linux/irqdomain.h>
+#include <linux/irqchip/chained_irq.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
 #include <asm/mach/irq.h>
 
+#ifdef CONFIG_EXYNOS_ATAGS
 #include <plat/cpu.h>
+#endif
 
 #include "irqchip.h"
 
@@ -25,16 +29,18 @@
 #define COMBINER_ENABLE_CLEAR	0x4
 #define COMBINER_INT_STATUS	0xC
 
+#define IRQ_IN_COMBINER		8
+
 static DEFINE_SPINLOCK(irq_controller_lock);
 
 struct combiner_chip_data {
-	unsigned int irq_offset;
+	unsigned int hwirq_offset;
 	unsigned int irq_mask;
 	void __iomem *base;
+	unsigned int parent_irq;
 };
 
 static struct irq_domain *combiner_irq_domain;
-static struct combiner_chip_data combiner_data[MAX_COMBINER_NR];
 
 static inline void __iomem *combiner_base(struct irq_data *data)
 {
@@ -75,11 +81,11 @@ static void combiner_handle_cascade_irq(unsigned int irq, struct irq_desc *desc)
 	if (status == 0)
 		goto out;
 
-	combiner_irq = __ffs(status);
+	combiner_irq = chip_data->hwirq_offset + __ffs(status);
+	cascade_irq = irq_find_mapping(combiner_irq_domain, combiner_irq);
 
-	cascade_irq = combiner_irq + (chip_data->irq_offset & ~31);
-	if (unlikely(cascade_irq >= NR_IRQS))
-		do_bad_IRQ(cascade_irq, desc);
+	if (unlikely(!cascade_irq))
+		do_bad_IRQ(irq, desc);
 	else
 		generic_handle_irq(cascade_irq);
 
@@ -87,39 +93,49 @@ static void combiner_handle_cascade_irq(unsigned int irq, struct irq_desc *desc)
 	chained_irq_exit(chip, desc);
 }
 
-static struct irq_chip combiner_chip = {
-	.name		= "COMBINER",
-	.irq_mask	= combiner_mask_irq,
-	.irq_unmask	= combiner_unmask_irq,
-};
-
-static void __init combiner_cascade_irq(unsigned int combiner_nr, unsigned int irq)
+#ifdef CONFIG_SMP
+static int combiner_set_affinity(struct irq_data *d,
+				 const struct cpumask *mask_val, bool force)
 {
-	unsigned int max_nr;
+	struct combiner_chip_data *chip_data = irq_data_get_irq_chip_data(d);
+	struct irq_chip *chip = irq_get_chip(chip_data->parent_irq);
+	struct irq_data *data = irq_get_irq_data(chip_data->parent_irq);
 
-	if (soc_is_exynos5250())
-		max_nr = EXYNOS5_MAX_COMBINER_NR;
+	if (chip && chip->irq_set_affinity)
+		return chip->irq_set_affinity(data, mask_val, force);
 	else
-		max_nr = EXYNOS4_MAX_COMBINER_NR;
+		return -EINVAL;
+}
+#endif
 
-	if (combiner_nr >= max_nr)
-		BUG();
-	if (irq_set_handler_data(irq, &combiner_data[combiner_nr]) != 0)
+static struct irq_chip combiner_chip = {
+	.name			= "COMBINER",
+	.irq_mask		= combiner_mask_irq,
+	.irq_unmask		= combiner_unmask_irq,
+#ifdef CONFIG_SMP
+	.irq_set_affinity	= combiner_set_affinity,
+#endif
+};
+
+static void __init combiner_cascade_irq(struct combiner_chip_data *combiner_data,
+					unsigned int irq)
+{
+	if (irq_set_handler_data(irq, combiner_data) != 0)
 		BUG();
 	irq_set_chained_handler(irq, combiner_handle_cascade_irq);
 }
 
-static void __init combiner_init_one(unsigned int combiner_nr,
-				     void __iomem *base)
+static void __init combiner_init_one(struct combiner_chip_data *combiner_data,
+				     unsigned int combiner_nr,
+				     void __iomem *base, unsigned int irq)
 {
-	combiner_data[combiner_nr].base = base;
-	combiner_data[combiner_nr].irq_offset = irq_find_mapping(
-		combiner_irq_domain, combiner_nr * MAX_IRQ_IN_COMBINER);
-	combiner_data[combiner_nr].irq_mask = 0xff << ((combiner_nr % 4) << 3);
+	combiner_data->base = base;
+	combiner_data->hwirq_offset = (combiner_nr & ~3) * IRQ_IN_COMBINER;
+	combiner_data->irq_mask = 0xff << ((combiner_nr % 4) << 3);
+	combiner_data->parent_irq = irq;
 
 	/* Disable all interrupts */
-	__raw_writel(combiner_data[combiner_nr].irq_mask,
-		     base + COMBINER_ENABLE_CLEAR);
+	__raw_writel(combiner_data->irq_mask, base + COMBINER_ENABLE_CLEAR);
 }
 
 #ifdef CONFIG_OF
@@ -135,7 +151,7 @@ static int combiner_irq_domain_xlate(struct irq_domain *d,
 	if (intsize < 2)
 		return -EINVAL;
 
-	*out_hwirq = intspec[0] * MAX_IRQ_IN_COMBINER + intspec[1];
+	*out_hwirq = intspec[0] * IRQ_IN_COMBINER + intspec[1];
 	*out_type = 0;
 
 	return 0;
@@ -154,6 +170,8 @@ static int combiner_irq_domain_xlate(struct irq_domain *d,
 static int combiner_irq_domain_map(struct irq_domain *d, unsigned int irq,
 				   irq_hw_number_t hw)
 {
+	struct combiner_chip_data *combiner_data = d->host_data;
+
 	irq_set_chip_and_handler(irq, &combiner_chip, handle_level_irq);
 	irq_set_chip_data(irq, &combiner_data[hw >> 3]);
 	set_irq_flags(irq, IRQF_VALID | IRQF_PROBE);
@@ -166,46 +184,61 @@ static struct irq_domain_ops combiner_irq_domain_ops = {
 	.map	= combiner_irq_domain_map,
 };
 
-void __init combiner_init(void __iomem *combiner_base,
-			  struct device_node *np)
+static unsigned int combiner_lookup_irq(int group)
 {
-	int i, irq, irq_base;
-	unsigned int max_nr, nr_irq;
-
-	if (np) {
-		if (of_property_read_u32(np, "samsung,combiner-nr", &max_nr)) {
-			pr_warning("%s: number of combiners not specified, "
-				"setting default as %d.\n",
-				__func__, EXYNOS4_MAX_COMBINER_NR);
-			max_nr = EXYNOS4_MAX_COMBINER_NR;
-		}
-	} else {
-		max_nr = soc_is_exynos5250() ? EXYNOS5_MAX_COMBINER_NR :
-						EXYNOS4_MAX_COMBINER_NR;
+#ifdef CONFIG_EXYNOS_ATAGS
+	if (group < EXYNOS4210_MAX_COMBINER_NR || soc_is_exynos5250())
+		return IRQ_SPI(group);
+
+	switch (group) {
+	case 16:
+		return IRQ_SPI(107);
+	case 17:
+		return IRQ_SPI(108);
+	case 18:
+		return IRQ_SPI(48);
+	case 19:
+		return IRQ_SPI(42);
 	}
-	nr_irq = max_nr * MAX_IRQ_IN_COMBINER;
+#endif
+	return 0;
+}
+
+void __init combiner_init(void __iomem *combiner_base,
+			  struct device_node *np,
+			  unsigned int max_nr,
+			  int irq_base)
+{
+	int i, irq;
+	unsigned int nr_irq;
+	struct combiner_chip_data *combiner_data;
+
+	nr_irq = max_nr * IRQ_IN_COMBINER;
 
-	irq_base = irq_alloc_descs(COMBINER_IRQ(0, 0), 1, nr_irq, 0);
-	if (IS_ERR_VALUE(irq_base)) {
-		irq_base = COMBINER_IRQ(0, 0);
-		pr_warning("%s: irq desc alloc failed. Continuing with %d as linux irq base\n", __func__, irq_base);
+	combiner_data = kcalloc(max_nr, sizeof (*combiner_data), GFP_KERNEL);
+	if (!combiner_data) {
+		pr_warning("%s: could not allocate combiner data\n", __func__);
+		return;
 	}
 
-	combiner_irq_domain = irq_domain_add_legacy(np, nr_irq, irq_base, 0,
-				&combiner_irq_domain_ops, &combiner_data);
+	combiner_irq_domain = irq_domain_add_simple(np, nr_irq, irq_base,
+				&combiner_irq_domain_ops, combiner_data);
 	if (WARN_ON(!combiner_irq_domain)) {
 		pr_warning("%s: irq domain init failed\n", __func__);
 		return;
 	}
 
 	for (i = 0; i < max_nr; i++) {
-		combiner_init_one(i, combiner_base + (i >> 2) * 0x10);
-		irq = IRQ_SPI(i);
 #ifdef CONFIG_OF
 		if (np)
 			irq = irq_of_parse_and_map(np, i);
+		else
 #endif
-		combiner_cascade_irq(i, irq);
+			irq = combiner_lookup_irq(i);
+
+		combiner_init_one(&combiner_data[i], i,
+				  combiner_base + (i >> 2) * 0x10, irq);
+		combiner_cascade_irq(&combiner_data[i], irq);
 	}
 }
 
@@ -214,6 +247,8 @@ static int __init combiner_of_init(struct device_node *np,
 				   struct device_node *parent)
 {
 	void __iomem *combiner_base;
+	unsigned int max_nr = 20;
+	int irq_base = -1;
 
 	combiner_base = of_iomap(np, 0);
 	if (!combiner_base) {
@@ -221,7 +256,20 @@ static int __init combiner_of_init(struct device_node *np,
 		return -ENXIO;
 	}
 
-	combiner_init(combiner_base, np);
+	if (of_property_read_u32(np, "samsung,combiner-nr", &max_nr)) {
+		pr_info("%s: number of combiners not specified, "
+			"setting default as %d.\n",
+			__func__, max_nr);
+	}
+
+	/* 
+	 * FIXME: This is a hardwired COMBINER_IRQ(0,0). Once all devices
+	 * get their IRQ from DT, remove this in order to get dynamic
+	 * allocation.
+	 */
+	irq_base = 160;
+
+	combiner_init(combiner_base, np, max_nr, irq_base);
 
 	return 0;
 }
diff --git a/drivers/irqchip/irq-armada-370-xp.c b/drivers/irqchip/irq-armada-370-xp.c
new file mode 100644
index 000000000000..bb328a366122
--- /dev/null
+++ b/drivers/irqchip/irq-armada-370-xp.c
@@ -0,0 +1,288 @@
+/*
+ * Marvell Armada 370 and Armada XP SoC IRQ handling
+ *
+ * Copyright (C) 2012 Marvell
+ *
+ * Lior Amsalem <alior@marvell.com>
+ * Gregory CLEMENT <gregory.clement@free-electrons.com>
+ * Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ * Ben Dooks <ben.dooks@codethink.co.uk>
+ *
+ * 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/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/irqdomain.h>
+#include <asm/mach/arch.h>
+#include <asm/exception.h>
+#include <asm/smp_plat.h>
+#include <asm/mach/irq.h>
+
+#include "irqchip.h"
+
+/* Interrupt Controller Registers Map */
+#define ARMADA_370_XP_INT_SET_MASK_OFFS		(0x48)
+#define ARMADA_370_XP_INT_CLEAR_MASK_OFFS	(0x4C)
+
+#define ARMADA_370_XP_INT_CONTROL		(0x00)
+#define ARMADA_370_XP_INT_SET_ENABLE_OFFS	(0x30)
+#define ARMADA_370_XP_INT_CLEAR_ENABLE_OFFS	(0x34)
+#define ARMADA_370_XP_INT_SOURCE_CTL(irq)	(0x100 + irq*4)
+
+#define ARMADA_370_XP_CPU_INTACK_OFFS		(0x44)
+
+#define ARMADA_370_XP_SW_TRIG_INT_OFFS           (0x4)
+#define ARMADA_370_XP_IN_DRBEL_MSK_OFFS          (0xc)
+#define ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS        (0x8)
+
+#define ARMADA_370_XP_MAX_PER_CPU_IRQS		(28)
+
+#define ARMADA_370_XP_TIMER0_PER_CPU_IRQ	(5)
+
+#define IPI_DOORBELL_START                      (0)
+#define IPI_DOORBELL_END                        (8)
+#define IPI_DOORBELL_MASK                       0xFF
+
+static DEFINE_RAW_SPINLOCK(irq_controller_lock);
+
+static void __iomem *per_cpu_int_base;
+static void __iomem *main_int_base;
+static struct irq_domain *armada_370_xp_mpic_domain;
+
+/*
+ * In SMP mode:
+ * For shared global interrupts, mask/unmask global enable bit
+ * For CPU interrupts, mask/unmask the calling CPU's bit
+ */
+static void armada_370_xp_irq_mask(struct irq_data *d)
+{
+	irq_hw_number_t hwirq = irqd_to_hwirq(d);
+
+	if (hwirq != ARMADA_370_XP_TIMER0_PER_CPU_IRQ)
+		writel(hwirq, main_int_base +
+				ARMADA_370_XP_INT_CLEAR_ENABLE_OFFS);
+	else
+		writel(hwirq, per_cpu_int_base +
+				ARMADA_370_XP_INT_SET_MASK_OFFS);
+}
+
+static void armada_370_xp_irq_unmask(struct irq_data *d)
+{
+	irq_hw_number_t hwirq = irqd_to_hwirq(d);
+
+	if (hwirq != ARMADA_370_XP_TIMER0_PER_CPU_IRQ)
+		writel(hwirq, main_int_base +
+				ARMADA_370_XP_INT_SET_ENABLE_OFFS);
+	else
+		writel(hwirq, per_cpu_int_base +
+				ARMADA_370_XP_INT_CLEAR_MASK_OFFS);
+}
+
+#ifdef CONFIG_SMP
+static int armada_xp_set_affinity(struct irq_data *d,
+				  const struct cpumask *mask_val, bool force)
+{
+	unsigned long reg;
+	unsigned long new_mask = 0;
+	unsigned long online_mask = 0;
+	unsigned long count = 0;
+	irq_hw_number_t hwirq = irqd_to_hwirq(d);
+	int cpu;
+
+	for_each_cpu(cpu, mask_val) {
+		new_mask |= 1 << cpu_logical_map(cpu);
+		count++;
+	}
+
+	/*
+	 * Forbid mutlicore interrupt affinity
+	 * This is required since the MPIC HW doesn't limit
+	 * several CPUs from acknowledging the same interrupt.
+	 */
+	if (count > 1)
+		return -EINVAL;
+
+	for_each_cpu(cpu, cpu_online_mask)
+		online_mask |= 1 << cpu_logical_map(cpu);
+
+	raw_spin_lock(&irq_controller_lock);
+
+	reg = readl(main_int_base + ARMADA_370_XP_INT_SOURCE_CTL(hwirq));
+	reg = (reg & (~online_mask)) | new_mask;
+	writel(reg, main_int_base + ARMADA_370_XP_INT_SOURCE_CTL(hwirq));
+
+	raw_spin_unlock(&irq_controller_lock);
+
+	return 0;
+}
+#endif
+
+static struct irq_chip armada_370_xp_irq_chip = {
+	.name		= "armada_370_xp_irq",
+	.irq_mask       = armada_370_xp_irq_mask,
+	.irq_mask_ack   = armada_370_xp_irq_mask,
+	.irq_unmask     = armada_370_xp_irq_unmask,
+#ifdef CONFIG_SMP
+	.irq_set_affinity = armada_xp_set_affinity,
+#endif
+};
+
+static int armada_370_xp_mpic_irq_map(struct irq_domain *h,
+				      unsigned int virq, irq_hw_number_t hw)
+{
+	armada_370_xp_irq_mask(irq_get_irq_data(virq));
+	if (hw != ARMADA_370_XP_TIMER0_PER_CPU_IRQ)
+		writel(hw, per_cpu_int_base +
+			ARMADA_370_XP_INT_CLEAR_MASK_OFFS);
+	else
+		writel(hw, main_int_base + ARMADA_370_XP_INT_SET_ENABLE_OFFS);
+	irq_set_status_flags(virq, IRQ_LEVEL);
+
+	if (hw == ARMADA_370_XP_TIMER0_PER_CPU_IRQ) {
+		irq_set_percpu_devid(virq);
+		irq_set_chip_and_handler(virq, &armada_370_xp_irq_chip,
+					handle_percpu_devid_irq);
+
+	} else {
+		irq_set_chip_and_handler(virq, &armada_370_xp_irq_chip,
+					handle_level_irq);
+	}
+	set_irq_flags(virq, IRQF_VALID | IRQF_PROBE);
+
+	return 0;
+}
+
+#ifdef CONFIG_SMP
+void armada_mpic_send_doorbell(const struct cpumask *mask, unsigned int irq)
+{
+	int cpu;
+	unsigned long map = 0;
+
+	/* Convert our logical CPU mask into a physical one. */
+	for_each_cpu(cpu, mask)
+		map |= 1 << cpu_logical_map(cpu);
+
+	/*
+	 * Ensure that stores to Normal memory are visible to the
+	 * other CPUs before issuing the IPI.
+	 */
+	dsb();
+
+	/* submit softirq */
+	writel((map << 8) | irq, main_int_base +
+		ARMADA_370_XP_SW_TRIG_INT_OFFS);
+}
+
+void armada_xp_mpic_smp_cpu_init(void)
+{
+	/* Clear pending IPIs */
+	writel(0, per_cpu_int_base + ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS);
+
+	/* Enable first 8 IPIs */
+	writel(IPI_DOORBELL_MASK, per_cpu_int_base +
+		ARMADA_370_XP_IN_DRBEL_MSK_OFFS);
+
+	/* Unmask IPI interrupt */
+	writel(0, per_cpu_int_base + ARMADA_370_XP_INT_CLEAR_MASK_OFFS);
+}
+#endif /* CONFIG_SMP */
+
+static struct irq_domain_ops armada_370_xp_mpic_irq_ops = {
+	.map = armada_370_xp_mpic_irq_map,
+	.xlate = irq_domain_xlate_onecell,
+};
+
+static asmlinkage void __exception_irq_entry
+armada_370_xp_handle_irq(struct pt_regs *regs)
+{
+	u32 irqstat, irqnr;
+
+	do {
+		irqstat = readl_relaxed(per_cpu_int_base +
+					ARMADA_370_XP_CPU_INTACK_OFFS);
+		irqnr = irqstat & 0x3FF;
+
+		if (irqnr > 1022)
+			break;
+
+		if (irqnr > 0) {
+			irqnr =	irq_find_mapping(armada_370_xp_mpic_domain,
+					irqnr);
+			handle_IRQ(irqnr, regs);
+			continue;
+		}
+#ifdef CONFIG_SMP
+		/* IPI Handling */
+		if (irqnr == 0) {
+			u32 ipimask, ipinr;
+
+			ipimask = readl_relaxed(per_cpu_int_base +
+						ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS)
+				& IPI_DOORBELL_MASK;
+
+			writel(~IPI_DOORBELL_MASK, per_cpu_int_base +
+				ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS);
+
+			/* Handle all pending doorbells */
+			for (ipinr = IPI_DOORBELL_START;
+			     ipinr < IPI_DOORBELL_END; ipinr++) {
+				if (ipimask & (0x1 << ipinr))
+					handle_IPI(ipinr, regs);
+			}
+			continue;
+		}
+#endif
+
+	} while (1);
+}
+
+static int __init armada_370_xp_mpic_of_init(struct device_node *node,
+					     struct device_node *parent)
+{
+	u32 control;
+
+	main_int_base = of_iomap(node, 0);
+	per_cpu_int_base = of_iomap(node, 1);
+
+	BUG_ON(!main_int_base);
+	BUG_ON(!per_cpu_int_base);
+
+	control = readl(main_int_base + ARMADA_370_XP_INT_CONTROL);
+
+	armada_370_xp_mpic_domain =
+		irq_domain_add_linear(node, (control >> 2) & 0x3ff,
+				&armada_370_xp_mpic_irq_ops, NULL);
+
+	if (!armada_370_xp_mpic_domain)
+		panic("Unable to add Armada_370_Xp MPIC irq domain (DT)\n");
+
+	irq_set_default_host(armada_370_xp_mpic_domain);
+
+#ifdef CONFIG_SMP
+	armada_xp_mpic_smp_cpu_init();
+
+	/*
+	 * Set the default affinity from all CPUs to the boot cpu.
+	 * This is required since the MPIC doesn't limit several CPUs
+	 * from acknowledging the same interrupt.
+	 */
+	cpumask_clear(irq_default_affinity);
+	cpumask_set_cpu(smp_processor_id(), irq_default_affinity);
+
+#endif
+
+	set_handle_irq(armada_370_xp_handle_irq);
+
+	return 0;
+}
+
+IRQCHIP_DECLARE(armada_370_xp_mpic, "marvell,mpic", armada_370_xp_mpic_of_init);
diff --git a/drivers/irqchip/irq-gic.c b/drivers/irqchip/irq-gic.c
index fc6aebf1e4b2..1760ceb68b7b 100644
--- a/drivers/irqchip/irq-gic.c
+++ b/drivers/irqchip/irq-gic.c
@@ -28,6 +28,7 @@
 #include <linux/module.h>
 #include <linux/list.h>
 #include <linux/smp.h>
+#include <linux/cpu.h>
 #include <linux/cpu_pm.h>
 #include <linux/cpumask.h>
 #include <linux/io.h>
@@ -38,12 +39,12 @@
 #include <linux/interrupt.h>
 #include <linux/percpu.h>
 #include <linux/slab.h>
+#include <linux/irqchip/chained_irq.h>
 #include <linux/irqchip/arm-gic.h>
 
 #include <asm/irq.h>
 #include <asm/exception.h>
 #include <asm/smp_plat.h>
-#include <asm/mach/irq.h>
 
 #include "irqchip.h"
 
@@ -127,7 +128,7 @@ static inline void gic_set_base_accessor(struct gic_chip_data *data,
 #else
 #define gic_data_dist_base(d)	((d)->dist_base.common_base)
 #define gic_data_cpu_base(d)	((d)->cpu_base.common_base)
-#define gic_set_base_accessor(d,f)
+#define gic_set_base_accessor(d, f)
 #endif
 
 static inline void __iomem *gic_dist_base(struct irq_data *d)
@@ -324,7 +325,7 @@ static void gic_handle_cascade_irq(unsigned int irq, struct irq_desc *desc)
 
 	cascade_irq = irq_find_mapping(chip_data->domain, gic_irq);
 	if (unlikely(gic_irq < 32 || gic_irq > 1020))
-		do_bad_IRQ(cascade_irq, desc);
+		handle_bad_irq(cascade_irq, desc);
 	else
 		generic_handle_irq(cascade_irq);
 
@@ -700,6 +701,25 @@ static int gic_irq_domain_xlate(struct irq_domain *d,
 	return 0;
 }
 
+#ifdef CONFIG_SMP
+static int __cpuinit gic_secondary_init(struct notifier_block *nfb,
+					unsigned long action, void *hcpu)
+{
+	if (action == CPU_STARTING)
+		gic_cpu_init(&gic_data[0]);
+	return NOTIFY_OK;
+}
+
+/*
+ * Notifier for enabling the GIC CPU interface. Set an arbitrarily high
+ * priority because the GIC needs to be up before the ARM generic timers.
+ */
+static struct notifier_block __cpuinitdata gic_cpu_notifier = {
+	.notifier_call = gic_secondary_init,
+	.priority = 100,
+};
+#endif
+
 const struct irq_domain_ops gic_irq_domain_ops = {
 	.map = gic_irq_domain_map,
 	.xlate = gic_irq_domain_xlate,
@@ -790,6 +810,7 @@ void __init gic_init_bases(unsigned int gic_nr, int irq_start,
 
 #ifdef CONFIG_SMP
 	set_smp_cross_call(gic_raise_softirq);
+	register_cpu_notifier(&gic_cpu_notifier);
 #endif
 
 	set_handle_irq(gic_handle_irq);
@@ -800,15 +821,8 @@ void __init gic_init_bases(unsigned int gic_nr, int irq_start,
 	gic_pm_init(gic);
 }
 
-void __cpuinit gic_secondary_init(unsigned int gic_nr)
-{
-	BUG_ON(gic_nr >= MAX_GIC_NR);
-
-	gic_cpu_init(&gic_data[gic_nr]);
-}
-
 #ifdef CONFIG_OF
-static int gic_cnt __initdata = 0;
+static int gic_cnt __initdata;
 
 int __init gic_of_init(struct device_node *node, struct device_node *parent)
 {
diff --git a/drivers/irqchip/irq-mxs.c b/drivers/irqchip/irq-mxs.c
new file mode 100644
index 000000000000..29889bbdcc6d
--- /dev/null
+++ b/drivers/irqchip/irq-mxs.c
@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) 2009-2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/stmp_device.h>
+#include <asm/exception.h>
+
+#include "irqchip.h"
+
+#define HW_ICOLL_VECTOR				0x0000
+#define HW_ICOLL_LEVELACK			0x0010
+#define HW_ICOLL_CTRL				0x0020
+#define HW_ICOLL_STAT_OFFSET			0x0070
+#define HW_ICOLL_INTERRUPTn_SET(n)		(0x0124 + (n) * 0x10)
+#define HW_ICOLL_INTERRUPTn_CLR(n)		(0x0128 + (n) * 0x10)
+#define BM_ICOLL_INTERRUPTn_ENABLE		0x00000004
+#define BV_ICOLL_LEVELACK_IRQLEVELACK__LEVEL0	0x1
+
+#define ICOLL_NUM_IRQS		128
+
+static void __iomem *icoll_base;
+static struct irq_domain *icoll_domain;
+
+static void icoll_ack_irq(struct irq_data *d)
+{
+	/*
+	 * The Interrupt Collector is able to prioritize irqs.
+	 * Currently only level 0 is used. So acking can use
+	 * BV_ICOLL_LEVELACK_IRQLEVELACK__LEVEL0 unconditionally.
+	 */
+	__raw_writel(BV_ICOLL_LEVELACK_IRQLEVELACK__LEVEL0,
+			icoll_base + HW_ICOLL_LEVELACK);
+}
+
+static void icoll_mask_irq(struct irq_data *d)
+{
+	__raw_writel(BM_ICOLL_INTERRUPTn_ENABLE,
+			icoll_base + HW_ICOLL_INTERRUPTn_CLR(d->hwirq));
+}
+
+static void icoll_unmask_irq(struct irq_data *d)
+{
+	__raw_writel(BM_ICOLL_INTERRUPTn_ENABLE,
+			icoll_base + HW_ICOLL_INTERRUPTn_SET(d->hwirq));
+}
+
+static struct irq_chip mxs_icoll_chip = {
+	.irq_ack = icoll_ack_irq,
+	.irq_mask = icoll_mask_irq,
+	.irq_unmask = icoll_unmask_irq,
+};
+
+asmlinkage void __exception_irq_entry icoll_handle_irq(struct pt_regs *regs)
+{
+	u32 irqnr;
+
+	do {
+		irqnr = __raw_readl(icoll_base + HW_ICOLL_STAT_OFFSET);
+		if (irqnr != 0x7f) {
+			__raw_writel(irqnr, icoll_base + HW_ICOLL_VECTOR);
+			irqnr = irq_find_mapping(icoll_domain, irqnr);
+			handle_IRQ(irqnr, regs);
+			continue;
+		}
+		break;
+	} while (1);
+}
+
+static int icoll_irq_domain_map(struct irq_domain *d, unsigned int virq,
+				irq_hw_number_t hw)
+{
+	irq_set_chip_and_handler(virq, &mxs_icoll_chip, handle_level_irq);
+	set_irq_flags(virq, IRQF_VALID);
+
+	return 0;
+}
+
+static struct irq_domain_ops icoll_irq_domain_ops = {
+	.map = icoll_irq_domain_map,
+	.xlate = irq_domain_xlate_onecell,
+};
+
+static void __init icoll_of_init(struct device_node *np,
+			  struct device_node *interrupt_parent)
+{
+	icoll_base = of_iomap(np, 0);
+	WARN_ON(!icoll_base);
+
+	/*
+	 * Interrupt Collector reset, which initializes the priority
+	 * for each irq to level 0.
+	 */
+	stmp_reset_block(icoll_base + HW_ICOLL_CTRL);
+
+	icoll_domain = irq_domain_add_linear(np, ICOLL_NUM_IRQS,
+					     &icoll_irq_domain_ops, NULL);
+	WARN_ON(!icoll_domain);
+}
+IRQCHIP_DECLARE(mxs, "fsl,icoll", icoll_of_init);
diff --git a/drivers/irqchip/irq-renesas-intc-irqpin.c b/drivers/irqchip/irq-renesas-intc-irqpin.c
new file mode 100644
index 000000000000..5a68e5accec1
--- /dev/null
+++ b/drivers/irqchip/irq-renesas-intc-irqpin.c
@@ -0,0 +1,547 @@
+/*
+ * Renesas INTC External IRQ Pin Driver
+ *
+ *  Copyright (C) 2013 Magnus Damm
+ *
+ * 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/init.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <linux/ioport.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_data/irq-renesas-intc-irqpin.h>
+
+#define INTC_IRQPIN_MAX 8 /* maximum 8 interrupts per driver instance */
+
+#define INTC_IRQPIN_REG_SENSE 0 /* ICRn */
+#define INTC_IRQPIN_REG_PRIO 1 /* INTPRInn */
+#define INTC_IRQPIN_REG_SOURCE 2 /* INTREQnn */
+#define INTC_IRQPIN_REG_MASK 3 /* INTMSKnn */
+#define INTC_IRQPIN_REG_CLEAR 4 /* INTMSKCLRnn */
+#define INTC_IRQPIN_REG_NR 5
+
+/* INTC external IRQ PIN hardware register access:
+ *
+ * SENSE is read-write 32-bit with 2-bits or 4-bits per IRQ (*)
+ * PRIO is read-write 32-bit with 4-bits per IRQ (**)
+ * SOURCE is read-only 32-bit or 8-bit with 1-bit per IRQ (***)
+ * MASK is write-only 32-bit or 8-bit with 1-bit per IRQ (***)
+ * CLEAR is write-only 32-bit or 8-bit with 1-bit per IRQ (***)
+ *
+ * (*) May be accessed by more than one driver instance - lock needed
+ * (**) Read-modify-write access by one driver instance - lock needed
+ * (***) Accessed by one driver instance only - no locking needed
+ */
+
+struct intc_irqpin_iomem {
+	void __iomem *iomem;
+	unsigned long (*read)(void __iomem *iomem);
+	void (*write)(void __iomem *iomem, unsigned long data);
+	int width;
+};
+
+struct intc_irqpin_irq {
+	int hw_irq;
+	int requested_irq;
+	int domain_irq;
+	struct intc_irqpin_priv *p;
+};
+
+struct intc_irqpin_priv {
+	struct intc_irqpin_iomem iomem[INTC_IRQPIN_REG_NR];
+	struct intc_irqpin_irq irq[INTC_IRQPIN_MAX];
+	struct renesas_intc_irqpin_config config;
+	unsigned int number_of_irqs;
+	struct platform_device *pdev;
+	struct irq_chip irq_chip;
+	struct irq_domain *irq_domain;
+	bool shared_irqs;
+	u8 shared_irq_mask;
+};
+
+static unsigned long intc_irqpin_read32(void __iomem *iomem)
+{
+	return ioread32(iomem);
+}
+
+static unsigned long intc_irqpin_read8(void __iomem *iomem)
+{
+	return ioread8(iomem);
+}
+
+static void intc_irqpin_write32(void __iomem *iomem, unsigned long data)
+{
+	iowrite32(data, iomem);
+}
+
+static void intc_irqpin_write8(void __iomem *iomem, unsigned long data)
+{
+	iowrite8(data, iomem);
+}
+
+static inline unsigned long intc_irqpin_read(struct intc_irqpin_priv *p,
+					     int reg)
+{
+	struct intc_irqpin_iomem *i = &p->iomem[reg];
+
+	return i->read(i->iomem);
+}
+
+static inline void intc_irqpin_write(struct intc_irqpin_priv *p,
+				     int reg, unsigned long data)
+{
+	struct intc_irqpin_iomem *i = &p->iomem[reg];
+
+	i->write(i->iomem, data);
+}
+
+static inline unsigned long intc_irqpin_hwirq_mask(struct intc_irqpin_priv *p,
+						   int reg, int hw_irq)
+{
+	return BIT((p->iomem[reg].width - 1) - hw_irq);
+}
+
+static inline void intc_irqpin_irq_write_hwirq(struct intc_irqpin_priv *p,
+					       int reg, int hw_irq)
+{
+	intc_irqpin_write(p, reg, intc_irqpin_hwirq_mask(p, reg, hw_irq));
+}
+
+static DEFINE_RAW_SPINLOCK(intc_irqpin_lock); /* only used by slow path */
+
+static void intc_irqpin_read_modify_write(struct intc_irqpin_priv *p,
+					  int reg, int shift,
+					  int width, int value)
+{
+	unsigned long flags;
+	unsigned long tmp;
+
+	raw_spin_lock_irqsave(&intc_irqpin_lock, flags);
+
+	tmp = intc_irqpin_read(p, reg);
+	tmp &= ~(((1 << width) - 1) << shift);
+	tmp |= value << shift;
+	intc_irqpin_write(p, reg, tmp);
+
+	raw_spin_unlock_irqrestore(&intc_irqpin_lock, flags);
+}
+
+static void intc_irqpin_mask_unmask_prio(struct intc_irqpin_priv *p,
+					 int irq, int do_mask)
+{
+	int bitfield_width = 4; /* PRIO assumed to have fixed bitfield width */
+	int shift = (7 - irq) * bitfield_width; /* PRIO assumed to be 32-bit */
+
+	intc_irqpin_read_modify_write(p, INTC_IRQPIN_REG_PRIO,
+				      shift, bitfield_width,
+				      do_mask ? 0 : (1 << bitfield_width) - 1);
+}
+
+static int intc_irqpin_set_sense(struct intc_irqpin_priv *p, int irq, int value)
+{
+	int bitfield_width = p->config.sense_bitfield_width;
+	int shift = (7 - irq) * bitfield_width; /* SENSE assumed to be 32-bit */
+
+	dev_dbg(&p->pdev->dev, "sense irq = %d, mode = %d\n", irq, value);
+
+	if (value >= (1 << bitfield_width))
+		return -EINVAL;
+
+	intc_irqpin_read_modify_write(p, INTC_IRQPIN_REG_SENSE, shift,
+				      bitfield_width, value);
+	return 0;
+}
+
+static void intc_irqpin_dbg(struct intc_irqpin_irq *i, char *str)
+{
+	dev_dbg(&i->p->pdev->dev, "%s (%d:%d:%d)\n",
+		str, i->requested_irq, i->hw_irq, i->domain_irq);
+}
+
+static void intc_irqpin_irq_enable(struct irq_data *d)
+{
+	struct intc_irqpin_priv *p = irq_data_get_irq_chip_data(d);
+	int hw_irq = irqd_to_hwirq(d);
+
+	intc_irqpin_dbg(&p->irq[hw_irq], "enable");
+	intc_irqpin_irq_write_hwirq(p, INTC_IRQPIN_REG_CLEAR, hw_irq);
+}
+
+static void intc_irqpin_irq_disable(struct irq_data *d)
+{
+	struct intc_irqpin_priv *p = irq_data_get_irq_chip_data(d);
+	int hw_irq = irqd_to_hwirq(d);
+
+	intc_irqpin_dbg(&p->irq[hw_irq], "disable");
+	intc_irqpin_irq_write_hwirq(p, INTC_IRQPIN_REG_MASK, hw_irq);
+}
+
+static void intc_irqpin_shared_irq_enable(struct irq_data *d)
+{
+	struct intc_irqpin_priv *p = irq_data_get_irq_chip_data(d);
+	int hw_irq = irqd_to_hwirq(d);
+
+	intc_irqpin_dbg(&p->irq[hw_irq], "shared enable");
+	intc_irqpin_irq_write_hwirq(p, INTC_IRQPIN_REG_CLEAR, hw_irq);
+
+	p->shared_irq_mask &= ~BIT(hw_irq);
+}
+
+static void intc_irqpin_shared_irq_disable(struct irq_data *d)
+{
+	struct intc_irqpin_priv *p = irq_data_get_irq_chip_data(d);
+	int hw_irq = irqd_to_hwirq(d);
+
+	intc_irqpin_dbg(&p->irq[hw_irq], "shared disable");
+	intc_irqpin_irq_write_hwirq(p, INTC_IRQPIN_REG_MASK, hw_irq);
+
+	p->shared_irq_mask |= BIT(hw_irq);
+}
+
+static void intc_irqpin_irq_enable_force(struct irq_data *d)
+{
+	struct intc_irqpin_priv *p = irq_data_get_irq_chip_data(d);
+	int irq = p->irq[irqd_to_hwirq(d)].requested_irq;
+
+	intc_irqpin_irq_enable(d);
+
+	/* enable interrupt through parent interrupt controller,
+	 * assumes non-shared interrupt with 1:1 mapping
+	 * needed for busted IRQs on some SoCs like sh73a0
+	 */
+	irq_get_chip(irq)->irq_unmask(irq_get_irq_data(irq));
+}
+
+static void intc_irqpin_irq_disable_force(struct irq_data *d)
+{
+	struct intc_irqpin_priv *p = irq_data_get_irq_chip_data(d);
+	int irq = p->irq[irqd_to_hwirq(d)].requested_irq;
+
+	/* disable interrupt through parent interrupt controller,
+	 * assumes non-shared interrupt with 1:1 mapping
+	 * needed for busted IRQs on some SoCs like sh73a0
+	 */
+	irq_get_chip(irq)->irq_mask(irq_get_irq_data(irq));
+	intc_irqpin_irq_disable(d);
+}
+
+#define INTC_IRQ_SENSE_VALID 0x10
+#define INTC_IRQ_SENSE(x) (x + INTC_IRQ_SENSE_VALID)
+
+static unsigned char intc_irqpin_sense[IRQ_TYPE_SENSE_MASK + 1] = {
+	[IRQ_TYPE_EDGE_FALLING] = INTC_IRQ_SENSE(0x00),
+	[IRQ_TYPE_EDGE_RISING] = INTC_IRQ_SENSE(0x01),
+	[IRQ_TYPE_LEVEL_LOW] = INTC_IRQ_SENSE(0x02),
+	[IRQ_TYPE_LEVEL_HIGH] = INTC_IRQ_SENSE(0x03),
+	[IRQ_TYPE_EDGE_BOTH] = INTC_IRQ_SENSE(0x04),
+};
+
+static int intc_irqpin_irq_set_type(struct irq_data *d, unsigned int type)
+{
+	unsigned char value = intc_irqpin_sense[type & IRQ_TYPE_SENSE_MASK];
+	struct intc_irqpin_priv *p = irq_data_get_irq_chip_data(d);
+
+	if (!(value & INTC_IRQ_SENSE_VALID))
+		return -EINVAL;
+
+	return intc_irqpin_set_sense(p, irqd_to_hwirq(d),
+				     value ^ INTC_IRQ_SENSE_VALID);
+}
+
+static irqreturn_t intc_irqpin_irq_handler(int irq, void *dev_id)
+{
+	struct intc_irqpin_irq *i = dev_id;
+	struct intc_irqpin_priv *p = i->p;
+	unsigned long bit;
+
+	intc_irqpin_dbg(i, "demux1");
+	bit = intc_irqpin_hwirq_mask(p, INTC_IRQPIN_REG_SOURCE, i->hw_irq);
+
+	if (intc_irqpin_read(p, INTC_IRQPIN_REG_SOURCE) & bit) {
+		intc_irqpin_write(p, INTC_IRQPIN_REG_SOURCE, ~bit);
+		intc_irqpin_dbg(i, "demux2");
+		generic_handle_irq(i->domain_irq);
+		return IRQ_HANDLED;
+	}
+	return IRQ_NONE;
+}
+
+static irqreturn_t intc_irqpin_shared_irq_handler(int irq, void *dev_id)
+{
+	struct intc_irqpin_priv *p = dev_id;
+	unsigned int reg_source = intc_irqpin_read(p, INTC_IRQPIN_REG_SOURCE);
+	irqreturn_t status = IRQ_NONE;
+	int k;
+
+	for (k = 0; k < 8; k++) {
+		if (reg_source & BIT(7 - k)) {
+			if (BIT(k) & p->shared_irq_mask)
+				continue;
+
+			status |= intc_irqpin_irq_handler(irq, &p->irq[k]);
+		}
+	}
+
+	return status;
+}
+
+static int intc_irqpin_irq_domain_map(struct irq_domain *h, unsigned int virq,
+				      irq_hw_number_t hw)
+{
+	struct intc_irqpin_priv *p = h->host_data;
+
+	p->irq[hw].domain_irq = virq;
+	p->irq[hw].hw_irq = hw;
+
+	intc_irqpin_dbg(&p->irq[hw], "map");
+	irq_set_chip_data(virq, h->host_data);
+	irq_set_chip_and_handler(virq, &p->irq_chip, handle_level_irq);
+	set_irq_flags(virq, IRQF_VALID); /* kill me now */
+	return 0;
+}
+
+static struct irq_domain_ops intc_irqpin_irq_domain_ops = {
+	.map	= intc_irqpin_irq_domain_map,
+	.xlate  = irq_domain_xlate_twocell,
+};
+
+static int intc_irqpin_probe(struct platform_device *pdev)
+{
+	struct renesas_intc_irqpin_config *pdata = pdev->dev.platform_data;
+	struct intc_irqpin_priv *p;
+	struct intc_irqpin_iomem *i;
+	struct resource *io[INTC_IRQPIN_REG_NR];
+	struct resource *irq;
+	struct irq_chip *irq_chip;
+	void (*enable_fn)(struct irq_data *d);
+	void (*disable_fn)(struct irq_data *d);
+	const char *name = dev_name(&pdev->dev);
+	int ref_irq;
+	int ret;
+	int k;
+
+	p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
+	if (!p) {
+		dev_err(&pdev->dev, "failed to allocate driver data\n");
+		ret = -ENOMEM;
+		goto err0;
+	}
+
+	/* deal with driver instance configuration */
+	if (pdata)
+		memcpy(&p->config, pdata, sizeof(*pdata));
+	if (!p->config.sense_bitfield_width)
+		p->config.sense_bitfield_width = 4; /* default to 4 bits */
+
+	p->pdev = pdev;
+	platform_set_drvdata(pdev, p);
+
+	/* get hold of manadatory IOMEM */
+	for (k = 0; k < INTC_IRQPIN_REG_NR; k++) {
+		io[k] = platform_get_resource(pdev, IORESOURCE_MEM, k);
+		if (!io[k]) {
+			dev_err(&pdev->dev, "not enough IOMEM resources\n");
+			ret = -EINVAL;
+			goto err0;
+		}
+	}
+
+	/* allow any number of IRQs between 1 and INTC_IRQPIN_MAX */
+	for (k = 0; k < INTC_IRQPIN_MAX; k++) {
+		irq = platform_get_resource(pdev, IORESOURCE_IRQ, k);
+		if (!irq)
+			break;
+
+		p->irq[k].p = p;
+		p->irq[k].requested_irq = irq->start;
+	}
+
+	p->number_of_irqs = k;
+	if (p->number_of_irqs < 1) {
+		dev_err(&pdev->dev, "not enough IRQ resources\n");
+		ret = -EINVAL;
+		goto err0;
+	}
+
+	/* ioremap IOMEM and setup read/write callbacks */
+	for (k = 0; k < INTC_IRQPIN_REG_NR; k++) {
+		i = &p->iomem[k];
+
+		switch (resource_size(io[k])) {
+		case 1:
+			i->width = 8;
+			i->read = intc_irqpin_read8;
+			i->write = intc_irqpin_write8;
+			break;
+		case 4:
+			i->width = 32;
+			i->read = intc_irqpin_read32;
+			i->write = intc_irqpin_write32;
+			break;
+		default:
+			dev_err(&pdev->dev, "IOMEM size mismatch\n");
+			ret = -EINVAL;
+			goto err0;
+		}
+
+		i->iomem = devm_ioremap_nocache(&pdev->dev, io[k]->start,
+						resource_size(io[k]));
+		if (!i->iomem) {
+			dev_err(&pdev->dev, "failed to remap IOMEM\n");
+			ret = -ENXIO;
+			goto err0;
+		}
+	}
+
+	/* mask all interrupts using priority */
+	for (k = 0; k < p->number_of_irqs; k++)
+		intc_irqpin_mask_unmask_prio(p, k, 1);
+
+	/* clear all pending interrupts */
+	intc_irqpin_write(p, INTC_IRQPIN_REG_SOURCE, 0x0);
+
+	/* scan for shared interrupt lines */
+	ref_irq = p->irq[0].requested_irq;
+	p->shared_irqs = true;
+	for (k = 1; k < p->number_of_irqs; k++) {
+		if (ref_irq != p->irq[k].requested_irq) {
+			p->shared_irqs = false;
+			break;
+		}
+	}
+
+	/* use more severe masking method if requested */
+	if (p->config.control_parent) {
+		enable_fn = intc_irqpin_irq_enable_force;
+		disable_fn = intc_irqpin_irq_disable_force;
+	} else if (!p->shared_irqs) {
+		enable_fn = intc_irqpin_irq_enable;
+		disable_fn = intc_irqpin_irq_disable;
+	} else {
+		enable_fn = intc_irqpin_shared_irq_enable;
+		disable_fn = intc_irqpin_shared_irq_disable;
+	}
+
+	irq_chip = &p->irq_chip;
+	irq_chip->name = name;
+	irq_chip->irq_mask = disable_fn;
+	irq_chip->irq_unmask = enable_fn;
+	irq_chip->irq_enable = enable_fn;
+	irq_chip->irq_disable = disable_fn;
+	irq_chip->irq_set_type = intc_irqpin_irq_set_type;
+	irq_chip->flags	= IRQCHIP_SKIP_SET_WAKE;
+
+	p->irq_domain = irq_domain_add_simple(pdev->dev.of_node,
+					      p->number_of_irqs,
+					      p->config.irq_base,
+					      &intc_irqpin_irq_domain_ops, p);
+	if (!p->irq_domain) {
+		ret = -ENXIO;
+		dev_err(&pdev->dev, "cannot initialize irq domain\n");
+		goto err0;
+	}
+
+	if (p->shared_irqs) {
+		/* request one shared interrupt */
+		if (devm_request_irq(&pdev->dev, p->irq[0].requested_irq,
+				intc_irqpin_shared_irq_handler,
+				IRQF_SHARED, name, p)) {
+			dev_err(&pdev->dev, "failed to request low IRQ\n");
+			ret = -ENOENT;
+			goto err1;
+		}
+	} else {
+		/* request interrupts one by one */
+		for (k = 0; k < p->number_of_irqs; k++) {
+			if (devm_request_irq(&pdev->dev,
+					p->irq[k].requested_irq,
+					intc_irqpin_irq_handler,
+					0, name, &p->irq[k])) {
+				dev_err(&pdev->dev,
+					"failed to request low IRQ\n");
+				ret = -ENOENT;
+				goto err1;
+			}
+		}
+	}
+
+	/* unmask all interrupts on prio level */
+	for (k = 0; k < p->number_of_irqs; k++)
+		intc_irqpin_mask_unmask_prio(p, k, 0);
+
+	dev_info(&pdev->dev, "driving %d irqs\n", p->number_of_irqs);
+
+	/* warn in case of mismatch if irq base is specified */
+	if (p->config.irq_base) {
+		if (p->config.irq_base != p->irq[0].domain_irq)
+			dev_warn(&pdev->dev, "irq base mismatch (%d/%d)\n",
+				 p->config.irq_base, p->irq[0].domain_irq);
+	}
+
+	return 0;
+
+err1:
+	irq_domain_remove(p->irq_domain);
+err0:
+	return ret;
+}
+
+static int intc_irqpin_remove(struct platform_device *pdev)
+{
+	struct intc_irqpin_priv *p = platform_get_drvdata(pdev);
+
+	irq_domain_remove(p->irq_domain);
+
+	return 0;
+}
+
+static const struct of_device_id intc_irqpin_dt_ids[] = {
+	{ .compatible = "renesas,intc-irqpin", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, intc_irqpin_dt_ids);
+
+static struct platform_driver intc_irqpin_device_driver = {
+	.probe		= intc_irqpin_probe,
+	.remove		= intc_irqpin_remove,
+	.driver		= {
+		.name	= "renesas_intc_irqpin",
+		.of_match_table = intc_irqpin_dt_ids,
+		.owner  = THIS_MODULE,
+	}
+};
+
+static int __init intc_irqpin_init(void)
+{
+	return platform_driver_register(&intc_irqpin_device_driver);
+}
+postcore_initcall(intc_irqpin_init);
+
+static void __exit intc_irqpin_exit(void)
+{
+	platform_driver_unregister(&intc_irqpin_device_driver);
+}
+module_exit(intc_irqpin_exit);
+
+MODULE_AUTHOR("Magnus Damm");
+MODULE_DESCRIPTION("Renesas INTC External IRQ Pin Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/irqchip/irq-renesas-irqc.c b/drivers/irqchip/irq-renesas-irqc.c
new file mode 100644
index 000000000000..927bff373aac
--- /dev/null
+++ b/drivers/irqchip/irq-renesas-irqc.c
@@ -0,0 +1,307 @@
+/*
+ * Renesas IRQC Driver
+ *
+ *  Copyright (C) 2013 Magnus Damm
+ *
+ * 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/init.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <linux/ioport.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_data/irq-renesas-irqc.h>
+
+#define IRQC_IRQ_MAX 32 /* maximum 32 interrupts per driver instance */
+
+#define IRQC_REQ_STS 0x00
+#define IRQC_EN_STS 0x04
+#define IRQC_EN_SET 0x08
+#define IRQC_INT_CPU_BASE(n) (0x000 + ((n) * 0x10))
+#define DETECT_STATUS 0x100
+#define IRQC_CONFIG(n) (0x180 + ((n) * 0x04))
+
+struct irqc_irq {
+	int hw_irq;
+	int requested_irq;
+	int domain_irq;
+	struct irqc_priv *p;
+};
+
+struct irqc_priv {
+	void __iomem *iomem;
+	void __iomem *cpu_int_base;
+	struct irqc_irq irq[IRQC_IRQ_MAX];
+	struct renesas_irqc_config config;
+	unsigned int number_of_irqs;
+	struct platform_device *pdev;
+	struct irq_chip irq_chip;
+	struct irq_domain *irq_domain;
+};
+
+static void irqc_dbg(struct irqc_irq *i, char *str)
+{
+	dev_dbg(&i->p->pdev->dev, "%s (%d:%d:%d)\n",
+		str, i->requested_irq, i->hw_irq, i->domain_irq);
+}
+
+static void irqc_irq_enable(struct irq_data *d)
+{
+	struct irqc_priv *p = irq_data_get_irq_chip_data(d);
+	int hw_irq = irqd_to_hwirq(d);
+
+	irqc_dbg(&p->irq[hw_irq], "enable");
+	iowrite32(BIT(hw_irq), p->cpu_int_base + IRQC_EN_SET);
+}
+
+static void irqc_irq_disable(struct irq_data *d)
+{
+	struct irqc_priv *p = irq_data_get_irq_chip_data(d);
+	int hw_irq = irqd_to_hwirq(d);
+
+	irqc_dbg(&p->irq[hw_irq], "disable");
+	iowrite32(BIT(hw_irq), p->cpu_int_base + IRQC_EN_STS);
+}
+
+#define INTC_IRQ_SENSE_VALID 0x10
+#define INTC_IRQ_SENSE(x) (x + INTC_IRQ_SENSE_VALID)
+
+static unsigned char irqc_sense[IRQ_TYPE_SENSE_MASK + 1] = {
+	[IRQ_TYPE_LEVEL_LOW] = INTC_IRQ_SENSE(0x01),
+	[IRQ_TYPE_LEVEL_HIGH] = INTC_IRQ_SENSE(0x02),
+	[IRQ_TYPE_EDGE_FALLING] = INTC_IRQ_SENSE(0x04), /* Synchronous */
+	[IRQ_TYPE_EDGE_RISING] = INTC_IRQ_SENSE(0x08), /* Synchronous */
+	[IRQ_TYPE_EDGE_BOTH] = INTC_IRQ_SENSE(0x0c),  /* Synchronous */
+};
+
+static int irqc_irq_set_type(struct irq_data *d, unsigned int type)
+{
+	struct irqc_priv *p = irq_data_get_irq_chip_data(d);
+	int hw_irq = irqd_to_hwirq(d);
+	unsigned char value = irqc_sense[type & IRQ_TYPE_SENSE_MASK];
+	unsigned long tmp;
+
+	irqc_dbg(&p->irq[hw_irq], "sense");
+
+	if (!(value & INTC_IRQ_SENSE_VALID))
+		return -EINVAL;
+
+	tmp = ioread32(p->iomem + IRQC_CONFIG(hw_irq));
+	tmp &= ~0x3f;
+	tmp |= value ^ INTC_IRQ_SENSE_VALID;
+	iowrite32(tmp, p->iomem + IRQC_CONFIG(hw_irq));
+	return 0;
+}
+
+static irqreturn_t irqc_irq_handler(int irq, void *dev_id)
+{
+	struct irqc_irq *i = dev_id;
+	struct irqc_priv *p = i->p;
+	unsigned long bit = BIT(i->hw_irq);
+
+	irqc_dbg(i, "demux1");
+
+	if (ioread32(p->iomem + DETECT_STATUS) & bit) {
+		iowrite32(bit, p->iomem + DETECT_STATUS);
+		irqc_dbg(i, "demux2");
+		generic_handle_irq(i->domain_irq);
+		return IRQ_HANDLED;
+	}
+	return IRQ_NONE;
+}
+
+static int irqc_irq_domain_map(struct irq_domain *h, unsigned int virq,
+			       irq_hw_number_t hw)
+{
+	struct irqc_priv *p = h->host_data;
+
+	p->irq[hw].domain_irq = virq;
+	p->irq[hw].hw_irq = hw;
+
+	irqc_dbg(&p->irq[hw], "map");
+	irq_set_chip_data(virq, h->host_data);
+	irq_set_chip_and_handler(virq, &p->irq_chip, handle_level_irq);
+	set_irq_flags(virq, IRQF_VALID); /* kill me now */
+	return 0;
+}
+
+static struct irq_domain_ops irqc_irq_domain_ops = {
+	.map	= irqc_irq_domain_map,
+	.xlate  = irq_domain_xlate_twocell,
+};
+
+static int irqc_probe(struct platform_device *pdev)
+{
+	struct renesas_irqc_config *pdata = pdev->dev.platform_data;
+	struct irqc_priv *p;
+	struct resource *io;
+	struct resource *irq;
+	struct irq_chip *irq_chip;
+	const char *name = dev_name(&pdev->dev);
+	int ret;
+	int k;
+
+	p = kzalloc(sizeof(*p), GFP_KERNEL);
+	if (!p) {
+		dev_err(&pdev->dev, "failed to allocate driver data\n");
+		ret = -ENOMEM;
+		goto err0;
+	}
+
+	/* deal with driver instance configuration */
+	if (pdata)
+		memcpy(&p->config, pdata, sizeof(*pdata));
+
+	p->pdev = pdev;
+	platform_set_drvdata(pdev, p);
+
+	/* get hold of manadatory IOMEM */
+	io = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!io) {
+		dev_err(&pdev->dev, "not enough IOMEM resources\n");
+		ret = -EINVAL;
+		goto err1;
+	}
+
+	/* allow any number of IRQs between 1 and IRQC_IRQ_MAX */
+	for (k = 0; k < IRQC_IRQ_MAX; k++) {
+		irq = platform_get_resource(pdev, IORESOURCE_IRQ, k);
+		if (!irq)
+			break;
+
+		p->irq[k].p = p;
+		p->irq[k].requested_irq = irq->start;
+	}
+
+	p->number_of_irqs = k;
+	if (p->number_of_irqs < 1) {
+		dev_err(&pdev->dev, "not enough IRQ resources\n");
+		ret = -EINVAL;
+		goto err1;
+	}
+
+	/* ioremap IOMEM and setup read/write callbacks */
+	p->iomem = ioremap_nocache(io->start, resource_size(io));
+	if (!p->iomem) {
+		dev_err(&pdev->dev, "failed to remap IOMEM\n");
+		ret = -ENXIO;
+		goto err2;
+	}
+
+	p->cpu_int_base = p->iomem + IRQC_INT_CPU_BASE(0); /* SYS-SPI */
+
+	irq_chip = &p->irq_chip;
+	irq_chip->name = name;
+	irq_chip->irq_mask = irqc_irq_disable;
+	irq_chip->irq_unmask = irqc_irq_enable;
+	irq_chip->irq_enable = irqc_irq_enable;
+	irq_chip->irq_disable = irqc_irq_disable;
+	irq_chip->irq_set_type = irqc_irq_set_type;
+	irq_chip->flags	= IRQCHIP_SKIP_SET_WAKE;
+
+	p->irq_domain = irq_domain_add_simple(pdev->dev.of_node,
+					      p->number_of_irqs,
+					      p->config.irq_base,
+					      &irqc_irq_domain_ops, p);
+	if (!p->irq_domain) {
+		ret = -ENXIO;
+		dev_err(&pdev->dev, "cannot initialize irq domain\n");
+		goto err2;
+	}
+
+	/* request interrupts one by one */
+	for (k = 0; k < p->number_of_irqs; k++) {
+		if (request_irq(p->irq[k].requested_irq, irqc_irq_handler,
+				0, name, &p->irq[k])) {
+			dev_err(&pdev->dev, "failed to request IRQ\n");
+			ret = -ENOENT;
+			goto err3;
+		}
+	}
+
+	dev_info(&pdev->dev, "driving %d irqs\n", p->number_of_irqs);
+
+	/* warn in case of mismatch if irq base is specified */
+	if (p->config.irq_base) {
+		if (p->config.irq_base != p->irq[0].domain_irq)
+			dev_warn(&pdev->dev, "irq base mismatch (%d/%d)\n",
+				 p->config.irq_base, p->irq[0].domain_irq);
+	}
+
+	return 0;
+err3:
+	for (; k >= 0; k--)
+		free_irq(p->irq[k - 1].requested_irq, &p->irq[k - 1]);
+
+	irq_domain_remove(p->irq_domain);
+err2:
+	iounmap(p->iomem);
+err1:
+	kfree(p);
+err0:
+	return ret;
+}
+
+static int irqc_remove(struct platform_device *pdev)
+{
+	struct irqc_priv *p = platform_get_drvdata(pdev);
+	int k;
+
+	for (k = 0; k < p->number_of_irqs; k++)
+		free_irq(p->irq[k].requested_irq, &p->irq[k]);
+
+	irq_domain_remove(p->irq_domain);
+	iounmap(p->iomem);
+	kfree(p);
+	return 0;
+}
+
+static const struct of_device_id irqc_dt_ids[] = {
+	{ .compatible = "renesas,irqc", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, irqc_dt_ids);
+
+static struct platform_driver irqc_device_driver = {
+	.probe		= irqc_probe,
+	.remove		= irqc_remove,
+	.driver		= {
+		.name	= "renesas_irqc",
+		.of_match_table	= irqc_dt_ids,
+		.owner	= THIS_MODULE,
+	}
+};
+
+static int __init irqc_init(void)
+{
+	return platform_driver_register(&irqc_device_driver);
+}
+postcore_initcall(irqc_init);
+
+static void __exit irqc_exit(void)
+{
+	platform_driver_unregister(&irqc_device_driver);
+}
+module_exit(irqc_exit);
+
+MODULE_AUTHOR("Magnus Damm");
+MODULE_DESCRIPTION("Renesas IRQC Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/irqchip/irq-s3c24xx.c b/drivers/irqchip/irq-s3c24xx.c
new file mode 100644
index 000000000000..bbcc944ed94f
--- /dev/null
+++ b/drivers/irqchip/irq-s3c24xx.c
@@ -0,0 +1,1356 @@
+/*
+ * S3C24XX IRQ handling
+ *
+ * Copyright (c) 2003-2004 Simtec Electronics
+ *	Ben Dooks <ben@simtec.co.uk>
+ * Copyright (c) 2012 Heiko Stuebner <heiko@sntech.de>
+ *
+ * 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.
+*/
+
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/ioport.h>
+#include <linux/device.h>
+#include <linux/irqdomain.h>
+#include <linux/irqchip/chained_irq.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+
+#include <asm/exception.h>
+#include <asm/mach/irq.h>
+
+#include <mach/regs-irq.h>
+#include <mach/regs-gpio.h>
+
+#include <plat/cpu.h>
+#include <plat/regs-irqtype.h>
+#include <plat/pm.h>
+
+#include "irqchip.h"
+
+#define S3C_IRQTYPE_NONE	0
+#define S3C_IRQTYPE_EINT	1
+#define S3C_IRQTYPE_EDGE	2
+#define S3C_IRQTYPE_LEVEL	3
+
+struct s3c_irq_data {
+	unsigned int type;
+	unsigned long offset;
+	unsigned long parent_irq;
+
+	/* data gets filled during init */
+	struct s3c_irq_intc *intc;
+	unsigned long sub_bits;
+	struct s3c_irq_intc *sub_intc;
+};
+
+/*
+ * Sructure holding the controller data
+ * @reg_pending		register holding pending irqs
+ * @reg_intpnd		special register intpnd in main intc
+ * @reg_mask		mask register
+ * @domain		irq_domain of the controller
+ * @parent		parent controller for ext and sub irqs
+ * @irqs		irq-data, always s3c_irq_data[32]
+ */
+struct s3c_irq_intc {
+	void __iomem		*reg_pending;
+	void __iomem		*reg_intpnd;
+	void __iomem		*reg_mask;
+	struct irq_domain	*domain;
+	struct s3c_irq_intc	*parent;
+	struct s3c_irq_data	*irqs;
+};
+
+/*
+ * Array holding pointers to the global controller structs
+ * [0] ... main_intc
+ * [1] ... sub_intc
+ * [2] ... main_intc2 on s3c2416
+ */
+static struct s3c_irq_intc *s3c_intc[3];
+
+static void s3c_irq_mask(struct irq_data *data)
+{
+	struct s3c_irq_data *irq_data = irq_data_get_irq_chip_data(data);
+	struct s3c_irq_intc *intc = irq_data->intc;
+	struct s3c_irq_intc *parent_intc = intc->parent;
+	struct s3c_irq_data *parent_data;
+	unsigned long mask;
+	unsigned int irqno;
+
+	mask = __raw_readl(intc->reg_mask);
+	mask |= (1UL << irq_data->offset);
+	__raw_writel(mask, intc->reg_mask);
+
+	if (parent_intc) {
+		parent_data = &parent_intc->irqs[irq_data->parent_irq];
+
+		/* check to see if we need to mask the parent IRQ
+		 * The parent_irq is always in main_intc, so the hwirq
+		 * for find_mapping does not need an offset in any case.
+		 */
+		if ((mask & parent_data->sub_bits) == parent_data->sub_bits) {
+			irqno = irq_find_mapping(parent_intc->domain,
+					 irq_data->parent_irq);
+			s3c_irq_mask(irq_get_irq_data(irqno));
+		}
+	}
+}
+
+static void s3c_irq_unmask(struct irq_data *data)
+{
+	struct s3c_irq_data *irq_data = irq_data_get_irq_chip_data(data);
+	struct s3c_irq_intc *intc = irq_data->intc;
+	struct s3c_irq_intc *parent_intc = intc->parent;
+	unsigned long mask;
+	unsigned int irqno;
+
+	mask = __raw_readl(intc->reg_mask);
+	mask &= ~(1UL << irq_data->offset);
+	__raw_writel(mask, intc->reg_mask);
+
+	if (parent_intc) {
+		irqno = irq_find_mapping(parent_intc->domain,
+					 irq_data->parent_irq);
+		s3c_irq_unmask(irq_get_irq_data(irqno));
+	}
+}
+
+static inline void s3c_irq_ack(struct irq_data *data)
+{
+	struct s3c_irq_data *irq_data = irq_data_get_irq_chip_data(data);
+	struct s3c_irq_intc *intc = irq_data->intc;
+	unsigned long bitval = 1UL << irq_data->offset;
+
+	__raw_writel(bitval, intc->reg_pending);
+	if (intc->reg_intpnd)
+		__raw_writel(bitval, intc->reg_intpnd);
+}
+
+static int s3c_irq_type(struct irq_data *data, unsigned int type)
+{
+	switch (type) {
+	case IRQ_TYPE_NONE:
+		break;
+	case IRQ_TYPE_EDGE_RISING:
+	case IRQ_TYPE_EDGE_FALLING:
+	case IRQ_TYPE_EDGE_BOTH:
+		irq_set_handler(data->irq, handle_edge_irq);
+		break;
+	case IRQ_TYPE_LEVEL_LOW:
+	case IRQ_TYPE_LEVEL_HIGH:
+		irq_set_handler(data->irq, handle_level_irq);
+		break;
+	default:
+		pr_err("No such irq type %d", type);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int s3c_irqext_type_set(void __iomem *gpcon_reg,
+			       void __iomem *extint_reg,
+			       unsigned long gpcon_offset,
+			       unsigned long extint_offset,
+			       unsigned int type)
+{
+	unsigned long newvalue = 0, value;
+
+	/* Set the GPIO to external interrupt mode */
+	value = __raw_readl(gpcon_reg);
+	value = (value & ~(3 << gpcon_offset)) | (0x02 << gpcon_offset);
+	__raw_writel(value, gpcon_reg);
+
+	/* Set the external interrupt to pointed trigger type */
+	switch (type)
+	{
+		case IRQ_TYPE_NONE:
+			pr_warn("No edge setting!\n");
+			break;
+
+		case IRQ_TYPE_EDGE_RISING:
+			newvalue = S3C2410_EXTINT_RISEEDGE;
+			break;
+
+		case IRQ_TYPE_EDGE_FALLING:
+			newvalue = S3C2410_EXTINT_FALLEDGE;
+			break;
+
+		case IRQ_TYPE_EDGE_BOTH:
+			newvalue = S3C2410_EXTINT_BOTHEDGE;
+			break;
+
+		case IRQ_TYPE_LEVEL_LOW:
+			newvalue = S3C2410_EXTINT_LOWLEV;
+			break;
+
+		case IRQ_TYPE_LEVEL_HIGH:
+			newvalue = S3C2410_EXTINT_HILEV;
+			break;
+
+		default:
+			pr_err("No such irq type %d", type);
+			return -EINVAL;
+	}
+
+	value = __raw_readl(extint_reg);
+	value = (value & ~(7 << extint_offset)) | (newvalue << extint_offset);
+	__raw_writel(value, extint_reg);
+
+	return 0;
+}
+
+static int s3c_irqext_type(struct irq_data *data, unsigned int type)
+{
+	void __iomem *extint_reg;
+	void __iomem *gpcon_reg;
+	unsigned long gpcon_offset, extint_offset;
+
+	if ((data->hwirq >= 4) && (data->hwirq <= 7)) {
+		gpcon_reg = S3C2410_GPFCON;
+		extint_reg = S3C24XX_EXTINT0;
+		gpcon_offset = (data->hwirq) * 2;
+		extint_offset = (data->hwirq) * 4;
+	} else if ((data->hwirq >= 8) && (data->hwirq <= 15)) {
+		gpcon_reg = S3C2410_GPGCON;
+		extint_reg = S3C24XX_EXTINT1;
+		gpcon_offset = (data->hwirq - 8) * 2;
+		extint_offset = (data->hwirq - 8) * 4;
+	} else if ((data->hwirq >= 16) && (data->hwirq <= 23)) {
+		gpcon_reg = S3C2410_GPGCON;
+		extint_reg = S3C24XX_EXTINT2;
+		gpcon_offset = (data->hwirq - 8) * 2;
+		extint_offset = (data->hwirq - 16) * 4;
+	} else {
+		return -EINVAL;
+	}
+
+	return s3c_irqext_type_set(gpcon_reg, extint_reg, gpcon_offset,
+				   extint_offset, type);
+}
+
+static int s3c_irqext0_type(struct irq_data *data, unsigned int type)
+{
+	void __iomem *extint_reg;
+	void __iomem *gpcon_reg;
+	unsigned long gpcon_offset, extint_offset;
+
+	if ((data->hwirq >= 0) && (data->hwirq <= 3)) {
+		gpcon_reg = S3C2410_GPFCON;
+		extint_reg = S3C24XX_EXTINT0;
+		gpcon_offset = (data->hwirq) * 2;
+		extint_offset = (data->hwirq) * 4;
+	} else {
+		return -EINVAL;
+	}
+
+	return s3c_irqext_type_set(gpcon_reg, extint_reg, gpcon_offset,
+				   extint_offset, type);
+}
+
+static struct irq_chip s3c_irq_chip = {
+	.name		= "s3c",
+	.irq_ack	= s3c_irq_ack,
+	.irq_mask	= s3c_irq_mask,
+	.irq_unmask	= s3c_irq_unmask,
+	.irq_set_type	= s3c_irq_type,
+	.irq_set_wake	= s3c_irq_wake
+};
+
+static struct irq_chip s3c_irq_level_chip = {
+	.name		= "s3c-level",
+	.irq_mask	= s3c_irq_mask,
+	.irq_unmask	= s3c_irq_unmask,
+	.irq_ack	= s3c_irq_ack,
+	.irq_set_type	= s3c_irq_type,
+};
+
+static struct irq_chip s3c_irqext_chip = {
+	.name		= "s3c-ext",
+	.irq_mask	= s3c_irq_mask,
+	.irq_unmask	= s3c_irq_unmask,
+	.irq_ack	= s3c_irq_ack,
+	.irq_set_type	= s3c_irqext_type,
+	.irq_set_wake	= s3c_irqext_wake
+};
+
+static struct irq_chip s3c_irq_eint0t4 = {
+	.name		= "s3c-ext0",
+	.irq_ack	= s3c_irq_ack,
+	.irq_mask	= s3c_irq_mask,
+	.irq_unmask	= s3c_irq_unmask,
+	.irq_set_wake	= s3c_irq_wake,
+	.irq_set_type	= s3c_irqext0_type,
+};
+
+static void s3c_irq_demux(unsigned int irq, struct irq_desc *desc)
+{
+	struct irq_chip *chip = irq_desc_get_chip(desc);
+	struct s3c_irq_data *irq_data = irq_desc_get_chip_data(desc);
+	struct s3c_irq_intc *intc = irq_data->intc;
+	struct s3c_irq_intc *sub_intc = irq_data->sub_intc;
+	unsigned long src;
+	unsigned long msk;
+	unsigned int n;
+	unsigned int offset;
+
+	/* we're using individual domains for the non-dt case
+	 * and one big domain for the dt case where the subintc
+	 * starts at hwirq number 32.
+	 */
+	offset = (intc->domain->of_node) ? 32 : 0;
+
+	chained_irq_enter(chip, desc);
+
+	src = __raw_readl(sub_intc->reg_pending);
+	msk = __raw_readl(sub_intc->reg_mask);
+
+	src &= ~msk;
+	src &= irq_data->sub_bits;
+
+	while (src) {
+		n = __ffs(src);
+		src &= ~(1 << n);
+		irq = irq_find_mapping(sub_intc->domain, offset + n);
+		generic_handle_irq(irq);
+	}
+
+	chained_irq_exit(chip, desc);
+}
+
+static inline int s3c24xx_handle_intc(struct s3c_irq_intc *intc,
+				      struct pt_regs *regs, int intc_offset)
+{
+	int pnd;
+	int offset;
+	int irq;
+
+	pnd = __raw_readl(intc->reg_intpnd);
+	if (!pnd)
+		return false;
+
+	/* non-dt machines use individual domains */
+	if (!intc->domain->of_node)
+		intc_offset = 0;
+
+	/* We have a problem that the INTOFFSET register does not always
+	 * show one interrupt. Occasionally we get two interrupts through
+	 * the prioritiser, and this causes the INTOFFSET register to show
+	 * what looks like the logical-or of the two interrupt numbers.
+	 *
+	 * Thanks to Klaus, Shannon, et al for helping to debug this problem
+	 */
+	offset = __raw_readl(intc->reg_intpnd + 4);
+
+	/* Find the bit manually, when the offset is wrong.
+	 * The pending register only ever contains the one bit of the next
+	 * interrupt to handle.
+	 */
+	if (!(pnd & (1 << offset)))
+		offset =  __ffs(pnd);
+
+	irq = irq_find_mapping(intc->domain, intc_offset + offset);
+	handle_IRQ(irq, regs);
+	return true;
+}
+
+asmlinkage void __exception_irq_entry s3c24xx_handle_irq(struct pt_regs *regs)
+{
+	do {
+		if (likely(s3c_intc[0]))
+			if (s3c24xx_handle_intc(s3c_intc[0], regs, 0))
+				continue;
+
+		if (s3c_intc[2])
+			if (s3c24xx_handle_intc(s3c_intc[2], regs, 64))
+				continue;
+
+		break;
+	} while (1);
+}
+
+#ifdef CONFIG_FIQ
+/**
+ * s3c24xx_set_fiq - set the FIQ routing
+ * @irq: IRQ number to route to FIQ on processor.
+ * @on: Whether to route @irq to the FIQ, or to remove the FIQ routing.
+ *
+ * Change the state of the IRQ to FIQ routing depending on @irq and @on. If
+ * @on is true, the @irq is checked to see if it can be routed and the
+ * interrupt controller updated to route the IRQ. If @on is false, the FIQ
+ * routing is cleared, regardless of which @irq is specified.
+ */
+int s3c24xx_set_fiq(unsigned int irq, bool on)
+{
+	u32 intmod;
+	unsigned offs;
+
+	if (on) {
+		offs = irq - FIQ_START;
+		if (offs > 31)
+			return -EINVAL;
+
+		intmod = 1 << offs;
+	} else {
+		intmod = 0;
+	}
+
+	__raw_writel(intmod, S3C2410_INTMOD);
+	return 0;
+}
+
+EXPORT_SYMBOL_GPL(s3c24xx_set_fiq);
+#endif
+
+static int s3c24xx_irq_map(struct irq_domain *h, unsigned int virq,
+							irq_hw_number_t hw)
+{
+	struct s3c_irq_intc *intc = h->host_data;
+	struct s3c_irq_data *irq_data = &intc->irqs[hw];
+	struct s3c_irq_intc *parent_intc;
+	struct s3c_irq_data *parent_irq_data;
+	unsigned int irqno;
+
+	/* attach controller pointer to irq_data */
+	irq_data->intc = intc;
+	irq_data->offset = hw;
+
+	parent_intc = intc->parent;
+
+	/* set handler and flags */
+	switch (irq_data->type) {
+	case S3C_IRQTYPE_NONE:
+		return 0;
+	case S3C_IRQTYPE_EINT:
+		/* On the S3C2412, the EINT0to3 have a parent irq
+		 * but need the s3c_irq_eint0t4 chip
+		 */
+		if (parent_intc && (!soc_is_s3c2412() || hw >= 4))
+			irq_set_chip_and_handler(virq, &s3c_irqext_chip,
+						 handle_edge_irq);
+		else
+			irq_set_chip_and_handler(virq, &s3c_irq_eint0t4,
+						 handle_edge_irq);
+		break;
+	case S3C_IRQTYPE_EDGE:
+		if (parent_intc || intc->reg_pending == S3C2416_SRCPND2)
+			irq_set_chip_and_handler(virq, &s3c_irq_level_chip,
+						 handle_edge_irq);
+		else
+			irq_set_chip_and_handler(virq, &s3c_irq_chip,
+						 handle_edge_irq);
+		break;
+	case S3C_IRQTYPE_LEVEL:
+		if (parent_intc)
+			irq_set_chip_and_handler(virq, &s3c_irq_level_chip,
+						 handle_level_irq);
+		else
+			irq_set_chip_and_handler(virq, &s3c_irq_chip,
+						 handle_level_irq);
+		break;
+	default:
+		pr_err("irq-s3c24xx: unsupported irqtype %d\n", irq_data->type);
+		return -EINVAL;
+	}
+
+	irq_set_chip_data(virq, irq_data);
+
+	set_irq_flags(virq, IRQF_VALID);
+
+	if (parent_intc && irq_data->type != S3C_IRQTYPE_NONE) {
+		if (irq_data->parent_irq > 31) {
+			pr_err("irq-s3c24xx: parent irq %lu is out of range\n",
+			       irq_data->parent_irq);
+			goto err;
+		}
+
+		parent_irq_data = &parent_intc->irqs[irq_data->parent_irq];
+		parent_irq_data->sub_intc = intc;
+		parent_irq_data->sub_bits |= (1UL << hw);
+
+		/* attach the demuxer to the parent irq */
+		irqno = irq_find_mapping(parent_intc->domain,
+					 irq_data->parent_irq);
+		if (!irqno) {
+			pr_err("irq-s3c24xx: could not find mapping for parent irq %lu\n",
+			       irq_data->parent_irq);
+			goto err;
+		}
+		irq_set_chained_handler(irqno, s3c_irq_demux);
+	}
+
+	return 0;
+
+err:
+	set_irq_flags(virq, 0);
+
+	/* the only error can result from bad mapping data*/
+	return -EINVAL;
+}
+
+static struct irq_domain_ops s3c24xx_irq_ops = {
+	.map = s3c24xx_irq_map,
+	.xlate = irq_domain_xlate_twocell,
+};
+
+static void s3c24xx_clear_intc(struct s3c_irq_intc *intc)
+{
+	void __iomem *reg_source;
+	unsigned long pend;
+	unsigned long last;
+	int i;
+
+	/* if intpnd is set, read the next pending irq from there */
+	reg_source = intc->reg_intpnd ? intc->reg_intpnd : intc->reg_pending;
+
+	last = 0;
+	for (i = 0; i < 4; i++) {
+		pend = __raw_readl(reg_source);
+
+		if (pend == 0 || pend == last)
+			break;
+
+		__raw_writel(pend, intc->reg_pending);
+		if (intc->reg_intpnd)
+			__raw_writel(pend, intc->reg_intpnd);
+
+		pr_info("irq: clearing pending status %08x\n", (int)pend);
+		last = pend;
+	}
+}
+
+static struct s3c_irq_intc * __init s3c24xx_init_intc(struct device_node *np,
+				       struct s3c_irq_data *irq_data,
+				       struct s3c_irq_intc *parent,
+				       unsigned long address)
+{
+	struct s3c_irq_intc *intc;
+	void __iomem *base = (void *)0xf6000000; /* static mapping */
+	int irq_num;
+	int irq_start;
+	int ret;
+
+	intc = kzalloc(sizeof(struct s3c_irq_intc), GFP_KERNEL);
+	if (!intc)
+		return ERR_PTR(-ENOMEM);
+
+	intc->irqs = irq_data;
+
+	if (parent)
+		intc->parent = parent;
+
+	/* select the correct data for the controller.
+	 * Need to hard code the irq num start and offset
+	 * to preserve the static mapping for now
+	 */
+	switch (address) {
+	case 0x4a000000:
+		pr_debug("irq: found main intc\n");
+		intc->reg_pending = base;
+		intc->reg_mask = base + 0x08;
+		intc->reg_intpnd = base + 0x10;
+		irq_num = 32;
+		irq_start = S3C2410_IRQ(0);
+		break;
+	case 0x4a000018:
+		pr_debug("irq: found subintc\n");
+		intc->reg_pending = base + 0x18;
+		intc->reg_mask = base + 0x1c;
+		irq_num = 29;
+		irq_start = S3C2410_IRQSUB(0);
+		break;
+	case 0x4a000040:
+		pr_debug("irq: found intc2\n");
+		intc->reg_pending = base + 0x40;
+		intc->reg_mask = base + 0x48;
+		intc->reg_intpnd = base + 0x50;
+		irq_num = 8;
+		irq_start = S3C2416_IRQ(0);
+		break;
+	case 0x560000a4:
+		pr_debug("irq: found eintc\n");
+		base = (void *)0xfd000000;
+
+		intc->reg_mask = base + 0xa4;
+		intc->reg_pending = base + 0xa8;
+		irq_num = 24;
+		irq_start = S3C2410_IRQ(32);
+		break;
+	default:
+		pr_err("irq: unsupported controller address\n");
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* now that all the data is complete, init the irq-domain */
+	s3c24xx_clear_intc(intc);
+	intc->domain = irq_domain_add_legacy(np, irq_num, irq_start,
+					     0, &s3c24xx_irq_ops,
+					     intc);
+	if (!intc->domain) {
+		pr_err("irq: could not create irq-domain\n");
+		ret = -EINVAL;
+		goto err;
+	}
+
+	set_handle_irq(s3c24xx_handle_irq);
+
+	return intc;
+
+err:
+	kfree(intc);
+	return ERR_PTR(ret);
+}
+
+static struct s3c_irq_data init_eint[32] = {
+	{ .type = S3C_IRQTYPE_NONE, }, /* reserved */
+	{ .type = S3C_IRQTYPE_NONE, }, /* reserved */
+	{ .type = S3C_IRQTYPE_NONE, }, /* reserved */
+	{ .type = S3C_IRQTYPE_NONE, }, /* reserved */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 4 }, /* EINT4 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 4 }, /* EINT5 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 4 }, /* EINT6 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 4 }, /* EINT7 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT8 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT9 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT10 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT11 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT12 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT13 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT14 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT15 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT16 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT17 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT18 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT19 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT20 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT21 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT22 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT23 */
+};
+
+#ifdef CONFIG_CPU_S3C2410
+static struct s3c_irq_data init_s3c2410base[32] = {
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT0 */
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT1 */
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT2 */
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT3 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT4to7 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT8to23 */
+	{ .type = S3C_IRQTYPE_NONE, }, /* reserved */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* nBATT_FLT */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TICK */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* WDT */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER2 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER3 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER4 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART2 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* LCD */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA2 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA3 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SDI */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SPI0 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART1 */
+	{ .type = S3C_IRQTYPE_NONE, }, /* reserved */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* USBD */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* USBH */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* IIC */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SPI1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* RTC */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* ADCPARENT */
+};
+
+static struct s3c_irq_data init_s3c2410subint[32] = {
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-ERR */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-ERR */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-ERR */
+	{ .type = S3C_IRQTYPE_EDGE, .parent_irq = 31 }, /* TC */
+	{ .type = S3C_IRQTYPE_EDGE, .parent_irq = 31 }, /* ADC */
+};
+
+void __init s3c2410_init_irq(void)
+{
+#ifdef CONFIG_FIQ
+	init_FIQ(FIQ_START);
+#endif
+
+	s3c_intc[0] = s3c24xx_init_intc(NULL, &init_s3c2410base[0], NULL,
+					0x4a000000);
+	if (IS_ERR(s3c_intc[0])) {
+		pr_err("irq: could not create main interrupt controller\n");
+		return;
+	}
+
+	s3c_intc[1] = s3c24xx_init_intc(NULL, &init_s3c2410subint[0],
+					s3c_intc[0], 0x4a000018);
+	s3c24xx_init_intc(NULL, &init_eint[0], s3c_intc[0], 0x560000a4);
+}
+#endif
+
+#ifdef CONFIG_CPU_S3C2412
+static struct s3c_irq_data init_s3c2412base[32] = {
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT0 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT1 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT2 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT3 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT4to7 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT8to23 */
+	{ .type = S3C_IRQTYPE_NONE, }, /* reserved */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* nBATT_FLT */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TICK */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* WDT */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER2 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER3 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER4 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART2 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* LCD */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA2 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA3 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* SDI/CF */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SPI0 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART1 */
+	{ .type = S3C_IRQTYPE_NONE, }, /* reserved */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* USBD */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* USBH */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* IIC */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SPI1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* RTC */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* ADCPARENT */
+};
+
+static struct s3c_irq_data init_s3c2412eint[32] = {
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 0 }, /* EINT0 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 1 }, /* EINT1 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 2 }, /* EINT2 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 3 }, /* EINT3 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 4 }, /* EINT4 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 4 }, /* EINT5 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 4 }, /* EINT6 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 4 }, /* EINT7 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT8 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT9 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT10 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT11 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT12 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT13 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT14 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT15 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT16 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT17 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT18 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT19 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT20 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT21 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT22 */
+	{ .type = S3C_IRQTYPE_EINT, .parent_irq = 5 }, /* EINT23 */
+};
+
+static struct s3c_irq_data init_s3c2412subint[32] = {
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-ERR */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-ERR */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-ERR */
+	{ .type = S3C_IRQTYPE_EDGE, .parent_irq = 31 }, /* TC */
+	{ .type = S3C_IRQTYPE_EDGE, .parent_irq = 31 }, /* ADC */
+	{ .type = S3C_IRQTYPE_NONE, },
+	{ .type = S3C_IRQTYPE_NONE, },
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 21 }, /* SDI */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 21 }, /* CF */
+};
+
+void __init s3c2412_init_irq(void)
+{
+	pr_info("S3C2412: IRQ Support\n");
+
+#ifdef CONFIG_FIQ
+	init_FIQ(FIQ_START);
+#endif
+
+	s3c_intc[0] = s3c24xx_init_intc(NULL, &init_s3c2412base[0], NULL,
+					0x4a000000);
+	if (IS_ERR(s3c_intc[0])) {
+		pr_err("irq: could not create main interrupt controller\n");
+		return;
+	}
+
+	s3c24xx_init_intc(NULL, &init_s3c2412eint[0], s3c_intc[0], 0x560000a4);
+	s3c_intc[1] = s3c24xx_init_intc(NULL, &init_s3c2412subint[0],
+					s3c_intc[0], 0x4a000018);
+}
+#endif
+
+#ifdef CONFIG_CPU_S3C2416
+static struct s3c_irq_data init_s3c2416base[32] = {
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT0 */
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT1 */
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT2 */
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT3 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT4to7 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT8to23 */
+	{ .type = S3C_IRQTYPE_NONE, }, /* reserved */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* nBATT_FLT */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TICK */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* WDT/AC97 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER2 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER3 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER4 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART2 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* LCD */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* DMA */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART3 */
+	{ .type = S3C_IRQTYPE_NONE, }, /* reserved */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SDI1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SDI0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SPI0 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* NAND */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* USBD */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* USBH */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* IIC */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART0 */
+	{ .type = S3C_IRQTYPE_NONE, },
+	{ .type = S3C_IRQTYPE_EDGE, }, /* RTC */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* ADCPARENT */
+};
+
+static struct s3c_irq_data init_s3c2416subint[32] = {
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-ERR */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-ERR */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-ERR */
+	{ .type = S3C_IRQTYPE_EDGE, .parent_irq = 31 }, /* TC */
+	{ .type = S3C_IRQTYPE_EDGE, .parent_irq = 31 }, /* ADC */
+	{ .type = S3C_IRQTYPE_NONE }, /* reserved */
+	{ .type = S3C_IRQTYPE_NONE }, /* reserved */
+	{ .type = S3C_IRQTYPE_NONE }, /* reserved */
+	{ .type = S3C_IRQTYPE_NONE }, /* reserved */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 16 }, /* LCD2 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 16 }, /* LCD3 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 16 }, /* LCD4 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 17 }, /* DMA0 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 17 }, /* DMA1 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 17 }, /* DMA2 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 17 }, /* DMA3 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 17 }, /* DMA4 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 17 }, /* DMA5 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 18 }, /* UART3-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 18 }, /* UART3-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 18 }, /* UART3-ERR */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 9 }, /* WDT */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 9 }, /* AC97 */
+};
+
+static struct s3c_irq_data init_s3c2416_second[32] = {
+	{ .type = S3C_IRQTYPE_EDGE }, /* 2D */
+	{ .type = S3C_IRQTYPE_NONE }, /* reserved */
+	{ .type = S3C_IRQTYPE_NONE }, /* reserved */
+	{ .type = S3C_IRQTYPE_NONE }, /* reserved */
+	{ .type = S3C_IRQTYPE_EDGE }, /* PCM0 */
+	{ .type = S3C_IRQTYPE_NONE }, /* reserved */
+	{ .type = S3C_IRQTYPE_EDGE }, /* I2S0 */
+};
+
+void __init s3c2416_init_irq(void)
+{
+	pr_info("S3C2416: IRQ Support\n");
+
+#ifdef CONFIG_FIQ
+	init_FIQ(FIQ_START);
+#endif
+
+	s3c_intc[0] = s3c24xx_init_intc(NULL, &init_s3c2416base[0], NULL,
+					0x4a000000);
+	if (IS_ERR(s3c_intc[0])) {
+		pr_err("irq: could not create main interrupt controller\n");
+		return;
+	}
+
+	s3c24xx_init_intc(NULL, &init_eint[0], s3c_intc[0], 0x560000a4);
+	s3c_intc[1] = s3c24xx_init_intc(NULL, &init_s3c2416subint[0],
+					s3c_intc[0], 0x4a000018);
+
+	s3c_intc[2] = s3c24xx_init_intc(NULL, &init_s3c2416_second[0],
+					NULL, 0x4a000040);
+}
+
+#endif
+
+#ifdef CONFIG_CPU_S3C2440
+static struct s3c_irq_data init_s3c2440base[32] = {
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT0 */
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT1 */
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT2 */
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT3 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT4to7 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT8to23 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* CAM */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* nBATT_FLT */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TICK */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* WDT/AC97 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER2 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER3 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER4 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART2 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* LCD */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA2 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA3 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SDI */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SPI0 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART1 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* NFCON */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* USBD */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* USBH */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* IIC */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SPI1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* RTC */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* ADCPARENT */
+};
+
+static struct s3c_irq_data init_s3c2440subint[32] = {
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-ERR */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-ERR */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-ERR */
+	{ .type = S3C_IRQTYPE_EDGE, .parent_irq = 31 }, /* TC */
+	{ .type = S3C_IRQTYPE_EDGE, .parent_irq = 31 }, /* ADC */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 6 }, /* CAM_C */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 6 }, /* CAM_P */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 9 }, /* WDT */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 9 }, /* AC97 */
+};
+
+void __init s3c2440_init_irq(void)
+{
+	pr_info("S3C2440: IRQ Support\n");
+
+#ifdef CONFIG_FIQ
+	init_FIQ(FIQ_START);
+#endif
+
+	s3c_intc[0] = s3c24xx_init_intc(NULL, &init_s3c2440base[0], NULL,
+					0x4a000000);
+	if (IS_ERR(s3c_intc[0])) {
+		pr_err("irq: could not create main interrupt controller\n");
+		return;
+	}
+
+	s3c24xx_init_intc(NULL, &init_eint[0], s3c_intc[0], 0x560000a4);
+	s3c_intc[1] = s3c24xx_init_intc(NULL, &init_s3c2440subint[0],
+					s3c_intc[0], 0x4a000018);
+}
+#endif
+
+#ifdef CONFIG_CPU_S3C2442
+static struct s3c_irq_data init_s3c2442base[32] = {
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT0 */
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT1 */
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT2 */
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT3 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT4to7 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT8to23 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* CAM */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* nBATT_FLT */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TICK */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* WDT */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER2 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER3 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER4 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART2 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* LCD */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA2 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* DMA3 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SDI */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SPI0 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART1 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* NFCON */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* USBD */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* USBH */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* IIC */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SPI1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* RTC */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* ADCPARENT */
+};
+
+static struct s3c_irq_data init_s3c2442subint[32] = {
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-ERR */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-ERR */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-ERR */
+	{ .type = S3C_IRQTYPE_EDGE, .parent_irq = 31 }, /* TC */
+	{ .type = S3C_IRQTYPE_EDGE, .parent_irq = 31 }, /* ADC */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 6 }, /* CAM_C */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 6 }, /* CAM_P */
+};
+
+void __init s3c2442_init_irq(void)
+{
+	pr_info("S3C2442: IRQ Support\n");
+
+#ifdef CONFIG_FIQ
+	init_FIQ(FIQ_START);
+#endif
+
+	s3c_intc[0] = s3c24xx_init_intc(NULL, &init_s3c2442base[0], NULL,
+					0x4a000000);
+	if (IS_ERR(s3c_intc[0])) {
+		pr_err("irq: could not create main interrupt controller\n");
+		return;
+	}
+
+	s3c24xx_init_intc(NULL, &init_eint[0], s3c_intc[0], 0x560000a4);
+	s3c_intc[1] = s3c24xx_init_intc(NULL, &init_s3c2442subint[0],
+					s3c_intc[0], 0x4a000018);
+}
+#endif
+
+#ifdef CONFIG_CPU_S3C2443
+static struct s3c_irq_data init_s3c2443base[32] = {
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT0 */
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT1 */
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT2 */
+	{ .type = S3C_IRQTYPE_EINT, }, /* EINT3 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT4to7 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* EINT8to23 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* CAM */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* nBATT_FLT */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TICK */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* WDT/AC97 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER2 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER3 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* TIMER4 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART2 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* LCD */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* DMA */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART3 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* CFON */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SDI1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SDI0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SPI0 */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* NAND */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* USBD */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* USBH */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* IIC */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* UART0 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* SPI1 */
+	{ .type = S3C_IRQTYPE_EDGE, }, /* RTC */
+	{ .type = S3C_IRQTYPE_LEVEL, }, /* ADCPARENT */
+};
+
+
+static struct s3c_irq_data init_s3c2443subint[32] = {
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 28 }, /* UART0-ERR */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 23 }, /* UART1-ERR */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 15 }, /* UART2-ERR */
+	{ .type = S3C_IRQTYPE_EDGE, .parent_irq = 31 }, /* TC */
+	{ .type = S3C_IRQTYPE_EDGE, .parent_irq = 31 }, /* ADC */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 6 }, /* CAM_C */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 6 }, /* CAM_P */
+	{ .type = S3C_IRQTYPE_NONE }, /* reserved */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 16 }, /* LCD1 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 16 }, /* LCD2 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 16 }, /* LCD3 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 16 }, /* LCD4 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 17 }, /* DMA0 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 17 }, /* DMA1 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 17 }, /* DMA2 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 17 }, /* DMA3 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 17 }, /* DMA4 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 17 }, /* DMA5 */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 18 }, /* UART3-RX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 18 }, /* UART3-TX */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 18 }, /* UART3-ERR */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 9 }, /* WDT */
+	{ .type = S3C_IRQTYPE_LEVEL, .parent_irq = 9 }, /* AC97 */
+};
+
+void __init s3c2443_init_irq(void)
+{
+	pr_info("S3C2443: IRQ Support\n");
+
+#ifdef CONFIG_FIQ
+	init_FIQ(FIQ_START);
+#endif
+
+	s3c_intc[0] = s3c24xx_init_intc(NULL, &init_s3c2443base[0], NULL,
+					0x4a000000);
+	if (IS_ERR(s3c_intc[0])) {
+		pr_err("irq: could not create main interrupt controller\n");
+		return;
+	}
+
+	s3c24xx_init_intc(NULL, &init_eint[0], s3c_intc[0], 0x560000a4);
+	s3c_intc[1] = s3c24xx_init_intc(NULL, &init_s3c2443subint[0],
+					s3c_intc[0], 0x4a000018);
+}
+#endif
+
+#ifdef CONFIG_OF
+static int s3c24xx_irq_map_of(struct irq_domain *h, unsigned int virq,
+							irq_hw_number_t hw)
+{
+	unsigned int ctrl_num = hw / 32;
+	unsigned int intc_hw = hw % 32;
+	struct s3c_irq_intc *intc = s3c_intc[ctrl_num];
+	struct s3c_irq_intc *parent_intc = intc->parent;
+	struct s3c_irq_data *irq_data = &intc->irqs[intc_hw];
+
+	/* attach controller pointer to irq_data */
+	irq_data->intc = intc;
+	irq_data->offset = intc_hw;
+
+	if (!parent_intc)
+		irq_set_chip_and_handler(virq, &s3c_irq_chip, handle_edge_irq);
+	else
+		irq_set_chip_and_handler(virq, &s3c_irq_level_chip,
+					 handle_edge_irq);
+
+	irq_set_chip_data(virq, irq_data);
+
+	set_irq_flags(virq, IRQF_VALID);
+
+	return 0;
+}
+
+/* Translate our of irq notation
+ * format: <ctrl_num ctrl_irq parent_irq type>
+ */
+static int s3c24xx_irq_xlate_of(struct irq_domain *d, struct device_node *n,
+			const u32 *intspec, unsigned int intsize,
+			irq_hw_number_t *out_hwirq, unsigned int *out_type)
+{
+	struct s3c_irq_intc *intc;
+	struct s3c_irq_intc *parent_intc;
+	struct s3c_irq_data *irq_data;
+	struct s3c_irq_data *parent_irq_data;
+	int irqno;
+
+	if (WARN_ON(intsize < 4))
+		return -EINVAL;
+
+	if (intspec[0] > 2 || !s3c_intc[intspec[0]]) {
+		pr_err("controller number %d invalid\n", intspec[0]);
+		return -EINVAL;
+	}
+	intc = s3c_intc[intspec[0]];
+
+	*out_hwirq = intspec[0] * 32 + intspec[2];
+	*out_type = intspec[3] & IRQ_TYPE_SENSE_MASK;
+
+	parent_intc = intc->parent;
+	if (parent_intc) {
+		irq_data = &intc->irqs[intspec[2]];
+		irq_data->parent_irq = intspec[1];
+		parent_irq_data = &parent_intc->irqs[irq_data->parent_irq];
+		parent_irq_data->sub_intc = intc;
+		parent_irq_data->sub_bits |= (1UL << intspec[2]);
+
+		/* parent_intc is always s3c_intc[0], so no offset */
+		irqno = irq_create_mapping(parent_intc->domain, intspec[1]);
+		if (irqno < 0) {
+			pr_err("irq: could not map parent interrupt\n");
+			return irqno;
+		}
+
+		irq_set_chained_handler(irqno, s3c_irq_demux);
+	}
+
+	return 0;
+}
+
+static struct irq_domain_ops s3c24xx_irq_ops_of = {
+	.map = s3c24xx_irq_map_of,
+	.xlate = s3c24xx_irq_xlate_of,
+};
+
+struct s3c24xx_irq_of_ctrl {
+	char			*name;
+	unsigned long		offset;
+	struct s3c_irq_intc	**handle;
+	struct s3c_irq_intc	**parent;
+	struct irq_domain_ops	*ops;
+};
+
+static int __init s3c_init_intc_of(struct device_node *np,
+			struct device_node *interrupt_parent,
+			struct s3c24xx_irq_of_ctrl *s3c_ctrl, int num_ctrl)
+{
+	struct s3c_irq_intc *intc;
+	struct s3c24xx_irq_of_ctrl *ctrl;
+	struct irq_domain *domain;
+	void __iomem *reg_base;
+	int i;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base) {
+		pr_err("irq-s3c24xx: could not map irq registers\n");
+		return -EINVAL;
+	}
+
+	domain = irq_domain_add_linear(np, num_ctrl * 32,
+						     &s3c24xx_irq_ops_of, NULL);
+	if (!domain) {
+		pr_err("irq: could not create irq-domain\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < num_ctrl; i++) {
+		ctrl = &s3c_ctrl[i];
+
+		pr_debug("irq: found controller %s\n", ctrl->name);
+
+		intc = kzalloc(sizeof(struct s3c_irq_intc), GFP_KERNEL);
+		if (!intc)
+			return -ENOMEM;
+
+		intc->domain = domain;
+		intc->irqs = kzalloc(sizeof(struct s3c_irq_data) * 32,
+				     GFP_KERNEL);
+		if (!intc->irqs) {
+			kfree(intc);
+			return -ENOMEM;
+		}
+
+		if (ctrl->parent) {
+			intc->reg_pending = reg_base + ctrl->offset;
+			intc->reg_mask = reg_base + ctrl->offset + 0x4;
+
+			if (*(ctrl->parent)) {
+				intc->parent = *(ctrl->parent);
+			} else {
+				pr_warn("irq: parent of %s missing\n",
+					ctrl->name);
+				kfree(intc->irqs);
+				kfree(intc);
+				continue;
+			}
+		} else {
+			intc->reg_pending = reg_base + ctrl->offset;
+			intc->reg_mask = reg_base + ctrl->offset + 0x08;
+			intc->reg_intpnd = reg_base + ctrl->offset + 0x10;
+		}
+
+		s3c24xx_clear_intc(intc);
+		s3c_intc[i] = intc;
+	}
+
+	set_handle_irq(s3c24xx_handle_irq);
+
+	return 0;
+}
+
+static struct s3c24xx_irq_of_ctrl s3c2410_ctrl[] = {
+	{
+		.name = "intc",
+		.offset = 0,
+	}, {
+		.name = "subintc",
+		.offset = 0x18,
+		.parent = &s3c_intc[0],
+	}
+};
+
+int __init s3c2410_init_intc_of(struct device_node *np,
+			struct device_node *interrupt_parent,
+			struct s3c24xx_irq_of_ctrl *ctrl, int num_ctrl)
+{
+	return s3c_init_intc_of(np, interrupt_parent,
+				s3c2410_ctrl, ARRAY_SIZE(s3c2410_ctrl));
+}
+IRQCHIP_DECLARE(s3c2410_irq, "samsung,s3c2410-irq", s3c2410_init_intc_of);
+
+static struct s3c24xx_irq_of_ctrl s3c2416_ctrl[] = {
+	{
+		.name = "intc",
+		.offset = 0,
+	}, {
+		.name = "subintc",
+		.offset = 0x18,
+		.parent = &s3c_intc[0],
+	}, {
+		.name = "intc2",
+		.offset = 0x40,
+	}
+};
+
+int __init s3c2416_init_intc_of(struct device_node *np,
+			struct device_node *interrupt_parent,
+			struct s3c24xx_irq_of_ctrl *ctrl, int num_ctrl)
+{
+	return s3c_init_intc_of(np, interrupt_parent,
+				s3c2416_ctrl, ARRAY_SIZE(s3c2416_ctrl));
+}
+IRQCHIP_DECLARE(s3c2416_irq, "samsung,s3c2416-irq", s3c2416_init_intc_of);
+#endif
diff --git a/drivers/irqchip/irq-sirfsoc.c b/drivers/irqchip/irq-sirfsoc.c
new file mode 100644
index 000000000000..69ea44ebcf61
--- /dev/null
+++ b/drivers/irqchip/irq-sirfsoc.c
@@ -0,0 +1,126 @@
+/*
+ * interrupt controller support for CSR SiRFprimaII
+ *
+ * Copyright (c) 2011 Cambridge Silicon Radio Limited, a CSR plc group company.
+ *
+ * Licensed under GPLv2 or later.
+ */
+
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/irqdomain.h>
+#include <linux/syscore_ops.h>
+#include <asm/mach/irq.h>
+#include <asm/exception.h>
+#include "irqchip.h"
+
+#define SIRFSOC_INT_RISC_MASK0          0x0018
+#define SIRFSOC_INT_RISC_MASK1          0x001C
+#define SIRFSOC_INT_RISC_LEVEL0         0x0020
+#define SIRFSOC_INT_RISC_LEVEL1         0x0024
+#define SIRFSOC_INIT_IRQ_ID		0x0038
+
+#define SIRFSOC_NUM_IRQS		128
+
+static struct irq_domain *sirfsoc_irqdomain;
+
+static __init void
+sirfsoc_alloc_gc(void __iomem *base, unsigned int irq_start, unsigned int num)
+{
+	struct irq_chip_generic *gc;
+	struct irq_chip_type *ct;
+
+	gc = irq_alloc_generic_chip("SIRFINTC", 1, irq_start, base, handle_level_irq);
+	ct = gc->chip_types;
+
+	ct->chip.irq_mask = irq_gc_mask_clr_bit;
+	ct->chip.irq_unmask = irq_gc_mask_set_bit;
+	ct->regs.mask = SIRFSOC_INT_RISC_MASK0;
+
+	irq_setup_generic_chip(gc, IRQ_MSK(num), IRQ_GC_INIT_MASK_CACHE, IRQ_NOREQUEST, 0);
+}
+
+static asmlinkage void __exception_irq_entry sirfsoc_handle_irq(struct pt_regs *regs)
+{
+	void __iomem *base = sirfsoc_irqdomain->host_data;
+	u32 irqstat, irqnr;
+
+	irqstat = readl_relaxed(base + SIRFSOC_INIT_IRQ_ID);
+	irqnr = irq_find_mapping(sirfsoc_irqdomain, irqstat & 0xff);
+
+	handle_IRQ(irqnr, regs);
+}
+
+static int __init sirfsoc_irq_init(struct device_node *np, struct device_node *parent)
+{
+	void __iomem *base = of_iomap(np, 0);
+	if (!base)
+		panic("unable to map intc cpu registers\n");
+
+	/* using legacy because irqchip_generic does not work with linear */
+	sirfsoc_irqdomain = irq_domain_add_legacy(np, SIRFSOC_NUM_IRQS, 0, 0,
+				 &irq_domain_simple_ops, base);
+
+	sirfsoc_alloc_gc(base, 0, 32);
+	sirfsoc_alloc_gc(base + 4, 32, SIRFSOC_NUM_IRQS - 32);
+
+	writel_relaxed(0, base + SIRFSOC_INT_RISC_LEVEL0);
+	writel_relaxed(0, base + SIRFSOC_INT_RISC_LEVEL1);
+
+	writel_relaxed(0, base + SIRFSOC_INT_RISC_MASK0);
+	writel_relaxed(0, base + SIRFSOC_INT_RISC_MASK1);
+
+	set_handle_irq(sirfsoc_handle_irq);
+
+	return 0;
+}
+IRQCHIP_DECLARE(sirfsoc_intc, "sirf,prima2-intc", sirfsoc_irq_init);
+
+struct sirfsoc_irq_status {
+	u32 mask0;
+	u32 mask1;
+	u32 level0;
+	u32 level1;
+};
+
+static struct sirfsoc_irq_status sirfsoc_irq_st;
+
+static int sirfsoc_irq_suspend(void)
+{
+	void __iomem *base = sirfsoc_irqdomain->host_data;
+
+	sirfsoc_irq_st.mask0 = readl_relaxed(base + SIRFSOC_INT_RISC_MASK0);
+	sirfsoc_irq_st.mask1 = readl_relaxed(base + SIRFSOC_INT_RISC_MASK1);
+	sirfsoc_irq_st.level0 = readl_relaxed(base + SIRFSOC_INT_RISC_LEVEL0);
+	sirfsoc_irq_st.level1 = readl_relaxed(base + SIRFSOC_INT_RISC_LEVEL1);
+
+	return 0;
+}
+
+static void sirfsoc_irq_resume(void)
+{
+	void __iomem *base = sirfsoc_irqdomain->host_data;
+
+	writel_relaxed(sirfsoc_irq_st.mask0, base + SIRFSOC_INT_RISC_MASK0);
+	writel_relaxed(sirfsoc_irq_st.mask1, base + SIRFSOC_INT_RISC_MASK1);
+	writel_relaxed(sirfsoc_irq_st.level0, base + SIRFSOC_INT_RISC_LEVEL0);
+	writel_relaxed(sirfsoc_irq_st.level1, base + SIRFSOC_INT_RISC_LEVEL1);
+}
+
+static struct syscore_ops sirfsoc_irq_syscore_ops = {
+	.suspend	= sirfsoc_irq_suspend,
+	.resume		= sirfsoc_irq_resume,
+};
+
+static int __init sirfsoc_irq_pm_init(void)
+{
+	if (!sirfsoc_irqdomain)
+		return 0;
+
+	register_syscore_ops(&sirfsoc_irq_syscore_ops);
+	return 0;
+}
+device_initcall(sirfsoc_irq_pm_init);
diff --git a/drivers/irqchip/irq-sun4i.c b/drivers/irqchip/irq-sun4i.c
new file mode 100644
index 000000000000..b66d4ae06898
--- /dev/null
+++ b/drivers/irqchip/irq-sun4i.c
@@ -0,0 +1,149 @@
+/*
+ * Allwinner A1X SoCs IRQ chip driver.
+ *
+ * Copyright (C) 2012 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * Based on code from
+ * Allwinner Technology Co., Ltd. <www.allwinnertech.com>
+ * Benn Huang <benn@allwinnertech.com>
+ *
+ * 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/io.h>
+#include <linux/irq.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+
+#include <asm/exception.h>
+#include <asm/mach/irq.h>
+
+#include "irqchip.h"
+
+#define SUN4I_IRQ_VECTOR_REG		0x00
+#define SUN4I_IRQ_PROTECTION_REG	0x08
+#define SUN4I_IRQ_NMI_CTRL_REG		0x0c
+#define SUN4I_IRQ_PENDING_REG(x)	(0x10 + 0x4 * x)
+#define SUN4I_IRQ_FIQ_PENDING_REG(x)	(0x20 + 0x4 * x)
+#define SUN4I_IRQ_ENABLE_REG(x)		(0x40 + 0x4 * x)
+#define SUN4I_IRQ_MASK_REG(x)		(0x50 + 0x4 * x)
+
+static void __iomem *sun4i_irq_base;
+static struct irq_domain *sun4i_irq_domain;
+
+static asmlinkage void __exception_irq_entry sun4i_handle_irq(struct pt_regs *regs);
+
+void sun4i_irq_ack(struct irq_data *irqd)
+{
+	unsigned int irq = irqd_to_hwirq(irqd);
+	unsigned int irq_off = irq % 32;
+	int reg = irq / 32;
+	u32 val;
+
+	val = readl(sun4i_irq_base + SUN4I_IRQ_PENDING_REG(reg));
+	writel(val | (1 << irq_off),
+	       sun4i_irq_base + SUN4I_IRQ_PENDING_REG(reg));
+}
+
+static void sun4i_irq_mask(struct irq_data *irqd)
+{
+	unsigned int irq = irqd_to_hwirq(irqd);
+	unsigned int irq_off = irq % 32;
+	int reg = irq / 32;
+	u32 val;
+
+	val = readl(sun4i_irq_base + SUN4I_IRQ_ENABLE_REG(reg));
+	writel(val & ~(1 << irq_off),
+	       sun4i_irq_base + SUN4I_IRQ_ENABLE_REG(reg));
+}
+
+static void sun4i_irq_unmask(struct irq_data *irqd)
+{
+	unsigned int irq = irqd_to_hwirq(irqd);
+	unsigned int irq_off = irq % 32;
+	int reg = irq / 32;
+	u32 val;
+
+	val = readl(sun4i_irq_base + SUN4I_IRQ_ENABLE_REG(reg));
+	writel(val | (1 << irq_off),
+	       sun4i_irq_base + SUN4I_IRQ_ENABLE_REG(reg));
+}
+
+static struct irq_chip sun4i_irq_chip = {
+	.name		= "sun4i_irq",
+	.irq_ack	= sun4i_irq_ack,
+	.irq_mask	= sun4i_irq_mask,
+	.irq_unmask	= sun4i_irq_unmask,
+};
+
+static int sun4i_irq_map(struct irq_domain *d, unsigned int virq,
+			 irq_hw_number_t hw)
+{
+	irq_set_chip_and_handler(virq, &sun4i_irq_chip,
+				 handle_level_irq);
+	set_irq_flags(virq, IRQF_VALID | IRQF_PROBE);
+
+	return 0;
+}
+
+static struct irq_domain_ops sun4i_irq_ops = {
+	.map = sun4i_irq_map,
+	.xlate = irq_domain_xlate_onecell,
+};
+
+static int __init sun4i_of_init(struct device_node *node,
+				struct device_node *parent)
+{
+	sun4i_irq_base = of_iomap(node, 0);
+	if (!sun4i_irq_base)
+		panic("%s: unable to map IC registers\n",
+			node->full_name);
+
+	/* Disable all interrupts */
+	writel(0, sun4i_irq_base + SUN4I_IRQ_ENABLE_REG(0));
+	writel(0, sun4i_irq_base + SUN4I_IRQ_ENABLE_REG(1));
+	writel(0, sun4i_irq_base + SUN4I_IRQ_ENABLE_REG(2));
+
+	/* Mask all the interrupts */
+	writel(0, sun4i_irq_base + SUN4I_IRQ_MASK_REG(0));
+	writel(0, sun4i_irq_base + SUN4I_IRQ_MASK_REG(1));
+	writel(0, sun4i_irq_base + SUN4I_IRQ_MASK_REG(2));
+
+	/* Clear all the pending interrupts */
+	writel(0xffffffff, sun4i_irq_base + SUN4I_IRQ_PENDING_REG(0));
+	writel(0xffffffff, sun4i_irq_base + SUN4I_IRQ_PENDING_REG(1));
+	writel(0xffffffff, sun4i_irq_base + SUN4I_IRQ_PENDING_REG(2));
+
+	/* Enable protection mode */
+	writel(0x01, sun4i_irq_base + SUN4I_IRQ_PROTECTION_REG);
+
+	/* Configure the external interrupt source type */
+	writel(0x00, sun4i_irq_base + SUN4I_IRQ_NMI_CTRL_REG);
+
+	sun4i_irq_domain = irq_domain_add_linear(node, 3 * 32,
+						 &sun4i_irq_ops, NULL);
+	if (!sun4i_irq_domain)
+		panic("%s: unable to create IRQ domain\n", node->full_name);
+
+	set_handle_irq(sun4i_handle_irq);
+
+	return 0;
+}
+IRQCHIP_DECLARE(allwinner_sun4i_ic, "allwinner,sun4i-ic", sun4i_of_init);
+
+static asmlinkage void __exception_irq_entry sun4i_handle_irq(struct pt_regs *regs)
+{
+	u32 irq, hwirq;
+
+	hwirq = readl(sun4i_irq_base + SUN4I_IRQ_VECTOR_REG) >> 2;
+	while (hwirq != 0) {
+		irq = irq_find_mapping(sun4i_irq_domain, hwirq);
+		handle_IRQ(irq, regs);
+		hwirq = readl(sun4i_irq_base + SUN4I_IRQ_VECTOR_REG) >> 2;
+	}
+}
diff --git a/drivers/irqchip/irq-sunxi.c b/drivers/irqchip/irq-sunxi.c
deleted file mode 100644
index 10974fa42653..000000000000
--- a/drivers/irqchip/irq-sunxi.c
+++ /dev/null
@@ -1,151 +0,0 @@
-/*
- * Allwinner A1X SoCs IRQ chip driver.
- *
- * Copyright (C) 2012 Maxime Ripard
- *
- * Maxime Ripard <maxime.ripard@free-electrons.com>
- *
- * Based on code from
- * Allwinner Technology Co., Ltd. <www.allwinnertech.com>
- * Benn Huang <benn@allwinnertech.com>
- *
- * 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/io.h>
-#include <linux/irq.h>
-#include <linux/of.h>
-#include <linux/of_address.h>
-#include <linux/of_irq.h>
-
-#include <linux/irqchip/sunxi.h>
-
-#define SUNXI_IRQ_VECTOR_REG		0x00
-#define SUNXI_IRQ_PROTECTION_REG	0x08
-#define SUNXI_IRQ_NMI_CTRL_REG		0x0c
-#define SUNXI_IRQ_PENDING_REG(x)	(0x10 + 0x4 * x)
-#define SUNXI_IRQ_FIQ_PENDING_REG(x)	(0x20 + 0x4 * x)
-#define SUNXI_IRQ_ENABLE_REG(x)		(0x40 + 0x4 * x)
-#define SUNXI_IRQ_MASK_REG(x)		(0x50 + 0x4 * x)
-
-static void __iomem *sunxi_irq_base;
-static struct irq_domain *sunxi_irq_domain;
-
-void sunxi_irq_ack(struct irq_data *irqd)
-{
-	unsigned int irq = irqd_to_hwirq(irqd);
-	unsigned int irq_off = irq % 32;
-	int reg = irq / 32;
-	u32 val;
-
-	val = readl(sunxi_irq_base + SUNXI_IRQ_PENDING_REG(reg));
-	writel(val | (1 << irq_off),
-	       sunxi_irq_base + SUNXI_IRQ_PENDING_REG(reg));
-}
-
-static void sunxi_irq_mask(struct irq_data *irqd)
-{
-	unsigned int irq = irqd_to_hwirq(irqd);
-	unsigned int irq_off = irq % 32;
-	int reg = irq / 32;
-	u32 val;
-
-	val = readl(sunxi_irq_base + SUNXI_IRQ_ENABLE_REG(reg));
-	writel(val & ~(1 << irq_off),
-	       sunxi_irq_base + SUNXI_IRQ_ENABLE_REG(reg));
-}
-
-static void sunxi_irq_unmask(struct irq_data *irqd)
-{
-	unsigned int irq = irqd_to_hwirq(irqd);
-	unsigned int irq_off = irq % 32;
-	int reg = irq / 32;
-	u32 val;
-
-	val = readl(sunxi_irq_base + SUNXI_IRQ_ENABLE_REG(reg));
-	writel(val | (1 << irq_off),
-	       sunxi_irq_base + SUNXI_IRQ_ENABLE_REG(reg));
-}
-
-static struct irq_chip sunxi_irq_chip = {
-	.name		= "sunxi_irq",
-	.irq_ack	= sunxi_irq_ack,
-	.irq_mask	= sunxi_irq_mask,
-	.irq_unmask	= sunxi_irq_unmask,
-};
-
-static int sunxi_irq_map(struct irq_domain *d, unsigned int virq,
-			 irq_hw_number_t hw)
-{
-	irq_set_chip_and_handler(virq, &sunxi_irq_chip,
-				 handle_level_irq);
-	set_irq_flags(virq, IRQF_VALID | IRQF_PROBE);
-
-	return 0;
-}
-
-static struct irq_domain_ops sunxi_irq_ops = {
-	.map = sunxi_irq_map,
-	.xlate = irq_domain_xlate_onecell,
-};
-
-static int __init sunxi_of_init(struct device_node *node,
-				struct device_node *parent)
-{
-	sunxi_irq_base = of_iomap(node, 0);
-	if (!sunxi_irq_base)
-		panic("%s: unable to map IC registers\n",
-			node->full_name);
-
-	/* Disable all interrupts */
-	writel(0, sunxi_irq_base + SUNXI_IRQ_ENABLE_REG(0));
-	writel(0, sunxi_irq_base + SUNXI_IRQ_ENABLE_REG(1));
-	writel(0, sunxi_irq_base + SUNXI_IRQ_ENABLE_REG(2));
-
-	/* Mask all the interrupts */
-	writel(0, sunxi_irq_base + SUNXI_IRQ_MASK_REG(0));
-	writel(0, sunxi_irq_base + SUNXI_IRQ_MASK_REG(1));
-	writel(0, sunxi_irq_base + SUNXI_IRQ_MASK_REG(2));
-
-	/* Clear all the pending interrupts */
-	writel(0xffffffff, sunxi_irq_base + SUNXI_IRQ_PENDING_REG(0));
-	writel(0xffffffff, sunxi_irq_base + SUNXI_IRQ_PENDING_REG(1));
-	writel(0xffffffff, sunxi_irq_base + SUNXI_IRQ_PENDING_REG(2));
-
-	/* Enable protection mode */
-	writel(0x01, sunxi_irq_base + SUNXI_IRQ_PROTECTION_REG);
-
-	/* Configure the external interrupt source type */
-	writel(0x00, sunxi_irq_base + SUNXI_IRQ_NMI_CTRL_REG);
-
-	sunxi_irq_domain = irq_domain_add_linear(node, 3 * 32,
-						 &sunxi_irq_ops, NULL);
-	if (!sunxi_irq_domain)
-		panic("%s: unable to create IRQ domain\n", node->full_name);
-
-	return 0;
-}
-
-static struct of_device_id sunxi_irq_dt_ids[] __initconst = {
-	{ .compatible = "allwinner,sunxi-ic", .data = sunxi_of_init },
-	{ }
-};
-
-void __init sunxi_init_irq(void)
-{
-	of_irq_init(sunxi_irq_dt_ids);
-}
-
-asmlinkage void __exception_irq_entry sunxi_handle_irq(struct pt_regs *regs)
-{
-	u32 irq, hwirq;
-
-	hwirq = readl(sunxi_irq_base + SUNXI_IRQ_VECTOR_REG) >> 2;
-	while (hwirq != 0) {
-		irq = irq_find_mapping(sunxi_irq_domain, hwirq);
-		handle_IRQ(irq, regs);
-		hwirq = readl(sunxi_irq_base + SUNXI_IRQ_VECTOR_REG) >> 2;
-	}
-}
diff --git a/drivers/irqchip/irq-vic.c b/drivers/irqchip/irq-vic.c
index 3cf97aaebe40..884d11c7355f 100644
--- a/drivers/irqchip/irq-vic.c
+++ b/drivers/irqchip/irq-vic.c
@@ -23,6 +23,7 @@
 #include <linux/init.h>
 #include <linux/list.h>
 #include <linux/io.h>
+#include <linux/irq.h>
 #include <linux/irqdomain.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
@@ -33,7 +34,7 @@
 #include <linux/irqchip/arm-vic.h>
 
 #include <asm/exception.h>
-#include <asm/mach/irq.h>
+#include <asm/irq.h>
 
 #include "irqchip.h"
 
diff --git a/drivers/irqchip/irq-vt8500.c b/drivers/irqchip/irq-vt8500.c
new file mode 100644
index 000000000000..d97059550a2c
--- /dev/null
+++ b/drivers/irqchip/irq-vt8500.c
@@ -0,0 +1,259 @@
+/*
+ *  arch/arm/mach-vt8500/irq.c
+ *
+ *  Copyright (C) 2012 Tony Prisk <linux@prisktech.co.nz>
+ *  Copyright (C) 2010 Alexey Charkov <alchark@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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+/*
+ * This file is copied and modified from the original irq.c provided by
+ * Alexey Charkov. Minor changes have been made for Device Tree Support.
+ */
+
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/interrupt.h>
+#include <linux/bitops.h>
+
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+
+#include <asm/irq.h>
+#include <asm/exception.h>
+#include <asm/mach/irq.h>
+
+#include "irqchip.h"
+
+#define VT8500_ICPC_IRQ		0x20
+#define VT8500_ICPC_FIQ		0x24
+#define VT8500_ICDC		0x40		/* Destination Control 64*u32 */
+#define VT8500_ICIS		0x80		/* Interrupt status, 16*u32 */
+
+/* ICPC */
+#define ICPC_MASK		0x3F
+#define ICPC_ROTATE		BIT(6)
+
+/* IC_DCTR */
+#define ICDC_IRQ		0x00
+#define ICDC_FIQ		0x01
+#define ICDC_DSS0		0x02
+#define ICDC_DSS1		0x03
+#define ICDC_DSS2		0x04
+#define ICDC_DSS3		0x05
+#define ICDC_DSS4		0x06
+#define ICDC_DSS5		0x07
+
+#define VT8500_INT_DISABLE	0
+#define VT8500_INT_ENABLE	BIT(3)
+
+#define VT8500_TRIGGER_HIGH	0
+#define VT8500_TRIGGER_RISING	BIT(5)
+#define VT8500_TRIGGER_FALLING	BIT(6)
+#define VT8500_EDGE		( VT8500_TRIGGER_RISING \
+				| VT8500_TRIGGER_FALLING)
+
+/* vt8500 has 1 intc, wm8505 and wm8650 have 2 */
+#define VT8500_INTC_MAX		2
+
+struct vt8500_irq_data {
+	void __iomem 		*base;		/* IO Memory base address */
+	struct irq_domain	*domain;	/* Domain for this controller */
+};
+
+/* Global variable for accessing io-mem addresses */
+static struct vt8500_irq_data intc[VT8500_INTC_MAX];
+static u32 active_cnt = 0;
+
+static void vt8500_irq_mask(struct irq_data *d)
+{
+	struct vt8500_irq_data *priv = d->domain->host_data;
+	void __iomem *base = priv->base;
+	void __iomem *stat_reg = base + VT8500_ICIS + (d->hwirq < 32 ? 0 : 4);
+	u8 edge, dctr;
+	u32 status;
+
+	edge = readb(base + VT8500_ICDC + d->hwirq) & VT8500_EDGE;
+	if (edge) {
+		status = readl(stat_reg);
+
+		status |= (1 << (d->hwirq & 0x1f));
+		writel(status, stat_reg);
+	} else {
+		dctr = readb(base + VT8500_ICDC + d->hwirq);
+		dctr &= ~VT8500_INT_ENABLE;
+		writeb(dctr, base + VT8500_ICDC + d->hwirq);
+	}
+}
+
+static void vt8500_irq_unmask(struct irq_data *d)
+{
+	struct vt8500_irq_data *priv = d->domain->host_data;
+	void __iomem *base = priv->base;
+	u8 dctr;
+
+	dctr = readb(base + VT8500_ICDC + d->hwirq);
+	dctr |= VT8500_INT_ENABLE;
+	writeb(dctr, base + VT8500_ICDC + d->hwirq);
+}
+
+static int vt8500_irq_set_type(struct irq_data *d, unsigned int flow_type)
+{
+	struct vt8500_irq_data *priv = d->domain->host_data;
+	void __iomem *base = priv->base;
+	u8 dctr;
+
+	dctr = readb(base + VT8500_ICDC + d->hwirq);
+	dctr &= ~VT8500_EDGE;
+
+	switch (flow_type) {
+	case IRQF_TRIGGER_LOW:
+		return -EINVAL;
+	case IRQF_TRIGGER_HIGH:
+		dctr |= VT8500_TRIGGER_HIGH;
+		__irq_set_handler_locked(d->irq, handle_level_irq);
+		break;
+	case IRQF_TRIGGER_FALLING:
+		dctr |= VT8500_TRIGGER_FALLING;
+		__irq_set_handler_locked(d->irq, handle_edge_irq);
+		break;
+	case IRQF_TRIGGER_RISING:
+		dctr |= VT8500_TRIGGER_RISING;
+		__irq_set_handler_locked(d->irq, handle_edge_irq);
+		break;
+	}
+	writeb(dctr, base + VT8500_ICDC + d->hwirq);
+
+	return 0;
+}
+
+static struct irq_chip vt8500_irq_chip = {
+	.name = "vt8500",
+	.irq_ack = vt8500_irq_mask,
+	.irq_mask = vt8500_irq_mask,
+	.irq_unmask = vt8500_irq_unmask,
+	.irq_set_type = vt8500_irq_set_type,
+};
+
+static void __init vt8500_init_irq_hw(void __iomem *base)
+{
+	u32 i;
+
+	/* Enable rotating priority for IRQ */
+	writel(ICPC_ROTATE, base + VT8500_ICPC_IRQ);
+	writel(0x00, base + VT8500_ICPC_FIQ);
+
+	/* Disable all interrupts and route them to IRQ */
+	for (i = 0; i < 64; i++)
+		writeb(VT8500_INT_DISABLE | ICDC_IRQ, base + VT8500_ICDC + i);
+}
+
+static int vt8500_irq_map(struct irq_domain *h, unsigned int virq,
+							irq_hw_number_t hw)
+{
+	irq_set_chip_and_handler(virq, &vt8500_irq_chip, handle_level_irq);
+	set_irq_flags(virq, IRQF_VALID);
+
+	return 0;
+}
+
+static struct irq_domain_ops vt8500_irq_domain_ops = {
+	.map = vt8500_irq_map,
+	.xlate = irq_domain_xlate_onecell,
+};
+
+asmlinkage void __exception_irq_entry vt8500_handle_irq(struct pt_regs *regs)
+{
+	u32 stat, i;
+	int irqnr, virq;
+	void __iomem *base;
+
+	/* Loop through each active controller */
+	for (i=0; i<active_cnt; i++) {
+		base = intc[i].base;
+		irqnr = readl_relaxed(base) & 0x3F;
+		/*
+		  Highest Priority register default = 63, so check that this
+		  is a real interrupt by checking the status register
+		*/
+		if (irqnr == 63) {
+			stat = readl_relaxed(base + VT8500_ICIS + 4);
+			if (!(stat & BIT(31)))
+				continue;
+		}
+
+		virq = irq_find_mapping(intc[i].domain, irqnr);
+		handle_IRQ(virq, regs);
+	}
+}
+
+int __init vt8500_irq_init(struct device_node *node, struct device_node *parent)
+{
+	int irq, i;
+	struct device_node *np = node;
+
+	if (active_cnt == VT8500_INTC_MAX) {
+		pr_err("%s: Interrupt controllers > VT8500_INTC_MAX\n",
+								__func__);
+		goto out;
+	}
+
+	intc[active_cnt].base = of_iomap(np, 0);
+	intc[active_cnt].domain = irq_domain_add_linear(node, 64,
+			&vt8500_irq_domain_ops,	&intc[active_cnt]);
+
+	if (!intc[active_cnt].base) {
+		pr_err("%s: Unable to map IO memory\n", __func__);
+		goto out;
+	}
+
+	if (!intc[active_cnt].domain) {
+		pr_err("%s: Unable to add irq domain!\n", __func__);
+		goto out;
+	}
+
+	set_handle_irq(vt8500_handle_irq);
+
+	vt8500_init_irq_hw(intc[active_cnt].base);
+
+	pr_info("vt8500-irq: Added interrupt controller\n");
+
+	active_cnt++;
+
+	/* check if this is a slaved controller */
+	if (of_irq_count(np) != 0) {
+		/* check that we have the correct number of interrupts */
+		if (of_irq_count(np) != 8) {
+			pr_err("%s: Incorrect IRQ map for slaved controller\n",
+					__func__);
+			return -EINVAL;
+		}
+
+		for (i = 0; i < 8; i++) {
+			irq = irq_of_parse_and_map(np, i);
+			enable_irq(irq);
+		}
+
+		pr_info("vt8500-irq: Enabled slave->parent interrupts\n");
+	}
+out:
+	return 0;
+}
+
+IRQCHIP_DECLARE(vt8500_irq, "via,vt8500-intc", vt8500_irq_init);
diff --git a/drivers/isdn/capi/capidrv.c b/drivers/isdn/capi/capidrv.c
index 832bc807ed20..cc9f1927a322 100644
--- a/drivers/isdn/capi/capidrv.c
+++ b/drivers/isdn/capi/capidrv.c
@@ -469,8 +469,7 @@ static int capidrv_add_ack(struct capidrv_ncci *nccip,
 {
 	struct ncci_datahandle_queue *n, **pp;
 
-	n = (struct ncci_datahandle_queue *)
-		kmalloc(sizeof(struct ncci_datahandle_queue), GFP_ATOMIC);
+	n = kmalloc(sizeof(struct ncci_datahandle_queue), GFP_ATOMIC);
 	if (!n) {
 		printk(KERN_ERR "capidrv: kmalloc ncci_datahandle failed\n");
 		return -1;
diff --git a/drivers/isdn/divert/isdn_divert.c b/drivers/isdn/divert/isdn_divert.c
index db432e635496..50749a70c5ca 100644
--- a/drivers/isdn/divert/isdn_divert.c
+++ b/drivers/isdn/divert/isdn_divert.c
@@ -441,8 +441,7 @@ static int isdn_divert_icall(isdn_ctrl *ic)
 
 		switch (dv->rule.action) {
 		case DEFLECT_IGNORE:
-			return (0);
-			break;
+			return 0;
 
 		case DEFLECT_ALERT:
 		case DEFLECT_PROCEED:
@@ -510,10 +509,9 @@ static int isdn_divert_icall(isdn_ctrl *ic)
 			break;
 
 		default:
-			return (0); /* ignore call */
-			break;
+			return 0; /* ignore call */
 		} /* switch action */
-		break;
+		break; /* will break the 'for' looping */
 	} /* scan_table */
 
 	if (cs) {
diff --git a/drivers/isdn/gigaset/capi.c b/drivers/isdn/gigaset/capi.c
index 03a0a01a4054..3286903a95d2 100644
--- a/drivers/isdn/gigaset/capi.c
+++ b/drivers/isdn/gigaset/capi.c
@@ -2334,7 +2334,7 @@ static int gigaset_proc_show(struct seq_file *m, void *v)
 
 static int gigaset_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, gigaset_proc_show, PDE(inode)->data);
+	return single_open(file, gigaset_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations gigaset_proc_fops = {
diff --git a/drivers/isdn/hardware/avm/b1.c b/drivers/isdn/hardware/avm/b1.c
index 821f7ac33b37..4d9b195547c5 100644
--- a/drivers/isdn/hardware/avm/b1.c
+++ b/drivers/isdn/hardware/avm/b1.c
@@ -702,7 +702,7 @@ static int b1ctl_proc_show(struct seq_file *m, void *v)
 
 static int b1ctl_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, b1ctl_proc_show, PDE(inode)->data);
+	return single_open(file, b1ctl_proc_show, PDE_DATA(inode));
 }
 
 const struct file_operations b1ctl_proc_fops = {
diff --git a/drivers/isdn/hardware/avm/b1dma.c b/drivers/isdn/hardware/avm/b1dma.c
index 0896aa86fc08..19b113faeb7b 100644
--- a/drivers/isdn/hardware/avm/b1dma.c
+++ b/drivers/isdn/hardware/avm/b1dma.c
@@ -944,7 +944,7 @@ static int b1dmactl_proc_show(struct seq_file *m, void *v)
 
 static int b1dmactl_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, b1dmactl_proc_show, PDE(inode)->data);
+	return single_open(file, b1dmactl_proc_show, PDE_DATA(inode));
 }
 
 const struct file_operations b1dmactl_proc_fops = {
diff --git a/drivers/isdn/hardware/avm/c4.c b/drivers/isdn/hardware/avm/c4.c
index 1d7fc44e3eef..5d00d72fe482 100644
--- a/drivers/isdn/hardware/avm/c4.c
+++ b/drivers/isdn/hardware/avm/c4.c
@@ -1129,7 +1129,7 @@ static int c4_proc_show(struct seq_file *m, void *v)
 
 static int c4_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, c4_proc_show, PDE(inode)->data);
+	return single_open(file, c4_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations c4_proc_fops = {
diff --git a/drivers/isdn/hardware/eicon/divasproc.c b/drivers/isdn/hardware/eicon/divasproc.c
index 3a4165c61196..56ce98a4e248 100644
--- a/drivers/isdn/hardware/eicon/divasproc.c
+++ b/drivers/isdn/hardware/eicon/divasproc.c
@@ -145,7 +145,7 @@ void remove_divas_proc(void)
 static ssize_t grp_opt_proc_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
-	diva_os_xdi_adapter_t *a = PDE(file_inode(file))->data;
+	diva_os_xdi_adapter_t *a = PDE_DATA(file_inode(file));
 	PISDN_ADAPTER IoAdapter = IoAdapters[a->controller - 1];
 
 	if ((count == 1) || (count == 2)) {
@@ -172,7 +172,7 @@ static ssize_t grp_opt_proc_write(struct file *file, const char __user *buffer,
 static ssize_t d_l1_down_proc_write(struct file *file, const char __user *buffer,
 				    size_t count, loff_t *pos)
 {
-	diva_os_xdi_adapter_t *a = PDE(file_inode(file))->data;
+	diva_os_xdi_adapter_t *a = PDE_DATA(file_inode(file));
 	PISDN_ADAPTER IoAdapter = IoAdapters[a->controller - 1];
 
 	if ((count == 1) || (count == 2)) {
@@ -210,7 +210,7 @@ static int d_l1_down_proc_show(struct seq_file *m, void *v)
 
 static int d_l1_down_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, d_l1_down_proc_show, PDE(inode)->data);
+	return single_open(file, d_l1_down_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations d_l1_down_proc_fops = {
@@ -236,7 +236,7 @@ static int grp_opt_proc_show(struct seq_file *m, void *v)
 
 static int grp_opt_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, grp_opt_proc_show, PDE(inode)->data);
+	return single_open(file, grp_opt_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations grp_opt_proc_fops = {
@@ -251,7 +251,7 @@ static const struct file_operations grp_opt_proc_fops = {
 static ssize_t info_proc_write(struct file *file, const char __user *buffer,
 			       size_t count, loff_t *pos)
 {
-	diva_os_xdi_adapter_t *a = PDE(file_inode(file))->data;
+	diva_os_xdi_adapter_t *a = PDE_DATA(file_inode(file));
 	PISDN_ADAPTER IoAdapter = IoAdapters[a->controller - 1];
 	char c[4];
 
@@ -335,7 +335,7 @@ static int info_proc_show(struct seq_file *m, void *v)
 
 static int info_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, info_proc_show, PDE(inode)->data);
+	return single_open(file, info_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations info_proc_fops = {
diff --git a/drivers/isdn/hisax/fsm.c b/drivers/isdn/hisax/fsm.c
index 1bb291021fdb..c7a94713e9ec 100644
--- a/drivers/isdn/hisax/fsm.c
+++ b/drivers/isdn/hisax/fsm.c
@@ -26,7 +26,7 @@ FsmNew(struct Fsm *fsm, struct FsmNode *fnlist, int fncount)
 {
 	int i;
 
-	fsm->jumpmatrix = (FSMFNPTR *)
+	fsm->jumpmatrix =
 		kzalloc(sizeof(FSMFNPTR) * fsm->state_count * fsm->event_count, GFP_KERNEL);
 	if (!fsm->jumpmatrix)
 		return -ENOMEM;
diff --git a/drivers/isdn/hisax/hfc_sx.c b/drivers/isdn/hisax/hfc_sx.c
index 90f34ae2b80f..dc4574f735ef 100644
--- a/drivers/isdn/hisax/hfc_sx.c
+++ b/drivers/isdn/hisax/hfc_sx.c
@@ -1479,7 +1479,7 @@ int setup_hfcsx(struct IsdnCard *card)
 			release_region(cs->hw.hfcsx.base, 2);
 			return (0);
 		}
-		if (!(cs->hw.hfcsx.extra = (void *)
+		if (!(cs->hw.hfcsx.extra =
 		      kmalloc(sizeof(struct hfcsx_extra), GFP_ATOMIC))) {
 			release_region(cs->hw.hfcsx.base, 2);
 			printk(KERN_WARNING "HFC-SX: unable to allocate memory\n");
diff --git a/drivers/isdn/hysdn/hycapi.c b/drivers/isdn/hysdn/hycapi.c
index 931f916c9c23..00aad10507d8 100644
--- a/drivers/isdn/hysdn/hycapi.c
+++ b/drivers/isdn/hysdn/hycapi.c
@@ -469,7 +469,7 @@ static int hycapi_proc_show(struct seq_file *m, void *v)
 
 static int hycapi_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, hycapi_proc_show, PDE(inode)->data);
+	return single_open(file, hycapi_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations hycapi_proc_fops = {
diff --git a/drivers/isdn/hysdn/hysdn_procconf.c b/drivers/isdn/hysdn/hysdn_procconf.c
index 8023d2510fba..73079213ec94 100644
--- a/drivers/isdn/hysdn/hysdn_procconf.c
+++ b/drivers/isdn/hysdn/hysdn_procconf.c
@@ -229,23 +229,12 @@ static int
 hysdn_conf_open(struct inode *ino, struct file *filep)
 {
 	hysdn_card *card;
-	struct proc_dir_entry *pd;
 	struct conf_writedata *cnf;
 	char *cp, *tmp;
 
 	/* now search the addressed card */
 	mutex_lock(&hysdn_conf_mutex);
-	card = card_root;
-	while (card) {
-		pd = card->procconf;
-		if (pd == PDE(ino))
-			break;
-		card = card->next;	/* search next entry */
-	}
-	if (!card) {
-		mutex_unlock(&hysdn_conf_mutex);
-		return (-ENODEV);	/* device is unknown/invalid */
-	}
+	card = PDE_DATA(ino);
 	if (card->debug_flags & (LOG_PROC_OPEN | LOG_PROC_ALL))
 		hysdn_addlog(card, "config open for uid=%d gid=%d mode=0x%x",
 			     filep->f_cred->fsuid, filep->f_cred->fsgid,
@@ -317,21 +306,9 @@ hysdn_conf_close(struct inode *ino, struct file *filep)
 	hysdn_card *card;
 	struct conf_writedata *cnf;
 	int retval = 0;
-	struct proc_dir_entry *pd;
 
 	mutex_lock(&hysdn_conf_mutex);
-	/* search the addressed card */
-	card = card_root;
-	while (card) {
-		pd = card->procconf;
-		if (pd == PDE(ino))
-			break;
-		card = card->next;	/* search next entry */
-	}
-	if (!card) {
-		mutex_unlock(&hysdn_conf_mutex);
-		return (-ENODEV);	/* device is unknown/invalid */
-	}
+	card = PDE_DATA(ino);
 	if (card->debug_flags & (LOG_PROC_OPEN | LOG_PROC_ALL))
 		hysdn_addlog(card, "config close for uid=%d gid=%d mode=0x%x",
 			     filep->f_cred->fsuid, filep->f_cred->fsgid,
@@ -394,10 +371,11 @@ hysdn_procconf_init(void)
 	while (card) {
 
 		sprintf(conf_name, "%s%d", PROC_CONF_BASENAME, card->myid);
-		if ((card->procconf = (void *) proc_create(conf_name,
+		if ((card->procconf = (void *) proc_create_data(conf_name,
 							   S_IFREG | S_IRUGO | S_IWUSR,
 							   hysdn_proc_entry,
-							   &conf_fops)) != NULL) {
+							   &conf_fops,
+							   card)) != NULL) {
 			hysdn_proclog_init(card);	/* init the log file entry */
 		}
 		card = card->next;	/* next entry */
diff --git a/drivers/isdn/hysdn/hysdn_proclog.c b/drivers/isdn/hysdn/hysdn_proclog.c
index 9a3ce93665c5..b61e8d5e84ad 100644
--- a/drivers/isdn/hysdn/hysdn_proclog.c
+++ b/drivers/isdn/hysdn/hysdn_proclog.c
@@ -173,27 +173,14 @@ hysdn_log_read(struct file *file, char __user *buf, size_t count, loff_t *off)
 {
 	struct log_data *inf;
 	int len;
-	struct proc_dir_entry *pde = PDE(file_inode(file));
-	struct procdata *pd = NULL;
-	hysdn_card *card;
+	hysdn_card *card = PDE_DATA(file_inode(file));
 
 	if (!*((struct log_data **) file->private_data)) {
+		struct procdata *pd = card->proclog;
 		if (file->f_flags & O_NONBLOCK)
 			return (-EAGAIN);
 
-		/* sorry, but we need to search the card */
-		card = card_root;
-		while (card) {
-			pd = card->proclog;
-			if (pd->log == pde)
-				break;
-			card = card->next;	/* search next entry */
-		}
-		if (card)
-			interruptible_sleep_on(&(pd->rd_queue));
-		else
-			return (-EAGAIN);
-
+		interruptible_sleep_on(&(pd->rd_queue));
 	}
 	if (!(inf = *((struct log_data **) file->private_data)))
 		return (0);
@@ -215,27 +202,15 @@ hysdn_log_read(struct file *file, char __user *buf, size_t count, loff_t *off)
 static int
 hysdn_log_open(struct inode *ino, struct file *filep)
 {
-	hysdn_card *card;
-	struct procdata *pd = NULL;
-	unsigned long flags;
+	hysdn_card *card = PDE_DATA(ino);
 
 	mutex_lock(&hysdn_log_mutex);
-	card = card_root;
-	while (card) {
-		pd = card->proclog;
-		if (pd->log == PDE(ino))
-			break;
-		card = card->next;	/* search next entry */
-	}
-	if (!card) {
-		mutex_unlock(&hysdn_log_mutex);
-		return (-ENODEV);	/* device is unknown/invalid */
-	}
-	filep->private_data = card;	/* remember our own card */
-
 	if ((filep->f_mode & (FMODE_READ | FMODE_WRITE)) == FMODE_WRITE) {
 		/* write only access -> write log level only */
+		filep->private_data = card;	/* remember our own card */
 	} else if ((filep->f_mode & (FMODE_READ | FMODE_WRITE)) == FMODE_READ) {
+		struct procdata *pd = card->proclog;
+		unsigned long flags;
 
 		/* read access -> log/debug read */
 		spin_lock_irqsave(&card->hysdn_lock, flags);
@@ -275,21 +250,13 @@ hysdn_log_close(struct inode *ino, struct file *filep)
 	} else {
 		/* read access -> log/debug read, mark one further file as closed */
 
-		pd = NULL;
 		inf = *((struct log_data **) filep->private_data);	/* get first log entry */
 		if (inf)
 			pd = (struct procdata *) inf->proc_ctrl;	/* still entries there */
 		else {
 			/* no info available -> search card */
-			card = card_root;
-			while (card) {
-				pd = card->proclog;
-				if (pd->log == PDE(ino))
-					break;
-				card = card->next;	/* search next entry */
-			}
-			if (card)
-				pd = card->proclog;	/* pointer to procfs log */
+			card = PDE_DATA(file_inode(filep));
+			pd = card->proclog;	/* pointer to procfs log */
 		}
 		if (pd)
 			pd->if_used--;	/* decrement interface usage count by one */
@@ -319,24 +286,12 @@ static unsigned int
 hysdn_log_poll(struct file *file, poll_table *wait)
 {
 	unsigned int mask = 0;
-	struct proc_dir_entry *pde = PDE(file_inode(file));
-	hysdn_card *card;
-	struct procdata *pd = NULL;
+	hysdn_card *card = PDE_DATA(file_inode(file));
+	struct procdata *pd = card->proclog;
 
 	if ((file->f_mode & (FMODE_READ | FMODE_WRITE)) == FMODE_WRITE)
 		return (mask);	/* no polling for write supported */
 
-	/* we need to search the card */
-	card = card_root;
-	while (card) {
-		pd = card->proclog;
-		if (pd->log == pde)
-			break;
-		card = card->next;	/* search next entry */
-	}
-	if (!card)
-		return (mask);	/* card not found */
-
 	poll_wait(file, &(pd->rd_queue), wait);
 
 	if (*((struct log_data **) file->private_data))
@@ -373,9 +328,9 @@ hysdn_proclog_init(hysdn_card *card)
 
 	if ((pd = kzalloc(sizeof(struct procdata), GFP_KERNEL)) != NULL) {
 		sprintf(pd->log_name, "%s%d", PROC_LOG_BASENAME, card->myid);
-		pd->log = proc_create(pd->log_name,
+		pd->log = proc_create_data(pd->log_name,
 				      S_IFREG | S_IRUGO | S_IWUSR, hysdn_proc_entry,
-				      &log_fops);
+				      &log_fops, card);
 
 		init_waitqueue_head(&(pd->rd_queue));
 
diff --git a/drivers/isdn/i4l/isdn_net.c b/drivers/isdn/i4l/isdn_net.c
index babc621a07fb..88d657dff474 100644
--- a/drivers/isdn/i4l/isdn_net.c
+++ b/drivers/isdn/i4l/isdn_net.c
@@ -1385,7 +1385,7 @@ isdn_net_type_trans(struct sk_buff *skb, struct net_device *dev)
 		if (memcmp(eth->h_dest, dev->dev_addr, ETH_ALEN))
 			skb->pkt_type = PACKET_OTHERHOST;
 	}
-	if (ntohs(eth->h_proto) >= 1536)
+	if (ntohs(eth->h_proto) >= ETH_P_802_3_MIN)
 		return eth->h_proto;
 
 	rawp = skb->data;
diff --git a/drivers/isdn/mISDN/socket.c b/drivers/isdn/mISDN/socket.c
index 8b07f83d48ad..e47dcb9d1e91 100644
--- a/drivers/isdn/mISDN/socket.c
+++ b/drivers/isdn/mISDN/socket.c
@@ -578,6 +578,7 @@ data_sock_getname(struct socket *sock, struct sockaddr *addr,
 	lock_sock(sk);
 
 	*addr_len = sizeof(*maddr);
+	maddr->family = AF_ISDN;
 	maddr->dev = _pms(sk)->dev->id;
 	maddr->channel = _pms(sk)->ch.nr;
 	maddr->sapi = _pms(sk)->ch.addr & 0xff;
diff --git a/drivers/isdn/mISDN/timerdev.c b/drivers/isdn/mISDN/timerdev.c
index 1094667d8f31..9438d7ec3308 100644
--- a/drivers/isdn/mISDN/timerdev.c
+++ b/drivers/isdn/mISDN/timerdev.c
@@ -64,7 +64,6 @@ mISDN_open(struct inode *ino, struct file *filep)
 	dev->work = 0;
 	init_waitqueue_head(&dev->wait);
 	filep->private_data = dev;
-	__module_get(THIS_MODULE);
 	return nonseekable_open(ino, filep);
 }
 
@@ -72,19 +71,28 @@ static int
 mISDN_close(struct inode *ino, struct file *filep)
 {
 	struct mISDNtimerdev	*dev = filep->private_data;
+	struct list_head	*list = &dev->pending;
 	struct mISDNtimer	*timer, *next;
 
 	if (*debug & DEBUG_TIMER)
 		printk(KERN_DEBUG "%s(%p,%p)\n", __func__, ino, filep);
-	list_for_each_entry_safe(timer, next, &dev->pending, list) {
-		del_timer(&timer->tl);
+
+	spin_lock_irq(&dev->lock);
+	while (!list_empty(list)) {
+		timer = list_first_entry(list, struct mISDNtimer, list);
+		spin_unlock_irq(&dev->lock);
+		del_timer_sync(&timer->tl);
+		spin_lock_irq(&dev->lock);
+		/* it might have been moved to ->expired */
+		list_del(&timer->list);
 		kfree(timer);
 	}
+	spin_unlock_irq(&dev->lock);
+
 	list_for_each_entry_safe(timer, next, &dev->expired, list) {
 		kfree(timer);
 	}
 	kfree(dev);
-	module_put(THIS_MODULE);
 	return 0;
 }
 
@@ -92,36 +100,41 @@ static ssize_t
 mISDN_read(struct file *filep, char __user *buf, size_t count, loff_t *off)
 {
 	struct mISDNtimerdev	*dev = filep->private_data;
+	struct list_head *list = &dev->expired;
 	struct mISDNtimer	*timer;
-	u_long	flags;
 	int	ret = 0;
 
 	if (*debug & DEBUG_TIMER)
 		printk(KERN_DEBUG "%s(%p, %p, %d, %p)\n", __func__,
 		       filep, buf, (int)count, off);
 
-	if (list_empty(&dev->expired) && (dev->work == 0)) {
+	if (count < sizeof(int))
+		return -ENOSPC;
+
+	spin_lock_irq(&dev->lock);
+	while (list_empty(list) && (dev->work == 0)) {
+		spin_unlock_irq(&dev->lock);
 		if (filep->f_flags & O_NONBLOCK)
 			return -EAGAIN;
 		wait_event_interruptible(dev->wait, (dev->work ||
-						     !list_empty(&dev->expired)));
+						     !list_empty(list)));
 		if (signal_pending(current))
 			return -ERESTARTSYS;
+		spin_lock_irq(&dev->lock);
 	}
-	if (count < sizeof(int))
-		return -ENOSPC;
 	if (dev->work)
 		dev->work = 0;
-	if (!list_empty(&dev->expired)) {
-		spin_lock_irqsave(&dev->lock, flags);
-		timer = (struct mISDNtimer *)dev->expired.next;
+	if (!list_empty(list)) {
+		timer = list_first_entry(list, struct mISDNtimer, list);
 		list_del(&timer->list);
-		spin_unlock_irqrestore(&dev->lock, flags);
+		spin_unlock_irq(&dev->lock);
 		if (put_user(timer->id, (int __user *)buf))
 			ret = -EFAULT;
 		else
 			ret = sizeof(int);
 		kfree(timer);
+	} else {
+		spin_unlock_irq(&dev->lock);
 	}
 	return ret;
 }
@@ -153,7 +166,8 @@ dev_expire_timer(unsigned long data)
 	u_long			flags;
 
 	spin_lock_irqsave(&timer->dev->lock, flags);
-	list_move_tail(&timer->list, &timer->dev->expired);
+	if (timer->id >= 0)
+		list_move_tail(&timer->list, &timer->dev->expired);
 	spin_unlock_irqrestore(&timer->dev->lock, flags);
 	wake_up_interruptible(&timer->dev->wait);
 }
@@ -162,7 +176,6 @@ static int
 misdn_add_timer(struct mISDNtimerdev *dev, int timeout)
 {
 	int			id;
-	u_long			flags;
 	struct mISDNtimer	*timer;
 
 	if (!timeout) {
@@ -173,19 +186,16 @@ misdn_add_timer(struct mISDNtimerdev *dev, int timeout)
 		timer = kzalloc(sizeof(struct mISDNtimer), GFP_KERNEL);
 		if (!timer)
 			return -ENOMEM;
-		spin_lock_irqsave(&dev->lock, flags);
-		timer->id = dev->next_id++;
+		timer->dev = dev;
+		setup_timer(&timer->tl, dev_expire_timer, (long)timer);
+		spin_lock_irq(&dev->lock);
+		id = timer->id = dev->next_id++;
 		if (dev->next_id < 0)
 			dev->next_id = 1;
 		list_add_tail(&timer->list, &dev->pending);
-		spin_unlock_irqrestore(&dev->lock, flags);
-		timer->dev = dev;
-		timer->tl.data = (long)timer;
-		timer->tl.function = dev_expire_timer;
-		init_timer(&timer->tl);
 		timer->tl.expires = jiffies + ((HZ * (u_long)timeout) / 1000);
 		add_timer(&timer->tl);
-		id = timer->id;
+		spin_unlock_irq(&dev->lock);
 	}
 	return id;
 }
@@ -193,26 +203,21 @@ misdn_add_timer(struct mISDNtimerdev *dev, int timeout)
 static int
 misdn_del_timer(struct mISDNtimerdev *dev, int id)
 {
-	u_long			flags;
 	struct mISDNtimer	*timer;
-	int			ret = 0;
 
-	spin_lock_irqsave(&dev->lock, flags);
+	spin_lock_irq(&dev->lock);
 	list_for_each_entry(timer, &dev->pending, list) {
 		if (timer->id == id) {
 			list_del_init(&timer->list);
-			/* RED-PEN AK: race -- timer can be still running on
-			 * other CPU. Needs reference count I think
-			 */
-			del_timer(&timer->tl);
-			ret = timer->id;
+			timer->id = -1;
+			spin_unlock_irq(&dev->lock);
+			del_timer_sync(&timer->tl);
 			kfree(timer);
-			goto unlock;
+			return id;
 		}
 	}
-unlock:
-	spin_unlock_irqrestore(&dev->lock, flags);
-	return ret;
+	spin_unlock_irq(&dev->lock);
+	return 0;
 }
 
 static long
@@ -262,6 +267,7 @@ mISDN_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 }
 
 static const struct file_operations mISDN_fops = {
+	.owner		= THIS_MODULE,
 	.read		= mISDN_read,
 	.poll		= mISDN_poll,
 	.unlocked_ioctl	= mISDN_ioctl,
diff --git a/drivers/isdn/sc/init.c b/drivers/isdn/sc/init.c
index 6b580b2c717f..ca997bd4e818 100644
--- a/drivers/isdn/sc/init.c
+++ b/drivers/isdn/sc/init.c
@@ -33,8 +33,8 @@ static unsigned long ram[] = {0, 0, 0, 0};
 static bool do_reset = 0;
 
 module_param_array(io, int, NULL, 0);
-module_param_array(irq, int, NULL, 0);
-module_param_array(ram, int, NULL, 0);
+module_param_array(irq, byte, NULL, 0);
+module_param_array(ram, long, NULL, 0);
 module_param(do_reset, bool, 0);
 
 static int identify_board(unsigned long, unsigned int);
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index ec50824c02ec..ef992293598a 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -173,7 +173,7 @@ config LEDS_PCA9532_GPIO
 config LEDS_GPIO
 	tristate "LED Support for GPIO connected LEDs"
 	depends on LEDS_CLASS
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  This option enables support for the LEDs connected to GPIO
 	  outputs. To be useful the particular board must have LEDs
@@ -194,8 +194,8 @@ config LEDS_LP3944
 	  module will be called leds-lp3944.
 
 config LEDS_LP55XX_COMMON
-	tristate "Common Driver for TI/National LP5521 and LP5523/55231"
-	depends on LEDS_LP5521 || LEDS_LP5523
+	tristate "Common Driver for TI/National LP5521, LP5523/55231 and LP5562"
+	depends on LEDS_LP5521 || LEDS_LP5523 || LEDS_LP5562
 	select FW_LOADER
 	help
 	  This option supports common operations for LP5521 and LP5523/55231
@@ -222,6 +222,16 @@ config LEDS_LP5523
 	  Driver provides direct control via LED class and interface for
 	  programming the engines.
 
+config LEDS_LP5562
+	tristate "LED Support for TI LP5562 LED driver chip"
+	depends on LEDS_CLASS && I2C
+	select LEDS_LP55XX_COMMON
+	help
+	  If you say yes here you get support for TI LP5562 LED driver.
+	  It is 4 channels chip with programmable engines.
+	  Driver provides direct control via LED class and interface for
+	  programming the engines.
+
 config LEDS_LP8788
 	tristate "LED support for the TI LP8788 PMIC"
 	depends on LEDS_CLASS
@@ -352,7 +362,7 @@ config LEDS_INTEL_SS4200
 config LEDS_LT3593
 	tristate "LED driver for LT3593 controllers"
 	depends on LEDS_CLASS
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  This option enables support for LEDs driven by a Linear Technology
 	  LT3593 controller. This controller uses a special one-wire pulse
@@ -421,7 +431,7 @@ config LEDS_ASIC3
 
 config LEDS_RENESAS_TPU
 	bool "LED support for Renesas TPU"
-	depends on LEDS_CLASS=y && HAVE_CLK && GENERIC_GPIO
+	depends on LEDS_CLASS=y && HAVE_CLK && GPIOLIB
 	help
 	  This option enables build of the LED TPU platform driver,
 	  suitable to drive any TPU channel on newer Renesas SoCs.
@@ -469,106 +479,7 @@ config LEDS_BLINKM
 	  This option enables support for the BlinkM RGB LED connected
 	  through I2C. Say Y to enable support for the BlinkM LED.
 
-config LEDS_TRIGGERS
-	bool "LED Trigger support"
-	depends on LEDS_CLASS
-	help
-	  This option enables trigger support for the leds class.
-	  These triggers allow kernel events to drive the LEDs and can
-	  be configured via sysfs. If unsure, say Y.
-
 comment "LED Triggers"
-
-config LEDS_TRIGGER_TIMER
-	tristate "LED Timer Trigger"
-	depends on LEDS_TRIGGERS
-	help
-	  This allows LEDs to be controlled by a programmable timer
-	  via sysfs. Some LED hardware can be programmed to start
-	  blinking the LED without any further software interaction.
-	  For more details read Documentation/leds/leds-class.txt.
-
-	  If unsure, say Y.
-
-config LEDS_TRIGGER_ONESHOT
-	tristate "LED One-shot Trigger"
-	depends on LEDS_TRIGGERS
-	help
-	  This allows LEDs to blink in one-shot pulses with parameters
-	  controlled via sysfs.  It's useful to notify the user on
-	  sporadic events, when there are no clear begin and end trap points,
-	  or on dense events, where this blinks the LED at constant rate if
-	  rearmed continuously.
-
-	  It also shows how to use the led_blink_set_oneshot() function.
-
-	  If unsure, say Y.
-
-config LEDS_TRIGGER_IDE_DISK
-	bool "LED IDE Disk Trigger"
-	depends on IDE_GD_ATA
-	depends on LEDS_TRIGGERS
-	help
-	  This allows LEDs to be controlled by IDE disk activity.
-	  If unsure, say Y.
-
-config LEDS_TRIGGER_HEARTBEAT
-	tristate "LED Heartbeat Trigger"
-	depends on LEDS_TRIGGERS
-	help
-	  This allows LEDs to be controlled by a CPU load average.
-	  The flash frequency is a hyperbolic function of the 1-minute
-	  load average.
-	  If unsure, say Y.
-
-config LEDS_TRIGGER_BACKLIGHT
-	tristate "LED backlight Trigger"
-	depends on LEDS_TRIGGERS
-	help
-	  This allows LEDs to be controlled as a backlight device: they
-	  turn off and on when the display is blanked and unblanked.
-
-	  If unsure, say N.
-
-config LEDS_TRIGGER_CPU
-	bool "LED CPU Trigger"
-	depends on LEDS_TRIGGERS
-	help
-	  This allows LEDs to be controlled by active CPUs. This shows
-	  the active CPUs across an array of LEDs so you can see which
-	  CPUs are active on the system at any given moment.
-
-	  If unsure, say N.
-
-config LEDS_TRIGGER_GPIO
-	tristate "LED GPIO Trigger"
-	depends on LEDS_TRIGGERS
-	depends on GPIOLIB
-	help
-	  This allows LEDs to be controlled by gpio events. It's good
-	  when using gpios as switches and triggering the needed LEDs
-	  from there. One use case is n810's keypad LEDs that could
-	  be triggered by this trigger when user slides up to show
-	  keypad.
-
-	  If unsure, say N.
-
-config LEDS_TRIGGER_DEFAULT_ON
-	tristate "LED Default ON Trigger"
-	depends on LEDS_TRIGGERS
-	help
-	  This allows LEDs to be initialised in the ON state.
-	  If unsure, say Y.
-
-comment "iptables trigger is under Netfilter config (LED target)"
-	depends on LEDS_TRIGGERS
-
-config LEDS_TRIGGER_TRANSIENT
-	tristate "LED Transient Trigger"
-	depends on LEDS_TRIGGERS
-	help
-	  This allows one time activation of a transient state on
-	  GPIO/PWM based hardware.
-	  If unsure, say Y.
+source "drivers/leds/trigger/Kconfig"
 
 endif # NEW_LEDS
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index 215e7e3b6173..ac2897732b02 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -26,6 +26,7 @@ obj-$(CONFIG_LEDS_LP3944)		+= leds-lp3944.o
 obj-$(CONFIG_LEDS_LP55XX_COMMON)	+= leds-lp55xx-common.o
 obj-$(CONFIG_LEDS_LP5521)		+= leds-lp5521.o
 obj-$(CONFIG_LEDS_LP5523)		+= leds-lp5523.o
+obj-$(CONFIG_LEDS_LP5562)		+= leds-lp5562.o
 obj-$(CONFIG_LEDS_LP8788)		+= leds-lp8788.o
 obj-$(CONFIG_LEDS_TCA6507)		+= leds-tca6507.o
 obj-$(CONFIG_LEDS_CLEVO_MAIL)		+= leds-clevo-mail.o
@@ -57,12 +58,4 @@ obj-$(CONFIG_LEDS_BLINKM)		+= leds-blinkm.o
 obj-$(CONFIG_LEDS_DAC124S085)		+= leds-dac124s085.o
 
 # LED Triggers
-obj-$(CONFIG_LEDS_TRIGGER_TIMER)	+= ledtrig-timer.o
-obj-$(CONFIG_LEDS_TRIGGER_ONESHOT)	+= ledtrig-oneshot.o
-obj-$(CONFIG_LEDS_TRIGGER_IDE_DISK)	+= ledtrig-ide-disk.o
-obj-$(CONFIG_LEDS_TRIGGER_HEARTBEAT)	+= ledtrig-heartbeat.o
-obj-$(CONFIG_LEDS_TRIGGER_BACKLIGHT)	+= ledtrig-backlight.o
-obj-$(CONFIG_LEDS_TRIGGER_GPIO)		+= ledtrig-gpio.o
-obj-$(CONFIG_LEDS_TRIGGER_CPU)		+= ledtrig-cpu.o
-obj-$(CONFIG_LEDS_TRIGGER_DEFAULT_ON)	+= ledtrig-default-on.o
-obj-$(CONFIG_LEDS_TRIGGER_TRANSIENT)	+= ledtrig-transient.o
+obj-$(CONFIG_LEDS_TRIGGERS)		+= trigger/
diff --git a/drivers/leds/leds-asic3.c b/drivers/leds/leds-asic3.c
index b474745e001b..cf9efe421c2b 100644
--- a/drivers/leds/leds-asic3.c
+++ b/drivers/leds/leds-asic3.c
@@ -134,6 +134,7 @@ static int asic3_led_remove(struct platform_device *pdev)
 	return mfd_cell_disable(pdev);
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int asic3_led_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
@@ -159,11 +160,9 @@ static int asic3_led_resume(struct device *dev)
 
 	return ret;
 }
+#endif
 
-static const struct dev_pm_ops asic3_led_pm_ops = {
-	.suspend	= asic3_led_suspend,
-	.resume		= asic3_led_resume,
-};
+static SIMPLE_DEV_PM_OPS(asic3_led_pm_ops, asic3_led_suspend, asic3_led_resume);
 
 static struct platform_driver asic3_led_driver = {
 	.probe		= asic3_led_probe,
diff --git a/drivers/leds/leds-atmel-pwm.c b/drivers/leds/leds-atmel-pwm.c
index 386773532d95..8a39c5b20f76 100644
--- a/drivers/leds/leds-atmel-pwm.c
+++ b/drivers/leds/leds-atmel-pwm.c
@@ -113,7 +113,7 @@ err:
 	return status;
 }
 
-static int __exit pwmled_remove(struct platform_device *pdev)
+static int pwmled_remove(struct platform_device *pdev)
 {
 	const struct gpio_led_platform_data	*pdata;
 	struct pwmled				*leds;
@@ -140,7 +140,7 @@ static struct platform_driver pwmled_driver = {
 	},
 	/* REVISIT add suspend() and resume() methods */
 	.probe =	pwmled_probe,
-	.remove =	__exit_p(pwmled_remove),
+	.remove =	pwmled_remove,
 };
 
 module_platform_driver(pwmled_driver);
diff --git a/drivers/leds/leds-bd2802.c b/drivers/leds/leds-bd2802.c
index 851517030cc1..2db04231a792 100644
--- a/drivers/leds/leds-bd2802.c
+++ b/drivers/leds/leds-bd2802.c
@@ -732,7 +732,7 @@ failed_unregister_dev_file:
 	return ret;
 }
 
-static int __exit bd2802_remove(struct i2c_client *client)
+static int bd2802_remove(struct i2c_client *client)
 {
 	struct bd2802_led *led = i2c_get_clientdata(client);
 	int i;
@@ -747,8 +747,7 @@ static int __exit bd2802_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-
+#ifdef CONFIG_PM_SLEEP
 static void bd2802_restore_state(struct bd2802_led *led)
 {
 	int i;
@@ -785,12 +784,9 @@ static int bd2802_resume(struct device *dev)
 
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(bd2802_pm, bd2802_suspend, bd2802_resume);
-#define BD2802_PM (&bd2802_pm)
-#else		/* CONFIG_PM */
-#define BD2802_PM NULL
-#endif
 
 static const struct i2c_device_id bd2802_id[] = {
 	{ "BD2802", 0 },
@@ -801,10 +797,10 @@ MODULE_DEVICE_TABLE(i2c, bd2802_id);
 static struct i2c_driver bd2802_i2c_driver = {
 	.driver	= {
 		.name	= "BD2802",
-		.pm	= BD2802_PM,
+		.pm	= &bd2802_pm,
 	},
 	.probe		= bd2802_probe,
-	.remove		= __exit_p(bd2802_remove),
+	.remove		= bd2802_remove,
 	.id_table	= bd2802_id,
 };
 
diff --git a/drivers/leds/leds-lm355x.c b/drivers/leds/leds-lm355x.c
index 4117235ba618..d81a8e7afd6c 100644
--- a/drivers/leds/leds-lm355x.c
+++ b/drivers/leds/leds-lm355x.c
@@ -477,6 +477,7 @@ static int lm355x_probe(struct i2c_client *client,
 	chip->cdev_flash.name = "flash";
 	chip->cdev_flash.max_brightness = 16;
 	chip->cdev_flash.brightness_set = lm355x_strobe_brightness_set;
+	chip->cdev_flash.default_trigger = "flash";
 	err = led_classdev_register((struct device *)
 				    &client->dev, &chip->cdev_flash);
 	if (err < 0)
@@ -486,6 +487,7 @@ static int lm355x_probe(struct i2c_client *client,
 	chip->cdev_torch.name = "torch";
 	chip->cdev_torch.max_brightness = 8;
 	chip->cdev_torch.brightness_set = lm355x_torch_brightness_set;
+	chip->cdev_torch.default_trigger = "torch";
 	err = led_classdev_register((struct device *)
 				    &client->dev, &chip->cdev_torch);
 	if (err < 0)
diff --git a/drivers/leds/leds-lm3642.c b/drivers/leds/leds-lm3642.c
index 9f428d9dfe91..f361bbef2dec 100644
--- a/drivers/leds/leds-lm3642.c
+++ b/drivers/leds/leds-lm3642.c
@@ -363,6 +363,7 @@ static int lm3642_probe(struct i2c_client *client,
 	chip->cdev_flash.name = "flash";
 	chip->cdev_flash.max_brightness = 16;
 	chip->cdev_flash.brightness_set = lm3642_strobe_brightness_set;
+	chip->cdev_flash.default_trigger = "flash";
 	err = led_classdev_register((struct device *)
 				    &client->dev, &chip->cdev_flash);
 	if (err < 0) {
@@ -380,6 +381,7 @@ static int lm3642_probe(struct i2c_client *client,
 	chip->cdev_torch.name = "torch";
 	chip->cdev_torch.max_brightness = 8;
 	chip->cdev_torch.brightness_set = lm3642_torch_brightness_set;
+	chip->cdev_torch.default_trigger = "torch";
 	err = led_classdev_register((struct device *)
 				    &client->dev, &chip->cdev_torch);
 	if (err < 0) {
diff --git a/drivers/leds/leds-lp5521.c b/drivers/leds/leds-lp5521.c
index 1001347ba70b..19752c928aa2 100644
--- a/drivers/leds/leds-lp5521.c
+++ b/drivers/leds/leds-lp5521.c
@@ -68,6 +68,18 @@
 #define LP5521_ENABLE_RUN_PROGRAM	\
 	(LP5521_ENABLE_DEFAULT | LP5521_EXEC_RUN)
 
+/* CONFIG register */
+#define LP5521_PWM_HF			0x40	/* PWM: 0 = 256Hz, 1 = 558Hz */
+#define LP5521_PWRSAVE_EN		0x20	/* 1 = Power save mode */
+#define LP5521_CP_MODE_OFF		0	/* Charge pump (CP) off */
+#define LP5521_CP_MODE_BYPASS		8	/* CP forced to bypass mode */
+#define LP5521_CP_MODE_1X5		0x10	/* CP forced to 1.5x mode */
+#define LP5521_CP_MODE_AUTO		0x18	/* Automatic mode selection */
+#define LP5521_R_TO_BATT		0x04	/* R out: 0 = CP, 1 = Vbat */
+#define LP5521_CLK_INT			0x01	/* Internal clock */
+#define LP5521_DEFAULT_CFG		\
+	(LP5521_PWM_HF | LP5521_PWRSAVE_EN | LP5521_CP_MODE_AUTO)
+
 /* Status */
 #define LP5521_EXT_CLK_USED		0x08
 
@@ -296,8 +308,11 @@ static int lp5521_post_init_device(struct lp55xx_chip *chip)
 	/* Set all PWMs to direct control mode */
 	ret = lp55xx_write(chip, LP5521_REG_OP_MODE, LP5521_CMD_DIRECT);
 
-	val = chip->pdata->update_config ?
-		: (LP5521_PWRSAVE_EN | LP5521_CP_MODE_AUTO | LP5521_R_TO_BATT);
+	/* Update configuration for the clock setting */
+	val = LP5521_DEFAULT_CFG;
+	if (!lp55xx_is_extclk_used(chip))
+		val |= LP5521_CLK_INT;
+
 	ret = lp55xx_write(chip, LP5521_REG_CONFIG, val);
 	if (ret)
 		return ret;
@@ -360,7 +375,8 @@ static ssize_t lp5521_selftest(struct device *dev,
 	mutex_lock(&chip->lock);
 	ret = lp5521_run_selftest(chip, buf);
 	mutex_unlock(&chip->lock);
-	return sprintf(buf, "%s\n", ret ? "FAIL" : "OK");
+
+	return scnprintf(buf, PAGE_SIZE, "%s\n", ret ? "FAIL" : "OK");
 }
 
 /* device attributes */
diff --git a/drivers/leds/leds-lp5562.c b/drivers/leds/leds-lp5562.c
new file mode 100644
index 000000000000..513f2390ca2d
--- /dev/null
+++ b/drivers/leds/leds-lp5562.c
@@ -0,0 +1,599 @@
+/*
+ * LP5562 LED driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ *
+ * Author: Milo(Woogyom) Kim <milo.kim@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.
+ */
+
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/leds.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_data/leds-lp55xx.h>
+#include <linux/slab.h>
+
+#include "leds-lp55xx-common.h"
+
+#define LP5562_PROGRAM_LENGTH		32
+#define LP5562_MAX_LEDS			4
+
+/* ENABLE Register 00h */
+#define LP5562_REG_ENABLE		0x00
+#define LP5562_EXEC_ENG1_M		0x30
+#define LP5562_EXEC_ENG2_M		0x0C
+#define LP5562_EXEC_ENG3_M		0x03
+#define LP5562_EXEC_M			0x3F
+#define LP5562_MASTER_ENABLE		0x40	/* Chip master enable */
+#define LP5562_LOGARITHMIC_PWM		0x80	/* Logarithmic PWM adjustment */
+#define LP5562_EXEC_RUN			0x2A
+#define LP5562_ENABLE_DEFAULT	\
+	(LP5562_MASTER_ENABLE | LP5562_LOGARITHMIC_PWM)
+#define LP5562_ENABLE_RUN_PROGRAM	\
+	(LP5562_ENABLE_DEFAULT | LP5562_EXEC_RUN)
+
+/* OPMODE Register 01h */
+#define LP5562_REG_OP_MODE		0x01
+#define LP5562_MODE_ENG1_M		0x30
+#define LP5562_MODE_ENG2_M		0x0C
+#define LP5562_MODE_ENG3_M		0x03
+#define LP5562_LOAD_ENG1		0x10
+#define LP5562_LOAD_ENG2		0x04
+#define LP5562_LOAD_ENG3		0x01
+#define LP5562_RUN_ENG1			0x20
+#define LP5562_RUN_ENG2			0x08
+#define LP5562_RUN_ENG3			0x02
+#define LP5562_ENG1_IS_LOADING(mode)	\
+	((mode & LP5562_MODE_ENG1_M) == LP5562_LOAD_ENG1)
+#define LP5562_ENG2_IS_LOADING(mode)	\
+	((mode & LP5562_MODE_ENG2_M) == LP5562_LOAD_ENG2)
+#define LP5562_ENG3_IS_LOADING(mode)	\
+	((mode & LP5562_MODE_ENG3_M) == LP5562_LOAD_ENG3)
+
+/* BRIGHTNESS Registers */
+#define LP5562_REG_R_PWM		0x04
+#define LP5562_REG_G_PWM		0x03
+#define LP5562_REG_B_PWM		0x02
+#define LP5562_REG_W_PWM		0x0E
+
+/* CURRENT Registers */
+#define LP5562_REG_R_CURRENT		0x07
+#define LP5562_REG_G_CURRENT		0x06
+#define LP5562_REG_B_CURRENT		0x05
+#define LP5562_REG_W_CURRENT		0x0F
+
+/* CONFIG Register 08h */
+#define LP5562_REG_CONFIG		0x08
+#define LP5562_PWM_HF			0x40
+#define LP5562_PWRSAVE_EN		0x20
+#define LP5562_CLK_INT			0x01	/* Internal clock */
+#define LP5562_DEFAULT_CFG		(LP5562_PWM_HF | LP5562_PWRSAVE_EN)
+
+/* RESET Register 0Dh */
+#define LP5562_REG_RESET		0x0D
+#define LP5562_RESET			0xFF
+
+/* PROGRAM ENGINE Registers */
+#define LP5562_REG_PROG_MEM_ENG1	0x10
+#define LP5562_REG_PROG_MEM_ENG2	0x30
+#define LP5562_REG_PROG_MEM_ENG3	0x50
+
+/* LEDMAP Register 70h */
+#define LP5562_REG_ENG_SEL		0x70
+#define LP5562_ENG_SEL_PWM		0
+#define LP5562_ENG_FOR_RGB_M		0x3F
+#define LP5562_ENG_SEL_RGB		0x1B	/* R:ENG1, G:ENG2, B:ENG3 */
+#define LP5562_ENG_FOR_W_M		0xC0
+#define LP5562_ENG1_FOR_W		0x40	/* W:ENG1 */
+#define LP5562_ENG2_FOR_W		0x80	/* W:ENG2 */
+#define LP5562_ENG3_FOR_W		0xC0	/* W:ENG3 */
+
+/* Program Commands */
+#define LP5562_CMD_DISABLE		0x00
+#define LP5562_CMD_LOAD			0x15
+#define LP5562_CMD_RUN			0x2A
+#define LP5562_CMD_DIRECT		0x3F
+#define LP5562_PATTERN_OFF		0
+
+static inline void lp5562_wait_opmode_done(void)
+{
+	/* operation mode change needs to be longer than 153 us */
+	usleep_range(200, 300);
+}
+
+static inline void lp5562_wait_enable_done(void)
+{
+	/* it takes more 488 us to update ENABLE register */
+	usleep_range(500, 600);
+}
+
+static void lp5562_set_led_current(struct lp55xx_led *led, u8 led_current)
+{
+	u8 addr[] = {
+		LP5562_REG_R_CURRENT,
+		LP5562_REG_G_CURRENT,
+		LP5562_REG_B_CURRENT,
+		LP5562_REG_W_CURRENT,
+	};
+
+	led->led_current = led_current;
+	lp55xx_write(led->chip, addr[led->chan_nr], led_current);
+}
+
+static void lp5562_load_engine(struct lp55xx_chip *chip)
+{
+	enum lp55xx_engine_index idx = chip->engine_idx;
+	u8 mask[] = {
+		[LP55XX_ENGINE_1] = LP5562_MODE_ENG1_M,
+		[LP55XX_ENGINE_2] = LP5562_MODE_ENG2_M,
+		[LP55XX_ENGINE_3] = LP5562_MODE_ENG3_M,
+	};
+
+	u8 val[] = {
+		[LP55XX_ENGINE_1] = LP5562_LOAD_ENG1,
+		[LP55XX_ENGINE_2] = LP5562_LOAD_ENG2,
+		[LP55XX_ENGINE_3] = LP5562_LOAD_ENG3,
+	};
+
+	lp55xx_update_bits(chip, LP5562_REG_OP_MODE, mask[idx], val[idx]);
+
+	lp5562_wait_opmode_done();
+}
+
+static void lp5562_stop_engine(struct lp55xx_chip *chip)
+{
+	lp55xx_write(chip, LP5562_REG_OP_MODE, LP5562_CMD_DISABLE);
+	lp5562_wait_opmode_done();
+}
+
+static void lp5562_run_engine(struct lp55xx_chip *chip, bool start)
+{
+	int ret;
+	u8 mode;
+	u8 exec;
+
+	/* stop engine */
+	if (!start) {
+		lp55xx_write(chip, LP5562_REG_ENABLE, LP5562_ENABLE_DEFAULT);
+		lp5562_wait_enable_done();
+		lp5562_stop_engine(chip);
+		lp55xx_write(chip, LP5562_REG_ENG_SEL, LP5562_ENG_SEL_PWM);
+		lp55xx_write(chip, LP5562_REG_OP_MODE, LP5562_CMD_DIRECT);
+		lp5562_wait_opmode_done();
+		return;
+	}
+
+	/*
+	 * To run the engine,
+	 * operation mode and enable register should updated at the same time
+	 */
+
+	ret = lp55xx_read(chip, LP5562_REG_OP_MODE, &mode);
+	if (ret)
+		return;
+
+	ret = lp55xx_read(chip, LP5562_REG_ENABLE, &exec);
+	if (ret)
+		return;
+
+	/* change operation mode to RUN only when each engine is loading */
+	if (LP5562_ENG1_IS_LOADING(mode)) {
+		mode = (mode & ~LP5562_MODE_ENG1_M) | LP5562_RUN_ENG1;
+		exec = (exec & ~LP5562_EXEC_ENG1_M) | LP5562_RUN_ENG1;
+	}
+
+	if (LP5562_ENG2_IS_LOADING(mode)) {
+		mode = (mode & ~LP5562_MODE_ENG2_M) | LP5562_RUN_ENG2;
+		exec = (exec & ~LP5562_EXEC_ENG2_M) | LP5562_RUN_ENG2;
+	}
+
+	if (LP5562_ENG3_IS_LOADING(mode)) {
+		mode = (mode & ~LP5562_MODE_ENG3_M) | LP5562_RUN_ENG3;
+		exec = (exec & ~LP5562_EXEC_ENG3_M) | LP5562_RUN_ENG3;
+	}
+
+	lp55xx_write(chip, LP5562_REG_OP_MODE, mode);
+	lp5562_wait_opmode_done();
+
+	lp55xx_update_bits(chip, LP5562_REG_ENABLE, LP5562_EXEC_M, exec);
+	lp5562_wait_enable_done();
+}
+
+static int lp5562_update_firmware(struct lp55xx_chip *chip,
+					const u8 *data, size_t size)
+{
+	enum lp55xx_engine_index idx = chip->engine_idx;
+	u8 pattern[LP5562_PROGRAM_LENGTH] = {0};
+	u8 addr[] = {
+		[LP55XX_ENGINE_1] = LP5562_REG_PROG_MEM_ENG1,
+		[LP55XX_ENGINE_2] = LP5562_REG_PROG_MEM_ENG2,
+		[LP55XX_ENGINE_3] = LP5562_REG_PROG_MEM_ENG3,
+	};
+	unsigned cmd;
+	char c[3];
+	int program_size;
+	int nrchars;
+	int offset = 0;
+	int ret;
+	int i;
+
+	/* clear program memory before updating */
+	for (i = 0; i < LP5562_PROGRAM_LENGTH; i++)
+		lp55xx_write(chip, addr[idx] + i, 0);
+
+	i = 0;
+	while ((offset < size - 1) && (i < LP5562_PROGRAM_LENGTH)) {
+		/* separate sscanfs because length is working only for %s */
+		ret = sscanf(data + offset, "%2s%n ", c, &nrchars);
+		if (ret != 1)
+			goto err;
+
+		ret = sscanf(c, "%2x", &cmd);
+		if (ret != 1)
+			goto err;
+
+		pattern[i] = (u8)cmd;
+		offset += nrchars;
+		i++;
+	}
+
+	/* Each instruction is 16bit long. Check that length is even */
+	if (i % 2)
+		goto err;
+
+	program_size = i;
+	for (i = 0; i < program_size; i++)
+		lp55xx_write(chip, addr[idx] + i, pattern[i]);
+
+	return 0;
+
+err:
+	dev_err(&chip->cl->dev, "wrong pattern format\n");
+	return -EINVAL;
+}
+
+static void lp5562_firmware_loaded(struct lp55xx_chip *chip)
+{
+	const struct firmware *fw = chip->fw;
+
+	if (fw->size > LP5562_PROGRAM_LENGTH) {
+		dev_err(&chip->cl->dev, "firmware data size overflow: %zu\n",
+			fw->size);
+		return;
+	}
+
+	/*
+	 * Program momery sequence
+	 *  1) set engine mode to "LOAD"
+	 *  2) write firmware data into program memory
+	 */
+
+	lp5562_load_engine(chip);
+	lp5562_update_firmware(chip, fw->data, fw->size);
+}
+
+static int lp5562_post_init_device(struct lp55xx_chip *chip)
+{
+	int ret;
+	u8 cfg = LP5562_DEFAULT_CFG;
+
+	/* Set all PWMs to direct control mode */
+	ret = lp55xx_write(chip, LP5562_REG_OP_MODE, LP5562_CMD_DIRECT);
+	if (ret)
+		return ret;
+
+	lp5562_wait_opmode_done();
+
+	/* Update configuration for the clock setting */
+	if (!lp55xx_is_extclk_used(chip))
+		cfg |= LP5562_CLK_INT;
+
+	ret = lp55xx_write(chip, LP5562_REG_CONFIG, cfg);
+	if (ret)
+		return ret;
+
+	/* Initialize all channels PWM to zero -> leds off */
+	lp55xx_write(chip, LP5562_REG_R_PWM, 0);
+	lp55xx_write(chip, LP5562_REG_G_PWM, 0);
+	lp55xx_write(chip, LP5562_REG_B_PWM, 0);
+	lp55xx_write(chip, LP5562_REG_W_PWM, 0);
+
+	/* Set LED map as register PWM by default */
+	lp55xx_write(chip, LP5562_REG_ENG_SEL, LP5562_ENG_SEL_PWM);
+
+	return 0;
+}
+
+static void lp5562_led_brightness_work(struct work_struct *work)
+{
+	struct lp55xx_led *led = container_of(work, struct lp55xx_led,
+					      brightness_work);
+	struct lp55xx_chip *chip = led->chip;
+	u8 addr[] = {
+		LP5562_REG_R_PWM,
+		LP5562_REG_G_PWM,
+		LP5562_REG_B_PWM,
+		LP5562_REG_W_PWM,
+	};
+
+	mutex_lock(&chip->lock);
+	lp55xx_write(chip, addr[led->chan_nr], led->brightness);
+	mutex_unlock(&chip->lock);
+}
+
+static void lp5562_write_program_memory(struct lp55xx_chip *chip,
+					u8 base, const u8 *rgb, int size)
+{
+	int i;
+
+	if (!rgb || size <= 0)
+		return;
+
+	for (i = 0; i < size; i++)
+		lp55xx_write(chip, base + i, *(rgb + i));
+
+	lp55xx_write(chip, base + i, 0);
+	lp55xx_write(chip, base + i + 1, 0);
+}
+
+/* check the size of program count */
+static inline bool _is_pc_overflow(struct lp55xx_predef_pattern *ptn)
+{
+	return (ptn->size_r >= LP5562_PROGRAM_LENGTH ||
+		ptn->size_g >= LP5562_PROGRAM_LENGTH ||
+		ptn->size_b >= LP5562_PROGRAM_LENGTH);
+}
+
+static int lp5562_run_predef_led_pattern(struct lp55xx_chip *chip, int mode)
+{
+	struct lp55xx_predef_pattern *ptn;
+	int i;
+
+	if (mode == LP5562_PATTERN_OFF) {
+		lp5562_run_engine(chip, false);
+		return 0;
+	}
+
+	ptn = chip->pdata->patterns + (mode - 1);
+	if (!ptn || _is_pc_overflow(ptn)) {
+		dev_err(&chip->cl->dev, "invalid pattern data\n");
+		return -EINVAL;
+	}
+
+	lp5562_stop_engine(chip);
+
+	/* Set LED map as RGB */
+	lp55xx_write(chip, LP5562_REG_ENG_SEL, LP5562_ENG_SEL_RGB);
+
+	/* Load engines */
+	for (i = LP55XX_ENGINE_1; i <= LP55XX_ENGINE_3; i++) {
+		chip->engine_idx = i;
+		lp5562_load_engine(chip);
+	}
+
+	/* Clear program registers */
+	lp55xx_write(chip, LP5562_REG_PROG_MEM_ENG1, 0);
+	lp55xx_write(chip, LP5562_REG_PROG_MEM_ENG1 + 1, 0);
+	lp55xx_write(chip, LP5562_REG_PROG_MEM_ENG2, 0);
+	lp55xx_write(chip, LP5562_REG_PROG_MEM_ENG2 + 1, 0);
+	lp55xx_write(chip, LP5562_REG_PROG_MEM_ENG3, 0);
+	lp55xx_write(chip, LP5562_REG_PROG_MEM_ENG3 + 1, 0);
+
+	/* Program engines */
+	lp5562_write_program_memory(chip, LP5562_REG_PROG_MEM_ENG1,
+				ptn->r, ptn->size_r);
+	lp5562_write_program_memory(chip, LP5562_REG_PROG_MEM_ENG2,
+				ptn->g, ptn->size_g);
+	lp5562_write_program_memory(chip, LP5562_REG_PROG_MEM_ENG3,
+				ptn->b, ptn->size_b);
+
+	/* Run engines */
+	lp5562_run_engine(chip, true);
+
+	return 0;
+}
+
+static ssize_t lp5562_store_pattern(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t len)
+{
+	struct lp55xx_led *led = i2c_get_clientdata(to_i2c_client(dev));
+	struct lp55xx_chip *chip = led->chip;
+	struct lp55xx_predef_pattern *ptn = chip->pdata->patterns;
+	int num_patterns = chip->pdata->num_patterns;
+	unsigned long mode;
+	int ret;
+
+	ret = kstrtoul(buf, 0, &mode);
+	if (ret)
+		return ret;
+
+	if (mode > num_patterns || !ptn)
+		return -EINVAL;
+
+	mutex_lock(&chip->lock);
+	ret = lp5562_run_predef_led_pattern(chip, mode);
+	mutex_unlock(&chip->lock);
+
+	if (ret)
+		return ret;
+
+	return len;
+}
+
+static ssize_t lp5562_store_engine_mux(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t len)
+{
+	struct lp55xx_led *led = i2c_get_clientdata(to_i2c_client(dev));
+	struct lp55xx_chip *chip = led->chip;
+	u8 mask;
+	u8 val;
+
+	/* LED map
+	 * R ... Engine 1 (fixed)
+	 * G ... Engine 2 (fixed)
+	 * B ... Engine 3 (fixed)
+	 * W ... Engine 1 or 2 or 3
+	 */
+
+	if (sysfs_streq(buf, "RGB")) {
+		mask = LP5562_ENG_FOR_RGB_M;
+		val = LP5562_ENG_SEL_RGB;
+	} else if (sysfs_streq(buf, "W")) {
+		enum lp55xx_engine_index idx = chip->engine_idx;
+
+		mask = LP5562_ENG_FOR_W_M;
+		switch (idx) {
+		case LP55XX_ENGINE_1:
+			val = LP5562_ENG1_FOR_W;
+			break;
+		case LP55XX_ENGINE_2:
+			val = LP5562_ENG2_FOR_W;
+			break;
+		case LP55XX_ENGINE_3:
+			val = LP5562_ENG3_FOR_W;
+			break;
+		default:
+			return -EINVAL;
+		}
+
+	} else {
+		dev_err(dev, "choose RGB or W\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	lp55xx_update_bits(chip, LP5562_REG_ENG_SEL, mask, val);
+	mutex_unlock(&chip->lock);
+
+	return len;
+}
+
+static DEVICE_ATTR(led_pattern, S_IWUSR, NULL, lp5562_store_pattern);
+static DEVICE_ATTR(engine_mux, S_IWUSR, NULL, lp5562_store_engine_mux);
+
+static struct attribute *lp5562_attributes[] = {
+	&dev_attr_led_pattern.attr,
+	&dev_attr_engine_mux.attr,
+	NULL,
+};
+
+static const struct attribute_group lp5562_group = {
+	.attrs = lp5562_attributes,
+};
+
+/* Chip specific configurations */
+static struct lp55xx_device_config lp5562_cfg = {
+	.max_channel  = LP5562_MAX_LEDS,
+	.reset = {
+		.addr = LP5562_REG_RESET,
+		.val  = LP5562_RESET,
+	},
+	.enable = {
+		.addr = LP5562_REG_ENABLE,
+		.val  = LP5562_ENABLE_DEFAULT,
+	},
+	.post_init_device   = lp5562_post_init_device,
+	.set_led_current    = lp5562_set_led_current,
+	.brightness_work_fn = lp5562_led_brightness_work,
+	.run_engine         = lp5562_run_engine,
+	.firmware_cb        = lp5562_firmware_loaded,
+	.dev_attr_group     = &lp5562_group,
+};
+
+static int lp5562_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int ret;
+	struct lp55xx_chip *chip;
+	struct lp55xx_led *led;
+	struct lp55xx_platform_data *pdata = client->dev.platform_data;
+
+	if (!pdata) {
+		dev_err(&client->dev, "no platform data\n");
+		return -EINVAL;
+	}
+
+	chip = devm_kzalloc(&client->dev, sizeof(*chip), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	led = devm_kzalloc(&client->dev,
+			sizeof(*led) * pdata->num_channels, GFP_KERNEL);
+	if (!led)
+		return -ENOMEM;
+
+	chip->cl = client;
+	chip->pdata = pdata;
+	chip->cfg = &lp5562_cfg;
+
+	mutex_init(&chip->lock);
+
+	i2c_set_clientdata(client, led);
+
+	ret = lp55xx_init_device(chip);
+	if (ret)
+		goto err_init;
+
+	ret = lp55xx_register_leds(led, chip);
+	if (ret)
+		goto err_register_leds;
+
+	ret = lp55xx_register_sysfs(chip);
+	if (ret) {
+		dev_err(&client->dev, "registering sysfs failed\n");
+		goto err_register_sysfs;
+	}
+
+	return 0;
+
+err_register_sysfs:
+	lp55xx_unregister_leds(led, chip);
+err_register_leds:
+	lp55xx_deinit_device(chip);
+err_init:
+	return ret;
+}
+
+static int lp5562_remove(struct i2c_client *client)
+{
+	struct lp55xx_led *led = i2c_get_clientdata(client);
+	struct lp55xx_chip *chip = led->chip;
+
+	lp5562_stop_engine(chip);
+
+	lp55xx_unregister_sysfs(chip);
+	lp55xx_unregister_leds(led, chip);
+	lp55xx_deinit_device(chip);
+
+	return 0;
+}
+
+static const struct i2c_device_id lp5562_id[] = {
+	{ "lp5562", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, lp5562_id);
+
+static struct i2c_driver lp5562_driver = {
+	.driver = {
+		.name	= "lp5562",
+	},
+	.probe		= lp5562_probe,
+	.remove		= lp5562_remove,
+	.id_table	= lp5562_id,
+};
+
+module_i2c_driver(lp5562_driver);
+
+MODULE_DESCRIPTION("Texas Instruments LP5562 LED Driver");
+MODULE_AUTHOR("Milo Kim");
+MODULE_LICENSE("GPL");
diff --git a/drivers/leds/leds-lp55xx-common.c b/drivers/leds/leds-lp55xx-common.c
index d9eb84157423..ba34199dc3d9 100644
--- a/drivers/leds/leds-lp55xx-common.c
+++ b/drivers/leds/leds-lp55xx-common.c
@@ -1,5 +1,5 @@
 /*
- * LP5521/LP5523/LP55231 Common Driver
+ * LP5521/LP5523/LP55231/LP5562 Common Driver
  *
  * Copyright 2012 Texas Instruments
  *
@@ -12,6 +12,7 @@
  * Derived from leds-lp5521.c, leds-lp5523.c
  */
 
+#include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/firmware.h>
 #include <linux/i2c.h>
@@ -21,6 +22,9 @@
 
 #include "leds-lp55xx-common.h"
 
+/* External clock rate */
+#define LP55XX_CLK_32K			32768
+
 static struct lp55xx_led *cdev_to_lp55xx_led(struct led_classdev *cdev)
 {
 	return container_of(cdev, struct lp55xx_led, cdev);
@@ -80,7 +84,7 @@ static ssize_t lp55xx_show_current(struct device *dev,
 {
 	struct lp55xx_led *led = dev_to_lp55xx_led(dev);
 
-	return sprintf(buf, "%d\n", led->led_current);
+	return scnprintf(buf, PAGE_SIZE, "%d\n", led->led_current);
 }
 
 static ssize_t lp55xx_store_current(struct device *dev,
@@ -113,7 +117,7 @@ static ssize_t lp55xx_show_max_current(struct device *dev,
 {
 	struct lp55xx_led *led = dev_to_lp55xx_led(dev);
 
-	return sprintf(buf, "%d\n", led->max_current);
+	return scnprintf(buf, PAGE_SIZE, "%d\n", led->max_current);
 }
 
 static DEVICE_ATTR(led_current, S_IRUGO | S_IWUSR, lp55xx_show_current,
@@ -357,6 +361,35 @@ int lp55xx_update_bits(struct lp55xx_chip *chip, u8 reg, u8 mask, u8 val)
 }
 EXPORT_SYMBOL_GPL(lp55xx_update_bits);
 
+bool lp55xx_is_extclk_used(struct lp55xx_chip *chip)
+{
+	struct clk *clk;
+	int err;
+
+	clk = devm_clk_get(&chip->cl->dev, "32k_clk");
+	if (IS_ERR(clk))
+		goto use_internal_clk;
+
+	err = clk_prepare_enable(clk);
+	if (err)
+		goto use_internal_clk;
+
+	if (clk_get_rate(clk) != LP55XX_CLK_32K) {
+		clk_disable_unprepare(clk);
+		goto use_internal_clk;
+	}
+
+	dev_info(&chip->cl->dev, "%dHz external clock used\n",	LP55XX_CLK_32K);
+
+	chip->clk = clk;
+	return true;
+
+use_internal_clk:
+	dev_info(&chip->cl->dev, "internal clock used\n");
+	return false;
+}
+EXPORT_SYMBOL_GPL(lp55xx_is_extclk_used);
+
 int lp55xx_init_device(struct lp55xx_chip *chip)
 {
 	struct lp55xx_platform_data *pdata;
@@ -421,6 +454,9 @@ void lp55xx_deinit_device(struct lp55xx_chip *chip)
 {
 	struct lp55xx_platform_data *pdata = chip->pdata;
 
+	if (chip->clk)
+		clk_disable_unprepare(chip->clk);
+
 	if (pdata->enable)
 		pdata->enable(0);
 
diff --git a/drivers/leds/leds-lp55xx-common.h b/drivers/leds/leds-lp55xx-common.h
index ece4761a1302..fa6a078bf547 100644
--- a/drivers/leds/leds-lp55xx-common.h
+++ b/drivers/leds/leds-lp55xx-common.h
@@ -83,6 +83,7 @@ struct lp55xx_device_config {
  */
 struct lp55xx_chip {
 	struct i2c_client *cl;
+	struct clk *clk;
 	struct lp55xx_platform_data *pdata;
 	struct mutex lock;	/* lock for user-space interface */
 	int num_leds;
@@ -117,6 +118,9 @@ extern int lp55xx_read(struct lp55xx_chip *chip, u8 reg, u8 *val);
 extern int lp55xx_update_bits(struct lp55xx_chip *chip, u8 reg,
 			u8 mask, u8 val);
 
+/* external clock detection */
+extern bool lp55xx_is_extclk_used(struct lp55xx_chip *chip);
+
 /* common device init/deinit functions */
 extern int lp55xx_init_device(struct lp55xx_chip *chip);
 extern void lp55xx_deinit_device(struct lp55xx_chip *chip);
diff --git a/drivers/leds/leds-lt3593.c b/drivers/leds/leds-lt3593.c
index c9b9e1fec587..ca48a7d5502d 100644
--- a/drivers/leds/leds-lt3593.c
+++ b/drivers/leds/leds-lt3593.c
@@ -106,8 +106,9 @@ static int create_lt3593_led(const struct gpio_led *template,
 	if (!template->retain_state_suspended)
 		led_dat->cdev.flags |= LED_CORE_SUSPENDRESUME;
 
-	ret = devm_gpio_request_one(parent, template->gpio,
-				    GPIOF_DIR_OUT | state, template->name);
+	ret = devm_gpio_request_one(parent, template->gpio, state ?
+				    GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW,
+				    template->name);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/leds/leds-ns2.c b/drivers/leds/leds-ns2.c
index d978171c25b4..70137b1eecf5 100644
--- a/drivers/leds/leds-ns2.c
+++ b/drivers/leds/leds-ns2.c
@@ -193,7 +193,8 @@ create_ns2_led(struct platform_device *pdev, struct ns2_led_data *led_dat,
 	enum ns2_led_modes mode;
 
 	ret = devm_gpio_request_one(&pdev->dev, template->cmd,
-			GPIOF_DIR_OUT | gpio_get_value(template->cmd),
+			gpio_get_value(template->cmd) ?
+			GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW,
 			template->name);
 	if (ret) {
 		dev_err(&pdev->dev, "%s: failed to setup command GPIO\n",
@@ -202,7 +203,8 @@ create_ns2_led(struct platform_device *pdev, struct ns2_led_data *led_dat,
 	}
 
 	ret = devm_gpio_request_one(&pdev->dev, template->slow,
-			GPIOF_DIR_OUT | gpio_get_value(template->slow),
+			gpio_get_value(template->slow) ?
+			GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW,
 			template->name);
 	if (ret) {
 		dev_err(&pdev->dev, "%s: failed to setup slow GPIO\n",
@@ -306,10 +308,21 @@ static const struct of_device_id of_ns2_leds_match[] = {
 };
 #endif /* CONFIG_OF_GPIO */
 
+struct ns2_led_priv {
+	int num_leds;
+	struct ns2_led_data leds_data[];
+};
+
+static inline int sizeof_ns2_led_priv(int num_leds)
+{
+	return sizeof(struct ns2_led_priv) +
+		      (sizeof(struct ns2_led_data) * num_leds);
+}
+
 static int ns2_led_probe(struct platform_device *pdev)
 {
 	struct ns2_led_platform_data *pdata = pdev->dev.platform_data;
-	struct ns2_led_data *leds_data;
+	struct ns2_led_priv *priv;
 	int i;
 	int ret;
 
@@ -330,21 +343,23 @@ static int ns2_led_probe(struct platform_device *pdev)
 		return -EINVAL;
 #endif /* CONFIG_OF_GPIO */
 
-	leds_data = devm_kzalloc(&pdev->dev, sizeof(struct ns2_led_data) *
-				 pdata->num_leds, GFP_KERNEL);
-	if (!leds_data)
+	priv = devm_kzalloc(&pdev->dev,
+			    sizeof_ns2_led_priv(pdata->num_leds), GFP_KERNEL);
+	if (!priv)
 		return -ENOMEM;
+	priv->num_leds = pdata->num_leds;
 
-	for (i = 0; i < pdata->num_leds; i++) {
-		ret = create_ns2_led(pdev, &leds_data[i], &pdata->leds[i]);
+	for (i = 0; i < priv->num_leds; i++) {
+		ret = create_ns2_led(pdev, &priv->leds_data[i],
+				     &pdata->leds[i]);
 		if (ret < 0) {
 			for (i = i - 1; i >= 0; i--)
-				delete_ns2_led(&leds_data[i]);
+				delete_ns2_led(&priv->leds_data[i]);
 			return ret;
 		}
 	}
 
-	platform_set_drvdata(pdev, leds_data);
+	platform_set_drvdata(pdev, priv);
 
 	return 0;
 }
@@ -352,13 +367,12 @@ static int ns2_led_probe(struct platform_device *pdev)
 static int ns2_led_remove(struct platform_device *pdev)
 {
 	int i;
-	struct ns2_led_platform_data *pdata = pdev->dev.platform_data;
-	struct ns2_led_data *leds_data;
+	struct ns2_led_priv *priv;
 
-	leds_data = platform_get_drvdata(pdev);
+	priv = platform_get_drvdata(pdev);
 
-	for (i = 0; i < pdata->num_leds; i++)
-		delete_ns2_led(&leds_data[i]);
+	for (i = 0; i < priv->num_leds; i++)
+		delete_ns2_led(&priv->leds_data[i]);
 
 	platform_set_drvdata(pdev, NULL);
 
diff --git a/drivers/leds/leds-pwm.c b/drivers/leds/leds-pwm.c
index a1ea5f6a8d39..faf52c005e8c 100644
--- a/drivers/leds/leds-pwm.c
+++ b/drivers/leds/leds-pwm.c
@@ -23,12 +23,16 @@
 #include <linux/pwm.h>
 #include <linux/leds_pwm.h>
 #include <linux/slab.h>
+#include <linux/workqueue.h>
 
 struct led_pwm_data {
 	struct led_classdev	cdev;
 	struct pwm_device	*pwm;
+	struct work_struct	work;
 	unsigned int		active_low;
 	unsigned int		period;
+	int			duty;
+	bool			can_sleep;
 };
 
 struct led_pwm_priv {
@@ -36,6 +40,26 @@ struct led_pwm_priv {
 	struct led_pwm_data leds[0];
 };
 
+static void __led_pwm_set(struct led_pwm_data *led_dat)
+{
+	int new_duty = led_dat->duty;
+
+	pwm_config(led_dat->pwm, new_duty, led_dat->period);
+
+	if (new_duty == 0)
+		pwm_disable(led_dat->pwm);
+	else
+		pwm_enable(led_dat->pwm);
+}
+
+static void led_pwm_work(struct work_struct *work)
+{
+	struct led_pwm_data *led_dat =
+		container_of(work, struct led_pwm_data, work);
+
+	__led_pwm_set(led_dat);
+}
+
 static void led_pwm_set(struct led_classdev *led_cdev,
 	enum led_brightness brightness)
 {
@@ -44,13 +68,12 @@ static void led_pwm_set(struct led_classdev *led_cdev,
 	unsigned int max = led_dat->cdev.max_brightness;
 	unsigned int period =  led_dat->period;
 
-	if (brightness == 0) {
-		pwm_config(led_dat->pwm, 0, period);
-		pwm_disable(led_dat->pwm);
-	} else {
-		pwm_config(led_dat->pwm, brightness * period / max, period);
-		pwm_enable(led_dat->pwm);
-	}
+	led_dat->duty = brightness * period / max;
+
+	if (led_dat->can_sleep)
+		schedule_work(&led_dat->work);
+	else
+		__led_pwm_set(led_dat);
 }
 
 static inline size_t sizeof_pwm_leds_priv(int num_leds)
@@ -100,6 +123,10 @@ static struct led_pwm_priv *led_pwm_create_of(struct platform_device *pdev)
 		led_dat->cdev.brightness = LED_OFF;
 		led_dat->cdev.flags |= LED_CORE_SUSPENDRESUME;
 
+		led_dat->can_sleep = pwm_can_sleep(led_dat->pwm);
+		if (led_dat->can_sleep)
+			INIT_WORK(&led_dat->work, led_pwm_work);
+
 		ret = led_classdev_register(&pdev->dev, &led_dat->cdev);
 		if (ret < 0) {
 			dev_err(&pdev->dev, "failed to register for %s\n",
@@ -153,6 +180,10 @@ static int led_pwm_probe(struct platform_device *pdev)
 			led_dat->cdev.max_brightness = cur_led->max_brightness;
 			led_dat->cdev.flags |= LED_CORE_SUSPENDRESUME;
 
+			led_dat->can_sleep = pwm_can_sleep(led_dat->pwm);
+			if (led_dat->can_sleep)
+				INIT_WORK(&led_dat->work, led_pwm_work);
+
 			ret = led_classdev_register(&pdev->dev, &led_dat->cdev);
 			if (ret < 0)
 				goto err;
@@ -180,8 +211,11 @@ static int led_pwm_remove(struct platform_device *pdev)
 	struct led_pwm_priv *priv = platform_get_drvdata(pdev);
 	int i;
 
-	for (i = 0; i < priv->num_leds; i++)
+	for (i = 0; i < priv->num_leds; i++) {
 		led_classdev_unregister(&priv->leds[i].cdev);
+		if (priv->leds[i].can_sleep)
+			cancel_work_sync(&priv->leds[i].work);
+	}
 
 	return 0;
 }
diff --git a/drivers/leds/leds-renesas-tpu.c b/drivers/leds/leds-renesas-tpu.c
index d3c2b7e68fbc..9483f1c1078d 100644
--- a/drivers/leds/leds-renesas-tpu.c
+++ b/drivers/leds/leds-renesas-tpu.c
@@ -205,7 +205,8 @@ static void r_tpu_set_pin(struct r_tpu_priv *p, enum r_tpu_pin new_state,
 		gpio_free(cfg->pin_gpio_fn);
 
 	if (new_state == R_TPU_PIN_GPIO)
-		gpio_request_one(cfg->pin_gpio, GPIOF_DIR_OUT | !!brightness,
+		gpio_request_one(cfg->pin_gpio, !!brightness ?
+				GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW,
 				cfg->name);
 
 	if (new_state == R_TPU_PIN_GPIO_FN)
diff --git a/drivers/leds/leds-tca6507.c b/drivers/leds/leds-tca6507.c
index 070ba0741b21..98fe021ba276 100644
--- a/drivers/leds/leds-tca6507.c
+++ b/drivers/leds/leds-tca6507.c
@@ -85,6 +85,7 @@
 #include <linux/gpio.h>
 #include <linux/workqueue.h>
 #include <linux/leds-tca6507.h>
+#include <linux/of.h>
 
 /* LED select registers determine the source that drives LED outputs */
 #define TCA6507_LS_LED_OFF	0x0	/* Output HI-Z (off) */
@@ -724,7 +725,6 @@ tca6507_led_dt_init(struct i2c_client *client)
 	return ERR_PTR(-ENODEV);
 }
 
-#define of_tca6507_leds_match NULL
 #endif
 
 static int tca6507_probe(struct i2c_client *client,
@@ -813,7 +813,7 @@ static struct i2c_driver tca6507_driver = {
 	.driver   = {
 		.name    = "leds-tca6507",
 		.owner   = THIS_MODULE,
-		.of_match_table = of_tca6507_leds_match,
+		.of_match_table = of_match_ptr(of_tca6507_leds_match),
 	},
 	.probe    = tca6507_probe,
 	.remove   = tca6507_remove,
diff --git a/drivers/leds/leds-wm8350.c b/drivers/leds/leds-wm8350.c
index ed15157c8f6c..8a181d56602d 100644
--- a/drivers/leds/leds-wm8350.c
+++ b/drivers/leds/leds-wm8350.c
@@ -129,7 +129,10 @@ static void wm8350_led_disable(struct wm8350_led *led)
 	ret = regulator_disable(led->isink);
 	if (ret != 0) {
 		dev_err(led->cdev.dev, "Failed to disable ISINK: %d\n", ret);
-		regulator_enable(led->dcdc);
+		ret = regulator_enable(led->dcdc);
+		if (ret != 0)
+			dev_err(led->cdev.dev, "Failed to reenable DCDC: %d\n",
+				ret);
 		return;
 	}
 
diff --git a/drivers/leds/trigger/Kconfig b/drivers/leds/trigger/Kconfig
new file mode 100644
index 000000000000..49794b47b51c
--- /dev/null
+++ b/drivers/leds/trigger/Kconfig
@@ -0,0 +1,111 @@
+menuconfig LEDS_TRIGGERS
+	bool "LED Trigger support"
+	depends on LEDS_CLASS
+	help
+	  This option enables trigger support for the leds class.
+	  These triggers allow kernel events to drive the LEDs and can
+	  be configured via sysfs. If unsure, say Y.
+
+if LEDS_TRIGGERS
+
+config LEDS_TRIGGER_TIMER
+	tristate "LED Timer Trigger"
+	depends on LEDS_TRIGGERS
+	help
+	  This allows LEDs to be controlled by a programmable timer
+	  via sysfs. Some LED hardware can be programmed to start
+	  blinking the LED without any further software interaction.
+	  For more details read Documentation/leds/leds-class.txt.
+
+	  If unsure, say Y.
+
+config LEDS_TRIGGER_ONESHOT
+	tristate "LED One-shot Trigger"
+	depends on LEDS_TRIGGERS
+	help
+	  This allows LEDs to blink in one-shot pulses with parameters
+	  controlled via sysfs.  It's useful to notify the user on
+	  sporadic events, when there are no clear begin and end trap points,
+	  or on dense events, where this blinks the LED at constant rate if
+	  rearmed continuously.
+
+	  It also shows how to use the led_blink_set_oneshot() function.
+
+	  If unsure, say Y.
+
+config LEDS_TRIGGER_IDE_DISK
+	bool "LED IDE Disk Trigger"
+	depends on IDE_GD_ATA
+	depends on LEDS_TRIGGERS
+	help
+	  This allows LEDs to be controlled by IDE disk activity.
+	  If unsure, say Y.
+
+config LEDS_TRIGGER_HEARTBEAT
+	tristate "LED Heartbeat Trigger"
+	depends on LEDS_TRIGGERS
+	help
+	  This allows LEDs to be controlled by a CPU load average.
+	  The flash frequency is a hyperbolic function of the 1-minute
+	  load average.
+	  If unsure, say Y.
+
+config LEDS_TRIGGER_BACKLIGHT
+	tristate "LED backlight Trigger"
+	depends on LEDS_TRIGGERS
+	help
+	  This allows LEDs to be controlled as a backlight device: they
+	  turn off and on when the display is blanked and unblanked.
+
+	  If unsure, say N.
+
+config LEDS_TRIGGER_CPU
+	bool "LED CPU Trigger"
+	depends on LEDS_TRIGGERS
+	help
+	  This allows LEDs to be controlled by active CPUs. This shows
+	  the active CPUs across an array of LEDs so you can see which
+	  CPUs are active on the system at any given moment.
+
+	  If unsure, say N.
+
+config LEDS_TRIGGER_GPIO
+	tristate "LED GPIO Trigger"
+	depends on LEDS_TRIGGERS
+	depends on GPIOLIB
+	help
+	  This allows LEDs to be controlled by gpio events. It's good
+	  when using gpios as switches and triggering the needed LEDs
+	  from there. One use case is n810's keypad LEDs that could
+	  be triggered by this trigger when user slides up to show
+	  keypad.
+
+	  If unsure, say N.
+
+config LEDS_TRIGGER_DEFAULT_ON
+	tristate "LED Default ON Trigger"
+	depends on LEDS_TRIGGERS
+	help
+	  This allows LEDs to be initialised in the ON state.
+	  If unsure, say Y.
+
+comment "iptables trigger is under Netfilter config (LED target)"
+	depends on LEDS_TRIGGERS
+
+config LEDS_TRIGGER_TRANSIENT
+	tristate "LED Transient Trigger"
+	depends on LEDS_TRIGGERS
+	help
+	  This allows one time activation of a transient state on
+	  GPIO/PWM based hardware.
+	  If unsure, say Y.
+
+config LEDS_TRIGGER_CAMERA
+	tristate "LED Camera Flash/Torch Trigger"
+	depends on LEDS_TRIGGERS
+	help
+	  This allows LEDs to be controlled as a camera flash/torch device.
+	  This enables direct flash/torch on/off by the driver, kernel space.
+	  If unsure, say Y.
+
+endif # LEDS_TRIGGERS
diff --git a/drivers/leds/trigger/Makefile b/drivers/leds/trigger/Makefile
new file mode 100644
index 000000000000..1abf48dacf7e
--- /dev/null
+++ b/drivers/leds/trigger/Makefile
@@ -0,0 +1,10 @@
+obj-$(CONFIG_LEDS_TRIGGER_TIMER)	+= ledtrig-timer.o
+obj-$(CONFIG_LEDS_TRIGGER_ONESHOT)	+= ledtrig-oneshot.o
+obj-$(CONFIG_LEDS_TRIGGER_IDE_DISK)	+= ledtrig-ide-disk.o
+obj-$(CONFIG_LEDS_TRIGGER_HEARTBEAT)	+= ledtrig-heartbeat.o
+obj-$(CONFIG_LEDS_TRIGGER_BACKLIGHT)	+= ledtrig-backlight.o
+obj-$(CONFIG_LEDS_TRIGGER_GPIO)		+= ledtrig-gpio.o
+obj-$(CONFIG_LEDS_TRIGGER_CPU)		+= ledtrig-cpu.o
+obj-$(CONFIG_LEDS_TRIGGER_DEFAULT_ON)	+= ledtrig-default-on.o
+obj-$(CONFIG_LEDS_TRIGGER_TRANSIENT)	+= ledtrig-transient.o
+obj-$(CONFIG_LEDS_TRIGGER_CAMERA)	+= ledtrig-camera.o
diff --git a/drivers/leds/ledtrig-backlight.c b/drivers/leds/trigger/ledtrig-backlight.c
index 027a2b15d7d8..3c9c88a07eb8 100644
--- a/drivers/leds/ledtrig-backlight.c
+++ b/drivers/leds/trigger/ledtrig-backlight.c
@@ -16,7 +16,7 @@
 #include <linux/init.h>
 #include <linux/fb.h>
 #include <linux/leds.h>
-#include "leds.h"
+#include "../leds.h"
 
 #define BLANK		1
 #define UNBLANK		0
diff --git a/drivers/leds/trigger/ledtrig-camera.c b/drivers/leds/trigger/ledtrig-camera.c
new file mode 100644
index 000000000000..9bd73a8bad5c
--- /dev/null
+++ b/drivers/leds/trigger/ledtrig-camera.c
@@ -0,0 +1,57 @@
+/*
+ * Camera Flash and Torch On/Off Trigger
+ *
+ * based on ledtrig-ide-disk.c
+ *
+ * Copyright 2013 Texas Instruments
+ *
+ * Author: Milo(Woogyom) Kim <milo.kim@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.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/leds.h>
+
+DEFINE_LED_TRIGGER(ledtrig_flash);
+DEFINE_LED_TRIGGER(ledtrig_torch);
+
+void ledtrig_flash_ctrl(bool on)
+{
+	enum led_brightness brt = on ? LED_FULL : LED_OFF;
+
+	led_trigger_event(ledtrig_flash, brt);
+}
+EXPORT_SYMBOL_GPL(ledtrig_flash_ctrl);
+
+void ledtrig_torch_ctrl(bool on)
+{
+	enum led_brightness brt = on ? LED_FULL : LED_OFF;
+
+	led_trigger_event(ledtrig_torch, brt);
+}
+EXPORT_SYMBOL_GPL(ledtrig_torch_ctrl);
+
+static int __init ledtrig_camera_init(void)
+{
+	led_trigger_register_simple("flash", &ledtrig_flash);
+	led_trigger_register_simple("torch", &ledtrig_torch);
+	return 0;
+}
+module_init(ledtrig_camera_init);
+
+static void __exit ledtrig_camera_exit(void)
+{
+	led_trigger_unregister_simple(ledtrig_torch);
+	led_trigger_unregister_simple(ledtrig_flash);
+}
+module_exit(ledtrig_camera_exit);
+
+MODULE_DESCRIPTION("LED Trigger for Camera Flash/Torch Control");
+MODULE_AUTHOR("Milo Kim");
+MODULE_LICENSE("GPL");
diff --git a/drivers/leds/ledtrig-cpu.c b/drivers/leds/trigger/ledtrig-cpu.c
index 4239b3955ff0..118335eccc56 100644
--- a/drivers/leds/ledtrig-cpu.c
+++ b/drivers/leds/trigger/ledtrig-cpu.c
@@ -26,7 +26,7 @@
 #include <linux/percpu.h>
 #include <linux/syscore_ops.h>
 #include <linux/rwsem.h>
-#include "leds.h"
+#include "../leds.h"
 
 #define MAX_NAME_LEN	8
 
diff --git a/drivers/leds/ledtrig-default-on.c b/drivers/leds/trigger/ledtrig-default-on.c
index eac1f1b1adac..81a91be8e18d 100644
--- a/drivers/leds/ledtrig-default-on.c
+++ b/drivers/leds/trigger/ledtrig-default-on.c
@@ -15,7 +15,7 @@
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/leds.h>
-#include "leds.h"
+#include "../leds.h"
 
 static void defon_trig_activate(struct led_classdev *led_cdev)
 {
diff --git a/drivers/leds/ledtrig-gpio.c b/drivers/leds/trigger/ledtrig-gpio.c
index 72e3ebfc281f..35812e3a37f2 100644
--- a/drivers/leds/ledtrig-gpio.c
+++ b/drivers/leds/trigger/ledtrig-gpio.c
@@ -17,7 +17,7 @@
 #include <linux/workqueue.h>
 #include <linux/leds.h>
 #include <linux/slab.h>
-#include "leds.h"
+#include "../leds.h"
 
 struct gpio_trig_data {
 	struct led_classdev *led;
diff --git a/drivers/leds/ledtrig-heartbeat.c b/drivers/leds/trigger/ledtrig-heartbeat.c
index 1edc7463ce83..5c8464a33172 100644
--- a/drivers/leds/ledtrig-heartbeat.c
+++ b/drivers/leds/trigger/ledtrig-heartbeat.c
@@ -19,7 +19,7 @@
 #include <linux/sched.h>
 #include <linux/leds.h>
 #include <linux/reboot.h>
-#include "leds.h"
+#include "../leds.h"
 
 static int panic_heartbeats;
 
diff --git a/drivers/leds/ledtrig-ide-disk.c b/drivers/leds/trigger/ledtrig-ide-disk.c
index 2cd7c0cf5924..2cd7c0cf5924 100644
--- a/drivers/leds/ledtrig-ide-disk.c
+++ b/drivers/leds/trigger/ledtrig-ide-disk.c
diff --git a/drivers/leds/ledtrig-oneshot.c b/drivers/leds/trigger/ledtrig-oneshot.c
index 2c029aa5c4f1..cb4c7466692a 100644
--- a/drivers/leds/ledtrig-oneshot.c
+++ b/drivers/leds/trigger/ledtrig-oneshot.c
@@ -18,7 +18,7 @@
 #include <linux/ctype.h>
 #include <linux/slab.h>
 #include <linux/leds.h>
-#include "leds.h"
+#include "../leds.h"
 
 #define DEFAULT_DELAY 100
 
diff --git a/drivers/leds/ledtrig-timer.c b/drivers/leds/trigger/ledtrig-timer.c
index f774d0592204..8d09327b5719 100644
--- a/drivers/leds/ledtrig-timer.c
+++ b/drivers/leds/trigger/ledtrig-timer.c
@@ -17,7 +17,6 @@
 #include <linux/device.h>
 #include <linux/ctype.h>
 #include <linux/leds.h>
-#include "leds.h"
 
 static ssize_t led_delay_on_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
diff --git a/drivers/leds/ledtrig-transient.c b/drivers/leds/trigger/ledtrig-transient.c
index 398f1042c43e..e5abc00bb00c 100644
--- a/drivers/leds/ledtrig-transient.c
+++ b/drivers/leds/trigger/ledtrig-transient.c
@@ -25,7 +25,7 @@
 #include <linux/slab.h>
 #include <linux/timer.h>
 #include <linux/leds.h>
-#include "leds.h"
+#include "../leds.h"
 
 struct transient_trig_data {
 	int activate;
diff --git a/drivers/lguest/Kconfig b/drivers/lguest/Kconfig
index 89875ea19ade..ee035ec4526b 100644
--- a/drivers/lguest/Kconfig
+++ b/drivers/lguest/Kconfig
@@ -5,10 +5,9 @@ config LGUEST
 	---help---
 	  This is a very simple module which allows you to run
 	  multiple instances of the same Linux kernel, using the
-	  "lguest" command found in the Documentation/virtual/lguest
-	  directory.
+	  "lguest" command found in the tools/lguest directory.
 
 	  Note that "lguest" is pronounced to rhyme with "fell quest",
-	  not "rustyvisor". See Documentation/virtual/lguest/lguest.txt.
+	  not "rustyvisor". See tools/lguest/lguest.txt.
 
 	  If unsure, say N.  If curious, say M.  If masochistic, say Y.
diff --git a/drivers/lguest/core.c b/drivers/lguest/core.c
index a5ebc0083d87..0bf1e4edf04d 100644
--- a/drivers/lguest/core.c
+++ b/drivers/lguest/core.c
@@ -20,9 +20,9 @@
 #include <asm/asm-offsets.h>
 #include "lg.h"
 
-
+unsigned long switcher_addr;
+struct page **lg_switcher_pages;
 static struct vm_struct *switcher_vma;
-static struct page **switcher_page;
 
 /* This One Big lock protects all inter-guest data structures. */
 DEFINE_MUTEX(lguest_lock);
@@ -52,13 +52,21 @@ static __init int map_switcher(void)
 	 * easy.
 	 */
 
+	/* We assume Switcher text fits into a single page. */
+	if (end_switcher_text - start_switcher_text > PAGE_SIZE) {
+		printk(KERN_ERR "lguest: switcher text too large (%zu)\n",
+		       end_switcher_text - start_switcher_text);
+		return -EINVAL;
+	}
+
 	/*
 	 * We allocate an array of struct page pointers.  map_vm_area() wants
 	 * this, rather than just an array of pages.
 	 */
-	switcher_page = kmalloc(sizeof(switcher_page[0])*TOTAL_SWITCHER_PAGES,
-				GFP_KERNEL);
-	if (!switcher_page) {
+	lg_switcher_pages = kmalloc(sizeof(lg_switcher_pages[0])
+				    * TOTAL_SWITCHER_PAGES,
+				    GFP_KERNEL);
+	if (!lg_switcher_pages) {
 		err = -ENOMEM;
 		goto out;
 	}
@@ -68,32 +76,29 @@ static __init int map_switcher(void)
 	 * so we make sure they're zeroed.
 	 */
 	for (i = 0; i < TOTAL_SWITCHER_PAGES; i++) {
-		switcher_page[i] = alloc_page(GFP_KERNEL|__GFP_ZERO);
-		if (!switcher_page[i]) {
+		lg_switcher_pages[i] = alloc_page(GFP_KERNEL|__GFP_ZERO);
+		if (!lg_switcher_pages[i]) {
 			err = -ENOMEM;
 			goto free_some_pages;
 		}
 	}
 
 	/*
-	 * First we check that the Switcher won't overlap the fixmap area at
-	 * the top of memory.  It's currently nowhere near, but it could have
-	 * very strange effects if it ever happened.
+	 * We place the Switcher underneath the fixmap area, which is the
+	 * highest virtual address we can get.  This is important, since we
+	 * tell the Guest it can't access this memory, so we want its ceiling
+	 * as high as possible.
 	 */
-	if (SWITCHER_ADDR + (TOTAL_SWITCHER_PAGES+1)*PAGE_SIZE > FIXADDR_START){
-		err = -ENOMEM;
-		printk("lguest: mapping switcher would thwack fixmap\n");
-		goto free_pages;
-	}
+	switcher_addr = FIXADDR_START - (TOTAL_SWITCHER_PAGES+1)*PAGE_SIZE;
 
 	/*
-	 * Now we reserve the "virtual memory area" we want: 0xFFC00000
-	 * (SWITCHER_ADDR).  We might not get it in theory, but in practice
-	 * it's worked so far.  The end address needs +1 because __get_vm_area
-	 * allocates an extra guard page, so we need space for that.
+	 * Now we reserve the "virtual memory area" we want.  We might
+	 * not get it in theory, but in practice it's worked so far.
+	 * The end address needs +1 because __get_vm_area allocates an
+	 * extra guard page, so we need space for that.
 	 */
 	switcher_vma = __get_vm_area(TOTAL_SWITCHER_PAGES * PAGE_SIZE,
-				     VM_ALLOC, SWITCHER_ADDR, SWITCHER_ADDR
+				     VM_ALLOC, switcher_addr, switcher_addr
 				     + (TOTAL_SWITCHER_PAGES+1) * PAGE_SIZE);
 	if (!switcher_vma) {
 		err = -ENOMEM;
@@ -103,12 +108,12 @@ static __init int map_switcher(void)
 
 	/*
 	 * This code actually sets up the pages we've allocated to appear at
-	 * SWITCHER_ADDR.  map_vm_area() takes the vma we allocated above, the
+	 * switcher_addr.  map_vm_area() takes the vma we allocated above, the
 	 * kind of pages we're mapping (kernel pages), and a pointer to our
 	 * array of struct pages.  It increments that pointer, but we don't
 	 * care.
 	 */
-	pagep = switcher_page;
+	pagep = lg_switcher_pages;
 	err = map_vm_area(switcher_vma, PAGE_KERNEL_EXEC, &pagep);
 	if (err) {
 		printk("lguest: map_vm_area failed: %i\n", err);
@@ -133,8 +138,8 @@ free_pages:
 	i = TOTAL_SWITCHER_PAGES;
 free_some_pages:
 	for (--i; i >= 0; i--)
-		__free_pages(switcher_page[i], 0);
-	kfree(switcher_page);
+		__free_pages(lg_switcher_pages[i], 0);
+	kfree(lg_switcher_pages);
 out:
 	return err;
 }
@@ -149,8 +154,8 @@ static void unmap_switcher(void)
 	vunmap(switcher_vma->addr);
 	/* Now we just need to free the pages we copied the switcher into */
 	for (i = 0; i < TOTAL_SWITCHER_PAGES; i++)
-		__free_pages(switcher_page[i], 0);
-	kfree(switcher_page);
+		__free_pages(lg_switcher_pages[i], 0);
+	kfree(lg_switcher_pages);
 }
 
 /*H:032
@@ -323,15 +328,10 @@ static int __init init(void)
 	if (err)
 		goto out;
 
-	/* Now we set up the pagetable implementation for the Guests. */
-	err = init_pagetables(switcher_page, SHARED_SWITCHER_PAGES);
-	if (err)
-		goto unmap;
-
 	/* We might need to reserve an interrupt vector. */
 	err = init_interrupts();
 	if (err)
-		goto free_pgtables;
+		goto unmap;
 
 	/* /dev/lguest needs to be registered. */
 	err = lguest_device_init();
@@ -346,8 +346,6 @@ static int __init init(void)
 
 free_interrupts:
 	free_interrupts();
-free_pgtables:
-	free_pagetables();
 unmap:
 	unmap_switcher();
 out:
@@ -359,7 +357,6 @@ static void __exit fini(void)
 {
 	lguest_device_remove();
 	free_interrupts();
-	free_pagetables();
 	unmap_switcher();
 
 	lguest_arch_host_fini();
diff --git a/drivers/lguest/lg.h b/drivers/lguest/lg.h
index 295df06e6590..2eef40be4c04 100644
--- a/drivers/lguest/lg.h
+++ b/drivers/lguest/lg.h
@@ -14,11 +14,10 @@
 
 #include <asm/lguest.h>
 
-void free_pagetables(void);
-int init_pagetables(struct page **switcher_page, unsigned int pages);
-
 struct pgdir {
 	unsigned long gpgdir;
+	bool switcher_mapped;
+	int last_host_cpu;
 	pgd_t *pgdir;
 };
 
@@ -124,6 +123,7 @@ bool lguest_address_ok(const struct lguest *lg,
 		       unsigned long addr, unsigned long len);
 void __lgread(struct lg_cpu *, void *, unsigned long, unsigned);
 void __lgwrite(struct lg_cpu *, unsigned long, const void *, unsigned);
+extern struct page **lg_switcher_pages;
 
 /*H:035
  * Using memory-copy operations like that is usually inconvient, so we
diff --git a/drivers/lguest/lguest_user.c b/drivers/lguest/lguest_user.c
index ff4a0bc9904d..4263f4cc8c55 100644
--- a/drivers/lguest/lguest_user.c
+++ b/drivers/lguest/lguest_user.c
@@ -250,13 +250,13 @@ static ssize_t read(struct file *file, char __user *user, size_t size,loff_t*o)
  */
 static int lg_cpu_start(struct lg_cpu *cpu, unsigned id, unsigned long start_ip)
 {
-	/* We have a limited number the number of CPUs in the lguest struct. */
+	/* We have a limited number of CPUs in the lguest struct. */
 	if (id >= ARRAY_SIZE(cpu->lg->cpus))
 		return -EINVAL;
 
 	/* Set up this CPU's id, and pointer back to the lguest struct. */
 	cpu->id = id;
-	cpu->lg = container_of((cpu - id), struct lguest, cpus[0]);
+	cpu->lg = container_of(cpu, struct lguest, cpus[id]);
 	cpu->lg->nr_cpus++;
 
 	/* Each CPU has a timer it can set. */
@@ -270,7 +270,7 @@ static int lg_cpu_start(struct lg_cpu *cpu, unsigned id, unsigned long start_ip)
 	if (!cpu->regs_page)
 		return -ENOMEM;
 
-	/* We actually put the registers at the bottom of the page. */
+	/* We actually put the registers at the end of the page. */
 	cpu->regs = (void *)cpu->regs_page + PAGE_SIZE - sizeof(*cpu->regs);
 
 	/*
diff --git a/drivers/lguest/page_tables.c b/drivers/lguest/page_tables.c
index 864baabaee25..699187ab3800 100644
--- a/drivers/lguest/page_tables.c
+++ b/drivers/lguest/page_tables.c
@@ -7,7 +7,7 @@
  * converted Guest pages when running the Guest.
 :*/
 
-/* Copyright (C) Rusty Russell IBM Corporation 2006.
+/* Copyright (C) Rusty Russell IBM Corporation 2013.
  * GPL v2 and any later version */
 #include <linux/mm.h>
 #include <linux/gfp.h>
@@ -62,22 +62,11 @@
  * will need the last pmd entry of the last pmd page.
  */
 #ifdef CONFIG_X86_PAE
-#define SWITCHER_PMD_INDEX 	(PTRS_PER_PMD - 1)
-#define RESERVE_MEM 		2U
 #define CHECK_GPGD_MASK		_PAGE_PRESENT
 #else
-#define RESERVE_MEM 		4U
 #define CHECK_GPGD_MASK		_PAGE_TABLE
 #endif
 
-/*
- * We actually need a separate PTE page for each CPU.  Remember that after the
- * Switcher code itself comes two pages for each CPU, and we don't want this
- * CPU's guest to see the pages of any other CPU.
- */
-static DEFINE_PER_CPU(pte_t *, switcher_pte_pages);
-#define switcher_pte_page(cpu) per_cpu(switcher_pte_pages, cpu)
-
 /*H:320
  * The page table code is curly enough to need helper functions to keep it
  * clear and clean.  The kernel itself provides many of them; one advantage
@@ -95,13 +84,6 @@ static pgd_t *spgd_addr(struct lg_cpu *cpu, u32 i, unsigned long vaddr)
 {
 	unsigned int index = pgd_index(vaddr);
 
-#ifndef CONFIG_X86_PAE
-	/* We kill any Guest trying to touch the Switcher addresses. */
-	if (index >= SWITCHER_PGD_INDEX) {
-		kill_guest(cpu, "attempt to access switcher pages");
-		index = 0;
-	}
-#endif
 	/* Return a pointer index'th pgd entry for the i'th page table. */
 	return &cpu->lg->pgdirs[i].pgdir[index];
 }
@@ -117,13 +99,6 @@ static pmd_t *spmd_addr(struct lg_cpu *cpu, pgd_t spgd, unsigned long vaddr)
 	unsigned int index = pmd_index(vaddr);
 	pmd_t *page;
 
-	/* We kill any Guest trying to touch the Switcher addresses. */
-	if (pgd_index(vaddr) == SWITCHER_PGD_INDEX &&
-					index >= SWITCHER_PMD_INDEX) {
-		kill_guest(cpu, "attempt to access switcher pages");
-		index = 0;
-	}
-
 	/* You should never call this if the PGD entry wasn't valid */
 	BUG_ON(!(pgd_flags(spgd) & _PAGE_PRESENT));
 	page = __va(pgd_pfn(spgd) << PAGE_SHIFT);
@@ -275,122 +250,177 @@ static void release_pte(pte_t pte)
 }
 /*:*/
 
-static void check_gpte(struct lg_cpu *cpu, pte_t gpte)
+static bool check_gpte(struct lg_cpu *cpu, pte_t gpte)
 {
 	if ((pte_flags(gpte) & _PAGE_PSE) ||
-	    pte_pfn(gpte) >= cpu->lg->pfn_limit)
+	    pte_pfn(gpte) >= cpu->lg->pfn_limit) {
 		kill_guest(cpu, "bad page table entry");
+		return false;
+	}
+	return true;
 }
 
-static void check_gpgd(struct lg_cpu *cpu, pgd_t gpgd)
+static bool check_gpgd(struct lg_cpu *cpu, pgd_t gpgd)
 {
 	if ((pgd_flags(gpgd) & ~CHECK_GPGD_MASK) ||
-	   (pgd_pfn(gpgd) >= cpu->lg->pfn_limit))
+	    (pgd_pfn(gpgd) >= cpu->lg->pfn_limit)) {
 		kill_guest(cpu, "bad page directory entry");
+		return false;
+	}
+	return true;
 }
 
 #ifdef CONFIG_X86_PAE
-static void check_gpmd(struct lg_cpu *cpu, pmd_t gpmd)
+static bool check_gpmd(struct lg_cpu *cpu, pmd_t gpmd)
 {
 	if ((pmd_flags(gpmd) & ~_PAGE_TABLE) ||
-	   (pmd_pfn(gpmd) >= cpu->lg->pfn_limit))
+	    (pmd_pfn(gpmd) >= cpu->lg->pfn_limit)) {
 		kill_guest(cpu, "bad page middle directory entry");
+		return false;
+	}
+	return true;
 }
 #endif
 
-/*H:330
- * (i) Looking up a page table entry when the Guest faults.
- *
- * We saw this call in run_guest(): when we see a page fault in the Guest, we
- * come here.  That's because we only set up the shadow page tables lazily as
- * they're needed, so we get page faults all the time and quietly fix them up
- * and return to the Guest without it knowing.
+/*H:331
+ * This is the core routine to walk the shadow page tables and find the page
+ * table entry for a specific address.
  *
- * If we fixed up the fault (ie. we mapped the address), this routine returns
- * true.  Otherwise, it was a real fault and we need to tell the Guest.
+ * If allocate is set, then we allocate any missing levels, setting the flags
+ * on the new page directory and mid-level directories using the arguments
+ * (which are copied from the Guest's page table entries).
  */
-bool demand_page(struct lg_cpu *cpu, unsigned long vaddr, int errcode)
+static pte_t *find_spte(struct lg_cpu *cpu, unsigned long vaddr, bool allocate,
+			int pgd_flags, int pmd_flags)
 {
-	pgd_t gpgd;
 	pgd_t *spgd;
-	unsigned long gpte_ptr;
-	pte_t gpte;
-	pte_t *spte;
-
 	/* Mid level for PAE. */
 #ifdef CONFIG_X86_PAE
 	pmd_t *spmd;
-	pmd_t gpmd;
 #endif
 
-	/* First step: get the top-level Guest page table entry. */
-	if (unlikely(cpu->linear_pages)) {
-		/* Faking up a linear mapping. */
-		gpgd = __pgd(CHECK_GPGD_MASK);
-	} else {
-		gpgd = lgread(cpu, gpgd_addr(cpu, vaddr), pgd_t);
-		/* Toplevel not present?  We can't map it in. */
-		if (!(pgd_flags(gpgd) & _PAGE_PRESENT))
-			return false;
-	}
-
-	/* Now look at the matching shadow entry. */
+	/* Get top level entry. */
 	spgd = spgd_addr(cpu, cpu->cpu_pgd, vaddr);
 	if (!(pgd_flags(*spgd) & _PAGE_PRESENT)) {
 		/* No shadow entry: allocate a new shadow PTE page. */
-		unsigned long ptepage = get_zeroed_page(GFP_KERNEL);
+		unsigned long ptepage;
+
+		/* If they didn't want us to allocate anything, stop. */
+		if (!allocate)
+			return NULL;
+
+		ptepage = get_zeroed_page(GFP_KERNEL);
 		/*
 		 * This is not really the Guest's fault, but killing it is
 		 * simple for this corner case.
 		 */
 		if (!ptepage) {
 			kill_guest(cpu, "out of memory allocating pte page");
-			return false;
+			return NULL;
 		}
-		/* We check that the Guest pgd is OK. */
-		check_gpgd(cpu, gpgd);
 		/*
 		 * And we copy the flags to the shadow PGD entry.  The page
 		 * number in the shadow PGD is the page we just allocated.
 		 */
-		set_pgd(spgd, __pgd(__pa(ptepage) | pgd_flags(gpgd)));
+		set_pgd(spgd, __pgd(__pa(ptepage) | pgd_flags));
 	}
 
+	/*
+	 * Intel's Physical Address Extension actually uses three levels of
+	 * page tables, so we need to look in the mid-level.
+	 */
 #ifdef CONFIG_X86_PAE
-	if (unlikely(cpu->linear_pages)) {
-		/* Faking up a linear mapping. */
-		gpmd = __pmd(_PAGE_TABLE);
-	} else {
-		gpmd = lgread(cpu, gpmd_addr(gpgd, vaddr), pmd_t);
-		/* Middle level not present?  We can't map it in. */
-		if (!(pmd_flags(gpmd) & _PAGE_PRESENT))
-			return false;
-	}
-
-	/* Now look at the matching shadow entry. */
+	/* Now look at the mid-level shadow entry. */
 	spmd = spmd_addr(cpu, *spgd, vaddr);
 
 	if (!(pmd_flags(*spmd) & _PAGE_PRESENT)) {
 		/* No shadow entry: allocate a new shadow PTE page. */
-		unsigned long ptepage = get_zeroed_page(GFP_KERNEL);
+		unsigned long ptepage;
+
+		/* If they didn't want us to allocate anything, stop. */
+		if (!allocate)
+			return NULL;
+
+		ptepage = get_zeroed_page(GFP_KERNEL);
 
 		/*
 		 * This is not really the Guest's fault, but killing it is
 		 * simple for this corner case.
 		 */
 		if (!ptepage) {
-			kill_guest(cpu, "out of memory allocating pte page");
-			return false;
+			kill_guest(cpu, "out of memory allocating pmd page");
+			return NULL;
 		}
 
-		/* We check that the Guest pmd is OK. */
-		check_gpmd(cpu, gpmd);
-
 		/*
 		 * And we copy the flags to the shadow PMD entry.  The page
 		 * number in the shadow PMD is the page we just allocated.
 		 */
-		set_pmd(spmd, __pmd(__pa(ptepage) | pmd_flags(gpmd)));
+		set_pmd(spmd, __pmd(__pa(ptepage) | pmd_flags));
+	}
+#endif
+
+	/* Get the pointer to the shadow PTE entry we're going to set. */
+	return spte_addr(cpu, *spgd, vaddr);
+}
+
+/*H:330
+ * (i) Looking up a page table entry when the Guest faults.
+ *
+ * We saw this call in run_guest(): when we see a page fault in the Guest, we
+ * come here.  That's because we only set up the shadow page tables lazily as
+ * they're needed, so we get page faults all the time and quietly fix them up
+ * and return to the Guest without it knowing.
+ *
+ * If we fixed up the fault (ie. we mapped the address), this routine returns
+ * true.  Otherwise, it was a real fault and we need to tell the Guest.
+ */
+bool demand_page(struct lg_cpu *cpu, unsigned long vaddr, int errcode)
+{
+	unsigned long gpte_ptr;
+	pte_t gpte;
+	pte_t *spte;
+	pmd_t gpmd;
+	pgd_t gpgd;
+
+	/* We never demand page the Switcher, so trying is a mistake. */
+	if (vaddr >= switcher_addr)
+		return false;
+
+	/* First step: get the top-level Guest page table entry. */
+	if (unlikely(cpu->linear_pages)) {
+		/* Faking up a linear mapping. */
+		gpgd = __pgd(CHECK_GPGD_MASK);
+	} else {
+		gpgd = lgread(cpu, gpgd_addr(cpu, vaddr), pgd_t);
+		/* Toplevel not present?  We can't map it in. */
+		if (!(pgd_flags(gpgd) & _PAGE_PRESENT))
+			return false;
+
+		/* 
+		 * This kills the Guest if it has weird flags or tries to
+		 * refer to a "physical" address outside the bounds.
+		 */
+		if (!check_gpgd(cpu, gpgd))
+			return false;
+	}
+
+	/* This "mid-level" entry is only used for non-linear, PAE mode. */
+	gpmd = __pmd(_PAGE_TABLE);
+
+#ifdef CONFIG_X86_PAE
+	if (likely(!cpu->linear_pages)) {
+		gpmd = lgread(cpu, gpmd_addr(gpgd, vaddr), pmd_t);
+		/* Middle level not present?  We can't map it in. */
+		if (!(pmd_flags(gpmd) & _PAGE_PRESENT))
+			return false;
+
+		/* 
+		 * This kills the Guest if it has weird flags or tries to
+		 * refer to a "physical" address outside the bounds.
+		 */
+		if (!check_gpmd(cpu, gpmd))
+			return false;
 	}
 
 	/*
@@ -433,7 +463,8 @@ bool demand_page(struct lg_cpu *cpu, unsigned long vaddr, int errcode)
 	 * Check that the Guest PTE flags are OK, and the page number is below
 	 * the pfn_limit (ie. not mapping the Launcher binary).
 	 */
-	check_gpte(cpu, gpte);
+	if (!check_gpte(cpu, gpte))
+		return false;
 
 	/* Add the _PAGE_ACCESSED and (for a write) _PAGE_DIRTY flag */
 	gpte = pte_mkyoung(gpte);
@@ -441,7 +472,9 @@ bool demand_page(struct lg_cpu *cpu, unsigned long vaddr, int errcode)
 		gpte = pte_mkdirty(gpte);
 
 	/* Get the pointer to the shadow PTE entry we're going to set. */
-	spte = spte_addr(cpu, *spgd, vaddr);
+	spte = find_spte(cpu, vaddr, true, pgd_flags(gpgd), pmd_flags(gpmd));
+	if (!spte)
+		return false;
 
 	/*
 	 * If there was a valid shadow PTE entry here before, we release it.
@@ -493,29 +526,23 @@ bool demand_page(struct lg_cpu *cpu, unsigned long vaddr, int errcode)
  */
 static bool page_writable(struct lg_cpu *cpu, unsigned long vaddr)
 {
-	pgd_t *spgd;
+	pte_t *spte;
 	unsigned long flags;
 
-#ifdef CONFIG_X86_PAE
-	pmd_t *spmd;
-#endif
-	/* Look at the current top level entry: is it present? */
-	spgd = spgd_addr(cpu, cpu->cpu_pgd, vaddr);
-	if (!(pgd_flags(*spgd) & _PAGE_PRESENT))
+	/* You can't put your stack in the Switcher! */
+	if (vaddr >= switcher_addr)
 		return false;
 
-#ifdef CONFIG_X86_PAE
-	spmd = spmd_addr(cpu, *spgd, vaddr);
-	if (!(pmd_flags(*spmd) & _PAGE_PRESENT))
+	/* If there's no shadow PTE, it's not writable. */
+	spte = find_spte(cpu, vaddr, false, 0, 0);
+	if (!spte)
 		return false;
-#endif
 
 	/*
 	 * Check the flags on the pte entry itself: it must be present and
 	 * writable.
 	 */
-	flags = pte_flags(*(spte_addr(cpu, *spgd, vaddr)));
-
+	flags = pte_flags(*spte);
 	return (flags & (_PAGE_PRESENT|_PAGE_RW)) == (_PAGE_PRESENT|_PAGE_RW);
 }
 
@@ -678,9 +705,6 @@ static unsigned int new_pgdir(struct lg_cpu *cpu,
 			      int *blank_pgdir)
 {
 	unsigned int next;
-#ifdef CONFIG_X86_PAE
-	pmd_t *pmd_table;
-#endif
 
 	/*
 	 * We pick one entry at random to throw out.  Choosing the Least
@@ -695,29 +719,11 @@ static unsigned int new_pgdir(struct lg_cpu *cpu,
 		if (!cpu->lg->pgdirs[next].pgdir)
 			next = cpu->cpu_pgd;
 		else {
-#ifdef CONFIG_X86_PAE
 			/*
-			 * In PAE mode, allocate a pmd page and populate the
-			 * last pgd entry.
+			 * This is a blank page, so there are no kernel
+			 * mappings: caller must map the stack!
 			 */
-			pmd_table = (pmd_t *)get_zeroed_page(GFP_KERNEL);
-			if (!pmd_table) {
-				free_page((long)cpu->lg->pgdirs[next].pgdir);
-				set_pgd(cpu->lg->pgdirs[next].pgdir, __pgd(0));
-				next = cpu->cpu_pgd;
-			} else {
-				set_pgd(cpu->lg->pgdirs[next].pgdir +
-					SWITCHER_PGD_INDEX,
-					__pgd(__pa(pmd_table) | _PAGE_PRESENT));
-				/*
-				 * This is a blank page, so there are no kernel
-				 * mappings: caller must map the stack!
-				 */
-				*blank_pgdir = 1;
-			}
-#else
 			*blank_pgdir = 1;
-#endif
 		}
 	}
 	/* Record which Guest toplevel this shadows. */
@@ -725,9 +731,50 @@ static unsigned int new_pgdir(struct lg_cpu *cpu,
 	/* Release all the non-kernel mappings. */
 	flush_user_mappings(cpu->lg, next);
 
+	/* This hasn't run on any CPU at all. */
+	cpu->lg->pgdirs[next].last_host_cpu = -1;
+
 	return next;
 }
 
+/*H:501
+ * We do need the Switcher code mapped at all times, so we allocate that
+ * part of the Guest page table here.  We map the Switcher code immediately,
+ * but defer mapping of the guest register page and IDT/LDT etc page until
+ * just before we run the guest in map_switcher_in_guest().
+ *
+ * We *could* do this setup in map_switcher_in_guest(), but at that point
+ * we've interrupts disabled, and allocating pages like that is fraught: we
+ * can't sleep if we need to free up some memory.
+ */
+static bool allocate_switcher_mapping(struct lg_cpu *cpu)
+{
+	int i;
+
+	for (i = 0; i < TOTAL_SWITCHER_PAGES; i++) {
+		pte_t *pte = find_spte(cpu, switcher_addr + i * PAGE_SIZE, true,
+				       CHECK_GPGD_MASK, _PAGE_TABLE);
+		if (!pte)
+			return false;
+
+		/*
+		 * Map the switcher page if not already there.  It might
+		 * already be there because we call allocate_switcher_mapping()
+		 * in guest_set_pgd() just in case it did discard our Switcher
+		 * mapping, but it probably didn't.
+		 */
+		if (i == 0 && !(pte_flags(*pte) & _PAGE_PRESENT)) {
+			/* Get a reference to the Switcher page. */
+			get_page(lg_switcher_pages[0]);
+			/* Create a read-only, exectuable, kernel-style PTE */
+			set_pte(pte,
+				mk_pte(lg_switcher_pages[0], PAGE_KERNEL_RX));
+		}
+	}
+	cpu->lg->pgdirs[cpu->cpu_pgd].switcher_mapped = true;
+	return true;
+}
+
 /*H:470
  * Finally, a routine which throws away everything: all PGD entries in all
  * the shadow page tables, including the Guest's kernel mappings.  This is used
@@ -738,28 +785,16 @@ static void release_all_pagetables(struct lguest *lg)
 	unsigned int i, j;
 
 	/* Every shadow pagetable this Guest has */
-	for (i = 0; i < ARRAY_SIZE(lg->pgdirs); i++)
-		if (lg->pgdirs[i].pgdir) {
-#ifdef CONFIG_X86_PAE
-			pgd_t *spgd;
-			pmd_t *pmdpage;
-			unsigned int k;
-
-			/* Get the last pmd page. */
-			spgd = lg->pgdirs[i].pgdir + SWITCHER_PGD_INDEX;
-			pmdpage = __va(pgd_pfn(*spgd) << PAGE_SHIFT);
-
-			/*
-			 * And release the pmd entries of that pmd page,
-			 * except for the switcher pmd.
-			 */
-			for (k = 0; k < SWITCHER_PMD_INDEX; k++)
-				release_pmd(&pmdpage[k]);
-#endif
-			/* Every PGD entry except the Switcher at the top */
-			for (j = 0; j < SWITCHER_PGD_INDEX; j++)
-				release_pgd(lg->pgdirs[i].pgdir + j);
-		}
+	for (i = 0; i < ARRAY_SIZE(lg->pgdirs); i++) {
+		if (!lg->pgdirs[i].pgdir)
+			continue;
+
+		/* Every PGD entry. */
+		for (j = 0; j < PTRS_PER_PGD; j++)
+			release_pgd(lg->pgdirs[i].pgdir + j);
+		lg->pgdirs[i].switcher_mapped = false;
+		lg->pgdirs[i].last_host_cpu = -1;
+	}
 }
 
 /*
@@ -773,6 +808,9 @@ void guest_pagetable_clear_all(struct lg_cpu *cpu)
 	release_all_pagetables(cpu->lg);
 	/* We need the Guest kernel stack mapped again. */
 	pin_stack_pages(cpu);
+	/* And we need Switcher allocated. */
+	if (!allocate_switcher_mapping(cpu))
+		kill_guest(cpu, "Cannot populate switcher mapping");
 }
 
 /*H:430
@@ -808,9 +846,17 @@ void guest_new_pagetable(struct lg_cpu *cpu, unsigned long pgtable)
 		newpgdir = new_pgdir(cpu, pgtable, &repin);
 	/* Change the current pgd index to the new one. */
 	cpu->cpu_pgd = newpgdir;
-	/* If it was completely blank, we map in the Guest kernel stack */
+	/*
+	 * If it was completely blank, we map in the Guest kernel stack and
+	 * the Switcher.
+	 */
 	if (repin)
 		pin_stack_pages(cpu);
+
+	if (!cpu->lg->pgdirs[cpu->cpu_pgd].switcher_mapped) {
+		if (!allocate_switcher_mapping(cpu))
+			kill_guest(cpu, "Cannot populate switcher mapping");
+	}
 }
 /*:*/
 
@@ -865,7 +911,8 @@ static void do_set_pte(struct lg_cpu *cpu, int idx,
 			 * micro-benchmark.
 			 */
 			if (pte_flags(gpte) & (_PAGE_DIRTY | _PAGE_ACCESSED)) {
-				check_gpte(cpu, gpte);
+				if (!check_gpte(cpu, gpte))
+					return;
 				set_pte(spte,
 					gpte_to_spte(cpu, gpte,
 						pte_flags(gpte) & _PAGE_DIRTY));
@@ -897,6 +944,12 @@ static void do_set_pte(struct lg_cpu *cpu, int idx,
 void guest_set_pte(struct lg_cpu *cpu,
 		   unsigned long gpgdir, unsigned long vaddr, pte_t gpte)
 {
+	/* We don't let you remap the Switcher; we need it to get back! */
+	if (vaddr >= switcher_addr) {
+		kill_guest(cpu, "attempt to set pte into Switcher pages");
+		return;
+	}
+
 	/*
 	 * Kernel mappings must be changed on all top levels.  Slow, but doesn't
 	 * happen often.
@@ -933,14 +986,23 @@ void guest_set_pgd(struct lguest *lg, unsigned long gpgdir, u32 idx)
 {
 	int pgdir;
 
-	if (idx >= SWITCHER_PGD_INDEX)
+	if (idx > PTRS_PER_PGD) {
+		kill_guest(&lg->cpus[0], "Attempt to set pgd %u/%u",
+			   idx, PTRS_PER_PGD);
 		return;
+	}
 
 	/* If they're talking about a page table we have a shadow for... */
 	pgdir = find_pgdir(lg, gpgdir);
-	if (pgdir < ARRAY_SIZE(lg->pgdirs))
+	if (pgdir < ARRAY_SIZE(lg->pgdirs)) {
 		/* ... throw it away. */
 		release_pgd(lg->pgdirs[pgdir].pgdir + idx);
+		/* That might have been the Switcher mapping, remap it. */
+		if (!allocate_switcher_mapping(&lg->cpus[0])) {
+			kill_guest(&lg->cpus[0],
+				   "Cannot populate switcher mapping");
+		}
+	}
 }
 
 #ifdef CONFIG_X86_PAE
@@ -958,6 +1020,9 @@ void guest_set_pmd(struct lguest *lg, unsigned long pmdp, u32 idx)
  * we will populate on future faults.  The Guest doesn't have any actual
  * pagetables yet, so we set linear_pages to tell demand_page() to fake it
  * for the moment.
+ *
+ * We do need the Switcher to be mapped at all times, so we allocate that
+ * part of the Guest page table here.
  */
 int init_guest_pagetable(struct lguest *lg)
 {
@@ -971,21 +1036,34 @@ int init_guest_pagetable(struct lguest *lg)
 
 	/* We start with a linear mapping until the initialize. */
 	cpu->linear_pages = true;
+
+	/* Allocate the page tables for the Switcher. */
+	if (!allocate_switcher_mapping(cpu)) {
+		release_all_pagetables(lg);
+		return -ENOMEM;
+	}
+
 	return 0;
 }
 
 /*H:508 When the Guest calls LHCALL_LGUEST_INIT we do more setup. */
 void page_table_guest_data_init(struct lg_cpu *cpu)
 {
+	/*
+	 * We tell the Guest that it can't use the virtual addresses
+	 * used by the Switcher.  This trick is equivalent to 4GB -
+	 * switcher_addr.
+	 */
+	u32 top = ~switcher_addr + 1;
+
 	/* We get the kernel address: above this is all kernel memory. */
 	if (get_user(cpu->lg->kernel_address,
-		&cpu->lg->lguest_data->kernel_address)
+		     &cpu->lg->lguest_data->kernel_address)
 		/*
-		 * We tell the Guest that it can't use the top 2 or 4 MB
-		 * of virtual addresses used by the Switcher.
+		 * We tell the Guest that it can't use the top virtual
+		 * addresses (used by the Switcher).
 		 */
-		|| put_user(RESERVE_MEM * 1024 * 1024,
-			    &cpu->lg->lguest_data->reserve_mem)) {
+	    || put_user(top, &cpu->lg->lguest_data->reserve_mem)) {
 		kill_guest(cpu, "bad guest page %p", cpu->lg->lguest_data);
 		return;
 	}
@@ -995,12 +1073,7 @@ void page_table_guest_data_init(struct lg_cpu *cpu)
 	 * "pgd_index(lg->kernel_address)".  This assumes it won't hit the
 	 * Switcher mappings, so check that now.
 	 */
-#ifdef CONFIG_X86_PAE
-	if (pgd_index(cpu->lg->kernel_address) == SWITCHER_PGD_INDEX &&
-		pmd_index(cpu->lg->kernel_address) == SWITCHER_PMD_INDEX)
-#else
-	if (pgd_index(cpu->lg->kernel_address) >= SWITCHER_PGD_INDEX)
-#endif
+	if (cpu->lg->kernel_address >= switcher_addr)
 		kill_guest(cpu, "bad kernel address %#lx",
 				 cpu->lg->kernel_address);
 }
@@ -1017,102 +1090,96 @@ void free_guest_pagetable(struct lguest *lg)
 		free_page((long)lg->pgdirs[i].pgdir);
 }
 
-/*H:480
- * (vi) Mapping the Switcher when the Guest is about to run.
- *
- * The Switcher and the two pages for this CPU need to be visible in the
- * Guest (and not the pages for other CPUs).  We have the appropriate PTE pages
- * for each CPU already set up, we just need to hook them in now we know which
- * Guest is about to run on this CPU.
+/*H:481
+ * This clears the Switcher mappings for cpu #i.
  */
-void map_switcher_in_guest(struct lg_cpu *cpu, struct lguest_pages *pages)
+static void remove_switcher_percpu_map(struct lg_cpu *cpu, unsigned int i)
 {
-	pte_t *switcher_pte_page = __this_cpu_read(switcher_pte_pages);
-	pte_t regs_pte;
+	unsigned long base = switcher_addr + PAGE_SIZE + i * PAGE_SIZE*2;
+	pte_t *pte;
 
-#ifdef CONFIG_X86_PAE
-	pmd_t switcher_pmd;
-	pmd_t *pmd_table;
-
-	switcher_pmd = pfn_pmd(__pa(switcher_pte_page) >> PAGE_SHIFT,
-			       PAGE_KERNEL_EXEC);
-
-	/* Figure out where the pmd page is, by reading the PGD, and converting
-	 * it to a virtual address. */
-	pmd_table = __va(pgd_pfn(cpu->lg->
-			pgdirs[cpu->cpu_pgd].pgdir[SWITCHER_PGD_INDEX])
-								<< PAGE_SHIFT);
-	/* Now write it into the shadow page table. */
-	set_pmd(&pmd_table[SWITCHER_PMD_INDEX], switcher_pmd);
-#else
-	pgd_t switcher_pgd;
+	/* Clear the mappings for both pages. */
+	pte = find_spte(cpu, base, false, 0, 0);
+	release_pte(*pte);
+	set_pte(pte, __pte(0));
 
-	/*
-	 * Make the last PGD entry for this Guest point to the Switcher's PTE
-	 * page for this CPU (with appropriate flags).
-	 */
-	switcher_pgd = __pgd(__pa(switcher_pte_page) | __PAGE_KERNEL_EXEC);
-
-	cpu->lg->pgdirs[cpu->cpu_pgd].pgdir[SWITCHER_PGD_INDEX] = switcher_pgd;
-
-#endif
-	/*
-	 * We also change the Switcher PTE page.  When we're running the Guest,
-	 * we want the Guest's "regs" page to appear where the first Switcher
-	 * page for this CPU is.  This is an optimization: when the Switcher
-	 * saves the Guest registers, it saves them into the first page of this
-	 * CPU's "struct lguest_pages": if we make sure the Guest's register
-	 * page is already mapped there, we don't have to copy them out
-	 * again.
-	 */
-	regs_pte = pfn_pte(__pa(cpu->regs_page) >> PAGE_SHIFT, PAGE_KERNEL);
-	set_pte(&switcher_pte_page[pte_index((unsigned long)pages)], regs_pte);
+	pte = find_spte(cpu, base + PAGE_SIZE, false, 0, 0);
+	release_pte(*pte);
+	set_pte(pte, __pte(0));
 }
-/*:*/
 
-static void free_switcher_pte_pages(void)
-{
-	unsigned int i;
-
-	for_each_possible_cpu(i)
-		free_page((long)switcher_pte_page(i));
-}
-
-/*H:520
- * Setting up the Switcher PTE page for given CPU is fairly easy, given
- * the CPU number and the "struct page"s for the Switcher code itself.
+/*H:480
+ * (vi) Mapping the Switcher when the Guest is about to run.
+ *
+ * The Switcher and the two pages for this CPU need to be visible in the Guest
+ * (and not the pages for other CPUs).
  *
- * Currently the Switcher is less than a page long, so "pages" is always 1.
+ * The pages for the pagetables have all been allocated before: we just need
+ * to make sure the actual PTEs are up-to-date for the CPU we're about to run
+ * on.
  */
-static __init void populate_switcher_pte_page(unsigned int cpu,
-					      struct page *switcher_page[],
-					      unsigned int pages)
+void map_switcher_in_guest(struct lg_cpu *cpu, struct lguest_pages *pages)
 {
-	unsigned int i;
-	pte_t *pte = switcher_pte_page(cpu);
+	unsigned long base;
+	struct page *percpu_switcher_page, *regs_page;
+	pte_t *pte;
+	struct pgdir *pgdir = &cpu->lg->pgdirs[cpu->cpu_pgd];
+
+	/* Switcher page should always be mapped by now! */
+	BUG_ON(!pgdir->switcher_mapped);
+
+	/* 
+	 * Remember that we have two pages for each Host CPU, so we can run a
+	 * Guest on each CPU without them interfering.  We need to make sure
+	 * those pages are mapped correctly in the Guest, but since we usually
+	 * run on the same CPU, we cache that, and only update the mappings
+	 * when we move.
+	 */
+	if (pgdir->last_host_cpu == raw_smp_processor_id())
+		return;
 
-	/* The first entries are easy: they map the Switcher code. */
-	for (i = 0; i < pages; i++) {
-		set_pte(&pte[i], mk_pte(switcher_page[i],
-				__pgprot(_PAGE_PRESENT|_PAGE_ACCESSED)));
+	/* -1 means unknown so we remove everything. */
+	if (pgdir->last_host_cpu == -1) {
+		unsigned int i;
+		for_each_possible_cpu(i)
+			remove_switcher_percpu_map(cpu, i);
+	} else {
+		/* We know exactly what CPU mapping to remove. */
+		remove_switcher_percpu_map(cpu, pgdir->last_host_cpu);
 	}
 
-	/* The only other thing we map is this CPU's pair of pages. */
-	i = pages + cpu*2;
-
-	/* First page (Guest registers) is writable from the Guest */
-	set_pte(&pte[i], pfn_pte(page_to_pfn(switcher_page[i]),
-			 __pgprot(_PAGE_PRESENT|_PAGE_ACCESSED|_PAGE_RW)));
+	/*
+	 * When we're running the Guest, we want the Guest's "regs" page to
+	 * appear where the first Switcher page for this CPU is.  This is an
+	 * optimization: when the Switcher saves the Guest registers, it saves
+	 * them into the first page of this CPU's "struct lguest_pages": if we
+	 * make sure the Guest's register page is already mapped there, we
+	 * don't have to copy them out again.
+	 */
+	/* Find the shadow PTE for this regs page. */
+	base = switcher_addr + PAGE_SIZE
+		+ raw_smp_processor_id() * sizeof(struct lguest_pages);
+	pte = find_spte(cpu, base, false, 0, 0);
+	regs_page = pfn_to_page(__pa(cpu->regs_page) >> PAGE_SHIFT);
+	get_page(regs_page);
+	set_pte(pte, mk_pte(regs_page, __pgprot(__PAGE_KERNEL & ~_PAGE_GLOBAL)));
 
 	/*
-	 * The second page contains the "struct lguest_ro_state", and is
-	 * read-only.
+	 * We map the second page of the struct lguest_pages read-only in
+	 * the Guest: the IDT, GDT and other things it's not supposed to
+	 * change.
 	 */
-	set_pte(&pte[i+1], pfn_pte(page_to_pfn(switcher_page[i+1]),
-			   __pgprot(_PAGE_PRESENT|_PAGE_ACCESSED)));
+	pte = find_spte(cpu, base + PAGE_SIZE, false, 0, 0);
+	percpu_switcher_page
+		= lg_switcher_pages[1 + raw_smp_processor_id()*2 + 1];
+	get_page(percpu_switcher_page);
+	set_pte(pte, mk_pte(percpu_switcher_page,
+			    __pgprot(__PAGE_KERNEL_RO & ~_PAGE_GLOBAL)));
+
+	pgdir->last_host_cpu = raw_smp_processor_id();
 }
 
-/*
+/*H:490
  * We've made it through the page table code.  Perhaps our tired brains are
  * still processing the details, or perhaps we're simply glad it's over.
  *
@@ -1124,29 +1191,3 @@ static __init void populate_switcher_pte_page(unsigned int cpu,
  *
  * There is just one file remaining in the Host.
  */
-
-/*H:510
- * At boot or module load time, init_pagetables() allocates and populates
- * the Switcher PTE page for each CPU.
- */
-__init int init_pagetables(struct page **switcher_page, unsigned int pages)
-{
-	unsigned int i;
-
-	for_each_possible_cpu(i) {
-		switcher_pte_page(i) = (pte_t *)get_zeroed_page(GFP_KERNEL);
-		if (!switcher_pte_page(i)) {
-			free_switcher_pte_pages();
-			return -ENOMEM;
-		}
-		populate_switcher_pte_page(i, switcher_page, pages);
-	}
-	return 0;
-}
-/*:*/
-
-/* Cleaning up simply involves freeing the PTE page for each CPU. */
-void free_pagetables(void)
-{
-	free_switcher_pte_pages();
-}
diff --git a/drivers/lguest/x86/core.c b/drivers/lguest/x86/core.c
index 4af12e1844d5..f0a3347b6441 100644
--- a/drivers/lguest/x86/core.c
+++ b/drivers/lguest/x86/core.c
@@ -59,14 +59,13 @@ static struct {
 /* Offset from where switcher.S was compiled to where we've copied it */
 static unsigned long switcher_offset(void)
 {
-	return SWITCHER_ADDR - (unsigned long)start_switcher_text;
+	return switcher_addr - (unsigned long)start_switcher_text;
 }
 
-/* This cpu's struct lguest_pages. */
+/* This cpu's struct lguest_pages (after the Switcher text page) */
 static struct lguest_pages *lguest_pages(unsigned int cpu)
 {
-	return &(((struct lguest_pages *)
-		  (SWITCHER_ADDR + SHARED_SWITCHER_PAGES*PAGE_SIZE))[cpu]);
+	return &(((struct lguest_pages *)(switcher_addr + PAGE_SIZE))[cpu]);
 }
 
 static DEFINE_PER_CPU(struct lg_cpu *, lg_last_cpu);
diff --git a/drivers/macintosh/smu.c b/drivers/macintosh/smu.c
index 9c6b96414862..b3b2d36c009e 100644
--- a/drivers/macintosh/smu.c
+++ b/drivers/macintosh/smu.c
@@ -120,11 +120,7 @@ static void smu_start_cmd(void)
 
 	DPRINTK("SMU: starting cmd %x, %d bytes data\n", cmd->cmd,
 		cmd->data_len);
-	DPRINTK("SMU: data buffer: %02x %02x %02x %02x %02x %02x %02x %02x\n",
-		((u8 *)cmd->data_buf)[0], ((u8 *)cmd->data_buf)[1],
-		((u8 *)cmd->data_buf)[2], ((u8 *)cmd->data_buf)[3],
-		((u8 *)cmd->data_buf)[4], ((u8 *)cmd->data_buf)[5],
-		((u8 *)cmd->data_buf)[6], ((u8 *)cmd->data_buf)[7]);
+	DPRINTK("SMU: data buffer: %8ph\n", cmd->data_buf);
 
 	/* Fill the SMU command buffer */
 	smu->cmd_buf->cmd = cmd->cmd;
diff --git a/drivers/macintosh/via-pmu.c b/drivers/macintosh/via-pmu.c
index 22b8ce4191cc..283e1b53c6be 100644
--- a/drivers/macintosh/via-pmu.c
+++ b/drivers/macintosh/via-pmu.c
@@ -750,8 +750,9 @@ done_battery_state_smart(struct adb_request* req)
 				voltage = (req->reply[8] << 8) | req->reply[9];
 				break;
 			default:
-				printk(KERN_WARNING "pmu.c : unrecognized battery info, len: %d, %02x %02x %02x %02x\n",
-					req->reply_len, req->reply[0], req->reply[1], req->reply[2], req->reply[3]);
+				pr_warn("pmu.c: unrecognized battery info, "
+					"len: %d, %4ph\n", req->reply_len,
+							   req->reply);
 				break;
 		}
 	}
@@ -869,7 +870,7 @@ static int pmu_battery_proc_show(struct seq_file *m, void *v)
 
 static int pmu_battery_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, pmu_battery_proc_show, PDE(inode)->data);
+	return single_open(file, pmu_battery_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations pmu_battery_proc_fops = {
diff --git a/drivers/md/Kconfig b/drivers/md/Kconfig
index 4d8d90b4fe78..3bfc8f1da9fe 100644
--- a/drivers/md/Kconfig
+++ b/drivers/md/Kconfig
@@ -174,6 +174,8 @@ config MD_FAULTY
 
 	  In unsure, say N.
 
+source "drivers/md/bcache/Kconfig"
+
 config BLK_DEV_DM
 	tristate "Device mapper support"
 	---help---
diff --git a/drivers/md/Makefile b/drivers/md/Makefile
index 7ceeaefc0e95..1439fd4ad9b1 100644
--- a/drivers/md/Makefile
+++ b/drivers/md/Makefile
@@ -29,6 +29,7 @@ obj-$(CONFIG_MD_RAID10)		+= raid10.o
 obj-$(CONFIG_MD_RAID456)	+= raid456.o
 obj-$(CONFIG_MD_MULTIPATH)	+= multipath.o
 obj-$(CONFIG_MD_FAULTY)		+= faulty.o
+obj-$(CONFIG_BCACHE)		+= bcache/
 obj-$(CONFIG_BLK_DEV_MD)	+= md-mod.o
 obj-$(CONFIG_BLK_DEV_DM)	+= dm-mod.o
 obj-$(CONFIG_DM_BUFIO)		+= dm-bufio.o
diff --git a/drivers/md/bcache/Kconfig b/drivers/md/bcache/Kconfig
new file mode 100644
index 000000000000..05c220d05e23
--- /dev/null
+++ b/drivers/md/bcache/Kconfig
@@ -0,0 +1,42 @@
+
+config BCACHE
+	tristate "Block device as cache"
+	select CLOSURES
+	---help---
+	Allows a block device to be used as cache for other devices; uses
+	a btree for indexing and the layout is optimized for SSDs.
+
+	See Documentation/bcache.txt for details.
+
+config BCACHE_DEBUG
+	bool "Bcache debugging"
+	depends on BCACHE
+	---help---
+	Don't select this option unless you're a developer
+
+	Enables extra debugging tools (primarily a fuzz tester)
+
+config BCACHE_EDEBUG
+	bool "Extended runtime checks"
+	depends on BCACHE
+	---help---
+	Don't select this option unless you're a developer
+
+	Enables extra runtime checks which significantly affect performance
+
+config BCACHE_CLOSURES_DEBUG
+	bool "Debug closures"
+	depends on BCACHE
+	select DEBUG_FS
+	---help---
+	Keeps all active closures in a linked list and provides a debugfs
+	interface to list them, which makes it possible to see asynchronous
+	operations that get stuck.
+
+# cgroup code needs to be updated:
+#
+#config CGROUP_BCACHE
+#	bool "Cgroup controls for bcache"
+#	depends on BCACHE && BLK_CGROUP
+#	---help---
+#	TODO
diff --git a/drivers/md/bcache/Makefile b/drivers/md/bcache/Makefile
new file mode 100644
index 000000000000..0e9c82523be6
--- /dev/null
+++ b/drivers/md/bcache/Makefile
@@ -0,0 +1,7 @@
+
+obj-$(CONFIG_BCACHE)	+= bcache.o
+
+bcache-y		:= alloc.o btree.o bset.o io.o journal.o writeback.o\
+	movinggc.o request.o super.o sysfs.o debug.o util.o trace.o stats.o closure.o
+
+CFLAGS_request.o	+= -Iblock
diff --git a/drivers/md/bcache/alloc.c b/drivers/md/bcache/alloc.c
new file mode 100644
index 000000000000..048f2947e08b
--- /dev/null
+++ b/drivers/md/bcache/alloc.c
@@ -0,0 +1,599 @@
+/*
+ * Primary bucket allocation code
+ *
+ * Copyright 2012 Google, Inc.
+ *
+ * Allocation in bcache is done in terms of buckets:
+ *
+ * Each bucket has associated an 8 bit gen; this gen corresponds to the gen in
+ * btree pointers - they must match for the pointer to be considered valid.
+ *
+ * Thus (assuming a bucket has no dirty data or metadata in it) we can reuse a
+ * bucket simply by incrementing its gen.
+ *
+ * The gens (along with the priorities; it's really the gens are important but
+ * the code is named as if it's the priorities) are written in an arbitrary list
+ * of buckets on disk, with a pointer to them in the journal header.
+ *
+ * When we invalidate a bucket, we have to write its new gen to disk and wait
+ * for that write to complete before we use it - otherwise after a crash we
+ * could have pointers that appeared to be good but pointed to data that had
+ * been overwritten.
+ *
+ * Since the gens and priorities are all stored contiguously on disk, we can
+ * batch this up: We fill up the free_inc list with freshly invalidated buckets,
+ * call prio_write(), and when prio_write() finishes we pull buckets off the
+ * free_inc list and optionally discard them.
+ *
+ * free_inc isn't the only freelist - if it was, we'd often to sleep while
+ * priorities and gens were being written before we could allocate. c->free is a
+ * smaller freelist, and buckets on that list are always ready to be used.
+ *
+ * If we've got discards enabled, that happens when a bucket moves from the
+ * free_inc list to the free list.
+ *
+ * There is another freelist, because sometimes we have buckets that we know
+ * have nothing pointing into them - these we can reuse without waiting for
+ * priorities to be rewritten. These come from freed btree nodes and buckets
+ * that garbage collection discovered no longer had valid keys pointing into
+ * them (because they were overwritten). That's the unused list - buckets on the
+ * unused list move to the free list, optionally being discarded in the process.
+ *
+ * It's also important to ensure that gens don't wrap around - with respect to
+ * either the oldest gen in the btree or the gen on disk. This is quite
+ * difficult to do in practice, but we explicitly guard against it anyways - if
+ * a bucket is in danger of wrapping around we simply skip invalidating it that
+ * time around, and we garbage collect or rewrite the priorities sooner than we
+ * would have otherwise.
+ *
+ * bch_bucket_alloc() allocates a single bucket from a specific cache.
+ *
+ * bch_bucket_alloc_set() allocates one or more buckets from different caches
+ * out of a cache set.
+ *
+ * free_some_buckets() drives all the processes described above. It's called
+ * from bch_bucket_alloc() and a few other places that need to make sure free
+ * buckets are ready.
+ *
+ * invalidate_buckets_(lru|fifo)() find buckets that are available to be
+ * invalidated, and then invalidate them and stick them on the free_inc list -
+ * in either lru or fifo order.
+ */
+
+#include "bcache.h"
+#include "btree.h"
+
+#include <linux/random.h>
+
+#define MAX_IN_FLIGHT_DISCARDS		8U
+
+/* Bucket heap / gen */
+
+uint8_t bch_inc_gen(struct cache *ca, struct bucket *b)
+{
+	uint8_t ret = ++b->gen;
+
+	ca->set->need_gc = max(ca->set->need_gc, bucket_gc_gen(b));
+	WARN_ON_ONCE(ca->set->need_gc > BUCKET_GC_GEN_MAX);
+
+	if (CACHE_SYNC(&ca->set->sb)) {
+		ca->need_save_prio = max(ca->need_save_prio,
+					 bucket_disk_gen(b));
+		WARN_ON_ONCE(ca->need_save_prio > BUCKET_DISK_GEN_MAX);
+	}
+
+	return ret;
+}
+
+void bch_rescale_priorities(struct cache_set *c, int sectors)
+{
+	struct cache *ca;
+	struct bucket *b;
+	unsigned next = c->nbuckets * c->sb.bucket_size / 1024;
+	unsigned i;
+	int r;
+
+	atomic_sub(sectors, &c->rescale);
+
+	do {
+		r = atomic_read(&c->rescale);
+
+		if (r >= 0)
+			return;
+	} while (atomic_cmpxchg(&c->rescale, r, r + next) != r);
+
+	mutex_lock(&c->bucket_lock);
+
+	c->min_prio = USHRT_MAX;
+
+	for_each_cache(ca, c, i)
+		for_each_bucket(b, ca)
+			if (b->prio &&
+			    b->prio != BTREE_PRIO &&
+			    !atomic_read(&b->pin)) {
+				b->prio--;
+				c->min_prio = min(c->min_prio, b->prio);
+			}
+
+	mutex_unlock(&c->bucket_lock);
+}
+
+/* Discard/TRIM */
+
+struct discard {
+	struct list_head	list;
+	struct work_struct	work;
+	struct cache		*ca;
+	long			bucket;
+
+	struct bio		bio;
+	struct bio_vec		bv;
+};
+
+static void discard_finish(struct work_struct *w)
+{
+	struct discard *d = container_of(w, struct discard, work);
+	struct cache *ca = d->ca;
+	char buf[BDEVNAME_SIZE];
+
+	if (!test_bit(BIO_UPTODATE, &d->bio.bi_flags)) {
+		pr_notice("discard error on %s, disabling",
+			 bdevname(ca->bdev, buf));
+		d->ca->discard = 0;
+	}
+
+	mutex_lock(&ca->set->bucket_lock);
+
+	fifo_push(&ca->free, d->bucket);
+	list_add(&d->list, &ca->discards);
+	atomic_dec(&ca->discards_in_flight);
+
+	mutex_unlock(&ca->set->bucket_lock);
+
+	closure_wake_up(&ca->set->bucket_wait);
+	wake_up(&ca->set->alloc_wait);
+
+	closure_put(&ca->set->cl);
+}
+
+static void discard_endio(struct bio *bio, int error)
+{
+	struct discard *d = container_of(bio, struct discard, bio);
+	schedule_work(&d->work);
+}
+
+static void do_discard(struct cache *ca, long bucket)
+{
+	struct discard *d = list_first_entry(&ca->discards,
+					     struct discard, list);
+
+	list_del(&d->list);
+	d->bucket = bucket;
+
+	atomic_inc(&ca->discards_in_flight);
+	closure_get(&ca->set->cl);
+
+	bio_init(&d->bio);
+
+	d->bio.bi_sector	= bucket_to_sector(ca->set, d->bucket);
+	d->bio.bi_bdev		= ca->bdev;
+	d->bio.bi_rw		= REQ_WRITE|REQ_DISCARD;
+	d->bio.bi_max_vecs	= 1;
+	d->bio.bi_io_vec	= d->bio.bi_inline_vecs;
+	d->bio.bi_size		= bucket_bytes(ca);
+	d->bio.bi_end_io	= discard_endio;
+	bio_set_prio(&d->bio, IOPRIO_PRIO_VALUE(IOPRIO_CLASS_IDLE, 0));
+
+	submit_bio(0, &d->bio);
+}
+
+/* Allocation */
+
+static inline bool can_inc_bucket_gen(struct bucket *b)
+{
+	return bucket_gc_gen(b) < BUCKET_GC_GEN_MAX &&
+		bucket_disk_gen(b) < BUCKET_DISK_GEN_MAX;
+}
+
+bool bch_bucket_add_unused(struct cache *ca, struct bucket *b)
+{
+	BUG_ON(GC_MARK(b) || GC_SECTORS_USED(b));
+
+	if (fifo_used(&ca->free) > ca->watermark[WATERMARK_MOVINGGC] &&
+	    CACHE_REPLACEMENT(&ca->sb) == CACHE_REPLACEMENT_FIFO)
+		return false;
+
+	b->prio = 0;
+
+	if (can_inc_bucket_gen(b) &&
+	    fifo_push(&ca->unused, b - ca->buckets)) {
+		atomic_inc(&b->pin);
+		return true;
+	}
+
+	return false;
+}
+
+static bool can_invalidate_bucket(struct cache *ca, struct bucket *b)
+{
+	return GC_MARK(b) == GC_MARK_RECLAIMABLE &&
+		!atomic_read(&b->pin) &&
+		can_inc_bucket_gen(b);
+}
+
+static void invalidate_one_bucket(struct cache *ca, struct bucket *b)
+{
+	bch_inc_gen(ca, b);
+	b->prio = INITIAL_PRIO;
+	atomic_inc(&b->pin);
+	fifo_push(&ca->free_inc, b - ca->buckets);
+}
+
+#define bucket_prio(b)				\
+	(((unsigned) (b->prio - ca->set->min_prio)) * GC_SECTORS_USED(b))
+
+#define bucket_max_cmp(l, r)	(bucket_prio(l) < bucket_prio(r))
+#define bucket_min_cmp(l, r)	(bucket_prio(l) > bucket_prio(r))
+
+static void invalidate_buckets_lru(struct cache *ca)
+{
+	struct bucket *b;
+	ssize_t i;
+
+	ca->heap.used = 0;
+
+	for_each_bucket(b, ca) {
+		/*
+		 * If we fill up the unused list, if we then return before
+		 * adding anything to the free_inc list we'll skip writing
+		 * prios/gens and just go back to allocating from the unused
+		 * list:
+		 */
+		if (fifo_full(&ca->unused))
+			return;
+
+		if (!can_invalidate_bucket(ca, b))
+			continue;
+
+		if (!GC_SECTORS_USED(b) &&
+		    bch_bucket_add_unused(ca, b))
+			continue;
+
+		if (!heap_full(&ca->heap))
+			heap_add(&ca->heap, b, bucket_max_cmp);
+		else if (bucket_max_cmp(b, heap_peek(&ca->heap))) {
+			ca->heap.data[0] = b;
+			heap_sift(&ca->heap, 0, bucket_max_cmp);
+		}
+	}
+
+	for (i = ca->heap.used / 2 - 1; i >= 0; --i)
+		heap_sift(&ca->heap, i, bucket_min_cmp);
+
+	while (!fifo_full(&ca->free_inc)) {
+		if (!heap_pop(&ca->heap, b, bucket_min_cmp)) {
+			/*
+			 * We don't want to be calling invalidate_buckets()
+			 * multiple times when it can't do anything
+			 */
+			ca->invalidate_needs_gc = 1;
+			bch_queue_gc(ca->set);
+			return;
+		}
+
+		invalidate_one_bucket(ca, b);
+	}
+}
+
+static void invalidate_buckets_fifo(struct cache *ca)
+{
+	struct bucket *b;
+	size_t checked = 0;
+
+	while (!fifo_full(&ca->free_inc)) {
+		if (ca->fifo_last_bucket <  ca->sb.first_bucket ||
+		    ca->fifo_last_bucket >= ca->sb.nbuckets)
+			ca->fifo_last_bucket = ca->sb.first_bucket;
+
+		b = ca->buckets + ca->fifo_last_bucket++;
+
+		if (can_invalidate_bucket(ca, b))
+			invalidate_one_bucket(ca, b);
+
+		if (++checked >= ca->sb.nbuckets) {
+			ca->invalidate_needs_gc = 1;
+			bch_queue_gc(ca->set);
+			return;
+		}
+	}
+}
+
+static void invalidate_buckets_random(struct cache *ca)
+{
+	struct bucket *b;
+	size_t checked = 0;
+
+	while (!fifo_full(&ca->free_inc)) {
+		size_t n;
+		get_random_bytes(&n, sizeof(n));
+
+		n %= (size_t) (ca->sb.nbuckets - ca->sb.first_bucket);
+		n += ca->sb.first_bucket;
+
+		b = ca->buckets + n;
+
+		if (can_invalidate_bucket(ca, b))
+			invalidate_one_bucket(ca, b);
+
+		if (++checked >= ca->sb.nbuckets / 2) {
+			ca->invalidate_needs_gc = 1;
+			bch_queue_gc(ca->set);
+			return;
+		}
+	}
+}
+
+static void invalidate_buckets(struct cache *ca)
+{
+	if (ca->invalidate_needs_gc)
+		return;
+
+	switch (CACHE_REPLACEMENT(&ca->sb)) {
+	case CACHE_REPLACEMENT_LRU:
+		invalidate_buckets_lru(ca);
+		break;
+	case CACHE_REPLACEMENT_FIFO:
+		invalidate_buckets_fifo(ca);
+		break;
+	case CACHE_REPLACEMENT_RANDOM:
+		invalidate_buckets_random(ca);
+		break;
+	}
+
+	pr_debug("free %zu/%zu free_inc %zu/%zu unused %zu/%zu",
+		 fifo_used(&ca->free), ca->free.size,
+		 fifo_used(&ca->free_inc), ca->free_inc.size,
+		 fifo_used(&ca->unused), ca->unused.size);
+}
+
+#define allocator_wait(ca, cond)					\
+do {									\
+	DEFINE_WAIT(__wait);						\
+									\
+	while (1) {							\
+		prepare_to_wait(&ca->set->alloc_wait,			\
+				&__wait, TASK_INTERRUPTIBLE);		\
+		if (cond)						\
+			break;						\
+									\
+		mutex_unlock(&(ca)->set->bucket_lock);			\
+		if (test_bit(CACHE_SET_STOPPING_2, &ca->set->flags)) {	\
+			finish_wait(&ca->set->alloc_wait, &__wait);	\
+			closure_return(cl);				\
+		}							\
+									\
+		schedule();						\
+		mutex_lock(&(ca)->set->bucket_lock);			\
+	}								\
+									\
+	finish_wait(&ca->set->alloc_wait, &__wait);			\
+} while (0)
+
+void bch_allocator_thread(struct closure *cl)
+{
+	struct cache *ca = container_of(cl, struct cache, alloc);
+
+	mutex_lock(&ca->set->bucket_lock);
+
+	while (1) {
+		/*
+		 * First, we pull buckets off of the unused and free_inc lists,
+		 * possibly issue discards to them, then we add the bucket to
+		 * the free list:
+		 */
+		while (1) {
+			long bucket;
+
+			if ((!atomic_read(&ca->set->prio_blocked) ||
+			     !CACHE_SYNC(&ca->set->sb)) &&
+			    !fifo_empty(&ca->unused))
+				fifo_pop(&ca->unused, bucket);
+			else if (!fifo_empty(&ca->free_inc))
+				fifo_pop(&ca->free_inc, bucket);
+			else
+				break;
+
+			allocator_wait(ca, (int) fifo_free(&ca->free) >
+				       atomic_read(&ca->discards_in_flight));
+
+			if (ca->discard) {
+				allocator_wait(ca, !list_empty(&ca->discards));
+				do_discard(ca, bucket);
+			} else {
+				fifo_push(&ca->free, bucket);
+				closure_wake_up(&ca->set->bucket_wait);
+			}
+		}
+
+		/*
+		 * We've run out of free buckets, we need to find some buckets
+		 * we can invalidate. First, invalidate them in memory and add
+		 * them to the free_inc list:
+		 */
+
+		allocator_wait(ca, ca->set->gc_mark_valid &&
+			       (ca->need_save_prio > 64 ||
+				!ca->invalidate_needs_gc));
+		invalidate_buckets(ca);
+
+		/*
+		 * Now, we write their new gens to disk so we can start writing
+		 * new stuff to them:
+		 */
+		allocator_wait(ca, !atomic_read(&ca->set->prio_blocked));
+		if (CACHE_SYNC(&ca->set->sb) &&
+		    (!fifo_empty(&ca->free_inc) ||
+		     ca->need_save_prio > 64))
+			bch_prio_write(ca);
+	}
+}
+
+long bch_bucket_alloc(struct cache *ca, unsigned watermark, struct closure *cl)
+{
+	long r = -1;
+again:
+	wake_up(&ca->set->alloc_wait);
+
+	if (fifo_used(&ca->free) > ca->watermark[watermark] &&
+	    fifo_pop(&ca->free, r)) {
+		struct bucket *b = ca->buckets + r;
+#ifdef CONFIG_BCACHE_EDEBUG
+		size_t iter;
+		long i;
+
+		for (iter = 0; iter < prio_buckets(ca) * 2; iter++)
+			BUG_ON(ca->prio_buckets[iter] == (uint64_t) r);
+
+		fifo_for_each(i, &ca->free, iter)
+			BUG_ON(i == r);
+		fifo_for_each(i, &ca->free_inc, iter)
+			BUG_ON(i == r);
+		fifo_for_each(i, &ca->unused, iter)
+			BUG_ON(i == r);
+#endif
+		BUG_ON(atomic_read(&b->pin) != 1);
+
+		SET_GC_SECTORS_USED(b, ca->sb.bucket_size);
+
+		if (watermark <= WATERMARK_METADATA) {
+			SET_GC_MARK(b, GC_MARK_METADATA);
+			b->prio = BTREE_PRIO;
+		} else {
+			SET_GC_MARK(b, GC_MARK_RECLAIMABLE);
+			b->prio = INITIAL_PRIO;
+		}
+
+		return r;
+	}
+
+	pr_debug("alloc failure: blocked %i free %zu free_inc %zu unused %zu",
+		 atomic_read(&ca->set->prio_blocked), fifo_used(&ca->free),
+		 fifo_used(&ca->free_inc), fifo_used(&ca->unused));
+
+	if (cl) {
+		closure_wait(&ca->set->bucket_wait, cl);
+
+		if (closure_blocking(cl)) {
+			mutex_unlock(&ca->set->bucket_lock);
+			closure_sync(cl);
+			mutex_lock(&ca->set->bucket_lock);
+			goto again;
+		}
+	}
+
+	return -1;
+}
+
+void bch_bucket_free(struct cache_set *c, struct bkey *k)
+{
+	unsigned i;
+
+	for (i = 0; i < KEY_PTRS(k); i++) {
+		struct bucket *b = PTR_BUCKET(c, k, i);
+
+		SET_GC_MARK(b, GC_MARK_RECLAIMABLE);
+		SET_GC_SECTORS_USED(b, 0);
+		bch_bucket_add_unused(PTR_CACHE(c, k, i), b);
+	}
+}
+
+int __bch_bucket_alloc_set(struct cache_set *c, unsigned watermark,
+			   struct bkey *k, int n, struct closure *cl)
+{
+	int i;
+
+	lockdep_assert_held(&c->bucket_lock);
+	BUG_ON(!n || n > c->caches_loaded || n > 8);
+
+	bkey_init(k);
+
+	/* sort by free space/prio of oldest data in caches */
+
+	for (i = 0; i < n; i++) {
+		struct cache *ca = c->cache_by_alloc[i];
+		long b = bch_bucket_alloc(ca, watermark, cl);
+
+		if (b == -1)
+			goto err;
+
+		k->ptr[i] = PTR(ca->buckets[b].gen,
+				bucket_to_sector(c, b),
+				ca->sb.nr_this_dev);
+
+		SET_KEY_PTRS(k, i + 1);
+	}
+
+	return 0;
+err:
+	bch_bucket_free(c, k);
+	__bkey_put(c, k);
+	return -1;
+}
+
+int bch_bucket_alloc_set(struct cache_set *c, unsigned watermark,
+			 struct bkey *k, int n, struct closure *cl)
+{
+	int ret;
+	mutex_lock(&c->bucket_lock);
+	ret = __bch_bucket_alloc_set(c, watermark, k, n, cl);
+	mutex_unlock(&c->bucket_lock);
+	return ret;
+}
+
+/* Init */
+
+void bch_cache_allocator_exit(struct cache *ca)
+{
+	struct discard *d;
+
+	while (!list_empty(&ca->discards)) {
+		d = list_first_entry(&ca->discards, struct discard, list);
+		cancel_work_sync(&d->work);
+		list_del(&d->list);
+		kfree(d);
+	}
+}
+
+int bch_cache_allocator_init(struct cache *ca)
+{
+	unsigned i;
+
+	/*
+	 * Reserve:
+	 * Prio/gen writes first
+	 * Then 8 for btree allocations
+	 * Then half for the moving garbage collector
+	 */
+
+	ca->watermark[WATERMARK_PRIO] = 0;
+
+	ca->watermark[WATERMARK_METADATA] = prio_buckets(ca);
+
+	ca->watermark[WATERMARK_MOVINGGC] = 8 +
+		ca->watermark[WATERMARK_METADATA];
+
+	ca->watermark[WATERMARK_NONE] = ca->free.size / 2 +
+		ca->watermark[WATERMARK_MOVINGGC];
+
+	for (i = 0; i < MAX_IN_FLIGHT_DISCARDS; i++) {
+		struct discard *d = kzalloc(sizeof(*d), GFP_KERNEL);
+		if (!d)
+			return -ENOMEM;
+
+		d->ca = ca;
+		INIT_WORK(&d->work, discard_finish);
+		list_add(&d->list, &ca->discards);
+	}
+
+	return 0;
+}
diff --git a/drivers/md/bcache/bcache.h b/drivers/md/bcache/bcache.h
new file mode 100644
index 000000000000..340146d7c17f
--- /dev/null
+++ b/drivers/md/bcache/bcache.h
@@ -0,0 +1,1259 @@
+#ifndef _BCACHE_H
+#define _BCACHE_H
+
+/*
+ * SOME HIGH LEVEL CODE DOCUMENTATION:
+ *
+ * Bcache mostly works with cache sets, cache devices, and backing devices.
+ *
+ * Support for multiple cache devices hasn't quite been finished off yet, but
+ * it's about 95% plumbed through. A cache set and its cache devices is sort of
+ * like a md raid array and its component devices. Most of the code doesn't care
+ * about individual cache devices, the main abstraction is the cache set.
+ *
+ * Multiple cache devices is intended to give us the ability to mirror dirty
+ * cached data and metadata, without mirroring clean cached data.
+ *
+ * Backing devices are different, in that they have a lifetime independent of a
+ * cache set. When you register a newly formatted backing device it'll come up
+ * in passthrough mode, and then you can attach and detach a backing device from
+ * a cache set at runtime - while it's mounted and in use. Detaching implicitly
+ * invalidates any cached data for that backing device.
+ *
+ * A cache set can have multiple (many) backing devices attached to it.
+ *
+ * There's also flash only volumes - this is the reason for the distinction
+ * between struct cached_dev and struct bcache_device. A flash only volume
+ * works much like a bcache device that has a backing device, except the
+ * "cached" data is always dirty. The end result is that we get thin
+ * provisioning with very little additional code.
+ *
+ * Flash only volumes work but they're not production ready because the moving
+ * garbage collector needs more work. More on that later.
+ *
+ * BUCKETS/ALLOCATION:
+ *
+ * Bcache is primarily designed for caching, which means that in normal
+ * operation all of our available space will be allocated. Thus, we need an
+ * efficient way of deleting things from the cache so we can write new things to
+ * it.
+ *
+ * To do this, we first divide the cache device up into buckets. A bucket is the
+ * unit of allocation; they're typically around 1 mb - anywhere from 128k to 2M+
+ * works efficiently.
+ *
+ * Each bucket has a 16 bit priority, and an 8 bit generation associated with
+ * it. The gens and priorities for all the buckets are stored contiguously and
+ * packed on disk (in a linked list of buckets - aside from the superblock, all
+ * of bcache's metadata is stored in buckets).
+ *
+ * The priority is used to implement an LRU. We reset a bucket's priority when
+ * we allocate it or on cache it, and every so often we decrement the priority
+ * of each bucket. It could be used to implement something more sophisticated,
+ * if anyone ever gets around to it.
+ *
+ * The generation is used for invalidating buckets. Each pointer also has an 8
+ * bit generation embedded in it; for a pointer to be considered valid, its gen
+ * must match the gen of the bucket it points into.  Thus, to reuse a bucket all
+ * we have to do is increment its gen (and write its new gen to disk; we batch
+ * this up).
+ *
+ * Bcache is entirely COW - we never write twice to a bucket, even buckets that
+ * contain metadata (including btree nodes).
+ *
+ * THE BTREE:
+ *
+ * Bcache is in large part design around the btree.
+ *
+ * At a high level, the btree is just an index of key -> ptr tuples.
+ *
+ * Keys represent extents, and thus have a size field. Keys also have a variable
+ * number of pointers attached to them (potentially zero, which is handy for
+ * invalidating the cache).
+ *
+ * The key itself is an inode:offset pair. The inode number corresponds to a
+ * backing device or a flash only volume. The offset is the ending offset of the
+ * extent within the inode - not the starting offset; this makes lookups
+ * slightly more convenient.
+ *
+ * Pointers contain the cache device id, the offset on that device, and an 8 bit
+ * generation number. More on the gen later.
+ *
+ * Index lookups are not fully abstracted - cache lookups in particular are
+ * still somewhat mixed in with the btree code, but things are headed in that
+ * direction.
+ *
+ * Updates are fairly well abstracted, though. There are two different ways of
+ * updating the btree; insert and replace.
+ *
+ * BTREE_INSERT will just take a list of keys and insert them into the btree -
+ * overwriting (possibly only partially) any extents they overlap with. This is
+ * used to update the index after a write.
+ *
+ * BTREE_REPLACE is really cmpxchg(); it inserts a key into the btree iff it is
+ * overwriting a key that matches another given key. This is used for inserting
+ * data into the cache after a cache miss, and for background writeback, and for
+ * the moving garbage collector.
+ *
+ * There is no "delete" operation; deleting things from the index is
+ * accomplished by either by invalidating pointers (by incrementing a bucket's
+ * gen) or by inserting a key with 0 pointers - which will overwrite anything
+ * previously present at that location in the index.
+ *
+ * This means that there are always stale/invalid keys in the btree. They're
+ * filtered out by the code that iterates through a btree node, and removed when
+ * a btree node is rewritten.
+ *
+ * BTREE NODES:
+ *
+ * Our unit of allocation is a bucket, and we we can't arbitrarily allocate and
+ * free smaller than a bucket - so, that's how big our btree nodes are.
+ *
+ * (If buckets are really big we'll only use part of the bucket for a btree node
+ * - no less than 1/4th - but a bucket still contains no more than a single
+ * btree node. I'd actually like to change this, but for now we rely on the
+ * bucket's gen for deleting btree nodes when we rewrite/split a node.)
+ *
+ * Anyways, btree nodes are big - big enough to be inefficient with a textbook
+ * btree implementation.
+ *
+ * The way this is solved is that btree nodes are internally log structured; we
+ * can append new keys to an existing btree node without rewriting it. This
+ * means each set of keys we write is sorted, but the node is not.
+ *
+ * We maintain this log structure in memory - keeping 1Mb of keys sorted would
+ * be expensive, and we have to distinguish between the keys we have written and
+ * the keys we haven't. So to do a lookup in a btree node, we have to search
+ * each sorted set. But we do merge written sets together lazily, so the cost of
+ * these extra searches is quite low (normally most of the keys in a btree node
+ * will be in one big set, and then there'll be one or two sets that are much
+ * smaller).
+ *
+ * This log structure makes bcache's btree more of a hybrid between a
+ * conventional btree and a compacting data structure, with some of the
+ * advantages of both.
+ *
+ * GARBAGE COLLECTION:
+ *
+ * We can't just invalidate any bucket - it might contain dirty data or
+ * metadata. If it once contained dirty data, other writes might overwrite it
+ * later, leaving no valid pointers into that bucket in the index.
+ *
+ * Thus, the primary purpose of garbage collection is to find buckets to reuse.
+ * It also counts how much valid data it each bucket currently contains, so that
+ * allocation can reuse buckets sooner when they've been mostly overwritten.
+ *
+ * It also does some things that are really internal to the btree
+ * implementation. If a btree node contains pointers that are stale by more than
+ * some threshold, it rewrites the btree node to avoid the bucket's generation
+ * wrapping around. It also merges adjacent btree nodes if they're empty enough.
+ *
+ * THE JOURNAL:
+ *
+ * Bcache's journal is not necessary for consistency; we always strictly
+ * order metadata writes so that the btree and everything else is consistent on
+ * disk in the event of an unclean shutdown, and in fact bcache had writeback
+ * caching (with recovery from unclean shutdown) before journalling was
+ * implemented.
+ *
+ * Rather, the journal is purely a performance optimization; we can't complete a
+ * write until we've updated the index on disk, otherwise the cache would be
+ * inconsistent in the event of an unclean shutdown. This means that without the
+ * journal, on random write workloads we constantly have to update all the leaf
+ * nodes in the btree, and those writes will be mostly empty (appending at most
+ * a few keys each) - highly inefficient in terms of amount of metadata writes,
+ * and it puts more strain on the various btree resorting/compacting code.
+ *
+ * The journal is just a log of keys we've inserted; on startup we just reinsert
+ * all the keys in the open journal entries. That means that when we're updating
+ * a node in the btree, we can wait until a 4k block of keys fills up before
+ * writing them out.
+ *
+ * For simplicity, we only journal updates to leaf nodes; updates to parent
+ * nodes are rare enough (since our leaf nodes are huge) that it wasn't worth
+ * the complexity to deal with journalling them (in particular, journal replay)
+ * - updates to non leaf nodes just happen synchronously (see btree_split()).
+ */
+
+#define pr_fmt(fmt) "bcache: %s() " fmt "\n", __func__
+
+#include <linux/bio.h>
+#include <linux/blktrace_api.h>
+#include <linux/kobject.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/rbtree.h>
+#include <linux/rwsem.h>
+#include <linux/types.h>
+#include <linux/workqueue.h>
+
+#include "util.h"
+#include "closure.h"
+
+struct bucket {
+	atomic_t	pin;
+	uint16_t	prio;
+	uint8_t		gen;
+	uint8_t		disk_gen;
+	uint8_t		last_gc; /* Most out of date gen in the btree */
+	uint8_t		gc_gen;
+	uint16_t	gc_mark;
+};
+
+/*
+ * I'd use bitfields for these, but I don't trust the compiler not to screw me
+ * as multiple threads touch struct bucket without locking
+ */
+
+BITMASK(GC_MARK,	 struct bucket, gc_mark, 0, 2);
+#define GC_MARK_RECLAIMABLE	0
+#define GC_MARK_DIRTY		1
+#define GC_MARK_METADATA	2
+BITMASK(GC_SECTORS_USED, struct bucket, gc_mark, 2, 14);
+
+struct bkey {
+	uint64_t	high;
+	uint64_t	low;
+	uint64_t	ptr[];
+};
+
+/* Enough for a key with 6 pointers */
+#define BKEY_PAD		8
+
+#define BKEY_PADDED(key)					\
+	union { struct bkey key; uint64_t key ## _pad[BKEY_PAD]; }
+
+/* Version 0: Cache device
+ * Version 1: Backing device
+ * Version 2: Seed pointer into btree node checksum
+ * Version 3: Cache device with new UUID format
+ * Version 4: Backing device with data offset
+ */
+#define BCACHE_SB_VERSION_CDEV			0
+#define BCACHE_SB_VERSION_BDEV			1
+#define BCACHE_SB_VERSION_CDEV_WITH_UUID	3
+#define BCACHE_SB_VERSION_BDEV_WITH_OFFSET	4
+#define BCACHE_SB_MAX_VERSION			4
+
+#define SB_SECTOR		8
+#define SB_SIZE			4096
+#define SB_LABEL_SIZE		32
+#define SB_JOURNAL_BUCKETS	256U
+/* SB_JOURNAL_BUCKETS must be divisible by BITS_PER_LONG */
+#define MAX_CACHES_PER_SET	8
+
+#define BDEV_DATA_START_DEFAULT	16	/* sectors */
+
+struct cache_sb {
+	uint64_t		csum;
+	uint64_t		offset;	/* sector where this sb was written */
+	uint64_t		version;
+
+	uint8_t			magic[16];
+
+	uint8_t			uuid[16];
+	union {
+		uint8_t		set_uuid[16];
+		uint64_t	set_magic;
+	};
+	uint8_t			label[SB_LABEL_SIZE];
+
+	uint64_t		flags;
+	uint64_t		seq;
+	uint64_t		pad[8];
+
+	union {
+	struct {
+		/* Cache devices */
+		uint64_t	nbuckets;	/* device size */
+
+		uint16_t	block_size;	/* sectors */
+		uint16_t	bucket_size;	/* sectors */
+
+		uint16_t	nr_in_set;
+		uint16_t	nr_this_dev;
+	};
+	struct {
+		/* Backing devices */
+		uint64_t	data_offset;
+
+		/*
+		 * block_size from the cache device section is still used by
+		 * backing devices, so don't add anything here until we fix
+		 * things to not need it for backing devices anymore
+		 */
+	};
+	};
+
+	uint32_t		last_mount;	/* time_t */
+
+	uint16_t		first_bucket;
+	union {
+		uint16_t	njournal_buckets;
+		uint16_t	keys;
+	};
+	uint64_t		d[SB_JOURNAL_BUCKETS];	/* journal buckets */
+};
+
+BITMASK(CACHE_SYNC,		struct cache_sb, flags, 0, 1);
+BITMASK(CACHE_DISCARD,		struct cache_sb, flags, 1, 1);
+BITMASK(CACHE_REPLACEMENT,	struct cache_sb, flags, 2, 3);
+#define CACHE_REPLACEMENT_LRU	0U
+#define CACHE_REPLACEMENT_FIFO	1U
+#define CACHE_REPLACEMENT_RANDOM 2U
+
+BITMASK(BDEV_CACHE_MODE,	struct cache_sb, flags, 0, 4);
+#define CACHE_MODE_WRITETHROUGH	0U
+#define CACHE_MODE_WRITEBACK	1U
+#define CACHE_MODE_WRITEAROUND	2U
+#define CACHE_MODE_NONE		3U
+BITMASK(BDEV_STATE,		struct cache_sb, flags, 61, 2);
+#define BDEV_STATE_NONE		0U
+#define BDEV_STATE_CLEAN	1U
+#define BDEV_STATE_DIRTY	2U
+#define BDEV_STATE_STALE	3U
+
+/* Version 1: Seed pointer into btree node checksum
+ */
+#define BCACHE_BSET_VERSION	1
+
+/*
+ * This is the on disk format for btree nodes - a btree node on disk is a list
+ * of these; within each set the keys are sorted
+ */
+struct bset {
+	uint64_t		csum;
+	uint64_t		magic;
+	uint64_t		seq;
+	uint32_t		version;
+	uint32_t		keys;
+
+	union {
+		struct bkey	start[0];
+		uint64_t	d[0];
+	};
+};
+
+/*
+ * On disk format for priorities and gens - see super.c near prio_write() for
+ * more.
+ */
+struct prio_set {
+	uint64_t		csum;
+	uint64_t		magic;
+	uint64_t		seq;
+	uint32_t		version;
+	uint32_t		pad;
+
+	uint64_t		next_bucket;
+
+	struct bucket_disk {
+		uint16_t	prio;
+		uint8_t		gen;
+	} __attribute((packed)) data[];
+};
+
+struct uuid_entry {
+	union {
+		struct {
+			uint8_t		uuid[16];
+			uint8_t		label[32];
+			uint32_t	first_reg;
+			uint32_t	last_reg;
+			uint32_t	invalidated;
+
+			uint32_t	flags;
+			/* Size of flash only volumes */
+			uint64_t	sectors;
+		};
+
+		uint8_t	pad[128];
+	};
+};
+
+BITMASK(UUID_FLASH_ONLY,	struct uuid_entry, flags, 0, 1);
+
+#include "journal.h"
+#include "stats.h"
+struct search;
+struct btree;
+struct keybuf;
+
+struct keybuf_key {
+	struct rb_node		node;
+	BKEY_PADDED(key);
+	void			*private;
+};
+
+typedef bool (keybuf_pred_fn)(struct keybuf *, struct bkey *);
+
+struct keybuf {
+	keybuf_pred_fn		*key_predicate;
+
+	struct bkey		last_scanned;
+	spinlock_t		lock;
+
+	/*
+	 * Beginning and end of range in rb tree - so that we can skip taking
+	 * lock and checking the rb tree when we need to check for overlapping
+	 * keys.
+	 */
+	struct bkey		start;
+	struct bkey		end;
+
+	struct rb_root		keys;
+
+#define KEYBUF_NR		100
+	DECLARE_ARRAY_ALLOCATOR(struct keybuf_key, freelist, KEYBUF_NR);
+};
+
+struct bio_split_pool {
+	struct bio_set		*bio_split;
+	mempool_t		*bio_split_hook;
+};
+
+struct bio_split_hook {
+	struct closure		cl;
+	struct bio_split_pool	*p;
+	struct bio		*bio;
+	bio_end_io_t		*bi_end_io;
+	void			*bi_private;
+};
+
+struct bcache_device {
+	struct closure		cl;
+
+	struct kobject		kobj;
+
+	struct cache_set	*c;
+	unsigned		id;
+#define BCACHEDEVNAME_SIZE	12
+	char			name[BCACHEDEVNAME_SIZE];
+
+	struct gendisk		*disk;
+
+	/* If nonzero, we're closing */
+	atomic_t		closing;
+
+	/* If nonzero, we're detaching/unregistering from cache set */
+	atomic_t		detaching;
+
+	atomic_long_t		sectors_dirty;
+	unsigned long		sectors_dirty_gc;
+	unsigned long		sectors_dirty_last;
+	long			sectors_dirty_derivative;
+
+	mempool_t		*unaligned_bvec;
+	struct bio_set		*bio_split;
+
+	unsigned		data_csum:1;
+
+	int (*cache_miss)(struct btree *, struct search *,
+			  struct bio *, unsigned);
+	int (*ioctl) (struct bcache_device *, fmode_t, unsigned, unsigned long);
+
+	struct bio_split_pool	bio_split_hook;
+};
+
+struct io {
+	/* Used to track sequential IO so it can be skipped */
+	struct hlist_node	hash;
+	struct list_head	lru;
+
+	unsigned long		jiffies;
+	unsigned		sequential;
+	sector_t		last;
+};
+
+struct cached_dev {
+	struct list_head	list;
+	struct bcache_device	disk;
+	struct block_device	*bdev;
+
+	struct cache_sb		sb;
+	struct bio		sb_bio;
+	struct bio_vec		sb_bv[1];
+	struct closure_with_waitlist sb_write;
+
+	/* Refcount on the cache set. Always nonzero when we're caching. */
+	atomic_t		count;
+	struct work_struct	detach;
+
+	/*
+	 * Device might not be running if it's dirty and the cache set hasn't
+	 * showed up yet.
+	 */
+	atomic_t		running;
+
+	/*
+	 * Writes take a shared lock from start to finish; scanning for dirty
+	 * data to refill the rb tree requires an exclusive lock.
+	 */
+	struct rw_semaphore	writeback_lock;
+
+	/*
+	 * Nonzero, and writeback has a refcount (d->count), iff there is dirty
+	 * data in the cache. Protected by writeback_lock; must have an
+	 * shared lock to set and exclusive lock to clear.
+	 */
+	atomic_t		has_dirty;
+
+	struct ratelimit	writeback_rate;
+	struct delayed_work	writeback_rate_update;
+
+	/*
+	 * Internal to the writeback code, so read_dirty() can keep track of
+	 * where it's at.
+	 */
+	sector_t		last_read;
+
+	/* Number of writeback bios in flight */
+	atomic_t		in_flight;
+	struct closure_with_timer writeback;
+	struct closure_waitlist	writeback_wait;
+
+	struct keybuf		writeback_keys;
+
+	/* For tracking sequential IO */
+#define RECENT_IO_BITS	7
+#define RECENT_IO	(1 << RECENT_IO_BITS)
+	struct io		io[RECENT_IO];
+	struct hlist_head	io_hash[RECENT_IO + 1];
+	struct list_head	io_lru;
+	spinlock_t		io_lock;
+
+	struct cache_accounting	accounting;
+
+	/* The rest of this all shows up in sysfs */
+	unsigned		sequential_cutoff;
+	unsigned		readahead;
+
+	unsigned		sequential_merge:1;
+	unsigned		verify:1;
+
+	unsigned		writeback_metadata:1;
+	unsigned		writeback_running:1;
+	unsigned char		writeback_percent;
+	unsigned		writeback_delay;
+
+	int			writeback_rate_change;
+	int64_t			writeback_rate_derivative;
+	uint64_t		writeback_rate_target;
+
+	unsigned		writeback_rate_update_seconds;
+	unsigned		writeback_rate_d_term;
+	unsigned		writeback_rate_p_term_inverse;
+	unsigned		writeback_rate_d_smooth;
+};
+
+enum alloc_watermarks {
+	WATERMARK_PRIO,
+	WATERMARK_METADATA,
+	WATERMARK_MOVINGGC,
+	WATERMARK_NONE,
+	WATERMARK_MAX
+};
+
+struct cache {
+	struct cache_set	*set;
+	struct cache_sb		sb;
+	struct bio		sb_bio;
+	struct bio_vec		sb_bv[1];
+
+	struct kobject		kobj;
+	struct block_device	*bdev;
+
+	unsigned		watermark[WATERMARK_MAX];
+
+	struct closure		alloc;
+	struct workqueue_struct	*alloc_workqueue;
+
+	struct closure		prio;
+	struct prio_set		*disk_buckets;
+
+	/*
+	 * When allocating new buckets, prio_write() gets first dibs - since we
+	 * may not be allocate at all without writing priorities and gens.
+	 * prio_buckets[] contains the last buckets we wrote priorities to (so
+	 * gc can mark them as metadata), prio_next[] contains the buckets
+	 * allocated for the next prio write.
+	 */
+	uint64_t		*prio_buckets;
+	uint64_t		*prio_last_buckets;
+
+	/*
+	 * free: Buckets that are ready to be used
+	 *
+	 * free_inc: Incoming buckets - these are buckets that currently have
+	 * cached data in them, and we can't reuse them until after we write
+	 * their new gen to disk. After prio_write() finishes writing the new
+	 * gens/prios, they'll be moved to the free list (and possibly discarded
+	 * in the process)
+	 *
+	 * unused: GC found nothing pointing into these buckets (possibly
+	 * because all the data they contained was overwritten), so we only
+	 * need to discard them before they can be moved to the free list.
+	 */
+	DECLARE_FIFO(long, free);
+	DECLARE_FIFO(long, free_inc);
+	DECLARE_FIFO(long, unused);
+
+	size_t			fifo_last_bucket;
+
+	/* Allocation stuff: */
+	struct bucket		*buckets;
+
+	DECLARE_HEAP(struct bucket *, heap);
+
+	/*
+	 * max(gen - disk_gen) for all buckets. When it gets too big we have to
+	 * call prio_write() to keep gens from wrapping.
+	 */
+	uint8_t			need_save_prio;
+	unsigned		gc_move_threshold;
+
+	/*
+	 * If nonzero, we know we aren't going to find any buckets to invalidate
+	 * until a gc finishes - otherwise we could pointlessly burn a ton of
+	 * cpu
+	 */
+	unsigned		invalidate_needs_gc:1;
+
+	bool			discard; /* Get rid of? */
+
+	/*
+	 * We preallocate structs for issuing discards to buckets, and keep them
+	 * on this list when they're not in use; do_discard() issues discards
+	 * whenever there's work to do and is called by free_some_buckets() and
+	 * when a discard finishes.
+	 */
+	atomic_t		discards_in_flight;
+	struct list_head	discards;
+
+	struct journal_device	journal;
+
+	/* The rest of this all shows up in sysfs */
+#define IO_ERROR_SHIFT		20
+	atomic_t		io_errors;
+	atomic_t		io_count;
+
+	atomic_long_t		meta_sectors_written;
+	atomic_long_t		btree_sectors_written;
+	atomic_long_t		sectors_written;
+
+	struct bio_split_pool	bio_split_hook;
+};
+
+struct gc_stat {
+	size_t			nodes;
+	size_t			key_bytes;
+
+	size_t			nkeys;
+	uint64_t		data;	/* sectors */
+	uint64_t		dirty;	/* sectors */
+	unsigned		in_use; /* percent */
+};
+
+/*
+ * Flag bits, for how the cache set is shutting down, and what phase it's at:
+ *
+ * CACHE_SET_UNREGISTERING means we're not just shutting down, we're detaching
+ * all the backing devices first (their cached data gets invalidated, and they
+ * won't automatically reattach).
+ *
+ * CACHE_SET_STOPPING always gets set first when we're closing down a cache set;
+ * we'll continue to run normally for awhile with CACHE_SET_STOPPING set (i.e.
+ * flushing dirty data).
+ *
+ * CACHE_SET_STOPPING_2 gets set at the last phase, when it's time to shut down
+ * the allocation thread.
+ */
+#define CACHE_SET_UNREGISTERING		0
+#define	CACHE_SET_STOPPING		1
+#define	CACHE_SET_STOPPING_2		2
+
+struct cache_set {
+	struct closure		cl;
+
+	struct list_head	list;
+	struct kobject		kobj;
+	struct kobject		internal;
+	struct dentry		*debug;
+	struct cache_accounting accounting;
+
+	unsigned long		flags;
+
+	struct cache_sb		sb;
+
+	struct cache		*cache[MAX_CACHES_PER_SET];
+	struct cache		*cache_by_alloc[MAX_CACHES_PER_SET];
+	int			caches_loaded;
+
+	struct bcache_device	**devices;
+	struct list_head	cached_devs;
+	uint64_t		cached_dev_sectors;
+	struct closure		caching;
+
+	struct closure_with_waitlist sb_write;
+
+	mempool_t		*search;
+	mempool_t		*bio_meta;
+	struct bio_set		*bio_split;
+
+	/* For the btree cache */
+	struct shrinker		shrink;
+
+	/* For the allocator itself */
+	wait_queue_head_t	alloc_wait;
+
+	/* For the btree cache and anything allocation related */
+	struct mutex		bucket_lock;
+
+	/* log2(bucket_size), in sectors */
+	unsigned short		bucket_bits;
+
+	/* log2(block_size), in sectors */
+	unsigned short		block_bits;
+
+	/*
+	 * Default number of pages for a new btree node - may be less than a
+	 * full bucket
+	 */
+	unsigned		btree_pages;
+
+	/*
+	 * Lists of struct btrees; lru is the list for structs that have memory
+	 * allocated for actual btree node, freed is for structs that do not.
+	 *
+	 * We never free a struct btree, except on shutdown - we just put it on
+	 * the btree_cache_freed list and reuse it later. This simplifies the
+	 * code, and it doesn't cost us much memory as the memory usage is
+	 * dominated by buffers that hold the actual btree node data and those
+	 * can be freed - and the number of struct btrees allocated is
+	 * effectively bounded.
+	 *
+	 * btree_cache_freeable effectively is a small cache - we use it because
+	 * high order page allocations can be rather expensive, and it's quite
+	 * common to delete and allocate btree nodes in quick succession. It
+	 * should never grow past ~2-3 nodes in practice.
+	 */
+	struct list_head	btree_cache;
+	struct list_head	btree_cache_freeable;
+	struct list_head	btree_cache_freed;
+
+	/* Number of elements in btree_cache + btree_cache_freeable lists */
+	unsigned		bucket_cache_used;
+
+	/*
+	 * If we need to allocate memory for a new btree node and that
+	 * allocation fails, we can cannibalize another node in the btree cache
+	 * to satisfy the allocation. However, only one thread can be doing this
+	 * at a time, for obvious reasons - try_harder and try_wait are
+	 * basically a lock for this that we can wait on asynchronously. The
+	 * btree_root() macro releases the lock when it returns.
+	 */
+	struct closure		*try_harder;
+	struct closure_waitlist	try_wait;
+	uint64_t		try_harder_start;
+
+	/*
+	 * When we free a btree node, we increment the gen of the bucket the
+	 * node is in - but we can't rewrite the prios and gens until we
+	 * finished whatever it is we were doing, otherwise after a crash the
+	 * btree node would be freed but for say a split, we might not have the
+	 * pointers to the new nodes inserted into the btree yet.
+	 *
+	 * This is a refcount that blocks prio_write() until the new keys are
+	 * written.
+	 */
+	atomic_t		prio_blocked;
+	struct closure_waitlist	bucket_wait;
+
+	/*
+	 * For any bio we don't skip we subtract the number of sectors from
+	 * rescale; when it hits 0 we rescale all the bucket priorities.
+	 */
+	atomic_t		rescale;
+	/*
+	 * When we invalidate buckets, we use both the priority and the amount
+	 * of good data to determine which buckets to reuse first - to weight
+	 * those together consistently we keep track of the smallest nonzero
+	 * priority of any bucket.
+	 */
+	uint16_t		min_prio;
+
+	/*
+	 * max(gen - gc_gen) for all buckets. When it gets too big we have to gc
+	 * to keep gens from wrapping around.
+	 */
+	uint8_t			need_gc;
+	struct gc_stat		gc_stats;
+	size_t			nbuckets;
+
+	struct closure_with_waitlist gc;
+	/* Where in the btree gc currently is */
+	struct bkey		gc_done;
+
+	/*
+	 * The allocation code needs gc_mark in struct bucket to be correct, but
+	 * it's not while a gc is in progress. Protected by bucket_lock.
+	 */
+	int			gc_mark_valid;
+
+	/* Counts how many sectors bio_insert has added to the cache */
+	atomic_t		sectors_to_gc;
+
+	struct closure		moving_gc;
+	struct closure_waitlist	moving_gc_wait;
+	struct keybuf		moving_gc_keys;
+	/* Number of moving GC bios in flight */
+	atomic_t		in_flight;
+
+	struct btree		*root;
+
+#ifdef CONFIG_BCACHE_DEBUG
+	struct btree		*verify_data;
+	struct mutex		verify_lock;
+#endif
+
+	unsigned		nr_uuids;
+	struct uuid_entry	*uuids;
+	BKEY_PADDED(uuid_bucket);
+	struct closure_with_waitlist uuid_write;
+
+	/*
+	 * A btree node on disk could have too many bsets for an iterator to fit
+	 * on the stack - this is a single element mempool for btree_read_work()
+	 */
+	struct mutex		fill_lock;
+	struct btree_iter	*fill_iter;
+
+	/*
+	 * btree_sort() is a merge sort and requires temporary space - single
+	 * element mempool
+	 */
+	struct mutex		sort_lock;
+	struct bset		*sort;
+
+	/* List of buckets we're currently writing data to */
+	struct list_head	data_buckets;
+	spinlock_t		data_bucket_lock;
+
+	struct journal		journal;
+
+#define CONGESTED_MAX		1024
+	unsigned		congested_last_us;
+	atomic_t		congested;
+
+	/* The rest of this all shows up in sysfs */
+	unsigned		congested_read_threshold_us;
+	unsigned		congested_write_threshold_us;
+
+	spinlock_t		sort_time_lock;
+	struct time_stats	sort_time;
+	struct time_stats	btree_gc_time;
+	struct time_stats	btree_split_time;
+	spinlock_t		btree_read_time_lock;
+	struct time_stats	btree_read_time;
+	struct time_stats	try_harder_time;
+
+	atomic_long_t		cache_read_races;
+	atomic_long_t		writeback_keys_done;
+	atomic_long_t		writeback_keys_failed;
+	unsigned		error_limit;
+	unsigned		error_decay;
+	unsigned short		journal_delay_ms;
+	unsigned		verify:1;
+	unsigned		key_merging_disabled:1;
+	unsigned		gc_always_rewrite:1;
+	unsigned		shrinker_disabled:1;
+	unsigned		copy_gc_enabled:1;
+
+#define BUCKET_HASH_BITS	12
+	struct hlist_head	bucket_hash[1 << BUCKET_HASH_BITS];
+};
+
+static inline bool key_merging_disabled(struct cache_set *c)
+{
+#ifdef CONFIG_BCACHE_DEBUG
+	return c->key_merging_disabled;
+#else
+	return 0;
+#endif
+}
+
+static inline bool SB_IS_BDEV(const struct cache_sb *sb)
+{
+	return sb->version == BCACHE_SB_VERSION_BDEV
+		|| sb->version == BCACHE_SB_VERSION_BDEV_WITH_OFFSET;
+}
+
+struct bbio {
+	unsigned		submit_time_us;
+	union {
+		struct bkey	key;
+		uint64_t	_pad[3];
+		/*
+		 * We only need pad = 3 here because we only ever carry around a
+		 * single pointer - i.e. the pointer we're doing io to/from.
+		 */
+	};
+	struct bio		bio;
+};
+
+static inline unsigned local_clock_us(void)
+{
+	return local_clock() >> 10;
+}
+
+#define MAX_BSETS		4U
+
+#define BTREE_PRIO		USHRT_MAX
+#define INITIAL_PRIO		32768
+
+#define btree_bytes(c)		((c)->btree_pages * PAGE_SIZE)
+#define btree_blocks(b)							\
+	((unsigned) (KEY_SIZE(&b->key) >> (b)->c->block_bits))
+
+#define btree_default_blocks(c)						\
+	((unsigned) ((PAGE_SECTORS * (c)->btree_pages) >> (c)->block_bits))
+
+#define bucket_pages(c)		((c)->sb.bucket_size / PAGE_SECTORS)
+#define bucket_bytes(c)		((c)->sb.bucket_size << 9)
+#define block_bytes(c)		((c)->sb.block_size << 9)
+
+#define __set_bytes(i, k)	(sizeof(*(i)) + (k) * sizeof(uint64_t))
+#define set_bytes(i)		__set_bytes(i, i->keys)
+
+#define __set_blocks(i, k, c)	DIV_ROUND_UP(__set_bytes(i, k), block_bytes(c))
+#define set_blocks(i, c)	__set_blocks(i, (i)->keys, c)
+
+#define node(i, j)		((struct bkey *) ((i)->d + (j)))
+#define end(i)			node(i, (i)->keys)
+
+#define index(i, b)							\
+	((size_t) (((void *) i - (void *) (b)->sets[0].data) /		\
+		   block_bytes(b->c)))
+
+#define btree_data_space(b)	(PAGE_SIZE << (b)->page_order)
+
+#define prios_per_bucket(c)				\
+	((bucket_bytes(c) - sizeof(struct prio_set)) /	\
+	 sizeof(struct bucket_disk))
+#define prio_buckets(c)					\
+	DIV_ROUND_UP((size_t) (c)->sb.nbuckets, prios_per_bucket(c))
+
+#define JSET_MAGIC		0x245235c1a3625032ULL
+#define PSET_MAGIC		0x6750e15f87337f91ULL
+#define BSET_MAGIC		0x90135c78b99e07f5ULL
+
+#define jset_magic(c)		((c)->sb.set_magic ^ JSET_MAGIC)
+#define pset_magic(c)		((c)->sb.set_magic ^ PSET_MAGIC)
+#define bset_magic(c)		((c)->sb.set_magic ^ BSET_MAGIC)
+
+/* Bkey fields: all units are in sectors */
+
+#define KEY_FIELD(name, field, offset, size)				\
+	BITMASK(name, struct bkey, field, offset, size)
+
+#define PTR_FIELD(name, offset, size)					\
+	static inline uint64_t name(const struct bkey *k, unsigned i)	\
+	{ return (k->ptr[i] >> offset) & ~(((uint64_t) ~0) << size); }	\
+									\
+	static inline void SET_##name(struct bkey *k, unsigned i, uint64_t v)\
+	{								\
+		k->ptr[i] &= ~(~((uint64_t) ~0 << size) << offset);	\
+		k->ptr[i] |= v << offset;				\
+	}
+
+KEY_FIELD(KEY_PTRS,	high, 60, 3)
+KEY_FIELD(HEADER_SIZE,	high, 58, 2)
+KEY_FIELD(KEY_CSUM,	high, 56, 2)
+KEY_FIELD(KEY_PINNED,	high, 55, 1)
+KEY_FIELD(KEY_DIRTY,	high, 36, 1)
+
+KEY_FIELD(KEY_SIZE,	high, 20, 16)
+KEY_FIELD(KEY_INODE,	high, 0,  20)
+
+/* Next time I change the on disk format, KEY_OFFSET() won't be 64 bits */
+
+static inline uint64_t KEY_OFFSET(const struct bkey *k)
+{
+	return k->low;
+}
+
+static inline void SET_KEY_OFFSET(struct bkey *k, uint64_t v)
+{
+	k->low = v;
+}
+
+PTR_FIELD(PTR_DEV,		51, 12)
+PTR_FIELD(PTR_OFFSET,		8,  43)
+PTR_FIELD(PTR_GEN,		0,  8)
+
+#define PTR_CHECK_DEV		((1 << 12) - 1)
+
+#define PTR(gen, offset, dev)						\
+	((((uint64_t) dev) << 51) | ((uint64_t) offset) << 8 | gen)
+
+static inline size_t sector_to_bucket(struct cache_set *c, sector_t s)
+{
+	return s >> c->bucket_bits;
+}
+
+static inline sector_t bucket_to_sector(struct cache_set *c, size_t b)
+{
+	return ((sector_t) b) << c->bucket_bits;
+}
+
+static inline sector_t bucket_remainder(struct cache_set *c, sector_t s)
+{
+	return s & (c->sb.bucket_size - 1);
+}
+
+static inline struct cache *PTR_CACHE(struct cache_set *c,
+				      const struct bkey *k,
+				      unsigned ptr)
+{
+	return c->cache[PTR_DEV(k, ptr)];
+}
+
+static inline size_t PTR_BUCKET_NR(struct cache_set *c,
+				   const struct bkey *k,
+				   unsigned ptr)
+{
+	return sector_to_bucket(c, PTR_OFFSET(k, ptr));
+}
+
+static inline struct bucket *PTR_BUCKET(struct cache_set *c,
+					const struct bkey *k,
+					unsigned ptr)
+{
+	return PTR_CACHE(c, k, ptr)->buckets + PTR_BUCKET_NR(c, k, ptr);
+}
+
+/* Btree key macros */
+
+/*
+ * The high bit being set is a relic from when we used it to do binary
+ * searches - it told you where a key started. It's not used anymore,
+ * and can probably be safely dropped.
+ */
+#define KEY(dev, sector, len)						\
+((struct bkey) {							\
+	.high = (1ULL << 63) | ((uint64_t) (len) << 20) | (dev),	\
+	.low = (sector)							\
+})
+
+static inline void bkey_init(struct bkey *k)
+{
+	*k = KEY(0, 0, 0);
+}
+
+#define KEY_START(k)		(KEY_OFFSET(k) - KEY_SIZE(k))
+#define START_KEY(k)		KEY(KEY_INODE(k), KEY_START(k), 0)
+#define MAX_KEY			KEY(~(~0 << 20), ((uint64_t) ~0) >> 1, 0)
+#define ZERO_KEY		KEY(0, 0, 0)
+
+/*
+ * This is used for various on disk data structures - cache_sb, prio_set, bset,
+ * jset: The checksum is _always_ the first 8 bytes of these structs
+ */
+#define csum_set(i)							\
+	bch_crc64(((void *) (i)) + sizeof(uint64_t),			\
+	      ((void *) end(i)) - (((void *) (i)) + sizeof(uint64_t)))
+
+/* Error handling macros */
+
+#define btree_bug(b, ...)						\
+do {									\
+	if (bch_cache_set_error((b)->c, __VA_ARGS__))			\
+		dump_stack();						\
+} while (0)
+
+#define cache_bug(c, ...)						\
+do {									\
+	if (bch_cache_set_error(c, __VA_ARGS__))			\
+		dump_stack();						\
+} while (0)
+
+#define btree_bug_on(cond, b, ...)					\
+do {									\
+	if (cond)							\
+		btree_bug(b, __VA_ARGS__);				\
+} while (0)
+
+#define cache_bug_on(cond, c, ...)					\
+do {									\
+	if (cond)							\
+		cache_bug(c, __VA_ARGS__);				\
+} while (0)
+
+#define cache_set_err_on(cond, c, ...)					\
+do {									\
+	if (cond)							\
+		bch_cache_set_error(c, __VA_ARGS__);			\
+} while (0)
+
+/* Looping macros */
+
+#define for_each_cache(ca, cs, iter)					\
+	for (iter = 0; ca = cs->cache[iter], iter < (cs)->sb.nr_in_set; iter++)
+
+#define for_each_bucket(b, ca)						\
+	for (b = (ca)->buckets + (ca)->sb.first_bucket;			\
+	     b < (ca)->buckets + (ca)->sb.nbuckets; b++)
+
+static inline void __bkey_put(struct cache_set *c, struct bkey *k)
+{
+	unsigned i;
+
+	for (i = 0; i < KEY_PTRS(k); i++)
+		atomic_dec_bug(&PTR_BUCKET(c, k, i)->pin);
+}
+
+/* Blktrace macros */
+
+#define blktrace_msg(c, fmt, ...)					\
+do {									\
+	struct request_queue *q = bdev_get_queue(c->bdev);		\
+	if (q)								\
+		blk_add_trace_msg(q, fmt, ##__VA_ARGS__);		\
+} while (0)
+
+#define blktrace_msg_all(s, fmt, ...)					\
+do {									\
+	struct cache *_c;						\
+	unsigned i;							\
+	for_each_cache(_c, (s), i)					\
+		blktrace_msg(_c, fmt, ##__VA_ARGS__);			\
+} while (0)
+
+static inline void cached_dev_put(struct cached_dev *dc)
+{
+	if (atomic_dec_and_test(&dc->count))
+		schedule_work(&dc->detach);
+}
+
+static inline bool cached_dev_get(struct cached_dev *dc)
+{
+	if (!atomic_inc_not_zero(&dc->count))
+		return false;
+
+	/* Paired with the mb in cached_dev_attach */
+	smp_mb__after_atomic_inc();
+	return true;
+}
+
+/*
+ * bucket_gc_gen() returns the difference between the bucket's current gen and
+ * the oldest gen of any pointer into that bucket in the btree (last_gc).
+ *
+ * bucket_disk_gen() returns the difference between the current gen and the gen
+ * on disk; they're both used to make sure gens don't wrap around.
+ */
+
+static inline uint8_t bucket_gc_gen(struct bucket *b)
+{
+	return b->gen - b->last_gc;
+}
+
+static inline uint8_t bucket_disk_gen(struct bucket *b)
+{
+	return b->gen - b->disk_gen;
+}
+
+#define BUCKET_GC_GEN_MAX	96U
+#define BUCKET_DISK_GEN_MAX	64U
+
+#define kobj_attribute_write(n, fn)					\
+	static struct kobj_attribute ksysfs_##n = __ATTR(n, S_IWUSR, NULL, fn)
+
+#define kobj_attribute_rw(n, show, store)				\
+	static struct kobj_attribute ksysfs_##n =			\
+		__ATTR(n, S_IWUSR|S_IRUSR, show, store)
+
+/* Forward declarations */
+
+void bch_writeback_queue(struct cached_dev *);
+void bch_writeback_add(struct cached_dev *, unsigned);
+
+void bch_count_io_errors(struct cache *, int, const char *);
+void bch_bbio_count_io_errors(struct cache_set *, struct bio *,
+			      int, const char *);
+void bch_bbio_endio(struct cache_set *, struct bio *, int, const char *);
+void bch_bbio_free(struct bio *, struct cache_set *);
+struct bio *bch_bbio_alloc(struct cache_set *);
+
+struct bio *bch_bio_split(struct bio *, int, gfp_t, struct bio_set *);
+void bch_generic_make_request(struct bio *, struct bio_split_pool *);
+void __bch_submit_bbio(struct bio *, struct cache_set *);
+void bch_submit_bbio(struct bio *, struct cache_set *, struct bkey *, unsigned);
+
+uint8_t bch_inc_gen(struct cache *, struct bucket *);
+void bch_rescale_priorities(struct cache_set *, int);
+bool bch_bucket_add_unused(struct cache *, struct bucket *);
+void bch_allocator_thread(struct closure *);
+
+long bch_bucket_alloc(struct cache *, unsigned, struct closure *);
+void bch_bucket_free(struct cache_set *, struct bkey *);
+
+int __bch_bucket_alloc_set(struct cache_set *, unsigned,
+			   struct bkey *, int, struct closure *);
+int bch_bucket_alloc_set(struct cache_set *, unsigned,
+			 struct bkey *, int, struct closure *);
+
+__printf(2, 3)
+bool bch_cache_set_error(struct cache_set *, const char *, ...);
+
+void bch_prio_write(struct cache *);
+void bch_write_bdev_super(struct cached_dev *, struct closure *);
+
+extern struct workqueue_struct *bcache_wq, *bch_gc_wq;
+extern const char * const bch_cache_modes[];
+extern struct mutex bch_register_lock;
+extern struct list_head bch_cache_sets;
+
+extern struct kobj_type bch_cached_dev_ktype;
+extern struct kobj_type bch_flash_dev_ktype;
+extern struct kobj_type bch_cache_set_ktype;
+extern struct kobj_type bch_cache_set_internal_ktype;
+extern struct kobj_type bch_cache_ktype;
+
+void bch_cached_dev_release(struct kobject *);
+void bch_flash_dev_release(struct kobject *);
+void bch_cache_set_release(struct kobject *);
+void bch_cache_release(struct kobject *);
+
+int bch_uuid_write(struct cache_set *);
+void bcache_write_super(struct cache_set *);
+
+int bch_flash_dev_create(struct cache_set *c, uint64_t size);
+
+int bch_cached_dev_attach(struct cached_dev *, struct cache_set *);
+void bch_cached_dev_detach(struct cached_dev *);
+void bch_cached_dev_run(struct cached_dev *);
+void bcache_device_stop(struct bcache_device *);
+
+void bch_cache_set_unregister(struct cache_set *);
+void bch_cache_set_stop(struct cache_set *);
+
+struct cache_set *bch_cache_set_alloc(struct cache_sb *);
+void bch_btree_cache_free(struct cache_set *);
+int bch_btree_cache_alloc(struct cache_set *);
+void bch_writeback_init_cached_dev(struct cached_dev *);
+void bch_moving_init_cache_set(struct cache_set *);
+
+void bch_cache_allocator_exit(struct cache *ca);
+int bch_cache_allocator_init(struct cache *ca);
+
+void bch_debug_exit(void);
+int bch_debug_init(struct kobject *);
+void bch_writeback_exit(void);
+int bch_writeback_init(void);
+void bch_request_exit(void);
+int bch_request_init(void);
+void bch_btree_exit(void);
+int bch_btree_init(void);
+
+#endif /* _BCACHE_H */
diff --git a/drivers/md/bcache/bset.c b/drivers/md/bcache/bset.c
new file mode 100644
index 000000000000..cb4578a327b9
--- /dev/null
+++ b/drivers/md/bcache/bset.c
@@ -0,0 +1,1192 @@
+/*
+ * Code for working with individual keys, and sorted sets of keys with in a
+ * btree node
+ *
+ * Copyright 2012 Google, Inc.
+ */
+
+#include "bcache.h"
+#include "btree.h"
+#include "debug.h"
+
+#include <linux/random.h>
+#include <linux/prefetch.h>
+
+/* Keylists */
+
+void bch_keylist_copy(struct keylist *dest, struct keylist *src)
+{
+	*dest = *src;
+
+	if (src->list == src->d) {
+		size_t n = (uint64_t *) src->top - src->d;
+		dest->top = (struct bkey *) &dest->d[n];
+		dest->list = dest->d;
+	}
+}
+
+int bch_keylist_realloc(struct keylist *l, int nptrs, struct cache_set *c)
+{
+	unsigned oldsize = (uint64_t *) l->top - l->list;
+	unsigned newsize = oldsize + 2 + nptrs;
+	uint64_t *new;
+
+	/* The journalling code doesn't handle the case where the keys to insert
+	 * is bigger than an empty write: If we just return -ENOMEM here,
+	 * bio_insert() and bio_invalidate() will insert the keys created so far
+	 * and finish the rest when the keylist is empty.
+	 */
+	if (newsize * sizeof(uint64_t) > block_bytes(c) - sizeof(struct jset))
+		return -ENOMEM;
+
+	newsize = roundup_pow_of_two(newsize);
+
+	if (newsize <= KEYLIST_INLINE ||
+	    roundup_pow_of_two(oldsize) == newsize)
+		return 0;
+
+	new = krealloc(l->list == l->d ? NULL : l->list,
+		       sizeof(uint64_t) * newsize, GFP_NOIO);
+
+	if (!new)
+		return -ENOMEM;
+
+	if (l->list == l->d)
+		memcpy(new, l->list, sizeof(uint64_t) * KEYLIST_INLINE);
+
+	l->list = new;
+	l->top = (struct bkey *) (&l->list[oldsize]);
+
+	return 0;
+}
+
+struct bkey *bch_keylist_pop(struct keylist *l)
+{
+	struct bkey *k = l->bottom;
+
+	if (k == l->top)
+		return NULL;
+
+	while (bkey_next(k) != l->top)
+		k = bkey_next(k);
+
+	return l->top = k;
+}
+
+/* Pointer validation */
+
+bool __bch_ptr_invalid(struct cache_set *c, int level, const struct bkey *k)
+{
+	unsigned i;
+
+	if (level && (!KEY_PTRS(k) || !KEY_SIZE(k) || KEY_DIRTY(k)))
+		goto bad;
+
+	if (!level && KEY_SIZE(k) > KEY_OFFSET(k))
+		goto bad;
+
+	if (!KEY_SIZE(k))
+		return true;
+
+	for (i = 0; i < KEY_PTRS(k); i++)
+		if (ptr_available(c, k, i)) {
+			struct cache *ca = PTR_CACHE(c, k, i);
+			size_t bucket = PTR_BUCKET_NR(c, k, i);
+			size_t r = bucket_remainder(c, PTR_OFFSET(k, i));
+
+			if (KEY_SIZE(k) + r > c->sb.bucket_size ||
+			    bucket <  ca->sb.first_bucket ||
+			    bucket >= ca->sb.nbuckets)
+				goto bad;
+		}
+
+	return false;
+bad:
+	cache_bug(c, "spotted bad key %s: %s", pkey(k), bch_ptr_status(c, k));
+	return true;
+}
+
+bool bch_ptr_bad(struct btree *b, const struct bkey *k)
+{
+	struct bucket *g;
+	unsigned i, stale;
+
+	if (!bkey_cmp(k, &ZERO_KEY) ||
+	    !KEY_PTRS(k) ||
+	    bch_ptr_invalid(b, k))
+		return true;
+
+	if (KEY_PTRS(k) && PTR_DEV(k, 0) == PTR_CHECK_DEV)
+		return true;
+
+	for (i = 0; i < KEY_PTRS(k); i++)
+		if (ptr_available(b->c, k, i)) {
+			g = PTR_BUCKET(b->c, k, i);
+			stale = ptr_stale(b->c, k, i);
+
+			btree_bug_on(stale > 96, b,
+				     "key too stale: %i, need_gc %u",
+				     stale, b->c->need_gc);
+
+			btree_bug_on(stale && KEY_DIRTY(k) && KEY_SIZE(k),
+				     b, "stale dirty pointer");
+
+			if (stale)
+				return true;
+
+#ifdef CONFIG_BCACHE_EDEBUG
+			if (!mutex_trylock(&b->c->bucket_lock))
+				continue;
+
+			if (b->level) {
+				if (KEY_DIRTY(k) ||
+				    g->prio != BTREE_PRIO ||
+				    (b->c->gc_mark_valid &&
+				     GC_MARK(g) != GC_MARK_METADATA))
+					goto bug;
+
+			} else {
+				if (g->prio == BTREE_PRIO)
+					goto bug;
+
+				if (KEY_DIRTY(k) &&
+				    b->c->gc_mark_valid &&
+				    GC_MARK(g) != GC_MARK_DIRTY)
+					goto bug;
+			}
+			mutex_unlock(&b->c->bucket_lock);
+#endif
+		}
+
+	return false;
+#ifdef CONFIG_BCACHE_EDEBUG
+bug:
+	mutex_unlock(&b->c->bucket_lock);
+	btree_bug(b,
+"inconsistent pointer %s: bucket %zu pin %i prio %i gen %i last_gc %i mark %llu gc_gen %i",
+		  pkey(k), PTR_BUCKET_NR(b->c, k, i), atomic_read(&g->pin),
+		  g->prio, g->gen, g->last_gc, GC_MARK(g), g->gc_gen);
+	return true;
+#endif
+}
+
+/* Key/pointer manipulation */
+
+void bch_bkey_copy_single_ptr(struct bkey *dest, const struct bkey *src,
+			      unsigned i)
+{
+	BUG_ON(i > KEY_PTRS(src));
+
+	/* Only copy the header, key, and one pointer. */
+	memcpy(dest, src, 2 * sizeof(uint64_t));
+	dest->ptr[0] = src->ptr[i];
+	SET_KEY_PTRS(dest, 1);
+	/* We didn't copy the checksum so clear that bit. */
+	SET_KEY_CSUM(dest, 0);
+}
+
+bool __bch_cut_front(const struct bkey *where, struct bkey *k)
+{
+	unsigned i, len = 0;
+
+	if (bkey_cmp(where, &START_KEY(k)) <= 0)
+		return false;
+
+	if (bkey_cmp(where, k) < 0)
+		len = KEY_OFFSET(k) - KEY_OFFSET(where);
+	else
+		bkey_copy_key(k, where);
+
+	for (i = 0; i < KEY_PTRS(k); i++)
+		SET_PTR_OFFSET(k, i, PTR_OFFSET(k, i) + KEY_SIZE(k) - len);
+
+	BUG_ON(len > KEY_SIZE(k));
+	SET_KEY_SIZE(k, len);
+	return true;
+}
+
+bool __bch_cut_back(const struct bkey *where, struct bkey *k)
+{
+	unsigned len = 0;
+
+	if (bkey_cmp(where, k) >= 0)
+		return false;
+
+	BUG_ON(KEY_INODE(where) != KEY_INODE(k));
+
+	if (bkey_cmp(where, &START_KEY(k)) > 0)
+		len = KEY_OFFSET(where) - KEY_START(k);
+
+	bkey_copy_key(k, where);
+
+	BUG_ON(len > KEY_SIZE(k));
+	SET_KEY_SIZE(k, len);
+	return true;
+}
+
+static uint64_t merge_chksums(struct bkey *l, struct bkey *r)
+{
+	return (l->ptr[KEY_PTRS(l)] + r->ptr[KEY_PTRS(r)]) &
+		~((uint64_t)1 << 63);
+}
+
+/* Tries to merge l and r: l should be lower than r
+ * Returns true if we were able to merge. If we did merge, l will be the merged
+ * key, r will be untouched.
+ */
+bool bch_bkey_try_merge(struct btree *b, struct bkey *l, struct bkey *r)
+{
+	unsigned i;
+
+	if (key_merging_disabled(b->c))
+		return false;
+
+	if (KEY_PTRS(l) != KEY_PTRS(r) ||
+	    KEY_DIRTY(l) != KEY_DIRTY(r) ||
+	    bkey_cmp(l, &START_KEY(r)))
+		return false;
+
+	for (i = 0; i < KEY_PTRS(l); i++)
+		if (l->ptr[i] + PTR(0, KEY_SIZE(l), 0) != r->ptr[i] ||
+		    PTR_BUCKET_NR(b->c, l, i) != PTR_BUCKET_NR(b->c, r, i))
+			return false;
+
+	/* Keys with no pointers aren't restricted to one bucket and could
+	 * overflow KEY_SIZE
+	 */
+	if (KEY_SIZE(l) + KEY_SIZE(r) > USHRT_MAX) {
+		SET_KEY_OFFSET(l, KEY_OFFSET(l) + USHRT_MAX - KEY_SIZE(l));
+		SET_KEY_SIZE(l, USHRT_MAX);
+
+		bch_cut_front(l, r);
+		return false;
+	}
+
+	if (KEY_CSUM(l)) {
+		if (KEY_CSUM(r))
+			l->ptr[KEY_PTRS(l)] = merge_chksums(l, r);
+		else
+			SET_KEY_CSUM(l, 0);
+	}
+
+	SET_KEY_OFFSET(l, KEY_OFFSET(l) + KEY_SIZE(r));
+	SET_KEY_SIZE(l, KEY_SIZE(l) + KEY_SIZE(r));
+
+	return true;
+}
+
+/* Binary tree stuff for auxiliary search trees */
+
+static unsigned inorder_next(unsigned j, unsigned size)
+{
+	if (j * 2 + 1 < size) {
+		j = j * 2 + 1;
+
+		while (j * 2 < size)
+			j *= 2;
+	} else
+		j >>= ffz(j) + 1;
+
+	return j;
+}
+
+static unsigned inorder_prev(unsigned j, unsigned size)
+{
+	if (j * 2 < size) {
+		j = j * 2;
+
+		while (j * 2 + 1 < size)
+			j = j * 2 + 1;
+	} else
+		j >>= ffs(j);
+
+	return j;
+}
+
+/* I have no idea why this code works... and I'm the one who wrote it
+ *
+ * However, I do know what it does:
+ * Given a binary tree constructed in an array (i.e. how you normally implement
+ * a heap), it converts a node in the tree - referenced by array index - to the
+ * index it would have if you did an inorder traversal.
+ *
+ * Also tested for every j, size up to size somewhere around 6 million.
+ *
+ * The binary tree starts at array index 1, not 0
+ * extra is a function of size:
+ *   extra = (size - rounddown_pow_of_two(size - 1)) << 1;
+ */
+static unsigned __to_inorder(unsigned j, unsigned size, unsigned extra)
+{
+	unsigned b = fls(j);
+	unsigned shift = fls(size - 1) - b;
+
+	j  ^= 1U << (b - 1);
+	j <<= 1;
+	j  |= 1;
+	j <<= shift;
+
+	if (j > extra)
+		j -= (j - extra) >> 1;
+
+	return j;
+}
+
+static unsigned to_inorder(unsigned j, struct bset_tree *t)
+{
+	return __to_inorder(j, t->size, t->extra);
+}
+
+static unsigned __inorder_to_tree(unsigned j, unsigned size, unsigned extra)
+{
+	unsigned shift;
+
+	if (j > extra)
+		j += j - extra;
+
+	shift = ffs(j);
+
+	j >>= shift;
+	j  |= roundup_pow_of_two(size) >> shift;
+
+	return j;
+}
+
+static unsigned inorder_to_tree(unsigned j, struct bset_tree *t)
+{
+	return __inorder_to_tree(j, t->size, t->extra);
+}
+
+#if 0
+void inorder_test(void)
+{
+	unsigned long done = 0;
+	ktime_t start = ktime_get();
+
+	for (unsigned size = 2;
+	     size < 65536000;
+	     size++) {
+		unsigned extra = (size - rounddown_pow_of_two(size - 1)) << 1;
+		unsigned i = 1, j = rounddown_pow_of_two(size - 1);
+
+		if (!(size % 4096))
+			printk(KERN_NOTICE "loop %u, %llu per us\n", size,
+			       done / ktime_us_delta(ktime_get(), start));
+
+		while (1) {
+			if (__inorder_to_tree(i, size, extra) != j)
+				panic("size %10u j %10u i %10u", size, j, i);
+
+			if (__to_inorder(j, size, extra) != i)
+				panic("size %10u j %10u i %10u", size, j, i);
+
+			if (j == rounddown_pow_of_two(size) - 1)
+				break;
+
+			BUG_ON(inorder_prev(inorder_next(j, size), size) != j);
+
+			j = inorder_next(j, size);
+			i++;
+		}
+
+		done += size - 1;
+	}
+}
+#endif
+
+/*
+ * Cacheline/offset <-> bkey pointer arithmatic:
+ *
+ * t->tree is a binary search tree in an array; each node corresponds to a key
+ * in one cacheline in t->set (BSET_CACHELINE bytes).
+ *
+ * This means we don't have to store the full index of the key that a node in
+ * the binary tree points to; to_inorder() gives us the cacheline, and then
+ * bkey_float->m gives us the offset within that cacheline, in units of 8 bytes.
+ *
+ * cacheline_to_bkey() and friends abstract out all the pointer arithmatic to
+ * make this work.
+ *
+ * To construct the bfloat for an arbitrary key we need to know what the key
+ * immediately preceding it is: we have to check if the two keys differ in the
+ * bits we're going to store in bkey_float->mantissa. t->prev[j] stores the size
+ * of the previous key so we can walk backwards to it from t->tree[j]'s key.
+ */
+
+static struct bkey *cacheline_to_bkey(struct bset_tree *t, unsigned cacheline,
+				      unsigned offset)
+{
+	return ((void *) t->data) + cacheline * BSET_CACHELINE + offset * 8;
+}
+
+static unsigned bkey_to_cacheline(struct bset_tree *t, struct bkey *k)
+{
+	return ((void *) k - (void *) t->data) / BSET_CACHELINE;
+}
+
+static unsigned bkey_to_cacheline_offset(struct bkey *k)
+{
+	return ((size_t) k & (BSET_CACHELINE - 1)) / sizeof(uint64_t);
+}
+
+static struct bkey *tree_to_bkey(struct bset_tree *t, unsigned j)
+{
+	return cacheline_to_bkey(t, to_inorder(j, t), t->tree[j].m);
+}
+
+static struct bkey *tree_to_prev_bkey(struct bset_tree *t, unsigned j)
+{
+	return (void *) (((uint64_t *) tree_to_bkey(t, j)) - t->prev[j]);
+}
+
+/*
+ * For the write set - the one we're currently inserting keys into - we don't
+ * maintain a full search tree, we just keep a simple lookup table in t->prev.
+ */
+static struct bkey *table_to_bkey(struct bset_tree *t, unsigned cacheline)
+{
+	return cacheline_to_bkey(t, cacheline, t->prev[cacheline]);
+}
+
+static inline uint64_t shrd128(uint64_t high, uint64_t low, uint8_t shift)
+{
+#ifdef CONFIG_X86_64
+	asm("shrd %[shift],%[high],%[low]"
+	    : [low] "+Rm" (low)
+	    : [high] "R" (high),
+	    [shift] "ci" (shift)
+	    : "cc");
+#else
+	low >>= shift;
+	low  |= (high << 1) << (63U - shift);
+#endif
+	return low;
+}
+
+static inline unsigned bfloat_mantissa(const struct bkey *k,
+				       struct bkey_float *f)
+{
+	const uint64_t *p = &k->low - (f->exponent >> 6);
+	return shrd128(p[-1], p[0], f->exponent & 63) & BKEY_MANTISSA_MASK;
+}
+
+static void make_bfloat(struct bset_tree *t, unsigned j)
+{
+	struct bkey_float *f = &t->tree[j];
+	struct bkey *m = tree_to_bkey(t, j);
+	struct bkey *p = tree_to_prev_bkey(t, j);
+
+	struct bkey *l = is_power_of_2(j)
+		? t->data->start
+		: tree_to_prev_bkey(t, j >> ffs(j));
+
+	struct bkey *r = is_power_of_2(j + 1)
+		? node(t->data, t->data->keys - bkey_u64s(&t->end))
+		: tree_to_bkey(t, j >> (ffz(j) + 1));
+
+	BUG_ON(m < l || m > r);
+	BUG_ON(bkey_next(p) != m);
+
+	if (KEY_INODE(l) != KEY_INODE(r))
+		f->exponent = fls64(KEY_INODE(r) ^ KEY_INODE(l)) + 64;
+	else
+		f->exponent = fls64(r->low ^ l->low);
+
+	f->exponent = max_t(int, f->exponent - BKEY_MANTISSA_BITS, 0);
+
+	/*
+	 * Setting f->exponent = 127 flags this node as failed, and causes the
+	 * lookup code to fall back to comparing against the original key.
+	 */
+
+	if (bfloat_mantissa(m, f) != bfloat_mantissa(p, f))
+		f->mantissa = bfloat_mantissa(m, f) - 1;
+	else
+		f->exponent = 127;
+}
+
+static void bset_alloc_tree(struct btree *b, struct bset_tree *t)
+{
+	if (t != b->sets) {
+		unsigned j = roundup(t[-1].size,
+				     64 / sizeof(struct bkey_float));
+
+		t->tree = t[-1].tree + j;
+		t->prev = t[-1].prev + j;
+	}
+
+	while (t < b->sets + MAX_BSETS)
+		t++->size = 0;
+}
+
+static void bset_build_unwritten_tree(struct btree *b)
+{
+	struct bset_tree *t = b->sets + b->nsets;
+
+	bset_alloc_tree(b, t);
+
+	if (t->tree != b->sets->tree + bset_tree_space(b)) {
+		t->prev[0] = bkey_to_cacheline_offset(t->data->start);
+		t->size = 1;
+	}
+}
+
+static void bset_build_written_tree(struct btree *b)
+{
+	struct bset_tree *t = b->sets + b->nsets;
+	struct bkey *k = t->data->start;
+	unsigned j, cacheline = 1;
+
+	bset_alloc_tree(b, t);
+
+	t->size = min_t(unsigned,
+			bkey_to_cacheline(t, end(t->data)),
+			b->sets->tree + bset_tree_space(b) - t->tree);
+
+	if (t->size < 2) {
+		t->size = 0;
+		return;
+	}
+
+	t->extra = (t->size - rounddown_pow_of_two(t->size - 1)) << 1;
+
+	/* First we figure out where the first key in each cacheline is */
+	for (j = inorder_next(0, t->size);
+	     j;
+	     j = inorder_next(j, t->size)) {
+		while (bkey_to_cacheline(t, k) != cacheline)
+			k = bkey_next(k);
+
+		t->prev[j] = bkey_u64s(k);
+		k = bkey_next(k);
+		cacheline++;
+		t->tree[j].m = bkey_to_cacheline_offset(k);
+	}
+
+	while (bkey_next(k) != end(t->data))
+		k = bkey_next(k);
+
+	t->end = *k;
+
+	/* Then we build the tree */
+	for (j = inorder_next(0, t->size);
+	     j;
+	     j = inorder_next(j, t->size))
+		make_bfloat(t, j);
+}
+
+void bch_bset_fix_invalidated_key(struct btree *b, struct bkey *k)
+{
+	struct bset_tree *t;
+	unsigned inorder, j = 1;
+
+	for (t = b->sets; t <= &b->sets[b->nsets]; t++)
+		if (k < end(t->data))
+			goto found_set;
+
+	BUG();
+found_set:
+	if (!t->size || !bset_written(b, t))
+		return;
+
+	inorder = bkey_to_cacheline(t, k);
+
+	if (k == t->data->start)
+		goto fix_left;
+
+	if (bkey_next(k) == end(t->data)) {
+		t->end = *k;
+		goto fix_right;
+	}
+
+	j = inorder_to_tree(inorder, t);
+
+	if (j &&
+	    j < t->size &&
+	    k == tree_to_bkey(t, j))
+fix_left:	do {
+			make_bfloat(t, j);
+			j = j * 2;
+		} while (j < t->size);
+
+	j = inorder_to_tree(inorder + 1, t);
+
+	if (j &&
+	    j < t->size &&
+	    k == tree_to_prev_bkey(t, j))
+fix_right:	do {
+			make_bfloat(t, j);
+			j = j * 2 + 1;
+		} while (j < t->size);
+}
+
+void bch_bset_fix_lookup_table(struct btree *b, struct bkey *k)
+{
+	struct bset_tree *t = &b->sets[b->nsets];
+	unsigned shift = bkey_u64s(k);
+	unsigned j = bkey_to_cacheline(t, k);
+
+	/* We're getting called from btree_split() or btree_gc, just bail out */
+	if (!t->size)
+		return;
+
+	/* k is the key we just inserted; we need to find the entry in the
+	 * lookup table for the first key that is strictly greater than k:
+	 * it's either k's cacheline or the next one
+	 */
+	if (j < t->size &&
+	    table_to_bkey(t, j) <= k)
+		j++;
+
+	/* Adjust all the lookup table entries, and find a new key for any that
+	 * have gotten too big
+	 */
+	for (; j < t->size; j++) {
+		t->prev[j] += shift;
+
+		if (t->prev[j] > 7) {
+			k = table_to_bkey(t, j - 1);
+
+			while (k < cacheline_to_bkey(t, j, 0))
+				k = bkey_next(k);
+
+			t->prev[j] = bkey_to_cacheline_offset(k);
+		}
+	}
+
+	if (t->size == b->sets->tree + bset_tree_space(b) - t->tree)
+		return;
+
+	/* Possibly add a new entry to the end of the lookup table */
+
+	for (k = table_to_bkey(t, t->size - 1);
+	     k != end(t->data);
+	     k = bkey_next(k))
+		if (t->size == bkey_to_cacheline(t, k)) {
+			t->prev[t->size] = bkey_to_cacheline_offset(k);
+			t->size++;
+		}
+}
+
+void bch_bset_init_next(struct btree *b)
+{
+	struct bset *i = write_block(b);
+
+	if (i != b->sets[0].data) {
+		b->sets[++b->nsets].data = i;
+		i->seq = b->sets[0].data->seq;
+	} else
+		get_random_bytes(&i->seq, sizeof(uint64_t));
+
+	i->magic	= bset_magic(b->c);
+	i->version	= 0;
+	i->keys		= 0;
+
+	bset_build_unwritten_tree(b);
+}
+
+struct bset_search_iter {
+	struct bkey *l, *r;
+};
+
+static struct bset_search_iter bset_search_write_set(struct btree *b,
+						     struct bset_tree *t,
+						     const struct bkey *search)
+{
+	unsigned li = 0, ri = t->size;
+
+	BUG_ON(!b->nsets &&
+	       t->size < bkey_to_cacheline(t, end(t->data)));
+
+	while (li + 1 != ri) {
+		unsigned m = (li + ri) >> 1;
+
+		if (bkey_cmp(table_to_bkey(t, m), search) > 0)
+			ri = m;
+		else
+			li = m;
+	}
+
+	return (struct bset_search_iter) {
+		table_to_bkey(t, li),
+		ri < t->size ? table_to_bkey(t, ri) : end(t->data)
+	};
+}
+
+static struct bset_search_iter bset_search_tree(struct btree *b,
+						struct bset_tree *t,
+						const struct bkey *search)
+{
+	struct bkey *l, *r;
+	struct bkey_float *f;
+	unsigned inorder, j, n = 1;
+
+	do {
+		unsigned p = n << 4;
+		p &= ((int) (p - t->size)) >> 31;
+
+		prefetch(&t->tree[p]);
+
+		j = n;
+		f = &t->tree[j];
+
+		/*
+		 * n = (f->mantissa > bfloat_mantissa())
+		 *	? j * 2
+		 *	: j * 2 + 1;
+		 *
+		 * We need to subtract 1 from f->mantissa for the sign bit trick
+		 * to work  - that's done in make_bfloat()
+		 */
+		if (likely(f->exponent != 127))
+			n = j * 2 + (((unsigned)
+				      (f->mantissa -
+				       bfloat_mantissa(search, f))) >> 31);
+		else
+			n = (bkey_cmp(tree_to_bkey(t, j), search) > 0)
+				? j * 2
+				: j * 2 + 1;
+	} while (n < t->size);
+
+	inorder = to_inorder(j, t);
+
+	/*
+	 * n would have been the node we recursed to - the low bit tells us if
+	 * we recursed left or recursed right.
+	 */
+	if (n & 1) {
+		l = cacheline_to_bkey(t, inorder, f->m);
+
+		if (++inorder != t->size) {
+			f = &t->tree[inorder_next(j, t->size)];
+			r = cacheline_to_bkey(t, inorder, f->m);
+		} else
+			r = end(t->data);
+	} else {
+		r = cacheline_to_bkey(t, inorder, f->m);
+
+		if (--inorder) {
+			f = &t->tree[inorder_prev(j, t->size)];
+			l = cacheline_to_bkey(t, inorder, f->m);
+		} else
+			l = t->data->start;
+	}
+
+	return (struct bset_search_iter) {l, r};
+}
+
+struct bkey *__bch_bset_search(struct btree *b, struct bset_tree *t,
+			       const struct bkey *search)
+{
+	struct bset_search_iter i;
+
+	/*
+	 * First, we search for a cacheline, then lastly we do a linear search
+	 * within that cacheline.
+	 *
+	 * To search for the cacheline, there's three different possibilities:
+	 *  * The set is too small to have a search tree, so we just do a linear
+	 *    search over the whole set.
+	 *  * The set is the one we're currently inserting into; keeping a full
+	 *    auxiliary search tree up to date would be too expensive, so we
+	 *    use a much simpler lookup table to do a binary search -
+	 *    bset_search_write_set().
+	 *  * Or we use the auxiliary search tree we constructed earlier -
+	 *    bset_search_tree()
+	 */
+
+	if (unlikely(!t->size)) {
+		i.l = t->data->start;
+		i.r = end(t->data);
+	} else if (bset_written(b, t)) {
+		/*
+		 * Each node in the auxiliary search tree covers a certain range
+		 * of bits, and keys above and below the set it covers might
+		 * differ outside those bits - so we have to special case the
+		 * start and end - handle that here:
+		 */
+
+		if (unlikely(bkey_cmp(search, &t->end) >= 0))
+			return end(t->data);
+
+		if (unlikely(bkey_cmp(search, t->data->start) < 0))
+			return t->data->start;
+
+		i = bset_search_tree(b, t, search);
+	} else
+		i = bset_search_write_set(b, t, search);
+
+#ifdef CONFIG_BCACHE_EDEBUG
+	BUG_ON(bset_written(b, t) &&
+	       i.l != t->data->start &&
+	       bkey_cmp(tree_to_prev_bkey(t,
+		  inorder_to_tree(bkey_to_cacheline(t, i.l), t)),
+			search) > 0);
+
+	BUG_ON(i.r != end(t->data) &&
+	       bkey_cmp(i.r, search) <= 0);
+#endif
+
+	while (likely(i.l != i.r) &&
+	       bkey_cmp(i.l, search) <= 0)
+		i.l = bkey_next(i.l);
+
+	return i.l;
+}
+
+/* Btree iterator */
+
+static inline bool btree_iter_cmp(struct btree_iter_set l,
+				  struct btree_iter_set r)
+{
+	int64_t c = bkey_cmp(&START_KEY(l.k), &START_KEY(r.k));
+
+	return c ? c > 0 : l.k < r.k;
+}
+
+static inline bool btree_iter_end(struct btree_iter *iter)
+{
+	return !iter->used;
+}
+
+void bch_btree_iter_push(struct btree_iter *iter, struct bkey *k,
+			 struct bkey *end)
+{
+	if (k != end)
+		BUG_ON(!heap_add(iter,
+				 ((struct btree_iter_set) { k, end }),
+				 btree_iter_cmp));
+}
+
+struct bkey *__bch_btree_iter_init(struct btree *b, struct btree_iter *iter,
+			       struct bkey *search, struct bset_tree *start)
+{
+	struct bkey *ret = NULL;
+	iter->size = ARRAY_SIZE(iter->data);
+	iter->used = 0;
+
+	for (; start <= &b->sets[b->nsets]; start++) {
+		ret = bch_bset_search(b, start, search);
+		bch_btree_iter_push(iter, ret, end(start->data));
+	}
+
+	return ret;
+}
+
+struct bkey *bch_btree_iter_next(struct btree_iter *iter)
+{
+	struct btree_iter_set unused;
+	struct bkey *ret = NULL;
+
+	if (!btree_iter_end(iter)) {
+		ret = iter->data->k;
+		iter->data->k = bkey_next(iter->data->k);
+
+		if (iter->data->k > iter->data->end) {
+			WARN_ONCE(1, "bset was corrupt!\n");
+			iter->data->k = iter->data->end;
+		}
+
+		if (iter->data->k == iter->data->end)
+			heap_pop(iter, unused, btree_iter_cmp);
+		else
+			heap_sift(iter, 0, btree_iter_cmp);
+	}
+
+	return ret;
+}
+
+struct bkey *bch_btree_iter_next_filter(struct btree_iter *iter,
+					struct btree *b, ptr_filter_fn fn)
+{
+	struct bkey *ret;
+
+	do {
+		ret = bch_btree_iter_next(iter);
+	} while (ret && fn(b, ret));
+
+	return ret;
+}
+
+struct bkey *bch_next_recurse_key(struct btree *b, struct bkey *search)
+{
+	struct btree_iter iter;
+
+	bch_btree_iter_init(b, &iter, search);
+	return bch_btree_iter_next_filter(&iter, b, bch_ptr_bad);
+}
+
+/* Mergesort */
+
+static void btree_sort_fixup(struct btree_iter *iter)
+{
+	while (iter->used > 1) {
+		struct btree_iter_set *top = iter->data, *i = top + 1;
+		struct bkey *k;
+
+		if (iter->used > 2 &&
+		    btree_iter_cmp(i[0], i[1]))
+			i++;
+
+		for (k = i->k;
+		     k != i->end && bkey_cmp(top->k, &START_KEY(k)) > 0;
+		     k = bkey_next(k))
+			if (top->k > i->k)
+				__bch_cut_front(top->k, k);
+			else if (KEY_SIZE(k))
+				bch_cut_back(&START_KEY(k), top->k);
+
+		if (top->k < i->k || k == i->k)
+			break;
+
+		heap_sift(iter, i - top, btree_iter_cmp);
+	}
+}
+
+static void btree_mergesort(struct btree *b, struct bset *out,
+			    struct btree_iter *iter,
+			    bool fixup, bool remove_stale)
+{
+	struct bkey *k, *last = NULL;
+	bool (*bad)(struct btree *, const struct bkey *) = remove_stale
+		? bch_ptr_bad
+		: bch_ptr_invalid;
+
+	while (!btree_iter_end(iter)) {
+		if (fixup && !b->level)
+			btree_sort_fixup(iter);
+
+		k = bch_btree_iter_next(iter);
+		if (bad(b, k))
+			continue;
+
+		if (!last) {
+			last = out->start;
+			bkey_copy(last, k);
+		} else if (b->level ||
+			   !bch_bkey_try_merge(b, last, k)) {
+			last = bkey_next(last);
+			bkey_copy(last, k);
+		}
+	}
+
+	out->keys = last ? (uint64_t *) bkey_next(last) - out->d : 0;
+
+	pr_debug("sorted %i keys", out->keys);
+	bch_check_key_order(b, out);
+}
+
+static void __btree_sort(struct btree *b, struct btree_iter *iter,
+			 unsigned start, unsigned order, bool fixup)
+{
+	uint64_t start_time;
+	bool remove_stale = !b->written;
+	struct bset *out = (void *) __get_free_pages(__GFP_NOWARN|GFP_NOIO,
+						     order);
+	if (!out) {
+		mutex_lock(&b->c->sort_lock);
+		out = b->c->sort;
+		order = ilog2(bucket_pages(b->c));
+	}
+
+	start_time = local_clock();
+
+	btree_mergesort(b, out, iter, fixup, remove_stale);
+	b->nsets = start;
+
+	if (!fixup && !start && b->written)
+		bch_btree_verify(b, out);
+
+	if (!start && order == b->page_order) {
+		/*
+		 * Our temporary buffer is the same size as the btree node's
+		 * buffer, we can just swap buffers instead of doing a big
+		 * memcpy()
+		 */
+
+		out->magic	= bset_magic(b->c);
+		out->seq	= b->sets[0].data->seq;
+		out->version	= b->sets[0].data->version;
+		swap(out, b->sets[0].data);
+
+		if (b->c->sort == b->sets[0].data)
+			b->c->sort = out;
+	} else {
+		b->sets[start].data->keys = out->keys;
+		memcpy(b->sets[start].data->start, out->start,
+		       (void *) end(out) - (void *) out->start);
+	}
+
+	if (out == b->c->sort)
+		mutex_unlock(&b->c->sort_lock);
+	else
+		free_pages((unsigned long) out, order);
+
+	if (b->written)
+		bset_build_written_tree(b);
+
+	if (!start) {
+		spin_lock(&b->c->sort_time_lock);
+		bch_time_stats_update(&b->c->sort_time, start_time);
+		spin_unlock(&b->c->sort_time_lock);
+	}
+}
+
+void bch_btree_sort_partial(struct btree *b, unsigned start)
+{
+	size_t oldsize = 0, order = b->page_order, keys = 0;
+	struct btree_iter iter;
+	__bch_btree_iter_init(b, &iter, NULL, &b->sets[start]);
+
+	BUG_ON(b->sets[b->nsets].data == write_block(b) &&
+	       (b->sets[b->nsets].size || b->nsets));
+
+	if (b->written)
+		oldsize = bch_count_data(b);
+
+	if (start) {
+		unsigned i;
+
+		for (i = start; i <= b->nsets; i++)
+			keys += b->sets[i].data->keys;
+
+		order = roundup_pow_of_two(__set_bytes(b->sets->data,
+						       keys)) / PAGE_SIZE;
+		if (order)
+			order = ilog2(order);
+	}
+
+	__btree_sort(b, &iter, start, order, false);
+
+	EBUG_ON(b->written && bch_count_data(b) != oldsize);
+}
+
+void bch_btree_sort_and_fix_extents(struct btree *b, struct btree_iter *iter)
+{
+	BUG_ON(!b->written);
+	__btree_sort(b, iter, 0, b->page_order, true);
+}
+
+void bch_btree_sort_into(struct btree *b, struct btree *new)
+{
+	uint64_t start_time = local_clock();
+
+	struct btree_iter iter;
+	bch_btree_iter_init(b, &iter, NULL);
+
+	btree_mergesort(b, new->sets->data, &iter, false, true);
+
+	spin_lock(&b->c->sort_time_lock);
+	bch_time_stats_update(&b->c->sort_time, start_time);
+	spin_unlock(&b->c->sort_time_lock);
+
+	bkey_copy_key(&new->key, &b->key);
+	new->sets->size = 0;
+}
+
+void bch_btree_sort_lazy(struct btree *b)
+{
+	if (b->nsets) {
+		unsigned i, j, keys = 0, total;
+
+		for (i = 0; i <= b->nsets; i++)
+			keys += b->sets[i].data->keys;
+
+		total = keys;
+
+		for (j = 0; j < b->nsets; j++) {
+			if (keys * 2 < total ||
+			    keys < 1000) {
+				bch_btree_sort_partial(b, j);
+				return;
+			}
+
+			keys -= b->sets[j].data->keys;
+		}
+
+		/* Must sort if b->nsets == 3 or we'll overflow */
+		if (b->nsets >= (MAX_BSETS - 1) - b->level) {
+			bch_btree_sort(b);
+			return;
+		}
+	}
+
+	bset_build_written_tree(b);
+}
+
+/* Sysfs stuff */
+
+struct bset_stats {
+	size_t nodes;
+	size_t sets_written, sets_unwritten;
+	size_t bytes_written, bytes_unwritten;
+	size_t floats, failed;
+};
+
+static int bch_btree_bset_stats(struct btree *b, struct btree_op *op,
+			    struct bset_stats *stats)
+{
+	struct bkey *k;
+	unsigned i;
+
+	stats->nodes++;
+
+	for (i = 0; i <= b->nsets; i++) {
+		struct bset_tree *t = &b->sets[i];
+		size_t bytes = t->data->keys * sizeof(uint64_t);
+		size_t j;
+
+		if (bset_written(b, t)) {
+			stats->sets_written++;
+			stats->bytes_written += bytes;
+
+			stats->floats += t->size - 1;
+
+			for (j = 1; j < t->size; j++)
+				if (t->tree[j].exponent == 127)
+					stats->failed++;
+		} else {
+			stats->sets_unwritten++;
+			stats->bytes_unwritten += bytes;
+		}
+	}
+
+	if (b->level) {
+		struct btree_iter iter;
+
+		for_each_key_filter(b, k, &iter, bch_ptr_bad) {
+			int ret = btree(bset_stats, k, b, op, stats);
+			if (ret)
+				return ret;
+		}
+	}
+
+	return 0;
+}
+
+int bch_bset_print_stats(struct cache_set *c, char *buf)
+{
+	struct btree_op op;
+	struct bset_stats t;
+	int ret;
+
+	bch_btree_op_init_stack(&op);
+	memset(&t, 0, sizeof(struct bset_stats));
+
+	ret = btree_root(bset_stats, c, &op, &t);
+	if (ret)
+		return ret;
+
+	return snprintf(buf, PAGE_SIZE,
+			"btree nodes:		%zu\n"
+			"written sets:		%zu\n"
+			"unwritten sets:		%zu\n"
+			"written key bytes:	%zu\n"
+			"unwritten key bytes:	%zu\n"
+			"floats:			%zu\n"
+			"failed:			%zu\n",
+			t.nodes,
+			t.sets_written, t.sets_unwritten,
+			t.bytes_written, t.bytes_unwritten,
+			t.floats, t.failed);
+}
diff --git a/drivers/md/bcache/bset.h b/drivers/md/bcache/bset.h
new file mode 100644
index 000000000000..57a9cff41546
--- /dev/null
+++ b/drivers/md/bcache/bset.h
@@ -0,0 +1,379 @@
+#ifndef _BCACHE_BSET_H
+#define _BCACHE_BSET_H
+
+/*
+ * BKEYS:
+ *
+ * A bkey contains a key, a size field, a variable number of pointers, and some
+ * ancillary flag bits.
+ *
+ * We use two different functions for validating bkeys, bch_ptr_invalid and
+ * bch_ptr_bad().
+ *
+ * bch_ptr_invalid() primarily filters out keys and pointers that would be
+ * invalid due to some sort of bug, whereas bch_ptr_bad() filters out keys and
+ * pointer that occur in normal practice but don't point to real data.
+ *
+ * The one exception to the rule that ptr_invalid() filters out invalid keys is
+ * that it also filters out keys of size 0 - these are keys that have been
+ * completely overwritten. It'd be safe to delete these in memory while leaving
+ * them on disk, just unnecessary work - so we filter them out when resorting
+ * instead.
+ *
+ * We can't filter out stale keys when we're resorting, because garbage
+ * collection needs to find them to ensure bucket gens don't wrap around -
+ * unless we're rewriting the btree node those stale keys still exist on disk.
+ *
+ * We also implement functions here for removing some number of sectors from the
+ * front or the back of a bkey - this is mainly used for fixing overlapping
+ * extents, by removing the overlapping sectors from the older key.
+ *
+ * BSETS:
+ *
+ * A bset is an array of bkeys laid out contiguously in memory in sorted order,
+ * along with a header. A btree node is made up of a number of these, written at
+ * different times.
+ *
+ * There could be many of them on disk, but we never allow there to be more than
+ * 4 in memory - we lazily resort as needed.
+ *
+ * We implement code here for creating and maintaining auxiliary search trees
+ * (described below) for searching an individial bset, and on top of that we
+ * implement a btree iterator.
+ *
+ * BTREE ITERATOR:
+ *
+ * Most of the code in bcache doesn't care about an individual bset - it needs
+ * to search entire btree nodes and iterate over them in sorted order.
+ *
+ * The btree iterator code serves both functions; it iterates through the keys
+ * in a btree node in sorted order, starting from either keys after a specific
+ * point (if you pass it a search key) or the start of the btree node.
+ *
+ * AUXILIARY SEARCH TREES:
+ *
+ * Since keys are variable length, we can't use a binary search on a bset - we
+ * wouldn't be able to find the start of the next key. But binary searches are
+ * slow anyways, due to terrible cache behaviour; bcache originally used binary
+ * searches and that code topped out at under 50k lookups/second.
+ *
+ * So we need to construct some sort of lookup table. Since we only insert keys
+ * into the last (unwritten) set, most of the keys within a given btree node are
+ * usually in sets that are mostly constant. We use two different types of
+ * lookup tables to take advantage of this.
+ *
+ * Both lookup tables share in common that they don't index every key in the
+ * set; they index one key every BSET_CACHELINE bytes, and then a linear search
+ * is used for the rest.
+ *
+ * For sets that have been written to disk and are no longer being inserted
+ * into, we construct a binary search tree in an array - traversing a binary
+ * search tree in an array gives excellent locality of reference and is very
+ * fast, since both children of any node are adjacent to each other in memory
+ * (and their grandchildren, and great grandchildren...) - this means
+ * prefetching can be used to great effect.
+ *
+ * It's quite useful performance wise to keep these nodes small - not just
+ * because they're more likely to be in L2, but also because we can prefetch
+ * more nodes on a single cacheline and thus prefetch more iterations in advance
+ * when traversing this tree.
+ *
+ * Nodes in the auxiliary search tree must contain both a key to compare against
+ * (we don't want to fetch the key from the set, that would defeat the purpose),
+ * and a pointer to the key. We use a few tricks to compress both of these.
+ *
+ * To compress the pointer, we take advantage of the fact that one node in the
+ * search tree corresponds to precisely BSET_CACHELINE bytes in the set. We have
+ * a function (to_inorder()) that takes the index of a node in a binary tree and
+ * returns what its index would be in an inorder traversal, so we only have to
+ * store the low bits of the offset.
+ *
+ * The key is 84 bits (KEY_DEV + key->key, the offset on the device). To
+ * compress that,  we take advantage of the fact that when we're traversing the
+ * search tree at every iteration we know that both our search key and the key
+ * we're looking for lie within some range - bounded by our previous
+ * comparisons. (We special case the start of a search so that this is true even
+ * at the root of the tree).
+ *
+ * So we know the key we're looking for is between a and b, and a and b don't
+ * differ higher than bit 50, we don't need to check anything higher than bit
+ * 50.
+ *
+ * We don't usually need the rest of the bits, either; we only need enough bits
+ * to partition the key range we're currently checking.  Consider key n - the
+ * key our auxiliary search tree node corresponds to, and key p, the key
+ * immediately preceding n.  The lowest bit we need to store in the auxiliary
+ * search tree is the highest bit that differs between n and p.
+ *
+ * Note that this could be bit 0 - we might sometimes need all 80 bits to do the
+ * comparison. But we'd really like our nodes in the auxiliary search tree to be
+ * of fixed size.
+ *
+ * The solution is to make them fixed size, and when we're constructing a node
+ * check if p and n differed in the bits we needed them to. If they don't we
+ * flag that node, and when doing lookups we fallback to comparing against the
+ * real key. As long as this doesn't happen to often (and it seems to reliably
+ * happen a bit less than 1% of the time), we win - even on failures, that key
+ * is then more likely to be in cache than if we were doing binary searches all
+ * the way, since we're touching so much less memory.
+ *
+ * The keys in the auxiliary search tree are stored in (software) floating
+ * point, with an exponent and a mantissa. The exponent needs to be big enough
+ * to address all the bits in the original key, but the number of bits in the
+ * mantissa is somewhat arbitrary; more bits just gets us fewer failures.
+ *
+ * We need 7 bits for the exponent and 3 bits for the key's offset (since keys
+ * are 8 byte aligned); using 22 bits for the mantissa means a node is 4 bytes.
+ * We need one node per 128 bytes in the btree node, which means the auxiliary
+ * search trees take up 3% as much memory as the btree itself.
+ *
+ * Constructing these auxiliary search trees is moderately expensive, and we
+ * don't want to be constantly rebuilding the search tree for the last set
+ * whenever we insert another key into it. For the unwritten set, we use a much
+ * simpler lookup table - it's just a flat array, so index i in the lookup table
+ * corresponds to the i range of BSET_CACHELINE bytes in the set. Indexing
+ * within each byte range works the same as with the auxiliary search trees.
+ *
+ * These are much easier to keep up to date when we insert a key - we do it
+ * somewhat lazily; when we shift a key up we usually just increment the pointer
+ * to it, only when it would overflow do we go to the trouble of finding the
+ * first key in that range of bytes again.
+ */
+
+/* Btree key comparison/iteration */
+
+struct btree_iter {
+	size_t size, used;
+	struct btree_iter_set {
+		struct bkey *k, *end;
+	} data[MAX_BSETS];
+};
+
+struct bset_tree {
+	/*
+	 * We construct a binary tree in an array as if the array
+	 * started at 1, so that things line up on the same cachelines
+	 * better: see comments in bset.c at cacheline_to_bkey() for
+	 * details
+	 */
+
+	/* size of the binary tree and prev array */
+	unsigned	size;
+
+	/* function of size - precalculated for to_inorder() */
+	unsigned	extra;
+
+	/* copy of the last key in the set */
+	struct bkey	end;
+	struct bkey_float *tree;
+
+	/*
+	 * The nodes in the bset tree point to specific keys - this
+	 * array holds the sizes of the previous key.
+	 *
+	 * Conceptually it's a member of struct bkey_float, but we want
+	 * to keep bkey_float to 4 bytes and prev isn't used in the fast
+	 * path.
+	 */
+	uint8_t		*prev;
+
+	/* The actual btree node, with pointers to each sorted set */
+	struct bset	*data;
+};
+
+static __always_inline int64_t bkey_cmp(const struct bkey *l,
+					const struct bkey *r)
+{
+	return unlikely(KEY_INODE(l) != KEY_INODE(r))
+		? (int64_t) KEY_INODE(l) - (int64_t) KEY_INODE(r)
+		: (int64_t) KEY_OFFSET(l) - (int64_t) KEY_OFFSET(r);
+}
+
+static inline size_t bkey_u64s(const struct bkey *k)
+{
+	BUG_ON(KEY_CSUM(k) > 1);
+	return 2 + KEY_PTRS(k) + (KEY_CSUM(k) ? 1 : 0);
+}
+
+static inline size_t bkey_bytes(const struct bkey *k)
+{
+	return bkey_u64s(k) * sizeof(uint64_t);
+}
+
+static inline void bkey_copy(struct bkey *dest, const struct bkey *src)
+{
+	memcpy(dest, src, bkey_bytes(src));
+}
+
+static inline void bkey_copy_key(struct bkey *dest, const struct bkey *src)
+{
+	if (!src)
+		src = &KEY(0, 0, 0);
+
+	SET_KEY_INODE(dest, KEY_INODE(src));
+	SET_KEY_OFFSET(dest, KEY_OFFSET(src));
+}
+
+static inline struct bkey *bkey_next(const struct bkey *k)
+{
+	uint64_t *d = (void *) k;
+	return (struct bkey *) (d + bkey_u64s(k));
+}
+
+/* Keylists */
+
+struct keylist {
+	struct bkey		*top;
+	union {
+		uint64_t		*list;
+		struct bkey		*bottom;
+	};
+
+	/* Enough room for btree_split's keys without realloc */
+#define KEYLIST_INLINE		16
+	uint64_t		d[KEYLIST_INLINE];
+};
+
+static inline void bch_keylist_init(struct keylist *l)
+{
+	l->top = (void *) (l->list = l->d);
+}
+
+static inline void bch_keylist_push(struct keylist *l)
+{
+	l->top = bkey_next(l->top);
+}
+
+static inline void bch_keylist_add(struct keylist *l, struct bkey *k)
+{
+	bkey_copy(l->top, k);
+	bch_keylist_push(l);
+}
+
+static inline bool bch_keylist_empty(struct keylist *l)
+{
+	return l->top == (void *) l->list;
+}
+
+static inline void bch_keylist_free(struct keylist *l)
+{
+	if (l->list != l->d)
+		kfree(l->list);
+}
+
+void bch_keylist_copy(struct keylist *, struct keylist *);
+struct bkey *bch_keylist_pop(struct keylist *);
+int bch_keylist_realloc(struct keylist *, int, struct cache_set *);
+
+void bch_bkey_copy_single_ptr(struct bkey *, const struct bkey *,
+			      unsigned);
+bool __bch_cut_front(const struct bkey *, struct bkey *);
+bool __bch_cut_back(const struct bkey *, struct bkey *);
+
+static inline bool bch_cut_front(const struct bkey *where, struct bkey *k)
+{
+	BUG_ON(bkey_cmp(where, k) > 0);
+	return __bch_cut_front(where, k);
+}
+
+static inline bool bch_cut_back(const struct bkey *where, struct bkey *k)
+{
+	BUG_ON(bkey_cmp(where, &START_KEY(k)) < 0);
+	return __bch_cut_back(where, k);
+}
+
+const char *bch_ptr_status(struct cache_set *, const struct bkey *);
+bool __bch_ptr_invalid(struct cache_set *, int level, const struct bkey *);
+bool bch_ptr_bad(struct btree *, const struct bkey *);
+
+static inline uint8_t gen_after(uint8_t a, uint8_t b)
+{
+	uint8_t r = a - b;
+	return r > 128U ? 0 : r;
+}
+
+static inline uint8_t ptr_stale(struct cache_set *c, const struct bkey *k,
+				unsigned i)
+{
+	return gen_after(PTR_BUCKET(c, k, i)->gen, PTR_GEN(k, i));
+}
+
+static inline bool ptr_available(struct cache_set *c, const struct bkey *k,
+				 unsigned i)
+{
+	return (PTR_DEV(k, i) < MAX_CACHES_PER_SET) && PTR_CACHE(c, k, i);
+}
+
+
+typedef bool (*ptr_filter_fn)(struct btree *, const struct bkey *);
+
+struct bkey *bch_next_recurse_key(struct btree *, struct bkey *);
+struct bkey *bch_btree_iter_next(struct btree_iter *);
+struct bkey *bch_btree_iter_next_filter(struct btree_iter *,
+					struct btree *, ptr_filter_fn);
+
+void bch_btree_iter_push(struct btree_iter *, struct bkey *, struct bkey *);
+struct bkey *__bch_btree_iter_init(struct btree *, struct btree_iter *,
+				   struct bkey *, struct bset_tree *);
+
+/* 32 bits total: */
+#define BKEY_MID_BITS		3
+#define BKEY_EXPONENT_BITS	7
+#define BKEY_MANTISSA_BITS	22
+#define BKEY_MANTISSA_MASK	((1 << BKEY_MANTISSA_BITS) - 1)
+
+struct bkey_float {
+	unsigned	exponent:BKEY_EXPONENT_BITS;
+	unsigned	m:BKEY_MID_BITS;
+	unsigned	mantissa:BKEY_MANTISSA_BITS;
+} __packed;
+
+/*
+ * BSET_CACHELINE was originally intended to match the hardware cacheline size -
+ * it used to be 64, but I realized the lookup code would touch slightly less
+ * memory if it was 128.
+ *
+ * It definites the number of bytes (in struct bset) per struct bkey_float in
+ * the auxiliar search tree - when we're done searching the bset_float tree we
+ * have this many bytes left that we do a linear search over.
+ *
+ * Since (after level 5) every level of the bset_tree is on a new cacheline,
+ * we're touching one fewer cacheline in the bset tree in exchange for one more
+ * cacheline in the linear search - but the linear search might stop before it
+ * gets to the second cacheline.
+ */
+
+#define BSET_CACHELINE		128
+#define bset_tree_space(b)	(btree_data_space(b) / BSET_CACHELINE)
+
+#define bset_tree_bytes(b)	(bset_tree_space(b) * sizeof(struct bkey_float))
+#define bset_prev_bytes(b)	(bset_tree_space(b) * sizeof(uint8_t))
+
+void bch_bset_init_next(struct btree *);
+
+void bch_bset_fix_invalidated_key(struct btree *, struct bkey *);
+void bch_bset_fix_lookup_table(struct btree *, struct bkey *);
+
+struct bkey *__bch_bset_search(struct btree *, struct bset_tree *,
+			   const struct bkey *);
+
+static inline struct bkey *bch_bset_search(struct btree *b, struct bset_tree *t,
+					   const struct bkey *search)
+{
+	return search ? __bch_bset_search(b, t, search) : t->data->start;
+}
+
+bool bch_bkey_try_merge(struct btree *, struct bkey *, struct bkey *);
+void bch_btree_sort_lazy(struct btree *);
+void bch_btree_sort_into(struct btree *, struct btree *);
+void bch_btree_sort_and_fix_extents(struct btree *, struct btree_iter *);
+void bch_btree_sort_partial(struct btree *, unsigned);
+
+static inline void bch_btree_sort(struct btree *b)
+{
+	bch_btree_sort_partial(b, 0);
+}
+
+int bch_bset_print_stats(struct cache_set *, char *);
+
+#endif
diff --git a/drivers/md/bcache/btree.c b/drivers/md/bcache/btree.c
new file mode 100644
index 000000000000..7a5658f04e62
--- /dev/null
+++ b/drivers/md/bcache/btree.c
@@ -0,0 +1,2503 @@
+/*
+ * Copyright (C) 2010 Kent Overstreet <kent.overstreet@gmail.com>
+ *
+ * Uses a block device as cache for other block devices; optimized for SSDs.
+ * All allocation is done in buckets, which should match the erase block size
+ * of the device.
+ *
+ * Buckets containing cached data are kept on a heap sorted by priority;
+ * bucket priority is increased on cache hit, and periodically all the buckets
+ * on the heap have their priority scaled down. This currently is just used as
+ * an LRU but in the future should allow for more intelligent heuristics.
+ *
+ * Buckets have an 8 bit counter; freeing is accomplished by incrementing the
+ * counter. Garbage collection is used to remove stale pointers.
+ *
+ * Indexing is done via a btree; nodes are not necessarily fully sorted, rather
+ * as keys are inserted we only sort the pages that have not yet been written.
+ * When garbage collection is run, we resort the entire node.
+ *
+ * All configuration is done via sysfs; see Documentation/bcache.txt.
+ */
+
+#include "bcache.h"
+#include "btree.h"
+#include "debug.h"
+#include "request.h"
+
+#include <linux/slab.h>
+#include <linux/bitops.h>
+#include <linux/hash.h>
+#include <linux/prefetch.h>
+#include <linux/random.h>
+#include <linux/rcupdate.h>
+#include <trace/events/bcache.h>
+
+/*
+ * Todo:
+ * register_bcache: Return errors out to userspace correctly
+ *
+ * Writeback: don't undirty key until after a cache flush
+ *
+ * Create an iterator for key pointers
+ *
+ * On btree write error, mark bucket such that it won't be freed from the cache
+ *
+ * Journalling:
+ *   Check for bad keys in replay
+ *   Propagate barriers
+ *   Refcount journal entries in journal_replay
+ *
+ * Garbage collection:
+ *   Finish incremental gc
+ *   Gc should free old UUIDs, data for invalid UUIDs
+ *
+ * Provide a way to list backing device UUIDs we have data cached for, and
+ * probably how long it's been since we've seen them, and a way to invalidate
+ * dirty data for devices that will never be attached again
+ *
+ * Keep 1 min/5 min/15 min statistics of how busy a block device has been, so
+ * that based on that and how much dirty data we have we can keep writeback
+ * from being starved
+ *
+ * Add a tracepoint or somesuch to watch for writeback starvation
+ *
+ * When btree depth > 1 and splitting an interior node, we have to make sure
+ * alloc_bucket() cannot fail. This should be true but is not completely
+ * obvious.
+ *
+ * Make sure all allocations get charged to the root cgroup
+ *
+ * Plugging?
+ *
+ * If data write is less than hard sector size of ssd, round up offset in open
+ * bucket to the next whole sector
+ *
+ * Also lookup by cgroup in get_open_bucket()
+ *
+ * Superblock needs to be fleshed out for multiple cache devices
+ *
+ * Add a sysfs tunable for the number of writeback IOs in flight
+ *
+ * Add a sysfs tunable for the number of open data buckets
+ *
+ * IO tracking: Can we track when one process is doing io on behalf of another?
+ * IO tracking: Don't use just an average, weigh more recent stuff higher
+ *
+ * Test module load/unload
+ */
+
+static const char * const op_types[] = {
+	"insert", "replace"
+};
+
+static const char *op_type(struct btree_op *op)
+{
+	return op_types[op->type];
+}
+
+#define MAX_NEED_GC		64
+#define MAX_SAVE_PRIO		72
+
+#define PTR_DIRTY_BIT		(((uint64_t) 1 << 36))
+
+#define PTR_HASH(c, k)							\
+	(((k)->ptr[0] >> c->bucket_bits) | PTR_GEN(k, 0))
+
+struct workqueue_struct *bch_gc_wq;
+static struct workqueue_struct *btree_io_wq;
+
+void bch_btree_op_init_stack(struct btree_op *op)
+{
+	memset(op, 0, sizeof(struct btree_op));
+	closure_init_stack(&op->cl);
+	op->lock = -1;
+	bch_keylist_init(&op->keys);
+}
+
+/* Btree key manipulation */
+
+static void bkey_put(struct cache_set *c, struct bkey *k, int level)
+{
+	if ((level && KEY_OFFSET(k)) || !level)
+		__bkey_put(c, k);
+}
+
+/* Btree IO */
+
+static uint64_t btree_csum_set(struct btree *b, struct bset *i)
+{
+	uint64_t crc = b->key.ptr[0];
+	void *data = (void *) i + 8, *end = end(i);
+
+	crc = bch_crc64_update(crc, data, end - data);
+	return crc ^ 0xffffffffffffffffULL;
+}
+
+static void btree_bio_endio(struct bio *bio, int error)
+{
+	struct closure *cl = bio->bi_private;
+	struct btree *b = container_of(cl, struct btree, io.cl);
+
+	if (error)
+		set_btree_node_io_error(b);
+
+	bch_bbio_count_io_errors(b->c, bio, error, (bio->bi_rw & WRITE)
+				 ? "writing btree" : "reading btree");
+	closure_put(cl);
+}
+
+static void btree_bio_init(struct btree *b)
+{
+	BUG_ON(b->bio);
+	b->bio = bch_bbio_alloc(b->c);
+
+	b->bio->bi_end_io	= btree_bio_endio;
+	b->bio->bi_private	= &b->io.cl;
+}
+
+void bch_btree_read_done(struct closure *cl)
+{
+	struct btree *b = container_of(cl, struct btree, io.cl);
+	struct bset *i = b->sets[0].data;
+	struct btree_iter *iter = b->c->fill_iter;
+	const char *err = "bad btree header";
+	BUG_ON(b->nsets || b->written);
+
+	bch_bbio_free(b->bio, b->c);
+	b->bio = NULL;
+
+	mutex_lock(&b->c->fill_lock);
+	iter->used = 0;
+
+	if (btree_node_io_error(b) ||
+	    !i->seq)
+		goto err;
+
+	for (;
+	     b->written < btree_blocks(b) && i->seq == b->sets[0].data->seq;
+	     i = write_block(b)) {
+		err = "unsupported bset version";
+		if (i->version > BCACHE_BSET_VERSION)
+			goto err;
+
+		err = "bad btree header";
+		if (b->written + set_blocks(i, b->c) > btree_blocks(b))
+			goto err;
+
+		err = "bad magic";
+		if (i->magic != bset_magic(b->c))
+			goto err;
+
+		err = "bad checksum";
+		switch (i->version) {
+		case 0:
+			if (i->csum != csum_set(i))
+				goto err;
+			break;
+		case BCACHE_BSET_VERSION:
+			if (i->csum != btree_csum_set(b, i))
+				goto err;
+			break;
+		}
+
+		err = "empty set";
+		if (i != b->sets[0].data && !i->keys)
+			goto err;
+
+		bch_btree_iter_push(iter, i->start, end(i));
+
+		b->written += set_blocks(i, b->c);
+	}
+
+	err = "corrupted btree";
+	for (i = write_block(b);
+	     index(i, b) < btree_blocks(b);
+	     i = ((void *) i) + block_bytes(b->c))
+		if (i->seq == b->sets[0].data->seq)
+			goto err;
+
+	bch_btree_sort_and_fix_extents(b, iter);
+
+	i = b->sets[0].data;
+	err = "short btree key";
+	if (b->sets[0].size &&
+	    bkey_cmp(&b->key, &b->sets[0].end) < 0)
+		goto err;
+
+	if (b->written < btree_blocks(b))
+		bch_bset_init_next(b);
+out:
+
+	mutex_unlock(&b->c->fill_lock);
+
+	spin_lock(&b->c->btree_read_time_lock);
+	bch_time_stats_update(&b->c->btree_read_time, b->io_start_time);
+	spin_unlock(&b->c->btree_read_time_lock);
+
+	smp_wmb(); /* read_done is our write lock */
+	set_btree_node_read_done(b);
+
+	closure_return(cl);
+err:
+	set_btree_node_io_error(b);
+	bch_cache_set_error(b->c, "%s at bucket %zu, block %zu, %u keys",
+			    err, PTR_BUCKET_NR(b->c, &b->key, 0),
+			    index(i, b), i->keys);
+	goto out;
+}
+
+void bch_btree_read(struct btree *b)
+{
+	BUG_ON(b->nsets || b->written);
+
+	if (!closure_trylock(&b->io.cl, &b->c->cl))
+		BUG();
+
+	b->io_start_time = local_clock();
+
+	btree_bio_init(b);
+	b->bio->bi_rw	= REQ_META|READ_SYNC;
+	b->bio->bi_size	= KEY_SIZE(&b->key) << 9;
+
+	bch_bio_map(b->bio, b->sets[0].data);
+
+	pr_debug("%s", pbtree(b));
+	trace_bcache_btree_read(b->bio);
+	bch_submit_bbio(b->bio, b->c, &b->key, 0);
+
+	continue_at(&b->io.cl, bch_btree_read_done, system_wq);
+}
+
+static void btree_complete_write(struct btree *b, struct btree_write *w)
+{
+	if (w->prio_blocked &&
+	    !atomic_sub_return(w->prio_blocked, &b->c->prio_blocked))
+		wake_up(&b->c->alloc_wait);
+
+	if (w->journal) {
+		atomic_dec_bug(w->journal);
+		__closure_wake_up(&b->c->journal.wait);
+	}
+
+	if (w->owner)
+		closure_put(w->owner);
+
+	w->prio_blocked	= 0;
+	w->journal	= NULL;
+	w->owner	= NULL;
+}
+
+static void __btree_write_done(struct closure *cl)
+{
+	struct btree *b = container_of(cl, struct btree, io.cl);
+	struct btree_write *w = btree_prev_write(b);
+
+	bch_bbio_free(b->bio, b->c);
+	b->bio = NULL;
+	btree_complete_write(b, w);
+
+	if (btree_node_dirty(b))
+		queue_delayed_work(btree_io_wq, &b->work,
+				   msecs_to_jiffies(30000));
+
+	closure_return(cl);
+}
+
+static void btree_write_done(struct closure *cl)
+{
+	struct btree *b = container_of(cl, struct btree, io.cl);
+	struct bio_vec *bv;
+	int n;
+
+	__bio_for_each_segment(bv, b->bio, n, 0)
+		__free_page(bv->bv_page);
+
+	__btree_write_done(cl);
+}
+
+static void do_btree_write(struct btree *b)
+{
+	struct closure *cl = &b->io.cl;
+	struct bset *i = b->sets[b->nsets].data;
+	BKEY_PADDED(key) k;
+
+	i->version	= BCACHE_BSET_VERSION;
+	i->csum		= btree_csum_set(b, i);
+
+	btree_bio_init(b);
+	b->bio->bi_rw	= REQ_META|WRITE_SYNC;
+	b->bio->bi_size	= set_blocks(i, b->c) * block_bytes(b->c);
+	bch_bio_map(b->bio, i);
+
+	bkey_copy(&k.key, &b->key);
+	SET_PTR_OFFSET(&k.key, 0, PTR_OFFSET(&k.key, 0) + bset_offset(b, i));
+
+	if (!bch_bio_alloc_pages(b->bio, GFP_NOIO)) {
+		int j;
+		struct bio_vec *bv;
+		void *base = (void *) ((unsigned long) i & ~(PAGE_SIZE - 1));
+
+		bio_for_each_segment(bv, b->bio, j)
+			memcpy(page_address(bv->bv_page),
+			       base + j * PAGE_SIZE, PAGE_SIZE);
+
+		trace_bcache_btree_write(b->bio);
+		bch_submit_bbio(b->bio, b->c, &k.key, 0);
+
+		continue_at(cl, btree_write_done, NULL);
+	} else {
+		b->bio->bi_vcnt = 0;
+		bch_bio_map(b->bio, i);
+
+		trace_bcache_btree_write(b->bio);
+		bch_submit_bbio(b->bio, b->c, &k.key, 0);
+
+		closure_sync(cl);
+		__btree_write_done(cl);
+	}
+}
+
+static void __btree_write(struct btree *b)
+{
+	struct bset *i = b->sets[b->nsets].data;
+
+	BUG_ON(current->bio_list);
+
+	closure_lock(&b->io, &b->c->cl);
+	cancel_delayed_work(&b->work);
+
+	clear_bit(BTREE_NODE_dirty,	 &b->flags);
+	change_bit(BTREE_NODE_write_idx, &b->flags);
+
+	bch_check_key_order(b, i);
+	BUG_ON(b->written && !i->keys);
+
+	do_btree_write(b);
+
+	pr_debug("%s block %i keys %i", pbtree(b), b->written, i->keys);
+
+	b->written += set_blocks(i, b->c);
+	atomic_long_add(set_blocks(i, b->c) * b->c->sb.block_size,
+			&PTR_CACHE(b->c, &b->key, 0)->btree_sectors_written);
+
+	bch_btree_sort_lazy(b);
+
+	if (b->written < btree_blocks(b))
+		bch_bset_init_next(b);
+}
+
+static void btree_write_work(struct work_struct *w)
+{
+	struct btree *b = container_of(to_delayed_work(w), struct btree, work);
+
+	down_write(&b->lock);
+
+	if (btree_node_dirty(b))
+		__btree_write(b);
+	up_write(&b->lock);
+}
+
+void bch_btree_write(struct btree *b, bool now, struct btree_op *op)
+{
+	struct bset *i = b->sets[b->nsets].data;
+	struct btree_write *w = btree_current_write(b);
+
+	BUG_ON(b->written &&
+	       (b->written >= btree_blocks(b) ||
+		i->seq != b->sets[0].data->seq ||
+		!i->keys));
+
+	if (!btree_node_dirty(b)) {
+		set_btree_node_dirty(b);
+		queue_delayed_work(btree_io_wq, &b->work,
+				   msecs_to_jiffies(30000));
+	}
+
+	w->prio_blocked += b->prio_blocked;
+	b->prio_blocked = 0;
+
+	if (op && op->journal && !b->level) {
+		if (w->journal &&
+		    journal_pin_cmp(b->c, w, op)) {
+			atomic_dec_bug(w->journal);
+			w->journal = NULL;
+		}
+
+		if (!w->journal) {
+			w->journal = op->journal;
+			atomic_inc(w->journal);
+		}
+	}
+
+	if (current->bio_list)
+		return;
+
+	/* Force write if set is too big */
+	if (now ||
+	    b->level ||
+	    set_bytes(i) > PAGE_SIZE - 48) {
+		if (op && now) {
+			/* Must wait on multiple writes */
+			BUG_ON(w->owner);
+			w->owner = &op->cl;
+			closure_get(&op->cl);
+		}
+
+		__btree_write(b);
+	}
+	BUG_ON(!b->written);
+}
+
+/*
+ * Btree in memory cache - allocation/freeing
+ * mca -> memory cache
+ */
+
+static void mca_reinit(struct btree *b)
+{
+	unsigned i;
+
+	b->flags	= 0;
+	b->written	= 0;
+	b->nsets	= 0;
+
+	for (i = 0; i < MAX_BSETS; i++)
+		b->sets[i].size = 0;
+	/*
+	 * Second loop starts at 1 because b->sets[0]->data is the memory we
+	 * allocated
+	 */
+	for (i = 1; i < MAX_BSETS; i++)
+		b->sets[i].data = NULL;
+}
+
+#define mca_reserve(c)	(((c->root && c->root->level)		\
+			  ? c->root->level : 1) * 8 + 16)
+#define mca_can_free(c)						\
+	max_t(int, 0, c->bucket_cache_used - mca_reserve(c))
+
+static void mca_data_free(struct btree *b)
+{
+	struct bset_tree *t = b->sets;
+	BUG_ON(!closure_is_unlocked(&b->io.cl));
+
+	if (bset_prev_bytes(b) < PAGE_SIZE)
+		kfree(t->prev);
+	else
+		free_pages((unsigned long) t->prev,
+			   get_order(bset_prev_bytes(b)));
+
+	if (bset_tree_bytes(b) < PAGE_SIZE)
+		kfree(t->tree);
+	else
+		free_pages((unsigned long) t->tree,
+			   get_order(bset_tree_bytes(b)));
+
+	free_pages((unsigned long) t->data, b->page_order);
+
+	t->prev = NULL;
+	t->tree = NULL;
+	t->data = NULL;
+	list_move(&b->list, &b->c->btree_cache_freed);
+	b->c->bucket_cache_used--;
+}
+
+static void mca_bucket_free(struct btree *b)
+{
+	BUG_ON(btree_node_dirty(b));
+
+	b->key.ptr[0] = 0;
+	hlist_del_init_rcu(&b->hash);
+	list_move(&b->list, &b->c->btree_cache_freeable);
+}
+
+static unsigned btree_order(struct bkey *k)
+{
+	return ilog2(KEY_SIZE(k) / PAGE_SECTORS ?: 1);
+}
+
+static void mca_data_alloc(struct btree *b, struct bkey *k, gfp_t gfp)
+{
+	struct bset_tree *t = b->sets;
+	BUG_ON(t->data);
+
+	b->page_order = max_t(unsigned,
+			      ilog2(b->c->btree_pages),
+			      btree_order(k));
+
+	t->data = (void *) __get_free_pages(gfp, b->page_order);
+	if (!t->data)
+		goto err;
+
+	t->tree = bset_tree_bytes(b) < PAGE_SIZE
+		? kmalloc(bset_tree_bytes(b), gfp)
+		: (void *) __get_free_pages(gfp, get_order(bset_tree_bytes(b)));
+	if (!t->tree)
+		goto err;
+
+	t->prev = bset_prev_bytes(b) < PAGE_SIZE
+		? kmalloc(bset_prev_bytes(b), gfp)
+		: (void *) __get_free_pages(gfp, get_order(bset_prev_bytes(b)));
+	if (!t->prev)
+		goto err;
+
+	list_move(&b->list, &b->c->btree_cache);
+	b->c->bucket_cache_used++;
+	return;
+err:
+	mca_data_free(b);
+}
+
+static struct btree *mca_bucket_alloc(struct cache_set *c,
+				      struct bkey *k, gfp_t gfp)
+{
+	struct btree *b = kzalloc(sizeof(struct btree), gfp);
+	if (!b)
+		return NULL;
+
+	init_rwsem(&b->lock);
+	lockdep_set_novalidate_class(&b->lock);
+	INIT_LIST_HEAD(&b->list);
+	INIT_DELAYED_WORK(&b->work, btree_write_work);
+	b->c = c;
+	closure_init_unlocked(&b->io);
+
+	mca_data_alloc(b, k, gfp);
+	return b;
+}
+
+static int mca_reap(struct btree *b, struct closure *cl, unsigned min_order)
+{
+	lockdep_assert_held(&b->c->bucket_lock);
+
+	if (!down_write_trylock(&b->lock))
+		return -ENOMEM;
+
+	if (b->page_order < min_order) {
+		rw_unlock(true, b);
+		return -ENOMEM;
+	}
+
+	BUG_ON(btree_node_dirty(b) && !b->sets[0].data);
+
+	if (cl && btree_node_dirty(b))
+		bch_btree_write(b, true, NULL);
+
+	if (cl)
+		closure_wait_event_async(&b->io.wait, cl,
+			 atomic_read(&b->io.cl.remaining) == -1);
+
+	if (btree_node_dirty(b) ||
+	    !closure_is_unlocked(&b->io.cl) ||
+	    work_pending(&b->work.work)) {
+		rw_unlock(true, b);
+		return -EAGAIN;
+	}
+
+	return 0;
+}
+
+static int bch_mca_shrink(struct shrinker *shrink, struct shrink_control *sc)
+{
+	struct cache_set *c = container_of(shrink, struct cache_set, shrink);
+	struct btree *b, *t;
+	unsigned long i, nr = sc->nr_to_scan;
+
+	if (c->shrinker_disabled)
+		return 0;
+
+	if (c->try_harder)
+		return 0;
+
+	/*
+	 * If nr == 0, we're supposed to return the number of items we have
+	 * cached. Not allowed to return -1.
+	 */
+	if (!nr)
+		return mca_can_free(c) * c->btree_pages;
+
+	/* Return -1 if we can't do anything right now */
+	if (sc->gfp_mask & __GFP_WAIT)
+		mutex_lock(&c->bucket_lock);
+	else if (!mutex_trylock(&c->bucket_lock))
+		return -1;
+
+	nr /= c->btree_pages;
+	nr = min_t(unsigned long, nr, mca_can_free(c));
+
+	i = 0;
+	list_for_each_entry_safe(b, t, &c->btree_cache_freeable, list) {
+		if (!nr)
+			break;
+
+		if (++i > 3 &&
+		    !mca_reap(b, NULL, 0)) {
+			mca_data_free(b);
+			rw_unlock(true, b);
+			--nr;
+		}
+	}
+
+	/*
+	 * Can happen right when we first start up, before we've read in any
+	 * btree nodes
+	 */
+	if (list_empty(&c->btree_cache))
+		goto out;
+
+	for (i = 0; nr && i < c->bucket_cache_used; i++) {
+		b = list_first_entry(&c->btree_cache, struct btree, list);
+		list_rotate_left(&c->btree_cache);
+
+		if (!b->accessed &&
+		    !mca_reap(b, NULL, 0)) {
+			mca_bucket_free(b);
+			mca_data_free(b);
+			rw_unlock(true, b);
+			--nr;
+		} else
+			b->accessed = 0;
+	}
+out:
+	nr = mca_can_free(c) * c->btree_pages;
+	mutex_unlock(&c->bucket_lock);
+	return nr;
+}
+
+void bch_btree_cache_free(struct cache_set *c)
+{
+	struct btree *b;
+	struct closure cl;
+	closure_init_stack(&cl);
+
+	if (c->shrink.list.next)
+		unregister_shrinker(&c->shrink);
+
+	mutex_lock(&c->bucket_lock);
+
+#ifdef CONFIG_BCACHE_DEBUG
+	if (c->verify_data)
+		list_move(&c->verify_data->list, &c->btree_cache);
+#endif
+
+	list_splice(&c->btree_cache_freeable,
+		    &c->btree_cache);
+
+	while (!list_empty(&c->btree_cache)) {
+		b = list_first_entry(&c->btree_cache, struct btree, list);
+
+		if (btree_node_dirty(b))
+			btree_complete_write(b, btree_current_write(b));
+		clear_bit(BTREE_NODE_dirty, &b->flags);
+
+		mca_data_free(b);
+	}
+
+	while (!list_empty(&c->btree_cache_freed)) {
+		b = list_first_entry(&c->btree_cache_freed,
+				     struct btree, list);
+		list_del(&b->list);
+		cancel_delayed_work_sync(&b->work);
+		kfree(b);
+	}
+
+	mutex_unlock(&c->bucket_lock);
+}
+
+int bch_btree_cache_alloc(struct cache_set *c)
+{
+	unsigned i;
+
+	/* XXX: doesn't check for errors */
+
+	closure_init_unlocked(&c->gc);
+
+	for (i = 0; i < mca_reserve(c); i++)
+		mca_bucket_alloc(c, &ZERO_KEY, GFP_KERNEL);
+
+	list_splice_init(&c->btree_cache,
+			 &c->btree_cache_freeable);
+
+#ifdef CONFIG_BCACHE_DEBUG
+	mutex_init(&c->verify_lock);
+
+	c->verify_data = mca_bucket_alloc(c, &ZERO_KEY, GFP_KERNEL);
+
+	if (c->verify_data &&
+	    c->verify_data->sets[0].data)
+		list_del_init(&c->verify_data->list);
+	else
+		c->verify_data = NULL;
+#endif
+
+	c->shrink.shrink = bch_mca_shrink;
+	c->shrink.seeks = 4;
+	c->shrink.batch = c->btree_pages * 2;
+	register_shrinker(&c->shrink);
+
+	return 0;
+}
+
+/* Btree in memory cache - hash table */
+
+static struct hlist_head *mca_hash(struct cache_set *c, struct bkey *k)
+{
+	return &c->bucket_hash[hash_32(PTR_HASH(c, k), BUCKET_HASH_BITS)];
+}
+
+static struct btree *mca_find(struct cache_set *c, struct bkey *k)
+{
+	struct btree *b;
+
+	rcu_read_lock();
+	hlist_for_each_entry_rcu(b, mca_hash(c, k), hash)
+		if (PTR_HASH(c, &b->key) == PTR_HASH(c, k))
+			goto out;
+	b = NULL;
+out:
+	rcu_read_unlock();
+	return b;
+}
+
+static struct btree *mca_cannibalize(struct cache_set *c, struct bkey *k,
+				     int level, struct closure *cl)
+{
+	int ret = -ENOMEM;
+	struct btree *i;
+
+	if (!cl)
+		return ERR_PTR(-ENOMEM);
+
+	/*
+	 * Trying to free up some memory - i.e. reuse some btree nodes - may
+	 * require initiating IO to flush the dirty part of the node. If we're
+	 * running under generic_make_request(), that IO will never finish and
+	 * we would deadlock. Returning -EAGAIN causes the cache lookup code to
+	 * punt to workqueue and retry.
+	 */
+	if (current->bio_list)
+		return ERR_PTR(-EAGAIN);
+
+	if (c->try_harder && c->try_harder != cl) {
+		closure_wait_event_async(&c->try_wait, cl, !c->try_harder);
+		return ERR_PTR(-EAGAIN);
+	}
+
+	/* XXX: tracepoint */
+	c->try_harder = cl;
+	c->try_harder_start = local_clock();
+retry:
+	list_for_each_entry_reverse(i, &c->btree_cache, list) {
+		int r = mca_reap(i, cl, btree_order(k));
+		if (!r)
+			return i;
+		if (r != -ENOMEM)
+			ret = r;
+	}
+
+	if (ret == -EAGAIN &&
+	    closure_blocking(cl)) {
+		mutex_unlock(&c->bucket_lock);
+		closure_sync(cl);
+		mutex_lock(&c->bucket_lock);
+		goto retry;
+	}
+
+	return ERR_PTR(ret);
+}
+
+/*
+ * We can only have one thread cannibalizing other cached btree nodes at a time,
+ * or we'll deadlock. We use an open coded mutex to ensure that, which a
+ * cannibalize_bucket() will take. This means every time we unlock the root of
+ * the btree, we need to release this lock if we have it held.
+ */
+void bch_cannibalize_unlock(struct cache_set *c, struct closure *cl)
+{
+	if (c->try_harder == cl) {
+		bch_time_stats_update(&c->try_harder_time, c->try_harder_start);
+		c->try_harder = NULL;
+		__closure_wake_up(&c->try_wait);
+	}
+}
+
+static struct btree *mca_alloc(struct cache_set *c, struct bkey *k,
+			       int level, struct closure *cl)
+{
+	struct btree *b;
+
+	lockdep_assert_held(&c->bucket_lock);
+
+	if (mca_find(c, k))
+		return NULL;
+
+	/* btree_free() doesn't free memory; it sticks the node on the end of
+	 * the list. Check if there's any freed nodes there:
+	 */
+	list_for_each_entry(b, &c->btree_cache_freeable, list)
+		if (!mca_reap(b, NULL, btree_order(k)))
+			goto out;
+
+	/* We never free struct btree itself, just the memory that holds the on
+	 * disk node. Check the freed list before allocating a new one:
+	 */
+	list_for_each_entry(b, &c->btree_cache_freed, list)
+		if (!mca_reap(b, NULL, 0)) {
+			mca_data_alloc(b, k, __GFP_NOWARN|GFP_NOIO);
+			if (!b->sets[0].data)
+				goto err;
+			else
+				goto out;
+		}
+
+	b = mca_bucket_alloc(c, k, __GFP_NOWARN|GFP_NOIO);
+	if (!b)
+		goto err;
+
+	BUG_ON(!down_write_trylock(&b->lock));
+	if (!b->sets->data)
+		goto err;
+out:
+	BUG_ON(!closure_is_unlocked(&b->io.cl));
+
+	bkey_copy(&b->key, k);
+	list_move(&b->list, &c->btree_cache);
+	hlist_del_init_rcu(&b->hash);
+	hlist_add_head_rcu(&b->hash, mca_hash(c, k));
+
+	lock_set_subclass(&b->lock.dep_map, level + 1, _THIS_IP_);
+	b->level	= level;
+
+	mca_reinit(b);
+
+	return b;
+err:
+	if (b)
+		rw_unlock(true, b);
+
+	b = mca_cannibalize(c, k, level, cl);
+	if (!IS_ERR(b))
+		goto out;
+
+	return b;
+}
+
+/**
+ * bch_btree_node_get - find a btree node in the cache and lock it, reading it
+ * in from disk if necessary.
+ *
+ * If IO is necessary, it uses the closure embedded in struct btree_op to wait;
+ * if that closure is in non blocking mode, will return -EAGAIN.
+ *
+ * The btree node will have either a read or a write lock held, depending on
+ * level and op->lock.
+ */
+struct btree *bch_btree_node_get(struct cache_set *c, struct bkey *k,
+				 int level, struct btree_op *op)
+{
+	int i = 0;
+	bool write = level <= op->lock;
+	struct btree *b;
+
+	BUG_ON(level < 0);
+retry:
+	b = mca_find(c, k);
+
+	if (!b) {
+		mutex_lock(&c->bucket_lock);
+		b = mca_alloc(c, k, level, &op->cl);
+		mutex_unlock(&c->bucket_lock);
+
+		if (!b)
+			goto retry;
+		if (IS_ERR(b))
+			return b;
+
+		bch_btree_read(b);
+
+		if (!write)
+			downgrade_write(&b->lock);
+	} else {
+		rw_lock(write, b, level);
+		if (PTR_HASH(c, &b->key) != PTR_HASH(c, k)) {
+			rw_unlock(write, b);
+			goto retry;
+		}
+		BUG_ON(b->level != level);
+	}
+
+	b->accessed = 1;
+
+	for (; i <= b->nsets && b->sets[i].size; i++) {
+		prefetch(b->sets[i].tree);
+		prefetch(b->sets[i].data);
+	}
+
+	for (; i <= b->nsets; i++)
+		prefetch(b->sets[i].data);
+
+	if (!closure_wait_event(&b->io.wait, &op->cl,
+				btree_node_read_done(b))) {
+		rw_unlock(write, b);
+		b = ERR_PTR(-EAGAIN);
+	} else if (btree_node_io_error(b)) {
+		rw_unlock(write, b);
+		b = ERR_PTR(-EIO);
+	} else
+		BUG_ON(!b->written);
+
+	return b;
+}
+
+static void btree_node_prefetch(struct cache_set *c, struct bkey *k, int level)
+{
+	struct btree *b;
+
+	mutex_lock(&c->bucket_lock);
+	b = mca_alloc(c, k, level, NULL);
+	mutex_unlock(&c->bucket_lock);
+
+	if (!IS_ERR_OR_NULL(b)) {
+		bch_btree_read(b);
+		rw_unlock(true, b);
+	}
+}
+
+/* Btree alloc */
+
+static void btree_node_free(struct btree *b, struct btree_op *op)
+{
+	unsigned i;
+
+	/*
+	 * The BUG_ON() in btree_node_get() implies that we must have a write
+	 * lock on parent to free or even invalidate a node
+	 */
+	BUG_ON(op->lock <= b->level);
+	BUG_ON(b == b->c->root);
+	pr_debug("bucket %s", pbtree(b));
+
+	if (btree_node_dirty(b))
+		btree_complete_write(b, btree_current_write(b));
+	clear_bit(BTREE_NODE_dirty, &b->flags);
+
+	if (b->prio_blocked &&
+	    !atomic_sub_return(b->prio_blocked, &b->c->prio_blocked))
+		wake_up(&b->c->alloc_wait);
+
+	b->prio_blocked = 0;
+
+	cancel_delayed_work(&b->work);
+
+	mutex_lock(&b->c->bucket_lock);
+
+	for (i = 0; i < KEY_PTRS(&b->key); i++) {
+		BUG_ON(atomic_read(&PTR_BUCKET(b->c, &b->key, i)->pin));
+
+		bch_inc_gen(PTR_CACHE(b->c, &b->key, i),
+			    PTR_BUCKET(b->c, &b->key, i));
+	}
+
+	bch_bucket_free(b->c, &b->key);
+	mca_bucket_free(b);
+	mutex_unlock(&b->c->bucket_lock);
+}
+
+struct btree *bch_btree_node_alloc(struct cache_set *c, int level,
+				   struct closure *cl)
+{
+	BKEY_PADDED(key) k;
+	struct btree *b = ERR_PTR(-EAGAIN);
+
+	mutex_lock(&c->bucket_lock);
+retry:
+	if (__bch_bucket_alloc_set(c, WATERMARK_METADATA, &k.key, 1, cl))
+		goto err;
+
+	SET_KEY_SIZE(&k.key, c->btree_pages * PAGE_SECTORS);
+
+	b = mca_alloc(c, &k.key, level, cl);
+	if (IS_ERR(b))
+		goto err_free;
+
+	if (!b) {
+		cache_bug(c,
+			"Tried to allocate bucket that was in btree cache");
+		__bkey_put(c, &k.key);
+		goto retry;
+	}
+
+	set_btree_node_read_done(b);
+	b->accessed = 1;
+	bch_bset_init_next(b);
+
+	mutex_unlock(&c->bucket_lock);
+	return b;
+err_free:
+	bch_bucket_free(c, &k.key);
+	__bkey_put(c, &k.key);
+err:
+	mutex_unlock(&c->bucket_lock);
+	return b;
+}
+
+static struct btree *btree_node_alloc_replacement(struct btree *b,
+						  struct closure *cl)
+{
+	struct btree *n = bch_btree_node_alloc(b->c, b->level, cl);
+	if (!IS_ERR_OR_NULL(n))
+		bch_btree_sort_into(b, n);
+
+	return n;
+}
+
+/* Garbage collection */
+
+uint8_t __bch_btree_mark_key(struct cache_set *c, int level, struct bkey *k)
+{
+	uint8_t stale = 0;
+	unsigned i;
+	struct bucket *g;
+
+	/*
+	 * ptr_invalid() can't return true for the keys that mark btree nodes as
+	 * freed, but since ptr_bad() returns true we'll never actually use them
+	 * for anything and thus we don't want mark their pointers here
+	 */
+	if (!bkey_cmp(k, &ZERO_KEY))
+		return stale;
+
+	for (i = 0; i < KEY_PTRS(k); i++) {
+		if (!ptr_available(c, k, i))
+			continue;
+
+		g = PTR_BUCKET(c, k, i);
+
+		if (gen_after(g->gc_gen, PTR_GEN(k, i)))
+			g->gc_gen = PTR_GEN(k, i);
+
+		if (ptr_stale(c, k, i)) {
+			stale = max(stale, ptr_stale(c, k, i));
+			continue;
+		}
+
+		cache_bug_on(GC_MARK(g) &&
+			     (GC_MARK(g) == GC_MARK_METADATA) != (level != 0),
+			     c, "inconsistent ptrs: mark = %llu, level = %i",
+			     GC_MARK(g), level);
+
+		if (level)
+			SET_GC_MARK(g, GC_MARK_METADATA);
+		else if (KEY_DIRTY(k))
+			SET_GC_MARK(g, GC_MARK_DIRTY);
+
+		/* guard against overflow */
+		SET_GC_SECTORS_USED(g, min_t(unsigned,
+					     GC_SECTORS_USED(g) + KEY_SIZE(k),
+					     (1 << 14) - 1));
+
+		BUG_ON(!GC_SECTORS_USED(g));
+	}
+
+	return stale;
+}
+
+#define btree_mark_key(b, k)	__bch_btree_mark_key(b->c, b->level, k)
+
+static int btree_gc_mark_node(struct btree *b, unsigned *keys,
+			      struct gc_stat *gc)
+{
+	uint8_t stale = 0;
+	unsigned last_dev = -1;
+	struct bcache_device *d = NULL;
+	struct bkey *k;
+	struct btree_iter iter;
+	struct bset_tree *t;
+
+	gc->nodes++;
+
+	for_each_key_filter(b, k, &iter, bch_ptr_invalid) {
+		if (last_dev != KEY_INODE(k)) {
+			last_dev = KEY_INODE(k);
+
+			d = KEY_INODE(k) < b->c->nr_uuids
+				? b->c->devices[last_dev]
+				: NULL;
+		}
+
+		stale = max(stale, btree_mark_key(b, k));
+
+		if (bch_ptr_bad(b, k))
+			continue;
+
+		*keys += bkey_u64s(k);
+
+		gc->key_bytes += bkey_u64s(k);
+		gc->nkeys++;
+
+		gc->data += KEY_SIZE(k);
+		if (KEY_DIRTY(k)) {
+			gc->dirty += KEY_SIZE(k);
+			if (d)
+				d->sectors_dirty_gc += KEY_SIZE(k);
+		}
+	}
+
+	for (t = b->sets; t <= &b->sets[b->nsets]; t++)
+		btree_bug_on(t->size &&
+			     bset_written(b, t) &&
+			     bkey_cmp(&b->key, &t->end) < 0,
+			     b, "found short btree key in gc");
+
+	return stale;
+}
+
+static struct btree *btree_gc_alloc(struct btree *b, struct bkey *k,
+				    struct btree_op *op)
+{
+	/*
+	 * We block priorities from being written for the duration of garbage
+	 * collection, so we can't sleep in btree_alloc() ->
+	 * bch_bucket_alloc_set(), or we'd risk deadlock - so we don't pass it
+	 * our closure.
+	 */
+	struct btree *n = btree_node_alloc_replacement(b, NULL);
+
+	if (!IS_ERR_OR_NULL(n)) {
+		swap(b, n);
+
+		memcpy(k->ptr, b->key.ptr,
+		       sizeof(uint64_t) * KEY_PTRS(&b->key));
+
+		__bkey_put(b->c, &b->key);
+		atomic_inc(&b->c->prio_blocked);
+		b->prio_blocked++;
+
+		btree_node_free(n, op);
+		up_write(&n->lock);
+	}
+
+	return b;
+}
+
+/*
+ * Leaving this at 2 until we've got incremental garbage collection done; it
+ * could be higher (and has been tested with 4) except that garbage collection
+ * could take much longer, adversely affecting latency.
+ */
+#define GC_MERGE_NODES	2U
+
+struct gc_merge_info {
+	struct btree	*b;
+	struct bkey	*k;
+	unsigned	keys;
+};
+
+static void btree_gc_coalesce(struct btree *b, struct btree_op *op,
+			      struct gc_stat *gc, struct gc_merge_info *r)
+{
+	unsigned nodes = 0, keys = 0, blocks;
+	int i;
+
+	while (nodes < GC_MERGE_NODES && r[nodes].b)
+		keys += r[nodes++].keys;
+
+	blocks = btree_default_blocks(b->c) * 2 / 3;
+
+	if (nodes < 2 ||
+	    __set_blocks(b->sets[0].data, keys, b->c) > blocks * (nodes - 1))
+		return;
+
+	for (i = nodes - 1; i >= 0; --i) {
+		if (r[i].b->written)
+			r[i].b = btree_gc_alloc(r[i].b, r[i].k, op);
+
+		if (r[i].b->written)
+			return;
+	}
+
+	for (i = nodes - 1; i > 0; --i) {
+		struct bset *n1 = r[i].b->sets->data;
+		struct bset *n2 = r[i - 1].b->sets->data;
+		struct bkey *k, *last = NULL;
+
+		keys = 0;
+
+		if (i == 1) {
+			/*
+			 * Last node we're not getting rid of - we're getting
+			 * rid of the node at r[0]. Have to try and fit all of
+			 * the remaining keys into this node; we can't ensure
+			 * they will always fit due to rounding and variable
+			 * length keys (shouldn't be possible in practice,
+			 * though)
+			 */
+			if (__set_blocks(n1, n1->keys + r->keys,
+					 b->c) > btree_blocks(r[i].b))
+				return;
+
+			keys = n2->keys;
+			last = &r->b->key;
+		} else
+			for (k = n2->start;
+			     k < end(n2);
+			     k = bkey_next(k)) {
+				if (__set_blocks(n1, n1->keys + keys +
+						 bkey_u64s(k), b->c) > blocks)
+					break;
+
+				last = k;
+				keys += bkey_u64s(k);
+			}
+
+		BUG_ON(__set_blocks(n1, n1->keys + keys,
+				    b->c) > btree_blocks(r[i].b));
+
+		if (last) {
+			bkey_copy_key(&r[i].b->key, last);
+			bkey_copy_key(r[i].k, last);
+		}
+
+		memcpy(end(n1),
+		       n2->start,
+		       (void *) node(n2, keys) - (void *) n2->start);
+
+		n1->keys += keys;
+
+		memmove(n2->start,
+			node(n2, keys),
+			(void *) end(n2) - (void *) node(n2, keys));
+
+		n2->keys -= keys;
+
+		r[i].keys	= n1->keys;
+		r[i - 1].keys	= n2->keys;
+	}
+
+	btree_node_free(r->b, op);
+	up_write(&r->b->lock);
+
+	pr_debug("coalesced %u nodes", nodes);
+
+	gc->nodes--;
+	nodes--;
+
+	memmove(&r[0], &r[1], sizeof(struct gc_merge_info) * nodes);
+	memset(&r[nodes], 0, sizeof(struct gc_merge_info));
+}
+
+static int btree_gc_recurse(struct btree *b, struct btree_op *op,
+			    struct closure *writes, struct gc_stat *gc)
+{
+	void write(struct btree *r)
+	{
+		if (!r->written)
+			bch_btree_write(r, true, op);
+		else if (btree_node_dirty(r)) {
+			BUG_ON(btree_current_write(r)->owner);
+			btree_current_write(r)->owner = writes;
+			closure_get(writes);
+
+			bch_btree_write(r, true, NULL);
+		}
+
+		up_write(&r->lock);
+	}
+
+	int ret = 0, stale;
+	unsigned i;
+	struct gc_merge_info r[GC_MERGE_NODES];
+
+	memset(r, 0, sizeof(r));
+
+	while ((r->k = bch_next_recurse_key(b, &b->c->gc_done))) {
+		r->b = bch_btree_node_get(b->c, r->k, b->level - 1, op);
+
+		if (IS_ERR(r->b)) {
+			ret = PTR_ERR(r->b);
+			break;
+		}
+
+		r->keys	= 0;
+		stale = btree_gc_mark_node(r->b, &r->keys, gc);
+
+		if (!b->written &&
+		    (r->b->level || stale > 10 ||
+		     b->c->gc_always_rewrite))
+			r->b = btree_gc_alloc(r->b, r->k, op);
+
+		if (r->b->level)
+			ret = btree_gc_recurse(r->b, op, writes, gc);
+
+		if (ret) {
+			write(r->b);
+			break;
+		}
+
+		bkey_copy_key(&b->c->gc_done, r->k);
+
+		if (!b->written)
+			btree_gc_coalesce(b, op, gc, r);
+
+		if (r[GC_MERGE_NODES - 1].b)
+			write(r[GC_MERGE_NODES - 1].b);
+
+		memmove(&r[1], &r[0],
+			sizeof(struct gc_merge_info) * (GC_MERGE_NODES - 1));
+
+		/* When we've got incremental GC working, we'll want to do
+		 * if (should_resched())
+		 *	return -EAGAIN;
+		 */
+		cond_resched();
+#if 0
+		if (need_resched()) {
+			ret = -EAGAIN;
+			break;
+		}
+#endif
+	}
+
+	for (i = 1; i < GC_MERGE_NODES && r[i].b; i++)
+		write(r[i].b);
+
+	/* Might have freed some children, must remove their keys */
+	if (!b->written)
+		bch_btree_sort(b);
+
+	return ret;
+}
+
+static int bch_btree_gc_root(struct btree *b, struct btree_op *op,
+			     struct closure *writes, struct gc_stat *gc)
+{
+	struct btree *n = NULL;
+	unsigned keys = 0;
+	int ret = 0, stale = btree_gc_mark_node(b, &keys, gc);
+
+	if (b->level || stale > 10)
+		n = btree_node_alloc_replacement(b, NULL);
+
+	if (!IS_ERR_OR_NULL(n))
+		swap(b, n);
+
+	if (b->level)
+		ret = btree_gc_recurse(b, op, writes, gc);
+
+	if (!b->written || btree_node_dirty(b)) {
+		atomic_inc(&b->c->prio_blocked);
+		b->prio_blocked++;
+		bch_btree_write(b, true, n ? op : NULL);
+	}
+
+	if (!IS_ERR_OR_NULL(n)) {
+		closure_sync(&op->cl);
+		bch_btree_set_root(b);
+		btree_node_free(n, op);
+		rw_unlock(true, b);
+	}
+
+	return ret;
+}
+
+static void btree_gc_start(struct cache_set *c)
+{
+	struct cache *ca;
+	struct bucket *b;
+	struct bcache_device **d;
+	unsigned i;
+
+	if (!c->gc_mark_valid)
+		return;
+
+	mutex_lock(&c->bucket_lock);
+
+	c->gc_mark_valid = 0;
+	c->gc_done = ZERO_KEY;
+
+	for_each_cache(ca, c, i)
+		for_each_bucket(b, ca) {
+			b->gc_gen = b->gen;
+			if (!atomic_read(&b->pin))
+				SET_GC_MARK(b, GC_MARK_RECLAIMABLE);
+		}
+
+	for (d = c->devices;
+	     d < c->devices + c->nr_uuids;
+	     d++)
+		if (*d)
+			(*d)->sectors_dirty_gc = 0;
+
+	mutex_unlock(&c->bucket_lock);
+}
+
+size_t bch_btree_gc_finish(struct cache_set *c)
+{
+	size_t available = 0;
+	struct bucket *b;
+	struct cache *ca;
+	struct bcache_device **d;
+	unsigned i;
+
+	mutex_lock(&c->bucket_lock);
+
+	set_gc_sectors(c);
+	c->gc_mark_valid = 1;
+	c->need_gc	= 0;
+
+	if (c->root)
+		for (i = 0; i < KEY_PTRS(&c->root->key); i++)
+			SET_GC_MARK(PTR_BUCKET(c, &c->root->key, i),
+				    GC_MARK_METADATA);
+
+	for (i = 0; i < KEY_PTRS(&c->uuid_bucket); i++)
+		SET_GC_MARK(PTR_BUCKET(c, &c->uuid_bucket, i),
+			    GC_MARK_METADATA);
+
+	for_each_cache(ca, c, i) {
+		uint64_t *i;
+
+		ca->invalidate_needs_gc = 0;
+
+		for (i = ca->sb.d; i < ca->sb.d + ca->sb.keys; i++)
+			SET_GC_MARK(ca->buckets + *i, GC_MARK_METADATA);
+
+		for (i = ca->prio_buckets;
+		     i < ca->prio_buckets + prio_buckets(ca) * 2; i++)
+			SET_GC_MARK(ca->buckets + *i, GC_MARK_METADATA);
+
+		for_each_bucket(b, ca) {
+			b->last_gc	= b->gc_gen;
+			c->need_gc	= max(c->need_gc, bucket_gc_gen(b));
+
+			if (!atomic_read(&b->pin) &&
+			    GC_MARK(b) == GC_MARK_RECLAIMABLE) {
+				available++;
+				if (!GC_SECTORS_USED(b))
+					bch_bucket_add_unused(ca, b);
+			}
+		}
+	}
+
+	for (d = c->devices;
+	     d < c->devices + c->nr_uuids;
+	     d++)
+		if (*d) {
+			unsigned long last =
+				atomic_long_read(&((*d)->sectors_dirty));
+			long difference = (*d)->sectors_dirty_gc - last;
+
+			pr_debug("sectors dirty off by %li", difference);
+
+			(*d)->sectors_dirty_last += difference;
+
+			atomic_long_set(&((*d)->sectors_dirty),
+					(*d)->sectors_dirty_gc);
+		}
+
+	mutex_unlock(&c->bucket_lock);
+	return available;
+}
+
+static void bch_btree_gc(struct closure *cl)
+{
+	struct cache_set *c = container_of(cl, struct cache_set, gc.cl);
+	int ret;
+	unsigned long available;
+	struct gc_stat stats;
+	struct closure writes;
+	struct btree_op op;
+
+	uint64_t start_time = local_clock();
+	trace_bcache_gc_start(c->sb.set_uuid);
+	blktrace_msg_all(c, "Starting gc");
+
+	memset(&stats, 0, sizeof(struct gc_stat));
+	closure_init_stack(&writes);
+	bch_btree_op_init_stack(&op);
+	op.lock = SHRT_MAX;
+
+	btree_gc_start(c);
+
+	ret = btree_root(gc_root, c, &op, &writes, &stats);
+	closure_sync(&op.cl);
+	closure_sync(&writes);
+
+	if (ret) {
+		blktrace_msg_all(c, "Stopped gc");
+		pr_warn("gc failed!");
+
+		continue_at(cl, bch_btree_gc, bch_gc_wq);
+	}
+
+	/* Possibly wait for new UUIDs or whatever to hit disk */
+	bch_journal_meta(c, &op.cl);
+	closure_sync(&op.cl);
+
+	available = bch_btree_gc_finish(c);
+
+	bch_time_stats_update(&c->btree_gc_time, start_time);
+
+	stats.key_bytes *= sizeof(uint64_t);
+	stats.dirty	<<= 9;
+	stats.data	<<= 9;
+	stats.in_use	= (c->nbuckets - available) * 100 / c->nbuckets;
+	memcpy(&c->gc_stats, &stats, sizeof(struct gc_stat));
+	blktrace_msg_all(c, "Finished gc");
+
+	trace_bcache_gc_end(c->sb.set_uuid);
+	wake_up(&c->alloc_wait);
+
+	continue_at(cl, bch_moving_gc, bch_gc_wq);
+}
+
+void bch_queue_gc(struct cache_set *c)
+{
+	closure_trylock_call(&c->gc.cl, bch_btree_gc, bch_gc_wq, &c->cl);
+}
+
+/* Initial partial gc */
+
+static int bch_btree_check_recurse(struct btree *b, struct btree_op *op,
+				   unsigned long **seen)
+{
+	int ret;
+	unsigned i;
+	struct bkey *k;
+	struct bucket *g;
+	struct btree_iter iter;
+
+	for_each_key_filter(b, k, &iter, bch_ptr_invalid) {
+		for (i = 0; i < KEY_PTRS(k); i++) {
+			if (!ptr_available(b->c, k, i))
+				continue;
+
+			g = PTR_BUCKET(b->c, k, i);
+
+			if (!__test_and_set_bit(PTR_BUCKET_NR(b->c, k, i),
+						seen[PTR_DEV(k, i)]) ||
+			    !ptr_stale(b->c, k, i)) {
+				g->gen = PTR_GEN(k, i);
+
+				if (b->level)
+					g->prio = BTREE_PRIO;
+				else if (g->prio == BTREE_PRIO)
+					g->prio = INITIAL_PRIO;
+			}
+		}
+
+		btree_mark_key(b, k);
+	}
+
+	if (b->level) {
+		k = bch_next_recurse_key(b, &ZERO_KEY);
+
+		while (k) {
+			struct bkey *p = bch_next_recurse_key(b, k);
+			if (p)
+				btree_node_prefetch(b->c, p, b->level - 1);
+
+			ret = btree(check_recurse, k, b, op, seen);
+			if (ret)
+				return ret;
+
+			k = p;
+		}
+	}
+
+	return 0;
+}
+
+int bch_btree_check(struct cache_set *c, struct btree_op *op)
+{
+	int ret = -ENOMEM;
+	unsigned i;
+	unsigned long *seen[MAX_CACHES_PER_SET];
+
+	memset(seen, 0, sizeof(seen));
+
+	for (i = 0; c->cache[i]; i++) {
+		size_t n = DIV_ROUND_UP(c->cache[i]->sb.nbuckets, 8);
+		seen[i] = kmalloc(n, GFP_KERNEL);
+		if (!seen[i])
+			goto err;
+
+		/* Disables the seen array until prio_read() uses it too */
+		memset(seen[i], 0xFF, n);
+	}
+
+	ret = btree_root(check_recurse, c, op, seen);
+err:
+	for (i = 0; i < MAX_CACHES_PER_SET; i++)
+		kfree(seen[i]);
+	return ret;
+}
+
+/* Btree insertion */
+
+static void shift_keys(struct btree *b, struct bkey *where, struct bkey *insert)
+{
+	struct bset *i = b->sets[b->nsets].data;
+
+	memmove((uint64_t *) where + bkey_u64s(insert),
+		where,
+		(void *) end(i) - (void *) where);
+
+	i->keys += bkey_u64s(insert);
+	bkey_copy(where, insert);
+	bch_bset_fix_lookup_table(b, where);
+}
+
+static bool fix_overlapping_extents(struct btree *b,
+				    struct bkey *insert,
+				    struct btree_iter *iter,
+				    struct btree_op *op)
+{
+	void subtract_dirty(struct bkey *k, int sectors)
+	{
+		struct bcache_device *d = b->c->devices[KEY_INODE(k)];
+
+		if (KEY_DIRTY(k) && d)
+			atomic_long_sub(sectors, &d->sectors_dirty);
+	}
+
+	unsigned old_size, sectors_found = 0;
+
+	while (1) {
+		struct bkey *k = bch_btree_iter_next(iter);
+		if (!k ||
+		    bkey_cmp(&START_KEY(k), insert) >= 0)
+			break;
+
+		if (bkey_cmp(k, &START_KEY(insert)) <= 0)
+			continue;
+
+		old_size = KEY_SIZE(k);
+
+		/*
+		 * We might overlap with 0 size extents; we can't skip these
+		 * because if they're in the set we're inserting to we have to
+		 * adjust them so they don't overlap with the key we're
+		 * inserting. But we don't want to check them for BTREE_REPLACE
+		 * operations.
+		 */
+
+		if (op->type == BTREE_REPLACE &&
+		    KEY_SIZE(k)) {
+			/*
+			 * k might have been split since we inserted/found the
+			 * key we're replacing
+			 */
+			unsigned i;
+			uint64_t offset = KEY_START(k) -
+				KEY_START(&op->replace);
+
+			/* But it must be a subset of the replace key */
+			if (KEY_START(k) < KEY_START(&op->replace) ||
+			    KEY_OFFSET(k) > KEY_OFFSET(&op->replace))
+				goto check_failed;
+
+			/* We didn't find a key that we were supposed to */
+			if (KEY_START(k) > KEY_START(insert) + sectors_found)
+				goto check_failed;
+
+			if (KEY_PTRS(&op->replace) != KEY_PTRS(k))
+				goto check_failed;
+
+			/* skip past gen */
+			offset <<= 8;
+
+			BUG_ON(!KEY_PTRS(&op->replace));
+
+			for (i = 0; i < KEY_PTRS(&op->replace); i++)
+				if (k->ptr[i] != op->replace.ptr[i] + offset)
+					goto check_failed;
+
+			sectors_found = KEY_OFFSET(k) - KEY_START(insert);
+		}
+
+		if (bkey_cmp(insert, k) < 0 &&
+		    bkey_cmp(&START_KEY(insert), &START_KEY(k)) > 0) {
+			/*
+			 * We overlapped in the middle of an existing key: that
+			 * means we have to split the old key. But we have to do
+			 * slightly different things depending on whether the
+			 * old key has been written out yet.
+			 */
+
+			struct bkey *top;
+
+			subtract_dirty(k, KEY_SIZE(insert));
+
+			if (bkey_written(b, k)) {
+				/*
+				 * We insert a new key to cover the top of the
+				 * old key, and the old key is modified in place
+				 * to represent the bottom split.
+				 *
+				 * It's completely arbitrary whether the new key
+				 * is the top or the bottom, but it has to match
+				 * up with what btree_sort_fixup() does - it
+				 * doesn't check for this kind of overlap, it
+				 * depends on us inserting a new key for the top
+				 * here.
+				 */
+				top = bch_bset_search(b, &b->sets[b->nsets],
+						      insert);
+				shift_keys(b, top, k);
+			} else {
+				BKEY_PADDED(key) temp;
+				bkey_copy(&temp.key, k);
+				shift_keys(b, k, &temp.key);
+				top = bkey_next(k);
+			}
+
+			bch_cut_front(insert, top);
+			bch_cut_back(&START_KEY(insert), k);
+			bch_bset_fix_invalidated_key(b, k);
+			return false;
+		}
+
+		if (bkey_cmp(insert, k) < 0) {
+			bch_cut_front(insert, k);
+		} else {
+			if (bkey_written(b, k) &&
+			    bkey_cmp(&START_KEY(insert), &START_KEY(k)) <= 0) {
+				/*
+				 * Completely overwrote, so we don't have to
+				 * invalidate the binary search tree
+				 */
+				bch_cut_front(k, k);
+			} else {
+				__bch_cut_back(&START_KEY(insert), k);
+				bch_bset_fix_invalidated_key(b, k);
+			}
+		}
+
+		subtract_dirty(k, old_size - KEY_SIZE(k));
+	}
+
+check_failed:
+	if (op->type == BTREE_REPLACE) {
+		if (!sectors_found) {
+			op->insert_collision = true;
+			return true;
+		} else if (sectors_found < KEY_SIZE(insert)) {
+			SET_KEY_OFFSET(insert, KEY_OFFSET(insert) -
+				       (KEY_SIZE(insert) - sectors_found));
+			SET_KEY_SIZE(insert, sectors_found);
+		}
+	}
+
+	return false;
+}
+
+static bool btree_insert_key(struct btree *b, struct btree_op *op,
+			     struct bkey *k)
+{
+	struct bset *i = b->sets[b->nsets].data;
+	struct bkey *m, *prev;
+	const char *status = "insert";
+
+	BUG_ON(bkey_cmp(k, &b->key) > 0);
+	BUG_ON(b->level && !KEY_PTRS(k));
+	BUG_ON(!b->level && !KEY_OFFSET(k));
+
+	if (!b->level) {
+		struct btree_iter iter;
+		struct bkey search = KEY(KEY_INODE(k), KEY_START(k), 0);
+
+		/*
+		 * bset_search() returns the first key that is strictly greater
+		 * than the search key - but for back merging, we want to find
+		 * the first key that is greater than or equal to KEY_START(k) -
+		 * unless KEY_START(k) is 0.
+		 */
+		if (KEY_OFFSET(&search))
+			SET_KEY_OFFSET(&search, KEY_OFFSET(&search) - 1);
+
+		prev = NULL;
+		m = bch_btree_iter_init(b, &iter, &search);
+
+		if (fix_overlapping_extents(b, k, &iter, op))
+			return false;
+
+		while (m != end(i) &&
+		       bkey_cmp(k, &START_KEY(m)) > 0)
+			prev = m, m = bkey_next(m);
+
+		if (key_merging_disabled(b->c))
+			goto insert;
+
+		/* prev is in the tree, if we merge we're done */
+		status = "back merging";
+		if (prev &&
+		    bch_bkey_try_merge(b, prev, k))
+			goto merged;
+
+		status = "overwrote front";
+		if (m != end(i) &&
+		    KEY_PTRS(m) == KEY_PTRS(k) && !KEY_SIZE(m))
+			goto copy;
+
+		status = "front merge";
+		if (m != end(i) &&
+		    bch_bkey_try_merge(b, k, m))
+			goto copy;
+	} else
+		m = bch_bset_search(b, &b->sets[b->nsets], k);
+
+insert:	shift_keys(b, m, k);
+copy:	bkey_copy(m, k);
+merged:
+	bch_check_keys(b, "%s for %s at %s: %s", status,
+		       op_type(op), pbtree(b), pkey(k));
+	bch_check_key_order_msg(b, i, "%s for %s at %s: %s", status,
+				op_type(op), pbtree(b), pkey(k));
+
+	if (b->level && !KEY_OFFSET(k))
+		b->prio_blocked++;
+
+	pr_debug("%s for %s at %s: %s", status,
+		 op_type(op), pbtree(b), pkey(k));
+
+	return true;
+}
+
+bool bch_btree_insert_keys(struct btree *b, struct btree_op *op)
+{
+	bool ret = false;
+	struct bkey *k;
+	unsigned oldsize = bch_count_data(b);
+
+	while ((k = bch_keylist_pop(&op->keys))) {
+		bkey_put(b->c, k, b->level);
+		ret |= btree_insert_key(b, op, k);
+	}
+
+	BUG_ON(bch_count_data(b) < oldsize);
+	return ret;
+}
+
+bool bch_btree_insert_check_key(struct btree *b, struct btree_op *op,
+				   struct bio *bio)
+{
+	bool ret = false;
+	uint64_t btree_ptr = b->key.ptr[0];
+	unsigned long seq = b->seq;
+	BKEY_PADDED(k) tmp;
+
+	rw_unlock(false, b);
+	rw_lock(true, b, b->level);
+
+	if (b->key.ptr[0] != btree_ptr ||
+	    b->seq != seq + 1 ||
+	    should_split(b))
+		goto out;
+
+	op->replace = KEY(op->inode, bio_end(bio), bio_sectors(bio));
+
+	SET_KEY_PTRS(&op->replace, 1);
+	get_random_bytes(&op->replace.ptr[0], sizeof(uint64_t));
+
+	SET_PTR_DEV(&op->replace, 0, PTR_CHECK_DEV);
+
+	bkey_copy(&tmp.k, &op->replace);
+
+	BUG_ON(op->type != BTREE_INSERT);
+	BUG_ON(!btree_insert_key(b, op, &tmp.k));
+	bch_btree_write(b, false, NULL);
+	ret = true;
+out:
+	downgrade_write(&b->lock);
+	return ret;
+}
+
+static int btree_split(struct btree *b, struct btree_op *op)
+{
+	bool split, root = b == b->c->root;
+	struct btree *n1, *n2 = NULL, *n3 = NULL;
+	uint64_t start_time = local_clock();
+
+	if (b->level)
+		set_closure_blocking(&op->cl);
+
+	n1 = btree_node_alloc_replacement(b, &op->cl);
+	if (IS_ERR(n1))
+		goto err;
+
+	split = set_blocks(n1->sets[0].data, n1->c) > (btree_blocks(b) * 4) / 5;
+
+	pr_debug("%ssplitting at %s keys %i", split ? "" : "not ",
+		 pbtree(b), n1->sets[0].data->keys);
+
+	if (split) {
+		unsigned keys = 0;
+
+		n2 = bch_btree_node_alloc(b->c, b->level, &op->cl);
+		if (IS_ERR(n2))
+			goto err_free1;
+
+		if (root) {
+			n3 = bch_btree_node_alloc(b->c, b->level + 1, &op->cl);
+			if (IS_ERR(n3))
+				goto err_free2;
+		}
+
+		bch_btree_insert_keys(n1, op);
+
+		/* Has to be a linear search because we don't have an auxiliary
+		 * search tree yet
+		 */
+
+		while (keys < (n1->sets[0].data->keys * 3) / 5)
+			keys += bkey_u64s(node(n1->sets[0].data, keys));
+
+		bkey_copy_key(&n1->key, node(n1->sets[0].data, keys));
+		keys += bkey_u64s(node(n1->sets[0].data, keys));
+
+		n2->sets[0].data->keys = n1->sets[0].data->keys - keys;
+		n1->sets[0].data->keys = keys;
+
+		memcpy(n2->sets[0].data->start,
+		       end(n1->sets[0].data),
+		       n2->sets[0].data->keys * sizeof(uint64_t));
+
+		bkey_copy_key(&n2->key, &b->key);
+
+		bch_keylist_add(&op->keys, &n2->key);
+		bch_btree_write(n2, true, op);
+		rw_unlock(true, n2);
+	} else
+		bch_btree_insert_keys(n1, op);
+
+	bch_keylist_add(&op->keys, &n1->key);
+	bch_btree_write(n1, true, op);
+
+	if (n3) {
+		bkey_copy_key(&n3->key, &MAX_KEY);
+		bch_btree_insert_keys(n3, op);
+		bch_btree_write(n3, true, op);
+
+		closure_sync(&op->cl);
+		bch_btree_set_root(n3);
+		rw_unlock(true, n3);
+	} else if (root) {
+		op->keys.top = op->keys.bottom;
+		closure_sync(&op->cl);
+		bch_btree_set_root(n1);
+	} else {
+		unsigned i;
+
+		bkey_copy(op->keys.top, &b->key);
+		bkey_copy_key(op->keys.top, &ZERO_KEY);
+
+		for (i = 0; i < KEY_PTRS(&b->key); i++) {
+			uint8_t g = PTR_BUCKET(b->c, &b->key, i)->gen + 1;
+
+			SET_PTR_GEN(op->keys.top, i, g);
+		}
+
+		bch_keylist_push(&op->keys);
+		closure_sync(&op->cl);
+		atomic_inc(&b->c->prio_blocked);
+	}
+
+	rw_unlock(true, n1);
+	btree_node_free(b, op);
+
+	bch_time_stats_update(&b->c->btree_split_time, start_time);
+
+	return 0;
+err_free2:
+	__bkey_put(n2->c, &n2->key);
+	btree_node_free(n2, op);
+	rw_unlock(true, n2);
+err_free1:
+	__bkey_put(n1->c, &n1->key);
+	btree_node_free(n1, op);
+	rw_unlock(true, n1);
+err:
+	if (n3 == ERR_PTR(-EAGAIN) ||
+	    n2 == ERR_PTR(-EAGAIN) ||
+	    n1 == ERR_PTR(-EAGAIN))
+		return -EAGAIN;
+
+	pr_warn("couldn't split");
+	return -ENOMEM;
+}
+
+static int bch_btree_insert_recurse(struct btree *b, struct btree_op *op,
+				    struct keylist *stack_keys)
+{
+	if (b->level) {
+		int ret;
+		struct bkey *insert = op->keys.bottom;
+		struct bkey *k = bch_next_recurse_key(b, &START_KEY(insert));
+
+		if (!k) {
+			btree_bug(b, "no key to recurse on at level %i/%i",
+				  b->level, b->c->root->level);
+
+			op->keys.top = op->keys.bottom;
+			return -EIO;
+		}
+
+		if (bkey_cmp(insert, k) > 0) {
+			unsigned i;
+
+			if (op->type == BTREE_REPLACE) {
+				__bkey_put(b->c, insert);
+				op->keys.top = op->keys.bottom;
+				op->insert_collision = true;
+				return 0;
+			}
+
+			for (i = 0; i < KEY_PTRS(insert); i++)
+				atomic_inc(&PTR_BUCKET(b->c, insert, i)->pin);
+
+			bkey_copy(stack_keys->top, insert);
+
+			bch_cut_back(k, insert);
+			bch_cut_front(k, stack_keys->top);
+
+			bch_keylist_push(stack_keys);
+		}
+
+		ret = btree(insert_recurse, k, b, op, stack_keys);
+		if (ret)
+			return ret;
+	}
+
+	if (!bch_keylist_empty(&op->keys)) {
+		if (should_split(b)) {
+			if (op->lock <= b->c->root->level) {
+				BUG_ON(b->level);
+				op->lock = b->c->root->level + 1;
+				return -EINTR;
+			}
+			return btree_split(b, op);
+		}
+
+		BUG_ON(write_block(b) != b->sets[b->nsets].data);
+
+		if (bch_btree_insert_keys(b, op))
+			bch_btree_write(b, false, op);
+	}
+
+	return 0;
+}
+
+int bch_btree_insert(struct btree_op *op, struct cache_set *c)
+{
+	int ret = 0;
+	struct keylist stack_keys;
+
+	/*
+	 * Don't want to block with the btree locked unless we have to,
+	 * otherwise we get deadlocks with try_harder and between split/gc
+	 */
+	clear_closure_blocking(&op->cl);
+
+	BUG_ON(bch_keylist_empty(&op->keys));
+	bch_keylist_copy(&stack_keys, &op->keys);
+	bch_keylist_init(&op->keys);
+
+	while (!bch_keylist_empty(&stack_keys) ||
+	       !bch_keylist_empty(&op->keys)) {
+		if (bch_keylist_empty(&op->keys)) {
+			bch_keylist_add(&op->keys,
+					bch_keylist_pop(&stack_keys));
+			op->lock = 0;
+		}
+
+		ret = btree_root(insert_recurse, c, op, &stack_keys);
+
+		if (ret == -EAGAIN) {
+			ret = 0;
+			closure_sync(&op->cl);
+		} else if (ret) {
+			struct bkey *k;
+
+			pr_err("error %i trying to insert key for %s",
+			       ret, op_type(op));
+
+			while ((k = bch_keylist_pop(&stack_keys) ?:
+				    bch_keylist_pop(&op->keys)))
+				bkey_put(c, k, 0);
+		}
+	}
+
+	bch_keylist_free(&stack_keys);
+
+	if (op->journal)
+		atomic_dec_bug(op->journal);
+	op->journal = NULL;
+	return ret;
+}
+
+void bch_btree_set_root(struct btree *b)
+{
+	unsigned i;
+
+	BUG_ON(!b->written);
+
+	for (i = 0; i < KEY_PTRS(&b->key); i++)
+		BUG_ON(PTR_BUCKET(b->c, &b->key, i)->prio != BTREE_PRIO);
+
+	mutex_lock(&b->c->bucket_lock);
+	list_del_init(&b->list);
+	mutex_unlock(&b->c->bucket_lock);
+
+	b->c->root = b;
+	__bkey_put(b->c, &b->key);
+
+	bch_journal_meta(b->c, NULL);
+	pr_debug("%s for %pf", pbtree(b), __builtin_return_address(0));
+}
+
+/* Cache lookup */
+
+static int submit_partial_cache_miss(struct btree *b, struct btree_op *op,
+				     struct bkey *k)
+{
+	struct search *s = container_of(op, struct search, op);
+	struct bio *bio = &s->bio.bio;
+	int ret = 0;
+
+	while (!ret &&
+	       !op->lookup_done) {
+		unsigned sectors = INT_MAX;
+
+		if (KEY_INODE(k) == op->inode) {
+			if (KEY_START(k) <= bio->bi_sector)
+				break;
+
+			sectors = min_t(uint64_t, sectors,
+					KEY_START(k) - bio->bi_sector);
+		}
+
+		ret = s->d->cache_miss(b, s, bio, sectors);
+	}
+
+	return ret;
+}
+
+/*
+ * Read from a single key, handling the initial cache miss if the key starts in
+ * the middle of the bio
+ */
+static int submit_partial_cache_hit(struct btree *b, struct btree_op *op,
+				    struct bkey *k)
+{
+	struct search *s = container_of(op, struct search, op);
+	struct bio *bio = &s->bio.bio;
+	unsigned ptr;
+	struct bio *n;
+
+	int ret = submit_partial_cache_miss(b, op, k);
+	if (ret || op->lookup_done)
+		return ret;
+
+	/* XXX: figure out best pointer - for multiple cache devices */
+	ptr = 0;
+
+	PTR_BUCKET(b->c, k, ptr)->prio = INITIAL_PRIO;
+
+	while (!op->lookup_done &&
+	       KEY_INODE(k) == op->inode &&
+	       bio->bi_sector < KEY_OFFSET(k)) {
+		struct bkey *bio_key;
+		sector_t sector = PTR_OFFSET(k, ptr) +
+			(bio->bi_sector - KEY_START(k));
+		unsigned sectors = min_t(uint64_t, INT_MAX,
+					 KEY_OFFSET(k) - bio->bi_sector);
+
+		n = bch_bio_split(bio, sectors, GFP_NOIO, s->d->bio_split);
+		if (!n)
+			return -EAGAIN;
+
+		if (n == bio)
+			op->lookup_done = true;
+
+		bio_key = &container_of(n, struct bbio, bio)->key;
+
+		/*
+		 * The bucket we're reading from might be reused while our bio
+		 * is in flight, and we could then end up reading the wrong
+		 * data.
+		 *
+		 * We guard against this by checking (in cache_read_endio()) if
+		 * the pointer is stale again; if so, we treat it as an error
+		 * and reread from the backing device (but we don't pass that
+		 * error up anywhere).
+		 */
+
+		bch_bkey_copy_single_ptr(bio_key, k, ptr);
+		SET_PTR_OFFSET(bio_key, 0, sector);
+
+		n->bi_end_io	= bch_cache_read_endio;
+		n->bi_private	= &s->cl;
+
+		trace_bcache_cache_hit(n);
+		__bch_submit_bbio(n, b->c);
+	}
+
+	return 0;
+}
+
+int bch_btree_search_recurse(struct btree *b, struct btree_op *op)
+{
+	struct search *s = container_of(op, struct search, op);
+	struct bio *bio = &s->bio.bio;
+
+	int ret = 0;
+	struct bkey *k;
+	struct btree_iter iter;
+	bch_btree_iter_init(b, &iter, &KEY(op->inode, bio->bi_sector, 0));
+
+	pr_debug("at %s searching for %u:%llu", pbtree(b), op->inode,
+		 (uint64_t) bio->bi_sector);
+
+	do {
+		k = bch_btree_iter_next_filter(&iter, b, bch_ptr_bad);
+		if (!k) {
+			/*
+			 * b->key would be exactly what we want, except that
+			 * pointers to btree nodes have nonzero size - we
+			 * wouldn't go far enough
+			 */
+
+			ret = submit_partial_cache_miss(b, op,
+					&KEY(KEY_INODE(&b->key),
+					     KEY_OFFSET(&b->key), 0));
+			break;
+		}
+
+		ret = b->level
+			? btree(search_recurse, k, b, op)
+			: submit_partial_cache_hit(b, op, k);
+	} while (!ret &&
+		 !op->lookup_done);
+
+	return ret;
+}
+
+/* Keybuf code */
+
+static inline int keybuf_cmp(struct keybuf_key *l, struct keybuf_key *r)
+{
+	/* Overlapping keys compare equal */
+	if (bkey_cmp(&l->key, &START_KEY(&r->key)) <= 0)
+		return -1;
+	if (bkey_cmp(&START_KEY(&l->key), &r->key) >= 0)
+		return 1;
+	return 0;
+}
+
+static inline int keybuf_nonoverlapping_cmp(struct keybuf_key *l,
+					    struct keybuf_key *r)
+{
+	return clamp_t(int64_t, bkey_cmp(&l->key, &r->key), -1, 1);
+}
+
+static int bch_btree_refill_keybuf(struct btree *b, struct btree_op *op,
+				   struct keybuf *buf, struct bkey *end)
+{
+	struct btree_iter iter;
+	bch_btree_iter_init(b, &iter, &buf->last_scanned);
+
+	while (!array_freelist_empty(&buf->freelist)) {
+		struct bkey *k = bch_btree_iter_next_filter(&iter, b,
+							    bch_ptr_bad);
+
+		if (!b->level) {
+			if (!k) {
+				buf->last_scanned = b->key;
+				break;
+			}
+
+			buf->last_scanned = *k;
+			if (bkey_cmp(&buf->last_scanned, end) >= 0)
+				break;
+
+			if (buf->key_predicate(buf, k)) {
+				struct keybuf_key *w;
+
+				pr_debug("%s", pkey(k));
+
+				spin_lock(&buf->lock);
+
+				w = array_alloc(&buf->freelist);
+
+				w->private = NULL;
+				bkey_copy(&w->key, k);
+
+				if (RB_INSERT(&buf->keys, w, node, keybuf_cmp))
+					array_free(&buf->freelist, w);
+
+				spin_unlock(&buf->lock);
+			}
+		} else {
+			if (!k)
+				break;
+
+			btree(refill_keybuf, k, b, op, buf, end);
+			/*
+			 * Might get an error here, but can't really do anything
+			 * and it'll get logged elsewhere. Just read what we
+			 * can.
+			 */
+
+			if (bkey_cmp(&buf->last_scanned, end) >= 0)
+				break;
+
+			cond_resched();
+		}
+	}
+
+	return 0;
+}
+
+void bch_refill_keybuf(struct cache_set *c, struct keybuf *buf,
+			  struct bkey *end)
+{
+	struct bkey start = buf->last_scanned;
+	struct btree_op op;
+	bch_btree_op_init_stack(&op);
+
+	cond_resched();
+
+	btree_root(refill_keybuf, c, &op, buf, end);
+	closure_sync(&op.cl);
+
+	pr_debug("found %s keys from %llu:%llu to %llu:%llu",
+		 RB_EMPTY_ROOT(&buf->keys) ? "no" :
+		 array_freelist_empty(&buf->freelist) ? "some" : "a few",
+		 KEY_INODE(&start), KEY_OFFSET(&start),
+		 KEY_INODE(&buf->last_scanned), KEY_OFFSET(&buf->last_scanned));
+
+	spin_lock(&buf->lock);
+
+	if (!RB_EMPTY_ROOT(&buf->keys)) {
+		struct keybuf_key *w;
+		w = RB_FIRST(&buf->keys, struct keybuf_key, node);
+		buf->start	= START_KEY(&w->key);
+
+		w = RB_LAST(&buf->keys, struct keybuf_key, node);
+		buf->end	= w->key;
+	} else {
+		buf->start	= MAX_KEY;
+		buf->end	= MAX_KEY;
+	}
+
+	spin_unlock(&buf->lock);
+}
+
+static void __bch_keybuf_del(struct keybuf *buf, struct keybuf_key *w)
+{
+	rb_erase(&w->node, &buf->keys);
+	array_free(&buf->freelist, w);
+}
+
+void bch_keybuf_del(struct keybuf *buf, struct keybuf_key *w)
+{
+	spin_lock(&buf->lock);
+	__bch_keybuf_del(buf, w);
+	spin_unlock(&buf->lock);
+}
+
+bool bch_keybuf_check_overlapping(struct keybuf *buf, struct bkey *start,
+				  struct bkey *end)
+{
+	bool ret = false;
+	struct keybuf_key *p, *w, s;
+	s.key = *start;
+
+	if (bkey_cmp(end, &buf->start) <= 0 ||
+	    bkey_cmp(start, &buf->end) >= 0)
+		return false;
+
+	spin_lock(&buf->lock);
+	w = RB_GREATER(&buf->keys, s, node, keybuf_nonoverlapping_cmp);
+
+	while (w && bkey_cmp(&START_KEY(&w->key), end) < 0) {
+		p = w;
+		w = RB_NEXT(w, node);
+
+		if (p->private)
+			ret = true;
+		else
+			__bch_keybuf_del(buf, p);
+	}
+
+	spin_unlock(&buf->lock);
+	return ret;
+}
+
+struct keybuf_key *bch_keybuf_next(struct keybuf *buf)
+{
+	struct keybuf_key *w;
+	spin_lock(&buf->lock);
+
+	w = RB_FIRST(&buf->keys, struct keybuf_key, node);
+
+	while (w && w->private)
+		w = RB_NEXT(w, node);
+
+	if (w)
+		w->private = ERR_PTR(-EINTR);
+
+	spin_unlock(&buf->lock);
+	return w;
+}
+
+struct keybuf_key *bch_keybuf_next_rescan(struct cache_set *c,
+					     struct keybuf *buf,
+					     struct bkey *end)
+{
+	struct keybuf_key *ret;
+
+	while (1) {
+		ret = bch_keybuf_next(buf);
+		if (ret)
+			break;
+
+		if (bkey_cmp(&buf->last_scanned, end) >= 0) {
+			pr_debug("scan finished");
+			break;
+		}
+
+		bch_refill_keybuf(c, buf, end);
+	}
+
+	return ret;
+}
+
+void bch_keybuf_init(struct keybuf *buf, keybuf_pred_fn *fn)
+{
+	buf->key_predicate	= fn;
+	buf->last_scanned	= MAX_KEY;
+	buf->keys		= RB_ROOT;
+
+	spin_lock_init(&buf->lock);
+	array_allocator_init(&buf->freelist);
+}
+
+void bch_btree_exit(void)
+{
+	if (btree_io_wq)
+		destroy_workqueue(btree_io_wq);
+	if (bch_gc_wq)
+		destroy_workqueue(bch_gc_wq);
+}
+
+int __init bch_btree_init(void)
+{
+	if (!(bch_gc_wq = create_singlethread_workqueue("bch_btree_gc")) ||
+	    !(btree_io_wq = create_singlethread_workqueue("bch_btree_io")))
+		return -ENOMEM;
+
+	return 0;
+}
diff --git a/drivers/md/bcache/btree.h b/drivers/md/bcache/btree.h
new file mode 100644
index 000000000000..af4a7092a28c
--- /dev/null
+++ b/drivers/md/bcache/btree.h
@@ -0,0 +1,405 @@
+#ifndef _BCACHE_BTREE_H
+#define _BCACHE_BTREE_H
+
+/*
+ * THE BTREE:
+ *
+ * At a high level, bcache's btree is relatively standard b+ tree. All keys and
+ * pointers are in the leaves; interior nodes only have pointers to the child
+ * nodes.
+ *
+ * In the interior nodes, a struct bkey always points to a child btree node, and
+ * the key is the highest key in the child node - except that the highest key in
+ * an interior node is always MAX_KEY. The size field refers to the size on disk
+ * of the child node - this would allow us to have variable sized btree nodes
+ * (handy for keeping the depth of the btree 1 by expanding just the root).
+ *
+ * Btree nodes are themselves log structured, but this is hidden fairly
+ * thoroughly. Btree nodes on disk will in practice have extents that overlap
+ * (because they were written at different times), but in memory we never have
+ * overlapping extents - when we read in a btree node from disk, the first thing
+ * we do is resort all the sets of keys with a mergesort, and in the same pass
+ * we check for overlapping extents and adjust them appropriately.
+ *
+ * struct btree_op is a central interface to the btree code. It's used for
+ * specifying read vs. write locking, and the embedded closure is used for
+ * waiting on IO or reserve memory.
+ *
+ * BTREE CACHE:
+ *
+ * Btree nodes are cached in memory; traversing the btree might require reading
+ * in btree nodes which is handled mostly transparently.
+ *
+ * bch_btree_node_get() looks up a btree node in the cache and reads it in from
+ * disk if necessary. This function is almost never called directly though - the
+ * btree() macro is used to get a btree node, call some function on it, and
+ * unlock the node after the function returns.
+ *
+ * The root is special cased - it's taken out of the cache's lru (thus pinning
+ * it in memory), so we can find the root of the btree by just dereferencing a
+ * pointer instead of looking it up in the cache. This makes locking a bit
+ * tricky, since the root pointer is protected by the lock in the btree node it
+ * points to - the btree_root() macro handles this.
+ *
+ * In various places we must be able to allocate memory for multiple btree nodes
+ * in order to make forward progress. To do this we use the btree cache itself
+ * as a reserve; if __get_free_pages() fails, we'll find a node in the btree
+ * cache we can reuse. We can't allow more than one thread to be doing this at a
+ * time, so there's a lock, implemented by a pointer to the btree_op closure -
+ * this allows the btree_root() macro to implicitly release this lock.
+ *
+ * BTREE IO:
+ *
+ * Btree nodes never have to be explicitly read in; bch_btree_node_get() handles
+ * this.
+ *
+ * For writing, we have two btree_write structs embeddded in struct btree - one
+ * write in flight, and one being set up, and we toggle between them.
+ *
+ * Writing is done with a single function -  bch_btree_write() really serves two
+ * different purposes and should be broken up into two different functions. When
+ * passing now = false, it merely indicates that the node is now dirty - calling
+ * it ensures that the dirty keys will be written at some point in the future.
+ *
+ * When passing now = true, bch_btree_write() causes a write to happen
+ * "immediately" (if there was already a write in flight, it'll cause the write
+ * to happen as soon as the previous write completes). It returns immediately
+ * though - but it takes a refcount on the closure in struct btree_op you passed
+ * to it, so a closure_sync() later can be used to wait for the write to
+ * complete.
+ *
+ * This is handy because btree_split() and garbage collection can issue writes
+ * in parallel, reducing the amount of time they have to hold write locks.
+ *
+ * LOCKING:
+ *
+ * When traversing the btree, we may need write locks starting at some level -
+ * inserting a key into the btree will typically only require a write lock on
+ * the leaf node.
+ *
+ * This is specified with the lock field in struct btree_op; lock = 0 means we
+ * take write locks at level <= 0, i.e. only leaf nodes. bch_btree_node_get()
+ * checks this field and returns the node with the appropriate lock held.
+ *
+ * If, after traversing the btree, the insertion code discovers it has to split
+ * then it must restart from the root and take new locks - to do this it changes
+ * the lock field and returns -EINTR, which causes the btree_root() macro to
+ * loop.
+ *
+ * Handling cache misses require a different mechanism for upgrading to a write
+ * lock. We do cache lookups with only a read lock held, but if we get a cache
+ * miss and we wish to insert this data into the cache, we have to insert a
+ * placeholder key to detect races - otherwise, we could race with a write and
+ * overwrite the data that was just written to the cache with stale data from
+ * the backing device.
+ *
+ * For this we use a sequence number that write locks and unlocks increment - to
+ * insert the check key it unlocks the btree node and then takes a write lock,
+ * and fails if the sequence number doesn't match.
+ */
+
+#include "bset.h"
+#include "debug.h"
+
+struct btree_write {
+	struct closure		*owner;
+	atomic_t		*journal;
+
+	/* If btree_split() frees a btree node, it writes a new pointer to that
+	 * btree node indicating it was freed; it takes a refcount on
+	 * c->prio_blocked because we can't write the gens until the new
+	 * pointer is on disk. This allows btree_write_endio() to release the
+	 * refcount that btree_split() took.
+	 */
+	int			prio_blocked;
+};
+
+struct btree {
+	/* Hottest entries first */
+	struct hlist_node	hash;
+
+	/* Key/pointer for this btree node */
+	BKEY_PADDED(key);
+
+	/* Single bit - set when accessed, cleared by shrinker */
+	unsigned long		accessed;
+	unsigned long		seq;
+	struct rw_semaphore	lock;
+	struct cache_set	*c;
+
+	unsigned long		flags;
+	uint16_t		written;	/* would be nice to kill */
+	uint8_t			level;
+	uint8_t			nsets;
+	uint8_t			page_order;
+
+	/*
+	 * Set of sorted keys - the real btree node - plus a binary search tree
+	 *
+	 * sets[0] is special; set[0]->tree, set[0]->prev and set[0]->data point
+	 * to the memory we have allocated for this btree node. Additionally,
+	 * set[0]->data points to the entire btree node as it exists on disk.
+	 */
+	struct bset_tree	sets[MAX_BSETS];
+
+	/* Used to refcount bio splits, also protects b->bio */
+	struct closure_with_waitlist	io;
+
+	/* Gets transferred to w->prio_blocked - see the comment there */
+	int			prio_blocked;
+
+	struct list_head	list;
+	struct delayed_work	work;
+
+	uint64_t		io_start_time;
+	struct btree_write	writes[2];
+	struct bio		*bio;
+};
+
+#define BTREE_FLAG(flag)						\
+static inline bool btree_node_ ## flag(struct btree *b)			\
+{	return test_bit(BTREE_NODE_ ## flag, &b->flags); }		\
+									\
+static inline void set_btree_node_ ## flag(struct btree *b)		\
+{	set_bit(BTREE_NODE_ ## flag, &b->flags); }			\
+
+enum btree_flags {
+	BTREE_NODE_read_done,
+	BTREE_NODE_io_error,
+	BTREE_NODE_dirty,
+	BTREE_NODE_write_idx,
+};
+
+BTREE_FLAG(read_done);
+BTREE_FLAG(io_error);
+BTREE_FLAG(dirty);
+BTREE_FLAG(write_idx);
+
+static inline struct btree_write *btree_current_write(struct btree *b)
+{
+	return b->writes + btree_node_write_idx(b);
+}
+
+static inline struct btree_write *btree_prev_write(struct btree *b)
+{
+	return b->writes + (btree_node_write_idx(b) ^ 1);
+}
+
+static inline unsigned bset_offset(struct btree *b, struct bset *i)
+{
+	return (((size_t) i) - ((size_t) b->sets->data)) >> 9;
+}
+
+static inline struct bset *write_block(struct btree *b)
+{
+	return ((void *) b->sets[0].data) + b->written * block_bytes(b->c);
+}
+
+static inline bool bset_written(struct btree *b, struct bset_tree *t)
+{
+	return t->data < write_block(b);
+}
+
+static inline bool bkey_written(struct btree *b, struct bkey *k)
+{
+	return k < write_block(b)->start;
+}
+
+static inline void set_gc_sectors(struct cache_set *c)
+{
+	atomic_set(&c->sectors_to_gc, c->sb.bucket_size * c->nbuckets / 8);
+}
+
+static inline bool bch_ptr_invalid(struct btree *b, const struct bkey *k)
+{
+	return __bch_ptr_invalid(b->c, b->level, k);
+}
+
+static inline struct bkey *bch_btree_iter_init(struct btree *b,
+					       struct btree_iter *iter,
+					       struct bkey *search)
+{
+	return __bch_btree_iter_init(b, iter, search, b->sets);
+}
+
+/* Looping macros */
+
+#define for_each_cached_btree(b, c, iter)				\
+	for (iter = 0;							\
+	     iter < ARRAY_SIZE((c)->bucket_hash);			\
+	     iter++)							\
+		hlist_for_each_entry_rcu((b), (c)->bucket_hash + iter, hash)
+
+#define for_each_key_filter(b, k, iter, filter)				\
+	for (bch_btree_iter_init((b), (iter), NULL);			\
+	     ((k) = bch_btree_iter_next_filter((iter), b, filter));)
+
+#define for_each_key(b, k, iter)					\
+	for (bch_btree_iter_init((b), (iter), NULL);			\
+	     ((k) = bch_btree_iter_next(iter));)
+
+/* Recursing down the btree */
+
+struct btree_op {
+	struct closure		cl;
+	struct cache_set	*c;
+
+	/* Journal entry we have a refcount on */
+	atomic_t		*journal;
+
+	/* Bio to be inserted into the cache */
+	struct bio		*cache_bio;
+
+	unsigned		inode;
+
+	uint16_t		write_prio;
+
+	/* Btree level at which we start taking write locks */
+	short			lock;
+
+	/* Btree insertion type */
+	enum {
+		BTREE_INSERT,
+		BTREE_REPLACE
+	} type:8;
+
+	unsigned		csum:1;
+	unsigned		skip:1;
+	unsigned		flush_journal:1;
+
+	unsigned		insert_data_done:1;
+	unsigned		lookup_done:1;
+	unsigned		insert_collision:1;
+
+	/* Anything after this point won't get zeroed in do_bio_hook() */
+
+	/* Keys to be inserted */
+	struct keylist		keys;
+	BKEY_PADDED(replace);
+};
+
+void bch_btree_op_init_stack(struct btree_op *);
+
+static inline void rw_lock(bool w, struct btree *b, int level)
+{
+	w ? down_write_nested(&b->lock, level + 1)
+	  : down_read_nested(&b->lock, level + 1);
+	if (w)
+		b->seq++;
+}
+
+static inline void rw_unlock(bool w, struct btree *b)
+{
+#ifdef CONFIG_BCACHE_EDEBUG
+	unsigned i;
+
+	if (w &&
+	    b->key.ptr[0] &&
+	    btree_node_read_done(b))
+		for (i = 0; i <= b->nsets; i++)
+			bch_check_key_order(b, b->sets[i].data);
+#endif
+
+	if (w)
+		b->seq++;
+	(w ? up_write : up_read)(&b->lock);
+}
+
+#define insert_lock(s, b)	((b)->level <= (s)->lock)
+
+/*
+ * These macros are for recursing down the btree - they handle the details of
+ * locking and looking up nodes in the cache for you. They're best treated as
+ * mere syntax when reading code that uses them.
+ *
+ * op->lock determines whether we take a read or a write lock at a given depth.
+ * If you've got a read lock and find that you need a write lock (i.e. you're
+ * going to have to split), set op->lock and return -EINTR; btree_root() will
+ * call you again and you'll have the correct lock.
+ */
+
+/**
+ * btree - recurse down the btree on a specified key
+ * @fn:		function to call, which will be passed the child node
+ * @key:	key to recurse on
+ * @b:		parent btree node
+ * @op:		pointer to struct btree_op
+ */
+#define btree(fn, key, b, op, ...)					\
+({									\
+	int _r, l = (b)->level - 1;					\
+	bool _w = l <= (op)->lock;					\
+	struct btree *_b = bch_btree_node_get((b)->c, key, l, op);	\
+	if (!IS_ERR(_b)) {						\
+		_r = bch_btree_ ## fn(_b, op, ##__VA_ARGS__);		\
+		rw_unlock(_w, _b);					\
+	} else								\
+		_r = PTR_ERR(_b);					\
+	_r;								\
+})
+
+/**
+ * btree_root - call a function on the root of the btree
+ * @fn:		function to call, which will be passed the child node
+ * @c:		cache set
+ * @op:		pointer to struct btree_op
+ */
+#define btree_root(fn, c, op, ...)					\
+({									\
+	int _r = -EINTR;						\
+	do {								\
+		struct btree *_b = (c)->root;				\
+		bool _w = insert_lock(op, _b);				\
+		rw_lock(_w, _b, _b->level);				\
+		if (_b == (c)->root &&					\
+		    _w == insert_lock(op, _b))				\
+			_r = bch_btree_ ## fn(_b, op, ##__VA_ARGS__);	\
+		rw_unlock(_w, _b);					\
+		bch_cannibalize_unlock(c, &(op)->cl);		\
+	} while (_r == -EINTR);						\
+									\
+	_r;								\
+})
+
+static inline bool should_split(struct btree *b)
+{
+	struct bset *i = write_block(b);
+	return b->written >= btree_blocks(b) ||
+		(i->seq == b->sets[0].data->seq &&
+		 b->written + __set_blocks(i, i->keys + 15, b->c)
+		 > btree_blocks(b));
+}
+
+void bch_btree_read_done(struct closure *);
+void bch_btree_read(struct btree *);
+void bch_btree_write(struct btree *b, bool now, struct btree_op *op);
+
+void bch_cannibalize_unlock(struct cache_set *, struct closure *);
+void bch_btree_set_root(struct btree *);
+struct btree *bch_btree_node_alloc(struct cache_set *, int, struct closure *);
+struct btree *bch_btree_node_get(struct cache_set *, struct bkey *,
+				int, struct btree_op *);
+
+bool bch_btree_insert_keys(struct btree *, struct btree_op *);
+bool bch_btree_insert_check_key(struct btree *, struct btree_op *,
+				   struct bio *);
+int bch_btree_insert(struct btree_op *, struct cache_set *);
+
+int bch_btree_search_recurse(struct btree *, struct btree_op *);
+
+void bch_queue_gc(struct cache_set *);
+size_t bch_btree_gc_finish(struct cache_set *);
+void bch_moving_gc(struct closure *);
+int bch_btree_check(struct cache_set *, struct btree_op *);
+uint8_t __bch_btree_mark_key(struct cache_set *, int, struct bkey *);
+
+void bch_keybuf_init(struct keybuf *, keybuf_pred_fn *);
+void bch_refill_keybuf(struct cache_set *, struct keybuf *, struct bkey *);
+bool bch_keybuf_check_overlapping(struct keybuf *, struct bkey *,
+				  struct bkey *);
+void bch_keybuf_del(struct keybuf *, struct keybuf_key *);
+struct keybuf_key *bch_keybuf_next(struct keybuf *);
+struct keybuf_key *bch_keybuf_next_rescan(struct cache_set *,
+					  struct keybuf *, struct bkey *);
+
+#endif
diff --git a/drivers/md/bcache/closure.c b/drivers/md/bcache/closure.c
new file mode 100644
index 000000000000..bd05a9a8c7cf
--- /dev/null
+++ b/drivers/md/bcache/closure.c
@@ -0,0 +1,345 @@
+/*
+ * Asynchronous refcounty things
+ *
+ * Copyright 2010, 2011 Kent Overstreet <kent.overstreet@gmail.com>
+ * Copyright 2012 Google, Inc.
+ */
+
+#include <linux/debugfs.h>
+#include <linux/module.h>
+#include <linux/seq_file.h>
+
+#include "closure.h"
+
+void closure_queue(struct closure *cl)
+{
+	struct workqueue_struct *wq = cl->wq;
+	if (wq) {
+		INIT_WORK(&cl->work, cl->work.func);
+		BUG_ON(!queue_work(wq, &cl->work));
+	} else
+		cl->fn(cl);
+}
+EXPORT_SYMBOL_GPL(closure_queue);
+
+#define CL_FIELD(type, field)					\
+	case TYPE_ ## type:					\
+	return &container_of(cl, struct type, cl)->field
+
+static struct closure_waitlist *closure_waitlist(struct closure *cl)
+{
+	switch (cl->type) {
+		CL_FIELD(closure_with_waitlist, wait);
+		CL_FIELD(closure_with_waitlist_and_timer, wait);
+	default:
+		return NULL;
+	}
+}
+
+static struct timer_list *closure_timer(struct closure *cl)
+{
+	switch (cl->type) {
+		CL_FIELD(closure_with_timer, timer);
+		CL_FIELD(closure_with_waitlist_and_timer, timer);
+	default:
+		return NULL;
+	}
+}
+
+static inline void closure_put_after_sub(struct closure *cl, int flags)
+{
+	int r = flags & CLOSURE_REMAINING_MASK;
+
+	BUG_ON(flags & CLOSURE_GUARD_MASK);
+	BUG_ON(!r && (flags & ~(CLOSURE_DESTRUCTOR|CLOSURE_BLOCKING)));
+
+	/* Must deliver precisely one wakeup */
+	if (r == 1 && (flags & CLOSURE_SLEEPING))
+		wake_up_process(cl->task);
+
+	if (!r) {
+		if (cl->fn && !(flags & CLOSURE_DESTRUCTOR)) {
+			/* CLOSURE_BLOCKING might be set - clear it */
+			atomic_set(&cl->remaining,
+				   CLOSURE_REMAINING_INITIALIZER);
+			closure_queue(cl);
+		} else {
+			struct closure *parent = cl->parent;
+			struct closure_waitlist *wait = closure_waitlist(cl);
+
+			closure_debug_destroy(cl);
+
+			atomic_set(&cl->remaining, -1);
+
+			if (wait)
+				closure_wake_up(wait);
+
+			if (cl->fn)
+				cl->fn(cl);
+
+			if (parent)
+				closure_put(parent);
+		}
+	}
+}
+
+/* For clearing flags with the same atomic op as a put */
+void closure_sub(struct closure *cl, int v)
+{
+	closure_put_after_sub(cl, atomic_sub_return(v, &cl->remaining));
+}
+EXPORT_SYMBOL_GPL(closure_sub);
+
+void closure_put(struct closure *cl)
+{
+	closure_put_after_sub(cl, atomic_dec_return(&cl->remaining));
+}
+EXPORT_SYMBOL_GPL(closure_put);
+
+static void set_waiting(struct closure *cl, unsigned long f)
+{
+#ifdef CONFIG_BCACHE_CLOSURES_DEBUG
+	cl->waiting_on = f;
+#endif
+}
+
+void __closure_wake_up(struct closure_waitlist *wait_list)
+{
+	struct llist_node *list;
+	struct closure *cl;
+	struct llist_node *reverse = NULL;
+
+	list = llist_del_all(&wait_list->list);
+
+	/* We first reverse the list to preserve FIFO ordering and fairness */
+
+	while (list) {
+		struct llist_node *t = list;
+		list = llist_next(list);
+
+		t->next = reverse;
+		reverse = t;
+	}
+
+	/* Then do the wakeups */
+
+	while (reverse) {
+		cl = container_of(reverse, struct closure, list);
+		reverse = llist_next(reverse);
+
+		set_waiting(cl, 0);
+		closure_sub(cl, CLOSURE_WAITING + 1);
+	}
+}
+EXPORT_SYMBOL_GPL(__closure_wake_up);
+
+bool closure_wait(struct closure_waitlist *list, struct closure *cl)
+{
+	if (atomic_read(&cl->remaining) & CLOSURE_WAITING)
+		return false;
+
+	set_waiting(cl, _RET_IP_);
+	atomic_add(CLOSURE_WAITING + 1, &cl->remaining);
+	llist_add(&cl->list, &list->list);
+
+	return true;
+}
+EXPORT_SYMBOL_GPL(closure_wait);
+
+/**
+ * closure_sync() - sleep until a closure a closure has nothing left to wait on
+ *
+ * Sleeps until the refcount hits 1 - the thread that's running the closure owns
+ * the last refcount.
+ */
+void closure_sync(struct closure *cl)
+{
+	while (1) {
+		__closure_start_sleep(cl);
+		closure_set_ret_ip(cl);
+
+		if ((atomic_read(&cl->remaining) &
+		     CLOSURE_REMAINING_MASK) == 1)
+			break;
+
+		schedule();
+	}
+
+	__closure_end_sleep(cl);
+}
+EXPORT_SYMBOL_GPL(closure_sync);
+
+/**
+ * closure_trylock() - try to acquire the closure, without waiting
+ * @cl:		closure to lock
+ *
+ * Returns true if the closure was succesfully locked.
+ */
+bool closure_trylock(struct closure *cl, struct closure *parent)
+{
+	if (atomic_cmpxchg(&cl->remaining, -1,
+			   CLOSURE_REMAINING_INITIALIZER) != -1)
+		return false;
+
+	closure_set_ret_ip(cl);
+
+	smp_mb();
+	cl->parent = parent;
+	if (parent)
+		closure_get(parent);
+
+	closure_debug_create(cl);
+	return true;
+}
+EXPORT_SYMBOL_GPL(closure_trylock);
+
+void __closure_lock(struct closure *cl, struct closure *parent,
+		    struct closure_waitlist *wait_list)
+{
+	struct closure wait;
+	closure_init_stack(&wait);
+
+	while (1) {
+		if (closure_trylock(cl, parent))
+			return;
+
+		closure_wait_event_sync(wait_list, &wait,
+					atomic_read(&cl->remaining) == -1);
+	}
+}
+EXPORT_SYMBOL_GPL(__closure_lock);
+
+static void closure_delay_timer_fn(unsigned long data)
+{
+	struct closure *cl = (struct closure *) data;
+	closure_sub(cl, CLOSURE_TIMER + 1);
+}
+
+void do_closure_timer_init(struct closure *cl)
+{
+	struct timer_list *timer = closure_timer(cl);
+
+	init_timer(timer);
+	timer->data	= (unsigned long) cl;
+	timer->function = closure_delay_timer_fn;
+}
+EXPORT_SYMBOL_GPL(do_closure_timer_init);
+
+bool __closure_delay(struct closure *cl, unsigned long delay,
+		     struct timer_list *timer)
+{
+	if (atomic_read(&cl->remaining) & CLOSURE_TIMER)
+		return false;
+
+	BUG_ON(timer_pending(timer));
+
+	timer->expires	= jiffies + delay;
+
+	atomic_add(CLOSURE_TIMER + 1, &cl->remaining);
+	add_timer(timer);
+	return true;
+}
+EXPORT_SYMBOL_GPL(__closure_delay);
+
+void __closure_flush(struct closure *cl, struct timer_list *timer)
+{
+	if (del_timer(timer))
+		closure_sub(cl, CLOSURE_TIMER + 1);
+}
+EXPORT_SYMBOL_GPL(__closure_flush);
+
+void __closure_flush_sync(struct closure *cl, struct timer_list *timer)
+{
+	if (del_timer_sync(timer))
+		closure_sub(cl, CLOSURE_TIMER + 1);
+}
+EXPORT_SYMBOL_GPL(__closure_flush_sync);
+
+#ifdef CONFIG_BCACHE_CLOSURES_DEBUG
+
+static LIST_HEAD(closure_list);
+static DEFINE_SPINLOCK(closure_list_lock);
+
+void closure_debug_create(struct closure *cl)
+{
+	unsigned long flags;
+
+	BUG_ON(cl->magic == CLOSURE_MAGIC_ALIVE);
+	cl->magic = CLOSURE_MAGIC_ALIVE;
+
+	spin_lock_irqsave(&closure_list_lock, flags);
+	list_add(&cl->all, &closure_list);
+	spin_unlock_irqrestore(&closure_list_lock, flags);
+}
+EXPORT_SYMBOL_GPL(closure_debug_create);
+
+void closure_debug_destroy(struct closure *cl)
+{
+	unsigned long flags;
+
+	BUG_ON(cl->magic != CLOSURE_MAGIC_ALIVE);
+	cl->magic = CLOSURE_MAGIC_DEAD;
+
+	spin_lock_irqsave(&closure_list_lock, flags);
+	list_del(&cl->all);
+	spin_unlock_irqrestore(&closure_list_lock, flags);
+}
+EXPORT_SYMBOL_GPL(closure_debug_destroy);
+
+static struct dentry *debug;
+
+#define work_data_bits(work) ((unsigned long *)(&(work)->data))
+
+static int debug_seq_show(struct seq_file *f, void *data)
+{
+	struct closure *cl;
+	spin_lock_irq(&closure_list_lock);
+
+	list_for_each_entry(cl, &closure_list, all) {
+		int r = atomic_read(&cl->remaining);
+
+		seq_printf(f, "%p: %pF -> %pf p %p r %i ",
+			   cl, (void *) cl->ip, cl->fn, cl->parent,
+			   r & CLOSURE_REMAINING_MASK);
+
+		seq_printf(f, "%s%s%s%s%s%s\n",
+			   test_bit(WORK_STRUCT_PENDING,
+				    work_data_bits(&cl->work)) ? "Q" : "",
+			   r & CLOSURE_RUNNING	? "R" : "",
+			   r & CLOSURE_BLOCKING	? "B" : "",
+			   r & CLOSURE_STACK	? "S" : "",
+			   r & CLOSURE_SLEEPING	? "Sl" : "",
+			   r & CLOSURE_TIMER	? "T" : "");
+
+		if (r & CLOSURE_WAITING)
+			seq_printf(f, " W %pF\n",
+				   (void *) cl->waiting_on);
+
+		seq_printf(f, "\n");
+	}
+
+	spin_unlock_irq(&closure_list_lock);
+	return 0;
+}
+
+static int debug_seq_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, debug_seq_show, NULL);
+}
+
+static const struct file_operations debug_ops = {
+	.owner		= THIS_MODULE,
+	.open		= debug_seq_open,
+	.read		= seq_read,
+	.release	= single_release
+};
+
+void __init closure_debug_init(void)
+{
+	debug = debugfs_create_file("closures", 0400, NULL, NULL, &debug_ops);
+}
+
+#endif
+
+MODULE_AUTHOR("Kent Overstreet <koverstreet@google.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/md/bcache/closure.h b/drivers/md/bcache/closure.h
new file mode 100644
index 000000000000..00039924ea9d
--- /dev/null
+++ b/drivers/md/bcache/closure.h
@@ -0,0 +1,672 @@
+#ifndef _LINUX_CLOSURE_H
+#define _LINUX_CLOSURE_H
+
+#include <linux/llist.h>
+#include <linux/sched.h>
+#include <linux/workqueue.h>
+
+/*
+ * Closure is perhaps the most overused and abused term in computer science, but
+ * since I've been unable to come up with anything better you're stuck with it
+ * again.
+ *
+ * What are closures?
+ *
+ * They embed a refcount. The basic idea is they count "things that are in
+ * progress" - in flight bios, some other thread that's doing something else -
+ * anything you might want to wait on.
+ *
+ * The refcount may be manipulated with closure_get() and closure_put().
+ * closure_put() is where many of the interesting things happen, when it causes
+ * the refcount to go to 0.
+ *
+ * Closures can be used to wait on things both synchronously and asynchronously,
+ * and synchronous and asynchronous use can be mixed without restriction. To
+ * wait synchronously, use closure_sync() - you will sleep until your closure's
+ * refcount hits 1.
+ *
+ * To wait asynchronously, use
+ *   continue_at(cl, next_function, workqueue);
+ *
+ * passing it, as you might expect, the function to run when nothing is pending
+ * and the workqueue to run that function out of.
+ *
+ * continue_at() also, critically, is a macro that returns the calling function.
+ * There's good reason for this.
+ *
+ * To use safely closures asynchronously, they must always have a refcount while
+ * they are running owned by the thread that is running them. Otherwise, suppose
+ * you submit some bios and wish to have a function run when they all complete:
+ *
+ * foo_endio(struct bio *bio, int error)
+ * {
+ *	closure_put(cl);
+ * }
+ *
+ * closure_init(cl);
+ *
+ * do_stuff();
+ * closure_get(cl);
+ * bio1->bi_endio = foo_endio;
+ * bio_submit(bio1);
+ *
+ * do_more_stuff();
+ * closure_get(cl);
+ * bio2->bi_endio = foo_endio;
+ * bio_submit(bio2);
+ *
+ * continue_at(cl, complete_some_read, system_wq);
+ *
+ * If closure's refcount started at 0, complete_some_read() could run before the
+ * second bio was submitted - which is almost always not what you want! More
+ * importantly, it wouldn't be possible to say whether the original thread or
+ * complete_some_read()'s thread owned the closure - and whatever state it was
+ * associated with!
+ *
+ * So, closure_init() initializes a closure's refcount to 1 - and when a
+ * closure_fn is run, the refcount will be reset to 1 first.
+ *
+ * Then, the rule is - if you got the refcount with closure_get(), release it
+ * with closure_put() (i.e, in a bio->bi_endio function). If you have a refcount
+ * on a closure because you called closure_init() or you were run out of a
+ * closure - _always_ use continue_at(). Doing so consistently will help
+ * eliminate an entire class of particularly pernicious races.
+ *
+ * For a closure to wait on an arbitrary event, we need to introduce waitlists:
+ *
+ * struct closure_waitlist list;
+ * closure_wait_event(list, cl, condition);
+ * closure_wake_up(wait_list);
+ *
+ * These work analagously to wait_event() and wake_up() - except that instead of
+ * operating on the current thread (for wait_event()) and lists of threads, they
+ * operate on an explicit closure and lists of closures.
+ *
+ * Because it's a closure we can now wait either synchronously or
+ * asynchronously. closure_wait_event() returns the current value of the
+ * condition, and if it returned false continue_at() or closure_sync() can be
+ * used to wait for it to become true.
+ *
+ * It's useful for waiting on things when you can't sleep in the context in
+ * which you must check the condition (perhaps a spinlock held, or you might be
+ * beneath generic_make_request() - in which case you can't sleep on IO).
+ *
+ * closure_wait_event() will wait either synchronously or asynchronously,
+ * depending on whether the closure is in blocking mode or not. You can pick a
+ * mode explicitly with closure_wait_event_sync() and
+ * closure_wait_event_async(), which do just what you might expect.
+ *
+ * Lastly, you might have a wait list dedicated to a specific event, and have no
+ * need for specifying the condition - you just want to wait until someone runs
+ * closure_wake_up() on the appropriate wait list. In that case, just use
+ * closure_wait(). It will return either true or false, depending on whether the
+ * closure was already on a wait list or not - a closure can only be on one wait
+ * list at a time.
+ *
+ * Parents:
+ *
+ * closure_init() takes two arguments - it takes the closure to initialize, and
+ * a (possibly null) parent.
+ *
+ * If parent is non null, the new closure will have a refcount for its lifetime;
+ * a closure is considered to be "finished" when its refcount hits 0 and the
+ * function to run is null. Hence
+ *
+ * continue_at(cl, NULL, NULL);
+ *
+ * returns up the (spaghetti) stack of closures, precisely like normal return
+ * returns up the C stack. continue_at() with non null fn is better thought of
+ * as doing a tail call.
+ *
+ * All this implies that a closure should typically be embedded in a particular
+ * struct (which its refcount will normally control the lifetime of), and that
+ * struct can very much be thought of as a stack frame.
+ *
+ * Locking:
+ *
+ * Closures are based on work items but they can be thought of as more like
+ * threads - in that like threads and unlike work items they have a well
+ * defined lifetime; they are created (with closure_init()) and eventually
+ * complete after a continue_at(cl, NULL, NULL).
+ *
+ * Suppose you've got some larger structure with a closure embedded in it that's
+ * used for periodically doing garbage collection. You only want one garbage
+ * collection happening at a time, so the natural thing to do is protect it with
+ * a lock. However, it's difficult to use a lock protecting a closure correctly
+ * because the unlock should come after the last continue_to() (additionally, if
+ * you're using the closure asynchronously a mutex won't work since a mutex has
+ * to be unlocked by the same process that locked it).
+ *
+ * So to make it less error prone and more efficient, we also have the ability
+ * to use closures as locks:
+ *
+ * closure_init_unlocked();
+ * closure_trylock();
+ *
+ * That's all we need for trylock() - the last closure_put() implicitly unlocks
+ * it for you.  But for closure_lock(), we also need a wait list:
+ *
+ * struct closure_with_waitlist frobnicator_cl;
+ *
+ * closure_init_unlocked(&frobnicator_cl);
+ * closure_lock(&frobnicator_cl);
+ *
+ * A closure_with_waitlist embeds a closure and a wait list - much like struct
+ * delayed_work embeds a work item and a timer_list. The important thing is, use
+ * it exactly like you would a regular closure and closure_put() will magically
+ * handle everything for you.
+ *
+ * We've got closures that embed timers, too. They're called, appropriately
+ * enough:
+ * struct closure_with_timer;
+ *
+ * This gives you access to closure_delay(). It takes a refcount for a specified
+ * number of jiffies - you could then call closure_sync() (for a slightly
+ * convoluted version of msleep()) or continue_at() - which gives you the same
+ * effect as using a delayed work item, except you can reuse the work_struct
+ * already embedded in struct closure.
+ *
+ * Lastly, there's struct closure_with_waitlist_and_timer. It does what you
+ * probably expect, if you happen to need the features of both. (You don't
+ * really want to know how all this is implemented, but if I've done my job
+ * right you shouldn't have to care).
+ */
+
+struct closure;
+typedef void (closure_fn) (struct closure *);
+
+struct closure_waitlist {
+	struct llist_head	list;
+};
+
+enum closure_type {
+	TYPE_closure				= 0,
+	TYPE_closure_with_waitlist		= 1,
+	TYPE_closure_with_timer			= 2,
+	TYPE_closure_with_waitlist_and_timer	= 3,
+	MAX_CLOSURE_TYPE			= 3,
+};
+
+enum closure_state {
+	/*
+	 * CLOSURE_BLOCKING: Causes closure_wait_event() to block, instead of
+	 * waiting asynchronously
+	 *
+	 * CLOSURE_WAITING: Set iff the closure is on a waitlist. Must be set by
+	 * the thread that owns the closure, and cleared by the thread that's
+	 * waking up the closure.
+	 *
+	 * CLOSURE_SLEEPING: Must be set before a thread uses a closure to sleep
+	 * - indicates that cl->task is valid and closure_put() may wake it up.
+	 * Only set or cleared by the thread that owns the closure.
+	 *
+	 * CLOSURE_TIMER: Analagous to CLOSURE_WAITING, indicates that a closure
+	 * has an outstanding timer. Must be set by the thread that owns the
+	 * closure, and cleared by the timer function when the timer goes off.
+	 *
+	 * The rest are for debugging and don't affect behaviour:
+	 *
+	 * CLOSURE_RUNNING: Set when a closure is running (i.e. by
+	 * closure_init() and when closure_put() runs then next function), and
+	 * must be cleared before remaining hits 0. Primarily to help guard
+	 * against incorrect usage and accidentally transferring references.
+	 * continue_at() and closure_return() clear it for you, if you're doing
+	 * something unusual you can use closure_set_dead() which also helps
+	 * annotate where references are being transferred.
+	 *
+	 * CLOSURE_STACK: Sanity check - remaining should never hit 0 on a
+	 * closure with this flag set
+	 */
+
+	CLOSURE_BITS_START	= (1 << 19),
+	CLOSURE_DESTRUCTOR	= (1 << 19),
+	CLOSURE_BLOCKING	= (1 << 21),
+	CLOSURE_WAITING		= (1 << 23),
+	CLOSURE_SLEEPING	= (1 << 25),
+	CLOSURE_TIMER		= (1 << 27),
+	CLOSURE_RUNNING		= (1 << 29),
+	CLOSURE_STACK		= (1 << 31),
+};
+
+#define CLOSURE_GUARD_MASK					\
+	((CLOSURE_DESTRUCTOR|CLOSURE_BLOCKING|CLOSURE_WAITING|	\
+	  CLOSURE_SLEEPING|CLOSURE_TIMER|CLOSURE_RUNNING|CLOSURE_STACK) << 1)
+
+#define CLOSURE_REMAINING_MASK		(CLOSURE_BITS_START - 1)
+#define CLOSURE_REMAINING_INITIALIZER	(1|CLOSURE_RUNNING)
+
+struct closure {
+	union {
+		struct {
+			struct workqueue_struct *wq;
+			struct task_struct	*task;
+			struct llist_node	list;
+			closure_fn		*fn;
+		};
+		struct work_struct	work;
+	};
+
+	struct closure		*parent;
+
+	atomic_t		remaining;
+
+	enum closure_type	type;
+
+#ifdef CONFIG_BCACHE_CLOSURES_DEBUG
+#define CLOSURE_MAGIC_DEAD	0xc054dead
+#define CLOSURE_MAGIC_ALIVE	0xc054a11e
+
+	unsigned		magic;
+	struct list_head	all;
+	unsigned long		ip;
+	unsigned long		waiting_on;
+#endif
+};
+
+struct closure_with_waitlist {
+	struct closure		cl;
+	struct closure_waitlist	wait;
+};
+
+struct closure_with_timer {
+	struct closure		cl;
+	struct timer_list	timer;
+};
+
+struct closure_with_waitlist_and_timer {
+	struct closure		cl;
+	struct closure_waitlist	wait;
+	struct timer_list	timer;
+};
+
+extern unsigned invalid_closure_type(void);
+
+#define __CLOSURE_TYPE(cl, _t)						\
+	  __builtin_types_compatible_p(typeof(cl), struct _t)		\
+		? TYPE_ ## _t :						\
+
+#define __closure_type(cl)						\
+(									\
+	__CLOSURE_TYPE(cl, closure)					\
+	__CLOSURE_TYPE(cl, closure_with_waitlist)			\
+	__CLOSURE_TYPE(cl, closure_with_timer)				\
+	__CLOSURE_TYPE(cl, closure_with_waitlist_and_timer)		\
+	invalid_closure_type()						\
+)
+
+void closure_sub(struct closure *cl, int v);
+void closure_put(struct closure *cl);
+void closure_queue(struct closure *cl);
+void __closure_wake_up(struct closure_waitlist *list);
+bool closure_wait(struct closure_waitlist *list, struct closure *cl);
+void closure_sync(struct closure *cl);
+
+bool closure_trylock(struct closure *cl, struct closure *parent);
+void __closure_lock(struct closure *cl, struct closure *parent,
+		    struct closure_waitlist *wait_list);
+
+void do_closure_timer_init(struct closure *cl);
+bool __closure_delay(struct closure *cl, unsigned long delay,
+		     struct timer_list *timer);
+void __closure_flush(struct closure *cl, struct timer_list *timer);
+void __closure_flush_sync(struct closure *cl, struct timer_list *timer);
+
+#ifdef CONFIG_BCACHE_CLOSURES_DEBUG
+
+void closure_debug_init(void);
+void closure_debug_create(struct closure *cl);
+void closure_debug_destroy(struct closure *cl);
+
+#else
+
+static inline void closure_debug_init(void) {}
+static inline void closure_debug_create(struct closure *cl) {}
+static inline void closure_debug_destroy(struct closure *cl) {}
+
+#endif
+
+static inline void closure_set_ip(struct closure *cl)
+{
+#ifdef CONFIG_BCACHE_CLOSURES_DEBUG
+	cl->ip = _THIS_IP_;
+#endif
+}
+
+static inline void closure_set_ret_ip(struct closure *cl)
+{
+#ifdef CONFIG_BCACHE_CLOSURES_DEBUG
+	cl->ip = _RET_IP_;
+#endif
+}
+
+static inline void closure_get(struct closure *cl)
+{
+#ifdef CONFIG_BCACHE_CLOSURES_DEBUG
+	BUG_ON((atomic_inc_return(&cl->remaining) &
+		CLOSURE_REMAINING_MASK) <= 1);
+#else
+	atomic_inc(&cl->remaining);
+#endif
+}
+
+static inline void closure_set_stopped(struct closure *cl)
+{
+	atomic_sub(CLOSURE_RUNNING, &cl->remaining);
+}
+
+static inline bool closure_is_stopped(struct closure *cl)
+{
+	return !(atomic_read(&cl->remaining) & CLOSURE_RUNNING);
+}
+
+static inline bool closure_is_unlocked(struct closure *cl)
+{
+	return atomic_read(&cl->remaining) == -1;
+}
+
+static inline void do_closure_init(struct closure *cl, struct closure *parent,
+				   bool running)
+{
+	switch (cl->type) {
+	case TYPE_closure_with_timer:
+	case TYPE_closure_with_waitlist_and_timer:
+		do_closure_timer_init(cl);
+	default:
+		break;
+	}
+
+	cl->parent = parent;
+	if (parent)
+		closure_get(parent);
+
+	if (running) {
+		closure_debug_create(cl);
+		atomic_set(&cl->remaining, CLOSURE_REMAINING_INITIALIZER);
+	} else
+		atomic_set(&cl->remaining, -1);
+
+	closure_set_ip(cl);
+}
+
+/*
+ * Hack to get at the embedded closure if there is one, by doing an unsafe cast:
+ * the result of __closure_type() is thrown away, it's used merely for type
+ * checking.
+ */
+#define __to_internal_closure(cl)				\
+({								\
+	BUILD_BUG_ON(__closure_type(*cl) > MAX_CLOSURE_TYPE);	\
+	(struct closure *) cl;					\
+})
+
+#define closure_init_type(cl, parent, running)			\
+do {								\
+	struct closure *_cl = __to_internal_closure(cl);	\
+	_cl->type = __closure_type(*(cl));			\
+	do_closure_init(_cl, parent, running);			\
+} while (0)
+
+/**
+ * __closure_init() - Initialize a closure, skipping the memset()
+ *
+ * May be used instead of closure_init() when memory has already been zeroed.
+ */
+#define __closure_init(cl, parent)				\
+	closure_init_type(cl, parent, true)
+
+/**
+ * closure_init() - Initialize a closure, setting the refcount to 1
+ * @cl:		closure to initialize
+ * @parent:	parent of the new closure. cl will take a refcount on it for its
+ *		lifetime; may be NULL.
+ */
+#define closure_init(cl, parent)				\
+do {								\
+	memset((cl), 0, sizeof(*(cl)));				\
+	__closure_init(cl, parent);				\
+} while (0)
+
+static inline void closure_init_stack(struct closure *cl)
+{
+	memset(cl, 0, sizeof(struct closure));
+	atomic_set(&cl->remaining, CLOSURE_REMAINING_INITIALIZER|
+		   CLOSURE_BLOCKING|CLOSURE_STACK);
+}
+
+/**
+ * closure_init_unlocked() - Initialize a closure but leave it unlocked.
+ * @cl:		closure to initialize
+ *
+ * For when the closure will be used as a lock. The closure may not be used
+ * until after a closure_lock() or closure_trylock().
+ */
+#define closure_init_unlocked(cl)				\
+do {								\
+	memset((cl), 0, sizeof(*(cl)));				\
+	closure_init_type(cl, NULL, false);			\
+} while (0)
+
+/**
+ * closure_lock() - lock and initialize a closure.
+ * @cl:		the closure to lock
+ * @parent:	the new parent for this closure
+ *
+ * The closure must be of one of the types that has a waitlist (otherwise we
+ * wouldn't be able to sleep on contention).
+ *
+ * @parent has exactly the same meaning as in closure_init(); if non null, the
+ * closure will take a reference on @parent which will be released when it is
+ * unlocked.
+ */
+#define closure_lock(cl, parent)				\
+	__closure_lock(__to_internal_closure(cl), parent, &(cl)->wait)
+
+/**
+ * closure_delay() - delay some number of jiffies
+ * @cl:		the closure that will sleep
+ * @delay:	the delay in jiffies
+ *
+ * Takes a refcount on @cl which will be released after @delay jiffies; this may
+ * be used to have a function run after a delay with continue_at(), or
+ * closure_sync() may be used for a convoluted version of msleep().
+ */
+#define closure_delay(cl, delay)			\
+	__closure_delay(__to_internal_closure(cl), delay, &(cl)->timer)
+
+#define closure_flush(cl)				\
+	__closure_flush(__to_internal_closure(cl), &(cl)->timer)
+
+#define closure_flush_sync(cl)				\
+	__closure_flush_sync(__to_internal_closure(cl), &(cl)->timer)
+
+static inline void __closure_end_sleep(struct closure *cl)
+{
+	__set_current_state(TASK_RUNNING);
+
+	if (atomic_read(&cl->remaining) & CLOSURE_SLEEPING)
+		atomic_sub(CLOSURE_SLEEPING, &cl->remaining);
+}
+
+static inline void __closure_start_sleep(struct closure *cl)
+{
+	closure_set_ip(cl);
+	cl->task = current;
+	set_current_state(TASK_UNINTERRUPTIBLE);
+
+	if (!(atomic_read(&cl->remaining) & CLOSURE_SLEEPING))
+		atomic_add(CLOSURE_SLEEPING, &cl->remaining);
+}
+
+/**
+ * closure_blocking() - returns true if the closure is in blocking mode.
+ *
+ * If a closure is in blocking mode, closure_wait_event() will sleep until the
+ * condition is true instead of waiting asynchronously.
+ */
+static inline bool closure_blocking(struct closure *cl)
+{
+	return atomic_read(&cl->remaining) & CLOSURE_BLOCKING;
+}
+
+/**
+ * set_closure_blocking() - put a closure in blocking mode.
+ *
+ * If a closure is in blocking mode, closure_wait_event() will sleep until the
+ * condition is true instead of waiting asynchronously.
+ *
+ * Not thread safe - can only be called by the thread running the closure.
+ */
+static inline void set_closure_blocking(struct closure *cl)
+{
+	if (!closure_blocking(cl))
+		atomic_add(CLOSURE_BLOCKING, &cl->remaining);
+}
+
+/*
+ * Not thread safe - can only be called by the thread running the closure.
+ */
+static inline void clear_closure_blocking(struct closure *cl)
+{
+	if (closure_blocking(cl))
+		atomic_sub(CLOSURE_BLOCKING, &cl->remaining);
+}
+
+/**
+ * closure_wake_up() - wake up all closures on a wait list.
+ */
+static inline void closure_wake_up(struct closure_waitlist *list)
+{
+	smp_mb();
+	__closure_wake_up(list);
+}
+
+/*
+ * Wait on an event, synchronously or asynchronously - analogous to wait_event()
+ * but for closures.
+ *
+ * The loop is oddly structured so as to avoid a race; we must check the
+ * condition again after we've added ourself to the waitlist. We know if we were
+ * already on the waitlist because closure_wait() returns false; thus, we only
+ * schedule or break if closure_wait() returns false. If it returns true, we
+ * just loop again - rechecking the condition.
+ *
+ * The __closure_wake_up() is necessary because we may race with the event
+ * becoming true; i.e. we see event false -> wait -> recheck condition, but the
+ * thread that made the event true may have called closure_wake_up() before we
+ * added ourself to the wait list.
+ *
+ * We have to call closure_sync() at the end instead of just
+ * __closure_end_sleep() because a different thread might've called
+ * closure_wake_up() before us and gotten preempted before they dropped the
+ * refcount on our closure. If this was a stack allocated closure, that would be
+ * bad.
+ */
+#define __closure_wait_event(list, cl, condition, _block)		\
+({									\
+	bool block = _block;						\
+	typeof(condition) ret;						\
+									\
+	while (1) {							\
+		ret = (condition);					\
+		if (ret) {						\
+			__closure_wake_up(list);			\
+			if (block)					\
+				closure_sync(cl);			\
+									\
+			break;						\
+		}							\
+									\
+		if (block)						\
+			__closure_start_sleep(cl);			\
+									\
+		if (!closure_wait(list, cl)) {				\
+			if (!block)					\
+				break;					\
+									\
+			schedule();					\
+		}							\
+	}								\
+									\
+	ret;								\
+})
+
+/**
+ * closure_wait_event() - wait on a condition, synchronously or asynchronously.
+ * @list:	the wait list to wait on
+ * @cl:		the closure that is doing the waiting
+ * @condition:	a C expression for the event to wait for
+ *
+ * If the closure is in blocking mode, sleeps until the @condition evaluates to
+ * true - exactly like wait_event().
+ *
+ * If the closure is not in blocking mode, waits asynchronously; if the
+ * condition is currently false the @cl is put onto @list and returns. @list
+ * owns a refcount on @cl; closure_sync() or continue_at() may be used later to
+ * wait for another thread to wake up @list, which drops the refcount on @cl.
+ *
+ * Returns the value of @condition; @cl will be on @list iff @condition was
+ * false.
+ *
+ * closure_wake_up(@list) must be called after changing any variable that could
+ * cause @condition to become true.
+ */
+#define closure_wait_event(list, cl, condition)				\
+	__closure_wait_event(list, cl, condition, closure_blocking(cl))
+
+#define closure_wait_event_async(list, cl, condition)			\
+	__closure_wait_event(list, cl, condition, false)
+
+#define closure_wait_event_sync(list, cl, condition)			\
+	__closure_wait_event(list, cl, condition, true)
+
+static inline void set_closure_fn(struct closure *cl, closure_fn *fn,
+				  struct workqueue_struct *wq)
+{
+	BUG_ON(object_is_on_stack(cl));
+	closure_set_ip(cl);
+	cl->fn = fn;
+	cl->wq = wq;
+	/* between atomic_dec() in closure_put() */
+	smp_mb__before_atomic_dec();
+}
+
+#define continue_at(_cl, _fn, _wq)					\
+do {									\
+	set_closure_fn(_cl, _fn, _wq);					\
+	closure_sub(_cl, CLOSURE_RUNNING + 1);				\
+	return;								\
+} while (0)
+
+#define closure_return(_cl)	continue_at((_cl), NULL, NULL)
+
+#define continue_at_nobarrier(_cl, _fn, _wq)				\
+do {									\
+	set_closure_fn(_cl, _fn, _wq);					\
+	closure_queue(cl);						\
+	return;								\
+} while (0)
+
+#define closure_return_with_destructor(_cl, _destructor)		\
+do {									\
+	set_closure_fn(_cl, _destructor, NULL);				\
+	closure_sub(_cl, CLOSURE_RUNNING - CLOSURE_DESTRUCTOR + 1);	\
+	return;								\
+} while (0)
+
+static inline void closure_call(struct closure *cl, closure_fn fn,
+				struct workqueue_struct *wq,
+				struct closure *parent)
+{
+	closure_init(cl, parent);
+	continue_at_nobarrier(cl, fn, wq);
+}
+
+static inline void closure_trylock_call(struct closure *cl, closure_fn fn,
+					struct workqueue_struct *wq,
+					struct closure *parent)
+{
+	if (closure_trylock(cl, parent))
+		continue_at_nobarrier(cl, fn, wq);
+}
+
+#endif /* _LINUX_CLOSURE_H */
diff --git a/drivers/md/bcache/debug.c b/drivers/md/bcache/debug.c
new file mode 100644
index 000000000000..89fd5204924e
--- /dev/null
+++ b/drivers/md/bcache/debug.c
@@ -0,0 +1,565 @@
+/*
+ * Assorted bcache debug code
+ *
+ * Copyright 2010, 2011 Kent Overstreet <kent.overstreet@gmail.com>
+ * Copyright 2012 Google, Inc.
+ */
+
+#include "bcache.h"
+#include "btree.h"
+#include "debug.h"
+#include "request.h"
+
+#include <linux/console.h>
+#include <linux/debugfs.h>
+#include <linux/module.h>
+#include <linux/random.h>
+#include <linux/seq_file.h>
+
+static struct dentry *debug;
+
+const char *bch_ptr_status(struct cache_set *c, const struct bkey *k)
+{
+	unsigned i;
+
+	for (i = 0; i < KEY_PTRS(k); i++)
+		if (ptr_available(c, k, i)) {
+			struct cache *ca = PTR_CACHE(c, k, i);
+			size_t bucket = PTR_BUCKET_NR(c, k, i);
+			size_t r = bucket_remainder(c, PTR_OFFSET(k, i));
+
+			if (KEY_SIZE(k) + r > c->sb.bucket_size)
+				return "bad, length too big";
+			if (bucket <  ca->sb.first_bucket)
+				return "bad, short offset";
+			if (bucket >= ca->sb.nbuckets)
+				return "bad, offset past end of device";
+			if (ptr_stale(c, k, i))
+				return "stale";
+		}
+
+	if (!bkey_cmp(k, &ZERO_KEY))
+		return "bad, null key";
+	if (!KEY_PTRS(k))
+		return "bad, no pointers";
+	if (!KEY_SIZE(k))
+		return "zeroed key";
+	return "";
+}
+
+struct keyprint_hack bch_pkey(const struct bkey *k)
+{
+	unsigned i = 0;
+	struct keyprint_hack r;
+	char *out = r.s, *end = r.s + KEYHACK_SIZE;
+
+#define p(...)	(out += scnprintf(out, end - out, __VA_ARGS__))
+
+	p("%llu:%llu len %llu -> [", KEY_INODE(k), KEY_OFFSET(k), KEY_SIZE(k));
+
+	if (KEY_PTRS(k))
+		while (1) {
+			p("%llu:%llu gen %llu",
+			  PTR_DEV(k, i), PTR_OFFSET(k, i), PTR_GEN(k, i));
+
+			if (++i == KEY_PTRS(k))
+				break;
+
+			p(", ");
+		}
+
+	p("]");
+
+	if (KEY_DIRTY(k))
+		p(" dirty");
+	if (KEY_CSUM(k))
+		p(" cs%llu %llx", KEY_CSUM(k), k->ptr[1]);
+#undef p
+	return r;
+}
+
+struct keyprint_hack bch_pbtree(const struct btree *b)
+{
+	struct keyprint_hack r;
+
+	snprintf(r.s, 40, "%zu level %i/%i", PTR_BUCKET_NR(b->c, &b->key, 0),
+		 b->level, b->c->root ? b->c->root->level : -1);
+	return r;
+}
+
+#if defined(CONFIG_BCACHE_DEBUG) || defined(CONFIG_BCACHE_EDEBUG)
+
+static bool skipped_backwards(struct btree *b, struct bkey *k)
+{
+	return bkey_cmp(k, (!b->level)
+			? &START_KEY(bkey_next(k))
+			: bkey_next(k)) > 0;
+}
+
+static void dump_bset(struct btree *b, struct bset *i)
+{
+	struct bkey *k;
+	unsigned j;
+
+	for (k = i->start; k < end(i); k = bkey_next(k)) {
+		printk(KERN_ERR "block %zu key %zi/%u: %s", index(i, b),
+		       (uint64_t *) k - i->d, i->keys, pkey(k));
+
+		for (j = 0; j < KEY_PTRS(k); j++) {
+			size_t n = PTR_BUCKET_NR(b->c, k, j);
+			printk(" bucket %zu", n);
+
+			if (n >= b->c->sb.first_bucket && n < b->c->sb.nbuckets)
+				printk(" prio %i",
+				       PTR_BUCKET(b->c, k, j)->prio);
+		}
+
+		printk(" %s\n", bch_ptr_status(b->c, k));
+
+		if (bkey_next(k) < end(i) &&
+		    skipped_backwards(b, k))
+			printk(KERN_ERR "Key skipped backwards\n");
+	}
+}
+
+#endif
+
+#ifdef CONFIG_BCACHE_DEBUG
+
+void bch_btree_verify(struct btree *b, struct bset *new)
+{
+	struct btree *v = b->c->verify_data;
+	struct closure cl;
+	closure_init_stack(&cl);
+
+	if (!b->c->verify)
+		return;
+
+	closure_wait_event(&b->io.wait, &cl,
+			   atomic_read(&b->io.cl.remaining) == -1);
+
+	mutex_lock(&b->c->verify_lock);
+
+	bkey_copy(&v->key, &b->key);
+	v->written = 0;
+	v->level = b->level;
+
+	bch_btree_read(v);
+	closure_wait_event(&v->io.wait, &cl,
+			   atomic_read(&b->io.cl.remaining) == -1);
+
+	if (new->keys != v->sets[0].data->keys ||
+	    memcmp(new->start,
+		   v->sets[0].data->start,
+		   (void *) end(new) - (void *) new->start)) {
+		unsigned i, j;
+
+		console_lock();
+
+		printk(KERN_ERR "*** original memory node:\n");
+		for (i = 0; i <= b->nsets; i++)
+			dump_bset(b, b->sets[i].data);
+
+		printk(KERN_ERR "*** sorted memory node:\n");
+		dump_bset(b, new);
+
+		printk(KERN_ERR "*** on disk node:\n");
+		dump_bset(v, v->sets[0].data);
+
+		for (j = 0; j < new->keys; j++)
+			if (new->d[j] != v->sets[0].data->d[j])
+				break;
+
+		console_unlock();
+		panic("verify failed at %u\n", j);
+	}
+
+	mutex_unlock(&b->c->verify_lock);
+}
+
+static void data_verify_endio(struct bio *bio, int error)
+{
+	struct closure *cl = bio->bi_private;
+	closure_put(cl);
+}
+
+void bch_data_verify(struct search *s)
+{
+	char name[BDEVNAME_SIZE];
+	struct cached_dev *dc = container_of(s->d, struct cached_dev, disk);
+	struct closure *cl = &s->cl;
+	struct bio *check;
+	struct bio_vec *bv;
+	int i;
+
+	if (!s->unaligned_bvec)
+		bio_for_each_segment(bv, s->orig_bio, i)
+			bv->bv_offset = 0, bv->bv_len = PAGE_SIZE;
+
+	check = bio_clone(s->orig_bio, GFP_NOIO);
+	if (!check)
+		return;
+
+	if (bch_bio_alloc_pages(check, GFP_NOIO))
+		goto out_put;
+
+	check->bi_rw		= READ_SYNC;
+	check->bi_private	= cl;
+	check->bi_end_io	= data_verify_endio;
+
+	closure_bio_submit(check, cl, &dc->disk);
+	closure_sync(cl);
+
+	bio_for_each_segment(bv, s->orig_bio, i) {
+		void *p1 = kmap(bv->bv_page);
+		void *p2 = kmap(check->bi_io_vec[i].bv_page);
+
+		if (memcmp(p1 + bv->bv_offset,
+			   p2 + bv->bv_offset,
+			   bv->bv_len))
+			printk(KERN_ERR
+			       "bcache (%s): verify failed at sector %llu\n",
+			       bdevname(dc->bdev, name),
+			       (uint64_t) s->orig_bio->bi_sector);
+
+		kunmap(bv->bv_page);
+		kunmap(check->bi_io_vec[i].bv_page);
+	}
+
+	__bio_for_each_segment(bv, check, i, 0)
+		__free_page(bv->bv_page);
+out_put:
+	bio_put(check);
+}
+
+#endif
+
+#ifdef CONFIG_BCACHE_EDEBUG
+
+unsigned bch_count_data(struct btree *b)
+{
+	unsigned ret = 0;
+	struct btree_iter iter;
+	struct bkey *k;
+
+	if (!b->level)
+		for_each_key(b, k, &iter)
+			ret += KEY_SIZE(k);
+	return ret;
+}
+
+static void vdump_bucket_and_panic(struct btree *b, const char *fmt,
+				   va_list args)
+{
+	unsigned i;
+
+	console_lock();
+
+	for (i = 0; i <= b->nsets; i++)
+		dump_bset(b, b->sets[i].data);
+
+	vprintk(fmt, args);
+
+	console_unlock();
+
+	panic("at %s\n", pbtree(b));
+}
+
+void bch_check_key_order_msg(struct btree *b, struct bset *i,
+			     const char *fmt, ...)
+{
+	struct bkey *k;
+
+	if (!i->keys)
+		return;
+
+	for (k = i->start; bkey_next(k) < end(i); k = bkey_next(k))
+		if (skipped_backwards(b, k)) {
+			va_list args;
+			va_start(args, fmt);
+
+			vdump_bucket_and_panic(b, fmt, args);
+			va_end(args);
+		}
+}
+
+void bch_check_keys(struct btree *b, const char *fmt, ...)
+{
+	va_list args;
+	struct bkey *k, *p = NULL;
+	struct btree_iter iter;
+
+	if (b->level)
+		return;
+
+	for_each_key(b, k, &iter) {
+		if (p && bkey_cmp(&START_KEY(p), &START_KEY(k)) > 0) {
+			printk(KERN_ERR "Keys out of order:\n");
+			goto bug;
+		}
+
+		if (bch_ptr_invalid(b, k))
+			continue;
+
+		if (p && bkey_cmp(p, &START_KEY(k)) > 0) {
+			printk(KERN_ERR "Overlapping keys:\n");
+			goto bug;
+		}
+		p = k;
+	}
+	return;
+bug:
+	va_start(args, fmt);
+	vdump_bucket_and_panic(b, fmt, args);
+	va_end(args);
+}
+
+#endif
+
+#ifdef CONFIG_DEBUG_FS
+
+/* XXX: cache set refcounting */
+
+struct dump_iterator {
+	char			buf[PAGE_SIZE];
+	size_t			bytes;
+	struct cache_set	*c;
+	struct keybuf		keys;
+};
+
+static bool dump_pred(struct keybuf *buf, struct bkey *k)
+{
+	return true;
+}
+
+static ssize_t bch_dump_read(struct file *file, char __user *buf,
+			     size_t size, loff_t *ppos)
+{
+	struct dump_iterator *i = file->private_data;
+	ssize_t ret = 0;
+
+	while (size) {
+		struct keybuf_key *w;
+		unsigned bytes = min(i->bytes, size);
+
+		int err = copy_to_user(buf, i->buf, bytes);
+		if (err)
+			return err;
+
+		ret	 += bytes;
+		buf	 += bytes;
+		size	 -= bytes;
+		i->bytes -= bytes;
+		memmove(i->buf, i->buf + bytes, i->bytes);
+
+		if (i->bytes)
+			break;
+
+		w = bch_keybuf_next_rescan(i->c, &i->keys, &MAX_KEY);
+		if (!w)
+			break;
+
+		i->bytes = snprintf(i->buf, PAGE_SIZE, "%s\n", pkey(&w->key));
+		bch_keybuf_del(&i->keys, w);
+	}
+
+	return ret;
+}
+
+static int bch_dump_open(struct inode *inode, struct file *file)
+{
+	struct cache_set *c = inode->i_private;
+	struct dump_iterator *i;
+
+	i = kzalloc(sizeof(struct dump_iterator), GFP_KERNEL);
+	if (!i)
+		return -ENOMEM;
+
+	file->private_data = i;
+	i->c = c;
+	bch_keybuf_init(&i->keys, dump_pred);
+	i->keys.last_scanned = KEY(0, 0, 0);
+
+	return 0;
+}
+
+static int bch_dump_release(struct inode *inode, struct file *file)
+{
+	kfree(file->private_data);
+	return 0;
+}
+
+static const struct file_operations cache_set_debug_ops = {
+	.owner		= THIS_MODULE,
+	.open		= bch_dump_open,
+	.read		= bch_dump_read,
+	.release	= bch_dump_release
+};
+
+void bch_debug_init_cache_set(struct cache_set *c)
+{
+	if (!IS_ERR_OR_NULL(debug)) {
+		char name[50];
+		snprintf(name, 50, "bcache-%pU", c->sb.set_uuid);
+
+		c->debug = debugfs_create_file(name, 0400, debug, c,
+					       &cache_set_debug_ops);
+	}
+}
+
+#endif
+
+/* Fuzz tester has rotted: */
+#if 0
+
+static ssize_t btree_fuzz(struct kobject *k, struct kobj_attribute *a,
+			  const char *buffer, size_t size)
+{
+	void dump(struct btree *b)
+	{
+		struct bset *i;
+
+		for (i = b->sets[0].data;
+		     index(i, b) < btree_blocks(b) &&
+		     i->seq == b->sets[0].data->seq;
+		     i = ((void *) i) + set_blocks(i, b->c) * block_bytes(b->c))
+			dump_bset(b, i);
+	}
+
+	struct cache_sb *sb;
+	struct cache_set *c;
+	struct btree *all[3], *b, *fill, *orig;
+	int j;
+
+	struct btree_op op;
+	bch_btree_op_init_stack(&op);
+
+	sb = kzalloc(sizeof(struct cache_sb), GFP_KERNEL);
+	if (!sb)
+		return -ENOMEM;
+
+	sb->bucket_size = 128;
+	sb->block_size = 4;
+
+	c = bch_cache_set_alloc(sb);
+	if (!c)
+		return -ENOMEM;
+
+	for (j = 0; j < 3; j++) {
+		BUG_ON(list_empty(&c->btree_cache));
+		all[j] = list_first_entry(&c->btree_cache, struct btree, list);
+		list_del_init(&all[j]->list);
+
+		all[j]->key = KEY(0, 0, c->sb.bucket_size);
+		bkey_copy_key(&all[j]->key, &MAX_KEY);
+	}
+
+	b = all[0];
+	fill = all[1];
+	orig = all[2];
+
+	while (1) {
+		for (j = 0; j < 3; j++)
+			all[j]->written = all[j]->nsets = 0;
+
+		bch_bset_init_next(b);
+
+		while (1) {
+			struct bset *i = write_block(b);
+			struct bkey *k = op.keys.top;
+			unsigned rand;
+
+			bkey_init(k);
+			rand = get_random_int();
+
+			op.type = rand & 1
+				? BTREE_INSERT
+				: BTREE_REPLACE;
+			rand >>= 1;
+
+			SET_KEY_SIZE(k, bucket_remainder(c, rand));
+			rand >>= c->bucket_bits;
+			rand &= 1024 * 512 - 1;
+			rand += c->sb.bucket_size;
+			SET_KEY_OFFSET(k, rand);
+#if 0
+			SET_KEY_PTRS(k, 1);
+#endif
+			bch_keylist_push(&op.keys);
+			bch_btree_insert_keys(b, &op);
+
+			if (should_split(b) ||
+			    set_blocks(i, b->c) !=
+			    __set_blocks(i, i->keys + 15, b->c)) {
+				i->csum = csum_set(i);
+
+				memcpy(write_block(fill),
+				       i, set_bytes(i));
+
+				b->written += set_blocks(i, b->c);
+				fill->written = b->written;
+				if (b->written == btree_blocks(b))
+					break;
+
+				bch_btree_sort_lazy(b);
+				bch_bset_init_next(b);
+			}
+		}
+
+		memcpy(orig->sets[0].data,
+		       fill->sets[0].data,
+		       btree_bytes(c));
+
+		bch_btree_sort(b);
+		fill->written = 0;
+		bch_btree_read_done(&fill->io.cl);
+
+		if (b->sets[0].data->keys != fill->sets[0].data->keys ||
+		    memcmp(b->sets[0].data->start,
+			   fill->sets[0].data->start,
+			   b->sets[0].data->keys * sizeof(uint64_t))) {
+			struct bset *i = b->sets[0].data;
+			struct bkey *k, *l;
+
+			for (k = i->start,
+			     l = fill->sets[0].data->start;
+			     k < end(i);
+			     k = bkey_next(k), l = bkey_next(l))
+				if (bkey_cmp(k, l) ||
+				    KEY_SIZE(k) != KEY_SIZE(l))
+					pr_err("key %zi differs: %s != %s",
+					       (uint64_t *) k - i->d,
+					       pkey(k), pkey(l));
+
+			for (j = 0; j < 3; j++) {
+				pr_err("**** Set %i ****", j);
+				dump(all[j]);
+			}
+			panic("\n");
+		}
+
+		pr_info("fuzz complete: %i keys", b->sets[0].data->keys);
+	}
+}
+
+kobj_attribute_write(fuzz, btree_fuzz);
+#endif
+
+void bch_debug_exit(void)
+{
+	if (!IS_ERR_OR_NULL(debug))
+		debugfs_remove_recursive(debug);
+}
+
+int __init bch_debug_init(struct kobject *kobj)
+{
+	int ret = 0;
+#if 0
+	ret = sysfs_create_file(kobj, &ksysfs_fuzz.attr);
+	if (ret)
+		return ret;
+#endif
+
+	debug = debugfs_create_dir("bcache", NULL);
+	return ret;
+}
diff --git a/drivers/md/bcache/debug.h b/drivers/md/bcache/debug.h
new file mode 100644
index 000000000000..f9378a218148
--- /dev/null
+++ b/drivers/md/bcache/debug.h
@@ -0,0 +1,54 @@
+#ifndef _BCACHE_DEBUG_H
+#define _BCACHE_DEBUG_H
+
+/* Btree/bkey debug printing */
+
+#define KEYHACK_SIZE 80
+struct keyprint_hack {
+	char s[KEYHACK_SIZE];
+};
+
+struct keyprint_hack bch_pkey(const struct bkey *k);
+struct keyprint_hack bch_pbtree(const struct btree *b);
+#define pkey(k)		(&bch_pkey(k).s[0])
+#define pbtree(b)	(&bch_pbtree(b).s[0])
+
+#ifdef CONFIG_BCACHE_EDEBUG
+
+unsigned bch_count_data(struct btree *);
+void bch_check_key_order_msg(struct btree *, struct bset *, const char *, ...);
+void bch_check_keys(struct btree *, const char *, ...);
+
+#define bch_check_key_order(b, i)			\
+	bch_check_key_order_msg(b, i, "keys out of order")
+#define EBUG_ON(cond)		BUG_ON(cond)
+
+#else /* EDEBUG */
+
+#define bch_count_data(b)				0
+#define bch_check_key_order(b, i)			do {} while (0)
+#define bch_check_key_order_msg(b, i, ...)		do {} while (0)
+#define bch_check_keys(b, ...)				do {} while (0)
+#define EBUG_ON(cond)					do {} while (0)
+
+#endif
+
+#ifdef CONFIG_BCACHE_DEBUG
+
+void bch_btree_verify(struct btree *, struct bset *);
+void bch_data_verify(struct search *);
+
+#else /* DEBUG */
+
+static inline void bch_btree_verify(struct btree *b, struct bset *i) {}
+static inline void bch_data_verify(struct search *s) {};
+
+#endif
+
+#ifdef CONFIG_DEBUG_FS
+void bch_debug_init_cache_set(struct cache_set *);
+#else
+static inline void bch_debug_init_cache_set(struct cache_set *c) {}
+#endif
+
+#endif
diff --git a/drivers/md/bcache/io.c b/drivers/md/bcache/io.c
new file mode 100644
index 000000000000..48efd4dea645
--- /dev/null
+++ b/drivers/md/bcache/io.c
@@ -0,0 +1,397 @@
+/*
+ * Some low level IO code, and hacks for various block layer limitations
+ *
+ * Copyright 2010, 2011 Kent Overstreet <kent.overstreet@gmail.com>
+ * Copyright 2012 Google, Inc.
+ */
+
+#include "bcache.h"
+#include "bset.h"
+#include "debug.h"
+
+static void bch_bi_idx_hack_endio(struct bio *bio, int error)
+{
+	struct bio *p = bio->bi_private;
+
+	bio_endio(p, error);
+	bio_put(bio);
+}
+
+static void bch_generic_make_request_hack(struct bio *bio)
+{
+	if (bio->bi_idx) {
+		struct bio *clone = bio_alloc(GFP_NOIO, bio_segments(bio));
+
+		memcpy(clone->bi_io_vec,
+		       bio_iovec(bio),
+		       bio_segments(bio) * sizeof(struct bio_vec));
+
+		clone->bi_sector	= bio->bi_sector;
+		clone->bi_bdev		= bio->bi_bdev;
+		clone->bi_rw		= bio->bi_rw;
+		clone->bi_vcnt		= bio_segments(bio);
+		clone->bi_size		= bio->bi_size;
+
+		clone->bi_private	= bio;
+		clone->bi_end_io	= bch_bi_idx_hack_endio;
+
+		bio = clone;
+	}
+
+	/*
+	 * Hack, since drivers that clone bios clone up to bi_max_vecs, but our
+	 * bios might have had more than that (before we split them per device
+	 * limitations).
+	 *
+	 * To be taken out once immutable bvec stuff is in.
+	 */
+	bio->bi_max_vecs = bio->bi_vcnt;
+
+	generic_make_request(bio);
+}
+
+/**
+ * bch_bio_split - split a bio
+ * @bio:	bio to split
+ * @sectors:	number of sectors to split from the front of @bio
+ * @gfp:	gfp mask
+ * @bs:		bio set to allocate from
+ *
+ * Allocates and returns a new bio which represents @sectors from the start of
+ * @bio, and updates @bio to represent the remaining sectors.
+ *
+ * If bio_sectors(@bio) was less than or equal to @sectors, returns @bio
+ * unchanged.
+ *
+ * The newly allocated bio will point to @bio's bi_io_vec, if the split was on a
+ * bvec boundry; it is the caller's responsibility to ensure that @bio is not
+ * freed before the split.
+ *
+ * If bch_bio_split() is running under generic_make_request(), it's not safe to
+ * allocate more than one bio from the same bio set. Therefore, if it is running
+ * under generic_make_request() it masks out __GFP_WAIT when doing the
+ * allocation. The caller must check for failure if there's any possibility of
+ * it being called from under generic_make_request(); it is then the caller's
+ * responsibility to retry from a safe context (by e.g. punting to workqueue).
+ */
+struct bio *bch_bio_split(struct bio *bio, int sectors,
+			  gfp_t gfp, struct bio_set *bs)
+{
+	unsigned idx = bio->bi_idx, vcnt = 0, nbytes = sectors << 9;
+	struct bio_vec *bv;
+	struct bio *ret = NULL;
+
+	BUG_ON(sectors <= 0);
+
+	/*
+	 * If we're being called from underneath generic_make_request() and we
+	 * already allocated any bios from this bio set, we risk deadlock if we
+	 * use the mempool. So instead, we possibly fail and let the caller punt
+	 * to workqueue or somesuch and retry in a safe context.
+	 */
+	if (current->bio_list)
+		gfp &= ~__GFP_WAIT;
+
+	if (sectors >= bio_sectors(bio))
+		return bio;
+
+	if (bio->bi_rw & REQ_DISCARD) {
+		ret = bio_alloc_bioset(gfp, 1, bs);
+		idx = 0;
+		goto out;
+	}
+
+	bio_for_each_segment(bv, bio, idx) {
+		vcnt = idx - bio->bi_idx;
+
+		if (!nbytes) {
+			ret = bio_alloc_bioset(gfp, vcnt, bs);
+			if (!ret)
+				return NULL;
+
+			memcpy(ret->bi_io_vec, bio_iovec(bio),
+			       sizeof(struct bio_vec) * vcnt);
+
+			break;
+		} else if (nbytes < bv->bv_len) {
+			ret = bio_alloc_bioset(gfp, ++vcnt, bs);
+			if (!ret)
+				return NULL;
+
+			memcpy(ret->bi_io_vec, bio_iovec(bio),
+			       sizeof(struct bio_vec) * vcnt);
+
+			ret->bi_io_vec[vcnt - 1].bv_len = nbytes;
+			bv->bv_offset	+= nbytes;
+			bv->bv_len	-= nbytes;
+			break;
+		}
+
+		nbytes -= bv->bv_len;
+	}
+out:
+	ret->bi_bdev	= bio->bi_bdev;
+	ret->bi_sector	= bio->bi_sector;
+	ret->bi_size	= sectors << 9;
+	ret->bi_rw	= bio->bi_rw;
+	ret->bi_vcnt	= vcnt;
+	ret->bi_max_vecs = vcnt;
+
+	bio->bi_sector	+= sectors;
+	bio->bi_size	-= sectors << 9;
+	bio->bi_idx	 = idx;
+
+	if (bio_integrity(bio)) {
+		if (bio_integrity_clone(ret, bio, gfp)) {
+			bio_put(ret);
+			return NULL;
+		}
+
+		bio_integrity_trim(ret, 0, bio_sectors(ret));
+		bio_integrity_trim(bio, bio_sectors(ret), bio_sectors(bio));
+	}
+
+	return ret;
+}
+
+static unsigned bch_bio_max_sectors(struct bio *bio)
+{
+	unsigned ret = bio_sectors(bio);
+	struct request_queue *q = bdev_get_queue(bio->bi_bdev);
+	unsigned max_segments = min_t(unsigned, BIO_MAX_PAGES,
+				      queue_max_segments(q));
+	struct bio_vec *bv, *end = bio_iovec(bio) +
+		min_t(int, bio_segments(bio), max_segments);
+
+	if (bio->bi_rw & REQ_DISCARD)
+		return min(ret, q->limits.max_discard_sectors);
+
+	if (bio_segments(bio) > max_segments ||
+	    q->merge_bvec_fn) {
+		ret = 0;
+
+		for (bv = bio_iovec(bio); bv < end; bv++) {
+			struct bvec_merge_data bvm = {
+				.bi_bdev	= bio->bi_bdev,
+				.bi_sector	= bio->bi_sector,
+				.bi_size	= ret << 9,
+				.bi_rw		= bio->bi_rw,
+			};
+
+			if (q->merge_bvec_fn &&
+			    q->merge_bvec_fn(q, &bvm, bv) < (int) bv->bv_len)
+				break;
+
+			ret += bv->bv_len >> 9;
+		}
+	}
+
+	ret = min(ret, queue_max_sectors(q));
+
+	WARN_ON(!ret);
+	ret = max_t(int, ret, bio_iovec(bio)->bv_len >> 9);
+
+	return ret;
+}
+
+static void bch_bio_submit_split_done(struct closure *cl)
+{
+	struct bio_split_hook *s = container_of(cl, struct bio_split_hook, cl);
+
+	s->bio->bi_end_io = s->bi_end_io;
+	s->bio->bi_private = s->bi_private;
+	bio_endio(s->bio, 0);
+
+	closure_debug_destroy(&s->cl);
+	mempool_free(s, s->p->bio_split_hook);
+}
+
+static void bch_bio_submit_split_endio(struct bio *bio, int error)
+{
+	struct closure *cl = bio->bi_private;
+	struct bio_split_hook *s = container_of(cl, struct bio_split_hook, cl);
+
+	if (error)
+		clear_bit(BIO_UPTODATE, &s->bio->bi_flags);
+
+	bio_put(bio);
+	closure_put(cl);
+}
+
+static void __bch_bio_submit_split(struct closure *cl)
+{
+	struct bio_split_hook *s = container_of(cl, struct bio_split_hook, cl);
+	struct bio *bio = s->bio, *n;
+
+	do {
+		n = bch_bio_split(bio, bch_bio_max_sectors(bio),
+				  GFP_NOIO, s->p->bio_split);
+		if (!n)
+			continue_at(cl, __bch_bio_submit_split, system_wq);
+
+		n->bi_end_io	= bch_bio_submit_split_endio;
+		n->bi_private	= cl;
+
+		closure_get(cl);
+		bch_generic_make_request_hack(n);
+	} while (n != bio);
+
+	continue_at(cl, bch_bio_submit_split_done, NULL);
+}
+
+void bch_generic_make_request(struct bio *bio, struct bio_split_pool *p)
+{
+	struct bio_split_hook *s;
+
+	if (!bio_has_data(bio) && !(bio->bi_rw & REQ_DISCARD))
+		goto submit;
+
+	if (bio_sectors(bio) <= bch_bio_max_sectors(bio))
+		goto submit;
+
+	s = mempool_alloc(p->bio_split_hook, GFP_NOIO);
+
+	s->bio		= bio;
+	s->p		= p;
+	s->bi_end_io	= bio->bi_end_io;
+	s->bi_private	= bio->bi_private;
+	bio_get(bio);
+
+	closure_call(&s->cl, __bch_bio_submit_split, NULL, NULL);
+	return;
+submit:
+	bch_generic_make_request_hack(bio);
+}
+
+/* Bios with headers */
+
+void bch_bbio_free(struct bio *bio, struct cache_set *c)
+{
+	struct bbio *b = container_of(bio, struct bbio, bio);
+	mempool_free(b, c->bio_meta);
+}
+
+struct bio *bch_bbio_alloc(struct cache_set *c)
+{
+	struct bbio *b = mempool_alloc(c->bio_meta, GFP_NOIO);
+	struct bio *bio = &b->bio;
+
+	bio_init(bio);
+	bio->bi_flags		|= BIO_POOL_NONE << BIO_POOL_OFFSET;
+	bio->bi_max_vecs	 = bucket_pages(c);
+	bio->bi_io_vec		 = bio->bi_inline_vecs;
+
+	return bio;
+}
+
+void __bch_submit_bbio(struct bio *bio, struct cache_set *c)
+{
+	struct bbio *b = container_of(bio, struct bbio, bio);
+
+	bio->bi_sector	= PTR_OFFSET(&b->key, 0);
+	bio->bi_bdev	= PTR_CACHE(c, &b->key, 0)->bdev;
+
+	b->submit_time_us = local_clock_us();
+	closure_bio_submit(bio, bio->bi_private, PTR_CACHE(c, &b->key, 0));
+}
+
+void bch_submit_bbio(struct bio *bio, struct cache_set *c,
+		     struct bkey *k, unsigned ptr)
+{
+	struct bbio *b = container_of(bio, struct bbio, bio);
+	bch_bkey_copy_single_ptr(&b->key, k, ptr);
+	__bch_submit_bbio(bio, c);
+}
+
+/* IO errors */
+
+void bch_count_io_errors(struct cache *ca, int error, const char *m)
+{
+	/*
+	 * The halflife of an error is:
+	 * log2(1/2)/log2(127/128) * refresh ~= 88 * refresh
+	 */
+
+	if (ca->set->error_decay) {
+		unsigned count = atomic_inc_return(&ca->io_count);
+
+		while (count > ca->set->error_decay) {
+			unsigned errors;
+			unsigned old = count;
+			unsigned new = count - ca->set->error_decay;
+
+			/*
+			 * First we subtract refresh from count; each time we
+			 * succesfully do so, we rescale the errors once:
+			 */
+
+			count = atomic_cmpxchg(&ca->io_count, old, new);
+
+			if (count == old) {
+				count = new;
+
+				errors = atomic_read(&ca->io_errors);
+				do {
+					old = errors;
+					new = ((uint64_t) errors * 127) / 128;
+					errors = atomic_cmpxchg(&ca->io_errors,
+								old, new);
+				} while (old != errors);
+			}
+		}
+	}
+
+	if (error) {
+		char buf[BDEVNAME_SIZE];
+		unsigned errors = atomic_add_return(1 << IO_ERROR_SHIFT,
+						    &ca->io_errors);
+		errors >>= IO_ERROR_SHIFT;
+
+		if (errors < ca->set->error_limit)
+			pr_err("%s: IO error on %s, recovering",
+			       bdevname(ca->bdev, buf), m);
+		else
+			bch_cache_set_error(ca->set,
+					    "%s: too many IO errors %s",
+					    bdevname(ca->bdev, buf), m);
+	}
+}
+
+void bch_bbio_count_io_errors(struct cache_set *c, struct bio *bio,
+			      int error, const char *m)
+{
+	struct bbio *b = container_of(bio, struct bbio, bio);
+	struct cache *ca = PTR_CACHE(c, &b->key, 0);
+
+	unsigned threshold = bio->bi_rw & REQ_WRITE
+		? c->congested_write_threshold_us
+		: c->congested_read_threshold_us;
+
+	if (threshold) {
+		unsigned t = local_clock_us();
+
+		int us = t - b->submit_time_us;
+		int congested = atomic_read(&c->congested);
+
+		if (us > (int) threshold) {
+			int ms = us / 1024;
+			c->congested_last_us = t;
+
+			ms = min(ms, CONGESTED_MAX + congested);
+			atomic_sub(ms, &c->congested);
+		} else if (congested < 0)
+			atomic_inc(&c->congested);
+	}
+
+	bch_count_io_errors(ca, error, m);
+}
+
+void bch_bbio_endio(struct cache_set *c, struct bio *bio,
+		    int error, const char *m)
+{
+	struct closure *cl = bio->bi_private;
+
+	bch_bbio_count_io_errors(c, bio, error, m);
+	bio_put(bio);
+	closure_put(cl);
+}
diff --git a/drivers/md/bcache/journal.c b/drivers/md/bcache/journal.c
new file mode 100644
index 000000000000..8c8dfdcd9d4c
--- /dev/null
+++ b/drivers/md/bcache/journal.c
@@ -0,0 +1,787 @@
+/*
+ * bcache journalling code, for btree insertions
+ *
+ * Copyright 2012 Google, Inc.
+ */
+
+#include "bcache.h"
+#include "btree.h"
+#include "debug.h"
+#include "request.h"
+
+/*
+ * Journal replay/recovery:
+ *
+ * This code is all driven from run_cache_set(); we first read the journal
+ * entries, do some other stuff, then we mark all the keys in the journal
+ * entries (same as garbage collection would), then we replay them - reinserting
+ * them into the cache in precisely the same order as they appear in the
+ * journal.
+ *
+ * We only journal keys that go in leaf nodes, which simplifies things quite a
+ * bit.
+ */
+
+static void journal_read_endio(struct bio *bio, int error)
+{
+	struct closure *cl = bio->bi_private;
+	closure_put(cl);
+}
+
+static int journal_read_bucket(struct cache *ca, struct list_head *list,
+			       struct btree_op *op, unsigned bucket_index)
+{
+	struct journal_device *ja = &ca->journal;
+	struct bio *bio = &ja->bio;
+
+	struct journal_replay *i;
+	struct jset *j, *data = ca->set->journal.w[0].data;
+	unsigned len, left, offset = 0;
+	int ret = 0;
+	sector_t bucket = bucket_to_sector(ca->set, ca->sb.d[bucket_index]);
+
+	pr_debug("reading %llu", (uint64_t) bucket);
+
+	while (offset < ca->sb.bucket_size) {
+reread:		left = ca->sb.bucket_size - offset;
+		len = min_t(unsigned, left, PAGE_SECTORS * 8);
+
+		bio_reset(bio);
+		bio->bi_sector	= bucket + offset;
+		bio->bi_bdev	= ca->bdev;
+		bio->bi_rw	= READ;
+		bio->bi_size	= len << 9;
+
+		bio->bi_end_io	= journal_read_endio;
+		bio->bi_private = &op->cl;
+		bch_bio_map(bio, data);
+
+		closure_bio_submit(bio, &op->cl, ca);
+		closure_sync(&op->cl);
+
+		/* This function could be simpler now since we no longer write
+		 * journal entries that overlap bucket boundaries; this means
+		 * the start of a bucket will always have a valid journal entry
+		 * if it has any journal entries at all.
+		 */
+
+		j = data;
+		while (len) {
+			struct list_head *where;
+			size_t blocks, bytes = set_bytes(j);
+
+			if (j->magic != jset_magic(ca->set))
+				return ret;
+
+			if (bytes > left << 9)
+				return ret;
+
+			if (bytes > len << 9)
+				goto reread;
+
+			if (j->csum != csum_set(j))
+				return ret;
+
+			blocks = set_blocks(j, ca->set);
+
+			while (!list_empty(list)) {
+				i = list_first_entry(list,
+					struct journal_replay, list);
+				if (i->j.seq >= j->last_seq)
+					break;
+				list_del(&i->list);
+				kfree(i);
+			}
+
+			list_for_each_entry_reverse(i, list, list) {
+				if (j->seq == i->j.seq)
+					goto next_set;
+
+				if (j->seq < i->j.last_seq)
+					goto next_set;
+
+				if (j->seq > i->j.seq) {
+					where = &i->list;
+					goto add;
+				}
+			}
+
+			where = list;
+add:
+			i = kmalloc(offsetof(struct journal_replay, j) +
+				    bytes, GFP_KERNEL);
+			if (!i)
+				return -ENOMEM;
+			memcpy(&i->j, j, bytes);
+			list_add(&i->list, where);
+			ret = 1;
+
+			ja->seq[bucket_index] = j->seq;
+next_set:
+			offset	+= blocks * ca->sb.block_size;
+			len	-= blocks * ca->sb.block_size;
+			j = ((void *) j) + blocks * block_bytes(ca);
+		}
+	}
+
+	return ret;
+}
+
+int bch_journal_read(struct cache_set *c, struct list_head *list,
+			struct btree_op *op)
+{
+#define read_bucket(b)							\
+	({								\
+		int ret = journal_read_bucket(ca, list, op, b);		\
+		__set_bit(b, bitmap);					\
+		if (ret < 0)						\
+			return ret;					\
+		ret;							\
+	})
+
+	struct cache *ca;
+	unsigned iter;
+
+	for_each_cache(ca, c, iter) {
+		struct journal_device *ja = &ca->journal;
+		unsigned long bitmap[SB_JOURNAL_BUCKETS / BITS_PER_LONG];
+		unsigned i, l, r, m;
+		uint64_t seq;
+
+		bitmap_zero(bitmap, SB_JOURNAL_BUCKETS);
+		pr_debug("%u journal buckets", ca->sb.njournal_buckets);
+
+		/* Read journal buckets ordered by golden ratio hash to quickly
+		 * find a sequence of buckets with valid journal entries
+		 */
+		for (i = 0; i < ca->sb.njournal_buckets; i++) {
+			l = (i * 2654435769U) % ca->sb.njournal_buckets;
+
+			if (test_bit(l, bitmap))
+				break;
+
+			if (read_bucket(l))
+				goto bsearch;
+		}
+
+		/* If that fails, check all the buckets we haven't checked
+		 * already
+		 */
+		pr_debug("falling back to linear search");
+
+		for (l = 0; l < ca->sb.njournal_buckets; l++) {
+			if (test_bit(l, bitmap))
+				continue;
+
+			if (read_bucket(l))
+				goto bsearch;
+		}
+bsearch:
+		/* Binary search */
+		m = r = find_next_bit(bitmap, ca->sb.njournal_buckets, l + 1);
+		pr_debug("starting binary search, l %u r %u", l, r);
+
+		while (l + 1 < r) {
+			m = (l + r) >> 1;
+
+			if (read_bucket(m))
+				l = m;
+			else
+				r = m;
+		}
+
+		/* Read buckets in reverse order until we stop finding more
+		 * journal entries
+		 */
+		pr_debug("finishing up");
+		l = m;
+
+		while (1) {
+			if (!l--)
+				l = ca->sb.njournal_buckets - 1;
+
+			if (l == m)
+				break;
+
+			if (test_bit(l, bitmap))
+				continue;
+
+			if (!read_bucket(l))
+				break;
+		}
+
+		seq = 0;
+
+		for (i = 0; i < ca->sb.njournal_buckets; i++)
+			if (ja->seq[i] > seq) {
+				seq = ja->seq[i];
+				ja->cur_idx = ja->discard_idx =
+					ja->last_idx = i;
+
+			}
+	}
+
+	c->journal.seq = list_entry(list->prev,
+				    struct journal_replay,
+				    list)->j.seq;
+
+	return 0;
+#undef read_bucket
+}
+
+void bch_journal_mark(struct cache_set *c, struct list_head *list)
+{
+	atomic_t p = { 0 };
+	struct bkey *k;
+	struct journal_replay *i;
+	struct journal *j = &c->journal;
+	uint64_t last = j->seq;
+
+	/*
+	 * journal.pin should never fill up - we never write a journal
+	 * entry when it would fill up. But if for some reason it does, we
+	 * iterate over the list in reverse order so that we can just skip that
+	 * refcount instead of bugging.
+	 */
+
+	list_for_each_entry_reverse(i, list, list) {
+		BUG_ON(last < i->j.seq);
+		i->pin = NULL;
+
+		while (last-- != i->j.seq)
+			if (fifo_free(&j->pin) > 1) {
+				fifo_push_front(&j->pin, p);
+				atomic_set(&fifo_front(&j->pin), 0);
+			}
+
+		if (fifo_free(&j->pin) > 1) {
+			fifo_push_front(&j->pin, p);
+			i->pin = &fifo_front(&j->pin);
+			atomic_set(i->pin, 1);
+		}
+
+		for (k = i->j.start;
+		     k < end(&i->j);
+		     k = bkey_next(k)) {
+			unsigned j;
+
+			for (j = 0; j < KEY_PTRS(k); j++) {
+				struct bucket *g = PTR_BUCKET(c, k, j);
+				atomic_inc(&g->pin);
+
+				if (g->prio == BTREE_PRIO &&
+				    !ptr_stale(c, k, j))
+					g->prio = INITIAL_PRIO;
+			}
+
+			__bch_btree_mark_key(c, 0, k);
+		}
+	}
+}
+
+int bch_journal_replay(struct cache_set *s, struct list_head *list,
+			  struct btree_op *op)
+{
+	int ret = 0, keys = 0, entries = 0;
+	struct bkey *k;
+	struct journal_replay *i =
+		list_entry(list->prev, struct journal_replay, list);
+
+	uint64_t start = i->j.last_seq, end = i->j.seq, n = start;
+
+	list_for_each_entry(i, list, list) {
+		BUG_ON(i->pin && atomic_read(i->pin) != 1);
+
+		if (n != i->j.seq)
+			pr_err(
+		"journal entries %llu-%llu missing! (replaying %llu-%llu)\n",
+		n, i->j.seq - 1, start, end);
+
+		for (k = i->j.start;
+		     k < end(&i->j);
+		     k = bkey_next(k)) {
+			pr_debug("%s", pkey(k));
+			bkey_copy(op->keys.top, k);
+			bch_keylist_push(&op->keys);
+
+			op->journal = i->pin;
+			atomic_inc(op->journal);
+
+			ret = bch_btree_insert(op, s);
+			if (ret)
+				goto err;
+
+			BUG_ON(!bch_keylist_empty(&op->keys));
+			keys++;
+
+			cond_resched();
+		}
+
+		if (i->pin)
+			atomic_dec(i->pin);
+		n = i->j.seq + 1;
+		entries++;
+	}
+
+	pr_info("journal replay done, %i keys in %i entries, seq %llu",
+		keys, entries, end);
+
+	while (!list_empty(list)) {
+		i = list_first_entry(list, struct journal_replay, list);
+		list_del(&i->list);
+		kfree(i);
+	}
+err:
+	closure_sync(&op->cl);
+	return ret;
+}
+
+/* Journalling */
+
+static void btree_flush_write(struct cache_set *c)
+{
+	/*
+	 * Try to find the btree node with that references the oldest journal
+	 * entry, best is our current candidate and is locked if non NULL:
+	 */
+	struct btree *b, *best = NULL;
+	unsigned iter;
+
+	for_each_cached_btree(b, c, iter) {
+		if (!down_write_trylock(&b->lock))
+			continue;
+
+		if (!btree_node_dirty(b) ||
+		    !btree_current_write(b)->journal) {
+			rw_unlock(true, b);
+			continue;
+		}
+
+		if (!best)
+			best = b;
+		else if (journal_pin_cmp(c,
+					 btree_current_write(best),
+					 btree_current_write(b))) {
+			rw_unlock(true, best);
+			best = b;
+		} else
+			rw_unlock(true, b);
+	}
+
+	if (best)
+		goto out;
+
+	/* We can't find the best btree node, just pick the first */
+	list_for_each_entry(b, &c->btree_cache, list)
+		if (!b->level && btree_node_dirty(b)) {
+			best = b;
+			rw_lock(true, best, best->level);
+			goto found;
+		}
+
+out:
+	if (!best)
+		return;
+found:
+	if (btree_node_dirty(best))
+		bch_btree_write(best, true, NULL);
+	rw_unlock(true, best);
+}
+
+#define last_seq(j)	((j)->seq - fifo_used(&(j)->pin) + 1)
+
+static void journal_discard_endio(struct bio *bio, int error)
+{
+	struct journal_device *ja =
+		container_of(bio, struct journal_device, discard_bio);
+	struct cache *ca = container_of(ja, struct cache, journal);
+
+	atomic_set(&ja->discard_in_flight, DISCARD_DONE);
+
+	closure_wake_up(&ca->set->journal.wait);
+	closure_put(&ca->set->cl);
+}
+
+static void journal_discard_work(struct work_struct *work)
+{
+	struct journal_device *ja =
+		container_of(work, struct journal_device, discard_work);
+
+	submit_bio(0, &ja->discard_bio);
+}
+
+static void do_journal_discard(struct cache *ca)
+{
+	struct journal_device *ja = &ca->journal;
+	struct bio *bio = &ja->discard_bio;
+
+	if (!ca->discard) {
+		ja->discard_idx = ja->last_idx;
+		return;
+	}
+
+	switch (atomic_read(&ja->discard_in_flight) == DISCARD_IN_FLIGHT) {
+	case DISCARD_IN_FLIGHT:
+		return;
+
+	case DISCARD_DONE:
+		ja->discard_idx = (ja->discard_idx + 1) %
+			ca->sb.njournal_buckets;
+
+		atomic_set(&ja->discard_in_flight, DISCARD_READY);
+		/* fallthrough */
+
+	case DISCARD_READY:
+		if (ja->discard_idx == ja->last_idx)
+			return;
+
+		atomic_set(&ja->discard_in_flight, DISCARD_IN_FLIGHT);
+
+		bio_init(bio);
+		bio->bi_sector		= bucket_to_sector(ca->set,
+						ca->sb.d[ja->discard_idx]);
+		bio->bi_bdev		= ca->bdev;
+		bio->bi_rw		= REQ_WRITE|REQ_DISCARD;
+		bio->bi_max_vecs	= 1;
+		bio->bi_io_vec		= bio->bi_inline_vecs;
+		bio->bi_size		= bucket_bytes(ca);
+		bio->bi_end_io		= journal_discard_endio;
+
+		closure_get(&ca->set->cl);
+		INIT_WORK(&ja->discard_work, journal_discard_work);
+		schedule_work(&ja->discard_work);
+	}
+}
+
+static void journal_reclaim(struct cache_set *c)
+{
+	struct bkey *k = &c->journal.key;
+	struct cache *ca;
+	uint64_t last_seq;
+	unsigned iter, n = 0;
+	atomic_t p;
+
+	while (!atomic_read(&fifo_front(&c->journal.pin)))
+		fifo_pop(&c->journal.pin, p);
+
+	last_seq = last_seq(&c->journal);
+
+	/* Update last_idx */
+
+	for_each_cache(ca, c, iter) {
+		struct journal_device *ja = &ca->journal;
+
+		while (ja->last_idx != ja->cur_idx &&
+		       ja->seq[ja->last_idx] < last_seq)
+			ja->last_idx = (ja->last_idx + 1) %
+				ca->sb.njournal_buckets;
+	}
+
+	for_each_cache(ca, c, iter)
+		do_journal_discard(ca);
+
+	if (c->journal.blocks_free)
+		return;
+
+	/*
+	 * Allocate:
+	 * XXX: Sort by free journal space
+	 */
+
+	for_each_cache(ca, c, iter) {
+		struct journal_device *ja = &ca->journal;
+		unsigned next = (ja->cur_idx + 1) % ca->sb.njournal_buckets;
+
+		/* No space available on this device */
+		if (next == ja->discard_idx)
+			continue;
+
+		ja->cur_idx = next;
+		k->ptr[n++] = PTR(0,
+				  bucket_to_sector(c, ca->sb.d[ja->cur_idx]),
+				  ca->sb.nr_this_dev);
+	}
+
+	bkey_init(k);
+	SET_KEY_PTRS(k, n);
+
+	if (n)
+		c->journal.blocks_free = c->sb.bucket_size >> c->block_bits;
+
+	if (!journal_full(&c->journal))
+		__closure_wake_up(&c->journal.wait);
+}
+
+void bch_journal_next(struct journal *j)
+{
+	atomic_t p = { 1 };
+
+	j->cur = (j->cur == j->w)
+		? &j->w[1]
+		: &j->w[0];
+
+	/*
+	 * The fifo_push() needs to happen at the same time as j->seq is
+	 * incremented for last_seq() to be calculated correctly
+	 */
+	BUG_ON(!fifo_push(&j->pin, p));
+	atomic_set(&fifo_back(&j->pin), 1);
+
+	j->cur->data->seq	= ++j->seq;
+	j->cur->need_write	= false;
+	j->cur->data->keys	= 0;
+
+	if (fifo_full(&j->pin))
+		pr_debug("journal_pin full (%zu)", fifo_used(&j->pin));
+}
+
+static void journal_write_endio(struct bio *bio, int error)
+{
+	struct journal_write *w = bio->bi_private;
+
+	cache_set_err_on(error, w->c, "journal io error");
+	closure_put(&w->c->journal.io.cl);
+}
+
+static void journal_write(struct closure *);
+
+static void journal_write_done(struct closure *cl)
+{
+	struct journal *j = container_of(cl, struct journal, io.cl);
+	struct cache_set *c = container_of(j, struct cache_set, journal);
+
+	struct journal_write *w = (j->cur == j->w)
+		? &j->w[1]
+		: &j->w[0];
+
+	__closure_wake_up(&w->wait);
+
+	if (c->journal_delay_ms)
+		closure_delay(&j->io, msecs_to_jiffies(c->journal_delay_ms));
+
+	continue_at(cl, journal_write, system_wq);
+}
+
+static void journal_write_unlocked(struct closure *cl)
+	__releases(c->journal.lock)
+{
+	struct cache_set *c = container_of(cl, struct cache_set, journal.io.cl);
+	struct cache *ca;
+	struct journal_write *w = c->journal.cur;
+	struct bkey *k = &c->journal.key;
+	unsigned i, sectors = set_blocks(w->data, c) * c->sb.block_size;
+
+	struct bio *bio;
+	struct bio_list list;
+	bio_list_init(&list);
+
+	if (!w->need_write) {
+		/*
+		 * XXX: have to unlock closure before we unlock journal lock,
+		 * else we race with bch_journal(). But this way we race
+		 * against cache set unregister. Doh.
+		 */
+		set_closure_fn(cl, NULL, NULL);
+		closure_sub(cl, CLOSURE_RUNNING + 1);
+		spin_unlock(&c->journal.lock);
+		return;
+	} else if (journal_full(&c->journal)) {
+		journal_reclaim(c);
+		spin_unlock(&c->journal.lock);
+
+		btree_flush_write(c);
+		continue_at(cl, journal_write, system_wq);
+	}
+
+	c->journal.blocks_free -= set_blocks(w->data, c);
+
+	w->data->btree_level = c->root->level;
+
+	bkey_copy(&w->data->btree_root, &c->root->key);
+	bkey_copy(&w->data->uuid_bucket, &c->uuid_bucket);
+
+	for_each_cache(ca, c, i)
+		w->data->prio_bucket[ca->sb.nr_this_dev] = ca->prio_buckets[0];
+
+	w->data->magic		= jset_magic(c);
+	w->data->version	= BCACHE_JSET_VERSION;
+	w->data->last_seq	= last_seq(&c->journal);
+	w->data->csum		= csum_set(w->data);
+
+	for (i = 0; i < KEY_PTRS(k); i++) {
+		ca = PTR_CACHE(c, k, i);
+		bio = &ca->journal.bio;
+
+		atomic_long_add(sectors, &ca->meta_sectors_written);
+
+		bio_reset(bio);
+		bio->bi_sector	= PTR_OFFSET(k, i);
+		bio->bi_bdev	= ca->bdev;
+		bio->bi_rw	= REQ_WRITE|REQ_SYNC|REQ_META|REQ_FLUSH;
+		bio->bi_size	= sectors << 9;
+
+		bio->bi_end_io	= journal_write_endio;
+		bio->bi_private = w;
+		bch_bio_map(bio, w->data);
+
+		trace_bcache_journal_write(bio);
+		bio_list_add(&list, bio);
+
+		SET_PTR_OFFSET(k, i, PTR_OFFSET(k, i) + sectors);
+
+		ca->journal.seq[ca->journal.cur_idx] = w->data->seq;
+	}
+
+	atomic_dec_bug(&fifo_back(&c->journal.pin));
+	bch_journal_next(&c->journal);
+	journal_reclaim(c);
+
+	spin_unlock(&c->journal.lock);
+
+	while ((bio = bio_list_pop(&list)))
+		closure_bio_submit(bio, cl, c->cache[0]);
+
+	continue_at(cl, journal_write_done, NULL);
+}
+
+static void journal_write(struct closure *cl)
+{
+	struct cache_set *c = container_of(cl, struct cache_set, journal.io.cl);
+
+	spin_lock(&c->journal.lock);
+	journal_write_unlocked(cl);
+}
+
+static void __journal_try_write(struct cache_set *c, bool noflush)
+	__releases(c->journal.lock)
+{
+	struct closure *cl = &c->journal.io.cl;
+
+	if (!closure_trylock(cl, &c->cl))
+		spin_unlock(&c->journal.lock);
+	else if (noflush && journal_full(&c->journal)) {
+		spin_unlock(&c->journal.lock);
+		continue_at(cl, journal_write, system_wq);
+	} else
+		journal_write_unlocked(cl);
+}
+
+#define journal_try_write(c)	__journal_try_write(c, false)
+
+void bch_journal_meta(struct cache_set *c, struct closure *cl)
+{
+	struct journal_write *w;
+
+	if (CACHE_SYNC(&c->sb)) {
+		spin_lock(&c->journal.lock);
+
+		w = c->journal.cur;
+		w->need_write = true;
+
+		if (cl)
+			BUG_ON(!closure_wait(&w->wait, cl));
+
+		__journal_try_write(c, true);
+	}
+}
+
+/*
+ * Entry point to the journalling code - bio_insert() and btree_invalidate()
+ * pass bch_journal() a list of keys to be journalled, and then
+ * bch_journal() hands those same keys off to btree_insert_async()
+ */
+
+void bch_journal(struct closure *cl)
+{
+	struct btree_op *op = container_of(cl, struct btree_op, cl);
+	struct cache_set *c = op->c;
+	struct journal_write *w;
+	size_t b, n = ((uint64_t *) op->keys.top) - op->keys.list;
+
+	if (op->type != BTREE_INSERT ||
+	    !CACHE_SYNC(&c->sb))
+		goto out;
+
+	/*
+	 * If we're looping because we errored, might already be waiting on
+	 * another journal write:
+	 */
+	while (atomic_read(&cl->parent->remaining) & CLOSURE_WAITING)
+		closure_sync(cl->parent);
+
+	spin_lock(&c->journal.lock);
+
+	if (journal_full(&c->journal)) {
+		/* XXX: tracepoint */
+		closure_wait(&c->journal.wait, cl);
+
+		journal_reclaim(c);
+		spin_unlock(&c->journal.lock);
+
+		btree_flush_write(c);
+		continue_at(cl, bch_journal, bcache_wq);
+	}
+
+	w = c->journal.cur;
+	w->need_write = true;
+	b = __set_blocks(w->data, w->data->keys + n, c);
+
+	if (b * c->sb.block_size > PAGE_SECTORS << JSET_BITS ||
+	    b > c->journal.blocks_free) {
+		/* XXX: If we were inserting so many keys that they won't fit in
+		 * an _empty_ journal write, we'll deadlock. For now, handle
+		 * this in bch_keylist_realloc() - but something to think about.
+		 */
+		BUG_ON(!w->data->keys);
+
+		/* XXX: tracepoint */
+		BUG_ON(!closure_wait(&w->wait, cl));
+
+		closure_flush(&c->journal.io);
+
+		journal_try_write(c);
+		continue_at(cl, bch_journal, bcache_wq);
+	}
+
+	memcpy(end(w->data), op->keys.list, n * sizeof(uint64_t));
+	w->data->keys += n;
+
+	op->journal = &fifo_back(&c->journal.pin);
+	atomic_inc(op->journal);
+
+	if (op->flush_journal) {
+		closure_flush(&c->journal.io);
+		closure_wait(&w->wait, cl->parent);
+	}
+
+	journal_try_write(c);
+out:
+	bch_btree_insert_async(cl);
+}
+
+void bch_journal_free(struct cache_set *c)
+{
+	free_pages((unsigned long) c->journal.w[1].data, JSET_BITS);
+	free_pages((unsigned long) c->journal.w[0].data, JSET_BITS);
+	free_fifo(&c->journal.pin);
+}
+
+int bch_journal_alloc(struct cache_set *c)
+{
+	struct journal *j = &c->journal;
+
+	closure_init_unlocked(&j->io);
+	spin_lock_init(&j->lock);
+
+	c->journal_delay_ms = 100;
+
+	j->w[0].c = c;
+	j->w[1].c = c;
+
+	if (!(init_fifo(&j->pin, JOURNAL_PIN, GFP_KERNEL)) ||
+	    !(j->w[0].data = (void *) __get_free_pages(GFP_KERNEL, JSET_BITS)) ||
+	    !(j->w[1].data = (void *) __get_free_pages(GFP_KERNEL, JSET_BITS)))
+		return -ENOMEM;
+
+	return 0;
+}
diff --git a/drivers/md/bcache/journal.h b/drivers/md/bcache/journal.h
new file mode 100644
index 000000000000..3d7851274b04
--- /dev/null
+++ b/drivers/md/bcache/journal.h
@@ -0,0 +1,215 @@
+#ifndef _BCACHE_JOURNAL_H
+#define _BCACHE_JOURNAL_H
+
+/*
+ * THE JOURNAL:
+ *
+ * The journal is treated as a circular buffer of buckets - a journal entry
+ * never spans two buckets. This means (not implemented yet) we can resize the
+ * journal at runtime, and will be needed for bcache on raw flash support.
+ *
+ * Journal entries contain a list of keys, ordered by the time they were
+ * inserted; thus journal replay just has to reinsert the keys.
+ *
+ * We also keep some things in the journal header that are logically part of the
+ * superblock - all the things that are frequently updated. This is for future
+ * bcache on raw flash support; the superblock (which will become another
+ * journal) can't be moved or wear leveled, so it contains just enough
+ * information to find the main journal, and the superblock only has to be
+ * rewritten when we want to move/wear level the main journal.
+ *
+ * Currently, we don't journal BTREE_REPLACE operations - this will hopefully be
+ * fixed eventually. This isn't a bug - BTREE_REPLACE is used for insertions
+ * from cache misses, which don't have to be journaled, and for writeback and
+ * moving gc we work around it by flushing the btree to disk before updating the
+ * gc information. But it is a potential issue with incremental garbage
+ * collection, and it's fragile.
+ *
+ * OPEN JOURNAL ENTRIES:
+ *
+ * Each journal entry contains, in the header, the sequence number of the last
+ * journal entry still open - i.e. that has keys that haven't been flushed to
+ * disk in the btree.
+ *
+ * We track this by maintaining a refcount for every open journal entry, in a
+ * fifo; each entry in the fifo corresponds to a particular journal
+ * entry/sequence number. When the refcount at the tail of the fifo goes to
+ * zero, we pop it off - thus, the size of the fifo tells us the number of open
+ * journal entries
+ *
+ * We take a refcount on a journal entry when we add some keys to a journal
+ * entry that we're going to insert (held by struct btree_op), and then when we
+ * insert those keys into the btree the btree write we're setting up takes a
+ * copy of that refcount (held by struct btree_write). That refcount is dropped
+ * when the btree write completes.
+ *
+ * A struct btree_write can only hold a refcount on a single journal entry, but
+ * might contain keys for many journal entries - we handle this by making sure
+ * it always has a refcount on the _oldest_ journal entry of all the journal
+ * entries it has keys for.
+ *
+ * JOURNAL RECLAIM:
+ *
+ * As mentioned previously, our fifo of refcounts tells us the number of open
+ * journal entries; from that and the current journal sequence number we compute
+ * last_seq - the oldest journal entry we still need. We write last_seq in each
+ * journal entry, and we also have to keep track of where it exists on disk so
+ * we don't overwrite it when we loop around the journal.
+ *
+ * To do that we track, for each journal bucket, the sequence number of the
+ * newest journal entry it contains - if we don't need that journal entry we
+ * don't need anything in that bucket anymore. From that we track the last
+ * journal bucket we still need; all this is tracked in struct journal_device
+ * and updated by journal_reclaim().
+ *
+ * JOURNAL FILLING UP:
+ *
+ * There are two ways the journal could fill up; either we could run out of
+ * space to write to, or we could have too many open journal entries and run out
+ * of room in the fifo of refcounts. Since those refcounts are decremented
+ * without any locking we can't safely resize that fifo, so we handle it the
+ * same way.
+ *
+ * If the journal fills up, we start flushing dirty btree nodes until we can
+ * allocate space for a journal write again - preferentially flushing btree
+ * nodes that are pinning the oldest journal entries first.
+ */
+
+#define BCACHE_JSET_VERSION_UUIDv1	1
+/* Always latest UUID format */
+#define BCACHE_JSET_VERSION_UUID	1
+#define BCACHE_JSET_VERSION		1
+
+/*
+ * On disk format for a journal entry:
+ * seq is monotonically increasing; every journal entry has its own unique
+ * sequence number.
+ *
+ * last_seq is the oldest journal entry that still has keys the btree hasn't
+ * flushed to disk yet.
+ *
+ * version is for on disk format changes.
+ */
+struct jset {
+	uint64_t		csum;
+	uint64_t		magic;
+	uint64_t		seq;
+	uint32_t		version;
+	uint32_t		keys;
+
+	uint64_t		last_seq;
+
+	BKEY_PADDED(uuid_bucket);
+	BKEY_PADDED(btree_root);
+	uint16_t		btree_level;
+	uint16_t		pad[3];
+
+	uint64_t		prio_bucket[MAX_CACHES_PER_SET];
+
+	union {
+		struct bkey	start[0];
+		uint64_t	d[0];
+	};
+};
+
+/*
+ * Only used for holding the journal entries we read in btree_journal_read()
+ * during cache_registration
+ */
+struct journal_replay {
+	struct list_head	list;
+	atomic_t		*pin;
+	struct jset		j;
+};
+
+/*
+ * We put two of these in struct journal; we used them for writes to the
+ * journal that are being staged or in flight.
+ */
+struct journal_write {
+	struct jset		*data;
+#define JSET_BITS		3
+
+	struct cache_set	*c;
+	struct closure_waitlist	wait;
+	bool			need_write;
+};
+
+/* Embedded in struct cache_set */
+struct journal {
+	spinlock_t		lock;
+	/* used when waiting because the journal was full */
+	struct closure_waitlist	wait;
+	struct closure_with_timer io;
+
+	/* Number of blocks free in the bucket(s) we're currently writing to */
+	unsigned		blocks_free;
+	uint64_t		seq;
+	DECLARE_FIFO(atomic_t, pin);
+
+	BKEY_PADDED(key);
+
+	struct journal_write	w[2], *cur;
+};
+
+/*
+ * Embedded in struct cache. First three fields refer to the array of journal
+ * buckets, in cache_sb.
+ */
+struct journal_device {
+	/*
+	 * For each journal bucket, contains the max sequence number of the
+	 * journal writes it contains - so we know when a bucket can be reused.
+	 */
+	uint64_t		seq[SB_JOURNAL_BUCKETS];
+
+	/* Journal bucket we're currently writing to */
+	unsigned		cur_idx;
+
+	/* Last journal bucket that still contains an open journal entry */
+	unsigned		last_idx;
+
+	/* Next journal bucket to be discarded */
+	unsigned		discard_idx;
+
+#define DISCARD_READY		0
+#define DISCARD_IN_FLIGHT	1
+#define DISCARD_DONE		2
+	/* 1 - discard in flight, -1 - discard completed */
+	atomic_t		discard_in_flight;
+
+	struct work_struct	discard_work;
+	struct bio		discard_bio;
+	struct bio_vec		discard_bv;
+
+	/* Bio for journal reads/writes to this device */
+	struct bio		bio;
+	struct bio_vec		bv[8];
+};
+
+#define journal_pin_cmp(c, l, r)				\
+	(fifo_idx(&(c)->journal.pin, (l)->journal) >		\
+	 fifo_idx(&(c)->journal.pin, (r)->journal))
+
+#define JOURNAL_PIN	20000
+
+#define journal_full(j)						\
+	(!(j)->blocks_free || fifo_free(&(j)->pin) <= 1)
+
+struct closure;
+struct cache_set;
+struct btree_op;
+
+void bch_journal(struct closure *);
+void bch_journal_next(struct journal *);
+void bch_journal_mark(struct cache_set *, struct list_head *);
+void bch_journal_meta(struct cache_set *, struct closure *);
+int bch_journal_read(struct cache_set *, struct list_head *,
+			struct btree_op *);
+int bch_journal_replay(struct cache_set *, struct list_head *,
+			  struct btree_op *);
+
+void bch_journal_free(struct cache_set *);
+int bch_journal_alloc(struct cache_set *);
+
+#endif /* _BCACHE_JOURNAL_H */
diff --git a/drivers/md/bcache/movinggc.c b/drivers/md/bcache/movinggc.c
new file mode 100644
index 000000000000..8589512c972e
--- /dev/null
+++ b/drivers/md/bcache/movinggc.c
@@ -0,0 +1,254 @@
+/*
+ * Moving/copying garbage collector
+ *
+ * Copyright 2012 Google, Inc.
+ */
+
+#include "bcache.h"
+#include "btree.h"
+#include "debug.h"
+#include "request.h"
+
+struct moving_io {
+	struct keybuf_key	*w;
+	struct search		s;
+	struct bbio		bio;
+};
+
+static bool moving_pred(struct keybuf *buf, struct bkey *k)
+{
+	struct cache_set *c = container_of(buf, struct cache_set,
+					   moving_gc_keys);
+	unsigned i;
+
+	for (i = 0; i < KEY_PTRS(k); i++) {
+		struct cache *ca = PTR_CACHE(c, k, i);
+		struct bucket *g = PTR_BUCKET(c, k, i);
+
+		if (GC_SECTORS_USED(g) < ca->gc_move_threshold)
+			return true;
+	}
+
+	return false;
+}
+
+/* Moving GC - IO loop */
+
+static void moving_io_destructor(struct closure *cl)
+{
+	struct moving_io *io = container_of(cl, struct moving_io, s.cl);
+	kfree(io);
+}
+
+static void write_moving_finish(struct closure *cl)
+{
+	struct moving_io *io = container_of(cl, struct moving_io, s.cl);
+	struct bio *bio = &io->bio.bio;
+	struct bio_vec *bv = bio_iovec_idx(bio, bio->bi_vcnt);
+
+	while (bv-- != bio->bi_io_vec)
+		__free_page(bv->bv_page);
+
+	pr_debug("%s %s", io->s.op.insert_collision
+		 ? "collision moving" : "moved",
+		 pkey(&io->w->key));
+
+	bch_keybuf_del(&io->s.op.c->moving_gc_keys, io->w);
+
+	atomic_dec_bug(&io->s.op.c->in_flight);
+	closure_wake_up(&io->s.op.c->moving_gc_wait);
+
+	closure_return_with_destructor(cl, moving_io_destructor);
+}
+
+static void read_moving_endio(struct bio *bio, int error)
+{
+	struct moving_io *io = container_of(bio->bi_private,
+					    struct moving_io, s.cl);
+
+	if (error)
+		io->s.error = error;
+
+	bch_bbio_endio(io->s.op.c, bio, error, "reading data to move");
+}
+
+static void moving_init(struct moving_io *io)
+{
+	struct bio *bio = &io->bio.bio;
+
+	bio_init(bio);
+	bio_get(bio);
+	bio_set_prio(bio, IOPRIO_PRIO_VALUE(IOPRIO_CLASS_IDLE, 0));
+
+	bio->bi_size		= KEY_SIZE(&io->w->key) << 9;
+	bio->bi_max_vecs	= DIV_ROUND_UP(KEY_SIZE(&io->w->key),
+					       PAGE_SECTORS);
+	bio->bi_private		= &io->s.cl;
+	bio->bi_io_vec		= bio->bi_inline_vecs;
+	bch_bio_map(bio, NULL);
+}
+
+static void write_moving(struct closure *cl)
+{
+	struct search *s = container_of(cl, struct search, cl);
+	struct moving_io *io = container_of(s, struct moving_io, s);
+
+	if (!s->error) {
+		trace_bcache_write_moving(&io->bio.bio);
+
+		moving_init(io);
+
+		io->bio.bio.bi_sector	= KEY_START(&io->w->key);
+		s->op.lock		= -1;
+		s->op.write_prio	= 1;
+		s->op.cache_bio		= &io->bio.bio;
+
+		s->writeback		= KEY_DIRTY(&io->w->key);
+		s->op.csum		= KEY_CSUM(&io->w->key);
+
+		s->op.type = BTREE_REPLACE;
+		bkey_copy(&s->op.replace, &io->w->key);
+
+		closure_init(&s->op.cl, cl);
+		bch_insert_data(&s->op.cl);
+	}
+
+	continue_at(cl, write_moving_finish, NULL);
+}
+
+static void read_moving_submit(struct closure *cl)
+{
+	struct search *s = container_of(cl, struct search, cl);
+	struct moving_io *io = container_of(s, struct moving_io, s);
+	struct bio *bio = &io->bio.bio;
+
+	trace_bcache_read_moving(bio);
+	bch_submit_bbio(bio, s->op.c, &io->w->key, 0);
+
+	continue_at(cl, write_moving, bch_gc_wq);
+}
+
+static void read_moving(struct closure *cl)
+{
+	struct cache_set *c = container_of(cl, struct cache_set, moving_gc);
+	struct keybuf_key *w;
+	struct moving_io *io;
+	struct bio *bio;
+
+	/* XXX: if we error, background writeback could stall indefinitely */
+
+	while (!test_bit(CACHE_SET_STOPPING, &c->flags)) {
+		w = bch_keybuf_next_rescan(c, &c->moving_gc_keys, &MAX_KEY);
+		if (!w)
+			break;
+
+		io = kzalloc(sizeof(struct moving_io) + sizeof(struct bio_vec)
+			     * DIV_ROUND_UP(KEY_SIZE(&w->key), PAGE_SECTORS),
+			     GFP_KERNEL);
+		if (!io)
+			goto err;
+
+		w->private	= io;
+		io->w		= w;
+		io->s.op.inode	= KEY_INODE(&w->key);
+		io->s.op.c	= c;
+
+		moving_init(io);
+		bio = &io->bio.bio;
+
+		bio->bi_rw	= READ;
+		bio->bi_end_io	= read_moving_endio;
+
+		if (bch_bio_alloc_pages(bio, GFP_KERNEL))
+			goto err;
+
+		pr_debug("%s", pkey(&w->key));
+
+		closure_call(&io->s.cl, read_moving_submit, NULL, &c->gc.cl);
+
+		if (atomic_inc_return(&c->in_flight) >= 64) {
+			closure_wait_event(&c->moving_gc_wait, cl,
+					   atomic_read(&c->in_flight) < 64);
+			continue_at(cl, read_moving, bch_gc_wq);
+		}
+	}
+
+	if (0) {
+err:		if (!IS_ERR_OR_NULL(w->private))
+			kfree(w->private);
+
+		bch_keybuf_del(&c->moving_gc_keys, w);
+	}
+
+	closure_return(cl);
+}
+
+static bool bucket_cmp(struct bucket *l, struct bucket *r)
+{
+	return GC_SECTORS_USED(l) < GC_SECTORS_USED(r);
+}
+
+static unsigned bucket_heap_top(struct cache *ca)
+{
+	return GC_SECTORS_USED(heap_peek(&ca->heap));
+}
+
+void bch_moving_gc(struct closure *cl)
+{
+	struct cache_set *c = container_of(cl, struct cache_set, gc.cl);
+	struct cache *ca;
+	struct bucket *b;
+	unsigned i;
+
+	if (!c->copy_gc_enabled)
+		closure_return(cl);
+
+	mutex_lock(&c->bucket_lock);
+
+	for_each_cache(ca, c, i) {
+		unsigned sectors_to_move = 0;
+		unsigned reserve_sectors = ca->sb.bucket_size *
+			min(fifo_used(&ca->free), ca->free.size / 2);
+
+		ca->heap.used = 0;
+
+		for_each_bucket(b, ca) {
+			if (!GC_SECTORS_USED(b))
+				continue;
+
+			if (!heap_full(&ca->heap)) {
+				sectors_to_move += GC_SECTORS_USED(b);
+				heap_add(&ca->heap, b, bucket_cmp);
+			} else if (bucket_cmp(b, heap_peek(&ca->heap))) {
+				sectors_to_move -= bucket_heap_top(ca);
+				sectors_to_move += GC_SECTORS_USED(b);
+
+				ca->heap.data[0] = b;
+				heap_sift(&ca->heap, 0, bucket_cmp);
+			}
+		}
+
+		while (sectors_to_move > reserve_sectors) {
+			heap_pop(&ca->heap, b, bucket_cmp);
+			sectors_to_move -= GC_SECTORS_USED(b);
+		}
+
+		ca->gc_move_threshold = bucket_heap_top(ca);
+
+		pr_debug("threshold %u", ca->gc_move_threshold);
+	}
+
+	mutex_unlock(&c->bucket_lock);
+
+	c->moving_gc_keys.last_scanned = ZERO_KEY;
+
+	closure_init(&c->moving_gc, cl);
+	read_moving(&c->moving_gc);
+
+	closure_return(cl);
+}
+
+void bch_moving_init_cache_set(struct cache_set *c)
+{
+	bch_keybuf_init(&c->moving_gc_keys, moving_pred);
+}
diff --git a/drivers/md/bcache/request.c b/drivers/md/bcache/request.c
new file mode 100644
index 000000000000..e5ff12e52d5b
--- /dev/null
+++ b/drivers/md/bcache/request.c
@@ -0,0 +1,1411 @@
+/*
+ * Main bcache entry point - handle a read or a write request and decide what to
+ * do with it; the make_request functions are called by the block layer.
+ *
+ * Copyright 2010, 2011 Kent Overstreet <kent.overstreet@gmail.com>
+ * Copyright 2012 Google, Inc.
+ */
+
+#include "bcache.h"
+#include "btree.h"
+#include "debug.h"
+#include "request.h"
+
+#include <linux/cgroup.h>
+#include <linux/module.h>
+#include <linux/hash.h>
+#include <linux/random.h>
+#include "blk-cgroup.h"
+
+#include <trace/events/bcache.h>
+
+#define CUTOFF_CACHE_ADD	95
+#define CUTOFF_CACHE_READA	90
+#define CUTOFF_WRITEBACK	50
+#define CUTOFF_WRITEBACK_SYNC	75
+
+struct kmem_cache *bch_search_cache;
+
+static void check_should_skip(struct cached_dev *, struct search *);
+
+/* Cgroup interface */
+
+#ifdef CONFIG_CGROUP_BCACHE
+static struct bch_cgroup bcache_default_cgroup = { .cache_mode = -1 };
+
+static struct bch_cgroup *cgroup_to_bcache(struct cgroup *cgroup)
+{
+	struct cgroup_subsys_state *css;
+	return cgroup &&
+		(css = cgroup_subsys_state(cgroup, bcache_subsys_id))
+		? container_of(css, struct bch_cgroup, css)
+		: &bcache_default_cgroup;
+}
+
+struct bch_cgroup *bch_bio_to_cgroup(struct bio *bio)
+{
+	struct cgroup_subsys_state *css = bio->bi_css
+		? cgroup_subsys_state(bio->bi_css->cgroup, bcache_subsys_id)
+		: task_subsys_state(current, bcache_subsys_id);
+
+	return css
+		? container_of(css, struct bch_cgroup, css)
+		: &bcache_default_cgroup;
+}
+
+static ssize_t cache_mode_read(struct cgroup *cgrp, struct cftype *cft,
+			struct file *file,
+			char __user *buf, size_t nbytes, loff_t *ppos)
+{
+	char tmp[1024];
+	int len = bch_snprint_string_list(tmp, PAGE_SIZE, bch_cache_modes,
+					  cgroup_to_bcache(cgrp)->cache_mode + 1);
+
+	if (len < 0)
+		return len;
+
+	return simple_read_from_buffer(buf, nbytes, ppos, tmp, len);
+}
+
+static int cache_mode_write(struct cgroup *cgrp, struct cftype *cft,
+			    const char *buf)
+{
+	int v = bch_read_string_list(buf, bch_cache_modes);
+	if (v < 0)
+		return v;
+
+	cgroup_to_bcache(cgrp)->cache_mode = v - 1;
+	return 0;
+}
+
+static u64 bch_verify_read(struct cgroup *cgrp, struct cftype *cft)
+{
+	return cgroup_to_bcache(cgrp)->verify;
+}
+
+static int bch_verify_write(struct cgroup *cgrp, struct cftype *cft, u64 val)
+{
+	cgroup_to_bcache(cgrp)->verify = val;
+	return 0;
+}
+
+static u64 bch_cache_hits_read(struct cgroup *cgrp, struct cftype *cft)
+{
+	struct bch_cgroup *bcachecg = cgroup_to_bcache(cgrp);
+	return atomic_read(&bcachecg->stats.cache_hits);
+}
+
+static u64 bch_cache_misses_read(struct cgroup *cgrp, struct cftype *cft)
+{
+	struct bch_cgroup *bcachecg = cgroup_to_bcache(cgrp);
+	return atomic_read(&bcachecg->stats.cache_misses);
+}
+
+static u64 bch_cache_bypass_hits_read(struct cgroup *cgrp,
+					 struct cftype *cft)
+{
+	struct bch_cgroup *bcachecg = cgroup_to_bcache(cgrp);
+	return atomic_read(&bcachecg->stats.cache_bypass_hits);
+}
+
+static u64 bch_cache_bypass_misses_read(struct cgroup *cgrp,
+					   struct cftype *cft)
+{
+	struct bch_cgroup *bcachecg = cgroup_to_bcache(cgrp);
+	return atomic_read(&bcachecg->stats.cache_bypass_misses);
+}
+
+static struct cftype bch_files[] = {
+	{
+		.name		= "cache_mode",
+		.read		= cache_mode_read,
+		.write_string	= cache_mode_write,
+	},
+	{
+		.name		= "verify",
+		.read_u64	= bch_verify_read,
+		.write_u64	= bch_verify_write,
+	},
+	{
+		.name		= "cache_hits",
+		.read_u64	= bch_cache_hits_read,
+	},
+	{
+		.name		= "cache_misses",
+		.read_u64	= bch_cache_misses_read,
+	},
+	{
+		.name		= "cache_bypass_hits",
+		.read_u64	= bch_cache_bypass_hits_read,
+	},
+	{
+		.name		= "cache_bypass_misses",
+		.read_u64	= bch_cache_bypass_misses_read,
+	},
+	{ }	/* terminate */
+};
+
+static void init_bch_cgroup(struct bch_cgroup *cg)
+{
+	cg->cache_mode = -1;
+}
+
+static struct cgroup_subsys_state *bcachecg_create(struct cgroup *cgroup)
+{
+	struct bch_cgroup *cg;
+
+	cg = kzalloc(sizeof(*cg), GFP_KERNEL);
+	if (!cg)
+		return ERR_PTR(-ENOMEM);
+	init_bch_cgroup(cg);
+	return &cg->css;
+}
+
+static void bcachecg_destroy(struct cgroup *cgroup)
+{
+	struct bch_cgroup *cg = cgroup_to_bcache(cgroup);
+	free_css_id(&bcache_subsys, &cg->css);
+	kfree(cg);
+}
+
+struct cgroup_subsys bcache_subsys = {
+	.create		= bcachecg_create,
+	.destroy	= bcachecg_destroy,
+	.subsys_id	= bcache_subsys_id,
+	.name		= "bcache",
+	.module		= THIS_MODULE,
+};
+EXPORT_SYMBOL_GPL(bcache_subsys);
+#endif
+
+static unsigned cache_mode(struct cached_dev *dc, struct bio *bio)
+{
+#ifdef CONFIG_CGROUP_BCACHE
+	int r = bch_bio_to_cgroup(bio)->cache_mode;
+	if (r >= 0)
+		return r;
+#endif
+	return BDEV_CACHE_MODE(&dc->sb);
+}
+
+static bool verify(struct cached_dev *dc, struct bio *bio)
+{
+#ifdef CONFIG_CGROUP_BCACHE
+	if (bch_bio_to_cgroup(bio)->verify)
+		return true;
+#endif
+	return dc->verify;
+}
+
+static void bio_csum(struct bio *bio, struct bkey *k)
+{
+	struct bio_vec *bv;
+	uint64_t csum = 0;
+	int i;
+
+	bio_for_each_segment(bv, bio, i) {
+		void *d = kmap(bv->bv_page) + bv->bv_offset;
+		csum = bch_crc64_update(csum, d, bv->bv_len);
+		kunmap(bv->bv_page);
+	}
+
+	k->ptr[KEY_PTRS(k)] = csum & (~0ULL >> 1);
+}
+
+/* Insert data into cache */
+
+static void bio_invalidate(struct closure *cl)
+{
+	struct btree_op *op = container_of(cl, struct btree_op, cl);
+	struct bio *bio = op->cache_bio;
+
+	pr_debug("invalidating %i sectors from %llu",
+		 bio_sectors(bio), (uint64_t) bio->bi_sector);
+
+	while (bio_sectors(bio)) {
+		unsigned len = min(bio_sectors(bio), 1U << 14);
+
+		if (bch_keylist_realloc(&op->keys, 0, op->c))
+			goto out;
+
+		bio->bi_sector	+= len;
+		bio->bi_size	-= len << 9;
+
+		bch_keylist_add(&op->keys,
+				&KEY(op->inode, bio->bi_sector, len));
+	}
+
+	op->insert_data_done = true;
+	bio_put(bio);
+out:
+	continue_at(cl, bch_journal, bcache_wq);
+}
+
+struct open_bucket {
+	struct list_head	list;
+	struct task_struct	*last;
+	unsigned		sectors_free;
+	BKEY_PADDED(key);
+};
+
+void bch_open_buckets_free(struct cache_set *c)
+{
+	struct open_bucket *b;
+
+	while (!list_empty(&c->data_buckets)) {
+		b = list_first_entry(&c->data_buckets,
+				     struct open_bucket, list);
+		list_del(&b->list);
+		kfree(b);
+	}
+}
+
+int bch_open_buckets_alloc(struct cache_set *c)
+{
+	int i;
+
+	spin_lock_init(&c->data_bucket_lock);
+
+	for (i = 0; i < 6; i++) {
+		struct open_bucket *b = kzalloc(sizeof(*b), GFP_KERNEL);
+		if (!b)
+			return -ENOMEM;
+
+		list_add(&b->list, &c->data_buckets);
+	}
+
+	return 0;
+}
+
+/*
+ * We keep multiple buckets open for writes, and try to segregate different
+ * write streams for better cache utilization: first we look for a bucket where
+ * the last write to it was sequential with the current write, and failing that
+ * we look for a bucket that was last used by the same task.
+ *
+ * The ideas is if you've got multiple tasks pulling data into the cache at the
+ * same time, you'll get better cache utilization if you try to segregate their
+ * data and preserve locality.
+ *
+ * For example, say you've starting Firefox at the same time you're copying a
+ * bunch of files. Firefox will likely end up being fairly hot and stay in the
+ * cache awhile, but the data you copied might not be; if you wrote all that
+ * data to the same buckets it'd get invalidated at the same time.
+ *
+ * Both of those tasks will be doing fairly random IO so we can't rely on
+ * detecting sequential IO to segregate their data, but going off of the task
+ * should be a sane heuristic.
+ */
+static struct open_bucket *pick_data_bucket(struct cache_set *c,
+					    const struct bkey *search,
+					    struct task_struct *task,
+					    struct bkey *alloc)
+{
+	struct open_bucket *ret, *ret_task = NULL;
+
+	list_for_each_entry_reverse(ret, &c->data_buckets, list)
+		if (!bkey_cmp(&ret->key, search))
+			goto found;
+		else if (ret->last == task)
+			ret_task = ret;
+
+	ret = ret_task ?: list_first_entry(&c->data_buckets,
+					   struct open_bucket, list);
+found:
+	if (!ret->sectors_free && KEY_PTRS(alloc)) {
+		ret->sectors_free = c->sb.bucket_size;
+		bkey_copy(&ret->key, alloc);
+		bkey_init(alloc);
+	}
+
+	if (!ret->sectors_free)
+		ret = NULL;
+
+	return ret;
+}
+
+/*
+ * Allocates some space in the cache to write to, and k to point to the newly
+ * allocated space, and updates KEY_SIZE(k) and KEY_OFFSET(k) (to point to the
+ * end of the newly allocated space).
+ *
+ * May allocate fewer sectors than @sectors, KEY_SIZE(k) indicates how many
+ * sectors were actually allocated.
+ *
+ * If s->writeback is true, will not fail.
+ */
+static bool bch_alloc_sectors(struct bkey *k, unsigned sectors,
+			      struct search *s)
+{
+	struct cache_set *c = s->op.c;
+	struct open_bucket *b;
+	BKEY_PADDED(key) alloc;
+	struct closure cl, *w = NULL;
+	unsigned i;
+
+	if (s->writeback) {
+		closure_init_stack(&cl);
+		w = &cl;
+	}
+
+	/*
+	 * We might have to allocate a new bucket, which we can't do with a
+	 * spinlock held. So if we have to allocate, we drop the lock, allocate
+	 * and then retry. KEY_PTRS() indicates whether alloc points to
+	 * allocated bucket(s).
+	 */
+
+	bkey_init(&alloc.key);
+	spin_lock(&c->data_bucket_lock);
+
+	while (!(b = pick_data_bucket(c, k, s->task, &alloc.key))) {
+		unsigned watermark = s->op.write_prio
+			? WATERMARK_MOVINGGC
+			: WATERMARK_NONE;
+
+		spin_unlock(&c->data_bucket_lock);
+
+		if (bch_bucket_alloc_set(c, watermark, &alloc.key, 1, w))
+			return false;
+
+		spin_lock(&c->data_bucket_lock);
+	}
+
+	/*
+	 * If we had to allocate, we might race and not need to allocate the
+	 * second time we call find_data_bucket(). If we allocated a bucket but
+	 * didn't use it, drop the refcount bch_bucket_alloc_set() took:
+	 */
+	if (KEY_PTRS(&alloc.key))
+		__bkey_put(c, &alloc.key);
+
+	for (i = 0; i < KEY_PTRS(&b->key); i++)
+		EBUG_ON(ptr_stale(c, &b->key, i));
+
+	/* Set up the pointer to the space we're allocating: */
+
+	for (i = 0; i < KEY_PTRS(&b->key); i++)
+		k->ptr[i] = b->key.ptr[i];
+
+	sectors = min(sectors, b->sectors_free);
+
+	SET_KEY_OFFSET(k, KEY_OFFSET(k) + sectors);
+	SET_KEY_SIZE(k, sectors);
+	SET_KEY_PTRS(k, KEY_PTRS(&b->key));
+
+	/*
+	 * Move b to the end of the lru, and keep track of what this bucket was
+	 * last used for:
+	 */
+	list_move_tail(&b->list, &c->data_buckets);
+	bkey_copy_key(&b->key, k);
+	b->last = s->task;
+
+	b->sectors_free	-= sectors;
+
+	for (i = 0; i < KEY_PTRS(&b->key); i++) {
+		SET_PTR_OFFSET(&b->key, i, PTR_OFFSET(&b->key, i) + sectors);
+
+		atomic_long_add(sectors,
+				&PTR_CACHE(c, &b->key, i)->sectors_written);
+	}
+
+	if (b->sectors_free < c->sb.block_size)
+		b->sectors_free = 0;
+
+	/*
+	 * k takes refcounts on the buckets it points to until it's inserted
+	 * into the btree, but if we're done with this bucket we just transfer
+	 * get_data_bucket()'s refcount.
+	 */
+	if (b->sectors_free)
+		for (i = 0; i < KEY_PTRS(&b->key); i++)
+			atomic_inc(&PTR_BUCKET(c, &b->key, i)->pin);
+
+	spin_unlock(&c->data_bucket_lock);
+	return true;
+}
+
+static void bch_insert_data_error(struct closure *cl)
+{
+	struct btree_op *op = container_of(cl, struct btree_op, cl);
+
+	/*
+	 * Our data write just errored, which means we've got a bunch of keys to
+	 * insert that point to data that wasn't succesfully written.
+	 *
+	 * We don't have to insert those keys but we still have to invalidate
+	 * that region of the cache - so, if we just strip off all the pointers
+	 * from the keys we'll accomplish just that.
+	 */
+
+	struct bkey *src = op->keys.bottom, *dst = op->keys.bottom;
+
+	while (src != op->keys.top) {
+		struct bkey *n = bkey_next(src);
+
+		SET_KEY_PTRS(src, 0);
+		bkey_copy(dst, src);
+
+		dst = bkey_next(dst);
+		src = n;
+	}
+
+	op->keys.top = dst;
+
+	bch_journal(cl);
+}
+
+static void bch_insert_data_endio(struct bio *bio, int error)
+{
+	struct closure *cl = bio->bi_private;
+	struct btree_op *op = container_of(cl, struct btree_op, cl);
+	struct search *s = container_of(op, struct search, op);
+
+	if (error) {
+		/* TODO: We could try to recover from this. */
+		if (s->writeback)
+			s->error = error;
+		else if (s->write)
+			set_closure_fn(cl, bch_insert_data_error, bcache_wq);
+		else
+			set_closure_fn(cl, NULL, NULL);
+	}
+
+	bch_bbio_endio(op->c, bio, error, "writing data to cache");
+}
+
+static void bch_insert_data_loop(struct closure *cl)
+{
+	struct btree_op *op = container_of(cl, struct btree_op, cl);
+	struct search *s = container_of(op, struct search, op);
+	struct bio *bio = op->cache_bio, *n;
+
+	if (op->skip)
+		return bio_invalidate(cl);
+
+	if (atomic_sub_return(bio_sectors(bio), &op->c->sectors_to_gc) < 0) {
+		set_gc_sectors(op->c);
+		bch_queue_gc(op->c);
+	}
+
+	do {
+		unsigned i;
+		struct bkey *k;
+		struct bio_set *split = s->d
+			? s->d->bio_split : op->c->bio_split;
+
+		/* 1 for the device pointer and 1 for the chksum */
+		if (bch_keylist_realloc(&op->keys,
+					1 + (op->csum ? 1 : 0),
+					op->c))
+			continue_at(cl, bch_journal, bcache_wq);
+
+		k = op->keys.top;
+		bkey_init(k);
+		SET_KEY_INODE(k, op->inode);
+		SET_KEY_OFFSET(k, bio->bi_sector);
+
+		if (!bch_alloc_sectors(k, bio_sectors(bio), s))
+			goto err;
+
+		n = bch_bio_split(bio, KEY_SIZE(k), GFP_NOIO, split);
+		if (!n) {
+			__bkey_put(op->c, k);
+			continue_at(cl, bch_insert_data_loop, bcache_wq);
+		}
+
+		n->bi_end_io	= bch_insert_data_endio;
+		n->bi_private	= cl;
+
+		if (s->writeback) {
+			SET_KEY_DIRTY(k, true);
+
+			for (i = 0; i < KEY_PTRS(k); i++)
+				SET_GC_MARK(PTR_BUCKET(op->c, k, i),
+					    GC_MARK_DIRTY);
+		}
+
+		SET_KEY_CSUM(k, op->csum);
+		if (KEY_CSUM(k))
+			bio_csum(n, k);
+
+		pr_debug("%s", pkey(k));
+		bch_keylist_push(&op->keys);
+
+		trace_bcache_cache_insert(n, n->bi_sector, n->bi_bdev);
+		n->bi_rw |= REQ_WRITE;
+		bch_submit_bbio(n, op->c, k, 0);
+	} while (n != bio);
+
+	op->insert_data_done = true;
+	continue_at(cl, bch_journal, bcache_wq);
+err:
+	/* bch_alloc_sectors() blocks if s->writeback = true */
+	BUG_ON(s->writeback);
+
+	/*
+	 * But if it's not a writeback write we'd rather just bail out if
+	 * there aren't any buckets ready to write to - it might take awhile and
+	 * we might be starving btree writes for gc or something.
+	 */
+
+	if (s->write) {
+		/*
+		 * Writethrough write: We can't complete the write until we've
+		 * updated the index. But we don't want to delay the write while
+		 * we wait for buckets to be freed up, so just invalidate the
+		 * rest of the write.
+		 */
+		op->skip = true;
+		return bio_invalidate(cl);
+	} else {
+		/*
+		 * From a cache miss, we can just insert the keys for the data
+		 * we have written or bail out if we didn't do anything.
+		 */
+		op->insert_data_done = true;
+		bio_put(bio);
+
+		if (!bch_keylist_empty(&op->keys))
+			continue_at(cl, bch_journal, bcache_wq);
+		else
+			closure_return(cl);
+	}
+}
+
+/**
+ * bch_insert_data - stick some data in the cache
+ *
+ * This is the starting point for any data to end up in a cache device; it could
+ * be from a normal write, or a writeback write, or a write to a flash only
+ * volume - it's also used by the moving garbage collector to compact data in
+ * mostly empty buckets.
+ *
+ * It first writes the data to the cache, creating a list of keys to be inserted
+ * (if the data had to be fragmented there will be multiple keys); after the
+ * data is written it calls bch_journal, and after the keys have been added to
+ * the next journal write they're inserted into the btree.
+ *
+ * It inserts the data in op->cache_bio; bi_sector is used for the key offset,
+ * and op->inode is used for the key inode.
+ *
+ * If op->skip is true, instead of inserting the data it invalidates the region
+ * of the cache represented by op->cache_bio and op->inode.
+ */
+void bch_insert_data(struct closure *cl)
+{
+	struct btree_op *op = container_of(cl, struct btree_op, cl);
+
+	bch_keylist_init(&op->keys);
+	bio_get(op->cache_bio);
+	bch_insert_data_loop(cl);
+}
+
+void bch_btree_insert_async(struct closure *cl)
+{
+	struct btree_op *op = container_of(cl, struct btree_op, cl);
+	struct search *s = container_of(op, struct search, op);
+
+	if (bch_btree_insert(op, op->c)) {
+		s->error		= -ENOMEM;
+		op->insert_data_done	= true;
+	}
+
+	if (op->insert_data_done) {
+		bch_keylist_free(&op->keys);
+		closure_return(cl);
+	} else
+		continue_at(cl, bch_insert_data_loop, bcache_wq);
+}
+
+/* Common code for the make_request functions */
+
+static void request_endio(struct bio *bio, int error)
+{
+	struct closure *cl = bio->bi_private;
+
+	if (error) {
+		struct search *s = container_of(cl, struct search, cl);
+		s->error = error;
+		/* Only cache read errors are recoverable */
+		s->recoverable = false;
+	}
+
+	bio_put(bio);
+	closure_put(cl);
+}
+
+void bch_cache_read_endio(struct bio *bio, int error)
+{
+	struct bbio *b = container_of(bio, struct bbio, bio);
+	struct closure *cl = bio->bi_private;
+	struct search *s = container_of(cl, struct search, cl);
+
+	/*
+	 * If the bucket was reused while our bio was in flight, we might have
+	 * read the wrong data. Set s->error but not error so it doesn't get
+	 * counted against the cache device, but we'll still reread the data
+	 * from the backing device.
+	 */
+
+	if (error)
+		s->error = error;
+	else if (ptr_stale(s->op.c, &b->key, 0)) {
+		atomic_long_inc(&s->op.c->cache_read_races);
+		s->error = -EINTR;
+	}
+
+	bch_bbio_endio(s->op.c, bio, error, "reading from cache");
+}
+
+static void bio_complete(struct search *s)
+{
+	if (s->orig_bio) {
+		int cpu, rw = bio_data_dir(s->orig_bio);
+		unsigned long duration = jiffies - s->start_time;
+
+		cpu = part_stat_lock();
+		part_round_stats(cpu, &s->d->disk->part0);
+		part_stat_add(cpu, &s->d->disk->part0, ticks[rw], duration);
+		part_stat_unlock();
+
+		trace_bcache_request_end(s, s->orig_bio);
+		bio_endio(s->orig_bio, s->error);
+		s->orig_bio = NULL;
+	}
+}
+
+static void do_bio_hook(struct search *s)
+{
+	struct bio *bio = &s->bio.bio;
+	memcpy(bio, s->orig_bio, sizeof(struct bio));
+
+	bio->bi_end_io		= request_endio;
+	bio->bi_private		= &s->cl;
+	atomic_set(&bio->bi_cnt, 3);
+}
+
+static void search_free(struct closure *cl)
+{
+	struct search *s = container_of(cl, struct search, cl);
+	bio_complete(s);
+
+	if (s->op.cache_bio)
+		bio_put(s->op.cache_bio);
+
+	if (s->unaligned_bvec)
+		mempool_free(s->bio.bio.bi_io_vec, s->d->unaligned_bvec);
+
+	closure_debug_destroy(cl);
+	mempool_free(s, s->d->c->search);
+}
+
+static struct search *search_alloc(struct bio *bio, struct bcache_device *d)
+{
+	struct bio_vec *bv;
+	struct search *s = mempool_alloc(d->c->search, GFP_NOIO);
+	memset(s, 0, offsetof(struct search, op.keys));
+
+	__closure_init(&s->cl, NULL);
+
+	s->op.inode		= d->id;
+	s->op.c			= d->c;
+	s->d			= d;
+	s->op.lock		= -1;
+	s->task			= current;
+	s->orig_bio		= bio;
+	s->write		= (bio->bi_rw & REQ_WRITE) != 0;
+	s->op.flush_journal	= (bio->bi_rw & REQ_FLUSH) != 0;
+	s->op.skip		= (bio->bi_rw & REQ_DISCARD) != 0;
+	s->recoverable		= 1;
+	s->start_time		= jiffies;
+	do_bio_hook(s);
+
+	if (bio->bi_size != bio_segments(bio) * PAGE_SIZE) {
+		bv = mempool_alloc(d->unaligned_bvec, GFP_NOIO);
+		memcpy(bv, bio_iovec(bio),
+		       sizeof(struct bio_vec) * bio_segments(bio));
+
+		s->bio.bio.bi_io_vec	= bv;
+		s->unaligned_bvec	= 1;
+	}
+
+	return s;
+}
+
+static void btree_read_async(struct closure *cl)
+{
+	struct btree_op *op = container_of(cl, struct btree_op, cl);
+
+	int ret = btree_root(search_recurse, op->c, op);
+
+	if (ret == -EAGAIN)
+		continue_at(cl, btree_read_async, bcache_wq);
+
+	closure_return(cl);
+}
+
+/* Cached devices */
+
+static void cached_dev_bio_complete(struct closure *cl)
+{
+	struct search *s = container_of(cl, struct search, cl);
+	struct cached_dev *dc = container_of(s->d, struct cached_dev, disk);
+
+	search_free(cl);
+	cached_dev_put(dc);
+}
+
+/* Process reads */
+
+static void cached_dev_read_complete(struct closure *cl)
+{
+	struct search *s = container_of(cl, struct search, cl);
+
+	if (s->op.insert_collision)
+		bch_mark_cache_miss_collision(s);
+
+	if (s->op.cache_bio) {
+		int i;
+		struct bio_vec *bv;
+
+		__bio_for_each_segment(bv, s->op.cache_bio, i, 0)
+			__free_page(bv->bv_page);
+	}
+
+	cached_dev_bio_complete(cl);
+}
+
+static void request_read_error(struct closure *cl)
+{
+	struct search *s = container_of(cl, struct search, cl);
+	struct bio_vec *bv;
+	int i;
+
+	if (s->recoverable) {
+		/* The cache read failed, but we can retry from the backing
+		 * device.
+		 */
+		pr_debug("recovering at sector %llu",
+			 (uint64_t) s->orig_bio->bi_sector);
+
+		s->error = 0;
+		bv = s->bio.bio.bi_io_vec;
+		do_bio_hook(s);
+		s->bio.bio.bi_io_vec = bv;
+
+		if (!s->unaligned_bvec)
+			bio_for_each_segment(bv, s->orig_bio, i)
+				bv->bv_offset = 0, bv->bv_len = PAGE_SIZE;
+		else
+			memcpy(s->bio.bio.bi_io_vec,
+			       bio_iovec(s->orig_bio),
+			       sizeof(struct bio_vec) *
+			       bio_segments(s->orig_bio));
+
+		/* XXX: invalidate cache */
+
+		trace_bcache_read_retry(&s->bio.bio);
+		closure_bio_submit(&s->bio.bio, &s->cl, s->d);
+	}
+
+	continue_at(cl, cached_dev_read_complete, NULL);
+}
+
+static void request_read_done(struct closure *cl)
+{
+	struct search *s = container_of(cl, struct search, cl);
+	struct cached_dev *dc = container_of(s->d, struct cached_dev, disk);
+
+	/*
+	 * s->cache_bio != NULL implies that we had a cache miss; cache_bio now
+	 * contains data ready to be inserted into the cache.
+	 *
+	 * First, we copy the data we just read from cache_bio's bounce buffers
+	 * to the buffers the original bio pointed to:
+	 */
+
+	if (s->op.cache_bio) {
+		struct bio_vec *src, *dst;
+		unsigned src_offset, dst_offset, bytes;
+		void *dst_ptr;
+
+		bio_reset(s->op.cache_bio);
+		s->op.cache_bio->bi_sector	= s->cache_miss->bi_sector;
+		s->op.cache_bio->bi_bdev	= s->cache_miss->bi_bdev;
+		s->op.cache_bio->bi_size	= s->cache_bio_sectors << 9;
+		bch_bio_map(s->op.cache_bio, NULL);
+
+		src = bio_iovec(s->op.cache_bio);
+		dst = bio_iovec(s->cache_miss);
+		src_offset = src->bv_offset;
+		dst_offset = dst->bv_offset;
+		dst_ptr = kmap(dst->bv_page);
+
+		while (1) {
+			if (dst_offset == dst->bv_offset + dst->bv_len) {
+				kunmap(dst->bv_page);
+				dst++;
+				if (dst == bio_iovec_idx(s->cache_miss,
+						s->cache_miss->bi_vcnt))
+					break;
+
+				dst_offset = dst->bv_offset;
+				dst_ptr = kmap(dst->bv_page);
+			}
+
+			if (src_offset == src->bv_offset + src->bv_len) {
+				src++;
+				if (src == bio_iovec_idx(s->op.cache_bio,
+						 s->op.cache_bio->bi_vcnt))
+					BUG();
+
+				src_offset = src->bv_offset;
+			}
+
+			bytes = min(dst->bv_offset + dst->bv_len - dst_offset,
+				    src->bv_offset + src->bv_len - src_offset);
+
+			memcpy(dst_ptr + dst_offset,
+			       page_address(src->bv_page) + src_offset,
+			       bytes);
+
+			src_offset	+= bytes;
+			dst_offset	+= bytes;
+		}
+
+		bio_put(s->cache_miss);
+		s->cache_miss = NULL;
+	}
+
+	if (verify(dc, &s->bio.bio) && s->recoverable)
+		bch_data_verify(s);
+
+	bio_complete(s);
+
+	if (s->op.cache_bio &&
+	    !test_bit(CACHE_SET_STOPPING, &s->op.c->flags)) {
+		s->op.type = BTREE_REPLACE;
+		closure_call(&s->op.cl, bch_insert_data, NULL, cl);
+	}
+
+	continue_at(cl, cached_dev_read_complete, NULL);
+}
+
+static void request_read_done_bh(struct closure *cl)
+{
+	struct search *s = container_of(cl, struct search, cl);
+	struct cached_dev *dc = container_of(s->d, struct cached_dev, disk);
+
+	bch_mark_cache_accounting(s, !s->cache_miss, s->op.skip);
+
+	if (s->error)
+		continue_at_nobarrier(cl, request_read_error, bcache_wq);
+	else if (s->op.cache_bio || verify(dc, &s->bio.bio))
+		continue_at_nobarrier(cl, request_read_done, bcache_wq);
+	else
+		continue_at_nobarrier(cl, cached_dev_read_complete, NULL);
+}
+
+static int cached_dev_cache_miss(struct btree *b, struct search *s,
+				 struct bio *bio, unsigned sectors)
+{
+	int ret = 0;
+	unsigned reada;
+	struct cached_dev *dc = container_of(s->d, struct cached_dev, disk);
+	struct bio *miss;
+
+	miss = bch_bio_split(bio, sectors, GFP_NOIO, s->d->bio_split);
+	if (!miss)
+		return -EAGAIN;
+
+	if (miss == bio)
+		s->op.lookup_done = true;
+
+	miss->bi_end_io		= request_endio;
+	miss->bi_private	= &s->cl;
+
+	if (s->cache_miss || s->op.skip)
+		goto out_submit;
+
+	if (miss != bio ||
+	    (bio->bi_rw & REQ_RAHEAD) ||
+	    (bio->bi_rw & REQ_META) ||
+	    s->op.c->gc_stats.in_use >= CUTOFF_CACHE_READA)
+		reada = 0;
+	else {
+		reada = min(dc->readahead >> 9,
+			    sectors - bio_sectors(miss));
+
+		if (bio_end(miss) + reada > bdev_sectors(miss->bi_bdev))
+			reada = bdev_sectors(miss->bi_bdev) - bio_end(miss);
+	}
+
+	s->cache_bio_sectors = bio_sectors(miss) + reada;
+	s->op.cache_bio = bio_alloc_bioset(GFP_NOWAIT,
+			DIV_ROUND_UP(s->cache_bio_sectors, PAGE_SECTORS),
+			dc->disk.bio_split);
+
+	if (!s->op.cache_bio)
+		goto out_submit;
+
+	s->op.cache_bio->bi_sector	= miss->bi_sector;
+	s->op.cache_bio->bi_bdev	= miss->bi_bdev;
+	s->op.cache_bio->bi_size	= s->cache_bio_sectors << 9;
+
+	s->op.cache_bio->bi_end_io	= request_endio;
+	s->op.cache_bio->bi_private	= &s->cl;
+
+	/* btree_search_recurse()'s btree iterator is no good anymore */
+	ret = -EINTR;
+	if (!bch_btree_insert_check_key(b, &s->op, s->op.cache_bio))
+		goto out_put;
+
+	bch_bio_map(s->op.cache_bio, NULL);
+	if (bch_bio_alloc_pages(s->op.cache_bio, __GFP_NOWARN|GFP_NOIO))
+		goto out_put;
+
+	s->cache_miss = miss;
+	bio_get(s->op.cache_bio);
+
+	trace_bcache_cache_miss(s->orig_bio);
+	closure_bio_submit(s->op.cache_bio, &s->cl, s->d);
+
+	return ret;
+out_put:
+	bio_put(s->op.cache_bio);
+	s->op.cache_bio = NULL;
+out_submit:
+	closure_bio_submit(miss, &s->cl, s->d);
+	return ret;
+}
+
+static void request_read(struct cached_dev *dc, struct search *s)
+{
+	struct closure *cl = &s->cl;
+
+	check_should_skip(dc, s);
+	closure_call(&s->op.cl, btree_read_async, NULL, cl);
+
+	continue_at(cl, request_read_done_bh, NULL);
+}
+
+/* Process writes */
+
+static void cached_dev_write_complete(struct closure *cl)
+{
+	struct search *s = container_of(cl, struct search, cl);
+	struct cached_dev *dc = container_of(s->d, struct cached_dev, disk);
+
+	up_read_non_owner(&dc->writeback_lock);
+	cached_dev_bio_complete(cl);
+}
+
+static bool should_writeback(struct cached_dev *dc, struct bio *bio)
+{
+	unsigned threshold = (bio->bi_rw & REQ_SYNC)
+		? CUTOFF_WRITEBACK_SYNC
+		: CUTOFF_WRITEBACK;
+
+	return !atomic_read(&dc->disk.detaching) &&
+		cache_mode(dc, bio) == CACHE_MODE_WRITEBACK &&
+		dc->disk.c->gc_stats.in_use < threshold;
+}
+
+static void request_write(struct cached_dev *dc, struct search *s)
+{
+	struct closure *cl = &s->cl;
+	struct bio *bio = &s->bio.bio;
+	struct bkey start, end;
+	start = KEY(dc->disk.id, bio->bi_sector, 0);
+	end = KEY(dc->disk.id, bio_end(bio), 0);
+
+	bch_keybuf_check_overlapping(&s->op.c->moving_gc_keys, &start, &end);
+
+	check_should_skip(dc, s);
+	down_read_non_owner(&dc->writeback_lock);
+
+	if (bch_keybuf_check_overlapping(&dc->writeback_keys, &start, &end)) {
+		s->op.skip	= false;
+		s->writeback	= true;
+	}
+
+	if (bio->bi_rw & REQ_DISCARD)
+		goto skip;
+
+	if (s->op.skip)
+		goto skip;
+
+	if (should_writeback(dc, s->orig_bio))
+		s->writeback = true;
+
+	if (!s->writeback) {
+		s->op.cache_bio = bio_clone_bioset(bio, GFP_NOIO,
+						   dc->disk.bio_split);
+
+		trace_bcache_writethrough(s->orig_bio);
+		closure_bio_submit(bio, cl, s->d);
+	} else {
+		s->op.cache_bio = bio;
+		trace_bcache_writeback(s->orig_bio);
+		bch_writeback_add(dc, bio_sectors(bio));
+	}
+out:
+	closure_call(&s->op.cl, bch_insert_data, NULL, cl);
+	continue_at(cl, cached_dev_write_complete, NULL);
+skip:
+	s->op.skip = true;
+	s->op.cache_bio = s->orig_bio;
+	bio_get(s->op.cache_bio);
+	trace_bcache_write_skip(s->orig_bio);
+
+	if ((bio->bi_rw & REQ_DISCARD) &&
+	    !blk_queue_discard(bdev_get_queue(dc->bdev)))
+		goto out;
+
+	closure_bio_submit(bio, cl, s->d);
+	goto out;
+}
+
+static void request_nodata(struct cached_dev *dc, struct search *s)
+{
+	struct closure *cl = &s->cl;
+	struct bio *bio = &s->bio.bio;
+
+	if (bio->bi_rw & REQ_DISCARD) {
+		request_write(dc, s);
+		return;
+	}
+
+	if (s->op.flush_journal)
+		bch_journal_meta(s->op.c, cl);
+
+	closure_bio_submit(bio, cl, s->d);
+
+	continue_at(cl, cached_dev_bio_complete, NULL);
+}
+
+/* Cached devices - read & write stuff */
+
+int bch_get_congested(struct cache_set *c)
+{
+	int i;
+
+	if (!c->congested_read_threshold_us &&
+	    !c->congested_write_threshold_us)
+		return 0;
+
+	i = (local_clock_us() - c->congested_last_us) / 1024;
+	if (i < 0)
+		return 0;
+
+	i += atomic_read(&c->congested);
+	if (i >= 0)
+		return 0;
+
+	i += CONGESTED_MAX;
+
+	return i <= 0 ? 1 : fract_exp_two(i, 6);
+}
+
+static void add_sequential(struct task_struct *t)
+{
+	ewma_add(t->sequential_io_avg,
+		 t->sequential_io, 8, 0);
+
+	t->sequential_io = 0;
+}
+
+static struct hlist_head *iohash(struct cached_dev *dc, uint64_t k)
+{
+	return &dc->io_hash[hash_64(k, RECENT_IO_BITS)];
+}
+
+static void check_should_skip(struct cached_dev *dc, struct search *s)
+{
+	struct cache_set *c = s->op.c;
+	struct bio *bio = &s->bio.bio;
+
+	long rand;
+	int cutoff = bch_get_congested(c);
+	unsigned mode = cache_mode(dc, bio);
+
+	if (atomic_read(&dc->disk.detaching) ||
+	    c->gc_stats.in_use > CUTOFF_CACHE_ADD ||
+	    (bio->bi_rw & REQ_DISCARD))
+		goto skip;
+
+	if (mode == CACHE_MODE_NONE ||
+	    (mode == CACHE_MODE_WRITEAROUND &&
+	     (bio->bi_rw & REQ_WRITE)))
+		goto skip;
+
+	if (bio->bi_sector   & (c->sb.block_size - 1) ||
+	    bio_sectors(bio) & (c->sb.block_size - 1)) {
+		pr_debug("skipping unaligned io");
+		goto skip;
+	}
+
+	if (!cutoff) {
+		cutoff = dc->sequential_cutoff >> 9;
+
+		if (!cutoff)
+			goto rescale;
+
+		if (mode == CACHE_MODE_WRITEBACK &&
+		    (bio->bi_rw & REQ_WRITE) &&
+		    (bio->bi_rw & REQ_SYNC))
+			goto rescale;
+	}
+
+	if (dc->sequential_merge) {
+		struct io *i;
+
+		spin_lock(&dc->io_lock);
+
+		hlist_for_each_entry(i, iohash(dc, bio->bi_sector), hash)
+			if (i->last == bio->bi_sector &&
+			    time_before(jiffies, i->jiffies))
+				goto found;
+
+		i = list_first_entry(&dc->io_lru, struct io, lru);
+
+		add_sequential(s->task);
+		i->sequential = 0;
+found:
+		if (i->sequential + bio->bi_size > i->sequential)
+			i->sequential	+= bio->bi_size;
+
+		i->last			 = bio_end(bio);
+		i->jiffies		 = jiffies + msecs_to_jiffies(5000);
+		s->task->sequential_io	 = i->sequential;
+
+		hlist_del(&i->hash);
+		hlist_add_head(&i->hash, iohash(dc, i->last));
+		list_move_tail(&i->lru, &dc->io_lru);
+
+		spin_unlock(&dc->io_lock);
+	} else {
+		s->task->sequential_io = bio->bi_size;
+
+		add_sequential(s->task);
+	}
+
+	rand = get_random_int();
+	cutoff -= bitmap_weight(&rand, BITS_PER_LONG);
+
+	if (cutoff <= (int) (max(s->task->sequential_io,
+				 s->task->sequential_io_avg) >> 9))
+		goto skip;
+
+rescale:
+	bch_rescale_priorities(c, bio_sectors(bio));
+	return;
+skip:
+	bch_mark_sectors_bypassed(s, bio_sectors(bio));
+	s->op.skip = true;
+}
+
+static void cached_dev_make_request(struct request_queue *q, struct bio *bio)
+{
+	struct search *s;
+	struct bcache_device *d = bio->bi_bdev->bd_disk->private_data;
+	struct cached_dev *dc = container_of(d, struct cached_dev, disk);
+	int cpu, rw = bio_data_dir(bio);
+
+	cpu = part_stat_lock();
+	part_stat_inc(cpu, &d->disk->part0, ios[rw]);
+	part_stat_add(cpu, &d->disk->part0, sectors[rw], bio_sectors(bio));
+	part_stat_unlock();
+
+	bio->bi_bdev = dc->bdev;
+	bio->bi_sector += dc->sb.data_offset;
+
+	if (cached_dev_get(dc)) {
+		s = search_alloc(bio, d);
+		trace_bcache_request_start(s, bio);
+
+		if (!bio_has_data(bio))
+			request_nodata(dc, s);
+		else if (rw)
+			request_write(dc, s);
+		else
+			request_read(dc, s);
+	} else {
+		if ((bio->bi_rw & REQ_DISCARD) &&
+		    !blk_queue_discard(bdev_get_queue(dc->bdev)))
+			bio_endio(bio, 0);
+		else
+			bch_generic_make_request(bio, &d->bio_split_hook);
+	}
+}
+
+static int cached_dev_ioctl(struct bcache_device *d, fmode_t mode,
+			    unsigned int cmd, unsigned long arg)
+{
+	struct cached_dev *dc = container_of(d, struct cached_dev, disk);
+	return __blkdev_driver_ioctl(dc->bdev, mode, cmd, arg);
+}
+
+static int cached_dev_congested(void *data, int bits)
+{
+	struct bcache_device *d = data;
+	struct cached_dev *dc = container_of(d, struct cached_dev, disk);
+	struct request_queue *q = bdev_get_queue(dc->bdev);
+	int ret = 0;
+
+	if (bdi_congested(&q->backing_dev_info, bits))
+		return 1;
+
+	if (cached_dev_get(dc)) {
+		unsigned i;
+		struct cache *ca;
+
+		for_each_cache(ca, d->c, i) {
+			q = bdev_get_queue(ca->bdev);
+			ret |= bdi_congested(&q->backing_dev_info, bits);
+		}
+
+		cached_dev_put(dc);
+	}
+
+	return ret;
+}
+
+void bch_cached_dev_request_init(struct cached_dev *dc)
+{
+	struct gendisk *g = dc->disk.disk;
+
+	g->queue->make_request_fn		= cached_dev_make_request;
+	g->queue->backing_dev_info.congested_fn = cached_dev_congested;
+	dc->disk.cache_miss			= cached_dev_cache_miss;
+	dc->disk.ioctl				= cached_dev_ioctl;
+}
+
+/* Flash backed devices */
+
+static int flash_dev_cache_miss(struct btree *b, struct search *s,
+				struct bio *bio, unsigned sectors)
+{
+	/* Zero fill bio */
+
+	while (bio->bi_idx != bio->bi_vcnt) {
+		struct bio_vec *bv = bio_iovec(bio);
+		unsigned j = min(bv->bv_len >> 9, sectors);
+
+		void *p = kmap(bv->bv_page);
+		memset(p + bv->bv_offset, 0, j << 9);
+		kunmap(bv->bv_page);
+
+		bv->bv_len	-= j << 9;
+		bv->bv_offset	+= j << 9;
+
+		if (bv->bv_len)
+			return 0;
+
+		bio->bi_sector	+= j;
+		bio->bi_size	-= j << 9;
+
+		bio->bi_idx++;
+		sectors		-= j;
+	}
+
+	s->op.lookup_done = true;
+
+	return 0;
+}
+
+static void flash_dev_make_request(struct request_queue *q, struct bio *bio)
+{
+	struct search *s;
+	struct closure *cl;
+	struct bcache_device *d = bio->bi_bdev->bd_disk->private_data;
+	int cpu, rw = bio_data_dir(bio);
+
+	cpu = part_stat_lock();
+	part_stat_inc(cpu, &d->disk->part0, ios[rw]);
+	part_stat_add(cpu, &d->disk->part0, sectors[rw], bio_sectors(bio));
+	part_stat_unlock();
+
+	s = search_alloc(bio, d);
+	cl = &s->cl;
+	bio = &s->bio.bio;
+
+	trace_bcache_request_start(s, bio);
+
+	if (bio_has_data(bio) && !rw) {
+		closure_call(&s->op.cl, btree_read_async, NULL, cl);
+	} else if (bio_has_data(bio) || s->op.skip) {
+		bch_keybuf_check_overlapping(&s->op.c->moving_gc_keys,
+					     &KEY(d->id, bio->bi_sector, 0),
+					     &KEY(d->id, bio_end(bio), 0));
+
+		s->writeback	= true;
+		s->op.cache_bio	= bio;
+
+		closure_call(&s->op.cl, bch_insert_data, NULL, cl);
+	} else {
+		/* No data - probably a cache flush */
+		if (s->op.flush_journal)
+			bch_journal_meta(s->op.c, cl);
+	}
+
+	continue_at(cl, search_free, NULL);
+}
+
+static int flash_dev_ioctl(struct bcache_device *d, fmode_t mode,
+			   unsigned int cmd, unsigned long arg)
+{
+	return -ENOTTY;
+}
+
+static int flash_dev_congested(void *data, int bits)
+{
+	struct bcache_device *d = data;
+	struct request_queue *q;
+	struct cache *ca;
+	unsigned i;
+	int ret = 0;
+
+	for_each_cache(ca, d->c, i) {
+		q = bdev_get_queue(ca->bdev);
+		ret |= bdi_congested(&q->backing_dev_info, bits);
+	}
+
+	return ret;
+}
+
+void bch_flash_dev_request_init(struct bcache_device *d)
+{
+	struct gendisk *g = d->disk;
+
+	g->queue->make_request_fn		= flash_dev_make_request;
+	g->queue->backing_dev_info.congested_fn = flash_dev_congested;
+	d->cache_miss				= flash_dev_cache_miss;
+	d->ioctl				= flash_dev_ioctl;
+}
+
+void bch_request_exit(void)
+{
+#ifdef CONFIG_CGROUP_BCACHE
+	cgroup_unload_subsys(&bcache_subsys);
+#endif
+	if (bch_search_cache)
+		kmem_cache_destroy(bch_search_cache);
+}
+
+int __init bch_request_init(void)
+{
+	bch_search_cache = KMEM_CACHE(search, 0);
+	if (!bch_search_cache)
+		return -ENOMEM;
+
+#ifdef CONFIG_CGROUP_BCACHE
+	cgroup_load_subsys(&bcache_subsys);
+	init_bch_cgroup(&bcache_default_cgroup);
+
+	cgroup_add_cftypes(&bcache_subsys, bch_files);
+#endif
+	return 0;
+}
diff --git a/drivers/md/bcache/request.h b/drivers/md/bcache/request.h
new file mode 100644
index 000000000000..254d9ab5707c
--- /dev/null
+++ b/drivers/md/bcache/request.h
@@ -0,0 +1,62 @@
+#ifndef _BCACHE_REQUEST_H_
+#define _BCACHE_REQUEST_H_
+
+#include <linux/cgroup.h>
+
+struct search {
+	/* Stack frame for bio_complete */
+	struct closure		cl;
+
+	struct bcache_device	*d;
+	struct task_struct	*task;
+
+	struct bbio		bio;
+	struct bio		*orig_bio;
+	struct bio		*cache_miss;
+	unsigned		cache_bio_sectors;
+
+	unsigned		recoverable:1;
+	unsigned		unaligned_bvec:1;
+
+	unsigned		write:1;
+	unsigned		writeback:1;
+
+	/* IO error returned to s->bio */
+	short			error;
+	unsigned long		start_time;
+
+	/* Anything past op->keys won't get zeroed in do_bio_hook */
+	struct btree_op		op;
+};
+
+void bch_cache_read_endio(struct bio *, int);
+int bch_get_congested(struct cache_set *);
+void bch_insert_data(struct closure *cl);
+void bch_btree_insert_async(struct closure *);
+void bch_cache_read_endio(struct bio *, int);
+
+void bch_open_buckets_free(struct cache_set *);
+int bch_open_buckets_alloc(struct cache_set *);
+
+void bch_cached_dev_request_init(struct cached_dev *dc);
+void bch_flash_dev_request_init(struct bcache_device *d);
+
+extern struct kmem_cache *bch_search_cache, *bch_passthrough_cache;
+
+struct bch_cgroup {
+#ifdef CONFIG_CGROUP_BCACHE
+	struct cgroup_subsys_state	css;
+#endif
+	/*
+	 * We subtract one from the index into bch_cache_modes[], so that
+	 * default == -1; this makes it so the rest match up with d->cache_mode,
+	 * and we use d->cache_mode if cgrp->cache_mode < 0
+	 */
+	short				cache_mode;
+	bool				verify;
+	struct cache_stat_collector	stats;
+};
+
+struct bch_cgroup *bch_bio_to_cgroup(struct bio *bio);
+
+#endif /* _BCACHE_REQUEST_H_ */
diff --git a/drivers/md/bcache/stats.c b/drivers/md/bcache/stats.c
new file mode 100644
index 000000000000..64e679449c2a
--- /dev/null
+++ b/drivers/md/bcache/stats.c
@@ -0,0 +1,246 @@
+/*
+ * bcache stats code
+ *
+ * Copyright 2012 Google, Inc.
+ */
+
+#include "bcache.h"
+#include "stats.h"
+#include "btree.h"
+#include "request.h"
+#include "sysfs.h"
+
+/*
+ * We keep absolute totals of various statistics, and addionally a set of three
+ * rolling averages.
+ *
+ * Every so often, a timer goes off and rescales the rolling averages.
+ * accounting_rescale[] is how many times the timer has to go off before we
+ * rescale each set of numbers; that gets us half lives of 5 minutes, one hour,
+ * and one day.
+ *
+ * accounting_delay is how often the timer goes off - 22 times in 5 minutes,
+ * and accounting_weight is what we use to rescale:
+ *
+ * pow(31 / 32, 22) ~= 1/2
+ *
+ * So that we don't have to increment each set of numbers every time we (say)
+ * get a cache hit, we increment a single atomic_t in acc->collector, and when
+ * the rescale function runs it resets the atomic counter to 0 and adds its
+ * old value to each of the exported numbers.
+ *
+ * To reduce rounding error, the numbers in struct cache_stats are all
+ * stored left shifted by 16, and scaled back in the sysfs show() function.
+ */
+
+static const unsigned DAY_RESCALE		= 288;
+static const unsigned HOUR_RESCALE		= 12;
+static const unsigned FIVE_MINUTE_RESCALE	= 1;
+static const unsigned accounting_delay		= (HZ * 300) / 22;
+static const unsigned accounting_weight		= 32;
+
+/* sysfs reading/writing */
+
+read_attribute(cache_hits);
+read_attribute(cache_misses);
+read_attribute(cache_bypass_hits);
+read_attribute(cache_bypass_misses);
+read_attribute(cache_hit_ratio);
+read_attribute(cache_readaheads);
+read_attribute(cache_miss_collisions);
+read_attribute(bypassed);
+
+SHOW(bch_stats)
+{
+	struct cache_stats *s =
+		container_of(kobj, struct cache_stats, kobj);
+#define var(stat)		(s->stat >> 16)
+	var_print(cache_hits);
+	var_print(cache_misses);
+	var_print(cache_bypass_hits);
+	var_print(cache_bypass_misses);
+
+	sysfs_print(cache_hit_ratio,
+		    DIV_SAFE(var(cache_hits) * 100,
+			     var(cache_hits) + var(cache_misses)));
+
+	var_print(cache_readaheads);
+	var_print(cache_miss_collisions);
+	sysfs_hprint(bypassed,	var(sectors_bypassed) << 9);
+#undef var
+	return 0;
+}
+
+STORE(bch_stats)
+{
+	return size;
+}
+
+static void bch_stats_release(struct kobject *k)
+{
+}
+
+static struct attribute *bch_stats_files[] = {
+	&sysfs_cache_hits,
+	&sysfs_cache_misses,
+	&sysfs_cache_bypass_hits,
+	&sysfs_cache_bypass_misses,
+	&sysfs_cache_hit_ratio,
+	&sysfs_cache_readaheads,
+	&sysfs_cache_miss_collisions,
+	&sysfs_bypassed,
+	NULL
+};
+static KTYPE(bch_stats);
+
+static void scale_accounting(unsigned long data);
+
+void bch_cache_accounting_init(struct cache_accounting *acc,
+			       struct closure *parent)
+{
+	kobject_init(&acc->total.kobj,		&bch_stats_ktype);
+	kobject_init(&acc->five_minute.kobj,	&bch_stats_ktype);
+	kobject_init(&acc->hour.kobj,		&bch_stats_ktype);
+	kobject_init(&acc->day.kobj,		&bch_stats_ktype);
+
+	closure_init(&acc->cl, parent);
+	init_timer(&acc->timer);
+	acc->timer.expires	= jiffies + accounting_delay;
+	acc->timer.data		= (unsigned long) acc;
+	acc->timer.function	= scale_accounting;
+	add_timer(&acc->timer);
+}
+
+int bch_cache_accounting_add_kobjs(struct cache_accounting *acc,
+				   struct kobject *parent)
+{
+	int ret = kobject_add(&acc->total.kobj, parent,
+			      "stats_total");
+	ret = ret ?: kobject_add(&acc->five_minute.kobj, parent,
+				 "stats_five_minute");
+	ret = ret ?: kobject_add(&acc->hour.kobj, parent,
+				 "stats_hour");
+	ret = ret ?: kobject_add(&acc->day.kobj, parent,
+				 "stats_day");
+	return ret;
+}
+
+void bch_cache_accounting_clear(struct cache_accounting *acc)
+{
+	memset(&acc->total.cache_hits,
+	       0,
+	       sizeof(unsigned long) * 7);
+}
+
+void bch_cache_accounting_destroy(struct cache_accounting *acc)
+{
+	kobject_put(&acc->total.kobj);
+	kobject_put(&acc->five_minute.kobj);
+	kobject_put(&acc->hour.kobj);
+	kobject_put(&acc->day.kobj);
+
+	atomic_set(&acc->closing, 1);
+	if (del_timer_sync(&acc->timer))
+		closure_return(&acc->cl);
+}
+
+/* EWMA scaling */
+
+static void scale_stat(unsigned long *stat)
+{
+	*stat =  ewma_add(*stat, 0, accounting_weight, 0);
+}
+
+static void scale_stats(struct cache_stats *stats, unsigned long rescale_at)
+{
+	if (++stats->rescale == rescale_at) {
+		stats->rescale = 0;
+		scale_stat(&stats->cache_hits);
+		scale_stat(&stats->cache_misses);
+		scale_stat(&stats->cache_bypass_hits);
+		scale_stat(&stats->cache_bypass_misses);
+		scale_stat(&stats->cache_readaheads);
+		scale_stat(&stats->cache_miss_collisions);
+		scale_stat(&stats->sectors_bypassed);
+	}
+}
+
+static void scale_accounting(unsigned long data)
+{
+	struct cache_accounting *acc = (struct cache_accounting *) data;
+
+#define move_stat(name) do {						\
+	unsigned t = atomic_xchg(&acc->collector.name, 0);		\
+	t <<= 16;							\
+	acc->five_minute.name += t;					\
+	acc->hour.name += t;						\
+	acc->day.name += t;						\
+	acc->total.name += t;						\
+} while (0)
+
+	move_stat(cache_hits);
+	move_stat(cache_misses);
+	move_stat(cache_bypass_hits);
+	move_stat(cache_bypass_misses);
+	move_stat(cache_readaheads);
+	move_stat(cache_miss_collisions);
+	move_stat(sectors_bypassed);
+
+	scale_stats(&acc->total, 0);
+	scale_stats(&acc->day, DAY_RESCALE);
+	scale_stats(&acc->hour, HOUR_RESCALE);
+	scale_stats(&acc->five_minute, FIVE_MINUTE_RESCALE);
+
+	acc->timer.expires += accounting_delay;
+
+	if (!atomic_read(&acc->closing))
+		add_timer(&acc->timer);
+	else
+		closure_return(&acc->cl);
+}
+
+static void mark_cache_stats(struct cache_stat_collector *stats,
+			     bool hit, bool bypass)
+{
+	if (!bypass)
+		if (hit)
+			atomic_inc(&stats->cache_hits);
+		else
+			atomic_inc(&stats->cache_misses);
+	else
+		if (hit)
+			atomic_inc(&stats->cache_bypass_hits);
+		else
+			atomic_inc(&stats->cache_bypass_misses);
+}
+
+void bch_mark_cache_accounting(struct search *s, bool hit, bool bypass)
+{
+	struct cached_dev *dc = container_of(s->d, struct cached_dev, disk);
+	mark_cache_stats(&dc->accounting.collector, hit, bypass);
+	mark_cache_stats(&s->op.c->accounting.collector, hit, bypass);
+#ifdef CONFIG_CGROUP_BCACHE
+	mark_cache_stats(&(bch_bio_to_cgroup(s->orig_bio)->stats), hit, bypass);
+#endif
+}
+
+void bch_mark_cache_readahead(struct search *s)
+{
+	struct cached_dev *dc = container_of(s->d, struct cached_dev, disk);
+	atomic_inc(&dc->accounting.collector.cache_readaheads);
+	atomic_inc(&s->op.c->accounting.collector.cache_readaheads);
+}
+
+void bch_mark_cache_miss_collision(struct search *s)
+{
+	struct cached_dev *dc = container_of(s->d, struct cached_dev, disk);
+	atomic_inc(&dc->accounting.collector.cache_miss_collisions);
+	atomic_inc(&s->op.c->accounting.collector.cache_miss_collisions);
+}
+
+void bch_mark_sectors_bypassed(struct search *s, int sectors)
+{
+	struct cached_dev *dc = container_of(s->d, struct cached_dev, disk);
+	atomic_add(sectors, &dc->accounting.collector.sectors_bypassed);
+	atomic_add(sectors, &s->op.c->accounting.collector.sectors_bypassed);
+}
diff --git a/drivers/md/bcache/stats.h b/drivers/md/bcache/stats.h
new file mode 100644
index 000000000000..c7c7a8fd29fe
--- /dev/null
+++ b/drivers/md/bcache/stats.h
@@ -0,0 +1,58 @@
+#ifndef _BCACHE_STATS_H_
+#define _BCACHE_STATS_H_
+
+struct cache_stat_collector {
+	atomic_t cache_hits;
+	atomic_t cache_misses;
+	atomic_t cache_bypass_hits;
+	atomic_t cache_bypass_misses;
+	atomic_t cache_readaheads;
+	atomic_t cache_miss_collisions;
+	atomic_t sectors_bypassed;
+};
+
+struct cache_stats {
+	struct kobject		kobj;
+
+	unsigned long cache_hits;
+	unsigned long cache_misses;
+	unsigned long cache_bypass_hits;
+	unsigned long cache_bypass_misses;
+	unsigned long cache_readaheads;
+	unsigned long cache_miss_collisions;
+	unsigned long sectors_bypassed;
+
+	unsigned		rescale;
+};
+
+struct cache_accounting {
+	struct closure		cl;
+	struct timer_list	timer;
+	atomic_t		closing;
+
+	struct cache_stat_collector collector;
+
+	struct cache_stats total;
+	struct cache_stats five_minute;
+	struct cache_stats hour;
+	struct cache_stats day;
+};
+
+struct search;
+
+void bch_cache_accounting_init(struct cache_accounting *acc,
+			       struct closure *parent);
+
+int bch_cache_accounting_add_kobjs(struct cache_accounting *acc,
+				   struct kobject *parent);
+
+void bch_cache_accounting_clear(struct cache_accounting *acc);
+
+void bch_cache_accounting_destroy(struct cache_accounting *acc);
+
+void bch_mark_cache_accounting(struct search *s, bool hit, bool bypass);
+void bch_mark_cache_readahead(struct search *s);
+void bch_mark_cache_miss_collision(struct search *s);
+void bch_mark_sectors_bypassed(struct search *s, int sectors);
+
+#endif /* _BCACHE_STATS_H_ */
diff --git a/drivers/md/bcache/super.c b/drivers/md/bcache/super.c
new file mode 100644
index 000000000000..c8046bc4aa57
--- /dev/null
+++ b/drivers/md/bcache/super.c
@@ -0,0 +1,1987 @@
+/*
+ * bcache setup/teardown code, and some metadata io - read a superblock and
+ * figure out what to do with it.
+ *
+ * Copyright 2010, 2011 Kent Overstreet <kent.overstreet@gmail.com>
+ * Copyright 2012 Google, Inc.
+ */
+
+#include "bcache.h"
+#include "btree.h"
+#include "debug.h"
+#include "request.h"
+
+#include <linux/buffer_head.h>
+#include <linux/debugfs.h>
+#include <linux/genhd.h>
+#include <linux/module.h>
+#include <linux/random.h>
+#include <linux/reboot.h>
+#include <linux/sysfs.h>
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Kent Overstreet <kent.overstreet@gmail.com>");
+
+static const char bcache_magic[] = {
+	0xc6, 0x85, 0x73, 0xf6, 0x4e, 0x1a, 0x45, 0xca,
+	0x82, 0x65, 0xf5, 0x7f, 0x48, 0xba, 0x6d, 0x81
+};
+
+static const char invalid_uuid[] = {
+	0xa0, 0x3e, 0xf8, 0xed, 0x3e, 0xe1, 0xb8, 0x78,
+	0xc8, 0x50, 0xfc, 0x5e, 0xcb, 0x16, 0xcd, 0x99
+};
+
+/* Default is -1; we skip past it for struct cached_dev's cache mode */
+const char * const bch_cache_modes[] = {
+	"default",
+	"writethrough",
+	"writeback",
+	"writearound",
+	"none",
+	NULL
+};
+
+struct uuid_entry_v0 {
+	uint8_t		uuid[16];
+	uint8_t		label[32];
+	uint32_t	first_reg;
+	uint32_t	last_reg;
+	uint32_t	invalidated;
+	uint32_t	pad;
+};
+
+static struct kobject *bcache_kobj;
+struct mutex bch_register_lock;
+LIST_HEAD(bch_cache_sets);
+static LIST_HEAD(uncached_devices);
+
+static int bcache_major, bcache_minor;
+static wait_queue_head_t unregister_wait;
+struct workqueue_struct *bcache_wq;
+
+#define BTREE_MAX_PAGES		(256 * 1024 / PAGE_SIZE)
+
+static void bio_split_pool_free(struct bio_split_pool *p)
+{
+	if (p->bio_split_hook)
+		mempool_destroy(p->bio_split_hook);
+
+	if (p->bio_split)
+		bioset_free(p->bio_split);
+}
+
+static int bio_split_pool_init(struct bio_split_pool *p)
+{
+	p->bio_split = bioset_create(4, 0);
+	if (!p->bio_split)
+		return -ENOMEM;
+
+	p->bio_split_hook = mempool_create_kmalloc_pool(4,
+				sizeof(struct bio_split_hook));
+	if (!p->bio_split_hook)
+		return -ENOMEM;
+
+	return 0;
+}
+
+/* Superblock */
+
+static const char *read_super(struct cache_sb *sb, struct block_device *bdev,
+			      struct page **res)
+{
+	const char *err;
+	struct cache_sb *s;
+	struct buffer_head *bh = __bread(bdev, 1, SB_SIZE);
+	unsigned i;
+
+	if (!bh)
+		return "IO error";
+
+	s = (struct cache_sb *) bh->b_data;
+
+	sb->offset		= le64_to_cpu(s->offset);
+	sb->version		= le64_to_cpu(s->version);
+
+	memcpy(sb->magic,	s->magic, 16);
+	memcpy(sb->uuid,	s->uuid, 16);
+	memcpy(sb->set_uuid,	s->set_uuid, 16);
+	memcpy(sb->label,	s->label, SB_LABEL_SIZE);
+
+	sb->flags		= le64_to_cpu(s->flags);
+	sb->seq			= le64_to_cpu(s->seq);
+	sb->last_mount		= le32_to_cpu(s->last_mount);
+	sb->first_bucket	= le16_to_cpu(s->first_bucket);
+	sb->keys		= le16_to_cpu(s->keys);
+
+	for (i = 0; i < SB_JOURNAL_BUCKETS; i++)
+		sb->d[i] = le64_to_cpu(s->d[i]);
+
+	pr_debug("read sb version %llu, flags %llu, seq %llu, journal size %u",
+		 sb->version, sb->flags, sb->seq, sb->keys);
+
+	err = "Not a bcache superblock";
+	if (sb->offset != SB_SECTOR)
+		goto err;
+
+	if (memcmp(sb->magic, bcache_magic, 16))
+		goto err;
+
+	err = "Too many journal buckets";
+	if (sb->keys > SB_JOURNAL_BUCKETS)
+		goto err;
+
+	err = "Bad checksum";
+	if (s->csum != csum_set(s))
+		goto err;
+
+	err = "Bad UUID";
+	if (bch_is_zero(sb->uuid, 16))
+		goto err;
+
+	sb->block_size	= le16_to_cpu(s->block_size);
+
+	err = "Superblock block size smaller than device block size";
+	if (sb->block_size << 9 < bdev_logical_block_size(bdev))
+		goto err;
+
+	switch (sb->version) {
+	case BCACHE_SB_VERSION_BDEV:
+		sb->data_offset	= BDEV_DATA_START_DEFAULT;
+		break;
+	case BCACHE_SB_VERSION_BDEV_WITH_OFFSET:
+		sb->data_offset	= le64_to_cpu(s->data_offset);
+
+		err = "Bad data offset";
+		if (sb->data_offset < BDEV_DATA_START_DEFAULT)
+			goto err;
+
+		break;
+	case BCACHE_SB_VERSION_CDEV:
+	case BCACHE_SB_VERSION_CDEV_WITH_UUID:
+		sb->nbuckets	= le64_to_cpu(s->nbuckets);
+		sb->block_size	= le16_to_cpu(s->block_size);
+		sb->bucket_size	= le16_to_cpu(s->bucket_size);
+
+		sb->nr_in_set	= le16_to_cpu(s->nr_in_set);
+		sb->nr_this_dev	= le16_to_cpu(s->nr_this_dev);
+
+		err = "Too many buckets";
+		if (sb->nbuckets > LONG_MAX)
+			goto err;
+
+		err = "Not enough buckets";
+		if (sb->nbuckets < 1 << 7)
+			goto err;
+
+		err = "Bad block/bucket size";
+		if (!is_power_of_2(sb->block_size) ||
+		    sb->block_size > PAGE_SECTORS ||
+		    !is_power_of_2(sb->bucket_size) ||
+		    sb->bucket_size < PAGE_SECTORS)
+			goto err;
+
+		err = "Invalid superblock: device too small";
+		if (get_capacity(bdev->bd_disk) < sb->bucket_size * sb->nbuckets)
+			goto err;
+
+		err = "Bad UUID";
+		if (bch_is_zero(sb->set_uuid, 16))
+			goto err;
+
+		err = "Bad cache device number in set";
+		if (!sb->nr_in_set ||
+		    sb->nr_in_set <= sb->nr_this_dev ||
+		    sb->nr_in_set > MAX_CACHES_PER_SET)
+			goto err;
+
+		err = "Journal buckets not sequential";
+		for (i = 0; i < sb->keys; i++)
+			if (sb->d[i] != sb->first_bucket + i)
+				goto err;
+
+		err = "Too many journal buckets";
+		if (sb->first_bucket + sb->keys > sb->nbuckets)
+			goto err;
+
+		err = "Invalid superblock: first bucket comes before end of super";
+		if (sb->first_bucket * sb->bucket_size < 16)
+			goto err;
+
+		break;
+	default:
+		err = "Unsupported superblock version";
+		goto err;
+	}
+
+	sb->last_mount = get_seconds();
+	err = NULL;
+
+	get_page(bh->b_page);
+	*res = bh->b_page;
+err:
+	put_bh(bh);
+	return err;
+}
+
+static void write_bdev_super_endio(struct bio *bio, int error)
+{
+	struct cached_dev *dc = bio->bi_private;
+	/* XXX: error checking */
+
+	closure_put(&dc->sb_write.cl);
+}
+
+static void __write_super(struct cache_sb *sb, struct bio *bio)
+{
+	struct cache_sb *out = page_address(bio->bi_io_vec[0].bv_page);
+	unsigned i;
+
+	bio->bi_sector	= SB_SECTOR;
+	bio->bi_rw	= REQ_SYNC|REQ_META;
+	bio->bi_size	= SB_SIZE;
+	bch_bio_map(bio, NULL);
+
+	out->offset		= cpu_to_le64(sb->offset);
+	out->version		= cpu_to_le64(sb->version);
+
+	memcpy(out->uuid,	sb->uuid, 16);
+	memcpy(out->set_uuid,	sb->set_uuid, 16);
+	memcpy(out->label,	sb->label, SB_LABEL_SIZE);
+
+	out->flags		= cpu_to_le64(sb->flags);
+	out->seq		= cpu_to_le64(sb->seq);
+
+	out->last_mount		= cpu_to_le32(sb->last_mount);
+	out->first_bucket	= cpu_to_le16(sb->first_bucket);
+	out->keys		= cpu_to_le16(sb->keys);
+
+	for (i = 0; i < sb->keys; i++)
+		out->d[i] = cpu_to_le64(sb->d[i]);
+
+	out->csum = csum_set(out);
+
+	pr_debug("ver %llu, flags %llu, seq %llu",
+		 sb->version, sb->flags, sb->seq);
+
+	submit_bio(REQ_WRITE, bio);
+}
+
+void bch_write_bdev_super(struct cached_dev *dc, struct closure *parent)
+{
+	struct closure *cl = &dc->sb_write.cl;
+	struct bio *bio = &dc->sb_bio;
+
+	closure_lock(&dc->sb_write, parent);
+
+	bio_reset(bio);
+	bio->bi_bdev	= dc->bdev;
+	bio->bi_end_io	= write_bdev_super_endio;
+	bio->bi_private = dc;
+
+	closure_get(cl);
+	__write_super(&dc->sb, bio);
+
+	closure_return(cl);
+}
+
+static void write_super_endio(struct bio *bio, int error)
+{
+	struct cache *ca = bio->bi_private;
+
+	bch_count_io_errors(ca, error, "writing superblock");
+	closure_put(&ca->set->sb_write.cl);
+}
+
+void bcache_write_super(struct cache_set *c)
+{
+	struct closure *cl = &c->sb_write.cl;
+	struct cache *ca;
+	unsigned i;
+
+	closure_lock(&c->sb_write, &c->cl);
+
+	c->sb.seq++;
+
+	for_each_cache(ca, c, i) {
+		struct bio *bio = &ca->sb_bio;
+
+		ca->sb.version		= BCACHE_SB_VERSION_CDEV_WITH_UUID;
+		ca->sb.seq		= c->sb.seq;
+		ca->sb.last_mount	= c->sb.last_mount;
+
+		SET_CACHE_SYNC(&ca->sb, CACHE_SYNC(&c->sb));
+
+		bio_reset(bio);
+		bio->bi_bdev	= ca->bdev;
+		bio->bi_end_io	= write_super_endio;
+		bio->bi_private = ca;
+
+		closure_get(cl);
+		__write_super(&ca->sb, bio);
+	}
+
+	closure_return(cl);
+}
+
+/* UUID io */
+
+static void uuid_endio(struct bio *bio, int error)
+{
+	struct closure *cl = bio->bi_private;
+	struct cache_set *c = container_of(cl, struct cache_set, uuid_write.cl);
+
+	cache_set_err_on(error, c, "accessing uuids");
+	bch_bbio_free(bio, c);
+	closure_put(cl);
+}
+
+static void uuid_io(struct cache_set *c, unsigned long rw,
+		    struct bkey *k, struct closure *parent)
+{
+	struct closure *cl = &c->uuid_write.cl;
+	struct uuid_entry *u;
+	unsigned i;
+
+	BUG_ON(!parent);
+	closure_lock(&c->uuid_write, parent);
+
+	for (i = 0; i < KEY_PTRS(k); i++) {
+		struct bio *bio = bch_bbio_alloc(c);
+
+		bio->bi_rw	= REQ_SYNC|REQ_META|rw;
+		bio->bi_size	= KEY_SIZE(k) << 9;
+
+		bio->bi_end_io	= uuid_endio;
+		bio->bi_private = cl;
+		bch_bio_map(bio, c->uuids);
+
+		bch_submit_bbio(bio, c, k, i);
+
+		if (!(rw & WRITE))
+			break;
+	}
+
+	pr_debug("%s UUIDs at %s", rw & REQ_WRITE ? "wrote" : "read",
+		 pkey(&c->uuid_bucket));
+
+	for (u = c->uuids; u < c->uuids + c->nr_uuids; u++)
+		if (!bch_is_zero(u->uuid, 16))
+			pr_debug("Slot %zi: %pU: %s: 1st: %u last: %u inv: %u",
+				 u - c->uuids, u->uuid, u->label,
+				 u->first_reg, u->last_reg, u->invalidated);
+
+	closure_return(cl);
+}
+
+static char *uuid_read(struct cache_set *c, struct jset *j, struct closure *cl)
+{
+	struct bkey *k = &j->uuid_bucket;
+
+	if (__bch_ptr_invalid(c, 1, k))
+		return "bad uuid pointer";
+
+	bkey_copy(&c->uuid_bucket, k);
+	uuid_io(c, READ_SYNC, k, cl);
+
+	if (j->version < BCACHE_JSET_VERSION_UUIDv1) {
+		struct uuid_entry_v0	*u0 = (void *) c->uuids;
+		struct uuid_entry	*u1 = (void *) c->uuids;
+		int i;
+
+		closure_sync(cl);
+
+		/*
+		 * Since the new uuid entry is bigger than the old, we have to
+		 * convert starting at the highest memory address and work down
+		 * in order to do it in place
+		 */
+
+		for (i = c->nr_uuids - 1;
+		     i >= 0;
+		     --i) {
+			memcpy(u1[i].uuid,	u0[i].uuid, 16);
+			memcpy(u1[i].label,	u0[i].label, 32);
+
+			u1[i].first_reg		= u0[i].first_reg;
+			u1[i].last_reg		= u0[i].last_reg;
+			u1[i].invalidated	= u0[i].invalidated;
+
+			u1[i].flags	= 0;
+			u1[i].sectors	= 0;
+		}
+	}
+
+	return NULL;
+}
+
+static int __uuid_write(struct cache_set *c)
+{
+	BKEY_PADDED(key) k;
+	struct closure cl;
+	closure_init_stack(&cl);
+
+	lockdep_assert_held(&bch_register_lock);
+
+	if (bch_bucket_alloc_set(c, WATERMARK_METADATA, &k.key, 1, &cl))
+		return 1;
+
+	SET_KEY_SIZE(&k.key, c->sb.bucket_size);
+	uuid_io(c, REQ_WRITE, &k.key, &cl);
+	closure_sync(&cl);
+
+	bkey_copy(&c->uuid_bucket, &k.key);
+	__bkey_put(c, &k.key);
+	return 0;
+}
+
+int bch_uuid_write(struct cache_set *c)
+{
+	int ret = __uuid_write(c);
+
+	if (!ret)
+		bch_journal_meta(c, NULL);
+
+	return ret;
+}
+
+static struct uuid_entry *uuid_find(struct cache_set *c, const char *uuid)
+{
+	struct uuid_entry *u;
+
+	for (u = c->uuids;
+	     u < c->uuids + c->nr_uuids; u++)
+		if (!memcmp(u->uuid, uuid, 16))
+			return u;
+
+	return NULL;
+}
+
+static struct uuid_entry *uuid_find_empty(struct cache_set *c)
+{
+	static const char zero_uuid[16] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0";
+	return uuid_find(c, zero_uuid);
+}
+
+/*
+ * Bucket priorities/gens:
+ *
+ * For each bucket, we store on disk its
+   * 8 bit gen
+   * 16 bit priority
+ *
+ * See alloc.c for an explanation of the gen. The priority is used to implement
+ * lru (and in the future other) cache replacement policies; for most purposes
+ * it's just an opaque integer.
+ *
+ * The gens and the priorities don't have a whole lot to do with each other, and
+ * it's actually the gens that must be written out at specific times - it's no
+ * big deal if the priorities don't get written, if we lose them we just reuse
+ * buckets in suboptimal order.
+ *
+ * On disk they're stored in a packed array, and in as many buckets are required
+ * to fit them all. The buckets we use to store them form a list; the journal
+ * header points to the first bucket, the first bucket points to the second
+ * bucket, et cetera.
+ *
+ * This code is used by the allocation code; periodically (whenever it runs out
+ * of buckets to allocate from) the allocation code will invalidate some
+ * buckets, but it can't use those buckets until their new gens are safely on
+ * disk.
+ */
+
+static void prio_endio(struct bio *bio, int error)
+{
+	struct cache *ca = bio->bi_private;
+
+	cache_set_err_on(error, ca->set, "accessing priorities");
+	bch_bbio_free(bio, ca->set);
+	closure_put(&ca->prio);
+}
+
+static void prio_io(struct cache *ca, uint64_t bucket, unsigned long rw)
+{
+	struct closure *cl = &ca->prio;
+	struct bio *bio = bch_bbio_alloc(ca->set);
+
+	closure_init_stack(cl);
+
+	bio->bi_sector	= bucket * ca->sb.bucket_size;
+	bio->bi_bdev	= ca->bdev;
+	bio->bi_rw	= REQ_SYNC|REQ_META|rw;
+	bio->bi_size	= bucket_bytes(ca);
+
+	bio->bi_end_io	= prio_endio;
+	bio->bi_private = ca;
+	bch_bio_map(bio, ca->disk_buckets);
+
+	closure_bio_submit(bio, &ca->prio, ca);
+	closure_sync(cl);
+}
+
+#define buckets_free(c)	"free %zu, free_inc %zu, unused %zu",		\
+	fifo_used(&c->free), fifo_used(&c->free_inc), fifo_used(&c->unused)
+
+void bch_prio_write(struct cache *ca)
+{
+	int i;
+	struct bucket *b;
+	struct closure cl;
+
+	closure_init_stack(&cl);
+
+	lockdep_assert_held(&ca->set->bucket_lock);
+
+	for (b = ca->buckets;
+	     b < ca->buckets + ca->sb.nbuckets; b++)
+		b->disk_gen = b->gen;
+
+	ca->disk_buckets->seq++;
+
+	atomic_long_add(ca->sb.bucket_size * prio_buckets(ca),
+			&ca->meta_sectors_written);
+
+	pr_debug("free %zu, free_inc %zu, unused %zu", fifo_used(&ca->free),
+		 fifo_used(&ca->free_inc), fifo_used(&ca->unused));
+	blktrace_msg(ca, "Starting priorities: " buckets_free(ca));
+
+	for (i = prio_buckets(ca) - 1; i >= 0; --i) {
+		long bucket;
+		struct prio_set *p = ca->disk_buckets;
+		struct bucket_disk *d = p->data;
+		struct bucket_disk *end = d + prios_per_bucket(ca);
+
+		for (b = ca->buckets + i * prios_per_bucket(ca);
+		     b < ca->buckets + ca->sb.nbuckets && d < end;
+		     b++, d++) {
+			d->prio = cpu_to_le16(b->prio);
+			d->gen = b->gen;
+		}
+
+		p->next_bucket	= ca->prio_buckets[i + 1];
+		p->magic	= pset_magic(ca);
+		p->csum		= bch_crc64(&p->magic, bucket_bytes(ca) - 8);
+
+		bucket = bch_bucket_alloc(ca, WATERMARK_PRIO, &cl);
+		BUG_ON(bucket == -1);
+
+		mutex_unlock(&ca->set->bucket_lock);
+		prio_io(ca, bucket, REQ_WRITE);
+		mutex_lock(&ca->set->bucket_lock);
+
+		ca->prio_buckets[i] = bucket;
+		atomic_dec_bug(&ca->buckets[bucket].pin);
+	}
+
+	mutex_unlock(&ca->set->bucket_lock);
+
+	bch_journal_meta(ca->set, &cl);
+	closure_sync(&cl);
+
+	mutex_lock(&ca->set->bucket_lock);
+
+	ca->need_save_prio = 0;
+
+	/*
+	 * Don't want the old priorities to get garbage collected until after we
+	 * finish writing the new ones, and they're journalled
+	 */
+	for (i = 0; i < prio_buckets(ca); i++)
+		ca->prio_last_buckets[i] = ca->prio_buckets[i];
+}
+
+static void prio_read(struct cache *ca, uint64_t bucket)
+{
+	struct prio_set *p = ca->disk_buckets;
+	struct bucket_disk *d = p->data + prios_per_bucket(ca), *end = d;
+	struct bucket *b;
+	unsigned bucket_nr = 0;
+
+	for (b = ca->buckets;
+	     b < ca->buckets + ca->sb.nbuckets;
+	     b++, d++) {
+		if (d == end) {
+			ca->prio_buckets[bucket_nr] = bucket;
+			ca->prio_last_buckets[bucket_nr] = bucket;
+			bucket_nr++;
+
+			prio_io(ca, bucket, READ_SYNC);
+
+			if (p->csum != bch_crc64(&p->magic, bucket_bytes(ca) - 8))
+				pr_warn("bad csum reading priorities");
+
+			if (p->magic != pset_magic(ca))
+				pr_warn("bad magic reading priorities");
+
+			bucket = p->next_bucket;
+			d = p->data;
+		}
+
+		b->prio = le16_to_cpu(d->prio);
+		b->gen = b->disk_gen = b->last_gc = b->gc_gen = d->gen;
+	}
+}
+
+/* Bcache device */
+
+static int open_dev(struct block_device *b, fmode_t mode)
+{
+	struct bcache_device *d = b->bd_disk->private_data;
+	if (atomic_read(&d->closing))
+		return -ENXIO;
+
+	closure_get(&d->cl);
+	return 0;
+}
+
+static int release_dev(struct gendisk *b, fmode_t mode)
+{
+	struct bcache_device *d = b->private_data;
+	closure_put(&d->cl);
+	return 0;
+}
+
+static int ioctl_dev(struct block_device *b, fmode_t mode,
+		     unsigned int cmd, unsigned long arg)
+{
+	struct bcache_device *d = b->bd_disk->private_data;
+	return d->ioctl(d, mode, cmd, arg);
+}
+
+static const struct block_device_operations bcache_ops = {
+	.open		= open_dev,
+	.release	= release_dev,
+	.ioctl		= ioctl_dev,
+	.owner		= THIS_MODULE,
+};
+
+void bcache_device_stop(struct bcache_device *d)
+{
+	if (!atomic_xchg(&d->closing, 1))
+		closure_queue(&d->cl);
+}
+
+static void bcache_device_unlink(struct bcache_device *d)
+{
+	unsigned i;
+	struct cache *ca;
+
+	sysfs_remove_link(&d->c->kobj, d->name);
+	sysfs_remove_link(&d->kobj, "cache");
+
+	for_each_cache(ca, d->c, i)
+		bd_unlink_disk_holder(ca->bdev, d->disk);
+}
+
+static void bcache_device_link(struct bcache_device *d, struct cache_set *c,
+			       const char *name)
+{
+	unsigned i;
+	struct cache *ca;
+
+	for_each_cache(ca, d->c, i)
+		bd_link_disk_holder(ca->bdev, d->disk);
+
+	snprintf(d->name, BCACHEDEVNAME_SIZE,
+		 "%s%u", name, d->id);
+
+	WARN(sysfs_create_link(&d->kobj, &c->kobj, "cache") ||
+	     sysfs_create_link(&c->kobj, &d->kobj, d->name),
+	     "Couldn't create device <-> cache set symlinks");
+}
+
+static void bcache_device_detach(struct bcache_device *d)
+{
+	lockdep_assert_held(&bch_register_lock);
+
+	if (atomic_read(&d->detaching)) {
+		struct uuid_entry *u = d->c->uuids + d->id;
+
+		SET_UUID_FLASH_ONLY(u, 0);
+		memcpy(u->uuid, invalid_uuid, 16);
+		u->invalidated = cpu_to_le32(get_seconds());
+		bch_uuid_write(d->c);
+
+		atomic_set(&d->detaching, 0);
+	}
+
+	bcache_device_unlink(d);
+
+	d->c->devices[d->id] = NULL;
+	closure_put(&d->c->caching);
+	d->c = NULL;
+}
+
+static void bcache_device_attach(struct bcache_device *d, struct cache_set *c,
+				 unsigned id)
+{
+	BUG_ON(test_bit(CACHE_SET_STOPPING, &c->flags));
+
+	d->id = id;
+	d->c = c;
+	c->devices[id] = d;
+
+	closure_get(&c->caching);
+}
+
+static void bcache_device_free(struct bcache_device *d)
+{
+	lockdep_assert_held(&bch_register_lock);
+
+	pr_info("%s stopped", d->disk->disk_name);
+
+	if (d->c)
+		bcache_device_detach(d);
+
+	if (d->disk)
+		del_gendisk(d->disk);
+	if (d->disk && d->disk->queue)
+		blk_cleanup_queue(d->disk->queue);
+	if (d->disk)
+		put_disk(d->disk);
+
+	bio_split_pool_free(&d->bio_split_hook);
+	if (d->unaligned_bvec)
+		mempool_destroy(d->unaligned_bvec);
+	if (d->bio_split)
+		bioset_free(d->bio_split);
+
+	closure_debug_destroy(&d->cl);
+}
+
+static int bcache_device_init(struct bcache_device *d, unsigned block_size)
+{
+	struct request_queue *q;
+
+	if (!(d->bio_split = bioset_create(4, offsetof(struct bbio, bio))) ||
+	    !(d->unaligned_bvec = mempool_create_kmalloc_pool(1,
+				sizeof(struct bio_vec) * BIO_MAX_PAGES)) ||
+	    bio_split_pool_init(&d->bio_split_hook))
+
+		return -ENOMEM;
+
+	d->disk = alloc_disk(1);
+	if (!d->disk)
+		return -ENOMEM;
+
+	snprintf(d->disk->disk_name, DISK_NAME_LEN, "bcache%i", bcache_minor);
+
+	d->disk->major		= bcache_major;
+	d->disk->first_minor	= bcache_minor++;
+	d->disk->fops		= &bcache_ops;
+	d->disk->private_data	= d;
+
+	q = blk_alloc_queue(GFP_KERNEL);
+	if (!q)
+		return -ENOMEM;
+
+	blk_queue_make_request(q, NULL);
+	d->disk->queue			= q;
+	q->queuedata			= d;
+	q->backing_dev_info.congested_data = d;
+	q->limits.max_hw_sectors	= UINT_MAX;
+	q->limits.max_sectors		= UINT_MAX;
+	q->limits.max_segment_size	= UINT_MAX;
+	q->limits.max_segments		= BIO_MAX_PAGES;
+	q->limits.max_discard_sectors	= UINT_MAX;
+	q->limits.io_min		= block_size;
+	q->limits.logical_block_size	= block_size;
+	q->limits.physical_block_size	= block_size;
+	set_bit(QUEUE_FLAG_NONROT,	&d->disk->queue->queue_flags);
+	set_bit(QUEUE_FLAG_DISCARD,	&d->disk->queue->queue_flags);
+
+	return 0;
+}
+
+/* Cached device */
+
+static void calc_cached_dev_sectors(struct cache_set *c)
+{
+	uint64_t sectors = 0;
+	struct cached_dev *dc;
+
+	list_for_each_entry(dc, &c->cached_devs, list)
+		sectors += bdev_sectors(dc->bdev);
+
+	c->cached_dev_sectors = sectors;
+}
+
+void bch_cached_dev_run(struct cached_dev *dc)
+{
+	struct bcache_device *d = &dc->disk;
+
+	if (atomic_xchg(&dc->running, 1))
+		return;
+
+	if (!d->c &&
+	    BDEV_STATE(&dc->sb) != BDEV_STATE_NONE) {
+		struct closure cl;
+		closure_init_stack(&cl);
+
+		SET_BDEV_STATE(&dc->sb, BDEV_STATE_STALE);
+		bch_write_bdev_super(dc, &cl);
+		closure_sync(&cl);
+	}
+
+	add_disk(d->disk);
+	bd_link_disk_holder(dc->bdev, dc->disk.disk);
+#if 0
+	char *env[] = { "SYMLINK=label" , NULL };
+	kobject_uevent_env(&disk_to_dev(d->disk)->kobj, KOBJ_CHANGE, env);
+#endif
+	if (sysfs_create_link(&d->kobj, &disk_to_dev(d->disk)->kobj, "dev") ||
+	    sysfs_create_link(&disk_to_dev(d->disk)->kobj, &d->kobj, "bcache"))
+		pr_debug("error creating sysfs link");
+}
+
+static void cached_dev_detach_finish(struct work_struct *w)
+{
+	struct cached_dev *dc = container_of(w, struct cached_dev, detach);
+	char buf[BDEVNAME_SIZE];
+	struct closure cl;
+	closure_init_stack(&cl);
+
+	BUG_ON(!atomic_read(&dc->disk.detaching));
+	BUG_ON(atomic_read(&dc->count));
+
+	mutex_lock(&bch_register_lock);
+
+	memset(&dc->sb.set_uuid, 0, 16);
+	SET_BDEV_STATE(&dc->sb, BDEV_STATE_NONE);
+
+	bch_write_bdev_super(dc, &cl);
+	closure_sync(&cl);
+
+	bcache_device_detach(&dc->disk);
+	list_move(&dc->list, &uncached_devices);
+
+	mutex_unlock(&bch_register_lock);
+
+	pr_info("Caching disabled for %s", bdevname(dc->bdev, buf));
+
+	/* Drop ref we took in cached_dev_detach() */
+	closure_put(&dc->disk.cl);
+}
+
+void bch_cached_dev_detach(struct cached_dev *dc)
+{
+	lockdep_assert_held(&bch_register_lock);
+
+	if (atomic_read(&dc->disk.closing))
+		return;
+
+	if (atomic_xchg(&dc->disk.detaching, 1))
+		return;
+
+	/*
+	 * Block the device from being closed and freed until we're finished
+	 * detaching
+	 */
+	closure_get(&dc->disk.cl);
+
+	bch_writeback_queue(dc);
+	cached_dev_put(dc);
+}
+
+int bch_cached_dev_attach(struct cached_dev *dc, struct cache_set *c)
+{
+	uint32_t rtime = cpu_to_le32(get_seconds());
+	struct uuid_entry *u;
+	char buf[BDEVNAME_SIZE];
+
+	bdevname(dc->bdev, buf);
+
+	if (memcmp(dc->sb.set_uuid, c->sb.set_uuid, 16))
+		return -ENOENT;
+
+	if (dc->disk.c) {
+		pr_err("Can't attach %s: already attached", buf);
+		return -EINVAL;
+	}
+
+	if (test_bit(CACHE_SET_STOPPING, &c->flags)) {
+		pr_err("Can't attach %s: shutting down", buf);
+		return -EINVAL;
+	}
+
+	if (dc->sb.block_size < c->sb.block_size) {
+		/* Will die */
+		pr_err("Couldn't attach %s: block size less than set's block size",
+		       buf);
+		return -EINVAL;
+	}
+
+	u = uuid_find(c, dc->sb.uuid);
+
+	if (u &&
+	    (BDEV_STATE(&dc->sb) == BDEV_STATE_STALE ||
+	     BDEV_STATE(&dc->sb) == BDEV_STATE_NONE)) {
+		memcpy(u->uuid, invalid_uuid, 16);
+		u->invalidated = cpu_to_le32(get_seconds());
+		u = NULL;
+	}
+
+	if (!u) {
+		if (BDEV_STATE(&dc->sb) == BDEV_STATE_DIRTY) {
+			pr_err("Couldn't find uuid for %s in set", buf);
+			return -ENOENT;
+		}
+
+		u = uuid_find_empty(c);
+		if (!u) {
+			pr_err("Not caching %s, no room for UUID", buf);
+			return -EINVAL;
+		}
+	}
+
+	/* Deadlocks since we're called via sysfs...
+	sysfs_remove_file(&dc->kobj, &sysfs_attach);
+	 */
+
+	if (bch_is_zero(u->uuid, 16)) {
+		struct closure cl;
+		closure_init_stack(&cl);
+
+		memcpy(u->uuid, dc->sb.uuid, 16);
+		memcpy(u->label, dc->sb.label, SB_LABEL_SIZE);
+		u->first_reg = u->last_reg = rtime;
+		bch_uuid_write(c);
+
+		memcpy(dc->sb.set_uuid, c->sb.set_uuid, 16);
+		SET_BDEV_STATE(&dc->sb, BDEV_STATE_CLEAN);
+
+		bch_write_bdev_super(dc, &cl);
+		closure_sync(&cl);
+	} else {
+		u->last_reg = rtime;
+		bch_uuid_write(c);
+	}
+
+	bcache_device_attach(&dc->disk, c, u - c->uuids);
+	list_move(&dc->list, &c->cached_devs);
+	calc_cached_dev_sectors(c);
+
+	smp_wmb();
+	/*
+	 * dc->c must be set before dc->count != 0 - paired with the mb in
+	 * cached_dev_get()
+	 */
+	atomic_set(&dc->count, 1);
+
+	if (BDEV_STATE(&dc->sb) == BDEV_STATE_DIRTY) {
+		atomic_set(&dc->has_dirty, 1);
+		atomic_inc(&dc->count);
+		bch_writeback_queue(dc);
+	}
+
+	bch_cached_dev_run(dc);
+	bcache_device_link(&dc->disk, c, "bdev");
+
+	pr_info("Caching %s as %s on set %pU",
+		bdevname(dc->bdev, buf), dc->disk.disk->disk_name,
+		dc->disk.c->sb.set_uuid);
+	return 0;
+}
+
+void bch_cached_dev_release(struct kobject *kobj)
+{
+	struct cached_dev *dc = container_of(kobj, struct cached_dev,
+					     disk.kobj);
+	kfree(dc);
+	module_put(THIS_MODULE);
+}
+
+static void cached_dev_free(struct closure *cl)
+{
+	struct cached_dev *dc = container_of(cl, struct cached_dev, disk.cl);
+
+	cancel_delayed_work_sync(&dc->writeback_rate_update);
+
+	mutex_lock(&bch_register_lock);
+
+	bd_unlink_disk_holder(dc->bdev, dc->disk.disk);
+	bcache_device_free(&dc->disk);
+	list_del(&dc->list);
+
+	mutex_unlock(&bch_register_lock);
+
+	if (!IS_ERR_OR_NULL(dc->bdev)) {
+		blk_sync_queue(bdev_get_queue(dc->bdev));
+		blkdev_put(dc->bdev, FMODE_READ|FMODE_WRITE|FMODE_EXCL);
+	}
+
+	wake_up(&unregister_wait);
+
+	kobject_put(&dc->disk.kobj);
+}
+
+static void cached_dev_flush(struct closure *cl)
+{
+	struct cached_dev *dc = container_of(cl, struct cached_dev, disk.cl);
+	struct bcache_device *d = &dc->disk;
+
+	bch_cache_accounting_destroy(&dc->accounting);
+	kobject_del(&d->kobj);
+
+	continue_at(cl, cached_dev_free, system_wq);
+}
+
+static int cached_dev_init(struct cached_dev *dc, unsigned block_size)
+{
+	int err;
+	struct io *io;
+
+	closure_init(&dc->disk.cl, NULL);
+	set_closure_fn(&dc->disk.cl, cached_dev_flush, system_wq);
+
+	__module_get(THIS_MODULE);
+	INIT_LIST_HEAD(&dc->list);
+	kobject_init(&dc->disk.kobj, &bch_cached_dev_ktype);
+
+	bch_cache_accounting_init(&dc->accounting, &dc->disk.cl);
+
+	err = bcache_device_init(&dc->disk, block_size);
+	if (err)
+		goto err;
+
+	spin_lock_init(&dc->io_lock);
+	closure_init_unlocked(&dc->sb_write);
+	INIT_WORK(&dc->detach, cached_dev_detach_finish);
+
+	dc->sequential_merge		= true;
+	dc->sequential_cutoff		= 4 << 20;
+
+	INIT_LIST_HEAD(&dc->io_lru);
+	dc->sb_bio.bi_max_vecs	= 1;
+	dc->sb_bio.bi_io_vec	= dc->sb_bio.bi_inline_vecs;
+
+	for (io = dc->io; io < dc->io + RECENT_IO; io++) {
+		list_add(&io->lru, &dc->io_lru);
+		hlist_add_head(&io->hash, dc->io_hash + RECENT_IO);
+	}
+
+	bch_writeback_init_cached_dev(dc);
+	return 0;
+err:
+	bcache_device_stop(&dc->disk);
+	return err;
+}
+
+/* Cached device - bcache superblock */
+
+static const char *register_bdev(struct cache_sb *sb, struct page *sb_page,
+				 struct block_device *bdev,
+				 struct cached_dev *dc)
+{
+	char name[BDEVNAME_SIZE];
+	const char *err = "cannot allocate memory";
+	struct gendisk *g;
+	struct cache_set *c;
+
+	if (!dc || cached_dev_init(dc, sb->block_size << 9) != 0)
+		return err;
+
+	memcpy(&dc->sb, sb, sizeof(struct cache_sb));
+	dc->sb_bio.bi_io_vec[0].bv_page = sb_page;
+	dc->bdev = bdev;
+	dc->bdev->bd_holder = dc;
+
+	g = dc->disk.disk;
+
+	set_capacity(g, dc->bdev->bd_part->nr_sects - dc->sb.data_offset);
+
+	g->queue->backing_dev_info.ra_pages =
+		max(g->queue->backing_dev_info.ra_pages,
+		    bdev->bd_queue->backing_dev_info.ra_pages);
+
+	bch_cached_dev_request_init(dc);
+
+	err = "error creating kobject";
+	if (kobject_add(&dc->disk.kobj, &part_to_dev(bdev->bd_part)->kobj,
+			"bcache"))
+		goto err;
+	if (bch_cache_accounting_add_kobjs(&dc->accounting, &dc->disk.kobj))
+		goto err;
+
+	list_add(&dc->list, &uncached_devices);
+	list_for_each_entry(c, &bch_cache_sets, list)
+		bch_cached_dev_attach(dc, c);
+
+	if (BDEV_STATE(&dc->sb) == BDEV_STATE_NONE ||
+	    BDEV_STATE(&dc->sb) == BDEV_STATE_STALE)
+		bch_cached_dev_run(dc);
+
+	return NULL;
+err:
+	kobject_put(&dc->disk.kobj);
+	pr_notice("error opening %s: %s", bdevname(bdev, name), err);
+	/*
+	 * Return NULL instead of an error because kobject_put() cleans
+	 * everything up
+	 */
+	return NULL;
+}
+
+/* Flash only volumes */
+
+void bch_flash_dev_release(struct kobject *kobj)
+{
+	struct bcache_device *d = container_of(kobj, struct bcache_device,
+					       kobj);
+	kfree(d);
+}
+
+static void flash_dev_free(struct closure *cl)
+{
+	struct bcache_device *d = container_of(cl, struct bcache_device, cl);
+	bcache_device_free(d);
+	kobject_put(&d->kobj);
+}
+
+static void flash_dev_flush(struct closure *cl)
+{
+	struct bcache_device *d = container_of(cl, struct bcache_device, cl);
+
+	bcache_device_unlink(d);
+	kobject_del(&d->kobj);
+	continue_at(cl, flash_dev_free, system_wq);
+}
+
+static int flash_dev_run(struct cache_set *c, struct uuid_entry *u)
+{
+	struct bcache_device *d = kzalloc(sizeof(struct bcache_device),
+					  GFP_KERNEL);
+	if (!d)
+		return -ENOMEM;
+
+	closure_init(&d->cl, NULL);
+	set_closure_fn(&d->cl, flash_dev_flush, system_wq);
+
+	kobject_init(&d->kobj, &bch_flash_dev_ktype);
+
+	if (bcache_device_init(d, block_bytes(c)))
+		goto err;
+
+	bcache_device_attach(d, c, u - c->uuids);
+	set_capacity(d->disk, u->sectors);
+	bch_flash_dev_request_init(d);
+	add_disk(d->disk);
+
+	if (kobject_add(&d->kobj, &disk_to_dev(d->disk)->kobj, "bcache"))
+		goto err;
+
+	bcache_device_link(d, c, "volume");
+
+	return 0;
+err:
+	kobject_put(&d->kobj);
+	return -ENOMEM;
+}
+
+static int flash_devs_run(struct cache_set *c)
+{
+	int ret = 0;
+	struct uuid_entry *u;
+
+	for (u = c->uuids;
+	     u < c->uuids + c->nr_uuids && !ret;
+	     u++)
+		if (UUID_FLASH_ONLY(u))
+			ret = flash_dev_run(c, u);
+
+	return ret;
+}
+
+int bch_flash_dev_create(struct cache_set *c, uint64_t size)
+{
+	struct uuid_entry *u;
+
+	if (test_bit(CACHE_SET_STOPPING, &c->flags))
+		return -EINTR;
+
+	u = uuid_find_empty(c);
+	if (!u) {
+		pr_err("Can't create volume, no room for UUID");
+		return -EINVAL;
+	}
+
+	get_random_bytes(u->uuid, 16);
+	memset(u->label, 0, 32);
+	u->first_reg = u->last_reg = cpu_to_le32(get_seconds());
+
+	SET_UUID_FLASH_ONLY(u, 1);
+	u->sectors = size >> 9;
+
+	bch_uuid_write(c);
+
+	return flash_dev_run(c, u);
+}
+
+/* Cache set */
+
+__printf(2, 3)
+bool bch_cache_set_error(struct cache_set *c, const char *fmt, ...)
+{
+	va_list args;
+
+	if (test_bit(CACHE_SET_STOPPING, &c->flags))
+		return false;
+
+	/* XXX: we can be called from atomic context
+	acquire_console_sem();
+	*/
+
+	printk(KERN_ERR "bcache: error on %pU: ", c->sb.set_uuid);
+
+	va_start(args, fmt);
+	vprintk(fmt, args);
+	va_end(args);
+
+	printk(", disabling caching\n");
+
+	bch_cache_set_unregister(c);
+	return true;
+}
+
+void bch_cache_set_release(struct kobject *kobj)
+{
+	struct cache_set *c = container_of(kobj, struct cache_set, kobj);
+	kfree(c);
+	module_put(THIS_MODULE);
+}
+
+static void cache_set_free(struct closure *cl)
+{
+	struct cache_set *c = container_of(cl, struct cache_set, cl);
+	struct cache *ca;
+	unsigned i;
+
+	if (!IS_ERR_OR_NULL(c->debug))
+		debugfs_remove(c->debug);
+
+	bch_open_buckets_free(c);
+	bch_btree_cache_free(c);
+	bch_journal_free(c);
+
+	for_each_cache(ca, c, i)
+		if (ca)
+			kobject_put(&ca->kobj);
+
+	free_pages((unsigned long) c->uuids, ilog2(bucket_pages(c)));
+	free_pages((unsigned long) c->sort, ilog2(bucket_pages(c)));
+
+	kfree(c->fill_iter);
+	if (c->bio_split)
+		bioset_free(c->bio_split);
+	if (c->bio_meta)
+		mempool_destroy(c->bio_meta);
+	if (c->search)
+		mempool_destroy(c->search);
+	kfree(c->devices);
+
+	mutex_lock(&bch_register_lock);
+	list_del(&c->list);
+	mutex_unlock(&bch_register_lock);
+
+	pr_info("Cache set %pU unregistered", c->sb.set_uuid);
+	wake_up(&unregister_wait);
+
+	closure_debug_destroy(&c->cl);
+	kobject_put(&c->kobj);
+}
+
+static void cache_set_flush(struct closure *cl)
+{
+	struct cache_set *c = container_of(cl, struct cache_set, caching);
+	struct btree *b;
+
+	/* Shut down allocator threads */
+	set_bit(CACHE_SET_STOPPING_2, &c->flags);
+	wake_up(&c->alloc_wait);
+
+	bch_cache_accounting_destroy(&c->accounting);
+
+	kobject_put(&c->internal);
+	kobject_del(&c->kobj);
+
+	if (!IS_ERR_OR_NULL(c->root))
+		list_add(&c->root->list, &c->btree_cache);
+
+	/* Should skip this if we're unregistering because of an error */
+	list_for_each_entry(b, &c->btree_cache, list)
+		if (btree_node_dirty(b))
+			bch_btree_write(b, true, NULL);
+
+	closure_return(cl);
+}
+
+static void __cache_set_unregister(struct closure *cl)
+{
+	struct cache_set *c = container_of(cl, struct cache_set, caching);
+	struct cached_dev *dc, *t;
+	size_t i;
+
+	mutex_lock(&bch_register_lock);
+
+	if (test_bit(CACHE_SET_UNREGISTERING, &c->flags))
+		list_for_each_entry_safe(dc, t, &c->cached_devs, list)
+			bch_cached_dev_detach(dc);
+
+	for (i = 0; i < c->nr_uuids; i++)
+		if (c->devices[i] && UUID_FLASH_ONLY(&c->uuids[i]))
+			bcache_device_stop(c->devices[i]);
+
+	mutex_unlock(&bch_register_lock);
+
+	continue_at(cl, cache_set_flush, system_wq);
+}
+
+void bch_cache_set_stop(struct cache_set *c)
+{
+	if (!test_and_set_bit(CACHE_SET_STOPPING, &c->flags))
+		closure_queue(&c->caching);
+}
+
+void bch_cache_set_unregister(struct cache_set *c)
+{
+	set_bit(CACHE_SET_UNREGISTERING, &c->flags);
+	bch_cache_set_stop(c);
+}
+
+#define alloc_bucket_pages(gfp, c)			\
+	((void *) __get_free_pages(__GFP_ZERO|gfp, ilog2(bucket_pages(c))))
+
+struct cache_set *bch_cache_set_alloc(struct cache_sb *sb)
+{
+	int iter_size;
+	struct cache_set *c = kzalloc(sizeof(struct cache_set), GFP_KERNEL);
+	if (!c)
+		return NULL;
+
+	__module_get(THIS_MODULE);
+	closure_init(&c->cl, NULL);
+	set_closure_fn(&c->cl, cache_set_free, system_wq);
+
+	closure_init(&c->caching, &c->cl);
+	set_closure_fn(&c->caching, __cache_set_unregister, system_wq);
+
+	/* Maybe create continue_at_noreturn() and use it here? */
+	closure_set_stopped(&c->cl);
+	closure_put(&c->cl);
+
+	kobject_init(&c->kobj, &bch_cache_set_ktype);
+	kobject_init(&c->internal, &bch_cache_set_internal_ktype);
+
+	bch_cache_accounting_init(&c->accounting, &c->cl);
+
+	memcpy(c->sb.set_uuid, sb->set_uuid, 16);
+	c->sb.block_size	= sb->block_size;
+	c->sb.bucket_size	= sb->bucket_size;
+	c->sb.nr_in_set		= sb->nr_in_set;
+	c->sb.last_mount	= sb->last_mount;
+	c->bucket_bits		= ilog2(sb->bucket_size);
+	c->block_bits		= ilog2(sb->block_size);
+	c->nr_uuids		= bucket_bytes(c) / sizeof(struct uuid_entry);
+
+	c->btree_pages		= c->sb.bucket_size / PAGE_SECTORS;
+	if (c->btree_pages > BTREE_MAX_PAGES)
+		c->btree_pages = max_t(int, c->btree_pages / 4,
+				       BTREE_MAX_PAGES);
+
+	init_waitqueue_head(&c->alloc_wait);
+	mutex_init(&c->bucket_lock);
+	mutex_init(&c->fill_lock);
+	mutex_init(&c->sort_lock);
+	spin_lock_init(&c->sort_time_lock);
+	closure_init_unlocked(&c->sb_write);
+	closure_init_unlocked(&c->uuid_write);
+	spin_lock_init(&c->btree_read_time_lock);
+	bch_moving_init_cache_set(c);
+
+	INIT_LIST_HEAD(&c->list);
+	INIT_LIST_HEAD(&c->cached_devs);
+	INIT_LIST_HEAD(&c->btree_cache);
+	INIT_LIST_HEAD(&c->btree_cache_freeable);
+	INIT_LIST_HEAD(&c->btree_cache_freed);
+	INIT_LIST_HEAD(&c->data_buckets);
+
+	c->search = mempool_create_slab_pool(32, bch_search_cache);
+	if (!c->search)
+		goto err;
+
+	iter_size = (sb->bucket_size / sb->block_size + 1) *
+		sizeof(struct btree_iter_set);
+
+	if (!(c->devices = kzalloc(c->nr_uuids * sizeof(void *), GFP_KERNEL)) ||
+	    !(c->bio_meta = mempool_create_kmalloc_pool(2,
+				sizeof(struct bbio) + sizeof(struct bio_vec) *
+				bucket_pages(c))) ||
+	    !(c->bio_split = bioset_create(4, offsetof(struct bbio, bio))) ||
+	    !(c->fill_iter = kmalloc(iter_size, GFP_KERNEL)) ||
+	    !(c->sort = alloc_bucket_pages(GFP_KERNEL, c)) ||
+	    !(c->uuids = alloc_bucket_pages(GFP_KERNEL, c)) ||
+	    bch_journal_alloc(c) ||
+	    bch_btree_cache_alloc(c) ||
+	    bch_open_buckets_alloc(c))
+		goto err;
+
+	c->fill_iter->size = sb->bucket_size / sb->block_size;
+
+	c->congested_read_threshold_us	= 2000;
+	c->congested_write_threshold_us	= 20000;
+	c->error_limit	= 8 << IO_ERROR_SHIFT;
+
+	return c;
+err:
+	bch_cache_set_unregister(c);
+	return NULL;
+}
+
+static void run_cache_set(struct cache_set *c)
+{
+	const char *err = "cannot allocate memory";
+	struct cached_dev *dc, *t;
+	struct cache *ca;
+	unsigned i;
+
+	struct btree_op op;
+	bch_btree_op_init_stack(&op);
+	op.lock = SHRT_MAX;
+
+	for_each_cache(ca, c, i)
+		c->nbuckets += ca->sb.nbuckets;
+
+	if (CACHE_SYNC(&c->sb)) {
+		LIST_HEAD(journal);
+		struct bkey *k;
+		struct jset *j;
+
+		err = "cannot allocate memory for journal";
+		if (bch_journal_read(c, &journal, &op))
+			goto err;
+
+		pr_debug("btree_journal_read() done");
+
+		err = "no journal entries found";
+		if (list_empty(&journal))
+			goto err;
+
+		j = &list_entry(journal.prev, struct journal_replay, list)->j;
+
+		err = "IO error reading priorities";
+		for_each_cache(ca, c, i)
+			prio_read(ca, j->prio_bucket[ca->sb.nr_this_dev]);
+
+		/*
+		 * If prio_read() fails it'll call cache_set_error and we'll
+		 * tear everything down right away, but if we perhaps checked
+		 * sooner we could avoid journal replay.
+		 */
+
+		k = &j->btree_root;
+
+		err = "bad btree root";
+		if (__bch_ptr_invalid(c, j->btree_level + 1, k))
+			goto err;
+
+		err = "error reading btree root";
+		c->root = bch_btree_node_get(c, k, j->btree_level, &op);
+		if (IS_ERR_OR_NULL(c->root))
+			goto err;
+
+		list_del_init(&c->root->list);
+		rw_unlock(true, c->root);
+
+		err = uuid_read(c, j, &op.cl);
+		if (err)
+			goto err;
+
+		err = "error in recovery";
+		if (bch_btree_check(c, &op))
+			goto err;
+
+		bch_journal_mark(c, &journal);
+		bch_btree_gc_finish(c);
+		pr_debug("btree_check() done");
+
+		/*
+		 * bcache_journal_next() can't happen sooner, or
+		 * btree_gc_finish() will give spurious errors about last_gc >
+		 * gc_gen - this is a hack but oh well.
+		 */
+		bch_journal_next(&c->journal);
+
+		for_each_cache(ca, c, i)
+			closure_call(&ca->alloc, bch_allocator_thread,
+				     system_wq, &c->cl);
+
+		/*
+		 * First place it's safe to allocate: btree_check() and
+		 * btree_gc_finish() have to run before we have buckets to
+		 * allocate, and bch_bucket_alloc_set() might cause a journal
+		 * entry to be written so bcache_journal_next() has to be called
+		 * first.
+		 *
+		 * If the uuids were in the old format we have to rewrite them
+		 * before the next journal entry is written:
+		 */
+		if (j->version < BCACHE_JSET_VERSION_UUID)
+			__uuid_write(c);
+
+		bch_journal_replay(c, &journal, &op);
+	} else {
+		pr_notice("invalidating existing data");
+		/* Don't want invalidate_buckets() to queue a gc yet */
+		closure_lock(&c->gc, NULL);
+
+		for_each_cache(ca, c, i) {
+			unsigned j;
+
+			ca->sb.keys = clamp_t(int, ca->sb.nbuckets >> 7,
+					      2, SB_JOURNAL_BUCKETS);
+
+			for (j = 0; j < ca->sb.keys; j++)
+				ca->sb.d[j] = ca->sb.first_bucket + j;
+		}
+
+		bch_btree_gc_finish(c);
+
+		for_each_cache(ca, c, i)
+			closure_call(&ca->alloc, bch_allocator_thread,
+				     ca->alloc_workqueue, &c->cl);
+
+		mutex_lock(&c->bucket_lock);
+		for_each_cache(ca, c, i)
+			bch_prio_write(ca);
+		mutex_unlock(&c->bucket_lock);
+
+		wake_up(&c->alloc_wait);
+
+		err = "cannot allocate new UUID bucket";
+		if (__uuid_write(c))
+			goto err_unlock_gc;
+
+		err = "cannot allocate new btree root";
+		c->root = bch_btree_node_alloc(c, 0, &op.cl);
+		if (IS_ERR_OR_NULL(c->root))
+			goto err_unlock_gc;
+
+		bkey_copy_key(&c->root->key, &MAX_KEY);
+		bch_btree_write(c->root, true, &op);
+
+		bch_btree_set_root(c->root);
+		rw_unlock(true, c->root);
+
+		/*
+		 * We don't want to write the first journal entry until
+		 * everything is set up - fortunately journal entries won't be
+		 * written until the SET_CACHE_SYNC() here:
+		 */
+		SET_CACHE_SYNC(&c->sb, true);
+
+		bch_journal_next(&c->journal);
+		bch_journal_meta(c, &op.cl);
+
+		/* Unlock */
+		closure_set_stopped(&c->gc.cl);
+		closure_put(&c->gc.cl);
+	}
+
+	closure_sync(&op.cl);
+	c->sb.last_mount = get_seconds();
+	bcache_write_super(c);
+
+	list_for_each_entry_safe(dc, t, &uncached_devices, list)
+		bch_cached_dev_attach(dc, c);
+
+	flash_devs_run(c);
+
+	return;
+err_unlock_gc:
+	closure_set_stopped(&c->gc.cl);
+	closure_put(&c->gc.cl);
+err:
+	closure_sync(&op.cl);
+	/* XXX: test this, it's broken */
+	bch_cache_set_error(c, err);
+}
+
+static bool can_attach_cache(struct cache *ca, struct cache_set *c)
+{
+	return ca->sb.block_size	== c->sb.block_size &&
+		ca->sb.bucket_size	== c->sb.block_size &&
+		ca->sb.nr_in_set	== c->sb.nr_in_set;
+}
+
+static const char *register_cache_set(struct cache *ca)
+{
+	char buf[12];
+	const char *err = "cannot allocate memory";
+	struct cache_set *c;
+
+	list_for_each_entry(c, &bch_cache_sets, list)
+		if (!memcmp(c->sb.set_uuid, ca->sb.set_uuid, 16)) {
+			if (c->cache[ca->sb.nr_this_dev])
+				return "duplicate cache set member";
+
+			if (!can_attach_cache(ca, c))
+				return "cache sb does not match set";
+
+			if (!CACHE_SYNC(&ca->sb))
+				SET_CACHE_SYNC(&c->sb, false);
+
+			goto found;
+		}
+
+	c = bch_cache_set_alloc(&ca->sb);
+	if (!c)
+		return err;
+
+	err = "error creating kobject";
+	if (kobject_add(&c->kobj, bcache_kobj, "%pU", c->sb.set_uuid) ||
+	    kobject_add(&c->internal, &c->kobj, "internal"))
+		goto err;
+
+	if (bch_cache_accounting_add_kobjs(&c->accounting, &c->kobj))
+		goto err;
+
+	bch_debug_init_cache_set(c);
+
+	list_add(&c->list, &bch_cache_sets);
+found:
+	sprintf(buf, "cache%i", ca->sb.nr_this_dev);
+	if (sysfs_create_link(&ca->kobj, &c->kobj, "set") ||
+	    sysfs_create_link(&c->kobj, &ca->kobj, buf))
+		goto err;
+
+	if (ca->sb.seq > c->sb.seq) {
+		c->sb.version		= ca->sb.version;
+		memcpy(c->sb.set_uuid, ca->sb.set_uuid, 16);
+		c->sb.flags             = ca->sb.flags;
+		c->sb.seq		= ca->sb.seq;
+		pr_debug("set version = %llu", c->sb.version);
+	}
+
+	ca->set = c;
+	ca->set->cache[ca->sb.nr_this_dev] = ca;
+	c->cache_by_alloc[c->caches_loaded++] = ca;
+
+	if (c->caches_loaded == c->sb.nr_in_set)
+		run_cache_set(c);
+
+	return NULL;
+err:
+	bch_cache_set_unregister(c);
+	return err;
+}
+
+/* Cache device */
+
+void bch_cache_release(struct kobject *kobj)
+{
+	struct cache *ca = container_of(kobj, struct cache, kobj);
+
+	if (ca->set)
+		ca->set->cache[ca->sb.nr_this_dev] = NULL;
+
+	bch_cache_allocator_exit(ca);
+
+	bio_split_pool_free(&ca->bio_split_hook);
+
+	if (ca->alloc_workqueue)
+		destroy_workqueue(ca->alloc_workqueue);
+
+	free_pages((unsigned long) ca->disk_buckets, ilog2(bucket_pages(ca)));
+	kfree(ca->prio_buckets);
+	vfree(ca->buckets);
+
+	free_heap(&ca->heap);
+	free_fifo(&ca->unused);
+	free_fifo(&ca->free_inc);
+	free_fifo(&ca->free);
+
+	if (ca->sb_bio.bi_inline_vecs[0].bv_page)
+		put_page(ca->sb_bio.bi_io_vec[0].bv_page);
+
+	if (!IS_ERR_OR_NULL(ca->bdev)) {
+		blk_sync_queue(bdev_get_queue(ca->bdev));
+		blkdev_put(ca->bdev, FMODE_READ|FMODE_WRITE|FMODE_EXCL);
+	}
+
+	kfree(ca);
+	module_put(THIS_MODULE);
+}
+
+static int cache_alloc(struct cache_sb *sb, struct cache *ca)
+{
+	size_t free;
+	struct bucket *b;
+
+	if (!ca)
+		return -ENOMEM;
+
+	__module_get(THIS_MODULE);
+	kobject_init(&ca->kobj, &bch_cache_ktype);
+
+	memcpy(&ca->sb, sb, sizeof(struct cache_sb));
+
+	INIT_LIST_HEAD(&ca->discards);
+
+	bio_init(&ca->sb_bio);
+	ca->sb_bio.bi_max_vecs	= 1;
+	ca->sb_bio.bi_io_vec	= ca->sb_bio.bi_inline_vecs;
+
+	bio_init(&ca->journal.bio);
+	ca->journal.bio.bi_max_vecs = 8;
+	ca->journal.bio.bi_io_vec = ca->journal.bio.bi_inline_vecs;
+
+	free = roundup_pow_of_two(ca->sb.nbuckets) >> 9;
+	free = max_t(size_t, free, (prio_buckets(ca) + 8) * 2);
+
+	if (!init_fifo(&ca->free,	free, GFP_KERNEL) ||
+	    !init_fifo(&ca->free_inc,	free << 2, GFP_KERNEL) ||
+	    !init_fifo(&ca->unused,	free << 2, GFP_KERNEL) ||
+	    !init_heap(&ca->heap,	free << 3, GFP_KERNEL) ||
+	    !(ca->buckets	= vmalloc(sizeof(struct bucket) *
+					  ca->sb.nbuckets)) ||
+	    !(ca->prio_buckets	= kzalloc(sizeof(uint64_t) * prio_buckets(ca) *
+					  2, GFP_KERNEL)) ||
+	    !(ca->disk_buckets	= alloc_bucket_pages(GFP_KERNEL, ca)) ||
+	    !(ca->alloc_workqueue = alloc_workqueue("bch_allocator", 0, 1)) ||
+	    bio_split_pool_init(&ca->bio_split_hook))
+		goto err;
+
+	ca->prio_last_buckets = ca->prio_buckets + prio_buckets(ca);
+
+	memset(ca->buckets, 0, ca->sb.nbuckets * sizeof(struct bucket));
+	for_each_bucket(b, ca)
+		atomic_set(&b->pin, 0);
+
+	if (bch_cache_allocator_init(ca))
+		goto err;
+
+	return 0;
+err:
+	kobject_put(&ca->kobj);
+	return -ENOMEM;
+}
+
+static const char *register_cache(struct cache_sb *sb, struct page *sb_page,
+				  struct block_device *bdev, struct cache *ca)
+{
+	char name[BDEVNAME_SIZE];
+	const char *err = "cannot allocate memory";
+
+	if (cache_alloc(sb, ca) != 0)
+		return err;
+
+	ca->sb_bio.bi_io_vec[0].bv_page = sb_page;
+	ca->bdev = bdev;
+	ca->bdev->bd_holder = ca;
+
+	if (blk_queue_discard(bdev_get_queue(ca->bdev)))
+		ca->discard = CACHE_DISCARD(&ca->sb);
+
+	err = "error creating kobject";
+	if (kobject_add(&ca->kobj, &part_to_dev(bdev->bd_part)->kobj, "bcache"))
+		goto err;
+
+	err = register_cache_set(ca);
+	if (err)
+		goto err;
+
+	pr_info("registered cache device %s", bdevname(bdev, name));
+
+	return NULL;
+err:
+	kobject_put(&ca->kobj);
+	pr_info("error opening %s: %s", bdevname(bdev, name), err);
+	/* Return NULL instead of an error because kobject_put() cleans
+	 * everything up
+	 */
+	return NULL;
+}
+
+/* Global interfaces/init */
+
+static ssize_t register_bcache(struct kobject *, struct kobj_attribute *,
+			       const char *, size_t);
+
+kobj_attribute_write(register,		register_bcache);
+kobj_attribute_write(register_quiet,	register_bcache);
+
+static ssize_t register_bcache(struct kobject *k, struct kobj_attribute *attr,
+			       const char *buffer, size_t size)
+{
+	ssize_t ret = size;
+	const char *err = "cannot allocate memory";
+	char *path = NULL;
+	struct cache_sb *sb = NULL;
+	struct block_device *bdev = NULL;
+	struct page *sb_page = NULL;
+
+	if (!try_module_get(THIS_MODULE))
+		return -EBUSY;
+
+	mutex_lock(&bch_register_lock);
+
+	if (!(path = kstrndup(buffer, size, GFP_KERNEL)) ||
+	    !(sb = kmalloc(sizeof(struct cache_sb), GFP_KERNEL)))
+		goto err;
+
+	err = "failed to open device";
+	bdev = blkdev_get_by_path(strim(path),
+				  FMODE_READ|FMODE_WRITE|FMODE_EXCL,
+				  sb);
+	if (bdev == ERR_PTR(-EBUSY))
+		err = "device busy";
+
+	if (IS_ERR(bdev) ||
+	    set_blocksize(bdev, 4096))
+		goto err;
+
+	err = read_super(sb, bdev, &sb_page);
+	if (err)
+		goto err_close;
+
+	if (SB_IS_BDEV(sb)) {
+		struct cached_dev *dc = kzalloc(sizeof(*dc), GFP_KERNEL);
+
+		err = register_bdev(sb, sb_page, bdev, dc);
+	} else {
+		struct cache *ca = kzalloc(sizeof(*ca), GFP_KERNEL);
+
+		err = register_cache(sb, sb_page, bdev, ca);
+	}
+
+	if (err) {
+		/* register_(bdev|cache) will only return an error if they
+		 * didn't get far enough to create the kobject - if they did,
+		 * the kobject destructor will do this cleanup.
+		 */
+		put_page(sb_page);
+err_close:
+		blkdev_put(bdev, FMODE_READ|FMODE_WRITE|FMODE_EXCL);
+err:
+		if (attr != &ksysfs_register_quiet)
+			pr_info("error opening %s: %s", path, err);
+		ret = -EINVAL;
+	}
+
+	kfree(sb);
+	kfree(path);
+	mutex_unlock(&bch_register_lock);
+	module_put(THIS_MODULE);
+	return ret;
+}
+
+static int bcache_reboot(struct notifier_block *n, unsigned long code, void *x)
+{
+	if (code == SYS_DOWN ||
+	    code == SYS_HALT ||
+	    code == SYS_POWER_OFF) {
+		DEFINE_WAIT(wait);
+		unsigned long start = jiffies;
+		bool stopped = false;
+
+		struct cache_set *c, *tc;
+		struct cached_dev *dc, *tdc;
+
+		mutex_lock(&bch_register_lock);
+
+		if (list_empty(&bch_cache_sets) &&
+		    list_empty(&uncached_devices))
+			goto out;
+
+		pr_info("Stopping all devices:");
+
+		list_for_each_entry_safe(c, tc, &bch_cache_sets, list)
+			bch_cache_set_stop(c);
+
+		list_for_each_entry_safe(dc, tdc, &uncached_devices, list)
+			bcache_device_stop(&dc->disk);
+
+		/* What's a condition variable? */
+		while (1) {
+			long timeout = start + 2 * HZ - jiffies;
+
+			stopped = list_empty(&bch_cache_sets) &&
+				list_empty(&uncached_devices);
+
+			if (timeout < 0 || stopped)
+				break;
+
+			prepare_to_wait(&unregister_wait, &wait,
+					TASK_UNINTERRUPTIBLE);
+
+			mutex_unlock(&bch_register_lock);
+			schedule_timeout(timeout);
+			mutex_lock(&bch_register_lock);
+		}
+
+		finish_wait(&unregister_wait, &wait);
+
+		if (stopped)
+			pr_info("All devices stopped");
+		else
+			pr_notice("Timeout waiting for devices to be closed");
+out:
+		mutex_unlock(&bch_register_lock);
+	}
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block reboot = {
+	.notifier_call	= bcache_reboot,
+	.priority	= INT_MAX, /* before any real devices */
+};
+
+static void bcache_exit(void)
+{
+	bch_debug_exit();
+	bch_writeback_exit();
+	bch_request_exit();
+	bch_btree_exit();
+	if (bcache_kobj)
+		kobject_put(bcache_kobj);
+	if (bcache_wq)
+		destroy_workqueue(bcache_wq);
+	unregister_blkdev(bcache_major, "bcache");
+	unregister_reboot_notifier(&reboot);
+}
+
+static int __init bcache_init(void)
+{
+	static const struct attribute *files[] = {
+		&ksysfs_register.attr,
+		&ksysfs_register_quiet.attr,
+		NULL
+	};
+
+	mutex_init(&bch_register_lock);
+	init_waitqueue_head(&unregister_wait);
+	register_reboot_notifier(&reboot);
+	closure_debug_init();
+
+	bcache_major = register_blkdev(0, "bcache");
+	if (bcache_major < 0)
+		return bcache_major;
+
+	if (!(bcache_wq = create_workqueue("bcache")) ||
+	    !(bcache_kobj = kobject_create_and_add("bcache", fs_kobj)) ||
+	    sysfs_create_files(bcache_kobj, files) ||
+	    bch_btree_init() ||
+	    bch_request_init() ||
+	    bch_writeback_init() ||
+	    bch_debug_init(bcache_kobj))
+		goto err;
+
+	return 0;
+err:
+	bcache_exit();
+	return -ENOMEM;
+}
+
+module_exit(bcache_exit);
+module_init(bcache_init);
diff --git a/drivers/md/bcache/sysfs.c b/drivers/md/bcache/sysfs.c
new file mode 100644
index 000000000000..4d9cca47e4c6
--- /dev/null
+++ b/drivers/md/bcache/sysfs.c
@@ -0,0 +1,817 @@
+/*
+ * bcache sysfs interfaces
+ *
+ * Copyright 2010, 2011 Kent Overstreet <kent.overstreet@gmail.com>
+ * Copyright 2012 Google, Inc.
+ */
+
+#include "bcache.h"
+#include "sysfs.h"
+#include "btree.h"
+#include "request.h"
+
+#include <linux/sort.h>
+
+static const char * const cache_replacement_policies[] = {
+	"lru",
+	"fifo",
+	"random",
+	NULL
+};
+
+write_attribute(attach);
+write_attribute(detach);
+write_attribute(unregister);
+write_attribute(stop);
+write_attribute(clear_stats);
+write_attribute(trigger_gc);
+write_attribute(prune_cache);
+write_attribute(flash_vol_create);
+
+read_attribute(bucket_size);
+read_attribute(block_size);
+read_attribute(nbuckets);
+read_attribute(tree_depth);
+read_attribute(root_usage_percent);
+read_attribute(priority_stats);
+read_attribute(btree_cache_size);
+read_attribute(btree_cache_max_chain);
+read_attribute(cache_available_percent);
+read_attribute(written);
+read_attribute(btree_written);
+read_attribute(metadata_written);
+read_attribute(active_journal_entries);
+
+sysfs_time_stats_attribute(btree_gc,	sec, ms);
+sysfs_time_stats_attribute(btree_split, sec, us);
+sysfs_time_stats_attribute(btree_sort,	ms,  us);
+sysfs_time_stats_attribute(btree_read,	ms,  us);
+sysfs_time_stats_attribute(try_harder,	ms,  us);
+
+read_attribute(btree_nodes);
+read_attribute(btree_used_percent);
+read_attribute(average_key_size);
+read_attribute(dirty_data);
+read_attribute(bset_tree_stats);
+
+read_attribute(state);
+read_attribute(cache_read_races);
+read_attribute(writeback_keys_done);
+read_attribute(writeback_keys_failed);
+read_attribute(io_errors);
+read_attribute(congested);
+rw_attribute(congested_read_threshold_us);
+rw_attribute(congested_write_threshold_us);
+
+rw_attribute(sequential_cutoff);
+rw_attribute(sequential_merge);
+rw_attribute(data_csum);
+rw_attribute(cache_mode);
+rw_attribute(writeback_metadata);
+rw_attribute(writeback_running);
+rw_attribute(writeback_percent);
+rw_attribute(writeback_delay);
+rw_attribute(writeback_rate);
+
+rw_attribute(writeback_rate_update_seconds);
+rw_attribute(writeback_rate_d_term);
+rw_attribute(writeback_rate_p_term_inverse);
+rw_attribute(writeback_rate_d_smooth);
+read_attribute(writeback_rate_debug);
+
+rw_attribute(synchronous);
+rw_attribute(journal_delay_ms);
+rw_attribute(discard);
+rw_attribute(running);
+rw_attribute(label);
+rw_attribute(readahead);
+rw_attribute(io_error_limit);
+rw_attribute(io_error_halflife);
+rw_attribute(verify);
+rw_attribute(key_merging_disabled);
+rw_attribute(gc_always_rewrite);
+rw_attribute(freelist_percent);
+rw_attribute(cache_replacement_policy);
+rw_attribute(btree_shrinker_disabled);
+rw_attribute(copy_gc_enabled);
+rw_attribute(size);
+
+SHOW(__bch_cached_dev)
+{
+	struct cached_dev *dc = container_of(kobj, struct cached_dev,
+					     disk.kobj);
+	const char *states[] = { "no cache", "clean", "dirty", "inconsistent" };
+
+#define var(stat)		(dc->stat)
+
+	if (attr == &sysfs_cache_mode)
+		return bch_snprint_string_list(buf, PAGE_SIZE,
+					       bch_cache_modes + 1,
+					       BDEV_CACHE_MODE(&dc->sb));
+
+	sysfs_printf(data_csum,		"%i", dc->disk.data_csum);
+	var_printf(verify,		"%i");
+	var_printf(writeback_metadata,	"%i");
+	var_printf(writeback_running,	"%i");
+	var_print(writeback_delay);
+	var_print(writeback_percent);
+	sysfs_print(writeback_rate,	dc->writeback_rate.rate);
+
+	var_print(writeback_rate_update_seconds);
+	var_print(writeback_rate_d_term);
+	var_print(writeback_rate_p_term_inverse);
+	var_print(writeback_rate_d_smooth);
+
+	if (attr == &sysfs_writeback_rate_debug) {
+		char dirty[20];
+		char derivative[20];
+		char target[20];
+		bch_hprint(dirty,
+		       atomic_long_read(&dc->disk.sectors_dirty) << 9);
+		bch_hprint(derivative,	dc->writeback_rate_derivative << 9);
+		bch_hprint(target,	dc->writeback_rate_target << 9);
+
+		return sprintf(buf,
+			       "rate:\t\t%u\n"
+			       "change:\t\t%i\n"
+			       "dirty:\t\t%s\n"
+			       "derivative:\t%s\n"
+			       "target:\t\t%s\n",
+			       dc->writeback_rate.rate,
+			       dc->writeback_rate_change,
+			       dirty, derivative, target);
+	}
+
+	sysfs_hprint(dirty_data,
+		     atomic_long_read(&dc->disk.sectors_dirty) << 9);
+
+	var_printf(sequential_merge,	"%i");
+	var_hprint(sequential_cutoff);
+	var_hprint(readahead);
+
+	sysfs_print(running,		atomic_read(&dc->running));
+	sysfs_print(state,		states[BDEV_STATE(&dc->sb)]);
+
+	if (attr == &sysfs_label) {
+		memcpy(buf, dc->sb.label, SB_LABEL_SIZE);
+		buf[SB_LABEL_SIZE + 1] = '\0';
+		strcat(buf, "\n");
+		return strlen(buf);
+	}
+
+#undef var
+	return 0;
+}
+SHOW_LOCKED(bch_cached_dev)
+
+STORE(__cached_dev)
+{
+	struct cached_dev *dc = container_of(kobj, struct cached_dev,
+					     disk.kobj);
+	unsigned v = size;
+	struct cache_set *c;
+
+#define d_strtoul(var)		sysfs_strtoul(var, dc->var)
+#define d_strtoi_h(var)		sysfs_hatoi(var, dc->var)
+
+	sysfs_strtoul(data_csum,	dc->disk.data_csum);
+	d_strtoul(verify);
+	d_strtoul(writeback_metadata);
+	d_strtoul(writeback_running);
+	d_strtoul(writeback_delay);
+	sysfs_strtoul_clamp(writeback_rate,
+			    dc->writeback_rate.rate, 1, 1000000);
+	sysfs_strtoul_clamp(writeback_percent, dc->writeback_percent, 0, 40);
+
+	d_strtoul(writeback_rate_update_seconds);
+	d_strtoul(writeback_rate_d_term);
+	d_strtoul(writeback_rate_p_term_inverse);
+	sysfs_strtoul_clamp(writeback_rate_p_term_inverse,
+			    dc->writeback_rate_p_term_inverse, 1, INT_MAX);
+	d_strtoul(writeback_rate_d_smooth);
+
+	d_strtoul(sequential_merge);
+	d_strtoi_h(sequential_cutoff);
+	d_strtoi_h(readahead);
+
+	if (attr == &sysfs_clear_stats)
+		bch_cache_accounting_clear(&dc->accounting);
+
+	if (attr == &sysfs_running &&
+	    strtoul_or_return(buf))
+		bch_cached_dev_run(dc);
+
+	if (attr == &sysfs_cache_mode) {
+		ssize_t v = bch_read_string_list(buf, bch_cache_modes + 1);
+
+		if (v < 0)
+			return v;
+
+		if ((unsigned) v != BDEV_CACHE_MODE(&dc->sb)) {
+			SET_BDEV_CACHE_MODE(&dc->sb, v);
+			bch_write_bdev_super(dc, NULL);
+		}
+	}
+
+	if (attr == &sysfs_label) {
+		memcpy(dc->sb.label, buf, SB_LABEL_SIZE);
+		bch_write_bdev_super(dc, NULL);
+		if (dc->disk.c) {
+			memcpy(dc->disk.c->uuids[dc->disk.id].label,
+			       buf, SB_LABEL_SIZE);
+			bch_uuid_write(dc->disk.c);
+		}
+	}
+
+	if (attr == &sysfs_attach) {
+		if (bch_parse_uuid(buf, dc->sb.set_uuid) < 16)
+			return -EINVAL;
+
+		list_for_each_entry(c, &bch_cache_sets, list) {
+			v = bch_cached_dev_attach(dc, c);
+			if (!v)
+				return size;
+		}
+
+		pr_err("Can't attach %s: cache set not found", buf);
+		size = v;
+	}
+
+	if (attr == &sysfs_detach && dc->disk.c)
+		bch_cached_dev_detach(dc);
+
+	if (attr == &sysfs_stop)
+		bcache_device_stop(&dc->disk);
+
+	return size;
+}
+
+STORE(bch_cached_dev)
+{
+	struct cached_dev *dc = container_of(kobj, struct cached_dev,
+					     disk.kobj);
+
+	mutex_lock(&bch_register_lock);
+	size = __cached_dev_store(kobj, attr, buf, size);
+
+	if (attr == &sysfs_writeback_running)
+		bch_writeback_queue(dc);
+
+	if (attr == &sysfs_writeback_percent)
+		schedule_delayed_work(&dc->writeback_rate_update,
+				      dc->writeback_rate_update_seconds * HZ);
+
+	mutex_unlock(&bch_register_lock);
+	return size;
+}
+
+static struct attribute *bch_cached_dev_files[] = {
+	&sysfs_attach,
+	&sysfs_detach,
+	&sysfs_stop,
+#if 0
+	&sysfs_data_csum,
+#endif
+	&sysfs_cache_mode,
+	&sysfs_writeback_metadata,
+	&sysfs_writeback_running,
+	&sysfs_writeback_delay,
+	&sysfs_writeback_percent,
+	&sysfs_writeback_rate,
+	&sysfs_writeback_rate_update_seconds,
+	&sysfs_writeback_rate_d_term,
+	&sysfs_writeback_rate_p_term_inverse,
+	&sysfs_writeback_rate_d_smooth,
+	&sysfs_writeback_rate_debug,
+	&sysfs_dirty_data,
+	&sysfs_sequential_cutoff,
+	&sysfs_sequential_merge,
+	&sysfs_clear_stats,
+	&sysfs_running,
+	&sysfs_state,
+	&sysfs_label,
+	&sysfs_readahead,
+#ifdef CONFIG_BCACHE_DEBUG
+	&sysfs_verify,
+#endif
+	NULL
+};
+KTYPE(bch_cached_dev);
+
+SHOW(bch_flash_dev)
+{
+	struct bcache_device *d = container_of(kobj, struct bcache_device,
+					       kobj);
+	struct uuid_entry *u = &d->c->uuids[d->id];
+
+	sysfs_printf(data_csum,	"%i", d->data_csum);
+	sysfs_hprint(size,	u->sectors << 9);
+
+	if (attr == &sysfs_label) {
+		memcpy(buf, u->label, SB_LABEL_SIZE);
+		buf[SB_LABEL_SIZE + 1] = '\0';
+		strcat(buf, "\n");
+		return strlen(buf);
+	}
+
+	return 0;
+}
+
+STORE(__bch_flash_dev)
+{
+	struct bcache_device *d = container_of(kobj, struct bcache_device,
+					       kobj);
+	struct uuid_entry *u = &d->c->uuids[d->id];
+
+	sysfs_strtoul(data_csum,	d->data_csum);
+
+	if (attr == &sysfs_size) {
+		uint64_t v;
+		strtoi_h_or_return(buf, v);
+
+		u->sectors = v >> 9;
+		bch_uuid_write(d->c);
+		set_capacity(d->disk, u->sectors);
+	}
+
+	if (attr == &sysfs_label) {
+		memcpy(u->label, buf, SB_LABEL_SIZE);
+		bch_uuid_write(d->c);
+	}
+
+	if (attr == &sysfs_unregister) {
+		atomic_set(&d->detaching, 1);
+		bcache_device_stop(d);
+	}
+
+	return size;
+}
+STORE_LOCKED(bch_flash_dev)
+
+static struct attribute *bch_flash_dev_files[] = {
+	&sysfs_unregister,
+#if 0
+	&sysfs_data_csum,
+#endif
+	&sysfs_label,
+	&sysfs_size,
+	NULL
+};
+KTYPE(bch_flash_dev);
+
+SHOW(__bch_cache_set)
+{
+	unsigned root_usage(struct cache_set *c)
+	{
+		unsigned bytes = 0;
+		struct bkey *k;
+		struct btree *b;
+		struct btree_iter iter;
+
+		goto lock_root;
+
+		do {
+			rw_unlock(false, b);
+lock_root:
+			b = c->root;
+			rw_lock(false, b, b->level);
+		} while (b != c->root);
+
+		for_each_key_filter(b, k, &iter, bch_ptr_bad)
+			bytes += bkey_bytes(k);
+
+		rw_unlock(false, b);
+
+		return (bytes * 100) / btree_bytes(c);
+	}
+
+	size_t cache_size(struct cache_set *c)
+	{
+		size_t ret = 0;
+		struct btree *b;
+
+		mutex_lock(&c->bucket_lock);
+		list_for_each_entry(b, &c->btree_cache, list)
+			ret += 1 << (b->page_order + PAGE_SHIFT);
+
+		mutex_unlock(&c->bucket_lock);
+		return ret;
+	}
+
+	unsigned cache_max_chain(struct cache_set *c)
+	{
+		unsigned ret = 0;
+		struct hlist_head *h;
+
+		mutex_lock(&c->bucket_lock);
+
+		for (h = c->bucket_hash;
+		     h < c->bucket_hash + (1 << BUCKET_HASH_BITS);
+		     h++) {
+			unsigned i = 0;
+			struct hlist_node *p;
+
+			hlist_for_each(p, h)
+				i++;
+
+			ret = max(ret, i);
+		}
+
+		mutex_unlock(&c->bucket_lock);
+		return ret;
+	}
+
+	unsigned btree_used(struct cache_set *c)
+	{
+		return div64_u64(c->gc_stats.key_bytes * 100,
+				 (c->gc_stats.nodes ?: 1) * btree_bytes(c));
+	}
+
+	unsigned average_key_size(struct cache_set *c)
+	{
+		return c->gc_stats.nkeys
+			? div64_u64(c->gc_stats.data, c->gc_stats.nkeys)
+			: 0;
+	}
+
+	struct cache_set *c = container_of(kobj, struct cache_set, kobj);
+
+	sysfs_print(synchronous,		CACHE_SYNC(&c->sb));
+	sysfs_print(journal_delay_ms,		c->journal_delay_ms);
+	sysfs_hprint(bucket_size,		bucket_bytes(c));
+	sysfs_hprint(block_size,		block_bytes(c));
+	sysfs_print(tree_depth,			c->root->level);
+	sysfs_print(root_usage_percent,		root_usage(c));
+
+	sysfs_hprint(btree_cache_size,		cache_size(c));
+	sysfs_print(btree_cache_max_chain,	cache_max_chain(c));
+	sysfs_print(cache_available_percent,	100 - c->gc_stats.in_use);
+
+	sysfs_print_time_stats(&c->btree_gc_time,	btree_gc, sec, ms);
+	sysfs_print_time_stats(&c->btree_split_time,	btree_split, sec, us);
+	sysfs_print_time_stats(&c->sort_time,		btree_sort, ms, us);
+	sysfs_print_time_stats(&c->btree_read_time,	btree_read, ms, us);
+	sysfs_print_time_stats(&c->try_harder_time,	try_harder, ms, us);
+
+	sysfs_print(btree_used_percent,	btree_used(c));
+	sysfs_print(btree_nodes,	c->gc_stats.nodes);
+	sysfs_hprint(dirty_data,	c->gc_stats.dirty);
+	sysfs_hprint(average_key_size,	average_key_size(c));
+
+	sysfs_print(cache_read_races,
+		    atomic_long_read(&c->cache_read_races));
+
+	sysfs_print(writeback_keys_done,
+		    atomic_long_read(&c->writeback_keys_done));
+	sysfs_print(writeback_keys_failed,
+		    atomic_long_read(&c->writeback_keys_failed));
+
+	/* See count_io_errors for why 88 */
+	sysfs_print(io_error_halflife,	c->error_decay * 88);
+	sysfs_print(io_error_limit,	c->error_limit >> IO_ERROR_SHIFT);
+
+	sysfs_hprint(congested,
+		     ((uint64_t) bch_get_congested(c)) << 9);
+	sysfs_print(congested_read_threshold_us,
+		    c->congested_read_threshold_us);
+	sysfs_print(congested_write_threshold_us,
+		    c->congested_write_threshold_us);
+
+	sysfs_print(active_journal_entries,	fifo_used(&c->journal.pin));
+	sysfs_printf(verify,			"%i", c->verify);
+	sysfs_printf(key_merging_disabled,	"%i", c->key_merging_disabled);
+	sysfs_printf(gc_always_rewrite,		"%i", c->gc_always_rewrite);
+	sysfs_printf(btree_shrinker_disabled,	"%i", c->shrinker_disabled);
+	sysfs_printf(copy_gc_enabled,		"%i", c->copy_gc_enabled);
+
+	if (attr == &sysfs_bset_tree_stats)
+		return bch_bset_print_stats(c, buf);
+
+	return 0;
+}
+SHOW_LOCKED(bch_cache_set)
+
+STORE(__bch_cache_set)
+{
+	struct cache_set *c = container_of(kobj, struct cache_set, kobj);
+
+	if (attr == &sysfs_unregister)
+		bch_cache_set_unregister(c);
+
+	if (attr == &sysfs_stop)
+		bch_cache_set_stop(c);
+
+	if (attr == &sysfs_synchronous) {
+		bool sync = strtoul_or_return(buf);
+
+		if (sync != CACHE_SYNC(&c->sb)) {
+			SET_CACHE_SYNC(&c->sb, sync);
+			bcache_write_super(c);
+		}
+	}
+
+	if (attr == &sysfs_flash_vol_create) {
+		int r;
+		uint64_t v;
+		strtoi_h_or_return(buf, v);
+
+		r = bch_flash_dev_create(c, v);
+		if (r)
+			return r;
+	}
+
+	if (attr == &sysfs_clear_stats) {
+		atomic_long_set(&c->writeback_keys_done,	0);
+		atomic_long_set(&c->writeback_keys_failed,	0);
+
+		memset(&c->gc_stats, 0, sizeof(struct gc_stat));
+		bch_cache_accounting_clear(&c->accounting);
+	}
+
+	if (attr == &sysfs_trigger_gc)
+		bch_queue_gc(c);
+
+	if (attr == &sysfs_prune_cache) {
+		struct shrink_control sc;
+		sc.gfp_mask = GFP_KERNEL;
+		sc.nr_to_scan = strtoul_or_return(buf);
+		c->shrink.shrink(&c->shrink, &sc);
+	}
+
+	sysfs_strtoul(congested_read_threshold_us,
+		      c->congested_read_threshold_us);
+	sysfs_strtoul(congested_write_threshold_us,
+		      c->congested_write_threshold_us);
+
+	if (attr == &sysfs_io_error_limit)
+		c->error_limit = strtoul_or_return(buf) << IO_ERROR_SHIFT;
+
+	/* See count_io_errors() for why 88 */
+	if (attr == &sysfs_io_error_halflife)
+		c->error_decay = strtoul_or_return(buf) / 88;
+
+	sysfs_strtoul(journal_delay_ms,		c->journal_delay_ms);
+	sysfs_strtoul(verify,			c->verify);
+	sysfs_strtoul(key_merging_disabled,	c->key_merging_disabled);
+	sysfs_strtoul(gc_always_rewrite,	c->gc_always_rewrite);
+	sysfs_strtoul(btree_shrinker_disabled,	c->shrinker_disabled);
+	sysfs_strtoul(copy_gc_enabled,		c->copy_gc_enabled);
+
+	return size;
+}
+STORE_LOCKED(bch_cache_set)
+
+SHOW(bch_cache_set_internal)
+{
+	struct cache_set *c = container_of(kobj, struct cache_set, internal);
+	return bch_cache_set_show(&c->kobj, attr, buf);
+}
+
+STORE(bch_cache_set_internal)
+{
+	struct cache_set *c = container_of(kobj, struct cache_set, internal);
+	return bch_cache_set_store(&c->kobj, attr, buf, size);
+}
+
+static void bch_cache_set_internal_release(struct kobject *k)
+{
+}
+
+static struct attribute *bch_cache_set_files[] = {
+	&sysfs_unregister,
+	&sysfs_stop,
+	&sysfs_synchronous,
+	&sysfs_journal_delay_ms,
+	&sysfs_flash_vol_create,
+
+	&sysfs_bucket_size,
+	&sysfs_block_size,
+	&sysfs_tree_depth,
+	&sysfs_root_usage_percent,
+	&sysfs_btree_cache_size,
+	&sysfs_cache_available_percent,
+
+	&sysfs_average_key_size,
+	&sysfs_dirty_data,
+
+	&sysfs_io_error_limit,
+	&sysfs_io_error_halflife,
+	&sysfs_congested,
+	&sysfs_congested_read_threshold_us,
+	&sysfs_congested_write_threshold_us,
+	&sysfs_clear_stats,
+	NULL
+};
+KTYPE(bch_cache_set);
+
+static struct attribute *bch_cache_set_internal_files[] = {
+	&sysfs_active_journal_entries,
+
+	sysfs_time_stats_attribute_list(btree_gc, sec, ms)
+	sysfs_time_stats_attribute_list(btree_split, sec, us)
+	sysfs_time_stats_attribute_list(btree_sort, ms, us)
+	sysfs_time_stats_attribute_list(btree_read, ms, us)
+	sysfs_time_stats_attribute_list(try_harder, ms, us)
+
+	&sysfs_btree_nodes,
+	&sysfs_btree_used_percent,
+	&sysfs_btree_cache_max_chain,
+
+	&sysfs_bset_tree_stats,
+	&sysfs_cache_read_races,
+	&sysfs_writeback_keys_done,
+	&sysfs_writeback_keys_failed,
+
+	&sysfs_trigger_gc,
+	&sysfs_prune_cache,
+#ifdef CONFIG_BCACHE_DEBUG
+	&sysfs_verify,
+	&sysfs_key_merging_disabled,
+#endif
+	&sysfs_gc_always_rewrite,
+	&sysfs_btree_shrinker_disabled,
+	&sysfs_copy_gc_enabled,
+	NULL
+};
+KTYPE(bch_cache_set_internal);
+
+SHOW(__bch_cache)
+{
+	struct cache *ca = container_of(kobj, struct cache, kobj);
+
+	sysfs_hprint(bucket_size,	bucket_bytes(ca));
+	sysfs_hprint(block_size,	block_bytes(ca));
+	sysfs_print(nbuckets,		ca->sb.nbuckets);
+	sysfs_print(discard,		ca->discard);
+	sysfs_hprint(written, atomic_long_read(&ca->sectors_written) << 9);
+	sysfs_hprint(btree_written,
+		     atomic_long_read(&ca->btree_sectors_written) << 9);
+	sysfs_hprint(metadata_written,
+		     (atomic_long_read(&ca->meta_sectors_written) +
+		      atomic_long_read(&ca->btree_sectors_written)) << 9);
+
+	sysfs_print(io_errors,
+		    atomic_read(&ca->io_errors) >> IO_ERROR_SHIFT);
+
+	sysfs_print(freelist_percent, ca->free.size * 100 /
+		    ((size_t) ca->sb.nbuckets));
+
+	if (attr == &sysfs_cache_replacement_policy)
+		return bch_snprint_string_list(buf, PAGE_SIZE,
+					       cache_replacement_policies,
+					       CACHE_REPLACEMENT(&ca->sb));
+
+	if (attr == &sysfs_priority_stats) {
+		int cmp(const void *l, const void *r)
+		{	return *((uint16_t *) r) - *((uint16_t *) l); }
+
+		/* Number of quantiles we compute */
+		const unsigned nq = 31;
+
+		size_t n = ca->sb.nbuckets, i, unused, btree;
+		uint64_t sum = 0;
+		uint16_t q[nq], *p, *cached;
+		ssize_t ret;
+
+		cached = p = vmalloc(ca->sb.nbuckets * sizeof(uint16_t));
+		if (!p)
+			return -ENOMEM;
+
+		mutex_lock(&ca->set->bucket_lock);
+		for (i = ca->sb.first_bucket; i < n; i++)
+			p[i] = ca->buckets[i].prio;
+		mutex_unlock(&ca->set->bucket_lock);
+
+		sort(p, n, sizeof(uint16_t), cmp, NULL);
+
+		while (n &&
+		       !cached[n - 1])
+			--n;
+
+		unused = ca->sb.nbuckets - n;
+
+		while (cached < p + n &&
+		       *cached == BTREE_PRIO)
+			cached++;
+
+		btree = cached - p;
+		n -= btree;
+
+		for (i = 0; i < n; i++)
+			sum += INITIAL_PRIO - cached[i];
+
+		if (n)
+			do_div(sum, n);
+
+		for (i = 0; i < nq; i++)
+			q[i] = INITIAL_PRIO - cached[n * (i + 1) / (nq + 1)];
+
+		vfree(p);
+
+		ret = snprintf(buf, PAGE_SIZE,
+			       "Unused:		%zu%%\n"
+			       "Metadata:	%zu%%\n"
+			       "Average:	%llu\n"
+			       "Sectors per Q:	%zu\n"
+			       "Quantiles:	[",
+			       unused * 100 / (size_t) ca->sb.nbuckets,
+			       btree * 100 / (size_t) ca->sb.nbuckets, sum,
+			       n * ca->sb.bucket_size / (nq + 1));
+
+		for (i = 0; i < nq && ret < (ssize_t) PAGE_SIZE; i++)
+			ret += snprintf(buf + ret, PAGE_SIZE - ret,
+					i < nq - 1 ? "%u " : "%u]\n", q[i]);
+
+		buf[PAGE_SIZE - 1] = '\0';
+		return ret;
+	}
+
+	return 0;
+}
+SHOW_LOCKED(bch_cache)
+
+STORE(__bch_cache)
+{
+	struct cache *ca = container_of(kobj, struct cache, kobj);
+
+	if (attr == &sysfs_discard) {
+		bool v = strtoul_or_return(buf);
+
+		if (blk_queue_discard(bdev_get_queue(ca->bdev)))
+			ca->discard = v;
+
+		if (v != CACHE_DISCARD(&ca->sb)) {
+			SET_CACHE_DISCARD(&ca->sb, v);
+			bcache_write_super(ca->set);
+		}
+	}
+
+	if (attr == &sysfs_cache_replacement_policy) {
+		ssize_t v = bch_read_string_list(buf, cache_replacement_policies);
+
+		if (v < 0)
+			return v;
+
+		if ((unsigned) v != CACHE_REPLACEMENT(&ca->sb)) {
+			mutex_lock(&ca->set->bucket_lock);
+			SET_CACHE_REPLACEMENT(&ca->sb, v);
+			mutex_unlock(&ca->set->bucket_lock);
+
+			bcache_write_super(ca->set);
+		}
+	}
+
+	if (attr == &sysfs_freelist_percent) {
+		DECLARE_FIFO(long, free);
+		long i;
+		size_t p = strtoul_or_return(buf);
+
+		p = clamp_t(size_t,
+			    ((size_t) ca->sb.nbuckets * p) / 100,
+			    roundup_pow_of_two(ca->sb.nbuckets) >> 9,
+			    ca->sb.nbuckets / 2);
+
+		if (!init_fifo_exact(&free, p, GFP_KERNEL))
+			return -ENOMEM;
+
+		mutex_lock(&ca->set->bucket_lock);
+
+		fifo_move(&free, &ca->free);
+		fifo_swap(&free, &ca->free);
+
+		mutex_unlock(&ca->set->bucket_lock);
+
+		while (fifo_pop(&free, i))
+			atomic_dec(&ca->buckets[i].pin);
+
+		free_fifo(&free);
+	}
+
+	if (attr == &sysfs_clear_stats) {
+		atomic_long_set(&ca->sectors_written, 0);
+		atomic_long_set(&ca->btree_sectors_written, 0);
+		atomic_long_set(&ca->meta_sectors_written, 0);
+		atomic_set(&ca->io_count, 0);
+		atomic_set(&ca->io_errors, 0);
+	}
+
+	return size;
+}
+STORE_LOCKED(bch_cache)
+
+static struct attribute *bch_cache_files[] = {
+	&sysfs_bucket_size,
+	&sysfs_block_size,
+	&sysfs_nbuckets,
+	&sysfs_priority_stats,
+	&sysfs_discard,
+	&sysfs_written,
+	&sysfs_btree_written,
+	&sysfs_metadata_written,
+	&sysfs_io_errors,
+	&sysfs_clear_stats,
+	&sysfs_freelist_percent,
+	&sysfs_cache_replacement_policy,
+	NULL
+};
+KTYPE(bch_cache);
diff --git a/drivers/md/bcache/sysfs.h b/drivers/md/bcache/sysfs.h
new file mode 100644
index 000000000000..0526fe92a683
--- /dev/null
+++ b/drivers/md/bcache/sysfs.h
@@ -0,0 +1,110 @@
+#ifndef _BCACHE_SYSFS_H_
+#define _BCACHE_SYSFS_H_
+
+#define KTYPE(type)							\
+struct kobj_type type ## _ktype = {					\
+	.release	= type ## _release,				\
+	.sysfs_ops	= &((const struct sysfs_ops) {			\
+		.show	= type ## _show,				\
+		.store	= type ## _store				\
+	}),								\
+	.default_attrs	= type ## _files				\
+}
+
+#define SHOW(fn)							\
+static ssize_t fn ## _show(struct kobject *kobj, struct attribute *attr,\
+			   char *buf)					\
+
+#define STORE(fn)							\
+static ssize_t fn ## _store(struct kobject *kobj, struct attribute *attr,\
+			    const char *buf, size_t size)		\
+
+#define SHOW_LOCKED(fn)							\
+SHOW(fn)								\
+{									\
+	ssize_t ret;							\
+	mutex_lock(&bch_register_lock);					\
+	ret = __ ## fn ## _show(kobj, attr, buf);			\
+	mutex_unlock(&bch_register_lock);				\
+	return ret;							\
+}
+
+#define STORE_LOCKED(fn)						\
+STORE(fn)								\
+{									\
+	ssize_t ret;							\
+	mutex_lock(&bch_register_lock);					\
+	ret = __ ## fn ## _store(kobj, attr, buf, size);		\
+	mutex_unlock(&bch_register_lock);				\
+	return ret;							\
+}
+
+#define __sysfs_attribute(_name, _mode)					\
+	static struct attribute sysfs_##_name =				\
+		{ .name = #_name, .mode = _mode }
+
+#define write_attribute(n)	__sysfs_attribute(n, S_IWUSR)
+#define read_attribute(n)	__sysfs_attribute(n, S_IRUGO)
+#define rw_attribute(n)		__sysfs_attribute(n, S_IRUGO|S_IWUSR)
+
+#define sysfs_printf(file, fmt, ...)					\
+do {									\
+	if (attr == &sysfs_ ## file)					\
+		return snprintf(buf, PAGE_SIZE, fmt "\n", __VA_ARGS__);	\
+} while (0)
+
+#define sysfs_print(file, var)						\
+do {									\
+	if (attr == &sysfs_ ## file)					\
+		return snprint(buf, PAGE_SIZE, var);			\
+} while (0)
+
+#define sysfs_hprint(file, val)						\
+do {									\
+	if (attr == &sysfs_ ## file) {					\
+		ssize_t ret = bch_hprint(buf, val);			\
+		strcat(buf, "\n");					\
+		return ret + 1;						\
+	}								\
+} while (0)
+
+#define var_printf(_var, fmt)	sysfs_printf(_var, fmt, var(_var))
+#define var_print(_var)		sysfs_print(_var, var(_var))
+#define var_hprint(_var)	sysfs_hprint(_var, var(_var))
+
+#define sysfs_strtoul(file, var)					\
+do {									\
+	if (attr == &sysfs_ ## file)					\
+		return strtoul_safe(buf, var) ?: (ssize_t) size;	\
+} while (0)
+
+#define sysfs_strtoul_clamp(file, var, min, max)			\
+do {									\
+	if (attr == &sysfs_ ## file)					\
+		return strtoul_safe_clamp(buf, var, min, max)		\
+			?: (ssize_t) size;				\
+} while (0)
+
+#define strtoul_or_return(cp)						\
+({									\
+	unsigned long _v;						\
+	int _r = kstrtoul(cp, 10, &_v);					\
+	if (_r)								\
+		return _r;						\
+	_v;								\
+})
+
+#define strtoi_h_or_return(cp, v)					\
+do {									\
+	int _r = strtoi_h(cp, &v);					\
+	if (_r)								\
+		return _r;						\
+} while (0)
+
+#define sysfs_hatoi(file, var)						\
+do {									\
+	if (attr == &sysfs_ ## file)					\
+		return strtoi_h(buf, &var) ?: (ssize_t) size;		\
+} while (0)
+
+#endif  /* _BCACHE_SYSFS_H_ */
diff --git a/drivers/md/bcache/trace.c b/drivers/md/bcache/trace.c
new file mode 100644
index 000000000000..983f9bb411bc
--- /dev/null
+++ b/drivers/md/bcache/trace.c
@@ -0,0 +1,26 @@
+#include "bcache.h"
+#include "btree.h"
+#include "request.h"
+
+#include <linux/module.h>
+
+#define CREATE_TRACE_POINTS
+#include <trace/events/bcache.h>
+
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_request_start);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_request_end);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_passthrough);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_cache_hit);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_cache_miss);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_read_retry);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_writethrough);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_writeback);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_write_skip);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_btree_read);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_btree_write);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_write_dirty);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_read_dirty);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_journal_write);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_cache_insert);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_gc_start);
+EXPORT_TRACEPOINT_SYMBOL_GPL(bcache_gc_end);
diff --git a/drivers/md/bcache/util.c b/drivers/md/bcache/util.c
new file mode 100644
index 000000000000..da3a99e85b1e
--- /dev/null
+++ b/drivers/md/bcache/util.c
@@ -0,0 +1,377 @@
+/*
+ * random utiility code, for bcache but in theory not specific to bcache
+ *
+ * Copyright 2010, 2011 Kent Overstreet <kent.overstreet@gmail.com>
+ * Copyright 2012 Google, Inc.
+ */
+
+#include <linux/bio.h>
+#include <linux/blkdev.h>
+#include <linux/ctype.h>
+#include <linux/debugfs.h>
+#include <linux/module.h>
+#include <linux/seq_file.h>
+#include <linux/types.h>
+
+#include "util.h"
+
+#define simple_strtoint(c, end, base)	simple_strtol(c, end, base)
+#define simple_strtouint(c, end, base)	simple_strtoul(c, end, base)
+
+#define STRTO_H(name, type)					\
+int bch_ ## name ## _h(const char *cp, type *res)		\
+{								\
+	int u = 0;						\
+	char *e;						\
+	type i = simple_ ## name(cp, &e, 10);			\
+								\
+	switch (tolower(*e)) {					\
+	default:						\
+		return -EINVAL;					\
+	case 'y':						\
+	case 'z':						\
+		u++;						\
+	case 'e':						\
+		u++;						\
+	case 'p':						\
+		u++;						\
+	case 't':						\
+		u++;						\
+	case 'g':						\
+		u++;						\
+	case 'm':						\
+		u++;						\
+	case 'k':						\
+		u++;						\
+		if (e++ == cp)					\
+			return -EINVAL;				\
+	case '\n':						\
+	case '\0':						\
+		if (*e == '\n')					\
+			e++;					\
+	}							\
+								\
+	if (*e)							\
+		return -EINVAL;					\
+								\
+	while (u--) {						\
+		if ((type) ~0 > 0 &&				\
+		    (type) ~0 / 1024 <= i)			\
+			return -EINVAL;				\
+		if ((i > 0 && ANYSINT_MAX(type) / 1024 < i) ||	\
+		    (i < 0 && -ANYSINT_MAX(type) / 1024 > i))	\
+			return -EINVAL;				\
+		i *= 1024;					\
+	}							\
+								\
+	*res = i;						\
+	return 0;						\
+}								\
+
+STRTO_H(strtoint, int)
+STRTO_H(strtouint, unsigned int)
+STRTO_H(strtoll, long long)
+STRTO_H(strtoull, unsigned long long)
+
+ssize_t bch_hprint(char *buf, int64_t v)
+{
+	static const char units[] = "?kMGTPEZY";
+	char dec[4] = "";
+	int u, t = 0;
+
+	for (u = 0; v >= 1024 || v <= -1024; u++) {
+		t = v & ~(~0 << 10);
+		v >>= 10;
+	}
+
+	if (!u)
+		return sprintf(buf, "%llu", v);
+
+	if (v < 100 && v > -100)
+		snprintf(dec, sizeof(dec), ".%i", t / 100);
+
+	return sprintf(buf, "%lli%s%c", v, dec, units[u]);
+}
+
+ssize_t bch_snprint_string_list(char *buf, size_t size, const char * const list[],
+			    size_t selected)
+{
+	char *out = buf;
+	size_t i;
+
+	for (i = 0; list[i]; i++)
+		out += snprintf(out, buf + size - out,
+				i == selected ? "[%s] " : "%s ", list[i]);
+
+	out[-1] = '\n';
+	return out - buf;
+}
+
+ssize_t bch_read_string_list(const char *buf, const char * const list[])
+{
+	size_t i;
+	char *s, *d = kstrndup(buf, PAGE_SIZE - 1, GFP_KERNEL);
+	if (!d)
+		return -ENOMEM;
+
+	s = strim(d);
+
+	for (i = 0; list[i]; i++)
+		if (!strcmp(list[i], s))
+			break;
+
+	kfree(d);
+
+	if (!list[i])
+		return -EINVAL;
+
+	return i;
+}
+
+bool bch_is_zero(const char *p, size_t n)
+{
+	size_t i;
+
+	for (i = 0; i < n; i++)
+		if (p[i])
+			return false;
+	return true;
+}
+
+int bch_parse_uuid(const char *s, char *uuid)
+{
+	size_t i, j, x;
+	memset(uuid, 0, 16);
+
+	for (i = 0, j = 0;
+	     i < strspn(s, "-0123456789:ABCDEFabcdef") && j < 32;
+	     i++) {
+		x = s[i] | 32;
+
+		switch (x) {
+		case '0'...'9':
+			x -= '0';
+			break;
+		case 'a'...'f':
+			x -= 'a' - 10;
+			break;
+		default:
+			continue;
+		}
+
+		if (!(j & 1))
+			x <<= 4;
+		uuid[j++ >> 1] |= x;
+	}
+	return i;
+}
+
+void bch_time_stats_update(struct time_stats *stats, uint64_t start_time)
+{
+	uint64_t now		= local_clock();
+	uint64_t duration	= time_after64(now, start_time)
+		? now - start_time : 0;
+	uint64_t last		= time_after64(now, stats->last)
+		? now - stats->last : 0;
+
+	stats->max_duration = max(stats->max_duration, duration);
+
+	if (stats->last) {
+		ewma_add(stats->average_duration, duration, 8, 8);
+
+		if (stats->average_frequency)
+			ewma_add(stats->average_frequency, last, 8, 8);
+		else
+			stats->average_frequency  = last << 8;
+	} else {
+		stats->average_duration  = duration << 8;
+	}
+
+	stats->last = now ?: 1;
+}
+
+unsigned bch_next_delay(struct ratelimit *d, uint64_t done)
+{
+	uint64_t now = local_clock();
+
+	d->next += div_u64(done, d->rate);
+
+	return time_after64(d->next, now)
+		? div_u64(d->next - now, NSEC_PER_SEC / HZ)
+		: 0;
+}
+
+void bch_bio_map(struct bio *bio, void *base)
+{
+	size_t size = bio->bi_size;
+	struct bio_vec *bv = bio->bi_io_vec;
+
+	BUG_ON(!bio->bi_size);
+	BUG_ON(bio->bi_vcnt);
+
+	bv->bv_offset = base ? ((unsigned long) base) % PAGE_SIZE : 0;
+	goto start;
+
+	for (; size; bio->bi_vcnt++, bv++) {
+		bv->bv_offset	= 0;
+start:		bv->bv_len	= min_t(size_t, PAGE_SIZE - bv->bv_offset,
+					size);
+		if (base) {
+			bv->bv_page = is_vmalloc_addr(base)
+				? vmalloc_to_page(base)
+				: virt_to_page(base);
+
+			base += bv->bv_len;
+		}
+
+		size -= bv->bv_len;
+	}
+}
+
+int bch_bio_alloc_pages(struct bio *bio, gfp_t gfp)
+{
+	int i;
+	struct bio_vec *bv;
+
+	bio_for_each_segment(bv, bio, i) {
+		bv->bv_page = alloc_page(gfp);
+		if (!bv->bv_page) {
+			while (bv-- != bio->bi_io_vec + bio->bi_idx)
+				__free_page(bv->bv_page);
+			return -ENOMEM;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Portions Copyright (c) 1996-2001, PostgreSQL Global Development Group (Any
+ * use permitted, subject to terms of PostgreSQL license; see.)
+
+ * If we have a 64-bit integer type, then a 64-bit CRC looks just like the
+ * usual sort of implementation. (See Ross Williams' excellent introduction
+ * A PAINLESS GUIDE TO CRC ERROR DETECTION ALGORITHMS, available from
+ * ftp://ftp.rocksoft.com/papers/crc_v3.txt or several other net sites.)
+ * If we have no working 64-bit type, then fake it with two 32-bit registers.
+ *
+ * The present implementation is a normal (not "reflected", in Williams'
+ * terms) 64-bit CRC, using initial all-ones register contents and a final
+ * bit inversion. The chosen polynomial is borrowed from the DLT1 spec
+ * (ECMA-182, available from http://www.ecma.ch/ecma1/STAND/ECMA-182.HTM):
+ *
+ * x^64 + x^62 + x^57 + x^55 + x^54 + x^53 + x^52 + x^47 + x^46 + x^45 +
+ * x^40 + x^39 + x^38 + x^37 + x^35 + x^33 + x^32 + x^31 + x^29 + x^27 +
+ * x^24 + x^23 + x^22 + x^21 + x^19 + x^17 + x^13 + x^12 + x^10 + x^9 +
+ * x^7 + x^4 + x + 1
+*/
+
+static const uint64_t crc_table[256] = {
+	0x0000000000000000ULL, 0x42F0E1EBA9EA3693ULL, 0x85E1C3D753D46D26ULL,
+	0xC711223CFA3E5BB5ULL, 0x493366450E42ECDFULL, 0x0BC387AEA7A8DA4CULL,
+	0xCCD2A5925D9681F9ULL, 0x8E224479F47CB76AULL, 0x9266CC8A1C85D9BEULL,
+	0xD0962D61B56FEF2DULL, 0x17870F5D4F51B498ULL, 0x5577EEB6E6BB820BULL,
+	0xDB55AACF12C73561ULL, 0x99A54B24BB2D03F2ULL, 0x5EB4691841135847ULL,
+	0x1C4488F3E8F96ED4ULL, 0x663D78FF90E185EFULL, 0x24CD9914390BB37CULL,
+	0xE3DCBB28C335E8C9ULL, 0xA12C5AC36ADFDE5AULL, 0x2F0E1EBA9EA36930ULL,
+	0x6DFEFF5137495FA3ULL, 0xAAEFDD6DCD770416ULL, 0xE81F3C86649D3285ULL,
+	0xF45BB4758C645C51ULL, 0xB6AB559E258E6AC2ULL, 0x71BA77A2DFB03177ULL,
+	0x334A9649765A07E4ULL, 0xBD68D2308226B08EULL, 0xFF9833DB2BCC861DULL,
+	0x388911E7D1F2DDA8ULL, 0x7A79F00C7818EB3BULL, 0xCC7AF1FF21C30BDEULL,
+	0x8E8A101488293D4DULL, 0x499B3228721766F8ULL, 0x0B6BD3C3DBFD506BULL,
+	0x854997BA2F81E701ULL, 0xC7B97651866BD192ULL, 0x00A8546D7C558A27ULL,
+	0x4258B586D5BFBCB4ULL, 0x5E1C3D753D46D260ULL, 0x1CECDC9E94ACE4F3ULL,
+	0xDBFDFEA26E92BF46ULL, 0x990D1F49C77889D5ULL, 0x172F5B3033043EBFULL,
+	0x55DFBADB9AEE082CULL, 0x92CE98E760D05399ULL, 0xD03E790CC93A650AULL,
+	0xAA478900B1228E31ULL, 0xE8B768EB18C8B8A2ULL, 0x2FA64AD7E2F6E317ULL,
+	0x6D56AB3C4B1CD584ULL, 0xE374EF45BF6062EEULL, 0xA1840EAE168A547DULL,
+	0x66952C92ECB40FC8ULL, 0x2465CD79455E395BULL, 0x3821458AADA7578FULL,
+	0x7AD1A461044D611CULL, 0xBDC0865DFE733AA9ULL, 0xFF3067B657990C3AULL,
+	0x711223CFA3E5BB50ULL, 0x33E2C2240A0F8DC3ULL, 0xF4F3E018F031D676ULL,
+	0xB60301F359DBE0E5ULL, 0xDA050215EA6C212FULL, 0x98F5E3FE438617BCULL,
+	0x5FE4C1C2B9B84C09ULL, 0x1D14202910527A9AULL, 0x93366450E42ECDF0ULL,
+	0xD1C685BB4DC4FB63ULL, 0x16D7A787B7FAA0D6ULL, 0x5427466C1E109645ULL,
+	0x4863CE9FF6E9F891ULL, 0x0A932F745F03CE02ULL, 0xCD820D48A53D95B7ULL,
+	0x8F72ECA30CD7A324ULL, 0x0150A8DAF8AB144EULL, 0x43A04931514122DDULL,
+	0x84B16B0DAB7F7968ULL, 0xC6418AE602954FFBULL, 0xBC387AEA7A8DA4C0ULL,
+	0xFEC89B01D3679253ULL, 0x39D9B93D2959C9E6ULL, 0x7B2958D680B3FF75ULL,
+	0xF50B1CAF74CF481FULL, 0xB7FBFD44DD257E8CULL, 0x70EADF78271B2539ULL,
+	0x321A3E938EF113AAULL, 0x2E5EB66066087D7EULL, 0x6CAE578BCFE24BEDULL,
+	0xABBF75B735DC1058ULL, 0xE94F945C9C3626CBULL, 0x676DD025684A91A1ULL,
+	0x259D31CEC1A0A732ULL, 0xE28C13F23B9EFC87ULL, 0xA07CF2199274CA14ULL,
+	0x167FF3EACBAF2AF1ULL, 0x548F120162451C62ULL, 0x939E303D987B47D7ULL,
+	0xD16ED1D631917144ULL, 0x5F4C95AFC5EDC62EULL, 0x1DBC74446C07F0BDULL,
+	0xDAAD56789639AB08ULL, 0x985DB7933FD39D9BULL, 0x84193F60D72AF34FULL,
+	0xC6E9DE8B7EC0C5DCULL, 0x01F8FCB784FE9E69ULL, 0x43081D5C2D14A8FAULL,
+	0xCD2A5925D9681F90ULL, 0x8FDAB8CE70822903ULL, 0x48CB9AF28ABC72B6ULL,
+	0x0A3B7B1923564425ULL, 0x70428B155B4EAF1EULL, 0x32B26AFEF2A4998DULL,
+	0xF5A348C2089AC238ULL, 0xB753A929A170F4ABULL, 0x3971ED50550C43C1ULL,
+	0x7B810CBBFCE67552ULL, 0xBC902E8706D82EE7ULL, 0xFE60CF6CAF321874ULL,
+	0xE224479F47CB76A0ULL, 0xA0D4A674EE214033ULL, 0x67C58448141F1B86ULL,
+	0x253565A3BDF52D15ULL, 0xAB1721DA49899A7FULL, 0xE9E7C031E063ACECULL,
+	0x2EF6E20D1A5DF759ULL, 0x6C0603E6B3B7C1CAULL, 0xF6FAE5C07D3274CDULL,
+	0xB40A042BD4D8425EULL, 0x731B26172EE619EBULL, 0x31EBC7FC870C2F78ULL,
+	0xBFC9838573709812ULL, 0xFD39626EDA9AAE81ULL, 0x3A28405220A4F534ULL,
+	0x78D8A1B9894EC3A7ULL, 0x649C294A61B7AD73ULL, 0x266CC8A1C85D9BE0ULL,
+	0xE17DEA9D3263C055ULL, 0xA38D0B769B89F6C6ULL, 0x2DAF4F0F6FF541ACULL,
+	0x6F5FAEE4C61F773FULL, 0xA84E8CD83C212C8AULL, 0xEABE6D3395CB1A19ULL,
+	0x90C79D3FEDD3F122ULL, 0xD2377CD44439C7B1ULL, 0x15265EE8BE079C04ULL,
+	0x57D6BF0317EDAA97ULL, 0xD9F4FB7AE3911DFDULL, 0x9B041A914A7B2B6EULL,
+	0x5C1538ADB04570DBULL, 0x1EE5D94619AF4648ULL, 0x02A151B5F156289CULL,
+	0x4051B05E58BC1E0FULL, 0x87409262A28245BAULL, 0xC5B073890B687329ULL,
+	0x4B9237F0FF14C443ULL, 0x0962D61B56FEF2D0ULL, 0xCE73F427ACC0A965ULL,
+	0x8C8315CC052A9FF6ULL, 0x3A80143F5CF17F13ULL, 0x7870F5D4F51B4980ULL,
+	0xBF61D7E80F251235ULL, 0xFD913603A6CF24A6ULL, 0x73B3727A52B393CCULL,
+	0x31439391FB59A55FULL, 0xF652B1AD0167FEEAULL, 0xB4A25046A88DC879ULL,
+	0xA8E6D8B54074A6ADULL, 0xEA16395EE99E903EULL, 0x2D071B6213A0CB8BULL,
+	0x6FF7FA89BA4AFD18ULL, 0xE1D5BEF04E364A72ULL, 0xA3255F1BE7DC7CE1ULL,
+	0x64347D271DE22754ULL, 0x26C49CCCB40811C7ULL, 0x5CBD6CC0CC10FAFCULL,
+	0x1E4D8D2B65FACC6FULL, 0xD95CAF179FC497DAULL, 0x9BAC4EFC362EA149ULL,
+	0x158E0A85C2521623ULL, 0x577EEB6E6BB820B0ULL, 0x906FC95291867B05ULL,
+	0xD29F28B9386C4D96ULL, 0xCEDBA04AD0952342ULL, 0x8C2B41A1797F15D1ULL,
+	0x4B3A639D83414E64ULL, 0x09CA82762AAB78F7ULL, 0x87E8C60FDED7CF9DULL,
+	0xC51827E4773DF90EULL, 0x020905D88D03A2BBULL, 0x40F9E43324E99428ULL,
+	0x2CFFE7D5975E55E2ULL, 0x6E0F063E3EB46371ULL, 0xA91E2402C48A38C4ULL,
+	0xEBEEC5E96D600E57ULL, 0x65CC8190991CB93DULL, 0x273C607B30F68FAEULL,
+	0xE02D4247CAC8D41BULL, 0xA2DDA3AC6322E288ULL, 0xBE992B5F8BDB8C5CULL,
+	0xFC69CAB42231BACFULL, 0x3B78E888D80FE17AULL, 0x7988096371E5D7E9ULL,
+	0xF7AA4D1A85996083ULL, 0xB55AACF12C735610ULL, 0x724B8ECDD64D0DA5ULL,
+	0x30BB6F267FA73B36ULL, 0x4AC29F2A07BFD00DULL, 0x08327EC1AE55E69EULL,
+	0xCF235CFD546BBD2BULL, 0x8DD3BD16FD818BB8ULL, 0x03F1F96F09FD3CD2ULL,
+	0x41011884A0170A41ULL, 0x86103AB85A2951F4ULL, 0xC4E0DB53F3C36767ULL,
+	0xD8A453A01B3A09B3ULL, 0x9A54B24BB2D03F20ULL, 0x5D45907748EE6495ULL,
+	0x1FB5719CE1045206ULL, 0x919735E51578E56CULL, 0xD367D40EBC92D3FFULL,
+	0x1476F63246AC884AULL, 0x568617D9EF46BED9ULL, 0xE085162AB69D5E3CULL,
+	0xA275F7C11F7768AFULL, 0x6564D5FDE549331AULL, 0x279434164CA30589ULL,
+	0xA9B6706FB8DFB2E3ULL, 0xEB46918411358470ULL, 0x2C57B3B8EB0BDFC5ULL,
+	0x6EA7525342E1E956ULL, 0x72E3DAA0AA188782ULL, 0x30133B4B03F2B111ULL,
+	0xF7021977F9CCEAA4ULL, 0xB5F2F89C5026DC37ULL, 0x3BD0BCE5A45A6B5DULL,
+	0x79205D0E0DB05DCEULL, 0xBE317F32F78E067BULL, 0xFCC19ED95E6430E8ULL,
+	0x86B86ED5267CDBD3ULL, 0xC4488F3E8F96ED40ULL, 0x0359AD0275A8B6F5ULL,
+	0x41A94CE9DC428066ULL, 0xCF8B0890283E370CULL, 0x8D7BE97B81D4019FULL,
+	0x4A6ACB477BEA5A2AULL, 0x089A2AACD2006CB9ULL, 0x14DEA25F3AF9026DULL,
+	0x562E43B4931334FEULL, 0x913F6188692D6F4BULL, 0xD3CF8063C0C759D8ULL,
+	0x5DEDC41A34BBEEB2ULL, 0x1F1D25F19D51D821ULL, 0xD80C07CD676F8394ULL,
+	0x9AFCE626CE85B507ULL,
+};
+
+uint64_t bch_crc64_update(uint64_t crc, const void *_data, size_t len)
+{
+	const unsigned char *data = _data;
+
+	while (len--) {
+		int i = ((int) (crc >> 56) ^ *data++) & 0xFF;
+		crc = crc_table[i] ^ (crc << 8);
+	}
+
+	return crc;
+}
+
+uint64_t bch_crc64(const void *data, size_t len)
+{
+	uint64_t crc = 0xffffffffffffffffULL;
+
+	crc = bch_crc64_update(crc, data, len);
+
+	return crc ^ 0xffffffffffffffffULL;
+}
diff --git a/drivers/md/bcache/util.h b/drivers/md/bcache/util.h
new file mode 100644
index 000000000000..577393e38c3a
--- /dev/null
+++ b/drivers/md/bcache/util.h
@@ -0,0 +1,589 @@
+
+#ifndef _BCACHE_UTIL_H
+#define _BCACHE_UTIL_H
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/llist.h>
+#include <linux/ratelimit.h>
+#include <linux/vmalloc.h>
+#include <linux/workqueue.h>
+
+#include "closure.h"
+
+#define PAGE_SECTORS		(PAGE_SIZE / 512)
+
+struct closure;
+
+#include <trace/events/bcache.h>
+
+#ifdef CONFIG_BCACHE_EDEBUG
+
+#define atomic_dec_bug(v)	BUG_ON(atomic_dec_return(v) < 0)
+#define atomic_inc_bug(v, i)	BUG_ON(atomic_inc_return(v) <= i)
+
+#else /* EDEBUG */
+
+#define atomic_dec_bug(v)	atomic_dec(v)
+#define atomic_inc_bug(v, i)	atomic_inc(v)
+
+#endif
+
+#define BITMASK(name, type, field, offset, size)		\
+static inline uint64_t name(const type *k)			\
+{ return (k->field >> offset) & ~(((uint64_t) ~0) << size); }	\
+								\
+static inline void SET_##name(type *k, uint64_t v)		\
+{								\
+	k->field &= ~(~((uint64_t) ~0 << size) << offset);	\
+	k->field |= v << offset;				\
+}
+
+#define DECLARE_HEAP(type, name)					\
+	struct {							\
+		size_t size, used;					\
+		type *data;						\
+	} name
+
+#define init_heap(heap, _size, gfp)					\
+({									\
+	size_t _bytes;							\
+	(heap)->used = 0;						\
+	(heap)->size = (_size);						\
+	_bytes = (heap)->size * sizeof(*(heap)->data);			\
+	(heap)->data = NULL;						\
+	if (_bytes < KMALLOC_MAX_SIZE)					\
+		(heap)->data = kmalloc(_bytes, (gfp));			\
+	if ((!(heap)->data) && ((gfp) & GFP_KERNEL))			\
+		(heap)->data = vmalloc(_bytes);				\
+	(heap)->data;							\
+})
+
+#define free_heap(heap)							\
+do {									\
+	if (is_vmalloc_addr((heap)->data))				\
+		vfree((heap)->data);					\
+	else								\
+		kfree((heap)->data);					\
+	(heap)->data = NULL;						\
+} while (0)
+
+#define heap_swap(h, i, j)	swap((h)->data[i], (h)->data[j])
+
+#define heap_sift(h, i, cmp)						\
+do {									\
+	size_t _r, _j = i;						\
+									\
+	for (; _j * 2 + 1 < (h)->used; _j = _r) {			\
+		_r = _j * 2 + 1;					\
+		if (_r + 1 < (h)->used &&				\
+		    cmp((h)->data[_r], (h)->data[_r + 1]))		\
+			_r++;						\
+									\
+		if (cmp((h)->data[_r], (h)->data[_j]))			\
+			break;						\
+		heap_swap(h, _r, _j);					\
+	}								\
+} while (0)
+
+#define heap_sift_down(h, i, cmp)					\
+do {									\
+	while (i) {							\
+		size_t p = (i - 1) / 2;					\
+		if (cmp((h)->data[i], (h)->data[p]))			\
+			break;						\
+		heap_swap(h, i, p);					\
+		i = p;							\
+	}								\
+} while (0)
+
+#define heap_add(h, d, cmp)						\
+({									\
+	bool _r = !heap_full(h);					\
+	if (_r) {							\
+		size_t _i = (h)->used++;				\
+		(h)->data[_i] = d;					\
+									\
+		heap_sift_down(h, _i, cmp);				\
+		heap_sift(h, _i, cmp);					\
+	}								\
+	_r;								\
+})
+
+#define heap_pop(h, d, cmp)						\
+({									\
+	bool _r = (h)->used;						\
+	if (_r) {							\
+		(d) = (h)->data[0];					\
+		(h)->used--;						\
+		heap_swap(h, 0, (h)->used);				\
+		heap_sift(h, 0, cmp);					\
+	}								\
+	_r;								\
+})
+
+#define heap_peek(h)	((h)->size ? (h)->data[0] : NULL)
+
+#define heap_full(h)	((h)->used == (h)->size)
+
+#define DECLARE_FIFO(type, name)					\
+	struct {							\
+		size_t front, back, size, mask;				\
+		type *data;						\
+	} name
+
+#define fifo_for_each(c, fifo, iter)					\
+	for (iter = (fifo)->front;					\
+	     c = (fifo)->data[iter], iter != (fifo)->back;		\
+	     iter = (iter + 1) & (fifo)->mask)
+
+#define __init_fifo(fifo, gfp)						\
+({									\
+	size_t _allocated_size, _bytes;					\
+	BUG_ON(!(fifo)->size);						\
+									\
+	_allocated_size = roundup_pow_of_two((fifo)->size + 1);		\
+	_bytes = _allocated_size * sizeof(*(fifo)->data);		\
+									\
+	(fifo)->mask = _allocated_size - 1;				\
+	(fifo)->front = (fifo)->back = 0;				\
+	(fifo)->data = NULL;						\
+									\
+	if (_bytes < KMALLOC_MAX_SIZE)					\
+		(fifo)->data = kmalloc(_bytes, (gfp));			\
+	if ((!(fifo)->data) && ((gfp) & GFP_KERNEL))			\
+		(fifo)->data = vmalloc(_bytes);				\
+	(fifo)->data;							\
+})
+
+#define init_fifo_exact(fifo, _size, gfp)				\
+({									\
+	(fifo)->size = (_size);						\
+	__init_fifo(fifo, gfp);						\
+})
+
+#define init_fifo(fifo, _size, gfp)					\
+({									\
+	(fifo)->size = (_size);						\
+	if ((fifo)->size > 4)						\
+		(fifo)->size = roundup_pow_of_two((fifo)->size) - 1;	\
+	__init_fifo(fifo, gfp);						\
+})
+
+#define free_fifo(fifo)							\
+do {									\
+	if (is_vmalloc_addr((fifo)->data))				\
+		vfree((fifo)->data);					\
+	else								\
+		kfree((fifo)->data);					\
+	(fifo)->data = NULL;						\
+} while (0)
+
+#define fifo_used(fifo)		(((fifo)->back - (fifo)->front) & (fifo)->mask)
+#define fifo_free(fifo)		((fifo)->size - fifo_used(fifo))
+
+#define fifo_empty(fifo)	(!fifo_used(fifo))
+#define fifo_full(fifo)		(!fifo_free(fifo))
+
+#define fifo_front(fifo)	((fifo)->data[(fifo)->front])
+#define fifo_back(fifo)							\
+	((fifo)->data[((fifo)->back - 1) & (fifo)->mask])
+
+#define fifo_idx(fifo, p)	(((p) - &fifo_front(fifo)) & (fifo)->mask)
+
+#define fifo_push_back(fifo, i)						\
+({									\
+	bool _r = !fifo_full((fifo));					\
+	if (_r) {							\
+		(fifo)->data[(fifo)->back++] = (i);			\
+		(fifo)->back &= (fifo)->mask;				\
+	}								\
+	_r;								\
+})
+
+#define fifo_pop_front(fifo, i)						\
+({									\
+	bool _r = !fifo_empty((fifo));					\
+	if (_r) {							\
+		(i) = (fifo)->data[(fifo)->front++];			\
+		(fifo)->front &= (fifo)->mask;				\
+	}								\
+	_r;								\
+})
+
+#define fifo_push_front(fifo, i)					\
+({									\
+	bool _r = !fifo_full((fifo));					\
+	if (_r) {							\
+		--(fifo)->front;					\
+		(fifo)->front &= (fifo)->mask;				\
+		(fifo)->data[(fifo)->front] = (i);			\
+	}								\
+	_r;								\
+})
+
+#define fifo_pop_back(fifo, i)						\
+({									\
+	bool _r = !fifo_empty((fifo));					\
+	if (_r) {							\
+		--(fifo)->back;						\
+		(fifo)->back &= (fifo)->mask;				\
+		(i) = (fifo)->data[(fifo)->back]			\
+	}								\
+	_r;								\
+})
+
+#define fifo_push(fifo, i)	fifo_push_back(fifo, (i))
+#define fifo_pop(fifo, i)	fifo_pop_front(fifo, (i))
+
+#define fifo_swap(l, r)							\
+do {									\
+	swap((l)->front, (r)->front);					\
+	swap((l)->back, (r)->back);					\
+	swap((l)->size, (r)->size);					\
+	swap((l)->mask, (r)->mask);					\
+	swap((l)->data, (r)->data);					\
+} while (0)
+
+#define fifo_move(dest, src)						\
+do {									\
+	typeof(*((dest)->data)) _t;					\
+	while (!fifo_full(dest) &&					\
+	       fifo_pop(src, _t))					\
+		fifo_push(dest, _t);					\
+} while (0)
+
+/*
+ * Simple array based allocator - preallocates a number of elements and you can
+ * never allocate more than that, also has no locking.
+ *
+ * Handy because if you know you only need a fixed number of elements you don't
+ * have to worry about memory allocation failure, and sometimes a mempool isn't
+ * what you want.
+ *
+ * We treat the free elements as entries in a singly linked list, and the
+ * freelist as a stack - allocating and freeing push and pop off the freelist.
+ */
+
+#define DECLARE_ARRAY_ALLOCATOR(type, name, size)			\
+	struct {							\
+		type	*freelist;					\
+		type	data[size];					\
+	} name
+
+#define array_alloc(array)						\
+({									\
+	typeof((array)->freelist) _ret = (array)->freelist;		\
+									\
+	if (_ret)							\
+		(array)->freelist = *((typeof((array)->freelist) *) _ret);\
+									\
+	_ret;								\
+})
+
+#define array_free(array, ptr)						\
+do {									\
+	typeof((array)->freelist) _ptr = ptr;				\
+									\
+	*((typeof((array)->freelist) *) _ptr) = (array)->freelist;	\
+	(array)->freelist = _ptr;					\
+} while (0)
+
+#define array_allocator_init(array)					\
+do {									\
+	typeof((array)->freelist) _i;					\
+									\
+	BUILD_BUG_ON(sizeof((array)->data[0]) < sizeof(void *));	\
+	(array)->freelist = NULL;					\
+									\
+	for (_i = (array)->data;					\
+	     _i < (array)->data + ARRAY_SIZE((array)->data);		\
+	     _i++)							\
+		array_free(array, _i);					\
+} while (0)
+
+#define array_freelist_empty(array)	((array)->freelist == NULL)
+
+#define ANYSINT_MAX(t)							\
+	((((t) 1 << (sizeof(t) * 8 - 2)) - (t) 1) * (t) 2 + (t) 1)
+
+int bch_strtoint_h(const char *, int *);
+int bch_strtouint_h(const char *, unsigned int *);
+int bch_strtoll_h(const char *, long long *);
+int bch_strtoull_h(const char *, unsigned long long *);
+
+static inline int bch_strtol_h(const char *cp, long *res)
+{
+#if BITS_PER_LONG == 32
+	return bch_strtoint_h(cp, (int *) res);
+#else
+	return bch_strtoll_h(cp, (long long *) res);
+#endif
+}
+
+static inline int bch_strtoul_h(const char *cp, long *res)
+{
+#if BITS_PER_LONG == 32
+	return bch_strtouint_h(cp, (unsigned int *) res);
+#else
+	return bch_strtoull_h(cp, (unsigned long long *) res);
+#endif
+}
+
+#define strtoi_h(cp, res)						\
+	(__builtin_types_compatible_p(typeof(*res), int)		\
+	? bch_strtoint_h(cp, (void *) res)				\
+	: __builtin_types_compatible_p(typeof(*res), long)		\
+	? bch_strtol_h(cp, (void *) res)				\
+	: __builtin_types_compatible_p(typeof(*res), long long)		\
+	? bch_strtoll_h(cp, (void *) res)				\
+	: __builtin_types_compatible_p(typeof(*res), unsigned int)	\
+	? bch_strtouint_h(cp, (void *) res)				\
+	: __builtin_types_compatible_p(typeof(*res), unsigned long)	\
+	? bch_strtoul_h(cp, (void *) res)				\
+	: __builtin_types_compatible_p(typeof(*res), unsigned long long)\
+	? bch_strtoull_h(cp, (void *) res) : -EINVAL)
+
+#define strtoul_safe(cp, var)						\
+({									\
+	unsigned long _v;						\
+	int _r = kstrtoul(cp, 10, &_v);					\
+	if (!_r)							\
+		var = _v;						\
+	_r;								\
+})
+
+#define strtoul_safe_clamp(cp, var, min, max)				\
+({									\
+	unsigned long _v;						\
+	int _r = kstrtoul(cp, 10, &_v);					\
+	if (!_r)							\
+		var = clamp_t(typeof(var), _v, min, max);		\
+	_r;								\
+})
+
+#define snprint(buf, size, var)						\
+	snprintf(buf, size,						\
+		__builtin_types_compatible_p(typeof(var), int)		\
+		     ? "%i\n" :						\
+		__builtin_types_compatible_p(typeof(var), unsigned)	\
+		     ? "%u\n" :						\
+		__builtin_types_compatible_p(typeof(var), long)		\
+		     ? "%li\n" :					\
+		__builtin_types_compatible_p(typeof(var), unsigned long)\
+		     ? "%lu\n" :					\
+		__builtin_types_compatible_p(typeof(var), int64_t)	\
+		     ? "%lli\n" :					\
+		__builtin_types_compatible_p(typeof(var), uint64_t)	\
+		     ? "%llu\n" :					\
+		__builtin_types_compatible_p(typeof(var), const char *)	\
+		     ? "%s\n" : "%i\n", var)
+
+ssize_t bch_hprint(char *buf, int64_t v);
+
+bool bch_is_zero(const char *p, size_t n);
+int bch_parse_uuid(const char *s, char *uuid);
+
+ssize_t bch_snprint_string_list(char *buf, size_t size, const char * const list[],
+			    size_t selected);
+
+ssize_t bch_read_string_list(const char *buf, const char * const list[]);
+
+struct time_stats {
+	/*
+	 * all fields are in nanoseconds, averages are ewmas stored left shifted
+	 * by 8
+	 */
+	uint64_t	max_duration;
+	uint64_t	average_duration;
+	uint64_t	average_frequency;
+	uint64_t	last;
+};
+
+void bch_time_stats_update(struct time_stats *stats, uint64_t time);
+
+#define NSEC_PER_ns			1L
+#define NSEC_PER_us			NSEC_PER_USEC
+#define NSEC_PER_ms			NSEC_PER_MSEC
+#define NSEC_PER_sec			NSEC_PER_SEC
+
+#define __print_time_stat(stats, name, stat, units)			\
+	sysfs_print(name ## _ ## stat ## _ ## units,			\
+		    div_u64((stats)->stat >> 8, NSEC_PER_ ## units))
+
+#define sysfs_print_time_stats(stats, name,				\
+			       frequency_units,				\
+			       duration_units)				\
+do {									\
+	__print_time_stat(stats, name,					\
+			  average_frequency,	frequency_units);	\
+	__print_time_stat(stats, name,					\
+			  average_duration,	duration_units);	\
+	__print_time_stat(stats, name,					\
+			  max_duration,		duration_units);	\
+									\
+	sysfs_print(name ## _last_ ## frequency_units, (stats)->last	\
+		    ? div_s64(local_clock() - (stats)->last,		\
+			      NSEC_PER_ ## frequency_units)		\
+		    : -1LL);						\
+} while (0)
+
+#define sysfs_time_stats_attribute(name,				\
+				   frequency_units,			\
+				   duration_units)			\
+read_attribute(name ## _average_frequency_ ## frequency_units);		\
+read_attribute(name ## _average_duration_ ## duration_units);		\
+read_attribute(name ## _max_duration_ ## duration_units);		\
+read_attribute(name ## _last_ ## frequency_units)
+
+#define sysfs_time_stats_attribute_list(name,				\
+					frequency_units,		\
+					duration_units)			\
+&sysfs_ ## name ## _average_frequency_ ## frequency_units,		\
+&sysfs_ ## name ## _average_duration_ ## duration_units,		\
+&sysfs_ ## name ## _max_duration_ ## duration_units,			\
+&sysfs_ ## name ## _last_ ## frequency_units,
+
+#define ewma_add(ewma, val, weight, factor)				\
+({									\
+	(ewma) *= (weight) - 1;						\
+	(ewma) += (val) << factor;					\
+	(ewma) /= (weight);						\
+	(ewma) >> factor;						\
+})
+
+struct ratelimit {
+	uint64_t		next;
+	unsigned		rate;
+};
+
+static inline void ratelimit_reset(struct ratelimit *d)
+{
+	d->next = local_clock();
+}
+
+unsigned bch_next_delay(struct ratelimit *d, uint64_t done);
+
+#define __DIV_SAFE(n, d, zero)						\
+({									\
+	typeof(n) _n = (n);						\
+	typeof(d) _d = (d);						\
+	_d ? _n / _d : zero;						\
+})
+
+#define DIV_SAFE(n, d)	__DIV_SAFE(n, d, 0)
+
+#define container_of_or_null(ptr, type, member)				\
+({									\
+	typeof(ptr) _ptr = ptr;						\
+	_ptr ? container_of(_ptr, type, member) : NULL;			\
+})
+
+#define RB_INSERT(root, new, member, cmp)				\
+({									\
+	__label__ dup;							\
+	struct rb_node **n = &(root)->rb_node, *parent = NULL;		\
+	typeof(new) this;						\
+	int res, ret = -1;						\
+									\
+	while (*n) {							\
+		parent = *n;						\
+		this = container_of(*n, typeof(*(new)), member);	\
+		res = cmp(new, this);					\
+		if (!res)						\
+			goto dup;					\
+		n = res < 0						\
+			? &(*n)->rb_left				\
+			: &(*n)->rb_right;				\
+	}								\
+									\
+	rb_link_node(&(new)->member, parent, n);			\
+	rb_insert_color(&(new)->member, root);				\
+	ret = 0;							\
+dup:									\
+	ret;								\
+})
+
+#define RB_SEARCH(root, search, member, cmp)				\
+({									\
+	struct rb_node *n = (root)->rb_node;				\
+	typeof(&(search)) this, ret = NULL;				\
+	int res;							\
+									\
+	while (n) {							\
+		this = container_of(n, typeof(search), member);		\
+		res = cmp(&(search), this);				\
+		if (!res) {						\
+			ret = this;					\
+			break;						\
+		}							\
+		n = res < 0						\
+			? n->rb_left					\
+			: n->rb_right;					\
+	}								\
+	ret;								\
+})
+
+#define RB_GREATER(root, search, member, cmp)				\
+({									\
+	struct rb_node *n = (root)->rb_node;				\
+	typeof(&(search)) this, ret = NULL;				\
+	int res;							\
+									\
+	while (n) {							\
+		this = container_of(n, typeof(search), member);		\
+		res = cmp(&(search), this);				\
+		if (res < 0) {						\
+			ret = this;					\
+			n = n->rb_left;					\
+		} else							\
+			n = n->rb_right;				\
+	}								\
+	ret;								\
+})
+
+#define RB_FIRST(root, type, member)					\
+	container_of_or_null(rb_first(root), type, member)
+
+#define RB_LAST(root, type, member)					\
+	container_of_or_null(rb_last(root), type, member)
+
+#define RB_NEXT(ptr, member)						\
+	container_of_or_null(rb_next(&(ptr)->member), typeof(*ptr), member)
+
+#define RB_PREV(ptr, member)						\
+	container_of_or_null(rb_prev(&(ptr)->member), typeof(*ptr), member)
+
+/* Does linear interpolation between powers of two */
+static inline unsigned fract_exp_two(unsigned x, unsigned fract_bits)
+{
+	unsigned fract = x & ~(~0 << fract_bits);
+
+	x >>= fract_bits;
+	x   = 1 << x;
+	x  += (x * fract) >> fract_bits;
+
+	return x;
+}
+
+#define bio_end(bio)	((bio)->bi_sector + bio_sectors(bio))
+
+void bch_bio_map(struct bio *bio, void *base);
+
+int bch_bio_alloc_pages(struct bio *bio, gfp_t gfp);
+
+static inline sector_t bdev_sectors(struct block_device *bdev)
+{
+	return bdev->bd_inode->i_size >> 9;
+}
+
+#define closure_bio_submit(bio, cl, dev)				\
+do {									\
+	closure_get(cl);						\
+	bch_generic_make_request(bio, &(dev)->bio_split_hook);		\
+} while (0)
+
+uint64_t bch_crc64_update(uint64_t, const void *, size_t);
+uint64_t bch_crc64(const void *, size_t);
+
+#endif /* _BCACHE_UTIL_H */
diff --git a/drivers/md/bcache/writeback.c b/drivers/md/bcache/writeback.c
new file mode 100644
index 000000000000..93e7e31a4bd3
--- /dev/null
+++ b/drivers/md/bcache/writeback.c
@@ -0,0 +1,414 @@
+/*
+ * background writeback - scan btree for dirty data and write it to the backing
+ * device
+ *
+ * Copyright 2010, 2011 Kent Overstreet <kent.overstreet@gmail.com>
+ * Copyright 2012 Google, Inc.
+ */
+
+#include "bcache.h"
+#include "btree.h"
+#include "debug.h"
+
+static struct workqueue_struct *dirty_wq;
+
+static void read_dirty(struct closure *);
+
+struct dirty_io {
+	struct closure		cl;
+	struct cached_dev	*dc;
+	struct bio		bio;
+};
+
+/* Rate limiting */
+
+static void __update_writeback_rate(struct cached_dev *dc)
+{
+	struct cache_set *c = dc->disk.c;
+	uint64_t cache_sectors = c->nbuckets * c->sb.bucket_size;
+	uint64_t cache_dirty_target =
+		div_u64(cache_sectors * dc->writeback_percent, 100);
+
+	int64_t target = div64_u64(cache_dirty_target * bdev_sectors(dc->bdev),
+				   c->cached_dev_sectors);
+
+	/* PD controller */
+
+	int change = 0;
+	int64_t error;
+	int64_t dirty = atomic_long_read(&dc->disk.sectors_dirty);
+	int64_t derivative = dirty - dc->disk.sectors_dirty_last;
+
+	dc->disk.sectors_dirty_last = dirty;
+
+	derivative *= dc->writeback_rate_d_term;
+	derivative = clamp(derivative, -dirty, dirty);
+
+	derivative = ewma_add(dc->disk.sectors_dirty_derivative, derivative,
+			      dc->writeback_rate_d_smooth, 0);
+
+	/* Avoid divide by zero */
+	if (!target)
+		goto out;
+
+	error = div64_s64((dirty + derivative - target) << 8, target);
+
+	change = div_s64((dc->writeback_rate.rate * error) >> 8,
+			 dc->writeback_rate_p_term_inverse);
+
+	/* Don't increase writeback rate if the device isn't keeping up */
+	if (change > 0 &&
+	    time_after64(local_clock(),
+			 dc->writeback_rate.next + 10 * NSEC_PER_MSEC))
+		change = 0;
+
+	dc->writeback_rate.rate =
+		clamp_t(int64_t, dc->writeback_rate.rate + change,
+			1, NSEC_PER_MSEC);
+out:
+	dc->writeback_rate_derivative = derivative;
+	dc->writeback_rate_change = change;
+	dc->writeback_rate_target = target;
+
+	schedule_delayed_work(&dc->writeback_rate_update,
+			      dc->writeback_rate_update_seconds * HZ);
+}
+
+static void update_writeback_rate(struct work_struct *work)
+{
+	struct cached_dev *dc = container_of(to_delayed_work(work),
+					     struct cached_dev,
+					     writeback_rate_update);
+
+	down_read(&dc->writeback_lock);
+
+	if (atomic_read(&dc->has_dirty) &&
+	    dc->writeback_percent)
+		__update_writeback_rate(dc);
+
+	up_read(&dc->writeback_lock);
+}
+
+static unsigned writeback_delay(struct cached_dev *dc, unsigned sectors)
+{
+	if (atomic_read(&dc->disk.detaching) ||
+	    !dc->writeback_percent)
+		return 0;
+
+	return bch_next_delay(&dc->writeback_rate, sectors * 10000000ULL);
+}
+
+/* Background writeback */
+
+static bool dirty_pred(struct keybuf *buf, struct bkey *k)
+{
+	return KEY_DIRTY(k);
+}
+
+static void dirty_init(struct keybuf_key *w)
+{
+	struct dirty_io *io = w->private;
+	struct bio *bio = &io->bio;
+
+	bio_init(bio);
+	if (!io->dc->writeback_percent)
+		bio_set_prio(bio, IOPRIO_PRIO_VALUE(IOPRIO_CLASS_IDLE, 0));
+
+	bio->bi_size		= KEY_SIZE(&w->key) << 9;
+	bio->bi_max_vecs	= DIV_ROUND_UP(KEY_SIZE(&w->key), PAGE_SECTORS);
+	bio->bi_private		= w;
+	bio->bi_io_vec		= bio->bi_inline_vecs;
+	bch_bio_map(bio, NULL);
+}
+
+static void refill_dirty(struct closure *cl)
+{
+	struct cached_dev *dc = container_of(cl, struct cached_dev,
+					     writeback.cl);
+	struct keybuf *buf = &dc->writeback_keys;
+	bool searched_from_start = false;
+	struct bkey end = MAX_KEY;
+	SET_KEY_INODE(&end, dc->disk.id);
+
+	if (!atomic_read(&dc->disk.detaching) &&
+	    !dc->writeback_running)
+		closure_return(cl);
+
+	down_write(&dc->writeback_lock);
+
+	if (!atomic_read(&dc->has_dirty)) {
+		SET_BDEV_STATE(&dc->sb, BDEV_STATE_CLEAN);
+		bch_write_bdev_super(dc, NULL);
+
+		up_write(&dc->writeback_lock);
+		closure_return(cl);
+	}
+
+	if (bkey_cmp(&buf->last_scanned, &end) >= 0) {
+		buf->last_scanned = KEY(dc->disk.id, 0, 0);
+		searched_from_start = true;
+	}
+
+	bch_refill_keybuf(dc->disk.c, buf, &end);
+
+	if (bkey_cmp(&buf->last_scanned, &end) >= 0 && searched_from_start) {
+		/* Searched the entire btree  - delay awhile */
+
+		if (RB_EMPTY_ROOT(&buf->keys)) {
+			atomic_set(&dc->has_dirty, 0);
+			cached_dev_put(dc);
+		}
+
+		if (!atomic_read(&dc->disk.detaching))
+			closure_delay(&dc->writeback, dc->writeback_delay * HZ);
+	}
+
+	up_write(&dc->writeback_lock);
+
+	ratelimit_reset(&dc->writeback_rate);
+
+	/* Punt to workqueue only so we don't recurse and blow the stack */
+	continue_at(cl, read_dirty, dirty_wq);
+}
+
+void bch_writeback_queue(struct cached_dev *dc)
+{
+	if (closure_trylock(&dc->writeback.cl, &dc->disk.cl)) {
+		if (!atomic_read(&dc->disk.detaching))
+			closure_delay(&dc->writeback, dc->writeback_delay * HZ);
+
+		continue_at(&dc->writeback.cl, refill_dirty, dirty_wq);
+	}
+}
+
+void bch_writeback_add(struct cached_dev *dc, unsigned sectors)
+{
+	atomic_long_add(sectors, &dc->disk.sectors_dirty);
+
+	if (!atomic_read(&dc->has_dirty) &&
+	    !atomic_xchg(&dc->has_dirty, 1)) {
+		atomic_inc(&dc->count);
+
+		if (BDEV_STATE(&dc->sb) != BDEV_STATE_DIRTY) {
+			SET_BDEV_STATE(&dc->sb, BDEV_STATE_DIRTY);
+			/* XXX: should do this synchronously */
+			bch_write_bdev_super(dc, NULL);
+		}
+
+		bch_writeback_queue(dc);
+
+		if (dc->writeback_percent)
+			schedule_delayed_work(&dc->writeback_rate_update,
+				      dc->writeback_rate_update_seconds * HZ);
+	}
+}
+
+/* Background writeback - IO loop */
+
+static void dirty_io_destructor(struct closure *cl)
+{
+	struct dirty_io *io = container_of(cl, struct dirty_io, cl);
+	kfree(io);
+}
+
+static void write_dirty_finish(struct closure *cl)
+{
+	struct dirty_io *io = container_of(cl, struct dirty_io, cl);
+	struct keybuf_key *w = io->bio.bi_private;
+	struct cached_dev *dc = io->dc;
+	struct bio_vec *bv = bio_iovec_idx(&io->bio, io->bio.bi_vcnt);
+
+	while (bv-- != io->bio.bi_io_vec)
+		__free_page(bv->bv_page);
+
+	/* This is kind of a dumb way of signalling errors. */
+	if (KEY_DIRTY(&w->key)) {
+		unsigned i;
+		struct btree_op op;
+		bch_btree_op_init_stack(&op);
+
+		op.type = BTREE_REPLACE;
+		bkey_copy(&op.replace, &w->key);
+
+		SET_KEY_DIRTY(&w->key, false);
+		bch_keylist_add(&op.keys, &w->key);
+
+		for (i = 0; i < KEY_PTRS(&w->key); i++)
+			atomic_inc(&PTR_BUCKET(dc->disk.c, &w->key, i)->pin);
+
+		pr_debug("clearing %s", pkey(&w->key));
+		bch_btree_insert(&op, dc->disk.c);
+		closure_sync(&op.cl);
+
+		atomic_long_inc(op.insert_collision
+				? &dc->disk.c->writeback_keys_failed
+				: &dc->disk.c->writeback_keys_done);
+	}
+
+	bch_keybuf_del(&dc->writeback_keys, w);
+	atomic_dec_bug(&dc->in_flight);
+
+	closure_wake_up(&dc->writeback_wait);
+
+	closure_return_with_destructor(cl, dirty_io_destructor);
+}
+
+static void dirty_endio(struct bio *bio, int error)
+{
+	struct keybuf_key *w = bio->bi_private;
+	struct dirty_io *io = w->private;
+
+	if (error)
+		SET_KEY_DIRTY(&w->key, false);
+
+	closure_put(&io->cl);
+}
+
+static void write_dirty(struct closure *cl)
+{
+	struct dirty_io *io = container_of(cl, struct dirty_io, cl);
+	struct keybuf_key *w = io->bio.bi_private;
+
+	dirty_init(w);
+	io->bio.bi_rw		= WRITE;
+	io->bio.bi_sector	= KEY_START(&w->key);
+	io->bio.bi_bdev		= io->dc->bdev;
+	io->bio.bi_end_io	= dirty_endio;
+
+	trace_bcache_write_dirty(&io->bio);
+	closure_bio_submit(&io->bio, cl, &io->dc->disk);
+
+	continue_at(cl, write_dirty_finish, dirty_wq);
+}
+
+static void read_dirty_endio(struct bio *bio, int error)
+{
+	struct keybuf_key *w = bio->bi_private;
+	struct dirty_io *io = w->private;
+
+	bch_count_io_errors(PTR_CACHE(io->dc->disk.c, &w->key, 0),
+			    error, "reading dirty data from cache");
+
+	dirty_endio(bio, error);
+}
+
+static void read_dirty_submit(struct closure *cl)
+{
+	struct dirty_io *io = container_of(cl, struct dirty_io, cl);
+
+	trace_bcache_read_dirty(&io->bio);
+	closure_bio_submit(&io->bio, cl, &io->dc->disk);
+
+	continue_at(cl, write_dirty, dirty_wq);
+}
+
+static void read_dirty(struct closure *cl)
+{
+	struct cached_dev *dc = container_of(cl, struct cached_dev,
+					     writeback.cl);
+	unsigned delay = writeback_delay(dc, 0);
+	struct keybuf_key *w;
+	struct dirty_io *io;
+
+	/*
+	 * XXX: if we error, background writeback just spins. Should use some
+	 * mempools.
+	 */
+
+	while (1) {
+		w = bch_keybuf_next(&dc->writeback_keys);
+		if (!w)
+			break;
+
+		BUG_ON(ptr_stale(dc->disk.c, &w->key, 0));
+
+		if (delay > 0 &&
+		    (KEY_START(&w->key) != dc->last_read ||
+		     jiffies_to_msecs(delay) > 50)) {
+			w->private = NULL;
+
+			closure_delay(&dc->writeback, delay);
+			continue_at(cl, read_dirty, dirty_wq);
+		}
+
+		dc->last_read	= KEY_OFFSET(&w->key);
+
+		io = kzalloc(sizeof(struct dirty_io) + sizeof(struct bio_vec)
+			     * DIV_ROUND_UP(KEY_SIZE(&w->key), PAGE_SECTORS),
+			     GFP_KERNEL);
+		if (!io)
+			goto err;
+
+		w->private	= io;
+		io->dc		= dc;
+
+		dirty_init(w);
+		io->bio.bi_sector	= PTR_OFFSET(&w->key, 0);
+		io->bio.bi_bdev		= PTR_CACHE(dc->disk.c,
+						    &w->key, 0)->bdev;
+		io->bio.bi_rw		= READ;
+		io->bio.bi_end_io	= read_dirty_endio;
+
+		if (bch_bio_alloc_pages(&io->bio, GFP_KERNEL))
+			goto err_free;
+
+		pr_debug("%s", pkey(&w->key));
+
+		closure_call(&io->cl, read_dirty_submit, NULL, &dc->disk.cl);
+
+		delay = writeback_delay(dc, KEY_SIZE(&w->key));
+
+		atomic_inc(&dc->in_flight);
+
+		if (!closure_wait_event(&dc->writeback_wait, cl,
+					atomic_read(&dc->in_flight) < 64))
+			continue_at(cl, read_dirty, dirty_wq);
+	}
+
+	if (0) {
+err_free:
+		kfree(w->private);
+err:
+		bch_keybuf_del(&dc->writeback_keys, w);
+	}
+
+	refill_dirty(cl);
+}
+
+void bch_writeback_init_cached_dev(struct cached_dev *dc)
+{
+	closure_init_unlocked(&dc->writeback);
+	init_rwsem(&dc->writeback_lock);
+
+	bch_keybuf_init(&dc->writeback_keys, dirty_pred);
+
+	dc->writeback_metadata		= true;
+	dc->writeback_running		= true;
+	dc->writeback_percent		= 10;
+	dc->writeback_delay		= 30;
+	dc->writeback_rate.rate		= 1024;
+
+	dc->writeback_rate_update_seconds = 30;
+	dc->writeback_rate_d_term	= 16;
+	dc->writeback_rate_p_term_inverse = 64;
+	dc->writeback_rate_d_smooth	= 8;
+
+	INIT_DELAYED_WORK(&dc->writeback_rate_update, update_writeback_rate);
+	schedule_delayed_work(&dc->writeback_rate_update,
+			      dc->writeback_rate_update_seconds * HZ);
+}
+
+void bch_writeback_exit(void)
+{
+	if (dirty_wq)
+		destroy_workqueue(dirty_wq);
+}
+
+int __init bch_writeback_init(void)
+{
+	dirty_wq = create_singlethread_workqueue("bcache_writeback");
+	if (!dirty_wq)
+		return -ENOMEM;
+
+	return 0;
+}
diff --git a/drivers/md/bitmap.c b/drivers/md/bitmap.c
index 4fd9d6aeff6a..5a2c75499824 100644
--- a/drivers/md/bitmap.c
+++ b/drivers/md/bitmap.c
@@ -846,7 +846,7 @@ static void bitmap_file_set_bit(struct bitmap *bitmap, sector_t block)
 	if (test_bit(BITMAP_HOSTENDIAN, &bitmap->flags))
 		set_bit(bit, kaddr);
 	else
-		test_and_set_bit_le(bit, kaddr);
+		set_bit_le(bit, kaddr);
 	kunmap_atomic(kaddr);
 	pr_debug("set file bit %lu page %lu\n", bit, page->index);
 	/* record page number so it gets flushed to disk when unplug occurs */
@@ -868,7 +868,7 @@ static void bitmap_file_clear_bit(struct bitmap *bitmap, sector_t block)
 	if (test_bit(BITMAP_HOSTENDIAN, &bitmap->flags))
 		clear_bit(bit, paddr);
 	else
-		test_and_clear_bit_le(bit, paddr);
+		clear_bit_le(bit, paddr);
 	kunmap_atomic(paddr);
 	if (!test_page_attr(bitmap, page->index, BITMAP_PAGE_NEEDWRITE)) {
 		set_page_attr(bitmap, page->index, BITMAP_PAGE_PENDING);
diff --git a/drivers/md/dm-crypt.c b/drivers/md/dm-crypt.c
index 13c15480d940..6d2d41ae9e32 100644
--- a/drivers/md/dm-crypt.c
+++ b/drivers/md/dm-crypt.c
@@ -858,8 +858,7 @@ static void crypt_free_buffer_pages(struct crypt_config *cc, struct bio *clone)
 	unsigned int i;
 	struct bio_vec *bv;
 
-	for (i = 0; i < clone->bi_vcnt; i++) {
-		bv = bio_iovec_idx(clone, i);
+	bio_for_each_segment_all(bv, clone, i) {
 		BUG_ON(!bv->bv_page);
 		mempool_free(bv->bv_page, cc->page_pool);
 		bv->bv_page = NULL;
diff --git a/drivers/md/dm-raid.c b/drivers/md/dm-raid.c
index 311e3d35b272..1d3fe1a40a9b 100644
--- a/drivers/md/dm-raid.c
+++ b/drivers/md/dm-raid.c
@@ -1279,6 +1279,31 @@ static int raid_map(struct dm_target *ti, struct bio *bio)
 	return DM_MAPIO_SUBMITTED;
 }
 
+static const char *decipher_sync_action(struct mddev *mddev)
+{
+	if (test_bit(MD_RECOVERY_FROZEN, &mddev->recovery))
+		return "frozen";
+
+	if (test_bit(MD_RECOVERY_RUNNING, &mddev->recovery) ||
+	    (!mddev->ro && test_bit(MD_RECOVERY_NEEDED, &mddev->recovery))) {
+		if (test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery))
+			return "reshape";
+
+		if (test_bit(MD_RECOVERY_SYNC, &mddev->recovery)) {
+			if (!test_bit(MD_RECOVERY_REQUESTED, &mddev->recovery))
+				return "resync";
+			else if (test_bit(MD_RECOVERY_CHECK, &mddev->recovery))
+				return "check";
+			return "repair";
+		}
+
+		if (test_bit(MD_RECOVERY_RECOVER, &mddev->recovery))
+			return "recover";
+	}
+
+	return "idle";
+}
+
 static void raid_status(struct dm_target *ti, status_type_t type,
 			unsigned status_flags, char *result, unsigned maxlen)
 {
@@ -1298,8 +1323,18 @@ static void raid_status(struct dm_target *ti, status_type_t type,
 			sync = rs->md.recovery_cp;
 
 		if (sync >= rs->md.resync_max_sectors) {
+			/*
+			 * Sync complete.
+			 */
 			array_in_sync = 1;
 			sync = rs->md.resync_max_sectors;
+		} else if (test_bit(MD_RECOVERY_REQUESTED, &rs->md.recovery)) {
+			/*
+			 * If "check" or "repair" is occurring, the array has
+			 * undergone and initial sync and the health characters
+			 * should not be 'a' anymore.
+			 */
+			array_in_sync = 1;
 		} else {
 			/*
 			 * The array may be doing an initial sync, or it may
@@ -1311,6 +1346,7 @@ static void raid_status(struct dm_target *ti, status_type_t type,
 				if (!test_bit(In_sync, &rs->dev[i].rdev.flags))
 					array_in_sync = 1;
 		}
+
 		/*
 		 * Status characters:
 		 *  'D' = Dead/Failed device
@@ -1339,6 +1375,21 @@ static void raid_status(struct dm_target *ti, status_type_t type,
 		       (unsigned long long) sync,
 		       (unsigned long long) rs->md.resync_max_sectors);
 
+		/*
+		 * Sync action:
+		 *   See Documentation/device-mapper/dm-raid.c for
+		 *   information on each of these states.
+		 */
+		DMEMIT(" %s", decipher_sync_action(&rs->md));
+
+		/*
+		 * resync_mismatches/mismatch_cnt
+		 *   This field shows the number of discrepancies found when
+		 *   performing a "check" of the array.
+		 */
+		DMEMIT(" %llu",
+		       (unsigned long long)
+		       atomic64_read(&rs->md.resync_mismatches));
 		break;
 	case STATUSTYPE_TABLE:
 		/* The string you would use to construct this array */
@@ -1425,7 +1476,62 @@ static void raid_status(struct dm_target *ti, status_type_t type,
 	}
 }
 
-static int raid_iterate_devices(struct dm_target *ti, iterate_devices_callout_fn fn, void *data)
+static int raid_message(struct dm_target *ti, unsigned argc, char **argv)
+{
+	struct raid_set *rs = ti->private;
+	struct mddev *mddev = &rs->md;
+
+	if (!strcasecmp(argv[0], "reshape")) {
+		DMERR("Reshape not supported.");
+		return -EINVAL;
+	}
+
+	if (!mddev->pers || !mddev->pers->sync_request)
+		return -EINVAL;
+
+	if (!strcasecmp(argv[0], "frozen"))
+		set_bit(MD_RECOVERY_FROZEN, &mddev->recovery);
+	else
+		clear_bit(MD_RECOVERY_FROZEN, &mddev->recovery);
+
+	if (!strcasecmp(argv[0], "idle") || !strcasecmp(argv[0], "frozen")) {
+		if (mddev->sync_thread) {
+			set_bit(MD_RECOVERY_INTR, &mddev->recovery);
+			md_reap_sync_thread(mddev);
+		}
+	} else if (test_bit(MD_RECOVERY_RUNNING, &mddev->recovery) ||
+		   test_bit(MD_RECOVERY_NEEDED, &mddev->recovery))
+		return -EBUSY;
+	else if (!strcasecmp(argv[0], "resync"))
+		set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
+	else if (!strcasecmp(argv[0], "recover")) {
+		set_bit(MD_RECOVERY_RECOVER, &mddev->recovery);
+		set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
+	} else {
+		if (!strcasecmp(argv[0], "check"))
+			set_bit(MD_RECOVERY_CHECK, &mddev->recovery);
+		else if (!!strcasecmp(argv[0], "repair"))
+			return -EINVAL;
+		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;
+		if (!mddev->suspended)
+			md_wakeup_thread(mddev->sync_thread);
+	}
+	set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
+	if (!mddev->suspended)
+		md_wakeup_thread(mddev->thread);
+
+	return 0;
+}
+
+static int raid_iterate_devices(struct dm_target *ti,
+				iterate_devices_callout_fn fn, void *data)
 {
 	struct raid_set *rs = ti->private;
 	unsigned i;
@@ -1482,12 +1588,13 @@ static void raid_resume(struct dm_target *ti)
 
 static struct target_type raid_target = {
 	.name = "raid",
-	.version = {1, 4, 2},
+	.version = {1, 5, 0},
 	.module = THIS_MODULE,
 	.ctr = raid_ctr,
 	.dtr = raid_dtr,
 	.map = raid_map,
 	.status = raid_status,
+	.message = raid_message,
 	.iterate_devices = raid_iterate_devices,
 	.io_hints = raid_io_hints,
 	.presuspend = raid_presuspend,
diff --git a/drivers/md/dm-raid1.c b/drivers/md/dm-raid1.c
index d053098c6a91..699b5be68d31 100644
--- a/drivers/md/dm-raid1.c
+++ b/drivers/md/dm-raid1.c
@@ -458,7 +458,7 @@ static void map_region(struct dm_io_region *io, struct mirror *m,
 {
 	io->bdev = m->dev->bdev;
 	io->sector = map_sector(m, bio);
-	io->count = bio->bi_size >> 9;
+	io->count = bio_sectors(bio);
 }
 
 static void hold_bio(struct mirror_set *ms, struct bio *bio)
diff --git a/drivers/md/dm-stripe.c b/drivers/md/dm-stripe.c
index d8837d313f54..ea5e878a30b9 100644
--- a/drivers/md/dm-stripe.c
+++ b/drivers/md/dm-stripe.c
@@ -258,7 +258,7 @@ static int stripe_map_range(struct stripe_c *sc, struct bio *bio,
 	sector_t begin, end;
 
 	stripe_map_range_sector(sc, bio->bi_sector, target_stripe, &begin);
-	stripe_map_range_sector(sc, bio->bi_sector + bio_sectors(bio),
+	stripe_map_range_sector(sc, bio_end_sector(bio),
 				target_stripe, &end);
 	if (begin < end) {
 		bio->bi_bdev = sc->stripe[target_stripe].dev->bdev;
diff --git a/drivers/md/dm-verity.c b/drivers/md/dm-verity.c
index a746f1d21c66..b948fd864d45 100644
--- a/drivers/md/dm-verity.c
+++ b/drivers/md/dm-verity.c
@@ -501,7 +501,7 @@ static int verity_map(struct dm_target *ti, struct bio *bio)
 		return -EIO;
 	}
 
-	if ((bio->bi_sector + bio_sectors(bio)) >>
+	if (bio_end_sector(bio) >>
 	    (v->data_dev_block_bits - SECTOR_SHIFT) > v->data_blocks) {
 		DMERR_LIMIT("io out of range");
 		return -EIO;
@@ -519,7 +519,7 @@ static int verity_map(struct dm_target *ti, struct bio *bio)
 
 	bio->bi_end_io = verity_end_io;
 	bio->bi_private = io;
-	io->io_vec_size = bio->bi_vcnt - bio->bi_idx;
+	io->io_vec_size = bio_segments(bio);
 	if (io->io_vec_size < DM_VERITY_IO_VEC_INLINE)
 		io->io_vec = io->io_vec_inline;
 	else
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index 9a0bdad9ad8f..d5370a94b2c1 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -339,7 +339,7 @@ out:
 	return md ? 0 : -ENXIO;
 }
 
-static int dm_blk_close(struct gendisk *disk, fmode_t mode)
+static void dm_blk_close(struct gendisk *disk, fmode_t mode)
 {
 	struct mapped_device *md = disk->private_data;
 
@@ -349,8 +349,6 @@ static int dm_blk_close(struct gendisk *disk, fmode_t mode)
 	dm_put(md);
 
 	spin_unlock(&_minor_lock);
-
-	return 0;
 }
 
 int dm_open_count(struct mapped_device *md)
diff --git a/drivers/md/faulty.c b/drivers/md/faulty.c
index 5e7dc772f5de..3193aefe982b 100644
--- a/drivers/md/faulty.c
+++ b/drivers/md/faulty.c
@@ -185,8 +185,7 @@ static void make_request(struct mddev *mddev, struct bio *bio)
 			return;
 		}
 
-		if (check_sector(conf, bio->bi_sector, bio->bi_sector+(bio->bi_size>>9),
-				 WRITE))
+		if (check_sector(conf, bio->bi_sector, bio_end_sector(bio), WRITE))
 			failit = 1;
 		if (check_mode(conf, WritePersistent)) {
 			add_sector(conf, bio->bi_sector, WritePersistent);
@@ -196,8 +195,7 @@ static void make_request(struct mddev *mddev, struct bio *bio)
 			failit = 1;
 	} else {
 		/* read request */
-		if (check_sector(conf, bio->bi_sector, bio->bi_sector + (bio->bi_size>>9),
-				 READ))
+		if (check_sector(conf, bio->bi_sector, bio_end_sector(bio), READ))
 			failit = 1;
 		if (check_mode(conf, ReadTransient))
 			failit = 1;
diff --git a/drivers/md/linear.c b/drivers/md/linear.c
index 21014836bdbf..f03fabd2b37b 100644
--- a/drivers/md/linear.c
+++ b/drivers/md/linear.c
@@ -317,8 +317,7 @@ static void linear_make_request(struct mddev *mddev, struct bio *bio)
 		bio_io_error(bio);
 		return;
 	}
-	if (unlikely(bio->bi_sector + (bio->bi_size >> 9) >
-		     tmp_dev->end_sector)) {
+	if (unlikely(bio_end_sector(bio) > tmp_dev->end_sector)) {
 		/* This bio crosses a device boundary, so we have to
 		 * split it.
 		 */
diff --git a/drivers/md/md.c b/drivers/md/md.c
index aeceedfc530b..681d1099a2d5 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -72,6 +72,9 @@ static DECLARE_WAIT_QUEUE_HEAD(resync_wait);
 static struct workqueue_struct *md_wq;
 static struct workqueue_struct *md_misc_wq;
 
+static int remove_and_add_spares(struct mddev *mddev,
+				 struct md_rdev *this);
+
 #define MD_BUG(x...) { printk("md: bug in file %s, line %d\n", __FILE__, __LINE__); md_print_devices(); }
 
 /*
@@ -194,21 +197,12 @@ void md_trim_bio(struct bio *bio, int offset, int size)
 	if (offset == 0 && size == bio->bi_size)
 		return;
 
-	bio->bi_sector += offset;
-	bio->bi_size = size;
-	offset <<= 9;
 	clear_bit(BIO_SEG_VALID, &bio->bi_flags);
 
-	while (bio->bi_idx < bio->bi_vcnt &&
-	       bio->bi_io_vec[bio->bi_idx].bv_len <= offset) {
-		/* remove this whole bio_vec */
-		offset -= bio->bi_io_vec[bio->bi_idx].bv_len;
-		bio->bi_idx++;
-	}
-	if (bio->bi_idx < bio->bi_vcnt) {
-		bio->bi_io_vec[bio->bi_idx].bv_offset += offset;
-		bio->bi_io_vec[bio->bi_idx].bv_len -= offset;
-	}
+	bio_advance(bio, offset << 9);
+
+	bio->bi_size = size;
+
 	/* avoid any complications with bi_idx being non-zero*/
 	if (bio->bi_idx) {
 		memmove(bio->bi_io_vec, bio->bi_io_vec+bio->bi_idx,
@@ -1564,8 +1558,8 @@ static int super_1_load(struct md_rdev *rdev, struct md_rdev *refdev, int minor_
 					     sector, count, 1) == 0)
 				return -EINVAL;
 		}
-	} else if (sb->bblog_offset == 0)
-		rdev->badblocks.shift = -1;
+	} else if (sb->bblog_offset != 0)
+		rdev->badblocks.shift = 0;
 
 	if (!refdev) {
 		ret = 1;
@@ -2411,6 +2405,11 @@ static void md_update_sb(struct mddev * mddev, int force_change)
 	int nospares = 0;
 	int any_badblocks_changed = 0;
 
+	if (mddev->ro) {
+		if (force_change)
+			set_bit(MD_CHANGE_DEVS, &mddev->flags);
+		return;
+	}
 repeat:
 	/* First make sure individual recovery_offsets are correct */
 	rdev_for_each(rdev, mddev) {
@@ -2800,12 +2799,10 @@ slot_store(struct md_rdev *rdev, const char *buf, size_t len)
 		/* personality does all needed checks */
 		if (rdev->mddev->pers->hot_remove_disk == NULL)
 			return -EINVAL;
-		err = rdev->mddev->pers->
-			hot_remove_disk(rdev->mddev, rdev);
-		if (err)
-			return err;
-		sysfs_unlink_rdev(rdev->mddev, rdev);
-		rdev->raid_disk = -1;
+		clear_bit(Blocked, &rdev->flags);
+		remove_and_add_spares(rdev->mddev, rdev);
+		if (rdev->raid_disk >= 0)
+			return -EBUSY;
 		set_bit(MD_RECOVERY_NEEDED, &rdev->mddev->recovery);
 		md_wakeup_thread(rdev->mddev->thread);
 	} else if (rdev->mddev->pers) {
@@ -3221,7 +3218,7 @@ int md_rdev_init(struct md_rdev *rdev)
 	 * be used - I wonder if that matters
 	 */
 	rdev->badblocks.count = 0;
-	rdev->badblocks.shift = 0;
+	rdev->badblocks.shift = -1; /* disabled until explicitly enabled */
 	rdev->badblocks.page = kmalloc(PAGE_SIZE, GFP_KERNEL);
 	seqlock_init(&rdev->badblocks.lock);
 	if (rdev->badblocks.page == NULL)
@@ -3293,9 +3290,6 @@ static struct md_rdev *md_import_device(dev_t newdev, int super_format, int supe
 			goto abort_free;
 		}
 	}
-	if (super_format == -1)
-		/* hot-add for 0.90, or non-persistent: so no badblocks */
-		rdev->badblocks.shift = -1;
 
 	return rdev;
 
@@ -4225,8 +4219,6 @@ action_show(struct mddev *mddev, char *page)
 	return sprintf(page, "%s\n", type);
 }
 
-static void reap_sync_thread(struct mddev *mddev);
-
 static ssize_t
 action_store(struct mddev *mddev, const char *page, size_t len)
 {
@@ -4241,7 +4233,7 @@ action_store(struct mddev *mddev, const char *page, size_t len)
 	if (cmd_match(page, "idle") || cmd_match(page, "frozen")) {
 		if (mddev->sync_thread) {
 			set_bit(MD_RECOVERY_INTR, &mddev->recovery);
-			reap_sync_thread(mddev);
+			md_reap_sync_thread(mddev);
 		}
 	} else if (test_bit(MD_RECOVERY_RUNNING, &mddev->recovery) ||
 		   test_bit(MD_RECOVERY_NEEDED, &mddev->recovery))
@@ -5279,7 +5271,7 @@ static void __md_stop_writes(struct mddev *mddev)
 	if (mddev->sync_thread) {
 		set_bit(MD_RECOVERY_FROZEN, &mddev->recovery);
 		set_bit(MD_RECOVERY_INTR, &mddev->recovery);
-		reap_sync_thread(mddev);
+		md_reap_sync_thread(mddev);
 	}
 
 	del_timer_sync(&mddev->safemode_timer);
@@ -5287,7 +5279,8 @@ static void __md_stop_writes(struct mddev *mddev)
 	bitmap_flush(mddev);
 	md_super_wait(mddev);
 
-	if (!mddev->in_sync || mddev->flags) {
+	if (mddev->ro == 0 &&
+	    (!mddev->in_sync || mddev->flags)) {
 		/* mark array as shutdown cleanly */
 		mddev->in_sync = 1;
 		md_update_sb(mddev, 1);
@@ -5810,7 +5803,7 @@ static int add_new_disk(struct mddev * mddev, mdu_disk_info_t *info)
 		else
 			sysfs_notify_dirent_safe(rdev->sysfs_state);
 
-		md_update_sb(mddev, 1);
+		set_bit(MD_CHANGE_DEVS, &mddev->flags);
 		if (mddev->degraded)
 			set_bit(MD_RECOVERY_RECOVER, &mddev->recovery);
 		set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
@@ -5877,6 +5870,9 @@ static int hot_remove_disk(struct mddev * mddev, dev_t dev)
 	if (!rdev)
 		return -ENXIO;
 
+	clear_bit(Blocked, &rdev->flags);
+	remove_and_add_spares(mddev, rdev);
+
 	if (rdev->raid_disk >= 0)
 		goto busy;
 
@@ -6490,6 +6486,28 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 		err = md_set_readonly(mddev, bdev);
 		goto done_unlock;
 
+	case HOT_REMOVE_DISK:
+		err = hot_remove_disk(mddev, new_decode_dev(arg));
+		goto done_unlock;
+
+	case ADD_NEW_DISK:
+		/* We can support ADD_NEW_DISK on read-only arrays
+		 * on if we are re-adding a preexisting device.
+		 * So require mddev->pers and MD_DISK_SYNC.
+		 */
+		if (mddev->pers) {
+			mdu_disk_info_t info;
+			if (copy_from_user(&info, argp, sizeof(info)))
+				err = -EFAULT;
+			else if (!(info.state & (1<<MD_DISK_SYNC)))
+				/* Need to clear read-only for this */
+				break;
+			else
+				err = add_new_disk(mddev, &info);
+			goto done_unlock;
+		}
+		break;
+
 	case BLKROSET:
 		if (get_user(ro, (int __user *)(arg))) {
 			err = -EFAULT;
@@ -6560,10 +6578,6 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 		goto done_unlock;
 	}
 
-	case HOT_REMOVE_DISK:
-		err = hot_remove_disk(mddev, new_decode_dev(arg));
-		goto done_unlock;
-
 	case HOT_ADD_DISK:
 		err = hot_add_disk(mddev, new_decode_dev(arg));
 		goto done_unlock;
@@ -6651,15 +6665,13 @@ static int md_open(struct block_device *bdev, fmode_t mode)
 	return err;
 }
 
-static int md_release(struct gendisk *disk, fmode_t mode)
+static void md_release(struct gendisk *disk, fmode_t mode)
 {
  	struct mddev *mddev = disk->private_data;
 
 	BUG_ON(!mddev);
 	atomic_dec(&mddev->openers);
 	mddev_put(mddev);
-
-	return 0;
 }
 
 static int md_media_changed(struct gendisk *disk)
@@ -7644,14 +7656,16 @@ void md_do_sync(struct md_thread *thread)
 }
 EXPORT_SYMBOL_GPL(md_do_sync);
 
-static int remove_and_add_spares(struct mddev *mddev)
+static int remove_and_add_spares(struct mddev *mddev,
+				 struct md_rdev *this)
 {
 	struct md_rdev *rdev;
 	int spares = 0;
 	int removed = 0;
 
 	rdev_for_each(rdev, mddev)
-		if (rdev->raid_disk >= 0 &&
+		if ((this == NULL || rdev == this) &&
+		    rdev->raid_disk >= 0 &&
 		    !test_bit(Blocked, &rdev->flags) &&
 		    (test_bit(Faulty, &rdev->flags) ||
 		     ! test_bit(In_sync, &rdev->flags)) &&
@@ -7666,74 +7680,52 @@ static int remove_and_add_spares(struct mddev *mddev)
 	if (removed && mddev->kobj.sd)
 		sysfs_notify(&mddev->kobj, NULL, "degraded");
 
+	if (this)
+		goto no_add;
+
 	rdev_for_each(rdev, mddev) {
 		if (rdev->raid_disk >= 0 &&
 		    !test_bit(In_sync, &rdev->flags) &&
 		    !test_bit(Faulty, &rdev->flags))
 			spares++;
-		if (rdev->raid_disk < 0
-		    && !test_bit(Faulty, &rdev->flags)) {
-			rdev->recovery_offset = 0;
-			if (mddev->pers->
-			    hot_add_disk(mddev, rdev) == 0) {
-				if (sysfs_link_rdev(mddev, rdev))
-					/* failure here is OK */;
-				spares++;
-				md_new_event(mddev);
-				set_bit(MD_CHANGE_DEVS, &mddev->flags);
-			}
+		if (rdev->raid_disk >= 0)
+			continue;
+		if (test_bit(Faulty, &rdev->flags))
+			continue;
+		if (mddev->ro &&
+		    rdev->saved_raid_disk < 0)
+			continue;
+
+		rdev->recovery_offset = 0;
+		if (rdev->saved_raid_disk >= 0 && mddev->in_sync) {
+			spin_lock_irq(&mddev->write_lock);
+			if (mddev->in_sync)
+				/* OK, this device, which is in_sync,
+				 * will definitely be noticed before
+				 * the next write, so recovery isn't
+				 * needed.
+				 */
+				rdev->recovery_offset = mddev->recovery_cp;
+			spin_unlock_irq(&mddev->write_lock);
+		}
+		if (mddev->ro && rdev->recovery_offset != MaxSector)
+			/* not safe to add this disk now */
+			continue;
+		if (mddev->pers->
+		    hot_add_disk(mddev, rdev) == 0) {
+			if (sysfs_link_rdev(mddev, rdev))
+				/* failure here is OK */;
+			spares++;
+			md_new_event(mddev);
+			set_bit(MD_CHANGE_DEVS, &mddev->flags);
 		}
 	}
+no_add:
 	if (removed)
 		set_bit(MD_CHANGE_DEVS, &mddev->flags);
 	return spares;
 }
 
-static void reap_sync_thread(struct mddev *mddev)
-{
-	struct md_rdev *rdev;
-
-	/* resync has finished, collect result */
-	md_unregister_thread(&mddev->sync_thread);
-	if (!test_bit(MD_RECOVERY_INTR, &mddev->recovery) &&
-	    !test_bit(MD_RECOVERY_REQUESTED, &mddev->recovery)) {
-		/* success...*/
-		/* activate any spares */
-		if (mddev->pers->spare_active(mddev)) {
-			sysfs_notify(&mddev->kobj, NULL,
-				     "degraded");
-			set_bit(MD_CHANGE_DEVS, &mddev->flags);
-		}
-	}
-	if (test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery) &&
-	    mddev->pers->finish_reshape)
-		mddev->pers->finish_reshape(mddev);
-
-	/* If array is no-longer degraded, then any saved_raid_disk
-	 * information must be scrapped.  Also if any device is now
-	 * In_sync we must scrape the saved_raid_disk for that device
-	 * do the superblock for an incrementally recovered device
-	 * written out.
-	 */
-	rdev_for_each(rdev, mddev)
-		if (!mddev->degraded ||
-		    test_bit(In_sync, &rdev->flags))
-			rdev->saved_raid_disk = -1;
-
-	md_update_sb(mddev, 1);
-	clear_bit(MD_RECOVERY_RUNNING, &mddev->recovery);
-	clear_bit(MD_RECOVERY_SYNC, &mddev->recovery);
-	clear_bit(MD_RECOVERY_RESHAPE, &mddev->recovery);
-	clear_bit(MD_RECOVERY_REQUESTED, &mddev->recovery);
-	clear_bit(MD_RECOVERY_CHECK, &mddev->recovery);
-	/* flag recovery needed just to double check */
-	set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
-	sysfs_notify_dirent_safe(mddev->sysfs_action);
-	md_new_event(mddev);
-	if (mddev->event_work.func)
-		queue_work(md_misc_wq, &mddev->event_work);
-}
-
 /*
  * This routine is regularly called by all per-raid-array threads to
  * deal with generic issues like resync and super-block update.
@@ -7789,22 +7781,16 @@ void md_check_recovery(struct mddev *mddev)
 		int spares = 0;
 
 		if (mddev->ro) {
-			/* Only thing we do on a ro array is remove
-			 * failed devices.
+			/* On a read-only array we can:
+			 * - remove failed devices
+			 * - add already-in_sync devices if the array itself
+			 *   is in-sync.
+			 * As we only add devices that are already in-sync,
+			 * we can activate the spares immediately.
 			 */
-			struct md_rdev *rdev;
-			rdev_for_each(rdev, mddev)
-				if (rdev->raid_disk >= 0 &&
-				    !test_bit(Blocked, &rdev->flags) &&
-				    test_bit(Faulty, &rdev->flags) &&
-				    atomic_read(&rdev->nr_pending)==0) {
-					if (mddev->pers->hot_remove_disk(
-						    mddev, rdev) == 0) {
-						sysfs_unlink_rdev(mddev, rdev);
-						rdev->raid_disk = -1;
-					}
-				}
 			clear_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
+			remove_and_add_spares(mddev, NULL);
+			mddev->pers->spare_active(mddev);
 			goto unlock;
 		}
 
@@ -7836,7 +7822,7 @@ void md_check_recovery(struct mddev *mddev)
 			goto unlock;
 		}
 		if (mddev->sync_thread) {
-			reap_sync_thread(mddev);
+			md_reap_sync_thread(mddev);
 			goto unlock;
 		}
 		/* Set RUNNING before clearing NEEDED to avoid
@@ -7867,7 +7853,7 @@ void md_check_recovery(struct mddev *mddev)
 				goto unlock;
 			set_bit(MD_RECOVERY_RESHAPE, &mddev->recovery);
 			clear_bit(MD_RECOVERY_RECOVER, &mddev->recovery);
-		} else if ((spares = remove_and_add_spares(mddev))) {
+		} else if ((spares = remove_and_add_spares(mddev, NULL))) {
 			clear_bit(MD_RECOVERY_SYNC, &mddev->recovery);
 			clear_bit(MD_RECOVERY_CHECK, &mddev->recovery);
 			clear_bit(MD_RECOVERY_REQUESTED, &mddev->recovery);
@@ -7917,6 +7903,51 @@ void md_check_recovery(struct mddev *mddev)
 	}
 }
 
+void md_reap_sync_thread(struct mddev *mddev)
+{
+	struct md_rdev *rdev;
+
+	/* resync has finished, collect result */
+	md_unregister_thread(&mddev->sync_thread);
+	if (!test_bit(MD_RECOVERY_INTR, &mddev->recovery) &&
+	    !test_bit(MD_RECOVERY_REQUESTED, &mddev->recovery)) {
+		/* success...*/
+		/* activate any spares */
+		if (mddev->pers->spare_active(mddev)) {
+			sysfs_notify(&mddev->kobj, NULL,
+				     "degraded");
+			set_bit(MD_CHANGE_DEVS, &mddev->flags);
+		}
+	}
+	if (test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery) &&
+	    mddev->pers->finish_reshape)
+		mddev->pers->finish_reshape(mddev);
+
+	/* If array is no-longer degraded, then any saved_raid_disk
+	 * information must be scrapped.  Also if any device is now
+	 * In_sync we must scrape the saved_raid_disk for that device
+	 * do the superblock for an incrementally recovered device
+	 * written out.
+	 */
+	rdev_for_each(rdev, mddev)
+		if (!mddev->degraded ||
+		    test_bit(In_sync, &rdev->flags))
+			rdev->saved_raid_disk = -1;
+
+	md_update_sb(mddev, 1);
+	clear_bit(MD_RECOVERY_RUNNING, &mddev->recovery);
+	clear_bit(MD_RECOVERY_SYNC, &mddev->recovery);
+	clear_bit(MD_RECOVERY_RESHAPE, &mddev->recovery);
+	clear_bit(MD_RECOVERY_REQUESTED, &mddev->recovery);
+	clear_bit(MD_RECOVERY_CHECK, &mddev->recovery);
+	/* flag recovery needed just to double check */
+	set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
+	sysfs_notify_dirent_safe(mddev->sysfs_action);
+	md_new_event(mddev);
+	if (mddev->event_work.func)
+		queue_work(md_misc_wq, &mddev->event_work);
+}
+
 void md_wait_for_blocked_rdev(struct md_rdev *rdev, struct mddev *mddev)
 {
 	sysfs_notify_dirent_safe(rdev->sysfs_state);
@@ -8642,6 +8673,7 @@ EXPORT_SYMBOL(md_register_thread);
 EXPORT_SYMBOL(md_unregister_thread);
 EXPORT_SYMBOL(md_wakeup_thread);
 EXPORT_SYMBOL(md_check_recovery);
+EXPORT_SYMBOL(md_reap_sync_thread);
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("MD RAID framework");
 MODULE_ALIAS("md");
diff --git a/drivers/md/md.h b/drivers/md/md.h
index d90fb1a879e1..653f992b687a 100644
--- a/drivers/md/md.h
+++ b/drivers/md/md.h
@@ -567,6 +567,7 @@ extern struct md_thread *md_register_thread(
 extern void md_unregister_thread(struct md_thread **threadp);
 extern void md_wakeup_thread(struct md_thread *thread);
 extern void md_check_recovery(struct mddev *mddev);
+extern void md_reap_sync_thread(struct mddev *mddev);
 extern void md_write_start(struct mddev *mddev, struct bio *bi);
 extern void md_write_end(struct mddev *mddev);
 extern void md_done_sync(struct mddev *mddev, int blocks, int ok);
diff --git a/drivers/md/raid0.c b/drivers/md/raid0.c
index 0505452de8d6..fcf65e512cf5 100644
--- a/drivers/md/raid0.c
+++ b/drivers/md/raid0.c
@@ -502,11 +502,11 @@ static inline int is_io_in_chunk_boundary(struct mddev *mddev,
 {
 	if (likely(is_power_of_2(chunk_sects))) {
 		return chunk_sects >= ((bio->bi_sector & (chunk_sects-1))
-					+ (bio->bi_size >> 9));
+					+ bio_sectors(bio));
 	} else{
 		sector_t sector = bio->bi_sector;
 		return chunk_sects >= (sector_div(sector, chunk_sects)
-						+ (bio->bi_size >> 9));
+						+ bio_sectors(bio));
 	}
 }
 
@@ -527,8 +527,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 && bio->bi_vcnt != 0) ||
-		    bio->bi_idx != 0)
+		if (bio_segments(bio) > 1)
 			goto bad_map;
 		/* This is a one page bio that upper layers
 		 * refuse to split for us, so we need to split it.
@@ -567,7 +566,7 @@ bad_map:
 	printk("md/raid0:%s: make_request bug: can't convert block across chunks"
 	       " or bigger than %dk %llu %d\n",
 	       mdname(mddev), chunk_sects / 2,
-	       (unsigned long long)bio->bi_sector, bio->bi_size >> 10);
+	       (unsigned long long)bio->bi_sector, bio_sectors(bio) / 2);
 
 	bio_io_error(bio);
 	return;
diff --git a/drivers/md/raid1.c b/drivers/md/raid1.c
index fd86b372692d..55951182af73 100644
--- a/drivers/md/raid1.c
+++ b/drivers/md/raid1.c
@@ -92,7 +92,6 @@ static void r1bio_pool_free(void *r1_bio, void *data)
 static void * r1buf_pool_alloc(gfp_t gfp_flags, void *data)
 {
 	struct pool_info *pi = data;
-	struct page *page;
 	struct r1bio *r1_bio;
 	struct bio *bio;
 	int i, j;
@@ -122,14 +121,10 @@ static void * r1buf_pool_alloc(gfp_t gfp_flags, void *data)
 		j = 1;
 	while(j--) {
 		bio = r1_bio->bios[j];
-		for (i = 0; i < RESYNC_PAGES; i++) {
-			page = alloc_page(gfp_flags);
-			if (unlikely(!page))
-				goto out_free_pages;
+		bio->bi_vcnt = RESYNC_PAGES;
 
-			bio->bi_io_vec[i].bv_page = page;
-			bio->bi_vcnt = i+1;
-		}
+		if (bio_alloc_pages(bio, gfp_flags))
+			goto out_free_bio;
 	}
 	/* If not user-requests, copy the page pointers to all bios */
 	if (!test_bit(MD_RECOVERY_REQUESTED, &pi->mddev->recovery)) {
@@ -143,11 +138,6 @@ static void * r1buf_pool_alloc(gfp_t gfp_flags, void *data)
 
 	return r1_bio;
 
-out_free_pages:
-	for (j=0 ; j < pi->raid_disks; j++)
-		for (i=0; i < r1_bio->bios[j]->bi_vcnt ; i++)
-			put_page(r1_bio->bios[j]->bi_io_vec[i].bv_page);
-	j = -1;
 out_free_bio:
 	while (++j < pi->raid_disks)
 		bio_put(r1_bio->bios[j]);
@@ -267,7 +257,7 @@ static void raid_end_bio_io(struct r1bio *r1_bio)
 			 (bio_data_dir(bio) == WRITE) ? "write" : "read",
 			 (unsigned long long) bio->bi_sector,
 			 (unsigned long long) bio->bi_sector +
-			 (bio->bi_size >> 9) - 1);
+			 bio_sectors(bio) - 1);
 
 		call_bio_endio(r1_bio);
 	}
@@ -458,7 +448,7 @@ static void raid1_end_write_request(struct bio *bio, int error)
 					 " %llu-%llu\n",
 					 (unsigned long long) mbio->bi_sector,
 					 (unsigned long long) mbio->bi_sector +
-					 (mbio->bi_size >> 9) - 1);
+					 bio_sectors(mbio) - 1);
 				call_bio_endio(r1_bio);
 			}
 		}
@@ -925,7 +915,7 @@ static void alloc_behind_pages(struct bio *bio, struct r1bio *r1_bio)
 	if (unlikely(!bvecs))
 		return;
 
-	bio_for_each_segment(bvec, bio, i) {
+	bio_for_each_segment_all(bvec, bio, i) {
 		bvecs[i] = *bvec;
 		bvecs[i].bv_page = alloc_page(GFP_NOIO);
 		if (unlikely(!bvecs[i].bv_page))
@@ -981,7 +971,12 @@ static void raid1_unplug(struct blk_plug_cb *cb, bool from_schedule)
 	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;
 	}
 	kfree(plug);
@@ -1018,7 +1013,7 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 	md_write_start(mddev, bio); /* wait on superblock update early */
 
 	if (bio_data_dir(bio) == WRITE &&
-	    bio->bi_sector + bio->bi_size/512 > mddev->suspend_lo &&
+	    bio_end_sector(bio) > mddev->suspend_lo &&
 	    bio->bi_sector < mddev->suspend_hi) {
 		/* As the suspend_* range is controlled by
 		 * userspace, we want an interruptible
@@ -1029,7 +1024,7 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 			flush_signals(current);
 			prepare_to_wait(&conf->wait_barrier,
 					&w, TASK_INTERRUPTIBLE);
-			if (bio->bi_sector + bio->bi_size/512 <= mddev->suspend_lo ||
+			if (bio_end_sector(bio) <= mddev->suspend_lo ||
 			    bio->bi_sector >= mddev->suspend_hi)
 				break;
 			schedule();
@@ -1049,7 +1044,7 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 	r1_bio = mempool_alloc(conf->r1bio_pool, GFP_NOIO);
 
 	r1_bio->master_bio = bio;
-	r1_bio->sectors = bio->bi_size >> 9;
+	r1_bio->sectors = bio_sectors(bio);
 	r1_bio->state = 0;
 	r1_bio->mddev = mddev;
 	r1_bio->sector = bio->bi_sector;
@@ -1127,7 +1122,7 @@ read_again:
 			r1_bio = mempool_alloc(conf->r1bio_pool, GFP_NOIO);
 
 			r1_bio->master_bio = bio;
-			r1_bio->sectors = (bio->bi_size >> 9) - sectors_handled;
+			r1_bio->sectors = bio_sectors(bio) - sectors_handled;
 			r1_bio->state = 0;
 			r1_bio->mddev = mddev;
 			r1_bio->sector = bio->bi_sector + sectors_handled;
@@ -1284,14 +1279,10 @@ read_again:
 			struct bio_vec *bvec;
 			int j;
 
-			/* Yes, I really want the '__' version so that
-			 * we clear any unused pointer in the io_vec, rather
-			 * than leave them unchanged.  This is important
-			 * because when we come to free the pages, we won't
-			 * know the original bi_idx, so we just free
-			 * them all
+			/*
+			 * We trimmed the bio, so _all is legit
 			 */
-			__bio_for_each_segment(bvec, mbio, j, 0)
+			bio_for_each_segment_all(bvec, mbio, j)
 				bvec->bv_page = r1_bio->behind_bvecs[j].bv_page;
 			if (test_bit(WriteMostly, &conf->mirrors[i].rdev->flags))
 				atomic_inc(&r1_bio->behind_remaining);
@@ -1329,14 +1320,14 @@ read_again:
 	/* Mustn't call r1_bio_write_done before this next test,
 	 * as it could result in the bio being freed.
 	 */
-	if (sectors_handled < (bio->bi_size >> 9)) {
+	if (sectors_handled < bio_sectors(bio)) {
 		r1_bio_write_done(r1_bio);
 		/* We need another r1_bio.  It has already been counted
 		 * in bio->bi_phys_segments
 		 */
 		r1_bio = mempool_alloc(conf->r1bio_pool, GFP_NOIO);
 		r1_bio->master_bio = bio;
-		r1_bio->sectors = (bio->bi_size >> 9) - sectors_handled;
+		r1_bio->sectors = bio_sectors(bio) - sectors_handled;
 		r1_bio->state = 0;
 		r1_bio->mddev = mddev;
 		r1_bio->sector = bio->bi_sector + sectors_handled;
@@ -1862,7 +1853,7 @@ static int process_checks(struct r1bio *r1_bio)
 		struct bio *sbio = r1_bio->bios[i];
 		int size;
 
-		if (r1_bio->bios[i]->bi_end_io != end_sync_read)
+		if (sbio->bi_end_io != end_sync_read)
 			continue;
 
 		if (test_bit(BIO_UPTODATE, &sbio->bi_flags)) {
@@ -1887,16 +1878,15 @@ static int process_checks(struct r1bio *r1_bio)
 			continue;
 		}
 		/* fixup the bio for reuse */
+		bio_reset(sbio);
 		sbio->bi_vcnt = vcnt;
 		sbio->bi_size = r1_bio->sectors << 9;
-		sbio->bi_idx = 0;
-		sbio->bi_phys_segments = 0;
-		sbio->bi_flags &= ~(BIO_POOL_MASK - 1);
-		sbio->bi_flags |= 1 << BIO_UPTODATE;
-		sbio->bi_next = NULL;
 		sbio->bi_sector = r1_bio->sector +
 			conf->mirrors[i].rdev->data_offset;
 		sbio->bi_bdev = conf->mirrors[i].rdev->bdev;
+		sbio->bi_end_io = end_sync_read;
+		sbio->bi_private = r1_bio;
+
 		size = sbio->bi_size;
 		for (j = 0; j < vcnt ; j++) {
 			struct bio_vec *bi;
@@ -1907,10 +1897,9 @@ static int process_checks(struct r1bio *r1_bio)
 			else
 				bi->bv_len = size;
 			size -= PAGE_SIZE;
-			memcpy(page_address(bi->bv_page),
-			       page_address(pbio->bi_io_vec[j].bv_page),
-			       PAGE_SIZE);
 		}
+
+		bio_copy_data(sbio, pbio);
 	}
 	return 0;
 }
@@ -1947,7 +1936,7 @@ static void sync_request_write(struct mddev *mddev, struct r1bio *r1_bio)
 		wbio->bi_rw = WRITE;
 		wbio->bi_end_io = end_sync_write;
 		atomic_inc(&r1_bio->remaining);
-		md_sync_acct(conf->mirrors[i].rdev->bdev, wbio->bi_size >> 9);
+		md_sync_acct(conf->mirrors[i].rdev->bdev, bio_sectors(wbio));
 
 		generic_make_request(wbio);
 	}
@@ -2059,32 +2048,11 @@ static void fix_read_error(struct r1conf *conf, int read_disk,
 	}
 }
 
-static void bi_complete(struct bio *bio, int error)
-{
-	complete((struct completion *)bio->bi_private);
-}
-
-static int submit_bio_wait(int rw, struct bio *bio)
-{
-	struct completion event;
-	rw |= REQ_SYNC;
-
-	init_completion(&event);
-	bio->bi_private = &event;
-	bio->bi_end_io = bi_complete;
-	submit_bio(rw, bio);
-	wait_for_completion(&event);
-
-	return test_bit(BIO_UPTODATE, &bio->bi_flags);
-}
-
 static int narrow_write_error(struct r1bio *r1_bio, int i)
 {
 	struct mddev *mddev = r1_bio->mddev;
 	struct r1conf *conf = mddev->private;
 	struct md_rdev *rdev = conf->mirrors[i].rdev;
-	int vcnt, idx;
-	struct bio_vec *vec;
 
 	/* bio has the data to be written to device 'i' where
 	 * we just recently had a write error.
@@ -2112,30 +2080,32 @@ static int narrow_write_error(struct r1bio *r1_bio, int i)
 		   & ~(sector_t)(block_sectors - 1))
 		- sector;
 
-	if (test_bit(R1BIO_BehindIO, &r1_bio->state)) {
-		vcnt = r1_bio->behind_page_count;
-		vec = r1_bio->behind_bvecs;
-		idx = 0;
-		while (vec[idx].bv_page == NULL)
-			idx++;
-	} else {
-		vcnt = r1_bio->master_bio->bi_vcnt;
-		vec = r1_bio->master_bio->bi_io_vec;
-		idx = r1_bio->master_bio->bi_idx;
-	}
 	while (sect_to_write) {
 		struct bio *wbio;
 		if (sectors > sect_to_write)
 			sectors = sect_to_write;
 		/* Write at 'sector' for 'sectors'*/
 
-		wbio = bio_alloc_mddev(GFP_NOIO, vcnt, mddev);
-		memcpy(wbio->bi_io_vec, vec, vcnt * sizeof(struct bio_vec));
-		wbio->bi_sector = r1_bio->sector;
+		if (test_bit(R1BIO_BehindIO, &r1_bio->state)) {
+			unsigned vcnt = r1_bio->behind_page_count;
+			struct bio_vec *vec = r1_bio->behind_bvecs;
+
+			while (!vec->bv_page) {
+				vec++;
+				vcnt--;
+			}
+
+			wbio = bio_alloc_mddev(GFP_NOIO, vcnt, mddev);
+			memcpy(wbio->bi_io_vec, vec, vcnt * sizeof(struct bio_vec));
+
+			wbio->bi_vcnt = vcnt;
+		} else {
+			wbio = bio_clone_mddev(r1_bio->master_bio, GFP_NOIO, mddev);
+		}
+
 		wbio->bi_rw = WRITE;
-		wbio->bi_vcnt = vcnt;
+		wbio->bi_sector = r1_bio->sector;
 		wbio->bi_size = r1_bio->sectors << 9;
-		wbio->bi_idx = idx;
 
 		md_trim_bio(wbio, sector - r1_bio->sector, sectors);
 		wbio->bi_sector += rdev->data_offset;
@@ -2284,8 +2254,7 @@ read_more:
 			r1_bio = mempool_alloc(conf->r1bio_pool, GFP_NOIO);
 
 			r1_bio->master_bio = mbio;
-			r1_bio->sectors = (mbio->bi_size >> 9)
-					  - sectors_handled;
+			r1_bio->sectors = bio_sectors(mbio) - sectors_handled;
 			r1_bio->state = 0;
 			set_bit(R1BIO_ReadError, &r1_bio->state);
 			r1_bio->mddev = mddev;
@@ -2459,18 +2428,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr, int *skipp
 	for (i = 0; i < conf->raid_disks * 2; i++) {
 		struct md_rdev *rdev;
 		bio = r1_bio->bios[i];
-
-		/* take from bio_init */
-		bio->bi_next = NULL;
-		bio->bi_flags &= ~(BIO_POOL_MASK-1);
-		bio->bi_flags |= 1 << BIO_UPTODATE;
-		bio->bi_rw = READ;
-		bio->bi_vcnt = 0;
-		bio->bi_idx = 0;
-		bio->bi_phys_segments = 0;
-		bio->bi_size = 0;
-		bio->bi_end_io = NULL;
-		bio->bi_private = NULL;
+		bio_reset(bio);
 
 		rdev = rcu_dereference(conf->mirrors[i].rdev);
 		if (rdev == NULL ||
@@ -2901,6 +2859,7 @@ static int stop(struct mddev *mddev)
 	if (conf->r1bio_pool)
 		mempool_destroy(conf->r1bio_pool);
 	kfree(conf->mirrors);
+	safe_put_page(conf->tmppage);
 	kfree(conf->poolinfo);
 	kfree(conf);
 	mddev->private = NULL;
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index 77b562d18a90..59d4daa5f4c7 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -1133,7 +1133,12 @@ static void raid10_unplug(struct blk_plug_cb *cb, bool from_schedule)
 	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;
 	}
 	kfree(plug);
@@ -1169,14 +1174,13 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 	/* If this request crosses a chunk boundary, we need to
 	 * split it.  This will only happen for 1 PAGE (or less) requests.
 	 */
-	if (unlikely((bio->bi_sector & chunk_mask) + (bio->bi_size >> 9)
+	if (unlikely((bio->bi_sector & chunk_mask) + bio_sectors(bio)
 		     > chunk_sects
 		     && (conf->geo.near_copies < conf->geo.raid_disks
 			 || 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 && bio->bi_vcnt != 0) ||
-		    bio->bi_idx != 0)
+		if (bio_segments(bio) > 1)
 			goto bad_map;
 		/* This is a one page bio that upper layers
 		 * refuse to split for us, so we need to split it.
@@ -1209,7 +1213,7 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 	bad_map:
 		printk("md/raid10:%s: make_request bug: can't convert block across chunks"
 		       " or bigger than %dk %llu %d\n", mdname(mddev), chunk_sects/2,
-		       (unsigned long long)bio->bi_sector, bio->bi_size >> 10);
+		       (unsigned long long)bio->bi_sector, bio_sectors(bio) / 2);
 
 		bio_io_error(bio);
 		return;
@@ -1224,7 +1228,7 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 	 */
 	wait_barrier(conf);
 
-	sectors = bio->bi_size >> 9;
+	sectors = bio_sectors(bio);
 	while (test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery) &&
 	    bio->bi_sector < conf->reshape_progress &&
 	    bio->bi_sector + sectors > conf->reshape_progress) {
@@ -1326,8 +1330,7 @@ read_again:
 			r10_bio = mempool_alloc(conf->r10bio_pool, GFP_NOIO);
 
 			r10_bio->master_bio = bio;
-			r10_bio->sectors = ((bio->bi_size >> 9)
-					    - sectors_handled);
+			r10_bio->sectors = bio_sectors(bio) - sectors_handled;
 			r10_bio->state = 0;
 			r10_bio->mddev = mddev;
 			r10_bio->sector = bio->bi_sector + sectors_handled;
@@ -1569,7 +1572,7 @@ retry_write:
 	 * after checking if we need to go around again.
 	 */
 
-	if (sectors_handled < (bio->bi_size >> 9)) {
+	if (sectors_handled < bio_sectors(bio)) {
 		one_write_done(r10_bio);
 		/* We need another r10_bio.  It has already been counted
 		 * in bio->bi_phys_segments.
@@ -1577,7 +1580,7 @@ retry_write:
 		r10_bio = mempool_alloc(conf->r10bio_pool, GFP_NOIO);
 
 		r10_bio->master_bio = bio;
-		r10_bio->sectors = (bio->bi_size >> 9) - sectors_handled;
+		r10_bio->sectors = bio_sectors(bio) - sectors_handled;
 
 		r10_bio->mddev = mddev;
 		r10_bio->sector = bio->bi_sector + sectors_handled;
@@ -2079,13 +2082,10 @@ static void sync_request_write(struct mddev *mddev, struct r10bio *r10_bio)
 		 * First we need to fixup bv_offset, bv_len and
 		 * bi_vecs, as the read request might have corrupted these
 		 */
+		bio_reset(tbio);
+
 		tbio->bi_vcnt = vcnt;
 		tbio->bi_size = r10_bio->sectors << 9;
-		tbio->bi_idx = 0;
-		tbio->bi_phys_segments = 0;
-		tbio->bi_flags &= ~(BIO_POOL_MASK - 1);
-		tbio->bi_flags |= 1 << BIO_UPTODATE;
-		tbio->bi_next = NULL;
 		tbio->bi_rw = WRITE;
 		tbio->bi_private = r10_bio;
 		tbio->bi_sector = r10_bio->devs[i].addr;
@@ -2103,7 +2103,7 @@ static void sync_request_write(struct mddev *mddev, struct r10bio *r10_bio)
 		d = r10_bio->devs[i].devnum;
 		atomic_inc(&conf->mirrors[d].rdev->nr_pending);
 		atomic_inc(&r10_bio->remaining);
-		md_sync_acct(conf->mirrors[d].rdev->bdev, tbio->bi_size >> 9);
+		md_sync_acct(conf->mirrors[d].rdev->bdev, bio_sectors(tbio));
 
 		tbio->bi_sector += conf->mirrors[d].rdev->data_offset;
 		tbio->bi_bdev = conf->mirrors[d].rdev->bdev;
@@ -2128,7 +2128,7 @@ static void sync_request_write(struct mddev *mddev, struct r10bio *r10_bio)
 		d = r10_bio->devs[i].devnum;
 		atomic_inc(&r10_bio->remaining);
 		md_sync_acct(conf->mirrors[d].replacement->bdev,
-			     tbio->bi_size >> 9);
+			     bio_sectors(tbio));
 		generic_make_request(tbio);
 	}
 
@@ -2254,13 +2254,13 @@ static void recovery_request_write(struct mddev *mddev, struct r10bio *r10_bio)
 	wbio2 = r10_bio->devs[1].repl_bio;
 	if (wbio->bi_end_io) {
 		atomic_inc(&conf->mirrors[d].rdev->nr_pending);
-		md_sync_acct(conf->mirrors[d].rdev->bdev, wbio->bi_size >> 9);
+		md_sync_acct(conf->mirrors[d].rdev->bdev, bio_sectors(wbio));
 		generic_make_request(wbio);
 	}
 	if (wbio2 && wbio2->bi_end_io) {
 		atomic_inc(&conf->mirrors[d].replacement->nr_pending);
 		md_sync_acct(conf->mirrors[d].replacement->bdev,
-			     wbio2->bi_size >> 9);
+			     bio_sectors(wbio2));
 		generic_make_request(wbio2);
 	}
 }
@@ -2531,25 +2531,6 @@ static void fix_read_error(struct r10conf *conf, struct mddev *mddev, struct r10
 	}
 }
 
-static void bi_complete(struct bio *bio, int error)
-{
-	complete((struct completion *)bio->bi_private);
-}
-
-static int submit_bio_wait(int rw, struct bio *bio)
-{
-	struct completion event;
-	rw |= REQ_SYNC;
-
-	init_completion(&event);
-	bio->bi_private = &event;
-	bio->bi_end_io = bi_complete;
-	submit_bio(rw, bio);
-	wait_for_completion(&event);
-
-	return test_bit(BIO_UPTODATE, &bio->bi_flags);
-}
-
 static int narrow_write_error(struct r10bio *r10_bio, int i)
 {
 	struct bio *bio = r10_bio->master_bio;
@@ -2690,8 +2671,7 @@ read_more:
 		r10_bio = mempool_alloc(conf->r10bio_pool,
 					GFP_NOIO);
 		r10_bio->master_bio = mbio;
-		r10_bio->sectors = (mbio->bi_size >> 9)
-			- sectors_handled;
+		r10_bio->sectors = bio_sectors(mbio) - sectors_handled;
 		r10_bio->state = 0;
 		set_bit(R10BIO_ReadError,
 			&r10_bio->state);
@@ -2913,6 +2893,22 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 		if (init_resync(conf))
 			return 0;
 
+	/*
+	 * Allow skipping a full rebuild for incremental assembly
+	 * of a clean array, like RAID1 does.
+	 */
+	if (mddev->bitmap == NULL &&
+	    mddev->recovery_cp == MaxSector &&
+	    !test_bit(MD_RECOVERY_REQUESTED, &mddev->recovery) &&
+	    conf->fullsync == 0) {
+		*skipped = 1;
+		max_sector = mddev->dev_sectors;
+		if (test_bit(MD_RECOVERY_SYNC, &mddev->recovery) ||
+		    test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery))
+			max_sector = mddev->resync_max_sectors;
+		return max_sector - sector_nr;
+	}
+
  skipped:
 	max_sector = mddev->dev_sectors;
 	if (test_bit(MD_RECOVERY_SYNC, &mddev->recovery) ||
@@ -3112,6 +3108,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 					}
 				}
 				bio = r10_bio->devs[0].bio;
+				bio_reset(bio);
 				bio->bi_next = biolist;
 				biolist = bio;
 				bio->bi_private = r10_bio;
@@ -3136,6 +3133,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 				rdev = mirror->rdev;
 				if (!test_bit(In_sync, &rdev->flags)) {
 					bio = r10_bio->devs[1].bio;
+					bio_reset(bio);
 					bio->bi_next = biolist;
 					biolist = bio;
 					bio->bi_private = r10_bio;
@@ -3164,6 +3162,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 				if (rdev == NULL || bio == NULL ||
 				    test_bit(Faulty, &rdev->flags))
 					break;
+				bio_reset(bio);
 				bio->bi_next = biolist;
 				biolist = bio;
 				bio->bi_private = r10_bio;
@@ -3262,7 +3261,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 				r10_bio->devs[i].repl_bio->bi_end_io = NULL;
 
 			bio = r10_bio->devs[i].bio;
-			bio->bi_end_io = NULL;
+			bio_reset(bio);
 			clear_bit(BIO_UPTODATE, &bio->bi_flags);
 			if (conf->mirrors[d].rdev == NULL ||
 			    test_bit(Faulty, &conf->mirrors[d].rdev->flags))
@@ -3299,6 +3298,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 
 			/* Need to set up for writing to the replacement */
 			bio = r10_bio->devs[i].repl_bio;
+			bio_reset(bio);
 			clear_bit(BIO_UPTODATE, &bio->bi_flags);
 
 			sector = r10_bio->devs[i].addr;
@@ -3332,17 +3332,6 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 		}
 	}
 
-	for (bio = biolist; bio ; bio=bio->bi_next) {
-
-		bio->bi_flags &= ~(BIO_POOL_MASK - 1);
-		if (bio->bi_end_io)
-			bio->bi_flags |= 1 << BIO_UPTODATE;
-		bio->bi_vcnt = 0;
-		bio->bi_idx = 0;
-		bio->bi_phys_segments = 0;
-		bio->bi_size = 0;
-	}
-
 	nr_sectors = 0;
 	if (sector_nr + max_sync < max_sector)
 		max_sector = sector_nr + max_sync;
@@ -3810,6 +3799,7 @@ static int stop(struct mddev *mddev)
 
 	if (conf->r10bio_pool)
 		mempool_destroy(conf->r10bio_pool);
+	safe_put_page(conf->tmppage);
 	kfree(conf->mirrors);
 	kfree(conf);
 	mddev->private = NULL;
@@ -4389,7 +4379,6 @@ read_more:
 	read_bio->bi_flags &= ~(BIO_POOL_MASK - 1);
 	read_bio->bi_flags |= 1 << BIO_UPTODATE;
 	read_bio->bi_vcnt = 0;
-	read_bio->bi_idx = 0;
 	read_bio->bi_size = 0;
 	r10_bio->master_bio = read_bio;
 	r10_bio->read_slot = r10_bio->devs[r10_bio->read_slot].devnum;
@@ -4413,17 +4402,14 @@ read_more:
 		}
 		if (!rdev2 || test_bit(Faulty, &rdev2->flags))
 			continue;
+
+		bio_reset(b);
 		b->bi_bdev = rdev2->bdev;
 		b->bi_sector = r10_bio->devs[s/2].addr + rdev2->new_data_offset;
 		b->bi_private = r10_bio;
 		b->bi_end_io = end_reshape_write;
 		b->bi_rw = WRITE;
-		b->bi_flags &= ~(BIO_POOL_MASK - 1);
-		b->bi_flags |= 1 << BIO_UPTODATE;
 		b->bi_next = blist;
-		b->bi_vcnt = 0;
-		b->bi_idx = 0;
-		b->bi_size = 0;
 		blist = b;
 	}
 
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index f4e87bfc7567..9359828ffe26 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -90,7 +90,7 @@ static inline struct hlist_head *stripe_hash(struct r5conf *conf, sector_t sect)
  */
 static inline struct bio *r5_next_bio(struct bio *bio, sector_t sector)
 {
-	int sectors = bio->bi_size >> 9;
+	int sectors = bio_sectors(bio);
 	if (bio->bi_sector + sectors < sector + STRIPE_SECTORS)
 		return bio->bi_next;
 	else
@@ -569,14 +569,6 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 		bi = &sh->dev[i].req;
 		rbi = &sh->dev[i].rreq; /* For writing to replacement */
 
-		bi->bi_rw = rw;
-		rbi->bi_rw = rw;
-		if (rw & WRITE) {
-			bi->bi_end_io = raid5_end_write_request;
-			rbi->bi_end_io = raid5_end_write_request;
-		} else
-			bi->bi_end_io = raid5_end_read_request;
-
 		rcu_read_lock();
 		rrdev = rcu_dereference(conf->disks[i].replacement);
 		smp_mb(); /* Ensure that if rrdev is NULL, rdev won't be */
@@ -651,7 +643,14 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 
 			set_bit(STRIPE_IO_STARTED, &sh->state);
 
+			bio_reset(bi);
 			bi->bi_bdev = rdev->bdev;
+			bi->bi_rw = rw;
+			bi->bi_end_io = (rw & WRITE)
+				? raid5_end_write_request
+				: raid5_end_read_request;
+			bi->bi_private = sh;
+
 			pr_debug("%s: for %llu schedule op %ld on disc %d\n",
 				__func__, (unsigned long long)sh->sector,
 				bi->bi_rw, i);
@@ -665,12 +664,9 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 			if (test_bit(R5_ReadNoMerge, &sh->dev[i].flags))
 				bi->bi_rw |= REQ_FLUSH;
 
-			bi->bi_flags = 1 << BIO_UPTODATE;
-			bi->bi_idx = 0;
 			bi->bi_io_vec[0].bv_len = STRIPE_SIZE;
 			bi->bi_io_vec[0].bv_offset = 0;
 			bi->bi_size = STRIPE_SIZE;
-			bi->bi_next = NULL;
 			if (rrdev)
 				set_bit(R5_DOUBLE_LOCKED, &sh->dev[i].flags);
 
@@ -687,7 +683,13 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 
 			set_bit(STRIPE_IO_STARTED, &sh->state);
 
+			bio_reset(rbi);
 			rbi->bi_bdev = rrdev->bdev;
+			rbi->bi_rw = rw;
+			BUG_ON(!(rw & WRITE));
+			rbi->bi_end_io = raid5_end_write_request;
+			rbi->bi_private = sh;
+
 			pr_debug("%s: for %llu schedule op %ld on "
 				 "replacement disc %d\n",
 				__func__, (unsigned long long)sh->sector,
@@ -699,12 +701,9 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 			else
 				rbi->bi_sector = (sh->sector
 						  + rrdev->data_offset);
-			rbi->bi_flags = 1 << BIO_UPTODATE;
-			rbi->bi_idx = 0;
 			rbi->bi_io_vec[0].bv_len = STRIPE_SIZE;
 			rbi->bi_io_vec[0].bv_offset = 0;
 			rbi->bi_size = STRIPE_SIZE;
-			rbi->bi_next = NULL;
 			if (conf->mddev->gendisk)
 				trace_block_bio_remap(bdev_get_queue(rbi->bi_bdev),
 						      rbi, disk_devt(conf->mddev->gendisk),
@@ -1887,8 +1886,15 @@ static void raid5_end_write_request(struct bio *bi, int error)
 					&rdev->mddev->recovery);
 		} else if (is_badblock(rdev, sh->sector,
 				       STRIPE_SECTORS,
-				       &first_bad, &bad_sectors))
+				       &first_bad, &bad_sectors)) {
 			set_bit(R5_MadeGood, &sh->dev[i].flags);
+			if (test_bit(R5_ReadError, &sh->dev[i].flags))
+				/* That was a successful write so make
+				 * sure it looks like we already did
+				 * a re-write.
+				 */
+				set_bit(R5_ReWrite, &sh->dev[i].flags);
+		}
 	}
 	rdev_dec_pending(rdev, conf->mddev);
 
@@ -2395,11 +2401,11 @@ static int add_stripe_bio(struct stripe_head *sh, struct bio *bi, int dd_idx, in
 	} else
 		bip = &sh->dev[dd_idx].toread;
 	while (*bip && (*bip)->bi_sector < bi->bi_sector) {
-		if ((*bip)->bi_sector + ((*bip)->bi_size >> 9) > bi->bi_sector)
+		if (bio_end_sector(*bip) > bi->bi_sector)
 			goto overlap;
 		bip = & (*bip)->bi_next;
 	}
-	if (*bip && (*bip)->bi_sector < bi->bi_sector + ((bi->bi_size)>>9))
+	if (*bip && (*bip)->bi_sector < bio_end_sector(bi))
 		goto overlap;
 
 	BUG_ON(*bip && bi->bi_next && (*bip) != bi->bi_next);
@@ -2415,8 +2421,8 @@ static int add_stripe_bio(struct stripe_head *sh, struct bio *bi, int dd_idx, in
 		     sector < sh->dev[dd_idx].sector + STRIPE_SECTORS &&
 			     bi && bi->bi_sector <= sector;
 		     bi = r5_next_bio(bi, sh->dev[dd_idx].sector)) {
-			if (bi->bi_sector + (bi->bi_size>>9) >= sector)
-				sector = bi->bi_sector + (bi->bi_size>>9);
+			if (bio_end_sector(bi) >= sector)
+				sector = bio_end_sector(bi);
 		}
 		if (sector >= sh->dev[dd_idx].sector + STRIPE_SECTORS)
 			set_bit(R5_OVERWRITE, &sh->dev[dd_idx].flags);
@@ -3842,7 +3848,7 @@ static int in_chunk_boundary(struct mddev *mddev, struct bio *bio)
 {
 	sector_t sector = bio->bi_sector + get_start_sect(bio->bi_bdev);
 	unsigned int chunk_sectors = mddev->chunk_sectors;
-	unsigned int bio_sectors = bio->bi_size >> 9;
+	unsigned int bio_sectors = bio_sectors(bio);
 
 	if (mddev->new_chunk_sectors < mddev->chunk_sectors)
 		chunk_sectors = mddev->new_chunk_sectors;
@@ -3934,7 +3940,7 @@ static int bio_fits_rdev(struct bio *bi)
 {
 	struct request_queue *q = bdev_get_queue(bi->bi_bdev);
 
-	if ((bi->bi_size>>9) > queue_max_sectors(q))
+	if (bio_sectors(bi) > queue_max_sectors(q))
 		return 0;
 	blk_recount_segments(q, bi);
 	if (bi->bi_phys_segments > queue_max_segments(q))
@@ -3981,7 +3987,7 @@ static int chunk_aligned_read(struct mddev *mddev, struct bio * raid_bio)
 						    0,
 						    &dd_idx, NULL);
 
-	end_sector = align_bi->bi_sector + (align_bi->bi_size >> 9);
+	end_sector = bio_end_sector(align_bi);
 	rcu_read_lock();
 	rdev = rcu_dereference(conf->disks[dd_idx].replacement);
 	if (!rdev || test_bit(Faulty, &rdev->flags) ||
@@ -4004,7 +4010,7 @@ static int chunk_aligned_read(struct mddev *mddev, struct bio * raid_bio)
 		align_bi->bi_flags &= ~(1 << BIO_SEG_VALID);
 
 		if (!bio_fits_rdev(align_bi) ||
-		    is_badblock(rdev, align_bi->bi_sector, align_bi->bi_size>>9,
+		    is_badblock(rdev, align_bi->bi_sector, bio_sectors(align_bi),
 				&first_bad, &bad_sectors)) {
 			/* too big in some way, or has a known bad block */
 			bio_put(align_bi);
@@ -4266,7 +4272,7 @@ static void make_request(struct mddev *mddev, struct bio * bi)
 	}
 
 	logical_sector = bi->bi_sector & ~((sector_t)STRIPE_SECTORS-1);
-	last_sector = bi->bi_sector + (bi->bi_size>>9);
+	last_sector = bio_end_sector(bi);
 	bi->bi_next = NULL;
 	bi->bi_phys_segments = 1;	/* over-loaded to count active stripes */
 
@@ -4672,9 +4678,10 @@ static inline sector_t sync_request(struct mddev *mddev, sector_t sector_nr, int
 		*skipped = 1;
 		return rv;
 	}
-	if (!bitmap_start_sync(mddev->bitmap, sector_nr, &sync_blocks, 1) &&
-	    !test_bit(MD_RECOVERY_REQUESTED, &mddev->recovery) &&
-	    !conf->fullsync && sync_blocks >= STRIPE_SECTORS) {
+	if (!test_bit(MD_RECOVERY_REQUESTED, &mddev->recovery) &&
+	    !conf->fullsync &&
+	    !bitmap_start_sync(mddev->bitmap, sector_nr, &sync_blocks, 1) &&
+	    sync_blocks >= STRIPE_SECTORS) {
 		/* we can skip this block, and probably more */
 		sync_blocks /= STRIPE_SECTORS;
 		*skipped = 1;
@@ -4731,7 +4738,7 @@ static int  retry_aligned_read(struct r5conf *conf, struct bio *raid_bio)
 	logical_sector = raid_bio->bi_sector & ~((sector_t)STRIPE_SECTORS-1);
 	sector = raid5_compute_sector(conf, logical_sector,
 				      0, &dd_idx, NULL);
-	last_sector = raid_bio->bi_sector + (raid_bio->bi_size>>9);
+	last_sector = bio_end_sector(raid_bio);
 
 	for (; logical_sector < last_sector;
 	     logical_sector += STRIPE_SECTORS,
diff --git a/drivers/media/common/Kconfig b/drivers/media/common/Kconfig
index 56c25e6299e9..b85f88c8ddbd 100644
--- a/drivers/media/common/Kconfig
+++ b/drivers/media/common/Kconfig
@@ -16,6 +16,10 @@ config VIDEO_TVEEPROM
 	tristate
 	depends on I2C
 
+config CYPRESS_FIRMWARE
+	tristate "Cypress firmware helper routines"
+	depends on USB
+
 source "drivers/media/common/b2c2/Kconfig"
 source "drivers/media/common/saa7146/Kconfig"
 source "drivers/media/common/siano/Kconfig"
diff --git a/drivers/media/common/Makefile b/drivers/media/common/Makefile
index 8f8d18755d15..d208de3b7cc0 100644
--- a/drivers/media/common/Makefile
+++ b/drivers/media/common/Makefile
@@ -2,3 +2,4 @@ obj-y += b2c2/ saa7146/ siano/
 obj-$(CONFIG_VIDEO_CX2341X) += cx2341x.o
 obj-$(CONFIG_VIDEO_BTCX)  += btcx-risc.o
 obj-$(CONFIG_VIDEO_TVEEPROM) += tveeprom.o
+obj-$(CONFIG_CYPRESS_FIRMWARE) += cypress_firmware.o
diff --git a/drivers/media/common/b2c2/flexcop-fe-tuner.c b/drivers/media/common/b2c2/flexcop-fe-tuner.c
index 850a6c606750..7e14e90d2922 100644
--- a/drivers/media/common/b2c2/flexcop-fe-tuner.c
+++ b/drivers/media/common/b2c2/flexcop-fe-tuner.c
@@ -325,7 +325,7 @@ static int skystar2_rev27_attach(struct flexcop_device *fc,
 	/* enable no_base_addr - no repeated start when reading */
 	fc->fc_i2c_adap[2].no_base_addr = 1;
 	if (!dvb_attach(isl6421_attach, fc->fe, &fc->fc_i2c_adap[2].i2c_adap,
-			0x08, 1, 1)) {
+			0x08, 1, 1, false)) {
 		err("ISL6421 could NOT be attached");
 		goto fail_isl;
 	}
@@ -391,7 +391,7 @@ static int skystar2_rev28_attach(struct flexcop_device *fc,
 
 	fc->fc_i2c_adap[2].no_base_addr = 1;
 	if (!dvb_attach(isl6421_attach, fc->fe, &fc->fc_i2c_adap[2].i2c_adap,
-			0x08, 0, 0)) {
+			0x08, 0, 0, false)) {
 		err("ISL6421 could NOT be attached");
 		fc->fc_i2c_adap[2].no_base_addr = 0;
 		return 0;
diff --git a/drivers/media/usb/dvb-usb-v2/cypress_firmware.c b/drivers/media/common/cypress_firmware.c
index 211df549f26a..577e82058fdc 100644
--- a/drivers/media/usb/dvb-usb-v2/cypress_firmware.c
+++ b/drivers/media/common/cypress_firmware.c
@@ -8,7 +8,10 @@
  *
  */
 
-#include "dvb_usb.h"
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+#include <linux/firmware.h>
 #include "cypress_firmware.h"
 
 struct usb_cypress_controller {
@@ -30,14 +33,42 @@ static const struct usb_cypress_controller cypress[] = {
 static int usb_cypress_writemem(struct usb_device *udev, u16 addr, u8 *data,
 		u8 len)
 {
-	dvb_usb_dbg_usb_control_msg(udev,
-			0xa0, USB_TYPE_VENDOR, addr, 0x00, data, len);
-
 	return usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
 			0xa0, USB_TYPE_VENDOR, addr, 0x00, data, len, 5000);
 }
 
-int usbv2_cypress_load_firmware(struct usb_device *udev,
+static int cypress_get_hexline(const struct firmware *fw,
+				struct hexline *hx, int *pos)
+{
+	u8 *b = (u8 *) &fw->data[*pos];
+	int data_offs = 4;
+
+	if (*pos >= fw->size)
+		return 0;
+
+	memset(hx, 0, sizeof(struct hexline));
+	hx->len = b[0];
+
+	if ((*pos + hx->len + 4) >= fw->size)
+		return -EINVAL;
+
+	hx->addr = b[1] | (b[2] << 8);
+	hx->type = b[3];
+
+	if (hx->type == 0x04) {
+		/* b[4] and b[5] are the Extended linear address record data
+		 * field */
+		hx->addr |= (b[4] << 24) | (b[5] << 16);
+	}
+
+	memcpy(hx->data, &b[data_offs], hx->len);
+	hx->chk = b[hx->len + data_offs];
+	*pos += hx->len + 5;
+
+	return *pos;
+}
+
+int cypress_load_firmware(struct usb_device *udev,
 		const struct firmware *fw, int type)
 {
 	struct hexline *hx;
@@ -61,7 +92,7 @@ int usbv2_cypress_load_firmware(struct usb_device *udev,
 
 	/* write firmware to memory */
 	for (;;) {
-		ret = dvb_usbv2_get_hexline(fw, hx, &pos);
+		ret = cypress_get_hexline(fw, hx, &pos);
 		if (ret < 0)
 			goto err_kfree;
 		else if (ret == 0)
@@ -71,9 +102,8 @@ int usbv2_cypress_load_firmware(struct usb_device *udev,
 		if (ret < 0) {
 			goto err_kfree;
 		} else if (ret != hx->len) {
-			dev_err(&udev->dev, "%s: error while transferring " \
-					"firmware (transferred size=%d, " \
-					"block size=%d)\n",
+			dev_err(&udev->dev,
+					"%s: error while transferring firmware (transferred size=%d, block size=%d)\n",
 					KBUILD_MODNAME, ret, hx->len);
 			ret = -EIO;
 			goto err_kfree;
@@ -95,39 +125,7 @@ err_kfree:
 	kfree(hx);
 	return ret;
 }
-EXPORT_SYMBOL(usbv2_cypress_load_firmware);
-
-int dvb_usbv2_get_hexline(const struct firmware *fw, struct hexline *hx,
-		int *pos)
-{
-	u8 *b = (u8 *) &fw->data[*pos];
-	int data_offs = 4;
-
-	if (*pos >= fw->size)
-		return 0;
-
-	memset(hx, 0, sizeof(struct hexline));
-	hx->len = b[0];
-
-	if ((*pos + hx->len + 4) >= fw->size)
-		return -EINVAL;
-
-	hx->addr = b[1] | (b[2] << 8);
-	hx->type = b[3];
-
-	if (hx->type == 0x04) {
-		/* b[4] and b[5] are the Extended linear address record data
-		 * field */
-		hx->addr |= (b[4] << 24) | (b[5] << 16);
-	}
-
-	memcpy(hx->data, &b[data_offs], hx->len);
-	hx->chk = b[hx->len + data_offs];
-	*pos += hx->len + 5;
-
-	return *pos;
-}
-EXPORT_SYMBOL(dvb_usbv2_get_hexline);
+EXPORT_SYMBOL(cypress_load_firmware);
 
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
 MODULE_DESCRIPTION("Cypress firmware download");
diff --git a/drivers/media/usb/dvb-usb-v2/cypress_firmware.h b/drivers/media/common/cypress_firmware.h
index 80085fd4132c..e493cbc7a528 100644
--- a/drivers/media/usb/dvb-usb-v2/cypress_firmware.h
+++ b/drivers/media/common/cypress_firmware.h
@@ -1,5 +1,4 @@
-/* cypress_firmware.h is part of the DVB USB library.
- *
+/*
  * Copyright (C) 2004-6 Patrick Boettcher (patrick.boettcher@desy.de)
  * see dvb-usb-init.c for copyright information.
  *
@@ -23,9 +22,7 @@ struct hexline {
 	u8 data[255];
 	u8 chk;
 };
-extern int usbv2_cypress_load_firmware(struct usb_device *,
-		const struct firmware *, int);
-extern int dvb_usbv2_get_hexline(const struct firmware *,
-		struct hexline *, int *);
+
+int cypress_load_firmware(struct usb_device *, const struct firmware *, int);
 
 #endif
diff --git a/drivers/media/common/saa7146/saa7146_video.c b/drivers/media/common/saa7146/saa7146_video.c
index 4143d61f79b1..fe907f2e8f59 100644
--- a/drivers/media/common/saa7146/saa7146_video.c
+++ b/drivers/media/common/saa7146/saa7146_video.c
@@ -832,7 +832,7 @@ static int vidioc_g_std(struct file *file, void *fh, v4l2_std_id *norm)
 	}
 	*/
 
-static int vidioc_s_std(struct file *file, void *fh, v4l2_std_id *id)
+static int vidioc_s_std(struct file *file, void *fh, v4l2_std_id id)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct saa7146_vv *vv = dev->vv_data;
@@ -856,7 +856,7 @@ static int vidioc_s_std(struct file *file, void *fh, v4l2_std_id *id)
 	}
 
 	for (i = 0; i < dev->ext_vv_data->num_stds; i++)
-		if (*id & dev->ext_vv_data->stds[i].id)
+		if (id & dev->ext_vv_data->stds[i].id)
 			break;
 	if (i != dev->ext_vv_data->num_stds) {
 		vv->standard = &dev->ext_vv_data->stds[i];
diff --git a/drivers/media/common/siano/Kconfig b/drivers/media/common/siano/Kconfig
index 68f0f604678e..f3f5ec44e685 100644
--- a/drivers/media/common/siano/Kconfig
+++ b/drivers/media/common/siano/Kconfig
@@ -17,3 +17,15 @@ config SMS_SIANO_RC
 	default y
 	---help---
 	  Choose Y to select Remote Controller support for Siano driver.
+
+config SMS_SIANO_DEBUGFS
+	bool "Enable debugfs for smsdvb"
+	depends on SMS_SIANO_MDTV
+	depends on DEBUG_FS
+	depends on SMS_USB_DRV
+	---help---
+	  Choose Y to enable visualizing a dump of the frontend
+	  statistics response packets via debugfs. Currently, works
+	  only with Siano USB devices.
+
+	  Useful only for developers. In doubt, say N.
diff --git a/drivers/media/common/siano/Makefile b/drivers/media/common/siano/Makefile
index 81b1e985bea5..4c0567f106b2 100644
--- a/drivers/media/common/siano/Makefile
+++ b/drivers/media/common/siano/Makefile
@@ -1,4 +1,5 @@
 smsmdtv-objs := smscoreapi.o sms-cards.o smsendian.o
+smsdvb-objs := smsdvb-main.o
 
 obj-$(CONFIG_SMS_SIANO_MDTV) += smsmdtv.o smsdvb.o
 
@@ -6,6 +7,10 @@ ifeq ($(CONFIG_SMS_SIANO_RC),y)
   smsmdtv-objs += smsir.o
 endif
 
+ifeq ($(CONFIG_SMS_SIANO_DEBUGFS),y)
+  smsdvb-objs += smsdvb-debugfs.o
+endif
+
 ccflags-y += -Idrivers/media/dvb-core
 ccflags-y += $(extra-cflags-y) $(extra-cflags-m)
 
diff --git a/drivers/media/common/siano/sms-cards.c b/drivers/media/common/siano/sms-cards.c
index 680c781c8dd6..82769993eeb7 100644
--- a/drivers/media/common/siano/sms-cards.c
+++ b/drivers/media/common/siano/sms-cards.c
@@ -28,43 +28,53 @@ MODULE_PARM_DESC(cards_dbg, "set debug level (info=1, adv=2 (or-able))");
 static struct sms_board sms_boards[] = {
 	[SMS_BOARD_UNKNOWN] = {
 		.name	= "Unknown board",
+		.type = SMS_UNKNOWN_TYPE,
+		.default_mode = DEVICE_MODE_NONE,
 	},
 	[SMS1XXX_BOARD_SIANO_STELLAR] = {
 		.name	= "Siano Stellar Digital Receiver",
 		.type	= SMS_STELLAR,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 	},
 	[SMS1XXX_BOARD_SIANO_NOVA_A] = {
 		.name	= "Siano Nova A Digital Receiver",
 		.type	= SMS_NOVA_A0,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 	},
 	[SMS1XXX_BOARD_SIANO_NOVA_B] = {
 		.name	= "Siano Nova B Digital Receiver",
 		.type	= SMS_NOVA_B0,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 	},
 	[SMS1XXX_BOARD_SIANO_VEGA] = {
 		.name	= "Siano Vega Digital Receiver",
 		.type	= SMS_VEGA,
+		.default_mode = DEVICE_MODE_CMMB,
 	},
 	[SMS1XXX_BOARD_HAUPPAUGE_CATAMOUNT] = {
 		.name	= "Hauppauge Catamount",
 		.type	= SMS_STELLAR,
-		.fw[DEVICE_MODE_DVBT_BDA] = "sms1xxx-stellar-dvbt-01.fw",
+		.fw[DEVICE_MODE_DVBT_BDA] = SMS_FW_DVBT_STELLAR,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 	},
 	[SMS1XXX_BOARD_HAUPPAUGE_OKEMO_A] = {
 		.name	= "Hauppauge Okemo-A",
 		.type	= SMS_NOVA_A0,
-		.fw[DEVICE_MODE_DVBT_BDA] = "sms1xxx-nova-a-dvbt-01.fw",
+		.fw[DEVICE_MODE_DVBT_BDA] = SMS_FW_DVBT_NOVA_A,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 	},
 	[SMS1XXX_BOARD_HAUPPAUGE_OKEMO_B] = {
 		.name	= "Hauppauge Okemo-B",
 		.type	= SMS_NOVA_B0,
-		.fw[DEVICE_MODE_DVBT_BDA] = "sms1xxx-nova-b-dvbt-01.fw",
+		.fw[DEVICE_MODE_DVBT_BDA] = SMS_FW_DVBT_NOVA_B,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 	},
 	[SMS1XXX_BOARD_HAUPPAUGE_WINDHAM] = {
 		.name	= "Hauppauge WinTV MiniStick",
 		.type	= SMS_NOVA_B0,
-		.fw[DEVICE_MODE_ISDBT_BDA] = "sms1xxx-hcw-55xxx-isdbt-02.fw",
-		.fw[DEVICE_MODE_DVBT_BDA] = "sms1xxx-hcw-55xxx-dvbt-02.fw",
+		.fw[DEVICE_MODE_ISDBT_BDA] = SMS_FW_ISDBT_HCW_55XXX,
+		.fw[DEVICE_MODE_DVBT_BDA]  = SMS_FW_DVBT_HCW_55XXX,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 		.rc_codes = RC_MAP_HAUPPAUGE,
 		.board_cfg.leds_power = 26,
 		.board_cfg.led0 = 27,
@@ -77,7 +87,8 @@ static struct sms_board sms_boards[] = {
 	[SMS1XXX_BOARD_HAUPPAUGE_TIGER_MINICARD] = {
 		.name	= "Hauppauge WinTV MiniCard",
 		.type	= SMS_NOVA_B0,
-		.fw[DEVICE_MODE_DVBT_BDA] = "sms1xxx-hcw-55xxx-dvbt-02.fw",
+		.fw[DEVICE_MODE_DVBT_BDA] = SMS_FW_DVBT_HCW_55XXX,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 		.lna_ctrl  = 29,
 		.board_cfg.foreign_lna0_ctrl = 29,
 		.rf_switch = 17,
@@ -86,18 +97,65 @@ static struct sms_board sms_boards[] = {
 	[SMS1XXX_BOARD_HAUPPAUGE_TIGER_MINICARD_R2] = {
 		.name	= "Hauppauge WinTV MiniCard",
 		.type	= SMS_NOVA_B0,
-		.fw[DEVICE_MODE_DVBT_BDA] = "sms1xxx-hcw-55xxx-dvbt-02.fw",
+		.fw[DEVICE_MODE_DVBT_BDA] = SMS_FW_DVBT_HCW_55XXX,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 		.lna_ctrl  = -1,
 	},
 	[SMS1XXX_BOARD_SIANO_NICE] = {
-	/* 11 */
 		.name = "Siano Nice Digital Receiver",
 		.type = SMS_NOVA_B0,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
 	},
 	[SMS1XXX_BOARD_SIANO_VENICE] = {
-	/* 12 */
 		.name = "Siano Venice Digital Receiver",
 		.type = SMS_VEGA,
+		.default_mode = DEVICE_MODE_CMMB,
+	},
+	[SMS1XXX_BOARD_SIANO_STELLAR_ROM] = {
+		.name = "Siano Stellar Digital Receiver ROM",
+		.type = SMS_STELLAR,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
+		.intf_num = 1,
+	},
+	[SMS1XXX_BOARD_ZTE_DVB_DATA_CARD] = {
+		.name = "ZTE Data Card Digital Receiver",
+		.type = SMS_NOVA_B0,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
+		.intf_num = 5,
+		.mtu = 15792,
+	},
+	[SMS1XXX_BOARD_ONDA_MDTV_DATA_CARD] = {
+		.name = "ONDA Data Card Digital Receiver",
+		.type = SMS_NOVA_B0,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
+		.intf_num = 6,
+		.mtu = 15792,
+	},
+	[SMS1XXX_BOARD_SIANO_MING] = {
+		.name = "Siano Ming Digital Receiver",
+		.type = SMS_MING,
+		.default_mode = DEVICE_MODE_CMMB,
+	},
+	[SMS1XXX_BOARD_SIANO_PELE] = {
+		.name = "Siano Pele Digital Receiver",
+		.type = SMS_PELE,
+		.default_mode = DEVICE_MODE_ISDBT_BDA,
+	},
+	[SMS1XXX_BOARD_SIANO_RIO] = {
+		.name = "Siano Rio Digital Receiver",
+		.type = SMS_RIO,
+		.default_mode = DEVICE_MODE_ISDBT_BDA,
+	},
+	[SMS1XXX_BOARD_SIANO_DENVER_1530] = {
+		.name = "Siano Denver (ATSC-M/H) Digital Receiver",
+		.type = SMS_DENVER_1530,
+		.default_mode = DEVICE_MODE_ATSC,
+		.crystal = 2400,
+	},
+	[SMS1XXX_BOARD_SIANO_DENVER_2160] = {
+		.name = "Siano Denver (TDMB) Digital Receiver",
+		.type = SMS_DENVER_2160,
+		.default_mode = DEVICE_MODE_DAB_TDMB,
 	},
 };
 
@@ -109,20 +167,21 @@ struct sms_board *sms_get_board(unsigned id)
 }
 EXPORT_SYMBOL_GPL(sms_get_board);
 static inline void sms_gpio_assign_11xx_default_led_config(
-		struct smscore_gpio_config *pGpioConfig) {
-	pGpioConfig->Direction = SMS_GPIO_DIRECTION_OUTPUT;
-	pGpioConfig->InputCharacteristics =
-		SMS_GPIO_INPUT_CHARACTERISTICS_NORMAL;
-	pGpioConfig->OutputDriving = SMS_GPIO_OUTPUT_DRIVING_4mA;
-	pGpioConfig->OutputSlewRate = SMS_GPIO_OUTPUT_SLEW_RATE_0_45_V_NS;
-	pGpioConfig->PullUpDown = SMS_GPIO_PULL_UP_DOWN_NONE;
+		struct smscore_config_gpio *p_gpio_config) {
+	p_gpio_config->direction = SMS_GPIO_DIRECTION_OUTPUT;
+	p_gpio_config->inputcharacteristics =
+		SMS_GPIO_INPUTCHARACTERISTICS_NORMAL;
+	p_gpio_config->outputdriving = SMS_GPIO_OUTPUTDRIVING_4mA;
+	p_gpio_config->outputslewrate = SMS_GPIO_OUTPUT_SLEW_RATE_0_45_V_NS;
+	p_gpio_config->pullupdown = SMS_GPIO_PULLUPDOWN_NONE;
 }
 
 int sms_board_event(struct smscore_device_t *coredev,
-		enum SMS_BOARD_EVENTS gevent) {
-	struct smscore_gpio_config MyGpioConfig;
+		    enum SMS_BOARD_EVENTS gevent)
+{
+	struct smscore_config_gpio my_gpio_config;
 
-	sms_gpio_assign_11xx_default_led_config(&MyGpioConfig);
+	sms_gpio_assign_11xx_default_led_config(&my_gpio_config);
 
 	switch (gevent) {
 	case BOARD_EVENT_POWER_INIT: /* including hotplug */
@@ -182,8 +241,8 @@ static int sms_set_gpio(struct smscore_device_t *coredev, int pin, int enable)
 		.direction            = SMS_GPIO_DIRECTION_OUTPUT,
 		.pullupdown           = SMS_GPIO_PULLUPDOWN_NONE,
 		.inputcharacteristics = SMS_GPIO_INPUTCHARACTERISTICS_NORMAL,
-		.outputslewrate       = SMS_GPIO_OUTPUTSLEWRATE_FAST,
-		.outputdriving        = SMS_GPIO_OUTPUTDRIVING_4mA,
+		.outputslewrate       = SMS_GPIO_OUTPUT_SLEW_RATE_FAST,
+		.outputdriving        = SMS_GPIO_OUTPUTDRIVING_S_4mA,
 	};
 
 	if (pin == 0)
@@ -293,19 +352,7 @@ EXPORT_SYMBOL_GPL(sms_board_lna_control);
 
 int sms_board_load_modules(int id)
 {
-	switch (id) {
-	case SMS1XXX_BOARD_HAUPPAUGE_CATAMOUNT:
-	case SMS1XXX_BOARD_HAUPPAUGE_OKEMO_A:
-	case SMS1XXX_BOARD_HAUPPAUGE_OKEMO_B:
-	case SMS1XXX_BOARD_HAUPPAUGE_WINDHAM:
-	case SMS1XXX_BOARD_HAUPPAUGE_TIGER_MINICARD:
-	case SMS1XXX_BOARD_HAUPPAUGE_TIGER_MINICARD_R2:
-		request_module("smsdvb");
-		break;
-	default:
-		/* do nothing */
-		break;
-	}
+	request_module("smsdvb");
 	return 0;
 }
 EXPORT_SYMBOL_GPL(sms_board_load_modules);
diff --git a/drivers/media/common/siano/sms-cards.h b/drivers/media/common/siano/sms-cards.h
index d8cdf756f7cf..c63b544c49c5 100644
--- a/drivers/media/common/siano/sms-cards.h
+++ b/drivers/media/common/siano/sms-cards.h
@@ -37,6 +37,14 @@
 #define SMS1XXX_BOARD_HAUPPAUGE_TIGER_MINICARD_R2 10
 #define SMS1XXX_BOARD_SIANO_NICE	11
 #define SMS1XXX_BOARD_SIANO_VENICE	12
+#define SMS1XXX_BOARD_SIANO_STELLAR_ROM 13
+#define SMS1XXX_BOARD_ZTE_DVB_DATA_CARD	14
+#define SMS1XXX_BOARD_ONDA_MDTV_DATA_CARD 15
+#define SMS1XXX_BOARD_SIANO_MING	16
+#define SMS1XXX_BOARD_SIANO_PELE	17
+#define SMS1XXX_BOARD_SIANO_RIO		18
+#define SMS1XXX_BOARD_SIANO_DENVER_1530	19
+#define SMS1XXX_BOARD_SIANO_DENVER_2160 20
 
 struct sms_board_gpio_cfg {
 	int lna_vhf_exist;
@@ -79,6 +87,12 @@ struct sms_board {
 
 	/* gpios */
 	int led_power, led_hi, led_lo, lna_ctrl, rf_switch;
+
+	char intf_num;
+	int default_mode;
+	unsigned int mtu;
+	unsigned int crystal;
+	struct sms_antenna_config_ST *antenna_config;
 };
 
 struct sms_board *sms_get_board(unsigned id);
diff --git a/drivers/media/common/siano/smscoreapi.c b/drivers/media/common/siano/smscoreapi.c
index 1842e64e6338..45ac9eea4882 100644
--- a/drivers/media/common/siano/smscoreapi.c
+++ b/drivers/media/common/siano/smscoreapi.c
@@ -37,7 +37,6 @@
 #include "smscoreapi.h"
 #include "sms-cards.h"
 #include "smsir.h"
-#include "smsendian.h"
 
 static int sms_dbg;
 module_param_named(debug, sms_dbg, int, 0644);
@@ -58,11 +57,350 @@ struct smscore_client_t {
 	struct list_head entry;
 	struct smscore_device_t *coredev;
 	void			*context;
-	struct list_head 	idlist;
-	onresponse_t	onresponse_handler;
+	struct list_head	idlist;
+	onresponse_t		onresponse_handler;
 	onremove_t		onremove_handler;
 };
 
+static char *siano_msgs[] = {
+	[MSG_TYPE_BASE_VAL                           - MSG_TYPE_BASE_VAL] = "MSG_TYPE_BASE_VAL",
+	[MSG_SMS_GET_VERSION_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_VERSION_REQ",
+	[MSG_SMS_GET_VERSION_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_VERSION_RES",
+	[MSG_SMS_MULTI_BRIDGE_CFG                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_MULTI_BRIDGE_CFG",
+	[MSG_SMS_GPIO_CONFIG_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_CONFIG_REQ",
+	[MSG_SMS_GPIO_CONFIG_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_CONFIG_RES",
+	[MSG_SMS_GPIO_SET_LEVEL_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_SET_LEVEL_REQ",
+	[MSG_SMS_GPIO_SET_LEVEL_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_SET_LEVEL_RES",
+	[MSG_SMS_GPIO_GET_LEVEL_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_GET_LEVEL_REQ",
+	[MSG_SMS_GPIO_GET_LEVEL_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_GET_LEVEL_RES",
+	[MSG_SMS_EEPROM_BURN_IND                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_EEPROM_BURN_IND",
+	[MSG_SMS_LOG_ENABLE_CHANGE_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_LOG_ENABLE_CHANGE_REQ",
+	[MSG_SMS_LOG_ENABLE_CHANGE_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_LOG_ENABLE_CHANGE_RES",
+	[MSG_SMS_SET_MAX_TX_MSG_LEN_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_MAX_TX_MSG_LEN_REQ",
+	[MSG_SMS_SET_MAX_TX_MSG_LEN_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_MAX_TX_MSG_LEN_RES",
+	[MSG_SMS_SPI_HALFDUPLEX_TOKEN_HOST_TO_DEVICE - MSG_TYPE_BASE_VAL] = "MSG_SMS_SPI_HALFDUPLEX_TOKEN_HOST_TO_DEVICE",
+	[MSG_SMS_SPI_HALFDUPLEX_TOKEN_DEVICE_TO_HOST - MSG_TYPE_BASE_VAL] = "MSG_SMS_SPI_HALFDUPLEX_TOKEN_DEVICE_TO_HOST",
+	[MSG_SMS_BACKGROUND_SCAN_FLAG_CHANGE_REQ     - MSG_TYPE_BASE_VAL] = "MSG_SMS_BACKGROUND_SCAN_FLAG_CHANGE_REQ",
+	[MSG_SMS_BACKGROUND_SCAN_FLAG_CHANGE_RES     - MSG_TYPE_BASE_VAL] = "MSG_SMS_BACKGROUND_SCAN_FLAG_CHANGE_RES",
+	[MSG_SMS_BACKGROUND_SCAN_SIGNAL_DETECTED_IND - MSG_TYPE_BASE_VAL] = "MSG_SMS_BACKGROUND_SCAN_SIGNAL_DETECTED_IND",
+	[MSG_SMS_BACKGROUND_SCAN_NO_SIGNAL_IND       - MSG_TYPE_BASE_VAL] = "MSG_SMS_BACKGROUND_SCAN_NO_SIGNAL_IND",
+	[MSG_SMS_CONFIGURE_RF_SWITCH_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_CONFIGURE_RF_SWITCH_REQ",
+	[MSG_SMS_CONFIGURE_RF_SWITCH_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_CONFIGURE_RF_SWITCH_RES",
+	[MSG_SMS_MRC_PATH_DISCONNECT_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_MRC_PATH_DISCONNECT_REQ",
+	[MSG_SMS_MRC_PATH_DISCONNECT_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_MRC_PATH_DISCONNECT_RES",
+	[MSG_SMS_RECEIVE_1SEG_THROUGH_FULLSEG_REQ    - MSG_TYPE_BASE_VAL] = "MSG_SMS_RECEIVE_1SEG_THROUGH_FULLSEG_REQ",
+	[MSG_SMS_RECEIVE_1SEG_THROUGH_FULLSEG_RES    - MSG_TYPE_BASE_VAL] = "MSG_SMS_RECEIVE_1SEG_THROUGH_FULLSEG_RES",
+	[MSG_SMS_RECEIVE_VHF_VIA_VHF_INPUT_REQ       - MSG_TYPE_BASE_VAL] = "MSG_SMS_RECEIVE_VHF_VIA_VHF_INPUT_REQ",
+	[MSG_SMS_RECEIVE_VHF_VIA_VHF_INPUT_RES       - MSG_TYPE_BASE_VAL] = "MSG_SMS_RECEIVE_VHF_VIA_VHF_INPUT_RES",
+	[MSG_WR_REG_RFT_REQ                          - MSG_TYPE_BASE_VAL] = "MSG_WR_REG_RFT_REQ",
+	[MSG_WR_REG_RFT_RES                          - MSG_TYPE_BASE_VAL] = "MSG_WR_REG_RFT_RES",
+	[MSG_RD_REG_RFT_REQ                          - MSG_TYPE_BASE_VAL] = "MSG_RD_REG_RFT_REQ",
+	[MSG_RD_REG_RFT_RES                          - MSG_TYPE_BASE_VAL] = "MSG_RD_REG_RFT_RES",
+	[MSG_RD_REG_ALL_RFT_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_RD_REG_ALL_RFT_REQ",
+	[MSG_RD_REG_ALL_RFT_RES                      - MSG_TYPE_BASE_VAL] = "MSG_RD_REG_ALL_RFT_RES",
+	[MSG_HELP_INT                                - MSG_TYPE_BASE_VAL] = "MSG_HELP_INT",
+	[MSG_RUN_SCRIPT_INT                          - MSG_TYPE_BASE_VAL] = "MSG_RUN_SCRIPT_INT",
+	[MSG_SMS_EWS_INBAND_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_EWS_INBAND_REQ",
+	[MSG_SMS_EWS_INBAND_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_EWS_INBAND_RES",
+	[MSG_SMS_RFS_SELECT_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_RFS_SELECT_REQ",
+	[MSG_SMS_RFS_SELECT_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_RFS_SELECT_RES",
+	[MSG_SMS_MB_GET_VER_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_GET_VER_REQ",
+	[MSG_SMS_MB_GET_VER_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_GET_VER_RES",
+	[MSG_SMS_MB_WRITE_CFGFILE_REQ                - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_WRITE_CFGFILE_REQ",
+	[MSG_SMS_MB_WRITE_CFGFILE_RES                - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_WRITE_CFGFILE_RES",
+	[MSG_SMS_MB_READ_CFGFILE_REQ                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_READ_CFGFILE_REQ",
+	[MSG_SMS_MB_READ_CFGFILE_RES                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_READ_CFGFILE_RES",
+	[MSG_SMS_RD_MEM_REQ                          - MSG_TYPE_BASE_VAL] = "MSG_SMS_RD_MEM_REQ",
+	[MSG_SMS_RD_MEM_RES                          - MSG_TYPE_BASE_VAL] = "MSG_SMS_RD_MEM_RES",
+	[MSG_SMS_WR_MEM_REQ                          - MSG_TYPE_BASE_VAL] = "MSG_SMS_WR_MEM_REQ",
+	[MSG_SMS_WR_MEM_RES                          - MSG_TYPE_BASE_VAL] = "MSG_SMS_WR_MEM_RES",
+	[MSG_SMS_UPDATE_MEM_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_UPDATE_MEM_REQ",
+	[MSG_SMS_UPDATE_MEM_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_UPDATE_MEM_RES",
+	[MSG_SMS_ISDBT_ENABLE_FULL_PARAMS_SET_REQ    - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_ENABLE_FULL_PARAMS_SET_REQ",
+	[MSG_SMS_ISDBT_ENABLE_FULL_PARAMS_SET_RES    - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_ENABLE_FULL_PARAMS_SET_RES",
+	[MSG_SMS_RF_TUNE_REQ                         - MSG_TYPE_BASE_VAL] = "MSG_SMS_RF_TUNE_REQ",
+	[MSG_SMS_RF_TUNE_RES                         - MSG_TYPE_BASE_VAL] = "MSG_SMS_RF_TUNE_RES",
+	[MSG_SMS_ISDBT_ENABLE_HIGH_MOBILITY_REQ      - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_ENABLE_HIGH_MOBILITY_REQ",
+	[MSG_SMS_ISDBT_ENABLE_HIGH_MOBILITY_RES      - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_ENABLE_HIGH_MOBILITY_RES",
+	[MSG_SMS_ISDBT_SB_RECEPTION_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_SB_RECEPTION_REQ",
+	[MSG_SMS_ISDBT_SB_RECEPTION_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_SB_RECEPTION_RES",
+	[MSG_SMS_GENERIC_EPROM_WRITE_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_GENERIC_EPROM_WRITE_REQ",
+	[MSG_SMS_GENERIC_EPROM_WRITE_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_GENERIC_EPROM_WRITE_RES",
+	[MSG_SMS_GENERIC_EPROM_READ_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_GENERIC_EPROM_READ_REQ",
+	[MSG_SMS_GENERIC_EPROM_READ_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_GENERIC_EPROM_READ_RES",
+	[MSG_SMS_EEPROM_WRITE_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_EEPROM_WRITE_REQ",
+	[MSG_SMS_EEPROM_WRITE_RES                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_EEPROM_WRITE_RES",
+	[MSG_SMS_CUSTOM_READ_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_CUSTOM_READ_REQ",
+	[MSG_SMS_CUSTOM_READ_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_CUSTOM_READ_RES",
+	[MSG_SMS_CUSTOM_WRITE_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_CUSTOM_WRITE_REQ",
+	[MSG_SMS_CUSTOM_WRITE_RES                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_CUSTOM_WRITE_RES",
+	[MSG_SMS_INIT_DEVICE_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_INIT_DEVICE_REQ",
+	[MSG_SMS_INIT_DEVICE_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_INIT_DEVICE_RES",
+	[MSG_SMS_ATSC_SET_ALL_IP_REQ                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_SET_ALL_IP_REQ",
+	[MSG_SMS_ATSC_SET_ALL_IP_RES                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_SET_ALL_IP_RES",
+	[MSG_SMS_ATSC_START_ENSEMBLE_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_START_ENSEMBLE_REQ",
+	[MSG_SMS_ATSC_START_ENSEMBLE_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_START_ENSEMBLE_RES",
+	[MSG_SMS_SET_OUTPUT_MODE_REQ                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_OUTPUT_MODE_REQ",
+	[MSG_SMS_SET_OUTPUT_MODE_RES                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_OUTPUT_MODE_RES",
+	[MSG_SMS_ATSC_IP_FILTER_GET_LIST_REQ         - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_GET_LIST_REQ",
+	[MSG_SMS_ATSC_IP_FILTER_GET_LIST_RES         - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_GET_LIST_RES",
+	[MSG_SMS_SUB_CHANNEL_START_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_SUB_CHANNEL_START_REQ",
+	[MSG_SMS_SUB_CHANNEL_START_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_SUB_CHANNEL_START_RES",
+	[MSG_SMS_SUB_CHANNEL_STOP_REQ                - MSG_TYPE_BASE_VAL] = "MSG_SMS_SUB_CHANNEL_STOP_REQ",
+	[MSG_SMS_SUB_CHANNEL_STOP_RES                - MSG_TYPE_BASE_VAL] = "MSG_SMS_SUB_CHANNEL_STOP_RES",
+	[MSG_SMS_ATSC_IP_FILTER_ADD_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_ADD_REQ",
+	[MSG_SMS_ATSC_IP_FILTER_ADD_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_ADD_RES",
+	[MSG_SMS_ATSC_IP_FILTER_REMOVE_REQ           - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_REMOVE_REQ",
+	[MSG_SMS_ATSC_IP_FILTER_REMOVE_RES           - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_REMOVE_RES",
+	[MSG_SMS_ATSC_IP_FILTER_REMOVE_ALL_REQ       - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_REMOVE_ALL_REQ",
+	[MSG_SMS_ATSC_IP_FILTER_REMOVE_ALL_RES       - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_IP_FILTER_REMOVE_ALL_RES",
+	[MSG_SMS_WAIT_CMD                            - MSG_TYPE_BASE_VAL] = "MSG_SMS_WAIT_CMD",
+	[MSG_SMS_ADD_PID_FILTER_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_ADD_PID_FILTER_REQ",
+	[MSG_SMS_ADD_PID_FILTER_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_ADD_PID_FILTER_RES",
+	[MSG_SMS_REMOVE_PID_FILTER_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_REMOVE_PID_FILTER_REQ",
+	[MSG_SMS_REMOVE_PID_FILTER_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_REMOVE_PID_FILTER_RES",
+	[MSG_SMS_FAST_INFORMATION_CHANNEL_REQ        - MSG_TYPE_BASE_VAL] = "MSG_SMS_FAST_INFORMATION_CHANNEL_REQ",
+	[MSG_SMS_FAST_INFORMATION_CHANNEL_RES        - MSG_TYPE_BASE_VAL] = "MSG_SMS_FAST_INFORMATION_CHANNEL_RES",
+	[MSG_SMS_DAB_CHANNEL                         - MSG_TYPE_BASE_VAL] = "MSG_SMS_DAB_CHANNEL",
+	[MSG_SMS_GET_PID_FILTER_LIST_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_PID_FILTER_LIST_REQ",
+	[MSG_SMS_GET_PID_FILTER_LIST_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_PID_FILTER_LIST_RES",
+	[MSG_SMS_POWER_DOWN_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_DOWN_REQ",
+	[MSG_SMS_POWER_DOWN_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_DOWN_RES",
+	[MSG_SMS_ATSC_SLT_EXIST_IND                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_SLT_EXIST_IND",
+	[MSG_SMS_ATSC_NO_SLT_IND                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_ATSC_NO_SLT_IND",
+	[MSG_SMS_GET_STATISTICS_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_STATISTICS_REQ",
+	[MSG_SMS_GET_STATISTICS_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_STATISTICS_RES",
+	[MSG_SMS_SEND_DUMP                           - MSG_TYPE_BASE_VAL] = "MSG_SMS_SEND_DUMP",
+	[MSG_SMS_SCAN_START_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_SCAN_START_REQ",
+	[MSG_SMS_SCAN_START_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_SCAN_START_RES",
+	[MSG_SMS_SCAN_STOP_REQ                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_SCAN_STOP_REQ",
+	[MSG_SMS_SCAN_STOP_RES                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_SCAN_STOP_RES",
+	[MSG_SMS_SCAN_PROGRESS_IND                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_SCAN_PROGRESS_IND",
+	[MSG_SMS_SCAN_COMPLETE_IND                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_SCAN_COMPLETE_IND",
+	[MSG_SMS_LOG_ITEM                            - MSG_TYPE_BASE_VAL] = "MSG_SMS_LOG_ITEM",
+	[MSG_SMS_DAB_SUBCHANNEL_RECONFIG_REQ         - MSG_TYPE_BASE_VAL] = "MSG_SMS_DAB_SUBCHANNEL_RECONFIG_REQ",
+	[MSG_SMS_DAB_SUBCHANNEL_RECONFIG_RES         - MSG_TYPE_BASE_VAL] = "MSG_SMS_DAB_SUBCHANNEL_RECONFIG_RES",
+	[MSG_SMS_HO_PER_SLICES_IND                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_PER_SLICES_IND",
+	[MSG_SMS_HO_INBAND_POWER_IND                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_INBAND_POWER_IND",
+	[MSG_SMS_MANUAL_DEMOD_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_MANUAL_DEMOD_REQ",
+	[MSG_SMS_HO_TUNE_ON_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_TUNE_ON_REQ",
+	[MSG_SMS_HO_TUNE_ON_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_TUNE_ON_RES",
+	[MSG_SMS_HO_TUNE_OFF_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_TUNE_OFF_REQ",
+	[MSG_SMS_HO_TUNE_OFF_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_TUNE_OFF_RES",
+	[MSG_SMS_HO_PEEK_FREQ_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_PEEK_FREQ_REQ",
+	[MSG_SMS_HO_PEEK_FREQ_RES                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_PEEK_FREQ_RES",
+	[MSG_SMS_HO_PEEK_FREQ_IND                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_HO_PEEK_FREQ_IND",
+	[MSG_SMS_MB_ATTEN_SET_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_ATTEN_SET_REQ",
+	[MSG_SMS_MB_ATTEN_SET_RES                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_MB_ATTEN_SET_RES",
+	[MSG_SMS_ENABLE_STAT_IN_I2C_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_ENABLE_STAT_IN_I2C_REQ",
+	[MSG_SMS_ENABLE_STAT_IN_I2C_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_ENABLE_STAT_IN_I2C_RES",
+	[MSG_SMS_SET_ANTENNA_CONFIG_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_ANTENNA_CONFIG_REQ",
+	[MSG_SMS_SET_ANTENNA_CONFIG_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_ANTENNA_CONFIG_RES",
+	[MSG_SMS_GET_STATISTICS_EX_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_STATISTICS_EX_REQ",
+	[MSG_SMS_GET_STATISTICS_EX_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_STATISTICS_EX_RES",
+	[MSG_SMS_SLEEP_RESUME_COMP_IND               - MSG_TYPE_BASE_VAL] = "MSG_SMS_SLEEP_RESUME_COMP_IND",
+	[MSG_SMS_SWITCH_HOST_INTERFACE_REQ           - MSG_TYPE_BASE_VAL] = "MSG_SMS_SWITCH_HOST_INTERFACE_REQ",
+	[MSG_SMS_SWITCH_HOST_INTERFACE_RES           - MSG_TYPE_BASE_VAL] = "MSG_SMS_SWITCH_HOST_INTERFACE_RES",
+	[MSG_SMS_DATA_DOWNLOAD_REQ                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_DOWNLOAD_REQ",
+	[MSG_SMS_DATA_DOWNLOAD_RES                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_DOWNLOAD_RES",
+	[MSG_SMS_DATA_VALIDITY_REQ                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_VALIDITY_REQ",
+	[MSG_SMS_DATA_VALIDITY_RES                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_VALIDITY_RES",
+	[MSG_SMS_SWDOWNLOAD_TRIGGER_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_SWDOWNLOAD_TRIGGER_REQ",
+	[MSG_SMS_SWDOWNLOAD_TRIGGER_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_SWDOWNLOAD_TRIGGER_RES",
+	[MSG_SMS_SWDOWNLOAD_BACKDOOR_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_SWDOWNLOAD_BACKDOOR_REQ",
+	[MSG_SMS_SWDOWNLOAD_BACKDOOR_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_SWDOWNLOAD_BACKDOOR_RES",
+	[MSG_SMS_GET_VERSION_EX_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_VERSION_EX_REQ",
+	[MSG_SMS_GET_VERSION_EX_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_VERSION_EX_RES",
+	[MSG_SMS_CLOCK_OUTPUT_CONFIG_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_CLOCK_OUTPUT_CONFIG_REQ",
+	[MSG_SMS_CLOCK_OUTPUT_CONFIG_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_CLOCK_OUTPUT_CONFIG_RES",
+	[MSG_SMS_I2C_SET_FREQ_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_I2C_SET_FREQ_REQ",
+	[MSG_SMS_I2C_SET_FREQ_RES                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_I2C_SET_FREQ_RES",
+	[MSG_SMS_GENERIC_I2C_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_GENERIC_I2C_REQ",
+	[MSG_SMS_GENERIC_I2C_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_GENERIC_I2C_RES",
+	[MSG_SMS_DVBT_BDA_DATA                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_DVBT_BDA_DATA",
+	[MSG_SW_RELOAD_REQ                           - MSG_TYPE_BASE_VAL] = "MSG_SW_RELOAD_REQ",
+	[MSG_SMS_DATA_MSG                            - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_MSG",
+	[MSG_TABLE_UPLOAD_REQ                        - MSG_TYPE_BASE_VAL] = "MSG_TABLE_UPLOAD_REQ",
+	[MSG_TABLE_UPLOAD_RES                        - MSG_TYPE_BASE_VAL] = "MSG_TABLE_UPLOAD_RES",
+	[MSG_SW_RELOAD_START_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SW_RELOAD_START_REQ",
+	[MSG_SW_RELOAD_START_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SW_RELOAD_START_RES",
+	[MSG_SW_RELOAD_EXEC_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SW_RELOAD_EXEC_REQ",
+	[MSG_SW_RELOAD_EXEC_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SW_RELOAD_EXEC_RES",
+	[MSG_SMS_SPI_INT_LINE_SET_REQ                - MSG_TYPE_BASE_VAL] = "MSG_SMS_SPI_INT_LINE_SET_REQ",
+	[MSG_SMS_SPI_INT_LINE_SET_RES                - MSG_TYPE_BASE_VAL] = "MSG_SMS_SPI_INT_LINE_SET_RES",
+	[MSG_SMS_GPIO_CONFIG_EX_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_CONFIG_EX_REQ",
+	[MSG_SMS_GPIO_CONFIG_EX_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_GPIO_CONFIG_EX_RES",
+	[MSG_SMS_WATCHDOG_ACT_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_WATCHDOG_ACT_REQ",
+	[MSG_SMS_WATCHDOG_ACT_RES                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_WATCHDOG_ACT_RES",
+	[MSG_SMS_LOOPBACK_REQ                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_LOOPBACK_REQ",
+	[MSG_SMS_LOOPBACK_RES                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_LOOPBACK_RES",
+	[MSG_SMS_RAW_CAPTURE_START_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_RAW_CAPTURE_START_REQ",
+	[MSG_SMS_RAW_CAPTURE_START_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_RAW_CAPTURE_START_RES",
+	[MSG_SMS_RAW_CAPTURE_ABORT_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_RAW_CAPTURE_ABORT_REQ",
+	[MSG_SMS_RAW_CAPTURE_ABORT_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_RAW_CAPTURE_ABORT_RES",
+	[MSG_SMS_RAW_CAPTURE_COMPLETE_IND            - MSG_TYPE_BASE_VAL] = "MSG_SMS_RAW_CAPTURE_COMPLETE_IND",
+	[MSG_SMS_DATA_PUMP_IND                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_PUMP_IND",
+	[MSG_SMS_DATA_PUMP_REQ                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_PUMP_REQ",
+	[MSG_SMS_DATA_PUMP_RES                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_DATA_PUMP_RES",
+	[MSG_SMS_FLASH_DL_REQ                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_FLASH_DL_REQ",
+	[MSG_SMS_EXEC_TEST_1_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_EXEC_TEST_1_REQ",
+	[MSG_SMS_EXEC_TEST_1_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_EXEC_TEST_1_RES",
+	[MSG_SMS_ENBALE_TS_INTERFACE_REQ             - MSG_TYPE_BASE_VAL] = "MSG_SMS_ENBALE_TS_INTERFACE_REQ",
+	[MSG_SMS_ENBALE_TS_INTERFACE_RES             - MSG_TYPE_BASE_VAL] = "MSG_SMS_ENBALE_TS_INTERFACE_RES",
+	[MSG_SMS_SPI_SET_BUS_WIDTH_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_SPI_SET_BUS_WIDTH_REQ",
+	[MSG_SMS_SPI_SET_BUS_WIDTH_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_SPI_SET_BUS_WIDTH_RES",
+	[MSG_SMS_SEND_EMM_REQ                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_SEND_EMM_REQ",
+	[MSG_SMS_SEND_EMM_RES                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_SEND_EMM_RES",
+	[MSG_SMS_DISABLE_TS_INTERFACE_REQ            - MSG_TYPE_BASE_VAL] = "MSG_SMS_DISABLE_TS_INTERFACE_REQ",
+	[MSG_SMS_DISABLE_TS_INTERFACE_RES            - MSG_TYPE_BASE_VAL] = "MSG_SMS_DISABLE_TS_INTERFACE_RES",
+	[MSG_SMS_IS_BUF_FREE_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_IS_BUF_FREE_REQ",
+	[MSG_SMS_IS_BUF_FREE_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_IS_BUF_FREE_RES",
+	[MSG_SMS_EXT_ANTENNA_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_EXT_ANTENNA_REQ",
+	[MSG_SMS_EXT_ANTENNA_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_EXT_ANTENNA_RES",
+	[MSG_SMS_CMMB_GET_NET_OF_FREQ_REQ_OBSOLETE   - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_NET_OF_FREQ_REQ_OBSOLETE",
+	[MSG_SMS_CMMB_GET_NET_OF_FREQ_RES_OBSOLETE   - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_NET_OF_FREQ_RES_OBSOLETE",
+	[MSG_SMS_BATTERY_LEVEL_REQ                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_BATTERY_LEVEL_REQ",
+	[MSG_SMS_BATTERY_LEVEL_RES                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_BATTERY_LEVEL_RES",
+	[MSG_SMS_CMMB_INJECT_TABLE_REQ_OBSOLETE      - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_INJECT_TABLE_REQ_OBSOLETE",
+	[MSG_SMS_CMMB_INJECT_TABLE_RES_OBSOLETE      - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_INJECT_TABLE_RES_OBSOLETE",
+	[MSG_SMS_FM_RADIO_BLOCK_IND                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_FM_RADIO_BLOCK_IND",
+	[MSG_SMS_HOST_NOTIFICATION_IND               - MSG_TYPE_BASE_VAL] = "MSG_SMS_HOST_NOTIFICATION_IND",
+	[MSG_SMS_CMMB_GET_CONTROL_TABLE_REQ_OBSOLETE - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_CONTROL_TABLE_REQ_OBSOLETE",
+	[MSG_SMS_CMMB_GET_CONTROL_TABLE_RES_OBSOLETE - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_CONTROL_TABLE_RES_OBSOLETE",
+	[MSG_SMS_CMMB_GET_NETWORKS_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_NETWORKS_REQ",
+	[MSG_SMS_CMMB_GET_NETWORKS_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_NETWORKS_RES",
+	[MSG_SMS_CMMB_START_SERVICE_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_START_SERVICE_REQ",
+	[MSG_SMS_CMMB_START_SERVICE_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_START_SERVICE_RES",
+	[MSG_SMS_CMMB_STOP_SERVICE_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_STOP_SERVICE_REQ",
+	[MSG_SMS_CMMB_STOP_SERVICE_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_STOP_SERVICE_RES",
+	[MSG_SMS_CMMB_ADD_CHANNEL_FILTER_REQ         - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_ADD_CHANNEL_FILTER_REQ",
+	[MSG_SMS_CMMB_ADD_CHANNEL_FILTER_RES         - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_ADD_CHANNEL_FILTER_RES",
+	[MSG_SMS_CMMB_REMOVE_CHANNEL_FILTER_REQ      - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_REMOVE_CHANNEL_FILTER_REQ",
+	[MSG_SMS_CMMB_REMOVE_CHANNEL_FILTER_RES      - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_REMOVE_CHANNEL_FILTER_RES",
+	[MSG_SMS_CMMB_START_CONTROL_INFO_REQ         - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_START_CONTROL_INFO_REQ",
+	[MSG_SMS_CMMB_START_CONTROL_INFO_RES         - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_START_CONTROL_INFO_RES",
+	[MSG_SMS_CMMB_STOP_CONTROL_INFO_REQ          - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_STOP_CONTROL_INFO_REQ",
+	[MSG_SMS_CMMB_STOP_CONTROL_INFO_RES          - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_STOP_CONTROL_INFO_RES",
+	[MSG_SMS_ISDBT_TUNE_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_TUNE_REQ",
+	[MSG_SMS_ISDBT_TUNE_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_ISDBT_TUNE_RES",
+	[MSG_SMS_TRANSMISSION_IND                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_TRANSMISSION_IND",
+	[MSG_SMS_PID_STATISTICS_IND                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_PID_STATISTICS_IND",
+	[MSG_SMS_POWER_DOWN_IND                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_DOWN_IND",
+	[MSG_SMS_POWER_DOWN_CONF                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_DOWN_CONF",
+	[MSG_SMS_POWER_UP_IND                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_UP_IND",
+	[MSG_SMS_POWER_UP_CONF                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_UP_CONF",
+	[MSG_SMS_POWER_MODE_SET_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_MODE_SET_REQ",
+	[MSG_SMS_POWER_MODE_SET_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_POWER_MODE_SET_RES",
+	[MSG_SMS_DEBUG_HOST_EVENT_REQ                - MSG_TYPE_BASE_VAL] = "MSG_SMS_DEBUG_HOST_EVENT_REQ",
+	[MSG_SMS_DEBUG_HOST_EVENT_RES                - MSG_TYPE_BASE_VAL] = "MSG_SMS_DEBUG_HOST_EVENT_RES",
+	[MSG_SMS_NEW_CRYSTAL_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_NEW_CRYSTAL_REQ",
+	[MSG_SMS_NEW_CRYSTAL_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_NEW_CRYSTAL_RES",
+	[MSG_SMS_CONFIG_SPI_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_CONFIG_SPI_REQ",
+	[MSG_SMS_CONFIG_SPI_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_CONFIG_SPI_RES",
+	[MSG_SMS_I2C_SHORT_STAT_IND                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_I2C_SHORT_STAT_IND",
+	[MSG_SMS_START_IR_REQ                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_START_IR_REQ",
+	[MSG_SMS_START_IR_RES                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_START_IR_RES",
+	[MSG_SMS_IR_SAMPLES_IND                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_IR_SAMPLES_IND",
+	[MSG_SMS_CMMB_CA_SERVICE_IND                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_CA_SERVICE_IND",
+	[MSG_SMS_SLAVE_DEVICE_DETECTED               - MSG_TYPE_BASE_VAL] = "MSG_SMS_SLAVE_DEVICE_DETECTED",
+	[MSG_SMS_INTERFACE_LOCK_IND                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_INTERFACE_LOCK_IND",
+	[MSG_SMS_INTERFACE_UNLOCK_IND                - MSG_TYPE_BASE_VAL] = "MSG_SMS_INTERFACE_UNLOCK_IND",
+	[MSG_SMS_SEND_ROSUM_BUFF_REQ                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SEND_ROSUM_BUFF_REQ",
+	[MSG_SMS_SEND_ROSUM_BUFF_RES                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SEND_ROSUM_BUFF_RES",
+	[MSG_SMS_ROSUM_BUFF                          - MSG_TYPE_BASE_VAL] = "MSG_SMS_ROSUM_BUFF",
+	[MSG_SMS_SET_AES128_KEY_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_AES128_KEY_REQ",
+	[MSG_SMS_SET_AES128_KEY_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_AES128_KEY_RES",
+	[MSG_SMS_MBBMS_WRITE_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_MBBMS_WRITE_REQ",
+	[MSG_SMS_MBBMS_WRITE_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_MBBMS_WRITE_RES",
+	[MSG_SMS_MBBMS_READ_IND                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_MBBMS_READ_IND",
+	[MSG_SMS_IQ_STREAM_START_REQ                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_IQ_STREAM_START_REQ",
+	[MSG_SMS_IQ_STREAM_START_RES                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_IQ_STREAM_START_RES",
+	[MSG_SMS_IQ_STREAM_STOP_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_IQ_STREAM_STOP_REQ",
+	[MSG_SMS_IQ_STREAM_STOP_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_IQ_STREAM_STOP_RES",
+	[MSG_SMS_IQ_STREAM_DATA_BLOCK                - MSG_TYPE_BASE_VAL] = "MSG_SMS_IQ_STREAM_DATA_BLOCK",
+	[MSG_SMS_GET_EEPROM_VERSION_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_EEPROM_VERSION_REQ",
+	[MSG_SMS_GET_EEPROM_VERSION_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_GET_EEPROM_VERSION_RES",
+	[MSG_SMS_SIGNAL_DETECTED_IND                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SIGNAL_DETECTED_IND",
+	[MSG_SMS_NO_SIGNAL_IND                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_NO_SIGNAL_IND",
+	[MSG_SMS_MRC_SHUTDOWN_SLAVE_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_MRC_SHUTDOWN_SLAVE_REQ",
+	[MSG_SMS_MRC_SHUTDOWN_SLAVE_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_MRC_SHUTDOWN_SLAVE_RES",
+	[MSG_SMS_MRC_BRINGUP_SLAVE_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_MRC_BRINGUP_SLAVE_REQ",
+	[MSG_SMS_MRC_BRINGUP_SLAVE_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_MRC_BRINGUP_SLAVE_RES",
+	[MSG_SMS_EXTERNAL_LNA_CTRL_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_EXTERNAL_LNA_CTRL_REQ",
+	[MSG_SMS_EXTERNAL_LNA_CTRL_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_EXTERNAL_LNA_CTRL_RES",
+	[MSG_SMS_SET_PERIODIC_STATISTICS_REQ         - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_PERIODIC_STATISTICS_REQ",
+	[MSG_SMS_SET_PERIODIC_STATISTICS_RES         - MSG_TYPE_BASE_VAL] = "MSG_SMS_SET_PERIODIC_STATISTICS_RES",
+	[MSG_SMS_CMMB_SET_AUTO_OUTPUT_TS0_REQ        - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SET_AUTO_OUTPUT_TS0_REQ",
+	[MSG_SMS_CMMB_SET_AUTO_OUTPUT_TS0_RES        - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SET_AUTO_OUTPUT_TS0_RES",
+	[LOCAL_TUNE                                  - MSG_TYPE_BASE_VAL] = "LOCAL_TUNE",
+	[LOCAL_IFFT_H_ICI                            - MSG_TYPE_BASE_VAL] = "LOCAL_IFFT_H_ICI",
+	[MSG_RESYNC_REQ                              - MSG_TYPE_BASE_VAL] = "MSG_RESYNC_REQ",
+	[MSG_SMS_CMMB_GET_MRC_STATISTICS_REQ         - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_MRC_STATISTICS_REQ",
+	[MSG_SMS_CMMB_GET_MRC_STATISTICS_RES         - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_MRC_STATISTICS_RES",
+	[MSG_SMS_LOG_EX_ITEM                         - MSG_TYPE_BASE_VAL] = "MSG_SMS_LOG_EX_ITEM",
+	[MSG_SMS_DEVICE_DATA_LOSS_IND                - MSG_TYPE_BASE_VAL] = "MSG_SMS_DEVICE_DATA_LOSS_IND",
+	[MSG_SMS_MRC_WATCHDOG_TRIGGERED_IND          - MSG_TYPE_BASE_VAL] = "MSG_SMS_MRC_WATCHDOG_TRIGGERED_IND",
+	[MSG_SMS_USER_MSG_REQ                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_USER_MSG_REQ",
+	[MSG_SMS_USER_MSG_RES                        - MSG_TYPE_BASE_VAL] = "MSG_SMS_USER_MSG_RES",
+	[MSG_SMS_SMART_CARD_INIT_REQ                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SMART_CARD_INIT_REQ",
+	[MSG_SMS_SMART_CARD_INIT_RES                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SMART_CARD_INIT_RES",
+	[MSG_SMS_SMART_CARD_WRITE_REQ                - MSG_TYPE_BASE_VAL] = "MSG_SMS_SMART_CARD_WRITE_REQ",
+	[MSG_SMS_SMART_CARD_WRITE_RES                - MSG_TYPE_BASE_VAL] = "MSG_SMS_SMART_CARD_WRITE_RES",
+	[MSG_SMS_SMART_CARD_READ_IND                 - MSG_TYPE_BASE_VAL] = "MSG_SMS_SMART_CARD_READ_IND",
+	[MSG_SMS_TSE_ENABLE_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_TSE_ENABLE_REQ",
+	[MSG_SMS_TSE_ENABLE_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_TSE_ENABLE_RES",
+	[MSG_SMS_CMMB_GET_SHORT_STATISTICS_REQ       - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_SHORT_STATISTICS_REQ",
+	[MSG_SMS_CMMB_GET_SHORT_STATISTICS_RES       - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_GET_SHORT_STATISTICS_RES",
+	[MSG_SMS_LED_CONFIG_REQ                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_LED_CONFIG_REQ",
+	[MSG_SMS_LED_CONFIG_RES                      - MSG_TYPE_BASE_VAL] = "MSG_SMS_LED_CONFIG_RES",
+	[MSG_PWM_ANTENNA_REQ                         - MSG_TYPE_BASE_VAL] = "MSG_PWM_ANTENNA_REQ",
+	[MSG_PWM_ANTENNA_RES                         - MSG_TYPE_BASE_VAL] = "MSG_PWM_ANTENNA_RES",
+	[MSG_SMS_CMMB_SMD_SN_REQ                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SMD_SN_REQ",
+	[MSG_SMS_CMMB_SMD_SN_RES                     - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SMD_SN_RES",
+	[MSG_SMS_CMMB_SET_CA_CW_REQ                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SET_CA_CW_REQ",
+	[MSG_SMS_CMMB_SET_CA_CW_RES                  - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SET_CA_CW_RES",
+	[MSG_SMS_CMMB_SET_CA_SALT_REQ                - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SET_CA_SALT_REQ",
+	[MSG_SMS_CMMB_SET_CA_SALT_RES                - MSG_TYPE_BASE_VAL] = "MSG_SMS_CMMB_SET_CA_SALT_RES",
+	[MSG_SMS_NSCD_INIT_REQ                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_NSCD_INIT_REQ",
+	[MSG_SMS_NSCD_INIT_RES                       - MSG_TYPE_BASE_VAL] = "MSG_SMS_NSCD_INIT_RES",
+	[MSG_SMS_NSCD_PROCESS_SECTION_REQ            - MSG_TYPE_BASE_VAL] = "MSG_SMS_NSCD_PROCESS_SECTION_REQ",
+	[MSG_SMS_NSCD_PROCESS_SECTION_RES            - MSG_TYPE_BASE_VAL] = "MSG_SMS_NSCD_PROCESS_SECTION_RES",
+	[MSG_SMS_DBD_CREATE_OBJECT_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_CREATE_OBJECT_REQ",
+	[MSG_SMS_DBD_CREATE_OBJECT_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_CREATE_OBJECT_RES",
+	[MSG_SMS_DBD_CONFIGURE_REQ                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_CONFIGURE_REQ",
+	[MSG_SMS_DBD_CONFIGURE_RES                   - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_CONFIGURE_RES",
+	[MSG_SMS_DBD_SET_KEYS_REQ                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_SET_KEYS_REQ",
+	[MSG_SMS_DBD_SET_KEYS_RES                    - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_SET_KEYS_RES",
+	[MSG_SMS_DBD_PROCESS_HEADER_REQ              - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_PROCESS_HEADER_REQ",
+	[MSG_SMS_DBD_PROCESS_HEADER_RES              - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_PROCESS_HEADER_RES",
+	[MSG_SMS_DBD_PROCESS_DATA_REQ                - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_PROCESS_DATA_REQ",
+	[MSG_SMS_DBD_PROCESS_DATA_RES                - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_PROCESS_DATA_RES",
+	[MSG_SMS_DBD_PROCESS_GET_DATA_REQ            - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_PROCESS_GET_DATA_REQ",
+	[MSG_SMS_DBD_PROCESS_GET_DATA_RES            - MSG_TYPE_BASE_VAL] = "MSG_SMS_DBD_PROCESS_GET_DATA_RES",
+	[MSG_SMS_NSCD_OPEN_SESSION_REQ               - MSG_TYPE_BASE_VAL] = "MSG_SMS_NSCD_OPEN_SESSION_REQ",
+	[MSG_SMS_NSCD_OPEN_SESSION_RES               - MSG_TYPE_BASE_VAL] = "MSG_SMS_NSCD_OPEN_SESSION_RES",
+	[MSG_SMS_SEND_HOST_DATA_TO_DEMUX_REQ         - MSG_TYPE_BASE_VAL] = "MSG_SMS_SEND_HOST_DATA_TO_DEMUX_REQ",
+	[MSG_SMS_SEND_HOST_DATA_TO_DEMUX_RES         - MSG_TYPE_BASE_VAL] = "MSG_SMS_SEND_HOST_DATA_TO_DEMUX_RES",
+	[MSG_LAST_MSG_TYPE                           - MSG_TYPE_BASE_VAL] = "MSG_LAST_MSG_TYPE",
+};
+
+char *smscore_translate_msg(enum msg_types msgtype)
+{
+	int i = msgtype - MSG_TYPE_BASE_VAL;
+	char *msg;
+
+	if (i < 0 || i >= ARRAY_SIZE(siano_msgs))
+		return "Unknown msg type";
+
+	msg = siano_msgs[i];
+
+	if (!*msg)
+		return "Unknown msg type";
+
+	return msg;
+}
+EXPORT_SYMBOL_GPL(smscore_translate_msg);
+
 void smscore_set_board_id(struct smscore_device_t *core, int id)
 {
 	core->board_id = id;
@@ -96,7 +434,7 @@ static struct mutex g_smscore_deviceslock;
 static struct list_head g_smscore_registry;
 static struct mutex g_smscore_registrylock;
 
-static int default_mode = 4;
+static int default_mode = DEVICE_MODE_NONE;
 
 module_param(default_mode, int, 0644);
 MODULE_PARM_DESC(default_mode, "default firmware id (device mode)");
@@ -151,10 +489,10 @@ static enum sms_device_type_st smscore_registry_gettype(char *devpath)
 	else
 		sms_err("No registry found.");
 
-	return -1;
+	return -EINVAL;
 }
 
-void smscore_registry_setmode(char *devpath, int mode)
+static void smscore_registry_setmode(char *devpath, int mode)
 {
 	struct smscore_registry_entry_t *entry;
 
@@ -294,10 +632,11 @@ static struct
 smscore_buffer_t *smscore_createbuffer(u8 *buffer, void *common_buffer,
 				       dma_addr_t common_buffer_phys)
 {
-	struct smscore_buffer_t *cb =
-		kmalloc(sizeof(struct smscore_buffer_t), GFP_KERNEL);
+	struct smscore_buffer_t *cb;
+
+	cb = kzalloc(sizeof(struct smscore_buffer_t), GFP_KERNEL);
 	if (!cb) {
-		sms_info("kmalloc(...) failed");
+		sms_info("kzalloc(...) failed");
 		return NULL;
 	}
 
@@ -344,6 +683,7 @@ int smscore_register_device(struct smsdevice_params_t *params,
 	/* init completion events */
 	init_completion(&dev->version_ex_done);
 	init_completion(&dev->data_download_done);
+	init_completion(&dev->data_validity_done);
 	init_completion(&dev->trigger_done);
 	init_completion(&dev->init_device_done);
 	init_completion(&dev->reload_start_done);
@@ -370,9 +710,10 @@ int smscore_register_device(struct smsdevice_params_t *params,
 	for (buffer = dev->common_buffer;
 	     dev->num_buffers < params->num_buffers;
 	     dev->num_buffers++, buffer += params->buffer_size) {
-		struct smscore_buffer_t *cb =
-			smscore_createbuffer(buffer, dev->common_buffer,
-					     dev->common_buffer_phys);
+		struct smscore_buffer_t *cb;
+
+		cb = smscore_createbuffer(buffer, dev->common_buffer,
+					  dev->common_buffer_phys);
 		if (!cb) {
 			smscore_unregister_device(dev);
 			return -ENOMEM;
@@ -384,6 +725,7 @@ int smscore_register_device(struct smsdevice_params_t *params,
 	sms_info("allocated %d buffers", dev->num_buffers);
 
 	dev->mode = DEVICE_MODE_NONE;
+	dev->board_id = SMS_BOARD_UNKNOWN;
 	dev->context = params->context;
 	dev->device = params->device;
 	dev->setmode_handler = params->setmode_handler;
@@ -413,7 +755,13 @@ EXPORT_SYMBOL_GPL(smscore_register_device);
 
 static int smscore_sendrequest_and_wait(struct smscore_device_t *coredev,
 		void *buffer, size_t size, struct completion *completion) {
-	int rc = coredev->sendrequest_handler(coredev->context, buffer, size);
+	int rc;
+
+	if (completion == NULL)
+		return -EINVAL;
+	init_completion(completion);
+
+	rc = coredev->sendrequest_handler(coredev->context, buffer, size);
 	if (rc < 0) {
 		sms_info("sendrequest returned error %d", rc);
 		return rc;
@@ -444,24 +792,22 @@ static int smscore_init_ir(struct smscore_device_t *coredev)
 		if	(rc != 0)
 			sms_err("Error initialization DTV IR sub-module");
 		else {
-			buffer = kmalloc(sizeof(struct SmsMsgData_ST2) +
+			buffer = kmalloc(sizeof(struct sms_msg_data2) +
 						SMS_DMA_ALIGNMENT,
 						GFP_KERNEL | GFP_DMA);
 			if (buffer) {
-				struct SmsMsgData_ST2 *msg =
-				(struct SmsMsgData_ST2 *)
+				struct sms_msg_data2 *msg =
+				(struct sms_msg_data2 *)
 				SMS_ALIGN_ADDRESS(buffer);
 
-				SMS_INIT_MSG(&msg->xMsgHeader,
+				SMS_INIT_MSG(&msg->x_msg_header,
 						MSG_SMS_START_IR_REQ,
-						sizeof(struct SmsMsgData_ST2));
-				msg->msgData[0] = coredev->ir.controller;
-				msg->msgData[1] = coredev->ir.timeout;
+						sizeof(struct sms_msg_data2));
+				msg->msg_data[0] = coredev->ir.controller;
+				msg->msg_data[1] = coredev->ir.timeout;
 
-				smsendian_handle_tx_message(
-					(struct SmsMsgHdr_ST2 *)msg);
 				rc = smscore_sendrequest_and_wait(coredev, msg,
-						msg->xMsgHeader. msgLength,
+						msg->x_msg_header. msg_length,
 						&coredev->ir_init_done);
 
 				kfree(buffer);
@@ -476,21 +822,82 @@ static int smscore_init_ir(struct smscore_device_t *coredev)
 }
 
 /**
+ * configures device features according to board configuration structure.
+ *
+ * @param coredev pointer to a coredev object returned by
+ *                smscore_register_device
+ *
+ * @return 0 on success, <0 on error.
+ */
+static int smscore_configure_board(struct smscore_device_t *coredev)
+{
+	struct sms_board *board;
+
+	board = sms_get_board(coredev->board_id);
+	if (!board) {
+		sms_err("no board configuration exist.");
+		return -EINVAL;
+	}
+
+	if (board->mtu) {
+		struct sms_msg_data mtu_msg;
+		sms_debug("set max transmit unit %d", board->mtu);
+
+		mtu_msg.x_msg_header.msg_src_id = 0;
+		mtu_msg.x_msg_header.msg_dst_id = HIF_TASK;
+		mtu_msg.x_msg_header.msg_flags = 0;
+		mtu_msg.x_msg_header.msg_type = MSG_SMS_SET_MAX_TX_MSG_LEN_REQ;
+		mtu_msg.x_msg_header.msg_length = sizeof(mtu_msg);
+		mtu_msg.msg_data[0] = board->mtu;
+
+		coredev->sendrequest_handler(coredev->context, &mtu_msg,
+					     sizeof(mtu_msg));
+	}
+
+	if (board->crystal) {
+		struct sms_msg_data crys_msg;
+		sms_debug("set crystal value %d", board->crystal);
+
+		SMS_INIT_MSG(&crys_msg.x_msg_header,
+				MSG_SMS_NEW_CRYSTAL_REQ,
+				sizeof(crys_msg));
+		crys_msg.msg_data[0] = board->crystal;
+
+		coredev->sendrequest_handler(coredev->context, &crys_msg,
+					     sizeof(crys_msg));
+	}
+
+	return 0;
+}
+
+/**
  * sets initial device mode and notifies client hotplugs that device is ready
  *
  * @param coredev pointer to a coredev object returned by
- * 		  smscore_register_device
+ *		  smscore_register_device
  *
  * @return 0 on success, <0 on error.
  */
 int smscore_start_device(struct smscore_device_t *coredev)
 {
-	int rc = smscore_set_device_mode(
-			coredev, smscore_registry_getmode(coredev->devpath));
+	int rc;
+	int board_id = smscore_get_board_id(coredev);
+	int mode = smscore_registry_getmode(coredev->devpath);
+
+	/* Device is initialized as DEVICE_MODE_NONE */
+	if (board_id != SMS_BOARD_UNKNOWN && mode == DEVICE_MODE_NONE)
+		mode = sms_get_board(board_id)->default_mode;
+
+	rc = smscore_set_device_mode(coredev, mode);
 	if (rc < 0) {
 		sms_info("set device mode faile , rc %d", rc);
 		return rc;
 	}
+	rc = smscore_configure_board(coredev);
+	if (rc < 0) {
+		sms_info("configure board failed , rc %d", rc);
+		return rc;
+	}
 
 	kmutex_lock(&g_smscore_deviceslock);
 
@@ -509,18 +916,19 @@ EXPORT_SYMBOL_GPL(smscore_start_device);
 static int smscore_load_firmware_family2(struct smscore_device_t *coredev,
 					 void *buffer, size_t size)
 {
-	struct SmsFirmware_ST *firmware = (struct SmsFirmware_ST *) buffer;
-	struct SmsMsgHdr_ST *msg;
-	u32 mem_address;
-	u8 *payload = firmware->Payload;
+	struct sms_firmware *firmware = (struct sms_firmware *) buffer;
+	struct sms_msg_data4 *msg;
+	u32 mem_address,  calc_checksum = 0;
+	u32 i, *ptr;
+	u8 *payload = firmware->payload;
 	int rc = 0;
-	firmware->StartAddress = le32_to_cpu(firmware->StartAddress);
-	firmware->Length = le32_to_cpu(firmware->Length);
+	firmware->start_address = le32_to_cpu(firmware->start_address);
+	firmware->length = le32_to_cpu(firmware->length);
 
-	mem_address = firmware->StartAddress;
+	mem_address = firmware->start_address;
 
 	sms_info("loading FW to addr 0x%x size %d",
-		 mem_address, firmware->Length);
+		 mem_address, firmware->length);
 	if (coredev->preload_handler) {
 		rc = coredev->preload_handler(coredev->context);
 		if (rc < 0)
@@ -534,35 +942,36 @@ static int smscore_load_firmware_family2(struct smscore_device_t *coredev,
 
 	if (coredev->mode != DEVICE_MODE_NONE) {
 		sms_debug("sending reload command.");
-		SMS_INIT_MSG(msg, MSG_SW_RELOAD_START_REQ,
-			     sizeof(struct SmsMsgHdr_ST));
+		SMS_INIT_MSG(&msg->x_msg_header, MSG_SW_RELOAD_START_REQ,
+			     sizeof(struct sms_msg_hdr));
 		rc = smscore_sendrequest_and_wait(coredev, msg,
-						  msg->msgLength,
+						  msg->x_msg_header.msg_length,
 						  &coredev->reload_start_done);
+		if (rc < 0) {
+			sms_err("device reload failed, rc %d", rc);
+			goto exit_fw_download;
+		}
 		mem_address = *(u32 *) &payload[20];
 	}
 
+	for (i = 0, ptr = (u32 *)firmware->payload; i < firmware->length/4 ;
+	     i++, ptr++)
+		calc_checksum += *ptr;
+
 	while (size && rc >= 0) {
-		struct SmsDataDownload_ST *DataMsg =
-			(struct SmsDataDownload_ST *) msg;
-		int payload_size = min((int) size, SMS_MAX_PAYLOAD_SIZE);
+		struct sms_data_download *data_msg =
+			(struct sms_data_download *) msg;
+		int payload_size = min_t(int, size, SMS_MAX_PAYLOAD_SIZE);
 
-		SMS_INIT_MSG(msg, MSG_SMS_DATA_DOWNLOAD_REQ,
-			     (u16)(sizeof(struct SmsMsgHdr_ST) +
+		SMS_INIT_MSG(&msg->x_msg_header, MSG_SMS_DATA_DOWNLOAD_REQ,
+			     (u16)(sizeof(struct sms_msg_hdr) +
 				      sizeof(u32) + payload_size));
 
-		DataMsg->MemAddr = mem_address;
-		memcpy(DataMsg->Payload, payload, payload_size);
+		data_msg->mem_addr = mem_address;
+		memcpy(data_msg->payload, payload, payload_size);
 
-		if ((coredev->device_flags & SMS_ROM_NO_RESPONSE) &&
-		    (coredev->mode == DEVICE_MODE_NONE))
-			rc = coredev->sendrequest_handler(
-				coredev->context, DataMsg,
-				DataMsg->xMsgHeader.msgLength);
-		else
-			rc = smscore_sendrequest_and_wait(
-				coredev, DataMsg,
-				DataMsg->xMsgHeader.msgLength,
+		rc = smscore_sendrequest_and_wait(coredev, data_msg,
+				data_msg->x_msg_header.msg_length,
 				&coredev->data_download_done);
 
 		payload += payload_size;
@@ -570,50 +979,158 @@ static int smscore_load_firmware_family2(struct smscore_device_t *coredev,
 		mem_address += payload_size;
 	}
 
-	if (rc >= 0) {
-		if (coredev->mode == DEVICE_MODE_NONE) {
-			struct SmsMsgData_ST *TriggerMsg =
-				(struct SmsMsgData_ST *) msg;
-
-			SMS_INIT_MSG(msg, MSG_SMS_SWDOWNLOAD_TRIGGER_REQ,
-				     sizeof(struct SmsMsgHdr_ST) +
-				     sizeof(u32) * 5);
-
-			TriggerMsg->msgData[0] = firmware->StartAddress;
-						/* Entry point */
-			TriggerMsg->msgData[1] = 5; /* Priority */
-			TriggerMsg->msgData[2] = 0x200; /* Stack size */
-			TriggerMsg->msgData[3] = 0; /* Parameter */
-			TriggerMsg->msgData[4] = 4; /* Task ID */
-
-			if (coredev->device_flags & SMS_ROM_NO_RESPONSE) {
-				rc = coredev->sendrequest_handler(
-					coredev->context, TriggerMsg,
-					TriggerMsg->xMsgHeader.msgLength);
-				msleep(100);
-			} else
-				rc = smscore_sendrequest_and_wait(
-					coredev, TriggerMsg,
-					TriggerMsg->xMsgHeader.msgLength,
+	if (rc < 0)
+		goto exit_fw_download;
+
+	sms_err("sending MSG_SMS_DATA_VALIDITY_REQ expecting 0x%x",
+		calc_checksum);
+	SMS_INIT_MSG(&msg->x_msg_header, MSG_SMS_DATA_VALIDITY_REQ,
+			sizeof(msg->x_msg_header) +
+			sizeof(u32) * 3);
+	msg->msg_data[0] = firmware->start_address;
+		/* Entry point */
+	msg->msg_data[1] = firmware->length;
+	msg->msg_data[2] = 0; /* Regular checksum*/
+	rc = smscore_sendrequest_and_wait(coredev, msg,
+					  msg->x_msg_header.msg_length,
+					  &coredev->data_validity_done);
+	if (rc < 0)
+		goto exit_fw_download;
+
+	if (coredev->mode == DEVICE_MODE_NONE) {
+		struct sms_msg_data *trigger_msg =
+			(struct sms_msg_data *) msg;
+
+		sms_debug("sending MSG_SMS_SWDOWNLOAD_TRIGGER_REQ");
+		SMS_INIT_MSG(&msg->x_msg_header,
+				MSG_SMS_SWDOWNLOAD_TRIGGER_REQ,
+				sizeof(struct sms_msg_hdr) +
+				sizeof(u32) * 5);
+
+		trigger_msg->msg_data[0] = firmware->start_address;
+					/* Entry point */
+		trigger_msg->msg_data[1] = 6; /* Priority */
+		trigger_msg->msg_data[2] = 0x200; /* Stack size */
+		trigger_msg->msg_data[3] = 0; /* Parameter */
+		trigger_msg->msg_data[4] = 4; /* Task ID */
+
+		rc = smscore_sendrequest_and_wait(coredev, trigger_msg,
+					trigger_msg->x_msg_header.msg_length,
 					&coredev->trigger_done);
-		} else {
-			SMS_INIT_MSG(msg, MSG_SW_RELOAD_EXEC_REQ,
-				     sizeof(struct SmsMsgHdr_ST));
-
-			rc = coredev->sendrequest_handler(coredev->context,
-							  msg, msg->msgLength);
-		}
-		msleep(500);
+	} else {
+		SMS_INIT_MSG(&msg->x_msg_header, MSG_SW_RELOAD_EXEC_REQ,
+				sizeof(struct sms_msg_hdr));
+		rc = coredev->sendrequest_handler(coredev->context, msg,
+				msg->x_msg_header.msg_length);
 	}
 
-	sms_debug("rc=%d, postload=%p ", rc,
-		  coredev->postload_handler);
+	if (rc < 0)
+		goto exit_fw_download;
+
+	/*
+	 * backward compatibility - wait to device_ready_done for
+	 * not more than 400 ms
+	 */
+	msleep(400);
 
+exit_fw_download:
 	kfree(msg);
 
-	return ((rc >= 0) && coredev->postload_handler) ?
-		coredev->postload_handler(coredev->context) :
-		rc;
+	if (coredev->postload_handler) {
+		sms_debug("rc=%d, postload=0x%p", rc, coredev->postload_handler);
+		if (rc >= 0)
+			return coredev->postload_handler(coredev->context);
+	}
+
+	sms_debug("rc=%d", rc);
+	return rc;
+}
+
+static char *smscore_fw_lkup[][DEVICE_MODE_MAX] = {
+	[SMS_NOVA_A0] = {
+		[DEVICE_MODE_DVBT]		= SMS_FW_DVB_NOVA_12MHZ,
+		[DEVICE_MODE_DVBH]		= SMS_FW_DVB_NOVA_12MHZ,
+		[DEVICE_MODE_DAB_TDMB]		= SMS_FW_TDMB_NOVA_12MHZ,
+		[DEVICE_MODE_DVBT_BDA]		= SMS_FW_DVB_NOVA_12MHZ,
+		[DEVICE_MODE_ISDBT]		= SMS_FW_ISDBT_NOVA_12MHZ,
+		[DEVICE_MODE_ISDBT_BDA]		= SMS_FW_ISDBT_NOVA_12MHZ,
+	},
+	[SMS_NOVA_B0] = {
+		[DEVICE_MODE_DVBT]		= SMS_FW_DVB_NOVA_12MHZ_B0,
+		[DEVICE_MODE_DVBH]		= SMS_FW_DVB_NOVA_12MHZ_B0,
+		[DEVICE_MODE_DAB_TDMB]		= SMS_FW_TDMB_NOVA_12MHZ_B0,
+		[DEVICE_MODE_DVBT_BDA]		= SMS_FW_DVB_NOVA_12MHZ_B0,
+		[DEVICE_MODE_ISDBT]		= SMS_FW_ISDBT_NOVA_12MHZ_B0,
+		[DEVICE_MODE_ISDBT_BDA]		= SMS_FW_ISDBT_NOVA_12MHZ_B0,
+		[DEVICE_MODE_FM_RADIO]		= SMS_FW_FM_RADIO,
+		[DEVICE_MODE_FM_RADIO_BDA]	= SMS_FW_FM_RADIO,
+	},
+	[SMS_VEGA] = {
+		[DEVICE_MODE_CMMB]		= SMS_FW_CMMB_VEGA_12MHZ,
+	},
+	[SMS_VENICE] = {
+		[DEVICE_MODE_CMMB]		= SMS_FW_CMMB_VENICE_12MHZ,
+	},
+	[SMS_MING] = {
+		[DEVICE_MODE_CMMB]		= SMS_FW_CMMB_MING_APP,
+	},
+	[SMS_PELE] = {
+		[DEVICE_MODE_ISDBT]		= SMS_FW_ISDBT_PELE,
+		[DEVICE_MODE_ISDBT_BDA]		= SMS_FW_ISDBT_PELE,
+	},
+	[SMS_RIO] = {
+		[DEVICE_MODE_DVBT]		= SMS_FW_DVB_RIO,
+		[DEVICE_MODE_DVBH]		= SMS_FW_DVBH_RIO,
+		[DEVICE_MODE_DVBT_BDA]		= SMS_FW_DVB_RIO,
+		[DEVICE_MODE_ISDBT]		= SMS_FW_ISDBT_RIO,
+		[DEVICE_MODE_ISDBT_BDA]		= SMS_FW_ISDBT_RIO,
+		[DEVICE_MODE_FM_RADIO]		= SMS_FW_FM_RADIO_RIO,
+		[DEVICE_MODE_FM_RADIO_BDA]	= SMS_FW_FM_RADIO_RIO,
+	},
+	[SMS_DENVER_1530] = {
+		[DEVICE_MODE_ATSC]		= SMS_FW_ATSC_DENVER,
+	},
+	[SMS_DENVER_2160] = {
+		[DEVICE_MODE_DAB_TDMB]		= SMS_FW_TDMB_DENVER,
+	},
+};
+
+/**
+ * get firmware file name from one of the two mechanisms : sms_boards or
+ * smscore_fw_lkup.
+ * @param coredev pointer to a coredev object returned by
+ *		  smscore_register_device
+ * @param mode requested mode of operation
+ * @param lookup if 1, always get the fw filename from smscore_fw_lkup
+ *	 table. if 0, try first to get from sms_boards
+ *
+ * @return 0 on success, <0 on error.
+ */
+static char *smscore_get_fw_filename(struct smscore_device_t *coredev,
+			      int mode)
+{
+	char **fw;
+	int board_id = smscore_get_board_id(coredev);
+	enum sms_device_type_st type;
+
+	type = smscore_registry_gettype(coredev->devpath);
+
+	/* Prevent looking outside the smscore_fw_lkup table */
+	if (type <= SMS_UNKNOWN_TYPE || type >= SMS_NUM_OF_DEVICE_TYPES)
+		return NULL;
+	if (mode <= DEVICE_MODE_NONE || mode >= DEVICE_MODE_MAX)
+		return NULL;
+
+	sms_debug("trying to get fw name from sms_boards board_id %d mode %d",
+		  board_id, mode);
+	fw = sms_get_board(board_id)->fw;
+	if (!fw || !fw[mode]) {
+		sms_debug("cannot find fw name in sms_boards, getting from lookup table mode %d type %d",
+			  mode, type);
+		return smscore_fw_lkup[type][mode];
+	}
+
+	return fw[mode];
 }
 
 /**
@@ -627,41 +1144,46 @@ static int smscore_load_firmware_family2(struct smscore_device_t *coredev,
  * @return 0 on success, <0 on error.
  */
 static int smscore_load_firmware_from_file(struct smscore_device_t *coredev,
-					   char *filename,
+					   int mode,
 					   loadfirmware_t loadfirmware_handler)
 {
 	int rc = -ENOENT;
+	u8 *fw_buf;
+	u32 fw_buf_size;
 	const struct firmware *fw;
-	u8 *fw_buffer;
 
-	if (loadfirmware_handler == NULL && !(coredev->device_flags &
-					      SMS_DEVICE_FAMILY2))
+	char *fw_filename = smscore_get_fw_filename(coredev, mode);
+	if (!fw_filename) {
+		sms_info("mode %d not supported on this device", mode);
+		return -ENOENT;
+	}
+	sms_debug("Firmware name: %s", fw_filename);
+
+	if (loadfirmware_handler == NULL && !(coredev->device_flags
+			& SMS_DEVICE_FAMILY2))
 		return -EINVAL;
 
-	rc = request_firmware(&fw, filename, coredev->device);
+	rc = request_firmware(&fw, fw_filename, coredev->device);
 	if (rc < 0) {
-		sms_info("failed to open \"%s\"", filename);
+		sms_info("failed to open \"%s\"", fw_filename);
 		return rc;
 	}
-	sms_info("read FW %s, size=%zd", filename, fw->size);
-	fw_buffer = kmalloc(ALIGN(fw->size, SMS_ALLOC_ALIGNMENT),
-			    GFP_KERNEL | GFP_DMA);
-	if (fw_buffer) {
-		memcpy(fw_buffer, fw->data, fw->size);
-
-		rc = (coredev->device_flags & SMS_DEVICE_FAMILY2) ?
-		      smscore_load_firmware_family2(coredev,
-						    fw_buffer,
-						    fw->size) :
-		      loadfirmware_handler(coredev->context,
-					   fw_buffer, fw->size);
-
-		kfree(fw_buffer);
-	} else {
+	sms_info("read fw %s, buffer size=0x%zx", fw_filename, fw->size);
+	fw_buf = kmalloc(ALIGN(fw->size, SMS_ALLOC_ALIGNMENT),
+			 GFP_KERNEL | GFP_DMA);
+	if (!fw_buf) {
 		sms_info("failed to allocate firmware buffer");
-		rc = -ENOMEM;
+		return -ENOMEM;
 	}
+	memcpy(fw_buf, fw->data, fw->size);
+	fw_buf_size = fw->size;
+
+	rc = (coredev->device_flags & SMS_DEVICE_FAMILY2) ?
+		smscore_load_firmware_family2(coredev, fw_buf, fw_buf_size)
+		: loadfirmware_handler(coredev->context, fw_buf,
+		fw_buf_size);
 
+	kfree(fw_buf);
 	release_firmware(fw);
 
 	return rc;
@@ -703,14 +1225,15 @@ void smscore_unregister_device(struct smscore_device_t *coredev)
 		if (num_buffers == coredev->num_buffers)
 			break;
 		if (++retry > 10) {
-			sms_info("exiting although "
-				 "not all buffers released.");
+			sms_info("exiting although not all buffers released.");
 			break;
 		}
 
 		sms_info("waiting for %d buffer(s)",
 			 coredev->num_buffers - num_buffers);
+		kmutex_unlock(&g_smscore_deviceslock);
 		msleep(100);
+		kmutex_lock(&g_smscore_deviceslock);
 	}
 
 	sms_info("freed %d buffers", num_buffers);
@@ -719,8 +1242,7 @@ void smscore_unregister_device(struct smscore_device_t *coredev)
 		dma_free_coherent(NULL, coredev->common_buffer_size,
 			coredev->common_buffer, coredev->common_buffer_phys);
 
-	if (coredev->fw_buf != NULL)
-		kfree(coredev->fw_buf);
+	kfree(coredev->fw_buf);
 
 	list_del(&coredev->entry);
 	kfree(coredev);
@@ -733,19 +1255,19 @@ EXPORT_SYMBOL_GPL(smscore_unregister_device);
 
 static int smscore_detect_mode(struct smscore_device_t *coredev)
 {
-	void *buffer = kmalloc(sizeof(struct SmsMsgHdr_ST) + SMS_DMA_ALIGNMENT,
+	void *buffer = kmalloc(sizeof(struct sms_msg_hdr) + SMS_DMA_ALIGNMENT,
 			       GFP_KERNEL | GFP_DMA);
-	struct SmsMsgHdr_ST *msg =
-		(struct SmsMsgHdr_ST *) SMS_ALIGN_ADDRESS(buffer);
+	struct sms_msg_hdr *msg =
+		(struct sms_msg_hdr *) SMS_ALIGN_ADDRESS(buffer);
 	int rc;
 
 	if (!buffer)
 		return -ENOMEM;
 
 	SMS_INIT_MSG(msg, MSG_SMS_GET_VERSION_EX_REQ,
-		     sizeof(struct SmsMsgHdr_ST));
+		     sizeof(struct sms_msg_hdr));
 
-	rc = smscore_sendrequest_and_wait(coredev, msg, msg->msgLength,
+	rc = smscore_sendrequest_and_wait(coredev, msg, msg->msg_length,
 					  &coredev->version_ex_done);
 	if (rc == -ETIME) {
 		sms_err("MSG_SMS_GET_VERSION_EX_REQ failed first try");
@@ -753,11 +1275,11 @@ static int smscore_detect_mode(struct smscore_device_t *coredev)
 		if (wait_for_completion_timeout(&coredev->resume_done,
 						msecs_to_jiffies(5000))) {
 			rc = smscore_sendrequest_and_wait(
-				coredev, msg, msg->msgLength,
+				coredev, msg, msg->msg_length,
 				&coredev->version_ex_done);
 			if (rc < 0)
-				sms_err("MSG_SMS_GET_VERSION_EX_REQ failed "
-					"second try, rc %d", rc);
+				sms_err("MSG_SMS_GET_VERSION_EX_REQ failed second try, rc %d",
+					rc);
 		} else
 			rc = -ETIME;
 	}
@@ -767,31 +1289,39 @@ static int smscore_detect_mode(struct smscore_device_t *coredev)
 	return rc;
 }
 
-static char *smscore_fw_lkup[][SMS_NUM_OF_DEVICE_TYPES] = {
-	/*Stellar		NOVA A0		Nova B0		VEGA*/
-	/*DVBT*/
-	{"none", "dvb_nova_12mhz.inp", "dvb_nova_12mhz_b0.inp", "none"},
-	/*DVBH*/
-	{"none", "dvb_nova_12mhz.inp", "dvb_nova_12mhz_b0.inp", "none"},
-	/*TDMB*/
-	{"none", "tdmb_nova_12mhz.inp", "tdmb_nova_12mhz_b0.inp", "none"},
-	/*DABIP*/
-	{"none", "none", "none", "none"},
-	/*BDA*/
-	{"none", "dvb_nova_12mhz.inp", "dvb_nova_12mhz_b0.inp", "none"},
-	/*ISDBT*/
-	{"none", "isdbt_nova_12mhz.inp", "isdbt_nova_12mhz_b0.inp", "none"},
-	/*ISDBTBDA*/
-	{"none", "isdbt_nova_12mhz.inp", "isdbt_nova_12mhz_b0.inp", "none"},
-	/*CMMB*/
-	{"none", "none", "none", "cmmb_vega_12mhz.inp"}
-};
-
-static inline char *sms_get_fw_name(struct smscore_device_t *coredev,
-				    int mode, enum sms_device_type_st type)
+/**
+ * send init device request and wait for response
+ *
+ * @param coredev pointer to a coredev object returned by
+ *                smscore_register_device
+ * @param mode requested mode of operation
+ *
+ * @return 0 on success, <0 on error.
+ */
+static int smscore_init_device(struct smscore_device_t *coredev, int mode)
 {
-	char **fw = sms_get_board(smscore_get_board_id(coredev))->fw;
-	return (fw && fw[mode]) ? fw[mode] : smscore_fw_lkup[mode][type];
+	void *buffer;
+	struct sms_msg_data *msg;
+	int rc = 0;
+
+	buffer = kmalloc(sizeof(struct sms_msg_data) +
+			SMS_DMA_ALIGNMENT, GFP_KERNEL | GFP_DMA);
+	if (!buffer) {
+		sms_err("Could not allocate buffer for init device message.");
+		return -ENOMEM;
+	}
+
+	msg = (struct sms_msg_data *)SMS_ALIGN_ADDRESS(buffer);
+	SMS_INIT_MSG(&msg->x_msg_header, MSG_SMS_INIT_DEVICE_REQ,
+			sizeof(struct sms_msg_data));
+	msg->msg_data[0] = mode;
+
+	rc = smscore_sendrequest_and_wait(coredev, msg,
+			msg->x_msg_header. msg_length,
+			&coredev->init_device_done);
+
+	kfree(buffer);
+	return rc;
 }
 
 /**
@@ -806,13 +1336,11 @@ static inline char *sms_get_fw_name(struct smscore_device_t *coredev,
  */
 int smscore_set_device_mode(struct smscore_device_t *coredev, int mode)
 {
-	void *buffer;
 	int rc = 0;
-	enum sms_device_type_st type;
 
 	sms_debug("set device mode to %d", mode);
 	if (coredev->device_flags & SMS_DEVICE_FAMILY2) {
-		if (mode < DEVICE_MODE_DVBT || mode >= DEVICE_MODE_RAW_TUNER) {
+		if (mode <= DEVICE_MODE_NONE || mode >= DEVICE_MODE_MAX) {
 			sms_err("invalid mode specified %d", mode);
 			return -EINVAL;
 		}
@@ -833,58 +1361,21 @@ int smscore_set_device_mode(struct smscore_device_t *coredev, int mode)
 		}
 
 		if (!(coredev->modes_supported & (1 << mode))) {
-			char *fw_filename;
-
-			type = smscore_registry_gettype(coredev->devpath);
-			fw_filename = sms_get_fw_name(coredev, mode, type);
-
 			rc = smscore_load_firmware_from_file(coredev,
-							     fw_filename, NULL);
-			if (rc < 0) {
-				sms_warn("error %d loading firmware: %s, "
-					 "trying again with default firmware",
-					 rc, fw_filename);
-
-				/* try again with the default firmware */
-				fw_filename = smscore_fw_lkup[mode][type];
-				rc = smscore_load_firmware_from_file(coredev,
-							     fw_filename, NULL);
-
-				if (rc < 0) {
-					sms_warn("error %d loading "
-						 "firmware: %s", rc,
-						 fw_filename);
-					return rc;
-				}
-			}
-			sms_log("firmware download success: %s", fw_filename);
-		} else
-			sms_info("mode %d supported by running "
-				 "firmware", mode);
-
-		buffer = kmalloc(sizeof(struct SmsMsgData_ST) +
-				 SMS_DMA_ALIGNMENT, GFP_KERNEL | GFP_DMA);
-		if (buffer) {
-			struct SmsMsgData_ST *msg =
-				(struct SmsMsgData_ST *)
-					SMS_ALIGN_ADDRESS(buffer);
-
-			SMS_INIT_MSG(&msg->xMsgHeader, MSG_SMS_INIT_DEVICE_REQ,
-				     sizeof(struct SmsMsgData_ST));
-			msg->msgData[0] = mode;
-
-			rc = smscore_sendrequest_and_wait(
-				coredev, msg, msg->xMsgHeader.msgLength,
-				&coredev->init_device_done);
-
-			kfree(buffer);
+							     mode, NULL);
+			if (rc >= 0)
+				sms_info("firmware download success");
 		} else {
-			sms_err("Could not allocate buffer for "
-				"init device message.");
-			rc = -ENOMEM;
+			sms_info("mode %d is already supported by running firmware",
+				 mode);
+		}
+		if (coredev->fw_version >= 0x800) {
+			rc = smscore_init_device(coredev, mode);
+			if (rc < 0)
+				sms_err("device init failed, rc %d.", rc);
 		}
 	} else {
-		if (mode < DEVICE_MODE_DVBT || mode > DEVICE_MODE_DVBT_BDA) {
+		if (mode <= DEVICE_MODE_NONE || mode >= DEVICE_MODE_MAX) {
 			sms_err("invalid mode specified %d", mode);
 			return -EINVAL;
 		}
@@ -900,12 +1391,32 @@ int smscore_set_device_mode(struct smscore_device_t *coredev, int mode)
 	}
 
 	if (rc >= 0) {
+		char *buffer;
 		coredev->mode = mode;
 		coredev->device_flags &= ~SMS_DEVICE_NOT_READY;
+
+		buffer = kmalloc(sizeof(struct sms_msg_data) +
+				 SMS_DMA_ALIGNMENT, GFP_KERNEL | GFP_DMA);
+		if (buffer) {
+			struct sms_msg_data *msg = (struct sms_msg_data *) SMS_ALIGN_ADDRESS(buffer);
+
+			SMS_INIT_MSG(&msg->x_msg_header, MSG_SMS_INIT_DEVICE_REQ,
+				     sizeof(struct sms_msg_data));
+			msg->msg_data[0] = mode;
+
+			rc = smscore_sendrequest_and_wait(
+				coredev, msg, msg->x_msg_header.msg_length,
+				&coredev->init_device_done);
+
+			kfree(buffer);
+		}
 	}
 
 	if (rc < 0)
 		sms_err("return error code %d.", rc);
+	else
+		sms_debug("Success setting device mode.");
+
 	return rc;
 }
 
@@ -971,7 +1482,7 @@ found:
  */
 void smscore_onresponse(struct smscore_device_t *coredev,
 		struct smscore_buffer_t *cb) {
-	struct SmsMsgHdr_ST *phdr = (struct SmsMsgHdr_ST *) ((u8 *) cb->p
+	struct sms_msg_hdr *phdr = (struct sms_msg_hdr *) ((u8 *) cb->p
 			+ cb->offset);
 	struct smscore_client_t *client;
 	int rc = -EBUSY;
@@ -983,7 +1494,7 @@ void smscore_onresponse(struct smscore_device_t *coredev,
 		last_sample_time = time_now;
 
 	if (time_now - last_sample_time > 10000) {
-		sms_debug("\ndata rate %d bytes/secs",
+		sms_debug("data rate %d bytes/secs",
 			  (int)((data_total * 1000) /
 				(time_now - last_sample_time)));
 
@@ -993,14 +1504,14 @@ void smscore_onresponse(struct smscore_device_t *coredev,
 
 	data_total += cb->size;
 	/* Do we need to re-route? */
-	if ((phdr->msgType == MSG_SMS_HO_PER_SLICES_IND) ||
-			(phdr->msgType == MSG_SMS_TRANSMISSION_IND)) {
+	if ((phdr->msg_type == MSG_SMS_HO_PER_SLICES_IND) ||
+			(phdr->msg_type == MSG_SMS_TRANSMISSION_IND)) {
 		if (coredev->mode == DEVICE_MODE_DVBT_BDA)
-			phdr->msgDstId = DVBT_BDA_CONTROL_MSG_ID;
+			phdr->msg_dst_id = DVBT_BDA_CONTROL_MSG_ID;
 	}
 
 
-	client = smscore_find_client(coredev, phdr->msgType, phdr->msgDstId);
+	client = smscore_find_client(coredev, phdr->msg_type, phdr->msg_dst_id);
 
 	/* If no client registered for type & id,
 	 * check for control client where type is not registered */
@@ -1008,57 +1519,75 @@ void smscore_onresponse(struct smscore_device_t *coredev,
 		rc = client->onresponse_handler(client->context, cb);
 
 	if (rc < 0) {
-		switch (phdr->msgType) {
+		switch (phdr->msg_type) {
+		case MSG_SMS_ISDBT_TUNE_RES:
+			break;
+		case MSG_SMS_RF_TUNE_RES:
+			break;
+		case MSG_SMS_SIGNAL_DETECTED_IND:
+			break;
+		case MSG_SMS_NO_SIGNAL_IND:
+			break;
+		case MSG_SMS_SPI_INT_LINE_SET_RES:
+			break;
+		case MSG_SMS_INTERFACE_LOCK_IND:
+			break;
+		case MSG_SMS_INTERFACE_UNLOCK_IND:
+			break;
 		case MSG_SMS_GET_VERSION_EX_RES:
 		{
-			struct SmsVersionRes_ST *ver =
-				(struct SmsVersionRes_ST *) phdr;
-			sms_debug("MSG_SMS_GET_VERSION_EX_RES "
-				  "id %d prots 0x%x ver %d.%d",
-				  ver->FirmwareId, ver->SupportedProtocols,
-				  ver->RomVersionMajor, ver->RomVersionMinor);
-
-			coredev->mode = ver->FirmwareId == 255 ?
-				DEVICE_MODE_NONE : ver->FirmwareId;
-			coredev->modes_supported = ver->SupportedProtocols;
+			struct sms_version_res *ver =
+				(struct sms_version_res *) phdr;
+			sms_debug("Firmware id %d prots 0x%x ver %d.%d",
+				  ver->firmware_id, ver->supported_protocols,
+				  ver->rom_ver_major, ver->rom_ver_minor);
+
+			coredev->mode = ver->firmware_id == 255 ?
+				DEVICE_MODE_NONE : ver->firmware_id;
+			coredev->modes_supported = ver->supported_protocols;
+			coredev->fw_version = ver->rom_ver_major << 8 |
+					      ver->rom_ver_minor;
 
 			complete(&coredev->version_ex_done);
 			break;
 		}
 		case MSG_SMS_INIT_DEVICE_RES:
-			sms_debug("MSG_SMS_INIT_DEVICE_RES");
 			complete(&coredev->init_device_done);
 			break;
 		case MSG_SW_RELOAD_START_RES:
-			sms_debug("MSG_SW_RELOAD_START_RES");
 			complete(&coredev->reload_start_done);
 			break;
+		case MSG_SMS_DATA_VALIDITY_RES:
+		{
+			struct sms_msg_data *validity = (struct sms_msg_data *) phdr;
+
+			sms_err("MSG_SMS_DATA_VALIDITY_RES, checksum = 0x%x",
+				validity->msg_data[0]);
+			complete(&coredev->data_validity_done);
+			break;
+		}
 		case MSG_SMS_DATA_DOWNLOAD_RES:
 			complete(&coredev->data_download_done);
 			break;
 		case MSG_SW_RELOAD_EXEC_RES:
-			sms_debug("MSG_SW_RELOAD_EXEC_RES");
 			break;
 		case MSG_SMS_SWDOWNLOAD_TRIGGER_RES:
-			sms_debug("MSG_SMS_SWDOWNLOAD_TRIGGER_RES");
 			complete(&coredev->trigger_done);
 			break;
 		case MSG_SMS_SLEEP_RESUME_COMP_IND:
 			complete(&coredev->resume_done);
 			break;
 		case MSG_SMS_GPIO_CONFIG_EX_RES:
-			sms_debug("MSG_SMS_GPIO_CONFIG_EX_RES");
 			complete(&coredev->gpio_configuration_done);
 			break;
 		case MSG_SMS_GPIO_SET_LEVEL_RES:
-			sms_debug("MSG_SMS_GPIO_SET_LEVEL_RES");
 			complete(&coredev->gpio_set_level_done);
 			break;
 		case MSG_SMS_GPIO_GET_LEVEL_RES:
 		{
 			u32 *msgdata = (u32 *) phdr;
 			coredev->gpio_get_res = msgdata[1];
-			sms_debug("MSG_SMS_GPIO_GET_LEVEL_RES gpio level %d",
+			sms_debug("gpio level %d",
 					coredev->gpio_get_res);
 			complete(&coredev->gpio_get_level_done);
 			break;
@@ -1070,12 +1599,24 @@ void smscore_onresponse(struct smscore_device_t *coredev,
 			sms_ir_event(coredev,
 				(const char *)
 				((char *)phdr
-				+ sizeof(struct SmsMsgHdr_ST)),
-				(int)phdr->msgLength
-				- sizeof(struct SmsMsgHdr_ST));
+				+ sizeof(struct sms_msg_hdr)),
+				(int)phdr->msg_length
+				- sizeof(struct sms_msg_hdr));
+			break;
+
+		case MSG_SMS_DVBT_BDA_DATA:
+			/*
+			 * It can be received here, if the frontend is
+			 * tuned into a valid channel and the proper firmware
+			 * is loaded. That happens when the module got removed
+			 * and re-inserted, without powering the device off
+			 */
 			break;
 
 		default:
+			sms_debug("message %s(%d) not handled.",
+				  smscore_translate_msg(phdr->msg_type),
+				  phdr->msg_type);
 			break;
 		}
 		smscore_putbuffer(coredev, cb);
@@ -1257,7 +1798,7 @@ int smsclient_sendrequest(struct smscore_client_t *client,
 			  void *buffer, size_t size)
 {
 	struct smscore_device_t *coredev;
-	struct SmsMsgHdr_ST *phdr = (struct SmsMsgHdr_ST *) buffer;
+	struct sms_msg_hdr *phdr = (struct sms_msg_hdr *) buffer;
 	int rc;
 
 	if (client == NULL) {
@@ -1274,7 +1815,7 @@ int smsclient_sendrequest(struct smscore_client_t *client,
 	}
 
 	rc = smscore_validate_client(client->coredev, client, 0,
-				     phdr->msgSrcId);
+				     phdr->msg_src_id);
 	if (rc < 0)
 		return rc;
 
@@ -1288,16 +1829,16 @@ int smscore_configure_gpio(struct smscore_device_t *coredev, u32 pin,
 			   struct smscore_config_gpio *pinconfig)
 {
 	struct {
-		struct SmsMsgHdr_ST hdr;
+		struct sms_msg_hdr hdr;
 		u32 data[6];
 	} msg;
 
 	if (coredev->device_flags & SMS_DEVICE_FAMILY2) {
-		msg.hdr.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-		msg.hdr.msgDstId = HIF_TASK;
-		msg.hdr.msgFlags = 0;
-		msg.hdr.msgType  = MSG_SMS_GPIO_CONFIG_EX_REQ;
-		msg.hdr.msgLength = sizeof(msg);
+		msg.hdr.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+		msg.hdr.msg_dst_id = HIF_TASK;
+		msg.hdr.msg_flags = 0;
+		msg.hdr.msg_type  = MSG_SMS_GPIO_CONFIG_EX_REQ;
+		msg.hdr.msg_length = sizeof(msg);
 
 		msg.data[0] = pin;
 		msg.data[1] = pinconfig->pullupdown;
@@ -1306,16 +1847,16 @@ int smscore_configure_gpio(struct smscore_device_t *coredev, u32 pin,
 		msg.data[2] = pinconfig->outputslewrate == 0 ? 3 : 0;
 
 		switch (pinconfig->outputdriving) {
-		case SMS_GPIO_OUTPUTDRIVING_16mA:
+		case SMS_GPIO_OUTPUTDRIVING_S_16mA:
 			msg.data[3] = 7; /* Nova - 16mA */
 			break;
-		case SMS_GPIO_OUTPUTDRIVING_12mA:
+		case SMS_GPIO_OUTPUTDRIVING_S_12mA:
 			msg.data[3] = 5; /* Nova - 11mA */
 			break;
-		case SMS_GPIO_OUTPUTDRIVING_8mA:
+		case SMS_GPIO_OUTPUTDRIVING_S_8mA:
 			msg.data[3] = 3; /* Nova - 7mA */
 			break;
-		case SMS_GPIO_OUTPUTDRIVING_4mA:
+		case SMS_GPIO_OUTPUTDRIVING_S_4mA:
 		default:
 			msg.data[3] = 2; /* Nova - 4mA */
 			break;
@@ -1333,18 +1874,18 @@ int smscore_configure_gpio(struct smscore_device_t *coredev, u32 pin,
 int smscore_set_gpio(struct smscore_device_t *coredev, u32 pin, int level)
 {
 	struct {
-		struct SmsMsgHdr_ST hdr;
+		struct sms_msg_hdr hdr;
 		u32 data[3];
 	} msg;
 
 	if (pin > MAX_GPIO_PIN_NUMBER)
 		return -EINVAL;
 
-	msg.hdr.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-	msg.hdr.msgDstId = HIF_TASK;
-	msg.hdr.msgFlags = 0;
-	msg.hdr.msgType  = MSG_SMS_GPIO_SET_LEVEL_REQ;
-	msg.hdr.msgLength = sizeof(msg);
+	msg.hdr.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	msg.hdr.msg_dst_id = HIF_TASK;
+	msg.hdr.msg_flags = 0;
+	msg.hdr.msg_type  = MSG_SMS_GPIO_SET_LEVEL_REQ;
+	msg.hdr.msg_length = sizeof(msg);
 
 	msg.data[0] = pin;
 	msg.data[1] = level ? 1 : 0;
@@ -1355,122 +1896,121 @@ int smscore_set_gpio(struct smscore_device_t *coredev, u32 pin, int level)
 }
 
 /* new GPIO management implementation */
-static int GetGpioPinParams(u32 PinNum, u32 *pTranslatedPinNum,
-		u32 *pGroupNum, u32 *pGroupCfg) {
-
-	*pGroupCfg = 1;
-
-	if (PinNum <= 1)	{
-		*pTranslatedPinNum = 0;
-		*pGroupNum = 9;
-		*pGroupCfg = 2;
-	} else if (PinNum >= 2 && PinNum <= 6) {
-		*pTranslatedPinNum = 2;
-		*pGroupNum = 0;
-		*pGroupCfg = 2;
-	} else if (PinNum >= 7 && PinNum <= 11) {
-		*pTranslatedPinNum = 7;
-		*pGroupNum = 1;
-	} else if (PinNum >= 12 && PinNum <= 15) {
-		*pTranslatedPinNum = 12;
-		*pGroupNum = 2;
-		*pGroupCfg = 3;
-	} else if (PinNum == 16) {
-		*pTranslatedPinNum = 16;
-		*pGroupNum = 23;
-	} else if (PinNum >= 17 && PinNum <= 24) {
-		*pTranslatedPinNum = 17;
-		*pGroupNum = 3;
-	} else if (PinNum == 25) {
-		*pTranslatedPinNum = 25;
-		*pGroupNum = 6;
-	} else if (PinNum >= 26 && PinNum <= 28) {
-		*pTranslatedPinNum = 26;
-		*pGroupNum = 4;
-	} else if (PinNum == 29) {
-		*pTranslatedPinNum = 29;
-		*pGroupNum = 5;
-		*pGroupCfg = 2;
-	} else if (PinNum == 30) {
-		*pTranslatedPinNum = 30;
-		*pGroupNum = 8;
-	} else if (PinNum == 31) {
-		*pTranslatedPinNum = 31;
-		*pGroupNum = 17;
+static int get_gpio_pin_params(u32 pin_num, u32 *p_translatedpin_num,
+		u32 *p_group_num, u32 *p_group_cfg) {
+
+	*p_group_cfg = 1;
+
+	if (pin_num <= 1)	{
+		*p_translatedpin_num = 0;
+		*p_group_num = 9;
+		*p_group_cfg = 2;
+	} else if (pin_num >= 2 && pin_num <= 6) {
+		*p_translatedpin_num = 2;
+		*p_group_num = 0;
+		*p_group_cfg = 2;
+	} else if (pin_num >= 7 && pin_num <= 11) {
+		*p_translatedpin_num = 7;
+		*p_group_num = 1;
+	} else if (pin_num >= 12 && pin_num <= 15) {
+		*p_translatedpin_num = 12;
+		*p_group_num = 2;
+		*p_group_cfg = 3;
+	} else if (pin_num == 16) {
+		*p_translatedpin_num = 16;
+		*p_group_num = 23;
+	} else if (pin_num >= 17 && pin_num <= 24) {
+		*p_translatedpin_num = 17;
+		*p_group_num = 3;
+	} else if (pin_num == 25) {
+		*p_translatedpin_num = 25;
+		*p_group_num = 6;
+	} else if (pin_num >= 26 && pin_num <= 28) {
+		*p_translatedpin_num = 26;
+		*p_group_num = 4;
+	} else if (pin_num == 29) {
+		*p_translatedpin_num = 29;
+		*p_group_num = 5;
+		*p_group_cfg = 2;
+	} else if (pin_num == 30) {
+		*p_translatedpin_num = 30;
+		*p_group_num = 8;
+	} else if (pin_num == 31) {
+		*p_translatedpin_num = 31;
+		*p_group_num = 17;
 	} else
 		return -1;
 
-	*pGroupCfg <<= 24;
+	*p_group_cfg <<= 24;
 
 	return 0;
 }
 
-int smscore_gpio_configure(struct smscore_device_t *coredev, u8 PinNum,
-		struct smscore_gpio_config *pGpioConfig) {
+int smscore_gpio_configure(struct smscore_device_t *coredev, u8 pin_num,
+		struct smscore_config_gpio *p_gpio_config) {
 
-	u32 totalLen;
-	u32 TranslatedPinNum = 0;
-	u32 GroupNum = 0;
-	u32 ElectricChar;
-	u32 groupCfg;
+	u32 total_len;
+	u32 translatedpin_num = 0;
+	u32 group_num = 0;
+	u32 electric_char;
+	u32 group_cfg;
 	void *buffer;
 	int rc;
 
-	struct SetGpioMsg {
-		struct SmsMsgHdr_ST xMsgHeader;
-		u32 msgData[6];
-	} *pMsg;
+	struct set_gpio_msg {
+		struct sms_msg_hdr x_msg_header;
+		u32 msg_data[6];
+	} *p_msg;
 
 
-	if (PinNum > MAX_GPIO_PIN_NUMBER)
+	if (pin_num > MAX_GPIO_PIN_NUMBER)
 		return -EINVAL;
 
-	if (pGpioConfig == NULL)
+	if (p_gpio_config == NULL)
 		return -EINVAL;
 
-	totalLen = sizeof(struct SmsMsgHdr_ST) + (sizeof(u32) * 6);
+	total_len = sizeof(struct sms_msg_hdr) + (sizeof(u32) * 6);
 
-	buffer = kmalloc(totalLen + SMS_DMA_ALIGNMENT,
+	buffer = kmalloc(total_len + SMS_DMA_ALIGNMENT,
 			GFP_KERNEL | GFP_DMA);
 	if (!buffer)
 		return -ENOMEM;
 
-	pMsg = (struct SetGpioMsg *) SMS_ALIGN_ADDRESS(buffer);
+	p_msg = (struct set_gpio_msg *) SMS_ALIGN_ADDRESS(buffer);
 
-	pMsg->xMsgHeader.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-	pMsg->xMsgHeader.msgDstId = HIF_TASK;
-	pMsg->xMsgHeader.msgFlags = 0;
-	pMsg->xMsgHeader.msgLength = (u16) totalLen;
-	pMsg->msgData[0] = PinNum;
+	p_msg->x_msg_header.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	p_msg->x_msg_header.msg_dst_id = HIF_TASK;
+	p_msg->x_msg_header.msg_flags = 0;
+	p_msg->x_msg_header.msg_length = (u16) total_len;
+	p_msg->msg_data[0] = pin_num;
 
 	if (!(coredev->device_flags & SMS_DEVICE_FAMILY2)) {
-		pMsg->xMsgHeader.msgType = MSG_SMS_GPIO_CONFIG_REQ;
-		if (GetGpioPinParams(PinNum, &TranslatedPinNum, &GroupNum,
-				&groupCfg) != 0) {
+		p_msg->x_msg_header.msg_type = MSG_SMS_GPIO_CONFIG_REQ;
+		if (get_gpio_pin_params(pin_num, &translatedpin_num, &group_num,
+				&group_cfg) != 0) {
 			rc = -EINVAL;
 			goto free;
 		}
 
-		pMsg->msgData[1] = TranslatedPinNum;
-		pMsg->msgData[2] = GroupNum;
-		ElectricChar = (pGpioConfig->PullUpDown)
-				| (pGpioConfig->InputCharacteristics << 2)
-				| (pGpioConfig->OutputSlewRate << 3)
-				| (pGpioConfig->OutputDriving << 4);
-		pMsg->msgData[3] = ElectricChar;
-		pMsg->msgData[4] = pGpioConfig->Direction;
-		pMsg->msgData[5] = groupCfg;
+		p_msg->msg_data[1] = translatedpin_num;
+		p_msg->msg_data[2] = group_num;
+		electric_char = (p_gpio_config->pullupdown)
+				| (p_gpio_config->inputcharacteristics << 2)
+				| (p_gpio_config->outputslewrate << 3)
+				| (p_gpio_config->outputdriving << 4);
+		p_msg->msg_data[3] = electric_char;
+		p_msg->msg_data[4] = p_gpio_config->direction;
+		p_msg->msg_data[5] = group_cfg;
 	} else {
-		pMsg->xMsgHeader.msgType = MSG_SMS_GPIO_CONFIG_EX_REQ;
-		pMsg->msgData[1] = pGpioConfig->PullUpDown;
-		pMsg->msgData[2] = pGpioConfig->OutputSlewRate;
-		pMsg->msgData[3] = pGpioConfig->OutputDriving;
-		pMsg->msgData[4] = pGpioConfig->Direction;
-		pMsg->msgData[5] = 0;
+		p_msg->x_msg_header.msg_type = MSG_SMS_GPIO_CONFIG_EX_REQ;
+		p_msg->msg_data[1] = p_gpio_config->pullupdown;
+		p_msg->msg_data[2] = p_gpio_config->outputslewrate;
+		p_msg->msg_data[3] = p_gpio_config->outputdriving;
+		p_msg->msg_data[4] = p_gpio_config->direction;
+		p_msg->msg_data[5] = 0;
 	}
 
-	smsendian_handle_tx_message((struct SmsMsgHdr_ST *)pMsg);
-	rc = smscore_sendrequest_and_wait(coredev, pMsg, totalLen,
+	rc = smscore_sendrequest_and_wait(coredev, p_msg, total_len,
 			&coredev->gpio_configuration_done);
 
 	if (rc != 0) {
@@ -1485,42 +2025,41 @@ free:
 	return rc;
 }
 
-int smscore_gpio_set_level(struct smscore_device_t *coredev, u8 PinNum,
-		u8 NewLevel) {
+int smscore_gpio_set_level(struct smscore_device_t *coredev, u8 pin_num,
+		u8 new_level) {
 
-	u32 totalLen;
+	u32 total_len;
 	int rc;
 	void *buffer;
 
-	struct SetGpioMsg {
-		struct SmsMsgHdr_ST xMsgHeader;
-		u32 msgData[3]; /* keep it 3 ! */
-	} *pMsg;
+	struct set_gpio_msg {
+		struct sms_msg_hdr x_msg_header;
+		u32 msg_data[3]; /* keep it 3 ! */
+	} *p_msg;
 
-	if ((NewLevel > 1) || (PinNum > MAX_GPIO_PIN_NUMBER))
+	if ((new_level > 1) || (pin_num > MAX_GPIO_PIN_NUMBER))
 		return -EINVAL;
 
-	totalLen = sizeof(struct SmsMsgHdr_ST) +
+	total_len = sizeof(struct sms_msg_hdr) +
 			(3 * sizeof(u32)); /* keep it 3 ! */
 
-	buffer = kmalloc(totalLen + SMS_DMA_ALIGNMENT,
+	buffer = kmalloc(total_len + SMS_DMA_ALIGNMENT,
 			GFP_KERNEL | GFP_DMA);
 	if (!buffer)
 		return -ENOMEM;
 
-	pMsg = (struct SetGpioMsg *) SMS_ALIGN_ADDRESS(buffer);
+	p_msg = (struct set_gpio_msg *) SMS_ALIGN_ADDRESS(buffer);
 
-	pMsg->xMsgHeader.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-	pMsg->xMsgHeader.msgDstId = HIF_TASK;
-	pMsg->xMsgHeader.msgFlags = 0;
-	pMsg->xMsgHeader.msgType = MSG_SMS_GPIO_SET_LEVEL_REQ;
-	pMsg->xMsgHeader.msgLength = (u16) totalLen;
-	pMsg->msgData[0] = PinNum;
-	pMsg->msgData[1] = NewLevel;
+	p_msg->x_msg_header.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	p_msg->x_msg_header.msg_dst_id = HIF_TASK;
+	p_msg->x_msg_header.msg_flags = 0;
+	p_msg->x_msg_header.msg_type = MSG_SMS_GPIO_SET_LEVEL_REQ;
+	p_msg->x_msg_header.msg_length = (u16) total_len;
+	p_msg->msg_data[0] = pin_num;
+	p_msg->msg_data[1] = new_level;
 
 	/* Send message to SMS */
-	smsendian_handle_tx_message((struct SmsMsgHdr_ST *)pMsg);
-	rc = smscore_sendrequest_and_wait(coredev, pMsg, totalLen,
+	rc = smscore_sendrequest_and_wait(coredev, p_msg, total_len,
 			&coredev->gpio_set_level_done);
 
 	if (rc != 0) {
@@ -1534,42 +2073,41 @@ int smscore_gpio_set_level(struct smscore_device_t *coredev, u8 PinNum,
 	return rc;
 }
 
-int smscore_gpio_get_level(struct smscore_device_t *coredev, u8 PinNum,
+int smscore_gpio_get_level(struct smscore_device_t *coredev, u8 pin_num,
 		u8 *level) {
 
-	u32 totalLen;
+	u32 total_len;
 	int rc;
 	void *buffer;
 
-	struct SetGpioMsg {
-		struct SmsMsgHdr_ST xMsgHeader;
-		u32 msgData[2];
-	} *pMsg;
+	struct set_gpio_msg {
+		struct sms_msg_hdr x_msg_header;
+		u32 msg_data[2];
+	} *p_msg;
 
 
-	if (PinNum > MAX_GPIO_PIN_NUMBER)
+	if (pin_num > MAX_GPIO_PIN_NUMBER)
 		return -EINVAL;
 
-	totalLen = sizeof(struct SmsMsgHdr_ST) + (2 * sizeof(u32));
+	total_len = sizeof(struct sms_msg_hdr) + (2 * sizeof(u32));
 
-	buffer = kmalloc(totalLen + SMS_DMA_ALIGNMENT,
+	buffer = kmalloc(total_len + SMS_DMA_ALIGNMENT,
 			GFP_KERNEL | GFP_DMA);
 	if (!buffer)
 		return -ENOMEM;
 
-	pMsg = (struct SetGpioMsg *) SMS_ALIGN_ADDRESS(buffer);
+	p_msg = (struct set_gpio_msg *) SMS_ALIGN_ADDRESS(buffer);
 
-	pMsg->xMsgHeader.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-	pMsg->xMsgHeader.msgDstId = HIF_TASK;
-	pMsg->xMsgHeader.msgFlags = 0;
-	pMsg->xMsgHeader.msgType = MSG_SMS_GPIO_GET_LEVEL_REQ;
-	pMsg->xMsgHeader.msgLength = (u16) totalLen;
-	pMsg->msgData[0] = PinNum;
-	pMsg->msgData[1] = 0;
+	p_msg->x_msg_header.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	p_msg->x_msg_header.msg_dst_id = HIF_TASK;
+	p_msg->x_msg_header.msg_flags = 0;
+	p_msg->x_msg_header.msg_type = MSG_SMS_GPIO_GET_LEVEL_REQ;
+	p_msg->x_msg_header.msg_length = (u16) total_len;
+	p_msg->msg_data[0] = pin_num;
+	p_msg->msg_data[1] = 0;
 
 	/* Send message to SMS */
-	smsendian_handle_tx_message((struct SmsMsgHdr_ST *)pMsg);
-	rc = smscore_sendrequest_and_wait(coredev, pMsg, totalLen,
+	rc = smscore_sendrequest_and_wait(coredev, p_msg, total_len,
 			&coredev->gpio_get_level_done);
 
 	if (rc != 0) {
@@ -1635,3 +2173,27 @@ module_exit(smscore_module_exit);
 MODULE_DESCRIPTION("Siano MDTV Core module");
 MODULE_AUTHOR("Siano Mobile Silicon, Inc. (uris@siano-ms.com)");
 MODULE_LICENSE("GPL");
+
+/* This should match what's defined at smscoreapi.h */
+MODULE_FIRMWARE(SMS_FW_ATSC_DENVER);
+MODULE_FIRMWARE(SMS_FW_CMMB_MING_APP);
+MODULE_FIRMWARE(SMS_FW_CMMB_VEGA_12MHZ);
+MODULE_FIRMWARE(SMS_FW_CMMB_VENICE_12MHZ);
+MODULE_FIRMWARE(SMS_FW_DVBH_RIO);
+MODULE_FIRMWARE(SMS_FW_DVB_NOVA_12MHZ_B0);
+MODULE_FIRMWARE(SMS_FW_DVB_NOVA_12MHZ);
+MODULE_FIRMWARE(SMS_FW_DVB_RIO);
+MODULE_FIRMWARE(SMS_FW_FM_RADIO);
+MODULE_FIRMWARE(SMS_FW_FM_RADIO_RIO);
+MODULE_FIRMWARE(SMS_FW_DVBT_HCW_55XXX);
+MODULE_FIRMWARE(SMS_FW_ISDBT_HCW_55XXX);
+MODULE_FIRMWARE(SMS_FW_ISDBT_NOVA_12MHZ_B0);
+MODULE_FIRMWARE(SMS_FW_ISDBT_NOVA_12MHZ);
+MODULE_FIRMWARE(SMS_FW_ISDBT_PELE);
+MODULE_FIRMWARE(SMS_FW_ISDBT_RIO);
+MODULE_FIRMWARE(SMS_FW_DVBT_NOVA_A);
+MODULE_FIRMWARE(SMS_FW_DVBT_NOVA_B);
+MODULE_FIRMWARE(SMS_FW_DVBT_STELLAR);
+MODULE_FIRMWARE(SMS_FW_TDMB_DENVER);
+MODULE_FIRMWARE(SMS_FW_TDMB_NOVA_12MHZ_B0);
+MODULE_FIRMWARE(SMS_FW_TDMB_NOVA_12MHZ);
diff --git a/drivers/media/common/siano/smscoreapi.h b/drivers/media/common/siano/smscoreapi.h
index c592ae090397..d0799e323364 100644
--- a/drivers/media/common/siano/smscoreapi.h
+++ b/drivers/media/common/siano/smscoreapi.h
@@ -40,9 +40,33 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define kmutex_trylock(_p_) mutex_trylock(_p_)
 #define kmutex_unlock(_p_) mutex_unlock(_p_)
 
-#ifndef min
-#define min(a, b) (((a) < (b)) ? (a) : (b))
-#endif
+/*
+ * Define the firmware names used by the driver.
+ * Those should match what's used at smscoreapi.c and sms-cards.c
+ * including the MODULE_FIRMWARE() macros at the end of smscoreapi.c
+ */
+#define SMS_FW_ATSC_DENVER         "atsc_denver.inp"
+#define SMS_FW_CMMB_MING_APP       "cmmb_ming_app.inp"
+#define SMS_FW_CMMB_VEGA_12MHZ     "cmmb_vega_12mhz.inp"
+#define SMS_FW_CMMB_VENICE_12MHZ   "cmmb_venice_12mhz.inp"
+#define SMS_FW_DVBH_RIO            "dvbh_rio.inp"
+#define SMS_FW_DVB_NOVA_12MHZ_B0   "dvb_nova_12mhz_b0.inp"
+#define SMS_FW_DVB_NOVA_12MHZ      "dvb_nova_12mhz.inp"
+#define SMS_FW_DVB_RIO             "dvb_rio.inp"
+#define SMS_FW_FM_RADIO            "fm_radio.inp"
+#define SMS_FW_FM_RADIO_RIO        "fm_radio_rio.inp"
+#define SMS_FW_DVBT_HCW_55XXX      "sms1xxx-hcw-55xxx-dvbt-02.fw"
+#define SMS_FW_ISDBT_HCW_55XXX     "sms1xxx-hcw-55xxx-isdbt-02.fw"
+#define SMS_FW_ISDBT_NOVA_12MHZ_B0 "isdbt_nova_12mhz_b0.inp"
+#define SMS_FW_ISDBT_NOVA_12MHZ    "isdbt_nova_12mhz.inp"
+#define SMS_FW_ISDBT_PELE          "isdbt_pele.inp"
+#define SMS_FW_ISDBT_RIO           "isdbt_rio.inp"
+#define SMS_FW_DVBT_NOVA_A         "sms1xxx-nova-a-dvbt-01.fw"
+#define SMS_FW_DVBT_NOVA_B         "sms1xxx-nova-b-dvbt-01.fw"
+#define SMS_FW_DVBT_STELLAR        "sms1xxx-stellar-dvbt-01.fw"
+#define SMS_FW_TDMB_DENVER         "tdmb_denver.inp"
+#define SMS_FW_TDMB_NOVA_12MHZ_B0  "tdmb_nova_12mhz_b0.inp"
+#define SMS_FW_TDMB_NOVA_12MHZ     "tdmb_nova_12mhz.inp"
 
 #define SMS_PROTOCOL_MAX_RAOUNDTRIP_MS			(10000)
 #define SMS_ALLOC_ALIGNMENT				128
@@ -50,18 +74,31 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define SMS_ALIGN_ADDRESS(addr) \
 	((((uintptr_t)(addr)) + (SMS_DMA_ALIGNMENT-1)) & ~(SMS_DMA_ALIGNMENT-1))
 
+#define SMS_DEVICE_FAMILY1				0
 #define SMS_DEVICE_FAMILY2				1
 #define SMS_ROM_NO_RESPONSE				2
 #define SMS_DEVICE_NOT_READY				0x8000000
 
 enum sms_device_type_st {
+	SMS_UNKNOWN_TYPE = -1,
 	SMS_STELLAR = 0,
 	SMS_NOVA_A0,
 	SMS_NOVA_B0,
 	SMS_VEGA,
+	SMS_VENICE,
+	SMS_MING,
+	SMS_PELE,
+	SMS_RIO,
+	SMS_DENVER_1530,
+	SMS_DENVER_2160,
 	SMS_NUM_OF_DEVICE_TYPES
 };
 
+enum sms_power_mode_st {
+	SMS_POWER_MODE_ACTIVE,
+	SMS_POWER_MODE_SUSPENDED
+};
+
 struct smscore_device_t;
 struct smscore_client_t;
 struct smscore_buffer_t;
@@ -149,6 +186,7 @@ struct smscore_device_t {
 
 	/* host <--> device messages */
 	struct completion version_ex_done, data_download_done, trigger_done;
+	struct completion data_validity_done, device_ready_done;
 	struct completion init_device_done, reload_start_done, resume_done;
 	struct completion gpio_configuration_done, gpio_set_level_done;
 	struct completion gpio_get_level_done, ir_init_done;
@@ -165,10 +203,17 @@ struct smscore_device_t {
 	/* Firmware */
 	u8 *fw_buf;
 	u32 fw_buf_size;
+	u16 fw_version;
 
 	/* Infrared (IR) */
 	struct ir_t ir;
 
+	/*
+	 * Identify if device is USB or not.
+	 * Used by smsdvb-sysfs to know the root node for debugfs
+	 */
+	bool is_usb_device;
+
 	int led_state;
 };
 
@@ -176,81 +221,363 @@ struct smscore_device_t {
 #define SMS_ANTENNA_GPIO_0					1
 #define SMS_ANTENNA_GPIO_1					0
 
-#define BW_8_MHZ							0
-#define BW_7_MHZ							1
-#define BW_6_MHZ							2
-#define BW_5_MHZ							3
-#define BW_ISDBT_1SEG						4
-#define BW_ISDBT_3SEG						5
+enum sms_bandwidth_mode {
+	BW_8_MHZ = 0,
+	BW_7_MHZ = 1,
+	BW_6_MHZ = 2,
+	BW_5_MHZ = 3,
+	BW_ISDBT_1SEG = 4,
+	BW_ISDBT_3SEG = 5,
+	BW_2_MHZ = 6,
+	BW_FM_RADIO = 7,
+	BW_ISDBT_13SEG = 8,
+	BW_1_5_MHZ = 15,
+	BW_UNKNOWN = 0xffff
+};
+
 
 #define MSG_HDR_FLAG_SPLIT_MSG				4
 
 #define MAX_GPIO_PIN_NUMBER					31
 
 #define HIF_TASK							11
+#define HIF_TASK_SLAVE					22
+#define HIF_TASK_SLAVE2					33
+#define HIF_TASK_SLAVE3					44
 #define SMS_HOST_LIB						150
 #define DVBT_BDA_CONTROL_MSG_ID				201
 
 #define SMS_MAX_PAYLOAD_SIZE				240
 #define SMS_TUNE_TIMEOUT					500
 
-#define MSG_SMS_GPIO_CONFIG_REQ				507
-#define MSG_SMS_GPIO_CONFIG_RES				508
-#define MSG_SMS_GPIO_SET_LEVEL_REQ			509
-#define MSG_SMS_GPIO_SET_LEVEL_RES			510
-#define MSG_SMS_GPIO_GET_LEVEL_REQ			511
-#define MSG_SMS_GPIO_GET_LEVEL_RES			512
-#define MSG_SMS_RF_TUNE_REQ					561
-#define MSG_SMS_RF_TUNE_RES					562
-#define MSG_SMS_INIT_DEVICE_REQ				578
-#define MSG_SMS_INIT_DEVICE_RES				579
-#define MSG_SMS_ADD_PID_FILTER_REQ			601
-#define MSG_SMS_ADD_PID_FILTER_RES			602
-#define MSG_SMS_REMOVE_PID_FILTER_REQ			603
-#define MSG_SMS_REMOVE_PID_FILTER_RES			604
-#define MSG_SMS_DAB_CHANNEL				607
-#define MSG_SMS_GET_PID_FILTER_LIST_REQ			608
-#define MSG_SMS_GET_PID_FILTER_LIST_RES			609
-#define MSG_SMS_GET_STATISTICS_RES			616
-#define MSG_SMS_GET_STATISTICS_REQ			615
-#define MSG_SMS_HO_PER_SLICES_IND			630
-#define MSG_SMS_SET_ANTENNA_CONFIG_REQ			651
-#define MSG_SMS_SET_ANTENNA_CONFIG_RES			652
-#define MSG_SMS_SLEEP_RESUME_COMP_IND			655
-#define MSG_SMS_DATA_DOWNLOAD_REQ			660
-#define MSG_SMS_DATA_DOWNLOAD_RES			661
-#define MSG_SMS_SWDOWNLOAD_TRIGGER_REQ		664
-#define MSG_SMS_SWDOWNLOAD_TRIGGER_RES		665
-#define MSG_SMS_SWDOWNLOAD_BACKDOOR_REQ		666
-#define MSG_SMS_SWDOWNLOAD_BACKDOOR_RES		667
-#define MSG_SMS_GET_VERSION_EX_REQ			668
-#define MSG_SMS_GET_VERSION_EX_RES			669
-#define MSG_SMS_SET_CLOCK_OUTPUT_REQ		670
-#define MSG_SMS_I2C_SET_FREQ_REQ			685
-#define MSG_SMS_GENERIC_I2C_REQ				687
-#define MSG_SMS_GENERIC_I2C_RES				688
-#define MSG_SMS_DVBT_BDA_DATA				693
-#define MSG_SW_RELOAD_REQ					697
-#define MSG_SMS_DATA_MSG					699
-#define MSG_SW_RELOAD_START_REQ				702
-#define MSG_SW_RELOAD_START_RES				703
-#define MSG_SW_RELOAD_EXEC_REQ				704
-#define MSG_SW_RELOAD_EXEC_RES				705
-#define MSG_SMS_SPI_INT_LINE_SET_REQ		710
-#define MSG_SMS_GPIO_CONFIG_EX_REQ			712
-#define MSG_SMS_GPIO_CONFIG_EX_RES			713
-#define MSG_SMS_ISDBT_TUNE_REQ				776
-#define MSG_SMS_ISDBT_TUNE_RES				777
-#define MSG_SMS_TRANSMISSION_IND			782
-#define MSG_SMS_START_IR_REQ				800
-#define MSG_SMS_START_IR_RES				801
-#define MSG_SMS_IR_SAMPLES_IND				802
-#define MSG_SMS_SIGNAL_DETECTED_IND			827
-#define MSG_SMS_NO_SIGNAL_IND				828
+enum msg_types {
+	MSG_TYPE_BASE_VAL = 500,
+	MSG_SMS_GET_VERSION_REQ = 503,
+	MSG_SMS_GET_VERSION_RES = 504,
+	MSG_SMS_MULTI_BRIDGE_CFG = 505,
+	MSG_SMS_GPIO_CONFIG_REQ = 507,
+	MSG_SMS_GPIO_CONFIG_RES = 508,
+	MSG_SMS_GPIO_SET_LEVEL_REQ = 509,
+	MSG_SMS_GPIO_SET_LEVEL_RES = 510,
+	MSG_SMS_GPIO_GET_LEVEL_REQ = 511,
+	MSG_SMS_GPIO_GET_LEVEL_RES = 512,
+	MSG_SMS_EEPROM_BURN_IND = 513,
+	MSG_SMS_LOG_ENABLE_CHANGE_REQ = 514,
+	MSG_SMS_LOG_ENABLE_CHANGE_RES = 515,
+	MSG_SMS_SET_MAX_TX_MSG_LEN_REQ = 516,
+	MSG_SMS_SET_MAX_TX_MSG_LEN_RES = 517,
+	MSG_SMS_SPI_HALFDUPLEX_TOKEN_HOST_TO_DEVICE = 518,
+	MSG_SMS_SPI_HALFDUPLEX_TOKEN_DEVICE_TO_HOST = 519,
+	MSG_SMS_BACKGROUND_SCAN_FLAG_CHANGE_REQ = 520,
+	MSG_SMS_BACKGROUND_SCAN_FLAG_CHANGE_RES = 521,
+	MSG_SMS_BACKGROUND_SCAN_SIGNAL_DETECTED_IND = 522,
+	MSG_SMS_BACKGROUND_SCAN_NO_SIGNAL_IND = 523,
+	MSG_SMS_CONFIGURE_RF_SWITCH_REQ = 524,
+	MSG_SMS_CONFIGURE_RF_SWITCH_RES = 525,
+	MSG_SMS_MRC_PATH_DISCONNECT_REQ = 526,
+	MSG_SMS_MRC_PATH_DISCONNECT_RES = 527,
+	MSG_SMS_RECEIVE_1SEG_THROUGH_FULLSEG_REQ = 528,
+	MSG_SMS_RECEIVE_1SEG_THROUGH_FULLSEG_RES = 529,
+	MSG_SMS_RECEIVE_VHF_VIA_VHF_INPUT_REQ = 530,
+	MSG_SMS_RECEIVE_VHF_VIA_VHF_INPUT_RES = 531,
+	MSG_WR_REG_RFT_REQ = 533,
+	MSG_WR_REG_RFT_RES = 534,
+	MSG_RD_REG_RFT_REQ = 535,
+	MSG_RD_REG_RFT_RES = 536,
+	MSG_RD_REG_ALL_RFT_REQ = 537,
+	MSG_RD_REG_ALL_RFT_RES = 538,
+	MSG_HELP_INT = 539,
+	MSG_RUN_SCRIPT_INT = 540,
+	MSG_SMS_EWS_INBAND_REQ = 541,
+	MSG_SMS_EWS_INBAND_RES = 542,
+	MSG_SMS_RFS_SELECT_REQ = 543,
+	MSG_SMS_RFS_SELECT_RES = 544,
+	MSG_SMS_MB_GET_VER_REQ = 545,
+	MSG_SMS_MB_GET_VER_RES = 546,
+	MSG_SMS_MB_WRITE_CFGFILE_REQ = 547,
+	MSG_SMS_MB_WRITE_CFGFILE_RES = 548,
+	MSG_SMS_MB_READ_CFGFILE_REQ = 549,
+	MSG_SMS_MB_READ_CFGFILE_RES = 550,
+	MSG_SMS_RD_MEM_REQ = 552,
+	MSG_SMS_RD_MEM_RES = 553,
+	MSG_SMS_WR_MEM_REQ = 554,
+	MSG_SMS_WR_MEM_RES = 555,
+	MSG_SMS_UPDATE_MEM_REQ = 556,
+	MSG_SMS_UPDATE_MEM_RES = 557,
+	MSG_SMS_ISDBT_ENABLE_FULL_PARAMS_SET_REQ = 558,
+	MSG_SMS_ISDBT_ENABLE_FULL_PARAMS_SET_RES = 559,
+	MSG_SMS_RF_TUNE_REQ = 561,
+	MSG_SMS_RF_TUNE_RES = 562,
+	MSG_SMS_ISDBT_ENABLE_HIGH_MOBILITY_REQ = 563,
+	MSG_SMS_ISDBT_ENABLE_HIGH_MOBILITY_RES = 564,
+	MSG_SMS_ISDBT_SB_RECEPTION_REQ = 565,
+	MSG_SMS_ISDBT_SB_RECEPTION_RES = 566,
+	MSG_SMS_GENERIC_EPROM_WRITE_REQ = 567,
+	MSG_SMS_GENERIC_EPROM_WRITE_RES = 568,
+	MSG_SMS_GENERIC_EPROM_READ_REQ = 569,
+	MSG_SMS_GENERIC_EPROM_READ_RES = 570,
+	MSG_SMS_EEPROM_WRITE_REQ = 571,
+	MSG_SMS_EEPROM_WRITE_RES = 572,
+	MSG_SMS_CUSTOM_READ_REQ = 574,
+	MSG_SMS_CUSTOM_READ_RES = 575,
+	MSG_SMS_CUSTOM_WRITE_REQ = 576,
+	MSG_SMS_CUSTOM_WRITE_RES = 577,
+	MSG_SMS_INIT_DEVICE_REQ = 578,
+	MSG_SMS_INIT_DEVICE_RES = 579,
+	MSG_SMS_ATSC_SET_ALL_IP_REQ = 580,
+	MSG_SMS_ATSC_SET_ALL_IP_RES = 581,
+	MSG_SMS_ATSC_START_ENSEMBLE_REQ = 582,
+	MSG_SMS_ATSC_START_ENSEMBLE_RES = 583,
+	MSG_SMS_SET_OUTPUT_MODE_REQ = 584,
+	MSG_SMS_SET_OUTPUT_MODE_RES = 585,
+	MSG_SMS_ATSC_IP_FILTER_GET_LIST_REQ = 586,
+	MSG_SMS_ATSC_IP_FILTER_GET_LIST_RES = 587,
+	MSG_SMS_SUB_CHANNEL_START_REQ = 589,
+	MSG_SMS_SUB_CHANNEL_START_RES = 590,
+	MSG_SMS_SUB_CHANNEL_STOP_REQ = 591,
+	MSG_SMS_SUB_CHANNEL_STOP_RES = 592,
+	MSG_SMS_ATSC_IP_FILTER_ADD_REQ = 593,
+	MSG_SMS_ATSC_IP_FILTER_ADD_RES = 594,
+	MSG_SMS_ATSC_IP_FILTER_REMOVE_REQ = 595,
+	MSG_SMS_ATSC_IP_FILTER_REMOVE_RES = 596,
+	MSG_SMS_ATSC_IP_FILTER_REMOVE_ALL_REQ = 597,
+	MSG_SMS_ATSC_IP_FILTER_REMOVE_ALL_RES = 598,
+	MSG_SMS_WAIT_CMD = 599,
+	MSG_SMS_ADD_PID_FILTER_REQ = 601,
+	MSG_SMS_ADD_PID_FILTER_RES = 602,
+	MSG_SMS_REMOVE_PID_FILTER_REQ = 603,
+	MSG_SMS_REMOVE_PID_FILTER_RES = 604,
+	MSG_SMS_FAST_INFORMATION_CHANNEL_REQ = 605,
+	MSG_SMS_FAST_INFORMATION_CHANNEL_RES = 606,
+	MSG_SMS_DAB_CHANNEL = 607,
+	MSG_SMS_GET_PID_FILTER_LIST_REQ = 608,
+	MSG_SMS_GET_PID_FILTER_LIST_RES = 609,
+	MSG_SMS_POWER_DOWN_REQ = 610,
+	MSG_SMS_POWER_DOWN_RES = 611,
+	MSG_SMS_ATSC_SLT_EXIST_IND = 612,
+	MSG_SMS_ATSC_NO_SLT_IND = 613,
+	MSG_SMS_GET_STATISTICS_REQ = 615,
+	MSG_SMS_GET_STATISTICS_RES = 616,
+	MSG_SMS_SEND_DUMP = 617,
+	MSG_SMS_SCAN_START_REQ = 618,
+	MSG_SMS_SCAN_START_RES = 619,
+	MSG_SMS_SCAN_STOP_REQ = 620,
+	MSG_SMS_SCAN_STOP_RES = 621,
+	MSG_SMS_SCAN_PROGRESS_IND = 622,
+	MSG_SMS_SCAN_COMPLETE_IND = 623,
+	MSG_SMS_LOG_ITEM = 624,
+	MSG_SMS_DAB_SUBCHANNEL_RECONFIG_REQ = 628,
+	MSG_SMS_DAB_SUBCHANNEL_RECONFIG_RES = 629,
+	MSG_SMS_HO_PER_SLICES_IND = 630,
+	MSG_SMS_HO_INBAND_POWER_IND = 631,
+	MSG_SMS_MANUAL_DEMOD_REQ = 632,
+	MSG_SMS_HO_TUNE_ON_REQ = 636,
+	MSG_SMS_HO_TUNE_ON_RES = 637,
+	MSG_SMS_HO_TUNE_OFF_REQ = 638,
+	MSG_SMS_HO_TUNE_OFF_RES = 639,
+	MSG_SMS_HO_PEEK_FREQ_REQ = 640,
+	MSG_SMS_HO_PEEK_FREQ_RES = 641,
+	MSG_SMS_HO_PEEK_FREQ_IND = 642,
+	MSG_SMS_MB_ATTEN_SET_REQ = 643,
+	MSG_SMS_MB_ATTEN_SET_RES = 644,
+	MSG_SMS_ENABLE_STAT_IN_I2C_REQ = 649,
+	MSG_SMS_ENABLE_STAT_IN_I2C_RES = 650,
+	MSG_SMS_SET_ANTENNA_CONFIG_REQ = 651,
+	MSG_SMS_SET_ANTENNA_CONFIG_RES = 652,
+	MSG_SMS_GET_STATISTICS_EX_REQ = 653,
+	MSG_SMS_GET_STATISTICS_EX_RES = 654,
+	MSG_SMS_SLEEP_RESUME_COMP_IND = 655,
+	MSG_SMS_SWITCH_HOST_INTERFACE_REQ = 656,
+	MSG_SMS_SWITCH_HOST_INTERFACE_RES = 657,
+	MSG_SMS_DATA_DOWNLOAD_REQ = 660,
+	MSG_SMS_DATA_DOWNLOAD_RES = 661,
+	MSG_SMS_DATA_VALIDITY_REQ = 662,
+	MSG_SMS_DATA_VALIDITY_RES = 663,
+	MSG_SMS_SWDOWNLOAD_TRIGGER_REQ = 664,
+	MSG_SMS_SWDOWNLOAD_TRIGGER_RES = 665,
+	MSG_SMS_SWDOWNLOAD_BACKDOOR_REQ = 666,
+	MSG_SMS_SWDOWNLOAD_BACKDOOR_RES = 667,
+	MSG_SMS_GET_VERSION_EX_REQ = 668,
+	MSG_SMS_GET_VERSION_EX_RES = 669,
+	MSG_SMS_CLOCK_OUTPUT_CONFIG_REQ = 670,
+	MSG_SMS_CLOCK_OUTPUT_CONFIG_RES = 671,
+	MSG_SMS_I2C_SET_FREQ_REQ = 685,
+	MSG_SMS_I2C_SET_FREQ_RES = 686,
+	MSG_SMS_GENERIC_I2C_REQ = 687,
+	MSG_SMS_GENERIC_I2C_RES = 688,
+	MSG_SMS_DVBT_BDA_DATA = 693,
+	MSG_SW_RELOAD_REQ = 697,
+	MSG_SMS_DATA_MSG = 699,
+	MSG_TABLE_UPLOAD_REQ = 700,
+	MSG_TABLE_UPLOAD_RES = 701,
+	MSG_SW_RELOAD_START_REQ = 702,
+	MSG_SW_RELOAD_START_RES = 703,
+	MSG_SW_RELOAD_EXEC_REQ = 704,
+	MSG_SW_RELOAD_EXEC_RES = 705,
+	MSG_SMS_SPI_INT_LINE_SET_REQ = 710,
+	MSG_SMS_SPI_INT_LINE_SET_RES = 711,
+	MSG_SMS_GPIO_CONFIG_EX_REQ = 712,
+	MSG_SMS_GPIO_CONFIG_EX_RES = 713,
+	MSG_SMS_WATCHDOG_ACT_REQ = 716,
+	MSG_SMS_WATCHDOG_ACT_RES = 717,
+	MSG_SMS_LOOPBACK_REQ = 718,
+	MSG_SMS_LOOPBACK_RES = 719,
+	MSG_SMS_RAW_CAPTURE_START_REQ = 720,
+	MSG_SMS_RAW_CAPTURE_START_RES = 721,
+	MSG_SMS_RAW_CAPTURE_ABORT_REQ = 722,
+	MSG_SMS_RAW_CAPTURE_ABORT_RES = 723,
+	MSG_SMS_RAW_CAPTURE_COMPLETE_IND = 728,
+	MSG_SMS_DATA_PUMP_IND = 729,
+	MSG_SMS_DATA_PUMP_REQ = 730,
+	MSG_SMS_DATA_PUMP_RES = 731,
+	MSG_SMS_FLASH_DL_REQ = 732,
+	MSG_SMS_EXEC_TEST_1_REQ = 734,
+	MSG_SMS_EXEC_TEST_1_RES = 735,
+	MSG_SMS_ENBALE_TS_INTERFACE_REQ = 736,
+	MSG_SMS_ENBALE_TS_INTERFACE_RES = 737,
+	MSG_SMS_SPI_SET_BUS_WIDTH_REQ = 738,
+	MSG_SMS_SPI_SET_BUS_WIDTH_RES = 739,
+	MSG_SMS_SEND_EMM_REQ = 740,
+	MSG_SMS_SEND_EMM_RES = 741,
+	MSG_SMS_DISABLE_TS_INTERFACE_REQ = 742,
+	MSG_SMS_DISABLE_TS_INTERFACE_RES = 743,
+	MSG_SMS_IS_BUF_FREE_REQ = 744,
+	MSG_SMS_IS_BUF_FREE_RES = 745,
+	MSG_SMS_EXT_ANTENNA_REQ = 746,
+	MSG_SMS_EXT_ANTENNA_RES = 747,
+	MSG_SMS_CMMB_GET_NET_OF_FREQ_REQ_OBSOLETE = 748,
+	MSG_SMS_CMMB_GET_NET_OF_FREQ_RES_OBSOLETE = 749,
+	MSG_SMS_BATTERY_LEVEL_REQ = 750,
+	MSG_SMS_BATTERY_LEVEL_RES = 751,
+	MSG_SMS_CMMB_INJECT_TABLE_REQ_OBSOLETE = 752,
+	MSG_SMS_CMMB_INJECT_TABLE_RES_OBSOLETE = 753,
+	MSG_SMS_FM_RADIO_BLOCK_IND = 754,
+	MSG_SMS_HOST_NOTIFICATION_IND = 755,
+	MSG_SMS_CMMB_GET_CONTROL_TABLE_REQ_OBSOLETE = 756,
+	MSG_SMS_CMMB_GET_CONTROL_TABLE_RES_OBSOLETE = 757,
+	MSG_SMS_CMMB_GET_NETWORKS_REQ = 760,
+	MSG_SMS_CMMB_GET_NETWORKS_RES = 761,
+	MSG_SMS_CMMB_START_SERVICE_REQ = 762,
+	MSG_SMS_CMMB_START_SERVICE_RES = 763,
+	MSG_SMS_CMMB_STOP_SERVICE_REQ = 764,
+	MSG_SMS_CMMB_STOP_SERVICE_RES = 765,
+	MSG_SMS_CMMB_ADD_CHANNEL_FILTER_REQ = 768,
+	MSG_SMS_CMMB_ADD_CHANNEL_FILTER_RES = 769,
+	MSG_SMS_CMMB_REMOVE_CHANNEL_FILTER_REQ = 770,
+	MSG_SMS_CMMB_REMOVE_CHANNEL_FILTER_RES = 771,
+	MSG_SMS_CMMB_START_CONTROL_INFO_REQ = 772,
+	MSG_SMS_CMMB_START_CONTROL_INFO_RES = 773,
+	MSG_SMS_CMMB_STOP_CONTROL_INFO_REQ = 774,
+	MSG_SMS_CMMB_STOP_CONTROL_INFO_RES = 775,
+	MSG_SMS_ISDBT_TUNE_REQ = 776,
+	MSG_SMS_ISDBT_TUNE_RES = 777,
+	MSG_SMS_TRANSMISSION_IND = 782,
+	MSG_SMS_PID_STATISTICS_IND = 783,
+	MSG_SMS_POWER_DOWN_IND = 784,
+	MSG_SMS_POWER_DOWN_CONF = 785,
+	MSG_SMS_POWER_UP_IND = 786,
+	MSG_SMS_POWER_UP_CONF = 787,
+	MSG_SMS_POWER_MODE_SET_REQ = 790,
+	MSG_SMS_POWER_MODE_SET_RES = 791,
+	MSG_SMS_DEBUG_HOST_EVENT_REQ = 792,
+	MSG_SMS_DEBUG_HOST_EVENT_RES = 793,
+	MSG_SMS_NEW_CRYSTAL_REQ = 794,
+	MSG_SMS_NEW_CRYSTAL_RES = 795,
+	MSG_SMS_CONFIG_SPI_REQ = 796,
+	MSG_SMS_CONFIG_SPI_RES = 797,
+	MSG_SMS_I2C_SHORT_STAT_IND = 798,
+	MSG_SMS_START_IR_REQ = 800,
+	MSG_SMS_START_IR_RES = 801,
+	MSG_SMS_IR_SAMPLES_IND = 802,
+	MSG_SMS_CMMB_CA_SERVICE_IND = 803,
+	MSG_SMS_SLAVE_DEVICE_DETECTED = 804,
+	MSG_SMS_INTERFACE_LOCK_IND = 805,
+	MSG_SMS_INTERFACE_UNLOCK_IND = 806,
+	MSG_SMS_SEND_ROSUM_BUFF_REQ = 810,
+	MSG_SMS_SEND_ROSUM_BUFF_RES = 811,
+	MSG_SMS_ROSUM_BUFF = 812,
+	MSG_SMS_SET_AES128_KEY_REQ = 815,
+	MSG_SMS_SET_AES128_KEY_RES = 816,
+	MSG_SMS_MBBMS_WRITE_REQ = 817,
+	MSG_SMS_MBBMS_WRITE_RES = 818,
+	MSG_SMS_MBBMS_READ_IND = 819,
+	MSG_SMS_IQ_STREAM_START_REQ = 820,
+	MSG_SMS_IQ_STREAM_START_RES = 821,
+	MSG_SMS_IQ_STREAM_STOP_REQ = 822,
+	MSG_SMS_IQ_STREAM_STOP_RES = 823,
+	MSG_SMS_IQ_STREAM_DATA_BLOCK = 824,
+	MSG_SMS_GET_EEPROM_VERSION_REQ = 825,
+	MSG_SMS_GET_EEPROM_VERSION_RES = 826,
+	MSG_SMS_SIGNAL_DETECTED_IND = 827,
+	MSG_SMS_NO_SIGNAL_IND = 828,
+	MSG_SMS_MRC_SHUTDOWN_SLAVE_REQ = 830,
+	MSG_SMS_MRC_SHUTDOWN_SLAVE_RES = 831,
+	MSG_SMS_MRC_BRINGUP_SLAVE_REQ = 832,
+	MSG_SMS_MRC_BRINGUP_SLAVE_RES = 833,
+	MSG_SMS_EXTERNAL_LNA_CTRL_REQ = 834,
+	MSG_SMS_EXTERNAL_LNA_CTRL_RES = 835,
+	MSG_SMS_SET_PERIODIC_STATISTICS_REQ = 836,
+	MSG_SMS_SET_PERIODIC_STATISTICS_RES = 837,
+	MSG_SMS_CMMB_SET_AUTO_OUTPUT_TS0_REQ = 838,
+	MSG_SMS_CMMB_SET_AUTO_OUTPUT_TS0_RES = 839,
+	LOCAL_TUNE = 850,
+	LOCAL_IFFT_H_ICI = 851,
+	MSG_RESYNC_REQ = 852,
+	MSG_SMS_CMMB_GET_MRC_STATISTICS_REQ = 853,
+	MSG_SMS_CMMB_GET_MRC_STATISTICS_RES = 854,
+	MSG_SMS_LOG_EX_ITEM = 855,
+	MSG_SMS_DEVICE_DATA_LOSS_IND = 856,
+	MSG_SMS_MRC_WATCHDOG_TRIGGERED_IND = 857,
+	MSG_SMS_USER_MSG_REQ = 858,
+	MSG_SMS_USER_MSG_RES = 859,
+	MSG_SMS_SMART_CARD_INIT_REQ = 860,
+	MSG_SMS_SMART_CARD_INIT_RES = 861,
+	MSG_SMS_SMART_CARD_WRITE_REQ = 862,
+	MSG_SMS_SMART_CARD_WRITE_RES = 863,
+	MSG_SMS_SMART_CARD_READ_IND = 864,
+	MSG_SMS_TSE_ENABLE_REQ = 866,
+	MSG_SMS_TSE_ENABLE_RES = 867,
+	MSG_SMS_CMMB_GET_SHORT_STATISTICS_REQ = 868,
+	MSG_SMS_CMMB_GET_SHORT_STATISTICS_RES = 869,
+	MSG_SMS_LED_CONFIG_REQ = 870,
+	MSG_SMS_LED_CONFIG_RES = 871,
+	MSG_PWM_ANTENNA_REQ = 872,
+	MSG_PWM_ANTENNA_RES = 873,
+	MSG_SMS_CMMB_SMD_SN_REQ = 874,
+	MSG_SMS_CMMB_SMD_SN_RES = 875,
+	MSG_SMS_CMMB_SET_CA_CW_REQ = 876,
+	MSG_SMS_CMMB_SET_CA_CW_RES = 877,
+	MSG_SMS_CMMB_SET_CA_SALT_REQ = 878,
+	MSG_SMS_CMMB_SET_CA_SALT_RES = 879,
+	MSG_SMS_NSCD_INIT_REQ = 880,
+	MSG_SMS_NSCD_INIT_RES = 881,
+	MSG_SMS_NSCD_PROCESS_SECTION_REQ = 882,
+	MSG_SMS_NSCD_PROCESS_SECTION_RES = 883,
+	MSG_SMS_DBD_CREATE_OBJECT_REQ = 884,
+	MSG_SMS_DBD_CREATE_OBJECT_RES = 885,
+	MSG_SMS_DBD_CONFIGURE_REQ = 886,
+	MSG_SMS_DBD_CONFIGURE_RES = 887,
+	MSG_SMS_DBD_SET_KEYS_REQ = 888,
+	MSG_SMS_DBD_SET_KEYS_RES = 889,
+	MSG_SMS_DBD_PROCESS_HEADER_REQ = 890,
+	MSG_SMS_DBD_PROCESS_HEADER_RES = 891,
+	MSG_SMS_DBD_PROCESS_DATA_REQ = 892,
+	MSG_SMS_DBD_PROCESS_DATA_RES = 893,
+	MSG_SMS_DBD_PROCESS_GET_DATA_REQ = 894,
+	MSG_SMS_DBD_PROCESS_GET_DATA_RES = 895,
+	MSG_SMS_NSCD_OPEN_SESSION_REQ = 896,
+	MSG_SMS_NSCD_OPEN_SESSION_RES = 897,
+	MSG_SMS_SEND_HOST_DATA_TO_DEMUX_REQ = 898,
+	MSG_SMS_SEND_HOST_DATA_TO_DEMUX_RES = 899,
+	MSG_LAST_MSG_TYPE = 900,
+};
 
 #define SMS_INIT_MSG_EX(ptr, type, src, dst, len) do { \
-	(ptr)->msgType = type; (ptr)->msgSrcId = src; (ptr)->msgDstId = dst; \
-	(ptr)->msgLength = len; (ptr)->msgFlags = 0; \
+	(ptr)->msg_type = type; \
+	(ptr)->msg_src_id = src; \
+	(ptr)->msg_dst_id = dst; \
+	(ptr)->msg_length = len; \
+	(ptr)->msg_flags = 0; \
 } while (0)
 
 #define SMS_INIT_MSG(ptr, type, len) \
@@ -277,228 +604,296 @@ enum SMS_DEVICE_MODE {
 	DEVICE_MODE_ISDBT_BDA,
 	DEVICE_MODE_CMMB,
 	DEVICE_MODE_RAW_TUNER,
+	DEVICE_MODE_FM_RADIO,
+	DEVICE_MODE_FM_RADIO_BDA,
+	DEVICE_MODE_ATSC,
 	DEVICE_MODE_MAX,
 };
 
-struct SmsMsgHdr_ST {
-	u16	msgType;
-	u8	msgSrcId;
-	u8	msgDstId;
-	u16	msgLength; /* Length of entire message, including header */
-	u16	msgFlags;
+struct sms_msg_hdr {
+	u16	msg_type;
+	u8	msg_src_id;
+	u8	msg_dst_id;
+	u16	msg_length; /* length of entire message, including header */
+	u16	msg_flags;
+};
+
+struct sms_msg_data {
+	struct sms_msg_hdr x_msg_header;
+	u32 msg_data[1];
 };
 
-struct SmsMsgData_ST {
-	struct SmsMsgHdr_ST xMsgHeader;
-	u32 msgData[1];
+struct sms_msg_data2 {
+	struct sms_msg_hdr x_msg_header;
+	u32 msg_data[2];
 };
 
-struct SmsMsgData_ST2 {
-	struct SmsMsgHdr_ST xMsgHeader;
-	u32 msgData[2];
+struct sms_msg_data4 {
+	struct sms_msg_hdr x_msg_header;
+	u32 msg_data[4];
 };
 
-struct SmsDataDownload_ST {
-	struct SmsMsgHdr_ST	xMsgHeader;
-	u32			MemAddr;
-	u8			Payload[SMS_MAX_PAYLOAD_SIZE];
+struct sms_data_download {
+	struct sms_msg_hdr	x_msg_header;
+	u32			mem_addr;
+	u8			payload[SMS_MAX_PAYLOAD_SIZE];
 };
 
-struct SmsVersionRes_ST {
-	struct SmsMsgHdr_ST	xMsgHeader;
+struct sms_version_res {
+	struct sms_msg_hdr	x_msg_header;
 
-	u16		ChipModel; /* e.g. 0x1102 for SMS-1102 "Nova" */
-	u8		Step; /* 0 - Step A */
-	u8		MetalFix; /* 0 - Metal 0 */
+	u16		chip_model; /* e.g. 0x1102 for SMS-1102 "Nova" */
+	u8		step; /* 0 - step A */
+	u8		metal_fix; /* 0 - Metal 0 */
 
-	/* FirmwareId 0xFF if ROM, otherwise the
+	/* firmware_id 0xFF if ROM, otherwise the
 	 * value indicated by SMSHOSTLIB_DEVICE_MODES_E */
-	u8 FirmwareId;
-	/* SupportedProtocols Bitwise OR combination of
+	u8 firmware_id;
+	/* supported_protocols Bitwise OR combination of
 					     * supported protocols */
-	u8 SupportedProtocols;
+	u8 supported_protocols;
 
-	u8		VersionMajor;
-	u8		VersionMinor;
-	u8		VersionPatch;
-	u8		VersionFieldPatch;
+	u8		version_major;
+	u8		version_minor;
+	u8		version_patch;
+	u8		version_field_patch;
 
-	u8		RomVersionMajor;
-	u8		RomVersionMinor;
-	u8		RomVersionPatch;
-	u8		RomVersionFieldPatch;
+	u8		rom_ver_major;
+	u8		rom_ver_minor;
+	u8		rom_ver_patch;
+	u8		rom_ver_field_patch;
 
 	u8		TextLabel[34];
 };
 
-struct SmsFirmware_ST {
-	u32			CheckSum;
-	u32			Length;
-	u32			StartAddress;
-	u8			Payload[1];
+struct sms_firmware {
+	u32			check_sum;
+	u32			length;
+	u32			start_address;
+	u8			payload[1];
 };
 
-/* Statistics information returned as response for
- * SmsHostApiGetStatistics_Req */
-struct SMSHOSTLIB_STATISTICS_ST {
-	u32 Reserved;		/* Reserved */
+/* statistics information returned as response for
+ * SmsHostApiGetstatistics_Req */
+struct sms_stats {
+	u32 reserved;		/* reserved */
 
 	/* Common parameters */
-	u32 IsRfLocked;		/* 0 - not locked, 1 - locked */
-	u32 IsDemodLocked;	/* 0 - not locked, 1 - locked */
-	u32 IsExternalLNAOn;	/* 0 - external LNA off, 1 - external LNA on */
+	u32 is_rf_locked;		/* 0 - not locked, 1 - locked */
+	u32 is_demod_locked;	/* 0 - not locked, 1 - locked */
+	u32 is_external_lna_on;	/* 0 - external LNA off, 1 - external LNA on */
 
 	/* Reception quality */
 	s32 SNR;		/* dB */
-	u32 BER;		/* Post Viterbi BER [1E-5] */
+	u32 ber;		/* Post Viterbi ber [1E-5] */
 	u32 FIB_CRC;		/* CRC errors percentage, valid only for DAB */
-	u32 TS_PER;		/* Transport stream PER,
+	u32 ts_per;		/* Transport stream PER,
 	0xFFFFFFFF indicate N/A, valid only for DVB-T/H */
 	u32 MFER;		/* DVB-H frame error rate in percentage,
 	0xFFFFFFFF indicate N/A, valid only for DVB-H */
 	s32 RSSI;		/* dBm */
-	s32 InBandPwr;		/* In band power in dBM */
-	s32 CarrierOffset;	/* Carrier Offset in bin/1024 */
+	s32 in_band_pwr;		/* In band power in dBM */
+	s32 carrier_offset;	/* Carrier Offset in bin/1024 */
 
 	/* Transmission parameters */
-	u32 Frequency;		/* Frequency in Hz */
-	u32 Bandwidth;		/* Bandwidth in MHz, valid only for DVB-T/H */
-	u32 TransmissionMode;	/* Transmission Mode, for DAB modes 1-4,
+	u32 frequency;		/* frequency in Hz */
+	u32 bandwidth;		/* bandwidth in MHz, valid only for DVB-T/H */
+	u32 transmission_mode;	/* Transmission Mode, for DAB modes 1-4,
 	for DVB-T/H FFT mode carriers in Kilos */
-	u32 ModemState;		/* from SMSHOSTLIB_DVB_MODEM_STATE_ET,
+	u32 modem_state;		/* from SMSHOSTLIB_DVB_MODEM_STATE_ET,
 	valid only for DVB-T/H */
-	u32 GuardInterval;	/* Guard Interval from
-	SMSHOSTLIB_GUARD_INTERVALS_ET, 	valid only for DVB-T/H */
-	u32 CodeRate;		/* Code Rate from SMSHOSTLIB_CODE_RATE_ET,
+	u32 guard_interval;	/* Guard Interval from
+	SMSHOSTLIB_GUARD_INTERVALS_ET,	valid only for DVB-T/H */
+	u32 code_rate;		/* Code Rate from SMSHOSTLIB_CODE_RATE_ET,
 	valid only for DVB-T/H */
-	u32 LPCodeRate;		/* Low Priority Code Rate from
+	u32 lp_code_rate;		/* Low Priority Code Rate from
 	SMSHOSTLIB_CODE_RATE_ET, valid only for DVB-T/H */
-	u32 Hierarchy;		/* Hierarchy from SMSHOSTLIB_HIERARCHY_ET,
+	u32 hierarchy;		/* hierarchy from SMSHOSTLIB_HIERARCHY_ET,
 	valid only for DVB-T/H */
-	u32 Constellation;	/* Constellation from
+	u32 constellation;	/* constellation from
 	SMSHOSTLIB_CONSTELLATION_ET, valid only for DVB-T/H */
 
 	/* Burst parameters, valid only for DVB-H */
-	u32 BurstSize;		/* Current burst size in bytes,
+	u32 burst_size;		/* Current burst size in bytes,
 	valid only for DVB-H */
-	u32 BurstDuration;	/* Current burst duration in mSec,
+	u32 burst_duration;	/* Current burst duration in mSec,
 	valid only for DVB-H */
-	u32 BurstCycleTime;	/* Current burst cycle time in mSec,
+	u32 burst_cycle_time;	/* Current burst cycle time in mSec,
 	valid only for DVB-H */
-	u32 CalculatedBurstCycleTime;/* Current burst cycle time in mSec,
+	u32 calc_burst_cycle_time;/* Current burst cycle time in mSec,
 	as calculated by demodulator, valid only for DVB-H */
-	u32 NumOfRows;		/* Number of rows in MPE table,
+	u32 num_of_rows;		/* Number of rows in MPE table,
 	valid only for DVB-H */
-	u32 NumOfPaddCols;	/* Number of padding columns in MPE table,
+	u32 num_of_padd_cols;	/* Number of padding columns in MPE table,
 	valid only for DVB-H */
-	u32 NumOfPunctCols;	/* Number of puncturing columns in MPE table,
+	u32 num_of_punct_cols;	/* Number of puncturing columns in MPE table,
 	valid only for DVB-H */
-	u32 ErrorTSPackets;	/* Number of erroneous
+	u32 error_ts_packets;	/* Number of erroneous
 	transport-stream packets */
-	u32 TotalTSPackets;	/* Total number of transport-stream packets */
-	u32 NumOfValidMpeTlbs;	/* Number of MPE tables which do not include
+	u32 total_ts_packets;	/* Total number of transport-stream packets */
+	u32 num_of_valid_mpe_tlbs;	/* Number of MPE tables which do not include
 	errors after MPE RS decoding */
-	u32 NumOfInvalidMpeTlbs;/* Number of MPE tables which include errors
+	u32 num_of_invalid_mpe_tlbs;/* Number of MPE tables which include errors
 	after MPE RS decoding */
-	u32 NumOfCorrectedMpeTlbs;/* Number of MPE tables which were
+	u32 num_of_corrected_mpe_tlbs;/* Number of MPE tables which were
 	corrected by MPE RS decoding */
 	/* Common params */
-	u32 BERErrorCount;	/* Number of errornous SYNC bits. */
-	u32 BERBitCount;	/* Total number of SYNC bits. */
+	u32 ber_error_count;	/* Number of errornous SYNC bits. */
+	u32 ber_bit_count;	/* Total number of SYNC bits. */
 
 	/* Interface information */
-	u32 SmsToHostTxErrors;	/* Total number of transmission errors. */
+	u32 sms_to_host_tx_errors;	/* Total number of transmission errors. */
 
 	/* DAB/T-DMB */
-	u32 PreBER; 		/* DAB/T-DMB only: Pre Viterbi BER [1E-5] */
+	u32 pre_ber;		/* DAB/T-DMB only: Pre Viterbi ber [1E-5] */
 
 	/* DVB-H TPS parameters */
-	u32 CellId;		/* TPS Cell ID in bits 15..0, bits 31..16 zero;
+	u32 cell_id;		/* TPS Cell ID in bits 15..0, bits 31..16 zero;
 	 if set to 0xFFFFFFFF cell_id not yet recovered */
-	u32 DvbhSrvIndHP;	/* DVB-H service indication info, bit 1 -
+	u32 dvbh_srv_ind_hp;	/* DVB-H service indication info, bit 1 -
 	Time Slicing indicator, bit 0 - MPE-FEC indicator */
-	u32 DvbhSrvIndLP;	/* DVB-H service indication info, bit 1 -
+	u32 dvbh_srv_ind_lp;	/* DVB-H service indication info, bit 1 -
 	Time Slicing indicator, bit 0 - MPE-FEC indicator */
 
-	u32 NumMPEReceived;	/* DVB-H, Num MPE section received */
+	u32 num_mpe_received;	/* DVB-H, Num MPE section received */
 
-	u32 ReservedFields[10];	/* Reserved */
+	u32 reservedFields[10];	/* reserved */
 };
 
-struct SmsMsgStatisticsInfo_ST {
-	u32 RequestResult;
+struct sms_msg_statistics_info {
+	u32 request_result;
 
-	struct SMSHOSTLIB_STATISTICS_ST Stat;
+	struct sms_stats stat;
 
 	/* Split the calc of the SNR in DAB */
-	u32 Signal; /* dB */
-	u32 Noise; /* dB */
+	u32 signal; /* dB */
+	u32 noise; /* dB */
 
 };
 
-struct SMSHOSTLIB_ISDBT_LAYER_STAT_ST {
+struct sms_isdbt_layer_stats {
 	/* Per-layer information */
-	u32 CodeRate; /* Code Rate from SMSHOSTLIB_CODE_RATE_ET,
+	u32 code_rate; /* Code Rate from SMSHOSTLIB_CODE_RATE_ET,
 		       * 255 means layer does not exist */
-	u32 Constellation; /* Constellation from SMSHOSTLIB_CONSTELLATION_ET,
+	u32 constellation; /* constellation from SMSHOSTLIB_CONSTELLATION_ET,
 			    * 255 means layer does not exist */
-	u32 BER; /* Post Viterbi BER [1E-5], 0xFFFFFFFF indicate N/A */
-	u32 BERErrorCount; /* Post Viterbi Error Bits Count */
-	u32 BERBitCount; /* Post Viterbi Total Bits Count */
-	u32 PreBER; /* Pre Viterbi BER [1E-5], 0xFFFFFFFF indicate N/A */
-	u32 TS_PER; /* Transport stream PER [%], 0xFFFFFFFF indicate N/A */
-	u32 ErrorTSPackets; /* Number of erroneous transport-stream packets */
-	u32 TotalTSPackets; /* Total number of transport-stream packets */
-	u32 TILdepthI; /* Time interleaver depth I parameter,
+	u32 ber; /* Post Viterbi ber [1E-5], 0xFFFFFFFF indicate N/A */
+	u32 ber_error_count; /* Post Viterbi Error Bits Count */
+	u32 ber_bit_count; /* Post Viterbi Total Bits Count */
+	u32 pre_ber; /* Pre Viterbi ber [1E-5], 0xFFFFFFFF indicate N/A */
+	u32 ts_per; /* Transport stream PER [%], 0xFFFFFFFF indicate N/A */
+	u32 error_ts_packets; /* Number of erroneous transport-stream packets */
+	u32 total_ts_packets; /* Total number of transport-stream packets */
+	u32 ti_ldepth_i; /* Time interleaver depth I parameter,
 			* 255 means layer does not exist */
-	u32 NumberOfSegments; /* Number of segments in layer A,
+	u32 number_of_segments; /* Number of segments in layer A,
 			       * 255 means layer does not exist */
-	u32 TMCCErrors; /* TMCC errors */
+	u32 tmcc_errors; /* TMCC errors */
 };
 
-struct SMSHOSTLIB_STATISTICS_ISDBT_ST {
-	u32 StatisticsType; /* Enumerator identifying the type of the
+struct sms_isdbt_stats {
+	u32 statistics_type; /* Enumerator identifying the type of the
 				* structure.  Values are the same as
 				* SMSHOSTLIB_DEVICE_MODES_E
 				*
 				* This field MUST always be first in any
 				* statistics structure */
 
-	u32 FullSize; /* Total size of the structure returned by the modem.
+	u32 full_size; /* Total size of the structure returned by the modem.
 		       * If the size requested by the host is smaller than
-		       * FullSize, the struct will be truncated */
+		       * full_size, the struct will be truncated */
 
 	/* Common parameters */
-	u32 IsRfLocked; /* 0 - not locked, 1 - locked */
-	u32 IsDemodLocked; /* 0 - not locked, 1 - locked */
-	u32 IsExternalLNAOn; /* 0 - external LNA off, 1 - external LNA on */
+	u32 is_rf_locked; /* 0 - not locked, 1 - locked */
+	u32 is_demod_locked; /* 0 - not locked, 1 - locked */
+	u32 is_external_lna_on; /* 0 - external LNA off, 1 - external LNA on */
 
 	/* Reception quality */
 	s32  SNR; /* dB */
 	s32  RSSI; /* dBm */
-	s32  InBandPwr; /* In band power in dBM */
-	s32  CarrierOffset; /* Carrier Offset in Hz */
+	s32  in_band_pwr; /* In band power in dBM */
+	s32  carrier_offset; /* Carrier Offset in Hz */
 
 	/* Transmission parameters */
-	u32 Frequency; /* Frequency in Hz */
-	u32 Bandwidth; /* Bandwidth in MHz */
-	u32 TransmissionMode; /* ISDB-T transmission mode */
-	u32 ModemState; /* 0 - Acquisition, 1 - Locked */
-	u32 GuardInterval; /* Guard Interval, 1 divided by value */
-	u32 SystemType; /* ISDB-T system type (ISDB-T / ISDB-Tsb) */
-	u32 PartialReception; /* TRUE - partial reception, FALSE otherwise */
-	u32 NumOfLayers; /* Number of ISDB-T layers in the network */
+	u32 frequency; /* frequency in Hz */
+	u32 bandwidth; /* bandwidth in MHz */
+	u32 transmission_mode; /* ISDB-T transmission mode */
+	u32 modem_state; /* 0 - Acquisition, 1 - Locked */
+	u32 guard_interval; /* Guard Interval, 1 divided by value */
+	u32 system_type; /* ISDB-T system type (ISDB-T / ISDB-Tsb) */
+	u32 partial_reception; /* TRUE - partial reception, FALSE otherwise */
+	u32 num_of_layers; /* Number of ISDB-T layers in the network */
 
 	/* Per-layer information */
 	/* Layers A, B and C */
-	struct SMSHOSTLIB_ISDBT_LAYER_STAT_ST	LayerInfo[3];
-	/* Per-layer statistics, see SMSHOSTLIB_ISDBT_LAYER_STAT_ST */
+	struct sms_isdbt_layer_stats	layer_info[3];
+	/* Per-layer statistics, see sms_isdbt_layer_stats */
 
 	/* Interface information */
-	u32 SmsToHostTxErrors; /* Total number of transmission errors. */
+	u32 sms_to_host_tx_errors; /* Total number of transmission errors. */
 };
 
-struct PID_STATISTICS_DATA_S {
+struct sms_isdbt_stats_ex {
+	u32 statistics_type; /* Enumerator identifying the type of the
+				* structure.  Values are the same as
+				* SMSHOSTLIB_DEVICE_MODES_E
+				*
+				* This field MUST always be first in any
+				* statistics structure */
+
+	u32 full_size; /* Total size of the structure returned by the modem.
+		       * If the size requested by the host is smaller than
+		       * full_size, the struct will be truncated */
+
+	/* Common parameters */
+	u32 is_rf_locked; /* 0 - not locked, 1 - locked */
+	u32 is_demod_locked; /* 0 - not locked, 1 - locked */
+	u32 is_external_lna_on; /* 0 - external LNA off, 1 - external LNA on */
+
+	/* Reception quality */
+	s32  SNR; /* dB */
+	s32  RSSI; /* dBm */
+	s32  in_band_pwr; /* In band power in dBM */
+	s32  carrier_offset; /* Carrier Offset in Hz */
+
+	/* Transmission parameters */
+	u32 frequency; /* frequency in Hz */
+	u32 bandwidth; /* bandwidth in MHz */
+	u32 transmission_mode; /* ISDB-T transmission mode */
+	u32 modem_state; /* 0 - Acquisition, 1 - Locked */
+	u32 guard_interval; /* Guard Interval, 1 divided by value */
+	u32 system_type; /* ISDB-T system type (ISDB-T / ISDB-Tsb) */
+	u32 partial_reception; /* TRUE - partial reception, FALSE otherwise */
+	u32 num_of_layers; /* Number of ISDB-T layers in the network */
+
+	u32 segment_number; /* Segment number for ISDB-Tsb */
+	u32 tune_bw;	   /* Tuned bandwidth - BW_ISDBT_1SEG / BW_ISDBT_3SEG */
+
+	/* Per-layer information */
+	/* Layers A, B and C */
+	struct sms_isdbt_layer_stats	layer_info[3];
+	/* Per-layer statistics, see sms_isdbt_layer_stats */
+
+	/* Interface information */
+	u32 reserved1;    /* Was sms_to_host_tx_errors - obsolete . */
+ /* Proprietary information */
+	u32 ext_antenna;    /* Obsolete field. */
+	u32 reception_quality;
+	u32 ews_alert_active;   /* signals if EWS alert is currently on */
+	u32 lna_on_off;	/* Internal LNA state: 0: OFF, 1: ON */
+
+	u32 rf_agc_level;	 /* RF AGC Level [linear units], full gain = 65535 (20dB) */
+	u32 bb_agc_level;    /* Baseband AGC level [linear units], full gain = 65535 (71.5dB) */
+	u32 fw_errors_counter;   /* Application errors - should be always zero */
+	u8 FwErrorsHistoryArr[8]; /* Last FW errors IDs - first is most recent, last is oldest */
+
+	s32  MRC_SNR;     /* dB */
+	u32 snr_full_res;    /* dB x 65536 */
+	u32 reserved4[4];
+};
+
+
+struct sms_pid_stats_data {
 	struct PID_BURST_S {
 		u32 size;
 		u32 padding_cols;
@@ -513,112 +908,155 @@ struct PID_STATISTICS_DATA_S {
 	u32 tot_cor_tbl;
 };
 
-struct PID_DATA_S {
+struct sms_pid_data {
 	u32 pid;
 	u32 num_rows;
-	struct PID_STATISTICS_DATA_S pid_statistics;
+	struct sms_pid_stats_data pid_statistics;
 };
 
 #define CORRECT_STAT_RSSI(_stat) ((_stat).RSSI *= -1)
-#define CORRECT_STAT_BANDWIDTH(_stat) (_stat.Bandwidth = 8 - _stat.Bandwidth)
+#define CORRECT_STAT_BANDWIDTH(_stat) (_stat.bandwidth = 8 - _stat.bandwidth)
 #define CORRECT_STAT_TRANSMISSON_MODE(_stat) \
-	if (_stat.TransmissionMode == 0) \
-		_stat.TransmissionMode = 2; \
-	else if (_stat.TransmissionMode == 1) \
-		_stat.TransmissionMode = 8; \
+	if (_stat.transmission_mode == 0) \
+		_stat.transmission_mode = 2; \
+	else if (_stat.transmission_mode == 1) \
+		_stat.transmission_mode = 8; \
 		else \
-			_stat.TransmissionMode = 4;
+			_stat.transmission_mode = 4;
 
-struct TRANSMISSION_STATISTICS_S {
-	u32 Frequency;		/* Frequency in Hz */
-	u32 Bandwidth;		/* Bandwidth in MHz */
-	u32 TransmissionMode;	/* FFT mode carriers in Kilos */
-	u32 GuardInterval;	/* Guard Interval from
+struct sms_tx_stats {
+	u32 frequency;		/* frequency in Hz */
+	u32 bandwidth;		/* bandwidth in MHz */
+	u32 transmission_mode;	/* FFT mode carriers in Kilos */
+	u32 guard_interval;	/* Guard Interval from
 	SMSHOSTLIB_GUARD_INTERVALS_ET */
-	u32 CodeRate;		/* Code Rate from SMSHOSTLIB_CODE_RATE_ET */
-	u32 LPCodeRate;		/* Low Priority Code Rate from
+	u32 code_rate;		/* Code Rate from SMSHOSTLIB_CODE_RATE_ET */
+	u32 lp_code_rate;		/* Low Priority Code Rate from
 	SMSHOSTLIB_CODE_RATE_ET */
-	u32 Hierarchy;		/* Hierarchy from SMSHOSTLIB_HIERARCHY_ET */
-	u32 Constellation;	/* Constellation from
+	u32 hierarchy;		/* hierarchy from SMSHOSTLIB_HIERARCHY_ET */
+	u32 constellation;	/* constellation from
 	SMSHOSTLIB_CONSTELLATION_ET */
 
 	/* DVB-H TPS parameters */
-	u32 CellId;		/* TPS Cell ID in bits 15..0, bits 31..16 zero;
+	u32 cell_id;		/* TPS Cell ID in bits 15..0, bits 31..16 zero;
 	 if set to 0xFFFFFFFF cell_id not yet recovered */
-	u32 DvbhSrvIndHP;	/* DVB-H service indication info, bit 1 -
+	u32 dvbh_srv_ind_hp;	/* DVB-H service indication info, bit 1 -
 	 Time Slicing indicator, bit 0 - MPE-FEC indicator */
-	u32 DvbhSrvIndLP;	/* DVB-H service indication info, bit 1 -
+	u32 dvbh_srv_ind_lp;	/* DVB-H service indication info, bit 1 -
 	 Time Slicing indicator, bit 0 - MPE-FEC indicator */
-	u32 IsDemodLocked;	/* 0 - not locked, 1 - locked */
+	u32 is_demod_locked;	/* 0 - not locked, 1 - locked */
+};
+
+struct sms_rx_stats {
+	u32 is_rf_locked;		/* 0 - not locked, 1 - locked */
+	u32 is_demod_locked;	/* 0 - not locked, 1 - locked */
+	u32 is_external_lna_on;	/* 0 - external LNA off, 1 - external LNA on */
+
+	u32 modem_state;		/* from SMSHOSTLIB_DVB_MODEM_STATE_ET */
+	s32 SNR;		/* dB */
+	u32 ber;		/* Post Viterbi ber [1E-5] */
+	u32 ber_error_count;	/* Number of erronous SYNC bits. */
+	u32 ber_bit_count;	/* Total number of SYNC bits. */
+	u32 ts_per;		/* Transport stream PER,
+	0xFFFFFFFF indicate N/A */
+	u32 MFER;		/* DVB-H frame error rate in percentage,
+	0xFFFFFFFF indicate N/A, valid only for DVB-H */
+	s32 RSSI;		/* dBm */
+	s32 in_band_pwr;		/* In band power in dBM */
+	s32 carrier_offset;	/* Carrier Offset in bin/1024 */
+	u32 error_ts_packets;	/* Number of erroneous
+	transport-stream packets */
+	u32 total_ts_packets;	/* Total number of transport-stream packets */
+
+	s32 MRC_SNR;		/* dB */
+	s32 MRC_RSSI;		/* dBm */
+	s32 mrc_in_band_pwr;	/* In band power in dBM */
 };
 
-struct RECEPTION_STATISTICS_S {
-	u32 IsRfLocked;		/* 0 - not locked, 1 - locked */
-	u32 IsDemodLocked;	/* 0 - not locked, 1 - locked */
-	u32 IsExternalLNAOn;	/* 0 - external LNA off, 1 - external LNA on */
+struct sms_rx_stats_ex {
+	u32 is_rf_locked;		/* 0 - not locked, 1 - locked */
+	u32 is_demod_locked;	/* 0 - not locked, 1 - locked */
+	u32 is_external_lna_on;	/* 0 - external LNA off, 1 - external LNA on */
 
-	u32 ModemState;		/* from SMSHOSTLIB_DVB_MODEM_STATE_ET */
+	u32 modem_state;		/* from SMSHOSTLIB_DVB_MODEM_STATE_ET */
 	s32 SNR;		/* dB */
-	u32 BER;		/* Post Viterbi BER [1E-5] */
-	u32 BERErrorCount;	/* Number of erronous SYNC bits. */
-	u32 BERBitCount;	/* Total number of SYNC bits. */
-	u32 TS_PER;		/* Transport stream PER,
+	u32 ber;		/* Post Viterbi ber [1E-5] */
+	u32 ber_error_count;	/* Number of erronous SYNC bits. */
+	u32 ber_bit_count;	/* Total number of SYNC bits. */
+	u32 ts_per;		/* Transport stream PER,
 	0xFFFFFFFF indicate N/A */
 	u32 MFER;		/* DVB-H frame error rate in percentage,
 	0xFFFFFFFF indicate N/A, valid only for DVB-H */
 	s32 RSSI;		/* dBm */
-	s32 InBandPwr;		/* In band power in dBM */
-	s32 CarrierOffset;	/* Carrier Offset in bin/1024 */
-	u32 ErrorTSPackets;	/* Number of erroneous
+	s32 in_band_pwr;		/* In band power in dBM */
+	s32 carrier_offset;	/* Carrier Offset in bin/1024 */
+	u32 error_ts_packets;	/* Number of erroneous
 	transport-stream packets */
-	u32 TotalTSPackets;	/* Total number of transport-stream packets */
+	u32 total_ts_packets;	/* Total number of transport-stream packets */
+
+	s32  ref_dev_ppm;
+	s32  freq_dev_hz;
 
 	s32 MRC_SNR;		/* dB */
 	s32 MRC_RSSI;		/* dBm */
-	s32 MRC_InBandPwr;	/* In band power in dBM */
+	s32 mrc_in_band_pwr;	/* In band power in dBM */
 };
 
 
-/* Statistics information returned as response for
- * SmsHostApiGetStatisticsEx_Req for DVB applications, SMS1100 and up */
-struct SMSHOSTLIB_STATISTICS_DVB_S {
+/* statistics information returned as response for
+ * SmsHostApiGetstatisticsEx_Req for DVB applications, SMS1100 and up */
+struct sms_stats_dvb {
 	/* Reception */
-	struct RECEPTION_STATISTICS_S ReceptionData;
+	struct sms_rx_stats reception_data;
 
 	/* Transmission parameters */
-	struct TRANSMISSION_STATISTICS_S TransmissionData;
+	struct sms_tx_stats transmission_data;
 
 	/* Burst parameters, valid only for DVB-H */
 #define	SRVM_MAX_PID_FILTERS 8
-	struct PID_DATA_S PidData[SRVM_MAX_PID_FILTERS];
+	struct sms_pid_data pid_data[SRVM_MAX_PID_FILTERS];
 };
 
-struct SRVM_SIGNAL_STATUS_S {
+/* statistics information returned as response for
+ * SmsHostApiGetstatisticsEx_Req for DVB applications, SMS1100 and up */
+struct sms_stats_dvb_ex {
+	/* Reception */
+	struct sms_rx_stats_ex reception_data;
+
+	/* Transmission parameters */
+	struct sms_tx_stats transmission_data;
+
+	/* Burst parameters, valid only for DVB-H */
+#define	SRVM_MAX_PID_FILTERS 8
+	struct sms_pid_data pid_data[SRVM_MAX_PID_FILTERS];
+};
+
+struct sms_srvm_signal_status {
 	u32 result;
 	u32 snr;
-	u32 tsPackets;
-	u32 etsPackets;
+	u32 ts_packets;
+	u32 ets_packets;
 	u32 constellation;
-	u32 hpCode;
-	u32 tpsSrvIndLP;
-	u32 tpsSrvIndHP;
-	u32 cellId;
+	u32 hp_code;
+	u32 tps_srv_ind_lp;
+	u32 tps_srv_ind_hp;
+	u32 cell_id;
 	u32 reason;
 
-	s32 inBandPower;
-	u32 requestId;
+	s32 in_band_power;
+	u32 request_id;
 };
 
-struct SMSHOSTLIB_I2C_REQ_ST {
-	u32	DeviceAddress; /* I2c device address */
-	u32	WriteCount; /* number of bytes to write */
-	u32	ReadCount; /* number of bytes to read */
+struct sms_i2c_req {
+	u32	device_address; /* I2c device address */
+	u32	write_count; /* number of bytes to write */
+	u32	read_count; /* number of bytes to read */
 	u8	Data[1];
 };
 
-struct SMSHOSTLIB_I2C_RES_ST {
-	u32	Status; /* non-zero value in case of failure */
-	u32	ReadCount; /* number of bytes read */
+struct sms_i2c_res {
+	u32	status; /* non-zero value in case of failure */
+	u32	read_count; /* number of bytes read */
 	u8	Data[1];
 };
 
@@ -638,59 +1076,39 @@ struct smscore_config_gpio {
 #define SMS_GPIO_INPUTCHARACTERISTICS_SCHMITT 1
 	u8 inputcharacteristics;
 
-#define SMS_GPIO_OUTPUTSLEWRATE_FAST 0
-#define SMS_GPIO_OUTPUTSLEWRATE_SLOW 1
-	u8 outputslewrate;
-
-#define SMS_GPIO_OUTPUTDRIVING_4mA  0
-#define SMS_GPIO_OUTPUTDRIVING_8mA  1
-#define SMS_GPIO_OUTPUTDRIVING_12mA 2
-#define SMS_GPIO_OUTPUTDRIVING_16mA 3
-	u8 outputdriving;
-};
-
-struct smscore_gpio_config {
-#define SMS_GPIO_DIRECTION_INPUT  0
-#define SMS_GPIO_DIRECTION_OUTPUT 1
-	u8 Direction;
-
-#define SMS_GPIO_PULL_UP_DOWN_NONE     0
-#define SMS_GPIO_PULL_UP_DOWN_PULLDOWN 1
-#define SMS_GPIO_PULL_UP_DOWN_PULLUP   2
-#define SMS_GPIO_PULL_UP_DOWN_KEEPER   3
-	u8 PullUpDown;
-
-#define SMS_GPIO_INPUT_CHARACTERISTICS_NORMAL  0
-#define SMS_GPIO_INPUT_CHARACTERISTICS_SCHMITT 1
-	u8 InputCharacteristics;
-
-#define SMS_GPIO_OUTPUT_SLEW_RATE_SLOW		1 /* 10xx */
-#define SMS_GPIO_OUTPUT_SLEW_RATE_FAST		0 /* 10xx */
+	/* 10xx */
+#define SMS_GPIO_OUTPUT_SLEW_RATE_FAST 0
+#define SMS_GPIO_OUTPUT_SLEW_WRATE_SLOW 1
 
+	/* 11xx */
+#define SMS_GPIO_OUTPUT_SLEW_RATE_0_45_V_NS	0
+#define SMS_GPIO_OUTPUT_SLEW_RATE_0_9_V_NS	1
+#define SMS_GPIO_OUTPUT_SLEW_RATE_1_7_V_NS	2
+#define SMS_GPIO_OUTPUT_SLEW_RATE_3_3_V_NS	3
 
-#define SMS_GPIO_OUTPUT_SLEW_RATE_0_45_V_NS	0 /* 11xx */
-#define SMS_GPIO_OUTPUT_SLEW_RATE_0_9_V_NS	1 /* 11xx */
-#define SMS_GPIO_OUTPUT_SLEW_RATE_1_7_V_NS	2 /* 11xx */
-#define SMS_GPIO_OUTPUT_SLEW_RATE_3_3_V_NS	3 /* 11xx */
-	u8 OutputSlewRate;
+	u8 outputslewrate;
 
-#define SMS_GPIO_OUTPUT_DRIVING_S_4mA		0 /* 10xx */
-#define SMS_GPIO_OUTPUT_DRIVING_S_8mA		1 /* 10xx */
-#define SMS_GPIO_OUTPUT_DRIVING_S_12mA		2 /* 10xx */
-#define SMS_GPIO_OUTPUT_DRIVING_S_16mA		3 /* 10xx */
+	/* 10xx */
+#define SMS_GPIO_OUTPUTDRIVING_S_4mA  0
+#define SMS_GPIO_OUTPUTDRIVING_S_8mA  1
+#define SMS_GPIO_OUTPUTDRIVING_S_12mA 2
+#define SMS_GPIO_OUTPUTDRIVING_S_16mA 3
+
+	/* 11xx*/
+#define SMS_GPIO_OUTPUTDRIVING_1_5mA	0
+#define SMS_GPIO_OUTPUTDRIVING_2_8mA	1
+#define SMS_GPIO_OUTPUTDRIVING_4mA	2
+#define SMS_GPIO_OUTPUTDRIVING_7mA	3
+#define SMS_GPIO_OUTPUTDRIVING_10mA	4
+#define SMS_GPIO_OUTPUTDRIVING_11mA	5
+#define SMS_GPIO_OUTPUTDRIVING_14mA	6
+#define SMS_GPIO_OUTPUTDRIVING_16mA	7
 
-#define SMS_GPIO_OUTPUT_DRIVING_1_5mA		0 /* 11xx */
-#define SMS_GPIO_OUTPUT_DRIVING_2_8mA		1 /* 11xx */
-#define SMS_GPIO_OUTPUT_DRIVING_4mA		2 /* 11xx */
-#define SMS_GPIO_OUTPUT_DRIVING_7mA		3 /* 11xx */
-#define SMS_GPIO_OUTPUT_DRIVING_10mA		4 /* 11xx */
-#define SMS_GPIO_OUTPUT_DRIVING_11mA		5 /* 11xx */
-#define SMS_GPIO_OUTPUT_DRIVING_14mA		6 /* 11xx */
-#define SMS_GPIO_OUTPUT_DRIVING_16mA		7 /* 11xx */
-	u8 OutputDriving;
+	u8 outputdriving;
 };
 
-extern void smscore_registry_setmode(char *devpath, int mode);
+char *smscore_translate_msg(enum msg_types msgtype);
+
 extern int smscore_registry_getmode(char *devpath);
 
 extern int smscore_register_hotplug(hotplug_t hotplug);
@@ -721,8 +1139,6 @@ extern void smscore_onresponse(struct smscore_device_t *coredev,
 extern int smscore_get_common_buffer_size(struct smscore_device_t *coredev);
 extern int smscore_map_common_buffer(struct smscore_device_t *coredev,
 				      struct vm_area_struct *vma);
-extern int smscore_get_fw_filename(struct smscore_device_t *coredev,
-				   int mode, char *filename);
 extern int smscore_send_fw_file(struct smscore_device_t *coredev,
 				u8 *ufwbuf, int size);
 
@@ -737,11 +1153,11 @@ int smscore_configure_gpio(struct smscore_device_t *coredev, u32 pin,
 int smscore_set_gpio(struct smscore_device_t *coredev, u32 pin, int level);
 
 /* new GPIO management */
-extern int smscore_gpio_configure(struct smscore_device_t *coredev, u8 PinNum,
-		struct smscore_gpio_config *pGpioConfig);
-extern int smscore_gpio_set_level(struct smscore_device_t *coredev, u8 PinNum,
-		u8 NewLevel);
-extern int smscore_gpio_get_level(struct smscore_device_t *coredev, u8 PinNum,
+extern int smscore_gpio_configure(struct smscore_device_t *coredev, u8 pin_num,
+		struct smscore_config_gpio *p_gpio_config);
+extern int smscore_gpio_set_level(struct smscore_device_t *coredev, u8 pin_num,
+		u8 new_level);
+extern int smscore_gpio_get_level(struct smscore_device_t *coredev, u8 pin_num,
 		u8 *level);
 
 void smscore_set_board_id(struct smscore_device_t *core, int id);
@@ -760,7 +1176,8 @@ int smscore_led_state(struct smscore_device_t *core, int led);
 
 #define dprintk(kern, lvl, fmt, arg...) do {\
 	if (sms_dbg & lvl) \
-		sms_printk(kern, fmt, ##arg); } while (0)
+		sms_printk(kern, fmt, ##arg); \
+} while (0)
 
 #define sms_log(fmt, arg...) sms_printk(KERN_INFO, fmt, ##arg)
 #define sms_err(fmt, arg...) \
diff --git a/drivers/media/common/siano/smsdvb-debugfs.c b/drivers/media/common/siano/smsdvb-debugfs.c
new file mode 100644
index 000000000000..0bb4430535f9
--- /dev/null
+++ b/drivers/media/common/siano/smsdvb-debugfs.c
@@ -0,0 +1,551 @@
+/***********************************************************************
+ *
+ * Copyright(c) 2013 Mauro Carvalho Chehab <mchehab@redhat.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, see <http://www.gnu.org/licenses/>.
+ *
+ ***********************************************************************/
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/debugfs.h>
+#include <linux/spinlock.h>
+#include <linux/usb.h>
+
+#include "dmxdev.h"
+#include "dvbdev.h"
+#include "dvb_demux.h"
+#include "dvb_frontend.h"
+
+#include "smscoreapi.h"
+
+#include "smsdvb.h"
+
+static struct dentry *smsdvb_debugfs_usb_root;
+
+struct smsdvb_debugfs {
+	struct kref		refcount;
+	spinlock_t		lock;
+
+	char			stats_data[PAGE_SIZE];
+	unsigned		stats_count;
+	bool			stats_was_read;
+
+	wait_queue_head_t	stats_queue;
+};
+
+static void smsdvb_print_dvb_stats(struct smsdvb_debugfs *debug_data,
+			    struct sms_stats *p)
+{
+	int n = 0;
+	char *buf;
+
+	spin_lock(&debug_data->lock);
+	if (debug_data->stats_count) {
+		spin_unlock(&debug_data->lock);
+		return;
+	}
+
+	buf = debug_data->stats_data;
+
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_rf_locked = %d\n", p->is_rf_locked);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_demod_locked = %d\n", p->is_demod_locked);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_external_lna_on = %d\n", p->is_external_lna_on);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "SNR = %d\n", p->SNR);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "ber = %d\n", p->ber);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "FIB_CRC = %d\n", p->FIB_CRC);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "ts_per = %d\n", p->ts_per);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "MFER = %d\n", p->MFER);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "RSSI = %d\n", p->RSSI);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "in_band_pwr = %d\n", p->in_band_pwr);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "carrier_offset = %d\n", p->carrier_offset);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "modem_state = %d\n", p->modem_state);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "frequency = %d\n", p->frequency);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "bandwidth = %d\n", p->bandwidth);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "transmission_mode = %d\n", p->transmission_mode);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "modem_state = %d\n", p->modem_state);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "guard_interval = %d\n", p->guard_interval);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "code_rate = %d\n", p->code_rate);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "lp_code_rate = %d\n", p->lp_code_rate);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "hierarchy = %d\n", p->hierarchy);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "constellation = %d\n", p->constellation);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "burst_size = %d\n", p->burst_size);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "burst_duration = %d\n", p->burst_duration);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "burst_cycle_time = %d\n", p->burst_cycle_time);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "calc_burst_cycle_time = %d\n",
+		      p->calc_burst_cycle_time);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_rows = %d\n", p->num_of_rows);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_padd_cols = %d\n", p->num_of_padd_cols);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_punct_cols = %d\n", p->num_of_punct_cols);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "error_ts_packets = %d\n", p->error_ts_packets);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "total_ts_packets = %d\n", p->total_ts_packets);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_valid_mpe_tlbs = %d\n", p->num_of_valid_mpe_tlbs);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_invalid_mpe_tlbs = %d\n", p->num_of_invalid_mpe_tlbs);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_corrected_mpe_tlbs = %d\n", p->num_of_corrected_mpe_tlbs);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "ber_error_count = %d\n", p->ber_error_count);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "ber_bit_count = %d\n", p->ber_bit_count);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "sms_to_host_tx_errors = %d\n", p->sms_to_host_tx_errors);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "pre_ber = %d\n", p->pre_ber);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "cell_id = %d\n", p->cell_id);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "dvbh_srv_ind_hp = %d\n", p->dvbh_srv_ind_hp);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "dvbh_srv_ind_lp = %d\n", p->dvbh_srv_ind_lp);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_mpe_received = %d\n", p->num_mpe_received);
+
+	debug_data->stats_count = n;
+	spin_unlock(&debug_data->lock);
+	wake_up(&debug_data->stats_queue);
+}
+
+static void smsdvb_print_isdb_stats(struct smsdvb_debugfs *debug_data,
+			     struct sms_isdbt_stats *p)
+{
+	int i, n = 0;
+	char *buf;
+
+	spin_lock(&debug_data->lock);
+	if (debug_data->stats_count) {
+		spin_unlock(&debug_data->lock);
+		return;
+	}
+
+	buf = debug_data->stats_data;
+
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "statistics_type = %d\t", p->statistics_type);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "full_size = %d\n", p->full_size);
+
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_rf_locked = %d\t\t", p->is_rf_locked);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_demod_locked = %d\t", p->is_demod_locked);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_external_lna_on = %d\n", p->is_external_lna_on);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "SNR = %d dB\t\t", p->SNR);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "RSSI = %d dBm\t\t", p->RSSI);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "in_band_pwr = %d dBm\n", p->in_band_pwr);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "carrier_offset = %d\t", p->carrier_offset);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "bandwidth = %d\t\t", p->bandwidth);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "frequency = %d Hz\n", p->frequency);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "transmission_mode = %d\t", p->transmission_mode);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "modem_state = %d\t\t", p->modem_state);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "guard_interval = %d\n", p->guard_interval);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "system_type = %d\t\t", p->system_type);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "partial_reception = %d\t", p->partial_reception);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_layers = %d\n", p->num_of_layers);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "sms_to_host_tx_errors = %d\n", p->sms_to_host_tx_errors);
+
+	for (i = 0; i < 3; i++) {
+		if (p->layer_info[i].number_of_segments < 1 ||
+		    p->layer_info[i].number_of_segments > 13)
+			continue;
+
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\nLayer %d\n", i);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tcode_rate = %d\t",
+			      p->layer_info[i].code_rate);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "constellation = %d\n",
+			      p->layer_info[i].constellation);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tber = %-5d\t",
+			      p->layer_info[i].ber);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tber_error_count = %-5d\t",
+			      p->layer_info[i].ber_error_count);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "ber_bit_count = %-5d\n",
+			      p->layer_info[i].ber_bit_count);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tpre_ber = %-5d\t",
+			      p->layer_info[i].pre_ber);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tts_per = %-5d\n",
+			      p->layer_info[i].ts_per);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\terror_ts_packets = %-5d\t",
+			      p->layer_info[i].error_ts_packets);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "total_ts_packets = %-5d\t",
+			      p->layer_info[i].total_ts_packets);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "ti_ldepth_i = %d\n",
+			      p->layer_info[i].ti_ldepth_i);
+		n += snprintf(&buf[n], PAGE_SIZE - n,
+			      "\tnumber_of_segments = %d\t",
+			      p->layer_info[i].number_of_segments);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "tmcc_errors = %d\n",
+			      p->layer_info[i].tmcc_errors);
+	}
+
+	debug_data->stats_count = n;
+	spin_unlock(&debug_data->lock);
+	wake_up(&debug_data->stats_queue);
+}
+
+static void smsdvb_print_isdb_stats_ex(struct smsdvb_debugfs *debug_data,
+				struct sms_isdbt_stats_ex *p)
+{
+	int i, n = 0;
+	char *buf;
+
+	spin_lock(&debug_data->lock);
+	if (debug_data->stats_count) {
+		spin_unlock(&debug_data->lock);
+		return;
+	}
+
+	buf = debug_data->stats_data;
+
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "statistics_type = %d\t", p->statistics_type);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "full_size = %d\n", p->full_size);
+
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_rf_locked = %d\t\t", p->is_rf_locked);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_demod_locked = %d\t", p->is_demod_locked);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "is_external_lna_on = %d\n", p->is_external_lna_on);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "SNR = %d dB\t\t", p->SNR);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "RSSI = %d dBm\t\t", p->RSSI);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "in_band_pwr = %d dBm\n", p->in_band_pwr);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "carrier_offset = %d\t", p->carrier_offset);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "bandwidth = %d\t\t", p->bandwidth);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "frequency = %d Hz\n", p->frequency);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "transmission_mode = %d\t", p->transmission_mode);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "modem_state = %d\t\t", p->modem_state);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "guard_interval = %d\n", p->guard_interval);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "system_type = %d\t\t", p->system_type);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "partial_reception = %d\t", p->partial_reception);
+	n += snprintf(&buf[n], PAGE_SIZE - n,
+		      "num_of_layers = %d\n", p->num_of_layers);
+	n += snprintf(&buf[n], PAGE_SIZE - n, "segment_number = %d\t",
+		      p->segment_number);
+	n += snprintf(&buf[n], PAGE_SIZE - n, "tune_bw = %d\n",
+		      p->tune_bw);
+
+	for (i = 0; i < 3; i++) {
+		if (p->layer_info[i].number_of_segments < 1 ||
+		    p->layer_info[i].number_of_segments > 13)
+			continue;
+
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\nLayer %d\n", i);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tcode_rate = %d\t",
+			      p->layer_info[i].code_rate);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "constellation = %d\n",
+			      p->layer_info[i].constellation);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tber = %-5d\t",
+			      p->layer_info[i].ber);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tber_error_count = %-5d\t",
+			      p->layer_info[i].ber_error_count);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "ber_bit_count = %-5d\n",
+			      p->layer_info[i].ber_bit_count);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tpre_ber = %-5d\t",
+			      p->layer_info[i].pre_ber);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\tts_per = %-5d\n",
+			      p->layer_info[i].ts_per);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "\terror_ts_packets = %-5d\t",
+			      p->layer_info[i].error_ts_packets);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "total_ts_packets = %-5d\t",
+			      p->layer_info[i].total_ts_packets);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "ti_ldepth_i = %d\n",
+			      p->layer_info[i].ti_ldepth_i);
+		n += snprintf(&buf[n], PAGE_SIZE - n,
+			      "\tnumber_of_segments = %d\t",
+			      p->layer_info[i].number_of_segments);
+		n += snprintf(&buf[n], PAGE_SIZE - n, "tmcc_errors = %d\n",
+			      p->layer_info[i].tmcc_errors);
+	}
+
+
+	debug_data->stats_count = n;
+	spin_unlock(&debug_data->lock);
+
+	wake_up(&debug_data->stats_queue);
+}
+
+static int smsdvb_stats_open(struct inode *inode, struct file *file)
+{
+	struct smsdvb_client_t *client = inode->i_private;
+	struct smsdvb_debugfs *debug_data = client->debug_data;
+
+	kref_get(&debug_data->refcount);
+
+	spin_lock(&debug_data->lock);
+	debug_data->stats_count = 0;
+	debug_data->stats_was_read = false;
+	spin_unlock(&debug_data->lock);
+
+	file->private_data = debug_data;
+
+	return 0;
+}
+
+static void smsdvb_debugfs_data_release(struct kref *ref)
+{
+	struct smsdvb_debugfs *debug_data;
+
+	debug_data = container_of(ref, struct smsdvb_debugfs, refcount);
+	kfree(debug_data);
+}
+
+static int smsdvb_stats_wait_read(struct smsdvb_debugfs *debug_data)
+{
+	int rc = 1;
+
+	spin_lock(&debug_data->lock);
+
+	if (debug_data->stats_was_read)
+		goto exit;
+
+	rc = debug_data->stats_count;
+
+exit:
+	spin_unlock(&debug_data->lock);
+	return rc;
+}
+
+static unsigned int smsdvb_stats_poll(struct file *file, poll_table *wait)
+{
+	struct smsdvb_debugfs *debug_data = file->private_data;
+	int rc;
+
+	kref_get(&debug_data->refcount);
+
+	poll_wait(file, &debug_data->stats_queue, wait);
+
+	rc = smsdvb_stats_wait_read(debug_data);
+	if (rc > 0)
+		rc = POLLIN | POLLRDNORM;
+
+	kref_put(&debug_data->refcount, smsdvb_debugfs_data_release);
+
+	return rc;
+}
+
+static ssize_t smsdvb_stats_read(struct file *file, char __user *user_buf,
+				      size_t nbytes, loff_t *ppos)
+{
+	int rc = 0, len;
+	struct smsdvb_debugfs *debug_data = file->private_data;
+
+	kref_get(&debug_data->refcount);
+
+	if (file->f_flags & O_NONBLOCK) {
+		rc = smsdvb_stats_wait_read(debug_data);
+		if (!rc) {
+			rc = -EWOULDBLOCK;
+			goto ret;
+		}
+	} else {
+		rc = wait_event_interruptible(debug_data->stats_queue,
+				      smsdvb_stats_wait_read(debug_data));
+		if (rc < 0)
+			goto ret;
+	}
+
+	if (debug_data->stats_was_read) {
+		rc = 0;	/* EOF */
+		goto ret;
+	}
+
+	len = debug_data->stats_count - *ppos;
+	if (len >= 0)
+		rc = simple_read_from_buffer(user_buf, nbytes, ppos,
+					     debug_data->stats_data, len);
+	else
+		rc = 0;
+
+	if (*ppos >= debug_data->stats_count) {
+		spin_lock(&debug_data->lock);
+		debug_data->stats_was_read = true;
+		spin_unlock(&debug_data->lock);
+	}
+ret:
+	kref_put(&debug_data->refcount, smsdvb_debugfs_data_release);
+	return rc;
+}
+
+static int smsdvb_stats_release(struct inode *inode, struct file *file)
+{
+	struct smsdvb_debugfs *debug_data = file->private_data;
+
+	spin_lock(&debug_data->lock);
+	debug_data->stats_was_read = true;	/* return EOF to read() */
+	spin_unlock(&debug_data->lock);
+	wake_up_interruptible_sync(&debug_data->stats_queue);
+
+	kref_put(&debug_data->refcount, smsdvb_debugfs_data_release);
+	file->private_data = NULL;
+
+	return 0;
+}
+
+static const struct file_operations debugfs_stats_ops = {
+	.open = smsdvb_stats_open,
+	.poll = smsdvb_stats_poll,
+	.read = smsdvb_stats_read,
+	.release = smsdvb_stats_release,
+	.llseek = generic_file_llseek,
+};
+
+/*
+ * Functions used by smsdvb, in order to create the interfaces
+ */
+
+int smsdvb_debugfs_create(struct smsdvb_client_t *client)
+{
+	struct smscore_device_t *coredev = client->coredev;
+	struct dentry *d;
+	struct smsdvb_debugfs *debug_data;
+
+	if (!smsdvb_debugfs_usb_root || !coredev->is_usb_device)
+		return -ENODEV;
+
+	client->debugfs = debugfs_create_dir(coredev->devpath,
+					     smsdvb_debugfs_usb_root);
+	if (IS_ERR_OR_NULL(client->debugfs)) {
+		pr_info("Unable to create debugfs %s directory.\n",
+			coredev->devpath);
+		return -ENODEV;
+	}
+
+	d = debugfs_create_file("stats", S_IRUGO | S_IWUSR, client->debugfs,
+				client, &debugfs_stats_ops);
+	if (!d) {
+		debugfs_remove(client->debugfs);
+		return -ENOMEM;
+	}
+
+	debug_data = kzalloc(sizeof(*client->debug_data), GFP_KERNEL);
+	if (!debug_data)
+		return -ENOMEM;
+
+	client->debug_data        = debug_data;
+	client->prt_dvb_stats     = smsdvb_print_dvb_stats;
+	client->prt_isdb_stats    = smsdvb_print_isdb_stats;
+	client->prt_isdb_stats_ex = smsdvb_print_isdb_stats_ex;
+
+	init_waitqueue_head(&debug_data->stats_queue);
+	spin_lock_init(&debug_data->lock);
+	kref_init(&debug_data->refcount);
+
+	return 0;
+}
+
+void smsdvb_debugfs_release(struct smsdvb_client_t *client)
+{
+	if (!client->debugfs)
+		return;
+
+	client->prt_dvb_stats     = NULL;
+	client->prt_isdb_stats    = NULL;
+	client->prt_isdb_stats_ex = NULL;
+
+	debugfs_remove_recursive(client->debugfs);
+	kref_put(&client->debug_data->refcount, smsdvb_debugfs_data_release);
+
+	client->debug_data = NULL;
+	client->debugfs = NULL;
+}
+
+int smsdvb_debugfs_register(void)
+{
+	struct dentry *d;
+
+	/*
+	 * FIXME: This was written to debug Siano USB devices. So, it creates
+	 * the debugfs node under <debugfs>/usb.
+	 * A similar logic would be needed for Siano sdio devices, but, in that
+	 * case, usb_debug_root is not a good choice.
+	 *
+	 * Perhaps the right fix here would be to create another sysfs root
+	 * node for sdio-based boards, but this may need some logic at sdio
+	 * subsystem.
+	 */
+	d = debugfs_create_dir("smsdvb", usb_debug_root);
+	if (IS_ERR_OR_NULL(d)) {
+		sms_err("Couldn't create sysfs node for smsdvb");
+		return PTR_ERR(d);
+	} else {
+		smsdvb_debugfs_usb_root = d;
+	}
+	return 0;
+}
+
+void smsdvb_debugfs_unregister(void)
+{
+	debugfs_remove_recursive(smsdvb_debugfs_usb_root);
+	smsdvb_debugfs_usb_root = NULL;
+}
diff --git a/drivers/media/common/siano/smsdvb-main.c b/drivers/media/common/siano/smsdvb-main.c
new file mode 100644
index 000000000000..297f1b2f9a32
--- /dev/null
+++ b/drivers/media/common/siano/smsdvb-main.c
@@ -0,0 +1,1230 @@
+/****************************************************************
+
+Siano Mobile Silicon, Inc.
+MDTV receiver kernel modules.
+Copyright (C) 2006-2008, Uri Shkolnik
+
+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, see <http://www.gnu.org/licenses/>.
+
+****************************************************************/
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <asm/div64.h>
+
+#include "dmxdev.h"
+#include "dvbdev.h"
+#include "dvb_demux.h"
+#include "dvb_frontend.h"
+
+#include "smscoreapi.h"
+#include "sms-cards.h"
+
+#include "smsdvb.h"
+
+DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
+
+static struct list_head g_smsdvb_clients;
+static struct mutex g_smsdvb_clientslock;
+
+static int sms_dbg;
+module_param_named(debug, sms_dbg, int, 0644);
+MODULE_PARM_DESC(debug, "set debug level (info=1, adv=2 (or-able))");
+
+
+u32 sms_to_guard_interval_table[] = {
+	[0] = GUARD_INTERVAL_1_32,
+	[1] = GUARD_INTERVAL_1_16,
+	[2] = GUARD_INTERVAL_1_8,
+	[3] = GUARD_INTERVAL_1_4,
+};
+
+u32 sms_to_code_rate_table[] = {
+	[0] = FEC_1_2,
+	[1] = FEC_2_3,
+	[2] = FEC_3_4,
+	[3] = FEC_5_6,
+	[4] = FEC_7_8,
+};
+
+
+u32 sms_to_hierarchy_table[] = {
+	[0] = HIERARCHY_NONE,
+	[1] = HIERARCHY_1,
+	[2] = HIERARCHY_2,
+	[3] = HIERARCHY_4,
+};
+
+u32 sms_to_modulation_table[] = {
+	[0] = QPSK,
+	[1] = QAM_16,
+	[2] = QAM_64,
+	[3] = DQPSK,
+};
+
+
+/* Events that may come from DVB v3 adapter */
+static void sms_board_dvb3_event(struct smsdvb_client_t *client,
+		enum SMS_DVB3_EVENTS event) {
+
+	struct smscore_device_t *coredev = client->coredev;
+	switch (event) {
+	case DVB3_EVENT_INIT:
+		sms_debug("DVB3_EVENT_INIT");
+		sms_board_event(coredev, BOARD_EVENT_BIND);
+		break;
+	case DVB3_EVENT_SLEEP:
+		sms_debug("DVB3_EVENT_SLEEP");
+		sms_board_event(coredev, BOARD_EVENT_POWER_SUSPEND);
+		break;
+	case DVB3_EVENT_HOTPLUG:
+		sms_debug("DVB3_EVENT_HOTPLUG");
+		sms_board_event(coredev, BOARD_EVENT_POWER_INIT);
+		break;
+	case DVB3_EVENT_FE_LOCK:
+		if (client->event_fe_state != DVB3_EVENT_FE_LOCK) {
+			client->event_fe_state = DVB3_EVENT_FE_LOCK;
+			sms_debug("DVB3_EVENT_FE_LOCK");
+			sms_board_event(coredev, BOARD_EVENT_FE_LOCK);
+		}
+		break;
+	case DVB3_EVENT_FE_UNLOCK:
+		if (client->event_fe_state != DVB3_EVENT_FE_UNLOCK) {
+			client->event_fe_state = DVB3_EVENT_FE_UNLOCK;
+			sms_debug("DVB3_EVENT_FE_UNLOCK");
+			sms_board_event(coredev, BOARD_EVENT_FE_UNLOCK);
+		}
+		break;
+	case DVB3_EVENT_UNC_OK:
+		if (client->event_unc_state != DVB3_EVENT_UNC_OK) {
+			client->event_unc_state = DVB3_EVENT_UNC_OK;
+			sms_debug("DVB3_EVENT_UNC_OK");
+			sms_board_event(coredev, BOARD_EVENT_MULTIPLEX_OK);
+		}
+		break;
+	case DVB3_EVENT_UNC_ERR:
+		if (client->event_unc_state != DVB3_EVENT_UNC_ERR) {
+			client->event_unc_state = DVB3_EVENT_UNC_ERR;
+			sms_debug("DVB3_EVENT_UNC_ERR");
+			sms_board_event(coredev, BOARD_EVENT_MULTIPLEX_ERRORS);
+		}
+		break;
+
+	default:
+		sms_err("Unknown dvb3 api event");
+		break;
+	}
+}
+
+static void smsdvb_stats_not_ready(struct dvb_frontend *fe)
+{
+	struct smsdvb_client_t *client =
+		container_of(fe, struct smsdvb_client_t, frontend);
+	struct smscore_device_t *coredev = client->coredev;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int i, n_layers;
+
+	switch (smscore_get_device_mode(coredev)) {
+	case DEVICE_MODE_ISDBT:
+	case DEVICE_MODE_ISDBT_BDA:
+		n_layers = 4;
+	default:
+		n_layers = 1;
+	}
+
+	/* Global stats */
+	c->strength.len = 1;
+	c->cnr.len = 1;
+	c->strength.stat[0].scale = FE_SCALE_DECIBEL;
+	c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+
+	/* Per-layer stats */
+	c->post_bit_error.len = n_layers;
+	c->post_bit_count.len = n_layers;
+	c->block_error.len = n_layers;
+	c->block_count.len = n_layers;
+
+	/*
+	 * Put all of them at FE_SCALE_NOT_AVAILABLE. They're dynamically
+	 * changed when the stats become available.
+	 */
+	for (i = 0; i < n_layers; i++) {
+		c->post_bit_error.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
+		c->post_bit_count.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
+		c->block_error.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
+		c->block_count.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
+	}
+}
+
+static inline int sms_to_mode(u32 mode)
+{
+	switch (mode) {
+	case 2:
+		return TRANSMISSION_MODE_2K;
+	case 4:
+		return TRANSMISSION_MODE_4K;
+	case 8:
+		return TRANSMISSION_MODE_8K;
+	}
+	return TRANSMISSION_MODE_AUTO;
+}
+
+static inline int sms_to_status(u32 is_demod_locked, u32 is_rf_locked)
+{
+	if (is_demod_locked)
+		return FE_HAS_SIGNAL  | FE_HAS_CARRIER | FE_HAS_VITERBI |
+		       FE_HAS_SYNC    | FE_HAS_LOCK;
+
+	if (is_rf_locked)
+		return FE_HAS_SIGNAL | FE_HAS_CARRIER;
+
+	return 0;
+}
+
+static inline u32 sms_to_bw(u32 value)
+{
+	return value * 1000000;
+}
+
+#define convert_from_table(value, table, defval) ({			\
+	u32 __ret;							\
+	if (value < ARRAY_SIZE(table))					\
+		__ret = table[value];					\
+	else								\
+		__ret = defval;						\
+	__ret;								\
+})
+
+#define sms_to_guard_interval(value)					\
+	convert_from_table(value, sms_to_guard_interval_table,		\
+			   GUARD_INTERVAL_AUTO);
+
+#define sms_to_code_rate(value)						\
+	convert_from_table(value, sms_to_code_rate_table,		\
+			   FEC_NONE);
+
+#define sms_to_hierarchy(value)						\
+	convert_from_table(value, sms_to_hierarchy_table,		\
+			   FEC_NONE);
+
+#define sms_to_modulation(value)					\
+	convert_from_table(value, sms_to_modulation_table,		\
+			   FEC_NONE);
+
+static void smsdvb_update_tx_params(struct smsdvb_client_t *client,
+				    struct sms_tx_stats *p)
+{
+	struct dvb_frontend *fe = &client->frontend;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+	c->frequency = p->frequency;
+	client->fe_status = sms_to_status(p->is_demod_locked, 0);
+	c->bandwidth_hz = sms_to_bw(p->bandwidth);
+	c->transmission_mode = sms_to_mode(p->transmission_mode);
+	c->guard_interval = sms_to_guard_interval(p->guard_interval);
+	c->code_rate_HP = sms_to_code_rate(p->code_rate);
+	c->code_rate_LP = sms_to_code_rate(p->lp_code_rate);
+	c->hierarchy = sms_to_hierarchy(p->hierarchy);
+	c->modulation = sms_to_modulation(p->constellation);
+}
+
+static void smsdvb_update_per_slices(struct smsdvb_client_t *client,
+				     struct RECEPTION_STATISTICS_PER_SLICES_S *p)
+{
+	struct dvb_frontend *fe = &client->frontend;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	u64 tmp;
+
+	client->fe_status = sms_to_status(p->is_demod_locked, p->is_rf_locked);
+	c->modulation = sms_to_modulation(p->constellation);
+
+	/* signal Strength, in DBm */
+	c->strength.stat[0].uvalue = p->in_band_power * 1000;
+
+	/* Carrier to noise ratio, in DB */
+	c->cnr.stat[0].svalue = p->snr * 1000;
+
+	/* PER/BER requires demod lock */
+	if (!p->is_demod_locked)
+		return;
+
+	/* TS PER */
+	client->last_per = c->block_error.stat[0].uvalue;
+	c->block_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_error.stat[0].uvalue += p->ets_packets;
+	c->block_count.stat[0].uvalue += p->ets_packets + p->ts_packets;
+
+	/* ber */
+	c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_error.stat[0].uvalue += p->ber_error_count;
+	c->post_bit_count.stat[0].uvalue += p->ber_bit_count;
+
+	/* Legacy PER/BER */
+	tmp = p->ets_packets * 65535;
+	do_div(tmp, p->ts_packets + p->ets_packets);
+	client->legacy_per = tmp;
+}
+
+static void smsdvb_update_dvb_stats(struct smsdvb_client_t *client,
+				    struct sms_stats *p)
+{
+	struct dvb_frontend *fe = &client->frontend;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+	if (client->prt_dvb_stats)
+		client->prt_dvb_stats(client->debug_data, p);
+
+	client->fe_status = sms_to_status(p->is_demod_locked, p->is_rf_locked);
+
+	/* Update DVB modulation parameters */
+	c->frequency = p->frequency;
+	client->fe_status = sms_to_status(p->is_demod_locked, 0);
+	c->bandwidth_hz = sms_to_bw(p->bandwidth);
+	c->transmission_mode = sms_to_mode(p->transmission_mode);
+	c->guard_interval = sms_to_guard_interval(p->guard_interval);
+	c->code_rate_HP = sms_to_code_rate(p->code_rate);
+	c->code_rate_LP = sms_to_code_rate(p->lp_code_rate);
+	c->hierarchy = sms_to_hierarchy(p->hierarchy);
+	c->modulation = sms_to_modulation(p->constellation);
+
+	/* update reception data */
+	c->lna = p->is_external_lna_on ? 1 : 0;
+
+	/* Carrier to noise ratio, in DB */
+	c->cnr.stat[0].svalue = p->SNR * 1000;
+
+	/* signal Strength, in DBm */
+	c->strength.stat[0].uvalue = p->in_band_pwr * 1000;
+
+	/* PER/BER requires demod lock */
+	if (!p->is_demod_locked)
+		return;
+
+	/* TS PER */
+	client->last_per = c->block_error.stat[0].uvalue;
+	c->block_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_error.stat[0].uvalue += p->error_ts_packets;
+	c->block_count.stat[0].uvalue += p->total_ts_packets;
+
+	/* ber */
+	c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_error.stat[0].uvalue += p->ber_error_count;
+	c->post_bit_count.stat[0].uvalue += p->ber_bit_count;
+
+	/* Legacy PER/BER */
+	client->legacy_ber = p->ber;
+};
+
+static void smsdvb_update_isdbt_stats(struct smsdvb_client_t *client,
+				      struct sms_isdbt_stats *p)
+{
+	struct dvb_frontend *fe = &client->frontend;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct sms_isdbt_layer_stats *lr;
+	int i, n_layers;
+
+	if (client->prt_isdb_stats)
+		client->prt_isdb_stats(client->debug_data, p);
+
+	client->fe_status = sms_to_status(p->is_demod_locked, p->is_rf_locked);
+
+	/*
+	 * Firmware 2.1 seems to report only lock status and
+	 * signal strength. The signal strength indicator is at the
+	 * wrong field.
+	 */
+	if (p->statistics_type == 0) {
+		c->strength.stat[0].uvalue = ((s32)p->transmission_mode) * 1000;
+		c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		return;
+	}
+
+	/* Update ISDB-T transmission parameters */
+	c->frequency = p->frequency;
+	c->bandwidth_hz = sms_to_bw(p->bandwidth);
+	c->transmission_mode = sms_to_mode(p->transmission_mode);
+	c->guard_interval = sms_to_guard_interval(p->guard_interval);
+	c->isdbt_partial_reception = p->partial_reception ? 1 : 0;
+	n_layers = p->num_of_layers;
+	if (n_layers < 1)
+		n_layers = 1;
+	if (n_layers > 3)
+		n_layers = 3;
+	c->isdbt_layer_enabled = 0;
+
+	/* update reception data */
+	c->lna = p->is_external_lna_on ? 1 : 0;
+
+	/* Carrier to noise ratio, in DB */
+	c->cnr.stat[0].svalue = p->SNR * 1000;
+
+	/* signal Strength, in DBm */
+	c->strength.stat[0].uvalue = p->in_band_pwr * 1000;
+
+	/* PER/BER and per-layer stats require demod lock */
+	if (!p->is_demod_locked)
+		return;
+
+	client->last_per = c->block_error.stat[0].uvalue;
+
+	/* Clears global counters, as the code below will sum it again */
+	c->block_error.stat[0].uvalue = 0;
+	c->block_count.stat[0].uvalue = 0;
+	c->block_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_error.stat[0].uvalue = 0;
+	c->post_bit_count.stat[0].uvalue = 0;
+	c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+
+	for (i = 0; i < n_layers; i++) {
+		lr = &p->layer_info[i];
+
+		/* Update per-layer transmission parameters */
+		if (lr->number_of_segments > 0 && lr->number_of_segments < 13) {
+			c->isdbt_layer_enabled |= 1 << i;
+			c->layer[i].segment_count = lr->number_of_segments;
+		} else {
+			continue;
+		}
+		c->layer[i].modulation = sms_to_modulation(lr->constellation);
+
+		/* TS PER */
+		c->block_error.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->block_count.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->block_error.stat[i + 1].uvalue += lr->error_ts_packets;
+		c->block_count.stat[i + 1].uvalue += lr->total_ts_packets;
+
+		/* Update global PER counter */
+		c->block_error.stat[0].uvalue += lr->error_ts_packets;
+		c->block_count.stat[0].uvalue += lr->total_ts_packets;
+
+		/* BER */
+		c->post_bit_error.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->post_bit_count.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->post_bit_error.stat[i + 1].uvalue += lr->ber_error_count;
+		c->post_bit_count.stat[i + 1].uvalue += lr->ber_bit_count;
+
+		/* Update global BER counter */
+		c->post_bit_error.stat[0].uvalue += lr->ber_error_count;
+		c->post_bit_count.stat[0].uvalue += lr->ber_bit_count;
+	}
+}
+
+static void smsdvb_update_isdbt_stats_ex(struct smsdvb_client_t *client,
+					 struct sms_isdbt_stats_ex *p)
+{
+	struct dvb_frontend *fe = &client->frontend;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct sms_isdbt_layer_stats *lr;
+	int i, n_layers;
+
+	if (client->prt_isdb_stats_ex)
+		client->prt_isdb_stats_ex(client->debug_data, p);
+
+	/* Update ISDB-T transmission parameters */
+	c->frequency = p->frequency;
+	client->fe_status = sms_to_status(p->is_demod_locked, 0);
+	c->bandwidth_hz = sms_to_bw(p->bandwidth);
+	c->transmission_mode = sms_to_mode(p->transmission_mode);
+	c->guard_interval = sms_to_guard_interval(p->guard_interval);
+	c->isdbt_partial_reception = p->partial_reception ? 1 : 0;
+	n_layers = p->num_of_layers;
+	if (n_layers < 1)
+		n_layers = 1;
+	if (n_layers > 3)
+		n_layers = 3;
+	c->isdbt_layer_enabled = 0;
+
+	/* update reception data */
+	c->lna = p->is_external_lna_on ? 1 : 0;
+
+	/* Carrier to noise ratio, in DB */
+	c->cnr.stat[0].svalue = p->SNR * 1000;
+
+	/* signal Strength, in DBm */
+	c->strength.stat[0].uvalue = p->in_band_pwr * 1000;
+
+	/* PER/BER and per-layer stats require demod lock */
+	if (!p->is_demod_locked)
+		return;
+
+	client->last_per = c->block_error.stat[0].uvalue;
+
+	/* Clears global counters, as the code below will sum it again */
+	c->block_error.stat[0].uvalue = 0;
+	c->block_count.stat[0].uvalue = 0;
+	c->block_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_error.stat[0].uvalue = 0;
+	c->post_bit_count.stat[0].uvalue = 0;
+	c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+
+	c->post_bit_error.len = n_layers + 1;
+	c->post_bit_count.len = n_layers + 1;
+	c->block_error.len = n_layers + 1;
+	c->block_count.len = n_layers + 1;
+	for (i = 0; i < n_layers; i++) {
+		lr = &p->layer_info[i];
+
+		/* Update per-layer transmission parameters */
+		if (lr->number_of_segments > 0 && lr->number_of_segments < 13) {
+			c->isdbt_layer_enabled |= 1 << i;
+			c->layer[i].segment_count = lr->number_of_segments;
+		} else {
+			continue;
+		}
+		c->layer[i].modulation = sms_to_modulation(lr->constellation);
+
+		/* TS PER */
+		c->block_error.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->block_count.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->block_error.stat[i + 1].uvalue += lr->error_ts_packets;
+		c->block_count.stat[i + 1].uvalue += lr->total_ts_packets;
+
+		/* Update global PER counter */
+		c->block_error.stat[0].uvalue += lr->error_ts_packets;
+		c->block_count.stat[0].uvalue += lr->total_ts_packets;
+
+		/* ber */
+		c->post_bit_error.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->post_bit_count.stat[i + 1].scale = FE_SCALE_COUNTER;
+		c->post_bit_error.stat[i + 1].uvalue += lr->ber_error_count;
+		c->post_bit_count.stat[i + 1].uvalue += lr->ber_bit_count;
+
+		/* Update global ber counter */
+		c->post_bit_error.stat[0].uvalue += lr->ber_error_count;
+		c->post_bit_count.stat[0].uvalue += lr->ber_bit_count;
+	}
+}
+
+static int smsdvb_onresponse(void *context, struct smscore_buffer_t *cb)
+{
+	struct smsdvb_client_t *client = (struct smsdvb_client_t *) context;
+	struct sms_msg_hdr *phdr = (struct sms_msg_hdr *) (((u8 *) cb->p)
+			+ cb->offset);
+	void *p = phdr + 1;
+	struct dvb_frontend *fe = &client->frontend;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	bool is_status_update = false;
+
+	switch (phdr->msg_type) {
+	case MSG_SMS_DVBT_BDA_DATA:
+		/*
+		 * Only feed data to dvb demux if are there any feed listening
+		 * to it and if the device has tuned
+		 */
+		if (client->feed_users && client->has_tuned)
+			dvb_dmx_swfilter(&client->demux, p,
+					 cb->size - sizeof(struct sms_msg_hdr));
+		break;
+
+	case MSG_SMS_RF_TUNE_RES:
+	case MSG_SMS_ISDBT_TUNE_RES:
+		complete(&client->tune_done);
+		break;
+
+	case MSG_SMS_SIGNAL_DETECTED_IND:
+		client->fe_status = FE_HAS_SIGNAL  | FE_HAS_CARRIER |
+				    FE_HAS_VITERBI | FE_HAS_SYNC    |
+				    FE_HAS_LOCK;
+
+		is_status_update = true;
+		break;
+
+	case MSG_SMS_NO_SIGNAL_IND:
+		client->fe_status = 0;
+
+		is_status_update = true;
+		break;
+
+	case MSG_SMS_TRANSMISSION_IND:
+		smsdvb_update_tx_params(client, p);
+
+		is_status_update = true;
+		break;
+
+	case MSG_SMS_HO_PER_SLICES_IND:
+		smsdvb_update_per_slices(client, p);
+
+		is_status_update = true;
+		break;
+
+	case MSG_SMS_GET_STATISTICS_RES:
+		switch (smscore_get_device_mode(client->coredev)) {
+		case DEVICE_MODE_ISDBT:
+		case DEVICE_MODE_ISDBT_BDA:
+			smsdvb_update_isdbt_stats(client, p);
+			break;
+		default:
+			/* Skip sms_msg_statistics_info:request_result field */
+			smsdvb_update_dvb_stats(client, p + sizeof(u32));
+		}
+
+		is_status_update = true;
+		break;
+
+	/* Only for ISDB-T */
+	case MSG_SMS_GET_STATISTICS_EX_RES:
+		/* Skip sms_msg_statistics_info:request_result field? */
+		smsdvb_update_isdbt_stats_ex(client, p + sizeof(u32));
+		is_status_update = true;
+		break;
+	default:
+		sms_info("message not handled");
+	}
+	smscore_putbuffer(client->coredev, cb);
+
+	if (is_status_update) {
+		if (client->fe_status & FE_HAS_LOCK) {
+			sms_board_dvb3_event(client, DVB3_EVENT_FE_LOCK);
+			if (client->last_per == c->block_error.stat[0].uvalue)
+				sms_board_dvb3_event(client, DVB3_EVENT_UNC_OK);
+			else
+				sms_board_dvb3_event(client, DVB3_EVENT_UNC_ERR);
+			client->has_tuned = true;
+		} else {
+			smsdvb_stats_not_ready(fe);
+			client->has_tuned = false;
+			sms_board_dvb3_event(client, DVB3_EVENT_FE_UNLOCK);
+		}
+		complete(&client->stats_done);
+	}
+
+	return 0;
+}
+
+static void smsdvb_unregister_client(struct smsdvb_client_t *client)
+{
+	/* must be called under clientslock */
+
+	list_del(&client->entry);
+
+	smsdvb_debugfs_release(client);
+	smscore_unregister_client(client->smsclient);
+	dvb_unregister_frontend(&client->frontend);
+	dvb_dmxdev_release(&client->dmxdev);
+	dvb_dmx_release(&client->demux);
+	dvb_unregister_adapter(&client->adapter);
+	kfree(client);
+}
+
+static void smsdvb_onremove(void *context)
+{
+	kmutex_lock(&g_smsdvb_clientslock);
+
+	smsdvb_unregister_client((struct smsdvb_client_t *) context);
+
+	kmutex_unlock(&g_smsdvb_clientslock);
+}
+
+static int smsdvb_start_feed(struct dvb_demux_feed *feed)
+{
+	struct smsdvb_client_t *client =
+		container_of(feed->demux, struct smsdvb_client_t, demux);
+	struct sms_msg_data pid_msg;
+
+	sms_debug("add pid %d(%x)",
+		  feed->pid, feed->pid);
+
+	client->feed_users++;
+
+	pid_msg.x_msg_header.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	pid_msg.x_msg_header.msg_dst_id = HIF_TASK;
+	pid_msg.x_msg_header.msg_flags = 0;
+	pid_msg.x_msg_header.msg_type  = MSG_SMS_ADD_PID_FILTER_REQ;
+	pid_msg.x_msg_header.msg_length = sizeof(pid_msg);
+	pid_msg.msg_data[0] = feed->pid;
+
+	return smsclient_sendrequest(client->smsclient,
+				     &pid_msg, sizeof(pid_msg));
+}
+
+static int smsdvb_stop_feed(struct dvb_demux_feed *feed)
+{
+	struct smsdvb_client_t *client =
+		container_of(feed->demux, struct smsdvb_client_t, demux);
+	struct sms_msg_data pid_msg;
+
+	sms_debug("remove pid %d(%x)",
+		  feed->pid, feed->pid);
+
+	client->feed_users--;
+
+	pid_msg.x_msg_header.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	pid_msg.x_msg_header.msg_dst_id = HIF_TASK;
+	pid_msg.x_msg_header.msg_flags = 0;
+	pid_msg.x_msg_header.msg_type  = MSG_SMS_REMOVE_PID_FILTER_REQ;
+	pid_msg.x_msg_header.msg_length = sizeof(pid_msg);
+	pid_msg.msg_data[0] = feed->pid;
+
+	return smsclient_sendrequest(client->smsclient,
+				     &pid_msg, sizeof(pid_msg));
+}
+
+static int smsdvb_sendrequest_and_wait(struct smsdvb_client_t *client,
+					void *buffer, size_t size,
+					struct completion *completion)
+{
+	int rc;
+
+	rc = smsclient_sendrequest(client->smsclient, buffer, size);
+	if (rc < 0)
+		return rc;
+
+	return wait_for_completion_timeout(completion,
+					   msecs_to_jiffies(2000)) ?
+						0 : -ETIME;
+}
+
+static int smsdvb_send_statistics_request(struct smsdvb_client_t *client)
+{
+	int rc;
+	struct sms_msg_hdr msg;
+
+	/* Don't request stats too fast */
+	if (client->get_stats_jiffies &&
+	   (!time_after(jiffies, client->get_stats_jiffies)))
+		return 0;
+	client->get_stats_jiffies = jiffies + msecs_to_jiffies(100);
+
+	msg.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	msg.msg_dst_id = HIF_TASK;
+	msg.msg_flags = 0;
+	msg.msg_length = sizeof(msg);
+
+	switch (smscore_get_device_mode(client->coredev)) {
+	case DEVICE_MODE_ISDBT:
+	case DEVICE_MODE_ISDBT_BDA:
+		/*
+		* Check for firmware version, to avoid breaking for old cards
+		*/
+		if (client->coredev->fw_version >= 0x800)
+			msg.msg_type = MSG_SMS_GET_STATISTICS_EX_REQ;
+		else
+			msg.msg_type = MSG_SMS_GET_STATISTICS_REQ;
+		break;
+	default:
+		msg.msg_type = MSG_SMS_GET_STATISTICS_REQ;
+	}
+
+	rc = smsdvb_sendrequest_and_wait(client, &msg, sizeof(msg),
+					 &client->stats_done);
+
+	return rc;
+}
+
+static inline int led_feedback(struct smsdvb_client_t *client)
+{
+	if (!(client->fe_status & FE_HAS_LOCK))
+		return sms_board_led_feedback(client->coredev, SMS_LED_OFF);
+
+	return sms_board_led_feedback(client->coredev,
+				     (client->legacy_ber == 0) ?
+				     SMS_LED_HI : SMS_LED_LO);
+}
+
+static int smsdvb_read_status(struct dvb_frontend *fe, fe_status_t *stat)
+{
+	int rc;
+	struct smsdvb_client_t *client;
+	client = container_of(fe, struct smsdvb_client_t, frontend);
+
+	rc = smsdvb_send_statistics_request(client);
+
+	*stat = client->fe_status;
+
+	led_feedback(client);
+
+	return rc;
+}
+
+static int smsdvb_read_ber(struct dvb_frontend *fe, u32 *ber)
+{
+	int rc;
+	struct smsdvb_client_t *client;
+
+	client = container_of(fe, struct smsdvb_client_t, frontend);
+
+	rc = smsdvb_send_statistics_request(client);
+
+	*ber = client->legacy_ber;
+
+	led_feedback(client);
+
+	return rc;
+}
+
+static int smsdvb_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int rc;
+	s32 power = (s32) c->strength.stat[0].uvalue;
+	struct smsdvb_client_t *client;
+
+	client = container_of(fe, struct smsdvb_client_t, frontend);
+
+	rc = smsdvb_send_statistics_request(client);
+
+	if (power < -95)
+		*strength = 0;
+		else if (power > -29)
+			*strength = 65535;
+		else
+			*strength = (power + 95) * 65535 / 66;
+
+	led_feedback(client);
+
+	return rc;
+}
+
+static int smsdvb_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int rc;
+	struct smsdvb_client_t *client;
+
+	client = container_of(fe, struct smsdvb_client_t, frontend);
+
+	rc = smsdvb_send_statistics_request(client);
+
+	/* Preferred scale for SNR with legacy API: 0.1 dB */
+	*snr = ((u32)c->cnr.stat[0].svalue) / 100;
+
+	led_feedback(client);
+
+	return rc;
+}
+
+static int smsdvb_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
+{
+	int rc;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct smsdvb_client_t *client;
+
+	client = container_of(fe, struct smsdvb_client_t, frontend);
+
+	rc = smsdvb_send_statistics_request(client);
+
+	*ucblocks = c->block_error.stat[0].uvalue;
+
+	led_feedback(client);
+
+	return rc;
+}
+
+static int smsdvb_get_tune_settings(struct dvb_frontend *fe,
+				    struct dvb_frontend_tune_settings *tune)
+{
+	sms_debug("");
+
+	tune->min_delay_ms = 400;
+	tune->step_size = 250000;
+	tune->max_drift = 0;
+	return 0;
+}
+
+static int smsdvb_dvbt_set_frontend(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct smsdvb_client_t *client =
+		container_of(fe, struct smsdvb_client_t, frontend);
+
+	struct {
+		struct sms_msg_hdr	msg;
+		u32		Data[3];
+	} msg;
+
+	int ret;
+
+	client->fe_status = 0;
+	client->event_fe_state = -1;
+	client->event_unc_state = -1;
+	fe->dtv_property_cache.delivery_system = SYS_DVBT;
+
+	msg.msg.msg_src_id = DVBT_BDA_CONTROL_MSG_ID;
+	msg.msg.msg_dst_id = HIF_TASK;
+	msg.msg.msg_flags = 0;
+	msg.msg.msg_type = MSG_SMS_RF_TUNE_REQ;
+	msg.msg.msg_length = sizeof(msg);
+	msg.Data[0] = c->frequency;
+	msg.Data[2] = 12000000;
+
+	sms_info("%s: freq %d band %d", __func__, c->frequency,
+		 c->bandwidth_hz);
+
+	switch (c->bandwidth_hz / 1000000) {
+	case 8:
+		msg.Data[1] = BW_8_MHZ;
+		break;
+	case 7:
+		msg.Data[1] = BW_7_MHZ;
+		break;
+	case 6:
+		msg.Data[1] = BW_6_MHZ;
+		break;
+	case 0:
+		return -EOPNOTSUPP;
+	default:
+		return -EINVAL;
+	}
+	/* Disable LNA, if any. An error is returned if no LNA is present */
+	ret = sms_board_lna_control(client->coredev, 0);
+	if (ret == 0) {
+		fe_status_t status;
+
+		/* tune with LNA off at first */
+		ret = smsdvb_sendrequest_and_wait(client, &msg, sizeof(msg),
+						  &client->tune_done);
+
+		smsdvb_read_status(fe, &status);
+
+		if (status & FE_HAS_LOCK)
+			return ret;
+
+		/* previous tune didn't lock - enable LNA and tune again */
+		sms_board_lna_control(client->coredev, 1);
+	}
+
+	return smsdvb_sendrequest_and_wait(client, &msg, sizeof(msg),
+					   &client->tune_done);
+}
+
+static int smsdvb_isdbt_set_frontend(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct smsdvb_client_t *client =
+		container_of(fe, struct smsdvb_client_t, frontend);
+	int board_id = smscore_get_board_id(client->coredev);
+	struct sms_board *board = sms_get_board(board_id);
+	enum sms_device_type_st type = board->type;
+	int ret;
+
+	struct {
+		struct sms_msg_hdr	msg;
+		u32		Data[4];
+	} msg;
+
+	fe->dtv_property_cache.delivery_system = SYS_ISDBT;
+
+	msg.msg.msg_src_id  = DVBT_BDA_CONTROL_MSG_ID;
+	msg.msg.msg_dst_id  = HIF_TASK;
+	msg.msg.msg_flags  = 0;
+	msg.msg.msg_type   = MSG_SMS_ISDBT_TUNE_REQ;
+	msg.msg.msg_length = sizeof(msg);
+
+	if (c->isdbt_sb_segment_idx == -1)
+		c->isdbt_sb_segment_idx = 0;
+
+	if (!c->isdbt_layer_enabled)
+		c->isdbt_layer_enabled = 7;
+
+	msg.Data[0] = c->frequency;
+	msg.Data[1] = BW_ISDBT_1SEG;
+	msg.Data[2] = 12000000;
+	msg.Data[3] = c->isdbt_sb_segment_idx;
+
+	if (c->isdbt_partial_reception) {
+		if ((type == SMS_PELE || type == SMS_RIO) &&
+		    c->isdbt_sb_segment_count > 3)
+			msg.Data[1] = BW_ISDBT_13SEG;
+		else if (c->isdbt_sb_segment_count > 1)
+			msg.Data[1] = BW_ISDBT_3SEG;
+	} else if (type == SMS_PELE || type == SMS_RIO)
+		msg.Data[1] = BW_ISDBT_13SEG;
+
+	c->bandwidth_hz = 6000000;
+
+	sms_info("%s: freq %d segwidth %d segindex %d", __func__,
+		 c->frequency, c->isdbt_sb_segment_count,
+		 c->isdbt_sb_segment_idx);
+
+	/* Disable LNA, if any. An error is returned if no LNA is present */
+	ret = sms_board_lna_control(client->coredev, 0);
+	if (ret == 0) {
+		fe_status_t status;
+
+		/* tune with LNA off at first */
+		ret = smsdvb_sendrequest_and_wait(client, &msg, sizeof(msg),
+						  &client->tune_done);
+
+		smsdvb_read_status(fe, &status);
+
+		if (status & FE_HAS_LOCK)
+			return ret;
+
+		/* previous tune didn't lock - enable LNA and tune again */
+		sms_board_lna_control(client->coredev, 1);
+	}
+	return smsdvb_sendrequest_and_wait(client, &msg, sizeof(msg),
+					   &client->tune_done);
+}
+
+static int smsdvb_set_frontend(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct smsdvb_client_t *client =
+		container_of(fe, struct smsdvb_client_t, frontend);
+	struct smscore_device_t *coredev = client->coredev;
+
+	smsdvb_stats_not_ready(fe);
+	c->strength.stat[0].uvalue = 0;
+	c->cnr.stat[0].uvalue = 0;
+
+	client->has_tuned = false;
+
+	switch (smscore_get_device_mode(coredev)) {
+	case DEVICE_MODE_DVBT:
+	case DEVICE_MODE_DVBT_BDA:
+		return smsdvb_dvbt_set_frontend(fe);
+	case DEVICE_MODE_ISDBT:
+	case DEVICE_MODE_ISDBT_BDA:
+		return smsdvb_isdbt_set_frontend(fe);
+	default:
+		return -EINVAL;
+	}
+}
+
+/* Nothing to do here, as stats are automatically updated */
+static int smsdvb_get_frontend(struct dvb_frontend *fe)
+{
+	return 0;
+}
+
+static int smsdvb_init(struct dvb_frontend *fe)
+{
+	struct smsdvb_client_t *client =
+		container_of(fe, struct smsdvb_client_t, frontend);
+
+	sms_board_power(client->coredev, 1);
+
+	sms_board_dvb3_event(client, DVB3_EVENT_INIT);
+	return 0;
+}
+
+static int smsdvb_sleep(struct dvb_frontend *fe)
+{
+	struct smsdvb_client_t *client =
+		container_of(fe, struct smsdvb_client_t, frontend);
+
+	sms_board_led_feedback(client->coredev, SMS_LED_OFF);
+	sms_board_power(client->coredev, 0);
+
+	sms_board_dvb3_event(client, DVB3_EVENT_SLEEP);
+
+	return 0;
+}
+
+static void smsdvb_release(struct dvb_frontend *fe)
+{
+	/* do nothing */
+}
+
+static struct dvb_frontend_ops smsdvb_fe_ops = {
+	.info = {
+		.name			= "Siano Mobile Digital MDTV Receiver",
+		.frequency_min		= 44250000,
+		.frequency_max		= 867250000,
+		.frequency_stepsize	= 250000,
+		.caps = FE_CAN_INVERSION_AUTO |
+			FE_CAN_FEC_1_2 | FE_CAN_FEC_2_3 | FE_CAN_FEC_3_4 |
+			FE_CAN_FEC_5_6 | FE_CAN_FEC_7_8 | FE_CAN_FEC_AUTO |
+			FE_CAN_QPSK | FE_CAN_QAM_16 | FE_CAN_QAM_64 |
+			FE_CAN_QAM_AUTO | FE_CAN_TRANSMISSION_MODE_AUTO |
+			FE_CAN_GUARD_INTERVAL_AUTO |
+			FE_CAN_RECOVER |
+			FE_CAN_HIERARCHY_AUTO,
+	},
+
+	.release = smsdvb_release,
+
+	.set_frontend = smsdvb_set_frontend,
+	.get_frontend = smsdvb_get_frontend,
+	.get_tune_settings = smsdvb_get_tune_settings,
+
+	.read_status = smsdvb_read_status,
+	.read_ber = smsdvb_read_ber,
+	.read_signal_strength = smsdvb_read_signal_strength,
+	.read_snr = smsdvb_read_snr,
+	.read_ucblocks = smsdvb_read_ucblocks,
+
+	.init = smsdvb_init,
+	.sleep = smsdvb_sleep,
+};
+
+static int smsdvb_hotplug(struct smscore_device_t *coredev,
+			  struct device *device, int arrival)
+{
+	struct smsclient_params_t params;
+	struct smsdvb_client_t *client;
+	int rc;
+
+	/* device removal handled by onremove callback */
+	if (!arrival)
+		return 0;
+	client = kzalloc(sizeof(struct smsdvb_client_t), GFP_KERNEL);
+	if (!client) {
+		sms_err("kmalloc() failed");
+		return -ENOMEM;
+	}
+
+	/* register dvb adapter */
+	rc = dvb_register_adapter(&client->adapter,
+				  sms_get_board(
+					smscore_get_board_id(coredev))->name,
+				  THIS_MODULE, device, adapter_nr);
+	if (rc < 0) {
+		sms_err("dvb_register_adapter() failed %d", rc);
+		goto adapter_error;
+	}
+
+	/* init dvb demux */
+	client->demux.dmx.capabilities = DMX_TS_FILTERING;
+	client->demux.filternum = 32; /* todo: nova ??? */
+	client->demux.feednum = 32;
+	client->demux.start_feed = smsdvb_start_feed;
+	client->demux.stop_feed = smsdvb_stop_feed;
+
+	rc = dvb_dmx_init(&client->demux);
+	if (rc < 0) {
+		sms_err("dvb_dmx_init failed %d", rc);
+		goto dvbdmx_error;
+	}
+
+	/* init dmxdev */
+	client->dmxdev.filternum = 32;
+	client->dmxdev.demux = &client->demux.dmx;
+	client->dmxdev.capabilities = 0;
+
+	rc = dvb_dmxdev_init(&client->dmxdev, &client->adapter);
+	if (rc < 0) {
+		sms_err("dvb_dmxdev_init failed %d", rc);
+		goto dmxdev_error;
+	}
+
+	/* init and register frontend */
+	memcpy(&client->frontend.ops, &smsdvb_fe_ops,
+	       sizeof(struct dvb_frontend_ops));
+
+	switch (smscore_get_device_mode(coredev)) {
+	case DEVICE_MODE_DVBT:
+	case DEVICE_MODE_DVBT_BDA:
+		client->frontend.ops.delsys[0] = SYS_DVBT;
+		break;
+	case DEVICE_MODE_ISDBT:
+	case DEVICE_MODE_ISDBT_BDA:
+		client->frontend.ops.delsys[0] = SYS_ISDBT;
+		break;
+	}
+
+	rc = dvb_register_frontend(&client->adapter, &client->frontend);
+	if (rc < 0) {
+		sms_err("frontend registration failed %d", rc);
+		goto frontend_error;
+	}
+
+	params.initial_id = 1;
+	params.data_type = MSG_SMS_DVBT_BDA_DATA;
+	params.onresponse_handler = smsdvb_onresponse;
+	params.onremove_handler = smsdvb_onremove;
+	params.context = client;
+
+	rc = smscore_register_client(coredev, &params, &client->smsclient);
+	if (rc < 0) {
+		sms_err("smscore_register_client() failed %d", rc);
+		goto client_error;
+	}
+
+	client->coredev = coredev;
+
+	init_completion(&client->tune_done);
+	init_completion(&client->stats_done);
+
+	kmutex_lock(&g_smsdvb_clientslock);
+
+	list_add(&client->entry, &g_smsdvb_clients);
+
+	kmutex_unlock(&g_smsdvb_clientslock);
+
+	client->event_fe_state = -1;
+	client->event_unc_state = -1;
+	sms_board_dvb3_event(client, DVB3_EVENT_HOTPLUG);
+
+	sms_info("success");
+	sms_board_setup(coredev);
+
+	if (smsdvb_debugfs_create(client) < 0)
+		sms_info("failed to create debugfs node");
+
+	return 0;
+
+client_error:
+	dvb_unregister_frontend(&client->frontend);
+
+frontend_error:
+	dvb_dmxdev_release(&client->dmxdev);
+
+dmxdev_error:
+	dvb_dmx_release(&client->demux);
+
+dvbdmx_error:
+	dvb_unregister_adapter(&client->adapter);
+
+adapter_error:
+	kfree(client);
+	return rc;
+}
+
+static int __init smsdvb_module_init(void)
+{
+	int rc;
+
+	INIT_LIST_HEAD(&g_smsdvb_clients);
+	kmutex_init(&g_smsdvb_clientslock);
+
+	smsdvb_debugfs_register();
+
+	rc = smscore_register_hotplug(smsdvb_hotplug);
+
+	sms_debug("");
+
+	return rc;
+}
+
+static void __exit smsdvb_module_exit(void)
+{
+	smscore_unregister_hotplug(smsdvb_hotplug);
+
+	kmutex_lock(&g_smsdvb_clientslock);
+
+	while (!list_empty(&g_smsdvb_clients))
+		smsdvb_unregister_client((struct smsdvb_client_t *)g_smsdvb_clients.next);
+
+	smsdvb_debugfs_unregister();
+
+	kmutex_unlock(&g_smsdvb_clientslock);
+}
+
+module_init(smsdvb_module_init);
+module_exit(smsdvb_module_exit);
+
+MODULE_DESCRIPTION("SMS DVB subsystem adaptation module");
+MODULE_AUTHOR("Siano Mobile Silicon, Inc. (uris@siano-ms.com)");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/common/siano/smsdvb.c b/drivers/media/common/siano/smsdvb.c
deleted file mode 100644
index aa77e54a8fae..000000000000
--- a/drivers/media/common/siano/smsdvb.c
+++ /dev/null
@@ -1,1078 +0,0 @@
-/****************************************************************
-
-Siano Mobile Silicon, Inc.
-MDTV receiver kernel modules.
-Copyright (C) 2006-2008, Uri Shkolnik
-
-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, see <http://www.gnu.org/licenses/>.
-
-****************************************************************/
-
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/init.h>
-
-#include "dmxdev.h"
-#include "dvbdev.h"
-#include "dvb_demux.h"
-#include "dvb_frontend.h"
-
-#include "smscoreapi.h"
-#include "smsendian.h"
-#include "sms-cards.h"
-
-DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
-
-struct smsdvb_client_t {
-	struct list_head entry;
-
-	struct smscore_device_t *coredev;
-	struct smscore_client_t *smsclient;
-
-	struct dvb_adapter      adapter;
-	struct dvb_demux        demux;
-	struct dmxdev           dmxdev;
-	struct dvb_frontend     frontend;
-
-	fe_status_t             fe_status;
-
-	struct completion       tune_done;
-
-	struct SMSHOSTLIB_STATISTICS_DVB_S sms_stat_dvb;
-	int event_fe_state;
-	int event_unc_state;
-};
-
-static struct list_head g_smsdvb_clients;
-static struct mutex g_smsdvb_clientslock;
-
-static int sms_dbg;
-module_param_named(debug, sms_dbg, int, 0644);
-MODULE_PARM_DESC(debug, "set debug level (info=1, adv=2 (or-able))");
-
-/* Events that may come from DVB v3 adapter */
-static void sms_board_dvb3_event(struct smsdvb_client_t *client,
-		enum SMS_DVB3_EVENTS event) {
-
-	struct smscore_device_t *coredev = client->coredev;
-	switch (event) {
-	case DVB3_EVENT_INIT:
-		sms_debug("DVB3_EVENT_INIT");
-		sms_board_event(coredev, BOARD_EVENT_BIND);
-		break;
-	case DVB3_EVENT_SLEEP:
-		sms_debug("DVB3_EVENT_SLEEP");
-		sms_board_event(coredev, BOARD_EVENT_POWER_SUSPEND);
-		break;
-	case DVB3_EVENT_HOTPLUG:
-		sms_debug("DVB3_EVENT_HOTPLUG");
-		sms_board_event(coredev, BOARD_EVENT_POWER_INIT);
-		break;
-	case DVB3_EVENT_FE_LOCK:
-		if (client->event_fe_state != DVB3_EVENT_FE_LOCK) {
-			client->event_fe_state = DVB3_EVENT_FE_LOCK;
-			sms_debug("DVB3_EVENT_FE_LOCK");
-			sms_board_event(coredev, BOARD_EVENT_FE_LOCK);
-		}
-		break;
-	case DVB3_EVENT_FE_UNLOCK:
-		if (client->event_fe_state != DVB3_EVENT_FE_UNLOCK) {
-			client->event_fe_state = DVB3_EVENT_FE_UNLOCK;
-			sms_debug("DVB3_EVENT_FE_UNLOCK");
-			sms_board_event(coredev, BOARD_EVENT_FE_UNLOCK);
-		}
-		break;
-	case DVB3_EVENT_UNC_OK:
-		if (client->event_unc_state != DVB3_EVENT_UNC_OK) {
-			client->event_unc_state = DVB3_EVENT_UNC_OK;
-			sms_debug("DVB3_EVENT_UNC_OK");
-			sms_board_event(coredev, BOARD_EVENT_MULTIPLEX_OK);
-		}
-		break;
-	case DVB3_EVENT_UNC_ERR:
-		if (client->event_unc_state != DVB3_EVENT_UNC_ERR) {
-			client->event_unc_state = DVB3_EVENT_UNC_ERR;
-			sms_debug("DVB3_EVENT_UNC_ERR");
-			sms_board_event(coredev, BOARD_EVENT_MULTIPLEX_ERRORS);
-		}
-		break;
-
-	default:
-		sms_err("Unknown dvb3 api event");
-		break;
-	}
-}
-
-
-static void smsdvb_update_dvb_stats(struct RECEPTION_STATISTICS_S *pReceptionData,
-				   struct SMSHOSTLIB_STATISTICS_ST *p)
-{
-	if (sms_dbg & 2) {
-		printk(KERN_DEBUG "Reserved = %d", p->Reserved);
-		printk(KERN_DEBUG "IsRfLocked = %d", p->IsRfLocked);
-		printk(KERN_DEBUG "IsDemodLocked = %d", p->IsDemodLocked);
-		printk(KERN_DEBUG "IsExternalLNAOn = %d", p->IsExternalLNAOn);
-		printk(KERN_DEBUG "SNR = %d", p->SNR);
-		printk(KERN_DEBUG "BER = %d", p->BER);
-		printk(KERN_DEBUG "FIB_CRC = %d", p->FIB_CRC);
-		printk(KERN_DEBUG "TS_PER = %d", p->TS_PER);
-		printk(KERN_DEBUG "MFER = %d", p->MFER);
-		printk(KERN_DEBUG "RSSI = %d", p->RSSI);
-		printk(KERN_DEBUG "InBandPwr = %d", p->InBandPwr);
-		printk(KERN_DEBUG "CarrierOffset = %d", p->CarrierOffset);
-		printk(KERN_DEBUG "Frequency = %d", p->Frequency);
-		printk(KERN_DEBUG "Bandwidth = %d", p->Bandwidth);
-		printk(KERN_DEBUG "TransmissionMode = %d", p->TransmissionMode);
-		printk(KERN_DEBUG "ModemState = %d", p->ModemState);
-		printk(KERN_DEBUG "GuardInterval = %d", p->GuardInterval);
-		printk(KERN_DEBUG "CodeRate = %d", p->CodeRate);
-		printk(KERN_DEBUG "LPCodeRate = %d", p->LPCodeRate);
-		printk(KERN_DEBUG "Hierarchy = %d", p->Hierarchy);
-		printk(KERN_DEBUG "Constellation = %d", p->Constellation);
-		printk(KERN_DEBUG "BurstSize = %d", p->BurstSize);
-		printk(KERN_DEBUG "BurstDuration = %d", p->BurstDuration);
-		printk(KERN_DEBUG "BurstCycleTime = %d", p->BurstCycleTime);
-		printk(KERN_DEBUG "CalculatedBurstCycleTime = %d", p->CalculatedBurstCycleTime);
-		printk(KERN_DEBUG "NumOfRows = %d", p->NumOfRows);
-		printk(KERN_DEBUG "NumOfPaddCols = %d", p->NumOfPaddCols);
-		printk(KERN_DEBUG "NumOfPunctCols = %d", p->NumOfPunctCols);
-		printk(KERN_DEBUG "ErrorTSPackets = %d", p->ErrorTSPackets);
-		printk(KERN_DEBUG "TotalTSPackets = %d", p->TotalTSPackets);
-		printk(KERN_DEBUG "NumOfValidMpeTlbs = %d", p->NumOfValidMpeTlbs);
-		printk(KERN_DEBUG "NumOfInvalidMpeTlbs = %d", p->NumOfInvalidMpeTlbs);
-		printk(KERN_DEBUG "NumOfCorrectedMpeTlbs = %d", p->NumOfCorrectedMpeTlbs);
-		printk(KERN_DEBUG "BERErrorCount = %d", p->BERErrorCount);
-		printk(KERN_DEBUG "BERBitCount = %d", p->BERBitCount);
-		printk(KERN_DEBUG "SmsToHostTxErrors = %d", p->SmsToHostTxErrors);
-		printk(KERN_DEBUG "PreBER = %d", p->PreBER);
-		printk(KERN_DEBUG "CellId = %d", p->CellId);
-		printk(KERN_DEBUG "DvbhSrvIndHP = %d", p->DvbhSrvIndHP);
-		printk(KERN_DEBUG "DvbhSrvIndLP = %d", p->DvbhSrvIndLP);
-		printk(KERN_DEBUG "NumMPEReceived = %d", p->NumMPEReceived);
-	}
-
-	pReceptionData->IsDemodLocked = p->IsDemodLocked;
-
-	pReceptionData->SNR = p->SNR;
-	pReceptionData->BER = p->BER;
-	pReceptionData->BERErrorCount = p->BERErrorCount;
-	pReceptionData->InBandPwr = p->InBandPwr;
-	pReceptionData->ErrorTSPackets = p->ErrorTSPackets;
-};
-
-
-static void smsdvb_update_isdbt_stats(struct RECEPTION_STATISTICS_S *pReceptionData,
-				    struct SMSHOSTLIB_STATISTICS_ISDBT_ST *p)
-{
-	int i;
-
-	if (sms_dbg & 2) {
-		printk(KERN_DEBUG "IsRfLocked = %d", p->IsRfLocked);
-		printk(KERN_DEBUG "IsDemodLocked = %d", p->IsDemodLocked);
-		printk(KERN_DEBUG "IsExternalLNAOn = %d", p->IsExternalLNAOn);
-		printk(KERN_DEBUG "SNR = %d", p->SNR);
-		printk(KERN_DEBUG "RSSI = %d", p->RSSI);
-		printk(KERN_DEBUG "InBandPwr = %d", p->InBandPwr);
-		printk(KERN_DEBUG "CarrierOffset = %d", p->CarrierOffset);
-		printk(KERN_DEBUG "Frequency = %d", p->Frequency);
-		printk(KERN_DEBUG "Bandwidth = %d", p->Bandwidth);
-		printk(KERN_DEBUG "TransmissionMode = %d", p->TransmissionMode);
-		printk(KERN_DEBUG "ModemState = %d", p->ModemState);
-		printk(KERN_DEBUG "GuardInterval = %d", p->GuardInterval);
-		printk(KERN_DEBUG "SystemType = %d", p->SystemType);
-		printk(KERN_DEBUG "PartialReception = %d", p->PartialReception);
-		printk(KERN_DEBUG "NumOfLayers = %d", p->NumOfLayers);
-		printk(KERN_DEBUG "SmsToHostTxErrors = %d", p->SmsToHostTxErrors);
-
-		for (i = 0; i < 3; i++) {
-			printk(KERN_DEBUG "%d: CodeRate = %d", i, p->LayerInfo[i].CodeRate);
-			printk(KERN_DEBUG "%d: Constellation = %d", i, p->LayerInfo[i].Constellation);
-			printk(KERN_DEBUG "%d: BER = %d", i, p->LayerInfo[i].BER);
-			printk(KERN_DEBUG "%d: BERErrorCount = %d", i, p->LayerInfo[i].BERErrorCount);
-			printk(KERN_DEBUG "%d: BERBitCount = %d", i, p->LayerInfo[i].BERBitCount);
-			printk(KERN_DEBUG "%d: PreBER = %d", i, p->LayerInfo[i].PreBER);
-			printk(KERN_DEBUG "%d: TS_PER = %d", i, p->LayerInfo[i].TS_PER);
-			printk(KERN_DEBUG "%d: ErrorTSPackets = %d", i, p->LayerInfo[i].ErrorTSPackets);
-			printk(KERN_DEBUG "%d: TotalTSPackets = %d", i, p->LayerInfo[i].TotalTSPackets);
-			printk(KERN_DEBUG "%d: TILdepthI = %d", i, p->LayerInfo[i].TILdepthI);
-			printk(KERN_DEBUG "%d: NumberOfSegments = %d", i, p->LayerInfo[i].NumberOfSegments);
-			printk(KERN_DEBUG "%d: TMCCErrors = %d", i, p->LayerInfo[i].TMCCErrors);
-		}
-	}
-
-	pReceptionData->IsDemodLocked = p->IsDemodLocked;
-
-	pReceptionData->SNR = p->SNR;
-	pReceptionData->InBandPwr = p->InBandPwr;
-
-	pReceptionData->ErrorTSPackets = 0;
-	pReceptionData->BER = 0;
-	pReceptionData->BERErrorCount = 0;
-	for (i = 0; i < 3; i++) {
-		pReceptionData->BER += p->LayerInfo[i].BER;
-		pReceptionData->BERErrorCount += p->LayerInfo[i].BERErrorCount;
-		pReceptionData->ErrorTSPackets += p->LayerInfo[i].ErrorTSPackets;
-	}
-}
-
-static int smsdvb_onresponse(void *context, struct smscore_buffer_t *cb)
-{
-	struct smsdvb_client_t *client = (struct smsdvb_client_t *) context;
-	struct SmsMsgHdr_ST *phdr = (struct SmsMsgHdr_ST *) (((u8 *) cb->p)
-			+ cb->offset);
-	u32 *pMsgData = (u32 *) phdr + 1;
-	/*u32 MsgDataLen = phdr->msgLength - sizeof(struct SmsMsgHdr_ST);*/
-	bool is_status_update = false;
-
-	smsendian_handle_rx_message((struct SmsMsgData_ST *) phdr);
-
-	switch (phdr->msgType) {
-	case MSG_SMS_DVBT_BDA_DATA:
-		dvb_dmx_swfilter(&client->demux, (u8 *)(phdr + 1),
-				 cb->size - sizeof(struct SmsMsgHdr_ST));
-		break;
-
-	case MSG_SMS_RF_TUNE_RES:
-	case MSG_SMS_ISDBT_TUNE_RES:
-		complete(&client->tune_done);
-		break;
-
-	case MSG_SMS_SIGNAL_DETECTED_IND:
-		sms_info("MSG_SMS_SIGNAL_DETECTED_IND");
-		client->sms_stat_dvb.TransmissionData.IsDemodLocked = true;
-		is_status_update = true;
-		break;
-
-	case MSG_SMS_NO_SIGNAL_IND:
-		sms_info("MSG_SMS_NO_SIGNAL_IND");
-		client->sms_stat_dvb.TransmissionData.IsDemodLocked = false;
-		is_status_update = true;
-		break;
-
-	case MSG_SMS_TRANSMISSION_IND: {
-		sms_info("MSG_SMS_TRANSMISSION_IND");
-
-		pMsgData++;
-		memcpy(&client->sms_stat_dvb.TransmissionData, pMsgData,
-				sizeof(struct TRANSMISSION_STATISTICS_S));
-
-		/* Mo need to correct guard interval
-		 * (as opposed to old statistics message).
-		 */
-		CORRECT_STAT_BANDWIDTH(client->sms_stat_dvb.TransmissionData);
-		CORRECT_STAT_TRANSMISSON_MODE(
-				client->sms_stat_dvb.TransmissionData);
-		is_status_update = true;
-		break;
-	}
-	case MSG_SMS_HO_PER_SLICES_IND: {
-		struct RECEPTION_STATISTICS_S *pReceptionData =
-				&client->sms_stat_dvb.ReceptionData;
-		struct SRVM_SIGNAL_STATUS_S SignalStatusData;
-
-		/*sms_info("MSG_SMS_HO_PER_SLICES_IND");*/
-		pMsgData++;
-		SignalStatusData.result = pMsgData[0];
-		SignalStatusData.snr = pMsgData[1];
-		SignalStatusData.inBandPower = (s32) pMsgData[2];
-		SignalStatusData.tsPackets = pMsgData[3];
-		SignalStatusData.etsPackets = pMsgData[4];
-		SignalStatusData.constellation = pMsgData[5];
-		SignalStatusData.hpCode = pMsgData[6];
-		SignalStatusData.tpsSrvIndLP = pMsgData[7] & 0x03;
-		SignalStatusData.tpsSrvIndHP = pMsgData[8] & 0x03;
-		SignalStatusData.cellId = pMsgData[9] & 0xFFFF;
-		SignalStatusData.reason = pMsgData[10];
-		SignalStatusData.requestId = pMsgData[11];
-		pReceptionData->IsRfLocked = pMsgData[16];
-		pReceptionData->IsDemodLocked = pMsgData[17];
-		pReceptionData->ModemState = pMsgData[12];
-		pReceptionData->SNR = pMsgData[1];
-		pReceptionData->BER = pMsgData[13];
-		pReceptionData->RSSI = pMsgData[14];
-		CORRECT_STAT_RSSI(client->sms_stat_dvb.ReceptionData);
-
-		pReceptionData->InBandPwr = (s32) pMsgData[2];
-		pReceptionData->CarrierOffset = (s32) pMsgData[15];
-		pReceptionData->TotalTSPackets = pMsgData[3];
-		pReceptionData->ErrorTSPackets = pMsgData[4];
-
-		/* TS PER */
-		if ((SignalStatusData.tsPackets + SignalStatusData.etsPackets)
-				> 0) {
-			pReceptionData->TS_PER = (SignalStatusData.etsPackets
-					* 100) / (SignalStatusData.tsPackets
-					+ SignalStatusData.etsPackets);
-		} else {
-			pReceptionData->TS_PER = 0;
-		}
-
-		pReceptionData->BERBitCount = pMsgData[18];
-		pReceptionData->BERErrorCount = pMsgData[19];
-
-		pReceptionData->MRC_SNR = pMsgData[20];
-		pReceptionData->MRC_InBandPwr = pMsgData[21];
-		pReceptionData->MRC_RSSI = pMsgData[22];
-
-		is_status_update = true;
-		break;
-	}
-	case MSG_SMS_GET_STATISTICS_RES: {
-		union {
-			struct SMSHOSTLIB_STATISTICS_ISDBT_ST  isdbt;
-			struct SmsMsgStatisticsInfo_ST         dvb;
-		} *p = (void *) (phdr + 1);
-		struct RECEPTION_STATISTICS_S *pReceptionData =
-				&client->sms_stat_dvb.ReceptionData;
-
-		sms_info("MSG_SMS_GET_STATISTICS_RES");
-
-		is_status_update = true;
-
-		switch (smscore_get_device_mode(client->coredev)) {
-		case DEVICE_MODE_ISDBT:
-		case DEVICE_MODE_ISDBT_BDA:
-			smsdvb_update_isdbt_stats(pReceptionData, &p->isdbt);
-			break;
-		default:
-			smsdvb_update_dvb_stats(pReceptionData, &p->dvb.Stat);
-		}
-		if (!pReceptionData->IsDemodLocked) {
-			pReceptionData->SNR = 0;
-			pReceptionData->BER = 0;
-			pReceptionData->BERErrorCount = 0;
-			pReceptionData->InBandPwr = 0;
-			pReceptionData->ErrorTSPackets = 0;
-		}
-
-		complete(&client->tune_done);
-		break;
-	}
-	default:
-		sms_info("Unhandled message %d", phdr->msgType);
-
-	}
-	smscore_putbuffer(client->coredev, cb);
-
-	if (is_status_update) {
-		if (client->sms_stat_dvb.ReceptionData.IsDemodLocked) {
-			client->fe_status = FE_HAS_SIGNAL | FE_HAS_CARRIER
-				| FE_HAS_VITERBI | FE_HAS_SYNC | FE_HAS_LOCK;
-			sms_board_dvb3_event(client, DVB3_EVENT_FE_LOCK);
-			if (client->sms_stat_dvb.ReceptionData.ErrorTSPackets
-					== 0)
-				sms_board_dvb3_event(client, DVB3_EVENT_UNC_OK);
-			else
-				sms_board_dvb3_event(client,
-						DVB3_EVENT_UNC_ERR);
-
-		} else {
-			if (client->sms_stat_dvb.ReceptionData.IsRfLocked)
-				client->fe_status = FE_HAS_SIGNAL | FE_HAS_CARRIER;
-			else
-				client->fe_status = 0;
-			sms_board_dvb3_event(client, DVB3_EVENT_FE_UNLOCK);
-		}
-	}
-
-	return 0;
-}
-
-static void smsdvb_unregister_client(struct smsdvb_client_t *client)
-{
-	/* must be called under clientslock */
-
-	list_del(&client->entry);
-
-	smscore_unregister_client(client->smsclient);
-	dvb_unregister_frontend(&client->frontend);
-	dvb_dmxdev_release(&client->dmxdev);
-	dvb_dmx_release(&client->demux);
-	dvb_unregister_adapter(&client->adapter);
-	kfree(client);
-}
-
-static void smsdvb_onremove(void *context)
-{
-	kmutex_lock(&g_smsdvb_clientslock);
-
-	smsdvb_unregister_client((struct smsdvb_client_t *) context);
-
-	kmutex_unlock(&g_smsdvb_clientslock);
-}
-
-static int smsdvb_start_feed(struct dvb_demux_feed *feed)
-{
-	struct smsdvb_client_t *client =
-		container_of(feed->demux, struct smsdvb_client_t, demux);
-	struct SmsMsgData_ST PidMsg;
-
-	sms_debug("add pid %d(%x)",
-		  feed->pid, feed->pid);
-
-	PidMsg.xMsgHeader.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-	PidMsg.xMsgHeader.msgDstId = HIF_TASK;
-	PidMsg.xMsgHeader.msgFlags = 0;
-	PidMsg.xMsgHeader.msgType  = MSG_SMS_ADD_PID_FILTER_REQ;
-	PidMsg.xMsgHeader.msgLength = sizeof(PidMsg);
-	PidMsg.msgData[0] = feed->pid;
-
-	smsendian_handle_tx_message((struct SmsMsgHdr_ST *)&PidMsg);
-	return smsclient_sendrequest(client->smsclient,
-				     &PidMsg, sizeof(PidMsg));
-}
-
-static int smsdvb_stop_feed(struct dvb_demux_feed *feed)
-{
-	struct smsdvb_client_t *client =
-		container_of(feed->demux, struct smsdvb_client_t, demux);
-	struct SmsMsgData_ST PidMsg;
-
-	sms_debug("remove pid %d(%x)",
-		  feed->pid, feed->pid);
-
-	PidMsg.xMsgHeader.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-	PidMsg.xMsgHeader.msgDstId = HIF_TASK;
-	PidMsg.xMsgHeader.msgFlags = 0;
-	PidMsg.xMsgHeader.msgType  = MSG_SMS_REMOVE_PID_FILTER_REQ;
-	PidMsg.xMsgHeader.msgLength = sizeof(PidMsg);
-	PidMsg.msgData[0] = feed->pid;
-
-	smsendian_handle_tx_message((struct SmsMsgHdr_ST *)&PidMsg);
-	return smsclient_sendrequest(client->smsclient,
-				     &PidMsg, sizeof(PidMsg));
-}
-
-static int smsdvb_sendrequest_and_wait(struct smsdvb_client_t *client,
-					void *buffer, size_t size,
-					struct completion *completion)
-{
-	int rc;
-
-	smsendian_handle_tx_message((struct SmsMsgHdr_ST *)buffer);
-	rc = smsclient_sendrequest(client->smsclient, buffer, size);
-	if (rc < 0)
-		return rc;
-
-	return wait_for_completion_timeout(completion,
-					   msecs_to_jiffies(2000)) ?
-						0 : -ETIME;
-}
-
-static int smsdvb_send_statistics_request(struct smsdvb_client_t *client)
-{
-	int rc;
-	struct SmsMsgHdr_ST Msg = { MSG_SMS_GET_STATISTICS_REQ,
-				    DVBT_BDA_CONTROL_MSG_ID,
-				    HIF_TASK,
-				    sizeof(struct SmsMsgHdr_ST), 0 };
-
-	rc = smsdvb_sendrequest_and_wait(client, &Msg, sizeof(Msg),
-					  &client->tune_done);
-
-	return rc;
-}
-
-static inline int led_feedback(struct smsdvb_client_t *client)
-{
-	if (client->fe_status & FE_HAS_LOCK)
-		return sms_board_led_feedback(client->coredev,
-			(client->sms_stat_dvb.ReceptionData.BER
-			== 0) ? SMS_LED_HI : SMS_LED_LO);
-	else
-		return sms_board_led_feedback(client->coredev, SMS_LED_OFF);
-}
-
-static int smsdvb_read_status(struct dvb_frontend *fe, fe_status_t *stat)
-{
-	int rc;
-	struct smsdvb_client_t *client;
-	client = container_of(fe, struct smsdvb_client_t, frontend);
-
-	rc = smsdvb_send_statistics_request(client);
-
-	*stat = client->fe_status;
-
-	led_feedback(client);
-
-	return rc;
-}
-
-static int smsdvb_read_ber(struct dvb_frontend *fe, u32 *ber)
-{
-	int rc;
-	struct smsdvb_client_t *client;
-	client = container_of(fe, struct smsdvb_client_t, frontend);
-
-	rc = smsdvb_send_statistics_request(client);
-
-	*ber = client->sms_stat_dvb.ReceptionData.BER;
-
-	led_feedback(client);
-
-	return rc;
-}
-
-static int smsdvb_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
-{
-	int rc;
-
-	struct smsdvb_client_t *client;
-	client = container_of(fe, struct smsdvb_client_t, frontend);
-
-	rc = smsdvb_send_statistics_request(client);
-
-	if (client->sms_stat_dvb.ReceptionData.InBandPwr < -95)
-		*strength = 0;
-		else if (client->sms_stat_dvb.ReceptionData.InBandPwr > -29)
-			*strength = 100;
-		else
-			*strength =
-				(client->sms_stat_dvb.ReceptionData.InBandPwr
-				+ 95) * 3 / 2;
-
-	led_feedback(client);
-
-	return rc;
-}
-
-static int smsdvb_read_snr(struct dvb_frontend *fe, u16 *snr)
-{
-	int rc;
-	struct smsdvb_client_t *client;
-	client = container_of(fe, struct smsdvb_client_t, frontend);
-
-	rc = smsdvb_send_statistics_request(client);
-
-	*snr = client->sms_stat_dvb.ReceptionData.SNR;
-
-	led_feedback(client);
-
-	return rc;
-}
-
-static int smsdvb_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
-{
-	int rc;
-	struct smsdvb_client_t *client;
-	client = container_of(fe, struct smsdvb_client_t, frontend);
-
-	rc = smsdvb_send_statistics_request(client);
-
-	*ucblocks = client->sms_stat_dvb.ReceptionData.ErrorTSPackets;
-
-	led_feedback(client);
-
-	return rc;
-}
-
-static int smsdvb_get_tune_settings(struct dvb_frontend *fe,
-				    struct dvb_frontend_tune_settings *tune)
-{
-	sms_debug("");
-
-	tune->min_delay_ms = 400;
-	tune->step_size = 250000;
-	tune->max_drift = 0;
-	return 0;
-}
-
-static int smsdvb_dvbt_set_frontend(struct dvb_frontend *fe)
-{
-	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	struct smsdvb_client_t *client =
-		container_of(fe, struct smsdvb_client_t, frontend);
-
-	struct {
-		struct SmsMsgHdr_ST	Msg;
-		u32		Data[3];
-	} Msg;
-
-	int ret;
-
-	client->fe_status = FE_HAS_SIGNAL;
-	client->event_fe_state = -1;
-	client->event_unc_state = -1;
-	fe->dtv_property_cache.delivery_system = SYS_DVBT;
-
-	Msg.Msg.msgSrcId = DVBT_BDA_CONTROL_MSG_ID;
-	Msg.Msg.msgDstId = HIF_TASK;
-	Msg.Msg.msgFlags = 0;
-	Msg.Msg.msgType = MSG_SMS_RF_TUNE_REQ;
-	Msg.Msg.msgLength = sizeof(Msg);
-	Msg.Data[0] = c->frequency;
-	Msg.Data[2] = 12000000;
-
-	sms_info("%s: freq %d band %d", __func__, c->frequency,
-		 c->bandwidth_hz);
-
-	switch (c->bandwidth_hz / 1000000) {
-	case 8:
-		Msg.Data[1] = BW_8_MHZ;
-		break;
-	case 7:
-		Msg.Data[1] = BW_7_MHZ;
-		break;
-	case 6:
-		Msg.Data[1] = BW_6_MHZ;
-		break;
-	case 0:
-		return -EOPNOTSUPP;
-	default:
-		return -EINVAL;
-	}
-	/* Disable LNA, if any. An error is returned if no LNA is present */
-	ret = sms_board_lna_control(client->coredev, 0);
-	if (ret == 0) {
-		fe_status_t status;
-
-		/* tune with LNA off at first */
-		ret = smsdvb_sendrequest_and_wait(client, &Msg, sizeof(Msg),
-						  &client->tune_done);
-
-		smsdvb_read_status(fe, &status);
-
-		if (status & FE_HAS_LOCK)
-			return ret;
-
-		/* previous tune didn't lock - enable LNA and tune again */
-		sms_board_lna_control(client->coredev, 1);
-	}
-
-	return smsdvb_sendrequest_and_wait(client, &Msg, sizeof(Msg),
-					   &client->tune_done);
-}
-
-static int smsdvb_isdbt_set_frontend(struct dvb_frontend *fe)
-{
-	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	struct smsdvb_client_t *client =
-		container_of(fe, struct smsdvb_client_t, frontend);
-
-	struct {
-		struct SmsMsgHdr_ST	Msg;
-		u32		Data[4];
-	} Msg;
-
-	fe->dtv_property_cache.delivery_system = SYS_ISDBT;
-
-	Msg.Msg.msgSrcId  = DVBT_BDA_CONTROL_MSG_ID;
-	Msg.Msg.msgDstId  = HIF_TASK;
-	Msg.Msg.msgFlags  = 0;
-	Msg.Msg.msgType   = MSG_SMS_ISDBT_TUNE_REQ;
-	Msg.Msg.msgLength = sizeof(Msg);
-
-	if (c->isdbt_sb_segment_idx == -1)
-		c->isdbt_sb_segment_idx = 0;
-
-	switch (c->isdbt_sb_segment_count) {
-	case 3:
-		Msg.Data[1] = BW_ISDBT_3SEG;
-		break;
-	case 1:
-		Msg.Data[1] = BW_ISDBT_1SEG;
-		break;
-	case 0:	/* AUTO */
-		switch (c->bandwidth_hz / 1000000) {
-		case 8:
-		case 7:
-			c->isdbt_sb_segment_count = 3;
-			Msg.Data[1] = BW_ISDBT_3SEG;
-			break;
-		case 6:
-			c->isdbt_sb_segment_count = 1;
-			Msg.Data[1] = BW_ISDBT_1SEG;
-			break;
-		default: /* Assumes 6 MHZ bw */
-			c->isdbt_sb_segment_count = 1;
-			c->bandwidth_hz = 6000;
-			Msg.Data[1] = BW_ISDBT_1SEG;
-			break;
-		}
-		break;
-	default:
-		sms_info("Segment count %d not supported", c->isdbt_sb_segment_count);
-		return -EINVAL;
-	}
-
-	Msg.Data[0] = c->frequency;
-	Msg.Data[2] = 12000000;
-	Msg.Data[3] = c->isdbt_sb_segment_idx;
-
-	sms_info("%s: freq %d segwidth %d segindex %d\n", __func__,
-		 c->frequency, c->isdbt_sb_segment_count,
-		 c->isdbt_sb_segment_idx);
-
-	return smsdvb_sendrequest_and_wait(client, &Msg, sizeof(Msg),
-					   &client->tune_done);
-}
-
-static int smsdvb_set_frontend(struct dvb_frontend *fe)
-{
-	struct smsdvb_client_t *client =
-		container_of(fe, struct smsdvb_client_t, frontend);
-	struct smscore_device_t *coredev = client->coredev;
-
-	switch (smscore_get_device_mode(coredev)) {
-	case DEVICE_MODE_DVBT:
-	case DEVICE_MODE_DVBT_BDA:
-		return smsdvb_dvbt_set_frontend(fe);
-	case DEVICE_MODE_ISDBT:
-	case DEVICE_MODE_ISDBT_BDA:
-		return smsdvb_isdbt_set_frontend(fe);
-	default:
-		return -EINVAL;
-	}
-}
-
-static int smsdvb_get_frontend(struct dvb_frontend *fe)
-{
-	struct dtv_frontend_properties *fep = &fe->dtv_property_cache;
-	struct smsdvb_client_t *client =
-		container_of(fe, struct smsdvb_client_t, frontend);
-	struct smscore_device_t *coredev = client->coredev;
-	struct TRANSMISSION_STATISTICS_S *td =
-		&client->sms_stat_dvb.TransmissionData;
-
-	switch (smscore_get_device_mode(coredev)) {
-	case DEVICE_MODE_DVBT:
-	case DEVICE_MODE_DVBT_BDA:
-		fep->frequency = td->Frequency;
-
-		switch (td->Bandwidth) {
-		case 6:
-			fep->bandwidth_hz = 6000000;
-			break;
-		case 7:
-			fep->bandwidth_hz = 7000000;
-			break;
-		case 8:
-			fep->bandwidth_hz = 8000000;
-			break;
-		}
-
-		switch (td->TransmissionMode) {
-		case 2:
-			fep->transmission_mode = TRANSMISSION_MODE_2K;
-			break;
-		case 8:
-			fep->transmission_mode = TRANSMISSION_MODE_8K;
-		}
-
-		switch (td->GuardInterval) {
-		case 0:
-			fep->guard_interval = GUARD_INTERVAL_1_32;
-			break;
-		case 1:
-			fep->guard_interval = GUARD_INTERVAL_1_16;
-			break;
-		case 2:
-			fep->guard_interval = GUARD_INTERVAL_1_8;
-			break;
-		case 3:
-			fep->guard_interval = GUARD_INTERVAL_1_4;
-			break;
-		}
-
-		switch (td->CodeRate) {
-		case 0:
-			fep->code_rate_HP = FEC_1_2;
-			break;
-		case 1:
-			fep->code_rate_HP = FEC_2_3;
-			break;
-		case 2:
-			fep->code_rate_HP = FEC_3_4;
-			break;
-		case 3:
-			fep->code_rate_HP = FEC_5_6;
-			break;
-		case 4:
-			fep->code_rate_HP = FEC_7_8;
-			break;
-		}
-
-		switch (td->LPCodeRate) {
-		case 0:
-			fep->code_rate_LP = FEC_1_2;
-			break;
-		case 1:
-			fep->code_rate_LP = FEC_2_3;
-			break;
-		case 2:
-			fep->code_rate_LP = FEC_3_4;
-			break;
-		case 3:
-			fep->code_rate_LP = FEC_5_6;
-			break;
-		case 4:
-			fep->code_rate_LP = FEC_7_8;
-			break;
-		}
-
-		switch (td->Constellation) {
-		case 0:
-			fep->modulation = QPSK;
-			break;
-		case 1:
-			fep->modulation = QAM_16;
-			break;
-		case 2:
-			fep->modulation = QAM_64;
-			break;
-		}
-
-		switch (td->Hierarchy) {
-		case 0:
-			fep->hierarchy = HIERARCHY_NONE;
-			break;
-		case 1:
-			fep->hierarchy = HIERARCHY_1;
-			break;
-		case 2:
-			fep->hierarchy = HIERARCHY_2;
-			break;
-		case 3:
-			fep->hierarchy = HIERARCHY_4;
-			break;
-		}
-
-		fep->inversion = INVERSION_AUTO;
-		break;
-	case DEVICE_MODE_ISDBT:
-	case DEVICE_MODE_ISDBT_BDA:
-		fep->frequency = td->Frequency;
-		fep->bandwidth_hz = 6000000;
-		/* todo: retrive the other parameters */
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int smsdvb_init(struct dvb_frontend *fe)
-{
-	struct smsdvb_client_t *client =
-		container_of(fe, struct smsdvb_client_t, frontend);
-
-	sms_board_power(client->coredev, 1);
-
-	sms_board_dvb3_event(client, DVB3_EVENT_INIT);
-	return 0;
-}
-
-static int smsdvb_sleep(struct dvb_frontend *fe)
-{
-	struct smsdvb_client_t *client =
-		container_of(fe, struct smsdvb_client_t, frontend);
-
-	sms_board_led_feedback(client->coredev, SMS_LED_OFF);
-	sms_board_power(client->coredev, 0);
-
-	sms_board_dvb3_event(client, DVB3_EVENT_SLEEP);
-
-	return 0;
-}
-
-static void smsdvb_release(struct dvb_frontend *fe)
-{
-	/* do nothing */
-}
-
-static struct dvb_frontend_ops smsdvb_fe_ops = {
-	.info = {
-		.name			= "Siano Mobile Digital MDTV Receiver",
-		.frequency_min		= 44250000,
-		.frequency_max		= 867250000,
-		.frequency_stepsize	= 250000,
-		.caps = FE_CAN_INVERSION_AUTO |
-			FE_CAN_FEC_1_2 | FE_CAN_FEC_2_3 | FE_CAN_FEC_3_4 |
-			FE_CAN_FEC_5_6 | FE_CAN_FEC_7_8 | FE_CAN_FEC_AUTO |
-			FE_CAN_QPSK | FE_CAN_QAM_16 | FE_CAN_QAM_64 |
-			FE_CAN_QAM_AUTO | FE_CAN_TRANSMISSION_MODE_AUTO |
-			FE_CAN_GUARD_INTERVAL_AUTO |
-			FE_CAN_RECOVER |
-			FE_CAN_HIERARCHY_AUTO,
-	},
-
-	.release = smsdvb_release,
-
-	.set_frontend = smsdvb_set_frontend,
-	.get_frontend = smsdvb_get_frontend,
-	.get_tune_settings = smsdvb_get_tune_settings,
-
-	.read_status = smsdvb_read_status,
-	.read_ber = smsdvb_read_ber,
-	.read_signal_strength = smsdvb_read_signal_strength,
-	.read_snr = smsdvb_read_snr,
-	.read_ucblocks = smsdvb_read_ucblocks,
-
-	.init = smsdvb_init,
-	.sleep = smsdvb_sleep,
-};
-
-static int smsdvb_hotplug(struct smscore_device_t *coredev,
-			  struct device *device, int arrival)
-{
-	struct smsclient_params_t params;
-	struct smsdvb_client_t *client;
-	int rc;
-
-	/* device removal handled by onremove callback */
-	if (!arrival)
-		return 0;
-	client = kzalloc(sizeof(struct smsdvb_client_t), GFP_KERNEL);
-	if (!client) {
-		sms_err("kmalloc() failed");
-		return -ENOMEM;
-	}
-
-	/* register dvb adapter */
-	rc = dvb_register_adapter(&client->adapter,
-				  sms_get_board(
-					smscore_get_board_id(coredev))->name,
-				  THIS_MODULE, device, adapter_nr);
-	if (rc < 0) {
-		sms_err("dvb_register_adapter() failed %d", rc);
-		goto adapter_error;
-	}
-
-	/* init dvb demux */
-	client->demux.dmx.capabilities = DMX_TS_FILTERING;
-	client->demux.filternum = 32; /* todo: nova ??? */
-	client->demux.feednum = 32;
-	client->demux.start_feed = smsdvb_start_feed;
-	client->demux.stop_feed = smsdvb_stop_feed;
-
-	rc = dvb_dmx_init(&client->demux);
-	if (rc < 0) {
-		sms_err("dvb_dmx_init failed %d", rc);
-		goto dvbdmx_error;
-	}
-
-	/* init dmxdev */
-	client->dmxdev.filternum = 32;
-	client->dmxdev.demux = &client->demux.dmx;
-	client->dmxdev.capabilities = 0;
-
-	rc = dvb_dmxdev_init(&client->dmxdev, &client->adapter);
-	if (rc < 0) {
-		sms_err("dvb_dmxdev_init failed %d", rc);
-		goto dmxdev_error;
-	}
-
-	/* init and register frontend */
-	memcpy(&client->frontend.ops, &smsdvb_fe_ops,
-	       sizeof(struct dvb_frontend_ops));
-
-	switch (smscore_get_device_mode(coredev)) {
-	case DEVICE_MODE_DVBT:
-	case DEVICE_MODE_DVBT_BDA:
-		client->frontend.ops.delsys[0] = SYS_DVBT;
-		break;
-	case DEVICE_MODE_ISDBT:
-	case DEVICE_MODE_ISDBT_BDA:
-		client->frontend.ops.delsys[0] = SYS_ISDBT;
-		break;
-	}
-
-	rc = dvb_register_frontend(&client->adapter, &client->frontend);
-	if (rc < 0) {
-		sms_err("frontend registration failed %d", rc);
-		goto frontend_error;
-	}
-
-	params.initial_id = 1;
-	params.data_type = MSG_SMS_DVBT_BDA_DATA;
-	params.onresponse_handler = smsdvb_onresponse;
-	params.onremove_handler = smsdvb_onremove;
-	params.context = client;
-
-	rc = smscore_register_client(coredev, &params, &client->smsclient);
-	if (rc < 0) {
-		sms_err("smscore_register_client() failed %d", rc);
-		goto client_error;
-	}
-
-	client->coredev = coredev;
-
-	init_completion(&client->tune_done);
-
-	kmutex_lock(&g_smsdvb_clientslock);
-
-	list_add(&client->entry, &g_smsdvb_clients);
-
-	kmutex_unlock(&g_smsdvb_clientslock);
-
-	client->event_fe_state = -1;
-	client->event_unc_state = -1;
-	sms_board_dvb3_event(client, DVB3_EVENT_HOTPLUG);
-
-	sms_info("success");
-	sms_board_setup(coredev);
-
-	return 0;
-
-client_error:
-	dvb_unregister_frontend(&client->frontend);
-
-frontend_error:
-	dvb_dmxdev_release(&client->dmxdev);
-
-dmxdev_error:
-	dvb_dmx_release(&client->demux);
-
-dvbdmx_error:
-	dvb_unregister_adapter(&client->adapter);
-
-adapter_error:
-	kfree(client);
-	return rc;
-}
-
-static int __init smsdvb_module_init(void)
-{
-	int rc;
-
-	INIT_LIST_HEAD(&g_smsdvb_clients);
-	kmutex_init(&g_smsdvb_clientslock);
-
-	rc = smscore_register_hotplug(smsdvb_hotplug);
-
-	sms_debug("");
-
-	return rc;
-}
-
-static void __exit smsdvb_module_exit(void)
-{
-	smscore_unregister_hotplug(smsdvb_hotplug);
-
-	kmutex_lock(&g_smsdvb_clientslock);
-
-	while (!list_empty(&g_smsdvb_clients))
-	       smsdvb_unregister_client(
-			(struct smsdvb_client_t *) g_smsdvb_clients.next);
-
-	kmutex_unlock(&g_smsdvb_clientslock);
-}
-
-module_init(smsdvb_module_init);
-module_exit(smsdvb_module_exit);
-
-MODULE_DESCRIPTION("SMS DVB subsystem adaptation module");
-MODULE_AUTHOR("Siano Mobile Silicon, Inc. (uris@siano-ms.com)");
-MODULE_LICENSE("GPL");
diff --git a/drivers/media/common/siano/smsdvb.h b/drivers/media/common/siano/smsdvb.h
new file mode 100644
index 000000000000..92c413ba0c79
--- /dev/null
+++ b/drivers/media/common/siano/smsdvb.h
@@ -0,0 +1,130 @@
+/***********************************************************************
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ *
+ ***********************************************************************/
+
+struct smsdvb_debugfs;
+struct smsdvb_client_t;
+
+typedef void (*sms_prt_dvb_stats_t)(struct smsdvb_debugfs *debug_data,
+				    struct sms_stats *p);
+
+typedef void (*sms_prt_isdb_stats_t)(struct smsdvb_debugfs *debug_data,
+				     struct sms_isdbt_stats *p);
+
+typedef void (*sms_prt_isdb_stats_ex_t)
+			(struct smsdvb_debugfs *debug_data,
+			 struct sms_isdbt_stats_ex *p);
+
+
+struct smsdvb_client_t {
+	struct list_head entry;
+
+	struct smscore_device_t *coredev;
+	struct smscore_client_t *smsclient;
+
+	struct dvb_adapter      adapter;
+	struct dvb_demux        demux;
+	struct dmxdev           dmxdev;
+	struct dvb_frontend     frontend;
+
+	fe_status_t             fe_status;
+
+	struct completion       tune_done;
+	struct completion       stats_done;
+
+	int last_per;
+
+	int legacy_ber, legacy_per;
+
+	int event_fe_state;
+	int event_unc_state;
+
+	unsigned long		get_stats_jiffies;
+
+	int			feed_users;
+	bool			has_tuned;
+
+	/* stats debugfs data */
+	struct dentry		*debugfs;
+
+	struct smsdvb_debugfs	*debug_data;
+
+	sms_prt_dvb_stats_t	prt_dvb_stats;
+	sms_prt_isdb_stats_t	prt_isdb_stats;
+	sms_prt_isdb_stats_ex_t	prt_isdb_stats_ex;
+};
+
+/*
+ * This struct is a mix of struct sms_rx_stats_ex and
+ * struct sms_srvm_signal_status.
+ * It was obtained by comparing the way it was filled by the original code
+ */
+struct RECEPTION_STATISTICS_PER_SLICES_S {
+	u32 result;
+	u32 snr;
+	s32 in_band_power;
+	u32 ts_packets;
+	u32 ets_packets;
+	u32 constellation;
+	u32 hp_code;
+	u32 tps_srv_ind_lp;
+	u32 tps_srv_ind_hp;
+	u32 cell_id;
+	u32 reason;
+	u32 request_id;
+	u32 modem_state;		/* from SMSHOSTLIB_DVB_MODEM_STATE_ET */
+
+	u32 ber;		/* Post Viterbi BER [1E-5] */
+	s32 RSSI;		/* dBm */
+	s32 carrier_offset;	/* Carrier Offset in bin/1024 */
+
+	u32 is_rf_locked;		/* 0 - not locked, 1 - locked */
+	u32 is_demod_locked;	/* 0 - not locked, 1 - locked */
+
+	u32 ber_bit_count;	/* Total number of SYNC bits. */
+	u32 ber_error_count;	/* Number of erronous SYNC bits. */
+
+	s32 MRC_SNR;		/* dB */
+	s32 mrc_in_band_pwr;	/* In band power in dBM */
+	s32 MRC_RSSI;		/* dBm */
+};
+
+/* From smsdvb-debugfs.c */
+#ifdef CONFIG_SMS_SIANO_DEBUGFS
+
+int smsdvb_debugfs_create(struct smsdvb_client_t *client);
+void smsdvb_debugfs_release(struct smsdvb_client_t *client);
+int smsdvb_debugfs_register(void);
+void smsdvb_debugfs_unregister(void);
+
+#else
+
+static inline int smsdvb_debugfs_create(struct smsdvb_client_t *client)
+{
+	return 0;
+}
+
+static inline void smsdvb_debugfs_release(struct smsdvb_client_t *client) {}
+
+static inline int smsdvb_debugfs_register(void)
+{
+	return 0;
+};
+
+static inline void smsdvb_debugfs_unregister(void) {};
+
+#endif
+
diff --git a/drivers/media/common/siano/smsendian.c b/drivers/media/common/siano/smsendian.c
index e2657c2f0109..bfe831c10b1c 100644
--- a/drivers/media/common/siano/smsendian.c
+++ b/drivers/media/common/siano/smsendian.c
@@ -28,23 +28,23 @@
 void smsendian_handle_tx_message(void *buffer)
 {
 #ifdef __BIG_ENDIAN
-	struct SmsMsgData_ST *msg = (struct SmsMsgData_ST *)buffer;
+	struct sms_msg_data *msg = (struct sms_msg_data *)buffer;
 	int i;
-	int msgWords;
+	int msg_words;
 
-	switch (msg->xMsgHeader.msgType) {
+	switch (msg->x_msg_header.msg_type) {
 	case MSG_SMS_DATA_DOWNLOAD_REQ:
 	{
-		msg->msgData[0] = le32_to_cpu(msg->msgData[0]);
+		msg->msg_data[0] = le32_to_cpu(msg->msg_data[0]);
 		break;
 	}
 
 	default:
-		msgWords = (msg->xMsgHeader.msgLength -
-				sizeof(struct SmsMsgHdr_ST))/4;
+		msg_words = (msg->x_msg_header.msg_length -
+				sizeof(struct sms_msg_hdr))/4;
 
-		for (i = 0; i < msgWords; i++)
-			msg->msgData[i] = le32_to_cpu(msg->msgData[i]);
+		for (i = 0; i < msg_words; i++)
+			msg->msg_data[i] = le32_to_cpu(msg->msg_data[i]);
 
 		break;
 	}
@@ -55,16 +55,16 @@ EXPORT_SYMBOL_GPL(smsendian_handle_tx_message);
 void smsendian_handle_rx_message(void *buffer)
 {
 #ifdef __BIG_ENDIAN
-	struct SmsMsgData_ST *msg = (struct SmsMsgData_ST *)buffer;
+	struct sms_msg_data *msg = (struct sms_msg_data *)buffer;
 	int i;
-	int msgWords;
+	int msg_words;
 
-	switch (msg->xMsgHeader.msgType) {
+	switch (msg->x_msg_header.msg_type) {
 	case MSG_SMS_GET_VERSION_EX_RES:
 	{
-		struct SmsVersionRes_ST *ver =
-			(struct SmsVersionRes_ST *) msg;
-		ver->ChipModel = le16_to_cpu(ver->ChipModel);
+		struct sms_version_res *ver =
+			(struct sms_version_res *) msg;
+		ver->chip_model = le16_to_cpu(ver->chip_model);
 		break;
 	}
 
@@ -77,11 +77,11 @@ void smsendian_handle_rx_message(void *buffer)
 
 	default:
 	{
-		msgWords = (msg->xMsgHeader.msgLength -
-				sizeof(struct SmsMsgHdr_ST))/4;
+		msg_words = (msg->x_msg_header.msg_length -
+				sizeof(struct sms_msg_hdr))/4;
 
-		for (i = 0; i < msgWords; i++)
-			msg->msgData[i] = le32_to_cpu(msg->msgData[i]);
+		for (i = 0; i < msg_words; i++)
+			msg->msg_data[i] = le32_to_cpu(msg->msg_data[i]);
 
 		break;
 	}
@@ -93,11 +93,11 @@ EXPORT_SYMBOL_GPL(smsendian_handle_rx_message);
 void smsendian_handle_message_header(void *msg)
 {
 #ifdef __BIG_ENDIAN
-	struct SmsMsgHdr_ST *phdr = (struct SmsMsgHdr_ST *)msg;
+	struct sms_msg_hdr *phdr = (struct sms_msg_hdr *)msg;
 
-	phdr->msgType = le16_to_cpu(phdr->msgType);
-	phdr->msgLength = le16_to_cpu(phdr->msgLength);
-	phdr->msgFlags = le16_to_cpu(phdr->msgFlags);
+	phdr->msg_type = le16_to_cpu(phdr->msg_type);
+	phdr->msg_length = le16_to_cpu(phdr->msg_length);
+	phdr->msg_flags = le16_to_cpu(phdr->msg_flags);
 #endif /* __BIG_ENDIAN */
 }
 EXPORT_SYMBOL_GPL(smsendian_handle_message_header);
diff --git a/drivers/media/common/siano/smsir.h b/drivers/media/common/siano/smsir.h
index 69b59b9eee28..fc8b7925c532 100644
--- a/drivers/media/common/siano/smsir.h
+++ b/drivers/media/common/siano/smsir.h
@@ -40,7 +40,6 @@ struct ir_t {
 	char phys[32];
 
 	char *rc_codes;
-	u64 protocol;
 
 	u32 timeout;
 	u32 controller;
diff --git a/drivers/media/dvb-core/demux.h b/drivers/media/dvb-core/demux.h
index eb91fd808c16..833191bcd810 100644
--- a/drivers/media/dvb-core/demux.h
+++ b/drivers/media/dvb-core/demux.h
@@ -83,45 +83,6 @@ enum dmx_success {
 #define TS_DEMUX        8   /* in case TS_PACKET is set, send the TS to
 			       the demux device, not to the dvr device */
 
-/* PES type for filters which write to built-in decoder */
-/* these should be kept identical to the types in dmx.h */
-
-enum dmx_ts_pes
-{  /* also send packets to decoder (if it exists) */
-	DMX_TS_PES_AUDIO0,
-	DMX_TS_PES_VIDEO0,
-	DMX_TS_PES_TELETEXT0,
-	DMX_TS_PES_SUBTITLE0,
-	DMX_TS_PES_PCR0,
-
-	DMX_TS_PES_AUDIO1,
-	DMX_TS_PES_VIDEO1,
-	DMX_TS_PES_TELETEXT1,
-	DMX_TS_PES_SUBTITLE1,
-	DMX_TS_PES_PCR1,
-
-	DMX_TS_PES_AUDIO2,
-	DMX_TS_PES_VIDEO2,
-	DMX_TS_PES_TELETEXT2,
-	DMX_TS_PES_SUBTITLE2,
-	DMX_TS_PES_PCR2,
-
-	DMX_TS_PES_AUDIO3,
-	DMX_TS_PES_VIDEO3,
-	DMX_TS_PES_TELETEXT3,
-	DMX_TS_PES_SUBTITLE3,
-	DMX_TS_PES_PCR3,
-
-	DMX_TS_PES_OTHER
-};
-
-#define DMX_TS_PES_AUDIO    DMX_TS_PES_AUDIO0
-#define DMX_TS_PES_VIDEO    DMX_TS_PES_VIDEO0
-#define DMX_TS_PES_TELETEXT DMX_TS_PES_TELETEXT0
-#define DMX_TS_PES_SUBTITLE DMX_TS_PES_SUBTITLE0
-#define DMX_TS_PES_PCR      DMX_TS_PES_PCR0
-
-
 struct dmx_ts_feed {
 	int is_filtering; /* Set to non-zero when filtering in progress */
 	struct dmx_demux *parent; /* Back-pointer */
diff --git a/drivers/media/dvb-core/dmxdev.c b/drivers/media/dvb-core/dmxdev.c
index d81dbb22aa81..a1a3a5159d71 100644
--- a/drivers/media/dvb-core/dmxdev.c
+++ b/drivers/media/dvb-core/dmxdev.c
@@ -569,7 +569,7 @@ static int dvb_dmxdev_start_feed(struct dmxdev *dmxdev,
 	dmx_output_t otype;
 	int ret;
 	int ts_type;
-	dmx_pes_type_t ts_pes;
+	enum dmx_ts_pes ts_pes;
 	struct dmx_ts_feed *tsfeed;
 
 	feed->ts = NULL;
@@ -852,7 +852,8 @@ static int dvb_dmxdev_filter_set(struct dmxdev *dmxdev,
 				 struct dmxdev_filter *dmxdevfilter,
 				 struct dmx_sct_filter_params *params)
 {
-	dprintk("function : %s\n", __func__);
+	dprintk("function : %s, PID=0x%04x, flags=%02x, timeout=%d\n",
+		__func__, params->pid, params->flags, params->timeout);
 
 	dvb_dmxdev_filter_stop(dmxdevfilter);
 
diff --git a/drivers/media/dvb-core/dvb-usb-ids.h b/drivers/media/dvb-core/dvb-usb-ids.h
index 399e1042d351..335a8f4695b4 100644
--- a/drivers/media/dvb-core/dvb-usb-ids.h
+++ b/drivers/media/dvb-core/dvb-usb-ids.h
@@ -124,8 +124,7 @@
 #define USB_PID_DIBCOM_STK7770P				0x1e80
 #define USB_PID_DIBCOM_NIM7090				0x1bb2
 #define USB_PID_DIBCOM_TFE7090PVR			0x1bb4
-#define USB_PID_DIBCOM_TFE7090E				0x1bb7
-#define USB_PID_DIBCOM_TFE7790E				0x1e6e
+#define USB_PID_DIBCOM_TFE7790P				0x1e6e
 #define USB_PID_DIBCOM_NIM9090M				0x2383
 #define USB_PID_DIBCOM_NIM9090MD			0x2384
 #define USB_PID_DPOSH_M9206_COLD			0x9206
diff --git a/drivers/media/dvb-core/dvb_demux.c b/drivers/media/dvb-core/dvb_demux.c
index d319717eb535..3485655fa082 100644
--- a/drivers/media/dvb-core/dvb_demux.c
+++ b/drivers/media/dvb-core/dvb_demux.c
@@ -440,20 +440,22 @@ static void dvb_dmx_swfilter_packet(struct dvb_demux *demux, const u8 *buf)
 		if (!dvb_demux_feed_err_pkts)
 			return;
 	} else /* if TEI bit is set, pid may be wrong- skip pkt counter */
-	if (demux->cnt_storage && dvb_demux_tscheck) {
-		/* check pkt counter */
-		if (pid < MAX_PID) {
-			if ((buf[3] & 0xf) != demux->cnt_storage[pid])
-				dprintk_tscheck("TS packet counter mismatch. "
-						"PID=0x%x expected 0x%x "
-						"got 0x%x\n",
+		if (demux->cnt_storage && dvb_demux_tscheck) {
+			/* check pkt counter */
+			if (pid < MAX_PID) {
+				if (buf[3] & 0x10)
+					demux->cnt_storage[pid] =
+						(demux->cnt_storage[pid] + 1) & 0xf;
+
+				if ((buf[3] & 0xf) != demux->cnt_storage[pid]) {
+					dprintk_tscheck("TS packet counter mismatch. PID=0x%x expected 0x%x got 0x%x\n",
 						pid, demux->cnt_storage[pid],
 						buf[3] & 0xf);
-
-			demux->cnt_storage[pid] = ((buf[3] & 0xf) + 1)&0xf;
+					demux->cnt_storage[pid] = buf[3] & 0xf;
+				}
+			}
+			/* end check */
 		}
-		/* end check */
-	}
 
 	list_for_each_entry(feed, &demux->feed_list, list_head) {
 		if ((feed->pid != pid) && (feed->pid != 0x2000))
@@ -672,7 +674,7 @@ static int dmx_ts_feed_set(struct dmx_ts_feed *ts_feed, u16 pid, int ts_type,
 		return -ERESTARTSYS;
 
 	if (ts_type & TS_DECODER) {
-		if (pes_type >= DMX_TS_PES_OTHER) {
+		if (pes_type >= DMX_PES_OTHER) {
 			mutex_unlock(&demux->mutex);
 			return -EINVAL;
 		}
@@ -844,7 +846,7 @@ static int dvbdmx_release_ts_feed(struct dmx_demux *dmx,
 
 	feed->pid = 0xffff;
 
-	if (feed->ts_type & TS_DECODER && feed->pes_type < DMX_TS_PES_OTHER)
+	if (feed->ts_type & TS_DECODER && feed->pes_type < DMX_PES_OTHER)
 		demux->pesfilter[feed->pes_type] = NULL;
 
 	mutex_unlock(&demux->mutex);
@@ -1266,7 +1268,7 @@ int dvb_dmx_init(struct dvb_demux *dvbdemux)
 
 	INIT_LIST_HEAD(&dvbdemux->frontend_list);
 
-	for (i = 0; i < DMX_TS_PES_OTHER; i++) {
+	for (i = 0; i < DMX_PES_OTHER; i++) {
 		dvbdemux->pesfilter[i] = NULL;
 		dvbdemux->pids[i] = 0xffff;
 	}
diff --git a/drivers/media/dvb-core/dvb_demux.h b/drivers/media/dvb-core/dvb_demux.h
index fa7188a253aa..ae7fc33c3231 100644
--- a/drivers/media/dvb-core/dvb_demux.h
+++ b/drivers/media/dvb-core/dvb_demux.h
@@ -119,8 +119,8 @@ struct dvb_demux {
 
 	struct list_head frontend_list;
 
-	struct dvb_demux_feed *pesfilter[DMX_TS_PES_OTHER];
-	u16 pids[DMX_TS_PES_OTHER];
+	struct dvb_demux_feed *pesfilter[DMX_PES_OTHER];
+	u16 pids[DMX_PES_OTHER];
 	int playing;
 	int recording;
 
diff --git a/drivers/media/dvb-core/dvb_frontend.c b/drivers/media/dvb-core/dvb_frontend.c
index 6e50a7581568..1f925e856974 100644
--- a/drivers/media/dvb-core/dvb_frontend.c
+++ b/drivers/media/dvb-core/dvb_frontend.c
@@ -920,7 +920,7 @@ static int dvb_frontend_clear_cache(struct dvb_frontend *fe)
 	u32 delsys;
 
 	delsys = c->delivery_system;
-	memset(c, 0, sizeof(struct dtv_frontend_properties));
+	memset(c, 0, offsetof(struct dtv_frontend_properties, strength));
 	c->delivery_system = delsys;
 
 	c->state = DTV_CLEAR;
@@ -1509,9 +1509,74 @@ static bool is_dvbv3_delsys(u32 delsys)
 	return status;
 }
 
-static int set_delivery_system(struct dvb_frontend *fe, u32 desired_system)
+/**
+ * emulate_delivery_system - emulate a DVBv5 delivery system with a DVBv3 type
+ * @fe:			struct frontend;
+ * @delsys:			DVBv5 type that will be used for emulation
+ *
+ * Provides emulation for delivery systems that are compatible with the old
+ * DVBv3 call. Among its usages, it provices support for ISDB-T, and allows
+ * using a DVB-S2 only frontend just like it were a DVB-S, if the frontent
+ * parameters are compatible with DVB-S spec.
+ */
+static int emulate_delivery_system(struct dvb_frontend *fe, u32 delsys)
 {
-	int ncaps, i;
+	int i;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+	c->delivery_system = delsys;
+
+	/*
+	 * If the call is for ISDB-T, put it into full-seg, auto mode, TV
+	 */
+	if (c->delivery_system == SYS_ISDBT) {
+		dev_dbg(fe->dvb->device,
+			"%s: Using defaults for SYS_ISDBT\n",
+			__func__);
+
+		if (!c->bandwidth_hz)
+			c->bandwidth_hz = 6000000;
+
+		c->isdbt_partial_reception = 0;
+		c->isdbt_sb_mode = 0;
+		c->isdbt_sb_subchannel = 0;
+		c->isdbt_sb_segment_idx = 0;
+		c->isdbt_sb_segment_count = 0;
+		c->isdbt_layer_enabled = 7;
+		for (i = 0; i < 3; i++) {
+			c->layer[i].fec = FEC_AUTO;
+			c->layer[i].modulation = QAM_AUTO;
+			c->layer[i].interleaving = 0;
+			c->layer[i].segment_count = 0;
+		}
+	}
+	dev_dbg(fe->dvb->device, "%s: change delivery system on cache to %d\n",
+		__func__, c->delivery_system);
+
+	return 0;
+}
+
+/**
+ * dvbv5_set_delivery_system - Sets the delivery system for a DVBv5 API call
+ * @fe:			frontend struct
+ * @desired_system:	delivery system requested by the user
+ *
+ * A DVBv5 call know what's the desired system it wants. So, set it.
+ *
+ * There are, however, a few known issues with early DVBv5 applications that
+ * are also handled by this logic:
+ *
+ * 1) Some early apps use SYS_UNDEFINED as the desired delivery system.
+ *    This is an API violation, but, as we don't want to break userspace,
+ *    convert it to the first supported delivery system.
+ * 2) Some apps might be using a DVBv5 call in a wrong way, passing, for
+ *    example, SYS_DVBT instead of SYS_ISDBT. This is because early usage of
+ *    ISDB-T provided backward compat with DVB-T.
+ */
+static int dvbv5_set_delivery_system(struct dvb_frontend *fe,
+				     u32 desired_system)
+{
+	int ncaps;
 	u32 delsys = SYS_UNDEFINED;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	enum dvbv3_emulation_type type;
@@ -1522,166 +1587,136 @@ static int set_delivery_system(struct dvb_frontend *fe, u32 desired_system)
 	 * assume that the application wants to use the first supported
 	 * delivery system.
 	 */
-	if (c->delivery_system == SYS_UNDEFINED)
-	        c->delivery_system = fe->ops.delsys[0];
+	if (desired_system == SYS_UNDEFINED)
+		desired_system = fe->ops.delsys[0];
 
-	if (desired_system == SYS_UNDEFINED) {
-		/*
-		 * A DVBv3 call doesn't know what's the desired system.
-		 * Also, DVBv3 applications don't know that ops.info->type
-		 * could be changed, and they simply dies when it doesn't
-		 * match.
-		 * So, don't change the current delivery system, as it
-		 * may be trying to do the wrong thing, like setting an
-		 * ISDB-T frontend as DVB-T. Instead, find the closest
-		 * DVBv3 system that matches the delivery system.
-		 */
-		if (is_dvbv3_delsys(c->delivery_system)) {
+	/*
+	 * This is a DVBv5 call. So, it likely knows the supported
+	 * delivery systems. So, check if the desired delivery system is
+	 * supported
+	 */
+	ncaps = 0;
+	while (fe->ops.delsys[ncaps] && ncaps < MAX_DELSYS) {
+		if (fe->ops.delsys[ncaps] == desired_system) {
+			c->delivery_system = desired_system;
 			dev_dbg(fe->dvb->device,
-					"%s: Using delivery system to %d\n",
-					__func__, c->delivery_system);
-			return 0;
-		}
-		type = dvbv3_type(c->delivery_system);
-		switch (type) {
-		case DVBV3_QPSK:
-			desired_system = SYS_DVBS;
-			break;
-		case DVBV3_QAM:
-			desired_system = SYS_DVBC_ANNEX_A;
-			break;
-		case DVBV3_ATSC:
-			desired_system = SYS_ATSC;
-			break;
-		case DVBV3_OFDM:
-			desired_system = SYS_DVBT;
-			break;
-		default:
-			dev_dbg(fe->dvb->device, "%s: This frontend doesn't support DVBv3 calls\n",
-					__func__);
-			return -EINVAL;
-		}
-		/*
-		 * Get a delivery system that is compatible with DVBv3
-		 * NOTE: in order for this to work with softwares like Kaffeine that
-		 *	uses a DVBv5 call for DVB-S2 and a DVBv3 call to go back to
-		 *	DVB-S, drivers that support both should put the SYS_DVBS entry
-		 *	before the SYS_DVBS2, otherwise it won't switch back to DVB-S.
-		 *	The real fix is that userspace applications should not use DVBv3
-		 *	and not trust on calling FE_SET_FRONTEND to switch the delivery
-		 *	system.
-		 */
-		ncaps = 0;
-		while (fe->ops.delsys[ncaps] && ncaps < MAX_DELSYS) {
-			if (fe->ops.delsys[ncaps] == desired_system) {
-				delsys = desired_system;
-				break;
-			}
-			ncaps++;
-		}
-		if (delsys == SYS_UNDEFINED) {
-			dev_dbg(fe->dvb->device, "%s: Couldn't find a delivery system that matches %d\n",
+					"%s: Changing delivery system to %d\n",
 					__func__, desired_system);
+			return 0;
 		}
-	} else {
-		/*
-		 * This is a DVBv5 call. So, it likely knows the supported
-		 * delivery systems.
-		 */
+		ncaps++;
+	}
 
-		/* Check if the desired delivery system is supported */
-		ncaps = 0;
-		while (fe->ops.delsys[ncaps] && ncaps < MAX_DELSYS) {
-			if (fe->ops.delsys[ncaps] == desired_system) {
-				c->delivery_system = desired_system;
-				dev_dbg(fe->dvb->device,
-						"%s: Changing delivery system to %d\n",
-						__func__, desired_system);
-				return 0;
-			}
-			ncaps++;
-		}
-		type = dvbv3_type(desired_system);
+	/*
+	 * The requested delivery system isn't supported. Maybe userspace
+	 * is requesting a DVBv3 compatible delivery system.
+	 *
+	 * The emulation only works if the desired system is one of the
+	 * delivery systems supported by DVBv3 API
+	 */
+	if (!is_dvbv3_delsys(desired_system)) {
+		dev_dbg(fe->dvb->device,
+			"%s: Delivery system %d not supported.\n",
+			__func__, desired_system);
+		return -EINVAL;
+	}
 
-		/*
-		 * The delivery system is not supported. See if it can be
-		 * emulated.
-		 * The emulation only works if the desired system is one of the
-		 * DVBv3 delivery systems
-		 */
-		if (!is_dvbv3_delsys(desired_system)) {
-			dev_dbg(fe->dvb->device,
-					"%s: can't use a DVBv3 FE_SET_FRONTEND call on this frontend\n",
-					__func__);
-			return -EINVAL;
-		}
+	type = dvbv3_type(desired_system);
 
-		/*
-		 * Get the last non-DVBv3 delivery system that has the same type
-		 * of the desired system
-		 */
-		ncaps = 0;
-		while (fe->ops.delsys[ncaps] && ncaps < MAX_DELSYS) {
-			if ((dvbv3_type(fe->ops.delsys[ncaps]) == type) &&
-			    !is_dvbv3_delsys(fe->ops.delsys[ncaps]))
-				delsys = fe->ops.delsys[ncaps];
-			ncaps++;
-		}
-		/* There's nothing compatible with the desired delivery system */
-		if (delsys == SYS_UNDEFINED) {
-			dev_dbg(fe->dvb->device,
-					"%s: Incompatible DVBv3 FE_SET_FRONTEND call for this frontend\n",
-					__func__);
-			return -EINVAL;
-		}
+	/*
+	* Get the last non-DVBv3 delivery system that has the same type
+	* of the desired system
+	*/
+	ncaps = 0;
+	while (fe->ops.delsys[ncaps] && ncaps < MAX_DELSYS) {
+		if (dvbv3_type(fe->ops.delsys[ncaps]) == type)
+			delsys = fe->ops.delsys[ncaps];
+		ncaps++;
 	}
 
-	c->delivery_system = delsys;
+	/* There's nothing compatible with the desired delivery system */
+	if (delsys == SYS_UNDEFINED) {
+		dev_dbg(fe->dvb->device,
+			"%s: Delivery system %d not supported on emulation mode.\n",
+			__func__, desired_system);
+		return -EINVAL;
+	}
+
+	dev_dbg(fe->dvb->device,
+		"%s: Using delivery system %d emulated as if it were %d\n",
+		__func__, delsys, desired_system);
+
+	return emulate_delivery_system(fe, desired_system);
+}
+
+/**
+ * dvbv3_set_delivery_system - Sets the delivery system for a DVBv3 API call
+ * @fe:	frontend struct
+ *
+ * A DVBv3 call doesn't know what's the desired system it wants. It also
+ * doesn't allow to switch between different types. Due to that, userspace
+ * should use DVBv5 instead.
+ * However, in order to avoid breaking userspace API, limited backward
+ * compatibility support is provided.
+ *
+ * There are some delivery systems that are incompatible with DVBv3 calls.
+ *
+ * This routine should work fine for frontends that support just one delivery
+ * system.
+ *
+ * For frontends that support multiple frontends:
+ * 1) It defaults to use the first supported delivery system. There's an
+ *    userspace application that allows changing it at runtime;
+ *
+ * 2) If the current delivery system is not compatible with DVBv3, it gets
+ *    the first one that it is compatible.
+ *
+ * NOTE: in order for this to work with applications like Kaffeine that
+ *	uses a DVBv5 call for DVB-S2 and a DVBv3 call to go back to
+ *	DVB-S, drivers that support both DVB-S and DVB-S2 should have the
+ *	SYS_DVBS entry before the SYS_DVBS2, otherwise it won't switch back
+ *	to DVB-S.
+ */
+static int dvbv3_set_delivery_system(struct dvb_frontend *fe)
+{
+	int ncaps;
+	u32 delsys = SYS_UNDEFINED;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+	/* If not set yet, defaults to the first supported delivery system */
+	if (c->delivery_system == SYS_UNDEFINED)
+		c->delivery_system = fe->ops.delsys[0];
 
 	/*
-	 * The DVBv3 or DVBv5 call is requesting a different system. So,
-	 * emulation is needed.
-	 *
-	 * Emulate newer delivery systems like ISDBT, DVBT and DTMB
-	 * for older DVBv5 applications. The emulation will try to use
-	 * the auto mode for most things, and will assume that the desired
-	 * delivery system is the last one at the ops.delsys[] array
+	 * Trivial case: just use the current one, if it already a DVBv3
+	 * delivery system
 	 */
-	dev_dbg(fe->dvb->device,
-			"%s: Using delivery system %d emulated as if it were a %d\n",
-			__func__, delsys, desired_system);
+	if (is_dvbv3_delsys(c->delivery_system)) {
+		dev_dbg(fe->dvb->device,
+				"%s: Using delivery system to %d\n",
+				__func__, c->delivery_system);
+		return 0;
+	}
 
 	/*
-	 * For now, handles ISDB-T calls. More code may be needed here for the
-	 * other emulated stuff
+	 * Seek for the first delivery system that it is compatible with a
+	 * DVBv3 standard
 	 */
-	if (type == DVBV3_OFDM) {
-		if (c->delivery_system == SYS_ISDBT) {
-			dev_dbg(fe->dvb->device,
-					"%s: Using defaults for SYS_ISDBT\n",
-					__func__);
-
-			if (!c->bandwidth_hz)
-				c->bandwidth_hz = 6000000;
-
-			c->isdbt_partial_reception = 0;
-			c->isdbt_sb_mode = 0;
-			c->isdbt_sb_subchannel = 0;
-			c->isdbt_sb_segment_idx = 0;
-			c->isdbt_sb_segment_count = 0;
-			c->isdbt_layer_enabled = 0;
-			for (i = 0; i < 3; i++) {
-				c->layer[i].fec = FEC_AUTO;
-				c->layer[i].modulation = QAM_AUTO;
-				c->layer[i].interleaving = 0;
-				c->layer[i].segment_count = 0;
-			}
+	ncaps = 0;
+	while (fe->ops.delsys[ncaps] && ncaps < MAX_DELSYS) {
+		if (dvbv3_type(fe->ops.delsys[ncaps]) != DVBV3_UNKNOWN) {
+			delsys = fe->ops.delsys[ncaps];
+			break;
 		}
+		ncaps++;
 	}
-	dev_dbg(fe->dvb->device, "%s: change delivery system on cache to %d\n",
-			__func__, c->delivery_system);
-
-	return 0;
+	if (delsys == SYS_UNDEFINED) {
+		dev_dbg(fe->dvb->device,
+			"%s: Couldn't find a delivery system that works with FE_SET_FRONTEND\n",
+			__func__);
+		return -EINVAL;
+	}
+	return emulate_delivery_system(fe, delsys);
 }
 
 static int dtv_property_process_set(struct dvb_frontend *fe,
@@ -1742,7 +1777,7 @@ static int dtv_property_process_set(struct dvb_frontend *fe,
 		c->rolloff = tvp->u.data;
 		break;
 	case DTV_DELIVERY_SYSTEM:
-		r = set_delivery_system(fe, tvp->u.data);
+		r = dvbv5_set_delivery_system(fe, tvp->u.data);
 		break;
 	case DTV_VOLTAGE:
 		c->voltage = tvp->u.data;
@@ -2335,7 +2370,7 @@ static int dvb_frontend_ioctl_legacy(struct file *file,
 		break;
 
 	case FE_SET_FRONTEND:
-		err = set_delivery_system(fe, SYS_UNDEFINED);
+		err = dvbv3_set_delivery_system(fe);
 		if (err)
 			break;
 
@@ -2492,11 +2527,8 @@ static int dvb_frontend_release(struct inode *inode, struct file *file)
 
 	if (dvbdev->users == -1) {
 		wake_up(&fepriv->wait_queue);
-		if (fepriv->exit != DVB_FE_NO_EXIT) {
-			fops_put(file->f_op);
-			file->f_op = NULL;
+		if (fepriv->exit != DVB_FE_NO_EXIT)
 			wake_up(&dvbdev->wait_queue);
-		}
 		if (fe->ops.ts_bus_ctrl)
 			fe->ops.ts_bus_ctrl(fe, 0);
 	}
@@ -2594,7 +2626,7 @@ int dvb_register_frontend(struct dvb_adapter* dvb,
 	 * first supported delivery system (ops->delsys[0])
 	 */
 
-        fe->dtv_property_cache.delivery_system = fe->ops.delsys[0];
+	fe->dtv_property_cache.delivery_system = fe->ops.delsys[0];
 	dvb_frontend_clear_cache(fe);
 
 	mutex_unlock(&frontend_mutex);
diff --git a/drivers/media/dvb-core/dvb_frontend.h b/drivers/media/dvb-core/dvb_frontend.h
index b34922a08156..371b6caf486c 100644
--- a/drivers/media/dvb-core/dvb_frontend.h
+++ b/drivers/media/dvb-core/dvb_frontend.h
@@ -245,8 +245,8 @@ struct analog_demod_ops {
 
 	void (*set_params)(struct dvb_frontend *fe,
 			   struct analog_parameters *params);
-	int  (*has_signal)(struct dvb_frontend *fe);
-	int  (*get_afc)(struct dvb_frontend *fe);
+	int  (*has_signal)(struct dvb_frontend *fe, u16 *signal);
+	int  (*get_afc)(struct dvb_frontend *fe, s32 *afc);
 	void (*tuner_status)(struct dvb_frontend *fe);
 	void (*standby)(struct dvb_frontend *fe);
 	void (*release)(struct dvb_frontend *fe);
diff --git a/drivers/media/dvb-core/dvb_net.c b/drivers/media/dvb-core/dvb_net.c
index 44225b186f6d..f91c80c0e9ec 100644
--- a/drivers/media/dvb-core/dvb_net.c
+++ b/drivers/media/dvb-core/dvb_net.c
@@ -185,7 +185,7 @@ static __be16 dvb_net_eth_type_trans(struct sk_buff *skb,
 			skb->pkt_type=PACKET_MULTICAST;
 	}
 
-	if (ntohs(eth->h_proto) >= 1536)
+	if (ntohs(eth->h_proto) >= ETH_P_802_3_MIN)
 		return eth->h_proto;
 
 	rawp = skb->data;
@@ -228,9 +228,9 @@ static int ule_test_sndu( struct dvb_net_priv *p )
 static int ule_bridged_sndu( struct dvb_net_priv *p )
 {
 	struct ethhdr *hdr = (struct ethhdr*) p->ule_next_hdr;
-	if(ntohs(hdr->h_proto) < 1536) {
+	if(ntohs(hdr->h_proto) < ETH_P_802_3_MIN) {
 		int framelen = p->ule_sndu_len - ((p->ule_next_hdr+sizeof(struct ethhdr)) - p->ule_skb->data);
-		/* A frame Type < 1536 for a bridged frame, introduces a LLC Length field. */
+		/* A frame Type < ETH_P_802_3_MIN for a bridged frame, introduces a LLC Length field. */
 		if(framelen != ntohs(hdr->h_proto)) {
 			return -1;
 		}
@@ -320,7 +320,7 @@ static int handle_ule_extensions( struct dvb_net_priv *p )
 			(int) p->ule_sndu_type, l, total_ext_len);
 #endif
 
-	} while (p->ule_sndu_type < 1536);
+	} while (p->ule_sndu_type < ETH_P_802_3_MIN);
 
 	return total_ext_len;
 }
@@ -712,7 +712,7 @@ static void dvb_net_ule( struct net_device *dev, const u8 *buf, size_t buf_len )
 				}
 
 				/* Handle ULE Extension Headers. */
-				if (priv->ule_sndu_type < 1536) {
+				if (priv->ule_sndu_type < ETH_P_802_3_MIN) {
 					/* There is an extension header.  Handle it accordingly. */
 					int l = handle_ule_extensions(priv);
 					if (l < 0) {
@@ -1044,7 +1044,7 @@ static int dvb_net_feed_start(struct net_device *dev)
 		ret = priv->tsfeed->set(priv->tsfeed,
 					priv->pid, /* pid */
 					TS_PACKET, /* type */
-					DMX_TS_PES_OTHER, /* pes type */
+					DMX_PES_OTHER, /* pes type */
 					32768,     /* circular buffer size */
 					timeout    /* timeout */
 					);
@@ -1479,11 +1479,8 @@ static int dvb_net_close(struct inode *inode, struct file *file)
 
 	dvb_generic_release(inode, file);
 
-	if(dvbdev->users == 1 && dvbnet->exit == 1) {
-		fops_put(file->f_op);
-		file->f_op = NULL;
+	if(dvbdev->users == 1 && dvbnet->exit == 1)
 		wake_up(&dvbdev->wait_queue);
-	}
 	return 0;
 }
 
diff --git a/drivers/media/dvb-frontends/Kconfig b/drivers/media/dvb-frontends/Kconfig
index 6f809a70c78e..0e2ec6f73b05 100644
--- a/drivers/media/dvb-frontends/Kconfig
+++ b/drivers/media/dvb-frontends/Kconfig
@@ -210,7 +210,7 @@ config DVB_SI21XX
 config DVB_TS2020
 	tristate "Montage Tehnology TS2020 based tuners"
 	depends on DVB_CORE && I2C
-	default m if DVB_FE_CUSTOMISE
+	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
 	  A DVB-S/S2 silicon tuner. Say Y when you want to support this tuner.
 
diff --git a/drivers/media/dvb-frontends/a8293.h b/drivers/media/dvb-frontends/a8293.h
index ed29e5504f76..b6ef6427cfa5 100644
--- a/drivers/media/dvb-frontends/a8293.h
+++ b/drivers/media/dvb-frontends/a8293.h
@@ -21,12 +21,13 @@
 #ifndef A8293_H
 #define A8293_H
 
+#include <linux/kconfig.h>
+
 struct a8293_config {
 	u8 i2c_addr;
 };
 
-#if defined(CONFIG_DVB_A8293) || \
-	(defined(CONFIG_DVB_A8293_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_A8293)
 extern struct dvb_frontend *a8293_attach(struct dvb_frontend *fe,
 	struct i2c_adapter *i2c, const struct a8293_config *cfg);
 #else
diff --git a/drivers/media/dvb-frontends/af9013.h b/drivers/media/dvb-frontends/af9013.h
index dc837d91327a..09273b2cd310 100644
--- a/drivers/media/dvb-frontends/af9013.h
+++ b/drivers/media/dvb-frontends/af9013.h
@@ -25,6 +25,7 @@
 #ifndef AF9013_H
 #define AF9013_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 /* AF9013/5 GPIOs (mostly guessed)
@@ -102,8 +103,7 @@ struct af9013_config {
 	u8 gpio[4];
 };
 
-#if defined(CONFIG_DVB_AF9013) || \
-	(defined(CONFIG_DVB_AF9013_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_AF9013)
 extern struct dvb_frontend *af9013_attach(const struct af9013_config *config,
 	struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/af9033.c b/drivers/media/dvb-frontends/af9033.c
index c9cad989b8b9..a777b4b944eb 100644
--- a/drivers/media/dvb-frontends/af9033.c
+++ b/drivers/media/dvb-frontends/af9033.c
@@ -156,6 +156,37 @@ static int af9033_rd_reg_mask(struct af9033_state *state, u32 reg, u8 *val,
 	return 0;
 }
 
+/* write reg val table using reg addr auto increment */
+static int af9033_wr_reg_val_tab(struct af9033_state *state,
+		const struct reg_val *tab, int tab_len)
+{
+	int ret, i, j;
+	u8 buf[tab_len];
+
+	dev_dbg(&state->i2c->dev, "%s: tab_len=%d\n", __func__, tab_len);
+
+	for (i = 0, j = 0; i < tab_len; i++) {
+		buf[j] = tab[i].val;
+
+		if (i == tab_len - 1 || tab[i].reg != tab[i + 1].reg - 1) {
+			ret = af9033_wr_regs(state, tab[i].reg - j, buf, j + 1);
+			if (ret < 0)
+				goto err;
+
+			j = 0;
+		} else {
+			j++;
+		}
+	}
+
+	return 0;
+
+err:
+	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
 static u32 af9033_div(struct af9033_state *state, u32 a, u32 b, u32 x)
 {
 	u32 r = 0, c = 0, i;
@@ -223,6 +254,7 @@ static int af9033_init(struct dvb_frontend *fe)
 		{ 0x80f986, state->ts_mode_parallel, 0x01 },
 		{ 0x00d827, 0x00, 0xff },
 		{ 0x00d829, 0x00, 0xff },
+		{ 0x800045, state->cfg.adc_multiplier, 0xff },
 	};
 
 	/* program clock control */
@@ -286,14 +318,29 @@ static int af9033_init(struct dvb_frontend *fe)
 
 	/* load OFSM settings */
 	dev_dbg(&state->i2c->dev, "%s: load ofsm settings\n", __func__);
-	len = ARRAY_SIZE(ofsm_init);
-	init = ofsm_init;
-	for (i = 0; i < len; i++) {
-		ret = af9033_wr_reg(state, init[i].reg, init[i].val);
-		if (ret < 0)
-			goto err;
+	switch (state->cfg.tuner) {
+	case AF9033_TUNER_IT9135_38:
+	case AF9033_TUNER_IT9135_51:
+	case AF9033_TUNER_IT9135_52:
+		len = ARRAY_SIZE(ofsm_init_it9135_v1);
+		init = ofsm_init_it9135_v1;
+		break;
+	case AF9033_TUNER_IT9135_60:
+	case AF9033_TUNER_IT9135_61:
+	case AF9033_TUNER_IT9135_62:
+		len = ARRAY_SIZE(ofsm_init_it9135_v2);
+		init = ofsm_init_it9135_v2;
+		break;
+	default:
+		len = ARRAY_SIZE(ofsm_init);
+		init = ofsm_init;
+		break;
 	}
 
+	ret = af9033_wr_reg_val_tab(state, init, len);
+	if (ret < 0)
+		goto err;
+
 	/* load tuner specific settings */
 	dev_dbg(&state->i2c->dev, "%s: load tuner specific settings\n",
 			__func__);
@@ -322,6 +369,30 @@ static int af9033_init(struct dvb_frontend *fe)
 		len = ARRAY_SIZE(tuner_init_fc0012);
 		init = tuner_init_fc0012;
 		break;
+	case AF9033_TUNER_IT9135_38:
+		len = ARRAY_SIZE(tuner_init_it9135_38);
+		init = tuner_init_it9135_38;
+		break;
+	case AF9033_TUNER_IT9135_51:
+		len = ARRAY_SIZE(tuner_init_it9135_51);
+		init = tuner_init_it9135_51;
+		break;
+	case AF9033_TUNER_IT9135_52:
+		len = ARRAY_SIZE(tuner_init_it9135_52);
+		init = tuner_init_it9135_52;
+		break;
+	case AF9033_TUNER_IT9135_60:
+		len = ARRAY_SIZE(tuner_init_it9135_60);
+		init = tuner_init_it9135_60;
+		break;
+	case AF9033_TUNER_IT9135_61:
+		len = ARRAY_SIZE(tuner_init_it9135_61);
+		init = tuner_init_it9135_61;
+		break;
+	case AF9033_TUNER_IT9135_62:
+		len = ARRAY_SIZE(tuner_init_it9135_62);
+		init = tuner_init_it9135_62;
+		break;
 	default:
 		dev_dbg(&state->i2c->dev, "%s: unsupported tuner ID=%d\n",
 				__func__, state->cfg.tuner);
@@ -329,11 +400,9 @@ static int af9033_init(struct dvb_frontend *fe)
 		goto err;
 	}
 
-	for (i = 0; i < len; i++) {
-		ret = af9033_wr_reg(state, init[i].reg, init[i].val);
-		if (ret < 0)
-			goto err;
-	}
+	ret = af9033_wr_reg_val_tab(state, init, len);
+	if (ret < 0)
+		goto err;
 
 	if (state->cfg.ts_mode == AF9033_TS_MODE_SERIAL) {
 		ret = af9033_wr_reg_mask(state, 0x00d91c, 0x01, 0x01);
@@ -349,6 +418,15 @@ static int af9033_init(struct dvb_frontend *fe)
 			goto err;
 	}
 
+	switch (state->cfg.tuner) {
+	case AF9033_TUNER_IT9135_60:
+	case AF9033_TUNER_IT9135_61:
+	case AF9033_TUNER_IT9135_62:
+		ret = af9033_wr_reg(state, 0x800000, 0x01);
+		if (ret < 0)
+			goto err;
+	}
+
 	state->bandwidth_hz = 0; /* force to program all parameters */
 
 	return 0;
@@ -415,7 +493,8 @@ err:
 static int af9033_get_tune_settings(struct dvb_frontend *fe,
 		struct dvb_frontend_tune_settings *fesettings)
 {
-	fesettings->min_delay_ms = 800;
+	/* 800 => 2000 because IT9135 v2 is slow to gain lock */
+	fesettings->min_delay_ms = 2000;
 	fesettings->step_size = 0;
 	fesettings->max_drift = 0;
 
@@ -498,17 +577,17 @@ static int af9033_set_frontend(struct dvb_frontend *fe)
 		if (spec_inv == -1)
 			freq_cw = 0x800000 - freq_cw;
 
-		/* get adc multiplies */
-		ret = af9033_rd_reg(state, 0x800045, &tmp);
-		if (ret < 0)
-			goto err;
-
-		if (tmp == 1)
+		if (state->cfg.adc_multiplier == AF9033_ADC_MULTIPLIER_2X)
 			freq_cw /= 2;
 
 		buf[0] = (freq_cw >>  0) & 0xff;
 		buf[1] = (freq_cw >>  8) & 0xff;
 		buf[2] = (freq_cw >> 16) & 0x7f;
+
+		/* FIXME: there seems to be calculation error here... */
+		if (if_frequency == 0)
+			buf[2] = 0;
+
 		ret = af9033_wr_regs(state, 0x800029, buf, 3);
 		if (ret < 0)
 			goto err;
@@ -934,13 +1013,24 @@ struct dvb_frontend *af9033_attach(const struct af9033_config *config,
 			buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]);
 
 	/* sleep */
-	ret = af9033_wr_reg(state, 0x80004c, 1);
-	if (ret < 0)
-		goto err;
+	switch (state->cfg.tuner) {
+	case AF9033_TUNER_IT9135_38:
+	case AF9033_TUNER_IT9135_51:
+	case AF9033_TUNER_IT9135_52:
+	case AF9033_TUNER_IT9135_60:
+	case AF9033_TUNER_IT9135_61:
+	case AF9033_TUNER_IT9135_62:
+		/* IT9135 did not like to sleep at that early */
+		break;
+	default:
+		ret = af9033_wr_reg(state, 0x80004c, 1);
+		if (ret < 0)
+			goto err;
 
-	ret = af9033_wr_reg(state, 0x800000, 0);
-	if (ret < 0)
-		goto err;
+		ret = af9033_wr_reg(state, 0x800000, 0);
+		if (ret < 0)
+			goto err;
+	}
 
 	/* configure internal TS mode */
 	switch (state->cfg.ts_mode) {
diff --git a/drivers/media/dvb-frontends/af9033.h b/drivers/media/dvb-frontends/af9033.h
index 82bd8c1513b6..c286e8f1ec02 100644
--- a/drivers/media/dvb-frontends/af9033.h
+++ b/drivers/media/dvb-frontends/af9033.h
@@ -22,6 +22,8 @@
 #ifndef AF9033_H
 #define AF9033_H
 
+#include <linux/kconfig.h>
+
 struct af9033_config {
 	/*
 	 * I2C address
@@ -36,6 +38,13 @@ struct af9033_config {
 	u32 clock;
 
 	/*
+	 * ADC multiplier
+	 */
+#define AF9033_ADC_MULTIPLIER_1X   0
+#define AF9033_ADC_MULTIPLIER_2X   1
+	u8 adc_multiplier;
+
+	/*
 	 * tuner
 	 */
 #define AF9033_TUNER_TUA9001     0x27 /* Infineon TUA 9001 */
@@ -44,6 +53,14 @@ struct af9033_config {
 #define AF9033_TUNER_MXL5007T    0xa0 /* MaxLinear MxL5007T */
 #define AF9033_TUNER_TDA18218    0xa1 /* NXP TDA 18218HN */
 #define AF9033_TUNER_FC2580      0x32 /* FCI FC2580 */
+/* 50-5f Omega */
+#define AF9033_TUNER_IT9135_38   0x38 /* Omega */
+#define AF9033_TUNER_IT9135_51   0x51 /* Omega LNA config 1 */
+#define AF9033_TUNER_IT9135_52   0x52 /* Omega LNA config 2 */
+/* 60-6f Omega v2 */
+#define AF9033_TUNER_IT9135_60   0x60 /* Omega v2 */
+#define AF9033_TUNER_IT9135_61   0x61 /* Omega v2 LNA config 1 */
+#define AF9033_TUNER_IT9135_62   0x62 /* Omega v2 LNA config 2 */
 	u8 tuner;
 
 	/*
@@ -61,8 +78,7 @@ struct af9033_config {
 };
 
 
-#if defined(CONFIG_DVB_AF9033) || \
-	(defined(CONFIG_DVB_AF9033_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_AF9033)
 extern struct dvb_frontend *af9033_attach(const struct af9033_config *config,
 	struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/af9033_priv.h b/drivers/media/dvb-frontends/af9033_priv.h
index e9bd78265543..fc2ad581e302 100644
--- a/drivers/media/dvb-frontends/af9033_priv.h
+++ b/drivers/media/dvb-frontends/af9033_priv.h
@@ -547,5 +547,1509 @@ static const struct reg_val tuner_init_fc2580[] = {
 	{ 0x80f1e6, 0x01 },
 };
 
-#endif /* AF9033_PRIV_H */
+static const struct reg_val ofsm_init_it9135_v1[] = {
+	{ 0x800051, 0x01 },
+	{ 0x800070, 0x0a },
+	{ 0x80007e, 0x04 },
+	{ 0x800081, 0x0a },
+	{ 0x80008a, 0x01 },
+	{ 0x80008e, 0x01 },
+	{ 0x800092, 0x06 },
+	{ 0x800099, 0x01 },
+	{ 0x80009f, 0xe1 },
+	{ 0x8000a0, 0xcf },
+	{ 0x8000a3, 0x01 },
+	{ 0x8000a5, 0x01 },
+	{ 0x8000a6, 0x01 },
+	{ 0x8000a9, 0x00 },
+	{ 0x8000aa, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c6, 0x19 },
+	{ 0x80f000, 0x0f },
+	{ 0x80f016, 0x10 },
+	{ 0x80f017, 0x04 },
+	{ 0x80f018, 0x05 },
+	{ 0x80f019, 0x04 },
+	{ 0x80f01a, 0x05 },
+	{ 0x80f021, 0x03 },
+	{ 0x80f022, 0x0a },
+	{ 0x80f023, 0x0a },
+	{ 0x80f02b, 0x00 },
+	{ 0x80f02c, 0x01 },
+	{ 0x80f064, 0x03 },
+	{ 0x80f065, 0xf9 },
+	{ 0x80f066, 0x03 },
+	{ 0x80f067, 0x01 },
+	{ 0x80f06f, 0xe0 },
+	{ 0x80f070, 0x03 },
+	{ 0x80f072, 0x0f },
+	{ 0x80f073, 0x03 },
+	{ 0x80f078, 0x00 },
+	{ 0x80f087, 0x00 },
+	{ 0x80f09b, 0x3f },
+	{ 0x80f09c, 0x00 },
+	{ 0x80f09d, 0x20 },
+	{ 0x80f09e, 0x00 },
+	{ 0x80f09f, 0x0c },
+	{ 0x80f0a0, 0x00 },
+	{ 0x80f130, 0x04 },
+	{ 0x80f132, 0x04 },
+	{ 0x80f144, 0x1a },
+	{ 0x80f146, 0x00 },
+	{ 0x80f14a, 0x01 },
+	{ 0x80f14c, 0x00 },
+	{ 0x80f14d, 0x00 },
+	{ 0x80f14f, 0x04 },
+	{ 0x80f158, 0x7f },
+	{ 0x80f15a, 0x00 },
+	{ 0x80f15b, 0x08 },
+	{ 0x80f15d, 0x03 },
+	{ 0x80f15e, 0x05 },
+	{ 0x80f163, 0x05 },
+	{ 0x80f166, 0x01 },
+	{ 0x80f167, 0x40 },
+	{ 0x80f168, 0x0f },
+	{ 0x80f17a, 0x00 },
+	{ 0x80f17b, 0x00 },
+	{ 0x80f183, 0x01 },
+	{ 0x80f19d, 0x40 },
+	{ 0x80f1bc, 0x36 },
+	{ 0x80f1bd, 0x00 },
+	{ 0x80f1cb, 0xa0 },
+	{ 0x80f1cc, 0x01 },
+	{ 0x80f204, 0x10 },
+	{ 0x80f214, 0x00 },
+	{ 0x80f40e, 0x0a },
+	{ 0x80f40f, 0x40 },
+	{ 0x80f410, 0x08 },
+	{ 0x80f55f, 0x0a },
+	{ 0x80f561, 0x15 },
+	{ 0x80f562, 0x20 },
+	{ 0x80f5df, 0xfb },
+	{ 0x80f5e0, 0x00 },
+	{ 0x80f5e3, 0x09 },
+	{ 0x80f5e4, 0x01 },
+	{ 0x80f5e5, 0x01 },
+	{ 0x80f5f8, 0x01 },
+	{ 0x80f5fd, 0x01 },
+	{ 0x80f600, 0x05 },
+	{ 0x80f601, 0x08 },
+	{ 0x80f602, 0x0b },
+	{ 0x80f603, 0x0e },
+	{ 0x80f604, 0x11 },
+	{ 0x80f605, 0x14 },
+	{ 0x80f606, 0x17 },
+	{ 0x80f607, 0x1f },
+	{ 0x80f60e, 0x00 },
+	{ 0x80f60f, 0x04 },
+	{ 0x80f610, 0x32 },
+	{ 0x80f611, 0x10 },
+	{ 0x80f707, 0xfc },
+	{ 0x80f708, 0x00 },
+	{ 0x80f709, 0x37 },
+	{ 0x80f70a, 0x00 },
+	{ 0x80f78b, 0x01 },
+	{ 0x80f80f, 0x40 },
+	{ 0x80f810, 0x54 },
+	{ 0x80f811, 0x5a },
+	{ 0x80f905, 0x01 },
+	{ 0x80fb06, 0x03 },
+	{ 0x80fd8b, 0x00 },
+};
+
+/* ITE Tech IT9135 Omega tuner init
+   AF9033_TUNER_IT9135_38   = 0x38 */
+static const struct reg_val tuner_init_it9135_38[] = {
+	{ 0x800043, 0x00 },
+	{ 0x800046, 0x38 },
+	{ 0x800051, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0x0a },
+	{ 0x800070, 0x0a },
+	{ 0x800071, 0x05 },
+	{ 0x800072, 0x02 },
+	{ 0x800075, 0x8c },
+	{ 0x800076, 0x8c },
+	{ 0x800077, 0x8c },
+	{ 0x800078, 0xc8 },
+	{ 0x800079, 0x01 },
+	{ 0x80007e, 0x04 },
+	{ 0x80007f, 0x00 },
+	{ 0x800081, 0x0a },
+	{ 0x800082, 0x12 },
+	{ 0x800083, 0x02 },
+	{ 0x800084, 0x0a },
+	{ 0x800085, 0x03 },
+	{ 0x800086, 0xc8 },
+	{ 0x800087, 0xb8 },
+	{ 0x800088, 0xd0 },
+	{ 0x800089, 0xc3 },
+	{ 0x80008a, 0x01 },
+	{ 0x80008e, 0x01 },
+	{ 0x800092, 0x06 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x800099, 0x01 },
+	{ 0x80009b, 0x3c },
+	{ 0x80009c, 0x28 },
+	{ 0x80009f, 0xe1 },
+	{ 0x8000a0, 0xcf },
+	{ 0x8000a3, 0x01 },
+	{ 0x8000a4, 0x5a },
+	{ 0x8000a5, 0x01 },
+	{ 0x8000a6, 0x01 },
+	{ 0x8000a9, 0x00 },
+	{ 0x8000aa, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000b3, 0x02 },
+	{ 0x8000b4, 0x32 },
+	{ 0x8000b6, 0x14 },
+	{ 0x8000c0, 0x11 },
+	{ 0x8000c1, 0x00 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c4, 0x00 },
+	{ 0x8000c6, 0x19 },
+	{ 0x8000c7, 0x00 },
+	{ 0x8000cc, 0x2e },
+	{ 0x8000cd, 0x51 },
+	{ 0x8000ce, 0x33 },
+	{ 0x8000f3, 0x05 },
+	{ 0x8000f4, 0x8c },
+	{ 0x8000f5, 0x8c },
+	{ 0x8000f8, 0x03 },
+	{ 0x8000f9, 0x06 },
+	{ 0x8000fa, 0x06 },
+	{ 0x8000fc, 0x02 },
+	{ 0x8000fd, 0x02 },
+	{ 0x8000fe, 0x02 },
+	{ 0x8000ff, 0x09 },
+	{ 0x800100, 0x50 },
+	{ 0x800101, 0x7b },
+	{ 0x800102, 0x77 },
+	{ 0x800103, 0x00 },
+	{ 0x800104, 0x02 },
+	{ 0x800105, 0xc8 },
+	{ 0x800106, 0x05 },
+	{ 0x800107, 0x7b },
+	{ 0x800109, 0x02 },
+	{ 0x800115, 0x0a },
+	{ 0x800116, 0x03 },
+	{ 0x800117, 0x02 },
+	{ 0x800118, 0x80 },
+	{ 0x80011a, 0xc8 },
+	{ 0x80011b, 0x7b },
+	{ 0x80011c, 0x8a },
+	{ 0x80011d, 0xa0 },
+	{ 0x800122, 0x02 },
+	{ 0x800123, 0x18 },
+	{ 0x800124, 0xc3 },
+	{ 0x800127, 0x00 },
+	{ 0x800128, 0x07 },
+	{ 0x80012a, 0x53 },
+	{ 0x80012b, 0x51 },
+	{ 0x80012c, 0x4e },
+	{ 0x80012d, 0x43 },
+	{ 0x800137, 0x01 },
+	{ 0x800138, 0x00 },
+	{ 0x800139, 0x07 },
+	{ 0x80013a, 0x00 },
+	{ 0x80013b, 0x06 },
+	{ 0x80013d, 0x00 },
+	{ 0x80013e, 0x01 },
+	{ 0x80013f, 0x5b },
+	{ 0x800140, 0xc8 },
+	{ 0x800141, 0x59 },
+	{ 0x80f000, 0x0f },
+	{ 0x80f016, 0x10 },
+	{ 0x80f017, 0x04 },
+	{ 0x80f018, 0x05 },
+	{ 0x80f019, 0x04 },
+	{ 0x80f01a, 0x05 },
+	{ 0x80f01f, 0x8c },
+	{ 0x80f020, 0x00 },
+	{ 0x80f021, 0x03 },
+	{ 0x80f022, 0x0a },
+	{ 0x80f023, 0x0a },
+	{ 0x80f029, 0x8c },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f02b, 0x00 },
+	{ 0x80f02c, 0x01 },
+	{ 0x80f064, 0x03 },
+	{ 0x80f065, 0xf9 },
+	{ 0x80f066, 0x03 },
+	{ 0x80f067, 0x01 },
+	{ 0x80f06f, 0xe0 },
+	{ 0x80f070, 0x03 },
+	{ 0x80f072, 0x0f },
+	{ 0x80f073, 0x03 },
+	{ 0x80f077, 0x01 },
+	{ 0x80f078, 0x00 },
+	{ 0x80f085, 0x00 },
+	{ 0x80f086, 0x02 },
+	{ 0x80f087, 0x00 },
+	{ 0x80f09b, 0x3f },
+	{ 0x80f09c, 0x00 },
+	{ 0x80f09d, 0x20 },
+	{ 0x80f09e, 0x00 },
+	{ 0x80f09f, 0x0c },
+	{ 0x80f0a0, 0x00 },
+	{ 0x80f130, 0x04 },
+	{ 0x80f132, 0x04 },
+	{ 0x80f144, 0x1a },
+	{ 0x80f146, 0x00 },
+	{ 0x80f14a, 0x01 },
+	{ 0x80f14c, 0x00 },
+	{ 0x80f14d, 0x00 },
+	{ 0x80f14f, 0x04 },
+	{ 0x80f158, 0x7f },
+	{ 0x80f15a, 0x00 },
+	{ 0x80f15b, 0x08 },
+	{ 0x80f15d, 0x03 },
+	{ 0x80f15e, 0x05 },
+	{ 0x80f163, 0x05 },
+	{ 0x80f166, 0x01 },
+	{ 0x80f167, 0x40 },
+	{ 0x80f168, 0x0f },
+	{ 0x80f17a, 0x00 },
+	{ 0x80f17b, 0x00 },
+	{ 0x80f183, 0x01 },
+	{ 0x80f19d, 0x40 },
+	{ 0x80f1bc, 0x36 },
+	{ 0x80f1bd, 0x00 },
+	{ 0x80f1cb, 0xa0 },
+	{ 0x80f1cc, 0x01 },
+	{ 0x80f204, 0x10 },
+	{ 0x80f214, 0x00 },
+	{ 0x80f24c, 0x88 },
+	{ 0x80f24d, 0x95 },
+	{ 0x80f24e, 0x9a },
+	{ 0x80f24f, 0x90 },
+	{ 0x80f25a, 0x07 },
+	{ 0x80f25b, 0xe8 },
+	{ 0x80f25c, 0x03 },
+	{ 0x80f25d, 0xb0 },
+	{ 0x80f25e, 0x04 },
+	{ 0x80f270, 0x01 },
+	{ 0x80f271, 0x02 },
+	{ 0x80f272, 0x01 },
+	{ 0x80f273, 0x02 },
+	{ 0x80f40e, 0x0a },
+	{ 0x80f40f, 0x40 },
+	{ 0x80f410, 0x08 },
+	{ 0x80f55f, 0x0a },
+	{ 0x80f561, 0x15 },
+	{ 0x80f562, 0x20 },
+	{ 0x80f5df, 0xfb },
+	{ 0x80f5e0, 0x00 },
+	{ 0x80f5e3, 0x09 },
+	{ 0x80f5e4, 0x01 },
+	{ 0x80f5e5, 0x01 },
+	{ 0x80f5f8, 0x01 },
+	{ 0x80f5fd, 0x01 },
+	{ 0x80f600, 0x05 },
+	{ 0x80f601, 0x08 },
+	{ 0x80f602, 0x0b },
+	{ 0x80f603, 0x0e },
+	{ 0x80f604, 0x11 },
+	{ 0x80f605, 0x14 },
+	{ 0x80f606, 0x17 },
+	{ 0x80f607, 0x1f },
+	{ 0x80f60e, 0x00 },
+	{ 0x80f60f, 0x04 },
+	{ 0x80f610, 0x32 },
+	{ 0x80f611, 0x10 },
+	{ 0x80f707, 0xfc },
+	{ 0x80f708, 0x00 },
+	{ 0x80f709, 0x37 },
+	{ 0x80f70a, 0x00 },
+	{ 0x80f78b, 0x01 },
+	{ 0x80f80f, 0x40 },
+	{ 0x80f810, 0x54 },
+	{ 0x80f811, 0x5a },
+	{ 0x80f905, 0x01 },
+	{ 0x80fb06, 0x03 },
+	{ 0x80fd8b, 0x00 },
+};
+
+/* ITE Tech IT9135 Omega LNA config 1 tuner init
+   AF9033_TUNER_IT9135_51   = 0x51 */
+static const struct reg_val tuner_init_it9135_51[] = {
+	{ 0x800043, 0x00 },
+	{ 0x800046, 0x51 },
+	{ 0x800051, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0x0a },
+	{ 0x800070, 0x0a },
+	{ 0x800071, 0x06 },
+	{ 0x800072, 0x02 },
+	{ 0x800075, 0x8c },
+	{ 0x800076, 0x8c },
+	{ 0x800077, 0x8c },
+	{ 0x800078, 0xc8 },
+	{ 0x800079, 0x01 },
+	{ 0x80007e, 0x04 },
+	{ 0x80007f, 0x00 },
+	{ 0x800081, 0x0a },
+	{ 0x800082, 0x12 },
+	{ 0x800083, 0x02 },
+	{ 0x800084, 0x0a },
+	{ 0x800085, 0x03 },
+	{ 0x800086, 0xc0 },
+	{ 0x800087, 0x96 },
+	{ 0x800088, 0xcf },
+	{ 0x800089, 0xc3 },
+	{ 0x80008a, 0x01 },
+	{ 0x80008e, 0x01 },
+	{ 0x800092, 0x06 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x800099, 0x01 },
+	{ 0x80009b, 0x3c },
+	{ 0x80009c, 0x28 },
+	{ 0x80009f, 0xe1 },
+	{ 0x8000a0, 0xcf },
+	{ 0x8000a3, 0x01 },
+	{ 0x8000a4, 0x5a },
+	{ 0x8000a5, 0x01 },
+	{ 0x8000a6, 0x01 },
+	{ 0x8000a9, 0x00 },
+	{ 0x8000aa, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000b3, 0x02 },
+	{ 0x8000b4, 0x3c },
+	{ 0x8000b6, 0x14 },
+	{ 0x8000c0, 0x11 },
+	{ 0x8000c1, 0x00 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c4, 0x00 },
+	{ 0x8000c6, 0x19 },
+	{ 0x8000c7, 0x00 },
+	{ 0x8000cc, 0x2e },
+	{ 0x8000cd, 0x51 },
+	{ 0x8000ce, 0x33 },
+	{ 0x8000f3, 0x05 },
+	{ 0x8000f4, 0x8c },
+	{ 0x8000f5, 0x8c },
+	{ 0x8000f8, 0x03 },
+	{ 0x8000f9, 0x06 },
+	{ 0x8000fa, 0x06 },
+	{ 0x8000fc, 0x03 },
+	{ 0x8000fd, 0x02 },
+	{ 0x8000fe, 0x02 },
+	{ 0x8000ff, 0x09 },
+	{ 0x800100, 0x50 },
+	{ 0x800101, 0x7a },
+	{ 0x800102, 0x77 },
+	{ 0x800103, 0x01 },
+	{ 0x800104, 0x02 },
+	{ 0x800105, 0xb0 },
+	{ 0x800106, 0x02 },
+	{ 0x800107, 0x7a },
+	{ 0x800109, 0x02 },
+	{ 0x800115, 0x0a },
+	{ 0x800116, 0x03 },
+	{ 0x800117, 0x02 },
+	{ 0x800118, 0x80 },
+	{ 0x80011a, 0xc0 },
+	{ 0x80011b, 0x7a },
+	{ 0x80011c, 0xac },
+	{ 0x80011d, 0x8c },
+	{ 0x800122, 0x02 },
+	{ 0x800123, 0x70 },
+	{ 0x800124, 0xa4 },
+	{ 0x800127, 0x00 },
+	{ 0x800128, 0x07 },
+	{ 0x80012a, 0x53 },
+	{ 0x80012b, 0x51 },
+	{ 0x80012c, 0x4e },
+	{ 0x80012d, 0x43 },
+	{ 0x800137, 0x01 },
+	{ 0x800138, 0x00 },
+	{ 0x800139, 0x07 },
+	{ 0x80013a, 0x00 },
+	{ 0x80013b, 0x06 },
+	{ 0x80013d, 0x00 },
+	{ 0x80013e, 0x01 },
+	{ 0x80013f, 0x5b },
+	{ 0x800140, 0xc0 },
+	{ 0x800141, 0x59 },
+	{ 0x80f000, 0x0f },
+	{ 0x80f016, 0x10 },
+	{ 0x80f017, 0x04 },
+	{ 0x80f018, 0x05 },
+	{ 0x80f019, 0x04 },
+	{ 0x80f01a, 0x05 },
+	{ 0x80f01f, 0x8c },
+	{ 0x80f020, 0x00 },
+	{ 0x80f021, 0x03 },
+	{ 0x80f022, 0x0a },
+	{ 0x80f023, 0x0a },
+	{ 0x80f029, 0x8c },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f02b, 0x00 },
+	{ 0x80f02c, 0x01 },
+	{ 0x80f064, 0x03 },
+	{ 0x80f065, 0xf9 },
+	{ 0x80f066, 0x03 },
+	{ 0x80f067, 0x01 },
+	{ 0x80f06f, 0xe0 },
+	{ 0x80f070, 0x03 },
+	{ 0x80f072, 0x0f },
+	{ 0x80f073, 0x03 },
+	{ 0x80f077, 0x01 },
+	{ 0x80f078, 0x00 },
+	{ 0x80f085, 0xc0 },
+	{ 0x80f086, 0x01 },
+	{ 0x80f087, 0x00 },
+	{ 0x80f09b, 0x3f },
+	{ 0x80f09c, 0x00 },
+	{ 0x80f09d, 0x20 },
+	{ 0x80f09e, 0x00 },
+	{ 0x80f09f, 0x0c },
+	{ 0x80f0a0, 0x00 },
+	{ 0x80f130, 0x04 },
+	{ 0x80f132, 0x04 },
+	{ 0x80f144, 0x1a },
+	{ 0x80f146, 0x00 },
+	{ 0x80f14a, 0x01 },
+	{ 0x80f14c, 0x00 },
+	{ 0x80f14d, 0x00 },
+	{ 0x80f14f, 0x04 },
+	{ 0x80f158, 0x7f },
+	{ 0x80f15a, 0x00 },
+	{ 0x80f15b, 0x08 },
+	{ 0x80f15d, 0x03 },
+	{ 0x80f15e, 0x05 },
+	{ 0x80f163, 0x05 },
+	{ 0x80f166, 0x01 },
+	{ 0x80f167, 0x40 },
+	{ 0x80f168, 0x0f },
+	{ 0x80f17a, 0x00 },
+	{ 0x80f17b, 0x00 },
+	{ 0x80f183, 0x01 },
+	{ 0x80f19d, 0x40 },
+	{ 0x80f1bc, 0x36 },
+	{ 0x80f1bd, 0x00 },
+	{ 0x80f1cb, 0xa0 },
+	{ 0x80f1cc, 0x01 },
+	{ 0x80f204, 0x10 },
+	{ 0x80f214, 0x00 },
+	{ 0x80f24c, 0x88 },
+	{ 0x80f24d, 0x95 },
+	{ 0x80f24e, 0x9a },
+	{ 0x80f24f, 0x90 },
+	{ 0x80f25a, 0x07 },
+	{ 0x80f25b, 0xe8 },
+	{ 0x80f25c, 0x03 },
+	{ 0x80f25d, 0xb0 },
+	{ 0x80f25e, 0x04 },
+	{ 0x80f270, 0x01 },
+	{ 0x80f271, 0x02 },
+	{ 0x80f272, 0x01 },
+	{ 0x80f273, 0x02 },
+	{ 0x80f40e, 0x0a },
+	{ 0x80f40f, 0x40 },
+	{ 0x80f410, 0x08 },
+	{ 0x80f55f, 0x0a },
+	{ 0x80f561, 0x15 },
+	{ 0x80f562, 0x20 },
+	{ 0x80f5df, 0xfb },
+	{ 0x80f5e0, 0x00 },
+	{ 0x80f5e3, 0x09 },
+	{ 0x80f5e4, 0x01 },
+	{ 0x80f5e5, 0x01 },
+	{ 0x80f5f8, 0x01 },
+	{ 0x80f5fd, 0x01 },
+	{ 0x80f600, 0x05 },
+	{ 0x80f601, 0x08 },
+	{ 0x80f602, 0x0b },
+	{ 0x80f603, 0x0e },
+	{ 0x80f604, 0x11 },
+	{ 0x80f605, 0x14 },
+	{ 0x80f606, 0x17 },
+	{ 0x80f607, 0x1f },
+	{ 0x80f60e, 0x00 },
+	{ 0x80f60f, 0x04 },
+	{ 0x80f610, 0x32 },
+	{ 0x80f611, 0x10 },
+	{ 0x80f707, 0xfc },
+	{ 0x80f708, 0x00 },
+	{ 0x80f709, 0x37 },
+	{ 0x80f70a, 0x00 },
+	{ 0x80f78b, 0x01 },
+	{ 0x80f80f, 0x40 },
+	{ 0x80f810, 0x54 },
+	{ 0x80f811, 0x5a },
+	{ 0x80f905, 0x01 },
+	{ 0x80fb06, 0x03 },
+	{ 0x80fd8b, 0x00 },
+};
+
+/* ITE Tech IT9135 Omega LNA config 2 tuner init
+   AF9033_TUNER_IT9135_52   = 0x52 */
+static const struct reg_val tuner_init_it9135_52[] = {
+	{ 0x800043, 0x00 },
+	{ 0x800046, 0x52 },
+	{ 0x800051, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0x10 },
+	{ 0x800070, 0x0a },
+	{ 0x800071, 0x05 },
+	{ 0x800072, 0x02 },
+	{ 0x800075, 0x8c },
+	{ 0x800076, 0x8c },
+	{ 0x800077, 0x8c },
+	{ 0x800078, 0xa0 },
+	{ 0x800079, 0x01 },
+	{ 0x80007e, 0x04 },
+	{ 0x80007f, 0x00 },
+	{ 0x800081, 0x0a },
+	{ 0x800082, 0x17 },
+	{ 0x800083, 0x03 },
+	{ 0x800084, 0x0a },
+	{ 0x800085, 0x03 },
+	{ 0x800086, 0xb3 },
+	{ 0x800087, 0x97 },
+	{ 0x800088, 0xc0 },
+	{ 0x800089, 0x9e },
+	{ 0x80008a, 0x01 },
+	{ 0x80008e, 0x01 },
+	{ 0x800092, 0x06 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x800099, 0x01 },
+	{ 0x80009b, 0x3c },
+	{ 0x80009c, 0x28 },
+	{ 0x80009f, 0xe1 },
+	{ 0x8000a0, 0xcf },
+	{ 0x8000a3, 0x01 },
+	{ 0x8000a4, 0x5c },
+	{ 0x8000a5, 0x01 },
+	{ 0x8000a6, 0x01 },
+	{ 0x8000a9, 0x00 },
+	{ 0x8000aa, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000b3, 0x02 },
+	{ 0x8000b4, 0x3c },
+	{ 0x8000b6, 0x14 },
+	{ 0x8000c0, 0x11 },
+	{ 0x8000c1, 0x00 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c4, 0x00 },
+	{ 0x8000c6, 0x19 },
+	{ 0x8000c7, 0x00 },
+	{ 0x8000cc, 0x2e },
+	{ 0x8000cd, 0x51 },
+	{ 0x8000ce, 0x33 },
+	{ 0x8000f3, 0x05 },
+	{ 0x8000f4, 0x91 },
+	{ 0x8000f5, 0x8c },
+	{ 0x8000f8, 0x03 },
+	{ 0x8000f9, 0x06 },
+	{ 0x8000fa, 0x06 },
+	{ 0x8000fc, 0x03 },
+	{ 0x8000fd, 0x02 },
+	{ 0x8000fe, 0x02 },
+	{ 0x8000ff, 0x09 },
+	{ 0x800100, 0x50 },
+	{ 0x800101, 0x74 },
+	{ 0x800102, 0x77 },
+	{ 0x800103, 0x02 },
+	{ 0x800104, 0x02 },
+	{ 0x800105, 0xa4 },
+	{ 0x800106, 0x02 },
+	{ 0x800107, 0x6e },
+	{ 0x800109, 0x02 },
+	{ 0x800115, 0x0a },
+	{ 0x800116, 0x03 },
+	{ 0x800117, 0x02 },
+	{ 0x800118, 0x80 },
+	{ 0x80011a, 0xcd },
+	{ 0x80011b, 0x62 },
+	{ 0x80011c, 0xa4 },
+	{ 0x80011d, 0x8c },
+	{ 0x800122, 0x03 },
+	{ 0x800123, 0x18 },
+	{ 0x800124, 0x9e },
+	{ 0x800127, 0x00 },
+	{ 0x800128, 0x07 },
+	{ 0x80012a, 0x53 },
+	{ 0x80012b, 0x51 },
+	{ 0x80012c, 0x4e },
+	{ 0x80012d, 0x43 },
+	{ 0x800137, 0x00 },
+	{ 0x800138, 0x00 },
+	{ 0x800139, 0x07 },
+	{ 0x80013a, 0x00 },
+	{ 0x80013b, 0x06 },
+	{ 0x80013d, 0x00 },
+	{ 0x80013e, 0x01 },
+	{ 0x80013f, 0x5b },
+	{ 0x800140, 0xb6 },
+	{ 0x800141, 0x59 },
+	{ 0x80f000, 0x0f },
+	{ 0x80f016, 0x10 },
+	{ 0x80f017, 0x04 },
+	{ 0x80f018, 0x05 },
+	{ 0x80f019, 0x04 },
+	{ 0x80f01a, 0x05 },
+	{ 0x80f01f, 0x8c },
+	{ 0x80f020, 0x00 },
+	{ 0x80f021, 0x03 },
+	{ 0x80f022, 0x0a },
+	{ 0x80f023, 0x0a },
+	{ 0x80f029, 0x8c },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f02b, 0x00 },
+	{ 0x80f02c, 0x01 },
+	{ 0x80f064, 0x03 },
+	{ 0x80f065, 0xf9 },
+	{ 0x80f066, 0x03 },
+	{ 0x80f067, 0x01 },
+	{ 0x80f06f, 0xe0 },
+	{ 0x80f070, 0x03 },
+	{ 0x80f072, 0x0f },
+	{ 0x80f073, 0x03 },
+	{ 0x80f077, 0x01 },
+	{ 0x80f078, 0x00 },
+	{ 0x80f085, 0xc0 },
+	{ 0x80f086, 0x01 },
+	{ 0x80f087, 0x00 },
+	{ 0x80f09b, 0x3f },
+	{ 0x80f09c, 0x00 },
+	{ 0x80f09d, 0x20 },
+	{ 0x80f09e, 0x00 },
+	{ 0x80f09f, 0x0c },
+	{ 0x80f0a0, 0x00 },
+	{ 0x80f130, 0x04 },
+	{ 0x80f132, 0x04 },
+	{ 0x80f144, 0x1a },
+	{ 0x80f146, 0x00 },
+	{ 0x80f14a, 0x01 },
+	{ 0x80f14c, 0x00 },
+	{ 0x80f14d, 0x00 },
+	{ 0x80f14f, 0x04 },
+	{ 0x80f158, 0x7f },
+	{ 0x80f15a, 0x00 },
+	{ 0x80f15b, 0x08 },
+	{ 0x80f15d, 0x03 },
+	{ 0x80f15e, 0x05 },
+	{ 0x80f163, 0x05 },
+	{ 0x80f166, 0x01 },
+	{ 0x80f167, 0x40 },
+	{ 0x80f168, 0x0f },
+	{ 0x80f17a, 0x00 },
+	{ 0x80f17b, 0x00 },
+	{ 0x80f183, 0x01 },
+	{ 0x80f19d, 0x40 },
+	{ 0x80f1bc, 0x36 },
+	{ 0x80f1bd, 0x00 },
+	{ 0x80f1cb, 0xa0 },
+	{ 0x80f1cc, 0x01 },
+	{ 0x80f204, 0x10 },
+	{ 0x80f214, 0x00 },
+	{ 0x80f24c, 0x88 },
+	{ 0x80f24d, 0x95 },
+	{ 0x80f24e, 0x9a },
+	{ 0x80f24f, 0x90 },
+	{ 0x80f25a, 0x07 },
+	{ 0x80f25b, 0xe8 },
+	{ 0x80f25c, 0x03 },
+	{ 0x80f25d, 0xb0 },
+	{ 0x80f25e, 0x04 },
+	{ 0x80f270, 0x01 },
+	{ 0x80f271, 0x02 },
+	{ 0x80f272, 0x01 },
+	{ 0x80f273, 0x02 },
+	{ 0x80f40e, 0x0a },
+	{ 0x80f40f, 0x40 },
+	{ 0x80f410, 0x08 },
+	{ 0x80f55f, 0x0a },
+	{ 0x80f561, 0x15 },
+	{ 0x80f562, 0x20 },
+	{ 0x80f5df, 0xfb },
+	{ 0x80f5e0, 0x00 },
+	{ 0x80f5e3, 0x09 },
+	{ 0x80f5e4, 0x01 },
+	{ 0x80f5e5, 0x01 },
+	{ 0x80f5f8, 0x01 },
+	{ 0x80f5fd, 0x01 },
+	{ 0x80f600, 0x05 },
+	{ 0x80f601, 0x08 },
+	{ 0x80f602, 0x0b },
+	{ 0x80f603, 0x0e },
+	{ 0x80f604, 0x11 },
+	{ 0x80f605, 0x14 },
+	{ 0x80f606, 0x17 },
+	{ 0x80f607, 0x1f },
+	{ 0x80f60e, 0x00 },
+	{ 0x80f60f, 0x04 },
+	{ 0x80f610, 0x32 },
+	{ 0x80f611, 0x10 },
+	{ 0x80f707, 0xfc },
+	{ 0x80f708, 0x00 },
+	{ 0x80f709, 0x37 },
+	{ 0x80f70a, 0x00 },
+	{ 0x80f78b, 0x01 },
+	{ 0x80f80f, 0x40 },
+	{ 0x80f810, 0x54 },
+	{ 0x80f811, 0x5a },
+	{ 0x80f905, 0x01 },
+	{ 0x80fb06, 0x03 },
+	{ 0x80fd8b, 0x00 },
+};
+
+static const struct reg_val ofsm_init_it9135_v2[] = {
+	{ 0x800051, 0x01 },
+	{ 0x800070, 0x0a },
+	{ 0x80007e, 0x04 },
+	{ 0x800081, 0x0a },
+	{ 0x80008a, 0x01 },
+	{ 0x80008e, 0x01 },
+	{ 0x800092, 0x06 },
+	{ 0x800099, 0x01 },
+	{ 0x80009f, 0xe1 },
+	{ 0x8000a0, 0xcf },
+	{ 0x8000a3, 0x01 },
+	{ 0x8000a5, 0x01 },
+	{ 0x8000a6, 0x01 },
+	{ 0x8000a9, 0x00 },
+	{ 0x8000aa, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c6, 0x19 },
+	{ 0x80f000, 0x0f },
+	{ 0x80f02b, 0x00 },
+	{ 0x80f064, 0x03 },
+	{ 0x80f065, 0xf9 },
+	{ 0x80f066, 0x03 },
+	{ 0x80f067, 0x01 },
+	{ 0x80f06f, 0xe0 },
+	{ 0x80f070, 0x03 },
+	{ 0x80f072, 0x0f },
+	{ 0x80f073, 0x03 },
+	{ 0x80f078, 0x00 },
+	{ 0x80f087, 0x00 },
+	{ 0x80f09b, 0x3f },
+	{ 0x80f09c, 0x00 },
+	{ 0x80f09d, 0x20 },
+	{ 0x80f09e, 0x00 },
+	{ 0x80f09f, 0x0c },
+	{ 0x80f0a0, 0x00 },
+	{ 0x80f130, 0x04 },
+	{ 0x80f132, 0x04 },
+	{ 0x80f144, 0x1a },
+	{ 0x80f146, 0x00 },
+	{ 0x80f14a, 0x01 },
+	{ 0x80f14c, 0x00 },
+	{ 0x80f14d, 0x00 },
+	{ 0x80f14f, 0x04 },
+	{ 0x80f158, 0x7f },
+	{ 0x80f15a, 0x00 },
+	{ 0x80f15b, 0x08 },
+	{ 0x80f15d, 0x03 },
+	{ 0x80f15e, 0x05 },
+	{ 0x80f163, 0x05 },
+	{ 0x80f166, 0x01 },
+	{ 0x80f167, 0x40 },
+	{ 0x80f168, 0x0f },
+	{ 0x80f17a, 0x00 },
+	{ 0x80f17b, 0x00 },
+	{ 0x80f183, 0x01 },
+	{ 0x80f19d, 0x40 },
+	{ 0x80f1bc, 0x36 },
+	{ 0x80f1bd, 0x00 },
+	{ 0x80f1cb, 0xa0 },
+	{ 0x80f1cc, 0x01 },
+	{ 0x80f204, 0x10 },
+	{ 0x80f214, 0x00 },
+	{ 0x80f40e, 0x0a },
+	{ 0x80f40f, 0x40 },
+	{ 0x80f410, 0x08 },
+	{ 0x80f55f, 0x0a },
+	{ 0x80f561, 0x15 },
+	{ 0x80f562, 0x20 },
+	{ 0x80f5e3, 0x09 },
+	{ 0x80f5e4, 0x01 },
+	{ 0x80f5e5, 0x01 },
+	{ 0x80f600, 0x05 },
+	{ 0x80f601, 0x08 },
+	{ 0x80f602, 0x0b },
+	{ 0x80f603, 0x0e },
+	{ 0x80f604, 0x11 },
+	{ 0x80f605, 0x14 },
+	{ 0x80f606, 0x17 },
+	{ 0x80f607, 0x1f },
+	{ 0x80f60e, 0x00 },
+	{ 0x80f60f, 0x04 },
+	{ 0x80f610, 0x32 },
+	{ 0x80f611, 0x10 },
+	{ 0x80f707, 0xfc },
+	{ 0x80f708, 0x00 },
+	{ 0x80f709, 0x37 },
+	{ 0x80f70a, 0x00 },
+	{ 0x80f78b, 0x01 },
+	{ 0x80f80f, 0x40 },
+	{ 0x80f810, 0x54 },
+	{ 0x80f811, 0x5a },
+	{ 0x80f905, 0x01 },
+	{ 0x80fb06, 0x03 },
+	{ 0x80fd8b, 0x00 },
+};
+
+/* ITE Tech IT9135 Omega v2 tuner init
+   AF9033_TUNER_IT9135_60   = 0x60 */
+static const struct reg_val tuner_init_it9135_60[] = {
+	{ 0x800043, 0x00 },
+	{ 0x800046, 0x60 },
+	{ 0x800051, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0x0a },
+	{ 0x80006a, 0x03 },
+	{ 0x800070, 0x0a },
+	{ 0x800071, 0x05 },
+	{ 0x800072, 0x02 },
+	{ 0x800075, 0x8c },
+	{ 0x800076, 0x8c },
+	{ 0x800077, 0x8c },
+	{ 0x800078, 0x8c },
+	{ 0x800079, 0x01 },
+	{ 0x80007e, 0x04 },
+	{ 0x800081, 0x0a },
+	{ 0x800082, 0x18 },
+	{ 0x800084, 0x0a },
+	{ 0x800085, 0x33 },
+	{ 0x800086, 0xbe },
+	{ 0x800087, 0xa0 },
+	{ 0x800088, 0xc6 },
+	{ 0x800089, 0xb6 },
+	{ 0x80008a, 0x01 },
+	{ 0x80008e, 0x01 },
+	{ 0x800092, 0x06 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x800099, 0x01 },
+	{ 0x80009b, 0x3c },
+	{ 0x80009c, 0x28 },
+	{ 0x80009f, 0xe1 },
+	{ 0x8000a0, 0xcf },
+	{ 0x8000a3, 0x01 },
+	{ 0x8000a4, 0x5a },
+	{ 0x8000a5, 0x01 },
+	{ 0x8000a6, 0x01 },
+	{ 0x8000a9, 0x00 },
+	{ 0x8000aa, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000b3, 0x02 },
+	{ 0x8000b4, 0x3a },
+	{ 0x8000b6, 0x14 },
+	{ 0x8000c0, 0x11 },
+	{ 0x8000c1, 0x00 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c3, 0x01 },
+	{ 0x8000c4, 0x00 },
+	{ 0x8000c6, 0x19 },
+	{ 0x8000c7, 0x00 },
+	{ 0x8000cb, 0x32 },
+	{ 0x8000cc, 0x2c },
+	{ 0x8000cd, 0x4f },
+	{ 0x8000ce, 0x30 },
+	{ 0x8000f3, 0x05 },
+	{ 0x8000f4, 0xa0 },
+	{ 0x8000f5, 0x8c },
+	{ 0x8000f8, 0x03 },
+	{ 0x8000f9, 0x06 },
+	{ 0x8000fa, 0x06 },
+	{ 0x8000fc, 0x03 },
+	{ 0x8000fd, 0x03 },
+	{ 0x8000fe, 0x02 },
+	{ 0x8000ff, 0x0a },
+	{ 0x800100, 0x50 },
+	{ 0x800101, 0x7b },
+	{ 0x800102, 0x8c },
+	{ 0x800103, 0x00 },
+	{ 0x800104, 0x02 },
+	{ 0x800105, 0xbe },
+	{ 0x800106, 0x00 },
+	{ 0x800109, 0x02 },
+	{ 0x800115, 0x0a },
+	{ 0x800116, 0x03 },
+	{ 0x80011a, 0xbe },
+	{ 0x800124, 0xae },
+	{ 0x800127, 0x00 },
+	{ 0x80012a, 0x56 },
+	{ 0x80012b, 0x50 },
+	{ 0x80012c, 0x47 },
+	{ 0x80012d, 0x42 },
+	{ 0x800137, 0x00 },
+	{ 0x80013b, 0x08 },
+	{ 0x80013f, 0x5b },
+	{ 0x800141, 0x59 },
+	{ 0x800142, 0xf9 },
+	{ 0x800143, 0x19 },
+	{ 0x800144, 0x00 },
+	{ 0x800145, 0x8c },
+	{ 0x800146, 0x8c },
+	{ 0x800147, 0x8c },
+	{ 0x800148, 0x6e },
+	{ 0x800149, 0x8c },
+	{ 0x80014a, 0x50 },
+	{ 0x80014b, 0x8c },
+	{ 0x80014d, 0xac },
+	{ 0x80014e, 0xc6 },
+	{ 0x80014f, 0x03 },
+	{ 0x800151, 0x1e },
+	{ 0x800153, 0xbc },
+	{ 0x800178, 0x09 },
+	{ 0x800181, 0x94 },
+	{ 0x800182, 0x6e },
+	{ 0x800185, 0x24 },
+	{ 0x800189, 0xbe },
+	{ 0x80018c, 0x03 },
+	{ 0x80018d, 0x5f },
+	{ 0x80018f, 0xa0 },
+	{ 0x800190, 0x5a },
+	{ 0x80ed02, 0xff },
+	{ 0x80ee42, 0xff },
+	{ 0x80ee82, 0xff },
+	{ 0x80f000, 0x0f },
+	{ 0x80f01f, 0x8c },
+	{ 0x80f020, 0x00 },
+	{ 0x80f029, 0x8c },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f02b, 0x00 },
+	{ 0x80f064, 0x03 },
+	{ 0x80f065, 0xf9 },
+	{ 0x80f066, 0x03 },
+	{ 0x80f067, 0x01 },
+	{ 0x80f06f, 0xe0 },
+	{ 0x80f070, 0x03 },
+	{ 0x80f072, 0x0f },
+	{ 0x80f073, 0x03 },
+	{ 0x80f077, 0x01 },
+	{ 0x80f078, 0x00 },
+	{ 0x80f087, 0x00 },
+	{ 0x80f09b, 0x3f },
+	{ 0x80f09c, 0x00 },
+	{ 0x80f09d, 0x20 },
+	{ 0x80f09e, 0x00 },
+	{ 0x80f09f, 0x0c },
+	{ 0x80f0a0, 0x00 },
+	{ 0x80f130, 0x04 },
+	{ 0x80f132, 0x04 },
+	{ 0x80f144, 0x1a },
+	{ 0x80f146, 0x00 },
+	{ 0x80f14a, 0x01 },
+	{ 0x80f14c, 0x00 },
+	{ 0x80f14d, 0x00 },
+	{ 0x80f14f, 0x04 },
+	{ 0x80f158, 0x7f },
+	{ 0x80f15a, 0x00 },
+	{ 0x80f15b, 0x08 },
+	{ 0x80f15d, 0x03 },
+	{ 0x80f15e, 0x05 },
+	{ 0x80f163, 0x05 },
+	{ 0x80f166, 0x01 },
+	{ 0x80f167, 0x40 },
+	{ 0x80f168, 0x0f },
+	{ 0x80f17a, 0x00 },
+	{ 0x80f17b, 0x00 },
+	{ 0x80f183, 0x01 },
+	{ 0x80f19d, 0x40 },
+	{ 0x80f1bc, 0x36 },
+	{ 0x80f1bd, 0x00 },
+	{ 0x80f1cb, 0xa0 },
+	{ 0x80f1cc, 0x01 },
+	{ 0x80f204, 0x10 },
+	{ 0x80f214, 0x00 },
+	{ 0x80f24c, 0x88 },
+	{ 0x80f24d, 0x95 },
+	{ 0x80f24e, 0x9a },
+	{ 0x80f24f, 0x90 },
+	{ 0x80f25a, 0x07 },
+	{ 0x80f25b, 0xe8 },
+	{ 0x80f25c, 0x03 },
+	{ 0x80f25d, 0xb0 },
+	{ 0x80f25e, 0x04 },
+	{ 0x80f270, 0x01 },
+	{ 0x80f271, 0x02 },
+	{ 0x80f272, 0x01 },
+	{ 0x80f273, 0x02 },
+	{ 0x80f40e, 0x0a },
+	{ 0x80f40f, 0x40 },
+	{ 0x80f410, 0x08 },
+	{ 0x80f55f, 0x0a },
+	{ 0x80f561, 0x15 },
+	{ 0x80f562, 0x20 },
+	{ 0x80f5e3, 0x09 },
+	{ 0x80f5e4, 0x01 },
+	{ 0x80f5e5, 0x01 },
+	{ 0x80f600, 0x05 },
+	{ 0x80f601, 0x08 },
+	{ 0x80f602, 0x0b },
+	{ 0x80f603, 0x0e },
+	{ 0x80f604, 0x11 },
+	{ 0x80f605, 0x14 },
+	{ 0x80f606, 0x17 },
+	{ 0x80f607, 0x1f },
+	{ 0x80f60e, 0x00 },
+	{ 0x80f60f, 0x04 },
+	{ 0x80f610, 0x32 },
+	{ 0x80f611, 0x10 },
+	{ 0x80f707, 0xfc },
+	{ 0x80f708, 0x00 },
+	{ 0x80f709, 0x37 },
+	{ 0x80f70a, 0x00 },
+	{ 0x80f78b, 0x01 },
+	{ 0x80f80f, 0x40 },
+	{ 0x80f810, 0x54 },
+	{ 0x80f811, 0x5a },
+	{ 0x80f905, 0x01 },
+	{ 0x80fb06, 0x03 },
+	{ 0x80fd8b, 0x00 },
+};
 
+/* ITE Tech IT9135 Omega v2 LNA config 1 tuner init
+   AF9033_TUNER_IT9135_61   = 0x61 */
+static const struct reg_val tuner_init_it9135_61[] = {
+	{ 0x800043, 0x00 },
+	{ 0x800046, 0x61 },
+	{ 0x800051, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0x06 },
+	{ 0x80006a, 0x03 },
+	{ 0x800070, 0x0a },
+	{ 0x800071, 0x05 },
+	{ 0x800072, 0x02 },
+	{ 0x800075, 0x8c },
+	{ 0x800076, 0x8c },
+	{ 0x800077, 0x8c },
+	{ 0x800078, 0x90 },
+	{ 0x800079, 0x01 },
+	{ 0x80007e, 0x04 },
+	{ 0x800081, 0x0a },
+	{ 0x800082, 0x12 },
+	{ 0x800084, 0x0a },
+	{ 0x800085, 0x33 },
+	{ 0x800086, 0xbc },
+	{ 0x800087, 0x9c },
+	{ 0x800088, 0xcc },
+	{ 0x800089, 0xa8 },
+	{ 0x80008a, 0x01 },
+	{ 0x80008e, 0x01 },
+	{ 0x800092, 0x06 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x800099, 0x01 },
+	{ 0x80009b, 0x3c },
+	{ 0x80009c, 0x28 },
+	{ 0x80009f, 0xe1 },
+	{ 0x8000a0, 0xcf },
+	{ 0x8000a3, 0x01 },
+	{ 0x8000a4, 0x5c },
+	{ 0x8000a5, 0x01 },
+	{ 0x8000a6, 0x01 },
+	{ 0x8000a9, 0x00 },
+	{ 0x8000aa, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000b3, 0x02 },
+	{ 0x8000b4, 0x3a },
+	{ 0x8000b6, 0x14 },
+	{ 0x8000c0, 0x11 },
+	{ 0x8000c1, 0x00 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c3, 0x01 },
+	{ 0x8000c4, 0x00 },
+	{ 0x8000c6, 0x19 },
+	{ 0x8000c7, 0x00 },
+	{ 0x8000cb, 0x32 },
+	{ 0x8000cc, 0x2c },
+	{ 0x8000cd, 0x4f },
+	{ 0x8000ce, 0x30 },
+	{ 0x8000f3, 0x05 },
+	{ 0x8000f4, 0xa0 },
+	{ 0x8000f5, 0x8c },
+	{ 0x8000f8, 0x03 },
+	{ 0x8000f9, 0x06 },
+	{ 0x8000fa, 0x06 },
+	{ 0x8000fc, 0x03 },
+	{ 0x8000fd, 0x03 },
+	{ 0x8000fe, 0x02 },
+	{ 0x8000ff, 0x08 },
+	{ 0x800100, 0x50 },
+	{ 0x800101, 0x7b },
+	{ 0x800102, 0x8c },
+	{ 0x800103, 0x01 },
+	{ 0x800104, 0x02 },
+	{ 0x800105, 0xc8 },
+	{ 0x800106, 0x00 },
+	{ 0x800109, 0x02 },
+	{ 0x800115, 0x0a },
+	{ 0x800116, 0x03 },
+	{ 0x80011a, 0xc6 },
+	{ 0x800124, 0xa8 },
+	{ 0x800127, 0x00 },
+	{ 0x80012a, 0x59 },
+	{ 0x80012b, 0x50 },
+	{ 0x80012c, 0x47 },
+	{ 0x80012d, 0x42 },
+	{ 0x800137, 0x00 },
+	{ 0x80013b, 0x05 },
+	{ 0x80013f, 0x5b },
+	{ 0x800141, 0x59 },
+	{ 0x800142, 0xf9 },
+	{ 0x800143, 0x59 },
+	{ 0x800144, 0x01 },
+	{ 0x800145, 0x8c },
+	{ 0x800146, 0x8c },
+	{ 0x800147, 0x8c },
+	{ 0x800148, 0x7b },
+	{ 0x800149, 0x8c },
+	{ 0x80014a, 0x50 },
+	{ 0x80014b, 0x8c },
+	{ 0x80014d, 0xa8 },
+	{ 0x80014e, 0xc6 },
+	{ 0x80014f, 0x03 },
+	{ 0x800151, 0x28 },
+	{ 0x800153, 0xcc },
+	{ 0x800178, 0x09 },
+	{ 0x800181, 0x9c },
+	{ 0x800182, 0x76 },
+	{ 0x800185, 0x28 },
+	{ 0x800189, 0xaa },
+	{ 0x80018c, 0x03 },
+	{ 0x80018d, 0x5f },
+	{ 0x80018f, 0xfb },
+	{ 0x800190, 0x5c },
+	{ 0x80ed02, 0xff },
+	{ 0x80ee42, 0xff },
+	{ 0x80ee82, 0xff },
+	{ 0x80f000, 0x0f },
+	{ 0x80f01f, 0x8c },
+	{ 0x80f020, 0x00 },
+	{ 0x80f029, 0x8c },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f02b, 0x00 },
+	{ 0x80f064, 0x03 },
+	{ 0x80f065, 0xf9 },
+	{ 0x80f066, 0x03 },
+	{ 0x80f067, 0x01 },
+	{ 0x80f06f, 0xe0 },
+	{ 0x80f070, 0x03 },
+	{ 0x80f072, 0x0f },
+	{ 0x80f073, 0x03 },
+	{ 0x80f077, 0x01 },
+	{ 0x80f078, 0x00 },
+	{ 0x80f087, 0x00 },
+	{ 0x80f09b, 0x3f },
+	{ 0x80f09c, 0x00 },
+	{ 0x80f09d, 0x20 },
+	{ 0x80f09e, 0x00 },
+	{ 0x80f09f, 0x0c },
+	{ 0x80f0a0, 0x00 },
+	{ 0x80f130, 0x04 },
+	{ 0x80f132, 0x04 },
+	{ 0x80f144, 0x1a },
+	{ 0x80f146, 0x00 },
+	{ 0x80f14a, 0x01 },
+	{ 0x80f14c, 0x00 },
+	{ 0x80f14d, 0x00 },
+	{ 0x80f14f, 0x04 },
+	{ 0x80f158, 0x7f },
+	{ 0x80f15a, 0x00 },
+	{ 0x80f15b, 0x08 },
+	{ 0x80f15d, 0x03 },
+	{ 0x80f15e, 0x05 },
+	{ 0x80f163, 0x05 },
+	{ 0x80f166, 0x01 },
+	{ 0x80f167, 0x40 },
+	{ 0x80f168, 0x0f },
+	{ 0x80f17a, 0x00 },
+	{ 0x80f17b, 0x00 },
+	{ 0x80f183, 0x01 },
+	{ 0x80f19d, 0x40 },
+	{ 0x80f1bc, 0x36 },
+	{ 0x80f1bd, 0x00 },
+	{ 0x80f1cb, 0xa0 },
+	{ 0x80f1cc, 0x01 },
+	{ 0x80f204, 0x10 },
+	{ 0x80f214, 0x00 },
+	{ 0x80f24c, 0x88 },
+	{ 0x80f24d, 0x95 },
+	{ 0x80f24e, 0x9a },
+	{ 0x80f24f, 0x90 },
+	{ 0x80f25a, 0x07 },
+	{ 0x80f25b, 0xe8 },
+	{ 0x80f25c, 0x03 },
+	{ 0x80f25d, 0xb0 },
+	{ 0x80f25e, 0x04 },
+	{ 0x80f270, 0x01 },
+	{ 0x80f271, 0x02 },
+	{ 0x80f272, 0x01 },
+	{ 0x80f273, 0x02 },
+	{ 0x80f40e, 0x0a },
+	{ 0x80f40f, 0x40 },
+	{ 0x80f410, 0x08 },
+	{ 0x80f55f, 0x0a },
+	{ 0x80f561, 0x15 },
+	{ 0x80f562, 0x20 },
+	{ 0x80f5e3, 0x09 },
+	{ 0x80f5e4, 0x01 },
+	{ 0x80f5e5, 0x01 },
+	{ 0x80f600, 0x05 },
+	{ 0x80f601, 0x08 },
+	{ 0x80f602, 0x0b },
+	{ 0x80f603, 0x0e },
+	{ 0x80f604, 0x11 },
+	{ 0x80f605, 0x14 },
+	{ 0x80f606, 0x17 },
+	{ 0x80f607, 0x1f },
+	{ 0x80f60e, 0x00 },
+	{ 0x80f60f, 0x04 },
+	{ 0x80f610, 0x32 },
+	{ 0x80f611, 0x10 },
+	{ 0x80f707, 0xfc },
+	{ 0x80f708, 0x00 },
+	{ 0x80f709, 0x37 },
+	{ 0x80f70a, 0x00 },
+	{ 0x80f78b, 0x01 },
+	{ 0x80f80f, 0x40 },
+	{ 0x80f810, 0x54 },
+	{ 0x80f811, 0x5a },
+	{ 0x80f905, 0x01 },
+	{ 0x80fb06, 0x03 },
+	{ 0x80fd8b, 0x00 },
+};
+
+/* ITE Tech IT9135 Omega v2 LNA config 2 tuner init
+   AF9033_TUNER_IT9135_62   = 0x62 */
+static const struct reg_val tuner_init_it9135_62[] = {
+	{ 0x800043, 0x00 },
+	{ 0x800046, 0x62 },
+	{ 0x800051, 0x01 },
+	{ 0x80005f, 0x00 },
+	{ 0x800060, 0x00 },
+	{ 0x800068, 0x0a },
+	{ 0x80006a, 0x03 },
+	{ 0x800070, 0x0a },
+	{ 0x800071, 0x05 },
+	{ 0x800072, 0x02 },
+	{ 0x800075, 0x8c },
+	{ 0x800076, 0x8c },
+	{ 0x800077, 0x8c },
+	{ 0x800078, 0x8c },
+	{ 0x800079, 0x01 },
+	{ 0x80007e, 0x04 },
+	{ 0x800081, 0x0a },
+	{ 0x800082, 0x12 },
+	{ 0x800084, 0x0a },
+	{ 0x800085, 0x33 },
+	{ 0x800086, 0xb8 },
+	{ 0x800087, 0x9c },
+	{ 0x800088, 0xb2 },
+	{ 0x800089, 0xa6 },
+	{ 0x80008a, 0x01 },
+	{ 0x80008e, 0x01 },
+	{ 0x800092, 0x06 },
+	{ 0x800093, 0x00 },
+	{ 0x800094, 0x00 },
+	{ 0x800095, 0x00 },
+	{ 0x800096, 0x00 },
+	{ 0x800099, 0x01 },
+	{ 0x80009b, 0x3c },
+	{ 0x80009c, 0x28 },
+	{ 0x80009f, 0xe1 },
+	{ 0x8000a0, 0xcf },
+	{ 0x8000a3, 0x01 },
+	{ 0x8000a4, 0x5a },
+	{ 0x8000a5, 0x01 },
+	{ 0x8000a6, 0x01 },
+	{ 0x8000a9, 0x00 },
+	{ 0x8000aa, 0x01 },
+	{ 0x8000b0, 0x01 },
+	{ 0x8000b3, 0x02 },
+	{ 0x8000b4, 0x3a },
+	{ 0x8000b6, 0x14 },
+	{ 0x8000c0, 0x11 },
+	{ 0x8000c1, 0x00 },
+	{ 0x8000c2, 0x05 },
+	{ 0x8000c3, 0x01 },
+	{ 0x8000c4, 0x00 },
+	{ 0x8000c6, 0x19 },
+	{ 0x8000c7, 0x00 },
+	{ 0x8000cb, 0x32 },
+	{ 0x8000cc, 0x2c },
+	{ 0x8000cd, 0x4f },
+	{ 0x8000ce, 0x30 },
+	{ 0x8000f3, 0x05 },
+	{ 0x8000f4, 0x8c },
+	{ 0x8000f5, 0x8c },
+	{ 0x8000f8, 0x03 },
+	{ 0x8000f9, 0x06 },
+	{ 0x8000fa, 0x06 },
+	{ 0x8000fc, 0x02 },
+	{ 0x8000fd, 0x03 },
+	{ 0x8000fe, 0x02 },
+	{ 0x8000ff, 0x09 },
+	{ 0x800100, 0x50 },
+	{ 0x800101, 0x6e },
+	{ 0x800102, 0x8c },
+	{ 0x800103, 0x02 },
+	{ 0x800104, 0x02 },
+	{ 0x800105, 0xc2 },
+	{ 0x800106, 0x00 },
+	{ 0x800109, 0x02 },
+	{ 0x800115, 0x0a },
+	{ 0x800116, 0x03 },
+	{ 0x80011a, 0xb8 },
+	{ 0x800124, 0xa8 },
+	{ 0x800127, 0x00 },
+	{ 0x80012a, 0x53 },
+	{ 0x80012b, 0x51 },
+	{ 0x80012c, 0x4e },
+	{ 0x80012d, 0x43 },
+	{ 0x800137, 0x00 },
+	{ 0x80013b, 0x05 },
+	{ 0x80013f, 0x5b },
+	{ 0x800141, 0x59 },
+	{ 0x800142, 0xf9 },
+	{ 0x800143, 0x59 },
+	{ 0x800144, 0x00 },
+	{ 0x800145, 0x8c },
+	{ 0x800146, 0x8c },
+	{ 0x800147, 0x8c },
+	{ 0x800148, 0x7b },
+	{ 0x800149, 0x8c },
+	{ 0x80014a, 0x50 },
+	{ 0x80014b, 0x70 },
+	{ 0x80014d, 0x96 },
+	{ 0x80014e, 0xd0 },
+	{ 0x80014f, 0x03 },
+	{ 0x800151, 0x28 },
+	{ 0x800153, 0xb2 },
+	{ 0x800178, 0x09 },
+	{ 0x800181, 0x9c },
+	{ 0x800182, 0x6e },
+	{ 0x800185, 0x24 },
+	{ 0x800189, 0xb8 },
+	{ 0x80018c, 0x03 },
+	{ 0x80018d, 0x5f },
+	{ 0x80018f, 0xfb },
+	{ 0x800190, 0x5a },
+	{ 0x80ed02, 0xff },
+	{ 0x80ee42, 0xff },
+	{ 0x80ee82, 0xff },
+	{ 0x80f000, 0x0f },
+	{ 0x80f01f, 0x8c },
+	{ 0x80f020, 0x00 },
+	{ 0x80f029, 0x8c },
+	{ 0x80f02a, 0x00 },
+	{ 0x80f02b, 0x00 },
+	{ 0x80f064, 0x03 },
+	{ 0x80f065, 0xf9 },
+	{ 0x80f066, 0x03 },
+	{ 0x80f067, 0x01 },
+	{ 0x80f06f, 0xe0 },
+	{ 0x80f070, 0x03 },
+	{ 0x80f072, 0x0f },
+	{ 0x80f073, 0x03 },
+	{ 0x80f077, 0x01 },
+	{ 0x80f078, 0x00 },
+	{ 0x80f087, 0x00 },
+	{ 0x80f09b, 0x3f },
+	{ 0x80f09c, 0x00 },
+	{ 0x80f09d, 0x20 },
+	{ 0x80f09e, 0x00 },
+	{ 0x80f09f, 0x0c },
+	{ 0x80f0a0, 0x00 },
+	{ 0x80f130, 0x04 },
+	{ 0x80f132, 0x04 },
+	{ 0x80f144, 0x1a },
+	{ 0x80f146, 0x00 },
+	{ 0x80f14a, 0x01 },
+	{ 0x80f14c, 0x00 },
+	{ 0x80f14d, 0x00 },
+	{ 0x80f14f, 0x04 },
+	{ 0x80f158, 0x7f },
+	{ 0x80f15a, 0x00 },
+	{ 0x80f15b, 0x08 },
+	{ 0x80f15d, 0x03 },
+	{ 0x80f15e, 0x05 },
+	{ 0x80f163, 0x05 },
+	{ 0x80f166, 0x01 },
+	{ 0x80f167, 0x40 },
+	{ 0x80f168, 0x0f },
+	{ 0x80f17a, 0x00 },
+	{ 0x80f17b, 0x00 },
+	{ 0x80f183, 0x01 },
+	{ 0x80f19d, 0x40 },
+	{ 0x80f1bc, 0x36 },
+	{ 0x80f1bd, 0x00 },
+	{ 0x80f1cb, 0xa0 },
+	{ 0x80f1cc, 0x01 },
+	{ 0x80f204, 0x10 },
+	{ 0x80f214, 0x00 },
+	{ 0x80f24c, 0x88 },
+	{ 0x80f24d, 0x95 },
+	{ 0x80f24e, 0x9a },
+	{ 0x80f24f, 0x90 },
+	{ 0x80f25a, 0x07 },
+	{ 0x80f25b, 0xe8 },
+	{ 0x80f25c, 0x03 },
+	{ 0x80f25d, 0xb0 },
+	{ 0x80f25e, 0x04 },
+	{ 0x80f270, 0x01 },
+	{ 0x80f271, 0x02 },
+	{ 0x80f272, 0x01 },
+	{ 0x80f273, 0x02 },
+	{ 0x80f40e, 0x0a },
+	{ 0x80f40f, 0x40 },
+	{ 0x80f410, 0x08 },
+	{ 0x80f55f, 0x0a },
+	{ 0x80f561, 0x15 },
+	{ 0x80f562, 0x20 },
+	{ 0x80f5e3, 0x09 },
+	{ 0x80f5e4, 0x01 },
+	{ 0x80f5e5, 0x01 },
+	{ 0x80f600, 0x05 },
+	{ 0x80f601, 0x08 },
+	{ 0x80f602, 0x0b },
+	{ 0x80f603, 0x0e },
+	{ 0x80f604, 0x11 },
+	{ 0x80f605, 0x14 },
+	{ 0x80f606, 0x17 },
+	{ 0x80f607, 0x1f },
+	{ 0x80f60e, 0x00 },
+	{ 0x80f60f, 0x04 },
+	{ 0x80f610, 0x32 },
+	{ 0x80f611, 0x10 },
+	{ 0x80f707, 0xfc },
+	{ 0x80f708, 0x00 },
+	{ 0x80f709, 0x37 },
+	{ 0x80f70a, 0x00 },
+	{ 0x80f78b, 0x01 },
+	{ 0x80f80f, 0x40 },
+	{ 0x80f810, 0x54 },
+	{ 0x80f811, 0x5a },
+	{ 0x80f905, 0x01 },
+	{ 0x80fb06, 0x03 },
+	{ 0x80fd8b, 0x00 },
+};
+
+#endif /* AF9033_PRIV_H */
diff --git a/drivers/media/dvb-frontends/atbm8830.h b/drivers/media/dvb-frontends/atbm8830.h
index 024273374bd8..8e0ac98f8d08 100644
--- a/drivers/media/dvb-frontends/atbm8830.h
+++ b/drivers/media/dvb-frontends/atbm8830.h
@@ -22,6 +22,7 @@
 #ifndef __ATBM8830_H__
 #define __ATBM8830_H__
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include <linux/i2c.h>
 
@@ -60,8 +61,7 @@ struct atbm8830_config {
 	u8 agc_hold_loop;
 };
 
-#if defined(CONFIG_DVB_ATBM8830) || \
-	(defined(CONFIG_DVB_ATBM8830_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_ATBM8830)
 extern struct dvb_frontend *atbm8830_attach(const struct atbm8830_config *config,
 		struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/au8522.h b/drivers/media/dvb-frontends/au8522.h
index 565dcf31af57..83fe9a615619 100644
--- a/drivers/media/dvb-frontends/au8522.h
+++ b/drivers/media/dvb-frontends/au8522.h
@@ -22,6 +22,7 @@
 #ifndef __AU8522_H__
 #define __AU8522_H__
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 enum au8522_if_freq {
@@ -60,8 +61,7 @@ struct au8522_config {
 	enum au8522_if_freq qam_if;
 };
 
-#if defined(CONFIG_DVB_AU8522) || 				\
-	    (defined(CONFIG_DVB_AU8522_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_AU8522_DTV)
 extern struct dvb_frontend *au8522_attach(const struct au8522_config *config,
 					  struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/au8522_decoder.c b/drivers/media/dvb-frontends/au8522_decoder.c
index 5243ba6295cc..2099f21e374d 100644
--- a/drivers/media/dvb-frontends/au8522_decoder.c
+++ b/drivers/media/dvb-frontends/au8522_decoder.c
@@ -229,15 +229,11 @@ static void setup_decoder_defaults(struct au8522_state *state, u8 input_mode)
 	/* Provide reasonable defaults for picture tuning values */
 	au8522_writereg(state, AU8522_TVDEC_SHARPNESSREG009H, 0x07);
 	au8522_writereg(state, AU8522_TVDEC_BRIGHTNESS_REG00AH, 0xed);
-	state->brightness = 0xed - 128;
 	au8522_writereg(state, AU8522_TVDEC_CONTRAST_REG00BH, 0x79);
-	state->contrast = 0x79;
 	au8522_writereg(state, AU8522_TVDEC_SATURATION_CB_REG00CH, 0x80);
 	au8522_writereg(state, AU8522_TVDEC_SATURATION_CR_REG00DH, 0x80);
-	state->saturation = 0x80;
 	au8522_writereg(state, AU8522_TVDEC_HUE_H_REG00EH, 0x00);
 	au8522_writereg(state, AU8522_TVDEC_HUE_L_REG00FH, 0x00);
-	state->hue = 0x00;
 
 	/* Other decoder registers */
 	au8522_writereg(state, AU8522_TVDEC_INT_MASK_REG010H, 0x00);
@@ -489,75 +485,32 @@ static void set_audio_input(struct au8522_state *state, int aud_input)
 
 /* ----------------------------------------------------------------------- */
 
-static int au8522_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int au8522_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct au8522_state *state = to_state(sd);
+	struct au8522_state *state =
+		container_of(ctrl->handler, struct au8522_state, hdl);
 
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		state->brightness = ctrl->value;
 		au8522_writereg(state, AU8522_TVDEC_BRIGHTNESS_REG00AH,
-				ctrl->value - 128);
+				ctrl->val - 128);
 		break;
 	case V4L2_CID_CONTRAST:
-		state->contrast = ctrl->value;
 		au8522_writereg(state, AU8522_TVDEC_CONTRAST_REG00BH,
-				ctrl->value);
+				ctrl->val);
 		break;
 	case V4L2_CID_SATURATION:
-		state->saturation = ctrl->value;
 		au8522_writereg(state, AU8522_TVDEC_SATURATION_CB_REG00CH,
-				ctrl->value);
+				ctrl->val);
 		au8522_writereg(state, AU8522_TVDEC_SATURATION_CR_REG00DH,
-				ctrl->value);
+				ctrl->val);
 		break;
 	case V4L2_CID_HUE:
-		state->hue = ctrl->value;
 		au8522_writereg(state, AU8522_TVDEC_HUE_H_REG00EH,
-				ctrl->value >> 8);
+				ctrl->val >> 8);
 		au8522_writereg(state, AU8522_TVDEC_HUE_L_REG00FH,
-				ctrl->value & 0xFF);
-		break;
-	case V4L2_CID_AUDIO_VOLUME:
-	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-	case V4L2_CID_AUDIO_BALANCE:
-	case V4L2_CID_AUDIO_MUTE:
-		/* Not yet implemented */
-	default:
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int au8522_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct au8522_state *state = to_state(sd);
-
-	/* Note that we are using values cached in the state structure instead
-	   of reading the registers due to issues with i2c reads not working
-	   properly/consistently yet on the HVR-950q */
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = state->brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = state->contrast;
+				ctrl->val & 0xFF);
 		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = state->saturation;
-		break;
-	case V4L2_CID_HUE:
-		ctrl->value = state->hue;
-		break;
-	case V4L2_CID_AUDIO_VOLUME:
-	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-	case V4L2_CID_AUDIO_BALANCE:
-	case V4L2_CID_AUDIO_MUTE:
-		/* Not yet supported */
 	default:
 		return -EINVAL;
 	}
@@ -583,7 +536,7 @@ static int au8522_g_register(struct v4l2_subdev *sd,
 }
 
 static int au8522_s_register(struct v4l2_subdev *sd,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct au8522_state *state = to_state(sd);
@@ -616,26 +569,6 @@ static int au8522_s_stream(struct v4l2_subdev *sd, int enable)
 	return 0;
 }
 
-static int au8522_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_CONTRAST:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1,
-					    AU8522_TVDEC_CONTRAST_REG00BH_CVBS);
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 109);
-	case V4L2_CID_SATURATION:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 128);
-	case V4L2_CID_HUE:
-		return v4l2_ctrl_query_fill(qc, -32768, 32768, 1, 0);
-	default:
-		break;
-	}
-
-	qc->type = 0;
-	return -EINVAL;
-}
-
 static int au8522_reset(struct v4l2_subdev *sd, u32 val)
 {
 	struct au8522_state *state = to_state(sd);
@@ -712,20 +645,11 @@ static int au8522_g_chip_ident(struct v4l2_subdev *sd,
 	return v4l2_chip_ident_i2c_client(client, chip, state->id, state->rev);
 }
 
-static int au8522_log_status(struct v4l2_subdev *sd)
-{
-	/* FIXME: Add some status info here */
-	return 0;
-}
-
 /* ----------------------------------------------------------------------- */
 
 static const struct v4l2_subdev_core_ops au8522_core_ops = {
-	.log_status = au8522_log_status,
+	.log_status = v4l2_ctrl_subdev_log_status,
 	.g_chip_ident = au8522_g_chip_ident,
-	.g_ctrl = au8522_g_ctrl,
-	.s_ctrl = au8522_s_ctrl,
-	.queryctrl = au8522_queryctrl,
 	.reset = au8522_reset,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.g_register = au8522_g_register,
@@ -753,12 +677,17 @@ static const struct v4l2_subdev_ops au8522_ops = {
 	.video = &au8522_video_ops,
 };
 
+static const struct v4l2_ctrl_ops au8522_ctrl_ops = {
+	.s_ctrl = au8522_s_ctrl,
+};
+
 /* ----------------------------------------------------------------------- */
 
 static int au8522_probe(struct i2c_client *client,
 			const struct i2c_device_id *did)
 {
 	struct au8522_state *state;
+	struct v4l2_ctrl_handler *hdl;
 	struct v4l2_subdev *sd;
 	int instance;
 	struct au8522_config *demod_config;
@@ -799,6 +728,27 @@ static int au8522_probe(struct i2c_client *client,
 	sd = &state->sd;
 	v4l2_i2c_subdev_init(sd, client, &au8522_ops);
 
+	hdl = &state->hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &au8522_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 109);
+	v4l2_ctrl_new_std(hdl, &au8522_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1,
+			AU8522_TVDEC_CONTRAST_REG00BH_CVBS);
+	v4l2_ctrl_new_std(hdl, &au8522_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &au8522_ctrl_ops,
+			V4L2_CID_HUE, -32768, 32767, 1, 0);
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		int err = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		kfree(demod_config);
+		kfree(state);
+		return err;
+	}
+
 	state->c = client;
 	state->vid_input = AU8522_COMPOSITE_CH1;
 	state->aud_input = AU8522_AUDIO_NONE;
@@ -815,6 +765,7 @@ static int au8522_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
 	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
 	au8522_release_state(to_state(sd));
 	return 0;
 }
diff --git a/drivers/media/dvb-frontends/au8522_priv.h b/drivers/media/dvb-frontends/au8522_priv.h
index 0529699a27bd..aa0f16d6b610 100644
--- a/drivers/media/dvb-frontends/au8522_priv.h
+++ b/drivers/media/dvb-frontends/au8522_priv.h
@@ -29,6 +29,7 @@
 #include <linux/delay.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 #include "au8522.h"
@@ -65,10 +66,7 @@ struct au8522_state {
 	int aud_input;
 	u32 id;
 	u32 rev;
-	u8 brightness;
-	u8 contrast;
-	u8 saturation;
-	s16 hue;
+	struct v4l2_ctrl_handler hdl;
 };
 
 /* These are routines shared by both the VSB/QAM demodulator and the analog
diff --git a/drivers/media/dvb-frontends/cx22702.h b/drivers/media/dvb-frontends/cx22702.h
index f154e1f428eb..0b1a6c2f9d5f 100644
--- a/drivers/media/dvb-frontends/cx22702.h
+++ b/drivers/media/dvb-frontends/cx22702.h
@@ -28,6 +28,7 @@
 #ifndef CX22702_H
 #define CX22702_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct cx22702_config {
@@ -40,8 +41,7 @@ struct cx22702_config {
 	u8 output_mode;
 };
 
-#if defined(CONFIG_DVB_CX22702) || (defined(CONFIG_DVB_CX22702_MODULE) \
-	&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_CX22702)
 extern struct dvb_frontend *cx22702_attach(
 	const struct cx22702_config *config,
 	struct i2c_adapter *i2c);
diff --git a/drivers/media/dvb-frontends/cx24113.h b/drivers/media/dvb-frontends/cx24113.h
index 01eb7b9c28f4..782711ba1a32 100644
--- a/drivers/media/dvb-frontends/cx24113.h
+++ b/drivers/media/dvb-frontends/cx24113.h
@@ -22,6 +22,8 @@
 #ifndef CX24113_H
 #define CX24113_H
 
+#include <linux/kconfig.h>
+
 struct dvb_frontend;
 
 struct cx24113_config {
@@ -30,8 +32,7 @@ struct cx24113_config {
 	u32 xtal_khz;
 };
 
-#if defined(CONFIG_DVB_TUNER_CX24113) || \
-	(defined(CONFIG_DVB_TUNER_CX24113_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_TUNER_CX24113)
 extern struct dvb_frontend *cx24113_attach(struct dvb_frontend *,
 	const struct cx24113_config *config, struct i2c_adapter *i2c);
 
diff --git a/drivers/media/dvb-frontends/cx24116.h b/drivers/media/dvb-frontends/cx24116.h
index 7d90ab949c03..2ec84fae3f9f 100644
--- a/drivers/media/dvb-frontends/cx24116.h
+++ b/drivers/media/dvb-frontends/cx24116.h
@@ -21,6 +21,7 @@
 #ifndef CX24116_H
 #define CX24116_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct cx24116_config {
@@ -40,8 +41,7 @@ struct cx24116_config {
 	u16 i2c_wr_max;
 };
 
-#if defined(CONFIG_DVB_CX24116) || \
-	(defined(CONFIG_DVB_CX24116_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_CX24116)
 extern struct dvb_frontend *cx24116_attach(
 	const struct cx24116_config *config,
 	struct i2c_adapter *i2c);
diff --git a/drivers/media/dvb-frontends/cx24123.c b/drivers/media/dvb-frontends/cx24123.c
index 68c88ab58e71..a771da3e9f99 100644
--- a/drivers/media/dvb-frontends/cx24123.c
+++ b/drivers/media/dvb-frontends/cx24123.c
@@ -26,6 +26,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/init.h>
+#include <asm/div64.h>
 
 #include "dvb_frontend.h"
 #include "cx24123.h"
@@ -452,7 +453,8 @@ static u32 cx24123_int_log2(u32 a, u32 b)
 
 static int cx24123_set_symbolrate(struct cx24123_state *state, u32 srate)
 {
-	u32 tmp, sample_rate, ratio, sample_gain;
+	u64 tmp;
+	u32 sample_rate, ratio, sample_gain;
 	u8 pll_mult;
 
 	/*  check if symbol rate is within limits */
@@ -482,27 +484,11 @@ static int cx24123_set_symbolrate(struct cx24123_state *state, u32 srate)
 
 	sample_rate = pll_mult * XTAL;
 
-	/*
-	    SYSSymbolRate[21:0] = (srate << 23) / sample_rate
-
-	    We have to use 32 bit unsigned arithmetic without precision loss.
-	    The maximum srate is 45000000 or 0x02AEA540. This number has
-	    only 6 clear bits on top, hence we can shift it left only 6 bits
-	    at a time. Borrowed from cx24110.c
-	*/
-
-	tmp = srate << 6;
-	ratio = tmp / sample_rate;
-
-	tmp = (tmp % sample_rate) << 6;
-	ratio = (ratio << 6) + (tmp / sample_rate);
-
-	tmp = (tmp % sample_rate) << 6;
-	ratio = (ratio << 6) + (tmp / sample_rate);
-
-	tmp = (tmp % sample_rate) << 5;
-	ratio = (ratio << 5) + (tmp / sample_rate);
+	/* SYSSymbolRate[21:0] = (srate << 23) / sample_rate */
 
+	tmp = ((u64)srate) << 23;
+	do_div(tmp, sample_rate);
+	ratio = (u32) tmp;
 
 	cx24123_writereg(state, 0x01, pll_mult * 6);
 
diff --git a/drivers/media/dvb-frontends/cx24123.h b/drivers/media/dvb-frontends/cx24123.h
index 51ae866e9fed..102e70d17c43 100644
--- a/drivers/media/dvb-frontends/cx24123.h
+++ b/drivers/media/dvb-frontends/cx24123.h
@@ -21,6 +21,7 @@
 #ifndef CX24123_H
 #define CX24123_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct cx24123_config {
@@ -38,8 +39,7 @@ struct cx24123_config {
 	void (*agc_callback) (struct dvb_frontend *);
 };
 
-#if defined(CONFIG_DVB_CX24123) || (defined(CONFIG_DVB_CX24123_MODULE) \
-	&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_CX24123)
 extern struct dvb_frontend *cx24123_attach(const struct cx24123_config *config,
 					   struct i2c_adapter *i2c);
 extern struct i2c_adapter *cx24123_get_tuner_i2c_adapter(struct dvb_frontend *);
diff --git a/drivers/media/dvb-frontends/cxd2820r.h b/drivers/media/dvb-frontends/cxd2820r.h
index 6acc21c581c5..82b3d93718f8 100644
--- a/drivers/media/dvb-frontends/cxd2820r.h
+++ b/drivers/media/dvb-frontends/cxd2820r.h
@@ -22,6 +22,7 @@
 #ifndef CXD2820R_H
 #define CXD2820R_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 #define CXD2820R_GPIO_D (0 << 0) /* disable */
@@ -65,8 +66,7 @@ struct cxd2820r_config {
 };
 
 
-#if defined(CONFIG_DVB_CXD2820R) || \
-	(defined(CONFIG_DVB_CXD2820R_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_CXD2820R)
 extern struct dvb_frontend *cxd2820r_attach(
 	const struct cxd2820r_config *config,
 	struct i2c_adapter *i2c,
diff --git a/drivers/media/dvb-frontends/cxd2820r_core.c b/drivers/media/dvb-frontends/cxd2820r_core.c
index 9b658c1cf39a..7ca5c69dd200 100644
--- a/drivers/media/dvb-frontends/cxd2820r_core.c
+++ b/drivers/media/dvb-frontends/cxd2820r_core.c
@@ -660,7 +660,8 @@ static const struct dvb_frontend_ops cxd2820r_ops = {
 			FE_CAN_GUARD_INTERVAL_AUTO	|
 			FE_CAN_HIERARCHY_AUTO		|
 			FE_CAN_MUTE_TS			|
-			FE_CAN_2G_MODULATION
+			FE_CAN_2G_MODULATION		|
+			FE_CAN_MULTISTREAM
 		},
 
 	.release		= cxd2820r_release,
diff --git a/drivers/media/dvb-frontends/cxd2820r_t2.c b/drivers/media/dvb-frontends/cxd2820r_t2.c
index e82d82a7a2eb..2ba130e245b6 100644
--- a/drivers/media/dvb-frontends/cxd2820r_t2.c
+++ b/drivers/media/dvb-frontends/cxd2820r_t2.c
@@ -124,6 +124,23 @@ int cxd2820r_set_frontend_t2(struct dvb_frontend *fe)
 	buf[1] = ((if_ctl >>  8) & 0xff);
 	buf[2] = ((if_ctl >>  0) & 0xff);
 
+	/* PLP filtering */
+	if (c->stream_id > 255) {
+		dev_dbg(&priv->i2c->dev, "%s: Disable PLP filtering\n", __func__);
+		ret = cxd2820r_wr_reg(priv, 0x023ad , 0);
+		if (ret)
+			goto error;
+	} else {
+		dev_dbg(&priv->i2c->dev, "%s: Enable PLP filtering = %d\n", __func__,
+				c->stream_id);
+		ret = cxd2820r_wr_reg(priv, 0x023af , c->stream_id & 0xFF);
+		if (ret)
+			goto error;
+		ret = cxd2820r_wr_reg(priv, 0x023ad , 1);
+		if (ret)
+			goto error;
+	}
+
 	ret = cxd2820r_wr_regs(priv, 0x020b6, buf, 3);
 	if (ret)
 		goto error;
diff --git a/drivers/media/dvb-frontends/dib0090.c b/drivers/media/dvb-frontends/dib0090.c
index d9fe60b4be48..3ee22ff76315 100644
--- a/drivers/media/dvb-frontends/dib0090.c
+++ b/drivers/media/dvb-frontends/dib0090.c
@@ -528,20 +528,19 @@ static void dib0090_reset_digital(struct dvb_frontend *fe, const struct dib0090_
 	u16 PllCfg, i, v;
 
 	HARD_RESET(state);
-
 	dib0090_write_reg(state, 0x24, EN_PLL | EN_CRYSTAL);
-	dib0090_write_reg(state, 0x1b, EN_DIGCLK | EN_PLL | EN_CRYSTAL);	/* PLL, DIG_CLK and CRYSTAL remain */
+	if (cfg->in_soc)
+		return;
 
-	if (!cfg->in_soc) {
-		/* adcClkOutRatio=8->7, release reset */
-		dib0090_write_reg(state, 0x20, ((cfg->io.adc_clock_ratio - 1) << 11) | (0 << 10) | (1 << 9) | (1 << 8) | (0 << 4) | 0);
-		if (cfg->clkoutdrive != 0)
-			dib0090_write_reg(state, 0x23, (0 << 15) | ((!cfg->analog_output) << 14) | (2 << 10) | (1 << 9) | (0 << 8)
-					  | (cfg->clkoutdrive << 5) | (cfg->clkouttobamse << 4) | (0 << 2) | (0));
-		else
-			dib0090_write_reg(state, 0x23, (0 << 15) | ((!cfg->analog_output) << 14) | (2 << 10) | (1 << 9) | (0 << 8)
-					  | (7 << 5) | (cfg->clkouttobamse << 4) | (0 << 2) | (0));
-	}
+	dib0090_write_reg(state, 0x1b, EN_DIGCLK | EN_PLL | EN_CRYSTAL);	/* PLL, DIG_CLK and CRYSTAL remain */
+	/* adcClkOutRatio=8->7, release reset */
+	dib0090_write_reg(state, 0x20, ((cfg->io.adc_clock_ratio - 1) << 11) | (0 << 10) | (1 << 9) | (1 << 8) | (0 << 4) | 0);
+	if (cfg->clkoutdrive != 0)
+		dib0090_write_reg(state, 0x23, (0 << 15) | ((!cfg->analog_output) << 14) | (2 << 10) | (1 << 9) | (0 << 8)
+				| (cfg->clkoutdrive << 5) | (cfg->clkouttobamse << 4) | (0 << 2) | (0));
+	else
+		dib0090_write_reg(state, 0x23, (0 << 15) | ((!cfg->analog_output) << 14) | (2 << 10) | (1 << 9) | (0 << 8)
+				| (7 << 5) | (cfg->clkouttobamse << 4) | (0 << 2) | (0));
 
 	/* Read Pll current config * */
 	PllCfg = dib0090_read_reg(state, 0x21);
@@ -694,192 +693,174 @@ void dib0090_dcc_freq(struct dvb_frontend *fe, u8 fast)
 EXPORT_SYMBOL(dib0090_dcc_freq);
 
 static const u16 bb_ramp_pwm_normal_socs[] = {
-	550,			/* max BB gain in 10th of dB */
-	(1 << 9) | 8,		/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> BB_RAMP2 */
+	550, /* max BB gain in 10th of dB */
+	(1<<9) | 8, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> BB_RAMP2 */
 	440,
-	(4 << 9) | 0,		/* BB_RAMP3 = 26dB */
-	(0 << 9) | 208,		/* BB_RAMP4 */
-	(4 << 9) | 208,		/* BB_RAMP5 = 29dB */
-	(0 << 9) | 440,		/* BB_RAMP6 */
+	(4  << 9) | 0, /* BB_RAMP3 = 26dB */
+	(0  << 9) | 208, /* BB_RAMP4 */
+	(4  << 9) | 208, /* BB_RAMP5 = 29dB */
+	(0  << 9) | 440, /* BB_RAMP6 */
 };
 
-static const u16 rf_ramp_pwm_cband_7090[] = {
-	280,			/* max RF gain in 10th of dB */
-	18,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
-	504,			/* ramp_max = maximum X used on the ramp */
-	(29 << 10) | 364,	/* RF_RAMP5, LNA 1 = 8dB */
-	(0 << 10) | 504,	/* RF_RAMP6, LNA 1 */
-	(60 << 10) | 228,	/* RF_RAMP7, LNA 2 = 7.7dB */
-	(0 << 10) | 364,	/* RF_RAMP8, LNA 2 */
-	(34 << 10) | 109,	/* GAIN_4_1, LNA 3 = 6.8dB */
-	(0 << 10) | 228,	/* GAIN_4_2, LNA 3 */
-	(37 << 10) | 0,		/* RF_RAMP3, LNA 4 = 6.2dB */
-	(0 << 10) | 109,	/* RF_RAMP4, LNA 4 */
+static const u16 rf_ramp_pwm_cband_7090p[] = {
+	280, /* max RF gain in 10th of dB */
+	18, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	504, /* ramp_max = maximum X used on the ramp */
+	(29 << 10) | 364, /* RF_RAMP5, LNA 1 = 8dB */
+	(0  << 10) | 504, /* RF_RAMP6, LNA 1 */
+	(60 << 10) | 228, /* RF_RAMP7, LNA 2 = 7.7dB */
+	(0  << 10) | 364, /* RF_RAMP8, LNA 2 */
+	(34 << 10) | 109, /* GAIN_4_1, LNA 3 = 6.8dB */
+	(0  << 10) | 228, /* GAIN_4_2, LNA 3 */
+	(37 << 10) | 0, /* RF_RAMP3, LNA 4 = 6.2dB */
+	(0  << 10) | 109, /* RF_RAMP4, LNA 4 */
 };
 
-static const uint16_t rf_ramp_pwm_cband_7090e_sensitivity[] = {
-	186,
-	40,
-	746,
-	(10 << 10) | 345,
-	(0  << 10) | 746,
-	(0 << 10) | 0,
-	(0  << 10) | 0,
-	(28 << 10) | 200,
-	(0  << 10) | 345,
-	(20 << 10) | 0,
-	(0  << 10) | 200,
+static const u16 rf_ramp_pwm_cband_7090e_sensitivity[] = {
+	186, /* max RF gain in 10th of dB */
+	40, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	746, /* ramp_max = maximum X used on the ramp */
+	(10 << 10) | 345, /* RF_RAMP5, LNA 1 = 10dB */
+	(0  << 10) | 746, /* RF_RAMP6, LNA 1 */
+	(0 << 10) | 0, /* RF_RAMP7, LNA 2 = 0 dB */
+	(0  << 10) | 0, /* RF_RAMP8, LNA 2 */
+	(28 << 10) | 200, /* GAIN_4_1, LNA 3 = 6.8dB */ /* 3.61 dB */
+	(0  << 10) | 345, /* GAIN_4_2, LNA 3 */
+	(20 << 10) | 0, /* RF_RAMP3, LNA 4 = 6.2dB */ /* 4.96 dB */
+	(0  << 10) | 200, /* RF_RAMP4, LNA 4 */
 };
 
-static const uint16_t rf_ramp_pwm_cband_7090e_aci[] = {
-	86,
-	40,
-	345,
-	(0 << 10) | 0,
-	(0 << 10) | 0,
-	(0 << 10) | 0,
-	(0 << 10) | 0,
-	(28 << 10) | 200,
-	(0  << 10) | 345,
-	(20 << 10) | 0,
-	(0  << 10) | 200,
+static const u16 rf_ramp_pwm_cband_7090e_aci[] = {
+	86, /* max RF gain in 10th of dB */
+	40, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	345, /* ramp_max = maximum X used on the ramp */
+	(0 << 10) | 0, /* RF_RAMP5, LNA 1 = 8dB */ /* 7.47 dB */
+	(0 << 10) | 0, /* RF_RAMP6, LNA 1 */
+	(0 << 10) | 0, /* RF_RAMP7, LNA 2 = 0 dB */
+	(0 << 10) | 0, /* RF_RAMP8, LNA 2 */
+	(28 << 10) | 200, /* GAIN_4_1, LNA 3 = 6.8dB */ /* 3.61 dB */
+	(0  << 10) | 345, /* GAIN_4_2, LNA 3 */
+	(20 << 10) | 0, /* RF_RAMP3, LNA 4 = 6.2dB */ /* 4.96 dB */
+	(0  << 10) | 200, /* RF_RAMP4, LNA 4 */
 };
 
 static const u16 rf_ramp_pwm_cband_8090[] = {
-	345,			/* max RF gain in 10th of dB */
-	29,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
-	1000,			/* ramp_max = maximum X used on the ramp */
-	(35 << 10) | 772,	/* RF_RAMP3, LNA 1 = 8dB */
-	(0 << 10) | 1000,	/* RF_RAMP4, LNA 1 */
-	(58 << 10) | 496,	/* RF_RAMP5, LNA 2 = 9.5dB */
-	(0 << 10) | 772,	/* RF_RAMP6, LNA 2 */
-	(27 << 10) | 200,	/* RF_RAMP7, LNA 3 = 10.5dB */
-	(0 << 10) | 496,	/* RF_RAMP8, LNA 3 */
-	(40 << 10) | 0,		/* GAIN_4_1, LNA 4 = 7dB */
-	(0 << 10) | 200,	/* GAIN_4_2, LNA 4 */
+	345, /* max RF gain in 10th of dB */
+	29, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	1000, /* ramp_max = maximum X used on the ramp */
+	(35 << 10) | 772, /* RF_RAMP3, LNA 1 = 8dB */
+	(0  << 10) | 1000, /* RF_RAMP4, LNA 1 */
+	(58 << 10) | 496, /* RF_RAMP5, LNA 2 = 9.5dB */
+	(0  << 10) | 772, /* RF_RAMP6, LNA 2 */
+	(27 << 10) | 200, /* RF_RAMP7, LNA 3 = 10.5dB */
+	(0  << 10) | 496, /* RF_RAMP8, LNA 3 */
+	(40 << 10) | 0, /* GAIN_4_1, LNA 4 = 7dB */
+	(0  << 10) | 200, /* GAIN_4_2, LNA 4 */
 };
 
 static const u16 rf_ramp_pwm_uhf_7090[] = {
-	407,			/* max RF gain in 10th of dB */
-	13,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
-	529,			/* ramp_max = maximum X used on the ramp */
-	(23 << 10) | 0,		/* RF_RAMP3, LNA 1 = 14.7dB */
-	(0 << 10) | 176,	/* RF_RAMP4, LNA 1 */
-	(63 << 10) | 400,	/* RF_RAMP5, LNA 2 = 8dB */
-	(0 << 10) | 529,	/* RF_RAMP6, LNA 2 */
-	(48 << 10) | 316,	/* RF_RAMP7, LNA 3 = 6.8dB */
-	(0 << 10) | 400,	/* RF_RAMP8, LNA 3 */
-	(29 << 10) | 176,	/* GAIN_4_1, LNA 4 = 11.5dB */
-	(0 << 10) | 316,	/* GAIN_4_2, LNA 4 */
+	407, /* max RF gain in 10th of dB */
+	13, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	529, /* ramp_max = maximum X used on the ramp */
+	(23 << 10) | 0, /* RF_RAMP3, LNA 1 = 14.7dB */
+	(0  << 10) | 176, /* RF_RAMP4, LNA 1 */
+	(63 << 10) | 400, /* RF_RAMP5, LNA 2 = 8dB */
+	(0  << 10) | 529, /* RF_RAMP6, LNA 2 */
+	(48 << 10) | 316, /* RF_RAMP7, LNA 3 = 6.8dB */
+	(0  << 10) | 400, /* RF_RAMP8, LNA 3 */
+	(29 << 10) | 176, /* GAIN_4_1, LNA 4 = 11.5dB */
+	(0  << 10) | 316, /* GAIN_4_2, LNA 4 */
 };
 
 static const u16 rf_ramp_pwm_uhf_8090[] = {
-	388,			/* max RF gain in 10th of dB */
-	26,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
-	1008,			/* ramp_max = maximum X used on the ramp */
-	(11 << 10) | 0,		/* RF_RAMP3, LNA 1 = 14.7dB */
-	(0 << 10) | 369,	/* RF_RAMP4, LNA 1 */
-	(41 << 10) | 809,	/* RF_RAMP5, LNA 2 = 8dB */
-	(0 << 10) | 1008,	/* RF_RAMP6, LNA 2 */
-	(27 << 10) | 659,	/* RF_RAMP7, LNA 3 = 6dB */
-	(0 << 10) | 809,	/* RF_RAMP8, LNA 3 */
-	(14 << 10) | 369,	/* GAIN_4_1, LNA 4 = 11.5dB */
-	(0 << 10) | 659,	/* GAIN_4_2, LNA 4 */
+	388, /* max RF gain in 10th of dB */
+	26, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	1008, /* ramp_max = maximum X used on the ramp */
+	(11 << 10) | 0, /* RF_RAMP3, LNA 1 = 14.7dB */
+	(0  << 10) | 369, /* RF_RAMP4, LNA 1 */
+	(41 << 10) | 809, /* RF_RAMP5, LNA 2 = 8dB */
+	(0  << 10) | 1008, /* RF_RAMP6, LNA 2 */
+	(27 << 10) | 659, /* RF_RAMP7, LNA 3 = 6dB */
+	(0  << 10) | 809, /* RF_RAMP8, LNA 3 */
+	(14 << 10) | 369, /* GAIN_4_1, LNA 4 = 11.5dB */
+	(0  << 10) | 659, /* GAIN_4_2, LNA 4 */
 };
 
-static const u16 rf_ramp_pwm_cband[] = {
-	0,			/* max RF gain in 10th of dB */
-	0,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> 0x2b */
-	0,			/* ramp_max = maximum X used on the ramp */
-	(0 << 10) | 0,		/* 0x2c, LNA 1 = 0dB */
-	(0 << 10) | 0,		/* 0x2d, LNA 1 */
-	(0 << 10) | 0,		/* 0x2e, LNA 2 = 0dB */
-	(0 << 10) | 0,		/* 0x2f, LNA 2 */
-	(0 << 10) | 0,		/* 0x30, LNA 3 = 0dB */
-	(0 << 10) | 0,		/* 0x31, LNA 3 */
-	(0 << 10) | 0,		/* GAIN_4_1, LNA 4 = 0dB */
-	(0 << 10) | 0,		/* GAIN_4_2, LNA 4 */
-};
-
-static const u16 rf_ramp_vhf[] = {
-	412,			/* max RF gain in 10th of dB */
-	132, 307, 127,		/* LNA1,  13.2dB */
-	105, 412, 255,		/* LNA2,  10.5dB */
-	50, 50, 127,		/* LNA3,  5dB */
-	125, 175, 127,		/* LNA4,  12.5dB */
-	0, 0, 127,		/* CBAND, 0dB */
-};
-
-static const u16 rf_ramp_uhf[] = {
-	412,			/* max RF gain in 10th of dB */
-	132, 307, 127,		/* LNA1  : total gain = 13.2dB, point on the ramp where this amp is full gain, value to write to get full gain */
-	105, 412, 255,		/* LNA2  : 10.5 dB */
-	50, 50, 127,		/* LNA3  :  5.0 dB */
-	125, 175, 127,		/* LNA4  : 12.5 dB */
-	0, 0, 127,		/* CBAND :  0.0 dB */
+/* GENERAL PWM ramp definition for all other Krosus */
+static const u16 bb_ramp_pwm_normal[] = {
+	500, /* max BB gain in 10th of dB */
+	8, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> BB_RAMP2 */
+	400,
+	(2  << 9) | 0, /* BB_RAMP3 = 21dB */
+	(0  << 9) | 168, /* BB_RAMP4 */
+	(2  << 9) | 168, /* BB_RAMP5 = 29dB */
+	(0  << 9) | 400, /* BB_RAMP6 */
 };
 
-static const u16 rf_ramp_cband_broadmatching[] =	/* for p1G only */
-{
-	314,			/* Calibrated at 200MHz order has been changed g4-g3-g2-g1 */
-	84, 314, 127,		/* LNA1 */
-	80, 230, 255,		/* LNA2 */
-	80, 150, 127,		/* LNA3  It was measured 12dB, do not lock if 120 */
-	70, 70, 127,		/* LNA4 */
-	0, 0, 127,		/* CBAND */
+static const u16 bb_ramp_pwm_boost[] = {
+	550, /* max BB gain in 10th of dB */
+	8, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> BB_RAMP2 */
+	440,
+	(2  << 9) | 0, /* BB_RAMP3 = 26dB */
+	(0  << 9) | 208, /* BB_RAMP4 */
+	(2  << 9) | 208, /* BB_RAMP5 = 29dB */
+	(0  << 9) | 440, /* BB_RAMP6 */
 };
 
-static const u16 rf_ramp_cband[] = {
-	332,			/* max RF gain in 10th of dB */
-	132, 252, 127,		/* LNA1,  dB */
-	80, 332, 255,		/* LNA2,  dB */
-	0, 0, 127,		/* LNA3,  dB */
-	0, 0, 127,		/* LNA4,  dB */
-	120, 120, 127,		/* LT1 CBAND */
+static const u16 rf_ramp_pwm_cband[] = {
+	314, /* max RF gain in 10th of dB */
+	33, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	1023, /* ramp_max = maximum X used on the ramp */
+	(8  << 10) | 743, /* RF_RAMP3, LNA 1 = 0dB */
+	(0  << 10) | 1023, /* RF_RAMP4, LNA 1 */
+	(15 << 10) | 469, /* RF_RAMP5, LNA 2 = 0dB */
+	(0  << 10) | 742, /* RF_RAMP6, LNA 2 */
+	(9  << 10) | 234, /* RF_RAMP7, LNA 3 = 0dB */
+	(0  << 10) | 468, /* RF_RAMP8, LNA 3 */
+	(9  << 10) | 0, /* GAIN_4_1, LNA 4 = 0dB */
+	(0  << 10) | 233, /* GAIN_4_2, LNA 4 */
 };
 
 static const u16 rf_ramp_pwm_vhf[] = {
-	404,			/* max RF gain in 10th of dB */
-	25,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> 0x2b */
-	1011,			/* ramp_max = maximum X used on the ramp */
-	(6 << 10) | 417,	/* 0x2c, LNA 1 = 13.2dB */
-	(0 << 10) | 756,	/* 0x2d, LNA 1 */
-	(16 << 10) | 756,	/* 0x2e, LNA 2 = 10.5dB */
-	(0 << 10) | 1011,	/* 0x2f, LNA 2 */
-	(16 << 10) | 290,	/* 0x30, LNA 3 = 5dB */
-	(0 << 10) | 417,	/* 0x31, LNA 3 */
-	(7 << 10) | 0,		/* GAIN_4_1, LNA 4 = 12.5dB */
-	(0 << 10) | 290,	/* GAIN_4_2, LNA 4 */
+	398, /* max RF gain in 10th of dB */
+	24, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	954, /* ramp_max = maximum X used on the ramp */
+	(7  << 10) | 0, /* RF_RAMP3, LNA 1 = 13.2dB */
+	(0  << 10) | 290, /* RF_RAMP4, LNA 1 */
+	(16 << 10) | 699, /* RF_RAMP5, LNA 2 = 10.5dB */
+	(0  << 10) | 954, /* RF_RAMP6, LNA 2 */
+	(17 << 10) | 580, /* RF_RAMP7, LNA 3 = 5dB */
+	(0  << 10) | 699, /* RF_RAMP8, LNA 3 */
+	(7  << 10) | 290, /* GAIN_4_1, LNA 4 = 12.5dB */
+	(0  << 10) | 580, /* GAIN_4_2, LNA 4 */
 };
 
 static const u16 rf_ramp_pwm_uhf[] = {
-	404,			/* max RF gain in 10th of dB */
-	25,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> 0x2b */
-	1011,			/* ramp_max = maximum X used on the ramp */
-	(6 << 10) | 417,	/* 0x2c, LNA 1 = 13.2dB */
-	(0 << 10) | 756,	/* 0x2d, LNA 1 */
-	(16 << 10) | 756,	/* 0x2e, LNA 2 = 10.5dB */
-	(0 << 10) | 1011,	/* 0x2f, LNA 2 */
-	(16 << 10) | 0,		/* 0x30, LNA 3 = 5dB */
-	(0 << 10) | 127,	/* 0x31, LNA 3 */
-	(7 << 10) | 127,	/* GAIN_4_1, LNA 4 = 12.5dB */
-	(0 << 10) | 417,	/* GAIN_4_2, LNA 4 */
+	398, /* max RF gain in 10th of dB */
+	24, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	954, /* ramp_max = maximum X used on the ramp */
+	(7  << 10) | 0, /* RF_RAMP3, LNA 1 = 13.2dB */
+	(0  << 10) | 290, /* RF_RAMP4, LNA 1 */
+	(16 << 10) | 699, /* RF_RAMP5, LNA 2 = 10.5dB */
+	(0  << 10) | 954, /* RF_RAMP6, LNA 2 */
+	(17 << 10) | 580, /* RF_RAMP7, LNA 3 = 5dB */
+	(0  << 10) | 699, /* RF_RAMP8, LNA 3 */
+	(7  << 10) | 290, /* GAIN_4_1, LNA 4 = 12.5dB */
+	(0  << 10) | 580, /* GAIN_4_2, LNA 4 */
 };
 
-static const u16 bb_ramp_boost[] = {
-	550,			/* max BB gain in 10th of dB */
-	260, 260, 26,		/* BB1, 26dB */
-	290, 550, 29,		/* BB2, 29dB */
-};
-
-static const u16 bb_ramp_pwm_normal[] = {
-	500,			/* max RF gain in 10th of dB */
-	8,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> 0x34 */
-	400,
-	(2 << 9) | 0,		/* 0x35 = 21dB */
-	(0 << 9) | 168,		/* 0x36 */
-	(2 << 9) | 168,		/* 0x37 = 29dB */
-	(0 << 9) | 400,		/* 0x38 */
+static const u16 rf_ramp_pwm_sband[] = {
+	253, /* max RF gain in 10th of dB */
+	38, /* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	961,
+	(4  << 10) | 0, /* RF_RAMP3, LNA 1 = 14.1dB */
+	(0  << 10) | 508, /* RF_RAMP4, LNA 1 */
+	(9  << 10) | 508, /* RF_RAMP5, LNA 2 = 11.2dB */
+	(0  << 10) | 961, /* RF_RAMP6, LNA 2 */
+	(0  << 10) | 0, /* RF_RAMP7, LNA 3 = 0dB */
+	(0  << 10) | 0, /* RF_RAMP8, LNA 3 */
+	(0  << 10) | 0, /* GAIN_4_1, LNA 4 = 0dB */
+	(0  << 10) | 0, /* GAIN_4_2, LNA 4 */
 };
 
 struct slope {
@@ -1089,70 +1070,69 @@ static void dib0090_set_bbramp_pwm(struct dib0090_state *state, const u16 * cfg)
 void dib0090_pwm_gain_reset(struct dvb_frontend *fe)
 {
 	struct dib0090_state *state = fe->tuner_priv;
-	/* reset the AGC */
+	u16 *bb_ramp = (u16 *)&bb_ramp_pwm_normal; /* default baseband config */
+	u16 *rf_ramp = NULL;
+	u8 en_pwm_rf_mux = 1;
 
+	/* reset the AGC */
 	if (state->config->use_pwm_agc) {
-#ifdef CONFIG_BAND_SBAND
-		if (state->current_band == BAND_SBAND) {
-			dib0090_set_rframp_pwm(state, rf_ramp_pwm_sband);
-			dib0090_set_bbramp_pwm(state, bb_ramp_pwm_boost);
-		} else
-#endif
-#ifdef CONFIG_BAND_CBAND
 		if (state->current_band == BAND_CBAND) {
 			if (state->identity.in_soc) {
-				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal_socs);
+				bb_ramp = (u16 *)&bb_ramp_pwm_normal_socs;
 				if (state->identity.version == SOC_8090_P1G_11R1 || state->identity.version == SOC_8090_P1G_21R1)
-					dib0090_set_rframp_pwm(state, rf_ramp_pwm_cband_8090);
-				else if (state->identity.version == SOC_7090_P1G_11R1
-						|| state->identity.version == SOC_7090_P1G_21R1) {
+					rf_ramp = (u16 *)&rf_ramp_pwm_cband_8090;
+				else if (state->identity.version == SOC_7090_P1G_11R1 || state->identity.version == SOC_7090_P1G_21R1) {
 					if (state->config->is_dib7090e) {
 						if (state->rf_ramp == NULL)
-							dib0090_set_rframp_pwm(state, rf_ramp_pwm_cband_7090e_sensitivity);
+							rf_ramp = (u16 *)&rf_ramp_pwm_cband_7090e_sensitivity;
 						else
-							dib0090_set_rframp_pwm(state, state->rf_ramp);
+							rf_ramp = (u16 *)state->rf_ramp;
 					} else
-						dib0090_set_rframp_pwm(state, rf_ramp_pwm_cband_7090);
+						rf_ramp = (u16 *)&rf_ramp_pwm_cband_7090p;
 				}
-			} else {
-				dib0090_set_rframp_pwm(state, rf_ramp_pwm_cband);
-				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal);
-			}
+			} else
+				rf_ramp = (u16 *)&rf_ramp_pwm_cband;
 		} else
-#endif
-#ifdef CONFIG_BAND_VHF
-		if (state->current_band == BAND_VHF) {
-			if (state->identity.in_soc) {
-				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal_socs);
-			} else {
-				dib0090_set_rframp_pwm(state, rf_ramp_pwm_vhf);
-				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal);
+
+			if (state->current_band == BAND_VHF) {
+				if (state->identity.in_soc) {
+					bb_ramp = (u16 *)&bb_ramp_pwm_normal_socs;
+					/* rf_ramp = &rf_ramp_pwm_vhf_socs; */ /* TODO */
+				} else
+					rf_ramp = (u16 *)&rf_ramp_pwm_vhf;
+			} else if (state->current_band == BAND_UHF) {
+				if (state->identity.in_soc) {
+					bb_ramp = (u16 *)&bb_ramp_pwm_normal_socs;
+					if (state->identity.version == SOC_8090_P1G_11R1 || state->identity.version == SOC_8090_P1G_21R1)
+						rf_ramp = (u16 *)&rf_ramp_pwm_uhf_8090;
+					else if (state->identity.version == SOC_7090_P1G_11R1 || state->identity.version == SOC_7090_P1G_21R1)
+						rf_ramp = (u16 *)&rf_ramp_pwm_uhf_7090;
+				} else
+					rf_ramp = (u16 *)&rf_ramp_pwm_uhf;
 			}
+		if (rf_ramp)
+			dib0090_set_rframp_pwm(state, rf_ramp);
+		dib0090_set_bbramp_pwm(state, bb_ramp);
+
+		/* activate the ramp generator using PWM control */
+		dprintk("ramp RF gain = %d BAND = %s version = %d", state->rf_ramp[0], (state->current_band == BAND_CBAND) ? "CBAND" : "NOT CBAND", state->identity.version & 0x1f);
+
+		if ((state->rf_ramp[0] == 0) || (state->current_band == BAND_CBAND && (state->identity.version & 0x1f) <= P1D_E_F)) {
+			dprintk("DE-Engage mux for direct gain reg control");
+			en_pwm_rf_mux = 0;
 		} else
-#endif
-		{
-			if (state->identity.in_soc) {
-				if (state->identity.version == SOC_8090_P1G_11R1 || state->identity.version == SOC_8090_P1G_21R1)
-					dib0090_set_rframp_pwm(state, rf_ramp_pwm_uhf_8090);
-				else if (state->identity.version == SOC_7090_P1G_11R1 || state->identity.version == SOC_7090_P1G_21R1)
-					dib0090_set_rframp_pwm(state, rf_ramp_pwm_uhf_7090);
-				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal_socs);
-			} else {
-				dib0090_set_rframp_pwm(state, rf_ramp_pwm_uhf);
-				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal);
-			}
-		}
+			dprintk("Engage mux for PWM control");
 
-		if (state->rf_ramp[0] != 0)
-			dib0090_write_reg(state, 0x32, (3 << 11));
-		else
-			dib0090_write_reg(state, 0x32, (0 << 11));
+		dib0090_write_reg(state, 0x32, (en_pwm_rf_mux << 12) | (en_pwm_rf_mux << 11));
 
-		dib0090_write_reg(state, 0x04, 0x03);
-		dib0090_write_reg(state, 0x39, (1 << 10));
+		/* Set fast servo cutoff to start AGC; 0 = 1KHz ; 1 = 50Hz ; 2 = 150Hz ; 3 = 50KHz ; 4 = servo fast*/
+		if (state->identity.version == SOC_7090_P1G_11R1 || state->identity.version == SOC_7090_P1G_21R1)
+			dib0090_write_reg(state, 0x04, 3);
+		else
+			dib0090_write_reg(state, 0x04, 1);
+		dib0090_write_reg(state, 0x39, (1 << 10)); /* 0 gain by default */
 	}
 }
-
 EXPORT_SYMBOL(dib0090_pwm_gain_reset);
 
 void dib0090_set_dc_servo(struct dvb_frontend *fe, u8 DC_servo_cutoff)
@@ -1193,22 +1173,22 @@ int dib0090_gain_control(struct dvb_frontend *fe)
 #endif
 #ifdef CONFIG_BAND_VHF
 		if (state->current_band == BAND_VHF && !state->identity.p1g) {
-			dib0090_set_rframp(state, rf_ramp_vhf);
-			dib0090_set_bbramp(state, bb_ramp_boost);
+			dib0090_set_rframp(state, rf_ramp_pwm_vhf);
+			dib0090_set_bbramp(state, bb_ramp_pwm_normal);
 		} else
 #endif
 #ifdef CONFIG_BAND_CBAND
 		if (state->current_band == BAND_CBAND && !state->identity.p1g) {
-			dib0090_set_rframp(state, rf_ramp_cband);
-			dib0090_set_bbramp(state, bb_ramp_boost);
+			dib0090_set_rframp(state, rf_ramp_pwm_cband);
+			dib0090_set_bbramp(state, bb_ramp_pwm_normal);
 		} else
 #endif
 		if ((state->current_band == BAND_CBAND || state->current_band == BAND_VHF) && state->identity.p1g) {
-			dib0090_set_rframp(state, rf_ramp_cband_broadmatching);
-			dib0090_set_bbramp(state, bb_ramp_boost);
+			dib0090_set_rframp(state, rf_ramp_pwm_cband_7090p);
+			dib0090_set_bbramp(state, bb_ramp_pwm_normal_socs);
 		} else {
-			dib0090_set_rframp(state, rf_ramp_uhf);
-			dib0090_set_bbramp(state, bb_ramp_boost);
+			dib0090_set_rframp(state, rf_ramp_pwm_uhf);
+			dib0090_set_bbramp(state, bb_ramp_pwm_normal);
 		}
 
 		dib0090_write_reg(state, 0x32, 0);
@@ -1553,14 +1533,16 @@ static void dib0090_set_EFUSE(struct dib0090_state *state)
 
 		if ((c >= CAP_VALUE_MAX) || (c <= CAP_VALUE_MIN))
 			c = 32;
+		else
+			c += 14;
 		if ((h >= HR_MAX) || (h <= HR_MIN))
 			h = 34;
 		if ((n >= POLY_MAX) || (n <= POLY_MIN))
 			n = 3;
 
-		dib0090_write_reg(state, 0x13, (h << 10)) ;
-		e2 = (n<<11) | ((h>>2)<<6) | (c);
-		dib0090_write_reg(state, 0x2, e2) ; /* Load the BB_2 */
+		dib0090_write_reg(state, 0x13, (h << 10));
+		e2 = (n << 11) | ((h >> 2)<<6) | c;
+		dib0090_write_reg(state, 0x2, e2); /* Load the BB_2 */
 	}
 }
 
diff --git a/drivers/media/dvb-frontends/dib3000mc.h b/drivers/media/dvb-frontends/dib3000mc.h
index d75ffad2d752..129d1425516a 100644
--- a/drivers/media/dvb-frontends/dib3000mc.h
+++ b/drivers/media/dvb-frontends/dib3000mc.h
@@ -13,6 +13,8 @@
 #ifndef DIB3000MC_H
 #define DIB3000MC_H
 
+#include <linux/kconfig.h>
+
 #include "dibx000_common.h"
 
 struct dib3000mc_config {
@@ -39,8 +41,7 @@ struct dib3000mc_config {
 #define DEFAULT_DIB3000MC_I2C_ADDRESS 16
 #define DEFAULT_DIB3000P_I2C_ADDRESS  24
 
-#if defined(CONFIG_DVB_DIB3000MC) || (defined(CONFIG_DVB_DIB3000MC_MODULE) && \
-				      defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DIB3000MC)
 extern struct dvb_frontend *dib3000mc_attach(struct i2c_adapter *i2c_adap,
 					     u8 i2c_addr,
 					     struct dib3000mc_config *cfg);
diff --git a/drivers/media/dvb-frontends/dib7000m.h b/drivers/media/dvb-frontends/dib7000m.h
index 81fcf2241c64..b585413f9a29 100644
--- a/drivers/media/dvb-frontends/dib7000m.h
+++ b/drivers/media/dvb-frontends/dib7000m.h
@@ -1,6 +1,8 @@
 #ifndef DIB7000M_H
 #define DIB7000M_H
 
+#include <linux/kconfig.h>
+
 #include "dibx000_common.h"
 
 struct dib7000m_config {
@@ -38,8 +40,7 @@ struct dib7000m_config {
 
 #define DEFAULT_DIB7000M_I2C_ADDRESS 18
 
-#if defined(CONFIG_DVB_DIB7000M) || (defined(CONFIG_DVB_DIB7000M_MODULE) && \
-				     defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DIB7000M)
 extern struct dvb_frontend *dib7000m_attach(struct i2c_adapter *i2c_adap,
 					    u8 i2c_addr,
 					    struct dib7000m_config *cfg);
diff --git a/drivers/media/dvb-frontends/dib7000p.c b/drivers/media/dvb-frontends/dib7000p.c
index 3e1eefada0e8..effb87f773b0 100644
--- a/drivers/media/dvb-frontends/dib7000p.c
+++ b/drivers/media/dvb-frontends/dib7000p.c
@@ -429,6 +429,13 @@ int dib7000p_get_agc_values(struct dvb_frontend *fe,
 }
 EXPORT_SYMBOL(dib7000p_get_agc_values);
 
+int dib7000p_set_agc1_min(struct dvb_frontend *fe, u16 v)
+{
+	struct dib7000p_state *state = fe->demodulator_priv;
+	return dib7000p_write_word(state, 108,  v);
+}
+EXPORT_SYMBOL(dib7000p_set_agc1_min);
+
 static void dib7000p_reset_pll(struct dib7000p_state *state)
 {
 	struct dibx000_bandwidth_config *bw = &state->cfg.bw[0];
@@ -821,6 +828,7 @@ static int dib7000p_agc_startup(struct dvb_frontend *demod)
 	u8 agc_split;
 	u16 reg;
 	u32 upd_demod_gain_period = 0x1000;
+	s32 frequency_offset = 0;
 
 	switch (state->agc_state) {
 	case 0:
@@ -841,7 +849,14 @@ static int dib7000p_agc_startup(struct dvb_frontend *demod)
 		if (dib7000p_set_agc_config(state, BAND_OF_FREQUENCY(ch->frequency / 1000)) != 0)
 			return -1;
 
-		dib7000p_set_dds(state, 0);
+		if (demod->ops.tuner_ops.get_frequency) {
+			u32 frequency_tuner;
+
+			demod->ops.tuner_ops.get_frequency(demod, &frequency_tuner);
+			frequency_offset = (s32)frequency_tuner / 1000 - ch->frequency / 1000;
+		}
+
+		dib7000p_set_dds(state, frequency_offset);
 		ret = 7;
 		(*agc_state)++;
 		break;
diff --git a/drivers/media/dvb-frontends/dib7000p.h b/drivers/media/dvb-frontends/dib7000p.h
index b61b03a6e1ed..d08cdff59bdf 100644
--- a/drivers/media/dvb-frontends/dib7000p.h
+++ b/drivers/media/dvb-frontends/dib7000p.h
@@ -1,6 +1,8 @@
 #ifndef DIB7000P_H
 #define DIB7000P_H
 
+#include <linux/kconfig.h>
+
 #include "dibx000_common.h"
 
 struct dib7000p_config {
@@ -44,8 +46,7 @@ struct dib7000p_config {
 
 #define DEFAULT_DIB7000P_I2C_ADDRESS 18
 
-#if defined(CONFIG_DVB_DIB7000P) || (defined(CONFIG_DVB_DIB7000P_MODULE) && \
-					defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DIB7000P)
 extern struct dvb_frontend *dib7000p_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, struct dib7000p_config *cfg);
 extern struct i2c_adapter *dib7000p_get_i2c_master(struct dvb_frontend *, enum dibx000_i2c_interface, int);
 extern int dib7000p_i2c_enumeration(struct i2c_adapter *i2c, int no_of_demods, u8 default_addr, struct dib7000p_config cfg[]);
@@ -62,6 +63,7 @@ extern struct i2c_adapter *dib7090_get_i2c_tuner(struct dvb_frontend *fe);
 extern int dib7090_slave_reset(struct dvb_frontend *fe);
 extern int dib7000p_get_agc_values(struct dvb_frontend *fe,
 		u16 *agc_global, u16 *agc1, u16 *agc2, u16 *wbd);
+extern int dib7000p_set_agc1_min(struct dvb_frontend *fe, u16 v);
 #else
 static inline struct dvb_frontend *dib7000p_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, struct dib7000p_config *cfg)
 {
@@ -153,6 +155,12 @@ static inline int dib7000p_get_agc_values(struct dvb_frontend *fe,
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return -ENODEV;
 }
+
+static inline int dib7000p_set_agc1_min(struct dvb_frontend *fe, u16 v)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
 #endif
 
 #endif
diff --git a/drivers/media/dvb-frontends/dib8000.c b/drivers/media/dvb-frontends/dib8000.c
index 1f3bcb5a1de8..a54182dd0e91 100644
--- a/drivers/media/dvb-frontends/dib8000.c
+++ b/drivers/media/dvb-frontends/dib8000.c
@@ -23,8 +23,8 @@
 #define LAYER_B   2
 #define LAYER_C   3
 
-#define FE_CALLBACK_TIME_NEVER 0xffffffff
 #define MAX_NUMBER_OF_FRONTENDS 6
+/* #define DIB8000_AGC_FREEZE */
 
 static int debug;
 module_param(debug, int, 0644);
@@ -32,8 +32,6 @@ MODULE_PARM_DESC(debug, "turn on debugging (default: 0)");
 
 #define dprintk(args...) do { if (debug) { printk(KERN_DEBUG "DiB8000: "); printk(args); printk("\n"); } } while (0)
 
-#define FE_STATUS_TUNE_FAILED 0
-
 struct i2c_device {
 	struct i2c_adapter *adap;
 	u8 addr;
@@ -42,6 +40,23 @@ struct i2c_device {
 	struct mutex *i2c_buffer_lock;
 };
 
+enum param_loop_step {
+	LOOP_TUNE_1,
+	LOOP_TUNE_2
+};
+
+enum dib8000_autosearch_step {
+	AS_START = 0,
+	AS_SEARCHING_FFT,
+	AS_SEARCHING_GUARD,
+	AS_DONE = 100,
+};
+
+enum timeout_mode {
+	SYMBOL_DEPENDENT_OFF = 0,
+	SYMBOL_DEPENDENT_ON,
+};
+
 struct dib8000_state {
 	struct dib8000_config cfg;
 
@@ -72,7 +87,7 @@ struct dib8000_state {
 	u16 revision;
 	u8 isdbt_cfg_loaded;
 	enum frontend_tune_state tune_state;
-	u32 status;
+	s32 status;
 
 	struct dvb_frontend *fe[MAX_NUMBER_OF_FRONTENDS];
 
@@ -85,6 +100,30 @@ struct dib8000_state {
 
 	u16 tuner_enable;
 	struct i2c_adapter dib8096p_tuner_adap;
+	u16 current_demod_bw;
+
+	u16 seg_mask;
+	u16 seg_diff_mask;
+	u16 mode;
+	u8 layer_b_nb_seg;
+	u8 layer_c_nb_seg;
+
+	u8 channel_parameters_set;
+	u16 autosearch_state;
+	u16 found_nfft;
+	u16 found_guard;
+	u8 subchannel;
+	u8 symbol_duration;
+	u32 timeout;
+	u8 longest_intlv_layer;
+	u16 output_mode;
+
+#ifdef DIB8000_AGC_FREEZE
+	u16 agc1_max;
+	u16 agc1_min;
+	u16 agc2_max;
+	u16 agc2_min;
+#endif
 };
 
 enum dib8000_power_mode {
@@ -338,9 +377,9 @@ static void dib8000_set_acquisition_mode(struct dib8000_state *state)
 static int dib8000_set_output_mode(struct dvb_frontend *fe, int mode)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
-
 	u16 outreg, fifo_threshold, smo_mode, sram = 0x0205;	/* by default SDRAM deintlv is enabled */
 
+	state->output_mode = mode;
 	outreg = 0;
 	fifo_threshold = 1792;
 	smo_mode = (dib8000_read_word(state, 299) & 0x0050) | (1 << 1);
@@ -399,8 +438,9 @@ static int dib8000_set_output_mode(struct dvb_frontend *fe, int mode)
 static int dib8000_set_diversity_in(struct dvb_frontend *fe, int onoff)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
-	u16 sync_wait = dib8000_read_word(state, 273) & 0xfff0;
+	u16 tmp, sync_wait = dib8000_read_word(state, 273) & 0xfff0;
 
+	dprintk("set diversity input to %i", onoff);
 	if (!state->differential_constellation) {
 		dib8000_write_word(state, 272, 1 << 9);	//dvsy_off_lmod4 = 1
 		dib8000_write_word(state, 273, sync_wait | (1 << 2) | 2);	// sync_enable = 1; comb_mode = 2
@@ -424,6 +464,13 @@ static int dib8000_set_diversity_in(struct dvb_frontend *fe, int onoff)
 		dib8000_write_word(state, 271, 1);
 		break;
 	}
+
+	if (state->revision == 0x8002) {
+		tmp = dib8000_read_word(state, 903);
+		dib8000_write_word(state, 903, tmp & ~(1 << 3));
+		msleep(30);
+		dib8000_write_word(state, 903, tmp | (1 << 3));
+	}
 	return 0;
 }
 
@@ -468,27 +515,6 @@ static void dib8000_set_power_mode(struct dib8000_state *state, enum dib8000_pow
 	dib8000_write_word(state, 1280, reg_1280);
 }
 
-static int dib8000_init_sdram(struct dib8000_state *state)
-{
-	u16 reg = 0;
-	dprintk("Init sdram");
-
-	reg = dib8000_read_word(state, 274)&0xfff0;
-	/* P_dintlv_delay_ram = 7 because of MobileSdram */
-	dib8000_write_word(state, 274, reg | 0x7);
-
-	dib8000_write_word(state, 1803, (7<<2));
-
-	reg = dib8000_read_word(state, 1280);
-	/* force restart P_restart_sdram */
-	dib8000_write_word(state, 1280,  reg | (1<<2));
-
-	/* release restart P_restart_sdram */
-	dib8000_write_word(state, 1280,  reg);
-
-	return 0;
-}
-
 static int dib8000_set_adc_state(struct dib8000_state *state, enum dibx000_adc_states no)
 {
 	int ret = 0;
@@ -584,18 +610,23 @@ static int dib8000_set_bandwidth(struct dvb_frontend *fe, u32 bw)
 
 static int dib8000_sad_calib(struct dib8000_state *state)
 {
+	u8 sad_sel = 3;
+
 	if (state->revision == 0x8090) {
-		dprintk("%s: the sad calibration is not needed for the dib8096P",
-				__func__);
-		return 0;
-	}
-	/* internal */
-	dib8000_write_word(state, 923, (0 << 1) | (0 << 0));
-	dib8000_write_word(state, 924, 776);	// 0.625*3.3 / 4096
+		dib8000_write_word(state, 922, (sad_sel << 2));
+		dib8000_write_word(state, 923, 2048);
+
+		dib8000_write_word(state, 922, (sad_sel << 2) | 0x1);
+		dib8000_write_word(state, 922, (sad_sel << 2));
+	} else {
+		/* internal */
+		dib8000_write_word(state, 923, (0 << 1) | (0 << 0));
+		dib8000_write_word(state, 924, 776);
 
-	/* do the calibration */
-	dib8000_write_word(state, 923, (1 << 0));
-	dib8000_write_word(state, 923, (0 << 0));
+		/* do the calibration */
+		dib8000_write_word(state, 923, (1 << 0));
+		dib8000_write_word(state, 923, (0 << 0));
+	}
 
 	msleep(1);
 	return 0;
@@ -609,8 +640,8 @@ int dib8000_set_wbd_ref(struct dvb_frontend *fe, u16 value)
 	state->wbd_ref = value;
 	return dib8000_write_word(state, 106, value);
 }
-
 EXPORT_SYMBOL(dib8000_set_wbd_ref);
+
 static void dib8000_reset_pll_common(struct dib8000_state *state, const struct dibx000_bandwidth_config *bw)
 {
 	dprintk("ifreq: %d %x, inversion: %d", bw->ifreq, bw->ifreq, bw->ifreq >> 25);
@@ -685,20 +716,23 @@ static void dib8000_reset_pll(struct dib8000_state *state)
 }
 
 int dib8000_update_pll(struct dvb_frontend *fe,
-		struct dibx000_bandwidth_config *pll)
+		struct dibx000_bandwidth_config *pll, u32 bw, u8 ratio)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
 	u16 reg_1857, reg_1856 = dib8000_read_word(state, 1856);
-	u8 loopdiv, prediv;
+	u8 loopdiv, prediv, oldprediv = state->cfg.pll->pll_prediv ;
 	u32 internal, xtal;
 
 	/* get back old values */
 	prediv = reg_1856 & 0x3f;
 	loopdiv = (reg_1856 >> 6) & 0x3f;
 
-	if ((pll != NULL) && (pll->pll_prediv != prediv ||
-				pll->pll_ratio != loopdiv)) {
-		dprintk("Updating pll (prediv: old =  %d new = %d ; loopdiv : old = %d new = %d)", prediv, pll->pll_prediv, loopdiv, pll->pll_ratio);
+	if ((pll == NULL) || (pll->pll_prediv == prediv &&
+				pll->pll_ratio == loopdiv))
+		return -EINVAL;
+
+	dprintk("Updating pll (prediv: old =  %d new = %d ; loopdiv : old = %d new = %d)", prediv, pll->pll_prediv, loopdiv, pll->pll_ratio);
+	if (state->revision == 0x8090) {
 		reg_1856 &= 0xf000;
 		reg_1857 = dib8000_read_word(state, 1857);
 		/* disable PLL */
@@ -729,10 +763,33 @@ int dib8000_update_pll(struct dvb_frontend *fe,
 		reg_1856 = dib8000_read_word(state, 1856);
 		dprintk("PLL Updated with prediv = %d and loopdiv = %d",
 				reg_1856&0x3f, (reg_1856>>6)&0x3f);
+	} else {
+		if (bw != state->current_demod_bw) {
+			/** Bandwidth change => force PLL update **/
+			dprintk("PLL: Bandwidth Change %d MHz -> %d MHz (prediv: %d->%d)", state->current_demod_bw / 1000, bw / 1000, oldprediv, state->cfg.pll->pll_prediv);
+
+			if (state->cfg.pll->pll_prediv != oldprediv) {
+				/** Full PLL change only if prediv is changed **/
+
+				/** full update => bypass and reconfigure **/
+				dprintk("PLL: New Setting for %d MHz Bandwidth (prediv: %d, ratio: %d)", bw/1000, state->cfg.pll->pll_prediv, state->cfg.pll->pll_ratio);
+				dib8000_write_word(state, 902, dib8000_read_word(state, 902) | (1<<3)); /* bypass PLL */
+				dib8000_reset_pll(state);
+				dib8000_write_word(state, 898, 0x0004); /* sad */
+			} else
+				ratio = state->cfg.pll->pll_ratio;
 
-		return 0;
-	}
-	return -EINVAL;
+			state->current_demod_bw = bw;
+		}
+
+		if (ratio != 0) {
+			/** ratio update => only change ratio **/
+			dprintk("PLL: Update ratio (prediv: %d, ratio: %d)", state->cfg.pll->pll_prediv, ratio);
+			dib8000_write_word(state, 901, (state->cfg.pll->pll_prediv << 8) | (ratio << 0)); /* only the PLL ratio is updated. */
+		}
+}
+
+	return 0;
 }
 EXPORT_SYMBOL(dib8000_update_pll);
 
@@ -928,7 +985,7 @@ static int dib8000_reset(struct dvb_frontend *fe)
 	dib8000_set_power_mode(state, DIB8000_POWER_ALL);
 
 	/* always leave the VBG voltage on - it consumes almost nothing but takes a long time to start */
-	dib8000_set_adc_state(state, DIBX000_VBG_ENABLE);
+	dib8000_set_adc_state(state, DIBX000_ADC_OFF);
 
 	/* restart all parts */
 	dib8000_write_word(state, 770, 0xffff);
@@ -992,12 +1049,11 @@ static int dib8000_reset(struct dvb_frontend *fe)
 			l = *n++;
 		}
 	}
-	if (state->revision != 0x8090)
-		dib8000_write_word(state, 903, (0 << 4) | 2);
+
 	state->isdbt_cfg_loaded = 0;
 
 	//div_cfg override for special configs
-	if (state->cfg.div_cfg != 0)
+	if ((state->revision != 8090) && (state->cfg.div_cfg != 0))
 		dib8000_write_word(state, 903, state->cfg.div_cfg);
 
 	/* unforce divstr regardless whether i2c enumeration was done or not */
@@ -1006,10 +1062,12 @@ static int dib8000_reset(struct dvb_frontend *fe)
 	dib8000_set_bandwidth(fe, 6000);
 
 	dib8000_set_adc_state(state, DIBX000_SLOW_ADC_ON);
-	if (state->revision != 0x8090) {
-		dib8000_sad_calib(state);
+	dib8000_sad_calib(state);
+	if (state->revision != 0x8090)
 		dib8000_set_adc_state(state, DIBX000_SLOW_ADC_OFF);
-	}
+
+	/* ber_rs_len = 3 */
+	dib8000_write_word(state, 285, (dib8000_read_word(state, 285) & ~0x60) | (3 << 5));
 
 	dib8000_set_power_mode(state, DIB8000_POWER_INTERFACE_ONLY);
 
@@ -1441,6 +1499,7 @@ static int dib8096p_set_output_mode(struct dvb_frontend *fe, int mode)
 	u8 prefer_mpeg_mux_use = 1;
 	int ret = 0;
 
+	state->output_mode = mode;
 	dib8096p_host_bus_drive(state, 1);
 
 	fifo_threshold = 1792;
@@ -1879,782 +1938,643 @@ static const u16 adc_target_16dB[11] = {
 };
 static const u8 permu_seg[] = { 6, 5, 7, 4, 8, 3, 9, 2, 10, 1, 11, 0, 12 };
 
-static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosearching)
+static u16 dib8000_set_layer(struct dib8000_state *state, u8 layer_index, u16 max_constellation)
 {
-	u16 mode, max_constellation, seg_diff_mask = 0, nbseg_diff = 0;
-	u8 guard, crate, constellation, timeI;
-	u16 i, coeff[4], P_cfr_left_edge = 0, P_cfr_right_edge = 0, seg_mask13 = 0x1fff;	// All 13 segments enabled
-	const s16 *ncoeff = NULL, *ana_fe;
-	u16 tmcc_pow = 0;
-	u16 coff_pow = 0x2800;
-	u16 init_prbs = 0xfff;
-	u16 ana_gain = 0;
-
-	if (state->revision == 0x8090)
-		dib8000_init_sdram(state);
+	u8  cr, constellation, time_intlv;
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
 
-	if (state->ber_monitored_layer != LAYER_ALL)
-		dib8000_write_word(state, 285, (dib8000_read_word(state, 285) & 0x60) | state->ber_monitored_layer);
-	else
-		dib8000_write_word(state, 285, dib8000_read_word(state, 285) & 0x60);
-
-	i = dib8000_read_word(state, 26) & 1;	// P_dds_invspec
-	dib8000_write_word(state, 26, state->fe[0]->dtv_property_cache.inversion^i);
-
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode) {
-		//compute new dds_freq for the seg and adjust prbs
-		int seg_offset =
-			state->fe[0]->dtv_property_cache.isdbt_sb_segment_idx -
-			(state->fe[0]->dtv_property_cache.isdbt_sb_segment_count / 2) -
-			(state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2);
-		int clk = state->cfg.pll->internal;
-		u32 segtodds = ((u32) (430 << 23) / clk) << 3;	// segtodds = SegBW / Fclk * pow(2,26)
-		int dds_offset = seg_offset * segtodds;
-		int new_dds, sub_channel;
-		if ((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2) == 0)
-			dds_offset -= (int)(segtodds / 2);
-
-		if (state->cfg.pll->ifreq == 0) {
-			if ((state->fe[0]->dtv_property_cache.inversion ^ i) == 0) {
-				dib8000_write_word(state, 26, dib8000_read_word(state, 26) | 1);
-				new_dds = dds_offset;
-			} else
-				new_dds = dds_offset;
-
-			// We shift tuning frequency if the wanted segment is :
-			//  - the segment of center frequency with an odd total number of segments
-			//  - the segment to the left of center frequency with an even total number of segments
-			//  - the segment to the right of center frequency with an even total number of segments
-			if ((state->fe[0]->dtv_property_cache.delivery_system == SYS_ISDBT)
-				&& (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1)
-					&& (((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2)
-					  && (state->fe[0]->dtv_property_cache.isdbt_sb_segment_idx ==
-				  ((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count / 2) + 1)))
-					 || (((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2) == 0)
-						 && (state->fe[0]->dtv_property_cache.isdbt_sb_segment_idx == (state->fe[0]->dtv_property_cache.isdbt_sb_segment_count / 2)))
-					 || (((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2) == 0)
-						 && (state->fe[0]->dtv_property_cache.isdbt_sb_segment_idx ==
-							 ((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count / 2) + 1)))
-					)) {
-				new_dds -= ((u32) (850 << 22) / clk) << 4;	// new_dds = 850 (freq shift in KHz) / Fclk * pow(2,26)
-			}
-		} else {
-			if ((state->fe[0]->dtv_property_cache.inversion ^ i) == 0)
-				new_dds = state->cfg.pll->ifreq - dds_offset;
-			else
-				new_dds = state->cfg.pll->ifreq + dds_offset;
-		}
-		dib8000_write_word(state, 27, (u16) ((new_dds >> 16) & 0x01ff));
-		dib8000_write_word(state, 28, (u16) (new_dds & 0xffff));
-		if (state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2)
-			sub_channel = ((state->fe[0]->dtv_property_cache.isdbt_sb_subchannel + (3 * seg_offset) + 1) % 41) / 3;
-		else
-			sub_channel = ((state->fe[0]->dtv_property_cache.isdbt_sb_subchannel + (3 * seg_offset)) % 41) / 3;
-		sub_channel -= 6;
-
-		if (state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_2K
-				|| state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_4K) {
-			dib8000_write_word(state, 219, dib8000_read_word(state, 219) | 0x1);	//adp_pass =1
-			dib8000_write_word(state, 190, dib8000_read_word(state, 190) | (0x1 << 14));	//pha3_force_pha_shift = 1
-		} else {
-			dib8000_write_word(state, 219, dib8000_read_word(state, 219) & 0xfffe);	//adp_pass =0
-			dib8000_write_word(state, 190, dib8000_read_word(state, 190) & 0xbfff);	//pha3_force_pha_shift = 0
-		}
-
-		switch (state->fe[0]->dtv_property_cache.transmission_mode) {
-		case TRANSMISSION_MODE_2K:
-			switch (sub_channel) {
-			case -6:
-				init_prbs = 0x0;
-				break;	// 41, 0, 1
-			case -5:
-				init_prbs = 0x423;
-				break;	// 02~04
-			case -4:
-				init_prbs = 0x9;
-				break;	// 05~07
-			case -3:
-				init_prbs = 0x5C7;
-				break;	// 08~10
-			case -2:
-				init_prbs = 0x7A6;
-				break;	// 11~13
-			case -1:
-				init_prbs = 0x3D8;
-				break;	// 14~16
-			case 0:
-				init_prbs = 0x527;
-				break;	// 17~19
-			case 1:
-				init_prbs = 0x7FF;
-				break;	// 20~22
-			case 2:
-				init_prbs = 0x79B;
-				break;	// 23~25
-			case 3:
-				init_prbs = 0x3D6;
-				break;	// 26~28
-			case 4:
-				init_prbs = 0x3A2;
-				break;	// 29~31
-			case 5:
-				init_prbs = 0x53B;
-				break;	// 32~34
-			case 6:
-				init_prbs = 0x2F4;
-				break;	// 35~37
-			default:
-			case 7:
-				init_prbs = 0x213;
-				break;	// 38~40
-			}
-			break;
-
-		case TRANSMISSION_MODE_4K:
-			switch (sub_channel) {
-			case -6:
-				init_prbs = 0x0;
-				break;	// 41, 0, 1
-			case -5:
-				init_prbs = 0x208;
-				break;	// 02~04
-			case -4:
-				init_prbs = 0xC3;
-				break;	// 05~07
-			case -3:
-				init_prbs = 0x7B9;
-				break;	// 08~10
-			case -2:
-				init_prbs = 0x423;
-				break;	// 11~13
-			case -1:
-				init_prbs = 0x5C7;
-				break;	// 14~16
-			case 0:
-				init_prbs = 0x3D8;
-				break;	// 17~19
-			case 1:
-				init_prbs = 0x7FF;
-				break;	// 20~22
-			case 2:
-				init_prbs = 0x3D6;
-				break;	// 23~25
-			case 3:
-				init_prbs = 0x53B;
-				break;	// 26~28
-			case 4:
-				init_prbs = 0x213;
-				break;	// 29~31
-			case 5:
-				init_prbs = 0x29;
-				break;	// 32~34
-			case 6:
-				init_prbs = 0xD0;
-				break;	// 35~37
-			default:
-			case 7:
-				init_prbs = 0x48E;
-				break;	// 38~40
-			}
-			break;
-
-		default:
-		case TRANSMISSION_MODE_8K:
-			switch (sub_channel) {
-			case -6:
-				init_prbs = 0x0;
-				break;	// 41, 0, 1
-			case -5:
-				init_prbs = 0x740;
-				break;	// 02~04
-			case -4:
-				init_prbs = 0x069;
-				break;	// 05~07
-			case -3:
-				init_prbs = 0x7DD;
-				break;	// 08~10
-			case -2:
-				init_prbs = 0x208;
-				break;	// 11~13
-			case -1:
-				init_prbs = 0x7B9;
-				break;	// 14~16
-			case 0:
-				init_prbs = 0x5C7;
-				break;	// 17~19
-			case 1:
-				init_prbs = 0x7FF;
-				break;	// 20~22
-			case 2:
-				init_prbs = 0x53B;
-				break;	// 23~25
-			case 3:
-				init_prbs = 0x29;
-				break;	// 26~28
-			case 4:
-				init_prbs = 0x48E;
-				break;	// 29~31
-			case 5:
-				init_prbs = 0x4C4;
-				break;	// 32~34
-			case 6:
-				init_prbs = 0x367;
-				break;	// 33~37
-			default:
-			case 7:
-				init_prbs = 0x684;
-				break;	// 38~40
-			}
-			break;
-		}
-	} else {
-		dib8000_write_word(state, 27, (u16) ((state->cfg.pll->ifreq >> 16) & 0x01ff));
-		dib8000_write_word(state, 28, (u16) (state->cfg.pll->ifreq & 0xffff));
-		dib8000_write_word(state, 26, (u16) ((state->cfg.pll->ifreq >> 25) & 0x0003));
-	}
-	/*P_mode == ?? */
-	dib8000_write_word(state, 10, (seq << 4));
-	//  dib8000_write_word(state, 287, (dib8000_read_word(state, 287) & 0xe000) | 0x1000);
-
-	switch (state->fe[0]->dtv_property_cache.guard_interval) {
-	case GUARD_INTERVAL_1_32:
-		guard = 0;
-		break;
-	case GUARD_INTERVAL_1_16:
-		guard = 1;
-		break;
-	case GUARD_INTERVAL_1_8:
-		guard = 2;
-		break;
-	case GUARD_INTERVAL_1_4:
-	default:
-		guard = 3;
-		break;
-	}
-
-	dib8000_write_word(state, 1, (init_prbs << 2) | (guard & 0x3));	// ADDR 1
-
-	max_constellation = DQPSK;
-	for (i = 0; i < 3; i++) {
-		switch (state->fe[0]->dtv_property_cache.layer[i].modulation) {
-		case DQPSK:
+	switch (c->layer[layer_index].modulation) {
+	case DQPSK:
 			constellation = 0;
 			break;
-		case QPSK:
+	case  QPSK:
 			constellation = 1;
 			break;
-		case QAM_16:
+	case QAM_16:
 			constellation = 2;
 			break;
-		case QAM_64:
-		default:
+	case QAM_64:
+	default:
 			constellation = 3;
 			break;
-		}
+	}
 
-		switch (state->fe[0]->dtv_property_cache.layer[i].fec) {
-		case FEC_1_2:
-			crate = 1;
+	switch (c->layer[layer_index].fec) {
+	case FEC_1_2:
+			cr = 1;
 			break;
-		case FEC_2_3:
-			crate = 2;
+	case FEC_2_3:
+			cr = 2;
 			break;
-		case FEC_3_4:
-			crate = 3;
+	case FEC_3_4:
+			cr = 3;
 			break;
-		case FEC_5_6:
-			crate = 5;
+	case FEC_5_6:
+			cr = 5;
 			break;
-		case FEC_7_8:
-		default:
-			crate = 7;
+	case FEC_7_8:
+	default:
+			cr = 7;
 			break;
-		}
+	}
 
-		if ((state->fe[0]->dtv_property_cache.layer[i].interleaving > 0) &&
-				((state->fe[0]->dtv_property_cache.layer[i].interleaving <= 3) ||
-				 (state->fe[0]->dtv_property_cache.layer[i].interleaving == 4 && state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1))
-			)
-			timeI = state->fe[0]->dtv_property_cache.layer[i].interleaving;
-		else
-			timeI = 0;
-		dib8000_write_word(state, 2 + i, (constellation << 10) | ((state->fe[0]->dtv_property_cache.layer[i].segment_count & 0xf) << 6) |
-					(crate << 3) | timeI);
-		if (state->fe[0]->dtv_property_cache.layer[i].segment_count > 0) {
-			switch (max_constellation) {
-			case DQPSK:
-			case QPSK:
-				if (state->fe[0]->dtv_property_cache.layer[i].modulation == QAM_16 ||
-					state->fe[0]->dtv_property_cache.layer[i].modulation == QAM_64)
-					max_constellation = state->fe[0]->dtv_property_cache.layer[i].modulation;
+	if ((c->layer[layer_index].interleaving > 0) && ((c->layer[layer_index].interleaving <= 3) || (c->layer[layer_index].interleaving == 4 && c->isdbt_sb_mode == 1)))
+		time_intlv = c->layer[layer_index].interleaving;
+	else
+		time_intlv = 0;
+
+	dib8000_write_word(state, 2 + layer_index, (constellation << 10) | ((c->layer[layer_index].segment_count & 0xf) << 6) | (cr << 3) | time_intlv);
+	if (c->layer[layer_index].segment_count > 0) {
+		switch (max_constellation) {
+		case DQPSK:
+		case QPSK:
+				if (c->layer[layer_index].modulation == QAM_16 || c->layer[layer_index].modulation == QAM_64)
+					max_constellation = c->layer[layer_index].modulation;
 				break;
-			case QAM_16:
-				if (state->fe[0]->dtv_property_cache.layer[i].modulation == QAM_64)
-					max_constellation = state->fe[0]->dtv_property_cache.layer[i].modulation;
+		case QAM_16:
+				if (c->layer[layer_index].modulation == QAM_64)
+					max_constellation = c->layer[layer_index].modulation;
 				break;
-			}
 		}
 	}
 
-	mode = fft_to_mode(state);
+	return  max_constellation;
+}
 
-	//dib8000_write_word(state, 5, 13); /*p_last_seg = 13*/
+static const u16 adp_Q64[4] = {0x0148, 0xfff0, 0x00a4, 0xfff8}; /* P_adp_regul_cnt 0.04, P_adp_noise_cnt -0.002, P_adp_regul_ext 0.02, P_adp_noise_ext -0.001 */
+static const u16 adp_Q16[4] = {0x023d, 0xffdf, 0x00a4, 0xfff0}; /* P_adp_regul_cnt 0.07, P_adp_noise_cnt -0.004, P_adp_regul_ext 0.02, P_adp_noise_ext -0.002 */
+static const u16 adp_Qdefault[4] = {0x099a, 0xffae, 0x0333, 0xfff8}; /* P_adp_regul_cnt 0.3,  P_adp_noise_cnt -0.01,  P_adp_regul_ext 0.1,  P_adp_noise_ext -0.002 */
+static u16 dib8000_adp_fine_tune(struct dib8000_state *state, u16 max_constellation)
+{
+	u16 i, ana_gain = 0;
+	const u16 *adp;
 
-	dib8000_write_word(state, 274, (dib8000_read_word(state, 274) & 0xffcf) |
-				((state->fe[0]->dtv_property_cache.isdbt_partial_reception & 1) << 5) | ((state->fe[0]->dtv_property_cache.
-												 isdbt_sb_mode & 1) << 4));
+	/* channel estimation fine configuration */
+	switch (max_constellation) {
+	case QAM_64:
+			ana_gain = 0x7;
+			adp = &adp_Q64[0];
+			break;
+	case QAM_16:
+			ana_gain = 0x7;
+			adp = &adp_Q16[0];
+			break;
+	default:
+			ana_gain = 0;
+			adp = &adp_Qdefault[0];
+			break;
+	}
 
-	dprintk("mode = %d ; guard = %d", mode, state->fe[0]->dtv_property_cache.guard_interval);
+	for (i = 0; i < 4; i++)
+		dib8000_write_word(state, 215 + i, adp[i]);
 
-	/* signal optimization parameter */
+	return ana_gain;
+}
 
-	if (state->fe[0]->dtv_property_cache.isdbt_partial_reception) {
-		seg_diff_mask = (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK) << permu_seg[0];
-		for (i = 1; i < 3; i++)
-			nbseg_diff +=
-				(state->fe[0]->dtv_property_cache.layer[i].modulation == DQPSK) * state->fe[0]->dtv_property_cache.layer[i].segment_count;
-		for (i = 0; i < nbseg_diff; i++)
-			seg_diff_mask |= 1 << permu_seg[i + 1];
-	} else {
-		for (i = 0; i < 3; i++)
-			nbseg_diff +=
-				(state->fe[0]->dtv_property_cache.layer[i].modulation == DQPSK) * state->fe[0]->dtv_property_cache.layer[i].segment_count;
-		for (i = 0; i < nbseg_diff; i++)
-			seg_diff_mask |= 1 << permu_seg[i];
+static void dib8000_update_ana_gain(struct dib8000_state *state, u16 ana_gain)
+{
+	u16 i;
+
+	dib8000_write_word(state, 116, ana_gain);
+
+	/* update ADC target depending on ana_gain */
+	if (ana_gain) { /* set -16dB ADC target for ana_gain=-1 */
+		for (i = 0; i < 10; i++)
+			dib8000_write_word(state, 80 + i, adc_target_16dB[i]);
+	} else { /* set -22dB ADC target for ana_gain=0 */
+		for (i = 0; i < 10; i++)
+			dib8000_write_word(state, 80 + i, adc_target_16dB[i] - 355);
 	}
-	dprintk("nbseg_diff = %X (%d)", seg_diff_mask, seg_diff_mask);
+}
 
-	state->differential_constellation = (seg_diff_mask != 0);
-	if (state->revision != 0x8090)
-		dib8000_set_diversity_in(state->fe[0], state->diversity_onoff);
-	else
-		dib8096p_set_diversity_in(state->fe[0], state->diversity_onoff);
+static void dib8000_load_ana_fe_coefs(struct dib8000_state *state, const s16 *ana_fe)
+{
+	u16 mode = 0;
 
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
-		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 1)
-			seg_mask13 = 0x00E0;
-		else		// 1-segment
-			seg_mask13 = 0x0040;
-	} else
-		seg_mask13 = 0x1fff;
+	if (state->isdbt_cfg_loaded == 0)
+		for (mode = 0; mode < 24; mode++)
+			dib8000_write_word(state, 117 + mode, ana_fe[mode]);
+}
 
-	// WRITE: Mode & Diff mask
-	dib8000_write_word(state, 0, (mode << 13) | seg_diff_mask);
+static const u16 lut_prbs_2k[14] = {
+	0, 0x423, 0x009, 0x5C7, 0x7A6, 0x3D8, 0x527, 0x7FF, 0x79B, 0x3D6, 0x3A2, 0x53B, 0x2F4, 0x213
+};
+static const u16 lut_prbs_4k[14] = {
+	0, 0x208, 0x0C3, 0x7B9, 0x423, 0x5C7, 0x3D8, 0x7FF, 0x3D6, 0x53B, 0x213, 0x029, 0x0D0, 0x48E
+};
+static const u16 lut_prbs_8k[14] = {
+	0, 0x740, 0x069, 0x7DD, 0x208, 0x7B9, 0x5C7, 0x7FF, 0x53B, 0x029, 0x48E, 0x4C4, 0x367, 0x684
+};
 
-	if ((seg_diff_mask) || (state->fe[0]->dtv_property_cache.isdbt_sb_mode))
-		dib8000_write_word(state, 268, (dib8000_read_word(state, 268) & 0xF9FF) | 0x0200);
-	else
-		dib8000_write_word(state, 268, (2 << 9) | 39);	//init value
+static u16 dib8000_get_init_prbs(struct dib8000_state *state, u16 subchannel)
+{
+	int sub_channel_prbs_group = 0;
 
-	// ---- SMALL ----
-	// P_small_seg_diff
-	dib8000_write_word(state, 352, seg_diff_mask);	// ADDR 352
+	sub_channel_prbs_group = (subchannel / 3) + 1;
+	dprintk("sub_channel_prbs_group = %d , subchannel =%d prbs = 0x%04x", sub_channel_prbs_group, subchannel, lut_prbs_8k[sub_channel_prbs_group]);
 
-	dib8000_write_word(state, 353, seg_mask13);	// ADDR 353
+	switch (state->fe[0]->dtv_property_cache.transmission_mode) {
+	case TRANSMISSION_MODE_2K:
+			return lut_prbs_2k[sub_channel_prbs_group];
+	case TRANSMISSION_MODE_4K:
+			return lut_prbs_4k[sub_channel_prbs_group];
+	default:
+	case TRANSMISSION_MODE_8K:
+			return lut_prbs_8k[sub_channel_prbs_group];
+	}
+}
+
+static void dib8000_set_13seg_channel(struct dib8000_state *state)
+{
+	u16 i;
+	u16 coff_pow = 0x2800;
 
-/*	// P_small_narrow_band=0, P_small_last_seg=13, P_small_offset_num_car=5 */
+	state->seg_mask = 0x1fff; /* All 13 segments enabled */
 
-	// ---- SMALL ----
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
-		switch (state->fe[0]->dtv_property_cache.transmission_mode) {
+	/* ---- COFF ---- Carloff, the most robust --- */
+	if (state->isdbt_cfg_loaded == 0) {  /* if not Sound Broadcasting mode : put default values for 13 segments */
+		dib8000_write_word(state, 180, (16 << 6) | 9);
+		dib8000_write_word(state, 187, (4 << 12) | (8 << 5) | 0x2);
+		coff_pow = 0x2800;
+		for (i = 0; i < 6; i++)
+			dib8000_write_word(state, 181+i, coff_pow);
+
+		/* P_ctrl_corm_thres4pre_freq_inh=1, P_ctrl_pre_freq_mode_sat=1 */
+		/* P_ctrl_pre_freq_mode_sat=1, P_ctrl_pre_freq_inh=0, P_ctrl_pre_freq_step = 3, P_pre_freq_win_len=1 */
+		dib8000_write_word(state, 338, (1 << 12) | (1 << 10) | (0 << 9) | (3 << 5) | 1);
+
+		/* P_ctrl_pre_freq_win_len=8, P_ctrl_pre_freq_thres_lockin=6 */
+		dib8000_write_word(state, 340, (8 << 6) | (6 << 0));
+		/* P_ctrl_pre_freq_thres_lockout=4, P_small_use_tmcc/ac/cp=1 */
+		dib8000_write_word(state, 341, (4 << 3) | (1 << 2) | (1 << 1) | (1 << 0));
+
+		dib8000_write_word(state, 228, 0);  /* default value */
+		dib8000_write_word(state, 265, 31); /* default value */
+		dib8000_write_word(state, 205, 0x200f); /* init value */
+	}
+
+	/*
+	 * make the cpil_coff_lock more robust but slower p_coff_winlen
+	 * 6bits; p_coff_thres_lock 6bits (for coff lock if needed)
+	 */
+
+	if (state->cfg.pll->ifreq == 0)
+		dib8000_write_word(state, 266, ~state->seg_mask | state->seg_diff_mask | 0x40); /* P_equal_noise_seg_inh */
+
+	dib8000_load_ana_fe_coefs(state, ana_fe_coeff_13seg);
+}
+
+static void dib8000_set_subchannel_prbs(struct dib8000_state *state, u16 init_prbs)
+{
+	u16 reg_1;
+
+	reg_1 = dib8000_read_word(state, 1);
+	dib8000_write_word(state, 1, (init_prbs << 2) | (reg_1 & 0x3)); /* ADDR 1 */
+}
+
+static void dib8000_small_fine_tune(struct dib8000_state *state)
+{
+	u16 i;
+	const s16 *ncoeff;
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+
+	dib8000_write_word(state, 352, state->seg_diff_mask);
+	dib8000_write_word(state, 353, state->seg_mask);
+
+	/* P_small_coef_ext_enable=ISDB-Tsb, P_small_narrow_band=ISDB-Tsb, P_small_last_seg=13, P_small_offset_num_car=5 */
+	dib8000_write_word(state, 351, (c->isdbt_sb_mode << 9) | (c->isdbt_sb_mode << 8) | (13 << 4) | 5);
+
+	if (c->isdbt_sb_mode) {
+		/* ---- SMALL ---- */
+		switch (c->transmission_mode) {
 		case TRANSMISSION_MODE_2K:
-			if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0) {
-				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK)
-					ncoeff = coeff_2k_sb_1seg_dqpsk;
-				else	// QPSK or QAM
-					ncoeff = coeff_2k_sb_1seg;
-			} else {	// 3-segments
-				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK) {
-					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK)
-						ncoeff = coeff_2k_sb_3seg_0dqpsk_1dqpsk;
-					else	// QPSK or QAM on external segments
-						ncoeff = coeff_2k_sb_3seg_0dqpsk;
-				} else {	// QPSK or QAM on central segment
-					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK)
-						ncoeff = coeff_2k_sb_3seg_1dqpsk;
-					else	// QPSK or QAM on external segments
-						ncoeff = coeff_2k_sb_3seg;
+				if (c->isdbt_partial_reception == 0) { /* 1-seg */
+					if (c->layer[0].modulation == DQPSK) /* DQPSK */
+						ncoeff = coeff_2k_sb_1seg_dqpsk;
+					else /* QPSK or QAM */
+						ncoeff = coeff_2k_sb_1seg;
+				} else { /* 3-segments */
+					if (c->layer[0].modulation == DQPSK) { /* DQPSK on central segment */
+						if (c->layer[1].modulation == DQPSK) /* DQPSK on external segments */
+							ncoeff = coeff_2k_sb_3seg_0dqpsk_1dqpsk;
+						else /* QPSK or QAM on external segments */
+							ncoeff = coeff_2k_sb_3seg_0dqpsk;
+					} else { /* QPSK or QAM on central segment */
+						if (c->layer[1].modulation == DQPSK) /* DQPSK on external segments */
+							ncoeff = coeff_2k_sb_3seg_1dqpsk;
+						else /* QPSK or QAM on external segments */
+							ncoeff = coeff_2k_sb_3seg;
+					}
 				}
-			}
-			break;
-
+				break;
 		case TRANSMISSION_MODE_4K:
-			if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0) {
-				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK)
-					ncoeff = coeff_4k_sb_1seg_dqpsk;
-				else	// QPSK or QAM
-					ncoeff = coeff_4k_sb_1seg;
-			} else {	// 3-segments
-				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK) {
-					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK) {
-						ncoeff = coeff_4k_sb_3seg_0dqpsk_1dqpsk;
-					} else {	// QPSK or QAM on external segments
-						ncoeff = coeff_4k_sb_3seg_0dqpsk;
+				if (c->isdbt_partial_reception == 0) { /* 1-seg */
+					if (c->layer[0].modulation == DQPSK) /* DQPSK */
+						ncoeff = coeff_4k_sb_1seg_dqpsk;
+					else /* QPSK or QAM */
+						ncoeff = coeff_4k_sb_1seg;
+				} else { /* 3-segments */
+					if (c->layer[0].modulation == DQPSK) { /* DQPSK on central segment */
+						if (c->layer[1].modulation == DQPSK) /* DQPSK on external segments */
+							ncoeff = coeff_4k_sb_3seg_0dqpsk_1dqpsk;
+						else /* QPSK or QAM on external segments */
+							ncoeff = coeff_4k_sb_3seg_0dqpsk;
+					} else { /* QPSK or QAM on central segment */
+						if (c->layer[1].modulation == DQPSK) /* DQPSK on external segments */
+							ncoeff = coeff_4k_sb_3seg_1dqpsk;
+						else /* QPSK or QAM on external segments */
+							ncoeff = coeff_4k_sb_3seg;
 					}
-				} else {	// QPSK or QAM on central segment
-					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK) {
-						ncoeff = coeff_4k_sb_3seg_1dqpsk;
-					} else	// QPSK or QAM on external segments
-						ncoeff = coeff_4k_sb_3seg;
 				}
-			}
-			break;
-
+				break;
 		case TRANSMISSION_MODE_AUTO:
 		case TRANSMISSION_MODE_8K:
 		default:
-			if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0) {
-				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK)
-					ncoeff = coeff_8k_sb_1seg_dqpsk;
-				else	// QPSK or QAM
-					ncoeff = coeff_8k_sb_1seg;
-			} else {	// 3-segments
-				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK) {
-					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK) {
-						ncoeff = coeff_8k_sb_3seg_0dqpsk_1dqpsk;
-					} else {	// QPSK or QAM on external segments
-						ncoeff = coeff_8k_sb_3seg_0dqpsk;
+				if (c->isdbt_partial_reception == 0) { /* 1-seg */
+					if (c->layer[0].modulation == DQPSK) /* DQPSK */
+						ncoeff = coeff_8k_sb_1seg_dqpsk;
+					else /* QPSK or QAM */
+						ncoeff = coeff_8k_sb_1seg;
+				} else { /* 3-segments */
+					if (c->layer[0].modulation == DQPSK) { /* DQPSK on central segment */
+						if (c->layer[1].modulation == DQPSK) /* DQPSK on external segments */
+							ncoeff = coeff_8k_sb_3seg_0dqpsk_1dqpsk;
+						else /* QPSK or QAM on external segments */
+							ncoeff = coeff_8k_sb_3seg_0dqpsk;
+					} else { /* QPSK or QAM on central segment */
+						if (c->layer[1].modulation == DQPSK) /* DQPSK on external segments */
+							ncoeff = coeff_8k_sb_3seg_1dqpsk;
+						else /* QPSK or QAM on external segments */
+							ncoeff = coeff_8k_sb_3seg;
 					}
-				} else {	// QPSK or QAM on central segment
-					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK) {
-						ncoeff = coeff_8k_sb_3seg_1dqpsk;
-					} else	// QPSK or QAM on external segments
-						ncoeff = coeff_8k_sb_3seg;
 				}
-			}
-			break;
+				break;
 		}
+
 		for (i = 0; i < 8; i++)
 			dib8000_write_word(state, 343 + i, ncoeff[i]);
 	}
+}
 
-	// P_small_coef_ext_enable=ISDB-Tsb, P_small_narrow_band=ISDB-Tsb, P_small_last_seg=13, P_small_offset_num_car=5
-	dib8000_write_word(state, 351,
-				(state->fe[0]->dtv_property_cache.isdbt_sb_mode << 9) | (state->fe[0]->dtv_property_cache.isdbt_sb_mode << 8) | (13 << 4) | 5);
+static const u16 coff_thres_1seg[3] = {300, 150, 80};
+static const u16 coff_thres_3seg[3] = {350, 300, 250};
+static void dib8000_set_sb_channel(struct dib8000_state *state)
+{
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	const u16 *coff;
+	u16 i;
 
-	// ---- COFF ----
-	// Carloff, the most robust
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
+	if (c->transmission_mode == TRANSMISSION_MODE_2K || c->transmission_mode == TRANSMISSION_MODE_4K) {
+		dib8000_write_word(state, 219, dib8000_read_word(state, 219) | 0x1); /* adp_pass =1 */
+		dib8000_write_word(state, 190, dib8000_read_word(state, 190) | (0x1 << 14)); /* pha3_force_pha_shift = 1 */
+	} else {
+		dib8000_write_word(state, 219, dib8000_read_word(state, 219) & 0xfffe); /* adp_pass =0 */
+		dib8000_write_word(state, 190, dib8000_read_word(state, 190) & 0xbfff); /* pha3_force_pha_shift = 0 */
+	}
 
-		// P_coff_cpil_alpha=4, P_coff_inh=0, P_coff_cpil_winlen=64
-		// P_coff_narrow_band=1, P_coff_square_val=1, P_coff_one_seg=~partial_rcpt, P_coff_use_tmcc=1, P_coff_use_ac=1
-		dib8000_write_word(state, 187,
-					(4 << 12) | (0 << 11) | (63 << 5) | (0x3 << 3) | ((~state->fe[0]->dtv_property_cache.isdbt_partial_reception & 1) << 2)
-					| 0x3);
+	if (c->isdbt_partial_reception == 1) /* 3-segments */
+		state->seg_mask = 0x00E0;
+	else /* 1-segment */
+		state->seg_mask = 0x0040;
 
-/*		// P_small_coef_ext_enable = 1 */
-/*		dib8000_write_word(state, 351, dib8000_read_word(state, 351) | 0x200); */
+	dib8000_write_word(state, 268, (dib8000_read_word(state, 268) & 0xF9FF) | 0x0200);
 
-		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0) {
+	/* ---- COFF ---- Carloff, the most robust --- */
+	/* P_coff_cpil_alpha=4, P_coff_inh=0, P_coff_cpil_winlen=64, P_coff_narrow_band=1, P_coff_square_val=1, P_coff_one_seg=~partial_rcpt, P_coff_use_tmcc=1, P_coff_use_ac=1 */
+	dib8000_write_word(state, 187, (4 << 12) | (0 << 11) | (63 << 5) | (0x3 << 3) | ((~c->isdbt_partial_reception & 1) << 2) | 0x3);
 
-			// P_coff_winlen=63, P_coff_thres_lock=15, P_coff_one_seg_width= (P_mode == 3) , P_coff_one_seg_sym= (P_mode-1)
-			if (mode == 3)
-				dib8000_write_word(state, 180, 0x1fcf | ((mode - 1) << 14));
-			else
-				dib8000_write_word(state, 180, 0x0fcf | ((mode - 1) << 14));
-			// P_ctrl_corm_thres4pre_freq_inh=1,P_ctrl_pre_freq_mode_sat=1,
-			// P_ctrl_pre_freq_inh=0, P_ctrl_pre_freq_step = 5, P_pre_freq_win_len=4
-			dib8000_write_word(state, 338, (1 << 12) | (1 << 10) | (0 << 9) | (5 << 5) | 4);
-			// P_ctrl_pre_freq_win_len=16, P_ctrl_pre_freq_thres_lockin=8
-			dib8000_write_word(state, 340, (16 << 6) | (8 << 0));
-			// P_ctrl_pre_freq_thres_lockout=6, P_small_use_tmcc/ac/cp=1
-			dib8000_write_word(state, 341, (6 << 3) | (1 << 2) | (1 << 1) | (1 << 0));
-
-			// P_coff_corthres_8k, 4k, 2k and P_coff_cpilthres_8k, 4k, 2k
-			dib8000_write_word(state, 181, 300);
-			dib8000_write_word(state, 182, 150);
-			dib8000_write_word(state, 183, 80);
-			dib8000_write_word(state, 184, 300);
-			dib8000_write_word(state, 185, 150);
-			dib8000_write_word(state, 186, 80);
-		} else {	// Sound Broadcasting mode 3 seg
-			// P_coff_one_seg_sym= 1, P_coff_one_seg_width= 1, P_coff_winlen=63, P_coff_thres_lock=15
-			/*	if (mode == 3) */
-			/*		dib8000_write_word(state, 180, 0x2fca | ((0) << 14)); */
-			/*	else */
-			/*		dib8000_write_word(state, 180, 0x2fca | ((1) << 14)); */
-			dib8000_write_word(state, 180, 0x1fcf | (1 << 14));
-
-			// P_ctrl_corm_thres4pre_freq_inh = 1, P_ctrl_pre_freq_mode_sat=1,
-			// P_ctrl_pre_freq_inh=0, P_ctrl_pre_freq_step = 4, P_pre_freq_win_len=4
-			dib8000_write_word(state, 338, (1 << 12) | (1 << 10) | (0 << 9) | (4 << 5) | 4);
-			// P_ctrl_pre_freq_win_len=16, P_ctrl_pre_freq_thres_lockin=8
-			dib8000_write_word(state, 340, (16 << 6) | (8 << 0));
-			//P_ctrl_pre_freq_thres_lockout=6, P_small_use_tmcc/ac/cp=1
-			dib8000_write_word(state, 341, (6 << 3) | (1 << 2) | (1 << 1) | (1 << 0));
-
-			// P_coff_corthres_8k, 4k, 2k and P_coff_cpilthres_8k, 4k, 2k
-			dib8000_write_word(state, 181, 350);
-			dib8000_write_word(state, 182, 300);
-			dib8000_write_word(state, 183, 250);
-			dib8000_write_word(state, 184, 350);
-			dib8000_write_word(state, 185, 300);
-			dib8000_write_word(state, 186, 250);
-		}
+	dib8000_write_word(state, 340, (16 << 6) | (8 << 0)); /* P_ctrl_pre_freq_win_len=16, P_ctrl_pre_freq_thres_lockin=8 */
+	dib8000_write_word(state, 341, (6 << 3) | (1 << 2) | (1 << 1) | (1 << 0));/* P_ctrl_pre_freq_thres_lockout=6, P_small_use_tmcc/ac/cp=1 */
 
-	} else if (state->isdbt_cfg_loaded == 0) {	// if not Sound Broadcasting mode : put default values for 13 segments
-		dib8000_write_word(state, 180, (16 << 6) | 9);
-		dib8000_write_word(state, 187, (4 << 12) | (8 << 5) | 0x2);
-		coff_pow = 0x2800;
-		for (i = 0; i < 6; i++)
-			dib8000_write_word(state, 181 + i, coff_pow);
+	/* Sound Broadcasting mode 1 seg */
+	if (c->isdbt_partial_reception == 0) {
+		/* P_coff_winlen=63, P_coff_thres_lock=15, P_coff_one_seg_width = (P_mode == 3) , P_coff_one_seg_sym = (P_mode-1) */
+		if (state->mode == 3)
+			dib8000_write_word(state, 180, 0x1fcf | ((state->mode - 1) << 14));
+		else
+			dib8000_write_word(state, 180, 0x0fcf | ((state->mode - 1) << 14));
 
-		// P_ctrl_corm_thres4pre_freq_inh=1, P_ctrl_pre_freq_mode_sat=1,
-		// P_ctrl_pre_freq_mode_sat=1, P_ctrl_pre_freq_inh=0, P_ctrl_pre_freq_step = 3, P_pre_freq_win_len=1
-		dib8000_write_word(state, 338, (1 << 12) | (1 << 10) | (0 << 9) | (3 << 5) | 1);
+		/* P_ctrl_corm_thres4pre_freq_inh=1,P_ctrl_pre_freq_mode_sat=1, P_ctrl_pre_freq_inh=0, P_ctrl_pre_freq_step = 5, P_pre_freq_win_len=4 */
+		dib8000_write_word(state, 338, (1 << 12) | (1 << 10) | (0 << 9) | (5 << 5) | 4);
+		coff = &coff_thres_1seg[0];
+	} else {   /* Sound Broadcasting mode 3 seg */
+		dib8000_write_word(state, 180, 0x1fcf | (1 << 14));
+		/* P_ctrl_corm_thres4pre_freq_inh = 1, P_ctrl_pre_freq_mode_sat=1, P_ctrl_pre_freq_inh=0, P_ctrl_pre_freq_step = 4, P_pre_freq_win_len=4 */
+		dib8000_write_word(state, 338, (1 << 12) | (1 << 10) | (0 << 9) | (4 << 5) | 4);
+		coff = &coff_thres_3seg[0];
+	}
 
-		// P_ctrl_pre_freq_win_len=8, P_ctrl_pre_freq_thres_lockin=6
-		dib8000_write_word(state, 340, (8 << 6) | (6 << 0));
-		// P_ctrl_pre_freq_thres_lockout=4, P_small_use_tmcc/ac/cp=1
-		dib8000_write_word(state, 341, (4 << 3) | (1 << 2) | (1 << 1) | (1 << 0));
+	dib8000_write_word(state, 228, 1); /* P_2d_mode_byp=1 */
+	dib8000_write_word(state, 205, dib8000_read_word(state, 205) & 0xfff0); /* P_cspu_win_cut = 0 */
+
+	if (c->isdbt_partial_reception == 0 && c->transmission_mode == TRANSMISSION_MODE_2K)
+		dib8000_write_word(state, 265, 15); /* P_equal_noise_sel = 15 */
+
+	/* Write COFF thres */
+	for (i = 0 ; i < 3; i++) {
+		dib8000_write_word(state, 181+i, coff[i]);
+		dib8000_write_word(state, 184+i, coff[i]);
 	}
-	// ---- FFT ----
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1 && state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0)
-		dib8000_write_word(state, 178, 64);	// P_fft_powrange=64
-	else
-		dib8000_write_word(state, 178, 32);	// P_fft_powrange=32
 
-	/* make the cpil_coff_lock more robust but slower p_coff_winlen
+	/*
+	 * make the cpil_coff_lock more robust but slower p_coff_winlen
 	 * 6bits; p_coff_thres_lock 6bits (for coff lock if needed)
 	 */
-	/* if ( ( nbseg_diff>0)&&(nbseg_diff<13))
-		dib8000_write_word(state, 187, (dib8000_read_word(state, 187) & 0xfffb) | (1 << 3)); */
-
-	dib8000_write_word(state, 189, ~seg_mask13 | seg_diff_mask);	/* P_lmod4_seg_inh       */
-	dib8000_write_word(state, 192, ~seg_mask13 | seg_diff_mask);	/* P_pha3_seg_inh        */
-	dib8000_write_word(state, 225, ~seg_mask13 | seg_diff_mask);	/* P_tac_seg_inh         */
-	if ((!state->fe[0]->dtv_property_cache.isdbt_sb_mode) && (state->cfg.pll->ifreq == 0))
-		dib8000_write_word(state, 266, ~seg_mask13 | seg_diff_mask | 0x40);	/* P_equal_noise_seg_inh */
-	else
-		dib8000_write_word(state, 266, ~seg_mask13 | seg_diff_mask);	/* P_equal_noise_seg_inh */
-	dib8000_write_word(state, 287, ~seg_mask13 | 0x1000);	/* P_tmcc_seg_inh        */
-	//dib8000_write_word(state, 288, ~seg_mask13 | seg_diff_mask); /* P_tmcc_seg_eq_inh */
-	if (!autosearching)
-		dib8000_write_word(state, 288, (~seg_mask13 | seg_diff_mask) & 0x1fff);	/* P_tmcc_seg_eq_inh */
+
+	dib8000_write_word(state, 266, ~state->seg_mask | state->seg_diff_mask); /* P_equal_noise_seg_inh */
+
+	if (c->isdbt_partial_reception == 0)
+		dib8000_write_word(state, 178, 64); /* P_fft_powrange = 64 */
 	else
-		dib8000_write_word(state, 288, 0x1fff);	//disable equalisation of the tmcc when autosearch to be able to find the DQPSK channels.
-	dprintk("287 = %X (%d)", ~seg_mask13 | 0x1000, ~seg_mask13 | 0x1000);
-
-	dib8000_write_word(state, 211, seg_mask13 & (~seg_diff_mask));	/* P_des_seg_enabled     */
-
-	/* offset loop parameters */
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
-		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0)
-			/* P_timf_alpha = (11-P_mode), P_corm_alpha=6, P_corm_thres=0x80 */
-			dib8000_write_word(state, 32, ((11 - mode) << 12) | (6 << 8) | 0x40);
-
-		else		// Sound Broadcasting mode 3 seg
-			/* P_timf_alpha = (10-P_mode), P_corm_alpha=6, P_corm_thres=0x80 */
-			dib8000_write_word(state, 32, ((10 - mode) << 12) | (6 << 8) | 0x60);
-	} else
-		// TODO in 13 seg, timf_alpha can always be the same or not ?
-		/* P_timf_alpha = (9-P_mode, P_corm_alpha=6, P_corm_thres=0x80 */
-		dib8000_write_word(state, 32, ((9 - mode) << 12) | (6 << 8) | 0x80);
-
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
-		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0)
-			/* P_ctrl_pha_off_max=3   P_ctrl_sfreq_inh =0  P_ctrl_sfreq_step = (11-P_mode)  */
-			dib8000_write_word(state, 37, (3 << 5) | (0 << 4) | (10 - mode));
-
-		else		// Sound Broadcasting mode 3 seg
-			/* P_ctrl_pha_off_max=3   P_ctrl_sfreq_inh =0  P_ctrl_sfreq_step = (10-P_mode)  */
-			dib8000_write_word(state, 37, (3 << 5) | (0 << 4) | (9 - mode));
-	} else
-		/* P_ctrl_pha_off_max=3   P_ctrl_sfreq_inh =0  P_ctrl_sfreq_step = 9  */
-		dib8000_write_word(state, 37, (3 << 5) | (0 << 4) | (8 - mode));
+		dib8000_write_word(state, 178, 32); /* P_fft_powrange = 32 */
+}
 
-	/* P_dvsy_sync_wait - reuse mode */
-	switch (state->fe[0]->dtv_property_cache.transmission_mode) {
-	case TRANSMISSION_MODE_8K:
-		mode = 256;
-		break;
-	case TRANSMISSION_MODE_4K:
-		mode = 128;
-		break;
-	default:
-	case TRANSMISSION_MODE_2K:
-		mode = 64;
-		break;
+static void dib8000_set_isdbt_common_channel(struct dib8000_state *state, u8 seq, u8 autosearching)
+{
+	u16 p_cfr_left_edge  = 0, p_cfr_right_edge = 0;
+	u16 tmcc_pow = 0, ana_gain = 0, tmp = 0, i = 0, nbseg_diff = 0 ;
+	u16 max_constellation = DQPSK;
+	int init_prbs;
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+
+	/* P_mode */
+	dib8000_write_word(state, 10, (seq << 4));
+
+	/* init mode */
+	state->mode = fft_to_mode(state);
+
+	/* set guard */
+	tmp = dib8000_read_word(state, 1);
+	dib8000_write_word(state, 1, (tmp&0xfffc) | (c->guard_interval & 0x3));
+
+	dib8000_write_word(state, 274, (dib8000_read_word(state, 274) & 0xffcf) | ((c->isdbt_partial_reception & 1) << 5) | ((c->isdbt_sb_mode & 1) << 4));
+
+	/* signal optimization parameter */
+	if (c->isdbt_partial_reception) {
+		state->seg_diff_mask = (c->layer[0].modulation == DQPSK) << permu_seg[0];
+		for (i = 1; i < 3; i++)
+			nbseg_diff += (c->layer[i].modulation == DQPSK) * c->layer[i].segment_count;
+		for (i = 0; i < nbseg_diff; i++)
+			state->seg_diff_mask |= 1 << permu_seg[i+1];
+	} else {
+		for (i = 0; i < 3; i++)
+			nbseg_diff += (c->layer[i].modulation == DQPSK) * c->layer[i].segment_count;
+		for (i = 0; i < nbseg_diff; i++)
+			state->seg_diff_mask |= 1 << permu_seg[i];
 	}
-	if (state->cfg.diversity_delay == 0)
-		mode = (mode * (1 << (guard)) * 3) / 2 + 48;	// add 50% SFN margin + compensate for one DVSY-fifo
+
+	if (state->seg_diff_mask)
+		dib8000_write_word(state, 268, (dib8000_read_word(state, 268) & 0xF9FF) | 0x0200);
 	else
-		mode = (mode * (1 << (guard)) * 3) / 2 + state->cfg.diversity_delay;	// add 50% SFN margin + compensate for DVSY-fifo
-	mode <<= 4;
-	dib8000_write_word(state, 273, (dib8000_read_word(state, 273) & 0x000f) | mode);
+		dib8000_write_word(state, 268, (2 << 9) | 39); /*init value */
 
-	/* channel estimation fine configuration */
-	switch (max_constellation) {
-	case QAM_64:
-		ana_gain = 0x7;	// -1 : avoid def_est saturation when ADC target is -16dB
-		coeff[0] = 0x0148;	/* P_adp_regul_cnt 0.04 */
-		coeff[1] = 0xfff0;	/* P_adp_noise_cnt -0.002 */
-		coeff[2] = 0x00a4;	/* P_adp_regul_ext 0.02 */
-		coeff[3] = 0xfff8;	/* P_adp_noise_ext -0.001 */
-		//if (!state->cfg.hostbus_diversity) //if diversity, we should prehaps use the configuration of the max_constallation -1
-		break;
-	case QAM_16:
-		ana_gain = 0x7;	// -1 : avoid def_est saturation when ADC target is -16dB
-		coeff[0] = 0x023d;	/* P_adp_regul_cnt 0.07 */
-		coeff[1] = 0xffdf;	/* P_adp_noise_cnt -0.004 */
-		coeff[2] = 0x00a4;	/* P_adp_regul_ext 0.02 */
-		coeff[3] = 0xfff0;	/* P_adp_noise_ext -0.002 */
-		//if (!((state->cfg.hostbus_diversity) && (max_constellation == QAM_16)))
-		break;
-	default:
-		ana_gain = 0;	// 0 : goes along with ADC target at -22dB to keep good mobile performance and lock at sensitivity level
-		coeff[0] = 0x099a;	/* P_adp_regul_cnt 0.3 */
-		coeff[1] = 0xffae;	/* P_adp_noise_cnt -0.01 */
-		coeff[2] = 0x0333;	/* P_adp_regul_ext 0.1 */
-		coeff[3] = 0xfff8;	/* P_adp_noise_ext -0.002 */
-		break;
+	for (i = 0; i < 3; i++)
+		max_constellation = dib8000_set_layer(state, i, max_constellation);
+	if (autosearching == 0) {
+		state->layer_b_nb_seg = c->layer[1].segment_count;
+		state->layer_c_nb_seg = c->layer[2].segment_count;
 	}
-	for (mode = 0; mode < 4; mode++)
-		dib8000_write_word(state, 215 + mode, coeff[mode]);
 
-	// update ana_gain depending on max constellation
-	dib8000_write_word(state, 116, ana_gain);
-	// update ADC target depending on ana_gain
-	if (ana_gain) {		// set -16dB ADC target for ana_gain=-1
-		for (i = 0; i < 10; i++)
-			dib8000_write_word(state, 80 + i, adc_target_16dB[i]);
-	} else {		// set -22dB ADC target for ana_gain=0
-		for (i = 0; i < 10; i++)
-			dib8000_write_word(state, 80 + i, adc_target_16dB[i] - 355);
+	/* WRITE: Mode & Diff mask */
+	dib8000_write_word(state, 0, (state->mode << 13) | state->seg_diff_mask);
+
+	state->differential_constellation = (state->seg_diff_mask != 0);
+
+	/* channel estimation fine configuration */
+	ana_gain = dib8000_adp_fine_tune(state, max_constellation);
+
+	/* update ana_gain depending on max constellation */
+	dib8000_update_ana_gain(state, ana_gain);
+
+	/* ---- ANA_FE ---- */
+	if (c->isdbt_partial_reception) /* 3-segments */
+		dib8000_load_ana_fe_coefs(state, ana_fe_coeff_3seg);
+	else
+		dib8000_load_ana_fe_coefs(state, ana_fe_coeff_1seg); /* 1-segment */
+
+	/* TSB or ISDBT ? apply it now */
+	if (c->isdbt_sb_mode) {
+		dib8000_set_sb_channel(state);
+		if (c->isdbt_sb_subchannel < 14)
+			init_prbs = dib8000_get_init_prbs(state, c->isdbt_sb_subchannel);
+		else
+			init_prbs = 0;
+	} else {
+		dib8000_set_13seg_channel(state);
+		init_prbs = 0xfff;
 	}
 
-	// ---- ANA_FE ----
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode) {
-		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 1)
-			ana_fe = ana_fe_coeff_3seg;
-		else		// 1-segment
-			ana_fe = ana_fe_coeff_1seg;
-	} else
-		ana_fe = ana_fe_coeff_13seg;
+	/* SMALL */
+	dib8000_small_fine_tune(state);
 
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1 || state->isdbt_cfg_loaded == 0)
-		for (mode = 0; mode < 24; mode++)
-			dib8000_write_word(state, 117 + mode, ana_fe[mode]);
+	dib8000_set_subchannel_prbs(state, init_prbs);
 
-	// ---- CHAN_BLK ----
+	/* ---- CHAN_BLK ---- */
 	for (i = 0; i < 13; i++) {
-		if ((((~seg_diff_mask) >> i) & 1) == 1) {
-			P_cfr_left_edge += (1 << i) * ((i == 0) || ((((seg_mask13 & (~seg_diff_mask)) >> (i - 1)) & 1) == 0));
-			P_cfr_right_edge += (1 << i) * ((i == 12) || ((((seg_mask13 & (~seg_diff_mask)) >> (i + 1)) & 1) == 0));
+		if ((((~state->seg_diff_mask) >> i) & 1) == 1) {
+			p_cfr_left_edge  += (1 << i) * ((i == 0) || ((((state->seg_mask & (~state->seg_diff_mask)) >> (i - 1)) & 1) == 0));
+			p_cfr_right_edge += (1 << i) * ((i == 12) || ((((state->seg_mask & (~state->seg_diff_mask)) >> (i + 1)) & 1) == 0));
 		}
 	}
-	dib8000_write_word(state, 222, P_cfr_left_edge);	// P_cfr_left_edge
-	dib8000_write_word(state, 223, P_cfr_right_edge);	// P_cfr_right_edge
-	// "P_cspu_left_edge"  not used => do not care
-	// "P_cspu_right_edge" not used => do not care
-
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
-		dib8000_write_word(state, 228, 1);	// P_2d_mode_byp=1
-		dib8000_write_word(state, 205, dib8000_read_word(state, 205) & 0xfff0);	// P_cspu_win_cut = 0
-		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0
-			&& state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_2K) {
-			//dib8000_write_word(state, 219, dib8000_read_word(state, 219) & 0xfffe); // P_adp_pass = 0
-			dib8000_write_word(state, 265, 15);	// P_equal_noise_sel = 15
-		}
-	} else if (state->isdbt_cfg_loaded == 0) {
-		dib8000_write_word(state, 228, 0);	// default value
-		dib8000_write_word(state, 265, 31);	// default value
-		dib8000_write_word(state, 205, 0x200f);	// init value
-	}
-	// ---- TMCC ----
+	dib8000_write_word(state, 222, p_cfr_left_edge); /* p_cfr_left_edge */
+	dib8000_write_word(state, 223, p_cfr_right_edge); /* p_cfr_right_edge */
+	/* "P_cspu_left_edge" & "P_cspu_right_edge" not used => do not care */
+
+	dib8000_write_word(state, 189, ~state->seg_mask | state->seg_diff_mask); /* P_lmod4_seg_inh */
+	dib8000_write_word(state, 192, ~state->seg_mask | state->seg_diff_mask); /* P_pha3_seg_inh */
+	dib8000_write_word(state, 225, ~state->seg_mask | state->seg_diff_mask); /* P_tac_seg_inh */
+
+	if (!autosearching)
+		dib8000_write_word(state, 288, (~state->seg_mask | state->seg_diff_mask) & 0x1fff); /* P_tmcc_seg_eq_inh */
+	else
+		dib8000_write_word(state, 288, 0x1fff); /*disable equalisation of the tmcc when autosearch to be able to find the DQPSK channels. */
+
+	dib8000_write_word(state, 211, state->seg_mask & (~state->seg_diff_mask)); /* P_des_seg_enabled */
+	dib8000_write_word(state, 287, ~state->seg_mask | 0x1000); /* P_tmcc_seg_inh */
+
+	dib8000_write_word(state, 178, 32); /* P_fft_powrange = 32 */
+
+	/* ---- TMCC ---- */
 	for (i = 0; i < 3; i++)
-		tmcc_pow +=
-			(((state->fe[0]->dtv_property_cache.layer[i].modulation == DQPSK) * 4 + 1) * state->fe[0]->dtv_property_cache.layer[i].segment_count);
-	// Quantif of "P_tmcc_dec_thres_?k" is (0, 5+mode, 9);
-	// Threshold is set at 1/4 of max power.
-	tmcc_pow *= (1 << (9 - 2));
-
-	dib8000_write_word(state, 290, tmcc_pow);	// P_tmcc_dec_thres_2k
-	dib8000_write_word(state, 291, tmcc_pow);	// P_tmcc_dec_thres_4k
-	dib8000_write_word(state, 292, tmcc_pow);	// P_tmcc_dec_thres_8k
-	//dib8000_write_word(state, 287, (1 << 13) | 0x1000 );
-	// ---- PHA3 ----
+		tmcc_pow += (((c->layer[i].modulation == DQPSK) * 4 + 1) * c->layer[i].segment_count) ;
+
+	/* Quantif of "P_tmcc_dec_thres_?k" is (0, 5+mode, 9); */
+	/* Threshold is set at 1/4 of max power. */
+	tmcc_pow *= (1 << (9-2));
+	dib8000_write_word(state, 290, tmcc_pow); /* P_tmcc_dec_thres_2k */
+	dib8000_write_word(state, 291, tmcc_pow); /* P_tmcc_dec_thres_4k */
+	dib8000_write_word(state, 292, tmcc_pow); /* P_tmcc_dec_thres_8k */
+	/*dib8000_write_word(state, 287, (1 << 13) | 0x1000 ); */
 
+	/* ---- PHA3 ---- */
 	if (state->isdbt_cfg_loaded == 0)
-		dib8000_write_word(state, 250, 3285);	/*p_2d_hspeed_thr0 */
+		dib8000_write_word(state, 250, 3285); /* p_2d_hspeed_thr0 */
 
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1)
-		state->isdbt_cfg_loaded = 0;
-	else
-		state->isdbt_cfg_loaded = 1;
+	state->isdbt_cfg_loaded = 0;
+}
 
+static u32 dib8000_wait_lock(struct dib8000_state *state, u32 internal,
+			     u32 wait0_ms, u32 wait1_ms, u32 wait2_ms)
+{
+	u32 value = 0;	/* P_search_end0 wait time */
+	u16 reg = 11;	/* P_search_end0 start addr */
+
+	for (reg = 11; reg < 16; reg += 2) {
+		if (reg == 11) {
+			if (state->revision == 0x8090)
+				value = internal * wait1_ms;
+			else
+				value = internal * wait0_ms;
+		} else if (reg == 13)
+			value = internal * wait1_ms;
+		else if (reg == 15)
+			value = internal * wait2_ms;
+		dib8000_write_word(state, reg, (u16)((value >> 16) & 0xffff));
+		dib8000_write_word(state, (reg + 1), (u16)(value & 0xffff));
+	}
+	return value;
 }
 
 static int dib8000_autosearch_start(struct dvb_frontend *fe)
 {
-	u8 factor;
-	u32 value;
 	struct dib8000_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	u8 slist = 0;
+	u32 value, internal = state->cfg.pll->internal;
 
-	int slist = 0;
-
-	state->fe[0]->dtv_property_cache.inversion = 0;
-	if (!state->fe[0]->dtv_property_cache.isdbt_sb_mode)
-		state->fe[0]->dtv_property_cache.layer[0].segment_count = 13;
-	state->fe[0]->dtv_property_cache.layer[0].modulation = QAM_64;
-	state->fe[0]->dtv_property_cache.layer[0].fec = FEC_2_3;
-	state->fe[0]->dtv_property_cache.layer[0].interleaving = 0;
-
-	//choose the right list, in sb, always do everything
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode) {
-		state->fe[0]->dtv_property_cache.transmission_mode = TRANSMISSION_MODE_8K;
-		state->fe[0]->dtv_property_cache.guard_interval = GUARD_INTERVAL_1_8;
-		slist = 7;
-		dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));
-	} else {
-		if (state->fe[0]->dtv_property_cache.guard_interval == GUARD_INTERVAL_AUTO) {
-			if (state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO) {
-				slist = 7;
-				dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));	// P_mode = 1 to have autosearch start ok with mode2
-			} else
-				slist = 3;
-		} else {
-			if (state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO) {
-				slist = 2;
-				dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));	// P_mode = 1
-			} else
-				slist = 0;
-		}
+	if (state->revision == 0x8090)
+		internal = dib8000_read32(state, 23) / 1000;
+
+	if (state->autosearch_state == AS_SEARCHING_FFT) {
+		dib8000_write_word(state,  37, 0x0065); /* P_ctrl_pha_off_max default values */
+		dib8000_write_word(state, 116, 0x0000); /* P_ana_gain to 0 */
 
-		if (state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO)
-			state->fe[0]->dtv_property_cache.transmission_mode = TRANSMISSION_MODE_8K;
-		if (state->fe[0]->dtv_property_cache.guard_interval == GUARD_INTERVAL_AUTO)
-			state->fe[0]->dtv_property_cache.guard_interval = GUARD_INTERVAL_1_8;
+		dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x1fff) | (0 << 13) | (1 << 15)); /* P_mode = 0, P_restart_search=1 */
+		dib8000_write_word(state, 1, (dib8000_read_word(state, 1) & 0xfffc) | 0); /* P_guard = 0 */
+		dib8000_write_word(state, 6, 0); /* P_lock0_mask = 0 */
+		dib8000_write_word(state, 7, 0); /* P_lock1_mask = 0 */
+		dib8000_write_word(state, 8, 0); /* P_lock2_mask = 0 */
+		dib8000_write_word(state, 10, (dib8000_read_word(state, 10) & 0x200) | (16 << 4) | (0 << 0)); /* P_search_list=16, P_search_maxtrial=0 */
 
-		dprintk("using list for autosearch : %d", slist);
-		dib8000_set_channel(state, (unsigned char)slist, 1);
-		//dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));  // P_mode = 1
+		if (state->revision == 0x8090)
+			value = dib8000_wait_lock(state, internal, 10, 10, 10); /* time in ms configure P_search_end0 P_search_end1 P_search_end2 */
+		else
+			value = dib8000_wait_lock(state, internal, 20, 20, 20); /* time in ms configure P_search_end0 P_search_end1 P_search_end2 */
 
-		factor = 1;
+		dib8000_write_word(state, 17, 0);
+		dib8000_write_word(state, 18, 200); /* P_search_rstst = 200 */
+		dib8000_write_word(state, 19, 0);
+		dib8000_write_word(state, 20, 400); /* P_search_rstend = 400 */
+		dib8000_write_word(state, 21, (value >> 16) & 0xffff); /* P_search_checkst */
+		dib8000_write_word(state, 22, value & 0xffff);
 
-		//set lock_mask values
+		if (state->revision == 0x8090)
+			dib8000_write_word(state, 32, (dib8000_read_word(state, 32) & 0xf0ff) | (0 << 8)); /* P_corm_alpha = 0 */
+		else
+			dib8000_write_word(state, 32, (dib8000_read_word(state, 32) & 0xf0ff) | (9 << 8)); /* P_corm_alpha = 3 */
+		dib8000_write_word(state, 355, 2); /* P_search_param_max = 2 */
+
+		/* P_search_param_select = (1 | 1<<4 | 1 << 8) */
+		dib8000_write_word(state, 356, 0);
+		dib8000_write_word(state, 357, 0x111);
+
+		dib8000_write_word(state, 770, (dib8000_read_word(state, 770) & 0xdfff) | (1 << 13)); /* P_restart_ccg = 1 */
+		dib8000_write_word(state, 770, (dib8000_read_word(state, 770) & 0xdfff) | (0 << 13)); /* P_restart_ccg = 0 */
+		dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x7ff) | (0 << 15) | (1 << 13)); /* P_restart_search = 0; */
+	} else if (state->autosearch_state == AS_SEARCHING_GUARD) {
+		c->transmission_mode = TRANSMISSION_MODE_8K;
+		c->guard_interval = GUARD_INTERVAL_1_8;
+		c->inversion = 0;
+		c->layer[0].modulation = QAM_64;
+		c->layer[0].fec = FEC_2_3;
+		c->layer[0].interleaving = 0;
+		c->layer[0].segment_count = 13;
+
+		slist = 16;
+		c->transmission_mode = state->found_nfft;
+
+		dib8000_set_isdbt_common_channel(state, slist, 1);
+
+		/* set lock_mask values */
 		dib8000_write_word(state, 6, 0x4);
-		dib8000_write_word(state, 7, 0x8);
+		if (state->revision == 0x8090)
+			dib8000_write_word(state, 7, ((1 << 12) | (1 << 11) | (1 << 10)));/* tmcc_dec_lock, tmcc_sync_lock, tmcc_data_lock, tmcc_bch_uncor */
+		else
+			dib8000_write_word(state, 7, 0x8);
 		dib8000_write_word(state, 8, 0x1000);
 
-		//set lock_mask wait time values
-		value = 50 * state->cfg.pll->internal * factor;
-		dib8000_write_word(state, 11, (u16) ((value >> 16) & 0xffff));	// lock0 wait time
-		dib8000_write_word(state, 12, (u16) (value & 0xffff));	// lock0 wait time
-		value = 100 * state->cfg.pll->internal * factor;
-		dib8000_write_word(state, 13, (u16) ((value >> 16) & 0xffff));	// lock1 wait time
-		dib8000_write_word(state, 14, (u16) (value & 0xffff));	// lock1 wait time
-		value = 1000 * state->cfg.pll->internal * factor;
-		dib8000_write_word(state, 15, (u16) ((value >> 16) & 0xffff));	// lock2 wait time
-		dib8000_write_word(state, 16, (u16) (value & 0xffff));	// lock2 wait time
+		/* set lock_mask wait time values */
+		if (state->revision == 0x8090)
+			dib8000_wait_lock(state, internal, 50, 100, 1000); /* time in ms configure P_search_end0 P_search_end1 P_search_end2 */
+		else
+			dib8000_wait_lock(state, internal, 50, 200, 1000); /* time in ms configure P_search_end0 P_search_end1 P_search_end2 */
+
+		dib8000_write_word(state, 355, 3); /* P_search_param_max = 3 */
+
+		/* P_search_param_select = 0xf; look for the 4 different guard intervals */
+		dib8000_write_word(state, 356, 0);
+		dib8000_write_word(state, 357, 0xf);
 
 		value = dib8000_read_word(state, 0);
-		dib8000_write_word(state, 0, (u16) ((1 << 15) | value));
-		dib8000_read_word(state, 1284);	// reset the INT. n_irq_pending
-		dib8000_write_word(state, 0, (u16) value);
+		dib8000_write_word(state, 0, (u16)((1 << 15) | value));
+		dib8000_read_word(state, 1284);  /* reset the INT. n_irq_pending */
+		dib8000_write_word(state, 0, (u16)value);
+	} else {
+		c->inversion = 0;
+		c->layer[0].modulation = QAM_64;
+		c->layer[0].fec = FEC_2_3;
+		c->layer[0].interleaving = 0;
+		c->layer[0].segment_count = 13;
+		if (!c->isdbt_sb_mode)
+			c->layer[0].segment_count = 13;
+
+		/* choose the right list, in sb, always do everything */
+		if (c->isdbt_sb_mode) {
+			slist = 7;
+			dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));
+		} else {
+			if (c->guard_interval == GUARD_INTERVAL_AUTO) {
+				if (c->transmission_mode == TRANSMISSION_MODE_AUTO) {
+					c->transmission_mode = TRANSMISSION_MODE_8K;
+					c->guard_interval = GUARD_INTERVAL_1_8;
+					slist = 7;
+					dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));  /* P_mode = 1 to have autosearch start ok with mode2 */
+				} else {
+					c->guard_interval = GUARD_INTERVAL_1_8;
+					slist = 3;
+				}
+			} else {
+				if (c->transmission_mode == TRANSMISSION_MODE_AUTO) {
+					c->transmission_mode = TRANSMISSION_MODE_8K;
+					slist = 2;
+					dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));  /* P_mode = 1 */
+				} else
+					slist = 0;
+			}
+		}
+		dprintk("Using list for autosearch : %d", slist);
 
-	}
+		dib8000_set_isdbt_common_channel(state, slist, 1);
+
+		/* set lock_mask values */
+		dib8000_write_word(state, 6, 0x4);
+		if (state->revision == 0x8090)
+			dib8000_write_word(state, 7, (1 << 12) | (1 << 11) | (1 << 10));
+		else
+			dib8000_write_word(state, 7, 0x8);
+		dib8000_write_word(state, 8, 0x1000);
 
+		/* set lock_mask wait time values */
+		if (state->revision == 0x8090)
+			dib8000_wait_lock(state, internal, 50, 200, 1000); /* time in ms configure P_search_end0 P_search_end1 P_search_end2 */
+		else
+			dib8000_wait_lock(state, internal, 50, 100, 1000); /* time in ms configure P_search_end0 P_search_end1 P_search_end2 */
+
+		value = dib8000_read_word(state, 0);
+		dib8000_write_word(state, 0, (u16)((1 << 15) | value));
+		dib8000_read_word(state, 1284);  /* reset the INT. n_irq_pending */
+		dib8000_write_word(state, 0, (u16)value);
+	}
 	return 0;
 }
 
@@ -2663,96 +2583,635 @@ static int dib8000_autosearch_irq(struct dvb_frontend *fe)
 	struct dib8000_state *state = fe->demodulator_priv;
 	u16 irq_pending = dib8000_read_word(state, 1284);
 
-	if (irq_pending & 0x1) {	// failed
-		dprintk("dib8000_autosearch_irq failed");
-		return 1;
-	}
+	if (state->autosearch_state == AS_SEARCHING_FFT) {
+		if (irq_pending & 0x1) {
+			dprintk("dib8000_autosearch_irq: max correlation result available");
+			return 3;
+		}
+	} else {
+		if (irq_pending & 0x1) {	/* failed */
+			dprintk("dib8000_autosearch_irq failed");
+			return 1;
+		}
 
-	if (irq_pending & 0x2) {	// succeeded
-		dprintk("dib8000_autosearch_irq succeeded");
-		return 2;
+		if (irq_pending & 0x2) {	/* succeeded */
+			dprintk("dib8000_autosearch_irq succeeded");
+			return 2;
+		}
 	}
 
 	return 0;		// still pending
 }
 
-static int dib8000_tune(struct dvb_frontend *fe)
+static void dib8000_viterbi_state(struct dib8000_state *state, u8 onoff)
 {
-	struct dib8000_state *state = fe->demodulator_priv;
-	int ret = 0;
-	u16 lock, value, mode;
+	u16 tmp;
 
-	// we are already tuned - just resuming from suspend
-	if (state == NULL)
-		return -EINVAL;
+	tmp = dib8000_read_word(state, 771);
+	if (onoff) /* start P_restart_chd : channel_decoder */
+		dib8000_write_word(state, 771, tmp & 0xfffd);
+	else /* stop P_restart_chd : channel_decoder */
+		dib8000_write_word(state, 771, tmp | (1<<1));
+}
+
+static void dib8000_set_dds(struct dib8000_state *state, s32 offset_khz)
+{
+	s16 unit_khz_dds_val;
+	u32 abs_offset_khz = ABS(offset_khz);
+	u32 dds = state->cfg.pll->ifreq & 0x1ffffff;
+	u8 invert = !!(state->cfg.pll->ifreq & (1 << 25));
+	u8 ratio;
 
-	mode = fft_to_mode(state);
+	if (state->revision == 0x8090) {
+		ratio = 4;
+		unit_khz_dds_val = (1<<26) / (dib8000_read32(state, 23) / 1000);
+		if (offset_khz < 0)
+			dds = (1 << 26) - (abs_offset_khz * unit_khz_dds_val);
+		else
+			dds = (abs_offset_khz * unit_khz_dds_val);
 
-	dib8000_set_bandwidth(fe, state->fe[0]->dtv_property_cache.bandwidth_hz / 1000);
-	dib8000_set_channel(state, 0, 0);
+		if (invert)
+			dds = (1<<26) - dds;
+	} else {
+		ratio = 2;
+		unit_khz_dds_val = (u16) (67108864 / state->cfg.pll->internal);
 
-	// restart demod
-	ret |= dib8000_write_word(state, 770, 0x4000);
-	ret |= dib8000_write_word(state, 770, 0x0000);
-	msleep(45);
+		if (offset_khz < 0)
+			unit_khz_dds_val *= -1;
 
-	/* P_ctrl_inh_cor=0, P_ctrl_alpha_cor=4, P_ctrl_inh_isi=0, P_ctrl_alpha_isi=3 */
-	/*  ret |= dib8000_write_word(state, 29, (0 << 9) | (4 << 5) | (0 << 4) | (3 << 0) );  workaround inh_isi stays at 1 */
+		/* IF tuner */
+		if (invert)
+			dds -= abs_offset_khz * unit_khz_dds_val;
+		else
+			dds += abs_offset_khz * unit_khz_dds_val;
+	}
 
-	// never achieved a lock before - wait for timfreq to update
-	if (state->timf == 0) {
-		if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
-			if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0)
-				msleep(300);
-			else	// Sound Broadcasting mode 3 seg
-				msleep(500);
-		} else		// 13 seg
-			msleep(200);
+	dprintk("setting a DDS frequency offset of %c%dkHz", invert ? '-' : ' ', dds / unit_khz_dds_val);
+
+	if (abs_offset_khz <= (state->cfg.pll->internal / ratio)) {
+		/* Max dds offset is the half of the demod freq */
+		dib8000_write_word(state, 26, invert);
+		dib8000_write_word(state, 27, (u16)(dds >> 16) & 0x1ff);
+		dib8000_write_word(state, 28, (u16)(dds & 0xffff));
 	}
-	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
-		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0) {
+}
 
-			/* P_timf_alpha = (13-P_mode) , P_corm_alpha=6, P_corm_thres=0x40  alpha to check on board */
-			dib8000_write_word(state, 32, ((13 - mode) << 12) | (6 << 8) | 0x40);
-			//dib8000_write_word(state, 32, (8 << 12) | (6 << 8) | 0x80);
+static void dib8000_set_frequency_offset(struct dib8000_state *state)
+{
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	int i;
+	u32 current_rf;
+	int total_dds_offset_khz;
 
-			/*  P_ctrl_sfreq_step= (12-P_mode)   P_ctrl_sfreq_inh =0     P_ctrl_pha_off_max  */
-			ret |= dib8000_write_word(state, 37, (12 - mode) | ((5 + mode) << 5));
+	if (state->fe[0]->ops.tuner_ops.get_frequency)
+		state->fe[0]->ops.tuner_ops.get_frequency(state->fe[0], &current_rf);
+	else
+		current_rf = c->frequency;
+	current_rf /= 1000;
+	total_dds_offset_khz = (int)current_rf - (int)c->frequency / 1000;
 
-		} else {	// Sound Broadcasting mode 3 seg
+	if (c->isdbt_sb_mode) {
+		state->subchannel = c->isdbt_sb_subchannel;
 
-			/* P_timf_alpha = (12-P_mode) , P_corm_alpha=6, P_corm_thres=0x60  alpha to check on board */
-			dib8000_write_word(state, 32, ((12 - mode) << 12) | (6 << 8) | 0x60);
+		i = dib8000_read_word(state, 26) & 1; /* P_dds_invspec */
+		dib8000_write_word(state, 26, c->inversion ^ i);
 
-			ret |= dib8000_write_word(state, 37, (11 - mode) | ((5 + mode) << 5));
+		if (state->cfg.pll->ifreq == 0) { /* low if tuner */
+			if ((c->inversion ^ i) == 0)
+				dib8000_write_word(state, 26, dib8000_read_word(state, 26) | 1);
+		} else {
+			if ((c->inversion ^ i) == 0)
+				total_dds_offset_khz *= -1;
 		}
+	}
 
-	} else {		// 13 seg
-		/* P_timf_alpha = 8 , P_corm_alpha=6, P_corm_thres=0x80  alpha to check on board */
-		dib8000_write_word(state, 32, ((11 - mode) << 12) | (6 << 8) | 0x80);
+	dprintk("%dkhz tuner offset (frequency = %dHz & current_rf = %dHz) total_dds_offset_hz = %d", c->frequency - current_rf, c->frequency, current_rf, total_dds_offset_khz);
 
-		ret |= dib8000_write_word(state, 37, (10 - mode) | ((5 + mode) << 5));
+	/* apply dds offset now */
+	dib8000_set_dds(state, total_dds_offset_khz);
+}
+
+static u16 LUT_isdbt_symbol_duration[4] = { 26, 101, 63 };
+
+static u32 dib8000_get_symbol_duration(struct dib8000_state *state)
+{
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	u16 i;
 
+	switch (c->transmission_mode) {
+	case TRANSMISSION_MODE_2K:
+			i = 0;
+			break;
+	case TRANSMISSION_MODE_4K:
+			i = 2;
+			break;
+	default:
+	case TRANSMISSION_MODE_AUTO:
+	case TRANSMISSION_MODE_8K:
+			i = 1;
+			break;
 	}
 
-	// we achieved a coff_cpil_lock - it's time to update the timf
-	if (state->revision != 0x8090)
-		lock = dib8000_read_word(state, 568);
+	return (LUT_isdbt_symbol_duration[i] / (c->bandwidth_hz / 1000)) + 1;
+}
+
+static void dib8000_set_isdbt_loop_params(struct dib8000_state *state, enum param_loop_step loop_step)
+{
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	u16 reg_32 = 0, reg_37 = 0;
+
+	switch (loop_step) {
+	case LOOP_TUNE_1:
+			if (c->isdbt_sb_mode)  {
+				if (c->isdbt_partial_reception == 0) {
+					reg_32 = ((11 - state->mode) << 12) | (6 << 8) | 0x40; /* P_timf_alpha = (11-P_mode), P_corm_alpha=6, P_corm_thres=0x40 */
+					reg_37 = (3 << 5) | (0 << 4) | (10 - state->mode); /* P_ctrl_pha_off_max=3   P_ctrl_sfreq_inh =0  P_ctrl_sfreq_step = (10-P_mode)  */
+				} else { /* Sound Broadcasting mode 3 seg */
+					reg_32 = ((10 - state->mode) << 12) | (6 << 8) | 0x60; /* P_timf_alpha = (10-P_mode), P_corm_alpha=6, P_corm_thres=0x60 */
+					reg_37 = (3 << 5) | (0 << 4) | (9 - state->mode); /* P_ctrl_pha_off_max=3   P_ctrl_sfreq_inh =0  P_ctrl_sfreq_step = (9-P_mode)  */
+				}
+			} else { /* 13-seg start conf offset loop parameters */
+				reg_32 = ((9 - state->mode) << 12) | (6 << 8) | 0x80; /* P_timf_alpha = (9-P_mode, P_corm_alpha=6, P_corm_thres=0x80 */
+				reg_37 = (3 << 5) | (0 << 4) | (8 - state->mode); /* P_ctrl_pha_off_max=3   P_ctrl_sfreq_inh =0  P_ctrl_sfreq_step = 9  */
+			}
+			break;
+	case LOOP_TUNE_2:
+			if (c->isdbt_sb_mode)  {
+				if (c->isdbt_partial_reception == 0) {  /* Sound Broadcasting mode 1 seg */
+					reg_32 = ((13-state->mode) << 12) | (6 << 8) | 0x40; /* P_timf_alpha = (13-P_mode) , P_corm_alpha=6, P_corm_thres=0x40*/
+					reg_37 = (12-state->mode) | ((5 + state->mode) << 5);
+				} else {  /* Sound Broadcasting mode 3 seg */
+					reg_32 = ((12-state->mode) << 12) | (6 << 8) | 0x60; /* P_timf_alpha = (12-P_mode) , P_corm_alpha=6, P_corm_thres=0x60 */
+					reg_37 = (11-state->mode) | ((5 + state->mode) << 5);
+				}
+			} else {  /* 13 seg */
+				reg_32 = ((11-state->mode) << 12) | (6 << 8) | 0x80; /* P_timf_alpha = 8 , P_corm_alpha=6, P_corm_thres=0x80 */
+				reg_37 = ((5+state->mode) << 5) | (10 - state->mode);
+			}
+			break;
+	}
+	dib8000_write_word(state, 32, reg_32);
+	dib8000_write_word(state, 37, reg_37);
+}
+
+static void dib8000_demod_restart(struct dib8000_state *state)
+{
+	dib8000_write_word(state, 770, 0x4000);
+	dib8000_write_word(state, 770, 0x0000);
+	return;
+}
+
+static void dib8000_set_sync_wait(struct dib8000_state *state)
+{
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	u16 sync_wait = 64;
+
+	/* P_dvsy_sync_wait - reuse mode */
+	switch (c->transmission_mode) {
+	case TRANSMISSION_MODE_8K:
+			sync_wait = 256;
+			break;
+	case TRANSMISSION_MODE_4K:
+			sync_wait = 128;
+			break;
+	default:
+	case TRANSMISSION_MODE_2K:
+			sync_wait =  64;
+			break;
+	}
+
+	if (state->cfg.diversity_delay == 0)
+		sync_wait = (sync_wait * (1 << (c->guard_interval)) * 3) / 2 + 48; /* add 50% SFN margin + compensate for one DVSY-fifo */
 	else
-		lock = dib8000_read_word(state, 570);
-	if ((lock >> 11) & 0x1)
-		dib8000_update_timf(state);
+		sync_wait = (sync_wait * (1 << (c->guard_interval)) * 3) / 2 + state->cfg.diversity_delay; /* add 50% SFN margin + compensate for DVSY-fifo */
 
-	//now that tune is finished, lock0 should lock on fec_mpeg to output this lock on MP_LOCK. It's changed in autosearch start
-	dib8000_write_word(state, 6, 0x200);
+	dib8000_write_word(state, 273, (dib8000_read_word(state, 273) & 0x000f) | (sync_wait << 4));
+}
 
-	if (state->revision == 0x8002) {
-		value = dib8000_read_word(state, 903);
-		dib8000_write_word(state, 903, value & ~(1 << 3));
-		msleep(1);
-		dib8000_write_word(state, 903, value | (1 << 3));
+static u32 dib8000_get_timeout(struct dib8000_state *state, u32 delay, enum timeout_mode mode)
+{
+	if (mode == SYMBOL_DEPENDENT_ON)
+		return systime() + (delay * state->symbol_duration);
+	else
+		return systime() + delay;
+}
+
+static s32 dib8000_get_status(struct dvb_frontend *fe)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+	return state->status;
+}
+
+enum frontend_tune_state dib8000_get_tune_state(struct dvb_frontend *fe)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+	return state->tune_state;
+}
+EXPORT_SYMBOL(dib8000_get_tune_state);
+
+int dib8000_set_tune_state(struct dvb_frontend *fe, enum frontend_tune_state tune_state)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+
+	state->tune_state = tune_state;
+	return 0;
+}
+EXPORT_SYMBOL(dib8000_set_tune_state);
+
+static int dib8000_tune_restart_from_demod(struct dvb_frontend *fe)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+
+	state->status = FE_STATUS_TUNE_PENDING;
+	state->tune_state = CT_DEMOD_START;
+	return 0;
+}
+
+static u16 dib8000_read_lock(struct dvb_frontend *fe)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+
+	if (state->revision == 0x8090)
+		return dib8000_read_word(state, 570);
+	return dib8000_read_word(state, 568);
+}
+
+static int dib8090p_init_sdram(struct dib8000_state *state)
+{
+	u16 reg = 0;
+	dprintk("init sdram");
+
+	reg = dib8000_read_word(state, 274) & 0xfff0;
+	dib8000_write_word(state, 274, reg | 0x7); /* P_dintlv_delay_ram = 7 because of MobileSdram */
+
+	dib8000_write_word(state, 1803, (7 << 2));
+
+	reg = dib8000_read_word(state, 1280);
+	dib8000_write_word(state, 1280,  reg | (1 << 2)); /* force restart P_restart_sdram */
+	dib8000_write_word(state, 1280,  reg); /* release restart P_restart_sdram */
+
+	return 0;
+}
+
+static int dib8000_tune(struct dvb_frontend *fe)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	enum frontend_tune_state *tune_state = &state->tune_state;
+
+	u16 locks, deeper_interleaver = 0, i;
+	int ret = 1; /* 1 symbol duration (in 100us unit) delay most of the time */
+
+	u32 *timeout = &state->timeout;
+	u32 now = systime();
+#ifdef DIB8000_AGC_FREEZE
+	u16 agc1, agc2;
+#endif
+
+	u32 corm[4] = {0, 0, 0, 0};
+	u8 find_index, max_value;
+
+#if 0
+	if (*tune_state < CT_DEMOD_STOP)
+		dprintk("IN: context status = %d, TUNE_STATE %d autosearch step = %u systime = %u", state->channel_parameters_set, *tune_state, state->autosearch_state, now);
+#endif
+
+	switch (*tune_state) {
+	case CT_DEMOD_START: /* 30 */
+			if (state->revision == 0x8090)
+				dib8090p_init_sdram(state);
+			state->status = FE_STATUS_TUNE_PENDING;
+			if ((c->delivery_system != SYS_ISDBT) ||
+					(c->inversion == INVERSION_AUTO) ||
+					(c->transmission_mode == TRANSMISSION_MODE_AUTO) ||
+					(c->guard_interval == GUARD_INTERVAL_AUTO) ||
+					(((c->isdbt_layer_enabled & (1 << 0)) != 0) &&
+					 (c->layer[0].segment_count != 0xff) &&
+					 (c->layer[0].segment_count != 0) &&
+					 ((c->layer[0].modulation == QAM_AUTO) ||
+					  (c->layer[0].fec == FEC_AUTO))) ||
+					(((c->isdbt_layer_enabled & (1 << 1)) != 0) &&
+					 (c->layer[1].segment_count != 0xff) &&
+					 (c->layer[1].segment_count != 0) &&
+					 ((c->layer[1].modulation == QAM_AUTO) ||
+					  (c->layer[1].fec == FEC_AUTO))) ||
+					(((c->isdbt_layer_enabled & (1 << 2)) != 0) &&
+					 (c->layer[2].segment_count != 0xff) &&
+					 (c->layer[2].segment_count != 0) &&
+					 ((c->layer[2].modulation == QAM_AUTO) ||
+					  (c->layer[2].fec == FEC_AUTO))) ||
+					(((c->layer[0].segment_count == 0) ||
+					  ((c->isdbt_layer_enabled & (1 << 0)) == 0)) &&
+					 ((c->layer[1].segment_count == 0) ||
+					  ((c->isdbt_layer_enabled & (2 << 0)) == 0)) &&
+					 ((c->layer[2].segment_count == 0) || ((c->isdbt_layer_enabled & (3 << 0)) == 0))))
+				state->channel_parameters_set = 0; /* auto search */
+			else
+				state->channel_parameters_set = 1; /* channel parameters are known */
+
+			dib8000_viterbi_state(state, 0); /* force chan dec in restart */
+
+			/* Layer monit */
+			dib8000_write_word(state, 285, dib8000_read_word(state, 285) & 0x60);
+
+			dib8000_set_frequency_offset(state);
+			dib8000_set_bandwidth(fe, c->bandwidth_hz / 1000);
+
+			if (state->channel_parameters_set == 0) { /* The channel struct is unknown, search it ! */
+#ifdef DIB8000_AGC_FREEZE
+				if (state->revision != 0x8090) {
+					state->agc1_max = dib8000_read_word(state, 108);
+					state->agc1_min = dib8000_read_word(state, 109);
+					state->agc2_max = dib8000_read_word(state, 110);
+					state->agc2_min = dib8000_read_word(state, 111);
+					agc1 = dib8000_read_word(state, 388);
+					agc2 = dib8000_read_word(state, 389);
+					dib8000_write_word(state, 108, agc1);
+					dib8000_write_word(state, 109, agc1);
+					dib8000_write_word(state, 110, agc2);
+					dib8000_write_word(state, 111, agc2);
+				}
+#endif
+				state->autosearch_state = AS_SEARCHING_FFT;
+				state->found_nfft = TRANSMISSION_MODE_AUTO;
+				state->found_guard = GUARD_INTERVAL_AUTO;
+				*tune_state = CT_DEMOD_SEARCH_NEXT;
+			} else { /* we already know the channel struct so TUNE only ! */
+				state->autosearch_state = AS_DONE;
+				*tune_state = CT_DEMOD_STEP_3;
+			}
+			state->symbol_duration = dib8000_get_symbol_duration(state);
+			break;
+
+	case CT_DEMOD_SEARCH_NEXT: /* 51 */
+			dib8000_autosearch_start(fe);
+			if (state->revision == 0x8090)
+				ret = 50;
+			else
+				ret = 15;
+			*tune_state = CT_DEMOD_STEP_1;
+			break;
+
+	case CT_DEMOD_STEP_1: /* 31 */
+			switch (dib8000_autosearch_irq(fe)) {
+			case 1: /* fail */
+					state->status = FE_STATUS_TUNE_FAILED;
+					state->autosearch_state = AS_DONE;
+					*tune_state = CT_DEMOD_STOP; /* else we are done here */
+					break;
+			case 2: /* Succes */
+					state->status = FE_STATUS_FFT_SUCCESS; /* signal to the upper layer, that there was a channel found and the parameters can be read */
+					*tune_state = CT_DEMOD_STEP_3;
+					if (state->autosearch_state == AS_SEARCHING_GUARD)
+						*tune_state = CT_DEMOD_STEP_2;
+					else
+						state->autosearch_state = AS_DONE;
+					break;
+			case 3: /* Autosearch FFT max correlation endded */
+					*tune_state = CT_DEMOD_STEP_2;
+					break;
+			}
+			break;
+
+	case CT_DEMOD_STEP_2:
+			switch (state->autosearch_state) {
+			case AS_SEARCHING_FFT:
+					/* searching for the correct FFT */
+				if (state->revision == 0x8090) {
+					corm[2] = (dib8000_read_word(state, 596) << 16) | (dib8000_read_word(state, 597));
+					corm[1] = (dib8000_read_word(state, 598) << 16) | (dib8000_read_word(state, 599));
+					corm[0] = (dib8000_read_word(state, 600) << 16) | (dib8000_read_word(state, 601));
+				} else {
+					corm[2] = (dib8000_read_word(state, 594) << 16) | (dib8000_read_word(state, 595));
+					corm[1] = (dib8000_read_word(state, 596) << 16) | (dib8000_read_word(state, 597));
+					corm[0] = (dib8000_read_word(state, 598) << 16) | (dib8000_read_word(state, 599));
+				}
+					/* dprintk("corm fft: %u %u %u", corm[0], corm[1], corm[2]); */
+
+					max_value = 0;
+					for (find_index = 1 ; find_index < 3 ; find_index++) {
+						if (corm[max_value] < corm[find_index])
+							max_value = find_index ;
+					}
+
+					switch (max_value) {
+					case 0:
+							state->found_nfft = TRANSMISSION_MODE_2K;
+							break;
+					case 1:
+							state->found_nfft = TRANSMISSION_MODE_4K;
+							break;
+					case 2:
+					default:
+							state->found_nfft = TRANSMISSION_MODE_8K;
+							break;
+					}
+					/* dprintk("Autosearch FFT has found Mode %d", max_value + 1); */
+
+					*tune_state = CT_DEMOD_SEARCH_NEXT;
+					state->autosearch_state = AS_SEARCHING_GUARD;
+					if (state->revision == 0x8090)
+						ret = 50;
+					else
+						ret = 10;
+					break;
+			case AS_SEARCHING_GUARD:
+					/* searching for the correct guard interval */
+					if (state->revision == 0x8090)
+						state->found_guard = dib8000_read_word(state, 572) & 0x3;
+					else
+						state->found_guard = dib8000_read_word(state, 570) & 0x3;
+					/* dprintk("guard interval found=%i", state->found_guard); */
+
+					*tune_state = CT_DEMOD_STEP_3;
+					break;
+			default:
+					/* the demod should never be in this state */
+					state->status = FE_STATUS_TUNE_FAILED;
+					state->autosearch_state = AS_DONE;
+					*tune_state = CT_DEMOD_STOP; /* else we are done here */
+					break;
+			}
+			break;
+
+	case CT_DEMOD_STEP_3: /* 33 */
+			state->symbol_duration = dib8000_get_symbol_duration(state);
+			dib8000_set_isdbt_loop_params(state, LOOP_TUNE_1);
+			dib8000_set_isdbt_common_channel(state, 0, 0);/* setting the known channel parameters here */
+			*tune_state = CT_DEMOD_STEP_4;
+			break;
+
+	case CT_DEMOD_STEP_4: /* (34) */
+			dib8000_demod_restart(state);
+
+			dib8000_set_sync_wait(state);
+			dib8000_set_diversity_in(state->fe[0], state->diversity_onoff);
+
+			locks = (dib8000_read_word(state, 180) >> 6) & 0x3f; /* P_coff_winlen ? */
+			/* coff should lock over P_coff_winlen ofdm symbols : give 3 times this lenght to lock */
+			*timeout = dib8000_get_timeout(state, 2 * locks, SYMBOL_DEPENDENT_ON);
+			*tune_state = CT_DEMOD_STEP_5;
+			break;
+
+	case CT_DEMOD_STEP_5: /* (35) */
+			locks = dib8000_read_lock(fe);
+			if (locks & (0x3 << 11)) { /* coff-lock and off_cpil_lock achieved */
+				dib8000_update_timf(state); /* we achieved a coff_cpil_lock - it's time to update the timf */
+				if (!state->differential_constellation) {
+					/* 2 times lmod4_win_len + 10 symbols (pipe delay after coff + nb to compute a 1st correlation) */
+					*timeout = dib8000_get_timeout(state, (20 * ((dib8000_read_word(state, 188)>>5)&0x1f)), SYMBOL_DEPENDENT_ON);
+					*tune_state = CT_DEMOD_STEP_7;
+				} else {
+					*tune_state = CT_DEMOD_STEP_8;
+				}
+			} else if (now > *timeout) {
+				*tune_state = CT_DEMOD_STEP_6; /* goto check for diversity input connection */
+			}
+			break;
+
+	case CT_DEMOD_STEP_6: /* (36)  if there is an input (diversity) */
+			if ((state->fe[1] != NULL) && (state->output_mode != OUTMODE_DIVERSITY)) {
+				/* if there is a diversity fe in input and this fe is has not already failled : wait here until this this fe has succedeed or failled */
+				if (dib8000_get_status(state->fe[1]) <= FE_STATUS_STD_SUCCESS) /* Something is locked on the input fe */
+					*tune_state = CT_DEMOD_STEP_8; /* go for mpeg */
+				else if (dib8000_get_status(state->fe[1]) >= FE_STATUS_TUNE_TIME_TOO_SHORT) { /* fe in input failled also, break the current one */
+					*tune_state = CT_DEMOD_STOP; /* else we are done here ; step 8 will close the loops and exit */
+					dib8000_viterbi_state(state, 1); /* start viterbi chandec */
+					dib8000_set_isdbt_loop_params(state, LOOP_TUNE_2);
+					state->status = FE_STATUS_TUNE_FAILED;
+				}
+			} else {
+				dib8000_viterbi_state(state, 1); /* start viterbi chandec */
+				dib8000_set_isdbt_loop_params(state, LOOP_TUNE_2);
+				*tune_state = CT_DEMOD_STOP; /* else we are done here ; step 8 will close the loops and exit */
+				state->status = FE_STATUS_TUNE_FAILED;
+			}
+			break;
+
+	case CT_DEMOD_STEP_7: /* 37 */
+			locks = dib8000_read_lock(fe);
+			if (locks & (1<<10)) { /* lmod4_lock */
+				ret = 14; /* wait for 14 symbols */
+				*tune_state = CT_DEMOD_STEP_8;
+			} else if (now > *timeout)
+				*tune_state = CT_DEMOD_STEP_6; /* goto check for diversity input connection */
+			break;
+
+	case CT_DEMOD_STEP_8: /* 38 */
+			dib8000_viterbi_state(state, 1); /* start viterbi chandec */
+			dib8000_set_isdbt_loop_params(state, LOOP_TUNE_2);
+
+			/* mpeg will never lock on this condition because init_prbs is not set : search for it !*/
+			if (c->isdbt_sb_mode
+			    && c->isdbt_sb_subchannel < 14
+			    && !state->differential_constellation) {
+				state->subchannel = 0;
+				*tune_state = CT_DEMOD_STEP_11;
+			} else {
+				*tune_state = CT_DEMOD_STEP_9;
+				state->status = FE_STATUS_LOCKED;
+			}
+			break;
+
+	case CT_DEMOD_STEP_9: /* 39 */
+			if ((state->revision == 0x8090) || ((dib8000_read_word(state, 1291) >> 9) & 0x1)) { /* fe capable of deinterleaving : esram */
+				/* defines timeout for mpeg lock depending on interleaver lenght of longest layer */
+				for (i = 0; i < 3; i++) {
+					if (c->layer[i].interleaving >= deeper_interleaver) {
+						dprintk("layer%i: time interleaver = %d ", i, c->layer[i].interleaving);
+						if (c->layer[i].segment_count > 0) { /* valid layer */
+							deeper_interleaver = c->layer[0].interleaving;
+							state->longest_intlv_layer = i;
+						}
+					}
+				}
+
+				if (deeper_interleaver == 0)
+					locks = 2; /* locks is the tmp local variable name */
+				else if (deeper_interleaver == 3)
+					locks = 8;
+				else
+					locks = 2 * deeper_interleaver;
+
+				if (state->diversity_onoff != 0) /* because of diversity sync */
+					locks *= 2;
+
+				*timeout = now + (2000 * locks); /* give the mpeg lock 800ms if sram is present */
+				dprintk("Deeper interleaver mode = %d on layer %d : timeout mult factor = %d => will use timeout = %d", deeper_interleaver, state->longest_intlv_layer, locks, *timeout);
+
+				*tune_state = CT_DEMOD_STEP_10;
+			} else
+				*tune_state = CT_DEMOD_STOP;
+			break;
+
+	case CT_DEMOD_STEP_10: /* 40 */
+			locks = dib8000_read_lock(fe);
+			if (locks&(1<<(7-state->longest_intlv_layer))) { /* mpeg lock : check the longest one */
+				dprintk("Mpeg locks [ L0 : %d | L1 : %d | L2 : %d ]", (locks>>7)&0x1, (locks>>6)&0x1, (locks>>5)&0x1);
+				if (c->isdbt_sb_mode
+				    && c->isdbt_sb_subchannel < 14
+				    && !state->differential_constellation)
+					/* signal to the upper layer, that there was a channel found and the parameters can be read */
+					state->status = FE_STATUS_DEMOD_SUCCESS;
+				else
+					state->status = FE_STATUS_DATA_LOCKED;
+				*tune_state = CT_DEMOD_STOP;
+			} else if (now > *timeout) {
+				if (c->isdbt_sb_mode
+				    && c->isdbt_sb_subchannel < 14
+				    && !state->differential_constellation) { /* continue to try init prbs autosearch */
+					state->subchannel += 3;
+					*tune_state = CT_DEMOD_STEP_11;
+				} else { /* we are done mpeg of the longest interleaver xas not locking but let's try if an other layer has locked in the same time */
+					if (locks & (0x7<<5)) {
+						dprintk("Mpeg locks [ L0 : %d | L1 : %d | L2 : %d ]", (locks>>7)&0x1, (locks>>6)&0x1, (locks>>5)&0x1);
+						state->status = FE_STATUS_DATA_LOCKED;
+					} else
+						state->status = FE_STATUS_TUNE_FAILED;
+					*tune_state = CT_DEMOD_STOP;
+				}
+			}
+			break;
+
+	case CT_DEMOD_STEP_11:  /* 41 : init prbs autosearch */
+			if (state->subchannel <= 41) {
+				dib8000_set_subchannel_prbs(state, dib8000_get_init_prbs(state, state->subchannel));
+				*tune_state = CT_DEMOD_STEP_9;
+			} else {
+				*tune_state = CT_DEMOD_STOP;
+				state->status = FE_STATUS_TUNE_FAILED;
+			}
+			break;
+
+	default:
+			break;
+	}
+
+	/* tuning is finished - cleanup the demod */
+	switch (*tune_state) {
+	case CT_DEMOD_STOP: /* (42) */
+#ifdef DIB8000_AGC_FREEZE
+			if ((state->revision != 0x8090) && (state->agc1_max != 0)) {
+				dib8000_write_word(state, 108, state->agc1_max);
+				dib8000_write_word(state, 109, state->agc1_min);
+				dib8000_write_word(state, 110, state->agc2_max);
+				dib8000_write_word(state, 111, state->agc2_min);
+				state->agc1_max = 0;
+				state->agc1_min = 0;
+				state->agc2_max = 0;
+				state->agc2_min = 0;
+			}
+#endif
+			ret = FE_CALLBACK_TIME_NEVER;
+			break;
+	default:
+			break;
 	}
 
+	if ((ret > 0) && (*tune_state > CT_DEMOD_STEP_3))
+		return ret * state->symbol_duration;
+	if ((ret > 0) && (ret < state->symbol_duration))
+		return state->symbol_duration; /* at least one symbol */
 	return ret;
 }
 
@@ -2767,7 +3226,7 @@ static int dib8000_wakeup(struct dvb_frontend *fe)
 	if (dib8000_set_adc_state(state, DIBX000_SLOW_ADC_ON) != 0)
 		dprintk("could not start Slow ADC");
 
-	if (state->revision != 0x8090)
+	if (state->revision == 0x8090)
 		dib8000_sad_calib(state);
 
 	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
@@ -2797,21 +3256,6 @@ static int dib8000_sleep(struct dvb_frontend *fe)
 	return dib8000_set_adc_state(state, DIBX000_SLOW_ADC_OFF) | dib8000_set_adc_state(state, DIBX000_ADC_OFF);
 }
 
-enum frontend_tune_state dib8000_get_tune_state(struct dvb_frontend *fe)
-{
-	struct dib8000_state *state = fe->demodulator_priv;
-	return state->tune_state;
-}
-EXPORT_SYMBOL(dib8000_get_tune_state);
-
-int dib8000_set_tune_state(struct dvb_frontend *fe, enum frontend_tune_state tune_state)
-{
-	struct dib8000_state *state = fe->demodulator_priv;
-	state->tune_state = tune_state;
-	return 0;
-}
-EXPORT_SYMBOL(dib8000_set_tune_state);
-
 static int dib8000_get_frontend(struct dvb_frontend *fe)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
@@ -2961,19 +3405,19 @@ static int dib8000_get_frontend(struct dvb_frontend *fe)
 static int dib8000_set_frontend(struct dvb_frontend *fe)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
-	u8 nbr_pending, exit_condition, index_frontend;
-	s8 index_frontend_success = -1;
-	int time, ret;
-	int  time_slave = FE_CALLBACK_TIME_NEVER;
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	int l, i, active, time, ret, time_slave = FE_CALLBACK_TIME_NEVER;
+	u8 exit_condition, index_frontend;
+	u32 delay, callback_time;
 
-	if (state->fe[0]->dtv_property_cache.frequency == 0) {
+	if (c->frequency == 0) {
 		dprintk("dib8000: must at least specify frequency ");
 		return 0;
 	}
 
-	if (state->fe[0]->dtv_property_cache.bandwidth_hz == 0) {
+	if (c->bandwidth_hz == 0) {
 		dprintk("dib8000: no bandwidth specified, set to default ");
-		state->fe[0]->dtv_property_cache.bandwidth_hz = 6000000;
+		c->bandwidth_hz = 6000000;
 	}
 
 	for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
@@ -2981,18 +3425,36 @@ static int dib8000_set_frontend(struct dvb_frontend *fe)
 		state->fe[index_frontend]->dtv_property_cache.delivery_system = SYS_ISDBT;
 		memcpy(&state->fe[index_frontend]->dtv_property_cache, &fe->dtv_property_cache, sizeof(struct dtv_frontend_properties));
 
-		if (state->revision != 0x8090)
-			dib8000_set_output_mode(state->fe[index_frontend],
-					OUTMODE_HIGH_Z);
-		else
-			dib8096p_set_output_mode(state->fe[index_frontend],
-					OUTMODE_HIGH_Z);
+		/* set output mode and diversity input */
+		if (state->revision != 0x8090) {
+			dib8000_set_diversity_in(state->fe[index_frontend], 1);
+			if (index_frontend != 0)
+				dib8000_set_output_mode(state->fe[index_frontend],
+						OUTMODE_DIVERSITY);
+			else
+				dib8000_set_output_mode(state->fe[0], OUTMODE_HIGH_Z);
+		} else {
+			dib8096p_set_diversity_in(state->fe[index_frontend], 1);
+			if (index_frontend != 0)
+				dib8096p_set_output_mode(state->fe[index_frontend],
+						OUTMODE_DIVERSITY);
+			else
+				dib8096p_set_output_mode(state->fe[0], OUTMODE_HIGH_Z);
+		}
+
+		/* tune the tuner */
 		if (state->fe[index_frontend]->ops.tuner_ops.set_params)
 			state->fe[index_frontend]->ops.tuner_ops.set_params(state->fe[index_frontend]);
 
 		dib8000_set_tune_state(state->fe[index_frontend], CT_AGC_START);
 	}
 
+	/* turn off the diversity of the last chip */
+	if (state->revision != 0x8090)
+		dib8000_set_diversity_in(state->fe[index_frontend - 1], 0);
+	else
+		dib8096p_set_diversity_in(state->fe[index_frontend - 1], 0);
+
 	/* start up the AGC */
 	do {
 		time = dib8000_agc_startup(state->fe[0]);
@@ -3019,139 +3481,88 @@ static int dib8000_set_frontend(struct dvb_frontend *fe)
 	for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++)
 		dib8000_set_tune_state(state->fe[index_frontend], CT_DEMOD_START);
 
-	if ((state->fe[0]->dtv_property_cache.delivery_system != SYS_ISDBT) ||
-			(state->fe[0]->dtv_property_cache.inversion == INVERSION_AUTO) ||
-			(state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO) ||
-			(state->fe[0]->dtv_property_cache.guard_interval == GUARD_INTERVAL_AUTO) ||
-			(((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (1 << 0)) != 0) &&
-			 (state->fe[0]->dtv_property_cache.layer[0].segment_count != 0xff) &&
-			 (state->fe[0]->dtv_property_cache.layer[0].segment_count != 0) &&
-			 ((state->fe[0]->dtv_property_cache.layer[0].modulation == QAM_AUTO) ||
-			  (state->fe[0]->dtv_property_cache.layer[0].fec == FEC_AUTO))) ||
-			(((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (1 << 1)) != 0) &&
-			 (state->fe[0]->dtv_property_cache.layer[1].segment_count != 0xff) &&
-			 (state->fe[0]->dtv_property_cache.layer[1].segment_count != 0) &&
-			 ((state->fe[0]->dtv_property_cache.layer[1].modulation == QAM_AUTO) ||
-			  (state->fe[0]->dtv_property_cache.layer[1].fec == FEC_AUTO))) ||
-			(((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (1 << 2)) != 0) &&
-			 (state->fe[0]->dtv_property_cache.layer[2].segment_count != 0xff) &&
-			 (state->fe[0]->dtv_property_cache.layer[2].segment_count != 0) &&
-			 ((state->fe[0]->dtv_property_cache.layer[2].modulation == QAM_AUTO) ||
-			  (state->fe[0]->dtv_property_cache.layer[2].fec == FEC_AUTO))) ||
-			(((state->fe[0]->dtv_property_cache.layer[0].segment_count == 0) ||
-			  ((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (1 << 0)) == 0)) &&
-			 ((state->fe[0]->dtv_property_cache.layer[1].segment_count == 0) ||
-			  ((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (2 << 0)) == 0)) &&
-			 ((state->fe[0]->dtv_property_cache.layer[2].segment_count == 0) || ((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (3 << 0)) == 0)))) {
-		int i = 100;
-		u8 found = 0;
-		u8 tune_failed = 0;
-
+	active = 1;
+	do {
+		callback_time = FE_CALLBACK_TIME_NEVER;
 		for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
-			dib8000_set_bandwidth(state->fe[index_frontend], fe->dtv_property_cache.bandwidth_hz / 1000);
-			dib8000_autosearch_start(state->fe[index_frontend]);
-		}
-
-		do {
-			msleep(20);
-			nbr_pending = 0;
-			exit_condition = 0; /* 0: tune pending; 1: tune failed; 2:tune success */
-			for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
-				if (((tune_failed >> index_frontend) & 0x1) == 0) {
-					found = dib8000_autosearch_irq(state->fe[index_frontend]);
-					switch (found) {
-					case 0: /* tune pending */
-						 nbr_pending++;
-						 break;
-					case 2:
-						 dprintk("autosearch succeed on the frontend%i", index_frontend);
-						 exit_condition = 2;
-						 index_frontend_success = index_frontend;
-						 break;
-					default:
-						 dprintk("unhandled autosearch result");
-					case 1:
-						 tune_failed |= (1 << index_frontend);
-						 dprintk("autosearch failed for the frontend%i", index_frontend);
-						 break;
+			delay = dib8000_tune(state->fe[index_frontend]);
+			if (delay != FE_CALLBACK_TIME_NEVER)
+				delay += systime();
+
+			/* we are in autosearch */
+			if (state->channel_parameters_set == 0) { /* searching */
+				if ((dib8000_get_status(state->fe[index_frontend]) == FE_STATUS_DEMOD_SUCCESS) || (dib8000_get_status(state->fe[index_frontend]) == FE_STATUS_FFT_SUCCESS)) {
+					dprintk("autosearch succeeded on fe%i", index_frontend);
+					dib8000_get_frontend(state->fe[index_frontend]); /* we read the channel parameters from the frontend which was successful */
+					state->channel_parameters_set = 1;
+
+					for (l = 0; (l < MAX_NUMBER_OF_FRONTENDS) && (state->fe[l] != NULL); l++) {
+						if (l != index_frontend) { /* and for all frontend except the successful one */
+							dib8000_tune_restart_from_demod(state->fe[l]);
+
+							state->fe[l]->dtv_property_cache.isdbt_sb_mode = state->fe[index_frontend]->dtv_property_cache.isdbt_sb_mode;
+							state->fe[l]->dtv_property_cache.inversion = state->fe[index_frontend]->dtv_property_cache.inversion;
+							state->fe[l]->dtv_property_cache.transmission_mode = state->fe[index_frontend]->dtv_property_cache.transmission_mode;
+							state->fe[l]->dtv_property_cache.guard_interval = state->fe[index_frontend]->dtv_property_cache.guard_interval;
+							state->fe[l]->dtv_property_cache.isdbt_partial_reception = state->fe[index_frontend]->dtv_property_cache.isdbt_partial_reception;
+							for (i = 0; i < 3; i++) {
+								state->fe[l]->dtv_property_cache.layer[i].segment_count = state->fe[index_frontend]->dtv_property_cache.layer[i].segment_count;
+								state->fe[l]->dtv_property_cache.layer[i].interleaving = state->fe[index_frontend]->dtv_property_cache.layer[i].interleaving;
+								state->fe[l]->dtv_property_cache.layer[i].fec = state->fe[index_frontend]->dtv_property_cache.layer[i].fec;
+								state->fe[l]->dtv_property_cache.layer[i].modulation = state->fe[index_frontend]->dtv_property_cache.layer[i].modulation;
+							}
+
+						}
 					}
 				}
 			}
-
-			/* if all tune are done and no success, exit: tune failed */
-			if ((nbr_pending == 0) && (exit_condition == 0))
-				exit_condition = 1;
-		} while ((exit_condition == 0) && i--);
-
-		if (exit_condition == 1) { /* tune failed */
-			dprintk("tune failed");
-			return 0;
+			if (delay < callback_time)
+				callback_time = delay;
+		}
+		/* tuning is done when the master frontend is done (failed or success) */
+		if (dib8000_get_status(state->fe[0]) == FE_STATUS_TUNE_FAILED ||
+				dib8000_get_status(state->fe[0]) == FE_STATUS_LOCKED ||
+				dib8000_get_status(state->fe[0]) == FE_STATUS_DATA_LOCKED) {
+			active = 0;
+			/* we need to wait for all frontends to be finished */
+			for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+				if (dib8000_get_tune_state(state->fe[index_frontend]) != CT_DEMOD_STOP)
+					active = 1;
+			}
+			if (active == 0)
+				dprintk("tuning done with status %d", dib8000_get_status(state->fe[0]));
 		}
 
-		dprintk("tune success on frontend%i", index_frontend_success);
-
-		dib8000_get_frontend(fe);
-	}
+		if ((active == 1) && (callback_time == FE_CALLBACK_TIME_NEVER)) {
+			dprintk("strange callback time something went wrong");
+			active = 0;
+		}
 
-	for (index_frontend = 0, ret = 0; (ret >= 0) && (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++)
-		ret = dib8000_tune(state->fe[index_frontend]);
+		while ((active == 1) && (systime() < callback_time))
+			msleep(100);
+	} while (active);
 
-	/* set output mode and diversity input */
-	if (state->revision != 0x8090) {
+	/* set output mode */
+	if (state->revision != 0x8090)
 		dib8000_set_output_mode(state->fe[0], state->cfg.output_mode);
-		for (index_frontend = 1;
-				(index_frontend < MAX_NUMBER_OF_FRONTENDS) &&
-				(state->fe[index_frontend] != NULL);
-				index_frontend++) {
-			dib8000_set_output_mode(state->fe[index_frontend],
-					OUTMODE_DIVERSITY);
-			dib8000_set_diversity_in(state->fe[index_frontend-1], 1);
-		}
-
-		/* turn off the diversity of the last chip */
-		dib8000_set_diversity_in(state->fe[index_frontend-1], 0);
-	} else {
+	else {
 		dib8096p_set_output_mode(state->fe[0], state->cfg.output_mode);
 		if (state->cfg.enMpegOutput == 0) {
 			dib8096p_setDibTxMux(state, MPEG_ON_DIBTX);
 			dib8096p_setHostBusMux(state, DIBTX_ON_HOSTBUS);
 		}
-		for (index_frontend = 1;
-				(index_frontend < MAX_NUMBER_OF_FRONTENDS) &&
-				(state->fe[index_frontend] != NULL);
-				index_frontend++) {
-			dib8096p_set_output_mode(state->fe[index_frontend],
-					OUTMODE_DIVERSITY);
-			dib8096p_set_diversity_in(state->fe[index_frontend-1], 1);
-		}
-
-		/* turn off the diversity of the last chip */
-		dib8096p_set_diversity_in(state->fe[index_frontend-1], 0);
 	}
 
 	return ret;
 }
 
-static u16 dib8000_read_lock(struct dvb_frontend *fe)
-{
-	struct dib8000_state *state = fe->demodulator_priv;
-
-	if (state->revision == 0x8090)
-		return dib8000_read_word(state, 570);
-	return dib8000_read_word(state, 568);
-}
-
 static int dib8000_read_status(struct dvb_frontend *fe, fe_status_t * stat)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
 	u16 lock_slave = 0, lock;
 	u8 index_frontend;
 
-	if (state->revision == 0x8090)
-		lock = dib8000_read_word(state, 570);
-	else
-		lock = dib8000_read_word(state, 568);
-
+	lock = dib8000_read_lock(fe);
 	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++)
 		lock_slave |= dib8000_read_lock(state->fe[index_frontend]);
 
@@ -3545,10 +3956,11 @@ struct dvb_frontend *dib8000_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, s
 	dib8000_reset(fe);
 
 	dib8000_write_word(state, 285, (dib8000_read_word(state, 285) & ~0x60) | (3 << 5));	/* ber_rs_len = 3 */
+	state->current_demod_bw = 6000;
 
 	return fe;
 
- error:
+error:
 	kfree(state);
 	return NULL;
 }
diff --git a/drivers/media/dvb-frontends/dib8000.h b/drivers/media/dvb-frontends/dib8000.h
index 9e7a2b170d55..b8c11e52c512 100644
--- a/drivers/media/dvb-frontends/dib8000.h
+++ b/drivers/media/dvb-frontends/dib8000.h
@@ -33,6 +33,8 @@ struct dib8000_config {
 	u8 output_mode;
 	u8 refclksel;
 	u8 enMpegOutput:1;
+
+	struct dibx000_bandwidth_config *plltable;
 };
 
 #define DEFAULT_DIB8000_I2C_ADDRESS 18
@@ -58,7 +60,7 @@ extern int dib8090p_get_dc_power(struct dvb_frontend *fe, u8 IQ);
 extern u32 dib8000_ctrl_timf(struct dvb_frontend *fe,
 		uint8_t op, uint32_t timf);
 extern int dib8000_update_pll(struct dvb_frontend *fe,
-		struct dibx000_bandwidth_config *pll);
+		struct dibx000_bandwidth_config *pll, u32 bw, u8 ratio);
 extern int dib8000_set_slave_frontend(struct dvb_frontend *fe, struct dvb_frontend *fe_slave);
 extern int dib8000_remove_slave_frontend(struct dvb_frontend *fe);
 extern struct dvb_frontend *dib8000_get_slave_frontend(struct dvb_frontend *fe, int slave_index);
@@ -147,7 +149,7 @@ static inline u32 dib8000_ctrl_timf(struct dvb_frontend *fe,
 	return 0;
 }
 static inline int dib8000_update_pll(struct dvb_frontend *fe,
-		struct dibx000_bandwidth_config *pll)
+		struct dibx000_bandwidth_config *pll, u32 bw, u8 ratio)
 {
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return -ENODEV;
diff --git a/drivers/media/dvb-frontends/dibx000_common.h b/drivers/media/dvb-frontends/dibx000_common.h
index 5f484881d7b1..b538e0555c95 100644
--- a/drivers/media/dvb-frontends/dibx000_common.h
+++ b/drivers/media/dvb-frontends/dibx000_common.h
@@ -193,7 +193,8 @@ enum frontend_tune_state {
 	CT_DEMOD_STEP_8,
 	CT_DEMOD_STEP_9,
 	CT_DEMOD_STEP_10,
-	CT_DEMOD_SEARCH_NEXT = 41,
+	CT_DEMOD_STEP_11,
+	CT_DEMOD_SEARCH_NEXT = 51,
 	CT_DEMOD_STEP_LOCKED,
 	CT_DEMOD_STOP,
 
diff --git a/drivers/media/dvb-frontends/drxd.h b/drivers/media/dvb-frontends/drxd.h
index 216c8c3702f8..5f1d6b5f1685 100644
--- a/drivers/media/dvb-frontends/drxd.h
+++ b/drivers/media/dvb-frontends/drxd.h
@@ -24,6 +24,7 @@
 #ifndef _DRXD_H_
 #define _DRXD_H_
 
+#include <linux/kconfig.h>
 #include <linux/types.h>
 #include <linux/i2c.h>
 
@@ -51,8 +52,7 @@ struct drxd_config {
 	 s16(*osc_deviation) (void *priv, s16 dev, int flag);
 };
 
-#if defined(CONFIG_DVB_DRXD) || \
-			(defined(CONFIG_DVB_DRXD_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DRXD)
 extern
 struct dvb_frontend *drxd_attach(const struct drxd_config *config,
 				 void *priv, struct i2c_adapter *i2c,
diff --git a/drivers/media/dvb-frontends/drxk.h b/drivers/media/dvb-frontends/drxk.h
index 94fecfbf14c1..e6667189ddce 100644
--- a/drivers/media/dvb-frontends/drxk.h
+++ b/drivers/media/dvb-frontends/drxk.h
@@ -1,6 +1,7 @@
 #ifndef _DRXK_H_
 #define _DRXK_H_
 
+#include <linux/kconfig.h>
 #include <linux/types.h>
 #include <linux/i2c.h>
 
@@ -52,8 +53,7 @@ struct drxk_config {
 	int		 qam_demod_parameter_count;
 };
 
-#if defined(CONFIG_DVB_DRXK) || (defined(CONFIG_DVB_DRXK_MODULE) \
-        && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DRXK)
 extern struct dvb_frontend *drxk_attach(const struct drxk_config *config,
 					struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/drxk_hard.c b/drivers/media/dvb-frontends/drxk_hard.c
index c2fc7da0d6bf..ec24d71e153d 100644
--- a/drivers/media/dvb-frontends/drxk_hard.c
+++ b/drivers/media/dvb-frontends/drxk_hard.c
@@ -1947,8 +1947,7 @@ static int ShutDown(struct drxk_state *state)
 	return 0;
 }
 
-static int GetLockStatus(struct drxk_state *state, u32 *pLockStatus,
-			 u32 Time)
+static int GetLockStatus(struct drxk_state *state, u32 *pLockStatus)
 {
 	int status = -EINVAL;
 
@@ -2490,32 +2489,6 @@ error:
 	return status;
 }
 
-static int ReadIFAgc(struct drxk_state *state, u32 *pValue)
-{
-	u16 agcDacLvl;
-	int status;
-	u16 Level = 0;
-
-	dprintk(1, "\n");
-
-	status = read16(state, IQM_AF_AGC_IF__A, &agcDacLvl);
-	if (status < 0) {
-		printk(KERN_ERR "drxk: Error %d on %s\n", status, __func__);
-		return status;
-	}
-
-	*pValue = 0;
-
-	if (agcDacLvl > DRXK_AGC_DAC_OFFSET)
-		Level = agcDacLvl - DRXK_AGC_DAC_OFFSET;
-	if (Level < 14000)
-		*pValue = (14000 - Level) / 4;
-	else
-		*pValue = 0;
-
-	return status;
-}
-
 static int GetQAMSignalToNoise(struct drxk_state *state,
 			       s32 *pSignalToNoise)
 {
@@ -2654,12 +2627,7 @@ static int GetDVBTSignalToNoise(struct drxk_state *state,
 		/* log(x) x = (16bits + 16bits) << 15 ->32 bits  */
 		c = Log10Times100(SqrErrIQ);
 
-		iMER = a + b;
-		/* No negative MER, clip to zero */
-		if (iMER > c)
-			iMER -= c;
-		else
-			iMER = 0;
+		iMER = a + b - c;
 	}
 	*pSignalToNoise = iMER;
 
@@ -6380,46 +6348,257 @@ static int drxk_set_parameters(struct dvb_frontend *fe)
 	fe->ops.tuner_ops.get_if_frequency(fe, &IF);
 	Start(state, 0, IF);
 
+	/* After set_frontend, stats aren't avaliable */
+	p->strength.stat[0].scale = FE_SCALE_RELATIVE;
+	p->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->block_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->block_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->pre_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->pre_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->post_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+
 	/* printk(KERN_DEBUG "drxk: %s IF=%d done\n", __func__, IF); */
 
 	return 0;
 }
 
-static int drxk_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int get_strength(struct drxk_state *state, u64 *strength)
 {
+	int status;
+	struct SCfgAgc   rfAgc, ifAgc;
+	u32          totalGain  = 0;
+	u32          atten      = 0;
+	u32          agcRange   = 0;
+	u16            scu_lvl  = 0;
+	u16            scu_coc  = 0;
+	/* FIXME: those are part of the tuner presets */
+	u16 tunerRfGain         = 50; /* Default value on az6007 driver */
+	u16 tunerIfGain         = 40; /* Default value on az6007 driver */
+
+	*strength = 0;
+
+	if (IsDVBT(state)) {
+		rfAgc = state->m_dvbtRfAgcCfg;
+		ifAgc = state->m_dvbtIfAgcCfg;
+	} else if (IsQAM(state)) {
+		rfAgc = state->m_qamRfAgcCfg;
+		ifAgc = state->m_qamIfAgcCfg;
+	} else {
+		rfAgc = state->m_atvRfAgcCfg;
+		ifAgc = state->m_atvIfAgcCfg;
+	}
+
+	if (rfAgc.ctrlMode == DRXK_AGC_CTRL_AUTO) {
+		/* SCU outputLevel */
+		status = read16(state, SCU_RAM_AGC_RF_IACCU_HI__A, &scu_lvl);
+		if (status < 0)
+			return status;
+
+		/* SCU c.o.c. */
+		read16(state, SCU_RAM_AGC_RF_IACCU_HI_CO__A, &scu_coc);
+		if (status < 0)
+			return status;
+
+		if (((u32) scu_lvl + (u32) scu_coc) < 0xffff)
+			rfAgc.outputLevel = scu_lvl + scu_coc;
+		else
+			rfAgc.outputLevel = 0xffff;
+
+		/* Take RF gain into account */
+		totalGain += tunerRfGain;
+
+		/* clip output value */
+		if (rfAgc.outputLevel < rfAgc.minOutputLevel)
+			rfAgc.outputLevel = rfAgc.minOutputLevel;
+		if (rfAgc.outputLevel > rfAgc.maxOutputLevel)
+			rfAgc.outputLevel = rfAgc.maxOutputLevel;
+
+		agcRange = (u32) (rfAgc.maxOutputLevel - rfAgc.minOutputLevel);
+		if (agcRange > 0) {
+			atten += 100UL *
+				((u32)(tunerRfGain)) *
+				((u32)(rfAgc.outputLevel - rfAgc.minOutputLevel))
+				/ agcRange;
+		}
+	}
+
+	if (ifAgc.ctrlMode == DRXK_AGC_CTRL_AUTO) {
+		status = read16(state, SCU_RAM_AGC_IF_IACCU_HI__A,
+				&ifAgc.outputLevel);
+		if (status < 0)
+			return status;
+
+		status = read16(state, SCU_RAM_AGC_INGAIN_TGT_MIN__A,
+				&ifAgc.top);
+		if (status < 0)
+			return status;
+
+		/* Take IF gain into account */
+		totalGain += (u32) tunerIfGain;
+
+		/* clip output value */
+		if (ifAgc.outputLevel < ifAgc.minOutputLevel)
+			ifAgc.outputLevel = ifAgc.minOutputLevel;
+		if (ifAgc.outputLevel > ifAgc.maxOutputLevel)
+			ifAgc.outputLevel = ifAgc.maxOutputLevel;
+
+		agcRange  = (u32) (ifAgc.maxOutputLevel - ifAgc.minOutputLevel);
+		if (agcRange > 0) {
+			atten += 100UL *
+				((u32)(tunerIfGain)) *
+				((u32)(ifAgc.outputLevel - ifAgc.minOutputLevel))
+				/ agcRange;
+		}
+	}
+
+	/*
+	 * Convert to 0..65535 scale.
+	 * If it can't be measured (AGC is disabled), just show 100%.
+	 */
+	if (totalGain > 0)
+		*strength = (65535UL * atten / totalGain / 100);
+	else
+		*strength = 65535;
+
+	return 0;
+}
+
+static int drxk_get_stats(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct drxk_state *state = fe->demodulator_priv;
+	int status;
 	u32 stat;
-
-	dprintk(1, "\n");
+	u16 reg16;
+	u32 post_bit_count;
+	u32 post_bit_err_count;
+	u32 post_bit_error_scale;
+	u32 pre_bit_err_count;
+	u32 pre_bit_count;
+	u32 pkt_count;
+	u32 pkt_error_count;
+	s32 cnr;
 
 	if (state->m_DrxkState == DRXK_NO_DEV)
 		return -ENODEV;
 	if (state->m_DrxkState == DRXK_UNINITIALIZED)
 		return -EAGAIN;
 
-	*status = 0;
-	GetLockStatus(state, &stat, 0);
+	/* get status */
+	state->fe_status = 0;
+	GetLockStatus(state, &stat);
 	if (stat == MPEG_LOCK)
-		*status |= 0x1f;
+		state->fe_status |= 0x1f;
 	if (stat == FEC_LOCK)
-		*status |= 0x0f;
+		state->fe_status |= 0x0f;
 	if (stat == DEMOD_LOCK)
-		*status |= 0x07;
-	return 0;
+		state->fe_status |= 0x07;
+
+	/*
+	 * Estimate signal strength from AGC
+	 */
+	get_strength(state, &c->strength.stat[0].uvalue);
+	c->strength.stat[0].scale = FE_SCALE_RELATIVE;
+
+
+	if (stat >= DEMOD_LOCK) {
+		GetSignalToNoise(state, &cnr);
+		c->cnr.stat[0].svalue = cnr * 100;
+		c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+	} else {
+		c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	}
+
+	if (stat < FEC_LOCK) {
+		c->block_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->block_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->pre_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->pre_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->post_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		return 0;
+	}
+
+	/* Get post BER */
+
+	/* BER measurement is valid if at least FEC lock is achieved */
+
+	/* OFDM_EC_VD_REQ_SMB_CNT__A and/or OFDM_EC_VD_REQ_BIT_CNT can be written
+		to set nr of symbols or bits over which
+		to measure EC_VD_REG_ERR_BIT_CNT__A . See CtrlSetCfg(). */
+
+	/* Read registers for post/preViterbi BER calculation */
+	status = read16(state, OFDM_EC_VD_ERR_BIT_CNT__A, &reg16);
+	if (status < 0)
+		goto error;
+	pre_bit_err_count = reg16;
+
+	status = read16(state, OFDM_EC_VD_IN_BIT_CNT__A , &reg16);
+	if (status < 0)
+		goto error;
+	pre_bit_count = reg16;
+
+	/* Number of bit-errors */
+	status = read16(state, FEC_RS_NR_BIT_ERRORS__A, &reg16);
+	if (status < 0)
+		goto error;
+	post_bit_err_count = reg16;
+
+	status = read16(state, FEC_RS_MEASUREMENT_PRESCALE__A, &reg16);
+	if (status < 0)
+		goto error;
+	post_bit_error_scale = reg16;
+
+	status = read16(state, FEC_RS_MEASUREMENT_PERIOD__A, &reg16);
+	if (status < 0)
+		goto error;
+	pkt_count = reg16;
+
+	status = read16(state, SCU_RAM_FEC_ACCUM_PKT_FAILURES__A, &reg16);
+	if (status < 0)
+		goto error;
+	pkt_error_count = reg16;
+	write16(state, SCU_RAM_FEC_ACCUM_PKT_FAILURES__A, 0);
+
+	post_bit_err_count *= post_bit_error_scale;
+
+	post_bit_count = pkt_count * 204 * 8;
+
+	/* Store the results */
+	c->block_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_error.stat[0].uvalue += pkt_error_count;
+	c->block_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->block_count.stat[0].uvalue += pkt_count;
+
+	c->pre_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->pre_bit_error.stat[0].uvalue += pre_bit_err_count;
+	c->pre_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->pre_bit_count.stat[0].uvalue += pre_bit_count;
+
+	c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_error.stat[0].uvalue += post_bit_err_count;
+	c->post_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+	c->post_bit_count.stat[0].uvalue += post_bit_count;
+
+error:
+	return status;
 }
 
-static int drxk_read_ber(struct dvb_frontend *fe, u32 *ber)
+
+static int drxk_read_status(struct dvb_frontend *fe, fe_status_t *status)
 {
 	struct drxk_state *state = fe->demodulator_priv;
+	int rc;
 
 	dprintk(1, "\n");
 
-	if (state->m_DrxkState == DRXK_NO_DEV)
-		return -ENODEV;
-	if (state->m_DrxkState == DRXK_UNINITIALIZED)
-		return -EAGAIN;
+	rc = drxk_get_stats(fe);
+	if (rc < 0)
+		return rc;
+
+	*status = state->fe_status;
 
-	*ber = 0;
 	return 0;
 }
 
@@ -6427,7 +6606,7 @@ static int drxk_read_signal_strength(struct dvb_frontend *fe,
 				     u16 *strength)
 {
 	struct drxk_state *state = fe->demodulator_priv;
-	u32 val = 0;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 
 	dprintk(1, "\n");
 
@@ -6436,8 +6615,7 @@ static int drxk_read_signal_strength(struct dvb_frontend *fe,
 	if (state->m_DrxkState == DRXK_UNINITIALIZED)
 		return -EAGAIN;
 
-	ReadIFAgc(state, &val);
-	*strength = val & 0xffff;
+	*strength = c->strength.stat[0].uvalue;
 	return 0;
 }
 
@@ -6454,6 +6632,10 @@ static int drxk_read_snr(struct dvb_frontend *fe, u16 *snr)
 		return -EAGAIN;
 
 	GetSignalToNoise(state, &snr2);
+
+	/* No negative SNR, clip to zero */
+	if (snr2 < 0)
+		snr2 = 0;
 	*snr = snr2 & 0xffff;
 	return 0;
 }
@@ -6529,7 +6711,6 @@ static struct dvb_frontend_ops drxk_ops = {
 	.get_tune_settings = drxk_get_tune_settings,
 
 	.read_status = drxk_read_status,
-	.read_ber = drxk_read_ber,
 	.read_signal_strength = drxk_read_signal_strength,
 	.read_snr = drxk_read_snr,
 	.read_ucblocks = drxk_read_ucblocks,
@@ -6538,6 +6719,7 @@ static struct dvb_frontend_ops drxk_ops = {
 struct dvb_frontend *drxk_attach(const struct drxk_config *config,
 				 struct i2c_adapter *i2c)
 {
+	struct dtv_frontend_properties *p;
 	struct drxk_state *state = NULL;
 	u8 adr = config->adr;
 	int status;
@@ -6618,6 +6800,27 @@ struct dvb_frontend *drxk_attach(const struct drxk_config *config,
 	} else if (init_drxk(state) < 0)
 		goto error;
 
+
+	/* Initialize stats */
+	p = &state->frontend.dtv_property_cache;
+	p->strength.len = 1;
+	p->cnr.len = 1;
+	p->block_error.len = 1;
+	p->block_count.len = 1;
+	p->pre_bit_error.len = 1;
+	p->pre_bit_count.len = 1;
+	p->post_bit_error.len = 1;
+	p->post_bit_count.len = 1;
+
+	p->strength.stat[0].scale = FE_SCALE_RELATIVE;
+	p->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->block_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->block_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->pre_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->pre_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	p->post_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+
 	printk(KERN_INFO "drxk: frontend initialized.\n");
 	return &state->frontend;
 
diff --git a/drivers/media/dvb-frontends/drxk_hard.h b/drivers/media/dvb-frontends/drxk_hard.h
index d18a896a9835..b8424f15f9a6 100644
--- a/drivers/media/dvb-frontends/drxk_hard.h
+++ b/drivers/media/dvb-frontends/drxk_hard.h
@@ -345,6 +345,8 @@ struct drxk_state {
 	bool	antenna_dvbt;
 	u16	antenna_gpio;
 
+	fe_status_t fe_status;
+
 	/* Firmware */
 	const char *microcode_name;
 	struct completion fw_wait_load;
diff --git a/drivers/media/dvb-frontends/drxk_map.h b/drivers/media/dvb-frontends/drxk_map.h
index 23e16c12f234..761613f9fd5a 100644
--- a/drivers/media/dvb-frontends/drxk_map.h
+++ b/drivers/media/dvb-frontends/drxk_map.h
@@ -10,6 +10,7 @@
 #define    FEC_RS_COMM_EXEC_STOP                                           0x0
 #define  FEC_RS_MEASUREMENT_PERIOD__A                                      0x1C30012
 #define  FEC_RS_MEASUREMENT_PRESCALE__A                                    0x1C30013
+#define FEC_RS_NR_BIT_ERRORS__A                                            0x1C30014
 #define  FEC_OC_MODE__A                                                    0x1C40011
 #define    FEC_OC_MODE_PARITY__M                                           0x1
 #define  FEC_OC_DTO_MODE__A                                                0x1C40014
@@ -129,6 +130,8 @@
 #define  OFDM_EC_SB_PRIOR__A                                               0x3410013
 #define    OFDM_EC_SB_PRIOR_HI                                             0x0
 #define    OFDM_EC_SB_PRIOR_LO                                             0x1
+#define OFDM_EC_VD_ERR_BIT_CNT__A                                          0x3420017
+#define OFDM_EC_VD_IN_BIT_CNT__A                                           0x3420018
 #define  OFDM_EQ_TOP_TD_TPS_CONST__A                                       0x3010054
 #define  OFDM_EQ_TOP_TD_TPS_CONST__M                                       0x3
 #define    OFDM_EQ_TOP_TD_TPS_CONST_64QAM                                  0x2
diff --git a/drivers/media/dvb-frontends/ds3000.h b/drivers/media/dvb-frontends/ds3000.h
index 478ad66c63d7..f9c21fb7af13 100644
--- a/drivers/media/dvb-frontends/ds3000.h
+++ b/drivers/media/dvb-frontends/ds3000.h
@@ -22,6 +22,7 @@
 #ifndef DS3000_H
 #define DS3000_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct ds3000_config {
@@ -34,8 +35,7 @@ struct ds3000_config {
 	void (*set_lock_led)(struct dvb_frontend *fe, int offon);
 };
 
-#if defined(CONFIG_DVB_DS3000) || \
-			(defined(CONFIG_DVB_DS3000_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DS3000)
 extern struct dvb_frontend *ds3000_attach(const struct ds3000_config *config,
 					struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/dvb_dummy_fe.h b/drivers/media/dvb-frontends/dvb_dummy_fe.h
index 1fcb987d6386..0cbf96105631 100644
--- a/drivers/media/dvb-frontends/dvb_dummy_fe.h
+++ b/drivers/media/dvb-frontends/dvb_dummy_fe.h
@@ -22,11 +22,11 @@
 #ifndef DVB_DUMMY_FE_H
 #define DVB_DUMMY_FE_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
-#if defined(CONFIG_DVB_DUMMY_FE) || (defined(CONFIG_DVB_DUMMY_FE_MODULE) && \
-defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DUMMY_FE)
 extern struct dvb_frontend* dvb_dummy_fe_ofdm_attach(void);
 extern struct dvb_frontend* dvb_dummy_fe_qpsk_attach(void);
 extern struct dvb_frontend* dvb_dummy_fe_qam_attach(void);
diff --git a/drivers/media/dvb-frontends/ec100.h b/drivers/media/dvb-frontends/ec100.h
index b8479719d7f1..37558403068d 100644
--- a/drivers/media/dvb-frontends/ec100.h
+++ b/drivers/media/dvb-frontends/ec100.h
@@ -22,6 +22,7 @@
 #ifndef EC100_H
 #define EC100_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct ec100_config {
@@ -30,8 +31,7 @@ struct ec100_config {
 };
 
 
-#if defined(CONFIG_DVB_EC100) || \
-	(defined(CONFIG_DVB_EC100_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_EC100)
 extern struct dvb_frontend *ec100_attach(const struct ec100_config *config,
 	struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/hd29l2.h b/drivers/media/dvb-frontends/hd29l2.h
index 4ad00d79aa77..05cd13028a91 100644
--- a/drivers/media/dvb-frontends/hd29l2.h
+++ b/drivers/media/dvb-frontends/hd29l2.h
@@ -23,6 +23,7 @@
 #ifndef HD29L2_H
 #define HD29L2_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct hd29l2_config {
@@ -50,8 +51,7 @@ struct hd29l2_config {
 };
 
 
-#if defined(CONFIG_DVB_HD29L2) || \
-	(defined(CONFIG_DVB_HD29L2_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_HD29L2)
 extern struct dvb_frontend *hd29l2_attach(const struct hd29l2_config *config,
 	struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/isl6421.c b/drivers/media/dvb-frontends/isl6421.c
index 0cb3f0f74c9c..c77002fcc8e2 100644
--- a/drivers/media/dvb-frontends/isl6421.c
+++ b/drivers/media/dvb-frontends/isl6421.c
@@ -89,6 +89,30 @@ static int isl6421_enable_high_lnb_voltage(struct dvb_frontend *fe, long arg)
 	return (i2c_transfer(isl6421->i2c, &msg, 1) == 1) ? 0 : -EIO;
 }
 
+static int isl6421_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+{
+	struct isl6421 *isl6421 = (struct isl6421 *) fe->sec_priv;
+	struct i2c_msg msg = { .addr = isl6421->i2c_addr, .flags = 0,
+			       .buf = &isl6421->config,
+			       .len = sizeof(isl6421->config) };
+
+	switch (tone) {
+	case SEC_TONE_ON:
+		isl6421->config |= ISL6421_ENT1;
+		break;
+	case SEC_TONE_OFF:
+		isl6421->config &= ~ISL6421_ENT1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	isl6421->config |= isl6421->override_or;
+	isl6421->config &= isl6421->override_and;
+
+	return (i2c_transfer(isl6421->i2c, &msg, 1) == 1) ? 0 : -EIO;
+}
+
 static void isl6421_release(struct dvb_frontend *fe)
 {
 	/* power off */
@@ -100,7 +124,7 @@ static void isl6421_release(struct dvb_frontend *fe)
 }
 
 struct dvb_frontend *isl6421_attach(struct dvb_frontend *fe, struct i2c_adapter *i2c, u8 i2c_addr,
-		   u8 override_set, u8 override_clear)
+		   u8 override_set, u8 override_clear, bool override_tone)
 {
 	struct isl6421 *isl6421 = kmalloc(sizeof(struct isl6421), GFP_KERNEL);
 	if (!isl6421)
@@ -131,6 +155,8 @@ struct dvb_frontend *isl6421_attach(struct dvb_frontend *fe, struct i2c_adapter
 	/* override frontend ops */
 	fe->ops.set_voltage = isl6421_set_voltage;
 	fe->ops.enable_high_lnb_voltage = isl6421_enable_high_lnb_voltage;
+	if (override_tone)
+		fe->ops.set_tone = isl6421_set_tone;
 
 	return fe;
 }
diff --git a/drivers/media/dvb-frontends/isl6421.h b/drivers/media/dvb-frontends/isl6421.h
index e7ca7d12b50a..630e7f8a150e 100644
--- a/drivers/media/dvb-frontends/isl6421.h
+++ b/drivers/media/dvb-frontends/isl6421.h
@@ -42,10 +42,10 @@
 #if IS_ENABLED(CONFIG_DVB_ISL6421)
 /* override_set and override_clear control which system register bits (above) to always set & clear */
 extern struct dvb_frontend *isl6421_attach(struct dvb_frontend *fe, struct i2c_adapter *i2c, u8 i2c_addr,
-			  u8 override_set, u8 override_clear);
+			  u8 override_set, u8 override_clear, bool override_tone);
 #else
 static inline struct dvb_frontend *isl6421_attach(struct dvb_frontend *fe, struct i2c_adapter *i2c, u8 i2c_addr,
-						  u8 override_set, u8 override_clear)
+						  u8 override_set, u8 override_clear, bool override_tone)
 {
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return NULL;
diff --git a/drivers/media/dvb-frontends/it913x-fe.h b/drivers/media/dvb-frontends/it913x-fe.h
index 07fa4594c12b..df0ad4207343 100644
--- a/drivers/media/dvb-frontends/it913x-fe.h
+++ b/drivers/media/dvb-frontends/it913x-fe.h
@@ -21,6 +21,7 @@
 #ifndef IT913X_FE_H
 #define IT913X_FE_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
@@ -38,8 +39,7 @@ struct ite_config {
 	u8 read_slevel;
 };
 
-#if defined(CONFIG_DVB_IT913X_FE) || (defined(CONFIG_DVB_IT913X_FE_MODULE) && \
-defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_IT913X_FE)
 extern struct dvb_frontend *it913x_fe_attach(struct i2c_adapter *i2c_adap,
 			u8 i2c_addr, struct ite_config *config);
 #else
diff --git a/drivers/media/dvb-frontends/ix2505v.h b/drivers/media/dvb-frontends/ix2505v.h
index 67e89d616d50..1a735a75aa98 100644
--- a/drivers/media/dvb-frontends/ix2505v.h
+++ b/drivers/media/dvb-frontends/ix2505v.h
@@ -20,6 +20,7 @@
 #ifndef DVB_IX2505V_H
 #define DVB_IX2505V_H
 
+#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
@@ -48,8 +49,7 @@ struct ix2505v_config {
 
 };
 
-#if defined(CONFIG_DVB_IX2505V) || \
-	(defined(CONFIG_DVB_IX2505V_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_IX2505V)
 extern struct dvb_frontend *ix2505v_attach(struct dvb_frontend *fe,
 	const struct ix2505v_config *config, struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/lg2160.h b/drivers/media/dvb-frontends/lg2160.h
index 9e2c0f41199a..194a07a78dc1 100644
--- a/drivers/media/dvb-frontends/lg2160.h
+++ b/drivers/media/dvb-frontends/lg2160.h
@@ -22,6 +22,7 @@
 #ifndef _LG2160_H_
 #define _LG2160_H_
 
+#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
@@ -56,18 +57,17 @@ struct lg2160_config {
 	u16 if_khz;
 
 	/* disable i2c repeater - 0:repeater enabled 1:repeater disabled */
-	int deny_i2c_rptr:1;
+	unsigned int deny_i2c_rptr:1;
 
 	/* spectral inversion - 0:disabled 1:enabled */
-	int spectral_inversion:1;
+	unsigned int spectral_inversion:1;
 
 	unsigned int output_if;
 	enum lg2160_spi_clock spi_clock;
 	enum lg_chip_type lg_chip;
 };
 
-#if defined(CONFIG_DVB_LG2160) || (defined(CONFIG_DVB_LG2160_MODULE) && \
-				     defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_LG2160)
 extern
 struct dvb_frontend *lg2160_attach(const struct lg2160_config *config,
 				     struct i2c_adapter *i2c_adap);
diff --git a/drivers/media/dvb-frontends/lgdt3305.h b/drivers/media/dvb-frontends/lgdt3305.h
index 02172eca4d47..d9ab556c1b27 100644
--- a/drivers/media/dvb-frontends/lgdt3305.h
+++ b/drivers/media/dvb-frontends/lgdt3305.h
@@ -22,6 +22,7 @@
 #ifndef _LGDT3305_H_
 #define _LGDT3305_H_
 
+#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
@@ -73,8 +74,7 @@ struct lgdt3305_config {
 	enum lgdt_demod_chip_type demod_chip;
 };
 
-#if defined(CONFIG_DVB_LGDT3305) || (defined(CONFIG_DVB_LGDT3305_MODULE) && \
-				     defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_LGDT3305)
 extern
 struct dvb_frontend *lgdt3305_attach(const struct lgdt3305_config *config,
 				     struct i2c_adapter *i2c_adap);
diff --git a/drivers/media/dvb-frontends/lgs8gl5.h b/drivers/media/dvb-frontends/lgs8gl5.h
index d14176787a7d..c2da59614727 100644
--- a/drivers/media/dvb-frontends/lgs8gl5.h
+++ b/drivers/media/dvb-frontends/lgs8gl5.h
@@ -23,6 +23,7 @@
 #ifndef LGS8GL5_H
 #define LGS8GL5_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct lgs8gl5_config {
@@ -30,8 +31,7 @@ struct lgs8gl5_config {
 	u8 demod_address;
 };
 
-#if defined(CONFIG_DVB_LGS8GL5) || \
-	(defined(CONFIG_DVB_LGS8GL5_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_LGS8GL5)
 extern struct dvb_frontend *lgs8gl5_attach(
 	const struct lgs8gl5_config *config, struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/lgs8gxx.h b/drivers/media/dvb-frontends/lgs8gxx.h
index 33c3c5e162fa..dadb78bf61a9 100644
--- a/drivers/media/dvb-frontends/lgs8gxx.h
+++ b/drivers/media/dvb-frontends/lgs8gxx.h
@@ -26,6 +26,7 @@
 #ifndef __LGS8GXX_H__
 #define __LGS8GXX_H__
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include <linux/i2c.h>
 
@@ -79,8 +80,7 @@ struct lgs8gxx_config {
 	u8 tuner_address;
 };
 
-#if defined(CONFIG_DVB_LGS8GXX) || \
-	(defined(CONFIG_DVB_LGS8GXX_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_LGS8GXX)
 extern struct dvb_frontend *lgs8gxx_attach(const struct lgs8gxx_config *config,
 					   struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/lnbh24.h b/drivers/media/dvb-frontends/lnbh24.h
index c059b165318f..b327a4f31d16 100644
--- a/drivers/media/dvb-frontends/lnbh24.h
+++ b/drivers/media/dvb-frontends/lnbh24.h
@@ -23,6 +23,8 @@
 #ifndef _LNBH24_H
 #define _LNBH24_H
 
+#include <linux/kconfig.h>
+
 /* system register bits */
 #define LNBH24_OLF	0x01
 #define LNBH24_OTF	0x02
@@ -35,8 +37,7 @@
 
 #include <linux/dvb/frontend.h>
 
-#if defined(CONFIG_DVB_LNBP21) || (defined(CONFIG_DVB_LNBP21_MODULE) \
-							&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_LNBP21)
 /* override_set and override_clear control which
    system register bits (above) to always set & clear */
 extern struct dvb_frontend *lnbh24_attach(struct dvb_frontend *fe,
diff --git a/drivers/media/dvb-frontends/lnbp21.h b/drivers/media/dvb-frontends/lnbp21.h
index fcdf1c650dde..dbcbcc2f20a3 100644
--- a/drivers/media/dvb-frontends/lnbp21.h
+++ b/drivers/media/dvb-frontends/lnbp21.h
@@ -27,6 +27,8 @@
 #ifndef _LNBP21_H
 #define _LNBP21_H
 
+#include <linux/kconfig.h>
+
 /* system register bits */
 /* [RO] 0=OK; 1=over current limit flag */
 #define LNBP21_OLF	0x01
@@ -55,8 +57,7 @@
 
 #include <linux/dvb/frontend.h>
 
-#if defined(CONFIG_DVB_LNBP21) || (defined(CONFIG_DVB_LNBP21_MODULE) \
-							&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_LNBP21)
 /* override_set and override_clear control which
  system register bits (above) to always set & clear */
 extern struct dvb_frontend *lnbp21_attach(struct dvb_frontend *fe,
diff --git a/drivers/media/dvb-frontends/lnbp22.h b/drivers/media/dvb-frontends/lnbp22.h
index 63e2dec7e68a..63861b311dd8 100644
--- a/drivers/media/dvb-frontends/lnbp22.h
+++ b/drivers/media/dvb-frontends/lnbp22.h
@@ -28,6 +28,8 @@
 #ifndef _LNBP22_H
 #define _LNBP22_H
 
+#include <linux/kconfig.h>
+
 /* Enable */
 #define LNBP22_EN	  0x10
 /* Voltage selection */
@@ -37,8 +39,7 @@
 
 #include <linux/dvb/frontend.h>
 
-#if defined(CONFIG_DVB_LNBP22) || \
-		(defined(CONFIG_DVB_LNBP22_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_LNBP22)
 /*
  * override_set and override_clear control which system register bits (above)
  * to always set & clear
diff --git a/drivers/media/dvb-frontends/m88rs2000.h b/drivers/media/dvb-frontends/m88rs2000.h
index 5a8023e5a4b8..14ce31e76ae6 100644
--- a/drivers/media/dvb-frontends/m88rs2000.h
+++ b/drivers/media/dvb-frontends/m88rs2000.h
@@ -20,6 +20,7 @@
 #ifndef M88RS2000_H
 #define M88RS2000_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
@@ -40,8 +41,7 @@ enum {
 	CALL_IS_READ,
 };
 
-#if defined(CONFIG_DVB_M88RS2000) || (defined(CONFIG_DVB_M88RS2000_MODULE) && \
-							defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_M88RS2000)
 extern struct dvb_frontend *m88rs2000_attach(
 	const struct m88rs2000_config *config, struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/mb86a20s.c b/drivers/media/dvb-frontends/mb86a20s.c
index 4faaf8053f26..856374bd3676 100644
--- a/drivers/media/dvb-frontends/mb86a20s.c
+++ b/drivers/media/dvb-frontends/mb86a20s.c
@@ -20,10 +20,24 @@
 #include "dvb_frontend.h"
 #include "mb86a20s.h"
 
+#define NUM_LAYERS 3
+
 static int debug = 1;
 module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "Activates frontend debugging (default:0)");
 
+enum mb86a20s_bandwidth {
+	MB86A20S_13SEG = 0,
+	MB86A20S_13SEG_PARTIAL = 1,
+	MB86A20S_1SEG = 2,
+	MB86A20S_3SEG = 3,
+};
+
+u8 mb86a20s_subchannel[] = {
+	0xb0, 0xc0, 0xd0, 0xe0,
+	0xf0, 0x00, 0x10, 0x20,
+};
+
 struct mb86a20s_state {
 	struct i2c_adapter *i2c;
 	const struct mb86a20s_config *config;
@@ -31,7 +45,13 @@ struct mb86a20s_state {
 
 	struct dvb_frontend frontend;
 
-	u32 estimated_rate[3];
+	u32 if_freq;
+	enum mb86a20s_bandwidth bw;
+	bool inversion;
+	u32 subchannel;
+
+	u32 estimated_rate[NUM_LAYERS];
+	unsigned long get_strength_time;
 
 	bool need_init;
 };
@@ -47,35 +67,33 @@ struct regdata {
  * Initialization sequence: Use whatevere default values that PV SBTVD
  * does on its initialisation, obtained via USB snoop
  */
-static struct regdata mb86a20s_init[] = {
+static struct regdata mb86a20s_init1[] = {
 	{ 0x70, 0x0f },
 	{ 0x70, 0xff },
 	{ 0x08, 0x01 },
-	{ 0x09, 0x3e },
-	{ 0x50, 0xd1 }, { 0x51, 0x22 },
-	{ 0x39, 0x01 },
-	{ 0x71, 0x00 },
-	{ 0x28, 0x2a }, { 0x29, 0x00 }, { 0x2a, 0xff }, { 0x2b, 0x80 },
-	{ 0x28, 0x20 }, { 0x29, 0x33 }, { 0x2a, 0xdf }, { 0x2b, 0xa9 },
+	{ 0x50, 0xd1 }, { 0x51, 0x20 },
+};
+
+static struct regdata mb86a20s_init2[] = {
 	{ 0x28, 0x22 }, { 0x29, 0x00 }, { 0x2a, 0x1f }, { 0x2b, 0xf0 },
 	{ 0x3b, 0x21 },
-	{ 0x3c, 0x3a },
+	{ 0x3c, 0x38 },
 	{ 0x01, 0x0d },
-	{ 0x04, 0x08 }, { 0x05, 0x05 },
+	{ 0x04, 0x08 }, { 0x05, 0x03 },
 	{ 0x04, 0x0e }, { 0x05, 0x00 },
-	{ 0x04, 0x0f }, { 0x05, 0x14 },
-	{ 0x04, 0x0b }, { 0x05, 0x8c },
+	{ 0x04, 0x0f }, { 0x05, 0x37 },
+	{ 0x04, 0x0b }, { 0x05, 0x78 },
 	{ 0x04, 0x00 }, { 0x05, 0x00 },
-	{ 0x04, 0x01 }, { 0x05, 0x07 },
-	{ 0x04, 0x02 }, { 0x05, 0x0f },
-	{ 0x04, 0x03 }, { 0x05, 0xa0 },
+	{ 0x04, 0x01 }, { 0x05, 0x1e },
+	{ 0x04, 0x02 }, { 0x05, 0x07 },
+	{ 0x04, 0x03 }, { 0x05, 0xd0 },
 	{ 0x04, 0x09 }, { 0x05, 0x00 },
 	{ 0x04, 0x0a }, { 0x05, 0xff },
-	{ 0x04, 0x27 }, { 0x05, 0x64 },
+	{ 0x04, 0x27 }, { 0x05, 0x00 },
 	{ 0x04, 0x28 }, { 0x05, 0x00 },
-	{ 0x04, 0x1e }, { 0x05, 0xff },
-	{ 0x04, 0x29 }, { 0x05, 0x0a },
-	{ 0x04, 0x32 }, { 0x05, 0x0a },
+	{ 0x04, 0x1e }, { 0x05, 0x00 },
+	{ 0x04, 0x29 }, { 0x05, 0x64 },
+	{ 0x04, 0x32 }, { 0x05, 0x02 },
 	{ 0x04, 0x14 }, { 0x05, 0x02 },
 	{ 0x04, 0x04 }, { 0x05, 0x00 },
 	{ 0x04, 0x05 }, { 0x05, 0x22 },
@@ -142,39 +160,39 @@ static struct regdata mb86a20s_init[] = {
 	{ 0x50, 0xd5 }, { 0x51, 0x01 },		/* Serial */
 	{ 0x50, 0xd6 }, { 0x51, 0x1f },
 	{ 0x50, 0xd2 }, { 0x51, 0x03 },
-	{ 0x50, 0xd7 }, { 0x51, 0x3f },
-	{ 0x28, 0x74 }, { 0x29, 0x00 }, { 0x28, 0x74 }, { 0x29, 0x40 },
-	{ 0x28, 0x46 }, { 0x29, 0x2c }, { 0x28, 0x46 }, { 0x29, 0x0c },
+	{ 0x50, 0xd7 }, { 0x51, 0xbf },
+	{ 0x28, 0x74 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0xff },
+	{ 0x28, 0x46 }, { 0x29, 0x00 }, { 0x2a, 0x1a }, { 0x2b, 0x0c },
 
 	{ 0x04, 0x40 }, { 0x05, 0x00 },
-	{ 0x28, 0x00 }, { 0x29, 0x10 },
-	{ 0x28, 0x05 }, { 0x29, 0x02 },
+	{ 0x28, 0x00 }, { 0x2b, 0x08 },
+	{ 0x28, 0x05 }, { 0x2b, 0x00 },
 	{ 0x1c, 0x01 },
-	{ 0x28, 0x06 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x03 },
-	{ 0x28, 0x07 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x0d },
-	{ 0x28, 0x08 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x02 },
-	{ 0x28, 0x09 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x01 },
-	{ 0x28, 0x0a }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x21 },
-	{ 0x28, 0x0b }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x29 },
-	{ 0x28, 0x0c }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x16 },
-	{ 0x28, 0x0d }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x31 },
-	{ 0x28, 0x0e }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x0e },
-	{ 0x28, 0x0f }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x4e },
-	{ 0x28, 0x10 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x46 },
-	{ 0x28, 0x11 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x0f },
-	{ 0x28, 0x12 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x56 },
-	{ 0x28, 0x13 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x35 },
-	{ 0x28, 0x14 }, { 0x29, 0x00 }, { 0x2a, 0x01 }, { 0x2b, 0xbe },
-	{ 0x28, 0x15 }, { 0x29, 0x00 }, { 0x2a, 0x01 }, { 0x2b, 0x84 },
-	{ 0x28, 0x16 }, { 0x29, 0x00 }, { 0x2a, 0x03 }, { 0x2b, 0xee },
-	{ 0x28, 0x17 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x98 },
-	{ 0x28, 0x18 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x9f },
-	{ 0x28, 0x19 }, { 0x29, 0x00 }, { 0x2a, 0x07 }, { 0x2b, 0xb2 },
-	{ 0x28, 0x1a }, { 0x29, 0x00 }, { 0x2a, 0x06 }, { 0x2b, 0xc2 },
-	{ 0x28, 0x1b }, { 0x29, 0x00 }, { 0x2a, 0x07 }, { 0x2b, 0x4a },
-	{ 0x28, 0x1c }, { 0x29, 0x00 }, { 0x2a, 0x01 }, { 0x2b, 0xbc },
-	{ 0x28, 0x1d }, { 0x29, 0x00 }, { 0x2a, 0x04 }, { 0x2b, 0xba },
-	{ 0x28, 0x1e }, { 0x29, 0x00 }, { 0x2a, 0x06 }, { 0x2b, 0x14 },
+	{ 0x28, 0x06 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x1f },
+	{ 0x28, 0x07 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x18 },
+	{ 0x28, 0x08 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x12 },
+	{ 0x28, 0x09 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x30 },
+	{ 0x28, 0x0a }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x37 },
+	{ 0x28, 0x0b }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x02 },
+	{ 0x28, 0x0c }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x09 },
+	{ 0x28, 0x0d }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x06 },
+	{ 0x28, 0x0e }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x7b },
+	{ 0x28, 0x0f }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x76 },
+	{ 0x28, 0x10 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x7d },
+	{ 0x28, 0x11 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x08 },
+	{ 0x28, 0x12 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x0b },
+	{ 0x28, 0x13 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x00 },
+	{ 0x28, 0x14 }, { 0x29, 0x00 }, { 0x2a, 0x01 }, { 0x2b, 0xf2 },
+	{ 0x28, 0x15 }, { 0x29, 0x00 }, { 0x2a, 0x01 }, { 0x2b, 0xf3 },
+	{ 0x28, 0x16 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x05 },
+	{ 0x28, 0x17 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x16 },
+	{ 0x28, 0x18 }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x0f },
+	{ 0x28, 0x19 }, { 0x29, 0x00 }, { 0x2a, 0x07 }, { 0x2b, 0xef },
+	{ 0x28, 0x1a }, { 0x29, 0x00 }, { 0x2a, 0x07 }, { 0x2b, 0xd8 },
+	{ 0x28, 0x1b }, { 0x29, 0x00 }, { 0x2a, 0x07 }, { 0x2b, 0xf1 },
+	{ 0x28, 0x1c }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x3d },
+	{ 0x28, 0x1d }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x94 },
+	{ 0x28, 0x1e }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0xba },
 	{ 0x50, 0x1e }, { 0x51, 0x5d },
 	{ 0x50, 0x22 }, { 0x51, 0x00 },
 	{ 0x50, 0x23 }, { 0x51, 0xc8 },
@@ -183,6 +201,8 @@ static struct regdata mb86a20s_init[] = {
 	{ 0x50, 0x26 }, { 0x51, 0x00 },
 	{ 0x50, 0x27 }, { 0x51, 0xc3 },
 	{ 0x50, 0x39 }, { 0x51, 0x02 },
+	{ 0xec, 0x0f },
+	{ 0xeb, 0x1f },
 	{ 0x28, 0x6a }, { 0x29, 0x00 }, { 0x2a, 0x00 }, { 0x2b, 0x00 },
 	{ 0xd0, 0x00 },
 };
@@ -308,15 +328,23 @@ static int mb86a20s_read_status(struct dvb_frontend *fe, fe_status_t *status)
 	dev_dbg(&state->i2c->dev, "%s: Status = 0x%02x (state = %d)\n",
 		 __func__, *status, val);
 
-	return 0;
+	return val;
 }
 
 static int mb86a20s_read_signal_strength(struct dvb_frontend *fe)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int rc;
 	unsigned rf_max, rf_min, rf;
 
+	if (state->get_strength_time &&
+	   (!time_after(jiffies, state->get_strength_time)))
+		return c->strength.stat[0].uvalue;
+
+	/* Reset its value if an error happen */
+	c->strength.stat[0].uvalue = 0;
+
 	/* Does a binary search to get RF strength */
 	rf_max = 0xfff;
 	rf_min = 0;
@@ -331,7 +359,7 @@ static int mb86a20s_read_signal_strength(struct dvb_frontend *fe)
 		rc = mb86a20s_writereg(state, 0x04, 0x20);
 		if (rc < 0)
 			return rc;
-		rc = mb86a20s_writereg(state, 0x04, rf);
+		rc = mb86a20s_writereg(state, 0x05, rf);
 		if (rc < 0)
 			return rc;
 
@@ -346,15 +374,19 @@ static int mb86a20s_read_signal_strength(struct dvb_frontend *fe)
 			rf = (rf_max + rf_min) / 2;
 
 			/* Rescale it from 2^12 (4096) to 2^16 */
-			rf <<= (16 - 12);
+			rf = rf << (16 - 12);
+			if (rf)
+				rf |= (1 << 12) - 1;
+
 			dev_dbg(&state->i2c->dev,
 				"%s: signal strength = %d (%d < RF=%d < %d)\n",
 				__func__, rf, rf_min, rf >> 4, rf_max);
-			return rf;
+			c->strength.stat[0].uvalue = rf;
+			state->get_strength_time = jiffies +
+						   msecs_to_jiffies(1000);
+			return 0;
 		}
 	} while (1);
-
-	return 0;
 }
 
 static int mb86a20s_get_modulation(struct mb86a20s_state *state,
@@ -534,12 +566,13 @@ static u32 isdbt_rate[3][5][4] = {
 };
 
 static void mb86a20s_layer_bitrate(struct dvb_frontend *fe, u32 layer,
-				   u32 modulation, u32 fec, u32 interleaving,
+				   u32 modulation, u32 forward_error_correction,
+				   u32 interleaving,
 				   u32 segment)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
 	u32 rate;
-	int m, f, i;
+	int mod, fec, guard;
 
 	/*
 	 * If modulation/fec/interleaving is not detected, the default is
@@ -550,54 +583,54 @@ static void mb86a20s_layer_bitrate(struct dvb_frontend *fe, u32 layer,
 	case DQPSK:
 	case QPSK:
 	default:
-		m = 0;
+		mod = 0;
 		break;
 	case QAM_16:
-		m = 1;
+		mod = 1;
 		break;
 	case QAM_64:
-		m = 2;
+		mod = 2;
 		break;
 	}
 
-	switch (fec) {
+	switch (forward_error_correction) {
 	default:
 	case FEC_1_2:
 	case FEC_AUTO:
-		f = 0;
+		fec = 0;
 		break;
 	case FEC_2_3:
-		f = 1;
+		fec = 1;
 		break;
 	case FEC_3_4:
-		f = 2;
+		fec = 2;
 		break;
 	case FEC_5_6:
-		f = 3;
+		fec = 3;
 		break;
 	case FEC_7_8:
-		f = 4;
+		fec = 4;
 		break;
 	}
 
 	switch (interleaving) {
 	default:
 	case GUARD_INTERVAL_1_4:
-		i = 0;
+		guard = 0;
 		break;
 	case GUARD_INTERVAL_1_8:
-		i = 1;
+		guard = 1;
 		break;
 	case GUARD_INTERVAL_1_16:
-		i = 2;
+		guard = 2;
 		break;
 	case GUARD_INTERVAL_1_32:
-		i = 3;
+		guard = 3;
 		break;
 	}
 
 	/* Samples BER at BER_SAMPLING_RATE seconds */
-	rate = isdbt_rate[m][f][i] * segment * BER_SAMPLING_RATE;
+	rate = isdbt_rate[mod][fec][guard] * segment * BER_SAMPLING_RATE;
 
 	/* Avoids sampling too quickly or to overflow the register */
 	if (rate < 256)
@@ -607,18 +640,18 @@ static void mb86a20s_layer_bitrate(struct dvb_frontend *fe, u32 layer,
 
 	dev_dbg(&state->i2c->dev,
 		"%s: layer %c bitrate: %d kbps; counter = %d (0x%06x)\n",
-	       __func__, 'A' + layer, segment * isdbt_rate[m][f][i]/1000,
+		__func__, 'A' + layer,
+		segment * isdbt_rate[mod][fec][guard]/1000,
 		rate, rate);
 
 	state->estimated_rate[layer] = rate;
 }
 
-
 static int mb86a20s_get_frontend(struct dvb_frontend *fe)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int i, rc;
+	int layer, rc;
 
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
@@ -636,43 +669,43 @@ static int mb86a20s_get_frontend(struct dvb_frontend *fe)
 
 	/* Get per-layer data */
 
-	for (i = 0; i < 3; i++) {
+	for (layer = 0; layer < NUM_LAYERS; layer++) {
 		dev_dbg(&state->i2c->dev, "%s: getting data for layer %c.\n",
-			__func__, 'A' + i);
+			__func__, 'A' + layer);
 
-		rc = mb86a20s_get_segment_count(state, i);
+		rc = mb86a20s_get_segment_count(state, layer);
 		if (rc < 0)
 			goto noperlayer_error;
 		if (rc >= 0 && rc < 14) {
-			c->layer[i].segment_count = rc;
+			c->layer[layer].segment_count = rc;
 		} else {
-			c->layer[i].segment_count = 0;
-			state->estimated_rate[i] = 0;
+			c->layer[layer].segment_count = 0;
+			state->estimated_rate[layer] = 0;
 			continue;
 		}
-		c->isdbt_layer_enabled |= 1 << i;
-		rc = mb86a20s_get_modulation(state, i);
+		c->isdbt_layer_enabled |= 1 << layer;
+		rc = mb86a20s_get_modulation(state, layer);
 		if (rc < 0)
 			goto noperlayer_error;
 		dev_dbg(&state->i2c->dev, "%s: modulation %d.\n",
 			__func__, rc);
-		c->layer[i].modulation = rc;
-		rc = mb86a20s_get_fec(state, i);
+		c->layer[layer].modulation = rc;
+		rc = mb86a20s_get_fec(state, layer);
 		if (rc < 0)
 			goto noperlayer_error;
 		dev_dbg(&state->i2c->dev, "%s: FEC %d.\n",
 			__func__, rc);
-		c->layer[i].fec = rc;
-		rc = mb86a20s_get_interleaving(state, i);
+		c->layer[layer].fec = rc;
+		rc = mb86a20s_get_interleaving(state, layer);
 		if (rc < 0)
 			goto noperlayer_error;
 		dev_dbg(&state->i2c->dev, "%s: interleaving %d.\n",
 			__func__, rc);
-		c->layer[i].interleaving = rc;
-		mb86a20s_layer_bitrate(fe, i, c->layer[i].modulation,
-				       c->layer[i].fec,
-				       c->layer[i].interleaving,
-				       c->layer[i].segment_count);
+		c->layer[layer].interleaving = rc;
+		mb86a20s_layer_bitrate(fe, layer, c->layer[layer].modulation,
+				       c->layer[layer].fec,
+				       c->layer[layer].interleaving,
+				       c->layer[layer].segment_count);
 	}
 
 	rc = mb86a20s_writereg(state, 0x6d, 0x84);
@@ -735,7 +768,6 @@ static int mb86a20s_reset_counters(struct dvb_frontend *fe)
 
 	/* Reset the counters, if the channel changed */
 	if (state->last_frequency != c->frequency) {
-		memset(&c->strength, 0, sizeof(c->strength));
 		memset(&c->cnr, 0, sizeof(c->cnr));
 		memset(&c->pre_bit_error, 0, sizeof(c->pre_bit_error));
 		memset(&c->pre_bit_count, 0, sizeof(c->pre_bit_count));
@@ -799,7 +831,7 @@ static int mb86a20s_get_pre_ber(struct dvb_frontend *fe,
 
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
-	if (layer >= 3)
+	if (layer >= NUM_LAYERS)
 		return -EINVAL;
 
 	/* Check if the BER measures are already available */
@@ -933,7 +965,7 @@ static int mb86a20s_get_post_ber(struct dvb_frontend *fe,
 
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
-	if (layer >= 3)
+	if (layer >= NUM_LAYERS)
 		return -EINVAL;
 
 	/* Check if the BER measures are already available */
@@ -1060,7 +1092,7 @@ static int mb86a20s_get_blk_error(struct dvb_frontend *fe,
 	u32 collect_rate;
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
-	if (layer >= 3)
+	if (layer >= NUM_LAYERS)
 		return -EINVAL;
 
 	/* Check if the PER measures are already available */
@@ -1095,7 +1127,7 @@ static int mb86a20s_get_blk_error(struct dvb_frontend *fe,
 	if (rc < 0)
 		return rc;
 	*error |= rc;
-	dev_err(&state->i2c->dev, "%s: block error for layer %c: %d.\n",
+	dev_dbg(&state->i2c->dev, "%s: block error for layer %c: %d.\n",
 		__func__, 'A' + layer, *error);
 
 	/* Read Bit Count */
@@ -1386,7 +1418,7 @@ static int mb86a20s_get_main_CNR(struct dvb_frontend *fe)
 		return rc;
 
 	if (!(rc & 0x40)) {
-		dev_info(&state->i2c->dev, "%s: CNR is not available yet.\n",
+		dev_dbg(&state->i2c->dev, "%s: CNR is not available yet.\n",
 			 __func__);
 		return -EBUSY;
 	}
@@ -1425,7 +1457,7 @@ static int mb86a20s_get_blk_error_layer_CNR(struct dvb_frontend *fe)
 	struct mb86a20s_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	u32 mer, cnr;
-	int rc, val, i;
+	int rc, val, layer;
 	struct linear_segments *segs;
 	unsigned segs_len;
 
@@ -1441,33 +1473,33 @@ static int mb86a20s_get_blk_error_layer_CNR(struct dvb_frontend *fe)
 
 	/* Check if data is available */
 	if (!(rc & 0x01)) {
-		dev_info(&state->i2c->dev,
+		dev_dbg(&state->i2c->dev,
 			"%s: MER measures aren't available yet.\n", __func__);
 		return -EBUSY;
 	}
 
 	/* Read all layers */
-	for (i = 0; i < 3; i++) {
-		if (!(c->isdbt_layer_enabled & (1 << i))) {
-			c->cnr.stat[1 + i].scale = FE_SCALE_NOT_AVAILABLE;
+	for (layer = 0; layer < NUM_LAYERS; layer++) {
+		if (!(c->isdbt_layer_enabled & (1 << layer))) {
+			c->cnr.stat[1 + layer].scale = FE_SCALE_NOT_AVAILABLE;
 			continue;
 		}
 
-		rc = mb86a20s_writereg(state, 0x50, 0x52 + i * 3);
+		rc = mb86a20s_writereg(state, 0x50, 0x52 + layer * 3);
 		if (rc < 0)
 			return rc;
 		rc = mb86a20s_readreg(state, 0x51);
 		if (rc < 0)
 			return rc;
 		mer = rc << 16;
-		rc = mb86a20s_writereg(state, 0x50, 0x53 + i * 3);
+		rc = mb86a20s_writereg(state, 0x50, 0x53 + layer * 3);
 		if (rc < 0)
 			return rc;
 		rc = mb86a20s_readreg(state, 0x51);
 		if (rc < 0)
 			return rc;
 		mer |= rc << 8;
-		rc = mb86a20s_writereg(state, 0x50, 0x54 + i * 3);
+		rc = mb86a20s_writereg(state, 0x50, 0x54 + layer * 3);
 		if (rc < 0)
 			return rc;
 		rc = mb86a20s_readreg(state, 0x51);
@@ -1475,7 +1507,7 @@ static int mb86a20s_get_blk_error_layer_CNR(struct dvb_frontend *fe)
 			return rc;
 		mer |= rc;
 
-		switch (c->layer[i].modulation) {
+		switch (c->layer[layer].modulation) {
 		case DQPSK:
 		case QPSK:
 			segs = cnr_qpsk_table;
@@ -1493,12 +1525,12 @@ static int mb86a20s_get_blk_error_layer_CNR(struct dvb_frontend *fe)
 		}
 		cnr = interpolate_value(mer, segs, segs_len);
 
-		c->cnr.stat[1 + i].scale = FE_SCALE_DECIBEL;
-		c->cnr.stat[1 + i].svalue = cnr;
+		c->cnr.stat[1 + layer].scale = FE_SCALE_DECIBEL;
+		c->cnr.stat[1 + layer].svalue = cnr;
 
 		dev_dbg(&state->i2c->dev,
 			"%s: CNR for layer %c is %d.%03d dB (MER = %d).\n",
-			__func__, 'A' + i, cnr / 1000, cnr % 1000, mer);
+			__func__, 'A' + layer, cnr / 1000, cnr % 1000, mer);
 
 	}
 
@@ -1526,7 +1558,7 @@ static void mb86a20s_stats_not_ready(struct dvb_frontend *fe)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int i;
+	int layer;
 
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
@@ -1536,35 +1568,35 @@ static void mb86a20s_stats_not_ready(struct dvb_frontend *fe)
 	c->strength.len = 1;
 
 	/* Per-layer stats - 3 layers + global */
-	c->cnr.len = 4;
-	c->pre_bit_error.len = 4;
-	c->pre_bit_count.len = 4;
-	c->post_bit_error.len = 4;
-	c->post_bit_count.len = 4;
-	c->block_error.len = 4;
-	c->block_count.len = 4;
+	c->cnr.len = NUM_LAYERS + 1;
+	c->pre_bit_error.len = NUM_LAYERS + 1;
+	c->pre_bit_count.len = NUM_LAYERS + 1;
+	c->post_bit_error.len = NUM_LAYERS + 1;
+	c->post_bit_count.len = NUM_LAYERS + 1;
+	c->block_error.len = NUM_LAYERS + 1;
+	c->block_count.len = NUM_LAYERS + 1;
 
 	/* Signal is always available */
 	c->strength.stat[0].scale = FE_SCALE_RELATIVE;
 	c->strength.stat[0].uvalue = 0;
 
 	/* Put all of them at FE_SCALE_NOT_AVAILABLE */
-	for (i = 0; i < 4; i++) {
-		c->cnr.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
-		c->pre_bit_error.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
-		c->pre_bit_count.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
-		c->post_bit_error.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
-		c->post_bit_count.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
-		c->block_error.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
-		c->block_count.stat[i].scale = FE_SCALE_NOT_AVAILABLE;
+	for (layer = 0; layer < NUM_LAYERS + 1; layer++) {
+		c->cnr.stat[layer].scale = FE_SCALE_NOT_AVAILABLE;
+		c->pre_bit_error.stat[layer].scale = FE_SCALE_NOT_AVAILABLE;
+		c->pre_bit_count.stat[layer].scale = FE_SCALE_NOT_AVAILABLE;
+		c->post_bit_error.stat[layer].scale = FE_SCALE_NOT_AVAILABLE;
+		c->post_bit_count.stat[layer].scale = FE_SCALE_NOT_AVAILABLE;
+		c->block_error.stat[layer].scale = FE_SCALE_NOT_AVAILABLE;
+		c->block_count.stat[layer].scale = FE_SCALE_NOT_AVAILABLE;
 	}
 }
 
-static int mb86a20s_get_stats(struct dvb_frontend *fe)
+static int mb86a20s_get_stats(struct dvb_frontend *fe, int status_nr)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int rc = 0, i;
+	int rc = 0, layer;
 	u32 bit_error = 0, bit_count = 0;
 	u32 t_pre_bit_error = 0, t_pre_bit_count = 0;
 	u32 t_post_bit_error = 0, t_post_bit_count = 0;
@@ -1580,90 +1612,98 @@ static int mb86a20s_get_stats(struct dvb_frontend *fe)
 	/* Get per-layer stats */
 	mb86a20s_get_blk_error_layer_CNR(fe);
 
-	for (i = 0; i < 3; i++) {
-		if (c->isdbt_layer_enabled & (1 << i)) {
+	/*
+	 * At state 7, only CNR is available
+	 * For BER measures, state=9 is required
+	 * FIXME: we may get MER measures with state=8
+	 */
+	if (status_nr < 9)
+		return 0;
+
+	for (layer = 0; layer < NUM_LAYERS; layer++) {
+		if (c->isdbt_layer_enabled & (1 << layer)) {
 			/* Layer is active and has rc segments */
 			active_layers++;
 
 			/* Handle BER before vterbi */
-			rc = mb86a20s_get_pre_ber(fe, i,
+			rc = mb86a20s_get_pre_ber(fe, layer,
 						  &bit_error, &bit_count);
 			if (rc >= 0) {
-				c->pre_bit_error.stat[1 + i].scale = FE_SCALE_COUNTER;
-				c->pre_bit_error.stat[1 + i].uvalue += bit_error;
-				c->pre_bit_count.stat[1 + i].scale = FE_SCALE_COUNTER;
-				c->pre_bit_count.stat[1 + i].uvalue += bit_count;
+				c->pre_bit_error.stat[1 + layer].scale = FE_SCALE_COUNTER;
+				c->pre_bit_error.stat[1 + layer].uvalue += bit_error;
+				c->pre_bit_count.stat[1 + layer].scale = FE_SCALE_COUNTER;
+				c->pre_bit_count.stat[1 + layer].uvalue += bit_count;
 			} else if (rc != -EBUSY) {
 				/*
 					* If an I/O error happened,
 					* measures are now unavailable
 					*/
-				c->pre_bit_error.stat[1 + i].scale = FE_SCALE_NOT_AVAILABLE;
-				c->pre_bit_count.stat[1 + i].scale = FE_SCALE_NOT_AVAILABLE;
+				c->pre_bit_error.stat[1 + layer].scale = FE_SCALE_NOT_AVAILABLE;
+				c->pre_bit_count.stat[1 + layer].scale = FE_SCALE_NOT_AVAILABLE;
 				dev_err(&state->i2c->dev,
 					"%s: Can't get BER for layer %c (error %d).\n",
-					__func__, 'A' + i, rc);
+					__func__, 'A' + layer, rc);
 			}
-			if (c->block_error.stat[1 + i].scale != FE_SCALE_NOT_AVAILABLE)
+			if (c->block_error.stat[1 + layer].scale != FE_SCALE_NOT_AVAILABLE)
 				pre_ber_layers++;
 
 			/* Handle BER post vterbi */
-			rc = mb86a20s_get_post_ber(fe, i,
+			rc = mb86a20s_get_post_ber(fe, layer,
 						   &bit_error, &bit_count);
 			if (rc >= 0) {
-				c->post_bit_error.stat[1 + i].scale = FE_SCALE_COUNTER;
-				c->post_bit_error.stat[1 + i].uvalue += bit_error;
-				c->post_bit_count.stat[1 + i].scale = FE_SCALE_COUNTER;
-				c->post_bit_count.stat[1 + i].uvalue += bit_count;
+				c->post_bit_error.stat[1 + layer].scale = FE_SCALE_COUNTER;
+				c->post_bit_error.stat[1 + layer].uvalue += bit_error;
+				c->post_bit_count.stat[1 + layer].scale = FE_SCALE_COUNTER;
+				c->post_bit_count.stat[1 + layer].uvalue += bit_count;
 			} else if (rc != -EBUSY) {
 				/*
 					* If an I/O error happened,
 					* measures are now unavailable
 					*/
-				c->post_bit_error.stat[1 + i].scale = FE_SCALE_NOT_AVAILABLE;
-				c->post_bit_count.stat[1 + i].scale = FE_SCALE_NOT_AVAILABLE;
+				c->post_bit_error.stat[1 + layer].scale = FE_SCALE_NOT_AVAILABLE;
+				c->post_bit_count.stat[1 + layer].scale = FE_SCALE_NOT_AVAILABLE;
 				dev_err(&state->i2c->dev,
 					"%s: Can't get BER for layer %c (error %d).\n",
-					__func__, 'A' + i, rc);
+					__func__, 'A' + layer, rc);
 			}
-			if (c->block_error.stat[1 + i].scale != FE_SCALE_NOT_AVAILABLE)
+			if (c->block_error.stat[1 + layer].scale != FE_SCALE_NOT_AVAILABLE)
 				post_ber_layers++;
 
 			/* Handle Block errors for PER/UCB reports */
-			rc = mb86a20s_get_blk_error(fe, i,
+			rc = mb86a20s_get_blk_error(fe, layer,
 						&block_error,
 						&block_count);
 			if (rc >= 0) {
-				c->block_error.stat[1 + i].scale = FE_SCALE_COUNTER;
-				c->block_error.stat[1 + i].uvalue += block_error;
-				c->block_count.stat[1 + i].scale = FE_SCALE_COUNTER;
-				c->block_count.stat[1 + i].uvalue += block_count;
+				c->block_error.stat[1 + layer].scale = FE_SCALE_COUNTER;
+				c->block_error.stat[1 + layer].uvalue += block_error;
+				c->block_count.stat[1 + layer].scale = FE_SCALE_COUNTER;
+				c->block_count.stat[1 + layer].uvalue += block_count;
 			} else if (rc != -EBUSY) {
 				/*
 					* If an I/O error happened,
 					* measures are now unavailable
 					*/
-				c->block_error.stat[1 + i].scale = FE_SCALE_NOT_AVAILABLE;
-				c->block_count.stat[1 + i].scale = FE_SCALE_NOT_AVAILABLE;
+				c->block_error.stat[1 + layer].scale = FE_SCALE_NOT_AVAILABLE;
+				c->block_count.stat[1 + layer].scale = FE_SCALE_NOT_AVAILABLE;
 				dev_err(&state->i2c->dev,
 					"%s: Can't get PER for layer %c (error %d).\n",
-					__func__, 'A' + i, rc);
+					__func__, 'A' + layer, rc);
 
 			}
-			if (c->block_error.stat[1 + i].scale != FE_SCALE_NOT_AVAILABLE)
+			if (c->block_error.stat[1 + layer].scale != FE_SCALE_NOT_AVAILABLE)
 				per_layers++;
 
 			/* Update total preBER */
-			t_pre_bit_error += c->pre_bit_error.stat[1 + i].uvalue;
-			t_pre_bit_count += c->pre_bit_count.stat[1 + i].uvalue;
+			t_pre_bit_error += c->pre_bit_error.stat[1 + layer].uvalue;
+			t_pre_bit_count += c->pre_bit_count.stat[1 + layer].uvalue;
 
 			/* Update total postBER */
-			t_post_bit_error += c->post_bit_error.stat[1 + i].uvalue;
-			t_post_bit_count += c->post_bit_count.stat[1 + i].uvalue;
+			t_post_bit_error += c->post_bit_error.stat[1 + layer].uvalue;
+			t_post_bit_count += c->post_bit_count.stat[1 + layer].uvalue;
 
 			/* Update total PER */
-			t_block_error += c->block_error.stat[1 + i].uvalue;
-			t_block_count += c->block_count.stat[1 + i].uvalue;
+			t_block_error += c->block_error.stat[1 + layer].uvalue;
+			t_block_count += c->block_count.stat[1 + layer].uvalue;
 		}
 	}
 
@@ -1737,8 +1777,10 @@ static int mb86a20s_get_stats(struct dvb_frontend *fe)
 static int mb86a20s_initfe(struct dvb_frontend *fe)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
+	u64 pll;
+	u32 fclk;
 	int rc;
-	u8  regD5 = 1;
+	u8  regD5 = 1, reg71, reg09 = 0x3a;
 
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
@@ -1746,10 +1788,78 @@ static int mb86a20s_initfe(struct dvb_frontend *fe)
 		fe->ops.i2c_gate_ctrl(fe, 0);
 
 	/* Initialize the frontend */
-	rc = mb86a20s_writeregdata(state, mb86a20s_init);
+	rc = mb86a20s_writeregdata(state, mb86a20s_init1);
 	if (rc < 0)
 		goto err;
 
+	if (!state->inversion)
+		reg09 |= 0x04;
+	rc = mb86a20s_writereg(state, 0x09, reg09);
+	if (rc < 0)
+		goto err;
+	if (!state->bw)
+		reg71 = 1;
+	else
+		reg71 = 0;
+	rc = mb86a20s_writereg(state, 0x39, reg71);
+	if (rc < 0)
+		goto err;
+	rc = mb86a20s_writereg(state, 0x71, state->bw);
+	if (rc < 0)
+		goto err;
+	if (state->subchannel) {
+		rc = mb86a20s_writereg(state, 0x44, state->subchannel);
+		if (rc < 0)
+			goto err;
+	}
+
+	fclk = state->config->fclk;
+	if (!fclk)
+		fclk = 32571428;
+
+	/* Adjust IF frequency to match tuner */
+	if (fe->ops.tuner_ops.get_if_frequency)
+		fe->ops.tuner_ops.get_if_frequency(fe, &state->if_freq);
+
+	if (!state->if_freq)
+		state->if_freq = 3300000;
+
+	pll = (((u64)1) << 34) * state->if_freq;
+	do_div(pll, 63 * fclk);
+	pll = (1 << 25) - pll;
+	rc = mb86a20s_writereg(state, 0x28, 0x2a);
+	if (rc < 0)
+		goto err;
+	rc = mb86a20s_writereg(state, 0x29, (pll >> 16) & 0xff);
+	if (rc < 0)
+		goto err;
+	rc = mb86a20s_writereg(state, 0x2a, (pll >> 8) & 0xff);
+	if (rc < 0)
+		goto err;
+	rc = mb86a20s_writereg(state, 0x2b, pll & 0xff);
+	if (rc < 0)
+		goto err;
+	dev_dbg(&state->i2c->dev, "%s: fclk=%d, IF=%d, clock reg=0x%06llx\n",
+		__func__, fclk, state->if_freq, (long long)pll);
+
+	/* pll = freq[Hz] * 2^24/10^6 / 16.285714286 */
+	pll = state->if_freq * 1677721600L;
+	do_div(pll, 1628571429L);
+	rc = mb86a20s_writereg(state, 0x28, 0x20);
+	if (rc < 0)
+		goto err;
+	rc = mb86a20s_writereg(state, 0x29, (pll >> 16) & 0xff);
+	if (rc < 0)
+		goto err;
+	rc = mb86a20s_writereg(state, 0x2a, (pll >> 8) & 0xff);
+	if (rc < 0)
+		goto err;
+	rc = mb86a20s_writereg(state, 0x2b, pll & 0xff);
+	if (rc < 0)
+		goto err;
+	dev_dbg(&state->i2c->dev, "%s: IF=%d, IF reg=0x%06llx\n",
+		__func__, state->if_freq, (long long)pll);
+
 	if (!state->config->is_serial) {
 		regD5 &= ~1;
 
@@ -1761,6 +1871,11 @@ static int mb86a20s_initfe(struct dvb_frontend *fe)
 			goto err;
 	}
 
+	rc = mb86a20s_writeregdata(state, mb86a20s_init2);
+	if (rc < 0)
+		goto err;
+
+
 err:
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 1);
@@ -1779,15 +1894,34 @@ err:
 static int mb86a20s_set_frontend(struct dvb_frontend *fe)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
-	int rc;
-#if 0
-	/*
-	 * FIXME: Properly implement the set frontend properties
-	 */
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-#endif
+	int rc, if_freq;
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
+	if (!c->isdbt_layer_enabled)
+		c->isdbt_layer_enabled = 7;
+
+	if (c->isdbt_layer_enabled == 1)
+		state->bw = MB86A20S_1SEG;
+	else if (c->isdbt_partial_reception)
+		state->bw = MB86A20S_13SEG_PARTIAL;
+	else
+		state->bw = MB86A20S_13SEG;
+
+	if (c->inversion == INVERSION_ON)
+		state->inversion = true;
+	else
+		state->inversion = false;
+
+	if (!c->isdbt_sb_mode) {
+		state->subchannel = 0;
+	} else {
+		if (c->isdbt_sb_subchannel >= ARRAY_SIZE(mb86a20s_subchannel))
+			c->isdbt_sb_subchannel = 0;
+
+		state->subchannel = mb86a20s_subchannel[c->isdbt_sb_subchannel];
+	}
+
 	/*
 	 * Gate should already be opened, but it doesn't hurt to
 	 * double-check
@@ -1796,6 +1930,9 @@ static int mb86a20s_set_frontend(struct dvb_frontend *fe)
 		fe->ops.i2c_gate_ctrl(fe, 1);
 	fe->ops.tuner_ops.set_params(fe);
 
+	if (fe->ops.tuner_ops.get_if_frequency)
+		fe->ops.tuner_ops.get_if_frequency(fe, &if_freq);
+
 	/*
 	 * Make it more reliable: if, for some reason, the initial
 	 * device initialization doesn't happen, initialize it when
@@ -1805,15 +1942,22 @@ static int mb86a20s_set_frontend(struct dvb_frontend *fe)
 	 * the agc callback logic is not called during DVB attach time,
 	 * causing mb86a20s to not be initialized with Kworld SBTVD.
 	 * So, this hack is needed, in order to make Kworld SBTVD to work.
+	 *
+	 * It is also needed to change the IF after the initial init.
+	 *
+	 * HACK: Always init the frontend when set_frontend is called:
+	 * it was noticed that, on some devices, it fails to lock on a
+	 * different channel. So, it is better to reset everything, even
+	 * wasting some time, than to loose channel lock.
 	 */
-	if (state->need_init)
-		mb86a20s_initfe(fe);
+	mb86a20s_initfe(fe);
 
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 0);
 
 	rc = mb86a20s_writeregdata(state, mb86a20s_reset_reception);
 	mb86a20s_reset_counters(fe);
+	mb86a20s_stats_not_ready(fe);
 
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 1);
@@ -1825,8 +1969,7 @@ static int mb86a20s_read_status_and_stats(struct dvb_frontend *fe,
 					  fe_status_t *status)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
-	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int rc;
+	int rc, status_nr;
 
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
 
@@ -1834,12 +1977,12 @@ static int mb86a20s_read_status_and_stats(struct dvb_frontend *fe,
 		fe->ops.i2c_gate_ctrl(fe, 0);
 
 	/* Get lock */
-	rc = mb86a20s_read_status(fe, status);
-	if (!(*status & FE_HAS_LOCK)) {
+	status_nr = mb86a20s_read_status(fe, status);
+	if (status_nr < 7) {
 		mb86a20s_stats_not_ready(fe);
 		mb86a20s_reset_frontend_cache(fe);
 	}
-	if (rc < 0) {
+	if (status_nr < 0) {
 		dev_err(&state->i2c->dev,
 			"%s: Can't read frontend lock status\n", __func__);
 		goto error;
@@ -1856,10 +1999,8 @@ static int mb86a20s_read_status_and_stats(struct dvb_frontend *fe,
 		rc = 0;		/* Status is OK */
 		goto error;
 	}
-	/* Fill signal strength */
-	c->strength.stat[0].uvalue = rc;
 
-	if (*status & FE_HAS_LOCK) {
+	if (status_nr >= 7) {
 		/* Get TMCC info*/
 		rc = mb86a20s_get_frontend(fe);
 		if (rc < 0) {
@@ -1870,7 +2011,7 @@ static int mb86a20s_read_status_and_stats(struct dvb_frontend *fe,
 		}
 
 		/* Get statistics */
-		rc = mb86a20s_get_stats(fe);
+		rc = mb86a20s_get_stats(fe, status_nr);
 		if (rc < 0 && rc != -EBUSY) {
 			dev_err(&state->i2c->dev,
 				"%s: Can't get FE statistics.\n", __func__);
@@ -1994,7 +2135,7 @@ static struct dvb_frontend_ops mb86a20s_ops = {
 	/* Use dib8000 values per default */
 	.info = {
 		.name = "Fujitsu mb86A20s",
-		.caps = FE_CAN_INVERSION_AUTO | FE_CAN_RECOVER |
+		.caps = FE_CAN_RECOVER  |
 			FE_CAN_FEC_1_2  | FE_CAN_FEC_2_3 | FE_CAN_FEC_3_4 |
 			FE_CAN_FEC_5_6  | FE_CAN_FEC_7_8 | FE_CAN_FEC_AUTO |
 			FE_CAN_QPSK     | FE_CAN_QAM_16  | FE_CAN_QAM_64 |
diff --git a/drivers/media/dvb-frontends/mb86a20s.h b/drivers/media/dvb-frontends/mb86a20s.h
index bf22e77888b9..6627a3976087 100644
--- a/drivers/media/dvb-frontends/mb86a20s.h
+++ b/drivers/media/dvb-frontends/mb86a20s.h
@@ -16,21 +16,25 @@
 #ifndef MB86A20S_H
 #define MB86A20S_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 /**
  * struct mb86a20s_config - Define the per-device attributes of the frontend
  *
+ * @fclk:		Clock frequency. If zero, assumes the default
+ *			(32.57142 Mhz)
  * @demod_address:	the demodulator's i2c address
+ * @is_serial:		if true, TS is serial. Otherwise, TS is parallel
  */
 
 struct mb86a20s_config {
-	u8 demod_address;
-	bool is_serial;
+	u32	fclk;
+	u8	demod_address;
+	bool	is_serial;
 };
 
-#if defined(CONFIG_DVB_MB86A20S) || (defined(CONFIG_DVB_MB86A20S_MODULE) \
-	&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_MB86A20S)
 extern struct dvb_frontend *mb86a20s_attach(const struct mb86a20s_config *config,
 					   struct i2c_adapter *i2c);
 extern struct i2c_adapter *mb86a20s_get_tuner_i2c_adapter(struct dvb_frontend *);
diff --git a/drivers/media/dvb-frontends/rtl2830.h b/drivers/media/dvb-frontends/rtl2830.h
index f4349a1fc03e..3313847fb0be 100644
--- a/drivers/media/dvb-frontends/rtl2830.h
+++ b/drivers/media/dvb-frontends/rtl2830.h
@@ -21,6 +21,7 @@
 #ifndef RTL2830_H
 #define RTL2830_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct rtl2830_config {
@@ -59,8 +60,7 @@ struct rtl2830_config {
 	u8 agc_targ_val;
 };
 
-#if defined(CONFIG_DVB_RTL2830) || \
-	(defined(CONFIG_DVB_RTL2830_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_RTL2830)
 extern struct dvb_frontend *rtl2830_attach(
 	const struct rtl2830_config *config,
 	struct i2c_adapter *i2c
diff --git a/drivers/media/dvb-frontends/rtl2832.c b/drivers/media/dvb-frontends/rtl2832.c
index 73887690b046..facb84841518 100644
--- a/drivers/media/dvb-frontends/rtl2832.c
+++ b/drivers/media/dvb-frontends/rtl2832.c
@@ -380,13 +380,41 @@ err:
 	return ret;
 }
 
-static int rtl2832_init(struct dvb_frontend *fe)
+
+static int rtl2832_set_if(struct dvb_frontend *fe, u32 if_freq)
 {
 	struct rtl2832_priv *priv = fe->demodulator_priv;
-	int i, ret, len;
-	u8 en_bbin;
+	int ret;
 	u64 pset_iffreq;
+	u8 en_bbin = (if_freq == 0 ? 0x1 : 0x0);
+
+	/*
+	* PSET_IFFREQ = - floor((IfFreqHz % CrystalFreqHz) * pow(2, 22)
+	*		/ CrystalFreqHz)
+	*/
+
+	pset_iffreq = if_freq % priv->cfg.xtal;
+	pset_iffreq *= 0x400000;
+	pset_iffreq = div_u64(pset_iffreq, priv->cfg.xtal);
+	pset_iffreq = -pset_iffreq;
+	pset_iffreq = pset_iffreq & 0x3fffff;
+	dev_dbg(&priv->i2c->dev, "%s: if_frequency=%d pset_iffreq=%08x\n",
+			__func__, if_freq, (unsigned)pset_iffreq);
+
+	ret = rtl2832_wr_demod_reg(priv, DVBT_EN_BBIN, en_bbin);
+	if (ret)
+		return ret;
+
+	ret = rtl2832_wr_demod_reg(priv, DVBT_PSET_IFFREQ, pset_iffreq);
+
+	return (ret);
+}
+
+static int rtl2832_init(struct dvb_frontend *fe)
+{
+	struct rtl2832_priv *priv = fe->demodulator_priv;
 	const struct rtl2832_reg_value *init;
+	int i, ret, len;
 
 	/* initialization values for the demodulator registers */
 	struct rtl2832_reg_value rtl2832_initial_regs[] = {
@@ -432,22 +460,10 @@ static int rtl2832_init(struct dvb_frontend *fe)
 		{DVBT_TR_THD_SET2,		0x6},
 		{DVBT_TRK_KC_I2,		0x5},
 		{DVBT_CR_THD_SET2,		0x1},
-		{DVBT_SPEC_INV,			0x0},
 	};
 
 	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
 
-	en_bbin = (priv->cfg.if_dvbt == 0 ? 0x1 : 0x0);
-
-	/*
-	* PSET_IFFREQ = - floor((IfFreqHz % CrystalFreqHz) * pow(2, 22)
-	*		/ CrystalFreqHz)
-	*/
-	pset_iffreq = priv->cfg.if_dvbt % priv->cfg.xtal;
-	pset_iffreq *= 0x400000;
-	pset_iffreq = div_u64(pset_iffreq, priv->cfg.xtal);
-	pset_iffreq = pset_iffreq & 0x3fffff;
-
 	for (i = 0; i < ARRAY_SIZE(rtl2832_initial_regs); i++) {
 		ret = rtl2832_wr_demod_reg(priv, rtl2832_initial_regs[i].reg,
 			rtl2832_initial_regs[i].value);
@@ -472,6 +488,10 @@ static int rtl2832_init(struct dvb_frontend *fe)
 		len = ARRAY_SIZE(rtl2832_tuner_init_e4000);
 		init = rtl2832_tuner_init_e4000;
 		break;
+	case RTL2832_TUNER_R820T:
+		len = ARRAY_SIZE(rtl2832_tuner_init_r820t);
+		init = rtl2832_tuner_init_r820t;
+		break;
 	default:
 		ret = -EINVAL;
 		goto err;
@@ -483,14 +503,26 @@ static int rtl2832_init(struct dvb_frontend *fe)
 			goto err;
 	}
 
-	/* if frequency settings */
-	ret = rtl2832_wr_demod_reg(priv, DVBT_EN_BBIN, en_bbin);
+	if (!fe->ops.tuner_ops.get_if_frequency) {
+		ret = rtl2832_set_if(fe, priv->cfg.if_dvbt);
 		if (ret)
 			goto err;
+	}
 
-	ret = rtl2832_wr_demod_reg(priv, DVBT_PSET_IFFREQ, pset_iffreq);
-		if (ret)
-			goto err;
+	/*
+	 * r820t NIM code does a software reset here at the demod -
+	 * may not be needed, as there's already a software reset at set_params()
+	 */
+#if 1
+	/* soft reset */
+	ret = rtl2832_wr_demod_reg(priv, DVBT_SOFT_RST, 0x1);
+	if (ret)
+		goto err;
+
+	ret = rtl2832_wr_demod_reg(priv, DVBT_SOFT_RST, 0x0);
+	if (ret)
+		goto err;
+#endif
 
 	priv->sleeping = false;
 
@@ -564,6 +596,19 @@ static int rtl2832_set_frontend(struct dvb_frontend *fe)
 	if (fe->ops.tuner_ops.set_params)
 		fe->ops.tuner_ops.set_params(fe);
 
+	/* If the frontend has get_if_frequency(), use it */
+	if (fe->ops.tuner_ops.get_if_frequency) {
+		u32 if_freq;
+
+		ret = fe->ops.tuner_ops.get_if_frequency(fe, &if_freq);
+		if (ret)
+			goto err;
+
+		ret = rtl2832_set_if(fe, if_freq);
+		if (ret)
+			goto err;
+	}
+
 	switch (c->bandwidth_hz) {
 	case 6000000:
 		i = 0;
diff --git a/drivers/media/dvb-frontends/rtl2832.h b/drivers/media/dvb-frontends/rtl2832.h
index 785a466eb065..91b2dcf5a6ea 100644
--- a/drivers/media/dvb-frontends/rtl2832.h
+++ b/drivers/media/dvb-frontends/rtl2832.h
@@ -21,6 +21,7 @@
 #ifndef RTL2832_H
 #define RTL2832_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct rtl2832_config {
@@ -51,11 +52,11 @@ struct rtl2832_config {
 #define RTL2832_TUNER_FC0012    0x26
 #define RTL2832_TUNER_E4000     0x27
 #define RTL2832_TUNER_FC0013    0x29
+#define RTL2832_TUNER_R820T	0x2a
 	u8 tuner;
 };
 
-#if defined(CONFIG_DVB_RTL2832) || \
-	(defined(CONFIG_DVB_RTL2832_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_RTL2832)
 extern struct dvb_frontend *rtl2832_attach(
 	const struct rtl2832_config *cfg,
 	struct i2c_adapter *i2c
diff --git a/drivers/media/dvb-frontends/rtl2832_priv.h b/drivers/media/dvb-frontends/rtl2832_priv.h
index 7d97ce9d2193..b5f2b80092ee 100644
--- a/drivers/media/dvb-frontends/rtl2832_priv.h
+++ b/drivers/media/dvb-frontends/rtl2832_priv.h
@@ -267,6 +267,7 @@ static const struct rtl2832_reg_value rtl2832_tuner_init_tua9001[] = {
 	{DVBT_OPT_ADC_IQ,                0x1},
 	{DVBT_AD_AVI,                    0x0},
 	{DVBT_AD_AVQ,                    0x0},
+	{DVBT_SPEC_INV,			 0x0},
 };
 
 static const struct rtl2832_reg_value rtl2832_tuner_init_fc0012[] = {
@@ -300,6 +301,7 @@ static const struct rtl2832_reg_value rtl2832_tuner_init_fc0012[] = {
 	{DVBT_GI_PGA_STATE,              0x0},
 	{DVBT_EN_AGC_PGA,                0x1},
 	{DVBT_IF_AGC_MAN,                0x0},
+	{DVBT_SPEC_INV,			 0x0},
 };
 
 static const struct rtl2832_reg_value rtl2832_tuner_init_e4000[] = {
@@ -337,6 +339,32 @@ static const struct rtl2832_reg_value rtl2832_tuner_init_e4000[] = {
 	{DVBT_REG_MONSEL,                0x1},
 	{DVBT_REG_MON,                   0x1},
 	{DVBT_REG_4MSEL,                 0x0},
+	{DVBT_SPEC_INV,			 0x0},
+};
+
+static const struct rtl2832_reg_value rtl2832_tuner_init_r820t[] = {
+	{DVBT_DAGC_TRG_VAL,		0x39},
+	{DVBT_AGC_TARG_VAL_0,		0x0},
+	{DVBT_AGC_TARG_VAL_8_1,		0x40},
+	{DVBT_AAGC_LOOP_GAIN,		0x16},
+	{DVBT_LOOP_GAIN2_3_0,		0x8},
+	{DVBT_LOOP_GAIN2_4,		0x1},
+	{DVBT_LOOP_GAIN3,		0x18},
+	{DVBT_VTOP1,			0x35},
+	{DVBT_VTOP2,			0x21},
+	{DVBT_VTOP3,			0x21},
+	{DVBT_KRF1,			0x0},
+	{DVBT_KRF2,			0x40},
+	{DVBT_KRF3,			0x10},
+	{DVBT_KRF4,			0x10},
+	{DVBT_IF_AGC_MIN,		0x80},
+	{DVBT_IF_AGC_MAX,		0x7f},
+	{DVBT_RF_AGC_MIN,		0x80},
+	{DVBT_RF_AGC_MAX,		0x7f},
+	{DVBT_POLAR_RF_AGC,		0x0},
+	{DVBT_POLAR_IF_AGC,		0x0},
+	{DVBT_AD7_SETTING,		0xe9f4},
+	{DVBT_SPEC_INV,			0x1},
 };
 
 #endif /* RTL2832_PRIV_H */
diff --git a/drivers/media/dvb-frontends/s5h1409.h b/drivers/media/dvb-frontends/s5h1409.h
index 91f2ebd1a534..63b1e0a34e4e 100644
--- a/drivers/media/dvb-frontends/s5h1409.h
+++ b/drivers/media/dvb-frontends/s5h1409.h
@@ -22,6 +22,7 @@
 #ifndef __S5H1409_H__
 #define __S5H1409_H__
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct s5h1409_config {
@@ -66,8 +67,7 @@ struct s5h1409_config {
 	u8 hvr1600_opt;
 };
 
-#if defined(CONFIG_DVB_S5H1409) || (defined(CONFIG_DVB_S5H1409_MODULE) \
-	&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_S5H1409)
 extern struct dvb_frontend *s5h1409_attach(const struct s5h1409_config *config,
 					   struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/s5h1411.h b/drivers/media/dvb-frontends/s5h1411.h
index 45ec0f82989c..e4f56871f982 100644
--- a/drivers/media/dvb-frontends/s5h1411.h
+++ b/drivers/media/dvb-frontends/s5h1411.h
@@ -22,6 +22,7 @@
 #ifndef __S5H1411_H__
 #define __S5H1411_H__
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 #define S5H1411_I2C_TOP_ADDR (0x32 >> 1)
@@ -68,8 +69,7 @@ struct s5h1411_config {
 	u8 status_mode;
 };
 
-#if defined(CONFIG_DVB_S5H1411) || \
-	(defined(CONFIG_DVB_S5H1411_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_S5H1411)
 extern struct dvb_frontend *s5h1411_attach(const struct s5h1411_config *config,
 					   struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/s5h1432.h b/drivers/media/dvb-frontends/s5h1432.h
index b57438c32546..70917dd2533a 100644
--- a/drivers/media/dvb-frontends/s5h1432.h
+++ b/drivers/media/dvb-frontends/s5h1432.h
@@ -22,6 +22,7 @@
 #ifndef __S5H1432_H__
 #define __S5H1432_H__
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 #define S5H1432_I2C_TOP_ADDR (0x02 >> 1)
@@ -74,8 +75,7 @@ struct s5h1432_config {
 	u8 status_mode;
 };
 
-#if defined(CONFIG_DVB_S5H1432) || \
-	(defined(CONFIG_DVB_S5H1432_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_S5H1432)
 extern struct dvb_frontend *s5h1432_attach(const struct s5h1432_config *config,
 					   struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/s921.h b/drivers/media/dvb-frontends/s921.h
index f220d8299c81..8d5e2a6e187c 100644
--- a/drivers/media/dvb-frontends/s921.h
+++ b/drivers/media/dvb-frontends/s921.h
@@ -17,6 +17,7 @@
 #ifndef S921_H
 #define S921_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct s921_config {
@@ -24,8 +25,7 @@ struct s921_config {
 	u8 demod_address;
 };
 
-#if defined(CONFIG_DVB_S921) || (defined(CONFIG_DVB_S921_MODULE) \
-	&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_S921)
 extern struct dvb_frontend *s921_attach(const struct s921_config *config,
 					   struct i2c_adapter *i2c);
 extern struct i2c_adapter *s921_get_tuner_i2c_adapter(struct dvb_frontend *);
diff --git a/drivers/media/dvb-frontends/si21xx.h b/drivers/media/dvb-frontends/si21xx.h
index 141b5b8a5f63..1509fed44a3a 100644
--- a/drivers/media/dvb-frontends/si21xx.h
+++ b/drivers/media/dvb-frontends/si21xx.h
@@ -1,6 +1,7 @@
 #ifndef SI21XX_H
 #define SI21XX_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
@@ -12,8 +13,7 @@ struct si21xx_config {
 	int min_delay_ms;
 };
 
-#if defined(CONFIG_DVB_SI21XX) || \
-		(defined(CONFIG_DVB_SI21XX_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_SI21XX)
 extern struct dvb_frontend *si21xx_attach(const struct si21xx_config *config,
 						struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/stb6000.h b/drivers/media/dvb-frontends/stb6000.h
index 7be479c22d5b..a768189bfaad 100644
--- a/drivers/media/dvb-frontends/stb6000.h
+++ b/drivers/media/dvb-frontends/stb6000.h
@@ -23,6 +23,7 @@
 #ifndef __DVB_STB6000_H__
 #define __DVB_STB6000_H__
 
+#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
@@ -34,8 +35,7 @@
  * @param i2c i2c adapter to use.
  * @return FE pointer on success, NULL on failure.
  */
-#if defined(CONFIG_DVB_STB6000) || (defined(CONFIG_DVB_STB6000_MODULE) \
-							&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_STB6000)
 extern struct dvb_frontend *stb6000_attach(struct dvb_frontend *fe, int addr,
 					   struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/stv0288.h b/drivers/media/dvb-frontends/stv0288.h
index f2b53db0606d..a0bd93107154 100644
--- a/drivers/media/dvb-frontends/stv0288.h
+++ b/drivers/media/dvb-frontends/stv0288.h
@@ -27,6 +27,7 @@
 #ifndef STV0288_H
 #define STV0288_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
@@ -42,8 +43,7 @@ struct stv0288_config {
 	int (*set_ts_params)(struct dvb_frontend *fe, int is_punctured);
 };
 
-#if defined(CONFIG_DVB_STV0288) || (defined(CONFIG_DVB_STV0288_MODULE) && \
-							defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_STV0288)
 extern struct dvb_frontend *stv0288_attach(const struct stv0288_config *config,
 					   struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/stv0367.h b/drivers/media/dvb-frontends/stv0367.h
index 93cc4a57eea0..ea80b341f094 100644
--- a/drivers/media/dvb-frontends/stv0367.h
+++ b/drivers/media/dvb-frontends/stv0367.h
@@ -26,6 +26,7 @@
 #ifndef STV0367_H
 #define STV0367_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
@@ -38,8 +39,7 @@ struct stv0367_config {
 	int clk_pol;
 };
 
-#if defined(CONFIG_DVB_STV0367) || (defined(CONFIG_DVB_STV0367_MODULE) \
-							&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_STV0367)
 extern struct
 dvb_frontend *stv0367ter_attach(const struct stv0367_config *config,
 					struct i2c_adapter *i2c);
diff --git a/drivers/media/dvb-frontends/stv0900.h b/drivers/media/dvb-frontends/stv0900.h
index 91c7ee8b2313..e2a6dc69ecb4 100644
--- a/drivers/media/dvb-frontends/stv0900.h
+++ b/drivers/media/dvb-frontends/stv0900.h
@@ -26,6 +26,7 @@
 #ifndef STV0900_H
 #define STV0900_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
@@ -57,8 +58,7 @@ struct stv0900_config {
 	void (*set_lock_led)(struct dvb_frontend *fe, int offon);
 };
 
-#if defined(CONFIG_DVB_STV0900) || (defined(CONFIG_DVB_STV0900_MODULE) \
-							&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_STV0900)
 extern struct dvb_frontend *stv0900_attach(const struct stv0900_config *config,
 					struct i2c_adapter *i2c, int demod);
 #else
diff --git a/drivers/media/dvb-frontends/stv090x.c b/drivers/media/dvb-frontends/stv090x.c
index f36eeefb76a6..56d470ad5a82 100644
--- a/drivers/media/dvb-frontends/stv090x.c
+++ b/drivers/media/dvb-frontends/stv090x.c
@@ -3906,12 +3906,12 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 		reg = stv090x_read_reg(state, STV090x_TSTTNR1);
 		STV090x_SETFIELD(reg, ADC1_PON_FIELD, 0);
 		if (stv090x_write_reg(state, STV090x_TSTTNR1, reg) < 0)
-			goto err;
+			goto err_unlock;
 		/* power off DiSEqC 1 */
 		reg = stv090x_read_reg(state, STV090x_TSTTNR2);
 		STV090x_SETFIELD(reg, DISEQC1_PON_FIELD, 0);
 		if (stv090x_write_reg(state, STV090x_TSTTNR2, reg) < 0)
-			goto err;
+			goto err_unlock;
 
 		/* check whether path 2 is already sleeping, that is when
 		   ADC2 is off */
@@ -3930,7 +3930,7 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 		if (full_standby)
 			STV090x_SETFIELD(reg, STOP_CLKFEC_FIELD, 1);
 		if (stv090x_write_reg(state, STV090x_STOPCLK1, reg) < 0)
-			goto err;
+			goto err_unlock;
 		reg = stv090x_read_reg(state, STV090x_STOPCLK2);
 		/* sampling 1 clock */
 		STV090x_SETFIELD(reg, STOP_CLKSAMP1_FIELD, 1);
@@ -3941,7 +3941,7 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 		if (full_standby)
 			STV090x_SETFIELD(reg, STOP_CLKTS_FIELD, 1);
 		if (stv090x_write_reg(state, STV090x_STOPCLK2, reg) < 0)
-			goto err;
+			goto err_unlock;
 		break;
 
 	case STV090x_DEMODULATOR_1:
@@ -3949,12 +3949,12 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 		reg = stv090x_read_reg(state, STV090x_TSTTNR3);
 		STV090x_SETFIELD(reg, ADC2_PON_FIELD, 0);
 		if (stv090x_write_reg(state, STV090x_TSTTNR3, reg) < 0)
-			goto err;
+			goto err_unlock;
 		/* power off DiSEqC 2 */
 		reg = stv090x_read_reg(state, STV090x_TSTTNR4);
 		STV090x_SETFIELD(reg, DISEQC2_PON_FIELD, 0);
 		if (stv090x_write_reg(state, STV090x_TSTTNR4, reg) < 0)
-			goto err;
+			goto err_unlock;
 
 		/* check whether path 1 is already sleeping, that is when
 		   ADC1 is off */
@@ -3973,7 +3973,7 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 		if (full_standby)
 			STV090x_SETFIELD(reg, STOP_CLKFEC_FIELD, 1);
 		if (stv090x_write_reg(state, STV090x_STOPCLK1, reg) < 0)
-			goto err;
+			goto err_unlock;
 		reg = stv090x_read_reg(state, STV090x_STOPCLK2);
 		/* sampling 2 clock */
 		STV090x_SETFIELD(reg, STOP_CLKSAMP2_FIELD, 1);
@@ -3984,7 +3984,7 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 		if (full_standby)
 			STV090x_SETFIELD(reg, STOP_CLKTS_FIELD, 1);
 		if (stv090x_write_reg(state, STV090x_STOPCLK2, reg) < 0)
-			goto err;
+			goto err_unlock;
 		break;
 
 	default:
@@ -3997,7 +3997,7 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 		reg = stv090x_read_reg(state, STV090x_SYNTCTRL);
 		STV090x_SETFIELD(reg, STANDBY_FIELD, 0x01);
 		if (stv090x_write_reg(state, STV090x_SYNTCTRL, reg) < 0)
-			goto err;
+			goto err_unlock;
 	}
 
 	mutex_unlock(&state->internal->demod_lock);
@@ -4005,8 +4005,10 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 
 err_gateoff:
 	stv090x_i2c_gate_ctrl(state, 0);
-err:
+	goto err;
+err_unlock:
 	mutex_unlock(&state->internal->demod_lock);
+err:
 	dprintk(FE_ERROR, 1, "I/O error");
 	return -1;
 }
diff --git a/drivers/media/dvb-frontends/stv6110.h b/drivers/media/dvb-frontends/stv6110.h
index fe71bba6a26e..8fa07e6a6745 100644
--- a/drivers/media/dvb-frontends/stv6110.h
+++ b/drivers/media/dvb-frontends/stv6110.h
@@ -25,6 +25,7 @@
 #ifndef __DVB_STV6110_H__
 #define __DVB_STV6110_H__
 
+#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
@@ -45,8 +46,7 @@ struct stv6110_config {
 	u8 clk_div;	/* divisor value for the output clock */
 };
 
-#if defined(CONFIG_DVB_STV6110) || (defined(CONFIG_DVB_STV6110_MODULE) \
-							&& defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_STV6110)
 extern struct dvb_frontend *stv6110_attach(struct dvb_frontend *fe,
 					const struct stv6110_config *config,
 					struct i2c_adapter *i2c);
diff --git a/drivers/media/dvb-frontends/tda10048.h b/drivers/media/dvb-frontends/tda10048.h
index fb2ef5ac9487..5e7bf4e47cb3 100644
--- a/drivers/media/dvb-frontends/tda10048.h
+++ b/drivers/media/dvb-frontends/tda10048.h
@@ -22,6 +22,7 @@
 #ifndef TDA10048_H
 #define TDA10048_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 #include <linux/firmware.h>
 
@@ -72,8 +73,7 @@ struct tda10048_config {
 	u8 pll_n;
 };
 
-#if defined(CONFIG_DVB_TDA10048) || \
-	(defined(CONFIG_DVB_TDA10048_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_TDA10048)
 extern struct dvb_frontend *tda10048_attach(
 	const struct tda10048_config *config,
 	struct i2c_adapter *i2c);
diff --git a/drivers/media/dvb-frontends/tda10071.h b/drivers/media/dvb-frontends/tda10071.h
index bff1c38df802..f9542f68fe78 100644
--- a/drivers/media/dvb-frontends/tda10071.h
+++ b/drivers/media/dvb-frontends/tda10071.h
@@ -21,6 +21,7 @@
 #ifndef TDA10071_H
 #define TDA10071_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct tda10071_config {
@@ -71,8 +72,7 @@ struct tda10071_config {
 };
 
 
-#if defined(CONFIG_DVB_TDA10071) || \
-	(defined(CONFIG_DVB_TDA10071_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_TDA10071)
 extern struct dvb_frontend *tda10071_attach(
 	const struct tda10071_config *config, struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/tda18271c2dd.h b/drivers/media/dvb-frontends/tda18271c2dd.h
index 1389c74e12ce..dd84f7b69bec 100644
--- a/drivers/media/dvb-frontends/tda18271c2dd.h
+++ b/drivers/media/dvb-frontends/tda18271c2dd.h
@@ -1,7 +1,9 @@
 #ifndef _TDA18271C2DD_H_
 #define _TDA18271C2DD_H_
-#if defined(CONFIG_DVB_TDA18271C2DD) || (defined(CONFIG_DVB_TDA18271C2DD_MODULE) \
-        && defined(MODULE))
+
+#include <linux/kconfig.h>
+
+#if IS_ENABLED(CONFIG_DVB_TDA18271C2DD)
 struct dvb_frontend *tda18271c2dd_attach(struct dvb_frontend *fe,
 					 struct i2c_adapter *i2c, u8 adr);
 #else
diff --git a/drivers/media/dvb-frontends/ts2020.h b/drivers/media/dvb-frontends/ts2020.h
index c7e64afa614a..5bcb9a71ca80 100644
--- a/drivers/media/dvb-frontends/ts2020.h
+++ b/drivers/media/dvb-frontends/ts2020.h
@@ -22,6 +22,7 @@
 #ifndef TS2020_H
 #define TS2020_H
 
+#include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
 struct ts2020_config {
@@ -29,8 +30,7 @@ struct ts2020_config {
 	u8 clk_out_div;
 };
 
-#if defined(CONFIG_DVB_TS2020) || \
-	(defined(CONFIG_DVB_TS2020_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_TS2020)
 
 extern struct dvb_frontend *ts2020_attach(
 	struct dvb_frontend *fe,
diff --git a/drivers/media/dvb-frontends/zl10036.h b/drivers/media/dvb-frontends/zl10036.h
index d84b8f8215e9..5f1e8217eeb6 100644
--- a/drivers/media/dvb-frontends/zl10036.h
+++ b/drivers/media/dvb-frontends/zl10036.h
@@ -21,6 +21,7 @@
 #ifndef DVB_ZL10036_H
 #define DVB_ZL10036_H
 
+#include <linux/kconfig.h>
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
@@ -37,8 +38,7 @@ struct zl10036_config {
 	int rf_loop_enable;
 };
 
-#if defined(CONFIG_DVB_ZL10036) || \
-	(defined(CONFIG_DVB_ZL10036_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_ZL10036)
 extern struct dvb_frontend *zl10036_attach(struct dvb_frontend *fe,
 	const struct zl10036_config *config, struct i2c_adapter *i2c);
 #else
diff --git a/drivers/media/dvb-frontends/zl10039.h b/drivers/media/dvb-frontends/zl10039.h
index 5eee7ea162a1..750b9bca9d02 100644
--- a/drivers/media/dvb-frontends/zl10039.h
+++ b/drivers/media/dvb-frontends/zl10039.h
@@ -22,8 +22,9 @@
 #ifndef ZL10039_H
 #define ZL10039_H
 
-#if defined(CONFIG_DVB_ZL10039) || (defined(CONFIG_DVB_ZL10039_MODULE) \
-	    && defined(MODULE))
+#include <linux/kconfig.h>
+
+#if IS_ENABLED(CONFIG_DVB_ZL10039)
 struct dvb_frontend *zl10039_attach(struct dvb_frontend *fe,
 					u8 i2c_addr,
 					struct i2c_adapter *i2c);
diff --git a/drivers/media/firewire/firedtv-dvb.c b/drivers/media/firewire/firedtv-dvb.c
index eb7496eab130..f710e17953e3 100644
--- a/drivers/media/firewire/firedtv-dvb.c
+++ b/drivers/media/firewire/firedtv-dvb.c
@@ -71,11 +71,11 @@ int fdtv_start_feed(struct dvb_demux_feed *dvbdmxfeed)
 
 	if (dvbdmxfeed->type == DMX_TYPE_TS) {
 		switch (dvbdmxfeed->pes_type) {
-		case DMX_TS_PES_VIDEO:
-		case DMX_TS_PES_AUDIO:
-		case DMX_TS_PES_TELETEXT:
-		case DMX_TS_PES_PCR:
-		case DMX_TS_PES_OTHER:
+		case DMX_PES_VIDEO:
+		case DMX_PES_AUDIO:
+		case DMX_PES_TELETEXT:
+		case DMX_PES_PCR:
+		case DMX_PES_OTHER:
 			c = alloc_channel(fdtv);
 			break;
 		default:
@@ -132,7 +132,7 @@ int fdtv_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 	      (demux->dmx.frontend->source != DMX_MEMORY_FE))) {
 
 		if (dvbdmxfeed->ts_type & TS_DECODER) {
-			if (dvbdmxfeed->pes_type >= DMX_TS_PES_OTHER ||
+			if (dvbdmxfeed->pes_type >= DMX_PES_OTHER ||
 			    !demux->pesfilter[dvbdmxfeed->pes_type])
 				return -EINVAL;
 
@@ -141,7 +141,7 @@ int fdtv_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 		}
 
 		if (!(dvbdmxfeed->ts_type & TS_DECODER &&
-		      dvbdmxfeed->pes_type < DMX_TS_PES_OTHER))
+		      dvbdmxfeed->pes_type < DMX_PES_OTHER))
 			return 0;
 	}
 
diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
index 7b771baa2212..f981d50a2a8c 100644
--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -112,6 +112,15 @@ config VIDEO_TLV320AIC23B
 	  To compile this driver as a module, choose M here: the
 	  module will be called tlv320aic23b.
 
+config VIDEO_UDA1342
+	tristate "Philips UDA1342 audio codec"
+	depends on VIDEO_V4L2 && I2C
+	---help---
+	  Support for the Philips UDA1342 audio codec.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called uda1342.
+
 config VIDEO_WM8775
 	tristate "Wolfson Microelectronics WM8775 audio ADC with input mixer"
 	depends on VIDEO_V4L2 && I2C
@@ -133,7 +142,7 @@ config VIDEO_WM8739
 	  module will be called wm8739.
 
 config VIDEO_VP27SMPX
-	tristate "Panasonic VP27s internal MPX"
+	tristate "Panasonic VP27's internal MPX"
 	depends on VIDEO_V4L2 && I2C
 	---help---
 	  Support for the internal MPX of the Panasonic VP27s tuner.
@@ -141,6 +150,15 @@ config VIDEO_VP27SMPX
 	  To compile this driver as a module, choose M here: the
 	  module will be called vp27smpx.
 
+config VIDEO_SONY_BTF_MPX
+	tristate "Sony BTF's internal MPX"
+	depends on VIDEO_V4L2 && I2C
+	help
+	  Support for the internal MPX of the Sony BTF-PG472Z tuner.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called sony-btf-mpx.
+
 comment "RDS decoders"
 
 config VIDEO_SAA6588
@@ -283,6 +301,35 @@ config VIDEO_TVP7002
 	  To compile this driver as a module, choose M here: the
 	  module will be called tvp7002.
 
+config VIDEO_TW2804
+	tristate "Techwell TW2804 multiple video decoder"
+	depends on VIDEO_V4L2 && I2C
+	---help---
+	  Support for the Techwell tw2804 multiple video decoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tw2804.
+
+config VIDEO_TW9903
+	tristate "Techwell TW9903 video decoder"
+	depends on VIDEO_V4L2 && I2C
+	---help---
+	  Support for the Techwell tw9903 multi-standard video decoder
+	  with high quality down scaler.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tw9903.
+
+config VIDEO_TW9906
+	tristate "Techwell TW9906 video decoder"
+	depends on VIDEO_V4L2 && I2C
+	---help---
+	  Support for the Techwell tw9906 enhanced multi-standard comb filter
+	  video decoder with YCbCr input support.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tw9906.
+
 config VIDEO_VPX3220
 	tristate "vpx3220a, vpx3216b & vpx3214c video decoders"
 	depends on VIDEO_V4L2 && I2C
@@ -386,6 +433,17 @@ config VIDEO_APTINA_PLL
 config VIDEO_SMIAPP_PLL
 	tristate
 
+config VIDEO_OV7640
+	tristate "OmniVision OV7640 sensor support"
+	depends on I2C && VIDEO_V4L2
+	depends on MEDIA_CAMERA_SUPPORT
+	---help---
+	  This is a Video4Linux2 sensor-level driver for the OmniVision
+	  OV7640 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov7640.
+
 config VIDEO_OV7670
 	tristate "OmniVision OV7670 sensor support"
 	depends on I2C && VIDEO_V4L2
@@ -501,8 +559,8 @@ config VIDEO_S5C73M3
 	tristate "Samsung S5C73M3 sensor support"
 	depends on I2C && SPI && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
 	---help---
-	This is a V4L2 sensor-level driver for Samsung S5C73M3
-	8 Mpixel camera.
+	  This is a V4L2 sensor-level driver for Samsung S5C73M3
+	  8 Mpixel camera.
 
 comment "Flash devices"
 
@@ -550,10 +608,10 @@ config VIDEO_UPD64083
 comment "Miscelaneous helper chips"
 
 config VIDEO_THS7303
-	tristate "THS7303 Video Amplifier"
-	depends on I2C
+	tristate "THS7303/53 Video Amplifier"
+	depends on VIDEO_V4L2 && I2C
 	help
-	  Support for TI THS7303 video amplifier
+	  Support for TI THS7303/53 video amplifier
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called ths7303.
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
index cfefd30cc1bc..720f42d9d9f4 100644
--- a/drivers/media/i2c/Makefile
+++ b/drivers/media/i2c/Makefile
@@ -37,16 +37,22 @@ obj-$(CONFIG_VIDEO_THS7303) += ths7303.o
 obj-$(CONFIG_VIDEO_TVP5150) += tvp5150.o
 obj-$(CONFIG_VIDEO_TVP514X) += tvp514x.o
 obj-$(CONFIG_VIDEO_TVP7002) += tvp7002.o
+obj-$(CONFIG_VIDEO_TW2804) += tw2804.o
+obj-$(CONFIG_VIDEO_TW9903) += tw9903.o
+obj-$(CONFIG_VIDEO_TW9906) += tw9906.o
 obj-$(CONFIG_VIDEO_CS5345) += cs5345.o
 obj-$(CONFIG_VIDEO_CS53L32A) += cs53l32a.o
 obj-$(CONFIG_VIDEO_M52790) += m52790.o
 obj-$(CONFIG_VIDEO_TLV320AIC23B) += tlv320aic23b.o
+obj-$(CONFIG_VIDEO_UDA1342) += uda1342.o
 obj-$(CONFIG_VIDEO_WM8775) += wm8775.o
 obj-$(CONFIG_VIDEO_WM8739) += wm8739.o
 obj-$(CONFIG_VIDEO_VP27SMPX) += vp27smpx.o
+obj-$(CONFIG_VIDEO_SONY_BTF_MPX) += sony-btf-mpx.o
 obj-$(CONFIG_VIDEO_UPD64031A) += upd64031a.o
 obj-$(CONFIG_VIDEO_UPD64083) += upd64083.o
-obj-$(CONFIG_VIDEO_OV7670) 	+= ov7670.o
+obj-$(CONFIG_VIDEO_OV7640) += ov7640.o
+obj-$(CONFIG_VIDEO_OV7670) += ov7670.o
 obj-$(CONFIG_VIDEO_OV9650) += ov9650.o
 obj-$(CONFIG_VIDEO_TCM825X) += tcm825x.o
 obj-$(CONFIG_VIDEO_MT9M032) += mt9m032.o
diff --git a/drivers/media/i2c/ad9389b.c b/drivers/media/i2c/ad9389b.c
index c2886b6a727d..58344b6c3a55 100644
--- a/drivers/media/i2c/ad9389b.c
+++ b/drivers/media/i2c/ad9389b.c
@@ -354,7 +354,7 @@ static int ad9389b_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *
 	return 0;
 }
 
-static int ad9389b_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int ad9389b_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/adp1653.c b/drivers/media/i2c/adp1653.c
index df163800c8e1..ef75abe5984c 100644
--- a/drivers/media/i2c/adp1653.c
+++ b/drivers/media/i2c/adp1653.c
@@ -447,7 +447,7 @@ free_and_quit:
 	return ret;
 }
 
-static int __exit adp1653_remove(struct i2c_client *client)
+static int adp1653_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
 	struct adp1653_flash *flash = to_adp1653_flash(subdev);
@@ -476,7 +476,7 @@ static struct i2c_driver adp1653_i2c_driver = {
 		.pm	= &adp1653_pm_ops,
 	},
 	.probe		= adp1653_probe,
-	.remove		= __exit_p(adp1653_remove),
+	.remove		= adp1653_remove,
 	.id_table	= adp1653_id_table,
 };
 
diff --git a/drivers/media/i2c/adv7180.c b/drivers/media/i2c/adv7180.c
index 34f39d3b3e3e..afd561ab190d 100644
--- a/drivers/media/i2c/adv7180.c
+++ b/drivers/media/i2c/adv7180.c
@@ -135,6 +135,10 @@ struct adv7180_state {
 
 static v4l2_std_id adv7180_std_to_v4l2(u8 status1)
 {
+	/* in case V4L2_IN_ST_NO_SIGNAL */
+	if (!(status1 & ADV7180_STATUS1_IN_LOCK))
+		return V4L2_STD_UNKNOWN;
+
 	switch (status1 & ADV7180_STATUS1_AUTOD_MASK) {
 	case ADV7180_STATUS1_AUTOD_NTSM_M_J:
 		return V4L2_STD_NTSC;
diff --git a/drivers/media/i2c/adv7183.c b/drivers/media/i2c/adv7183.c
index 6fed5b74e743..56a1fa4af0fe 100644
--- a/drivers/media/i2c/adv7183.c
+++ b/drivers/media/i2c/adv7183.c
@@ -507,7 +507,7 @@ static int adv7183_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *
 	return 0;
 }
 
-static int adv7183_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int adv7183_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/adv7604.c b/drivers/media/i2c/adv7604.c
index f47555b1000a..31a63c9324fe 100644
--- a/drivers/media/i2c/adv7604.c
+++ b/drivers/media/i2c/adv7604.c
@@ -699,7 +699,7 @@ static int adv7604_g_register(struct v4l2_subdev *sd,
 }
 
 static int adv7604_s_register(struct v4l2_subdev *sd,
-					struct v4l2_dbg_register *reg)
+					const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/ak881x.c b/drivers/media/i2c/ak881x.c
index ba674656b10d..fd47465e4f6a 100644
--- a/drivers/media/i2c/ak881x.c
+++ b/drivers/media/i2c/ak881x.c
@@ -101,7 +101,7 @@ static int ak881x_g_register(struct v4l2_subdev *sd,
 }
 
 static int ak881x_s_register(struct v4l2_subdev *sd,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/cs5345.c b/drivers/media/i2c/cs5345.c
index c8581e26fa9c..1d2f7c8512b5 100644
--- a/drivers/media/i2c/cs5345.c
+++ b/drivers/media/i2c/cs5345.c
@@ -110,7 +110,7 @@ static int cs5345_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *r
 	return 0;
 }
 
-static int cs5345_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int cs5345_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/cx25840/cx25840-core.c b/drivers/media/i2c/cx25840/cx25840-core.c
index f4149eb4d7b4..12fb9b2eb887 100644
--- a/drivers/media/i2c/cx25840/cx25840-core.c
+++ b/drivers/media/i2c/cx25840/cx25840-core.c
@@ -1671,7 +1671,7 @@ static int cx25840_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *
 	return 0;
 }
 
-static int cx25840_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int cx25840_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
@@ -1835,7 +1835,7 @@ static int cx25840_s_audio_routing(struct v4l2_subdev *sd,
 	return set_input(client, state->vid_input, input);
 }
 
-static int cx25840_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *freq)
+static int cx25840_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *freq)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
@@ -1881,7 +1881,7 @@ static int cx25840_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	return 0;
 }
 
-static int cx25840_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int cx25840_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct cx25840_state *state = to_state(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
diff --git a/drivers/media/i2c/ir-kbd-i2c.c b/drivers/media/i2c/ir-kbd-i2c.c
index 08ae067b2b6f..8e2f79cb045e 100644
--- a/drivers/media/i2c/ir-kbd-i2c.c
+++ b/drivers/media/i2c/ir-kbd-i2c.c
@@ -230,7 +230,7 @@ static int get_key_avermedia_cardbus(struct IR_i2c *ir,
 		return 0;
 
 	dprintk(1, "read key 0x%02x/0x%02x\n", key, keygroup);
-	if (keygroup < 2 || keygroup > 3) {
+	if (keygroup < 2 || keygroup > 4) {
 		/* Only a warning */
 		dprintk(1, "warning: invalid key group 0x%02x for key 0x%02x\n",
 								keygroup, key);
@@ -239,6 +239,10 @@ static int get_key_avermedia_cardbus(struct IR_i2c *ir,
 
 	*ir_key = key;
 	*ir_raw = key;
+	if (!strcmp(ir->ir_codes, RC_MAP_AVERMEDIA_M733A_RM_K6)) {
+		*ir_key |= keygroup << 8;
+		*ir_raw |= keygroup << 8;
+	}
 	return 1;
 }
 
@@ -332,6 +336,13 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		rc_type     = RC_BIT_OTHER;
 		ir_codes    = RC_MAP_AVERMEDIA_CARDBUS;
 		break;
+	case 0x41:
+		name        = "AVerMedia EM78P153";
+		ir->get_key = get_key_avermedia_cardbus;
+		rc_type     = RC_BIT_OTHER;
+		/* RM-KV remote, seems to be same as RM-K6 */
+		ir_codes    = RC_MAP_AVERMEDIA_M733A_RM_K6;
+		break;
 	case 0x71:
 		name        = "Hauppauge/Zilog Z8";
 		ir->get_key = get_key_haup_xvr;
@@ -423,6 +434,7 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	 */
 	rc->map_name       = ir->ir_codes;
 	rc->allowed_protos = rc_type;
+	rc->enabled_protocols = rc_type;
 	if (!rc->driver_name)
 		rc->driver_name = MODULE_NAME;
 
diff --git a/drivers/media/i2c/m52790.c b/drivers/media/i2c/m52790.c
index 0991576f4c82..39f50fd2b8d2 100644
--- a/drivers/media/i2c/m52790.c
+++ b/drivers/media/i2c/m52790.c
@@ -96,7 +96,7 @@ static int m52790_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *r
 	return 0;
 }
 
-static int m52790_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int m52790_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct m52790_state *state = to_state(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
diff --git a/drivers/media/i2c/msp3400-driver.c b/drivers/media/i2c/msp3400-driver.c
index 766305f69a28..54a9dd394f45 100644
--- a/drivers/media/i2c/msp3400-driver.c
+++ b/drivers/media/i2c/msp3400-driver.c
@@ -445,7 +445,7 @@ static int msp_s_radio(struct v4l2_subdev *sd)
 	return 0;
 }
 
-static int msp_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *freq)
+static int msp_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *freq)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
@@ -535,7 +535,7 @@ static int msp_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	return 0;
 }
 
-static int msp_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int msp_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct msp_state *state = to_state(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
diff --git a/drivers/media/i2c/mt9m032.c b/drivers/media/i2c/mt9m032.c
index f80c1d7ec884..8edb3d8f7b90 100644
--- a/drivers/media/i2c/mt9m032.c
+++ b/drivers/media/i2c/mt9m032.c
@@ -87,9 +87,27 @@
 #define MT9M032_RESTART					0x0b
 #define MT9M032_RESET					0x0d
 #define MT9M032_PLL_CONFIG1				0x11
-#define		MT9M032_PLL_CONFIG1_OUTDIV_MASK		0x3f
+#define		MT9M032_PLL_CONFIG1_PREDIV_MASK		0x3f
 #define		MT9M032_PLL_CONFIG1_MUL_SHIFT		8
 #define MT9M032_READ_MODE1				0x1e
+#define		MT9M032_READ_MODE1_OUTPUT_BAD_FRAMES	(1 << 13)
+#define		MT9M032_READ_MODE1_MAINTAIN_FRAME_RATE	(1 << 12)
+#define		MT9M032_READ_MODE1_XOR_LINE_VALID	(1 << 11)
+#define		MT9M032_READ_MODE1_CONT_LINE_VALID	(1 << 10)
+#define		MT9M032_READ_MODE1_INVERT_TRIGGER	(1 << 9)
+#define		MT9M032_READ_MODE1_SNAPSHOT		(1 << 8)
+#define		MT9M032_READ_MODE1_GLOBAL_RESET		(1 << 7)
+#define		MT9M032_READ_MODE1_BULB_EXPOSURE	(1 << 6)
+#define		MT9M032_READ_MODE1_INVERT_STROBE	(1 << 5)
+#define		MT9M032_READ_MODE1_STROBE_ENABLE	(1 << 4)
+#define		MT9M032_READ_MODE1_STROBE_START_TRIG1	(0 << 2)
+#define		MT9M032_READ_MODE1_STROBE_START_EXP	(1 << 2)
+#define		MT9M032_READ_MODE1_STROBE_START_SHUTTER	(2 << 2)
+#define		MT9M032_READ_MODE1_STROBE_START_TRIG2	(3 << 2)
+#define		MT9M032_READ_MODE1_STROBE_END_TRIG1	(0 << 0)
+#define		MT9M032_READ_MODE1_STROBE_END_EXP	(1 << 0)
+#define		MT9M032_READ_MODE1_STROBE_END_SHUTTER	(2 << 0)
+#define		MT9M032_READ_MODE1_STROBE_END_TRIG2	(3 << 0)
 #define MT9M032_READ_MODE2				0x20
 #define		MT9M032_READ_MODE2_VFLIP_SHIFT		15
 #define		MT9M032_READ_MODE2_HFLIP_SHIFT		14
@@ -106,6 +124,8 @@
 #define		MT9M032_GAIN_AMUL_SHIFT			6
 #define		MT9M032_GAIN_ANALOG_MASK		0x3f
 #define MT9M032_FORMATTER1				0x9e
+#define		MT9M032_FORMATTER1_PLL_P1_6		(1 << 8)
+#define		MT9M032_FORMATTER1_PARALLEL		(1 << 12)
 #define MT9M032_FORMATTER2				0x9f
 #define		MT9M032_FORMATTER2_DOUT_EN		0x1000
 #define		MT9M032_FORMATTER2_PIXCLK_EN		0x2000
@@ -121,8 +141,6 @@
 #define		MT9P031_PLL_CONTROL_PWROFF		0x0050
 #define		MT9P031_PLL_CONTROL_PWRON		0x0051
 #define		MT9P031_PLL_CONTROL_USEPLL		0x0052
-#define MT9P031_PLL_CONFIG2				0x11
-#define		MT9P031_PLL_CONFIG2_P1_DIV_MASK		0x1f
 
 struct mt9m032 {
 	struct v4l2_subdev subdev;
@@ -255,13 +273,14 @@ static int mt9m032_setup_pll(struct mt9m032 *sensor)
 		.n_max = 64,
 		.m_min = 16,
 		.m_max = 255,
-		.p1_min = 1,
-		.p1_max = 128,
+		.p1_min = 6,
+		.p1_max = 7,
 	};
 
 	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
 	struct mt9m032_platform_data *pdata = sensor->pdata;
 	struct aptina_pll pll;
+	u16 reg_val;
 	int ret;
 
 	pll.ext_clock = pdata->ext_clock;
@@ -274,18 +293,21 @@ static int mt9m032_setup_pll(struct mt9m032 *sensor)
 	sensor->pix_clock = pdata->pix_clock;
 
 	ret = mt9m032_write(client, MT9M032_PLL_CONFIG1,
-			    (pll.m << MT9M032_PLL_CONFIG1_MUL_SHIFT)
-			    | (pll.p1 - 1));
-	if (!ret)
-		ret = mt9m032_write(client, MT9P031_PLL_CONFIG2, pll.n - 1);
+			    (pll.m << MT9M032_PLL_CONFIG1_MUL_SHIFT) |
+			    ((pll.n - 1) & MT9M032_PLL_CONFIG1_PREDIV_MASK));
 	if (!ret)
 		ret = mt9m032_write(client, MT9P031_PLL_CONTROL,
 				    MT9P031_PLL_CONTROL_PWRON |
 				    MT9P031_PLL_CONTROL_USEPLL);
 	if (!ret)		/* more reserved, Continuous, Master Mode */
-		ret = mt9m032_write(client, MT9M032_READ_MODE1, 0x8006);
-	if (!ret)		/* Set 14-bit mode, select 7 divider */
-		ret = mt9m032_write(client, MT9M032_FORMATTER1, 0x111e);
+		ret = mt9m032_write(client, MT9M032_READ_MODE1, 0x8000 |
+				    MT9M032_READ_MODE1_STROBE_START_EXP |
+				    MT9M032_READ_MODE1_STROBE_END_SHUTTER);
+	if (!ret) {
+		reg_val = (pll.p1 == 6 ? MT9M032_FORMATTER1_PLL_P1_6 : 0)
+			| MT9M032_FORMATTER1_PARALLEL | 0x001e; /* 14-bit */
+		ret = mt9m032_write(client, MT9M032_FORMATTER1, reg_val);
+	}
 
 	return ret;
 }
@@ -548,7 +570,7 @@ static int mt9m032_g_register(struct v4l2_subdev *sd,
 }
 
 static int mt9m032_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct mt9m032 *sensor = to_mt9m032(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
diff --git a/drivers/media/i2c/mt9p031.c b/drivers/media/i2c/mt9p031.c
index e32833262d32..28cf95b37285 100644
--- a/drivers/media/i2c/mt9p031.c
+++ b/drivers/media/i2c/mt9p031.c
@@ -12,6 +12,7 @@
  * published by the Free Software Foundation.
  */
 
+#include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/gpio.h>
@@ -19,6 +20,7 @@
 #include <linux/i2c.h>
 #include <linux/log2.h>
 #include <linux/pm.h>
+#include <linux/regulator/consumer.h>
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 
@@ -121,6 +123,11 @@ struct mt9p031 {
 	struct mutex power_lock; /* lock to protect power_count */
 	int power_count;
 
+	struct clk *clk;
+	struct regulator *vaa;
+	struct regulator *vdd;
+	struct regulator *vdd_io;
+
 	enum mt9p031_model model;
 	struct aptina_pll pll;
 	int reset;
@@ -195,7 +202,7 @@ static int mt9p031_reset(struct mt9p031 *mt9p031)
 					  0);
 }
 
-static int mt9p031_pll_setup(struct mt9p031 *mt9p031)
+static int mt9p031_clk_setup(struct mt9p031 *mt9p031)
 {
 	static const struct aptina_pll_limits limits = {
 		.ext_clock_min = 6000000,
@@ -216,6 +223,12 @@ static int mt9p031_pll_setup(struct mt9p031 *mt9p031)
 	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 	struct mt9p031_platform_data *pdata = mt9p031->pdata;
 
+	mt9p031->clk = devm_clk_get(&client->dev, NULL);
+	if (IS_ERR(mt9p031->clk))
+		return PTR_ERR(mt9p031->clk);
+
+	clk_set_rate(mt9p031->clk, pdata->ext_freq);
+
 	mt9p031->pll.ext_clock = pdata->ext_freq;
 	mt9p031->pll.pix_clock = pdata->target_freq;
 
@@ -264,10 +277,14 @@ static int mt9p031_power_on(struct mt9p031 *mt9p031)
 		usleep_range(1000, 2000);
 	}
 
+	/* Bring up the supplies */
+	regulator_enable(mt9p031->vdd);
+	regulator_enable(mt9p031->vdd_io);
+	regulator_enable(mt9p031->vaa);
+
 	/* Emable clock */
-	if (mt9p031->pdata->set_xclk)
-		mt9p031->pdata->set_xclk(&mt9p031->subdev,
-					 mt9p031->pdata->ext_freq);
+	if (mt9p031->clk)
+		clk_prepare_enable(mt9p031->clk);
 
 	/* Now RESET_BAR must be high */
 	if (mt9p031->reset != -1) {
@@ -285,8 +302,12 @@ static void mt9p031_power_off(struct mt9p031 *mt9p031)
 		usleep_range(1000, 2000);
 	}
 
-	if (mt9p031->pdata->set_xclk)
-		mt9p031->pdata->set_xclk(&mt9p031->subdev, 0);
+	regulator_disable(mt9p031->vaa);
+	regulator_disable(mt9p031->vdd_io);
+	regulator_disable(mt9p031->vdd);
+
+	if (mt9p031->clk)
+		clk_disable_unprepare(mt9p031->clk);
 }
 
 static int __mt9p031_set_power(struct mt9p031 *mt9p031, bool on)
@@ -927,7 +948,7 @@ static int mt9p031_probe(struct i2c_client *client,
 		return -EIO;
 	}
 
-	mt9p031 = kzalloc(sizeof(*mt9p031), GFP_KERNEL);
+	mt9p031 = devm_kzalloc(&client->dev, sizeof(*mt9p031), GFP_KERNEL);
 	if (mt9p031 == NULL)
 		return -ENOMEM;
 
@@ -937,6 +958,16 @@ static int mt9p031_probe(struct i2c_client *client,
 	mt9p031->model = did->driver_data;
 	mt9p031->reset = -1;
 
+	mt9p031->vaa = devm_regulator_get(&client->dev, "vaa");
+	mt9p031->vdd = devm_regulator_get(&client->dev, "vdd");
+	mt9p031->vdd_io = devm_regulator_get(&client->dev, "vdd_io");
+
+	if (IS_ERR(mt9p031->vaa) || IS_ERR(mt9p031->vdd) ||
+	    IS_ERR(mt9p031->vdd_io)) {
+		dev_err(&client->dev, "Unable to get regulators\n");
+		return -ENODEV;
+	}
+
 	v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
 
 	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
@@ -1001,24 +1032,20 @@ static int mt9p031_probe(struct i2c_client *client,
 	mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
 
 	if (pdata->reset != -1) {
-		ret = gpio_request_one(pdata->reset, GPIOF_OUT_INIT_LOW,
-				       "mt9p031_rst");
+		ret = devm_gpio_request_one(&client->dev, pdata->reset,
+					    GPIOF_OUT_INIT_LOW, "mt9p031_rst");
 		if (ret < 0)
 			goto done;
 
 		mt9p031->reset = pdata->reset;
 	}
 
-	ret = mt9p031_pll_setup(mt9p031);
+	ret = mt9p031_clk_setup(mt9p031);
 
 done:
 	if (ret < 0) {
-		if (mt9p031->reset != -1)
-			gpio_free(mt9p031->reset);
-
 		v4l2_ctrl_handler_free(&mt9p031->ctrls);
 		media_entity_cleanup(&mt9p031->subdev.entity);
-		kfree(mt9p031);
 	}
 
 	return ret;
@@ -1032,9 +1059,6 @@ static int mt9p031_remove(struct i2c_client *client)
 	v4l2_ctrl_handler_free(&mt9p031->ctrls);
 	v4l2_device_unregister_subdev(subdev);
 	media_entity_cleanup(&subdev->entity);
-	if (mt9p031->reset != -1)
-		gpio_free(mt9p031->reset);
-	kfree(mt9p031);
 
 	return 0;
 }
diff --git a/drivers/media/i2c/mt9v011.c b/drivers/media/i2c/mt9v011.c
index 73b7688cbebd..3f415fd12de3 100644
--- a/drivers/media/i2c/mt9v011.c
+++ b/drivers/media/i2c/mt9v011.c
@@ -421,7 +421,7 @@ static int mt9v011_g_register(struct v4l2_subdev *sd,
 }
 
 static int mt9v011_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/staging/media/go7007/wis-ov7640.c b/drivers/media/i2c/ov7640.c
index 9f01657f884a..b0cc927e8b19 100644
--- a/drivers/staging/media/go7007/wis-ov7640.c
+++ b/drivers/media/i2c/ov7640.c
@@ -15,21 +15,18 @@
  * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
  */
 
-#include <linux/module.h>
 #include <linux/init.h>
+#include <linux/module.h>
 #include <linux/i2c.h>
 #include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <linux/slab.h>
 
-#include "wis-i2c.h"
-
-struct wis_ov7640 {
-	int brightness;
-	int contrast;
-	int saturation;
-	int hue;
-};
+MODULE_DESCRIPTION("OmniVision ov7640 sensor driver");
+MODULE_LICENSE("GPL v2");
 
-static u8 initial_registers[] = {
+static const u8 initial_registers[] = {
 	0x12, 0x80,
 	0x12, 0x54,
 	0x14, 0x24,
@@ -39,7 +36,7 @@ static u8 initial_registers[] = {
 	0xFF, 0xFF, /* Terminator (reg 0xFF is unused) */
 };
 
-static int write_regs(struct i2c_client *client, u8 *regs)
+static int write_regs(struct i2c_client *client, const u8 *regs)
 {
 	int i;
 
@@ -49,48 +46,61 @@ static int write_regs(struct i2c_client *client, u8 *regs)
 	return 0;
 }
 
-static int wis_ov7640_probe(struct i2c_client *client,
-			    const struct i2c_device_id *id)
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_ops ov7640_ops;
+
+static int ov7640_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
 {
 	struct i2c_adapter *adapter = client->adapter;
+	struct v4l2_subdev *sd;
 
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
 		return -ENODEV;
 
+	sd = kzalloc(sizeof(struct v4l2_subdev), GFP_KERNEL);
+	if (sd == NULL)
+		return -ENOMEM;
+	v4l2_i2c_subdev_init(sd, client, &ov7640_ops);
+
 	client->flags = I2C_CLIENT_SCCB;
 
-	dev_dbg(&client->dev,
-		"wis-ov7640: initializing OV7640 at address %d on %s\n",
-		client->addr, adapter->name);
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
 
 	if (write_regs(client, initial_registers) < 0) {
-		dev_err(&client->dev, "wis-ov7640: error initializing OV7640\n");
+		v4l_err(client, "error initializing OV7640\n");
+		kfree(sd);
 		return -ENODEV;
 	}
 
 	return 0;
 }
 
-static int wis_ov7640_remove(struct i2c_client *client)
+
+static int ov7640_remove(struct i2c_client *client)
 {
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(sd);
 	return 0;
 }
 
-static const struct i2c_device_id wis_ov7640_id[] = {
-	{ "wis_ov7640", 0 },
+static const struct i2c_device_id ov7640_id[] = {
+	{ "ov7640", 0 },
 	{ }
 };
-MODULE_DEVICE_TABLE(i2c, wis_ov7640_id);
+MODULE_DEVICE_TABLE(i2c, ov7640_id);
 
-static struct i2c_driver wis_ov7640_driver = {
+static struct i2c_driver ov7640_driver = {
 	.driver = {
-		.name	= "WIS OV7640 I2C driver",
+		.owner	= THIS_MODULE,
+		.name	= "ov7640",
 	},
-	.probe		= wis_ov7640_probe,
-	.remove		= wis_ov7640_remove,
-	.id_table	= wis_ov7640_id,
+	.probe = ov7640_probe,
+	.remove = ov7640_remove,
+	.id_table = ov7640_id,
 };
-
-module_i2c_driver(wis_ov7640_driver);
-
-MODULE_LICENSE("GPL v2");
+module_i2c_driver(ov7640_driver);
diff --git a/drivers/media/i2c/ov7670.c b/drivers/media/i2c/ov7670.c
index 05ed5b8e7f88..617ad3fff4aa 100644
--- a/drivers/media/i2c/ov7670.c
+++ b/drivers/media/i2c/ov7670.c
@@ -1487,7 +1487,7 @@ static int ov7670_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *r
 	return ret;
 }
 
-static int ov7670_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int ov7670_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3-core.c b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
index 5dbb65e1f6b7..cb52438e53ac 100644
--- a/drivers/media/i2c/s5c73m3/s5c73m3-core.c
+++ b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
@@ -357,7 +357,7 @@ static int s5c73m3_load_fw(struct v4l2_subdev *sd)
 		return -EINVAL;
 	}
 
-	v4l2_info(sd, "Loading firmware (%s, %d B)\n", fw_name, fw->size);
+	v4l2_info(sd, "Loading firmware (%s, %zu B)\n", fw_name, fw->size);
 
 	ret = s5c73m3_spi_write(state, fw->data, fw->size, 64);
 
@@ -1457,6 +1457,12 @@ static int s5c73m3_oif_registered(struct v4l2_subdev *sd)
 	return ret;
 }
 
+static void s5c73m3_oif_unregistered(struct v4l2_subdev *sd)
+{
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
+	v4l2_device_unregister_subdev(&state->sensor_sd);
+}
+
 static const struct v4l2_subdev_internal_ops s5c73m3_internal_ops = {
 	.open		= s5c73m3_open,
 };
@@ -1474,6 +1480,7 @@ static const struct v4l2_subdev_ops s5c73m3_subdev_ops = {
 
 static const struct v4l2_subdev_internal_ops oif_internal_ops = {
 	.registered	= s5c73m3_oif_registered,
+	.unregistered	= s5c73m3_oif_unregistered,
 	.open		= s5c73m3_oif_open,
 };
 
@@ -1668,13 +1675,17 @@ out_err1:
 
 static int s5c73m3_remove(struct i2c_client *client)
 {
-	struct v4l2_subdev *sd = i2c_get_clientdata(client);
-	struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
+	struct v4l2_subdev *oif_sd = i2c_get_clientdata(client);
+	struct s5c73m3 *state = oif_sd_to_s5c73m3(oif_sd);
+	struct v4l2_subdev *sensor_sd = &state->sensor_sd;
 
-	v4l2_device_unregister_subdev(sd);
+	v4l2_device_unregister_subdev(oif_sd);
 
-	v4l2_ctrl_handler_free(sd->ctrl_handler);
-	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(oif_sd->ctrl_handler);
+	media_entity_cleanup(&oif_sd->entity);
+
+	v4l2_device_unregister_subdev(sensor_sd);
+	media_entity_cleanup(&sensor_sd->entity);
 
 	s5c73m3_unregister_spi_driver(state);
 	s5c73m3_free_gpios(state);
diff --git a/drivers/media/i2c/saa6588.c b/drivers/media/i2c/saa6588.c
index 0caac50d7cf4..b4e1ccbd87ec 100644
--- a/drivers/media/i2c/saa6588.c
+++ b/drivers/media/i2c/saa6588.c
@@ -435,7 +435,7 @@ static int saa6588_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	return 0;
 }
 
-static int saa6588_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int saa6588_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct saa6588 *s = to_saa6588(sd);
 
diff --git a/drivers/media/i2c/saa7115.c b/drivers/media/i2c/saa7115.c
index 6b6788cd08f6..52c717d977c9 100644
--- a/drivers/media/i2c/saa7115.c
+++ b/drivers/media/i2c/saa7115.c
@@ -83,9 +83,10 @@ struct saa711x_state {
 	u32 ident;
 	u32 audclk_freq;
 	u32 crystal_freq;
-	u8 ucgc;
+	bool ucgc;
 	u8 cgcdiv;
-	u8 apll;
+	bool apll;
+	bool double_asclk;
 };
 
 static inline struct saa711x_state *to_state(struct v4l2_subdev *sd)
@@ -732,8 +733,12 @@ static int saa711x_s_clock_freq(struct v4l2_subdev *sd, u32 freq)
 	if (state->apll)
 		acc |= 0x08;
 
+	if (state->double_asclk) {
+		acpf <<= 1;
+		acni <<= 1;
+	}
 	saa711x_write(sd, R_38_CLK_RATIO_AMXCLK_TO_ASCLK, 0x03);
-	saa711x_write(sd, R_39_CLK_RATIO_ASCLK_TO_ALRCLK, 0x10);
+	saa711x_write(sd, R_39_CLK_RATIO_ASCLK_TO_ALRCLK, 0x10 << state->double_asclk);
 	saa711x_write(sd, R_3A_AUD_CLK_GEN_BASIC_SETUP, acc);
 
 	saa711x_write(sd, R_30_AUD_MAST_CLK_CYCLES_PER_FIELD, acpf & 0xff);
@@ -1259,6 +1264,12 @@ static int saa711x_s_routing(struct v4l2_subdev *sd,
 				(saa711x_read(sd, R_83_X_PORT_I_O_ENA_AND_OUT_CLK) & 0xfe) |
 				(state->output & 0x01));
 	}
+	if (state->ident > V4L2_IDENT_SAA7111A) {
+		if (config & SAA7115_IDQ_IS_DEFAULT)
+			saa711x_write(sd, R_85_I_PORT_SIGNAL_POLAR, 0x20);
+		else
+			saa711x_write(sd, R_85_I_PORT_SIGNAL_POLAR, 0x21);
+	}
 	return 0;
 }
 
@@ -1296,9 +1307,10 @@ static int saa711x_s_crystal_freq(struct v4l2_subdev *sd, u32 freq, u32 flags)
 	if (freq != SAA7115_FREQ_32_11_MHZ && freq != SAA7115_FREQ_24_576_MHZ)
 		return -EINVAL;
 	state->crystal_freq = freq;
+	state->double_asclk = flags & SAA7115_FREQ_FL_DOUBLE_ASCLK;
 	state->cgcdiv = (flags & SAA7115_FREQ_FL_CGCDIV) ? 3 : 4;
-	state->ucgc = (flags & SAA7115_FREQ_FL_UCGC) ? 1 : 0;
-	state->apll = (flags & SAA7115_FREQ_FL_APLL) ? 1 : 0;
+	state->ucgc = flags & SAA7115_FREQ_FL_UCGC;
+	state->apll = flags & SAA7115_FREQ_FL_APLL;
 	saa711x_s_clock_freq(sd, state->audclk_freq);
 	return 0;
 }
@@ -1354,6 +1366,34 @@ static int saa711x_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
 	 */
 
 	reg1f = saa711x_read(sd, R_1F_STATUS_BYTE_2_VD_DEC);
+
+	if (state->ident == V4L2_IDENT_SAA7115) {
+		reg1e = saa711x_read(sd, R_1E_STATUS_BYTE_1_VD_DEC);
+
+		v4l2_dbg(1, debug, sd, "Status byte 1 (0x1e)=0x%02x\n", reg1e);
+
+		switch (reg1e & 0x03) {
+		case 1:
+			*std &= V4L2_STD_NTSC;
+			break;
+		case 2:
+			/*
+			 * V4L2_STD_PAL just cover the european PAL standards.
+			 * This is wrong, as the device could also be using an
+			 * other PAL standard.
+			 */
+			*std &= V4L2_STD_PAL   | V4L2_STD_PAL_N  | V4L2_STD_PAL_Nc |
+				V4L2_STD_PAL_M | V4L2_STD_PAL_60;
+			break;
+		case 3:
+			*std &= V4L2_STD_SECAM;
+			break;
+		default:
+			/* Can't detect anything */
+			break;
+		}
+	}
+
 	v4l2_dbg(1, debug, sd, "Status byte 2 (0x1f)=0x%02x\n", reg1f);
 
 	/* horizontal/vertical not locked */
@@ -1365,34 +1405,6 @@ static int saa711x_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
 	else
 		*std &= V4L2_STD_625_50;
 
-	if (state->ident != V4L2_IDENT_SAA7115)
-		goto ret;
-
-	reg1e = saa711x_read(sd, R_1E_STATUS_BYTE_1_VD_DEC);
-
-	switch (reg1e & 0x03) {
-	case 1:
-		*std &= V4L2_STD_NTSC;
-		break;
-	case 2:
-		/*
-		 * V4L2_STD_PAL just cover the european PAL standards.
-		 * This is wrong, as the device could also be using an
-		 * other PAL standard.
-		 */
-		*std &= V4L2_STD_PAL   | V4L2_STD_PAL_N  | V4L2_STD_PAL_Nc |
-			V4L2_STD_PAL_M | V4L2_STD_PAL_60;
-		break;
-	case 3:
-		*std &= V4L2_STD_SECAM;
-		break;
-	default:
-		/* Can't detect anything */
-		break;
-	}
-
-	v4l2_dbg(1, debug, sd, "Status byte 1 (0x1e)=0x%02x\n", reg1e);
-
 ret:
 	v4l2_dbg(1, debug, sd, "detected std mask = %08Lx\n", *std);
 
@@ -1428,7 +1440,7 @@ static int saa711x_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *
 	return 0;
 }
 
-static int saa711x_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int saa711x_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/saa7127.c b/drivers/media/i2c/saa7127.c
index b745f68fbc92..8a47ac10927f 100644
--- a/drivers/media/i2c/saa7127.c
+++ b/drivers/media/i2c/saa7127.c
@@ -672,7 +672,7 @@ static int saa7127_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *
 	return 0;
 }
 
-static int saa7127_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int saa7127_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/saa717x.c b/drivers/media/i2c/saa717x.c
index 1e84466515aa..cf3a0aa7e45e 100644
--- a/drivers/media/i2c/saa717x.c
+++ b/drivers/media/i2c/saa717x.c
@@ -988,7 +988,7 @@ static int saa717x_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *
 	return 0;
 }
 
-static int saa717x_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int saa717x_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	u16 addr = reg->reg & 0xffff;
@@ -1113,7 +1113,7 @@ static int saa717x_s_stream(struct v4l2_subdev *sd, int enable)
 }
 
 /* change audio mode */
-static int saa717x_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int saa717x_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct saa717x_state *decoder = to_state(sd);
 	int audio_mode;
diff --git a/drivers/media/i2c/smiapp/smiapp-core.c b/drivers/media/i2c/smiapp/smiapp-core.c
index 83c7ed7ffcc2..cae4f4683851 100644
--- a/drivers/media/i2c/smiapp/smiapp-core.c
+++ b/drivers/media/i2c/smiapp/smiapp-core.c
@@ -2833,7 +2833,7 @@ static int smiapp_probe(struct i2c_client *client,
 				 sensor->src->pads, 0);
 }
 
-static int __exit smiapp_remove(struct i2c_client *client)
+static int smiapp_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
 	struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
@@ -2881,7 +2881,7 @@ static struct i2c_driver smiapp_i2c_driver = {
 		.pm = &smiapp_pm_ops,
 	},
 	.probe	= smiapp_probe,
-	.remove	= __exit_p(smiapp_remove),
+	.remove	= smiapp_remove,
 	.id_table = smiapp_id_table,
 };
 
diff --git a/drivers/media/i2c/soc_camera/Kconfig b/drivers/media/i2c/soc_camera/Kconfig
index 6dff2b7ad520..23d352f0adf0 100644
--- a/drivers/media/i2c/soc_camera/Kconfig
+++ b/drivers/media/i2c/soc_camera/Kconfig
@@ -9,7 +9,6 @@ config SOC_CAMERA_IMX074
 config SOC_CAMERA_MT9M001
 	tristate "mt9m001 support"
 	depends on SOC_CAMERA && I2C
-	select GPIO_PCA953X if MT9M001_PCA9536_SWITCH
 	help
 	  This driver supports MT9M001 cameras from Micron, monochrome
 	  and colour models.
@@ -36,7 +35,6 @@ config SOC_CAMERA_MT9T112
 config SOC_CAMERA_MT9V022
 	tristate "mt9v022 and mt9v024 support"
 	depends on SOC_CAMERA && I2C
-	select GPIO_PCA953X if MT9V022_PCA9536_SWITCH
 	help
 	  This driver supports MT9V022 cameras from Micron
 
diff --git a/drivers/media/i2c/soc_camera/mt9m001.c b/drivers/media/i2c/soc_camera/mt9m001.c
index bcdc86175549..dd9089805757 100644
--- a/drivers/media/i2c/soc_camera/mt9m001.c
+++ b/drivers/media/i2c/soc_camera/mt9m001.c
@@ -360,7 +360,7 @@ static int mt9m001_g_register(struct v4l2_subdev *sd,
 }
 
 static int mt9m001_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/mt9m111.c b/drivers/media/i2c/soc_camera/mt9m111.c
index bbc4ff99603c..8bd4e0d2ea03 100644
--- a/drivers/media/i2c/soc_camera/mt9m111.c
+++ b/drivers/media/i2c/soc_camera/mt9m111.c
@@ -641,7 +641,7 @@ static int mt9m111_g_register(struct v4l2_subdev *sd,
 }
 
 static int mt9m111_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
@@ -701,11 +701,11 @@ static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain)
 	return reg_write(GLOBAL_GAIN, val);
 }
 
-static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int on)
+static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int val)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 
-	if (on)
+	if (val == V4L2_EXPOSURE_AUTO)
 		return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
 	return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
 }
@@ -785,8 +785,6 @@ static int mt9m111_init(struct mt9m111 *mt9m111)
 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 	int ret;
 
-	/* Default HIGHPOWER context */
-	mt9m111->ctx = &context_b;
 	ret = mt9m111_enable(mt9m111);
 	if (!ret)
 		ret = mt9m111_reset(mt9m111);
@@ -975,6 +973,9 @@ static int mt9m111_probe(struct i2c_client *client,
 	if (!mt9m111)
 		return -ENOMEM;
 
+	/* Default HIGHPOWER context */
+	mt9m111->ctx = &context_b;
+
 	v4l2_i2c_subdev_init(&mt9m111->subdev, client, &mt9m111_subdev_ops);
 	v4l2_ctrl_handler_init(&mt9m111->hdl, 5);
 	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
diff --git a/drivers/media/i2c/soc_camera/mt9t031.c b/drivers/media/i2c/soc_camera/mt9t031.c
index d80d044ebf15..26a15b87a9a2 100644
--- a/drivers/media/i2c/soc_camera/mt9t031.c
+++ b/drivers/media/i2c/soc_camera/mt9t031.c
@@ -430,7 +430,7 @@ static int mt9t031_g_register(struct v4l2_subdev *sd,
 }
 
 static int mt9t031_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/mt9t112.c b/drivers/media/i2c/soc_camera/mt9t112.c
index 188e29b03273..a7256b732804 100644
--- a/drivers/media/i2c/soc_camera/mt9t112.c
+++ b/drivers/media/i2c/soc_camera/mt9t112.c
@@ -766,7 +766,7 @@ static int mt9t112_g_register(struct v4l2_subdev *sd,
 }
 
 static int mt9t112_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	int ret;
diff --git a/drivers/media/i2c/soc_camera/mt9v022.c b/drivers/media/i2c/soc_camera/mt9v022.c
index a5e65d6a0781..a295e598486f 100644
--- a/drivers/media/i2c/soc_camera/mt9v022.c
+++ b/drivers/media/i2c/soc_camera/mt9v022.c
@@ -275,6 +275,7 @@ static int mt9v022_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9v022 *mt9v022 = to_mt9v022(client);
 	struct v4l2_rect rect = a->c;
+	int min_row, min_blank;
 	int ret;
 
 	/* Bayer format - even size lengths */
@@ -310,13 +311,21 @@ static int mt9v022_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 		ret = reg_write(client, MT9V022_COLUMN_START, rect.left);
 	if (!ret)
 		ret = reg_write(client, MT9V022_ROW_START, rect.top);
+	/*
+	 * mt9v022: min total row time is 660 columns, min blanking is 43
+	 * mt9v024: min total row time is 690 columns, min blanking is 61
+	 */
+	if (is_mt9v024(mt9v022->chip_version)) {
+		min_row = 690;
+		min_blank = 61;
+	} else {
+		min_row = 660;
+		min_blank = 43;
+	}
 	if (!ret)
-		/*
-		 * Default 94, Phytec driver says:
-		 * "width + horizontal blank >= 660"
-		 */
 		ret = v4l2_ctrl_s_ctrl(mt9v022->hblank,
-				rect.width > 660 - 43 ? 43 : 660 - rect.width);
+				rect.width > min_row - min_blank ?
+				min_blank : min_row - rect.width);
 	if (!ret)
 		ret = v4l2_ctrl_s_ctrl(mt9v022->vblank, 45);
 	if (!ret)
@@ -488,7 +497,7 @@ static int mt9v022_g_register(struct v4l2_subdev *sd,
 }
 
 static int mt9v022_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/ov2640.c b/drivers/media/i2c/soc_camera/ov2640.c
index 0f520f693b6e..e3168424f9ba 100644
--- a/drivers/media/i2c/soc_camera/ov2640.c
+++ b/drivers/media/i2c/soc_camera/ov2640.c
@@ -756,7 +756,7 @@ static int ov2640_g_register(struct v4l2_subdev *sd,
 }
 
 static int ov2640_s_register(struct v4l2_subdev *sd,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/ov5642.c b/drivers/media/i2c/soc_camera/ov5642.c
index 9d53309619d2..9aa56de69eed 100644
--- a/drivers/media/i2c/soc_camera/ov5642.c
+++ b/drivers/media/i2c/soc_camera/ov5642.c
@@ -708,7 +708,7 @@ static int ov5642_get_register(struct v4l2_subdev *sd, struct v4l2_dbg_register
 	return ret;
 }
 
-static int ov5642_set_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int ov5642_set_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/ov6650.c b/drivers/media/i2c/soc_camera/ov6650.c
index dbe4f564e6b2..991202d4bbae 100644
--- a/drivers/media/i2c/soc_camera/ov6650.c
+++ b/drivers/media/i2c/soc_camera/ov6650.c
@@ -421,7 +421,7 @@ static int ov6650_get_register(struct v4l2_subdev *sd,
 }
 
 static int ov6650_set_register(struct v4l2_subdev *sd,
-				struct v4l2_dbg_register *reg)
+				const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/ov772x.c b/drivers/media/i2c/soc_camera/ov772x.c
index fbeb5b2f3ae5..713d62e349f6 100644
--- a/drivers/media/i2c/soc_camera/ov772x.c
+++ b/drivers/media/i2c/soc_camera/ov772x.c
@@ -652,7 +652,7 @@ static int ov772x_g_register(struct v4l2_subdev *sd,
 }
 
 static int ov772x_s_register(struct v4l2_subdev *sd,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/ov9640.c b/drivers/media/i2c/soc_camera/ov9640.c
index 05993041be31..20ca62d371c1 100644
--- a/drivers/media/i2c/soc_camera/ov9640.c
+++ b/drivers/media/i2c/soc_camera/ov9640.c
@@ -322,7 +322,7 @@ static int ov9640_get_register(struct v4l2_subdev *sd,
 }
 
 static int ov9640_set_register(struct v4l2_subdev *sd,
-				struct v4l2_dbg_register *reg)
+				const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/ov9740.c b/drivers/media/i2c/soc_camera/ov9740.c
index 2f236da80165..012bd6271124 100644
--- a/drivers/media/i2c/soc_camera/ov9740.c
+++ b/drivers/media/i2c/soc_camera/ov9740.c
@@ -835,7 +835,7 @@ static int ov9740_get_register(struct v4l2_subdev *sd,
 }
 
 static int ov9740_set_register(struct v4l2_subdev *sd,
-			       struct v4l2_dbg_register *reg)
+			       const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/rj54n1cb0c.c b/drivers/media/i2c/soc_camera/rj54n1cb0c.c
index 5c92679bfefb..1f9ec3b06b4e 100644
--- a/drivers/media/i2c/soc_camera/rj54n1cb0c.c
+++ b/drivers/media/i2c/soc_camera/rj54n1cb0c.c
@@ -1161,7 +1161,7 @@ static int rj54n1_g_register(struct v4l2_subdev *sd,
 }
 
 static int rj54n1_s_register(struct v4l2_subdev *sd,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/soc_camera/tw9910.c b/drivers/media/i2c/soc_camera/tw9910.c
index 7d2074601881..bad90b16a6dd 100644
--- a/drivers/media/i2c/soc_camera/tw9910.c
+++ b/drivers/media/i2c/soc_camera/tw9910.c
@@ -554,7 +554,7 @@ static int tw9910_g_register(struct v4l2_subdev *sd,
 }
 
 static int tw9910_s_register(struct v4l2_subdev *sd,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/sony-btf-mpx.c b/drivers/media/i2c/sony-btf-mpx.c
new file mode 100644
index 000000000000..38cbea98764c
--- /dev/null
+++ b/drivers/media/i2c/sony-btf-mpx.c
@@ -0,0 +1,399 @@
+/*
+ * Copyright (C) 2005-2006 Micronas USA Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License (Version 2) as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/tuner.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-device.h>
+#include <linux/slab.h>
+
+MODULE_DESCRIPTION("sony-btf-mpx driver");
+MODULE_LICENSE("GPL v2");
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level 0=off(default) 1=on\n");
+
+/* #define MPX_DEBUG */
+
+/*
+ * Note:
+ *
+ * AS(IF/MPX) pin:      LOW      HIGH/OPEN
+ * IF/MPX address:   0x42/0x40   0x43/0x44
+ */
+
+
+static int force_mpx_mode = -1;
+module_param(force_mpx_mode, int, 0644);
+
+struct sony_btf_mpx {
+	struct v4l2_subdev sd;
+	int mpxmode;
+	u32 audmode;
+};
+
+static inline struct sony_btf_mpx *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct sony_btf_mpx, sd);
+}
+
+static int mpx_write(struct i2c_client *client, int dev, int addr, int val)
+{
+	u8 buffer[5];
+	struct i2c_msg msg;
+
+	buffer[0] = dev;
+	buffer[1] = addr >> 8;
+	buffer[2] = addr & 0xff;
+	buffer[3] = val >> 8;
+	buffer[4] = val & 0xff;
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 5;
+	msg.buf = buffer;
+	i2c_transfer(client->adapter, &msg, 1);
+	return 0;
+}
+
+/*
+ * MPX register values for the BTF-PG472Z:
+ *
+ *                                 FM_     NICAM_  SCART_
+ *          MODUS  SOURCE    ACB   PRESCAL PRESCAL PRESCAL SYSTEM  VOLUME
+ *         10/0030 12/0008 12/0013 12/000E 12/0010 12/0000 10/0020 12/0000
+ *         ---------------------------------------------------------------
+ * Auto     1003    0020    0100    2603    5000    XXXX    0001    7500
+ *
+ * B/G
+ *  Mono    1003    0020    0100    2603    5000    XXXX    0003    7500
+ *  A2      1003    0020    0100    2601    5000    XXXX    0003    7500
+ *  NICAM   1003    0120    0100    2603    5000    XXXX    0008    7500
+ *
+ * I
+ *  Mono    1003    0020    0100    2603    7900    XXXX    000A    7500
+ *  NICAM   1003    0120    0100    2603    7900    XXXX    000A    7500
+ *
+ * D/K
+ *  Mono    1003    0020    0100    2603    5000    XXXX    0004    7500
+ *  A2-1    1003    0020    0100    2601    5000    XXXX    0004    7500
+ *  A2-2    1003    0020    0100    2601    5000    XXXX    0005    7500
+ *  A2-3    1003    0020    0100    2601    5000    XXXX    0007    7500
+ *  NICAM   1003    0120    0100    2603    5000    XXXX    000B    7500
+ *
+ * L/L'
+ *  Mono    0003    0200    0100    7C03    5000    2200    0009    7500
+ *  NICAM   0003    0120    0100    7C03    5000    XXXX    0009    7500
+ *
+ * M
+ *  Mono    1003    0200    0100    2B03    5000    2B00    0002    7500
+ *
+ * For Asia, replace the 0x26XX in FM_PRESCALE with 0x14XX.
+ *
+ * Bilingual selection in A2/NICAM:
+ *
+ *         High byte of SOURCE     Left chan   Right chan
+ *                 0x01              MAIN         SUB
+ *                 0x03              MAIN         MAIN
+ *                 0x04              SUB          SUB
+ *
+ * Force mono in NICAM by setting the high byte of SOURCE to 0x02 (L/L') or
+ * 0x00 (all other bands).  Force mono in A2 with FMONO_A2:
+ *
+ *                      FMONO_A2
+ *                      10/0022
+ *                      --------
+ *     Forced mono ON     07F0
+ *     Forced mono OFF    0190
+ */
+
+static const struct {
+	enum { AUD_MONO, AUD_A2, AUD_NICAM, AUD_NICAM_L } audio_mode;
+	u16 modus;
+	u16 source;
+	u16 acb;
+	u16 fm_prescale;
+	u16 nicam_prescale;
+	u16 scart_prescale;
+	u16 system;
+	u16 volume;
+} mpx_audio_modes[] = {
+	/* Auto */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
+					0x5000, 0x0000, 0x0001, 0x7500 },
+	/* B/G Mono */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
+					0x5000, 0x0000, 0x0003, 0x7500 },
+	/* B/G A2 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
+					0x5000, 0x0000, 0x0003, 0x7500 },
+	/* B/G NICAM */ { AUD_NICAM,	0x1003, 0x0120, 0x0100, 0x2603,
+					0x5000, 0x0000, 0x0008, 0x7500 },
+	/* I Mono */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
+					0x7900, 0x0000, 0x000A, 0x7500 },
+	/* I NICAM */	{ AUD_NICAM,	0x1003, 0x0120, 0x0100, 0x2603,
+					0x7900, 0x0000, 0x000A, 0x7500 },
+	/* D/K Mono */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
+					0x5000, 0x0000, 0x0004, 0x7500 },
+	/* D/K A2-1 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
+					0x5000, 0x0000, 0x0004, 0x7500 },
+	/* D/K A2-2 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
+					0x5000, 0x0000, 0x0005, 0x7500 },
+	/* D/K A2-3 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
+					0x5000, 0x0000, 0x0007, 0x7500 },
+	/* D/K NICAM */	{ AUD_NICAM,	0x1003, 0x0120, 0x0100, 0x2603,
+					0x5000, 0x0000, 0x000B, 0x7500 },
+	/* L/L' Mono */	{ AUD_MONO,	0x0003, 0x0200, 0x0100, 0x7C03,
+					0x5000, 0x2200, 0x0009, 0x7500 },
+	/* L/L' NICAM */{ AUD_NICAM_L,	0x0003, 0x0120, 0x0100, 0x7C03,
+					0x5000, 0x0000, 0x0009, 0x7500 },
+};
+
+#define MPX_NUM_MODES	ARRAY_SIZE(mpx_audio_modes)
+
+static int mpx_setup(struct sony_btf_mpx *t)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&t->sd);
+	u16 source = 0;
+	u8 buffer[3];
+	struct i2c_msg msg;
+	int mode = t->mpxmode;
+
+	/* reset MPX */
+	buffer[0] = 0x00;
+	buffer[1] = 0x80;
+	buffer[2] = 0x00;
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = buffer;
+	i2c_transfer(client->adapter, &msg, 1);
+	buffer[1] = 0x00;
+	i2c_transfer(client->adapter, &msg, 1);
+
+	if (t->audmode != V4L2_TUNER_MODE_MONO)
+		mode++;
+
+	if (mpx_audio_modes[mode].audio_mode != AUD_MONO) {
+		switch (t->audmode) {
+		case V4L2_TUNER_MODE_MONO:
+			switch (mpx_audio_modes[mode].audio_mode) {
+			case AUD_A2:
+				source = mpx_audio_modes[mode].source;
+				break;
+			case AUD_NICAM:
+				source = 0x0000;
+				break;
+			case AUD_NICAM_L:
+				source = 0x0200;
+				break;
+			default:
+				break;
+			}
+			break;
+		case V4L2_TUNER_MODE_STEREO:
+			source = mpx_audio_modes[mode].source;
+			break;
+		case V4L2_TUNER_MODE_LANG1:
+			source = 0x0300;
+			break;
+		case V4L2_TUNER_MODE_LANG2:
+			source = 0x0400;
+			break;
+		}
+		source |= mpx_audio_modes[mode].source & 0x00ff;
+	} else
+		source = mpx_audio_modes[mode].source;
+
+	mpx_write(client, 0x10, 0x0030, mpx_audio_modes[mode].modus);
+	mpx_write(client, 0x12, 0x0008, source);
+	mpx_write(client, 0x12, 0x0013, mpx_audio_modes[mode].acb);
+	mpx_write(client, 0x12, 0x000e,
+			mpx_audio_modes[mode].fm_prescale);
+	mpx_write(client, 0x12, 0x0010,
+			mpx_audio_modes[mode].nicam_prescale);
+	mpx_write(client, 0x12, 0x000d,
+			mpx_audio_modes[mode].scart_prescale);
+	mpx_write(client, 0x10, 0x0020, mpx_audio_modes[mode].system);
+	mpx_write(client, 0x12, 0x0000, mpx_audio_modes[mode].volume);
+	if (mpx_audio_modes[mode].audio_mode == AUD_A2)
+		mpx_write(client, 0x10, 0x0022,
+			t->audmode == V4L2_TUNER_MODE_MONO ? 0x07f0 : 0x0190);
+
+#ifdef MPX_DEBUG
+	{
+		u8 buf1[3], buf2[2];
+		struct i2c_msg msgs[2];
+
+		v4l2_info(client,
+			"MPX registers: %04x %04x %04x %04x %04x %04x %04x %04x\n",
+			mpx_audio_modes[mode].modus,
+			source,
+			mpx_audio_modes[mode].acb,
+			mpx_audio_modes[mode].fm_prescale,
+			mpx_audio_modes[mode].nicam_prescale,
+			mpx_audio_modes[mode].scart_prescale,
+			mpx_audio_modes[mode].system,
+			mpx_audio_modes[mode].volume);
+		buf1[0] = 0x11;
+		buf1[1] = 0x00;
+		buf1[2] = 0x7e;
+		msgs[0].addr = client->addr;
+		msgs[0].flags = 0;
+		msgs[0].len = 3;
+		msgs[0].buf = buf1;
+		msgs[1].addr = client->addr;
+		msgs[1].flags = I2C_M_RD;
+		msgs[1].len = 2;
+		msgs[1].buf = buf2;
+		i2c_transfer(client->adapter, msgs, 2);
+		v4l2_info(client, "MPX system: %02x%02x\n",
+				buf2[0], buf2[1]);
+		buf1[0] = 0x11;
+		buf1[1] = 0x02;
+		buf1[2] = 0x00;
+		i2c_transfer(client->adapter, msgs, 2);
+		v4l2_info(client, "MPX status: %02x%02x\n",
+				buf2[0], buf2[1]);
+	}
+#endif
+	return 0;
+}
+
+
+static int sony_btf_mpx_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct sony_btf_mpx *t = to_state(sd);
+	int default_mpx_mode = 0;
+
+	if (std & V4L2_STD_PAL_BG)
+		default_mpx_mode = 1;
+	else if (std & V4L2_STD_PAL_I)
+		default_mpx_mode = 4;
+	else if (std & V4L2_STD_PAL_DK)
+		default_mpx_mode = 6;
+	else if (std & V4L2_STD_SECAM_L)
+		default_mpx_mode = 11;
+
+	if (default_mpx_mode != t->mpxmode) {
+		t->mpxmode = default_mpx_mode;
+		mpx_setup(t);
+	}
+	return 0;
+}
+
+static int sony_btf_mpx_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+{
+	struct sony_btf_mpx *t = to_state(sd);
+
+	vt->capability = V4L2_TUNER_CAP_NORM |
+		V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_LANG1 |
+		V4L2_TUNER_CAP_LANG2;
+	vt->rxsubchans = V4L2_TUNER_SUB_MONO |
+		V4L2_TUNER_SUB_STEREO | V4L2_TUNER_SUB_LANG1 |
+		V4L2_TUNER_SUB_LANG2;
+	vt->audmode = t->audmode;
+	return 0;
+}
+
+static int sony_btf_mpx_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
+{
+	struct sony_btf_mpx *t = to_state(sd);
+
+	if (vt->type != V4L2_TUNER_ANALOG_TV)
+		return -EINVAL;
+
+	if (vt->audmode != t->audmode) {
+		t->audmode = vt->audmode;
+		mpx_setup(t);
+	}
+	return 0;
+}
+
+/* --------------------------------------------------------------------------*/
+
+static const struct v4l2_subdev_core_ops sony_btf_mpx_core_ops = {
+	.s_std = sony_btf_mpx_s_std,
+};
+
+static const struct v4l2_subdev_tuner_ops sony_btf_mpx_tuner_ops = {
+	.s_tuner = sony_btf_mpx_s_tuner,
+	.g_tuner = sony_btf_mpx_g_tuner,
+};
+
+static const struct v4l2_subdev_ops sony_btf_mpx_ops = {
+	.core = &sony_btf_mpx_core_ops,
+	.tuner = &sony_btf_mpx_tuner_ops,
+};
+
+/* --------------------------------------------------------------------------*/
+
+static int sony_btf_mpx_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
+{
+	struct sony_btf_mpx *t;
+	struct v4l2_subdev *sd;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_I2C_BLOCK))
+		return -ENODEV;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	t = kzalloc(sizeof(struct sony_btf_mpx), GFP_KERNEL);
+	if (t == NULL)
+		return -ENOMEM;
+
+	sd = &t->sd;
+	v4l2_i2c_subdev_init(sd, client, &sony_btf_mpx_ops);
+
+	/* Initialize sony_btf_mpx */
+	t->mpxmode = 0;
+	t->audmode = V4L2_TUNER_MODE_STEREO;
+
+	return 0;
+}
+
+static int sony_btf_mpx_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_state(sd));
+
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id sony_btf_mpx_id[] = {
+	{ "sony-btf-mpx", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, sony_btf_mpx_id);
+
+static struct i2c_driver sony_btf_mpx_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "sony-btf-mpx",
+	},
+	.probe = sony_btf_mpx_probe,
+	.remove = sony_btf_mpx_remove,
+	.id_table = sony_btf_mpx_id,
+};
+module_i2c_driver(sony_btf_mpx_driver);
diff --git a/drivers/media/i2c/tda7432.c b/drivers/media/i2c/tda7432.c
index f7707e65761e..28b5121881f5 100644
--- a/drivers/media/i2c/tda7432.c
+++ b/drivers/media/i2c/tda7432.c
@@ -35,6 +35,7 @@
 
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
 #include <media/i2c-addr.h>
 
 #ifndef VIDEO_AUDIO_BALANCE
@@ -60,13 +61,17 @@ MODULE_PARM_DESC(maxvol, "Set maximium volume to +20dB(0) else +0dB(1). Default
 
 struct tda7432 {
 	struct v4l2_subdev sd;
-	int addr;
-	int input;
-	int volume;
-	int muted;
-	int bass, treble;
-	int lf, lr, rf, rr;
-	int loud;
+	struct v4l2_ctrl_handler hdl;
+	struct {
+		/* bass/treble cluster */
+		struct v4l2_ctrl *bass;
+		struct v4l2_ctrl *treble;
+	};
+	struct {
+		/* mute/balance cluster */
+		struct v4l2_ctrl *mute;
+		struct v4l2_ctrl *balance;
+	};
 };
 
 static inline struct tda7432 *to_state(struct v4l2_subdev *sd)
@@ -74,6 +79,11 @@ static inline struct tda7432 *to_state(struct v4l2_subdev *sd)
 	return container_of(sd, struct tda7432, sd);
 }
 
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct tda7432, hdl)->sd;
+}
+
 /* The TDA7432 is made by STS-Thompson
  * http://www.st.com
  * http://us.st.com/stonline/books/pdf/docs/4056.pdf
@@ -227,24 +237,22 @@ static int tda7432_write(struct v4l2_subdev *sd, int subaddr, int val)
 static int tda7432_set(struct v4l2_subdev *sd)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct tda7432 *t = to_state(sd);
 	unsigned char buf[16];
 
-	v4l2_dbg(1, debug, sd,
-		"tda7432: 7432_set(0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x)\n",
-		t->input, t->volume, t->bass, t->treble, t->lf, t->lr,
-		t->rf, t->rr, t->loud);
 	buf[0]  = TDA7432_IN;
-	buf[1]  = t->input;
-	buf[2]  = t->volume;
-	buf[3]  = t->bass;
-	buf[4]	= t->treble;
-	buf[5]  = t->lf;
-	buf[6]  = t->lr;
-	buf[7]  = t->rf;
-	buf[8]  = t->rr;
-	buf[9]  = t->loud;
-	if (10 != i2c_master_send(client, buf, 10)) {
+	buf[1]  = TDA7432_STEREO_IN |  /* Main (stereo) input   */
+		  TDA7432_BASS_SYM  |  /* Symmetric bass cut    */
+		  TDA7432_BASS_NORM;   /* Normal bass range     */
+	buf[2]  = 0x3b;
+	if (loudness)			 /* Turn loudness on?     */
+		buf[2] |= TDA7432_LD_ON;
+	buf[3]  = TDA7432_TREBLE_0DB | (TDA7432_BASS_0DB << 4);
+	buf[4]  = TDA7432_ATTEN_0DB;
+	buf[5]  = TDA7432_ATTEN_0DB;
+	buf[6]  = TDA7432_ATTEN_0DB;
+	buf[7]  = TDA7432_ATTEN_0DB;
+	buf[8]  = loudness;
+	if (9 != i2c_master_send(client, buf, 9)) {
 		v4l2_err(sd, "I/O error, trying tda7432_set\n");
 		return -1;
 	}
@@ -252,174 +260,86 @@ static int tda7432_set(struct v4l2_subdev *sd)
 	return 0;
 }
 
-static void do_tda7432_init(struct v4l2_subdev *sd)
-{
-	struct tda7432 *t = to_state(sd);
-
-	v4l2_dbg(2, debug, sd, "In tda7432_init\n");
-
-	t->input  = TDA7432_STEREO_IN |  /* Main (stereo) input   */
-		    TDA7432_BASS_SYM  |  /* Symmetric bass cut    */
-		    TDA7432_BASS_NORM;   /* Normal bass range     */
-	t->volume =  0x3b ;				 /* -27dB Volume            */
-	if (loudness)			 /* Turn loudness on?     */
-		t->volume |= TDA7432_LD_ON;
-	t->muted    = 1;
-	t->treble   = TDA7432_TREBLE_0DB; /* 0dB Treble            */
-	t->bass		= TDA7432_BASS_0DB; 	 /* 0dB Bass              */
-	t->lf     = TDA7432_ATTEN_0DB;	 /* 0dB attenuation       */
-	t->lr     = TDA7432_ATTEN_0DB;	 /* 0dB attenuation       */
-	t->rf     = TDA7432_ATTEN_0DB;	 /* 0dB attenuation       */
-	t->rr     = TDA7432_ATTEN_0DB;	 /* 0dB attenuation       */
-	t->loud   = loudness;		 /* insmod parameter      */
-
-	tda7432_set(sd);
-}
-
-static int tda7432_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int tda7432_log_status(struct v4l2_subdev *sd)
 {
-	struct tda7432 *t = to_state(sd);
+	struct tda7432 *state = to_state(sd);
 
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_MUTE:
-		ctrl->value=t->muted;
-		return 0;
-	case V4L2_CID_AUDIO_VOLUME:
-		if (!maxvol){  /* max +20db */
-			ctrl->value = ( 0x6f - (t->volume & 0x7F) ) * 630;
-		} else {       /* max 0db   */
-			ctrl->value = ( 0x6f - (t->volume & 0x7F) ) * 829;
-		}
-		return 0;
-	case V4L2_CID_AUDIO_BALANCE:
-	{
-		if ( (t->lf) < (t->rf) )
-			/* right is attenuated, balance shifted left */
-			ctrl->value = (32768 - 1057*(t->rf));
-		else
-			/* left is attenuated, balance shifted right */
-			ctrl->value = (32768 + 1057*(t->lf));
-		return 0;
-	}
-	case V4L2_CID_AUDIO_BASS:
-	{
-		/* Bass/treble 4 bits each */
-		int bass=t->bass;
-		if(bass >= 0x8)
-			bass = ~(bass - 0x8) & 0xf;
-		ctrl->value = (bass << 12)+(bass << 8)+(bass << 4)+(bass);
-		return 0;
-	}
-	case V4L2_CID_AUDIO_TREBLE:
-	{
-		int treble=t->treble;
-		if(treble >= 0x8)
-			treble = ~(treble - 0x8) & 0xf;
-		ctrl->value = (treble << 12)+(treble << 8)+(treble << 4)+(treble);
-		return 0;
-	}
-	}
-	return -EINVAL;
+	v4l2_ctrl_handler_log_status(&state->hdl, sd->name);
+	return 0;
 }
 
-static int tda7432_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int tda7432_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+	struct v4l2_subdev *sd = to_sd(ctrl);
 	struct tda7432 *t = to_state(sd);
+	u8 bass, treble, volume;
+	u8 lf, lr, rf, rr;
 
 	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_MUTE:
-		t->muted=ctrl->value;
-		break;
-	case V4L2_CID_AUDIO_VOLUME:
-		if(!maxvol){ /* max +20db */
-			t->volume = 0x6f - ((ctrl->value)/630);
-		} else {    /* max 0db   */
-			t->volume = 0x6f - ((ctrl->value)/829);
-		}
-		if (loudness)		/* Turn on the loudness bit */
-			t->volume |= TDA7432_LD_ON;
-
-		tda7432_write(sd, TDA7432_VL, t->volume);
-		return 0;
-	case V4L2_CID_AUDIO_BALANCE:
-		if (ctrl->value < 32768) {
+		if (t->balance->val < 0) {
 			/* shifted to left, attenuate right */
-			t->rr = (32768 - ctrl->value)/1057;
-			t->rf = t->rr;
-			t->lr = TDA7432_ATTEN_0DB;
-			t->lf = TDA7432_ATTEN_0DB;
-		} else if(ctrl->value > 32769) {
+			rr = rf = -t->balance->val;
+			lr = lf = TDA7432_ATTEN_0DB;
+		} else if (t->balance->val > 0) {
 			/* shifted to right, attenuate left */
-			t->lf = (ctrl->value - 32768)/1057;
-			t->lr = t->lf;
-			t->rr = TDA7432_ATTEN_0DB;
-			t->rf = TDA7432_ATTEN_0DB;
+			rr = rf = TDA7432_ATTEN_0DB;
+			lr = lf = t->balance->val;
 		} else {
 			/* centered */
-			t->rr = TDA7432_ATTEN_0DB;
-			t->rf = TDA7432_ATTEN_0DB;
-			t->lf = TDA7432_ATTEN_0DB;
-			t->lr = TDA7432_ATTEN_0DB;
+			rr = rf = TDA7432_ATTEN_0DB;
+			lr = lf = TDA7432_ATTEN_0DB;
 		}
-		break;
-	case V4L2_CID_AUDIO_BASS:
-		t->bass = ctrl->value >> 12;
-		if(t->bass>= 0x8)
-				t->bass = (~t->bass & 0xf) + 0x8 ;
-
-		tda7432_write(sd, TDA7432_TN, 0x10 | (t->bass << 4) | t->treble);
+		if (t->mute->val) {
+			lf |= TDA7432_MUTE;
+			lr |= TDA7432_MUTE;
+			lf |= TDA7432_MUTE;
+			rr |= TDA7432_MUTE;
+		}
+		/* Mute & update balance*/
+		tda7432_write(sd, TDA7432_LF, lf);
+		tda7432_write(sd, TDA7432_LR, lr);
+		tda7432_write(sd, TDA7432_RF, rf);
+		tda7432_write(sd, TDA7432_RR, rr);
 		return 0;
-	case V4L2_CID_AUDIO_TREBLE:
-		t->treble= ctrl->value >> 12;
-		if(t->treble>= 0x8)
-				t->treble = (~t->treble & 0xf) + 0x8 ;
+	case V4L2_CID_AUDIO_VOLUME:
+		volume = 0x6f - ctrl->val;
+		if (loudness)		/* Turn on the loudness bit */
+			volume |= TDA7432_LD_ON;
 
-		tda7432_write(sd, TDA7432_TN, 0x10 | (t->bass << 4) | t->treble);
+		tda7432_write(sd, TDA7432_VL, volume);
 		return 0;
-	default:
-		return -EINVAL;
-	}
-
-	/* Used for both mute and balance changes */
-	if (t->muted)
-	{
-		/* Mute & update balance*/
-		tda7432_write(sd, TDA7432_LF, t->lf | TDA7432_MUTE);
-		tda7432_write(sd, TDA7432_LR, t->lr | TDA7432_MUTE);
-		tda7432_write(sd, TDA7432_RF, t->rf | TDA7432_MUTE);
-		tda7432_write(sd, TDA7432_RR, t->rr | TDA7432_MUTE);
-	} else {
-		tda7432_write(sd, TDA7432_LF, t->lf);
-		tda7432_write(sd, TDA7432_LR, t->lr);
-		tda7432_write(sd, TDA7432_RF, t->rf);
-		tda7432_write(sd, TDA7432_RR, t->rr);
-	}
-	return 0;
-}
-
-static int tda7432_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_AUDIO_VOLUME:
-		return v4l2_ctrl_query_fill(qc, 0, 65535, 65535 / 100, 58880);
-	case V4L2_CID_AUDIO_MUTE:
-		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 0);
-	case V4L2_CID_AUDIO_BALANCE:
 	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-		return v4l2_ctrl_query_fill(qc, 0, 65535, 65535 / 100, 32768);
+		bass = t->bass->val;
+		treble = t->treble->val;
+		if (bass >= 0x8)
+			bass = 14 - (bass - 8);
+		if (treble >= 0x8)
+			treble = 14 - (treble - 8);
+
+		tda7432_write(sd, TDA7432_TN, 0x10 | (bass << 4) | treble);
+		return 0;
 	}
 	return -EINVAL;
 }
 
 /* ----------------------------------------------------------------------- */
 
-static const struct v4l2_subdev_core_ops tda7432_core_ops = {
-	.queryctrl = tda7432_queryctrl,
-	.g_ctrl = tda7432_g_ctrl,
+static const struct v4l2_ctrl_ops tda7432_ctrl_ops = {
 	.s_ctrl = tda7432_s_ctrl,
 };
 
+static const struct v4l2_subdev_core_ops tda7432_core_ops = {
+	.log_status = tda7432_log_status,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+};
+
 static const struct v4l2_subdev_ops tda7432_ops = {
 	.core = &tda7432_core_ops,
 };
@@ -444,6 +364,28 @@ static int tda7432_probe(struct i2c_client *client,
 		return -ENOMEM;
 	sd = &t->sd;
 	v4l2_i2c_subdev_init(sd, client, &tda7432_ops);
+	v4l2_ctrl_handler_init(&t->hdl, 5);
+	v4l2_ctrl_new_std(&t->hdl, &tda7432_ctrl_ops,
+		V4L2_CID_AUDIO_VOLUME, 0, maxvol ? 0x68 : 0x4f, 1, maxvol ? 0x5d : 0x47);
+	t->mute = v4l2_ctrl_new_std(&t->hdl, &tda7432_ctrl_ops,
+		V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	t->balance = v4l2_ctrl_new_std(&t->hdl, &tda7432_ctrl_ops,
+		V4L2_CID_AUDIO_BALANCE, -31, 31, 1, 0);
+	t->bass = v4l2_ctrl_new_std(&t->hdl, &tda7432_ctrl_ops,
+		V4L2_CID_AUDIO_BASS, 0, 14, 1, 7);
+	t->treble = v4l2_ctrl_new_std(&t->hdl, &tda7432_ctrl_ops,
+		V4L2_CID_AUDIO_TREBLE, 0, 14, 1, 7);
+	sd->ctrl_handler = &t->hdl;
+	if (t->hdl.error) {
+		int err = t->hdl.error;
+
+		v4l2_ctrl_handler_free(&t->hdl);
+		kfree(t);
+		return err;
+	}
+	v4l2_ctrl_cluster(2, &t->bass);
+	v4l2_ctrl_cluster(2, &t->mute);
+	v4l2_ctrl_handler_setup(&t->hdl);
 	if (loudness < 0 || loudness > 15) {
 		v4l2_warn(sd, "loudness parameter must be between 0 and 15\n");
 		if (loudness < 0)
@@ -452,17 +394,19 @@ static int tda7432_probe(struct i2c_client *client,
 			loudness = 15;
 	}
 
-	do_tda7432_init(sd);
+	tda7432_set(sd);
 	return 0;
 }
 
 static int tda7432_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct tda7432 *t = to_state(sd);
 
-	do_tda7432_init(sd);
+	tda7432_set(sd);
 	v4l2_device_unregister_subdev(sd);
-	kfree(to_state(sd));
+	v4l2_ctrl_handler_free(&t->hdl);
+	kfree(t);
 	return 0;
 }
 
diff --git a/drivers/media/i2c/tda9840.c b/drivers/media/i2c/tda9840.c
index 3d7ddd93282d..01441e35d88b 100644
--- a/drivers/media/i2c/tda9840.c
+++ b/drivers/media/i2c/tda9840.c
@@ -87,7 +87,7 @@ static int tda9840_status(struct v4l2_subdev *sd)
 	return byte & 0x60;
 }
 
-static int tda9840_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *t)
+static int tda9840_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *t)
 {
 	int stat = tda9840_status(sd);
 	int byte;
diff --git a/drivers/media/i2c/ths7303.c b/drivers/media/i2c/ths7303.c
index e747524ba6ed..c4339556a2ea 100644
--- a/drivers/media/i2c/ths7303.c
+++ b/drivers/media/i2c/ths7303.c
@@ -1,7 +1,15 @@
 /*
- * ths7303- THS7303 Video Amplifier driver
+ * ths7303/53- THS7303/53 Video Amplifier driver
  *
  * Copyright (C) 2009 Texas Instruments Incorporated - http://www.ti.com/
+ * Copyright 2013 Cisco Systems, Inc. and/or its affiliates.
+ *
+ * Author: Chaithrika U S <chaithrika@ti.com>
+ *
+ * Contributors:
+ *     Hans Verkuil <hans.verkuil@cisco.com>
+ *     Lad, Prabhakar <prabhakar.lad@ti.com>
+ *     Martin Bugge <marbugge@cisco.com>
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License as
@@ -13,25 +21,27 @@
  * GNU General Public License for more details.
  */
 
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/ctype.h>
-#include <linux/slab.h>
 #include <linux/i2c.h>
-#include <linux/device.h>
-#include <linux/delay.h>
 #include <linux/module.h>
-#include <linux/uaccess.h>
-#include <linux/videodev2.h>
+#include <linux/slab.h>
 
-#include <media/v4l2-device.h>
-#include <media/v4l2-subdev.h>
+#include <media/ths7303.h>
 #include <media/v4l2-chip-ident.h>
+#include <media/v4l2-device.h>
 
 #define THS7303_CHANNEL_1	1
 #define THS7303_CHANNEL_2	2
 #define THS7303_CHANNEL_3	3
 
+struct ths7303_state {
+	struct v4l2_subdev		sd;
+	struct ths7303_platform_data	pdata;
+	struct v4l2_bt_timings		bt;
+	int std_id;
+	int stream_on;
+	int driver_data;
+};
+
 enum ths7303_filter_mode {
 	THS7303_FILTER_MODE_480I_576I,
 	THS7303_FILTER_MODE_480P_576P,
@@ -48,64 +58,84 @@ static int debug;
 module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "Debug level 0-1");
 
+static inline struct ths7303_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ths7303_state, sd);
+}
+
+static int ths7303_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+static int ths7303_write(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		ret = i2c_smbus_write_byte_data(client, reg, val);
+		if (ret == 0)
+			return 0;
+	}
+	return ret;
+}
+
 /* following function is used to set ths7303 */
 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 = 0;
-	u8 temp;
-
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ths7303_state *state = to_state(sd);
+	struct ths7303_platform_data *pdata = &state->pdata;
+	u8 val, sel = 0;
+	int err, disable = 0;
 
 	if (!client)
 		return -EINVAL;
 
 	switch (mode) {
 	case THS7303_FILTER_MODE_1080P:
-		val = (3 << 6);
-		val |= (3 << 3);
+		sel = 0x3;	/*1080p and SXGA/UXGA */
 		break;
 	case THS7303_FILTER_MODE_720P_1080I:
-		val = (2 << 6);
-		val |= (2 << 3);
+		sel = 0x2;	/*720p, 1080i and SVGA/XGA */
 		break;
 	case THS7303_FILTER_MODE_480P_576P:
-		val = (1 << 6);
-		val |= (1 << 3);
+		sel = 0x1;	/* EDTV 480p/576p and VGA */
 		break;
 	case THS7303_FILTER_MODE_480I_576I:
+		sel = 0x0;	/* SDTV, S-Video, 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;
+
+	val = (sel << 6) | (sel << 3);
 	if (!disable)
-		val |= input_bias_luma;
-	err = i2c_smbus_write_byte_data(client, THS7303_CHANNEL_2, val);
+		val |= (pdata->ch_1 & 0x27);
+	err = ths7303_write(sd, THS7303_CHANNEL_1, val);
 	if (err)
 		goto out;
 
-	/* setup two chroma channels */
+	val = (sel << 6) | (sel << 3);
 	if (!disable)
-		temp |= input_bias_chroma;
-
-	err = i2c_smbus_write_byte_data(client, THS7303_CHANNEL_1, temp);
+		val |= (pdata->ch_2 & 0x27);
+	err = ths7303_write(sd, THS7303_CHANNEL_2, val);
 	if (err)
 		goto out;
 
-	err = i2c_smbus_write_byte_data(client, THS7303_CHANNEL_3, temp);
+	val = (sel << 6) | (sel << 3);
+	if (!disable)
+		val |= (pdata->ch_3 & 0x27);
+	err = ths7303_write(sd, THS7303_CHANNEL_3, val);
 	if (err)
 		goto out;
-	return err;
+
+	return 0;
 out:
 	pr_info("write byte data failed\n");
 	return err;
@@ -113,49 +143,209 @@ out:
 
 static int ths7303_s_std_output(struct v4l2_subdev *sd, v4l2_std_id norm)
 {
-	if (norm & (V4L2_STD_ALL & ~V4L2_STD_SECAM))
+	struct ths7303_state *state = to_state(sd);
+
+	if (norm & (V4L2_STD_ALL & ~V4L2_STD_SECAM)) {
+		state->std_id = 1;
+		state->bt.pixelclock = 0;
 		return ths7303_setval(sd, THS7303_FILTER_MODE_480I_576I);
-	else
-		return ths7303_setval(sd, THS7303_FILTER_MODE_DISABLE);
+	}
+
+	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)
+static int ths7303_config(struct v4l2_subdev *sd)
 {
-	u32 height = dv_timings->bt.height;
-	int interlaced = dv_timings->bt.interlaced;
-	int res = 0;
+	struct ths7303_state *state = to_state(sd);
+	int res;
+
+	if (!state->stream_on) {
+		ths7303_write(sd, THS7303_CHANNEL_1,
+			      (ths7303_read(sd, THS7303_CHANNEL_1) & 0xf8) |
+			      0x00);
+		ths7303_write(sd, THS7303_CHANNEL_2,
+			      (ths7303_read(sd, THS7303_CHANNEL_2) & 0xf8) |
+			      0x00);
+		ths7303_write(sd, THS7303_CHANNEL_3,
+			      (ths7303_read(sd, THS7303_CHANNEL_3) & 0xf8) |
+			      0x00);
+		return 0;
+	}
 
-	if (height == 1080 && !interlaced)
+	if (state->bt.pixelclock > 120000000)
 		res = ths7303_setval(sd, THS7303_FILTER_MODE_1080P);
-	else if ((height == 720 && !interlaced) ||
-			(height == 1080 && interlaced))
+	else if (state->bt.pixelclock > 70000000)
 		res = ths7303_setval(sd, THS7303_FILTER_MODE_720P_1080I);
-	else if ((height == 480 || height == 576) && !interlaced)
+	else if (state->bt.pixelclock > 20000000)
 		res = ths7303_setval(sd, THS7303_FILTER_MODE_480P_576P);
+	else if (state->std_id)
+		res = ths7303_setval(sd, THS7303_FILTER_MODE_480I_576I);
 	else
 		/* disable all channels */
 		res = ths7303_setval(sd, THS7303_FILTER_MODE_DISABLE);
 
 	return res;
+
+}
+
+static int ths7303_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ths7303_state *state = to_state(sd);
+
+	state->stream_on = enable;
+
+	return ths7303_config(sd);
+}
+
+/* for setting filter for HD output */
+static int ths7303_s_dv_timings(struct v4l2_subdev *sd,
+			       struct v4l2_dv_timings *dv_timings)
+{
+	struct ths7303_state *state = to_state(sd);
+
+	if (!dv_timings || dv_timings->type != V4L2_DV_BT_656_1120)
+		return -EINVAL;
+
+	state->bt = dv_timings->bt;
+	state->std_id = 0;
+
+	return ths7303_config(sd);
 }
 
 static int ths7303_g_chip_ident(struct v4l2_subdev *sd,
 				struct v4l2_dbg_chip_ident *chip)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ths7303_state *state = to_state(sd);
 
-	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_THS7303, 0);
+	return v4l2_chip_ident_i2c_client(client, chip, state->driver_data, 0);
 }
 
 static const struct v4l2_subdev_video_ops ths7303_video_ops = {
+	.s_stream	= ths7303_s_stream,
 	.s_std_output	= ths7303_s_std_output,
-	.s_dv_timings    = ths7303_s_dv_timings,
+	.s_dv_timings   = ths7303_s_dv_timings,
 };
 
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+
+static int ths7303_g_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	reg->size = 1;
+	reg->val = ths7303_read(sd, reg->reg);
+	return 0;
+}
+
+static int ths7303_s_register(struct v4l2_subdev *sd,
+			      const struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	ths7303_write(sd, reg->reg, reg->val);
+	return 0;
+}
+#endif
+
+static const char * const stc_lpf_sel_txt[4] = {
+	"500-kHz Filter",
+	"2.5-MHz Filter",
+	"5-MHz Filter",
+	"5-MHz Filter",
+};
+
+static const char * const in_mux_sel_txt[2] = {
+	"Input A Select",
+	"Input B Select",
+};
+
+static const char * const lpf_freq_sel_txt[4] = {
+	"9-MHz LPF",
+	"16-MHz LPF",
+	"35-MHz LPF",
+	"Bypass LPF",
+};
+
+static const char * const in_bias_sel_dis_cont_txt[8] = {
+	"Disable Channel",
+	"Mute Function - No Output",
+	"DC Bias Select",
+	"DC Bias + 250 mV Offset Select",
+	"AC Bias Select",
+	"Sync Tip Clamp with low bias",
+	"Sync Tip Clamp with mid bias",
+	"Sync Tip Clamp with high bias",
+};
+
+static void ths7303_log_channel_status(struct v4l2_subdev *sd, u8 reg)
+{
+	u8 val = ths7303_read(sd, reg);
+
+	if ((val & 0x7) == 0) {
+		v4l2_info(sd, "Channel %d Off\n", reg);
+		return;
+	}
+
+	v4l2_info(sd, "Channel %d On\n", reg);
+	v4l2_info(sd, "  value 0x%x\n", val);
+	v4l2_info(sd, "  %s\n", stc_lpf_sel_txt[(val >> 6) & 0x3]);
+	v4l2_info(sd, "  %s\n", in_mux_sel_txt[(val >> 5) & 0x1]);
+	v4l2_info(sd, "  %s\n", lpf_freq_sel_txt[(val >> 3) & 0x3]);
+	v4l2_info(sd, "  %s\n", in_bias_sel_dis_cont_txt[(val >> 0) & 0x7]);
+}
+
+static int ths7303_log_status(struct v4l2_subdev *sd)
+{
+	struct ths7303_state *state = to_state(sd);
+
+	v4l2_info(sd, "stream %s\n", state->stream_on ? "On" : "Off");
+
+	if (state->bt.pixelclock) {
+		struct v4l2_bt_timings *bt = bt = &state->bt;
+		u32 frame_width, frame_height;
+
+		frame_width = bt->width + bt->hfrontporch +
+			      bt->hsync + bt->hbackporch;
+		frame_height = bt->height + bt->vfrontporch +
+			       bt->vsync + bt->vbackporch;
+		v4l2_info(sd,
+			  "timings: %dx%d%s%d (%dx%d). Pix freq. = %d Hz. Polarities = 0x%x\n",
+			  bt->width, bt->height, bt->interlaced ? "i" : "p",
+			  (frame_height * frame_width) > 0 ?
+			  (int)bt->pixelclock /
+			  (frame_height * frame_width) : 0,
+			  frame_width, frame_height,
+			  (int)bt->pixelclock, bt->polarities);
+	} else {
+		v4l2_info(sd, "no timings set\n");
+	}
+
+	ths7303_log_channel_status(sd, THS7303_CHANNEL_1);
+	ths7303_log_channel_status(sd, THS7303_CHANNEL_2);
+	ths7303_log_channel_status(sd, THS7303_CHANNEL_3);
+
+	return 0;
+}
+
 static const struct v4l2_subdev_core_ops ths7303_core_ops = {
 	.g_chip_ident = ths7303_g_chip_ident,
+	.log_status = ths7303_log_status,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = ths7303_g_register,
+	.s_register = ths7303_s_register,
+#endif
 };
 
 static const struct v4l2_subdev_ops ths7303_ops = {
@@ -163,11 +353,38 @@ static const struct v4l2_subdev_ops ths7303_ops = {
 	.video 	= &ths7303_video_ops,
 };
 
+static int ths7303_setup(struct v4l2_subdev *sd)
+{
+	struct ths7303_state *state = to_state(sd);
+	struct ths7303_platform_data *pdata = &state->pdata;
+	int ret;
+	u8 mask;
+
+	state->stream_on = pdata->init_enable;
+
+	mask = state->stream_on ? 0xff : 0xf8;
+
+	ret = ths7303_write(sd, THS7303_CHANNEL_1, pdata->ch_1 & mask);
+	if (ret)
+		return ret;
+
+	ret = ths7303_write(sd, THS7303_CHANNEL_2, pdata->ch_2 & mask);
+	if (ret)
+		return ret;
+
+	ret = ths7303_write(sd, THS7303_CHANNEL_3, pdata->ch_3 & mask);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
 static int ths7303_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
+	struct ths7303_platform_data *pdata = client->dev.platform_data;
+	struct ths7303_state *state;
 	struct v4l2_subdev *sd;
-	v4l2_std_id std_id = V4L2_STD_NTSC;
 
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
 		return -ENODEV;
@@ -175,13 +392,28 @@ static int ths7303_probe(struct i2c_client *client,
 	v4l_info(client, "chip found @ 0x%x (%s)\n",
 			client->addr << 1, client->adapter->name);
 
-	sd = devm_kzalloc(&client->dev, sizeof(struct v4l2_subdev), GFP_KERNEL);
-	if (sd == NULL)
+	state = devm_kzalloc(&client->dev, sizeof(struct ths7303_state),
+			     GFP_KERNEL);
+	if (!state)
 		return -ENOMEM;
 
+	if (!pdata)
+		v4l_warn(client, "No platform data, using default data!\n");
+	else
+		state->pdata = *pdata;
+
+	sd = &state->sd;
 	v4l2_i2c_subdev_init(sd, client, &ths7303_ops);
 
-	return ths7303_s_std_output(sd, std_id);
+	/* store the driver data to differntiate the chip */
+	state->driver_data = (int)id->driver_data;
+
+	if (ths7303_setup(sd) < 0) {
+		v4l_err(client, "init failed\n");
+		return -EIO;
+	}
+
+	return 0;
 }
 
 static int ths7303_remove(struct i2c_client *client)
@@ -194,7 +426,8 @@ static int ths7303_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ths7303_id[] = {
-	{"ths7303", 0},
+	{"ths7303", V4L2_IDENT_THS7303},
+	{"ths7353", V4L2_IDENT_THS7353},
 	{},
 };
 
@@ -203,7 +436,7 @@ MODULE_DEVICE_TABLE(i2c, ths7303_id);
 static struct i2c_driver ths7303_driver = {
 	.driver = {
 		.owner	= THIS_MODULE,
-		.name	= "ths7303",
+		.name	= "ths73x3",
 	},
 	.probe		= ths7303_probe,
 	.remove		= ths7303_remove,
diff --git a/drivers/media/i2c/tvaudio.c b/drivers/media/i2c/tvaudio.c
index e3b33b78dd21..b72a59d3216a 100644
--- a/drivers/media/i2c/tvaudio.c
+++ b/drivers/media/i2c/tvaudio.c
@@ -1761,7 +1761,7 @@ static int tvaudio_s_routing(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int tvaudio_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int tvaudio_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct CHIPSTATE *chip = to_state(sd);
 	struct CHIPDESC *desc = chip->desc;
@@ -1803,7 +1803,7 @@ static int tvaudio_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 
 	vt->audmode = chip->audmode;
 	vt->rxsubchans = desc->getrxsubchans(chip);
-	vt->capability = V4L2_TUNER_CAP_STEREO |
+	vt->capability |= V4L2_TUNER_CAP_STEREO |
 		V4L2_TUNER_CAP_LANG1 | V4L2_TUNER_CAP_LANG2;
 
 	return 0;
@@ -1817,7 +1817,7 @@ static int tvaudio_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
 	return 0;
 }
 
-static int tvaudio_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *freq)
+static int tvaudio_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *freq)
 {
 	struct CHIPSTATE *chip = to_state(sd);
 	struct CHIPDESC *desc = chip->desc;
diff --git a/drivers/media/i2c/tvp514x.c b/drivers/media/i2c/tvp514x.c
index aa94ebc2d755..ab8f3fee7e94 100644
--- a/drivers/media/i2c/tvp514x.c
+++ b/drivers/media/i2c/tvp514x.c
@@ -12,6 +12,7 @@
  *     Hardik Shah <hardik.shah@ti.com>
  *     Manjunath Hadli <mrh@ti.com>
  *     Karicheri Muralidharan <m-karicheri2@ti.com>
+ *     Prabhakar Lad <prabhakar.lad@ti.com>
  *
  * This package is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -33,6 +34,7 @@
 #include <linux/delay.h>
 #include <linux/videodev2.h>
 #include <linux/module.h>
+#include <linux/v4l2-mediabus.h>
 
 #include <media/v4l2-device.h>
 #include <media/v4l2-common.h>
@@ -40,12 +42,10 @@
 #include <media/v4l2-chip-ident.h>
 #include <media/v4l2-ctrls.h>
 #include <media/tvp514x.h>
+#include <media/media-entity.h>
 
 #include "tvp514x_regs.h"
 
-/* Module Name */
-#define TVP514X_MODULE_NAME		"tvp514x"
-
 /* Private macros for TVP */
 #define I2C_RETRY_COUNT                 (5)
 #define LOCK_RETRY_COUNT                (5)
@@ -91,6 +91,9 @@ static int tvp514x_s_stream(struct v4l2_subdev *sd, int enable);
  * @pdata: Board specific
  * @ver: Chip version
  * @streaming: TVP5146/47 decoder streaming - enabled or disabled.
+ * @pix: Current pixel format
+ * @num_fmts: Number of formats
+ * @fmt_list: Format list
  * @current_std: Current standard
  * @num_stds: Number of standards
  * @std_list: Standards list
@@ -106,12 +109,20 @@ struct tvp514x_decoder {
 	int ver;
 	int streaming;
 
+	struct v4l2_pix_format pix;
+	int num_fmts;
+	const struct v4l2_fmtdesc *fmt_list;
+
 	enum tvp514x_std current_std;
 	int num_stds;
 	const struct tvp514x_std_info *std_list;
 	/* Input and Output Routing parameters */
 	u32 input;
 	u32 output;
+
+	/* mc related members */
+	struct media_pad pad;
+	struct v4l2_mbus_framefmt format;
 };
 
 /* TVP514x default register values */
@@ -200,6 +211,21 @@ static struct tvp514x_reg tvp514x_reg_list_default[] = {
 };
 
 /**
+ * List of image formats supported by TVP5146/47 decoder
+ * Currently we are using 8 bit mode only, but can be
+ * extended to 10/20 bit mode.
+ */
+static const struct v4l2_fmtdesc tvp514x_fmt_list[] = {
+	{
+	 .index		= 0,
+	 .type		= V4L2_BUF_TYPE_VIDEO_CAPTURE,
+	 .flags		= 0,
+	 .description	= "8-bit UYVY 4:2:2 Format",
+	 .pixelformat	= V4L2_PIX_FMT_UYVY,
+	},
+};
+
+/**
  * Supported standards -
  *
  * Currently supports two standards only, need to add support for rest of the
@@ -733,7 +759,7 @@ tvp514x_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
 }
 
 /**
- * tvp514x_mbus_fmt_cap() - V4L2 decoder interface handler for try/s/g_mbus_fmt
+ * tvp514x_mbus_fmt() - V4L2 decoder interface handler for try/s/g_mbus_fmt
  * @sd: pointer to standard V4L2 sub-device structure
  * @f: pointer to the mediabus format structure
  *
@@ -751,12 +777,11 @@ tvp514x_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f)
 	/* Calculate height and width based on current standard */
 	current_std = decoder->current_std;
 
-	f->code = V4L2_MBUS_FMT_YUYV10_2X10;
+	f->code = V4L2_MBUS_FMT_YUYV8_2X8;
 	f->width = decoder->std_list[current_std].width;
 	f->height = decoder->std_list[current_std].height;
 	f->field = V4L2_FIELD_INTERLACED;
 	f->colorspace = V4L2_COLORSPACE_SMPTE170M;
-
 	v4l2_dbg(1, debug, sd, "MBUS_FMT: Width - %d, Height - %d\n",
 			f->width, f->height);
 	return 0;
@@ -892,6 +917,88 @@ static const struct v4l2_ctrl_ops tvp514x_ctrl_ops = {
 	.s_ctrl = tvp514x_s_ctrl,
 };
 
+/**
+ * tvp514x_enum_mbus_code() - V4L2 decoder interface handler for enum_mbus_code
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @fh: file handle
+ * @code: pointer to v4l2_subdev_mbus_code_enum structure
+ *
+ * Enumertaes mbus codes supported
+ */
+static int tvp514x_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	u32 pad = code->pad;
+	u32 index = code->index;
+
+	memset(code, 0, sizeof(*code));
+	code->index = index;
+	code->pad = pad;
+
+	if (index != 0)
+		return -EINVAL;
+
+	code->code = V4L2_MBUS_FMT_YUYV8_2X8;
+
+	return 0;
+}
+
+/**
+ * tvp514x_get_pad_format() - V4L2 decoder interface handler for get pad format
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @fh: file handle
+ * @format: pointer to v4l2_subdev_format structure
+ *
+ * Retrieves pad format which is active or tried based on requirement
+ */
+static int tvp514x_get_pad_format(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_format *format)
+{
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+	__u32 which = format->which;
+
+	if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		format->format = decoder->format;
+		return 0;
+	}
+
+	format->format.code = V4L2_MBUS_FMT_YUYV8_2X8;
+	format->format.width = tvp514x_std_list[decoder->current_std].width;
+	format->format.height = tvp514x_std_list[decoder->current_std].height;
+	format->format.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	format->format.field = V4L2_FIELD_INTERLACED;
+
+	return 0;
+}
+
+/**
+ * tvp514x_set_pad_format() - V4L2 decoder interface handler for set pad format
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @fh: file handle
+ * @format: pointer to v4l2_subdev_format structure
+ *
+ * Set pad format for the output pad
+ */
+static int tvp514x_set_pad_format(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_format *fmt)
+{
+	struct tvp514x_decoder *decoder = to_decoder(sd);
+
+	if (fmt->format.field != V4L2_FIELD_INTERLACED ||
+	    fmt->format.code != V4L2_MBUS_FMT_YUYV8_2X8 ||
+	    fmt->format.colorspace != V4L2_COLORSPACE_SMPTE170M ||
+	    fmt->format.width != tvp514x_std_list[decoder->current_std].width ||
+	    fmt->format.height != tvp514x_std_list[decoder->current_std].height)
+		return -EINVAL;
+
+	decoder->format = fmt->format;
+
+	return 0;
+}
+
 static const struct v4l2_subdev_core_ops tvp514x_core_ops = {
 	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
 	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
@@ -915,13 +1022,33 @@ static const struct v4l2_subdev_video_ops tvp514x_video_ops = {
 	.s_stream = tvp514x_s_stream,
 };
 
+static const struct v4l2_subdev_pad_ops tvp514x_pad_ops = {
+	.enum_mbus_code = tvp514x_enum_mbus_code,
+	.get_fmt = tvp514x_get_pad_format,
+	.set_fmt = tvp514x_set_pad_format,
+};
+
 static const struct v4l2_subdev_ops tvp514x_ops = {
 	.core = &tvp514x_core_ops,
 	.video = &tvp514x_video_ops,
+	.pad = &tvp514x_pad_ops,
 };
 
 static struct tvp514x_decoder tvp514x_dev = {
 	.streaming = 0,
+	.fmt_list = tvp514x_fmt_list,
+	.num_fmts = ARRAY_SIZE(tvp514x_fmt_list),
+	.pix = {
+		/* Default to NTSC 8-bit YUV 422 */
+		.width		= NTSC_NUM_ACTIVE_PIXELS,
+		.height		= NTSC_NUM_ACTIVE_LINES,
+		.pixelformat	= V4L2_PIX_FMT_UYVY,
+		.field		= V4L2_FIELD_INTERLACED,
+		.bytesperline	= NTSC_NUM_ACTIVE_PIXELS * 2,
+		.sizeimage	= NTSC_NUM_ACTIVE_PIXELS * 2 *
+					NTSC_NUM_ACTIVE_LINES,
+		.colorspace	= V4L2_COLORSPACE_SMPTE170M,
+		},
 	.current_std = STD_NTSC_MJ,
 	.std_list = tvp514x_std_list,
 	.num_stds = ARRAY_SIZE(tvp514x_std_list),
@@ -941,6 +1068,7 @@ tvp514x_probe(struct i2c_client *client, const struct i2c_device_id *id)
 {
 	struct tvp514x_decoder *decoder;
 	struct v4l2_subdev *sd;
+	int ret;
 
 	/* Check if the adapter supports the needed features */
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
@@ -981,7 +1109,21 @@ tvp514x_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	/* Register with V4L2 layer as slave device */
 	sd = &decoder->sd;
 	v4l2_i2c_subdev_init(sd, client, &tvp514x_ops);
-
+	strlcpy(sd->name, TVP514X_MODULE_NAME, sizeof(sd->name));
+
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	decoder->pad.flags = MEDIA_PAD_FL_SOURCE;
+	decoder->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	decoder->sd.entity.flags |= MEDIA_ENT_T_V4L2_SUBDEV_DECODER;
+
+	ret = media_entity_init(&decoder->sd.entity, 1, &decoder->pad, 0);
+	if (ret < 0) {
+		v4l2_err(sd, "%s decoder driver failed to register !!\n",
+			 sd->name);
+		kfree(decoder);
+		return ret;
+	}
+#endif
 	v4l2_ctrl_handler_init(&decoder->hdl, 5);
 	v4l2_ctrl_new_std(&decoder->hdl, &tvp514x_ctrl_ops,
 		V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
@@ -995,10 +1137,10 @@ tvp514x_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
 	sd->ctrl_handler = &decoder->hdl;
 	if (decoder->hdl.error) {
-		int err = decoder->hdl.error;
+		ret = decoder->hdl.error;
 
 		v4l2_ctrl_handler_free(&decoder->hdl);
-		return err;
+		return ret;
 	}
 	v4l2_ctrl_handler_setup(&decoder->hdl);
 
@@ -1021,6 +1163,9 @@ static int tvp514x_remove(struct i2c_client *client)
 	struct tvp514x_decoder *decoder = to_decoder(sd);
 
 	v4l2_device_unregister_subdev(sd);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	media_entity_cleanup(&decoder->sd.entity);
+#endif
 	v4l2_ctrl_handler_free(&decoder->hdl);
 	return 0;
 }
diff --git a/drivers/media/i2c/tvp5150.c b/drivers/media/i2c/tvp5150.c
index 5967e1a0c809..485159a3c0b7 100644
--- a/drivers/media/i2c/tvp5150.c
+++ b/drivers/media/i2c/tvp5150.c
@@ -1067,7 +1067,7 @@ static int tvp5150_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *
 	return 0;
 }
 
-static int tvp5150_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int tvp5150_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/tvp7002.c b/drivers/media/i2c/tvp7002.c
index 537f6b4d4918..027809cca5f5 100644
--- a/drivers/media/i2c/tvp7002.c
+++ b/drivers/media/i2c/tvp7002.c
@@ -326,9 +326,8 @@ static const struct i2c_reg_value tvp7002_parms_720P50[] = {
 	{ TVP7002_EOR, 0xff, TVP7002_RESERVED }
 };
 
-/* Preset definition for handling device operation */
-struct tvp7002_preset_definition {
-	u32 preset;
+/* Timings definition for handling device operation */
+struct tvp7002_timings_definition {
 	struct v4l2_dv_timings timings;
 	const struct i2c_reg_value *p_settings;
 	enum v4l2_colorspace color_space;
@@ -339,10 +338,9 @@ struct tvp7002_preset_definition {
 	u16 cpl_max;
 };
 
-/* Struct list for digital video presets */
-static const struct tvp7002_preset_definition tvp7002_presets[] = {
+/* Struct list for digital video timings */
+static const struct tvp7002_timings_definition tvp7002_timings[] = {
 	{
-		V4L2_DV_720P60,
 		V4L2_DV_BT_CEA_1280X720P60,
 		tvp7002_parms_720P60,
 		V4L2_COLORSPACE_REC709,
@@ -353,7 +351,6 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 		153
 	},
 	{
-		V4L2_DV_1080I60,
 		V4L2_DV_BT_CEA_1920X1080I60,
 		tvp7002_parms_1080I60,
 		V4L2_COLORSPACE_REC709,
@@ -364,7 +361,6 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 		205
 	},
 	{
-		V4L2_DV_1080I50,
 		V4L2_DV_BT_CEA_1920X1080I50,
 		tvp7002_parms_1080I50,
 		V4L2_COLORSPACE_REC709,
@@ -375,7 +371,6 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 		245
 	},
 	{
-		V4L2_DV_720P50,
 		V4L2_DV_BT_CEA_1280X720P50,
 		tvp7002_parms_720P50,
 		V4L2_COLORSPACE_REC709,
@@ -386,7 +381,6 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 		183
 	},
 	{
-		V4L2_DV_1080P60,
 		V4L2_DV_BT_CEA_1920X1080P60,
 		tvp7002_parms_1080P60,
 		V4L2_COLORSPACE_REC709,
@@ -397,7 +391,6 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 		102
 	},
 	{
-		V4L2_DV_480P59_94,
 		V4L2_DV_BT_CEA_720X480P59_94,
 		tvp7002_parms_480P,
 		V4L2_COLORSPACE_SMPTE170M,
@@ -408,7 +401,6 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 		0xffff
 	},
 	{
-		V4L2_DV_576P50,
 		V4L2_DV_BT_CEA_720X576P50,
 		tvp7002_parms_576P,
 		V4L2_COLORSPACE_SMPTE170M,
@@ -420,7 +412,7 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 	}
 };
 
-#define NUM_PRESETS	ARRAY_SIZE(tvp7002_presets)
+#define NUM_TIMINGS ARRAY_SIZE(tvp7002_timings)
 
 /* Device definition */
 struct tvp7002 {
@@ -431,7 +423,7 @@ struct tvp7002 {
 	int ver;
 	int streaming;
 
-	const struct tvp7002_preset_definition *current_preset;
+	const struct tvp7002_timings_definition *current_timings;
 };
 
 /*
@@ -588,32 +580,6 @@ static int tvp7002_write_inittab(struct v4l2_subdev *sd,
 	return error;
 }
 
-/*
- * tvp7002_s_dv_preset() - Set digital video preset
- * @sd: ptr to v4l2_subdev struct
- * @dv_preset: ptr to v4l2_dv_preset struct
- *
- * Set the digital video preset for a TVP7002 decoder device.
- * Returns zero when successful or -EINVAL if register access fails.
- */
-static int tvp7002_s_dv_preset(struct v4l2_subdev *sd,
-					struct v4l2_dv_preset *dv_preset)
-{
-	struct tvp7002 *device = to_tvp7002(sd);
-	u32 preset;
-	int i;
-
-	for (i = 0; i < NUM_PRESETS; i++) {
-		preset = tvp7002_presets[i].preset;
-		if (preset == dv_preset->preset) {
-			device->current_preset = &tvp7002_presets[i];
-			return tvp7002_write_inittab(sd, tvp7002_presets[i].p_settings);
-		}
-	}
-
-	return -EINVAL;
-}
-
 static int tvp7002_s_dv_timings(struct v4l2_subdev *sd,
 					struct v4l2_dv_timings *dv_timings)
 {
@@ -623,12 +589,12 @@ static int tvp7002_s_dv_timings(struct v4l2_subdev *sd,
 
 	if (dv_timings->type != V4L2_DV_BT_656_1120)
 		return -EINVAL;
-	for (i = 0; i < NUM_PRESETS; i++) {
-		const struct v4l2_bt_timings *t = &tvp7002_presets[i].timings.bt;
+	for (i = 0; i < NUM_TIMINGS; i++) {
+		const struct v4l2_bt_timings *t = &tvp7002_timings[i].timings.bt;
 
 		if (!memcmp(bt, t, &bt->standards - &bt->width)) {
-			device->current_preset = &tvp7002_presets[i];
-			return tvp7002_write_inittab(sd, tvp7002_presets[i].p_settings);
+			device->current_timings = &tvp7002_timings[i];
+			return tvp7002_write_inittab(sd, tvp7002_timings[i].p_settings);
 		}
 	}
 	return -EINVAL;
@@ -639,7 +605,7 @@ static int tvp7002_g_dv_timings(struct v4l2_subdev *sd,
 {
 	struct tvp7002 *device = to_tvp7002(sd);
 
-	*dv_timings = device->current_preset->timings;
+	*dv_timings = device->current_timings->timings;
 	return 0;
 }
 
@@ -677,19 +643,13 @@ static int tvp7002_s_ctrl(struct v4l2_ctrl *ctrl)
 static int tvp7002_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f)
 {
 	struct tvp7002 *device = to_tvp7002(sd);
-	struct v4l2_dv_enum_preset e_preset;
-	int error;
+	const struct v4l2_bt_timings *bt = &device->current_timings->timings.bt;
 
-	/* Calculate height and width based on current standard */
-	error = v4l_fill_dv_preset_info(device->current_preset->preset, &e_preset);
-	if (error)
-		return error;
-
-	f->width = e_preset.width;
-	f->height = e_preset.height;
+	f->width = bt->width;
+	f->height = bt->height;
 	f->code = V4L2_MBUS_FMT_YUYV10_1X20;
-	f->field = device->current_preset->scanmode;
-	f->colorspace = device->current_preset->color_space;
+	f->field = device->current_timings->scanmode;
+	f->colorspace = device->current_timings->color_space;
 
 	v4l2_dbg(1, debug, sd, "MBUS_FMT: Width - %d, Height - %d",
 			f->width, f->height);
@@ -697,16 +657,16 @@ static int tvp7002_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f
 }
 
 /*
- * tvp7002_query_dv_preset() - query DV preset
+ * tvp7002_query_dv() - query DV timings
  * @sd: pointer to standard V4L2 sub-device structure
- * @qpreset: standard V4L2 v4l2_dv_preset structure
+ * @index: index into the tvp7002_timings array
  *
- * Returns the current DV preset by TVP7002. If no active input is
+ * Returns the current DV timings detected by TVP7002. If no active input is
  * detected, returns -EINVAL
  */
 static int tvp7002_query_dv(struct v4l2_subdev *sd, int *index)
 {
-	const struct tvp7002_preset_definition *presets = tvp7002_presets;
+	const struct tvp7002_timings_definition *timings = tvp7002_timings;
 	u8 progressive;
 	u32 lpfr;
 	u32 cpln;
@@ -717,7 +677,7 @@ static int tvp7002_query_dv(struct v4l2_subdev *sd, int *index)
 	u8 cpl_msb;
 
 	/* Return invalid index if no active input is detected */
-	*index = NUM_PRESETS;
+	*index = NUM_TIMINGS;
 
 	/* Read standards from device registers */
 	tvp7002_read_err(sd, TVP7002_L_FRAME_STAT_LSBS, &lpf_lsb, &error);
@@ -738,39 +698,23 @@ static int tvp7002_query_dv(struct v4l2_subdev *sd, int *index)
 	progressive = (lpf_msb & TVP7002_INPR_MASK) >> TVP7002_IP_SHIFT;
 
 	/* Do checking of video modes */
-	for (*index = 0; *index < NUM_PRESETS; (*index)++, presets++)
-		if (lpfr == presets->lines_per_frame &&
-			progressive == presets->progressive) {
-			if (presets->cpl_min == 0xffff)
+	for (*index = 0; *index < NUM_TIMINGS; (*index)++, timings++)
+		if (lpfr == timings->lines_per_frame &&
+			progressive == timings->progressive) {
+			if (timings->cpl_min == 0xffff)
 				break;
-			if (cpln >= presets->cpl_min && cpln <= presets->cpl_max)
+			if (cpln >= timings->cpl_min && cpln <= timings->cpl_max)
 				break;
 		}
 
-	if (*index == NUM_PRESETS) {
+	if (*index == NUM_TIMINGS) {
 		v4l2_dbg(1, debug, sd, "detection failed: lpf = %x, cpl = %x\n",
 								lpfr, cpln);
 		return -ENOLINK;
 	}
 
 	/* Update lines per frame and clocks per line info */
-	v4l2_dbg(1, debug, sd, "detected preset: %d\n", *index);
-	return 0;
-}
-
-static int tvp7002_query_dv_preset(struct v4l2_subdev *sd,
-					struct v4l2_dv_preset *qpreset)
-{
-	int index;
-	int err = tvp7002_query_dv(sd, &index);
-
-	if (err || index == NUM_PRESETS) {
-		qpreset->preset = V4L2_DV_INVALID;
-		if (err == -ENOLINK)
-			err = 0;
-		return err;
-	}
-	qpreset->preset = tvp7002_presets[index].preset;
+	v4l2_dbg(1, debug, sd, "detected timings: %d\n", *index);
 	return 0;
 }
 
@@ -782,7 +726,7 @@ static int tvp7002_query_dv_timings(struct v4l2_subdev *sd,
 
 	if (err)
 		return err;
-	*timings = tvp7002_presets[index].timings;
+	*timings = tvp7002_timings[index].timings;
 	return 0;
 }
 
@@ -824,7 +768,7 @@ static int tvp7002_g_register(struct v4l2_subdev *sd,
  * -EPERM if call not allowed.
  */
 static int tvp7002_s_register(struct v4l2_subdev *sd,
-						struct v4l2_dbg_register *reg)
+						const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
@@ -896,35 +840,21 @@ static int tvp7002_s_stream(struct v4l2_subdev *sd, int enable)
  */
 static int tvp7002_log_status(struct v4l2_subdev *sd)
 {
-	const struct tvp7002_preset_definition *presets = tvp7002_presets;
 	struct tvp7002 *device = to_tvp7002(sd);
-	struct v4l2_dv_enum_preset e_preset;
-	struct v4l2_dv_preset detected;
-	int i;
-
-	detected.preset = V4L2_DV_INVALID;
-	/* Find my current standard*/
-	tvp7002_query_dv_preset(sd, &detected);
+	const struct v4l2_bt_timings *bt;
+	int detected;
 
-	/* Print standard related code values */
-	for (i = 0; i < NUM_PRESETS; i++, presets++)
-		if (presets->preset == detected.preset)
-			break;
+	/* Find my current timings */
+	tvp7002_query_dv(sd, &detected);
 
-	if (v4l_fill_dv_preset_info(device->current_preset->preset, &e_preset))
-		return -EINVAL;
-
-	v4l2_info(sd, "Selected DV Preset: %s\n", e_preset.name);
-	v4l2_info(sd, "   Pixels per line: %u\n", e_preset.width);
-	v4l2_info(sd, "   Lines per frame: %u\n\n", e_preset.height);
-	if (i == NUM_PRESETS) {
-		v4l2_info(sd, "Detected DV Preset: None\n");
+	bt = &device->current_timings->timings.bt;
+	v4l2_info(sd, "Selected DV Timings: %ux%u\n", bt->width, bt->height);
+	if (detected == NUM_TIMINGS) {
+		v4l2_info(sd, "Detected DV Timings: None\n");
 	} else {
-		if (v4l_fill_dv_preset_info(presets->preset, &e_preset))
-			return -EINVAL;
-		v4l2_info(sd, "Detected DV Preset: %s\n", e_preset.name);
-		v4l2_info(sd, "  Pixels per line: %u\n", e_preset.width);
-		v4l2_info(sd, "  Lines per frame: %u\n\n", e_preset.height);
+		bt = &tvp7002_timings[detected].timings.bt;
+		v4l2_info(sd, "Detected DV Timings: %ux%u\n",
+				bt->width, bt->height);
 	}
 	v4l2_info(sd, "Streaming enabled: %s\n",
 					device->streaming ? "yes" : "no");
@@ -935,31 +865,14 @@ static int tvp7002_log_status(struct v4l2_subdev *sd)
 	return 0;
 }
 
-/*
- * tvp7002_enum_dv_presets() - Enum supported digital video formats
- * @sd: pointer to standard V4L2 sub-device structure
- * @preset: pointer to format struct
- *
- * Enumerate supported digital video formats.
- */
-static int tvp7002_enum_dv_presets(struct v4l2_subdev *sd,
-		struct v4l2_dv_enum_preset *preset)
-{
-	/* Check requested format index is within range */
-	if (preset->index >= NUM_PRESETS)
-		return -EINVAL;
-
-	return v4l_fill_dv_preset_info(tvp7002_presets[preset->index].preset, preset);
-}
-
 static int tvp7002_enum_dv_timings(struct v4l2_subdev *sd,
 		struct v4l2_enum_dv_timings *timings)
 {
 	/* Check requested format index is within range */
-	if (timings->index >= NUM_PRESETS)
+	if (timings->index >= NUM_TIMINGS)
 		return -EINVAL;
 
-	timings->timings = tvp7002_presets[timings->index].timings;
+	timings->timings = tvp7002_timings[timings->index].timings;
 	return 0;
 }
 
@@ -986,9 +899,6 @@ static const struct v4l2_subdev_core_ops tvp7002_core_ops = {
 
 /* Specific video subsystem operation handlers */
 static const struct v4l2_subdev_video_ops tvp7002_video_ops = {
-	.enum_dv_presets = tvp7002_enum_dv_presets,
-	.s_dv_preset = tvp7002_s_dv_preset,
-	.query_dv_preset = tvp7002_query_dv_preset,
 	.g_dv_timings = tvp7002_g_dv_timings,
 	.s_dv_timings = tvp7002_s_dv_timings,
 	.enum_dv_timings = tvp7002_enum_dv_timings,
@@ -1019,7 +929,7 @@ static int tvp7002_probe(struct i2c_client *c, const struct i2c_device_id *id)
 {
 	struct v4l2_subdev *sd;
 	struct tvp7002 *device;
-	struct v4l2_dv_preset preset;
+	struct v4l2_dv_timings timings;
 	int polarity_a;
 	int polarity_b;
 	u8 revision;
@@ -1043,7 +953,7 @@ static int tvp7002_probe(struct i2c_client *c, const struct i2c_device_id *id)
 
 	sd = &device->sd;
 	device->pdata = c->dev.platform_data;
-	device->current_preset = tvp7002_presets;
+	device->current_timings = tvp7002_timings;
 
 	/* Tell v4l2 the device is ready */
 	v4l2_i2c_subdev_init(sd, c, &tvp7002_ops);
@@ -1080,8 +990,8 @@ static int tvp7002_probe(struct i2c_client *c, const struct i2c_device_id *id)
 		return error;
 
 	/* Set registers according to default video mode */
-	preset.preset = device->current_preset->preset;
-	error = tvp7002_s_dv_preset(sd, &preset);
+	timings = device->current_timings->timings;
+	error = tvp7002_s_dv_timings(sd, &timings);
 
 	v4l2_ctrl_handler_init(&device->hdl, 1);
 	v4l2_ctrl_new_std(&device->hdl, &tvp7002_ctrl_ops,
diff --git a/drivers/media/i2c/tw2804.c b/drivers/media/i2c/tw2804.c
new file mode 100644
index 000000000000..c5dc2c3bf2d7
--- /dev/null
+++ b/drivers/media/i2c/tw2804.c
@@ -0,0 +1,453 @@
+/*
+ * Copyright (C) 2005-2006 Micronas USA Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License (Version 2) as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/ioctl.h>
+#include <linux/slab.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
+
+#define TW2804_REG_AUTOGAIN		0x02
+#define TW2804_REG_HUE			0x0f
+#define TW2804_REG_SATURATION		0x10
+#define TW2804_REG_CONTRAST		0x11
+#define TW2804_REG_BRIGHTNESS		0x12
+#define TW2804_REG_COLOR_KILLER		0x14
+#define TW2804_REG_GAIN			0x3c
+#define TW2804_REG_CHROMA_GAIN		0x3d
+#define TW2804_REG_BLUE_BALANCE		0x3e
+#define TW2804_REG_RED_BALANCE		0x3f
+
+struct tw2804 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	u8 channel:2;
+	u8 input:1;
+	int norm;
+};
+
+static const u8 global_registers[] = {
+	0x39, 0x00,
+	0x3a, 0xff,
+	0x3b, 0x84,
+	0x3c, 0x80,
+	0x3d, 0x80,
+	0x3e, 0x82,
+	0x3f, 0x82,
+	0x78, 0x00,
+	0xff, 0xff, /* Terminator (reg 0xff does not exist) */
+};
+
+static const u8 channel_registers[] = {
+	0x01, 0xc4,
+	0x02, 0xa5,
+	0x03, 0x20,
+	0x04, 0xd0,
+	0x05, 0x20,
+	0x06, 0xd0,
+	0x07, 0x88,
+	0x08, 0x20,
+	0x09, 0x07,
+	0x0a, 0xf0,
+	0x0b, 0x07,
+	0x0c, 0xf0,
+	0x0d, 0x40,
+	0x0e, 0xd2,
+	0x0f, 0x80,
+	0x10, 0x80,
+	0x11, 0x80,
+	0x12, 0x80,
+	0x13, 0x1f,
+	0x14, 0x00,
+	0x15, 0x00,
+	0x16, 0x00,
+	0x17, 0x00,
+	0x18, 0xff,
+	0x19, 0xff,
+	0x1a, 0xff,
+	0x1b, 0xff,
+	0x1c, 0xff,
+	0x1d, 0xff,
+	0x1e, 0xff,
+	0x1f, 0xff,
+	0x20, 0x07,
+	0x21, 0x07,
+	0x22, 0x00,
+	0x23, 0x91,
+	0x24, 0x51,
+	0x25, 0x03,
+	0x26, 0x00,
+	0x27, 0x00,
+	0x28, 0x00,
+	0x29, 0x00,
+	0x2a, 0x00,
+	0x2b, 0x00,
+	0x2c, 0x00,
+	0x2d, 0x00,
+	0x2e, 0x00,
+	0x2f, 0x00,
+	0x30, 0x00,
+	0x31, 0x00,
+	0x32, 0x00,
+	0x33, 0x00,
+	0x34, 0x00,
+	0x35, 0x00,
+	0x36, 0x00,
+	0x37, 0x00,
+	0xff, 0xff, /* Terminator (reg 0xff does not exist) */
+};
+
+static int write_reg(struct i2c_client *client, u8 reg, u8 value, u8 channel)
+{
+	return i2c_smbus_write_byte_data(client, reg | (channel << 6), value);
+}
+
+static int write_regs(struct i2c_client *client, const u8 *regs, u8 channel)
+{
+	int ret;
+	int i;
+
+	for (i = 0; regs[i] != 0xff; i += 2) {
+		ret = i2c_smbus_write_byte_data(client,
+				regs[i] | (channel << 6), regs[i + 1]);
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
+static int read_reg(struct i2c_client *client, u8 reg, u8 channel)
+{
+	return i2c_smbus_read_byte_data(client, (reg) | (channel << 6));
+}
+
+static inline struct tw2804 *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tw2804, sd);
+}
+
+static inline struct tw2804 *to_state_from_ctrl(struct v4l2_ctrl *ctrl)
+{
+	return container_of(ctrl->handler, struct tw2804, hdl);
+}
+
+static int tw2804_log_status(struct v4l2_subdev *sd)
+{
+	struct tw2804 *state = to_state(sd);
+
+	v4l2_info(sd, "Standard: %s\n",
+			state->norm & V4L2_STD_525_60 ? "60 Hz" : "50 Hz");
+	v4l2_info(sd, "Channel: %d\n", state->channel);
+	v4l2_info(sd, "Input: %d\n", state->input);
+	return v4l2_ctrl_subdev_log_status(sd);
+}
+
+/*
+ * These volatile controls are needed because all four channels share
+ * these controls. So a change made to them through one channel would
+ * require another channel to be updated.
+ *
+ * Normally this would have been done in a different way, but since the one
+ * board that uses this driver sees this single chip as if it was on four
+ * different i2c adapters (each adapter belonging to a separate instance of
+ * the same USB driver) there is no reliable method that I have found to let
+ * the instances know about each other.
+ *
+ * So implementing these global registers as volatile is the best we can do.
+ */
+static int tw2804_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tw2804 *state = to_state_from_ctrl(ctrl);
+	struct i2c_client *client = v4l2_get_subdevdata(&state->sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_GAIN:
+		ctrl->val = read_reg(client, TW2804_REG_GAIN, 0);
+		return 0;
+
+	case V4L2_CID_CHROMA_GAIN:
+		ctrl->val = read_reg(client, TW2804_REG_CHROMA_GAIN, 0);
+		return 0;
+
+	case V4L2_CID_BLUE_BALANCE:
+		ctrl->val = read_reg(client, TW2804_REG_BLUE_BALANCE, 0);
+		return 0;
+
+	case V4L2_CID_RED_BALANCE:
+		ctrl->val = read_reg(client, TW2804_REG_RED_BALANCE, 0);
+		return 0;
+	}
+	return 0;
+}
+
+static int tw2804_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tw2804 *state = to_state_from_ctrl(ctrl);
+	struct i2c_client *client = v4l2_get_subdevdata(&state->sd);
+	int addr;
+	int reg;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		addr = TW2804_REG_AUTOGAIN;
+		reg = read_reg(client, addr, state->channel);
+		if (reg < 0)
+			return reg;
+		if (ctrl->val == 0)
+			reg &= ~(1 << 7);
+		else
+			reg |= 1 << 7;
+		return write_reg(client, addr, reg, state->channel);
+
+	case V4L2_CID_COLOR_KILLER:
+		addr = TW2804_REG_COLOR_KILLER;
+		reg = read_reg(client, addr, state->channel);
+		if (reg < 0)
+			return reg;
+		reg = (reg & ~(0x03)) | (ctrl->val == 0 ? 0x02 : 0x03);
+		return write_reg(client, addr, reg, state->channel);
+
+	case V4L2_CID_GAIN:
+		return write_reg(client, TW2804_REG_GAIN, ctrl->val, 0);
+
+	case V4L2_CID_CHROMA_GAIN:
+		return write_reg(client, TW2804_REG_CHROMA_GAIN, ctrl->val, 0);
+
+	case V4L2_CID_BLUE_BALANCE:
+		return write_reg(client, TW2804_REG_BLUE_BALANCE, ctrl->val, 0);
+
+	case V4L2_CID_RED_BALANCE:
+		return write_reg(client, TW2804_REG_RED_BALANCE, ctrl->val, 0);
+
+	case V4L2_CID_BRIGHTNESS:
+		return write_reg(client, TW2804_REG_BRIGHTNESS,
+				ctrl->val, state->channel);
+
+	case V4L2_CID_CONTRAST:
+		return write_reg(client, TW2804_REG_CONTRAST,
+				ctrl->val, state->channel);
+
+	case V4L2_CID_SATURATION:
+		return write_reg(client, TW2804_REG_SATURATION,
+				ctrl->val, state->channel);
+
+	case V4L2_CID_HUE:
+		return write_reg(client, TW2804_REG_HUE,
+				ctrl->val, state->channel);
+
+	default:
+		break;
+	}
+	return -EINVAL;
+}
+
+static int tw2804_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct tw2804 *dec = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	bool is_60hz = norm & V4L2_STD_525_60;
+	u8 regs[] = {
+		0x01, is_60hz ? 0xc4 : 0x84,
+		0x09, is_60hz ? 0x07 : 0x04,
+		0x0a, is_60hz ? 0xf0 : 0x20,
+		0x0b, is_60hz ? 0x07 : 0x04,
+		0x0c, is_60hz ? 0xf0 : 0x20,
+		0x0d, is_60hz ? 0x40 : 0x4a,
+		0x16, is_60hz ? 0x00 : 0x40,
+		0x17, is_60hz ? 0x00 : 0x40,
+		0x20, is_60hz ? 0x07 : 0x0f,
+		0x21, is_60hz ? 0x07 : 0x0f,
+		0xff, 0xff,
+	};
+
+	write_regs(client, regs, dec->channel);
+	dec->norm = norm;
+	return 0;
+}
+
+static int tw2804_s_video_routing(struct v4l2_subdev *sd, u32 input, u32 output,
+	u32 config)
+{
+	struct tw2804 *dec = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int reg;
+
+	if (config && config - 1 != dec->channel) {
+		if (config > 4) {
+			dev_err(&client->dev,
+				"channel %d is not between 1 and 4!\n", config);
+			return -EINVAL;
+		}
+		dec->channel = config - 1;
+		dev_dbg(&client->dev, "initializing TW2804 channel %d\n",
+			dec->channel);
+		if (dec->channel == 0 &&
+				write_regs(client, global_registers, 0) < 0) {
+			dev_err(&client->dev,
+				"error initializing TW2804 global registers\n");
+			return -EIO;
+		}
+		if (write_regs(client, channel_registers, dec->channel) < 0) {
+			dev_err(&client->dev,
+				"error initializing TW2804 channel %d\n",
+				dec->channel);
+			return -EIO;
+		}
+	}
+
+	if (input > 1)
+		return -EINVAL;
+
+	if (input == dec->input)
+		return 0;
+
+	reg = read_reg(client, 0x22, dec->channel);
+
+	if (reg >= 0) {
+		if (input == 0)
+			reg &= ~(1 << 2);
+		else
+			reg |= 1 << 2;
+		reg = write_reg(client, 0x22, reg, dec->channel);
+	}
+
+	if (reg >= 0)
+		dec->input = input;
+	else
+		return reg;
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops tw2804_ctrl_ops = {
+	.g_volatile_ctrl = tw2804_g_volatile_ctrl,
+	.s_ctrl = tw2804_s_ctrl,
+};
+
+static const struct v4l2_subdev_video_ops tw2804_video_ops = {
+	.s_routing = tw2804_s_video_routing,
+};
+
+static const struct v4l2_subdev_core_ops tw2804_core_ops = {
+	.log_status = tw2804_log_status,
+	.s_std = tw2804_s_std,
+};
+
+static const struct v4l2_subdev_ops tw2804_ops = {
+	.core = &tw2804_core_ops,
+	.video = &tw2804_video_ops,
+};
+
+static int tw2804_probe(struct i2c_client *client,
+			    const struct i2c_device_id *id)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	struct tw2804 *state;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl *ctrl;
+	int err;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	state = kzalloc(sizeof(struct tw2804), GFP_KERNEL);
+
+	if (state == NULL)
+		return -ENOMEM;
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &tw2804_ops);
+	state->channel = -1;
+	state->norm = V4L2_STD_NTSC;
+
+	v4l2_ctrl_handler_init(&state->hdl, 10);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_CONTRAST, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_SATURATION, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_HUE, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_COLOR_KILLER, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_AUTOGAIN, 0, 1, 1, 0);
+	ctrl = v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_GAIN, 0, 255, 1, 128);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrl = v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_CHROMA_GAIN, 0, 255, 1, 128);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrl = v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_BLUE_BALANCE, 0, 255, 1, 122);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrl = v4l2_ctrl_new_std(&state->hdl, &tw2804_ctrl_ops,
+				V4L2_CID_RED_BALANCE, 0, 255, 1, 122);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	sd->ctrl_handler = &state->hdl;
+	err = state->hdl.error;
+	if (err) {
+		v4l2_ctrl_handler_free(&state->hdl);
+		kfree(state);
+		return err;
+	}
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	return 0;
+}
+
+static int tw2804_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct tw2804 *state = to_state(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+	kfree(state);
+	return 0;
+}
+
+static const struct i2c_device_id tw2804_id[] = {
+	{ "tw2804", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tw2804_id);
+
+static struct i2c_driver tw2804_driver = {
+	.driver = {
+		.name	= "tw2804",
+	},
+	.probe		= tw2804_probe,
+	.remove		= tw2804_remove,
+	.id_table	= tw2804_id,
+};
+
+module_i2c_driver(tw2804_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("TW2804/TW2802 V4L2 i2c driver");
+MODULE_AUTHOR("Micronas USA Inc");
diff --git a/drivers/media/i2c/tw9903.c b/drivers/media/i2c/tw9903.c
new file mode 100644
index 000000000000..87880b19d8c3
--- /dev/null
+++ b/drivers/media/i2c/tw9903.c
@@ -0,0 +1,279 @@
+/*
+ * Copyright (C) 2005-2006 Micronas USA Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License (Version 2) as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/ioctl.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <linux/slab.h>
+
+MODULE_DESCRIPTION("TW9903 I2C subdev driver");
+MODULE_LICENSE("GPL v2");
+
+/*
+ * This driver is based on the wis-tw9903.c source that was in
+ * drivers/staging/media/go7007. That source had commented out code for
+ * saturation and scaling (neither seemed to work). If anyone ever gets
+ * hardware to test this driver, then that code might be useful to look at.
+ * You need to get the kernel sources of, say, kernel 3.8 where that
+ * wis-tw9903 driver is still present.
+ */
+
+struct tw9903 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	v4l2_std_id norm;
+};
+
+static inline struct tw9903 *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tw9903, sd);
+}
+
+static const u8 initial_registers[] = {
+	0x02, 0x44, /* input 1, composite */
+	0x03, 0x92, /* correct digital format */
+	0x04, 0x00,
+	0x05, 0x80, /* or 0x00 for PAL */
+	0x06, 0x40, /* second internal current reference */
+	0x07, 0x02, /* window */
+	0x08, 0x14, /* window */
+	0x09, 0xf0, /* window */
+	0x0a, 0x81, /* window */
+	0x0b, 0xd0, /* window */
+	0x0c, 0x8c,
+	0x0d, 0x00, /* scaling */
+	0x0e, 0x11, /* scaling */
+	0x0f, 0x00, /* scaling */
+	0x10, 0x00, /* brightness */
+	0x11, 0x60, /* contrast */
+	0x12, 0x01, /* sharpness */
+	0x13, 0x7f, /* U gain */
+	0x14, 0x5a, /* V gain */
+	0x15, 0x00, /* hue */
+	0x16, 0xc3, /* sharpness */
+	0x18, 0x00,
+	0x19, 0x58, /* vbi */
+	0x1a, 0x80,
+	0x1c, 0x0f, /* video norm */
+	0x1d, 0x7f, /* video norm */
+	0x20, 0xa0, /* clamping gain (working 0x50) */
+	0x21, 0x22,
+	0x22, 0xf0,
+	0x23, 0xfe,
+	0x24, 0x3c,
+	0x25, 0x38,
+	0x26, 0x44,
+	0x27, 0x20,
+	0x28, 0x00,
+	0x29, 0x15,
+	0x2a, 0xa0,
+	0x2b, 0x44,
+	0x2c, 0x37,
+	0x2d, 0x00,
+	0x2e, 0xa5, /* burst PLL control (working: a9) */
+	0x2f, 0xe0, /* 0xea is blue test frame -- 0xe0 for normal */
+	0x31, 0x00,
+	0x33, 0x22,
+	0x34, 0x11,
+	0x35, 0x35,
+	0x3b, 0x05,
+	0x06, 0xc0, /* reset device */
+	0x00, 0x00, /* Terminator (reg 0x00 is read-only) */
+};
+
+static int write_reg(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static int write_regs(struct v4l2_subdev *sd, const u8 *regs)
+{
+	int i;
+
+	for (i = 0; regs[i] != 0x00; i += 2)
+		if (write_reg(sd, regs[i], regs[i + 1]) < 0)
+			return -1;
+	return 0;
+}
+
+static int tw9903_s_video_routing(struct v4l2_subdev *sd, u32 input,
+				      u32 output, u32 config)
+{
+	write_reg(sd, 0x02, 0x40 | (input << 1));
+	return 0;
+}
+
+static int tw9903_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct tw9903 *dec = to_state(sd);
+	bool is_60hz = norm & V4L2_STD_525_60;
+	static const u8 config_60hz[] = {
+		0x05, 0x80,
+		0x07, 0x02,
+		0x08, 0x14,
+		0x09, 0xf0,
+		0,    0,
+	};
+	static const u8 config_50hz[] = {
+		0x05, 0x00,
+		0x07, 0x12,
+		0x08, 0x18,
+		0x09, 0x20,
+		0,    0,
+	};
+
+	write_regs(sd, is_60hz ? config_60hz : config_50hz);
+	dec->norm = norm;
+	return 0;
+}
+
+
+static int tw9903_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tw9903 *dec = container_of(ctrl->handler, struct tw9903, hdl);
+	struct v4l2_subdev *sd = &dec->sd;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		write_reg(sd, 0x10, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		write_reg(sd, 0x11, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		write_reg(sd, 0x15, ctrl->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int tw9903_log_status(struct v4l2_subdev *sd)
+{
+	struct tw9903 *dec = to_state(sd);
+	bool is_60hz = dec->norm & V4L2_STD_525_60;
+
+	v4l2_info(sd, "Standard: %d Hz\n", is_60hz ? 60 : 50);
+	v4l2_ctrl_subdev_log_status(sd);
+	return 0;
+}
+
+/* --------------------------------------------------------------------------*/
+
+static const struct v4l2_ctrl_ops tw9903_ctrl_ops = {
+	.s_ctrl = tw9903_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops tw9903_core_ops = {
+	.log_status = tw9903_log_status,
+	.s_std = tw9903_s_std,
+};
+
+static const struct v4l2_subdev_video_ops tw9903_video_ops = {
+	.s_routing = tw9903_s_video_routing,
+};
+
+static const struct v4l2_subdev_ops tw9903_ops = {
+	.core = &tw9903_core_ops,
+	.video = &tw9903_video_ops,
+};
+
+/* --------------------------------------------------------------------------*/
+
+static int tw9903_probe(struct i2c_client *client,
+			     const struct i2c_device_id *id)
+{
+	struct tw9903 *dec;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl_handler *hdl;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	dec = kzalloc(sizeof(struct tw9903), GFP_KERNEL);
+	if (dec == NULL)
+		return -ENOMEM;
+	sd = &dec->sd;
+	v4l2_i2c_subdev_init(sd, client, &tw9903_ops);
+	hdl = &dec->hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &tw9903_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &tw9903_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 255, 1, 0x60);
+	v4l2_ctrl_new_std(hdl, &tw9903_ctrl_ops,
+		V4L2_CID_HUE, -128, 127, 1, 0);
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		int err = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		kfree(dec);
+		return err;
+	}
+
+	/* Initialize tw9903 */
+	dec->norm = V4L2_STD_NTSC;
+
+	if (write_regs(sd, initial_registers) < 0) {
+		v4l2_err(client, "error initializing TW9903\n");
+		kfree(dec);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int tw9903_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&to_state(sd)->hdl);
+	kfree(to_state(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id tw9903_id[] = {
+	{ "tw9903", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tw9903_id);
+
+static struct i2c_driver tw9903_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "tw9903",
+	},
+	.probe = tw9903_probe,
+	.remove = tw9903_remove,
+	.id_table = tw9903_id,
+};
+module_i2c_driver(tw9903_driver);
diff --git a/drivers/media/i2c/tw9906.c b/drivers/media/i2c/tw9906.c
new file mode 100644
index 000000000000..accd79e5a7fd
--- /dev/null
+++ b/drivers/media/i2c/tw9906.c
@@ -0,0 +1,247 @@
+/*
+ * Copyright (C) 2005-2006 Micronas USA Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License (Version 2) as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/ioctl.h>
+#include <linux/slab.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+MODULE_DESCRIPTION("TW9906 I2C subdev driver");
+MODULE_LICENSE("GPL v2");
+
+struct tw9906 {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	v4l2_std_id norm;
+};
+
+static inline struct tw9906 *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct tw9906, sd);
+}
+
+static const u8 initial_registers[] = {
+	0x02, 0x40, /* input 0, composite */
+	0x03, 0xa2, /* correct digital format */
+	0x05, 0x81, /* or 0x01 for PAL */
+	0x07, 0x02, /* window */
+	0x08, 0x14, /* window */
+	0x09, 0xf0, /* window */
+	0x0a, 0x10, /* window */
+	0x0b, 0xd0, /* window */
+	0x0d, 0x00, /* scaling */
+	0x0e, 0x11, /* scaling */
+	0x0f, 0x00, /* scaling */
+	0x10, 0x00, /* brightness */
+	0x11, 0x60, /* contrast */
+	0x12, 0x11, /* sharpness */
+	0x13, 0x7e, /* U gain */
+	0x14, 0x7e, /* V gain */
+	0x15, 0x00, /* hue */
+	0x19, 0x57, /* vbi */
+	0x1a, 0x0f,
+	0x1b, 0x40,
+	0x29, 0x03,
+	0x55, 0x00,
+	0x6b, 0x26,
+	0x6c, 0x36,
+	0x6d, 0xf0,
+	0x6e, 0x41,
+	0x6f, 0x13,
+	0xad, 0x70,
+	0x00, 0x00, /* Terminator (reg 0x00 is read-only) */
+};
+
+static int write_reg(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static int write_regs(struct v4l2_subdev *sd, const u8 *regs)
+{
+	int i;
+
+	for (i = 0; regs[i] != 0x00; i += 2)
+		if (write_reg(sd, regs[i], regs[i + 1]) < 0)
+			return -1;
+	return 0;
+}
+
+static int tw9906_s_video_routing(struct v4l2_subdev *sd, u32 input,
+				      u32 output, u32 config)
+{
+	write_reg(sd, 0x02, 0x40 | (input << 1));
+	return 0;
+}
+
+static int tw9906_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct tw9906 *dec = to_state(sd);
+	bool is_60hz = norm & V4L2_STD_525_60;
+	static const u8 config_60hz[] = {
+		0x05, 0x81,
+		0x07, 0x02,
+		0x08, 0x14,
+		0x09, 0xf0,
+		0,    0,
+	};
+	static const u8 config_50hz[] = {
+		0x05, 0x01,
+		0x07, 0x12,
+		0x08, 0x18,
+		0x09, 0x20,
+		0,    0,
+	};
+
+	write_regs(sd, is_60hz ? config_60hz : config_50hz);
+	dec->norm = norm;
+	return 0;
+}
+
+static int tw9906_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tw9906 *dec = container_of(ctrl->handler, struct tw9906, hdl);
+	struct v4l2_subdev *sd = &dec->sd;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		write_reg(sd, 0x10, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		write_reg(sd, 0x11, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		write_reg(sd, 0x15, ctrl->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int tw9906_log_status(struct v4l2_subdev *sd)
+{
+	struct tw9906 *dec = to_state(sd);
+	bool is_60hz = dec->norm & V4L2_STD_525_60;
+
+	v4l2_info(sd, "Standard: %d Hz\n", is_60hz ? 60 : 50);
+	v4l2_ctrl_subdev_log_status(sd);
+	return 0;
+}
+
+/* --------------------------------------------------------------------------*/
+
+static const struct v4l2_ctrl_ops tw9906_ctrl_ops = {
+	.s_ctrl = tw9906_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops tw9906_core_ops = {
+	.log_status = tw9906_log_status,
+	.s_std = tw9906_s_std,
+};
+
+static const struct v4l2_subdev_video_ops tw9906_video_ops = {
+	.s_routing = tw9906_s_video_routing,
+};
+
+static const struct v4l2_subdev_ops tw9906_ops = {
+	.core = &tw9906_core_ops,
+	.video = &tw9906_video_ops,
+};
+
+static int tw9906_probe(struct i2c_client *client,
+			     const struct i2c_device_id *id)
+{
+	struct tw9906 *dec;
+	struct v4l2_subdev *sd;
+	struct v4l2_ctrl_handler *hdl;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	dec = kzalloc(sizeof(struct tw9906), GFP_KERNEL);
+	if (dec == NULL)
+		return -ENOMEM;
+	sd = &dec->sd;
+	v4l2_i2c_subdev_init(sd, client, &tw9906_ops);
+	hdl = &dec->hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &tw9906_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &tw9906_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 255, 1, 0x60);
+	v4l2_ctrl_new_std(hdl, &tw9906_ctrl_ops,
+		V4L2_CID_HUE, -128, 127, 1, 0);
+	sd->ctrl_handler = hdl;
+	if (hdl->error) {
+		int err = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		kfree(dec);
+		return err;
+	}
+
+	/* Initialize tw9906 */
+	dec->norm = V4L2_STD_NTSC;
+
+	if (write_regs(sd, initial_registers) < 0) {
+		v4l2_err(client, "error initializing TW9906\n");
+		kfree(dec);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int tw9906_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&to_state(sd)->hdl);
+	kfree(to_state(sd));
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id tw9906_id[] = {
+	{ "tw9906", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tw9906_id);
+
+static struct i2c_driver tw9906_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "tw9906",
+	},
+	.probe = tw9906_probe,
+	.remove = tw9906_remove,
+	.id_table = tw9906_id,
+};
+module_i2c_driver(tw9906_driver);
diff --git a/drivers/staging/media/go7007/wis-uda1342.c b/drivers/media/i2c/uda1342.c
index 582ea120a531..3af408556d27 100644
--- a/drivers/staging/media/go7007/wis-uda1342.c
+++ b/drivers/media/i2c/uda1342.c
@@ -19,10 +19,9 @@
 #include <linux/init.h>
 #include <linux/i2c.h>
 #include <linux/videodev2.h>
-#include <media/tvaudio.h>
-#include <media/v4l2-common.h>
-
-#include "wis-i2c.h"
+#include <media/v4l2-device.h>
+#include <media/uda1342.h>
+#include <linux/slab.h>
 
 static int write_reg(struct i2c_client *client, int reg, int value)
 {
@@ -31,38 +30,38 @@ static int write_reg(struct i2c_client *client, int reg, int value)
 	return 0;
 }
 
-static int wis_uda1342_command(struct i2c_client *client,
-				unsigned int cmd, void *arg)
+static int uda1342_s_routing(struct v4l2_subdev *sd,
+		u32 input, u32 output, u32 config)
 {
-	switch (cmd) {
-	case VIDIOC_S_AUDIO:
-	{
-		int *inp = arg;
-
-		switch (*inp) {
-		case TVAUDIO_INPUT_TUNER:
-			write_reg(client, 0x00, 0x1441); /* select input 2 */
-			break;
-		case TVAUDIO_INPUT_EXTERN:
-			write_reg(client, 0x00, 0x1241); /* select input 1 */
-			break;
-		default:
-			dev_err(&client->dev, "input %d not supported\n",
-				*inp);
-			break;
-		}
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	switch (input) {
+	case UDA1342_IN1:
+		write_reg(client, 0x00, 0x1241); /* select input 1 */
+		break;
+	case UDA1342_IN2:
+		write_reg(client, 0x00, 0x1441); /* select input 2 */
 		break;
-	}
 	default:
+		v4l2_err(sd, "input %d not supported\n", input);
 		break;
 	}
 	return 0;
 }
 
-static int wis_uda1342_probe(struct i2c_client *client,
+static const struct v4l2_subdev_audio_ops uda1342_audio_ops = {
+	.s_routing = uda1342_s_routing,
+};
+
+static const struct v4l2_subdev_ops uda1342_ops = {
+	.audio = &uda1342_audio_ops,
+};
+
+static int uda1342_probe(struct i2c_client *client,
 			     const struct i2c_device_id *id)
 {
 	struct i2c_adapter *adapter = client->adapter;
+	struct v4l2_subdev *sd;
 
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA))
 		return -ENODEV;
@@ -70,33 +69,45 @@ static int wis_uda1342_probe(struct i2c_client *client,
 	dev_dbg(&client->dev, "initializing UDA1342 at address %d on %s\n",
 		client->addr, adapter->name);
 
+	sd = kzalloc(sizeof(struct v4l2_subdev), GFP_KERNEL);
+	if (sd == NULL)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(sd, client, &uda1342_ops);
+
 	write_reg(client, 0x00, 0x8000); /* reset registers */
 	write_reg(client, 0x00, 0x1241); /* select input 1 */
 
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
 	return 0;
 }
 
-static int wis_uda1342_remove(struct i2c_client *client)
+static int uda1342_remove(struct i2c_client *client)
 {
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(sd);
 	return 0;
 }
 
-static const struct i2c_device_id wis_uda1342_id[] = {
-	{ "wis_uda1342", 0 },
+static const struct i2c_device_id uda1342_id[] = {
+	{ "uda1342", 0 },
 	{ }
 };
-MODULE_DEVICE_TABLE(i2c, wis_uda1342_id);
+MODULE_DEVICE_TABLE(i2c, uda1342_id);
 
-static struct i2c_driver wis_uda1342_driver = {
+static struct i2c_driver uda1342_driver = {
 	.driver = {
-		.name	= "WIS UDA1342 I2C driver",
+		.name	= "uda1342",
 	},
-	.probe		= wis_uda1342_probe,
-	.remove		= wis_uda1342_remove,
-	.command	= wis_uda1342_command,
-	.id_table	= wis_uda1342_id,
+	.probe		= uda1342_probe,
+	.remove		= uda1342_remove,
+	.id_table	= uda1342_id,
 };
 
-module_i2c_driver(wis_uda1342_driver);
+module_i2c_driver(uda1342_driver);
 
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/upd64031a.c b/drivers/media/i2c/upd64031a.c
index 1e7446542091..f0a09214c519 100644
--- a/drivers/media/i2c/upd64031a.c
+++ b/drivers/media/i2c/upd64031a.c
@@ -111,7 +111,7 @@ static void upd64031a_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 /* ------------------------------------------------------------------------ */
 
 /* The input changed due to new input or channel changed */
-static int upd64031a_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *freq)
+static int upd64031a_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *freq)
 {
 	struct upd64031a_state *state = to_state(sd);
 	u8 reg = state->regs[R00];
@@ -175,7 +175,7 @@ static int upd64031a_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register
 	return 0;
 }
 
-static int upd64031a_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int upd64031a_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/upd64083.c b/drivers/media/i2c/upd64083.c
index 75d6acc62018..343e0215f74c 100644
--- a/drivers/media/i2c/upd64083.c
+++ b/drivers/media/i2c/upd64083.c
@@ -133,7 +133,7 @@ static int upd64083_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register
 	return 0;
 }
 
-static int upd64083_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int upd64083_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/vp27smpx.c b/drivers/media/i2c/vp27smpx.c
index 7cfbc9d94a48..e71f139695af 100644
--- a/drivers/media/i2c/vp27smpx.c
+++ b/drivers/media/i2c/vp27smpx.c
@@ -90,7 +90,7 @@ static int vp27smpx_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
 	return 0;
 }
 
-static int vp27smpx_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int vp27smpx_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct vp27smpx_state *state = to_state(sd);
 
diff --git a/drivers/media/i2c/vs6624.c b/drivers/media/i2c/vs6624.c
index 9ac1b8c3a837..f366fad6269e 100644
--- a/drivers/media/i2c/vs6624.c
+++ b/drivers/media/i2c/vs6624.c
@@ -748,7 +748,7 @@ static int vs6624_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *r
 	return 0;
 }
 
-static int vs6624_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+static int vs6624_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/i2c/wm8775.c b/drivers/media/i2c/wm8775.c
index bee77ea9f49e..27c27b4ae238 100644
--- a/drivers/media/i2c/wm8775.c
+++ b/drivers/media/i2c/wm8775.c
@@ -174,7 +174,7 @@ static int wm8775_log_status(struct v4l2_subdev *sd)
 	return 0;
 }
 
-static int wm8775_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *freq)
+static int wm8775_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *freq)
 {
 	wm8775_set_audio(sd, 0);
 	return 0;
diff --git a/drivers/media/media-device.c b/drivers/media/media-device.c
index d01fcb7e87c2..1957c0df08fd 100644
--- a/drivers/media/media-device.c
+++ b/drivers/media/media-device.c
@@ -20,10 +20,11 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
-#include <linux/types.h>
+#include <linux/compat.h>
+#include <linux/export.h>
 #include <linux/ioctl.h>
 #include <linux/media.h>
-#include <linux/export.h>
+#include <linux/types.h>
 
 #include <media/media-device.h>
 #include <media/media-devnode.h>
@@ -101,9 +102,12 @@ static long media_device_enum_entities(struct media_device *mdev,
 		return -EINVAL;
 
 	u_ent.id = ent->id;
-	u_ent.name[0] = '\0';
-	if (ent->name)
-		strlcpy(u_ent.name, ent->name, sizeof(u_ent.name));
+	if (ent->name) {
+		strncpy(u_ent.name, ent->name, sizeof(u_ent.name));
+		u_ent.name[sizeof(u_ent.name) - 1] = '\0';
+	} else {
+		memset(u_ent.name, 0, sizeof(u_ent.name));
+	}
 	u_ent.type = ent->type;
 	u_ent.revision = ent->revision;
 	u_ent.flags = ent->flags;
@@ -124,35 +128,31 @@ static void media_device_kpad_to_upad(const struct media_pad *kpad,
 	upad->flags = kpad->flags;
 }
 
-static long media_device_enum_links(struct media_device *mdev,
-				    struct media_links_enum __user *ulinks)
+static long __media_device_enum_links(struct media_device *mdev,
+				      struct media_links_enum *links)
 {
 	struct media_entity *entity;
-	struct media_links_enum links;
 
-	if (copy_from_user(&links, ulinks, sizeof(links)))
-		return -EFAULT;
-
-	entity = find_entity(mdev, links.entity);
+	entity = find_entity(mdev, links->entity);
 	if (entity == NULL)
 		return -EINVAL;
 
-	if (links.pads) {
+	if (links->pads) {
 		unsigned int p;
 
 		for (p = 0; p < entity->num_pads; p++) {
 			struct media_pad_desc pad;
 			media_device_kpad_to_upad(&entity->pads[p], &pad);
-			if (copy_to_user(&links.pads[p], &pad, sizeof(pad)))
+			if (copy_to_user(&links->pads[p], &pad, sizeof(pad)))
 				return -EFAULT;
 		}
 	}
 
-	if (links.links) {
+	if (links->links) {
 		struct media_link_desc __user *ulink;
 		unsigned int l;
 
-		for (l = 0, ulink = links.links; l < entity->num_links; l++) {
+		for (l = 0, ulink = links->links; l < entity->num_links; l++) {
 			struct media_link_desc link;
 
 			/* Ignore backlinks. */
@@ -169,8 +169,26 @@ static long media_device_enum_links(struct media_device *mdev,
 			ulink++;
 		}
 	}
+
+	return 0;
+}
+
+static long media_device_enum_links(struct media_device *mdev,
+				    struct media_links_enum __user *ulinks)
+{
+	struct media_links_enum links;
+	int rval;
+
+	if (copy_from_user(&links, ulinks, sizeof(links)))
+		return -EFAULT;
+
+	rval = __media_device_enum_links(mdev, &links);
+	if (rval < 0)
+		return rval;
+
 	if (copy_to_user(ulinks, &links, sizeof(*ulinks)))
 		return -EFAULT;
+
 	return 0;
 }
 
@@ -251,10 +269,71 @@ static long media_device_ioctl(struct file *filp, unsigned int cmd,
 	return ret;
 }
 
+#ifdef CONFIG_COMPAT
+
+struct media_links_enum32 {
+	__u32 entity;
+	compat_uptr_t pads; /* struct media_pad_desc * */
+	compat_uptr_t links; /* struct media_link_desc * */
+	__u32 reserved[4];
+};
+
+static long media_device_enum_links32(struct media_device *mdev,
+				      struct media_links_enum32 __user *ulinks)
+{
+	struct media_links_enum links;
+	compat_uptr_t pads_ptr, links_ptr;
+
+	memset(&links, 0, sizeof(links));
+
+	if (get_user(links.entity, &ulinks->entity)
+	    || get_user(pads_ptr, &ulinks->pads)
+	    || get_user(links_ptr, &ulinks->links))
+		return -EFAULT;
+
+	links.pads = compat_ptr(pads_ptr);
+	links.links = compat_ptr(links_ptr);
+
+	return __media_device_enum_links(mdev, &links);
+}
+
+#define MEDIA_IOC_ENUM_LINKS32		_IOWR('|', 0x02, struct media_links_enum32)
+
+static long media_device_compat_ioctl(struct file *filp, unsigned int cmd,
+				      unsigned long arg)
+{
+	struct media_devnode *devnode = media_devnode_data(filp);
+	struct media_device *dev = to_media_device(devnode);
+	long ret;
+
+	switch (cmd) {
+	case MEDIA_IOC_DEVICE_INFO:
+	case MEDIA_IOC_ENUM_ENTITIES:
+	case MEDIA_IOC_SETUP_LINK:
+		return media_device_ioctl(filp, cmd, arg);
+
+	case MEDIA_IOC_ENUM_LINKS32:
+		mutex_lock(&dev->graph_mutex);
+		ret = media_device_enum_links32(dev,
+				(struct media_links_enum32 __user *)arg);
+		mutex_unlock(&dev->graph_mutex);
+		break;
+
+	default:
+		ret = -ENOIOCTLCMD;
+	}
+
+	return ret;
+}
+#endif /* CONFIG_COMPAT */
+
 static const struct media_file_operations media_device_fops = {
 	.owner = THIS_MODULE,
 	.open = media_device_open,
 	.ioctl = media_device_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl = media_device_compat_ioctl,
+#endif /* CONFIG_COMPAT */
 	.release = media_device_close,
 };
 
diff --git a/drivers/media/media-devnode.c b/drivers/media/media-devnode.c
index 023b2a1cbb9b..fb0f0469fad7 100644
--- a/drivers/media/media-devnode.c
+++ b/drivers/media/media-devnode.c
@@ -116,19 +116,41 @@ static unsigned int media_poll(struct file *filp,
 	return mdev->fops->poll(filp, poll);
 }
 
-static long media_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+static long
+__media_ioctl(struct file *filp, unsigned int cmd, unsigned long arg,
+	      long (*ioctl_func)(struct file *filp, unsigned int cmd,
+				 unsigned long arg))
 {
 	struct media_devnode *mdev = media_devnode_data(filp);
 
-	if (!mdev->fops->ioctl)
+	if (!ioctl_func)
 		return -ENOTTY;
 
 	if (!media_devnode_is_registered(mdev))
 		return -EIO;
 
-	return mdev->fops->ioctl(filp, cmd, arg);
+	return ioctl_func(filp, cmd, arg);
+}
+
+static long media_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	struct media_devnode *mdev = media_devnode_data(filp);
+
+	return __media_ioctl(filp, cmd, arg, mdev->fops->ioctl);
 }
 
+#ifdef CONFIG_COMPAT
+
+static long media_compat_ioctl(struct file *filp, unsigned int cmd,
+			       unsigned long arg)
+{
+	struct media_devnode *mdev = media_devnode_data(filp);
+
+	return __media_ioctl(filp, cmd, arg, mdev->fops->compat_ioctl);
+}
+
+#endif /* CONFIG_COMPAT */
+
 /* Override for the open function */
 static int media_open(struct inode *inode, struct file *filp)
 {
@@ -188,6 +210,9 @@ static const struct file_operations media_devnode_fops = {
 	.write = media_write,
 	.open = media_open,
 	.unlocked_ioctl = media_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl = media_compat_ioctl,
+#endif /* CONFIG_COMPAT */
 	.release = media_release,
 	.poll = media_poll,
 	.llseek = no_llseek,
diff --git a/drivers/media/mmc/siano/smssdio.c b/drivers/media/mmc/siano/smssdio.c
index 15d34935e00b..912c2814c6cf 100644
--- a/drivers/media/mmc/siano/smssdio.c
+++ b/drivers/media/mmc/siano/smssdio.c
@@ -43,6 +43,7 @@
 
 #include "smscoreapi.h"
 #include "sms-cards.h"
+#include "smsendian.h"
 
 /* Registers */
 
@@ -61,6 +62,16 @@ static const struct sdio_device_id smssdio_ids[] = {
 	 .driver_data = SMS1XXX_BOARD_SIANO_VEGA},
 	{SDIO_DEVICE(SDIO_VENDOR_ID_SIANO, SDIO_DEVICE_ID_SIANO_VENICE),
 	 .driver_data = SMS1XXX_BOARD_SIANO_VEGA},
+	{SDIO_DEVICE(SDIO_VENDOR_ID_SIANO, 0x302),
+	.driver_data = SMS1XXX_BOARD_SIANO_MING},
+	{SDIO_DEVICE(SDIO_VENDOR_ID_SIANO, 0x500),
+	.driver_data = SMS1XXX_BOARD_SIANO_PELE},
+	{SDIO_DEVICE(SDIO_VENDOR_ID_SIANO, 0x600),
+	.driver_data = SMS1XXX_BOARD_SIANO_RIO},
+	{SDIO_DEVICE(SDIO_VENDOR_ID_SIANO, 0x700),
+	.driver_data = SMS1XXX_BOARD_SIANO_DENVER_2160},
+	{SDIO_DEVICE(SDIO_VENDOR_ID_SIANO, 0x800),
+	.driver_data = SMS1XXX_BOARD_SIANO_DENVER_1530},
 	{ /* end: all zeroes */ },
 };
 
@@ -87,6 +98,7 @@ static int smssdio_sendrequest(void *context, void *buffer, size_t size)
 
 	sdio_claim_host(smsdev->func);
 
+	smsendian_handle_tx_message((struct sms_msg_data *) buffer);
 	while (size >= smsdev->func->cur_blksize) {
 		ret = sdio_memcpy_toio(smsdev->func, SMSSDIO_DATA,
 					buffer, smsdev->func->cur_blksize);
@@ -118,7 +130,7 @@ static void smssdio_interrupt(struct sdio_func *func)
 
 	struct smssdio_device *smsdev;
 	struct smscore_buffer_t *cb;
-	struct SmsMsgHdr_ST *hdr;
+	struct sms_msg_hdr *hdr;
 	size_t size;
 
 	smsdev = sdio_get_drvdata(func);
@@ -151,20 +163,20 @@ static void smssdio_interrupt(struct sdio_func *func)
 
 		hdr = cb->p;
 
-		if (hdr->msgFlags & MSG_HDR_FLAG_SPLIT_MSG) {
+		if (hdr->msg_flags & MSG_HDR_FLAG_SPLIT_MSG) {
 			smsdev->split_cb = cb;
 			return;
 		}
 
-		if (hdr->msgLength > smsdev->func->cur_blksize)
-			size = hdr->msgLength - smsdev->func->cur_blksize;
+		if (hdr->msg_length > smsdev->func->cur_blksize)
+			size = hdr->msg_length - smsdev->func->cur_blksize;
 		else
 			size = 0;
 	} else {
 		cb = smsdev->split_cb;
 		hdr = cb->p;
 
-		size = hdr->msgLength - sizeof(struct SmsMsgHdr_ST);
+		size = hdr->msg_length - sizeof(struct sms_msg_hdr);
 
 		smsdev->split_cb = NULL;
 	}
@@ -172,7 +184,7 @@ static void smssdio_interrupt(struct sdio_func *func)
 	if (size) {
 		void *buffer;
 
-		buffer = cb->p + (hdr->msgLength - size);
+		buffer = cb->p + (hdr->msg_length - size);
 		size = ALIGN(size, SMSSDIO_BLOCK_SIZE);
 
 		BUG_ON(smsdev->func->cur_blksize != SMSSDIO_BLOCK_SIZE);
@@ -218,9 +230,10 @@ static void smssdio_interrupt(struct sdio_func *func)
 		}
 	}
 
-	cb->size = hdr->msgLength;
+	cb->size = hdr->msg_length;
 	cb->offset = 0;
 
+	smsendian_handle_rx_message((struct sms_msg_data *) cb->p);
 	smscore_onresponse(smsdev->coredev, cb);
 }
 
diff --git a/drivers/media/parport/pms.c b/drivers/media/parport/pms.c
index 77f9c92186f4..66c957a02ba7 100644
--- a/drivers/media/parport/pms.c
+++ b/drivers/media/parport/pms.c
@@ -735,12 +735,12 @@ static int pms_g_std(struct file *file, void *fh, v4l2_std_id *std)
 	return 0;
 }
 
-static int pms_s_std(struct file *file, void *fh, v4l2_std_id *std)
+static int pms_s_std(struct file *file, void *fh, v4l2_std_id std)
 {
 	struct pms *dev = video_drvdata(file);
 	int ret = 0;
 
-	dev->std = *std;
+	dev->std = std;
 	if (dev->std & V4L2_STD_NTSC) {
 		pms_framerate(dev, 30);
 		pms_secamcross(dev, 0);
diff --git a/drivers/media/pci/bt8xx/bttv-cards.c b/drivers/media/pci/bt8xx/bttv-cards.c
index c4c59175e52c..b7dc921e1b91 100644
--- a/drivers/media/pci/bt8xx/bttv-cards.c
+++ b/drivers/media/pci/bt8xx/bttv-cards.c
@@ -3547,6 +3547,16 @@ void bttv_init_card2(struct bttv *btv)
 	if (btv->sd_msp34xx)
 		return;
 
+	/* Now see if we can find one of the tvaudio devices. */
+	btv->sd_tvaudio = v4l2_i2c_new_subdev(&btv->c.v4l2_dev,
+		&btv->c.i2c_adap, "tvaudio", 0, tvaudio_addrs());
+	if (btv->sd_tvaudio) {
+		/* There may be two tvaudio chips on the card, so try to
+		   find another. */
+		v4l2_i2c_new_subdev(&btv->c.v4l2_dev,
+			&btv->c.i2c_adap, "tvaudio", 0, tvaudio_addrs());
+	}
+
 	/* it might also be a tda7432. */
 	if (!bttv_tvcards[btv->c.type].no_tda7432) {
 		static const unsigned short addrs[] = {
@@ -3554,14 +3564,11 @@ void bttv_init_card2(struct bttv *btv)
 			I2C_CLIENT_END
 		};
 
-		if (v4l2_i2c_new_subdev(&btv->c.v4l2_dev,
-				&btv->c.i2c_adap, "tda7432", 0, addrs))
+		btv->sd_tda7432 = v4l2_i2c_new_subdev(&btv->c.v4l2_dev,
+				&btv->c.i2c_adap, "tda7432", 0, addrs);
+		if (btv->sd_tda7432)
 			return;
 	}
-
-	/* Now see if we can find one of the tvaudio devices. */
-	btv->sd_tvaudio = v4l2_i2c_new_subdev(&btv->c.v4l2_dev,
-		&btv->c.i2c_adap, "tvaudio", 0, tvaudio_addrs());
 	if (btv->sd_tvaudio)
 		return;
 
@@ -3940,7 +3947,7 @@ static void avermedia_eeprom(struct bttv *btv)
 u32 bttv_tda9880_setnorm(struct bttv *btv, u32 gpiobits)
 {
 
-	if (btv->audio == TVAUDIO_INPUT_TUNER) {
+	if (btv->audio_input == TVAUDIO_INPUT_TUNER) {
 		if (bttv_tvnorms[btv->tvnorm].v4l2_id & V4L2_STD_MN)
 			gpiobits |= 0x10000;
 		else
diff --git a/drivers/media/pci/bt8xx/bttv-driver.c b/drivers/media/pci/bt8xx/bttv-driver.c
index 54579e4c740b..e7d088413411 100644
--- a/drivers/media/pci/bt8xx/bttv-driver.c
+++ b/drivers/media/pci/bt8xx/bttv-driver.c
@@ -49,6 +49,8 @@
 #include "bttvp.h"
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-chip-ident.h>
 #include <media/tvaudio.h>
 #include <media/msp3400.h>
 
@@ -93,7 +95,7 @@ static unsigned int combfilter;
 static unsigned int lumafilter;
 static unsigned int automute    = 1;
 static unsigned int chroma_agc;
-static unsigned int adc_crush   = 1;
+static unsigned int agc_crush   = 1;
 static unsigned int whitecrush_upper = 0xCF;
 static unsigned int whitecrush_lower = 0x7F;
 static unsigned int vcr_hack;
@@ -125,7 +127,7 @@ module_param(combfilter,        int, 0444);
 module_param(lumafilter,        int, 0444);
 module_param(automute,          int, 0444);
 module_param(chroma_agc,        int, 0444);
-module_param(adc_crush,         int, 0444);
+module_param(agc_crush,         int, 0444);
 module_param(whitecrush_upper,  int, 0444);
 module_param(whitecrush_lower,  int, 0444);
 module_param(vcr_hack,          int, 0444);
@@ -138,27 +140,27 @@ module_param_array(video_nr,    int, NULL, 0444);
 module_param_array(radio_nr,    int, NULL, 0444);
 module_param_array(vbi_nr,      int, NULL, 0444);
 
-MODULE_PARM_DESC(radio,"The TV card supports radio, default is 0 (no)");
-MODULE_PARM_DESC(bigendian,"byte order of the framebuffer, default is native endian");
-MODULE_PARM_DESC(bttv_verbose,"verbose startup messages, default is 1 (yes)");
-MODULE_PARM_DESC(bttv_gpio,"log gpio changes, default is 0 (no)");
-MODULE_PARM_DESC(bttv_debug,"debug messages, default is 0 (no)");
-MODULE_PARM_DESC(irq_debug,"irq handler debug messages, default is 0 (no)");
+MODULE_PARM_DESC(radio, "The TV card supports radio, default is 0 (no)");
+MODULE_PARM_DESC(bigendian, "byte order of the framebuffer, default is native endian");
+MODULE_PARM_DESC(bttv_verbose, "verbose startup messages, default is 1 (yes)");
+MODULE_PARM_DESC(bttv_gpio, "log gpio changes, default is 0 (no)");
+MODULE_PARM_DESC(bttv_debug, "debug messages, default is 0 (no)");
+MODULE_PARM_DESC(irq_debug, "irq handler debug messages, default is 0 (no)");
 MODULE_PARM_DESC(disable_ir, "disable infrared remote support");
-MODULE_PARM_DESC(gbuffers,"number of capture buffers. range 2-32, default 8");
-MODULE_PARM_DESC(gbufsize,"size of the capture buffers, default is 0x208000");
-MODULE_PARM_DESC(reset_crop,"reset cropping parameters at open(), default "
+MODULE_PARM_DESC(gbuffers, "number of capture buffers. range 2-32, default 8");
+MODULE_PARM_DESC(gbufsize, "size of the capture buffers, default is 0x208000");
+MODULE_PARM_DESC(reset_crop, "reset cropping parameters at open(), default "
 		 "is 1 (yes) for compatibility with older applications");
-MODULE_PARM_DESC(automute,"mute audio on bad/missing video signal, default is 1 (yes)");
-MODULE_PARM_DESC(chroma_agc,"enables the AGC of chroma signal, default is 0 (no)");
-MODULE_PARM_DESC(adc_crush,"enables the luminance ADC crush, default is 1 (yes)");
-MODULE_PARM_DESC(whitecrush_upper,"sets the white crush upper value, default is 207");
-MODULE_PARM_DESC(whitecrush_lower,"sets the white crush lower value, default is 127");
-MODULE_PARM_DESC(vcr_hack,"enables the VCR hack (improves synch on poor VCR tapes), default is 0 (no)");
-MODULE_PARM_DESC(irq_iswitch,"switch inputs in irq handler");
-MODULE_PARM_DESC(uv_ratio,"ratio between u and v gains, default is 50");
-MODULE_PARM_DESC(full_luma_range,"use the full luma range, default is 0 (no)");
-MODULE_PARM_DESC(coring,"set the luma coring level, default is 0 (no)");
+MODULE_PARM_DESC(automute, "mute audio on bad/missing video signal, default is 1 (yes)");
+MODULE_PARM_DESC(chroma_agc, "enables the AGC of chroma signal, default is 0 (no)");
+MODULE_PARM_DESC(agc_crush, "enables the luminance AGC crush, default is 1 (yes)");
+MODULE_PARM_DESC(whitecrush_upper, "sets the white crush upper value, default is 207");
+MODULE_PARM_DESC(whitecrush_lower, "sets the white crush lower value, default is 127");
+MODULE_PARM_DESC(vcr_hack, "enables the VCR hack (improves synch on poor VCR tapes), default is 0 (no)");
+MODULE_PARM_DESC(irq_iswitch, "switch inputs in irq handler");
+MODULE_PARM_DESC(uv_ratio, "ratio between u and v gains, default is 50");
+MODULE_PARM_DESC(full_luma_range, "use the full luma range, default is 0 (no)");
+MODULE_PARM_DESC(coring, "set the luma coring level, default is 0 (no)");
 MODULE_PARM_DESC(video_nr, "video device numbers");
 MODULE_PARM_DESC(vbi_nr, "vbi device numbers");
 MODULE_PARM_DESC(radio_nr, "radio device numbers");
@@ -168,6 +170,17 @@ MODULE_AUTHOR("Ralph Metzler & Marcus Metzler & Gerd Knorr");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(BTTV_VERSION);
 
+#define V4L2_CID_PRIVATE_COMBFILTER		(V4L2_CID_USER_BTTV_BASE + 0)
+#define V4L2_CID_PRIVATE_AUTOMUTE		(V4L2_CID_USER_BTTV_BASE + 1)
+#define V4L2_CID_PRIVATE_LUMAFILTER		(V4L2_CID_USER_BTTV_BASE + 2)
+#define V4L2_CID_PRIVATE_AGC_CRUSH		(V4L2_CID_USER_BTTV_BASE + 3)
+#define V4L2_CID_PRIVATE_VCR_HACK		(V4L2_CID_USER_BTTV_BASE + 4)
+#define V4L2_CID_PRIVATE_WHITECRUSH_LOWER	(V4L2_CID_USER_BTTV_BASE + 5)
+#define V4L2_CID_PRIVATE_WHITECRUSH_UPPER	(V4L2_CID_USER_BTTV_BASE + 6)
+#define V4L2_CID_PRIVATE_UV_RATIO		(V4L2_CID_USER_BTTV_BASE + 7)
+#define V4L2_CID_PRIVATE_FULL_LUMA_RANGE	(V4L2_CID_USER_BTTV_BASE + 8)
+#define V4L2_CID_PRIVATE_CORING			(V4L2_CID_USER_BTTV_BASE + 9)
+
 /* ----------------------------------------------------------------------- */
 /* sysfs                                                                   */
 
@@ -632,198 +645,6 @@ static const struct bttv_format formats[] = {
 static const unsigned int FORMATS = ARRAY_SIZE(formats);
 
 /* ----------------------------------------------------------------------- */
-
-#define V4L2_CID_PRIVATE_CHROMA_AGC  (V4L2_CID_PRIVATE_BASE + 0)
-#define V4L2_CID_PRIVATE_COMBFILTER  (V4L2_CID_PRIVATE_BASE + 1)
-#define V4L2_CID_PRIVATE_AUTOMUTE    (V4L2_CID_PRIVATE_BASE + 2)
-#define V4L2_CID_PRIVATE_LUMAFILTER  (V4L2_CID_PRIVATE_BASE + 3)
-#define V4L2_CID_PRIVATE_AGC_CRUSH   (V4L2_CID_PRIVATE_BASE + 4)
-#define V4L2_CID_PRIVATE_VCR_HACK    (V4L2_CID_PRIVATE_BASE + 5)
-#define V4L2_CID_PRIVATE_WHITECRUSH_UPPER   (V4L2_CID_PRIVATE_BASE + 6)
-#define V4L2_CID_PRIVATE_WHITECRUSH_LOWER   (V4L2_CID_PRIVATE_BASE + 7)
-#define V4L2_CID_PRIVATE_UV_RATIO    (V4L2_CID_PRIVATE_BASE + 8)
-#define V4L2_CID_PRIVATE_FULL_LUMA_RANGE    (V4L2_CID_PRIVATE_BASE + 9)
-#define V4L2_CID_PRIVATE_CORING      (V4L2_CID_PRIVATE_BASE + 10)
-#define V4L2_CID_PRIVATE_LASTP1      (V4L2_CID_PRIVATE_BASE + 11)
-
-static const struct v4l2_queryctrl no_ctl = {
-	.name  = "42",
-	.flags = V4L2_CTRL_FLAG_DISABLED,
-};
-static const struct v4l2_queryctrl bttv_ctls[] = {
-	/* --- video --- */
-	{
-		.id            = V4L2_CID_BRIGHTNESS,
-		.name          = "Brightness",
-		.minimum       = 0,
-		.maximum       = 65535,
-		.step          = 256,
-		.default_value = 32768,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_CONTRAST,
-		.name          = "Contrast",
-		.minimum       = 0,
-		.maximum       = 65535,
-		.step          = 128,
-		.default_value = 27648,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_SATURATION,
-		.name          = "Saturation",
-		.minimum       = 0,
-		.maximum       = 65535,
-		.step          = 128,
-		.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,
-		.maximum       = 65535,
-		.step          = 256,
-		.default_value = 32768,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},
-	/* --- audio --- */
-	{
-		.id            = V4L2_CID_AUDIO_MUTE,
-		.name          = "Mute",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_AUDIO_VOLUME,
-		.name          = "Volume",
-		.minimum       = 0,
-		.maximum       = 65535,
-		.step          = 65535/100,
-		.default_value = 65535,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_AUDIO_BALANCE,
-		.name          = "Balance",
-		.minimum       = 0,
-		.maximum       = 65535,
-		.step          = 65535/100,
-		.default_value = 32768,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_AUDIO_BASS,
-		.name          = "Bass",
-		.minimum       = 0,
-		.maximum       = 65535,
-		.step          = 65535/100,
-		.default_value = 32768,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_AUDIO_TREBLE,
-		.name          = "Treble",
-		.minimum       = 0,
-		.maximum       = 65535,
-		.step          = 65535/100,
-		.default_value = 32768,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},
-	/* --- private --- */
-	{
-		.id            = V4L2_CID_PRIVATE_CHROMA_AGC,
-		.name          = "chroma agc",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_PRIVATE_COMBFILTER,
-		.name          = "combfilter",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_PRIVATE_AUTOMUTE,
-		.name          = "automute",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_PRIVATE_LUMAFILTER,
-		.name          = "luma decimation filter",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_PRIVATE_AGC_CRUSH,
-		.name          = "agc crush",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_PRIVATE_VCR_HACK,
-		.name          = "vcr hack",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_PRIVATE_WHITECRUSH_UPPER,
-		.name          = "whitecrush upper",
-		.minimum       = 0,
-		.maximum       = 255,
-		.step          = 1,
-		.default_value = 0xCF,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_PRIVATE_WHITECRUSH_LOWER,
-		.name          = "whitecrush lower",
-		.minimum       = 0,
-		.maximum       = 255,
-		.step          = 1,
-		.default_value = 0x7F,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_PRIVATE_UV_RATIO,
-		.name          = "uv ratio",
-		.minimum       = 0,
-		.maximum       = 100,
-		.step          = 1,
-		.default_value = 50,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_PRIVATE_FULL_LUMA_RANGE,
-		.name          = "full luma range",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_PRIVATE_CORING,
-		.name          = "coring",
-		.minimum       = 0,
-		.maximum       = 3,
-		.step          = 1,
-		.default_value = 0,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	}
-
-
-
-};
-
-static const struct v4l2_queryctrl *ctrl_by_id(int id)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(bttv_ctls); i++)
-		if (bttv_ctls[i].id == id)
-			return bttv_ctls+i;
-
-	return NULL;
-}
-
-/* ----------------------------------------------------------------------- */
 /* resource management                                                     */
 
 /*
@@ -1178,23 +999,20 @@ static char *audio_modes[] = {
 	"audio: intern", "audio: mute"
 };
 
-static int
-audio_mux(struct bttv *btv, int input, int mute)
+static void
+audio_mux_gpio(struct bttv *btv, int input, int mute)
 {
-	int gpio_val, signal;
-	struct v4l2_control ctrl;
+	int gpio_val, signal, mute_gpio;
 
 	gpio_inout(bttv_tvcards[btv->c.type].gpiomask,
 		   bttv_tvcards[btv->c.type].gpiomask);
 	signal = btread(BT848_DSTATUS) & BT848_DSTATUS_HLOC;
 
-	btv->mute = mute;
-	btv->audio = input;
-
 	/* automute */
-	mute = mute || (btv->opt_automute && !signal && !btv->radio_user);
+	mute_gpio = mute || (btv->opt_automute && (!signal || !btv->users)
+				&& !btv->has_radio_tuner);
 
-	if (mute)
+	if (mute_gpio)
 		gpio_val = bttv_tvcards[btv->c.type].gpiomute;
 	else
 		gpio_val = bttv_tvcards[btv->c.type].gpiomux[input];
@@ -1210,13 +1028,39 @@ audio_mux(struct bttv *btv, int input, int mute)
 	}
 
 	if (bttv_gpio)
-		bttv_gpio_tracking(btv, audio_modes[mute ? 4 : input]);
-	if (in_interrupt())
-		return 0;
+		bttv_gpio_tracking(btv, audio_modes[mute_gpio ? 4 : input]);
+}
+
+static int
+audio_mute(struct bttv *btv, int mute)
+{
+	struct v4l2_ctrl *ctrl;
+
+	audio_mux_gpio(btv, btv->audio_input, mute);
+
+	if (btv->sd_msp34xx) {
+		ctrl = v4l2_ctrl_find(btv->sd_msp34xx->ctrl_handler, V4L2_CID_AUDIO_MUTE);
+		if (ctrl)
+			v4l2_ctrl_s_ctrl(ctrl, mute);
+	}
+	if (btv->sd_tvaudio) {
+		ctrl = v4l2_ctrl_find(btv->sd_tvaudio->ctrl_handler, V4L2_CID_AUDIO_MUTE);
+		if (ctrl)
+			v4l2_ctrl_s_ctrl(ctrl, mute);
+	}
+	if (btv->sd_tda7432) {
+		ctrl = v4l2_ctrl_find(btv->sd_tda7432->ctrl_handler, V4L2_CID_AUDIO_MUTE);
+		if (ctrl)
+			v4l2_ctrl_s_ctrl(ctrl, mute);
+	}
+	return 0;
+}
+
+static int
+audio_input(struct bttv *btv, int input)
+{
+	audio_mux_gpio(btv, input, btv->mute);
 
-	ctrl.id = V4L2_CID_AUDIO_MUTE;
-	ctrl.value = btv->mute;
-	bttv_call_all(btv, core, s_ctrl, &ctrl);
 	if (btv->sd_msp34xx) {
 		u32 in;
 
@@ -1265,23 +1109,11 @@ audio_mux(struct bttv *btv, int input, int mute)
 	}
 	if (btv->sd_tvaudio) {
 		v4l2_subdev_call(btv->sd_tvaudio, audio, s_routing,
-				input, 0, 0);
+				 input, 0, 0);
 	}
 	return 0;
 }
 
-static inline int
-audio_mute(struct bttv *btv, int mute)
-{
-	return audio_mux(btv, btv->audio, mute);
-}
-
-static inline int
-audio_input(struct bttv *btv, int input)
-{
-	return audio_mux(btv, input, btv->mute);
-}
-
 static void
 bttv_crop_calc_limits(struct bttv_crop *c)
 {
@@ -1375,8 +1207,9 @@ set_input(struct bttv *btv, unsigned int input, unsigned int norm)
 	} else {
 		video_mux(btv,input);
 	}
-	audio_input(btv, (btv->tuner_type != TUNER_ABSENT && input == 0) ?
-			 TVAUDIO_INPUT_TUNER : TVAUDIO_INPUT_EXTERN);
+	btv->audio_input = (btv->tuner_type != TUNER_ABSENT && input == 0) ?
+				TVAUDIO_INPUT_TUNER : TVAUDIO_INPUT_EXTERN;
+	audio_input(btv, btv->audio_input);
 	set_tvnorm(btv, norm);
 }
 
@@ -1404,8 +1237,6 @@ static void init_irqreg(struct bttv *btv)
 
 static void init_bt848(struct bttv *btv)
 {
-	int val;
-
 	if (bttv_tvcards[btv->c.type].no_video) {
 		/* very basic init only */
 		init_irqreg(btv);
@@ -1425,30 +1256,10 @@ static void init_bt848(struct bttv *btv)
 		BT848_GPIO_DMA_CTL_GPINTI,
 		BT848_GPIO_DMA_CTL);
 
-	val = btv->opt_chroma_agc ? BT848_SCLOOP_CAGC : 0;
-	btwrite(val, BT848_E_SCLOOP);
-	btwrite(val, BT848_O_SCLOOP);
-
 	btwrite(0x20, BT848_E_VSCALE_HI);
 	btwrite(0x20, BT848_O_VSCALE_HI);
-	btwrite(BT848_ADC_RESERVED | (btv->opt_adc_crush ? BT848_ADC_CRUSH : 0),
-		BT848_ADC);
-
-	btwrite(whitecrush_upper, BT848_WC_UP);
-	btwrite(whitecrush_lower, BT848_WC_DOWN);
-
-	if (btv->opt_lumafilter) {
-		btwrite(0, BT848_E_CONTROL);
-		btwrite(0, BT848_O_CONTROL);
-	} else {
-		btwrite(BT848_CONTROL_LDEC, BT848_E_CONTROL);
-		btwrite(BT848_CONTROL_LDEC, BT848_O_CONTROL);
-	}
 
-	bt848_bright(btv,   btv->bright);
-	bt848_hue(btv,      btv->hue);
-	bt848_contrast(btv, btv->contrast);
-	bt848_sat(btv,      btv->saturation);
+	v4l2_ctrl_handler_setup(&btv->ctrl_handler);
 
 	/* interrupt */
 	init_irqreg(btv);
@@ -1470,103 +1281,26 @@ static void bttv_reinit_bt848(struct bttv *btv)
 	set_input(btv, btv->input, btv->tvnorm);
 }
 
-static int bttv_g_ctrl(struct file *file, void *priv,
-					struct v4l2_control *c)
+static int bttv_s_ctrl(struct v4l2_ctrl *c)
 {
-	struct bttv_fh *fh = priv;
-	struct bttv *btv = fh->btv;
-
-	switch (c->id) {
-	case V4L2_CID_BRIGHTNESS:
-		c->value = btv->bright;
-		break;
-	case V4L2_CID_HUE:
-		c->value = btv->hue;
-		break;
-	case V4L2_CID_CONTRAST:
-		c->value = btv->contrast;
-		break;
-	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:
-	case V4L2_CID_AUDIO_BALANCE:
-	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-		bttv_call_all(btv, core, g_ctrl, c);
-		break;
-
-	case V4L2_CID_PRIVATE_CHROMA_AGC:
-		c->value = btv->opt_chroma_agc;
-		break;
-	case V4L2_CID_PRIVATE_COMBFILTER:
-		c->value = btv->opt_combfilter;
-		break;
-	case V4L2_CID_PRIVATE_LUMAFILTER:
-		c->value = btv->opt_lumafilter;
-		break;
-	case V4L2_CID_PRIVATE_AUTOMUTE:
-		c->value = btv->opt_automute;
-		break;
-	case V4L2_CID_PRIVATE_AGC_CRUSH:
-		c->value = btv->opt_adc_crush;
-		break;
-	case V4L2_CID_PRIVATE_VCR_HACK:
-		c->value = btv->opt_vcr_hack;
-		break;
-	case V4L2_CID_PRIVATE_WHITECRUSH_UPPER:
-		c->value = btv->opt_whitecrush_upper;
-		break;
-	case V4L2_CID_PRIVATE_WHITECRUSH_LOWER:
-		c->value = btv->opt_whitecrush_lower;
-		break;
-	case V4L2_CID_PRIVATE_UV_RATIO:
-		c->value = btv->opt_uv_ratio;
-		break;
-	case V4L2_CID_PRIVATE_FULL_LUMA_RANGE:
-		c->value = btv->opt_full_luma_range;
-		break;
-	case V4L2_CID_PRIVATE_CORING:
-		c->value = btv->opt_coring;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int bttv_s_ctrl(struct file *file, void *f,
-					struct v4l2_control *c)
-{
-	int err;
-	struct bttv_fh *fh = f;
-	struct bttv *btv = fh->btv;
-
-	err = v4l2_prio_check(&btv->prio, fh->prio);
-	if (0 != err)
-		return err;
+	struct bttv *btv = container_of(c->handler, struct bttv, ctrl_handler);
+	int val;
 
 	switch (c->id) {
 	case V4L2_CID_BRIGHTNESS:
-		bt848_bright(btv, c->value);
+		bt848_bright(btv, c->val);
 		break;
 	case V4L2_CID_HUE:
-		bt848_hue(btv, c->value);
+		bt848_hue(btv, c->val);
 		break;
 	case V4L2_CID_CONTRAST:
-		bt848_contrast(btv, c->value);
+		bt848_contrast(btv, c->val);
 		break;
 	case V4L2_CID_SATURATION:
-		bt848_sat(btv, c->value);
+		bt848_sat(btv, c->val);
 		break;
 	case V4L2_CID_COLOR_KILLER:
-		btv->opt_color_killer = c->value;
-		if (btv->opt_color_killer) {
+		if (c->val) {
 			btor(BT848_SCLOOP_CKILL, BT848_E_SCLOOP);
 			btor(BT848_SCLOOP_CKILL, BT848_O_SCLOOP);
 		} else {
@@ -1575,36 +1309,23 @@ static int bttv_s_ctrl(struct file *file, void *f,
 		}
 		break;
 	case V4L2_CID_AUDIO_MUTE:
-		audio_mute(btv, c->value);
-		/* fall through */
-	case V4L2_CID_AUDIO_VOLUME:
-		if (btv->volume_gpio)
-			btv->volume_gpio(btv, c->value);
-
-		bttv_call_all(btv, core, s_ctrl, c);
+		audio_mute(btv, c->val);
+		btv->mute = c->val;
 		break;
-	case V4L2_CID_AUDIO_BALANCE:
-	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-		bttv_call_all(btv, core, s_ctrl, c);
+	case V4L2_CID_AUDIO_VOLUME:
+		btv->volume_gpio(btv, c->val);
 		break;
 
-	case V4L2_CID_PRIVATE_CHROMA_AGC:
-		btv->opt_chroma_agc = c->value;
-		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);
-		}
+	case V4L2_CID_CHROMA_AGC:
+		val = c->val ? BT848_SCLOOP_CAGC : 0;
+		btwrite(val, BT848_E_SCLOOP);
+		btwrite(val, BT848_O_SCLOOP);
 		break;
 	case V4L2_CID_PRIVATE_COMBFILTER:
-		btv->opt_combfilter = c->value;
+		btv->opt_combfilter = c->val;
 		break;
 	case V4L2_CID_PRIVATE_LUMAFILTER:
-		btv->opt_lumafilter = c->value;
-		if (btv->opt_lumafilter) {
+		if (c->val) {
 			btand(~BT848_CONTROL_LDEC, BT848_E_CONTROL);
 			btand(~BT848_CONTROL_LDEC, BT848_O_CONTROL);
 		} else {
@@ -1613,36 +1334,31 @@ static int bttv_s_ctrl(struct file *file, void *f,
 		}
 		break;
 	case V4L2_CID_PRIVATE_AUTOMUTE:
-		btv->opt_automute = c->value;
+		btv->opt_automute = c->val;
 		break;
 	case V4L2_CID_PRIVATE_AGC_CRUSH:
-		btv->opt_adc_crush = c->value;
 		btwrite(BT848_ADC_RESERVED |
-				(btv->opt_adc_crush ? BT848_ADC_CRUSH : 0),
+				(c->val ? BT848_ADC_CRUSH : 0),
 				BT848_ADC);
 		break;
 	case V4L2_CID_PRIVATE_VCR_HACK:
-		btv->opt_vcr_hack = c->value;
+		btv->opt_vcr_hack = c->val;
 		break;
 	case V4L2_CID_PRIVATE_WHITECRUSH_UPPER:
-		btv->opt_whitecrush_upper = c->value;
-		btwrite(c->value, BT848_WC_UP);
+		btwrite(c->val, BT848_WC_UP);
 		break;
 	case V4L2_CID_PRIVATE_WHITECRUSH_LOWER:
-		btv->opt_whitecrush_lower = c->value;
-		btwrite(c->value, BT848_WC_DOWN);
+		btwrite(c->val, BT848_WC_DOWN);
 		break;
 	case V4L2_CID_PRIVATE_UV_RATIO:
-		btv->opt_uv_ratio = c->value;
+		btv->opt_uv_ratio = c->val;
 		bt848_sat(btv, btv->saturation);
 		break;
 	case V4L2_CID_PRIVATE_FULL_LUMA_RANGE:
-		btv->opt_full_luma_range = c->value;
-		btaor((c->value<<7), ~BT848_OFORM_RANGE, BT848_OFORM);
+		btaor((c->val << 7), ~BT848_OFORM_RANGE, BT848_OFORM);
 		break;
 	case V4L2_CID_PRIVATE_CORING:
-		btv->opt_coring = c->value;
-		btaor((c->value<<5), ~BT848_OFORM_CORE32, BT848_OFORM);
+		btaor((c->val << 5), ~BT848_OFORM_CORE32, BT848_OFORM);
 		break;
 	default:
 		return -EINVAL;
@@ -1652,6 +1368,121 @@ static int bttv_s_ctrl(struct file *file, void *f,
 
 /* ----------------------------------------------------------------------- */
 
+static const struct v4l2_ctrl_ops bttv_ctrl_ops = {
+	.s_ctrl = bttv_s_ctrl,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_combfilter = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_COMBFILTER,
+	.name = "Comb Filter",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_automute = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_AUTOMUTE,
+	.name = "Auto Mute",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_lumafilter = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_LUMAFILTER,
+	.name = "Luma Decimation Filter",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_agc_crush = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_AGC_CRUSH,
+	.name = "AGC Crush",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_vcr_hack = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_VCR_HACK,
+	.name = "VCR Hack",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_whitecrush_lower = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_WHITECRUSH_LOWER,
+	.name = "Whitecrush Lower",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 255,
+	.step = 1,
+	.def = 0x7f,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_whitecrush_upper = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_WHITECRUSH_UPPER,
+	.name = "Whitecrush Upper",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 255,
+	.step = 1,
+	.def = 0xcf,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_uv_ratio = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_UV_RATIO,
+	.name = "UV Ratio",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 100,
+	.step = 1,
+	.def = 50,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_full_luma = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_FULL_LUMA_RANGE,
+	.name = "Full Luma Range",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+};
+
+static struct v4l2_ctrl_config bttv_ctrl_coring = {
+	.ops = &bttv_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_CORING,
+	.name = "Coring",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 3,
+	.step = 1,
+};
+
+
+/* ----------------------------------------------------------------------- */
+
 void bttv_gpio_tracking(struct bttv *btv, char *comment)
 {
 	unsigned int outbits, data;
@@ -1881,25 +1712,33 @@ static struct videobuf_queue_ops bttv_video_qops = {
 	.buf_release  = buffer_release,
 };
 
-static int bttv_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static void radio_enable(struct bttv *btv)
+{
+	/* Switch to the radio tuner */
+	if (!btv->has_radio_tuner) {
+		btv->has_radio_tuner = 1;
+		bttv_call_all(btv, tuner, s_radio);
+		btv->audio_input = TVAUDIO_INPUT_RADIO;
+		audio_input(btv, btv->audio_input);
+	}
+}
+
+static int bttv_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct bttv_fh *fh  = priv;
 	struct bttv *btv = fh->btv;
 	unsigned int i;
-	int err;
-
-	err = v4l2_prio_check(&btv->prio, fh->prio);
-	if (err)
-		goto err;
+	int err = 0;
 
 	for (i = 0; i < BTTV_TVNORMS; i++)
-		if (*id & bttv_tvnorms[i].v4l2_id)
+		if (id & bttv_tvnorms[i].v4l2_id)
 			break;
 	if (i == BTTV_TVNORMS) {
 		err = -EINVAL;
 		goto err;
 	}
 
+	btv->std = id;
 	set_tvnorm(btv, i);
 
 err:
@@ -1907,6 +1746,15 @@ err:
 	return err;
 }
 
+static int bttv_g_std(struct file *file, void *priv, v4l2_std_id *id)
+{
+	struct bttv_fh *fh  = priv;
+	struct bttv *btv = fh->btv;
+
+	*id = btv->std;
+	return 0;
+}
+
 static int bttv_querystd(struct file *file, void *f, v4l2_std_id *id)
 {
 	struct bttv_fh *fh = f;
@@ -1932,7 +1780,7 @@ static int bttv_enum_input(struct file *file, void *priv,
 	}
 
 	i->type     = V4L2_INPUT_TYPE_CAMERA;
-	i->audioset = 1;
+	i->audioset = 0;
 
 	if (btv->tuner_type != TUNER_ABSENT && i->index == 0) {
 		sprintf(i->name, "Television");
@@ -1974,49 +1822,29 @@ static int bttv_s_input(struct file *file, void *priv, unsigned int i)
 	struct bttv_fh *fh  = priv;
 	struct bttv *btv = fh->btv;
 
-	int err;
-
-	err = v4l2_prio_check(&btv->prio, fh->prio);
-	if (unlikely(err))
-		goto err;
-
-	if (i > bttv_tvcards[btv->c.type].video_inputs) {
-		err = -EINVAL;
-		goto err;
-	}
+	if (i >= bttv_tvcards[btv->c.type].video_inputs)
+		return -EINVAL;
 
 	set_input(btv, i, btv->tvnorm);
-
-err:
 	return 0;
 }
 
 static int bttv_s_tuner(struct file *file, void *priv,
-					struct v4l2_tuner *t)
+					const struct v4l2_tuner *t)
 {
 	struct bttv_fh *fh  = priv;
 	struct bttv *btv = fh->btv;
-	int err;
 
-	if (unlikely(0 != t->index))
+	if (t->index)
 		return -EINVAL;
 
-	if (unlikely(btv->tuner_type == TUNER_ABSENT)) {
-		err = -EINVAL;
-		goto err;
-	}
-
-	err = v4l2_prio_check(&btv->prio, fh->prio);
-	if (unlikely(err))
-		goto err;
-
 	bttv_call_all(btv, tuner, s_tuner, t);
 
-	if (btv->audio_mode_gpio)
-		btv->audio_mode_gpio(btv, t, 1);
-
-err:
+	if (btv->audio_mode_gpio) {
+		struct v4l2_tuner copy = *t;
 
+		btv->audio_mode_gpio(btv, &copy, 1);
+	}
 	return 0;
 }
 
@@ -2026,49 +1854,81 @@ static int bttv_g_frequency(struct file *file, void *priv,
 	struct bttv_fh *fh  = priv;
 	struct bttv *btv = fh->btv;
 
-	f->type = btv->radio_user ? V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
-	f->frequency = btv->freq;
+	if (f->tuner)
+		return -EINVAL;
+
+	if (f->type == V4L2_TUNER_RADIO)
+		radio_enable(btv);
+	f->frequency = f->type == V4L2_TUNER_RADIO ?
+				btv->radio_freq : btv->tv_freq;
 
 	return 0;
 }
 
+static void bttv_set_frequency(struct bttv *btv, const struct v4l2_frequency *f)
+{
+	struct v4l2_frequency new_freq = *f;
+
+	bttv_call_all(btv, tuner, s_frequency, f);
+	/* s_frequency may clamp the frequency, so get the actual
+	   frequency before assigning radio/tv_freq. */
+	bttv_call_all(btv, tuner, g_frequency, &new_freq);
+	if (new_freq.type == V4L2_TUNER_RADIO) {
+		radio_enable(btv);
+		btv->radio_freq = new_freq.frequency;
+		if (btv->has_matchbox)
+			tea5757_set_freq(btv, btv->radio_freq);
+	} else {
+		btv->tv_freq = new_freq.frequency;
+	}
+}
+
 static int bttv_s_frequency(struct file *file, void *priv,
-					struct v4l2_frequency *f)
+					const struct v4l2_frequency *f)
 {
 	struct bttv_fh *fh  = priv;
 	struct bttv *btv = fh->btv;
-	int err;
 
-	if (unlikely(f->tuner != 0))
+	if (f->tuner)
 		return -EINVAL;
 
-	err = v4l2_prio_check(&btv->prio, fh->prio);
-	if (unlikely(err))
-		goto err;
-
-	if (unlikely(f->type != (btv->radio_user
-		? V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV))) {
-		err = -EINVAL;
-		goto err;
-	}
-	btv->freq = f->frequency;
-	bttv_call_all(btv, tuner, s_frequency, f);
-	if (btv->has_matchbox && btv->radio_user)
-		tea5757_set_freq(btv, btv->freq);
-err:
-
+	bttv_set_frequency(btv, f);
 	return 0;
 }
 
 static int bttv_log_status(struct file *file, void *f)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct bttv_fh *fh  = f;
 	struct bttv *btv = fh->btv;
 
+	v4l2_ctrl_handler_log_status(vdev->ctrl_handler, btv->c.v4l2_dev.name);
 	bttv_call_all(btv, core, log_status);
 	return 0;
 }
 
+static int bttv_g_chip_ident(struct file *file, void *f, struct v4l2_dbg_chip_ident *chip)
+{
+	struct bttv_fh *fh  = f;
+	struct bttv *btv = fh->btv;
+
+	chip->ident = V4L2_IDENT_NONE;
+	chip->revision = 0;
+	if (chip->match.type == V4L2_CHIP_MATCH_HOST) {
+		if (v4l2_chip_match_host(&chip->match)) {
+			chip->ident = btv->id;
+			if (chip->ident == PCI_DEVICE_ID_FUSION879)
+				chip->ident = V4L2_IDENT_BT879;
+		}
+		return 0;
+	}
+	if (chip->match.type != V4L2_CHIP_MATCH_I2C_DRIVER &&
+	    chip->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
+		return -EINVAL;
+	/* TODO: is this correct? */
+	return bttv_call_all_err(btv, core, g_chip_ident, chip);
+}
+
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 static int bttv_g_register(struct file *file, void *f,
 					struct v4l2_dbg_register *reg)
@@ -2079,8 +1939,12 @@ static int bttv_g_register(struct file *file, void *f,
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
 
-	if (!v4l2_chip_match_host(&reg->match))
-		return -EINVAL;
+	if (!v4l2_chip_match_host(&reg->match)) {
+		/* TODO: subdev errors should not be ignored, this should become a
+		   subdev helper function. */
+		bttv_call_all(btv, core, g_register, reg);
+		return 0;
+	}
 
 	/* bt848 has a 12-bit register space */
 	reg->reg &= 0xfff;
@@ -2091,7 +1955,7 @@ static int bttv_g_register(struct file *file, void *f,
 }
 
 static int bttv_s_register(struct file *file, void *f,
-					struct v4l2_dbg_register *reg)
+					const struct v4l2_dbg_register *reg)
 {
 	struct bttv_fh *fh = f;
 	struct bttv *btv = fh->btv;
@@ -2099,12 +1963,15 @@ static int bttv_s_register(struct file *file, void *f,
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
 
-	if (!v4l2_chip_match_host(&reg->match))
-		return -EINVAL;
+	if (!v4l2_chip_match_host(&reg->match)) {
+		/* TODO: subdev errors should not be ignored, this should become a
+		   subdev helper function. */
+		bttv_call_all(btv, core, s_register, reg);
+		return 0;
+	}
 
 	/* bt848 has a 12-bit register space */
-	reg->reg &= 0xfff;
-	btwrite(reg->val, reg->reg);
+	btwrite(reg->val, reg->reg & 0xfff);
 
 	return 0;
 }
@@ -2273,22 +2140,33 @@ limit_scaled_size_lock       (struct bttv_fh *               fh,
    may also adjust the current cropping parameters to get closer
    to the desired window size. */
 static int
-verify_window_lock		(struct bttv_fh *               fh,
-			 struct v4l2_window *           win,
-			 int                            adjust_size,
-			 int                            adjust_crop)
+verify_window_lock(struct bttv_fh *fh, struct v4l2_window *win,
+			 int adjust_size, int adjust_crop)
 {
 	enum v4l2_field field;
 	unsigned int width_mask;
 	int rc;
 
-	if (win->w.width  < 48 || win->w.height < 32)
-		return -EINVAL;
+	if (win->w.width < 48)
+		win->w.width = 48;
+	if (win->w.height < 32)
+		win->w.height = 32;
 	if (win->clipcount > 2048)
-		return -EINVAL;
+		win->clipcount = 2048;
 
+	win->chromakey = 0;
+	win->global_alpha = 0;
 	field = win->field;
 
+	switch (field) {
+	case V4L2_FIELD_TOP:
+	case V4L2_FIELD_BOTTOM:
+	case V4L2_FIELD_INTERLACED:
+		break;
+	default:
+		field = V4L2_FIELD_ANY;
+		break;
+	}
 	if (V4L2_FIELD_ANY == field) {
 		__s32 height2;
 
@@ -2297,18 +2175,11 @@ verify_window_lock		(struct bttv_fh *               fh,
 			? V4L2_FIELD_INTERLACED
 			: V4L2_FIELD_TOP;
 	}
-	switch (field) {
-	case V4L2_FIELD_TOP:
-	case V4L2_FIELD_BOTTOM:
-	case V4L2_FIELD_INTERLACED:
-		break;
-	default:
-		return -EINVAL;
-	}
+	win->field = field;
 
-	/* 4-byte alignment. */
 	if (NULL == fh->ovfmt)
 		return -EINVAL;
+	/* 4-byte alignment. */
 	width_mask = ~0;
 	switch (fh->ovfmt->depth) {
 	case 8:
@@ -2333,8 +2204,6 @@ verify_window_lock		(struct bttv_fh *               fh,
 			       adjust_size, adjust_crop);
 	if (0 != rc)
 		return rc;
-
-	win->field = field;
 	return 0;
 }
 
@@ -2491,6 +2360,7 @@ static int bttv_g_fmt_vid_cap(struct file *file, void *priv,
 				fh->width, fh->height);
 	f->fmt.pix.field        = fh->cap.field;
 	f->fmt.pix.pixelformat  = fh->fmt->fourcc;
+	f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
 
 	return 0;
 }
@@ -2514,6 +2384,7 @@ static int bttv_try_fmt_vid_cap(struct file *file, void *priv,
 	struct bttv *btv = fh->btv;
 	enum v4l2_field field;
 	__s32 width, height;
+	__s32 height2;
 	int rc;
 
 	fmt = format_by_fourcc(f->fmt.pix.pixelformat);
@@ -2522,30 +2393,25 @@ static int bttv_try_fmt_vid_cap(struct file *file, void *priv,
 
 	field = f->fmt.pix.field;
 
-	if (V4L2_FIELD_ANY == field) {
-		__s32 height2;
-
-		height2 = btv->crop[!!fh->do_crop].rect.height >> 1;
-		field = (f->fmt.pix.height > height2)
-			? V4L2_FIELD_INTERLACED
-			: V4L2_FIELD_BOTTOM;
-	}
-
-	if (V4L2_FIELD_SEQ_BT == field)
-		field = V4L2_FIELD_SEQ_TB;
-
 	switch (field) {
 	case V4L2_FIELD_TOP:
 	case V4L2_FIELD_BOTTOM:
 	case V4L2_FIELD_ALTERNATE:
 	case V4L2_FIELD_INTERLACED:
 		break;
+	case V4L2_FIELD_SEQ_BT:
 	case V4L2_FIELD_SEQ_TB:
-		if (fmt->flags & FORMAT_FLAGS_PLANAR)
-			return -EINVAL;
+		if (!(fmt->flags & FORMAT_FLAGS_PLANAR)) {
+			field = V4L2_FIELD_SEQ_TB;
+			break;
+		}
+		/* fall through */
+	default: /* FIELD_ANY case */
+		height2 = btv->crop[!!fh->do_crop].rect.height >> 1;
+		field = (f->fmt.pix.height > height2)
+			? V4L2_FIELD_INTERLACED
+			: V4L2_FIELD_BOTTOM;
 		break;
-	default:
-		return -EINVAL;
 	}
 
 	width = f->fmt.pix.width;
@@ -2562,6 +2428,7 @@ static int bttv_try_fmt_vid_cap(struct file *file, void *priv,
 	/* update data for the application */
 	f->fmt.pix.field = field;
 	pix_format_set_size(&f->fmt.pix, fmt, width, height);
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 
 	return 0;
 }
@@ -2571,9 +2438,10 @@ static int bttv_try_fmt_vid_overlay(struct file *file, void *priv,
 {
 	struct bttv_fh *fh = priv;
 
-	return verify_window_lock(fh, &f->fmt.win,
+	verify_window_lock(fh, &f->fmt.win,
 			/* adjust_size */ 1,
 			/* adjust_crop */ 0);
+	return 0;
 }
 
 static int bttv_s_fmt_vid_cap(struct file *file, void *priv,
@@ -2640,6 +2508,7 @@ static int bttv_s_fmt_vid_overlay(struct file *file, void *priv,
 static int bttv_querycap(struct file *file, void  *priv,
 				struct v4l2_capability *cap)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct bttv_fh *fh = priv;
 	struct bttv *btv = fh->btv;
 
@@ -2652,11 +2521,15 @@ static int bttv_querycap(struct file *file, void  *priv,
 		 "PCI:%s", pci_name(btv->c.pci));
 	cap->capabilities =
 		V4L2_CAP_VIDEO_CAPTURE |
-		V4L2_CAP_VBI_CAPTURE |
 		V4L2_CAP_READWRITE |
-		V4L2_CAP_STREAMING;
+		V4L2_CAP_STREAMING |
+		V4L2_CAP_DEVICE_CAPS;
 	if (no_overlay <= 0)
 		cap->capabilities |= V4L2_CAP_VIDEO_OVERLAY;
+	if (btv->vbi_dev)
+		cap->capabilities |= V4L2_CAP_VBI_CAPTURE;
+	if (btv->radio_dev)
+		cap->capabilities |= V4L2_CAP_RADIO;
 
 	/*
 	 * No need to lock here: those vars are initialized during board
@@ -2666,6 +2539,25 @@ static int bttv_querycap(struct file *file, void  *priv,
 		cap->capabilities |= V4L2_CAP_RDS_CAPTURE;
 	if (btv->tuner_type != TUNER_ABSENT)
 		cap->capabilities |= V4L2_CAP_TUNER;
+	if (vdev->vfl_type == VFL_TYPE_GRABBER)
+		cap->device_caps = cap->capabilities &
+			(V4L2_CAP_VIDEO_CAPTURE |
+			 V4L2_CAP_READWRITE |
+			 V4L2_CAP_STREAMING |
+			 V4L2_CAP_VIDEO_OVERLAY |
+			 V4L2_CAP_TUNER);
+	else if (vdev->vfl_type == VFL_TYPE_VBI)
+		cap->device_caps = cap->capabilities &
+			(V4L2_CAP_VBI_CAPTURE |
+			 V4L2_CAP_READWRITE |
+			 V4L2_CAP_STREAMING |
+			 V4L2_CAP_TUNER);
+	else {
+		cap->device_caps = V4L2_CAP_RADIO | V4L2_CAP_TUNER;
+		if (btv->has_saa6588)
+			cap->device_caps |= V4L2_CAP_READWRITE |
+						V4L2_CAP_RDS_CAPTURE;
+	}
 	return 0;
 }
 
@@ -2728,6 +2620,7 @@ static int bttv_g_fbuf(struct file *file, void *f,
 
 	*fb = btv->fbuf;
 	fb->capability = V4L2_FBUF_CAP_LIST_CLIPPING;
+	fb->flags = V4L2_FBUF_FLAG_PRIMARY;
 	if (fh->ovfmt)
 		fb->fmt.pixelformat  = fh->ovfmt->fourcc;
 	return 0;
@@ -2901,36 +2794,15 @@ static int bttv_streamoff(struct file *file, void *priv,
 	return 0;
 }
 
-static int bttv_queryctrl(struct file *file, void *priv,
-					struct v4l2_queryctrl *c)
-{
-	struct bttv_fh *fh = priv;
-	struct bttv *btv = fh->btv;
-	const struct v4l2_queryctrl *ctrl;
-
-	if ((c->id <  V4L2_CID_BASE ||
-	     c->id >= V4L2_CID_LASTP1) &&
-	    (c->id <  V4L2_CID_PRIVATE_BASE ||
-	     c->id >= V4L2_CID_PRIVATE_LASTP1))
-		return -EINVAL;
-
-	if (!btv->volume_gpio && (c->id == V4L2_CID_AUDIO_VOLUME))
-		*c = no_ctl;
-	else {
-		ctrl = ctrl_by_id(c->id);
-
-		*c = (NULL != ctrl) ? *ctrl : no_ctl;
-	}
-
-	return 0;
-}
-
 static int bttv_g_parm(struct file *file, void *f,
 				struct v4l2_streamparm *parm)
 {
 	struct bttv_fh *fh = f;
 	struct bttv *btv = fh->btv;
 
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	parm->parm.capture.readbuffers = gbuffers;
 	v4l2_video_std_frame_period(bttv_tvnorms[btv->tvnorm].v4l2_id,
 				    &parm->parm.capture.timeperframe);
 
@@ -2943,15 +2815,13 @@ static int bttv_g_tuner(struct file *file, void *priv,
 	struct bttv_fh *fh = priv;
 	struct bttv *btv = fh->btv;
 
-	if (btv->tuner_type == TUNER_ABSENT)
-		return -EINVAL;
 	if (0 != t->index)
 		return -EINVAL;
 
 	t->rxsubchans = V4L2_TUNER_SUB_MONO;
+	t->capability = V4L2_TUNER_CAP_NORM;
 	bttv_call_all(btv, tuner, g_tuner, t);
 	strcpy(t->name, "Television");
-	t->capability = V4L2_TUNER_CAP_NORM;
 	t->type       = V4L2_TUNER_ANALOG_TV;
 	if (btread(BT848_DSTATUS)&BT848_DSTATUS_HLOC)
 		t->signal = 0xffff;
@@ -2962,28 +2832,6 @@ static int bttv_g_tuner(struct file *file, void *priv,
 	return 0;
 }
 
-static int bttv_g_priority(struct file *file, void *f, enum v4l2_priority *p)
-{
-	struct bttv_fh *fh = f;
-	struct bttv *btv = fh->btv;
-
-	*p = v4l2_prio_max(&btv->prio);
-
-	return 0;
-}
-
-static int bttv_s_priority(struct file *file, void *f,
-					enum v4l2_priority prio)
-{
-	struct bttv_fh *fh = f;
-	struct bttv *btv = fh->btv;
-	int	rc;
-
-	rc = v4l2_prio_change(&btv->prio, &fh->prio, prio);
-
-	return rc;
-}
-
 static int bttv_cropcap(struct file *file, void *priv,
 				struct v4l2_cropcap *cap)
 {
@@ -3036,11 +2884,6 @@ static int bttv_s_crop(struct file *file, void *f, const struct v4l2_crop *crop)
 	/* Make sure tvnorm, vbi_end and the current cropping
 	   parameters remain consistent until we're done. Note
 	   read() may change vbi_end in check_alloc_btres_lock(). */
-	retval = v4l2_prio_check(&btv->prio, fh->prio);
-	if (0 != retval) {
-		return retval;
-	}
-
 	retval = -EBUSY;
 
 	if (locked_btres(fh->btv, VIDEO_RESOURCES)) {
@@ -3098,23 +2941,6 @@ static int bttv_s_crop(struct file *file, void *f, const struct v4l2_crop *crop)
 	return 0;
 }
 
-static int bttv_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
-{
-	if (unlikely(a->index))
-		return -EINVAL;
-
-	strcpy(a->name, "audio");
-	return 0;
-}
-
-static int bttv_s_audio(struct file *file, void *priv, const struct v4l2_audio *a)
-{
-	if (unlikely(a->index))
-		return -EINVAL;
-
-	return 0;
-}
-
 static ssize_t bttv_read(struct file *file, char __user *data,
 			 size_t count, loff_t *ppos)
 {
@@ -3154,34 +2980,43 @@ static unsigned int bttv_poll(struct file *file, poll_table *wait)
 	struct bttv_fh *fh = file->private_data;
 	struct bttv_buffer *buf;
 	enum v4l2_field field;
-	unsigned int rc = POLLERR;
+	unsigned int rc = 0;
+	unsigned long req_events = poll_requested_events(wait);
+
+	if (v4l2_event_pending(&fh->fh))
+		rc = POLLPRI;
+	else if (req_events & POLLPRI)
+		poll_wait(file, &fh->fh.wait, wait);
+
+	if (!(req_events & (POLLIN | POLLRDNORM)))
+		return rc;
 
 	if (V4L2_BUF_TYPE_VBI_CAPTURE == fh->type) {
 		if (!check_alloc_btres_lock(fh->btv,fh,RESOURCE_VBI))
-			return POLLERR;
-		return videobuf_poll_stream(file, &fh->vbi, wait);
+			return rc | POLLERR;
+		return rc | videobuf_poll_stream(file, &fh->vbi, wait);
 	}
 
 	if (check_btres(fh,RESOURCE_VIDEO_STREAM)) {
 		/* streaming capture */
 		if (list_empty(&fh->cap.stream))
-			goto err;
+			return rc | POLLERR;
 		buf = list_entry(fh->cap.stream.next,struct bttv_buffer,vb.stream);
 	} else {
 		/* read() capture */
 		if (NULL == fh->cap.read_buf) {
 			/* need to capture a new frame */
 			if (locked_btres(fh->btv,RESOURCE_VIDEO_STREAM))
-				goto err;
+				return rc | POLLERR;
 			fh->cap.read_buf = videobuf_sg_alloc(fh->cap.msize);
 			if (NULL == fh->cap.read_buf)
-				goto err;
+				return rc | POLLERR;
 			fh->cap.read_buf->memory = V4L2_MEMORY_USERPTR;
 			field = videobuf_next_field(&fh->cap);
 			if (0 != fh->cap.ops->buf_prepare(&fh->cap,fh->cap.read_buf,field)) {
 				kfree (fh->cap.read_buf);
 				fh->cap.read_buf = NULL;
-				goto err;
+				return rc | POLLERR;
 			}
 			fh->cap.ops->buf_queue(&fh->cap,fh->cap.read_buf);
 			fh->cap.read_off = 0;
@@ -3192,10 +3027,7 @@ static unsigned int bttv_poll(struct file *file, poll_table *wait)
 	poll_wait(file, &buf->vb.done, wait);
 	if (buf->vb.state == VIDEOBUF_DONE ||
 	    buf->vb.state == VIDEOBUF_ERROR)
-		rc =  POLLIN|POLLRDNORM;
-	else
-		rc = 0;
-err:
+		rc = rc | POLLIN|POLLRDNORM;
 	return rc;
 }
 
@@ -3224,15 +3056,15 @@ static int bttv_open(struct file *file)
 	fh = kmalloc(sizeof(*fh), GFP_KERNEL);
 	if (unlikely(!fh))
 		return -ENOMEM;
+	btv->users++;
 	file->private_data = fh;
 
 	*fh = btv->init;
+	v4l2_fh_init(&fh->fh, vdev);
 
 	fh->type = type;
 	fh->ov.setup_ok = 0;
 
-	v4l2_prio_open(&btv->prio, &fh->prio);
-
 	videobuf_queue_sg_init(&fh->cap, &bttv_video_qops,
 			    &btv->c.pci->dev, &btv->s_lock,
 			    V4L2_BUF_TYPE_VIDEO_CAPTURE,
@@ -3247,8 +3079,7 @@ static int bttv_open(struct file *file)
 			    fh, &btv->lock);
 	set_tvnorm(btv,btv->tvnorm);
 	set_input(btv, btv->input, btv->tvnorm);
-
-	btv->users++;
+	audio_mute(btv, btv->mute);
 
 	/* The V4L2 spec requires one global set of cropping parameters
 	   which only change on request. These are stored in btv->crop[1].
@@ -3267,6 +3098,7 @@ static int bttv_open(struct file *file)
 	bttv_vbi_fmt_reset(&fh->vbi_fmt, btv->tvnorm);
 
 	bttv_field_count(btv);
+	v4l2_fh_add(&fh->fh);
 	return 0;
 }
 
@@ -3302,16 +3134,17 @@ static int bttv_release(struct file *file)
 
 	videobuf_mmap_free(&fh->cap);
 	videobuf_mmap_free(&fh->vbi);
-	v4l2_prio_close(&btv->prio, fh->prio);
 	file->private_data = NULL;
-	kfree(fh);
 
 	btv->users--;
 	bttv_field_count(btv);
 
 	if (!btv->users)
-		audio_mute(btv, 1);
+		audio_mute(btv, btv->mute);
 
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
+	kfree(fh);
 	return 0;
 }
 
@@ -3350,20 +3183,16 @@ static const struct v4l2_ioctl_ops bttv_ioctl_ops = {
 	.vidioc_g_fmt_vbi_cap           = bttv_g_fmt_vbi_cap,
 	.vidioc_try_fmt_vbi_cap         = bttv_try_fmt_vbi_cap,
 	.vidioc_s_fmt_vbi_cap           = bttv_s_fmt_vbi_cap,
-	.vidioc_g_audio                 = bttv_g_audio,
-	.vidioc_s_audio                 = bttv_s_audio,
 	.vidioc_cropcap                 = bttv_cropcap,
 	.vidioc_reqbufs                 = bttv_reqbufs,
 	.vidioc_querybuf                = bttv_querybuf,
 	.vidioc_qbuf                    = bttv_qbuf,
 	.vidioc_dqbuf                   = bttv_dqbuf,
 	.vidioc_s_std                   = bttv_s_std,
+	.vidioc_g_std                   = bttv_g_std,
 	.vidioc_enum_input              = bttv_enum_input,
 	.vidioc_g_input                 = bttv_g_input,
 	.vidioc_s_input                 = bttv_s_input,
-	.vidioc_queryctrl               = bttv_queryctrl,
-	.vidioc_g_ctrl                  = bttv_g_ctrl,
-	.vidioc_s_ctrl                  = bttv_s_ctrl,
 	.vidioc_streamon                = bttv_streamon,
 	.vidioc_streamoff               = bttv_streamoff,
 	.vidioc_g_tuner                 = bttv_g_tuner,
@@ -3373,13 +3202,14 @@ static const struct v4l2_ioctl_ops bttv_ioctl_ops = {
 	.vidioc_g_fbuf                  = bttv_g_fbuf,
 	.vidioc_s_fbuf                  = bttv_s_fbuf,
 	.vidioc_overlay                 = bttv_overlay,
-	.vidioc_g_priority              = bttv_g_priority,
-	.vidioc_s_priority              = bttv_s_priority,
 	.vidioc_g_parm                  = bttv_g_parm,
 	.vidioc_g_frequency             = bttv_g_frequency,
 	.vidioc_s_frequency             = bttv_s_frequency,
 	.vidioc_log_status		= bttv_log_status,
 	.vidioc_querystd		= bttv_querystd,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+	.vidioc_g_chip_ident		= bttv_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.vidioc_g_register		= bttv_g_register,
 	.vidioc_s_register		= bttv_s_register,
@@ -3390,7 +3220,6 @@ static struct video_device bttv_video_template = {
 	.fops         = &bttv_fops,
 	.ioctl_ops    = &bttv_ioctl_ops,
 	.tvnorms      = BTTV_NORMS,
-	.current_norm = V4L2_STD_PAL,
 };
 
 /* ----------------------------------------------------------------------- */
@@ -3412,13 +3241,12 @@ static int radio_open(struct file *file)
 		return -ENOMEM;
 	file->private_data = fh;
 	*fh = btv->init;
-
-	v4l2_prio_open(&btv->prio, &fh->prio);
+	v4l2_fh_init(&fh->fh, vdev);
 
 	btv->radio_user++;
+	audio_mute(btv, btv->mute);
 
-	bttv_call_all(btv, tuner, s_radio);
-	audio_input(btv,TVAUDIO_INPUT_RADIO);
+	v4l2_fh_add(&fh->fh);
 
 	return 0;
 }
@@ -3429,28 +3257,17 @@ static int radio_release(struct file *file)
 	struct bttv *btv = fh->btv;
 	struct saa6588_command cmd;
 
-	v4l2_prio_close(&btv->prio, fh->prio);
 	file->private_data = NULL;
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
 	kfree(fh);
 
 	btv->radio_user--;
 
 	bttv_call_all(btv, core, ioctl, SAA6588_CMD_CLOSE, &cmd);
 
-	return 0;
-}
-
-static int radio_querycap(struct file *file, void *priv,
-					struct v4l2_capability *cap)
-{
-	struct bttv_fh *fh = priv;
-	struct bttv *btv = fh->btv;
-
-	strcpy(cap->driver, "bttv");
-	strlcpy(cap->card, btv->radio_dev->name, sizeof(cap->card));
-	sprintf(cap->bus_info, "PCI:%s", pci_name(btv->c.pci));
-	cap->capabilities = V4L2_CAP_TUNER;
-
+	if (btv->radio_user == 0)
+		btv->has_radio_tuner = 0;
 	return 0;
 }
 
@@ -3459,12 +3276,11 @@ static int radio_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 	struct bttv_fh *fh = priv;
 	struct bttv *btv = fh->btv;
 
-	if (btv->tuner_type == TUNER_ABSENT)
-		return -EINVAL;
 	if (0 != t->index)
 		return -EINVAL;
 	strcpy(t->name, "Radio");
 	t->type = V4L2_TUNER_RADIO;
+	radio_enable(btv);
 
 	bttv_call_all(btv, tuner, g_tuner, t);
 
@@ -3474,31 +3290,8 @@ static int radio_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 	return 0;
 }
 
-static int radio_enum_input(struct file *file, void *priv,
-				struct v4l2_input *i)
-{
-	if (i->index != 0)
-		return -EINVAL;
-
-	strcpy(i->name, "Radio");
-	i->type = V4L2_INPUT_TYPE_TUNER;
-
-	return 0;
-}
-
-static int radio_g_audio(struct file *file, void *priv,
-					struct v4l2_audio *a)
-{
-	if (unlikely(a->index))
-		return -EINVAL;
-
-	strcpy(a->name, "Radio");
-
-	return 0;
-}
-
 static int radio_s_tuner(struct file *file, void *priv,
-					struct v4l2_tuner *t)
+					const struct v4l2_tuner *t)
 {
 	struct bttv_fh *fh = priv;
 	struct bttv *btv = fh->btv;
@@ -3506,56 +3299,11 @@ static int radio_s_tuner(struct file *file, void *priv,
 	if (0 != t->index)
 		return -EINVAL;
 
+	radio_enable(btv);
 	bttv_call_all(btv, tuner, s_tuner, t);
 	return 0;
 }
 
-static int radio_s_audio(struct file *file, void *priv,
-					const struct v4l2_audio *a)
-{
-	if (unlikely(a->index))
-		return -EINVAL;
-
-	return 0;
-}
-
-static int radio_s_input(struct file *filp, void *priv, unsigned int i)
-{
-	if (unlikely(i))
-		return -EINVAL;
-
-	return 0;
-}
-
-static int radio_s_std(struct file *file, void *fh, v4l2_std_id *norm)
-{
-	return 0;
-}
-
-static int radio_queryctrl(struct file *file, void *priv,
-					struct v4l2_queryctrl *c)
-{
-	const struct v4l2_queryctrl *ctrl;
-
-	if (c->id <  V4L2_CID_BASE ||
-			c->id >= V4L2_CID_LASTP1)
-		return -EINVAL;
-
-	if (c->id == V4L2_CID_AUDIO_MUTE) {
-		ctrl = ctrl_by_id(c->id);
-		*c = *ctrl;
-	} else
-		*c = no_ctl;
-
-	return 0;
-}
-
-static int radio_g_input(struct file *filp, void *priv, unsigned int *i)
-{
-	*i = 0;
-	return 0;
-}
-
 static ssize_t radio_read(struct file *file, char __user *data,
 			 size_t count, loff_t *ppos)
 {
@@ -3566,6 +3314,7 @@ static ssize_t radio_read(struct file *file, char __user *data,
 	cmd.buffer = data;
 	cmd.instance = file;
 	cmd.result = -ENODEV;
+	radio_enable(btv);
 
 	bttv_call_all(btv, core, ioctl, SAA6588_CMD_READ, &cmd);
 
@@ -3576,10 +3325,18 @@ static unsigned int radio_poll(struct file *file, poll_table *wait)
 {
 	struct bttv_fh *fh = file->private_data;
 	struct bttv *btv = fh->btv;
+	unsigned long req_events = poll_requested_events(wait);
 	struct saa6588_command cmd;
+	unsigned int res = 0;
+
+	if (v4l2_event_pending(&fh->fh))
+		res = POLLPRI;
+	else if (req_events & POLLPRI)
+		poll_wait(file, &fh->fh.wait, wait);
+	radio_enable(btv);
 	cmd.instance = file;
 	cmd.event_list = wait;
-	cmd.result = -ENODEV;
+	cmd.result = res;
 	bttv_call_all(btv, core, ioctl, SAA6588_CMD_POLL, &cmd);
 
 	return cmd.result;
@@ -3596,20 +3353,14 @@ static const struct v4l2_file_operations radio_fops =
 };
 
 static const struct v4l2_ioctl_ops radio_ioctl_ops = {
-	.vidioc_querycap        = radio_querycap,
+	.vidioc_querycap        = bttv_querycap,
+	.vidioc_log_status	= bttv_log_status,
 	.vidioc_g_tuner         = radio_g_tuner,
-	.vidioc_enum_input      = radio_enum_input,
-	.vidioc_g_audio         = radio_g_audio,
 	.vidioc_s_tuner         = radio_s_tuner,
-	.vidioc_s_audio         = radio_s_audio,
-	.vidioc_s_input         = radio_s_input,
-	.vidioc_s_std           = radio_s_std,
-	.vidioc_queryctrl       = radio_queryctrl,
-	.vidioc_g_input         = radio_g_input,
-	.vidioc_g_ctrl          = bttv_g_ctrl,
-	.vidioc_s_ctrl          = bttv_s_ctrl,
 	.vidioc_g_frequency     = bttv_g_frequency,
 	.vidioc_s_frequency     = bttv_s_frequency,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static struct video_device radio_template = {
@@ -4112,7 +3863,8 @@ static irqreturn_t bttv_irq(int irq, void *dev_id)
 			bttv_irq_switch_video(btv);
 
 		if ((astat & BT848_INT_HLOCK)  &&  btv->opt_automute)
-			audio_mute(btv, btv->mute);  /* trigger automute */
+			/* trigger automute */
+			audio_mux_gpio(btv, btv->audio_input, btv->mute);
 
 		if (astat & (BT848_INT_SCERR|BT848_INT_OCERR)) {
 			pr_info("%d: %s%s @ %08x,",
@@ -4161,7 +3913,7 @@ static irqreturn_t bttv_irq(int irq, void *dev_id)
 
 
 /* ----------------------------------------------------------------------- */
-/* initialitation                                                          */
+/* initialization                                                          */
 
 static struct video_device *vdev_init(struct bttv *btv,
 				      const struct video_device *template,
@@ -4176,10 +3928,17 @@ static struct video_device *vdev_init(struct bttv *btv,
 	vfd->v4l2_dev = &btv->c.v4l2_dev;
 	vfd->release = video_device_release;
 	vfd->debug   = bttv_debug;
+	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
 	video_set_drvdata(vfd, btv);
 	snprintf(vfd->name, sizeof(vfd->name), "BT%d%s %s (%s)",
 		 btv->id, (btv->id==848 && btv->revision==0x12) ? "A" : "",
 		 type_name, bttv_tvcards[btv->c.type].name);
+	if (btv->tuner_type == TUNER_ABSENT) {
+		v4l2_disable_ioctl(vfd, VIDIOC_G_FREQUENCY);
+		v4l2_disable_ioctl(vfd, VIDIOC_S_FREQUENCY);
+		v4l2_disable_ioctl(vfd, VIDIOC_G_TUNER);
+		v4l2_disable_ioctl(vfd, VIDIOC_S_TUNER);
+	}
 	return vfd;
 }
 
@@ -4247,6 +4006,7 @@ static int bttv_register_video(struct bttv *btv)
 	btv->radio_dev = vdev_init(btv, &radio_template, "radio");
 	if (NULL == btv->radio_dev)
 		goto err;
+	btv->radio_dev->ctrl_handler = &btv->radio_ctrl_handler;
 	if (video_register_device(btv->radio_dev, VFL_TYPE_RADIO,
 				  radio_nr[btv->c.nr]) < 0)
 		goto err;
@@ -4277,9 +4037,15 @@ static void pci_set_command(struct pci_dev *dev)
 
 static int bttv_probe(struct pci_dev *dev, const struct pci_device_id *pci_id)
 {
+	struct v4l2_frequency init_freq = {
+		.tuner = 0,
+		.type = V4L2_TUNER_ANALOG_TV,
+		.frequency = 980,
+	};
 	int result;
 	unsigned char lat;
 	struct bttv *btv;
+	struct v4l2_ctrl_handler *hdl;
 
 	if (bttv_num == BTTV_MAX)
 		return -ENOMEM;
@@ -4301,7 +4067,6 @@ static int bttv_probe(struct pci_dev *dev, const struct pci_device_id *pci_id)
 	INIT_LIST_HEAD(&btv->c.subs);
 	INIT_LIST_HEAD(&btv->capture);
 	INIT_LIST_HEAD(&btv->vcapture);
-	v4l2_prio_init(&btv->prio);
 
 	init_timer(&btv->timeout);
 	btv->timeout.function = bttv_irq_timeout;
@@ -4339,6 +4104,10 @@ static int bttv_probe(struct pci_dev *dev, const struct pci_device_id *pci_id)
 		pr_warn("%d: v4l2_device_register() failed\n", btv->c.nr);
 		goto fail0;
 	}
+	hdl = &btv->ctrl_handler;
+	v4l2_ctrl_handler_init(hdl, 20);
+	btv->c.v4l2_dev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(&btv->radio_ctrl_handler, 6);
 
 	btv->revision = dev->revision;
 	pci_read_config_byte(dev, PCI_LATENCY_TIMER, &lat);
@@ -4375,16 +4144,19 @@ static int bttv_probe(struct pci_dev *dev, const struct pci_device_id *pci_id)
 
 	/* init options from insmod args */
 	btv->opt_combfilter = combfilter;
-	btv->opt_lumafilter = lumafilter;
+	bttv_ctrl_combfilter.def = combfilter;
+	bttv_ctrl_lumafilter.def = lumafilter;
 	btv->opt_automute   = automute;
-	btv->opt_chroma_agc = chroma_agc;
-	btv->opt_adc_crush  = adc_crush;
+	bttv_ctrl_automute.def = automute;
+	bttv_ctrl_agc_crush.def = agc_crush;
 	btv->opt_vcr_hack   = vcr_hack;
-	btv->opt_whitecrush_upper  = whitecrush_upper;
-	btv->opt_whitecrush_lower  = whitecrush_lower;
+	bttv_ctrl_vcr_hack.def = vcr_hack;
+	bttv_ctrl_whitecrush_upper.def = whitecrush_upper;
+	bttv_ctrl_whitecrush_lower.def = whitecrush_lower;
 	btv->opt_uv_ratio   = uv_ratio;
-	btv->opt_full_luma_range   = full_luma_range;
-	btv->opt_coring     = coring;
+	bttv_ctrl_uv_ratio.def = uv_ratio;
+	bttv_ctrl_full_luma.def = full_luma_range;
+	bttv_ctrl_coring.def = coring;
 
 	/* fill struct bttv with some useful defaults */
 	btv->init.btv         = btv;
@@ -4393,8 +4165,39 @@ static int bttv_probe(struct pci_dev *dev, const struct pci_device_id *pci_id)
 	btv->init.fmt         = format_by_fourcc(V4L2_PIX_FMT_BGR24);
 	btv->init.width       = 320;
 	btv->init.height      = 240;
+	btv->init.ov.w.width  = 320;
+	btv->init.ov.w.height = 240;
+	btv->init.ov.field    = V4L2_FIELD_INTERLACED;
 	btv->input = 0;
 
+	v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 0xff00, 0x100, 32768);
+	v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 0xff80, 0x80, 0x6c00);
+	v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 0xff80, 0x80, 32768);
+	v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+			V4L2_CID_COLOR_KILLER, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+			V4L2_CID_HUE, 0, 0xff00, 0x100, 32768);
+	v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+			V4L2_CID_CHROMA_AGC, 0, 1, 1, !!chroma_agc);
+	v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+		V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	if (btv->volume_gpio)
+		v4l2_ctrl_new_std(hdl, &bttv_ctrl_ops,
+			V4L2_CID_AUDIO_VOLUME, 0, 0xff00, 0x100, 0xff00);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_combfilter, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_automute, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_lumafilter, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_agc_crush, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_vcr_hack, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_whitecrush_lower, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_whitecrush_upper, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_uv_ratio, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_full_luma, NULL);
+	v4l2_ctrl_new_custom(hdl, &bttv_ctrl_coring, NULL);
+
 	/* initialize hardware */
 	if (bttv_gpio)
 		bttv_gpio_tracking(btv,"pre-init");
@@ -4417,21 +4220,34 @@ static int bttv_probe(struct pci_dev *dev, const struct pci_device_id *pci_id)
 	/* some card-specific stuff (needs working i2c) */
 	bttv_init_card2(btv);
 	bttv_init_tuner(btv);
+	if (btv->tuner_type != TUNER_ABSENT) {
+		bttv_set_frequency(btv, &init_freq);
+		btv->radio_freq = 90500 * 16; /* 90.5Mhz default */
+	}
+	btv->std = V4L2_STD_PAL;
 	init_irqreg(btv);
+	v4l2_ctrl_handler_setup(hdl);
+	if (hdl->error) {
+		result = hdl->error;
+		goto fail2;
+	}
+	/* mute device */
+	audio_mute(btv, 1);
 
 	/* register video4linux + input */
 	if (!bttv_tvcards[btv->c.type].no_video) {
-		bttv_register_video(btv);
-		bt848_bright(btv,32768);
-		bt848_contrast(btv, 27648);
-		bt848_hue(btv,32768);
-		bt848_sat(btv,32768);
-		audio_mute(btv, 1);
+		v4l2_ctrl_add_handler(&btv->radio_ctrl_handler, hdl,
+				v4l2_ctrl_radio_filter);
+		if (btv->radio_ctrl_handler.error) {
+			result = btv->radio_ctrl_handler.error;
+			goto fail2;
+		}
 		set_input(btv, 0, btv->tvnorm);
 		bttv_crop_reset(&btv->crop[0], btv->tvnorm);
 		btv->crop[1] = btv->crop[0]; /* current = default */
 		disclaim_vbi_lines(btv);
 		disclaim_video_lines(btv);
+		bttv_register_video(btv);
 	}
 
 	/* add subdevices and autoload dvb-bt8xx if needed */
@@ -4453,6 +4269,8 @@ fail2:
 	free_irq(btv->c.pci->irq,btv);
 
 fail1:
+	v4l2_ctrl_handler_free(&btv->ctrl_handler);
+	v4l2_ctrl_handler_free(&btv->radio_ctrl_handler);
 	v4l2_device_unregister(&btv->c.v4l2_dev);
 
 fail0:
@@ -4494,9 +4312,11 @@ static void bttv_remove(struct pci_dev *pci_dev)
 	bttv_unregister_video(btv);
 
 	/* free allocated memory */
+	v4l2_ctrl_handler_free(&btv->ctrl_handler);
+	v4l2_ctrl_handler_free(&btv->radio_ctrl_handler);
 	btcx_riscmem_free(btv->c.pci,&btv->main);
 
-	/* free ressources */
+	/* free resources */
 	free_irq(btv->c.pci->irq,btv);
 	iounmap(btv->bt848_mmio);
 	release_mem_region(pci_resource_start(btv->c.pci,0),
diff --git a/drivers/media/pci/bt8xx/bttv-i2c.c b/drivers/media/pci/bt8xx/bttv-i2c.c
index c63c643ed1f8..d43911deb617 100644
--- a/drivers/media/pci/bt8xx/bttv-i2c.c
+++ b/drivers/media/pci/bt8xx/bttv-i2c.c
@@ -394,3 +394,11 @@ int init_bttv_i2c(struct bttv *btv)
 
 	return btv->i2c_rc;
 }
+
+int fini_bttv_i2c(struct bttv *btv)
+{
+	if (btv->i2c_rc == 0)
+		i2c_del_adapter(&btv->c.i2c_adap);
+
+	return 0;
+}
diff --git a/drivers/media/pci/bt8xx/bttv-input.c b/drivers/media/pci/bt8xx/bttv-input.c
index 04207a799055..f36821367d8d 100644
--- a/drivers/media/pci/bt8xx/bttv-input.c
+++ b/drivers/media/pci/bt8xx/bttv-input.c
@@ -375,6 +375,7 @@ void init_bttv_i2c_ir(struct bttv *btv)
 		I2C_CLIENT_END
 	};
 	struct i2c_board_info info;
+	struct i2c_client *i2c_dev;
 
 	if (0 != btv->i2c_rc)
 		return;
@@ -390,7 +391,12 @@ void init_bttv_i2c_ir(struct bttv *btv)
 		btv->init_data.ir_codes = RC_MAP_PV951;
 		info.addr = 0x4b;
 		break;
-	default:
+	}
+
+	if (btv->init_data.name) {
+		info.platform_data = &btv->init_data;
+		i2c_dev = i2c_new_device(&btv->c.i2c_adap, &info);
+	} else {
 		/*
 		 * The external IR receiver is at i2c address 0x34 (0x35 for
 		 * reads).  Future Hauppauge cards will have an internal
@@ -399,24 +405,14 @@ void init_bttv_i2c_ir(struct bttv *btv)
 		 * internal.
 		 * That's why we probe 0x1a (~0x34) first. CB
 		 */
-
-		i2c_new_probed_device(&btv->c.i2c_adap, &info, addr_list, NULL);
-		return;
+		i2c_dev = i2c_new_probed_device(&btv->c.i2c_adap, &info, addr_list, NULL);
 	}
+	if (NULL == i2c_dev)
+		return;
 
-	if (btv->init_data.name)
-		info.platform_data = &btv->init_data;
-	i2c_new_device(&btv->c.i2c_adap, &info);
-
-	return;
-}
-
-int fini_bttv_i2c(struct bttv *btv)
-{
-	if (0 != btv->i2c_rc)
-		return 0;
-
-	return i2c_del_adapter(&btv->c.i2c_adap);
+#if defined(CONFIG_MODULES) && defined(MODULE)
+	request_module("ir-kbd-i2c");
+#endif
 }
 
 int bttv_input_init(struct bttv *btv)
diff --git a/drivers/media/pci/bt8xx/bttv.h b/drivers/media/pci/bt8xx/bttv.h
index 79a11240a590..6139ce26dc2c 100644
--- a/drivers/media/pci/bt8xx/bttv.h
+++ b/drivers/media/pci/bt8xx/bttv.h
@@ -359,6 +359,9 @@ void bttv_gpio_bits(struct bttv_core *core, u32 mask, u32 bits);
 #define bttv_call_all(btv, o, f, args...) \
 	v4l2_device_call_all(&btv->c.v4l2_dev, 0, o, f, ##args)
 
+#define bttv_call_all_err(btv, o, f, args...) \
+	v4l2_device_call_until_err(&btv->c.v4l2_dev, 0, o, f, ##args)
+
 extern int bttv_I2CRead(struct bttv *btv, unsigned char addr, char *probe_for);
 extern int bttv_I2CWrite(struct bttv *btv, unsigned char addr, unsigned char b1,
 			 unsigned char b2, int both);
diff --git a/drivers/media/pci/bt8xx/bttvp.h b/drivers/media/pci/bt8xx/bttvp.h
index 9ec0adba236c..9c1cc2c50ee2 100644
--- a/drivers/media/pci/bt8xx/bttvp.h
+++ b/drivers/media/pci/bt8xx/bttvp.h
@@ -33,9 +33,11 @@
 #include <linux/input.h>
 #include <linux/mutex.h>
 #include <linux/scatterlist.h>
+#include <linux/device.h>
 #include <asm/io.h>
 #include <media/v4l2-common.h>
-#include <linux/device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
 #include <media/videobuf-dma-sg.h>
 #include <media/tveeprom.h>
 #include <media/rc-core.h>
@@ -214,11 +216,11 @@ struct bttv_crop {
 };
 
 struct bttv_fh {
+	/* This must be the first field in this struct */
+	struct v4l2_fh		 fh;
+
 	struct bttv              *btv;
 	int resources;
-#ifdef VIDIOC_G_PRIORITY
-	enum v4l2_priority       prio;
-#endif
 	enum v4l2_buf_type       type;
 
 	/* video capture */
@@ -298,6 +300,10 @@ extern int no_overlay;
 /* bttv-input.c                                               */
 
 extern void init_bttv_i2c_ir(struct bttv *btv);
+
+/* ---------------------------------------------------------- */
+/* bttv-i2c.c                                                 */
+extern int init_bttv_i2c(struct bttv *btv);
 extern int fini_bttv_i2c(struct bttv *btv);
 
 /* ---------------------------------------------------------- */
@@ -308,7 +314,6 @@ extern unsigned int bttv_verbose;
 extern unsigned int bttv_debug;
 extern unsigned int bttv_gpio;
 extern void bttv_gpio_tracking(struct bttv *btv, char *comment);
-extern int init_bttv_i2c(struct bttv *btv);
 
 #define dprintk(fmt, ...)			\
 do {						\
@@ -393,12 +398,17 @@ struct bttv {
 	wait_queue_head_t          i2c_queue;
 	struct v4l2_subdev 	  *sd_msp34xx;
 	struct v4l2_subdev 	  *sd_tvaudio;
+	struct v4l2_subdev	  *sd_tda7432;
 
 	/* video4linux (1) */
 	struct video_device *video_dev;
 	struct video_device *radio_dev;
 	struct video_device *vbi_dev;
 
+	/* controls */
+	struct v4l2_ctrl_handler   ctrl_handler;
+	struct v4l2_ctrl_handler   radio_ctrl_handler;
+
 	/* infrared remote */
 	int has_remote;
 	struct bttv_ir *remote;
@@ -410,38 +420,30 @@ struct bttv {
 	spinlock_t s_lock;
 	struct mutex lock;
 	int resources;
-#ifdef VIDIOC_G_PRIORITY
-	struct v4l2_prio_state prio;
-#endif
 
 	/* video state */
 	unsigned int input;
-	unsigned int audio;
+	unsigned int audio_input;
 	unsigned int mute;
-	unsigned long freq;
+	unsigned long tv_freq;
 	unsigned int tvnorm;
+	v4l2_std_id std;
 	int hue, contrast, bright, saturation;
 	struct v4l2_framebuffer fbuf;
 	unsigned int field_count;
 
 	/* various options */
 	int opt_combfilter;
-	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;
-	int opt_whitecrush_lower;
 	int opt_uv_ratio;
-	int opt_full_luma_range;
-	int opt_coring;
 
 	/* radio data/state */
 	int has_radio;
+	int has_radio_tuner;
 	int radio_user;
 	int radio_uses_msp_demodulator;
+	unsigned long radio_freq;
 
 	/* miro/pinnacle + Aimslab VHX
 	   philips matchbox (tea5757 radio tuner) support */
diff --git a/drivers/media/pci/cx18/cx18-av-core.c b/drivers/media/pci/cx18/cx18-av-core.c
index f164b7f610a5..38b1d64ffc27 100644
--- a/drivers/media/pci/cx18/cx18-av-core.c
+++ b/drivers/media/pci/cx18/cx18-av-core.c
@@ -576,7 +576,7 @@ static void input_change(struct cx18 *cx)
 }
 
 static int cx18_av_s_frequency(struct v4l2_subdev *sd,
-			       struct v4l2_frequency *freq)
+			       const struct v4l2_frequency *freq)
 {
 	struct cx18 *cx = v4l2_get_subdevdata(sd);
 	input_change(cx);
@@ -809,7 +809,7 @@ static int cx18_av_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	return 0;
 }
 
-static int cx18_av_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int cx18_av_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct cx18_av_state *state = to_cx18_av_state(sd);
 	struct cx18 *cx = v4l2_get_subdevdata(sd);
@@ -1266,7 +1266,7 @@ static int cx18_av_g_register(struct v4l2_subdev *sd,
 }
 
 static int cx18_av_s_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct cx18 *cx = v4l2_get_subdevdata(sd);
 
diff --git a/drivers/media/pci/cx18/cx18-driver.c b/drivers/media/pci/cx18/cx18-driver.c
index 613e5ae7d5ca..67b61cf3e03a 100644
--- a/drivers/media/pci/cx18/cx18-driver.c
+++ b/drivers/media/pci/cx18/cx18-driver.c
@@ -1243,7 +1243,7 @@ int cx18_init_on_first_open(struct cx18 *cx)
 	   in one place. */
 	cx->std++;		/* Force full standard initialization */
 	std = (cx->tuner_std == V4L2_STD_ALL) ? V4L2_STD_NTSC_M : cx->tuner_std;
-	cx18_s_std(NULL, &fh, &std);
+	cx18_s_std(NULL, &fh, std);
 	cx18_s_frequency(NULL, &fh, &vf);
 	return 0;
 }
diff --git a/drivers/media/pci/cx18/cx18-ioctl.c b/drivers/media/pci/cx18/cx18-ioctl.c
index cd8d2c2b1624..aee7b6dacbfe 100644
--- a/drivers/media/pci/cx18/cx18-ioctl.c
+++ b/drivers/media/pci/cx18/cx18-ioctl.c
@@ -415,42 +415,34 @@ static int cx18_g_chip_ident(struct file *file, void *fh,
 }
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-static int cx18_cxc(struct cx18 *cx, unsigned int cmd, void *arg)
-{
-	struct v4l2_dbg_register *regs = arg;
-
-	if (!capable(CAP_SYS_ADMIN))
-		return -EPERM;
-	if (regs->reg >= CX18_MEM_OFFSET + CX18_MEM_SIZE)
-		return -EINVAL;
-
-	regs->size = 4;
-	if (cmd == VIDIOC_DBG_S_REGISTER)
-		cx18_write_enc(cx, regs->val, regs->reg);
-	else
-		regs->val = cx18_read_enc(cx, regs->reg);
-	return 0;
-}
-
 static int cx18_g_register(struct file *file, void *fh,
 				struct v4l2_dbg_register *reg)
 {
 	struct cx18 *cx = fh2id(fh)->cx;
 
-	if (v4l2_chip_match_host(&reg->match))
-		return cx18_cxc(cx, VIDIOC_DBG_G_REGISTER, reg);
+	if (v4l2_chip_match_host(&reg->match)) {
+		if (reg->reg >= CX18_MEM_OFFSET + CX18_MEM_SIZE)
+			return -EINVAL;
+		reg->size = 4;
+		reg->val = cx18_read_enc(cx, reg->reg);
+		return 0;
+	}
 	/* FIXME - errors shouldn't be ignored */
 	cx18_call_all(cx, core, g_register, reg);
 	return 0;
 }
 
 static int cx18_s_register(struct file *file, void *fh,
-				struct v4l2_dbg_register *reg)
+				const struct v4l2_dbg_register *reg)
 {
 	struct cx18 *cx = fh2id(fh)->cx;
 
-	if (v4l2_chip_match_host(&reg->match))
-		return cx18_cxc(cx, VIDIOC_DBG_S_REGISTER, reg);
+	if (v4l2_chip_match_host(&reg->match)) {
+		if (reg->reg >= CX18_MEM_OFFSET + CX18_MEM_SIZE)
+			return -EINVAL;
+		cx18_write_enc(cx, reg->val, reg->reg);
+		return 0;
+	}
 	/* FIXME - errors shouldn't be ignored */
 	cx18_call_all(cx, core, s_register, reg);
 	return 0;
@@ -614,7 +606,7 @@ static int cx18_g_frequency(struct file *file, void *fh,
 	return 0;
 }
 
-int cx18_s_frequency(struct file *file, void *fh, struct v4l2_frequency *vf)
+int cx18_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf)
 {
 	struct cx18_open_id *id = fh2id(fh);
 	struct cx18 *cx = id->cx;
@@ -637,15 +629,15 @@ static int cx18_g_std(struct file *file, void *fh, v4l2_std_id *std)
 	return 0;
 }
 
-int cx18_s_std(struct file *file, void *fh, v4l2_std_id *std)
+int cx18_s_std(struct file *file, void *fh, v4l2_std_id std)
 {
 	struct cx18_open_id *id = fh2id(fh);
 	struct cx18 *cx = id->cx;
 
-	if ((*std & V4L2_STD_ALL) == 0)
+	if ((std & V4L2_STD_ALL) == 0)
 		return -EINVAL;
 
-	if (*std == cx->std)
+	if (std == cx->std)
 		return 0;
 
 	if (test_bit(CX18_F_I_RADIO_USER, &cx->i_flags) ||
@@ -656,8 +648,8 @@ int cx18_s_std(struct file *file, void *fh, v4l2_std_id *std)
 		return -EBUSY;
 	}
 
-	cx->std = *std;
-	cx->is_60hz = (*std & V4L2_STD_525_60) ? 1 : 0;
+	cx->std = std;
+	cx->is_60hz = (std & V4L2_STD_525_60) ? 1 : 0;
 	cx->is_50hz = !cx->is_60hz;
 	cx2341x_handler_set_50hz(&cx->cxhdl, cx->is_50hz);
 	cx->cxhdl.width = 720;
@@ -673,7 +665,7 @@ int cx18_s_std(struct file *file, void *fh, v4l2_std_id *std)
 	return 0;
 }
 
-static int cx18_s_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+static int cx18_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
 {
 	struct cx18_open_id *id = fh2id(fh);
 	struct cx18 *cx = id->cx;
@@ -1118,7 +1110,7 @@ static int cx18_log_status(struct file *file, void *fh)
 }
 
 static long cx18_default(struct file *file, void *fh, bool valid_prio,
-							int cmd, void *arg)
+			 unsigned int cmd, void *arg)
 {
 	struct cx18 *cx = fh2id(fh)->cx;
 
diff --git a/drivers/media/pci/cx18/cx18-ioctl.h b/drivers/media/pci/cx18/cx18-ioctl.h
index 2f9dd591ee0f..43433969d633 100644
--- a/drivers/media/pci/cx18/cx18-ioctl.h
+++ b/drivers/media/pci/cx18/cx18-ioctl.h
@@ -26,6 +26,6 @@ u16 cx18_service2vbi(int type);
 void cx18_expand_service_set(struct v4l2_sliced_vbi_format *fmt, int is_pal);
 u16 cx18_get_service_set(struct v4l2_sliced_vbi_format *fmt);
 void cx18_set_funcs(struct video_device *vdev);
-int cx18_s_std(struct file *file, void *fh, v4l2_std_id *std);
-int cx18_s_frequency(struct file *file, void *fh, struct v4l2_frequency *vf);
+int cx18_s_std(struct file *file, void *fh, v4l2_std_id std);
+int cx18_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf);
 int cx18_s_input(struct file *file, void *fh, unsigned int inp);
diff --git a/drivers/media/pci/cx23885/altera-ci.h b/drivers/media/pci/cx23885/altera-ci.h
index 70e4fd69ad9e..4998c96caebe 100644
--- a/drivers/media/pci/cx23885/altera-ci.h
+++ b/drivers/media/pci/cx23885/altera-ci.h
@@ -24,6 +24,8 @@
 #ifndef __ALTERA_CI_H
 #define __ALTERA_CI_H
 
+#include <linux/kconfig.h>
+
 #define ALT_DATA	0x000000ff
 #define ALT_TDI		0x00008000
 #define ALT_TDO		0x00004000
@@ -41,8 +43,7 @@ struct altera_ci_config {
 	int (*fpga_rw) (void *dev, int ad_rg, int val, int rw);
 };
 
-#if defined(CONFIG_MEDIA_ALTERA_CI) || (defined(CONFIG_MEDIA_ALTERA_CI_MODULE) \
-							&& defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_ALTERA_CI)
 
 extern int altera_ci_init(struct altera_ci_config *config, int ci_nr);
 extern void altera_ci_release(void *dev, int ci_nr);
diff --git a/drivers/media/pci/cx23885/cx23885-417.c b/drivers/media/pci/cx23885/cx23885-417.c
index 5d5052d0253f..6dea11a7a858 100644
--- a/drivers/media/pci/cx23885/cx23885-417.c
+++ b/drivers/media/pci/cx23885/cx23885-417.c
@@ -1222,14 +1222,14 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct cx23885_fh  *fh  = file->private_data;
 	struct cx23885_dev *dev = fh->dev;
 	unsigned int i;
 
 	for (i = 0; i < ARRAY_SIZE(cx23885_tvnorms); i++)
-		if (*id & cx23885_tvnorms[i].id)
+		if (id & cx23885_tvnorms[i].id)
 			break;
 	if (i == ARRAY_SIZE(cx23885_tvnorms))
 		return -EINVAL;
@@ -1237,7 +1237,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
 
 	/* Have the drier core notify the subdevices */
 	mutex_lock(&dev->lock);
-	cx23885_set_tvnorm(dev, *id);
+	cx23885_set_tvnorm(dev, id);
 	mutex_unlock(&dev->lock);
 
 	return 0;
@@ -1280,7 +1280,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct cx23885_fh  *fh  = file->private_data;
 	struct cx23885_dev *dev = fh->dev;
@@ -1311,7 +1311,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-	struct v4l2_frequency *f)
+	const struct v4l2_frequency *f)
 {
 	return cx23885_set_frequency(file, priv, f);
 }
diff --git a/drivers/media/pci/cx23885/cx23885-ioctl.c b/drivers/media/pci/cx23885/cx23885-ioctl.c
index ea9a614f3bbf..acdb6d58db58 100644
--- a/drivers/media/pci/cx23885/cx23885-ioctl.c
+++ b/drivers/media/pci/cx23885/cx23885-ioctl.c
@@ -158,18 +158,17 @@ int cx23885_g_register(struct file *file, void *fh,
 }
 
 static int cx23885_s_host_register(struct cx23885_dev *dev,
-				   struct v4l2_dbg_register *reg)
+				   const struct v4l2_dbg_register *reg)
 {
 	if ((reg->reg & 0x3) != 0 || reg->reg >= pci_resource_len(dev->pci, 0))
 		return -EINVAL;
 
-	reg->size = 4;
 	cx_write(reg->reg, reg->val);
 	return 0;
 }
 
 static int cx23417_s_register(struct cx23885_dev *dev,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	if (dev->v4l_device == NULL)
 		return -EINVAL;
@@ -179,13 +178,11 @@ static int cx23417_s_register(struct cx23885_dev *dev,
 
 	if (mc417_register_write(dev, (u16) reg->reg, (u32) reg->val))
 		return -EINVAL; /* V4L2 spec, but -EREMOTEIO really */
-
-	reg->size = 4;
 	return 0;
 }
 
 int cx23885_s_register(struct file *file, void *fh,
-		       struct v4l2_dbg_register *reg)
+		       const struct v4l2_dbg_register *reg)
 {
 	struct cx23885_dev *dev = ((struct cx23885_fh *)fh)->dev;
 
diff --git a/drivers/media/pci/cx23885/cx23885-ioctl.h b/drivers/media/pci/cx23885/cx23885-ioctl.h
index 315be0ca5a04..a6080964a9ee 100644
--- a/drivers/media/pci/cx23885/cx23885-ioctl.h
+++ b/drivers/media/pci/cx23885/cx23885-ioctl.h
@@ -33,7 +33,7 @@ int cx23885_g_register(struct file *file, void *fh,
 
 
 int cx23885_s_register(struct file *file, void *fh,
-		       struct v4l2_dbg_register *reg);
+		       const struct v4l2_dbg_register *reg);
 
 #endif
 #endif
diff --git a/drivers/media/pci/cx23885/cx23885-video.c b/drivers/media/pci/cx23885/cx23885-video.c
index 5991bc8dc158..ed08c89adde0 100644
--- a/drivers/media/pci/cx23885/cx23885-video.c
+++ b/drivers/media/pci/cx23885/cx23885-video.c
@@ -1259,13 +1259,13 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *tvnorms)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id tvnorms)
 {
 	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
 	dprintk(1, "%s()\n", __func__);
 
 	mutex_lock(&dev->lock);
-	cx23885_set_tvnorm(dev, *tvnorms);
+	cx23885_set_tvnorm(dev, tvnorms);
 	mutex_unlock(&dev->lock);
 
 	return 0;
@@ -1486,7 +1486,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
 
@@ -1518,7 +1518,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 	return 0;
 }
 
-static int cx23885_set_freq(struct cx23885_dev *dev, struct v4l2_frequency *f)
+static int cx23885_set_freq(struct cx23885_dev *dev, const struct v4l2_frequency *f)
 {
 	struct v4l2_control ctrl;
 
@@ -1550,7 +1550,7 @@ static int cx23885_set_freq(struct cx23885_dev *dev, struct v4l2_frequency *f)
 }
 
 static int cx23885_set_freq_via_ops(struct cx23885_dev *dev,
-	struct v4l2_frequency *f)
+	const struct v4l2_frequency *f)
 {
 	struct v4l2_control ctrl;
 	struct videobuf_dvb_frontend *vfe;
@@ -1608,7 +1608,7 @@ static int cx23885_set_freq_via_ops(struct cx23885_dev *dev,
 }
 
 int cx23885_set_frequency(struct file *file, void *priv,
-	struct v4l2_frequency *f)
+	const struct v4l2_frequency *f)
 {
 	struct cx23885_fh *fh = priv;
 	struct cx23885_dev *dev = fh->dev;
@@ -1628,7 +1628,7 @@ int cx23885_set_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-	struct v4l2_frequency *f)
+	const struct v4l2_frequency *f)
 {
 	return cx23885_set_frequency(file, priv, f);
 }
diff --git a/drivers/media/pci/cx23885/cx23885.h b/drivers/media/pci/cx23885/cx23885.h
index 59c322d870f2..5687d3f678db 100644
--- a/drivers/media/pci/cx23885/cx23885.h
+++ b/drivers/media/pci/cx23885/cx23885.h
@@ -587,7 +587,7 @@ extern void cx23885_video_wakeup(struct cx23885_dev *dev,
 int cx23885_enum_input(struct cx23885_dev *dev, struct v4l2_input *i);
 int cx23885_set_input(struct file *file, void *priv, unsigned int i);
 int cx23885_get_input(struct file *file, void *priv, unsigned int *i);
-int cx23885_set_frequency(struct file *file, void *priv, struct v4l2_frequency *f);
+int cx23885_set_frequency(struct file *file, void *priv, const struct v4l2_frequency *f);
 int cx23885_set_control(struct cx23885_dev *dev, struct v4l2_control *ctl);
 int cx23885_get_control(struct cx23885_dev *dev, struct v4l2_control *ctl);
 int cx23885_set_tvnorm(struct cx23885_dev *dev, v4l2_std_id norm);
diff --git a/drivers/media/pci/cx23885/cx23888-ir.c b/drivers/media/pci/cx23885/cx23888-ir.c
index d51eed051d59..fa672fe41079 100644
--- a/drivers/media/pci/cx23885/cx23888-ir.c
+++ b/drivers/media/pci/cx23885/cx23888-ir.c
@@ -1124,7 +1124,7 @@ static int cx23888_ir_g_register(struct v4l2_subdev *sd,
 }
 
 static int cx23888_ir_s_register(struct v4l2_subdev *sd,
-				 struct v4l2_dbg_register *reg)
+				 const struct v4l2_dbg_register *reg)
 {
 	struct cx23888_ir_state *state = to_state(sd);
 	u32 addr = CX23888_IR_REG_BASE + (u32) reg->reg;
diff --git a/drivers/media/pci/cx25821/Kconfig b/drivers/media/pci/cx25821/Kconfig
index 4017c9420348..6439a847680c 100644
--- a/drivers/media/pci/cx25821/Kconfig
+++ b/drivers/media/pci/cx25821/Kconfig
@@ -1,14 +1,9 @@
 config VIDEO_CX25821
 	tristate "Conexant cx25821 support"
-	depends on DVB_CORE && VIDEO_DEV && PCI && I2C
+	depends on VIDEO_DEV && PCI && I2C
 	select I2C_ALGOBIT
 	select VIDEO_BTCX
-	select VIDEO_TVEEPROM
-	depends on RC_CORE
-	select VIDEOBUF_DVB
 	select VIDEOBUF_DMA_SG
-	select VIDEO_CX25840
-	select VIDEO_CX2341X
 	---help---
 	  This is a video4linux driver for Conexant 25821 based
 	  TV cards.
diff --git a/drivers/media/pci/cx25821/Makefile b/drivers/media/pci/cx25821/Makefile
index caa32b7b51f8..fb76c3d3713a 100644
--- a/drivers/media/pci/cx25821/Makefile
+++ b/drivers/media/pci/cx25821/Makefile
@@ -1,14 +1,9 @@
 cx25821-y   := cx25821-core.o cx25821-cards.o cx25821-i2c.o \
 		       cx25821-gpio.o cx25821-medusa-video.o \
-		       cx25821-video.o cx25821-video-upstream.o \
-		       cx25821-video-upstream-ch2.o \
-		       cx25821-audio-upstream.o
+		       cx25821-video.o cx25821-video-upstream.o
 
 obj-$(CONFIG_VIDEO_CX25821) += cx25821.o
 obj-$(CONFIG_VIDEO_CX25821_ALSA) += cx25821-alsa.o
 
 ccflags-y += -Idrivers/media/i2c
 ccflags-y += -Idrivers/media/common
-ccflags-y += -Idrivers/media/tuners
-ccflags-y += -Idrivers/media/dvb-core
-ccflags-y += -Idrivers/media/dvb-frontends
diff --git a/drivers/media/pci/cx25821/cx25821-alsa.c b/drivers/media/pci/cx25821/cx25821-alsa.c
index 1858a45dd081..6e91e84d6bf9 100644
--- a/drivers/media/pci/cx25821/cx25821-alsa.c
+++ b/drivers/media/pci/cx25821/cx25821-alsa.c
@@ -59,7 +59,6 @@ do {									\
 	Data type declarations - Can be moded to a header file later
  ****************************************************************************/
 
-static struct snd_card *snd_cx25821_cards[SNDRV_CARDS];
 static int devno;
 
 struct cx25821_audio_buffer {
@@ -151,7 +150,7 @@ static int _cx25821_start_audio_dma(struct cx25821_audio_dev *chip)
 {
 	struct cx25821_audio_buffer *buf = chip->buf;
 	struct cx25821_dev *dev = chip->dev;
-	struct sram_channel *audio_ch =
+	const struct sram_channel *audio_ch =
 	    &cx25821_sram_channels[AUDIO_SRAM_CHANNEL];
 	u32 tmp = 0;
 
@@ -627,34 +626,6 @@ static DEFINE_PCI_DEVICE_TABLE(cx25821_audio_pci_tbl) = {
 MODULE_DEVICE_TABLE(pci, cx25821_audio_pci_tbl);
 
 /*
- * Not used in the function snd_cx25821_dev_free so removing
- * from the file.
- */
-/*
-static int snd_cx25821_free(struct cx25821_audio_dev *chip)
-{
-	if (chip->irq >= 0)
-		free_irq(chip->irq, chip);
-
-	cx25821_dev_unregister(chip->dev);
-	pci_disable_device(chip->pci);
-
-	return 0;
-}
-*/
-
-/*
- * Component Destructor
- */
-static void snd_cx25821_dev_free(struct snd_card *card)
-{
-	struct cx25821_audio_dev *chip = card->private_data;
-
-	/* snd_cx25821_free(chip); */
-	snd_card_free(chip->card);
-}
-
-/*
  * Alsa Constructor - Component probe
  */
 static int cx25821_audio_initdev(struct cx25821_dev *dev)
@@ -685,7 +656,6 @@ static int cx25821_audio_initdev(struct cx25821_dev *dev)
 	strcpy(card->driver, "cx25821");
 
 	/* Card "creation" */
-	card->private_free = snd_cx25821_dev_free;
 	chip = card->private_data;
 	spin_lock_init(&chip->reg_lock);
 
@@ -729,8 +699,7 @@ static int cx25821_audio_initdev(struct cx25821_dev *dev)
 		goto error;
 	}
 
-	snd_cx25821_cards[devno] = card;
-
+	dev->card = card;
 	devno++;
 	return 0;
 
@@ -742,9 +711,33 @@ error:
 /****************************************************************************
 				LINUX MODULE INIT
  ****************************************************************************/
+
+static int cx25821_alsa_exit_callback(struct device *dev, void *data)
+{
+	struct v4l2_device *v4l2_dev = dev_get_drvdata(dev);
+	struct cx25821_dev *cxdev = get_cx25821(v4l2_dev);
+
+	snd_card_free(cxdev->card);
+	return 0;
+}
+
 static void cx25821_audio_fini(void)
 {
-	snd_card_free(snd_cx25821_cards[0]);
+	struct device_driver *drv = driver_find("cx25821", &pci_bus_type);
+	int ret;
+
+	ret = driver_for_each_device(drv, NULL, NULL, cx25821_alsa_exit_callback);
+	if (ret)
+		pr_err("%s failed to find a cx25821 driver.\n", __func__);
+}
+
+static int cx25821_alsa_init_callback(struct device *dev, void *data)
+{
+	struct v4l2_device *v4l2_dev = dev_get_drvdata(dev);
+	struct cx25821_dev *cxdev = get_cx25821(v4l2_dev);
+
+	cx25821_audio_initdev(cxdev);
+	return 0;
 }
 
 /*
@@ -756,29 +749,11 @@ static void cx25821_audio_fini(void)
  */
 static int cx25821_alsa_init(void)
 {
-	struct cx25821_dev *dev = NULL;
-	struct list_head *list;
+	struct device_driver *drv = driver_find("cx25821", &pci_bus_type);
 
-	mutex_lock(&cx25821_devlist_mutex);
-	list_for_each(list, &cx25821_devlist) {
-		dev = list_entry(list, struct cx25821_dev, devlist);
-		cx25821_audio_initdev(dev);
-	}
-	mutex_unlock(&cx25821_devlist_mutex);
-
-	if (dev == NULL)
-		pr_info("ERROR ALSA: no cx25821 cards found\n");
-
-	return 0;
+	return driver_for_each_device(drv, NULL, NULL, cx25821_alsa_init_callback);
 
 }
 
 late_initcall(cx25821_alsa_init);
 module_exit(cx25821_audio_fini);
-
-/* ----------------------------------------------------------- */
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
diff --git a/drivers/media/pci/cx25821/cx25821-audio-upstream.c b/drivers/media/pci/cx25821/cx25821-audio-upstream.c
index 87491ca05ee5..68dbc2dbc982 100644
--- a/drivers/media/pci/cx25821/cx25821-audio-upstream.c
+++ b/drivers/media/pci/cx25821/cx25821-audio-upstream.c
@@ -45,7 +45,7 @@ static int _intr_msk = FLD_AUD_SRC_RISCI1 | FLD_AUD_SRC_OF |
 			FLD_AUD_SRC_SYNC | FLD_AUD_SRC_OPC_ERR;
 
 static int cx25821_sram_channel_setup_upstream_audio(struct cx25821_dev *dev,
-					      struct sram_channel *ch,
+					      const struct sram_channel *ch,
 					      unsigned int bpl, u32 risc)
 {
 	unsigned int i, lines;
@@ -106,7 +106,7 @@ static __le32 *cx25821_risc_field_upstream_audio(struct cx25821_dev *dev,
 						 int fifo_enable)
 {
 	unsigned int line;
-	struct sram_channel *sram_ch =
+	const struct sram_channel *sram_ch =
 		dev->channels[dev->_audio_upstream_channel].sram_channels;
 	int offset = 0;
 
@@ -215,7 +215,7 @@ static void cx25821_free_memory_audio(struct cx25821_dev *dev)
 
 void cx25821_stop_upstream_audio(struct cx25821_dev *dev)
 {
-	struct sram_channel *sram_ch =
+	const struct sram_channel *sram_ch =
 		dev->channels[AUDIO_UPSTREAM_SRAM_CHANNEL_B].sram_channels;
 	u32 tmp = 0;
 
@@ -257,81 +257,48 @@ void cx25821_free_mem_upstream_audio(struct cx25821_dev *dev)
 }
 
 static int cx25821_get_audio_data(struct cx25821_dev *dev,
-			   struct sram_channel *sram_ch)
+			   const struct sram_channel *sram_ch)
 {
-	struct file *myfile;
+	struct file *file;
 	int frame_index_temp = dev->_audioframe_index;
 	int i = 0;
-	int line_size = AUDIO_LINE_SIZE;
 	int frame_size = AUDIO_DATA_BUF_SZ;
 	int frame_offset = frame_size * frame_index_temp;
-	ssize_t vfs_read_retval = 0;
-	char mybuf[line_size];
+	char mybuf[AUDIO_LINE_SIZE];
 	loff_t file_offset = dev->_audioframe_count * frame_size;
-	loff_t pos;
-	mm_segment_t old_fs;
+	char *p = NULL;
 
 	if (dev->_audiofile_status == END_OF_FILE)
 		return 0;
 
-	myfile = filp_open(dev->_audiofilename, O_RDONLY | O_LARGEFILE, 0);
+	file = filp_open(dev->_audiofilename, O_RDONLY | O_LARGEFILE, 0);
+	if (IS_ERR(file)) {
+		pr_err("%s(): ERROR opening file(%s) with errno = %ld!\n",
+		       __func__, dev->_audiofilename, -PTR_ERR(file));
+		return PTR_ERR(file);
+	}
 
-	if (IS_ERR(myfile)) {
-		const int open_errno = -PTR_ERR(myfile);
-		pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
-		       __func__, dev->_audiofilename, open_errno);
-		return PTR_ERR(myfile);
-	} else {
-		if (!(myfile->f_op)) {
-			pr_err("%s(): File has no file operations registered!\n",
-				__func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
+	if (dev->_audiodata_buf_virt_addr)
+		p = (char *)dev->_audiodata_buf_virt_addr + frame_offset;
 
-		if (!myfile->f_op->read) {
-			pr_err("%s(): File has no READ operations registered!\n",
+	for (i = 0; i < dev->_audio_lines_count; i++) {
+		int n = kernel_read(file, file_offset, mybuf, AUDIO_LINE_SIZE);
+		if (n < AUDIO_LINE_SIZE) {
+			pr_info("Done: exit %s() since no more bytes to read from Audio file\n",
 				__func__);
-			filp_close(myfile, NULL);
-			return -EIO;
+			dev->_audiofile_status = END_OF_FILE;
+			fput(file);
+			return 0;
 		}
-
-		pos = myfile->f_pos;
-		old_fs = get_fs();
-		set_fs(KERNEL_DS);
-
-		for (i = 0; i < dev->_audio_lines_count; i++) {
-			pos = file_offset;
-
-			vfs_read_retval = vfs_read(myfile, mybuf, line_size,
-									&pos);
-
-			if (vfs_read_retval > 0 && vfs_read_retval == line_size
-			    && dev->_audiodata_buf_virt_addr != NULL) {
-				memcpy((void *)(dev->_audiodata_buf_virt_addr +
-						frame_offset / 4), mybuf,
-					vfs_read_retval);
-			}
-
-			file_offset += vfs_read_retval;
-			frame_offset += vfs_read_retval;
-
-			if (vfs_read_retval < line_size) {
-				pr_info("Done: exit %s() since no more bytes to read from Audio file\n",
-					__func__);
-				break;
-			}
+		dev->_audiofile_status = IN_PROGRESS;
+		if (p) {
+			memcpy(p, mybuf, n);
+			p += n;
 		}
-
-		if (i > 0)
-			dev->_audioframe_count++;
-
-		dev->_audiofile_status = (vfs_read_retval == line_size) ?
-						IN_PROGRESS : END_OF_FILE;
-
-		set_fs(old_fs);
-		filp_close(myfile, NULL);
+		file_offset += n;
 	}
+	dev->_audioframe_count++;
+	fput(file);
 
 	return 0;
 }
@@ -352,88 +319,48 @@ static void cx25821_audioups_handler(struct work_struct *work)
 }
 
 static int cx25821_openfile_audio(struct cx25821_dev *dev,
-			   struct sram_channel *sram_ch)
+			   const struct sram_channel *sram_ch)
 {
-	struct file *myfile;
-	int i = 0, j = 0;
-	int line_size = AUDIO_LINE_SIZE;
-	ssize_t vfs_read_retval = 0;
-	char mybuf[line_size];
-	loff_t pos;
-	loff_t offset = (unsigned long)0;
-	mm_segment_t old_fs;
-
-	myfile = filp_open(dev->_audiofilename, O_RDONLY | O_LARGEFILE, 0);
-
-	if (IS_ERR(myfile)) {
-		const int open_errno = -PTR_ERR(myfile);
-		pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
-			__func__, dev->_audiofilename, open_errno);
-		return PTR_ERR(myfile);
-	} else {
-		if (!(myfile->f_op)) {
-			pr_err("%s(): File has no file operations registered!\n",
-				__func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		if (!myfile->f_op->read) {
-			pr_err("%s(): File has no READ operations registered!\n",
-				__func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		pos = myfile->f_pos;
-		old_fs = get_fs();
-		set_fs(KERNEL_DS);
-
-		for (j = 0; j < NUM_AUDIO_FRAMES; j++) {
-			for (i = 0; i < dev->_audio_lines_count; i++) {
-				pos = offset;
-
-				vfs_read_retval = vfs_read(myfile, mybuf,
-						line_size, &pos);
-
-				if (vfs_read_retval > 0 &&
-				    vfs_read_retval == line_size &&
-				    dev->_audiodata_buf_virt_addr != NULL) {
-					memcpy((void *)(dev->
-							_audiodata_buf_virt_addr
-							+ offset / 4), mybuf,
-					       vfs_read_retval);
-				}
+	char *p = (void *)dev->_audiodata_buf_virt_addr;
+	struct file *file;
+	loff_t offset;
+	int i, j;
+
+	file = filp_open(dev->_audiofilename, O_RDONLY | O_LARGEFILE, 0);
+	if (IS_ERR(file)) {
+		pr_err("%s(): ERROR opening file(%s) with errno = %ld!\n",
+			__func__, dev->_audiofilename, PTR_ERR(file));
+		return PTR_ERR(file);
+	}
 
-				offset += vfs_read_retval;
+	for (j = 0, offset = 0; j < NUM_AUDIO_FRAMES; j++) {
+		for (i = 0; i < dev->_audio_lines_count; i++) {
+			char buf[AUDIO_LINE_SIZE];
+			int n = kernel_read(file, offset, buf,
+						AUDIO_LINE_SIZE);
 
-				if (vfs_read_retval < line_size) {
-					pr_info("Done: exit %s() since no more bytes to read from Audio file\n",
-						__func__);
-					break;
-				}
+			if (n < AUDIO_LINE_SIZE) {
+				pr_info("Done: exit %s() since no more bytes to read from Audio file\n",
+					__func__);
+				dev->_audiofile_status = END_OF_FILE;
+				fput(file);
+				return 0;
 			}
 
-			if (i > 0)
-				dev->_audioframe_count++;
+			if (p)
+				memcpy(p + offset, buf, n);
 
-			if (vfs_read_retval < line_size)
-				break;
+			offset += n;
 		}
-
-		dev->_audiofile_status = (vfs_read_retval == line_size) ?
-						IN_PROGRESS : END_OF_FILE;
-
-		set_fs(old_fs);
-		myfile->f_pos = 0;
-		filp_close(myfile, NULL);
+		dev->_audioframe_count++;
 	}
-
+	dev->_audiofile_status = IN_PROGRESS;
+	fput(file);
 	return 0;
 }
 
 static int cx25821_audio_upstream_buffer_prepare(struct cx25821_dev *dev,
-						 struct sram_channel *sram_ch,
+						 const struct sram_channel *sram_ch,
 						 int bpl)
 {
 	int ret = 0;
@@ -495,7 +422,7 @@ static int cx25821_audio_upstream_irq(struct cx25821_dev *dev, int chan_num,
 {
 	int i = 0;
 	u32 int_msk_tmp;
-	struct sram_channel *channel = dev->channels[chan_num].sram_channels;
+	const struct sram_channel *channel = dev->channels[chan_num].sram_channels;
 	dma_addr_t risc_phys_jump_addr;
 	__le32 *rp;
 
@@ -587,7 +514,7 @@ static irqreturn_t cx25821_upstream_irq_audio(int irq, void *dev_id)
 	struct cx25821_dev *dev = dev_id;
 	u32 audio_status;
 	int handled = 0;
-	struct sram_channel *sram_ch;
+	const struct sram_channel *sram_ch;
 
 	if (!dev)
 		return -1;
@@ -611,7 +538,7 @@ static irqreturn_t cx25821_upstream_irq_audio(int irq, void *dev_id)
 }
 
 static void cx25821_wait_fifo_enable(struct cx25821_dev *dev,
-				     struct sram_channel *sram_ch)
+				     const struct sram_channel *sram_ch)
 {
 	int count = 0;
 	u32 tmp;
@@ -635,7 +562,7 @@ static void cx25821_wait_fifo_enable(struct cx25821_dev *dev,
 }
 
 static int cx25821_start_audio_dma_upstream(struct cx25821_dev *dev,
-					    struct sram_channel *sram_ch)
+					    const struct sram_channel *sram_ch)
 {
 	u32 tmp = 0;
 	int err = 0;
@@ -699,7 +626,7 @@ fail_irq:
 
 int cx25821_audio_upstream_init(struct cx25821_dev *dev, int channel_select)
 {
-	struct sram_channel *sram_ch;
+	const struct sram_channel *sram_ch;
 	int err = 0;
 
 	if (dev->_audio_is_running) {
@@ -728,26 +655,17 @@ int cx25821_audio_upstream_init(struct cx25821_dev *dev, int channel_select)
 	dev->_audio_lines_count = LINES_PER_AUDIO_BUFFER;
 	_line_size = AUDIO_LINE_SIZE;
 
-	if (dev->input_audiofilename) {
+	if ((dev->input_audiofilename) &&
+	    (strcmp(dev->input_audiofilename, "") != 0))
 		dev->_audiofilename = kstrdup(dev->input_audiofilename,
 					      GFP_KERNEL);
-
-		if (!dev->_audiofilename) {
-			err = -ENOMEM;
-			goto error;
-		}
-
-		/* Default if filename is empty string */
-		if (strcmp(dev->input_audiofilename, "") == 0)
-			dev->_audiofilename = "/root/audioGOOD.wav";
-	} else {
+	else
 		dev->_audiofilename = kstrdup(_defaultAudioName,
 					      GFP_KERNEL);
 
-		if (!dev->_audiofilename) {
-			err = -ENOMEM;
-			goto error;
-		}
+	if (!dev->_audiofilename) {
+		err = -ENOMEM;
+		goto error;
 	}
 
 	cx25821_sram_channel_setup_upstream_audio(dev, sram_ch,
diff --git a/drivers/media/pci/cx25821/cx25821-cards.c b/drivers/media/pci/cx25821/cx25821-cards.c
index 99988c988095..3b409feb03d8 100644
--- a/drivers/media/pci/cx25821/cx25821-cards.c
+++ b/drivers/media/pci/cx25821/cx25821-cards.c
@@ -26,11 +26,8 @@
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/pci.h>
-#include <linux/delay.h>
-#include <media/cx25840.h>
 
 #include "cx25821.h"
-#include "tuner-xc2028.h"
 
 /* board config info */
 
@@ -45,28 +42,8 @@ struct cx25821_board cx25821_boards[] = {
 		.name = "CX25821",
 		.portb = CX25821_RAW,
 		.portc = CX25821_264,
-		.input[0].type = CX25821_VMUX_COMPOSITE,
 	},
 
 };
 
 const unsigned int cx25821_bcount = ARRAY_SIZE(cx25821_boards);
-
-struct cx25821_subid cx25821_subids[] = {
-	{
-		.subvendor = 0x14f1,
-		.subdevice = 0x0920,
-		.card = CX25821_BOARD,
-	},
-};
-
-void cx25821_card_setup(struct cx25821_dev *dev)
-{
-	static u8 eeprom[256];
-
-	if (dev->i2c_bus[0].i2c_rc == 0) {
-		dev->i2c_bus[0].i2c_client.addr = 0xa0 >> 1;
-		tveeprom_read(&dev->i2c_bus[0].i2c_client, eeprom,
-				sizeof(eeprom));
-	}
-}
diff --git a/drivers/media/pci/cx25821/cx25821-core.c b/drivers/media/pci/cx25821/cx25821-core.c
index 1884e2cc35e9..b762c5b2ca10 100644
--- a/drivers/media/pci/cx25821/cx25821-core.c
+++ b/drivers/media/pci/cx25821/cx25821-core.c
@@ -41,14 +41,7 @@ static unsigned int card[] = {[0 ... (CX25821_MAXBOARDS - 1)] = UNSET };
 module_param_array(card, int, NULL, 0444);
 MODULE_PARM_DESC(card, "card type");
 
-static unsigned int cx25821_devcount;
-
-DEFINE_MUTEX(cx25821_devlist_mutex);
-EXPORT_SYMBOL(cx25821_devlist_mutex);
-LIST_HEAD(cx25821_devlist);
-EXPORT_SYMBOL(cx25821_devlist);
-
-struct sram_channel cx25821_sram_channels[] = {
+const struct sram_channel cx25821_sram_channels[] = {
 	[SRAM_CH00] = {
 		.i = SRAM_CH00,
 		.name = "VID A",
@@ -317,20 +310,6 @@ struct sram_channel cx25821_sram_channels[] = {
 };
 EXPORT_SYMBOL(cx25821_sram_channels);
 
-struct sram_channel *channel0 = &cx25821_sram_channels[SRAM_CH00];
-struct sram_channel *channel1 = &cx25821_sram_channels[SRAM_CH01];
-struct sram_channel *channel2 = &cx25821_sram_channels[SRAM_CH02];
-struct sram_channel *channel3 = &cx25821_sram_channels[SRAM_CH03];
-struct sram_channel *channel4 = &cx25821_sram_channels[SRAM_CH04];
-struct sram_channel *channel5 = &cx25821_sram_channels[SRAM_CH05];
-struct sram_channel *channel6 = &cx25821_sram_channels[SRAM_CH06];
-struct sram_channel *channel7 = &cx25821_sram_channels[SRAM_CH07];
-struct sram_channel *channel9 = &cx25821_sram_channels[SRAM_CH09];
-struct sram_channel *channel10 = &cx25821_sram_channels[SRAM_CH10];
-struct sram_channel *channel11 = &cx25821_sram_channels[SRAM_CH11];
-
-struct cx25821_dmaqueue mpegq;
-
 static int cx25821_risc_decode(u32 risc)
 {
 	static const char * const instr[16] = {
@@ -457,7 +436,7 @@ static void cx25821_registers_init(struct cx25821_dev *dev)
 }
 
 int cx25821_sram_channel_setup(struct cx25821_dev *dev,
-			       struct sram_channel *ch,
+			       const struct sram_channel *ch,
 			       unsigned int bpl, u32 risc)
 {
 	unsigned int i, lines;
@@ -523,10 +502,9 @@ int cx25821_sram_channel_setup(struct cx25821_dev *dev,
 
 	return 0;
 }
-EXPORT_SYMBOL(cx25821_sram_channel_setup);
 
 int cx25821_sram_channel_setup_audio(struct cx25821_dev *dev,
-				     struct sram_channel *ch,
+				     const struct sram_channel *ch,
 				     unsigned int bpl, u32 risc)
 {
 	unsigned int i, lines;
@@ -592,7 +570,7 @@ int cx25821_sram_channel_setup_audio(struct cx25821_dev *dev,
 }
 EXPORT_SYMBOL(cx25821_sram_channel_setup_audio);
 
-void cx25821_sram_channel_dump(struct cx25821_dev *dev, struct sram_channel *ch)
+void cx25821_sram_channel_dump(struct cx25821_dev *dev, const struct sram_channel *ch)
 {
 	static char *name[] = {
 		"init risc lo",
@@ -652,10 +630,9 @@ void cx25821_sram_channel_dump(struct cx25821_dev *dev, struct sram_channel *ch)
 	pr_warn("        :   cnt2_reg: 0x%08x\n",
 		cx_read(ch->cnt2_reg));
 }
-EXPORT_SYMBOL(cx25821_sram_channel_dump);
 
 void cx25821_sram_channel_dump_audio(struct cx25821_dev *dev,
-				     struct sram_channel *ch)
+				     const struct sram_channel *ch)
 {
 	static const char * const name[] = {
 		"init risc lo",
@@ -798,12 +775,12 @@ void cx25821_set_pixel_format(struct cx25821_dev *dev, int channel_select,
 	if (channel_select <= 7 && channel_select >= 0) {
 		cx_write(dev->channels[channel_select].sram_channels->pix_frmt,
 				format);
-		dev->channels[channel_select].pixel_formats = format;
 	}
+	dev->channels[channel_select].pixel_formats = format;
 }
 
 static void cx25821_set_vip_mode(struct cx25821_dev *dev,
-				 struct sram_channel *ch)
+				 const struct sram_channel *ch)
 {
 	cx_write(ch->pix_frmt, PIXEL_FRMT_422);
 	cx_write(ch->vip_ctl, PIXEL_ENGINE_VIP1);
@@ -837,12 +814,13 @@ static void cx25821_initialize(struct cx25821_dev *dev)
 		cx25821_sram_channel_setup(dev, dev->channels[i].sram_channels,
 						1440, 0);
 		dev->channels[i].pixel_formats = PIXEL_FRMT_422;
-		dev->channels[i].use_cif_resolution = FALSE;
+		dev->channels[i].use_cif_resolution = 0;
 	}
 
 	/* Probably only affect Downstream */
 	for (i = VID_UPSTREAM_SRAM_CHANNEL_I;
 		i <= VID_UPSTREAM_SRAM_CHANNEL_J; i++) {
+		dev->channels[i].pixel_formats = PIXEL_FRMT_422;
 		cx25821_set_vip_mode(dev, dev->channels[i].sram_channels);
 	}
 
@@ -868,8 +846,7 @@ static void cx25821_dev_checkrevision(struct cx25821_dev *dev)
 {
 	dev->hwrevision = cx_read(RDR_CFG2) & 0xff;
 
-	pr_info("%s(): Hardware revision = 0x%02x\n",
-		__func__, dev->hwrevision);
+	pr_info("Hardware revision = 0x%02x\n", dev->hwrevision);
 }
 
 static void cx25821_iounmap(struct cx25821_dev *dev)
@@ -879,7 +856,6 @@ static void cx25821_iounmap(struct cx25821_dev *dev)
 
 	/* Releasing IO memory */
 	if (dev->lmmio != NULL) {
-		CX25821_INFO("Releasing lmmio.\n");
 		iounmap(dev->lmmio);
 		dev->lmmio = NULL;
 	}
@@ -887,23 +863,14 @@ static void cx25821_iounmap(struct cx25821_dev *dev)
 
 static int cx25821_dev_setup(struct cx25821_dev *dev)
 {
+	static unsigned int cx25821_devcount;
 	int i;
 
-	pr_info("\n***********************************\n");
-	pr_info("cx25821 set up\n");
-	pr_info("***********************************\n\n");
-
 	mutex_init(&dev->lock);
 
-	atomic_inc(&dev->refcount);
-
 	dev->nr = ++cx25821_devcount;
 	sprintf(dev->name, "cx25821[%d]", dev->nr);
 
-	mutex_lock(&cx25821_devlist_mutex);
-	list_add_tail(&dev->devlist, &cx25821_devlist);
-	mutex_unlock(&cx25821_devlist_mutex);
-
 	if (dev->pci->device != 0x8210) {
 		pr_info("%s(): Exiting. Incorrect Hardware device = 0x%02x\n",
 			__func__, dev->pci->device);
@@ -914,8 +881,11 @@ static int cx25821_dev_setup(struct cx25821_dev *dev)
 
 	/* Apply a sensible clock frequency for the PCIe bridge */
 	dev->clk_freq = 28000000;
-	for (i = 0; i < MAX_VID_CHANNEL_NUM; i++)
+	for (i = 0; i < MAX_VID_CHANNEL_NUM; i++) {
+		dev->channels[i].dev = dev;
+		dev->channels[i].id = i;
 		dev->channels[i].sram_channels = &cx25821_sram_channels[i];
+	}
 
 	if (dev->nr > 1)
 		CX25821_INFO("dev->nr > 1!");
@@ -978,63 +948,15 @@ static int cx25821_dev_setup(struct cx25821_dev *dev)
 /*  cx25821_i2c_register(&dev->i2c_bus[1]);
  *  cx25821_i2c_register(&dev->i2c_bus[2]); */
 
-	CX25821_INFO("i2c register! bus->i2c_rc = %d\n",
-			dev->i2c_bus[0].i2c_rc);
-
-	cx25821_card_setup(dev);
-
 	if (medusa_video_init(dev) < 0)
 		CX25821_ERR("%s(): Failed to initialize medusa!\n", __func__);
 
 	cx25821_video_register(dev);
 
-	/* register IOCTL device */
-	dev->ioctl_dev = cx25821_vdev_init(dev, dev->pci,
-			&cx25821_videoioctl_template, "video");
-
-	if (video_register_device
-	    (dev->ioctl_dev, VFL_TYPE_GRABBER, VIDEO_IOCTL_CH) < 0) {
-		cx25821_videoioctl_unregister(dev);
-		pr_err("%s(): Failed to register video adapter for IOCTL, so unregistering videoioctl device\n",
-		       __func__);
-	}
-
 	cx25821_dev_checkrevision(dev);
-	CX25821_INFO("setup done!\n");
-
 	return 0;
 }
 
-void cx25821_start_upstream_video_ch1(struct cx25821_dev *dev,
-				      struct upstream_user_struct *up_data)
-{
-	dev->_isNTSC = !strcmp(dev->vid_stdname, "NTSC") ? 1 : 0;
-
-	dev->tvnorm = !dev->_isNTSC ? V4L2_STD_PAL_BG : V4L2_STD_NTSC_M;
-	medusa_set_videostandard(dev);
-
-	cx25821_vidupstream_init_ch1(dev, dev->channel_select,
-				     dev->pixel_format);
-}
-
-void cx25821_start_upstream_video_ch2(struct cx25821_dev *dev,
-				      struct upstream_user_struct *up_data)
-{
-	dev->_isNTSC_ch2 = !strcmp(dev->vid_stdname_ch2, "NTSC") ? 1 : 0;
-
-	dev->tvnorm = !dev->_isNTSC_ch2 ? V4L2_STD_PAL_BG : V4L2_STD_NTSC_M;
-	medusa_set_videostandard(dev);
-
-	cx25821_vidupstream_init_ch2(dev, dev->channel_select_ch2,
-				     dev->pixel_format_ch2);
-}
-
-void cx25821_start_upstream_audio(struct cx25821_dev *dev,
-				  struct upstream_user_struct *up_data)
-{
-	cx25821_audio_upstream_init(dev, AUDIO_UPSTREAM_SRAM_CHANNEL_B);
-}
-
 void cx25821_dev_unregister(struct cx25821_dev *dev)
 {
 	int i;
@@ -1042,25 +964,16 @@ void cx25821_dev_unregister(struct cx25821_dev *dev)
 	if (!dev->base_io_addr)
 		return;
 
-	cx25821_free_mem_upstream_ch1(dev);
-	cx25821_free_mem_upstream_ch2(dev);
-	cx25821_free_mem_upstream_audio(dev);
-
 	release_mem_region(dev->base_io_addr, pci_resource_len(dev->pci, 0));
 
-	if (!atomic_dec_and_test(&dev->refcount))
-		return;
-
-	for (i = 0; i < VID_CHANNEL_NUM; i++)
-		cx25821_video_unregister(dev, i);
-
-	for (i = VID_UPSTREAM_SRAM_CHANNEL_I;
-	     i <= AUDIO_UPSTREAM_SRAM_CHANNEL_B; i++) {
+	for (i = 0; i < MAX_VID_CHANNEL_NUM - 1; i++) {
+		if (i == SRAM_CH08) /* audio channel */
+			continue;
+		if (i == SRAM_CH09 || i == SRAM_CH10)
+			cx25821_free_mem_upstream(&dev->channels[i]);
 		cx25821_video_unregister(dev, i);
 	}
 
-	cx25821_videoioctl_unregister(dev);
-
 	cx25821_i2c_unregister(&dev->i2c_bus[0]);
 	cx25821_iounmap(dev);
 }
@@ -1385,8 +1298,6 @@ static int cx25821_initdev(struct pci_dev *pci_dev,
 		goto fail_unregister_device;
 	}
 
-	pr_info("Athena pci enable !\n");
-
 	err = cx25821_dev_setup(dev);
 	if (err) {
 		if (err == -EBUSY)
@@ -1445,10 +1356,6 @@ static void cx25821_finidev(struct pci_dev *pci_dev)
 	if (pci_dev->irq)
 		free_irq(pci_dev->irq, dev);
 
-	mutex_lock(&cx25821_devlist_mutex);
-	list_del(&dev->devlist);
-	mutex_unlock(&cx25821_devlist_mutex);
-
 	cx25821_dev_unregister(dev);
 	v4l2_device_unregister(v4l2_dev);
 	kfree(dev);
diff --git a/drivers/media/pci/cx25821/cx25821-gpio.c b/drivers/media/pci/cx25821/cx25821-gpio.c
index 29e43b03c85e..95e8ddf62947 100644
--- a/drivers/media/pci/cx25821/cx25821-gpio.c
+++ b/drivers/media/pci/cx25821/cx25821-gpio.c
@@ -20,6 +20,7 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include <linux/module.h>
 #include "cx25821.h"
 
 /********************* GPIO stuffs *********************/
diff --git a/drivers/media/pci/cx25821/cx25821-i2c.c b/drivers/media/pci/cx25821/cx25821-i2c.c
index a8dc945bbe17..dca37c7dba73 100644
--- a/drivers/media/pci/cx25821/cx25821-i2c.c
+++ b/drivers/media/pci/cx25821/cx25821-i2c.c
@@ -23,8 +23,9 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#include "cx25821.h"
+#include <linux/module.h>
 #include <linux/i2c.h>
+#include "cx25821.h"
 
 static unsigned int i2c_debug;
 module_param(i2c_debug, int, 0644);
diff --git a/drivers/media/pci/cx25821/cx25821-medusa-video.c b/drivers/media/pci/cx25821/cx25821-medusa-video.c
index 6a92e5c70c2a..22fa04415ccc 100644
--- a/drivers/media/pci/cx25821/cx25821-medusa-video.c
+++ b/drivers/media/pci/cx25821/cx25821-medusa-video.c
@@ -94,8 +94,6 @@ static int medusa_initialize_ntsc(struct cx25821_dev *dev)
 	u32 value = 0;
 	u32 tmp = 0;
 
-	mutex_lock(&dev->lock);
-
 	for (i = 0; i < MAX_DECODERS; i++) {
 		/* set video format NTSC-M */
 		value = cx25821_i2c_read(&dev->i2c_bus[0],
@@ -222,8 +220,6 @@ static int medusa_initialize_ntsc(struct cx25821_dev *dev)
 	value |= 0x00080200;
 	ret_val = cx25821_i2c_write(&dev->i2c_bus[0], BYP_AB_CTRL, value);
 
-	mutex_unlock(&dev->lock);
-
 	return ret_val;
 }
 
@@ -265,8 +261,6 @@ static int medusa_initialize_pal(struct cx25821_dev *dev)
 	u32 value = 0;
 	u32 tmp = 0;
 
-	mutex_lock(&dev->lock);
-
 	for (i = 0; i < MAX_DECODERS; i++) {
 		/* set video format PAL-BDGHI */
 		value = cx25821_i2c_read(&dev->i2c_bus[0],
@@ -397,14 +391,12 @@ static int medusa_initialize_pal(struct cx25821_dev *dev)
 	value &= 0xFFF7FDFF;
 	ret_val = cx25821_i2c_write(&dev->i2c_bus[0], BYP_AB_CTRL, value);
 
-	mutex_unlock(&dev->lock);
-
 	return ret_val;
 }
 
 int medusa_set_videostandard(struct cx25821_dev *dev)
 {
-	int status = STATUS_SUCCESS;
+	int status = 0;
 	u32 value = 0, tmp = 0;
 
 	if (dev->tvnorm & V4L2_STD_PAL_BG || dev->tvnorm & V4L2_STD_PAL_DK)
@@ -434,8 +426,6 @@ void medusa_set_resolution(struct cx25821_dev *dev, int width,
 	u32 vscale = 0x0;
 	const int MAX_WIDTH = 720;
 
-	mutex_lock(&dev->lock);
-
 	/* validate the width */
 	if (width > MAX_WIDTH) {
 		pr_info("%s(): width %d > MAX_WIDTH %d ! resetting to MAX_WIDTH\n",
@@ -485,8 +475,6 @@ void medusa_set_resolution(struct cx25821_dev *dev, int width,
 		cx25821_i2c_write(&dev->i2c_bus[0],
 				VSCALE_CTRL + (0x200 * decoder), vscale);
 	}
-
-	mutex_unlock(&dev->lock);
 }
 
 static void medusa_set_decoderduration(struct cx25821_dev *dev, int decoder,
@@ -496,11 +484,8 @@ static void medusa_set_decoderduration(struct cx25821_dev *dev, int decoder,
 	u32 tmp = 0;
 	u32 disp_cnt_reg = DISP_AB_CNT;
 
-	mutex_lock(&dev->lock);
-
 	/* no support */
 	if (decoder < VDEC_A || decoder > VDEC_H) {
-		mutex_unlock(&dev->lock);
 		return;
 	}
 
@@ -535,8 +520,6 @@ static void medusa_set_decoderduration(struct cx25821_dev *dev, int decoder,
 	}
 
 	cx25821_i2c_write(&dev->i2c_bus[0], disp_cnt_reg, fld_cnt);
-
-	mutex_unlock(&dev->lock);
 }
 
 /* Map to Medusa register setting */
@@ -587,10 +570,8 @@ int medusa_set_brightness(struct cx25821_dev *dev, int brightness, int decoder)
 	int value = 0;
 	u32 val = 0, tmp = 0;
 
-	mutex_lock(&dev->lock);
 	if ((brightness > VIDEO_PROCAMP_MAX) ||
 	    (brightness < VIDEO_PROCAMP_MIN)) {
-		mutex_unlock(&dev->lock);
 		return -1;
 	}
 	ret_val = mapM(VIDEO_PROCAMP_MIN, VIDEO_PROCAMP_MAX, brightness,
@@ -601,7 +582,6 @@ int medusa_set_brightness(struct cx25821_dev *dev, int brightness, int decoder)
 	val &= 0xFFFFFF00;
 	ret_val |= cx25821_i2c_write(&dev->i2c_bus[0],
 			VDEC_A_BRITE_CTRL + (0x200 * decoder), val | value);
-	mutex_unlock(&dev->lock);
 	return ret_val;
 }
 
@@ -611,10 +591,7 @@ int medusa_set_contrast(struct cx25821_dev *dev, int contrast, int decoder)
 	int value = 0;
 	u32 val = 0, tmp = 0;
 
-	mutex_lock(&dev->lock);
-
 	if ((contrast > VIDEO_PROCAMP_MAX) || (contrast < VIDEO_PROCAMP_MIN)) {
-		mutex_unlock(&dev->lock);
 		return -1;
 	}
 
@@ -626,7 +603,6 @@ int medusa_set_contrast(struct cx25821_dev *dev, int contrast, int decoder)
 	ret_val |= cx25821_i2c_write(&dev->i2c_bus[0],
 			VDEC_A_CNTRST_CTRL + (0x200 * decoder), val | value);
 
-	mutex_unlock(&dev->lock);
 	return ret_val;
 }
 
@@ -636,10 +612,7 @@ int medusa_set_hue(struct cx25821_dev *dev, int hue, int decoder)
 	int value = 0;
 	u32 val = 0, tmp = 0;
 
-	mutex_lock(&dev->lock);
-
 	if ((hue > VIDEO_PROCAMP_MAX) || (hue < VIDEO_PROCAMP_MIN)) {
-		mutex_unlock(&dev->lock);
 		return -1;
 	}
 
@@ -654,7 +627,6 @@ int medusa_set_hue(struct cx25821_dev *dev, int hue, int decoder)
 	ret_val |= cx25821_i2c_write(&dev->i2c_bus[0],
 			VDEC_A_HUE_CTRL + (0x200 * decoder), val | value);
 
-	mutex_unlock(&dev->lock);
 	return ret_val;
 }
 
@@ -664,11 +636,8 @@ int medusa_set_saturation(struct cx25821_dev *dev, int saturation, int decoder)
 	int value = 0;
 	u32 val = 0, tmp = 0;
 
-	mutex_lock(&dev->lock);
-
 	if ((saturation > VIDEO_PROCAMP_MAX) ||
 	    (saturation < VIDEO_PROCAMP_MIN)) {
-		mutex_unlock(&dev->lock);
 		return -1;
 	}
 
@@ -687,7 +656,6 @@ int medusa_set_saturation(struct cx25821_dev *dev, int saturation, int decoder)
 	ret_val |= cx25821_i2c_write(&dev->i2c_bus[0],
 			VDEC_A_VSAT_CTRL + (0x200 * decoder), val | value);
 
-	mutex_unlock(&dev->lock);
 	return ret_val;
 }
 
@@ -699,8 +667,6 @@ int medusa_video_init(struct cx25821_dev *dev)
 	int ret_val = 0;
 	int i = 0;
 
-	mutex_lock(&dev->lock);
-
 	_num_decoders = dev->_max_num_decoders;
 
 	/* disable Auto source selection on all video decoders */
@@ -719,13 +685,9 @@ int medusa_video_init(struct cx25821_dev *dev)
 	if (ret_val < 0)
 		goto error;
 
-	mutex_unlock(&dev->lock);
-
 	for (i = 0; i < _num_decoders; i++)
 		medusa_set_decoderduration(dev, i, _display_field_cnt[i]);
 
-	mutex_lock(&dev->lock);
-
 	/* Select monitor as DENC A input, power up the DAC */
 	value = cx25821_i2c_read(&dev->i2c_bus[0], DENC_AB_CTRL, &tmp);
 	value &= 0xFF70FF70;
@@ -774,14 +736,8 @@ int medusa_video_init(struct cx25821_dev *dev)
 	if (ret_val < 0)
 		goto error;
 
-
-	mutex_unlock(&dev->lock);
-
 	ret_val = medusa_set_videostandard(dev);
 
-	return ret_val;
-
 error:
-	mutex_unlock(&dev->lock);
 	return ret_val;
 }
diff --git a/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c b/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c
deleted file mode 100644
index cf2723c7197f..000000000000
--- a/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c
+++ /dev/null
@@ -1,800 +0,0 @@
-/*
- *  Driver for the Conexant CX25821 PCIe bridge
- *
- *  Copyright (C) 2009 Conexant Systems Inc.
- *  Authors  <hiep.huynh@conexant.com>, <shu.lin@conexant.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., 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include "cx25821-video.h"
-#include "cx25821-video-upstream-ch2.h"
-
-#include <linux/fs.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/module.h>
-#include <linux/syscalls.h>
-#include <linux/file.h>
-#include <linux/fcntl.h>
-#include <linux/slab.h>
-#include <linux/uaccess.h>
-
-MODULE_DESCRIPTION("v4l2 driver module for cx25821 based TV cards");
-MODULE_AUTHOR("Hiep Huynh <hiep.huynh@conexant.com>");
-MODULE_LICENSE("GPL");
-
-static int _intr_msk = FLD_VID_SRC_RISC1 | FLD_VID_SRC_UF | FLD_VID_SRC_SYNC |
-			FLD_VID_SRC_OPC_ERR;
-
-static __le32 *cx25821_update_riscprogram_ch2(struct cx25821_dev *dev,
-					      __le32 *rp, unsigned int offset,
-					      unsigned int bpl, u32 sync_line,
-					      unsigned int lines,
-					      int fifo_enable, int field_type)
-{
-	unsigned int line, i;
-	int dist_betwn_starts = bpl * 2;
-
-	*(rp++) = cpu_to_le32(RISC_RESYNC | sync_line);
-
-	if (USE_RISC_NOOP_VIDEO) {
-		for (i = 0; i < NUM_NO_OPS; i++)
-			*(rp++) = cpu_to_le32(RISC_NOOP);
-	}
-
-	/* scan lines */
-	for (line = 0; line < lines; line++) {
-		*(rp++) = cpu_to_le32(RISC_READ | RISC_SOL | RISC_EOL | bpl);
-		*(rp++) = cpu_to_le32(dev->_data_buf_phys_addr_ch2 + offset);
-		*(rp++) = cpu_to_le32(0);	/* bits 63-32 */
-
-		if ((lines <= NTSC_FIELD_HEIGHT) ||
-		    (line < (NTSC_FIELD_HEIGHT - 1)) || !(dev->_isNTSC_ch2)) {
-			offset += dist_betwn_starts;
-		}
-	}
-
-	return rp;
-}
-
-static __le32 *cx25821_risc_field_upstream_ch2(struct cx25821_dev *dev,
-					       __le32 *rp,
-					       dma_addr_t databuf_phys_addr,
-					       unsigned int offset,
-					       u32 sync_line, unsigned int bpl,
-					       unsigned int lines,
-					       int fifo_enable, int field_type)
-{
-	unsigned int line, i;
-	struct sram_channel *sram_ch =
-		dev->channels[dev->_channel2_upstream_select].sram_channels;
-	int dist_betwn_starts = bpl * 2;
-
-	/* sync instruction */
-	if (sync_line != NO_SYNC_LINE)
-		*(rp++) = cpu_to_le32(RISC_RESYNC | sync_line);
-
-	if (USE_RISC_NOOP_VIDEO) {
-		for (i = 0; i < NUM_NO_OPS; i++)
-			*(rp++) = cpu_to_le32(RISC_NOOP);
-	}
-
-	/* scan lines */
-	for (line = 0; line < lines; line++) {
-		*(rp++) = cpu_to_le32(RISC_READ | RISC_SOL | RISC_EOL | bpl);
-		*(rp++) = cpu_to_le32(databuf_phys_addr + offset);
-		*(rp++) = cpu_to_le32(0);	/* bits 63-32 */
-
-		if ((lines <= NTSC_FIELD_HEIGHT) ||
-		    (line < (NTSC_FIELD_HEIGHT - 1)) || !(dev->_isNTSC_ch2)) {
-			offset += dist_betwn_starts;
-		}
-
-	       /*
-		 check if we need to enable the FIFO after the first 4 lines
-		  For the upstream video channel, the risc engine will enable
-		  the FIFO.
-	       */
-		if (fifo_enable && line == 3) {
-			*(rp++) = RISC_WRITECR;
-			*(rp++) = sram_ch->dma_ctl;
-			*(rp++) = FLD_VID_FIFO_EN;
-			*(rp++) = 0x00000001;
-		}
-	}
-
-	return rp;
-}
-
-static int cx25821_risc_buffer_upstream_ch2(struct cx25821_dev *dev,
-					    struct pci_dev *pci,
-					    unsigned int top_offset,
-					    unsigned int bpl,
-					    unsigned int lines)
-{
-	__le32 *rp;
-	int fifo_enable = 0;
-	int singlefield_lines = lines >> 1; /*get line count for single field */
-	int odd_num_lines = singlefield_lines;
-	int frame = 0;
-	int frame_size = 0;
-	int databuf_offset = 0;
-	int risc_program_size = 0;
-	int risc_flag = RISC_CNT_RESET;
-	unsigned int bottom_offset = bpl;
-	dma_addr_t risc_phys_jump_addr;
-
-	if (dev->_isNTSC_ch2) {
-		odd_num_lines = singlefield_lines + 1;
-		risc_program_size = FRAME1_VID_PROG_SIZE;
-		if (bpl == Y411_LINE_SZ)
-			frame_size = FRAME_SIZE_NTSC_Y411;
-		else
-			frame_size = FRAME_SIZE_NTSC_Y422;
-	} else {
-		risc_program_size = PAL_VID_PROG_SIZE;
-		if (bpl == Y411_LINE_SZ)
-			frame_size = FRAME_SIZE_PAL_Y411;
-		else
-			frame_size = FRAME_SIZE_PAL_Y422;
-	}
-
-	/* Virtual address of Risc buffer program */
-	rp = dev->_dma_virt_addr_ch2;
-
-	for (frame = 0; frame < NUM_FRAMES; frame++) {
-		databuf_offset = frame_size * frame;
-
-		if (UNSET != top_offset) {
-			fifo_enable = (frame == 0) ? FIFO_ENABLE : FIFO_DISABLE;
-			rp = cx25821_risc_field_upstream_ch2(dev, rp,
-				dev->_data_buf_phys_addr_ch2 + databuf_offset,
-				top_offset, 0, bpl, odd_num_lines, fifo_enable,
-				ODD_FIELD);
-		}
-
-		fifo_enable = FIFO_DISABLE;
-
-		/* Even field */
-		rp = cx25821_risc_field_upstream_ch2(dev, rp,
-				dev->_data_buf_phys_addr_ch2 + databuf_offset,
-				bottom_offset, 0x200, bpl, singlefield_lines,
-				fifo_enable, EVEN_FIELD);
-
-		if (frame == 0) {
-			risc_flag = RISC_CNT_RESET;
-			risc_phys_jump_addr = dev->_dma_phys_start_addr_ch2 +
-					risc_program_size;
-		} else {
-			risc_flag = RISC_CNT_INC;
-			risc_phys_jump_addr = dev->_dma_phys_start_addr_ch2;
-		}
-
-	       /*
-		* Loop to 2ndFrameRISC or to Start of
-		* Risc program & generate IRQ
-		*/
-		*(rp++) = cpu_to_le32(RISC_JUMP | RISC_IRQ1 | risc_flag);
-		*(rp++) = cpu_to_le32(risc_phys_jump_addr);
-		*(rp++) = cpu_to_le32(0);
-	}
-
-	return 0;
-}
-
-void cx25821_stop_upstream_video_ch2(struct cx25821_dev *dev)
-{
-	struct sram_channel *sram_ch =
-		dev->channels[VID_UPSTREAM_SRAM_CHANNEL_J].sram_channels;
-	u32 tmp = 0;
-
-	if (!dev->_is_running_ch2) {
-		pr_info("No video file is currently running so return!\n");
-		return;
-	}
-	/* Disable RISC interrupts */
-	tmp = cx_read(sram_ch->int_msk);
-	cx_write(sram_ch->int_msk, tmp & ~_intr_msk);
-
-	/* Turn OFF risc and fifo */
-	tmp = cx_read(sram_ch->dma_ctl);
-	cx_write(sram_ch->dma_ctl, tmp & ~(FLD_VID_FIFO_EN | FLD_VID_RISC_EN));
-
-	/* Clear data buffer memory */
-	if (dev->_data_buf_virt_addr_ch2)
-		memset(dev->_data_buf_virt_addr_ch2, 0,
-		       dev->_data_buf_size_ch2);
-
-	dev->_is_running_ch2 = 0;
-	dev->_is_first_frame_ch2 = 0;
-	dev->_frame_count_ch2 = 0;
-	dev->_file_status_ch2 = END_OF_FILE;
-
-	kfree(dev->_irq_queues_ch2);
-	dev->_irq_queues_ch2 = NULL;
-
-	kfree(dev->_filename_ch2);
-
-	tmp = cx_read(VID_CH_MODE_SEL);
-	cx_write(VID_CH_MODE_SEL, tmp & 0xFFFFFE00);
-}
-
-void cx25821_free_mem_upstream_ch2(struct cx25821_dev *dev)
-{
-	if (dev->_is_running_ch2)
-		cx25821_stop_upstream_video_ch2(dev);
-
-	if (dev->_dma_virt_addr_ch2) {
-		pci_free_consistent(dev->pci, dev->_risc_size_ch2,
-				    dev->_dma_virt_addr_ch2,
-				    dev->_dma_phys_addr_ch2);
-		dev->_dma_virt_addr_ch2 = NULL;
-	}
-
-	if (dev->_data_buf_virt_addr_ch2) {
-		pci_free_consistent(dev->pci, dev->_data_buf_size_ch2,
-				    dev->_data_buf_virt_addr_ch2,
-				    dev->_data_buf_phys_addr_ch2);
-		dev->_data_buf_virt_addr_ch2 = NULL;
-	}
-}
-
-static int cx25821_get_frame_ch2(struct cx25821_dev *dev,
-				 struct sram_channel *sram_ch)
-{
-	struct file *myfile;
-	int frame_index_temp = dev->_frame_index_ch2;
-	int i = 0;
-	int line_size = (dev->_pixel_format_ch2 == PIXEL_FRMT_411) ?
-		Y411_LINE_SZ : Y422_LINE_SZ;
-	int frame_size = 0;
-	int frame_offset = 0;
-	ssize_t vfs_read_retval = 0;
-	char mybuf[line_size];
-	loff_t file_offset;
-	loff_t pos;
-	mm_segment_t old_fs;
-
-	if (dev->_file_status_ch2 == END_OF_FILE)
-		return 0;
-
-	if (dev->_isNTSC_ch2) {
-		frame_size = (line_size == Y411_LINE_SZ) ?
-			FRAME_SIZE_NTSC_Y411 : FRAME_SIZE_NTSC_Y422;
-	} else {
-		frame_size = (line_size == Y411_LINE_SZ) ?
-			FRAME_SIZE_PAL_Y411 : FRAME_SIZE_PAL_Y422;
-	}
-
-	frame_offset = (frame_index_temp > 0) ? frame_size : 0;
-	file_offset = dev->_frame_count_ch2 * frame_size;
-
-	myfile = filp_open(dev->_filename_ch2, O_RDONLY | O_LARGEFILE, 0);
-	if (IS_ERR(myfile)) {
-		const int open_errno = -PTR_ERR(myfile);
-		pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
-		       __func__, dev->_filename_ch2, open_errno);
-		return PTR_ERR(myfile);
-	} else {
-		if (!(myfile->f_op)) {
-			pr_err("%s(): File has no file operations registered!\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		if (!myfile->f_op->read) {
-			pr_err("%s(): File has no READ operations registered!\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		pos = myfile->f_pos;
-		old_fs = get_fs();
-		set_fs(KERNEL_DS);
-
-		for (i = 0; i < dev->_lines_count_ch2; i++) {
-			pos = file_offset;
-
-			vfs_read_retval = vfs_read(myfile, mybuf, line_size,
-					&pos);
-
-			if (vfs_read_retval > 0 && vfs_read_retval == line_size
-			    && dev->_data_buf_virt_addr_ch2 != NULL) {
-				memcpy((void *)(dev->_data_buf_virt_addr_ch2 +
-						frame_offset / 4), mybuf,
-						vfs_read_retval);
-			}
-
-			file_offset += vfs_read_retval;
-			frame_offset += vfs_read_retval;
-
-			if (vfs_read_retval < line_size) {
-				pr_info("Done: exit %s() since no more bytes to read from Video file\n",
-					__func__);
-				break;
-			}
-		}
-
-		if (i > 0)
-			dev->_frame_count_ch2++;
-
-		dev->_file_status_ch2 = (vfs_read_retval == line_size) ?
-			IN_PROGRESS : END_OF_FILE;
-
-		set_fs(old_fs);
-		filp_close(myfile, NULL);
-	}
-
-	return 0;
-}
-
-static void cx25821_vidups_handler_ch2(struct work_struct *work)
-{
-	struct cx25821_dev *dev = container_of(work, struct cx25821_dev,
-			_irq_work_entry_ch2);
-
-	if (!dev) {
-		pr_err("ERROR %s(): since container_of(work_struct) FAILED!\n",
-		       __func__);
-		return;
-	}
-
-	cx25821_get_frame_ch2(dev, dev->channels[dev->
-			_channel2_upstream_select].sram_channels);
-}
-
-static int cx25821_openfile_ch2(struct cx25821_dev *dev,
-				struct sram_channel *sram_ch)
-{
-	struct file *myfile;
-	int i = 0, j = 0;
-	int line_size = (dev->_pixel_format_ch2 == PIXEL_FRMT_411) ?
-		Y411_LINE_SZ : Y422_LINE_SZ;
-	ssize_t vfs_read_retval = 0;
-	char mybuf[line_size];
-	loff_t pos;
-	loff_t offset = (unsigned long)0;
-	mm_segment_t old_fs;
-
-	myfile = filp_open(dev->_filename_ch2, O_RDONLY | O_LARGEFILE, 0);
-
-	if (IS_ERR(myfile)) {
-		const int open_errno = -PTR_ERR(myfile);
-		pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
-		       __func__, dev->_filename_ch2, open_errno);
-		return PTR_ERR(myfile);
-	} else {
-		if (!(myfile->f_op)) {
-			pr_err("%s(): File has no file operations registered!\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		if (!myfile->f_op->read) {
-			pr_err("%s(): File has no READ operations registered!  Returning\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		pos = myfile->f_pos;
-		old_fs = get_fs();
-		set_fs(KERNEL_DS);
-
-		for (j = 0; j < NUM_FRAMES; j++) {
-			for (i = 0; i < dev->_lines_count_ch2; i++) {
-				pos = offset;
-
-				vfs_read_retval = vfs_read(myfile, mybuf,
-						line_size, &pos);
-
-				if (vfs_read_retval > 0 &&
-				    vfs_read_retval == line_size &&
-				    dev->_data_buf_virt_addr_ch2 != NULL) {
-					memcpy((void *)(dev->
-							_data_buf_virt_addr_ch2
-							+ offset / 4), mybuf,
-							vfs_read_retval);
-				}
-
-				offset += vfs_read_retval;
-
-				if (vfs_read_retval < line_size) {
-					pr_info("Done: exit %s() since no more bytes to read from Video file\n",
-						__func__);
-					break;
-				}
-			}
-
-			if (i > 0)
-				dev->_frame_count_ch2++;
-
-			if (vfs_read_retval < line_size)
-				break;
-		}
-
-		dev->_file_status_ch2 = (vfs_read_retval == line_size) ?
-			IN_PROGRESS : END_OF_FILE;
-
-		set_fs(old_fs);
-		myfile->f_pos = 0;
-		filp_close(myfile, NULL);
-	}
-
-	return 0;
-}
-
-static int cx25821_upstream_buffer_prepare_ch2(struct cx25821_dev *dev,
-					       struct sram_channel *sram_ch,
-					       int bpl)
-{
-	int ret = 0;
-	dma_addr_t dma_addr;
-	dma_addr_t data_dma_addr;
-
-	if (dev->_dma_virt_addr_ch2 != NULL) {
-		pci_free_consistent(dev->pci, dev->upstream_riscbuf_size_ch2,
-				    dev->_dma_virt_addr_ch2,
-				    dev->_dma_phys_addr_ch2);
-	}
-
-	dev->_dma_virt_addr_ch2 = pci_alloc_consistent(dev->pci,
-			dev->upstream_riscbuf_size_ch2, &dma_addr);
-	dev->_dma_virt_start_addr_ch2 = dev->_dma_virt_addr_ch2;
-	dev->_dma_phys_start_addr_ch2 = dma_addr;
-	dev->_dma_phys_addr_ch2 = dma_addr;
-	dev->_risc_size_ch2 = dev->upstream_riscbuf_size_ch2;
-
-	if (!dev->_dma_virt_addr_ch2) {
-		pr_err("FAILED to allocate memory for Risc buffer! Returning\n");
-		return -ENOMEM;
-	}
-
-	/* Iniitize at this address until n bytes to 0 */
-	memset(dev->_dma_virt_addr_ch2, 0, dev->_risc_size_ch2);
-
-	if (dev->_data_buf_virt_addr_ch2 != NULL) {
-		pci_free_consistent(dev->pci, dev->upstream_databuf_size_ch2,
-				    dev->_data_buf_virt_addr_ch2,
-				    dev->_data_buf_phys_addr_ch2);
-	}
-	/* For Video Data buffer allocation */
-	dev->_data_buf_virt_addr_ch2 = pci_alloc_consistent(dev->pci,
-			dev->upstream_databuf_size_ch2, &data_dma_addr);
-	dev->_data_buf_phys_addr_ch2 = data_dma_addr;
-	dev->_data_buf_size_ch2 = dev->upstream_databuf_size_ch2;
-
-	if (!dev->_data_buf_virt_addr_ch2) {
-		pr_err("FAILED to allocate memory for data buffer! Returning\n");
-		return -ENOMEM;
-	}
-
-	/* Initialize at this address until n bytes to 0 */
-	memset(dev->_data_buf_virt_addr_ch2, 0, dev->_data_buf_size_ch2);
-
-	ret = cx25821_openfile_ch2(dev, sram_ch);
-	if (ret < 0)
-		return ret;
-
-	/* Creating RISC programs */
-	ret = cx25821_risc_buffer_upstream_ch2(dev, dev->pci, 0, bpl,
-						dev->_lines_count_ch2);
-	if (ret < 0) {
-		pr_info("Failed creating Video Upstream Risc programs!\n");
-		goto error;
-	}
-
-	return 0;
-
-error:
-	return ret;
-}
-
-static int cx25821_video_upstream_irq_ch2(struct cx25821_dev *dev,
-					  int chan_num,
-					  u32 status)
-{
-	u32 int_msk_tmp;
-	struct sram_channel *channel = dev->channels[chan_num].sram_channels;
-	int singlefield_lines = NTSC_FIELD_HEIGHT;
-	int line_size_in_bytes = Y422_LINE_SZ;
-	int odd_risc_prog_size = 0;
-	dma_addr_t risc_phys_jump_addr;
-	__le32 *rp;
-
-	if (status & FLD_VID_SRC_RISC1) {
-		/* We should only process one program per call */
-		u32 prog_cnt = cx_read(channel->gpcnt);
-
-		/*
-		 *  Since we've identified our IRQ, clear our bits from the
-		 *  interrupt mask and interrupt status registers
-		 */
-		int_msk_tmp = cx_read(channel->int_msk);
-		cx_write(channel->int_msk, int_msk_tmp & ~_intr_msk);
-		cx_write(channel->int_stat, _intr_msk);
-
-		spin_lock(&dev->slock);
-
-		dev->_frame_index_ch2 = prog_cnt;
-
-		queue_work(dev->_irq_queues_ch2, &dev->_irq_work_entry_ch2);
-
-		if (dev->_is_first_frame_ch2) {
-			dev->_is_first_frame_ch2 = 0;
-
-			if (dev->_isNTSC_ch2) {
-				singlefield_lines += 1;
-				odd_risc_prog_size = ODD_FLD_NTSC_PROG_SIZE;
-			} else {
-				singlefield_lines = PAL_FIELD_HEIGHT;
-				odd_risc_prog_size = ODD_FLD_PAL_PROG_SIZE;
-			}
-
-			if (dev->_dma_virt_start_addr_ch2 != NULL) {
-				if (dev->_pixel_format_ch2 == PIXEL_FRMT_411)
-					line_size_in_bytes = Y411_LINE_SZ;
-				else
-					line_size_in_bytes = Y422_LINE_SZ;
-				risc_phys_jump_addr =
-					dev->_dma_phys_start_addr_ch2 +
-					odd_risc_prog_size;
-
-				rp = cx25821_update_riscprogram_ch2(dev,
-						dev->_dma_virt_start_addr_ch2,
-						TOP_OFFSET, line_size_in_bytes,
-						0x0, singlefield_lines,
-						FIFO_DISABLE, ODD_FIELD);
-
-			       /* Jump to Even Risc program of 1st Frame */
-				*(rp++) = cpu_to_le32(RISC_JUMP);
-				*(rp++) = cpu_to_le32(risc_phys_jump_addr);
-				*(rp++) = cpu_to_le32(0);
-			}
-		}
-
-		spin_unlock(&dev->slock);
-	}
-
-	if (dev->_file_status_ch2 == END_OF_FILE) {
-		pr_info("EOF Channel 2 Framecount = %d\n",
-			dev->_frame_count_ch2);
-		return -1;
-	}
-	/* ElSE, set the interrupt mask register, re-enable irq. */
-	int_msk_tmp = cx_read(channel->int_msk);
-	cx_write(channel->int_msk, int_msk_tmp |= _intr_msk);
-
-	return 0;
-}
-
-static irqreturn_t cx25821_upstream_irq_ch2(int irq, void *dev_id)
-{
-	struct cx25821_dev *dev = dev_id;
-	u32 vid_status;
-	int handled = 0;
-	int channel_num = 0;
-	struct sram_channel *sram_ch;
-
-	if (!dev)
-		return -1;
-
-	channel_num = VID_UPSTREAM_SRAM_CHANNEL_J;
-	sram_ch = dev->channels[channel_num].sram_channels;
-
-	vid_status = cx_read(sram_ch->int_stat);
-
-	/* Only deal with our interrupt */
-	if (vid_status)
-		handled = cx25821_video_upstream_irq_ch2(dev, channel_num,
-				vid_status);
-
-	if (handled < 0)
-		cx25821_stop_upstream_video_ch2(dev);
-	else
-		handled += handled;
-
-	return IRQ_RETVAL(handled);
-}
-
-static void cx25821_set_pixelengine_ch2(struct cx25821_dev *dev,
-					struct sram_channel *ch, int pix_format)
-{
-	int width = WIDTH_D1;
-	int height = dev->_lines_count_ch2;
-	int num_lines, odd_num_lines;
-	u32 value;
-	int vip_mode = PIXEL_ENGINE_VIP1;
-
-	value = ((pix_format & 0x3) << 12) | (vip_mode & 0x7);
-	value &= 0xFFFFFFEF;
-	value |= dev->_isNTSC_ch2 ? 0 : 0x10;
-	cx_write(ch->vid_fmt_ctl, value);
-
-	/*
-	 *  set number of active pixels in each line. Default is 720
-	 * pixels in both NTSC and PAL format
-	 */
-	cx_write(ch->vid_active_ctl1, width);
-
-	num_lines = (height / 2) & 0x3FF;
-	odd_num_lines = num_lines;
-
-	if (dev->_isNTSC_ch2)
-		odd_num_lines += 1;
-
-	value = (num_lines << 16) | odd_num_lines;
-
-	/* set number of active lines in field 0 (top) and field 1 (bottom) */
-	cx_write(ch->vid_active_ctl2, value);
-
-	cx_write(ch->vid_cdt_size, VID_CDT_SIZE >> 3);
-}
-
-static int cx25821_start_video_dma_upstream_ch2(struct cx25821_dev *dev,
-						struct sram_channel *sram_ch)
-{
-	u32 tmp = 0;
-	int err = 0;
-
-	/*
-	 *  656/VIP SRC Upstream Channel I & J and 7 - Host Bus Interface
-	 * for channel A-C
-	 */
-	tmp = cx_read(VID_CH_MODE_SEL);
-	cx_write(VID_CH_MODE_SEL, tmp | 0x1B0001FF);
-
-	/*
-	 *  Set the physical start address of the RISC program in the initial
-	 *  program counter(IPC) member of the cmds.
-	 */
-	cx_write(sram_ch->cmds_start + 0, dev->_dma_phys_addr_ch2);
-	cx_write(sram_ch->cmds_start + 4, 0); /* Risc IPC High 64 bits 63-32 */
-
-	/* reset counter */
-	cx_write(sram_ch->gpcnt_ctl, 3);
-
-	/* Clear our bits from the interrupt status register. */
-	cx_write(sram_ch->int_stat, _intr_msk);
-
-	/* Set the interrupt mask register, enable irq. */
-	cx_set(PCI_INT_MSK, cx_read(PCI_INT_MSK) | (1 << sram_ch->irq_bit));
-	tmp = cx_read(sram_ch->int_msk);
-	cx_write(sram_ch->int_msk, tmp |= _intr_msk);
-
-	err = request_irq(dev->pci->irq, cx25821_upstream_irq_ch2,
-			IRQF_SHARED, dev->name, dev);
-	if (err < 0) {
-		pr_err("%s: can't get upstream IRQ %d\n",
-		       dev->name, dev->pci->irq);
-		goto fail_irq;
-	}
-	/* Start the DMA  engine */
-	tmp = cx_read(sram_ch->dma_ctl);
-	cx_set(sram_ch->dma_ctl, tmp | FLD_VID_RISC_EN);
-
-	dev->_is_running_ch2 = 1;
-	dev->_is_first_frame_ch2 = 1;
-
-	return 0;
-
-fail_irq:
-	cx25821_dev_unregister(dev);
-	return err;
-}
-
-int cx25821_vidupstream_init_ch2(struct cx25821_dev *dev, int channel_select,
-				 int pixel_format)
-{
-	struct sram_channel *sram_ch;
-	u32 tmp;
-	int err = 0;
-	int data_frame_size = 0;
-	int risc_buffer_size = 0;
-
-	if (dev->_is_running_ch2) {
-		pr_info("Video Channel is still running so return!\n");
-		return 0;
-	}
-
-	dev->_channel2_upstream_select = channel_select;
-	sram_ch = dev->channels[channel_select].sram_channels;
-
-	INIT_WORK(&dev->_irq_work_entry_ch2, cx25821_vidups_handler_ch2);
-	dev->_irq_queues_ch2 =
-	    create_singlethread_workqueue("cx25821_workqueue2");
-
-	if (!dev->_irq_queues_ch2) {
-		pr_err("create_singlethread_workqueue() for Video FAILED!\n");
-		return -ENOMEM;
-	}
-	/*
-	 * 656/VIP SRC Upstream Channel I & J and 7 -
-	 * Host Bus Interface for channel A-C
-	 */
-	tmp = cx_read(VID_CH_MODE_SEL);
-	cx_write(VID_CH_MODE_SEL, tmp | 0x1B0001FF);
-
-	dev->_is_running_ch2 = 0;
-	dev->_frame_count_ch2 = 0;
-	dev->_file_status_ch2 = RESET_STATUS;
-	dev->_lines_count_ch2 = dev->_isNTSC_ch2 ? 480 : 576;
-	dev->_pixel_format_ch2 = pixel_format;
-	dev->_line_size_ch2 = (dev->_pixel_format_ch2 == PIXEL_FRMT_422) ?
-		(WIDTH_D1 * 2) : (WIDTH_D1 * 3) / 2;
-	data_frame_size = dev->_isNTSC_ch2 ? NTSC_DATA_BUF_SZ : PAL_DATA_BUF_SZ;
-	risc_buffer_size = dev->_isNTSC_ch2 ?
-		NTSC_RISC_BUF_SIZE : PAL_RISC_BUF_SIZE;
-
-	if (dev->input_filename_ch2)
-		dev->_filename_ch2 = kstrdup(dev->input_filename_ch2,
-								GFP_KERNEL);
-	else
-		dev->_filename_ch2 = kstrdup(dev->_defaultname_ch2,
-								GFP_KERNEL);
-
-	if (!dev->_filename_ch2) {
-		err = -ENOENT;
-		goto error;
-	}
-
-	/* Default if filename is empty string */
-	if (strcmp(dev->_filename_ch2, "") == 0) {
-		if (dev->_isNTSC_ch2) {
-			dev->_filename_ch2 = (dev->_pixel_format_ch2 ==
-				PIXEL_FRMT_411) ? "/root/vid411.yuv" :
-				"/root/vidtest.yuv";
-		} else {
-			dev->_filename_ch2 = (dev->_pixel_format_ch2 ==
-				PIXEL_FRMT_411) ? "/root/pal411.yuv" :
-				"/root/pal422.yuv";
-		}
-	}
-
-	err = cx25821_sram_channel_setup_upstream(dev, sram_ch,
-						dev->_line_size_ch2, 0);
-
-	/* setup fifo + format */
-	cx25821_set_pixelengine_ch2(dev, sram_ch, dev->_pixel_format_ch2);
-
-	dev->upstream_riscbuf_size_ch2 = risc_buffer_size * 2;
-	dev->upstream_databuf_size_ch2 = data_frame_size * 2;
-
-	/* Allocating buffers and prepare RISC program */
-	err = cx25821_upstream_buffer_prepare_ch2(dev, sram_ch,
-						dev->_line_size_ch2);
-	if (err < 0) {
-		pr_err("%s: Failed to set up Video upstream buffers!\n",
-		       dev->name);
-		goto error;
-	}
-
-	cx25821_start_video_dma_upstream_ch2(dev, sram_ch);
-
-	return 0;
-
-error:
-	cx25821_dev_unregister(dev);
-
-	return err;
-}
diff --git a/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.h b/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.h
deleted file mode 100644
index d42dab59b663..000000000000
--- a/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.h
+++ /dev/null
@@ -1,138 +0,0 @@
-/*
- *  Driver for the Conexant CX25821 PCIe bridge
- *
- *  Copyright (C) 2009 Conexant Systems Inc.
- *  Authors  <hiep.huynh@conexant.com>, <shu.lin@conexant.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., 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-
-#include <linux/mutex.h>
-#include <linux/workqueue.h>
-
-#define OPEN_FILE_1           0
-#define NUM_PROGS             8
-#define NUM_FRAMES            2
-#define ODD_FIELD             0
-#define EVEN_FIELD            1
-#define TOP_OFFSET            0
-#define FIFO_DISABLE          0
-#define FIFO_ENABLE           1
-#define TEST_FRAMES           5
-#define END_OF_FILE           0
-#define IN_PROGRESS           1
-#define RESET_STATUS          -1
-#define NUM_NO_OPS            5
-
-/* PAL and NTSC line sizes and number of lines. */
-#define WIDTH_D1              720
-#define NTSC_LINES_PER_FRAME  480
-#define PAL_LINES_PER_FRAME   576
-#define PAL_LINE_SZ           1440
-#define Y422_LINE_SZ          1440
-#define Y411_LINE_SZ          1080
-#define NTSC_FIELD_HEIGHT     240
-#define NTSC_ODD_FLD_LINES    241
-#define PAL_FIELD_HEIGHT      288
-
-#define FRAME_SIZE_NTSC_Y422    (NTSC_LINES_PER_FRAME * Y422_LINE_SZ)
-#define FRAME_SIZE_NTSC_Y411    (NTSC_LINES_PER_FRAME * Y411_LINE_SZ)
-#define FRAME_SIZE_PAL_Y422     (PAL_LINES_PER_FRAME * Y422_LINE_SZ)
-#define FRAME_SIZE_PAL_Y411     (PAL_LINES_PER_FRAME * Y411_LINE_SZ)
-
-#define NTSC_DATA_BUF_SZ        (Y422_LINE_SZ * NTSC_LINES_PER_FRAME)
-#define PAL_DATA_BUF_SZ         (Y422_LINE_SZ * PAL_LINES_PER_FRAME)
-
-#define RISC_WRITECR_INSTRUCTION_SIZE   16
-#define RISC_SYNC_INSTRUCTION_SIZE      4
-#define JUMP_INSTRUCTION_SIZE           12
-#define MAXSIZE_NO_OPS                  36
-#define DWORD_SIZE                      4
-
-#define USE_RISC_NOOP_VIDEO   1
-
-#ifdef USE_RISC_NOOP_VIDEO
-#define PAL_US_VID_PROG_SIZE						\
-	(PAL_FIELD_HEIGHT * 3 * DWORD_SIZE +				\
-	 RISC_WRITECR_INSTRUCTION_SIZE + RISC_SYNC_INSTRUCTION_SIZE +	\
-	 NUM_NO_OPS * DWORD_SIZE)
-
-#define PAL_RISC_BUF_SIZE         (2 * PAL_US_VID_PROG_SIZE)
-
-#define PAL_VID_PROG_SIZE						\
-	((PAL_FIELD_HEIGHT * 2) * 3 * DWORD_SIZE +			\
-	 2 * RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE + \
-	 JUMP_INSTRUCTION_SIZE + 2 * NUM_NO_OPS * DWORD_SIZE)
-
-#define ODD_FLD_PAL_PROG_SIZE						\
-	(PAL_FIELD_HEIGHT * 3 * DWORD_SIZE +				\
-	 RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE +	\
-	 NUM_NO_OPS * DWORD_SIZE)
-
-#define NTSC_US_VID_PROG_SIZE						\
-	((NTSC_ODD_FLD_LINES + 1) * 3 * DWORD_SIZE +			\
-	 RISC_WRITECR_INSTRUCTION_SIZE + JUMP_INSTRUCTION_SIZE +	\
-	 NUM_NO_OPS * DWORD_SIZE)
-
-#define NTSC_RISC_BUF_SIZE						\
-	(2 * (RISC_SYNC_INSTRUCTION_SIZE + NTSC_US_VID_PROG_SIZE))
-
-#define FRAME1_VID_PROG_SIZE						\
-	((NTSC_ODD_FLD_LINES + NTSC_FIELD_HEIGHT) *			\
-	 3 * DWORD_SIZE + 2 * RISC_SYNC_INSTRUCTION_SIZE +		\
-	 RISC_WRITECR_INSTRUCTION_SIZE + JUMP_INSTRUCTION_SIZE +	\
-	 2 * NUM_NO_OPS * DWORD_SIZE)
-
-#define ODD_FLD_NTSC_PROG_SIZE						\
-	(NTSC_ODD_FLD_LINES * 3 * DWORD_SIZE +				\
-	 RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE +	\
-	 NUM_NO_OPS * DWORD_SIZE)
-#endif
-
-#ifndef USE_RISC_NOOP_VIDEO
-#define PAL_US_VID_PROG_SIZE						\
-	((PAL_FIELD_HEIGHT + 1) * 3 * DWORD_SIZE +			\
-	 RISC_WRITECR_INSTRUCTION_SIZE)
-
-#define PAL_RISC_BUF_SIZE						\
-	(2 * (RISC_SYNC_INSTRUCTION_SIZE + PAL_US_VID_PROG_SIZE))
-
-#define PAL_VID_PROG_SIZE						\
-	((PAL_FIELD_HEIGHT * 2) * 3 * DWORD_SIZE +			\
-	 2 * RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE + \
-	 JUMP_INSTRUCTION_SIZE)
-
-#define ODD_FLD_PAL_PROG_SIZE						\
-	(PAL_FIELD_HEIGHT * 3 * DWORD_SIZE +				\
-	 RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE)
-
-#define ODD_FLD_NTSC_PROG_SIZE						\
-	(NTSC_ODD_FLD_LINES * 3 * DWORD_SIZE +				\
-	 RISC_SYNC_INSTRUCTION_SIZE + RISC_WRITECR_INSTRUCTION_SIZE)
-
-#define NTSC_US_VID_PROG_SIZE						\
-	((NTSC_ODD_FLD_LINES + 1) * 3 * DWORD_SIZE +			\
-	 RISC_WRITECR_INSTRUCTION_SIZE + JUMP_INSTRUCTION_SIZE)
-
-#define NTSC_RISC_BUF_SIZE						\
-	(2 * (RISC_SYNC_INSTRUCTION_SIZE + NTSC_US_VID_PROG_SIZE))
-
-#define FRAME1_VID_PROG_SIZE						\
-	((NTSC_ODD_FLD_LINES + NTSC_FIELD_HEIGHT) *			\
-	 3 * DWORD_SIZE + 2 * RISC_SYNC_INSTRUCTION_SIZE +		\
-	 RISC_WRITECR_INSTRUCTION_SIZE + JUMP_INSTRUCTION_SIZE)
-
-#endif
diff --git a/drivers/media/pci/cx25821/cx25821-video-upstream.c b/drivers/media/pci/cx25821/cx25821-video-upstream.c
index 7fc97110d973..88ffef410c50 100644
--- a/drivers/media/pci/cx25821/cx25821-video-upstream.c
+++ b/drivers/media/pci/cx25821/cx25821-video-upstream.c
@@ -25,16 +25,11 @@
 #include "cx25821-video.h"
 #include "cx25821-video-upstream.h"
 
-#include <linux/fs.h>
 #include <linux/errno.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/module.h>
-#include <linux/syscalls.h>
-#include <linux/file.h>
-#include <linux/fcntl.h>
 #include <linux/slab.h>
-#include <linux/uaccess.h>
 
 MODULE_DESCRIPTION("v4l2 driver module for cx25821 based TV cards");
 MODULE_AUTHOR("Hiep Huynh <hiep.huynh@conexant.com>");
@@ -44,7 +39,7 @@ static int _intr_msk = FLD_VID_SRC_RISC1 | FLD_VID_SRC_UF | FLD_VID_SRC_SYNC |
 			FLD_VID_SRC_OPC_ERR;
 
 int cx25821_sram_channel_setup_upstream(struct cx25821_dev *dev,
-					struct sram_channel *ch,
+					const struct sram_channel *ch,
 					unsigned int bpl, u32 risc)
 {
 	unsigned int i, lines;
@@ -97,12 +92,13 @@ int cx25821_sram_channel_setup_upstream(struct cx25821_dev *dev,
 	return 0;
 }
 
-static __le32 *cx25821_update_riscprogram(struct cx25821_dev *dev,
+static __le32 *cx25821_update_riscprogram(struct cx25821_channel *chan,
 					  __le32 *rp, unsigned int offset,
 					  unsigned int bpl, u32 sync_line,
 					  unsigned int lines, int fifo_enable,
 					  int field_type)
 {
+	struct cx25821_video_out_data *out = chan->out;
 	unsigned int line, i;
 	int dist_betwn_starts = bpl * 2;
 
@@ -116,11 +112,11 @@ static __le32 *cx25821_update_riscprogram(struct cx25821_dev *dev,
 	/* scan lines */
 	for (line = 0; line < lines; line++) {
 		*(rp++) = cpu_to_le32(RISC_READ | RISC_SOL | RISC_EOL | bpl);
-		*(rp++) = cpu_to_le32(dev->_data_buf_phys_addr + offset);
+		*(rp++) = cpu_to_le32(out->_data_buf_phys_addr + offset);
 		*(rp++) = cpu_to_le32(0);	/* bits 63-32 */
 
 		if ((lines <= NTSC_FIELD_HEIGHT)
-		    || (line < (NTSC_FIELD_HEIGHT - 1)) || !(dev->_isNTSC)) {
+		    || (line < (NTSC_FIELD_HEIGHT - 1)) || !(out->is_60hz)) {
 			offset += dist_betwn_starts;
 		}
 	}
@@ -128,15 +124,15 @@ static __le32 *cx25821_update_riscprogram(struct cx25821_dev *dev,
 	return rp;
 }
 
-static __le32 *cx25821_risc_field_upstream(struct cx25821_dev *dev, __le32 * rp,
+static __le32 *cx25821_risc_field_upstream(struct cx25821_channel *chan, __le32 *rp,
 					   dma_addr_t databuf_phys_addr,
 					   unsigned int offset, u32 sync_line,
 					   unsigned int bpl, unsigned int lines,
 					   int fifo_enable, int field_type)
 {
+	struct cx25821_video_out_data *out = chan->out;
 	unsigned int line, i;
-	struct sram_channel *sram_ch =
-		dev->channels[dev->_channel_upstream_select].sram_channels;
+	const struct sram_channel *sram_ch = chan->sram_channels;
 	int dist_betwn_starts = bpl * 2;
 
 	/* sync instruction */
@@ -155,7 +151,7 @@ static __le32 *cx25821_risc_field_upstream(struct cx25821_dev *dev, __le32 * rp,
 		*(rp++) = cpu_to_le32(0);	/* bits 63-32 */
 
 		if ((lines <= NTSC_FIELD_HEIGHT)
-		    || (line < (NTSC_FIELD_HEIGHT - 1)) || !(dev->_isNTSC))
+		    || (line < (NTSC_FIELD_HEIGHT - 1)) || !(out->is_60hz))
 			/* to skip the other field line */
 			offset += dist_betwn_starts;
 
@@ -173,11 +169,12 @@ static __le32 *cx25821_risc_field_upstream(struct cx25821_dev *dev, __le32 * rp,
 	return rp;
 }
 
-static int cx25821_risc_buffer_upstream(struct cx25821_dev *dev,
+static int cx25821_risc_buffer_upstream(struct cx25821_channel *chan,
 					struct pci_dev *pci,
 					unsigned int top_offset,
 					unsigned int bpl, unsigned int lines)
 {
+	struct cx25821_video_out_data *out = chan->out;
 	__le32 *rp;
 	int fifo_enable = 0;
 	/* get line count for single field */
@@ -191,7 +188,7 @@ static int cx25821_risc_buffer_upstream(struct cx25821_dev *dev,
 	unsigned int bottom_offset = bpl;
 	dma_addr_t risc_phys_jump_addr;
 
-	if (dev->_isNTSC) {
+	if (out->is_60hz) {
 		odd_num_lines = singlefield_lines + 1;
 		risc_program_size = FRAME1_VID_PROG_SIZE;
 		frame_size = (bpl == Y411_LINE_SZ) ?
@@ -203,15 +200,15 @@ static int cx25821_risc_buffer_upstream(struct cx25821_dev *dev,
 	}
 
 	/* Virtual address of Risc buffer program */
-	rp = dev->_dma_virt_addr;
+	rp = out->_dma_virt_addr;
 
 	for (frame = 0; frame < NUM_FRAMES; frame++) {
 		databuf_offset = frame_size * frame;
 
 		if (UNSET != top_offset) {
 			fifo_enable = (frame == 0) ? FIFO_ENABLE : FIFO_DISABLE;
-			rp = cx25821_risc_field_upstream(dev, rp,
-					dev->_data_buf_phys_addr +
+			rp = cx25821_risc_field_upstream(chan, rp,
+					out->_data_buf_phys_addr +
 					databuf_offset, top_offset, 0, bpl,
 					odd_num_lines, fifo_enable, ODD_FIELD);
 		}
@@ -219,18 +216,18 @@ static int cx25821_risc_buffer_upstream(struct cx25821_dev *dev,
 		fifo_enable = FIFO_DISABLE;
 
 		/* Even Field */
-		rp = cx25821_risc_field_upstream(dev, rp,
-						 dev->_data_buf_phys_addr +
+		rp = cx25821_risc_field_upstream(chan, rp,
+						 out->_data_buf_phys_addr +
 						 databuf_offset, bottom_offset,
 						 0x200, bpl, singlefield_lines,
 						 fifo_enable, EVEN_FIELD);
 
 		if (frame == 0) {
 			risc_flag = RISC_CNT_RESET;
-			risc_phys_jump_addr = dev->_dma_phys_start_addr +
+			risc_phys_jump_addr = out->_dma_phys_start_addr +
 				risc_program_size;
 		} else {
-			risc_phys_jump_addr = dev->_dma_phys_start_addr;
+			risc_phys_jump_addr = out->_dma_phys_start_addr;
 			risc_flag = RISC_CNT_INC;
 		}
 
@@ -245,16 +242,21 @@ static int cx25821_risc_buffer_upstream(struct cx25821_dev *dev,
 	return 0;
 }
 
-void cx25821_stop_upstream_video_ch1(struct cx25821_dev *dev)
+void cx25821_stop_upstream_video(struct cx25821_channel *chan)
 {
-	struct sram_channel *sram_ch =
-		dev->channels[VID_UPSTREAM_SRAM_CHANNEL_I].sram_channels;
+	struct cx25821_video_out_data *out = chan->out;
+	struct cx25821_dev *dev = chan->dev;
+	const struct sram_channel *sram_ch = chan->sram_channels;
 	u32 tmp = 0;
 
-	if (!dev->_is_running) {
+	if (!out->_is_running) {
 		pr_info("No video file is currently running so return!\n");
 		return;
 	}
+
+	/* Set the interrupt mask register, disable irq. */
+	cx_set(PCI_INT_MSK, cx_read(PCI_INT_MSK) & ~(1 << sram_ch->irq_bit));
+
 	/* Disable RISC interrupts */
 	tmp = cx_read(sram_ch->int_msk);
 	cx_write(sram_ch->int_msk, tmp & ~_intr_msk);
@@ -263,283 +265,133 @@ void cx25821_stop_upstream_video_ch1(struct cx25821_dev *dev)
 	tmp = cx_read(sram_ch->dma_ctl);
 	cx_write(sram_ch->dma_ctl, tmp & ~(FLD_VID_FIFO_EN | FLD_VID_RISC_EN));
 
-	/* Clear data buffer memory */
-	if (dev->_data_buf_virt_addr)
-		memset(dev->_data_buf_virt_addr, 0, dev->_data_buf_size);
+	free_irq(dev->pci->irq, chan);
 
-	dev->_is_running = 0;
-	dev->_is_first_frame = 0;
-	dev->_frame_count = 0;
-	dev->_file_status = END_OF_FILE;
-
-	kfree(dev->_irq_queues);
-	dev->_irq_queues = NULL;
+	/* Clear data buffer memory */
+	if (out->_data_buf_virt_addr)
+		memset(out->_data_buf_virt_addr, 0, out->_data_buf_size);
 
-	kfree(dev->_filename);
+	out->_is_running = 0;
+	out->_is_first_frame = 0;
+	out->_frame_count = 0;
+	out->_file_status = END_OF_FILE;
 
 	tmp = cx_read(VID_CH_MODE_SEL);
 	cx_write(VID_CH_MODE_SEL, tmp & 0xFFFFFE00);
 }
 
-void cx25821_free_mem_upstream_ch1(struct cx25821_dev *dev)
+void cx25821_free_mem_upstream(struct cx25821_channel *chan)
 {
-	if (dev->_is_running)
-		cx25821_stop_upstream_video_ch1(dev);
+	struct cx25821_video_out_data *out = chan->out;
+	struct cx25821_dev *dev = chan->dev;
 
-	if (dev->_dma_virt_addr) {
-		pci_free_consistent(dev->pci, dev->_risc_size,
-				    dev->_dma_virt_addr, dev->_dma_phys_addr);
-		dev->_dma_virt_addr = NULL;
+	if (out->_is_running)
+		cx25821_stop_upstream_video(chan);
+
+	if (out->_dma_virt_addr) {
+		pci_free_consistent(dev->pci, out->_risc_size,
+				    out->_dma_virt_addr, out->_dma_phys_addr);
+		out->_dma_virt_addr = NULL;
 	}
 
-	if (dev->_data_buf_virt_addr) {
-		pci_free_consistent(dev->pci, dev->_data_buf_size,
-				    dev->_data_buf_virt_addr,
-				    dev->_data_buf_phys_addr);
-		dev->_data_buf_virt_addr = NULL;
+	if (out->_data_buf_virt_addr) {
+		pci_free_consistent(dev->pci, out->_data_buf_size,
+				    out->_data_buf_virt_addr,
+				    out->_data_buf_phys_addr);
+		out->_data_buf_virt_addr = NULL;
 	}
 }
 
-static int cx25821_get_frame(struct cx25821_dev *dev,
-			     struct sram_channel *sram_ch)
+int cx25821_write_frame(struct cx25821_channel *chan,
+		const char __user *data, size_t count)
 {
-	struct file *myfile;
-	int frame_index_temp = dev->_frame_index;
-	int i = 0;
-	int line_size = (dev->_pixel_format == PIXEL_FRMT_411) ?
+	struct cx25821_video_out_data *out = chan->out;
+	int line_size = (out->_pixel_format == PIXEL_FRMT_411) ?
 		Y411_LINE_SZ : Y422_LINE_SZ;
 	int frame_size = 0;
 	int frame_offset = 0;
-	ssize_t vfs_read_retval = 0;
-	char mybuf[line_size];
-	loff_t file_offset;
-	loff_t pos;
-	mm_segment_t old_fs;
-
-	if (dev->_file_status == END_OF_FILE)
-		return 0;
+	int curpos = out->curpos;
 
-	if (dev->_isNTSC)
+	if (out->is_60hz)
 		frame_size = (line_size == Y411_LINE_SZ) ?
 			FRAME_SIZE_NTSC_Y411 : FRAME_SIZE_NTSC_Y422;
 	else
 		frame_size = (line_size == Y411_LINE_SZ) ?
 			FRAME_SIZE_PAL_Y411 : FRAME_SIZE_PAL_Y422;
 
-	frame_offset = (frame_index_temp > 0) ? frame_size : 0;
-	file_offset = dev->_frame_count * frame_size;
-
-	myfile = filp_open(dev->_filename, O_RDONLY | O_LARGEFILE, 0);
-
-	if (IS_ERR(myfile)) {
-		const int open_errno = -PTR_ERR(myfile);
-		pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
-		       __func__, dev->_filename, open_errno);
-		return PTR_ERR(myfile);
-	} else {
-		if (!(myfile->f_op)) {
-			pr_err("%s(): File has no file operations registered!\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		if (!myfile->f_op->read) {
-			pr_err("%s(): File has no READ operations registered!\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		pos = myfile->f_pos;
-		old_fs = get_fs();
-		set_fs(KERNEL_DS);
-
-		for (i = 0; i < dev->_lines_count; i++) {
-			pos = file_offset;
-
-			vfs_read_retval = vfs_read(myfile, mybuf, line_size,
-					&pos);
-
-			if (vfs_read_retval > 0 && vfs_read_retval == line_size
-			    && dev->_data_buf_virt_addr != NULL) {
-				memcpy((void *)(dev->_data_buf_virt_addr +
-						frame_offset / 4), mybuf,
-				       vfs_read_retval);
-			}
-
-			file_offset += vfs_read_retval;
-			frame_offset += vfs_read_retval;
-
-			if (vfs_read_retval < line_size) {
-				pr_info("Done: exit %s() since no more bytes to read from Video file\n",
-					__func__);
-				break;
-			}
-		}
-
-		if (i > 0)
-			dev->_frame_count++;
-
-		dev->_file_status = (vfs_read_retval == line_size) ?
-			IN_PROGRESS : END_OF_FILE;
-
-		set_fs(old_fs);
-		filp_close(myfile, NULL);
-	}
-
-	return 0;
-}
-
-static void cx25821_vidups_handler(struct work_struct *work)
-{
-	struct cx25821_dev *dev = container_of(work, struct cx25821_dev,
-			_irq_work_entry);
-
-	if (!dev) {
-		pr_err("ERROR %s(): since container_of(work_struct) FAILED!\n",
-		       __func__);
-		return;
+	if (curpos == 0) {
+		out->cur_frame_index = out->_frame_index;
+		if (wait_event_interruptible(out->waitq, out->cur_frame_index != out->_frame_index))
+			return -EINTR;
+		out->cur_frame_index = out->_frame_index;
 	}
 
-	cx25821_get_frame(dev, dev->channels[dev->_channel_upstream_select].
-			sram_channels);
-}
-
-static int cx25821_openfile(struct cx25821_dev *dev,
-			    struct sram_channel *sram_ch)
-{
-	struct file *myfile;
-	int i = 0, j = 0;
-	int line_size = (dev->_pixel_format == PIXEL_FRMT_411) ?
-		Y411_LINE_SZ : Y422_LINE_SZ;
-	ssize_t vfs_read_retval = 0;
-	char mybuf[line_size];
-	loff_t pos;
-	loff_t offset = (unsigned long)0;
-	mm_segment_t old_fs;
-
-	myfile = filp_open(dev->_filename, O_RDONLY | O_LARGEFILE, 0);
-
-	if (IS_ERR(myfile)) {
-		const int open_errno = -PTR_ERR(myfile);
-		pr_err("%s(): ERROR opening file(%s) with errno = %d!\n",
-		       __func__, dev->_filename, open_errno);
-		return PTR_ERR(myfile);
-	} else {
-		if (!(myfile->f_op)) {
-			pr_err("%s(): File has no file operations registered!\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		if (!myfile->f_op->read) {
-			pr_err("%s(): File has no READ operations registered!  Returning\n",
-			       __func__);
-			filp_close(myfile, NULL);
-			return -EIO;
-		}
-
-		pos = myfile->f_pos;
-		old_fs = get_fs();
-		set_fs(KERNEL_DS);
-
-		for (j = 0; j < NUM_FRAMES; j++) {
-			for (i = 0; i < dev->_lines_count; i++) {
-				pos = offset;
-
-				vfs_read_retval = vfs_read(myfile, mybuf,
-						line_size, &pos);
-
-				if (vfs_read_retval > 0
-				    && vfs_read_retval == line_size
-				    && dev->_data_buf_virt_addr != NULL) {
-					memcpy((void *)(dev->
-							_data_buf_virt_addr +
-							offset / 4), mybuf,
-					       vfs_read_retval);
-				}
-
-				offset += vfs_read_retval;
-
-				if (vfs_read_retval < line_size) {
-					pr_info("Done: exit %s() since no more bytes to read from Video file\n",
-						__func__);
-					break;
-				}
-			}
+	frame_offset = out->cur_frame_index ? frame_size : 0;
 
-			if (i > 0)
-				dev->_frame_count++;
-
-			if (vfs_read_retval < line_size)
-				break;
-		}
-
-		dev->_file_status = (vfs_read_retval == line_size) ?
-			IN_PROGRESS : END_OF_FILE;
-
-		set_fs(old_fs);
-		myfile->f_pos = 0;
-		filp_close(myfile, NULL);
+	if (frame_size - curpos < count)
+		count = frame_size - curpos;
+	memcpy((char *)out->_data_buf_virt_addr + frame_offset + curpos,
+			data, count);
+	curpos += count;
+	if (curpos == frame_size) {
+		out->_frame_count++;
+		curpos = 0;
 	}
+	out->curpos = curpos;
 
-	return 0;
+	return count;
 }
 
-static int cx25821_upstream_buffer_prepare(struct cx25821_dev *dev,
-					   struct sram_channel *sram_ch,
+static int cx25821_upstream_buffer_prepare(struct cx25821_channel *chan,
+					   const struct sram_channel *sram_ch,
 					   int bpl)
 {
+	struct cx25821_video_out_data *out = chan->out;
+	struct cx25821_dev *dev = chan->dev;
 	int ret = 0;
 	dma_addr_t dma_addr;
 	dma_addr_t data_dma_addr;
 
-	if (dev->_dma_virt_addr != NULL)
-		pci_free_consistent(dev->pci, dev->upstream_riscbuf_size,
-				dev->_dma_virt_addr, dev->_dma_phys_addr);
+	if (out->_dma_virt_addr != NULL)
+		pci_free_consistent(dev->pci, out->upstream_riscbuf_size,
+				out->_dma_virt_addr, out->_dma_phys_addr);
 
-	dev->_dma_virt_addr = pci_alloc_consistent(dev->pci,
-			dev->upstream_riscbuf_size, &dma_addr);
-	dev->_dma_virt_start_addr = dev->_dma_virt_addr;
-	dev->_dma_phys_start_addr = dma_addr;
-	dev->_dma_phys_addr = dma_addr;
-	dev->_risc_size = dev->upstream_riscbuf_size;
+	out->_dma_virt_addr = pci_alloc_consistent(dev->pci,
+			out->upstream_riscbuf_size, &dma_addr);
+	out->_dma_virt_start_addr = out->_dma_virt_addr;
+	out->_dma_phys_start_addr = dma_addr;
+	out->_dma_phys_addr = dma_addr;
+	out->_risc_size = out->upstream_riscbuf_size;
 
-	if (!dev->_dma_virt_addr) {
+	if (!out->_dma_virt_addr) {
 		pr_err("FAILED to allocate memory for Risc buffer! Returning\n");
 		return -ENOMEM;
 	}
 
 	/* Clear memory at address */
-	memset(dev->_dma_virt_addr, 0, dev->_risc_size);
+	memset(out->_dma_virt_addr, 0, out->_risc_size);
 
-	if (dev->_data_buf_virt_addr != NULL)
-		pci_free_consistent(dev->pci, dev->upstream_databuf_size,
-				dev->_data_buf_virt_addr,
-				dev->_data_buf_phys_addr);
+	if (out->_data_buf_virt_addr != NULL)
+		pci_free_consistent(dev->pci, out->upstream_databuf_size,
+				out->_data_buf_virt_addr,
+				out->_data_buf_phys_addr);
 	/* For Video Data buffer allocation */
-	dev->_data_buf_virt_addr = pci_alloc_consistent(dev->pci,
-			dev->upstream_databuf_size, &data_dma_addr);
-	dev->_data_buf_phys_addr = data_dma_addr;
-	dev->_data_buf_size = dev->upstream_databuf_size;
+	out->_data_buf_virt_addr = pci_alloc_consistent(dev->pci,
+			out->upstream_databuf_size, &data_dma_addr);
+	out->_data_buf_phys_addr = data_dma_addr;
+	out->_data_buf_size = out->upstream_databuf_size;
 
-	if (!dev->_data_buf_virt_addr) {
+	if (!out->_data_buf_virt_addr) {
 		pr_err("FAILED to allocate memory for data buffer! Returning\n");
 		return -ENOMEM;
 	}
 
 	/* Clear memory at address */
-	memset(dev->_data_buf_virt_addr, 0, dev->_data_buf_size);
-
-	ret = cx25821_openfile(dev, sram_ch);
-	if (ret < 0)
-		return ret;
+	memset(out->_data_buf_virt_addr, 0, out->_data_buf_size);
 
 	/* Create RISC programs */
-	ret = cx25821_risc_buffer_upstream(dev, dev->pci, 0, bpl,
-			dev->_lines_count);
+	ret = cx25821_risc_buffer_upstream(chan, dev->pci, 0, bpl,
+			out->_lines_count);
 	if (ret < 0) {
 		pr_info("Failed creating Video Upstream Risc programs!\n");
 		goto error;
@@ -551,11 +403,12 @@ error:
 	return ret;
 }
 
-static int cx25821_video_upstream_irq(struct cx25821_dev *dev, int chan_num,
-				      u32 status)
+static int cx25821_video_upstream_irq(struct cx25821_channel *chan, u32 status)
 {
+	struct cx25821_video_out_data *out = chan->out;
+	struct cx25821_dev *dev = chan->dev;
 	u32 int_msk_tmp;
-	struct sram_channel *channel = dev->channels[chan_num].sram_channels;
+	const struct sram_channel *channel = chan->sram_channels;
 	int singlefield_lines = NTSC_FIELD_HEIGHT;
 	int line_size_in_bytes = Y422_LINE_SZ;
 	int odd_risc_prog_size = 0;
@@ -572,16 +425,16 @@ static int cx25821_video_upstream_irq(struct cx25821_dev *dev, int chan_num,
 		cx_write(channel->int_msk, int_msk_tmp & ~_intr_msk);
 		cx_write(channel->int_stat, _intr_msk);
 
-		spin_lock(&dev->slock);
+		wake_up(&out->waitq);
 
-		dev->_frame_index = prog_cnt;
+		spin_lock(&dev->slock);
 
-		queue_work(dev->_irq_queues, &dev->_irq_work_entry);
+		out->_frame_index = prog_cnt;
 
-		if (dev->_is_first_frame) {
-			dev->_is_first_frame = 0;
+		if (out->_is_first_frame) {
+			out->_is_first_frame = 0;
 
-			if (dev->_isNTSC) {
+			if (out->is_60hz) {
 				singlefield_lines += 1;
 				odd_risc_prog_size = ODD_FLD_NTSC_PROG_SIZE;
 			} else {
@@ -589,17 +442,17 @@ static int cx25821_video_upstream_irq(struct cx25821_dev *dev, int chan_num,
 				odd_risc_prog_size = ODD_FLD_PAL_PROG_SIZE;
 			}
 
-			if (dev->_dma_virt_start_addr != NULL) {
+			if (out->_dma_virt_start_addr != NULL) {
 				line_size_in_bytes =
-				    (dev->_pixel_format ==
+				    (out->_pixel_format ==
 				     PIXEL_FRMT_411) ? Y411_LINE_SZ :
 				    Y422_LINE_SZ;
 				risc_phys_jump_addr =
-				    dev->_dma_phys_start_addr +
+				    out->_dma_phys_start_addr +
 				    odd_risc_prog_size;
 
-				rp = cx25821_update_riscprogram(dev,
-					dev->_dma_virt_start_addr, TOP_OFFSET,
+				rp = cx25821_update_riscprogram(chan,
+					out->_dma_virt_start_addr, TOP_OFFSET,
 					line_size_in_bytes, 0x0,
 					singlefield_lines, FIFO_DISABLE,
 					ODD_FIELD);
@@ -626,8 +479,8 @@ static int cx25821_video_upstream_irq(struct cx25821_dev *dev, int chan_num,
 			       __func__);
 	}
 
-	if (dev->_file_status == END_OF_FILE) {
-		pr_err("EOF Channel 1 Framecount = %d\n", dev->_frame_count);
+	if (out->_file_status == END_OF_FILE) {
+		pr_err("EOF Channel 1 Framecount = %d\n", out->_frame_count);
 		return -1;
 	}
 	/* ElSE, set the interrupt mask register, re-enable irq. */
@@ -639,47 +492,41 @@ static int cx25821_video_upstream_irq(struct cx25821_dev *dev, int chan_num,
 
 static irqreturn_t cx25821_upstream_irq(int irq, void *dev_id)
 {
-	struct cx25821_dev *dev = dev_id;
+	struct cx25821_channel *chan = dev_id;
+	struct cx25821_dev *dev = chan->dev;
 	u32 vid_status;
 	int handled = 0;
-	int channel_num = 0;
-	struct sram_channel *sram_ch;
+	const struct sram_channel *sram_ch;
 
 	if (!dev)
 		return -1;
 
-	channel_num = VID_UPSTREAM_SRAM_CHANNEL_I;
-
-	sram_ch = dev->channels[channel_num].sram_channels;
+	sram_ch = chan->sram_channels;
 
 	vid_status = cx_read(sram_ch->int_stat);
 
 	/* Only deal with our interrupt */
 	if (vid_status)
-		handled = cx25821_video_upstream_irq(dev, channel_num,
-				vid_status);
-
-	if (handled < 0)
-		cx25821_stop_upstream_video_ch1(dev);
-	else
-		handled += handled;
+		handled = cx25821_video_upstream_irq(chan, vid_status);
 
 	return IRQ_RETVAL(handled);
 }
 
-static void cx25821_set_pixelengine(struct cx25821_dev *dev,
-				    struct sram_channel *ch,
+static void cx25821_set_pixelengine(struct cx25821_channel *chan,
+				    const struct sram_channel *ch,
 				    int pix_format)
 {
+	struct cx25821_video_out_data *out = chan->out;
+	struct cx25821_dev *dev = chan->dev;
 	int width = WIDTH_D1;
-	int height = dev->_lines_count;
+	int height = out->_lines_count;
 	int num_lines, odd_num_lines;
 	u32 value;
 	int vip_mode = OUTPUT_FRMT_656;
 
 	value = ((pix_format & 0x3) << 12) | (vip_mode & 0x7);
 	value &= 0xFFFFFFEF;
-	value |= dev->_isNTSC ? 0 : 0x10;
+	value |= out->is_60hz ? 0 : 0x10;
 	cx_write(ch->vid_fmt_ctl, value);
 
 	/* set number of active pixels in each line.
@@ -689,7 +536,7 @@ static void cx25821_set_pixelengine(struct cx25821_dev *dev,
 	num_lines = (height / 2) & 0x3FF;
 	odd_num_lines = num_lines;
 
-	if (dev->_isNTSC)
+	if (out->is_60hz)
 		odd_num_lines += 1;
 
 	value = (num_lines << 16) | odd_num_lines;
@@ -700,9 +547,11 @@ static void cx25821_set_pixelengine(struct cx25821_dev *dev,
 	cx_write(ch->vid_cdt_size, VID_CDT_SIZE >> 3);
 }
 
-static int cx25821_start_video_dma_upstream(struct cx25821_dev *dev,
-					    struct sram_channel *sram_ch)
+static int cx25821_start_video_dma_upstream(struct cx25821_channel *chan,
+					    const struct sram_channel *sram_ch)
 {
+	struct cx25821_video_out_data *out = chan->out;
+	struct cx25821_dev *dev = chan->dev;
 	u32 tmp = 0;
 	int err = 0;
 
@@ -715,7 +564,7 @@ static int cx25821_start_video_dma_upstream(struct cx25821_dev *dev,
 	/* Set the physical start address of the RISC program in the initial
 	 * program counter(IPC) member of the cmds.
 	 */
-	cx_write(sram_ch->cmds_start + 0, dev->_dma_phys_addr);
+	cx_write(sram_ch->cmds_start + 0, out->_dma_phys_addr);
 	/* Risc IPC High 64 bits 63-32 */
 	cx_write(sram_ch->cmds_start + 4, 0);
 
@@ -731,7 +580,7 @@ static int cx25821_start_video_dma_upstream(struct cx25821_dev *dev,
 	cx_write(sram_ch->int_msk, tmp |= _intr_msk);
 
 	err = request_irq(dev->pci->irq, cx25821_upstream_irq,
-			IRQF_SHARED, dev->name, dev);
+			IRQF_SHARED, dev->name, chan);
 	if (err < 0) {
 		pr_err("%s: can't get upstream IRQ %d\n",
 		       dev->name, dev->pci->irq);
@@ -742,8 +591,8 @@ static int cx25821_start_video_dma_upstream(struct cx25821_dev *dev,
 	tmp = cx_read(sram_ch->dma_ctl);
 	cx_set(sram_ch->dma_ctl, tmp | FLD_VID_RISC_EN);
 
-	dev->_is_running = 1;
-	dev->_is_first_frame = 1;
+	out->_is_running = 1;
+	out->_is_first_frame = 1;
 
 	return 0;
 
@@ -752,107 +601,71 @@ fail_irq:
 	return err;
 }
 
-int cx25821_vidupstream_init_ch1(struct cx25821_dev *dev, int channel_select,
+int cx25821_vidupstream_init(struct cx25821_channel *chan,
 				 int pixel_format)
 {
-	struct sram_channel *sram_ch;
+	struct cx25821_video_out_data *out = chan->out;
+	struct cx25821_dev *dev = chan->dev;
+	const struct sram_channel *sram_ch;
 	u32 tmp;
 	int err = 0;
 	int data_frame_size = 0;
 	int risc_buffer_size = 0;
-	int str_length = 0;
 
-	if (dev->_is_running) {
+	if (out->_is_running) {
 		pr_info("Video Channel is still running so return!\n");
 		return 0;
 	}
 
-	dev->_channel_upstream_select = channel_select;
-	sram_ch = dev->channels[channel_select].sram_channels;
+	sram_ch = chan->sram_channels;
 
-	INIT_WORK(&dev->_irq_work_entry, cx25821_vidups_handler);
-	dev->_irq_queues = create_singlethread_workqueue("cx25821_workqueue");
+	out->is_60hz = dev->tvnorm & V4L2_STD_525_60;
 
-	if (!dev->_irq_queues) {
-		pr_err("create_singlethread_workqueue() for Video FAILED!\n");
-		return -ENOMEM;
-	}
 	/* 656/VIP SRC Upstream Channel I & J and 7 - Host Bus Interface for
 	 * channel A-C
 	 */
 	tmp = cx_read(VID_CH_MODE_SEL);
 	cx_write(VID_CH_MODE_SEL, tmp | 0x1B0001FF);
 
-	dev->_is_running = 0;
-	dev->_frame_count = 0;
-	dev->_file_status = RESET_STATUS;
-	dev->_lines_count = dev->_isNTSC ? 480 : 576;
-	dev->_pixel_format = pixel_format;
-	dev->_line_size = (dev->_pixel_format == PIXEL_FRMT_422) ?
+	out->_is_running = 0;
+	out->_frame_count = 0;
+	out->_file_status = RESET_STATUS;
+	out->_lines_count = out->is_60hz ? 480 : 576;
+	out->_pixel_format = pixel_format;
+	out->_line_size = (out->_pixel_format == PIXEL_FRMT_422) ?
 		(WIDTH_D1 * 2) : (WIDTH_D1 * 3) / 2;
-	data_frame_size = dev->_isNTSC ? NTSC_DATA_BUF_SZ : PAL_DATA_BUF_SZ;
-	risc_buffer_size = dev->_isNTSC ?
+	data_frame_size = out->is_60hz ? NTSC_DATA_BUF_SZ : PAL_DATA_BUF_SZ;
+	risc_buffer_size = out->is_60hz ?
 		NTSC_RISC_BUF_SIZE : PAL_RISC_BUF_SIZE;
 
-	if (dev->input_filename) {
-		str_length = strlen(dev->input_filename);
-		dev->_filename = kmemdup(dev->input_filename, str_length + 1,
-					 GFP_KERNEL);
-
-		if (!dev->_filename) {
-			err = -ENOENT;
-			goto error;
-		}
-	} else {
-		str_length = strlen(dev->_defaultname);
-		dev->_filename = kmemdup(dev->_defaultname, str_length + 1,
-					 GFP_KERNEL);
-
-		if (!dev->_filename) {
-			err = -ENOENT;
-			goto error;
-		}
-	}
-
-	/* Default if filename is empty string */
-	if (strcmp(dev->_filename, "") == 0) {
-		if (dev->_isNTSC) {
-			dev->_filename =
-				(dev->_pixel_format == PIXEL_FRMT_411) ?
-				"/root/vid411.yuv" : "/root/vidtest.yuv";
-		} else {
-			dev->_filename =
-				(dev->_pixel_format == PIXEL_FRMT_411) ?
-				"/root/pal411.yuv" : "/root/pal422.yuv";
-		}
-	}
-
-	dev->_is_running = 0;
-	dev->_frame_count = 0;
-	dev->_file_status = RESET_STATUS;
-	dev->_lines_count = dev->_isNTSC ? 480 : 576;
-	dev->_pixel_format = pixel_format;
-	dev->_line_size = (dev->_pixel_format == PIXEL_FRMT_422) ?
+	out->_is_running = 0;
+	out->_frame_count = 0;
+	out->_file_status = RESET_STATUS;
+	out->_lines_count = out->is_60hz ? 480 : 576;
+	out->_pixel_format = pixel_format;
+	out->_line_size = (out->_pixel_format == PIXEL_FRMT_422) ?
 		(WIDTH_D1 * 2) : (WIDTH_D1 * 3) / 2;
+	out->curpos = 0;
+	init_waitqueue_head(&out->waitq);
 
 	err = cx25821_sram_channel_setup_upstream(dev, sram_ch,
-			dev->_line_size, 0);
+			out->_line_size, 0);
 
 	/* setup fifo + format */
-	cx25821_set_pixelengine(dev, sram_ch, dev->_pixel_format);
+	cx25821_set_pixelengine(chan, sram_ch, out->_pixel_format);
 
-	dev->upstream_riscbuf_size = risc_buffer_size * 2;
-	dev->upstream_databuf_size = data_frame_size * 2;
+	out->upstream_riscbuf_size = risc_buffer_size * 2;
+	out->upstream_databuf_size = data_frame_size * 2;
 
 	/* Allocating buffers and prepare RISC program */
-	err = cx25821_upstream_buffer_prepare(dev, sram_ch, dev->_line_size);
+	err = cx25821_upstream_buffer_prepare(chan, sram_ch, out->_line_size);
 	if (err < 0) {
 		pr_err("%s: Failed to set up Video upstream buffers!\n",
 		       dev->name);
 		goto error;
 	}
 
-	cx25821_start_video_dma_upstream(dev, sram_ch);
+	cx25821_start_video_dma_upstream(chan, sram_ch);
 
 	return 0;
 
diff --git a/drivers/media/pci/cx25821/cx25821-video.c b/drivers/media/pci/cx25821/cx25821-video.c
index 31ce7698acb9..d270819fd875 100644
--- a/drivers/media/pci/cx25821/cx25821-video.c
+++ b/drivers/media/pci/cx25821/cx25821-video.c
@@ -33,13 +33,10 @@ MODULE_AUTHOR("Hiep Huynh <hiep.huynh@conexant.com>");
 MODULE_LICENSE("GPL");
 
 static unsigned int video_nr[] = {[0 ... (CX25821_MAXBOARDS - 1)] = UNSET };
-static unsigned int radio_nr[] = {[0 ... (CX25821_MAXBOARDS - 1)] = UNSET };
 
 module_param_array(video_nr, int, NULL, 0444);
-module_param_array(radio_nr, int, NULL, 0444);
 
 MODULE_PARM_DESC(video_nr, "video device numbers");
-MODULE_PARM_DESC(radio_nr, "radio device numbers");
 
 static unsigned int video_debug = VIDEO_DEBUG;
 module_param(video_debug, int, 0644);
@@ -49,24 +46,14 @@ static unsigned int irq_debug;
 module_param(irq_debug, int, 0644);
 MODULE_PARM_DESC(irq_debug, "enable debug messages [IRQ handler]");
 
-unsigned int vid_limit = 16;
+static unsigned int vid_limit = 16;
 module_param(vid_limit, int, 0644);
 MODULE_PARM_DESC(vid_limit, "capture memory limit in megabytes");
 
-static void cx25821_init_controls(struct cx25821_dev *dev, int chan_num);
-
-static const struct v4l2_file_operations video_fops;
-static const struct v4l2_ioctl_ops video_ioctl_ops;
-
 #define FORMAT_FLAGS_PACKED       0x01
 
-struct cx25821_fmt formats[] = {
+static const struct cx25821_fmt formats[] = {
 	{
-		.name = "8 bpp, gray",
-		.fourcc = V4L2_PIX_FMT_GREY,
-		.depth = 8,
-		.flags = FORMAT_FLAGS_PACKED,
-	 }, {
 		.name = "4:1:1, packed, Y41P",
 		.fourcc = V4L2_PIX_FMT_Y41P,
 		.depth = 12,
@@ -76,36 +63,16 @@ struct cx25821_fmt formats[] = {
 		.fourcc = V4L2_PIX_FMT_YUYV,
 		.depth = 16,
 		.flags = FORMAT_FLAGS_PACKED,
-	}, {
-		.name = "4:2:2, packed, UYVY",
-		.fourcc = V4L2_PIX_FMT_UYVY,
-		.depth = 16,
-		.flags = FORMAT_FLAGS_PACKED,
-	}, {
-		.name = "4:2:0, YUV",
-		.fourcc = V4L2_PIX_FMT_YUV420,
-		.depth = 12,
-		.flags = FORMAT_FLAGS_PACKED,
 	},
 };
 
-int cx25821_get_format_size(void)
-{
-	return ARRAY_SIZE(formats);
-}
-
-struct cx25821_fmt *cx25821_format_by_fourcc(unsigned int fourcc)
+static const struct cx25821_fmt *cx25821_format_by_fourcc(unsigned int fourcc)
 {
 	unsigned int i;
 
-	if (fourcc == V4L2_PIX_FMT_Y41P || fourcc == V4L2_PIX_FMT_YUV411P)
-		return formats + 1;
-
 	for (i = 0; i < ARRAY_SIZE(formats); i++)
 		if (formats[i].fourcc == fourcc)
 			return formats + i;
-
-	pr_err("%s(0x%08x) NOT FOUND\n", __func__, fourcc);
 	return NULL;
 }
 
@@ -144,129 +111,10 @@ void cx25821_video_wakeup(struct cx25821_dev *dev, struct cx25821_dmaqueue *q,
 		pr_err("%s: %d buffers handled (should be 1)\n", __func__, bc);
 }
 
-#ifdef TUNER_FLAG
-int cx25821_set_tvnorm(struct cx25821_dev *dev, v4l2_std_id norm)
-{
-	dprintk(1, "%s(norm = 0x%08x) name: [%s]\n",
-		__func__, (unsigned int)norm, v4l2_norm_to_name(norm));
-
-	dev->tvnorm = norm;
-
-	/* Tell the internal A/V decoder */
-	cx25821_call_all(dev, core, s_std, norm);
-
-	return 0;
-}
-#endif
-
-struct video_device *cx25821_vdev_init(struct cx25821_dev *dev,
-				       struct pci_dev *pci,
-				       struct video_device *template,
-				       char *type)
-{
-	struct video_device *vfd;
-	dprintk(1, "%s()\n", __func__);
-
-	vfd = video_device_alloc();
-	if (NULL == vfd)
-		return NULL;
-	*vfd = *template;
-	vfd->v4l2_dev = &dev->v4l2_dev;
-	vfd->release = video_device_release;
-	snprintf(vfd->name, sizeof(vfd->name), "%s %s (%s)", dev->name, type,
-		 cx25821_boards[dev->board].name);
-	video_set_drvdata(vfd, dev);
-	return vfd;
-}
-
-/*
-static int cx25821_ctrl_query(struct v4l2_queryctrl *qctrl)
-{
-	int i;
-
-	if (qctrl->id < V4L2_CID_BASE || qctrl->id >= V4L2_CID_LASTP1)
-		return -EINVAL;
-	for (i = 0; i < CX25821_CTLS; i++)
-		if (cx25821_ctls[i].v.id == qctrl->id)
-			break;
-	if (i == CX25821_CTLS) {
-		*qctrl = no_ctl;
-		return 0;
-	}
-	*qctrl = cx25821_ctls[i].v;
-	return 0;
-}
-*/
-
-/* resource management */
-int cx25821_res_get(struct cx25821_dev *dev, struct cx25821_fh *fh,
-		    unsigned int bit)
-{
-	dprintk(1, "%s()\n", __func__);
-	if (fh->resources & bit)
-		/* have it already allocated */
-		return 1;
-
-	/* is it free? */
-	mutex_lock(&dev->lock);
-	if (dev->channels[fh->channel_id].resources & bit) {
-		/* no, someone else uses it */
-		mutex_unlock(&dev->lock);
-		return 0;
-	}
-	/* it's free, grab it */
-	fh->resources |= bit;
-	dev->channels[fh->channel_id].resources |= bit;
-	dprintk(1, "res: get %d\n", bit);
-	mutex_unlock(&dev->lock);
-	return 1;
-}
-
-int cx25821_res_check(struct cx25821_fh *fh, unsigned int bit)
-{
-	return fh->resources & bit;
-}
-
-int cx25821_res_locked(struct cx25821_fh *fh, unsigned int bit)
-{
-	return fh->dev->channels[fh->channel_id].resources & bit;
-}
-
-void cx25821_res_free(struct cx25821_dev *dev, struct cx25821_fh *fh,
-		      unsigned int bits)
-{
-	BUG_ON((fh->resources & bits) != bits);
-	dprintk(1, "%s()\n", __func__);
-
-	mutex_lock(&dev->lock);
-	fh->resources &= ~bits;
-	dev->channels[fh->channel_id].resources &= ~bits;
-	dprintk(1, "res: put %d\n", bits);
-	mutex_unlock(&dev->lock);
-}
-
-int cx25821_video_mux(struct cx25821_dev *dev, unsigned int input)
-{
-	struct v4l2_routing route;
-	memset(&route, 0, sizeof(route));
-
-	dprintk(1, "%s(): video_mux: %d [vmux=%d, gpio=0x%x,0x%x,0x%x,0x%x]\n",
-		__func__, input, INPUT(input)->vmux, INPUT(input)->gpio0,
-		INPUT(input)->gpio1, INPUT(input)->gpio2, INPUT(input)->gpio3);
-	dev->input = input;
-
-	route.input = INPUT(input)->vmux;
-
-	/* Tell the internal A/V decoder */
-	cx25821_call_all(dev, video, s_routing, INPUT(input)->vmux, 0, 0);
-
-	return 0;
-}
-
 int cx25821_start_video_dma(struct cx25821_dev *dev,
 			    struct cx25821_dmaqueue *q,
 			    struct cx25821_buffer *buf,
-			    struct sram_channel *channel)
+			    const struct sram_channel *channel)
 {
 	int tmp = 0;
 
@@ -293,7 +141,7 @@ int cx25821_start_video_dma(struct cx25821_dev *dev,
 
 static int cx25821_restart_video_queue(struct cx25821_dev *dev,
 				       struct cx25821_dmaqueue *q,
-				       struct sram_channel *channel)
+				       const struct sram_channel *channel)
 {
 	struct cx25821_buffer *buf, *prev;
 	struct list_head *item;
@@ -346,8 +194,8 @@ static void cx25821_vid_timeout(unsigned long data)
 {
 	struct cx25821_data *timeout_data = (struct cx25821_data *)data;
 	struct cx25821_dev *dev = timeout_data->dev;
-	struct sram_channel *channel = timeout_data->channel;
-	struct cx25821_dmaqueue *q = &dev->channels[channel->i].vidq;
+	const struct sram_channel *channel = timeout_data->channel;
+	struct cx25821_dmaqueue *q = &dev->channels[channel->i].dma_vidq;
 	struct cx25821_buffer *buf;
 	unsigned long flags;
 
@@ -373,7 +221,7 @@ int cx25821_video_irq(struct cx25821_dev *dev, int chan_num, u32 status)
 	u32 count = 0;
 	int handled = 0;
 	u32 mask;
-	struct sram_channel *channel = dev->channels[chan_num].sram_channels;
+	const struct sram_channel *channel = dev->channels[chan_num].sram_channels;
 
 	mask = cx_read(channel->int_msk);
 	if (0 == (status & mask))
@@ -393,7 +241,7 @@ int cx25821_video_irq(struct cx25821_dev *dev, int chan_num, u32 status)
 	if (status & FLD_VID_DST_RISC1) {
 		spin_lock(&dev->slock);
 		count = cx_read(channel->gpcnt);
-		cx25821_video_wakeup(dev, &dev->channels[channel->i].vidq,
+		cx25821_video_wakeup(dev, &dev->channels[channel->i].dma_vidq,
 				count);
 		spin_unlock(&dev->slock);
 		handled++;
@@ -404,122 +252,19 @@ int cx25821_video_irq(struct cx25821_dev *dev, int chan_num, u32 status)
 		dprintk(2, "stopper video\n");
 		spin_lock(&dev->slock);
 		cx25821_restart_video_queue(dev,
-				&dev->channels[channel->i].vidq, channel);
+				&dev->channels[channel->i].dma_vidq, channel);
 		spin_unlock(&dev->slock);
 		handled++;
 	}
 	return handled;
 }
 
-void cx25821_videoioctl_unregister(struct cx25821_dev *dev)
-{
-	if (dev->ioctl_dev) {
-		if (video_is_registered(dev->ioctl_dev))
-			video_unregister_device(dev->ioctl_dev);
-		else
-			video_device_release(dev->ioctl_dev);
-
-		dev->ioctl_dev = NULL;
-	}
-}
-
-void cx25821_video_unregister(struct cx25821_dev *dev, int chan_num)
-{
-	cx_clear(PCI_INT_MSK, 1);
-
-	if (dev->channels[chan_num].video_dev) {
-		if (video_is_registered(dev->channels[chan_num].video_dev))
-			video_unregister_device(
-					dev->channels[chan_num].video_dev);
-		else
-			video_device_release(
-					dev->channels[chan_num].video_dev);
-
-		dev->channels[chan_num].video_dev = NULL;
-
-		btcx_riscmem_free(dev->pci,
-				&dev->channels[chan_num].vidq.stopper);
-
-		pr_warn("device %d released!\n", chan_num);
-	}
-
-}
-
-int cx25821_video_register(struct cx25821_dev *dev)
-{
-	int err;
-	int i;
-
-	struct video_device cx25821_video_device = {
-		.name = "cx25821-video",
-		.fops = &video_fops,
-		.minor = -1,
-		.ioctl_ops = &video_ioctl_ops,
-		.tvnorms = CX25821_NORMS,
-		.current_norm = V4L2_STD_NTSC_M,
-	};
-
-	spin_lock_init(&dev->slock);
-
-	for (i = 0; i < VID_CHANNEL_NUM; ++i) {
-		cx25821_init_controls(dev, i);
-
-		cx25821_risc_stopper(dev->pci, &dev->channels[i].vidq.stopper,
-			dev->channels[i].sram_channels->dma_ctl, 0x11, 0);
-
-		dev->channels[i].sram_channels = &cx25821_sram_channels[i];
-		dev->channels[i].video_dev = NULL;
-		dev->channels[i].resources = 0;
-
-		cx_write(dev->channels[i].sram_channels->int_stat, 0xffffffff);
-
-		INIT_LIST_HEAD(&dev->channels[i].vidq.active);
-		INIT_LIST_HEAD(&dev->channels[i].vidq.queued);
-
-		dev->channels[i].timeout_data.dev = dev;
-		dev->channels[i].timeout_data.channel =
-			&cx25821_sram_channels[i];
-		dev->channels[i].vidq.timeout.function = cx25821_vid_timeout;
-		dev->channels[i].vidq.timeout.data =
-			(unsigned long)&dev->channels[i].timeout_data;
-		init_timer(&dev->channels[i].vidq.timeout);
-
-		/* register v4l devices */
-		dev->channels[i].video_dev = cx25821_vdev_init(dev, dev->pci,
-				&cx25821_video_device, "video");
-
-		err = video_register_device(dev->channels[i].video_dev,
-				VFL_TYPE_GRABBER, video_nr[dev->nr]);
-
-		if (err < 0)
-			goto fail_unreg;
-
-	}
-
-	/* set PCI interrupt */
-	cx_set(PCI_INT_MSK, 0xff);
-
-	/* initial device configuration */
-	mutex_lock(&dev->lock);
-#ifdef TUNER_FLAG
-	dev->tvnorm = cx25821_video_device.current_norm;
-	cx25821_set_tvnorm(dev, dev->tvnorm);
-#endif
-	mutex_unlock(&dev->lock);
-
-	return 0;
-
-fail_unreg:
-	cx25821_video_unregister(dev, i);
-	return err;
-}
-
-int cx25821_buffer_setup(struct videobuf_queue *q, unsigned int *count,
+static int cx25821_buffer_setup(struct videobuf_queue *q, unsigned int *count,
 		 unsigned int *size)
 {
-	struct cx25821_fh *fh = q->priv_data;
+	struct cx25821_channel *chan = q->priv_data;
 
-	*size = fh->fmt->depth * fh->width * fh->height >> 3;
+	*size = chan->fmt->depth * chan->width * chan->height >> 3;
 
 	if (0 == *count)
 		*count = 32;
@@ -530,35 +275,34 @@ int cx25821_buffer_setup(struct videobuf_queue *q, unsigned int *count,
 	return 0;
 }
 
-int cx25821_buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
+static int cx25821_buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
 		   enum v4l2_field field)
 {
-	struct cx25821_fh *fh = q->priv_data;
-	struct cx25821_dev *dev = fh->dev;
+	struct cx25821_channel *chan = q->priv_data;
+	struct cx25821_dev *dev = chan->dev;
 	struct cx25821_buffer *buf =
 		container_of(vb, struct cx25821_buffer, vb);
 	int rc, init_buffer = 0;
 	u32 line0_offset;
 	struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
 	int bpl_local = LINE_SIZE_D1;
-	int channel_opened = fh->channel_id;
 
-	BUG_ON(NULL == fh->fmt);
-	if (fh->width < 48 || fh->width > 720 ||
-	    fh->height < 32 || fh->height > 576)
+	BUG_ON(NULL == chan->fmt);
+	if (chan->width < 48 || chan->width > 720 ||
+	    chan->height < 32 || chan->height > 576)
 		return -EINVAL;
 
-	buf->vb.size = (fh->width * fh->height * fh->fmt->depth) >> 3;
+	buf->vb.size = (chan->width * chan->height * chan->fmt->depth) >> 3;
 
 	if (0 != buf->vb.baddr && buf->vb.bsize < buf->vb.size)
 		return -EINVAL;
 
-	if (buf->fmt != fh->fmt ||
-	    buf->vb.width != fh->width ||
-	    buf->vb.height != fh->height || buf->vb.field != field) {
-		buf->fmt = fh->fmt;
-		buf->vb.width = fh->width;
-		buf->vb.height = fh->height;
+	if (buf->fmt != chan->fmt ||
+	    buf->vb.width != chan->width ||
+	    buf->vb.height != chan->height || buf->vb.field != field) {
+		buf->fmt = chan->fmt;
+		buf->vb.width = chan->width;
+		buf->vb.height = chan->height;
 		buf->vb.field = field;
 		init_buffer = 1;
 	}
@@ -575,34 +319,21 @@ int cx25821_buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
 	dprintk(1, "init_buffer=%d\n", init_buffer);
 
 	if (init_buffer) {
-
-		channel_opened = dev->channel_opened;
-		if (channel_opened < 0 || channel_opened > 7)
-			channel_opened = 7;
-
-		if (dev->channels[channel_opened].pixel_formats ==
-				PIXEL_FRMT_411)
+		if (chan->pixel_formats == PIXEL_FRMT_411)
 			buf->bpl = (buf->fmt->depth * buf->vb.width) >> 3;
 		else
 			buf->bpl = (buf->fmt->depth >> 3) * (buf->vb.width);
 
-		if (dev->channels[channel_opened].pixel_formats ==
-				PIXEL_FRMT_411) {
+		if (chan->pixel_formats == PIXEL_FRMT_411) {
 			bpl_local = buf->bpl;
 		} else {
 			bpl_local = buf->bpl;   /* Default */
 
-			if (channel_opened >= 0 && channel_opened <= 7) {
-				if (dev->channels[channel_opened]
-						.use_cif_resolution) {
-					if (dev->tvnorm & V4L2_STD_PAL_BG ||
-					    dev->tvnorm & V4L2_STD_PAL_DK)
-						bpl_local = 352 << 1;
-					else
-						bpl_local = dev->channels[
-							channel_opened].
-							cif_width << 1;
-				}
+			if (chan->use_cif_resolution) {
+				if (dev->tvnorm & V4L2_STD_625_50)
+					bpl_local = 352 << 1;
+				else
+					bpl_local = chan->cif_width << 1;
 			}
 		}
 
@@ -645,8 +376,8 @@ int cx25821_buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
 	}
 
 	dprintk(2, "[%p/%d] buffer_prep - %dx%d %dbpp \"%s\" - dma=0x%08lx\n",
-		buf, buf->vb.i, fh->width, fh->height, fh->fmt->depth,
-		fh->fmt->name, (unsigned long)buf->risc.dma);
+		buf, buf->vb.i, chan->width, chan->height, chan->fmt->depth,
+		chan->fmt->name, (unsigned long)buf->risc.dma);
 
 	buf->vb.state = VIDEOBUF_PREPARED;
 
@@ -657,7 +388,7 @@ fail:
 	return rc;
 }
 
-void cx25821_buffer_release(struct videobuf_queue *q,
+static void cx25821_buffer_release(struct videobuf_queue *q,
 			    struct videobuf_buffer *vb)
 {
 	struct cx25821_buffer *buf =
@@ -666,33 +397,11 @@ void cx25821_buffer_release(struct videobuf_queue *q,
 	cx25821_free_buffer(q, buf);
 }
 
-struct videobuf_queue *get_queue(struct cx25821_fh *fh)
+static int cx25821_video_mmap(struct file *file, struct vm_area_struct *vma)
 {
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		return &fh->vidq;
-	default:
-		BUG();
-		return NULL;
-	}
-}
+	struct cx25821_channel *chan = video_drvdata(file);
 
-int cx25821_get_resource(struct cx25821_fh *fh, int resource)
-{
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		return resource;
-	default:
-		BUG();
-		return 0;
-	}
-}
-
-int cx25821_video_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct cx25821_fh *fh = file->private_data;
-
-	return videobuf_mmap_mapper(get_queue(fh), vma);
+	return videobuf_mmap_mapper(&chan->vidq, vma);
 }
 
 
@@ -701,9 +410,9 @@ static void buffer_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
 	struct cx25821_buffer *buf =
 		container_of(vb, struct cx25821_buffer, vb);
 	struct cx25821_buffer *prev;
-	struct cx25821_fh *fh = vq->priv_data;
-	struct cx25821_dev *dev = fh->dev;
-	struct cx25821_dmaqueue *q = &dev->channels[fh->channel_id].vidq;
+	struct cx25821_channel *chan = vq->priv_data;
+	struct cx25821_dev *dev = chan->dev;
+	struct cx25821_dmaqueue *q = &dev->channels[chan->id].dma_vidq;
 
 	/* add jump to stopper */
 	buf->risc.jmp[0] = cpu_to_le32(RISC_JUMP | RISC_IRQ1 | RISC_CNT_INC);
@@ -720,8 +429,7 @@ static void buffer_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
 
 	} else if (list_empty(&q->active)) {
 		list_add_tail(&buf->vb.queue, &q->active);
-		cx25821_start_video_dma(dev, q, buf,
-				dev->channels[fh->channel_id].sram_channels);
+		cx25821_start_video_dma(dev, q, buf, chan->sram_channels);
 		buf->vb.state = VIDEOBUF_ACTIVE;
 		buf->count = q->count++;
 		mod_timer(&q->timeout, jiffies + BUFFER_TIMEOUT);
@@ -762,1183 +470,487 @@ static struct videobuf_queue_ops cx25821_video_qops = {
 	.buf_release = cx25821_buffer_release,
 };
 
-static int video_open(struct file *file)
-{
-	struct video_device *vdev = video_devdata(file);
-	struct cx25821_dev *h, *dev = video_drvdata(file);
-	struct cx25821_fh *fh;
-	struct list_head *list;
-	int minor = video_devdata(file)->minor;
-	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	u32 pix_format;
-	int ch_id = 0;
-	int i;
-
-	dprintk(1, "open dev=%s type=%s\n", video_device_node_name(vdev),
-			v4l2_type_names[type]);
-
-	/* allocate + initialize per filehandle data */
-	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
-	if (NULL == fh)
-		return -ENOMEM;
-
-	mutex_lock(&cx25821_devlist_mutex);
-
-	list_for_each(list, &cx25821_devlist)
-	{
-		h = list_entry(list, struct cx25821_dev, devlist);
-
-		for (i = 0; i < MAX_VID_CHANNEL_NUM; i++) {
-			if (h->channels[i].video_dev &&
-			    h->channels[i].video_dev->minor == minor) {
-				dev = h;
-				ch_id = i;
-				type  = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-			}
-		}
-	}
-
-	if (NULL == dev) {
-		mutex_unlock(&cx25821_devlist_mutex);
-		kfree(fh);
-		return -ENODEV;
-	}
-
-	file->private_data = fh;
-	fh->dev = dev;
-	fh->type = type;
-	fh->width = 720;
-	fh->channel_id = ch_id;
-
-	if (dev->tvnorm & V4L2_STD_PAL_BG || dev->tvnorm & V4L2_STD_PAL_DK)
-		fh->height = 576;
-	else
-		fh->height = 480;
-
-	dev->channel_opened = fh->channel_id;
-	if (dev->channels[ch_id].pixel_formats == PIXEL_FRMT_411)
-		pix_format = V4L2_PIX_FMT_Y41P;
-	else
-		pix_format = V4L2_PIX_FMT_YUYV;
-	fh->fmt = cx25821_format_by_fourcc(pix_format);
-
-	v4l2_prio_open(&dev->channels[ch_id].prio, &fh->prio);
-
-	videobuf_queue_sg_init(&fh->vidq, &cx25821_video_qops, &dev->pci->dev,
-			&dev->slock, V4L2_BUF_TYPE_VIDEO_CAPTURE,
-			V4L2_FIELD_INTERLACED, sizeof(struct cx25821_buffer),
-			fh, NULL);
-
-	dprintk(1, "post videobuf_queue_init()\n");
-	mutex_unlock(&cx25821_devlist_mutex);
-
-	return 0;
-}
-
 static ssize_t video_read(struct file *file, char __user * data, size_t count,
 			 loff_t *ppos)
 {
-	struct cx25821_fh *fh = file->private_data;
-
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		if (cx25821_res_locked(fh, RESOURCE_VIDEO0))
-			return -EBUSY;
+	struct v4l2_fh *fh = file->private_data;
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
+	int err = 0;
 
-		return videobuf_read_one(&fh->vidq, data, count, ppos,
-					file->f_flags & O_NONBLOCK);
-
-	default:
-		BUG();
-		return 0;
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	if (chan->streaming_fh && chan->streaming_fh != fh) {
+		err = -EBUSY;
+		goto unlock;
 	}
+	chan->streaming_fh = fh;
+
+	err = videobuf_read_one(&chan->vidq, data, count, ppos,
+				file->f_flags & O_NONBLOCK);
+unlock:
+	mutex_unlock(&dev->lock);
+	return err;
 }
 
 static unsigned int video_poll(struct file *file,
 			      struct poll_table_struct *wait)
 {
-	struct cx25821_fh *fh = file->private_data;
-	struct cx25821_buffer *buf;
-
-	if (cx25821_res_check(fh, RESOURCE_VIDEO0)) {
-		/* streaming capture */
-		if (list_empty(&fh->vidq.stream))
-			return POLLERR;
-		buf = list_entry(fh->vidq.stream.next,
-				struct cx25821_buffer, vb.stream);
-	} else {
-		/* read() capture */
-		buf = (struct cx25821_buffer *)fh->vidq.read_buf;
-		if (NULL == buf)
-			return POLLERR;
-	}
-
-	poll_wait(file, &buf->vb.done, wait);
-	if (buf->vb.state == VIDEOBUF_DONE || buf->vb.state == VIDEOBUF_ERROR) {
-		if (buf->vb.state == VIDEOBUF_DONE) {
-			struct cx25821_dev *dev = fh->dev;
-
-			if (dev && dev->channels[fh->channel_id]
-					.use_cif_resolution) {
-				u8 cam_id = *((char *)buf->vb.baddr + 3);
-				memcpy((char *)buf->vb.baddr,
-				      (char *)buf->vb.baddr + (fh->width * 2),
-				      (fh->width * 2));
-				*((char *)buf->vb.baddr + 3) = cam_id;
-			}
+	struct cx25821_channel *chan = video_drvdata(file);
+	unsigned long req_events = poll_requested_events(wait);
+	unsigned int res = v4l2_ctrl_poll(file, wait);
+
+	if (req_events & (POLLIN | POLLRDNORM))
+		res |= videobuf_poll_stream(file, &chan->vidq, wait);
+	return res;
+
+	/* This doesn't belong in poll(). This can be done
+	 * much better with vb2. We keep this code here as a
+	 * reminder.
+	if ((res & POLLIN) && buf->vb.state == VIDEOBUF_DONE) {
+		struct cx25821_dev *dev = chan->dev;
+
+		if (dev && chan->use_cif_resolution) {
+			u8 cam_id = *((char *)buf->vb.baddr + 3);
+			memcpy((char *)buf->vb.baddr,
+					(char *)buf->vb.baddr + (chan->width * 2),
+					(chan->width * 2));
+			*((char *)buf->vb.baddr + 3) = cam_id;
 		}
-
-		return POLLIN | POLLRDNORM;
 	}
-
-	return 0;
+	 */
 }
 
 static int video_release(struct file *file)
 {
-	struct cx25821_fh *fh = file->private_data;
-	struct cx25821_dev *dev = fh->dev;
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct v4l2_fh *fh = file->private_data;
+	struct cx25821_dev *dev = chan->dev;
+	const struct sram_channel *sram_ch =
+		dev->channels[0].sram_channels;
 
+	mutex_lock(&dev->lock);
 	/* stop the risc engine and fifo */
-	cx_write(channel0->dma_ctl, 0); /* FIFO and RISC disable */
+	cx_write(sram_ch->dma_ctl, 0); /* FIFO and RISC disable */
 
 	/* stop video capture */
-	if (cx25821_res_check(fh, RESOURCE_VIDEO0)) {
-		videobuf_queue_cancel(&fh->vidq);
-		cx25821_res_free(dev, fh, RESOURCE_VIDEO0);
+	if (chan->streaming_fh == fh) {
+		videobuf_queue_cancel(&chan->vidq);
+		chan->streaming_fh = NULL;
 	}
 
-	if (fh->vidq.read_buf) {
-		cx25821_buffer_release(&fh->vidq, fh->vidq.read_buf);
-		kfree(fh->vidq.read_buf);
+	if (chan->vidq.read_buf) {
+		cx25821_buffer_release(&chan->vidq, chan->vidq.read_buf);
+		kfree(chan->vidq.read_buf);
 	}
 
-	videobuf_mmap_free(&fh->vidq);
-
-	v4l2_prio_close(&dev->channels[fh->channel_id].prio, fh->prio);
-	file->private_data = NULL;
-	kfree(fh);
+	videobuf_mmap_free(&chan->vidq);
+	mutex_unlock(&dev->lock);
 
-	return 0;
+	return v4l2_fh_release(file);
 }
 
-static int vidioc_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
-{
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = fh->dev;
+/* VIDEO IOCTLS */
 
-	if (unlikely(fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE))
+static int cx25821_vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
+			    struct v4l2_fmtdesc *f)
+{
+	if (unlikely(f->index >= ARRAY_SIZE(formats)))
 		return -EINVAL;
 
-	if (unlikely(i != fh->type))
-		return -EINVAL;
+	strlcpy(f->description, formats[f->index].name, sizeof(f->description));
+	f->pixelformat = formats[f->index].fourcc;
 
-	if (unlikely(!cx25821_res_get(dev, fh, cx25821_get_resource(fh,
-						RESOURCE_VIDEO0))))
-		return -EBUSY;
+	return 0;
+}
+
+static int cx25821_vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+				 struct v4l2_format *f)
+{
+	struct cx25821_channel *chan = video_drvdata(file);
+
+	f->fmt.pix.width = chan->width;
+	f->fmt.pix.height = chan->height;
+	f->fmt.pix.field = chan->vidq.field;
+	f->fmt.pix.pixelformat = chan->fmt->fourcc;
+	f->fmt.pix.bytesperline = (chan->width * chan->fmt->depth) >> 3;
+	f->fmt.pix.sizeimage = chan->height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
 
-	return videobuf_streamon(get_queue(fh));
+	return 0;
 }
 
-static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
+static int cx25821_vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+				   struct v4l2_format *f)
 {
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = fh->dev;
-	int err, res;
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
+	const struct cx25821_fmt *fmt;
+	enum v4l2_field field = f->fmt.pix.field;
+	unsigned int maxh;
+	unsigned w;
 
-	if (fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-	if (i != fh->type)
+	fmt = cx25821_format_by_fourcc(f->fmt.pix.pixelformat);
+	if (NULL == fmt)
 		return -EINVAL;
+	maxh = (dev->tvnorm & V4L2_STD_625_50) ? 576 : 480;
+
+	w = f->fmt.pix.width;
+	if (field != V4L2_FIELD_BOTTOM)
+		field = V4L2_FIELD_TOP;
+	if (w < 352) {
+		w = 176;
+		f->fmt.pix.height = maxh / 4;
+	} else if (w < 720) {
+		w = 352;
+		f->fmt.pix.height = maxh / 2;
+	} else {
+		w = 720;
+		f->fmt.pix.height = maxh;
+		field = V4L2_FIELD_INTERLACED;
+	}
+	f->fmt.pix.field = field;
+	f->fmt.pix.width = w;
+	f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
+	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
 
-	res = cx25821_get_resource(fh, RESOURCE_VIDEO0);
-	err = videobuf_streamoff(get_queue(fh));
-	if (err < 0)
-		return err;
-	cx25821_res_free(dev, fh, res);
 	return 0;
 }
 
 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	struct v4l2_mbus_framefmt mbus_fmt;
-	int err;
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
 	int pix_format = PIXEL_FRMT_422;
+	int err;
 
-	if (fh) {
-		err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
-				      fh->prio);
-		if (0 != err)
-			return err;
-	}
-
-	dprintk(2, "%s()\n", __func__);
 	err = cx25821_vidioc_try_fmt_vid_cap(file, priv, f);
 
 	if (0 != err)
 		return err;
 
-	fh->fmt = cx25821_format_by_fourcc(f->fmt.pix.pixelformat);
-	fh->vidq.field = f->fmt.pix.field;
-
-	/* check if width and height is valid based on set standard */
-	if (cx25821_is_valid_width(f->fmt.pix.width, dev->tvnorm))
-		fh->width = f->fmt.pix.width;
-
-	if (cx25821_is_valid_height(f->fmt.pix.height, dev->tvnorm))
-		fh->height = f->fmt.pix.height;
+	chan->fmt = cx25821_format_by_fourcc(f->fmt.pix.pixelformat);
+	chan->vidq.field = f->fmt.pix.field;
+	chan->width = f->fmt.pix.width;
+	chan->height = f->fmt.pix.height;
 
 	if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_Y41P)
 		pix_format = PIXEL_FRMT_411;
-	else if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV)
-		pix_format = PIXEL_FRMT_422;
 	else
-		return -EINVAL;
+		pix_format = PIXEL_FRMT_422;
 
 	cx25821_set_pixel_format(dev, SRAM_CH00, pix_format);
 
 	/* check if cif resolution */
-	if (fh->width == 320 || fh->width == 352)
-		dev->channels[fh->channel_id].use_cif_resolution = 1;
+	if (chan->width == 320 || chan->width == 352)
+		chan->use_cif_resolution = 1;
 	else
-		dev->channels[fh->channel_id].use_cif_resolution = 0;
-
-	dev->channels[fh->channel_id].cif_width = fh->width;
-	medusa_set_resolution(dev, fh->width, SRAM_CH00);
-
-	dprintk(2, "%s(): width=%d height=%d field=%d\n", __func__, fh->width,
-		fh->height, fh->vidq.field);
-	v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, V4L2_MBUS_FMT_FIXED);
-	cx25821_call_all(dev, video, s_mbus_fmt, &mbus_fmt);
+		chan->use_cif_resolution = 0;
 
+	chan->cif_width = chan->width;
+	medusa_set_resolution(dev, chan->width, SRAM_CH00);
 	return 0;
 }
 
-static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+static int vidioc_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
 {
-	int ret_val = 0;
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+	struct cx25821_channel *chan = video_drvdata(file);
 
-	ret_val = videobuf_dqbuf(get_queue(fh), p, file->f_flags & O_NONBLOCK);
+	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
 
-	p->sequence = dev->channels[fh->channel_id].vidq.count;
+	if (chan->streaming_fh && chan->streaming_fh != priv)
+		return -EBUSY;
+	chan->streaming_fh = priv;
 
-	return ret_val;
+	return videobuf_streamon(&chan->vidq);
 }
 
-static int vidioc_log_status(struct file *file, void *priv)
+static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
 {
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	struct cx25821_fh *fh = priv;
-	char name[32 + 2];
+	struct cx25821_channel *chan = video_drvdata(file);
 
-	struct sram_channel *sram_ch = dev->channels[fh->channel_id]
-								.sram_channels;
-	u32 tmp = 0;
-
-	snprintf(name, sizeof(name), "%s/2", dev->name);
-	pr_info("%s/2: ============  START LOG STATUS  ============\n",
-		dev->name);
-	cx25821_call_all(dev, core, log_status);
-	tmp = cx_read(sram_ch->dma_ctl);
-	pr_info("Video input 0 is %s\n",
-		(tmp & 0x11) ? "streaming" : "stopped");
-	pr_info("%s/2: =============  END LOG STATUS  =============\n",
-		dev->name);
-	return 0;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *priv,
-			struct v4l2_control *ctl)
-{
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	int err;
+	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
 
-	if (fh) {
-		err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
-				      fh->prio);
-		if (0 != err)
-			return err;
-	}
+	if (chan->streaming_fh && chan->streaming_fh != priv)
+		return -EBUSY;
+	if (chan->streaming_fh == NULL)
+		return 0;
 
-	return cx25821_set_control(dev, ctl, fh->channel_id);
+	chan->streaming_fh = NULL;
+	return videobuf_streamoff(&chan->vidq);
 }
 
-/* VIDEO IOCTLS */
-int cx25821_vidioc_g_fmt_vid_cap(struct file *file, void *priv,
-				 struct v4l2_format *f)
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
 {
-	struct cx25821_fh *fh = priv;
+	int ret_val = 0;
+	struct cx25821_channel *chan = video_drvdata(file);
 
-	f->fmt.pix.width = fh->width;
-	f->fmt.pix.height = fh->height;
-	f->fmt.pix.field = fh->vidq.field;
-	f->fmt.pix.pixelformat = fh->fmt->fourcc;
-	f->fmt.pix.bytesperline = (f->fmt.pix.width * fh->fmt->depth) >> 3;
-	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	ret_val = videobuf_dqbuf(&chan->vidq, p, file->f_flags & O_NONBLOCK);
+	p->sequence = chan->dma_vidq.count;
 
-	return 0;
+	return ret_val;
 }
 
-int cx25821_vidioc_try_fmt_vid_cap(struct file *file, void *priv,
-				   struct v4l2_format *f)
+static int vidioc_log_status(struct file *file, void *priv)
 {
-	struct cx25821_fmt *fmt;
-	enum v4l2_field field;
-	unsigned int maxw, maxh;
-
-	fmt = cx25821_format_by_fourcc(f->fmt.pix.pixelformat);
-	if (NULL == fmt)
-		return -EINVAL;
-
-	field = f->fmt.pix.field;
-	maxw = 720;
-	maxh = 576;
-
-	if (V4L2_FIELD_ANY == field) {
-		if (f->fmt.pix.height > maxh / 2)
-			field = V4L2_FIELD_INTERLACED;
-		else
-			field = V4L2_FIELD_TOP;
-	}
-
-	switch (field) {
-	case V4L2_FIELD_TOP:
-	case V4L2_FIELD_BOTTOM:
-		maxh = maxh / 2;
-		break;
-	case V4L2_FIELD_INTERLACED:
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	f->fmt.pix.field = field;
-	if (f->fmt.pix.height < 32)
-		f->fmt.pix.height = 32;
-	if (f->fmt.pix.height > maxh)
-		f->fmt.pix.height = maxh;
-	if (f->fmt.pix.width < 48)
-		f->fmt.pix.width = 48;
-	if (f->fmt.pix.width > maxw)
-		f->fmt.pix.width = maxw;
-	f->fmt.pix.width &= ~0x03;
-	f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
-	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
+	const struct sram_channel *sram_ch = chan->sram_channels;
+	u32 tmp = 0;
 
+	tmp = cx_read(sram_ch->dma_ctl);
+	pr_info("Video input 0 is %s\n",
+		(tmp & 0x11) ? "streaming" : "stopped");
 	return 0;
 }
 
-int cx25821_vidioc_querycap(struct file *file, void *priv,
+
+static int cx25821_vidioc_querycap(struct file *file, void *priv,
 			    struct v4l2_capability *cap)
 {
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
+	const u32 cap_input = V4L2_CAP_VIDEO_CAPTURE |
+			V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+	const u32 cap_output = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_READWRITE;
 
 	strcpy(cap->driver, "cx25821");
 	strlcpy(cap->card, cx25821_boards[dev->board].name, sizeof(cap->card));
 	sprintf(cap->bus_info, "PCIe:%s", pci_name(dev->pci));
-	cap->version = CX25821_VERSION_CODE;
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
-		V4L2_CAP_STREAMING;
-	if (UNSET != dev->tuner_type)
-		cap->capabilities |= V4L2_CAP_TUNER;
-	return 0;
-}
-
-int cx25821_vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
-			    struct v4l2_fmtdesc *f)
-{
-	if (unlikely(f->index >= ARRAY_SIZE(formats)))
-		return -EINVAL;
-
-	strlcpy(f->description, formats[f->index].name, sizeof(f->description));
-	f->pixelformat = formats[f->index].fourcc;
-
+	if (chan->id >= VID_CHANNEL_NUM)
+		cap->device_caps = cap_output;
+	else
+		cap->device_caps = cap_input;
+	cap->capabilities = cap_input | cap_output | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
-int cx25821_vidioc_reqbufs(struct file *file, void *priv,
+static int cx25821_vidioc_reqbufs(struct file *file, void *priv,
 			   struct v4l2_requestbuffers *p)
 {
-	struct cx25821_fh *fh = priv;
-	return videobuf_reqbufs(get_queue(fh), p);
+	struct cx25821_channel *chan = video_drvdata(file);
+
+	return videobuf_reqbufs(&chan->vidq, p);
 }
 
-int cx25821_vidioc_querybuf(struct file *file, void *priv,
+static int cx25821_vidioc_querybuf(struct file *file, void *priv,
 			    struct v4l2_buffer *p)
 {
-	struct cx25821_fh *fh = priv;
-	return videobuf_querybuf(get_queue(fh), p);
-}
+	struct cx25821_channel *chan = video_drvdata(file);
 
-int cx25821_vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
-{
-	struct cx25821_fh *fh = priv;
-	return videobuf_qbuf(get_queue(fh), p);
+	return videobuf_querybuf(&chan->vidq, p);
 }
 
-int cx25821_vidioc_g_priority(struct file *file, void *f, enum v4l2_priority *p)
+static int cx25821_vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
 {
-	struct cx25821_dev *dev = ((struct cx25821_fh *)f)->dev;
-	struct cx25821_fh *fh = f;
+	struct cx25821_channel *chan = video_drvdata(file);
 
-	*p = v4l2_prio_max(&dev->channels[fh->channel_id].prio);
-
-	return 0;
+	return videobuf_qbuf(&chan->vidq, p);
 }
 
-int cx25821_vidioc_s_priority(struct file *file, void *f,
-			      enum v4l2_priority prio)
+static int cx25821_vidioc_g_std(struct file *file, void *priv, v4l2_std_id *tvnorms)
 {
-	struct cx25821_fh *fh = f;
-	struct cx25821_dev *dev = ((struct cx25821_fh *)f)->dev;
+	struct cx25821_channel *chan = video_drvdata(file);
 
-	return v4l2_prio_change(&dev->channels[fh->channel_id].prio, &fh->prio,
-			prio);
+	*tvnorms = chan->dev->tvnorm;
+	return 0;
 }
 
-#ifdef TUNER_FLAG
-int cx25821_vidioc_s_std(struct file *file, void *priv, v4l2_std_id * tvnorms)
+static int cx25821_vidioc_s_std(struct file *file, void *priv,
+				v4l2_std_id tvnorms)
 {
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	int err;
-
-	dprintk(1, "%s()\n", __func__);
-
-	if (fh) {
-		err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
-				      fh->prio);
-		if (0 != err)
-			return err;
-	}
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
 
-	if (dev->tvnorm == *tvnorms)
+	if (dev->tvnorm == tvnorms)
 		return 0;
 
-	mutex_lock(&dev->lock);
-	cx25821_set_tvnorm(dev, *tvnorms);
-	mutex_unlock(&dev->lock);
+	dev->tvnorm = tvnorms;
+	chan->width = 720;
+	chan->height = (dev->tvnorm & V4L2_STD_625_50) ? 576 : 480;
 
 	medusa_set_videostandard(dev);
 
 	return 0;
 }
-#endif
 
-int cx25821_enum_input(struct cx25821_dev *dev, struct v4l2_input *i)
+static int cx25821_vidioc_enum_input(struct file *file, void *priv,
+			      struct v4l2_input *i)
 {
-	static const char * const iname[] = {
-		[CX25821_VMUX_COMPOSITE] = "Composite",
-		[CX25821_VMUX_SVIDEO] = "S-Video",
-		[CX25821_VMUX_DEBUG] = "for debug only",
-	};
-	unsigned int n;
-	dprintk(1, "%s()\n", __func__);
-
-	n = i->index;
-	if (n >= 2)
-		return -EINVAL;
-
-	if (0 == INPUT(n)->type)
+	if (i->index)
 		return -EINVAL;
 
 	i->type = V4L2_INPUT_TYPE_CAMERA;
-	strcpy(i->name, iname[INPUT(n)->type]);
-
 	i->std = CX25821_NORMS;
+	strcpy(i->name, "Composite");
 	return 0;
 }
 
-int cx25821_vidioc_enum_input(struct file *file, void *priv,
-			      struct v4l2_input *i)
-{
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	dprintk(1, "%s()\n", __func__);
-	return cx25821_enum_input(dev, i);
-}
-
-int cx25821_vidioc_g_input(struct file *file, void *priv, unsigned int *i)
-{
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-
-	*i = dev->input;
-	dprintk(1, "%s(): returns %d\n", __func__, *i);
-	return 0;
-}
-
-int cx25821_vidioc_s_input(struct file *file, void *priv, unsigned int i)
-{
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	int err;
-
-	dprintk(1, "%s(%d)\n", __func__, i);
-
-	if (fh) {
-		err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
-				      fh->prio);
-		if (0 != err)
-			return err;
-	}
-
-	if (i >= CX25821_NR_INPUT) {
-		dprintk(1, "%s(): -EINVAL\n", __func__);
-		return -EINVAL;
-	}
-
-	mutex_lock(&dev->lock);
-	cx25821_video_mux(dev, i);
-	mutex_unlock(&dev->lock);
-	return 0;
-}
-
-#ifdef TUNER_FLAG
-int cx25821_vidioc_g_frequency(struct file *file, void *priv,
-			       struct v4l2_frequency *f)
-{
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev = fh->dev;
-
-	f->frequency = dev->freq;
-
-	cx25821_call_all(dev, tuner, g_frequency, f);
-
-	return 0;
-}
-
-int cx25821_set_freq(struct cx25821_dev *dev, struct v4l2_frequency *f)
-{
-	mutex_lock(&dev->lock);
-	dev->freq = f->frequency;
-
-	cx25821_call_all(dev, tuner, s_frequency, f);
-
-	/* When changing channels it is required to reset TVAUDIO */
-	msleep(10);
-
-	mutex_unlock(&dev->lock);
-
-	return 0;
-}
-
-int cx25821_vidioc_s_frequency(struct file *file, void *priv,
-			       struct v4l2_frequency *f)
-{
-	struct cx25821_fh *fh = priv;
-	struct cx25821_dev *dev;
-	int err;
-
-	if (fh) {
-		dev = fh->dev;
-		err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
-				      fh->prio);
-		if (0 != err)
-			return err;
-	} else {
-		pr_err("Invalid fh pointer!\n");
-		return -EINVAL;
-	}
-
-	return cx25821_set_freq(dev, f);
-}
-#endif
-
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-int cx25821_vidioc_g_register(struct file *file, void *fh,
-		      struct v4l2_dbg_register *reg)
-{
-	struct cx25821_dev *dev = ((struct cx25821_fh *)fh)->dev;
-
-	if (!v4l2_chip_match_host(&reg->match))
-		return -EINVAL;
-
-	cx25821_call_all(dev, core, g_register, reg);
-
-	return 0;
-}
-
-int cx25821_vidioc_s_register(struct file *file, void *fh,
-		      struct v4l2_dbg_register *reg)
+static int cx25821_vidioc_g_input(struct file *file, void *priv, unsigned int *i)
 {
-	struct cx25821_dev *dev = ((struct cx25821_fh *)fh)->dev;
-
-	if (!v4l2_chip_match_host(&reg->match))
-		return -EINVAL;
-
-	cx25821_call_all(dev, core, s_register, reg);
-
+	*i = 0;
 	return 0;
 }
 
-#endif
-
-#ifdef TUNER_FLAG
-int cx25821_vidioc_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
+static int cx25821_vidioc_s_input(struct file *file, void *priv, unsigned int i)
 {
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-
-	if (unlikely(UNSET == dev->tuner_type))
-		return -EINVAL;
-	if (0 != t->index)
-		return -EINVAL;
-
-	strcpy(t->name, "Television");
-	t->type = V4L2_TUNER_ANALOG_TV;
-	t->capability = V4L2_TUNER_CAP_NORM;
-	t->rangehigh = 0xffffffffUL;
-
-	t->signal = 0xffff;	/* LOCKED */
-	return 0;
+	return i ? -EINVAL : 0;
 }
 
-int cx25821_vidioc_s_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
+static int cx25821_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	struct cx25821_fh *fh = priv;
-	int err;
-
-	if (fh) {
-		err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
-				      fh->prio);
-		if (0 != err)
-			return err;
-	}
+	struct cx25821_channel *chan =
+		container_of(ctrl->handler, struct cx25821_channel, hdl);
+	struct cx25821_dev *dev = chan->dev;
 
-	dprintk(1, "%s()\n", __func__);
-	if (UNSET == dev->tuner_type)
-		return -EINVAL;
-	if (0 != t->index)
-		return -EINVAL;
-
-	return 0;
-}
-
-#endif
-/*****************************************************************************/
-static const struct v4l2_queryctrl no_ctl = {
-	.name = "42",
-	.flags = V4L2_CTRL_FLAG_DISABLED,
-};
-
-static struct v4l2_queryctrl cx25821_ctls[] = {
-	/* --- video --- */
-	{
-		.id = V4L2_CID_BRIGHTNESS,
-		.name = "Brightness",
-		.minimum = 0,
-		.maximum = 10000,
-		.step = 1,
-		.default_value = 6200,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-	}, {
-		.id = V4L2_CID_CONTRAST,
-		.name = "Contrast",
-		.minimum = 0,
-		.maximum = 10000,
-		.step = 1,
-		.default_value = 5000,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-	}, {
-		.id = V4L2_CID_SATURATION,
-		.name = "Saturation",
-		.minimum = 0,
-		.maximum = 10000,
-		.step = 1,
-		.default_value = 5000,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-	}, {
-		.id = V4L2_CID_HUE,
-		.name = "Hue",
-		.minimum = 0,
-		.maximum = 10000,
-		.step = 1,
-		.default_value = 5000,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-	}
-};
-static const int CX25821_CTLS = ARRAY_SIZE(cx25821_ctls);
-
-static int cx25821_ctrl_query(struct v4l2_queryctrl *qctrl)
-{
-	int i;
-
-	if (qctrl->id < V4L2_CID_BASE || qctrl->id >= V4L2_CID_LASTP1)
-		return -EINVAL;
-	for (i = 0; i < CX25821_CTLS; i++)
-		if (cx25821_ctls[i].id == qctrl->id)
-			break;
-	if (i == CX25821_CTLS) {
-		*qctrl = no_ctl;
-		return 0;
-	}
-	*qctrl = cx25821_ctls[i];
-	return 0;
-}
-
-int cx25821_vidioc_queryctrl(struct file *file, void *priv,
-		     struct v4l2_queryctrl *qctrl)
-{
-	return cx25821_ctrl_query(qctrl);
-}
-
-/* ------------------------------------------------------------------ */
-/* VIDEO CTRL IOCTLS                                                  */
-
-static const struct v4l2_queryctrl *ctrl_by_id(unsigned int id)
-{
-	unsigned int i;
-
-	for (i = 0; i < CX25821_CTLS; i++)
-		if (cx25821_ctls[i].id == id)
-			return cx25821_ctls + i;
-	return NULL;
-}
-
-int cx25821_vidioc_g_ctrl(struct file *file, void *priv,
-			  struct v4l2_control *ctl)
-{
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	struct cx25821_fh *fh = priv;
-
-	const struct v4l2_queryctrl *ctrl;
-
-	ctrl = ctrl_by_id(ctl->id);
-
-	if (NULL == ctrl)
-		return -EINVAL;
-	switch (ctl->id) {
+	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		ctl->value = dev->channels[fh->channel_id].ctl_bright;
+		medusa_set_brightness(dev, ctrl->val, chan->id);
 		break;
 	case V4L2_CID_HUE:
-		ctl->value = dev->channels[fh->channel_id].ctl_hue;
+		medusa_set_hue(dev, ctrl->val, chan->id);
 		break;
 	case V4L2_CID_CONTRAST:
-		ctl->value = dev->channels[fh->channel_id].ctl_contrast;
+		medusa_set_contrast(dev, ctrl->val, chan->id);
 		break;
 	case V4L2_CID_SATURATION:
-		ctl->value = dev->channels[fh->channel_id].ctl_saturation;
-		break;
-	}
-	return 0;
-}
-
-int cx25821_set_control(struct cx25821_dev *dev,
-			struct v4l2_control *ctl, int chan_num)
-{
-	int err;
-	const struct v4l2_queryctrl *ctrl;
-
-	err = -EINVAL;
-
-	ctrl = ctrl_by_id(ctl->id);
-
-	if (NULL == ctrl)
-		return err;
-
-	switch (ctrl->type) {
-	case V4L2_CTRL_TYPE_BOOLEAN:
-	case V4L2_CTRL_TYPE_MENU:
-	case V4L2_CTRL_TYPE_INTEGER:
-		if (ctl->value < ctrl->minimum)
-			ctl->value = ctrl->minimum;
-		if (ctl->value > ctrl->maximum)
-			ctl->value = ctrl->maximum;
+		medusa_set_saturation(dev, ctrl->val, chan->id);
 		break;
 	default:
-		/* nothing */ ;
-	}
-
-	switch (ctl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		dev->channels[chan_num].ctl_bright = ctl->value;
-		medusa_set_brightness(dev, ctl->value, chan_num);
-		break;
-	case V4L2_CID_HUE:
-		dev->channels[chan_num].ctl_hue = ctl->value;
-		medusa_set_hue(dev, ctl->value, chan_num);
-		break;
-	case V4L2_CID_CONTRAST:
-		dev->channels[chan_num].ctl_contrast = ctl->value;
-		medusa_set_contrast(dev, ctl->value, chan_num);
-		break;
-	case V4L2_CID_SATURATION:
-		dev->channels[chan_num].ctl_saturation = ctl->value;
-		medusa_set_saturation(dev, ctl->value, chan_num);
-		break;
-	}
-
-	err = 0;
-
-	return err;
-}
-
-static void cx25821_init_controls(struct cx25821_dev *dev, int chan_num)
-{
-	struct v4l2_control ctrl;
-	int i;
-	for (i = 0; i < CX25821_CTLS; i++) {
-		ctrl.id = cx25821_ctls[i].id;
-		ctrl.value = cx25821_ctls[i].default_value;
-
-		cx25821_set_control(dev, &ctrl, chan_num);
-	}
-}
-
-int cx25821_vidioc_cropcap(struct file *file, void *priv,
-			   struct v4l2_cropcap *cropcap)
-{
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-
-	if (cropcap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
-	cropcap->bounds.top = 0;
-	cropcap->bounds.left = 0;
-	cropcap->bounds.width = 720;
-	cropcap->bounds.height = dev->tvnorm == V4L2_STD_PAL_BG ? 576 : 480;
-	cropcap->pixelaspect.numerator =
-		dev->tvnorm == V4L2_STD_PAL_BG ? 59 : 10;
-	cropcap->pixelaspect.denominator =
-		dev->tvnorm == V4L2_STD_PAL_BG ? 54 : 11;
-	cropcap->defrect = cropcap->bounds;
-	return 0;
-}
-
-int cx25821_vidioc_s_crop(struct file *file, void *priv, const struct v4l2_crop *crop)
-{
-	struct cx25821_dev *dev = ((struct cx25821_fh *)priv)->dev;
-	struct cx25821_fh *fh = priv;
-	int err;
-
-	if (fh) {
-		err = v4l2_prio_check(&dev->channels[fh->channel_id].prio,
-				      fh->prio);
-		if (0 != err)
-			return err;
 	}
-	/* cx25821_vidioc_s_crop not supported */
-	return -EINVAL;
-}
-
-int cx25821_vidioc_g_crop(struct file *file, void *priv, struct v4l2_crop *crop)
-{
-	/* cx25821_vidioc_g_crop not supported */
-	return -EINVAL;
+	return 0;
 }
 
-int cx25821_vidioc_querystd(struct file *file, void *priv, v4l2_std_id * norm)
+static int cx25821_vidioc_enum_output(struct file *file, void *priv,
+			      struct v4l2_output *o)
 {
-	/* medusa does not support video standard sensing of current input */
-	*norm = CX25821_NORMS;
+	if (o->index)
+		return -EINVAL;
 
+	o->type = V4L2_INPUT_TYPE_CAMERA;
+	o->std = CX25821_NORMS;
+	strcpy(o->name, "Composite");
 	return 0;
 }
 
-int cx25821_is_valid_width(u32 width, v4l2_std_id tvnorm)
+static int cx25821_vidioc_g_output(struct file *file, void *priv, unsigned int *o)
 {
-	if (tvnorm == V4L2_STD_PAL_BG) {
-		if (width == 352 || width == 720)
-			return 1;
-		else
-			return 0;
-	}
-
-	if (tvnorm == V4L2_STD_NTSC_M) {
-		if (width == 320 || width == 352 || width == 720)
-			return 1;
-		else
-			return 0;
-	}
+	*o = 0;
 	return 0;
 }
 
-int cx25821_is_valid_height(u32 height, v4l2_std_id tvnorm)
+static int cx25821_vidioc_s_output(struct file *file, void *priv, unsigned int o)
 {
-	if (tvnorm == V4L2_STD_PAL_BG) {
-		if (height == 576 || height == 288)
-			return 1;
-		else
-			return 0;
-	}
-
-	if (tvnorm == V4L2_STD_NTSC_M) {
-		if (height == 480 || height == 240)
-			return 1;
-		else
-			return 0;
-	}
-
-	return 0;
+	return o ? -EINVAL : 0;
 }
 
-static long video_ioctl_upstream9(struct file *file, unsigned int cmd,
-				 unsigned long arg)
+static int cx25821_vidioc_try_fmt_vid_out(struct file *file, void *priv,
+				   struct v4l2_format *f)
 {
-	struct cx25821_fh *fh = file->private_data;
-	struct cx25821_dev *dev = fh->dev;
-	int command = 0;
-	struct upstream_user_struct *data_from_user;
-
-	data_from_user = (struct upstream_user_struct *)arg;
-
-	if (!data_from_user) {
-		pr_err("%s(): Upstream data is INVALID. Returning\n", __func__);
-		return 0;
-	}
-
-	command = data_from_user->command;
-
-	if (command != UPSTREAM_START_VIDEO && command != UPSTREAM_STOP_VIDEO)
-		return 0;
-
-	dev->input_filename = data_from_user->input_filename;
-	dev->input_audiofilename = data_from_user->input_filename;
-	dev->vid_stdname = data_from_user->vid_stdname;
-	dev->pixel_format = data_from_user->pixel_format;
-	dev->channel_select = data_from_user->channel_select;
-	dev->command = data_from_user->command;
-
-	switch (command) {
-	case UPSTREAM_START_VIDEO:
-		cx25821_start_upstream_video_ch1(dev, data_from_user);
-		break;
-
-	case UPSTREAM_STOP_VIDEO:
-		cx25821_stop_upstream_video_ch1(dev);
-		break;
-	}
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
+	const struct cx25821_fmt *fmt;
 
+	fmt = cx25821_format_by_fourcc(f->fmt.pix.pixelformat);
+	if (NULL == fmt)
+		return -EINVAL;
+	f->fmt.pix.width = 720;
+	f->fmt.pix.height = (dev->tvnorm & V4L2_STD_625_50) ? 576 : 480;
+	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
+	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
 	return 0;
 }
 
-static long video_ioctl_upstream10(struct file *file, unsigned int cmd,
-				  unsigned long arg)
+static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
 {
-	struct cx25821_fh *fh = file->private_data;
-	struct cx25821_dev *dev = fh->dev;
-	int command = 0;
-	struct upstream_user_struct *data_from_user;
-
-	data_from_user = (struct upstream_user_struct *)arg;
-
-	if (!data_from_user) {
-		pr_err("%s(): Upstream data is INVALID. Returning\n", __func__);
-		return 0;
-	}
-
-	command = data_from_user->command;
-
-	if (command != UPSTREAM_START_VIDEO && command != UPSTREAM_STOP_VIDEO)
-		return 0;
-
-	dev->input_filename_ch2 = data_from_user->input_filename;
-	dev->input_audiofilename = data_from_user->input_filename;
-	dev->vid_stdname_ch2 = data_from_user->vid_stdname;
-	dev->pixel_format_ch2 = data_from_user->pixel_format;
-	dev->channel_select_ch2 = data_from_user->channel_select;
-	dev->command_ch2 = data_from_user->command;
+	struct cx25821_channel *chan = video_drvdata(file);
+	int err;
 
-	switch (command) {
-	case UPSTREAM_START_VIDEO:
-		cx25821_start_upstream_video_ch2(dev, data_from_user);
-		break;
+	err = cx25821_vidioc_try_fmt_vid_out(file, priv, f);
 
-	case UPSTREAM_STOP_VIDEO:
-		cx25821_stop_upstream_video_ch2(dev);
-		break;
-	}
+	if (0 != err)
+		return err;
 
+	chan->fmt = cx25821_format_by_fourcc(f->fmt.pix.pixelformat);
+	chan->vidq.field = f->fmt.pix.field;
+	chan->width = f->fmt.pix.width;
+	chan->height = f->fmt.pix.height;
+	if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_Y41P)
+		chan->pixel_formats = PIXEL_FRMT_411;
+	else
+		chan->pixel_formats = PIXEL_FRMT_422;
 	return 0;
 }
 
-static long video_ioctl_upstream11(struct file *file, unsigned int cmd,
-				  unsigned long arg)
+static ssize_t video_write(struct file *file, const char __user *data, size_t count,
+			 loff_t *ppos)
 {
-	struct cx25821_fh *fh = file->private_data;
-	struct cx25821_dev *dev = fh->dev;
-	int command = 0;
-	struct upstream_user_struct *data_from_user;
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
+	struct v4l2_fh *fh = file->private_data;
+	int err = 0;
 
-	data_from_user = (struct upstream_user_struct *)arg;
-
-	if (!data_from_user) {
-		pr_err("%s(): Upstream data is INVALID. Returning\n", __func__);
-		return 0;
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	if (chan->streaming_fh && chan->streaming_fh != fh) {
+		err = -EBUSY;
+		goto unlock;
 	}
-
-	command = data_from_user->command;
-
-	if (command != UPSTREAM_START_AUDIO && command != UPSTREAM_STOP_AUDIO)
-		return 0;
-
-	dev->input_filename = data_from_user->input_filename;
-	dev->input_audiofilename = data_from_user->input_filename;
-	dev->vid_stdname = data_from_user->vid_stdname;
-	dev->pixel_format = data_from_user->pixel_format;
-	dev->channel_select = data_from_user->channel_select;
-	dev->command = data_from_user->command;
-
-	switch (command) {
-	case UPSTREAM_START_AUDIO:
-		cx25821_start_upstream_audio(dev, data_from_user);
-		break;
-
-	case UPSTREAM_STOP_AUDIO:
-		cx25821_stop_upstream_audio(dev);
-		break;
+	if (!chan->streaming_fh) {
+		err = cx25821_vidupstream_init(chan, chan->pixel_formats);
+		if (err)
+			goto unlock;
+		chan->streaming_fh = fh;
 	}
 
-	return 0;
+	err = cx25821_write_frame(chan, data, count);
+	count -= err;
+	*ppos += err;
+
+unlock:
+	mutex_unlock(&dev->lock);
+	return err;
 }
 
-static long video_ioctl_set(struct file *file, unsigned int cmd,
-			   unsigned long arg)
+static int video_out_release(struct file *file)
 {
-	struct cx25821_fh *fh = file->private_data;
-	struct cx25821_dev *dev = fh->dev;
-	struct downstream_user_struct *data_from_user;
-	int command;
-	int width = 720;
-	int selected_channel = 0;
-	int pix_format = 0;
-	int i = 0;
-	int cif_enable = 0;
-	int cif_width = 0;
-
-	data_from_user = (struct downstream_user_struct *)arg;
-
-	if (!data_from_user) {
-		pr_err("%s(): User data is INVALID. Returning\n", __func__);
-		return 0;
-	}
-
-	command = data_from_user->command;
-
-	if (command != SET_VIDEO_STD && command != SET_PIXEL_FORMAT
-	   && command != ENABLE_CIF_RESOLUTION && command != REG_READ
-	   && command != REG_WRITE && command != MEDUSA_READ
-	   && command != MEDUSA_WRITE) {
-		return 0;
-	}
-
-	switch (command) {
-	case SET_VIDEO_STD:
-		if (!strcmp(data_from_user->vid_stdname, "PAL"))
-			dev->tvnorm = V4L2_STD_PAL_BG;
-		else
-			dev->tvnorm = V4L2_STD_NTSC_M;
-		medusa_set_videostandard(dev);
-		break;
-
-	case SET_PIXEL_FORMAT:
-		selected_channel = data_from_user->decoder_select;
-		pix_format = data_from_user->pixel_format;
-
-		if (!(selected_channel <= 7 && selected_channel >= 0)) {
-			selected_channel -= 4;
-			selected_channel = selected_channel % 8;
-		}
-
-		if (selected_channel >= 0)
-			cx25821_set_pixel_format(dev, selected_channel,
-						pix_format);
-
-		break;
-
-	case ENABLE_CIF_RESOLUTION:
-		selected_channel = data_from_user->decoder_select;
-		cif_enable = data_from_user->cif_resolution_enable;
-		cif_width = data_from_user->cif_width;
-
-		if (cif_enable) {
-			if (dev->tvnorm & V4L2_STD_PAL_BG
-			    || dev->tvnorm & V4L2_STD_PAL_DK) {
-				width = 352;
-			} else {
-				width = cif_width;
-				if (cif_width != 320 && cif_width != 352)
-					width = 320;
-			}
-		}
-
-		if (!(selected_channel <= 7 && selected_channel >= 0)) {
-			selected_channel -= 4;
-			selected_channel = selected_channel % 8;
-		}
-
-		if (selected_channel <= 7 && selected_channel >= 0) {
-			dev->channels[selected_channel].use_cif_resolution =
-				cif_enable;
-			dev->channels[selected_channel].cif_width = width;
-		} else {
-			for (i = 0; i < VID_CHANNEL_NUM; i++) {
-				dev->channels[i].use_cif_resolution =
-					cif_enable;
-				dev->channels[i].cif_width = width;
-			}
-		}
+	struct cx25821_channel *chan = video_drvdata(file);
+	struct cx25821_dev *dev = chan->dev;
+	struct v4l2_fh *fh = file->private_data;
 
-		medusa_set_resolution(dev, width, selected_channel);
-		break;
-	case REG_READ:
-		data_from_user->reg_data = cx_read(data_from_user->reg_address);
-		break;
-	case REG_WRITE:
-		cx_write(data_from_user->reg_address, data_from_user->reg_data);
-		break;
-	case MEDUSA_READ:
-		cx25821_i2c_read(&dev->i2c_bus[0],
-					 (u16) data_from_user->reg_address,
-					 &data_from_user->reg_data);
-		break;
-	case MEDUSA_WRITE:
-		cx25821_i2c_write(&dev->i2c_bus[0],
-				  (u16) data_from_user->reg_address,
-				  data_from_user->reg_data);
-		break;
+	mutex_lock(&dev->lock);
+	if (chan->streaming_fh == fh) {
+		cx25821_stop_upstream_video(chan);
+		chan->streaming_fh = NULL;
 	}
+	mutex_unlock(&dev->lock);
 
-	return 0;
+	return v4l2_fh_release(file);
 }
 
-static long cx25821_video_ioctl(struct file *file,
-				unsigned int cmd, unsigned long arg)
-{
-	int ret = 0;
-
-	struct cx25821_fh *fh = file->private_data;
-
-	/* check to see if it's the video upstream */
-	if (fh->channel_id == SRAM_CH09) {
-		ret = video_ioctl_upstream9(file, cmd, arg);
-		return ret;
-	} else if (fh->channel_id == SRAM_CH10) {
-		ret = video_ioctl_upstream10(file, cmd, arg);
-		return ret;
-	} else if (fh->channel_id == SRAM_CH11) {
-		ret = video_ioctl_upstream11(file, cmd, arg);
-		ret = video_ioctl_set(file, cmd, arg);
-		return ret;
-	}
-
-	return video_ioctl2(file, cmd, arg);
-}
+static const struct v4l2_ctrl_ops cx25821_ctrl_ops = {
+	.s_ctrl = cx25821_s_ctrl,
+};
 
-/* exported stuff */
 static const struct v4l2_file_operations video_fops = {
 	.owner = THIS_MODULE,
-	.open = video_open,
+	.open = v4l2_fh_open,
 	.release = video_release,
 	.read = video_read,
 	.poll = video_poll,
 	.mmap = cx25821_video_mmap,
-	.ioctl = cx25821_video_ioctl,
+	.unlocked_ioctl = video_ioctl2,
 };
 
 static const struct v4l2_ioctl_ops video_ioctl_ops = {
@@ -1951,40 +963,170 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_querybuf = cx25821_vidioc_querybuf,
 	.vidioc_qbuf = cx25821_vidioc_qbuf,
 	.vidioc_dqbuf = vidioc_dqbuf,
-#ifdef TUNER_FLAG
+	.vidioc_g_std = cx25821_vidioc_g_std,
 	.vidioc_s_std = cx25821_vidioc_s_std,
-	.vidioc_querystd = cx25821_vidioc_querystd,
-#endif
-	.vidioc_cropcap = cx25821_vidioc_cropcap,
-	.vidioc_s_crop = cx25821_vidioc_s_crop,
-	.vidioc_g_crop = cx25821_vidioc_g_crop,
 	.vidioc_enum_input = cx25821_vidioc_enum_input,
 	.vidioc_g_input = cx25821_vidioc_g_input,
 	.vidioc_s_input = cx25821_vidioc_s_input,
-	.vidioc_g_ctrl = cx25821_vidioc_g_ctrl,
-	.vidioc_s_ctrl = vidioc_s_ctrl,
-	.vidioc_queryctrl = cx25821_vidioc_queryctrl,
 	.vidioc_streamon = vidioc_streamon,
 	.vidioc_streamoff = vidioc_streamoff,
 	.vidioc_log_status = vidioc_log_status,
-	.vidioc_g_priority = cx25821_vidioc_g_priority,
-	.vidioc_s_priority = cx25821_vidioc_s_priority,
-#ifdef TUNER_FLAG
-	.vidioc_g_tuner = cx25821_vidioc_g_tuner,
-	.vidioc_s_tuner = cx25821_vidioc_s_tuner,
-	.vidioc_g_frequency = cx25821_vidioc_g_frequency,
-	.vidioc_s_frequency = cx25821_vidioc_s_frequency,
-#endif
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-	.vidioc_g_register = cx25821_vidioc_g_register,
-	.vidioc_s_register = cx25821_vidioc_s_register,
-#endif
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
-struct video_device cx25821_videoioctl_template = {
-	.name = "cx25821-videoioctl",
+static const struct video_device cx25821_video_device = {
+	.name = "cx25821-video",
 	.fops = &video_fops,
+	.release = video_device_release_empty,
+	.minor = -1,
 	.ioctl_ops = &video_ioctl_ops,
 	.tvnorms = CX25821_NORMS,
-	.current_norm = V4L2_STD_NTSC_M,
 };
+
+static const struct v4l2_file_operations video_out_fops = {
+	.owner = THIS_MODULE,
+	.open = v4l2_fh_open,
+	.write = video_write,
+	.release = video_out_release,
+	.unlocked_ioctl = video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops video_out_ioctl_ops = {
+	.vidioc_querycap = cx25821_vidioc_querycap,
+	.vidioc_enum_fmt_vid_out = cx25821_vidioc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_out = cx25821_vidioc_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_out = cx25821_vidioc_try_fmt_vid_out,
+	.vidioc_s_fmt_vid_out = vidioc_s_fmt_vid_out,
+	.vidioc_g_std = cx25821_vidioc_g_std,
+	.vidioc_s_std = cx25821_vidioc_s_std,
+	.vidioc_enum_output = cx25821_vidioc_enum_output,
+	.vidioc_g_output = cx25821_vidioc_g_output,
+	.vidioc_s_output = cx25821_vidioc_s_output,
+	.vidioc_log_status = vidioc_log_status,
+};
+
+static const struct video_device cx25821_video_out_device = {
+	.name = "cx25821-video",
+	.fops = &video_out_fops,
+	.release = video_device_release_empty,
+	.minor = -1,
+	.ioctl_ops = &video_out_ioctl_ops,
+	.tvnorms = CX25821_NORMS,
+};
+
+void cx25821_video_unregister(struct cx25821_dev *dev, int chan_num)
+{
+	cx_clear(PCI_INT_MSK, 1);
+
+	if (video_is_registered(&dev->channels[chan_num].vdev)) {
+		video_unregister_device(&dev->channels[chan_num].vdev);
+		v4l2_ctrl_handler_free(&dev->channels[chan_num].hdl);
+
+		btcx_riscmem_free(dev->pci,
+				&dev->channels[chan_num].dma_vidq.stopper);
+	}
+}
+
+int cx25821_video_register(struct cx25821_dev *dev)
+{
+	int err;
+	int i;
+
+	/* initial device configuration */
+	dev->tvnorm = V4L2_STD_NTSC_M;
+
+	spin_lock_init(&dev->slock);
+
+	for (i = 0; i < MAX_VID_CHANNEL_NUM - 1; ++i) {
+		struct cx25821_channel *chan = &dev->channels[i];
+		struct video_device *vdev = &chan->vdev;
+		struct v4l2_ctrl_handler *hdl = &chan->hdl;
+		bool is_output = i > SRAM_CH08;
+
+		if (i == SRAM_CH08) /* audio channel */
+			continue;
+
+		if (!is_output) {
+			v4l2_ctrl_handler_init(hdl, 4);
+			v4l2_ctrl_new_std(hdl, &cx25821_ctrl_ops,
+					V4L2_CID_BRIGHTNESS, 0, 10000, 1, 6200);
+			v4l2_ctrl_new_std(hdl, &cx25821_ctrl_ops,
+					V4L2_CID_CONTRAST, 0, 10000, 1, 5000);
+			v4l2_ctrl_new_std(hdl, &cx25821_ctrl_ops,
+					V4L2_CID_SATURATION, 0, 10000, 1, 5000);
+			v4l2_ctrl_new_std(hdl, &cx25821_ctrl_ops,
+					V4L2_CID_HUE, 0, 10000, 1, 5000);
+			if (hdl->error) {
+				err = hdl->error;
+				goto fail_unreg;
+			}
+			err = v4l2_ctrl_handler_setup(hdl);
+			if (err)
+				goto fail_unreg;
+		} else {
+			chan->out = &dev->vid_out_data[i - SRAM_CH09];
+			chan->out->chan = chan;
+		}
+
+		cx25821_risc_stopper(dev->pci, &chan->dma_vidq.stopper,
+			chan->sram_channels->dma_ctl, 0x11, 0);
+
+		chan->sram_channels = &cx25821_sram_channels[i];
+		chan->width = 720;
+		if (dev->tvnorm & V4L2_STD_625_50)
+			chan->height = 576;
+		else
+			chan->height = 480;
+
+		if (chan->pixel_formats == PIXEL_FRMT_411)
+			chan->fmt = cx25821_format_by_fourcc(V4L2_PIX_FMT_Y41P);
+		else
+			chan->fmt = cx25821_format_by_fourcc(V4L2_PIX_FMT_YUYV);
+
+		cx_write(chan->sram_channels->int_stat, 0xffffffff);
+
+		INIT_LIST_HEAD(&chan->dma_vidq.active);
+		INIT_LIST_HEAD(&chan->dma_vidq.queued);
+
+		chan->timeout_data.dev = dev;
+		chan->timeout_data.channel = &cx25821_sram_channels[i];
+		chan->dma_vidq.timeout.function = cx25821_vid_timeout;
+		chan->dma_vidq.timeout.data = (unsigned long)&chan->timeout_data;
+		init_timer(&chan->dma_vidq.timeout);
+
+		if (!is_output)
+			videobuf_queue_sg_init(&chan->vidq, &cx25821_video_qops, &dev->pci->dev,
+				&dev->slock, V4L2_BUF_TYPE_VIDEO_CAPTURE,
+				V4L2_FIELD_INTERLACED, sizeof(struct cx25821_buffer),
+				chan, &dev->lock);
+
+		/* register v4l devices */
+		*vdev = is_output ? cx25821_video_out_device : cx25821_video_device;
+		vdev->v4l2_dev = &dev->v4l2_dev;
+		if (!is_output)
+			vdev->ctrl_handler = hdl;
+		else
+			vdev->vfl_dir = VFL_DIR_TX;
+		vdev->lock = &dev->lock;
+		set_bit(V4L2_FL_USE_FH_PRIO, &vdev->flags);
+		snprintf(vdev->name, sizeof(vdev->name), "%s #%d", dev->name, i);
+		video_set_drvdata(vdev, chan);
+
+		err = video_register_device(vdev, VFL_TYPE_GRABBER,
+					    video_nr[dev->nr]);
+
+		if (err < 0)
+			goto fail_unreg;
+	}
+
+	/* set PCI interrupt */
+	cx_set(PCI_INT_MSK, 0xff);
+
+	return 0;
+
+fail_unreg:
+	while (i >= 0)
+		cx25821_video_unregister(dev, i--);
+	return err;
+}
diff --git a/drivers/media/pci/cx25821/cx25821-video.h b/drivers/media/pci/cx25821/cx25821-video.h
index c265e35b37c3..ab63b3858acf 100644
--- a/drivers/media/pci/cx25821/cx25821-video.h
+++ b/drivers/media/pci/cx25821/cx25821-video.h
@@ -39,8 +39,7 @@
 #include "cx25821.h"
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
-
-#define TUNER_FLAG
+#include <media/v4l2-event.h>
 
 #define VIDEO_DEBUG 0
 
@@ -50,137 +49,17 @@ do {									\
 		printk(KERN_DEBUG "%s/0: " fmt, dev->name, ##arg);	\
 } while (0)
 
-/* For IOCTL to identify running upstream */
-#define UPSTREAM_START_VIDEO        700
-#define UPSTREAM_STOP_VIDEO         701
-#define UPSTREAM_START_AUDIO        702
-#define UPSTREAM_STOP_AUDIO         703
-#define UPSTREAM_DUMP_REGISTERS     702
-#define SET_VIDEO_STD               800
-#define SET_PIXEL_FORMAT            1000
-#define ENABLE_CIF_RESOLUTION       1001
-
-#define REG_READ		    900
-#define REG_WRITE		    901
-#define MEDUSA_READ		    910
-#define MEDUSA_WRITE		    911
-
-extern struct sram_channel *channel0;
-extern struct sram_channel *channel1;
-extern struct sram_channel *channel2;
-extern struct sram_channel *channel3;
-extern struct sram_channel *channel4;
-extern struct sram_channel *channel5;
-extern struct sram_channel *channel6;
-extern struct sram_channel *channel7;
-extern struct sram_channel *channel9;
-extern struct sram_channel *channel10;
-extern struct sram_channel *channel11;
-extern struct video_device cx25821_videoioctl_template;
-/* extern const u32 *ctrl_classes[]; */
-
-extern unsigned int vid_limit;
-
 #define FORMAT_FLAGS_PACKED       0x01
-extern struct cx25821_fmt formats[];
-extern struct cx25821_fmt *cx25821_format_by_fourcc(unsigned int fourcc);
-extern struct cx25821_data timeout_data[MAX_VID_CHANNEL_NUM];
-
 extern void cx25821_video_wakeup(struct cx25821_dev *dev,
 				 struct cx25821_dmaqueue *q, u32 count);
 
-#ifdef TUNER_FLAG
-extern int cx25821_set_tvnorm(struct cx25821_dev *dev, v4l2_std_id norm);
-#endif
-
-extern int cx25821_res_get(struct cx25821_dev *dev, struct cx25821_fh *fh,
-			   unsigned int bit);
-extern int cx25821_res_check(struct cx25821_fh *fh, unsigned int bit);
-extern int cx25821_res_locked(struct cx25821_fh *fh, unsigned int bit);
-extern void cx25821_res_free(struct cx25821_dev *dev, struct cx25821_fh *fh,
-			     unsigned int bits);
-extern int cx25821_video_mux(struct cx25821_dev *dev, unsigned int input);
 extern int cx25821_start_video_dma(struct cx25821_dev *dev,
 				   struct cx25821_dmaqueue *q,
 				   struct cx25821_buffer *buf,
-				   struct sram_channel *channel);
+				   const struct sram_channel *channel);
 
-extern int cx25821_set_scale(struct cx25821_dev *dev, unsigned int width,
-			     unsigned int height, enum v4l2_field field);
 extern int cx25821_video_irq(struct cx25821_dev *dev, int chan_num, u32 status);
 extern void cx25821_video_unregister(struct cx25821_dev *dev, int chan_num);
 extern int cx25821_video_register(struct cx25821_dev *dev);
-extern int cx25821_get_format_size(void);
-
-extern int cx25821_buffer_setup(struct videobuf_queue *q, unsigned int *count,
-				unsigned int *size);
-extern int cx25821_buffer_prepare(struct videobuf_queue *q,
-				  struct videobuf_buffer *vb,
-				  enum v4l2_field field);
-extern void cx25821_buffer_release(struct videobuf_queue *q,
-				   struct videobuf_buffer *vb);
-extern struct videobuf_queue *get_queue(struct cx25821_fh *fh);
-extern int cx25821_get_resource(struct cx25821_fh *fh, int resource);
-extern int cx25821_video_mmap(struct file *file, struct vm_area_struct *vma);
-extern int cx25821_vidioc_try_fmt_vid_cap(struct file *file, void *priv,
-					  struct v4l2_format *f);
-extern int cx25821_vidioc_querycap(struct file *file, void *priv,
-				   struct v4l2_capability *cap);
-extern int cx25821_vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
-					   struct v4l2_fmtdesc *f);
-extern int cx25821_vidioc_reqbufs(struct file *file, void *priv,
-				  struct v4l2_requestbuffers *p);
-extern int cx25821_vidioc_querybuf(struct file *file, void *priv,
-				   struct v4l2_buffer *p);
-extern int cx25821_vidioc_qbuf(struct file *file, void *priv,
-			       struct v4l2_buffer *p);
-extern int cx25821_vidioc_s_std(struct file *file, void *priv,
-				v4l2_std_id *tvnorms);
-extern int cx25821_enum_input(struct cx25821_dev *dev, struct v4l2_input *i);
-extern int cx25821_vidioc_enum_input(struct file *file, void *priv,
-				     struct v4l2_input *i);
-extern int cx25821_vidioc_g_input(struct file *file, void *priv,
-				  unsigned int *i);
-extern int cx25821_vidioc_s_input(struct file *file, void *priv,
-				  unsigned int i);
-extern int cx25821_vidioc_g_ctrl(struct file *file, void *priv,
-				 struct v4l2_control *ctl);
-extern int cx25821_vidioc_g_fmt_vid_cap(struct file *file, void *priv,
-					struct v4l2_format *f);
-extern int cx25821_vidioc_g_frequency(struct file *file, void *priv,
-				      struct v4l2_frequency *f);
-extern int cx25821_set_freq(struct cx25821_dev *dev, struct v4l2_frequency *f);
-extern int cx25821_vidioc_s_frequency(struct file *file, void *priv,
-				      struct v4l2_frequency *f);
-extern int cx25821_vidioc_g_register(struct file *file, void *fh,
-				     struct v4l2_dbg_register *reg);
-extern int cx25821_vidioc_s_register(struct file *file, void *fh,
-				     struct v4l2_dbg_register *reg);
-extern int cx25821_vidioc_g_tuner(struct file *file, void *priv,
-				  struct v4l2_tuner *t);
-extern int cx25821_vidioc_s_tuner(struct file *file, void *priv,
-				  struct v4l2_tuner *t);
-
-extern int cx25821_is_valid_width(u32 width, v4l2_std_id tvnorm);
-extern int cx25821_is_valid_height(u32 height, v4l2_std_id tvnorm);
-
-extern int cx25821_vidioc_g_priority(struct file *file, void *f,
-				     enum v4l2_priority *p);
-extern int cx25821_vidioc_s_priority(struct file *file, void *f,
-				     enum v4l2_priority prio);
-
-extern int cx25821_vidioc_queryctrl(struct file *file, void *priv,
-				    struct v4l2_queryctrl *qctrl);
-extern int cx25821_set_control(struct cx25821_dev *dev,
-			       struct v4l2_control *ctrl, int chan_num);
-
-extern int cx25821_vidioc_cropcap(struct file *file, void *fh,
-				  struct v4l2_cropcap *cropcap);
-extern int cx25821_vidioc_s_crop(struct file *file, void *priv,
-				 const struct v4l2_crop *crop);
-extern int cx25821_vidioc_g_crop(struct file *file, void *priv,
-				 struct v4l2_crop *crop);
 
-extern int cx25821_vidioc_querystd(struct file *file, void *priv,
-				   v4l2_std_id *norm);
 #endif
diff --git a/drivers/media/pci/cx25821/cx25821.h b/drivers/media/pci/cx25821/cx25821.h
index 8a9c0c869412..90bdc196929f 100644
--- a/drivers/media/pci/cx25821/cx25821.h
+++ b/drivers/media/pci/cx25821/cx25821.h
@@ -33,17 +33,14 @@
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
-#include <media/tuner.h>
-#include <media/tveeprom.h>
+#include <media/v4l2-ctrls.h>
 #include <media/videobuf-dma-sg.h>
-#include <media/videobuf-dvb.h>
 
 #include "btcx-risc.h"
 #include "cx25821-reg.h"
 #include "cx25821-medusa-reg.h"
 #include "cx25821-sram.h"
 #include "cx25821-audio.h"
-#include "media/cx2341x.h"
 
 #include <linux/version.h>
 #include <linux/mutex.h>
@@ -55,8 +52,6 @@
 
 #define CX25821_MAXBOARDS 2
 
-#define TRUE    1
-#define FALSE   0
 #define LINE_SIZE_D1    1440
 
 /* Number of decoders and encoders */
@@ -67,7 +62,6 @@
 
 /* Max number of inputs by card */
 #define MAX_CX25821_INPUT     8
-#define INPUT(nr) (&cx25821_boards[dev->board].input[nr])
 #define RESOURCE_VIDEO0       1
 #define RESOURCE_VIDEO1       2
 #define RESOURCE_VIDEO2       4
@@ -80,7 +74,6 @@
 #define RESOURCE_VIDEO9       512
 #define RESOURCE_VIDEO10      1024
 #define RESOURCE_VIDEO11      2048
-#define RESOURCE_VIDEO_IOCTL  4096
 
 #define BUFFER_TIMEOUT     (HZ)	/* 0.5 seconds */
 
@@ -97,7 +90,6 @@
 #define CX25821_BOARD_CONEXANT_ATHENA10 1
 #define MAX_VID_CHANNEL_NUM     12
 #define VID_CHANNEL_NUM 8
-#define CX25821_NR_INPUT 2
 
 struct cx25821_fmt {
 	char *name;
@@ -107,14 +99,6 @@ struct cx25821_fmt {
 	u32 cxformat;
 };
 
-struct cx25821_ctrl {
-	struct v4l2_queryctrl v;
-	u32 off;
-	u32 reg;
-	u32 mask;
-	u32 shift;
-};
-
 struct cx25821_tvnorm {
 	char *name;
 	v4l2_std_id id;
@@ -122,40 +106,6 @@ struct cx25821_tvnorm {
 	u32 cxoformat;
 };
 
-struct cx25821_fh {
-	struct cx25821_dev *dev;
-	enum v4l2_buf_type type;
-	int radio;
-	u32 resources;
-
-	enum v4l2_priority prio;
-
-	/* video overlay */
-	struct v4l2_window win;
-	struct v4l2_clip *clips;
-	unsigned int nclips;
-
-	/* video capture */
-	struct cx25821_fmt *fmt;
-	unsigned int width, height;
-	int channel_id;
-
-	/* vbi capture */
-	struct videobuf_queue vidq;
-	struct videobuf_queue vbiq;
-
-	/* H264 Encoder specifics ONLY */
-	struct videobuf_queue mpegq;
-	atomic_t v4l_reading;
-};
-
-enum cx25821_itype {
-	CX25821_VMUX_COMPOSITE = 1,
-	CX25821_VMUX_SVIDEO,
-	CX25821_VMUX_DEBUG,
-	CX25821_RADIO,
-};
-
 enum cx25821_src_sel_type {
 	CX25821_SRC_SEL_EXT_656_VIDEO = 0,
 	CX25821_SRC_SEL_PARALLEL_MPEG_VIDEO
@@ -169,16 +119,10 @@ struct cx25821_buffer {
 	/* cx25821 specific */
 	unsigned int bpl;
 	struct btcx_riscmem risc;
-	struct cx25821_fmt *fmt;
+	const struct cx25821_fmt *fmt;
 	u32 count;
 };
 
-struct cx25821_input {
-	enum cx25821_itype type;
-	unsigned int vmux;
-	u32 gpio0, gpio1, gpio2, gpio3;
-};
-
 enum port {
 	CX25821_UNDEFINED = 0,
 	CX25821_RAW,
@@ -190,19 +134,8 @@ struct cx25821_board {
 	enum port porta;
 	enum port portb;
 	enum port portc;
-	unsigned int tuner_type;
-	unsigned int radio_type;
-	unsigned char tuner_addr;
-	unsigned char radio_addr;
 
 	u32 clk_freq;
-	struct cx25821_input input[CX25821_NR_INPUT];
-};
-
-struct cx25821_subid {
-	u16 subvendor;
-	u16 subdevice;
-	u32 card;
 };
 
 struct cx25821_i2c {
@@ -234,34 +167,70 @@ struct cx25821_dmaqueue {
 
 struct cx25821_data {
 	struct cx25821_dev *dev;
-	struct sram_channel *channel;
+	const struct sram_channel *channel;
+};
+
+struct cx25821_dev;
+
+struct cx25821_channel;
+
+struct cx25821_video_out_data {
+	struct cx25821_channel *chan;
+	int _line_size;
+	int _prog_cnt;
+	int _pixel_format;
+	int _is_first_frame;
+	int _is_running;
+	int _file_status;
+	int _lines_count;
+	int _frame_count;
+	unsigned int _risc_size;
+
+	__le32 *_dma_virt_start_addr;
+	__le32 *_dma_virt_addr;
+	dma_addr_t _dma_phys_addr;
+	dma_addr_t _dma_phys_start_addr;
+
+	unsigned int _data_buf_size;
+	__le32 *_data_buf_virt_addr;
+	dma_addr_t _data_buf_phys_addr;
+
+	u32 upstream_riscbuf_size;
+	u32 upstream_databuf_size;
+	int is_60hz;
+	int _frame_index;
+	int cur_frame_index;
+	int curpos;
+	wait_queue_head_t waitq;
 };
 
 struct cx25821_channel {
-	struct v4l2_prio_state prio;
+	unsigned id;
+	struct cx25821_dev *dev;
+	struct v4l2_fh *streaming_fh;
 
-	int ctl_bright;
-	int ctl_contrast;
-	int ctl_hue;
-	int ctl_saturation;
+	struct v4l2_ctrl_handler hdl;
 	struct cx25821_data timeout_data;
 
-	struct video_device *video_dev;
-	struct cx25821_dmaqueue vidq;
-
-	struct sram_channel *sram_channels;
+	struct video_device vdev;
+	struct cx25821_dmaqueue dma_vidq;
+	struct videobuf_queue vidq;
 
-	struct mutex lock;
-	int resources;
+	const struct sram_channel *sram_channels;
 
+	const struct cx25821_fmt *fmt;
+	unsigned int width, height;
 	int pixel_formats;
 	int use_cif_resolution;
 	int cif_width;
+
+	/* video output data for the video output channel */
+	struct cx25821_video_out_data *out;
 };
 
+struct snd_card;
+
 struct cx25821_dev {
-	struct list_head devlist;
-	atomic_t refcount;
 	struct v4l2_device v4l2_dev;
 
 	/* pci stuff */
@@ -273,6 +242,8 @@ struct cx25821_dev {
 	u8 __iomem *bmmio;
 	int pci_irqmask;
 	int hwrevision;
+	/* used by cx25821-alsa */
+	struct snd_card *card;
 
 	u32 clk_freq;
 
@@ -289,17 +260,8 @@ struct cx25821_dev {
 	char name[32];
 
 	/* Analog video */
-	u32 resources;
 	unsigned int input;
-	u32 tvaudio;
 	v4l2_std_id tvnorm;
-	unsigned int tuner_type;
-	unsigned char tuner_addr;
-	unsigned int radio_type;
-	unsigned char radio_addr;
-	unsigned int has_radio;
-	unsigned int videc_type;
-	unsigned char videc_addr;
 	unsigned short _max_num_decoders;
 
 	/* Analog Audio Upstream */
@@ -323,132 +285,26 @@ struct cx25821_dev {
 	__le32 *_audiodata_buf_virt_addr;
 	dma_addr_t _audiodata_buf_phys_addr;
 	char *_audiofilename;
-
-	/* V4l */
-	u32 freq;
-	struct video_device *vbi_dev;
-	struct video_device *radio_dev;
-	struct video_device *ioctl_dev;
-
-	spinlock_t slock;
-
-	/* Video Upstream */
-	int _line_size;
-	int _prog_cnt;
-	int _pixel_format;
-	int _is_first_frame;
-	int _is_running;
-	int _file_status;
-	int _lines_count;
-	int _frame_count;
-	int _channel_upstream_select;
-	unsigned int _risc_size;
-
-	__le32 *_dma_virt_start_addr;
-	__le32 *_dma_virt_addr;
-	dma_addr_t _dma_phys_addr;
-	dma_addr_t _dma_phys_start_addr;
-
-	unsigned int _data_buf_size;
-	__le32 *_data_buf_virt_addr;
-	dma_addr_t _data_buf_phys_addr;
-	char *_filename;
-	char *_defaultname;
-
-	int _line_size_ch2;
-	int _prog_cnt_ch2;
-	int _pixel_format_ch2;
-	int _is_first_frame_ch2;
-	int _is_running_ch2;
-	int _file_status_ch2;
-	int _lines_count_ch2;
-	int _frame_count_ch2;
-	int _channel2_upstream_select;
-	unsigned int _risc_size_ch2;
-
-	__le32 *_dma_virt_start_addr_ch2;
-	__le32 *_dma_virt_addr_ch2;
-	dma_addr_t _dma_phys_addr_ch2;
-	dma_addr_t _dma_phys_start_addr_ch2;
-
-	unsigned int _data_buf_size_ch2;
-	__le32 *_data_buf_virt_addr_ch2;
-	dma_addr_t _data_buf_phys_addr_ch2;
-	char *_filename_ch2;
-	char *_defaultname_ch2;
-
-	/* MPEG Encoder ONLY settings */
-	u32 cx23417_mailbox;
-	struct cx2341x_mpeg_params mpeg_params;
-	struct video_device *v4l_device;
-	atomic_t v4l_reader_count;
-	struct cx25821_tvnorm encodernorm;
-
-	u32 upstream_riscbuf_size;
-	u32 upstream_databuf_size;
-	u32 upstream_riscbuf_size_ch2;
-	u32 upstream_databuf_size_ch2;
 	u32 audio_upstream_riscbuf_size;
 	u32 audio_upstream_databuf_size;
-	int _isNTSC;
-	int _frame_index;
 	int _audioframe_index;
-	struct workqueue_struct *_irq_queues;
-	struct work_struct _irq_work_entry;
-	struct workqueue_struct *_irq_queues_ch2;
-	struct work_struct _irq_work_entry_ch2;
 	struct workqueue_struct *_irq_audio_queues;
 	struct work_struct _audio_work_entry;
-	char *input_filename;
-	char *input_filename_ch2;
-	int _frame_index_ch2;
-	int _isNTSC_ch2;
-	char *vid_stdname_ch2;
-	int pixel_format_ch2;
-	int channel_select_ch2;
-	int command_ch2;
 	char *input_audiofilename;
-	char *vid_stdname;
-	int pixel_format;
-	int channel_select;
-	int command;
-	int channel_opened;
-};
 
-struct upstream_user_struct {
-	char *input_filename;
-	char *vid_stdname;
-	int pixel_format;
-	int channel_select;
-	int command;
-};
+	/* V4l */
+	spinlock_t slock;
 
-struct downstream_user_struct {
-	char *vid_stdname;
-	int pixel_format;
-	int cif_resolution_enable;
-	int cif_width;
-	int decoder_select;
-	int command;
-	int reg_address;
-	int reg_data;
+	/* Video Upstream */
+	struct cx25821_video_out_data vid_out_data[2];
 };
 
-extern struct upstream_user_struct *up_data;
-
 static inline struct cx25821_dev *get_cx25821(struct v4l2_device *v4l2_dev)
 {
 	return container_of(v4l2_dev, struct cx25821_dev, v4l2_dev);
 }
 
-#define cx25821_call_all(dev, o, f, args...) \
-	v4l2_device_call_all(&dev->v4l2_dev, 0, o, f, ##args)
-
-extern struct list_head cx25821_devlist;
-extern struct mutex cx25821_devlist_mutex;
-
 extern struct cx25821_board cx25821_boards[];
-extern struct cx25821_subid cx25821_subids[];
 
 #define SRAM_CH00  0		/* Video A */
 #define SRAM_CH01  1		/* Video B */
@@ -467,7 +323,6 @@ extern struct cx25821_subid cx25821_subids[];
 #define VID_UPSTREAM_SRAM_CHANNEL_I     SRAM_CH09
 #define VID_UPSTREAM_SRAM_CHANNEL_J     SRAM_CH10
 #define AUDIO_UPSTREAM_SRAM_CHANNEL_B   SRAM_CH11
-#define VIDEO_IOCTL_CH  11
 
 struct sram_channel {
 	char *name;
@@ -503,10 +358,8 @@ struct sram_channel {
 	u32 jumponly;
 	u32 irq_bit;
 };
-extern struct sram_channel cx25821_sram_channels[];
 
-#define STATUS_SUCCESS         0
-#define STATUS_UNSUCCESSFUL    -1
+extern const struct sram_channel cx25821_sram_channels[];
 
 #define cx_read(reg)             readl(dev->lmmio + ((reg)>>2))
 #define cx_write(reg, value)     writel((value), dev->lmmio + ((reg)>>2))
@@ -529,8 +382,6 @@ extern struct sram_channel cx25821_sram_channels[];
 	pr_info("(%d): " fmt, dev->board, ##args)
 
 extern int cx25821_i2c_register(struct cx25821_i2c *bus);
-extern void cx25821_card_setup(struct cx25821_dev *dev);
-extern int cx25821_ir_init(struct cx25821_dev *dev);
 extern int cx25821_i2c_read(struct cx25821_i2c *bus, u16 reg_addr, int *value);
 extern int cx25821_i2c_write(struct cx25821_i2c *bus, u16 reg_addr, int value);
 extern int cx25821_i2c_unregister(struct cx25821_i2c *bus);
@@ -551,7 +402,7 @@ extern int medusa_set_saturation(struct cx25821_dev *dev, int saturation,
 				 int decoder);
 
 extern int cx25821_sram_channel_setup(struct cx25821_dev *dev,
-				      struct sram_channel *ch, unsigned int bpl,
+				      const struct sram_channel *ch, unsigned int bpl,
 				      u32 risc);
 
 extern int cx25821_risc_buffer(struct pci_dev *pci, struct btcx_riscmem *risc,
@@ -570,46 +421,31 @@ extern void cx25821_free_buffer(struct videobuf_queue *q,
 extern int cx25821_risc_stopper(struct pci_dev *pci, struct btcx_riscmem *risc,
 				u32 reg, u32 mask, u32 value);
 extern void cx25821_sram_channel_dump(struct cx25821_dev *dev,
-				      struct sram_channel *ch);
+				      const struct sram_channel *ch);
 extern void cx25821_sram_channel_dump_audio(struct cx25821_dev *dev,
-					    struct sram_channel *ch);
+					    const struct sram_channel *ch);
 
 extern struct cx25821_dev *cx25821_dev_get(struct pci_dev *pci);
 extern void cx25821_print_irqbits(char *name, char *tag, char **strings,
 				  int len, u32 bits, u32 mask);
 extern void cx25821_dev_unregister(struct cx25821_dev *dev);
 extern int cx25821_sram_channel_setup_audio(struct cx25821_dev *dev,
-					    struct sram_channel *ch,
+					    const struct sram_channel *ch,
 					    unsigned int bpl, u32 risc);
 
-extern int cx25821_vidupstream_init_ch1(struct cx25821_dev *dev,
-					int channel_select, int pixel_format);
-extern int cx25821_vidupstream_init_ch2(struct cx25821_dev *dev,
-					int channel_select, int pixel_format);
+extern int cx25821_vidupstream_init(struct cx25821_channel *chan, int pixel_format);
 extern int cx25821_audio_upstream_init(struct cx25821_dev *dev,
 				       int channel_select);
-extern void cx25821_free_mem_upstream_ch1(struct cx25821_dev *dev);
-extern void cx25821_free_mem_upstream_ch2(struct cx25821_dev *dev);
+extern int cx25821_write_frame(struct cx25821_channel *chan,
+		const char __user *data, size_t count);
+extern void cx25821_free_mem_upstream(struct cx25821_channel *chan);
 extern void cx25821_free_mem_upstream_audio(struct cx25821_dev *dev);
-extern void cx25821_start_upstream_video_ch1(struct cx25821_dev *dev,
-					     struct upstream_user_struct
-					     *up_data);
-extern void cx25821_start_upstream_video_ch2(struct cx25821_dev *dev,
-					     struct upstream_user_struct
-					     *up_data);
-extern void cx25821_start_upstream_audio(struct cx25821_dev *dev,
-					 struct upstream_user_struct *up_data);
-extern void cx25821_stop_upstream_video_ch1(struct cx25821_dev *dev);
-extern void cx25821_stop_upstream_video_ch2(struct cx25821_dev *dev);
+extern void cx25821_stop_upstream_video(struct cx25821_channel *chan);
 extern void cx25821_stop_upstream_audio(struct cx25821_dev *dev);
 extern int cx25821_sram_channel_setup_upstream(struct cx25821_dev *dev,
-					       struct sram_channel *ch,
+					       const struct sram_channel *ch,
 					       unsigned int bpl, u32 risc);
 extern void cx25821_set_pixel_format(struct cx25821_dev *dev, int channel,
 				     u32 format);
-extern void cx25821_videoioctl_unregister(struct cx25821_dev *dev);
-extern struct video_device *cx25821_vdev_init(struct cx25821_dev *dev,
-					      struct pci_dev *pci,
-					      struct video_device *template,
-					      char *type);
+
 #endif
diff --git a/drivers/media/pci/cx88/cx88-blackbird.c b/drivers/media/pci/cx88/cx88-blackbird.c
index a6ff8a6f4fc0..150bb76e7839 100644
--- a/drivers/media/pci/cx88/cx88-blackbird.c
+++ b/drivers/media/pci/cx88/cx88-blackbird.c
@@ -815,7 +815,7 @@ static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
 }
 
 static int vidioc_s_frequency (struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct cx8802_fh  *fh   = priv;
 	struct cx8802_dev *dev  = fh->dev;
@@ -918,7 +918,7 @@ static int vidioc_g_tuner (struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner (struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct cx88_core  *core = ((struct cx8802_fh *)priv)->dev->core;
 
@@ -939,12 +939,12 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *tvnorm)
 	return 0;
 }
 
-static int vidioc_s_std (struct file *file, void *priv, v4l2_std_id *id)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct cx88_core  *core = ((struct cx8802_fh *)priv)->dev->core;
 
 	mutex_lock(&core->lock);
-	cx88_set_tvnorm(core,*id);
+	cx88_set_tvnorm(core, id);
 	mutex_unlock(&core->lock);
 	return 0;
 }
diff --git a/drivers/media/pci/cx88/cx88-cards.c b/drivers/media/pci/cx88/cx88-cards.c
index e2e0b8faf7a4..a87a0e19593e 100644
--- a/drivers/media/pci/cx88/cx88-cards.c
+++ b/drivers/media/pci/cx88/cx88-cards.c
@@ -59,6 +59,11 @@ MODULE_PARM_DESC(disable_ir, "Disable IR support");
 #define err_printk(core, fmt, arg...) \
 	printk(KERN_ERR "%s: " fmt, core->name , ## arg)
 
+#define dprintk(level,fmt, arg...)	do {				\
+	if (cx88_core_debug >= level)					\
+		printk(KERN_DEBUG "%s: " fmt, core->name , ## arg);	\
+	} while(0)
+
 
 /* ------------------------------------------------------------------ */
 /* board config info                                                  */
@@ -2855,6 +2860,7 @@ static void hauppauge_eeprom(struct cx88_core *core, u8 *eeprom_data)
 	core->board.tuner_type = tv.tuner_type;
 	core->tuner_formats = tv.tuner_formats;
 	core->board.radio.type = tv.has_radio ? CX88_RADIO : 0;
+	core->model = tv.model;
 
 	/* Make sure we support the board model */
 	switch (tv.model)
@@ -3133,7 +3139,7 @@ static int cx88_xc2028_tuner_callback(struct cx88_core *core,
 	case XC2028_TUNER_RESET:
 		switch (INPUT(core->input).type) {
 		case CX88_RADIO:
-			info_printk(core, "setting GPIO to radio!\n");
+			dprintk(1, "setting GPIO to radio!\n");
 			cx_write(MO_GP0_IO, 0x4ff);
 			mdelay(250);
 			cx_write(MO_GP2_IO, 0xff);
@@ -3141,7 +3147,7 @@ static int cx88_xc2028_tuner_callback(struct cx88_core *core,
 			break;
 		case CX88_VMUX_DVB:	/* Digital TV*/
 		default:		/* Analog TV */
-			info_printk(core, "setting GPIO to TV!\n");
+			dprintk(1, "setting GPIO to TV!\n");
 			break;
 		}
 		cx_write(MO_GP1_IO, 0x101010);
@@ -3199,8 +3205,7 @@ static int cx88_xc5000_tuner_callback(struct cx88_core *core,
 			   not having any tuning at all. */
 			return 0;
 		} else {
-			err_printk(core, "xc5000: unknown tuner "
-				   "callback command.\n");
+			dprintk(1, "xc5000: unknown tuner callback command.\n");
 			return -EINVAL;
 		}
 		break;
@@ -3211,8 +3216,7 @@ static int cx88_xc5000_tuner_callback(struct cx88_core *core,
 			cx_set(MO_GP0_IO, 0x00000010);
 			return 0;
 		} else {
-			printk(KERN_ERR
-				"xc5000: unknown tuner callback command.\n");
+			dprintk(1, "xc5000: unknown tuner callback command.\n");
 			return -EINVAL;
 		}
 		break;
@@ -3242,13 +3246,13 @@ int cx88_tuner_callback(void *priv, int component, int command, int arg)
 
 	switch (core->board.tuner_type) {
 		case TUNER_XC2028:
-			info_printk(core, "Calling XC2028/3028 callback\n");
+			dprintk(1, "Calling XC2028/3028 callback\n");
 			return cx88_xc2028_tuner_callback(core, command, arg);
 		case TUNER_XC4000:
-			info_printk(core, "Calling XC4000 callback\n");
+			dprintk(1, "Calling XC4000 callback\n");
 			return cx88_xc4000_tuner_callback(core, command, arg);
 		case TUNER_XC5000:
-			info_printk(core, "Calling XC5000 callback\n");
+			dprintk(1, "Calling XC5000 callback\n");
 			return cx88_xc5000_tuner_callback(core, command, arg);
 	}
 	err_printk(core, "Error: Calling callback for tuner %d\n",
@@ -3589,8 +3593,8 @@ static void cx88_card_setup(struct cx88_core *core)
 		memset(&xc2028_cfg, 0, sizeof(xc2028_cfg));
 		xc2028_cfg.tuner = TUNER_XC2028;
 		xc2028_cfg.priv  = &ctl;
-		info_printk(core, "Asking xc2028/3028 to load firmware %s\n",
-			    ctl.fname);
+		dprintk(1, "Asking xc2028/3028 to load firmware %s\n",
+			ctl.fname);
 		call_all(core, tuner, s_config, &xc2028_cfg);
 	}
 	call_all(core, core, s_power, 0);
@@ -3759,8 +3763,8 @@ struct cx88_core *cx88_core_create(struct pci_dev *pci, int nr)
 	if (radio[core->nr] != UNSET)
 		core->board.radio_type = radio[core->nr];
 
-	info_printk(core, "TV tuner type %d, Radio tuner type %d\n",
-		    core->board.tuner_type, core->board.radio_type);
+	dprintk(1, "TV tuner type %d, Radio tuner type %d\n",
+		core->board.tuner_type, core->board.radio_type);
 
 	/* init hardware */
 	cx88_reset(core);
diff --git a/drivers/media/pci/cx88/cx88-core.c b/drivers/media/pci/cx88/cx88-core.c
index 39f095c37ffd..c8f3dcc579d4 100644
--- a/drivers/media/pci/cx88/cx88-core.c
+++ b/drivers/media/pci/cx88/cx88-core.c
@@ -48,9 +48,9 @@ MODULE_LICENSE("GPL");
 
 /* ------------------------------------------------------------------ */
 
-static unsigned int core_debug;
-module_param(core_debug,int,0644);
-MODULE_PARM_DESC(core_debug,"enable debug messages [core]");
+unsigned int cx88_core_debug;
+module_param_named(core_debug, cx88_core_debug, int, 0644);
+MODULE_PARM_DESC(core_debug, "enable debug messages [core]");
 
 static unsigned int nicam;
 module_param(nicam,int,0644);
@@ -60,8 +60,10 @@ static unsigned int nocomb;
 module_param(nocomb,int,0644);
 MODULE_PARM_DESC(nocomb,"disable comb filter");
 
-#define dprintk(level,fmt, arg...)	if (core_debug >= level)	\
-	printk(KERN_DEBUG "%s: " fmt, core->name , ## arg)
+#define dprintk(level,fmt, arg...)	do {				\
+	if (cx88_core_debug >= level)					\
+		printk(KERN_DEBUG "%s: " fmt, core->name , ## arg);	\
+	} while(0)
 
 static unsigned int cx88_devcount;
 static LIST_HEAD(cx88_devlist);
diff --git a/drivers/media/pci/cx88/cx88-dvb.c b/drivers/media/pci/cx88/cx88-dvb.c
index 672b267a2d3e..053ed1ba1d85 100644
--- a/drivers/media/pci/cx88/cx88-dvb.c
+++ b/drivers/media/pci/cx88/cx88-dvb.c
@@ -1042,7 +1042,7 @@ static int dvb_register(struct cx8802_dev *dev)
 			if (!dvb_attach(isl6421_attach,
 					fe0->dvb.frontend,
 					&dev->core->i2c_adap,
-					0x08, ISL6421_DCL, 0x00))
+					0x08, ISL6421_DCL, 0x00, false))
 				goto frontend_detach;
 		}
 		/* MFE frontend 2 */
@@ -1279,8 +1279,16 @@ static int dvb_register(struct cx8802_dev *dev)
 					       &hauppauge_novas_config,
 					       &core->i2c_adap);
 		if (fe0->dvb.frontend) {
+			bool override_tone;
+
+			if (core->model == 92001)
+				override_tone = true;
+			else
+				override_tone = false;
+
 			if (!dvb_attach(isl6421_attach, fe0->dvb.frontend,
-					&core->i2c_adap, 0x08, ISL6421_DCL, 0x00))
+					&core->i2c_adap, 0x08, ISL6421_DCL, 0x00,
+					override_tone))
 				goto frontend_detach;
 		}
 		break;
@@ -1403,7 +1411,7 @@ static int dvb_register(struct cx8802_dev *dev)
 			if (!dvb_attach(isl6421_attach,
 					fe0->dvb.frontend,
 					&dev->core->i2c_adap,
-					0x08, ISL6421_DCL, 0x00))
+					0x08, ISL6421_DCL, 0x00, false))
 				goto frontend_detach;
 		}
 		/* MFE frontend 2 */
@@ -1431,7 +1439,7 @@ static int dvb_register(struct cx8802_dev *dev)
 			if (!dvb_attach(isl6421_attach,
 					fe0->dvb.frontend,
 					&dev->core->i2c_adap,
-					0x08, ISL6421_DCL, 0x00))
+					0x08, ISL6421_DCL, 0x00, false))
 				goto frontend_detach;
 		}
 		break;
diff --git a/drivers/media/pci/cx88/cx88-mpeg.c b/drivers/media/pci/cx88/cx88-mpeg.c
index c9d3182f79d5..2d3507eb4897 100644
--- a/drivers/media/pci/cx88/cx88-mpeg.c
+++ b/drivers/media/pci/cx88/cx88-mpeg.c
@@ -532,16 +532,17 @@ static int cx8802_suspend_common(struct pci_dev *pci_dev, pm_message_t state)
 {
 	struct cx8802_dev *dev = pci_get_drvdata(pci_dev);
 	struct cx88_core *core = dev->core;
+	unsigned long flags;
 
 	/* stop mpeg dma */
-	spin_lock(&dev->slock);
+	spin_lock_irqsave(&dev->slock, flags);
 	if (!list_empty(&dev->mpegq.active)) {
 		dprintk( 2, "suspend\n" );
 		printk("%s: suspend mpeg\n", core->name);
 		cx8802_stop_dma(dev);
 		del_timer(&dev->mpegq.timeout);
 	}
-	spin_unlock(&dev->slock);
+	spin_unlock_irqrestore(&dev->slock, flags);
 
 	/* FIXME -- shutdown device */
 	cx88_shutdown(dev->core);
@@ -558,6 +559,7 @@ static int cx8802_resume_common(struct pci_dev *pci_dev)
 {
 	struct cx8802_dev *dev = pci_get_drvdata(pci_dev);
 	struct cx88_core *core = dev->core;
+	unsigned long flags;
 	int err;
 
 	if (dev->state.disabled) {
@@ -584,12 +586,12 @@ static int cx8802_resume_common(struct pci_dev *pci_dev)
 	cx88_reset(dev->core);
 
 	/* restart video+vbi capture */
-	spin_lock(&dev->slock);
+	spin_lock_irqsave(&dev->slock, flags);
 	if (!list_empty(&dev->mpegq.active)) {
 		printk("%s: resume mpeg\n", core->name);
 		cx8802_restart_queue(dev,&dev->mpegq);
 	}
-	spin_unlock(&dev->slock);
+	spin_unlock_irqrestore(&dev->slock, flags);
 
 	return 0;
 }
diff --git a/drivers/media/pci/cx88/cx88-video.c b/drivers/media/pci/cx88/cx88-video.c
index bc78354262ac..1b00615fd395 100644
--- a/drivers/media/pci/cx88/cx88-video.c
+++ b/drivers/media/pci/cx88/cx88-video.c
@@ -1193,12 +1193,12 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *tvnorm)
 	return 0;
 }
 
-static int vidioc_s_std (struct file *file, void *priv, v4l2_std_id *tvnorms)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id tvnorms)
 {
 	struct cx88_core  *core = ((struct cx8800_fh *)priv)->dev->core;
 
 	mutex_lock(&core->lock);
-	cx88_set_tvnorm(core,*tvnorms);
+	cx88_set_tvnorm(core, tvnorms);
 	mutex_unlock(&core->lock);
 
 	return 0;
@@ -1289,7 +1289,7 @@ static int vidioc_g_tuner (struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner (struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct cx88_core  *core = ((struct cx8800_fh *)priv)->dev->core;
 
@@ -1321,8 +1321,10 @@ static int vidioc_g_frequency (struct file *file, void *priv,
 }
 
 int cx88_set_freq (struct cx88_core  *core,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
+	struct v4l2_frequency new_freq = *f;
+
 	if (unlikely(UNSET == core->board.tuner_type))
 		return -EINVAL;
 	if (unlikely(f->tuner != 0))
@@ -1331,8 +1333,8 @@ int cx88_set_freq (struct cx88_core  *core,
 	mutex_lock(&core->lock);
 	cx88_newstation(core);
 	call_all(core, tuner, s_frequency, f);
-	call_all(core, tuner, g_frequency, f);
-	core->freq = f->frequency;
+	call_all(core, tuner, g_frequency, &new_freq);
+	core->freq = new_freq.frequency;
 
 	/* When changing channels it is required to reset TVAUDIO */
 	msleep (10);
@@ -1345,7 +1347,7 @@ int cx88_set_freq (struct cx88_core  *core,
 EXPORT_SYMBOL(cx88_set_freq);
 
 static int vidioc_s_frequency (struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct cx8800_fh  *fh   = priv;
 	struct cx88_core  *core = fh->dev->core;
@@ -1378,7 +1380,7 @@ static int vidioc_g_register (struct file *file, void *fh,
 }
 
 static int vidioc_s_register (struct file *file, void *fh,
-				struct v4l2_dbg_register *reg)
+				const struct v4l2_dbg_register *reg)
 {
 	struct cx88_core *core = ((struct cx8800_fh*)fh)->dev->core;
 
@@ -1407,20 +1409,15 @@ static int radio_g_tuner (struct file *file, void *priv,
 	return 0;
 }
 
-/* FIXME: Should add a standard for radio */
-
 static int radio_s_tuner (struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct cx88_core  *core = ((struct cx8800_fh *)priv)->dev->core;
 
 	if (0 != t->index)
 		return -EINVAL;
-	if (t->audmode > V4L2_TUNER_MODE_STEREO)
-		t->audmode = V4L2_TUNER_MODE_STEREO;
 
 	call_all(core, tuner, s_tuner, t);
-
 	return 0;
 }
 
@@ -1957,9 +1954,10 @@ static int cx8800_suspend(struct pci_dev *pci_dev, pm_message_t state)
 {
 	struct cx8800_dev *dev = pci_get_drvdata(pci_dev);
 	struct cx88_core *core = dev->core;
+	unsigned long flags;
 
 	/* stop video+vbi capture */
-	spin_lock(&dev->slock);
+	spin_lock_irqsave(&dev->slock, flags);
 	if (!list_empty(&dev->vidq.active)) {
 		printk("%s/0: suspend video\n", core->name);
 		stop_video_dma(dev);
@@ -1970,7 +1968,7 @@ static int cx8800_suspend(struct pci_dev *pci_dev, pm_message_t state)
 		cx8800_stop_vbi_dma(dev);
 		del_timer(&dev->vbiq.timeout);
 	}
-	spin_unlock(&dev->slock);
+	spin_unlock_irqrestore(&dev->slock, flags);
 
 	if (core->ir)
 		cx88_ir_stop(core);
@@ -1989,6 +1987,7 @@ static int cx8800_resume(struct pci_dev *pci_dev)
 {
 	struct cx8800_dev *dev = pci_get_drvdata(pci_dev);
 	struct cx88_core *core = dev->core;
+	unsigned long flags;
 	int err;
 
 	if (dev->state.disabled) {
@@ -2019,7 +2018,7 @@ static int cx8800_resume(struct pci_dev *pci_dev)
 	cx_set(MO_PCI_INTMSK, core->pci_irqmask);
 
 	/* restart video+vbi capture */
-	spin_lock(&dev->slock);
+	spin_lock_irqsave(&dev->slock, flags);
 	if (!list_empty(&dev->vidq.active)) {
 		printk("%s/0: resume video\n", core->name);
 		restart_video_queue(dev,&dev->vidq);
@@ -2028,7 +2027,7 @@ static int cx8800_resume(struct pci_dev *pci_dev)
 		printk("%s/0: resume vbi\n", core->name);
 		cx8800_restart_vbi_queue(dev,&dev->vbiq);
 	}
-	spin_unlock(&dev->slock);
+	spin_unlock_irqrestore(&dev->slock, flags);
 
 	return 0;
 }
diff --git a/drivers/media/pci/cx88/cx88.h b/drivers/media/pci/cx88/cx88.h
index feff53c0a251..51ce2c0e8bc1 100644
--- a/drivers/media/pci/cx88/cx88.h
+++ b/drivers/media/pci/cx88/cx88.h
@@ -334,6 +334,7 @@ struct cx88_core {
 	/* board name */
 	int                        nr;
 	char                       name[32];
+	u32			   model;
 
 	/* pci stuff */
 	int                        pci_bus;
@@ -617,6 +618,8 @@ struct cx8802_dev {
 /* ----------------------------------------------------------- */
 /* cx88-core.c                                                 */
 
+extern unsigned int cx88_core_debug;
+
 extern void cx88_print_irqbits(const char *name, const char *tag, const char *strings[],
 			       int len, u32 bits, u32 mask);
 
@@ -740,7 +743,7 @@ void cx8802_cancel_buffers(struct cx8802_dev *dev);
 /* ----------------------------------------------------------- */
 /* cx88-video.c*/
 int cx88_enum_input (struct cx88_core  *core,struct v4l2_input *i);
-int cx88_set_freq (struct cx88_core  *core,struct v4l2_frequency *f);
+int cx88_set_freq(struct cx88_core  *core, const struct v4l2_frequency *f);
 int cx88_video_mux(struct cx88_core *core, unsigned int input);
 void cx88_querycap(struct file *file, struct cx88_core *core,
 		struct v4l2_capability *cap);
diff --git a/drivers/media/pci/ivtv/ivtv-driver.c b/drivers/media/pci/ivtv/ivtv-driver.c
index 2928e7287da8..07b8460953b6 100644
--- a/drivers/media/pci/ivtv/ivtv-driver.c
+++ b/drivers/media/pci/ivtv/ivtv-driver.c
@@ -1387,7 +1387,7 @@ int ivtv_init_on_first_open(struct ivtv *itv)
 	if (!itv->has_cx23415)
 		write_reg_sync(0x03, IVTV_REG_DMACONTROL);
 
-	ivtv_s_std_enc(itv, &itv->tuner_std);
+	ivtv_s_std_enc(itv, itv->tuner_std);
 
 	/* Default interrupts enabled. For the PVR350 this includes the
 	   decoder VSYNC interrupt, which is always on. It is not only used
@@ -1397,7 +1397,7 @@ int ivtv_init_on_first_open(struct ivtv *itv)
 	if (itv->v4l2_cap & V4L2_CAP_VIDEO_OUTPUT) {
 		ivtv_clear_irq_mask(itv, IVTV_IRQ_MASK_INIT | IVTV_IRQ_DEC_VSYNC);
 		ivtv_set_osd_alpha(itv);
-		ivtv_s_std_dec(itv, &itv->tuner_std);
+		ivtv_s_std_dec(itv, itv->tuner_std);
 	} else {
 		ivtv_clear_irq_mask(itv, IVTV_IRQ_MASK_INIT);
 	}
diff --git a/drivers/media/pci/ivtv/ivtv-firmware.c b/drivers/media/pci/ivtv/ivtv-firmware.c
index 68387d4369d6..ed73edd2bcd3 100644
--- a/drivers/media/pci/ivtv/ivtv-firmware.c
+++ b/drivers/media/pci/ivtv/ivtv-firmware.c
@@ -302,7 +302,7 @@ static int ivtv_firmware_restart(struct ivtv *itv)
 	/* Restore encoder video standard */
 	std = itv->std;
 	itv->std = 0;
-	ivtv_s_std_enc(itv, &std);
+	ivtv_s_std_enc(itv, std);
 
 	if (itv->v4l2_cap & V4L2_CAP_VIDEO_OUTPUT) {
 		ivtv_init_mpeg_decoder(itv);
@@ -310,7 +310,7 @@ static int ivtv_firmware_restart(struct ivtv *itv)
 		/* Restore decoder video standard */
 		std = itv->std_out;
 		itv->std_out = 0;
-		ivtv_s_std_dec(itv, &std);
+		ivtv_s_std_dec(itv, std);
 
 		/* Restore framebuffer if active */
 		if (itv->ivtvfb_restore)
diff --git a/drivers/media/pci/ivtv/ivtv-gpio.c b/drivers/media/pci/ivtv/ivtv-gpio.c
index 8f0d07789053..af52def700cc 100644
--- a/drivers/media/pci/ivtv/ivtv-gpio.c
+++ b/drivers/media/pci/ivtv/ivtv-gpio.c
@@ -192,7 +192,7 @@ static int subdev_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	return 0;
 }
 
-static int subdev_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int subdev_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct ivtv *itv = sd_to_ivtv(sd);
 	u16 mask, data;
diff --git a/drivers/media/pci/ivtv/ivtv-ioctl.c b/drivers/media/pci/ivtv/ivtv-ioctl.c
index 7a8b0d0b6127..9cbbce0eaedc 100644
--- a/drivers/media/pci/ivtv/ivtv-ioctl.c
+++ b/drivers/media/pci/ivtv/ivtv-ioctl.c
@@ -711,28 +711,26 @@ static int ivtv_g_chip_ident(struct file *file, void *fh, struct v4l2_dbg_chip_i
 }
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-static int ivtv_itvc(struct ivtv *itv, unsigned int cmd, void *arg)
+static int ivtv_itvc(struct ivtv *itv, bool get, u64 reg, u64 *val)
 {
-	struct v4l2_dbg_register *regs = arg;
 	volatile u8 __iomem *reg_start;
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
-	if (regs->reg >= IVTV_REG_OFFSET && regs->reg < IVTV_REG_OFFSET + IVTV_REG_SIZE)
+	if (reg >= IVTV_REG_OFFSET && reg < IVTV_REG_OFFSET + IVTV_REG_SIZE)
 		reg_start = itv->reg_mem - IVTV_REG_OFFSET;
-	else if (itv->has_cx23415 && regs->reg >= IVTV_DECODER_OFFSET &&
-			regs->reg < IVTV_DECODER_OFFSET + IVTV_DECODER_SIZE)
+	else if (itv->has_cx23415 && reg >= IVTV_DECODER_OFFSET &&
+			reg < IVTV_DECODER_OFFSET + IVTV_DECODER_SIZE)
 		reg_start = itv->dec_mem - IVTV_DECODER_OFFSET;
-	else if (regs->reg < IVTV_ENCODER_SIZE)
+	else if (reg < IVTV_ENCODER_SIZE)
 		reg_start = itv->enc_mem;
 	else
 		return -EINVAL;
 
-	regs->size = 4;
-	if (cmd == VIDIOC_DBG_G_REGISTER)
-		regs->val = readl(regs->reg + reg_start);
+	if (get)
+		*val = readl(reg + reg_start);
 	else
-		writel(regs->val, regs->reg + reg_start);
+		writel(*val, reg + reg_start);
 	return 0;
 }
 
@@ -740,20 +738,25 @@ static int ivtv_g_register(struct file *file, void *fh, struct v4l2_dbg_register
 {
 	struct ivtv *itv = fh2id(fh)->itv;
 
-	if (v4l2_chip_match_host(&reg->match))
-		return ivtv_itvc(itv, VIDIOC_DBG_G_REGISTER, reg);
+	if (v4l2_chip_match_host(&reg->match)) {
+		reg->size = 4;
+		return ivtv_itvc(itv, true, reg->reg, &reg->val);
+	}
 	/* TODO: subdev errors should not be ignored, this should become a
 	   subdev helper function. */
 	ivtv_call_all(itv, core, g_register, reg);
 	return 0;
 }
 
-static int ivtv_s_register(struct file *file, void *fh, struct v4l2_dbg_register *reg)
+static int ivtv_s_register(struct file *file, void *fh, const struct v4l2_dbg_register *reg)
 {
 	struct ivtv *itv = fh2id(fh)->itv;
 
-	if (v4l2_chip_match_host(&reg->match))
-		return ivtv_itvc(itv, VIDIOC_DBG_S_REGISTER, reg);
+	if (v4l2_chip_match_host(&reg->match)) {
+		u64 val = reg->val;
+
+		return ivtv_itvc(itv, false, reg->reg, &val);
+	}
 	/* TODO: subdev errors should not be ignored, this should become a
 	   subdev helper function. */
 	ivtv_call_all(itv, core, s_register, reg);
@@ -1078,7 +1081,7 @@ static int ivtv_g_frequency(struct file *file, void *fh, struct v4l2_frequency *
 	return 0;
 }
 
-int ivtv_s_frequency(struct file *file, void *fh, struct v4l2_frequency *vf)
+int ivtv_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf)
 {
 	struct ivtv *itv = fh2id(fh)->itv;
 	struct ivtv_stream *s = &itv->streams[fh2id(fh)->type];
@@ -1103,10 +1106,10 @@ static int ivtv_g_std(struct file *file, void *fh, v4l2_std_id *std)
 	return 0;
 }
 
-void ivtv_s_std_enc(struct ivtv *itv, v4l2_std_id *std)
+void ivtv_s_std_enc(struct ivtv *itv, v4l2_std_id std)
 {
-	itv->std = *std;
-	itv->is_60hz = (*std & V4L2_STD_525_60) ? 1 : 0;
+	itv->std = std;
+	itv->is_60hz = (std & V4L2_STD_525_60) ? 1 : 0;
 	itv->is_50hz = !itv->is_60hz;
 	cx2341x_handler_set_50hz(&itv->cxhdl, itv->is_50hz);
 	itv->cxhdl.width = 720;
@@ -1122,15 +1125,15 @@ void ivtv_s_std_enc(struct ivtv *itv, v4l2_std_id *std)
 	ivtv_call_all(itv, core, s_std, itv->std);
 }
 
-void ivtv_s_std_dec(struct ivtv *itv, v4l2_std_id *std)
+void ivtv_s_std_dec(struct ivtv *itv, v4l2_std_id std)
 {
 	struct yuv_playback_info *yi = &itv->yuv_info;
 	DEFINE_WAIT(wait);
 	int f;
 
 	/* set display standard */
-	itv->std_out = *std;
-	itv->is_out_60hz = (*std & V4L2_STD_525_60) ? 1 : 0;
+	itv->std_out = std;
+	itv->is_out_60hz = (std & V4L2_STD_525_60) ? 1 : 0;
 	itv->is_out_50hz = !itv->is_out_60hz;
 	ivtv_call_all(itv, video, s_std_output, itv->std_out);
 
@@ -1168,14 +1171,14 @@ void ivtv_s_std_dec(struct ivtv *itv, v4l2_std_id *std)
 	}
 }
 
-static int ivtv_s_std(struct file *file, void *fh, v4l2_std_id *std)
+static int ivtv_s_std(struct file *file, void *fh, v4l2_std_id std)
 {
 	struct ivtv *itv = fh2id(fh)->itv;
 
-	if ((*std & V4L2_STD_ALL) == 0)
+	if ((std & V4L2_STD_ALL) == 0)
 		return -EINVAL;
 
-	if (*std == itv->std)
+	if (std == itv->std)
 		return 0;
 
 	if (test_bit(IVTV_F_I_RADIO_USER, &itv->i_flags) ||
@@ -1196,7 +1199,7 @@ static int ivtv_s_std(struct file *file, void *fh, v4l2_std_id *std)
 	return 0;
 }
 
-static int ivtv_s_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+static int ivtv_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
 {
 	struct ivtv_open_id *id = fh2id(fh);
 	struct ivtv *itv = id->itv;
@@ -1804,7 +1807,7 @@ static int ivtv_decoder_ioctls(struct file *filp, unsigned int cmd, void *arg)
 }
 
 static long ivtv_default(struct file *file, void *fh, bool valid_prio,
-			 int cmd, void *arg)
+			 unsigned int cmd, void *arg)
 {
 	struct ivtv *itv = fh2id(fh)->itv;
 
diff --git a/drivers/media/pci/ivtv/ivtv-ioctl.h b/drivers/media/pci/ivtv/ivtv-ioctl.h
index 7c553d16579b..75c397756116 100644
--- a/drivers/media/pci/ivtv/ivtv-ioctl.h
+++ b/drivers/media/pci/ivtv/ivtv-ioctl.h
@@ -27,9 +27,9 @@ u16 ivtv_get_service_set(struct v4l2_sliced_vbi_format *fmt);
 void ivtv_set_osd_alpha(struct ivtv *itv);
 int ivtv_set_speed(struct ivtv *itv, int speed);
 void ivtv_set_funcs(struct video_device *vdev);
-void ivtv_s_std_enc(struct ivtv *itv, v4l2_std_id *std);
-void ivtv_s_std_dec(struct ivtv *itv, v4l2_std_id *std);
-int ivtv_s_frequency(struct file *file, void *fh, struct v4l2_frequency *vf);
+void ivtv_s_std_enc(struct ivtv *itv, v4l2_std_id std);
+void ivtv_s_std_dec(struct ivtv *itv, v4l2_std_id std);
+int ivtv_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf);
 int ivtv_s_input(struct file *file, void *fh, unsigned int inp);
 
 #endif
diff --git a/drivers/media/pci/ivtv/ivtvfb.c b/drivers/media/pci/ivtv/ivtvfb.c
index 05b94aa8ba32..9ff1230192e8 100644
--- a/drivers/media/pci/ivtv/ivtvfb.c
+++ b/drivers/media/pci/ivtv/ivtvfb.c
@@ -1171,8 +1171,7 @@ static void ivtvfb_release_buffers (struct ivtv *itv)
 		fb_dealloc_cmap(&oi->ivtvfb_info.cmap);
 
 	/* Release pseudo palette */
-	if (oi->ivtvfb_info.pseudo_palette)
-		kfree(oi->ivtvfb_info.pseudo_palette);
+	kfree(oi->ivtvfb_info.pseudo_palette);
 
 #ifdef CONFIG_MTRR
 	if (oi->fb_end_aligned_physaddr) {
diff --git a/drivers/media/pci/mantis/mantis_i2c.c b/drivers/media/pci/mantis/mantis_i2c.c
index 937fb9d50213..895ddba3c0fb 100644
--- a/drivers/media/pci/mantis/mantis_i2c.c
+++ b/drivers/media/pci/mantis/mantis_i2c.c
@@ -261,6 +261,8 @@ int mantis_i2c_exit(struct mantis_pci *mantis)
 	mmwrite((intmask & ~MANTIS_INT_I2CDONE), MANTIS_INT_MASK);
 
 	dprintk(MANTIS_DEBUG, 1, "Removing I2C adapter");
-	return i2c_del_adapter(&mantis->adapter);
+	i2c_del_adapter(&mantis->adapter);
+
+	return 0;
 }
 EXPORT_SYMBOL_GPL(mantis_i2c_exit);
diff --git a/drivers/media/pci/meye/meye.c b/drivers/media/pci/meye/meye.c
index 7859c43479d7..2381b05432e6 100644
--- a/drivers/media/pci/meye/meye.c
+++ b/drivers/media/pci/meye/meye.c
@@ -1410,7 +1410,7 @@ static int vidioc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
 }
 
 static long vidioc_default(struct file *file, void *fh, bool valid_prio,
-						int cmd, void *arg)
+			   unsigned int cmd, void *arg)
 {
 	switch (cmd) {
 	case MEYEIOC_G_PARAMS:
diff --git a/drivers/media/pci/saa7134/saa7134-cards.c b/drivers/media/pci/saa7134/saa7134-cards.c
index dc68cf1070f7..d45e7f6ff332 100644
--- a/drivers/media/pci/saa7134/saa7134-cards.c
+++ b/drivers/media/pci/saa7134/saa7134-cards.c
@@ -50,6 +50,11 @@ static char name_svideo[]  = "S-Video";
 /* ------------------------------------------------------------------ */
 /* board config info                                                  */
 
+static struct tda18271_std_map aver_a706_std_map = {
+	.fm_radio = { .if_freq = 5500, .fm_rfn = 0, .agc_mode = 3, .std = 0,
+		      .if_lvl = 0, .rfagc_top = 0x2c, },
+};
+
 /* If radio_type !=UNSET, radio_addr should be specified
  */
 
@@ -2760,7 +2765,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config   = 0,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x0200000,
 		.inputs = {{
@@ -3291,7 +3296,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 1,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_ON },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x000200000,
 		.inputs         = {{
@@ -3395,7 +3400,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 1,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_ON },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x0200100,
 		.inputs         = {{
@@ -3426,7 +3431,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 3,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_ON_BRIDGE },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.ts_type	= SAA7134_MPEG_TS_SERIAL,
 		.ts_force_val   = 1,
@@ -3459,7 +3464,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 3,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_ON_BRIDGE },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.ts_type	= SAA7134_MPEG_TS_SERIAL,
 		.gpiomask       = 0x0800100, /* GPIO 21 is an INPUT */
@@ -3683,7 +3688,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x0200000,
 		.inputs = {{
@@ -3736,7 +3741,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x0200000,
 		.inputs = {{
@@ -3754,7 +3759,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.gpiomask	= 1 << 21,
 		.mpeg           = SAA7134_MPEG_DVB,
 		.inputs         = {{
@@ -3887,7 +3892,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 0,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.inputs = {{
 			.name   = name_tv, /* FIXME: analog tv untested */
@@ -3903,7 +3908,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.gpiomask       = 0x020200000,
 		.inputs         = {{
 			.name = name_tv,
@@ -3937,7 +3942,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type	= UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config	= 0,
+		.tda829x_conf	= { .lna_cfg = TDA8290_LNA_OFF },
 		.gpiomask	= 0x020200000,
 		.inputs		= {{
 			.name = name_tv,
@@ -4737,7 +4742,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x0200000,
 		.inputs = {{
@@ -4823,7 +4828,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type   = UNSET,
 		.tuner_addr   = ADDR_UNSET,
 		.radio_addr   = ADDR_UNSET,
-		.tuner_config = 0,
+		.tda829x_conf = { .lna_cfg = TDA8290_LNA_OFF },
 		.mpeg         = SAA7134_MPEG_DVB,
 		.inputs       = {{
 			.name = name_tv,
@@ -4847,7 +4852,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x0200000,
 		.inputs = { {
@@ -5057,7 +5062,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.gpiomask       = 1 << 21,
 		.mpeg           = SAA7134_MPEG_DVB,
 		.inputs         = {{
@@ -5087,7 +5092,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 2,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_GP0_HIGH_OFF },
 		.gpiomask       = 1 << 21,
 		.mpeg           = SAA7134_MPEG_DVB,
 		.inputs         = {{
@@ -5176,7 +5181,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.tuner_config   = 0,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.gpiomask       = 0x0200000,
 		.inputs = { {
@@ -5406,7 +5411,7 @@ struct saa7134_board saa7134_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.tuner_config   = 0,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_OFF },
 		.mpeg           = SAA7134_MPEG_DVB,
 		.ts_type	= SAA7134_MPEG_TS_PARALLEL,
 		.inputs         = {{
@@ -5629,7 +5634,7 @@ struct saa7134_board saa7134_boards[] = {
 		.audio_clock	= 0x00187de7,
 		.tuner_type	= TUNER_PHILIPS_TDA8290,
 		.radio_type	= UNSET,
-		.tuner_config	= 3,
+		.tda829x_conf	= { .lna_cfg = TDA8290_LNA_ON_BRIDGE },
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.gpiomask	= 0x02050000,
@@ -5790,6 +5795,38 @@ struct saa7134_board saa7134_boards[] = {
 			.gpio = 0x6010000,
 		} },
 	},
+	[SAA7134_BOARD_AVERMEDIA_A706] = {
+		.name           = "AverMedia AverTV Satellite Hybrid+FM A706",
+		.audio_clock    = 0x00187de7,
+		.tuner_type     = TUNER_PHILIPS_TDA8290,
+		.radio_type     = UNSET,
+		.tuner_addr     = ADDR_UNSET,
+		.radio_addr     = ADDR_UNSET,
+		.tda829x_conf   = { .lna_cfg = TDA8290_LNA_OFF,
+				    .no_i2c_gate = 1,
+				    .tda18271_std_map = &aver_a706_std_map },
+		.gpiomask       = 1 << 11,
+		.mpeg           = SAA7134_MPEG_DVB,
+		.inputs         = {{
+			.name = name_tv,
+			.vmux = 1,
+			.amux = TV,
+			.tv   = 1,
+		}, {
+			.name = name_comp,
+			.vmux = 4,
+			.amux = LINE1,
+		}, {
+			.name = name_svideo,
+			.vmux = 8,
+			.amux = LINE1,
+		} },
+		.radio = {
+			.name = name_radio,
+			.amux = TV,
+			.gpio = 0x0000800,
+		},
+	},
 
 };
 
@@ -7037,6 +7074,12 @@ struct pci_device_id saa7134_pci_tbl[] = {
 		.subdevice    = 0x0911,
 		.driver_data  = SAA7134_BOARD_SENSORAY811_911,
 	}, {
+		.vendor       = PCI_VENDOR_ID_PHILIPS,
+		.device       = PCI_DEVICE_ID_PHILIPS_SAA7133,
+		.subvendor    = 0x1461, /* Avermedia Technologies Inc */
+		.subdevice    = 0x2055, /* AverTV Satellite Hybrid+FM A706 */
+		.driver_data  = SAA7134_BOARD_AVERMEDIA_A706,
+	}, {
 		/* --- boards without eeprom + subsystem ID --- */
 		.vendor       = PCI_VENDOR_ID_PHILIPS,
 		.device       = PCI_DEVICE_ID_PHILIPS_SAA7134,
@@ -7585,6 +7628,17 @@ int saa7134_board_init1(struct saa7134_dev *dev)
 		saa_andorl(SAA7134_GPIO_GPMODE0 >> 2,   0x80040100, 0x80040100);
 		saa_andorl(SAA7134_GPIO_GPSTATUS0 >> 2, 0x80040100, 0x00040100);
 		break;
+	case SAA7134_BOARD_AVERMEDIA_A706:
+		/* radio antenna select: tristate both as in Windows driver */
+		saa7134_set_gpio(dev, 12, 3);	/* TV antenna */
+		saa7134_set_gpio(dev, 13, 3);	/* FM antenna */
+		dev->has_remote = SAA7134_REMOTE_I2C;
+		/*
+		 * Disable CE5039 DVB-S tuner now (SLEEP pin high) to prevent
+		 * it from interfering with analog tuner detection
+		 */
+		saa7134_set_gpio(dev, 23, 1);
+		break;
 	case SAA7134_BOARD_VIDEOMATE_S350:
 		dev->has_remote = SAA7134_REMOTE_GPIO;
 		saa_andorl(SAA7134_GPIO_GPMODE0 >> 2,   0x0000C000, 0x0000C000);
@@ -7633,7 +7687,7 @@ static void saa7134_tuner_setup(struct saa7134_dev *dev)
 	if ((dev->tuner_type != TUNER_ABSENT) && (dev->tuner_type != UNSET)) {
 		tun_setup.type = dev->tuner_type;
 		tun_setup.addr = dev->tuner_addr;
-		tun_setup.config = saa7134_boards[dev->board].tuner_config;
+		tun_setup.config = &saa7134_boards[dev->board].tda829x_conf;
 		tun_setup.tuner_callback = saa7134_tuner_callback;
 
 		tun_setup.mode_mask = mode_mask;
diff --git a/drivers/media/pci/saa7134/saa7134-core.c b/drivers/media/pci/saa7134/saa7134-core.c
index 8fd24e7c9403..45f0aca597ae 100644
--- a/drivers/media/pci/saa7134/saa7134-core.c
+++ b/drivers/media/pci/saa7134/saa7134-core.c
@@ -805,6 +805,7 @@ static struct video_device *vdev_init(struct saa7134_dev *dev,
 	vfd->debug   = video_debug;
 	snprintf(vfd->name, sizeof(vfd->name), "%s %s (%s)",
 		 dev->name, type, saa7134_boards[dev->board].name);
+	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
 	video_set_drvdata(vfd, dev);
 	return vfd;
 }
@@ -1028,8 +1029,6 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 		}
 	}
 
-	v4l2_prio_init(&dev->prio);
-
 	mutex_lock(&saa7134_devlist_lock);
 	list_for_each_entry(mops, &mops_list, next)
 		mpeg_ops_attach(mops, dev);
diff --git a/drivers/media/pci/saa7134/saa7134-dvb.c b/drivers/media/pci/saa7134/saa7134-dvb.c
index 27915e501db9..4a08ae31df2e 100644
--- a/drivers/media/pci/saa7134/saa7134-dvb.c
+++ b/drivers/media/pci/saa7134/saa7134-dvb.c
@@ -1073,6 +1073,10 @@ static struct mt312_config zl10313_compro_s350_config = {
 	.demod_address = 0x0e,
 };
 
+static struct mt312_config zl10313_avermedia_a706_config = {
+	.demod_address = 0x0e,
+};
+
 static struct lgdt3305_config hcw_lgdt3305_config = {
 	.i2c_addr           = 0x0e,
 	.mpeg_mode          = LGDT3305_MPEG_SERIAL,
@@ -1391,8 +1395,9 @@ static int dvb_init(struct saa7134_dev *dev)
 					wprintk("%s: Lifeview Trio, No tda826x found!\n", __func__);
 					goto detach_frontend;
 				}
-				if (dvb_attach(isl6421_attach, fe0->dvb.frontend, &dev->i2c_adap,
-										0x08, 0, 0) == NULL) {
+				if (dvb_attach(isl6421_attach, fe0->dvb.frontend,
+					       &dev->i2c_adap,
+					       0x08, 0, 0, false) == NULL) {
 					wprintk("%s: Lifeview Trio, No ISL6421 found!\n", __func__);
 					goto detach_frontend;
 				}
@@ -1509,7 +1514,8 @@ static int dvb_init(struct saa7134_dev *dev)
 				goto detach_frontend;
 			}
 			if (dvb_attach(isl6421_attach, fe0->dvb.frontend,
-				       &dev->i2c_adap, 0x08, 0, 0) == NULL) {
+				       &dev->i2c_adap,
+				       0x08, 0, 0, false) == NULL) {
 				wprintk("%s: No ISL6421 found!\n", __func__);
 				goto detach_frontend;
 			}
@@ -1820,6 +1826,25 @@ static int dvb_init(struct saa7134_dev *dev)
 				   &prohdtv_pro2_tda18271_config);
 		}
 		break;
+	case SAA7134_BOARD_AVERMEDIA_A706:
+		/* Enable all DVB-S devices now */
+		/* CE5039 DVB-S tuner SLEEP pin low */
+		saa7134_set_gpio(dev, 23, 0);
+		/* CE6313 DVB-S demod SLEEP pin low */
+		saa7134_set_gpio(dev, 9, 0);
+		/* CE6313 DVB-S demod RESET# pin high */
+		saa7134_set_gpio(dev, 25, 1);
+		msleep(1);
+		fe0->dvb.frontend = dvb_attach(mt312_attach,
+				&zl10313_avermedia_a706_config, &dev->i2c_adap);
+		if (fe0->dvb.frontend) {
+			fe0->dvb.frontend->ops.i2c_gate_ctrl = NULL;
+			if (dvb_attach(zl10039_attach, fe0->dvb.frontend,
+					0x60, &dev->i2c_adap) == NULL)
+				wprintk("%s: No zl10039 found!\n",
+					__func__);
+		}
+		break;
 	default:
 		wprintk("Huh? unknown DVB card?\n");
 		break;
diff --git a/drivers/media/pci/saa7134/saa7134-empress.c b/drivers/media/pci/saa7134/saa7134-empress.c
index 4df79c656909..66a70814004c 100644
--- a/drivers/media/pci/saa7134/saa7134-empress.c
+++ b/drivers/media/pci/saa7134/saa7134-empress.c
@@ -428,7 +428,7 @@ static int empress_g_chip_ident(struct file *file, void *fh,
 	return -EINVAL;
 }
 
-static int empress_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static int empress_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct saa7134_dev *dev = file->private_data;
 
diff --git a/drivers/media/pci/saa7134/saa7134-i2c.c b/drivers/media/pci/saa7134/saa7134-i2c.c
index a176ec3285e0..c68169d75804 100644
--- a/drivers/media/pci/saa7134/saa7134-i2c.c
+++ b/drivers/media/pci/saa7134/saa7134-i2c.c
@@ -256,6 +256,7 @@ static int saa7134_i2c_xfer(struct i2c_adapter *i2c_adap,
 				addr |= 1;
 			if (i > 0 && msgs[i].flags &
 			    I2C_M_RD && msgs[i].addr != 0x40 &&
+			    msgs[i].addr != 0x41 &&
 			    msgs[i].addr != 0x19) {
 				/* workaround for a saa7134 i2c bug
 				 * needed to talk to the mt352 demux
diff --git a/drivers/media/pci/saa7134/saa7134-input.c b/drivers/media/pci/saa7134/saa7134-input.c
index e761262f7475..6f4312663bdf 100644
--- a/drivers/media/pci/saa7134/saa7134-input.c
+++ b/drivers/media/pci/saa7134/saa7134-input.c
@@ -997,6 +997,9 @@ void saa7134_probe_i2c_ir(struct saa7134_dev *dev)
 	case SAA7134_BOARD_AVERMEDIA_CARDBUS_506:
 		info.addr = 0x40;
 		break;
+	case SAA7134_BOARD_AVERMEDIA_A706:
+		info.addr = 0x41;
+		break;
 	case SAA7134_BOARD_FLYDVB_TRIO:
 		dev->init_data.name = "FlyDVB Trio";
 		dev->init_data.get_key = get_key_flydvb_trio;
diff --git a/drivers/media/pci/saa7134/saa7134-tvaudio.c b/drivers/media/pci/saa7134/saa7134-tvaudio.c
index b7a99bee2f98..0f34e09d98dc 100644
--- a/drivers/media/pci/saa7134/saa7134-tvaudio.c
+++ b/drivers/media/pci/saa7134/saa7134-tvaudio.c
@@ -796,6 +796,7 @@ static int tvaudio_thread_ddep(void *data)
 			dprintk("FM Radio\n");
 			if (dev->tuner_type == TUNER_PHILIPS_TDA8290) {
 				norms = (0x11 << 2) | 0x01;
+				/* set IF frequency to 5.5 MHz */
 				saa_dsp_writel(dev, 0x42c >> 2, 0x729555);
 			} else {
 				norms = (0x0f << 2) | 0x01;
diff --git a/drivers/media/pci/saa7134/saa7134-video.c b/drivers/media/pci/saa7134/saa7134-video.c
index 7c503fb68526..cc409380ee16 100644
--- a/drivers/media/pci/saa7134/saa7134-video.c
+++ b/drivers/media/pci/saa7134/saa7134-video.c
@@ -1176,14 +1176,6 @@ int saa7134_s_ctrl_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, str
 	int restart_overlay = 0;
 	int err;
 
-	/* When called from the empress code fh == NULL.
-	   That needs to be fixed somehow, but for now this is
-	   good enough. */
-	if (fh) {
-		err = v4l2_prio_check(&dev->prio, fh->prio);
-		if (0 != err)
-			return err;
-	}
 	err = -EINVAL;
 
 	mutex_lock(&dev->lock);
@@ -1352,6 +1344,7 @@ static int video_open(struct file *file)
 	if (NULL == fh)
 		return -ENOMEM;
 
+	v4l2_fh_init(&fh->fh, vdev);
 	file->private_data = fh;
 	fh->dev      = dev;
 	fh->radio    = radio;
@@ -1359,7 +1352,6 @@ static int video_open(struct file *file)
 	fh->fmt      = format_by_fourcc(V4L2_PIX_FMT_BGR24);
 	fh->width    = 720;
 	fh->height   = 576;
-	v4l2_prio_open(&dev->prio, &fh->prio);
 
 	videobuf_queue_sg_init(&fh->cap, &video_qops,
 			    &dev->pci->dev, &dev->slock,
@@ -1384,6 +1376,8 @@ static int video_open(struct file *file)
 		/* switch to video/vbi mode */
 		video_mux(dev,dev->ctl_input);
 	}
+	v4l2_fh_add(&fh->fh);
+
 	return 0;
 }
 
@@ -1504,7 +1498,8 @@ static int video_release(struct file *file)
 	saa7134_pgtable_free(dev->pci,&fh->pt_cap);
 	saa7134_pgtable_free(dev->pci,&fh->pt_vbi);
 
-	v4l2_prio_close(&dev->prio, fh->prio);
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
 	file->private_data = NULL;
 	kfree(fh);
 	return 0;
@@ -1557,6 +1552,7 @@ static int saa7134_try_get_set_fmt_vbi_cap(struct file *file, void *priv,
 	struct saa7134_dev *dev = fh->dev;
 	struct saa7134_tvnorm *norm = dev->tvnorm;
 
+	memset(&f->fmt.vbi.reserved, 0, sizeof(f->fmt.vbi.reserved));
 	f->fmt.vbi.sampling_rate = 6750000 * 4;
 	f->fmt.vbi.samples_per_line = 2048 /* VBI_LINE_LENGTH */;
 	f->fmt.vbi.sample_format = V4L2_PIX_FMT_GREY;
@@ -1755,7 +1751,6 @@ static int saa7134_enum_input(struct file *file, void *priv,
 	strcpy(i->name, card_in(dev, n).name);
 	if (card_in(dev, n).tv)
 		i->type = V4L2_INPUT_TYPE_TUNER;
-	i->audioset = 1;
 	if (n == dev->ctl_input) {
 		int v1 = saa_readb(SAA7134_STATUS_VIDEO1);
 		int v2 = saa_readb(SAA7134_STATUS_VIDEO2);
@@ -1784,11 +1779,6 @@ static int saa7134_s_input(struct file *file, void *priv, unsigned int i)
 {
 	struct saa7134_fh *fh = priv;
 	struct saa7134_dev *dev = fh->dev;
-	int err;
-
-	err = v4l2_prio_check(&dev->prio, fh->prio);
-	if (0 != err)
-		return err;
 
 	if (i >= SAA7134_INPUT_MAX)
 		return -EINVAL;
@@ -1805,6 +1795,8 @@ static int saa7134_querycap(struct file *file, void  *priv,
 {
 	struct saa7134_fh *fh = priv;
 	struct saa7134_dev *dev = fh->dev;
+	struct video_device *vdev = video_devdata(file);
+	u32 radio_caps, video_caps, vbi_caps;
 
 	unsigned int tuner_type = dev->tuner_type;
 
@@ -1812,54 +1804,67 @@ static int saa7134_querycap(struct file *file, void  *priv,
 	strlcpy(cap->card, saa7134_boards[dev->board].name,
 		sizeof(cap->card));
 	sprintf(cap->bus_info, "PCI:%s", pci_name(dev->pci));
-	cap->capabilities =
-		V4L2_CAP_VIDEO_CAPTURE |
-		V4L2_CAP_VBI_CAPTURE |
-		V4L2_CAP_READWRITE |
-		V4L2_CAP_STREAMING |
-		V4L2_CAP_TUNER;
+
+	cap->device_caps = V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+	if ((tuner_type != TUNER_ABSENT) && (tuner_type != UNSET))
+		cap->device_caps |= V4L2_CAP_TUNER;
+
+	radio_caps = V4L2_CAP_RADIO;
 	if (dev->has_rds)
-		cap->capabilities |= V4L2_CAP_RDS_CAPTURE;
+		radio_caps |= V4L2_CAP_RDS_CAPTURE;
+
+	video_caps = V4L2_CAP_VIDEO_CAPTURE;
 	if (saa7134_no_overlay <= 0)
-		cap->capabilities |= V4L2_CAP_VIDEO_OVERLAY;
+		video_caps |= V4L2_CAP_VIDEO_OVERLAY;
+
+	vbi_caps = V4L2_CAP_VBI_CAPTURE;
+
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_RADIO:
+		cap->device_caps |= radio_caps;
+		break;
+	case VFL_TYPE_GRABBER:
+		cap->device_caps |= video_caps;
+		break;
+	case VFL_TYPE_VBI:
+		cap->device_caps |= vbi_caps;
+		break;
+	}
+	cap->capabilities = radio_caps | video_caps | vbi_caps |
+		cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	if (vdev->vfl_type == VFL_TYPE_RADIO) {
+		cap->device_caps &= ~V4L2_CAP_STREAMING;
+		if (!dev->has_rds)
+			cap->device_caps &= ~V4L2_CAP_READWRITE;
+	}
 
-	if ((tuner_type == TUNER_ABSENT) || (tuner_type == UNSET))
-		cap->capabilities &= ~V4L2_CAP_TUNER;
 	return 0;
 }
 
-int saa7134_s_std_internal(struct saa7134_dev *dev, struct saa7134_fh *fh, v4l2_std_id *id)
+int saa7134_s_std_internal(struct saa7134_dev *dev, struct saa7134_fh *fh, v4l2_std_id id)
 {
 	unsigned long flags;
 	unsigned int i;
 	v4l2_std_id fixup;
-	int err;
 
-	/* When called from the empress code fh == NULL.
-	   That needs to be fixed somehow, but for now this is
-	   good enough. */
-	if (fh) {
-		err = v4l2_prio_check(&dev->prio, fh->prio);
-		if (0 != err)
-			return err;
-	} else if (res_locked(dev, RESOURCE_OVERLAY)) {
+	if (!fh && res_locked(dev, RESOURCE_OVERLAY)) {
 		/* Don't change the std from the mpeg device
 		   if overlay is active. */
 		return -EBUSY;
 	}
 
 	for (i = 0; i < TVNORMS; i++)
-		if (*id == tvnorms[i].id)
+		if (id == tvnorms[i].id)
 			break;
 
 	if (i == TVNORMS)
 		for (i = 0; i < TVNORMS; i++)
-			if (*id & tvnorms[i].id)
+			if (id & tvnorms[i].id)
 				break;
 	if (i == TVNORMS)
 		return -EINVAL;
 
-	if ((*id & V4L2_STD_SECAM) && (secam[0] != '-')) {
+	if ((id & V4L2_STD_SECAM) && (secam[0] != '-')) {
 		if (secam[0] == 'L' || secam[0] == 'l') {
 			if (secam[1] == 'C' || secam[1] == 'c')
 				fixup = V4L2_STD_SECAM_LC;
@@ -1879,7 +1884,7 @@ int saa7134_s_std_internal(struct saa7134_dev *dev, struct saa7134_fh *fh, v4l2_
 			return -EINVAL;
 	}
 
-	*id = tvnorms[i].id;
+	id = tvnorms[i].id;
 
 	mutex_lock(&dev->lock);
 	if (fh && res_check(fh, RESOURCE_OVERLAY)) {
@@ -1901,7 +1906,7 @@ int saa7134_s_std_internal(struct saa7134_dev *dev, struct saa7134_fh *fh, v4l2_
 }
 EXPORT_SYMBOL_GPL(saa7134_s_std_internal);
 
-static int saa7134_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static int saa7134_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct saa7134_fh *fh = priv;
 
@@ -2009,11 +2014,11 @@ static int saa7134_g_tuner(struct file *file, void *priv,
 	if (NULL != card_in(dev, n).name) {
 		strcpy(t->name, "Television");
 		t->type = V4L2_TUNER_ANALOG_TV;
+		saa_call_all(dev, tuner, g_tuner, t);
 		t->capability = V4L2_TUNER_CAP_NORM |
 			V4L2_TUNER_CAP_STEREO |
 			V4L2_TUNER_CAP_LANG1 |
 			V4L2_TUNER_CAP_LANG2;
-		t->rangehigh = 0xffffffffUL;
 		t->rxsubchans = saa7134_tvaudio_getstereo(dev);
 		t->audmode = saa7134_tvaudio_rx2mode(t->rxsubchans);
 	}
@@ -2023,15 +2028,14 @@ static int saa7134_g_tuner(struct file *file, void *priv,
 }
 
 static int saa7134_s_tuner(struct file *file, void *priv,
-					struct v4l2_tuner *t)
+					const struct v4l2_tuner *t)
 {
 	struct saa7134_fh *fh = priv;
 	struct saa7134_dev *dev = fh->dev;
-	int rx, mode, err;
+	int rx, mode;
 
-	err = v4l2_prio_check(&dev->prio, fh->prio);
-	if (0 != err)
-		return err;
+	if (0 != t->index)
+		return -EINVAL;
 
 	mode = dev->thread.mode;
 	if (UNSET == mode) {
@@ -2050,22 +2054,20 @@ static int saa7134_g_frequency(struct file *file, void *priv,
 	struct saa7134_fh *fh = priv;
 	struct saa7134_dev *dev = fh->dev;
 
+	if (0 != f->tuner)
+		return -EINVAL;
+
 	f->type = fh->radio ? V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
-	f->frequency = dev->ctl_freq;
+	saa_call_all(dev, tuner, g_frequency, f);
 
 	return 0;
 }
 
 static int saa7134_s_frequency(struct file *file, void *priv,
-					struct v4l2_frequency *f)
+					const struct v4l2_frequency *f)
 {
 	struct saa7134_fh *fh = priv;
 	struct saa7134_dev *dev = fh->dev;
-	int err;
-
-	err = v4l2_prio_check(&dev->prio, fh->prio);
-	if (0 != err)
-		return err;
 
 	if (0 != f->tuner)
 		return -EINVAL;
@@ -2074,7 +2076,6 @@ static int saa7134_s_frequency(struct file *file, void *priv,
 	if (1 == fh->radio && V4L2_TUNER_RADIO != f->type)
 		return -EINVAL;
 	mutex_lock(&dev->lock);
-	dev->ctl_freq = f->frequency;
 
 	saa_call_all(dev, tuner, s_frequency, f);
 
@@ -2083,35 +2084,6 @@ static int saa7134_s_frequency(struct file *file, void *priv,
 	return 0;
 }
 
-static int saa7134_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
-{
-	strcpy(a->name, "audio");
-	return 0;
-}
-
-static int saa7134_s_audio(struct file *file, void *priv, const struct v4l2_audio *a)
-{
-	return 0;
-}
-
-static int saa7134_g_priority(struct file *file, void *f, enum v4l2_priority *p)
-{
-	struct saa7134_fh *fh = f;
-	struct saa7134_dev *dev = fh->dev;
-
-	*p = v4l2_prio_max(&dev->prio);
-	return 0;
-}
-
-static int saa7134_s_priority(struct file *file, void *f,
-					enum v4l2_priority prio)
-{
-	struct saa7134_fh *fh = f;
-	struct saa7134_dev *dev = fh->dev;
-
-	return v4l2_prio_change(&dev->prio, &fh->prio, prio);
-}
-
 static int saa7134_enum_fmt_vid_cap(struct file *file, void  *priv,
 					struct v4l2_fmtdesc *f)
 {
@@ -2279,12 +2251,6 @@ static int saa7134_streamoff(struct file *file, void *priv,
 	return 0;
 }
 
-static int saa7134_g_parm(struct file *file, void *fh,
-				struct v4l2_streamparm *parm)
-{
-	return 0;
-}
-
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 static int vidioc_g_register (struct file *file, void *priv,
 			      struct v4l2_dbg_register *reg)
@@ -2300,7 +2266,7 @@ static int vidioc_g_register (struct file *file, void *priv,
 }
 
 static int vidioc_s_register (struct file *file, void *priv,
-				struct v4l2_dbg_register *reg)
+				const struct v4l2_dbg_register *reg)
 {
 	struct saa7134_fh *fh = priv;
 	struct saa7134_dev *dev = fh->dev;
@@ -2312,19 +2278,6 @@ static int vidioc_s_register (struct file *file, void *priv,
 }
 #endif
 
-static int radio_querycap(struct file *file, void *priv,
-					struct v4l2_capability *cap)
-{
-	struct saa7134_fh *fh = file->private_data;
-	struct saa7134_dev *dev = fh->dev;
-
-	strcpy(cap->driver, "saa7134");
-	strlcpy(cap->card, saa7134_boards[dev->board].name, sizeof(cap->card));
-	sprintf(cap->bus_info, "PCI:%s", pci_name(dev->pci));
-	cap->capabilities = V4L2_CAP_TUNER;
-	return 0;
-}
-
 static int radio_g_tuner(struct file *file, void *priv,
 					struct v4l2_tuner *t)
 {
@@ -2339,6 +2292,7 @@ static int radio_g_tuner(struct file *file, void *priv,
 	t->type = V4L2_TUNER_RADIO;
 
 	saa_call_all(dev, tuner, g_tuner, t);
+	t->audmode &= V4L2_TUNER_MODE_MONO | V4L2_TUNER_MODE_STEREO;
 	if (dev->input->amux == TV) {
 		t->signal = 0xf800 - ((saa_readb(0x581) & 0x1f) << 11);
 		t->rxsubchans = (saa_readb(0x529) & 0x08) ?
@@ -2347,7 +2301,7 @@ static int radio_g_tuner(struct file *file, void *priv,
 	return 0;
 }
 static int radio_s_tuner(struct file *file, void *priv,
-					struct v4l2_tuner *t)
+					const struct v4l2_tuner *t)
 {
 	struct saa7134_fh *fh = file->private_data;
 	struct saa7134_dev *dev = fh->dev;
@@ -2377,26 +2331,12 @@ static int radio_g_input(struct file *filp, void *priv, unsigned int *i)
 	return 0;
 }
 
-static int radio_g_audio(struct file *file, void *priv,
-					struct v4l2_audio *a)
-{
-	memset(a, 0, sizeof(*a));
-	strcpy(a->name, "Radio");
-	return 0;
-}
-
-static int radio_s_audio(struct file *file, void *priv,
-					const struct v4l2_audio *a)
-{
-	return 0;
-}
-
 static int radio_s_input(struct file *filp, void *priv, unsigned int i)
 {
 	return 0;
 }
 
-static int radio_s_std(struct file *file, void *fh, v4l2_std_id *norm)
+static int radio_s_std(struct file *file, void *fh, v4l2_std_id norm)
 {
 	return 0;
 }
@@ -2441,8 +2381,6 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_g_fmt_vbi_cap		= saa7134_try_get_set_fmt_vbi_cap,
 	.vidioc_try_fmt_vbi_cap		= saa7134_try_get_set_fmt_vbi_cap,
 	.vidioc_s_fmt_vbi_cap		= saa7134_try_get_set_fmt_vbi_cap,
-	.vidioc_g_audio			= saa7134_g_audio,
-	.vidioc_s_audio			= saa7134_s_audio,
 	.vidioc_cropcap			= saa7134_cropcap,
 	.vidioc_reqbufs			= saa7134_reqbufs,
 	.vidioc_querybuf		= saa7134_querybuf,
@@ -2465,9 +2403,6 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_g_fbuf			= saa7134_g_fbuf,
 	.vidioc_s_fbuf			= saa7134_s_fbuf,
 	.vidioc_overlay			= saa7134_overlay,
-	.vidioc_g_priority		= saa7134_g_priority,
-	.vidioc_s_priority		= saa7134_s_priority,
-	.vidioc_g_parm			= saa7134_g_parm,
 	.vidioc_g_frequency		= saa7134_g_frequency,
 	.vidioc_s_frequency		= saa7134_s_frequency,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
@@ -2486,12 +2421,10 @@ static const struct v4l2_file_operations radio_fops = {
 };
 
 static const struct v4l2_ioctl_ops radio_ioctl_ops = {
-	.vidioc_querycap	= radio_querycap,
+	.vidioc_querycap	= saa7134_querycap,
 	.vidioc_g_tuner		= radio_g_tuner,
 	.vidioc_enum_input	= radio_enum_input,
-	.vidioc_g_audio		= radio_g_audio,
 	.vidioc_s_tuner		= radio_s_tuner,
-	.vidioc_s_audio		= radio_s_audio,
 	.vidioc_s_input		= radio_s_input,
 	.vidioc_s_std		= radio_s_std,
 	.vidioc_queryctrl	= radio_queryctrl,
diff --git a/drivers/media/pci/saa7134/saa7134.h b/drivers/media/pci/saa7134/saa7134.h
index 71eefef5e324..d2ad16c1569a 100644
--- a/drivers/media/pci/saa7134/saa7134.h
+++ b/drivers/media/pci/saa7134/saa7134.h
@@ -36,6 +36,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
 #include <media/tuner.h>
 #include <media/rc-core.h>
 #include <media/ir-kbd-i2c.h>
@@ -45,6 +46,7 @@
 #if IS_ENABLED(CONFIG_VIDEO_SAA7134_DVB)
 #include <media/videobuf-dvb.h>
 #endif
+#include "tda8290.h"
 
 #define UNSET (-1U)
 
@@ -334,6 +336,7 @@ struct saa7134_card_ir {
 #define SAA7134_BOARD_KWORLD_PC150U         189
 #define SAA7134_BOARD_ASUSTeK_PS3_100      190
 #define SAA7134_BOARD_HAWELL_HW_9004V1      191
+#define SAA7134_BOARD_AVERMEDIA_A706		192
 
 #define SAA7134_MAXBOARDS 32
 #define SAA7134_INPUT_MAX 8
@@ -390,7 +393,7 @@ struct saa7134_board {
 	unsigned char		rds_addr;
 
 	unsigned int            tda9887_conf;
-	unsigned int            tuner_config;
+	struct tda829x_config   tda829x_conf;
 
 	/* peripheral I/O */
 	enum saa7134_video_out  video_out;
@@ -466,11 +469,11 @@ struct saa7134_dmaqueue {
 
 /* video filehandle status */
 struct saa7134_fh {
+	struct v4l2_fh             fh;
 	struct saa7134_dev         *dev;
 	unsigned int               radio;
 	enum v4l2_buf_type         type;
 	unsigned int               resources;
-	enum v4l2_priority	   prio;
 	struct pm_qos_request	   qos_request;
 
 	/* video overlay */
@@ -542,7 +545,6 @@ struct saa7134_dev {
 	struct list_head           devlist;
 	struct mutex               lock;
 	spinlock_t                 slock;
-	struct v4l2_prio_state     prio;
 	struct v4l2_device         v4l2_dev;
 	/* workstruct for loading modules */
 	struct work_struct request_module_wk;
@@ -605,7 +607,6 @@ struct saa7134_dev {
 	int                        ctl_contrast;
 	int                        ctl_hue;
 	int                        ctl_saturation;
-	int                        ctl_freq;
 	int                        ctl_mute;             /* audio */
 	int                        ctl_volume;
 	int                        ctl_invert;           /* private */
@@ -766,7 +767,7 @@ extern struct video_device saa7134_radio_template;
 int saa7134_s_ctrl_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, struct v4l2_control *c);
 int saa7134_g_ctrl_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, struct v4l2_control *c);
 int saa7134_queryctrl(struct file *file, void *priv, struct v4l2_queryctrl *c);
-int saa7134_s_std_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, v4l2_std_id *id);
+int saa7134_s_std_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, v4l2_std_id id);
 
 int saa7134_videoport_init(struct saa7134_dev *dev);
 void saa7134_set_tvnorm_hw(struct saa7134_dev *dev);
diff --git a/drivers/media/pci/saa7146/mxb.c b/drivers/media/pci/saa7146/mxb.c
index 91369daad722..71e8bead321b 100644
--- a/drivers/media/pci/saa7146/mxb.c
+++ b/drivers/media/pci/saa7146/mxb.c
@@ -560,7 +560,7 @@ static int vidioc_g_tuner(struct file *file, void *fh, struct v4l2_tuner *t)
 	return call_all(dev, tuner, g_tuner, t);
 }
 
-static int vidioc_s_tuner(struct file *file, void *fh, struct v4l2_tuner *t)
+static int vidioc_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *t)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct mxb *mxb = (struct mxb *)dev->ext_priv;
@@ -595,7 +595,7 @@ static int vidioc_g_frequency(struct file *file, void *fh, struct v4l2_frequency
 	return 0;
 }
 
-static int vidioc_s_frequency(struct file *file, void *fh, struct v4l2_frequency *f)
+static int vidioc_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *f)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct mxb *mxb = (struct mxb *)dev->ext_priv;
@@ -612,8 +612,8 @@ static int vidioc_s_frequency(struct file *file, void *fh, struct v4l2_frequency
 	/* tune in desired frequency */
 	tuner_call(mxb, tuner, s_frequency, f);
 	/* let the tuner subdev clamp the frequency to the tuner range */
-	tuner_call(mxb, tuner, g_frequency, f);
 	mxb->cur_freq = *f;
+	tuner_call(mxb, tuner, g_frequency, &mxb->cur_freq);
 	if (mxb->cur_audinput == 0)
 		mxb_update_audmode(mxb);
 
@@ -680,7 +680,7 @@ static int vidioc_g_register(struct file *file, void *fh, struct v4l2_dbg_regist
 	return 0;
 }
 
-static int vidioc_s_register(struct file *file, void *fh, struct v4l2_dbg_register *reg)
+static int vidioc_s_register(struct file *file, void *fh, const struct v4l2_dbg_register *reg)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 
@@ -688,7 +688,6 @@ static int vidioc_s_register(struct file *file, void *fh, struct v4l2_dbg_regist
 		return -EPERM;
 	if (v4l2_chip_match_host(&reg->match)) {
 		saa7146_write(dev, reg->reg, reg->val);
-		reg->size = 4;
 		return 0;
 	}
 	return call_all(dev, core, s_register, reg);
diff --git a/drivers/media/pci/saa7164/saa7164-encoder.c b/drivers/media/pci/saa7164/saa7164-encoder.c
index 9bb0903ee5f1..0b74fb2300dd 100644
--- a/drivers/media/pci/saa7164/saa7164-encoder.c
+++ b/drivers/media/pci/saa7164/saa7164-encoder.c
@@ -211,17 +211,17 @@ static int saa7164_encoder_initialize(struct saa7164_port *port)
 }
 
 /* -- V4L2 --------------------------------------------------------- */
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct saa7164_encoder_fh *fh = file->private_data;
 	struct saa7164_port *port = fh->port;
 	struct saa7164_dev *dev = port->dev;
 	unsigned int i;
 
-	dprintk(DBGLVL_ENC, "%s(id=0x%x)\n", __func__, (u32)*id);
+	dprintk(DBGLVL_ENC, "%s(id=0x%x)\n", __func__, (u32)id);
 
 	for (i = 0; i < ARRAY_SIZE(saa7164_tvnorms); i++) {
-		if (*id & saa7164_tvnorms[i].id)
+		if (id & saa7164_tvnorms[i].id)
 			break;
 	}
 	if (i == ARRAY_SIZE(saa7164_tvnorms))
@@ -234,7 +234,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
 	 */
 	saa7164_api_set_audio_std(port);
 
-	dprintk(DBGLVL_ENC, "%s(id=0x%x) OK\n", __func__, (u32)*id);
+	dprintk(DBGLVL_ENC, "%s(id=0x%x) OK\n", __func__, (u32)id);
 
 	return 0;
 }
@@ -318,7 +318,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-	struct v4l2_tuner *t)
+	const struct v4l2_tuner *t)
 {
 	/* Update the A/V core */
 	return 0;
@@ -337,7 +337,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-	struct v4l2_frequency *f)
+	const struct v4l2_frequency *f)
 {
 	struct saa7164_encoder_fh *fh = file->private_data;
 	struct saa7164_port *port = fh->port;
@@ -1313,7 +1313,7 @@ static int saa7164_g_register(struct file *file, void *fh,
 }
 
 static int saa7164_s_register(struct file *file, void *fh,
-			      struct v4l2_dbg_register *reg)
+			      const struct v4l2_dbg_register *reg)
 {
 	struct saa7164_port *port = ((struct saa7164_encoder_fh *)fh)->port;
 	struct saa7164_dev *dev = port->dev;
diff --git a/drivers/media/pci/saa7164/saa7164-vbi.c b/drivers/media/pci/saa7164/saa7164-vbi.c
index b4532299c0ed..da224eb39b95 100644
--- a/drivers/media/pci/saa7164/saa7164-vbi.c
+++ b/drivers/media/pci/saa7164/saa7164-vbi.c
@@ -183,17 +183,17 @@ static int saa7164_vbi_initialize(struct saa7164_port *port)
 }
 
 /* -- V4L2 --------------------------------------------------------- */
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct saa7164_vbi_fh *fh = file->private_data;
 	struct saa7164_port *port = fh->port;
 	struct saa7164_dev *dev = port->dev;
 	unsigned int i;
 
-	dprintk(DBGLVL_VBI, "%s(id=0x%x)\n", __func__, (u32)*id);
+	dprintk(DBGLVL_VBI, "%s(id=0x%x)\n", __func__, (u32)id);
 
 	for (i = 0; i < ARRAY_SIZE(saa7164_tvnorms); i++) {
-		if (*id & saa7164_tvnorms[i].id)
+		if (id & saa7164_tvnorms[i].id)
 			break;
 	}
 	if (i == ARRAY_SIZE(saa7164_tvnorms))
@@ -206,7 +206,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
 	 */
 	saa7164_api_set_audio_std(port);
 
-	dprintk(DBGLVL_VBI, "%s(id=0x%x) OK\n", __func__, (u32)*id);
+	dprintk(DBGLVL_VBI, "%s(id=0x%x) OK\n", __func__, (u32)id);
 
 	return 0;
 }
@@ -290,7 +290,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-	struct v4l2_tuner *t)
+	const struct v4l2_tuner *t)
 {
 	/* Update the A/V core */
 	return 0;
@@ -309,7 +309,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-	struct v4l2_frequency *f)
+	const struct v4l2_frequency *f)
 {
 	struct saa7164_vbi_fh *fh = file->private_data;
 	struct saa7164_port *port = fh->port;
diff --git a/drivers/media/pci/sta2x11/Kconfig b/drivers/media/pci/sta2x11/Kconfig
index a94ccad02066..03130157db83 100644
--- a/drivers/media/pci/sta2x11/Kconfig
+++ b/drivers/media/pci/sta2x11/Kconfig
@@ -4,6 +4,7 @@ config STA2X11_VIP
 	select VIDEO_ADV7180 if MEDIA_SUBDRV_AUTOSELECT
 	select VIDEOBUF2_DMA_CONTIG
 	depends on PCI && VIDEO_V4L2 && VIRT_TO_BUS
+	depends on I2C
 	help
 	  Say Y for support for STA2X11 VIP (Video Input Port) capture
 	  device.
diff --git a/drivers/media/pci/sta2x11/sta2x11_vip.c b/drivers/media/pci/sta2x11/sta2x11_vip.c
index 4b703fe8c953..7005695aa4bd 100644
--- a/drivers/media/pci/sta2x11/sta2x11_vip.c
+++ b/drivers/media/pci/sta2x11/sta2x11_vip.c
@@ -439,22 +439,22 @@ static int vidioc_querycap(struct file *file, void *priv,
  *
  * other, returned from video DAC.
  */
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *std)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id std)
 {
 	struct sta2x11_vip *vip = video_drvdata(file);
 	v4l2_std_id oldstd = vip->std, newstd;
 	int status;
 
-	if (V4L2_STD_ALL == *std) {
-		v4l2_subdev_call(vip->decoder, core, s_std, *std);
+	if (V4L2_STD_ALL == std) {
+		v4l2_subdev_call(vip->decoder, core, s_std, std);
 		ssleep(2);
 		v4l2_subdev_call(vip->decoder, video, querystd, &newstd);
 		v4l2_subdev_call(vip->decoder, video, g_input_status, &status);
 		if (status & V4L2_IN_ST_NO_SIGNAL)
 			return -EIO;
-		*std = vip->std = newstd;
-		if (oldstd != *std) {
-			if (V4L2_STD_525_60 & (*std))
+		std = vip->std = newstd;
+		if (oldstd != std) {
+			if (V4L2_STD_525_60 & std)
 				vip->format = formats_60[0];
 			else
 				vip->format = formats_50[0];
@@ -462,14 +462,14 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *std)
 		return 0;
 	}
 
-	if (oldstd != *std) {
-		if (V4L2_STD_525_60 & (*std))
+	if (oldstd != std) {
+		if (V4L2_STD_525_60 & std)
 			vip->format = formats_60[0];
 		else
 			vip->format = formats_50[0];
 	}
 
-	return v4l2_subdev_call(vip->decoder, core, s_std, *std);
+	return v4l2_subdev_call(vip->decoder, core, s_std, std);
 }
 
 /**
diff --git a/drivers/media/pci/ttpci/av7110.c b/drivers/media/pci/ttpci/av7110.c
index 3dc7aa9b6f40..f38329d29daa 100644
--- a/drivers/media/pci/ttpci/av7110.c
+++ b/drivers/media/pci/ttpci/av7110.c
@@ -990,7 +990,7 @@ static int av7110_start_feed(struct dvb_demux_feed *feed)
 
 	if (feed->type == DMX_TYPE_TS) {
 		if ((feed->ts_type & TS_DECODER) &&
-		    (feed->pes_type <= DMX_TS_PES_PCR)) {
+		    (feed->pes_type <= DMX_PES_PCR)) {
 			switch (demux->dmx.frontend->source) {
 			case DMX_MEMORY_FE:
 				if (feed->ts_type & TS_DECODER)
@@ -1051,14 +1051,14 @@ static int av7110_stop_feed(struct dvb_demux_feed *feed)
 
 	if (feed->type == DMX_TYPE_TS) {
 		if (feed->ts_type & TS_DECODER) {
-			if (feed->pes_type >= DMX_TS_PES_OTHER ||
+			if (feed->pes_type >= DMX_PES_OTHER ||
 			    !demux->pesfilter[feed->pes_type])
 				return -EINVAL;
 			demux->pids[feed->pes_type] |= 0x8000;
 			demux->pesfilter[feed->pes_type] = NULL;
 		}
 		if (feed->ts_type & TS_DECODER &&
-		    feed->pes_type < DMX_TS_PES_OTHER) {
+		    feed->pes_type < DMX_PES_OTHER) {
 			ret = dvb_feed_stop_pid(feed);
 		} else
 			if ((feed->ts_type & TS_PACKET) &&
diff --git a/drivers/media/pci/ttpci/av7110_ir.c b/drivers/media/pci/ttpci/av7110_ir.c
index eb822862a646..0e763a784e2b 100644
--- a/drivers/media/pci/ttpci/av7110_ir.c
+++ b/drivers/media/pci/ttpci/av7110_ir.c
@@ -375,7 +375,7 @@ int av7110_ir_init(struct av7110 *av7110)
 	if (av_cnt == 1) {
 		e = proc_create("av7110_ir", S_IWUSR, NULL, &av7110_ir_proc_fops);
 		if (e)
-			e->size = 4 + 256 * sizeof(u16);
+			proc_set_size(e, 4 + 256 * sizeof(u16));
 	}
 
 	tasklet_init(&av7110->ir.ir_tasklet, av7110_emit_key, (unsigned long) &av7110->ir);
diff --git a/drivers/media/pci/ttpci/av7110_v4l.c b/drivers/media/pci/ttpci/av7110_v4l.c
index 730e906ea912..6c4076acb131 100644
--- a/drivers/media/pci/ttpci/av7110_v4l.c
+++ b/drivers/media/pci/ttpci/av7110_v4l.c
@@ -366,7 +366,7 @@ static int vidioc_g_tuner(struct file *file, void *fh, struct v4l2_tuner *t)
 	return 0;
 }
 
-static int vidioc_s_tuner(struct file *file, void *fh, struct v4l2_tuner *t)
+static int vidioc_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *t)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
@@ -426,7 +426,7 @@ static int vidioc_g_frequency(struct file *file, void *fh, struct v4l2_frequency
 	return 0;
 }
 
-static int vidioc_s_frequency(struct file *file, void *fh, struct v4l2_frequency *f)
+static int vidioc_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *f)
 {
 	struct saa7146_dev *dev = ((struct saa7146_fh *)fh)->dev;
 	struct av7110 *av7110 = (struct av7110 *)dev->ext_priv;
diff --git a/drivers/media/pci/ttpci/budget.c b/drivers/media/pci/ttpci/budget.c
index 7e6e43ae5c51..6ccc48833fd8 100644
--- a/drivers/media/pci/ttpci/budget.c
+++ b/drivers/media/pci/ttpci/budget.c
@@ -537,6 +537,16 @@ static void frontend_init(struct budget *budget)
 		}
 		break;
 
+	case 0x4f52: /* Cards based on Philips Semi Sylt PCI ref. design */
+		budget->dvb_frontend = dvb_attach(stv0299_attach, &alps_bsru6_config, &budget->i2c_adap);
+		if (budget->dvb_frontend) {
+			printk(KERN_INFO "budget: tuner ALPS BSRU6 in Philips Semi. Sylt detected\n");
+			budget->dvb_frontend->ops.tuner_ops.set_params = alps_bsru6_tuner_set_params;
+			budget->dvb_frontend->tuner_priv = &budget->i2c_adap;
+			break;
+		}
+		break;
+
 	case 0x4f60: /* Fujitsu Siemens Activy Budget-S PCI rev AL (stv0299/tsa5059) */
 	{
 		int subtype = i2c_readreg(&budget->i2c_adap, 0x50, 0x67);
@@ -818,6 +828,7 @@ MAKE_BUDGET_INFO(fsacs1, "Fujitsu Siemens Activy Budget-S PCI (rev AL/alps front
 MAKE_BUDGET_INFO(fsact,	 "Fujitsu Siemens Activy Budget-T PCI (rev GR/Grundig frontend)", BUDGET_FS_ACTIVY);
 MAKE_BUDGET_INFO(fsact1, "Fujitsu Siemens Activy Budget-T PCI (rev AL/ALPS TDHD1-204A)", BUDGET_FS_ACTIVY);
 MAKE_BUDGET_INFO(omicom, "Omicom S2 PCI", BUDGET_TT);
+MAKE_BUDGET_INFO(sylt,   "Philips Semi Sylt PCI", BUDGET_TT_HW_DISEQC);
 
 static struct pci_device_id pci_tbl[] = {
 	MAKE_EXTENSION_PCI(ttbs,  0x13c2, 0x1003),
@@ -832,6 +843,7 @@ static struct pci_device_id pci_tbl[] = {
 	MAKE_EXTENSION_PCI(fsact1, 0x1131, 0x5f60),
 	MAKE_EXTENSION_PCI(fsact, 0x1131, 0x5f61),
 	MAKE_EXTENSION_PCI(omicom, 0x14c4, 0x1020),
+	MAKE_EXTENSION_PCI(sylt, 0x1131, 0x4f52),
 	{
 		.vendor    = 0,
 	}
diff --git a/drivers/media/pci/zoran/zoran_driver.c b/drivers/media/pci/zoran/zoran_driver.c
index 2e8f518f298f..1168a84a737d 100644
--- a/drivers/media/pci/zoran/zoran_driver.c
+++ b/drivers/media/pci/zoran/zoran_driver.c
@@ -2435,14 +2435,14 @@ static int zoran_g_std(struct file *file, void *__fh, v4l2_std_id *std)
 	return 0;
 }
 
-static int zoran_s_std(struct file *file, void *__fh, v4l2_std_id *std)
+static int zoran_s_std(struct file *file, void *__fh, v4l2_std_id std)
 {
 	struct zoran_fh *fh = __fh;
 	struct zoran *zr = fh->zr;
 	int res = 0;
 
 	mutex_lock(&zr->resource_lock);
-	res = zoran_set_norm(zr, *std);
+	res = zoran_set_norm(zr, std);
 	if (res)
 		goto sstd_unlock_and_return;
 
diff --git a/drivers/media/pci/zoran/zoran_procfs.c b/drivers/media/pci/zoran/zoran_procfs.c
index e084b0a21b1b..f7ceee0cdefd 100644
--- a/drivers/media/pci/zoran/zoran_procfs.c
+++ b/drivers/media/pci/zoran/zoran_procfs.c
@@ -130,14 +130,14 @@ static int zoran_show(struct seq_file *p, void *v)
 
 static int zoran_open(struct inode *inode, struct file *file)
 {
-	struct zoran *data = PDE(inode)->data;
+	struct zoran *data = PDE_DATA(inode);
 	return single_open(file, zoran_show, data);
 }
 
 static ssize_t zoran_write(struct file *file, const char __user *buffer,
 			size_t count, loff_t *ppos)
 {
-	struct zoran *zr = PDE(file_inode(file))->data;
+	struct zoran *zr = PDE_DATA(file_inode(file));
 	char *string, *sp;
 	char *line, *ldelim, *varname, *svar, *tdelim;
 
@@ -201,7 +201,7 @@ zoran_proc_init (struct zoran *zr)
 		dprintk(2,
 			KERN_INFO
 			"%s: procfs entry /proc/%s allocated. data=%p\n",
-			ZR_DEVNAME(zr), name, zr->zoran_proc->data);
+			ZR_DEVNAME(zr), name, zr);
 	} else {
 		dprintk(1, KERN_ERR "%s: Unable to initialise /proc/%s\n",
 			ZR_DEVNAME(zr), name);
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index 26500094de7c..0494d2769fd7 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -122,7 +122,7 @@ config VIDEO_S3C_CAMIF
 	  will be called s3c-camif.
 
 source "drivers/media/platform/soc_camera/Kconfig"
-source "drivers/media/platform/s5p-fimc/Kconfig"
+source "drivers/media/platform/exynos4-is/Kconfig"
 source "drivers/media/platform/s5p-tv/Kconfig"
 
 endif # V4L_PLATFORM_DRIVERS
diff --git a/drivers/media/platform/Makefile b/drivers/media/platform/Makefile
index 42089ba3600f..eee28dd78d7d 100644
--- a/drivers/media/platform/Makefile
+++ b/drivers/media/platform/Makefile
@@ -30,7 +30,7 @@ obj-$(CONFIG_VIDEO_SH_VEU)		+= sh_veu.o
 obj-$(CONFIG_VIDEO_MEM2MEM_DEINTERLACE)	+= m2m-deinterlace.o
 
 obj-$(CONFIG_VIDEO_S3C_CAMIF) 		+= s3c-camif/
-obj-$(CONFIG_VIDEO_SAMSUNG_S5P_FIMC) 	+= s5p-fimc/
+obj-$(CONFIG_VIDEO_SAMSUNG_EXYNOS4_IS) 	+= exynos4-is/
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_JPEG)	+= s5p-jpeg/
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_MFC)	+= s5p-mfc/
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_TV)	+= s5p-tv/
diff --git a/drivers/media/platform/blackfin/bfin_capture.c b/drivers/media/platform/blackfin/bfin_capture.c
index 5f209d5810dc..0e55b087076f 100644
--- a/drivers/media/platform/blackfin/bfin_capture.c
+++ b/drivers/media/platform/blackfin/bfin_capture.c
@@ -384,7 +384,7 @@ static int bcap_start_streaming(struct vb2_queue *vq, unsigned int count)
 	params.ppi_control = bcap_dev->cfg->ppi_control;
 	params.int_mask = bcap_dev->cfg->int_mask;
 	if (bcap_dev->cfg->inputs[bcap_dev->cur_input].capabilities
-			& V4L2_IN_CAP_CUSTOM_TIMINGS) {
+			& V4L2_IN_CAP_DV_TIMINGS) {
 		struct v4l2_bt_timings *bt = &bcap_dev->dv_timings.bt;
 
 		params.hdelay = bt->hsync + bt->hbackporch;
@@ -633,7 +633,7 @@ static int bcap_g_std(struct file *file, void *priv, v4l2_std_id *std)
 	return 0;
 }
 
-static int bcap_s_std(struct file *file, void *priv, v4l2_std_id *std)
+static int bcap_s_std(struct file *file, void *priv, v4l2_std_id std)
 {
 	struct bcap_device *bcap_dev = video_drvdata(file);
 	int ret;
@@ -641,11 +641,11 @@ static int bcap_s_std(struct file *file, void *priv, v4l2_std_id *std)
 	if (vb2_is_busy(&bcap_dev->buffer_queue))
 		return -EBUSY;
 
-	ret = v4l2_subdev_call(bcap_dev->sd, core, s_std, *std);
+	ret = v4l2_subdev_call(bcap_dev->sd, core, s_std, std);
 	if (ret < 0)
 		return ret;
 
-	bcap_dev->std = *std;
+	bcap_dev->std = std;
 	return 0;
 }
 
@@ -890,7 +890,7 @@ static int bcap_dbg_g_register(struct file *file, void *priv,
 }
 
 static int bcap_dbg_s_register(struct file *file, void *priv,
-		struct v4l2_dbg_register *reg)
+		const struct v4l2_dbg_register *reg)
 {
 	struct bcap_device *bcap_dev = video_drvdata(file);
 
@@ -1029,6 +1029,7 @@ static int bcap_probe(struct platform_device *pdev)
 	q->buf_struct_size = sizeof(struct bcap_buffer);
 	q->ops = &bcap_video_qops;
 	q->mem_ops = &vb2_dma_contig_memops;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	vb2_queue_init(q);
 
@@ -1110,7 +1111,7 @@ static int bcap_probe(struct platform_device *pdev)
 		}
 		bcap_dev->std = std;
 	}
-	if (config->inputs[0].capabilities & V4L2_IN_CAP_CUSTOM_TIMINGS) {
+	if (config->inputs[0].capabilities & V4L2_IN_CAP_DV_TIMINGS) {
 		struct v4l2_dv_timings dv_timings;
 		ret = v4l2_subdev_call(bcap_dev->sd, video,
 				g_dv_timings, &dv_timings);
diff --git a/drivers/media/platform/coda.c b/drivers/media/platform/coda.c
index b931c2a5c7fc..48b8d7af386d 100644
--- a/drivers/media/platform/coda.c
+++ b/drivers/media/platform/coda.c
@@ -1427,6 +1427,7 @@ static int coda_queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	src_vq->ops = &coda_qops;
 	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -1438,6 +1439,7 @@ static int coda_queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	dst_vq->ops = &coda_qops;
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	return vb2_queue_init(dst_vq);
 }
@@ -1633,6 +1635,9 @@ static irqreturn_t coda_irq_handler(int irq, void *data)
 		dst_buf->v4l2_buf.flags &= ~V4L2_BUF_FLAG_KEYFRAME;
 	}
 
+	dst_buf->v4l2_buf.timestamp = src_buf->v4l2_buf.timestamp;
+	dst_buf->v4l2_buf.timecode = src_buf->v4l2_buf.timecode;
+
 	v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
 	v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
 
diff --git a/drivers/media/platform/davinci/Kconfig b/drivers/media/platform/davinci/Kconfig
index ccfde4eb626a..afb3aec1320e 100644
--- a/drivers/media/platform/davinci/Kconfig
+++ b/drivers/media/platform/davinci/Kconfig
@@ -1,79 +1,47 @@
 config VIDEO_DAVINCI_VPIF_DISPLAY
-	tristate "DM646x/DA850/OMAPL138 EVM Video Display"
-	depends on VIDEO_DEV && (MACH_DAVINCI_DM6467_EVM || MACH_DAVINCI_DA850_EVM)
+	tristate "TI DaVinci VPIF V4L2-Display driver"
+	depends on VIDEO_DEV && ARCH_DAVINCI
 	select VIDEOBUF2_DMA_CONTIG
-	select VIDEO_DAVINCI_VPIF
 	select VIDEO_ADV7343 if MEDIA_SUBDRV_AUTOSELECT
 	select VIDEO_THS7303 if MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Enables Davinci VPIF module used for display devices.
-	  This module is common for following DM6467/DA850/OMAPL138
-	  based display devices.
+	  This module is used for display on TI DM6467/DA850/OMAPL138
+	  SoCs.
 
-	  To compile this driver as a module, choose M here: the
-	  module will be called vpif_display.
+	  To compile this driver as a module, choose M here. There will
+	  be two modules called vpif.ko and vpif_display.ko
 
 config VIDEO_DAVINCI_VPIF_CAPTURE
-	tristate "DM646x/DA850/OMAPL138 EVM Video Capture"
-	depends on VIDEO_DEV && (MACH_DAVINCI_DM6467_EVM || MACH_DAVINCI_DA850_EVM)
+	tristate "TI DaVinci VPIF video capture driver"
+	depends on VIDEO_DEV && ARCH_DAVINCI
 	select VIDEOBUF2_DMA_CONTIG
-	select VIDEO_DAVINCI_VPIF
 	help
-	  Enables Davinci VPIF module used for captur devices.
-	  This module is common for following DM6467/DA850/OMAPL138
-	  based capture devices.
+	  Enables Davinci VPIF module used for capture devices.
+	  This module is used for capture on TI DM6467/DA850/OMAPL138
+	  SoCs.
 
-	  To compile this driver as a module, choose M here: the
-	  module will be called vpif_capture.
+	  To compile this driver as a module, choose M here. There will
+	  be two modules called vpif.ko and vpif_capture.ko
 
-config VIDEO_DAVINCI_VPIF
-	tristate "DaVinci VPIF Driver"
-	depends on VIDEO_DAVINCI_VPIF_DISPLAY || VIDEO_DAVINCI_VPIF_CAPTURE
-	help
-	  Support for DaVinci VPIF Driver.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called vpif.
-
-config VIDEO_VPSS_SYSTEM
-	tristate "VPSS System module driver"
-	depends on ARCH_DAVINCI
-	help
-	  Support for vpss system module for video driver
-
-config VIDEO_VPFE_CAPTURE
-	tristate "VPFE Video Capture Driver"
+config VIDEO_DM6446_CCDC
+	tristate "TI DM6446 CCDC video capture driver"
 	depends on VIDEO_V4L2 && (ARCH_DAVINCI || ARCH_OMAP3)
-	depends on I2C
 	select VIDEOBUF_DMA_CONTIG
 	help
-	  Support for DMx/AMx VPFE based frame grabber. This is the
-	  common V4L2 module for following DMx/AMx SoCs from Texas
-	  Instruments:- DM6446, DM365, DM355 & AM3517/05.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called vpfe-capture.
-
-config VIDEO_DM6446_CCDC
-	tristate "DM6446 CCDC HW module"
-	depends on VIDEO_VPFE_CAPTURE
-	select VIDEO_VPSS_SYSTEM
-	default y
-	help
 	   Enables DaVinci CCD hw module. DaVinci CCDC hw interfaces
 	   with decoder modules such as TVP5146 over BT656 or
 	   sensor module such as MT9T001 over a raw interface. This
 	   module configures the interface and CCDC/ISIF to do
 	   video frame capture from slave decoders.
 
-	   To compile this driver as a module, choose M here: the
-	   module will be called vpfe.
+	   To compile this driver as a module, choose M here. There will
+	   be three modules called vpfe_capture.ko, vpss.ko and dm644x_ccdc.ko
 
 config VIDEO_DM355_CCDC
-	tristate "DM355 CCDC HW module"
-	depends on ARCH_DAVINCI_DM355 && VIDEO_VPFE_CAPTURE
-	select VIDEO_VPSS_SYSTEM
-	default y
+	tristate "TI DM355 CCDC video capture driver"
+	depends on VIDEO_V4L2 && ARCH_DAVINCI
+	select VIDEOBUF_DMA_CONTIG
 	help
 	   Enables DM355 CCD hw module. DM355 CCDC hw interfaces
 	   with decoder modules such as TVP5146 over BT656 or
@@ -81,31 +49,30 @@ config VIDEO_DM355_CCDC
 	   module configures the interface and CCDC/ISIF to do
 	   video frame capture from a slave decoders
 
-	   To compile this driver as a module, choose M here: the
-	   module will be called vpfe.
+	   To compile this driver as a module, choose M here. There will
+	   be three modules called vpfe_capture.ko, vpss.ko and dm355_ccdc.ko
 
-config VIDEO_ISIF
-	tristate "ISIF HW module"
-	depends on ARCH_DAVINCI_DM365 && VIDEO_VPFE_CAPTURE
-	select VIDEO_VPSS_SYSTEM
-	default y
+config VIDEO_DM365_ISIF
+	tristate "TI DM365 ISIF video capture driver"
+	depends on VIDEO_V4L2 && ARCH_DAVINCI
+	select VIDEOBUF_DMA_CONTIG
 	help
 	   Enables ISIF hw module. This is the hardware module for
-	   configuring ISIF in VPFE to capture Raw Bayer RGB data  from
+	   configuring ISIF in VPFE to capture Raw Bayer RGB data from
 	   a image sensor or YUV data from a YUV source.
 
-	   To compile this driver as a module, choose M here: the
-	   module will be called vpfe.
+	   To compile this driver as a module, choose M here. There will
+	   be three modules called vpfe_capture.ko, vpss.ko and isif.ko
 
 config VIDEO_DAVINCI_VPBE_DISPLAY
-	tristate "DM644X/DM365/DM355 VPBE HW module"
-	depends on ARCH_DAVINCI_DM644x || ARCH_DAVINCI_DM355 || ARCH_DAVINCI_DM365
-	select VIDEO_VPSS_SYSTEM
+	tristate "TI DaVinci VPBE V4L2-Display driver"
+	depends on ARCH_DAVINCI
 	select VIDEOBUF2_DMA_CONTIG
 	help
 	    Enables Davinci VPBE module used for display devices.
-	    This module is common for following DM644x/DM365/DM355
+	    This module is used for display on TI DM644x/DM365/DM355
 	    based display devices.
 
-	    To compile this driver as a module, choose M here: the
-	    module will be called vpbe.
+	    To compile this driver as a module, choose M here. There will
+	    be five modules created called vpss.ko, vpbe.ko, vpbe_osd.ko,
+	    vpbe_venc.ko and vpbe_display.ko
diff --git a/drivers/media/platform/davinci/Makefile b/drivers/media/platform/davinci/Makefile
index f40f5219ca50..d74d9eeb0e9e 100644
--- a/drivers/media/platform/davinci/Makefile
+++ b/drivers/media/platform/davinci/Makefile
@@ -2,19 +2,14 @@
 # Makefile for the davinci video device drivers.
 #
 
-# VPIF
-obj-$(CONFIG_VIDEO_DAVINCI_VPIF) += vpif.o
-
 #VPIF Display driver
-obj-$(CONFIG_VIDEO_DAVINCI_VPIF_DISPLAY) += vpif_display.o
+obj-$(CONFIG_VIDEO_DAVINCI_VPIF_DISPLAY) += vpif.o vpif_display.o
 #VPIF Capture driver
-obj-$(CONFIG_VIDEO_DAVINCI_VPIF_CAPTURE) += vpif_capture.o
+obj-$(CONFIG_VIDEO_DAVINCI_VPIF_CAPTURE) += vpif.o vpif_capture.o
 
 # Capture: DM6446 and DM355
-obj-$(CONFIG_VIDEO_VPSS_SYSTEM) += vpss.o
-obj-$(CONFIG_VIDEO_VPFE_CAPTURE) += vpfe_capture.o
-obj-$(CONFIG_VIDEO_DM6446_CCDC) += dm644x_ccdc.o
-obj-$(CONFIG_VIDEO_DM355_CCDC) += dm355_ccdc.o
-obj-$(CONFIG_VIDEO_ISIF) += isif.o
-obj-$(CONFIG_VIDEO_DAVINCI_VPBE_DISPLAY) += vpbe.o vpbe_osd.o \
+obj-$(CONFIG_VIDEO_DM6446_CCDC) += vpfe_capture.o vpss.o dm644x_ccdc.o
+obj-$(CONFIG_VIDEO_DM355_CCDC) += vpfe_capture.o vpss.o dm355_ccdc.o
+obj-$(CONFIG_VIDEO_DM365_ISIF) += vpfe_capture.o vpss.o isif.o
+obj-$(CONFIG_VIDEO_DAVINCI_VPBE_DISPLAY) += vpss.o vpbe.o vpbe_osd.o \
 	vpbe_venc.o vpbe_display.o
diff --git a/drivers/media/platform/davinci/dm355_ccdc.c b/drivers/media/platform/davinci/dm355_ccdc.c
index 4277e4ad810c..05f8fb7f7b70 100644
--- a/drivers/media/platform/davinci/dm355_ccdc.c
+++ b/drivers/media/platform/davinci/dm355_ccdc.c
@@ -37,7 +37,6 @@
 #include <linux/platform_device.h>
 #include <linux/uaccess.h>
 #include <linux/videodev2.h>
-#include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/module.h>
 
@@ -59,10 +58,6 @@ static struct ccdc_oper_config {
 	struct ccdc_params_raw bayer;
 	/* YCbCr configuration */
 	struct ccdc_params_ycbcr ycbcr;
-	/* Master clock */
-	struct clk *mclk;
-	/* slave clock */
-	struct clk *sclk;
 	/* ccdc base address */
 	void __iomem *base_addr;
 } ccdc_cfg = {
@@ -85,7 +80,7 @@ static struct ccdc_oper_config {
 			.mfilt1 = CCDC_NO_MEDIAN_FILTER1,
 			.mfilt2 = CCDC_NO_MEDIAN_FILTER2,
 			.alaw = {
-				.gama_wd = 2,
+				.gamma_wd = 2,
 			},
 			.blk_clamp = {
 				.sample_pixel = 1,
@@ -303,8 +298,8 @@ static int validate_ccdc_param(struct ccdc_config_params_raw *ccdcparam)
 	}
 
 	if (ccdcparam->alaw.enable) {
-		if (ccdcparam->alaw.gama_wd < CCDC_GAMMA_BITS_13_4 ||
-		    ccdcparam->alaw.gama_wd > CCDC_GAMMA_BITS_09_0) {
+		if (ccdcparam->alaw.gamma_wd < CCDC_GAMMA_BITS_13_4 ||
+		    ccdcparam->alaw.gamma_wd > CCDC_GAMMA_BITS_09_0) {
 			dev_dbg(ccdc_cfg.dev, "Invalid value of ALAW\n");
 			return -EINVAL;
 		}
@@ -680,8 +675,8 @@ static int ccdc_config_raw(void)
 	/* Enable and configure aLaw register if needed */
 	if (config_params->alaw.enable) {
 		val |= (CCDC_ALAW_ENABLE |
-			((config_params->alaw.gama_wd &
-			CCDC_ALAW_GAMA_WD_MASK) <<
+			((config_params->alaw.gamma_wd &
+			CCDC_ALAW_GAMMA_WD_MASK) <<
 			CCDC_GAMMAWD_INPUT_SHIFT));
 	}
 
@@ -997,32 +992,10 @@ static int dm355_ccdc_probe(struct platform_device *pdev)
 		goto fail_nomem;
 	}
 
-	/* Get and enable Master clock */
-	ccdc_cfg.mclk = clk_get(&pdev->dev, "master");
-	if (IS_ERR(ccdc_cfg.mclk)) {
-		status = PTR_ERR(ccdc_cfg.mclk);
-		goto fail_nomap;
-	}
-	if (clk_prepare_enable(ccdc_cfg.mclk)) {
-		status = -ENODEV;
-		goto fail_mclk;
-	}
-
-	/* Get and enable Slave clock */
-	ccdc_cfg.sclk = clk_get(&pdev->dev, "slave");
-	if (IS_ERR(ccdc_cfg.sclk)) {
-		status = PTR_ERR(ccdc_cfg.sclk);
-		goto fail_mclk;
-	}
-	if (clk_prepare_enable(ccdc_cfg.sclk)) {
-		status = -ENODEV;
-		goto fail_sclk;
-	}
-
 	/* Platform data holds setup_pinmux function ptr */
 	if (NULL == pdev->dev.platform_data) {
 		status = -ENODEV;
-		goto fail_sclk;
+		goto fail_nomap;
 	}
 	setup_pinmux = pdev->dev.platform_data;
 	/*
@@ -1033,12 +1006,6 @@ static int dm355_ccdc_probe(struct platform_device *pdev)
 	ccdc_cfg.dev = &pdev->dev;
 	printk(KERN_NOTICE "%s is registered with vpfe.\n", ccdc_hw_dev.name);
 	return 0;
-fail_sclk:
-	clk_disable_unprepare(ccdc_cfg.sclk);
-	clk_put(ccdc_cfg.sclk);
-fail_mclk:
-	clk_disable_unprepare(ccdc_cfg.mclk);
-	clk_put(ccdc_cfg.mclk);
 fail_nomap:
 	iounmap(ccdc_cfg.base_addr);
 fail_nomem:
@@ -1052,10 +1019,6 @@ static int dm355_ccdc_remove(struct platform_device *pdev)
 {
 	struct resource	*res;
 
-	clk_disable_unprepare(ccdc_cfg.sclk);
-	clk_disable_unprepare(ccdc_cfg.mclk);
-	clk_put(ccdc_cfg.mclk);
-	clk_put(ccdc_cfg.sclk);
 	iounmap(ccdc_cfg.base_addr);
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (res)
diff --git a/drivers/media/platform/davinci/dm355_ccdc_regs.h b/drivers/media/platform/davinci/dm355_ccdc_regs.h
index d6d2ef0533b5..2e1946e0b99f 100644
--- a/drivers/media/platform/davinci/dm355_ccdc_regs.h
+++ b/drivers/media/platform/davinci/dm355_ccdc_regs.h
@@ -153,7 +153,7 @@
 #define CCDC_VDHDEN_ENABLE			(1 << 16)
 #define CCDC_LPF_ENABLE				(1 << 14)
 #define CCDC_ALAW_ENABLE			1
-#define CCDC_ALAW_GAMA_WD_MASK			7
+#define CCDC_ALAW_GAMMA_WD_MASK			7
 #define CCDC_REC656IF_BT656_EN			3
 
 #define CCDC_FMTCFG_FMTMODE_MASK 		3
diff --git a/drivers/media/platform/davinci/dm644x_ccdc.c b/drivers/media/platform/davinci/dm644x_ccdc.c
index 318e80512998..30fa08405d61 100644
--- a/drivers/media/platform/davinci/dm644x_ccdc.c
+++ b/drivers/media/platform/davinci/dm644x_ccdc.c
@@ -38,7 +38,6 @@
 #include <linux/uaccess.h>
 #include <linux/videodev2.h>
 #include <linux/gfp.h>
-#include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/module.h>
 
@@ -60,10 +59,6 @@ static struct ccdc_oper_config {
 	struct ccdc_params_raw bayer;
 	/* YCbCr configuration */
 	struct ccdc_params_ycbcr ycbcr;
-	/* Master clock */
-	struct clk *mclk;
-	/* slave clock */
-	struct clk *sclk;
 	/* ccdc base address */
 	void __iomem *base_addr;
 } ccdc_cfg = {
@@ -228,9 +223,12 @@ static void ccdc_readregs(void)
 static int validate_ccdc_param(struct ccdc_config_params_raw *ccdcparam)
 {
 	if (ccdcparam->alaw.enable) {
-		if ((ccdcparam->alaw.gama_wd > CCDC_GAMMA_BITS_09_0) ||
-		    (ccdcparam->alaw.gama_wd < CCDC_GAMMA_BITS_15_6) ||
-		    (ccdcparam->alaw.gama_wd < ccdcparam->data_sz)) {
+		u8 max_gamma = ccdc_gamma_width_max_bit(ccdcparam->alaw.gamma_wd);
+		u8 max_data = ccdc_data_size_max_bit(ccdcparam->data_sz);
+
+		if ((ccdcparam->alaw.gamma_wd > CCDC_GAMMA_BITS_09_0) ||
+		    (ccdcparam->alaw.gamma_wd < CCDC_GAMMA_BITS_15_6) ||
+		    (max_gamma > max_data)) {
 			dev_dbg(ccdc_cfg.dev, "\nInvalid data line select");
 			return -1;
 		}
@@ -560,8 +558,8 @@ void ccdc_config_raw(void)
 
 	/* Enable and configure aLaw register if needed */
 	if (config_params->alaw.enable) {
-		val = ((config_params->alaw.gama_wd &
-		      CCDC_ALAW_GAMA_WD_MASK) | CCDC_ALAW_ENABLE);
+		val = ((config_params->alaw.gamma_wd &
+		      CCDC_ALAW_GAMMA_WD_MASK) | CCDC_ALAW_ENABLE);
 		regw(val, CCDC_ALAW);
 		dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to ALAW...\n", val);
 	}
@@ -988,38 +986,9 @@ static int dm644x_ccdc_probe(struct platform_device *pdev)
 		goto fail_nomem;
 	}
 
-	/* Get and enable Master clock */
-	ccdc_cfg.mclk = clk_get(&pdev->dev, "master");
-	if (IS_ERR(ccdc_cfg.mclk)) {
-		status = PTR_ERR(ccdc_cfg.mclk);
-		goto fail_nomap;
-	}
-	if (clk_prepare_enable(ccdc_cfg.mclk)) {
-		status = -ENODEV;
-		goto fail_mclk;
-	}
-
-	/* Get and enable Slave clock */
-	ccdc_cfg.sclk = clk_get(&pdev->dev, "slave");
-	if (IS_ERR(ccdc_cfg.sclk)) {
-		status = PTR_ERR(ccdc_cfg.sclk);
-		goto fail_mclk;
-	}
-	if (clk_prepare_enable(ccdc_cfg.sclk)) {
-		status = -ENODEV;
-		goto fail_sclk;
-	}
 	ccdc_cfg.dev = &pdev->dev;
 	printk(KERN_NOTICE "%s is registered with vpfe.\n", ccdc_hw_dev.name);
 	return 0;
-fail_sclk:
-	clk_disable_unprepare(ccdc_cfg.sclk);
-	clk_put(ccdc_cfg.sclk);
-fail_mclk:
-	clk_disable_unprepare(ccdc_cfg.mclk);
-	clk_put(ccdc_cfg.mclk);
-fail_nomap:
-	iounmap(ccdc_cfg.base_addr);
 fail_nomem:
 	release_mem_region(res->start, resource_size(res));
 fail_nores:
@@ -1031,10 +1000,6 @@ static int dm644x_ccdc_remove(struct platform_device *pdev)
 {
 	struct resource	*res;
 
-	clk_disable_unprepare(ccdc_cfg.mclk);
-	clk_disable_unprepare(ccdc_cfg.sclk);
-	clk_put(ccdc_cfg.mclk);
-	clk_put(ccdc_cfg.sclk);
 	iounmap(ccdc_cfg.base_addr);
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (res)
@@ -1049,18 +1014,12 @@ static int dm644x_ccdc_suspend(struct device *dev)
 	ccdc_save_context();
 	/* Disable CCDC */
 	ccdc_enable(0);
-	/* Disable both master and slave clock */
-	clk_disable_unprepare(ccdc_cfg.mclk);
-	clk_disable_unprepare(ccdc_cfg.sclk);
 
 	return 0;
 }
 
 static int dm644x_ccdc_resume(struct device *dev)
 {
-	/* Enable both master and slave clock */
-	clk_prepare_enable(ccdc_cfg.mclk);
-	clk_prepare_enable(ccdc_cfg.sclk);
 	/* Restore CCDC context */
 	ccdc_restore_context();
 
diff --git a/drivers/media/platform/davinci/dm644x_ccdc_regs.h b/drivers/media/platform/davinci/dm644x_ccdc_regs.h
index 90370e414e2c..2b0aca5383f0 100644
--- a/drivers/media/platform/davinci/dm644x_ccdc_regs.h
+++ b/drivers/media/platform/davinci/dm644x_ccdc_regs.h
@@ -84,7 +84,7 @@
 #define CCDC_VDHDEN_ENABLE			(1 << 16)
 #define CCDC_LPF_ENABLE				(1 << 14)
 #define CCDC_ALAW_ENABLE			(1 << 3)
-#define CCDC_ALAW_GAMA_WD_MASK			7
+#define CCDC_ALAW_GAMMA_WD_MASK			7
 #define CCDC_BLK_CLAMP_ENABLE			(1 << 31)
 #define CCDC_BLK_SGAIN_MASK			0x1F
 #define CCDC_BLK_ST_PXL_MASK			0x7FFF
diff --git a/drivers/media/platform/davinci/isif.c b/drivers/media/platform/davinci/isif.c
index 5050f9265f48..3332cca632e5 100644
--- a/drivers/media/platform/davinci/isif.c
+++ b/drivers/media/platform/davinci/isif.c
@@ -32,7 +32,6 @@
 #include <linux/uaccess.h>
 #include <linux/io.h>
 #include <linux/videodev2.h>
-#include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/module.h>
 
@@ -88,8 +87,6 @@ static struct isif_oper_config {
 	struct isif_ycbcr_config ycbcr;
 	struct isif_params_raw bayer;
 	enum isif_data_pack data_pack;
-	/* Master clock */
-	struct clk *mclk;
 	/* ISIF base address */
 	void __iomem *base_addr;
 	/* ISIF Linear Table 0 */
@@ -604,7 +601,7 @@ static int isif_config_raw(void)
 	if (module_params->compress.alg == ISIF_ALAW)
 		val |= ISIF_ALAW_ENABLE;
 
-	val |= (params->data_msb << ISIF_ALAW_GAMA_WD_SHIFT);
+	val |= (params->data_msb << ISIF_ALAW_GAMMA_WD_SHIFT);
 	regw(val, CGAMMAWD);
 
 	/* Configure DPCM compression settings */
@@ -1039,6 +1036,10 @@ static int isif_probe(struct platform_device *pdev)
 	void *__iomem addr;
 	int status = 0, i;
 
+	/* Platform data holds setup_pinmux function ptr */
+	if (!pdev->dev.platform_data)
+		return -ENODEV;
+
 	/*
 	 * first try to register with vpfe. If not correct platform, then we
 	 * don't have to iomap
@@ -1047,22 +1048,6 @@ static int isif_probe(struct platform_device *pdev)
 	if (status < 0)
 		return status;
 
-	/* Get and enable Master clock */
-	isif_cfg.mclk = clk_get(&pdev->dev, "master");
-	if (IS_ERR(isif_cfg.mclk)) {
-		status = PTR_ERR(isif_cfg.mclk);
-		goto fail_mclk;
-	}
-	if (clk_prepare_enable(isif_cfg.mclk)) {
-		status = -ENODEV;
-		goto fail_mclk;
-	}
-
-	/* Platform data holds setup_pinmux function ptr */
-	if (NULL == pdev->dev.platform_data) {
-		status = -ENODEV;
-		goto fail_mclk;
-	}
 	setup_pinmux = pdev->dev.platform_data;
 	/*
 	 * setup Mux configuration for ccdc which may be different for
@@ -1124,9 +1109,6 @@ fail_nobase_res:
 		release_mem_region(res->start, resource_size(res));
 		i--;
 	}
-fail_mclk:
-	clk_disable_unprepare(isif_cfg.mclk);
-	clk_put(isif_cfg.mclk);
 	vpfe_unregister_ccdc_device(&isif_hw_dev);
 	return status;
 }
@@ -1146,8 +1128,6 @@ static int isif_remove(struct platform_device *pdev)
 		i++;
 	}
 	vpfe_unregister_ccdc_device(&isif_hw_dev);
-	clk_disable_unprepare(isif_cfg.mclk);
-	clk_put(isif_cfg.mclk);
 	return 0;
 }
 
diff --git a/drivers/media/platform/davinci/isif_regs.h b/drivers/media/platform/davinci/isif_regs.h
index aa69a463c122..3993aece821b 100644
--- a/drivers/media/platform/davinci/isif_regs.h
+++ b/drivers/media/platform/davinci/isif_regs.h
@@ -203,8 +203,8 @@
 #define ISIF_LPF_MASK				1
 
 /* GAMMAWD registers */
-#define ISIF_ALAW_GAMA_WD_MASK			0xF
-#define ISIF_ALAW_GAMA_WD_SHIFT			1
+#define ISIF_ALAW_GAMMA_WD_MASK			0xF
+#define ISIF_ALAW_GAMMA_WD_SHIFT		1
 #define ISIF_ALAW_ENABLE			1
 #define ISIF_GAMMAWD_CFA_SHIFT			5
 
diff --git a/drivers/media/platform/davinci/vpbe.c b/drivers/media/platform/davinci/vpbe.c
index 4ca0f9a2ad8a..33b9660b7f77 100644
--- a/drivers/media/platform/davinci/vpbe.c
+++ b/drivers/media/platform/davinci/vpbe.c
@@ -344,7 +344,7 @@ static int vpbe_s_dv_timings(struct vpbe_device *vpbe_dev,
 		return -EINVAL;
 
 	for (i = 0; i < output->num_modes; i++) {
-		if (output->modes[i].timings_type == VPBE_ENC_CUSTOM_TIMINGS &&
+		if (output->modes[i].timings_type == VPBE_ENC_DV_TIMINGS &&
 		    !memcmp(&output->modes[i].dv_timings,
 				dv_timings, sizeof(*dv_timings)))
 			break;
@@ -385,7 +385,7 @@ 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_CUSTOM_TIMINGS) {
+	  VPBE_ENC_DV_TIMINGS) {
 		*dv_timings = vpbe_dev->current_timings.dv_timings;
 		return 0;
 	}
@@ -412,7 +412,7 @@ static int vpbe_enum_dv_timings(struct vpbe_device *vpbe_dev,
 		return -EINVAL;
 
 	for (i = 0; i < output->num_modes; i++) {
-		if (output->modes[i].timings_type == VPBE_ENC_CUSTOM_TIMINGS) {
+		if (output->modes[i].timings_type == VPBE_ENC_DV_TIMINGS) {
 			if (j == timings->index)
 				break;
 			j++;
@@ -431,7 +431,7 @@ static int vpbe_enum_dv_timings(struct vpbe_device *vpbe_dev,
  * Sets the standard if supported by the current encoder. Return the status.
  * 0 - success & -EINVAL on error
  */
-static int vpbe_s_std(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id)
+static int vpbe_s_std(struct vpbe_device *vpbe_dev, v4l2_std_id std_id)
 {
 	struct vpbe_config *cfg = vpbe_dev->cfg;
 	int out_index = vpbe_dev->current_out_index;
@@ -442,14 +442,14 @@ static int vpbe_s_std(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id)
 		V4L2_OUT_CAP_STD))
 		return -EINVAL;
 
-	ret = vpbe_get_std_info(vpbe_dev, *std_id);
+	ret = vpbe_get_std_info(vpbe_dev, std_id);
 	if (ret)
 		return ret;
 
 	mutex_lock(&vpbe_dev->lock);
 
 	ret = v4l2_subdev_call(vpbe_dev->encoders[sd_index], video,
-			       s_std_output, *std_id);
+			       s_std_output, std_id);
 	/* set the lcd controller output for the given mode */
 	if (!ret) {
 		struct osd_state *osd_device = vpbe_dev->osd_device;
@@ -513,9 +513,9 @@ 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->std_id);
+						 preset_mode->std_id);
 			if (preset_mode->timings_type &
-						VPBE_ENC_CUSTOM_TIMINGS) {
+						VPBE_ENC_DV_TIMINGS) {
 				dv_timings =
 					preset_mode->dv_timings;
 				return vpbe_s_dv_timings(vpbe_dev, &dv_timings);
diff --git a/drivers/media/platform/davinci/vpbe_display.c b/drivers/media/platform/davinci/vpbe_display.c
index 5e6b0cab514b..1802f11e939f 100644
--- a/drivers/media/platform/davinci/vpbe_display.c
+++ b/drivers/media/platform/davinci/vpbe_display.c
@@ -983,7 +983,7 @@ static int vpbe_display_try_fmt(struct file *file, void *priv,
  * 0 - success & -EINVAL on error
  */
 static int vpbe_display_s_std(struct file *file, void *priv,
-				v4l2_std_id *std_id)
+				v4l2_std_id std_id)
 {
 	struct vpbe_fh *fh = priv;
 	struct vpbe_layer *layer = fh->layer;
@@ -1176,10 +1176,6 @@ vpbe_display_s_dv_timings(struct file *file, void *priv,
 			"Failed to set the dv timings info\n");
 		return -EINVAL;
 	}
-	/* set the current norm to zero to be consistent. If STD is used
-	 * v4l2 layer will set the norm properly on successful s_std call
-	 */
-	layer->video_dev.current_norm = 0;
 
 	return 0;
 }
@@ -1202,7 +1198,7 @@ vpbe_display_g_dv_timings(struct file *file, void *priv,
 	/* Get the given standard in the encoder */
 
 	if (vpbe_dev->current_timings.timings_type &
-				VPBE_ENC_CUSTOM_TIMINGS) {
+				VPBE_ENC_DV_TIMINGS) {
 		*dv_timings = vpbe_dev->current_timings.dv_timings;
 	} else {
 		return -EINVAL;
@@ -1404,6 +1400,7 @@ static int vpbe_display_reqbufs(struct file *file, void *priv,
 	q->ops = &video_qops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct vpbe_disp_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	ret = vb2_queue_init(q);
 	if (ret) {
@@ -1600,7 +1597,7 @@ static int vpbe_display_g_register(struct file *file, void *priv,
 }
 
 static int vpbe_display_s_register(struct file *file, void *priv,
-			struct v4l2_dbg_register *reg)
+			const struct v4l2_dbg_register *reg)
 {
 	return 0;
 }
@@ -1693,12 +1690,8 @@ static int init_vpbe_layer(int i, struct vpbe_display *disp_dev,
 	vbd->vfl_dir	= VFL_DIR_TX;
 
 	if (disp_dev->vpbe_dev->current_timings.timings_type &
-			VPBE_ENC_STD) {
+			VPBE_ENC_STD)
 		vbd->tvnorms = (V4L2_STD_525_60 | V4L2_STD_625_50);
-		vbd->current_norm =
-			disp_dev->vpbe_dev->current_timings.std_id;
-	} else
-		vbd->current_norm = 0;
 
 	snprintf(vbd->name, sizeof(vbd->name),
 			"DaVinci_VPBE Display_DRIVER_V%d.%d.%d",
diff --git a/drivers/media/platform/davinci/vpbe_osd.c b/drivers/media/platform/davinci/vpbe_osd.c
index 12ad17c52ef3..396a51cbede7 100644
--- a/drivers/media/platform/davinci/vpbe_osd.c
+++ b/drivers/media/platform/davinci/vpbe_osd.c
@@ -52,6 +52,9 @@ static struct platform_device_id vpbe_osd_devtype[] = {
 		.name = DM355_VPBE_OSD_SUBDEV_NAME,
 		.driver_data = VPBE_VERSION_3,
 	},
+	{
+		/* sentinel */
+	}
 };
 
 MODULE_DEVICE_TABLE(platform, vpbe_osd_devtype);
diff --git a/drivers/media/platform/davinci/vpbe_venc.c b/drivers/media/platform/davinci/vpbe_venc.c
index bdbebd59df98..87eef9be08ed 100644
--- a/drivers/media/platform/davinci/vpbe_venc.c
+++ b/drivers/media/platform/davinci/vpbe_venc.c
@@ -51,6 +51,9 @@ static struct platform_device_id vpbe_venc_devtype[] = {
 		.name = DM355_VPBE_VENC_SUBDEV_NAME,
 		.driver_data = VPBE_VERSION_3,
 	},
+	{
+		/* sentinel */
+	}
 };
 
 MODULE_DEVICE_TABLE(platform, vpbe_venc_devtype);
@@ -199,6 +202,25 @@ static void venc_enabledigitaloutput(struct v4l2_subdev *sd, int benable)
 	}
 }
 
+static void
+venc_enable_vpss_clock(int venc_type,
+		       enum vpbe_enc_timings_type type,
+		       unsigned int pclock)
+{
+	if (venc_type == VPBE_VERSION_1)
+		return;
+
+	if (venc_type == VPBE_VERSION_2 && (type == VPBE_ENC_STD || (type ==
+	    VPBE_ENC_DV_TIMINGS && pclock <= 27000000))) {
+		vpss_enable_clock(VPSS_VENC_CLOCK_SEL, 1);
+		vpss_enable_clock(VPSS_VPBE_CLOCK, 1);
+		return;
+	}
+
+	if (venc_type == VPBE_VERSION_3 && type == VPBE_ENC_STD)
+		vpss_enable_clock(VPSS_VENC_CLOCK_SEL, 0);
+}
+
 #define VDAC_CONFIG_SD_V3	0x0E21A6B6
 #define VDAC_CONFIG_SD_V2	0x081141CF
 /*
@@ -217,6 +239,7 @@ static int venc_set_ntsc(struct v4l2_subdev *sd)
 	if (pdata->setup_clock(VPBE_ENC_STD, V4L2_STD_525_60) < 0)
 		return -EINVAL;
 
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_STD, V4L2_STD_525_60);
 	venc_enabledigitaloutput(sd, 0);
 
 	if (venc->venc_type == VPBE_VERSION_3) {
@@ -262,6 +285,7 @@ static int venc_set_pal(struct v4l2_subdev *sd)
 	if (venc->pdata->setup_clock(VPBE_ENC_STD, V4L2_STD_625_50) < 0)
 		return -EINVAL;
 
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_STD, V4L2_STD_625_50);
 	venc_enabledigitaloutput(sd, 0);
 
 	if (venc->venc_type == VPBE_VERSION_3) {
@@ -313,9 +337,10 @@ 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_CUSTOM_TIMINGS, 27000000) < 0)
+	if (pdata->setup_clock(VPBE_ENC_DV_TIMINGS, 27000000) < 0)
 		return -EINVAL;
 
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_DV_TIMINGS, 27000000);
 	venc_enabledigitaloutput(sd, 0);
 
 	if (venc->venc_type == VPBE_VERSION_2)
@@ -360,9 +385,10 @@ static int venc_set_576p50(struct v4l2_subdev *sd)
 	    venc->venc_type != VPBE_VERSION_2)
 		return -EINVAL;
 	/* Setup clock at VPSS & VENC for SD */
-	if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 27000000) < 0)
+	if (pdata->setup_clock(VPBE_ENC_DV_TIMINGS, 27000000) < 0)
 		return -EINVAL;
 
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_DV_TIMINGS, 27000000);
 	venc_enabledigitaloutput(sd, 0);
 
 	if (venc->venc_type == VPBE_VERSION_2)
@@ -400,9 +426,10 @@ 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_CUSTOM_TIMINGS, 74250000) < 0)
+	if (pdata->setup_clock(VPBE_ENC_DV_TIMINGS, 74250000) < 0)
 		return -EINVAL;
 
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_DV_TIMINGS, 74250000);
 	venc_enabledigitaloutput(sd, 0);
 
 	venc_write(sd, VENC_OSDCLK0, 0);
@@ -428,9 +455,10 @@ 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_CUSTOM_TIMINGS, 74250000) < 0)
+	if (pdata->setup_clock(VPBE_ENC_DV_TIMINGS, 74250000) < 0)
 		return -EINVAL;
 
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_DV_TIMINGS, 74250000);
 	venc_enabledigitaloutput(sd, 0);
 
 	venc_write(sd, VENC_OSDCLK0, 0);
diff --git a/drivers/media/platform/davinci/vpfe_capture.c b/drivers/media/platform/davinci/vpfe_capture.c
index 28d019da4c01..8c50d3074866 100644
--- a/drivers/media/platform/davinci/vpfe_capture.c
+++ b/drivers/media/platform/davinci/vpfe_capture.c
@@ -376,7 +376,7 @@ static int vpfe_config_ccdc_image_format(struct vpfe_device *vpfe_dev)
  * values in ccdc
  */
 static int vpfe_config_image_format(struct vpfe_device *vpfe_dev,
-				    const v4l2_std_id *std_id)
+				    v4l2_std_id std_id)
 {
 	struct vpfe_subdev_info *sdinfo = vpfe_dev->current_subdev;
 	struct v4l2_mbus_framefmt mbus_fmt;
@@ -384,7 +384,7 @@ static int vpfe_config_image_format(struct vpfe_device *vpfe_dev,
 	int i, ret = 0;
 
 	for (i = 0; i < ARRAY_SIZE(vpfe_standards); i++) {
-		if (vpfe_standards[i].std_id & *std_id) {
+		if (vpfe_standards[i].std_id & std_id) {
 			vpfe_dev->std_info.active_pixels =
 					vpfe_standards[i].width;
 			vpfe_dev->std_info.active_lines =
@@ -461,7 +461,7 @@ static int vpfe_initialize_device(struct vpfe_device *vpfe_dev)
 
 	/* Configure the default format information */
 	ret = vpfe_config_image_format(vpfe_dev,
-				&vpfe_standards[vpfe_dev->std_index].std_id);
+				vpfe_standards[vpfe_dev->std_index].std_id);
 	if (ret)
 		return ret;
 
@@ -1107,6 +1107,7 @@ static int vpfe_g_input(struct file *file, void *priv, unsigned int *index)
 static int vpfe_s_input(struct file *file, void *priv, unsigned int index)
 {
 	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct v4l2_subdev *sd;
 	struct vpfe_subdev_info *sdinfo;
 	int subdev_index, inp_index;
 	struct vpfe_route *route;
@@ -1138,14 +1139,15 @@ static int vpfe_s_input(struct file *file, void *priv, unsigned int index)
 	}
 
 	sdinfo = &vpfe_dev->cfg->sub_devs[subdev_index];
+	sd = vpfe_dev->sd[subdev_index];
 	route = &sdinfo->routes[inp_index];
 	if (route && sdinfo->can_route) {
 		input = route->input;
 		output = route->output;
 	}
 
-	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
-					 video, s_routing, input, output, 0);
+	if (sd)
+		ret = v4l2_subdev_call(sd, video, s_routing, input, output, 0);
 
 	if (ret) {
 		v4l2_err(&vpfe_dev->v4l2_dev,
@@ -1154,6 +1156,8 @@ static int vpfe_s_input(struct file *file, void *priv, unsigned int index)
 		goto unlock_out;
 	}
 	vpfe_dev->current_subdev = sdinfo;
+	if (sd)
+		vpfe_dev->v4l2_dev.ctrl_handler = sd->ctrl_handler;
 	vpfe_dev->current_input = index;
 	vpfe_dev->std_index = 0;
 
@@ -1164,7 +1168,7 @@ static int vpfe_s_input(struct file *file, void *priv, unsigned int index)
 
 	/* set the default image parameters in the device */
 	ret = vpfe_config_image_format(vpfe_dev,
-				&vpfe_standards[vpfe_dev->std_index].std_id);
+				vpfe_standards[vpfe_dev->std_index].std_id);
 unlock_out:
 	mutex_unlock(&vpfe_dev->lock);
 	return ret;
@@ -1189,7 +1193,7 @@ static int vpfe_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
 	return ret;
 }
 
-static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
+static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 {
 	struct vpfe_device *vpfe_dev = video_drvdata(file);
 	struct vpfe_subdev_info *sdinfo;
@@ -1211,7 +1215,7 @@ static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
 	}
 
 	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
-					 core, s_std, *std_id);
+					 core, s_std, std_id);
 	if (ret < 0) {
 		v4l2_err(&vpfe_dev->v4l2_dev, "Failed to set standard\n");
 		goto unlock_out;
@@ -1439,41 +1443,6 @@ static int vpfe_dqbuf(struct file *file, void *priv,
 				      buf, file->f_flags & O_NONBLOCK);
 }
 
-static int vpfe_queryctrl(struct file *file, void *priv,
-		struct v4l2_queryctrl *qctrl)
-{
-	struct vpfe_device *vpfe_dev = video_drvdata(file);
-	struct vpfe_subdev_info *sdinfo;
-
-	sdinfo = vpfe_dev->current_subdev;
-
-	return v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
-					 core, queryctrl, qctrl);
-
-}
-
-static int vpfe_g_ctrl(struct file *file, void *priv, struct v4l2_control *ctrl)
-{
-	struct vpfe_device *vpfe_dev = video_drvdata(file);
-	struct vpfe_subdev_info *sdinfo;
-
-	sdinfo = vpfe_dev->current_subdev;
-
-	return v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
-					 core, g_ctrl, ctrl);
-}
-
-static int vpfe_s_ctrl(struct file *file, void *priv, struct v4l2_control *ctrl)
-{
-	struct vpfe_device *vpfe_dev = video_drvdata(file);
-	struct vpfe_subdev_info *sdinfo;
-
-	sdinfo = vpfe_dev->current_subdev;
-
-	return v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
-					 core, s_ctrl, ctrl);
-}
-
 /*
  * vpfe_calculate_offsets : This function calculates buffers offset
  * for top and bottom field
@@ -1717,7 +1686,7 @@ unlock_out:
 
 
 static long vpfe_param_handler(struct file *file, void *priv,
-		bool valid_prio, int cmd, void *param)
+		bool valid_prio, unsigned int cmd, void *param)
 {
 	struct vpfe_device *vpfe_dev = video_drvdata(file);
 	int ret = 0;
@@ -1781,9 +1750,6 @@ static const struct v4l2_ioctl_ops vpfe_ioctl_ops = {
 	.vidioc_querystd	 = vpfe_querystd,
 	.vidioc_s_std		 = vpfe_s_std,
 	.vidioc_g_std		 = vpfe_g_std,
-	.vidioc_queryctrl	 = vpfe_queryctrl,
-	.vidioc_g_ctrl		 = vpfe_g_ctrl,
-	.vidioc_s_ctrl		 = vpfe_s_ctrl,
 	.vidioc_reqbufs		 = vpfe_reqbufs,
 	.vidioc_querybuf	 = vpfe_querybuf,
 	.vidioc_qbuf		 = vpfe_qbuf,
@@ -1918,7 +1884,6 @@ static int vpfe_probe(struct platform_device *pdev)
 	vfd->fops		= &vpfe_fops;
 	vfd->ioctl_ops		= &vpfe_ioctl_ops;
 	vfd->tvnorms		= 0;
-	vfd->current_norm	= V4L2_STD_PAL;
 	vfd->v4l2_dev 		= &vpfe_dev->v4l2_dev;
 	snprintf(vfd->name, sizeof(vfd->name),
 		 "%s_V%d.%d.%d",
@@ -2007,6 +1972,7 @@ static int vpfe_probe(struct platform_device *pdev)
 
 	/* set first sub device as current one */
 	vpfe_dev->current_subdev = &vpfe_cfg->sub_devs[0];
+	vpfe_dev->v4l2_dev.ctrl_handler = vpfe_dev->sd[0]->ctrl_handler;
 
 	/* We have at least one sub device to work with */
 	mutex_unlock(&ccdc_lock);
diff --git a/drivers/media/platform/davinci/vpif.c b/drivers/media/platform/davinci/vpif.c
index 28638a86f129..ea82a8bd2803 100644
--- a/drivers/media/platform/davinci/vpif.c
+++ b/drivers/media/platform/davinci/vpif.c
@@ -23,8 +23,8 @@
 #include <linux/spinlock.h>
 #include <linux/kernel.h>
 #include <linux/io.h>
-#include <linux/clk.h>
 #include <linux/err.h>
+#include <linux/pm_runtime.h>
 #include <linux/v4l2-dv-timings.h>
 
 #include <mach/hardware.h>
@@ -44,13 +44,13 @@ static struct resource	*res;
 spinlock_t vpif_lock;
 
 void __iomem *vpif_base;
-struct clk *vpif_clk;
+EXPORT_SYMBOL_GPL(vpif_base);
 
 /**
- * ch_params: video standard configuration parameters for vpif
+ * vpif_ch_params: video standard configuration parameters for vpif
  * The table must include all presets from supported subdevices.
  */
-const struct vpif_channel_config_params ch_params[] = {
+const struct vpif_channel_config_params vpif_ch_params[] = {
 	/* HDTV formats */
 	{
 		.name = "480p59_94",
@@ -220,8 +220,10 @@ const struct vpif_channel_config_params ch_params[] = {
 		.stdid = V4L2_STD_625_50,
 	},
 };
+EXPORT_SYMBOL_GPL(vpif_ch_params);
 
-const unsigned int vpif_ch_params_count = ARRAY_SIZE(ch_params);
+const unsigned int vpif_ch_params_count = ARRAY_SIZE(vpif_ch_params);
+EXPORT_SYMBOL_GPL(vpif_ch_params_count);
 
 static inline void vpif_wr_bit(u32 reg, u32 bit, u32 val)
 {
@@ -439,19 +441,13 @@ static int vpif_probe(struct platform_device *pdev)
 		goto fail;
 	}
 
-	vpif_clk = clk_get(&pdev->dev, "vpif");
-	if (IS_ERR(vpif_clk)) {
-		status = PTR_ERR(vpif_clk);
-		goto clk_fail;
-	}
-	clk_prepare_enable(vpif_clk);
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_get(&pdev->dev);
 
 	spin_lock_init(&vpif_lock);
 	dev_info(&pdev->dev, "vpif probe success\n");
 	return 0;
 
-clk_fail:
-	iounmap(vpif_base);
 fail:
 	release_mem_region(res->start, res_len);
 	return status;
@@ -459,11 +455,7 @@ fail:
 
 static int vpif_remove(struct platform_device *pdev)
 {
-	if (vpif_clk) {
-		clk_disable_unprepare(vpif_clk);
-		clk_put(vpif_clk);
-	}
-
+	pm_runtime_disable(&pdev->dev);
 	iounmap(vpif_base);
 	release_mem_region(res->start, res_len);
 	return 0;
@@ -472,13 +464,13 @@ static int vpif_remove(struct platform_device *pdev)
 #ifdef CONFIG_PM
 static int vpif_suspend(struct device *dev)
 {
-	clk_disable_unprepare(vpif_clk);
+	pm_runtime_put(dev);
 	return 0;
 }
 
 static int vpif_resume(struct device *dev)
 {
-	clk_prepare_enable(vpif_clk);
+	pm_runtime_get(dev);
 	return 0;
 }
 
diff --git a/drivers/media/platform/davinci/vpif.h b/drivers/media/platform/davinci/vpif.h
index a1ab6a0f4e9e..9956e6788693 100644
--- a/drivers/media/platform/davinci/vpif.h
+++ b/drivers/media/platform/davinci/vpif.h
@@ -638,7 +638,7 @@ struct vpif_channel_config_params {
 };
 
 extern const unsigned int vpif_ch_params_count;
-extern const struct vpif_channel_config_params ch_params[];
+extern const struct vpif_channel_config_params vpif_ch_params[];
 
 struct vpif_video_params;
 struct vpif_params;
diff --git a/drivers/media/platform/davinci/vpif_capture.c b/drivers/media/platform/davinci/vpif_capture.c
index 5892d2bc8eee..5f98df1fc8a0 100644
--- a/drivers/media/platform/davinci/vpif_capture.c
+++ b/drivers/media/platform/davinci/vpif_capture.c
@@ -563,7 +563,7 @@ static int vpif_update_std_info(struct channel_obj *ch)
 	vpif_dbg(2, debug, "vpif_update_std_info\n");
 
 	for (index = 0; index < vpif_ch_params_count; index++) {
-		config = &ch_params[index];
+		config = &vpif_ch_params[index];
 		if (config->hd_sd == 0) {
 			vpif_dbg(2, debug, "SD format\n");
 			if (config->stdid & vid_ch->stdid) {
@@ -1035,6 +1035,7 @@ static int vpif_reqbufs(struct file *file, void *priv,
 	q->ops = &video_qops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct vpif_cap_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	ret = vb2_queue_init(q);
 	if (ret) {
@@ -1394,7 +1395,7 @@ static int vpif_g_std(struct file *file, void *priv, v4l2_std_id *std)
  * @priv: file handle
  * @std_id: ptr to std id
  */
-static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
+static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 {
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
@@ -1423,7 +1424,7 @@ static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
 	fh->initialized = 1;
 
 	/* Call encoder subdevice function to set the standard */
-	ch->video.stdid = *std_id;
+	ch->video.stdid = std_id;
 	memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings));
 
 	/* Get the information about the standard */
@@ -1436,7 +1437,7 @@ 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(ch->sd, core, s_std, *std_id);
+	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;
@@ -1923,7 +1924,8 @@ static int vpif_dbg_g_register(struct file *file, void *priv,
  * Returns zero or -EINVAL if write operations fails.
  */
 static int vpif_dbg_s_register(struct file *file, void *priv,
-		struct v4l2_dbg_register *reg){
+		const struct v4l2_dbg_register *reg)
+{
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
 
diff --git a/drivers/media/platform/davinci/vpif_display.c b/drivers/media/platform/davinci/vpif_display.c
index dd249c96126d..1b3fb5ca2ad4 100644
--- a/drivers/media/platform/davinci/vpif_display.c
+++ b/drivers/media/platform/davinci/vpif_display.c
@@ -511,7 +511,7 @@ static int vpif_update_std_info(struct channel_obj *ch)
 	int i;
 
 	for (i = 0; i < vpif_ch_params_count; i++) {
-		config = &ch_params[i];
+		config = &vpif_ch_params[i];
 		if (config->hd_sd == 0) {
 			vpif_dbg(2, debug, "SD format\n");
 			if (config->stdid & vid_ch->stdid) {
@@ -1001,6 +1001,7 @@ static int vpif_reqbufs(struct file *file, void *priv,
 	q->ops = &video_qops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct vpif_disp_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	ret = vb2_queue_init(q);
 	if (ret) {
@@ -1058,14 +1059,14 @@ static int vpif_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 	return vb2_qbuf(&common->buffer_queue, buf);
 }
 
-static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
+static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 {
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 	int ret = 0;
 
-	if (!(*std_id & VPIF_V4L2_STD))
+	if (!(std_id & VPIF_V4L2_STD))
 		return -EINVAL;
 
 	if (common->started) {
@@ -1074,7 +1075,7 @@ static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
 	}
 
 	/* Call encoder subdevice function to set the standard */
-	ch->video.stdid = *std_id;
+	ch->video.stdid = std_id;
 	memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings));
 	/* Get the information about the standard */
 	if (vpif_update_resolution(ch))
@@ -1092,14 +1093,14 @@ static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
 	vpif_config_format(ch);
 
 	ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, video,
-						s_std_output, *std_id);
+						s_std_output, std_id);
 	if (ret < 0) {
 		vpif_err("Failed to set output standard\n");
 		return ret;
 	}
 
 	ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, core,
-							s_std, *std_id);
+							s_std, std_id);
 	if (ret < 0)
 		vpif_err("Failed to set standard for sub devices\n");
 	return ret;
@@ -1567,7 +1568,8 @@ static int vpif_dbg_g_register(struct file *file, void *priv,
  * Returns zero or -EINVAL if write operations fails.
  */
 static int vpif_dbg_s_register(struct file *file, void *priv,
-		struct v4l2_dbg_register *reg){
+		const struct v4l2_dbg_register *reg)
+{
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
 
diff --git a/drivers/media/platform/davinci/vpss.c b/drivers/media/platform/davinci/vpss.c
index a19c552232d1..8a2f01e344ee 100644
--- a/drivers/media/platform/davinci/vpss.c
+++ b/drivers/media/platform/davinci/vpss.c
@@ -17,14 +17,11 @@
  *
  * common vpss system module platform driver for all video drivers.
  */
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/init.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
-#include <linux/spinlock.h>
-#include <linux/compiler.h>
 #include <linux/io.h>
+#include <linux/pm_runtime.h>
+
 #include <media/davinci/vpss.h>
 
 MODULE_LICENSE("GPL");
@@ -99,7 +96,7 @@ enum vpss_platform_type {
 
 /*
  * vpss operations. Depends on platform. Not all functions are available
- * on all platforms. The api, first check if a functio is available before
+ * on all platforms. The api, first check if a function is available before
  * invoking it. In the probe, the function ptrs are initialized based on
  * vpss name. vpss name can be "dm355_vpss", "dm644x_vpss" etc.
  */
@@ -114,7 +111,7 @@ struct vpss_hw_ops {
 	void (*set_sync_pol)(struct vpss_sync_pol);
 	/* set the PG_FRAME_SIZE register*/
 	void (*set_pg_frame_size)(struct vpss_pg_frame_size);
-	/* check and clear interrupt if occured */
+	/* check and clear interrupt if occurred */
 	int (*dma_complete_interrupt)(void);
 };
 
@@ -233,7 +230,7 @@ EXPORT_SYMBOL(vpss_clear_wbl_overflow);
 
 /*
  *  dm355_enable_clock - Enable VPSS Clock
- *  @clock_sel: CLock to be enabled/disabled
+ *  @clock_sel: Clock to be enabled/disabled
  *  @en: enable/disable flag
  *
  *  This is called to enable or disable a vpss clock
@@ -490,6 +487,10 @@ static int vpss_probe(struct platform_device *pdev)
 	} else
 		oper_cfg.hw_ops.clear_wbl_overflow = dm644x_clear_wbl_overflow;
 
+	pm_runtime_enable(&pdev->dev);
+
+	pm_runtime_get(&pdev->dev);
+
 	spin_lock_init(&oper_cfg.vpss_lock);
 	dev_info(&pdev->dev, "%s vpss probe success\n", platform_name);
 	return 0;
@@ -507,6 +508,7 @@ static int vpss_remove(struct platform_device *pdev)
 {
 	struct resource		*res;
 
+	pm_runtime_disable(&pdev->dev);
 	iounmap(oper_cfg.vpss_regs_base0);
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	release_mem_region(res->start, resource_size(res));
@@ -518,10 +520,28 @@ static int vpss_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static int vpss_suspend(struct device *dev)
+{
+	pm_runtime_put(dev);
+	return 0;
+}
+
+static int vpss_resume(struct device *dev)
+{
+	pm_runtime_get(dev);
+	return 0;
+}
+
+static const struct dev_pm_ops vpss_pm_ops = {
+	.suspend = vpss_suspend,
+	.resume = vpss_resume,
+};
+
 static struct platform_driver vpss_driver = {
 	.driver = {
 		.name	= "vpss",
 		.owner = THIS_MODULE,
+		.pm = &vpss_pm_ops,
 	},
 	.remove = vpss_remove,
 	.probe = vpss_probe,
diff --git a/drivers/media/platform/exynos-gsc/gsc-m2m.c b/drivers/media/platform/exynos-gsc/gsc-m2m.c
index 386c0a7a3a52..40a73f7d20da 100644
--- a/drivers/media/platform/exynos-gsc/gsc-m2m.c
+++ b/drivers/media/platform/exynos-gsc/gsc-m2m.c
@@ -80,6 +80,9 @@ void gsc_m2m_job_finish(struct gsc_ctx *ctx, int vb_state)
 	dst_vb = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
 
 	if (src_vb && dst_vb) {
+		src_vb->v4l2_buf.timestamp = dst_vb->v4l2_buf.timestamp;
+		src_vb->v4l2_buf.timecode = dst_vb->v4l2_buf.timecode;
+
 		v4l2_m2m_buf_done(src_vb, vb_state);
 		v4l2_m2m_buf_done(dst_vb, vb_state);
 
@@ -584,6 +587,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->ops = &gsc_m2m_qops;
 	src_vq->mem_ops = &vb2_dma_contig_memops;
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -596,6 +600,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->ops = &gsc_m2m_qops;
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	return vb2_queue_init(dst_vq);
 }
diff --git a/drivers/media/platform/exynos-gsc/gsc-regs.c b/drivers/media/platform/exynos-gsc/gsc-regs.c
index 6f5b5a486cf3..e22d147a6940 100644
--- a/drivers/media/platform/exynos-gsc/gsc-regs.c
+++ b/drivers/media/platform/exynos-gsc/gsc-regs.c
@@ -12,7 +12,6 @@
 
 #include <linux/io.h>
 #include <linux/delay.h>
-#include <mach/map.h>
 
 #include "gsc-core.h"
 
diff --git a/drivers/media/platform/s5p-fimc/Kconfig b/drivers/media/platform/exynos4-is/Kconfig
index f997a5203b7c..6ff99b5849f9 100644
--- a/drivers/media/platform/s5p-fimc/Kconfig
+++ b/drivers/media/platform/exynos4-is/Kconfig
@@ -1,21 +1,22 @@
 
-config VIDEO_SAMSUNG_S5P_FIMC
-	bool "Samsung S5P/EXYNOS SoC camera interface driver (experimental)"
+config VIDEO_SAMSUNG_EXYNOS4_IS
+	bool "Samsung S5P/EXYNOS4 SoC series Camera Subsystem driver"
 	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && PLAT_S5P && PM_RUNTIME
 	help
 	  Say Y here to enable camera host interface devices for
 	  Samsung S5P and EXYNOS SoC series.
 
-if VIDEO_SAMSUNG_S5P_FIMC
+if VIDEO_SAMSUNG_EXYNOS4_IS
 
 config VIDEO_S5P_FIMC
 	tristate "S5P/EXYNOS4 FIMC/CAMIF camera interface driver"
 	depends on I2C
 	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
+	select MFD_SYSCON if OF
 	help
 	  This is a V4L2 driver for Samsung S5P and EXYNOS4 SoC camera host
-	  interface and video postprocessor (FIMC and FIMC-LITE) devices.
+	  interface and video postprocessor (FIMC) devices.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called s5p-fimc.
@@ -45,4 +46,16 @@ config VIDEO_EXYNOS_FIMC_LITE
 	  module will be called exynos-fimc-lite.
 endif
 
+config VIDEO_EXYNOS4_FIMC_IS
+	tristate "EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver"
+	select VIDEOBUF2_DMA_CONTIG
+	depends on OF
+	select FW_LOADER
+	help
+	  This is a V4L2 driver for Samsung EXYNOS4x12 SoC series
+	  FIMC-IS (Imaging Subsystem).
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called exynos4-fimc-is.
+
 endif # VIDEO_SAMSUNG_S5P_FIMC
diff --git a/drivers/media/platform/s5p-fimc/Makefile b/drivers/media/platform/exynos4-is/Makefile
index 46485143e1ca..f25f46377399 100644
--- a/drivers/media/platform/s5p-fimc/Makefile
+++ b/drivers/media/platform/exynos4-is/Makefile
@@ -1,7 +1,10 @@
-s5p-fimc-objs := fimc-core.o fimc-reg.o fimc-m2m.o fimc-capture.o fimc-mdevice.o
+s5p-fimc-objs := fimc-core.o fimc-reg.o fimc-m2m.o fimc-capture.o media-dev.o
 exynos-fimc-lite-objs += fimc-lite-reg.o fimc-lite.o
+exynos-fimc-is-objs := fimc-is.o fimc-isp.o fimc-is-sensor.o fimc-is-regs.o
+exynos-fimc-is-objs += fimc-is-param.o fimc-is-errno.o fimc-is-i2c.o
 s5p-csis-objs := mipi-csis.o
 
 obj-$(CONFIG_VIDEO_S5P_MIPI_CSIS)	+= s5p-csis.o
 obj-$(CONFIG_VIDEO_EXYNOS_FIMC_LITE)	+= exynos-fimc-lite.o
+obj-$(CONFIG_VIDEO_EXYNOS4_FIMC_IS)	+= exynos-fimc-is.o
 obj-$(CONFIG_VIDEO_S5P_FIMC)		+= s5p-fimc.o
diff --git a/drivers/media/platform/s5p-fimc/fimc-capture.c b/drivers/media/platform/exynos4-is/fimc-capture.c
index f553cc2a8ee8..528f41369364 100644
--- a/drivers/media/platform/s5p-fimc/fimc-capture.c
+++ b/drivers/media/platform/exynos4-is/fimc-capture.c
@@ -27,32 +27,33 @@
 #include <media/videobuf2-core.h>
 #include <media/videobuf2-dma-contig.h>
 
-#include "fimc-mdevice.h"
+#include "media-dev.h"
 #include "fimc-core.h"
 #include "fimc-reg.h"
 
 static int fimc_capture_hw_init(struct fimc_dev *fimc)
 {
+	struct fimc_source_info *si = &fimc->vid_cap.source_config;
 	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
-	struct fimc_pipeline *p = &fimc->pipeline;
-	struct fimc_sensor_info *sensor;
+	int ret;
 	unsigned long flags;
-	int ret = 0;
 
-	if (p->subdevs[IDX_SENSOR] == NULL || ctx == NULL)
-		return -ENXIO;
-	if (ctx->s_frame.fmt == NULL)
+	if (ctx == NULL || ctx->s_frame.fmt == NULL)
 		return -EINVAL;
 
-	sensor = v4l2_get_subdev_hostdata(p->subdevs[IDX_SENSOR]);
+	if (si->fimc_bus_type == FIMC_BUS_TYPE_ISP_WRITEBACK) {
+		ret = fimc_hw_camblk_cfg_writeback(fimc);
+		if (ret < 0)
+			return ret;
+	}
 
 	spin_lock_irqsave(&fimc->slock, flags);
 	fimc_prepare_dma_offset(ctx, &ctx->d_frame);
 	fimc_set_yuv_order(ctx);
 
-	fimc_hw_set_camera_polarity(fimc, &sensor->pdata);
-	fimc_hw_set_camera_type(fimc, &sensor->pdata);
-	fimc_hw_set_camera_source(fimc, &sensor->pdata);
+	fimc_hw_set_camera_polarity(fimc, si);
+	fimc_hw_set_camera_type(fimc, si);
+	fimc_hw_set_camera_source(fimc, si);
 	fimc_hw_set_camera_offset(fimc, &ctx->s_frame);
 
 	ret = fimc_set_scaler_info(ctx);
@@ -65,7 +66,7 @@ static int fimc_capture_hw_init(struct fimc_dev *fimc)
 		fimc_hw_set_effect(ctx);
 		fimc_hw_set_output_path(ctx);
 		fimc_hw_set_out_dma(ctx);
-		if (fimc->variant->has_alpha)
+		if (fimc->drv_data->alpha_color)
 			fimc_hw_set_rgb_alpha(ctx);
 		clear_bit(ST_CAPT_APPLY_CFG, &fimc->state);
 	}
@@ -168,7 +169,7 @@ static int fimc_capture_config_update(struct fimc_ctx *ctx)
 	fimc_hw_set_effect(ctx);
 	fimc_prepare_dma_offset(ctx, &ctx->d_frame);
 	fimc_hw_set_out_dma(ctx);
-	if (fimc->variant->has_alpha)
+	if (fimc->drv_data->alpha_color)
 		fimc_hw_set_rgb_alpha(ctx);
 
 	clear_bit(ST_CAPT_APPLY_CFG, &fimc->state);
@@ -286,8 +287,8 @@ static int start_streaming(struct vb2_queue *q, unsigned int count)
 		fimc_activate_capture(ctx);
 
 		if (!test_and_set_bit(ST_CAPT_ISP_STREAM, &fimc->state))
-			fimc_pipeline_call(fimc, set_stream,
-					   &fimc->pipeline, 1);
+			return fimc_pipeline_call(fimc, set_stream,
+						  &fimc->pipeline, 1);
 	}
 
 	return 0;
@@ -443,35 +444,28 @@ static void buffer_queue(struct vb2_buffer *vb)
 	if (vb2_is_streaming(&vid_cap->vbq) &&
 	    vid_cap->active_buf_cnt >= min_bufs &&
 	    !test_and_set_bit(ST_CAPT_STREAM, &fimc->state)) {
+		int ret;
+
 		fimc_activate_capture(ctx);
 		spin_unlock_irqrestore(&fimc->slock, flags);
 
-		if (!test_and_set_bit(ST_CAPT_ISP_STREAM, &fimc->state))
-			fimc_pipeline_call(fimc, set_stream,
-					   &fimc->pipeline, 1);
+		if (test_and_set_bit(ST_CAPT_ISP_STREAM, &fimc->state))
+			return;
+
+		ret = fimc_pipeline_call(fimc, set_stream, &fimc->pipeline, 1);
+		if (ret < 0)
+			v4l2_err(&vid_cap->vfd, "stream on failed: %d\n", ret);
 		return;
 	}
 	spin_unlock_irqrestore(&fimc->slock, flags);
 }
 
-static void fimc_lock(struct vb2_queue *vq)
-{
-	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
-	mutex_lock(&ctx->fimc_dev->lock);
-}
-
-static void fimc_unlock(struct vb2_queue *vq)
-{
-	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
-	mutex_unlock(&ctx->fimc_dev->lock);
-}
-
 static struct vb2_ops fimc_capture_qops = {
 	.queue_setup		= queue_setup,
 	.buf_prepare		= buffer_prepare,
 	.buf_queue		= buffer_queue,
-	.wait_prepare		= fimc_unlock,
-	.wait_finish		= fimc_lock,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
 	.start_streaming	= start_streaming,
 	.stop_streaming		= stop_streaming,
 };
@@ -530,7 +524,7 @@ static int fimc_capture_open(struct file *file)
 		goto unlock;
 	}
 
-	if (++fimc->vid_cap.refcnt == 1) {
+	if (v4l2_fh_is_singular_file(file)) {
 		ret = fimc_pipeline_call(fimc, open, &fimc->pipeline,
 					 &fimc->vid_cap.vfd.entity, true);
 
@@ -543,8 +537,9 @@ static int fimc_capture_open(struct file *file)
 		if (ret < 0) {
 			clear_bit(ST_CAPT_BUSY, &fimc->state);
 			pm_runtime_put_sync(&fimc->pdev->dev);
-			fimc->vid_cap.refcnt--;
 			v4l2_fh_release(file);
+		} else {
+			fimc->vid_cap.refcnt++;
 		}
 	}
 unlock:
@@ -553,59 +548,34 @@ unlock:
 	return ret;
 }
 
-static int fimc_capture_close(struct file *file)
+static int fimc_capture_release(struct file *file)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
 	int ret;
 
 	dbg("pid: %d, state: 0x%lx", task_pid_nr(current), fimc->state);
 
 	mutex_lock(&fimc->lock);
 
-	if (--fimc->vid_cap.refcnt == 0) {
+	if (v4l2_fh_is_singular_file(file)) {
+		if (vc->streaming) {
+			media_entity_pipeline_stop(&vc->vfd.entity);
+			vc->streaming = false;
+		}
 		clear_bit(ST_CAPT_BUSY, &fimc->state);
 		fimc_stop_capture(fimc, false);
 		fimc_pipeline_call(fimc, close, &fimc->pipeline);
 		clear_bit(ST_CAPT_SUSPENDED, &fimc->state);
+		fimc->vid_cap.refcnt--;
 	}
 
 	pm_runtime_put(&fimc->pdev->dev);
 
-	if (fimc->vid_cap.refcnt == 0) {
-		vb2_queue_release(&fimc->vid_cap.vbq);
+	if (v4l2_fh_is_singular_file(file))
 		fimc_ctrls_delete(fimc->vid_cap.ctx);
-	}
-
-	ret = v4l2_fh_release(file);
-
-	mutex_unlock(&fimc->lock);
-	return ret;
-}
-
-static unsigned int fimc_capture_poll(struct file *file,
-				      struct poll_table_struct *wait)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-	int ret;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return POLL_ERR;
 
-	ret = vb2_poll(&fimc->vid_cap.vbq, file, wait);
-	mutex_unlock(&fimc->lock);
-
-	return ret;
-}
-
-static int fimc_capture_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-	int ret;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
-	ret = vb2_mmap(&fimc->vid_cap.vbq, vma);
+	ret = vb2_fop_release(file);
 	mutex_unlock(&fimc->lock);
 
 	return ret;
@@ -614,10 +584,10 @@ static int fimc_capture_mmap(struct file *file, struct vm_area_struct *vma)
 static const struct v4l2_file_operations fimc_capture_fops = {
 	.owner		= THIS_MODULE,
 	.open		= fimc_capture_open,
-	.release	= fimc_capture_close,
-	.poll		= fimc_capture_poll,
+	.release	= fimc_capture_release,
+	.poll		= vb2_fop_poll,
 	.unlocked_ioctl	= video_ioctl2,
-	.mmap		= fimc_capture_mmap,
+	.mmap		= vb2_fop_mmap,
 };
 
 /*
@@ -642,18 +612,22 @@ static struct fimc_fmt *fimc_capture_try_format(struct fimc_ctx *ctx,
 	    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)
+	if (fourcc && *fourcc != V4L2_PIX_FMT_JPEG && pad == FIMC_SD_PAD_SOURCE)
 		mask |= FMT_FLAGS_M2M;
 
+	if (pad == FIMC_SD_PAD_SINK_FIFO)
+		mask = FMT_FLAGS_WRITEBACK;
+
 	ffmt = fimc_find_format(fourcc, code, mask, 0);
 	if (WARN_ON(!ffmt))
 		return NULL;
+
 	if (code)
 		*code = ffmt->mbus_code;
 	if (fourcc)
 		*fourcc = ffmt->fourcc;
 
-	if (pad == FIMC_SD_PAD_SINK) {
+	if (pad != FIMC_SD_PAD_SOURCE) {
 		max_w = fimc_fmt_is_user_defined(ffmt->color) ?
 			pl->scaler_dis_w : pl->scaler_en_w;
 		/* Apply the camera input interface pixel constraints */
@@ -768,16 +742,13 @@ static void fimc_capture_try_selection(struct fimc_ctx *ctx,
 /*
  * The video node ioctl operations
  */
-static int fimc_vidioc_querycap_capture(struct file *file, void *priv,
+static int fimc_cap_querycap(struct file *file, void *priv,
 					struct v4l2_capability *cap)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
 
-	strncpy(cap->driver, fimc->pdev->name, sizeof(cap->driver) - 1);
-	strncpy(cap->card, fimc->pdev->name, sizeof(cap->card) - 1);
-	cap->bus_info[0] = 0;
-	cap->capabilities = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE_MPLANE;
-
+	__fimc_vidioc_querycap(&fimc->pdev->dev, cap, V4L2_CAP_STREAMING |
+					V4L2_CAP_VIDEO_CAPTURE_MPLANE);
 	return 0;
 }
 
@@ -887,7 +858,7 @@ static int fimc_pipeline_try_format(struct fimc_ctx *ctx,
 		tfmt->width  = mf->width;
 		tfmt->height = mf->height;
 		ffmt = fimc_capture_try_format(ctx, &tfmt->width, &tfmt->height,
-					NULL, &fcc, FIMC_SD_PAD_SINK);
+					NULL, &fcc, FIMC_SD_PAD_SINK_CAM);
 		ffmt = fimc_capture_try_format(ctx, &tfmt->width, &tfmt->height,
 					NULL, &fcc, FIMC_SD_PAD_SOURCE);
 		if (ffmt && ffmt->mbus_code)
@@ -974,7 +945,7 @@ static int fimc_cap_try_fmt_mplane(struct file *file, void *fh,
 	if (fimc_jpeg_fourcc(pix->pixelformat)) {
 		fimc_capture_try_format(ctx, &pix->width, &pix->height,
 					NULL, &pix->pixelformat,
-					FIMC_SD_PAD_SINK);
+					FIMC_SD_PAD_SINK_CAM);
 		ctx->s_frame.f_width  = pix->width;
 		ctx->s_frame.f_height = pix->height;
 	}
@@ -1028,7 +999,7 @@ static int __fimc_capture_set_format(struct fimc_dev *fimc,
 {
 	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
 	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
-	struct v4l2_mbus_framefmt *mf = &fimc->vid_cap.mf;
+	struct v4l2_mbus_framefmt *mf = &fimc->vid_cap.ci_fmt;
 	struct fimc_frame *ff = &ctx->d_frame;
 	struct fimc_fmt *s_fmt = NULL;
 	int ret, i;
@@ -1040,7 +1011,7 @@ static int __fimc_capture_set_format(struct fimc_dev *fimc,
 	if (fimc_jpeg_fourcc(pix->pixelformat)) {
 		fimc_capture_try_format(ctx, &pix->width, &pix->height,
 					NULL, &pix->pixelformat,
-					FIMC_SD_PAD_SINK);
+					FIMC_SD_PAD_SINK_CAM);
 		ctx->s_frame.f_width  = pix->width;
 		ctx->s_frame.f_height = pix->height;
 	}
@@ -1157,44 +1128,51 @@ static int fimc_cap_g_input(struct file *file, void *priv, unsigned int *i)
 static int fimc_pipeline_validate(struct fimc_dev *fimc)
 {
 	struct v4l2_subdev_format sink_fmt, src_fmt;
-	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
-	struct v4l2_subdev *sd;
-	struct media_pad *pad;
-	int ret;
-
-	/* Start with the video capture node pad */
-	pad = media_entity_remote_source(&vid_cap->vd_pad);
-	if (pad == NULL)
-		return -EPIPE;
-	/* FIMC.{N} subdevice */
-	sd = media_entity_to_v4l2_subdev(pad->entity);
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct v4l2_subdev *sd = &vc->subdev;
+	struct media_pad *sink_pad, *src_pad;
+	int i, ret;
 
 	while (1) {
-		/* Retrieve format at the sink pad */
-		pad = &sd->entity.pads[0];
-		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+		/*
+		 * Find current entity sink pad and any remote sink pad linked
+		 * to it. We stop if there is no sink pad in current entity or
+		 * it is not linked to any other remote entity.
+		 */
+		src_pad = NULL;
+
+		for (i = 0; i < sd->entity.num_pads; i++) {
+			struct media_pad *p = &sd->entity.pads[i];
+
+			if (p->flags & MEDIA_PAD_FL_SINK) {
+				sink_pad = p;
+				src_pad = media_entity_remote_source(sink_pad);
+				if (src_pad)
+					break;
+			}
+		}
+
+		if (src_pad == NULL ||
+		    media_entity_type(src_pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
 			break;
+
 		/* Don't call FIMC subdev operation to avoid nested locking */
-		if (sd == &fimc->vid_cap.subdev) {
-			struct fimc_frame *ff = &vid_cap->ctx->s_frame;
+		if (sd == &vc->subdev) {
+			struct fimc_frame *ff = &vc->ctx->s_frame;
 			sink_fmt.format.width = ff->f_width;
 			sink_fmt.format.height = ff->f_height;
 			sink_fmt.format.code = ff->fmt ? ff->fmt->mbus_code : 0;
 		} else {
-			sink_fmt.pad = pad->index;
+			sink_fmt.pad = sink_pad->index;
 			sink_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 			ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &sink_fmt);
 			if (ret < 0 && ret != -ENOIOCTLCMD)
 				return -EPIPE;
 		}
-		/* Retrieve format at the source pad */
-		pad = media_entity_remote_source(pad);
-		if (pad == NULL ||
-		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
-			break;
 
-		sd = media_entity_to_v4l2_subdev(pad->entity);
-		src_fmt.pad = pad->index;
+		/* Retrieve format at the source pad */
+		sd = media_entity_to_v4l2_subdev(src_pad->entity);
+		src_fmt.pad = src_pad->index;
 		src_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &src_fmt);
 		if (ret < 0 && ret != -ENOIOCTLCMD)
@@ -1208,7 +1186,7 @@ static int fimc_pipeline_validate(struct fimc_dev *fimc)
 		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;
+			struct fimc_frame *frame = &vc->ctx->d_frame;
 			unsigned int i;
 
 			ret = fimc_get_sensor_frame_desc(sd, plane_fmt,
@@ -1230,96 +1208,80 @@ static int fimc_cap_streamon(struct file *file, void *priv,
 {
 	struct fimc_dev *fimc = video_drvdata(file);
 	struct fimc_pipeline *p = &fimc->pipeline;
-	struct v4l2_subdev *sd = p->subdevs[IDX_SENSOR];
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct media_entity *entity = &vc->vfd.entity;
+	struct fimc_source_info *si = NULL;
+	struct v4l2_subdev *sd;
 	int ret;
 
 	if (fimc_capture_active(fimc))
 		return -EBUSY;
 
-	ret = media_entity_pipeline_start(&sd->entity, p->m_pipeline);
+	ret = media_entity_pipeline_start(entity, p->m_pipeline);
 	if (ret < 0)
 		return ret;
 
-	if (fimc->vid_cap.user_subdev_api) {
+	sd = p->subdevs[IDX_SENSOR];
+	if (sd)
+		si = v4l2_get_subdev_hostdata(sd);
+
+	if (si == NULL) {
+		ret = -EPIPE;
+		goto err_p_stop;
+	}
+	/*
+	 * Save configuration data related to currently attached image
+	 * sensor or other data source, e.g. FIMC-IS.
+	 */
+	vc->source_config = *si;
+
+	if (vc->input == GRP_ID_FIMC_IS)
+		vc->source_config.fimc_bus_type = FIMC_BUS_TYPE_ISP_WRITEBACK;
+
+	if (vc->user_subdev_api) {
 		ret = fimc_pipeline_validate(fimc);
-		if (ret < 0) {
-			media_entity_pipeline_stop(&sd->entity);
-			return ret;
-		}
+		if (ret < 0)
+			goto err_p_stop;
 	}
-	return vb2_streamon(&fimc->vid_cap.vbq, type);
+
+	ret = vb2_ioctl_streamon(file, priv, type);
+	if (!ret) {
+		vc->streaming = true;
+		return ret;
+	}
+
+err_p_stop:
+	media_entity_pipeline_stop(entity);
+	return ret;
 }
 
 static int fimc_cap_streamoff(struct file *file, void *priv,
 			    enum v4l2_buf_type type)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
-	struct v4l2_subdev *sd = fimc->pipeline.subdevs[IDX_SENSOR];
 	int ret;
 
-	ret = vb2_streamoff(&fimc->vid_cap.vbq, type);
-	if (ret == 0)
-		media_entity_pipeline_stop(&sd->entity);
-	return ret;
+	ret = vb2_ioctl_streamoff(file, priv, type);
+	if (ret < 0)
+		return ret;
+
+	media_entity_pipeline_stop(&fimc->vid_cap.vfd.entity);
+	fimc->vid_cap.streaming = false;
+	return 0;
 }
 
 static int fimc_cap_reqbufs(struct file *file, void *priv,
 			    struct v4l2_requestbuffers *reqbufs)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
-	int ret = vb2_reqbufs(&fimc->vid_cap.vbq, reqbufs);
+	int ret;
+
+	ret = vb2_ioctl_reqbufs(file, priv, reqbufs);
 
 	if (!ret)
 		fimc->vid_cap.reqbufs_count = reqbufs->count;
-	return ret;
-}
-
-static int fimc_cap_querybuf(struct file *file, void *priv,
-			   struct v4l2_buffer *buf)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-
-	return vb2_querybuf(&fimc->vid_cap.vbq, buf);
-}
-
-static int fimc_cap_qbuf(struct file *file, void *priv,
-			  struct v4l2_buffer *buf)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-
-	return vb2_qbuf(&fimc->vid_cap.vbq, buf);
-}
-
-static int fimc_cap_expbuf(struct file *file, void *priv,
-			  struct v4l2_exportbuffer *eb)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-
-	return vb2_expbuf(&fimc->vid_cap.vbq, eb);
-}
-
-static int fimc_cap_dqbuf(struct file *file, void *priv,
-			   struct v4l2_buffer *buf)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-
-	return vb2_dqbuf(&fimc->vid_cap.vbq, buf, file->f_flags & O_NONBLOCK);
-}
-
-static int fimc_cap_create_bufs(struct file *file, void *priv,
-				struct v4l2_create_buffers *create)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
 
-	return vb2_create_bufs(&fimc->vid_cap.vbq, create);
-}
-
-static int fimc_cap_prepare_buf(struct file *file, void *priv,
-				struct v4l2_buffer *b)
-{
-	struct fimc_dev *fimc = video_drvdata(file);
-
-	return vb2_prepare_buf(&fimc->vid_cap.vbq, b);
+	return ret;
 }
 
 static int fimc_cap_g_selection(struct file *file, void *fh,
@@ -1410,7 +1372,7 @@ static int fimc_cap_s_selection(struct file *file, void *fh,
 }
 
 static const struct v4l2_ioctl_ops fimc_capture_ioctl_ops = {
-	.vidioc_querycap		= fimc_vidioc_querycap_capture,
+	.vidioc_querycap		= fimc_cap_querycap,
 
 	.vidioc_enum_fmt_vid_cap_mplane	= fimc_cap_enum_fmt_mplane,
 	.vidioc_try_fmt_vid_cap_mplane	= fimc_cap_try_fmt_mplane,
@@ -1418,14 +1380,12 @@ static const struct v4l2_ioctl_ops fimc_capture_ioctl_ops = {
 	.vidioc_g_fmt_vid_cap_mplane	= fimc_cap_g_fmt_mplane,
 
 	.vidioc_reqbufs			= fimc_cap_reqbufs,
-	.vidioc_querybuf		= fimc_cap_querybuf,
-
-	.vidioc_qbuf			= fimc_cap_qbuf,
-	.vidioc_dqbuf			= fimc_cap_dqbuf,
-	.vidioc_expbuf			= fimc_cap_expbuf,
-
-	.vidioc_prepare_buf		= fimc_cap_prepare_buf,
-	.vidioc_create_bufs		= fimc_cap_create_bufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
 
 	.vidioc_streamon		= fimc_cap_streamon,
 	.vidioc_streamoff		= fimc_cap_streamoff,
@@ -1487,7 +1447,7 @@ static const struct media_entity_operations fimc_sd_media_ops = {
 void fimc_sensor_notify(struct v4l2_subdev *sd, unsigned int notification,
 			void *arg)
 {
-	struct fimc_sensor_info	*sensor;
+	struct fimc_source_info	*si;
 	struct fimc_vid_buffer *buf;
 	struct fimc_md *fmd;
 	struct fimc_dev *fimc;
@@ -1496,11 +1456,12 @@ void fimc_sensor_notify(struct v4l2_subdev *sd, unsigned int notification,
 	if (sd == NULL)
 		return;
 
-	sensor = v4l2_get_subdev_hostdata(sd);
+	si = v4l2_get_subdev_hostdata(sd);
 	fmd = entity_to_fimc_mdev(&sd->entity);
 
 	spin_lock_irqsave(&fmd->slock, flags);
-	fimc = sensor ? sensor->host : NULL;
+
+	fimc = si ? source_to_sensor_info(si)->host : NULL;
 
 	if (fimc && arg && notification == S5P_FIMC_TX_END_NOTIFY &&
 	    test_bit(ST_CAPT_PEND, &fimc->state)) {
@@ -1537,25 +1498,37 @@ static int fimc_subdev_get_fmt(struct v4l2_subdev *sd,
 {
 	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
 	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
+	struct fimc_frame *ff = &ctx->s_frame;
 	struct v4l2_mbus_framefmt *mf;
-	struct fimc_frame *ff;
 
 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 		mf = v4l2_subdev_get_try_format(fh, fmt->pad);
 		fmt->format = *mf;
 		return 0;
 	}
-	mf = &fmt->format;
-	mf->colorspace = V4L2_COLORSPACE_JPEG;
-	ff = fmt->pad == FIMC_SD_PAD_SINK ? &ctx->s_frame : &ctx->d_frame;
 
+	mf = &fmt->format;
 	mutex_lock(&fimc->lock);
-	/* The pixel code is same on both input and output pad */
-	if (!WARN_ON(ctx->s_frame.fmt == NULL))
-		mf->code = ctx->s_frame.fmt->mbus_code;
-	mf->width  = ff->f_width;
-	mf->height = ff->f_height;
+
+	switch (fmt->pad) {
+	case FIMC_SD_PAD_SOURCE:
+		if (!WARN_ON(ff->fmt == NULL))
+			mf->code = ff->fmt->mbus_code;
+		/* Sink pads crop rectangle size */
+		mf->width = ff->width;
+		mf->height = ff->height;
+		break;
+	case FIMC_SD_PAD_SINK_FIFO:
+		*mf = fimc->vid_cap.wb_fmt;
+		break;
+	case FIMC_SD_PAD_SINK_CAM:
+	default:
+		*mf = fimc->vid_cap.ci_fmt;
+		break;
+	}
+
 	mutex_unlock(&fimc->lock);
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
 
 	return 0;
 }
@@ -1566,15 +1539,15 @@ static int fimc_subdev_set_fmt(struct v4l2_subdev *sd,
 {
 	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *mf = &fmt->format;
-	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct fimc_ctx *ctx = vc->ctx;
 	struct fimc_frame *ff;
 	struct fimc_fmt *ffmt;
 
 	dbg("pad%d: code: 0x%x, %dx%d",
 	    fmt->pad, mf->code, mf->width, mf->height);
 
-	if (fmt->pad == FIMC_SD_PAD_SOURCE &&
-	    vb2_is_busy(&fimc->vid_cap.vbq))
+	if (fmt->pad == FIMC_SD_PAD_SOURCE && vb2_is_busy(&vc->vbq))
 		return -EBUSY;
 
 	mutex_lock(&fimc->lock);
@@ -1596,21 +1569,32 @@ static int fimc_subdev_set_fmt(struct v4l2_subdev *sd,
 	fimc_alpha_ctrl_update(ctx);
 
 	fimc_capture_mark_jpeg_xfer(ctx, ffmt->color);
-
-	ff = fmt->pad == FIMC_SD_PAD_SINK ?
-		&ctx->s_frame : &ctx->d_frame;
+	if (fmt->pad == FIMC_SD_PAD_SOURCE) {
+		ff = &ctx->d_frame;
+		/* Sink pads crop rectangle size */
+		mf->width = ctx->s_frame.width;
+		mf->height = ctx->s_frame.height;
+	} else {
+		ff = &ctx->s_frame;
+	}
 
 	mutex_lock(&fimc->lock);
 	set_frame_bounds(ff, mf->width, mf->height);
-	fimc->vid_cap.mf = *mf;
+
+	if (fmt->pad == FIMC_SD_PAD_SINK_FIFO)
+		vc->wb_fmt = *mf;
+	else if (fmt->pad == FIMC_SD_PAD_SINK_CAM)
+		vc->ci_fmt = *mf;
+
 	ff->fmt = ffmt;
 
 	/* Reset the crop rectangle if required. */
 	if (!(fmt->pad == FIMC_SD_PAD_SOURCE && (ctx->state & FIMC_COMPOSE)))
 		set_frame_crop(ff, 0, 0, mf->width, mf->height);
 
-	if (fmt->pad == FIMC_SD_PAD_SINK)
+	if (fmt->pad != FIMC_SD_PAD_SOURCE)
 		ctx->state &= ~FIMC_COMPOSE;
+
 	mutex_unlock(&fimc->lock);
 	return 0;
 }
@@ -1625,7 +1609,7 @@ static int fimc_subdev_get_selection(struct v4l2_subdev *sd,
 	struct v4l2_rect *r = &sel->r;
 	struct v4l2_rect *try_sel;
 
-	if (sel->pad != FIMC_SD_PAD_SINK)
+	if (sel->pad == FIMC_SD_PAD_SOURCE)
 		return -EINVAL;
 
 	mutex_lock(&fimc->lock);
@@ -1681,7 +1665,7 @@ static int fimc_subdev_set_selection(struct v4l2_subdev *sd,
 	struct v4l2_rect *try_sel;
 	unsigned long flags;
 
-	if (sel->pad != FIMC_SD_PAD_SINK)
+	if (sel->pad == FIMC_SD_PAD_SOURCE)
 		return -EINVAL;
 
 	mutex_lock(&fimc->lock);
@@ -1752,9 +1736,9 @@ static int fimc_register_capture_device(struct fimc_dev *fimc,
 				 struct v4l2_device *v4l2_dev)
 {
 	struct video_device *vfd = &fimc->vid_cap.vfd;
-	struct fimc_vid_cap *vid_cap;
+	struct vb2_queue *q = &fimc->vid_cap.vbq;
 	struct fimc_ctx *ctx;
-	struct vb2_queue *q;
+	struct fimc_vid_cap *vid_cap;
 	int ret = -ENOMEM;
 
 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
@@ -1776,27 +1760,27 @@ static int fimc_register_capture_device(struct fimc_dev *fimc,
 	vfd->v4l2_dev	= v4l2_dev;
 	vfd->minor	= -1;
 	vfd->release	= video_device_release_empty;
+	vfd->queue	= q;
 	vfd->lock	= &fimc->lock;
 
 	video_set_drvdata(vfd, fimc);
-
 	vid_cap = &fimc->vid_cap;
 	vid_cap->active_buf_cnt = 0;
-	vid_cap->reqbufs_count  = 0;
-	vid_cap->refcnt = 0;
+	vid_cap->reqbufs_count = 0;
+	vid_cap->ctx = ctx;
 
 	INIT_LIST_HEAD(&vid_cap->pending_buf_q);
 	INIT_LIST_HEAD(&vid_cap->active_buf_q);
-	vid_cap->ctx = ctx;
 
-	q = &fimc->vid_cap.vbq;
 	memset(q, 0, sizeof(*q));
 	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
 	q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
-	q->drv_priv = fimc->vid_cap.ctx;
+	q->drv_priv = ctx;
 	q->ops = &fimc_capture_qops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct fimc_vid_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &fimc->lock;
 
 	ret = vb2_queue_init(q);
 	if (ret)
@@ -1882,10 +1866,11 @@ int fimc_initialize_capture_subdev(struct fimc_dev *fimc)
 	int ret;
 
 	v4l2_subdev_init(sd, &fimc_subdev_ops);
-	sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
-	snprintf(sd->name, sizeof(sd->name), "FIMC.%d", fimc->pdev->id);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, sizeof(sd->name), "FIMC.%d", fimc->id);
 
-	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK_CAM].flags = MEDIA_PAD_FL_SINK;
+	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK_FIFO].flags = MEDIA_PAD_FL_SINK;
 	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
 	ret = media_entity_init(&sd->entity, FIMC_SD_PADS_NUM,
 				fimc->vid_cap.sd_pads, 0);
diff --git a/drivers/media/platform/s5p-fimc/fimc-core.c b/drivers/media/platform/exynos4-is/fimc-core.c
index 0f513dd19f86..379a5e9d52a7 100644
--- a/drivers/media/platform/s5p-fimc/fimc-core.c
+++ b/drivers/media/platform/exynos4-is/fimc-core.c
@@ -20,7 +20,10 @@
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/list.h>
+#include <linux/mfd/syscon.h>
 #include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
 #include <media/v4l2-ioctl.h>
@@ -29,7 +32,7 @@
 
 #include "fimc-core.h"
 #include "fimc-reg.h"
-#include "fimc-mdevice.h"
+#include "media-dev.h"
 
 static char *fimc_clocks[MAX_FIMC_CLOCKS] = {
 	"sclk_fimc", "fimc"
@@ -77,6 +80,10 @@ static struct fimc_fmt fimc_formats[] = {
 		.colplanes	= 1,
 		.flags		= FMT_FLAGS_M2M_OUT | FMT_HAS_ALPHA,
 	}, {
+		.name		= "YUV 4:4:4",
+		.mbus_code	= V4L2_MBUS_FMT_YUV10_1X30,
+		.flags		= FMT_FLAGS_WRITEBACK,
+	}, {
 		.name		= "YUV 4:2:2 packed, YCbYCr",
 		.fourcc		= V4L2_PIX_FMT_YUYV,
 		.depth		= { 16 },
@@ -206,6 +213,17 @@ struct fimc_fmt *fimc_get_format(unsigned int index)
 	return &fimc_formats[index];
 }
 
+void __fimc_vidioc_querycap(struct device *dev, struct v4l2_capability *cap,
+						unsigned int caps)
+{
+	strlcpy(cap->driver, dev->driver->name, sizeof(cap->driver));
+	strlcpy(cap->card, dev->driver->name, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+				"platform:%s", dev_name(dev));
+	cap->device_caps = caps;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+}
+
 int fimc_check_scaler_ratio(struct fimc_ctx *ctx, int sw, int sh,
 			    int dw, int dh, int rotation)
 {
@@ -405,34 +423,34 @@ void fimc_set_yuv_order(struct fimc_ctx *ctx)
 	/* Set order for 1 plane input formats. */
 	switch (ctx->s_frame.fmt->color) {
 	case FIMC_FMT_YCRYCB422:
-		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_CBYCRY;
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_YCRYCB;
 		break;
 	case FIMC_FMT_CBYCRY422:
-		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_YCRYCB;
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_CBYCRY;
 		break;
 	case FIMC_FMT_CRYCBY422:
-		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_YCBYCR;
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_CRYCBY;
 		break;
 	case FIMC_FMT_YCBYCR422:
 	default:
-		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_CRYCBY;
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_YCBYCR;
 		break;
 	}
 	dbg("ctx->in_order_1p= %d", ctx->in_order_1p);
 
 	switch (ctx->d_frame.fmt->color) {
 	case FIMC_FMT_YCRYCB422:
-		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_CBYCRY;
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_YCRYCB;
 		break;
 	case FIMC_FMT_CBYCRY422:
-		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_YCRYCB;
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_CBYCRY;
 		break;
 	case FIMC_FMT_CRYCBY422:
-		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_YCBYCR;
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_CRYCBY;
 		break;
 	case FIMC_FMT_YCBYCR422:
 	default:
-		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_CRYCBY;
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_YCBYCR;
 		break;
 	}
 	dbg("ctx->out_order_1p= %d", ctx->out_order_1p);
@@ -440,14 +458,14 @@ void fimc_set_yuv_order(struct fimc_ctx *ctx)
 
 void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 {
-	const struct fimc_variant *variant = ctx->fimc_dev->variant;
+	bool pix_hoff = ctx->fimc_dev->drv_data->dma_pix_hoff;
 	u32 i, depth = 0;
 
 	for (i = 0; i < f->fmt->colplanes; i++)
 		depth += f->fmt->depth[i];
 
 	f->dma_offset.y_h = f->offs_h;
-	if (!variant->pix_hoff)
+	if (!pix_hoff)
 		f->dma_offset.y_h *= (depth >> 3);
 
 	f->dma_offset.y_v = f->offs_v;
@@ -458,7 +476,7 @@ void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 	f->dma_offset.cr_h = f->offs_h;
 	f->dma_offset.cr_v = f->offs_v;
 
-	if (!variant->pix_hoff) {
+	if (!pix_hoff) {
 		if (f->fmt->colplanes == 3) {
 			f->dma_offset.cb_h >>= 1;
 			f->dma_offset.cr_h >>= 1;
@@ -589,7 +607,6 @@ static const struct v4l2_ctrl_ops fimc_ctrl_ops = {
 
 int fimc_ctrls_create(struct fimc_ctx *ctx)
 {
-	const struct fimc_variant *variant = ctx->fimc_dev->variant;
 	unsigned int max_alpha = fimc_get_alpha_mask(ctx->d_frame.fmt);
 	struct fimc_ctrls *ctrls = &ctx->ctrls;
 	struct v4l2_ctrl_handler *handler = &ctrls->handler;
@@ -606,7 +623,7 @@ int fimc_ctrls_create(struct fimc_ctx *ctx)
 	ctrls->vflip = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
 					V4L2_CID_VFLIP, 0, 1, 1, 0);
 
-	if (variant->has_alpha)
+	if (ctx->fimc_dev->drv_data->alpha_color)
 		ctrls->alpha = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
 					V4L2_CID_ALPHA_COMPONENT,
 					0, max_alpha, 1, 0);
@@ -677,7 +694,7 @@ void fimc_alpha_ctrl_update(struct fimc_ctx *ctx)
 	struct fimc_dev *fimc = ctx->fimc_dev;
 	struct v4l2_ctrl *ctrl = ctx->ctrls.alpha;
 
-	if (ctrl == NULL || !fimc->variant->has_alpha)
+	if (ctrl == NULL || !fimc->drv_data->alpha_color)
 		return;
 
 	v4l2_ctrl_lock(ctrl);
@@ -865,43 +882,113 @@ static int fimc_m2m_resume(struct fimc_dev *fimc)
 	return 0;
 }
 
+static const struct of_device_id fimc_of_match[];
+
+static int fimc_parse_dt(struct fimc_dev *fimc, u32 *clk_freq)
+{
+	struct device *dev = &fimc->pdev->dev;
+	struct device_node *node = dev->of_node;
+	const struct of_device_id *of_id;
+	struct fimc_variant *v;
+	struct fimc_pix_limit *lim;
+	u32 args[FIMC_PIX_LIMITS_MAX];
+	int ret;
+
+	if (of_property_read_bool(node, "samsung,lcd-wb"))
+		return -ENODEV;
+
+	v = devm_kzalloc(dev, sizeof(*v) + sizeof(*lim), GFP_KERNEL);
+	if (!v)
+		return -ENOMEM;
+
+	of_id = of_match_node(fimc_of_match, node);
+	if (!of_id)
+		return -EINVAL;
+	fimc->drv_data = of_id->data;
+	ret = of_property_read_u32_array(node, "samsung,pix-limits",
+					 args, FIMC_PIX_LIMITS_MAX);
+	if (ret < 0)
+		return ret;
+
+	lim = (struct fimc_pix_limit *)&v[1];
+
+	lim->scaler_en_w = args[0];
+	lim->scaler_dis_w = args[1];
+	lim->out_rot_en_w = args[2];
+	lim->out_rot_dis_w = args[3];
+	v->pix_limit = lim;
+
+	ret = of_property_read_u32_array(node, "samsung,min-pix-sizes",
+								args, 2);
+	v->min_inp_pixsize = ret ? FIMC_DEF_MIN_SIZE : args[0];
+	v->min_out_pixsize = ret ? FIMC_DEF_MIN_SIZE : args[1];
+	ret = of_property_read_u32_array(node, "samsung,min-pix-alignment",
+								args, 2);
+	v->min_vsize_align = ret ? FIMC_DEF_HEIGHT_ALIGN : args[0];
+	v->hor_offs_align = ret ? FIMC_DEF_HOR_OFFS_ALIGN : args[1];
+
+	ret = of_property_read_u32(node, "samsung,rotators", &args[1]);
+	v->has_inp_rot = ret ? 1 : args[1] & 0x01;
+	v->has_out_rot = ret ? 1 : args[1] & 0x10;
+	v->has_mainscaler_ext = of_property_read_bool(node,
+					"samsung,mainscaler-ext");
+
+	v->has_isp_wb = of_property_read_bool(node, "samsung,isp-wb");
+	v->has_cam_if = of_property_read_bool(node, "samsung,cam-if");
+	of_property_read_u32(node, "clock-frequency", clk_freq);
+	fimc->id = of_alias_get_id(node, "fimc");
+
+	fimc->variant = v;
+	return 0;
+}
+
 static int fimc_probe(struct platform_device *pdev)
 {
-	const struct fimc_drvdata *drv_data = fimc_get_drvdata(pdev);
-	struct s5p_platform_fimc *pdata;
+	struct device *dev = &pdev->dev;
+	u32 lclk_freq = 0;
 	struct fimc_dev *fimc;
 	struct resource *res;
 	int ret = 0;
 
-	if (pdev->id >= drv_data->num_entities) {
-		dev_err(&pdev->dev, "Invalid platform device id: %d\n",
-			pdev->id);
-		return -EINVAL;
-	}
-
-	fimc = devm_kzalloc(&pdev->dev, sizeof(*fimc), GFP_KERNEL);
+	fimc = devm_kzalloc(dev, sizeof(*fimc), GFP_KERNEL);
 	if (!fimc)
 		return -ENOMEM;
 
-	fimc->id = pdev->id;
-
-	fimc->variant = drv_data->variant[fimc->id];
 	fimc->pdev = pdev;
-	pdata = pdev->dev.platform_data;
-	fimc->pdata = pdata;
+
+	if (dev->of_node) {
+		ret = fimc_parse_dt(fimc, &lclk_freq);
+		if (ret < 0)
+			return ret;
+	} else {
+		fimc->drv_data = fimc_get_drvdata(pdev);
+		fimc->id = pdev->id;
+	}
+	if (!fimc->drv_data || fimc->id >= fimc->drv_data->num_entities ||
+	    fimc->id < 0) {
+		dev_err(dev, "Invalid driver data or device id (%d)\n",
+			fimc->id);
+		return -EINVAL;
+	}
+	if (!dev->of_node)
+		fimc->variant = fimc->drv_data->variant[fimc->id];
 
 	init_waitqueue_head(&fimc->irq_queue);
 	spin_lock_init(&fimc->slock);
 	mutex_init(&fimc->lock);
 
+	fimc->sysreg = fimc_get_sysreg_regmap(dev->of_node);
+	if (IS_ERR(fimc->sysreg))
+		return PTR_ERR(fimc->sysreg);
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	fimc->regs = devm_ioremap_resource(&pdev->dev, res);
+	fimc->regs = devm_ioremap_resource(dev, res);
 	if (IS_ERR(fimc->regs))
 		return PTR_ERR(fimc->regs);
 
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (res == NULL) {
-		dev_err(&pdev->dev, "Failed to get IRQ resource\n");
+		dev_err(dev, "Failed to get IRQ resource\n");
 		return -ENXIO;
 	}
 
@@ -909,7 +996,10 @@ static int fimc_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	ret = clk_set_rate(fimc->clock[CLK_BUS], drv_data->lclk_frequency);
+	if (lclk_freq == 0)
+		lclk_freq = fimc->drv_data->lclk_frequency;
+
+	ret = clk_set_rate(fimc->clock[CLK_BUS], lclk_freq);
 	if (ret < 0)
 		return ret;
 
@@ -917,10 +1007,10 @@ static int fimc_probe(struct platform_device *pdev)
 	if (ret < 0)
 		return ret;
 
-	ret = devm_request_irq(&pdev->dev, res->start, fimc_irq_handler,
-			       0, dev_name(&pdev->dev), fimc);
+	ret = devm_request_irq(dev, res->start, fimc_irq_handler,
+			       0, dev_name(dev), fimc);
 	if (ret) {
-		dev_err(&pdev->dev, "failed to install irq (%d)\n", ret);
+		dev_err(dev, "failed to install irq (%d)\n", ret);
 		goto err_clk;
 	}
 
@@ -929,23 +1019,23 @@ static int fimc_probe(struct platform_device *pdev)
 		goto err_clk;
 
 	platform_set_drvdata(pdev, fimc);
-	pm_runtime_enable(&pdev->dev);
-	ret = pm_runtime_get_sync(&pdev->dev);
+	pm_runtime_enable(dev);
+	ret = pm_runtime_get_sync(dev);
 	if (ret < 0)
 		goto err_sd;
 	/* Initialize contiguous memory allocator */
-	fimc->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	fimc->alloc_ctx = vb2_dma_contig_init_ctx(dev);
 	if (IS_ERR(fimc->alloc_ctx)) {
 		ret = PTR_ERR(fimc->alloc_ctx);
 		goto err_pm;
 	}
 
-	dev_dbg(&pdev->dev, "FIMC.%d registered successfully\n", fimc->id);
+	dev_dbg(dev, "FIMC.%d registered successfully\n", fimc->id);
 
-	pm_runtime_put(&pdev->dev);
+	pm_runtime_put(dev);
 	return 0;
 err_pm:
-	pm_runtime_put(&pdev->dev);
+	pm_runtime_put(dev);
 err_sd:
 	fimc_unregister_capture_subdev(fimc);
 err_clk:
@@ -1048,35 +1138,21 @@ static const struct fimc_pix_limit s5p_pix_limit[4] = {
 	[0] = {
 		.scaler_en_w	= 3264,
 		.scaler_dis_w	= 8192,
-		.in_rot_en_h	= 1920,
-		.in_rot_dis_w	= 8192,
 		.out_rot_en_w	= 1920,
 		.out_rot_dis_w	= 4224,
 	},
 	[1] = {
 		.scaler_en_w	= 4224,
 		.scaler_dis_w	= 8192,
-		.in_rot_en_h	= 1920,
-		.in_rot_dis_w	= 8192,
 		.out_rot_en_w	= 1920,
 		.out_rot_dis_w	= 4224,
 	},
 	[2] = {
 		.scaler_en_w	= 1920,
 		.scaler_dis_w	= 8192,
-		.in_rot_en_h	= 1280,
-		.in_rot_dis_w	= 8192,
 		.out_rot_en_w	= 1280,
 		.out_rot_dis_w	= 1920,
 	},
-	[3] = {
-		.scaler_en_w	= 1920,
-		.scaler_dis_w	= 8192,
-		.in_rot_en_h	= 1366,
-		.in_rot_dis_w	= 8192,
-		.out_rot_en_w	= 1366,
-		.out_rot_dis_w	= 1920,
-	},
 };
 
 static const struct fimc_variant fimc0_variant_s5p = {
@@ -1087,7 +1163,6 @@ static const struct fimc_variant fimc0_variant_s5p = {
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 8,
 	.min_vsize_align = 16,
-	.out_buf_count	 = 4,
 	.pix_limit	 = &s5p_pix_limit[0],
 };
 
@@ -1097,12 +1172,10 @@ static const struct fimc_variant fimc2_variant_s5p = {
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 8,
 	.min_vsize_align = 16,
-	.out_buf_count	 = 4,
 	.pix_limit	 = &s5p_pix_limit[1],
 };
 
 static const struct fimc_variant fimc0_variant_s5pv210 = {
-	.pix_hoff	 = 1,
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
 	.has_cam_if	 = 1,
@@ -1110,12 +1183,10 @@ static const struct fimc_variant fimc0_variant_s5pv210 = {
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 8,
 	.min_vsize_align = 16,
-	.out_buf_count	 = 4,
 	.pix_limit	 = &s5p_pix_limit[1],
 };
 
 static const struct fimc_variant fimc1_variant_s5pv210 = {
-	.pix_hoff	 = 1,
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
 	.has_cam_if	 = 1,
@@ -1124,80 +1195,18 @@ static const struct fimc_variant fimc1_variant_s5pv210 = {
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 1,
 	.min_vsize_align = 1,
-	.out_buf_count	 = 4,
 	.pix_limit	 = &s5p_pix_limit[2],
 };
 
 static const struct fimc_variant fimc2_variant_s5pv210 = {
 	.has_cam_if	 = 1,
-	.pix_hoff	 = 1,
 	.min_inp_pixsize = 16,
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 8,
 	.min_vsize_align = 16,
-	.out_buf_count	 = 4,
 	.pix_limit	 = &s5p_pix_limit[2],
 };
 
-static const struct fimc_variant fimc0_variant_exynos4210 = {
-	.pix_hoff	 = 1,
-	.has_inp_rot	 = 1,
-	.has_out_rot	 = 1,
-	.has_cam_if	 = 1,
-	.has_cistatus2	 = 1,
-	.has_mainscaler_ext = 1,
-	.has_alpha	 = 1,
-	.min_inp_pixsize = 16,
-	.min_out_pixsize = 16,
-	.hor_offs_align	 = 2,
-	.min_vsize_align = 1,
-	.out_buf_count	 = 32,
-	.pix_limit	 = &s5p_pix_limit[1],
-};
-
-static const struct fimc_variant fimc3_variant_exynos4210 = {
-	.pix_hoff	 = 1,
-	.has_cistatus2	 = 1,
-	.has_mainscaler_ext = 1,
-	.has_alpha	 = 1,
-	.min_inp_pixsize = 16,
-	.min_out_pixsize = 16,
-	.hor_offs_align	 = 2,
-	.min_vsize_align = 1,
-	.out_buf_count	 = 32,
-	.pix_limit	 = &s5p_pix_limit[3],
-};
-
-static const struct fimc_variant fimc0_variant_exynos4x12 = {
-	.pix_hoff		= 1,
-	.has_inp_rot		= 1,
-	.has_out_rot		= 1,
-	.has_cam_if		= 1,
-	.has_isp_wb		= 1,
-	.has_cistatus2		= 1,
-	.has_mainscaler_ext	= 1,
-	.has_alpha		= 1,
-	.min_inp_pixsize	= 16,
-	.min_out_pixsize	= 16,
-	.hor_offs_align		= 2,
-	.min_vsize_align	= 1,
-	.out_buf_count		= 32,
-	.pix_limit		= &s5p_pix_limit[1],
-};
-
-static const struct fimc_variant fimc3_variant_exynos4x12 = {
-	.pix_hoff		= 1,
-	.has_cistatus2		= 1,
-	.has_mainscaler_ext	= 1,
-	.has_alpha		= 1,
-	.min_inp_pixsize	= 16,
-	.min_out_pixsize	= 16,
-	.hor_offs_align		= 2,
-	.min_vsize_align	= 1,
-	.out_buf_count		= 32,
-	.pix_limit		= &s5p_pix_limit[3],
-};
-
 /* S5PC100 */
 static const struct fimc_drvdata fimc_drvdata_s5p = {
 	.variant = {
@@ -1205,8 +1214,9 @@ static const struct fimc_drvdata fimc_drvdata_s5p = {
 		[1] = &fimc0_variant_s5p,
 		[2] = &fimc2_variant_s5p,
 	},
-	.num_entities = 3,
+	.num_entities	= 3,
 	.lclk_frequency = 133000000UL,
+	.out_buf_count	= 4,
 };
 
 /* S5PV210, S5PC110 */
@@ -1216,32 +1226,30 @@ static const struct fimc_drvdata fimc_drvdata_s5pv210 = {
 		[1] = &fimc1_variant_s5pv210,
 		[2] = &fimc2_variant_s5pv210,
 	},
-	.num_entities = 3,
-	.lclk_frequency = 166000000UL,
+	.num_entities	= 3,
+	.lclk_frequency	= 166000000UL,
+	.out_buf_count	= 4,
+	.dma_pix_hoff	= 1,
 };
 
 /* EXYNOS4210, S5PV310, S5PC210 */
 static const struct fimc_drvdata fimc_drvdata_exynos4210 = {
-	.variant = {
-		[0] = &fimc0_variant_exynos4210,
-		[1] = &fimc0_variant_exynos4210,
-		[2] = &fimc0_variant_exynos4210,
-		[3] = &fimc3_variant_exynos4210,
-	},
-	.num_entities = 4,
+	.num_entities	= 4,
 	.lclk_frequency = 166000000UL,
+	.dma_pix_hoff	= 1,
+	.cistatus2	= 1,
+	.alpha_color	= 1,
+	.out_buf_count	= 32,
 };
 
 /* EXYNOS4212, EXYNOS4412 */
 static const struct fimc_drvdata fimc_drvdata_exynos4x12 = {
-	.variant = {
-		[0] = &fimc0_variant_exynos4x12,
-		[1] = &fimc0_variant_exynos4x12,
-		[2] = &fimc0_variant_exynos4x12,
-		[3] = &fimc3_variant_exynos4x12,
-	},
-	.num_entities = 4,
-	.lclk_frequency = 166000000UL,
+	.num_entities	= 4,
+	.lclk_frequency	= 166000000UL,
+	.dma_pix_hoff	= 1,
+	.cistatus2	= 1,
+	.alpha_color	= 1,
+	.out_buf_count	= 32,
 };
 
 static const struct platform_device_id fimc_driver_ids[] = {
@@ -1258,9 +1266,22 @@ static const struct platform_device_id fimc_driver_ids[] = {
 		.name		= "exynos4x12-fimc",
 		.driver_data	= (unsigned long)&fimc_drvdata_exynos4x12,
 	},
-	{},
+	{ },
+};
+
+static const struct of_device_id fimc_of_match[] = {
+	{
+		.compatible = "samsung,s5pv210-fimc",
+		.data = &fimc_drvdata_s5pv210,
+	}, {
+		.compatible = "samsung,exynos4210-fimc",
+		.data = &fimc_drvdata_exynos4210,
+	}, {
+		.compatible = "samsung,exynos4212-fimc",
+		.data = &fimc_drvdata_exynos4x12,
+	},
+	{ /* sentinel */ },
 };
-MODULE_DEVICE_TABLE(platform, fimc_driver_ids);
 
 static const struct dev_pm_ops fimc_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(fimc_suspend, fimc_resume)
@@ -1272,9 +1293,10 @@ static struct platform_driver fimc_driver = {
 	.remove		= fimc_remove,
 	.id_table	= fimc_driver_ids,
 	.driver = {
-		.name	= FIMC_MODULE_NAME,
-		.owner	= THIS_MODULE,
-		.pm     = &fimc_pm_ops,
+		.of_match_table = fimc_of_match,
+		.name		= FIMC_DRIVER_NAME,
+		.owner		= THIS_MODULE,
+		.pm     	= &fimc_pm_ops,
 	}
 };
 
diff --git a/drivers/media/platform/s5p-fimc/fimc-core.h b/drivers/media/platform/exynos4-is/fimc-core.h
index 412d50708f75..539a3f71c16a 100644
--- a/drivers/media/platform/s5p-fimc/fimc-core.h
+++ b/drivers/media/platform/exynos4-is/fimc-core.h
@@ -12,8 +12,10 @@
 /*#define DEBUG*/
 
 #include <linux/platform_device.h>
+#include <linux/regmap.h>
 #include <linux/sched.h>
 #include <linux/spinlock.h>
+#include <linux/mfd/syscon.h>
 #include <linux/types.h>
 #include <linux/videodev2.h>
 #include <linux/io.h>
@@ -33,7 +35,7 @@
 /* Time to wait for next frame VSYNC interrupt while stopping operation. */
 #define FIMC_SHUTDOWN_TIMEOUT	((100*HZ)/1000)
 #define MAX_FIMC_CLOCKS		2
-#define FIMC_MODULE_NAME	"s5p-fimc"
+#define FIMC_DRIVER_NAME	"exynos4-fimc"
 #define FIMC_MAX_DEVS		4
 #define FIMC_MAX_OUT_BUFS	4
 #define SCALER_MAX_HRATIO	64
@@ -42,6 +44,10 @@
 #define FIMC_CAMIF_MAX_HEIGHT	0x2000
 #define FIMC_MAX_JPEG_BUF_SIZE	(10 * SZ_1M)
 #define FIMC_MAX_PLANES		3
+#define FIMC_PIX_LIMITS_MAX	4
+#define FIMC_DEF_MIN_SIZE	16
+#define FIMC_DEF_HEIGHT_ALIGN	2
+#define FIMC_DEF_HOR_OFFS_ALIGN	1
 
 /* indices to the clocks array */
 enum {
@@ -132,36 +138,6 @@ enum fimc_color_fmt {
 #define	FIMC_COLOR_RANGE_NARROW		(1 << 3)
 
 /**
- * struct fimc_fmt - the driver's internal color format data
- * @mbus_code: Media Bus pixel code, -1 if not applicable
- * @name: format description
- * @fourcc: the fourcc code for this format, 0 if not applicable
- * @color: the corresponding 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 {
-	enum v4l2_mbus_pixelcode mbus_code;
-	char	*name;
-	u32	fourcc;
-	u32	color;
-	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)
-};
-
-/**
  * struct fimc_dma_offset - pixel offset information for DMA
  * @y_h:	y value horizontal offset
  * @y_v:	y value vertical offset
@@ -299,9 +275,10 @@ struct fimc_m2m_device {
 	int			refcnt;
 };
 
-#define FIMC_SD_PAD_SINK	0
-#define FIMC_SD_PAD_SOURCE	1
-#define FIMC_SD_PADS_NUM	2
+#define FIMC_SD_PAD_SINK_CAM	0
+#define FIMC_SD_PAD_SINK_FIFO	1
+#define FIMC_SD_PAD_SOURCE	2
+#define FIMC_SD_PADS_NUM	3
 
 /**
  * struct fimc_vid_cap - camera capture device information
@@ -310,7 +287,9 @@ struct fimc_m2m_device {
  * @subdev: subdev exposing the FIMC processing block
  * @vd_pad: fimc video capture node pad
  * @sd_pads: fimc video processing block pads
- * @mf: media bus format at the FIMC camera input (and the scaler output) pad
+ * @ci_fmt: image format at the FIMC camera input (and the scaler output)
+ * @wb_fmt: image format at the FIMC ISP Writeback input
+ * @source_config: external image source related configuration structure
  * @pending_buf_q: the pending buffer queue head
  * @active_buf_q: the queue head of buffers scheduled in hardware
  * @vbq: the capture am video buffer queue
@@ -329,8 +308,10 @@ struct fimc_vid_cap {
 	struct video_device		vfd;
 	struct v4l2_subdev		subdev;
 	struct media_pad		vd_pad;
-	struct v4l2_mbus_framefmt	mf;
 	struct media_pad		sd_pads[FIMC_SD_PADS_NUM];
+	struct v4l2_mbus_framefmt	ci_fmt;
+	struct v4l2_mbus_framefmt	wb_fmt;
+	struct fimc_source_info		source_config;
 	struct list_head		pending_buf_q;
 	struct list_head		active_buf_q;
 	struct vb2_queue		vbq;
@@ -338,6 +319,7 @@ struct fimc_vid_cap {
 	int				buf_index;
 	unsigned int			frame_count;
 	unsigned int			reqbufs_count;
+	bool				streaming;
 	int				input_index;
 	int				refcnt;
 	u32				input;
@@ -365,10 +347,8 @@ struct fimc_pix_limit {
 
 /**
  * struct fimc_variant - FIMC device variant information
- * @pix_hoff: indicate whether horizontal offset is in pixels or in bytes
  * @has_inp_rot: set if has input rotator
  * @has_out_rot: set if has output rotator
- * @has_cistatus2: 1 if CISTATUS2 register is present in this IP revision
  * @has_mainscaler_ext: 1 if extended mainscaler ratios in CIEXTEN register
  *			 are present in this IP revision
  * @has_cam_if: set if this instance has a camera input interface
@@ -378,23 +358,18 @@ struct fimc_pix_limit {
  * @min_out_pixsize: minimum output pixel size
  * @hor_offs_align: horizontal pixel offset aligment
  * @min_vsize_align: minimum vertical pixel size alignment
- * @out_buf_count: the number of buffers in output DMA sequence
  */
 struct fimc_variant {
-	unsigned int	pix_hoff:1;
 	unsigned int	has_inp_rot:1;
 	unsigned int	has_out_rot:1;
-	unsigned int	has_cistatus2:1;
 	unsigned int	has_mainscaler_ext:1;
 	unsigned int	has_cam_if:1;
 	unsigned int	has_isp_wb:1;
-	unsigned int	has_alpha:1;
 	const struct fimc_pix_limit *pix_limit;
 	u16		min_inp_pixsize;
 	u16		min_out_pixsize;
 	u16		hor_offs_align;
 	u16		min_vsize_align;
-	u16		out_buf_count;
 };
 
 /**
@@ -402,11 +377,20 @@ struct fimc_variant {
  * @variant: variant information for this device
  * @num_entities: number of fimc instances available in a SoC
  * @lclk_frequency: local bus clock frequency
+ * @cistatus2: 1 if the FIMC IPs have CISTATUS2 register
+ * @dma_pix_hoff: the horizontal DMA offset unit: 1 - pixels, 0 - bytes
+ * @alpha_color: 1 if alpha color component is supported
+ * @out_buf_count: maximum number of output DMA buffers supported
  */
 struct fimc_drvdata {
 	const struct fimc_variant *variant[FIMC_MAX_DEVS];
 	int num_entities;
 	unsigned long lclk_frequency;
+	/* Fields common to all FIMC IP instances */
+	u8 cistatus2;
+	u8 dma_pix_hoff;
+	u8 alpha_color;
+	u8 out_buf_count;
 };
 
 #define fimc_get_drvdata(_pdev) \
@@ -420,6 +404,7 @@ struct fimc_ctx;
  * @lock:	the mutex protecting this data structure
  * @pdev:	pointer to the FIMC platform device
  * @pdata:	pointer to the device platform data
+ * @sysreg:	pointer to the SYSREG regmap
  * @variant:	the IP variant information
  * @id:		FIMC device index (0..FIMC_MAX_DEVS)
  * @clock:	clocks required for FIMC operation
@@ -437,8 +422,10 @@ struct fimc_dev {
 	struct mutex			lock;
 	struct platform_device		*pdev;
 	struct s5p_platform_fimc	*pdata;
+	struct regmap			*sysreg;
 	const struct fimc_variant	*variant;
-	u16				id;
+	const struct fimc_drvdata	*drv_data;
+	int				id;
 	struct clk			*clock[MAX_FIMC_CLOCKS];
 	void __iomem			*regs;
 	wait_queue_head_t		irq_queue;
@@ -633,6 +620,8 @@ static inline struct fimc_frame *ctx_get_frame(struct fimc_ctx *ctx,
 /* fimc-core.c */
 int fimc_vidioc_enum_fmt_mplane(struct file *file, void *priv,
 				struct v4l2_fmtdesc *f);
+void __fimc_vidioc_querycap(struct device *dev, struct v4l2_capability *cap,
+						unsigned int caps);
 int fimc_ctrls_create(struct fimc_ctx *ctx);
 void fimc_ctrls_delete(struct fimc_ctx *ctx);
 void fimc_ctrls_activate(struct fimc_ctx *ctx, bool active);
@@ -660,6 +649,15 @@ void fimc_unregister_m2m_device(struct fimc_dev *fimc);
 int fimc_register_driver(void);
 void fimc_unregister_driver(void);
 
+#ifdef CONFIG_MFD_SYSCON
+static inline struct regmap * fimc_get_sysreg_regmap(struct device_node *node)
+{
+	return syscon_regmap_lookup_by_phandle(node, "samsung,sysreg");
+}
+#else
+#define fimc_get_sysreg_regmap(node) (NULL)
+#endif
+
 /* -----------------------------------------------------*/
 /* fimc-m2m.c */
 void fimc_m2m_job_finish(struct fimc_ctx *ctx, int vb_state);
diff --git a/drivers/media/platform/exynos4-is/fimc-is-command.h b/drivers/media/platform/exynos4-is/fimc-is-command.h
new file mode 100644
index 000000000000..0d1f52e394b1
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-command.h
@@ -0,0 +1,137 @@
+/*
+ * Samsung Exynos4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * FIMC-IS command set definitions
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@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
+ * published by the Free Software Foundation.
+ */
+
+#ifndef FIMC_IS_CMD_H_
+#define FIMC_IS_CMD_H_
+
+#define FIMC_IS_COMMAND_VER	110 /* FIMC-IS command set version 1.10 */
+
+/* Enumeration of commands beetween the FIMC-IS and the host processor. */
+
+/* HOST to FIMC-IS */
+#define HIC_PREVIEW_STILL	0x0001
+#define HIC_PREVIEW_VIDEO	0x0002
+#define HIC_CAPTURE_STILL	0x0003
+#define HIC_CAPTURE_VIDEO	0x0004
+#define HIC_STREAM_ON		0x0005
+#define HIC_STREAM_OFF		0x0006
+#define HIC_SET_PARAMETER	0x0007
+#define HIC_GET_PARAMETER	0x0008
+#define HIC_SET_TUNE		0x0009
+#define HIC_GET_STATUS		0x000b
+/* Sensor part */
+#define HIC_OPEN_SENSOR		0x000c
+#define HIC_CLOSE_SENSOR	0x000d
+#define HIC_SIMMIAN_INIT	0x000e
+#define HIC_SIMMIAN_WRITE	0x000f
+#define HIC_SIMMIAN_READ	0x0010
+#define HIC_POWER_DOWN		0x0011
+#define HIC_GET_SET_FILE_ADDR	0x0012
+#define HIC_LOAD_SET_FILE	0x0013
+#define HIC_MSG_CONFIG		0x0014
+#define HIC_MSG_TEST		0x0015
+/* FIMC-IS to HOST */
+#define IHC_GET_SENSOR_NUM	0x1000
+#define IHC_SET_SHOT_MARK	0x1001
+/* parameter1: frame number */
+/* parameter2: confidence level (smile 0~100) */
+/* parameter3: confidence level (blink 0~100) */
+#define IHC_SET_FACE_MARK	0x1002
+/* parameter1: coordinate count */
+/* parameter2: coordinate buffer address */
+#define IHC_FRAME_DONE		0x1003
+/* parameter1: frame start number */
+/* parameter2: frame count */
+#define IHC_AA_DONE		0x1004
+#define IHC_NOT_READY		0x1005
+
+#define IH_REPLY_DONE		0x2000
+#define IH_REPLY_NOT_DONE	0x2001
+
+enum fimc_is_scenario {
+	IS_SC_PREVIEW_STILL,
+	IS_SC_PREVIEW_VIDEO,
+	IS_SC_CAPTURE_STILL,
+	IS_SC_CAPTURE_VIDEO,
+	IS_SC_MAX
+};
+
+enum fimc_is_sub_scenario {
+	IS_SC_SUB_DEFAULT,
+	IS_SC_SUB_PS_VTCALL,
+	IS_SC_SUB_CS_VTCALL,
+	IS_SC_SUB_PV_VTCALL,
+	IS_SC_SUB_CV_VTCALL,
+};
+
+struct is_common_regs {
+	u32 hicmd;
+	u32 hic_sensorid;
+	u32 hic_param[4];
+	u32 reserved1[4];
+
+	u32 ihcmd;
+	u32 ihc_sensorid;
+	u32 ihc_param[4];
+	u32 reserved2[4];
+
+	u32 isp_sensor_id;
+	u32 isp_param[2];
+	u32 reserved3[1];
+
+	u32 scc_sensor_id;
+	u32 scc_param[2];
+	u32 reserved4[1];
+
+	u32 dnr_sensor_id;
+	u32 dnr_param[2];
+	u32 reserved5[1];
+
+	u32 scp_sensor_id;
+	u32 scp_param[2];
+	u32 reserved6[29];
+} __packed;
+
+struct is_mcuctl_reg {
+	u32 mcuctl;
+	u32 bboar;
+
+	u32 intgr0;
+	u32 intcr0;
+	u32 intmr0;
+	u32 intsr0;
+	u32 intmsr0;
+
+	u32 intgr1;
+	u32 intcr1;
+	u32 intmr1;
+	u32 intsr1;
+	u32 intmsr1;
+
+	u32 intcr2;
+	u32 intmr2;
+	u32 intsr2;
+	u32 intmsr2;
+
+	u32 gpoctrl;
+	u32 cpoenctlr;
+	u32 gpictlr;
+
+	u32 reserved[0xd];
+
+	struct is_common_regs common;
+} __packed;
+
+#endif /* FIMC_IS_CMD_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-is-errno.c b/drivers/media/platform/exynos4-is/fimc-is-errno.c
new file mode 100644
index 000000000000..e8519e151c1a
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-errno.c
@@ -0,0 +1,272 @@
+/*
+ * Samsung Exynos4 SoC series FIMC-IS slave interface driver
+ *
+ * Error log interface functions
+ *
+ * Copyright (C) 2011 - 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@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
+ * published by the Free Software Foundation.
+ */
+
+#include "fimc-is-errno.h"
+
+const char * const fimc_is_param_strerr(unsigned int error)
+{
+	switch (error) {
+	case ERROR_COMMON_CMD:
+		return "ERROR_COMMON_CMD: Invalid Command";
+	case ERROR_COMMON_PARAMETER:
+		return "ERROR_COMMON_PARAMETER: Invalid Parameter";
+	case ERROR_COMMON_SETFILE_LOAD:
+		return "ERROR_COMMON_SETFILE_LOAD: Illegal Setfile Loading";
+	case ERROR_COMMON_SETFILE_ADJUST:
+		return "ERROR_COMMON_SETFILE_ADJUST: Setfile isn't adjusted";
+	case ERROR_COMMON_SETFILE_INDEX:
+		return "ERROR_COMMON_SETFILE_INDEX: Invalid setfile index";
+	case ERROR_COMMON_INPUT_PATH:
+		return "ERROR_COMMON_INPUT_PATH: Input path can be changed in ready state";
+	case ERROR_COMMON_INPUT_INIT:
+		return "ERROR_COMMON_INPUT_INIT: IP can not start if input path is not set";
+	case ERROR_COMMON_OUTPUT_PATH:
+		return "ERROR_COMMON_OUTPUT_PATH: Output path can be changed in ready state (stop)";
+	case ERROR_COMMON_OUTPUT_INIT:
+		return "ERROR_COMMON_OUTPUT_INIT: IP can not start if output path is not set";
+	case ERROR_CONTROL_BYPASS:
+		return "ERROR_CONTROL_BYPASS";
+	case ERROR_OTF_INPUT_FORMAT:
+		return "ERROR_OTF_INPUT_FORMAT: Invalid format  (DRC: YUV444, FD: YUV444, 422, 420)";
+	case ERROR_OTF_INPUT_WIDTH:
+		return "ERROR_OTF_INPUT_WIDTH: Invalid width (DRC: 128~8192, FD: 32~8190)";
+	case ERROR_OTF_INPUT_HEIGHT:
+		return "ERROR_OTF_INPUT_HEIGHT: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
+	case ERROR_OTF_INPUT_BIT_WIDTH:
+		return "ERROR_OTF_INPUT_BIT_WIDTH: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
+	case ERROR_DMA_INPUT_WIDTH:
+		return "ERROR_DMA_INPUT_WIDTH: Invalid width (DRC: 128~8192, FD: 32~8190)";
+	case ERROR_DMA_INPUT_HEIGHT:
+		return "ERROR_DMA_INPUT_HEIGHT: Invalid height (DRC: 64~8192, FD: 16~8190)";
+	case ERROR_DMA_INPUT_FORMAT:
+		return "ERROR_DMA_INPUT_FORMAT: Invalid format (DRC: YUV444 or YUV422, FD: YUV444,422,420)";
+	case ERROR_DMA_INPUT_BIT_WIDTH:
+		return "ERROR_DMA_INPUT_BIT_WIDTH: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
+	case ERROR_DMA_INPUT_ORDER:
+		return "ERROR_DMA_INPUT_ORDER: Invalid order(DRC: YYCbCr,YCbYCr,FD:NO,YYCbCr,YCbYCr,CbCr,CrCb)";
+	case ERROR_DMA_INPUT_PLANE:
+		return "ERROR_DMA_INPUT_PLANE: Invalid palne (DRC: 3, FD: 1, 2, 3)";
+	case ERROR_OTF_OUTPUT_WIDTH:
+		return "ERROR_OTF_OUTPUT_WIDTH: Invalid width (DRC: 128~8192)";
+	case ERROR_OTF_OUTPUT_HEIGHT:
+		return "ERROR_OTF_OUTPUT_HEIGHT: Invalid height (DRC: 64~8192)";
+	case ERROR_OTF_OUTPUT_FORMAT:
+		return "ERROR_OTF_OUTPUT_FORMAT: Invalid format (DRC: YUV444)";
+	case ERROR_OTF_OUTPUT_BIT_WIDTH:
+		return "ERROR_OTF_OUTPUT_BIT_WIDTH: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
+	case ERROR_DMA_OUTPUT_WIDTH:
+		return "ERROR_DMA_OUTPUT_WIDTH";
+	case ERROR_DMA_OUTPUT_HEIGHT:
+		return "ERROR_DMA_OUTPUT_HEIGHT";
+	case ERROR_DMA_OUTPUT_FORMAT:
+		return "ERROR_DMA_OUTPUT_FORMAT";
+	case ERROR_DMA_OUTPUT_BIT_WIDTH:
+		return "ERROR_DMA_OUTPUT_BIT_WIDTH";
+	case ERROR_DMA_OUTPUT_PLANE:
+		return "ERROR_DMA_OUTPUT_PLANE";
+	case ERROR_DMA_OUTPUT_ORDER:
+		return "ERROR_DMA_OUTPUT_ORDER";
+
+	/* Sensor Error(100~199) */
+	case ERROR_SENSOR_I2C_FAIL:
+		return "ERROR_SENSOR_I2C_FAIL";
+	case ERROR_SENSOR_INVALID_FRAMERATE:
+		return "ERROR_SENSOR_INVALID_FRAMERATE";
+	case ERROR_SENSOR_INVALID_EXPOSURETIME:
+		return "ERROR_SENSOR_INVALID_EXPOSURETIME";
+	case ERROR_SENSOR_INVALID_SIZE:
+		return "ERROR_SENSOR_INVALID_SIZE";
+	case ERROR_SENSOR_INVALID_SETTING:
+		return "ERROR_SENSOR_INVALID_SETTING";
+	case ERROR_SENSOR_ACTURATOR_INIT_FAIL:
+		return "ERROR_SENSOR_ACTURATOR_INIT_FAIL";
+	case ERROR_SENSOR_INVALID_AF_POS:
+		return "ERROR_SENSOR_INVALID_AF_POS";
+	case ERROR_SENSOR_UNSUPPORT_FUNC:
+		return "ERROR_SENSOR_UNSUPPORT_FUNC";
+	case ERROR_SENSOR_UNSUPPORT_PERI:
+		return "ERROR_SENSOR_UNSUPPORT_PERI";
+	case ERROR_SENSOR_UNSUPPORT_AF:
+		return "ERROR_SENSOR_UNSUPPORT_AF";
+
+	/* ISP Error (200~299) */
+	case ERROR_ISP_AF_BUSY:
+		return "ERROR_ISP_AF_BUSY";
+	case ERROR_ISP_AF_INVALID_COMMAND:
+		return "ERROR_ISP_AF_INVALID_COMMAND";
+	case ERROR_ISP_AF_INVALID_MODE:
+		return "ERROR_ISP_AF_INVALID_MODE";
+
+	/* DRC Error (300~399) */
+	/* FD Error  (400~499) */
+	case ERROR_FD_CONFIG_MAX_NUMBER_STATE:
+		return "ERROR_FD_CONFIG_MAX_NUMBER_STATE";
+	case ERROR_FD_CONFIG_MAX_NUMBER_INVALID:
+		return "ERROR_FD_CONFIG_MAX_NUMBER_INVALID";
+	case ERROR_FD_CONFIG_YAW_ANGLE_STATE:
+		return "ERROR_FD_CONFIG_YAW_ANGLE_STATE";
+	case ERROR_FD_CONFIG_YAW_ANGLE_INVALID:
+		return "ERROR_FD_CONFIG_YAW_ANGLE_INVALID\n";
+	case ERROR_FD_CONFIG_ROLL_ANGLE_STATE:
+		return "ERROR_FD_CONFIG_ROLL_ANGLE_STATE";
+	case ERROR_FD_CONFIG_ROLL_ANGLE_INVALID:
+		return "ERROR_FD_CONFIG_ROLL_ANGLE_INVALID";
+	case ERROR_FD_CONFIG_SMILE_MODE_INVALID:
+		return "ERROR_FD_CONFIG_SMILE_MODE_INVALID";
+	case ERROR_FD_CONFIG_BLINK_MODE_INVALID:
+		return "ERROR_FD_CONFIG_BLINK_MODE_INVALID";
+	case ERROR_FD_CONFIG_EYES_DETECT_INVALID:
+		return "ERROR_FD_CONFIG_EYES_DETECT_INVALID";
+	case ERROR_FD_CONFIG_MOUTH_DETECT_INVALID:
+		return "ERROR_FD_CONFIG_MOUTH_DETECT_INVALID";
+	case ERROR_FD_CONFIG_ORIENTATION_STATE:
+		return "ERROR_FD_CONFIG_ORIENTATION_STATE";
+	case ERROR_FD_CONFIG_ORIENTATION_INVALID:
+		return "ERROR_FD_CONFIG_ORIENTATION_INVALID";
+	case ERROR_FD_CONFIG_ORIENTATION_VALUE_INVALID:
+		return "ERROR_FD_CONFIG_ORIENTATION_VALUE_INVALID";
+	case ERROR_FD_RESULT:
+		return "ERROR_FD_RESULT";
+	case ERROR_FD_MODE:
+		return "ERROR_FD_MODE";
+	default:
+		return "Unknown";
+	}
+}
+
+const char * const fimc_is_strerr(unsigned int error)
+{
+	error &= ~IS_ERROR_TIME_OUT_FLAG;
+
+	switch (error) {
+	/* General */
+	case IS_ERROR_INVALID_COMMAND:
+		return "IS_ERROR_INVALID_COMMAND";
+	case IS_ERROR_REQUEST_FAIL:
+		return "IS_ERROR_REQUEST_FAIL";
+	case IS_ERROR_INVALID_SCENARIO:
+		return "IS_ERROR_INVALID_SCENARIO";
+	case IS_ERROR_INVALID_SENSORID:
+		return "IS_ERROR_INVALID_SENSORID";
+	case IS_ERROR_INVALID_MODE_CHANGE:
+		return "IS_ERROR_INVALID_MODE_CHANGE";
+	case IS_ERROR_INVALID_MAGIC_NUMBER:
+		return "IS_ERROR_INVALID_MAGIC_NUMBER";
+	case IS_ERROR_INVALID_SETFILE_HDR:
+		return "IS_ERROR_INVALID_SETFILE_HDR";
+	case IS_ERROR_BUSY:
+		return "IS_ERROR_BUSY";
+	case IS_ERROR_SET_PARAMETER:
+		return "IS_ERROR_SET_PARAMETER";
+	case IS_ERROR_INVALID_PATH:
+		return "IS_ERROR_INVALID_PATH";
+	case IS_ERROR_OPEN_SENSOR_FAIL:
+		return "IS_ERROR_OPEN_SENSOR_FAIL";
+	case IS_ERROR_ENTRY_MSG_THREAD_DOWN:
+		return "IS_ERROR_ENTRY_MSG_THREAD_DOWN";
+	case IS_ERROR_ISP_FRAME_END_NOT_DONE:
+		return "IS_ERROR_ISP_FRAME_END_NOT_DONE";
+	case IS_ERROR_DRC_FRAME_END_NOT_DONE:
+		return "IS_ERROR_DRC_FRAME_END_NOT_DONE";
+	case IS_ERROR_SCALERC_FRAME_END_NOT_DONE:
+		return "IS_ERROR_SCALERC_FRAME_END_NOT_DONE";
+	case IS_ERROR_ODC_FRAME_END_NOT_DONE:
+		return "IS_ERROR_ODC_FRAME_END_NOT_DONE";
+	case IS_ERROR_DIS_FRAME_END_NOT_DONE:
+		return "IS_ERROR_DIS_FRAME_END_NOT_DONE";
+	case IS_ERROR_TDNR_FRAME_END_NOT_DONE:
+		return "IS_ERROR_TDNR_FRAME_END_NOT_DONE";
+	case IS_ERROR_SCALERP_FRAME_END_NOT_DONE:
+		return "IS_ERROR_SCALERP_FRAME_END_NOT_DONE";
+	case IS_ERROR_WAIT_STREAM_OFF_NOT_DONE:
+		return "IS_ERROR_WAIT_STREAM_OFF_NOT_DONE";
+	case IS_ERROR_NO_MSG_IS_RECEIVED:
+		return "IS_ERROR_NO_MSG_IS_RECEIVED";
+	case IS_ERROR_SENSOR_MSG_FAIL:
+		return "IS_ERROR_SENSOR_MSG_FAIL";
+	case IS_ERROR_ISP_MSG_FAIL:
+		return "IS_ERROR_ISP_MSG_FAIL";
+	case IS_ERROR_DRC_MSG_FAIL:
+		return "IS_ERROR_DRC_MSG_FAIL";
+	case IS_ERROR_LHFD_MSG_FAIL:
+		return "IS_ERROR_LHFD_MSG_FAIL";
+	case IS_ERROR_UNKNOWN:
+		return "IS_ERROR_UNKNOWN";
+
+	/* Sensor */
+	case IS_ERROR_SENSOR_PWRDN_FAIL:
+		return "IS_ERROR_SENSOR_PWRDN_FAIL";
+
+	/* ISP */
+	case IS_ERROR_ISP_PWRDN_FAIL:
+		return "IS_ERROR_ISP_PWRDN_FAIL";
+	case IS_ERROR_ISP_MULTIPLE_INPUT:
+		return "IS_ERROR_ISP_MULTIPLE_INPUT";
+	case IS_ERROR_ISP_ABSENT_INPUT:
+		return "IS_ERROR_ISP_ABSENT_INPUT";
+	case IS_ERROR_ISP_ABSENT_OUTPUT:
+		return "IS_ERROR_ISP_ABSENT_OUTPUT";
+	case IS_ERROR_ISP_NONADJACENT_OUTPUT:
+		return "IS_ERROR_ISP_NONADJACENT_OUTPUT";
+	case IS_ERROR_ISP_FORMAT_MISMATCH:
+		return "IS_ERROR_ISP_FORMAT_MISMATCH";
+	case IS_ERROR_ISP_WIDTH_MISMATCH:
+		return "IS_ERROR_ISP_WIDTH_MISMATCH";
+	case IS_ERROR_ISP_HEIGHT_MISMATCH:
+		return "IS_ERROR_ISP_HEIGHT_MISMATCH";
+	case IS_ERROR_ISP_BITWIDTH_MISMATCH:
+		return "IS_ERROR_ISP_BITWIDTH_MISMATCH";
+	case IS_ERROR_ISP_FRAME_END_TIME_OUT:
+		return "IS_ERROR_ISP_FRAME_END_TIME_OUT";
+
+	/* DRC */
+	case IS_ERROR_DRC_PWRDN_FAIL:
+		return "IS_ERROR_DRC_PWRDN_FAIL";
+	case IS_ERROR_DRC_MULTIPLE_INPUT:
+		return "IS_ERROR_DRC_MULTIPLE_INPUT";
+	case IS_ERROR_DRC_ABSENT_INPUT:
+		return "IS_ERROR_DRC_ABSENT_INPUT";
+	case IS_ERROR_DRC_NONADJACENT_INPUT:
+		return "IS_ERROR_DRC_NONADJACENT_INPUT";
+	case IS_ERROR_DRC_ABSENT_OUTPUT:
+		return "IS_ERROR_DRC_ABSENT_OUTPUT";
+	case IS_ERROR_DRC_NONADJACENT_OUTPUT:
+		return "IS_ERROR_DRC_NONADJACENT_OUTPUT";
+	case IS_ERROR_DRC_FORMAT_MISMATCH:
+		return "IS_ERROR_DRC_FORMAT_MISMATCH";
+	case IS_ERROR_DRC_WIDTH_MISMATCH:
+		return "IS_ERROR_DRC_WIDTH_MISMATCH";
+	case IS_ERROR_DRC_HEIGHT_MISMATCH:
+		return "IS_ERROR_DRC_HEIGHT_MISMATCH";
+	case IS_ERROR_DRC_BITWIDTH_MISMATCH:
+		return "IS_ERROR_DRC_BITWIDTH_MISMATCH";
+	case IS_ERROR_DRC_FRAME_END_TIME_OUT:
+		return "IS_ERROR_DRC_FRAME_END_TIME_OUT";
+
+	/* FD */
+	case IS_ERROR_FD_PWRDN_FAIL:
+		return "IS_ERROR_FD_PWRDN_FAIL";
+	case IS_ERROR_FD_MULTIPLE_INPUT:
+		return "IS_ERROR_FD_MULTIPLE_INPUT";
+	case IS_ERROR_FD_ABSENT_INPUT:
+		return "IS_ERROR_FD_ABSENT_INPUT";
+	case IS_ERROR_FD_NONADJACENT_INPUT:
+		return "IS_ERROR_FD_NONADJACENT_INPUT";
+	case IS_ERROR_LHFD_FRAME_END_TIME_OUT:
+		return "IS_ERROR_LHFD_FRAME_END_TIME_OUT";
+	default:
+		return "Unknown";
+	}
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-is-errno.h b/drivers/media/platform/exynos4-is/fimc-is-errno.h
new file mode 100644
index 000000000000..3de6f6da6f87
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-errno.h
@@ -0,0 +1,248 @@
+/*
+ * Samsung Exynos4 SoC series FIMC-IS slave interface driver
+ *
+ * FIMC-IS error code definition
+ *
+ * Copyright (C) 2011 - 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@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
+ * published by the Free Software Foundation.
+*/
+
+#ifndef FIMC_IS_ERR_H_
+#define FIMC_IS_ERR_H_
+
+#define IS_ERROR_VER			011 /* IS ERROR VERSION 0.11 */
+
+enum {
+	IS_ERROR_NONE,
+
+	/* General 1 ~ 99 */
+	IS_ERROR_INVALID_COMMAND,
+	IS_ERROR_REQUEST_FAIL,
+	IS_ERROR_INVALID_SCENARIO,
+	IS_ERROR_INVALID_SENSORID,
+	IS_ERROR_INVALID_MODE_CHANGE,
+	IS_ERROR_INVALID_MAGIC_NUMBER,
+	IS_ERROR_INVALID_SETFILE_HDR,
+	IS_ERROR_BUSY,
+	IS_ERROR_SET_PARAMETER,
+	IS_ERROR_INVALID_PATH,
+	IS_ERROR_OPEN_SENSOR_FAIL,
+	IS_ERROR_ENTRY_MSG_THREAD_DOWN,
+	IS_ERROR_ISP_FRAME_END_NOT_DONE,
+	IS_ERROR_DRC_FRAME_END_NOT_DONE,
+	IS_ERROR_SCALERC_FRAME_END_NOT_DONE,
+	IS_ERROR_ODC_FRAME_END_NOT_DONE,
+	IS_ERROR_DIS_FRAME_END_NOT_DONE,
+	IS_ERROR_TDNR_FRAME_END_NOT_DONE,
+	IS_ERROR_SCALERP_FRAME_END_NOT_DONE,
+	IS_ERROR_WAIT_STREAM_OFF_NOT_DONE,
+	IS_ERROR_NO_MSG_IS_RECEIVED,
+	IS_ERROR_SENSOR_MSG_FAIL,
+	IS_ERROR_ISP_MSG_FAIL,
+	IS_ERROR_DRC_MSG_FAIL,
+	IS_ERROR_SCALERC_MSG_FAIL,
+	IS_ERROR_ODC_MSG_FAIL,
+	IS_ERROR_DIS_MSG_FAIL,
+	IS_ERROR_TDNR_MSG_FAIL,
+	IS_ERROR_SCALERP_MSG_FAIL,
+	IS_ERROR_LHFD_MSG_FAIL,
+	IS_ERROR_LHFD_INTERNAL_STOP,
+
+	/* Sensor 100 ~ 199 */
+	IS_ERROR_SENSOR_PWRDN_FAIL	= 100,
+	IS_ERROR_SENSOR_STREAM_ON_FAIL,
+	IS_ERROR_SENSOR_STREAM_OFF_FAIL,
+
+	/* ISP 200 ~ 299 */
+	IS_ERROR_ISP_PWRDN_FAIL		= 200,
+	IS_ERROR_ISP_MULTIPLE_INPUT,
+	IS_ERROR_ISP_ABSENT_INPUT,
+	IS_ERROR_ISP_ABSENT_OUTPUT,
+	IS_ERROR_ISP_NONADJACENT_OUTPUT,
+	IS_ERROR_ISP_FORMAT_MISMATCH,
+	IS_ERROR_ISP_WIDTH_MISMATCH,
+	IS_ERROR_ISP_HEIGHT_MISMATCH,
+	IS_ERROR_ISP_BITWIDTH_MISMATCH,
+	IS_ERROR_ISP_FRAME_END_TIME_OUT,
+
+	/* DRC 300 ~ 399 */
+	IS_ERROR_DRC_PWRDN_FAIL		= 300,
+	IS_ERROR_DRC_MULTIPLE_INPUT,
+	IS_ERROR_DRC_ABSENT_INPUT,
+	IS_ERROR_DRC_NONADJACENT_INPUT,
+	IS_ERROR_DRC_ABSENT_OUTPUT,
+	IS_ERROR_DRC_NONADJACENT_OUTPUT,
+	IS_ERROR_DRC_FORMAT_MISMATCH,
+	IS_ERROR_DRC_WIDTH_MISMATCH,
+	IS_ERROR_DRC_HEIGHT_MISMATCH,
+	IS_ERROR_DRC_BITWIDTH_MISMATCH,
+	IS_ERROR_DRC_FRAME_END_TIME_OUT,
+
+	/* SCALERC 400 ~ 499 */
+	IS_ERROR_SCALERC_PWRDN_FAIL	= 400,
+
+	/* ODC 500 ~ 599 */
+	IS_ERROR_ODC_PWRDN_FAIL		= 500,
+
+	/* DIS 600 ~ 699 */
+	IS_ERROR_DIS_PWRDN_FAIL		= 600,
+
+	/* TDNR 700 ~ 799 */
+	IS_ERROR_TDNR_PWRDN_FAIL	= 700,
+
+	/* SCALERC 800 ~ 899 */
+	IS_ERROR_SCALERP_PWRDN_FAIL	= 800,
+
+	/* FD 900 ~ 999 */
+	IS_ERROR_FD_PWRDN_FAIL		= 900,
+	IS_ERROR_FD_MULTIPLE_INPUT,
+	IS_ERROR_FD_ABSENT_INPUT,
+	IS_ERROR_FD_NONADJACENT_INPUT,
+	IS_ERROR_LHFD_FRAME_END_TIME_OUT,
+
+	IS_ERROR_UNKNOWN		= 1000,
+};
+
+#define IS_ERROR_TIME_OUT_FLAG	0x80000000
+
+/* Set parameter error enum */
+enum fimc_is_error {
+	/* Common error (0~99) */
+	ERROR_COMMON_NONE		= 0,
+	ERROR_COMMON_CMD		= 1,	/* Invalid command */
+	ERROR_COMMON_PARAMETER		= 2,	/* Invalid parameter */
+	/* setfile is not loaded before adjusting */
+	ERROR_COMMON_SETFILE_LOAD	= 3,
+	/* setfile is not Adjusted before runnng. */
+	ERROR_COMMON_SETFILE_ADJUST	= 4,
+	/* Index of setfile is not valid (0~MAX_SETFILE_NUM-1) */
+	ERROR_COMMON_SETFILE_INDEX	= 5,
+	/* Input path can be changed in ready state(stop) */
+	ERROR_COMMON_INPUT_PATH		= 6,
+	/* IP can not start if input path is not set */
+	ERROR_COMMON_INPUT_INIT		= 7,
+	/* Output path can be changed in ready state (stop) */
+	ERROR_COMMON_OUTPUT_PATH	= 8,
+	/* IP can not start if output path is not set */
+	ERROR_COMMON_OUTPUT_INIT	= 9,
+
+	ERROR_CONTROL_NONE		= ERROR_COMMON_NONE,
+	ERROR_CONTROL_BYPASS		= 11,	/* Enable or Disable */
+
+	ERROR_OTF_INPUT_NONE		= ERROR_COMMON_NONE,
+	ERROR_OTF_INPUT_CMD		= 21,
+	/* invalid format  (DRC: YUV444, FD: YUV444, 422, 420) */
+	ERROR_OTF_INPUT_FORMAT		= 22,
+	/* invalid width (DRC: 128~8192, FD: 32~8190) */
+	ERROR_OTF_INPUT_WIDTH		= 23,
+	/* invalid height (DRC: 64~8192, FD: 16~8190) */
+	ERROR_OTF_INPUT_HEIGHT		= 24,
+	/* invalid bit-width (DRC: 8~12bits, FD: 8bit) */
+	ERROR_OTF_INPUT_BIT_WIDTH	= 25,
+	/* invalid FrameTime for ISP */
+	ERROR_OTF_INPUT_USER_FRAMETIIME	= 26,
+
+	ERROR_DMA_INPUT_NONE		= ERROR_COMMON_NONE,
+	/* invalid width (DRC: 128~8192, FD: 32~8190) */
+	ERROR_DMA_INPUT_WIDTH		= 31,
+	/* invalid height (DRC: 64~8192, FD: 16~8190) */
+	ERROR_DMA_INPUT_HEIGHT		= 32,
+	/* invalid format (DRC: YUV444 or YUV422, FD: YUV444, 422, 420) */
+	ERROR_DMA_INPUT_FORMAT		= 33,
+	/* invalid bit-width (DRC: 8~12bit, FD: 8bit) */
+	ERROR_DMA_INPUT_BIT_WIDTH	= 34,
+	/* invalid order(DRC: YYCbCrorYCbYCr, FD:NO,YYCbCr,YCbYCr,CbCr,CrCb) */
+	ERROR_DMA_INPUT_ORDER		= 35,
+	/* invalid palne (DRC: 3, FD: 1, 2, 3) */
+	ERROR_DMA_INPUT_PLANE		= 36,
+
+	ERROR_OTF_OUTPUT_NONE		= ERROR_COMMON_NONE,
+	/* invalid width (DRC: 128~8192) */
+	ERROR_OTF_OUTPUT_WIDTH		= 41,
+	/* invalid height (DRC: 64~8192) */
+	ERROR_OTF_OUTPUT_HEIGHT		= 42,
+	/* invalid format (DRC: YUV444) */
+	ERROR_OTF_OUTPUT_FORMAT		= 43,
+	/* invalid bit-width (DRC: 8~12bits) */
+	ERROR_OTF_OUTPUT_BIT_WIDTH	= 44,
+
+	ERROR_DMA_OUTPUT_NONE		= ERROR_COMMON_NONE,
+	ERROR_DMA_OUTPUT_WIDTH		= 51,	/* invalid width */
+	ERROR_DMA_OUTPUT_HEIGHT		= 52,	/* invalid height */
+	ERROR_DMA_OUTPUT_FORMAT		= 53,	/* invalid format */
+	ERROR_DMA_OUTPUT_BIT_WIDTH	= 54,	/* invalid bit-width */
+	ERROR_DMA_OUTPUT_PLANE		= 55,	/* invalid plane */
+	ERROR_DMA_OUTPUT_ORDER		= 56,	/* invalid order */
+
+	ERROR_GLOBAL_SHOTMODE_NONE	= ERROR_COMMON_NONE,
+
+	/* SENSOR Error(100~199) */
+	ERROR_SENSOR_NONE		= ERROR_COMMON_NONE,
+	ERROR_SENSOR_I2C_FAIL		= 101,
+	ERROR_SENSOR_INVALID_FRAMERATE,
+	ERROR_SENSOR_INVALID_EXPOSURETIME,
+	ERROR_SENSOR_INVALID_SIZE,
+	ERROR_SENSOR_INVALID_SETTING,
+	ERROR_SENSOR_ACTURATOR_INIT_FAIL,
+	ERROR_SENSOR_INVALID_AF_POS,
+	ERROR_SENSOR_UNSUPPORT_FUNC,
+	ERROR_SENSOR_UNSUPPORT_PERI,
+	ERROR_SENSOR_UNSUPPORT_AF,
+
+	/* ISP Error (200~299) */
+	ERROR_ISP_AF_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_AF_BUSY		= 201,
+	ERROR_ISP_AF_INVALID_COMMAND	= 202,
+	ERROR_ISP_AF_INVALID_MODE	= 203,
+	ERROR_ISP_FLASH_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_AWB_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_IMAGE_EFFECT_NONE	= ERROR_COMMON_NONE,
+	ERROR_ISP_ISO_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_ADJUST_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_METERING_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_AFC_NONE		= ERROR_COMMON_NONE,
+
+	/* DRC Error (300~399) */
+
+	/* FD Error  (400~499) */
+	ERROR_FD_NONE					= ERROR_COMMON_NONE,
+	/* Invalid max number (1~16) */
+	ERROR_FD_CONFIG_MAX_NUMBER_STATE		= 401,
+	ERROR_FD_CONFIG_MAX_NUMBER_INVALID		= 402,
+	ERROR_FD_CONFIG_YAW_ANGLE_STATE			= 403,
+	ERROR_FD_CONFIG_YAW_ANGLE_INVALID		= 404,
+	ERROR_FD_CONFIG_ROLL_ANGLE_STATE		= 405,
+	ERROR_FD_CONFIG_ROLL_ANGLE_INVALID		= 406,
+	ERROR_FD_CONFIG_SMILE_MODE_INVALID		= 407,
+	ERROR_FD_CONFIG_BLINK_MODE_INVALID		= 408,
+	ERROR_FD_CONFIG_EYES_DETECT_INVALID		= 409,
+	ERROR_FD_CONFIG_MOUTH_DETECT_INVALID		= 410,
+	ERROR_FD_CONFIG_ORIENTATION_STATE		= 411,
+	ERROR_FD_CONFIG_ORIENTATION_INVALID		= 412,
+	ERROR_FD_CONFIG_ORIENTATION_VALUE_INVALID	= 413,
+	/* PARAM_FdResultStr can be only applied in ready-state or stream off */
+	ERROR_FD_RESULT					= 414,
+	/* PARAM_FdModeStr can be only applied in ready-state or stream off */
+	ERROR_FD_MODE					= 415,
+	/* Scaler Error  (500 ~ 599) */
+	ERROR_SCALER_NO_NONE				= ERROR_COMMON_NONE,
+	ERROR_SCALER_DMA_OUTSEL				= 501,
+	ERROR_SCALER_H_RATIO				= 502,
+	ERROR_SCALER_V_RATIO				= 503,
+
+	ERROR_SCALER_IMAGE_EFFECT			= 510,
+
+	ERROR_SCALER_ROTATE				= 520,
+	ERROR_SCALER_FLIP				= 521,
+};
+
+const char * const fimc_is_strerr(unsigned int error);
+const char * const fimc_is_param_strerr(unsigned int error);
+
+#endif /* FIMC_IS_ERR_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-is-i2c.c b/drivers/media/platform/exynos4-is/fimc-is-i2c.c
new file mode 100644
index 000000000000..c397777d7cbb
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-i2c.c
@@ -0,0 +1,126 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Author: 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
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/module.h>
+#include <linux/of_i2c.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include "fimc-is-i2c.h"
+
+struct fimc_is_i2c {
+	struct i2c_adapter adapter;
+	struct clk *clock;
+};
+
+/*
+ * An empty algorithm is used as the actual I2C bus controller driver
+ * is implemented in the FIMC-IS subsystem firmware and the host CPU
+ * doesn't access the I2C bus controller.
+ */
+static const struct i2c_algorithm fimc_is_i2c_algorithm;
+
+static int fimc_is_i2c_probe(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct fimc_is_i2c *isp_i2c;
+	struct i2c_adapter *i2c_adap;
+	int ret;
+
+	isp_i2c = devm_kzalloc(&pdev->dev, sizeof(*isp_i2c), GFP_KERNEL);
+	if (!isp_i2c)
+		return -ENOMEM;
+
+	isp_i2c->clock = devm_clk_get(&pdev->dev, "i2c_isp");
+	if (IS_ERR(isp_i2c->clock)) {
+		dev_err(&pdev->dev, "failed to get the clock\n");
+		return PTR_ERR(isp_i2c->clock);
+	}
+
+	i2c_adap = &isp_i2c->adapter;
+	i2c_adap->dev.of_node = node;
+	i2c_adap->dev.parent = &pdev->dev;
+	strlcpy(i2c_adap->name, "exynos4x12-isp-i2c", sizeof(i2c_adap->name));
+	i2c_adap->owner = THIS_MODULE;
+	i2c_adap->algo = &fimc_is_i2c_algorithm;
+	i2c_adap->class = I2C_CLASS_SPD;
+
+	ret = i2c_add_adapter(i2c_adap);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to add I2C bus %s\n",
+						node->full_name);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, isp_i2c);
+
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_enable(&i2c_adap->dev);
+
+	of_i2c_register_devices(i2c_adap);
+
+	return 0;
+}
+
+static int fimc_is_i2c_remove(struct platform_device *pdev)
+{
+	struct fimc_is_i2c *isp_i2c = platform_get_drvdata(pdev);
+
+	pm_runtime_disable(&isp_i2c->adapter.dev);
+	pm_runtime_disable(&pdev->dev);
+	i2c_del_adapter(&isp_i2c->adapter);
+
+	return 0;
+}
+
+static int fimc_is_i2c_suspend(struct device *dev)
+{
+	struct fimc_is_i2c *isp_i2c = dev_get_drvdata(dev);
+	clk_disable_unprepare(isp_i2c->clock);
+	return 0;
+}
+
+static int fimc_is_i2c_resume(struct device *dev)
+{
+	struct fimc_is_i2c *isp_i2c = dev_get_drvdata(dev);
+	return clk_prepare_enable(isp_i2c->clock);
+}
+
+UNIVERSAL_DEV_PM_OPS(fimc_is_i2c_pm_ops, fimc_is_i2c_suspend,
+		     fimc_is_i2c_resume, NULL);
+
+static const struct of_device_id fimc_is_i2c_of_match[] = {
+	{ .compatible = FIMC_IS_I2C_COMPATIBLE },
+	{ },
+};
+
+static struct platform_driver fimc_is_i2c_driver = {
+	.probe		= fimc_is_i2c_probe,
+	.remove		= fimc_is_i2c_remove,
+	.driver = {
+		.of_match_table = fimc_is_i2c_of_match,
+		.name		= "fimc-isp-i2c",
+		.owner		= THIS_MODULE,
+		.pm		= &fimc_is_i2c_pm_ops,
+	}
+};
+
+int fimc_is_register_i2c_driver(void)
+{
+	return platform_driver_register(&fimc_is_i2c_driver);
+}
+
+void fimc_is_unregister_i2c_driver(void)
+{
+	platform_driver_unregister(&fimc_is_i2c_driver);
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-is-i2c.h b/drivers/media/platform/exynos4-is/fimc-is-i2c.h
new file mode 100644
index 000000000000..0d38d6bb963b
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-i2c.h
@@ -0,0 +1,15 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ * 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
+ * published by the Free Software Foundation.
+ */
+
+#define FIMC_IS_I2C_COMPATIBLE	"samsung,exynos4212-i2c-isp"
+
+int fimc_is_register_i2c_driver(void);
+void fimc_is_unregister_i2c_driver(void);
diff --git a/drivers/media/platform/exynos4-is/fimc-is-param.c b/drivers/media/platform/exynos4-is/fimc-is-param.c
new file mode 100644
index 000000000000..53fe2a2b4db3
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-param.c
@@ -0,0 +1,900 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@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
+ * published by the Free Software Foundation.
+ */
+#define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
+
+#include <linux/bitops.h>
+#include <linux/bug.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+
+#include "fimc-is.h"
+#include "fimc-is-command.h"
+#include "fimc-is-errno.h"
+#include "fimc-is-param.h"
+#include "fimc-is-regs.h"
+#include "fimc-is-sensor.h"
+
+static void __hw_param_copy(void *dst, void *src)
+{
+	memcpy(dst, src, FIMC_IS_PARAM_MAX_SIZE);
+}
+
+void __fimc_is_hw_update_param_global_shotmode(struct fimc_is *is)
+{
+	struct param_global_shotmode *dst, *src;
+
+	dst = &is->is_p_region->parameter.global.shotmode;
+	src = &is->config[is->config_index].global.shotmode;
+	__hw_param_copy(dst, src);
+}
+
+void __fimc_is_hw_update_param_sensor_framerate(struct fimc_is *is)
+{
+	struct param_sensor_framerate *dst, *src;
+
+	dst = &is->is_p_region->parameter.sensor.frame_rate;
+	src = &is->config[is->config_index].sensor.frame_rate;
+	__hw_param_copy(dst, src);
+}
+
+int __fimc_is_hw_update_param(struct fimc_is *is, u32 offset)
+{
+	struct is_param_region *par = &is->is_p_region->parameter;
+	struct chain_config *cfg = &is->config[is->config_index];
+
+	switch (offset) {
+	case PARAM_ISP_CONTROL:
+		__hw_param_copy(&par->isp.control, &cfg->isp.control);
+		break;
+
+	case PARAM_ISP_OTF_INPUT:
+		__hw_param_copy(&par->isp.otf_input, &cfg->isp.otf_input);
+		break;
+
+	case PARAM_ISP_DMA1_INPUT:
+		__hw_param_copy(&par->isp.dma1_input, &cfg->isp.dma1_input);
+		break;
+
+	case PARAM_ISP_DMA2_INPUT:
+		__hw_param_copy(&par->isp.dma2_input, &cfg->isp.dma2_input);
+		break;
+
+	case PARAM_ISP_AA:
+		__hw_param_copy(&par->isp.aa, &cfg->isp.aa);
+		break;
+
+	case PARAM_ISP_FLASH:
+		__hw_param_copy(&par->isp.flash, &cfg->isp.flash);
+		break;
+
+	case PARAM_ISP_AWB:
+		__hw_param_copy(&par->isp.awb, &cfg->isp.awb);
+		break;
+
+	case PARAM_ISP_IMAGE_EFFECT:
+		__hw_param_copy(&par->isp.effect, &cfg->isp.effect);
+		break;
+
+	case PARAM_ISP_ISO:
+		__hw_param_copy(&par->isp.iso, &cfg->isp.iso);
+		break;
+
+	case PARAM_ISP_ADJUST:
+		__hw_param_copy(&par->isp.adjust, &cfg->isp.adjust);
+		break;
+
+	case PARAM_ISP_METERING:
+		__hw_param_copy(&par->isp.metering, &cfg->isp.metering);
+		break;
+
+	case PARAM_ISP_AFC:
+		__hw_param_copy(&par->isp.afc, &cfg->isp.afc);
+		break;
+
+	case PARAM_ISP_OTF_OUTPUT:
+		__hw_param_copy(&par->isp.otf_output, &cfg->isp.otf_output);
+		break;
+
+	case PARAM_ISP_DMA1_OUTPUT:
+		__hw_param_copy(&par->isp.dma1_output, &cfg->isp.dma1_output);
+		break;
+
+	case PARAM_ISP_DMA2_OUTPUT:
+		__hw_param_copy(&par->isp.dma2_output, &cfg->isp.dma2_output);
+		break;
+
+	case PARAM_DRC_CONTROL:
+		__hw_param_copy(&par->drc.control, &cfg->drc.control);
+		break;
+
+	case PARAM_DRC_OTF_INPUT:
+		__hw_param_copy(&par->drc.otf_input, &cfg->drc.otf_input);
+		break;
+
+	case PARAM_DRC_DMA_INPUT:
+		__hw_param_copy(&par->drc.dma_input, &cfg->drc.dma_input);
+		break;
+
+	case PARAM_DRC_OTF_OUTPUT:
+		__hw_param_copy(&par->drc.otf_output, &cfg->drc.otf_output);
+		break;
+
+	case PARAM_FD_CONTROL:
+		__hw_param_copy(&par->fd.control, &cfg->fd.control);
+		break;
+
+	case PARAM_FD_OTF_INPUT:
+		__hw_param_copy(&par->fd.otf_input, &cfg->fd.otf_input);
+		break;
+
+	case PARAM_FD_DMA_INPUT:
+		__hw_param_copy(&par->fd.dma_input, &cfg->fd.dma_input);
+		break;
+
+	case PARAM_FD_CONFIG:
+		__hw_param_copy(&par->fd.config, &cfg->fd.config);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+unsigned int __get_pending_param_count(struct fimc_is *is)
+{
+	struct chain_config *config = &is->config[is->config_index];
+	unsigned long flags;
+	unsigned int count;
+
+	spin_lock_irqsave(&is->slock, flags);
+	count = hweight32(config->p_region_index1);
+	count += hweight32(config->p_region_index2);
+	spin_unlock_irqrestore(&is->slock, flags);
+
+	return count;
+}
+
+int __is_hw_update_params(struct fimc_is *is)
+{
+	unsigned long *p_index1, *p_index2;
+	int i, id, ret = 0;
+
+	id = is->config_index;
+	p_index1 = &is->config[id].p_region_index1;
+	p_index2 = &is->config[id].p_region_index2;
+
+	if (test_bit(PARAM_GLOBAL_SHOTMODE, p_index1))
+		__fimc_is_hw_update_param_global_shotmode(is);
+
+	if (test_bit(PARAM_SENSOR_FRAME_RATE, p_index1))
+		__fimc_is_hw_update_param_sensor_framerate(is);
+
+	for (i = PARAM_ISP_CONTROL; i < PARAM_DRC_CONTROL; i++) {
+		if (test_bit(i, p_index1))
+			ret = __fimc_is_hw_update_param(is, i);
+	}
+
+	for (i = PARAM_DRC_CONTROL; i < PARAM_SCALERC_CONTROL; i++) {
+		if (test_bit(i, p_index1))
+			ret = __fimc_is_hw_update_param(is, i);
+	}
+
+	for (i = PARAM_FD_CONTROL; i <= PARAM_FD_CONFIG; i++) {
+		if (test_bit((i - 32), p_index2))
+			ret = __fimc_is_hw_update_param(is, i);
+	}
+
+	return ret;
+}
+
+void __is_get_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf)
+{
+	struct isp_param *isp;
+
+	isp = &is->config[is->config_index].isp;
+	mf->width = isp->otf_input.width;
+	mf->height = isp->otf_input.height;
+}
+
+void __is_set_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+	struct drc_param *drc;
+	struct fd_param *fd;
+
+	isp = &is->config[index].isp;
+	drc = &is->config[index].drc;
+	fd = &is->config[index].fd;
+
+	/* Update isp size info (OTF only) */
+	isp->otf_input.width = mf->width;
+	isp->otf_input.height = mf->height;
+	isp->otf_output.width = mf->width;
+	isp->otf_output.height = mf->height;
+	/* Update drc size info (OTF only) */
+	drc->otf_input.width = mf->width;
+	drc->otf_input.height = mf->height;
+	drc->otf_output.width = mf->width;
+	drc->otf_output.height = mf->height;
+	/* Update fd size info (OTF only) */
+	fd->otf_input.width = mf->width;
+	fd->otf_input.height = mf->height;
+
+	if (test_bit(PARAM_ISP_OTF_INPUT,
+		      &is->config[index].p_region_index1))
+		return;
+
+	/* Update field */
+	fimc_is_set_param_bit(is, PARAM_ISP_OTF_INPUT);
+	fimc_is_set_param_bit(is, PARAM_ISP_OTF_OUTPUT);
+	fimc_is_set_param_bit(is, PARAM_DRC_OTF_INPUT);
+	fimc_is_set_param_bit(is, PARAM_DRC_OTF_OUTPUT);
+	fimc_is_set_param_bit(is, PARAM_FD_OTF_INPUT);
+}
+
+int fimc_is_hw_get_sensor_max_framerate(struct fimc_is *is)
+{
+	switch (is->sensor->drvdata->id) {
+	case FIMC_IS_SENSOR_ID_S5K6A3:
+		return 30;
+	default:
+		return 15;
+	}
+}
+
+void __is_set_sensor(struct fimc_is *is, int fps)
+{
+	unsigned int index = is->config_index;
+	struct sensor_param *sensor;
+	struct isp_param *isp;
+
+	sensor = &is->config[index].sensor;
+	isp = &is->config[index].isp;
+
+	if (fps == 0) {
+		sensor->frame_rate.frame_rate =
+				fimc_is_hw_get_sensor_max_framerate(is);
+		isp->otf_input.frametime_min = 0;
+		isp->otf_input.frametime_max = 66666;
+	} else {
+		sensor->frame_rate.frame_rate = fps;
+		isp->otf_input.frametime_min = 0;
+		isp->otf_input.frametime_max = (u32)1000000 / fps;
+	}
+
+	fimc_is_set_param_bit(is, PARAM_SENSOR_FRAME_RATE);
+	fimc_is_set_param_bit(is, PARAM_ISP_OTF_INPUT);
+}
+
+void __is_set_init_isp_aa(struct fimc_is *is)
+{
+	struct isp_param *isp;
+
+	isp = &is->config[is->config_index].isp;
+
+	isp->aa.cmd = ISP_AA_COMMAND_START;
+	isp->aa.target = ISP_AA_TARGET_AF | ISP_AA_TARGET_AE |
+			 ISP_AA_TARGET_AWB;
+	isp->aa.mode = 0;
+	isp->aa.scene = 0;
+	isp->aa.sleep = 0;
+	isp->aa.face = 0;
+	isp->aa.touch_x = 0;
+	isp->aa.touch_y = 0;
+	isp->aa.manual_af_setting = 0;
+	isp->aa.err = ISP_AF_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_AA);
+}
+
+void __is_set_isp_flash(struct fimc_is *is, u32 cmd, u32 redeye)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp = &is->config[index].isp;
+
+	isp->flash.cmd = cmd;
+	isp->flash.redeye = redeye;
+	isp->flash.err = ISP_FLASH_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_FLASH);
+}
+
+void __is_set_isp_awb(struct fimc_is *is, u32 cmd, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+
+	isp = &is->config[index].isp;
+
+	isp->awb.cmd = cmd;
+	isp->awb.illumination = val;
+	isp->awb.err = ISP_AWB_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_AWB);
+}
+
+void __is_set_isp_effect(struct fimc_is *is, u32 cmd)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+
+	isp = &is->config[index].isp;
+
+	isp->effect.cmd = cmd;
+	isp->effect.err = ISP_IMAGE_EFFECT_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_IMAGE_EFFECT);
+}
+
+void __is_set_isp_iso(struct fimc_is *is, u32 cmd, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+
+	isp = &is->config[index].isp;
+
+	isp->iso.cmd = cmd;
+	isp->iso.value = val;
+	isp->iso.err = ISP_ISO_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_ISO);
+}
+
+void __is_set_isp_adjust(struct fimc_is *is, u32 cmd, u32 val)
+{
+	unsigned int index = is->config_index;
+	unsigned long *p_index;
+	struct isp_param *isp;
+
+	p_index = &is->config[index].p_region_index1;
+	isp = &is->config[index].isp;
+
+	switch (cmd) {
+	case ISP_ADJUST_COMMAND_MANUAL_CONTRAST:
+		isp->adjust.contrast = val;
+		break;
+	case ISP_ADJUST_COMMAND_MANUAL_SATURATION:
+		isp->adjust.saturation = val;
+		break;
+	case ISP_ADJUST_COMMAND_MANUAL_SHARPNESS:
+		isp->adjust.sharpness = val;
+		break;
+	case ISP_ADJUST_COMMAND_MANUAL_EXPOSURE:
+		isp->adjust.exposure = val;
+		break;
+	case ISP_ADJUST_COMMAND_MANUAL_BRIGHTNESS:
+		isp->adjust.brightness = val;
+		break;
+	case ISP_ADJUST_COMMAND_MANUAL_HUE:
+		isp->adjust.hue = val;
+		break;
+	case ISP_ADJUST_COMMAND_AUTO:
+		isp->adjust.contrast = 0;
+		isp->adjust.saturation = 0;
+		isp->adjust.sharpness = 0;
+		isp->adjust.exposure = 0;
+		isp->adjust.brightness = 0;
+		isp->adjust.hue = 0;
+		break;
+	}
+
+	if (!test_bit(PARAM_ISP_ADJUST, p_index)) {
+		isp->adjust.cmd = cmd;
+		isp->adjust.err = ISP_ADJUST_ERROR_NONE;
+		fimc_is_set_param_bit(is, PARAM_ISP_ADJUST);
+	} else {
+		isp->adjust.cmd |= cmd;
+	}
+}
+
+void __is_set_isp_metering(struct fimc_is *is, u32 id, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index1;
+	isp = &is->config[index].isp;
+
+	switch (id) {
+	case IS_METERING_CONFIG_CMD:
+		isp->metering.cmd = val;
+		break;
+	case IS_METERING_CONFIG_WIN_POS_X:
+		isp->metering.win_pos_x = val;
+		break;
+	case IS_METERING_CONFIG_WIN_POS_Y:
+		isp->metering.win_pos_y = val;
+		break;
+	case IS_METERING_CONFIG_WIN_WIDTH:
+		isp->metering.win_width = val;
+		break;
+	case IS_METERING_CONFIG_WIN_HEIGHT:
+		isp->metering.win_height = val;
+		break;
+	default:
+		return;
+	}
+
+	if (!test_bit(PARAM_ISP_METERING, p_index)) {
+		isp->metering.err = ISP_METERING_ERROR_NONE;
+		fimc_is_set_param_bit(is, PARAM_ISP_METERING);
+	}
+}
+
+void __is_set_isp_afc(struct fimc_is *is, u32 cmd, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+
+	isp = &is->config[index].isp;
+
+	isp->afc.cmd = cmd;
+	isp->afc.manual = val;
+	isp->afc.err = ISP_AFC_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_AFC);
+}
+
+void __is_set_drc_control(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct drc_param *drc;
+
+	drc = &is->config[index].drc;
+
+	drc->control.bypass = val;
+
+	fimc_is_set_param_bit(is, PARAM_DRC_CONTROL);
+}
+
+void __is_set_fd_control(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->control.cmd = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index))
+		fimc_is_set_param_bit(is, PARAM_FD_CONTROL);
+}
+
+void __is_set_fd_config_maxface(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.max_number = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_MAXIMUM_NUMBER;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_MAXIMUM_NUMBER;
+	}
+}
+
+void __is_set_fd_config_rollangle(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.roll_angle = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_ROLL_ANGLE;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_ROLL_ANGLE;
+	}
+}
+
+void __is_set_fd_config_yawangle(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.yaw_angle = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_YAW_ANGLE;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_YAW_ANGLE;
+	}
+}
+
+void __is_set_fd_config_smilemode(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.smile_mode = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_SMILE_MODE;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_SMILE_MODE;
+	}
+}
+
+void __is_set_fd_config_blinkmode(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.blink_mode = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_BLINK_MODE;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_BLINK_MODE;
+	}
+}
+
+void __is_set_fd_config_eyedetect(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.eye_detect = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_EYES_DETECT;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_EYES_DETECT;
+	}
+}
+
+void __is_set_fd_config_mouthdetect(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.mouth_detect = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_MOUTH_DETECT;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_MOUTH_DETECT;
+	}
+}
+
+void __is_set_fd_config_orientation(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.orientation = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_ORIENTATION;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_ORIENTATION;
+	}
+}
+
+void __is_set_fd_config_orientation_val(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index2;
+	fd = &is->config[index].fd;
+
+	fd->config.orientation_value = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_ORIENTATION_VALUE;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_ORIENTATION_VALUE;
+	}
+}
+
+void fimc_is_set_initial_params(struct fimc_is *is)
+{
+	struct global_param *global;
+	struct sensor_param *sensor;
+	struct isp_param *isp;
+	struct drc_param *drc;
+	struct fd_param *fd;
+	unsigned long *p_index1, *p_index2;
+	unsigned int index;
+
+	index = is->config_index;
+	global = &is->config[index].global;
+	sensor = &is->config[index].sensor;
+	isp = &is->config[index].isp;
+	drc = &is->config[index].drc;
+	fd = &is->config[index].fd;
+	p_index1 = &is->config[index].p_region_index1;
+	p_index2 = &is->config[index].p_region_index2;
+
+	/* Global */
+	global->shotmode.cmd = 1;
+	fimc_is_set_param_bit(is, PARAM_GLOBAL_SHOTMODE);
+
+	/* ISP */
+	isp->control.cmd = CONTROL_COMMAND_START;
+	isp->control.bypass = CONTROL_BYPASS_DISABLE;
+	isp->control.err = CONTROL_ERROR_NONE;
+	fimc_is_set_param_bit(is, PARAM_ISP_CONTROL);
+
+	isp->otf_input.cmd = OTF_INPUT_COMMAND_ENABLE;
+	if (!test_bit(PARAM_ISP_OTF_INPUT, p_index1)) {
+		isp->otf_input.width = DEFAULT_PREVIEW_STILL_WIDTH;
+		isp->otf_input.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+		fimc_is_set_param_bit(is, PARAM_ISP_OTF_INPUT);
+	}
+	if (is->sensor->test_pattern)
+		isp->otf_input.format = OTF_INPUT_FORMAT_STRGEN_COLORBAR_BAYER;
+	else
+		isp->otf_input.format = OTF_INPUT_FORMAT_BAYER;
+	isp->otf_input.bitwidth = 10;
+	isp->otf_input.order = OTF_INPUT_ORDER_BAYER_GR_BG;
+	isp->otf_input.crop_offset_x = 0;
+	isp->otf_input.crop_offset_y = 0;
+	isp->otf_input.err = OTF_INPUT_ERROR_NONE;
+
+	isp->dma1_input.cmd = DMA_INPUT_COMMAND_DISABLE;
+	isp->dma1_input.width = 0;
+	isp->dma1_input.height = 0;
+	isp->dma1_input.format = 0;
+	isp->dma1_input.bitwidth = 0;
+	isp->dma1_input.plane = 0;
+	isp->dma1_input.order = 0;
+	isp->dma1_input.buffer_number = 0;
+	isp->dma1_input.width = 0;
+	isp->dma1_input.err = DMA_INPUT_ERROR_NONE;
+	fimc_is_set_param_bit(is, PARAM_ISP_DMA1_INPUT);
+
+	isp->dma2_input.cmd = DMA_INPUT_COMMAND_DISABLE;
+	isp->dma2_input.width = 0;
+	isp->dma2_input.height = 0;
+	isp->dma2_input.format = 0;
+	isp->dma2_input.bitwidth = 0;
+	isp->dma2_input.plane = 0;
+	isp->dma2_input.order = 0;
+	isp->dma2_input.buffer_number = 0;
+	isp->dma2_input.width = 0;
+	isp->dma2_input.err = DMA_INPUT_ERROR_NONE;
+	fimc_is_set_param_bit(is, PARAM_ISP_DMA2_INPUT);
+
+	isp->aa.cmd = ISP_AA_COMMAND_START;
+	isp->aa.target = ISP_AA_TARGET_AE | ISP_AA_TARGET_AWB;
+	fimc_is_set_param_bit(is, PARAM_ISP_AA);
+
+	if (!test_bit(PARAM_ISP_FLASH, p_index1))
+		__is_set_isp_flash(is, ISP_FLASH_COMMAND_DISABLE,
+						ISP_FLASH_REDEYE_DISABLE);
+
+	if (!test_bit(PARAM_ISP_AWB, p_index1))
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_AUTO, 0);
+
+	if (!test_bit(PARAM_ISP_IMAGE_EFFECT, p_index1))
+		__is_set_isp_effect(is, ISP_IMAGE_EFFECT_DISABLE);
+
+	if (!test_bit(PARAM_ISP_ISO, p_index1))
+		__is_set_isp_iso(is, ISP_ISO_COMMAND_AUTO, 0);
+
+	if (!test_bit(PARAM_ISP_ADJUST, p_index1)) {
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_CONTRAST, 0);
+		__is_set_isp_adjust(is,
+				ISP_ADJUST_COMMAND_MANUAL_SATURATION, 0);
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_SHARPNESS, 0);
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_EXPOSURE, 0);
+		__is_set_isp_adjust(is,
+				ISP_ADJUST_COMMAND_MANUAL_BRIGHTNESS, 0);
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_HUE, 0);
+	}
+
+	if (!test_bit(PARAM_ISP_METERING, p_index1)) {
+		__is_set_isp_metering(is, 0, ISP_METERING_COMMAND_CENTER);
+		__is_set_isp_metering(is, 1, 0);
+		__is_set_isp_metering(is, 2, 0);
+		__is_set_isp_metering(is, 3, 0);
+		__is_set_isp_metering(is, 4, 0);
+	}
+
+	if (!test_bit(PARAM_ISP_AFC, p_index1))
+		__is_set_isp_afc(is, ISP_AFC_COMMAND_AUTO, 0);
+
+	isp->otf_output.cmd = OTF_OUTPUT_COMMAND_ENABLE;
+	if (!test_bit(PARAM_ISP_OTF_OUTPUT, p_index1)) {
+		isp->otf_output.width = DEFAULT_PREVIEW_STILL_WIDTH;
+		isp->otf_output.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+		fimc_is_set_param_bit(is, PARAM_ISP_OTF_OUTPUT);
+	}
+	isp->otf_output.format = OTF_OUTPUT_FORMAT_YUV444;
+	isp->otf_output.bitwidth = 12;
+	isp->otf_output.order = 0;
+	isp->otf_output.err = OTF_OUTPUT_ERROR_NONE;
+
+	if (!test_bit(PARAM_ISP_DMA1_OUTPUT, p_index1)) {
+		isp->dma1_output.cmd = DMA_OUTPUT_COMMAND_DISABLE;
+		isp->dma1_output.width = 0;
+		isp->dma1_output.height = 0;
+		isp->dma1_output.format = 0;
+		isp->dma1_output.bitwidth = 0;
+		isp->dma1_output.plane = 0;
+		isp->dma1_output.order = 0;
+		isp->dma1_output.buffer_number = 0;
+		isp->dma1_output.buffer_address = 0;
+		isp->dma1_output.notify_dma_done = 0;
+		isp->dma1_output.dma_out_mask = 0;
+		isp->dma1_output.err = DMA_OUTPUT_ERROR_NONE;
+		fimc_is_set_param_bit(is, PARAM_ISP_DMA1_OUTPUT);
+	}
+
+	if (!test_bit(PARAM_ISP_DMA2_OUTPUT, p_index1)) {
+		isp->dma2_output.cmd = DMA_OUTPUT_COMMAND_DISABLE;
+		isp->dma2_output.width = 0;
+		isp->dma2_output.height = 0;
+		isp->dma2_output.format = 0;
+		isp->dma2_output.bitwidth = 0;
+		isp->dma2_output.plane = 0;
+		isp->dma2_output.order = 0;
+		isp->dma2_output.buffer_number = 0;
+		isp->dma2_output.buffer_address = 0;
+		isp->dma2_output.notify_dma_done = 0;
+		isp->dma2_output.dma_out_mask = 0;
+		isp->dma2_output.err = DMA_OUTPUT_ERROR_NONE;
+		fimc_is_set_param_bit(is, PARAM_ISP_DMA2_OUTPUT);
+	}
+
+	/* Sensor */
+	if (!test_bit(PARAM_SENSOR_FRAME_RATE, p_index1)) {
+		if (is->config_index == 0)
+			__is_set_sensor(is, 0);
+	}
+
+	/* DRC */
+	drc->control.cmd = CONTROL_COMMAND_START;
+	__is_set_drc_control(is, CONTROL_BYPASS_ENABLE);
+
+	drc->otf_input.cmd = OTF_INPUT_COMMAND_ENABLE;
+	if (!test_bit(PARAM_DRC_OTF_INPUT, p_index1)) {
+		drc->otf_input.width = DEFAULT_PREVIEW_STILL_WIDTH;
+		drc->otf_input.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+		fimc_is_set_param_bit(is, PARAM_DRC_OTF_INPUT);
+	}
+	drc->otf_input.format = OTF_INPUT_FORMAT_YUV444;
+	drc->otf_input.bitwidth = 12;
+	drc->otf_input.order = 0;
+	drc->otf_input.err = OTF_INPUT_ERROR_NONE;
+
+	drc->dma_input.cmd = DMA_INPUT_COMMAND_DISABLE;
+	drc->dma_input.width = 0;
+	drc->dma_input.height = 0;
+	drc->dma_input.format = 0;
+	drc->dma_input.bitwidth = 0;
+	drc->dma_input.plane = 0;
+	drc->dma_input.order = 0;
+	drc->dma_input.buffer_number = 0;
+	drc->dma_input.width = 0;
+	drc->dma_input.err = DMA_INPUT_ERROR_NONE;
+	fimc_is_set_param_bit(is, PARAM_DRC_DMA_INPUT);
+
+	drc->otf_output.cmd = OTF_OUTPUT_COMMAND_ENABLE;
+	if (!test_bit(PARAM_DRC_OTF_OUTPUT, p_index1)) {
+		drc->otf_output.width = DEFAULT_PREVIEW_STILL_WIDTH;
+		drc->otf_output.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+		fimc_is_set_param_bit(is, PARAM_DRC_OTF_OUTPUT);
+	}
+	drc->otf_output.format = OTF_OUTPUT_FORMAT_YUV444;
+	drc->otf_output.bitwidth = 8;
+	drc->otf_output.order = 0;
+	drc->otf_output.err = OTF_OUTPUT_ERROR_NONE;
+
+	/* FD */
+	__is_set_fd_control(is, CONTROL_COMMAND_STOP);
+	fd->control.bypass = CONTROL_BYPASS_DISABLE;
+
+	fd->otf_input.cmd = OTF_INPUT_COMMAND_ENABLE;
+	if (!test_bit((PARAM_FD_OTF_INPUT - 32), p_index2)) {
+		fd->otf_input.width = DEFAULT_PREVIEW_STILL_WIDTH;
+		fd->otf_input.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+		fimc_is_set_param_bit(is, PARAM_FD_OTF_INPUT);
+	}
+
+	fd->otf_input.format = OTF_INPUT_FORMAT_YUV444;
+	fd->otf_input.bitwidth = 8;
+	fd->otf_input.order = 0;
+	fd->otf_input.err = OTF_INPUT_ERROR_NONE;
+
+	fd->dma_input.cmd = DMA_INPUT_COMMAND_DISABLE;
+	fd->dma_input.width = 0;
+	fd->dma_input.height = 0;
+	fd->dma_input.format = 0;
+	fd->dma_input.bitwidth = 0;
+	fd->dma_input.plane = 0;
+	fd->dma_input.order = 0;
+	fd->dma_input.buffer_number = 0;
+	fd->dma_input.width = 0;
+	fd->dma_input.err = DMA_INPUT_ERROR_NONE;
+	fimc_is_set_param_bit(is, PARAM_FD_DMA_INPUT);
+
+	__is_set_fd_config_maxface(is, 5);
+	__is_set_fd_config_rollangle(is, FD_CONFIG_ROLL_ANGLE_FULL);
+	__is_set_fd_config_yawangle(is, FD_CONFIG_YAW_ANGLE_45_90);
+	__is_set_fd_config_smilemode(is, FD_CONFIG_SMILE_MODE_DISABLE);
+	__is_set_fd_config_blinkmode(is, FD_CONFIG_BLINK_MODE_DISABLE);
+	__is_set_fd_config_eyedetect(is, FD_CONFIG_EYES_DETECT_ENABLE);
+	__is_set_fd_config_mouthdetect(is, FD_CONFIG_MOUTH_DETECT_DISABLE);
+	__is_set_fd_config_orientation(is, FD_CONFIG_ORIENTATION_DISABLE);
+	__is_set_fd_config_orientation_val(is, 0);
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-is-param.h b/drivers/media/platform/exynos4-is/fimc-is-param.h
new file mode 100644
index 000000000000..f9358c27ae2d
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-param.h
@@ -0,0 +1,1020 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2011 - 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@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
+ * published by the Free Software Foundation.
+ */
+#ifndef FIMC_IS_PARAM_H_
+#define FIMC_IS_PARAM_H_
+
+#include <linux/compiler.h>
+
+#define FIMC_IS_CONFIG_TIMEOUT		3000 /* ms */
+#define IS_DEFAULT_WIDTH		1280
+#define IS_DEFAULT_HEIGHT		720
+
+#define DEFAULT_PREVIEW_STILL_WIDTH	IS_DEFAULT_WIDTH
+#define DEFAULT_PREVIEW_STILL_HEIGHT	IS_DEFAULT_HEIGHT
+#define DEFAULT_CAPTURE_STILL_WIDTH	IS_DEFAULT_WIDTH
+#define DEFAULT_CAPTURE_STILL_HEIGHT	IS_DEFAULT_HEIGHT
+#define DEFAULT_PREVIEW_VIDEO_WIDTH	IS_DEFAULT_WIDTH
+#define DEFAULT_PREVIEW_VIDEO_HEIGHT	IS_DEFAULT_HEIGHT
+#define DEFAULT_CAPTURE_VIDEO_WIDTH	IS_DEFAULT_WIDTH
+#define DEFAULT_CAPTURE_VIDEO_HEIGHT	IS_DEFAULT_HEIGHT
+
+#define DEFAULT_PREVIEW_STILL_FRAMERATE	30
+#define DEFAULT_CAPTURE_STILL_FRAMERATE	15
+#define DEFAULT_PREVIEW_VIDEO_FRAMERATE	30
+#define DEFAULT_CAPTURE_VIDEO_FRAMERATE	30
+
+#define FIMC_IS_REGION_VER		124 /* IS REGION VERSION 1.24 */
+#define FIMC_IS_PARAM_SIZE		(FIMC_IS_REGION_SIZE + 1)
+#define FIMC_IS_MAGIC_NUMBER		0x01020304
+#define FIMC_IS_PARAM_MAX_SIZE		64 /* in bytes */
+#define FIMC_IS_PARAM_MAX_ENTRIES	(FIMC_IS_PARAM_MAX_SIZE / 4)
+
+/* The parameter bitmask bit definitions. */
+enum is_param_bit {
+	PARAM_GLOBAL_SHOTMODE,
+	PARAM_SENSOR_CONTROL,
+	PARAM_SENSOR_OTF_OUTPUT,
+	PARAM_SENSOR_FRAME_RATE,
+	PARAM_BUFFER_CONTROL,
+	PARAM_BUFFER_OTF_INPUT,
+	PARAM_BUFFER_OTF_OUTPUT,
+	PARAM_ISP_CONTROL,
+	PARAM_ISP_OTF_INPUT,
+	PARAM_ISP_DMA1_INPUT,
+	/* 10 */
+	PARAM_ISP_DMA2_INPUT,
+	PARAM_ISP_AA,
+	PARAM_ISP_FLASH,
+	PARAM_ISP_AWB,
+	PARAM_ISP_IMAGE_EFFECT,
+	PARAM_ISP_ISO,
+	PARAM_ISP_ADJUST,
+	PARAM_ISP_METERING,
+	PARAM_ISP_AFC,
+	PARAM_ISP_OTF_OUTPUT,
+	/* 20 */
+	PARAM_ISP_DMA1_OUTPUT,
+	PARAM_ISP_DMA2_OUTPUT,
+	PARAM_DRC_CONTROL,
+	PARAM_DRC_OTF_INPUT,
+	PARAM_DRC_DMA_INPUT,
+	PARAM_DRC_OTF_OUTPUT,
+	PARAM_SCALERC_CONTROL,
+	PARAM_SCALERC_OTF_INPUT,
+	PARAM_SCALERC_IMAGE_EFFECT,
+	PARAM_SCALERC_INPUT_CROP,
+	/* 30 */
+	PARAM_SCALERC_OUTPUT_CROP,
+	PARAM_SCALERC_OTF_OUTPUT,
+	PARAM_SCALERC_DMA_OUTPUT,
+	PARAM_ODC_CONTROL,
+	PARAM_ODC_OTF_INPUT,
+	PARAM_ODC_OTF_OUTPUT,
+	PARAM_DIS_CONTROL,
+	PARAM_DIS_OTF_INPUT,
+	PARAM_DIS_OTF_OUTPUT,
+	PARAM_TDNR_CONTROL,
+	/* 40 */
+	PARAM_TDNR_OTF_INPUT,
+	PARAM_TDNR_1ST_FRAME,
+	PARAM_TDNR_OTF_OUTPUT,
+	PARAM_TDNR_DMA_OUTPUT,
+	PARAM_SCALERP_CONTROL,
+	PARAM_SCALERP_OTF_INPUT,
+	PARAM_SCALERP_IMAGE_EFFECT,
+	PARAM_SCALERP_INPUT_CROP,
+	PARAM_SCALERP_OUTPUT_CROP,
+	PARAM_SCALERP_ROTATION,
+	/* 50 */
+	PARAM_SCALERP_FLIP,
+	PARAM_SCALERP_OTF_OUTPUT,
+	PARAM_SCALERP_DMA_OUTPUT,
+	PARAM_FD_CONTROL,
+	PARAM_FD_OTF_INPUT,
+	PARAM_FD_DMA_INPUT,
+	PARAM_FD_CONFIG,
+};
+
+/* Interrupt map */
+#define	FIMC_IS_INT_GENERAL			0
+#define	FIMC_IS_INT_FRAME_DONE_ISP		1
+
+/* Input */
+
+#define CONTROL_COMMAND_STOP			0
+#define CONTROL_COMMAND_START			1
+
+#define CONTROL_BYPASS_DISABLE			0
+#define CONTROL_BYPASS_ENABLE			1
+
+#define CONTROL_ERROR_NONE			0
+
+/* OTF (On-The-Fly) input interface commands */
+#define OTF_INPUT_COMMAND_DISABLE		0
+#define OTF_INPUT_COMMAND_ENABLE		1
+
+/* OTF input interface color formats */
+enum oft_input_fmt {
+	OTF_INPUT_FORMAT_BAYER			= 0, /* 1 channel */
+	OTF_INPUT_FORMAT_YUV444			= 1, /* 3 channels */
+	OTF_INPUT_FORMAT_YUV422			= 2, /* 3 channels */
+	OTF_INPUT_FORMAT_YUV420			= 3, /* 3 channels */
+	OTF_INPUT_FORMAT_STRGEN_COLORBAR_BAYER	= 10,
+	OTF_INPUT_FORMAT_BAYER_DMA		= 11,
+};
+
+#define OTF_INPUT_ORDER_BAYER_GR_BG		0
+
+/* OTF input error codes */
+#define OTF_INPUT_ERROR_NONE			0 /* Input setting is done */
+
+/* DMA input commands */
+#define DMA_INPUT_COMMAND_DISABLE		0
+#define DMA_INPUT_COMMAND_ENABLE		1
+
+/* DMA input color formats */
+enum dma_input_fmt {
+	DMA_INPUT_FORMAT_BAYER			= 0,
+	DMA_INPUT_FORMAT_YUV444			= 1,
+	DMA_INPUT_FORMAT_YUV422			= 2,
+	DMA_INPUT_FORMAT_YUV420			= 3,
+};
+
+enum dma_input_order {
+	/* (for DMA_INPUT_PLANE_3) */
+	DMA_INPUT_ORDER_NO	= 0,
+	/* (only valid at DMA_INPUT_PLANE_2) */
+	DMA_INPUT_ORDER_CBCR	= 1,
+	/* (only valid at DMA_INPUT_PLANE_2) */
+	DMA_INPUT_ORDER_CRCB	= 2,
+	/* (only valid at DMA_INPUT_PLANE_1 & DMA_INPUT_FORMAT_YUV444) */
+	DMA_INPUT_ORDER_YCBCR	= 3,
+	/* (only valid at DMA_INPUT_FORMAT_YUV422 & DMA_INPUT_PLANE_1) */
+	DMA_INPUT_ORDER_YYCBCR	= 4,
+	/* (only valid at DMA_INPUT_FORMAT_YUV422 & DMA_INPUT_PLANE_1) */
+	DMA_INPUT_ORDER_YCBYCR	= 5,
+	/* (only valid at DMA_INPUT_FORMAT_YUV422 & DMA_INPUT_PLANE_1) */
+	DMA_INPUT_ORDER_YCRYCB	= 6,
+	/* (only valid at DMA_INPUT_FORMAT_YUV422 & DMA_INPUT_PLANE_1) */
+	DMA_INPUT_ORDER_CBYCRY	= 7,
+	/* (only valid at DMA_INPUT_FORMAT_YUV422 & DMA_INPUT_PLANE_1) */
+	DMA_INPUT_ORDER_CRYCBY	= 8,
+	/* (only valid at DMA_INPUT_FORMAT_BAYER) */
+	DMA_INPUT_ORDER_GR_BG	= 9
+};
+
+#define DMA_INPUT_ERROR_NONE			0 /* DMA input setting
+						     is done */
+/*
+ * Data output parameter definitions
+ */
+#define OTF_OUTPUT_CROP_DISABLE			0
+#define OTF_OUTPUT_CROP_ENABLE			1
+
+#define OTF_OUTPUT_COMMAND_DISABLE		0
+#define OTF_OUTPUT_COMMAND_ENABLE		1
+
+enum otf_output_fmt {
+	OTF_OUTPUT_FORMAT_YUV444		= 1,
+	OTF_OUTPUT_FORMAT_YUV422		= 2,
+	OTF_OUTPUT_FORMAT_YUV420		= 3,
+	OTF_OUTPUT_FORMAT_RGB			= 4,
+};
+
+#define OTF_OUTPUT_ORDER_BAYER_GR_BG		0
+
+#define OTF_OUTPUT_ERROR_NONE			0 /* Output Setting is done */
+
+#define DMA_OUTPUT_COMMAND_DISABLE		0
+#define DMA_OUTPUT_COMMAND_ENABLE		1
+
+enum dma_output_fmt {
+	DMA_OUTPUT_FORMAT_BAYER			= 0,
+	DMA_OUTPUT_FORMAT_YUV444		= 1,
+	DMA_OUTPUT_FORMAT_YUV422		= 2,
+	DMA_OUTPUT_FORMAT_YUV420		= 3,
+	DMA_OUTPUT_FORMAT_RGB			= 4,
+};
+
+enum dma_output_order {
+	DMA_OUTPUT_ORDER_NO		= 0,
+	/* for DMA_OUTPUT_PLANE_3 */
+	DMA_OUTPUT_ORDER_CBCR		= 1,
+	/* only valid at DMA_INPUT_PLANE_2) */
+	DMA_OUTPUT_ORDER_CRCB		= 2,
+	/* only valid at DMA_OUTPUT_PLANE_2) */
+	DMA_OUTPUT_ORDER_YYCBCR		= 3,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV422 & DMA_OUTPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_YCBYCR		= 4,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV422 & DMA_OUTPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_YCRYCB		= 5,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV422 & DMA_OUTPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CBYCRY		= 6,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV422 & DMA_OUTPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CRYCBY		= 7,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV422 & DMA_OUTPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_YCBCR		= 8,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CRYCB		= 9,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CRCBY		= 10,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CBYCR		= 11,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_YCRCB		= 12,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CBCRY		= 13,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_BGR		= 14,
+	/* only valid at DMA_OUTPUT_FORMAT_RGB */
+	DMA_OUTPUT_ORDER_GB_BG		= 15
+	/* only valid at DMA_OUTPUT_FORMAT_BAYER */
+};
+
+/* enum dma_output_notify_dma_done */
+#define DMA_OUTPUT_NOTIFY_DMA_DONE_DISABLE	0
+#define DMA_OUTPUT_NOTIFY_DMA_DONE_ENABLE	1
+
+/* DMA output error codes */
+#define DMA_OUTPUT_ERROR_NONE			0 /* DMA output setting
+						     is done */
+
+/* ----------------------  Global  ----------------------------------- */
+#define GLOBAL_SHOTMODE_ERROR_NONE		0 /* shot-mode setting
+						     is done */
+/* 3A lock commands */
+#define ISP_AA_COMMAND_START			0
+#define ISP_AA_COMMAND_STOP			1
+
+/* 3A lock target */
+#define ISP_AA_TARGET_AF			1
+#define ISP_AA_TARGET_AE			2
+#define ISP_AA_TARGET_AWB			4
+
+enum isp_af_mode {
+	ISP_AF_MODE_MANUAL			= 0,
+	ISP_AF_MODE_SINGLE			= 1,
+	ISP_AF_MODE_CONTINUOUS			= 2,
+	ISP_AF_MODE_TOUCH			= 3,
+	ISP_AF_MODE_SLEEP			= 4,
+	ISP_AF_MODE_INIT			= 5,
+	ISP_AF_MODE_SET_CENTER_WINDOW		= 6,
+	ISP_AF_MODE_SET_TOUCH_WINDOW		= 7
+};
+
+/* Face AF commands */
+#define ISP_AF_FACE_DISABLE			0
+#define ISP_AF_FACE_ENABLE			1
+
+/* AF range */
+#define ISP_AF_RANGE_NORMAL			0
+#define ISP_AF_RANGE_MACRO			1
+
+/* AF sleep */
+#define ISP_AF_SLEEP_OFF			0
+#define ISP_AF_SLEEP_ON				1
+
+/* Continuous AF commands */
+#define ISP_AF_CONTINUOUS_DISABLE		0
+#define ISP_AF_CONTINUOUS_ENABLE		1
+
+/* ISP AF error codes */
+#define ISP_AF_ERROR_NONE			0 /* AF mode change is done */
+#define ISP_AF_ERROR_NONE_LOCK_DONE		1 /* AF lock is done */
+
+/* Flash commands */
+#define ISP_FLASH_COMMAND_DISABLE		0
+#define ISP_FLASH_COMMAND_MANUAL_ON		1 /* (forced flash) */
+#define ISP_FLASH_COMMAND_AUTO			2
+#define ISP_FLASH_COMMAND_TORCH			3 /* 3 sec */
+
+/* Flash red-eye commads */
+#define ISP_FLASH_REDEYE_DISABLE		0
+#define ISP_FLASH_REDEYE_ENABLE			1
+
+/* Flash error codes */
+#define ISP_FLASH_ERROR_NONE			0 /* Flash setting is done */
+
+/* --------------------------  AWB  ------------------------------------ */
+enum isp_awb_command {
+	ISP_AWB_COMMAND_AUTO			= 0,
+	ISP_AWB_COMMAND_ILLUMINATION		= 1,
+	ISP_AWB_COMMAND_MANUAL			= 2
+};
+
+enum isp_awb_illumination {
+	ISP_AWB_ILLUMINATION_DAYLIGHT		= 0,
+	ISP_AWB_ILLUMINATION_CLOUDY		= 1,
+	ISP_AWB_ILLUMINATION_TUNGSTEN		= 2,
+	ISP_AWB_ILLUMINATION_FLUORESCENT	= 3
+};
+
+/* ISP AWN error codes */
+#define ISP_AWB_ERROR_NONE			0 /* AWB setting is done */
+
+/* --------------------------  Effect  ----------------------------------- */
+enum isp_imageeffect_command {
+	ISP_IMAGE_EFFECT_DISABLE		= 0,
+	ISP_IMAGE_EFFECT_MONOCHROME		= 1,
+	ISP_IMAGE_EFFECT_NEGATIVE_MONO		= 2,
+	ISP_IMAGE_EFFECT_NEGATIVE_COLOR		= 3,
+	ISP_IMAGE_EFFECT_SEPIA			= 4
+};
+
+/* Image effect error codes */
+#define ISP_IMAGE_EFFECT_ERROR_NONE		0 /* Image effect setting
+						     is done */
+/* ISO commands */
+#define ISP_ISO_COMMAND_AUTO			0
+#define ISP_ISO_COMMAND_MANUAL			1
+
+/* ISO error codes */
+#define ISP_ISO_ERROR_NONE			0 /* ISO setting is done */
+
+/* ISP adjust commands */
+#define ISP_ADJUST_COMMAND_AUTO			(0 << 0)
+#define ISP_ADJUST_COMMAND_MANUAL_CONTRAST	(1 << 0)
+#define ISP_ADJUST_COMMAND_MANUAL_SATURATION	(1 << 1)
+#define ISP_ADJUST_COMMAND_MANUAL_SHARPNESS	(1 << 2)
+#define ISP_ADJUST_COMMAND_MANUAL_EXPOSURE	(1 << 3)
+#define ISP_ADJUST_COMMAND_MANUAL_BRIGHTNESS	(1 << 4)
+#define ISP_ADJUST_COMMAND_MANUAL_HUE		(1 << 5)
+#define ISP_ADJUST_COMMAND_MANUAL_ALL		0x7f
+
+/* ISP adjustment error codes */
+#define ISP_ADJUST_ERROR_NONE			0 /* Adjust setting is done */
+
+/*
+ *  Exposure metering
+ */
+enum isp_metering_command {
+	ISP_METERING_COMMAND_AVERAGE	= 0,
+	ISP_METERING_COMMAND_SPOT	= 1,
+	ISP_METERING_COMMAND_MATRIX	= 2,
+	ISP_METERING_COMMAND_CENTER	= 3
+};
+
+/* ISP metering error codes */
+#define ISP_METERING_ERROR_NONE		0 /* Metering setting is done */
+
+/*
+ * AFC
+ */
+enum isp_afc_command {
+	ISP_AFC_COMMAND_DISABLE		= 0,
+	ISP_AFC_COMMAND_AUTO		= 1,
+	ISP_AFC_COMMAND_MANUAL		= 2,
+};
+
+#define ISP_AFC_MANUAL_50HZ		50
+#define ISP_AFC_MANUAL_60HZ		60
+
+/* ------------------------  SCENE MODE--------------------------------- */
+enum isp_scene_mode {
+	ISP_SCENE_NONE			= 0,
+	ISP_SCENE_PORTRAIT		= 1,
+	ISP_SCENE_LANDSCAPE		= 2,
+	ISP_SCENE_SPORTS		= 3,
+	ISP_SCENE_PARTYINDOOR		= 4,
+	ISP_SCENE_BEACHSNOW		= 5,
+	ISP_SCENE_SUNSET		= 6,
+	ISP_SCENE_DAWN			= 7,
+	ISP_SCENE_FALL			= 8,
+	ISP_SCENE_NIGHT			= 9,
+	ISP_SCENE_AGAINSTLIGHTWLIGHT	= 10,
+	ISP_SCENE_AGAINSTLIGHTWOLIGHT	= 11,
+	ISP_SCENE_FIRE			= 12,
+	ISP_SCENE_TEXT			= 13,
+	ISP_SCENE_CANDLE		= 14
+};
+
+/* AFC error codes */
+#define ISP_AFC_ERROR_NONE		0 /* AFC setting is done */
+
+/* ----------------------------  FD  ------------------------------------- */
+enum fd_config_command {
+	FD_CONFIG_COMMAND_MAXIMUM_NUMBER	= 0x1,
+	FD_CONFIG_COMMAND_ROLL_ANGLE		= 0x2,
+	FD_CONFIG_COMMAND_YAW_ANGLE		= 0x4,
+	FD_CONFIG_COMMAND_SMILE_MODE		= 0x8,
+	FD_CONFIG_COMMAND_BLINK_MODE		= 0x10,
+	FD_CONFIG_COMMAND_EYES_DETECT		= 0x20,
+	FD_CONFIG_COMMAND_MOUTH_DETECT		= 0x40,
+	FD_CONFIG_COMMAND_ORIENTATION		= 0x80,
+	FD_CONFIG_COMMAND_ORIENTATION_VALUE	= 0x100
+};
+
+enum fd_config_roll_angle {
+	FD_CONFIG_ROLL_ANGLE_BASIC		= 0,
+	FD_CONFIG_ROLL_ANGLE_PRECISE_BASIC	= 1,
+	FD_CONFIG_ROLL_ANGLE_SIDES		= 2,
+	FD_CONFIG_ROLL_ANGLE_PRECISE_SIDES	= 3,
+	FD_CONFIG_ROLL_ANGLE_FULL		= 4,
+	FD_CONFIG_ROLL_ANGLE_PRECISE_FULL	= 5,
+};
+
+enum fd_config_yaw_angle {
+	FD_CONFIG_YAW_ANGLE_0			= 0,
+	FD_CONFIG_YAW_ANGLE_45			= 1,
+	FD_CONFIG_YAW_ANGLE_90			= 2,
+	FD_CONFIG_YAW_ANGLE_45_90		= 3,
+};
+
+/* Smile mode configuration */
+#define FD_CONFIG_SMILE_MODE_DISABLE		0
+#define FD_CONFIG_SMILE_MODE_ENABLE		1
+
+/* Blink mode configuration */
+#define FD_CONFIG_BLINK_MODE_DISABLE		0
+#define FD_CONFIG_BLINK_MODE_ENABLE		1
+
+/* Eyes detection configuration */
+#define FD_CONFIG_EYES_DETECT_DISABLE		0
+#define FD_CONFIG_EYES_DETECT_ENABLE		1
+
+/* Mouth detection configuration */
+#define FD_CONFIG_MOUTH_DETECT_DISABLE		0
+#define FD_CONFIG_MOUTH_DETECT_ENABLE		1
+
+#define FD_CONFIG_ORIENTATION_DISABLE		0
+#define FD_CONFIG_ORIENTATION_ENABLE		1
+
+struct param_control {
+	u32 cmd;
+	u32 bypass;
+	u32 buffer_address;
+	u32 buffer_size;
+	u32 skip_frames; /* only valid at ISP */
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 6];
+	u32 err;
+};
+
+struct param_otf_input {
+	u32 cmd;
+	u32 width;
+	u32 height;
+	u32 format;
+	u32 bitwidth;
+	u32 order;
+	u32 crop_offset_x;
+	u32 crop_offset_y;
+	u32 crop_width;
+	u32 crop_height;
+	u32 frametime_min;
+	u32 frametime_max;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 13];
+	u32 err;
+};
+
+struct param_dma_input {
+	u32 cmd;
+	u32 width;
+	u32 height;
+	u32 format;
+	u32 bitwidth;
+	u32 plane;
+	u32 order;
+	u32 buffer_number;
+	u32 buffer_address;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 10];
+	u32 err;
+};
+
+struct param_otf_output {
+	u32 cmd;
+	u32 width;
+	u32 height;
+	u32 format;
+	u32 bitwidth;
+	u32 order;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 7];
+	u32 err;
+};
+
+struct param_dma_output {
+	u32 cmd;
+	u32 width;
+	u32 height;
+	u32 format;
+	u32 bitwidth;
+	u32 plane;
+	u32 order;
+	u32 buffer_number;
+	u32 buffer_address;
+	u32 notify_dma_done;
+	u32 dma_out_mask;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 12];
+	u32 err;
+};
+
+struct param_global_shotmode {
+	u32 cmd;
+	u32 skip_frames;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 3];
+	u32 err;
+};
+
+struct param_sensor_framerate {
+	u32 frame_rate;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 2];
+	u32 err;
+};
+
+struct param_isp_aa {
+	u32 cmd;
+	u32 target;
+	u32 mode;
+	u32 scene;
+	u32 sleep;
+	u32 face;
+	u32 touch_x;
+	u32 touch_y;
+	u32 manual_af_setting;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 10];
+	u32 err;
+};
+
+struct param_isp_flash {
+	u32 cmd;
+	u32 redeye;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 3];
+	u32 err;
+};
+
+struct param_isp_awb {
+	u32 cmd;
+	u32 illumination;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 3];
+	u32 err;
+};
+
+struct param_isp_imageeffect {
+	u32 cmd;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 2];
+	u32 err;
+};
+
+struct param_isp_iso {
+	u32 cmd;
+	u32 value;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 3];
+	u32 err;
+};
+
+struct param_isp_adjust {
+	u32 cmd;
+	s32 contrast;
+	s32 saturation;
+	s32 sharpness;
+	s32 exposure;
+	s32 brightness;
+	s32 hue;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 8];
+	u32 err;
+};
+
+struct param_isp_metering {
+	u32 cmd;
+	u32 win_pos_x;
+	u32 win_pos_y;
+	u32 win_width;
+	u32 win_height;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 6];
+	u32 err;
+};
+
+struct param_isp_afc {
+	u32 cmd;
+	u32 manual;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 3];
+	u32 err;
+};
+
+struct param_scaler_imageeffect {
+	u32 cmd;
+	u32 arbitrary_cb;
+	u32 arbitrary_cr;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 4];
+	u32 err;
+};
+
+struct param_scaler_input_crop {
+	u32 cmd;
+	u32 crop_offset_x;
+	u32 crop_offset_y;
+	u32 crop_width;
+	u32 crop_height;
+	u32 in_width;
+	u32 in_height;
+	u32 out_width;
+	u32 out_height;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 10];
+	u32 err;
+};
+
+struct param_scaler_output_crop {
+	u32 cmd;
+	u32 crop_offset_x;
+	u32 crop_offset_y;
+	u32 crop_width;
+	u32 crop_height;
+	u32 out_format;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 7];
+	u32 err;
+};
+
+struct param_scaler_rotation {
+	u32 cmd;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 2];
+	u32 err;
+};
+
+struct param_scaler_flip {
+	u32 cmd;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 2];
+	u32 err;
+};
+
+struct param_3dnr_1stframe {
+	u32 cmd;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 2];
+	u32 err;
+};
+
+struct param_fd_config {
+	u32 cmd;
+	u32 max_number;
+	u32 roll_angle;
+	u32 yaw_angle;
+	u32 smile_mode;
+	u32 blink_mode;
+	u32 eye_detect;
+	u32 mouth_detect;
+	u32 orientation;
+	u32 orientation_value;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 11];
+	u32 err;
+};
+
+struct global_param {
+	struct param_global_shotmode	shotmode;
+};
+
+struct sensor_param {
+	struct param_control		control;
+	struct param_otf_output		otf_output;
+	struct param_sensor_framerate	frame_rate;
+} __packed;
+
+struct buffer_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_otf_output		otf_output;
+} __packed;
+
+struct isp_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_dma_input		dma1_input;
+	struct param_dma_input		dma2_input;
+	struct param_isp_aa		aa;
+	struct param_isp_flash		flash;
+	struct param_isp_awb		awb;
+	struct param_isp_imageeffect	effect;
+	struct param_isp_iso		iso;
+	struct param_isp_adjust		adjust;
+	struct param_isp_metering	metering;
+	struct param_isp_afc		afc;
+	struct param_otf_output		otf_output;
+	struct param_dma_output		dma1_output;
+	struct param_dma_output		dma2_output;
+} __packed;
+
+struct drc_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_dma_input		dma_input;
+	struct param_otf_output		otf_output;
+} __packed;
+
+struct scalerc_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_scaler_imageeffect	effect;
+	struct param_scaler_input_crop	input_crop;
+	struct param_scaler_output_crop	output_crop;
+	struct param_otf_output		otf_output;
+	struct param_dma_output		dma_output;
+} __packed;
+
+struct odc_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_otf_output		otf_output;
+} __packed;
+
+struct dis_param {
+	struct param_control		control;
+	struct param_otf_output		otf_input;
+	struct param_otf_output		otf_output;
+} __packed;
+
+struct tdnr_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_3dnr_1stframe	frame;
+	struct param_otf_output		otf_output;
+	struct param_dma_output		dma_output;
+} __packed;
+
+struct scalerp_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_scaler_imageeffect	effect;
+	struct param_scaler_input_crop	input_crop;
+	struct param_scaler_output_crop	output_crop;
+	struct param_scaler_rotation	rotation;
+	struct param_scaler_flip	flip;
+	struct param_otf_output		otf_output;
+	struct param_dma_output		dma_output;
+} __packed;
+
+struct fd_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_dma_input		dma_input;
+	struct param_fd_config		config;
+} __packed;
+
+struct is_param_region {
+	struct global_param		global;
+	struct sensor_param		sensor;
+	struct buffer_param		buf;
+	struct isp_param		isp;
+	struct drc_param		drc;
+	struct scalerc_param		scalerc;
+	struct odc_param		odc;
+	struct dis_param		dis;
+	struct tdnr_param		tdnr;
+	struct scalerp_param		scalerp;
+	struct fd_param			fd;
+} __packed;
+
+#define NUMBER_OF_GAMMA_CURVE_POINTS	32
+
+struct is_tune_sensor {
+	u32 exposure;
+	u32 analog_gain;
+	u32 frame_rate;
+	u32 actuator_position;
+};
+
+struct is_tune_gammacurve {
+	u32 num_pts_x[NUMBER_OF_GAMMA_CURVE_POINTS];
+	u32 num_pts_y_r[NUMBER_OF_GAMMA_CURVE_POINTS];
+	u32 num_pts_y_g[NUMBER_OF_GAMMA_CURVE_POINTS];
+	u32 num_pts_y_b[NUMBER_OF_GAMMA_CURVE_POINTS];
+};
+
+struct is_tune_isp {
+	/* Brightness level: range 0...100, default 7. */
+	u32 brightness_level;
+	/* Contrast level: range -127...127, default 0. */
+	s32 contrast_level;
+	/* Saturation level: range -127...127, default 0. */
+	s32 saturation_level;
+	s32 gamma_level;
+	struct is_tune_gammacurve gamma_curve[4];
+	/* Hue: range -127...127, default 0. */
+	s32 hue;
+	/* Sharpness blur: range -127...127, default 0. */
+	s32 sharpness_blur;
+	/* Despeckle : range -127~127, default : 0 */
+	s32 despeckle;
+	/* Edge color supression: range -127...127, default 0. */
+	s32 edge_color_supression;
+	/* Noise reduction: range -127...127, default 0. */
+	s32 noise_reduction;
+	/* (32 * 4 + 9) * 4 = 548 bytes */
+} __packed;
+
+struct is_tune_region {
+	struct is_tune_sensor sensor;
+	struct is_tune_isp isp;
+} __packed;
+
+struct rational {
+	u32 num;
+	u32 den;
+};
+
+struct srational {
+	s32 num;
+	s32 den;
+};
+
+#define FLASH_FIRED_SHIFT			0
+#define FLASH_NOT_FIRED				0
+#define FLASH_FIRED				1
+
+#define FLASH_STROBE_SHIFT			1
+#define FLASH_STROBE_NO_DETECTION		0
+#define FLASH_STROBE_RESERVED			1
+#define FLASH_STROBE_RETURN_LIGHT_NOT_DETECTED	2
+#define FLASH_STROBE_RETURN_LIGHT_DETECTED	3
+
+#define FLASH_MODE_SHIFT			3
+#define FLASH_MODE_UNKNOWN			0
+#define FLASH_MODE_COMPULSORY_FLASH_FIRING	1
+#define FLASH_MODE_COMPULSORY_FLASH_SUPPRESSION	2
+#define FLASH_MODE_AUTO_MODE			3
+
+#define FLASH_FUNCTION_SHIFT			5
+#define FLASH_FUNCTION_PRESENT			0
+#define FLASH_FUNCTION_NONE			1
+
+#define FLASH_RED_EYE_SHIFT			6
+#define FLASH_RED_EYE_DISABLED			0
+#define FLASH_RED_EYE_SUPPORTED			1
+
+enum apex_aperture_value {
+	F1_0	= 0,
+	F1_4	= 1,
+	F2_0	= 2,
+	F2_8	= 3,
+	F4_0	= 4,
+	F5_6	= 5,
+	F8_9	= 6,
+	F11_0	= 7,
+	F16_0	= 8,
+	F22_0	= 9,
+	F32_0	= 10,
+};
+
+struct exif_attribute {
+	struct rational exposure_time;
+	struct srational shutter_speed;
+	u32 iso_speed_rating;
+	u32 flash;
+	struct srational brightness;
+} __packed;
+
+struct is_frame_header {
+	u32 valid;
+	u32 bad_mark;
+	u32 captured;
+	u32 frame_number;
+	struct exif_attribute exif;
+} __packed;
+
+struct is_fd_rect {
+	u32 offset_x;
+	u32 offset_y;
+	u32 width;
+	u32 height;
+};
+
+struct is_face_marker {
+	u32 frame_number;
+	struct is_fd_rect face;
+	struct is_fd_rect left_eye;
+	struct is_fd_rect right_eye;
+	struct is_fd_rect mouth;
+	u32 roll_angle;
+	u32 yaw_angle;
+	u32 confidence;
+	s32 smile_level;
+	s32 blink_level;
+} __packed;
+
+#define MAX_FRAME_COUNT				8
+#define MAX_FRAME_COUNT_PREVIEW			4
+#define MAX_FRAME_COUNT_CAPTURE			1
+#define MAX_FACE_COUNT				16
+#define MAX_SHARED_COUNT			500
+
+struct is_region {
+	struct is_param_region parameter;
+	struct is_tune_region tune;
+	struct is_frame_header header[MAX_FRAME_COUNT];
+	struct is_face_marker face[MAX_FACE_COUNT];
+	u32 shared[MAX_SHARED_COUNT];
+} __packed;
+
+struct is_debug_frame_descriptor {
+	u32 sensor_frame_time;
+	u32 sensor_exposure_time;
+	s32 sensor_analog_gain;
+	/* monitor for AA */
+	u32 req_lei;
+
+	u32 next_next_lei_exp;
+	u32 next_next_lei_a_gain;
+	u32 next_next_lei_d_gain;
+	u32 next_next_lei_statlei;
+	u32 next_next_lei_lei;
+
+	u32 dummy0;
+};
+
+#define MAX_FRAMEDESCRIPTOR_CONTEXT_NUM	(30*20)	/* 600 frames */
+#define MAX_VERSION_DISPLAY_BUF	32
+
+struct is_share_region {
+	u32 frame_time;
+	u32 exposure_time;
+	s32 analog_gain;
+
+	u32 r_gain;
+	u32 g_gain;
+	u32 b_gain;
+
+	u32 af_position;
+	u32 af_status;
+	/* 0 : SIRC_ISP_CAMERA_AUTOFOCUSMESSAGE_NOMESSAGE */
+	/* 1 : SIRC_ISP_CAMERA_AUTOFOCUSMESSAGE_REACHED */
+	/* 2 : SIRC_ISP_CAMERA_AUTOFOCUSMESSAGE_UNABLETOREACH */
+	/* 3 : SIRC_ISP_CAMERA_AUTOFOCUSMESSAGE_LOST */
+	/* default : unknown */
+	u32 af_scene_type;
+
+	u32 frame_descp_onoff_control;
+	u32 frame_descp_update_done;
+	u32 frame_descp_idx;
+	u32 frame_descp_max_idx;
+	struct is_debug_frame_descriptor
+		dbg_frame_descp_ctx[MAX_FRAMEDESCRIPTOR_CONTEXT_NUM];
+
+	u32 chip_id;
+	u32 chip_rev_no;
+	u8 isp_fw_ver_no[MAX_VERSION_DISPLAY_BUF];
+	u8 isp_fw_ver_date[MAX_VERSION_DISPLAY_BUF];
+	u8 sirc_sdk_ver_no[MAX_VERSION_DISPLAY_BUF];
+	u8 sirc_sdk_rev_no[MAX_VERSION_DISPLAY_BUF];
+	u8 sirc_sdk_rev_date[MAX_VERSION_DISPLAY_BUF];
+} __packed;
+
+struct is_debug_control {
+	u32 write_point;	/* 0~ 500KB boundary */
+	u32 assert_flag;	/* 0: Not invoked, 1: Invoked */
+	u32 pabort_flag;	/* 0: Not invoked, 1: Invoked */
+	u32 dabort_flag;	/* 0: Not invoked, 1: Invoked */
+};
+
+struct sensor_open_extended {
+	u32 actuator_type;
+	u32 mclk;
+	u32 mipi_lane_num;
+	u32 mipi_speed;
+	/* Skip setfile loading when fast_open_sensor is not 0 */
+	u32 fast_open_sensor;
+	/* Activating sensor self calibration mode (6A3) */
+	u32 self_calibration_mode;
+	/* This field is to adjust I2c clock based on ACLK200 */
+	/* This value is varied in case of rev 0.2 */
+	u32 i2c_sclk;
+};
+
+struct fimc_is;
+
+int fimc_is_hw_get_sensor_max_framerate(struct fimc_is *is);
+void fimc_is_set_initial_params(struct fimc_is *is);
+unsigned int __get_pending_param_count(struct fimc_is *is);
+
+int  __is_hw_update_params(struct fimc_is *is);
+void __is_get_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf);
+void __is_set_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf);
+void __is_set_sensor(struct fimc_is *is, int fps);
+void __is_set_isp_aa_ae(struct fimc_is *is);
+void __is_set_isp_flash(struct fimc_is *is, u32 cmd, u32 redeye);
+void __is_set_isp_awb(struct fimc_is *is, u32 cmd, u32 val);
+void __is_set_isp_effect(struct fimc_is *is, u32 cmd);
+void __is_set_isp_iso(struct fimc_is *is, u32 cmd, u32 val);
+void __is_set_isp_adjust(struct fimc_is *is, u32 cmd, u32 val);
+void __is_set_isp_metering(struct fimc_is *is, u32 id, u32 val);
+void __is_set_isp_afc(struct fimc_is *is, u32 cmd, u32 val);
+void __is_set_drc_control(struct fimc_is *is, u32 val);
+void __is_set_fd_control(struct fimc_is *is, u32 val);
+void __is_set_fd_config_maxface(struct fimc_is *is, u32 val);
+void __is_set_fd_config_rollangle(struct fimc_is *is, u32 val);
+void __is_set_fd_config_yawangle(struct fimc_is *is, u32 val);
+void __is_set_fd_config_smilemode(struct fimc_is *is, u32 val);
+void __is_set_fd_config_blinkmode(struct fimc_is *is, u32 val);
+void __is_set_fd_config_eyedetect(struct fimc_is *is, u32 val);
+void __is_set_fd_config_mouthdetect(struct fimc_is *is, u32 val);
+void __is_set_fd_config_orientation(struct fimc_is *is, u32 val);
+void __is_set_fd_config_orientation_val(struct fimc_is *is, u32 val);
+void __is_set_isp_aa_af_mode(struct fimc_is *is, int cmd);
+void __is_set_isp_aa_af_start_stop(struct fimc_is *is, int cmd);
+
+#endif
diff --git a/drivers/media/platform/exynos4-is/fimc-is-regs.c b/drivers/media/platform/exynos4-is/fimc-is-regs.c
new file mode 100644
index 000000000000..b0ff67bc1b05
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-regs.c
@@ -0,0 +1,243 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2012 - 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@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
+ * published by the Free Software Foundation.
+ */
+#include <linux/delay.h>
+
+#include "fimc-is.h"
+#include "fimc-is-command.h"
+#include "fimc-is-regs.h"
+#include "fimc-is-sensor.h"
+
+void fimc_is_fw_clear_irq1(struct fimc_is *is, unsigned int nr)
+{
+	mcuctl_write(1UL << nr, is, MCUCTL_REG_INTCR1);
+}
+
+void fimc_is_fw_clear_irq2(struct fimc_is *is)
+{
+	u32 cfg = mcuctl_read(is, MCUCTL_REG_INTSR2);
+	mcuctl_write(cfg, is, MCUCTL_REG_INTCR2);
+}
+
+void fimc_is_hw_set_intgr0_gd0(struct fimc_is *is)
+{
+	mcuctl_write(INTGR0_INTGD(0), is, MCUCTL_REG_INTGR0);
+}
+
+int fimc_is_hw_wait_intsr0_intsd0(struct fimc_is *is)
+{
+	unsigned int timeout = 2000;
+	u32 cfg, status;
+
+	cfg = mcuctl_read(is, MCUCTL_REG_INTSR0);
+	status = INTSR0_GET_INTSD(0, cfg);
+
+	while (status) {
+		cfg = mcuctl_read(is, MCUCTL_REG_INTSR0);
+		status = INTSR0_GET_INTSD(0, cfg);
+		if (timeout == 0) {
+			dev_warn(&is->pdev->dev, "%s timeout\n",
+				 __func__);
+			return -ETIME;
+		}
+		timeout--;
+		udelay(1);
+	}
+	return 0;
+}
+
+int fimc_is_hw_wait_intmsr0_intmsd0(struct fimc_is *is)
+{
+	unsigned int timeout = 2000;
+	u32 cfg, status;
+
+	cfg = mcuctl_read(is, MCUCTL_REG_INTMSR0);
+	status = INTMSR0_GET_INTMSD(0, cfg);
+
+	while (status) {
+		cfg = mcuctl_read(is, MCUCTL_REG_INTMSR0);
+		status = INTMSR0_GET_INTMSD(0, cfg);
+		if (timeout == 0) {
+			dev_warn(&is->pdev->dev, "%s timeout\n",
+				 __func__);
+			return -ETIME;
+		}
+		timeout--;
+		udelay(1);
+	}
+	return 0;
+}
+
+int fimc_is_hw_set_param(struct fimc_is *is)
+{
+	struct chain_config *config = &is->config[is->config_index];
+	unsigned int param_count = __get_pending_param_count(is);
+
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+
+	mcuctl_write(HIC_SET_PARAMETER, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(is->config_index, is, MCUCTL_REG_ISSR(2));
+
+	mcuctl_write(param_count, is, MCUCTL_REG_ISSR(3));
+	mcuctl_write(config->p_region_index1, is, MCUCTL_REG_ISSR(4));
+	mcuctl_write(config->p_region_index2, is, MCUCTL_REG_ISSR(5));
+
+	fimc_is_hw_set_intgr0_gd0(is);
+	return 0;
+}
+
+int fimc_is_hw_set_tune(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+
+	mcuctl_write(HIC_SET_TUNE, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(is->h2i_cmd.entry_id, is, MCUCTL_REG_ISSR(2));
+
+	fimc_is_hw_set_intgr0_gd0(is);
+	return 0;
+}
+
+#define FIMC_IS_MAX_PARAMS	4
+
+int fimc_is_hw_get_params(struct fimc_is *is, unsigned int num_args)
+{
+	int i;
+
+	if (num_args > FIMC_IS_MAX_PARAMS)
+		return -EINVAL;
+
+	is->i2h_cmd.num_args = num_args;
+
+	for (i = 0; i < FIMC_IS_MAX_PARAMS; i++) {
+		if (i < num_args)
+			is->i2h_cmd.args[i] = mcuctl_read(is,
+					MCUCTL_REG_ISSR(12 + i));
+		else
+			is->i2h_cmd.args[i] = 0;
+	}
+	return 0;
+}
+
+void fimc_is_hw_set_sensor_num(struct fimc_is *is)
+{
+	pr_debug("setting sensor index to: %d\n", is->sensor_index);
+
+	mcuctl_write(IH_REPLY_DONE, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(IHC_GET_SENSOR_NUM, is, MCUCTL_REG_ISSR(2));
+	mcuctl_write(FIMC_IS_SENSOR_NUM, is, MCUCTL_REG_ISSR(3));
+}
+
+void fimc_is_hw_close_sensor(struct fimc_is *is, unsigned int index)
+{
+	if (is->sensor_index != index)
+		return;
+
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_CLOSE_SENSOR, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(2));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+void fimc_is_hw_get_setfile_addr(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_GET_SET_FILE_ADDR, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+void fimc_is_hw_load_setfile(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_LOAD_SET_FILE, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+int fimc_is_hw_change_mode(struct fimc_is *is)
+{
+	const u8 cmd[] = {
+		HIC_PREVIEW_STILL, HIC_PREVIEW_VIDEO,
+		HIC_CAPTURE_STILL, HIC_CAPTURE_VIDEO,
+	};
+
+	if (WARN_ON(is->config_index > ARRAY_SIZE(cmd)))
+		return -EINVAL;
+
+	mcuctl_write(cmd[is->config_index], is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(is->setfile.sub_index, is, MCUCTL_REG_ISSR(2));
+	fimc_is_hw_set_intgr0_gd0(is);
+	return 0;
+}
+
+void fimc_is_hw_stream_on(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_STREAM_ON, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(0, is, MCUCTL_REG_ISSR(2));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+void fimc_is_hw_stream_off(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_STREAM_OFF, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+void fimc_is_hw_subip_power_off(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_POWER_DOWN, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+int fimc_is_itf_s_param(struct fimc_is *is, bool update)
+{
+	int ret;
+
+	if (update)
+		__is_hw_update_params(is);
+
+	fimc_is_mem_barrier();
+
+	clear_bit(IS_ST_BLOCK_CMD_CLEARED, &is->state);
+	fimc_is_hw_set_param(is);
+	ret = fimc_is_wait_event(is, IS_ST_BLOCK_CMD_CLEARED, 1,
+				FIMC_IS_CONFIG_TIMEOUT);
+	if (ret < 0)
+		dev_err(&is->pdev->dev, "%s() timeout\n", __func__);
+
+	return ret;
+}
+
+int fimc_is_itf_mode_change(struct fimc_is *is)
+{
+	int ret;
+
+	clear_bit(IS_ST_CHANGE_MODE, &is->state);
+	fimc_is_hw_change_mode(is);
+	ret = fimc_is_wait_event(is, IS_ST_CHANGE_MODE, 1,
+				FIMC_IS_CONFIG_TIMEOUT);
+	if (!ret < 0)
+		dev_err(&is->pdev->dev, "%s(): mode change (%d) timeout\n",
+			__func__, is->config_index);
+	return ret;
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-is-regs.h b/drivers/media/platform/exynos4-is/fimc-is-regs.h
new file mode 100644
index 000000000000..5fa2fda46742
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-regs.h
@@ -0,0 +1,164 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *          Younghwan Joo <yhwan.joo@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 FIMC_IS_REG_H_
+#define FIMC_IS_REG_H_
+
+/* WDT_ISP register */
+#define REG_WDT_ISP			0x00170000
+
+/* MCUCTL registers base offset */
+#define MCUCTL_BASE			0x00180000
+
+/* MCU Controller Register */
+#define MCUCTL_REG_MCUCTRL		(MCUCTL_BASE + 0x00)
+#define MCUCTRL_MSWRST			(1 << 0)
+
+/* Boot Base Offset Address Register */
+#define MCUCTL_REG_BBOAR		(MCUCTL_BASE + 0x04)
+
+/* Interrupt Generation Register 0 from Host CPU to VIC */
+#define MCUCTL_REG_INTGR0		(MCUCTL_BASE + 0x08)
+/* __n = 0...9 */
+#define INTGR0_INTGC(__n)		(1 << ((__n) + 16))
+/* __n = 0...5 */
+#define INTGR0_INTGD(__n)		(1 << (__n))
+
+/* Interrupt Clear Register 0 from Host CPU to VIC */
+#define MCUCTL_REG_INTCR0		(MCUCTL_BASE + 0x0c)
+/* __n = 0...9 */
+#define INTCR0_INTGC(__n)		(1 << ((__n) + 16))
+/* __n = 0...5 */
+#define INTCR0_INTCD(__n)		(1 << ((__n) + 16))
+
+/* Interrupt Mask Register 0 from Host CPU to VIC */
+#define MCUCTL_REG_INTMR0		(MCUCTL_BASE + 0x10)
+/* __n = 0...9 */
+#define INTMR0_INTMC(__n)		(1 << ((__n) + 16))
+/* __n = 0...5 */
+#define INTMR0_INTMD(__n)		(1 << (__n))
+
+/* Interrupt Status Register 0 from Host CPU to VIC */
+#define MCUCTL_REG_INTSR0		(MCUCTL_BASE + 0x14)
+/* __n (bit number) = 0...4 */
+#define INTSR0_GET_INTSD(x, __n)	(((x) >> (__n)) & 0x1)
+/* __n (bit number) = 0...9 */
+#define INTSR0_GET_INTSC(x, __n)	(((x) >> ((__n) + 16)) & 0x1)
+
+/* Interrupt Mask Status Register 0 from Host CPU to VIC */
+#define MCUCTL_REG_INTMSR0		(MCUCTL_BASE + 0x18)
+/* __n (bit number) = 0...4 */
+#define INTMSR0_GET_INTMSD(x, __n)	(((x) >> (__n)) & 0x1)
+/* __n (bit number) = 0...9 */
+#define INTMSR0_GET_INTMSC(x, __n)	(((x) >> ((__n) + 16)) & 0x1)
+
+/* Interrupt Generation Register 1 from ISP CPU to Host IC */
+#define MCUCTL_REG_INTGR1		(MCUCTL_BASE + 0x1c)
+/* __n = 0...9 */
+#define INTGR1_INTGC(__n)		(1 << (__n))
+
+/* Interrupt Clear Register 1 from ISP CPU to Host IC */
+#define MCUCTL_REG_INTCR1		(MCUCTL_BASE + 0x20)
+/* __n = 0...9 */
+#define INTCR1_INTCC(__n)		(1 << (__n))
+
+/* Interrupt Mask Register 1 from ISP CPU to Host IC */
+#define MCUCTL_REG_INTMR1		(MCUCTL_BASE + 0x24)
+/* __n = 0...9 */
+#define INTMR1_INTMC(__n)		(1 << (__n))
+
+/* Interrupt Status Register 1 from ISP CPU to Host IC */
+#define MCUCTL_REG_INTSR1		(MCUCTL_BASE + 0x28)
+/* Interrupt Mask Status Register 1 from ISP CPU to Host IC */
+#define MCUCTL_REG_INTMSR1		(MCUCTL_BASE + 0x2c)
+
+/* Interrupt Clear Register 2 from ISP BLK's interrupts to Host IC */
+#define MCUCTL_REG_INTCR2		(MCUCTL_BASE + 0x30)
+/* __n = 0...5 */
+#define INTCR2_INTCC(__n)		(1 << ((__n) + 16))
+
+/* Interrupt Mask Register 2 from ISP BLK's interrupts to Host IC */
+#define MCUCTL_REG_INTMR2		(MCUCTL_BASE + 0x34)
+/* __n = 0...25 */
+#define INTMR2_INTMCIS(__n)		(1 << (__n))
+
+/* Interrupt Status Register 2 from ISP BLK's interrupts to Host IC */
+#define MCUCTL_REG_INTSR2		(MCUCTL_BASE + 0x38)
+/* Interrupt Mask Status Register 2 from ISP BLK's interrupts to Host IC */
+#define MCUCTL_REG_INTMSR2		(MCUCTL_BASE + 0x3c)
+
+/* General Purpose Output Control Register (0~17) */
+#define MCUCTL_REG_GPOCTLR		(MCUCTL_BASE + 0x40)
+/* __n = 0...17 */
+#define GPOCTLR_GPOG(__n)		(1 << (__n))
+
+/* General Purpose Pad Output Enable Register (0~17) */
+#define MCUCTL_REG_GPOENCTLR		(MCUCTL_BASE + 0x44)
+/* __n = 0...17 */
+#define GPOENCTLR_GPOEN(__n)		(1 << (__n))
+
+/* General Purpose Input Control Register (0~17) */
+#define MCUCTL_REG_GPICTLR		(MCUCTL_BASE + 0x48)
+
+/* Shared registers between ISP CPU and the host CPU - ISSRxx */
+
+/* ISSR(1): Command Host -> IS */
+/* ISSR(1): Sensor ID for Command, ISSR2...5 = Parameter 1...4 */
+
+/* ISSR(10): Reply IS -> Host */
+/* ISSR(11): Sensor ID for Reply, ISSR12...15 = Parameter 1...4 */
+
+/* ISSR(20): ISP_FRAME_DONE : SENSOR ID */
+/* ISSR(21): ISP_FRAME_DONE : PARAMETER 1 */
+
+/* ISSR(24): SCALERC_FRAME_DONE : SENSOR ID */
+/* ISSR(25): SCALERC_FRAME_DONE : PARAMETER 1 */
+
+/* ISSR(28): 3DNR_FRAME_DONE : SENSOR ID */
+/* ISSR(29): 3DNR_FRAME_DONE : PARAMETER 1 */
+
+/* ISSR(32): SCALERP_FRAME_DONE : SENSOR ID */
+/* ISSR(33): SCALERP_FRAME_DONE : PARAMETER 1 */
+
+/* __n = 0...63 */
+#define MCUCTL_REG_ISSR(__n)		(MCUCTL_BASE + 0x80 + ((__n) * 4))
+
+/* PMU ISP register offsets */
+#define REG_CMU_RESET_ISP_SYS_PWR_REG	0x1174
+#define REG_CMU_SYSCLK_ISP_SYS_PWR_REG	0x13b8
+#define REG_PMU_ISP_ARM_SYS		0x1050
+#define REG_PMU_ISP_ARM_CONFIGURATION	0x2280
+#define REG_PMU_ISP_ARM_STATUS		0x2284
+#define REG_PMU_ISP_ARM_OPTION		0x2288
+
+void fimc_is_fw_clear_irq1(struct fimc_is *is, unsigned int bit);
+void fimc_is_fw_clear_irq2(struct fimc_is *is);
+int fimc_is_hw_get_params(struct fimc_is *is, unsigned int num);
+
+void fimc_is_hw_set_intgr0_gd0(struct fimc_is *is);
+int fimc_is_hw_wait_intsr0_intsd0(struct fimc_is *is);
+int fimc_is_hw_wait_intmsr0_intmsd0(struct fimc_is *is);
+void fimc_is_hw_set_sensor_num(struct fimc_is *is);
+void fimc_is_hw_stream_on(struct fimc_is *is);
+void fimc_is_hw_stream_off(struct fimc_is *is);
+int fimc_is_hw_set_param(struct fimc_is *is);
+int fimc_is_hw_change_mode(struct fimc_is *is);
+
+void fimc_is_hw_close_sensor(struct fimc_is *is, unsigned int index);
+void fimc_is_hw_get_setfile_addr(struct fimc_is *is);
+void fimc_is_hw_load_setfile(struct fimc_is *is);
+void fimc_is_hw_subip_power_off(struct fimc_is *is);
+
+int fimc_is_itf_s_param(struct fimc_is *is, bool update);
+int fimc_is_itf_mode_change(struct fimc_is *is);
+
+#endif /* FIMC_IS_REG_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-is-sensor.c b/drivers/media/platform/exynos4-is/fimc-is-sensor.c
new file mode 100644
index 000000000000..6647421e5d3a
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-sensor.c
@@ -0,0 +1,305 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Author: 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
+ * published by the Free Software Foundation.
+ */
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_gpio.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <media/v4l2-subdev.h>
+
+#include "fimc-is.h"
+#include "fimc-is-sensor.h"
+
+#define DRIVER_NAME "FIMC-IS-SENSOR"
+
+static const char * const sensor_supply_names[] = {
+	"svdda",
+	"svddio",
+};
+
+static const struct v4l2_mbus_framefmt fimc_is_sensor_formats[] = {
+	{
+		.code = V4L2_MBUS_FMT_SGRBG10_1X10,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.field = V4L2_FIELD_NONE,
+	}
+};
+
+static const struct v4l2_mbus_framefmt *find_sensor_format(
+	struct v4l2_mbus_framefmt *mf)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(fimc_is_sensor_formats); i++)
+		if (mf->code == fimc_is_sensor_formats[i].code)
+			return &fimc_is_sensor_formats[i];
+
+	return &fimc_is_sensor_formats[0];
+}
+
+static int fimc_is_sensor_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(fimc_is_sensor_formats))
+		return -EINVAL;
+
+	code->code = fimc_is_sensor_formats[code->index].code;
+	return 0;
+}
+
+static void fimc_is_sensor_try_format(struct fimc_is_sensor *sensor,
+				      struct v4l2_mbus_framefmt *mf)
+{
+	const struct sensor_drv_data *dd = sensor->drvdata;
+	const struct v4l2_mbus_framefmt *fmt;
+
+	fmt = find_sensor_format(mf);
+	mf->code = fmt->code;
+	v4l_bound_align_image(&mf->width, 16 + 8, dd->width, 0,
+			      &mf->height, 12 + 8, dd->height, 0, 0);
+}
+
+static struct v4l2_mbus_framefmt *__fimc_is_sensor_get_format(
+		struct fimc_is_sensor *sensor, struct v4l2_subdev_fh *fh,
+		u32 pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return fh ? v4l2_subdev_get_try_format(fh, pad) : NULL;
+
+	return &sensor->format;
+}
+
+static int fimc_is_sensor_set_fmt(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_format *fmt)
+{
+	struct fimc_is_sensor *sensor = sd_to_fimc_is_sensor(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	fimc_is_sensor_try_format(sensor, &fmt->format);
+
+	mf = __fimc_is_sensor_get_format(sensor, fh, fmt->pad, fmt->which);
+	if (mf) {
+		mutex_lock(&sensor->lock);
+		if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+			*mf = fmt->format;
+		mutex_unlock(&sensor->lock);
+	}
+	return 0;
+}
+
+static int fimc_is_sensor_get_fmt(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_format *fmt)
+{
+	struct fimc_is_sensor *sensor = sd_to_fimc_is_sensor(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	mf = __fimc_is_sensor_get_format(sensor, fh, fmt->pad, fmt->which);
+
+	mutex_lock(&sensor->lock);
+	fmt->format = *mf;
+	mutex_unlock(&sensor->lock);
+	return 0;
+}
+
+static struct v4l2_subdev_pad_ops fimc_is_sensor_pad_ops = {
+	.enum_mbus_code	= fimc_is_sensor_enum_mbus_code,
+	.get_fmt	= fimc_is_sensor_get_fmt,
+	.set_fmt	= fimc_is_sensor_set_fmt,
+};
+
+static int fimc_is_sensor_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *format = v4l2_subdev_get_try_format(fh, 0);
+
+	*format		= fimc_is_sensor_formats[0];
+	format->width	= FIMC_IS_SENSOR_DEF_PIX_WIDTH;
+	format->height	= FIMC_IS_SENSOR_DEF_PIX_HEIGHT;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops fimc_is_sensor_sd_internal_ops = {
+	.open = fimc_is_sensor_open,
+};
+
+static int fimc_is_sensor_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct fimc_is_sensor *sensor = sd_to_fimc_is_sensor(sd);
+	int gpio = sensor->gpio_reset;
+	int ret;
+
+	if (on) {
+		ret = pm_runtime_get(sensor->dev);
+		if (ret < 0)
+			return ret;
+
+		ret = regulator_bulk_enable(SENSOR_NUM_SUPPLIES,
+					    sensor->supplies);
+		if (ret < 0) {
+			pm_runtime_put(sensor->dev);
+			return ret;
+		}
+		if (gpio_is_valid(gpio)) {
+			gpio_set_value(gpio, 1);
+			usleep_range(600, 800);
+			gpio_set_value(gpio, 0);
+			usleep_range(10000, 11000);
+			gpio_set_value(gpio, 1);
+		}
+
+		/* A delay needed for the sensor initialization. */
+		msleep(20);
+	} else {
+		if (gpio_is_valid(gpio))
+			gpio_set_value(gpio, 0);
+
+		ret = regulator_bulk_disable(SENSOR_NUM_SUPPLIES,
+					     sensor->supplies);
+		if (!ret)
+			pm_runtime_put(sensor->dev);
+	}
+
+	pr_info("%s:%d: on: %d, ret: %d\n", __func__, __LINE__, on, ret);
+
+	return ret;
+}
+
+static struct v4l2_subdev_core_ops fimc_is_sensor_core_ops = {
+	.s_power = fimc_is_sensor_s_power,
+};
+
+static struct v4l2_subdev_ops fimc_is_sensor_subdev_ops = {
+	.core = &fimc_is_sensor_core_ops,
+	.pad = &fimc_is_sensor_pad_ops,
+};
+
+static const struct of_device_id fimc_is_sensor_of_match[];
+
+static int fimc_is_sensor_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
+{
+	struct device *dev = &client->dev;
+	struct fimc_is_sensor *sensor;
+	const struct of_device_id *of_id;
+	struct v4l2_subdev *sd;
+	int gpio, i, ret;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	mutex_init(&sensor->lock);
+	sensor->gpio_reset = -EINVAL;
+
+	gpio = of_get_gpio_flags(dev->of_node, 0, NULL);
+	if (gpio_is_valid(gpio)) {
+		ret = devm_gpio_request_one(dev, gpio, GPIOF_OUT_INIT_LOW,
+							DRIVER_NAME);
+		if (ret < 0)
+			return ret;
+	}
+	sensor->gpio_reset = gpio;
+
+	for (i = 0; i < SENSOR_NUM_SUPPLIES; i++)
+		sensor->supplies[i].supply = sensor_supply_names[i];
+
+	ret = devm_regulator_bulk_get(&client->dev, SENSOR_NUM_SUPPLIES,
+				      sensor->supplies);
+	if (ret < 0)
+		return ret;
+
+	of_id = of_match_node(fimc_is_sensor_of_match, dev->of_node);
+	if (!of_id)
+		return -ENODEV;
+
+	sensor->drvdata = of_id->data;
+	sensor->dev = dev;
+
+	sd = &sensor->subdev;
+	v4l2_i2c_subdev_init(sd, client, &fimc_is_sensor_subdev_ops);
+	snprintf(sd->name, sizeof(sd->name), sensor->drvdata->subdev_name);
+	sensor->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	sensor->format.code = fimc_is_sensor_formats[0].code;
+	sensor->format.width = FIMC_IS_SENSOR_DEF_PIX_WIDTH;
+	sensor->format.height = FIMC_IS_SENSOR_DEF_PIX_HEIGHT;
+
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&sd->entity, 1, &sensor->pad, 0);
+	if (ret < 0)
+		return ret;
+
+	pm_runtime_no_callbacks(dev);
+	pm_runtime_enable(dev);
+
+	return ret;
+}
+
+static int fimc_is_sensor_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	media_entity_cleanup(&sd->entity);
+	return 0;
+}
+
+static const struct i2c_device_id fimc_is_sensor_ids[] = {
+	{ }
+};
+
+static const struct sensor_drv_data s5k6a3_drvdata = {
+	.id		= FIMC_IS_SENSOR_ID_S5K6A3,
+	.subdev_name	= "S5K6A3",
+	.width		= S5K6A3_SENSOR_WIDTH,
+	.height		= S5K6A3_SENSOR_HEIGHT,
+};
+
+static const struct of_device_id fimc_is_sensor_of_match[] = {
+	{
+		.compatible	= "samsung,s5k6a3",
+		.data		= &s5k6a3_drvdata,
+	},
+	{  }
+};
+
+static struct i2c_driver fimc_is_sensor_driver = {
+	.driver = {
+		.of_match_table	= fimc_is_sensor_of_match,
+		.name		= DRIVER_NAME,
+		.owner		= THIS_MODULE,
+	},
+	.probe		= fimc_is_sensor_probe,
+	.remove		= fimc_is_sensor_remove,
+	.id_table	= fimc_is_sensor_ids,
+};
+
+int fimc_is_register_sensor_driver(void)
+{
+	return i2c_add_driver(&fimc_is_sensor_driver);
+}
+
+void fimc_is_unregister_sensor_driver(void)
+{
+	i2c_del_driver(&fimc_is_sensor_driver);
+}
+
+MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
+MODULE_DESCRIPTION("Exynos4x12 FIMC-IS image sensor subdev driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/exynos4-is/fimc-is-sensor.h b/drivers/media/platform/exynos4-is/fimc-is-sensor.h
new file mode 100644
index 000000000000..6036d49a6c68
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-sensor.h
@@ -0,0 +1,89 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors:  Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *	     Younghwan Joo <yhwan.joo@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 FIMC_IS_SENSOR_H_
+#define FIMC_IS_SENSOR_H_
+
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-subdev.h>
+
+#define FIMC_IS_SENSOR_OPEN_TIMEOUT	2000 /* ms */
+
+#define FIMC_IS_SENSOR_DEF_PIX_WIDTH	1296
+#define FIMC_IS_SENSOR_DEF_PIX_HEIGHT	732
+
+#define S5K6A3_SENSOR_WIDTH		1392
+#define S5K6A3_SENSOR_HEIGHT		1392
+
+#define SENSOR_NUM_SUPPLIES		2
+
+enum fimc_is_sensor_id {
+	FIMC_IS_SENSOR_ID_S5K3H2 = 1,
+	FIMC_IS_SENSOR_ID_S5K6A3,
+	FIMC_IS_SENSOR_ID_S5K4E5,
+	FIMC_IS_SENSOR_ID_S5K3H7,
+	FIMC_IS_SENSOR_ID_CUSTOM,
+	FIMC_IS_SENSOR_ID_END
+};
+
+#define IS_SENSOR_CTRL_BUS_I2C0		0
+#define IS_SENSOR_CTRL_BUS_I2C1		1
+
+struct sensor_drv_data {
+	enum fimc_is_sensor_id id;
+	const char * const subdev_name;
+	unsigned int width;
+	unsigned int height;
+};
+
+/**
+ * struct fimc_is_sensor - fimc-is sensor data structure
+ * @dev: pointer to this I2C client device structure
+ * @subdev: the image sensor's v4l2 subdev
+ * @pad: subdev media source pad
+ * @supplies: image sensor's voltage regulator supplies
+ * @gpio_reset: GPIO connected to the sensor's reset pin
+ * @drvdata: a pointer to the sensor's parameters data structure
+ * @i2c_bus: ISP I2C bus index (0...1)
+ * @test_pattern: true to enable video test pattern
+ * @lock: mutex protecting the structure's members below
+ * @format: media bus format at the sensor's source pad
+ */
+struct fimc_is_sensor {
+	struct device *dev;
+	struct v4l2_subdev subdev;
+	struct media_pad pad;
+	struct regulator_bulk_data supplies[SENSOR_NUM_SUPPLIES];
+	int gpio_reset;
+	const struct sensor_drv_data *drvdata;
+	unsigned int i2c_bus;
+	bool test_pattern;
+
+	struct mutex lock;
+	struct v4l2_mbus_framefmt format;
+};
+
+static inline
+struct fimc_is_sensor *sd_to_fimc_is_sensor(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct fimc_is_sensor, subdev);
+}
+
+int fimc_is_register_sensor_driver(void);
+void fimc_is_unregister_sensor_driver(void);
+
+#endif /* FIMC_IS_SENSOR_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-is.c b/drivers/media/platform/exynos4-is/fimc-is.c
new file mode 100644
index 000000000000..47c6363d04e2
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is.c
@@ -0,0 +1,1007 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *          Younghwan Joo <yhwan.joo@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.
+ */
+#define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
+
+#include <linux/device.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/dma-contiguous.h>
+#include <linux/errno.h>
+#include <linux/firmware.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_i2c.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-of.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "media-dev.h"
+#include "fimc-is.h"
+#include "fimc-is-command.h"
+#include "fimc-is-errno.h"
+#include "fimc-is-i2c.h"
+#include "fimc-is-param.h"
+#include "fimc-is-regs.h"
+
+
+static char *fimc_is_clocks[ISS_CLKS_MAX] = {
+	[ISS_CLK_PPMUISPX]		= "ppmuispx",
+	[ISS_CLK_PPMUISPMX]		= "ppmuispmx",
+	[ISS_CLK_LITE0]			= "lite0",
+	[ISS_CLK_LITE1]			= "lite1",
+	[ISS_CLK_MPLL]			= "mpll",
+	[ISS_CLK_SYSREG]		= "sysreg",
+	[ISS_CLK_ISP]			= "isp",
+	[ISS_CLK_DRC]			= "drc",
+	[ISS_CLK_FD]			= "fd",
+	[ISS_CLK_MCUISP]		= "mcuisp",
+	[ISS_CLK_UART]			= "uart",
+	[ISS_CLK_ISP_DIV0]		= "ispdiv0",
+	[ISS_CLK_ISP_DIV1]		= "ispdiv1",
+	[ISS_CLK_MCUISP_DIV0]		= "mcuispdiv0",
+	[ISS_CLK_MCUISP_DIV1]		= "mcuispdiv1",
+	[ISS_CLK_ACLK200]		= "aclk200",
+	[ISS_CLK_ACLK200_DIV]		= "div_aclk200",
+	[ISS_CLK_ACLK400MCUISP]		= "aclk400mcuisp",
+	[ISS_CLK_ACLK400MCUISP_DIV]	= "div_aclk400mcuisp",
+};
+
+static void fimc_is_put_clocks(struct fimc_is *is)
+{
+	int i;
+
+	for (i = 0; i < ISS_CLKS_MAX; i++) {
+		if (IS_ERR(is->clocks[i]))
+			continue;
+		clk_unprepare(is->clocks[i]);
+		clk_put(is->clocks[i]);
+		is->clocks[i] = ERR_PTR(-EINVAL);
+	}
+}
+
+static int fimc_is_get_clocks(struct fimc_is *is)
+{
+	int i, ret;
+
+	for (i = 0; i < ISS_CLKS_MAX; i++)
+		is->clocks[i] = ERR_PTR(-EINVAL);
+
+	for (i = 0; i < ISS_CLKS_MAX; i++) {
+		is->clocks[i] = clk_get(&is->pdev->dev, fimc_is_clocks[i]);
+		if (IS_ERR(is->clocks[i])) {
+			ret = PTR_ERR(is->clocks[i]);
+			goto err;
+		}
+		ret = clk_prepare(is->clocks[i]);
+		if (ret < 0) {
+			clk_put(is->clocks[i]);
+			is->clocks[i] = ERR_PTR(-EINVAL);
+			goto err;
+		}
+	}
+
+	return 0;
+err:
+	fimc_is_put_clocks(is);
+	dev_err(&is->pdev->dev, "failed to get clock: %s\n",
+		fimc_is_clocks[i]);
+	return -ENXIO;
+}
+
+static int fimc_is_setup_clocks(struct fimc_is *is)
+{
+	int ret;
+
+	ret = clk_set_parent(is->clocks[ISS_CLK_ACLK200],
+					is->clocks[ISS_CLK_ACLK200_DIV]);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_set_parent(is->clocks[ISS_CLK_ACLK400MCUISP],
+					is->clocks[ISS_CLK_ACLK400MCUISP_DIV]);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_set_rate(is->clocks[ISS_CLK_ISP_DIV0], ACLK_AXI_FREQUENCY);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_set_rate(is->clocks[ISS_CLK_ISP_DIV1], ACLK_AXI_FREQUENCY);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_set_rate(is->clocks[ISS_CLK_MCUISP_DIV0],
+					ATCLK_MCUISP_FREQUENCY);
+	if (ret < 0)
+		return ret;
+
+	return clk_set_rate(is->clocks[ISS_CLK_MCUISP_DIV1],
+					ATCLK_MCUISP_FREQUENCY);
+}
+
+int fimc_is_enable_clocks(struct fimc_is *is)
+{
+	int i, ret;
+
+	for (i = 0; i < ISS_GATE_CLKS_MAX; i++) {
+		if (IS_ERR(is->clocks[i]))
+			continue;
+		ret = clk_enable(is->clocks[i]);
+		if (ret < 0) {
+			dev_err(&is->pdev->dev, "clock %s enable failed\n",
+				fimc_is_clocks[i]);
+			for (--i; i >= 0; i--)
+				clk_disable(is->clocks[i]);
+			return ret;
+		}
+		pr_debug("enabled clock: %s\n", fimc_is_clocks[i]);
+	}
+	return 0;
+}
+
+void fimc_is_disable_clocks(struct fimc_is *is)
+{
+	int i;
+
+	for (i = 0; i < ISS_GATE_CLKS_MAX; i++) {
+		if (!IS_ERR(is->clocks[i])) {
+			clk_disable(is->clocks[i]);
+			pr_debug("disabled clock: %s\n", fimc_is_clocks[i]);
+		}
+	}
+}
+
+static int fimc_is_parse_sensor_config(struct fimc_is_sensor *sensor,
+				       struct device_node *np)
+{
+	u32 tmp = 0;
+	int ret;
+
+	np = v4l2_of_get_next_endpoint(np, NULL);
+	if (!np)
+		return -ENXIO;
+	np = v4l2_of_get_remote_port(np);
+	if (!np)
+		return -ENXIO;
+
+	/* Use MIPI-CSIS channel id to determine the ISP I2C bus index. */
+	ret = of_property_read_u32(np, "reg", &tmp);
+	sensor->i2c_bus = tmp - FIMC_INPUT_MIPI_CSI2_0;
+
+	return ret;
+}
+
+static int fimc_is_register_subdevs(struct fimc_is *is)
+{
+	struct device_node *adapter, *child;
+	int ret;
+
+	ret = fimc_isp_subdev_create(&is->isp);
+	if (ret < 0)
+		return ret;
+
+	for_each_compatible_node(adapter, NULL, FIMC_IS_I2C_COMPATIBLE) {
+		if (!of_find_device_by_node(adapter)) {
+			of_node_put(adapter);
+			return -EPROBE_DEFER;
+		}
+
+		for_each_available_child_of_node(adapter, child) {
+			struct i2c_client *client;
+			struct v4l2_subdev *sd;
+
+			client = of_find_i2c_device_by_node(child);
+			if (!client)
+				goto e_retry;
+
+			sd = i2c_get_clientdata(client);
+			if (!sd)
+				goto e_retry;
+
+			/* FIXME: Add support for multiple sensors. */
+			if (WARN_ON(is->sensor))
+				continue;
+
+			is->sensor = sd_to_fimc_is_sensor(sd);
+
+			if (fimc_is_parse_sensor_config(is->sensor, child)) {
+				dev_warn(&is->pdev->dev, "DT parse error: %s\n",
+							 child->full_name);
+			}
+			pr_debug("%s(): registered subdev: %p\n",
+				 __func__, sd->name);
+		}
+	}
+	return 0;
+
+e_retry:
+	of_node_put(child);
+	return -EPROBE_DEFER;
+}
+
+static int fimc_is_unregister_subdevs(struct fimc_is *is)
+{
+	fimc_isp_subdev_destroy(&is->isp);
+	is->sensor = NULL;
+	return 0;
+}
+
+static int fimc_is_load_setfile(struct fimc_is *is, char *file_name)
+{
+	const struct firmware *fw;
+	void *buf;
+	int ret;
+
+	ret = request_firmware(&fw, file_name, &is->pdev->dev);
+	if (ret < 0) {
+		dev_err(&is->pdev->dev, "firmware request failed (%d)\n", ret);
+		return ret;
+	}
+	buf = is->memory.vaddr + is->setfile.base;
+	memcpy(buf, fw->data, fw->size);
+	fimc_is_mem_barrier();
+	is->setfile.size = fw->size;
+
+	pr_debug("mem vaddr: %p, setfile buf: %p\n", is->memory.vaddr, buf);
+
+	memcpy(is->fw.setfile_info,
+		fw->data + fw->size - FIMC_IS_SETFILE_INFO_LEN,
+		FIMC_IS_SETFILE_INFO_LEN - 1);
+
+	is->fw.setfile_info[FIMC_IS_SETFILE_INFO_LEN - 1] = '\0';
+	is->setfile.state = 1;
+
+	pr_debug("FIMC-IS setfile loaded: base: %#x, size: %zu B\n",
+		 is->setfile.base, fw->size);
+
+	release_firmware(fw);
+	return ret;
+}
+
+int fimc_is_cpu_set_power(struct fimc_is *is, int on)
+{
+	unsigned int timeout = FIMC_IS_POWER_ON_TIMEOUT;
+
+	if (on) {
+		/* Disable watchdog */
+		mcuctl_write(0, is, REG_WDT_ISP);
+
+		/* Cortex-A5 start address setting */
+		mcuctl_write(is->memory.paddr, is, MCUCTL_REG_BBOAR);
+
+		/* Enable and start Cortex-A5 */
+		pmuisp_write(0x18000, is, REG_PMU_ISP_ARM_OPTION);
+		pmuisp_write(0x1, is, REG_PMU_ISP_ARM_CONFIGURATION);
+	} else {
+		/* A5 power off */
+		pmuisp_write(0x10000, is, REG_PMU_ISP_ARM_OPTION);
+		pmuisp_write(0x0, is, REG_PMU_ISP_ARM_CONFIGURATION);
+
+		while (pmuisp_read(is, REG_PMU_ISP_ARM_STATUS) & 1) {
+			if (timeout == 0)
+				return -ETIME;
+			timeout--;
+			udelay(1);
+		}
+	}
+
+	return 0;
+}
+
+/* Wait until @bit of @is->state is set to @state in the interrupt handler. */
+int fimc_is_wait_event(struct fimc_is *is, unsigned long bit,
+		       unsigned int state, unsigned int timeout)
+{
+
+	int ret = wait_event_timeout(is->irq_queue,
+				     !state ^ test_bit(bit, &is->state),
+				     timeout);
+	if (ret == 0) {
+		dev_WARN(&is->pdev->dev, "%s() timed out\n", __func__);
+		return -ETIME;
+	}
+	return 0;
+}
+
+int fimc_is_start_firmware(struct fimc_is *is)
+{
+	struct device *dev = &is->pdev->dev;
+	int ret;
+
+	memcpy(is->memory.vaddr, is->fw.f_w->data, is->fw.f_w->size);
+	wmb();
+
+	ret = fimc_is_cpu_set_power(is, 1);
+	if (ret < 0)
+		return ret;
+
+	ret = fimc_is_wait_event(is, IS_ST_A5_PWR_ON, 1,
+				 msecs_to_jiffies(FIMC_IS_FW_LOAD_TIMEOUT));
+	if (ret < 0)
+		dev_err(dev, "FIMC-IS CPU power on failed\n");
+
+	return ret;
+}
+
+/* Allocate working memory for the FIMC-IS CPU. */
+static int fimc_is_alloc_cpu_memory(struct fimc_is *is)
+{
+	struct device *dev = &is->pdev->dev;
+
+	is->memory.vaddr = dma_alloc_coherent(dev, FIMC_IS_CPU_MEM_SIZE,
+					      &is->memory.paddr, GFP_KERNEL);
+	if (is->memory.vaddr == NULL)
+		return -ENOMEM;
+
+	is->memory.size = FIMC_IS_CPU_MEM_SIZE;
+	memset(is->memory.vaddr, 0, is->memory.size);
+
+	dev_info(dev, "FIMC-IS CPU memory base: %#x\n", (u32)is->memory.paddr);
+
+	if (((u32)is->memory.paddr) & FIMC_IS_FW_ADDR_MASK) {
+		dev_err(dev, "invalid firmware memory alignment: %#x\n",
+			(u32)is->memory.paddr);
+		dma_free_coherent(dev, is->memory.size, is->memory.vaddr,
+				  is->memory.paddr);
+		return -EIO;
+	}
+
+	is->is_p_region = (struct is_region *)(is->memory.vaddr +
+				FIMC_IS_CPU_MEM_SIZE - FIMC_IS_REGION_SIZE);
+
+	is->is_dma_p_region = is->memory.paddr +
+				FIMC_IS_CPU_MEM_SIZE - FIMC_IS_REGION_SIZE;
+
+	is->is_shared_region = (struct is_share_region *)(is->memory.vaddr +
+				FIMC_IS_SHARED_REGION_OFFSET);
+	return 0;
+}
+
+static void fimc_is_free_cpu_memory(struct fimc_is *is)
+{
+	struct device *dev = &is->pdev->dev;
+
+	dma_free_coherent(dev, is->memory.size, is->memory.vaddr,
+			  is->memory.paddr);
+}
+
+static void fimc_is_load_firmware(const struct firmware *fw, void *context)
+{
+	struct fimc_is *is = context;
+	struct device *dev = &is->pdev->dev;
+	void *buf;
+	int ret;
+
+	if (fw == NULL) {
+		dev_err(dev, "firmware request failed\n");
+		return;
+	}
+	mutex_lock(&is->lock);
+
+	if (fw->size < FIMC_IS_FW_SIZE_MIN || fw->size > FIMC_IS_FW_SIZE_MAX) {
+		dev_err(dev, "wrong firmware size: %d\n", fw->size);
+		goto done;
+	}
+
+	is->fw.size = fw->size;
+
+	ret = fimc_is_alloc_cpu_memory(is);
+	if (ret < 0) {
+		dev_err(dev, "failed to allocate FIMC-IS CPU memory\n");
+		goto done;
+	}
+
+	memcpy(is->memory.vaddr, fw->data, fw->size);
+	wmb();
+
+	/* Read firmware description. */
+	buf = (void *)(is->memory.vaddr + fw->size - FIMC_IS_FW_DESC_LEN);
+	memcpy(&is->fw.info, buf, FIMC_IS_FW_INFO_LEN);
+	is->fw.info[FIMC_IS_FW_INFO_LEN] = 0;
+
+	buf = (void *)(is->memory.vaddr + fw->size - FIMC_IS_FW_VER_LEN);
+	memcpy(&is->fw.version, buf, FIMC_IS_FW_VER_LEN);
+	is->fw.version[FIMC_IS_FW_VER_LEN - 1] = 0;
+
+	is->fw.state = 1;
+
+	dev_info(dev, "loaded firmware: %s, rev. %s\n",
+		 is->fw.info, is->fw.version);
+	dev_dbg(dev, "FW size: %d, paddr: %#x\n", fw->size, is->memory.paddr);
+
+	is->is_shared_region->chip_id = 0xe4412;
+	is->is_shared_region->chip_rev_no = 1;
+
+	fimc_is_mem_barrier();
+
+	/*
+	 * FIXME: The firmware is not being released for now, as it is
+	 * needed around for copying to the IS working memory every
+	 * time before the Cortex-A5 is restarted.
+	 */
+	if (is->fw.f_w)
+		release_firmware(is->fw.f_w);
+	is->fw.f_w = fw;
+done:
+	mutex_unlock(&is->lock);
+}
+
+static int fimc_is_request_firmware(struct fimc_is *is, const char *fw_name)
+{
+	return request_firmware_nowait(THIS_MODULE,
+				FW_ACTION_HOTPLUG, fw_name, &is->pdev->dev,
+				GFP_KERNEL, is, fimc_is_load_firmware);
+}
+
+/* General IS interrupt handler */
+static void fimc_is_general_irq_handler(struct fimc_is *is)
+{
+	is->i2h_cmd.cmd = mcuctl_read(is, MCUCTL_REG_ISSR(10));
+
+	switch (is->i2h_cmd.cmd) {
+	case IHC_GET_SENSOR_NUM:
+		fimc_is_hw_get_params(is, 1);
+		fimc_is_hw_wait_intmsr0_intmsd0(is);
+		fimc_is_hw_set_sensor_num(is);
+		pr_debug("ISP FW version: %#x\n", is->i2h_cmd.args[0]);
+		break;
+	case IHC_SET_FACE_MARK:
+	case IHC_FRAME_DONE:
+		fimc_is_hw_get_params(is, 2);
+		break;
+	case IHC_SET_SHOT_MARK:
+	case IHC_AA_DONE:
+	case IH_REPLY_DONE:
+		fimc_is_hw_get_params(is, 3);
+		break;
+	case IH_REPLY_NOT_DONE:
+		fimc_is_hw_get_params(is, 4);
+		break;
+	case IHC_NOT_READY:
+		break;
+	default:
+		pr_info("unknown command: %#x\n", is->i2h_cmd.cmd);
+	}
+
+	fimc_is_fw_clear_irq1(is, FIMC_IS_INT_GENERAL);
+
+	switch (is->i2h_cmd.cmd) {
+	case IHC_GET_SENSOR_NUM:
+		fimc_is_hw_set_intgr0_gd0(is);
+		set_bit(IS_ST_A5_PWR_ON, &is->state);
+		break;
+
+	case IHC_SET_SHOT_MARK:
+		break;
+
+	case IHC_SET_FACE_MARK:
+		is->fd_header.count = is->i2h_cmd.args[0];
+		is->fd_header.index = is->i2h_cmd.args[1];
+		is->fd_header.offset = 0;
+		break;
+
+	case IHC_FRAME_DONE:
+		break;
+
+	case IHC_AA_DONE:
+		pr_debug("AA_DONE - %d, %d, %d\n", is->i2h_cmd.args[0],
+			 is->i2h_cmd.args[1], is->i2h_cmd.args[2]);
+		break;
+
+	case IH_REPLY_DONE:
+		pr_debug("ISR_DONE: args[0]: %#x\n", is->i2h_cmd.args[0]);
+
+		switch (is->i2h_cmd.args[0]) {
+		case HIC_PREVIEW_STILL...HIC_CAPTURE_VIDEO:
+			/* Get CAC margin */
+			set_bit(IS_ST_CHANGE_MODE, &is->state);
+			is->isp.cac_margin_x = is->i2h_cmd.args[1];
+			is->isp.cac_margin_y = is->i2h_cmd.args[2];
+			pr_debug("CAC margin (x,y): (%d,%d)\n",
+				 is->isp.cac_margin_x, is->isp.cac_margin_y);
+			break;
+
+		case HIC_STREAM_ON:
+			clear_bit(IS_ST_STREAM_OFF, &is->state);
+			set_bit(IS_ST_STREAM_ON, &is->state);
+			break;
+
+		case HIC_STREAM_OFF:
+			clear_bit(IS_ST_STREAM_ON, &is->state);
+			set_bit(IS_ST_STREAM_OFF, &is->state);
+			break;
+
+		case HIC_SET_PARAMETER:
+			is->config[is->config_index].p_region_index1 = 0;
+			is->config[is->config_index].p_region_index2 = 0;
+			set_bit(IS_ST_BLOCK_CMD_CLEARED, &is->state);
+			pr_debug("HIC_SET_PARAMETER\n");
+			break;
+
+		case HIC_GET_PARAMETER:
+			break;
+
+		case HIC_SET_TUNE:
+			break;
+
+		case HIC_GET_STATUS:
+			break;
+
+		case HIC_OPEN_SENSOR:
+			set_bit(IS_ST_OPEN_SENSOR, &is->state);
+			pr_debug("data lanes: %d, settle line: %d\n",
+				 is->i2h_cmd.args[2], is->i2h_cmd.args[1]);
+			break;
+
+		case HIC_CLOSE_SENSOR:
+			clear_bit(IS_ST_OPEN_SENSOR, &is->state);
+			is->sensor_index = 0;
+			break;
+
+		case HIC_MSG_TEST:
+			pr_debug("config MSG level completed\n");
+			break;
+
+		case HIC_POWER_DOWN:
+			clear_bit(IS_ST_PWR_SUBIP_ON, &is->state);
+			break;
+
+		case HIC_GET_SET_FILE_ADDR:
+			is->setfile.base = is->i2h_cmd.args[1];
+			set_bit(IS_ST_SETFILE_LOADED, &is->state);
+			break;
+
+		case HIC_LOAD_SET_FILE:
+			set_bit(IS_ST_SETFILE_LOADED, &is->state);
+			break;
+		}
+		break;
+
+	case IH_REPLY_NOT_DONE:
+		pr_err("ISR_NDONE: %d: %#x, %s\n", is->i2h_cmd.args[0],
+		       is->i2h_cmd.args[1],
+		       fimc_is_strerr(is->i2h_cmd.args[1]));
+
+		if (is->i2h_cmd.args[1] & IS_ERROR_TIME_OUT_FLAG)
+			pr_err("IS_ERROR_TIME_OUT\n");
+
+		switch (is->i2h_cmd.args[1]) {
+		case IS_ERROR_SET_PARAMETER:
+			fimc_is_mem_barrier();
+		}
+
+		switch (is->i2h_cmd.args[0]) {
+		case HIC_SET_PARAMETER:
+			is->config[is->config_index].p_region_index1 = 0;
+			is->config[is->config_index].p_region_index2 = 0;
+			set_bit(IS_ST_BLOCK_CMD_CLEARED, &is->state);
+			break;
+		}
+		break;
+
+	case IHC_NOT_READY:
+		pr_err("IS control sequence error: Not Ready\n");
+		break;
+	}
+
+	wake_up(&is->irq_queue);
+}
+
+static irqreturn_t fimc_is_irq_handler(int irq, void *priv)
+{
+	struct fimc_is *is = priv;
+	unsigned long flags;
+	u32 status;
+
+	spin_lock_irqsave(&is->slock, flags);
+	status = mcuctl_read(is, MCUCTL_REG_INTSR1);
+
+	if (status & (1UL << FIMC_IS_INT_GENERAL))
+		fimc_is_general_irq_handler(is);
+
+	if (status & (1UL << FIMC_IS_INT_FRAME_DONE_ISP))
+		fimc_isp_irq_handler(is);
+
+	spin_unlock_irqrestore(&is->slock, flags);
+	return IRQ_HANDLED;
+}
+
+static int fimc_is_hw_open_sensor(struct fimc_is *is,
+				  struct fimc_is_sensor *sensor)
+{
+	struct sensor_open_extended *soe = (void *)&is->is_p_region->shared;
+
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+
+	soe->self_calibration_mode = 1;
+	soe->actuator_type = 0;
+	soe->mipi_lane_num = 0;
+	soe->mclk = 0;
+	soe->mipi_speed	= 0;
+	soe->fast_open_sensor = 0;
+	soe->i2c_sclk = 88000000;
+
+	fimc_is_mem_barrier();
+
+	mcuctl_write(HIC_OPEN_SENSOR, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(sensor->drvdata->id, is, MCUCTL_REG_ISSR(2));
+	mcuctl_write(sensor->i2c_bus, is, MCUCTL_REG_ISSR(3));
+	mcuctl_write(is->is_dma_p_region, is, MCUCTL_REG_ISSR(4));
+
+	fimc_is_hw_set_intgr0_gd0(is);
+
+	return fimc_is_wait_event(is, IS_ST_OPEN_SENSOR, 1,
+				  FIMC_IS_SENSOR_OPEN_TIMEOUT);
+}
+
+
+int fimc_is_hw_initialize(struct fimc_is *is)
+{
+	const int config_ids[] = {
+		IS_SC_PREVIEW_STILL, IS_SC_PREVIEW_VIDEO,
+		IS_SC_CAPTURE_STILL, IS_SC_CAPTURE_VIDEO
+	};
+	struct device *dev = &is->pdev->dev;
+	u32 prev_id;
+	int i, ret;
+
+	/* Sensor initialization. */
+	ret = fimc_is_hw_open_sensor(is, is->sensor);
+	if (ret < 0)
+		return ret;
+
+	/* Get the setfile address. */
+	fimc_is_hw_get_setfile_addr(is);
+
+	ret = fimc_is_wait_event(is, IS_ST_SETFILE_LOADED, 1,
+				 FIMC_IS_CONFIG_TIMEOUT);
+	if (ret < 0) {
+		dev_err(dev, "get setfile address timed out\n");
+		return ret;
+	}
+	pr_debug("setfile.base: %#x\n", is->setfile.base);
+
+	/* Load the setfile. */
+	fimc_is_load_setfile(is, FIMC_IS_SETFILE_6A3);
+	clear_bit(IS_ST_SETFILE_LOADED, &is->state);
+	fimc_is_hw_load_setfile(is);
+	ret = fimc_is_wait_event(is, IS_ST_SETFILE_LOADED, 1,
+				 FIMC_IS_CONFIG_TIMEOUT);
+	if (ret < 0) {
+		dev_err(dev, "loading setfile timed out\n");
+		return ret;
+	}
+
+	pr_debug("setfile: base: %#x, size: %d\n",
+		 is->setfile.base, is->setfile.size);
+	pr_info("FIMC-IS Setfile info: %s\n", is->fw.setfile_info);
+
+	/* Check magic number. */
+	if (is->is_p_region->shared[MAX_SHARED_COUNT - 1] !=
+	    FIMC_IS_MAGIC_NUMBER) {
+		dev_err(dev, "magic number error!\n");
+		return -EIO;
+	}
+
+	pr_debug("shared region: %#x, parameter region: %#x\n",
+		 is->memory.paddr + FIMC_IS_SHARED_REGION_OFFSET,
+		 is->is_dma_p_region);
+
+	is->setfile.sub_index = 0;
+
+	/* Stream off. */
+	fimc_is_hw_stream_off(is);
+	ret = fimc_is_wait_event(is, IS_ST_STREAM_OFF, 1,
+				 FIMC_IS_CONFIG_TIMEOUT);
+	if (ret < 0) {
+		dev_err(dev, "stream off timeout\n");
+		return ret;
+	}
+
+	/* Preserve previous mode. */
+	prev_id = is->config_index;
+
+	/* Set initial parameter values. */
+	for (i = 0; i < ARRAY_SIZE(config_ids); i++) {
+		is->config_index = config_ids[i];
+		fimc_is_set_initial_params(is);
+		ret = fimc_is_itf_s_param(is, true);
+		if (ret < 0) {
+			is->config_index = prev_id;
+			return ret;
+		}
+	}
+	is->config_index = prev_id;
+
+	set_bit(IS_ST_INIT_DONE, &is->state);
+	dev_info(dev, "initialization sequence completed (%d)\n",
+						is->config_index);
+	return 0;
+}
+
+static int fimc_is_log_show(struct seq_file *s, void *data)
+{
+	struct fimc_is *is = s->private;
+	const u8 *buf = is->memory.vaddr + FIMC_IS_DEBUG_REGION_OFFSET;
+
+	if (is->memory.vaddr == NULL) {
+		dev_err(&is->pdev->dev, "firmware memory is not initialized\n");
+		return -EIO;
+	}
+
+	seq_printf(s, "%s\n", buf);
+	return 0;
+}
+
+static int fimc_is_debugfs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, fimc_is_log_show, inode->i_private);
+}
+
+static const struct file_operations fimc_is_debugfs_fops = {
+	.open		= fimc_is_debugfs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static void fimc_is_debugfs_remove(struct fimc_is *is)
+{
+	debugfs_remove_recursive(is->debugfs_entry);
+	is->debugfs_entry = NULL;
+}
+
+static int fimc_is_debugfs_create(struct fimc_is *is)
+{
+	struct dentry *dentry;
+
+	is->debugfs_entry = debugfs_create_dir("fimc_is", NULL);
+
+	dentry = debugfs_create_file("fw_log", S_IRUGO, is->debugfs_entry,
+				     is, &fimc_is_debugfs_fops);
+	if (!dentry)
+		fimc_is_debugfs_remove(is);
+
+	return is->debugfs_entry == NULL ? -EIO : 0;
+}
+
+static int fimc_is_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct fimc_is *is;
+	struct resource res;
+	struct device_node *node;
+	int ret;
+
+	is = devm_kzalloc(&pdev->dev, sizeof(*is), GFP_KERNEL);
+	if (!is)
+		return -ENOMEM;
+
+	is->pdev = pdev;
+	is->isp.pdev = pdev;
+
+	init_waitqueue_head(&is->irq_queue);
+	spin_lock_init(&is->slock);
+	mutex_init(&is->lock);
+
+	ret = of_address_to_resource(dev->of_node, 0, &res);
+	if (ret < 0)
+		return ret;
+
+	is->regs = devm_ioremap_resource(dev, &res);
+	if (IS_ERR(is->regs))
+		return PTR_ERR(is->regs);
+
+	node = of_get_child_by_name(dev->of_node, "pmu");
+	if (!node)
+		return -ENODEV;
+
+	is->pmu_regs = of_iomap(node, 0);
+	if (!is->pmu_regs)
+		return -ENOMEM;
+
+	is->irq = irq_of_parse_and_map(dev->of_node, 0);
+	if (is->irq < 0) {
+		dev_err(dev, "no irq found\n");
+		return is->irq;
+	}
+
+	ret = fimc_is_get_clocks(is);
+	if (ret < 0)
+		return ret;
+
+	platform_set_drvdata(pdev, is);
+
+	ret = request_irq(is->irq, fimc_is_irq_handler, 0, dev_name(dev), is);
+	if (ret < 0) {
+		dev_err(dev, "irq request failed\n");
+		goto err_clk;
+	}
+	pm_runtime_enable(dev);
+	/*
+	 * Enable only the ISP power domain, keep FIMC-IS clocks off until
+	 * the whole clock tree is configured. The ISP power domain needs
+	 * be active in order to acces any CMU_ISP clock registers.
+	 */
+	ret = pm_runtime_get_sync(dev);
+	if (ret < 0)
+		goto err_irq;
+
+	ret = fimc_is_setup_clocks(is);
+	pm_runtime_put_sync(dev);
+
+	if (ret < 0)
+		goto err_irq;
+
+	is->clk_init = true;
+
+	is->alloc_ctx = vb2_dma_contig_init_ctx(dev);
+	if (IS_ERR(is->alloc_ctx)) {
+		ret = PTR_ERR(is->alloc_ctx);
+		goto err_irq;
+	}
+	/*
+	 * Register FIMC-IS V4L2 subdevs to this driver. The video nodes
+	 * will be created within the subdev's registered() callback.
+	 */
+	ret = fimc_is_register_subdevs(is);
+	if (ret < 0)
+		goto err_vb;
+
+	ret = fimc_is_debugfs_create(is);
+	if (ret < 0)
+		goto err_sd;
+
+	ret = fimc_is_request_firmware(is, FIMC_IS_FW_FILENAME);
+	if (ret < 0)
+		goto err_dfs;
+
+	dev_dbg(dev, "FIMC-IS registered successfully\n");
+	return 0;
+
+err_dfs:
+	fimc_is_debugfs_remove(is);
+err_vb:
+	vb2_dma_contig_cleanup_ctx(is->alloc_ctx);
+err_sd:
+	fimc_is_unregister_subdevs(is);
+err_irq:
+	free_irq(is->irq, is);
+err_clk:
+	fimc_is_put_clocks(is);
+	return ret;
+}
+
+static int fimc_is_runtime_resume(struct device *dev)
+{
+	struct fimc_is *is = dev_get_drvdata(dev);
+
+	if (!is->clk_init)
+		return 0;
+
+	return fimc_is_enable_clocks(is);
+}
+
+static int fimc_is_runtime_suspend(struct device *dev)
+{
+	struct fimc_is *is = dev_get_drvdata(dev);
+
+	if (is->clk_init)
+		fimc_is_disable_clocks(is);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int fimc_is_resume(struct device *dev)
+{
+	/* TODO: */
+	return 0;
+}
+
+static int fimc_is_suspend(struct device *dev)
+{
+	struct fimc_is *is = dev_get_drvdata(dev);
+
+	/* TODO: */
+	if (test_bit(IS_ST_A5_PWR_ON, &is->state))
+		return -EBUSY;
+
+	return 0;
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static int fimc_is_remove(struct platform_device *pdev)
+{
+	struct fimc_is *is = platform_get_drvdata(pdev);
+
+	pm_runtime_disable(&pdev->dev);
+	pm_runtime_set_suspended(&pdev->dev);
+	free_irq(is->irq, is);
+	fimc_is_unregister_subdevs(is);
+	vb2_dma_contig_cleanup_ctx(is->alloc_ctx);
+	fimc_is_put_clocks(is);
+	fimc_is_debugfs_remove(is);
+	release_firmware(is->fw.f_w);
+	fimc_is_free_cpu_memory(is);
+
+	return 0;
+}
+
+static const struct of_device_id fimc_is_of_match[] = {
+	{ .compatible = "samsung,exynos4212-fimc-is" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, fimc_is_of_match);
+
+static const struct dev_pm_ops fimc_is_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(fimc_is_suspend, fimc_is_resume)
+	SET_RUNTIME_PM_OPS(fimc_is_runtime_suspend, fimc_is_runtime_resume,
+			   NULL)
+};
+
+static struct platform_driver fimc_is_driver = {
+	.probe		= fimc_is_probe,
+	.remove		= fimc_is_remove,
+	.driver = {
+		.of_match_table	= fimc_is_of_match,
+		.name		= FIMC_IS_DRV_NAME,
+		.owner		= THIS_MODULE,
+		.pm		= &fimc_is_pm_ops,
+	}
+};
+
+static int fimc_is_module_init(void)
+{
+	int ret;
+
+	ret = fimc_is_register_sensor_driver();
+	if (ret < 0)
+		return ret;
+
+	ret = fimc_is_register_i2c_driver();
+	if (ret < 0)
+		goto err_sens;
+
+	ret = platform_driver_register(&fimc_is_driver);
+	if (!ret)
+		return ret;
+
+	fimc_is_unregister_i2c_driver();
+err_sens:
+	fimc_is_unregister_sensor_driver();
+	return ret;
+}
+
+static void fimc_is_module_exit(void)
+{
+	fimc_is_unregister_sensor_driver();
+	fimc_is_unregister_i2c_driver();
+	platform_driver_unregister(&fimc_is_driver);
+}
+
+module_init(fimc_is_module_init);
+module_exit(fimc_is_module_exit);
+
+MODULE_ALIAS("platform:" FIMC_IS_DRV_NAME);
+MODULE_AUTHOR("Younghwan Joo <yhwan.joo@samsung.com>");
+MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
diff --git a/drivers/media/platform/exynos4-is/fimc-is.h b/drivers/media/platform/exynos4-is/fimc-is.h
new file mode 100644
index 000000000000..f5275a5b0156
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is.h
@@ -0,0 +1,345 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@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
+ * published by the Free Software Foundation.
+ */
+#ifndef FIMC_IS_H_
+#define FIMC_IS_H_
+
+#include <asm/barrier.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/sizes.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <media/videobuf2-core.h>
+#include <media/v4l2-ctrls.h>
+
+#include "fimc-isp.h"
+#include "fimc-is-command.h"
+#include "fimc-is-sensor.h"
+#include "fimc-is-param.h"
+#include "fimc-is-regs.h"
+
+#define FIMC_IS_DRV_NAME		"exynos4-fimc-is"
+
+#define FIMC_IS_FW_FILENAME		"fimc_is_fw.bin"
+#define FIMC_IS_SETFILE_6A3		"setfile.bin"
+
+#define FIMC_IS_FW_LOAD_TIMEOUT		1000 /* ms */
+#define FIMC_IS_POWER_ON_TIMEOUT	1000 /* us */
+
+#define FIMC_IS_SENSOR_NUM		2
+
+/* Memory definitions */
+#define FIMC_IS_CPU_MEM_SIZE		(0xa00000)
+#define FIMC_IS_CPU_BASE_MASK		((1 << 26) - 1)
+#define FIMC_IS_REGION_SIZE		0x5000
+
+#define FIMC_IS_DEBUG_REGION_OFFSET	0x0084b000
+#define FIMC_IS_SHARED_REGION_OFFSET	0x008c0000
+#define FIMC_IS_FW_INFO_LEN		31
+#define FIMC_IS_FW_VER_LEN		7
+#define FIMC_IS_FW_DESC_LEN		(FIMC_IS_FW_INFO_LEN + \
+					 FIMC_IS_FW_VER_LEN)
+#define FIMC_IS_SETFILE_INFO_LEN	39
+
+#define FIMC_IS_EXTRA_MEM_SIZE		(FIMC_IS_EXTRA_FW_SIZE + \
+					 FIMC_IS_EXTRA_SETFILE_SIZE + 0x1000)
+#define FIMC_IS_EXTRA_FW_SIZE		0x180000
+#define FIMC_IS_EXTRA_SETFILE_SIZE	0x4b000
+
+/* TODO: revisit */
+#define FIMC_IS_FW_ADDR_MASK		((1 << 26) - 1)
+#define FIMC_IS_FW_SIZE_MAX		(SZ_4M)
+#define FIMC_IS_FW_SIZE_MIN		(SZ_32K)
+
+#define ATCLK_MCUISP_FREQUENCY		100000000UL
+#define ACLK_AXI_FREQUENCY		100000000UL
+
+enum {
+	ISS_CLK_PPMUISPX,
+	ISS_CLK_PPMUISPMX,
+	ISS_CLK_LITE0,
+	ISS_CLK_LITE1,
+	ISS_CLK_MPLL,
+	ISS_CLK_SYSREG,
+	ISS_CLK_ISP,
+	ISS_CLK_DRC,
+	ISS_CLK_FD,
+	ISS_CLK_MCUISP,
+	ISS_CLK_UART,
+	ISS_GATE_CLKS_MAX,
+	ISS_CLK_ISP_DIV0 = ISS_GATE_CLKS_MAX,
+	ISS_CLK_ISP_DIV1,
+	ISS_CLK_MCUISP_DIV0,
+	ISS_CLK_MCUISP_DIV1,
+	ISS_CLK_ACLK200,
+	ISS_CLK_ACLK200_DIV,
+	ISS_CLK_ACLK400MCUISP,
+	ISS_CLK_ACLK400MCUISP_DIV,
+	ISS_CLKS_MAX
+};
+
+/* The driver's internal state flags */
+enum {
+	IS_ST_IDLE,
+	IS_ST_PWR_ON,
+	IS_ST_A5_PWR_ON,
+	IS_ST_FW_LOADED,
+	IS_ST_OPEN_SENSOR,
+	IS_ST_SETFILE_LOADED,
+	IS_ST_INIT_DONE,
+	IS_ST_STREAM_ON,
+	IS_ST_STREAM_OFF,
+	IS_ST_CHANGE_MODE,
+	IS_ST_BLOCK_CMD_CLEARED,
+	IS_ST_SET_ZOOM,
+	IS_ST_PWR_SUBIP_ON,
+	IS_ST_END,
+};
+
+enum af_state {
+	FIMC_IS_AF_IDLE		= 0,
+	FIMC_IS_AF_SETCONFIG	= 1,
+	FIMC_IS_AF_RUNNING	= 2,
+	FIMC_IS_AF_LOCK		= 3,
+	FIMC_IS_AF_ABORT	= 4,
+	FIMC_IS_AF_FAILED	= 5,
+};
+
+enum af_lock_state {
+	FIMC_IS_AF_UNLOCKED	= 0,
+	FIMC_IS_AF_LOCKED	= 2
+};
+
+enum ae_lock_state {
+	FIMC_IS_AE_UNLOCKED	= 0,
+	FIMC_IS_AE_LOCKED	= 1
+};
+
+enum awb_lock_state {
+	FIMC_IS_AWB_UNLOCKED	= 0,
+	FIMC_IS_AWB_LOCKED	= 1
+};
+
+enum {
+	IS_METERING_CONFIG_CMD,
+	IS_METERING_CONFIG_WIN_POS_X,
+	IS_METERING_CONFIG_WIN_POS_Y,
+	IS_METERING_CONFIG_WIN_WIDTH,
+	IS_METERING_CONFIG_WIN_HEIGHT,
+	IS_METERING_CONFIG_MAX
+};
+
+struct is_setfile {
+	const struct firmware *info;
+	int state;
+	u32 sub_index;
+	u32 base;
+	size_t size;
+};
+
+struct is_fd_result_header {
+	u32 offset;
+	u32 count;
+	u32 index;
+	u32 curr_index;
+	u32 width;
+	u32 height;
+};
+
+struct is_af_info {
+	u16 mode;
+	u32 af_state;
+	u32 af_lock_state;
+	u32 ae_lock_state;
+	u32 awb_lock_state;
+	u16 pos_x;
+	u16 pos_y;
+	u16 prev_pos_x;
+	u16 prev_pos_y;
+	u16 use_af;
+};
+
+struct fimc_is_firmware {
+	const struct firmware *f_w;
+
+	dma_addr_t paddr;
+	void *vaddr;
+	unsigned int size;
+
+	char info[FIMC_IS_FW_INFO_LEN + 1];
+	char version[FIMC_IS_FW_VER_LEN + 1];
+	char setfile_info[FIMC_IS_SETFILE_INFO_LEN + 1];
+	u8 state;
+};
+
+struct fimc_is_memory {
+	/* physical base address */
+	dma_addr_t paddr;
+	/* virtual base address */
+	void *vaddr;
+	/* total length */
+	unsigned int size;
+};
+
+#define FIMC_IS_I2H_MAX_ARGS	12
+
+struct i2h_cmd {
+	u32 cmd;
+	u32 sensor_id;
+	u16 num_args;
+	u32 args[FIMC_IS_I2H_MAX_ARGS];
+};
+
+struct h2i_cmd {
+	u16 cmd_type;
+	u32 entry_id;
+};
+
+#define FIMC_IS_DEBUG_MSG	0x3f
+#define FIMC_IS_DEBUG_LEVEL	3
+
+struct fimc_is_setfile {
+	const struct firmware *info;
+	unsigned int state;
+	unsigned int size;
+	u32 sub_index;
+	u32 base;
+};
+
+struct chain_config {
+	struct global_param	global;
+	struct sensor_param	sensor;
+	struct isp_param	isp;
+	struct drc_param	drc;
+	struct fd_param		fd;
+
+	unsigned long		p_region_index1;
+	unsigned long		p_region_index2;
+};
+
+/**
+ * struct fimc_is - fimc-is data structure
+ * @pdev: pointer to FIMC-IS platform device
+ * @pctrl: pointer to pinctrl structure for this device
+ * @v4l2_dev: pointer to top the level v4l2_device
+ * @alloc_ctx: videobuf2 memory allocator context
+ * @lock: mutex serializing video device and the subdev operations
+ * @slock: spinlock protecting this data structure and the hw registers
+ * @clocks: FIMC-LITE gate clock
+ * @regs: MCUCTL mmapped registers region
+ * @pmu_regs: PMU ISP mmapped registers region
+ * @irq_queue: interrupt handling waitqueue
+ * @lpm: low power mode flag
+ * @state: internal driver's state flags
+ */
+struct fimc_is {
+	struct platform_device		*pdev;
+	struct pinctrl			*pctrl;
+	struct v4l2_device		*v4l2_dev;
+
+	struct fimc_is_firmware		fw;
+	struct fimc_is_memory		memory;
+	struct firmware			*f_w;
+
+	struct fimc_isp			isp;
+	struct fimc_is_sensor		*sensor;
+	struct fimc_is_setfile		setfile;
+
+	struct vb2_alloc_ctx		*alloc_ctx;
+	struct v4l2_ctrl_handler	ctrl_handler;
+
+	struct mutex			lock;
+	spinlock_t			slock;
+
+	struct clk			*clocks[ISS_CLKS_MAX];
+	bool				clk_init;
+	void __iomem			*regs;
+	void __iomem			*pmu_regs;
+	int				irq;
+	wait_queue_head_t		irq_queue;
+	u8				lpm;
+
+	unsigned long			state;
+	unsigned int			sensor_index;
+
+	struct i2h_cmd			i2h_cmd;
+	struct h2i_cmd			h2i_cmd;
+	struct is_fd_result_header	fd_header;
+
+	struct chain_config		config[IS_SC_MAX];
+	unsigned			config_index;
+
+	struct is_region		*is_p_region;
+	dma_addr_t			is_dma_p_region;
+	struct is_share_region		*is_shared_region;
+	struct is_af_info		af;
+
+	struct dentry			*debugfs_entry;
+};
+
+static inline struct fimc_is *fimc_isp_to_is(struct fimc_isp *isp)
+{
+	return container_of(isp, struct fimc_is, isp);
+}
+
+static inline void fimc_is_mem_barrier(void)
+{
+	mb();
+}
+
+static inline void fimc_is_set_param_bit(struct fimc_is *is, int num)
+{
+	struct chain_config *cfg = &is->config[is->config_index];
+
+	if (num >= 32)
+		set_bit(num - 32, &cfg->p_region_index2);
+	else
+		set_bit(num, &cfg->p_region_index1);
+}
+
+static inline void fimc_is_set_param_ctrl_cmd(struct fimc_is *is, int cmd)
+{
+	is->is_p_region->parameter.isp.control.cmd = cmd;
+}
+
+static inline void mcuctl_write(u32 v, struct fimc_is *is, unsigned int offset)
+{
+	writel(v, is->regs + offset);
+}
+
+static inline u32 mcuctl_read(struct fimc_is *is, unsigned int offset)
+{
+	return readl(is->regs + offset);
+}
+
+static inline void pmuisp_write(u32 v, struct fimc_is *is, unsigned int offset)
+{
+	writel(v, is->pmu_regs + offset);
+}
+
+static inline u32 pmuisp_read(struct fimc_is *is, unsigned int offset)
+{
+	return readl(is->pmu_regs + offset);
+}
+
+int fimc_is_wait_event(struct fimc_is *is, unsigned long bit,
+		       unsigned int state, unsigned int timeout);
+int fimc_is_cpu_set_power(struct fimc_is *is, int on);
+int fimc_is_start_firmware(struct fimc_is *is);
+int fimc_is_hw_initialize(struct fimc_is *is);
+void fimc_is_log_dump(const char *level, const void *buf, size_t len);
+
+#endif /* FIMC_IS_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-isp.c b/drivers/media/platform/exynos4-is/fimc-isp.c
new file mode 100644
index 000000000000..d63947f7b302
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-isp.c
@@ -0,0 +1,703 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *          Younghwan Joo <yhwan.joo@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.
+ */
+#define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
+
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/printk.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-device.h>
+
+#include "media-dev.h"
+#include "fimc-is-command.h"
+#include "fimc-is-param.h"
+#include "fimc-is-regs.h"
+#include "fimc-is.h"
+
+static int debug;
+module_param_named(debug_isp, debug, int, S_IRUGO | S_IWUSR);
+
+static const struct fimc_fmt fimc_isp_formats[FIMC_ISP_NUM_FORMATS] = {
+	{
+		.name		= "RAW8 (GRBG)",
+		.fourcc		= V4L2_PIX_FMT_SGRBG8,
+		.depth		= { 8 },
+		.color		= FIMC_FMT_RAW8,
+		.memplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_SGRBG8_1X8,
+	}, {
+		.name		= "RAW10 (GRBG)",
+		.fourcc		= V4L2_PIX_FMT_SGRBG10,
+		.depth		= { 10 },
+		.color		= FIMC_FMT_RAW10,
+		.memplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_SGRBG10_1X10,
+	}, {
+		.name		= "RAW12 (GRBG)",
+		.fourcc		= V4L2_PIX_FMT_SGRBG12,
+		.depth		= { 12 },
+		.color		= FIMC_FMT_RAW12,
+		.memplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_SGRBG12_1X12,
+	},
+};
+
+/**
+ * fimc_isp_find_format - lookup color format by fourcc or media bus code
+ * @pixelformat: fourcc to match, ignored if null
+ * @mbus_code: media bus code to match, ignored if null
+ * @index: index to the fimc_isp_formats array, ignored if negative
+ */
+const struct fimc_fmt *fimc_isp_find_format(const u32 *pixelformat,
+					const u32 *mbus_code, int index)
+{
+	const struct fimc_fmt *fmt, *def_fmt = NULL;
+	unsigned int i;
+	int id = 0;
+
+	if (index >= (int)ARRAY_SIZE(fimc_isp_formats))
+		return NULL;
+
+	for (i = 0; i < ARRAY_SIZE(fimc_isp_formats); ++i) {
+		fmt = &fimc_isp_formats[i];
+		if (pixelformat && fmt->fourcc == *pixelformat)
+			return fmt;
+		if (mbus_code && fmt->mbus_code == *mbus_code)
+			return fmt;
+		if (index == id)
+			def_fmt = fmt;
+		id++;
+	}
+	return def_fmt;
+}
+
+void fimc_isp_irq_handler(struct fimc_is *is)
+{
+	is->i2h_cmd.args[0] = mcuctl_read(is, MCUCTL_REG_ISSR(20));
+	is->i2h_cmd.args[1] = mcuctl_read(is, MCUCTL_REG_ISSR(21));
+
+	fimc_is_fw_clear_irq1(is, FIMC_IS_INT_FRAME_DONE_ISP);
+
+	/* TODO: Complete ISP DMA interrupt handler */
+	wake_up(&is->irq_queue);
+}
+
+/* Capture subdev media entity operations */
+static int fimc_is_link_setup(struct media_entity *entity,
+				const struct media_pad *local,
+				const struct media_pad *remote, u32 flags)
+{
+	return 0;
+}
+
+static const struct media_entity_operations fimc_is_subdev_media_ops = {
+	.link_setup = fimc_is_link_setup,
+};
+
+static int fimc_is_subdev_enum_mbus_code(struct v4l2_subdev *sd,
+				struct v4l2_subdev_fh *fh,
+				struct v4l2_subdev_mbus_code_enum *code)
+{
+	const struct fimc_fmt *fmt;
+
+	fmt = fimc_isp_find_format(NULL, NULL, code->index);
+	if (!fmt)
+		return -EINVAL;
+	code->code = fmt->mbus_code;
+	return 0;
+}
+
+static int fimc_isp_subdev_get_fmt(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_fh *fh,
+				   struct v4l2_subdev_format *fmt)
+{
+	struct fimc_isp *isp = v4l2_get_subdevdata(sd);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	struct v4l2_mbus_framefmt cur_fmt;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(fh, fmt->pad);
+		fmt->format = *mf;
+		return 0;
+	}
+
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+
+	mutex_lock(&isp->subdev_lock);
+	__is_get_frame_size(is, &cur_fmt);
+
+	if (fmt->pad == FIMC_ISP_SD_PAD_SINK) {
+		/* full camera input frame size */
+		mf->width = cur_fmt.width + FIMC_ISP_CAC_MARGIN_WIDTH;
+		mf->height = cur_fmt.height + FIMC_ISP_CAC_MARGIN_HEIGHT;
+		mf->code = V4L2_MBUS_FMT_SGRBG10_1X10;
+	} else {
+		/* crop size */
+		mf->width = cur_fmt.width;
+		mf->height = cur_fmt.height;
+		mf->code = V4L2_MBUS_FMT_YUV10_1X30;
+	}
+
+	mutex_unlock(&isp->subdev_lock);
+
+	v4l2_dbg(1, debug, sd, "%s: pad%d: fmt: 0x%x, %dx%d\n",
+		 __func__, fmt->pad, mf->code, mf->width, mf->height);
+
+	return 0;
+}
+
+static void __isp_subdev_try_format(struct fimc_isp *isp,
+				   struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+
+	if (fmt->pad == FIMC_ISP_SD_PAD_SINK) {
+		v4l_bound_align_image(&mf->width, FIMC_ISP_SINK_WIDTH_MIN,
+				FIMC_ISP_SINK_WIDTH_MAX, 0,
+				&mf->height, FIMC_ISP_SINK_HEIGHT_MIN,
+				FIMC_ISP_SINK_HEIGHT_MAX, 0, 0);
+		isp->subdev_fmt = *mf;
+	} else {
+		/* Allow changing format only on sink pad */
+		mf->width = isp->subdev_fmt.width - FIMC_ISP_CAC_MARGIN_WIDTH;
+		mf->height = isp->subdev_fmt.height - FIMC_ISP_CAC_MARGIN_HEIGHT;
+		mf->code = isp->subdev_fmt.code;
+	}
+}
+
+static int fimc_isp_subdev_set_fmt(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_fh *fh,
+				   struct v4l2_subdev_format *fmt)
+{
+	struct fimc_isp *isp = v4l2_get_subdevdata(sd);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	int ret = 0;
+
+	v4l2_dbg(1, debug, sd, "%s: pad%d: code: 0x%x, %dx%d\n",
+		 __func__, fmt->pad, mf->code, mf->width, mf->height);
+
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+
+	mutex_lock(&isp->subdev_lock);
+	__isp_subdev_try_format(isp, fmt);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(fh, fmt->pad);
+		*mf = fmt->format;
+		mutex_unlock(&isp->subdev_lock);
+		return 0;
+	}
+
+	if (sd->entity.stream_count == 0)
+		__is_set_frame_size(is, mf);
+	else
+		ret = -EBUSY;
+	mutex_unlock(&isp->subdev_lock);
+
+	return ret;
+}
+
+static int fimc_isp_subdev_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct fimc_isp *isp = v4l2_get_subdevdata(sd);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	int ret;
+
+	v4l2_dbg(1, debug, sd, "%s: on: %d\n", __func__, on);
+
+	if (!test_bit(IS_ST_INIT_DONE, &is->state))
+		return -EBUSY;
+
+	fimc_is_mem_barrier();
+
+	if (on) {
+		if (__get_pending_param_count(is)) {
+			ret = fimc_is_itf_s_param(is, true);
+			if (ret < 0)
+				return ret;
+		}
+
+		v4l2_dbg(1, debug, sd, "changing mode to %d\n",
+						is->config_index);
+		ret = fimc_is_itf_mode_change(is);
+		if (ret)
+			return -EINVAL;
+
+		clear_bit(IS_ST_STREAM_ON, &is->state);
+		fimc_is_hw_stream_on(is);
+		ret = fimc_is_wait_event(is, IS_ST_STREAM_ON, 1,
+					 FIMC_IS_CONFIG_TIMEOUT);
+		if (ret < 0) {
+			v4l2_err(sd, "stream on timeout\n");
+			return ret;
+		}
+	} else {
+		clear_bit(IS_ST_STREAM_OFF, &is->state);
+		fimc_is_hw_stream_off(is);
+		ret = fimc_is_wait_event(is, IS_ST_STREAM_OFF, 1,
+					 FIMC_IS_CONFIG_TIMEOUT);
+		if (ret < 0) {
+			v4l2_err(sd, "stream off timeout\n");
+			return ret;
+		}
+		is->setfile.sub_index = 0;
+	}
+
+	return 0;
+}
+
+static int fimc_isp_subdev_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct fimc_isp *isp = v4l2_get_subdevdata(sd);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	int ret = 0;
+
+	pr_debug("on: %d\n", on);
+
+	if (on) {
+		ret = pm_runtime_get_sync(&is->pdev->dev);
+		if (ret < 0)
+			return ret;
+		set_bit(IS_ST_PWR_ON, &is->state);
+
+		ret = fimc_is_start_firmware(is);
+		if (ret < 0) {
+			v4l2_err(sd, "firmware booting failed\n");
+			pm_runtime_put(&is->pdev->dev);
+			return ret;
+		}
+		set_bit(IS_ST_PWR_SUBIP_ON, &is->state);
+
+		ret = fimc_is_hw_initialize(is);
+	} else {
+		/* Close sensor */
+		if (!test_bit(IS_ST_PWR_ON, &is->state)) {
+			fimc_is_hw_close_sensor(is, 0);
+
+			ret = fimc_is_wait_event(is, IS_ST_OPEN_SENSOR, 0,
+						 FIMC_IS_CONFIG_TIMEOUT);
+			if (ret < 0) {
+				v4l2_err(sd, "sensor close timeout\n");
+				return ret;
+			}
+		}
+
+		/* SUB IP power off */
+		if (test_bit(IS_ST_PWR_SUBIP_ON, &is->state)) {
+			fimc_is_hw_subip_power_off(is);
+			ret = fimc_is_wait_event(is, IS_ST_PWR_SUBIP_ON, 0,
+						 FIMC_IS_CONFIG_TIMEOUT);
+			if (ret < 0) {
+				v4l2_err(sd, "sub-IP power off timeout\n");
+				return ret;
+			}
+		}
+
+		fimc_is_cpu_set_power(is, 0);
+		pm_runtime_put_sync(&is->pdev->dev);
+
+		clear_bit(IS_ST_PWR_ON, &is->state);
+		clear_bit(IS_ST_INIT_DONE, &is->state);
+		is->state = 0;
+		is->config[is->config_index].p_region_index1 = 0;
+		is->config[is->config_index].p_region_index2 = 0;
+		set_bit(IS_ST_IDLE, &is->state);
+		wmb();
+	}
+
+	return ret;
+}
+
+static int fimc_isp_subdev_open(struct v4l2_subdev *sd,
+				struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt fmt;
+	struct v4l2_mbus_framefmt *format;
+
+	format = v4l2_subdev_get_try_format(fh, FIMC_ISP_SD_PAD_SINK);
+
+	fmt.colorspace = V4L2_COLORSPACE_SRGB;
+	fmt.code = fimc_isp_formats[0].mbus_code;
+	fmt.width = DEFAULT_PREVIEW_STILL_WIDTH + FIMC_ISP_CAC_MARGIN_WIDTH;
+	fmt.height = DEFAULT_PREVIEW_STILL_HEIGHT + FIMC_ISP_CAC_MARGIN_HEIGHT;
+	fmt.field = V4L2_FIELD_NONE;
+	*format = fmt;
+
+	format = v4l2_subdev_get_try_format(fh, FIMC_ISP_SD_PAD_SRC_FIFO);
+	fmt.width = DEFAULT_PREVIEW_STILL_WIDTH;
+	fmt.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+	*format = fmt;
+
+	format = v4l2_subdev_get_try_format(fh, FIMC_ISP_SD_PAD_SRC_DMA);
+	*format = fmt;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops fimc_is_subdev_internal_ops = {
+	.open = fimc_isp_subdev_open,
+};
+
+static const struct v4l2_subdev_pad_ops fimc_is_subdev_pad_ops = {
+	.enum_mbus_code = fimc_is_subdev_enum_mbus_code,
+	.get_fmt = fimc_isp_subdev_get_fmt,
+	.set_fmt = fimc_isp_subdev_set_fmt,
+};
+
+static const struct v4l2_subdev_video_ops fimc_is_subdev_video_ops = {
+	.s_stream = fimc_isp_subdev_s_stream,
+};
+
+static const struct v4l2_subdev_core_ops fimc_is_core_ops = {
+	.s_power = fimc_isp_subdev_s_power,
+};
+
+static struct v4l2_subdev_ops fimc_is_subdev_ops = {
+	.core = &fimc_is_core_ops,
+	.video = &fimc_is_subdev_video_ops,
+	.pad = &fimc_is_subdev_pad_ops,
+};
+
+static int __ctrl_set_white_balance(struct fimc_is *is, int value)
+{
+	switch (value) {
+	case V4L2_WHITE_BALANCE_AUTO:
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_AUTO, 0);
+		break;
+	case V4L2_WHITE_BALANCE_DAYLIGHT:
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_ILLUMINATION,
+					ISP_AWB_ILLUMINATION_DAYLIGHT);
+		break;
+	case V4L2_WHITE_BALANCE_CLOUDY:
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_ILLUMINATION,
+					ISP_AWB_ILLUMINATION_CLOUDY);
+		break;
+	case V4L2_WHITE_BALANCE_INCANDESCENT:
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_ILLUMINATION,
+					ISP_AWB_ILLUMINATION_TUNGSTEN);
+		break;
+	case V4L2_WHITE_BALANCE_FLUORESCENT:
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_ILLUMINATION,
+					ISP_AWB_ILLUMINATION_FLUORESCENT);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int __ctrl_set_aewb_lock(struct fimc_is *is,
+				      struct v4l2_ctrl *ctrl)
+{
+	bool awb_lock = ctrl->val & V4L2_LOCK_WHITE_BALANCE;
+	bool ae_lock = ctrl->val & V4L2_LOCK_EXPOSURE;
+	struct isp_param *isp = &is->is_p_region->parameter.isp;
+	int cmd, ret;
+
+	cmd = ae_lock ? ISP_AA_COMMAND_STOP : ISP_AA_COMMAND_START;
+	isp->aa.cmd = cmd;
+	isp->aa.target = ISP_AA_TARGET_AE;
+	fimc_is_set_param_bit(is, PARAM_ISP_AA);
+	is->af.ae_lock_state = ae_lock;
+	wmb();
+
+	ret = fimc_is_itf_s_param(is, false);
+	if (ret < 0)
+		return ret;
+
+	cmd = awb_lock ? ISP_AA_COMMAND_STOP : ISP_AA_COMMAND_START;
+	isp->aa.cmd = cmd;
+	isp->aa.target = ISP_AA_TARGET_AE;
+	fimc_is_set_param_bit(is, PARAM_ISP_AA);
+	is->af.awb_lock_state = awb_lock;
+	wmb();
+
+	return fimc_is_itf_s_param(is, false);
+}
+
+/* Supported manual ISO values */
+static const s64 iso_qmenu[] = {
+	50, 100, 200, 400, 800,
+};
+
+static int __ctrl_set_iso(struct fimc_is *is, int value)
+{
+	unsigned int idx, iso;
+
+	if (value == V4L2_ISO_SENSITIVITY_AUTO) {
+		__is_set_isp_iso(is, ISP_ISO_COMMAND_AUTO, 0);
+		return 0;
+	}
+	idx = is->isp.ctrls.iso->val;
+	if (idx >= ARRAY_SIZE(iso_qmenu))
+		return -EINVAL;
+
+	iso = iso_qmenu[idx];
+	__is_set_isp_iso(is, ISP_ISO_COMMAND_MANUAL, iso);
+	return 0;
+}
+
+static int __ctrl_set_metering(struct fimc_is *is, unsigned int value)
+{
+	unsigned int val;
+
+	switch (value) {
+	case V4L2_EXPOSURE_METERING_AVERAGE:
+		val = ISP_METERING_COMMAND_AVERAGE;
+		break;
+	case V4L2_EXPOSURE_METERING_CENTER_WEIGHTED:
+		val = ISP_METERING_COMMAND_CENTER;
+		break;
+	case V4L2_EXPOSURE_METERING_SPOT:
+		val = ISP_METERING_COMMAND_SPOT;
+		break;
+	case V4L2_EXPOSURE_METERING_MATRIX:
+		val = ISP_METERING_COMMAND_MATRIX;
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	__is_set_isp_metering(is, IS_METERING_CONFIG_CMD, val);
+	return 0;
+}
+
+static int __ctrl_set_afc(struct fimc_is *is, int value)
+{
+	switch (value) {
+	case V4L2_CID_POWER_LINE_FREQUENCY_DISABLED:
+		__is_set_isp_afc(is, ISP_AFC_COMMAND_DISABLE, 0);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY_50HZ:
+		__is_set_isp_afc(is, ISP_AFC_COMMAND_MANUAL, 50);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY_60HZ:
+		__is_set_isp_afc(is, ISP_AFC_COMMAND_MANUAL, 60);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY_AUTO:
+		__is_set_isp_afc(is, ISP_AFC_COMMAND_AUTO, 0);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int __ctrl_set_image_effect(struct fimc_is *is, int value)
+{
+	static const u8 effects[][2] = {
+		{ V4L2_COLORFX_NONE,	 ISP_IMAGE_EFFECT_DISABLE },
+		{ V4L2_COLORFX_BW,	 ISP_IMAGE_EFFECT_MONOCHROME },
+		{ V4L2_COLORFX_SEPIA,	 ISP_IMAGE_EFFECT_SEPIA },
+		{ V4L2_COLORFX_NEGATIVE, ISP_IMAGE_EFFECT_NEGATIVE_MONO },
+		{ 16 /* TODO */,	 ISP_IMAGE_EFFECT_NEGATIVE_COLOR },
+	};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(effects); i++) {
+		if (effects[i][0] != value)
+			continue;
+
+		__is_set_isp_effect(is, effects[i][1]);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int fimc_is_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct fimc_isp *isp = ctrl_to_fimc_isp(ctrl);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	bool set_param = true;
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_CONTRAST:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_CONTRAST,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_SATURATION:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_SATURATION,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_SHARPNESS:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_SHARPNESS,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE_ABSOLUTE:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_EXPOSURE,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_BRIGHTNESS:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_BRIGHTNESS,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_HUE:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_HUE,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE_METERING:
+		ret = __ctrl_set_metering(is, ctrl->val);
+		break;
+
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE:
+		ret = __ctrl_set_white_balance(is, ctrl->val);
+		break;
+
+	case V4L2_CID_3A_LOCK:
+		ret = __ctrl_set_aewb_lock(is, ctrl);
+		set_param = false;
+		break;
+
+	case V4L2_CID_ISO_SENSITIVITY_AUTO:
+		ret = __ctrl_set_iso(is, ctrl->val);
+		break;
+
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		ret = __ctrl_set_afc(is, ctrl->val);
+		break;
+
+	case V4L2_CID_COLORFX:
+		__ctrl_set_image_effect(is, ctrl->val);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	if (ret < 0) {
+		v4l2_err(&isp->subdev, "Failed to set control: %s (%d)\n",
+						ctrl->name, ctrl->val);
+		return ret;
+	}
+
+	if (set_param && test_bit(IS_ST_STREAM_ON, &is->state))
+		return fimc_is_itf_s_param(is, true);
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops fimc_isp_ctrl_ops = {
+	.s_ctrl	= fimc_is_s_ctrl,
+};
+
+int fimc_isp_subdev_create(struct fimc_isp *isp)
+{
+	const struct v4l2_ctrl_ops *ops = &fimc_isp_ctrl_ops;
+	struct v4l2_ctrl_handler *handler = &isp->ctrls.handler;
+	struct v4l2_subdev *sd = &isp->subdev;
+	struct fimc_isp_ctrls *ctrls = &isp->ctrls;
+	int ret;
+
+	mutex_init(&isp->subdev_lock);
+
+	v4l2_subdev_init(sd, &fimc_is_subdev_ops);
+	sd->grp_id = GRP_ID_FIMC_IS;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, sizeof(sd->name), "FIMC-IS-ISP");
+
+	isp->subdev_pads[FIMC_ISP_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	isp->subdev_pads[FIMC_ISP_SD_PAD_SRC_FIFO].flags = MEDIA_PAD_FL_SOURCE;
+	isp->subdev_pads[FIMC_ISP_SD_PAD_SRC_DMA].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&sd->entity, FIMC_ISP_SD_PADS_NUM,
+				isp->subdev_pads, 0);
+	if (ret)
+		return ret;
+
+	v4l2_ctrl_handler_init(handler, 20);
+
+	ctrls->saturation = v4l2_ctrl_new_std(handler, ops, V4L2_CID_SATURATION,
+						-2, 2, 1, 0);
+	ctrls->brightness = v4l2_ctrl_new_std(handler, ops, V4L2_CID_BRIGHTNESS,
+						-4, 4, 1, 0);
+	ctrls->contrast = v4l2_ctrl_new_std(handler, ops, V4L2_CID_CONTRAST,
+						-2, 2, 1, 0);
+	ctrls->sharpness = v4l2_ctrl_new_std(handler, ops, V4L2_CID_SHARPNESS,
+						-2, 2, 1, 0);
+	ctrls->hue = v4l2_ctrl_new_std(handler, ops, V4L2_CID_HUE,
+						-2, 2, 1, 0);
+
+	ctrls->auto_wb = v4l2_ctrl_new_std_menu(handler, ops,
+					V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE,
+					8, ~0x14e, V4L2_WHITE_BALANCE_AUTO);
+
+	ctrls->exposure = v4l2_ctrl_new_std(handler, ops,
+					V4L2_CID_EXPOSURE_ABSOLUTE,
+					-4, 4, 1, 0);
+
+	ctrls->exp_metering = v4l2_ctrl_new_std_menu(handler, ops,
+					V4L2_CID_EXPOSURE_METERING, 3,
+					~0xf, V4L2_EXPOSURE_METERING_AVERAGE);
+
+	v4l2_ctrl_new_std_menu(handler, ops, V4L2_CID_POWER_LINE_FREQUENCY,
+					V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+					V4L2_CID_POWER_LINE_FREQUENCY_AUTO);
+	/* ISO sensitivity */
+	ctrls->auto_iso = v4l2_ctrl_new_std_menu(handler, ops,
+			V4L2_CID_ISO_SENSITIVITY_AUTO, 1, 0,
+			V4L2_ISO_SENSITIVITY_AUTO);
+
+	ctrls->iso = v4l2_ctrl_new_int_menu(handler, ops,
+			V4L2_CID_ISO_SENSITIVITY, ARRAY_SIZE(iso_qmenu) - 1,
+			ARRAY_SIZE(iso_qmenu)/2 - 1, iso_qmenu);
+
+	ctrls->aewb_lock = v4l2_ctrl_new_std(handler, ops,
+					V4L2_CID_3A_LOCK, 0, 0x3, 0, 0);
+
+	/* TODO: Add support for NEGATIVE_COLOR option */
+	ctrls->colorfx = v4l2_ctrl_new_std_menu(handler, ops, V4L2_CID_COLORFX,
+			V4L2_COLORFX_SET_CBCR + 1, ~0x1000f, V4L2_COLORFX_NONE);
+
+	if (handler->error) {
+		media_entity_cleanup(&sd->entity);
+		return handler->error;
+	}
+
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_iso,
+			V4L2_ISO_SENSITIVITY_MANUAL, false);
+
+	sd->ctrl_handler = handler;
+	sd->internal_ops = &fimc_is_subdev_internal_ops;
+	sd->entity.ops = &fimc_is_subdev_media_ops;
+	v4l2_set_subdevdata(sd, isp);
+
+	return 0;
+}
+
+void fimc_isp_subdev_destroy(struct fimc_isp *isp)
+{
+	struct v4l2_subdev *sd = &isp->subdev;
+
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(&isp->ctrls.handler);
+	v4l2_set_subdevdata(sd, NULL);
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-isp.h b/drivers/media/platform/exynos4-is/fimc-isp.h
new file mode 100644
index 000000000000..800aba7ab4a7
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-isp.h
@@ -0,0 +1,181 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *          Younghwan Joo <yhwan.joo@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 FIMC_ISP_H_
+#define FIMC_ISP_H_
+
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <media/media-entity.h>
+#include <media/videobuf2-core.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mediabus.h>
+#include <media/s5p_fimc.h>
+
+/* FIXME: revisit these constraints */
+#define FIMC_ISP_SINK_WIDTH_MIN		(16 + 8)
+#define FIMC_ISP_SINK_HEIGHT_MIN	(12 + 8)
+#define FIMC_ISP_SOURCE_WIDTH_MIN	8
+#define FIMC_ISP_SOURC_HEIGHT_MIN	8
+#define FIMC_ISP_CAC_MARGIN_WIDTH	16
+#define FIMC_ISP_CAC_MARGIN_HEIGHT	12
+
+#define FIMC_ISP_SINK_WIDTH_MAX		(4000 - 16)
+#define FIMC_ISP_SINK_HEIGHT_MAX	(4000 + 12)
+#define FIMC_ISP_SOURCE_WIDTH_MAX	4000
+#define FIMC_ISP_SOURC_HEIGHT_MAX	4000
+
+#define FIMC_ISP_NUM_FORMATS		3
+#define FIMC_ISP_REQ_BUFS_MIN		2
+
+#define FIMC_ISP_SD_PAD_SINK		0
+#define FIMC_ISP_SD_PAD_SRC_FIFO	1
+#define FIMC_ISP_SD_PAD_SRC_DMA		2
+#define FIMC_ISP_SD_PADS_NUM		3
+#define FIMC_ISP_MAX_PLANES		1
+
+/**
+ * struct fimc_isp_frame - source/target frame properties
+ * @width: full image width
+ * @height: full image height
+ * @rect: crop/composition rectangle
+ */
+struct fimc_isp_frame {
+	u16 width;
+	u16 height;
+	struct v4l2_rect rect;
+};
+
+struct fimc_isp_ctrls {
+	struct v4l2_ctrl_handler handler;
+
+	/* Auto white balance */
+	struct v4l2_ctrl *auto_wb;
+	/* Auto ISO control cluster */
+	struct {
+		struct v4l2_ctrl *auto_iso;
+		struct v4l2_ctrl *iso;
+	};
+	/* Adjust - contrast */
+	struct v4l2_ctrl *contrast;
+	/* Adjust - saturation */
+	struct v4l2_ctrl *saturation;
+	/* Adjust - sharpness */
+	struct v4l2_ctrl *sharpness;
+	/* Adjust - brightness */
+	struct v4l2_ctrl *brightness;
+	/* Adjust - hue */
+	struct v4l2_ctrl *hue;
+
+	/* Auto/manual exposure */
+	struct v4l2_ctrl *auto_exp;
+	/* Manual exposure value */
+	struct v4l2_ctrl *exposure;
+	/* AE/AWB lock/unlock */
+	struct v4l2_ctrl *aewb_lock;
+	/* Exposure metering mode */
+	struct v4l2_ctrl *exp_metering;
+	/* AFC */
+	struct v4l2_ctrl *afc;
+	/* ISP image effect */
+	struct v4l2_ctrl *colorfx;
+};
+
+/**
+ * struct fimc_is_video - fimc-is video device structure
+ * @vdev: video_device structure
+ * @type: video device type (CAPTURE/OUTPUT)
+ * @pad: video device media (sink) pad
+ * @pending_buf_q: pending buffers queue head
+ * @active_buf_q: a queue head of buffers scheduled in hardware
+ * @vb_queue: vb2 buffer queue
+ * @active_buf_count: number of video buffers scheduled in hardware
+ * @frame_count: counter of frames dequeued to user space
+ * @reqbufs_count: number of buffers requested with REQBUFS ioctl
+ * @format: current pixel format
+ */
+struct fimc_is_video {
+	struct video_device	vdev;
+	enum v4l2_buf_type	type;
+	struct media_pad	pad;
+	struct list_head	pending_buf_q;
+	struct list_head	active_buf_q;
+	struct vb2_queue	vb_queue;
+	unsigned int		frame_count;
+	unsigned int		reqbufs_count;
+	int			streaming;
+	unsigned long		payload[FIMC_ISP_MAX_PLANES];
+	const struct fimc_fmt	*format;
+};
+
+/**
+ * struct fimc_isp - FIMC-IS ISP data structure
+ * @pdev: pointer to FIMC-IS platform device
+ * @alloc_ctx: videobuf2 memory allocator context
+ * @subdev: ISP v4l2_subdev
+ * @subdev_pads: the ISP subdev media pads
+ * @ctrl_handler: v4l2 controls handler
+ * @test_pattern: test pattern controls
+ * @pipeline: video capture pipeline data structure
+ * @video_lock: mutex serializing video device and the subdev operations
+ * @fmt: pointer to color format description structure
+ * @payload: image size in bytes (w x h x bpp)
+ * @inp_frame: camera input frame structure
+ * @out_frame: DMA output frame structure
+ * @source_subdev_grp_id: group id of remote source subdev
+ * @cac_margin_x: horizontal CAC margin in pixels
+ * @cac_margin_y: vertical CAC margin in pixels
+ * @state: driver state flags
+ * @video_capture: the ISP block video capture device
+ */
+struct fimc_isp {
+	struct platform_device		*pdev;
+	struct vb2_alloc_ctx		*alloc_ctx;
+	struct v4l2_subdev		subdev;
+	struct media_pad		subdev_pads[FIMC_ISP_SD_PADS_NUM];
+	struct v4l2_mbus_framefmt	subdev_fmt;
+	struct v4l2_ctrl		*test_pattern;
+	struct fimc_isp_ctrls		ctrls;
+
+	struct mutex			video_lock;
+	struct mutex			subdev_lock;
+
+	struct fimc_isp_frame		inp_frame;
+	struct fimc_isp_frame		out_frame;
+	unsigned int			source_subdev_grp_id;
+
+	unsigned int			cac_margin_x;
+	unsigned int			cac_margin_y;
+
+	unsigned long			state;
+
+	struct fimc_is_video		video_capture;
+};
+
+#define ctrl_to_fimc_isp(_ctrl) \
+	container_of(ctrl->handler, struct fimc_isp, ctrls.handler)
+
+struct fimc_is;
+
+int fimc_isp_subdev_create(struct fimc_isp *isp);
+void fimc_isp_subdev_destroy(struct fimc_isp *isp);
+void fimc_isp_irq_handler(struct fimc_is *is);
+int fimc_is_create_controls(struct fimc_isp *isp);
+int fimc_is_delete_controls(struct fimc_isp *isp);
+const struct fimc_fmt *fimc_isp_find_format(const u32 *pixelformat,
+					const u32 *mbus_code, int index);
+#endif /* FIMC_ISP_H_ */
diff --git a/drivers/media/platform/s5p-fimc/fimc-lite-reg.c b/drivers/media/platform/exynos4-is/fimc-lite-reg.c
index ac9663ce2a49..8cc0d39a2fea 100644
--- a/drivers/media/platform/s5p-fimc/fimc-lite-reg.c
+++ b/drivers/media/platform/exynos4-is/fimc-lite-reg.c
@@ -127,7 +127,7 @@ static const u32 src_pixfmt_map[8][3] = {
 /* Set camera input pixel format and resolution */
 void flite_hw_set_source_format(struct fimc_lite *dev, struct flite_frame *f)
 {
-	enum v4l2_mbus_pixelcode pixelcode = dev->fmt->mbus_code;
+	enum v4l2_mbus_pixelcode pixelcode = f->fmt->mbus_code;
 	int i = ARRAY_SIZE(src_pixfmt_map);
 	u32 cfg;
 
@@ -227,7 +227,7 @@ static void flite_hw_set_out_order(struct fimc_lite *dev, struct flite_frame *f)
 	int i = ARRAY_SIZE(pixcode);
 
 	while (--i >= 0)
-		if (pixcode[i][0] == dev->fmt->mbus_code)
+		if (pixcode[i][0] == f->fmt->mbus_code)
 			break;
 	cfg &= ~FLITE_REG_CIODMAFMT_YCBCR_ORDER_MASK;
 	writel(cfg | pixcode[i][1], dev->regs + FLITE_REG_CIODMAFMT);
diff --git a/drivers/media/platform/s5p-fimc/fimc-lite-reg.h b/drivers/media/platform/exynos4-is/fimc-lite-reg.h
index 0e345844c13a..390383941c19 100644
--- a/drivers/media/platform/s5p-fimc/fimc-lite-reg.h
+++ b/drivers/media/platform/exynos4-is/fimc-lite-reg.h
@@ -72,10 +72,10 @@
 #define FLITE_REG_CIODMAFMT			0x18
 #define FLITE_REG_CIODMAFMT_RAW_CON		(1 << 15)
 #define FLITE_REG_CIODMAFMT_PACK12		(1 << 14)
-#define FLITE_REG_CIODMAFMT_CRYCBY		(0 << 4)
-#define FLITE_REG_CIODMAFMT_CBYCRY		(1 << 4)
-#define FLITE_REG_CIODMAFMT_YCRYCB		(2 << 4)
-#define FLITE_REG_CIODMAFMT_YCBYCR		(3 << 4)
+#define FLITE_REG_CIODMAFMT_YCBYCR		(0 << 4)
+#define FLITE_REG_CIODMAFMT_YCRYCB		(1 << 4)
+#define FLITE_REG_CIODMAFMT_CBYCRY		(2 << 4)
+#define FLITE_REG_CIODMAFMT_CRYCBY		(3 << 4)
 #define FLITE_REG_CIODMAFMT_YCBCR_ORDER_MASK	(0x3 << 4)
 
 /* Camera Output Canvas */
diff --git a/drivers/media/platform/s5p-fimc/fimc-lite.c b/drivers/media/platform/exynos4-is/fimc-lite.c
index bbc35de7db27..14bb7bc8adbe 100644
--- a/drivers/media/platform/s5p-fimc/fimc-lite.c
+++ b/drivers/media/platform/exynos4-is/fimc-lite.c
@@ -11,12 +11,14 @@
 #define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
 
 #include <linux/bug.h>
+#include <linux/clk.h>
 #include <linux/device.h>
 #include <linux/errno.h>
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
 #include <linux/list.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/types.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
@@ -30,7 +32,6 @@
 #include <media/videobuf2-dma-contig.h>
 #include <media/s5p_fimc.h>
 
-#include "fimc-mdevice.h"
 #include "fimc-core.h"
 #include "fimc-lite.h"
 #include "fimc-lite-reg.h"
@@ -46,6 +47,7 @@ static const struct fimc_fmt fimc_lite_formats[] = {
 		.color		= FIMC_FMT_YCBYCR422,
 		.memplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_YUYV8_2X8,
+		.flags		= FMT_FLAGS_YUV,
 	}, {
 		.name		= "YUV 4:2:2 packed, CbYCrY",
 		.fourcc		= V4L2_PIX_FMT_UYVY,
@@ -53,6 +55,7 @@ static const struct fimc_fmt fimc_lite_formats[] = {
 		.color		= FIMC_FMT_CBYCRY422,
 		.memplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_UYVY8_2X8,
+		.flags		= FMT_FLAGS_YUV,
 	}, {
 		.name		= "YUV 4:2:2 packed, CrYCbY",
 		.fourcc		= V4L2_PIX_FMT_VYUY,
@@ -60,6 +63,7 @@ static const struct fimc_fmt fimc_lite_formats[] = {
 		.color		= FIMC_FMT_CRYCBY422,
 		.memplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_VYUY8_2X8,
+		.flags		= FMT_FLAGS_YUV,
 	}, {
 		.name		= "YUV 4:2:2 packed, YCrYCb",
 		.fourcc		= V4L2_PIX_FMT_YVYU,
@@ -67,6 +71,7 @@ static const struct fimc_fmt fimc_lite_formats[] = {
 		.color		= FIMC_FMT_YCRYCB422,
 		.memplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_YVYU8_2X8,
+		.flags		= FMT_FLAGS_YUV,
 	}, {
 		.name		= "RAW8 (GRBG)",
 		.fourcc		= V4L2_PIX_FMT_SGRBG8,
@@ -74,6 +79,7 @@ static const struct fimc_fmt fimc_lite_formats[] = {
 		.color		= FIMC_FMT_RAW8,
 		.memplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_SGRBG8_1X8,
+		.flags		= FMT_FLAGS_RAW_BAYER,
 	}, {
 		.name		= "RAW10 (GRBG)",
 		.fourcc		= V4L2_PIX_FMT_SGRBG10,
@@ -81,6 +87,7 @@ static const struct fimc_fmt fimc_lite_formats[] = {
 		.color		= FIMC_FMT_RAW10,
 		.memplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_SGRBG10_1X10,
+		.flags		= FMT_FLAGS_RAW_BAYER,
 	}, {
 		.name		= "RAW12 (GRBG)",
 		.fourcc		= V4L2_PIX_FMT_SGRBG12,
@@ -88,6 +95,7 @@ static const struct fimc_fmt fimc_lite_formats[] = {
 		.color		= FIMC_FMT_RAW12,
 		.memplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_SGRBG12_1X12,
+		.flags		= FMT_FLAGS_RAW_BAYER,
 	},
 };
 
@@ -95,10 +103,11 @@ static const struct fimc_fmt fimc_lite_formats[] = {
  * fimc_lite_find_format - lookup fimc color format by fourcc or media bus code
  * @pixelformat: fourcc to match, ignored if null
  * @mbus_code: media bus code to match, ignored if null
+ * @mask: the color format flags to match
  * @index: index to the fimc_lite_formats array, ignored if negative
  */
 static const struct fimc_fmt *fimc_lite_find_format(const u32 *pixelformat,
-					const u32 *mbus_code, int index)
+			const u32 *mbus_code, unsigned int mask, int index)
 {
 	const struct fimc_fmt *fmt, *def_fmt = NULL;
 	unsigned int i;
@@ -109,6 +118,8 @@ static const struct fimc_fmt *fimc_lite_find_format(const u32 *pixelformat,
 
 	for (i = 0; i < ARRAY_SIZE(fimc_lite_formats); ++i) {
 		fmt = &fimc_lite_formats[i];
+		if (mask && !(fmt->flags & mask))
+			continue;
 		if (pixelformat && fmt->fourcc == *pixelformat)
 			return fmt;
 		if (mbus_code && fmt->mbus_code == *mbus_code)
@@ -120,26 +131,49 @@ static const struct fimc_fmt *fimc_lite_find_format(const u32 *pixelformat,
 	return def_fmt;
 }
 
+/* Called with the media graph mutex held or @me stream_count > 0. */
+static struct v4l2_subdev *__find_remote_sensor(struct media_entity *me)
+{
+	struct media_pad *pad = &me->pads[0];
+	struct v4l2_subdev *sd;
+
+	while (pad->flags & MEDIA_PAD_FL_SINK) {
+		/* source pad */
+		pad = media_entity_remote_source(pad);
+		if (pad == NULL ||
+		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+			break;
+
+		sd = media_entity_to_v4l2_subdev(pad->entity);
+
+		if (sd->grp_id == GRP_ID_FIMC_IS_SENSOR ||
+		    sd->grp_id == GRP_ID_SENSOR)
+			return sd;
+		/* sink pad */
+		pad = &sd->entity.pads[0];
+	}
+	return NULL;
+}
+
 static int fimc_lite_hw_init(struct fimc_lite *fimc, bool isp_output)
 {
-	struct fimc_pipeline *pipeline = &fimc->pipeline;
-	struct v4l2_subdev *sensor;
-	struct fimc_sensor_info *si;
+	struct fimc_source_info *si;
 	unsigned long flags;
 
-	sensor = isp_output ? fimc->sensor : pipeline->subdevs[IDX_SENSOR];
-
-	if (sensor == NULL)
+	if (fimc->sensor == NULL)
 		return -ENXIO;
 
-	if (fimc->fmt == NULL)
+	if (fimc->inp_frame.fmt == NULL || fimc->out_frame.fmt == NULL)
 		return -EINVAL;
 
 	/* Get sensor configuration data from the sensor subdev */
-	si = v4l2_get_subdev_hostdata(sensor);
+	si = v4l2_get_subdev_hostdata(fimc->sensor);
+	if (!si)
+		return -EINVAL;
+
 	spin_lock_irqsave(&fimc->slock, flags);
 
-	flite_hw_set_camera_bus(fimc, &si->pdata);
+	flite_hw_set_camera_bus(fimc, si);
 	flite_hw_set_source_format(fimc, &fimc->inp_frame);
 	flite_hw_set_window_offset(fimc, &fimc->inp_frame);
 	flite_hw_set_output_dma(fimc, &fimc->out_frame, !isp_output);
@@ -339,13 +373,13 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *pfmt,
 	const struct v4l2_pix_format_mplane *pixm = NULL;
 	struct fimc_lite *fimc = vq->drv_priv;
 	struct flite_frame *frame = &fimc->out_frame;
-	const struct fimc_fmt *fmt = fimc->fmt;
+	const struct fimc_fmt *fmt = frame->fmt;
 	unsigned long wh;
 	int i;
 
 	if (pfmt) {
 		pixm = &pfmt->fmt.pix_mp;
-		fmt = fimc_lite_find_format(&pixm->pixelformat, NULL, -1);
+		fmt = fimc_lite_find_format(&pixm->pixelformat, NULL, 0, -1);
 		wh = pixm->width * pixm->height;
 	} else {
 		wh = frame->f_width * frame->f_height;
@@ -374,10 +408,10 @@ static int buffer_prepare(struct vb2_buffer *vb)
 	struct fimc_lite *fimc = vq->drv_priv;
 	int i;
 
-	if (fimc->fmt == NULL)
+	if (fimc->out_frame.fmt == NULL)
 		return -EINVAL;
 
-	for (i = 0; i < fimc->fmt->memplanes; i++) {
+	for (i = 0; i < fimc->out_frame.fmt->memplanes; i++) {
 		unsigned long size = fimc->payload[i];
 
 		if (vb2_plane_size(vb, i) < size) {
@@ -425,24 +459,12 @@ static void buffer_queue(struct vb2_buffer *vb)
 	spin_unlock_irqrestore(&fimc->slock, flags);
 }
 
-static void fimc_lock(struct vb2_queue *vq)
-{
-	struct fimc_lite *fimc = vb2_get_drv_priv(vq);
-	mutex_lock(&fimc->lock);
-}
-
-static void fimc_unlock(struct vb2_queue *vq)
-{
-	struct fimc_lite *fimc = vb2_get_drv_priv(vq);
-	mutex_unlock(&fimc->lock);
-}
-
 static const struct vb2_ops fimc_lite_qops = {
 	.queue_setup	 = queue_setup,
 	.buf_prepare	 = buffer_prepare,
 	.buf_queue	 = buffer_queue,
-	.wait_prepare	 = fimc_unlock,
-	.wait_finish	 = fimc_lock,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
 	.start_streaming = start_streaming,
 	.stop_streaming	 = stop_streaming,
 };
@@ -467,99 +489,73 @@ static int fimc_lite_open(struct file *file)
 	mutex_lock(&fimc->lock);
 	if (atomic_read(&fimc->out_path) != FIMC_IO_DMA) {
 		ret = -EBUSY;
-		goto done;
+		goto unlock;
 	}
 
 	set_bit(ST_FLITE_IN_USE, &fimc->state);
 	ret = pm_runtime_get_sync(&fimc->pdev->dev);
 	if (ret < 0)
-		goto done;
+		goto unlock;
 
 	ret = v4l2_fh_open(file);
 	if (ret < 0)
-		goto done;
+		goto err_pm;
 
-	if (++fimc->ref_count == 1 &&
-	    atomic_read(&fimc->out_path) == FIMC_IO_DMA) {
-		ret = fimc_pipeline_call(fimc, open, &fimc->pipeline,
-					 &fimc->vfd.entity, true);
-		if (ret < 0) {
-			pm_runtime_put_sync(&fimc->pdev->dev);
-			fimc->ref_count--;
-			v4l2_fh_release(file);
-			clear_bit(ST_FLITE_IN_USE, &fimc->state);
-		}
+	if (!v4l2_fh_is_singular_file(file) ||
+	    atomic_read(&fimc->out_path) != FIMC_IO_DMA)
+		goto unlock;
 
+	ret = fimc_pipeline_call(fimc, open, &fimc->pipeline,
+						me, true);
+	if (!ret) {
 		fimc_lite_clear_event_counters(fimc);
+		fimc->ref_count++;
+		goto unlock;
 	}
-done:
+
+	v4l2_fh_release(file);
+err_pm:
+	pm_runtime_put_sync(&fimc->pdev->dev);
+	clear_bit(ST_FLITE_IN_USE, &fimc->state);
+unlock:
 	mutex_unlock(&fimc->lock);
 	mutex_unlock(&me->parent->graph_mutex);
 	return ret;
 }
 
-static int fimc_lite_close(struct file *file)
+static int fimc_lite_release(struct file *file)
 {
 	struct fimc_lite *fimc = video_drvdata(file);
-	int ret;
 
 	mutex_lock(&fimc->lock);
 
-	if (--fimc->ref_count == 0 &&
+	if (v4l2_fh_is_singular_file(file) &&
 	    atomic_read(&fimc->out_path) == FIMC_IO_DMA) {
+		if (fimc->streaming) {
+			media_entity_pipeline_stop(&fimc->vfd.entity);
+			fimc->streaming = false;
+		}
 		clear_bit(ST_FLITE_IN_USE, &fimc->state);
 		fimc_lite_stop_capture(fimc, false);
 		fimc_pipeline_call(fimc, close, &fimc->pipeline);
-		clear_bit(ST_FLITE_SUSPENDED, &fimc->state);
+		fimc->ref_count--;
 	}
 
+	vb2_fop_release(file);
 	pm_runtime_put(&fimc->pdev->dev);
+	clear_bit(ST_FLITE_SUSPENDED, &fimc->state);
 
-	if (fimc->ref_count == 0)
-		vb2_queue_release(&fimc->vb_queue);
-
-	ret = v4l2_fh_release(file);
-
-	mutex_unlock(&fimc->lock);
-	return ret;
-}
-
-static unsigned int fimc_lite_poll(struct file *file,
-				   struct poll_table_struct *wait)
-{
-	struct fimc_lite *fimc = video_drvdata(file);
-	int ret;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return POLL_ERR;
-
-	ret = vb2_poll(&fimc->vb_queue, file, wait);
-	mutex_unlock(&fimc->lock);
-
-	return ret;
-}
-
-static int fimc_lite_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct fimc_lite *fimc = video_drvdata(file);
-	int ret;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
-	ret = vb2_mmap(&fimc->vb_queue, vma);
 	mutex_unlock(&fimc->lock);
-
-	return ret;
+	return 0;
 }
 
 static const struct v4l2_file_operations fimc_lite_fops = {
 	.owner		= THIS_MODULE,
 	.open		= fimc_lite_open,
-	.release	= fimc_lite_close,
-	.poll		= fimc_lite_poll,
+	.release	= fimc_lite_release,
+	.poll		= vb2_fop_poll,
 	.unlocked_ioctl	= video_ioctl2,
-	.mmap		= fimc_lite_mmap,
+	.mmap		= vb2_fop_mmap,
 };
 
 /*
@@ -570,10 +566,23 @@ static const struct fimc_fmt *fimc_lite_try_format(struct fimc_lite *fimc,
 					u32 *width, u32 *height,
 					u32 *code, u32 *fourcc, int pad)
 {
-	struct flite_variant *variant = fimc->variant;
+	struct flite_drvdata *dd = fimc->dd;
 	const struct fimc_fmt *fmt;
+	unsigned int flags = 0;
 
-	fmt = fimc_lite_find_format(fourcc, code, 0);
+	if (pad == FLITE_SD_PAD_SINK) {
+		v4l_bound_align_image(width, 8, dd->max_width,
+				      ffs(dd->out_width_align) - 1,
+				      height, 0, dd->max_height, 0, 0);
+	} else {
+		v4l_bound_align_image(width, 8, fimc->inp_frame.rect.width,
+				      ffs(dd->out_width_align) - 1,
+				      height, 0, fimc->inp_frame.rect.height,
+				      0, 0);
+		flags = fimc->inp_frame.fmt->flags;
+	}
+
+	fmt = fimc_lite_find_format(fourcc, code, flags, 0);
 	if (WARN_ON(!fmt))
 		return NULL;
 
@@ -582,17 +591,6 @@ static const struct fimc_fmt *fimc_lite_try_format(struct fimc_lite *fimc,
 	if (fourcc)
 		*fourcc = fmt->fourcc;
 
-	if (pad == FLITE_SD_PAD_SINK) {
-		v4l_bound_align_image(width, 8, variant->max_width,
-				      ffs(variant->out_width_align) - 1,
-				      height, 0, variant->max_height, 0, 0);
-	} else {
-		v4l_bound_align_image(width, 8, fimc->inp_frame.rect.width,
-				      ffs(variant->out_width_align) - 1,
-				      height, 0, fimc->inp_frame.rect.height,
-				      0, 0);
-	}
-
 	v4l2_dbg(1, debug, &fimc->subdev, "code: 0x%x, %dx%d\n",
 		 code ? *code : 0, *width, *height);
 
@@ -608,7 +606,7 @@ static void fimc_lite_try_crop(struct fimc_lite *fimc, struct v4l2_rect *r)
 
 	/* Adjust left/top if cropping rectangle got out of bounds */
 	r->left = clamp_t(u32, r->left, 0, frame->f_width - r->width);
-	r->left = round_down(r->left, fimc->variant->win_hor_offs_align);
+	r->left = round_down(r->left, fimc->dd->win_hor_offs_align);
 	r->top  = clamp_t(u32, r->top, 0, frame->f_height - r->height);
 
 	v4l2_dbg(1, debug, &fimc->subdev, "(%d,%d)/%dx%d, sink fmt: %dx%d\n",
@@ -628,7 +626,7 @@ static void fimc_lite_try_compose(struct fimc_lite *fimc, struct v4l2_rect *r)
 
 	/* Adjust left/top if the composing rectangle got out of bounds */
 	r->left = clamp_t(u32, r->left, 0, frame->f_width - r->width);
-	r->left = round_down(r->left, fimc->variant->out_hor_offs_align);
+	r->left = round_down(r->left, fimc->dd->out_hor_offs_align);
 	r->top  = clamp_t(u32, r->top, 0, fimc->out_frame.f_height - r->height);
 
 	v4l2_dbg(1, debug, &fimc->subdev, "(%d,%d)/%dx%d, source fmt: %dx%d\n",
@@ -671,7 +669,7 @@ static int fimc_lite_g_fmt_mplane(struct file *file, void *fh,
 	struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
 	struct v4l2_plane_pix_format *plane_fmt = &pixm->plane_fmt[0];
 	struct flite_frame *frame = &fimc->out_frame;
-	const struct fimc_fmt *fmt = fimc->fmt;
+	const struct fimc_fmt *fmt = frame->fmt;
 
 	plane_fmt->bytesperline = (frame->f_width * fmt->depth[0]) / 8;
 	plane_fmt->sizeimage = plane_fmt->bytesperline * frame->f_height;
@@ -689,18 +687,31 @@ static int fimc_lite_try_fmt(struct fimc_lite *fimc,
 			     struct v4l2_pix_format_mplane *pixm,
 			     const struct fimc_fmt **ffmt)
 {
-	struct flite_variant *variant = fimc->variant;
 	u32 bpl = pixm->plane_fmt[0].bytesperline;
+	struct flite_drvdata *dd = fimc->dd;
+	const struct fimc_fmt *inp_fmt = fimc->inp_frame.fmt;
 	const struct fimc_fmt *fmt;
 
-	fmt = fimc_lite_find_format(&pixm->pixelformat, NULL, 0);
+	if (WARN_ON(inp_fmt == NULL))
+		return -EINVAL;
+	/*
+	 * We allow some flexibility only for YUV formats. In case of raw
+	 * raw Bayer the FIMC-LITE's output format must match its camera
+	 * interface input format.
+	 */
+	if (inp_fmt->flags & FMT_FLAGS_YUV)
+		fmt = fimc_lite_find_format(&pixm->pixelformat, NULL,
+						inp_fmt->flags, 0);
+	else
+		fmt = inp_fmt;
+
 	if (WARN_ON(fmt == NULL))
 		return -EINVAL;
 	if (ffmt)
 		*ffmt = fmt;
-	v4l_bound_align_image(&pixm->width, 8, variant->max_width,
-			      ffs(variant->out_width_align) - 1,
-			      &pixm->height, 0, variant->max_height, 0, 0);
+	v4l_bound_align_image(&pixm->width, 8, dd->max_width,
+			      ffs(dd->out_width_align) - 1,
+			      &pixm->height, 0, dd->max_height, 0, 0);
 
 	if ((bpl == 0 || ((bpl * 8) / fmt->depth[0]) < pixm->width))
 		pixm->plane_fmt[0].bytesperline = (pixm->width *
@@ -720,7 +731,6 @@ static int fimc_lite_try_fmt_mplane(struct file *file, void *fh,
 				    struct v4l2_format *f)
 {
 	struct fimc_lite *fimc = video_drvdata(file);
-
 	return fimc_lite_try_fmt(fimc, &f->fmt.pix_mp, NULL);
 }
 
@@ -740,7 +750,7 @@ static int fimc_lite_s_fmt_mplane(struct file *file, void *priv,
 	if (ret < 0)
 		return ret;
 
-	fimc->fmt = fmt;
+	frame->fmt = fmt;
 	fimc->payload[0] = max((pixm->width * pixm->height * fmt->depth[0]) / 8,
 			       pixm->plane_fmt[0].sizeimage);
 	frame->f_width = pixm->width;
@@ -766,7 +776,7 @@ static int fimc_pipeline_validate(struct fimc_lite *fimc)
 			struct flite_frame *ff = &fimc->out_frame;
 			sink_fmt.format.width = ff->f_width;
 			sink_fmt.format.height = ff->f_height;
-			sink_fmt.format.code = fimc->fmt->mbus_code;
+			sink_fmt.format.code = fimc->inp_frame.fmt->mbus_code;
 		} else {
 			sink_fmt.pad = pad->index;
 			sink_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
@@ -800,37 +810,47 @@ static int fimc_lite_streamon(struct file *file, void *priv,
 			      enum v4l2_buf_type type)
 {
 	struct fimc_lite *fimc = video_drvdata(file);
-	struct v4l2_subdev *sensor = fimc->pipeline.subdevs[IDX_SENSOR];
+	struct media_entity *entity = &fimc->vfd.entity;
 	struct fimc_pipeline *p = &fimc->pipeline;
 	int ret;
 
 	if (fimc_lite_active(fimc))
 		return -EBUSY;
 
-	ret = media_entity_pipeline_start(&sensor->entity, p->m_pipeline);
+	ret = media_entity_pipeline_start(entity, p->m_pipeline);
 	if (ret < 0)
 		return ret;
 
 	ret = fimc_pipeline_validate(fimc);
-	if (ret) {
-		media_entity_pipeline_stop(&sensor->entity);
+	if (ret < 0)
+		goto err_p_stop;
+
+	fimc->sensor = __find_remote_sensor(&fimc->subdev.entity);
+
+	ret = vb2_ioctl_streamon(file, priv, type);
+	if (!ret) {
+		fimc->streaming = true;
 		return ret;
 	}
 
-	return vb2_streamon(&fimc->vb_queue, type);
+err_p_stop:
+	media_entity_pipeline_stop(entity);
+	return 0;
 }
 
 static int fimc_lite_streamoff(struct file *file, void *priv,
 			       enum v4l2_buf_type type)
 {
 	struct fimc_lite *fimc = video_drvdata(file);
-	struct v4l2_subdev *sd = fimc->pipeline.subdevs[IDX_SENSOR];
 	int ret;
 
-	ret = vb2_streamoff(&fimc->vb_queue, type);
-	if (ret == 0)
-		media_entity_pipeline_stop(&sd->entity);
-	return ret;
+	ret = vb2_ioctl_streamoff(file, priv, type);
+	if (ret < 0)
+		return ret;
+
+	media_entity_pipeline_stop(&fimc->vfd.entity);
+	fimc->streaming = false;
+	return 0;
 }
 
 static int fimc_lite_reqbufs(struct file *file, void *priv,
@@ -840,53 +860,13 @@ static int fimc_lite_reqbufs(struct file *file, void *priv,
 	int ret;
 
 	reqbufs->count = max_t(u32, FLITE_REQ_BUFS_MIN, reqbufs->count);
-	ret = vb2_reqbufs(&fimc->vb_queue, reqbufs);
+	ret = vb2_ioctl_reqbufs(file, priv, reqbufs);
 	if (!ret)
 		fimc->reqbufs_count = reqbufs->count;
 
 	return ret;
 }
 
-static int fimc_lite_querybuf(struct file *file, void *priv,
-			      struct v4l2_buffer *buf)
-{
-	struct fimc_lite *fimc = video_drvdata(file);
-
-	return vb2_querybuf(&fimc->vb_queue, buf);
-}
-
-static int fimc_lite_qbuf(struct file *file, void *priv,
-			  struct v4l2_buffer *buf)
-{
-	struct fimc_lite *fimc = video_drvdata(file);
-
-	return vb2_qbuf(&fimc->vb_queue, buf);
-}
-
-static int fimc_lite_dqbuf(struct file *file, void *priv,
-			   struct v4l2_buffer *buf)
-{
-	struct fimc_lite *fimc = video_drvdata(file);
-
-	return vb2_dqbuf(&fimc->vb_queue, buf, file->f_flags & O_NONBLOCK);
-}
-
-static int fimc_lite_create_bufs(struct file *file, void *priv,
-				 struct v4l2_create_buffers *create)
-{
-	struct fimc_lite *fimc = video_drvdata(file);
-
-	return vb2_create_bufs(&fimc->vb_queue, create);
-}
-
-static int fimc_lite_prepare_buf(struct file *file, void *priv,
-				 struct v4l2_buffer *b)
-{
-	struct fimc_lite *fimc = video_drvdata(file);
-
-	return vb2_prepare_buf(&fimc->vb_queue, b);
-}
-
 /* Return 1 if rectangle a is enclosed in rectangle b, or 0 otherwise. */
 static int enclosed_rectangle(struct v4l2_rect *a, struct v4l2_rect *b)
 {
@@ -966,38 +946,15 @@ static const struct v4l2_ioctl_ops fimc_lite_ioctl_ops = {
 	.vidioc_g_selection		= fimc_lite_g_selection,
 	.vidioc_s_selection		= fimc_lite_s_selection,
 	.vidioc_reqbufs			= fimc_lite_reqbufs,
-	.vidioc_querybuf		= fimc_lite_querybuf,
-	.vidioc_prepare_buf		= fimc_lite_prepare_buf,
-	.vidioc_create_bufs		= fimc_lite_create_bufs,
-	.vidioc_qbuf			= fimc_lite_qbuf,
-	.vidioc_dqbuf			= fimc_lite_dqbuf,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
 	.vidioc_streamon		= fimc_lite_streamon,
 	.vidioc_streamoff		= fimc_lite_streamoff,
 };
 
-/* Called with the media graph mutex held */
-static struct v4l2_subdev *__find_remote_sensor(struct media_entity *me)
-{
-	struct media_pad *pad = &me->pads[0];
-	struct v4l2_subdev *sd;
-
-	while (pad->flags & MEDIA_PAD_FL_SINK) {
-		/* source pad */
-		pad = media_entity_remote_source(pad);
-		if (pad == NULL ||
-		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
-			break;
-
-		sd = media_entity_to_v4l2_subdev(pad->entity);
-
-		if (sd->grp_id == GRP_ID_FIMC_IS_SENSOR)
-			return sd;
-		/* sink pad */
-		pad = &sd->entity.pads[0];
-	}
-	return NULL;
-}
-
 /* Capture subdev media entity operations */
 static int fimc_lite_link_setup(struct media_entity *entity,
 				const struct media_pad *local,
@@ -1072,7 +1029,7 @@ static int fimc_lite_subdev_enum_mbus_code(struct v4l2_subdev *sd,
 {
 	const struct fimc_fmt *fmt;
 
-	fmt = fimc_lite_find_format(NULL, NULL, code->index);
+	fmt = fimc_lite_find_format(NULL, NULL, 0, code->index);
 	if (!fmt)
 		return -EINVAL;
 	code->code = fmt->mbus_code;
@@ -1085,7 +1042,7 @@ static int fimc_lite_subdev_get_fmt(struct v4l2_subdev *sd,
 {
 	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *mf = &fmt->format;
-	struct flite_frame *f = &fimc->out_frame;
+	struct flite_frame *f = &fimc->inp_frame;
 
 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 		mf = v4l2_subdev_get_try_format(fh, fmt->pad);
@@ -1095,7 +1052,7 @@ static int fimc_lite_subdev_get_fmt(struct v4l2_subdev *sd,
 	mf->colorspace = V4L2_COLORSPACE_JPEG;
 
 	mutex_lock(&fimc->lock);
-	mf->code = fimc->fmt->mbus_code;
+	mf->code = f->fmt->mbus_code;
 
 	if (fmt->pad == FLITE_SD_PAD_SINK) {
 		/* full camera input frame size */
@@ -1147,7 +1104,7 @@ static int fimc_lite_subdev_set_fmt(struct v4l2_subdev *sd,
 	if (fmt->pad == FLITE_SD_PAD_SINK) {
 		sink->f_width = mf->width;
 		sink->f_height = mf->height;
-		fimc->fmt = ffmt;
+		sink->fmt = ffmt;
 		/* Set sink crop rectangle */
 		sink->rect.width = mf->width;
 		sink->rect.height = mf->height;
@@ -1159,7 +1116,7 @@ static int fimc_lite_subdev_set_fmt(struct v4l2_subdev *sd,
 		source->f_height = mf->height;
 	} else {
 		/* Allow changing format only on sink pad */
-		mf->code = fimc->fmt->mbus_code;
+		mf->code = sink->fmt->mbus_code;
 		mf->width = sink->rect.width;
 		mf->height = sink->rect.height;
 	}
@@ -1300,7 +1257,8 @@ static int fimc_lite_subdev_registered(struct v4l2_subdev *sd)
 
 	memset(vfd, 0, sizeof(*vfd));
 
-	fimc->fmt = &fimc_lite_formats[0];
+	fimc->inp_frame.fmt = &fimc_lite_formats[0];
+	fimc->out_frame.fmt = &fimc_lite_formats[0];
 	atomic_set(&fimc->out_path, FIMC_IO_DMA);
 
 	snprintf(vfd->name, sizeof(vfd->name), "fimc-lite.%d.capture",
@@ -1311,8 +1269,7 @@ static int fimc_lite_subdev_registered(struct v4l2_subdev *sd)
 	vfd->v4l2_dev = sd->v4l2_dev;
 	vfd->minor = -1;
 	vfd->release = video_device_release_empty;
-	vfd->lock = &fimc->lock;
-	fimc->ref_count = 0;
+	vfd->queue = q;
 	fimc->reqbufs_count = 0;
 
 	INIT_LIST_HEAD(&fimc->pending_buf_q);
@@ -1325,6 +1282,8 @@ static int fimc_lite_subdev_registered(struct v4l2_subdev *sd)
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct flite_buffer);
 	q->drv_priv = fimc;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &fimc->lock;
 
 	ret = vb2_queue_init(q);
 	if (ret < 0)
@@ -1418,7 +1377,7 @@ static int fimc_lite_create_capture_subdev(struct fimc_lite *fimc)
 	int ret;
 
 	v4l2_subdev_init(sd, &fimc_lite_subdev_ops);
-	sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	snprintf(sd->name, sizeof(sd->name), "FIMC-LITE.%d", fimc->index);
 
 	fimc->subdev_pads[FLITE_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
@@ -1440,6 +1399,7 @@ static int fimc_lite_create_capture_subdev(struct fimc_lite *fimc)
 	sd->ctrl_handler = handler;
 	sd->internal_ops = &fimc_lite_subdev_internal_ops;
 	sd->entity.ops = &fimc_lite_subdev_media_ops;
+	sd->owner = THIS_MODULE;
 	v4l2_set_subdevdata(sd, fimc);
 
 	return 0;
@@ -1481,19 +1441,35 @@ static int fimc_lite_clk_get(struct fimc_lite *fimc)
 	return ret;
 }
 
+static const struct of_device_id flite_of_match[];
+
 static int fimc_lite_probe(struct platform_device *pdev)
 {
-	struct flite_drvdata *drv_data = fimc_lite_get_drvdata(pdev);
+	struct flite_drvdata *drv_data = NULL;
+	struct device *dev = &pdev->dev;
+	const struct of_device_id *of_id;
 	struct fimc_lite *fimc;
 	struct resource *res;
 	int ret;
 
-	fimc = devm_kzalloc(&pdev->dev, sizeof(*fimc), GFP_KERNEL);
+	fimc = devm_kzalloc(dev, sizeof(*fimc), GFP_KERNEL);
 	if (!fimc)
 		return -ENOMEM;
 
-	fimc->index = pdev->id;
-	fimc->variant = drv_data->variant[fimc->index];
+	if (dev->of_node) {
+		of_id = of_match_node(flite_of_match, dev->of_node);
+		if (of_id)
+			drv_data = (struct flite_drvdata *)of_id->data;
+		fimc->index = of_alias_get_id(dev->of_node, "fimc-lite");
+	} else {
+		drv_data = fimc_lite_get_drvdata(pdev);
+		fimc->index = pdev->id;
+	}
+
+	if (!drv_data || fimc->index < 0 || fimc->index >= FIMC_LITE_MAX_DEVS)
+		return -EINVAL;
+
+	fimc->dd = drv_data;
 	fimc->pdev = pdev;
 
 	init_waitqueue_head(&fimc->irq_queue);
@@ -1501,13 +1477,13 @@ static int fimc_lite_probe(struct platform_device *pdev)
 	mutex_init(&fimc->lock);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	fimc->regs = devm_ioremap_resource(&pdev->dev, res);
+	fimc->regs = devm_ioremap_resource(dev, res);
 	if (IS_ERR(fimc->regs))
 		return PTR_ERR(fimc->regs);
 
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (res == NULL) {
-		dev_err(&pdev->dev, "Failed to get IRQ resource\n");
+		dev_err(dev, "Failed to get IRQ resource\n");
 		return -ENXIO;
 	}
 
@@ -1515,10 +1491,10 @@ static int fimc_lite_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	ret = devm_request_irq(&pdev->dev, res->start, flite_irq_handler,
-			       0, dev_name(&pdev->dev), fimc);
+	ret = devm_request_irq(dev, res->start, flite_irq_handler,
+			       0, dev_name(dev), fimc);
 	if (ret) {
-		dev_err(&pdev->dev, "Failed to install irq (%d)\n", ret);
+		dev_err(dev, "Failed to install irq (%d)\n", ret);
 		goto err_clk;
 	}
 
@@ -1528,23 +1504,23 @@ static int fimc_lite_probe(struct platform_device *pdev)
 		goto err_clk;
 
 	platform_set_drvdata(pdev, fimc);
-	pm_runtime_enable(&pdev->dev);
-	ret = pm_runtime_get_sync(&pdev->dev);
+	pm_runtime_enable(dev);
+	ret = pm_runtime_get_sync(dev);
 	if (ret < 0)
 		goto err_sd;
 
-	fimc->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	fimc->alloc_ctx = vb2_dma_contig_init_ctx(dev);
 	if (IS_ERR(fimc->alloc_ctx)) {
 		ret = PTR_ERR(fimc->alloc_ctx);
 		goto err_pm;
 	}
-	pm_runtime_put(&pdev->dev);
+	pm_runtime_put(dev);
 
-	dev_dbg(&pdev->dev, "FIMC-LITE.%d registered successfully\n",
+	dev_dbg(dev, "FIMC-LITE.%d registered successfully\n",
 		fimc->index);
 	return 0;
 err_pm:
-	pm_runtime_put(&pdev->dev);
+	pm_runtime_put(dev);
 err_sd:
 	fimc_lite_unregister_capture_subdev(fimc);
 err_clk:
@@ -1635,7 +1611,14 @@ static int fimc_lite_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct flite_variant fimc_lite0_variant_exynos4 = {
+static const struct dev_pm_ops fimc_lite_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(fimc_lite_suspend, fimc_lite_resume)
+	SET_RUNTIME_PM_OPS(fimc_lite_runtime_suspend, fimc_lite_runtime_resume,
+			   NULL)
+};
+
+/* EXYNOS4212, EXYNOS4412 */
+static struct flite_drvdata fimc_lite_drvdata_exynos4 = {
 	.max_width		= 8192,
 	.max_height		= 8192,
 	.out_width_align	= 8,
@@ -1643,14 +1626,6 @@ static struct flite_variant fimc_lite0_variant_exynos4 = {
 	.out_hor_offs_align	= 8,
 };
 
-/* EXYNOS4212, EXYNOS4412 */
-static struct flite_drvdata fimc_lite_drvdata_exynos4 = {
-	.variant = {
-		[0] = &fimc_lite0_variant_exynos4,
-		[1] = &fimc_lite0_variant_exynos4,
-	},
-};
-
 static struct platform_device_id fimc_lite_driver_ids[] = {
 	{
 		.name		= "exynos-fimc-lite",
@@ -1660,17 +1635,21 @@ static struct platform_device_id fimc_lite_driver_ids[] = {
 };
 MODULE_DEVICE_TABLE(platform, fimc_lite_driver_ids);
 
-static const struct dev_pm_ops fimc_lite_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(fimc_lite_suspend, fimc_lite_resume)
-	SET_RUNTIME_PM_OPS(fimc_lite_runtime_suspend, fimc_lite_runtime_resume,
-			   NULL)
+static const struct of_device_id flite_of_match[] = {
+	{
+		.compatible = "samsung,exynos4212-fimc-lite",
+		.data = &fimc_lite_drvdata_exynos4,
+	},
+	{ /* sentinel */ },
 };
+MODULE_DEVICE_TABLE(of, flite_of_match);
 
 static struct platform_driver fimc_lite_driver = {
 	.probe		= fimc_lite_probe,
 	.remove		= fimc_lite_remove,
 	.id_table	= fimc_lite_driver_ids,
 	.driver = {
+		.of_match_table = flite_of_match,
 		.name		= FIMC_LITE_DRV_NAME,
 		.owner		= THIS_MODULE,
 		.pm		= &fimc_lite_pm_ops,
diff --git a/drivers/media/platform/s5p-fimc/fimc-lite.h b/drivers/media/platform/exynos4-is/fimc-lite.h
index 7085761f8c4b..47da5e049247 100644
--- a/drivers/media/platform/s5p-fimc/fimc-lite.h
+++ b/drivers/media/platform/exynos4-is/fimc-lite.h
@@ -20,12 +20,11 @@
 
 #include <media/media-entity.h>
 #include <media/videobuf2-core.h>
+#include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-mediabus.h>
 #include <media/s5p_fimc.h>
 
-#include "fimc-core.h"
-
 #define FIMC_LITE_DRV_NAME	"exynos-fimc-lite"
 #define FLITE_CLK_NAME		"flite"
 #define FIMC_LITE_MAX_DEVS	2
@@ -49,7 +48,7 @@ enum {
 #define FLITE_SD_PAD_SOURCE_ISP	2
 #define FLITE_SD_PADS_NUM	3
 
-struct flite_variant {
+struct flite_drvdata {
 	unsigned short max_width;
 	unsigned short max_height;
 	unsigned short out_width_align;
@@ -57,10 +56,6 @@ struct flite_variant {
 	unsigned short out_hor_offs_align;
 };
 
-struct flite_drvdata {
-	struct flite_variant *variant[FIMC_LITE_MAX_DEVS];
-};
-
 #define fimc_lite_get_drvdata(_pdev) \
 	((struct flite_drvdata *) platform_get_device_id(_pdev)->driver_data)
 
@@ -75,11 +70,13 @@ struct fimc_lite_events {
  * @f_width: full pixel width
  * @f_height: full pixel height
  * @rect: crop/composition rectangle
+ * @fmt: pointer to pixel format description data structure
  */
 struct flite_frame {
 	u16 f_width;
 	u16 f_height;
 	struct v4l2_rect rect;
+	const struct fimc_fmt *fmt;
 };
 
 /**
@@ -97,7 +94,7 @@ struct flite_buffer {
 /**
  * struct fimc_lite - fimc lite structure
  * @pdev: pointer to FIMC-LITE platform device
- * @variant: variant information for this IP
+ * @dd: SoC specific driver data structure
  * @v4l2_dev: pointer to top the level v4l2_device
  * @vfd: video device node
  * @fh: v4l2 file handle
@@ -116,7 +113,6 @@ struct flite_buffer {
  * @clock: FIMC-LITE gate clock
  * @regs: memory mapped io registers
  * @irq_queue: interrupt handler waitqueue
- * @fmt: pointer to color format description structure
  * @payload: image size in bytes (w x h x bpp)
  * @inp_frame: camera input frame structure
  * @out_frame: DMA output frame structure
@@ -133,7 +129,7 @@ struct flite_buffer {
  */
 struct fimc_lite {
 	struct platform_device	*pdev;
-	struct flite_variant	*variant;
+	struct flite_drvdata	*dd;
 	struct v4l2_device	*v4l2_dev;
 	struct video_device	vfd;
 	struct v4l2_fh		fh;
@@ -144,7 +140,7 @@ struct fimc_lite {
 	struct v4l2_subdev	*sensor;
 	struct v4l2_ctrl_handler ctrl_handler;
 	struct v4l2_ctrl	*test_pattern;
-	u32			index;
+	int			index;
 	struct fimc_pipeline	pipeline;
 	const struct fimc_pipeline_ops *pipeline_ops;
 
@@ -155,7 +151,6 @@ struct fimc_lite {
 	void __iomem		*regs;
 	wait_queue_head_t	irq_queue;
 
-	const struct fimc_fmt	*fmt;
 	unsigned long		payload[FLITE_MAX_PLANES];
 	struct flite_frame	inp_frame;
 	struct flite_frame	out_frame;
@@ -171,6 +166,7 @@ struct fimc_lite {
 	int			ref_count;
 
 	struct fimc_lite_events	events;
+	bool			streaming;
 };
 
 static inline bool fimc_lite_active(struct fimc_lite *fimc)
diff --git a/drivers/media/platform/s5p-fimc/fimc-m2m.c b/drivers/media/platform/exynos4-is/fimc-m2m.c
index f3d535cdd87f..bde1f47f7ed3 100644
--- a/drivers/media/platform/s5p-fimc/fimc-m2m.c
+++ b/drivers/media/platform/exynos4-is/fimc-m2m.c
@@ -29,8 +29,7 @@
 
 #include "fimc-core.h"
 #include "fimc-reg.h"
-#include "fimc-mdevice.h"
-
+#include "media-dev.h"
 
 static unsigned int get_m2m_fmt_flags(unsigned int stream_type)
 {
@@ -100,7 +99,7 @@ static int stop_streaming(struct vb2_queue *q)
 
 static void fimc_device_run(void *priv)
 {
-	struct vb2_buffer *vb = NULL;
+	struct vb2_buffer *src_vb, *dst_vb;
 	struct fimc_ctx *ctx = priv;
 	struct fimc_frame *sf, *df;
 	struct fimc_dev *fimc;
@@ -123,16 +122,18 @@ static void fimc_device_run(void *priv)
 		fimc_prepare_dma_offset(ctx, df);
 	}
 
-	vb = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
-	ret = fimc_prepare_addr(ctx, vb, sf, &sf->paddr);
+	src_vb = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+	ret = fimc_prepare_addr(ctx, src_vb, sf, &sf->paddr);
 	if (ret)
 		goto dma_unlock;
 
-	vb = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
-	ret = fimc_prepare_addr(ctx, vb, df, &df->paddr);
+	dst_vb = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+	ret = fimc_prepare_addr(ctx, dst_vb, df, &df->paddr);
 	if (ret)
 		goto dma_unlock;
 
+	dst_vb->v4l2_buf.timestamp = src_vb->v4l2_buf.timestamp;
+
 	/* Reconfigure hardware if the context has changed. */
 	if (fimc->m2m.ctx != ctx) {
 		ctx->state |= FIMC_PARAMS;
@@ -152,7 +153,7 @@ static void fimc_device_run(void *priv)
 		fimc_hw_set_rotation(ctx);
 		fimc_hw_set_effect(ctx);
 		fimc_hw_set_out_dma(ctx);
-		if (fimc->variant->has_alpha)
+		if (fimc->drv_data->alpha_color)
 			fimc_hw_set_rgb_alpha(ctx);
 		fimc_hw_set_output_path(ctx);
 	}
@@ -250,22 +251,20 @@ static struct vb2_ops fimc_qops = {
  * V4L2 ioctl handlers
  */
 static int fimc_m2m_querycap(struct file *file, void *fh,
-			     struct v4l2_capability *cap)
+				     struct v4l2_capability *cap)
 {
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct fimc_dev *fimc = video_drvdata(file);
+	unsigned int caps;
 
-	strncpy(cap->driver, fimc->pdev->name, sizeof(cap->driver) - 1);
-	strncpy(cap->card, fimc->pdev->name, sizeof(cap->card) - 1);
-	cap->bus_info[0] = 0;
 	/*
 	 * This is only a mem-to-mem video device. The capture and output
 	 * device capability flags are left only for backward compatibility
 	 * and are scheduled for removal.
 	 */
-	cap->capabilities = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE |
+	caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE |
 		V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
 
+	__fimc_vidioc_querycap(&fimc->pdev->dev, cap, caps);
 	return 0;
 }
 
@@ -623,6 +622,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->ops = &fimc_qops;
 	src_vq->mem_ops = &vb2_dma_contig_memops;
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -634,6 +634,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->ops = &fimc_qops;
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	return vb2_queue_init(dst_vq);
 }
@@ -667,16 +668,15 @@ static int fimc_m2m_open(struct file *file)
 	struct fimc_ctx *ctx;
 	int ret = -EBUSY;
 
-	dbg("pid: %d, state: 0x%lx, refcnt: %d",
-	    task_pid_nr(current), fimc->state, fimc->vid_cap.refcnt);
+	pr_debug("pid: %d, state: %#lx\n", task_pid_nr(current), fimc->state);
 
 	if (mutex_lock_interruptible(&fimc->lock))
 		return -ERESTARTSYS;
 	/*
-	 * Return if the corresponding video capture node
-	 * is already opened.
+	 * Don't allow simultaneous open() of the mem-to-mem and the
+	 * capture video node that belong to same FIMC IP instance.
 	 */
-	if (fimc->vid_cap.refcnt > 0)
+	if (test_bit(ST_CAPT_BUSY, &fimc->state))
 		goto unlock;
 
 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
diff --git a/drivers/media/platform/s5p-fimc/fimc-reg.c b/drivers/media/platform/exynos4-is/fimc-reg.c
index 50b97c75b956..f079f36099de 100644
--- a/drivers/media/platform/s5p-fimc/fimc-reg.c
+++ b/drivers/media/platform/exynos4-is/fimc-reg.c
@@ -1,22 +1,24 @@
 /*
  * Register interface file for Samsung Camera Interface (FIMC) driver
  *
- * Copyright (C) 2010 - 2012 Samsung Electronics Co., Ltd.
- * Sylwester Nawrocki, <s.nawrocki@samsung.com>
+ * Copyright (C) 2010 - 2013 Samsung Electronics Co., Ltd.
+ * 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
  * published by the Free Software Foundation.
 */
 
-#include <linux/io.h>
 #include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/regmap.h>
+
 #include <media/s5p_fimc.h>
+#include "media-dev.h"
 
 #include "fimc-reg.h"
 #include "fimc-core.h"
 
-
 void fimc_hw_reset(struct fimc_dev *dev)
 {
 	u32 cfg;
@@ -35,7 +37,7 @@ void fimc_hw_reset(struct fimc_dev *dev)
 	cfg &= ~FIMC_REG_CIGCTRL_SWRST;
 	writel(cfg, dev->regs + FIMC_REG_CIGCTRL);
 
-	if (dev->variant->out_buf_count > 4)
+	if (dev->drv_data->out_buf_count > 4)
 		fimc_hw_set_dma_seq(dev, 0xF);
 }
 
@@ -447,7 +449,8 @@ void fimc_hw_set_in_dma(struct fimc_ctx *ctx)
 		 | FIMC_REG_MSCTRL_IN_BURST_COUNT_MASK
 		 | FIMC_REG_MSCTRL_INPUT_MASK
 		 | FIMC_REG_MSCTRL_C_INT_IN_MASK
-		 | FIMC_REG_MSCTRL_2P_IN_ORDER_MASK);
+		 | FIMC_REG_MSCTRL_2P_IN_ORDER_MASK
+		 | FIMC_REG_MSCTRL_ORDER422_MASK);
 
 	cfg |= (FIMC_REG_MSCTRL_IN_BURST_COUNT(4)
 		| FIMC_REG_MSCTRL_INPUT_MEMORY
@@ -598,7 +601,8 @@ static const struct mbus_pixfmt_desc pix_desc[] = {
 int fimc_hw_set_camera_source(struct fimc_dev *fimc,
 			      struct fimc_source_info *source)
 {
-	struct fimc_frame *f = &fimc->vid_cap.ctx->s_frame;
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct fimc_frame *f = &vc->ctx->s_frame;
 	u32 bus_width, cfg = 0;
 	int i;
 
@@ -606,7 +610,7 @@ int fimc_hw_set_camera_source(struct fimc_dev *fimc,
 	case FIMC_BUS_TYPE_ITU_601:
 	case FIMC_BUS_TYPE_ITU_656:
 		for (i = 0; i < ARRAY_SIZE(pix_desc); i++) {
-			if (fimc->vid_cap.mf.code == pix_desc[i].pixelcode) {
+			if (vc->ci_fmt.code == pix_desc[i].pixelcode) {
 				cfg = pix_desc[i].cisrcfmt;
 				bus_width = pix_desc[i].bus_width;
 				break;
@@ -614,9 +618,9 @@ int fimc_hw_set_camera_source(struct fimc_dev *fimc,
 		}
 
 		if (i == ARRAY_SIZE(pix_desc)) {
-			v4l2_err(&fimc->vid_cap.vfd,
+			v4l2_err(&vc->vfd,
 				 "Camera color format not supported: %d\n",
-				 fimc->vid_cap.mf.code);
+				 vc->ci_fmt.code);
 			return -EINVAL;
 		}
 
@@ -631,6 +635,10 @@ int fimc_hw_set_camera_source(struct fimc_dev *fimc,
 		if (fimc_fmt_is_user_defined(f->fmt->color))
 			cfg |= FIMC_REG_CISRCFMT_ITU601_8BIT;
 		break;
+	default:
+	case FIMC_BUS_TYPE_ISP_WRITEBACK:
+		/* Anything to do here ? */
+		break;
 	}
 
 	cfg |= (f->o_width << 16) | f->o_height;
@@ -660,16 +668,17 @@ void fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f)
 int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 			    struct fimc_source_info *source)
 {
-	u32 cfg, tmp;
 	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
 	u32 csis_data_alignment = 32;
+	u32 cfg, tmp;
 
 	cfg = readl(fimc->regs + FIMC_REG_CIGCTRL);
 
 	/* Select ITU B interface, disable Writeback path and test pattern. */
 	cfg &= ~(FIMC_REG_CIGCTRL_TESTPAT_MASK | FIMC_REG_CIGCTRL_SELCAM_ITU_A |
 		FIMC_REG_CIGCTRL_SELCAM_MIPI | FIMC_REG_CIGCTRL_CAMIF_SELWB |
-		FIMC_REG_CIGCTRL_SELCAM_MIPI_A | FIMC_REG_CIGCTRL_CAM_JPEG);
+		FIMC_REG_CIGCTRL_SELCAM_MIPI_A | FIMC_REG_CIGCTRL_CAM_JPEG |
+		FIMC_REG_CIGCTRL_SELWB_A);
 
 	switch (source->fimc_bus_type) {
 	case FIMC_BUS_TYPE_MIPI_CSI2:
@@ -679,7 +688,7 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 			cfg |= FIMC_REG_CIGCTRL_SELCAM_MIPI_A;
 
 		/* TODO: add remaining supported formats. */
-		switch (vid_cap->mf.code) {
+		switch (vid_cap->ci_fmt.code) {
 		case V4L2_MBUS_FMT_VYUY8_2X8:
 			tmp = FIMC_REG_CSIIMGFMT_YCBCR422_8BIT;
 			break;
@@ -691,7 +700,7 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 		default:
 			v4l2_err(&vid_cap->vfd,
 				 "Not supported camera pixel format: %#x\n",
-				 vid_cap->mf.code);
+				 vid_cap->ci_fmt.code);
 			return -EINVAL;
 		}
 		tmp |= (csis_data_alignment == 32) << 8;
@@ -704,6 +713,12 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 		break;
 	case FIMC_BUS_TYPE_LCD_WRITEBACK_A:
 		cfg |= FIMC_REG_CIGCTRL_CAMIF_SELWB;
+		/* fall through */
+	case FIMC_BUS_TYPE_ISP_WRITEBACK:
+		if (fimc->variant->has_isp_wb)
+			cfg |= FIMC_REG_CIGCTRL_CAMIF_SELWB;
+		else
+			WARN_ONCE(1, "ISP Writeback input is not supported\n");
 		break;
 	default:
 		v4l2_err(&vid_cap->vfd, "Invalid FIMC bus type selected: %d\n",
@@ -747,7 +762,7 @@ s32 fimc_hw_get_frame_index(struct fimc_dev *dev)
 {
 	s32 reg;
 
-	if (dev->variant->has_cistatus2) {
+	if (dev->drv_data->cistatus2) {
 		reg = readl(dev->regs + FIMC_REG_CISTATUS2) & 0x3f;
 		return reg - 1;
 	}
@@ -763,7 +778,7 @@ s32 fimc_hw_get_prev_frame_index(struct fimc_dev *dev)
 {
 	s32 reg;
 
-	if (!dev->variant->has_cistatus2)
+	if (!dev->drv_data->cistatus2)
 		return -1;
 
 	reg = readl(dev->regs + FIMC_REG_CISTATUS2);
@@ -784,3 +799,43 @@ void fimc_deactivate_capture(struct fimc_dev *fimc)
 	fimc_hw_enable_scaler(fimc, false);
 	fimc_hw_en_lastirq(fimc, false);
 }
+
+int fimc_hw_camblk_cfg_writeback(struct fimc_dev *fimc)
+{
+	struct regmap *map = fimc->sysreg;
+	unsigned int mask, val, camblk_cfg;
+	int ret;
+
+	if (map == NULL)
+		return 0;
+
+	ret = regmap_read(map, SYSREG_CAMBLK, &camblk_cfg);
+	if (ret < 0 || ((camblk_cfg & 0x00700000) >> 20 != 0x3))
+		return ret;
+
+	if (!WARN(fimc->id >= 3, "not supported id: %d\n", fimc->id))
+		val = 0x1 << (fimc->id + 20);
+	else
+		val = 0;
+
+	mask = SYSREG_CAMBLK_FIFORST_ISP | SYSREG_CAMBLK_ISPWB_FULL_EN;
+	ret = regmap_update_bits(map, SYSREG_CAMBLK, mask, val);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(1000, 2000);
+
+	val |= SYSREG_CAMBLK_FIFORST_ISP;
+	ret = regmap_update_bits(map, SYSREG_CAMBLK, mask, val);
+	if (ret < 0)
+		return ret;
+
+	mask = SYSREG_ISPBLK_FIFORST_CAM_BLK;
+	ret = regmap_update_bits(map, SYSREG_ISPBLK, mask, ~mask);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(1000, 2000);
+
+	return regmap_update_bits(map, SYSREG_ISPBLK, mask, mask);
+}
diff --git a/drivers/media/platform/s5p-fimc/fimc-reg.h b/drivers/media/platform/exynos4-is/fimc-reg.h
index 1a40df6d1a80..6c97798c75a5 100644
--- a/drivers/media/platform/s5p-fimc/fimc-reg.h
+++ b/drivers/media/platform/exynos4-is/fimc-reg.h
@@ -52,6 +52,8 @@
 #define FIMC_REG_CIGCTRL_IRQ_CLR		(1 << 19)
 #define FIMC_REG_CIGCTRL_IRQ_ENABLE		(1 << 16)
 #define FIMC_REG_CIGCTRL_SHDW_DISABLE		(1 << 12)
+/* 0 - selects Writeback A (LCD), 1 - selects Writeback B (LCD/ISP) */
+#define FIMC_REG_CIGCTRL_SELWB_A		(1 << 10)
 #define FIMC_REG_CIGCTRL_CAM_JPEG		(1 << 8)
 #define FIMC_REG_CIGCTRL_SELCAM_MIPI_A		(1 << 7)
 #define FIMC_REG_CIGCTRL_CAMIF_SELWB		(1 << 6)
@@ -93,10 +95,10 @@
 /* Output DMA control */
 #define FIMC_REG_CIOCTRL			0x4c
 #define FIMC_REG_CIOCTRL_ORDER422_MASK		(3 << 0)
-#define FIMC_REG_CIOCTRL_ORDER422_CRYCBY	(0 << 0)
-#define FIMC_REG_CIOCTRL_ORDER422_CBYCRY	(1 << 0)
-#define FIMC_REG_CIOCTRL_ORDER422_YCRYCB	(2 << 0)
-#define FIMC_REG_CIOCTRL_ORDER422_YCBYCR	(3 << 0)
+#define FIMC_REG_CIOCTRL_ORDER422_YCBYCR	(0 << 0)
+#define FIMC_REG_CIOCTRL_ORDER422_YCRYCB	(1 << 0)
+#define FIMC_REG_CIOCTRL_ORDER422_CBYCRY	(2 << 0)
+#define FIMC_REG_CIOCTRL_ORDER422_CRYCBY	(3 << 0)
 #define FIMC_REG_CIOCTRL_LASTIRQ_ENABLE		(1 << 2)
 #define FIMC_REG_CIOCTRL_YCBCR_3PLANE		(0 << 3)
 #define FIMC_REG_CIOCTRL_YCBCR_2PLANE		(1 << 3)
@@ -218,10 +220,10 @@
 #define FIMC_REG_MSCTRL_FLIP_180		(3 << 13)
 #define FIMC_REG_MSCTRL_FIFO_CTRL_FULL		(1 << 12)
 #define FIMC_REG_MSCTRL_ORDER422_SHIFT		4
-#define FIMC_REG_MSCTRL_ORDER422_YCBYCR		(0 << 4)
-#define FIMC_REG_MSCTRL_ORDER422_CBYCRY		(1 << 4)
-#define FIMC_REG_MSCTRL_ORDER422_YCRYCB		(2 << 4)
-#define FIMC_REG_MSCTRL_ORDER422_CRYCBY		(3 << 4)
+#define FIMC_REG_MSCTRL_ORDER422_CRYCBY		(0 << 4)
+#define FIMC_REG_MSCTRL_ORDER422_YCRYCB		(1 << 4)
+#define FIMC_REG_MSCTRL_ORDER422_CBYCRY		(2 << 4)
+#define FIMC_REG_MSCTRL_ORDER422_YCBYCR		(3 << 4)
 #define FIMC_REG_MSCTRL_ORDER422_MASK		(3 << 4)
 #define FIMC_REG_MSCTRL_INPUT_EXTCAM		(0 << 3)
 #define FIMC_REG_MSCTRL_INPUT_MEMORY		(1 << 3)
@@ -276,6 +278,14 @@
 /* Output frame buffer sequence mask */
 #define FIMC_REG_CIFCNTSEQ			0x1fc
 
+/* SYSREG ISP Writeback register address offsets */
+#define SYSREG_ISPBLK				0x020c
+#define SYSREG_ISPBLK_FIFORST_CAM_BLK		(1 << 7)
+
+#define SYSREG_CAMBLK				0x0218
+#define SYSREG_CAMBLK_FIFORST_ISP		(1 << 15)
+#define SYSREG_CAMBLK_ISPWB_FULL_EN		(7 << 20)
+
 /*
  * Function declarations
  */
@@ -309,6 +319,7 @@ void fimc_hw_activate_input_dma(struct fimc_dev *dev, bool on);
 void fimc_hw_disable_capture(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);
+int fimc_hw_camblk_cfg_writeback(struct fimc_dev *fimc);
 void fimc_activate_capture(struct fimc_ctx *ctx);
 void fimc_deactivate_capture(struct fimc_dev *fimc);
 
diff --git a/drivers/media/platform/s5p-fimc/fimc-mdevice.c b/drivers/media/platform/exynos4-is/media-dev.c
index cd38d708ab58..15ef8f28239b 100644
--- a/drivers/media/platform/s5p-fimc/fimc-mdevice.c
+++ b/drivers/media/platform/exynos4-is/media-dev.c
@@ -17,32 +17,37 @@
 #include <linux/kernel.h>
 #include <linux/list.h>
 #include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/of_device.h>
+#include <linux/of_i2c.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/types.h>
 #include <linux/slab.h>
 #include <media/v4l2-ctrls.h>
+#include <media/v4l2-of.h>
 #include <media/media-device.h>
 #include <media/s5p_fimc.h>
 
+#include "media-dev.h"
 #include "fimc-core.h"
+#include "fimc-is.h"
 #include "fimc-lite.h"
-#include "fimc-mdevice.h"
 #include "mipi-csis.h"
 
 static int __fimc_md_set_camclk(struct fimc_md *fmd,
-				struct fimc_sensor_info *s_info,
+				struct fimc_source_info *si,
 				bool on);
 /**
  * fimc_pipeline_prepare - update pipeline information with subdevice pointers
- * @fimc: fimc device terminating the pipeline
+ * @me: media entity terminating the pipeline
  *
  * Caller holds the graph mutex.
  */
 static void fimc_pipeline_prepare(struct fimc_pipeline *p,
 				  struct media_entity *me)
 {
-	struct media_pad *pad = &me->pads[0];
 	struct v4l2_subdev *sd;
 	int i;
 
@@ -50,15 +55,21 @@ static void fimc_pipeline_prepare(struct fimc_pipeline *p,
 		p->subdevs[i] = NULL;
 
 	while (1) {
-		if (!(pad->flags & MEDIA_PAD_FL_SINK))
-			break;
+		struct media_pad *pad = NULL;
+
+		/* Find remote source pad */
+		for (i = 0; i < me->num_pads; i++) {
+			struct media_pad *spad = &me->pads[i];
+			if (!(spad->flags & MEDIA_PAD_FL_SINK))
+				continue;
+			pad = media_entity_remote_source(spad);
+			if (pad)
+				break;
+		}
 
-		/* source pad */
-		pad = media_entity_remote_source(pad);
 		if (pad == NULL ||
 		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
 			break;
-
 		sd = media_entity_to_v4l2_subdev(pad->entity);
 
 		switch (sd->grp_id) {
@@ -75,12 +86,15 @@ static void fimc_pipeline_prepare(struct fimc_pipeline *p,
 		case GRP_ID_FIMC:
 			/* No need to control FIMC subdev through subdev ops */
 			break;
+		case GRP_ID_FIMC_IS:
+			p->subdevs[IDX_IS_ISP] = sd;
+			break;
 		default:
-			pr_warn("%s: Unknown subdev grp_id: %#x\n",
-				__func__, sd->grp_id);
+			break;
 		}
-		/* sink pad */
-		pad = &sd->entity.pads[0];
+		me = &sd->entity;
+		if (me->num_pads == 1)
+			break;
 	}
 }
 
@@ -117,49 +131,81 @@ static int __subdev_set_power(struct v4l2_subdev *sd, int on)
  *
  * Needs to be called with the graph mutex held.
  */
-static int fimc_pipeline_s_power(struct fimc_pipeline *p, bool state)
+static int fimc_pipeline_s_power(struct fimc_pipeline *p, bool on)
 {
-	unsigned int i;
-	int ret;
+	static const u8 seq[2][IDX_MAX - 1] = {
+		{ IDX_IS_ISP, IDX_SENSOR, IDX_CSIS, IDX_FLITE },
+		{ IDX_CSIS, IDX_FLITE, IDX_SENSOR, IDX_IS_ISP },
+	};
+	int i, ret = 0;
 
 	if (p->subdevs[IDX_SENSOR] == NULL)
 		return -ENXIO;
 
-	for (i = 0; i < IDX_MAX; i++) {
-		unsigned int idx = state ? (IDX_MAX - 1) - i : i;
+	for (i = 0; i < IDX_MAX - 1; i++) {
+		unsigned int idx = seq[on][i];
+
+		ret = __subdev_set_power(p->subdevs[idx], on);
+
 
-		ret = __subdev_set_power(p->subdevs[idx], state);
 		if (ret < 0 && ret != -ENXIO)
-			return ret;
+			goto error;
 	}
-
 	return 0;
+error:
+	for (; i >= 0; i--) {
+		unsigned int idx = seq[on][i];
+		__subdev_set_power(p->subdevs[idx], !on);
+	}
+	return ret;
 }
 
 /**
  * __fimc_pipeline_open - update the pipeline information, enable power
  *                        of all pipeline subdevs and the sensor clock
  * @me: media entity to start graph walk with
- * @prep: true to acquire sensor (and csis) subdevs
+ * @prepare: true to walk the current pipeline and acquire all subdevs
  *
  * Called with the graph mutex held.
  */
 static int __fimc_pipeline_open(struct fimc_pipeline *p,
-				struct media_entity *me, bool prep)
+				struct media_entity *me, bool prepare)
 {
+	struct fimc_md *fmd = entity_to_fimc_mdev(me);
+	struct v4l2_subdev *sd;
 	int ret;
 
-	if (prep)
+	if (WARN_ON(p == NULL || me == NULL))
+		return -EINVAL;
+
+	if (prepare)
 		fimc_pipeline_prepare(p, me);
 
-	if (p->subdevs[IDX_SENSOR] == NULL)
+	sd = p->subdevs[IDX_SENSOR];
+	if (sd == NULL)
 		return -EINVAL;
 
-	ret = fimc_md_set_camclk(p->subdevs[IDX_SENSOR], true);
-	if (ret)
-		return ret;
+	/* Disable PXLASYNC clock if this pipeline includes FIMC-IS */
+	if (!IS_ERR(fmd->wbclk[CLK_IDX_WB_B]) && p->subdevs[IDX_IS_ISP]) {
+		ret = clk_prepare_enable(fmd->wbclk[CLK_IDX_WB_B]);
+		if (ret < 0)
+			return ret;
+	}
+	ret = fimc_md_set_camclk(sd, true);
+	if (ret < 0)
+		goto err_wbclk;
+
+	ret = fimc_pipeline_s_power(p, 1);
+	if (!ret)
+		return 0;
+
+	fimc_md_set_camclk(sd, false);
+
+err_wbclk:
+	if (!IS_ERR(fmd->wbclk[CLK_IDX_WB_B]) && p->subdevs[IDX_IS_ISP])
+		clk_disable_unprepare(fmd->wbclk[CLK_IDX_WB_B]);
 
-	return fimc_pipeline_s_power(p, 1);
+	return ret;
 }
 
 /**
@@ -170,41 +216,58 @@ static int __fimc_pipeline_open(struct fimc_pipeline *p,
  */
 static int __fimc_pipeline_close(struct fimc_pipeline *p)
 {
+	struct v4l2_subdev *sd = p ? p->subdevs[IDX_SENSOR] : NULL;
+	struct fimc_md *fmd;
 	int ret = 0;
 
-	if (!p || !p->subdevs[IDX_SENSOR])
+	if (WARN_ON(sd == NULL))
 		return -EINVAL;
 
 	if (p->subdevs[IDX_SENSOR]) {
 		ret = fimc_pipeline_s_power(p, 0);
-		fimc_md_set_camclk(p->subdevs[IDX_SENSOR], false);
+		fimc_md_set_camclk(sd, false);
 	}
+
+	fmd = entity_to_fimc_mdev(&sd->entity);
+
+	/* Disable PXLASYNC clock if this pipeline includes FIMC-IS */
+	if (!IS_ERR(fmd->wbclk[CLK_IDX_WB_B]) && p->subdevs[IDX_IS_ISP])
+		clk_disable_unprepare(fmd->wbclk[CLK_IDX_WB_B]);
+
 	return ret == -ENXIO ? 0 : ret;
 }
 
 /**
- * __fimc_pipeline_s_stream - invoke s_stream on pipeline subdevs
+ * __fimc_pipeline_s_stream - call s_stream() on pipeline subdevs
  * @pipeline: video pipeline structure
- * @on: passed as the s_stream call argument
+ * @on: passed as the s_stream() callback argument
  */
 static int __fimc_pipeline_s_stream(struct fimc_pipeline *p, bool on)
 {
-	int i, ret;
+	static const u8 seq[2][IDX_MAX] = {
+		{ IDX_FIMC, IDX_SENSOR, IDX_IS_ISP, IDX_CSIS, IDX_FLITE },
+		{ IDX_CSIS, IDX_FLITE, IDX_FIMC, IDX_SENSOR, IDX_IS_ISP },
+	};
+	int i, ret = 0;
 
 	if (p->subdevs[IDX_SENSOR] == NULL)
 		return -ENODEV;
 
 	for (i = 0; i < IDX_MAX; i++) {
-		unsigned int idx = on ? (IDX_MAX - 1) - i : i;
+		unsigned int idx = seq[on][i];
 
 		ret = v4l2_subdev_call(p->subdevs[idx], video, s_stream, on);
 
 		if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV)
-			return ret;
+			goto error;
 	}
-
 	return 0;
-
+error:
+	for (; i >= 0; i--) {
+		unsigned int idx = seq[on][i];
+		v4l2_subdev_call(p->subdevs[idx], video, s_stream, !on);
+	}
+	return ret;
 }
 
 /* Media pipeline operations for the FIMC/FIMC-LITE video device driver */
@@ -218,35 +281,40 @@ static const struct fimc_pipeline_ops fimc_pipeline_ops = {
  * Sensor subdevice helper functions
  */
 static struct v4l2_subdev *fimc_md_register_sensor(struct fimc_md *fmd,
-				   struct fimc_sensor_info *s_info)
+						struct fimc_source_info *si)
 {
 	struct i2c_adapter *adapter;
 	struct v4l2_subdev *sd = NULL;
 
-	if (!s_info || !fmd)
+	if (!si || !fmd)
 		return NULL;
+	/*
+	 * If FIMC bus type is not Writeback FIFO assume it is same
+	 * as sensor_bus_type.
+	 */
+	si->fimc_bus_type = si->sensor_bus_type;
 
-	adapter = i2c_get_adapter(s_info->pdata.i2c_bus_num);
+	adapter = i2c_get_adapter(si->i2c_bus_num);
 	if (!adapter) {
 		v4l2_warn(&fmd->v4l2_dev,
 			  "Failed to get I2C adapter %d, deferring probe\n",
-			  s_info->pdata.i2c_bus_num);
+			  si->i2c_bus_num);
 		return ERR_PTR(-EPROBE_DEFER);
 	}
 	sd = v4l2_i2c_new_subdev_board(&fmd->v4l2_dev, adapter,
-				       s_info->pdata.board_info, NULL);
+						si->board_info, NULL);
 	if (IS_ERR_OR_NULL(sd)) {
 		i2c_put_adapter(adapter);
 		v4l2_warn(&fmd->v4l2_dev,
 			  "Failed to acquire subdev %s, deferring probe\n",
-			  s_info->pdata.board_info->type);
+			  si->board_info->type);
 		return ERR_PTR(-EPROBE_DEFER);
 	}
-	v4l2_set_subdev_hostdata(sd, s_info);
+	v4l2_set_subdev_hostdata(sd, si);
 	sd->grp_id = GRP_ID_SENSOR;
 
 	v4l2_info(&fmd->v4l2_dev, "Registered sensor subdevice %s\n",
-		  s_info->pdata.board_info->type);
+		  sd->name);
 	return sd;
 }
 
@@ -257,57 +325,262 @@ static void fimc_md_unregister_sensor(struct v4l2_subdev *sd)
 
 	if (!client)
 		return;
+
 	v4l2_device_unregister_subdev(sd);
-	adapter = client->adapter;
-	i2c_unregister_device(client);
-	if (adapter)
-		i2c_put_adapter(adapter);
+
+	if (!client->dev.of_node) {
+		adapter = client->adapter;
+		i2c_unregister_device(client);
+		if (adapter)
+			i2c_put_adapter(adapter);
+	}
+}
+
+#ifdef CONFIG_OF
+/* Register I2C client subdev associated with @node. */
+static int fimc_md_of_add_sensor(struct fimc_md *fmd,
+				 struct device_node *node, int index)
+{
+	struct fimc_sensor_info *si;
+	struct i2c_client *client;
+	struct v4l2_subdev *sd;
+	int ret;
+
+	if (WARN_ON(index >= ARRAY_SIZE(fmd->sensor)))
+		return -EINVAL;
+	si = &fmd->sensor[index];
+
+	client = of_find_i2c_device_by_node(node);
+	if (!client)
+		return -EPROBE_DEFER;
+
+	device_lock(&client->dev);
+
+	if (!client->driver ||
+	    !try_module_get(client->driver->driver.owner)) {
+		ret = -EPROBE_DEFER;
+		v4l2_info(&fmd->v4l2_dev, "No driver found for %s\n",
+						node->full_name);
+		goto dev_put;
+	}
+
+	/* Enable sensor's master clock */
+	ret = __fimc_md_set_camclk(fmd, &si->pdata, true);
+	if (ret < 0)
+		goto mod_put;
+	sd = i2c_get_clientdata(client);
+
+	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
+	__fimc_md_set_camclk(fmd, &si->pdata, false);
+	if (ret < 0)
+		goto mod_put;
+
+	v4l2_set_subdev_hostdata(sd, &si->pdata);
+	if (si->pdata.fimc_bus_type == FIMC_BUS_TYPE_ISP_WRITEBACK)
+		sd->grp_id = GRP_ID_FIMC_IS_SENSOR;
+	else
+		sd->grp_id = GRP_ID_SENSOR;
+
+	si->subdev = sd;
+	v4l2_info(&fmd->v4l2_dev, "Registered sensor subdevice: %s (%d)\n",
+		  sd->name, fmd->num_sensors);
+	fmd->num_sensors++;
+
+mod_put:
+	module_put(client->driver->driver.owner);
+dev_put:
+	device_unlock(&client->dev);
+	put_device(&client->dev);
+	return ret;
+}
+
+/* Parse port node and register as a sub-device any sensor specified there. */
+static int fimc_md_parse_port_node(struct fimc_md *fmd,
+				   struct device_node *port,
+				   unsigned int index)
+{
+	struct device_node *rem, *ep, *np;
+	struct fimc_source_info *pd;
+	struct v4l2_of_endpoint endpoint;
+	int ret;
+	u32 val;
+
+	pd = &fmd->sensor[index].pdata;
+
+	/* Assume here a port node can have only one endpoint node. */
+	ep = of_get_next_child(port, NULL);
+	if (!ep)
+		return 0;
+
+	v4l2_of_parse_endpoint(ep, &endpoint);
+	if (WARN_ON(endpoint.port == 0) || index >= FIMC_MAX_SENSORS)
+		return -EINVAL;
+
+	pd->mux_id = (endpoint.port - 1) & 0x1;
+
+	rem = v4l2_of_get_remote_port_parent(ep);
+	of_node_put(ep);
+	if (rem == NULL) {
+		v4l2_info(&fmd->v4l2_dev, "Remote device at %s not found\n",
+							ep->full_name);
+		return 0;
+	}
+	if (!of_property_read_u32(rem, "samsung,camclk-out", &val))
+		pd->clk_id = val;
+
+	if (!of_property_read_u32(rem, "clock-frequency", &val))
+		pd->clk_frequency = val;
+
+	if (pd->clk_frequency == 0) {
+		v4l2_err(&fmd->v4l2_dev, "Wrong clock frequency at node %s\n",
+			 rem->full_name);
+		of_node_put(rem);
+		return -EINVAL;
+	}
+
+	if (fimc_input_is_parallel(endpoint.port)) {
+		if (endpoint.bus_type == V4L2_MBUS_PARALLEL)
+			pd->sensor_bus_type = FIMC_BUS_TYPE_ITU_601;
+		else
+			pd->sensor_bus_type = FIMC_BUS_TYPE_ITU_656;
+		pd->flags = endpoint.bus.parallel.flags;
+	} else if (fimc_input_is_mipi_csi(endpoint.port)) {
+		/*
+		 * MIPI CSI-2: only input mux selection and
+		 * the sensor's clock frequency is needed.
+		 */
+		pd->sensor_bus_type = FIMC_BUS_TYPE_MIPI_CSI2;
+	} else {
+		v4l2_err(&fmd->v4l2_dev, "Wrong port id (%u) at node %s\n",
+			 endpoint.port, rem->full_name);
+	}
+	/*
+	 * For FIMC-IS handled sensors, that are placed under i2c-isp device
+	 * node, FIMC is connected to the FIMC-IS through its ISP Writeback
+	 * input. Sensors are attached to the FIMC-LITE hostdata interface
+	 * directly or through MIPI-CSIS, depending on the external media bus
+	 * used. This needs to be handled in a more reliable way, not by just
+	 * checking parent's node name.
+	 */
+	np = of_get_parent(rem);
+
+	if (np && !of_node_cmp(np->name, "i2c-isp"))
+		pd->fimc_bus_type = FIMC_BUS_TYPE_ISP_WRITEBACK;
+	else
+		pd->fimc_bus_type = pd->sensor_bus_type;
+
+	ret = fimc_md_of_add_sensor(fmd, rem, index);
+	of_node_put(rem);
+
+	return ret;
+}
+
+/* Register all SoC external sub-devices */
+static int fimc_md_of_sensors_register(struct fimc_md *fmd,
+				       struct device_node *np)
+{
+	struct device_node *parent = fmd->pdev->dev.of_node;
+	struct device_node *node, *ports;
+	int index = 0;
+	int ret;
+
+	/* Attach sensors linked to MIPI CSI-2 receivers */
+	for_each_available_child_of_node(parent, node) {
+		struct device_node *port;
+
+		if (of_node_cmp(node->name, "csis"))
+			continue;
+		/* The csis node can have only port subnode. */
+		port = of_get_next_child(node, NULL);
+		if (!port)
+			continue;
+
+		ret = fimc_md_parse_port_node(fmd, port, index);
+		if (ret < 0)
+			return ret;
+		index++;
+	}
+
+	/* Attach sensors listed in the parallel-ports node */
+	ports = of_get_child_by_name(parent, "parallel-ports");
+	if (!ports)
+		return 0;
+
+	for_each_child_of_node(ports, node) {
+		ret = fimc_md_parse_port_node(fmd, node, index);
+		if (ret < 0)
+			break;
+		index++;
+	}
+
+	return 0;
+}
+
+static int __of_get_csis_id(struct device_node *np)
+{
+	u32 reg = 0;
+
+	np = of_get_child_by_name(np, "port");
+	if (!np)
+		return -EINVAL;
+	of_property_read_u32(np, "reg", &reg);
+	return reg - FIMC_INPUT_MIPI_CSI2_0;
 }
+#else
+#define fimc_md_of_sensors_register(fmd, np) (-ENOSYS)
+#define __of_get_csis_id(np) (-ENOSYS)
+#endif
 
 static int fimc_md_register_sensor_entities(struct fimc_md *fmd)
 {
 	struct s5p_platform_fimc *pdata = fmd->pdev->dev.platform_data;
-	struct fimc_dev *fd = NULL;
-	int num_clients, ret, i;
+	struct device_node *of_node = fmd->pdev->dev.of_node;
+	int num_clients = 0;
+	int ret, i;
 
 	/*
 	 * Runtime resume one of the FIMC entities to make sure
 	 * the sclk_cam clocks are not globally disabled.
 	 */
-	for (i = 0; !fd && i < ARRAY_SIZE(fmd->fimc); i++)
-		if (fmd->fimc[i])
-			fd = fmd->fimc[i];
-	if (!fd)
+	if (!fmd->pmf)
 		return -ENXIO;
-	ret = pm_runtime_get_sync(&fd->pdev->dev);
+
+	ret = pm_runtime_get_sync(fmd->pmf);
 	if (ret < 0)
 		return ret;
 
-	WARN_ON(pdata->num_clients > ARRAY_SIZE(fmd->sensor));
-	num_clients = min_t(u32, pdata->num_clients, ARRAY_SIZE(fmd->sensor));
-
-	fmd->num_sensors = num_clients;
-	for (i = 0; i < num_clients; i++) {
-		struct v4l2_subdev *sd;
-
-		fmd->sensor[i].pdata = pdata->source_info[i];
-		ret = __fimc_md_set_camclk(fmd, &fmd->sensor[i], true);
-		if (ret)
-			break;
-		sd = fimc_md_register_sensor(fmd, &fmd->sensor[i]);
-		ret = __fimc_md_set_camclk(fmd, &fmd->sensor[i], false);
-
-		if (!IS_ERR(sd)) {
-			fmd->sensor[i].subdev = sd;
-		} else {
-			fmd->sensor[i].subdev = NULL;
-			ret = PTR_ERR(sd);
-			break;
+	if (of_node) {
+		fmd->num_sensors = 0;
+		ret = fimc_md_of_sensors_register(fmd, of_node);
+	} else if (pdata) {
+		WARN_ON(pdata->num_clients > ARRAY_SIZE(fmd->sensor));
+		num_clients = min_t(u32, pdata->num_clients,
+				    ARRAY_SIZE(fmd->sensor));
+		fmd->num_sensors = num_clients;
+
+		for (i = 0; i < num_clients; i++) {
+			struct fimc_sensor_info *si = &fmd->sensor[i];
+			struct v4l2_subdev *sd;
+
+			si->pdata = pdata->source_info[i];
+			ret = __fimc_md_set_camclk(fmd, &si->pdata, true);
+			if (ret)
+				break;
+			sd = fimc_md_register_sensor(fmd, &si->pdata);
+			ret = __fimc_md_set_camclk(fmd, &si->pdata, false);
+
+			if (IS_ERR(sd)) {
+				si->subdev = NULL;
+				ret = PTR_ERR(sd);
+				break;
+			}
+			si->subdev = sd;
+			if (ret)
+				break;
 		}
-		if (ret)
-			break;
 	}
-	pm_runtime_put(&fd->pdev->dev);
+
+	pm_runtime_put(fmd->pmf);
 	return ret;
 }
 
@@ -352,6 +625,8 @@ static int register_fimc_entity(struct fimc_md *fmd, struct fimc_dev *fimc)
 
 	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
 	if (!ret) {
+		if (!fmd->pmf && fimc->pdev)
+			fmd->pmf = &fimc->pdev->dev;
 		fmd->fimc[fimc->id] = fimc;
 		fimc->vid_cap.user_subdev_api = fmd->user_subdev_api;
 	} else {
@@ -368,13 +643,13 @@ static int register_csis_entity(struct fimc_md *fmd,
 	struct device_node *node = pdev->dev.of_node;
 	int id, ret;
 
-	id = node ? of_alias_get_id(node, "csis") : max(0, pdev->id);
+	id = node ? __of_get_csis_id(node) : max(0, pdev->id);
 
-	if (WARN_ON(id >= CSIS_MAX_ENTITIES || fmd->csis[id].sd))
-		return -EBUSY;
+	if (WARN_ON(id < 0 || id >= CSIS_MAX_ENTITIES))
+		return -ENOENT;
 
-	if (WARN_ON(id >= CSIS_MAX_ENTITIES))
-		return 0;
+	if (WARN_ON(fmd->csis[id].sd))
+		return -EBUSY;
 
 	sd->grp_id = GRP_ID_CSIS;
 	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
@@ -386,6 +661,22 @@ static int register_csis_entity(struct fimc_md *fmd,
 	return ret;
 }
 
+static int register_fimc_is_entity(struct fimc_md *fmd, struct fimc_is *is)
+{
+	struct v4l2_subdev *sd = &is->isp.subdev;
+	int ret;
+
+	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
+	if (ret) {
+		v4l2_err(&fmd->v4l2_dev,
+			 "Failed to register FIMC-ISP (%d)\n", ret);
+		return ret;
+	}
+
+	fmd->fimc_is = is;
+	return 0;
+}
+
 static int fimc_md_register_platform_entity(struct fimc_md *fmd,
 					    struct platform_device *pdev,
 					    int plat_entity)
@@ -413,6 +704,9 @@ static int fimc_md_register_platform_entity(struct fimc_md *fmd,
 		case IDX_CSIS:
 			ret = register_csis_entity(fmd, pdev, drvdata);
 			break;
+		case IDX_IS_ISP:
+			ret = register_fimc_is_entity(fmd, drvdata);
+			break;
 		default:
 			ret = -ENODEV;
 		}
@@ -457,6 +751,47 @@ static int fimc_md_pdev_match(struct device *dev, void *data)
 	return 0;
 }
 
+/* Register FIMC, FIMC-LITE and CSIS media entities */
+#ifdef CONFIG_OF
+static int fimc_md_register_of_platform_entities(struct fimc_md *fmd,
+						 struct device_node *parent)
+{
+	struct device_node *node;
+	int ret = 0;
+
+	for_each_available_child_of_node(parent, node) {
+		struct platform_device *pdev;
+		int plat_entity = -1;
+
+		pdev = of_find_device_by_node(node);
+		if (!pdev)
+			continue;
+
+		/* If driver of any entity isn't ready try all again later. */
+		if (!strcmp(node->name, CSIS_OF_NODE_NAME))
+			plat_entity = IDX_CSIS;
+		else if	(!strcmp(node->name, FIMC_IS_OF_NODE_NAME))
+			plat_entity = IDX_IS_ISP;
+		else if (!strcmp(node->name, FIMC_LITE_OF_NODE_NAME))
+			plat_entity = IDX_FLITE;
+		else if	(!strcmp(node->name, FIMC_OF_NODE_NAME) &&
+			 !of_property_read_bool(node, "samsung,lcd-wb"))
+			plat_entity = IDX_FIMC;
+
+		if (plat_entity >= 0)
+			ret = fimc_md_register_platform_entity(fmd, pdev,
+							plat_entity);
+		put_device(&pdev->dev);
+		if (ret < 0)
+			break;
+	}
+
+	return ret;
+}
+#else
+#define fimc_md_register_of_platform_entities(fmd, node) (-ENOSYS)
+#endif
+
 static void fimc_md_unregister_entities(struct fimc_md *fmd)
 {
 	int i;
@@ -479,7 +814,6 @@ static void fimc_md_unregister_entities(struct fimc_md *fmd)
 		if (fmd->csis[i].sd == NULL)
 			continue;
 		v4l2_device_unregister_subdev(fmd->csis[i].sd);
-		module_put(fmd->csis[i].sd->owner);
 		fmd->csis[i].sd = NULL;
 	}
 	for (i = 0; i < fmd->num_sensors; i++) {
@@ -488,6 +822,10 @@ static void fimc_md_unregister_entities(struct fimc_md *fmd)
 		fimc_md_unregister_sensor(fmd->sensor[i].subdev);
 		fmd->sensor[i].subdev = NULL;
 	}
+
+	if (fmd->fimc_is)
+		v4l2_device_unregister_subdev(&fmd->fimc_is->isp.subdev);
+
 	v4l2_info(&fmd->v4l2_dev, "Unregistered all entities\n");
 }
 
@@ -504,12 +842,19 @@ static int __fimc_md_create_fimc_sink_links(struct fimc_md *fmd,
 					    struct v4l2_subdev *sensor,
 					    int pad, int link_mask)
 {
-	struct fimc_sensor_info *s_info = NULL;
+	struct fimc_source_info *si = NULL;
 	struct media_entity *sink;
 	unsigned int flags = 0;
-	int ret, i;
+	int i, ret = 0;
 
-	for (i = 0; i < FIMC_MAX_DEVS; i++) {
+	if (sensor) {
+		si = v4l2_get_subdev_hostdata(sensor);
+		/* Skip direct FIMC links in the logical FIMC-IS sensor path */
+		if (si && si->fimc_bus_type == FIMC_BUS_TYPE_ISP_WRITEBACK)
+			ret = 1;
+	}
+
+	for (i = 0; !ret && i < FIMC_MAX_DEVS; i++) {
 		if (!fmd->fimc[i])
 			continue;
 		/*
@@ -523,7 +868,7 @@ static int __fimc_md_create_fimc_sink_links(struct fimc_md *fmd,
 
 		sink = &fmd->fimc[i]->vid_cap.subdev.entity;
 		ret = media_entity_create_link(source, pad, sink,
-					      FIMC_SD_PAD_SINK, flags);
+					      FIMC_SD_PAD_SINK_CAM, flags);
 		if (ret)
 			return ret;
 
@@ -538,11 +883,13 @@ static int __fimc_md_create_fimc_sink_links(struct fimc_md *fmd,
 
 		if (flags == 0 || sensor == NULL)
 			continue;
-		s_info = v4l2_get_subdev_hostdata(sensor);
-		if (!WARN_ON(s_info == NULL)) {
+
+		if (!WARN_ON(si == NULL)) {
 			unsigned long irq_flags;
+			struct fimc_sensor_info *inf = source_to_sensor_info(si);
+
 			spin_lock_irqsave(&fmd->slock, irq_flags);
-			s_info->host = fmd->fimc[i];
+			inf->host = fmd->fimc[i];
 			spin_unlock_irqrestore(&fmd->slock, irq_flags);
 		}
 	}
@@ -551,25 +898,20 @@ static int __fimc_md_create_fimc_sink_links(struct fimc_md *fmd,
 		if (!fmd->fimc_lite[i])
 			continue;
 
-		if (link_mask & (1 << (i + FIMC_MAX_DEVS)))
-			flags = MEDIA_LNK_FL_ENABLED;
-		else
-			flags = 0;
-
 		sink = &fmd->fimc_lite[i]->subdev.entity;
 		ret = media_entity_create_link(source, pad, sink,
-					       FLITE_SD_PAD_SINK, flags);
+					       FLITE_SD_PAD_SINK, 0);
 		if (ret)
 			return ret;
 
 		/* Notify FIMC-LITE subdev entity */
 		ret = media_entity_call(sink, link_setup, &sink->pads[0],
-					&source->pads[pad], flags);
+					&source->pads[pad], 0);
 		if (ret)
 			break;
 
-		v4l2_info(&fmd->v4l2_dev, "created link [%s] %c> [%s]\n",
-			  source->name, flags ? '=' : '-', sink->name);
+		v4l2_info(&fmd->v4l2_dev, "created link [%s] -> [%s]\n",
+			  source->name, sink->name);
 	}
 	return 0;
 }
@@ -578,21 +920,50 @@ static int __fimc_md_create_fimc_sink_links(struct fimc_md *fmd,
 static int __fimc_md_create_flite_source_links(struct fimc_md *fmd)
 {
 	struct media_entity *source, *sink;
-	unsigned int flags = MEDIA_LNK_FL_ENABLED;
 	int i, ret = 0;
 
 	for (i = 0; i < FIMC_LITE_MAX_DEVS; i++) {
 		struct fimc_lite *fimc = fmd->fimc_lite[i];
+
 		if (fimc == NULL)
 			continue;
+
 		source = &fimc->subdev.entity;
 		sink = &fimc->vfd.entity;
 		/* FIMC-LITE's subdev and video node */
 		ret = media_entity_create_link(source, FLITE_SD_PAD_SOURCE_DMA,
-					       sink, 0, flags);
+					       sink, 0, 0);
+		if (ret)
+			break;
+		/* Link from FIMC-LITE to IS-ISP subdev */
+		sink = &fmd->fimc_is->isp.subdev.entity;
+		ret = media_entity_create_link(source, FLITE_SD_PAD_SOURCE_ISP,
+					       sink, 0, 0);
 		if (ret)
 			break;
-		/* TODO: create links to other entities */
+	}
+
+	return ret;
+}
+
+/* Create FIMC-IS links */
+static int __fimc_md_create_fimc_is_links(struct fimc_md *fmd)
+{
+	struct media_entity *source, *sink;
+	int i, ret;
+
+	source = &fmd->fimc_is->isp.subdev.entity;
+
+	for (i = 0; i < FIMC_MAX_DEVS; i++) {
+		if (fmd->fimc[i] == NULL)
+			continue;
+
+		/* Link from IS-ISP subdev to FIMC */
+		sink = &fmd->fimc[i]->vid_cap.subdev.entity;
+		ret = media_entity_create_link(source, FIMC_ISP_SD_PAD_SRC_FIFO,
+					       sink, FIMC_SD_PAD_SINK_FIFO, 0);
+		if (ret)
+			return ret;
 	}
 
 	return ret;
@@ -615,7 +986,6 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 	struct v4l2_subdev *csi_sensors[CSIS_MAX_ENTITIES] = { NULL };
 	struct v4l2_subdev *sensor, *csis;
 	struct fimc_source_info *pdata;
-	struct fimc_sensor_info *s_info;
 	struct media_entity *source, *sink;
 	int i, pad, fimc_id = 0, ret = 0;
 	u32 flags, link_mask = 0;
@@ -625,12 +995,11 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 			continue;
 
 		sensor = fmd->sensor[i].subdev;
-		s_info = v4l2_get_subdev_hostdata(sensor);
-		if (!s_info)
+		pdata = v4l2_get_subdev_hostdata(sensor);
+		if (!pdata)
 			continue;
 
 		source = NULL;
-		pdata = &s_info->pdata;
 
 		switch (pdata->sensor_bus_type) {
 		case FIMC_BUS_TYPE_MIPI_CSI2:
@@ -680,6 +1049,7 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 	for (i = 0; i < CSIS_MAX_ENTITIES; i++) {
 		if (fmd->csis[i].sd == NULL)
 			continue;
+
 		source = &fmd->csis[i].sd->entity;
 		pad = CSIS_PAD_SOURCE;
 		sensor = csi_sensors[i];
@@ -694,19 +1064,28 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 	for (i = 0; i < FIMC_MAX_DEVS; i++) {
 		if (!fmd->fimc[i])
 			continue;
+
 		source = &fmd->fimc[i]->vid_cap.subdev.entity;
 		sink = &fmd->fimc[i]->vid_cap.vfd.entity;
+
 		ret = media_entity_create_link(source, FIMC_SD_PAD_SOURCE,
 					      sink, 0, flags);
 		if (ret)
 			break;
 	}
 
-	return __fimc_md_create_flite_source_links(fmd);
+	ret = __fimc_md_create_flite_source_links(fmd);
+	if (ret < 0)
+		return ret;
+
+	if (fmd->use_isp)
+		ret = __fimc_md_create_fimc_is_links(fmd);
+
+	return ret;
 }
 
 /*
- * The peripheral sensor clock management.
+ * The peripheral sensor and CAM_BLK (PIXELASYNCMx) clocks management.
  */
 static void fimc_md_put_clocks(struct fimc_md *fmd)
 {
@@ -719,6 +1098,14 @@ static void fimc_md_put_clocks(struct fimc_md *fmd)
 		clk_put(fmd->camclk[i].clock);
 		fmd->camclk[i].clock = ERR_PTR(-EINVAL);
 	}
+
+	/* Writeback (PIXELASYNCMx) clocks */
+	for (i = 0; i < FIMC_MAX_WBCLKS; i++) {
+		if (IS_ERR(fmd->wbclk[i]))
+			continue;
+		clk_put(fmd->wbclk[i]);
+		fmd->wbclk[i] = ERR_PTR(-EINVAL);
+	}
 }
 
 static int fimc_md_get_clocks(struct fimc_md *fmd)
@@ -755,35 +1142,59 @@ static int fimc_md_get_clocks(struct fimc_md *fmd)
 	if (ret)
 		fimc_md_put_clocks(fmd);
 
+	if (!fmd->use_isp)
+		return 0;
+	/*
+	 * For now get only PIXELASYNCM1 clock (Writeback B/ISP),
+	 * leave PIXELASYNCM0 out for the LCD Writeback driver.
+	 */
+	fmd->wbclk[CLK_IDX_WB_A] = ERR_PTR(-EINVAL);
+
+	for (i = CLK_IDX_WB_B; i < FIMC_MAX_WBCLKS; i++) {
+		snprintf(clk_name, sizeof(clk_name), "pxl_async%u", i);
+		clock = clk_get(dev, clk_name);
+		if (IS_ERR(clock)) {
+			v4l2_err(&fmd->v4l2_dev, "Failed to get clock: %s\n",
+				  clk_name);
+			ret = PTR_ERR(clock);
+			break;
+		}
+		fmd->wbclk[i] = clock;
+	}
+	if (ret)
+		fimc_md_put_clocks(fmd);
+
 	return ret;
 }
 
 static int __fimc_md_set_camclk(struct fimc_md *fmd,
-				struct fimc_sensor_info *s_info,
+				struct fimc_source_info *si,
 				bool on)
 {
-	struct fimc_source_info *pdata = &s_info->pdata;
 	struct fimc_camclk_info *camclk;
 	int ret = 0;
 
-	if (WARN_ON(pdata->clk_id >= FIMC_MAX_CAMCLKS) || fmd == NULL)
+	if (WARN_ON(si->clk_id >= FIMC_MAX_CAMCLKS) || !fmd || !fmd->pmf)
 		return -EINVAL;
 
-	camclk = &fmd->camclk[pdata->clk_id];
+	camclk = &fmd->camclk[si->clk_id];
 
 	dbg("camclk %d, f: %lu, use_count: %d, on: %d",
-	    pdata->clk_id, pdata->clk_frequency, camclk->use_count, on);
+	    si->clk_id, si->clk_frequency, camclk->use_count, on);
 
 	if (on) {
 		if (camclk->use_count > 0 &&
-		    camclk->frequency != pdata->clk_frequency)
+		    camclk->frequency != si->clk_frequency)
 			return -EINVAL;
 
 		if (camclk->use_count++ == 0) {
-			clk_set_rate(camclk->clock, pdata->clk_frequency);
-			camclk->frequency = pdata->clk_frequency;
+			clk_set_rate(camclk->clock, si->clk_frequency);
+			camclk->frequency = si->clk_frequency;
+			ret = pm_runtime_get_sync(fmd->pmf);
+			if (ret < 0)
+				return ret;
 			ret = clk_enable(camclk->clock);
-			dbg("Enabled camclk %d: f: %lu", pdata->clk_id,
+			dbg("Enabled camclk %d: f: %lu", si->clk_id,
 			    clk_get_rate(camclk->clock));
 		}
 		return ret;
@@ -794,7 +1205,8 @@ static int __fimc_md_set_camclk(struct fimc_md *fmd,
 
 	if (--camclk->use_count == 0) {
 		clk_disable(camclk->clock);
-		dbg("Disabled camclk %d", pdata->clk_id);
+		pm_runtime_put(fmd->pmf);
+		dbg("Disabled camclk %d", si->clk_id);
 	}
 	return ret;
 }
@@ -813,10 +1225,10 @@ static int __fimc_md_set_camclk(struct fimc_md *fmd,
  */
 int fimc_md_set_camclk(struct v4l2_subdev *sd, bool on)
 {
-	struct fimc_sensor_info *s_info = v4l2_get_subdev_hostdata(sd);
+	struct fimc_source_info *si = v4l2_get_subdev_hostdata(sd);
 	struct fimc_md *fmd = entity_to_fimc_mdev(&sd->entity);
 
-	return __fimc_md_set_camclk(fmd, s_info, on);
+	return __fimc_md_set_camclk(fmd, si, on);
 }
 
 static int fimc_md_link_notify(struct media_pad *source,
@@ -926,13 +1338,33 @@ static ssize_t fimc_md_sysfs_store(struct device *dev,
 static DEVICE_ATTR(subdev_conf_mode, S_IWUSR | S_IRUGO,
 		   fimc_md_sysfs_show, fimc_md_sysfs_store);
 
+static int fimc_md_get_pinctrl(struct fimc_md *fmd)
+{
+	struct device *dev = &fmd->pdev->dev;
+	struct fimc_pinctrl *pctl = &fmd->pinctl;
+
+	pctl->pinctrl = devm_pinctrl_get(dev);
+	if (IS_ERR(pctl->pinctrl))
+		return PTR_ERR(pctl->pinctrl);
+
+	pctl->state_default = pinctrl_lookup_state(pctl->pinctrl,
+					PINCTRL_STATE_DEFAULT);
+	if (IS_ERR(pctl->state_default))
+		return PTR_ERR(pctl->state_default);
+
+	pctl->state_idle = pinctrl_lookup_state(pctl->pinctrl,
+					PINCTRL_STATE_IDLE);
+	return 0;
+}
+
 static int fimc_md_probe(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	struct v4l2_device *v4l2_dev;
 	struct fimc_md *fmd;
 	int ret;
 
-	fmd = devm_kzalloc(&pdev->dev, sizeof(*fmd), GFP_KERNEL);
+	fmd = devm_kzalloc(dev, sizeof(*fmd), GFP_KERNEL);
 	if (!fmd)
 		return -ENOMEM;
 
@@ -942,15 +1374,16 @@ static int fimc_md_probe(struct platform_device *pdev)
 	strlcpy(fmd->media_dev.model, "SAMSUNG S5P FIMC",
 		sizeof(fmd->media_dev.model));
 	fmd->media_dev.link_notify = fimc_md_link_notify;
-	fmd->media_dev.dev = &pdev->dev;
+	fmd->media_dev.dev = dev;
 
 	v4l2_dev = &fmd->v4l2_dev;
 	v4l2_dev->mdev = &fmd->media_dev;
 	v4l2_dev->notify = fimc_sensor_notify;
-	snprintf(v4l2_dev->name, sizeof(v4l2_dev->name), "%s",
-		 dev_name(&pdev->dev));
+	strlcpy(v4l2_dev->name, "s5p-fimc-md", sizeof(v4l2_dev->name));
 
-	ret = v4l2_device_register(&pdev->dev, &fmd->v4l2_dev);
+	fmd->use_isp = fimc_md_is_isp_available(dev->of_node);
+
+	ret = v4l2_device_register(dev, &fmd->v4l2_dev);
 	if (ret < 0) {
 		v4l2_err(v4l2_dev, "Failed to register v4l2_device: %d\n", ret);
 		return ret;
@@ -964,21 +1397,32 @@ static int fimc_md_probe(struct platform_device *pdev)
 	if (ret)
 		goto err_clk;
 
-	fmd->user_subdev_api = false;
+	fmd->user_subdev_api = (dev->of_node != NULL);
 
 	/* Protect the media graph while we're registering entities */
 	mutex_lock(&fmd->media_dev.graph_mutex);
 
-	ret = bus_for_each_dev(&platform_bus_type, NULL, fmd,
-					fimc_md_pdev_match);
+	ret = fimc_md_get_pinctrl(fmd);
+	if (ret < 0) {
+		if (ret != EPROBE_DEFER)
+			dev_err(dev, "Failed to get pinctrl: %d\n", ret);
+		goto err_unlock;
+	}
+
+	if (dev->of_node)
+		ret = fimc_md_register_of_platform_entities(fmd, dev->of_node);
+	else
+		ret = bus_for_each_dev(&platform_bus_type, NULL, fmd,
+						fimc_md_pdev_match);
 	if (ret)
 		goto err_unlock;
 
-	if (pdev->dev.platform_data) {
+	if (dev->platform_data || dev->of_node) {
 		ret = fimc_md_register_sensor_entities(fmd);
 		if (ret)
 			goto err_unlock;
 	}
+
 	ret = fimc_md_create_links(fmd);
 	if (ret)
 		goto err_unlock;
@@ -1018,12 +1462,25 @@ static int fimc_md_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static struct platform_device_id fimc_driver_ids[] __always_unused = {
+	{ .name = "s5p-fimc-md" },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, fimc_driver_ids);
+
+static const struct of_device_id fimc_md_of_match[] = {
+	{ .compatible = "samsung,fimc" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, fimc_md_of_match);
+
 static struct platform_driver fimc_md_driver = {
 	.probe		= fimc_md_probe,
 	.remove		= fimc_md_remove,
 	.driver = {
-		.name	= "s5p-fimc-md",
-		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(fimc_md_of_match),
+		.name		= "s5p-fimc-md",
+		.owner		= THIS_MODULE,
 	}
 };
 
diff --git a/drivers/media/platform/s5p-fimc/fimc-mdevice.h b/drivers/media/platform/exynos4-is/media-dev.h
index 06b0d8276fd2..44d86b61d660 100644
--- a/drivers/media/platform/s5p-fimc/fimc-mdevice.h
+++ b/drivers/media/platform/exynos4-is/media-dev.h
@@ -12,6 +12,8 @@
 #include <linux/clk.h>
 #include <linux/platform_device.h>
 #include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/pinctrl/consumer.h>
 #include <media/media-device.h>
 #include <media/media-entity.h>
 #include <media/v4l2-device.h>
@@ -21,18 +23,23 @@
 #include "fimc-lite.h"
 #include "mipi-csis.h"
 
-/* Group IDs of sensor, MIPI-CSIS, FIMC-LITE and the writeback subdevs. */
-#define GRP_ID_SENSOR		(1 << 8)
-#define GRP_ID_FIMC_IS_SENSOR	(1 << 9)
-#define GRP_ID_WRITEBACK	(1 << 10)
-#define GRP_ID_CSIS		(1 << 11)
-#define GRP_ID_FIMC		(1 << 12)
-#define GRP_ID_FLITE		(1 << 13)
-#define GRP_ID_FIMC_IS		(1 << 14)
+#define FIMC_OF_NODE_NAME	"fimc"
+#define FIMC_LITE_OF_NODE_NAME	"fimc-lite"
+#define FIMC_IS_OF_NODE_NAME	"fimc-is"
+#define CSIS_OF_NODE_NAME	"csis"
+
+#define PINCTRL_STATE_IDLE	"idle"
 
 #define FIMC_MAX_SENSORS	8
 #define FIMC_MAX_CAMCLKS	2
 
+/* LCD/ISP Writeback clocks (PIXELASYNCMx) */
+enum {
+	CLK_IDX_WB_A,
+	CLK_IDX_WB_B,
+	FIMC_MAX_WBCLKS
+};
+
 struct fimc_csis_info {
 	struct v4l2_subdev *sd;
 	int id;
@@ -65,9 +72,15 @@ struct fimc_sensor_info {
  * @num_sensors: actual number of registered sensors
  * @camclk: external sensor clock information
  * @fimc: array of registered fimc devices
+ * @fimc_is: fimc-is data structure
+ * @use_isp: set to true when FIMC-IS subsystem is used
+ * @pmf: handle to the CAMCLK clock control FIMC helper device
  * @media_dev: top level media device
  * @v4l2_dev: top level v4l2_device holding up the subdevs
  * @pdev: platform device this media device is hooked up into
+ * @pinctrl: camera port pinctrl handle
+ * @state_default: pinctrl default state handle
+ * @state_idle: pinctrl idle state handle
  * @user_subdev_api: true if subdevs are not configured by the host driver
  * @slock: spinlock protecting @sensor array
  */
@@ -76,11 +89,20 @@ struct fimc_md {
 	struct fimc_sensor_info sensor[FIMC_MAX_SENSORS];
 	int num_sensors;
 	struct fimc_camclk_info camclk[FIMC_MAX_CAMCLKS];
+	struct clk *wbclk[FIMC_MAX_WBCLKS];
 	struct fimc_lite *fimc_lite[FIMC_LITE_MAX_DEVS];
 	struct fimc_dev *fimc[FIMC_MAX_DEVS];
+	struct fimc_is *fimc_is;
+	bool use_isp;
+	struct device *pmf;
 	struct media_device media_dev;
 	struct v4l2_device v4l2_dev;
 	struct platform_device *pdev;
+	struct fimc_pinctrl {
+		struct pinctrl *pinctrl;
+		struct pinctrl_state *state_default;
+		struct pinctrl_state *state_idle;
+	} pinctl;
 	bool user_subdev_api;
 	spinlock_t slock;
 };
@@ -93,6 +115,12 @@ struct fimc_md {
 
 #define subdev_has_devnode(__sd) (__sd->flags & V4L2_SUBDEV_FL_HAS_DEVNODE)
 
+static inline
+struct fimc_sensor_info *source_to_sensor_info(struct fimc_source_info *si)
+{
+	return container_of(si, struct fimc_sensor_info, pdata);
+}
+
 static inline struct fimc_md *entity_to_fimc_mdev(struct media_entity *me)
 {
 	return me->parent == NULL ? NULL :
@@ -111,4 +139,14 @@ static inline void fimc_md_graph_unlock(struct fimc_dev *fimc)
 
 int fimc_md_set_camclk(struct v4l2_subdev *sd, bool on);
 
+#ifdef CONFIG_OF
+static inline bool fimc_md_is_isp_available(struct device_node *node)
+{
+	node = of_get_child_by_name(node, FIMC_IS_OF_NODE_NAME);
+	return node ? of_device_is_available(node) : false;
+}
+#else
+#define fimc_md_is_isp_available(node) (false)
+#endif /* CONFIG_OF */
+
 #endif
diff --git a/drivers/media/platform/s5p-fimc/mipi-csis.c b/drivers/media/platform/exynos4-is/mipi-csis.c
index 981863d05aaa..a2eda9d5ac87 100644
--- a/drivers/media/platform/s5p-fimc/mipi-csis.c
+++ b/drivers/media/platform/exynos4-is/mipi-csis.c
@@ -19,14 +19,18 @@
 #include <linux/kernel.h>
 #include <linux/memory.h>
 #include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_data/mipi-csis.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/regulator/consumer.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/videodev2.h>
+#include <media/s5p_fimc.h>
+#include <media/v4l2-of.h>
 #include <media/v4l2-subdev.h>
-#include <linux/platform_data/mipi-csis.h>
+
 #include "mipi-csis.h"
 
 static int debug;
@@ -113,6 +117,7 @@ static char *csi_clock_name[] = {
 	[CSIS_CLK_GATE] = "csis",
 };
 #define NUM_CSIS_CLOCKS	ARRAY_SIZE(csi_clock_name)
+#define DEFAULT_SCLK_CSIS_FREQ	166000000UL
 
 static const char * const csis_supply_name[] = {
 	"vddcore",  /* CSIS Core (1.0V, 1.1V or 1.2V) suppply */
@@ -167,6 +172,11 @@ struct csis_pktbuf {
  * @clock: CSIS clocks
  * @irq: requested s5p-mipi-csis irq number
  * @flags: the state variable for power and streaming control
+ * @clock_frequency: device bus clock frequency
+ * @hs_settle: HS-RX settle time
+ * @num_lanes: number of MIPI-CSI data lanes used
+ * @max_num_lanes: maximum number of MIPI-CSI data lanes supported
+ * @wclk_ext: CSI wrapper clock: 0 - bus clock, 1 - external SCLK_CAM
  * @csis_fmt: current CSIS pixel format
  * @format: common media bus format for the source and sink pad
  * @slock: spinlock protecting structure members below
@@ -184,6 +194,13 @@ struct csis_state {
 	struct clk *clock[NUM_CSIS_CLOCKS];
 	int irq;
 	u32 flags;
+
+	u32 clk_frequency;
+	u32 hs_settle;
+	u32 num_lanes;
+	u32 max_num_lanes;
+	u8 wclk_ext;
+
 	const struct csis_pix_format *csis_fmt;
 	struct v4l2_mbus_framefmt format;
 
@@ -273,7 +290,6 @@ static void s5pcsis_reset(struct csis_state *state)
 
 static void s5pcsis_system_enable(struct csis_state *state, int on)
 {
-	struct s5p_platform_mipi_csis *pdata = state->pdev->dev.platform_data;
 	u32 val, mask;
 
 	val = s5pcsis_read(state, S5PCSIS_CTRL);
@@ -286,7 +302,7 @@ static void s5pcsis_system_enable(struct csis_state *state, int on)
 	val = s5pcsis_read(state, S5PCSIS_DPHYCTRL);
 	val &= ~S5PCSIS_DPHYCTRL_ENABLE;
 	if (on) {
-		mask = (1 << (pdata->lanes + 1)) - 1;
+		mask = (1 << (state->num_lanes + 1)) - 1;
 		val |= (mask & S5PCSIS_DPHYCTRL_ENABLE);
 	}
 	s5pcsis_write(state, S5PCSIS_DPHYCTRL, val);
@@ -321,15 +337,14 @@ static void s5pcsis_set_hsync_settle(struct csis_state *state, int settle)
 
 static void s5pcsis_set_params(struct csis_state *state)
 {
-	struct s5p_platform_mipi_csis *pdata = state->pdev->dev.platform_data;
 	u32 val;
 
 	val = s5pcsis_read(state, S5PCSIS_CONFIG);
-	val = (val & ~S5PCSIS_CFG_NR_LANE_MASK) | (pdata->lanes - 1);
+	val = (val & ~S5PCSIS_CFG_NR_LANE_MASK) | (state->num_lanes - 1);
 	s5pcsis_write(state, S5PCSIS_CONFIG, val);
 
 	__s5pcsis_set_format(state);
-	s5pcsis_set_hsync_settle(state, pdata->hs_settle);
+	s5pcsis_set_hsync_settle(state, state->hs_settle);
 
 	val = s5pcsis_read(state, S5PCSIS_CTRL);
 	if (state->csis_fmt->data_alignment == 32)
@@ -338,7 +353,7 @@ static void s5pcsis_set_params(struct csis_state *state)
 		val &= ~S5PCSIS_CTRL_ALIGN_32BIT;
 
 	val &= ~S5PCSIS_CTRL_WCLK_EXTCLK;
-	if (pdata->wclk_source)
+	if (state->wclk_ext)
 		val |= S5PCSIS_CTRL_WCLK_EXTCLK;
 	s5pcsis_write(state, S5PCSIS_CTRL, val);
 
@@ -534,10 +549,10 @@ static struct csis_pix_format const *s5pcsis_try_format(
 
 static struct v4l2_mbus_framefmt *__s5pcsis_get_format(
 		struct csis_state *state, struct v4l2_subdev_fh *fh,
-		u32 pad, enum v4l2_subdev_format_whence which)
+		enum v4l2_subdev_format_whence which)
 {
 	if (which == V4L2_SUBDEV_FORMAT_TRY)
-		return fh ? v4l2_subdev_get_try_format(fh, pad) : NULL;
+		return fh ? v4l2_subdev_get_try_format(fh, 0) : NULL;
 
 	return &state->format;
 }
@@ -549,10 +564,7 @@ static int s5pcsis_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 	struct csis_pix_format const *csis_fmt;
 	struct v4l2_mbus_framefmt *mf;
 
-	if (fmt->pad != CSIS_PAD_SOURCE && fmt->pad != CSIS_PAD_SINK)
-		return -EINVAL;
-
-	mf = __s5pcsis_get_format(state, fh, fmt->pad, fmt->which);
+	mf = __s5pcsis_get_format(state, fh, fmt->which);
 
 	if (fmt->pad == CSIS_PAD_SOURCE) {
 		if (mf) {
@@ -579,10 +591,7 @@ static int s5pcsis_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 	struct csis_state *state = sd_to_csis_state(sd);
 	struct v4l2_mbus_framefmt *mf;
 
-	if (fmt->pad != CSIS_PAD_SOURCE && fmt->pad != CSIS_PAD_SINK)
-		return -EINVAL;
-
-	mf = __s5pcsis_get_format(state, fh, fmt->pad, fmt->which);
+	mf = __s5pcsis_get_format(state, fh, fmt->which);
 	if (!mf)
 		return -EINVAL;
 
@@ -701,52 +710,111 @@ static irqreturn_t s5pcsis_irq_handler(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+static int s5pcsis_get_platform_data(struct platform_device *pdev,
+				     struct csis_state *state)
+{
+	struct s5p_platform_mipi_csis *pdata = pdev->dev.platform_data;
+
+	if (pdata == NULL) {
+		dev_err(&pdev->dev, "Platform data not specified\n");
+		return -EINVAL;
+	}
+
+	state->clk_frequency = pdata->clk_rate;
+	state->num_lanes = pdata->lanes;
+	state->hs_settle = pdata->hs_settle;
+	state->index = max(0, pdev->id);
+	state->max_num_lanes = state->index ? CSIS1_MAX_LANES :
+					      CSIS0_MAX_LANES;
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static int s5pcsis_parse_dt(struct platform_device *pdev,
+			    struct csis_state *state)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct v4l2_of_endpoint endpoint;
+
+	if (of_property_read_u32(node, "clock-frequency",
+				 &state->clk_frequency))
+		state->clk_frequency = DEFAULT_SCLK_CSIS_FREQ;
+	if (of_property_read_u32(node, "bus-width",
+				 &state->max_num_lanes))
+		return -EINVAL;
+
+	node = v4l2_of_get_next_endpoint(node, NULL);
+	if (!node) {
+		dev_err(&pdev->dev, "No port node at %s\n",
+					node->full_name);
+		return -EINVAL;
+	}
+	/* Get port node and validate MIPI-CSI channel id. */
+	v4l2_of_parse_endpoint(node, &endpoint);
+
+	state->index = endpoint.port - FIMC_INPUT_MIPI_CSI2_0;
+	if (state->index < 0 || state->index >= CSIS_MAX_ENTITIES)
+		return -ENXIO;
+
+	/* Get MIPI CSI-2 bus configration from the endpoint node. */
+	of_property_read_u32(node, "samsung,csis-hs-settle",
+					&state->hs_settle);
+	state->wclk_ext = of_property_read_bool(node,
+					"samsung,csis-wclk");
+
+	state->num_lanes = endpoint.bus.mipi_csi2.num_data_lanes;
+
+	of_node_put(node);
+	return 0;
+}
+#else
+#define s5pcsis_parse_dt(pdev, state) (-ENOSYS)
+#endif
+
 static int s5pcsis_probe(struct platform_device *pdev)
 {
-	struct s5p_platform_mipi_csis *pdata;
+	struct device *dev = &pdev->dev;
 	struct resource *mem_res;
 	struct csis_state *state;
 	int ret = -ENOMEM;
 	int i;
 
-	state = devm_kzalloc(&pdev->dev, sizeof(*state), GFP_KERNEL);
+	state = devm_kzalloc(dev, sizeof(*state), GFP_KERNEL);
 	if (!state)
 		return -ENOMEM;
 
 	mutex_init(&state->lock);
 	spin_lock_init(&state->slock);
-
 	state->pdev = pdev;
-	state->index = max(0, pdev->id);
 
-	pdata = pdev->dev.platform_data;
-	if (pdata == NULL) {
-		dev_err(&pdev->dev, "Platform data not fully specified\n");
-		return -EINVAL;
-	}
+	if (dev->of_node)
+		ret = s5pcsis_parse_dt(pdev, state);
+	else
+		ret = s5pcsis_get_platform_data(pdev, state);
+	if (ret < 0)
+		return ret;
 
-	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);
+	if (state->num_lanes == 0 || state->num_lanes > state->max_num_lanes) {
+		dev_err(dev, "Unsupported number of data lanes: %d (max. %d)\n",
+			state->num_lanes, state->max_num_lanes);
 		return -EINVAL;
 	}
 
 	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	state->regs = devm_ioremap_resource(&pdev->dev, mem_res);
+	state->regs = devm_ioremap_resource(dev, mem_res);
 	if (IS_ERR(state->regs))
 		return PTR_ERR(state->regs);
 
 	state->irq = platform_get_irq(pdev, 0);
 	if (state->irq < 0) {
-		dev_err(&pdev->dev, "Failed to get irq\n");
+		dev_err(dev, "Failed to get irq\n");
 		return state->irq;
 	}
 
 	for (i = 0; i < CSIS_NUM_SUPPLIES; i++)
 		state->supplies[i].supply = csis_supply_name[i];
 
-	ret = devm_regulator_bulk_get(&pdev->dev, CSIS_NUM_SUPPLIES,
+	ret = devm_regulator_bulk_get(dev, CSIS_NUM_SUPPLIES,
 				 state->supplies);
 	if (ret)
 		return ret;
@@ -755,11 +823,11 @@ static int s5pcsis_probe(struct platform_device *pdev)
 	if (ret < 0)
 		return ret;
 
-	if (pdata->clk_rate)
+	if (state->clk_frequency)
 		ret = clk_set_rate(state->clock[CSIS_CLK_MUX],
-				   pdata->clk_rate);
+				   state->clk_frequency);
 	else
-		dev_WARN(&pdev->dev, "No clock frequency specified!\n");
+		dev_WARN(dev, "No clock frequency specified!\n");
 	if (ret < 0)
 		goto e_clkput;
 
@@ -767,16 +835,17 @@ static int s5pcsis_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto e_clkput;
 
-	ret = devm_request_irq(&pdev->dev, state->irq, s5pcsis_irq_handler,
-			       0, dev_name(&pdev->dev), state);
+	ret = devm_request_irq(dev, state->irq, s5pcsis_irq_handler,
+			       0, dev_name(dev), state);
 	if (ret) {
-		dev_err(&pdev->dev, "Interrupt request failed\n");
+		dev_err(dev, "Interrupt request failed\n");
 		goto e_clkdis;
 	}
 
 	v4l2_subdev_init(&state->sd, &s5pcsis_subdev_ops);
 	state->sd.owner = THIS_MODULE;
-	strlcpy(state->sd.name, dev_name(&pdev->dev), sizeof(state->sd.name));
+	snprintf(state->sd.name, sizeof(state->sd.name), "%s.%d",
+		 CSIS_SUBDEV_NAME, state->index);
 	state->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	state->csis_fmt = &s5pcsis_formats[0];
 
@@ -796,10 +865,12 @@ static int s5pcsis_probe(struct platform_device *pdev)
 
 	/* .. and a pointer to the subdev. */
 	platform_set_drvdata(pdev, &state->sd);
-
 	memcpy(state->events, s5pcsis_events, sizeof(state->events));
+	pm_runtime_enable(dev);
 
-	pm_runtime_enable(&pdev->dev);
+	dev_info(&pdev->dev, "lanes: %d, hs_settle: %d, wclk: %d, freq: %u\n",
+		 state->num_lanes, state->hs_settle, state->wclk_ext,
+		 state->clk_frequency);
 	return 0;
 
 e_clkdis:
@@ -923,13 +994,21 @@ static const struct dev_pm_ops s5pcsis_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(s5pcsis_suspend, s5pcsis_resume)
 };
 
+static const struct of_device_id s5pcsis_of_match[] = {
+	{ .compatible = "samsung,s5pv210-csis" },
+	{ .compatible = "samsung,exynos4210-csis" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, s5pcsis_of_match);
+
 static struct platform_driver s5pcsis_driver = {
 	.probe		= s5pcsis_probe,
 	.remove		= s5pcsis_remove,
 	.driver		= {
-		.name	= CSIS_DRIVER_NAME,
-		.owner	= THIS_MODULE,
-		.pm	= &s5pcsis_pm_ops,
+		.of_match_table = s5pcsis_of_match,
+		.name		= CSIS_DRIVER_NAME,
+		.owner		= THIS_MODULE,
+		.pm		= &s5pcsis_pm_ops,
 	},
 };
 
diff --git a/drivers/media/platform/s5p-fimc/mipi-csis.h b/drivers/media/platform/exynos4-is/mipi-csis.h
index 2709286396e1..28c11c4085d8 100644
--- a/drivers/media/platform/s5p-fimc/mipi-csis.h
+++ b/drivers/media/platform/exynos4-is/mipi-csis.h
@@ -11,6 +11,7 @@
 #define S5P_MIPI_CSIS_H_
 
 #define CSIS_DRIVER_NAME	"s5p-mipi-csis"
+#define CSIS_SUBDEV_NAME	CSIS_DRIVER_NAME
 #define CSIS_MAX_ENTITIES	2
 #define CSIS0_MAX_LANES		4
 #define CSIS1_MAX_LANES		2
diff --git a/drivers/media/platform/fsl-viu.c b/drivers/media/platform/fsl-viu.c
index 5f7db3f1f6f5..3a6a0dcdc3e4 100644
--- a/drivers/media/platform/fsl-viu.c
+++ b/drivers/media/platform/fsl-viu.c
@@ -957,12 +957,12 @@ static int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct viu_fh *fh = priv;
 
-	fh->dev->std = *id;
-	decoder_call(fh->dev, core, s_std, *id);
+	fh->dev->std = id;
+	decoder_call(fh->dev, core, s_std, id);
 	return 0;
 }
 
diff --git a/drivers/media/platform/m2m-deinterlace.c b/drivers/media/platform/m2m-deinterlace.c
index 6c4db9b98989..758564649589 100644
--- a/drivers/media/platform/m2m-deinterlace.c
+++ b/drivers/media/platform/m2m-deinterlace.c
@@ -207,6 +207,9 @@ static void dma_callback(void *data)
 	src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
 	dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
 
+	src_vb->v4l2_buf.timestamp = dst_vb->v4l2_buf.timestamp;
+	src_vb->v4l2_buf.timecode = dst_vb->v4l2_buf.timecode;
+
 	v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
 	v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
 
@@ -866,6 +869,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	src_vq->ops = &deinterlace_qops;
 	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 	q_data[V4L2_M2M_SRC].fmt = &formats[0];
 	q_data[V4L2_M2M_SRC].width = 640;
 	q_data[V4L2_M2M_SRC].height = 480;
@@ -882,6 +886,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	dst_vq->ops = &deinterlace_qops;
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 	q_data[V4L2_M2M_DST].fmt = &formats[0];
 	q_data[V4L2_M2M_DST].width = 640;
 	q_data[V4L2_M2M_DST].height = 480;
diff --git a/drivers/media/platform/marvell-ccic/mcam-core.c b/drivers/media/platform/marvell-ccic/mcam-core.c
index 92a33f081852..64ab91edfb81 100644
--- a/drivers/media/platform/marvell-ccic/mcam-core.c
+++ b/drivers/media/platform/marvell-ccic/mcam-core.c
@@ -1357,7 +1357,7 @@ static int mcam_vidioc_s_input(struct file *filp, void *priv, unsigned int i)
 }
 
 /* from vivi.c */
-static int mcam_vidioc_s_std(struct file *filp, void *priv, v4l2_std_id *a)
+static int mcam_vidioc_s_std(struct file *filp, void *priv, v4l2_std_id a)
 {
 	return 0;
 }
@@ -1445,7 +1445,7 @@ static int mcam_vidioc_g_register(struct file *file, void *priv,
 }
 
 static int mcam_vidioc_s_register(struct file *file, void *priv,
-		struct v4l2_dbg_register *reg)
+		const struct v4l2_dbg_register *reg)
 {
 	struct mcam_camera *cam = priv;
 
diff --git a/drivers/media/platform/mem2mem_testdev.c b/drivers/media/platform/mem2mem_testdev.c
index 7487d7208dea..4cc7f65d7d76 100644
--- a/drivers/media/platform/mem2mem_testdev.c
+++ b/drivers/media/platform/mem2mem_testdev.c
@@ -38,6 +38,10 @@ MODULE_AUTHOR("Pawel Osciak, <pawel@osciak.com>");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("0.1.1");
 
+static unsigned debug;
+module_param(debug, uint, 0644);
+MODULE_PARM_DESC(debug, "activates debug info");
+
 #define MIN_W 32
 #define MIN_H 32
 #define MAX_W 640
@@ -67,7 +71,7 @@ MODULE_VERSION("0.1.1");
 #define MEM2MEM_VFLIP	(1 << 1)
 
 #define dprintk(dev, fmt, arg...) \
-	v4l2_dbg(1, 1, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
+	v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
 
 
 static void m2mtest_dev_release(struct device *dev)
@@ -234,6 +238,10 @@ static int device_process(struct m2mtest_ctx *ctx,
 	bytes_left = bytesperline - tile_w * MEM2MEM_NUM_TILES;
 	w = 0;
 
+	memcpy(&out_vb->v4l2_buf.timestamp,
+			&in_vb->v4l2_buf.timestamp,
+			sizeof(struct timeval));
+
 	switch (ctx->mode) {
 	case MEM2MEM_HFLIP | MEM2MEM_VFLIP:
 		p_out += bytesperline * height - bytes_left;
@@ -844,6 +852,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *ds
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	src_vq->ops = &m2mtest_qops;
 	src_vq->mem_ops = &vb2_vmalloc_memops;
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -855,6 +864,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *ds
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	dst_vq->ops = &m2mtest_qops;
 	dst_vq->mem_ops = &vb2_vmalloc_memops;
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	return vb2_queue_init(dst_vq);
 }
diff --git a/drivers/media/platform/mx2_emmaprp.c b/drivers/media/platform/mx2_emmaprp.c
index 4b9e0a28616a..f7440e585b6b 100644
--- a/drivers/media/platform/mx2_emmaprp.c
+++ b/drivers/media/platform/mx2_emmaprp.c
@@ -377,6 +377,9 @@ static irqreturn_t emmaprp_irq(int irq_emma, void *data)
 			src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
 			dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
 
+			src_vb->v4l2_buf.timestamp = dst_vb->v4l2_buf.timestamp;
+			src_vb->v4l2_buf.timecode = dst_vb->v4l2_buf.timecode;
+
 			spin_lock_irqsave(&pcdev->irqlock, flags);
 			v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
 			v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
@@ -763,6 +766,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	src_vq->ops = &emmaprp_qops;
 	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -774,6 +778,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	dst_vq->ops = &emmaprp_qops;
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	return vb2_queue_init(dst_vq);
 }
diff --git a/drivers/media/platform/omap/omap_vout.c b/drivers/media/platform/omap/omap_vout.c
index 96c4a17e4280..477268a2415f 100644
--- a/drivers/media/platform/omap/omap_vout.c
+++ b/drivers/media/platform/omap/omap_vout.c
@@ -648,9 +648,12 @@ static void omap_vout_isr(void *arg, unsigned int irqstatus)
 
 	/* First save the configuration in ovelray structure */
 	ret = omapvid_init(vout, addr);
-	if (ret)
+	if (ret) {
 		printk(KERN_ERR VOUT_NAME
 			"failed to set overlay info\n");
+		goto vout_isr_err;
+	}
+
 	/* Enable the pipeline and set the Go bit */
 	ret = omapvid_apply_changes(vout);
 	if (ret)
@@ -1660,13 +1663,16 @@ static int vidioc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
 	mask = DISPC_IRQ_VSYNC | DISPC_IRQ_EVSYNC_EVEN | DISPC_IRQ_EVSYNC_ODD
 		| DISPC_IRQ_VSYNC2;
 
-	omap_dispc_register_isr(omap_vout_isr, vout, mask);
-
 	/* First save the configuration in ovelray structure */
 	ret = omapvid_init(vout, addr);
-	if (ret)
+	if (ret) {
 		v4l2_err(&vout->vid_dev->v4l2_dev,
 				"failed to set overlay info\n");
+		goto streamon_err1;
+	}
+
+	omap_dispc_register_isr(omap_vout_isr, vout, mask);
+
 	/* Enable the pipeline and set the Go bit */
 	ret = omapvid_apply_changes(vout);
 	if (ret)
diff --git a/drivers/media/platform/omap3isp/isp.c b/drivers/media/platform/omap3isp/isp.c
index 6e5ad8ec0a22..1d7dbd5c0fba 100644
--- a/drivers/media/platform/omap3isp/isp.c
+++ b/drivers/media/platform/omap3isp/isp.c
@@ -55,6 +55,7 @@
 #include <asm/cacheflush.h>
 
 #include <linux/clk.h>
+#include <linux/clkdev.h>
 #include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/dma-mapping.h>
@@ -148,6 +149,201 @@ void omap3isp_flush(struct isp_device *isp)
 	isp_reg_readl(isp, OMAP3_ISP_IOMEM_MAIN, ISP_REVISION);
 }
 
+/* -----------------------------------------------------------------------------
+ * XCLK
+ */
+
+#define to_isp_xclk(_hw)	container_of(_hw, struct isp_xclk, hw)
+
+static void isp_xclk_update(struct isp_xclk *xclk, u32 divider)
+{
+	switch (xclk->id) {
+	case ISP_XCLK_A:
+		isp_reg_clr_set(xclk->isp, OMAP3_ISP_IOMEM_MAIN, ISP_TCTRL_CTRL,
+				ISPTCTRL_CTRL_DIVA_MASK,
+				divider << ISPTCTRL_CTRL_DIVA_SHIFT);
+		break;
+	case ISP_XCLK_B:
+		isp_reg_clr_set(xclk->isp, OMAP3_ISP_IOMEM_MAIN, ISP_TCTRL_CTRL,
+				ISPTCTRL_CTRL_DIVB_MASK,
+				divider << ISPTCTRL_CTRL_DIVB_SHIFT);
+		break;
+	}
+}
+
+static int isp_xclk_prepare(struct clk_hw *hw)
+{
+	struct isp_xclk *xclk = to_isp_xclk(hw);
+
+	omap3isp_get(xclk->isp);
+
+	return 0;
+}
+
+static void isp_xclk_unprepare(struct clk_hw *hw)
+{
+	struct isp_xclk *xclk = to_isp_xclk(hw);
+
+	omap3isp_put(xclk->isp);
+}
+
+static int isp_xclk_enable(struct clk_hw *hw)
+{
+	struct isp_xclk *xclk = to_isp_xclk(hw);
+	unsigned long flags;
+
+	spin_lock_irqsave(&xclk->lock, flags);
+	isp_xclk_update(xclk, xclk->divider);
+	xclk->enabled = true;
+	spin_unlock_irqrestore(&xclk->lock, flags);
+
+	return 0;
+}
+
+static void isp_xclk_disable(struct clk_hw *hw)
+{
+	struct isp_xclk *xclk = to_isp_xclk(hw);
+	unsigned long flags;
+
+	spin_lock_irqsave(&xclk->lock, flags);
+	isp_xclk_update(xclk, 0);
+	xclk->enabled = false;
+	spin_unlock_irqrestore(&xclk->lock, flags);
+}
+
+static unsigned long isp_xclk_recalc_rate(struct clk_hw *hw,
+					  unsigned long parent_rate)
+{
+	struct isp_xclk *xclk = to_isp_xclk(hw);
+
+	return parent_rate / xclk->divider;
+}
+
+static u32 isp_xclk_calc_divider(unsigned long *rate, unsigned long parent_rate)
+{
+	u32 divider;
+
+	if (*rate >= parent_rate) {
+		*rate = parent_rate;
+		return ISPTCTRL_CTRL_DIV_BYPASS;
+	}
+
+	divider = DIV_ROUND_CLOSEST(parent_rate, *rate);
+	if (divider >= ISPTCTRL_CTRL_DIV_BYPASS)
+		divider = ISPTCTRL_CTRL_DIV_BYPASS - 1;
+
+	*rate = parent_rate / divider;
+	return divider;
+}
+
+static long isp_xclk_round_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long *parent_rate)
+{
+	isp_xclk_calc_divider(&rate, *parent_rate);
+	return rate;
+}
+
+static int isp_xclk_set_rate(struct clk_hw *hw, unsigned long rate,
+			     unsigned long parent_rate)
+{
+	struct isp_xclk *xclk = to_isp_xclk(hw);
+	unsigned long flags;
+	u32 divider;
+
+	divider = isp_xclk_calc_divider(&rate, parent_rate);
+
+	spin_lock_irqsave(&xclk->lock, flags);
+
+	xclk->divider = divider;
+	if (xclk->enabled)
+		isp_xclk_update(xclk, divider);
+
+	spin_unlock_irqrestore(&xclk->lock, flags);
+
+	dev_dbg(xclk->isp->dev, "%s: cam_xclk%c set to %lu Hz (div %u)\n",
+		__func__, xclk->id == ISP_XCLK_A ? 'a' : 'b', rate, divider);
+	return 0;
+}
+
+static const struct clk_ops isp_xclk_ops = {
+	.prepare = isp_xclk_prepare,
+	.unprepare = isp_xclk_unprepare,
+	.enable = isp_xclk_enable,
+	.disable = isp_xclk_disable,
+	.recalc_rate = isp_xclk_recalc_rate,
+	.round_rate = isp_xclk_round_rate,
+	.set_rate = isp_xclk_set_rate,
+};
+
+static const char *isp_xclk_parent_name = "cam_mclk";
+
+static const struct clk_init_data isp_xclk_init_data = {
+	.name = "cam_xclk",
+	.ops = &isp_xclk_ops,
+	.parent_names = &isp_xclk_parent_name,
+	.num_parents = 1,
+};
+
+static int isp_xclk_init(struct isp_device *isp)
+{
+	struct isp_platform_data *pdata = isp->pdata;
+	struct clk_init_data init;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(isp->xclks); ++i) {
+		struct isp_xclk *xclk = &isp->xclks[i];
+		struct clk *clk;
+
+		xclk->isp = isp;
+		xclk->id = i == 0 ? ISP_XCLK_A : ISP_XCLK_B;
+		xclk->divider = 1;
+		spin_lock_init(&xclk->lock);
+
+		init.name = i == 0 ? "cam_xclka" : "cam_xclkb";
+		init.ops = &isp_xclk_ops;
+		init.parent_names = &isp_xclk_parent_name;
+		init.num_parents = 1;
+
+		xclk->hw.init = &init;
+
+		clk = devm_clk_register(isp->dev, &xclk->hw);
+		if (IS_ERR(clk))
+			return PTR_ERR(clk);
+
+		if (pdata->xclks[i].con_id == NULL &&
+		    pdata->xclks[i].dev_id == NULL)
+			continue;
+
+		xclk->lookup = kzalloc(sizeof(*xclk->lookup), GFP_KERNEL);
+		if (xclk->lookup == NULL)
+			return -ENOMEM;
+
+		xclk->lookup->con_id = pdata->xclks[i].con_id;
+		xclk->lookup->dev_id = pdata->xclks[i].dev_id;
+		xclk->lookup->clk = clk;
+
+		clkdev_add(xclk->lookup);
+	}
+
+	return 0;
+}
+
+static void isp_xclk_cleanup(struct isp_device *isp)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(isp->xclks); ++i) {
+		struct isp_xclk *xclk = &isp->xclks[i];
+
+		if (xclk->lookup)
+			clkdev_drop(xclk->lookup);
+	}
+}
+
+/* -----------------------------------------------------------------------------
+ * Interrupts
+ */
+
 /*
  * isp_enable_interrupts - Enable ISP interrupts.
  * @isp: OMAP3 ISP device
@@ -180,80 +376,6 @@ static void isp_disable_interrupts(struct isp_device *isp)
 	isp_reg_writel(isp, 0, OMAP3_ISP_IOMEM_MAIN, ISP_IRQ0ENABLE);
 }
 
-/**
- * isp_set_xclk - Configures the specified cam_xclk to the desired frequency.
- * @isp: OMAP3 ISP device
- * @xclk: Desired frequency of the clock in Hz. 0 = stable low, 1 is stable high
- * @xclksel: XCLK to configure (0 = A, 1 = B).
- *
- * Configures the specified MCLK divisor in the ISP timing control register
- * (TCTRL_CTRL) to generate the desired xclk clock value.
- *
- * Divisor = cam_mclk_hz / xclk
- *
- * Returns the final frequency that is actually being generated
- **/
-static u32 isp_set_xclk(struct isp_device *isp, u32 xclk, u8 xclksel)
-{
-	u32 divisor;
-	u32 currentxclk;
-	unsigned long mclk_hz;
-
-	if (!omap3isp_get(isp))
-		return 0;
-
-	mclk_hz = clk_get_rate(isp->clock[ISP_CLK_CAM_MCLK]);
-
-	if (xclk >= mclk_hz) {
-		divisor = ISPTCTRL_CTRL_DIV_BYPASS;
-		currentxclk = mclk_hz;
-	} else if (xclk >= 2) {
-		divisor = mclk_hz / xclk;
-		if (divisor >= ISPTCTRL_CTRL_DIV_BYPASS)
-			divisor = ISPTCTRL_CTRL_DIV_BYPASS - 1;
-		currentxclk = mclk_hz / divisor;
-	} else {
-		divisor = xclk;
-		currentxclk = 0;
-	}
-
-	switch (xclksel) {
-	case ISP_XCLK_A:
-		isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_MAIN, ISP_TCTRL_CTRL,
-				ISPTCTRL_CTRL_DIVA_MASK,
-				divisor << ISPTCTRL_CTRL_DIVA_SHIFT);
-		dev_dbg(isp->dev, "isp_set_xclk(): cam_xclka set to %d Hz\n",
-			currentxclk);
-		break;
-	case ISP_XCLK_B:
-		isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_MAIN, ISP_TCTRL_CTRL,
-				ISPTCTRL_CTRL_DIVB_MASK,
-				divisor << ISPTCTRL_CTRL_DIVB_SHIFT);
-		dev_dbg(isp->dev, "isp_set_xclk(): cam_xclkb set to %d Hz\n",
-			currentxclk);
-		break;
-	case ISP_XCLK_NONE:
-	default:
-		omap3isp_put(isp);
-		dev_dbg(isp->dev, "ISP_ERR: isp_set_xclk(): Invalid requested "
-			"xclk. Must be 0 (A) or 1 (B).\n");
-		return -EINVAL;
-	}
-
-	/* Do we go from stable whatever to clock? */
-	if (divisor >= 2 && isp->xclk_divisor[xclksel - 1] < 2)
-		omap3isp_get(isp);
-	/* Stopping the clock. */
-	else if (divisor < 2 && isp->xclk_divisor[xclksel - 1] >= 2)
-		omap3isp_put(isp);
-
-	isp->xclk_divisor[xclksel - 1] = divisor;
-
-	omap3isp_put(isp);
-
-	return currentxclk;
-}
-
 /*
  * isp_core_init - ISP core settings
  * @isp: OMAP3 ISP device
@@ -1969,6 +2091,7 @@ static int isp_remove(struct platform_device *pdev)
 
 	isp_unregister_entities(isp);
 	isp_cleanup_modules(isp);
+	isp_xclk_cleanup(isp);
 
 	__omap3isp_get(isp, false);
 	iommu_detach_device(isp->domain, &pdev->dev);
@@ -2042,7 +2165,6 @@ static int isp_probe(struct platform_device *pdev)
 	}
 
 	isp->autoidle = autoidle;
-	isp->platform_cb.set_xclk = isp_set_xclk;
 
 	mutex_init(&isp->isp_mutex);
 	spin_lock_init(&isp->stat_lock);
@@ -2093,6 +2215,10 @@ static int isp_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto error_isp;
 
+	ret = isp_xclk_init(isp);
+	if (ret < 0)
+		goto error_isp;
+
 	/* Memory resources */
 	for (m = 0; m < ARRAY_SIZE(isp_res_maps); m++)
 		if (isp->revision == isp_res_maps[m].isp_rev)
@@ -2162,6 +2288,7 @@ detach_dev:
 free_domain:
 	iommu_domain_free(isp->domain);
 error_isp:
+	isp_xclk_cleanup(isp);
 	omap3isp_put(isp);
 error:
 	platform_set_drvdata(pdev, NULL);
diff --git a/drivers/media/platform/omap3isp/isp.h b/drivers/media/platform/omap3isp/isp.h
index c77e1f2ae5ca..cd3eff45ae7d 100644
--- a/drivers/media/platform/omap3isp/isp.h
+++ b/drivers/media/platform/omap3isp/isp.h
@@ -29,6 +29,7 @@
 
 #include <media/omap3isp.h>
 #include <media/v4l2-device.h>
+#include <linux/clk-provider.h>
 #include <linux/device.h>
 #include <linux/io.h>
 #include <linux/iommu.h>
@@ -125,8 +126,20 @@ struct isp_reg {
 	u32 val;
 };
 
-struct isp_platform_callback {
-	u32 (*set_xclk)(struct isp_device *isp, u32 xclk, u8 xclksel);
+enum isp_xclk_id {
+	ISP_XCLK_A,
+	ISP_XCLK_B,
+};
+
+struct isp_xclk {
+	struct isp_device *isp;
+	struct clk_hw hw;
+	struct clk_lookup *lookup;
+	enum isp_xclk_id id;
+
+	spinlock_t lock;	/* Protects enabled and divider */
+	bool enabled;
+	unsigned int divider;
 };
 
 /*
@@ -149,6 +162,7 @@ struct isp_platform_callback {
  * @cam_mclk: Pointer to camera functional clock structure.
  * @csi2_fck: Pointer to camera CSI2 complexIO clock structure.
  * @l3_ick: Pointer to OMAP3 L3 bus interface clock.
+ * @xclks: External clocks provided by the ISP
  * @irq: Currently attached ISP ISR callbacks information structure.
  * @isp_af: Pointer to current settings for ISP AutoFocus SCM.
  * @isp_hist: Pointer to current settings for ISP Histogram SCM.
@@ -185,12 +199,12 @@ struct isp_device {
 	int has_context;
 	int ref_count;
 	unsigned int autoidle;
-	u32 xclk_divisor[2];	/* Two clocks, a and b. */
 #define ISP_CLK_CAM_ICK		0
 #define ISP_CLK_CAM_MCLK	1
 #define ISP_CLK_CSI2_FCK	2
 #define ISP_CLK_L3_ICK		3
 	struct clk *clock[4];
+	struct isp_xclk xclks[2];
 
 	/* ISP modules */
 	struct ispstat isp_af;
@@ -209,8 +223,6 @@ struct isp_device {
 	unsigned int subclk_resources;
 
 	struct iommu_domain *domain;
-
-	struct isp_platform_callback platform_cb;
 };
 
 #define v4l2_dev_to_isp_device(dev) \
diff --git a/drivers/media/platform/s3c-camif/camif-capture.c b/drivers/media/platform/s3c-camif/camif-capture.c
index a55793c3d811..70438a0f62ae 100644
--- a/drivers/media/platform/s3c-camif/camif-capture.c
+++ b/drivers/media/platform/s3c-camif/camif-capture.c
@@ -934,12 +934,19 @@ static int s3c_camif_reqbufs(struct file *file, void *priv,
 		vp->owner = NULL;
 
 	ret = vb2_reqbufs(&vp->vb_queue, rb);
-	if (!ret) {
-		vp->reqbufs_count = rb->count;
-		if (vp->owner == NULL && rb->count > 0)
-			vp->owner = priv;
+	if (ret < 0)
+		return ret;
+
+	if (rb->count && rb->count < CAMIF_REQ_BUFS_MIN) {
+		rb->count = 0;
+		vb2_reqbufs(&vp->vb_queue, rb);
+		ret = -ENOMEM;
 	}
 
+	vp->reqbufs_count = rb->count;
+	if (vp->owner == NULL && rb->count > 0)
+		vp->owner = priv;
+
 	return ret;
 }
 
@@ -1153,6 +1160,7 @@ int s3c_camif_register_video_node(struct camif_dev *camif, int idx)
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct camif_buffer);
 	q->drv_priv = vp;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	ret = vb2_queue_init(q);
 	if (ret)
diff --git a/drivers/media/platform/s5p-g2d/g2d.c b/drivers/media/platform/s5p-g2d/g2d.c
index aaaf276a5a6c..553d87e5ceab 100644
--- a/drivers/media/platform/s5p-g2d/g2d.c
+++ b/drivers/media/platform/s5p-g2d/g2d.c
@@ -18,6 +18,7 @@
 #include <linux/slab.h>
 #include <linux/clk.h>
 #include <linux/interrupt.h>
+#include <linux/of.h>
 
 #include <linux/platform_device.h>
 #include <media/v4l2-mem2mem.h>
@@ -157,6 +158,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->ops = &g2d_qops;
 	src_vq->mem_ops = &vb2_dma_contig_memops;
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -168,6 +170,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->ops = &g2d_qops;
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	return vb2_queue_init(dst_vq);
 }
@@ -634,6 +637,9 @@ static irqreturn_t g2d_isr(int irq, void *prv)
 	BUG_ON(src == NULL);
 	BUG_ON(dst == NULL);
 
+	dst->v4l2_buf.timecode = src->v4l2_buf.timecode;
+	dst->v4l2_buf.timestamp = src->v4l2_buf.timestamp;
+
 	v4l2_m2m_buf_done(src, VB2_BUF_STATE_DONE);
 	v4l2_m2m_buf_done(dst, VB2_BUF_STATE_DONE);
 	v4l2_m2m_job_finish(dev->m2m_dev, ctx->m2m_ctx);
@@ -695,11 +701,14 @@ static struct v4l2_m2m_ops g2d_m2m_ops = {
 	.unlock		= g2d_unlock,
 };
 
+static const struct of_device_id exynos_g2d_match[];
+
 static int g2d_probe(struct platform_device *pdev)
 {
 	struct g2d_dev *dev;
 	struct video_device *vfd;
 	struct resource *res;
+	const struct of_device_id *of_id;
 	int ret = 0;
 
 	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
@@ -794,7 +803,17 @@ static int g2d_probe(struct platform_device *pdev)
 	}
 
 	def_frame.stride = (def_frame.width * def_frame.fmt->depth) >> 3;
-	dev->variant = g2d_get_drv_data(pdev);
+
+	if (!pdev->dev.of_node) {
+		dev->variant = g2d_get_drv_data(pdev);
+	} else {
+		of_id = of_match_node(exynos_g2d_match, pdev->dev.of_node);
+		if (!of_id) {
+			ret = -ENODEV;
+			goto unreg_video_dev;
+		}
+		dev->variant = (struct g2d_variant *)of_id->data;
+	}
 
 	return 0;
 
@@ -835,13 +854,25 @@ static int g2d_remove(struct platform_device *pdev)
 }
 
 static struct g2d_variant g2d_drvdata_v3x = {
-	.hw_rev = TYPE_G2D_3X,
+	.hw_rev = TYPE_G2D_3X, /* Revision 3.0 for S5PV210 and Exynos4210 */
 };
 
 static struct g2d_variant g2d_drvdata_v4x = {
 	.hw_rev = TYPE_G2D_4X, /* Revision 4.1 for Exynos4X12 and Exynos5 */
 };
 
+static const struct of_device_id exynos_g2d_match[] = {
+	{
+		.compatible = "samsung,s5pv210-g2d",
+		.data = &g2d_drvdata_v3x,
+	}, {
+		.compatible = "samsung,exynos4212-g2d",
+		.data = &g2d_drvdata_v4x,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, exynos_g2d_match);
+
 static struct platform_device_id g2d_driver_ids[] = {
 	{
 		.name = "s5p-g2d",
@@ -861,6 +892,7 @@ static struct platform_driver g2d_pdrv = {
 	.driver		= {
 		.name = G2D_NAME,
 		.owner = THIS_MODULE,
+		.of_match_table = exynos_g2d_match,
 	},
 };
 
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.c b/drivers/media/platform/s5p-jpeg/jpeg-core.c
index 3b023752bcb4..15d23968d1de 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.c
@@ -1229,6 +1229,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	src_vq->ops = &s5p_jpeg_qops;
 	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -1240,6 +1241,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	dst_vq->ops = &s5p_jpeg_qops;
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	return vb2_queue_init(dst_vq);
 }
@@ -1287,6 +1289,9 @@ static irqreturn_t s5p_jpeg_irq(int irq, void *dev_id)
 		payload_size = jpeg_compressed_size(jpeg->regs);
 	}
 
+	dst_buf->v4l2_buf.timecode = src_buf->v4l2_buf.timecode;
+	dst_buf->v4l2_buf.timestamp = src_buf->v4l2_buf.timestamp;
+
 	v4l2_m2m_buf_done(src_buf, state);
 	if (curr_ctx->mode == S5P_JPEG_ENCODE)
 		vb2_set_plane_payload(dst_buf, 0, payload_size);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc.c b/drivers/media/platform/s5p-mfc/s5p_mfc.c
index 1cb6d57987c6..01f9ae0dadb0 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc.c
@@ -243,12 +243,10 @@ static void s5p_mfc_handle_frame_copy_time(struct s5p_mfc_ctx *ctx)
 	src_buf = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
 	list_for_each_entry(dst_buf, &ctx->dst_queue, list) {
 		if (vb2_dma_contig_plane_dma_addr(dst_buf->b, 0) == dec_y_addr) {
-			memcpy(&dst_buf->b->v4l2_buf.timecode,
-				&src_buf->b->v4l2_buf.timecode,
-				sizeof(struct v4l2_timecode));
-			memcpy(&dst_buf->b->v4l2_buf.timestamp,
-				&src_buf->b->v4l2_buf.timestamp,
-				sizeof(struct timeval));
+			dst_buf->b->v4l2_buf.timecode =
+						src_buf->b->v4l2_buf.timecode;
+			dst_buf->b->v4l2_buf.timestamp =
+						src_buf->b->v4l2_buf.timestamp;
 			switch (frame_type) {
 			case S5P_FIMV_DECODE_FRAME_I_FRAME:
 				dst_buf->b->v4l2_buf.flags |=
@@ -386,6 +384,8 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
 		} else {
 			mfc_debug(2, "MFC needs next buffer\n");
 			ctx->consumed_stream = 0;
+			if (src_buf->flags & MFC_BUF_FLAG_EOS)
+				ctx->state = MFCINST_FINISHING;
 			list_del(&src_buf->list);
 			ctx->src_queue_cnt--;
 			if (s5p_mfc_hw_call(dev->mfc_ops, err_dec, err) > 0)
@@ -804,6 +804,7 @@ static int s5p_mfc_open(struct file *file)
 		goto err_queue_init;
 	}
 	q->mem_ops = (struct vb2_mem_ops *)&vb2_dma_contig_memops;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 	ret = vb2_queue_init(q);
 	if (ret) {
 		mfc_err("Failed to initialize videobuf2 queue(capture)\n");
@@ -825,6 +826,7 @@ static int s5p_mfc_open(struct file *file)
 		goto err_queue_init;
 	}
 	q->mem_ops = (struct vb2_mem_ops *)&vb2_dma_contig_memops;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 	ret = vb2_queue_init(q);
 	if (ret) {
 		mfc_err("Failed to initialize videobuf2 queue(output)\n");
@@ -1016,7 +1018,7 @@ static void *mfc_get_drv_data(struct platform_device *pdev);
 
 static int s5p_mfc_alloc_memdevs(struct s5p_mfc_dev *dev)
 {
-	unsigned int mem_info[2];
+	unsigned int mem_info[2] = { };
 
 	dev->mem_dev_l = devm_kzalloc(&dev->plat_dev->dev,
 			sizeof(struct device), GFP_KERNEL);
@@ -1106,7 +1108,8 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	}
 
 	if (pdev->dev.of_node) {
-		if (s5p_mfc_alloc_memdevs(dev) < 0)
+		ret = s5p_mfc_alloc_memdevs(dev);
+		if (ret < 0)
 			goto err_res;
 	} else {
 		dev->mem_dev_l = device_find_child(&dev->plat_dev->dev,
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
index 138778083c63..ad4f1df0a18e 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
@@ -16,7 +16,7 @@
 #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,
+static int s5p_mfc_cmd_host2risc_v5(struct s5p_mfc_dev *dev, int cmd,
 				struct s5p_mfc_cmd_args *args)
 {
 	int cur_cmd;
@@ -41,7 +41,7 @@ int s5p_mfc_cmd_host2risc_v5(struct s5p_mfc_dev *dev, int cmd,
 }
 
 /* Initialize the MFC */
-int s5p_mfc_sys_init_cmd_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_sys_init_cmd_v5(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_cmd_args h2r_args;
 
@@ -52,7 +52,7 @@ int s5p_mfc_sys_init_cmd_v5(struct s5p_mfc_dev *dev)
 }
 
 /* Suspend the MFC hardware */
-int s5p_mfc_sleep_cmd_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_sleep_cmd_v5(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_cmd_args h2r_args;
 
@@ -61,7 +61,7 @@ int s5p_mfc_sleep_cmd_v5(struct s5p_mfc_dev *dev)
 }
 
 /* Wake up the MFC hardware */
-int s5p_mfc_wakeup_cmd_v5(struct s5p_mfc_dev *dev)
+static int s5p_mfc_wakeup_cmd_v5(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_cmd_args h2r_args;
 
@@ -71,7 +71,7 @@ int s5p_mfc_wakeup_cmd_v5(struct s5p_mfc_dev *dev)
 }
 
 
-int s5p_mfc_open_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
+static 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;
@@ -124,7 +124,7 @@ int s5p_mfc_open_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
 	return ret;
 }
 
-int s5p_mfc_close_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
+static 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;
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
index 754bfbcb1c43..5708fc3d9b4d 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
@@ -17,7 +17,7 @@
 #include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
 
-int s5p_mfc_cmd_host2risc_v6(struct s5p_mfc_dev *dev, int cmd,
+static 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);
@@ -32,7 +32,7 @@ int s5p_mfc_cmd_host2risc_v6(struct s5p_mfc_dev *dev, int cmd,
 	return 0;
 }
 
-int s5p_mfc_sys_init_cmd_v6(struct s5p_mfc_dev *dev)
+static 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;
@@ -44,7 +44,7 @@ int s5p_mfc_sys_init_cmd_v6(struct s5p_mfc_dev *dev)
 					&h2r_args);
 }
 
-int s5p_mfc_sleep_cmd_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_sleep_cmd_v6(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_cmd_args h2r_args;
 
@@ -53,7 +53,7 @@ int s5p_mfc_sleep_cmd_v6(struct s5p_mfc_dev *dev)
 			&h2r_args);
 }
 
-int s5p_mfc_wakeup_cmd_v6(struct s5p_mfc_dev *dev)
+static int s5p_mfc_wakeup_cmd_v6(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_cmd_args h2r_args;
 
@@ -63,7 +63,7 @@ int s5p_mfc_wakeup_cmd_v6(struct s5p_mfc_dev *dev)
 }
 
 /* Open a new instance and get its number */
-int s5p_mfc_open_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
+static 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;
@@ -121,7 +121,7 @@ int s5p_mfc_open_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
 }
 
 /* Close instance */
-int s5p_mfc_close_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
+static 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;
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
index 4582473978ca..4af53bd2f182 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
@@ -22,6 +22,7 @@
 #include <linux/videodev2.h>
 #include <linux/workqueue.h>
 #include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 #include <media/videobuf2-core.h>
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
@@ -623,17 +624,27 @@ static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 /* Dequeue a buffer */
 static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 {
+	const struct v4l2_event ev = {
+		.type = V4L2_EVENT_EOS
+	};
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	int ret;
 
 	if (ctx->state == MFCINST_ERROR) {
 		mfc_err("Call on DQBUF after unrecoverable error\n");
 		return -EIO;
 	}
 	if (buf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
-		return vb2_dqbuf(&ctx->vq_src, buf, file->f_flags & O_NONBLOCK);
-	else if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
-		return vb2_dqbuf(&ctx->vq_dst, buf, file->f_flags & O_NONBLOCK);
-	return -EINVAL;
+		ret = vb2_dqbuf(&ctx->vq_src, buf, file->f_flags & O_NONBLOCK);
+	else if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		ret = vb2_dqbuf(&ctx->vq_dst, buf, file->f_flags & O_NONBLOCK);
+		if (ret == 0 && ctx->state == MFCINST_FINISHED &&
+				list_empty(&ctx->vq_dst.done_list))
+			v4l2_event_queue_fh(&ctx->fh, &ev);
+	} else {
+		ret = -EINVAL;
+	}
+	return ret;
 }
 
 /* Export DMA buffer */
@@ -809,6 +820,59 @@ static int vidioc_g_crop(struct file *file, void *priv,
 	return 0;
 }
 
+int vidioc_decoder_cmd(struct file *file, void *priv,
+						struct v4l2_decoder_cmd *cmd)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf *buf;
+	unsigned long flags;
+
+	switch (cmd->cmd) {
+	case V4L2_ENC_CMD_STOP:
+		if (cmd->flags != 0)
+			return -EINVAL;
+
+		if (!ctx->vq_src.streaming)
+			return -EINVAL;
+
+		spin_lock_irqsave(&dev->irqlock, flags);
+		if (list_empty(&ctx->src_queue)) {
+			mfc_err("EOS: empty src queue, entering finishing state");
+			ctx->state = MFCINST_FINISHING;
+			if (s5p_mfc_ctx_ready(ctx))
+				set_work_bit_irqsave(ctx);
+			spin_unlock_irqrestore(&dev->irqlock, flags);
+			s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+		} else {
+			mfc_err("EOS: marking last buffer of stream");
+			buf = list_entry(ctx->src_queue.prev,
+						struct s5p_mfc_buf, list);
+			if (buf->flags & MFC_BUF_FLAG_USED)
+				ctx->state = MFCINST_FINISHING;
+			else
+				buf->flags |= MFC_BUF_FLAG_EOS;
+			spin_unlock_irqrestore(&dev->irqlock, flags);
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int vidioc_subscribe_event(struct v4l2_fh *fh,
+				const struct  v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_EOS:
+		return v4l2_event_subscribe(fh, sub, 2, NULL);
+	default:
+		return -EINVAL;
+	}
+}
+
+
 /* v4l2_ioctl_ops */
 static const struct v4l2_ioctl_ops s5p_mfc_dec_ioctl_ops = {
 	.vidioc_querycap = vidioc_querycap,
@@ -830,6 +894,9 @@ static const struct v4l2_ioctl_ops s5p_mfc_dec_ioctl_ops = {
 	.vidioc_streamon = vidioc_streamon,
 	.vidioc_streamoff = vidioc_streamoff,
 	.vidioc_g_crop = vidioc_g_crop,
+	.vidioc_decoder_cmd = vidioc_decoder_cmd,
+	.vidioc_subscribe_event = vidioc_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static int s5p_mfc_queue_setup(struct vb2_queue *vq,
@@ -1147,3 +1214,4 @@ void s5p_mfc_dec_init(struct s5p_mfc_ctx *ctx)
 	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_opr_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
index f61dba837899..0af05a2d1cd4 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
@@ -34,7 +34,7 @@
 #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)
+static 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;
@@ -55,13 +55,13 @@ int s5p_mfc_alloc_dec_temp_buffers_v5(struct s5p_mfc_ctx *ctx)
 
 
 /* Release temporary buffers for decoding */
-void s5p_mfc_release_dec_desc_buffer_v5(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_release_dec_desc_buffer_v5(struct s5p_mfc_ctx *ctx)
 {
 	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->dsc);
 }
 
 /* Allocate codec buffers */
-int s5p_mfc_alloc_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
+static 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;
@@ -193,14 +193,14 @@ int s5p_mfc_alloc_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
 }
 
 /* Release buffers allocated for codec */
-void s5p_mfc_release_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_release_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
 {
 	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->bank1);
 	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_r, &ctx->bank2);
 }
 
 /* Allocate memory for instance data buffer */
-int s5p_mfc_alloc_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
+static 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;
@@ -241,20 +241,20 @@ int s5p_mfc_alloc_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
 }
 
 /* Release instance buffer */
-void s5p_mfc_release_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_release_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
 {
 	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->ctx);
 	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->shm);
 }
 
-int s5p_mfc_alloc_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
+static 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)
+static void s5p_mfc_release_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
 {
 	/* NOP */
 }
@@ -273,7 +273,7 @@ static unsigned int s5p_mfc_read_info_v5(struct s5p_mfc_ctx *ctx,
 	return readl(ctx->shm.virt + ofs);
 }
 
-void s5p_mfc_dec_calc_dpb_size_v5(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_dec_calc_dpb_size_v5(struct s5p_mfc_ctx *ctx)
 {
 	unsigned int guard_width, guard_height;
 
@@ -315,7 +315,7 @@ void s5p_mfc_dec_calc_dpb_size_v5(struct s5p_mfc_ctx *ctx)
 	}
 }
 
-void s5p_mfc_enc_calc_src_size_v5(struct s5p_mfc_ctx *ctx)
+static 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);
@@ -361,8 +361,9 @@ static void s5p_mfc_set_shared_buffer(struct s5p_mfc_ctx *ctx)
 }
 
 /* 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)
+static 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;
 
@@ -374,7 +375,7 @@ int s5p_mfc_set_dec_stream_buffer_v5(struct s5p_mfc_ctx *ctx, int buf_addr,
 }
 
 /* Set decoding frame buffer */
-int s5p_mfc_set_dec_frame_buffer_v5(struct s5p_mfc_ctx *ctx)
+static 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;
@@ -506,7 +507,7 @@ int s5p_mfc_set_dec_frame_buffer_v5(struct s5p_mfc_ctx *ctx)
 }
 
 /* Set registers for encoding stream buffer */
-int s5p_mfc_set_enc_stream_buffer_v5(struct s5p_mfc_ctx *ctx,
+static 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;
@@ -516,7 +517,7 @@ int s5p_mfc_set_enc_stream_buffer_v5(struct s5p_mfc_ctx *ctx,
 	return 0;
 }
 
-void s5p_mfc_set_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
+static 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;
@@ -525,7 +526,7 @@ void s5p_mfc_set_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
 	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,
+static 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;
@@ -537,7 +538,7 @@ void s5p_mfc_get_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
 }
 
 /* Set encoding ref & codec buffer */
-int s5p_mfc_set_enc_ref_buffer_v5(struct s5p_mfc_ctx *ctx)
+static 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;
@@ -1041,7 +1042,7 @@ static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
 }
 
 /* Initialize decoding */
-int s5p_mfc_init_decode_v5(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_init_decode_v5(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 
@@ -1077,7 +1078,7 @@ static void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
 }
 
 /* Decode a single frame */
-int s5p_mfc_decode_one_frame_v5(struct s5p_mfc_ctx *ctx,
+static 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;
@@ -1106,7 +1107,7 @@ int s5p_mfc_decode_one_frame_v5(struct s5p_mfc_ctx *ctx,
 	return 0;
 }
 
-int s5p_mfc_init_encode_v5(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_init_encode_v5(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 
@@ -1128,7 +1129,7 @@ int s5p_mfc_init_encode_v5(struct s5p_mfc_ctx *ctx)
 }
 
 /* Encode a single frame */
-int s5p_mfc_encode_one_frame_v5(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_encode_one_frame_v5(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	int cmd;
@@ -1187,6 +1188,15 @@ static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
 	unsigned long flags;
 	unsigned int index;
 
+	if (ctx->state == MFCINST_FINISHING) {
+		last_frame = MFC_DEC_LAST_FRAME;
+		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, last_frame);
+		return 0;
+	}
+
 	spin_lock_irqsave(&dev->irqlock, flags);
 	/* Frames are being decoded */
 	if (list_empty(&ctx->src_queue)) {
@@ -1353,7 +1363,7 @@ static int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
 }
 
 /* Try running an operation on hardware */
-void s5p_mfc_try_run_v5(struct s5p_mfc_dev *dev)
+static void s5p_mfc_try_run_v5(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_ctx *ctx;
 	int new_ctx;
@@ -1469,7 +1479,7 @@ void s5p_mfc_try_run_v5(struct s5p_mfc_dev *dev)
 }
 
 
-void s5p_mfc_cleanup_queue_v5(struct list_head *lh, struct vb2_queue *vq)
+static void s5p_mfc_cleanup_queue_v5(struct list_head *lh, struct vb2_queue *vq)
 {
 	struct s5p_mfc_buf *b;
 	int i;
@@ -1483,52 +1493,52 @@ void s5p_mfc_cleanup_queue_v5(struct list_head *lh, struct vb2_queue *vq)
 	}
 }
 
-void s5p_mfc_clear_int_flags_v5(struct s5p_mfc_dev *dev)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static int s5p_mfc_get_int_reason_v5(struct s5p_mfc_dev *dev)
 {
 	int reason;
 	reason = mfc_read(dev, S5P_FIMV_RISC2HOST_CMD) &
@@ -1576,98 +1586,98 @@ int s5p_mfc_get_int_reason_v5(struct s5p_mfc_dev *dev)
 	return reason;
 }
 
-int s5p_mfc_get_int_err_v5(struct s5p_mfc_dev *dev)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static unsigned int s5p_mfc_get_crop_info_v_v5(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v5(ctx, CROP_INFO_V);
 }
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
index 98a9c093a6b9..7e76fce2e524 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
@@ -49,7 +49,7 @@
 #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)
+static int s5p_mfc_alloc_dec_temp_buffers_v6(struct s5p_mfc_ctx *ctx)
 {
 	/* NOP */
 
@@ -57,19 +57,19 @@ int s5p_mfc_alloc_dec_temp_buffers_v6(struct s5p_mfc_ctx *ctx)
 }
 
 /* Release temproary buffers for decoding */
-void s5p_mfc_release_dec_desc_buffer_v6(struct s5p_mfc_ctx *ctx)
+static 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)
+static 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)
+static 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;
@@ -203,13 +203,13 @@ int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
 }
 
 /* Release buffers allocated for codec */
-void s5p_mfc_release_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_release_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
 {
 	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->bank1);
 }
 
 /* Allocate memory for instance data buffer */
-int s5p_mfc_alloc_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
+static 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;
@@ -258,13 +258,13 @@ int s5p_mfc_alloc_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
 }
 
 /* Release instance buffer */
-void s5p_mfc_release_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_release_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
 {
 	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->ctx);
 }
 
 /* Allocate context buffers for SYS_INIT */
-int s5p_mfc_alloc_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
+static 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;
 	int ret;
@@ -287,7 +287,7 @@ int s5p_mfc_alloc_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
 }
 
 /* Release context buffers for SYS_INIT */
-void s5p_mfc_release_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
+static void s5p_mfc_release_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
 {
 	s5p_mfc_release_priv_buf(dev->mem_dev_l, &dev->ctx_buf);
 }
@@ -306,7 +306,7 @@ static int calc_plane(int width, int height)
 		(mbY * S5P_FIMV_NUM_PIXELS_IN_MB_ROW_V6);
 }
 
-void s5p_mfc_dec_calc_dpb_size_v6(struct s5p_mfc_ctx *ctx)
+static 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);
@@ -326,7 +326,7 @@ void s5p_mfc_dec_calc_dpb_size_v6(struct s5p_mfc_ctx *ctx)
 	}
 }
 
-void s5p_mfc_enc_calc_src_size_v6(struct s5p_mfc_ctx *ctx)
+static void s5p_mfc_enc_calc_src_size_v6(struct s5p_mfc_ctx *ctx)
 {
 	unsigned int mb_width, mb_height;
 
@@ -339,8 +339,9 @@ void s5p_mfc_enc_calc_src_size_v6(struct s5p_mfc_ctx *ctx)
 }
 
 /* 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)
+static 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;
@@ -359,7 +360,7 @@ int s5p_mfc_set_dec_stream_buffer_v6(struct s5p_mfc_ctx *ctx, int buf_addr,
 }
 
 /* Set decoding frame buffer */
-int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
+static 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;
@@ -440,7 +441,7 @@ int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
 }
 
 /* Set registers for encoding stream buffer */
-int s5p_mfc_set_enc_stream_buffer_v6(struct s5p_mfc_ctx *ctx,
+static 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;
@@ -454,7 +455,7 @@ int s5p_mfc_set_enc_stream_buffer_v6(struct s5p_mfc_ctx *ctx,
 	return 0;
 }
 
-void s5p_mfc_set_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
+static 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;
@@ -466,7 +467,7 @@ void s5p_mfc_set_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
 	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,
+static 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;
@@ -483,7 +484,7 @@ void s5p_mfc_get_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
 }
 
 /* Set encoding ref & codec buffer */
-int s5p_mfc_set_enc_ref_buffer_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_set_enc_ref_buffer_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	size_t buf_addr1;
@@ -1147,7 +1148,7 @@ static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
 }
 
 /* Initialize decoding */
-int s5p_mfc_init_decode_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_init_decode_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	unsigned int reg = 0;
@@ -1215,7 +1216,7 @@ static inline void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
 }
 
 /* Decode a single frame */
-int s5p_mfc_decode_one_frame_v6(struct s5p_mfc_ctx *ctx,
+static 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;
@@ -1244,7 +1245,7 @@ int s5p_mfc_decode_one_frame_v6(struct s5p_mfc_ctx *ctx,
 	return 0;
 }
 
-int s5p_mfc_init_encode_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_init_encode_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 
@@ -1267,7 +1268,7 @@ int s5p_mfc_init_encode_v6(struct s5p_mfc_ctx *ctx)
 	return 0;
 }
 
-int s5p_mfc_h264_set_aso_slice_order_v6(struct s5p_mfc_ctx *ctx)
+static 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;
@@ -1283,7 +1284,7 @@ int s5p_mfc_h264_set_aso_slice_order_v6(struct s5p_mfc_ctx *ctx)
 }
 
 /* Encode a single frame */
-int s5p_mfc_encode_one_frame_v6(struct s5p_mfc_ctx *ctx)
+static int s5p_mfc_encode_one_frame_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 
@@ -1362,8 +1363,16 @@ static inline int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx)
 	unsigned long flags;
 	int last_frame = 0;
 
-	spin_lock_irqsave(&dev->irqlock, flags);
+	if (ctx->state == MFCINST_FINISHING) {
+		last_frame = MFC_DEC_LAST_FRAME;
+		s5p_mfc_set_dec_stream_buffer_v6(ctx, 0, 0, 0);
+		dev->curr_ctx = ctx->num;
+		s5p_mfc_clean_ctx_int_flags(ctx);
+		s5p_mfc_decode_one_frame_v6(ctx, last_frame);
+		return 0;
+	}
 
+	spin_lock_irqsave(&dev->irqlock, flags);
 	/* Frames are being decoded */
 	if (list_empty(&ctx->src_queue)) {
 		mfc_debug(2, "No src buffers.\n");
@@ -1540,7 +1549,7 @@ static inline int s5p_mfc_run_init_enc_buffers(struct s5p_mfc_ctx *ctx)
 }
 
 /* Try running an operation on hardware */
-void s5p_mfc_try_run_v6(struct s5p_mfc_dev *dev)
+static void s5p_mfc_try_run_v6(struct s5p_mfc_dev *dev)
 {
 	struct s5p_mfc_ctx *ctx;
 	int new_ctx;
@@ -1663,7 +1672,7 @@ void s5p_mfc_try_run_v6(struct s5p_mfc_dev *dev)
 }
 
 
-void s5p_mfc_cleanup_queue_v6(struct list_head *lh, struct vb2_queue *vq)
+static void s5p_mfc_cleanup_queue_v6(struct list_head *lh, struct vb2_queue *vq)
 {
 	struct s5p_mfc_buf *b;
 	int i;
@@ -1677,13 +1686,13 @@ void s5p_mfc_cleanup_queue_v6(struct list_head *lh, struct vb2_queue *vq)
 	}
 }
 
-void s5p_mfc_clear_int_flags_v6(struct s5p_mfc_dev *dev)
+static 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,
+static void s5p_mfc_write_info_v6(struct s5p_mfc_ctx *ctx, unsigned int data,
 		unsigned int ofs)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
@@ -1693,7 +1702,8 @@ void s5p_mfc_write_info_v6(struct s5p_mfc_ctx *ctx, unsigned int data,
 	s5p_mfc_clock_off();
 }
 
-unsigned int s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned int ofs)
+static unsigned int
+s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned int ofs)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	int ret;
@@ -1705,140 +1715,140 @@ unsigned int s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned int ofs)
 	return ret;
 }
 
-int s5p_mfc_get_dspl_y_adr_v6(struct s5p_mfc_dev *dev)
+static 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)
+static int s5p_mfc_get_dec_y_adr_v6(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_D_DECODED_LUMA_ADDR_V6);
 }
 
-int s5p_mfc_get_dspl_status_v6(struct s5p_mfc_dev *dev)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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)
+static 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);
 }
diff --git a/drivers/media/platform/s5p-tv/hdmi_drv.c b/drivers/media/platform/s5p-tv/hdmi_drv.c
index 8de1b3dce459..4e86626dad4b 100644
--- a/drivers/media/platform/s5p-tv/hdmi_drv.c
+++ b/drivers/media/platform/s5p-tv/hdmi_drv.c
@@ -31,6 +31,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/clk.h>
 #include <linux/regulator/consumer.h>
+#include <linux/v4l2-dv-timings.h>
 
 #include <media/s5p_hdmi.h>
 #include <media/v4l2-common.h>
@@ -43,9 +44,6 @@ MODULE_AUTHOR("Tomasz Stanislawski, <t.stanislaws@samsung.com>");
 MODULE_DESCRIPTION("Samsung HDMI");
 MODULE_LICENSE("GPL");
 
-/* default preset configured on probe */
-#define HDMI_DEFAULT_PRESET V4L2_DV_480P59_94
-
 struct hdmi_pulse {
 	u32 beg;
 	u32 end;
@@ -91,8 +89,8 @@ struct hdmi_device {
 	const struct hdmi_timings *cur_conf;
 	/** flag indicating that timings are dirty */
 	int cur_conf_dirty;
-	/** current preset */
-	u32 cur_preset;
+	/** current timings */
+	struct v4l2_dv_timings cur_timings;
 	/** other resources */
 	struct hdmi_resources res;
 };
@@ -252,7 +250,6 @@ static int hdmi_conf_apply(struct hdmi_device *hdmi_dev)
 {
 	struct device *dev = hdmi_dev->dev;
 	const struct hdmi_timings *conf = hdmi_dev->cur_conf;
-	struct v4l2_dv_preset preset;
 	int ret;
 
 	dev_dbg(dev, "%s\n", __func__);
@@ -267,11 +264,11 @@ static int hdmi_conf_apply(struct hdmi_device *hdmi_dev)
 	hdmi_write_mask(hdmi_dev, HDMI_PHY_RSTOUT,  0, HDMI_PHY_SW_RSTOUT);
 	mdelay(10);
 
-	/* configure presets */
-	preset.preset = hdmi_dev->cur_preset;
-	ret = v4l2_subdev_call(hdmi_dev->phy_sd, video, s_dv_preset, &preset);
+	/* configure timings */
+	ret = v4l2_subdev_call(hdmi_dev->phy_sd, video, s_dv_timings,
+				&hdmi_dev->cur_timings);
 	if (ret) {
-		dev_err(dev, "failed to set preset (%u)\n", preset.preset);
+		dev_err(dev, "failed to set timings\n");
 		return ret;
 	}
 
@@ -475,33 +472,26 @@ static const struct hdmi_timings hdmi_timings_1080p50 = {
 	.vsyn[0] = { .beg = 0 + 4, .end = 5 + 4},
 };
 
+/* default hdmi_timings index of the timings configured on probe */
+#define HDMI_DEFAULT_TIMINGS_IDX (0)
+
 static const struct {
-	u32 preset;
-	const struct hdmi_timings *timings;
+	bool reduced_fps;
+	const struct v4l2_dv_timings dv_timings;
+	const struct hdmi_timings *hdmi_timings;
 } hdmi_timings[] = {
-	{ V4L2_DV_480P59_94, &hdmi_timings_480p },
-	{ V4L2_DV_576P50, &hdmi_timings_576p50 },
-	{ V4L2_DV_720P50, &hdmi_timings_720p50 },
-	{ V4L2_DV_720P59_94, &hdmi_timings_720p60 },
-	{ V4L2_DV_720P60, &hdmi_timings_720p60 },
-	{ V4L2_DV_1080P24, &hdmi_timings_1080p24 },
-	{ V4L2_DV_1080P30, &hdmi_timings_1080p60 },
-	{ V4L2_DV_1080P50, &hdmi_timings_1080p50 },
-	{ V4L2_DV_1080I50, &hdmi_timings_1080i50 },
-	{ V4L2_DV_1080I60, &hdmi_timings_1080i60 },
-	{ V4L2_DV_1080P60, &hdmi_timings_1080p60 },
+	{ false, V4L2_DV_BT_CEA_720X480P59_94, &hdmi_timings_480p    },
+	{ false, V4L2_DV_BT_CEA_720X576P50,    &hdmi_timings_576p50  },
+	{ false, V4L2_DV_BT_CEA_1280X720P50,   &hdmi_timings_720p50  },
+	{ true,  V4L2_DV_BT_CEA_1280X720P60,   &hdmi_timings_720p60  },
+	{ false, V4L2_DV_BT_CEA_1920X1080P24,  &hdmi_timings_1080p24 },
+	{ false, V4L2_DV_BT_CEA_1920X1080P30,  &hdmi_timings_1080p60 },
+	{ false, V4L2_DV_BT_CEA_1920X1080P50,  &hdmi_timings_1080p50 },
+	{ false, V4L2_DV_BT_CEA_1920X1080I50,  &hdmi_timings_1080i50 },
+	{ false, V4L2_DV_BT_CEA_1920X1080I60,  &hdmi_timings_1080i60 },
+	{ false, V4L2_DV_BT_CEA_1920X1080P60,  &hdmi_timings_1080p60 },
 };
 
-static const struct hdmi_timings *hdmi_preset2timings(u32 preset)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(hdmi_timings); ++i)
-		if (hdmi_timings[i].preset == preset)
-			return  hdmi_timings[i].timings;
-	return NULL;
-}
-
 static int hdmi_streamon(struct hdmi_device *hdev)
 {
 	struct device *dev = hdev->dev;
@@ -621,29 +611,33 @@ static int hdmi_s_power(struct v4l2_subdev *sd, int on)
 	return IS_ERR_VALUE(ret) ? ret : 0;
 }
 
-static int hdmi_s_dv_preset(struct v4l2_subdev *sd,
-	struct v4l2_dv_preset *preset)
+static int hdmi_s_dv_timings(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings *timings)
 {
 	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
 	struct device *dev = hdev->dev;
-	const struct hdmi_timings *conf;
+	int i;
 
-	conf = hdmi_preset2timings(preset->preset);
-	if (conf == NULL) {
-		dev_err(dev, "preset (%u) not supported\n", preset->preset);
+	for (i = 0; i < ARRAY_SIZE(hdmi_timings); i++)
+		if (v4l_match_dv_timings(&hdmi_timings[i].dv_timings,
+					timings, 0))
+			break;
+	if (i == ARRAY_SIZE(hdmi_timings)) {
+		dev_err(dev, "timings not supported\n");
 		return -EINVAL;
 	}
-	hdev->cur_conf = conf;
+	hdev->cur_conf = hdmi_timings[i].hdmi_timings;
 	hdev->cur_conf_dirty = 1;
-	hdev->cur_preset = preset->preset;
+	hdev->cur_timings = *timings;
+	if (!hdmi_timings[i].reduced_fps)
+		hdev->cur_timings.bt.flags &= ~V4L2_DV_FL_CAN_REDUCE_FPS;
 	return 0;
 }
 
-static int hdmi_g_dv_preset(struct v4l2_subdev *sd,
-	struct v4l2_dv_preset *preset)
+static int hdmi_g_dv_timings(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings *timings)
 {
-	memset(preset, 0, sizeof(*preset));
-	preset->preset = sd_to_hdmi_dev(sd)->cur_preset;
+	*timings = sd_to_hdmi_dev(sd)->cur_timings;
 	return 0;
 }
 
@@ -670,13 +664,33 @@ static int hdmi_g_mbus_fmt(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int hdmi_enum_dv_presets(struct v4l2_subdev *sd,
-	struct v4l2_dv_enum_preset *preset)
+static int hdmi_enum_dv_timings(struct v4l2_subdev *sd,
+	struct v4l2_enum_dv_timings *timings)
 {
-	if (preset->index >= ARRAY_SIZE(hdmi_timings))
+	if (timings->index >= ARRAY_SIZE(hdmi_timings))
 		return -EINVAL;
-	return v4l_fill_dv_preset_info(hdmi_timings[preset->index].preset,
-		preset);
+	timings->timings = hdmi_timings[timings->index].dv_timings;
+	if (!hdmi_timings[timings->index].reduced_fps)
+		timings->timings.bt.flags &= ~V4L2_DV_FL_CAN_REDUCE_FPS;
+	return 0;
+}
+
+static int hdmi_dv_timings_cap(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings_cap *cap)
+{
+	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
+
+	/* Let the phy fill in the pixelclock range */
+	v4l2_subdev_call(hdev->phy_sd, video, dv_timings_cap, cap);
+	cap->type = V4L2_DV_BT_656_1120;
+	cap->bt.min_width = 720;
+	cap->bt.max_width = 1920;
+	cap->bt.min_height = 480;
+	cap->bt.max_height = 1080;
+	cap->bt.standards = V4L2_DV_BT_STD_CEA861;
+	cap->bt.capabilities = V4L2_DV_BT_CAP_INTERLACED |
+			       V4L2_DV_BT_CAP_PROGRESSIVE;
+	return 0;
 }
 
 static const struct v4l2_subdev_core_ops hdmi_sd_core_ops = {
@@ -684,9 +698,10 @@ static const struct v4l2_subdev_core_ops hdmi_sd_core_ops = {
 };
 
 static const struct v4l2_subdev_video_ops hdmi_sd_video_ops = {
-	.s_dv_preset = hdmi_s_dv_preset,
-	.g_dv_preset = hdmi_g_dv_preset,
-	.enum_dv_presets = hdmi_enum_dv_presets,
+	.s_dv_timings = hdmi_s_dv_timings,
+	.g_dv_timings = hdmi_g_dv_timings,
+	.enum_dv_timings = hdmi_enum_dv_timings,
+	.dv_timings_cap = hdmi_dv_timings_cap,
 	.g_mbus_fmt = hdmi_g_mbus_fmt,
 	.s_stream = hdmi_s_stream,
 };
@@ -956,9 +971,11 @@ static int hdmi_probe(struct platform_device *pdev)
 	sd->owner = THIS_MODULE;
 
 	strlcpy(sd->name, "s5p-hdmi", sizeof(sd->name));
-	hdmi_dev->cur_preset = HDMI_DEFAULT_PRESET;
-	/* FIXME: missing fail preset is not supported */
-	hdmi_dev->cur_conf = hdmi_preset2timings(hdmi_dev->cur_preset);
+	hdmi_dev->cur_timings =
+		hdmi_timings[HDMI_DEFAULT_TIMINGS_IDX].dv_timings;
+	/* FIXME: missing fail timings is not supported */
+	hdmi_dev->cur_conf =
+		hdmi_timings[HDMI_DEFAULT_TIMINGS_IDX].hdmi_timings;
 	hdmi_dev->cur_conf_dirty = 1;
 
 	/* storing subdev for call that have only access to struct device */
diff --git a/drivers/media/platform/s5p-tv/hdmiphy_drv.c b/drivers/media/platform/s5p-tv/hdmiphy_drv.c
index 80717cec76ae..e19a0af1ea4f 100644
--- a/drivers/media/platform/s5p-tv/hdmiphy_drv.c
+++ b/drivers/media/platform/s5p-tv/hdmiphy_drv.c
@@ -176,35 +176,9 @@ static inline struct hdmiphy_ctx *sd_to_ctx(struct v4l2_subdev *sd)
 	return container_of(sd, struct hdmiphy_ctx, sd);
 }
 
-static unsigned long hdmiphy_preset_to_pixclk(u32 preset)
+static const u8 *hdmiphy_find_conf(unsigned long pixclk,
+		const struct hdmiphy_conf *conf)
 {
-	static const unsigned long pixclk[] = {
-		[V4L2_DV_480P59_94] =  27000000,
-		[V4L2_DV_576P50]    =  27000000,
-		[V4L2_DV_720P59_94] =  74176000,
-		[V4L2_DV_720P50]    =  74250000,
-		[V4L2_DV_720P60]    =  74250000,
-		[V4L2_DV_1080P24]   =  74250000,
-		[V4L2_DV_1080P30]   =  74250000,
-		[V4L2_DV_1080I50]   =  74250000,
-		[V4L2_DV_1080I60]   =  74250000,
-		[V4L2_DV_1080P50]   = 148500000,
-		[V4L2_DV_1080P60]   = 148500000,
-	};
-	if (preset < ARRAY_SIZE(pixclk))
-		return pixclk[preset];
-	else
-		return 0;
-}
-
-static const u8 *hdmiphy_find_conf(u32 preset, const struct hdmiphy_conf *conf)
-{
-	unsigned long pixclk;
-
-	pixclk = hdmiphy_preset_to_pixclk(preset);
-	if (!pixclk)
-		return NULL;
-
 	for (; conf->pixclk; ++conf)
 		if (conf->pixclk == pixclk)
 			return conf->data;
@@ -217,8 +191,8 @@ static int hdmiphy_s_power(struct v4l2_subdev *sd, int on)
 	return 0;
 }
 
-static int hdmiphy_s_dv_preset(struct v4l2_subdev *sd,
-	struct v4l2_dv_preset *preset)
+static int hdmiphy_s_dv_timings(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings *timings)
 {
 	const u8 *data;
 	u8 buffer[32];
@@ -226,9 +200,12 @@ static int hdmiphy_s_dv_preset(struct v4l2_subdev *sd,
 	struct hdmiphy_ctx *ctx = sd_to_ctx(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct device *dev = &client->dev;
+	unsigned long pixclk = timings->bt.pixelclock;
 
-	dev_info(dev, "s_dv_preset(preset = %d)\n", preset->preset);
-	data = hdmiphy_find_conf(preset->preset, ctx->conf_tab);
+	dev_info(dev, "s_dv_timings\n");
+	if ((timings->bt.flags & V4L2_DV_FL_REDUCED_FPS) && pixclk == 74250000)
+		pixclk = 74176000;
+	data = hdmiphy_find_conf(pixclk, ctx->conf_tab);
 	if (!data) {
 		dev_err(dev, "format not supported\n");
 		return -EINVAL;
@@ -245,6 +222,17 @@ static int hdmiphy_s_dv_preset(struct v4l2_subdev *sd,
 	return 0;
 }
 
+static int hdmiphy_dv_timings_cap(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings_cap *cap)
+{
+	cap->type = V4L2_DV_BT_656_1120;
+	/* The phy only determines the pixelclock, leave the other values
+	 * at 0 to signify that we have no information for them. */
+	cap->bt.min_pixelclock = 27000000;
+	cap->bt.max_pixelclock = 148500000;
+	return 0;
+}
+
 static int hdmiphy_s_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
@@ -270,7 +258,8 @@ static const struct v4l2_subdev_core_ops hdmiphy_core_ops = {
 };
 
 static const struct v4l2_subdev_video_ops hdmiphy_video_ops = {
-	.s_dv_preset = hdmiphy_s_dv_preset,
+	.s_dv_timings = hdmiphy_s_dv_timings,
+	.dv_timings_cap = hdmiphy_dv_timings_cap,
 	.s_stream =  hdmiphy_s_stream,
 };
 
diff --git a/drivers/media/platform/s5p-tv/mixer_video.c b/drivers/media/platform/s5p-tv/mixer_video.c
index 82142a2d6d93..ef0efdf422fe 100644
--- a/drivers/media/platform/s5p-tv/mixer_video.c
+++ b/drivers/media/platform/s5p-tv/mixer_video.c
@@ -501,8 +501,8 @@ fail:
 	return -ERANGE;
 }
 
-static int mxr_enum_dv_presets(struct file *file, void *fh,
-	struct v4l2_dv_enum_preset *preset)
+static int mxr_enum_dv_timings(struct file *file, void *fh,
+	struct v4l2_enum_dv_timings *timings)
 {
 	struct mxr_layer *layer = video_drvdata(file);
 	struct mxr_device *mdev = layer->mdev;
@@ -510,14 +510,14 @@ static int mxr_enum_dv_presets(struct file *file, void *fh,
 
 	/* lock protects from changing sd_out */
 	mutex_lock(&mdev->mutex);
-	ret = v4l2_subdev_call(to_outsd(mdev), video, enum_dv_presets, preset);
+	ret = v4l2_subdev_call(to_outsd(mdev), video, enum_dv_timings, timings);
 	mutex_unlock(&mdev->mutex);
 
 	return ret ? -EINVAL : 0;
 }
 
-static int mxr_s_dv_preset(struct file *file, void *fh,
-	struct v4l2_dv_preset *preset)
+static int mxr_s_dv_timings(struct file *file, void *fh,
+	struct v4l2_dv_timings *timings)
 {
 	struct mxr_layer *layer = video_drvdata(file);
 	struct mxr_device *mdev = layer->mdev;
@@ -526,7 +526,7 @@ static int mxr_s_dv_preset(struct file *file, void *fh,
 	/* lock protects from changing sd_out */
 	mutex_lock(&mdev->mutex);
 
-	/* preset change cannot be done while there is an entity
+	/* timings change cannot be done while there is an entity
 	 * dependant on output configuration
 	 */
 	if (mdev->n_output > 0) {
@@ -534,7 +534,7 @@ static int mxr_s_dv_preset(struct file *file, void *fh,
 		return -EBUSY;
 	}
 
-	ret = v4l2_subdev_call(to_outsd(mdev), video, s_dv_preset, preset);
+	ret = v4l2_subdev_call(to_outsd(mdev), video, s_dv_timings, timings);
 
 	mutex_unlock(&mdev->mutex);
 
@@ -544,8 +544,8 @@ static int mxr_s_dv_preset(struct file *file, void *fh,
 	return ret ? -EINVAL : 0;
 }
 
-static int mxr_g_dv_preset(struct file *file, void *fh,
-	struct v4l2_dv_preset *preset)
+static int mxr_g_dv_timings(struct file *file, void *fh,
+	struct v4l2_dv_timings *timings)
 {
 	struct mxr_layer *layer = video_drvdata(file);
 	struct mxr_device *mdev = layer->mdev;
@@ -553,13 +553,28 @@ static int mxr_g_dv_preset(struct file *file, void *fh,
 
 	/* lock protects from changing sd_out */
 	mutex_lock(&mdev->mutex);
-	ret = v4l2_subdev_call(to_outsd(mdev), video, g_dv_preset, preset);
+	ret = v4l2_subdev_call(to_outsd(mdev), video, g_dv_timings, timings);
 	mutex_unlock(&mdev->mutex);
 
 	return ret ? -EINVAL : 0;
 }
 
-static int mxr_s_std(struct file *file, void *fh, v4l2_std_id *norm)
+static int mxr_dv_timings_cap(struct file *file, void *fh,
+	struct v4l2_dv_timings_cap *cap)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	struct mxr_device *mdev = layer->mdev;
+	int ret;
+
+	/* lock protects from changing sd_out */
+	mutex_lock(&mdev->mutex);
+	ret = v4l2_subdev_call(to_outsd(mdev), video, dv_timings_cap, cap);
+	mutex_unlock(&mdev->mutex);
+
+	return ret ? -EINVAL : 0;
+}
+
+static int mxr_s_std(struct file *file, void *fh, v4l2_std_id norm)
 {
 	struct mxr_layer *layer = video_drvdata(file);
 	struct mxr_device *mdev = layer->mdev;
@@ -576,7 +591,7 @@ static int mxr_s_std(struct file *file, void *fh, v4l2_std_id *norm)
 		return -EBUSY;
 	}
 
-	ret = v4l2_subdev_call(to_outsd(mdev), video, s_std_output, *norm);
+	ret = v4l2_subdev_call(to_outsd(mdev), video, s_std_output, norm);
 
 	mutex_unlock(&mdev->mutex);
 
@@ -616,8 +631,8 @@ static int mxr_enum_output(struct file *file, void *fh, struct v4l2_output *a)
 	/* try to obtain supported tv norms */
 	v4l2_subdev_call(sd, video, g_tvnorms_output, &a->std);
 	a->capabilities = 0;
-	if (sd->ops->video && sd->ops->video->s_dv_preset)
-		a->capabilities |= V4L2_OUT_CAP_PRESETS;
+	if (sd->ops->video && sd->ops->video->s_dv_timings)
+		a->capabilities |= V4L2_OUT_CAP_DV_TIMINGS;
 	if (sd->ops->video && sd->ops->video->s_std_output)
 		a->capabilities |= V4L2_OUT_CAP_STD;
 	a->type = V4L2_OUTPUT_TYPE_ANALOG;
@@ -738,10 +753,11 @@ static const struct v4l2_ioctl_ops mxr_ioctl_ops = {
 	/* Streaming control */
 	.vidioc_streamon = mxr_streamon,
 	.vidioc_streamoff = mxr_streamoff,
-	/* Preset functions */
-	.vidioc_enum_dv_presets = mxr_enum_dv_presets,
-	.vidioc_s_dv_preset = mxr_s_dv_preset,
-	.vidioc_g_dv_preset = mxr_g_dv_preset,
+	/* DV Timings functions */
+	.vidioc_enum_dv_timings = mxr_enum_dv_timings,
+	.vidioc_s_dv_timings = mxr_s_dv_timings,
+	.vidioc_g_dv_timings = mxr_g_dv_timings,
+	.vidioc_dv_timings_cap = mxr_dv_timings_cap,
 	/* analog TV standard functions */
 	.vidioc_s_std = mxr_s_std,
 	.vidioc_g_std = mxr_g_std,
diff --git a/drivers/media/platform/s5p-tv/sii9234_drv.c b/drivers/media/platform/s5p-tv/sii9234_drv.c
index d90d2286090b..39b77d24b9c2 100644
--- a/drivers/media/platform/s5p-tv/sii9234_drv.c
+++ b/drivers/media/platform/s5p-tv/sii9234_drv.c
@@ -23,9 +23,6 @@
 #include <linux/regulator/machine.h>
 #include <linux/slab.h>
 
-#include <mach/gpio.h>
-#include <plat/gpio-cfg.h>
-
 #include <media/sii9234.h>
 #include <media/v4l2-subdev.h>
 
diff --git a/drivers/media/platform/sh_veu.c b/drivers/media/platform/sh_veu.c
index cb54c69d5748..0b32cc3f6a47 100644
--- a/drivers/media/platform/sh_veu.c
+++ b/drivers/media/platform/sh_veu.c
@@ -10,6 +10,7 @@
  * published by the Free Software Foundation
  */
 
+#include <linux/err.h>
 #include <linux/fs.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
@@ -1164,9 +1165,9 @@ static int sh_veu_probe(struct platform_device *pdev)
 
 	veu->is_2h = resource_size(reg_res) == 0x22c;
 
-	veu->base = devm_request_and_ioremap(&pdev->dev, reg_res);
-	if (!veu->base)
-		return -ENOMEM;
+	veu->base = devm_ioremap_resource(&pdev->dev, reg_res);
+	if (IS_ERR(veu->base))
+		return PTR_ERR(veu->base);
 
 	ret = devm_request_threaded_irq(&pdev->dev, irq, sh_veu_isr, sh_veu_bh,
 					0, "veu", veu);
@@ -1248,18 +1249,7 @@ static struct platform_driver __refdata sh_veu_pdrv = {
 	},
 };
 
-static int __init sh_veu_init(void)
-{
-	return platform_driver_probe(&sh_veu_pdrv, sh_veu_probe);
-}
-
-static void __exit sh_veu_exit(void)
-{
-	platform_driver_unregister(&sh_veu_pdrv);
-}
-
-module_init(sh_veu_init);
-module_exit(sh_veu_exit);
+module_platform_driver_probe(sh_veu_pdrv, sh_veu_probe);
 
 MODULE_DESCRIPTION("sh-mobile VEU mem2mem driver");
 MODULE_AUTHOR("Guennadi Liakhovetski, <g.liakhovetski@gmx.de>");
diff --git a/drivers/media/platform/sh_vou.c b/drivers/media/platform/sh_vou.c
index 66c8da18df84..7d0235069c87 100644
--- a/drivers/media/platform/sh_vou.c
+++ b/drivers/media/platform/sh_vou.c
@@ -881,29 +881,29 @@ static u32 sh_vou_ntsc_mode(enum sh_vou_bus_fmt bus_fmt)
 	}
 }
 
-static int sh_vou_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
+static int sh_vou_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 {
 	struct sh_vou_device *vou_dev = video_drvdata(file);
 	int ret;
 
-	dev_dbg(vou_dev->v4l2_dev.dev, "%s(): 0x%llx\n", __func__, *std_id);
+	dev_dbg(vou_dev->v4l2_dev.dev, "%s(): 0x%llx\n", __func__, std_id);
 
-	if (*std_id & ~vou_dev->vdev->tvnorms)
+	if (std_id & ~vou_dev->vdev->tvnorms)
 		return -EINVAL;
 
 	ret = v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0, video,
-					 s_std_output, *std_id);
+					 s_std_output, std_id);
 	/* Shall we continue, if the subdev doesn't support .s_std_output()? */
 	if (ret < 0 && ret != -ENOIOCTLCMD)
 		return ret;
 
-	if (*std_id & V4L2_STD_525_60)
+	if (std_id & V4L2_STD_525_60)
 		sh_vou_reg_ab_set(vou_dev, VOUCR,
 			sh_vou_ntsc_mode(vou_dev->pdata->bus_fmt) << 29, 7 << 29);
 	else
 		sh_vou_reg_ab_set(vou_dev, VOUCR, 5 << 29, 7 << 29);
 
-	vou_dev->std = *std_id;
+	vou_dev->std = std_id;
 
 	return 0;
 }
@@ -1266,7 +1266,7 @@ static int sh_vou_g_register(struct file *file, void *fh,
 }
 
 static int sh_vou_s_register(struct file *file, void *fh,
-				 struct v4l2_dbg_register *reg)
+				 const struct v4l2_dbg_register *reg)
 {
 	struct sh_vou_device *vou_dev = video_drvdata(file);
 
@@ -1485,18 +1485,7 @@ static struct platform_driver __refdata sh_vou = {
 	},
 };
 
-static int __init sh_vou_init(void)
-{
-	return platform_driver_probe(&sh_vou, sh_vou_probe);
-}
-
-static void __exit sh_vou_exit(void)
-{
-	platform_driver_unregister(&sh_vou);
-}
-
-module_init(sh_vou_init);
-module_exit(sh_vou_exit);
+module_platform_driver_probe(sh_vou, sh_vou_probe);
 
 MODULE_DESCRIPTION("SuperH VOU driver");
 MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
diff --git a/drivers/media/platform/soc_camera/atmel-isi.c b/drivers/media/platform/soc_camera/atmel-isi.c
index 82dbf99d347c..1abbb36d0755 100644
--- a/drivers/media/platform/soc_camera/atmel-isi.c
+++ b/drivers/media/platform/soc_camera/atmel-isi.c
@@ -514,6 +514,7 @@ static int isi_camera_init_videobuf(struct vb2_queue *q,
 	q->buf_struct_size = sizeof(struct frame_buffer);
 	q->ops = &isi_video_qops;
 	q->mem_ops = &vb2_dma_contig_memops;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	return vb2_queue_init(q);
 }
@@ -1020,7 +1021,7 @@ static int atmel_isi_probe(struct platform_device *pdev)
 	isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
 
 	irq = platform_get_irq(pdev, 0);
-	if (irq < 0) {
+	if (IS_ERR_VALUE(irq)) {
 		ret = irq;
 		goto err_req_irq;
 	}
@@ -1073,7 +1074,6 @@ err_clk_prepare_pclk:
 }
 
 static struct platform_driver atmel_isi_driver = {
-	.probe		= atmel_isi_probe,
 	.remove		= atmel_isi_remove,
 	.driver		= {
 		.name = "atmel_isi",
@@ -1081,17 +1081,7 @@ static struct platform_driver atmel_isi_driver = {
 	},
 };
 
-static int __init atmel_isi_init_module(void)
-{
-	return  platform_driver_probe(&atmel_isi_driver, &atmel_isi_probe);
-}
-
-static void __exit atmel_isi_exit(void)
-{
-	platform_driver_unregister(&atmel_isi_driver);
-}
-module_init(atmel_isi_init_module);
-module_exit(atmel_isi_exit);
+module_platform_driver_probe(atmel_isi_driver, atmel_isi_probe);
 
 MODULE_AUTHOR("Josh Wu <josh.wu@atmel.com>");
 MODULE_DESCRIPTION("The V4L2 driver for Atmel Linux");
diff --git a/drivers/media/platform/soc_camera/mx1_camera.c b/drivers/media/platform/soc_camera/mx1_camera.c
index 25b2a285dc86..a3fd8d63546c 100644
--- a/drivers/media/platform/soc_camera/mx1_camera.c
+++ b/drivers/media/platform/soc_camera/mx1_camera.c
@@ -776,7 +776,7 @@ static int __init mx1_camera_probe(struct platform_device *pdev)
 	/* request irq */
 	err = claim_fiq(&fh);
 	if (err) {
-		dev_err(&pdev->dev, "Camera interrupt register failed \n");
+		dev_err(&pdev->dev, "Camera interrupt register failed\n");
 		goto exit_free_dma;
 	}
 
@@ -853,24 +853,13 @@ static int __exit mx1_camera_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver mx1_camera_driver = {
-	.driver 	= {
+	.driver		= {
 		.name	= DRIVER_NAME,
 	},
 	.remove		= __exit_p(mx1_camera_remove),
 };
 
-static int __init mx1_camera_init(void)
-{
-	return platform_driver_probe(&mx1_camera_driver, mx1_camera_probe);
-}
-
-static void __exit mx1_camera_exit(void)
-{
-	return platform_driver_unregister(&mx1_camera_driver);
-}
-
-module_init(mx1_camera_init);
-module_exit(mx1_camera_exit);
+module_platform_driver_probe(mx1_camera_driver, mx1_camera_probe);
 
 MODULE_DESCRIPTION("i.MX1/i.MXL SoC Camera Host driver");
 MODULE_AUTHOR("Paulius Zaleckas <paulius.zaleckas@teltonika.lt>");
diff --git a/drivers/media/platform/soc_camera/mx2_camera.c b/drivers/media/platform/soc_camera/mx2_camera.c
index ffba7d91f413..5bbeb43e4531 100644
--- a/drivers/media/platform/soc_camera/mx2_camera.c
+++ b/drivers/media/platform/soc_camera/mx2_camera.c
@@ -797,6 +797,7 @@ static int mx2_camera_init_videobuf(struct vb2_queue *q,
 	q->ops = &mx2_videobuf_ops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct mx2_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	return vb2_queue_init(q);
 }
@@ -1453,7 +1454,7 @@ static int mx27_camera_emma_init(struct platform_device *pdev)
 	err = devm_request_irq(pcdev->dev, irq_emma, mx27_camera_emma_irq, 0,
 			       MX2_CAM_DRV_NAME, pcdev);
 	if (err) {
-		dev_err(pcdev->dev, "Camera EMMA interrupt register failed \n");
+		dev_err(pcdev->dev, "Camera EMMA interrupt register failed\n");
 		goto out;
 	}
 
@@ -1614,15 +1615,14 @@ static int mx2_camera_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver mx2_camera_driver = {
-	.driver 	= {
+	.driver		= {
 		.name	= MX2_CAM_DRV_NAME,
 	},
 	.id_table	= mx2_camera_devtype,
 	.remove		= mx2_camera_remove,
-	.probe		= mx2_camera_probe,
 };
 
-module_platform_driver(mx2_camera_driver);
+module_platform_driver_probe(mx2_camera_driver, mx2_camera_probe);
 
 MODULE_DESCRIPTION("i.MX27 SoC Camera Host driver");
 MODULE_AUTHOR("Sascha Hauer <sha@pengutronix.de>");
diff --git a/drivers/media/platform/soc_camera/mx3_camera.c b/drivers/media/platform/soc_camera/mx3_camera.c
index f5cbb92db545..5da337736cd8 100644
--- a/drivers/media/platform/soc_camera/mx3_camera.c
+++ b/drivers/media/platform/soc_camera/mx3_camera.c
@@ -455,6 +455,7 @@ static int mx3_camera_init_videobuf(struct vb2_queue *q,
 	q->ops = &mx3_videobuf_ops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct mx3_camera_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	return vb2_queue_init(q);
 }
@@ -1275,7 +1276,7 @@ static int mx3_camera_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver mx3_camera_driver = {
-	.driver 	= {
+	.driver		= {
 		.name	= MX3_CAM_DRV_NAME,
 	},
 	.probe		= mx3_camera_probe,
diff --git a/drivers/media/platform/soc_camera/omap1_camera.c b/drivers/media/platform/soc_camera/omap1_camera.c
index 2547bf88f79f..9689a6e89b7f 100644
--- a/drivers/media/platform/soc_camera/omap1_camera.c
+++ b/drivers/media/platform/soc_camera/omap1_camera.c
@@ -1546,7 +1546,7 @@ static struct soc_camera_host_ops omap1_host_ops = {
 	.poll		= omap1_cam_poll,
 };
 
-static int __init omap1_cam_probe(struct platform_device *pdev)
+static int omap1_cam_probe(struct platform_device *pdev)
 {
 	struct omap1_cam_dev *pcdev;
 	struct resource *res;
@@ -1677,7 +1677,7 @@ exit:
 	return err;
 }
 
-static int __exit omap1_cam_remove(struct platform_device *pdev)
+static int omap1_cam_remove(struct platform_device *pdev)
 {
 	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
 	struct omap1_cam_dev *pcdev = container_of(soc_host,
@@ -1709,7 +1709,7 @@ static struct platform_driver omap1_cam_driver = {
 		.name	= DRIVER_NAME,
 	},
 	.probe		= omap1_cam_probe,
-	.remove		= __exit_p(omap1_cam_remove),
+	.remove		= omap1_cam_remove,
 };
 
 module_platform_driver(omap1_cam_driver);
diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
index 395e2e043615..d665242e8207 100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@ -15,6 +15,7 @@
 #include <linux/io.h>
 #include <linux/delay.h>
 #include <linux/dma-mapping.h>
+#include <linux/err.h>
 #include <linux/errno.h>
 #include <linux/fs.h>
 #include <linux/interrupt.h>
@@ -1710,9 +1711,10 @@ static int pxa_camera_probe(struct platform_device *pdev)
 	/*
 	 * Request the regions.
 	 */
-	base = devm_request_and_ioremap(&pdev->dev, res);
-	if (!base)
-		return -ENOMEM;
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
 	pcdev->irq = irq;
 	pcdev->base = base;
 
@@ -1794,13 +1796,13 @@ static int pxa_camera_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct dev_pm_ops pxa_camera_pm = {
+static const struct dev_pm_ops pxa_camera_pm = {
 	.suspend	= pxa_camera_suspend,
 	.resume		= pxa_camera_resume,
 };
 
 static struct platform_driver pxa_camera_driver = {
-	.driver 	= {
+	.driver		= {
 		.name	= PXA_CAM_DRV_NAME,
 		.pm	= &pxa_camera_pm,
 	},
diff --git a/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c b/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
index bb08a46432f4..143d29fe0137 100644
--- a/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
+++ b/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
@@ -20,6 +20,7 @@
 #include <linux/completion.h>
 #include <linux/delay.h>
 #include <linux/dma-mapping.h>
+#include <linux/err.h>
 #include <linux/errno.h>
 #include <linux/fs.h>
 #include <linux/interrupt.h>
@@ -2026,6 +2027,7 @@ static int sh_mobile_ceu_init_videobuf(struct vb2_queue *q,
 	q->ops = &sh_mobile_ceu_videobuf_ops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct sh_mobile_ceu_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	return vb2_queue_init(q);
 }
@@ -2110,11 +2112,9 @@ static int sh_mobile_ceu_probe(struct platform_device *pdev)
 	pcdev->max_width = pcdev->pdata->max_width ? : 2560;
 	pcdev->max_height = pcdev->pdata->max_height ? : 1920;
 
-	base = devm_request_and_ioremap(&pdev->dev, res);
-	if (!base) {
-		dev_err(&pdev->dev, "Unable to ioremap CEU registers.\n");
-		return -ENXIO;
-	}
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
 
 	pcdev->irq = irq;
 	pcdev->base = base;
@@ -2288,7 +2288,7 @@ static const struct dev_pm_ops sh_mobile_ceu_dev_pm_ops = {
 };
 
 static struct platform_driver sh_mobile_ceu_driver = {
-	.driver 	= {
+	.driver		= {
 		.name	= "sh_mobile_ceu",
 		.pm	= &sh_mobile_ceu_dev_pm_ops,
 	},
diff --git a/drivers/media/platform/soc_camera/sh_mobile_csi2.c b/drivers/media/platform/soc_camera/sh_mobile_csi2.c
index 42c559eb4937..09cb4fc88f34 100644
--- a/drivers/media/platform/soc_camera/sh_mobile_csi2.c
+++ b/drivers/media/platform/soc_camera/sh_mobile_csi2.c
@@ -9,6 +9,7 @@
  */
 
 #include <linux/delay.h>
+#include <linux/err.h>
 #include <linux/i2c.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
@@ -324,11 +325,9 @@ static int sh_csi2_probe(struct platform_device *pdev)
 
 	priv->irq = irq;
 
-	priv->base = devm_request_and_ioremap(&pdev->dev, res);
-	if (!priv->base) {
-		dev_err(&pdev->dev, "Unable to ioremap CSI2 registers.\n");
-		return -ENXIO;
-	}
+	priv->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->base))
+		return PTR_ERR(priv->base);
 
 	priv->pdev = pdev;
 	platform_set_drvdata(pdev, priv);
diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c
index 8ec98051ea73..eea832c5fd01 100644
--- a/drivers/media/platform/soc_camera/soc_camera.c
+++ b/drivers/media/platform/soc_camera/soc_camera.c
@@ -256,12 +256,12 @@ static int soc_camera_s_input(struct file *file, void *priv, unsigned int i)
 	return 0;
 }
 
-static int soc_camera_s_std(struct file *file, void *priv, v4l2_std_id *a)
+static int soc_camera_s_std(struct file *file, void *priv, v4l2_std_id a)
 {
 	struct soc_camera_device *icd = file->private_data;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 
-	return v4l2_subdev_call(sd, core, s_std, *a);
+	return v4l2_subdev_call(sd, core, s_std, a);
 }
 
 static int soc_camera_g_std(struct file *file, void *priv, v4l2_std_id *a)
@@ -508,36 +508,49 @@ static int soc_camera_set_fmt(struct soc_camera_device *icd,
 static int soc_camera_open(struct file *file)
 {
 	struct video_device *vdev = video_devdata(file);
-	struct soc_camera_device *icd = dev_get_drvdata(vdev->parent);
-	struct soc_camera_desc *sdesc = to_soc_camera_desc(icd);
+	struct soc_camera_device *icd;
 	struct soc_camera_host *ici;
 	int ret;
 
-	if (!to_soc_camera_control(icd))
-		/* No device driver attached */
-		return -ENODEV;
-
 	/*
 	 * Don't mess with the host during probe: wait until the loop in
-	 * scan_add_host() completes
+	 * scan_add_host() completes. Also protect against a race with
+	 * soc_camera_host_unregister().
 	 */
 	if (mutex_lock_interruptible(&list_lock))
 		return -ERESTARTSYS;
+
+	if (!vdev || !video_is_registered(vdev)) {
+		mutex_unlock(&list_lock);
+		return -ENODEV;
+	}
+
+	icd = dev_get_drvdata(vdev->parent);
 	ici = to_soc_camera_host(icd->parent);
+
+	ret = try_module_get(ici->ops->owner) ? 0 : -ENODEV;
 	mutex_unlock(&list_lock);
 
-	if (mutex_lock_interruptible(&ici->host_lock))
-		return -ERESTARTSYS;
-	if (!try_module_get(ici->ops->owner)) {
+	if (ret < 0) {
 		dev_err(icd->pdev, "Couldn't lock capture bus driver.\n");
-		ret = -EINVAL;
-		goto emodule;
+		return ret;
 	}
 
+	if (!to_soc_camera_control(icd)) {
+		/* No device driver attached */
+		ret = -ENODEV;
+		goto econtrol;
+	}
+
+	if (mutex_lock_interruptible(&ici->host_lock)) {
+		ret = -ERESTARTSYS;
+		goto elockhost;
+	}
 	icd->use_count++;
 
 	/* Now we really have to activate the camera */
 	if (icd->use_count == 1) {
+		struct soc_camera_desc *sdesc = to_soc_camera_desc(icd);
 		/* Restore parameters before the last close() per V4L2 API */
 		struct v4l2_format f = {
 			.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
@@ -609,9 +622,10 @@ epower:
 	ici->ops->remove(icd);
 eiciadd:
 	icd->use_count--;
-	module_put(ici->ops->owner);
-emodule:
 	mutex_unlock(&ici->host_lock);
+elockhost:
+econtrol:
+	module_put(ici->ops->owner);
 
 	return ret;
 }
@@ -1042,7 +1056,7 @@ static int soc_camera_g_register(struct file *file, void *fh,
 }
 
 static int soc_camera_s_register(struct file *file, void *fh,
-				 struct v4l2_dbg_register *reg)
+				 const struct v4l2_dbg_register *reg)
 {
 	struct soc_camera_device *icd = file->private_data;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
diff --git a/drivers/media/platform/soc_camera/soc_camera_platform.c b/drivers/media/platform/soc_camera/soc_camera_platform.c
index ce3b1d6a4734..1b7a88ca195b 100644
--- a/drivers/media/platform/soc_camera/soc_camera_platform.c
+++ b/drivers/media/platform/soc_camera/soc_camera_platform.c
@@ -188,7 +188,7 @@ static int soc_camera_platform_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver soc_camera_platform_driver = {
-	.driver 	= {
+	.driver		= {
 		.name	= "soc_camera_platform",
 		.owner	= THIS_MODULE,
 	},
diff --git a/drivers/media/platform/soc_camera/soc_mediabus.c b/drivers/media/platform/soc_camera/soc_mediabus.c
index 89dce097a827..dc02deca7563 100644
--- a/drivers/media/platform/soc_camera/soc_mediabus.c
+++ b/drivers/media/platform/soc_camera/soc_mediabus.c
@@ -73,7 +73,7 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.name			= "RGB555X",
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
-		.order			= SOC_MBUS_ORDER_LE,
+		.order			= SOC_MBUS_ORDER_BE,
 		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
@@ -93,10 +93,46 @@ static const struct soc_mbus_lookup mbus_fmt[] = {
 		.name			= "RGB565X",
 		.bits_per_sample	= 8,
 		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
-		.order			= SOC_MBUS_ORDER_LE,
+		.order			= SOC_MBUS_ORDER_BE,
 		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
 }, {
+	.code = V4L2_MBUS_FMT_RGB666_1X18,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB32,
+		.name			= "RGB666/32bpp",
+		.bits_per_sample	= 18,
+		.packing		= SOC_MBUS_PACKING_EXTEND32,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+}, {
+	.code = V4L2_MBUS_FMT_RGB888_1X24,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB32,
+		.name			= "RGB888/32bpp",
+		.bits_per_sample	= 24,
+		.packing		= SOC_MBUS_PACKING_EXTEND32,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+}, {
+	.code = V4L2_MBUS_FMT_RGB888_2X12_BE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB32,
+		.name			= "RGB888/32bpp",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_EXTEND32,
+		.order			= SOC_MBUS_ORDER_BE,
+	},
+}, {
+	.code = V4L2_MBUS_FMT_RGB888_2X12_LE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB32,
+		.name			= "RGB888/32bpp",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_EXTEND32,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+}, {
 	.code = V4L2_MBUS_FMT_SBGGR8_1X8,
 	.fmt = {
 		.fourcc			= V4L2_PIX_FMT_SBGGR8,
@@ -358,6 +394,10 @@ int soc_mbus_samples_per_pixel(const struct soc_mbus_pixelfmt *mf,
 		*numerator = 1;
 		*denominator = 1;
 		return 0;
+	case SOC_MBUS_PACKING_EXTEND32:
+		*numerator = 1;
+		*denominator = 1;
+		return 0;
 	case SOC_MBUS_PACKING_2X8_PADHI:
 	case SOC_MBUS_PACKING_2X8_PADLO:
 		*numerator = 2;
@@ -392,6 +432,8 @@ s32 soc_mbus_bytes_per_line(u32 width, const struct soc_mbus_pixelfmt *mf)
 		return width * 3 / 2;
 	case SOC_MBUS_PACKING_VARIABLE:
 		return 0;
+	case SOC_MBUS_PACKING_EXTEND32:
+		return width * 4;
 	}
 	return -EINVAL;
 }
diff --git a/drivers/media/platform/timblogiw.c b/drivers/media/platform/timblogiw.c
index c3a2a4484401..a2f7bdd5104f 100644
--- a/drivers/media/platform/timblogiw.c
+++ b/drivers/media/platform/timblogiw.c
@@ -78,7 +78,7 @@ struct timblogiw_buffer {
 	struct timblogiw_fh	*fh;
 };
 
-const struct timblogiw_tvnorm timblogiw_tvnorms[] = {
+static const struct timblogiw_tvnorm timblogiw_tvnorms[] = {
 	{
 		.std			= V4L2_STD_PAL,
 		.width			= 720,
@@ -336,7 +336,7 @@ static int timblogiw_g_std(struct file *file, void  *priv, v4l2_std_id *std)
 	return 0;
 }
 
-static int timblogiw_s_std(struct file *file, void  *priv, v4l2_std_id *std)
+static int timblogiw_s_std(struct file *file, void  *priv, v4l2_std_id std)
 {
 	struct video_device *vdev = video_devdata(file);
 	struct timblogiw *lw = video_get_drvdata(vdev);
@@ -348,10 +348,10 @@ static int timblogiw_s_std(struct file *file, void  *priv, v4l2_std_id *std)
 	mutex_lock(&lw->lock);
 
 	if (TIMBLOGIW_HAS_DECODER(lw))
-		err = v4l2_subdev_call(lw->sd_enc, core, s_std, *std);
+		err = v4l2_subdev_call(lw->sd_enc, core, s_std, std);
 
 	if (!err)
-		fh->cur_norm = timblogiw_get_norm(*std);
+		fh->cur_norm = timblogiw_get_norm(std);
 
 	mutex_unlock(&lw->lock);
 
diff --git a/drivers/media/platform/via-camera.c b/drivers/media/platform/via-camera.c
index b051c4a28554..a794cd6c4441 100644
--- a/drivers/media/platform/via-camera.c
+++ b/drivers/media/platform/via-camera.c
@@ -847,7 +847,7 @@ static int viacam_s_input(struct file *filp, void *priv, unsigned int i)
 	return 0;
 }
 
-static int viacam_s_std(struct file *filp, void *priv, v4l2_std_id *std)
+static int viacam_s_std(struct file *filp, void *priv, v4l2_std_id std)
 {
 	return 0;
 }
diff --git a/drivers/media/platform/vino.c b/drivers/media/platform/vino.c
index eb5d6f955709..c6af974c5b45 100644
--- a/drivers/media/platform/vino.c
+++ b/drivers/media/platform/vino.c
@@ -3042,7 +3042,7 @@ static int vino_g_std(struct file *file, void *__fh,
 }
 
 static int vino_s_std(struct file *file, void *__fh,
-			   v4l2_std_id *std)
+			   v4l2_std_id std)
 {
 	struct vino_channel_settings *vcs = video_drvdata(file);
 	unsigned long flags;
@@ -3056,7 +3056,7 @@ static int vino_s_std(struct file *file, void *__fh,
 	}
 
 	/* check if the standard is valid for the current input */
-	if ((*std) & vino_inputs[vcs->input].std) {
+	if (std & vino_inputs[vcs->input].std) {
 		dprintk("standard accepted\n");
 
 		/* change the video norm for SAA7191
@@ -3065,13 +3065,13 @@ static int vino_s_std(struct file *file, void *__fh,
 		if (vcs->input == VINO_INPUT_D1)
 			goto out;
 
-		if ((*std) & V4L2_STD_PAL) {
+		if (std & V4L2_STD_PAL) {
 			ret = vino_set_data_norm(vcs, VINO_DATA_NORM_PAL,
 						 &flags);
-		} else if ((*std) & V4L2_STD_NTSC) {
+		} else if (std & V4L2_STD_NTSC) {
 			ret = vino_set_data_norm(vcs, VINO_DATA_NORM_NTSC,
 						 &flags);
-		} else if ((*std) & V4L2_STD_SECAM) {
+		} else if (std & V4L2_STD_SECAM) {
 			ret = vino_set_data_norm(vcs, VINO_DATA_NORM_SECAM,
 						 &flags);
 		} else {
diff --git a/drivers/media/platform/vivi.c b/drivers/media/platform/vivi.c
index 8a33a712f480..85bc314382d3 100644
--- a/drivers/media/platform/vivi.c
+++ b/drivers/media/platform/vivi.c
@@ -1093,6 +1093,15 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 		return 0;
 
 	dev->input = i;
+	/*
+	 * Modify the brightness range depending on the input.
+	 * This makes it easy to use vivi to test if applications can
+	 * handle control range modifications and is also how this is
+	 * typically used in practice as different inputs may be hooked
+	 * up to different receivers with different control ranges.
+	 */
+	v4l2_ctrl_modify_range(dev->brightness,
+			128 * i, 255 + 128 * i, 1, 127 + 128 * i);
 	precalculate_bars(dev);
 	precalculate_line(dev);
 	return 0;
@@ -1429,6 +1438,7 @@ static int __init vivi_create_instance(int inst)
 	q->buf_struct_size = sizeof(struct vivi_buffer);
 	q->ops = &vivi_video_qops;
 	q->mem_ops = &vb2_vmalloc_memops;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	ret = vb2_queue_init(q);
 	if (ret)
diff --git a/drivers/media/radio/Kconfig b/drivers/media/radio/Kconfig
index 24e64a09884c..c0beee2fa37c 100644
--- a/drivers/media/radio/Kconfig
+++ b/drivers/media/radio/Kconfig
@@ -18,6 +18,22 @@ config RADIO_SI470X
 
 source "drivers/media/radio/si470x/Kconfig"
 
+config RADIO_SI476X
+	tristate "Silicon Laboratories Si476x I2C FM Radio"
+	depends on I2C && VIDEO_V4L2
+	depends on MFD_SI476X_CORE
+	select SND_SOC_SI476X
+	---help---
+	  Choose Y here if you have this FM radio chip.
+
+	  In order to control your radio card, you will need to use programs
+	  that are compatible with the Video For Linux 2 API.  Information on
+	  this API and pointers to "v4l2" programs may be found at
+	  <file:Documentation/video4linux/API.html>.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called radio-si476x.
+
 config USB_MR800
 	tristate "AverMedia MR 800 USB FM radio support"
 	depends on USB && VIDEO_V4L2
diff --git a/drivers/media/radio/Makefile b/drivers/media/radio/Makefile
index 303eaebdb85a..0dcdb320cfc7 100644
--- a/drivers/media/radio/Makefile
+++ b/drivers/media/radio/Makefile
@@ -19,6 +19,7 @@ obj-$(CONFIG_RADIO_GEMTEK) += radio-gemtek.o
 obj-$(CONFIG_RADIO_TRUST) += radio-trust.o
 obj-$(CONFIG_I2C_SI4713) += si4713-i2c.o
 obj-$(CONFIG_RADIO_SI4713) += radio-si4713.o
+obj-$(CONFIG_RADIO_SI476X) += radio-si476x.o
 obj-$(CONFIG_RADIO_MIROPCM20) += radio-miropcm20.o
 obj-$(CONFIG_USB_DSBR) += dsbr100.o
 obj-$(CONFIG_RADIO_SI470X) += si470x/
diff --git a/drivers/media/radio/dsbr100.c b/drivers/media/radio/dsbr100.c
index 63b112b555b2..142c2ee64d31 100644
--- a/drivers/media/radio/dsbr100.c
+++ b/drivers/media/radio/dsbr100.c
@@ -208,13 +208,13 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *v)
+				const struct v4l2_tuner *v)
 {
 	return v->index ? -EINVAL : 0;
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct dsbr100_device *radio = video_drvdata(file);
 
diff --git a/drivers/media/radio/radio-cadet.c b/drivers/media/radio/radio-cadet.c
index 643d80ac28fb..545c04cf7226 100644
--- a/drivers/media/radio/radio-cadet.c
+++ b/drivers/media/radio/radio-cadet.c
@@ -90,6 +90,26 @@ static u16 sigtable[2][4] = {
 	{ 2185, 4369, 13107, 65535 },
 };
 
+static const struct v4l2_frequency_band bands[] = {
+	{
+		.index = 0,
+		.type = V4L2_TUNER_RADIO,
+		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow = 8320,      /* 520 kHz */
+		.rangehigh = 26400,    /* 1650 kHz */
+		.modulation = V4L2_BAND_MODULATION_AM,
+	}, {
+		.index = 1,
+		.type = V4L2_TUNER_RADIO,
+		.capability = V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_RDS |
+			V4L2_TUNER_CAP_RDS_BLOCK_IO | V4L2_TUNER_CAP_LOW |
+			V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow = 1400000,   /* 87.5 MHz */
+		.rangehigh = 1728000,  /* 108.0 MHz */
+		.modulation = V4L2_BAND_MODULATION_FM,
+	},
+};
+
 
 static int cadet_getstereo(struct cadet *dev)
 {
@@ -196,6 +216,8 @@ static void cadet_setfreq(struct cadet *dev, unsigned freq)
 	int i, j, test;
 	int curvol;
 
+	freq = clamp(freq, bands[dev->is_fm_band].rangelow,
+			   bands[dev->is_fm_band].rangehigh);
 	dev->curfreq = freq;
 	/*
 	 * Formulate a fifo command
@@ -337,26 +359,6 @@ static int vidioc_querycap(struct file *file, void *priv,
 	return 0;
 }
 
-static const struct v4l2_frequency_band bands[] = {
-	{
-		.index = 0,
-		.type = V4L2_TUNER_RADIO,
-		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_FREQ_BANDS,
-		.rangelow = 8320,      /* 520 kHz */
-		.rangehigh = 26400,    /* 1650 kHz */
-		.modulation = V4L2_BAND_MODULATION_AM,
-	}, {
-		.index = 1,
-		.type = V4L2_TUNER_RADIO,
-		.capability = V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_RDS |
-			V4L2_TUNER_CAP_RDS_BLOCK_IO | V4L2_TUNER_CAP_LOW |
-			V4L2_TUNER_CAP_FREQ_BANDS,
-		.rangelow = 1400000,   /* 87.5 MHz */
-		.rangehigh = 1728000,  /* 108.0 MHz */
-		.modulation = V4L2_BAND_MODULATION_FM,
-	},
-};
-
 static int vidioc_g_tuner(struct file *file, void *priv,
 				struct v4l2_tuner *v)
 {
@@ -388,7 +390,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *v)
+				const struct v4l2_tuner *v)
 {
 	return v->index ? -EINVAL : 0;
 }
@@ -418,7 +420,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct cadet *dev = video_drvdata(file);
 
@@ -426,8 +428,6 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 		return -EINVAL;
 	dev->is_fm_band =
 		f->frequency >= (bands[0].rangehigh + bands[1].rangelow) / 2;
-	clamp(f->frequency, bands[dev->is_fm_band].rangelow,
-			    bands[dev->is_fm_band].rangehigh);
 	cadet_setfreq(dev, f->frequency);
 	return 0;
 }
diff --git a/drivers/media/radio/radio-isa.c b/drivers/media/radio/radio-isa.c
index 84b7b9f4385e..6ff350831d56 100644
--- a/drivers/media/radio/radio-isa.c
+++ b/drivers/media/radio/radio-isa.c
@@ -51,8 +51,8 @@ static int radio_isa_querycap(struct file *file, void  *priv,
 	strlcpy(v->card, isa->drv->card, sizeof(v->card));
 	snprintf(v->bus_info, sizeof(v->bus_info), "ISA:%s", isa->v4l2_dev.name);
 
-	v->capabilities = V4L2_CAP_TUNER | V4L2_CAP_RADIO;
-	v->device_caps = v->capabilities | V4L2_CAP_DEVICE_CAPS;
+	v->device_caps = V4L2_CAP_TUNER | V4L2_CAP_RADIO;
+	v->capabilities = v->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -87,7 +87,7 @@ static int radio_isa_g_tuner(struct file *file, void *priv,
 }
 
 static int radio_isa_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *v)
+				const struct v4l2_tuner *v)
 {
 	struct radio_isa_card *isa = video_drvdata(file);
 	const struct radio_isa_ops *ops = isa->drv->ops;
@@ -102,17 +102,18 @@ static int radio_isa_s_tuner(struct file *file, void *priv,
 }
 
 static int radio_isa_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct radio_isa_card *isa = video_drvdata(file);
+	u32 freq = f->frequency;
 	int res;
 
 	if (f->tuner != 0 || f->type != V4L2_TUNER_RADIO)
 		return -EINVAL;
-	f->frequency = clamp(f->frequency, FREQ_LOW, FREQ_HIGH);
-	res = isa->drv->ops->s_frequency(isa, f->frequency);
+	freq = clamp(freq, FREQ_LOW, FREQ_HIGH);
+	res = isa->drv->ops->s_frequency(isa, freq);
 	if (res == 0)
-		isa->freq = f->frequency;
+		isa->freq = freq;
 	return res;
 }
 
diff --git a/drivers/media/radio/radio-keene.c b/drivers/media/radio/radio-keene.c
index 296941a9ae25..4c9ae767fb31 100644
--- a/drivers/media/radio/radio-keene.c
+++ b/drivers/media/radio/radio-keene.c
@@ -215,15 +215,15 @@ static int vidioc_s_modulator(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct keene_device *radio = video_drvdata(file);
+	unsigned freq = f->frequency;
 
 	if (f->tuner != 0 || f->type != V4L2_TUNER_RADIO)
 		return -EINVAL;
-	f->frequency = clamp(f->frequency,
-			FREQ_MIN * FREQ_MUL, FREQ_MAX * FREQ_MUL);
-	return keene_cmd_main(radio, f->frequency, true);
+	freq = clamp(freq, FREQ_MIN * FREQ_MUL, FREQ_MAX * FREQ_MUL);
+	return keene_cmd_main(radio, freq, true);
 }
 
 static int vidioc_g_frequency(struct file *file, void *priv,
diff --git a/drivers/media/radio/radio-ma901.c b/drivers/media/radio/radio-ma901.c
index 348dafc0318a..a85b064cb7be 100644
--- a/drivers/media/radio/radio-ma901.c
+++ b/drivers/media/radio/radio-ma901.c
@@ -239,7 +239,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 
 /* vidioc_s_tuner - set tuner attributes */
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *v)
+				const struct v4l2_tuner *v)
 {
 	struct ma901radio_device *radio = video_drvdata(file);
 
@@ -257,7 +257,7 @@ static int vidioc_s_tuner(struct file *file, void *priv,
 
 /* vidioc_s_frequency - set tuner radio frequency */
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct ma901radio_device *radio = video_drvdata(file);
 
diff --git a/drivers/media/radio/radio-miropcm20.c b/drivers/media/radio/radio-miropcm20.c
index 3d0ff4404d12..a7e93d7477dd 100644
--- a/drivers/media/radio/radio-miropcm20.c
+++ b/drivers/media/radio/radio-miropcm20.c
@@ -103,16 +103,18 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *v)
+				const struct v4l2_tuner *v)
 {
 	struct pcm20 *dev = video_drvdata(file);
 
 	if (v->index)
 		return -EINVAL;
 	if (v->audmode > V4L2_TUNER_MODE_STEREO)
-		v->audmode = V4L2_TUNER_MODE_STEREO;
+		dev->audmode = V4L2_TUNER_MODE_STEREO;
+	else
+		dev->audmode = v->audmode;
 	snd_aci_cmd(dev->aci, ACI_SET_TUNERMONO,
-			v->audmode == V4L2_TUNER_MODE_MONO, -1);
+			dev->audmode == V4L2_TUNER_MODE_MONO, -1);
 	return 0;
 }
 
@@ -131,14 +133,14 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct pcm20 *dev = video_drvdata(file);
 
 	if (f->tuner != 0 || f->type != V4L2_TUNER_RADIO)
 		return -EINVAL;
 
-	dev->freq = clamp(f->frequency, 87 * 16000U, 108 * 16000U);
+	dev->freq = clamp_t(u32, f->frequency, 87 * 16000U, 108 * 16000U);
 	pcm20_setfreq(dev, dev->freq);
 	return 0;
 }
diff --git a/drivers/media/radio/radio-mr800.c b/drivers/media/radio/radio-mr800.c
index 9c5a267b60b4..a360227ca3ab 100644
--- a/drivers/media/radio/radio-mr800.c
+++ b/drivers/media/radio/radio-mr800.c
@@ -203,10 +203,14 @@ static int amradio_set_mute(struct amradio_device *radio, bool mute)
 /* set a frequency, freq is defined by v4l's TUNER_LOW, i.e. 1/16th kHz */
 static int amradio_set_freq(struct amradio_device *radio, int freq)
 {
-	unsigned short freq_send = 0x10 + (freq >> 3) / 25;
+	unsigned short freq_send;
 	u8 buf[3];
 	int retval;
 
+	/* we need to be sure that frequency isn't out of range */
+	freq = clamp_t(unsigned, freq, FREQ_MIN * FREQ_MUL, FREQ_MAX * FREQ_MUL);
+	freq_send = 0x10 + (freq >> 3) / 25;
+
 	/* frequency is calculated from freq_send and placed in first 2 bytes */
 	buf[0] = (freq_send >> 8) & 0xff;
 	buf[1] = freq_send & 0xff;
@@ -305,7 +309,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 
 /* vidioc_s_tuner - set tuner attributes */
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *v)
+				const struct v4l2_tuner *v)
 {
 	struct amradio_device *radio = video_drvdata(file);
 
@@ -323,14 +327,13 @@ static int vidioc_s_tuner(struct file *file, void *priv,
 
 /* vidioc_s_frequency - set tuner radio frequency */
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct amradio_device *radio = video_drvdata(file);
 
 	if (f->tuner != 0)
 		return -EINVAL;
-	return amradio_set_freq(radio, clamp_t(unsigned, f->frequency,
-				FREQ_MIN * FREQ_MUL, FREQ_MAX * FREQ_MUL));
+	return amradio_set_freq(radio, f->frequency);
 }
 
 /* vidioc_g_frequency - get tuner radio frequency */
@@ -389,6 +392,7 @@ static int vidioc_s_hw_freq_seek(struct file *file, void *priv,
 			continue;
 		amradio_send_cmd(radio, AMRADIO_GET_FREQ, 0, NULL, 0, true);
 		if (radio->buffer[1] || radio->buffer[2]) {
+			/* To check: sometimes radio->curfreq is set to out of range value */
 			radio->curfreq = (radio->buffer[1] << 8) | radio->buffer[2];
 			radio->curfreq = (radio->curfreq - 0x10) * 200;
 			amradio_send_cmd(radio, AMRADIO_STOP_SEARCH,
diff --git a/drivers/media/radio/radio-rtrack2.c b/drivers/media/radio/radio-rtrack2.c
index b1f844c64fde..09cfbc373c92 100644
--- a/drivers/media/radio/radio-rtrack2.c
+++ b/drivers/media/radio/radio-rtrack2.c
@@ -8,6 +8,8 @@
  *
  * Converted to the radio-isa framework by Hans Verkuil <hans.verkuil@cisco.com>
  * Converted to V4L2 API by Mauro Carvalho Chehab <mchehab@infradead.org>
+ *
+ * Fully tested with actual hardware and the v4l2-compliance tool.
  */
 
 #include <linux/module.h>	/* Modules 			*/
@@ -81,8 +83,7 @@ static int rtrack2_s_frequency(struct radio_isa_card *isa, u32 freq)
 			zero(isa);
 
 	outb_p(0xc8, isa->io);
-	if (!v4l2_ctrl_g_ctrl(isa->mute))
-		outb_p(0, isa->io);
+	outb_p(v4l2_ctrl_g_ctrl(isa->mute), isa->io);
 	return 0;
 }
 
diff --git a/drivers/media/radio/radio-sf16fmi.c b/drivers/media/radio/radio-sf16fmi.c
index 637a55564958..adfcc61bdf0b 100644
--- a/drivers/media/radio/radio-sf16fmi.c
+++ b/drivers/media/radio/radio-sf16fmi.c
@@ -145,13 +145,13 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-					struct v4l2_tuner *v)
+					const struct v4l2_tuner *v)
 {
 	return v->index ? -EINVAL : 0;
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-					struct v4l2_frequency *f)
+					const struct v4l2_frequency *f)
 {
 	struct fmi *fmi = video_drvdata(file);
 
diff --git a/drivers/media/radio/radio-si4713.c b/drivers/media/radio/radio-si4713.c
index 1507c9d508d7..ba4cfc946868 100644
--- a/drivers/media/radio/radio-si4713.c
+++ b/drivers/media/radio/radio-si4713.c
@@ -31,6 +31,9 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 #include <media/radio-si4713.h>
 
 /* module parameters */
@@ -39,54 +42,30 @@ module_param(radio_nr, int, 0);
 MODULE_PARM_DESC(radio_nr,
 		 "Minor number for radio device (-1 ==> auto assign)");
 
-MODULE_LICENSE("GPL");
+MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Eduardo Valentin <eduardo.valentin@nokia.com>");
 MODULE_DESCRIPTION("Platform driver for Si4713 FM Radio Transmitter");
 MODULE_VERSION("0.0.1");
+MODULE_ALIAS("platform:radio-si4713");
 
 /* Driver state struct */
 struct radio_si4713_device {
 	struct v4l2_device		v4l2_dev;
-	struct video_device		*radio_dev;
+	struct video_device		radio_dev;
+	struct mutex lock;
 };
 
 /* radio_si4713_fops - file operations interface */
 static const struct v4l2_file_operations radio_si4713_fops = {
 	.owner		= THIS_MODULE,
+	.open = v4l2_fh_open,
+	.release = v4l2_fh_release,
+	.poll = v4l2_ctrl_poll,
 	/* Note: locking is done at the subdev level in the i2c driver. */
 	.unlocked_ioctl	= video_ioctl2,
 };
 
 /* Video4Linux Interface */
-static int radio_si4713_fill_audout(struct v4l2_audioout *vao)
-{
-	/* TODO: check presence of audio output */
-	strlcpy(vao->name, "FM Modulator Audio Out", 32);
-
-	return 0;
-}
-
-static int radio_si4713_enumaudout(struct file *file, void *priv,
-						struct v4l2_audioout *vao)
-{
-	return radio_si4713_fill_audout(vao);
-}
-
-static int radio_si4713_g_audout(struct file *file, void *priv,
-					struct v4l2_audioout *vao)
-{
-	int rval = radio_si4713_fill_audout(vao);
-
-	vao->index = 0;
-
-	return rval;
-}
-
-static int radio_si4713_s_audout(struct file *file, void *priv,
-					const struct v4l2_audioout *vao)
-{
-	return vao->index ? -EINVAL : 0;
-}
 
 /* radio_si4713_querycap - query device capabilities */
 static int radio_si4713_querycap(struct file *file, void *priv,
@@ -94,67 +73,15 @@ static int radio_si4713_querycap(struct file *file, void *priv,
 {
 	strlcpy(capability->driver, "radio-si4713", sizeof(capability->driver));
 	strlcpy(capability->card, "Silicon Labs Si4713 Modulator",
-				sizeof(capability->card));
-	capability->capabilities = V4L2_CAP_MODULATOR | V4L2_CAP_RDS_OUTPUT;
+		sizeof(capability->card));
+	strlcpy(capability->bus_info, "platform:radio-si4713",
+		sizeof(capability->bus_info));
+	capability->device_caps = V4L2_CAP_MODULATOR | V4L2_CAP_RDS_OUTPUT;
+	capability->capabilities = capability->device_caps | V4L2_CAP_DEVICE_CAPS;
 
 	return 0;
 }
 
-/* radio_si4713_queryctrl - enumerate control items */
-static int radio_si4713_queryctrl(struct file *file, void *priv,
-						struct v4l2_queryctrl *qc)
-{
-	/* Must be sorted from low to high control ID! */
-	static const u32 user_ctrls[] = {
-		V4L2_CID_USER_CLASS,
-		V4L2_CID_AUDIO_MUTE,
-		0
-	};
-
-	/* Must be sorted from low to high control ID! */
-	static const u32 fmtx_ctrls[] = {
-		V4L2_CID_FM_TX_CLASS,
-		V4L2_CID_RDS_TX_DEVIATION,
-		V4L2_CID_RDS_TX_PI,
-		V4L2_CID_RDS_TX_PTY,
-		V4L2_CID_RDS_TX_PS_NAME,
-		V4L2_CID_RDS_TX_RADIO_TEXT,
-		V4L2_CID_AUDIO_LIMITER_ENABLED,
-		V4L2_CID_AUDIO_LIMITER_RELEASE_TIME,
-		V4L2_CID_AUDIO_LIMITER_DEVIATION,
-		V4L2_CID_AUDIO_COMPRESSION_ENABLED,
-		V4L2_CID_AUDIO_COMPRESSION_GAIN,
-		V4L2_CID_AUDIO_COMPRESSION_THRESHOLD,
-		V4L2_CID_AUDIO_COMPRESSION_ATTACK_TIME,
-		V4L2_CID_AUDIO_COMPRESSION_RELEASE_TIME,
-		V4L2_CID_PILOT_TONE_ENABLED,
-		V4L2_CID_PILOT_TONE_DEVIATION,
-		V4L2_CID_PILOT_TONE_FREQUENCY,
-		V4L2_CID_TUNE_PREEMPHASIS,
-		V4L2_CID_TUNE_POWER_LEVEL,
-		V4L2_CID_TUNE_ANTENNA_CAPACITOR,
-		0
-	};
-	static const u32 *ctrl_classes[] = {
-		user_ctrls,
-		fmtx_ctrls,
-		NULL
-	};
-	struct radio_si4713_device *rsdev;
-
-	rsdev = video_get_drvdata(video_devdata(file));
-
-	qc->id = v4l2_ctrl_next(ctrl_classes, qc->id);
-	if (qc->id == 0)
-		return -EINVAL;
-
-	if (qc->id == V4L2_CID_USER_CLASS || qc->id == V4L2_CID_FM_TX_CLASS)
-		return v4l2_ctrl_query_fill(qc, 0, 0, 0, 0);
-
-	return v4l2_device_call_until_err(&rsdev->v4l2_dev, 0, core,
-						queryctrl, qc);
-}
-
 /*
  * v4l2 ioctl call backs.
  * we are just a wrapper for v4l2_sub_devs.
@@ -164,83 +91,50 @@ static inline struct v4l2_device *get_v4l2_dev(struct file *file)
 	return &((struct radio_si4713_device *)video_drvdata(file))->v4l2_dev;
 }
 
-static int radio_si4713_g_ext_ctrls(struct file *file, void *p,
-						struct v4l2_ext_controls *vecs)
-{
-	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, core,
-							g_ext_ctrls, vecs);
-}
-
-static int radio_si4713_s_ext_ctrls(struct file *file, void *p,
-						struct v4l2_ext_controls *vecs)
-{
-	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, core,
-							s_ext_ctrls, vecs);
-}
-
-static int radio_si4713_g_ctrl(struct file *file, void *p,
-						struct v4l2_control *vc)
-{
-	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, core,
-							g_ctrl, vc);
-}
-
-static int radio_si4713_s_ctrl(struct file *file, void *p,
-						struct v4l2_control *vc)
-{
-	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, core,
-							s_ctrl, vc);
-}
-
 static int radio_si4713_g_modulator(struct file *file, void *p,
-						struct v4l2_modulator *vm)
+				    struct v4l2_modulator *vm)
 {
 	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, tuner,
-							g_modulator, vm);
+					  g_modulator, vm);
 }
 
 static int radio_si4713_s_modulator(struct file *file, void *p,
-						const struct v4l2_modulator *vm)
+				    const struct v4l2_modulator *vm)
 {
 	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, tuner,
-							s_modulator, vm);
+					  s_modulator, vm);
 }
 
 static int radio_si4713_g_frequency(struct file *file, void *p,
-						struct v4l2_frequency *vf)
+				    struct v4l2_frequency *vf)
 {
 	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, tuner,
-							g_frequency, vf);
+					  g_frequency, vf);
 }
 
 static int radio_si4713_s_frequency(struct file *file, void *p,
-						struct v4l2_frequency *vf)
+				    const struct v4l2_frequency *vf)
 {
 	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, tuner,
-							s_frequency, vf);
+					  s_frequency, vf);
 }
 
 static long radio_si4713_default(struct file *file, void *p,
-				bool valid_prio, int cmd, void *arg)
+				 bool valid_prio, unsigned int cmd, void *arg)
 {
 	return v4l2_device_call_until_err(get_v4l2_dev(file), 0, core,
-							ioctl, cmd, arg);
+					  ioctl, cmd, arg);
 }
 
 static struct v4l2_ioctl_ops radio_si4713_ioctl_ops = {
-	.vidioc_enumaudout	= radio_si4713_enumaudout,
-	.vidioc_g_audout	= radio_si4713_g_audout,
-	.vidioc_s_audout	= radio_si4713_s_audout,
 	.vidioc_querycap	= radio_si4713_querycap,
-	.vidioc_queryctrl	= radio_si4713_queryctrl,
-	.vidioc_g_ext_ctrls	= radio_si4713_g_ext_ctrls,
-	.vidioc_s_ext_ctrls	= radio_si4713_s_ext_ctrls,
-	.vidioc_g_ctrl		= radio_si4713_g_ctrl,
-	.vidioc_s_ctrl		= radio_si4713_s_ctrl,
 	.vidioc_g_modulator	= radio_si4713_g_modulator,
 	.vidioc_s_modulator	= radio_si4713_s_modulator,
 	.vidioc_g_frequency	= radio_si4713_g_frequency,
 	.vidioc_s_frequency	= radio_si4713_s_frequency,
+	.vidioc_log_status      = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 	.vidioc_default		= radio_si4713_default,
 };
 
@@ -248,7 +142,7 @@ static struct v4l2_ioctl_ops radio_si4713_ioctl_ops = {
 static struct video_device radio_si4713_vdev_template = {
 	.fops			= &radio_si4713_fops,
 	.name			= "radio-si4713",
-	.release		= video_device_release,
+	.release		= video_device_release_empty,
 	.ioctl_ops		= &radio_si4713_ioctl_ops,
 	.vfl_dir		= VFL_DIR_TX,
 };
@@ -275,6 +169,7 @@ static int radio_si4713_pdriver_probe(struct platform_device *pdev)
 		rval = -ENOMEM;
 		goto exit;
 	}
+	mutex_init(&rsdev->lock);
 
 	rval = v4l2_device_register(&pdev->dev, &rsdev->v4l2_dev);
 	if (rval) {
@@ -285,40 +180,35 @@ static int radio_si4713_pdriver_probe(struct platform_device *pdev)
 	adapter = i2c_get_adapter(pdata->i2c_bus);
 	if (!adapter) {
 		dev_err(&pdev->dev, "Cannot get i2c adapter %d\n",
-							pdata->i2c_bus);
+			pdata->i2c_bus);
 		rval = -ENODEV;
 		goto unregister_v4l2_dev;
 	}
 
 	sd = v4l2_i2c_new_subdev_board(&rsdev->v4l2_dev, adapter,
-					pdata->subdev_board_info, NULL);
+				       pdata->subdev_board_info, NULL);
 	if (!sd) {
 		dev_err(&pdev->dev, "Cannot get v4l2 subdevice\n");
 		rval = -ENODEV;
 		goto put_adapter;
 	}
 
-	rsdev->radio_dev = video_device_alloc();
-	if (!rsdev->radio_dev) {
-		dev_err(&pdev->dev, "Failed to alloc video device.\n");
-		rval = -ENOMEM;
-		goto put_adapter;
-	}
-
-	memcpy(rsdev->radio_dev, &radio_si4713_vdev_template,
-			sizeof(radio_si4713_vdev_template));
-	video_set_drvdata(rsdev->radio_dev, rsdev);
-	if (video_register_device(rsdev->radio_dev, VFL_TYPE_RADIO, radio_nr)) {
+	rsdev->radio_dev = radio_si4713_vdev_template;
+	rsdev->radio_dev.v4l2_dev = &rsdev->v4l2_dev;
+	rsdev->radio_dev.ctrl_handler = sd->ctrl_handler;
+	set_bit(V4L2_FL_USE_FH_PRIO, &rsdev->radio_dev.flags);
+	/* Serialize all access to the si4713 */
+	rsdev->radio_dev.lock = &rsdev->lock;
+	video_set_drvdata(&rsdev->radio_dev, rsdev);
+	if (video_register_device(&rsdev->radio_dev, VFL_TYPE_RADIO, radio_nr)) {
 		dev_err(&pdev->dev, "Could not register video device.\n");
 		rval = -EIO;
-		goto free_vdev;
+		goto put_adapter;
 	}
 	dev_info(&pdev->dev, "New device successfully probed\n");
 
 	goto exit;
 
-free_vdev:
-	video_device_release(rsdev->radio_dev);
 put_adapter:
 	i2c_put_adapter(adapter);
 unregister_v4l2_dev:
@@ -328,17 +218,16 @@ exit:
 }
 
 /* radio_si4713_pdriver_remove - remove the device */
-static int __exit radio_si4713_pdriver_remove(struct platform_device *pdev)
+static int radio_si4713_pdriver_remove(struct platform_device *pdev)
 {
 	struct v4l2_device *v4l2_dev = platform_get_drvdata(pdev);
-	struct radio_si4713_device *rsdev = container_of(v4l2_dev,
-						struct radio_si4713_device,
-						v4l2_dev);
 	struct v4l2_subdev *sd = list_entry(v4l2_dev->subdevs.next,
 					    struct v4l2_subdev, list);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct radio_si4713_device *rsdev;
 
-	video_unregister_device(rsdev->radio_dev);
+	rsdev = container_of(v4l2_dev, struct radio_si4713_device, v4l2_dev);
+	video_unregister_device(&rsdev->radio_dev);
 	i2c_put_adapter(client->adapter);
 	v4l2_device_unregister(&rsdev->v4l2_dev);
 
@@ -348,9 +237,10 @@ static int __exit radio_si4713_pdriver_remove(struct platform_device *pdev)
 static struct platform_driver radio_si4713_pdriver = {
 	.driver		= {
 		.name	= "radio-si4713",
+		.owner	= THIS_MODULE,
 	},
 	.probe		= radio_si4713_pdriver_probe,
-	.remove         = __exit_p(radio_si4713_pdriver_remove),
+	.remove         = radio_si4713_pdriver_remove,
 };
 
 module_platform_driver(radio_si4713_pdriver);
diff --git a/drivers/media/radio/radio-si476x.c b/drivers/media/radio/radio-si476x.c
new file mode 100644
index 000000000000..9430c6a29937
--- /dev/null
+++ b/drivers/media/radio/radio-si476x.c
@@ -0,0 +1,1599 @@
+/*
+ * drivers/media/radio/radio-si476x.c -- V4L2 driver for SI476X chips
+ *
+ * Copyright (C) 2012 Innovative Converged Devices(ICD)
+ * Copyright (C) 2013 Andrey Smirnov
+ *
+ * Author: Andrey Smirnov <andrew.smirnov@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; 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.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/atomic.h>
+#include <linux/videodev2.h>
+#include <linux/mutex.h>
+#include <linux/debugfs.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-device.h>
+
+#include <media/si476x.h>
+#include <linux/mfd/si476x-core.h>
+
+#define FM_FREQ_RANGE_LOW   64000000
+#define FM_FREQ_RANGE_HIGH 108000000
+
+#define AM_FREQ_RANGE_LOW    520000
+#define AM_FREQ_RANGE_HIGH 30000000
+
+#define PWRLINEFLTR (1 << 8)
+
+#define FREQ_MUL (10000000 / 625)
+
+#define SI476X_PHDIV_STATUS_LINK_LOCKED(status) (0b10000000 & (status))
+
+#define DRIVER_NAME "si476x-radio"
+#define DRIVER_CARD "SI476x AM/FM Receiver"
+
+enum si476x_freq_bands {
+	SI476X_BAND_FM,
+	SI476X_BAND_AM,
+};
+
+static const struct v4l2_frequency_band si476x_bands[] = {
+	[SI476X_BAND_FM] = {
+		.type		= V4L2_TUNER_RADIO,
+		.index		= SI476X_BAND_FM,
+		.capability	= V4L2_TUNER_CAP_LOW
+		| V4L2_TUNER_CAP_STEREO
+		| V4L2_TUNER_CAP_RDS
+		| V4L2_TUNER_CAP_RDS_BLOCK_IO
+		| V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow	=  64 * FREQ_MUL,
+		.rangehigh	= 108 * FREQ_MUL,
+		.modulation	= V4L2_BAND_MODULATION_FM,
+	},
+	[SI476X_BAND_AM] = {
+		.type		= V4L2_TUNER_RADIO,
+		.index		= SI476X_BAND_AM,
+		.capability	= V4L2_TUNER_CAP_LOW
+		| V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow	= 0.52 * FREQ_MUL,
+		.rangehigh	= 30 * FREQ_MUL,
+		.modulation	= V4L2_BAND_MODULATION_AM,
+	},
+};
+
+static inline bool si476x_radio_freq_is_inside_of_the_band(u32 freq, int band)
+{
+	return freq >= si476x_bands[band].rangelow &&
+		freq <= si476x_bands[band].rangehigh;
+}
+
+static inline bool si476x_radio_range_is_inside_of_the_band(u32 low, u32 high,
+							    int band)
+{
+	return low  >= si476x_bands[band].rangelow &&
+		high <= si476x_bands[band].rangehigh;
+}
+
+static int si476x_radio_s_ctrl(struct v4l2_ctrl *ctrl);
+static int si476x_radio_g_volatile_ctrl(struct v4l2_ctrl *ctrl);
+
+enum phase_diversity_modes_idx {
+	SI476X_IDX_PHDIV_DISABLED,
+	SI476X_IDX_PHDIV_PRIMARY_COMBINING,
+	SI476X_IDX_PHDIV_PRIMARY_ANTENNA,
+	SI476X_IDX_PHDIV_SECONDARY_ANTENNA,
+	SI476X_IDX_PHDIV_SECONDARY_COMBINING,
+};
+
+static const char * const phase_diversity_modes[] = {
+	[SI476X_IDX_PHDIV_DISABLED]		= "Disabled",
+	[SI476X_IDX_PHDIV_PRIMARY_COMBINING]	= "Primary with Secondary",
+	[SI476X_IDX_PHDIV_PRIMARY_ANTENNA]	= "Primary Antenna",
+	[SI476X_IDX_PHDIV_SECONDARY_ANTENNA]	= "Secondary Antenna",
+	[SI476X_IDX_PHDIV_SECONDARY_COMBINING]	= "Secondary with Primary",
+};
+
+static inline enum phase_diversity_modes_idx
+si476x_phase_diversity_mode_to_idx(enum si476x_phase_diversity_mode mode)
+{
+	switch (mode) {
+	default:		/* FALLTHROUGH */
+	case SI476X_PHDIV_DISABLED:
+		return SI476X_IDX_PHDIV_DISABLED;
+	case SI476X_PHDIV_PRIMARY_COMBINING:
+		return SI476X_IDX_PHDIV_PRIMARY_COMBINING;
+	case SI476X_PHDIV_PRIMARY_ANTENNA:
+		return SI476X_IDX_PHDIV_PRIMARY_ANTENNA;
+	case SI476X_PHDIV_SECONDARY_ANTENNA:
+		return SI476X_IDX_PHDIV_SECONDARY_ANTENNA;
+	case SI476X_PHDIV_SECONDARY_COMBINING:
+		return SI476X_IDX_PHDIV_SECONDARY_COMBINING;
+	}
+}
+
+static inline enum si476x_phase_diversity_mode
+si476x_phase_diversity_idx_to_mode(enum phase_diversity_modes_idx idx)
+{
+	static const int idx_to_value[] = {
+		[SI476X_IDX_PHDIV_DISABLED]		= SI476X_PHDIV_DISABLED,
+		[SI476X_IDX_PHDIV_PRIMARY_COMBINING]	= SI476X_PHDIV_PRIMARY_COMBINING,
+		[SI476X_IDX_PHDIV_PRIMARY_ANTENNA]	= SI476X_PHDIV_PRIMARY_ANTENNA,
+		[SI476X_IDX_PHDIV_SECONDARY_ANTENNA]	= SI476X_PHDIV_SECONDARY_ANTENNA,
+		[SI476X_IDX_PHDIV_SECONDARY_COMBINING]	= SI476X_PHDIV_SECONDARY_COMBINING,
+	};
+
+	return idx_to_value[idx];
+}
+
+static const struct v4l2_ctrl_ops si476x_ctrl_ops = {
+	.g_volatile_ctrl	= si476x_radio_g_volatile_ctrl,
+	.s_ctrl			= si476x_radio_s_ctrl,
+};
+
+
+enum si476x_ctrl_idx {
+	SI476X_IDX_RSSI_THRESHOLD,
+	SI476X_IDX_SNR_THRESHOLD,
+	SI476X_IDX_MAX_TUNE_ERROR,
+	SI476X_IDX_HARMONICS_COUNT,
+	SI476X_IDX_DIVERSITY_MODE,
+	SI476X_IDX_INTERCHIP_LINK,
+};
+static struct v4l2_ctrl_config si476x_ctrls[] = {
+
+	/**
+	 * SI476X during its station seeking(or tuning) process uses several
+	 * parameters to detrmine if "the station" is valid:
+	 *
+	 *	- Signal's SNR(in dBuV) must be lower than
+	 *	#V4L2_CID_SI476X_SNR_THRESHOLD
+	 *	- Signal's RSSI(in dBuV) must be greater than
+	 *	#V4L2_CID_SI476X_RSSI_THRESHOLD
+	 *	- Signal's frequency deviation(in units of 2ppm) must not be
+	 *	more than #V4L2_CID_SI476X_MAX_TUNE_ERROR
+	 */
+	[SI476X_IDX_RSSI_THRESHOLD] = {
+		.ops	= &si476x_ctrl_ops,
+		.id	= V4L2_CID_SI476X_RSSI_THRESHOLD,
+		.name	= "Valid RSSI Threshold",
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.min	= -128,
+		.max	= 127,
+		.step	= 1,
+	},
+	[SI476X_IDX_SNR_THRESHOLD] = {
+		.ops	= &si476x_ctrl_ops,
+		.id	= V4L2_CID_SI476X_SNR_THRESHOLD,
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.name	= "Valid SNR Threshold",
+		.min	= -128,
+		.max	= 127,
+		.step	= 1,
+	},
+	[SI476X_IDX_MAX_TUNE_ERROR] = {
+		.ops	= &si476x_ctrl_ops,
+		.id	= V4L2_CID_SI476X_MAX_TUNE_ERROR,
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.name	= "Max Tune Errors",
+		.min	= 0,
+		.max	= 126 * 2,
+		.step	= 2,
+	},
+
+	/**
+	 * #V4L2_CID_SI476X_HARMONICS_COUNT -- number of harmonics
+	 * built-in power-line noise supression filter is to reject
+	 * during AM-mode operation.
+	 */
+	[SI476X_IDX_HARMONICS_COUNT] = {
+		.ops	= &si476x_ctrl_ops,
+		.id	= V4L2_CID_SI476X_HARMONICS_COUNT,
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+
+		.name	= "Count of Harmonics to Reject",
+		.min	= 0,
+		.max	= 20,
+		.step	= 1,
+	},
+
+	/**
+	 * #V4L2_CID_SI476X_DIVERSITY_MODE -- configuration which
+	 * two tuners working in diversity mode are to work in.
+	 *
+	 *  - #SI476X_IDX_PHDIV_DISABLED diversity mode disabled
+	 *  - #SI476X_IDX_PHDIV_PRIMARY_COMBINING diversity mode is
+	 *  on, primary tuner's antenna is the main one.
+	 *  - #SI476X_IDX_PHDIV_PRIMARY_ANTENNA diversity mode is
+	 *  off, primary tuner's antenna is the main one.
+	 *  - #SI476X_IDX_PHDIV_SECONDARY_ANTENNA diversity mode is
+	 *  off, secondary tuner's antenna is the main one.
+	 *  - #SI476X_IDX_PHDIV_SECONDARY_COMBINING diversity mode is
+	 *  on, secondary tuner's antenna is the main one.
+	 */
+	[SI476X_IDX_DIVERSITY_MODE] = {
+		.ops	= &si476x_ctrl_ops,
+		.id	= V4L2_CID_SI476X_DIVERSITY_MODE,
+		.type	= V4L2_CTRL_TYPE_MENU,
+		.name	= "Phase Diversity Mode",
+		.qmenu	= phase_diversity_modes,
+		.min	= 0,
+		.max	= ARRAY_SIZE(phase_diversity_modes) - 1,
+	},
+
+	/**
+	 * #V4L2_CID_SI476X_INTERCHIP_LINK -- inter-chip link in
+	 * diversity mode indicator. Allows user to determine if two
+	 * chips working in diversity mode have established a link
+	 * between each other and if the system as a whole uses
+	 * signals from both antennas to receive FM radio.
+	 */
+	[SI476X_IDX_INTERCHIP_LINK] = {
+		.ops	= &si476x_ctrl_ops,
+		.id	= V4L2_CID_SI476X_INTERCHIP_LINK,
+		.type	= V4L2_CTRL_TYPE_BOOLEAN,
+		.flags  = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_VOLATILE,
+		.name	= "Inter-Chip Link",
+		.min	= 0,
+		.max	= 1,
+		.step	= 1,
+	},
+};
+
+struct si476x_radio;
+
+/**
+ * struct si476x_radio_ops - vtable of tuner functions
+ *
+ * This table holds pointers to functions implementing particular
+ * operations depending on the mode in which the tuner chip was
+ * configured to start in. If the function is not supported
+ * corresponding element is set to #NULL.
+ *
+ * @tune_freq: Tune chip to a specific frequency
+ * @seek_start: Star station seeking
+ * @rsq_status: Get Recieved Signal Quality(RSQ) status
+ * @rds_blckcnt: Get recived RDS blocks count
+ * @phase_diversity: Change phase diversity mode of the tuner
+ * @phase_div_status: Get phase diversity mode status
+ * @acf_status: Get the status of Automatically Controlled
+ * Features(ACF)
+ * @agc_status: Get Automatic Gain Control(AGC) status
+ */
+struct si476x_radio_ops {
+	int (*tune_freq)(struct si476x_core *, struct si476x_tune_freq_args *);
+	int (*seek_start)(struct si476x_core *, bool, bool);
+	int (*rsq_status)(struct si476x_core *, struct si476x_rsq_status_args *,
+			  struct si476x_rsq_status_report *);
+	int (*rds_blckcnt)(struct si476x_core *, bool,
+			   struct si476x_rds_blockcount_report *);
+
+	int (*phase_diversity)(struct si476x_core *,
+			       enum si476x_phase_diversity_mode);
+	int (*phase_div_status)(struct si476x_core *);
+	int (*acf_status)(struct si476x_core *,
+			  struct si476x_acf_status_report *);
+	int (*agc_status)(struct si476x_core *,
+			  struct si476x_agc_status_report *);
+};
+
+/**
+ * struct si476x_radio - radio device
+ *
+ * @core: Pointer to underlying core device
+ * @videodev: Pointer to video device created by V4L2 subsystem
+ * @ops: Vtable of functions. See struct si476x_radio_ops for details
+ * @kref: Reference counter
+ * @core_lock: An r/w semaphore to brebvent the deletion of underlying
+ * core structure is the radio device is being used
+ */
+struct si476x_radio {
+	struct v4l2_device v4l2dev;
+	struct video_device videodev;
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	struct si476x_core  *core;
+	/* This field should not be accesses unless core lock is held */
+	const struct si476x_radio_ops *ops;
+
+	struct dentry	*debugfs;
+	u32 audmode;
+};
+
+static inline struct si476x_radio *
+v4l2_dev_to_radio(struct v4l2_device *d)
+{
+	return container_of(d, struct si476x_radio, v4l2dev);
+}
+
+static inline struct si476x_radio *
+v4l2_ctrl_handler_to_radio(struct v4l2_ctrl_handler *d)
+{
+	return container_of(d, struct si476x_radio, ctrl_handler);
+}
+
+/*
+ * si476x_vidioc_querycap - query device capabilities
+ */
+static int si476x_radio_querycap(struct file *file, void *priv,
+				 struct v4l2_capability *capability)
+{
+	struct si476x_radio *radio = video_drvdata(file);
+
+	strlcpy(capability->driver, radio->v4l2dev.name,
+		sizeof(capability->driver));
+	strlcpy(capability->card,   DRIVER_CARD, sizeof(capability->card));
+	snprintf(capability->bus_info, sizeof(capability->bus_info),
+		 "platform:%s", radio->v4l2dev.name);
+
+	capability->device_caps = V4L2_CAP_TUNER
+		| V4L2_CAP_RADIO
+		| V4L2_CAP_HW_FREQ_SEEK;
+
+	si476x_core_lock(radio->core);
+	if (!si476x_core_is_a_secondary_tuner(radio->core))
+		capability->device_caps |= V4L2_CAP_RDS_CAPTURE
+			| V4L2_CAP_READWRITE;
+	si476x_core_unlock(radio->core);
+
+	capability->capabilities = capability->device_caps
+		| V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int si476x_radio_enum_freq_bands(struct file *file, void *priv,
+					struct v4l2_frequency_band *band)
+{
+	int err;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	if (band->tuner != 0)
+		return -EINVAL;
+
+	switch (radio->core->chip_id) {
+		/* AM/FM tuners -- all bands are supported */
+	case SI476X_CHIP_SI4761:
+	case SI476X_CHIP_SI4764:
+		if (band->index < ARRAY_SIZE(si476x_bands)) {
+			*band = si476x_bands[band->index];
+			err = 0;
+		} else {
+			err = -EINVAL;
+		}
+		break;
+		/* FM companion tuner chips -- only FM bands are
+		 * supported */
+	case SI476X_CHIP_SI4768:
+		if (band->index == SI476X_BAND_FM) {
+			*band = si476x_bands[band->index];
+			err = 0;
+		} else {
+			err = -EINVAL;
+		}
+		break;
+	default:
+		err = -EINVAL;
+	}
+
+	return err;
+}
+
+static int si476x_radio_g_tuner(struct file *file, void *priv,
+				struct v4l2_tuner *tuner)
+{
+	int err;
+	struct si476x_rsq_status_report report;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	struct si476x_rsq_status_args args = {
+		.primary	= false,
+		.rsqack		= false,
+		.attune		= false,
+		.cancel		= false,
+		.stcack		= false,
+	};
+
+	if (tuner->index != 0)
+		return -EINVAL;
+
+	tuner->type       = V4L2_TUNER_RADIO;
+	tuner->capability = V4L2_TUNER_CAP_LOW /* Measure frequencies
+						 * in multiples of
+						 * 62.5 Hz */
+		| V4L2_TUNER_CAP_STEREO
+		| V4L2_TUNER_CAP_HWSEEK_BOUNDED
+		| V4L2_TUNER_CAP_HWSEEK_WRAP
+		| V4L2_TUNER_CAP_HWSEEK_PROG_LIM;
+
+	si476x_core_lock(radio->core);
+
+	if (si476x_core_is_a_secondary_tuner(radio->core)) {
+		strlcpy(tuner->name, "FM (secondary)", sizeof(tuner->name));
+		tuner->rxsubchans = 0;
+		tuner->rangelow = si476x_bands[SI476X_BAND_FM].rangelow;
+	} else if (si476x_core_has_am(radio->core)) {
+		if (si476x_core_is_a_primary_tuner(radio->core))
+			strlcpy(tuner->name, "AM/FM (primary)",
+				sizeof(tuner->name));
+		else
+			strlcpy(tuner->name, "AM/FM", sizeof(tuner->name));
+
+		tuner->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_STEREO
+			| V4L2_TUNER_SUB_RDS;
+		tuner->capability |= V4L2_TUNER_CAP_RDS
+			| V4L2_TUNER_CAP_RDS_BLOCK_IO
+			| V4L2_TUNER_CAP_FREQ_BANDS;
+
+		tuner->rangelow = si476x_bands[SI476X_BAND_AM].rangelow;
+	} else {
+		strlcpy(tuner->name, "FM", sizeof(tuner->name));
+		tuner->rxsubchans = V4L2_TUNER_SUB_RDS;
+		tuner->capability |= V4L2_TUNER_CAP_RDS
+			| V4L2_TUNER_CAP_RDS_BLOCK_IO
+			| V4L2_TUNER_CAP_FREQ_BANDS;
+		tuner->rangelow = si476x_bands[SI476X_BAND_FM].rangelow;
+	}
+
+	tuner->audmode = radio->audmode;
+
+	tuner->afc = 1;
+	tuner->rangehigh = si476x_bands[SI476X_BAND_FM].rangehigh;
+
+	err = radio->ops->rsq_status(radio->core,
+				     &args, &report);
+	if (err < 0) {
+		tuner->signal = 0;
+	} else {
+		/*
+		 * tuner->signal value range: 0x0000 .. 0xFFFF,
+		 * report.rssi: -128 .. 127
+		 */
+		tuner->signal = (report.rssi + 128) * 257;
+	}
+	si476x_core_unlock(radio->core);
+
+	return err;
+}
+
+static int si476x_radio_s_tuner(struct file *file, void *priv,
+				const struct v4l2_tuner *tuner)
+{
+	struct si476x_radio *radio = video_drvdata(file);
+
+	if (tuner->index != 0)
+		return -EINVAL;
+
+	if (tuner->audmode == V4L2_TUNER_MODE_MONO ||
+	    tuner->audmode == V4L2_TUNER_MODE_STEREO)
+		radio->audmode = tuner->audmode;
+	else
+		radio->audmode = V4L2_TUNER_MODE_STEREO;
+
+	return 0;
+}
+
+static int si476x_radio_init_vtable(struct si476x_radio *radio,
+				    enum si476x_func func)
+{
+	static const struct si476x_radio_ops fm_ops = {
+		.tune_freq		= si476x_core_cmd_fm_tune_freq,
+		.seek_start		= si476x_core_cmd_fm_seek_start,
+		.rsq_status		= si476x_core_cmd_fm_rsq_status,
+		.rds_blckcnt		= si476x_core_cmd_fm_rds_blockcount,
+		.phase_diversity	= si476x_core_cmd_fm_phase_diversity,
+		.phase_div_status	= si476x_core_cmd_fm_phase_div_status,
+		.acf_status		= si476x_core_cmd_fm_acf_status,
+		.agc_status		= si476x_core_cmd_agc_status,
+	};
+
+	static const struct si476x_radio_ops am_ops = {
+		.tune_freq		= si476x_core_cmd_am_tune_freq,
+		.seek_start		= si476x_core_cmd_am_seek_start,
+		.rsq_status		= si476x_core_cmd_am_rsq_status,
+		.rds_blckcnt		= NULL,
+		.phase_diversity	= NULL,
+		.phase_div_status	= NULL,
+		.acf_status		= si476x_core_cmd_am_acf_status,
+		.agc_status		= NULL,
+	};
+
+	switch (func) {
+	case SI476X_FUNC_FM_RECEIVER:
+		radio->ops = &fm_ops;
+		return 0;
+
+	case SI476X_FUNC_AM_RECEIVER:
+		radio->ops = &am_ops;
+		return 0;
+	default:
+		WARN(1, "Unexpected tuner function value\n");
+		return -EINVAL;
+	}
+}
+
+static int si476x_radio_pretune(struct si476x_radio *radio,
+				enum si476x_func func)
+{
+	int retval;
+
+	struct si476x_tune_freq_args args = {
+		.zifsr		= false,
+		.hd		= false,
+		.injside	= SI476X_INJSIDE_AUTO,
+		.tunemode	= SI476X_TM_VALIDATED_NORMAL_TUNE,
+		.smoothmetrics	= SI476X_SM_INITIALIZE_AUDIO,
+		.antcap		= 0,
+	};
+
+	switch (func) {
+	case SI476X_FUNC_FM_RECEIVER:
+		args.freq = v4l2_to_si476x(radio->core,
+					   92 * FREQ_MUL);
+		retval = radio->ops->tune_freq(radio->core, &args);
+		break;
+	case SI476X_FUNC_AM_RECEIVER:
+		args.freq = v4l2_to_si476x(radio->core,
+					   0.6 * FREQ_MUL);
+		retval = radio->ops->tune_freq(radio->core, &args);
+		break;
+	default:
+		WARN(1, "Unexpected tuner function value\n");
+		retval = -EINVAL;
+	}
+
+	return retval;
+}
+static int si476x_radio_do_post_powerup_init(struct si476x_radio *radio,
+					     enum si476x_func func)
+{
+	int err;
+
+	/* regcache_mark_dirty(radio->core->regmap); */
+	err = regcache_sync_region(radio->core->regmap,
+				   SI476X_PROP_DIGITAL_IO_INPUT_SAMPLE_RATE,
+				   SI476X_PROP_DIGITAL_IO_OUTPUT_FORMAT);
+		if (err < 0)
+			return err;
+
+	err = regcache_sync_region(radio->core->regmap,
+				   SI476X_PROP_AUDIO_DEEMPHASIS,
+				   SI476X_PROP_AUDIO_PWR_LINE_FILTER);
+	if (err < 0)
+		return err;
+
+	err = regcache_sync_region(radio->core->regmap,
+				   SI476X_PROP_INT_CTL_ENABLE,
+				   SI476X_PROP_INT_CTL_ENABLE);
+	if (err < 0)
+		return err;
+
+	/*
+	 * Is there any point in restoring SNR and the like
+	 * when switching between AM/FM?
+	 */
+	err = regcache_sync_region(radio->core->regmap,
+				   SI476X_PROP_VALID_MAX_TUNE_ERROR,
+				   SI476X_PROP_VALID_MAX_TUNE_ERROR);
+	if (err < 0)
+		return err;
+
+	err = regcache_sync_region(radio->core->regmap,
+				   SI476X_PROP_VALID_SNR_THRESHOLD,
+				   SI476X_PROP_VALID_RSSI_THRESHOLD);
+	if (err < 0)
+		return err;
+
+	if (func == SI476X_FUNC_FM_RECEIVER) {
+		if (si476x_core_has_diversity(radio->core)) {
+			err = si476x_core_cmd_fm_phase_diversity(radio->core,
+								 radio->core->diversity_mode);
+			if (err < 0)
+				return err;
+		}
+
+		err = regcache_sync_region(radio->core->regmap,
+					   SI476X_PROP_FM_RDS_INTERRUPT_SOURCE,
+					   SI476X_PROP_FM_RDS_CONFIG);
+		if (err < 0)
+			return err;
+	}
+
+	return si476x_radio_init_vtable(radio, func);
+
+}
+
+static int si476x_radio_change_func(struct si476x_radio *radio,
+				    enum si476x_func func)
+{
+	int err;
+	bool soft;
+	/*
+	 * Since power/up down is a very time consuming operation,
+	 * try to avoid doing it if the requested mode matches the one
+	 * the tuner is in
+	 */
+	if (func == radio->core->power_up_parameters.func)
+		return 0;
+
+	soft = true;
+	err = si476x_core_stop(radio->core, soft);
+	if (err < 0) {
+		/*
+		 * OK, if the chip does not want to play nice let's
+		 * try to reset it in more brutal way
+		 */
+		soft = false;
+		err = si476x_core_stop(radio->core, soft);
+		if (err < 0)
+			return err;
+	}
+	/*
+	  Set the desired radio tuner function
+	 */
+	radio->core->power_up_parameters.func = func;
+
+	err = si476x_core_start(radio->core, soft);
+	if (err < 0)
+		return err;
+
+	/*
+	 * No need to do the rest of manipulations for the bootlader
+	 * mode
+	 */
+	if (func != SI476X_FUNC_FM_RECEIVER &&
+	    func != SI476X_FUNC_AM_RECEIVER)
+		return err;
+
+	return si476x_radio_do_post_powerup_init(radio, func);
+}
+
+static int si476x_radio_g_frequency(struct file *file, void *priv,
+			      struct v4l2_frequency *f)
+{
+	int err;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	if (f->tuner != 0 ||
+	    f->type  != V4L2_TUNER_RADIO)
+		return -EINVAL;
+
+	si476x_core_lock(radio->core);
+
+	if (radio->ops->rsq_status) {
+		struct si476x_rsq_status_report report;
+		struct si476x_rsq_status_args   args = {
+			.primary	= false,
+			.rsqack		= false,
+			.attune		= true,
+			.cancel		= false,
+			.stcack		= false,
+		};
+
+		err = radio->ops->rsq_status(radio->core, &args, &report);
+		if (!err)
+			f->frequency = si476x_to_v4l2(radio->core,
+						      report.readfreq);
+	} else {
+		err = -EINVAL;
+	}
+
+	si476x_core_unlock(radio->core);
+
+	return err;
+}
+
+static int si476x_radio_s_frequency(struct file *file, void *priv,
+				    const struct v4l2_frequency *f)
+{
+	int err;
+	u32 freq = f->frequency;
+	struct si476x_tune_freq_args args;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	const u32 midrange = (si476x_bands[SI476X_BAND_AM].rangehigh +
+			      si476x_bands[SI476X_BAND_FM].rangelow) / 2;
+	const int band = (freq > midrange) ?
+		SI476X_BAND_FM : SI476X_BAND_AM;
+	const enum si476x_func func = (band == SI476X_BAND_AM) ?
+		SI476X_FUNC_AM_RECEIVER : SI476X_FUNC_FM_RECEIVER;
+
+	if (f->tuner != 0 ||
+	    f->type  != V4L2_TUNER_RADIO)
+		return -EINVAL;
+
+	si476x_core_lock(radio->core);
+
+	freq = clamp(freq,
+		     si476x_bands[band].rangelow,
+		     si476x_bands[band].rangehigh);
+
+	if (si476x_radio_freq_is_inside_of_the_band(freq,
+						    SI476X_BAND_AM) &&
+	    (!si476x_core_has_am(radio->core) ||
+	     si476x_core_is_a_secondary_tuner(radio->core))) {
+		err = -EINVAL;
+		goto unlock;
+	}
+
+	err = si476x_radio_change_func(radio, func);
+	if (err < 0)
+		goto unlock;
+
+	args.zifsr		= false;
+	args.hd			= false;
+	args.injside		= SI476X_INJSIDE_AUTO;
+	args.freq		= v4l2_to_si476x(radio->core, freq);
+	args.tunemode		= SI476X_TM_VALIDATED_NORMAL_TUNE;
+	args.smoothmetrics	= SI476X_SM_INITIALIZE_AUDIO;
+	args.antcap		= 0;
+
+	err = radio->ops->tune_freq(radio->core, &args);
+
+unlock:
+	si476x_core_unlock(radio->core);
+	return err;
+}
+
+static int si476x_radio_s_hw_freq_seek(struct file *file, void *priv,
+				       const struct v4l2_hw_freq_seek *seek)
+{
+	int err;
+	enum si476x_func func;
+	u32 rangelow, rangehigh;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	if (file->f_flags & O_NONBLOCK)
+		return -EAGAIN;
+
+	if (seek->tuner != 0 ||
+	    seek->type  != V4L2_TUNER_RADIO)
+		return -EINVAL;
+
+	si476x_core_lock(radio->core);
+
+	if (!seek->rangelow) {
+		err = regmap_read(radio->core->regmap,
+				  SI476X_PROP_SEEK_BAND_BOTTOM,
+				  &rangelow);
+		if (!err)
+			rangelow = si476x_to_v4l2(radio->core, rangelow);
+		else
+			goto unlock;
+	}
+	if (!seek->rangehigh) {
+		err = regmap_read(radio->core->regmap,
+				  SI476X_PROP_SEEK_BAND_TOP,
+				  &rangehigh);
+		if (!err)
+			rangehigh = si476x_to_v4l2(radio->core, rangehigh);
+		else
+			goto unlock;
+	}
+
+	if (rangelow > rangehigh) {
+		err = -EINVAL;
+		goto unlock;
+	}
+
+	if (si476x_radio_range_is_inside_of_the_band(rangelow, rangehigh,
+						     SI476X_BAND_FM)) {
+		func = SI476X_FUNC_FM_RECEIVER;
+
+	} else if (si476x_core_has_am(radio->core) &&
+		   si476x_radio_range_is_inside_of_the_band(rangelow, rangehigh,
+							    SI476X_BAND_AM)) {
+		func = SI476X_FUNC_AM_RECEIVER;
+	} else {
+		err = -EINVAL;
+		goto unlock;
+	}
+
+	err = si476x_radio_change_func(radio, func);
+	if (err < 0)
+		goto unlock;
+
+	if (seek->rangehigh) {
+		err = regmap_write(radio->core->regmap,
+				   SI476X_PROP_SEEK_BAND_TOP,
+				   v4l2_to_si476x(radio->core,
+						  seek->rangehigh));
+		if (err)
+			goto unlock;
+	}
+	if (seek->rangelow) {
+		err = regmap_write(radio->core->regmap,
+				   SI476X_PROP_SEEK_BAND_BOTTOM,
+				   v4l2_to_si476x(radio->core,
+						  seek->rangelow));
+		if (err)
+			goto unlock;
+	}
+	if (seek->spacing) {
+		err = regmap_write(radio->core->regmap,
+				     SI476X_PROP_SEEK_FREQUENCY_SPACING,
+				     v4l2_to_si476x(radio->core,
+						    seek->spacing));
+		if (err)
+			goto unlock;
+	}
+
+	err = radio->ops->seek_start(radio->core,
+				     seek->seek_upward,
+				     seek->wrap_around);
+unlock:
+	si476x_core_unlock(radio->core);
+
+
+
+	return err;
+}
+
+static int si476x_radio_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	int retval;
+	struct si476x_radio *radio = v4l2_ctrl_handler_to_radio(ctrl->handler);
+
+	si476x_core_lock(radio->core);
+
+	switch (ctrl->id) {
+	case V4L2_CID_SI476X_INTERCHIP_LINK:
+		if (si476x_core_has_diversity(radio->core)) {
+			if (radio->ops->phase_diversity) {
+				retval = radio->ops->phase_div_status(radio->core);
+				if (retval < 0)
+					break;
+
+				ctrl->val = !!SI476X_PHDIV_STATUS_LINK_LOCKED(retval);
+				retval = 0;
+				break;
+			} else {
+				retval = -ENOTTY;
+				break;
+			}
+		}
+		retval = -EINVAL;
+		break;
+	default:
+		retval = -EINVAL;
+		break;
+	}
+	si476x_core_unlock(radio->core);
+	return retval;
+
+}
+
+static int si476x_radio_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	int retval;
+	enum si476x_phase_diversity_mode mode;
+	struct si476x_radio *radio = v4l2_ctrl_handler_to_radio(ctrl->handler);
+
+	si476x_core_lock(radio->core);
+
+	switch (ctrl->id) {
+	case V4L2_CID_SI476X_HARMONICS_COUNT:
+		retval = regmap_update_bits(radio->core->regmap,
+					    SI476X_PROP_AUDIO_PWR_LINE_FILTER,
+					    SI476X_PROP_PWR_HARMONICS_MASK,
+					    ctrl->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		switch (ctrl->val) {
+		case V4L2_CID_POWER_LINE_FREQUENCY_DISABLED:
+			retval = regmap_update_bits(radio->core->regmap,
+						    SI476X_PROP_AUDIO_PWR_LINE_FILTER,
+						    SI476X_PROP_PWR_ENABLE_MASK,
+						    0);
+			break;
+		case V4L2_CID_POWER_LINE_FREQUENCY_50HZ:
+			retval = regmap_update_bits(radio->core->regmap,
+						    SI476X_PROP_AUDIO_PWR_LINE_FILTER,
+						    SI476X_PROP_PWR_GRID_MASK,
+						    SI476X_PROP_PWR_GRID_50HZ);
+			break;
+		case V4L2_CID_POWER_LINE_FREQUENCY_60HZ:
+			retval = regmap_update_bits(radio->core->regmap,
+						    SI476X_PROP_AUDIO_PWR_LINE_FILTER,
+						    SI476X_PROP_PWR_GRID_MASK,
+						    SI476X_PROP_PWR_GRID_60HZ);
+			break;
+		default:
+			retval = -EINVAL;
+			break;
+		}
+		break;
+	case V4L2_CID_SI476X_RSSI_THRESHOLD:
+		retval = regmap_write(radio->core->regmap,
+				      SI476X_PROP_VALID_RSSI_THRESHOLD,
+				      ctrl->val);
+		break;
+	case V4L2_CID_SI476X_SNR_THRESHOLD:
+		retval = regmap_write(radio->core->regmap,
+				      SI476X_PROP_VALID_SNR_THRESHOLD,
+				      ctrl->val);
+		break;
+	case V4L2_CID_SI476X_MAX_TUNE_ERROR:
+		retval = regmap_write(radio->core->regmap,
+				      SI476X_PROP_VALID_MAX_TUNE_ERROR,
+				      ctrl->val);
+		break;
+	case V4L2_CID_RDS_RECEPTION:
+		/*
+		 * It looks like RDS related properties are
+		 * inaccesable when tuner is in AM mode, so cache the
+		 * changes
+		 */
+		if (si476x_core_is_in_am_receiver_mode(radio->core))
+			regcache_cache_only(radio->core->regmap, true);
+
+		if (ctrl->val) {
+			retval = regmap_write(radio->core->regmap,
+					      SI476X_PROP_FM_RDS_INTERRUPT_FIFO_COUNT,
+					      radio->core->rds_fifo_depth);
+			if (retval < 0)
+				break;
+
+			if (radio->core->client->irq) {
+				retval = regmap_write(radio->core->regmap,
+						      SI476X_PROP_FM_RDS_INTERRUPT_SOURCE,
+						      SI476X_RDSRECV);
+				if (retval < 0)
+					break;
+			}
+
+			/* Drain RDS FIFO before enabling RDS processing */
+			retval = si476x_core_cmd_fm_rds_status(radio->core,
+							       false,
+							       true,
+							       true,
+							       NULL);
+			if (retval < 0)
+				break;
+
+			retval = regmap_update_bits(radio->core->regmap,
+						    SI476X_PROP_FM_RDS_CONFIG,
+						    SI476X_PROP_RDSEN_MASK,
+						    SI476X_PROP_RDSEN);
+		} else {
+			retval = regmap_update_bits(radio->core->regmap,
+						    SI476X_PROP_FM_RDS_CONFIG,
+						    SI476X_PROP_RDSEN_MASK,
+						    !SI476X_PROP_RDSEN);
+		}
+
+		if (si476x_core_is_in_am_receiver_mode(radio->core))
+			regcache_cache_only(radio->core->regmap, false);
+		break;
+	case V4L2_CID_TUNE_DEEMPHASIS:
+		retval = regmap_write(radio->core->regmap,
+				      SI476X_PROP_AUDIO_DEEMPHASIS,
+				      ctrl->val);
+		break;
+
+	case V4L2_CID_SI476X_DIVERSITY_MODE:
+		mode = si476x_phase_diversity_idx_to_mode(ctrl->val);
+
+		if (mode == radio->core->diversity_mode) {
+			retval = 0;
+			break;
+		}
+
+		if (si476x_core_is_in_am_receiver_mode(radio->core)) {
+			/*
+			 * Diversity cannot be configured while tuner
+			 * is in AM mode so save the changes and carry on.
+			 */
+			radio->core->diversity_mode = mode;
+			retval = 0;
+		} else {
+			retval = radio->ops->phase_diversity(radio->core, mode);
+			if (!retval)
+				radio->core->diversity_mode = mode;
+		}
+		break;
+
+	default:
+		retval = -EINVAL;
+		break;
+	}
+
+	si476x_core_unlock(radio->core);
+
+	return retval;
+}
+
+static int si476x_radio_g_chip_ident(struct file *file, void *fh,
+				     struct v4l2_dbg_chip_ident *chip)
+{
+	if (chip->match.type == V4L2_CHIP_MATCH_HOST &&
+	    v4l2_chip_match_host(&chip->match))
+		return 0;
+	return -EINVAL;
+}
+
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int si476x_radio_g_register(struct file *file, void *fh,
+				   struct v4l2_dbg_register *reg)
+{
+	int err;
+	unsigned int value;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	si476x_core_lock(radio->core);
+	reg->size = 2;
+	err = regmap_read(radio->core->regmap,
+			  (unsigned int)reg->reg, &value);
+	reg->val = value;
+	si476x_core_unlock(radio->core);
+
+	return err;
+}
+static int si476x_radio_s_register(struct file *file, void *fh,
+				   const struct v4l2_dbg_register *reg)
+{
+
+	int err;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	si476x_core_lock(radio->core);
+	err = regmap_write(radio->core->regmap,
+			   (unsigned int)reg->reg,
+			   (unsigned int)reg->val);
+	si476x_core_unlock(radio->core);
+
+	return err;
+}
+#endif
+
+static int si476x_radio_fops_open(struct file *file)
+{
+	struct si476x_radio *radio = video_drvdata(file);
+	int err;
+
+	err = v4l2_fh_open(file);
+	if (err)
+		return err;
+
+	if (v4l2_fh_is_singular_file(file)) {
+		si476x_core_lock(radio->core);
+		err = si476x_core_set_power_state(radio->core,
+						  SI476X_POWER_UP_FULL);
+		if (err < 0)
+			goto done;
+
+		err = si476x_radio_do_post_powerup_init(radio,
+							radio->core->power_up_parameters.func);
+		if (err < 0)
+			goto power_down;
+
+		err = si476x_radio_pretune(radio,
+					   radio->core->power_up_parameters.func);
+		if (err < 0)
+			goto power_down;
+
+		si476x_core_unlock(radio->core);
+		/*Must be done after si476x_core_unlock to prevent a deadlock*/
+		v4l2_ctrl_handler_setup(&radio->ctrl_handler);
+	}
+
+	return err;
+
+power_down:
+	si476x_core_set_power_state(radio->core,
+				    SI476X_POWER_DOWN);
+done:
+	si476x_core_unlock(radio->core);
+	v4l2_fh_release(file);
+
+	return err;
+}
+
+static int si476x_radio_fops_release(struct file *file)
+{
+	int err;
+	struct si476x_radio *radio = video_drvdata(file);
+
+	if (v4l2_fh_is_singular_file(file) &&
+	    atomic_read(&radio->core->is_alive))
+		si476x_core_set_power_state(radio->core,
+					    SI476X_POWER_DOWN);
+
+	err = v4l2_fh_release(file);
+
+	return err;
+}
+
+static ssize_t si476x_radio_fops_read(struct file *file, char __user *buf,
+				      size_t count, loff_t *ppos)
+{
+	ssize_t      rval;
+	size_t       fifo_len;
+	unsigned int copied;
+
+	struct si476x_radio *radio = video_drvdata(file);
+
+	/* block if no new data available */
+	if (kfifo_is_empty(&radio->core->rds_fifo)) {
+		if (file->f_flags & O_NONBLOCK)
+			return -EWOULDBLOCK;
+
+		rval = wait_event_interruptible(radio->core->rds_read_queue,
+						(!kfifo_is_empty(&radio->core->rds_fifo) ||
+						 !atomic_read(&radio->core->is_alive)));
+		if (rval < 0)
+			return -EINTR;
+
+		if (!atomic_read(&radio->core->is_alive))
+			return -ENODEV;
+	}
+
+	fifo_len = kfifo_len(&radio->core->rds_fifo);
+
+	if (kfifo_to_user(&radio->core->rds_fifo, buf,
+			  min(fifo_len, count),
+			  &copied) != 0) {
+		dev_warn(&radio->videodev.dev,
+			 "Error during FIFO to userspace copy\n");
+		rval = -EIO;
+	} else {
+		rval = (ssize_t)copied;
+	}
+
+	return rval;
+}
+
+static unsigned int si476x_radio_fops_poll(struct file *file,
+				struct poll_table_struct *pts)
+{
+	struct si476x_radio *radio = video_drvdata(file);
+	unsigned long req_events = poll_requested_events(pts);
+	unsigned int err = v4l2_ctrl_poll(file, pts);
+
+	if (req_events & (POLLIN | POLLRDNORM)) {
+		if (atomic_read(&radio->core->is_alive))
+			poll_wait(file, &radio->core->rds_read_queue, pts);
+
+		if (!atomic_read(&radio->core->is_alive))
+			err = POLLHUP;
+
+		if (!kfifo_is_empty(&radio->core->rds_fifo))
+			err = POLLIN | POLLRDNORM;
+	}
+
+	return err;
+}
+
+static const struct v4l2_file_operations si476x_fops = {
+	.owner			= THIS_MODULE,
+	.read			= si476x_radio_fops_read,
+	.poll			= si476x_radio_fops_poll,
+	.unlocked_ioctl		= video_ioctl2,
+	.open			= si476x_radio_fops_open,
+	.release		= si476x_radio_fops_release,
+};
+
+
+static const struct v4l2_ioctl_ops si4761_ioctl_ops = {
+	.vidioc_querycap		= si476x_radio_querycap,
+	.vidioc_g_tuner			= si476x_radio_g_tuner,
+	.vidioc_s_tuner			= si476x_radio_s_tuner,
+
+	.vidioc_g_frequency		= si476x_radio_g_frequency,
+	.vidioc_s_frequency		= si476x_radio_s_frequency,
+	.vidioc_s_hw_freq_seek		= si476x_radio_s_hw_freq_seek,
+	.vidioc_enum_freq_bands		= si476x_radio_enum_freq_bands,
+
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+
+	.vidioc_g_chip_ident		= si476x_radio_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_register		= si476x_radio_g_register,
+	.vidioc_s_register		= si476x_radio_s_register,
+#endif
+};
+
+
+static const struct video_device si476x_viddev_template = {
+	.fops			= &si476x_fops,
+	.name			= DRIVER_NAME,
+	.release		= video_device_release_empty,
+};
+
+
+
+static ssize_t si476x_radio_read_acf_blob(struct file *file,
+					  char __user *user_buf,
+					  size_t count, loff_t *ppos)
+{
+	int err;
+	struct si476x_radio *radio = file->private_data;
+	struct si476x_acf_status_report report;
+
+	si476x_core_lock(radio->core);
+	if (radio->ops->acf_status)
+		err = radio->ops->acf_status(radio->core, &report);
+	else
+		err = -ENOENT;
+	si476x_core_unlock(radio->core);
+
+	if (err < 0)
+		return err;
+
+	return simple_read_from_buffer(user_buf, count, ppos, &report,
+				       sizeof(report));
+}
+
+static const struct file_operations radio_acf_fops = {
+	.open	= simple_open,
+	.llseek = default_llseek,
+	.read	= si476x_radio_read_acf_blob,
+};
+
+static ssize_t si476x_radio_read_rds_blckcnt_blob(struct file *file,
+						  char __user *user_buf,
+						  size_t count, loff_t *ppos)
+{
+	int err;
+	struct si476x_radio *radio = file->private_data;
+	struct si476x_rds_blockcount_report report;
+
+	si476x_core_lock(radio->core);
+	if (radio->ops->rds_blckcnt)
+		err = radio->ops->rds_blckcnt(radio->core, true,
+					       &report);
+	else
+		err = -ENOENT;
+	si476x_core_unlock(radio->core);
+
+	if (err < 0)
+		return err;
+
+	return simple_read_from_buffer(user_buf, count, ppos, &report,
+				       sizeof(report));
+}
+
+static const struct file_operations radio_rds_blckcnt_fops = {
+	.open	= simple_open,
+	.llseek = default_llseek,
+	.read	= si476x_radio_read_rds_blckcnt_blob,
+};
+
+static ssize_t si476x_radio_read_agc_blob(struct file *file,
+					  char __user *user_buf,
+					  size_t count, loff_t *ppos)
+{
+	int err;
+	struct si476x_radio *radio = file->private_data;
+	struct si476x_agc_status_report report;
+
+	si476x_core_lock(radio->core);
+	if (radio->ops->rds_blckcnt)
+		err = radio->ops->agc_status(radio->core, &report);
+	else
+		err = -ENOENT;
+	si476x_core_unlock(radio->core);
+
+	if (err < 0)
+		return err;
+
+	return simple_read_from_buffer(user_buf, count, ppos, &report,
+				       sizeof(report));
+}
+
+static const struct file_operations radio_agc_fops = {
+	.open	= simple_open,
+	.llseek = default_llseek,
+	.read	= si476x_radio_read_agc_blob,
+};
+
+static ssize_t si476x_radio_read_rsq_blob(struct file *file,
+					  char __user *user_buf,
+					  size_t count, loff_t *ppos)
+{
+	int err;
+	struct si476x_radio *radio = file->private_data;
+	struct si476x_rsq_status_report report;
+	struct si476x_rsq_status_args args = {
+		.primary	= false,
+		.rsqack		= false,
+		.attune		= false,
+		.cancel		= false,
+		.stcack		= false,
+	};
+
+	si476x_core_lock(radio->core);
+	if (radio->ops->rds_blckcnt)
+		err = radio->ops->rsq_status(radio->core, &args, &report);
+	else
+		err = -ENOENT;
+	si476x_core_unlock(radio->core);
+
+	if (err < 0)
+		return err;
+
+	return simple_read_from_buffer(user_buf, count, ppos, &report,
+				       sizeof(report));
+}
+
+static const struct file_operations radio_rsq_fops = {
+	.open	= simple_open,
+	.llseek = default_llseek,
+	.read	= si476x_radio_read_rsq_blob,
+};
+
+static ssize_t si476x_radio_read_rsq_primary_blob(struct file *file,
+						  char __user *user_buf,
+						  size_t count, loff_t *ppos)
+{
+	int err;
+	struct si476x_radio *radio = file->private_data;
+	struct si476x_rsq_status_report report;
+	struct si476x_rsq_status_args args = {
+		.primary	= true,
+		.rsqack		= false,
+		.attune		= false,
+		.cancel		= false,
+		.stcack		= false,
+	};
+
+	si476x_core_lock(radio->core);
+	if (radio->ops->rds_blckcnt)
+		err = radio->ops->rsq_status(radio->core, &args, &report);
+	else
+		err = -ENOENT;
+	si476x_core_unlock(radio->core);
+
+	if (err < 0)
+		return err;
+
+	return simple_read_from_buffer(user_buf, count, ppos, &report,
+				       sizeof(report));
+}
+
+static const struct file_operations radio_rsq_primary_fops = {
+	.open	= simple_open,
+	.llseek = default_llseek,
+	.read	= si476x_radio_read_rsq_primary_blob,
+};
+
+
+static int si476x_radio_init_debugfs(struct si476x_radio *radio)
+{
+	struct dentry	*dentry;
+	int		ret;
+
+	dentry = debugfs_create_dir(dev_name(radio->v4l2dev.dev), NULL);
+	if (IS_ERR(dentry)) {
+		ret = PTR_ERR(dentry);
+		goto exit;
+	}
+	radio->debugfs = dentry;
+
+	dentry = debugfs_create_file("acf", S_IRUGO,
+				     radio->debugfs, radio, &radio_acf_fops);
+	if (IS_ERR(dentry)) {
+		ret = PTR_ERR(dentry);
+		goto cleanup;
+	}
+
+	dentry = debugfs_create_file("rds_blckcnt", S_IRUGO,
+				     radio->debugfs, radio,
+				     &radio_rds_blckcnt_fops);
+	if (IS_ERR(dentry)) {
+		ret = PTR_ERR(dentry);
+		goto cleanup;
+	}
+
+	dentry = debugfs_create_file("agc", S_IRUGO,
+				     radio->debugfs, radio, &radio_agc_fops);
+	if (IS_ERR(dentry)) {
+		ret = PTR_ERR(dentry);
+		goto cleanup;
+	}
+
+	dentry = debugfs_create_file("rsq", S_IRUGO,
+				     radio->debugfs, radio, &radio_rsq_fops);
+	if (IS_ERR(dentry)) {
+		ret = PTR_ERR(dentry);
+		goto cleanup;
+	}
+
+	dentry = debugfs_create_file("rsq_primary", S_IRUGO,
+				     radio->debugfs, radio,
+				     &radio_rsq_primary_fops);
+	if (IS_ERR(dentry)) {
+		ret = PTR_ERR(dentry);
+		goto cleanup;
+	}
+
+	return 0;
+cleanup:
+	debugfs_remove_recursive(radio->debugfs);
+exit:
+	return ret;
+}
+
+
+static int si476x_radio_add_new_custom(struct si476x_radio *radio,
+				       enum si476x_ctrl_idx idx)
+{
+	int rval;
+	struct v4l2_ctrl *ctrl;
+
+	ctrl = v4l2_ctrl_new_custom(&radio->ctrl_handler,
+				    &si476x_ctrls[idx],
+				    NULL);
+	rval = radio->ctrl_handler.error;
+	if (ctrl == NULL && rval)
+		dev_err(radio->v4l2dev.dev,
+			"Could not initialize '%s' control %d\n",
+			si476x_ctrls[idx].name, rval);
+
+	return rval;
+}
+
+static int si476x_radio_probe(struct platform_device *pdev)
+{
+	int rval;
+	struct si476x_radio *radio;
+	struct v4l2_ctrl *ctrl;
+
+	static atomic_t instance = ATOMIC_INIT(0);
+
+	radio = devm_kzalloc(&pdev->dev, sizeof(*radio), GFP_KERNEL);
+	if (!radio)
+		return -ENOMEM;
+
+	radio->core = i2c_mfd_cell_to_core(&pdev->dev);
+
+	v4l2_device_set_name(&radio->v4l2dev, DRIVER_NAME, &instance);
+
+	rval = v4l2_device_register(&pdev->dev, &radio->v4l2dev);
+	if (rval) {
+		dev_err(&pdev->dev, "Cannot register v4l2_device.\n");
+		return rval;
+	}
+
+	memcpy(&radio->videodev, &si476x_viddev_template,
+	       sizeof(struct video_device));
+
+	radio->videodev.v4l2_dev  = &radio->v4l2dev;
+	radio->videodev.ioctl_ops = &si4761_ioctl_ops;
+
+	video_set_drvdata(&radio->videodev, radio);
+	platform_set_drvdata(pdev, radio);
+
+	set_bit(V4L2_FL_USE_FH_PRIO, &radio->videodev.flags);
+
+	radio->v4l2dev.ctrl_handler = &radio->ctrl_handler;
+	v4l2_ctrl_handler_init(&radio->ctrl_handler,
+			       1 + ARRAY_SIZE(si476x_ctrls));
+
+	if (si476x_core_has_am(radio->core)) {
+		ctrl = v4l2_ctrl_new_std_menu(&radio->ctrl_handler,
+					      &si476x_ctrl_ops,
+					      V4L2_CID_POWER_LINE_FREQUENCY,
+					      V4L2_CID_POWER_LINE_FREQUENCY_60HZ,
+					      0, 0);
+		rval = radio->ctrl_handler.error;
+		if (ctrl == NULL && rval) {
+			dev_err(&pdev->dev, "Could not initialize V4L2_CID_POWER_LINE_FREQUENCY control %d\n",
+				rval);
+			goto exit;
+		}
+
+		rval = si476x_radio_add_new_custom(radio,
+						   SI476X_IDX_HARMONICS_COUNT);
+		if (rval < 0)
+			goto exit;
+	}
+
+	rval = si476x_radio_add_new_custom(radio, SI476X_IDX_RSSI_THRESHOLD);
+	if (rval < 0)
+		goto exit;
+
+	rval = si476x_radio_add_new_custom(radio, SI476X_IDX_SNR_THRESHOLD);
+	if (rval < 0)
+		goto exit;
+
+	rval = si476x_radio_add_new_custom(radio, SI476X_IDX_MAX_TUNE_ERROR);
+	if (rval < 0)
+		goto exit;
+
+	ctrl = v4l2_ctrl_new_std_menu(&radio->ctrl_handler,
+				      &si476x_ctrl_ops,
+				      V4L2_CID_TUNE_DEEMPHASIS,
+				      V4L2_DEEMPHASIS_75_uS, 0, 0);
+	rval = radio->ctrl_handler.error;
+	if (ctrl == NULL && rval) {
+		dev_err(&pdev->dev, "Could not initialize V4L2_CID_TUNE_DEEMPHASIS control %d\n",
+			rval);
+		goto exit;
+	}
+
+	ctrl = v4l2_ctrl_new_std(&radio->ctrl_handler, &si476x_ctrl_ops,
+				 V4L2_CID_RDS_RECEPTION,
+				 0, 1, 1, 1);
+	rval = radio->ctrl_handler.error;
+	if (ctrl == NULL && rval) {
+		dev_err(&pdev->dev, "Could not initialize V4L2_CID_RDS_RECEPTION control %d\n",
+			rval);
+		goto exit;
+	}
+
+	if (si476x_core_has_diversity(radio->core)) {
+		si476x_ctrls[SI476X_IDX_DIVERSITY_MODE].def =
+			si476x_phase_diversity_mode_to_idx(radio->core->diversity_mode);
+		si476x_radio_add_new_custom(radio, SI476X_IDX_DIVERSITY_MODE);
+		if (rval < 0)
+			goto exit;
+
+		si476x_radio_add_new_custom(radio, SI476X_IDX_INTERCHIP_LINK);
+		if (rval < 0)
+			goto exit;
+	}
+
+	/* register video device */
+	rval = video_register_device(&radio->videodev, VFL_TYPE_RADIO, -1);
+	if (rval < 0) {
+		dev_err(&pdev->dev, "Could not register video device\n");
+		goto exit;
+	}
+
+	rval = si476x_radio_init_debugfs(radio);
+	if (rval < 0) {
+		dev_err(&pdev->dev, "Could not creat debugfs interface\n");
+		goto exit;
+	}
+
+	return 0;
+exit:
+	v4l2_ctrl_handler_free(radio->videodev.ctrl_handler);
+	return rval;
+}
+
+static int si476x_radio_remove(struct platform_device *pdev)
+{
+	struct si476x_radio *radio = platform_get_drvdata(pdev);
+
+	v4l2_ctrl_handler_free(radio->videodev.ctrl_handler);
+	video_unregister_device(&radio->videodev);
+	v4l2_device_unregister(&radio->v4l2dev);
+	debugfs_remove_recursive(radio->debugfs);
+
+	return 0;
+}
+
+MODULE_ALIAS("platform:si476x-radio");
+
+static struct platform_driver si476x_radio_driver = {
+	.driver		= {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= si476x_radio_probe,
+	.remove		= si476x_radio_remove,
+};
+module_platform_driver(si476x_radio_driver);
+
+MODULE_AUTHOR("Andrey Smirnov <andrew.smirnov@gmail.com>");
+MODULE_DESCRIPTION("Driver for Si4761/64/68 AM/FM Radio MFD Cell");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/radio/radio-tea5764.c b/drivers/media/radio/radio-tea5764.c
index 1978516af67e..38d563d62595 100644
--- a/drivers/media/radio/radio-tea5764.c
+++ b/drivers/media/radio/radio-tea5764.c
@@ -339,7 +339,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *v)
+				const struct v4l2_tuner *v)
 {
 	struct tea5764_device *radio = video_drvdata(file);
 
@@ -351,7 +351,7 @@ static int vidioc_s_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct tea5764_device *radio = video_drvdata(file);
 
diff --git a/drivers/media/radio/radio-tea5777.c b/drivers/media/radio/radio-tea5777.c
index 4b5190d4fea5..e2455970725a 100644
--- a/drivers/media/radio/radio-tea5777.c
+++ b/drivers/media/radio/radio-tea5777.c
@@ -336,7 +336,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-					struct v4l2_tuner *v)
+					const struct v4l2_tuner *v)
 {
 	struct radio_tea5777 *tea = video_drvdata(file);
 	u32 orig_audmode = tea->audmode;
@@ -344,10 +344,9 @@ static int vidioc_s_tuner(struct file *file, void *priv,
 	if (v->index)
 		return -EINVAL;
 
-	if (v->audmode > V4L2_TUNER_MODE_STEREO)
-		v->audmode = V4L2_TUNER_MODE_STEREO;
-
 	tea->audmode = v->audmode;
+	if (tea->audmode > V4L2_TUNER_MODE_STEREO)
+		tea->audmode = V4L2_TUNER_MODE_STEREO;
 
 	if (tea->audmode != orig_audmode && tea->band == BAND_FM)
 		return radio_tea5777_set_freq(tea);
@@ -368,7 +367,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-					struct v4l2_frequency *f)
+					const struct v4l2_frequency *f)
 {
 	struct radio_tea5777 *tea = video_drvdata(file);
 
diff --git a/drivers/media/radio/radio-timb.c b/drivers/media/radio/radio-timb.c
index b87effeb5dc6..bb7b143b65d1 100644
--- a/drivers/media/radio/radio-timb.c
+++ b/drivers/media/radio/radio-timb.c
@@ -56,7 +56,7 @@ static int timbradio_vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int timbradio_vidioc_s_tuner(struct file *file, void *priv,
-	struct v4l2_tuner *v)
+	const struct v4l2_tuner *v)
 {
 	struct timbradio *tr = video_drvdata(file);
 	return v4l2_subdev_call(tr->sd_tuner, tuner, s_tuner, v);
@@ -91,7 +91,7 @@ static int timbradio_vidioc_s_audio(struct file *file, void *priv,
 }
 
 static int timbradio_vidioc_s_frequency(struct file *file, void *priv,
-	struct v4l2_frequency *f)
+	const struct v4l2_frequency *f)
 {
 	struct timbradio *tr = video_drvdata(file);
 	return v4l2_subdev_call(tr->sd_tuner, tuner, s_frequency, f);
diff --git a/drivers/media/radio/radio-wl1273.c b/drivers/media/radio/radio-wl1273.c
index 02151e0e6e63..97c2c18803ef 100644
--- a/drivers/media/radio/radio-wl1273.c
+++ b/drivers/media/radio/radio-wl1273.c
@@ -1559,7 +1559,7 @@ out:
 }
 
 static int wl1273_fm_vidioc_s_tuner(struct file *file, void *priv,
-				    struct v4l2_tuner *tuner)
+				    const struct v4l2_tuner *tuner)
 {
 	struct wl1273_device *radio = video_get_drvdata(video_devdata(file));
 	struct wl1273_core *core = radio->core;
@@ -1640,7 +1640,7 @@ static int wl1273_fm_vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int wl1273_fm_vidioc_s_frequency(struct file *file, void *priv,
-					struct v4l2_frequency *freq)
+					const struct v4l2_frequency *freq)
 {
 	struct wl1273_device *radio = video_get_drvdata(video_devdata(file));
 	struct wl1273_core *core = radio->core;
diff --git a/drivers/media/radio/si470x/radio-si470x-common.c b/drivers/media/radio/si470x/radio-si470x-common.c
index 18989388ddc1..5c57e5b0f949 100644
--- a/drivers/media/radio/si470x/radio-si470x-common.c
+++ b/drivers/media/radio/si470x/radio-si470x-common.c
@@ -636,7 +636,7 @@ static int si470x_vidioc_g_tuner(struct file *file, void *priv,
  * si470x_vidioc_s_tuner - set tuner attributes
  */
 static int si470x_vidioc_s_tuner(struct file *file, void *priv,
-		struct v4l2_tuner *tuner)
+		const struct v4l2_tuner *tuner)
 {
 	struct si470x_device *radio = video_drvdata(file);
 
@@ -678,7 +678,7 @@ static int si470x_vidioc_g_frequency(struct file *file, void *priv,
  * si470x_vidioc_s_frequency - set tuner or modulator radio frequency
  */
 static int si470x_vidioc_s_frequency(struct file *file, void *priv,
-		struct v4l2_frequency *freq)
+		const struct v4l2_frequency *freq)
 {
 	struct si470x_device *radio = video_drvdata(file);
 	int retval;
diff --git a/drivers/media/radio/si4713-i2c.c b/drivers/media/radio/si4713-i2c.c
index bd61b3bd0ca3..fe160882ee10 100644
--- a/drivers/media/radio/si4713-i2c.c
+++ b/drivers/media/radio/si4713-i2c.c
@@ -21,7 +21,6 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-#include <linux/mutex.h>
 #include <linux/completion.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
@@ -53,8 +52,6 @@ static const char *si4713_supply_names[SI4713_NUM_SUPPLIES] = {
 
 #define DEFAULT_RDS_PI			0x00
 #define DEFAULT_RDS_PTY			0x00
-#define DEFAULT_RDS_PS_NAME		""
-#define DEFAULT_RDS_RADIO_TEXT		DEFAULT_RDS_PS_NAME
 #define DEFAULT_RDS_DEVIATION		0x00C8
 #define DEFAULT_RDS_PS_REPEAT_COUNT	0x0003
 #define DEFAULT_LIMITER_RTIME		0x1392
@@ -108,7 +105,6 @@ static const char *si4713_supply_names[SI4713_NUM_SUPPLIES] = {
 					(status & SI4713_ERR))
 /* mute definition */
 #define set_mute(p)	((p & 1) | ((p & 1) << 1));
-#define get_mute(p)	(p & 0x01)
 
 #ifdef DEBUG
 #define DBG_BUFFER(device, message, buffer, size)			\
@@ -190,21 +186,6 @@ static int usecs_to_dev(unsigned long usecs, unsigned long const array[],
 	return rval;
 }
 
-static unsigned long dev_to_usecs(int value, unsigned long const array[],
-			int size)
-{
-	int i;
-	int rval = -EINVAL;
-
-	for (i = 0; i < size / 2; i++)
-		if (array[i * 2] == value) {
-			rval = array[(i * 2) + 1];
-			break;
-		}
-
-	return rval;
-}
-
 /* si4713_handler: IRQ handler, just complete work */
 static irqreturn_t si4713_handler(int irq, void *dev)
 {
@@ -458,15 +439,13 @@ static int si4713_checkrev(struct si4713_device *sdev)
 	int rval;
 	u8 resp[SI4713_GETREV_NRESP];
 
-	mutex_lock(&sdev->mutex);
-
 	rval = si4713_send_command(sdev, SI4713_CMD_GET_REV,
 					NULL, 0,
 					resp, ARRAY_SIZE(resp),
 					DEFAULT_TIMEOUT);
 
 	if (rval < 0)
-		goto unlock;
+		return rval;
 
 	if (resp[1] == SI4713_PRODUCT_NUMBER) {
 		v4l2_info(&sdev->sd, "chip found @ 0x%02x (%s)\n",
@@ -475,9 +454,6 @@ static int si4713_checkrev(struct si4713_device *sdev)
 		v4l2_err(&sdev->sd, "Invalid product number\n");
 		rval = -EINVAL;
 	}
-
-unlock:
-	mutex_unlock(&sdev->mutex);
 	return rval;
 }
 
@@ -778,17 +754,9 @@ static int si4713_tx_rds_ps(struct si4713_device *sdev, u8 psid,
 
 static int si4713_set_power_state(struct si4713_device *sdev, u8 value)
 {
-	int rval;
-
-	mutex_lock(&sdev->mutex);
-
 	if (value)
-		rval = si4713_powerup(sdev);
-	else
-		rval = si4713_powerdown(sdev);
-
-	mutex_unlock(&sdev->mutex);
-	return rval;
+		return si4713_powerup(sdev);
+	return si4713_powerdown(sdev);
 }
 
 static int si4713_set_mute(struct si4713_device *sdev, u16 mute)
@@ -797,17 +765,10 @@ static int si4713_set_mute(struct si4713_device *sdev, u16 mute)
 
 	mute = set_mute(mute);
 
-	mutex_lock(&sdev->mutex);
-
 	if (sdev->power_state)
 		rval = si4713_write_property(sdev,
 				SI4713_TX_LINE_INPUT_MUTE, mute);
 
-	if (rval >= 0)
-		sdev->mute = get_mute(mute);
-
-	mutex_unlock(&sdev->mutex);
-
 	return rval;
 }
 
@@ -820,15 +781,13 @@ static int si4713_set_rds_ps_name(struct si4713_device *sdev, char *ps_name)
 	if (!strlen(ps_name))
 		memset(ps_name, 0, MAX_RDS_PS_NAME + 1);
 
-	mutex_lock(&sdev->mutex);
-
 	if (sdev->power_state) {
 		/* Write the new ps name and clear the padding */
 		for (i = 0; i < MAX_RDS_PS_NAME; i += (RDS_BLOCK / 2)) {
 			rval = si4713_tx_rds_ps(sdev, (i / (RDS_BLOCK / 2)),
 						ps_name + i);
 			if (rval < 0)
-				goto unlock;
+				return rval;
 		}
 
 		/* Setup the size to be sent */
@@ -841,19 +800,15 @@ static int si4713_set_rds_ps_name(struct si4713_device *sdev, char *ps_name)
 				SI4713_TX_RDS_PS_MESSAGE_COUNT,
 				rds_ps_nblocks(len));
 		if (rval < 0)
-			goto unlock;
+			return rval;
 
 		rval = si4713_write_property(sdev,
 				SI4713_TX_RDS_PS_REPEAT_COUNT,
 				DEFAULT_RDS_PS_REPEAT_COUNT * 2);
 		if (rval < 0)
-			goto unlock;
+			return rval;
 	}
 
-	strncpy(sdev->rds_info.ps_name, ps_name, MAX_RDS_PS_NAME);
-
-unlock:
-	mutex_unlock(&sdev->mutex);
 	return rval;
 }
 
@@ -864,27 +819,24 @@ static int si4713_set_rds_radio_text(struct si4713_device *sdev, char *rt)
 	u8 b_index = 0, cr_inserted = 0;
 	s8 left;
 
-	mutex_lock(&sdev->mutex);
-
 	if (!sdev->power_state)
-		goto copy;
+		return rval;
 
 	rval = si4713_tx_rds_buff(sdev, RDS_BLOCK_CLEAR, 0, 0, 0, &left);
 	if (rval < 0)
-		goto unlock;
+		return rval;
 
 	if (!strlen(rt))
-		goto copy;
+		return rval;
 
 	do {
 		/* RDS spec says that if the last block isn't used,
 		 * then apply a carriage return
 		 */
-		if (t_index < (RDS_RADIOTEXT_INDEX_MAX *
-			RDS_RADIOTEXT_BLK_SIZE)) {
+		if (t_index < (RDS_RADIOTEXT_INDEX_MAX * RDS_RADIOTEXT_BLK_SIZE)) {
 			for (i = 0; i < RDS_RADIOTEXT_BLK_SIZE; i++) {
-				if (!rt[t_index + i] || rt[t_index + i] ==
-					RDS_CARRIAGE_RETURN) {
+				if (!rt[t_index + i] ||
+				    rt[t_index + i] == RDS_CARRIAGE_RETURN) {
 					rt[t_index + i] = RDS_CARRIAGE_RETURN;
 					cr_inserted = 1;
 					break;
@@ -898,7 +850,7 @@ static int si4713_set_rds_radio_text(struct si4713_device *sdev, char *rt)
 				compose_u16(rt[t_index + 2], rt[t_index + 3]),
 				&left);
 		if (rval < 0)
-			goto unlock;
+			return rval;
 
 		t_index += RDS_RADIOTEXT_BLK_SIZE;
 
@@ -906,16 +858,38 @@ static int si4713_set_rds_radio_text(struct si4713_device *sdev, char *rt)
 			break;
 	} while (left > 0);
 
-copy:
-	strncpy(sdev->rds_info.radio_text, rt, MAX_RDS_RADIO_TEXT);
+	return rval;
+}
 
-unlock:
-	mutex_unlock(&sdev->mutex);
+/*
+ * si4713_update_tune_status - update properties from tx_tune_status
+ * command. Must be called with sdev->mutex held.
+ * @sdev: si4713_device structure for the device we are communicating
+ */
+static int si4713_update_tune_status(struct si4713_device *sdev)
+{
+	int rval;
+	u16 f = 0;
+	u8 p = 0, a = 0, n = 0;
+
+	rval = si4713_tx_tune_status(sdev, 0x00, &f, &p, &a, &n);
+
+	if (rval < 0)
+		goto exit;
+
+/*	TODO: check that power_level and antenna_capacitor really are not
+	changed by the hardware. If they are, then these controls should become
+	volatiles.
+	sdev->power_level = p;
+	sdev->antenna_capacitor = a;*/
+	sdev->tune_rnl = n;
+
+exit:
 	return rval;
 }
 
 static int si4713_choose_econtrol_action(struct si4713_device *sdev, u32 id,
-		u32 **shadow, s32 *bit, s32 *mask, u16 *property, int *mul,
+		s32 *bit, s32 *mask, u16 *property, int *mul,
 		unsigned long **table, int *size)
 {
 	s32 rval = 0;
@@ -925,157 +899,71 @@ static int si4713_choose_econtrol_action(struct si4713_device *sdev, u32 id,
 	case V4L2_CID_RDS_TX_PI:
 		*property = SI4713_TX_RDS_PI;
 		*mul = 1;
-		*shadow = &sdev->rds_info.pi;
 		break;
 	case V4L2_CID_AUDIO_COMPRESSION_THRESHOLD:
 		*property = SI4713_TX_ACOMP_THRESHOLD;
 		*mul = 1;
-		*shadow = &sdev->acomp_info.threshold;
 		break;
 	case V4L2_CID_AUDIO_COMPRESSION_GAIN:
 		*property = SI4713_TX_ACOMP_GAIN;
 		*mul = 1;
-		*shadow = &sdev->acomp_info.gain;
 		break;
 	case V4L2_CID_PILOT_TONE_FREQUENCY:
 		*property = SI4713_TX_PILOT_FREQUENCY;
 		*mul = 1;
-		*shadow = &sdev->pilot_info.frequency;
 		break;
 	case V4L2_CID_AUDIO_COMPRESSION_ATTACK_TIME:
 		*property = SI4713_TX_ACOMP_ATTACK_TIME;
 		*mul = ATTACK_TIME_UNIT;
-		*shadow = &sdev->acomp_info.attack_time;
 		break;
 	case V4L2_CID_PILOT_TONE_DEVIATION:
 		*property = SI4713_TX_PILOT_DEVIATION;
 		*mul = 10;
-		*shadow = &sdev->pilot_info.deviation;
 		break;
 	case V4L2_CID_AUDIO_LIMITER_DEVIATION:
 		*property = SI4713_TX_AUDIO_DEVIATION;
 		*mul = 10;
-		*shadow = &sdev->limiter_info.deviation;
 		break;
 	case V4L2_CID_RDS_TX_DEVIATION:
 		*property = SI4713_TX_RDS_DEVIATION;
 		*mul = 1;
-		*shadow = &sdev->rds_info.deviation;
 		break;
 
 	case V4L2_CID_RDS_TX_PTY:
 		*property = SI4713_TX_RDS_PS_MISC;
 		*bit = 5;
 		*mask = 0x1F << 5;
-		*shadow = &sdev->rds_info.pty;
 		break;
 	case V4L2_CID_AUDIO_LIMITER_ENABLED:
 		*property = SI4713_TX_ACOMP_ENABLE;
 		*bit = 1;
 		*mask = 1 << 1;
-		*shadow = &sdev->limiter_info.enabled;
 		break;
 	case V4L2_CID_AUDIO_COMPRESSION_ENABLED:
 		*property = SI4713_TX_ACOMP_ENABLE;
 		*bit = 0;
 		*mask = 1 << 0;
-		*shadow = &sdev->acomp_info.enabled;
 		break;
 	case V4L2_CID_PILOT_TONE_ENABLED:
 		*property = SI4713_TX_COMPONENT_ENABLE;
 		*bit = 0;
 		*mask = 1 << 0;
-		*shadow = &sdev->pilot_info.enabled;
 		break;
 
 	case V4L2_CID_AUDIO_LIMITER_RELEASE_TIME:
 		*property = SI4713_TX_LIMITER_RELEASE_TIME;
 		*table = limiter_times;
 		*size = ARRAY_SIZE(limiter_times);
-		*shadow = &sdev->limiter_info.release_time;
 		break;
 	case V4L2_CID_AUDIO_COMPRESSION_RELEASE_TIME:
 		*property = SI4713_TX_ACOMP_RELEASE_TIME;
 		*table = acomp_rtimes;
 		*size = ARRAY_SIZE(acomp_rtimes);
-		*shadow = &sdev->acomp_info.release_time;
 		break;
 	case V4L2_CID_TUNE_PREEMPHASIS:
 		*property = SI4713_TX_PREEMPHASIS;
 		*table = preemphasis_values;
 		*size = ARRAY_SIZE(preemphasis_values);
-		*shadow = &sdev->preemphasis;
-		break;
-
-	default:
-		rval = -EINVAL;
-	}
-
-	return rval;
-}
-
-static int si4713_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc);
-
-/* write string property */
-static int si4713_write_econtrol_string(struct si4713_device *sdev,
-				struct v4l2_ext_control *control)
-{
-	struct v4l2_queryctrl vqc;
-	int len;
-	s32 rval = 0;
-
-	vqc.id = control->id;
-	rval = si4713_queryctrl(&sdev->sd, &vqc);
-	if (rval < 0)
-		goto exit;
-
-	switch (control->id) {
-	case V4L2_CID_RDS_TX_PS_NAME: {
-		char ps_name[MAX_RDS_PS_NAME + 1];
-
-		len = control->size - 1;
-		if (len < 0 || len > MAX_RDS_PS_NAME) {
-			rval = -ERANGE;
-			goto exit;
-		}
-		rval = copy_from_user(ps_name, control->string, len);
-		if (rval) {
-			rval = -EFAULT;
-			goto exit;
-		}
-		ps_name[len] = '\0';
-
-		if (strlen(ps_name) % vqc.step) {
-			rval = -ERANGE;
-			goto exit;
-		}
-
-		rval = si4713_set_rds_ps_name(sdev, ps_name);
-	}
-		break;
-
-	case V4L2_CID_RDS_TX_RADIO_TEXT: {
-		char radio_text[MAX_RDS_RADIO_TEXT + 1];
-
-		len = control->size - 1;
-		if (len < 0 || len > MAX_RDS_RADIO_TEXT) {
-			rval = -ERANGE;
-			goto exit;
-		}
-		rval = copy_from_user(radio_text, control->string, len);
-		if (rval) {
-			rval = -EFAULT;
-			goto exit;
-		}
-		radio_text[len] = '\0';
-
-		if (strlen(radio_text) % vqc.step) {
-			rval = -ERANGE;
-			goto exit;
-		}
-
-		rval = si4713_set_rds_radio_text(sdev, radio_text);
-	}
 		break;
 
 	default:
@@ -1083,136 +971,10 @@ static int si4713_write_econtrol_string(struct si4713_device *sdev,
 		break;
 	}
 
-exit:
-	return rval;
-}
-
-static int validate_range(struct v4l2_subdev *sd,
-					struct v4l2_ext_control *control)
-{
-	struct v4l2_queryctrl vqc;
-	int rval;
-
-	vqc.id = control->id;
-	rval = si4713_queryctrl(sd, &vqc);
-	if (rval < 0)
-		goto exit;
-
-	if (control->value < vqc.minimum || control->value > vqc.maximum)
-		rval = -ERANGE;
-
-exit:
-	return rval;
-}
-
-/* properties which use tx_tune_power*/
-static int si4713_write_econtrol_tune(struct si4713_device *sdev,
-				struct v4l2_ext_control *control)
-{
-	s32 rval = 0;
-	u8 power, antcap;
-
-	rval = validate_range(&sdev->sd, control);
-	if (rval < 0)
-		goto exit;
-
-	mutex_lock(&sdev->mutex);
-
-	switch (control->id) {
-	case V4L2_CID_TUNE_POWER_LEVEL:
-		power = control->value;
-		antcap = sdev->antenna_capacitor;
-		break;
-	case V4L2_CID_TUNE_ANTENNA_CAPACITOR:
-		power = sdev->power_level;
-		antcap = control->value;
-		break;
-	default:
-		rval = -EINVAL;
-		goto unlock;
-	}
-
-	if (sdev->power_state)
-		rval = si4713_tx_tune_power(sdev, power, antcap);
-
-	if (rval == 0) {
-		sdev->power_level = power;
-		sdev->antenna_capacitor = antcap;
-	}
-
-unlock:
-	mutex_unlock(&sdev->mutex);
-exit:
-	return rval;
-}
-
-static int si4713_write_econtrol_integers(struct si4713_device *sdev,
-					struct v4l2_ext_control *control)
-{
-	s32 rval;
-	u32 *shadow = NULL, val = 0;
-	s32 bit = 0, mask = 0;
-	u16 property = 0;
-	int mul = 0;
-	unsigned long *table = NULL;
-	int size = 0;
-
-	rval = validate_range(&sdev->sd, control);
-	if (rval < 0)
-		goto exit;
-
-	rval = si4713_choose_econtrol_action(sdev, control->id, &shadow, &bit,
-			&mask, &property, &mul, &table, &size);
-	if (rval < 0)
-		goto exit;
-
-	val = control->value;
-	if (mul) {
-		val = control->value / mul;
-	} else if (table) {
-		rval = usecs_to_dev(control->value, table, size);
-		if (rval < 0)
-			goto exit;
-		val = rval;
-		rval = 0;
-	}
-
-	mutex_lock(&sdev->mutex);
-
-	if (sdev->power_state) {
-		if (mask) {
-			rval = si4713_read_property(sdev, property, &val);
-			if (rval < 0)
-				goto unlock;
-			val = set_bits(val, control->value, bit, mask);
-		}
-
-		rval = si4713_write_property(sdev, property, val);
-		if (rval < 0)
-			goto unlock;
-		if (mask)
-			val = control->value;
-	}
-
-	if (mul) {
-		*shadow = val * mul;
-	} else if (table) {
-		rval = dev_to_usecs(val, table, size);
-		if (rval < 0)
-			goto unlock;
-		*shadow = rval;
-		rval = 0;
-	} else {
-		*shadow = val;
-	}
-
-unlock:
-	mutex_unlock(&sdev->mutex);
-exit:
 	return rval;
 }
 
-static int si4713_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f);
+static int si4713_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *f);
 static int si4713_s_modulator(struct v4l2_subdev *sd, const struct v4l2_modulator *);
 /*
  * si4713_setup - Sets the device up with current configuration.
@@ -1220,111 +982,25 @@ static int si4713_s_modulator(struct v4l2_subdev *sd, const struct v4l2_modulato
  */
 static int si4713_setup(struct si4713_device *sdev)
 {
-	struct v4l2_ext_control ctrl;
 	struct v4l2_frequency f;
 	struct v4l2_modulator vm;
-	struct si4713_device *tmp;
-	int rval = 0;
-
-	tmp = kmalloc(sizeof(*tmp), GFP_KERNEL);
-	if (!tmp)
-		return -ENOMEM;
-
-	/* Get a local copy to avoid race */
-	mutex_lock(&sdev->mutex);
-	memcpy(tmp, sdev, sizeof(*sdev));
-	mutex_unlock(&sdev->mutex);
-
-	ctrl.id = V4L2_CID_RDS_TX_PI;
-	ctrl.value = tmp->rds_info.pi;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_COMPRESSION_THRESHOLD;
-	ctrl.value = tmp->acomp_info.threshold;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_COMPRESSION_GAIN;
-	ctrl.value = tmp->acomp_info.gain;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_PILOT_TONE_FREQUENCY;
-	ctrl.value = tmp->pilot_info.frequency;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_COMPRESSION_ATTACK_TIME;
-	ctrl.value = tmp->acomp_info.attack_time;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_PILOT_TONE_DEVIATION;
-	ctrl.value = tmp->pilot_info.deviation;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_LIMITER_DEVIATION;
-	ctrl.value = tmp->limiter_info.deviation;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_RDS_TX_DEVIATION;
-	ctrl.value = tmp->rds_info.deviation;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_RDS_TX_PTY;
-	ctrl.value = tmp->rds_info.pty;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_LIMITER_ENABLED;
-	ctrl.value = tmp->limiter_info.enabled;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_COMPRESSION_ENABLED;
-	ctrl.value = tmp->acomp_info.enabled;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_PILOT_TONE_ENABLED;
-	ctrl.value = tmp->pilot_info.enabled;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_LIMITER_RELEASE_TIME;
-	ctrl.value = tmp->limiter_info.release_time;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_AUDIO_COMPRESSION_RELEASE_TIME;
-	ctrl.value = tmp->acomp_info.release_time;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_TUNE_PREEMPHASIS;
-	ctrl.value = tmp->preemphasis;
-	rval |= si4713_write_econtrol_integers(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_RDS_TX_PS_NAME;
-	rval |= si4713_set_rds_ps_name(sdev, tmp->rds_info.ps_name);
-
-	ctrl.id = V4L2_CID_RDS_TX_RADIO_TEXT;
-	rval |= si4713_set_rds_radio_text(sdev, tmp->rds_info.radio_text);
+	int rval;
 
 	/* Device procedure needs to set frequency first */
-	f.frequency = tmp->frequency ? tmp->frequency : DEFAULT_FREQUENCY;
+	f.tuner = 0;
+	f.frequency = sdev->frequency ? sdev->frequency : DEFAULT_FREQUENCY;
 	f.frequency = si4713_to_v4l2(f.frequency);
-	rval |= si4713_s_frequency(&sdev->sd, &f);
-
-	ctrl.id = V4L2_CID_TUNE_POWER_LEVEL;
-	ctrl.value = tmp->power_level;
-	rval |= si4713_write_econtrol_tune(sdev, &ctrl);
-
-	ctrl.id = V4L2_CID_TUNE_ANTENNA_CAPACITOR;
-	ctrl.value = tmp->antenna_capacitor;
-	rval |= si4713_write_econtrol_tune(sdev, &ctrl);
+	rval = si4713_s_frequency(&sdev->sd, &f);
 
 	vm.index = 0;
-	if (tmp->stereo)
+	if (sdev->stereo)
 		vm.txsubchans = V4L2_TUNER_SUB_STEREO;
 	else
 		vm.txsubchans = V4L2_TUNER_SUB_MONO;
-	if (tmp->rds_info.enabled)
+	if (sdev->rds_enabled)
 		vm.txsubchans |= V4L2_TUNER_SUB_RDS;
 	si4713_s_modulator(&sdev->sd, &vm);
 
-	kfree(tmp);
-
 	return rval;
 }
 
@@ -1338,434 +1014,126 @@ static int si4713_initialize(struct si4713_device *sdev)
 
 	rval = si4713_set_power_state(sdev, POWER_ON);
 	if (rval < 0)
-		goto exit;
+		return rval;
 
 	rval = si4713_checkrev(sdev);
 	if (rval < 0)
-		goto exit;
+		return rval;
 
 	rval = si4713_set_power_state(sdev, POWER_OFF);
 	if (rval < 0)
-		goto exit;
-
-	mutex_lock(&sdev->mutex);
-
-	sdev->rds_info.pi = DEFAULT_RDS_PI;
-	sdev->rds_info.pty = DEFAULT_RDS_PTY;
-	sdev->rds_info.deviation = DEFAULT_RDS_DEVIATION;
-	strlcpy(sdev->rds_info.ps_name, DEFAULT_RDS_PS_NAME, MAX_RDS_PS_NAME);
-	strlcpy(sdev->rds_info.radio_text, DEFAULT_RDS_RADIO_TEXT,
-							MAX_RDS_RADIO_TEXT);
-	sdev->rds_info.enabled = 1;
-
-	sdev->limiter_info.release_time = DEFAULT_LIMITER_RTIME;
-	sdev->limiter_info.deviation = DEFAULT_LIMITER_DEV;
-	sdev->limiter_info.enabled = 1;
-
-	sdev->pilot_info.deviation = DEFAULT_PILOT_DEVIATION;
-	sdev->pilot_info.frequency = DEFAULT_PILOT_FREQUENCY;
-	sdev->pilot_info.enabled = 1;
+		return rval;
 
-	sdev->acomp_info.release_time = DEFAULT_ACOMP_RTIME;
-	sdev->acomp_info.attack_time = DEFAULT_ACOMP_ATIME;
-	sdev->acomp_info.threshold = DEFAULT_ACOMP_THRESHOLD;
-	sdev->acomp_info.gain = DEFAULT_ACOMP_GAIN;
-	sdev->acomp_info.enabled = 1;
 
 	sdev->frequency = DEFAULT_FREQUENCY;
-	sdev->preemphasis = DEFAULT_PREEMPHASIS;
-	sdev->mute = DEFAULT_MUTE;
-	sdev->power_level = DEFAULT_POWER_LEVEL;
-	sdev->antenna_capacitor = 0;
 	sdev->stereo = 1;
 	sdev->tune_rnl = DEFAULT_TUNE_RNL;
-
-	mutex_unlock(&sdev->mutex);
-
-exit:
-	return rval;
-}
-
-/* read string property */
-static int si4713_read_econtrol_string(struct si4713_device *sdev,
-				struct v4l2_ext_control *control)
-{
-	s32 rval = 0;
-
-	switch (control->id) {
-	case V4L2_CID_RDS_TX_PS_NAME:
-		if (strlen(sdev->rds_info.ps_name) + 1 > control->size) {
-			control->size = MAX_RDS_PS_NAME + 1;
-			rval = -ENOSPC;
-			goto exit;
-		}
-		rval = copy_to_user(control->string, sdev->rds_info.ps_name,
-					strlen(sdev->rds_info.ps_name) + 1);
-		if (rval)
-			rval = -EFAULT;
-		break;
-
-	case V4L2_CID_RDS_TX_RADIO_TEXT:
-		if (strlen(sdev->rds_info.radio_text) + 1 > control->size) {
-			control->size = MAX_RDS_RADIO_TEXT + 1;
-			rval = -ENOSPC;
-			goto exit;
-		}
-		rval = copy_to_user(control->string, sdev->rds_info.radio_text,
-					strlen(sdev->rds_info.radio_text) + 1);
-		if (rval)
-			rval = -EFAULT;
-		break;
-
-	default:
-		rval = -EINVAL;
-		break;
-	}
-
-exit:
-	return rval;
-}
-
-/*
- * si4713_update_tune_status - update properties from tx_tune_status
- * command. Must be called with sdev->mutex held.
- * @sdev: si4713_device structure for the device we are communicating
- */
-static int si4713_update_tune_status(struct si4713_device *sdev)
-{
-	int rval;
-	u16 f = 0;
-	u8 p = 0, a = 0, n = 0;
-
-	rval = si4713_tx_tune_status(sdev, 0x00, &f, &p, &a, &n);
-
-	if (rval < 0)
-		goto exit;
-
-	sdev->power_level = p;
-	sdev->antenna_capacitor = a;
-	sdev->tune_rnl = n;
-
-exit:
-	return rval;
-}
-
-/* properties which use tx_tune_status */
-static int si4713_read_econtrol_tune(struct si4713_device *sdev,
-				struct v4l2_ext_control *control)
-{
-	s32 rval = 0;
-
-	mutex_lock(&sdev->mutex);
-
-	if (sdev->power_state) {
-		rval = si4713_update_tune_status(sdev);
-		if (rval < 0)
-			goto unlock;
-	}
-
-	switch (control->id) {
-	case V4L2_CID_TUNE_POWER_LEVEL:
-		control->value = sdev->power_level;
-		break;
-	case V4L2_CID_TUNE_ANTENNA_CAPACITOR:
-		control->value = sdev->antenna_capacitor;
-		break;
-	default:
-		rval = -EINVAL;
-	}
-
-unlock:
-	mutex_unlock(&sdev->mutex);
-	return rval;
+	return 0;
 }
 
-static int si4713_read_econtrol_integers(struct si4713_device *sdev,
-				struct v4l2_ext_control *control)
+/* si4713_s_ctrl - set the value of a control */
+static int si4713_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	s32 rval;
-	u32 *shadow = NULL, val = 0;
+	struct si4713_device *sdev =
+		container_of(ctrl->handler, struct si4713_device, ctrl_handler);
+	u32 val = 0;
 	s32 bit = 0, mask = 0;
 	u16 property = 0;
 	int mul = 0;
 	unsigned long *table = NULL;
 	int size = 0;
+	bool force = false;
+	int c;
+	int ret = 0;
 
-	rval = si4713_choose_econtrol_action(sdev, control->id, &shadow, &bit,
-			&mask, &property, &mul, &table, &size);
-	if (rval < 0)
-		goto exit;
-
-	mutex_lock(&sdev->mutex);
-
-	if (sdev->power_state) {
-		rval = si4713_read_property(sdev, property, &val);
-		if (rval < 0)
-			goto unlock;
-
-		/* Keep negative values for threshold */
-		if (control->id == V4L2_CID_AUDIO_COMPRESSION_THRESHOLD)
-			*shadow = (s16)val;
-		else if (mask)
-			*shadow = get_status_bit(val, bit, mask);
-		else if (mul)
-			*shadow = val * mul;
-		else
-			*shadow = dev_to_usecs(val, table, size);
-	}
-
-	control->value = *shadow;
-
-unlock:
-	mutex_unlock(&sdev->mutex);
-exit:
-	return rval;
-}
-
-/*
- * Video4Linux Subdev Interface
- */
-/* si4713_s_ext_ctrls - set extended controls value */
-static int si4713_s_ext_ctrls(struct v4l2_subdev *sd,
-				struct v4l2_ext_controls *ctrls)
-{
-	struct si4713_device *sdev = to_si4713_device(sd);
-	int i;
-
-	if (ctrls->ctrl_class != V4L2_CTRL_CLASS_FM_TX)
+	if (ctrl->id != V4L2_CID_AUDIO_MUTE)
 		return -EINVAL;
-
-	for (i = 0; i < ctrls->count; i++) {
-		int err;
-
-		switch ((ctrls->controls + i)->id) {
-		case V4L2_CID_RDS_TX_PS_NAME:
-		case V4L2_CID_RDS_TX_RADIO_TEXT:
-			err = si4713_write_econtrol_string(sdev,
-							ctrls->controls + i);
-			break;
-		case V4L2_CID_TUNE_ANTENNA_CAPACITOR:
-		case V4L2_CID_TUNE_POWER_LEVEL:
-			err = si4713_write_econtrol_tune(sdev,
-							ctrls->controls + i);
-			break;
-		default:
-			err = si4713_write_econtrol_integers(sdev,
-							ctrls->controls + i);
-		}
-
-		if (err < 0) {
-			ctrls->error_idx = i;
-			return err;
+	if (ctrl->is_new) {
+		if (ctrl->val) {
+			ret = si4713_set_mute(sdev, ctrl->val);
+			if (!ret)
+				ret = si4713_set_power_state(sdev, POWER_DOWN);
+			return ret;
 		}
+		ret = si4713_set_power_state(sdev, POWER_UP);
+		if (!ret)
+			ret = si4713_set_mute(sdev, ctrl->val);
+		if (!ret)
+			ret = si4713_setup(sdev);
+		if (ret)
+			return ret;
+		force = true;
 	}
 
-	return 0;
-}
+	if (!sdev->power_state)
+		return 0;
 
-/* si4713_g_ext_ctrls - get extended controls value */
-static int si4713_g_ext_ctrls(struct v4l2_subdev *sd,
-				struct v4l2_ext_controls *ctrls)
-{
-	struct si4713_device *sdev = to_si4713_device(sd);
-	int i;
+	for (c = 1; !ret && c < ctrl->ncontrols; c++) {
+		ctrl = ctrl->cluster[c];
 
-	if (ctrls->ctrl_class != V4L2_CTRL_CLASS_FM_TX)
-		return -EINVAL;
+		if (!force && !ctrl->is_new)
+			continue;
 
-	for (i = 0; i < ctrls->count; i++) {
-		int err;
-
-		switch ((ctrls->controls + i)->id) {
+		switch (ctrl->id) {
 		case V4L2_CID_RDS_TX_PS_NAME:
+			ret = si4713_set_rds_ps_name(sdev, ctrl->string);
+			break;
+
 		case V4L2_CID_RDS_TX_RADIO_TEXT:
-			err = si4713_read_econtrol_string(sdev,
-							ctrls->controls + i);
+			ret = si4713_set_rds_radio_text(sdev, ctrl->string);
 			break;
+
 		case V4L2_CID_TUNE_ANTENNA_CAPACITOR:
+			/* don't handle this control if we force setting all
+			 * controls since in that case it will be handled by
+			 * V4L2_CID_TUNE_POWER_LEVEL. */
+			if (force)
+				break;
+			/* fall through */
 		case V4L2_CID_TUNE_POWER_LEVEL:
-			err = si4713_read_econtrol_tune(sdev,
-							ctrls->controls + i);
+			ret = si4713_tx_tune_power(sdev,
+				sdev->tune_pwr_level->val, sdev->tune_ant_cap->val);
+			if (!ret) {
+				/* Make sure we don't set this twice */
+				sdev->tune_ant_cap->is_new = false;
+				sdev->tune_pwr_level->is_new = false;
+			}
 			break;
-		default:
-			err = si4713_read_econtrol_integers(sdev,
-							ctrls->controls + i);
-		}
-
-		if (err < 0) {
-			ctrls->error_idx = i;
-			return err;
-		}
-	}
-
-	return 0;
-}
-
-/* si4713_queryctrl - enumerate control items */
-static int si4713_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
-{
-	int rval = 0;
-
-	switch (qc->id) {
-	/* User class controls */
-	case V4L2_CID_AUDIO_MUTE:
-		rval = v4l2_ctrl_query_fill(qc, 0, 1, 1, DEFAULT_MUTE);
-		break;
-	/* FM_TX class controls */
-	case V4L2_CID_RDS_TX_PI:
-		rval = v4l2_ctrl_query_fill(qc, 0, 0xFFFF, 1, DEFAULT_RDS_PI);
-		break;
-	case V4L2_CID_RDS_TX_PTY:
-		rval = v4l2_ctrl_query_fill(qc, 0, 31, 1, DEFAULT_RDS_PTY);
-		break;
-	case V4L2_CID_RDS_TX_DEVIATION:
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_RDS_DEVIATION,
-						10, DEFAULT_RDS_DEVIATION);
-		break;
-	case V4L2_CID_RDS_TX_PS_NAME:
-		/*
-		 * Report step as 8. From RDS spec, psname
-		 * should be 8. But there are receivers which scroll strings
-		 * sized as 8xN.
-		 */
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_RDS_PS_NAME, 8, 0);
-		break;
-	case V4L2_CID_RDS_TX_RADIO_TEXT:
-		/*
-		 * Report step as 32 (2A block). From RDS spec,
-		 * radio text should be 32 for 2A block. But there are receivers
-		 * which scroll strings sized as 32xN. Setting default to 32.
-		 */
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_RDS_RADIO_TEXT, 32, 0);
-		break;
-
-	case V4L2_CID_AUDIO_LIMITER_ENABLED:
-		rval = v4l2_ctrl_query_fill(qc, 0, 1, 1, 1);
-		break;
-	case V4L2_CID_AUDIO_LIMITER_RELEASE_TIME:
-		rval = v4l2_ctrl_query_fill(qc, 250, MAX_LIMITER_RELEASE_TIME,
-						50, DEFAULT_LIMITER_RTIME);
-		break;
-	case V4L2_CID_AUDIO_LIMITER_DEVIATION:
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_LIMITER_DEVIATION,
-						10, DEFAULT_LIMITER_DEV);
-		break;
-
-	case V4L2_CID_AUDIO_COMPRESSION_ENABLED:
-		rval = v4l2_ctrl_query_fill(qc, 0, 1, 1, 1);
-		break;
-	case V4L2_CID_AUDIO_COMPRESSION_GAIN:
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_ACOMP_GAIN, 1,
-						DEFAULT_ACOMP_GAIN);
-		break;
-	case V4L2_CID_AUDIO_COMPRESSION_THRESHOLD:
-		rval = v4l2_ctrl_query_fill(qc, MIN_ACOMP_THRESHOLD,
-						MAX_ACOMP_THRESHOLD, 1,
-						DEFAULT_ACOMP_THRESHOLD);
-		break;
-	case V4L2_CID_AUDIO_COMPRESSION_ATTACK_TIME:
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_ACOMP_ATTACK_TIME,
-						500, DEFAULT_ACOMP_ATIME);
-		break;
-	case V4L2_CID_AUDIO_COMPRESSION_RELEASE_TIME:
-		rval = v4l2_ctrl_query_fill(qc, 100000, MAX_ACOMP_RELEASE_TIME,
-						100000, DEFAULT_ACOMP_RTIME);
-		break;
-
-	case V4L2_CID_PILOT_TONE_ENABLED:
-		rval = v4l2_ctrl_query_fill(qc, 0, 1, 1, 1);
-		break;
-	case V4L2_CID_PILOT_TONE_DEVIATION:
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_PILOT_DEVIATION,
-						10, DEFAULT_PILOT_DEVIATION);
-		break;
-	case V4L2_CID_PILOT_TONE_FREQUENCY:
-		rval = v4l2_ctrl_query_fill(qc, 0, MAX_PILOT_FREQUENCY,
-						1, DEFAULT_PILOT_FREQUENCY);
-		break;
-
-	case V4L2_CID_TUNE_PREEMPHASIS:
-		rval = v4l2_ctrl_query_fill(qc, V4L2_PREEMPHASIS_DISABLED,
-						V4L2_PREEMPHASIS_75_uS, 1,
-						V4L2_PREEMPHASIS_50_uS);
-		break;
-	case V4L2_CID_TUNE_POWER_LEVEL:
-		rval = v4l2_ctrl_query_fill(qc, 0, 120, 1, DEFAULT_POWER_LEVEL);
-		break;
-	case V4L2_CID_TUNE_ANTENNA_CAPACITOR:
-		rval = v4l2_ctrl_query_fill(qc, 0, 191, 1, 0);
-		break;
-	default:
-		rval = -EINVAL;
-		break;
-	}
-
-	return rval;
-}
-
-/* si4713_g_ctrl - get the value of a control */
-static int si4713_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct si4713_device *sdev = to_si4713_device(sd);
-	int rval = 0;
-
-	if (!sdev)
-		return -ENODEV;
-
-	mutex_lock(&sdev->mutex);
-
-	if (sdev->power_state) {
-		rval = si4713_read_property(sdev, SI4713_TX_LINE_INPUT_MUTE,
-						&sdev->mute);
-
-		if (rval < 0)
-			goto unlock;
-	}
-
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_MUTE:
-		ctrl->value = get_mute(sdev->mute);
-		break;
-	}
-
-unlock:
-	mutex_unlock(&sdev->mutex);
-	return rval;
-}
-
-/* si4713_s_ctrl - set the value of a control */
-static int si4713_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct si4713_device *sdev = to_si4713_device(sd);
-	int rval = 0;
-
-	if (!sdev)
-		return -ENODEV;
-
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_MUTE:
-		if (ctrl->value) {
-			rval = si4713_set_mute(sdev, ctrl->value);
-			if (rval < 0)
-				goto exit;
 
-			rval = si4713_set_power_state(sdev, POWER_DOWN);
-		} else {
-			rval = si4713_set_power_state(sdev, POWER_UP);
-			if (rval < 0)
-				goto exit;
+		default:
+			ret = si4713_choose_econtrol_action(sdev, ctrl->id, &bit,
+					&mask, &property, &mul, &table, &size);
+			if (ret < 0)
+				break;
+
+			val = ctrl->val;
+			if (mul) {
+				val = val / mul;
+			} else if (table) {
+				ret = usecs_to_dev(val, table, size);
+				if (ret < 0)
+					break;
+				val = ret;
+				ret = 0;
+			}
 
-			rval = si4713_setup(sdev);
-			if (rval < 0)
-				goto exit;
+			if (mask) {
+				ret = si4713_read_property(sdev, property, &val);
+				if (ret < 0)
+					break;
+				val = set_bits(val, ctrl->val, bit, mask);
+			}
 
-			rval = si4713_set_mute(sdev, ctrl->value);
+			ret = si4713_write_property(sdev, property, val);
+			if (ret < 0)
+				break;
+			if (mask)
+				val = ctrl->val;
+			break;
 		}
-		break;
 	}
 
-exit:
-	return rval;
+	return ret;
 }
 
 /* si4713_ioctl - deal with private ioctls (only rnl for now) */
@@ -1779,7 +1147,6 @@ static long si4713_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
 	if (!arg)
 		return -EINVAL;
 
-	mutex_lock(&sdev->mutex);
 	switch (cmd) {
 	case SI4713_IOC_MEASURE_RNL:
 		frequency = v4l2_to_si4713(rnl->frequency);
@@ -1788,11 +1155,11 @@ static long si4713_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
 			/* Set desired measurement frequency */
 			rval = si4713_tx_tune_measure(sdev, frequency, 0);
 			if (rval < 0)
-				goto unlock;
+				return rval;
 			/* get results from tune status */
 			rval = si4713_update_tune_status(sdev);
 			if (rval < 0)
-				goto unlock;
+				return rval;
 		}
 		rnl->rnl = sdev->tune_rnl;
 		break;
@@ -1802,35 +1169,20 @@ static long si4713_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
 		rval = -ENOIOCTLCMD;
 	}
 
-unlock:
-	mutex_unlock(&sdev->mutex);
 	return rval;
 }
 
-static const struct v4l2_subdev_core_ops si4713_subdev_core_ops = {
-	.queryctrl	= si4713_queryctrl,
-	.g_ext_ctrls	= si4713_g_ext_ctrls,
-	.s_ext_ctrls	= si4713_s_ext_ctrls,
-	.g_ctrl		= si4713_g_ctrl,
-	.s_ctrl		= si4713_s_ctrl,
-	.ioctl		= si4713_ioctl,
-};
-
 /* si4713_g_modulator - get modulator attributes */
 static int si4713_g_modulator(struct v4l2_subdev *sd, struct v4l2_modulator *vm)
 {
 	struct si4713_device *sdev = to_si4713_device(sd);
 	int rval = 0;
 
-	if (!sdev) {
-		rval = -ENODEV;
-		goto exit;
-	}
+	if (!sdev)
+		return -ENODEV;
 
-	if (vm->index > 0) {
-		rval = -EINVAL;
-		goto exit;
-	}
+	if (vm->index > 0)
+		return -EINVAL;
 
 	strncpy(vm->name, "FM Modulator", 32);
 	vm->capability = V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_LOW |
@@ -1840,18 +1192,15 @@ static int si4713_g_modulator(struct v4l2_subdev *sd, struct v4l2_modulator *vm)
 	vm->rangelow = si4713_to_v4l2(FREQ_RANGE_LOW);
 	vm->rangehigh = si4713_to_v4l2(FREQ_RANGE_HIGH);
 
-	mutex_lock(&sdev->mutex);
-
 	if (sdev->power_state) {
 		u32 comp_en = 0;
 
 		rval = si4713_read_property(sdev, SI4713_TX_COMPONENT_ENABLE,
 						&comp_en);
 		if (rval < 0)
-			goto unlock;
+			return rval;
 
 		sdev->stereo = get_status_bit(comp_en, 1, 1 << 1);
-		sdev->rds_info.enabled = get_status_bit(comp_en, 2, 1 << 2);
 	}
 
 	/* Report current audio mode: mono or stereo */
@@ -1861,14 +1210,11 @@ static int si4713_g_modulator(struct v4l2_subdev *sd, struct v4l2_modulator *vm)
 		vm->txsubchans = V4L2_TUNER_SUB_MONO;
 
 	/* Report rds feature status */
-	if (sdev->rds_info.enabled)
+	if (sdev->rds_enabled)
 		vm->txsubchans |= V4L2_TUNER_SUB_RDS;
 	else
 		vm->txsubchans &= ~V4L2_TUNER_SUB_RDS;
 
-unlock:
-	mutex_unlock(&sdev->mutex);
-exit:
 	return rval;
 }
 
@@ -1896,13 +1242,11 @@ static int si4713_s_modulator(struct v4l2_subdev *sd, const struct v4l2_modulato
 
 	rds = !!(vm->txsubchans & V4L2_TUNER_SUB_RDS);
 
-	mutex_lock(&sdev->mutex);
-
 	if (sdev->power_state) {
 		rval = si4713_read_property(sdev,
 						SI4713_TX_COMPONENT_ENABLE, &p);
 		if (rval < 0)
-			goto unlock;
+			return rval;
 
 		p = set_bits(p, stereo, 1, 1 << 1);
 		p = set_bits(p, rds, 2, 1 << 2);
@@ -1910,14 +1254,12 @@ static int si4713_s_modulator(struct v4l2_subdev *sd, const struct v4l2_modulato
 		rval = si4713_write_property(sdev,
 						SI4713_TX_COMPONENT_ENABLE, p);
 		if (rval < 0)
-			goto unlock;
+			return rval;
 	}
 
 	sdev->stereo = stereo;
-	sdev->rds_info.enabled = rds;
+	sdev->rds_enabled = rds;
 
-unlock:
-	mutex_unlock(&sdev->mutex);
 	return rval;
 }
 
@@ -1927,9 +1269,8 @@ static int si4713_g_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
 	struct si4713_device *sdev = to_si4713_device(sd);
 	int rval = 0;
 
-	f->type = V4L2_TUNER_RADIO;
-
-	mutex_lock(&sdev->mutex);
+	if (f->tuner)
+		return -EINVAL;
 
 	if (sdev->power_state) {
 		u16 freq;
@@ -1937,46 +1278,49 @@ static int si4713_g_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
 
 		rval = si4713_tx_tune_status(sdev, 0x00, &freq, &p, &a, &n);
 		if (rval < 0)
-			goto unlock;
+			return rval;
 
 		sdev->frequency = freq;
 	}
 
 	f->frequency = si4713_to_v4l2(sdev->frequency);
 
-unlock:
-	mutex_unlock(&sdev->mutex);
 	return rval;
 }
 
 /* si4713_s_frequency - set tuner or modulator radio frequency */
-static int si4713_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
+static int si4713_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *f)
 {
 	struct si4713_device *sdev = to_si4713_device(sd);
 	int rval = 0;
 	u16 frequency = v4l2_to_si4713(f->frequency);
 
-	/* Check frequency range */
-	if (frequency < FREQ_RANGE_LOW || frequency > FREQ_RANGE_HIGH)
-		return -EDOM;
+	if (f->tuner)
+		return -EINVAL;
 
-	mutex_lock(&sdev->mutex);
+	/* Check frequency range */
+	frequency = clamp_t(u16, frequency, FREQ_RANGE_LOW, FREQ_RANGE_HIGH);
 
 	if (sdev->power_state) {
 		rval = si4713_tx_tune_freq(sdev, frequency);
 		if (rval < 0)
-			goto unlock;
+			return rval;
 		frequency = rval;
 		rval = 0;
 	}
 	sdev->frequency = frequency;
-	f->frequency = si4713_to_v4l2(frequency);
 
-unlock:
-	mutex_unlock(&sdev->mutex);
 	return rval;
 }
 
+static const struct v4l2_ctrl_ops si4713_ctrl_ops = {
+	.s_ctrl = si4713_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops si4713_subdev_core_ops = {
+	.ioctl		= si4713_ioctl,
+};
+
 static const struct v4l2_subdev_tuner_ops si4713_subdev_tuner_ops = {
 	.g_frequency	= si4713_g_frequency,
 	.s_frequency	= si4713_s_frequency,
@@ -1998,6 +1342,7 @@ static int si4713_probe(struct i2c_client *client,
 {
 	struct si4713_device *sdev;
 	struct si4713_platform_data *pdata = client->dev.platform_data;
+	struct v4l2_ctrl_handler *hdl;
 	int rval, i;
 
 	sdev = kzalloc(sizeof *sdev, GFP_KERNEL);
@@ -2031,9 +1376,84 @@ static int si4713_probe(struct i2c_client *client,
 
 	v4l2_i2c_subdev_init(&sdev->sd, client, &si4713_subdev_ops);
 
-	mutex_init(&sdev->mutex);
 	init_completion(&sdev->work);
 
+	hdl = &sdev->ctrl_handler;
+	v4l2_ctrl_handler_init(hdl, 20);
+	sdev->mute = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, DEFAULT_MUTE);
+
+	sdev->rds_pi = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_RDS_TX_PI, 0, 0xffff, 1, DEFAULT_RDS_PI);
+	sdev->rds_pty = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_RDS_TX_PTY, 0, 31, 1, DEFAULT_RDS_PTY);
+	sdev->rds_deviation = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_RDS_TX_DEVIATION, 0, MAX_RDS_DEVIATION,
+			10, DEFAULT_RDS_DEVIATION);
+	/*
+	 * Report step as 8. From RDS spec, psname
+	 * should be 8. But there are receivers which scroll strings
+	 * sized as 8xN.
+	 */
+	sdev->rds_ps_name = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_RDS_TX_PS_NAME, 0, MAX_RDS_PS_NAME, 8, 0);
+	/*
+	 * Report step as 32 (2A block). From RDS spec,
+	 * radio text should be 32 for 2A block. But there are receivers
+	 * which scroll strings sized as 32xN. Setting default to 32.
+	 */
+	sdev->rds_radio_text = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_RDS_TX_RADIO_TEXT, 0, MAX_RDS_RADIO_TEXT, 32, 0);
+
+	sdev->limiter_enabled = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_LIMITER_ENABLED, 0, 1, 1, 1);
+	sdev->limiter_release_time = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_LIMITER_RELEASE_TIME, 250,
+			MAX_LIMITER_RELEASE_TIME, 10, DEFAULT_LIMITER_RTIME);
+	sdev->limiter_deviation = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_LIMITER_DEVIATION, 0,
+			MAX_LIMITER_DEVIATION, 10, DEFAULT_LIMITER_DEV);
+
+	sdev->compression_enabled = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_COMPRESSION_ENABLED, 0, 1, 1, 1);
+	sdev->compression_gain = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_COMPRESSION_GAIN, 0, MAX_ACOMP_GAIN, 1,
+			DEFAULT_ACOMP_GAIN);
+	sdev->compression_threshold = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_COMPRESSION_THRESHOLD, MIN_ACOMP_THRESHOLD,
+			MAX_ACOMP_THRESHOLD, 1,
+			DEFAULT_ACOMP_THRESHOLD);
+	sdev->compression_attack_time = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_COMPRESSION_ATTACK_TIME, 0,
+			MAX_ACOMP_ATTACK_TIME, 500, DEFAULT_ACOMP_ATIME);
+	sdev->compression_release_time = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_AUDIO_COMPRESSION_RELEASE_TIME, 100000,
+			MAX_ACOMP_RELEASE_TIME, 100000, DEFAULT_ACOMP_RTIME);
+
+	sdev->pilot_tone_enabled = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_PILOT_TONE_ENABLED, 0, 1, 1, 1);
+	sdev->pilot_tone_deviation = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_PILOT_TONE_DEVIATION, 0, MAX_PILOT_DEVIATION,
+			10, DEFAULT_PILOT_DEVIATION);
+	sdev->pilot_tone_freq = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_PILOT_TONE_FREQUENCY, 0, MAX_PILOT_FREQUENCY,
+			1, DEFAULT_PILOT_FREQUENCY);
+
+	sdev->tune_preemphasis = v4l2_ctrl_new_std_menu(hdl, &si4713_ctrl_ops,
+			V4L2_CID_TUNE_PREEMPHASIS,
+			V4L2_PREEMPHASIS_75_uS, 0, V4L2_PREEMPHASIS_50_uS);
+	sdev->tune_pwr_level = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_TUNE_POWER_LEVEL, 0, 120, 1, DEFAULT_POWER_LEVEL);
+	sdev->tune_ant_cap = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
+			V4L2_CID_TUNE_ANTENNA_CAPACITOR, 0, 191, 1, 0);
+
+	if (hdl->error) {
+		rval = hdl->error;
+		goto free_ctrls;
+	}
+	v4l2_ctrl_cluster(20, &sdev->mute);
+	sdev->sd.ctrl_handler = hdl;
+
 	if (client->irq) {
 		rval = request_irq(client->irq,
 			si4713_handler, IRQF_TRIGGER_FALLING | IRQF_DISABLED,
@@ -2058,6 +1478,8 @@ static int si4713_probe(struct i2c_client *client,
 free_irq:
 	if (client->irq)
 		free_irq(client->irq, sdev);
+free_ctrls:
+	v4l2_ctrl_handler_free(hdl);
 put_reg:
 	regulator_bulk_free(ARRAY_SIZE(sdev->supplies), sdev->supplies);
 free_gpio:
@@ -2082,6 +1504,7 @@ static int si4713_remove(struct i2c_client *client)
 		free_irq(client->irq, sdev);
 
 	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
 	regulator_bulk_free(ARRAY_SIZE(sdev->supplies), sdev->supplies);
 	if (gpio_is_valid(sdev->gpio_reset))
 		gpio_free(sdev->gpio_reset);
diff --git a/drivers/media/radio/si4713-i2c.h b/drivers/media/radio/si4713-i2c.h
index c6dfa7fb101c..25cdea26343b 100644
--- a/drivers/media/radio/si4713-i2c.h
+++ b/drivers/media/radio/si4713-i2c.h
@@ -16,6 +16,7 @@
 #define SI4713_I2C_H
 
 #include <media/v4l2-subdev.h>
+#include <media/v4l2-ctrls.h>
 #include <media/si4713.h>
 
 #define SI4713_PRODUCT_NUMBER		0x0D
@@ -160,56 +161,33 @@
 #define POWER_UP			0x01
 #define POWER_DOWN			0x00
 
-struct rds_info {
-	u32 pi;
 #define MAX_RDS_PTY			31
-	u32 pty;
 #define MAX_RDS_DEVIATION		90000
-	u32 deviation;
+
 /*
  * PSNAME is known to be defined as 8 character sized (RDS Spec).
  * However, there is receivers which scroll PSNAME 8xN sized.
  */
 #define MAX_RDS_PS_NAME			96
-	u8 ps_name[MAX_RDS_PS_NAME + 1];
+
 /*
  * MAX_RDS_RADIO_TEXT is known to be defined as 32 (2A group) or 64 (2B group)
  * character sized (RDS Spec).
  * However, there is receivers which scroll them as well.
  */
 #define MAX_RDS_RADIO_TEXT		384
-	u8 radio_text[MAX_RDS_RADIO_TEXT + 1];
-	u32 enabled;
-};
 
-struct limiter_info {
 #define MAX_LIMITER_RELEASE_TIME	102390
-	u32 release_time;
 #define MAX_LIMITER_DEVIATION		90000
-	u32 deviation;
-	u32 enabled;
-};
 
-struct pilot_info {
 #define MAX_PILOT_DEVIATION		90000
-	u32 deviation;
 #define MAX_PILOT_FREQUENCY		19000
-	u32 frequency;
-	u32 enabled;
-};
 
-struct acomp_info {
 #define MAX_ACOMP_RELEASE_TIME		1000000
-	u32 release_time;
 #define MAX_ACOMP_ATTACK_TIME		5000
-	u32 attack_time;
 #define MAX_ACOMP_THRESHOLD		0
 #define MIN_ACOMP_THRESHOLD		(-40)
-	s32 threshold;
 #define MAX_ACOMP_GAIN			20
-	u32 gain;
-	u32 enabled;
-};
 
 #define SI4713_NUM_SUPPLIES		2
 
@@ -219,21 +197,41 @@ struct acomp_info {
 struct si4713_device {
 	/* v4l2_subdev and i2c reference (v4l2_subdev priv data) */
 	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler ctrl_handler;
 	/* private data structures */
-	struct mutex mutex;
+	struct { /* si4713 control cluster */
+		/* This is one big cluster since the mute control
+		 * powers off the device and after unmuting again all
+		 * controls need to be set at once. The only way of doing
+		 * that is by making it one big cluster. */
+		struct v4l2_ctrl *mute;
+		struct v4l2_ctrl *rds_ps_name;
+		struct v4l2_ctrl *rds_radio_text;
+		struct v4l2_ctrl *rds_pi;
+		struct v4l2_ctrl *rds_deviation;
+		struct v4l2_ctrl *rds_pty;
+		struct v4l2_ctrl *compression_enabled;
+		struct v4l2_ctrl *compression_threshold;
+		struct v4l2_ctrl *compression_gain;
+		struct v4l2_ctrl *compression_attack_time;
+		struct v4l2_ctrl *compression_release_time;
+		struct v4l2_ctrl *pilot_tone_enabled;
+		struct v4l2_ctrl *pilot_tone_freq;
+		struct v4l2_ctrl *pilot_tone_deviation;
+		struct v4l2_ctrl *limiter_enabled;
+		struct v4l2_ctrl *limiter_deviation;
+		struct v4l2_ctrl *limiter_release_time;
+		struct v4l2_ctrl *tune_preemphasis;
+		struct v4l2_ctrl *tune_pwr_level;
+		struct v4l2_ctrl *tune_ant_cap;
+	};
 	struct completion work;
-	struct rds_info rds_info;
-	struct limiter_info limiter_info;
-	struct pilot_info pilot_info;
-	struct acomp_info acomp_info;
 	struct regulator_bulk_data supplies[SI4713_NUM_SUPPLIES];
 	int gpio_reset;
+	u32 power_state;
+	u32 rds_enabled;
 	u32 frequency;
 	u32 preemphasis;
-	u32 mute;
-	u32 power_level;
-	u32 power_state;
-	u32 antenna_capacitor;
 	u32 stereo;
 	u32 tune_rnl;
 };
diff --git a/drivers/media/radio/tef6862.c b/drivers/media/radio/tef6862.c
index b18c2dc268ba..82c6c9475d7c 100644
--- a/drivers/media/radio/tef6862.c
+++ b/drivers/media/radio/tef6862.c
@@ -96,12 +96,12 @@ static int tef6862_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *v)
 	return 0;
 }
 
-static int tef6862_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *v)
+static int tef6862_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *v)
 {
 	return v->index ? -EINVAL : 0;
 }
 
-static int tef6862_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
+static int tef6862_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *f)
 {
 	struct tef6862_state *state = to_state(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
diff --git a/drivers/media/radio/wl128x/fmdrv_v4l2.c b/drivers/media/radio/wl128x/fmdrv_v4l2.c
index 0a8ee8fab924..5dec323f4247 100644
--- a/drivers/media/radio/wl128x/fmdrv_v4l2.c
+++ b/drivers/media/radio/wl128x/fmdrv_v4l2.c
@@ -331,7 +331,7 @@ static int fm_v4l2_vidioc_g_tuner(struct file *file, void *priv,
  * Should we set other tuner attributes, too?
  */
 static int fm_v4l2_vidioc_s_tuner(struct file *file, void *priv,
-		struct v4l2_tuner *tuner)
+		const struct v4l2_tuner *tuner)
 {
 	struct fmdev *fmdev = video_drvdata(file);
 	u16 aud_mode;
@@ -388,7 +388,7 @@ static int fm_v4l2_vidioc_g_freq(struct file *file, void *priv,
 
 /* Set tuner or modulator radio frequency */
 static int fm_v4l2_vidioc_s_freq(struct file *file, void *priv,
-		struct v4l2_frequency *freq)
+		const struct v4l2_frequency *freq)
 {
 	struct fmdev *fmdev = video_drvdata(file);
 
@@ -396,9 +396,7 @@ static int fm_v4l2_vidioc_s_freq(struct file *file, void *priv,
 	 * As V4L2_TUNER_CAP_LOW is set 1 user sends the frequency
 	 * in units of 62.5 Hz.
 	 */
-	freq->frequency = (u32)(freq->frequency / 16);
-
-	return fmc_set_freq(fmdev, freq->frequency);
+	return fmc_set_freq(fmdev, freq->frequency / 16);
 }
 
 /* Set hardware frequency seek. If current mode is NOT RX, set it RX. */
diff --git a/drivers/media/rc/ene_ir.c b/drivers/media/rc/ene_ir.c
index ee6c984cade2..ed184f68c17c 100644
--- a/drivers/media/rc/ene_ir.c
+++ b/drivers/media/rc/ene_ir.c
@@ -1098,6 +1098,7 @@ exit_release_hw_io:
 	release_region(dev->hw_io, ENE_IO_SIZE);
 exit_unregister_device:
 	rc_unregister_device(rdev);
+	rdev = NULL;
 exit_free_dev_rdev:
 	rc_free_device(rdev);
 	kfree(dev);
diff --git a/drivers/media/rc/imon.c b/drivers/media/rc/imon.c
index dec203bb06f6..72e3fa652481 100644
--- a/drivers/media/rc/imon.c
+++ b/drivers/media/rc/imon.c
@@ -112,6 +112,7 @@ struct imon_context {
 	bool tx_control;
 	unsigned char usb_rx_buf[8];
 	unsigned char usb_tx_buf[8];
+	unsigned int send_packet_delay;
 
 	struct tx_t {
 		unsigned char data_buf[35];	/* user data buffer */
@@ -185,6 +186,10 @@ enum {
 	IMON_KEY_PANEL	= 2,
 };
 
+enum {
+	IMON_NEED_20MS_PKT_DELAY = 1
+};
+
 /*
  * USB Device ID for iMON USB Control Boards
  *
@@ -215,7 +220,7 @@ static struct usb_device_id imon_usb_id_table[] = {
 	/* SoundGraph iMON OEM Touch LCD (IR & 4.3" VGA LCD) */
 	{ USB_DEVICE(0x15c2, 0x0035) },
 	/* SoundGraph iMON OEM VFD (IR & VFD) */
-	{ USB_DEVICE(0x15c2, 0x0036) },
+	{ USB_DEVICE(0x15c2, 0x0036), .driver_info = IMON_NEED_20MS_PKT_DELAY },
 	/* device specifics unknown */
 	{ USB_DEVICE(0x15c2, 0x0037) },
 	/* SoundGraph iMON OEM LCD (IR & LCD) */
@@ -523,8 +528,10 @@ static int send_packet(struct imon_context *ictx)
 		mutex_unlock(&ictx->lock);
 		retval = wait_for_completion_interruptible(
 				&ictx->tx.finished);
-		if (retval)
+		if (retval) {
+			usb_kill_urb(ictx->tx_urb);
 			pr_err_ratelimited("task interrupted\n");
+		}
 		mutex_lock(&ictx->lock);
 
 		retval = ictx->tx.status;
@@ -535,12 +542,12 @@ static int send_packet(struct imon_context *ictx)
 	kfree(control_req);
 
 	/*
-	 * Induce a mandatory 5ms delay before returning, as otherwise,
+	 * Induce a mandatory delay before returning, as otherwise,
 	 * send_packet can get called so rapidly as to overwhelm the device,
 	 * particularly on faster systems and/or those with quirky usb.
 	 */
-	timeout = msecs_to_jiffies(5);
-	set_current_state(TASK_UNINTERRUPTIBLE);
+	timeout = msecs_to_jiffies(ictx->send_packet_delay);
+	set_current_state(TASK_INTERRUPTIBLE);
 	schedule_timeout(timeout);
 
 	return retval;
@@ -1568,11 +1575,6 @@ static void imon_incoming_packet(struct imon_context *ictx,
 	if (press_type < 0)
 		goto not_input_data;
 
-	spin_lock_irqsave(&ictx->kc_lock, flags);
-	if (ictx->kc == KEY_UNKNOWN)
-		goto unknown_key;
-	spin_unlock_irqrestore(&ictx->kc_lock, flags);
-
 	if (ktype != IMON_KEY_PANEL) {
 		if (press_type == 0)
 			rc_keyup(ictx->rdev);
@@ -1615,12 +1617,6 @@ static void imon_incoming_packet(struct imon_context *ictx,
 
 	return;
 
-unknown_key:
-	spin_unlock_irqrestore(&ictx->kc_lock, flags);
-	dev_info(dev, "%s: unknown keypress, code 0x%llx\n", __func__,
-		 (long long)scancode);
-	return;
-
 not_input_data:
 	if (len != 8) {
 		dev_warn(dev, "imon %s: invalid incoming packet "
@@ -2099,7 +2095,8 @@ static bool imon_find_endpoints(struct imon_context *ictx,
 
 }
 
-static struct imon_context *imon_init_intf0(struct usb_interface *intf)
+static struct imon_context *imon_init_intf0(struct usb_interface *intf,
+					    const struct usb_device_id *id)
 {
 	struct imon_context *ictx;
 	struct urb *rx_urb;
@@ -2139,6 +2136,10 @@ static struct imon_context *imon_init_intf0(struct usb_interface *intf)
 	ictx->vendor  = le16_to_cpu(ictx->usbdev_intf0->descriptor.idVendor);
 	ictx->product = le16_to_cpu(ictx->usbdev_intf0->descriptor.idProduct);
 
+	/* default send_packet delay is 5ms but some devices need more */
+	ictx->send_packet_delay = id->driver_info & IMON_NEED_20MS_PKT_DELAY ?
+				  20 : 5;
+
 	ret = -ENODEV;
 	iface_desc = intf->cur_altsetting;
 	if (!imon_find_endpoints(ictx, iface_desc)) {
@@ -2317,7 +2318,7 @@ static int imon_probe(struct usb_interface *interface,
 	first_if_ctx = usb_get_intfdata(first_if);
 
 	if (ifnum == 0) {
-		ictx = imon_init_intf0(interface);
+		ictx = imon_init_intf0(interface, id);
 		if (!ictx) {
 			pr_err("failed to initialize context!\n");
 			ret = -ENODEV;
@@ -2325,7 +2326,14 @@ static int imon_probe(struct usb_interface *interface,
 		}
 
 	} else {
-	/* this is the secondary interface on the device */
+		/* this is the secondary interface on the device */
+
+		/* fail early if first intf failed to register */
+		if (!first_if_ctx) {
+			ret = -ENODEV;
+			goto fail;
+		}
+
 		ictx = imon_init_intf1(interface, first_if_ctx);
 		if (!ictx) {
 			pr_err("failed to attach to context!\n");
diff --git a/drivers/media/rc/ir-jvc-decoder.c b/drivers/media/rc/ir-jvc-decoder.c
index 69edffb9fe9a..3948138ca870 100644
--- a/drivers/media/rc/ir-jvc-decoder.c
+++ b/drivers/media/rc/ir-jvc-decoder.c
@@ -47,7 +47,7 @@ static int ir_jvc_decode(struct rc_dev *dev, struct ir_raw_event ev)
 {
 	struct jvc_dec *data = &dev->raw->jvc;
 
-	if (!(dev->raw->enabled_protocols & RC_BIT_JVC))
+	if (!(dev->enabled_protocols & RC_BIT_JVC))
 		return 0;
 
 	if (!is_timing_event(ev)) {
diff --git a/drivers/media/rc/ir-lirc-codec.c b/drivers/media/rc/ir-lirc-codec.c
index 9945e5e7f61a..e4561264e124 100644
--- a/drivers/media/rc/ir-lirc-codec.c
+++ b/drivers/media/rc/ir-lirc-codec.c
@@ -35,7 +35,7 @@ static int ir_lirc_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	struct lirc_codec *lirc = &dev->raw->lirc;
 	int sample;
 
-	if (!(dev->raw->enabled_protocols & RC_BIT_LIRC))
+	if (!(dev->enabled_protocols & RC_BIT_LIRC))
 		return 0;
 
 	if (!dev->raw->lirc.drv || !dev->raw->lirc.drv->rbuf)
@@ -307,7 +307,7 @@ static void ir_lirc_close(void *data)
 	return;
 }
 
-static struct file_operations lirc_fops = {
+static const struct file_operations lirc_fops = {
 	.owner		= THIS_MODULE,
 	.write		= ir_lirc_transmit_ir,
 	.unlocked_ioctl	= ir_lirc_ioctl,
diff --git a/drivers/media/rc/ir-mce_kbd-decoder.c b/drivers/media/rc/ir-mce_kbd-decoder.c
index 33fafa4cf7cb..9f3c9b59f30c 100644
--- a/drivers/media/rc/ir-mce_kbd-decoder.c
+++ b/drivers/media/rc/ir-mce_kbd-decoder.c
@@ -216,7 +216,7 @@ static int ir_mce_kbd_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u32 scancode;
 	unsigned long delay;
 
-	if (!(dev->raw->enabled_protocols & RC_BIT_MCE_KBD))
+	if (!(dev->enabled_protocols & RC_BIT_MCE_KBD))
 		return 0;
 
 	if (!is_timing_event(ev)) {
diff --git a/drivers/media/rc/ir-nec-decoder.c b/drivers/media/rc/ir-nec-decoder.c
index a47ee3634969..9a9009411439 100644
--- a/drivers/media/rc/ir-nec-decoder.c
+++ b/drivers/media/rc/ir-nec-decoder.c
@@ -52,7 +52,7 @@ static int ir_nec_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u8 address, not_address, command, not_command;
 	bool send_32bits = false;
 
-	if (!(dev->raw->enabled_protocols & RC_BIT_NEC))
+	if (!(dev->enabled_protocols & RC_BIT_NEC))
 		return 0;
 
 	if (!is_timing_event(ev)) {
diff --git a/drivers/media/rc/ir-raw.c b/drivers/media/rc/ir-raw.c
index 17c94be9f24c..5c42750c7b71 100644
--- a/drivers/media/rc/ir-raw.c
+++ b/drivers/media/rc/ir-raw.c
@@ -256,7 +256,7 @@ int ir_raw_event_register(struct rc_dev *dev)
 		return -ENOMEM;
 
 	dev->raw->dev = dev;
-	dev->raw->enabled_protocols = ~0;
+	dev->enabled_protocols = ~0;
 	rc = kfifo_alloc(&dev->raw->kfifo,
 			 sizeof(struct ir_raw_event) * MAX_IR_EVENT_SIZE,
 			 GFP_KERNEL);
diff --git a/drivers/media/rc/ir-rc5-decoder.c b/drivers/media/rc/ir-rc5-decoder.c
index 5b4d1ddeac4e..4e53a319c5d8 100644
--- a/drivers/media/rc/ir-rc5-decoder.c
+++ b/drivers/media/rc/ir-rc5-decoder.c
@@ -52,7 +52,7 @@ static int ir_rc5_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u8 toggle;
 	u32 scancode;
 
-	if (!(dev->raw->enabled_protocols & (RC_BIT_RC5 | RC_BIT_RC5X)))
+	if (!(dev->enabled_protocols & (RC_BIT_RC5 | RC_BIT_RC5X)))
 		return 0;
 
 	if (!is_timing_event(ev)) {
@@ -128,7 +128,7 @@ again:
 		if (data->wanted_bits == RC5X_NBITS) {
 			/* RC5X */
 			u8 xdata, command, system;
-			if (!(dev->raw->enabled_protocols & RC_BIT_RC5X)) {
+			if (!(dev->enabled_protocols & RC_BIT_RC5X)) {
 				data->state = STATE_INACTIVE;
 				return 0;
 			}
@@ -145,7 +145,7 @@ again:
 		} else {
 			/* RC5 */
 			u8 command, system;
-			if (!(dev->raw->enabled_protocols & RC_BIT_RC5)) {
+			if (!(dev->enabled_protocols & RC_BIT_RC5)) {
 				data->state = STATE_INACTIVE;
 				return 0;
 			}
diff --git a/drivers/media/rc/ir-rc5-sz-decoder.c b/drivers/media/rc/ir-rc5-sz-decoder.c
index fd807a8308d8..865fe84fd854 100644
--- a/drivers/media/rc/ir-rc5-sz-decoder.c
+++ b/drivers/media/rc/ir-rc5-sz-decoder.c
@@ -48,7 +48,7 @@ static int ir_rc5_sz_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u8 toggle, command, system;
 	u32 scancode;
 
-	if (!(dev->raw->enabled_protocols & RC_BIT_RC5_SZ))
+	if (!(dev->enabled_protocols & RC_BIT_RC5_SZ))
 		return 0;
 
 	if (!is_timing_event(ev)) {
diff --git a/drivers/media/rc/ir-rc6-decoder.c b/drivers/media/rc/ir-rc6-decoder.c
index e19072ffb36c..7cba7d33a3fa 100644
--- a/drivers/media/rc/ir-rc6-decoder.c
+++ b/drivers/media/rc/ir-rc6-decoder.c
@@ -89,7 +89,7 @@ static int ir_rc6_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u32 scancode;
 	u8 toggle;
 
-	if (!(dev->raw->enabled_protocols &
+	if (!(dev->enabled_protocols &
 	      (RC_BIT_RC6_0 | RC_BIT_RC6_6A_20 | RC_BIT_RC6_6A_24 |
 	       RC_BIT_RC6_6A_32 | RC_BIT_RC6_MCE)))
 		return 0;
diff --git a/drivers/media/rc/ir-rx51.c b/drivers/media/rc/ir-rx51.c
index 8ead492d03aa..31b955bf7664 100644
--- a/drivers/media/rc/ir-rx51.c
+++ b/drivers/media/rc/ir-rx51.c
@@ -464,14 +464,14 @@ static int lirc_rx51_probe(struct platform_device *dev)
 	return 0;
 }
 
-static int __exit lirc_rx51_remove(struct platform_device *dev)
+static int lirc_rx51_remove(struct platform_device *dev)
 {
 	return lirc_unregister_driver(lirc_rx51_driver.minor);
 }
 
 struct platform_driver lirc_rx51_platform_driver = {
 	.probe		= lirc_rx51_probe,
-	.remove		= __exit_p(lirc_rx51_remove),
+	.remove		= lirc_rx51_remove,
 	.suspend	= lirc_rx51_suspend,
 	.resume		= lirc_rx51_resume,
 	.driver		= {
diff --git a/drivers/media/rc/ir-sanyo-decoder.c b/drivers/media/rc/ir-sanyo-decoder.c
index 7e69a3b65370..0a06205b5677 100644
--- a/drivers/media/rc/ir-sanyo-decoder.c
+++ b/drivers/media/rc/ir-sanyo-decoder.c
@@ -58,7 +58,7 @@ static int ir_sanyo_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u32 scancode;
 	u8 address, command, not_command;
 
-	if (!(dev->raw->enabled_protocols & RC_BIT_SANYO))
+	if (!(dev->enabled_protocols & RC_BIT_SANYO))
 		return 0;
 
 	if (!is_timing_event(ev)) {
diff --git a/drivers/media/rc/ir-sony-decoder.c b/drivers/media/rc/ir-sony-decoder.c
index fb914342cf4d..29ab9c2db060 100644
--- a/drivers/media/rc/ir-sony-decoder.c
+++ b/drivers/media/rc/ir-sony-decoder.c
@@ -45,7 +45,7 @@ static int ir_sony_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u32 scancode;
 	u8 device, subdevice, function;
 
-	if (!(dev->raw->enabled_protocols &
+	if (!(dev->enabled_protocols &
 	      (RC_BIT_SONY12 | RC_BIT_SONY15 | RC_BIT_SONY20)))
 		return 0;
 
@@ -124,7 +124,7 @@ static int ir_sony_decode(struct rc_dev *dev, struct ir_raw_event ev)
 
 		switch (data->count) {
 		case 12:
-			if (!(dev->raw->enabled_protocols & RC_BIT_SONY12)) {
+			if (!(dev->enabled_protocols & RC_BIT_SONY12)) {
 				data->state = STATE_INACTIVE;
 				return 0;
 			}
@@ -133,7 +133,7 @@ static int ir_sony_decode(struct rc_dev *dev, struct ir_raw_event ev)
 			function  = bitrev8((data->bits >>  4) & 0xFE);
 			break;
 		case 15:
-			if (!(dev->raw->enabled_protocols & RC_BIT_SONY15)) {
+			if (!(dev->enabled_protocols & RC_BIT_SONY15)) {
 				data->state = STATE_INACTIVE;
 				return 0;
 			}
@@ -142,7 +142,7 @@ static int ir_sony_decode(struct rc_dev *dev, struct ir_raw_event ev)
 			function  = bitrev8((data->bits >>  7) & 0xFE);
 			break;
 		case 20:
-			if (!(dev->raw->enabled_protocols & RC_BIT_SONY20)) {
+			if (!(dev->enabled_protocols & RC_BIT_SONY20)) {
 				data->state = STATE_INACTIVE;
 				return 0;
 			}
diff --git a/drivers/media/rc/ite-cir.c b/drivers/media/rc/ite-cir.c
index dd8237324c09..63b42252166a 100644
--- a/drivers/media/rc/ite-cir.c
+++ b/drivers/media/rc/ite-cir.c
@@ -1613,6 +1613,7 @@ exit_release_cir_addr:
 	release_region(itdev->cir_addr, itdev->params.io_region_size);
 exit_unregister_device:
 	rc_unregister_device(rdev);
+	rdev = NULL;
 exit_free_dev_rdev:
 	rc_free_device(rdev);
 	kfree(itdev);
diff --git a/drivers/media/rc/keymaps/Makefile b/drivers/media/rc/keymaps/Makefile
index 778661971aed..5ab94ea4bc28 100644
--- a/drivers/media/rc/keymaps/Makefile
+++ b/drivers/media/rc/keymaps/Makefile
@@ -78,6 +78,7 @@ obj-$(CONFIG_RC_MAP) += rc-adstech-dvb-t-pci.o \
 			rc-hauppauge.o \
 			rc-rc6-mce.o \
 			rc-real-audio-220-32-keys.o \
+			rc-reddo.o \
 			rc-snapstream-firefly.o \
 			rc-streamzap.o \
 			rc-tbs-nec.o \
@@ -88,7 +89,7 @@ obj-$(CONFIG_RC_MAP) += rc-adstech-dvb-t-pci.o \
 			rc-tevii-nec.o \
 			rc-tivo.o \
 			rc-total-media-in-hand.o \
-                       rc-total-media-in-hand-02.o \
+			rc-total-media-in-hand-02.o \
 			rc-trekstor.o \
 			rc-tt-1500.o \
 			rc-twinhan1027.o \
diff --git a/drivers/media/rc/keymaps/rc-reddo.c b/drivers/media/rc/keymaps/rc-reddo.c
new file mode 100644
index 000000000000..b80b336e9284
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-reddo.c
@@ -0,0 +1,86 @@
+/*
+ * MSI DIGIVOX mini III remote controller keytable
+ *
+ * Copyright (C) 2013 Antti Palosaari <crope@iki.fi>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License along
+ *    with this program; if not, write to the Free Software Foundation, Inc.,
+ *    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <media/rc-map.h>
+#include <linux/module.h>
+
+/*
+ * Derived from MSI DIGIVOX mini III remote (rc-msi-digivox-iii.c)
+ *
+ * Differences between these remotes are:
+ *
+ * 1) scancode 0x61d601 is mapped to different button:
+ *    MSI DIGIVOX mini III   "Source" = KEY_VIDEO
+ *    Reddo                     "EPG" = KEY_EPG
+ *
+ * 2) Reddo remote has less buttons. Missing buttons are: colored buttons,
+ *    navigation buttons and main power button.
+ */
+
+static struct rc_map_table reddo[] = {
+	{ 0x61d601, KEY_EPG },             /* EPG */
+	{ 0x61d602, KEY_3 },
+	{ 0x61d604, KEY_1 },
+	{ 0x61d605, KEY_5 },
+	{ 0x61d606, KEY_6 },
+	{ 0x61d607, KEY_CHANNELDOWN },     /* CH- */
+	{ 0x61d608, KEY_2 },
+	{ 0x61d609, KEY_CHANNELUP },       /* CH+ */
+	{ 0x61d60a, KEY_9 },
+	{ 0x61d60b, KEY_ZOOM },            /* Zoom */
+	{ 0x61d60c, KEY_7 },
+	{ 0x61d60d, KEY_8 },
+	{ 0x61d60e, KEY_VOLUMEUP },        /* Vol+ */
+	{ 0x61d60f, KEY_4 },
+	{ 0x61d610, KEY_ESC },             /* [back up arrow] */
+	{ 0x61d611, KEY_0 },
+	{ 0x61d612, KEY_OK },              /* [enter arrow] */
+	{ 0x61d613, KEY_VOLUMEDOWN },      /* Vol- */
+	{ 0x61d614, KEY_RECORD },          /* Rec */
+	{ 0x61d615, KEY_STOP },            /* Stop */
+	{ 0x61d616, KEY_PLAY },            /* Play */
+	{ 0x61d617, KEY_MUTE },            /* Mute */
+	{ 0x61d643, KEY_POWER2 },          /* [red power button] */
+};
+
+static struct rc_map_list reddo_map = {
+	.map = {
+		.scan    = reddo,
+		.size    = ARRAY_SIZE(reddo),
+		.rc_type = RC_TYPE_NEC,
+		.name    = RC_MAP_REDDO,
+	}
+};
+
+static int __init init_rc_map_reddo(void)
+{
+	return rc_map_register(&reddo_map);
+}
+
+static void __exit exit_rc_map_reddo(void)
+{
+	rc_map_unregister(&reddo_map);
+}
+
+module_init(init_rc_map_reddo)
+module_exit(exit_rc_map_reddo)
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
diff --git a/drivers/media/rc/lirc_dev.c b/drivers/media/rc/lirc_dev.c
index 5247d94fea29..8dc057b273f2 100644
--- a/drivers/media/rc/lirc_dev.c
+++ b/drivers/media/rc/lirc_dev.c
@@ -152,7 +152,7 @@ static int lirc_thread(void *irctl)
 }
 
 
-static struct file_operations lirc_dev_fops = {
+static const struct file_operations lirc_dev_fops = {
 	.owner		= THIS_MODULE,
 	.read		= lirc_dev_fop_read,
 	.write		= lirc_dev_fop_write,
diff --git a/drivers/media/rc/mceusb.c b/drivers/media/rc/mceusb.c
index 5b5b6e6f79e8..3c761014d3ce 100644
--- a/drivers/media/rc/mceusb.c
+++ b/drivers/media/rc/mceusb.c
@@ -482,7 +482,7 @@ static char SET_RX_SENSOR[]	= {MCE_CMD_PORT_IR,
 				   MCE_RSP_EQIRRXPORTEN, 0x00};
 */
 
-static int mceusb_cmdsize(u8 cmd, u8 subcmd)
+static int mceusb_cmd_datasize(u8 cmd, u8 subcmd)
 {
 	int datasize = 0;
 
@@ -493,6 +493,9 @@ static int mceusb_cmdsize(u8 cmd, u8 subcmd)
 		break;
 	case MCE_CMD_PORT_SYS:
 		switch (subcmd) {
+		case MCE_RSP_GETPORTSTATUS:
+			datasize = 5;
+			break;
 		case MCE_RSP_EQWAKEVERSION:
 			datasize = 4;
 			break;
@@ -500,6 +503,9 @@ static int mceusb_cmdsize(u8 cmd, u8 subcmd)
 			datasize = 2;
 			break;
 		case MCE_RSP_EQWAKESUPPORT:
+		case MCE_RSP_GETWAKESOURCE:
+		case MCE_RSP_EQDEVDETAILS:
+		case MCE_RSP_EQEMVER:
 			datasize = 1;
 			break;
 		}
@@ -509,6 +515,7 @@ static int mceusb_cmdsize(u8 cmd, u8 subcmd)
 		case MCE_RSP_EQIRCFS:
 		case MCE_RSP_EQIRTIMEOUT:
 		case MCE_RSP_EQIRRXCFCNT:
+		case MCE_RSP_EQIRNUMPORTS:
 			datasize = 2;
 			break;
 		case MCE_CMD_SIG_END:
@@ -968,7 +975,7 @@ static void mceusb_process_ir_data(struct mceusb_dev *ir, int buf_len)
 	for (; i < buf_len; i++) {
 		switch (ir->parser_state) {
 		case SUBCMD:
-			ir->rem = mceusb_cmdsize(ir->cmd, ir->buf_in[i]);
+			ir->rem = mceusb_cmd_datasize(ir->cmd, ir->buf_in[i]);
 			mceusb_dev_printdata(ir, ir->buf_in, i - 1,
 					     ir->rem + 2, false);
 			mceusb_handle_command(ir, i);
diff --git a/drivers/media/rc/nuvoton-cir.c b/drivers/media/rc/nuvoton-cir.c
index 40125d779049..21ee0dc1b7ec 100644
--- a/drivers/media/rc/nuvoton-cir.c
+++ b/drivers/media/rc/nuvoton-cir.c
@@ -1107,6 +1107,7 @@ exit_release_cir_addr:
 	release_region(nvt->cir_addr, CIR_IOREG_LENGTH);
 exit_unregister_device:
 	rc_unregister_device(rdev);
+	rdev = NULL;
 exit_free_dev_rdev:
 	rc_free_device(rdev);
 	kfree(nvt);
diff --git a/drivers/media/rc/rc-core-priv.h b/drivers/media/rc/rc-core-priv.h
index 5d87287ed372..70a180bb0bd0 100644
--- a/drivers/media/rc/rc-core-priv.h
+++ b/drivers/media/rc/rc-core-priv.h
@@ -39,7 +39,6 @@ struct ir_raw_event_ctrl {
 	ktime_t				last_event;	/* when last event occurred */
 	enum raw_event_type		last_type;	/* last event type */
 	struct rc_dev			*dev;		/* pointer to the parent rc_dev */
-	u64				enabled_protocols; /* enabled raw protocol decoders */
 
 	/* raw decoder state follows */
 	struct ir_raw_event prev_ev;
diff --git a/drivers/media/rc/rc-main.c b/drivers/media/rc/rc-main.c
index 759a40a42eaa..1cf382a0b277 100644
--- a/drivers/media/rc/rc-main.c
+++ b/drivers/media/rc/rc-main.c
@@ -715,14 +715,14 @@ static void ir_close(struct input_dev *idev)
 }
 
 /* class for /sys/class/rc */
-static char *ir_devnode(struct device *dev, umode_t *mode)
+static char *rc_devnode(struct device *dev, umode_t *mode)
 {
 	return kasprintf(GFP_KERNEL, "rc/%s", dev_name(dev));
 }
 
-static struct class ir_input_class = {
+static struct class rc_class = {
 	.name		= "rc",
-	.devnode	= ir_devnode,
+	.devnode	= rc_devnode,
 };
 
 /*
@@ -783,13 +783,12 @@ static ssize_t show_protocols(struct device *device,
 
 	mutex_lock(&dev->lock);
 
-	if (dev->driver_type == RC_DRIVER_SCANCODE) {
-		enabled = dev->rc_map.rc_type;
+	enabled = dev->enabled_protocols;
+	if (dev->driver_type == RC_DRIVER_SCANCODE)
 		allowed = dev->allowed_protos;
-	} else if (dev->raw) {
-		enabled = dev->raw->enabled_protocols;
+	else if (dev->raw)
 		allowed = ir_raw_get_allowed_protocols();
-	} else {
+	else {
 		mutex_unlock(&dev->lock);
 		return -ENODEV;
 	}
@@ -847,7 +846,6 @@ static ssize_t store_protocols(struct device *device,
 	u64 type;
 	u64 mask;
 	int rc, i, count = 0;
-	unsigned long flags;
 	ssize_t ret;
 
 	/* Device is being removed */
@@ -856,15 +854,12 @@ static ssize_t store_protocols(struct device *device,
 
 	mutex_lock(&dev->lock);
 
-	if (dev->driver_type == RC_DRIVER_SCANCODE)
-		type = dev->rc_map.rc_type;
-	else if (dev->raw)
-		type = dev->raw->enabled_protocols;
-	else {
+	if (dev->driver_type != RC_DRIVER_SCANCODE && !dev->raw) {
 		IR_dprintk(1, "Protocol switching not supported\n");
 		ret = -EINVAL;
 		goto out;
 	}
+	type = dev->enabled_protocols;
 
 	while ((tmp = strsep((char **) &data, " \n")) != NULL) {
 		if (!*tmp)
@@ -922,14 +917,7 @@ static ssize_t store_protocols(struct device *device,
 		}
 	}
 
-	if (dev->driver_type == RC_DRIVER_SCANCODE) {
-		spin_lock_irqsave(&dev->rc_map.lock, flags);
-		dev->rc_map.rc_type = type;
-		spin_unlock_irqrestore(&dev->rc_map.lock, flags);
-	} else {
-		dev->raw->enabled_protocols = type;
-	}
-
+	dev->enabled_protocols = type;
 	IR_dprintk(1, "Current protocol(s): 0x%llx\n",
 		   (long long)type);
 
@@ -1016,7 +1004,7 @@ struct rc_dev *rc_allocate_device(void)
 	setup_timer(&dev->timer_keyup, ir_timer_keyup, (unsigned long)dev);
 
 	dev->dev.type = &rc_dev_type;
-	dev->dev.class = &ir_input_class;
+	dev->dev.class = &rc_class;
 	device_initialize(&dev->dev);
 
 	__module_get(THIS_MODULE);
@@ -1068,9 +1056,8 @@ int rc_register_device(struct rc_dev *dev)
 	/*
 	 * Take the lock here, as the device sysfs node will appear
 	 * when device_add() is called, which may trigger an ir-keytable udev
-	 * rule, which will in turn call show_protocols and access either
-	 * dev->rc_map.rc_type or dev->raw->enabled_protocols before it has
-	 * been initialized.
+	 * rule, which will in turn call show_protocols and access
+	 * dev->enabled_protocols before it has been initialized.
 	 */
 	mutex_lock(&dev->lock);
 
@@ -1132,6 +1119,7 @@ int rc_register_device(struct rc_dev *dev)
 		rc = dev->change_protocol(dev, &rc_type);
 		if (rc < 0)
 			goto out_raw;
+		dev->enabled_protocols = rc_type;
 	}
 
 	mutex_unlock(&dev->lock);
@@ -1190,7 +1178,7 @@ EXPORT_SYMBOL_GPL(rc_unregister_device);
 
 static int __init rc_core_init(void)
 {
-	int rc = class_register(&ir_input_class);
+	int rc = class_register(&rc_class);
 	if (rc) {
 		printk(KERN_ERR "rc_core: unable to register rc class\n");
 		return rc;
@@ -1203,11 +1191,11 @@ static int __init rc_core_init(void)
 
 static void __exit rc_core_exit(void)
 {
-	class_unregister(&ir_input_class);
+	class_unregister(&rc_class);
 	rc_map_unregister(&empty_map);
 }
 
-module_init(rc_core_init);
+subsys_initcall(rc_core_init);
 module_exit(rc_core_exit);
 
 int rc_core_debug;    /* ir_debug level (0,1,2) */
diff --git a/drivers/media/rc/redrat3.c b/drivers/media/rc/redrat3.c
index 1b37fe2779f8..12167a6b5472 100644
--- a/drivers/media/rc/redrat3.c
+++ b/drivers/media/rc/redrat3.c
@@ -45,6 +45,7 @@
  *
  */
 
+#include <asm/unaligned.h>
 #include <linux/device.h>
 #include <linux/module.h>
 #include <linux/slab.h>
@@ -53,7 +54,6 @@
 #include <media/rc-core.h>
 
 /* Driver Information */
-#define DRIVER_VERSION "0.70"
 #define DRIVER_AUTHOR "Jarod Wilson <jarod@redhat.com>"
 #define DRIVER_AUTHOR2 "The Dweller, Stephen Cox"
 #define DRIVER_DESC "RedRat3 USB IR Transceiver Driver"
@@ -129,25 +129,11 @@ static int debug;
 /* USB bulk-in IR data endpoint address */
 #define RR3_BULK_IN_EP_ADDR	0x82
 
-/* Raw Modulated signal data value offsets */
-#define RR3_PAUSE_OFFSET	0
-#define RR3_FREQ_COUNT_OFFSET	4
-#define RR3_NUM_PERIOD_OFFSET	6
-#define RR3_MAX_LENGTHS_OFFSET	8
-#define RR3_NUM_LENGTHS_OFFSET	9
-#define RR3_MAX_SIGS_OFFSET	10
-#define RR3_NUM_SIGS_OFFSET	12
-#define RR3_REPEATS_OFFSET	14
-
 /* Size of the fixed-length portion of the signal */
-#define RR3_HEADER_LENGTH	15
 #define RR3_DRIVER_MAXLENS	128
 #define RR3_MAX_SIG_SIZE	512
-#define RR3_MAX_BUF_SIZE	\
-	((2 * RR3_HEADER_LENGTH) + RR3_DRIVER_MAXLENS + RR3_MAX_SIG_SIZE)
 #define RR3_TIME_UNIT		50
 #define RR3_END_OF_SIGNAL	0x7f
-#define RR3_TX_HEADER_OFFSET	4
 #define RR3_TX_TRAILER_LEN	2
 #define RR3_RX_MIN_TIMEOUT	5
 #define RR3_RX_MAX_TIMEOUT	2000
@@ -159,6 +145,32 @@ static int debug;
 #define USB_RR3USB_PRODUCT_ID	0x0001
 #define USB_RR3IIUSB_PRODUCT_ID	0x0005
 
+struct redrat3_header {
+	__be16 length;
+	__be16 transfer_type;
+} __packed;
+
+/* sending and receiving irdata */
+struct redrat3_irdata {
+	struct redrat3_header header;
+	__be32 pause;
+	__be16 mod_freq_count;
+	__be16 num_periods;
+	__u8 max_lengths;
+	__u8 no_lengths;
+	__be16 max_sig_size;
+	__be16 sig_size;
+	__u8 no_repeats;
+	__be16 lens[RR3_DRIVER_MAXLENS]; /* not aligned */
+	__u8 sigdata[RR3_MAX_SIG_SIZE];
+} __packed;
+
+/* firmware errors */
+struct redrat3_error {
+	struct redrat3_header header;
+	__be16 fw_error;
+} __packed;
+
 /* table of devices that work with this driver */
 static struct usb_device_id redrat3_dev_table[] = {
 	/* Original version of the RedRat3 */
@@ -180,20 +192,15 @@ struct redrat3_dev {
 	/* the receive endpoint */
 	struct usb_endpoint_descriptor *ep_in;
 	/* the buffer to receive data */
-	unsigned char *bulk_in_buf;
+	void *bulk_in_buf;
 	/* urb used to read ir data */
 	struct urb *read_urb;
 
 	/* the send endpoint */
 	struct usb_endpoint_descriptor *ep_out;
-	/* the buffer to send data */
-	unsigned char *bulk_out_buf;
-	/* the urb used to send data */
-	struct urb *write_urb;
 
 	/* usb dma */
 	dma_addr_t dma_in;
-	dma_addr_t dma_out;
 
 	/* rx signal timeout timer */
 	struct timer_list rx_timeout;
@@ -205,72 +212,15 @@ struct redrat3_dev {
 	bool transmitting;
 
 	/* store for current packet */
-	char pbuf[RR3_MAX_BUF_SIZE];
-	u16 pktlen;
-	u16 pkttype;
+	struct redrat3_irdata irdata;
 	u16 bytes_read;
-	/* indicate whether we are going to reprocess
-	 * the USB callback with a bigger buffer */
-	int buftoosmall;
-	char *datap;
 
 	u32 carrier;
 
-	char name[128];
+	char name[64];
 	char phys[64];
 };
 
-/* All incoming data buffers adhere to a very specific data format */
-struct redrat3_signal_header {
-	u16 length;	/* Length of data being transferred */
-	u16 transfer_type; /* Type of data transferred */
-	u32 pause;	/* Pause between main and repeat signals */
-	u16 mod_freq_count; /* Value of timer on mod. freq. measurement */
-	u16 no_periods;	/* No. of periods over which mod. freq. is measured */
-	u8 max_lengths;	/* Max no. of lengths (i.e. size of array) */
-	u8 no_lengths;	/* Actual no. of elements in lengths array */
-	u16 max_sig_size; /* Max no. of values in signal data array */
-	u16 sig_size;	/* Acuto no. of values in signal data array */
-	u8 no_repeats;	/* No. of repeats of repeat signal section */
-	/* Here forward is the lengths and signal data */
-};
-
-static void redrat3_dump_signal_header(struct redrat3_signal_header *header)
-{
-	pr_info("%s:\n", __func__);
-	pr_info(" * length: %u, transfer_type: 0x%02x\n",
-		header->length, header->transfer_type);
-	pr_info(" * pause: %u, freq_count: %u, no_periods: %u\n",
-		header->pause, header->mod_freq_count, header->no_periods);
-	pr_info(" * lengths: %u (max: %u)\n",
-		header->no_lengths, header->max_lengths);
-	pr_info(" * sig_size: %u (max: %u)\n",
-		header->sig_size, header->max_sig_size);
-	pr_info(" * repeats: %u\n", header->no_repeats);
-}
-
-static void redrat3_dump_signal_data(char *buffer, u16 len)
-{
-	int offset, i;
-	char *data_vals;
-
-	pr_info("%s:", __func__);
-
-	offset = RR3_TX_HEADER_OFFSET + RR3_HEADER_LENGTH
-		 + (RR3_DRIVER_MAXLENS * sizeof(u16));
-
-	/* read RR3_DRIVER_MAXLENS from ctrl msg */
-	data_vals = buffer + offset;
-
-	for (i = 0; i < len; i++) {
-		if (i % 10 == 0)
-			pr_cont("\n * ");
-		pr_cont("%02x ", *data_vals++);
-	}
-
-	pr_cont("\n");
-}
-
 /*
  * redrat3_issue_async
  *
@@ -283,7 +233,6 @@ static void redrat3_issue_async(struct redrat3_dev *rr3)
 
 	rr3_ftr(rr3->dev, "Entering %s\n", __func__);
 
-	memset(rr3->bulk_in_buf, 0, rr3->ep_in->wMaxPacketSize);
 	res = usb_submit_urb(rr3->read_urb, GFP_ATOMIC);
 	if (res)
 		rr3_dbg(rr3->dev, "%s: receive request FAILED! "
@@ -352,13 +301,14 @@ static void redrat3_dump_fw_error(struct redrat3_dev *rr3, int code)
 	}
 }
 
-static u32 redrat3_val_to_mod_freq(struct redrat3_signal_header *ph)
+static u32 redrat3_val_to_mod_freq(struct redrat3_irdata *irdata)
 {
 	u32 mod_freq = 0;
+	u16 mod_freq_count = be16_to_cpu(irdata->mod_freq_count);
 
-	if (ph->mod_freq_count != 0)
-		mod_freq = (RR3_CLK * ph->no_periods) /
-				(ph->mod_freq_count * RR3_CLK_PER_COUNT);
+	if (mod_freq_count != 0)
+		mod_freq = (RR3_CLK * be16_to_cpu(irdata->num_periods)) /
+			(mod_freq_count * RR3_CLK_PER_COUNT);
 
 	return mod_freq;
 }
@@ -396,7 +346,6 @@ static u32 redrat3_us_to_len(u32 microsec)
 
 	/* don't allow zero lengths to go back, breaks lirc */
 	return result ? result : 1;
-
 }
 
 /* timer callback to send reset event */
@@ -411,16 +360,11 @@ static void redrat3_rx_timeout(unsigned long data)
 static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 {
 	DEFINE_IR_RAW_EVENT(rawir);
-	struct redrat3_signal_header header;
 	struct device *dev;
-	int i, trailer = 0;
+	unsigned i, trailer = 0;
+	unsigned sig_size, single_len, offset, val;
 	unsigned long delay;
-	u32 mod_freq, single_len;
-	u16 *len_vals;
-	u8 *data_vals;
-	u32 tmp32;
-	u16 tmp16;
-	char *sig_data;
+	u32 mod_freq;
 
 	if (!rr3) {
 		pr_err("%s called with no context!\n", __func__);
@@ -430,57 +374,20 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 	rr3_ftr(rr3->dev, "Entered %s\n", __func__);
 
 	dev = rr3->dev;
-	sig_data = rr3->pbuf;
-
-	header.length = rr3->pktlen;
-	header.transfer_type = rr3->pkttype;
-
-	/* Sanity check */
-	if (!(header.length >= RR3_HEADER_LENGTH))
-		dev_warn(dev, "read returned less than rr3 header len\n");
 
 	/* Make sure we reset the IR kfifo after a bit of inactivity */
 	delay = usecs_to_jiffies(rr3->hw_timeout);
 	mod_timer(&rr3->rx_timeout, jiffies + delay);
 
-	memcpy(&tmp32, sig_data + RR3_PAUSE_OFFSET, sizeof(tmp32));
-	header.pause = be32_to_cpu(tmp32);
-
-	memcpy(&tmp16, sig_data + RR3_FREQ_COUNT_OFFSET, sizeof(tmp16));
-	header.mod_freq_count = be16_to_cpu(tmp16);
-
-	memcpy(&tmp16, sig_data + RR3_NUM_PERIOD_OFFSET, sizeof(tmp16));
-	header.no_periods = be16_to_cpu(tmp16);
-
-	header.max_lengths = sig_data[RR3_MAX_LENGTHS_OFFSET];
-	header.no_lengths = sig_data[RR3_NUM_LENGTHS_OFFSET];
-
-	memcpy(&tmp16, sig_data + RR3_MAX_SIGS_OFFSET, sizeof(tmp16));
-	header.max_sig_size = be16_to_cpu(tmp16);
-
-	memcpy(&tmp16, sig_data + RR3_NUM_SIGS_OFFSET, sizeof(tmp16));
-	header.sig_size = be16_to_cpu(tmp16);
-
-	header.no_repeats= sig_data[RR3_REPEATS_OFFSET];
-
-	if (debug) {
-		redrat3_dump_signal_header(&header);
-		redrat3_dump_signal_data(sig_data, header.sig_size);
-	}
-
-	mod_freq = redrat3_val_to_mod_freq(&header);
+	mod_freq = redrat3_val_to_mod_freq(&rr3->irdata);
 	rr3_dbg(dev, "Got mod_freq of %u\n", mod_freq);
 
-	/* Here we pull out the 'length' values from the signal */
-	len_vals = (u16 *)(sig_data + RR3_HEADER_LENGTH);
-
-	data_vals = sig_data + RR3_HEADER_LENGTH +
-		    (header.max_lengths * sizeof(u16));
-
 	/* process each rr3 encoded byte into an int */
-	for (i = 0; i < header.sig_size; i++) {
-		u16 val = len_vals[data_vals[i]];
-		single_len = redrat3_len_to_us((u32)be16_to_cpu(val));
+	sig_size = be16_to_cpu(rr3->irdata.sig_size);
+	for (i = 0; i < sig_size; i++) {
+		offset = rr3->irdata.sigdata[i];
+		val = get_unaligned_be16(&rr3->irdata.lens[offset]);
+		single_len = redrat3_len_to_us(val);
 
 		/* we should always get pulse/space/pulse/space samples */
 		if (i % 2)
@@ -515,8 +422,6 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 
 	rr3_dbg(dev, "calling ir_raw_event_handle\n");
 	ir_raw_event_handle(rr3->rc);
-
-	return;
 }
 
 /* Util fn to send rr3 cmds */
@@ -540,7 +445,7 @@ static u8 redrat3_send_cmd(int cmd, struct redrat3_dev *rr3)
 			__func__, res, *data);
 		res = -EIO;
 	} else
-		res = (u8)data[0];
+		res = data[0];
 
 	kfree(data);
 
@@ -598,22 +503,18 @@ static inline void redrat3_delete(struct redrat3_dev *rr3,
 {
 	rr3_ftr(rr3->dev, "%s cleaning up\n", __func__);
 	usb_kill_urb(rr3->read_urb);
-	usb_kill_urb(rr3->write_urb);
 
 	usb_free_urb(rr3->read_urb);
-	usb_free_urb(rr3->write_urb);
 
-	usb_free_coherent(udev, rr3->ep_in->wMaxPacketSize,
+	usb_free_coherent(udev, le16_to_cpu(rr3->ep_in->wMaxPacketSize),
 			  rr3->bulk_in_buf, rr3->dma_in);
-	usb_free_coherent(udev, rr3->ep_out->wMaxPacketSize,
-			  rr3->bulk_out_buf, rr3->dma_out);
 
 	kfree(rr3);
 }
 
 static u32 redrat3_get_timeout(struct redrat3_dev *rr3)
 {
-	u32 *tmp;
+	__be32 *tmp;
 	u32 timeout = MS_TO_US(150); /* a sane default, if things go haywire */
 	int len, ret, pipe;
 
@@ -628,14 +529,16 @@ static u32 redrat3_get_timeout(struct redrat3_dev *rr3)
 	ret = usb_control_msg(rr3->udev, pipe, RR3_GET_IR_PARAM,
 			      USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
 			      RR3_IR_IO_SIG_TIMEOUT, 0, tmp, len, HZ * 5);
-	if (ret != len) {
+	if (ret != len)
 		dev_warn(rr3->dev, "Failed to read timeout from hardware\n");
-		return timeout;
+	else {
+		timeout = redrat3_len_to_us(be32_to_cpup(tmp));
+
+		rr3_dbg(rr3->dev, "Got timeout of %d ms\n", timeout / 1000);
 	}
 
-	timeout = redrat3_len_to_us(be32_to_cpu(*tmp));
+	kfree(tmp);
 
-	rr3_dbg(rr3->dev, "Got timeout of %d ms\n", timeout / 1000);
 	return timeout;
 }
 
@@ -652,7 +555,7 @@ static void redrat3_reset(struct redrat3_dev *rr3)
 	rxpipe = usb_rcvctrlpipe(udev, 0);
 	txpipe = usb_sndctrlpipe(udev, 0);
 
-	val = kzalloc(len, GFP_KERNEL);
+	val = kmalloc(len, GFP_KERNEL);
 	if (!val) {
 		dev_err(dev, "Memory allocation failure\n");
 		return;
@@ -706,82 +609,74 @@ static void redrat3_get_firmware_rev(struct redrat3_dev *rr3)
 	rr3_ftr(rr3->dev, "Exiting %s\n", __func__);
 }
 
-static void redrat3_read_packet_start(struct redrat3_dev *rr3, int len)
+static void redrat3_read_packet_start(struct redrat3_dev *rr3, unsigned len)
 {
-	u16 tx_error;
-	u16 hdrlen;
+	struct redrat3_header *header = rr3->bulk_in_buf;
+	unsigned pktlen, pkttype;
 
 	rr3_ftr(rr3->dev, "Entering %s\n", __func__);
 
 	/* grab the Length and type of transfer */
-	memcpy(&(rr3->pktlen), (unsigned char *) rr3->bulk_in_buf,
-	       sizeof(rr3->pktlen));
-	memcpy(&(rr3->pkttype), ((unsigned char *) rr3->bulk_in_buf +
-		sizeof(rr3->pktlen)),
-	       sizeof(rr3->pkttype));
+	pktlen = be16_to_cpu(header->length);
+	pkttype = be16_to_cpu(header->transfer_type);
 
-	/*data needs conversion to know what its real values are*/
-	rr3->pktlen = be16_to_cpu(rr3->pktlen);
-	rr3->pkttype = be16_to_cpu(rr3->pkttype);
+	if (pktlen > sizeof(rr3->irdata)) {
+		dev_warn(rr3->dev, "packet length %u too large\n", pktlen);
+		return;
+	}
 
-	switch (rr3->pkttype) {
+	switch (pkttype) {
 	case RR3_ERROR:
-		memcpy(&tx_error, ((unsigned char *)rr3->bulk_in_buf
-			+ (sizeof(rr3->pktlen) + sizeof(rr3->pkttype))),
-		       sizeof(tx_error));
-		tx_error = be16_to_cpu(tx_error);
-		redrat3_dump_fw_error(rr3, tx_error);
+		if (len >= sizeof(struct redrat3_error)) {
+			struct redrat3_error *error = rr3->bulk_in_buf;
+			unsigned fw_error = be16_to_cpu(error->fw_error);
+			redrat3_dump_fw_error(rr3, fw_error);
+		}
 		break;
 
 	case RR3_MOD_SIGNAL_IN:
-		hdrlen = sizeof(rr3->pktlen) + sizeof(rr3->pkttype);
+		memcpy(&rr3->irdata, rr3->bulk_in_buf, len);
 		rr3->bytes_read = len;
-		rr3->bytes_read -= hdrlen;
-		rr3->datap = &(rr3->pbuf[0]);
-
-		memcpy(rr3->datap, ((unsigned char *)rr3->bulk_in_buf + hdrlen),
-		       rr3->bytes_read);
-		rr3->datap += rr3->bytes_read;
 		rr3_dbg(rr3->dev, "bytes_read %d, pktlen %d\n",
-			rr3->bytes_read, rr3->pktlen);
+			rr3->bytes_read, pktlen);
 		break;
 
 	default:
-		rr3_dbg(rr3->dev, "ignoring packet with type 0x%02x, "
-			"len of %d, 0x%02x\n", rr3->pkttype, len, rr3->pktlen);
+		rr3_dbg(rr3->dev, "ignoring packet with type 0x%02x, len of %d, 0x%02x\n",
+						pkttype, len, pktlen);
 		break;
 	}
 }
 
-static void redrat3_read_packet_continue(struct redrat3_dev *rr3, int len)
+static void redrat3_read_packet_continue(struct redrat3_dev *rr3, unsigned len)
 {
+	void *irdata = &rr3->irdata;
 
 	rr3_ftr(rr3->dev, "Entering %s\n", __func__);
 
-	memcpy(rr3->datap, (unsigned char *)rr3->bulk_in_buf, len);
-	rr3->datap += len;
+	if (len + rr3->bytes_read > sizeof(rr3->irdata)) {
+		dev_warn(rr3->dev, "too much data for packet\n");
+		rr3->bytes_read = 0;
+		return;
+	}
+
+	memcpy(irdata + rr3->bytes_read, rr3->bulk_in_buf, len);
 
 	rr3->bytes_read += len;
-	rr3_dbg(rr3->dev, "bytes_read %d, pktlen %d\n",
-		rr3->bytes_read, rr3->pktlen);
+	rr3_dbg(rr3->dev, "bytes_read %d, pktlen %d\n", rr3->bytes_read,
+				 be16_to_cpu(rr3->irdata.header.length));
 }
 
 /* gather IR data from incoming urb, process it when we have enough */
-static int redrat3_get_ir_data(struct redrat3_dev *rr3, int len)
+static int redrat3_get_ir_data(struct redrat3_dev *rr3, unsigned len)
 {
 	struct device *dev = rr3->dev;
+	unsigned pkttype;
 	int ret = 0;
 
 	rr3_ftr(dev, "Entering %s\n", __func__);
 
-	if (rr3->pktlen > RR3_MAX_BUF_SIZE) {
-		dev_err(rr3->dev, "error: packet larger than buffer\n");
-		ret = -EINVAL;
-		goto out;
-	}
-
-	if ((rr3->bytes_read == 0) &&
-	    (len >= (sizeof(rr3->pkttype) + sizeof(rr3->pktlen)))) {
+	if (rr3->bytes_read == 0 && len >= sizeof(struct redrat3_header)) {
 		redrat3_read_packet_start(rr3, len);
 	} else if (rr3->bytes_read != 0) {
 		redrat3_read_packet_continue(rr3, len);
@@ -791,31 +686,25 @@ static int redrat3_get_ir_data(struct redrat3_dev *rr3, int len)
 		goto out;
 	}
 
-	if (rr3->bytes_read > rr3->pktlen) {
-		dev_err(dev, "bytes_read (%d) greater than pktlen (%d)\n",
-			rr3->bytes_read, rr3->pktlen);
-		ret = -EINVAL;
-		goto out;
-	} else if (rr3->bytes_read < rr3->pktlen)
+	if (rr3->bytes_read < be16_to_cpu(rr3->irdata.header.length))
 		/* we're still accumulating data */
 		return 0;
 
 	/* if we get here, we've got IR data to decode */
-	if (rr3->pkttype == RR3_MOD_SIGNAL_IN)
+	pkttype = be16_to_cpu(rr3->irdata.header.transfer_type);
+	if (pkttype == RR3_MOD_SIGNAL_IN)
 		redrat3_process_ir_data(rr3);
 	else
-		rr3_dbg(dev, "discarding non-signal data packet "
-			"(type 0x%02x)\n", rr3->pkttype);
+		rr3_dbg(dev, "discarding non-signal data packet (type 0x%02x)\n",
+								pkttype);
 
 out:
 	rr3->bytes_read = 0;
-	rr3->pktlen = 0;
-	rr3->pkttype = 0;
 	return ret;
 }
 
 /* callback function from USB when async USB request has completed */
-static void redrat3_handle_async(struct urb *urb, struct pt_regs *regs)
+static void redrat3_handle_async(struct urb *urb)
 {
 	struct redrat3_dev *rr3;
 	int ret;
@@ -851,34 +740,16 @@ static void redrat3_handle_async(struct urb *urb, struct pt_regs *regs)
 	default:
 		dev_warn(rr3->dev, "Error: urb status = %d\n", urb->status);
 		rr3->bytes_read = 0;
-		rr3->pktlen = 0;
-		rr3->pkttype = 0;
 		break;
 	}
 }
 
-static void redrat3_write_bulk_callback(struct urb *urb, struct pt_regs *regs)
-{
-	struct redrat3_dev *rr3;
-	int len;
-
-	if (!urb)
-		return;
-
-	rr3 = urb->context;
-	if (rr3) {
-		len = urb->actual_length;
-		rr3_ftr(rr3->dev, "%s: called (status=%d len=%d)\n",
-			__func__, urb->status, len);
-	}
-}
-
 static u16 mod_freq_to_val(unsigned int mod_freq)
 {
 	int mult = 6000000;
 
 	/* Clk used in mod. freq. generation is CLK24/4. */
-	return (u16)(65536 - (mult / mod_freq));
+	return 65536 - (mult / mod_freq);
 }
 
 static int redrat3_set_tx_carrier(struct rc_dev *rcdev, u32 carrier)
@@ -900,17 +771,12 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
 {
 	struct redrat3_dev *rr3 = rcdev->priv;
 	struct device *dev = rr3->dev;
-	struct redrat3_signal_header header;
-	int i, j, ret, ret_len, offset;
+	struct redrat3_irdata *irdata = NULL;
+	int ret, ret_len;
 	int lencheck, cur_sample_len, pipe;
-	char *buffer = NULL, *sigdata = NULL;
 	int *sample_lens = NULL;
-	u32 tmpi;
-	u16 tmps;
-	u8 *datap;
 	u8 curlencheck = 0;
-	u16 *lengths_ptr;
-	int sendbuf_len;
+	unsigned i, sendbuf_len;
 
 	rr3_ftr(dev, "Entering %s\n", __func__);
 
@@ -931,8 +797,19 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
 		goto out;
 	}
 
+	irdata = kzalloc(sizeof(*irdata), GFP_KERNEL);
+	if (!irdata) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
 	for (i = 0; i < count; i++) {
 		cur_sample_len = redrat3_us_to_len(txbuf[i]);
+		if (cur_sample_len > 0xffff) {
+			dev_warn(dev, "transmit period of %uus truncated to %uus\n",
+					txbuf[i], redrat3_len_to_us(0xffff));
+			cur_sample_len = 0xffff;
+		}
 		for (lencheck = 0; lencheck < curlencheck; lencheck++) {
 			if (sample_lens[lencheck] == cur_sample_len)
 				break;
@@ -944,94 +821,41 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
 				/* now convert the value to a proper
 				 * rr3 value.. */
 				sample_lens[curlencheck] = cur_sample_len;
+				put_unaligned_be16(cur_sample_len,
+						&irdata->lens[curlencheck]);
 				curlencheck++;
 			} else {
 				count = i - 1;
 				break;
 			}
 		}
+		irdata->sigdata[i] = lencheck;
 	}
 
-	sigdata = kzalloc((count + RR3_TX_TRAILER_LEN), GFP_KERNEL);
-	if (!sigdata) {
-		ret = -ENOMEM;
-		goto out;
-	}
-
-	sigdata[count] = RR3_END_OF_SIGNAL;
-	sigdata[count + 1] = RR3_END_OF_SIGNAL;
-	for (i = 0; i < count; i++) {
-		for (j = 0; j < curlencheck; j++) {
-			if (sample_lens[j] == redrat3_us_to_len(txbuf[i]))
-				sigdata[i] = j;
-		}
-	}
-
-	offset = RR3_TX_HEADER_OFFSET;
-	sendbuf_len = RR3_HEADER_LENGTH + (sizeof(u16) * RR3_DRIVER_MAXLENS)
-			+ count + RR3_TX_TRAILER_LEN + offset;
-
-	buffer = kzalloc(sendbuf_len, GFP_KERNEL);
-	if (!buffer) {
-		ret = -ENOMEM;
-		goto out;
-	}
+	irdata->sigdata[count] = RR3_END_OF_SIGNAL;
+	irdata->sigdata[count + 1] = RR3_END_OF_SIGNAL;
 
+	sendbuf_len = offsetof(struct redrat3_irdata,
+					sigdata[count + RR3_TX_TRAILER_LEN]);
 	/* fill in our packet header */
-	header.length = sendbuf_len - offset;
-	header.transfer_type = RR3_MOD_SIGNAL_OUT;
-	header.pause = redrat3_len_to_us(100);
-	header.mod_freq_count = mod_freq_to_val(rr3->carrier);
-	header.no_periods = 0; /* n/a to transmit */
-	header.max_lengths = RR3_DRIVER_MAXLENS;
-	header.no_lengths = curlencheck;
-	header.max_sig_size = RR3_MAX_SIG_SIZE;
-	header.sig_size = count + RR3_TX_TRAILER_LEN;
-	/* we currently rely on repeat handling in the IR encoding source */
-	header.no_repeats = 0;
-
-	tmps = cpu_to_be16(header.length);
-	memcpy(buffer, &tmps, 2);
-
-	tmps = cpu_to_be16(header.transfer_type);
-	memcpy(buffer + 2, &tmps, 2);
-
-	tmpi = cpu_to_be32(header.pause);
-	memcpy(buffer + offset, &tmpi, sizeof(tmpi));
-
-	tmps = cpu_to_be16(header.mod_freq_count);
-	memcpy(buffer + offset + RR3_FREQ_COUNT_OFFSET, &tmps, 2);
-
-	buffer[offset + RR3_NUM_LENGTHS_OFFSET] = header.no_lengths;
-
-	tmps = cpu_to_be16(header.sig_size);
-	memcpy(buffer + offset + RR3_NUM_SIGS_OFFSET, &tmps, 2);
-
-	buffer[offset + RR3_REPEATS_OFFSET] = header.no_repeats;
-
-	lengths_ptr = (u16 *)(buffer + offset + RR3_HEADER_LENGTH);
-	for (i = 0; i < curlencheck; ++i)
-		lengths_ptr[i] = cpu_to_be16(sample_lens[i]);
-
-	datap = (u8 *)(buffer + offset + RR3_HEADER_LENGTH +
-			    (sizeof(u16) * RR3_DRIVER_MAXLENS));
-	memcpy(datap, sigdata, (count + RR3_TX_TRAILER_LEN));
-
-	if (debug) {
-		redrat3_dump_signal_header(&header);
-		redrat3_dump_signal_data(buffer, header.sig_size);
-	}
+	irdata->header.length = cpu_to_be16(sendbuf_len -
+						sizeof(struct redrat3_header));
+	irdata->header.transfer_type = cpu_to_be16(RR3_MOD_SIGNAL_OUT);
+	irdata->pause = cpu_to_be32(redrat3_len_to_us(100));
+	irdata->mod_freq_count = cpu_to_be16(mod_freq_to_val(rr3->carrier));
+	irdata->no_lengths = curlencheck;
+	irdata->sig_size = cpu_to_be16(count + RR3_TX_TRAILER_LEN);
 
 	pipe = usb_sndbulkpipe(rr3->udev, rr3->ep_out->bEndpointAddress);
-	tmps = usb_bulk_msg(rr3->udev, pipe, buffer,
+	ret = usb_bulk_msg(rr3->udev, pipe, irdata,
 			    sendbuf_len, &ret_len, 10 * HZ);
-	rr3_dbg(dev, "sent %d bytes, (ret %d)\n", ret_len, tmps);
+	rr3_dbg(dev, "sent %d bytes, (ret %d)\n", ret_len, ret);
 
 	/* now tell the hardware to transmit what we sent it */
 	pipe = usb_rcvctrlpipe(rr3->udev, 0);
 	ret = usb_control_msg(rr3->udev, pipe, RR3_TX_SEND_SIGNAL,
 			      USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-			      0, 0, buffer, 2, HZ * 10);
+			      0, 0, irdata, 2, HZ * 10);
 
 	if (ret < 0)
 		dev_err(dev, "Error: control msg send failed, rc %d\n", ret);
@@ -1040,8 +864,7 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
 
 out:
 	kfree(sample_lens);
-	kfree(buffer);
-	kfree(sigdata);
+	kfree(irdata);
 
 	rr3->transmitting = false;
 	/* rr3 re-enables rc detector because it was enabled before */
@@ -1165,38 +988,18 @@ static int redrat3_dev_probe(struct usb_interface *intf,
 	}
 
 	rr3->ep_in = ep_in;
-	rr3->bulk_in_buf = usb_alloc_coherent(udev, ep_in->wMaxPacketSize,
-					      GFP_ATOMIC, &rr3->dma_in);
+	rr3->bulk_in_buf = usb_alloc_coherent(udev,
+		le16_to_cpu(ep_in->wMaxPacketSize), GFP_ATOMIC, &rr3->dma_in);
 	if (!rr3->bulk_in_buf) {
 		dev_err(dev, "Read buffer allocation failure\n");
 		goto error;
 	}
 
 	pipe = usb_rcvbulkpipe(udev, ep_in->bEndpointAddress);
-	usb_fill_bulk_urb(rr3->read_urb, udev, pipe,
-			  rr3->bulk_in_buf, ep_in->wMaxPacketSize,
-			  (usb_complete_t)redrat3_handle_async, rr3);
-
-	/* set up bulk-out endpoint*/
-	rr3->write_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!rr3->write_urb) {
-		dev_err(dev, "Write urb allocation failure\n");
-		goto error;
-	}
+	usb_fill_bulk_urb(rr3->read_urb, udev, pipe, rr3->bulk_in_buf,
+		le16_to_cpu(ep_in->wMaxPacketSize), redrat3_handle_async, rr3);
 
 	rr3->ep_out = ep_out;
-	rr3->bulk_out_buf = usb_alloc_coherent(udev, ep_out->wMaxPacketSize,
-					       GFP_ATOMIC, &rr3->dma_out);
-	if (!rr3->bulk_out_buf) {
-		dev_err(dev, "Write buffer allocation failure\n");
-		goto error;
-	}
-
-	pipe = usb_sndbulkpipe(udev, ep_out->bEndpointAddress);
-	usb_fill_bulk_urb(rr3->write_urb, udev, pipe,
-			  rr3->bulk_out_buf, ep_out->wMaxPacketSize,
-			  (usb_complete_t)redrat3_write_bulk_callback, rr3);
-
 	rr3->udev = udev;
 
 	redrat3_reset(rr3);
diff --git a/drivers/media/rc/ttusbir.c b/drivers/media/rc/ttusbir.c
index cf0d47f57fb2..891762d167ed 100644
--- a/drivers/media/rc/ttusbir.c
+++ b/drivers/media/rc/ttusbir.c
@@ -347,6 +347,7 @@ static int ttusbir_probe(struct usb_interface *intf,
 	return 0;
 out3:
 	rc_unregister_device(rc);
+	rc = NULL;
 out2:
 	led_classdev_unregister(&tt->led);
 out:
diff --git a/drivers/media/rc/winbond-cir.c b/drivers/media/rc/winbond-cir.c
index 535a18dccbd0..87af2d3ba601 100644
--- a/drivers/media/rc/winbond-cir.c
+++ b/drivers/media/rc/winbond-cir.c
@@ -1151,6 +1151,7 @@ exit_release_wbase:
 	release_region(data->wbase, WAKEUP_IOMEM_LEN);
 exit_unregister_device:
 	rc_unregister_device(data->dev);
+	data->dev = NULL;
 exit_free_rc:
 	rc_free_device(data->dev);
 exit_unregister_led:
diff --git a/drivers/media/tuners/Kconfig b/drivers/media/tuners/Kconfig
index e8fdf713fce8..f6768cad001a 100644
--- a/drivers/media/tuners/Kconfig
+++ b/drivers/media/tuners/Kconfig
@@ -241,4 +241,18 @@ config MEDIA_TUNER_TUA9001
 	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Infineon TUA 9001 silicon tuner driver.
+
+config MEDIA_TUNER_IT913X
+	tristate "ITE Tech IT913x silicon tuner"
+	depends on MEDIA_SUPPORT && I2C
+	default m if !MEDIA_SUBDRV_AUTOSELECT
+	help
+	  ITE Tech IT913x silicon tuner driver.
+
+config MEDIA_TUNER_R820T
+	tristate "Rafael Micro R820T silicon tuner"
+	depends on MEDIA_SUPPORT && I2C
+	default m if !MEDIA_SUBDRV_AUTOSELECT
+	help
+	  Rafael Micro R820T silicon tuner driver.
 endmenu
diff --git a/drivers/media/tuners/Makefile b/drivers/media/tuners/Makefile
index 5e569b1083cf..308f108eadba 100644
--- a/drivers/media/tuners/Makefile
+++ b/drivers/media/tuners/Makefile
@@ -34,6 +34,8 @@ obj-$(CONFIG_MEDIA_TUNER_TUA9001) += tua9001.o
 obj-$(CONFIG_MEDIA_TUNER_FC0011) += fc0011.o
 obj-$(CONFIG_MEDIA_TUNER_FC0012) += fc0012.o
 obj-$(CONFIG_MEDIA_TUNER_FC0013) += fc0013.o
+obj-$(CONFIG_MEDIA_TUNER_IT913X) += tuner_it913x.o
+obj-$(CONFIG_MEDIA_TUNER_R820T) += r820t.o
 
 ccflags-y += -I$(srctree)/drivers/media/dvb-core
 ccflags-y += -I$(srctree)/drivers/media/dvb-frontends
diff --git a/drivers/media/tuners/e4000.h b/drivers/media/tuners/e4000.h
index 71b1935eb3d2..3783a0bdf855 100644
--- a/drivers/media/tuners/e4000.h
+++ b/drivers/media/tuners/e4000.h
@@ -21,6 +21,7 @@
 #ifndef E4000_H
 #define E4000_H
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 struct e4000_config {
@@ -36,8 +37,7 @@ struct e4000_config {
 	u32 clock;
 };
 
-#if defined(CONFIG_MEDIA_TUNER_E4000) || \
-	(defined(CONFIG_MEDIA_TUNER_E4000_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_E4000)
 extern struct dvb_frontend *e4000_attach(struct dvb_frontend *fe,
 		struct i2c_adapter *i2c, const struct e4000_config *cfg);
 #else
diff --git a/drivers/media/tuners/fc0011.h b/drivers/media/tuners/fc0011.h
index 0ee581f122d2..43ec893a6877 100644
--- a/drivers/media/tuners/fc0011.h
+++ b/drivers/media/tuners/fc0011.h
@@ -1,6 +1,7 @@
 #ifndef LINUX_FC0011_H_
 #define LINUX_FC0011_H_
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 
@@ -22,8 +23,7 @@ enum fc0011_fe_callback_commands {
 	FC0011_FE_CALLBACK_RESET,
 };
 
-#if defined(CONFIG_MEDIA_TUNER_FC0011) ||\
-    defined(CONFIG_MEDIA_TUNER_FC0011_MODULE)
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_FC0011)
 struct dvb_frontend *fc0011_attach(struct dvb_frontend *fe,
 				   struct i2c_adapter *i2c,
 				   const struct fc0011_config *config);
diff --git a/drivers/media/tuners/fc0012.h b/drivers/media/tuners/fc0012.h
index 54508fcc3469..1d08057e3275 100644
--- a/drivers/media/tuners/fc0012.h
+++ b/drivers/media/tuners/fc0012.h
@@ -21,6 +21,7 @@
 #ifndef _FC0012_H_
 #define _FC0012_H_
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 #include "fc001x-common.h"
 
@@ -48,8 +49,7 @@ struct fc0012_config {
 	bool clock_out;
 };
 
-#if defined(CONFIG_MEDIA_TUNER_FC0012) || \
-	(defined(CONFIG_MEDIA_TUNER_FC0012_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_FC0012)
 extern struct dvb_frontend *fc0012_attach(struct dvb_frontend *fe,
 					struct i2c_adapter *i2c,
 					const struct fc0012_config *cfg);
diff --git a/drivers/media/tuners/fc0013.h b/drivers/media/tuners/fc0013.h
index 594efd64aeec..d65d5b37f56e 100644
--- a/drivers/media/tuners/fc0013.h
+++ b/drivers/media/tuners/fc0013.h
@@ -22,11 +22,11 @@
 #ifndef _FC0013_H_
 #define _FC0013_H_
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 #include "fc001x-common.h"
 
-#if defined(CONFIG_MEDIA_TUNER_FC0013) || \
-	(defined(CONFIG_MEDIA_TUNER_FC0013_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_FC0013)
 extern struct dvb_frontend *fc0013_attach(struct dvb_frontend *fe,
 					struct i2c_adapter *i2c,
 					u8 i2c_address, int dual_master,
diff --git a/drivers/media/tuners/fc2580.h b/drivers/media/tuners/fc2580.h
index 222601e5d23b..9c43c1cc82d9 100644
--- a/drivers/media/tuners/fc2580.h
+++ b/drivers/media/tuners/fc2580.h
@@ -21,6 +21,7 @@
 #ifndef FC2580_H
 #define FC2580_H
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 struct fc2580_config {
@@ -36,8 +37,7 @@ struct fc2580_config {
 	u32 clock;
 };
 
-#if defined(CONFIG_MEDIA_TUNER_FC2580) || \
-	(defined(CONFIG_MEDIA_TUNER_FC2580_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_FC2580)
 extern struct dvb_frontend *fc2580_attach(struct dvb_frontend *fe,
 	struct i2c_adapter *i2c, const struct fc2580_config *cfg);
 #else
diff --git a/drivers/media/tuners/max2165.h b/drivers/media/tuners/max2165.h
index c063c36a93d3..26e1dc64bb67 100644
--- a/drivers/media/tuners/max2165.h
+++ b/drivers/media/tuners/max2165.h
@@ -22,6 +22,8 @@
 #ifndef __MAX2165_H__
 #define __MAX2165_H__
 
+#include <linux/kconfig.h>
+
 struct dvb_frontend;
 struct i2c_adapter;
 
@@ -30,8 +32,7 @@ struct max2165_config {
 	u8 osc_clk; /* in MHz, selectable values: 4,16,18,20,22,24,26,28 */
 };
 
-#if defined(CONFIG_MEDIA_TUNER_MAX2165) || \
-    (defined(CONFIG_MEDIA_TUNER_MAX2165_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_MAX2165)
 extern struct dvb_frontend *max2165_attach(struct dvb_frontend *fe,
 	struct i2c_adapter *i2c,
 	struct max2165_config *cfg);
diff --git a/drivers/media/tuners/mc44s803.h b/drivers/media/tuners/mc44s803.h
index 34f3892d3f6d..9aae50aca2b7 100644
--- a/drivers/media/tuners/mc44s803.h
+++ b/drivers/media/tuners/mc44s803.h
@@ -22,6 +22,8 @@
 #ifndef MC44S803_H
 #define MC44S803_H
 
+#include <linux/kconfig.h>
+
 struct dvb_frontend;
 struct i2c_adapter;
 
@@ -30,8 +32,7 @@ struct mc44s803_config {
 	u8 dig_out;
 };
 
-#if defined(CONFIG_MEDIA_TUNER_MC44S803) || \
-    (defined(CONFIG_MEDIA_TUNER_MC44S803_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_MC44S803)
 extern struct dvb_frontend *mc44s803_attach(struct dvb_frontend *fe,
 	 struct i2c_adapter *i2c, struct mc44s803_config *cfg);
 #else
diff --git a/drivers/media/tuners/mxl5005s.h b/drivers/media/tuners/mxl5005s.h
index fc8a1ffc53b4..ae8db885ad87 100644
--- a/drivers/media/tuners/mxl5005s.h
+++ b/drivers/media/tuners/mxl5005s.h
@@ -23,6 +23,8 @@
 #ifndef __MXL5005S_H
 #define __MXL5005S_H
 
+#include <linux/kconfig.h>
+
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
 
@@ -116,8 +118,7 @@ struct mxl5005s_config {
 	u8 AgcMasterByte;
 };
 
-#if defined(CONFIG_MEDIA_TUNER_MXL5005S) || \
-	(defined(CONFIG_MEDIA_TUNER_MXL5005S_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_MXL5005S)
 extern struct dvb_frontend *mxl5005s_attach(struct dvb_frontend *fe,
 					    struct i2c_adapter *i2c,
 					    struct mxl5005s_config *config);
diff --git a/drivers/media/tuners/r820t.c b/drivers/media/tuners/r820t.c
new file mode 100644
index 000000000000..4835021aa3b6
--- /dev/null
+++ b/drivers/media/tuners/r820t.c
@@ -0,0 +1,2355 @@
+/*
+ * Rafael Micro R820T driver
+ *
+ * Copyright (C) 2013 Mauro Carvalho Chehab <mchehab@redhat.com>
+ *
+ * This driver was written from scratch, based on an existing driver
+ * that it is part of rtl-sdr git tree, released under GPLv2:
+ *	https://groups.google.com/forum/#!topic/ultra-cheap-sdr/Y3rBEOFtHug
+ *	https://github.com/n1gp/gr-baz
+ *
+ * From what I understood from the threads, the original driver was converted
+ * to userspace from a Realtek tree. I couldn't find the original tree.
+ * However, the original driver look awkward on my eyes. So, I decided to
+ * write a new version from it from the scratch, while trying to reproduce
+ * everything found there.
+ *
+ * TODO:
+ *	After locking, the original driver seems to have some routines to
+ *		improve reception. This was not implemented here yet.
+ *
+ *	RF Gain set/get is not implemented.
+ *
+ *    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.
+ *
+ */
+
+#include <linux/videodev2.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/bitrev.h>
+
+#include "tuner-i2c.h"
+#include "r820t.h"
+
+/*
+ * FIXME: I think that there are only 32 registers, but better safe than
+ *	  sorry. After finishing the driver, we may review it.
+ */
+#define REG_SHADOW_START	5
+#define NUM_REGS		27
+#define NUM_IMR			5
+#define IMR_TRIAL		9
+
+#define VER_NUM  49
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "enable verbose debug messages");
+
+static int no_imr_cal;
+module_param(no_imr_cal, int, 0444);
+MODULE_PARM_DESC(no_imr_cal, "Disable IMR calibration at module init");
+
+
+/*
+ * enums and structures
+ */
+
+enum xtal_cap_value {
+	XTAL_LOW_CAP_30P = 0,
+	XTAL_LOW_CAP_20P,
+	XTAL_LOW_CAP_10P,
+	XTAL_LOW_CAP_0P,
+	XTAL_HIGH_CAP_0P
+};
+
+struct r820t_sect_type {
+	u8	phase_y;
+	u8	gain_x;
+	u16	value;
+};
+
+struct r820t_priv {
+	struct list_head		hybrid_tuner_instance_list;
+	const struct r820t_config	*cfg;
+	struct tuner_i2c_props		i2c_props;
+	struct mutex			lock;
+
+	u8				regs[NUM_REGS];
+	u8				buf[NUM_REGS + 1];
+	enum xtal_cap_value		xtal_cap_sel;
+	u16				pll;	/* kHz */
+	u32				int_freq;
+	u8				fil_cal_code;
+	bool				imr_done;
+	bool				has_lock;
+	bool				init_done;
+	struct r820t_sect_type		imr_data[NUM_IMR];
+
+	/* Store current mode */
+	u32				delsys;
+	enum v4l2_tuner_type		type;
+	v4l2_std_id			std;
+	u32				bw;	/* in MHz */
+};
+
+struct r820t_freq_range {
+	u32	freq;
+	u8	open_d;
+	u8	rf_mux_ploy;
+	u8	tf_c;
+	u8	xtal_cap20p;
+	u8	xtal_cap10p;
+	u8	xtal_cap0p;
+	u8	imr_mem;		/* Not used, currently */
+};
+
+#define VCO_POWER_REF   0x02
+#define DIP_FREQ	32000000
+
+/*
+ * Static constants
+ */
+
+static LIST_HEAD(hybrid_tuner_instance_list);
+static DEFINE_MUTEX(r820t_list_mutex);
+
+/* Those initial values start from REG_SHADOW_START */
+static const u8 r820t_init_array[NUM_REGS] = {
+	0x83, 0x32, 0x75,			/* 05 to 07 */
+	0xc0, 0x40, 0xd6, 0x6c,			/* 08 to 0b */
+	0xf5, 0x63, 0x75, 0x68,			/* 0c to 0f */
+	0x6c, 0x83, 0x80, 0x00,			/* 10 to 13 */
+	0x0f, 0x00, 0xc0, 0x30,			/* 14 to 17 */
+	0x48, 0xcc, 0x60, 0x00,			/* 18 to 1b */
+	0x54, 0xae, 0x4a, 0xc0			/* 1c to 1f */
+};
+
+/* Tuner frequency ranges */
+static const struct r820t_freq_range freq_ranges[] = {
+	{
+		.freq = 0,
+		.open_d = 0x08,		/* low */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0xdf,		/* R27[7:0]  band2,band0 */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 50,		/* Start freq, in MHz */
+		.open_d = 0x08,		/* low */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0xbe,		/* R27[7:0]  band4,band1  */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 55,		/* Start freq, in MHz */
+		.open_d = 0x08,		/* low */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x8b,		/* R27[7:0]  band7,band4 */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 60,		/* Start freq, in MHz */
+		.open_d = 0x08,		/* low */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x7b,		/* R27[7:0]  band8,band4 */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 65,		/* Start freq, in MHz */
+		.open_d = 0x08,		/* low */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x69,		/* R27[7:0]  band9,band6 */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 70,		/* Start freq, in MHz */
+		.open_d = 0x08,		/* low */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x58,		/* R27[7:0]  band10,band7 */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 75,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x44,		/* R27[7:0]  band11,band11 */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 80,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x44,		/* R27[7:0]  band11,band11 */
+		.xtal_cap20p = 0x02,	/* R16[1:0]  20pF (10)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 90,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x34,		/* R27[7:0]  band12,band11 */
+		.xtal_cap20p = 0x01,	/* R16[1:0]  10pF (01)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 100,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x34,		/* R27[7:0]  band12,band11 */
+		.xtal_cap20p = 0x01,	/* R16[1:0]  10pF (01)    */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 0,
+	}, {
+		.freq = 110,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x24,		/* R27[7:0]  band13,band11 */
+		.xtal_cap20p = 0x01,	/* R16[1:0]  10pF (01)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 1,
+	}, {
+		.freq = 120,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x24,		/* R27[7:0]  band13,band11 */
+		.xtal_cap20p = 0x01,	/* R16[1:0]  10pF (01)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 1,
+	}, {
+		.freq = 140,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x14,		/* R27[7:0]  band14,band11 */
+		.xtal_cap20p = 0x01,	/* R16[1:0]  10pF (01)   */
+		.xtal_cap10p = 0x01,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 1,
+	}, {
+		.freq = 180,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x13,		/* R27[7:0]  band14,band12 */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 1,
+	}, {
+		.freq = 220,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x13,		/* R27[7:0]  band14,band12 */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 2,
+	}, {
+		.freq = 250,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x11,		/* R27[7:0]  highest,highest */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 2,
+	}, {
+		.freq = 280,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x02,	/* R26[7:6]=0 (LPF)  R26[1:0]=2 (low) */
+		.tf_c = 0x00,		/* R27[7:0]  highest,highest */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 2,
+	}, {
+		.freq = 310,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x41,	/* R26[7:6]=1 (bypass)  R26[1:0]=1 (middle) */
+		.tf_c = 0x00,		/* R27[7:0]  highest,highest */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 2,
+	}, {
+		.freq = 450,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x41,	/* R26[7:6]=1 (bypass)  R26[1:0]=1 (middle) */
+		.tf_c = 0x00,		/* R27[7:0]  highest,highest */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 3,
+	}, {
+		.freq = 588,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x40,	/* R26[7:6]=1 (bypass)  R26[1:0]=0 (highest) */
+		.tf_c = 0x00,		/* R27[7:0]  highest,highest */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 3,
+	}, {
+		.freq = 650,		/* Start freq, in MHz */
+		.open_d = 0x00,		/* high */
+		.rf_mux_ploy = 0x40,	/* R26[7:6]=1 (bypass)  R26[1:0]=0 (highest) */
+		.tf_c = 0x00,		/* R27[7:0]  highest,highest */
+		.xtal_cap20p = 0x00,	/* R16[1:0]  0pF (00)   */
+		.xtal_cap10p = 0x00,
+		.xtal_cap0p = 0x00,
+		.imr_mem = 4,
+	}
+};
+
+static int r820t_xtal_capacitor[][2] = {
+	{ 0x0b, XTAL_LOW_CAP_30P },
+	{ 0x02, XTAL_LOW_CAP_20P },
+	{ 0x01, XTAL_LOW_CAP_10P },
+	{ 0x00, XTAL_LOW_CAP_0P  },
+	{ 0x10, XTAL_HIGH_CAP_0P },
+};
+
+/*
+ * measured with a Racal 6103E GSM test set at 928 MHz with -60 dBm
+ * input power, for raw results see:
+ *	http://steve-m.de/projects/rtl-sdr/gain_measurement/r820t/
+ */
+
+static const int r820t_lna_gain_steps[]  = {
+	0, 9, 13, 40, 38, 13, 31, 22, 26, 31, 26, 14, 19, 5, 35, 13
+};
+
+static const int r820t_mixer_gain_steps[]  = {
+	0, 5, 10, 10, 19, 9, 10, 25, 17, 10, 8, 16, 13, 6, 3, -8
+};
+
+/*
+ * I2C read/write code and shadow registers logic
+ */
+static void shadow_store(struct r820t_priv *priv, u8 reg, const u8 *val,
+			 int len)
+{
+	int r = reg - REG_SHADOW_START;
+
+	if (r < 0) {
+		len += r;
+		r = 0;
+	}
+	if (len <= 0)
+		return;
+	if (len > NUM_REGS)
+		len = NUM_REGS;
+
+	tuner_dbg("%s: prev  reg=%02x len=%d: %*ph\n",
+		  __func__, r + REG_SHADOW_START, len, len, val);
+
+	memcpy(&priv->regs[r], val, len);
+}
+
+static int r820t_write(struct r820t_priv *priv, u8 reg, const u8 *val,
+		       int len)
+{
+	int rc, size, pos = 0;
+
+	/* Store the shadow registers */
+	shadow_store(priv, reg, val, len);
+
+	do {
+		if (len > priv->cfg->max_i2c_msg_len - 1)
+			size = priv->cfg->max_i2c_msg_len - 1;
+		else
+			size = len;
+
+		/* Fill I2C buffer */
+		priv->buf[0] = reg;
+		memcpy(&priv->buf[1], &val[pos], size);
+
+		rc = tuner_i2c_xfer_send(&priv->i2c_props, priv->buf, size + 1);
+		if (rc != size + 1) {
+			tuner_info("%s: i2c wr failed=%d reg=%02x len=%d: %*ph\n",
+				   __func__, rc, reg, size, size, &priv->buf[1]);
+			if (rc < 0)
+				return rc;
+			return -EREMOTEIO;
+		}
+		tuner_dbg("%s: i2c wr reg=%02x len=%d: %*ph\n",
+			  __func__, reg, size, size, &priv->buf[1]);
+
+		reg += size;
+		len -= size;
+		pos += size;
+	} while (len > 0);
+
+	return 0;
+}
+
+static int r820t_write_reg(struct r820t_priv *priv, u8 reg, u8 val)
+{
+	return r820t_write(priv, reg, &val, 1);
+}
+
+static int r820t_read_cache_reg(struct r820t_priv *priv, int reg)
+{
+	reg -= REG_SHADOW_START;
+
+	if (reg >= 0 && reg < NUM_REGS)
+		return priv->regs[reg];
+	else
+		return -EINVAL;
+}
+
+static int r820t_write_reg_mask(struct r820t_priv *priv, u8 reg, u8 val,
+				u8 bit_mask)
+{
+	int rc = r820t_read_cache_reg(priv, reg);
+
+	if (rc < 0)
+		return rc;
+
+	val = (rc & ~bit_mask) | (val & bit_mask);
+
+	return r820t_write(priv, reg, &val, 1);
+}
+
+static int r820t_read(struct r820t_priv *priv, u8 reg, u8 *val, int len)
+{
+	int rc, i;
+	u8 *p = &priv->buf[1];
+
+	priv->buf[0] = reg;
+
+	rc = tuner_i2c_xfer_send_recv(&priv->i2c_props, priv->buf, 1, p, len);
+	if (rc != len) {
+		tuner_info("%s: i2c rd failed=%d reg=%02x len=%d: %*ph\n",
+			   __func__, rc, reg, len, len, p);
+		if (rc < 0)
+			return rc;
+		return -EREMOTEIO;
+	}
+
+	/* Copy data to the output buffer */
+	for (i = 0; i < len; i++)
+		val[i] = bitrev8(p[i]);
+
+	tuner_dbg("%s: i2c rd reg=%02x len=%d: %*ph\n",
+		  __func__, reg, len, len, val);
+
+	return 0;
+}
+
+/*
+ * r820t tuning logic
+ */
+
+static int r820t_set_mux(struct r820t_priv *priv, u32 freq)
+{
+	const struct r820t_freq_range *range;
+	int i, rc;
+	u8 val, reg08, reg09;
+
+	/* Get the proper frequency range */
+	freq = freq / 1000000;
+	for (i = 0; i < ARRAY_SIZE(freq_ranges) - 1; i++) {
+		if (freq < freq_ranges[i + 1].freq)
+			break;
+	}
+	range = &freq_ranges[i];
+
+	tuner_dbg("set r820t range#%d for frequency %d MHz\n", i, freq);
+
+	/* Open Drain */
+	rc = r820t_write_reg_mask(priv, 0x17, range->open_d, 0x08);
+	if (rc < 0)
+		return rc;
+
+	/* RF_MUX,Polymux */
+	rc = r820t_write_reg_mask(priv, 0x1a, range->rf_mux_ploy, 0xc3);
+	if (rc < 0)
+		return rc;
+
+	/* TF BAND */
+	rc = r820t_write_reg(priv, 0x1b, range->tf_c);
+	if (rc < 0)
+		return rc;
+
+	/* XTAL CAP & Drive */
+	switch (priv->xtal_cap_sel) {
+	case XTAL_LOW_CAP_30P:
+	case XTAL_LOW_CAP_20P:
+		val = range->xtal_cap20p | 0x08;
+		break;
+	case XTAL_LOW_CAP_10P:
+		val = range->xtal_cap10p | 0x08;
+		break;
+	case XTAL_HIGH_CAP_0P:
+		val = range->xtal_cap0p | 0x00;
+		break;
+	default:
+	case XTAL_LOW_CAP_0P:
+		val = range->xtal_cap0p | 0x08;
+		break;
+	}
+	rc = r820t_write_reg_mask(priv, 0x10, val, 0x0b);
+	if (rc < 0)
+		return rc;
+
+	if (priv->imr_done) {
+		reg08 = priv->imr_data[range->imr_mem].gain_x;
+		reg09 = priv->imr_data[range->imr_mem].phase_y;
+	} else {
+		reg08 = 0;
+		reg09 = 0;
+	}
+	rc = r820t_write_reg_mask(priv, 0x08, reg08, 0x3f);
+	if (rc < 0)
+		return rc;
+
+	rc = r820t_write_reg_mask(priv, 0x09, reg09, 0x3f);
+
+	return rc;
+}
+
+static int r820t_set_pll(struct r820t_priv *priv, enum v4l2_tuner_type type,
+			 u32 freq)
+{
+	u32 vco_freq;
+	int rc, i;
+	unsigned sleep_time = 10000;
+	u32 vco_fra;		/* VCO contribution by SDM (kHz) */
+	u32 vco_min  = 1770000;
+	u32 vco_max  = vco_min * 2;
+	u32 pll_ref;
+	u16 n_sdm = 2;
+	u16 sdm = 0;
+	u8 mix_div = 2;
+	u8 div_buf = 0;
+	u8 div_num = 0;
+	u8 refdiv2 = 0;
+	u8 ni, si, nint, vco_fine_tune, val;
+	u8 data[5];
+
+	/* Frequency in kHz */
+	freq = freq / 1000;
+	pll_ref = priv->cfg->xtal / 1000;
+
+#if 0
+	/* Doesn't exist on rtl-sdk, and on field tests, caused troubles */
+	if ((priv->cfg->rafael_chip == CHIP_R620D) ||
+	   (priv->cfg->rafael_chip == CHIP_R828D) ||
+	   (priv->cfg->rafael_chip == CHIP_R828)) {
+		/* ref set refdiv2, reffreq = Xtal/2 on ATV application */
+		if (type != V4L2_TUNER_DIGITAL_TV) {
+			pll_ref /= 2;
+			refdiv2 = 0x10;
+			sleep_time = 20000;
+		}
+	} else {
+		if (priv->cfg->xtal > 24000000) {
+			pll_ref /= 2;
+			refdiv2 = 0x10;
+		}
+	}
+#endif
+
+	rc = r820t_write_reg_mask(priv, 0x10, refdiv2, 0x10);
+	if (rc < 0)
+		return rc;
+
+	/* set pll autotune = 128kHz */
+	rc = r820t_write_reg_mask(priv, 0x1a, 0x00, 0x0c);
+	if (rc < 0)
+		return rc;
+
+	/* set VCO current = 100 */
+	rc = r820t_write_reg_mask(priv, 0x12, 0x80, 0xe0);
+	if (rc < 0)
+		return rc;
+
+	/* Calculate divider */
+	while (mix_div <= 64) {
+		if (((freq * mix_div) >= vco_min) &&
+		   ((freq * mix_div) < vco_max)) {
+			div_buf = mix_div;
+			while (div_buf > 2) {
+				div_buf = div_buf >> 1;
+				div_num++;
+			}
+			break;
+		}
+		mix_div = mix_div << 1;
+	}
+
+	rc = r820t_read(priv, 0x00, data, sizeof(data));
+	if (rc < 0)
+		return rc;
+
+	vco_fine_tune = (data[4] & 0x30) >> 4;
+
+	if (vco_fine_tune > VCO_POWER_REF)
+		div_num = div_num - 1;
+	else if (vco_fine_tune < VCO_POWER_REF)
+		div_num = div_num + 1;
+
+	rc = r820t_write_reg_mask(priv, 0x10, div_num << 5, 0xe0);
+	if (rc < 0)
+		return rc;
+
+	vco_freq = freq * mix_div;
+	nint = vco_freq / (2 * pll_ref);
+	vco_fra = vco_freq - 2 * pll_ref * nint;
+
+	/* boundary spur prevention */
+	if (vco_fra < pll_ref / 64) {
+		vco_fra = 0;
+	} else if (vco_fra > pll_ref * 127 / 64) {
+		vco_fra = 0;
+		nint++;
+	} else if ((vco_fra > pll_ref * 127 / 128) && (vco_fra < pll_ref)) {
+		vco_fra = pll_ref * 127 / 128;
+	} else if ((vco_fra > pll_ref) && (vco_fra < pll_ref * 129 / 128)) {
+		vco_fra = pll_ref * 129 / 128;
+	}
+
+	if (nint > 63) {
+		tuner_info("No valid PLL values for %u kHz!\n", freq);
+		return -EINVAL;
+	}
+
+	ni = (nint - 13) / 4;
+	si = nint - 4 * ni - 13;
+
+	rc = r820t_write_reg(priv, 0x14, ni + (si << 6));
+	if (rc < 0)
+		return rc;
+
+	/* pw_sdm */
+	if (!vco_fra)
+		val = 0x08;
+	else
+		val = 0x00;
+
+	rc = r820t_write_reg_mask(priv, 0x12, val, 0x08);
+	if (rc < 0)
+		return rc;
+
+	/* sdm calculator */
+	while (vco_fra > 1) {
+		if (vco_fra > (2 * pll_ref / n_sdm)) {
+			sdm = sdm + 32768 / (n_sdm / 2);
+			vco_fra = vco_fra - 2 * pll_ref / n_sdm;
+			if (n_sdm >= 0x8000)
+				break;
+		}
+		n_sdm = n_sdm << 1;
+	}
+
+	tuner_dbg("freq %d kHz, pll ref %d%s, sdm=0x%04x\n",
+		  freq, pll_ref, refdiv2 ? " / 2" : "", sdm);
+
+	rc = r820t_write_reg(priv, 0x16, sdm >> 8);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x15, sdm & 0xff);
+	if (rc < 0)
+		return rc;
+
+	for (i = 0; i < 2; i++) {
+		usleep_range(sleep_time, sleep_time + 1000);
+
+		/* Check if PLL has locked */
+		rc = r820t_read(priv, 0x00, data, 3);
+		if (rc < 0)
+			return rc;
+		if (data[2] & 0x40)
+			break;
+
+		if (!i) {
+			/* Didn't lock. Increase VCO current */
+			rc = r820t_write_reg_mask(priv, 0x12, 0x60, 0xe0);
+			if (rc < 0)
+				return rc;
+		}
+	}
+
+	if (!(data[2] & 0x40)) {
+		priv->has_lock = false;
+		return 0;
+	}
+
+	priv->has_lock = true;
+	tuner_dbg("tuner has lock at frequency %d kHz\n", freq);
+
+	/* set pll autotune = 8kHz */
+	rc = r820t_write_reg_mask(priv, 0x1a, 0x08, 0x08);
+
+	return rc;
+}
+
+static int r820t_sysfreq_sel(struct r820t_priv *priv, u32 freq,
+			     enum v4l2_tuner_type type,
+			     v4l2_std_id std,
+			     u32 delsys)
+{
+	int rc;
+	u8 mixer_top, lna_top, cp_cur, div_buf_cur, lna_vth_l, mixer_vth_l;
+	u8 air_cable1_in, cable2_in, pre_dect, lna_discharge, filter_cur;
+
+	tuner_dbg("adjusting tuner parameters for the standard\n");
+
+	switch (delsys) {
+	case SYS_DVBT:
+		if ((freq == 506000000) || (freq == 666000000) ||
+		   (freq == 818000000)) {
+			mixer_top = 0x14;	/* mixer top:14 , top-1, low-discharge */
+			lna_top = 0xe5;		/* detect bw 3, lna top:4, predet top:2 */
+			cp_cur = 0x28;		/* 101, 0.2 */
+			div_buf_cur = 0x20;	/* 10, 200u */
+		} else {
+			mixer_top = 0x24;	/* mixer top:13 , top-1, low-discharge */
+			lna_top = 0xe5;		/* detect bw 3, lna top:4, predet top:2 */
+			cp_cur = 0x38;		/* 111, auto */
+			div_buf_cur = 0x30;	/* 11, 150u */
+		}
+		lna_vth_l = 0x53;		/* lna vth 0.84	,  vtl 0.64 */
+		mixer_vth_l = 0x75;		/* mixer vth 1.04, vtl 0.84 */
+		air_cable1_in = 0x00;
+		cable2_in = 0x00;
+		pre_dect = 0x40;
+		lna_discharge = 14;
+		filter_cur = 0x40;		/* 10, low */
+		break;
+	case SYS_DVBT2:
+		mixer_top = 0x24;	/* mixer top:13 , top-1, low-discharge */
+		lna_top = 0xe5;		/* detect bw 3, lna top:4, predet top:2 */
+		lna_vth_l = 0x53;	/* lna vth 0.84	,  vtl 0.64 */
+		mixer_vth_l = 0x75;	/* mixer vth 1.04, vtl 0.84 */
+		air_cable1_in = 0x00;
+		cable2_in = 0x00;
+		pre_dect = 0x40;
+		lna_discharge = 14;
+		cp_cur = 0x38;		/* 111, auto */
+		div_buf_cur = 0x30;	/* 11, 150u */
+		filter_cur = 0x40;	/* 10, low */
+		break;
+	case SYS_ISDBT:
+		mixer_top = 0x24;	/* mixer top:13 , top-1, low-discharge */
+		lna_top = 0xe5;		/* detect bw 3, lna top:4, predet top:2 */
+		lna_vth_l = 0x75;	/* lna vth 1.04	,  vtl 0.84 */
+		mixer_vth_l = 0x75;	/* mixer vth 1.04, vtl 0.84 */
+		air_cable1_in = 0x00;
+		cable2_in = 0x00;
+		pre_dect = 0x40;
+		lna_discharge = 14;
+		cp_cur = 0x38;		/* 111, auto */
+		div_buf_cur = 0x30;	/* 11, 150u */
+		filter_cur = 0x40;	/* 10, low */
+		break;
+	default: /* DVB-T 8M */
+		mixer_top = 0x24;	/* mixer top:13 , top-1, low-discharge */
+		lna_top = 0xe5;		/* detect bw 3, lna top:4, predet top:2 */
+		lna_vth_l = 0x53;	/* lna vth 0.84	,  vtl 0.64 */
+		mixer_vth_l = 0x75;	/* mixer vth 1.04, vtl 0.84 */
+		air_cable1_in = 0x00;
+		cable2_in = 0x00;
+		pre_dect = 0x40;
+		lna_discharge = 14;
+		cp_cur = 0x38;		/* 111, auto */
+		div_buf_cur = 0x30;	/* 11, 150u */
+		filter_cur = 0x40;	/* 10, low */
+		break;
+	}
+
+	if (priv->cfg->use_diplexer &&
+	   ((priv->cfg->rafael_chip == CHIP_R820T) ||
+	   (priv->cfg->rafael_chip == CHIP_R828S) ||
+	   (priv->cfg->rafael_chip == CHIP_R820C))) {
+		if (freq > DIP_FREQ)
+			air_cable1_in = 0x00;
+		else
+			air_cable1_in = 0x60;
+		cable2_in = 0x00;
+	}
+
+
+	if (priv->cfg->use_predetect) {
+		rc = r820t_write_reg_mask(priv, 0x06, pre_dect, 0x40);
+		if (rc < 0)
+			return rc;
+	}
+
+	rc = r820t_write_reg_mask(priv, 0x1d, lna_top, 0xc7);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg_mask(priv, 0x1c, mixer_top, 0xf8);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x0d, lna_vth_l);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x0e, mixer_vth_l);
+	if (rc < 0)
+		return rc;
+
+	/* Air-IN only for Astrometa */
+	rc = r820t_write_reg_mask(priv, 0x05, air_cable1_in, 0x60);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg_mask(priv, 0x06, cable2_in, 0x08);
+	if (rc < 0)
+		return rc;
+
+	rc = r820t_write_reg_mask(priv, 0x11, cp_cur, 0x38);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg_mask(priv, 0x17, div_buf_cur, 0x30);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg_mask(priv, 0x0a, filter_cur, 0x60);
+	if (rc < 0)
+		return rc;
+	/*
+	 * Original driver initializes regs 0x05 and 0x06 with the
+	 * same value again on this point. Probably, it is just an
+	 * error there
+	 */
+
+	/*
+	 * Set LNA
+	 */
+
+	tuner_dbg("adjusting LNA parameters\n");
+	if (type != V4L2_TUNER_ANALOG_TV) {
+		/* LNA TOP: lowest */
+		rc = r820t_write_reg_mask(priv, 0x1d, 0, 0x38);
+		if (rc < 0)
+			return rc;
+
+		/* 0: normal mode */
+		rc = r820t_write_reg_mask(priv, 0x1c, 0, 0x04);
+		if (rc < 0)
+			return rc;
+
+		/* 0: PRE_DECT off */
+		rc = r820t_write_reg_mask(priv, 0x06, 0, 0x40);
+		if (rc < 0)
+			return rc;
+
+		/* agc clk 250hz */
+		rc = r820t_write_reg_mask(priv, 0x1a, 0x30, 0x30);
+		if (rc < 0)
+			return rc;
+
+		msleep(250);
+
+		/* write LNA TOP = 3 */
+		rc = r820t_write_reg_mask(priv, 0x1d, 0x18, 0x38);
+		if (rc < 0)
+			return rc;
+
+		/*
+		 * write discharge mode
+		 * FIXME: IMHO, the mask here is wrong, but it matches
+		 * what's there at the original driver
+		 */
+		rc = r820t_write_reg_mask(priv, 0x1c, mixer_top, 0x04);
+		if (rc < 0)
+			return rc;
+
+		/* LNA discharge current */
+		rc = r820t_write_reg_mask(priv, 0x1e, lna_discharge, 0x1f);
+		if (rc < 0)
+			return rc;
+
+		/* agc clk 60hz */
+		rc = r820t_write_reg_mask(priv, 0x1a, 0x20, 0x30);
+		if (rc < 0)
+			return rc;
+	} else {
+		/* PRE_DECT off */
+		rc = r820t_write_reg_mask(priv, 0x06, 0, 0x40);
+		if (rc < 0)
+			return rc;
+
+		/* write LNA TOP */
+		rc = r820t_write_reg_mask(priv, 0x1d, lna_top, 0x38);
+		if (rc < 0)
+			return rc;
+
+		/*
+		 * write discharge mode
+		 * FIXME: IMHO, the mask here is wrong, but it matches
+		 * what's there at the original driver
+		 */
+		rc = r820t_write_reg_mask(priv, 0x1c, mixer_top, 0x04);
+		if (rc < 0)
+			return rc;
+
+		/* LNA discharge current */
+		rc = r820t_write_reg_mask(priv, 0x1e, lna_discharge, 0x1f);
+		if (rc < 0)
+			return rc;
+
+		/* agc clk 1Khz, external det1 cap 1u */
+		rc = r820t_write_reg_mask(priv, 0x1a, 0x00, 0x30);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_write_reg_mask(priv, 0x10, 0x00, 0x04);
+		if (rc < 0)
+			return rc;
+	 }
+	 return 0;
+}
+
+static int r820t_set_tv_standard(struct r820t_priv *priv,
+				 unsigned bw,
+				 enum v4l2_tuner_type type,
+				 v4l2_std_id std, u32 delsys)
+
+{
+	int rc, i;
+	u32 if_khz, filt_cal_lo;
+	u8 data[5], val;
+	u8 filt_gain, img_r, filt_q, hp_cor, ext_enable, loop_through;
+	u8 lt_att, flt_ext_widest, polyfil_cur;
+	bool need_calibration;
+
+	tuner_dbg("selecting the delivery system\n");
+
+	if (delsys == SYS_ISDBT) {
+		if_khz = 4063;
+		filt_cal_lo = 59000;
+		filt_gain = 0x10;	/* +3db, 6mhz on */
+		img_r = 0x00;		/* image negative */
+		filt_q = 0x10;		/* r10[4]:low q(1'b1) */
+		hp_cor = 0x6a;		/* 1.7m disable, +2cap, 1.25mhz */
+		ext_enable = 0x40;	/* r30[6], ext enable; r30[5]:0 ext at lna max */
+		loop_through = 0x00;	/* r5[7], lt on */
+		lt_att = 0x00;		/* r31[7], lt att enable */
+		flt_ext_widest = 0x00;	/* r15[7]: flt_ext_wide off */
+		polyfil_cur = 0x60;	/* r25[6:5]:min */
+	} else {
+		if (bw <= 6) {
+			if_khz = 3570;
+			filt_cal_lo = 56000;	/* 52000->56000 */
+			filt_gain = 0x10;	/* +3db, 6mhz on */
+			img_r = 0x00;		/* image negative */
+			filt_q = 0x10;		/* r10[4]:low q(1'b1) */
+			hp_cor = 0x6b;		/* 1.7m disable, +2cap, 1.0mhz */
+			ext_enable = 0x60;	/* r30[6]=1 ext enable; r30[5]:1 ext at lna max-1 */
+			loop_through = 0x00;	/* r5[7], lt on */
+			lt_att = 0x00;		/* r31[7], lt att enable */
+			flt_ext_widest = 0x00;	/* r15[7]: flt_ext_wide off */
+			polyfil_cur = 0x60;	/* r25[6:5]:min */
+		} else if (bw == 7) {
+#if 0
+			/*
+			 * There are two 7 MHz tables defined on the original
+			 * driver, but just the second one seems to be visible
+			 * by rtl2832. Keep this one here commented, as it
+			 * might be needed in the future
+			 */
+
+			if_khz = 4070;
+			filt_cal_lo = 60000;
+			filt_gain = 0x10;	/* +3db, 6mhz on */
+			img_r = 0x00;		/* image negative */
+			filt_q = 0x10;		/* r10[4]:low q(1'b1) */
+			hp_cor = 0x2b;		/* 1.7m disable, +1cap, 1.0mhz */
+			ext_enable = 0x60;	/* r30[6]=1 ext enable; r30[5]:1 ext at lna max-1 */
+			loop_through = 0x00;	/* r5[7], lt on */
+			lt_att = 0x00;		/* r31[7], lt att enable */
+			flt_ext_widest = 0x00;	/* r15[7]: flt_ext_wide off */
+			polyfil_cur = 0x60;	/* r25[6:5]:min */
+#endif
+			/* 7 MHz, second table */
+			if_khz = 4570;
+			filt_cal_lo = 63000;
+			filt_gain = 0x10;	/* +3db, 6mhz on */
+			img_r = 0x00;		/* image negative */
+			filt_q = 0x10;		/* r10[4]:low q(1'b1) */
+			hp_cor = 0x2a;		/* 1.7m disable, +1cap, 1.25mhz */
+			ext_enable = 0x60;	/* r30[6]=1 ext enable; r30[5]:1 ext at lna max-1 */
+			loop_through = 0x00;	/* r5[7], lt on */
+			lt_att = 0x00;		/* r31[7], lt att enable */
+			flt_ext_widest = 0x00;	/* r15[7]: flt_ext_wide off */
+			polyfil_cur = 0x60;	/* r25[6:5]:min */
+		} else {
+			if_khz = 4570;
+			filt_cal_lo = 68500;
+			filt_gain = 0x10;	/* +3db, 6mhz on */
+			img_r = 0x00;		/* image negative */
+			filt_q = 0x10;		/* r10[4]:low q(1'b1) */
+			hp_cor = 0x0b;		/* 1.7m disable, +0cap, 1.0mhz */
+			ext_enable = 0x60;	/* r30[6]=1 ext enable; r30[5]:1 ext at lna max-1 */
+			loop_through = 0x00;	/* r5[7], lt on */
+			lt_att = 0x00;		/* r31[7], lt att enable */
+			flt_ext_widest = 0x00;	/* r15[7]: flt_ext_wide off */
+			polyfil_cur = 0x60;	/* r25[6:5]:min */
+		}
+	}
+
+	/* Initialize the shadow registers */
+	memcpy(priv->regs, r820t_init_array, sizeof(r820t_init_array));
+
+	/* Init Flag & Xtal_check Result */
+	if (priv->imr_done)
+		val = 1 | priv->xtal_cap_sel << 1;
+	else
+		val = 0;
+	rc = r820t_write_reg_mask(priv, 0x0c, val, 0x0f);
+	if (rc < 0)
+		return rc;
+
+	/* version */
+	rc = r820t_write_reg_mask(priv, 0x13, VER_NUM, 0x3f);
+	if (rc < 0)
+		return rc;
+
+	/* for LT Gain test */
+	if (type != V4L2_TUNER_ANALOG_TV) {
+		rc = r820t_write_reg_mask(priv, 0x1d, 0x00, 0x38);
+		if (rc < 0)
+			return rc;
+		usleep_range(1000, 2000);
+	}
+	priv->int_freq = if_khz * 1000;
+
+	/* Check if standard changed. If so, filter calibration is needed */
+	if (type != priv->type)
+		need_calibration = true;
+	else if ((type == V4L2_TUNER_ANALOG_TV) && (std != priv->std))
+		need_calibration = true;
+	else if ((type == V4L2_TUNER_DIGITAL_TV) &&
+		 ((delsys != priv->delsys) || bw != priv->bw))
+		need_calibration = true;
+	else
+		need_calibration = false;
+
+	if (need_calibration) {
+		tuner_dbg("calibrating the tuner\n");
+		for (i = 0; i < 2; i++) {
+			/* Set filt_cap */
+			rc = r820t_write_reg_mask(priv, 0x0b, hp_cor, 0x60);
+			if (rc < 0)
+				return rc;
+
+			/* set cali clk =on */
+			rc = r820t_write_reg_mask(priv, 0x0f, 0x04, 0x04);
+			if (rc < 0)
+				return rc;
+
+			/* X'tal cap 0pF for PLL */
+			rc = r820t_write_reg_mask(priv, 0x10, 0x00, 0x03);
+			if (rc < 0)
+				return rc;
+
+			rc = r820t_set_pll(priv, type, filt_cal_lo * 1000);
+			if (rc < 0 || !priv->has_lock)
+				return rc;
+
+			/* Start Trigger */
+			rc = r820t_write_reg_mask(priv, 0x0b, 0x10, 0x10);
+			if (rc < 0)
+				return rc;
+
+			usleep_range(1000, 2000);
+
+			/* Stop Trigger */
+			rc = r820t_write_reg_mask(priv, 0x0b, 0x00, 0x10);
+			if (rc < 0)
+				return rc;
+
+			/* set cali clk =off */
+			rc = r820t_write_reg_mask(priv, 0x0f, 0x00, 0x04);
+			if (rc < 0)
+				return rc;
+
+			/* Check if calibration worked */
+			rc = r820t_read(priv, 0x00, data, sizeof(data));
+			if (rc < 0)
+				return rc;
+
+			priv->fil_cal_code = data[4] & 0x0f;
+			if (priv->fil_cal_code && priv->fil_cal_code != 0x0f)
+				break;
+		}
+		/* narrowest */
+		if (priv->fil_cal_code == 0x0f)
+			priv->fil_cal_code = 0;
+	}
+
+	rc = r820t_write_reg_mask(priv, 0x0a,
+				  filt_q | priv->fil_cal_code, 0x1f);
+	if (rc < 0)
+		return rc;
+
+	/* Set BW, Filter_gain, & HP corner */
+	rc = r820t_write_reg_mask(priv, 0x0b, hp_cor, 0xef);
+	if (rc < 0)
+		return rc;
+
+
+	/* Set Img_R */
+	rc = r820t_write_reg_mask(priv, 0x07, img_r, 0x80);
+	if (rc < 0)
+		return rc;
+
+	/* Set filt_3dB, V6MHz */
+	rc = r820t_write_reg_mask(priv, 0x06, filt_gain, 0x30);
+	if (rc < 0)
+		return rc;
+
+	/* channel filter extension */
+	rc = r820t_write_reg_mask(priv, 0x1e, ext_enable, 0x60);
+	if (rc < 0)
+		return rc;
+
+	/* Loop through */
+	rc = r820t_write_reg_mask(priv, 0x05, loop_through, 0x80);
+	if (rc < 0)
+		return rc;
+
+	/* Loop through attenuation */
+	rc = r820t_write_reg_mask(priv, 0x1f, lt_att, 0x80);
+	if (rc < 0)
+		return rc;
+
+	/* filter extension widest */
+	rc = r820t_write_reg_mask(priv, 0x0f, flt_ext_widest, 0x80);
+	if (rc < 0)
+		return rc;
+
+	/* RF poly filter current */
+	rc = r820t_write_reg_mask(priv, 0x19, polyfil_cur, 0x60);
+	if (rc < 0)
+		return rc;
+
+	/* Store current standard. If it changes, re-calibrate the tuner */
+	priv->delsys = delsys;
+	priv->type = type;
+	priv->std = std;
+	priv->bw = bw;
+
+	return 0;
+}
+
+static int r820t_read_gain(struct r820t_priv *priv)
+{
+	u8 data[4];
+	int rc;
+
+	rc = r820t_read(priv, 0x00, data, sizeof(data));
+	if (rc < 0)
+		return rc;
+
+	return ((data[3] & 0x0f) << 1) + ((data[3] & 0xf0) >> 4);
+}
+
+#if 0
+/* FIXME: This routine requires more testing */
+static int r820t_set_gain_mode(struct r820t_priv *priv,
+			       bool set_manual_gain,
+			       int gain)
+{
+	int rc;
+
+	if (set_manual_gain) {
+		int i, total_gain = 0;
+		uint8_t mix_index = 0, lna_index = 0;
+		u8 data[4];
+
+		/* LNA auto off */
+		rc = r820t_write_reg_mask(priv, 0x05, 0x10, 0x10);
+		if (rc < 0)
+			return rc;
+
+		 /* Mixer auto off */
+		rc = r820t_write_reg_mask(priv, 0x07, 0, 0x10);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_read(priv, 0x00, data, sizeof(data));
+		if (rc < 0)
+			return rc;
+
+		/* set fixed VGA gain for now (16.3 dB) */
+		rc = r820t_write_reg_mask(priv, 0x0c, 0x08, 0x9f);
+		if (rc < 0)
+			return rc;
+
+		for (i = 0; i < 15; i++) {
+			if (total_gain >= gain)
+				break;
+
+			total_gain += r820t_lna_gain_steps[++lna_index];
+
+			if (total_gain >= gain)
+				break;
+
+			total_gain += r820t_mixer_gain_steps[++mix_index];
+		}
+
+		/* set LNA gain */
+		rc = r820t_write_reg_mask(priv, 0x05, lna_index, 0x0f);
+		if (rc < 0)
+			return rc;
+
+		/* set Mixer gain */
+		rc = r820t_write_reg_mask(priv, 0x07, mix_index, 0x0f);
+		if (rc < 0)
+			return rc;
+	} else {
+		/* LNA */
+		rc = r820t_write_reg_mask(priv, 0x05, 0, 0x10);
+		if (rc < 0)
+			return rc;
+
+		/* Mixer */
+		rc = r820t_write_reg_mask(priv, 0x07, 0x10, 0x10);
+		if (rc < 0)
+			return rc;
+
+		/* set fixed VGA gain for now (26.5 dB) */
+		rc = r820t_write_reg_mask(priv, 0x0c, 0x0b, 0x9f);
+		if (rc < 0)
+			return rc;
+	}
+
+	return 0;
+}
+#endif
+
+static int generic_set_freq(struct dvb_frontend *fe,
+			    u32 freq /* in HZ */,
+			    unsigned bw,
+			    enum v4l2_tuner_type type,
+			    v4l2_std_id std, u32 delsys)
+{
+	struct r820t_priv		*priv = fe->tuner_priv;
+	int				rc = -EINVAL;
+	u32				lo_freq;
+
+	tuner_dbg("should set frequency to %d kHz, bw %d MHz\n",
+		  freq / 1000, bw);
+
+	rc = r820t_set_tv_standard(priv, bw, type, std, delsys);
+	if (rc < 0)
+		goto err;
+
+	if ((type == V4L2_TUNER_ANALOG_TV) && (std == V4L2_STD_SECAM_LC))
+		lo_freq = freq - priv->int_freq;
+	 else
+		lo_freq = freq + priv->int_freq;
+
+	rc = r820t_set_mux(priv, lo_freq);
+	if (rc < 0)
+		goto err;
+
+	rc = r820t_set_pll(priv, type, lo_freq);
+	if (rc < 0 || !priv->has_lock)
+		goto err;
+
+	rc = r820t_sysfreq_sel(priv, freq, type, std, delsys);
+	if (rc < 0)
+		goto err;
+
+	tuner_dbg("%s: PLL locked on frequency %d Hz, gain=%d\n",
+		  __func__, freq, r820t_read_gain(priv));
+
+err:
+
+	if (rc < 0)
+		tuner_dbg("%s: failed=%d\n", __func__, rc);
+	return rc;
+}
+
+/*
+ * r820t standby logic
+ */
+
+static int r820t_standby(struct r820t_priv *priv)
+{
+	int rc;
+
+	/* If device was not initialized yet, don't need to standby */
+	if (!priv->init_done)
+		return 0;
+
+	rc = r820t_write_reg(priv, 0x06, 0xb1);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x05, 0x03);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x07, 0x3a);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x08, 0x40);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x09, 0xc0);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x0a, 0x36);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x0c, 0x35);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x0f, 0x68);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x11, 0x03);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x17, 0xf4);
+	if (rc < 0)
+		return rc;
+	rc = r820t_write_reg(priv, 0x19, 0x0c);
+
+	/* Force initial calibration */
+	priv->type = -1;
+
+	return rc;
+}
+
+/*
+ * r820t device init logic
+ */
+
+static int r820t_xtal_check(struct r820t_priv *priv)
+{
+	int rc, i;
+	u8 data[3], val;
+
+	/* Initialize the shadow registers */
+	memcpy(priv->regs, r820t_init_array, sizeof(r820t_init_array));
+
+	/* cap 30pF & Drive Low */
+	rc = r820t_write_reg_mask(priv, 0x10, 0x0b, 0x0b);
+	if (rc < 0)
+		return rc;
+
+	/* set pll autotune = 128kHz */
+	rc = r820t_write_reg_mask(priv, 0x1a, 0x00, 0x0c);
+	if (rc < 0)
+		return rc;
+
+	/* set manual initial reg = 111111;  */
+	rc = r820t_write_reg_mask(priv, 0x13, 0x7f, 0x7f);
+	if (rc < 0)
+		return rc;
+
+	/* set auto */
+	rc = r820t_write_reg_mask(priv, 0x13, 0x00, 0x40);
+	if (rc < 0)
+		return rc;
+
+	/* Try several xtal capacitor alternatives */
+	for (i = 0; i < ARRAY_SIZE(r820t_xtal_capacitor); i++) {
+		rc = r820t_write_reg_mask(priv, 0x10,
+					  r820t_xtal_capacitor[i][0], 0x1b);
+		if (rc < 0)
+			return rc;
+
+		usleep_range(5000, 6000);
+
+		rc = r820t_read(priv, 0x00, data, sizeof(data));
+		if (rc < 0)
+			return rc;
+		if (!(data[2] & 0x40))
+			continue;
+
+		val = data[2] & 0x3f;
+
+		if (priv->cfg->xtal == 16000000 && (val > 29 || val < 23))
+			break;
+
+		if (val != 0x3f)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(r820t_xtal_capacitor))
+		return -EINVAL;
+
+	return r820t_xtal_capacitor[i][1];
+}
+
+static int r820t_imr_prepare(struct r820t_priv *priv)
+{
+	int rc;
+
+	/* Initialize the shadow registers */
+	memcpy(priv->regs, r820t_init_array, sizeof(r820t_init_array));
+
+	/* lna off (air-in off) */
+	rc = r820t_write_reg_mask(priv, 0x05, 0x20, 0x20);
+	if (rc < 0)
+		return rc;
+
+	/* mixer gain mode = manual */
+	rc = r820t_write_reg_mask(priv, 0x07, 0, 0x10);
+	if (rc < 0)
+		return rc;
+
+	/* filter corner = lowest */
+	rc = r820t_write_reg_mask(priv, 0x0a, 0x0f, 0x0f);
+	if (rc < 0)
+		return rc;
+
+	/* filter bw=+2cap, hp=5M */
+	rc = r820t_write_reg_mask(priv, 0x0b, 0x60, 0x6f);
+	if (rc < 0)
+		return rc;
+
+	/* adc=on, vga code mode, gain = 26.5dB   */
+	rc = r820t_write_reg_mask(priv, 0x0c, 0x0b, 0x9f);
+	if (rc < 0)
+		return rc;
+
+	/* ring clk = on */
+	rc = r820t_write_reg_mask(priv, 0x0f, 0, 0x08);
+	if (rc < 0)
+		return rc;
+
+	/* ring power = on */
+	rc = r820t_write_reg_mask(priv, 0x18, 0x10, 0x10);
+	if (rc < 0)
+		return rc;
+
+	/* from ring = ring pll in */
+	rc = r820t_write_reg_mask(priv, 0x1c, 0x02, 0x02);
+	if (rc < 0)
+		return rc;
+
+	/* sw_pdect = det3 */
+	rc = r820t_write_reg_mask(priv, 0x1e, 0x80, 0x80);
+	if (rc < 0)
+		return rc;
+
+	/* Set filt_3dB */
+	rc = r820t_write_reg_mask(priv, 0x06, 0x20, 0x20);
+
+	return rc;
+}
+
+static int r820t_multi_read(struct r820t_priv *priv)
+{
+	int rc, i;
+	u8 data[2], min = 0, max = 255, sum = 0;
+
+	usleep_range(5000, 6000);
+
+	for (i = 0; i < 6; i++) {
+		rc = r820t_read(priv, 0x00, data, sizeof(data));
+		if (rc < 0)
+			return rc;
+
+		sum += data[1];
+
+		if (data[1] < min)
+			min = data[1];
+
+		if (data[1] > max)
+			max = data[1];
+	}
+	rc = sum - max - min;
+
+	return rc;
+}
+
+static int r820t_imr_cross(struct r820t_priv *priv,
+			   struct r820t_sect_type iq_point[3],
+			   u8 *x_direct)
+{
+	struct r820t_sect_type cross[5]; /* (0,0)(0,Q-1)(0,I-1)(Q-1,0)(I-1,0) */
+	struct r820t_sect_type tmp;
+	int i, rc;
+	u8 reg08, reg09;
+
+	reg08 = r820t_read_cache_reg(priv, 8) & 0xc0;
+	reg09 = r820t_read_cache_reg(priv, 9) & 0xc0;
+
+	tmp.gain_x = 0;
+	tmp.phase_y = 0;
+	tmp.value = 255;
+
+	for (i = 0; i < 5; i++) {
+		switch (i) {
+		case 0:
+			cross[i].gain_x  = reg08;
+			cross[i].phase_y = reg09;
+			break;
+		case 1:
+			cross[i].gain_x  = reg08;		/* 0 */
+			cross[i].phase_y = reg09 + 1;		/* Q-1 */
+			break;
+		case 2:
+			cross[i].gain_x  = reg08;		/* 0 */
+			cross[i].phase_y = (reg09 | 0x20) + 1;	/* I-1 */
+			break;
+		case 3:
+			cross[i].gain_x  = reg08 + 1;		/* Q-1 */
+			cross[i].phase_y = reg09;
+			break;
+		default:
+			cross[i].gain_x  = (reg08 | 0x20) + 1;	/* I-1 */
+			cross[i].phase_y = reg09;
+		}
+
+		rc = r820t_write_reg(priv, 0x08, cross[i].gain_x);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_write_reg(priv, 0x09, cross[i].phase_y);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_multi_read(priv);
+		if (rc < 0)
+			return rc;
+
+		cross[i].value = rc;
+
+		if (cross[i].value < tmp.value)
+			memcpy(&tmp, &cross[i], sizeof(tmp));
+	}
+
+	if ((tmp.phase_y & 0x1f) == 1) {	/* y-direction */
+		*x_direct = 0;
+
+		iq_point[0] = cross[0];
+		iq_point[1] = cross[1];
+		iq_point[2] = cross[2];
+	} else {				/* (0,0) or x-direction */
+		*x_direct = 1;
+
+		iq_point[0] = cross[0];
+		iq_point[1] = cross[3];
+		iq_point[2] = cross[4];
+	}
+	return 0;
+}
+
+static void r820t_compre_cor(struct r820t_sect_type iq[3])
+{
+	int i;
+
+	for (i = 3; i > 0; i--) {
+		if (iq[0].value > iq[i - 1].value)
+			swap(iq[0], iq[i - 1]);
+	}
+}
+
+static int r820t_compre_step(struct r820t_priv *priv,
+			     struct r820t_sect_type iq[3], u8 reg)
+{
+	int rc;
+	struct r820t_sect_type tmp;
+
+	/*
+	 * Purpose: if (Gain<9 or Phase<9), Gain+1 or Phase+1 and compare
+	 * with min value:
+	 *  new < min => update to min and continue
+	 *  new > min => Exit
+	 */
+
+	/* min value already saved in iq[0] */
+	tmp.phase_y = iq[0].phase_y;
+	tmp.gain_x  = iq[0].gain_x;
+
+	while (((tmp.gain_x & 0x1f) < IMR_TRIAL) &&
+	      ((tmp.phase_y & 0x1f) < IMR_TRIAL)) {
+		if (reg == 0x08)
+			tmp.gain_x++;
+		else
+			tmp.phase_y++;
+
+		rc = r820t_write_reg(priv, 0x08, tmp.gain_x);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_write_reg(priv, 0x09, tmp.phase_y);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_multi_read(priv);
+		if (rc < 0)
+			return rc;
+		tmp.value = rc;
+
+		if (tmp.value <= iq[0].value) {
+			iq[0].gain_x  = tmp.gain_x;
+			iq[0].phase_y = tmp.phase_y;
+			iq[0].value   = tmp.value;
+		} else {
+			return 0;
+		}
+
+	}
+
+	return 0;
+}
+
+static int r820t_iq_tree(struct r820t_priv *priv,
+			 struct r820t_sect_type iq[3],
+			 u8 fix_val, u8 var_val, u8 fix_reg)
+{
+	int rc, i;
+	u8 tmp, var_reg;
+
+	/*
+	 * record IMC results by input gain/phase location then adjust
+	 * gain or phase positive 1 step and negtive 1 step,
+	 * both record results
+	 */
+
+	if (fix_reg == 0x08)
+		var_reg = 0x09;
+	else
+		var_reg = 0x08;
+
+	for (i = 0; i < 3; i++) {
+		rc = r820t_write_reg(priv, fix_reg, fix_val);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_write_reg(priv, var_reg, var_val);
+		if (rc < 0)
+			return rc;
+
+		rc = r820t_multi_read(priv);
+		if (rc < 0)
+			return rc;
+		iq[i].value = rc;
+
+		if (fix_reg == 0x08) {
+			iq[i].gain_x  = fix_val;
+			iq[i].phase_y = var_val;
+		} else {
+			iq[i].phase_y = fix_val;
+			iq[i].gain_x  = var_val;
+		}
+
+		if (i == 0) {  /* try right-side point */
+			var_val++;
+		} else if (i == 1) { /* try left-side point */
+			 /* if absolute location is 1, change I/Q direction */
+			if ((var_val & 0x1f) < 0x02) {
+				tmp = 2 - (var_val & 0x1f);
+
+				/* b[5]:I/Q selection. 0:Q-path, 1:I-path */
+				if (var_val & 0x20) {
+					var_val &= 0xc0;
+					var_val |= tmp;
+				} else {
+					var_val |= 0x20 | tmp;
+				}
+			} else {
+				var_val -= 2;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int r820t_section(struct r820t_priv *priv,
+			 struct r820t_sect_type *iq_point)
+{
+	int rc;
+	struct r820t_sect_type compare_iq[3], compare_bet[3];
+
+	/* Try X-1 column and save min result to compare_bet[0] */
+	if (!(iq_point->gain_x & 0x1f))
+		compare_iq[0].gain_x = ((iq_point->gain_x) & 0xdf) + 1;  /* Q-path, Gain=1 */
+	else
+		compare_iq[0].gain_x  = iq_point->gain_x - 1;  /* left point */
+	compare_iq[0].phase_y = iq_point->phase_y;
+
+	/* y-direction */
+	rc = r820t_iq_tree(priv, compare_iq,  compare_iq[0].gain_x,
+			compare_iq[0].phase_y, 0x08);
+	if (rc < 0)
+		return rc;
+
+	r820t_compre_cor(compare_iq);
+
+	compare_bet[0] = compare_iq[0];
+
+	/* Try X column and save min result to compare_bet[1] */
+	compare_iq[0].gain_x  = iq_point->gain_x;
+	compare_iq[0].phase_y = iq_point->phase_y;
+
+	rc = r820t_iq_tree(priv, compare_iq,  compare_iq[0].gain_x,
+			   compare_iq[0].phase_y, 0x08);
+	if (rc < 0)
+		return rc;
+
+	r820t_compre_cor(compare_iq);
+
+	compare_bet[1] = compare_iq[0];
+
+	/* Try X+1 column and save min result to compare_bet[2] */
+	if ((iq_point->gain_x & 0x1f) == 0x00)
+		compare_iq[0].gain_x = ((iq_point->gain_x) | 0x20) + 1;  /* I-path, Gain=1 */
+	else
+		compare_iq[0].gain_x = iq_point->gain_x + 1;
+	compare_iq[0].phase_y = iq_point->phase_y;
+
+	rc = r820t_iq_tree(priv, compare_iq,  compare_iq[0].gain_x,
+			   compare_iq[0].phase_y, 0x08);
+	if (rc < 0)
+		return rc;
+
+	r820t_compre_cor(compare_iq);
+
+	compare_bet[2] = compare_iq[0];
+
+	r820t_compre_cor(compare_bet);
+
+	*iq_point = compare_bet[0];
+
+	return 0;
+}
+
+static int r820t_vga_adjust(struct r820t_priv *priv)
+{
+	int rc;
+	u8 vga_count;
+
+	/* increase vga power to let image significant */
+	for (vga_count = 12; vga_count < 16; vga_count++) {
+		rc = r820t_write_reg_mask(priv, 0x0c, vga_count, 0x0f);
+		if (rc < 0)
+			return rc;
+
+		usleep_range(10000, 11000);
+
+		rc = r820t_multi_read(priv);
+		if (rc < 0)
+			return rc;
+
+		if (rc > 40 * 4)
+			break;
+	}
+
+	return 0;
+}
+
+static int r820t_iq(struct r820t_priv *priv, struct r820t_sect_type *iq_pont)
+{
+	struct r820t_sect_type compare_iq[3];
+	int rc;
+	u8 x_direction = 0;  /* 1:x, 0:y */
+	u8 dir_reg, other_reg;
+
+	r820t_vga_adjust(priv);
+
+	rc = r820t_imr_cross(priv, compare_iq, &x_direction);
+	if (rc < 0)
+		return rc;
+
+	if (x_direction == 1) {
+		dir_reg   = 0x08;
+		other_reg = 0x09;
+	} else {
+		dir_reg   = 0x09;
+		other_reg = 0x08;
+	}
+
+	/* compare and find min of 3 points. determine i/q direction */
+	r820t_compre_cor(compare_iq);
+
+	/* increase step to find min value of this direction */
+	rc = r820t_compre_step(priv, compare_iq, dir_reg);
+	if (rc < 0)
+		return rc;
+
+	/* the other direction */
+	rc = r820t_iq_tree(priv, compare_iq,  compare_iq[0].gain_x,
+				compare_iq[0].phase_y, dir_reg);
+	if (rc < 0)
+		return rc;
+
+	/* compare and find min of 3 points. determine i/q direction */
+	r820t_compre_cor(compare_iq);
+
+	/* increase step to find min value on this direction */
+	rc = r820t_compre_step(priv, compare_iq, other_reg);
+	if (rc < 0)
+		return rc;
+
+	/* check 3 points again */
+	rc = r820t_iq_tree(priv, compare_iq,  compare_iq[0].gain_x,
+				compare_iq[0].phase_y, other_reg);
+	if (rc < 0)
+		return rc;
+
+	r820t_compre_cor(compare_iq);
+
+	/* section-9 check */
+	rc = r820t_section(priv, compare_iq);
+
+	*iq_pont = compare_iq[0];
+
+	/* reset gain/phase control setting */
+	rc = r820t_write_reg_mask(priv, 0x08, 0, 0x3f);
+	if (rc < 0)
+		return rc;
+
+	rc = r820t_write_reg_mask(priv, 0x09, 0, 0x3f);
+
+	return rc;
+}
+
+static int r820t_f_imr(struct r820t_priv *priv, struct r820t_sect_type *iq_pont)
+{
+	int rc;
+
+	r820t_vga_adjust(priv);
+
+	/*
+	 * search surrounding points from previous point
+	 * try (x-1), (x), (x+1) columns, and find min IMR result point
+	 */
+	rc = r820t_section(priv, iq_pont);
+	if (rc < 0)
+		return rc;
+
+	return 0;
+}
+
+static int r820t_imr(struct r820t_priv *priv, unsigned imr_mem, bool im_flag)
+{
+	struct r820t_sect_type imr_point;
+	int rc;
+	u32 ring_vco, ring_freq, ring_ref;
+	u8 n_ring, n;
+	int reg18, reg19, reg1f;
+
+	if (priv->cfg->xtal > 24000000)
+		ring_ref = priv->cfg->xtal / 2;
+	else
+		ring_ref = priv->cfg->xtal;
+
+	n_ring = 15;
+	for (n = 0; n < 16; n++) {
+		if ((16 + n) * 8 * ring_ref >= 3100000) {
+			n_ring = n;
+			break;
+		}
+	}
+
+	reg18 = r820t_read_cache_reg(priv, 0x18);
+	reg19 = r820t_read_cache_reg(priv, 0x19);
+	reg1f = r820t_read_cache_reg(priv, 0x1f);
+
+	reg18 &= 0xf0;      /* set ring[3:0] */
+	reg18 |= n_ring;
+
+	ring_vco = (16 + n_ring) * 8 * ring_ref;
+
+	reg18 &= 0xdf;   /* clear ring_se23 */
+	reg19 &= 0xfc;   /* clear ring_seldiv */
+	reg1f &= 0xfc;   /* clear ring_att */
+
+	switch (imr_mem) {
+	case 0:
+		ring_freq = ring_vco / 48;
+		reg18 |= 0x20;  /* ring_se23 = 1 */
+		reg19 |= 0x03;  /* ring_seldiv = 3 */
+		reg1f |= 0x02;  /* ring_att 10 */
+		break;
+	case 1:
+		ring_freq = ring_vco / 16;
+		reg18 |= 0x00;  /* ring_se23 = 0 */
+		reg19 |= 0x02;  /* ring_seldiv = 2 */
+		reg1f |= 0x00;  /* pw_ring 00 */
+		break;
+	case 2:
+		ring_freq = ring_vco / 8;
+		reg18 |= 0x00;  /* ring_se23 = 0 */
+		reg19 |= 0x01;  /* ring_seldiv = 1 */
+		reg1f |= 0x03;  /* pw_ring 11 */
+		break;
+	case 3:
+		ring_freq = ring_vco / 6;
+		reg18 |= 0x20;  /* ring_se23 = 1 */
+		reg19 |= 0x00;  /* ring_seldiv = 0 */
+		reg1f |= 0x03;  /* pw_ring 11 */
+		break;
+	case 4:
+		ring_freq = ring_vco / 4;
+		reg18 |= 0x00;  /* ring_se23 = 0 */
+		reg19 |= 0x00;  /* ring_seldiv = 0 */
+		reg1f |= 0x01;  /* pw_ring 01 */
+		break;
+	default:
+		ring_freq = ring_vco / 4;
+		reg18 |= 0x00;  /* ring_se23 = 0 */
+		reg19 |= 0x00;  /* ring_seldiv = 0 */
+		reg1f |= 0x01;  /* pw_ring 01 */
+		break;
+	}
+
+
+	/* write pw_ring, n_ring, ringdiv2 registers */
+
+	/* n_ring, ring_se23 */
+	rc = r820t_write_reg(priv, 0x18, reg18);
+	if (rc < 0)
+		return rc;
+
+	/* ring_sediv */
+	rc = r820t_write_reg(priv, 0x19, reg19);
+	if (rc < 0)
+		return rc;
+
+	/* pw_ring */
+	rc = r820t_write_reg(priv, 0x1f, reg1f);
+	if (rc < 0)
+		return rc;
+
+	/* mux input freq ~ rf_in freq */
+	rc = r820t_set_mux(priv, (ring_freq - 5300) * 1000);
+	if (rc < 0)
+		return rc;
+
+	rc = r820t_set_pll(priv, V4L2_TUNER_DIGITAL_TV,
+			   (ring_freq - 5300) * 1000);
+	if (!priv->has_lock)
+		rc = -EINVAL;
+	if (rc < 0)
+		return rc;
+
+	if (im_flag) {
+		rc = r820t_iq(priv, &imr_point);
+	} else {
+		imr_point.gain_x  = priv->imr_data[3].gain_x;
+		imr_point.phase_y = priv->imr_data[3].phase_y;
+		imr_point.value   = priv->imr_data[3].value;
+
+		rc = r820t_f_imr(priv, &imr_point);
+	}
+	if (rc < 0)
+		return rc;
+
+	/* save IMR value */
+	switch (imr_mem) {
+	case 0:
+		priv->imr_data[0].gain_x  = imr_point.gain_x;
+		priv->imr_data[0].phase_y = imr_point.phase_y;
+		priv->imr_data[0].value   = imr_point.value;
+		break;
+	case 1:
+		priv->imr_data[1].gain_x  = imr_point.gain_x;
+		priv->imr_data[1].phase_y = imr_point.phase_y;
+		priv->imr_data[1].value   = imr_point.value;
+		break;
+	case 2:
+		priv->imr_data[2].gain_x  = imr_point.gain_x;
+		priv->imr_data[2].phase_y = imr_point.phase_y;
+		priv->imr_data[2].value   = imr_point.value;
+		break;
+	case 3:
+		priv->imr_data[3].gain_x  = imr_point.gain_x;
+		priv->imr_data[3].phase_y = imr_point.phase_y;
+		priv->imr_data[3].value   = imr_point.value;
+		break;
+	case 4:
+		priv->imr_data[4].gain_x  = imr_point.gain_x;
+		priv->imr_data[4].phase_y = imr_point.phase_y;
+		priv->imr_data[4].value   = imr_point.value;
+		break;
+	default:
+		priv->imr_data[4].gain_x  = imr_point.gain_x;
+		priv->imr_data[4].phase_y = imr_point.phase_y;
+		priv->imr_data[4].value   = imr_point.value;
+		break;
+	}
+
+	return 0;
+}
+
+static int r820t_imr_callibrate(struct r820t_priv *priv)
+{
+	int rc, i;
+	int xtal_cap = 0;
+
+	if (priv->init_done)
+		return 0;
+
+	/* Detect Xtal capacitance */
+	if ((priv->cfg->rafael_chip == CHIP_R820T) ||
+	    (priv->cfg->rafael_chip == CHIP_R828S) ||
+	    (priv->cfg->rafael_chip == CHIP_R820C)) {
+		priv->xtal_cap_sel = XTAL_HIGH_CAP_0P;
+	} else {
+		/* Initialize registers */
+		rc = r820t_write(priv, 0x05,
+				r820t_init_array, sizeof(r820t_init_array));
+		if (rc < 0)
+			return rc;
+		for (i = 0; i < 3; i++) {
+			rc = r820t_xtal_check(priv);
+			if (rc < 0)
+				return rc;
+			if (!i || rc > xtal_cap)
+				xtal_cap = rc;
+		}
+		priv->xtal_cap_sel = xtal_cap;
+	}
+
+	/*
+	 * Disables IMR callibration. That emulates the same behaviour
+	 * as what is done by rtl-sdr userspace library. Useful for testing
+	 */
+	if (no_imr_cal) {
+		priv->init_done = true;
+
+		return 0;
+	}
+
+	/* Initialize registers */
+	rc = r820t_write(priv, 0x05,
+			 r820t_init_array, sizeof(r820t_init_array));
+	if (rc < 0)
+		return rc;
+
+	rc = r820t_imr_prepare(priv);
+	if (rc < 0)
+		return rc;
+
+	rc = r820t_imr(priv, 3, true);
+	if (rc < 0)
+		return rc;
+	rc = r820t_imr(priv, 1, false);
+	if (rc < 0)
+		return rc;
+	rc = r820t_imr(priv, 0, false);
+	if (rc < 0)
+		return rc;
+	rc = r820t_imr(priv, 2, false);
+	if (rc < 0)
+		return rc;
+	rc = r820t_imr(priv, 4, false);
+	if (rc < 0)
+		return rc;
+
+	priv->init_done = true;
+	priv->imr_done = true;
+
+	return 0;
+}
+
+#if 0
+/* Not used, for now */
+static int r820t_gpio(struct r820t_priv *priv, bool enable)
+{
+	return r820t_write_reg_mask(priv, 0x0f, enable ? 1 : 0, 0x01);
+}
+#endif
+
+/*
+ *  r820t frontend operations and tuner attach code
+ *
+ * All driver locks and i2c control are only in this part of the code
+ */
+
+static int r820t_init(struct dvb_frontend *fe)
+{
+	struct r820t_priv *priv = fe->tuner_priv;
+	int rc;
+
+	tuner_dbg("%s:\n", __func__);
+
+	mutex_lock(&priv->lock);
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+
+	rc = r820t_imr_callibrate(priv);
+	if (rc < 0)
+		goto err;
+
+	/* Initialize registers */
+	rc = r820t_write(priv, 0x05,
+			 r820t_init_array, sizeof(r820t_init_array));
+
+err:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0);
+	mutex_unlock(&priv->lock);
+
+	if (rc < 0)
+		tuner_dbg("%s: failed=%d\n", __func__, rc);
+	return rc;
+}
+
+static int r820t_sleep(struct dvb_frontend *fe)
+{
+	struct r820t_priv *priv = fe->tuner_priv;
+	int rc;
+
+	tuner_dbg("%s:\n", __func__);
+
+	mutex_lock(&priv->lock);
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+
+	rc = r820t_standby(priv);
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0);
+	mutex_unlock(&priv->lock);
+
+	tuner_dbg("%s: failed=%d\n", __func__, rc);
+	return rc;
+}
+
+static int r820t_set_analog_freq(struct dvb_frontend *fe,
+				 struct analog_parameters *p)
+{
+	struct r820t_priv *priv = fe->tuner_priv;
+	unsigned bw;
+	int rc;
+
+	tuner_dbg("%s called\n", __func__);
+
+	/* if std is not defined, choose one */
+	if (!p->std)
+		p->std = V4L2_STD_MN;
+
+	if ((p->std == V4L2_STD_PAL_M) || (p->std == V4L2_STD_NTSC))
+		bw = 6;
+	else
+		bw = 8;
+
+	mutex_lock(&priv->lock);
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+
+	rc = generic_set_freq(fe, 62500l * p->frequency, bw,
+			      V4L2_TUNER_ANALOG_TV, p->std, SYS_UNDEFINED);
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0);
+	mutex_unlock(&priv->lock);
+
+	return rc;
+}
+
+static int r820t_set_params(struct dvb_frontend *fe)
+{
+	struct r820t_priv *priv = fe->tuner_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int rc;
+	unsigned bw;
+
+	tuner_dbg("%s: delivery_system=%d frequency=%d bandwidth_hz=%d\n",
+		__func__, c->delivery_system, c->frequency, c->bandwidth_hz);
+
+	mutex_lock(&priv->lock);
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+
+	bw = (c->bandwidth_hz + 500000) / 1000000;
+	if (!bw)
+		bw = 8;
+
+	rc = generic_set_freq(fe, c->frequency, bw,
+			      V4L2_TUNER_DIGITAL_TV, 0, c->delivery_system);
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0);
+	mutex_unlock(&priv->lock);
+
+	if (rc)
+		tuner_dbg("%s: failed=%d\n", __func__, rc);
+	return rc;
+}
+
+static int r820t_signal(struct dvb_frontend *fe, u16 *strength)
+{
+	struct r820t_priv *priv = fe->tuner_priv;
+	int rc = 0;
+
+	mutex_lock(&priv->lock);
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+
+	if (priv->has_lock) {
+		rc = r820t_read_gain(priv);
+		if (rc < 0)
+			goto err;
+
+		/* A higher gain at LNA means a lower signal strength */
+		*strength = (45 - rc) << 4 | 0xff;
+		if (*strength == 0xff)
+			*strength = 0;
+	} else {
+		*strength = 0;
+	}
+
+err:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0);
+	mutex_unlock(&priv->lock);
+
+	tuner_dbg("%s: %s, gain=%d strength=%d\n",
+		  __func__,
+		  priv->has_lock ? "PLL locked" : "no signal",
+		  rc, *strength);
+
+	return 0;
+}
+
+static int r820t_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	struct r820t_priv *priv = fe->tuner_priv;
+
+	tuner_dbg("%s:\n", __func__);
+
+	*frequency = priv->int_freq;
+
+	return 0;
+}
+
+static int r820t_release(struct dvb_frontend *fe)
+{
+	struct r820t_priv *priv = fe->tuner_priv;
+
+	tuner_dbg("%s:\n", __func__);
+
+	mutex_lock(&r820t_list_mutex);
+
+	if (priv)
+		hybrid_tuner_release_state(priv);
+
+	mutex_unlock(&r820t_list_mutex);
+
+	kfree(fe->tuner_priv);
+	fe->tuner_priv = NULL;
+
+	return 0;
+}
+
+static const struct dvb_tuner_ops r820t_tuner_ops = {
+	.info = {
+		.name           = "Rafael Micro R820T",
+		.frequency_min  =   42000000,
+		.frequency_max  = 1002000000,
+	},
+	.init = r820t_init,
+	.release = r820t_release,
+	.sleep = r820t_sleep,
+	.set_params = r820t_set_params,
+	.set_analog_params = r820t_set_analog_freq,
+	.get_if_frequency = r820t_get_if_frequency,
+	.get_rf_strength = r820t_signal,
+};
+
+struct dvb_frontend *r820t_attach(struct dvb_frontend *fe,
+				  struct i2c_adapter *i2c,
+				  const struct r820t_config *cfg)
+{
+	struct r820t_priv *priv;
+	int rc = -ENODEV;
+	u8 data[5];
+	int instance;
+
+	mutex_lock(&r820t_list_mutex);
+
+	instance = hybrid_tuner_request_state(struct r820t_priv, priv,
+					      hybrid_tuner_instance_list,
+					      i2c, cfg->i2c_addr,
+					      "r820t");
+	switch (instance) {
+	case 0:
+		/* memory allocation failure */
+		goto err_no_gate;
+		break;
+	case 1:
+		/* new tuner instance */
+		priv->cfg = cfg;
+
+		mutex_init(&priv->lock);
+
+		fe->tuner_priv = priv;
+		break;
+	case 2:
+		/* existing tuner instance */
+		fe->tuner_priv = priv;
+		break;
+	}
+
+	memcpy(&fe->ops.tuner_ops, &r820t_tuner_ops, sizeof(r820t_tuner_ops));
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+
+	/* check if the tuner is there */
+	rc = r820t_read(priv, 0x00, data, sizeof(data));
+	if (rc < 0)
+		goto err;
+
+	rc = r820t_sleep(fe);
+	if (rc < 0)
+		goto err;
+
+	tuner_info("Rafael Micro r820t successfully identified\n");
+
+	fe->tuner_priv = priv;
+	memcpy(&fe->ops.tuner_ops, &r820t_tuner_ops,
+			sizeof(struct dvb_tuner_ops));
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0);
+
+	mutex_unlock(&r820t_list_mutex);
+
+	return fe;
+err:
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0);
+
+err_no_gate:
+	mutex_unlock(&r820t_list_mutex);
+
+	tuner_info("%s: failed=%d\n", __func__, rc);
+	r820t_release(fe);
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(r820t_attach);
+
+MODULE_DESCRIPTION("Rafael Micro r820t silicon tuner driver");
+MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/tuners/r820t.h b/drivers/media/tuners/r820t.h
new file mode 100644
index 000000000000..48af3548027d
--- /dev/null
+++ b/drivers/media/tuners/r820t.h
@@ -0,0 +1,59 @@
+/*
+ * Elonics R820T silicon tuner driver
+ *
+ * Copyright (C) 2012 Antti Palosaari <crope@iki.fi>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License along
+ *    with this program; if not, write to the Free Software Foundation, Inc.,
+ *    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#ifndef R820T_H
+#define R820T_H
+
+#include <linux/kconfig.h>
+#include "dvb_frontend.h"
+
+enum r820t_chip {
+	CHIP_R820T,
+	CHIP_R620D,
+	CHIP_R828D,
+	CHIP_R828,
+	CHIP_R828S,
+	CHIP_R820C,
+};
+
+struct r820t_config {
+	u8 i2c_addr;		/* 0x34 */
+	u32 xtal;
+	enum r820t_chip rafael_chip;
+	unsigned max_i2c_msg_len;
+	bool use_diplexer;
+	bool use_predetect;
+};
+
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_R820T)
+struct dvb_frontend *r820t_attach(struct dvb_frontend *fe,
+				  struct i2c_adapter *i2c,
+				  const struct r820t_config *cfg);
+#else
+static inline struct dvb_frontend *r820t_attach(struct dvb_frontend *fe,
+						struct i2c_adapter *i2c,
+						const struct r820t_config *cfg)
+{
+	pr_warn("%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif
+
+#endif
diff --git a/drivers/media/tuners/tda18212.h b/drivers/media/tuners/tda18212.h
index 9bd5da4aabb7..7e0d503baf05 100644
--- a/drivers/media/tuners/tda18212.h
+++ b/drivers/media/tuners/tda18212.h
@@ -21,6 +21,7 @@
 #ifndef TDA18212_H
 #define TDA18212_H
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 struct tda18212_config {
@@ -36,8 +37,7 @@ struct tda18212_config {
 	u16 if_dvbc;
 };
 
-#if defined(CONFIG_MEDIA_TUNER_TDA18212) || \
-	(defined(CONFIG_MEDIA_TUNER_TDA18212_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_TDA18212)
 extern struct dvb_frontend *tda18212_attach(struct dvb_frontend *fe,
 	struct i2c_adapter *i2c, struct tda18212_config *cfg);
 #else
diff --git a/drivers/media/tuners/tda18218.h b/drivers/media/tuners/tda18218.h
index b4180d180029..366410e0cc9a 100644
--- a/drivers/media/tuners/tda18218.h
+++ b/drivers/media/tuners/tda18218.h
@@ -21,6 +21,7 @@
 #ifndef TDA18218_H
 #define TDA18218_H
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 struct tda18218_config {
@@ -29,8 +30,7 @@ struct tda18218_config {
 	u8 loop_through:1;
 };
 
-#if defined(CONFIG_MEDIA_TUNER_TDA18218) || \
-	(defined(CONFIG_MEDIA_TUNER_TDA18218_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_TDA18218)
 extern struct dvb_frontend *tda18218_attach(struct dvb_frontend *fe,
 	struct i2c_adapter *i2c, struct tda18218_config *cfg);
 #else
diff --git a/drivers/media/tuners/tda18271-fe.c b/drivers/media/tuners/tda18271-fe.c
index e7786862dab2..4995b890c164 100644
--- a/drivers/media/tuners/tda18271-fe.c
+++ b/drivers/media/tuners/tda18271-fe.c
@@ -21,6 +21,7 @@
 #include <linux/delay.h>
 #include <linux/videodev2.h>
 #include "tda18271-priv.h"
+#include "tda8290.h"
 
 int tda18271_debug;
 module_param_named(debug, tda18271_debug, int, 0644);
@@ -867,12 +868,12 @@ static int tda18271_agc(struct dvb_frontend *fe)
 	int ret = 0;
 
 	switch (priv->config) {
-	case 0:
+	case TDA8290_LNA_OFF:
 		/* no external agc configuration required */
 		if (tda18271_debug & DBG_ADV)
 			tda_dbg("no agc configuration provided\n");
 		break;
-	case 3:
+	case TDA8290_LNA_ON_BRIDGE:
 		/* switch with GPIO of saa713x */
 		tda_dbg("invoking callback\n");
 		if (fe->callback)
@@ -881,8 +882,8 @@ static int tda18271_agc(struct dvb_frontend *fe)
 					   TDA18271_CALLBACK_CMD_AGC_ENABLE,
 					   priv->mode);
 		break;
-	case 1:
-	case 2:
+	case TDA8290_LNA_GP0_HIGH_ON:
+	case TDA8290_LNA_GP0_HIGH_OFF:
 	default:
 		/* n/a - currently not supported */
 		tda_err("unsupported configuration: %d\n", priv->config);
diff --git a/drivers/media/tuners/tda827x.c b/drivers/media/tuners/tda827x.c
index a0d176267470..73453a255cdc 100644
--- a/drivers/media/tuners/tda827x.c
+++ b/drivers/media/tuners/tda827x.c
@@ -479,10 +479,10 @@ static void tda827xa_lna_gain(struct dvb_frontend *fe, int high,
 			dprintk("setting LNA to low gain\n");
 	}
 	switch (priv->cfg->config) {
-	case 0: /* no LNA */
+	case TDA8290_LNA_OFF: /* no LNA */
 		break;
-	case 1: /* switch is GPIO 0 of tda8290 */
-	case 2:
+	case TDA8290_LNA_GP0_HIGH_ON: /* switch is GPIO 0 of tda8290 */
+	case TDA8290_LNA_GP0_HIGH_OFF:
 		if (params == NULL) {
 			gp_func = 0;
 			arg  = 0;
@@ -499,11 +499,11 @@ static void tda827xa_lna_gain(struct dvb_frontend *fe, int high,
 				     DVB_FRONTEND_COMPONENT_TUNER,
 				     gp_func, arg);
 		buf[1] = high ? 0 : 1;
-		if (priv->cfg->config == 2)
+		if (priv->cfg->config == TDA8290_LNA_GP0_HIGH_OFF)
 			buf[1] = high ? 1 : 0;
 		tuner_transfer(fe, &msg, 1);
 		break;
-	case 3: /* switch with GPIO of saa713x */
+	case TDA8290_LNA_ON_BRIDGE: /* switch with GPIO of saa713x */
 		if (fe->callback)
 			fe->callback(priv->i2c_adap->algo_data,
 				     DVB_FRONTEND_COMPONENT_TUNER, 0, high);
diff --git a/drivers/media/tuners/tda827x.h b/drivers/media/tuners/tda827x.h
index 9432b5b6121b..b64292152baf 100644
--- a/drivers/media/tuners/tda827x.h
+++ b/drivers/media/tuners/tda827x.h
@@ -26,6 +26,7 @@
 
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
+#include "tda8290.h"
 
 struct tda827x_config
 {
@@ -34,7 +35,7 @@ struct tda827x_config
 	int (*sleep) (struct dvb_frontend *fe);
 
 	/* interface to tda829x driver */
-	unsigned int config;
+	enum tda8290_lna config;
 	int 	     switch_addr;
 
 	void (*agcf)(struct dvb_frontend *fe);
diff --git a/drivers/media/tuners/tda8290.c b/drivers/media/tuners/tda8290.c
index 8c4852114eeb..ab4106c17b4c 100644
--- a/drivers/media/tuners/tda8290.c
+++ b/drivers/media/tuners/tda8290.c
@@ -54,6 +54,7 @@ struct tda8290_priv {
 #define TDA18271 16
 
 	struct tda827x_config cfg;
+	struct tda18271_std_map *tda18271_std_map;
 };
 
 /*---------------------------------------------------------------------*/
@@ -233,7 +234,8 @@ static void tda8290_set_params(struct dvb_frontend *fe,
 	}
 
 
-	tda8290_i2c_bridge(fe, 1);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 1);
 
 	if (fe->ops.tuner_ops.set_analog_params)
 		fe->ops.tuner_ops.set_analog_params(fe, params);
@@ -302,7 +304,8 @@ static void tda8290_set_params(struct dvb_frontend *fe,
 		}
 	}
 
-	tda8290_i2c_bridge(fe, 0);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 0);
 	tuner_i2c_xfer_send(&priv->i2c_props, if_agc_set, 2);
 }
 
@@ -388,7 +391,7 @@ static void tda8295_agc2_out(struct dvb_frontend *fe, int enable)
 	tuner_i2c_xfer_send(&priv->i2c_props, set_gpio_val, 2);
 }
 
-static int tda8295_has_signal(struct dvb_frontend *fe)
+static int tda8295_has_signal(struct dvb_frontend *fe, u16 *signal)
 {
 	struct tda8290_priv *priv = fe->analog_demod_priv;
 
@@ -396,7 +399,8 @@ static int tda8295_has_signal(struct dvb_frontend *fe)
 	unsigned char ret;
 
 	tuner_i2c_xfer_send_recv(&priv->i2c_props, &hvpll_stat, 1, &ret, 1);
-	return (ret & 0x01) ? 65535 : 0;
+	*signal = (ret & 0x01) ? 65535 : 0;
+	return 0;
 }
 
 /*---------------------------------------------------------------------*/
@@ -405,7 +409,7 @@ static void tda8295_set_params(struct dvb_frontend *fe,
 			       struct analog_parameters *params)
 {
 	struct tda8290_priv *priv = fe->analog_demod_priv;
-
+	u16 signal = 0;
 	unsigned char blanking_mode[]     = { 0x1d, 0x00 };
 
 	set_audio(fe, params);
@@ -424,7 +428,8 @@ static void tda8295_set_params(struct dvb_frontend *fe,
 	tuner_i2c_xfer_send(&priv->i2c_props, blanking_mode, 2);
 	msleep(20);
 
-	tda8295_i2c_bridge(fe, 1);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 1);
 
 	if (fe->ops.tuner_ops.set_analog_params)
 		fe->ops.tuner_ops.set_analog_params(fe, params);
@@ -432,17 +437,19 @@ static void tda8295_set_params(struct dvb_frontend *fe,
 	if (priv->cfg.agcf)
 		priv->cfg.agcf(fe);
 
-	if (tda8295_has_signal(fe))
+	tda8295_has_signal(fe, &signal);
+	if (signal)
 		tuner_dbg("tda8295 is locked\n");
 	else
 		tuner_dbg("tda8295 not locked, no signal?\n");
 
-	tda8295_i2c_bridge(fe, 0);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 0);
 }
 
 /*---------------------------------------------------------------------*/
 
-static int tda8290_has_signal(struct dvb_frontend *fe)
+static int tda8290_has_signal(struct dvb_frontend *fe, u16 *signal)
 {
 	struct tda8290_priv *priv = fe->analog_demod_priv;
 
@@ -451,7 +458,8 @@ static int tda8290_has_signal(struct dvb_frontend *fe)
 
 	tuner_i2c_xfer_send_recv(&priv->i2c_props,
 				 i2c_get_afc, ARRAY_SIZE(i2c_get_afc), &afc, 1);
-	return (afc & 0x80)? 65535:0;
+	*signal = (afc & 0x80) ? 65535 : 0;
+	return 0;
 }
 
 /*---------------------------------------------------------------------*/
@@ -465,11 +473,13 @@ static void tda8290_standby(struct dvb_frontend *fe)
 	unsigned char tda8290_agc_tri[] = { 0x02, 0x20 };
 	struct i2c_msg msg = {.addr = priv->tda827x_addr, .flags=0, .buf=cb1, .len = 2};
 
-	tda8290_i2c_bridge(fe, 1);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 1);
 	if (priv->ver & TDA8275A)
 		cb1[1] = 0x90;
 	i2c_transfer(priv->i2c_props.adap, &msg, 1);
-	tda8290_i2c_bridge(fe, 0);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 0);
 	tuner_i2c_xfer_send(&priv->i2c_props, tda8290_agc_tri, 2);
 	tuner_i2c_xfer_send(&priv->i2c_props, tda8290_standby, 2);
 }
@@ -489,7 +499,8 @@ static void tda8290_init_if(struct dvb_frontend *fe)
 	unsigned char set_GP00_CF[] = { 0x20, 0x01 };
 	unsigned char set_GP01_CF[] = { 0x20, 0x0B };
 
-	if ((priv->cfg.config == 1) || (priv->cfg.config == 2))
+	if ((priv->cfg.config == TDA8290_LNA_GP0_HIGH_ON) ||
+	    (priv->cfg.config == TDA8290_LNA_GP0_HIGH_OFF))
 		tuner_i2c_xfer_send(&priv->i2c_props, set_GP00_CF, 2);
 	else
 		tuner_i2c_xfer_send(&priv->i2c_props, set_GP01_CF, 2);
@@ -537,9 +548,11 @@ static void tda8290_init_tuner(struct dvb_frontend *fe)
 	if (priv->ver & TDA8275A)
 		msg.buf = tda8275a_init;
 
-	tda8290_i2c_bridge(fe, 1);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 1);
 	i2c_transfer(priv->i2c_props.adap, &msg, 1);
-	tda8290_i2c_bridge(fe, 0);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 0);
 }
 
 /*---------------------------------------------------------------------*/
@@ -565,19 +578,13 @@ static struct tda18271_config tda829x_tda18271_config = {
 static int tda829x_find_tuner(struct dvb_frontend *fe)
 {
 	struct tda8290_priv *priv = fe->analog_demod_priv;
-	struct analog_demod_ops *analog_ops = &fe->ops.analog_ops;
 	int i, ret, tuners_found;
 	u32 tuner_addrs;
 	u8 data;
 	struct i2c_msg msg = { .flags = I2C_M_RD, .buf = &data, .len = 1 };
 
-	if (!analog_ops->i2c_gate_ctrl) {
-		printk(KERN_ERR "tda8290: no gate control were provided!\n");
-
-		return -EINVAL;
-	}
-
-	analog_ops->i2c_gate_ctrl(fe, 1);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 1);
 
 	/* probe for tuner chip */
 	tuners_found = 0;
@@ -595,7 +602,8 @@ static int tda829x_find_tuner(struct dvb_frontend *fe)
 	   give a response now
 	 */
 
-	analog_ops->i2c_gate_ctrl(fe, 0);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 0);
 
 	if (tuners_found > 1)
 		for (i = 0; i < tuners_found; i++) {
@@ -618,18 +626,21 @@ static int tda829x_find_tuner(struct dvb_frontend *fe)
 	priv->tda827x_addr = tuner_addrs;
 	msg.addr = tuner_addrs;
 
-	analog_ops->i2c_gate_ctrl(fe, 1);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 1);
 	ret = i2c_transfer(priv->i2c_props.adap, &msg, 1);
 
 	if (ret != 1) {
 		tuner_warn("tuner access failed!\n");
-		analog_ops->i2c_gate_ctrl(fe, 0);
+		if (fe->ops.analog_ops.i2c_gate_ctrl)
+			fe->ops.analog_ops.i2c_gate_ctrl(fe, 0);
 		return -EREMOTEIO;
 	}
 
 	if ((data == 0x83) || (data == 0x84)) {
 		priv->ver |= TDA18271;
 		tda829x_tda18271_config.config = priv->cfg.config;
+		tda829x_tda18271_config.std_map = priv->tda18271_std_map;
 		dvb_attach(tda18271_attach, fe, priv->tda827x_addr,
 			   priv->i2c_props.adap, &tda829x_tda18271_config);
 	} else {
@@ -648,7 +659,8 @@ static int tda829x_find_tuner(struct dvb_frontend *fe)
 	if (fe->ops.tuner_ops.sleep)
 		fe->ops.tuner_ops.sleep(fe);
 
-	analog_ops->i2c_gate_ctrl(fe, 0);
+	if (fe->ops.analog_ops.i2c_gate_ctrl)
+		fe->ops.analog_ops.i2c_gate_ctrl(fe, 0);
 
 	return 0;
 }
@@ -740,8 +752,10 @@ struct dvb_frontend *tda829x_attach(struct dvb_frontend *fe,
 	priv->i2c_props.addr     = i2c_addr;
 	priv->i2c_props.adap     = i2c_adap;
 	priv->i2c_props.name     = "tda829x";
-	if (cfg)
-		priv->cfg.config         = cfg->lna_cfg;
+	if (cfg) {
+		priv->cfg.config = cfg->lna_cfg;
+		priv->tda18271_std_map = cfg->tda18271_std_map;
+	}
 
 	if (tda8290_probe(&priv->i2c_props) == 0) {
 		priv->ver = TDA8290;
@@ -755,6 +769,9 @@ struct dvb_frontend *tda829x_attach(struct dvb_frontend *fe,
 		       sizeof(struct analog_demod_ops));
 	}
 
+	if (cfg && cfg->no_i2c_gate)
+		fe->ops.analog_ops.i2c_gate_ctrl = NULL;
+
 	if (!(cfg) || (TDA829X_PROBE_TUNER == cfg->probe_tuner)) {
 		tda8295_power(fe, 1);
 		if (tda829x_find_tuner(fe) < 0)
diff --git a/drivers/media/tuners/tda8290.h b/drivers/media/tuners/tda8290.h
index e12ecbaa35a4..cf96e585785e 100644
--- a/drivers/media/tuners/tda8290.h
+++ b/drivers/media/tuners/tda8290.h
@@ -19,13 +19,23 @@
 
 #include <linux/i2c.h>
 #include "dvb_frontend.h"
+#include "tda18271.h"
+
+enum tda8290_lna {
+	TDA8290_LNA_OFF = 0,
+	TDA8290_LNA_GP0_HIGH_ON = 1,
+	TDA8290_LNA_GP0_HIGH_OFF = 2,
+	TDA8290_LNA_ON_BRIDGE = 3,
+};
 
 struct tda829x_config {
-	unsigned int lna_cfg;
+	enum tda8290_lna lna_cfg;
 
 	unsigned int probe_tuner:1;
 #define TDA829X_PROBE_TUNER 0
 #define TDA829X_DONT_PROBE  1
+	unsigned int no_i2c_gate:1;
+	struct tda18271_std_map *tda18271_std_map;
 };
 
 #if IS_ENABLED(CONFIG_MEDIA_TUNER_TDA8290)
diff --git a/drivers/media/tuners/tda9887.c b/drivers/media/tuners/tda9887.c
index cdb645d57438..300005c535ba 100644
--- a/drivers/media/tuners/tda9887.c
+++ b/drivers/media/tuners/tda9887.c
@@ -596,22 +596,22 @@ static void tda9887_tuner_status(struct dvb_frontend *fe)
 		   priv->data[1], priv->data[2], priv->data[3]);
 }
 
-static int tda9887_get_afc(struct dvb_frontend *fe)
+static int tda9887_get_afc(struct dvb_frontend *fe, s32 *afc)
 {
 	struct tda9887_priv *priv = fe->analog_demod_priv;
-	static int AFC_BITS_2_kHz[] = {
+	static const int AFC_BITS_2_kHz[] = {
 		-12500,  -37500,  -62500,  -97500,
 		-112500, -137500, -162500, -187500,
 		187500,  162500,  137500,  112500,
 		97500 ,  62500,   37500 ,  12500
 	};
-	int afc=0;
 	__u8 reg = 0;
 
-	if (1 == tuner_i2c_xfer_recv(&priv->i2c_props,&reg,1))
-		afc = AFC_BITS_2_kHz[(reg>>1)&0x0f];
-
-	return afc;
+	if (priv->mode != V4L2_TUNER_RADIO)
+		return 0;
+	if (1 == tuner_i2c_xfer_recv(&priv->i2c_props, &reg, 1))
+		*afc = AFC_BITS_2_kHz[(reg >> 1) & 0x0f];
+	return 0;
 }
 
 static void tda9887_standby(struct dvb_frontend *fe)
diff --git a/drivers/media/tuners/tua9001.h b/drivers/media/tuners/tua9001.h
index cf5b815feff9..26358da1c100 100644
--- a/drivers/media/tuners/tua9001.h
+++ b/drivers/media/tuners/tua9001.h
@@ -21,6 +21,7 @@
 #ifndef TUA9001_H
 #define TUA9001_H
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 struct tua9001_config {
@@ -50,8 +51,7 @@ struct tua9001_config {
 #define TUA9001_CMD_RESETN  1
 #define TUA9001_CMD_RXEN    2
 
-#if defined(CONFIG_MEDIA_TUNER_TUA9001) || \
-	(defined(CONFIG_MEDIA_TUNER_TUA9001_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_TUA9001)
 extern struct dvb_frontend *tua9001_attach(struct dvb_frontend *fe,
 		struct i2c_adapter *i2c, struct tua9001_config *cfg);
 #else
diff --git a/drivers/media/tuners/tuner-simple.c b/drivers/media/tuners/tuner-simple.c
index 39e7e583c8c0..ca274c2d8c70 100644
--- a/drivers/media/tuners/tuner-simple.c
+++ b/drivers/media/tuners/tuner-simple.c
@@ -115,6 +115,7 @@ struct tuner_simple_priv {
 
 	u32 frequency;
 	u32 bandwidth;
+	bool radio_mode;
 };
 
 /* ---------------------------------------------------------------------- */
@@ -189,7 +190,7 @@ static int simple_get_rf_strength(struct dvb_frontend *fe, u16 *strength)
 	struct tuner_simple_priv *priv = fe->tuner_priv;
 	int signal;
 
-	if (priv->i2c_props.adap == NULL)
+	if (priv->i2c_props.adap == NULL || !priv->radio_mode)
 		return -EINVAL;
 
 	signal = tuner_signal(tuner_read_status(fe));
@@ -776,11 +777,13 @@ static int simple_set_params(struct dvb_frontend *fe,
 
 	switch (params->mode) {
 	case V4L2_TUNER_RADIO:
+		priv->radio_mode = true;
 		ret = simple_set_radio_freq(fe, params);
 		priv->frequency = params->frequency * 125 / 2;
 		break;
 	case V4L2_TUNER_ANALOG_TV:
 	case V4L2_TUNER_DIGITAL_TV:
+		priv->radio_mode = false;
 		ret = simple_set_tv_freq(fe, params);
 		priv->frequency = params->frequency * 62500;
 		break;
diff --git a/drivers/media/tuners/tuner-types.c b/drivers/media/tuners/tuner-types.c
index 2da4440c16ee..98bc15a388be 100644
--- a/drivers/media/tuners/tuner-types.c
+++ b/drivers/media/tuners/tuner-types.c
@@ -1381,6 +1381,58 @@ static struct tuner_params tuner_philips_fq1236_mk5_params[] = {
 	},
 };
 
+/* --------- Sony BTF-PG472Z PAL/SECAM ------- */
+
+static struct tuner_range tuner_sony_btf_pg472z_ranges[] = {
+	{ 16 * 144.25 /*MHz*/, 0xc6, 0x01, },
+	{ 16 * 427.25 /*MHz*/, 0xc6, 0x02, },
+	{ 16 * 999.99        , 0xc6, 0x04, },
+};
+
+static struct tuner_params tuner_sony_btf_pg472z_params[] = {
+	{
+		.type   = TUNER_PARAM_TYPE_PAL,
+		.ranges = tuner_sony_btf_pg472z_ranges,
+		.count  = ARRAY_SIZE(tuner_sony_btf_pg472z_ranges),
+		.has_tda9887 = 1,
+		.port1_active = 1,
+		.port2_invert_for_secam_lc = 1,
+	},
+};
+
+/* 90-99 */
+/* --------- Sony BTF-PG467Z NTSC-M-JP ------- */
+
+static struct tuner_range tuner_sony_btf_pg467z_ranges[] = {
+	{ 16 * 220.25 /*MHz*/, 0xc6, 0x01, },
+	{ 16 * 467.25 /*MHz*/, 0xc6, 0x02, },
+	{ 16 * 999.99        , 0xc6, 0x04, },
+};
+
+static struct tuner_params tuner_sony_btf_pg467z_params[] = {
+	{
+		.type   = TUNER_PARAM_TYPE_NTSC,
+		.ranges = tuner_sony_btf_pg467z_ranges,
+		.count  = ARRAY_SIZE(tuner_sony_btf_pg467z_ranges),
+	},
+};
+
+/* --------- Sony BTF-PG463Z NTSC-M ------- */
+
+static struct tuner_range tuner_sony_btf_pg463z_ranges[] = {
+	{ 16 * 130.25 /*MHz*/, 0xc6, 0x01, },
+	{ 16 * 364.25 /*MHz*/, 0xc6, 0x02, },
+	{ 16 * 999.99        , 0xc6, 0x04, },
+};
+
+static struct tuner_params tuner_sony_btf_pg463z_params[] = {
+	{
+		.type   = TUNER_PARAM_TYPE_NTSC,
+		.ranges = tuner_sony_btf_pg463z_ranges,
+		.count  = ARRAY_SIZE(tuner_sony_btf_pg463z_ranges),
+	},
+};
+
 /* --------------------------------------------------------------------- */
 
 struct tunertype tuners[] = {
@@ -1872,6 +1924,23 @@ struct tunertype tuners[] = {
 		.name   = "Xceive 5000C tuner",
 		/* see xc5000.c for details */
 	},
+	[TUNER_SONY_BTF_PG472Z] = {
+		.name   = "Sony BTF-PG472Z PAL/SECAM",
+		.params = tuner_sony_btf_pg472z_params,
+		.count  = ARRAY_SIZE(tuner_sony_btf_pg472z_params),
+	},
+
+	/* 90-99 */
+	[TUNER_SONY_BTF_PK467Z] = {
+		.name   = "Sony BTF-PK467Z NTSC-M-JP",
+		.params = tuner_sony_btf_pg467z_params,
+		.count  = ARRAY_SIZE(tuner_sony_btf_pg467z_params),
+	},
+	[TUNER_SONY_BTF_PB463Z] = {
+		.name   = "Sony BTF-PB463Z NTSC-M",
+		.params = tuner_sony_btf_pg463z_params,
+		.count  = ARRAY_SIZE(tuner_sony_btf_pg463z_params),
+	},
 };
 EXPORT_SYMBOL(tuners);
 
diff --git a/drivers/media/tuners/tuner-xc2028.c b/drivers/media/tuners/tuner-xc2028.c
index 09451737c77e..878d2c4d9e8e 100644
--- a/drivers/media/tuners/tuner-xc2028.c
+++ b/drivers/media/tuners/tuner-xc2028.c
@@ -1378,8 +1378,7 @@ static int xc2028_set_config(struct dvb_frontend *fe, void *priv_cfg)
 	 * For the firmware name, keep a local copy of the string,
 	 * in order to avoid troubles during device release.
 	 */
-	if (priv->ctrl.fname)
-		kfree(priv->ctrl.fname);
+	kfree(priv->ctrl.fname);
 	memcpy(&priv->ctrl, p, sizeof(priv->ctrl));
 	if (p->fname) {
 		priv->ctrl.fname = kstrdup(p->fname, GFP_KERNEL);
diff --git a/drivers/media/tuners/tuner_it913x.c b/drivers/media/tuners/tuner_it913x.c
new file mode 100644
index 000000000000..6f30d7e535b8
--- /dev/null
+++ b/drivers/media/tuners/tuner_it913x.c
@@ -0,0 +1,447 @@
+/*
+ * ITE Tech IT9137 silicon tuner driver
+ *
+ *  Copyright (C) 2011 Malcolm Priestley (tvboxspy@gmail.com)
+ *  IT9137 Copyright (C) ITE Tech Inc.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.=
+ */
+
+#include "tuner_it913x_priv.h"
+
+struct it913x_state {
+	struct i2c_adapter *i2c_adap;
+	u8 i2c_addr;
+	u8 chip_ver;
+	u8 tuner_type;
+	u8 firmware_ver;
+	u16 tun_xtal;
+	u8 tun_fdiv;
+	u8 tun_clk_mode;
+	u32 tun_fn_min;
+};
+
+/* read multiple registers */
+static int it913x_rd_regs(struct it913x_state *state,
+		u32 reg, u8 *data, u8 count)
+{
+	int ret;
+	u8 b[3];
+	struct i2c_msg msg[2] = {
+		{ .addr = state->i2c_addr, .flags = 0,
+			.buf = b, .len = sizeof(b) },
+		{ .addr = state->i2c_addr, .flags = I2C_M_RD,
+			.buf = data, .len = count }
+	};
+	b[0] = (u8)(reg >> 16) & 0xff;
+	b[1] = (u8)(reg >> 8) & 0xff;
+	b[2] = (u8) reg & 0xff;
+	b[0] |= 0x80; /* All reads from demodulator */
+
+	ret = i2c_transfer(state->i2c_adap, msg, 2);
+
+	return ret;
+}
+
+/* read single register */
+static int it913x_rd_reg(struct it913x_state *state, u32 reg)
+{
+	int ret;
+	u8 b[1];
+	ret = it913x_rd_regs(state, reg, &b[0], sizeof(b));
+	return (ret < 0) ? -ENODEV : b[0];
+}
+
+/* write multiple registers */
+static int it913x_wr_regs(struct it913x_state *state,
+		u8 pro, u32 reg, u8 buf[], u8 count)
+{
+	u8 b[256];
+	struct i2c_msg msg[1] = {
+		{ .addr = state->i2c_addr, .flags = 0,
+		  .buf = b, .len = 3 + count }
+	};
+	int ret;
+	b[0] = (u8)(reg >> 16) & 0xff;
+	b[1] = (u8)(reg >> 8) & 0xff;
+	b[2] = (u8) reg & 0xff;
+	memcpy(&b[3], buf, count);
+
+	if (pro == PRO_DMOD)
+		b[0] |= 0x80;
+
+	ret = i2c_transfer(state->i2c_adap, msg, 1);
+
+	if (ret < 0)
+		return -EIO;
+
+	return 0;
+}
+
+/* write single register */
+static int it913x_wr_reg(struct it913x_state *state,
+		u8 pro, u32 reg, u32 data)
+{
+	int ret;
+	u8 b[4];
+	u8 s;
+
+	b[0] = data >> 24;
+	b[1] = (data >> 16) & 0xff;
+	b[2] = (data >> 8) & 0xff;
+	b[3] = data & 0xff;
+	/* expand write as needed */
+	if (data < 0x100)
+		s = 3;
+	else if (data < 0x1000)
+		s = 2;
+	else if (data < 0x100000)
+		s = 1;
+	else
+		s = 0;
+
+	ret = it913x_wr_regs(state, pro, reg, &b[s], sizeof(b) - s);
+
+	return ret;
+}
+
+static int it913x_script_loader(struct it913x_state *state,
+		struct it913xset *loadscript)
+{
+	int ret, i;
+	if (loadscript == NULL)
+		return -EINVAL;
+
+	for (i = 0; i < 1000; ++i) {
+		if (loadscript[i].pro == 0xff)
+			break;
+		ret = it913x_wr_regs(state, loadscript[i].pro,
+			loadscript[i].address,
+			loadscript[i].reg, loadscript[i].count);
+		if (ret < 0)
+			return -ENODEV;
+	}
+	return 0;
+}
+
+static int it913x_init(struct dvb_frontend *fe)
+{
+	struct it913x_state *state = fe->tuner_priv;
+	int ret, i, reg;
+	u8 val, nv_val;
+	u8 nv[] = {48, 32, 24, 16, 12, 8, 6, 4, 2};
+	u8 b[2];
+
+	reg = it913x_rd_reg(state, 0xec86);
+	switch (reg) {
+	case 0:
+		state->tun_clk_mode = reg;
+		state->tun_xtal = 2000;
+		state->tun_fdiv = 3;
+		val = 16;
+		break;
+	case -ENODEV:
+		return -ENODEV;
+	case 1:
+	default:
+		state->tun_clk_mode = reg;
+		state->tun_xtal = 640;
+		state->tun_fdiv = 1;
+		val = 6;
+		break;
+	}
+
+	reg = it913x_rd_reg(state, 0xed03);
+
+	if (reg < 0)
+		return -ENODEV;
+	else if (reg < ARRAY_SIZE(nv))
+		nv_val = nv[reg];
+	else
+		nv_val = 2;
+
+	for (i = 0; i < 50; i++) {
+		ret = it913x_rd_regs(state, 0xed23, &b[0], sizeof(b));
+		reg = (b[1] << 8) + b[0];
+		if (reg > 0)
+			break;
+		if (ret < 0)
+			return -ENODEV;
+		udelay(2000);
+	}
+	state->tun_fn_min = state->tun_xtal * reg;
+	state->tun_fn_min /= (state->tun_fdiv * nv_val);
+	dev_dbg(&state->i2c_adap->dev, "%s: Tuner fn_min %d\n", __func__,
+			state->tun_fn_min);
+
+	if (state->chip_ver > 1)
+		msleep(50);
+	else {
+		for (i = 0; i < 50; i++) {
+			reg = it913x_rd_reg(state, 0xec82);
+			if (reg > 0)
+				break;
+			if (reg < 0)
+				return -ENODEV;
+			udelay(2000);
+		}
+	}
+
+	/* Power Up Tuner - common all versions */
+	ret = it913x_wr_reg(state, PRO_DMOD, 0xec40, 0x1);
+	ret |= it913x_wr_reg(state, PRO_DMOD, 0xfba8, 0x0);
+	ret |= it913x_wr_reg(state, PRO_DMOD, 0xec57, 0x0);
+	ret |= it913x_wr_reg(state, PRO_DMOD, 0xec58, 0x0);
+
+	return it913x_wr_reg(state, PRO_DMOD, 0xed81, val);
+}
+
+static int it9137_set_params(struct dvb_frontend *fe)
+{
+	struct it913x_state *state = fe->tuner_priv;
+	struct it913xset *set_tuner = set_it9137_template;
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	u32 bandwidth = p->bandwidth_hz;
+	u32 frequency_m = p->frequency;
+	int ret, reg;
+	u32 frequency = frequency_m / 1000;
+	u32 freq, temp_f, tmp;
+	u16 iqik_m_cal;
+	u16 n_div;
+	u8 n;
+	u8 l_band;
+	u8 lna_band;
+	u8 bw;
+
+	if (state->firmware_ver == 1)
+		set_tuner = set_it9135_template;
+	else
+		set_tuner = set_it9137_template;
+
+	dev_dbg(&state->i2c_adap->dev, "%s: Tuner Frequency %d Bandwidth %d\n",
+			__func__, frequency, bandwidth);
+
+	if (frequency >= 51000 && frequency <= 440000) {
+		l_band = 0;
+		lna_band = 0;
+	} else if (frequency > 440000 && frequency <= 484000) {
+		l_band = 1;
+		lna_band = 1;
+	} else if (frequency > 484000 && frequency <= 533000) {
+		l_band = 1;
+		lna_band = 2;
+	} else if (frequency > 533000 && frequency <= 587000) {
+		l_band = 1;
+		lna_band = 3;
+	} else if (frequency > 587000 && frequency <= 645000) {
+		l_band = 1;
+		lna_band = 4;
+	} else if (frequency > 645000 && frequency <= 710000) {
+		l_band = 1;
+		lna_band = 5;
+	} else if (frequency > 710000 && frequency <= 782000) {
+		l_band = 1;
+		lna_band = 6;
+	} else if (frequency > 782000 && frequency <= 860000) {
+		l_band = 1;
+		lna_band = 7;
+	} else if (frequency > 1450000 && frequency <= 1492000) {
+		l_band = 1;
+		lna_band = 0;
+	} else if (frequency > 1660000 && frequency <= 1685000) {
+		l_band = 1;
+		lna_band = 1;
+	} else
+		return -EINVAL;
+	set_tuner[0].reg[0] = lna_band;
+
+	switch (bandwidth) {
+	case 5000000:
+		bw = 0;
+		break;
+	case 6000000:
+		bw = 2;
+		break;
+	case 7000000:
+		bw = 4;
+		break;
+	default:
+	case 8000000:
+		bw = 6;
+		break;
+	}
+
+	set_tuner[1].reg[0] = bw;
+	set_tuner[2].reg[0] = 0xa0 | (l_band << 3);
+
+	if (frequency > 53000 && frequency <= 74000) {
+		n_div = 48;
+		n = 0;
+	} else if (frequency > 74000 && frequency <= 111000) {
+		n_div = 32;
+		n = 1;
+	} else if (frequency > 111000 && frequency <= 148000) {
+		n_div = 24;
+		n = 2;
+	} else if (frequency > 148000 && frequency <= 222000) {
+		n_div = 16;
+		n = 3;
+	} else if (frequency > 222000 && frequency <= 296000) {
+		n_div = 12;
+		n = 4;
+	} else if (frequency > 296000 && frequency <= 445000) {
+		n_div = 8;
+		n = 5;
+	} else if (frequency > 445000 && frequency <= state->tun_fn_min) {
+		n_div = 6;
+		n = 6;
+	} else if (frequency > state->tun_fn_min && frequency <= 950000) {
+		n_div = 4;
+		n = 7;
+	} else if (frequency > 1450000 && frequency <= 1680000) {
+		n_div = 2;
+		n = 0;
+	} else
+		return -EINVAL;
+
+	reg = it913x_rd_reg(state, 0xed81);
+	iqik_m_cal = (u16)reg * n_div;
+
+	if (reg < 0x20) {
+		if (state->tun_clk_mode == 0)
+			iqik_m_cal = (iqik_m_cal * 9) >> 5;
+		else
+			iqik_m_cal >>= 1;
+	} else {
+		iqik_m_cal = 0x40 - iqik_m_cal;
+		if (state->tun_clk_mode == 0)
+			iqik_m_cal = ~((iqik_m_cal * 9) >> 5);
+		else
+			iqik_m_cal = ~(iqik_m_cal >> 1);
+	}
+
+	temp_f = frequency * (u32)n_div * (u32)state->tun_fdiv;
+	freq = temp_f / state->tun_xtal;
+	tmp = freq * state->tun_xtal;
+
+	if ((temp_f - tmp) >= (state->tun_xtal >> 1))
+		freq++;
+
+	freq += (u32) n << 13;
+	/* Frequency OMEGA_IQIK_M_CAL_MID*/
+	temp_f = freq + (u32)iqik_m_cal;
+
+	set_tuner[3].reg[0] =  temp_f & 0xff;
+	set_tuner[4].reg[0] =  (temp_f >> 8) & 0xff;
+
+	dev_dbg(&state->i2c_adap->dev, "%s: High Frequency = %04x\n",
+			__func__, temp_f);
+
+	/* Lower frequency */
+	set_tuner[5].reg[0] =  freq & 0xff;
+	set_tuner[6].reg[0] =  (freq >> 8) & 0xff;
+
+	dev_dbg(&state->i2c_adap->dev, "%s: low Frequency = %04x\n",
+			__func__, freq);
+
+	ret = it913x_script_loader(state, set_tuner);
+
+	return (ret < 0) ? -ENODEV : 0;
+}
+
+/* Power sequence */
+/* Power Up	Tuner on -> Frontend suspend off -> Tuner clk on */
+/* Power Down	Frontend suspend on -> Tuner clk off -> Tuner off */
+
+static int it913x_sleep(struct dvb_frontend *fe)
+{
+	struct it913x_state *state = fe->tuner_priv;
+	return it913x_script_loader(state, it9137_tuner_off);
+}
+
+static int it913x_release(struct dvb_frontend *fe)
+{
+	kfree(fe->tuner_priv);
+	return 0;
+}
+
+static const struct dvb_tuner_ops it913x_tuner_ops = {
+	.info = {
+		.name           = "ITE Tech IT913X",
+		.frequency_min  = 174000000,
+		.frequency_max  = 862000000,
+	},
+
+	.release = it913x_release,
+
+	.init = it913x_init,
+	.sleep = it913x_sleep,
+	.set_params = it9137_set_params,
+};
+
+struct dvb_frontend *it913x_attach(struct dvb_frontend *fe,
+		struct i2c_adapter *i2c_adap, u8 i2c_addr, u8 config)
+{
+	struct it913x_state *state = NULL;
+
+	/* allocate memory for the internal state */
+	state = kzalloc(sizeof(struct it913x_state), GFP_KERNEL);
+	if (state == NULL)
+		return NULL;
+
+	state->i2c_adap = i2c_adap;
+	state->i2c_addr = i2c_addr;
+
+	switch (config) {
+	case AF9033_TUNER_IT9135_38:
+	case AF9033_TUNER_IT9135_51:
+	case AF9033_TUNER_IT9135_52:
+		state->chip_ver = 0x01;
+		break;
+	case AF9033_TUNER_IT9135_60:
+	case AF9033_TUNER_IT9135_61:
+	case AF9033_TUNER_IT9135_62:
+		state->chip_ver = 0x02;
+		break;
+	default:
+		dev_dbg(&i2c_adap->dev,
+				"%s: invalid config=%02x\n", __func__, config);
+		goto error;
+	}
+
+	state->tuner_type = config;
+	state->firmware_ver = 1;
+
+	fe->tuner_priv = state;
+	memcpy(&fe->ops.tuner_ops, &it913x_tuner_ops,
+			sizeof(struct dvb_tuner_ops));
+
+	dev_info(&i2c_adap->dev,
+			"%s: ITE Tech IT913X successfully attached\n",
+			KBUILD_MODNAME);
+	dev_dbg(&i2c_adap->dev, "%s: config=%02x chip_ver=%02x\n",
+			__func__, config, state->chip_ver);
+
+	return fe;
+error:
+	kfree(state);
+	return NULL;
+}
+EXPORT_SYMBOL(it913x_attach);
+
+MODULE_DESCRIPTION("ITE Tech IT913X silicon tuner driver");
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/tuners/tuner_it913x.h b/drivers/media/tuners/tuner_it913x.h
new file mode 100644
index 000000000000..12dd36bd9e79
--- /dev/null
+++ b/drivers/media/tuners/tuner_it913x.h
@@ -0,0 +1,45 @@
+/*
+ * ITE Tech IT9137 silicon tuner driver
+ *
+ *  Copyright (C) 2011 Malcolm Priestley (tvboxspy@gmail.com)
+ *  IT9137 Copyright (C) ITE Tech Inc.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.=
+ */
+
+#ifndef IT913X_H
+#define IT913X_H
+
+#include "dvb_frontend.h"
+
+#if defined(CONFIG_MEDIA_TUNER_IT913X) || \
+	(defined(CONFIG_MEDIA_TUNER_IT913X_MODULE) && defined(MODULE))
+extern struct dvb_frontend *it913x_attach(struct dvb_frontend *fe,
+	struct i2c_adapter *i2c_adap,
+	u8 i2c_addr,
+	u8 config);
+#else
+static inline struct dvb_frontend *it913x_attach(struct dvb_frontend *fe,
+	struct i2c_adapter *i2c_adap,
+	u8 i2c_addr,
+	u8 config)
+{
+	pr_warn("%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif
+
+#endif
diff --git a/drivers/media/tuners/tuner_it913x_priv.h b/drivers/media/tuners/tuner_it913x_priv.h
new file mode 100644
index 000000000000..ce652108aa5d
--- /dev/null
+++ b/drivers/media/tuners/tuner_it913x_priv.h
@@ -0,0 +1,78 @@
+/*
+ * ITE Tech IT9137 silicon tuner driver
+ *
+ *  Copyright (C) 2011 Malcolm Priestley (tvboxspy@gmail.com)
+ *  IT9137 Copyright (C) ITE Tech Inc.
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.=
+ */
+
+#ifndef IT913X_PRIV_H
+#define IT913X_PRIV_H
+
+#include "tuner_it913x.h"
+#include "af9033.h"
+
+#define PRO_LINK		0x0
+#define PRO_DMOD		0x1
+#define TRIGGER_OFSM		0x0000
+
+struct it913xset {	u32 pro;
+			u32 address;
+			u8 reg[15];
+			u8 count;
+};
+
+/* Tuner setting scripts (still keeping it9137) */
+static struct it913xset it9137_tuner_off[] = {
+	{PRO_DMOD, 0xfba8, {0x01}, 0x01}, /* Tuner Clock Off  */
+	{PRO_DMOD, 0xec40, {0x00}, 0x01}, /* Power Down Tuner */
+	{PRO_DMOD, 0xec02, {0x3f, 0x1f, 0x3f, 0x3f}, 0x04},
+	{PRO_DMOD, 0xec06, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+				0x00, 0x00, 0x00, 0x00}, 0x0c},
+	{PRO_DMOD, 0xec12, {0x00, 0x00, 0x00, 0x00}, 0x04},
+	{PRO_DMOD, 0xec17, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+				0x00}, 0x09},
+	{PRO_DMOD, 0xec22, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+				0x00, 0x00}, 0x0a},
+	{PRO_DMOD, 0xec20, {0x00}, 0x01},
+	{PRO_DMOD, 0xec3f, {0x01}, 0x01},
+	{0xff, 0x0000, {0x00}, 0x00}, /* Terminating Entry */
+};
+
+static struct it913xset set_it9135_template[] = {
+	{PRO_DMOD, 0xee06, {0x00}, 0x01},
+	{PRO_DMOD, 0xec56, {0x00}, 0x01},
+	{PRO_DMOD, 0xec4c, {0x00}, 0x01},
+	{PRO_DMOD, 0xec4d, {0x00}, 0x01},
+	{PRO_DMOD, 0xec4e, {0x00}, 0x01},
+	{PRO_DMOD, 0x011e, {0x00}, 0x01}, /* Older Devices */
+	{PRO_DMOD, 0x011f, {0x00}, 0x01},
+	{0xff, 0x0000, {0x00}, 0x00}, /* Terminating Entry */
+};
+
+static struct it913xset set_it9137_template[] = {
+	{PRO_DMOD, 0xee06, {0x00}, 0x01},
+	{PRO_DMOD, 0xec56, {0x00}, 0x01},
+	{PRO_DMOD, 0xec4c, {0x00}, 0x01},
+	{PRO_DMOD, 0xec4d, {0x00}, 0x01},
+	{PRO_DMOD, 0xec4e, {0x00}, 0x01},
+	{PRO_DMOD, 0xec4f, {0x00}, 0x01},
+	{PRO_DMOD, 0xec50, {0x00}, 0x01},
+	{0xff, 0x0000, {0x00}, 0x00}, /* Terminating Entry */
+};
+
+#endif
diff --git a/drivers/media/tuners/xc5000.c b/drivers/media/tuners/xc5000.c
index d6be1b613c52..5cd09a681b6a 100644
--- a/drivers/media/tuners/xc5000.c
+++ b/drivers/media/tuners/xc5000.c
@@ -422,13 +422,19 @@ static int xc_initialize(struct xc5000_priv *priv)
 }
 
 static int xc_SetTVStandard(struct xc5000_priv *priv,
-	u16 VideoMode, u16 AudioMode)
+	u16 VideoMode, u16 AudioMode, u8 RadioMode)
 {
 	int ret;
 	dprintk(1, "%s(0x%04x,0x%04x)\n", __func__, VideoMode, AudioMode);
-	dprintk(1, "%s() Standard = %s\n",
-		__func__,
-		XC5000_Standard[priv->video_standard].Name);
+	if (RadioMode) {
+		dprintk(1, "%s() Standard = %s\n",
+			__func__,
+			XC5000_Standard[RadioMode].Name);
+	} else {
+		dprintk(1, "%s() Standard = %s\n",
+			__func__,
+			XC5000_Standard[priv->video_standard].Name);
+	}
 
 	ret = xc_write_reg(priv, XREG_VIDEO_MODE, VideoMode);
 	if (ret == XC_RESULT_SUCCESS)
@@ -824,7 +830,7 @@ static int xc5000_set_params(struct dvb_frontend *fe)
 
 	ret = xc_SetTVStandard(priv,
 		XC5000_Standard[priv->video_standard].VideoMode,
-		XC5000_Standard[priv->video_standard].AudioMode);
+		XC5000_Standard[priv->video_standard].AudioMode, 0);
 	if (ret != XC_RESULT_SUCCESS) {
 		printk(KERN_ERR "xc5000: xc_SetTVStandard failed\n");
 		return -EREMOTEIO;
@@ -940,7 +946,7 @@ tune_channel:
 
 	ret = xc_SetTVStandard(priv,
 		XC5000_Standard[priv->video_standard].VideoMode,
-		XC5000_Standard[priv->video_standard].AudioMode);
+		XC5000_Standard[priv->video_standard].AudioMode, 0);
 	if (ret != XC_RESULT_SUCCESS) {
 		printk(KERN_ERR "xc5000: xc_SetTVStandard failed\n");
 		return -EREMOTEIO;
@@ -1003,7 +1009,7 @@ static int xc5000_set_radio_freq(struct dvb_frontend *fe,
 	priv->rf_mode = XC_RF_MODE_AIR;
 
 	ret = xc_SetTVStandard(priv, XC5000_Standard[radio_input].VideoMode,
-			       XC5000_Standard[radio_input].AudioMode);
+			       XC5000_Standard[radio_input].AudioMode, radio_input);
 
 	if (ret != XC_RESULT_SUCCESS) {
 		printk(KERN_ERR "xc5000: xc_SetTVStandard failed\n");
diff --git a/drivers/media/tuners/xc5000.h b/drivers/media/tuners/xc5000.h
index b1a547494625..7245cae19f0c 100644
--- a/drivers/media/tuners/xc5000.h
+++ b/drivers/media/tuners/xc5000.h
@@ -22,6 +22,7 @@
 #ifndef __XC5000_H__
 #define __XC5000_H__
 
+#include <linux/kconfig.h>
 #include <linux/firmware.h>
 
 struct dvb_frontend;
@@ -56,8 +57,7 @@ struct xc5000_config {
  * it's passed back to a bridge during tuner_callback().
  */
 
-#if defined(CONFIG_MEDIA_TUNER_XC5000) || \
-    (defined(CONFIG_MEDIA_TUNER_XC5000_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_MEDIA_TUNER_XC5000)
 extern struct dvb_frontend *xc5000_attach(struct dvb_frontend *fe,
 					  struct i2c_adapter *i2c,
 					  const struct xc5000_config *cfg);
diff --git a/drivers/media/usb/au0828/au0828-core.c b/drivers/media/usb/au0828/au0828-core.c
index 1e6f40ef1c6b..bd9d19a73efd 100644
--- a/drivers/media/usb/au0828/au0828-core.c
+++ b/drivers/media/usb/au0828/au0828-core.c
@@ -125,6 +125,26 @@ static int recv_control_msg(struct au0828_dev *dev, u16 request, u32 value,
 	return status;
 }
 
+static void au0828_usb_release(struct au0828_dev *dev)
+{
+	/* I2C */
+	au0828_i2c_unregister(dev);
+
+	kfree(dev);
+}
+
+#ifdef CONFIG_VIDEO_AU0828_V4L2
+static void au0828_usb_v4l2_release(struct v4l2_device *v4l2_dev)
+{
+	struct au0828_dev *dev =
+		container_of(v4l2_dev, struct au0828_dev, v4l2_dev);
+
+	v4l2_ctrl_handler_free(&dev->v4l2_ctrl_hdl);
+	v4l2_device_unregister(&dev->v4l2_dev);
+	au0828_usb_release(dev);
+}
+#endif
+
 static void au0828_usb_disconnect(struct usb_interface *interface)
 {
 	struct au0828_dev *dev = usb_get_intfdata(interface);
@@ -134,26 +154,19 @@ static void au0828_usb_disconnect(struct usb_interface *interface)
 	/* Digital TV */
 	au0828_dvb_unregister(dev);
 
-#ifdef CONFIG_VIDEO_AU0828_V4L2
-	if (AUVI_INPUT(0).type != AU0828_VMUX_UNDEFINED)
-		au0828_analog_unregister(dev);
-#endif
-
-	/* I2C */
-	au0828_i2c_unregister(dev);
-
-#ifdef CONFIG_VIDEO_AU0828_V4L2
-	v4l2_device_unregister(&dev->v4l2_dev);
-#endif
-
 	usb_set_intfdata(interface, NULL);
-
 	mutex_lock(&dev->mutex);
 	dev->usbdev = NULL;
 	mutex_unlock(&dev->mutex);
-
-	kfree(dev);
-
+#ifdef CONFIG_VIDEO_AU0828_V4L2
+	if (AUVI_INPUT(0).type != AU0828_VMUX_UNDEFINED) {
+		au0828_analog_unregister(dev);
+		v4l2_device_disconnect(&dev->v4l2_dev);
+		v4l2_device_put(&dev->v4l2_dev);
+		return;
+	}
+#endif
+	au0828_usb_release(dev);
 }
 
 static int au0828_usb_probe(struct usb_interface *interface,
@@ -202,15 +215,27 @@ static int au0828_usb_probe(struct usb_interface *interface,
 	dev->boardnr = id->driver_info;
 
 #ifdef CONFIG_VIDEO_AU0828_V4L2
+	dev->v4l2_dev.release = au0828_usb_v4l2_release;
+
 	/* Create the v4l2_device */
 	retval = v4l2_device_register(&interface->dev, &dev->v4l2_dev);
 	if (retval) {
-		printk(KERN_ERR "%s() v4l2_device_register failed\n",
+		pr_err("%s() v4l2_device_register failed\n",
+		       __func__);
+		mutex_unlock(&dev->lock);
+		kfree(dev);
+		return retval;
+	}
+	/* This control handler will inherit the controls from au8522 */
+	retval = v4l2_ctrl_handler_init(&dev->v4l2_ctrl_hdl, 4);
+	if (retval) {
+		pr_err("%s() v4l2_ctrl_handler_init failed\n",
 		       __func__);
 		mutex_unlock(&dev->lock);
 		kfree(dev);
-		return -EIO;
+		return retval;
 	}
+	dev->v4l2_dev.ctrl_handler = &dev->v4l2_ctrl_hdl;
 #endif
 
 	/* Power Up the bridge */
diff --git a/drivers/media/usb/au0828/au0828-video.c b/drivers/media/usb/au0828/au0828-video.c
index 8b9e8268e911..75ac9947cdac 100644
--- a/drivers/media/usb/au0828/au0828-video.c
+++ b/drivers/media/usb/au0828/au0828-video.c
@@ -35,6 +35,7 @@
 #include <linux/suspend.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
 #include <media/v4l2-chip-ident.h>
 #include <media/tuner.h>
 #include "au0828.h"
@@ -58,6 +59,12 @@ do {\
 	} \
   } while (0)
 
+static inline void i2c_gate_ctrl(struct au0828_dev *dev, int val)
+{
+	if (dev->dvb.frontend && dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl)
+		dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl(dev->dvb.frontend, val);
+}
+
 static inline void print_err_status(struct au0828_dev *dev,
 				    int packet, int status)
 {
@@ -988,20 +995,22 @@ static int au0828_v4l2_open(struct file *filp)
 
 	fh->type = type;
 	fh->dev = dev;
+	v4l2_fh_init(&fh->fh, vdev);
 	filp->private_data = fh;
 
-	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && dev->users == 0) {
+	if (mutex_lock_interruptible(&dev->lock)) {
+		kfree(fh);
+		return -ERESTARTSYS;
+	}
+	if (dev->users == 0) {
 		/* set au0828 interface0 to AS5 here again */
 		ret = usb_set_interface(dev->usbdev, 0, 5);
 		if (ret < 0) {
+			mutex_unlock(&dev->lock);
 			printk(KERN_INFO "Au0828 can't set alternate to 5!\n");
+			kfree(fh);
 			return -EBUSY;
 		}
-		dev->width = NTSC_STD_W;
-		dev->height = NTSC_STD_H;
-		dev->frame_size = dev->width * dev->height * 2;
-		dev->field_size = dev->width * dev->height;
-		dev->bytesperline = dev->width * 2;
 
 		au0828_analog_stream_enable(dev);
 		au0828_analog_stream_reset(dev);
@@ -1014,6 +1023,7 @@ static int au0828_v4l2_open(struct file *filp)
 	}
 
 	dev->users++;
+	mutex_unlock(&dev->lock);
 
 	videobuf_queue_vmalloc_init(&fh->vb_vidq, &au0828_video_qops,
 				    NULL, &dev->slock,
@@ -1023,14 +1033,13 @@ static int au0828_v4l2_open(struct file *filp)
 				    &dev->lock);
 
 	/* VBI Setup */
-	dev->vbi_width = 720;
-	dev->vbi_height = 1;
 	videobuf_queue_vmalloc_init(&fh->vb_vbiq, &au0828_vbi_qops,
 				    NULL, &dev->slock,
 				    V4L2_BUF_TYPE_VBI_CAPTURE,
 				    V4L2_FIELD_SEQ_TB,
 				    sizeof(struct au0828_buffer), fh,
 				    &dev->lock);
+	v4l2_fh_add(&fh->fh);
 	return ret;
 }
 
@@ -1040,6 +1049,9 @@ static int au0828_v4l2_close(struct file *filp)
 	struct au0828_fh *fh = filp->private_data;
 	struct au0828_dev *dev = fh->dev;
 
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
+	mutex_lock(&dev->lock);
 	if (res_check(fh, AU0828_RESOURCE_VIDEO)) {
 		/* Cancel timeout thread in case they didn't call streamoff */
 		dev->vid_timeout_running = 0;
@@ -1058,19 +1070,14 @@ static int au0828_v4l2_close(struct file *filp)
 		res_free(fh, AU0828_RESOURCE_VBI);
 	}
 
-	if (dev->users == 1) {
-		if (dev->dev_state & DEV_DISCONNECTED) {
-			au0828_analog_unregister(dev);
-			kfree(dev);
-			return 0;
-		}
-
+	if (dev->users == 1 && video_is_registered(video_devdata(filp))) {
 		au0828_analog_stream_disable(dev);
 
 		au0828_uninit_isoc(dev);
 
 		/* Save some power by putting tuner to sleep */
 		v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_power, 0);
+		dev->std_set_in_tuner_core = 0;
 
 		/* When close the device, set the usb intf0 into alt0 to free
 		   USB bandwidth */
@@ -1078,6 +1085,7 @@ static int au0828_v4l2_close(struct file *filp)
 		if (ret < 0)
 			printk(KERN_INFO "Au0828 can't set alternate to 0!\n");
 	}
+	mutex_unlock(&dev->lock);
 
 	videobuf_mmap_free(&fh->vb_vidq);
 	videobuf_mmap_free(&fh->vb_vbiq);
@@ -1087,6 +1095,26 @@ static int au0828_v4l2_close(struct file *filp)
 	return 0;
 }
 
+/* Must be called with dev->lock held */
+static void au0828_init_tuner(struct au0828_dev *dev)
+{
+	struct v4l2_frequency f = {
+		.frequency = dev->ctrl_freq,
+		.type = V4L2_TUNER_ANALOG_TV,
+	};
+
+	if (dev->std_set_in_tuner_core)
+		return;
+	dev->std_set_in_tuner_core = 1;
+	i2c_gate_ctrl(dev, 1);
+	/* If we've never sent the standard in tuner core, do so now.
+	   We don't do this at device probe because we don't want to
+	   incur the cost of a firmware load */
+	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, dev->std);
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_frequency, &f);
+	i2c_gate_ctrl(dev, 0);
+}
+
 static ssize_t au0828_v4l2_read(struct file *filp, char __user *buf,
 				size_t count, loff_t *pos)
 {
@@ -1098,6 +1126,11 @@ static ssize_t au0828_v4l2_read(struct file *filp, char __user *buf,
 	if (rc < 0)
 		return rc;
 
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	au0828_init_tuner(dev);
+	mutex_unlock(&dev->lock);
+
 	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
 		if (res_locked(dev, AU0828_RESOURCE_VIDEO))
 			return -EBUSY;
@@ -1128,23 +1161,32 @@ static unsigned int au0828_v4l2_poll(struct file *filp, poll_table *wait)
 {
 	struct au0828_fh *fh = filp->private_data;
 	struct au0828_dev *dev = fh->dev;
-	int rc;
+	unsigned long req_events = poll_requested_events(wait);
+	unsigned int res;
 
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
+	if (check_dev(dev) < 0)
+		return POLLERR;
+
+	res = v4l2_ctrl_poll(filp, wait);
+	if (!(req_events & (POLLIN | POLLRDNORM)))
+		return res;
+
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	au0828_init_tuner(dev);
+	mutex_unlock(&dev->lock);
 
 	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
 		if (!res_get(fh, AU0828_RESOURCE_VIDEO))
 			return POLLERR;
-		return videobuf_poll_stream(filp, &fh->vb_vidq, wait);
-	} else if (fh->type == V4L2_BUF_TYPE_VBI_CAPTURE) {
+		return res | videobuf_poll_stream(filp, &fh->vb_vidq, wait);
+	}
+	if (fh->type == V4L2_BUF_TYPE_VBI_CAPTURE) {
 		if (!res_get(fh, AU0828_RESOURCE_VBI))
 			return POLLERR;
-		return videobuf_poll_stream(filp, &fh->vb_vbiq, wait);
-	} else {
-		return POLLERR;
+		return res | videobuf_poll_stream(filp, &fh->vb_vbiq, wait);
 	}
+	return POLLERR;
 }
 
 static int au0828_v4l2_mmap(struct file *filp, struct vm_area_struct *vma)
@@ -1172,9 +1214,6 @@ static int au0828_set_format(struct au0828_dev *dev, unsigned int cmd,
 	int width = format->fmt.pix.width;
 	int height = format->fmt.pix.height;
 
-	if (format->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
 	/* If they are demanding a format other than the one we support,
 	   bail out (tvtime asks for UYVY and then retries with YUYV) */
 	if (format->fmt.pix.pixelformat != V4L2_PIX_FMT_UYVY)
@@ -1193,6 +1232,7 @@ static int au0828_set_format(struct au0828_dev *dev, unsigned int cmd,
 	format->fmt.pix.sizeimage = width * height * 2;
 	format->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 	format->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	format->fmt.pix.priv = 0;
 
 	if (cmd == VIDIOC_TRY_FMT)
 		return 0;
@@ -1226,35 +1266,28 @@ static int au0828_set_format(struct au0828_dev *dev, unsigned int cmd,
 }
 
 
-static int vidioc_queryctrl(struct file *file, void *priv,
-			    struct v4l2_queryctrl *qc)
-{
-	struct au0828_fh *fh = priv;
-	struct au0828_dev *dev = fh->dev;
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, queryctrl, qc);
-	if (qc->type)
-		return 0;
-	else
-		return -EINVAL;
-}
-
 static int vidioc_querycap(struct file *file, void  *priv,
 			   struct v4l2_capability *cap)
 {
-	struct au0828_fh *fh  = priv;
+	struct video_device *vdev = video_devdata(file);
+	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
 
 	strlcpy(cap->driver, "au0828", sizeof(cap->driver));
 	strlcpy(cap->card, dev->board.name, sizeof(cap->card));
-	strlcpy(cap->bus_info, dev->v4l2_dev.name, sizeof(cap->bus_info));
+	usb_make_path(dev->usbdev, cap->bus_info, sizeof(cap->bus_info));
 
-	/*set the device capabilities */
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
-		V4L2_CAP_VBI_CAPTURE |
-		V4L2_CAP_AUDIO |
+	/* set the device capabilities */
+	cap->device_caps = V4L2_CAP_AUDIO |
 		V4L2_CAP_READWRITE |
 		V4L2_CAP_STREAMING |
 		V4L2_CAP_TUNER;
+	if (vdev->vfl_type == VFL_TYPE_GRABBER)
+		cap->device_caps |= V4L2_CAP_VIDEO_CAPTURE;
+	else
+		cap->device_caps |= V4L2_CAP_VBI_CAPTURE;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS |
+		V4L2_CAP_VBI_CAPTURE | V4L2_CAP_VIDEO_CAPTURE;
 	return 0;
 }
 
@@ -1286,6 +1319,7 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 	f->fmt.pix.sizeimage = dev->frame_size;
 	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M; /* NTSC/PAL */
 	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	f->fmt.pix.priv = 0;
 	return 0;
 }
 
@@ -1320,24 +1354,34 @@ out:
 	return rc;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id * norm)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 {
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
 
-	if (dev->dvb.frontend && dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl)
-		dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl(dev->dvb.frontend, 1);
+	dev->std = norm;
+
+	au0828_init_tuner(dev);
+
+	i2c_gate_ctrl(dev, 1);
 
 	/* FIXME: when we support something other than NTSC, we are going to
 	   have to make the au0828 bridge adjust the size of its capture
 	   buffer, which is currently hardcoded at 720x480 */
 
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, *norm);
-	dev->std_set_in_tuner_core = 1;
+	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, norm);
 
-	if (dev->dvb.frontend && dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl)
-		dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl(dev->dvb.frontend, 0);
+	i2c_gate_ctrl(dev, 0);
+
+	return 0;
+}
+
+static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *norm)
+{
+	struct au0828_fh *fh = priv;
+	struct au0828_dev *dev = fh->dev;
 
+	*norm = dev->std;
 	return 0;
 }
 
@@ -1368,10 +1412,13 @@ static int vidioc_enum_input(struct file *file, void *priv,
 	input->index = tmp;
 	strcpy(input->name, inames[AUVI_INPUT(tmp).type]);
 	if ((AUVI_INPUT(tmp).type == AU0828_VMUX_TELEVISION) ||
-	    (AUVI_INPUT(tmp).type == AU0828_VMUX_CABLE))
+	    (AUVI_INPUT(tmp).type == AU0828_VMUX_CABLE)) {
 		input->type |= V4L2_INPUT_TYPE_TUNER;
-	else
+		input->audioset = 1;
+	} else {
 		input->type |= V4L2_INPUT_TYPE_CAMERA;
+		input->audioset = 2;
+	}
 
 	input->std = dev->vdev->tvnorms;
 
@@ -1386,32 +1433,25 @@ static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
 	return 0;
 }
 
-static int vidioc_s_input(struct file *file, void *priv, unsigned int index)
+static void au0828_s_input(struct au0828_dev *dev, int index)
 {
-	struct au0828_fh *fh = priv;
-	struct au0828_dev *dev = fh->dev;
 	int i;
 
-	dprintk(1, "VIDIOC_S_INPUT in function %s, input=%d\n", __func__,
-		index);
-	if (index >= AU0828_MAX_INPUT)
-		return -EINVAL;
-	if (AUVI_INPUT(index).type == 0)
-		return -EINVAL;
-	dev->ctrl_input = index;
-
 	switch (AUVI_INPUT(index).type) {
 	case AU0828_VMUX_SVIDEO:
 		dev->input_type = AU0828_VMUX_SVIDEO;
+		dev->ctrl_ainput = 1;
 		break;
 	case AU0828_VMUX_COMPOSITE:
 		dev->input_type = AU0828_VMUX_COMPOSITE;
+		dev->ctrl_ainput = 1;
 		break;
 	case AU0828_VMUX_TELEVISION:
 		dev->input_type = AU0828_VMUX_TELEVISION;
+		dev->ctrl_ainput = 0;
 		break;
 	default:
-		dprintk(1, "VIDIOC_S_INPUT unknown input type set [%d]\n",
+		dprintk(1, "unknown input type set [%d]\n",
 			AUVI_INPUT(index).type);
 		break;
 	}
@@ -1442,55 +1482,60 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int index)
 
 	v4l2_device_call_all(&dev->v4l2_dev, 0, audio, s_routing,
 			AUVI_INPUT(index).amux, 0, 0);
-	return 0;
 }
 
-static int vidioc_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
+static int vidioc_s_input(struct file *file, void *priv, unsigned int index)
 {
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
-	unsigned int index = a->index;
 
+	dprintk(1, "VIDIOC_S_INPUT in function %s, input=%d\n", __func__,
+		index);
+	if (index >= AU0828_MAX_INPUT)
+		return -EINVAL;
+	if (AUVI_INPUT(index).type == 0)
+		return -EINVAL;
+	dev->ctrl_input = index;
+	au0828_s_input(dev, index);
+	return 0;
+}
+
+static int vidioc_enumaudio(struct file *file, void *priv, struct v4l2_audio *a)
+{
 	if (a->index > 1)
 		return -EINVAL;
 
-	index = dev->ctrl_ainput;
-	if (index == 0)
+	if (a->index == 0)
 		strcpy(a->name, "Television");
 	else
 		strcpy(a->name, "Line in");
 
 	a->capability = V4L2_AUDCAP_STEREO;
-	a->index = index;
 	return 0;
 }
 
-static int vidioc_s_audio(struct file *file, void *priv, const struct v4l2_audio *a)
+static int vidioc_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
 {
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
-	if (a->index != dev->ctrl_ainput)
-		return -EINVAL;
-	return 0;
-}
 
-static int vidioc_g_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
-{
-	struct au0828_fh *fh = priv;
-	struct au0828_dev *dev = fh->dev;
+	a->index = dev->ctrl_ainput;
+	if (a->index == 0)
+		strcpy(a->name, "Television");
+	else
+		strcpy(a->name, "Line in");
 
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, g_ctrl, ctrl);
+	a->capability = V4L2_AUDCAP_STEREO;
 	return 0;
-
 }
 
-static int vidioc_s_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
+static int vidioc_s_audio(struct file *file, void *priv, const struct v4l2_audio *a)
 {
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_ctrl, ctrl);
+
+	if (a->index != dev->ctrl_ainput)
+		return -EINVAL;
 	return 0;
 }
 
@@ -1503,12 +1548,16 @@ static int vidioc_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 		return -EINVAL;
 
 	strcpy(t->name, "Auvitek tuner");
+
+	au0828_init_tuner(dev);
+	i2c_gate_ctrl(dev, 1);
 	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_tuner, t);
+	i2c_gate_ctrl(dev, 0);
 	return 0;
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
@@ -1516,15 +1565,10 @@ static int vidioc_s_tuner(struct file *file, void *priv,
 	if (t->index != 0)
 		return -EINVAL;
 
-	t->type = V4L2_TUNER_ANALOG_TV;
-
-	if (dev->dvb.frontend && dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl)
-		dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl(dev->dvb.frontend, 1);
-
+	au0828_init_tuner(dev);
+	i2c_gate_ctrl(dev, 1);
 	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_tuner, t);
-
-	if (dev->dvb.frontend && dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl)
-		dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl(dev->dvb.frontend, 0);
+	i2c_gate_ctrl(dev, 0);
 
 	dprintk(1, "VIDIOC_S_TUNER: signal = %x, afc = %x\n", t->signal,
 		t->afc);
@@ -1539,40 +1583,31 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
 
-	freq->type = V4L2_TUNER_ANALOG_TV;
+	if (freq->tuner != 0)
+		return -EINVAL;
 	freq->frequency = dev->ctrl_freq;
 	return 0;
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *freq)
+				const struct v4l2_frequency *freq)
 {
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
+	struct v4l2_frequency new_freq = *freq;
 
 	if (freq->tuner != 0)
 		return -EINVAL;
-	if (freq->type != V4L2_TUNER_ANALOG_TV)
-		return -EINVAL;
-
-	dev->ctrl_freq = freq->frequency;
 
-	if (dev->dvb.frontend && dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl)
-		dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl(dev->dvb.frontend, 1);
-
-	if (dev->std_set_in_tuner_core == 0) {
-	  /* If we've never sent the standard in tuner core, do so now.  We
-	     don't do this at device probe because we don't want to incur
-	     the cost of a firmware load */
-	  v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std,
-			       dev->vdev->tvnorms);
-	  dev->std_set_in_tuner_core = 1;
-	}
+	au0828_init_tuner(dev);
+	i2c_gate_ctrl(dev, 1);
 
 	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_frequency, freq);
+	/* Get the actual set (and possibly clamped) frequency */
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_frequency, &new_freq);
+	dev->ctrl_freq = new_freq.frequency;
 
-	if (dev->dvb.frontend && dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl)
-		dev->dvb.frontend->ops.analog_ops.i2c_gate_ctrl(dev->dvb.frontend, 0);
+	i2c_gate_ctrl(dev, 0);
 
 	au0828_analog_stream_reset(dev);
 
@@ -1598,6 +1633,7 @@ static int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
 	format->fmt.vbi.count[1] = dev->vbi_height;
 	format->fmt.vbi.start[0] = 21;
 	format->fmt.vbi.start[1] = 284;
+	memset(format->fmt.vbi.reserved, 0, sizeof(format->fmt.vbi.reserved));
 
 	return 0;
 }
@@ -1664,6 +1700,7 @@ static int vidioc_streamon(struct file *file, void *priv,
 	if (unlikely(!res_get(fh, get_ressource(fh))))
 		return -EBUSY;
 
+	au0828_init_tuner(dev);
 	if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
 		au0828_analog_stream_enable(dev);
 		v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_stream, 1);
@@ -1756,7 +1793,7 @@ static int vidioc_g_register(struct file *file, void *priv,
 }
 
 static int vidioc_s_register(struct file *file, void *priv,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct au0828_fh *fh = priv;
 	struct au0828_dev *dev = fh->dev;
@@ -1773,6 +1810,15 @@ static int vidioc_s_register(struct file *file, void *priv,
 }
 #endif
 
+static int vidioc_log_status(struct file *file, void *fh)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	v4l2_ctrl_log_status(file, fh);
+	v4l2_device_call_all(vdev->v4l2_dev, 0, core, log_status);
+	return 0;
+}
+
 static int vidioc_reqbufs(struct file *file, void *priv,
 			  struct v4l2_requestbuffers *rb)
 {
@@ -1872,7 +1918,9 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_try_fmt_vid_cap     = vidioc_try_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap       = vidioc_s_fmt_vid_cap,
 	.vidioc_g_fmt_vbi_cap       = vidioc_g_fmt_vbi_cap,
+	.vidioc_try_fmt_vbi_cap     = vidioc_g_fmt_vbi_cap,
 	.vidioc_s_fmt_vbi_cap       = vidioc_g_fmt_vbi_cap,
+	.vidioc_enumaudio           = vidioc_enumaudio,
 	.vidioc_g_audio             = vidioc_g_audio,
 	.vidioc_s_audio             = vidioc_s_audio,
 	.vidioc_cropcap             = vidioc_cropcap,
@@ -1881,12 +1929,10 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_qbuf                = vidioc_qbuf,
 	.vidioc_dqbuf               = vidioc_dqbuf,
 	.vidioc_s_std               = vidioc_s_std,
+	.vidioc_g_std               = vidioc_g_std,
 	.vidioc_enum_input          = vidioc_enum_input,
 	.vidioc_g_input             = vidioc_g_input,
 	.vidioc_s_input             = vidioc_s_input,
-	.vidioc_queryctrl           = vidioc_queryctrl,
-	.vidioc_g_ctrl              = vidioc_g_ctrl,
-	.vidioc_s_ctrl              = vidioc_s_ctrl,
 	.vidioc_streamon            = vidioc_streamon,
 	.vidioc_streamoff           = vidioc_streamoff,
 	.vidioc_g_tuner             = vidioc_g_tuner,
@@ -1898,6 +1944,9 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_s_register          = vidioc_s_register,
 #endif
 	.vidioc_g_chip_ident        = vidioc_g_chip_ident,
+	.vidioc_log_status	    = vidioc_log_status,
+	.vidioc_subscribe_event     = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event   = v4l2_event_unsubscribe,
 };
 
 static const struct video_device au0828_video_template = {
@@ -1905,7 +1954,6 @@ static const struct video_device au0828_video_template = {
 	.release                    = video_device_release,
 	.ioctl_ops 		    = &video_ioctl_ops,
 	.tvnorms                    = V4L2_STD_NTSC_M,
-	.current_norm               = V4L2_STD_NTSC_M,
 };
 
 /**************************************************************************/
@@ -1972,7 +2020,12 @@ int au0828_analog_register(struct au0828_dev *dev,
 	dev->field_size = dev->width * dev->height;
 	dev->frame_size = dev->field_size << 1;
 	dev->bytesperline = dev->width << 1;
+	dev->vbi_width = 720;
+	dev->vbi_height = 1;
 	dev->ctrl_ainput = 0;
+	dev->ctrl_freq = 960;
+	dev->std = V4L2_STD_NTSC_M;
+	au0828_s_input(dev, 0);
 
 	/* allocate and fill v4l2 video struct */
 	dev->vdev = video_device_alloc();
@@ -1991,14 +2044,16 @@ int au0828_analog_register(struct au0828_dev *dev,
 
 	/* Fill the video capture device struct */
 	*dev->vdev = au0828_video_template;
-	dev->vdev->parent = &dev->usbdev->dev;
+	dev->vdev->v4l2_dev = &dev->v4l2_dev;
 	dev->vdev->lock = &dev->lock;
+	set_bit(V4L2_FL_USE_FH_PRIO, &dev->vdev->flags);
 	strcpy(dev->vdev->name, "au0828a video");
 
 	/* Setup the VBI device */
 	*dev->vbi_dev = au0828_video_template;
-	dev->vbi_dev->parent = &dev->usbdev->dev;
+	dev->vbi_dev->v4l2_dev = &dev->v4l2_dev;
 	dev->vbi_dev->lock = &dev->lock;
+	set_bit(V4L2_FL_USE_FH_PRIO, &dev->vbi_dev->flags);
 	strcpy(dev->vbi_dev->name, "au0828a vbi");
 
 	/* Register the v4l2 device */
diff --git a/drivers/media/usb/au0828/au0828.h b/drivers/media/usb/au0828/au0828.h
index e579ff69ca4a..ef1f57f22be7 100644
--- a/drivers/media/usb/au0828/au0828.h
+++ b/drivers/media/usb/au0828/au0828.h
@@ -28,6 +28,8 @@
 #include <linux/videodev2.h>
 #include <media/videobuf-vmalloc.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
 
 /* DVB */
 #include "demux.h"
@@ -118,6 +120,9 @@ enum au0828_dev_state {
 };
 
 struct au0828_fh {
+	/* must be the first field of this struct! */
+	struct v4l2_fh fh;
+
 	struct au0828_dev *dev;
 	unsigned int  resources;
 
@@ -202,6 +207,7 @@ struct au0828_dev {
 #ifdef CONFIG_VIDEO_AU0828_V4L2
 	/* Analog */
 	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler v4l2_ctrl_hdl;
 #endif
 	int users;
 	unsigned int resources;	/* resources in use */
@@ -216,6 +222,7 @@ struct au0828_dev {
 	int vbi_width;
 	int vbi_height;
 	u32 vbi_read;
+	v4l2_std_id std;
 	u32 field_size;
 	u32 frame_size;
 	u32 bytesperline;
diff --git a/drivers/media/usb/cx231xx/cx231xx-417.c b/drivers/media/usb/cx231xx/cx231xx-417.c
index 28688dbcb609..f548db8043d4 100644
--- a/drivers/media/usb/cx231xx/cx231xx-417.c
+++ b/drivers/media/usb/cx231xx/cx231xx-417.c
@@ -34,11 +34,12 @@
 #include <linux/vmalloc.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
 #include <media/cx2341x.h>
+#include <media/tuner.h>
 #include <linux/usb.h>
 
 #include "cx231xx.h"
-/*#include "cx23885-ioctl.h"*/
 
 #define CX231xx_FIRM_IMAGE_SIZE 376836
 #define CX231xx_FIRM_IMAGE_NAME "v4l-cx23885-enc.fw"
@@ -75,9 +76,11 @@
 static unsigned int mpegbufs = 8;
 module_param(mpegbufs, int, 0644);
 MODULE_PARM_DESC(mpegbufs, "number of mpeg buffers, range 2-32");
+
 static unsigned int mpeglines = 128;
 module_param(mpeglines, int, 0644);
 MODULE_PARM_DESC(mpeglines, "number of lines in an MPEG buffer, range 2-32");
+
 static unsigned int mpeglinesize = 512;
 module_param(mpeglinesize, int, 0644);
 MODULE_PARM_DESC(mpeglinesize,
@@ -86,10 +89,10 @@ MODULE_PARM_DESC(mpeglinesize,
 static unsigned int v4l_debug = 1;
 module_param(v4l_debug, int, 0644);
 MODULE_PARM_DESC(v4l_debug, "enable V4L debug messages");
-struct cx231xx_dmaqueue *dma_qq;
+
 #define dprintk(level, fmt, arg...)\
 	do { if (v4l_debug >= level) \
-		printk(KERN_INFO "%s: " fmt, \
+		pr_info("%s: " fmt, \
 		(dev) ? dev->name : "cx231xx[?]", ## arg); \
 	} while (0)
 
@@ -131,11 +134,13 @@ static struct cx231xx_tvnorm cx231xx_tvnorms[] = {
 };
 
 /* ------------------------------------------------------------------ */
+
 enum cx231xx_capture_type {
 	CX231xx_MPEG_CAPTURE,
 	CX231xx_RAW_CAPTURE,
 	CX231xx_RAW_PASSTHRU_CAPTURE
 };
+
 enum cx231xx_capture_bits {
 	CX231xx_RAW_BITS_NONE             = 0x00,
 	CX231xx_RAW_BITS_YUV_CAPTURE      = 0x01,
@@ -144,33 +149,40 @@ enum cx231xx_capture_bits {
 	CX231xx_RAW_BITS_PASSTHRU_CAPTURE = 0x08,
 	CX231xx_RAW_BITS_TO_HOST_CAPTURE  = 0x10
 };
+
 enum cx231xx_capture_end {
 	CX231xx_END_AT_GOP, /* stop at the end of gop, generate irq */
 	CX231xx_END_NOW, /* stop immediately, no irq */
 };
+
 enum cx231xx_framerate {
 	CX231xx_FRAMERATE_NTSC_30, /* NTSC: 30fps */
 	CX231xx_FRAMERATE_PAL_25   /* PAL: 25fps */
 };
+
 enum cx231xx_stream_port {
 	CX231xx_OUTPUT_PORT_MEMORY,
 	CX231xx_OUTPUT_PORT_STREAMING,
 	CX231xx_OUTPUT_PORT_SERIAL
 };
+
 enum cx231xx_data_xfer_status {
 	CX231xx_MORE_BUFFERS_FOLLOW,
 	CX231xx_LAST_BUFFER,
 };
+
 enum cx231xx_picture_mask {
 	CX231xx_PICTURE_MASK_NONE,
 	CX231xx_PICTURE_MASK_I_FRAMES,
 	CX231xx_PICTURE_MASK_I_P_FRAMES = 0x3,
 	CX231xx_PICTURE_MASK_ALL_FRAMES = 0x7,
 };
+
 enum cx231xx_vbi_mode_bits {
 	CX231xx_VBI_BITS_SLICED,
 	CX231xx_VBI_BITS_RAW,
 };
+
 enum cx231xx_vbi_insertion_bits {
 	CX231xx_VBI_BITS_INSERT_IN_XTENSION_USR_DATA,
 	CX231xx_VBI_BITS_INSERT_IN_PRIVATE_PACKETS = 0x1 << 1,
@@ -178,56 +190,69 @@ enum cx231xx_vbi_insertion_bits {
 	CX231xx_VBI_BITS_SEPARATE_STREAM_USR_DATA = 0x4 << 1,
 	CX231xx_VBI_BITS_SEPARATE_STREAM_PRV_DATA = 0x5 << 1,
 };
+
 enum cx231xx_dma_unit {
 	CX231xx_DMA_BYTES,
 	CX231xx_DMA_FRAMES,
 };
+
 enum cx231xx_dma_transfer_status_bits {
 	CX231xx_DMA_TRANSFER_BITS_DONE = 0x01,
 	CX231xx_DMA_TRANSFER_BITS_ERROR = 0x04,
 	CX231xx_DMA_TRANSFER_BITS_LL_ERROR = 0x10,
 };
+
 enum cx231xx_pause {
 	CX231xx_PAUSE_ENCODING,
 	CX231xx_RESUME_ENCODING,
 };
+
 enum cx231xx_copyright {
 	CX231xx_COPYRIGHT_OFF,
 	CX231xx_COPYRIGHT_ON,
 };
+
 enum cx231xx_notification_type {
 	CX231xx_NOTIFICATION_REFRESH,
 };
+
 enum cx231xx_notification_status {
 	CX231xx_NOTIFICATION_OFF,
 	CX231xx_NOTIFICATION_ON,
 };
+
 enum cx231xx_notification_mailbox {
 	CX231xx_NOTIFICATION_NO_MAILBOX = -1,
 };
+
 enum cx231xx_field1_lines {
 	CX231xx_FIELD1_SAA7114 = 0x00EF, /* 239 */
 	CX231xx_FIELD1_SAA7115 = 0x00F0, /* 240 */
 	CX231xx_FIELD1_MICRONAS = 0x0105, /* 261 */
 };
+
 enum cx231xx_field2_lines {
 	CX231xx_FIELD2_SAA7114 = 0x00EF, /* 239 */
 	CX231xx_FIELD2_SAA7115 = 0x00F0, /* 240 */
 	CX231xx_FIELD2_MICRONAS = 0x0106, /* 262 */
 };
+
 enum cx231xx_custom_data_type {
 	CX231xx_CUSTOM_EXTENSION_USR_DATA,
 	CX231xx_CUSTOM_PRIVATE_PACKET,
 };
+
 enum cx231xx_mute {
 	CX231xx_UNMUTE,
 	CX231xx_MUTE,
 };
+
 enum cx231xx_mute_video_mask {
 	CX231xx_MUTE_VIDEO_V_MASK = 0x0000FF00,
 	CX231xx_MUTE_VIDEO_U_MASK = 0x00FF0000,
 	CX231xx_MUTE_VIDEO_Y_MASK = 0xFF000000,
 };
+
 enum cx231xx_mute_video_shift {
 	CX231xx_MUTE_VIDEO_V_SHIFT = 8,
 	CX231xx_MUTE_VIDEO_U_SHIFT = 16,
@@ -296,41 +321,43 @@ enum cx231xx_mute_video_shift {
 
 
 #define CX23417_GPIO_MASK 0xFC0003FF
-static int setITVCReg(struct cx231xx *dev, u32 gpio_direction, u32 value)
+
+static int set_itvc_reg(struct cx231xx *dev, u32 gpio_direction, u32 value)
 {
 	int status = 0;
 	u32 _gpio_direction = 0;
 
 	_gpio_direction = _gpio_direction & CX23417_GPIO_MASK;
-	_gpio_direction = _gpio_direction|gpio_direction;
+	_gpio_direction = _gpio_direction | gpio_direction;
 	status = cx231xx_send_gpio_cmd(dev, _gpio_direction,
 			 (u8 *)&value, 4, 0, 0);
 	return status;
 }
-static int getITVCReg(struct cx231xx *dev, u32 gpio_direction, u32 *pValue)
+
+static int get_itvc_reg(struct cx231xx *dev, u32 gpio_direction, u32 *val_ptr)
 {
 	int status = 0;
 	u32 _gpio_direction = 0;
 
 	_gpio_direction = _gpio_direction & CX23417_GPIO_MASK;
-	_gpio_direction = _gpio_direction|gpio_direction;
+	_gpio_direction = _gpio_direction | gpio_direction;
 
 	status = cx231xx_send_gpio_cmd(dev, _gpio_direction,
-		 (u8 *)pValue, 4, 0, 1);
+		 (u8 *)val_ptr, 4, 0, 1);
 	return status;
 }
 
-static int waitForMciComplete(struct cx231xx *dev)
+static int wait_for_mci_complete(struct cx231xx *dev)
 {
 	u32 gpio;
-	u32 gpio_driection = 0;
+	u32 gpio_direction = 0;
 	u8 count = 0;
-	getITVCReg(dev, gpio_driection, &gpio);
+	get_itvc_reg(dev, gpio_direction, &gpio);
 
 	while (!(gpio&0x020000)) {
 		msleep(10);
 
-		getITVCReg(dev, gpio_driection, &gpio);
+		get_itvc_reg(dev, gpio_direction, &gpio);
 
 		if (count++ > 100) {
 			dprintk(3, "ERROR: Timeout - gpio=%x\n", gpio);
@@ -345,57 +372,57 @@ static int mc417_register_write(struct cx231xx *dev, u16 address, u32 value)
 	u32 temp;
 	int status = 0;
 
-	temp = 0x82|MCI_REGISTER_DATA_BYTE0|((value&0x000000FF)<<8);
-	temp = temp<<10;
-	status = setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_REGISTER_DATA_BYTE0 | ((value & 0x000000FF) << 8);
+	temp = temp << 10;
+	status = set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 	if (status < 0)
 		return status;
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write data byte 1;*/
-	temp = 0x82|MCI_REGISTER_DATA_BYTE1|(value&0x0000FF00);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_REGISTER_DATA_BYTE1 | (value & 0x0000FF00);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write data byte 2;*/
-	temp = 0x82|MCI_REGISTER_DATA_BYTE2|((value&0x00FF0000)>>8);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_REGISTER_DATA_BYTE2 | ((value & 0x00FF0000) >> 8);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write data byte 3;*/
-	temp = 0x82|MCI_REGISTER_DATA_BYTE3|((value&0xFF000000)>>16);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_REGISTER_DATA_BYTE3 | ((value & 0xFF000000) >> 16);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write address byte 0;*/
-	temp = 0x82|MCI_REGISTER_ADDRESS_BYTE0|((address&0x000000FF)<<8);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_REGISTER_ADDRESS_BYTE0 | ((address & 0x000000FF) << 8);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write address byte 1;*/
-	temp = 0x82|MCI_REGISTER_ADDRESS_BYTE1|(address&0x0000FF00);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_REGISTER_ADDRESS_BYTE1 | (address & 0x0000FF00);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*Write that the mode is write.*/
 	temp = 0x82 | MCI_REGISTER_MODE | MCI_MODE_REGISTER_WRITE;
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
-	return waitForMciComplete(dev);
+	return wait_for_mci_complete(dev);
 }
 
 static int mc417_register_read(struct cx231xx *dev, u16 address, u32 *value)
@@ -407,70 +434,68 @@ static int mc417_register_read(struct cx231xx *dev, u16 address, u32 *value)
 
 	temp = 0x82 | MCI_REGISTER_ADDRESS_BYTE0 | ((address & 0x00FF) << 8);
 	temp = temp << 10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 	temp = temp | ((0x05) << 10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write address byte 1;*/
 	temp = 0x82 | MCI_REGISTER_ADDRESS_BYTE1 | (address & 0xFF00);
 	temp = temp << 10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 	temp = temp | ((0x05) << 10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write that the mode is read;*/
 	temp = 0x82 | MCI_REGISTER_MODE | MCI_MODE_REGISTER_READ;
 	temp = temp << 10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 	temp = temp | ((0x05) << 10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*wait for the MIRDY line to be asserted ,
 	signalling that the read is done;*/
-	ret = waitForMciComplete(dev);
+	ret = wait_for_mci_complete(dev);
 
 	/*switch the DATA- GPIO to input mode;*/
 
 	/*Read data byte 0;*/
 	temp = (0x82 | MCI_REGISTER_DATA_BYTE0) << 10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
 	temp = ((0x81 | MCI_REGISTER_DATA_BYTE0) << 10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
 	return_value |= ((temp & 0x03FC0000) >> 18);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87 << 10));
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	/* Read data byte 1;*/
 	temp = (0x82 | MCI_REGISTER_DATA_BYTE1) << 10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
 	temp = ((0x81 | MCI_REGISTER_DATA_BYTE1) << 10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
 
 	return_value |= ((temp & 0x03FC0000) >> 10);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87 << 10));
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	/*Read data byte 2;*/
 	temp = (0x82 | MCI_REGISTER_DATA_BYTE2) << 10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
 	temp = ((0x81 | MCI_REGISTER_DATA_BYTE2) << 10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
 	return_value |= ((temp & 0x03FC0000) >> 2);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87 << 10));
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	/*Read data byte 3;*/
 	temp = (0x82 | MCI_REGISTER_DATA_BYTE3) << 10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
 	temp = ((0x81 | MCI_REGISTER_DATA_BYTE3) << 10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
 	return_value |= ((temp & 0x03FC0000) << 6);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87 << 10));
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	*value  = return_value;
-
-
 	return ret;
 }
 
@@ -481,59 +506,59 @@ static int mc417_memory_write(struct cx231xx *dev, u32 address, u32 value)
 	u32 temp;
 	int ret = 0;
 
-	temp = 0x82 | MCI_MEMORY_DATA_BYTE0|((value & 0x000000FF) << 8);
+	temp = 0x82 | MCI_MEMORY_DATA_BYTE0 | ((value & 0x000000FF) << 8);
 	temp = temp << 10;
-	ret = setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	ret = set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 	if (ret < 0)
 		return ret;
-	temp = temp | ((0x05) << 10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write data byte 1;*/
 	temp = 0x82 | MCI_MEMORY_DATA_BYTE1 | (value & 0x0000FF00);
 	temp = temp << 10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp | ((0x05) << 10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write data byte 2;*/
-	temp = 0x82|MCI_MEMORY_DATA_BYTE2|((value&0x00FF0000)>>8);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_DATA_BYTE2 | ((value & 0x00FF0000) >> 8);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write data byte 3;*/
-	temp = 0x82|MCI_MEMORY_DATA_BYTE3|((value&0xFF000000)>>16);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_DATA_BYTE3 | ((value & 0xFF000000) >> 16);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/* write address byte 2;*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE2 | MCI_MODE_MEMORY_WRITE |
-		((address & 0x003F0000)>>8);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE2 | MCI_MODE_MEMORY_WRITE |
+		((address & 0x003F0000) >> 8);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/* write address byte 1;*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE1 | (address & 0xFF00);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE1 | (address & 0xFF00);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/* write address byte 0;*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE0|((address & 0x00FF)<<8);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE0 | ((address & 0x00FF) << 8);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*wait for MIRDY line;*/
-	waitForMciComplete(dev);
+	wait_for_mci_complete(dev);
 
 	return 0;
 }
@@ -545,68 +570,68 @@ static int mc417_memory_read(struct cx231xx *dev, u32 address, u32 *value)
 	int ret = 0;
 
 	/*write address byte 2;*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE2 | MCI_MODE_MEMORY_READ |
-		((address & 0x003F0000)>>8);
-	temp = temp<<10;
-	ret = setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE2 | MCI_MODE_MEMORY_READ |
+		((address & 0x003F0000) >> 8);
+	temp = temp << 10;
+	ret = set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 	if (ret < 0)
 		return ret;
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write address byte 1*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE1 | (address & 0xFF00);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE1 | (address & 0xFF00);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*write address byte 0*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE0 | ((address & 0x00FF)<<8);
-	temp = temp<<10;
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
-	temp = temp|((0x05)<<10);
-	setITVCReg(dev, ITVC_WRITE_DIR, temp);
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE0 | ((address & 0x00FF) << 8);
+	temp = temp << 10;
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
+	temp = temp | (0x05 << 10);
+	set_itvc_reg(dev, ITVC_WRITE_DIR, temp);
 
 	/*Wait for MIRDY line*/
-	ret = waitForMciComplete(dev);
+	ret = wait_for_mci_complete(dev);
 
 
 	/*Read data byte 3;*/
-	temp = (0x82|MCI_MEMORY_DATA_BYTE3)<<10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	temp = ((0x81|MCI_MEMORY_DATA_BYTE3)<<10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
-	return_value |= ((temp&0x03FC0000)<<6);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87<<10));
+	temp = (0x82 | MCI_MEMORY_DATA_BYTE3) << 10;
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	temp = ((0x81 | MCI_MEMORY_DATA_BYTE3) << 10);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
+	return_value |= ((temp & 0x03FC0000) << 6);
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	/*Read data byte 2;*/
-	temp = (0x82|MCI_MEMORY_DATA_BYTE2)<<10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	temp = ((0x81|MCI_MEMORY_DATA_BYTE2)<<10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
-	return_value |= ((temp&0x03FC0000)>>2);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87<<10));
+	temp = (0x82 | MCI_MEMORY_DATA_BYTE2) << 10;
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	temp = ((0x81 | MCI_MEMORY_DATA_BYTE2) << 10);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
+	return_value |= ((temp & 0x03FC0000) >> 2);
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	/* Read data byte 1;*/
-	temp = (0x82|MCI_MEMORY_DATA_BYTE1)<<10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	temp = ((0x81|MCI_MEMORY_DATA_BYTE1)<<10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
-	return_value |= ((temp&0x03FC0000)>>10);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87<<10));
+	temp = (0x82 | MCI_MEMORY_DATA_BYTE1) << 10;
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	temp = ((0x81 | MCI_MEMORY_DATA_BYTE1) << 10);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
+	return_value |= ((temp & 0x03FC0000) >> 10);
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	/*Read data byte 0;*/
-	temp = (0x82|MCI_MEMORY_DATA_BYTE0)<<10;
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	temp = ((0x81|MCI_MEMORY_DATA_BYTE0)<<10);
-	setITVCReg(dev, ITVC_READ_DIR, temp);
-	getITVCReg(dev, ITVC_READ_DIR, &temp);
-	return_value |= ((temp&0x03FC0000)>>18);
-	setITVCReg(dev, ITVC_READ_DIR, (0x87<<10));
+	temp = (0x82 | MCI_MEMORY_DATA_BYTE0) << 10;
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	temp = ((0x81 | MCI_MEMORY_DATA_BYTE0) << 10);
+	set_itvc_reg(dev, ITVC_READ_DIR, temp);
+	get_itvc_reg(dev, ITVC_READ_DIR, &temp);
+	return_value |= ((temp & 0x03FC0000) >> 18);
+	set_itvc_reg(dev, ITVC_READ_DIR, (0x87 << 10));
 
 	*value  = return_value;
 	return ret;
@@ -619,94 +644,91 @@ static char *cmd_to_str(int cmd)
 {
 	switch (cmd) {
 	case CX2341X_ENC_PING_FW:
-		return  "PING_FW";
+		return "PING_FW";
 	case CX2341X_ENC_START_CAPTURE:
-		return  "START_CAPTURE";
+		return "START_CAPTURE";
 	case CX2341X_ENC_STOP_CAPTURE:
-		return  "STOP_CAPTURE";
+		return "STOP_CAPTURE";
 	case CX2341X_ENC_SET_AUDIO_ID:
-		return  "SET_AUDIO_ID";
+		return "SET_AUDIO_ID";
 	case CX2341X_ENC_SET_VIDEO_ID:
-		return  "SET_VIDEO_ID";
+		return "SET_VIDEO_ID";
 	case CX2341X_ENC_SET_PCR_ID:
-		return  "SET_PCR_PID";
+		return "SET_PCR_PID";
 	case CX2341X_ENC_SET_FRAME_RATE:
-		return  "SET_FRAME_RATE";
+		return "SET_FRAME_RATE";
 	case CX2341X_ENC_SET_FRAME_SIZE:
-		return  "SET_FRAME_SIZE";
+		return "SET_FRAME_SIZE";
 	case CX2341X_ENC_SET_BIT_RATE:
-		return  "SET_BIT_RATE";
+		return "SET_BIT_RATE";
 	case CX2341X_ENC_SET_GOP_PROPERTIES:
-		return  "SET_GOP_PROPERTIES";
+		return "SET_GOP_PROPERTIES";
 	case CX2341X_ENC_SET_ASPECT_RATIO:
-		return  "SET_ASPECT_RATIO";
+		return "SET_ASPECT_RATIO";
 	case CX2341X_ENC_SET_DNR_FILTER_MODE:
-		return  "SET_DNR_FILTER_PROPS";
+		return "SET_DNR_FILTER_PROPS";
 	case CX2341X_ENC_SET_DNR_FILTER_PROPS:
-		return  "SET_DNR_FILTER_PROPS";
+		return "SET_DNR_FILTER_PROPS";
 	case CX2341X_ENC_SET_CORING_LEVELS:
-		return  "SET_CORING_LEVELS";
+		return "SET_CORING_LEVELS";
 	case CX2341X_ENC_SET_SPATIAL_FILTER_TYPE:
-		return  "SET_SPATIAL_FILTER_TYPE";
+		return "SET_SPATIAL_FILTER_TYPE";
 	case CX2341X_ENC_SET_VBI_LINE:
-		return  "SET_VBI_LINE";
+		return "SET_VBI_LINE";
 	case CX2341X_ENC_SET_STREAM_TYPE:
-		return  "SET_STREAM_TYPE";
+		return "SET_STREAM_TYPE";
 	case CX2341X_ENC_SET_OUTPUT_PORT:
-		return  "SET_OUTPUT_PORT";
+		return "SET_OUTPUT_PORT";
 	case CX2341X_ENC_SET_AUDIO_PROPERTIES:
-		return  "SET_AUDIO_PROPERTIES";
+		return "SET_AUDIO_PROPERTIES";
 	case CX2341X_ENC_HALT_FW:
-		return  "HALT_FW";
+		return "HALT_FW";
 	case CX2341X_ENC_GET_VERSION:
-		return  "GET_VERSION";
+		return "GET_VERSION";
 	case CX2341X_ENC_SET_GOP_CLOSURE:
-		return  "SET_GOP_CLOSURE";
+		return "SET_GOP_CLOSURE";
 	case CX2341X_ENC_GET_SEQ_END:
-		return  "GET_SEQ_END";
+		return "GET_SEQ_END";
 	case CX2341X_ENC_SET_PGM_INDEX_INFO:
-		return  "SET_PGM_INDEX_INFO";
+		return "SET_PGM_INDEX_INFO";
 	case CX2341X_ENC_SET_VBI_CONFIG:
-		return  "SET_VBI_CONFIG";
+		return "SET_VBI_CONFIG";
 	case CX2341X_ENC_SET_DMA_BLOCK_SIZE:
-		return  "SET_DMA_BLOCK_SIZE";
+		return "SET_DMA_BLOCK_SIZE";
 	case CX2341X_ENC_GET_PREV_DMA_INFO_MB_10:
-		return  "GET_PREV_DMA_INFO_MB_10";
+		return "GET_PREV_DMA_INFO_MB_10";
 	case CX2341X_ENC_GET_PREV_DMA_INFO_MB_9:
-		return  "GET_PREV_DMA_INFO_MB_9";
+		return "GET_PREV_DMA_INFO_MB_9";
 	case CX2341X_ENC_SCHED_DMA_TO_HOST:
-		return  "SCHED_DMA_TO_HOST";
+		return "SCHED_DMA_TO_HOST";
 	case CX2341X_ENC_INITIALIZE_INPUT:
-		return  "INITIALIZE_INPUT";
+		return "INITIALIZE_INPUT";
 	case CX2341X_ENC_SET_FRAME_DROP_RATE:
-		return  "SET_FRAME_DROP_RATE";
+		return "SET_FRAME_DROP_RATE";
 	case CX2341X_ENC_PAUSE_ENCODER:
-		return  "PAUSE_ENCODER";
+		return "PAUSE_ENCODER";
 	case CX2341X_ENC_REFRESH_INPUT:
-		return  "REFRESH_INPUT";
+		return "REFRESH_INPUT";
 	case CX2341X_ENC_SET_COPYRIGHT:
-		return  "SET_COPYRIGHT";
+		return "SET_COPYRIGHT";
 	case CX2341X_ENC_SET_EVENT_NOTIFICATION:
-		return  "SET_EVENT_NOTIFICATION";
+		return "SET_EVENT_NOTIFICATION";
 	case CX2341X_ENC_SET_NUM_VSYNC_LINES:
-		return  "SET_NUM_VSYNC_LINES";
+		return "SET_NUM_VSYNC_LINES";
 	case CX2341X_ENC_SET_PLACEHOLDER:
-		return  "SET_PLACEHOLDER";
+		return "SET_PLACEHOLDER";
 	case CX2341X_ENC_MUTE_VIDEO:
-		return  "MUTE_VIDEO";
+		return "MUTE_VIDEO";
 	case CX2341X_ENC_MUTE_AUDIO:
-		return  "MUTE_AUDIO";
+		return "MUTE_AUDIO";
 	case CX2341X_ENC_MISC:
-		return  "MISC";
+		return "MISC";
 	default:
 		return "UNKNOWN";
 	}
 }
 
-static int cx231xx_mbox_func(void *priv,
-			     u32 command,
-			     int in,
-			     int out,
+static int cx231xx_mbox_func(void *priv, u32 command, int in, int out,
 			     u32 data[CX2341X_MBOX_MAX_DATA])
 {
 	struct cx231xx *dev = priv;
@@ -721,11 +743,9 @@ static int cx231xx_mbox_func(void *priv,
 	   without side effects */
 	mc417_memory_read(dev, dev->cx23417_mailbox - 4, &value);
 	if (value != 0x12345678) {
-		dprintk(3,
-			"Firmware and/or mailbox pointer not initialized "
-			"or corrupted, signature = 0x%x, cmd = %s\n", value,
-			cmd_to_str(command));
-		return -1;
+		dprintk(3, "Firmware and/or mailbox pointer not initialized or corrupted, signature = 0x%x, cmd = %s\n",
+			value, cmd_to_str(command));
+		return -EIO;
 	}
 
 	/* This read looks at 32 bits, but flag is only 8 bits.
@@ -733,9 +753,9 @@ static int cx231xx_mbox_func(void *priv,
 	 */
 	mc417_memory_read(dev, dev->cx23417_mailbox, &flag);
 	if (flag) {
-		dprintk(3, "ERROR: Mailbox appears to be in use "
-			"(%x), cmd = %s\n", flag, cmd_to_str(command));
-		return -1;
+		dprintk(3, "ERROR: Mailbox appears to be in use (%x), cmd = %s\n",
+				flag, cmd_to_str(command));
+		return -EBUSY;
 	}
 
 	flag |= 1; /* tell 'em we're working on it */
@@ -764,7 +784,7 @@ static int cx231xx_mbox_func(void *priv,
 			break;
 		if (time_after(jiffies, timeout)) {
 			dprintk(3, "ERROR: API Mailbox timeout\n");
-			return -1;
+			return -EIO;
 		}
 		udelay(10);
 	}
@@ -781,17 +801,14 @@ static int cx231xx_mbox_func(void *priv,
 	flag = 0;
 	mc417_memory_write(dev, dev->cx23417_mailbox, flag);
 
-	return retval;
+	return 0;
 }
 
 /* We don't need to call the API often, so using just one
  * mailbox will probably suffice
  */
-static int cx231xx_api_cmd(struct cx231xx *dev,
-			   u32 command,
-			   u32 inputcnt,
-			   u32 outputcnt,
-			   ...)
+static int cx231xx_api_cmd(struct cx231xx *dev, u32 command,
+		u32 inputcnt, u32 outputcnt, ...)
 {
 	u32 data[CX2341X_MBOX_MAX_DATA];
 	va_list vargs;
@@ -813,6 +830,7 @@ static int cx231xx_api_cmd(struct cx231xx *dev,
 	return err;
 }
 
+
 static int cx231xx_find_mailbox(struct cx231xx *dev)
 {
 	u32 signature[4] = {
@@ -834,81 +852,80 @@ static int cx231xx_find_mailbox(struct cx231xx *dev)
 		else
 			signaturecnt = 0;
 		if (4 == signaturecnt) {
-			dprintk(1, "Mailbox signature found at 0x%x\n", i+1);
-			return i+1;
+			dprintk(1, "Mailbox signature found at 0x%x\n", i + 1);
+			return i + 1;
 		}
 	}
 	dprintk(3, "Mailbox signature values not found!\n");
 	return -1;
 }
 
-static void mciWriteMemoryToGPIO(struct cx231xx *dev, u32 address, u32 value,
+static void mci_write_memory_to_gpio(struct cx231xx *dev, u32 address, u32 value,
 		u32 *p_fw_image)
 {
-
 	u32 temp = 0;
 	int i = 0;
 
-	temp = 0x82|MCI_MEMORY_DATA_BYTE0|((value&0x000000FF)<<8);
-	temp = temp<<10;
+	temp = 0x82 | MCI_MEMORY_DATA_BYTE0 | ((value & 0x000000FF) << 8);
+	temp = temp << 10;
 	*p_fw_image = temp;
 	p_fw_image++;
-	temp = temp|((0x05)<<10);
+	temp = temp | (0x05 << 10);
 	*p_fw_image = temp;
 	p_fw_image++;
 
 	/*write data byte 1;*/
-	temp = 0x82|MCI_MEMORY_DATA_BYTE1|(value&0x0000FF00);
-	temp = temp<<10;
+	temp = 0x82 | MCI_MEMORY_DATA_BYTE1 | (value & 0x0000FF00);
+	temp = temp << 10;
 	*p_fw_image = temp;
 	p_fw_image++;
-	temp = temp|((0x05)<<10);
+	temp = temp | (0x05 << 10);
 	*p_fw_image = temp;
 	p_fw_image++;
 
 	/*write data byte 2;*/
-	temp = 0x82|MCI_MEMORY_DATA_BYTE2|((value&0x00FF0000)>>8);
-	temp = temp<<10;
+	temp = 0x82 | MCI_MEMORY_DATA_BYTE2 | ((value & 0x00FF0000) >> 8);
+	temp = temp << 10;
 	*p_fw_image = temp;
 	p_fw_image++;
-	temp = temp|((0x05)<<10);
+	temp = temp | (0x05 << 10);
 	*p_fw_image = temp;
 	p_fw_image++;
 
 	/*write data byte 3;*/
-	temp = 0x82|MCI_MEMORY_DATA_BYTE3|((value&0xFF000000)>>16);
-	temp = temp<<10;
+	temp = 0x82 | MCI_MEMORY_DATA_BYTE3 | ((value & 0xFF000000) >> 16);
+	temp = temp << 10;
 	*p_fw_image = temp;
 	p_fw_image++;
-	temp = temp|((0x05)<<10);
+	temp = temp | (0x05 << 10);
 	*p_fw_image = temp;
 	p_fw_image++;
 
 	/* write address byte 2;*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE2 | MCI_MODE_MEMORY_WRITE |
-		((address & 0x003F0000)>>8);
-	temp = temp<<10;
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE2 | MCI_MODE_MEMORY_WRITE |
+		((address & 0x003F0000) >> 8);
+	temp = temp << 10;
 	*p_fw_image = temp;
 	p_fw_image++;
-	temp = temp|((0x05)<<10);
+	temp = temp | (0x05 << 10);
 	*p_fw_image = temp;
 	p_fw_image++;
 
 	/* write address byte 1;*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE1 | (address & 0xFF00);
-	temp = temp<<10;
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE1 | (address & 0xFF00);
+	temp = temp << 10;
 	*p_fw_image = temp;
 	p_fw_image++;
-	temp = temp|((0x05)<<10);
+	temp = temp | (0x05 << 10);
 	*p_fw_image = temp;
 	p_fw_image++;
 
 	/* write address byte 0;*/
-	temp = 0x82|MCI_MEMORY_ADDRESS_BYTE0|((address & 0x00FF)<<8);
-	temp = temp<<10;
+	temp = 0x82 | MCI_MEMORY_ADDRESS_BYTE0 | ((address & 0x00FF) << 8);
+	temp = temp << 10;
 	*p_fw_image = temp;
 	p_fw_image++;
-	temp = temp|((0x05)<<10);
+	temp = temp | (0x05 << 10);
 	*p_fw_image = temp;
 	p_fw_image++;
 
@@ -971,8 +988,7 @@ static int cx231xx_load_firmware(struct cx231xx *dev)
 		IVTV_REG_APU, 0);
 
 	if (retval != 0) {
-		printk(KERN_ERR "%s: Error with mc417_register_write\n",
-			__func__);
+		pr_err("%s: Error with mc417_register_write\n", __func__);
 		return -1;
 	}
 
@@ -980,25 +996,21 @@ static int cx231xx_load_firmware(struct cx231xx *dev)
 				  &dev->udev->dev);
 
 	if (retval != 0) {
-		printk(KERN_ERR
-			"ERROR: Hotplug firmware request failed (%s).\n",
+		pr_err("ERROR: Hotplug firmware request failed (%s).\n",
 			CX231xx_FIRM_IMAGE_NAME);
-		printk(KERN_ERR "Please fix your hotplug setup, the board will "
-			"not work without firmware loaded!\n");
+		pr_err("Please fix your hotplug setup, the board will not work without firmware loaded!\n");
 		return -1;
 	}
 
 	if (firmware->size != CX231xx_FIRM_IMAGE_SIZE) {
-		printk(KERN_ERR "ERROR: Firmware size mismatch "
-			"(have %zd, expected %d)\n",
+		pr_err("ERROR: Firmware size mismatch (have %zd, expected %d)\n",
 			firmware->size, CX231xx_FIRM_IMAGE_SIZE);
 		release_firmware(firmware);
 		return -1;
 	}
 
 	if (0 != memcmp(firmware->data, magic, 8)) {
-		printk(KERN_ERR
-			"ERROR: Firmware magic mismatch, wrong file?\n");
+		pr_err("ERROR: Firmware magic mismatch, wrong file?\n");
 		release_firmware(firmware);
 		return -1;
 	}
@@ -1013,7 +1025,7 @@ static int cx231xx_load_firmware(struct cx231xx *dev)
 		 transfer_size += 4) {
 		fw_data = *p_fw_data;
 
-		 mciWriteMemoryToGPIO(dev, address, fw_data, p_current_fw);
+		mci_write_memory_to_gpio(dev, address, fw_data, p_current_fw);
 		address = address + 1;
 		p_current_fw += 20;
 		p_fw_data += 1;
@@ -1045,7 +1057,7 @@ static int cx231xx_load_firmware(struct cx231xx *dev)
 	retval |= mc417_register_write(dev, IVTV_REG_HW_BLOCKS,
 		IVTV_CMD_HW_BLOCKS_RST);
 	if (retval < 0) {
-		printk(KERN_ERR "%s: Error with mc417_register_write\n",
+		pr_err("%s: Error with mc417_register_write\n",
 			__func__);
 		return retval;
 	}
@@ -1057,7 +1069,7 @@ static int cx231xx_load_firmware(struct cx231xx *dev)
 	retval |= mc417_register_write(dev, IVTV_REG_VPU, value & 0xFFFFFFE8);
 
 	if (retval < 0) {
-		printk(KERN_ERR "%s: Error with mc417_register_write\n",
+		pr_err("%s: Error with mc417_register_write\n",
 			__func__);
 		return retval;
 	}
@@ -1082,10 +1094,10 @@ static void cx231xx_codec_settings(struct cx231xx *dev)
 	cx231xx_api_cmd(dev, CX2341X_ENC_SET_FRAME_SIZE, 2, 0,
 				dev->ts1.height, dev->ts1.width);
 
-	dev->mpeg_params.width = dev->ts1.width;
-	dev->mpeg_params.height = dev->ts1.height;
+	dev->mpeg_ctrl_handler.width = dev->ts1.width;
+	dev->mpeg_ctrl_handler.height = dev->ts1.height;
 
-	cx2341x_update(dev, cx231xx_mbox_func, NULL, &dev->mpeg_params);
+	cx2341x_handler_setup(&dev->mpeg_ctrl_handler);
 
 	cx231xx_api_cmd(dev, CX2341X_ENC_MISC, 2, 0, 3, 1);
 	cx231xx_api_cmd(dev, CX2341X_ENC_MISC, 2, 0, 4, 1);
@@ -1105,27 +1117,25 @@ static int cx231xx_initialize_codec(struct cx231xx *dev)
 		dprintk(2, "%s() PING OK\n", __func__);
 		retval = cx231xx_load_firmware(dev);
 		if (retval < 0) {
-			printk(KERN_ERR "%s() f/w load failed\n", __func__);
+			pr_err("%s() f/w load failed\n", __func__);
 			return retval;
 		}
 		retval = cx231xx_find_mailbox(dev);
 		if (retval < 0) {
-			printk(KERN_ERR "%s() mailbox < 0, error\n",
+			pr_err("%s() mailbox < 0, error\n",
 				__func__);
 			return -1;
 		}
 		dev->cx23417_mailbox = retval;
 		retval = cx231xx_api_cmd(dev, CX2341X_ENC_PING_FW, 0, 0);
 		if (retval < 0) {
-			printk(KERN_ERR
-				"ERROR: cx23417 firmware ping failed!\n");
+			pr_err("ERROR: cx23417 firmware ping failed!\n");
 			return -1;
 		}
 		retval = cx231xx_api_cmd(dev, CX2341X_ENC_GET_VERSION, 0, 1,
 			&version);
 		if (retval < 0) {
-			printk(KERN_ERR "ERROR: cx23417 firmware get encoder :"
-				"version failed!\n");
+			pr_err("ERROR: cx23417 firmware get encoder: version failed!\n");
 			return -1;
 		}
 		dprintk(1, "cx23417 firmware version is 0x%08x\n", version);
@@ -1134,7 +1144,7 @@ static int cx231xx_initialize_codec(struct cx231xx *dev)
 
 	for (i = 0; i < 1; i++) {
 		retval = mc417_register_read(dev, 0x20f8, &val);
-		dprintk(3, "***before enable656() VIM Capture Lines =%d ***\n",
+		dprintk(3, "***before enable656() VIM Capture Lines = %d ***\n",
 				 val);
 		if (retval < 0)
 			return retval;
@@ -1202,7 +1212,7 @@ static int cx231xx_initialize_codec(struct cx231xx *dev)
 
 	for (i = 0; i < 1; i++) {
 		mc417_register_read(dev, 0x20f8, &val);
-	dprintk(3, "***VIM Capture Lines =%d ***\n", val);
+		dprintk(3, "***VIM Capture Lines =%d ***\n", val);
 	}
 
 	return 0;
@@ -1223,6 +1233,7 @@ static int bb_buf_setup(struct videobuf_queue *q,
 
 	return 0;
 }
+
 static void free_buffer(struct videobuf_queue *vq, struct cx231xx_buffer *buf)
 {
 	struct cx231xx_fh *fh = vq->priv_data;
@@ -1249,91 +1260,85 @@ static void free_buffer(struct videobuf_queue *vq, struct cx231xx_buffer *buf)
 static void buffer_copy(struct cx231xx *dev, char *data, int len, struct urb *urb,
 		struct cx231xx_dmaqueue *dma_q)
 {
-		void *vbuf;
-		struct cx231xx_buffer *buf;
-		u32 tail_data = 0;
-		char *p_data;
-
-		if (dma_q->mpeg_buffer_done == 0) {
-			if (list_empty(&dma_q->active))
-				return;
-
-			buf = list_entry(dma_q->active.next,
-					struct cx231xx_buffer, vb.queue);
-			dev->video_mode.isoc_ctl.buf = buf;
-			dma_q->mpeg_buffer_done = 1;
-		}
-		/* Fill buffer */
-		buf = dev->video_mode.isoc_ctl.buf;
-		vbuf = videobuf_to_vmalloc(&buf->vb);
-
-		if ((dma_q->mpeg_buffer_completed+len) <
-		   mpeglines*mpeglinesize) {
-			if (dma_q->add_ps_package_head ==
-			   CX231XX_NEED_ADD_PS_PACKAGE_HEAD) {
-				memcpy(vbuf+dma_q->mpeg_buffer_completed,
-				       dma_q->ps_head, 3);
-				dma_q->mpeg_buffer_completed =
-				  dma_q->mpeg_buffer_completed + 3;
-				dma_q->add_ps_package_head =
-				  CX231XX_NONEED_PS_PACKAGE_HEAD;
-			}
-			memcpy(vbuf+dma_q->mpeg_buffer_completed, data, len);
-			dma_q->mpeg_buffer_completed =
-			  dma_q->mpeg_buffer_completed + len;
-		} else {
-			dma_q->mpeg_buffer_done = 0;
-
-			tail_data =
-			  mpeglines*mpeglinesize - dma_q->mpeg_buffer_completed;
-			memcpy(vbuf+dma_q->mpeg_buffer_completed,
-			       data, tail_data);
-
-			buf->vb.state = VIDEOBUF_DONE;
-			buf->vb.field_count++;
-			v4l2_get_timestamp(&buf->vb.ts);
-			list_del(&buf->vb.queue);
-			wake_up(&buf->vb.done);
-			dma_q->mpeg_buffer_completed = 0;
-
-			if (len - tail_data > 0) {
-				p_data = data + tail_data;
-				dma_q->left_data_count = len - tail_data;
-				memcpy(dma_q->p_left_data,
-				       p_data, len - tail_data);
-			}
-
-		}
-
-	    return;
-}
-
-static void buffer_filled(char *data, int len, struct urb *urb,
-		struct cx231xx_dmaqueue *dma_q)
-{
-		void *vbuf;
-		struct cx231xx_buffer *buf;
+	void *vbuf;
+	struct cx231xx_buffer *buf;
+	u32 tail_data = 0;
+	char *p_data;
 
+	if (dma_q->mpeg_buffer_done == 0) {
 		if (list_empty(&dma_q->active))
 			return;
 
-
 		buf = list_entry(dma_q->active.next,
-				 struct cx231xx_buffer, vb.queue);
+				struct cx231xx_buffer, vb.queue);
+		dev->video_mode.isoc_ctl.buf = buf;
+		dma_q->mpeg_buffer_done = 1;
+	}
+	/* Fill buffer */
+	buf = dev->video_mode.isoc_ctl.buf;
+	vbuf = videobuf_to_vmalloc(&buf->vb);
+
+	if ((dma_q->mpeg_buffer_completed+len) <
+			mpeglines*mpeglinesize) {
+		if (dma_q->add_ps_package_head ==
+				CX231XX_NEED_ADD_PS_PACKAGE_HEAD) {
+			memcpy(vbuf+dma_q->mpeg_buffer_completed,
+					dma_q->ps_head, 3);
+			dma_q->mpeg_buffer_completed =
+				dma_q->mpeg_buffer_completed + 3;
+			dma_q->add_ps_package_head =
+				CX231XX_NONEED_PS_PACKAGE_HEAD;
+		}
+		memcpy(vbuf+dma_q->mpeg_buffer_completed, data, len);
+		dma_q->mpeg_buffer_completed =
+			dma_q->mpeg_buffer_completed + len;
+	} else {
+		dma_q->mpeg_buffer_done = 0;
 
+		tail_data =
+			mpeglines*mpeglinesize - dma_q->mpeg_buffer_completed;
+		memcpy(vbuf+dma_q->mpeg_buffer_completed,
+				data, tail_data);
 
-		/* Fill buffer */
-		vbuf = videobuf_to_vmalloc(&buf->vb);
-		memcpy(vbuf, data, len);
 		buf->vb.state = VIDEOBUF_DONE;
 		buf->vb.field_count++;
 		v4l2_get_timestamp(&buf->vb.ts);
 		list_del(&buf->vb.queue);
 		wake_up(&buf->vb.done);
+		dma_q->mpeg_buffer_completed = 0;
 
-	    return;
+		if (len - tail_data > 0) {
+			p_data = data + tail_data;
+			dma_q->left_data_count = len - tail_data;
+			memcpy(dma_q->p_left_data,
+					p_data, len - tail_data);
+		}
+	}
 }
-static inline int cx231xx_isoc_copy(struct cx231xx *dev, struct urb *urb)
+
+static void buffer_filled(char *data, int len, struct urb *urb,
+		struct cx231xx_dmaqueue *dma_q)
+{
+	void *vbuf;
+	struct cx231xx_buffer *buf;
+
+	if (list_empty(&dma_q->active))
+		return;
+
+	buf = list_entry(dma_q->active.next,
+			struct cx231xx_buffer, vb.queue);
+
+	/* Fill buffer */
+	vbuf = videobuf_to_vmalloc(&buf->vb);
+	memcpy(vbuf, data, len);
+	buf->vb.state = VIDEOBUF_DONE;
+	buf->vb.field_count++;
+	v4l2_get_timestamp(&buf->vb.ts);
+	list_del(&buf->vb.queue);
+	wake_up(&buf->vb.done);
+}
+
+static int cx231xx_isoc_copy(struct cx231xx *dev, struct urb *urb)
 {
 	struct cx231xx_dmaqueue *dma_q = urb->context;
 	unsigned char *p_buffer;
@@ -1358,11 +1363,9 @@ static inline int cx231xx_isoc_copy(struct cx231xx *dev, struct urb *urb)
 
 	return 0;
 }
-static inline int cx231xx_bulk_copy(struct cx231xx *dev, struct urb *urb)
-{
 
-	/*char *outp;*/
-	/*struct cx231xx_buffer *buf;*/
+static int cx231xx_bulk_copy(struct cx231xx *dev, struct urb *urb)
+{
 	struct cx231xx_dmaqueue *dma_q = urb->context;
 	unsigned char *p_buffer, *buffer;
 	u32 buffer_size = 0;
@@ -1393,8 +1396,6 @@ static int bb_buf_prepare(struct videobuf_queue *q,
 	int rc = 0, urb_init = 0;
 	int size = fh->dev->ts1.ts_packet_size * fh->dev->ts1.ts_packet_count;
 
-	dma_qq = &dev->video_mode.vidq;
-
 	if (0 != buf->vb.baddr  &&  buf->vb.bsize < size)
 		return -EINVAL;
 	buf->vb.width = fh->dev->ts1.ts_packet_size;
@@ -1482,36 +1483,6 @@ static struct videobuf_queue_ops cx231xx_qops = {
 
 /* ------------------------------------------------------------------ */
 
-static const u32 *ctrl_classes[] = {
-	cx2341x_mpeg_ctrls,
-	NULL
-};
-
-static int cx231xx_queryctrl(struct cx231xx *dev,
-	struct v4l2_queryctrl *qctrl)
-{
-	qctrl->id = v4l2_ctrl_next(ctrl_classes, qctrl->id);
-	if (qctrl->id == 0)
-		return -EINVAL;
-
-	/* MPEG V4L2 controls */
-	if (cx2341x_ctrl_query(&dev->mpeg_params, qctrl))
-		qctrl->flags |= V4L2_CTRL_FLAG_DISABLED;
-
-	return 0;
-}
-
-static int cx231xx_querymenu(struct cx231xx *dev,
-	struct v4l2_querymenu *qmenu)
-{
-	struct v4l2_queryctrl qctrl;
-
-	qctrl.id = qmenu->id;
-	cx231xx_queryctrl(dev, &qctrl);
-	return v4l2_ctrl_query_menu(qmenu, &qctrl,
-		cx2341x_ctrl_get_menu(&dev->mpeg_params, qmenu->id));
-}
-
 static int vidioc_g_std(struct file *file, void *fh0, v4l2_std_id *norm)
 {
 	struct cx231xx_fh  *fh  = file->private_data;
@@ -1520,14 +1491,15 @@ static int vidioc_g_std(struct file *file, void *fh0, v4l2_std_id *norm)
 	*norm = dev->encodernorm.id;
 	return 0;
 }
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
+
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct cx231xx_fh  *fh  = file->private_data;
 	struct cx231xx *dev = fh->dev;
 	unsigned int i;
 
 	for (i = 0; i < ARRAY_SIZE(cx231xx_tvnorms); i++)
-		if (*id & cx231xx_tvnorms[i].id)
+		if (id & cx231xx_tvnorms[i].id)
 			break;
 	if (i == ARRAY_SIZE(cx231xx_tvnorms))
 		return -EINVAL;
@@ -1537,12 +1509,12 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
 		dprintk(3, "encodernorm set to NTSC\n");
 		dev->norm = V4L2_STD_NTSC;
 		dev->ts1.height = 480;
-		dev->mpeg_params.is_50hz = 0;
+		cx2341x_handler_set_50hz(&dev->mpeg_ctrl_handler, false);
 	} else {
 		dprintk(3, "encodernorm set to PAL\n");
 		dev->norm = V4L2_STD_PAL_B;
 		dev->ts1.height = 576;
-		dev->mpeg_params.is_50hz = 1;
+		cx2341x_handler_set_50hz(&dev->mpeg_ctrl_handler, true);
 	}
 	call_all(dev, core, s_std, dev->norm);
 	/* do mode control overrides */
@@ -1551,161 +1523,23 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
 	dprintk(3, "exit vidioc_s_std() i=0x%x\n", i);
 	return 0;
 }
-static int vidioc_g_audio(struct file *file, void *fh,
-					struct v4l2_audio *a)
-{
-		struct v4l2_audio *vin = a;
-
-		int ret = -EINVAL;
-		if (vin->index > 0)
-			return ret;
-		strncpy(vin->name, "VideoGrabber Audio", 14);
-		vin->capability = V4L2_AUDCAP_STEREO;
-return 0;
-}
-static int vidioc_enumaudio(struct file *file, void *fh,
-					struct v4l2_audio *a)
-{
-		struct v4l2_audio *vin = a;
-
-		int ret = -EINVAL;
-
-		if (vin->index > 0)
-			return ret;
-		strncpy(vin->name, "VideoGrabber Audio", 14);
-		vin->capability = V4L2_AUDCAP_STEREO;
-
-
-return 0;
-}
-static const char *iname[] = {
-	[CX231XX_VMUX_COMPOSITE1] = "Composite1",
-	[CX231XX_VMUX_SVIDEO]     = "S-Video",
-	[CX231XX_VMUX_TELEVISION] = "Television",
-	[CX231XX_VMUX_CABLE]      = "Cable TV",
-	[CX231XX_VMUX_DVB]        = "DVB",
-	[CX231XX_VMUX_DEBUG]      = "for debug only",
-};
-static int vidioc_enum_input(struct file *file, void *priv,
-				struct v4l2_input *i)
-{
-	struct cx231xx_fh  *fh  = file->private_data;
-	struct cx231xx *dev = fh->dev;
-	struct cx231xx_input *input;
-	int n;
-	dprintk(3, "enter vidioc_enum_input()i->index=%d\n", i->index);
-
-	if (i->index >= 4)
-		return -EINVAL;
-
-
-	input = &cx231xx_boards[dev->model].input[i->index];
-
-	if (input->type == 0)
-		return -EINVAL;
-
-	/* FIXME
-	 * strcpy(i->name, input->name); */
-
-	n = i->index;
-	strcpy(i->name, iname[INPUT(n)->type]);
-
-	if (input->type == CX231XX_VMUX_TELEVISION ||
-	    input->type == CX231XX_VMUX_CABLE)
-		i->type = V4L2_INPUT_TYPE_TUNER;
-	else
-		i->type  = V4L2_INPUT_TYPE_CAMERA;
-
-
-	return 0;
-}
-
-static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
-{
-	*i = 0;
-	return  0;
-}
-
-static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
-{
-	struct cx231xx_fh  *fh  = file->private_data;
-	struct cx231xx *dev = fh->dev;
-
-	dprintk(3, "enter vidioc_s_input() i=%d\n", i);
-
-	mutex_lock(&dev->lock);
-
-	video_mux(dev, i);
-
-	mutex_unlock(&dev->lock);
-
-	if (i >= 4)
-		return -EINVAL;
-	dev->input = i;
-	dprintk(3, "exit vidioc_s_input()\n");
-	return 0;
-}
-
-static int vidioc_g_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
-{
-	return 0;
-}
-
-static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
-{
-	return 0;
-}
-
-static int vidioc_g_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
-{
-	return 0;
-}
-
-static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
-{
-
-
-	return 0;
-}
 
 static int vidioc_s_ctrl(struct file *file, void *priv,
 				struct v4l2_control *ctl)
 {
 	struct cx231xx_fh  *fh  = file->private_data;
 	struct cx231xx *dev = fh->dev;
+
 	dprintk(3, "enter vidioc_s_ctrl()\n");
 	/* Update the A/V core */
 	call_all(dev, core, s_ctrl, ctl);
 	dprintk(3, "exit vidioc_s_ctrl()\n");
 	return 0;
 }
-static struct v4l2_capability pvr_capability = {
-	.driver         = "cx231xx",
-	.card           = "VideoGrabber",
-	.bus_info       = "usb",
-	.version        = 1,
-	.capabilities   = (V4L2_CAP_VIDEO_CAPTURE |
-			   V4L2_CAP_TUNER | V4L2_CAP_AUDIO | V4L2_CAP_RADIO |
-			 V4L2_CAP_STREAMING | V4L2_CAP_READWRITE),
-};
-static int vidioc_querycap(struct file *file, void  *priv,
-				struct v4l2_capability *cap)
-{
-
-
-
-		memcpy(cap, &pvr_capability, sizeof(struct v4l2_capability));
-	return 0;
-}
 
 static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
 					struct v4l2_fmtdesc *f)
 {
-
 	if (f->index != 0)
 		return -EINVAL;
 
@@ -1720,17 +1554,18 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 {
 	struct cx231xx_fh  *fh  = file->private_data;
 	struct cx231xx *dev = fh->dev;
+
 	dprintk(3, "enter vidioc_g_fmt_vid_cap()\n");
-	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
+	f->fmt.pix.pixelformat = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.bytesperline = 0;
-	f->fmt.pix.sizeimage    =
-		dev->ts1.ts_packet_size * dev->ts1.ts_packet_count;
-	f->fmt.pix.colorspace   = 0;
-	f->fmt.pix.width        = dev->ts1.width;
-	f->fmt.pix.height       = dev->ts1.height;
-	f->fmt.pix.field        = fh->vidq.field;
-	dprintk(1, "VIDIOC_G_FMT: w: %d, h: %d, f: %d\n",
-		dev->ts1.width, dev->ts1.height, fh->vidq.field);
+	f->fmt.pix.sizeimage = mpeglines * mpeglinesize;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.width = dev->ts1.width;
+	f->fmt.pix.height = dev->ts1.height;
+	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	f->fmt.pix.priv = 0;
+	dprintk(1, "VIDIOC_G_FMT: w: %d, h: %d\n",
+		dev->ts1.width, dev->ts1.height);
 	dprintk(3, "exit vidioc_g_fmt_vid_cap()\n");
 	return 0;
 }
@@ -1740,25 +1575,20 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 {
 	struct cx231xx_fh  *fh  = file->private_data;
 	struct cx231xx *dev = fh->dev;
+
 	dprintk(3, "enter vidioc_try_fmt_vid_cap()\n");
-	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
+	f->fmt.pix.pixelformat = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.bytesperline = 0;
-	f->fmt.pix.sizeimage    =
-		dev->ts1.ts_packet_size * dev->ts1.ts_packet_count;
-	f->fmt.pix.colorspace   = 0;
-	dprintk(1, "VIDIOC_TRY_FMT: w: %d, h: %d, f: %d\n",
-		dev->ts1.width, dev->ts1.height, fh->vidq.field);
+	f->fmt.pix.sizeimage = mpeglines * mpeglinesize;
+	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
+	dprintk(1, "VIDIOC_TRY_FMT: w: %d, h: %d\n",
+		dev->ts1.width, dev->ts1.height);
 	dprintk(3, "exit vidioc_try_fmt_vid_cap()\n");
 	return 0;
 }
 
-static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
-				struct v4l2_format *f)
-{
-
-	return 0;
-}
-
 static int vidioc_reqbufs(struct file *file, void *priv,
 				struct v4l2_requestbuffers *p)
 {
@@ -1795,22 +1625,22 @@ static int vidioc_streamon(struct file *file, void *priv,
 				enum v4l2_buf_type i)
 {
 	struct cx231xx_fh  *fh  = file->private_data;
-
 	struct cx231xx *dev = fh->dev;
+
 	dprintk(3, "enter vidioc_streamon()\n");
-		cx231xx_set_alt_setting(dev, INDEX_TS1, 0);
-		cx231xx_set_mode(dev, CX231XX_DIGITAL_MODE);
-		if (dev->USE_ISO)
-			cx231xx_init_isoc(dev, CX231XX_NUM_PACKETS,
-				       CX231XX_NUM_BUFS,
-				       dev->video_mode.max_pkt_size,
-				       cx231xx_isoc_copy);
-		else {
-			cx231xx_init_bulk(dev, 320,
-				       5,
-				       dev->ts1_mode.max_pkt_size,
-				       cx231xx_bulk_copy);
-		}
+	cx231xx_set_alt_setting(dev, INDEX_TS1, 0);
+	cx231xx_set_mode(dev, CX231XX_DIGITAL_MODE);
+	if (dev->USE_ISO)
+		cx231xx_init_isoc(dev, CX231XX_NUM_PACKETS,
+				CX231XX_NUM_BUFS,
+				dev->video_mode.max_pkt_size,
+				cx231xx_isoc_copy);
+	else {
+		cx231xx_init_bulk(dev, 320,
+				5,
+				dev->ts1_mode.max_pkt_size,
+				cx231xx_bulk_copy);
+	}
 	dprintk(3, "exit vidioc_streamon()\n");
 	return videobuf_streamon(&fh->vidq);
 }
@@ -1822,117 +1652,25 @@ static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
 	return videobuf_streamoff(&fh->vidq);
 }
 
-static int vidioc_g_ext_ctrls(struct file *file, void *priv,
-				struct v4l2_ext_controls *f)
-{
-	struct cx231xx_fh  *fh  = priv;
-	struct cx231xx *dev = fh->dev;
-	dprintk(3, "enter vidioc_g_ext_ctrls()\n");
-	if (f->ctrl_class != V4L2_CTRL_CLASS_MPEG)
-		return -EINVAL;
-	dprintk(3, "exit vidioc_g_ext_ctrls()\n");
-	return cx2341x_ext_ctrls(&dev->mpeg_params, 0, f, VIDIOC_G_EXT_CTRLS);
-}
-
-static int vidioc_s_ext_ctrls(struct file *file, void *priv,
-				struct v4l2_ext_controls *f)
-{
-	struct cx231xx_fh  *fh  = priv;
-	struct cx231xx *dev = fh->dev;
-	struct cx2341x_mpeg_params p;
-	int err;
-	dprintk(3, "enter vidioc_s_ext_ctrls()\n");
-	if (f->ctrl_class != V4L2_CTRL_CLASS_MPEG)
-		return -EINVAL;
-
-	p = dev->mpeg_params;
-	err = cx2341x_ext_ctrls(&p, 0, f, VIDIOC_TRY_EXT_CTRLS);
-	if (err == 0) {
-		err = cx2341x_update(dev, cx231xx_mbox_func,
-			&dev->mpeg_params, &p);
-		dev->mpeg_params = p;
-	}
-
-	return err;
-
-
-return 0;
-}
-
-static int vidioc_try_ext_ctrls(struct file *file, void *priv,
-				struct v4l2_ext_controls *f)
-{
-	struct cx231xx_fh  *fh  = priv;
-	struct cx231xx *dev = fh->dev;
-	struct cx2341x_mpeg_params p;
-	int err;
-	dprintk(3, "enter vidioc_try_ext_ctrls()\n");
-	if (f->ctrl_class != V4L2_CTRL_CLASS_MPEG)
-		return -EINVAL;
-
-	p = dev->mpeg_params;
-	err = cx2341x_ext_ctrls(&p, 0, f, VIDIOC_TRY_EXT_CTRLS);
-	dprintk(3, "exit vidioc_try_ext_ctrls() err=%d\n", err);
-	return err;
-}
-
 static int vidioc_log_status(struct file *file, void *priv)
 {
 	struct cx231xx_fh  *fh  = priv;
 	struct cx231xx *dev = fh->dev;
-	char name[32 + 2];
 
-	snprintf(name, sizeof(name), "%s/2", dev->name);
-	dprintk(3,
-		"%s/2: ============  START LOG STATUS  ============\n",
-	       dev->name);
 	call_all(dev, core, log_status);
-	cx2341x_log_status(&dev->mpeg_params, name);
-	dprintk(3,
-		"%s/2: =============  END LOG STATUS  =============\n",
-	       dev->name);
-	return 0;
-}
-
-static int vidioc_querymenu(struct file *file, void *priv,
-				struct v4l2_querymenu *a)
-{
-	struct cx231xx_fh  *fh  = priv;
-	struct cx231xx *dev = fh->dev;
-	dprintk(3, "enter vidioc_querymenu()\n");
-	dprintk(3, "exit vidioc_querymenu()\n");
-	return cx231xx_querymenu(dev, a);
-}
-
-static int vidioc_queryctrl(struct file *file, void *priv,
-				struct v4l2_queryctrl *c)
-{
-	struct cx231xx_fh  *fh  = priv;
-	struct cx231xx *dev = fh->dev;
-	dprintk(3, "enter vidioc_queryctrl()\n");
-	dprintk(3, "exit vidioc_queryctrl()\n");
-	return cx231xx_queryctrl(dev, c);
+	return v4l2_ctrl_log_status(file, priv);
 }
 
 static int mpeg_open(struct file *file)
 {
-	int minor = video_devdata(file)->minor;
-	struct cx231xx *h, *dev = NULL;
-	/*struct list_head *list;*/
+	struct video_device *vdev = video_devdata(file);
+	struct cx231xx *dev = video_drvdata(file);
 	struct cx231xx_fh *fh;
-	/*u32 value = 0;*/
 
 	dprintk(2, "%s()\n", __func__);
 
-	list_for_each_entry(h, &cx231xx_devlist, devlist) {
-		if (h->v4l_device->minor == minor)
-			dev = h;
-	}
-
-	if (dev == NULL)
-		return -ENODEV;
-
-	mutex_lock(&dev->lock);
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
 
 	/* allocate + initialize per filehandle data */
 	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
@@ -1942,29 +1680,30 @@ static int mpeg_open(struct file *file)
 	}
 
 	file->private_data = fh;
-	fh->dev      = dev;
+	v4l2_fh_init(&fh->fh, vdev);
+	fh->dev = dev;
 
 
 	videobuf_queue_vmalloc_init(&fh->vidq, &cx231xx_qops,
 			    NULL, &dev->video_mode.slock,
 			    V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_INTERLACED,
-			    sizeof(struct cx231xx_buffer), fh, NULL);
+			    sizeof(struct cx231xx_buffer), fh, &dev->lock);
 /*
 	videobuf_queue_sg_init(&fh->vidq, &cx231xx_qops,
 			    &dev->udev->dev, &dev->ts1.slock,
 			    V4L2_BUF_TYPE_VIDEO_CAPTURE,
 			    V4L2_FIELD_INTERLACED,
 			    sizeof(struct cx231xx_buffer),
-			    fh, NULL);
+			    fh, &dev->lock);
 */
 
-
 	cx231xx_set_alt_setting(dev, INDEX_VANC, 1);
 	cx231xx_set_gpio_value(dev, 2, 0);
 
 	cx231xx_initialize_codec(dev);
 
 	mutex_unlock(&dev->lock);
+	v4l2_fh_add(&fh->fh);
 	cx231xx_start_TS1(dev);
 
 	return 0;
@@ -1977,25 +1716,20 @@ static int mpeg_release(struct file *file)
 
 	dprintk(3, "mpeg_release()! dev=0x%p\n", dev);
 
-	if (!dev) {
-		dprintk(3, "abort!!!\n");
-		return 0;
-	}
-
 	mutex_lock(&dev->lock);
 
 	cx231xx_stop_TS1(dev);
 
-		/* do this before setting alternate! */
-		if (dev->USE_ISO)
-			cx231xx_uninit_isoc(dev);
-		else
-			cx231xx_uninit_bulk(dev);
-		cx231xx_set_mode(dev, CX231XX_SUSPEND);
+	/* do this before setting alternate! */
+	if (dev->USE_ISO)
+		cx231xx_uninit_isoc(dev);
+	else
+		cx231xx_uninit_bulk(dev);
+	cx231xx_set_mode(dev, CX231XX_SUSPEND);
 
-		cx231xx_api_cmd(fh->dev, CX2341X_ENC_STOP_CAPTURE, 3, 0,
-				CX231xx_END_NOW, CX231xx_MPEG_CAPTURE,
-				CX231xx_RAW_BITS_NONE);
+	cx231xx_api_cmd(fh->dev, CX2341X_ENC_STOP_CAPTURE, 3, 0,
+			CX231xx_END_NOW, CX231xx_MPEG_CAPTURE,
+			CX231xx_RAW_BITS_NONE);
 
 	/* FIXME: Review this crap */
 	/* Shut device down on last close */
@@ -2015,7 +1749,8 @@ static int mpeg_release(struct file *file)
 		videobuf_read_stop(&fh->vidq);
 
 	videobuf_mmap_free(&fh->vidq);
-	file->private_data = NULL;
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
 	kfree(fh);
 	mutex_unlock(&dev->lock);
 	return 0;
@@ -2027,7 +1762,6 @@ static ssize_t mpeg_read(struct file *file, char __user *data,
 	struct cx231xx_fh *fh = file->private_data;
 	struct cx231xx *dev = fh->dev;
 
-
 	/* Deal w/ A/V decoder * and mpeg encoder sync issues. */
 	/* Start mpeg encoder on first read. */
 	if (atomic_cmpxchg(&fh->v4l_reading, 0, 1) == 0) {
@@ -2044,12 +1778,23 @@ static ssize_t mpeg_read(struct file *file, char __user *data,
 static unsigned int mpeg_poll(struct file *file,
 	struct poll_table_struct *wait)
 {
+	unsigned long req_events = poll_requested_events(wait);
 	struct cx231xx_fh *fh = file->private_data;
-	/*struct cx231xx *dev = fh->dev;*/
+	struct cx231xx *dev = fh->dev;
+	unsigned int res = 0;
+
+	if (v4l2_event_pending(&fh->fh))
+		res |= POLLPRI;
+	else
+		poll_wait(file, &fh->fh.wait, wait);
 
-	/*dprintk(2, "%s\n", __func__);*/
+	if (!(req_events & (POLLIN | POLLRDNORM)))
+		return res;
 
-	return videobuf_poll_stream(file, &fh->vidq, wait);
+	mutex_lock(&dev->lock);
+	res |= videobuf_poll_stream(file, &fh->vidq, wait);
+	mutex_unlock(&dev->lock);
+	return res;
 }
 
 static int mpeg_mmap(struct file *file, struct vm_area_struct *vma)
@@ -2069,44 +1814,39 @@ static struct v4l2_file_operations mpeg_fops = {
 	.read	       = mpeg_read,
 	.poll          = mpeg_poll,
 	.mmap	       = mpeg_mmap,
-	.ioctl	       = video_ioctl2,
+	.unlocked_ioctl = video_ioctl2,
 };
 
 static const struct v4l2_ioctl_ops mpeg_ioctl_ops = {
 	.vidioc_s_std		 = vidioc_s_std,
 	.vidioc_g_std		 = vidioc_g_std,
-	.vidioc_enum_input	 = vidioc_enum_input,
-	.vidioc_enumaudio	 = vidioc_enumaudio,
-	.vidioc_g_audio		 = vidioc_g_audio,
-	.vidioc_g_input		 = vidioc_g_input,
-	.vidioc_s_input		 = vidioc_s_input,
-	.vidioc_g_tuner		 = vidioc_g_tuner,
-	.vidioc_s_tuner		 = vidioc_s_tuner,
-	.vidioc_g_frequency	 = vidioc_g_frequency,
-	.vidioc_s_frequency	 = vidioc_s_frequency,
+	.vidioc_g_tuner          = cx231xx_g_tuner,
+	.vidioc_s_tuner          = cx231xx_s_tuner,
+	.vidioc_g_frequency      = cx231xx_g_frequency,
+	.vidioc_s_frequency      = cx231xx_s_frequency,
+	.vidioc_enum_input	 = cx231xx_enum_input,
+	.vidioc_g_input		 = cx231xx_g_input,
+	.vidioc_s_input		 = cx231xx_s_input,
 	.vidioc_s_ctrl		 = vidioc_s_ctrl,
-	.vidioc_querycap	 = vidioc_querycap,
+	.vidioc_querycap	 = cx231xx_querycap,
 	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap	 = vidioc_g_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap	 = vidioc_try_fmt_vid_cap,
-	.vidioc_s_fmt_vid_cap	 = vidioc_s_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	 = vidioc_try_fmt_vid_cap,
 	.vidioc_reqbufs		 = vidioc_reqbufs,
 	.vidioc_querybuf	 = vidioc_querybuf,
 	.vidioc_qbuf		 = vidioc_qbuf,
 	.vidioc_dqbuf		 = vidioc_dqbuf,
 	.vidioc_streamon	 = vidioc_streamon,
 	.vidioc_streamoff	 = vidioc_streamoff,
-	.vidioc_g_ext_ctrls	 = vidioc_g_ext_ctrls,
-	.vidioc_s_ext_ctrls	 = vidioc_s_ext_ctrls,
-	.vidioc_try_ext_ctrls	 = vidioc_try_ext_ctrls,
 	.vidioc_log_status	 = vidioc_log_status,
-	.vidioc_querymenu	 = vidioc_querymenu,
-	.vidioc_queryctrl	 = vidioc_queryctrl,
-/*	.vidioc_g_chip_ident	 = cx231xx_g_chip_ident,*/
+	.vidioc_g_chip_ident	 = cx231xx_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-/*	.vidioc_g_register	 = cx231xx_g_register,*/
-/*	.vidioc_s_register	 = cx231xx_s_register,*/
+	.vidioc_g_register	 = cx231xx_g_register,
+	.vidioc_s_register	 = cx231xx_s_register,
 #endif
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static struct video_device cx231xx_mpeg_template = {
@@ -2114,8 +1854,7 @@ static struct video_device cx231xx_mpeg_template = {
 	.fops          = &mpeg_fops,
 	.ioctl_ops     = &mpeg_ioctl_ops,
 	.minor         = -1,
-	.tvnorms       = CX231xx_NORMS,
-	.current_norm  = V4L2_STD_NTSC_M,
+	.tvnorms       = V4L2_STD_ALL,
 };
 
 void cx231xx_417_unregister(struct cx231xx *dev)
@@ -2128,10 +1867,44 @@ void cx231xx_417_unregister(struct cx231xx *dev)
 			video_unregister_device(dev->v4l_device);
 		else
 			video_device_release(dev->v4l_device);
+		v4l2_ctrl_handler_free(&dev->mpeg_ctrl_handler.hdl);
 		dev->v4l_device = NULL;
 	}
 }
 
+static int cx231xx_s_video_encoding(struct cx2341x_handler *cxhdl, u32 val)
+{
+	struct cx231xx *dev = container_of(cxhdl, struct cx231xx, mpeg_ctrl_handler);
+	int is_mpeg1 = val == V4L2_MPEG_VIDEO_ENCODING_MPEG_1;
+	struct v4l2_mbus_framefmt fmt;
+
+	/* fix videodecoder resolution */
+	fmt.width = cxhdl->width / (is_mpeg1 ? 2 : 1);
+	fmt.height = cxhdl->height;
+	fmt.code = V4L2_MBUS_FMT_FIXED;
+	v4l2_subdev_call(dev->sd_cx25840, video, s_mbus_fmt, &fmt);
+	return 0;
+}
+
+static int cx231xx_s_audio_sampling_freq(struct cx2341x_handler *cxhdl, u32 idx)
+{
+	static const u32 freqs[3] = { 44100, 48000, 32000 };
+	struct cx231xx *dev = container_of(cxhdl, struct cx231xx, mpeg_ctrl_handler);
+
+	/* The audio clock of the digitizer must match the codec sample
+	   rate otherwise you get some very strange effects. */
+	if (idx < ARRAY_SIZE(freqs))
+		call_all(dev, audio, s_clock_freq, freqs[idx]);
+	return 0;
+}
+
+static struct cx2341x_handler_ops cx231xx_ops = {
+	/* needed for the video clock freq */
+	.s_audio_sampling_freq = cx231xx_s_audio_sampling_freq,
+	/* needed for setting up the video resolution */
+	.s_video_encoding = cx231xx_s_video_encoding,
+};
+
 static struct video_device *cx231xx_video_dev_alloc(
 	struct cx231xx *dev,
 	struct usb_device *usbdev,
@@ -2145,12 +1918,21 @@ static struct video_device *cx231xx_video_dev_alloc(
 	if (NULL == vfd)
 		return NULL;
 	*vfd = *template;
-	vfd->minor = -1;
 	snprintf(vfd->name, sizeof(vfd->name), "%s %s (%s)", dev->name,
 		type, cx231xx_boards[dev->model].name);
 
 	vfd->v4l2_dev = &dev->v4l2_dev;
+	vfd->lock = &dev->lock;
 	vfd->release = video_device_release;
+	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
+	vfd->ctrl_handler = &dev->mpeg_ctrl_handler.hdl;
+	video_set_drvdata(vfd, dev);
+	if (dev->tuner_type == TUNER_ABSENT) {
+		v4l2_disable_ioctl(vfd, VIDIOC_G_FREQUENCY);
+		v4l2_disable_ioctl(vfd, VIDIOC_S_FREQUENCY);
+		v4l2_disable_ioctl(vfd, VIDIOC_G_TUNER);
+		v4l2_disable_ioctl(vfd, VIDIOC_S_TUNER);
+	}
 
 	return vfd;
 
@@ -2173,10 +1955,27 @@ int cx231xx_417_register(struct cx231xx *dev)
 		tsport->height = 576;
 
 	tsport->width = 720;
-	cx2341x_fill_defaults(&dev->mpeg_params);
+	err = cx2341x_handler_init(&dev->mpeg_ctrl_handler, 50);
+	if (err) {
+		dprintk(3, "%s: can't init cx2341x controls\n", dev->name);
+		return err;
+	}
+	dev->mpeg_ctrl_handler.func = cx231xx_mbox_func;
+	dev->mpeg_ctrl_handler.priv = dev;
+	dev->mpeg_ctrl_handler.ops = &cx231xx_ops;
+	if (dev->sd_cx25840)
+		v4l2_ctrl_add_handler(&dev->mpeg_ctrl_handler.hdl,
+				dev->sd_cx25840->ctrl_handler, NULL);
+	if (dev->mpeg_ctrl_handler.hdl.error) {
+		err = dev->mpeg_ctrl_handler.hdl.error;
+		dprintk(3, "%s: can't add cx25840 controls\n", dev->name);
+		v4l2_ctrl_handler_free(&dev->mpeg_ctrl_handler.hdl);
+		return err;
+	}
 	dev->norm = V4L2_STD_NTSC;
 
-	dev->mpeg_params.port = CX2341X_PORT_SERIAL;
+	dev->mpeg_ctrl_handler.port = CX2341X_PORT_SERIAL;
+	cx2341x_handler_set_50hz(&dev->mpeg_ctrl_handler, false);
 
 	/* Allocate and initialize V4L video device */
 	dev->v4l_device = cx231xx_video_dev_alloc(dev,
@@ -2185,6 +1984,7 @@ int cx231xx_417_register(struct cx231xx *dev)
 		VFL_TYPE_GRABBER, -1);
 	if (err < 0) {
 		dprintk(3, "%s: can't register mpeg device\n", dev->name);
+		v4l2_ctrl_handler_free(&dev->mpeg_ctrl_handler.hdl);
 		return err;
 	}
 
diff --git a/drivers/media/usb/cx231xx/cx231xx-audio.c b/drivers/media/usb/cx231xx/cx231xx-audio.c
index b4c99c7270cf..81a1d971d797 100644
--- a/drivers/media/usb/cx231xx/cx231xx-audio.c
+++ b/drivers/media/usb/cx231xx/cx231xx-audio.c
@@ -449,9 +449,6 @@ static int snd_cx231xx_capture_open(struct snd_pcm_substream *substream)
 		return -ENODEV;
 	}
 
-	/* Sets volume, mute, etc */
-	dev->mute = 0;
-
 	/* set alternate setting for audio interface */
 	/* 1 - 48000 samples per sec */
 	mutex_lock(&dev->lock);
@@ -503,7 +500,6 @@ static int snd_cx231xx_pcm_close(struct snd_pcm_substream *substream)
 		return ret;
 	}
 
-	dev->mute = 1;
 	dev->adev.users--;
 	mutex_unlock(&dev->lock);
 
@@ -708,8 +704,8 @@ static int cx231xx_audio_init(struct cx231xx *dev)
 					    audio_index + 1];
 
 	adev->end_point_addr =
-	    le16_to_cpu(uif->altsetting[0].endpoint[isoc_pipe].desc.
-			bEndpointAddress);
+	    uif->altsetting[0].endpoint[isoc_pipe].desc.
+			bEndpointAddress;
 
 	adev->num_alt = uif->num_altsetting;
 	cx231xx_info("EndPoint Addr 0x%x, Alternate settings: %i\n",
diff --git a/drivers/media/usb/cx231xx/cx231xx-avcore.c b/drivers/media/usb/cx231xx/cx231xx-avcore.c
index 722207913740..235ba657d52e 100644
--- a/drivers/media/usb/cx231xx/cx231xx-avcore.c
+++ b/drivers/media/usb/cx231xx/cx231xx-avcore.c
@@ -357,6 +357,7 @@ int cx231xx_afe_update_power_control(struct cx231xx *dev,
 	case CX231XX_BOARD_PV_PLAYTV_USB_HYBRID:
 	case CX231XX_BOARD_HAUPPAUGE_USB2_FM_PAL:
 	case CX231XX_BOARD_HAUPPAUGE_USB2_FM_NTSC:
+	case CX231XX_BOARD_OTG102:
 		if (avmode == POLARIS_AVMODE_ANALOGT_TV) {
 			while (afe_power_status != (FLD_PWRDN_TUNING_BIAS |
 						FLD_PWRDN_ENABLE_PLL)) {
@@ -1720,6 +1721,7 @@ int cx231xx_dif_set_standard(struct cx231xx *dev, u32 standard)
 	case CX231XX_BOARD_CNXT_RDU_250:
 	case CX231XX_BOARD_CNXT_VIDEO_GRABBER:
 	case CX231XX_BOARD_HAUPPAUGE_EXETER:
+	case CX231XX_BOARD_OTG102:
 		func_mode = 0x03;
 		break;
 	case CX231XX_BOARD_CNXT_RDE_253S:
@@ -2133,7 +2135,7 @@ int cx231xx_tuner_post_channel_change(struct cx231xx *dev)
 
 	status = vid_blk_write_word(dev, DIF_AGC_IF_REF, dwval);
 
-	return status;
+	return status == sizeof(dwval) ? 0 : -EIO;
 }
 
 /******************************************************************************
@@ -2221,7 +2223,7 @@ int cx231xx_set_power_mode(struct cx231xx *dev, enum AV_MODE mode)
 	if (status < 0)
 		return status;
 
-	tmp = *((u32 *) value);
+	tmp = le32_to_cpu(*((u32 *) value));
 
 	switch (mode) {
 	case POLARIS_AVMODE_ENXTERNAL_AV:
@@ -2442,7 +2444,7 @@ int cx231xx_power_suspend(struct cx231xx *dev)
 	if (status > 0)
 		return status;
 
-	tmp = *((u32 *) value);
+	tmp = le32_to_cpu(*((u32 *) value));
 	tmp &= (~PWR_MODE_MASK);
 
 	value[0] = (u8) tmp;
@@ -2470,7 +2472,7 @@ int cx231xx_start_stream(struct cx231xx *dev, u32 ep_mask)
 	if (status < 0)
 		return status;
 
-	tmp = *((u32 *) value);
+	tmp = le32_to_cpu(*((u32 *) value));
 	tmp |= ep_mask;
 	value[0] = (u8) tmp;
 	value[1] = (u8) (tmp >> 8);
@@ -2495,7 +2497,7 @@ int cx231xx_stop_stream(struct cx231xx *dev, u32 ep_mask)
 	if (status < 0)
 		return status;
 
-	tmp = *((u32 *) value);
+	tmp = le32_to_cpu(*((u32 *) value));
 	tmp &= (~ep_mask);
 	value[0] = (u8) tmp;
 	value[1] = (u8) (tmp >> 8);
@@ -2638,20 +2640,23 @@ EXPORT_SYMBOL_GPL(cx231xx_capture_start);
 /*****************************************************************************
 *                   G P I O   B I T control functions                        *
 ******************************************************************************/
-int cx231xx_set_gpio_bit(struct cx231xx *dev, u32 gpio_bit, u8 *gpio_val)
+static int cx231xx_set_gpio_bit(struct cx231xx *dev, u32 gpio_bit, u32 gpio_val)
 {
 	int status = 0;
 
-	status = cx231xx_send_gpio_cmd(dev, gpio_bit, gpio_val, 4, 0, 0);
+	gpio_val = cpu_to_le32(gpio_val);
+	status = cx231xx_send_gpio_cmd(dev, gpio_bit, (u8 *)&gpio_val, 4, 0, 0);
 
 	return status;
 }
 
-int cx231xx_get_gpio_bit(struct cx231xx *dev, u32 gpio_bit, u8 *gpio_val)
+static int cx231xx_get_gpio_bit(struct cx231xx *dev, u32 gpio_bit, u32 *gpio_val)
 {
+	u32 tmp;
 	int status = 0;
 
-	status = cx231xx_send_gpio_cmd(dev, gpio_bit, gpio_val, 4, 0, 1);
+	status = cx231xx_send_gpio_cmd(dev, gpio_bit, (u8 *)&tmp, 4, 0, 1);
+	*gpio_val = le32_to_cpu(tmp);
 
 	return status;
 }
@@ -2683,7 +2688,7 @@ int cx231xx_set_gpio_direction(struct cx231xx *dev,
 	else
 		value = dev->gpio_dir | (1 << pin_number);
 
-	status = cx231xx_set_gpio_bit(dev, value, (u8 *) &dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, value, dev->gpio_val);
 
 	/* cache the value for future */
 	dev->gpio_dir = value;
@@ -2717,7 +2722,7 @@ int cx231xx_set_gpio_value(struct cx231xx *dev, int pin_number, int pin_value)
 		value = dev->gpio_dir | (1 << pin_number);
 		dev->gpio_dir = value;
 		status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-					      (u8 *) &dev->gpio_val);
+					      dev->gpio_val);
 		value = 0;
 	}
 
@@ -2730,7 +2735,7 @@ int cx231xx_set_gpio_value(struct cx231xx *dev, int pin_number, int pin_value)
 	dev->gpio_val = value;
 
 	/* toggle bit0 of GP_IO */
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	return status;
 }
@@ -2748,7 +2753,7 @@ int cx231xx_gpio_i2c_start(struct cx231xx *dev)
 	dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
 	dev->gpio_val |= 1 << dev->board.tuner_sda_gpio;
 
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 	if (status < 0)
 		return -EINVAL;
 
@@ -2756,7 +2761,7 @@ int cx231xx_gpio_i2c_start(struct cx231xx *dev)
 	dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
 	dev->gpio_val &= ~(1 << dev->board.tuner_sda_gpio);
 
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 	if (status < 0)
 		return -EINVAL;
 
@@ -2764,7 +2769,7 @@ int cx231xx_gpio_i2c_start(struct cx231xx *dev)
 	dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
 	dev->gpio_val &= ~(1 << dev->board.tuner_sda_gpio);
 
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 	if (status < 0)
 		return -EINVAL;
 
@@ -2782,7 +2787,7 @@ int cx231xx_gpio_i2c_end(struct cx231xx *dev)
 	dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
 	dev->gpio_val &= ~(1 << dev->board.tuner_sda_gpio);
 
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 	if (status < 0)
 		return -EINVAL;
 
@@ -2790,7 +2795,7 @@ int cx231xx_gpio_i2c_end(struct cx231xx *dev)
 	dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
 	dev->gpio_val &= ~(1 << dev->board.tuner_sda_gpio);
 
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 	if (status < 0)
 		return -EINVAL;
 
@@ -2800,7 +2805,7 @@ int cx231xx_gpio_i2c_end(struct cx231xx *dev)
 	dev->gpio_dir &= ~(1 << dev->board.tuner_sda_gpio);
 
 	status =
-	    cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	    cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 	if (status < 0)
 		return -EINVAL;
 
@@ -2822,33 +2827,33 @@ int cx231xx_gpio_i2c_write_byte(struct cx231xx *dev, u8 data)
 			dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
 			dev->gpio_val &= ~(1 << dev->board.tuner_sda_gpio);
 			status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-						      (u8 *)&dev->gpio_val);
+						      dev->gpio_val);
 
 			/* set SCL to output 1; set SDA to output 0     */
 			dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
 			status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-						      (u8 *)&dev->gpio_val);
+						      dev->gpio_val);
 
 			/* set SCL to output 0; set SDA to output 0     */
 			dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
 			status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-						      (u8 *)&dev->gpio_val);
+						      dev->gpio_val);
 		} else {
 			/* set SCL to output 0; set SDA to output 1     */
 			dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
 			dev->gpio_val |= 1 << dev->board.tuner_sda_gpio;
 			status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-						      (u8 *)&dev->gpio_val);
+						      dev->gpio_val);
 
 			/* set SCL to output 1; set SDA to output 1     */
 			dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
 			status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-						      (u8 *)&dev->gpio_val);
+						      dev->gpio_val);
 
 			/* set SCL to output 0; set SDA to output 1     */
 			dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
 			status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-						      (u8 *)&dev->gpio_val);
+						      dev->gpio_val);
 		}
 	}
 	return status;
@@ -2867,17 +2872,17 @@ int cx231xx_gpio_i2c_read_byte(struct cx231xx *dev, u8 *buf)
 		/* set SCL to output 0; set SDA to input */
 		dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
 		status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-					      (u8 *)&dev->gpio_val);
+					      dev->gpio_val);
 
 		/* set SCL to output 1; set SDA to input */
 		dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
 		status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
-					      (u8 *)&dev->gpio_val);
+					      dev->gpio_val);
 
 		/* get SDA data bit */
 		gpio_logic_value = dev->gpio_val;
 		status = cx231xx_get_gpio_bit(dev, dev->gpio_dir,
-					      (u8 *)&dev->gpio_val);
+					      &dev->gpio_val);
 		if ((dev->gpio_val & (1 << dev->board.tuner_sda_gpio)) != 0)
 			value |= (1 << (8 - i - 1));
 
@@ -2888,7 +2893,7 @@ int cx231xx_gpio_i2c_read_byte(struct cx231xx *dev, u8 *buf)
 	   !!!set SDA to input, never to modify SDA direction at
 	   the same times */
 	dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	/* store the value */
 	*buf = value & 0xff;
@@ -2909,12 +2914,12 @@ int cx231xx_gpio_i2c_read_ack(struct cx231xx *dev)
 	dev->gpio_dir &= ~(1 << dev->board.tuner_scl_gpio);
 
 	gpio_logic_value = dev->gpio_val;
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	do {
 		msleep(2);
 		status = cx231xx_get_gpio_bit(dev, dev->gpio_dir,
-					      (u8 *)&dev->gpio_val);
+					      &dev->gpio_val);
 		nCnt--;
 	} while (((dev->gpio_val &
 			  (1 << dev->board.tuner_scl_gpio)) == 0) &&
@@ -2929,7 +2934,7 @@ int cx231xx_gpio_i2c_read_ack(struct cx231xx *dev)
 	 * through clock stretch, slave has given a SCL signal,
 	 * so the SDA data can be directly read.
 	 */
-	status = cx231xx_get_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_get_gpio_bit(dev, dev->gpio_dir, &dev->gpio_val);
 
 	if ((dev->gpio_val & 1 << dev->board.tuner_sda_gpio) == 0) {
 		dev->gpio_val = gpio_logic_value;
@@ -2945,7 +2950,7 @@ int cx231xx_gpio_i2c_read_ack(struct cx231xx *dev)
 	dev->gpio_val = gpio_logic_value;
 	dev->gpio_dir |= (1 << dev->board.tuner_scl_gpio);
 	dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	return status;
 }
@@ -2956,24 +2961,24 @@ int cx231xx_gpio_i2c_write_ack(struct cx231xx *dev)
 
 	/* set SDA to ouput */
 	dev->gpio_dir |= 1 << dev->board.tuner_sda_gpio;
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	/* set SCL = 0 (output); set SDA = 0 (output) */
 	dev->gpio_val &= ~(1 << dev->board.tuner_sda_gpio);
 	dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	/* set SCL = 1 (output); set SDA = 0 (output) */
 	dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	/* set SCL = 0 (output); set SDA = 0 (output) */
 	dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	/* set SDA to input,and then the slave will read data from SDA. */
 	dev->gpio_dir &= ~(1 << dev->board.tuner_sda_gpio);
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	return status;
 }
@@ -2985,15 +2990,15 @@ int cx231xx_gpio_i2c_write_nak(struct cx231xx *dev)
 	/* set scl to output ; set sda to input */
 	dev->gpio_dir |= 1 << dev->board.tuner_scl_gpio;
 	dev->gpio_dir &= ~(1 << dev->board.tuner_sda_gpio);
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	/* set scl to output 0; set sda to input */
 	dev->gpio_val &= ~(1 << dev->board.tuner_scl_gpio);
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	/* set scl to output 1; set sda to input */
 	dev->gpio_val |= 1 << dev->board.tuner_scl_gpio;
-	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, (u8 *)&dev->gpio_val);
+	status = cx231xx_set_gpio_bit(dev, dev->gpio_dir, dev->gpio_val);
 
 	return status;
 }
diff --git a/drivers/media/usb/cx231xx/cx231xx-cards.c b/drivers/media/usb/cx231xx/cx231xx-cards.c
index 8d529565f163..13249e5a7891 100644
--- a/drivers/media/usb/cx231xx/cx231xx-cards.c
+++ b/drivers/media/usb/cx231xx/cx231xx-cards.c
@@ -263,7 +263,11 @@ struct cx231xx_board cx231xx_boards[] = {
 		.norm = V4L2_STD_PAL,
 		.no_alt_vanc = 1,
 		.external_av = 1,
-		.has_417 = 1,
+		.dont_use_port_3 = 1,
+		/* Actually, it has a 417, but it isn't working correctly.
+		 * So set to 0 for now until someone can manage to get this
+		 * to work reliably. */
+		.has_417 = 0,
 
 		.input = {{
 				.type = CX231XX_VMUX_COMPOSITE1,
@@ -630,6 +634,39 @@ struct cx231xx_board cx231xx_boards[] = {
 			.gpio = NULL,
 		} },
 	},
+	[CX231XX_BOARD_OTG102] = {
+		.name = "Geniatech OTG102",
+		.tuner_type = TUNER_ABSENT,
+		.decoder = CX231XX_AVDECODER,
+		.output_mode = OUT_MODE_VIP11,
+		.ctl_pin_status_mask = 0xFFFFFFC4,
+		.agc_analog_digital_select_gpio = 0x0c, 
+			/* According with PV CxPlrCAP.inf file */
+		.gpio_pin_status_mask = 0x4001000,
+		.norm = V4L2_STD_NTSC,
+		.no_alt_vanc = 1,
+		.external_av = 1,
+		.dont_use_port_3 = 1,
+		/*.has_417 = 1, */
+		/* This board is believed to have a hardware encoding chip
+		 * supporting mpeg1/2/4, but as the 417 is apparently not
+		 * working for the reference board it is not here either. */
+
+		.input = {{
+				.type = CX231XX_VMUX_COMPOSITE1,
+				.vmux = CX231XX_VIN_2_1,
+				.amux = CX231XX_AMUX_LINE_IN,
+				.gpio = NULL,
+			}, {
+				.type = CX231XX_VMUX_SVIDEO,
+				.vmux = CX231XX_VIN_1_1 |
+					(CX231XX_VIN_1_2 << 8) |
+					CX25840_SVIDEO_ON,
+				.amux = CX231XX_AMUX_LINE_IN,
+				.gpio = NULL,
+			}
+		},
+	},
 };
 const unsigned int cx231xx_bcount = ARRAY_SIZE(cx231xx_boards);
 
@@ -671,6 +708,8 @@ struct usb_device_id cx231xx_id_table[] = {
 	 .driver_info = CX231XX_BOARD_ICONBIT_U100},
 	{USB_DEVICE(0x0fd9, 0x0037),
 	 .driver_info = CX231XX_BOARD_ELGATO_VIDEO_CAPTURE_V2},
+	{USB_DEVICE(0x1f4d, 0x0102),
+	 .driver_info = CX231XX_BOARD_OTG102},
 	{},
 };
 
@@ -846,8 +885,6 @@ void cx231xx_card_setup(struct cx231xx *dev)
 int cx231xx_config(struct cx231xx *dev)
 {
 	/* TBD need to add cx231xx specific code */
-	dev->mute = 1;		/* maybe not the right place... */
-	dev->volume = 0x1f;
 
 	return 0;
 }
@@ -1187,8 +1224,8 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
 	uif = udev->actconfig->interface[dev->current_pcb_config.
 		       hs_config_info[0].interface_info.video_index + 1];
 
-	dev->video_mode.end_point_addr = le16_to_cpu(uif->altsetting[0].
-			endpoint[isoc_pipe].desc.bEndpointAddress);
+	dev->video_mode.end_point_addr = uif->altsetting[0].
+			endpoint[isoc_pipe].desc.bEndpointAddress;
 
 	dev->video_mode.num_alt = uif->num_altsetting;
 	cx231xx_info("EndPoint Addr 0x%x, Alternate settings: %i\n",
@@ -1221,8 +1258,8 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
 				       vanc_index + 1];
 
 	dev->vbi_mode.end_point_addr =
-	    le16_to_cpu(uif->altsetting[0].endpoint[isoc_pipe].desc.
-			bEndpointAddress);
+	    uif->altsetting[0].endpoint[isoc_pipe].desc.
+			bEndpointAddress;
 
 	dev->vbi_mode.num_alt = uif->num_altsetting;
 	cx231xx_info("EndPoint Addr 0x%x, Alternate settings: %i\n",
@@ -1256,8 +1293,8 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
 				       hanc_index + 1];
 
 	dev->sliced_cc_mode.end_point_addr =
-	    le16_to_cpu(uif->altsetting[0].endpoint[isoc_pipe].desc.
-			bEndpointAddress);
+	    uif->altsetting[0].endpoint[isoc_pipe].desc.
+			bEndpointAddress;
 
 	dev->sliced_cc_mode.num_alt = uif->num_altsetting;
 	cx231xx_info("EndPoint Addr 0x%x, Alternate settings: %i\n",
@@ -1292,8 +1329,8 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
 					       ts1_index + 1];
 
 		dev->ts1_mode.end_point_addr =
-		    le16_to_cpu(uif->altsetting[0].endpoint[isoc_pipe].
-				desc.bEndpointAddress);
+		    uif->altsetting[0].endpoint[isoc_pipe].
+				desc.bEndpointAddress;
 
 		dev->ts1_mode.num_alt = uif->num_altsetting;
 		cx231xx_info("EndPoint Addr 0x%x, Alternate settings: %i\n",
diff --git a/drivers/media/usb/cx231xx/cx231xx-core.c b/drivers/media/usb/cx231xx/cx231xx-core.c
index 05358d486135..4ba3ce09b713 100644
--- a/drivers/media/usb/cx231xx/cx231xx-core.c
+++ b/drivers/media/usb/cx231xx/cx231xx-core.c
@@ -1488,7 +1488,7 @@ int cx231xx_mode_register(struct cx231xx *dev, u16 address, u32 mode)
 	if (status < 0)
 		return status;
 
-	tmp = *((u32 *) value);
+	tmp = le32_to_cpu(*((u32 *) value));
 	tmp |= mode;
 
 	value[0] = (u8) tmp;
diff --git a/drivers/media/usb/cx231xx/cx231xx-dvb.c b/drivers/media/usb/cx231xx/cx231xx-dvb.c
index 7c4e360ba9bc..14e26106fd72 100644
--- a/drivers/media/usb/cx231xx/cx231xx-dvb.c
+++ b/drivers/media/usb/cx231xx/cx231xx-dvb.c
@@ -89,8 +89,8 @@ static struct tda18271_std_map cnxt_rde253s_tda18271_std_map = {
 };
 
 static struct tda18271_std_map mb86a20s_tda18271_config = {
-	.dvbt_6   = { .if_freq = 3300, .agc_mode = 3, .std = 4,
-		      .if_lvl = 7, .rfagc_top = 0x37, },
+	.dvbt_6   = { .if_freq = 4000, .agc_mode = 3, .std = 4,
+		      .if_lvl = 0, .rfagc_top = 0x37, },
 };
 
 static struct tda18271_config cnxt_rde253s_tunerconfig = {
diff --git a/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.c b/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.c
index 7473c33e823e..d7308ab7a90f 100644
--- a/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.c
+++ b/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.c
@@ -672,7 +672,7 @@ u32 initialize_cx231xx(struct cx231xx *dev)
 	pcb config it is related to */
 	cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, BOARD_CFG_STAT, data, 4);
 
-	config_info = *((u32 *) data);
+	config_info = le32_to_cpu(*((u32 *) data));
 	usb_speed = (u8) (config_info & 0x1);
 
 	/* Verify this device belongs to Bus power or Self power device */
diff --git a/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.h b/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.h
index f5e46e89f3ab..b3c6190e0c69 100644
--- a/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.h
+++ b/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.h
@@ -68,11 +68,6 @@ enum USB_SPEED{
 	HIGH_SPEED = 0x1	/* 1: high speed */
 };
 
-enum _true_false{
-	FALSE = 0,
-	TRUE = 1
-};
-
 #define TS_MASK         0x6
 enum TS_PORT{
 	NO_TS_PORT = 0x0,	/* 2'b00: Neither port used. PCB not a Hybrid,
diff --git a/drivers/media/usb/cx231xx/cx231xx-vbi.c b/drivers/media/usb/cx231xx/cx231xx-vbi.c
index 46e3892557c2..1340ff268817 100644
--- a/drivers/media/usb/cx231xx/cx231xx-vbi.c
+++ b/drivers/media/usb/cx231xx/cx231xx-vbi.c
@@ -70,10 +70,10 @@ static inline void print_err_status(struct cx231xx *dev, int packet, int status)
 		break;
 	}
 	if (packet < 0) {
-		cx231xx_err(DRIVER_NAME "URB status %d [%s].\n", status,
+		cx231xx_err("URB status %d [%s].\n", status,
 			    errmsg);
 	} else {
-		cx231xx_err(DRIVER_NAME "URB packet %d, status %d [%s].\n",
+		cx231xx_err("URB packet %d, status %d [%s].\n",
 			    packet, status, errmsg);
 	}
 }
@@ -317,7 +317,7 @@ static void cx231xx_irq_vbi_callback(struct urb *urb)
 	case -ESHUTDOWN:
 		return;
 	default:		/* error */
-		cx231xx_err(DRIVER_NAME "urb completition error %d.\n",
+		cx231xx_err("urb completition error %d.\n",
 			    urb->status);
 		break;
 	}
@@ -332,7 +332,7 @@ static void cx231xx_irq_vbi_callback(struct urb *urb)
 
 	urb->status = usb_submit_urb(urb, GFP_ATOMIC);
 	if (urb->status) {
-		cx231xx_err(DRIVER_NAME "urb resubmit failed (error=%i)\n",
+		cx231xx_err("urb resubmit failed (error=%i)\n",
 			    urb->status);
 	}
 }
@@ -345,7 +345,7 @@ void cx231xx_uninit_vbi_isoc(struct cx231xx *dev)
 	struct urb *urb;
 	int i;
 
-	cx231xx_info(DRIVER_NAME "cx231xx: called cx231xx_uninit_vbi_isoc\n");
+	cx231xx_info("called cx231xx_uninit_vbi_isoc\n");
 
 	dev->vbi_mode.bulk_ctl.nfields = -1;
 	for (i = 0; i < dev->vbi_mode.bulk_ctl.num_bufs; i++) {
@@ -394,7 +394,7 @@ int cx231xx_init_vbi_isoc(struct cx231xx *dev, int max_packets,
 	struct urb *urb;
 	int rc;
 
-	cx231xx_info(DRIVER_NAME "cx231xx: called cx231xx_prepare_isoc\n");
+	cx231xx_info("called cx231xx_vbi_isoc\n");
 
 	/* De-allocates all pending stuff */
 	cx231xx_uninit_vbi_isoc(dev);
@@ -442,8 +442,7 @@ int cx231xx_init_vbi_isoc(struct cx231xx *dev, int max_packets,
 
 		urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!urb) {
-			cx231xx_err(DRIVER_NAME
-				    ": cannot alloc bulk_ctl.urb %i\n", i);
+			cx231xx_err("cannot alloc bulk_ctl.urb %i\n", i);
 			cx231xx_uninit_vbi_isoc(dev);
 			return -ENOMEM;
 		}
@@ -453,8 +452,7 @@ int cx231xx_init_vbi_isoc(struct cx231xx *dev, int max_packets,
 		dev->vbi_mode.bulk_ctl.transfer_buffer[i] =
 		    kzalloc(sb_size, GFP_KERNEL);
 		if (!dev->vbi_mode.bulk_ctl.transfer_buffer[i]) {
-			cx231xx_err(DRIVER_NAME
-				    ": unable to allocate %i bytes for transfer"
+			cx231xx_err("unable to allocate %i bytes for transfer"
 				    " buffer %i%s\n", sb_size, i,
 				    in_interrupt() ? " while in int" : "");
 			cx231xx_uninit_vbi_isoc(dev);
@@ -473,8 +471,7 @@ int cx231xx_init_vbi_isoc(struct cx231xx *dev, int max_packets,
 	for (i = 0; i < dev->vbi_mode.bulk_ctl.num_bufs; i++) {
 		rc = usb_submit_urb(dev->vbi_mode.bulk_ctl.urb[i], GFP_ATOMIC);
 		if (rc) {
-			cx231xx_err(DRIVER_NAME
-				    ": submit of urb %i failed (error=%i)\n", i,
+			cx231xx_err("submit of urb %i failed (error=%i)\n", i,
 				    rc);
 			cx231xx_uninit_vbi_isoc(dev);
 			return rc;
@@ -526,7 +523,7 @@ static inline void vbi_buffer_filled(struct cx231xx *dev,
 				     struct cx231xx_buffer *buf)
 {
 	/* Advice that buffer was filled */
-	/* cx231xx_info(DRIVER_NAME "[%p/%d] wakeup\n", buf, buf->vb.i); */
+	/* cx231xx_info("[%p/%d] wakeup\n", buf, buf->vb.i); */
 
 	buf->vb.state = VIDEOBUF_DONE;
 	buf->vb.field_count++;
@@ -618,7 +615,7 @@ static inline void get_next_vbi_buf(struct cx231xx_dmaqueue *dma_q,
 	char *outp;
 
 	if (list_empty(&dma_q->active)) {
-		cx231xx_err(DRIVER_NAME ": No active queue to serve\n");
+		cx231xx_err("No active queue to serve\n");
 		dev->vbi_mode.bulk_ctl.buf = NULL;
 		*buf = NULL;
 		return;
diff --git a/drivers/media/usb/cx231xx/cx231xx-video.c b/drivers/media/usb/cx231xx/cx231xx-video.c
index 06376d904c9f..cd221474e1b9 100644
--- a/drivers/media/usb/cx231xx/cx231xx-video.c
+++ b/drivers/media/usb/cx231xx/cx231xx-video.c
@@ -35,6 +35,7 @@
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
 #include <media/v4l2-chip-ident.h>
 #include <media/msp3400.h>
 #include <media/tuner.h>
@@ -100,125 +101,6 @@ static struct cx231xx_fmt format[] = {
 	 },
 };
 
-/* supported controls */
-/* Common to all boards */
-
-/* ------------------------------------------------------------------- */
-
-static const struct v4l2_queryctrl no_ctl = {
-	.name = "42",
-	.flags = V4L2_CTRL_FLAG_DISABLED,
-};
-
-static struct cx231xx_ctrl cx231xx_ctls[] = {
-	/* --- video --- */
-	{
-		.v = {
-			.id = V4L2_CID_BRIGHTNESS,
-			.name = "Brightness",
-			.minimum = 0x00,
-			.maximum = 0xff,
-			.step = 1,
-			.default_value = 0x7f,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off = 128,
-		.reg = LUMA_CTRL,
-		.mask = 0x00ff,
-		.shift = 0,
-	}, {
-		.v = {
-			.id = V4L2_CID_CONTRAST,
-			.name = "Contrast",
-			.minimum = 0,
-			.maximum = 0xff,
-			.step = 1,
-			.default_value = 0x3f,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off = 0,
-		.reg = LUMA_CTRL,
-		.mask = 0xff00,
-		.shift = 8,
-	}, {
-		.v = {
-			.id = V4L2_CID_HUE,
-			.name = "Hue",
-			.minimum = 0,
-			.maximum = 0xff,
-			.step = 1,
-			.default_value = 0x7f,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off = 128,
-		.reg = CHROMA_CTRL,
-		.mask = 0xff0000,
-		.shift = 16,
-	}, {
-	/* strictly, this only describes only U saturation.
-	* V saturation is handled specially through code.
-	*/
-		.v = {
-			.id = V4L2_CID_SATURATION,
-			.name = "Saturation",
-			.minimum = 0,
-			.maximum = 0xff,
-			.step = 1,
-			.default_value = 0x7f,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off = 0,
-		.reg = CHROMA_CTRL,
-		.mask = 0x00ff,
-		.shift = 0,
-	}, {
-		/* --- audio --- */
-		.v = {
-			.id = V4L2_CID_AUDIO_MUTE,
-			.name = "Mute",
-			.minimum = 0,
-			.maximum = 1,
-			.default_value = 1,
-			.type = V4L2_CTRL_TYPE_BOOLEAN,
-		},
-		.reg = PATH1_CTL1,
-		.mask = (0x1f << 24),
-		.shift = 24,
-	}, {
-		.v = {
-			.id = V4L2_CID_AUDIO_VOLUME,
-			.name = "Volume",
-			.minimum = 0,
-			.maximum = 0x3f,
-			.step = 1,
-			.default_value = 0x3f,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.reg = PATH1_VOL_CTL,
-		.mask = 0xff,
-		.shift = 0,
-	}
-};
-static const int CX231XX_CTLS = ARRAY_SIZE(cx231xx_ctls);
-
-static const u32 cx231xx_user_ctrls[] = {
-	V4L2_CID_USER_CLASS,
-	V4L2_CID_BRIGHTNESS,
-	V4L2_CID_CONTRAST,
-	V4L2_CID_SATURATION,
-	V4L2_CID_HUE,
-	V4L2_CID_AUDIO_VOLUME,
-#if 0
-	V4L2_CID_AUDIO_BALANCE,
-#endif
-	V4L2_CID_AUDIO_MUTE,
-	0
-};
-
-static const u32 *ctrl_classes[] = {
-	cx231xx_user_ctrls,
-	NULL
-};
 
 /* ------------------------------------------------------------------
 	Video buffer and parser functions
@@ -1005,6 +887,7 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 
 	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	f->fmt.pix.priv = 0;
 
 	return 0;
 }
@@ -1045,10 +928,11 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 	f->fmt.pix.width = width;
 	f->fmt.pix.height = height;
 	f->fmt.pix.pixelformat = fmt->fourcc;
-	f->fmt.pix.bytesperline = (dev->width * fmt->depth + 7) >> 3;
+	f->fmt.pix.bytesperline = (width * fmt->depth + 7) >> 3;
 	f->fmt.pix.sizeimage = f->fmt.pix.bytesperline * height;
 	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	f->fmt.pix.priv = 0;
 
 	return 0;
 }
@@ -1103,39 +987,39 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *norm)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
 	struct v4l2_mbus_framefmt mbus_fmt;
-	struct v4l2_format f;
 	int rc;
 
 	rc = check_dev(dev);
 	if (rc < 0)
 		return rc;
 
-	cx231xx_info("vidioc_s_std : 0x%x\n", (unsigned int)*norm);
+	if (dev->norm == norm)
+		return 0;
 
-	dev->norm = *norm;
+	if (videobuf_queue_is_busy(&fh->vb_vidq))
+		return -EBUSY;
+
+	dev->norm = norm;
 
 	/* Adjusts width/height, if needed */
-	f.fmt.pix.width = dev->width;
-	f.fmt.pix.height = dev->height;
-	vidioc_try_fmt_vid_cap(file, priv, &f);
+	dev->width = 720;
+	dev->height = (dev->norm & V4L2_STD_625_50) ? 576 : 480;
 
 	call_all(dev, core, s_std, dev->norm);
 
 	/* We need to reset basic properties in the decoder related to
 	   resolution (since a standard change effects things like the number
 	   of lines in VACT, etc) */
-	v4l2_fill_mbus_format(&mbus_fmt, &f.fmt.pix, V4L2_MBUS_FMT_FIXED);
+	memset(&mbus_fmt, 0, sizeof(mbus_fmt));
+	mbus_fmt.code = V4L2_MBUS_FMT_FIXED;
+	mbus_fmt.width = dev->width;
+	mbus_fmt.height = dev->height;
 	call_all(dev, video, s_mbus_fmt, &mbus_fmt);
-	v4l2_fill_pix_format(&f.fmt.pix, &mbus_fmt);
-
-	/* set new image size */
-	dev->width = f.fmt.pix.width;
-	dev->height = f.fmt.pix.height;
 
 	/* do mode control overrides */
 	cx231xx_do_mode_ctrl_overrides(dev);
@@ -1152,7 +1036,7 @@ static const char *iname[] = {
 	[CX231XX_VMUX_DEBUG]      = "for debug only",
 };
 
-static int vidioc_enum_input(struct file *file, void *priv,
+int cx231xx_enum_input(struct file *file, void *priv,
 			     struct v4l2_input *i)
 {
 	struct cx231xx_fh *fh = priv;
@@ -1192,7 +1076,7 @@ static int vidioc_enum_input(struct file *file, void *priv,
 	return 0;
 }
 
-static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+int cx231xx_g_input(struct file *file, void *priv, unsigned int *i)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
@@ -1202,7 +1086,7 @@ static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
 	return 0;
 }
 
-static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
+int cx231xx_s_input(struct file *file, void *priv, unsigned int i)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
@@ -1231,117 +1115,7 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 	return 0;
 }
 
-static int vidioc_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
-{
-	struct cx231xx_fh *fh = priv;
-	struct cx231xx *dev = fh->dev;
-
-	switch (a->index) {
-	case CX231XX_AMUX_VIDEO:
-		strcpy(a->name, "Television");
-		break;
-	case CX231XX_AMUX_LINE_IN:
-		strcpy(a->name, "Line In");
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	a->index = dev->ctl_ainput;
-	a->capability = V4L2_AUDCAP_STEREO;
-
-	return 0;
-}
-
-static int vidioc_s_audio(struct file *file, void *priv, const struct v4l2_audio *a)
-{
-	struct cx231xx_fh *fh = priv;
-	struct cx231xx *dev = fh->dev;
-	int status = 0;
-
-	/* Doesn't allow manual routing */
-	if (a->index != dev->ctl_ainput)
-		return -EINVAL;
-
-	dev->ctl_ainput = INPUT(a->index)->amux;
-	status = cx231xx_set_audio_input(dev, dev->ctl_ainput);
-
-	return status;
-}
-
-static int vidioc_queryctrl(struct file *file, void *priv,
-			    struct v4l2_queryctrl *qc)
-{
-	struct cx231xx_fh *fh = priv;
-	struct cx231xx *dev = fh->dev;
-	int id = qc->id;
-	int i;
-	int rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
-
-	qc->id = v4l2_ctrl_next(ctrl_classes, qc->id);
-	if (unlikely(qc->id == 0))
-		return -EINVAL;
-
-	memset(qc, 0, sizeof(*qc));
-
-	qc->id = id;
-
-	if (qc->id < V4L2_CID_BASE || qc->id >= V4L2_CID_LASTP1)
-		return -EINVAL;
-
-	for (i = 0; i < CX231XX_CTLS; i++)
-		if (cx231xx_ctls[i].v.id == qc->id)
-			break;
-
-	if (i == CX231XX_CTLS) {
-		*qc = no_ctl;
-		return 0;
-	}
-	*qc = cx231xx_ctls[i].v;
-
-	call_all(dev, core, queryctrl, qc);
-
-	if (qc->type)
-		return 0;
-	else
-		return -EINVAL;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
-{
-	struct cx231xx_fh *fh = priv;
-	struct cx231xx *dev = fh->dev;
-	int rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
-
-	call_all(dev, core, g_ctrl, ctrl);
-	return rc;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
-{
-	struct cx231xx_fh *fh = priv;
-	struct cx231xx *dev = fh->dev;
-	int rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
-
-	call_all(dev, core, s_ctrl, ctrl);
-	return rc;
-}
-
-static int vidioc_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
+int cx231xx_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
@@ -1360,11 +1134,12 @@ static int vidioc_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 	t->capability = V4L2_TUNER_CAP_NORM;
 	t->rangehigh = 0xffffffffUL;
 	t->signal = 0xffff;	/* LOCKED */
+	call_all(dev, tuner, g_tuner, t);
 
 	return 0;
 }
 
-static int vidioc_s_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
+int cx231xx_s_tuner(struct file *file, void *priv, const struct v4l2_tuner *t)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
@@ -1382,25 +1157,26 @@ static int vidioc_s_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 	return 0;
 }
 
-static int vidioc_g_frequency(struct file *file, void *priv,
+int cx231xx_g_frequency(struct file *file, void *priv,
 			      struct v4l2_frequency *f)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
 
-	f->type = fh->radio ? V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
-	f->frequency = dev->ctl_freq;
+	if (f->tuner)
+		return -EINVAL;
 
-	call_all(dev, tuner, g_frequency, f);
+	f->frequency = dev->ctl_freq;
 
 	return 0;
 }
 
-static int vidioc_s_frequency(struct file *file, void *priv,
-			      struct v4l2_frequency *f)
+int cx231xx_s_frequency(struct file *file, void *priv,
+			      const struct v4l2_frequency *f)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
+	struct v4l2_frequency new_freq = *f;
 	int rc;
 	u32 if_frequency = 5400000;
 
@@ -1415,16 +1191,12 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 	if (0 != f->tuner)
 		return -EINVAL;
 
-	if (unlikely(0 == fh->radio && f->type != V4L2_TUNER_ANALOG_TV))
-		return -EINVAL;
-	if (unlikely(1 == fh->radio && f->type != V4L2_TUNER_RADIO))
-		return -EINVAL;
-
 	/* set pre channel change settings in DIF first */
 	rc = cx231xx_tuner_pre_channel_change(dev);
 
-	dev->ctl_freq = f->frequency;
 	call_all(dev, tuner, s_frequency, f);
+	call_all(dev, tuner, g_frequency, &new_freq);
+	dev->ctl_freq = new_freq.frequency;
 
 	/* set post channel change settings in DIF first */
 	rc = cx231xx_tuner_post_channel_change(dev);
@@ -1456,6 +1228,19 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 	return rc;
 }
 
+int cx231xx_g_chip_ident(struct file *file, void *fh,
+			struct v4l2_dbg_chip_ident *chip)
+{
+	chip->ident = V4L2_IDENT_NONE;
+	chip->revision = 0;
+	if (chip->match.type == V4L2_CHIP_MATCH_HOST) {
+		if (v4l2_chip_match_host(&chip->match))
+			chip->ident = V4L2_IDENT_CX23100;
+		return 0;
+	}
+	return -EINVAL;
+}
+
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 
 /*
@@ -1471,7 +1256,7 @@ static int vidioc_s_frequency(struct file *file, void *priv,
   if type == i2caddr, then <chip> is the 7-bit I2C address
 */
 
-static int vidioc_g_register(struct file *file, void *priv,
+int cx231xx_g_register(struct file *file, void *priv,
 			     struct v4l2_dbg_register *reg)
 {
 	struct cx231xx_fh *fh = priv;
@@ -1618,8 +1403,8 @@ static int vidioc_g_register(struct file *file, void *priv,
 	return ret;
 }
 
-static int vidioc_s_register(struct file *file, void *priv,
-			     struct v4l2_dbg_register *reg)
+int cx231xx_s_register(struct file *file, void *priv,
+			     const struct v4l2_dbg_register *reg)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
@@ -1837,9 +1622,6 @@ static int vidioc_streamoff(struct file *file, void *priv,
 	if (rc < 0)
 		return rc;
 
-	if ((fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
-	    (fh->type != V4L2_BUF_TYPE_VBI_CAPTURE))
-		return -EINVAL;
 	if (type != fh->type)
 		return -EINVAL;
 
@@ -1851,9 +1633,10 @@ static int vidioc_streamoff(struct file *file, void *priv,
 	return 0;
 }
 
-static int vidioc_querycap(struct file *file, void *priv,
+int cx231xx_querycap(struct file *file, void *priv,
 			   struct v4l2_capability *cap)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
 
@@ -1861,17 +1644,22 @@ static int vidioc_querycap(struct file *file, void *priv,
 	strlcpy(cap->card, cx231xx_boards[dev->model].name, sizeof(cap->card));
 	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
 
-	cap->capabilities = V4L2_CAP_VBI_CAPTURE |
-#if 0
-		V4L2_CAP_SLICED_VBI_CAPTURE |
-#endif
-		V4L2_CAP_VIDEO_CAPTURE	|
-		V4L2_CAP_AUDIO		|
-		V4L2_CAP_READWRITE	|
-		V4L2_CAP_STREAMING;
-
+	if (vdev->vfl_type == VFL_TYPE_RADIO)
+		cap->device_caps = V4L2_CAP_RADIO;
+	else {
+		cap->device_caps = V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+		if (vdev->vfl_type == VFL_TYPE_VBI)
+			cap->device_caps |= V4L2_CAP_VBI_CAPTURE;
+		else
+			cap->device_caps |= V4L2_CAP_VIDEO_CAPTURE;
+	}
 	if (dev->tuner_type != TUNER_ABSENT)
-		cap->capabilities |= V4L2_CAP_TUNER;
+		cap->device_caps |= V4L2_CAP_TUNER;
+	cap->capabilities = cap->device_caps | V4L2_CAP_READWRITE |
+		V4L2_CAP_VBI_CAPTURE | V4L2_CAP_VIDEO_CAPTURE |
+		V4L2_CAP_STREAMING | V4L2_CAP_DEVICE_CAPS;
+	if (dev->radio_dev)
+		cap->capabilities |= V4L2_CAP_RADIO;
 
 	return 0;
 }
@@ -1888,47 +1676,6 @@ static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
 	return 0;
 }
 
-/* Sliced VBI ioctls */
-static int vidioc_g_fmt_sliced_vbi_cap(struct file *file, void *priv,
-				       struct v4l2_format *f)
-{
-	struct cx231xx_fh *fh = priv;
-	struct cx231xx *dev = fh->dev;
-	int rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
-
-	f->fmt.sliced.service_set = 0;
-
-	call_all(dev, vbi, g_sliced_fmt, &f->fmt.sliced);
-
-	if (f->fmt.sliced.service_set == 0)
-		rc = -EINVAL;
-
-	return rc;
-}
-
-static int vidioc_try_set_sliced_vbi_cap(struct file *file, void *priv,
-					 struct v4l2_format *f)
-{
-	struct cx231xx_fh *fh = priv;
-	struct cx231xx *dev = fh->dev;
-	int rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
-
-	call_all(dev, vbi, g_sliced_fmt, &f->fmt.sliced);
-
-	if (f->fmt.sliced.service_set == 0)
-		return -EINVAL;
-
-	return 0;
-}
-
 /* RAW VBI ioctls */
 
 static int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
@@ -1936,6 +1683,7 @@ static int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
+
 	f->fmt.vbi.sampling_rate = 6750000 * 4;
 	f->fmt.vbi.samples_per_line = VBI_LINE_LENGTH;
 	f->fmt.vbi.sample_format = V4L2_PIX_FMT_GREY;
@@ -1947,6 +1695,7 @@ static int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
 	f->fmt.vbi.start[1] = (dev->norm & V4L2_STD_625_50) ?
 	    PAL_VBI_START_LINE + 312 : NTSC_VBI_START_LINE + 263;
 	f->fmt.vbi.count[1] = f->fmt.vbi.count[0];
+	memset(f->fmt.vbi.reserved, 0, sizeof(f->fmt.vbi.reserved));
 
 	return 0;
 
@@ -1958,12 +1707,6 @@ static int vidioc_try_fmt_vbi_cap(struct file *file, void *priv,
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
 
-	if (dev->vbi_stream_on && !fh->stream_on) {
-		cx231xx_errdev("%s device in use by another fh\n", __func__);
-		return -EBUSY;
-	}
-
-	f->type = V4L2_BUF_TYPE_VBI_CAPTURE;
 	f->fmt.vbi.sampling_rate = 6750000 * 4;
 	f->fmt.vbi.samples_per_line = VBI_LINE_LENGTH;
 	f->fmt.vbi.sample_format = V4L2_PIX_FMT_GREY;
@@ -1976,11 +1719,25 @@ static int vidioc_try_fmt_vbi_cap(struct file *file, void *priv,
 	f->fmt.vbi.start[1] = (dev->norm & V4L2_STD_625_50) ?
 	    PAL_VBI_START_LINE + 312 : NTSC_VBI_START_LINE + 263;
 	f->fmt.vbi.count[1] = f->fmt.vbi.count[0];
+	memset(f->fmt.vbi.reserved, 0, sizeof(f->fmt.vbi.reserved));
 
 	return 0;
 
 }
 
+static int vidioc_s_fmt_vbi_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct cx231xx_fh *fh = priv;
+	struct cx231xx *dev = fh->dev;
+
+	if (dev->vbi_stream_on && !fh->stream_on) {
+		cx231xx_errdev("%s device in use by another fh\n", __func__);
+		return -EBUSY;
+	}
+	return vidioc_try_fmt_vbi_cap(file, priv, f);
+}
+
 static int vidioc_reqbufs(struct file *file, void *priv,
 			  struct v4l2_requestbuffers *rb)
 {
@@ -2038,58 +1795,24 @@ static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *b)
 /* RADIO ESPECIFIC IOCTLS                                      */
 /* ----------------------------------------------------------- */
 
-static int radio_querycap(struct file *file, void *priv,
-			  struct v4l2_capability *cap)
-{
-	struct cx231xx *dev = ((struct cx231xx_fh *)priv)->dev;
-
-	strlcpy(cap->driver, "cx231xx", sizeof(cap->driver));
-	strlcpy(cap->card, cx231xx_boards[dev->model].name, sizeof(cap->card));
-	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
-
-	cap->capabilities = V4L2_CAP_TUNER;
-	return 0;
-}
-
 static int radio_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 {
 	struct cx231xx *dev = ((struct cx231xx_fh *)priv)->dev;
 
-	if (unlikely(t->index > 0))
+	if (t->index)
 		return -EINVAL;
 
 	strcpy(t->name, "Radio");
-	t->type = V4L2_TUNER_RADIO;
-
-	call_all(dev, tuner, s_tuner, t);
-
-	return 0;
-}
 
-static int radio_enum_input(struct file *file, void *priv, struct v4l2_input *i)
-{
-	if (i->index != 0)
-		return -EINVAL;
-	strcpy(i->name, "Radio");
-	i->type = V4L2_INPUT_TYPE_TUNER;
+	call_all(dev, tuner, g_tuner, t);
 
 	return 0;
 }
-
-static int radio_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
-{
-	if (unlikely(a->index))
-		return -EINVAL;
-
-	strcpy(a->name, "Radio");
-	return 0;
-}
-
-static int radio_s_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
+static int radio_s_tuner(struct file *file, void *priv, const struct v4l2_tuner *t)
 {
 	struct cx231xx *dev = ((struct cx231xx_fh *)priv)->dev;
 
-	if (0 != t->index)
+	if (t->index)
 		return -EINVAL;
 
 	call_all(dev, tuner, s_tuner, t);
@@ -2097,36 +1820,6 @@ static int radio_s_tuner(struct file *file, void *priv, struct v4l2_tuner *t)
 	return 0;
 }
 
-static int radio_s_audio(struct file *file, void *fh, const struct v4l2_audio *a)
-{
-	return 0;
-}
-
-static int radio_s_input(struct file *file, void *fh, unsigned int i)
-{
-	return 0;
-}
-
-static int radio_queryctrl(struct file *file, void *priv,
-			   struct v4l2_queryctrl *c)
-{
-	int i;
-
-	if (c->id < V4L2_CID_BASE || c->id >= V4L2_CID_LASTP1)
-		return -EINVAL;
-	if (c->id == V4L2_CID_AUDIO_MUTE) {
-		for (i = 0; i < CX231XX_CTLS; i++) {
-			if (cx231xx_ctls[i].v.id == c->id)
-				break;
-		}
-		if (i == CX231XX_CTLS)
-			return -EINVAL;
-		*c = cx231xx_ctls[i].v;
-	} else
-		*c = no_ctl;
-	return 0;
-}
-
 /*
  * cx231xx_v4l2_open()
  * inits the device and starts isoc transfer
@@ -2174,14 +1867,11 @@ static int cx231xx_v4l2_open(struct file *filp)
 		return -ERESTARTSYS;
 	}
 	fh->dev = dev;
-	fh->radio = radio;
 	fh->type = fh_type;
 	filp->private_data = fh;
+	v4l2_fh_init(&fh->fh, vdev);
 
 	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && dev->users == 0) {
-		dev->width = norm_maxw(dev);
-		dev->height = norm_maxh(dev);
-
 		/* Power up in Analog TV mode */
 		if (dev->board.external_av)
 			cx231xx_set_power_mode(dev,
@@ -2204,7 +1894,7 @@ static int cx231xx_v4l2_open(struct file *filp)
 		dev->video_input = dev->video_input > 2 ? 2 : dev->video_input;
 
 	}
-	if (fh->radio) {
+	if (radio) {
 		cx231xx_videodbg("video_open: setting radio device\n");
 
 		/* cx231xx_start_radio(dev); */
@@ -2232,6 +1922,7 @@ static int cx231xx_v4l2_open(struct file *filp)
 					    fh, &dev->lock);
 	}
 	mutex_unlock(&dev->lock);
+	v4l2_fh_add(&fh->fh);
 
 	return errCode;
 }
@@ -2275,6 +1966,8 @@ void cx231xx_release_analog_resources(struct cx231xx *dev)
 			video_device_release(dev->vdev);
 		dev->vdev = NULL;
 	}
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	v4l2_ctrl_handler_free(&dev->radio_ctrl_handler);
 }
 
 /*
@@ -2324,12 +2017,15 @@ static int cx231xx_close(struct file *filp)
 			else
 				cx231xx_set_alt_setting(dev, INDEX_HANC, 0);
 
+			v4l2_fh_del(&fh->fh);
+			v4l2_fh_exit(&fh->fh);
 			kfree(fh);
 			dev->users--;
 			wake_up_interruptible_nr(&dev->open, 1);
 			return 0;
 		}
 
+	v4l2_fh_del(&fh->fh);
 	dev->users--;
 	if (!dev->users) {
 		videobuf_stop(&fh->vb_vidq);
@@ -2356,6 +2052,7 @@ static int cx231xx_close(struct file *filp)
 		/* set alternate 0 */
 		cx231xx_set_alt_setting(dev, INDEX_VIDEO, 0);
 	}
+	v4l2_fh_exit(&fh->fh);
 	kfree(fh);
 	wake_up_interruptible_nr(&dev->open, 1);
 	return 0;
@@ -2412,29 +2109,37 @@ cx231xx_v4l2_read(struct file *filp, char __user *buf, size_t count,
  */
 static unsigned int cx231xx_v4l2_poll(struct file *filp, poll_table *wait)
 {
+	unsigned long req_events = poll_requested_events(wait);
 	struct cx231xx_fh *fh = filp->private_data;
 	struct cx231xx *dev = fh->dev;
+	unsigned res = 0;
 	int rc;
 
 	rc = check_dev(dev);
 	if (rc < 0)
-		return rc;
+		return POLLERR;
 
 	rc = res_get(fh);
 
 	if (unlikely(rc < 0))
 		return POLLERR;
 
+	if (v4l2_event_pending(&fh->fh))
+		res |= POLLPRI;
+	else
+		poll_wait(filp, &fh->fh.wait, wait);
+
+	if (!(req_events & (POLLIN | POLLRDNORM)))
+		return res;
+
 	if ((V4L2_BUF_TYPE_VIDEO_CAPTURE == fh->type) ||
 	    (V4L2_BUF_TYPE_VBI_CAPTURE == fh->type)) {
-		unsigned int res;
-
 		mutex_lock(&dev->lock);
-		res = videobuf_poll_stream(filp, &fh->vb_vidq, wait);
+		res |= videobuf_poll_stream(filp, &fh->vb_vidq, wait);
 		mutex_unlock(&dev->lock);
 		return res;
 	}
-	return POLLERR;
+	return res | POLLERR;
 }
 
 /*
@@ -2479,41 +2184,37 @@ static const struct v4l2_file_operations cx231xx_v4l_fops = {
 };
 
 static const struct v4l2_ioctl_ops video_ioctl_ops = {
-	.vidioc_querycap               = vidioc_querycap,
+	.vidioc_querycap               = cx231xx_querycap,
 	.vidioc_enum_fmt_vid_cap       = vidioc_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap          = vidioc_g_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap        = vidioc_try_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap          = vidioc_s_fmt_vid_cap,
 	.vidioc_g_fmt_vbi_cap          = vidioc_g_fmt_vbi_cap,
 	.vidioc_try_fmt_vbi_cap        = vidioc_try_fmt_vbi_cap,
-	.vidioc_s_fmt_vbi_cap          = vidioc_try_fmt_vbi_cap,
-	.vidioc_g_audio                =  vidioc_g_audio,
-	.vidioc_s_audio                = vidioc_s_audio,
+	.vidioc_s_fmt_vbi_cap          = vidioc_s_fmt_vbi_cap,
 	.vidioc_cropcap                = vidioc_cropcap,
-	.vidioc_g_fmt_sliced_vbi_cap   = vidioc_g_fmt_sliced_vbi_cap,
-	.vidioc_try_fmt_sliced_vbi_cap = vidioc_try_set_sliced_vbi_cap,
 	.vidioc_reqbufs                = vidioc_reqbufs,
 	.vidioc_querybuf               = vidioc_querybuf,
 	.vidioc_qbuf                   = vidioc_qbuf,
 	.vidioc_dqbuf                  = vidioc_dqbuf,
 	.vidioc_s_std                  = vidioc_s_std,
 	.vidioc_g_std                  = vidioc_g_std,
-	.vidioc_enum_input             = vidioc_enum_input,
-	.vidioc_g_input                = vidioc_g_input,
-	.vidioc_s_input                = vidioc_s_input,
-	.vidioc_queryctrl              = vidioc_queryctrl,
-	.vidioc_g_ctrl                 = vidioc_g_ctrl,
-	.vidioc_s_ctrl                 = vidioc_s_ctrl,
+	.vidioc_enum_input             = cx231xx_enum_input,
+	.vidioc_g_input                = cx231xx_g_input,
+	.vidioc_s_input                = cx231xx_s_input,
 	.vidioc_streamon               = vidioc_streamon,
 	.vidioc_streamoff              = vidioc_streamoff,
-	.vidioc_g_tuner                = vidioc_g_tuner,
-	.vidioc_s_tuner                = vidioc_s_tuner,
-	.vidioc_g_frequency            = vidioc_g_frequency,
-	.vidioc_s_frequency            = vidioc_s_frequency,
+	.vidioc_g_tuner                = cx231xx_g_tuner,
+	.vidioc_s_tuner                = cx231xx_s_tuner,
+	.vidioc_g_frequency            = cx231xx_g_frequency,
+	.vidioc_s_frequency            = cx231xx_s_frequency,
+	.vidioc_g_chip_ident           = cx231xx_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-	.vidioc_g_register             = vidioc_g_register,
-	.vidioc_s_register             = vidioc_s_register,
+	.vidioc_g_register             = cx231xx_g_register,
+	.vidioc_s_register             = cx231xx_s_register,
 #endif
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static struct video_device cx231xx_vbi_template;
@@ -2523,33 +2224,29 @@ static const struct video_device cx231xx_video_template = {
 	.release      = video_device_release,
 	.ioctl_ops    = &video_ioctl_ops,
 	.tvnorms      = V4L2_STD_ALL,
-	.current_norm = V4L2_STD_PAL,
 };
 
 static const struct v4l2_file_operations radio_fops = {
 	.owner   = THIS_MODULE,
 	.open   = cx231xx_v4l2_open,
 	.release = cx231xx_v4l2_close,
-	.ioctl   = video_ioctl2,
+	.poll = v4l2_ctrl_poll,
+	.unlocked_ioctl = video_ioctl2,
 };
 
 static const struct v4l2_ioctl_ops radio_ioctl_ops = {
-	.vidioc_querycap    = radio_querycap,
+	.vidioc_querycap    = cx231xx_querycap,
 	.vidioc_g_tuner     = radio_g_tuner,
-	.vidioc_enum_input  = radio_enum_input,
-	.vidioc_g_audio     = radio_g_audio,
 	.vidioc_s_tuner     = radio_s_tuner,
-	.vidioc_s_audio     = radio_s_audio,
-	.vidioc_s_input     = radio_s_input,
-	.vidioc_queryctrl   = radio_queryctrl,
-	.vidioc_g_ctrl      = vidioc_g_ctrl,
-	.vidioc_s_ctrl      = vidioc_s_ctrl,
-	.vidioc_g_frequency = vidioc_g_frequency,
-	.vidioc_s_frequency = vidioc_s_frequency,
+	.vidioc_g_frequency = cx231xx_g_frequency,
+	.vidioc_s_frequency = cx231xx_s_frequency,
+	.vidioc_g_chip_ident = cx231xx_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-	.vidioc_g_register  = vidioc_g_register,
-	.vidioc_s_register  = vidioc_s_register,
+	.vidioc_g_register  = cx231xx_g_register,
+	.vidioc_s_register  = cx231xx_s_register,
 #endif
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static struct video_device cx231xx_radio_template = {
@@ -2575,10 +2272,17 @@ static struct video_device *cx231xx_vdev_init(struct cx231xx *dev,
 	vfd->release = video_device_release;
 	vfd->debug = video_debug;
 	vfd->lock = &dev->lock;
+	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
 
 	snprintf(vfd->name, sizeof(vfd->name), "%s %s", dev->name, type_name);
 
 	video_set_drvdata(vfd, dev);
+	if (dev->tuner_type == TUNER_ABSENT) {
+		v4l2_disable_ioctl(vfd, VIDIOC_G_FREQUENCY);
+		v4l2_disable_ioctl(vfd, VIDIOC_S_FREQUENCY);
+		v4l2_disable_ioctl(vfd, VIDIOC_G_TUNER);
+		v4l2_disable_ioctl(vfd, VIDIOC_S_TUNER);
+	}
 	return vfd;
 }
 
@@ -2590,7 +2294,7 @@ int cx231xx_register_analog_devices(struct cx231xx *dev)
 		     dev->name, CX231XX_VERSION);
 
 	/* set default norm */
-	/*dev->norm = cx231xx_video_template.current_norm; */
+	dev->norm = V4L2_STD_PAL;
 	dev->width = norm_maxw(dev);
 	dev->height = norm_maxh(dev);
 	dev->interlaced = 0;
@@ -2601,9 +2305,23 @@ int cx231xx_register_analog_devices(struct cx231xx *dev)
 	/* Set the initial input */
 	video_mux(dev, dev->video_input);
 
-	/* Audio defaults */
-	dev->mute = 1;
-	dev->volume = 0x1f;
+	call_all(dev, core, s_std, dev->norm);
+
+	v4l2_ctrl_handler_init(&dev->ctrl_handler, 10);
+	v4l2_ctrl_handler_init(&dev->radio_ctrl_handler, 5);
+
+	if (dev->sd_cx25840) {
+		v4l2_ctrl_add_handler(&dev->ctrl_handler,
+				dev->sd_cx25840->ctrl_handler, NULL);
+		v4l2_ctrl_add_handler(&dev->radio_ctrl_handler,
+				dev->sd_cx25840->ctrl_handler,
+				v4l2_ctrl_radio_filter);
+	}
+
+	if (dev->ctrl_handler.error)
+		return dev->ctrl_handler.error;
+	if (dev->radio_ctrl_handler.error)
+		return dev->radio_ctrl_handler.error;
 
 	/* enable vbi capturing */
 	/* write code here...  */
@@ -2615,6 +2333,7 @@ int cx231xx_register_analog_devices(struct cx231xx *dev)
 		return -ENODEV;
 	}
 
+	dev->vdev->ctrl_handler = &dev->ctrl_handler;
 	/* register v4l2 video video_device */
 	ret = video_register_device(dev->vdev, VFL_TYPE_GRABBER,
 				    video_nr[dev->devno]);
@@ -2634,6 +2353,11 @@ int cx231xx_register_analog_devices(struct cx231xx *dev)
 	/* Allocate and fill vbi video_device struct */
 	dev->vbi_dev = cx231xx_vdev_init(dev, &cx231xx_vbi_template, "vbi");
 
+	if (!dev->vbi_dev) {
+		cx231xx_errdev("cannot allocate video_device.\n");
+		return -ENODEV;
+	}
+	dev->vbi_dev->ctrl_handler = &dev->ctrl_handler;
 	/* register v4l2 vbi video_device */
 	ret = video_register_device(dev->vbi_dev, VFL_TYPE_VBI,
 				    vbi_nr[dev->devno]);
@@ -2652,6 +2376,7 @@ int cx231xx_register_analog_devices(struct cx231xx *dev)
 			cx231xx_errdev("cannot allocate video_device.\n");
 			return -ENODEV;
 		}
+		dev->radio_dev->ctrl_handler = &dev->radio_ctrl_handler;
 		ret = video_register_device(dev->radio_dev, VFL_TYPE_RADIO,
 					    radio_nr[dev->devno]);
 		if (ret < 0) {
diff --git a/drivers/media/usb/cx231xx/cx231xx.h b/drivers/media/usb/cx231xx/cx231xx.h
index 3e11462be0d0..5ad9fd61d3c8 100644
--- a/drivers/media/usb/cx231xx/cx231xx.h
+++ b/drivers/media/usb/cx231xx/cx231xx.h
@@ -33,6 +33,8 @@
 
 #include <media/videobuf-vmalloc.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
 #include <media/rc-core.h>
 #include <media/ir-kbd-i2c.h>
 #include <media/videobuf-dvb.h>
@@ -69,6 +71,7 @@
 #define CX231XX_BOARD_HAUPPAUGE_USB2_FM_PAL 14
 #define CX231XX_BOARD_HAUPPAUGE_USB2_FM_NTSC 15
 #define CX231XX_BOARD_ELGATO_VIDEO_CAPTURE_V2 16
+#define CX231XX_BOARD_OTG102 17
 
 /* Limits minimum and default number of buffers */
 #define CX231XX_MIN_BUF                 4
@@ -428,27 +431,12 @@ struct cx231xx_audio {
 struct cx231xx;
 
 struct cx231xx_fh {
+	struct v4l2_fh fh;
 	struct cx231xx *dev;
 	unsigned int stream_on:1;	/* Locks streams */
-	int radio;
-
-	struct videobuf_queue vb_vidq;
-
 	enum v4l2_buf_type type;
 
-
-
-/*following is copyed from cx23885.h*/
-	u32                        resources;
-
-	/* video overlay */
-	struct v4l2_window         win;
-	struct v4l2_clip           *clips;
-	unsigned int               nclips;
-
-	/* video capture */
-	struct cx23417_fmt         *fmt;
-	unsigned int               width, height;
+	struct videobuf_queue vb_vidq;
 
 	/* vbi capture */
 	struct videobuf_queue      vidq;
@@ -516,14 +504,6 @@ struct cx231xx_tvnorm {
 	u32		cxoformat;
 };
 
-struct cx231xx_ctrl {
-	struct v4l2_queryctrl v;
-	u32 off;
-	u32 reg;
-	u32 mask;
-	u32 shift;
-};
-
 enum TRANSFER_TYPE {
 	Raw_Video = 0,
 	Audio,
@@ -631,6 +611,9 @@ struct cx231xx {
 	struct v4l2_device v4l2_dev;
 	struct v4l2_subdev *sd_cx25840;
 	struct v4l2_subdev *sd_tuner;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl_handler radio_ctrl_handler;
+	struct cx2341x_handler mpeg_ctrl_handler;
 
 	struct work_struct wq_trigger;		/* Trigger to start/stop audio for alsa module */
 	atomic_t	   stream_started;	/* stream should be running if true */
@@ -653,8 +636,6 @@ struct cx231xx {
 	v4l2_std_id norm;	/* selected tv norm */
 	int ctl_freq;		/* selected frequency */
 	unsigned int ctl_ainput;	/* selected audio input */
-	int mute;
-	int volume;
 
 	/* frame properties */
 	int width;		/* current frame width */
@@ -736,7 +717,6 @@ struct cx231xx {
 	u8 USE_ISO;
 	struct cx231xx_tvnorm      encodernorm;
 	struct cx231xx_tsport      ts1, ts2;
-	struct cx2341x_mpeg_params mpeg_params;
 	struct video_device        *v4l_device;
 	atomic_t                   v4l_reader_count;
 	u32                        freq;
@@ -866,8 +846,6 @@ int cx231xx_send_usb_command(struct cx231xx_i2c *i2c_bus,
 /* Gpio related functions */
 int cx231xx_send_gpio_cmd(struct cx231xx *dev, u32 gpio_bit, u8 *gpio_val,
 			  u8 len, u8 request, u8 direction);
-int cx231xx_set_gpio_bit(struct cx231xx *dev, u32 gpio_bit, u8 *gpio_val);
-int cx231xx_get_gpio_bit(struct cx231xx *dev, u32 gpio_bit, u8 *gpio_val);
 int cx231xx_set_gpio_value(struct cx231xx *dev, int pin_number, int pin_value);
 int cx231xx_set_gpio_direction(struct cx231xx *dev, int pin_number,
 			       int pin_value);
@@ -955,6 +933,23 @@ int cx231xx_register_extension(struct cx231xx_ops *dev);
 void cx231xx_unregister_extension(struct cx231xx_ops *dev);
 void cx231xx_init_extension(struct cx231xx *dev);
 void cx231xx_close_extension(struct cx231xx *dev);
+int cx231xx_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap);
+int cx231xx_g_tuner(struct file *file, void *priv, struct v4l2_tuner *t);
+int cx231xx_s_tuner(struct file *file, void *priv, const struct v4l2_tuner *t);
+int cx231xx_g_frequency(struct file *file, void *priv,
+			      struct v4l2_frequency *f);
+int cx231xx_s_frequency(struct file *file, void *priv,
+			      const struct v4l2_frequency *f);
+int cx231xx_enum_input(struct file *file, void *priv,
+			     struct v4l2_input *i);
+int cx231xx_g_input(struct file *file, void *priv, unsigned int *i);
+int cx231xx_s_input(struct file *file, void *priv, unsigned int i);
+int cx231xx_g_chip_ident(struct file *file, void *fh, struct v4l2_dbg_chip_ident *chip);
+int cx231xx_g_register(struct file *file, void *priv,
+			     struct v4l2_dbg_register *reg);
+int cx231xx_s_register(struct file *file, void *priv,
+			     const struct v4l2_dbg_register *reg);
 
 /* Provided by cx231xx-cards.c */
 extern void cx231xx_pre_card_setup(struct cx231xx *dev);
diff --git a/drivers/media/usb/dvb-usb-v2/Kconfig b/drivers/media/usb/dvb-usb-v2/Kconfig
index 692224d97d06..a3c8ecf22078 100644
--- a/drivers/media/usb/dvb-usb-v2/Kconfig
+++ b/drivers/media/usb/dvb-usb-v2/Kconfig
@@ -13,10 +13,6 @@ config DVB_USB_V2
 
 	  Say Y if you own a USB DVB device.
 
-config DVB_USB_CYPRESS_FIRMWARE
-	tristate "Cypress firmware helper routines"
-	depends on DVB_USB_V2
-
 config DVB_USB_AF9015
 	tristate "Afatech AF9015 DVB-T USB2.0 support"
 	depends on DVB_USB_V2
@@ -41,6 +37,7 @@ config DVB_USB_AF9035
 	select MEDIA_TUNER_MXL5007T if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_TDA18218 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_FC2580 if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_IT913X if MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Say Y here to support the Afatech AF9035 based DVB USB receiver.
 
@@ -72,7 +69,7 @@ config DVB_USB_AU6610
 config DVB_USB_AZ6007
 	tristate "AzureWave 6007 and clones DVB-T/C USB2.0 support"
 	depends on DVB_USB_V2
-	select DVB_USB_CYPRESS_FIRMWARE
+	select CYPRESS_FIRMWARE
 	select DVB_DRXK if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_MT2063 if MEDIA_SUBDRV_AUTOSELECT
 	help
@@ -146,6 +143,7 @@ config DVB_USB_RTL28XXU
 	select MEDIA_TUNER_FC0013 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_E4000 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_FC2580 if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_R820T if MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Say Y here to support the Realtek RTL28xxU DVB USB receiver.
 
diff --git a/drivers/media/usb/dvb-usb-v2/Makefile b/drivers/media/usb/dvb-usb-v2/Makefile
index b76f58e6c64f..2c06714b9ef0 100644
--- a/drivers/media/usb/dvb-usb-v2/Makefile
+++ b/drivers/media/usb/dvb-usb-v2/Makefile
@@ -1,9 +1,6 @@
 dvb_usb_v2-objs := dvb_usb_core.o dvb_usb_urb.o usb_urb.o
 obj-$(CONFIG_DVB_USB_V2) += dvb_usb_v2.o
 
-dvb_usb_cypress_firmware-objs := cypress_firmware.o
-obj-$(CONFIG_DVB_USB_CYPRESS_FIRMWARE) += dvb_usb_cypress_firmware.o
-
 dvb-usb-af9015-objs := af9015.o
 obj-$(CONFIG_DVB_USB_AF9015) += dvb-usb-af9015.o
 
@@ -46,4 +43,4 @@ obj-$(CONFIG_DVB_USB_RTL28XXU) += dvb-usb-rtl28xxu.o
 ccflags-y += -I$(srctree)/drivers/media/dvb-core
 ccflags-y += -I$(srctree)/drivers/media/dvb-frontends
 ccflags-y += -I$(srctree)/drivers/media/tuners
-
+ccflags-y += -I$(srctree)/drivers/media/common
diff --git a/drivers/media/usb/dvb-usb-v2/af9015.c b/drivers/media/usb/dvb-usb-v2/af9015.c
index b86d0f27a398..d556042cf312 100644
--- a/drivers/media/usb/dvb-usb-v2/af9015.c
+++ b/drivers/media/usb/dvb-usb-v2/af9015.c
@@ -30,22 +30,22 @@ DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
 static int af9015_ctrl_msg(struct dvb_usb_device *d, struct req_t *req)
 {
-#define BUF_LEN 63
 #define REQ_HDR_LEN 8 /* send header size */
 #define ACK_HDR_LEN 2 /* rece header size */
 	struct af9015_state *state = d_to_priv(d);
 	int ret, wlen, rlen;
-	u8 buf[BUF_LEN];
 	u8 write = 1;
 
-	buf[0] = req->cmd;
-	buf[1] = state->seq++;
-	buf[2] = req->i2c_addr;
-	buf[3] = req->addr >> 8;
-	buf[4] = req->addr & 0xff;
-	buf[5] = req->mbox;
-	buf[6] = req->addr_len;
-	buf[7] = req->data_len;
+	mutex_lock(&d->usb_mutex);
+
+	state->buf[0] = req->cmd;
+	state->buf[1] = state->seq++;
+	state->buf[2] = req->i2c_addr;
+	state->buf[3] = req->addr >> 8;
+	state->buf[4] = req->addr & 0xff;
+	state->buf[5] = req->mbox;
+	state->buf[6] = req->addr_len;
+	state->buf[7] = req->data_len;
 
 	switch (req->cmd) {
 	case GET_CONFIG:
@@ -55,14 +55,14 @@ static int af9015_ctrl_msg(struct dvb_usb_device *d, struct req_t *req)
 		break;
 	case READ_I2C:
 		write = 0;
-		buf[2] |= 0x01; /* set I2C direction */
+		state->buf[2] |= 0x01; /* set I2C direction */
 	case WRITE_I2C:
-		buf[0] = READ_WRITE_I2C;
+		state->buf[0] = READ_WRITE_I2C;
 		break;
 	case WRITE_MEMORY:
 		if (((req->addr & 0xff00) == 0xff00) ||
 		    ((req->addr & 0xff00) == 0xae00))
-			buf[0] = WRITE_VIRTUAL_MEMORY;
+			state->buf[0] = WRITE_VIRTUAL_MEMORY;
 	case WRITE_VIRTUAL_MEMORY:
 	case COPY_FIRMWARE:
 	case DOWNLOAD_FIRMWARE:
@@ -90,7 +90,7 @@ static int af9015_ctrl_msg(struct dvb_usb_device *d, struct req_t *req)
 	rlen = ACK_HDR_LEN;
 	if (write) {
 		wlen += req->data_len;
-		memcpy(&buf[REQ_HDR_LEN], req->data, req->data_len);
+		memcpy(&state->buf[REQ_HDR_LEN], req->data, req->data_len);
 	} else {
 		rlen += req->data_len;
 	}
@@ -99,22 +99,25 @@ static int af9015_ctrl_msg(struct dvb_usb_device *d, struct req_t *req)
 	if (req->cmd == DOWNLOAD_FIRMWARE || req->cmd == RECONNECT_USB)
 		rlen = 0;
 
-	ret = dvb_usbv2_generic_rw(d, buf, wlen, buf, rlen);
+	ret = dvb_usbv2_generic_rw_locked(d,
+			state->buf, wlen, state->buf, rlen);
 	if (ret)
 		goto error;
 
 	/* check status */
-	if (rlen && buf[1]) {
+	if (rlen && state->buf[1]) {
 		dev_err(&d->udev->dev, "%s: command failed=%d\n",
-				KBUILD_MODNAME, buf[1]);
+				KBUILD_MODNAME, state->buf[1]);
 		ret = -EIO;
 		goto error;
 	}
 
 	/* read request, copy returned data to return buf */
 	if (!write)
-		memcpy(req->data, &buf[ACK_HDR_LEN], req->data_len);
+		memcpy(req->data, &state->buf[ACK_HDR_LEN], req->data_len);
 error:
+	mutex_unlock(&d->usb_mutex);
+
 	return ret;
 }
 
@@ -1317,6 +1320,43 @@ static int af9015_get_rc_config(struct dvb_usb_device *d, struct dvb_usb_rc *rc)
 	#define af9015_get_rc_config NULL
 #endif
 
+static int af9015_probe(struct usb_interface *intf,
+		const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	char manufacturer[sizeof("ITE Technologies, Inc.")];
+
+	memset(manufacturer, 0, sizeof(manufacturer));
+	usb_string(udev, udev->descriptor.iManufacturer,
+			manufacturer, sizeof(manufacturer));
+	/*
+	 * There is two devices having same ID but different chipset. One uses
+	 * AF9015 and the other IT9135 chipset. Only difference seen on lsusb
+	 * is iManufacturer string.
+	 *
+	 * idVendor           0x0ccd TerraTec Electronic GmbH
+	 * idProduct          0x0099
+	 * bcdDevice            2.00
+	 * iManufacturer           1 Afatech
+	 * iProduct                2 DVB-T 2
+	 *
+	 * idVendor           0x0ccd TerraTec Electronic GmbH
+	 * idProduct          0x0099
+	 * bcdDevice            2.00
+	 * iManufacturer           1 ITE Technologies, Inc.
+	 * iProduct                2 DVB-T TV Stick
+	 */
+	if ((le16_to_cpu(udev->descriptor.idVendor) == USB_VID_TERRATEC) &&
+			(le16_to_cpu(udev->descriptor.idProduct) == 0x0099)) {
+		if (!strcmp("ITE Technologies, Inc.", manufacturer)) {
+			dev_dbg(&udev->dev, "%s: rejecting device\n", __func__);
+			return -ENODEV;
+		}
+	}
+
+	return dvb_usbv2_probe(intf, id);
+}
+
 /* interface 0 is used by DVB-T receiver and
    interface 1 is for remote controller (HID) */
 static struct dvb_usb_device_properties af9015_props = {
@@ -1425,6 +1465,7 @@ static const struct usb_device_id af9015_id_table[] = {
 		&af9015_props, "AverMedia AVerTV Volar M (A815Mac)", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_TERRATEC, USB_PID_TERRATEC_CINERGY_T_STICK_RC,
 		&af9015_props, "TerraTec Cinergy T Stick RC", RC_MAP_TERRATEC_SLIM_2) },
+	/* XXX: that same ID [0ccd:0099] is used by af9035 driver too */
 	{ DVB_USB_DEVICE(USB_VID_TERRATEC, USB_PID_TERRATEC_CINERGY_T_STICK_DUAL_RC,
 		&af9015_props, "TerraTec Cinergy T Stick Dual RC", RC_MAP_TERRATEC_SLIM) },
 	{ DVB_USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_A850T,
@@ -1441,7 +1482,7 @@ MODULE_DEVICE_TABLE(usb, af9015_id_table);
 static struct usb_driver af9015_usb_driver = {
 	.name = KBUILD_MODNAME,
 	.id_table = af9015_id_table,
-	.probe = dvb_usbv2_probe,
+	.probe = af9015_probe,
 	.disconnect = dvb_usbv2_disconnect,
 	.suspend = dvb_usbv2_suspend,
 	.resume = dvb_usbv2_resume,
diff --git a/drivers/media/usb/dvb-usb-v2/af9015.h b/drivers/media/usb/dvb-usb-v2/af9015.h
index 533637dedd23..3a6f3ad1eadb 100644
--- a/drivers/media/usb/dvb-usb-v2/af9015.h
+++ b/drivers/media/usb/dvb-usb-v2/af9015.h
@@ -115,7 +115,9 @@ enum af9015_ir_mode {
 	AF9015_IR_MODE_POLLING, /* just guess */
 };
 
+#define BUF_LEN 63
 struct af9015_state {
+	u8 buf[BUF_LEN]; /* bulk USB control message */
 	u8 ir_mode;
 	u8 rc_repeat;
 	u32 rc_keycode;
diff --git a/drivers/media/usb/dvb-usb-v2/af9035.c b/drivers/media/usb/dvb-usb-v2/af9035.c
index f11cc42454f0..b638fc1cd574 100644
--- a/drivers/media/usb/dvb-usb-v2/af9035.c
+++ b/drivers/media/usb/dvb-usb-v2/af9035.c
@@ -41,79 +41,84 @@ static u16 af9035_checksum(const u8 *buf, size_t len)
 
 static int af9035_ctrl_msg(struct dvb_usb_device *d, struct usb_req *req)
 {
-#define BUF_LEN 64
 #define REQ_HDR_LEN 4 /* send header size */
 #define ACK_HDR_LEN 3 /* rece header size */
 #define CHECKSUM_LEN 2
 #define USB_TIMEOUT 2000
 	struct state *state = d_to_priv(d);
 	int ret, wlen, rlen;
-	u8 buf[BUF_LEN];
 	u16 checksum, tmp_checksum;
 
+	mutex_lock(&d->usb_mutex);
+
 	/* buffer overflow check */
 	if (req->wlen > (BUF_LEN - REQ_HDR_LEN - CHECKSUM_LEN) ||
 			req->rlen > (BUF_LEN - ACK_HDR_LEN - CHECKSUM_LEN)) {
 		dev_err(&d->udev->dev, "%s: too much data wlen=%d rlen=%d\n",
 				__func__, req->wlen, req->rlen);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto exit;
 	}
 
-	buf[0] = REQ_HDR_LEN + req->wlen + CHECKSUM_LEN - 1;
-	buf[1] = req->mbox;
-	buf[2] = req->cmd;
-	buf[3] = state->seq++;
-	memcpy(&buf[REQ_HDR_LEN], req->wbuf, req->wlen);
+	state->buf[0] = REQ_HDR_LEN + req->wlen + CHECKSUM_LEN - 1;
+	state->buf[1] = req->mbox;
+	state->buf[2] = req->cmd;
+	state->buf[3] = state->seq++;
+	memcpy(&state->buf[REQ_HDR_LEN], req->wbuf, req->wlen);
 
 	wlen = REQ_HDR_LEN + req->wlen + CHECKSUM_LEN;
 	rlen = ACK_HDR_LEN + req->rlen + CHECKSUM_LEN;
 
 	/* calc and add checksum */
-	checksum = af9035_checksum(buf, buf[0] - 1);
-	buf[buf[0] - 1] = (checksum >> 8);
-	buf[buf[0] - 0] = (checksum & 0xff);
+	checksum = af9035_checksum(state->buf, state->buf[0] - 1);
+	state->buf[state->buf[0] - 1] = (checksum >> 8);
+	state->buf[state->buf[0] - 0] = (checksum & 0xff);
 
 	/* no ack for these packets */
 	if (req->cmd == CMD_FW_DL)
 		rlen = 0;
 
-	ret = dvb_usbv2_generic_rw(d, buf, wlen, buf, rlen);
+	ret = dvb_usbv2_generic_rw_locked(d,
+			state->buf, wlen, state->buf, rlen);
 	if (ret)
-		goto err;
+		goto exit;
 
 	/* no ack for those packets */
 	if (req->cmd == CMD_FW_DL)
 		goto exit;
 
 	/* verify checksum */
-	checksum = af9035_checksum(buf, rlen - 2);
-	tmp_checksum = (buf[rlen - 2] << 8) | buf[rlen - 1];
+	checksum = af9035_checksum(state->buf, rlen - 2);
+	tmp_checksum = (state->buf[rlen - 2] << 8) | state->buf[rlen - 1];
 	if (tmp_checksum != checksum) {
 		dev_err(&d->udev->dev, "%s: command=%02x checksum mismatch " \
 				"(%04x != %04x)\n", KBUILD_MODNAME, req->cmd,
 				tmp_checksum, checksum);
 		ret = -EIO;
-		goto err;
+		goto exit;
 	}
 
 	/* check status */
-	if (buf[2]) {
+	if (state->buf[2]) {
+		/* fw returns status 1 when IR code was not received */
+		if (req->cmd == CMD_IR_GET || state->buf[2] == 1) {
+			ret = 1;
+			goto exit;
+		}
+
 		dev_dbg(&d->udev->dev, "%s: command=%02x failed fw error=%d\n",
-				__func__, req->cmd, buf[2]);
+				__func__, req->cmd, state->buf[2]);
 		ret = -EIO;
-		goto err;
+		goto exit;
 	}
 
 	/* read request, copy returned data to return buf */
 	if (req->rlen)
-		memcpy(req->rbuf, &buf[ACK_HDR_LEN], req->rlen);
-
+		memcpy(req->rbuf, &state->buf[ACK_HDR_LEN], req->rlen);
 exit:
-	return 0;
-
-err:
-	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
-
+	mutex_unlock(&d->usb_mutex);
+	if (ret < 0)
+		dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -292,12 +297,40 @@ static struct i2c_algorithm af9035_i2c_algo = {
 
 static int af9035_identify_state(struct dvb_usb_device *d, const char **name)
 {
+	struct state *state = d_to_priv(d);
 	int ret;
 	u8 wbuf[1] = { 1 };
 	u8 rbuf[4];
 	struct usb_req req = { CMD_FW_QUERYINFO, 0, sizeof(wbuf), wbuf,
 			sizeof(rbuf), rbuf };
 
+	ret = af9035_rd_regs(d, 0x1222, rbuf, 3);
+	if (ret < 0)
+		goto err;
+
+	state->chip_version = rbuf[0];
+	state->chip_type = rbuf[2] << 8 | rbuf[1] << 0;
+
+	ret = af9035_rd_reg(d, 0x384f, &state->prechip_version);
+	if (ret < 0)
+		goto err;
+
+	dev_info(&d->udev->dev,
+			"%s: prechip_version=%02x chip_version=%02x chip_type=%04x\n",
+			__func__, state->prechip_version, state->chip_version,
+			state->chip_type);
+
+	if (state->chip_type == 0x9135) {
+		if (state->chip_version == 0x02)
+			*name = AF9035_FIRMWARE_IT9135_V2;
+		else
+			*name = AF9035_FIRMWARE_IT9135_V1;
+		state->eeprom_addr = EEPROM_BASE_IT9135;
+	} else {
+		*name = AF9035_FIRMWARE_AF9035;
+		state->eeprom_addr = EEPROM_BASE_AF9035;
+	}
+
 	ret = af9035_ctrl_msg(d, &req);
 	if (ret < 0)
 		goto err;
@@ -316,66 +349,19 @@ err:
 	return ret;
 }
 
-static int af9035_download_firmware(struct dvb_usb_device *d,
+static int af9035_download_firmware_old(struct dvb_usb_device *d,
 		const struct firmware *fw)
 {
 	int ret, i, j, len;
 	u8 wbuf[1];
-	u8 rbuf[4];
 	struct usb_req req = { 0, 0, 0, NULL, 0, NULL };
 	struct usb_req req_fw_dl = { CMD_FW_DL, 0, 0, wbuf, 0, NULL };
-	struct usb_req req_fw_ver = { CMD_FW_QUERYINFO, 0, 1, wbuf, 4, rbuf } ;
-	u8 hdr_core, tmp;
+	u8 hdr_core;
 	u16 hdr_addr, hdr_data_len, hdr_checksum;
 	#define MAX_DATA 58
 	#define HDR_SIZE 7
 
 	/*
-	 * In case of dual tuner configuration we need to do some extra
-	 * initialization in order to download firmware to slave demod too,
-	 * which is done by master demod.
-	 * Master feeds also clock and controls power via GPIO.
-	 */
-	ret = af9035_rd_reg(d, EEPROM_DUAL_MODE, &tmp);
-	if (ret < 0)
-		goto err;
-
-	if (tmp) {
-		/* configure gpioh1, reset & power slave demod */
-		ret = af9035_wr_reg_mask(d, 0x00d8b0, 0x01, 0x01);
-		if (ret < 0)
-			goto err;
-
-		ret = af9035_wr_reg_mask(d, 0x00d8b1, 0x01, 0x01);
-		if (ret < 0)
-			goto err;
-
-		ret = af9035_wr_reg_mask(d, 0x00d8af, 0x00, 0x01);
-		if (ret < 0)
-			goto err;
-
-		usleep_range(10000, 50000);
-
-		ret = af9035_wr_reg_mask(d, 0x00d8af, 0x01, 0x01);
-		if (ret < 0)
-			goto err;
-
-		/* tell the slave I2C address */
-		ret = af9035_rd_reg(d, EEPROM_2ND_DEMOD_ADDR, &tmp);
-		if (ret < 0)
-			goto err;
-
-		ret = af9035_wr_reg(d, 0x00417f, tmp);
-		if (ret < 0)
-			goto err;
-
-		/* enable clock out */
-		ret = af9035_wr_reg_mask(d, 0x00d81a, 0x01, 0x01);
-		if (ret < 0)
-			goto err;
-	}
-
-	/*
 	 * Thanks to Daniel Glöckner <daniel-gl@gmx.net> about that info!
 	 *
 	 * byte 0: MCS 51 core
@@ -441,28 +427,6 @@ static int af9035_download_firmware(struct dvb_usb_device *d,
 	if (i)
 		dev_warn(&d->udev->dev, "%s: bad firmware\n", KBUILD_MODNAME);
 
-	/* firmware loaded, request boot */
-	req.cmd = CMD_FW_BOOT;
-	ret = af9035_ctrl_msg(d, &req);
-	if (ret < 0)
-		goto err;
-
-	/* ensure firmware starts */
-	wbuf[0] = 1;
-	ret = af9035_ctrl_msg(d, &req_fw_ver);
-	if (ret < 0)
-		goto err;
-
-	if (!(rbuf[0] || rbuf[1] || rbuf[2] || rbuf[3])) {
-		dev_err(&d->udev->dev, "%s: firmware did not run\n",
-				KBUILD_MODNAME);
-		ret = -ENODEV;
-		goto err;
-	}
-
-	dev_info(&d->udev->dev, "%s: firmware version=%d.%d.%d.%d",
-			KBUILD_MODNAME, rbuf[0], rbuf[1], rbuf[2], rbuf[3]);
-
 	return 0;
 
 err:
@@ -471,15 +435,11 @@ err:
 	return ret;
 }
 
-static int af9035_download_firmware_it9135(struct dvb_usb_device *d,
+static int af9035_download_firmware_new(struct dvb_usb_device *d,
 		const struct firmware *fw)
 {
 	int ret, i, i_prev;
-	u8 wbuf[1];
-	u8 rbuf[4];
-	struct usb_req req = { 0, 0, 0, NULL, 0, NULL };
 	struct usb_req req_fw_dl = { CMD_FW_SCATTER_WR, 0, 0, NULL, 0, NULL };
-	struct usb_req req_fw_ver = { CMD_FW_QUERYINFO, 0, 1, wbuf, 4, rbuf } ;
 	#define HDR_SIZE 7
 
 	/*
@@ -494,7 +454,6 @@ static int af9035_download_firmware_it9135(struct dvb_usb_device *d,
 	 * 5: addr LSB
 	 * 6: count of data bytes ?
 	 */
-
 	for (i = HDR_SIZE, i_prev = 0; i <= fw->size; i++) {
 		if (i == fw->size ||
 				(fw->data[i + 0] == 0x03 &&
@@ -513,6 +472,86 @@ static int af9035_download_firmware_it9135(struct dvb_usb_device *d,
 		}
 	}
 
+	return 0;
+
+err:
+	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_download_firmware(struct dvb_usb_device *d,
+		const struct firmware *fw)
+{
+	struct state *state = d_to_priv(d);
+	int ret;
+	u8 wbuf[1];
+	u8 rbuf[4];
+	u8 tmp;
+	struct usb_req req = { 0, 0, 0, NULL, 0, NULL };
+	struct usb_req req_fw_ver = { CMD_FW_QUERYINFO, 0, 1, wbuf, 4, rbuf } ;
+	dev_dbg(&d->udev->dev, "%s:\n", __func__);
+
+	/*
+	 * In case of dual tuner configuration we need to do some extra
+	 * initialization in order to download firmware to slave demod too,
+	 * which is done by master demod.
+	 * Master feeds also clock and controls power via GPIO.
+	 */
+	ret = af9035_rd_reg(d, state->eeprom_addr + EEPROM_DUAL_MODE, &tmp);
+	if (ret < 0)
+		goto err;
+
+	if (tmp) {
+		/* configure gpioh1, reset & power slave demod */
+		ret = af9035_wr_reg_mask(d, 0x00d8b0, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(d, 0x00d8b1, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		ret = af9035_wr_reg_mask(d, 0x00d8af, 0x00, 0x01);
+		if (ret < 0)
+			goto err;
+
+		usleep_range(10000, 50000);
+
+		ret = af9035_wr_reg_mask(d, 0x00d8af, 0x01, 0x01);
+		if (ret < 0)
+			goto err;
+
+		/* tell the slave I2C address */
+		ret = af9035_rd_reg(d,
+				state->eeprom_addr + EEPROM_2ND_DEMOD_ADDR,
+				&tmp);
+		if (ret < 0)
+			goto err;
+
+		if (state->chip_type == 0x9135) {
+			ret = af9035_wr_reg(d, 0x004bfb, tmp);
+			if (ret < 0)
+				goto err;
+		} else {
+			ret = af9035_wr_reg(d, 0x00417f, tmp);
+			if (ret < 0)
+				goto err;
+
+			/* enable clock out */
+			ret = af9035_wr_reg_mask(d, 0x00d81a, 0x01, 0x01);
+			if (ret < 0)
+				goto err;
+		}
+	}
+
+	if (fw->data[0] == 0x01)
+		ret = af9035_download_firmware_old(d, fw);
+	else
+		ret = af9035_download_firmware_new(d, fw);
+	if (ret < 0)
+		goto err;
+
 	/* firmware loaded, request boot */
 	req.cmd = CMD_FW_BOOT;
 	ret = af9035_ctrl_msg(d, &req);
@@ -546,15 +585,42 @@ err:
 static int af9035_read_config(struct dvb_usb_device *d)
 {
 	struct state *state = d_to_priv(d);
-	int ret, i, eeprom_shift = 0;
+	int ret, i;
 	u8 tmp;
-	u16 tmp16;
+	u16 tmp16, addr;
 
 	/* demod I2C "address" */
 	state->af9033_config[0].i2c_addr = 0x38;
+	state->af9033_config[0].adc_multiplier = AF9033_ADC_MULTIPLIER_2X;
+	state->af9033_config[1].adc_multiplier = AF9033_ADC_MULTIPLIER_2X;
+	state->af9033_config[0].ts_mode = AF9033_TS_MODE_USB;
+	state->af9033_config[1].ts_mode = AF9033_TS_MODE_SERIAL;
+
+	/* eeprom memory mapped location */
+	if (state->chip_type == 0x9135) {
+		if (state->chip_version == 0x02) {
+			state->af9033_config[0].tuner = AF9033_TUNER_IT9135_60;
+			state->af9033_config[1].tuner = AF9033_TUNER_IT9135_60;
+			tmp16 = 0x00461d;
+		} else {
+			state->af9033_config[0].tuner = AF9033_TUNER_IT9135_38;
+			state->af9033_config[1].tuner = AF9033_TUNER_IT9135_38;
+			tmp16 = 0x00461b;
+		}
+
+		/* check if eeprom exists */
+		ret = af9035_rd_reg(d, tmp16, &tmp);
+		if (ret < 0)
+			goto err;
+
+		if (tmp == 0x00) {
+			dev_dbg(&d->udev->dev, "%s: no eeprom\n", __func__);
+			goto skip_eeprom;
+		}
+	}
 
 	/* check if there is dual tuners */
-	ret = af9035_rd_reg(d, EEPROM_DUAL_MODE, &tmp);
+	ret = af9035_rd_reg(d, state->eeprom_addr + EEPROM_DUAL_MODE, &tmp);
 	if (ret < 0)
 		goto err;
 
@@ -564,7 +630,9 @@ static int af9035_read_config(struct dvb_usb_device *d)
 
 	if (state->dual_mode) {
 		/* read 2nd demodulator I2C address */
-		ret = af9035_rd_reg(d, EEPROM_2ND_DEMOD_ADDR, &tmp);
+		ret = af9035_rd_reg(d,
+				state->eeprom_addr + EEPROM_2ND_DEMOD_ADDR,
+				&tmp);
 		if (ret < 0)
 			goto err;
 
@@ -573,17 +641,25 @@ static int af9035_read_config(struct dvb_usb_device *d)
 				__func__, tmp);
 	}
 
+	addr = state->eeprom_addr;
+
 	for (i = 0; i < state->dual_mode + 1; i++) {
 		/* tuner */
-		ret = af9035_rd_reg(d, EEPROM_1_TUNER_ID + eeprom_shift, &tmp);
+		ret = af9035_rd_reg(d, addr + EEPROM_1_TUNER_ID, &tmp);
 		if (ret < 0)
 			goto err;
 
-		state->af9033_config[i].tuner = tmp;
+		if (tmp == 0x00)
+			dev_dbg(&d->udev->dev,
+					"%s: [%d]tuner not set, using default\n",
+					__func__, i);
+		else
+			state->af9033_config[i].tuner = tmp;
+
 		dev_dbg(&d->udev->dev, "%s: [%d]tuner=%02x\n",
-				__func__, i, tmp);
+				__func__, i, state->af9033_config[i].tuner);
 
-		switch (tmp) {
+		switch (state->af9033_config[i].tuner) {
 		case AF9033_TUNER_TUA9001:
 		case AF9033_TUNER_FC0011:
 		case AF9033_TUNER_MXL5007T:
@@ -592,32 +668,46 @@ static int af9035_read_config(struct dvb_usb_device *d)
 		case AF9033_TUNER_FC0012:
 			state->af9033_config[i].spec_inv = 1;
 			break;
+		case AF9033_TUNER_IT9135_38:
+		case AF9033_TUNER_IT9135_51:
+		case AF9033_TUNER_IT9135_52:
+		case AF9033_TUNER_IT9135_60:
+		case AF9033_TUNER_IT9135_61:
+		case AF9033_TUNER_IT9135_62:
+			break;
 		default:
-			dev_warn(&d->udev->dev, "%s: tuner id=%02x not " \
-					"supported, please report!",
+			dev_warn(&d->udev->dev,
+					"%s: tuner id=%02x not supported, please report!",
 					KBUILD_MODNAME, tmp);
 		}
 
 		/* disable dual mode if driver does not support it */
 		if (i == 1)
-			switch (tmp) {
+			switch (state->af9033_config[i].tuner) {
 			case AF9033_TUNER_FC0012:
+			case AF9033_TUNER_IT9135_38:
+			case AF9033_TUNER_IT9135_51:
+			case AF9033_TUNER_IT9135_52:
+			case AF9033_TUNER_IT9135_60:
+			case AF9033_TUNER_IT9135_61:
+			case AF9033_TUNER_IT9135_62:
+			case AF9033_TUNER_MXL5007T:
 				break;
 			default:
 				state->dual_mode = false;
-				dev_info(&d->udev->dev, "%s: driver does not " \
-						"support 2nd tuner and will " \
-						"disable it", KBUILD_MODNAME);
+				dev_info(&d->udev->dev,
+						"%s: driver does not support 2nd tuner and will disable it",
+						KBUILD_MODNAME);
 		}
 
 		/* tuner IF frequency */
-		ret = af9035_rd_reg(d, EEPROM_1_IFFREQ_L + eeprom_shift, &tmp);
+		ret = af9035_rd_reg(d, addr + EEPROM_1_IF_L, &tmp);
 		if (ret < 0)
 			goto err;
 
 		tmp16 = tmp;
 
-		ret = af9035_rd_reg(d, EEPROM_1_IFFREQ_H + eeprom_shift, &tmp);
+		ret = af9035_rd_reg(d, addr + EEPROM_1_IF_H, &tmp);
 		if (ret < 0)
 			goto err;
 
@@ -625,9 +715,10 @@ static int af9035_read_config(struct dvb_usb_device *d)
 
 		dev_dbg(&d->udev->dev, "%s: [%d]IF=%d\n", __func__, i, tmp16);
 
-		eeprom_shift = 0x10; /* shift for the 2nd tuner params */
+		addr += 0x10; /* shift for the 2nd tuner params */
 	}
 
+skip_eeprom:
 	/* get demod clock */
 	ret = af9035_rd_reg(d, 0x00d800, &tmp);
 	if (ret < 0)
@@ -635,34 +726,12 @@ static int af9035_read_config(struct dvb_usb_device *d)
 
 	tmp = (tmp >> 0) & 0x0f;
 
-	for (i = 0; i < ARRAY_SIZE(state->af9033_config); i++)
-		state->af9033_config[i].clock = clock_lut[tmp];
-
-	return 0;
-
-err:
-	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
-
-	return ret;
-}
-
-static int af9035_read_config_it9135(struct dvb_usb_device *d)
-{
-	struct state *state = d_to_priv(d);
-	int ret, i;
-	u8 tmp;
-
-	state->dual_mode = false;
-
-	/* get demod clock */
-	ret = af9035_rd_reg(d, 0x00d800, &tmp);
-	if (ret < 0)
-		goto err;
-
-	tmp = (tmp >> 0) & 0x0f;
-
-	for (i = 0; i < ARRAY_SIZE(state->af9033_config); i++)
-		state->af9033_config[i].clock = clock_lut_it9135[tmp];
+	for (i = 0; i < ARRAY_SIZE(state->af9033_config); i++) {
+		if (state->chip_type == 0x9135)
+			state->af9033_config[i].clock = clock_lut_it9135[tmp];
+		else
+			state->af9033_config[i].clock = clock_lut_af9035[tmp];
+	}
 
 	return 0;
 
@@ -821,7 +890,12 @@ static int af9035_frontend_callback(void *adapter_priv, int component,
 static int af9035_get_adapter_count(struct dvb_usb_device *d)
 {
 	struct state *state = d_to_priv(d);
-	return state->dual_mode + 1;
+
+	/* disable 2nd adapter as we don't have PID filters implemented */
+	if (d->udev->speed == USB_SPEED_FULL)
+		return 1;
+	else
+		return state->dual_mode + 1;
 }
 
 static int af9035_frontend_attach(struct dvb_usb_adapter *adap)
@@ -829,6 +903,7 @@ static int af9035_frontend_attach(struct dvb_usb_adapter *adap)
 	struct state *state = adap_to_priv(adap);
 	struct dvb_usb_device *d = adap_to_d(adap);
 	int ret;
+	dev_dbg(&d->udev->dev, "%s:\n", __func__);
 
 	if (!state->af9033_config[adap->id].tuner) {
 		/* unsupported tuner */
@@ -836,20 +911,6 @@ static int af9035_frontend_attach(struct dvb_usb_adapter *adap)
 		goto err;
 	}
 
-	if (adap->id == 0) {
-		state->af9033_config[0].ts_mode = AF9033_TS_MODE_USB;
-		state->af9033_config[1].ts_mode = AF9033_TS_MODE_SERIAL;
-
-		ret = af9035_wr_reg(d, 0x00417f,
-				state->af9033_config[1].i2c_addr);
-		if (ret < 0)
-			goto err;
-
-		ret = af9035_wr_reg(d, 0x00d81a, state->dual_mode);
-		if (ret < 0)
-			goto err;
-	}
-
 	/* attach demodulator */
 	adap->fe[0] = dvb_attach(af9033_attach, &state->af9033_config[adap->id],
 			&d->i2c_adap);
@@ -928,6 +989,8 @@ static int af9035_tuner_attach(struct dvb_usb_adapter *adap)
 	struct dvb_frontend *fe;
 	struct i2c_msg msg[1];
 	u8 tuner_addr;
+	dev_dbg(&d->udev->dev, "%s:\n", __func__);
+
 	/*
 	 * XXX: Hack used in that function: we abuse unused I2C address bit [7]
 	 * to carry info about used I2C bus for dual tuner configuration.
@@ -1082,6 +1145,17 @@ static int af9035_tuner_attach(struct dvb_usb_adapter *adap)
 		fe = dvb_attach(fc0012_attach, adap->fe[0], &d->i2c_adap,
 				&af9035_fc0012_config[adap->id]);
 		break;
+	case AF9033_TUNER_IT9135_38:
+	case AF9033_TUNER_IT9135_51:
+	case AF9033_TUNER_IT9135_52:
+	case AF9033_TUNER_IT9135_60:
+	case AF9033_TUNER_IT9135_61:
+	case AF9033_TUNER_IT9135_62:
+		/* attach tuner */
+		fe = dvb_attach(it913x_attach, adap->fe[0], &d->i2c_adap,
+				state->af9033_config[adap->id].i2c_addr,
+				state->af9033_config[0].tuner);
+		break;
 	default:
 		fe = NULL;
 	}
@@ -1103,8 +1177,8 @@ static int af9035_init(struct dvb_usb_device *d)
 {
 	struct state *state = d_to_priv(d);
 	int ret, i;
-	u16 frame_size = 87 * 188 / 4;
-	u8  packet_size = 512 / 4;
+	u16 frame_size = (d->udev->speed == USB_SPEED_FULL ? 5 : 87) * 188 / 4;
+	u8 packet_size = (d->udev->speed == USB_SPEED_FULL ? 64 : 512) / 4;
 	struct reg_val_mask tab[] = {
 		{ 0x80f99d, 0x01, 0x01 },
 		{ 0x80f9a4, 0x01, 0x01 },
@@ -1149,40 +1223,49 @@ err:
 #if IS_ENABLED(CONFIG_RC_CORE)
 static int af9035_rc_query(struct dvb_usb_device *d)
 {
-	unsigned int key;
-	unsigned char b[4];
 	int ret;
-	struct usb_req req = { CMD_IR_GET, 0, 0, NULL, 4, b };
+	u32 key;
+	u8 buf[4];
+	struct usb_req req = { CMD_IR_GET, 0, 0, NULL, 4, buf };
 
 	ret = af9035_ctrl_msg(d, &req);
-	if (ret < 0)
+	if (ret == 1)
+		return 0;
+	else if (ret < 0)
 		goto err;
 
-	if ((b[2] + b[3]) == 0xff) {
-		if ((b[0] + b[1]) == 0xff) {
-			/* NEC */
-			key = b[0] << 8 | b[2];
+	if ((buf[2] + buf[3]) == 0xff) {
+		if ((buf[0] + buf[1]) == 0xff) {
+			/* NEC standard 16bit */
+			key = buf[0] << 8 | buf[2];
 		} else {
-			/* ext. NEC */
-			key = b[0] << 16 | b[1] << 8 | b[2];
+			/* NEC extended 24bit */
+			key = buf[0] << 16 | buf[1] << 8 | buf[2];
 		}
 	} else {
-		key = b[0] << 24 | b[1] << 16 | b[2] << 8 | b[3];
+		/* NEC full code 32bit */
+		key = buf[0] << 24 | buf[1] << 16 | buf[2] << 8 | buf[3];
 	}
 
+	dev_dbg(&d->udev->dev, "%s: %*ph\n", __func__, 4, buf);
+
 	rc_keydown(d->rc_dev, key, 0);
 
-err:
-	/* ignore errors */
 	return 0;
+
+err:
+	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
 }
 
 static int af9035_get_rc_config(struct dvb_usb_device *d, struct dvb_usb_rc *rc)
 {
+	struct state *state = d_to_priv(d);
 	int ret;
 	u8 tmp;
 
-	ret = af9035_rd_reg(d, EEPROM_IR_MODE, &tmp);
+	ret = af9035_rd_reg(d, state->eeprom_addr + EEPROM_IR_MODE, &tmp);
 	if (ret < 0)
 		goto err;
 
@@ -1190,7 +1273,8 @@ static int af9035_get_rc_config(struct dvb_usb_device *d, struct dvb_usb_rc *rc)
 
 	/* don't activate rc if in HID mode or if not available */
 	if (tmp == 5) {
-		ret = af9035_rd_reg(d, EEPROM_IR_TYPE, &tmp);
+		ret = af9035_rd_reg(d, state->eeprom_addr + EEPROM_IR_TYPE,
+				&tmp);
 		if (ret < 0)
 			goto err;
 
@@ -1225,6 +1309,109 @@ err:
 	#define af9035_get_rc_config NULL
 #endif
 
+static int af9035_get_stream_config(struct dvb_frontend *fe, u8 *ts_type,
+		struct usb_data_stream_properties *stream)
+{
+	struct dvb_usb_device *d = fe_to_d(fe);
+	dev_dbg(&d->udev->dev, "%s: adap=%d\n", __func__, fe_to_adap(fe)->id);
+
+	if (d->udev->speed == USB_SPEED_FULL)
+		stream->u.bulk.buffersize = 5 * 188;
+
+	return 0;
+}
+
+/*
+ * FIXME: PID filter is property of demodulator and should be moved to the
+ * correct driver. Also we support only adapter #0 PID filter and will
+ * disable adapter #1 if USB1.1 is used.
+ */
+static int af9035_pid_filter_ctrl(struct dvb_usb_adapter *adap, int onoff)
+{
+	struct dvb_usb_device *d = adap_to_d(adap);
+	int ret;
+
+	dev_dbg(&d->udev->dev, "%s: onoff=%d\n", __func__, onoff);
+
+	ret = af9035_wr_reg_mask(d, 0x80f993, onoff, 0x01);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+
+err:
+	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_pid_filter(struct dvb_usb_adapter *adap, int index, u16 pid,
+		int onoff)
+{
+	struct dvb_usb_device *d = adap_to_d(adap);
+	int ret;
+	u8 wbuf[2] = {(pid >> 0) & 0xff, (pid >> 8) & 0xff};
+
+	dev_dbg(&d->udev->dev, "%s: index=%d pid=%04x onoff=%d\n",
+			__func__, index, pid, onoff);
+
+	ret = af9035_wr_regs(d, 0x80f996, wbuf, 2);
+	if (ret < 0)
+		goto err;
+
+	ret = af9035_wr_reg(d, 0x80f994, onoff);
+	if (ret < 0)
+		goto err;
+
+	ret = af9035_wr_reg(d, 0x80f995, index);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+
+err:
+	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_probe(struct usb_interface *intf,
+		const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	char manufacturer[sizeof("Afatech")];
+
+	memset(manufacturer, 0, sizeof(manufacturer));
+	usb_string(udev, udev->descriptor.iManufacturer,
+			manufacturer, sizeof(manufacturer));
+	/*
+	 * There is two devices having same ID but different chipset. One uses
+	 * AF9015 and the other IT9135 chipset. Only difference seen on lsusb
+	 * is iManufacturer string.
+	 *
+	 * idVendor           0x0ccd TerraTec Electronic GmbH
+	 * idProduct          0x0099
+	 * bcdDevice            2.00
+	 * iManufacturer           1 Afatech
+	 * iProduct                2 DVB-T 2
+	 *
+	 * idVendor           0x0ccd TerraTec Electronic GmbH
+	 * idProduct          0x0099
+	 * bcdDevice            2.00
+	 * iManufacturer           1 ITE Technologies, Inc.
+	 * iProduct                2 DVB-T TV Stick
+	 */
+	if ((le16_to_cpu(udev->descriptor.idVendor) == USB_VID_TERRATEC) &&
+			(le16_to_cpu(udev->descriptor.idProduct) == 0x0099)) {
+		if (!strcmp("Afatech", manufacturer)) {
+			dev_dbg(&udev->dev, "%s: rejecting device\n", __func__);
+			return -ENODEV;
+		}
+	}
+
+	return dvb_usbv2_probe(intf, id);
+}
+
 /* interface 0 is used by DVB-T receiver and
    interface 1 is for remote controller (HID) */
 static const struct dvb_usb_device_properties af9035_props = {
@@ -1237,7 +1424,6 @@ static const struct dvb_usb_device_properties af9035_props = {
 	.generic_bulk_ctrl_endpoint_response = 0x81,
 
 	.identify_state = af9035_identify_state,
-	.firmware = AF9035_FIRMWARE_AF9035,
 	.download_firmware = af9035_download_firmware,
 
 	.i2c_algo = &af9035_i2c_algo,
@@ -1246,40 +1432,18 @@ static const struct dvb_usb_device_properties af9035_props = {
 	.tuner_attach = af9035_tuner_attach,
 	.init = af9035_init,
 	.get_rc_config = af9035_get_rc_config,
+	.get_stream_config = af9035_get_stream_config,
 
 	.get_adapter_count = af9035_get_adapter_count,
 	.adapter = {
 		{
-			.stream = DVB_USB_STREAM_BULK(0x84, 6, 87 * 188),
-		}, {
-			.stream = DVB_USB_STREAM_BULK(0x85, 6, 87 * 188),
-		},
-	},
-};
-
-static const struct dvb_usb_device_properties it9135_props = {
-	.driver_name = KBUILD_MODNAME,
-	.owner = THIS_MODULE,
-	.adapter_nr = adapter_nr,
-	.size_of_priv = sizeof(struct state),
+			.caps = DVB_USB_ADAP_HAS_PID_FILTER |
+				DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 
-	.generic_bulk_ctrl_endpoint = 0x02,
-	.generic_bulk_ctrl_endpoint_response = 0x81,
-
-	.identify_state = af9035_identify_state,
-	.firmware = AF9035_FIRMWARE_IT9135,
-	.download_firmware = af9035_download_firmware_it9135,
+			.pid_filter_count = 32,
+			.pid_filter_ctrl = af9035_pid_filter_ctrl,
+			.pid_filter = af9035_pid_filter,
 
-	.i2c_algo = &af9035_i2c_algo,
-	.read_config = af9035_read_config_it9135,
-	.frontend_attach = af9035_frontend_attach,
-	.tuner_attach = af9035_tuner_attach,
-	.init = af9035_init,
-	.get_rc_config = af9035_get_rc_config,
-
-	.num_adapters = 1,
-	.adapter = {
-		{
 			.stream = DVB_USB_STREAM_BULK(0x84, 6, 87 * 188),
 		}, {
 			.stream = DVB_USB_STREAM_BULK(0x85, 6, 87 * 188),
@@ -1288,6 +1452,7 @@ static const struct dvb_usb_device_properties it9135_props = {
 };
 
 static const struct usb_device_id af9035_id_table[] = {
+	/* AF9035 devices */
 	{ DVB_USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9035_9035,
 		&af9035_props, "Afatech AF9035 reference design", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9035_1000,
@@ -1312,6 +1477,18 @@ static const struct usb_device_id af9035_id_table[] = {
 		&af9035_props, "AVerMedia Twinstar (A825)", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_ASUS, USB_PID_ASUS_U3100MINI_PLUS,
 		&af9035_props, "Asus U3100Mini Plus", NULL) },
+        { DVB_USB_DEVICE(USB_VID_TERRATEC, 0x00aa,
+		&af9035_props, "TerraTec Cinergy T Stick (rev. 2)", NULL) },
+	/* IT9135 devices */
+#if 0
+	{ DVB_USB_DEVICE(0x048d, 0x9135,
+		&af9035_props, "IT9135 reference design", NULL) },
+	{ DVB_USB_DEVICE(0x048d, 0x9006,
+		&af9035_props, "IT9135 reference design", NULL) },
+#endif
+	/* XXX: that same ID [0ccd:0099] is used by af9015 driver too */
+	{ DVB_USB_DEVICE(USB_VID_TERRATEC, 0x0099,
+		&af9035_props, "TerraTec Cinergy T Stick Dual RC (rev. 2)", NULL) },
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, af9035_id_table);
@@ -1319,7 +1496,7 @@ MODULE_DEVICE_TABLE(usb, af9035_id_table);
 static struct usb_driver af9035_usb_driver = {
 	.name = KBUILD_MODNAME,
 	.id_table = af9035_id_table,
-	.probe = dvb_usbv2_probe,
+	.probe = af9035_probe,
 	.disconnect = dvb_usbv2_disconnect,
 	.suspend = dvb_usbv2_suspend,
 	.resume = dvb_usbv2_resume,
@@ -1334,4 +1511,5 @@ MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
 MODULE_DESCRIPTION("Afatech AF9035 driver");
 MODULE_LICENSE("GPL");
 MODULE_FIRMWARE(AF9035_FIRMWARE_AF9035);
-MODULE_FIRMWARE(AF9035_FIRMWARE_IT9135);
+MODULE_FIRMWARE(AF9035_FIRMWARE_IT9135_V1);
+MODULE_FIRMWARE(AF9035_FIRMWARE_IT9135_V2);
diff --git a/drivers/media/usb/dvb-usb-v2/af9035.h b/drivers/media/usb/dvb-usb-v2/af9035.h
index 29f3eec22c2c..b5827ca3a01e 100644
--- a/drivers/media/usb/dvb-usb-v2/af9035.h
+++ b/drivers/media/usb/dvb-usb-v2/af9035.h
@@ -30,6 +30,7 @@
 #include "mxl5007t.h"
 #include "tda18218.h"
 #include "fc2580.h"
+#include "tuner_it913x.h"
 
 struct reg_val {
 	u32 reg;
@@ -52,12 +53,18 @@ struct usb_req {
 };
 
 struct state {
+#define BUF_LEN 64
+	u8 buf[BUF_LEN];
 	u8 seq; /* packet sequence number */
-	bool dual_mode;
+	u8 prechip_version;
+	u8 chip_version;
+	u16 chip_type;
+	u8 dual_mode:1;
+	u16 eeprom_addr;
 	struct af9033_config af9033_config[2];
 };
 
-u32 clock_lut[] = {
+static const u32 clock_lut_af9035[] = {
 	20480000, /*      FPGA */
 	16384000, /* 16.38 MHz */
 	20480000, /* 20.48 MHz */
@@ -72,7 +79,7 @@ u32 clock_lut[] = {
 	12000000, /* 12.00 MHz */
 };
 
-u32 clock_lut_it9135[] = {
+static const u32 clock_lut_it9135[] = {
 	12000000, /* 12.00 MHz */
 	20480000, /* 20.48 MHz */
 	36000000, /* 36.00 MHz */
@@ -86,19 +93,31 @@ u32 clock_lut_it9135[] = {
 };
 
 #define AF9035_FIRMWARE_AF9035 "dvb-usb-af9035-02.fw"
-#define AF9035_FIRMWARE_IT9135 "dvb-usb-it9135-01.fw"
+#define AF9035_FIRMWARE_IT9135_V1 "dvb-usb-it9135-01.fw"
+#define AF9035_FIRMWARE_IT9135_V2 "dvb-usb-it9135-02.fw"
 
-/* EEPROM locations */
-#define EEPROM_IR_MODE            0x430d
-#define EEPROM_DUAL_MODE          0x4326
-#define EEPROM_2ND_DEMOD_ADDR     0x4327
-#define EEPROM_IR_TYPE            0x4329
-#define EEPROM_1_IFFREQ_L         0x432d
-#define EEPROM_1_IFFREQ_H         0x432e
-#define EEPROM_1_TUNER_ID         0x4331
-#define EEPROM_2_IFFREQ_L         0x433d
-#define EEPROM_2_IFFREQ_H         0x433e
-#define EEPROM_2_TUNER_ID         0x4341
+/*
+ * eeprom is memory mapped as read only. Writing that memory mapped address
+ * will not corrupt eeprom.
+ *
+ * eeprom has value 0x00 single mode and 0x03 for dual mode as far as I have
+ * seen to this day.
+ */
+
+#define EEPROM_BASE_AF9035        0x42fd
+#define EEPROM_BASE_IT9135        0x499c
+#define EEPROM_SHIFT                0x10
+
+#define EEPROM_IR_MODE              0x10
+#define EEPROM_DUAL_MODE            0x29
+#define EEPROM_2ND_DEMOD_ADDR       0x2a
+#define EEPROM_IR_TYPE              0x2c
+#define EEPROM_1_IF_L               0x30
+#define EEPROM_1_IF_H               0x31
+#define EEPROM_1_TUNER_ID           0x34
+#define EEPROM_2_IF_L               0x40
+#define EEPROM_2_IF_H               0x41
+#define EEPROM_2_TUNER_ID           0x44
 
 /* USB commands */
 #define CMD_MEM_RD                  0x00
diff --git a/drivers/media/usb/dvb-usb-v2/anysee.c b/drivers/media/usb/dvb-usb-v2/anysee.c
index a20d691d0b63..90cfa35ef6e6 100644
--- a/drivers/media/usb/dvb-usb-v2/anysee.c
+++ b/drivers/media/usb/dvb-usb-v2/anysee.c
@@ -45,25 +45,24 @@
 #include "cxd2820r.h"
 
 DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
-static DEFINE_MUTEX(anysee_usb_mutex);
 
-static int anysee_ctrl_msg(struct dvb_usb_device *d, u8 *sbuf, u8 slen,
-	u8 *rbuf, u8 rlen)
+static int anysee_ctrl_msg(struct dvb_usb_device *d,
+		u8 *sbuf, u8 slen, u8 *rbuf, u8 rlen)
 {
 	struct anysee_state *state = d_to_priv(d);
 	int act_len, ret, i;
-	u8 buf[64];
 
-	memcpy(&buf[0], sbuf, slen);
-	buf[60] = state->seq++;
+	mutex_lock(&d->usb_mutex);
 
-	mutex_lock(&anysee_usb_mutex);
+	memcpy(&state->buf[0], sbuf, slen);
+	state->buf[60] = state->seq++;
 
-	dev_dbg(&d->udev->dev, "%s: >>> %*ph\n", __func__, slen, buf);
+	dev_dbg(&d->udev->dev, "%s: >>> %*ph\n", __func__, slen, state->buf);
 
 	/* We need receive one message more after dvb_usb_generic_rw due
 	   to weird transaction flow, which is 1 x send + 2 x receive. */
-	ret = dvb_usbv2_generic_rw(d, buf, sizeof(buf), buf, sizeof(buf));
+	ret = dvb_usbv2_generic_rw_locked(d, state->buf, sizeof(state->buf),
+			state->buf, sizeof(state->buf));
 	if (ret)
 		goto error_unlock;
 
@@ -82,20 +81,19 @@ static int anysee_ctrl_msg(struct dvb_usb_device *d, u8 *sbuf, u8 slen,
 	for (i = 0; i < 3; i++) {
 		/* receive 2nd answer */
 		ret = usb_bulk_msg(d->udev, usb_rcvbulkpipe(d->udev,
-			d->props->generic_bulk_ctrl_endpoint), buf, sizeof(buf),
-			&act_len, 2000);
-
+				d->props->generic_bulk_ctrl_endpoint),
+				state->buf, sizeof(state->buf), &act_len, 2000);
 		if (ret) {
-			dev_dbg(&d->udev->dev, "%s: recv bulk message " \
-					"failed=%d\n", __func__, ret);
+			dev_dbg(&d->udev->dev,
+					"%s: recv bulk message failed=%d\n",
+					__func__, ret);
 		} else {
 			dev_dbg(&d->udev->dev, "%s: <<< %*ph\n", __func__,
-					rlen, buf);
-
-			if (buf[63] != 0x4f)
-				dev_dbg(&d->udev->dev, "%s: cmd failed\n",
-						__func__);
+					rlen, state->buf);
 
+			if (state->buf[63] != 0x4f)
+				dev_dbg(&d->udev->dev,
+						"%s: cmd failed\n", __func__);
 			break;
 		}
 	}
@@ -109,11 +107,10 @@ static int anysee_ctrl_msg(struct dvb_usb_device *d, u8 *sbuf, u8 slen,
 
 	/* read request, copy returned data to return buf */
 	if (rbuf && rlen)
-		memcpy(rbuf, buf, rlen);
+		memcpy(rbuf, state->buf, rlen);
 
 error_unlock:
-	mutex_unlock(&anysee_usb_mutex);
-
+	mutex_unlock(&d->usb_mutex);
 	return ret;
 }
 
@@ -638,7 +635,7 @@ static int anysee_frontend_attach(struct dvb_usb_adapter *adap)
 {
 	struct anysee_state *state = adap_to_priv(adap);
 	struct dvb_usb_device *d = adap_to_d(adap);
-	int ret;
+	int ret = 0;
 	u8 tmp;
 	struct i2c_msg msg[2] = {
 		{
@@ -884,9 +881,8 @@ static int anysee_frontend_attach(struct dvb_usb_adapter *adap)
 	if (!adap->fe[0]) {
 		/* we have no frontend :-( */
 		ret = -ENODEV;
-		dev_err(&d->udev->dev, "%s: Unsupported Anysee version. " \
-				"Please report the " \
-				"<linux-media@vger.kernel.org>.\n",
+		dev_err(&d->udev->dev,
+				"%s: Unsupported Anysee version. Please report to <linux-media@vger.kernel.org>.\n",
 				KBUILD_MODNAME);
 	}
 error:
diff --git a/drivers/media/usb/dvb-usb-v2/anysee.h b/drivers/media/usb/dvb-usb-v2/anysee.h
index c1a4273f14ff..8f426d9fc6e1 100644
--- a/drivers/media/usb/dvb-usb-v2/anysee.h
+++ b/drivers/media/usb/dvb-usb-v2/anysee.h
@@ -52,8 +52,9 @@ enum cmd {
 };
 
 struct anysee_state {
-	u8 hw; /* PCB ID */
+	u8 buf[64];
 	u8 seq;
+	u8 hw; /* PCB ID */
 	u8 fe_id:1; /* frondend ID */
 	u8 has_ci:1;
 	u8 ci_attached:1;
diff --git a/drivers/media/usb/dvb-usb-v2/az6007.c b/drivers/media/usb/dvb-usb-v2/az6007.c
index 70ec80d8be71..44c64ef361bf 100644
--- a/drivers/media/usb/dvb-usb-v2/az6007.c
+++ b/drivers/media/usb/dvb-usb-v2/az6007.c
@@ -842,7 +842,7 @@ static int az6007_download_firmware(struct dvb_usb_device *d,
 {
 	pr_debug("Loading az6007 firmware\n");
 
-	return usbv2_cypress_load_firmware(d->udev, fw, CYPRESS_FX2);
+	return cypress_load_firmware(d->udev, fw, CYPRESS_FX2);
 }
 
 /* DVB USB Driver stuff */
diff --git a/drivers/media/usb/dvb-usb-v2/dvb_usb.h b/drivers/media/usb/dvb-usb-v2/dvb_usb.h
index 3cac8bd0b116..658c6d47fdff 100644
--- a/drivers/media/usb/dvb-usb-v2/dvb_usb.h
+++ b/drivers/media/usb/dvb-usb-v2/dvb_usb.h
@@ -329,13 +329,16 @@ struct dvb_usb_adapter {
 	u8 feed_count;
 	u8 max_feed_count;
 	s8 active_fe;
+#define ADAP_INIT                0
+#define ADAP_SLEEP               1
+#define ADAP_STREAMING           2
+	unsigned long state_bits;
 
 	/* dvb */
 	struct dvb_adapter   dvb_adap;
 	struct dmxdev        dmxdev;
 	struct dvb_demux     demux;
 	struct dvb_net       dvb_net;
-	struct mutex         sync_mutex;
 
 	struct dvb_frontend *fe[MAX_NO_OF_FE_PER_ADAP];
 	int (*fe_init[MAX_NO_OF_FE_PER_ADAP]) (struct dvb_frontend *);
@@ -400,5 +403,9 @@ extern int dvb_usbv2_reset_resume(struct usb_interface *);
 /* the generic read/write method for device control */
 extern int dvb_usbv2_generic_rw(struct dvb_usb_device *, u8 *, u16, u8 *, u16);
 extern int dvb_usbv2_generic_write(struct dvb_usb_device *, u8 *, u16);
+/* caller must hold lock when locked versions are called */
+extern int dvb_usbv2_generic_rw_locked(struct dvb_usb_device *,
+		u8 *, u16, u8 *, u16);
+extern int dvb_usbv2_generic_write_locked(struct dvb_usb_device *, u8 *, u16);
 
 #endif
diff --git a/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c b/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
index 086792055912..19f6737d9817 100644
--- a/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
+++ b/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
@@ -28,10 +28,11 @@ MODULE_PARM_DESC(disable_rc_polling,
 static int dvb_usb_force_pid_filter_usage;
 module_param_named(force_pid_filter_usage, dvb_usb_force_pid_filter_usage,
 		int, 0444);
-MODULE_PARM_DESC(force_pid_filter_usage, "force all DVB USB devices to use a " \
-		"PID filter, if any (default: 0)");
+MODULE_PARM_DESC(force_pid_filter_usage,
+		"force all DVB USB devices to use a PID filter, if any (default: 0)");
 
-static int dvb_usbv2_download_firmware(struct dvb_usb_device *d, const char *name)
+static int dvb_usbv2_download_firmware(struct dvb_usb_device *d,
+		const char *name)
 {
 	int ret;
 	const struct firmware *fw;
@@ -44,10 +45,9 @@ static int dvb_usbv2_download_firmware(struct dvb_usb_device *d, const char *nam
 
 	ret = request_firmware(&fw, name, &d->udev->dev);
 	if (ret < 0) {
-		dev_err(&d->udev->dev, "%s: Did not find the firmware file "\
-				"'%s'. Please see linux/Documentation/dvb/ " \
-				"for more details on firmware-problems. " \
-				"Status %d\n", KBUILD_MODNAME, name, ret);
+		dev_err(&d->udev->dev,
+				"%s: Did not find the firmware file '%s'. Please see linux/Documentation/dvb/ for more details on firmware-problems. Status %d\n",
+				KBUILD_MODNAME, name, ret);
 		goto err;
 	}
 
@@ -181,9 +181,9 @@ static int dvb_usbv2_remote_init(struct dvb_usb_device *d)
 		/* initialize a work queue for handling polling */
 		INIT_DELAYED_WORK(&d->rc_query_work,
 				dvb_usb_read_remote_control);
-		dev_info(&d->udev->dev, "%s: schedule remote query interval " \
-				"to %d msecs\n", KBUILD_MODNAME,
-				d->rc.interval);
+		dev_info(&d->udev->dev,
+				"%s: schedule remote query interval to %d msecs\n",
+				KBUILD_MODNAME, d->rc.interval);
 		schedule_delayed_work(&d->rc_query_work,
 				msecs_to_jiffies(d->rc.interval));
 		d->rc_polling_active = true;
@@ -253,128 +253,159 @@ static int dvb_usbv2_adapter_stream_exit(struct dvb_usb_adapter *adap)
 	return usb_urb_exitv2(&adap->stream);
 }
 
-static inline int dvb_usb_ctrl_feed(struct dvb_demux_feed *dvbdmxfeed,
-		int count)
+static int wait_schedule(void *ptr)
+{
+	schedule();
+
+	return 0;
+}
+
+static int dvb_usb_start_feed(struct dvb_demux_feed *dvbdmxfeed)
 {
 	struct dvb_usb_adapter *adap = dvbdmxfeed->demux->priv;
 	struct dvb_usb_device *d = adap_to_d(adap);
-	int ret;
-	dev_dbg(&d->udev->dev, "%s: adap=%d active_fe=%d feed_type=%d " \
-			"setting pid [%s]: %04x (%04d) at index %d '%s'\n",
+	int ret = 0;
+	struct usb_data_stream_properties stream_props;
+	dev_dbg(&d->udev->dev,
+			"%s: adap=%d active_fe=%d feed_type=%d setting pid [%s]: %04x (%04d) at index %d\n",
 			__func__, adap->id, adap->active_fe, dvbdmxfeed->type,
 			adap->pid_filtering ? "yes" : "no", dvbdmxfeed->pid,
-			dvbdmxfeed->pid, dvbdmxfeed->index,
-			(count == 1) ? "on" : "off");
+			dvbdmxfeed->pid, dvbdmxfeed->index);
+
+	/* wait init is done */
+	wait_on_bit(&adap->state_bits, ADAP_INIT, wait_schedule,
+			TASK_UNINTERRUPTIBLE);
 
 	if (adap->active_fe == -1)
 		return -EINVAL;
 
-	adap->feed_count += count;
-
-	/* stop feeding if it is last pid */
-	if (adap->feed_count == 0) {
-		dev_dbg(&d->udev->dev, "%s: stop feeding\n", __func__);
-
-		if (d->props->streaming_ctrl) {
-			ret = d->props->streaming_ctrl(
-					adap->fe[adap->active_fe], 0);
-			if (ret < 0) {
-				dev_err(&d->udev->dev, "%s: streaming_ctrl() " \
-						"failed=%d\n", KBUILD_MODNAME,
-						ret);
-				usb_urb_killv2(&adap->stream);
-				goto err_mutex_unlock;
-			}
-		}
-		usb_urb_killv2(&adap->stream);
-		mutex_unlock(&adap->sync_mutex);
-	}
+	/* skip feed setup if we are already feeding */
+	if (adap->feed_count++ > 0)
+		goto skip_feed_start;
 
-	/* activate the pid on the device pid filter */
-	if (adap->props->caps & DVB_USB_ADAP_HAS_PID_FILTER &&
-			adap->pid_filtering && adap->props->pid_filter) {
-		ret = adap->props->pid_filter(adap, dvbdmxfeed->index,
-				dvbdmxfeed->pid, (count == 1) ? 1 : 0);
-		if (ret < 0)
-			dev_err(&d->udev->dev, "%s: pid_filter() failed=%d\n",
+	/* set 'streaming' status bit */
+	set_bit(ADAP_STREAMING, &adap->state_bits);
+
+	/* resolve input and output streaming parameters */
+	if (d->props->get_stream_config) {
+		memcpy(&stream_props, &adap->props->stream,
+				sizeof(struct usb_data_stream_properties));
+		ret = d->props->get_stream_config(adap->fe[adap->active_fe],
+				&adap->ts_type, &stream_props);
+		if (ret)
+			dev_err(&d->udev->dev,
+					"%s: get_stream_config() failed=%d\n",
 					KBUILD_MODNAME, ret);
+	} else {
+		stream_props = adap->props->stream;
 	}
 
-	/* start feeding if it is first pid */
-	if (adap->feed_count == 1 && count == 1) {
-		struct usb_data_stream_properties stream_props;
-		mutex_lock(&adap->sync_mutex);
-		dev_dbg(&d->udev->dev, "%s: start feeding\n", __func__);
+	switch (adap->ts_type) {
+	case DVB_USB_FE_TS_TYPE_204:
+		adap->stream.complete = dvb_usb_data_complete_204;
+		break;
+	case DVB_USB_FE_TS_TYPE_RAW:
+		adap->stream.complete = dvb_usb_data_complete_raw;
+		break;
+	case DVB_USB_FE_TS_TYPE_188:
+	default:
+		adap->stream.complete = dvb_usb_data_complete;
+		break;
+	}
 
-		/* resolve input and output streaming paramters */
-		if (d->props->get_stream_config) {
-			memcpy(&stream_props, &adap->props->stream,
-				sizeof(struct usb_data_stream_properties));
-			ret = d->props->get_stream_config(
-					adap->fe[adap->active_fe],
-					&adap->ts_type, &stream_props);
-			if (ret < 0)
-				goto err_mutex_unlock;
-		} else {
-			stream_props = adap->props->stream;
-		}
+	/* submit USB streaming packets */
+	usb_urb_submitv2(&adap->stream, &stream_props);
 
-		switch (adap->ts_type) {
-		case DVB_USB_FE_TS_TYPE_204:
-			adap->stream.complete = dvb_usb_data_complete_204;
-			break;
-		case DVB_USB_FE_TS_TYPE_RAW:
-			adap->stream.complete = dvb_usb_data_complete_raw;
-			break;
-		case DVB_USB_FE_TS_TYPE_188:
-		default:
-			adap->stream.complete = dvb_usb_data_complete;
-			break;
-		}
+	/* enable HW PID filter */
+	if (adap->pid_filtering && adap->props->pid_filter_ctrl) {
+		ret = adap->props->pid_filter_ctrl(adap, 1);
+		if (ret)
+			dev_err(&d->udev->dev,
+					"%s: pid_filter_ctrl() failed=%d\n",
+					KBUILD_MODNAME, ret);
+	}
 
-		usb_urb_submitv2(&adap->stream, &stream_props);
-
-		if (adap->props->caps & DVB_USB_ADAP_HAS_PID_FILTER &&
-				adap->props->caps &
-				DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF &&
-				adap->props->pid_filter_ctrl) {
-			ret = adap->props->pid_filter_ctrl(adap,
-					adap->pid_filtering);
-			if (ret < 0) {
-				dev_err(&d->udev->dev, "%s: " \
-						"pid_filter_ctrl() failed=%d\n",
-						KBUILD_MODNAME, ret);
-				goto err_mutex_unlock;
-			}
-		}
+	/* ask device to start streaming */
+	if (d->props->streaming_ctrl) {
+		ret = d->props->streaming_ctrl(adap->fe[adap->active_fe], 1);
+		if (ret)
+			dev_err(&d->udev->dev,
+					"%s: streaming_ctrl() failed=%d\n",
+					KBUILD_MODNAME, ret);
+	}
+skip_feed_start:
 
-		if (d->props->streaming_ctrl) {
-			ret = d->props->streaming_ctrl(
-					adap->fe[adap->active_fe], 1);
-			if (ret < 0) {
-				dev_err(&d->udev->dev, "%s: streaming_ctrl() " \
-						"failed=%d\n", KBUILD_MODNAME,
-						ret);
-				goto err_mutex_unlock;
-			}
-		}
+	/* add PID to device HW PID filter */
+	if (adap->pid_filtering && adap->props->pid_filter) {
+		ret = adap->props->pid_filter(adap, dvbdmxfeed->index,
+				dvbdmxfeed->pid, 1);
+		if (ret)
+			dev_err(&d->udev->dev, "%s: pid_filter() failed=%d\n",
+					KBUILD_MODNAME, ret);
 	}
 
-	return 0;
-err_mutex_unlock:
-	mutex_unlock(&adap->sync_mutex);
-	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+	if (ret)
+		dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
-static int dvb_usb_start_feed(struct dvb_demux_feed *dvbdmxfeed)
-{
-	return dvb_usb_ctrl_feed(dvbdmxfeed, 1);
-}
-
 static int dvb_usb_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 {
-	return dvb_usb_ctrl_feed(dvbdmxfeed, -1);
+	struct dvb_usb_adapter *adap = dvbdmxfeed->demux->priv;
+	struct dvb_usb_device *d = adap_to_d(adap);
+	int ret = 0;
+	dev_dbg(&d->udev->dev,
+			"%s: adap=%d active_fe=%d feed_type=%d setting pid [%s]: %04x (%04d) at index %d\n",
+			__func__, adap->id, adap->active_fe, dvbdmxfeed->type,
+			adap->pid_filtering ? "yes" : "no", dvbdmxfeed->pid,
+			dvbdmxfeed->pid, dvbdmxfeed->index);
+
+	if (adap->active_fe == -1)
+		return -EINVAL;
+
+	/* remove PID from device HW PID filter */
+	if (adap->pid_filtering && adap->props->pid_filter) {
+		ret = adap->props->pid_filter(adap, dvbdmxfeed->index,
+				dvbdmxfeed->pid, 0);
+		if (ret)
+			dev_err(&d->udev->dev, "%s: pid_filter() failed=%d\n",
+					KBUILD_MODNAME, ret);
+	}
+
+	/* we cannot stop streaming until last PID is removed */
+	if (--adap->feed_count > 0)
+		goto skip_feed_stop;
+
+	/* ask device to stop streaming */
+	if (d->props->streaming_ctrl) {
+		ret = d->props->streaming_ctrl(adap->fe[adap->active_fe], 0);
+		if (ret)
+			dev_err(&d->udev->dev,
+					"%s: streaming_ctrl() failed=%d\n",
+					KBUILD_MODNAME, ret);
+	}
+
+	/* disable HW PID filter */
+	if (adap->pid_filtering && adap->props->pid_filter_ctrl) {
+		ret = adap->props->pid_filter_ctrl(adap, 0);
+		if (ret)
+			dev_err(&d->udev->dev,
+					"%s: pid_filter_ctrl() failed=%d\n",
+					KBUILD_MODNAME, ret);
+	}
+
+	/* kill USB streaming packets */
+	usb_urb_killv2(&adap->stream);
+
+	/* clear 'streaming' status bit */
+	clear_bit(ADAP_STREAMING, &adap->state_bits);
+	smp_mb__after_clear_bit();
+	wake_up_bit(&adap->state_bits, ADAP_STREAMING);
+skip_feed_stop:
+
+	if (ret)
+		dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
 }
 
 static int dvb_usbv2_adapter_dvb_init(struct dvb_usb_adapter *adap)
@@ -435,8 +466,6 @@ static int dvb_usbv2_adapter_dvb_init(struct dvb_usb_adapter *adap)
 		goto err_dvb_net_init;
 	}
 
-	mutex_init(&adap->sync_mutex);
-
 	return 0;
 err_dvb_net_init:
 	dvb_dmxdev_release(&adap->dmxdev);
@@ -500,7 +529,7 @@ static int dvb_usb_fe_init(struct dvb_frontend *fe)
 
 	if (!adap->suspend_resume_active) {
 		adap->active_fe = fe->id;
-		mutex_lock(&adap->sync_mutex);
+		set_bit(ADAP_INIT, &adap->state_bits);
 	}
 
 	ret = dvb_usbv2_device_power_ctrl(d, 1);
@@ -519,8 +548,11 @@ static int dvb_usb_fe_init(struct dvb_frontend *fe)
 			goto err;
 	}
 err:
-	if (!adap->suspend_resume_active)
-		mutex_unlock(&adap->sync_mutex);
+	if (!adap->suspend_resume_active) {
+		clear_bit(ADAP_INIT, &adap->state_bits);
+		smp_mb__after_clear_bit();
+		wake_up_bit(&adap->state_bits, ADAP_INIT);
+	}
 
 	dev_dbg(&d->udev->dev, "%s: ret=%d\n", __func__, ret);
 	return ret;
@@ -534,8 +566,11 @@ static int dvb_usb_fe_sleep(struct dvb_frontend *fe)
 	dev_dbg(&d->udev->dev, "%s: adap=%d fe=%d\n", __func__, adap->id,
 			fe->id);
 
-	if (!adap->suspend_resume_active)
-		mutex_lock(&adap->sync_mutex);
+	if (!adap->suspend_resume_active) {
+		set_bit(ADAP_SLEEP, &adap->state_bits);
+		wait_on_bit(&adap->state_bits, ADAP_STREAMING, wait_schedule,
+				TASK_UNINTERRUPTIBLE);
+	}
 
 	if (adap->fe_sleep[fe->id]) {
 		ret = adap->fe_sleep[fe->id](fe);
@@ -555,7 +590,9 @@ static int dvb_usb_fe_sleep(struct dvb_frontend *fe)
 err:
 	if (!adap->suspend_resume_active) {
 		adap->active_fe = -1;
-		mutex_unlock(&adap->sync_mutex);
+		clear_bit(ADAP_SLEEP, &adap->state_bits);
+		smp_mb__after_clear_bit();
+		wake_up_bit(&adap->state_bits, ADAP_SLEEP);
 	}
 
 	dev_dbg(&d->udev->dev, "%s: ret=%d\n", __func__, ret);
@@ -574,8 +611,9 @@ static int dvb_usbv2_adapter_frontend_init(struct dvb_usb_adapter *adap)
 	if (d->props->frontend_attach) {
 		ret = d->props->frontend_attach(adap);
 		if (ret < 0) {
-			dev_dbg(&d->udev->dev, "%s: frontend_attach() " \
-					"failed=%d\n", __func__, ret);
+			dev_dbg(&d->udev->dev,
+					"%s: frontend_attach() failed=%d\n",
+					__func__, ret);
 			goto err_dvb_frontend_detach;
 		}
 	} else {
@@ -595,8 +633,9 @@ static int dvb_usbv2_adapter_frontend_init(struct dvb_usb_adapter *adap)
 
 		ret = dvb_register_frontend(&adap->dvb_adap, adap->fe[i]);
 		if (ret < 0) {
-			dev_err(&d->udev->dev, "%s: frontend%d registration " \
-					"failed\n", KBUILD_MODNAME, i);
+			dev_err(&d->udev->dev,
+					"%s: frontend%d registration failed\n",
+					KBUILD_MODNAME, i);
 			goto err_dvb_unregister_frontend;
 		}
 
@@ -670,33 +709,33 @@ static int dvb_usbv2_adapter_init(struct dvb_usb_device *d)
 		/* speed - when running at FULL speed we need a HW PID filter */
 		if (d->udev->speed == USB_SPEED_FULL &&
 				!(adap->props->caps & DVB_USB_ADAP_HAS_PID_FILTER)) {
-			dev_err(&d->udev->dev, "%s: this USB2.0 device " \
-					"cannot be run on a USB1.1 port (it " \
-					"lacks a hardware PID filter)\n",
+			dev_err(&d->udev->dev,
+					"%s: this USB2.0 device cannot be run on a USB1.1 port (it lacks a hardware PID filter)\n",
 					KBUILD_MODNAME);
 			ret = -ENODEV;
 			goto err;
 		} else if ((d->udev->speed == USB_SPEED_FULL &&
 				adap->props->caps & DVB_USB_ADAP_HAS_PID_FILTER) ||
 				(adap->props->caps & DVB_USB_ADAP_NEED_PID_FILTERING)) {
-			dev_info(&d->udev->dev, "%s: will use the device's " \
-					"hardware PID filter " \
-					"(table count: %d)\n", KBUILD_MODNAME,
+			dev_info(&d->udev->dev,
+					"%s: will use the device's hardware PID filter (table count: %d)\n",
+					KBUILD_MODNAME,
 					adap->props->pid_filter_count);
 			adap->pid_filtering  = 1;
 			adap->max_feed_count = adap->props->pid_filter_count;
 		} else {
-			dev_info(&d->udev->dev, "%s: will pass the complete " \
-					"MPEG2 transport stream to the " \
-					"software demuxer\n", KBUILD_MODNAME);
+			dev_info(&d->udev->dev,
+					"%s: will pass the complete MPEG2 transport stream to the software demuxer\n",
+					KBUILD_MODNAME);
 			adap->pid_filtering  = 0;
 			adap->max_feed_count = 255;
 		}
 
 		if (!adap->pid_filtering && dvb_usb_force_pid_filter_usage &&
 				adap->props->caps & DVB_USB_ADAP_HAS_PID_FILTER) {
-			dev_info(&d->udev->dev, "%s: PID filter enabled by " \
-					"module option\n", KBUILD_MODNAME);
+			dev_info(&d->udev->dev,
+					"%s: PID filter enabled by module option\n",
+					KBUILD_MODNAME);
 			adap->pid_filtering  = 1;
 			adap->max_feed_count = adap->props->pid_filter_count;
 		}
@@ -825,8 +864,9 @@ static void dvb_usbv2_init_work(struct work_struct *work)
 		if (ret == 0) {
 			;
 		} else if (ret == COLD) {
-			dev_info(&d->udev->dev, "%s: found a '%s' in cold " \
-					"state\n", KBUILD_MODNAME, d->name);
+			dev_info(&d->udev->dev,
+					"%s: found a '%s' in cold state\n",
+					KBUILD_MODNAME, d->name);
 
 			if (!name)
 				name = d->props->firmware;
@@ -868,8 +908,9 @@ static void dvb_usbv2_init_work(struct work_struct *work)
 	if (ret < 0)
 		goto err_usb_driver_release_interface;
 
-	dev_info(&d->udev->dev, "%s: '%s' successfully initialized and " \
-			"connected\n", KBUILD_MODNAME, d->name);
+	dev_info(&d->udev->dev,
+			"%s: '%s' successfully initialized and connected\n",
+			KBUILD_MODNAME, d->name);
 
 	return;
 err_usb_driver_release_interface:
diff --git a/drivers/media/usb/dvb-usb-v2/dvb_usb_urb.c b/drivers/media/usb/dvb-usb-v2/dvb_usb_urb.c
index 5716662b4834..33ff97e708e3 100644
--- a/drivers/media/usb/dvb-usb-v2/dvb_usb_urb.c
+++ b/drivers/media/usb/dvb-usb-v2/dvb_usb_urb.c
@@ -21,8 +21,8 @@
 
 #include "dvb_usb_common.h"
 
-int dvb_usbv2_generic_rw(struct dvb_usb_device *d, u8 *wbuf, u16 wlen, u8 *rbuf,
-		u16 rlen)
+static int dvb_usb_v2_generic_io(struct dvb_usb_device *d,
+		u8 *wbuf, u16 wlen, u8 *rbuf, u16 rlen)
 {
 	int ret, actual_length;
 
@@ -32,8 +32,6 @@ int dvb_usbv2_generic_rw(struct dvb_usb_device *d, u8 *wbuf, u16 wlen, u8 *rbuf,
 		return -EINVAL;
 	}
 
-	mutex_lock(&d->usb_mutex);
-
 	dev_dbg(&d->udev->dev, "%s: >>> %*ph\n", __func__, wlen, wbuf);
 
 	ret = usb_bulk_msg(d->udev, usb_sndbulkpipe(d->udev,
@@ -56,20 +54,51 @@ int dvb_usbv2_generic_rw(struct dvb_usb_device *d, u8 *wbuf, u16 wlen, u8 *rbuf,
 				d->props->generic_bulk_ctrl_endpoint_response),
 				rbuf, rlen, &actual_length, 2000);
 		if (ret)
-			dev_err(&d->udev->dev, "%s: 2nd usb_bulk_msg() " \
-					"failed=%d\n", KBUILD_MODNAME, ret);
+			dev_err(&d->udev->dev,
+					"%s: 2nd usb_bulk_msg() failed=%d\n",
+					KBUILD_MODNAME, ret);
 
 		dev_dbg(&d->udev->dev, "%s: <<< %*ph\n", __func__,
 				actual_length, rbuf);
 	}
 
+	return ret;
+}
+
+int dvb_usbv2_generic_rw(struct dvb_usb_device *d,
+		u8 *wbuf, u16 wlen, u8 *rbuf, u16 rlen)
+{
+	int ret;
+
+	mutex_lock(&d->usb_mutex);
+	ret = dvb_usb_v2_generic_io(d, wbuf, wlen, rbuf, rlen);
 	mutex_unlock(&d->usb_mutex);
+
 	return ret;
 }
 EXPORT_SYMBOL(dvb_usbv2_generic_rw);
 
 int dvb_usbv2_generic_write(struct dvb_usb_device *d, u8 *buf, u16 len)
 {
-	return dvb_usbv2_generic_rw(d, buf, len, NULL, 0);
+	int ret;
+
+	mutex_lock(&d->usb_mutex);
+	ret = dvb_usb_v2_generic_io(d, buf, len, NULL, 0);
+	mutex_unlock(&d->usb_mutex);
+
+	return ret;
 }
 EXPORT_SYMBOL(dvb_usbv2_generic_write);
+
+int dvb_usbv2_generic_rw_locked(struct dvb_usb_device *d,
+		u8 *wbuf, u16 wlen, u8 *rbuf, u16 rlen)
+{
+	return dvb_usb_v2_generic_io(d, wbuf, wlen, rbuf, rlen);
+}
+EXPORT_SYMBOL(dvb_usbv2_generic_rw_locked);
+
+int dvb_usbv2_generic_write_locked(struct dvb_usb_device *d, u8 *buf, u16 len)
+{
+	return dvb_usb_v2_generic_io(d, buf, len, NULL, 0);
+}
+EXPORT_SYMBOL(dvb_usbv2_generic_write_locked);
diff --git a/drivers/media/usb/dvb-usb-v2/it913x.c b/drivers/media/usb/dvb-usb-v2/it913x.c
index 833847995c65..e48cdeb9df41 100644
--- a/drivers/media/usb/dvb-usb-v2/it913x.c
+++ b/drivers/media/usb/dvb-usb-v2/it913x.c
@@ -218,6 +218,7 @@ static int it913x_pid_filter_ctrl(struct dvb_usb_adapter *adap, int onoff)
 
 	deb_info(1, "PID_C  (%02x)", onoff);
 
+	st->pid_filter_onoff = adap->pid_filtering;
 	ret = it913x_wr_reg(d, pro, PID_EN, st->pid_filter_onoff);
 
 	mutex_unlock(&d->i2c_mutex);
diff --git a/drivers/media/usb/dvb-usb-v2/lmedm04.c b/drivers/media/usb/dvb-usb-v2/lmedm04.c
index f30c58cecbba..b3fd0ffa3c3f 100644
--- a/drivers/media/usb/dvb-usb-v2/lmedm04.c
+++ b/drivers/media/usb/dvb-usb-v2/lmedm04.c
@@ -1241,10 +1241,13 @@ static int lme2510_get_stream_config(struct dvb_frontend *fe, u8 *ts_type,
 		struct usb_data_stream_properties *stream)
 {
 	struct dvb_usb_adapter *adap = fe_to_adap(fe);
-	struct dvb_usb_device *d = adap_to_d(adap);
+	struct dvb_usb_device *d;
 
 	if (adap == NULL)
 		return 0;
+
+	d = adap_to_d(adap);
+
 	/* Turn PID filter on the fly by module option */
 	if (pid_filter == 2) {
 		adap->pid_filtering  = 1;
@@ -1299,8 +1302,7 @@ static void lme2510_exit(struct dvb_usb_device *d)
 
 	if (d != NULL) {
 		usb_buffer = lme2510_exit_int(d);
-		if (usb_buffer != NULL)
-			kfree(usb_buffer);
+		kfree(usb_buffer);
 	}
 }
 
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h
index 432706ae5274..3f3f8bfd190b 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h
@@ -21,6 +21,7 @@
 #ifndef __MXL111SF_DEMOD_H__
 #define __MXL111SF_DEMOD_H__
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 #include "mxl111sf.h"
 
@@ -31,8 +32,7 @@ struct mxl111sf_demod_config {
 			    struct mxl111sf_reg_ctrl_info *ctrl_reg_info);
 };
 
-#if defined(CONFIG_DVB_USB_MXL111SF) || \
-	(defined(CONFIG_DVB_USB_MXL111SF_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_USB_MXL111SF)
 extern
 struct dvb_frontend *mxl111sf_demod_attach(struct mxl111sf_state *mxl_state,
 					   struct mxl111sf_demod_config *cfg);
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h b/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h
index ff333960b184..90f583e5d6a6 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h
@@ -21,8 +21,8 @@
 #ifndef __MXL111SF_TUNER_H__
 #define __MXL111SF_TUNER_H__
 
+#include <linux/kconfig.h>
 #include "dvb_frontend.h"
-
 #include "mxl111sf.h"
 
 enum mxl_if_freq {
@@ -60,8 +60,7 @@ struct mxl111sf_tuner_config {
 
 /* ------------------------------------------------------------------------ */
 
-#if defined(CONFIG_DVB_USB_MXL111SF) || \
-	(defined(CONFIG_DVB_USB_MXL111SF_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_USB_MXL111SF)
 extern
 struct dvb_frontend *mxl111sf_tuner_attach(struct dvb_frontend *fe,
 					   struct mxl111sf_state *mxl_state,
diff --git a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
index d98387a3c95e..22015fe1a0f3 100644
--- a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
+++ b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
@@ -33,6 +33,7 @@
 #include "e4000.h"
 #include "fc2580.h"
 #include "tua9001.h"
+#include "r820t.h"
 
 DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
@@ -375,6 +376,7 @@ static int rtl2832u_read_config(struct dvb_usb_device *d)
 	struct rtl28xxu_req req_mxl5007t = {0xd9c0, CMD_I2C_RD, 1, buf};
 	struct rtl28xxu_req req_e4000 = {0x02c8, CMD_I2C_RD, 1, buf};
 	struct rtl28xxu_req req_tda18272 = {0x00c0, CMD_I2C_RD, 2, buf};
+	struct rtl28xxu_req req_r820t = {0x0034, CMD_I2C_RD, 5, buf};
 
 	dev_dbg(&d->udev->dev, "%s:\n", __func__);
 
@@ -479,6 +481,14 @@ static int rtl2832u_read_config(struct dvb_usb_device *d)
 		goto found;
 	}
 
+	/* check R820T by reading tuner stats at I2C addr 0x1a */
+	ret = rtl28xxu_ctrl_msg(d, &req_r820t);
+	if (ret == 0) {
+		priv->tuner = TUNER_RTL2832_R820T;
+		priv->tuner_name = "R820T";
+		goto found;
+	}
+
 found:
 	dev_dbg(&d->udev->dev, "%s: tuner=%s\n", __func__, priv->tuner_name);
 
@@ -589,6 +599,12 @@ static struct rtl2832_config rtl28xxu_rtl2832_e4000_config = {
 	.tuner = TUNER_RTL2832_E4000,
 };
 
+static struct rtl2832_config rtl28xxu_rtl2832_r820t_config = {
+	.i2c_addr = 0x10,
+	.xtal = 28800000,
+	.tuner = TUNER_RTL2832_R820T,
+};
+
 static int rtl2832u_fc0012_tuner_callback(struct dvb_usb_device *d,
 		int cmd, int arg)
 {
@@ -728,6 +744,9 @@ static int rtl2832u_frontend_attach(struct dvb_usb_adapter *adap)
 	case TUNER_RTL2832_E4000:
 		rtl2832_config = &rtl28xxu_rtl2832_e4000_config;
 		break;
+	case TUNER_RTL2832_R820T:
+		rtl2832_config = &rtl28xxu_rtl2832_r820t_config;
+		break;
 	default:
 		dev_err(&d->udev->dev, "%s: unknown tuner=%s\n",
 				KBUILD_MODNAME, priv->tuner_name);
@@ -840,6 +859,13 @@ static const struct fc0012_config rtl2832u_fc0012_config = {
 	.xtal_freq = FC_XTAL_28_8_MHZ,
 };
 
+static const struct r820t_config rtl2832u_r820t_config = {
+	.i2c_addr = 0x1a,
+	.xtal = 28800000,
+	.max_i2c_msg_len = 2,
+	.rafael_chip = CHIP_R820T,
+};
+
 static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 {
 	int ret;
@@ -889,6 +915,14 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 		fe = dvb_attach(tua9001_attach, adap->fe[0], &d->i2c_adap,
 				&rtl2832u_tua9001_config);
 		break;
+	case TUNER_RTL2832_R820T:
+		fe = dvb_attach(r820t_attach, adap->fe[0], &d->i2c_adap,
+				&rtl2832u_r820t_config);
+
+		/* Use tuner to get the signal strength */
+		adap->fe[0]->ops.read_signal_strength =
+				adap->fe[0]->ops.tuner_ops.get_rf_strength;
+		break;
 	default:
 		fe = NULL;
 		dev_err(&d->udev->dev, "%s: unknown tuner=%d\n", KBUILD_MODNAME,
@@ -1372,6 +1406,8 @@ static const struct usb_device_id rtl28xxu_id_table[] = {
 		&rtl2832u_props, "Digivox Micro Hd", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_COMPRO, 0x0620,
 		&rtl2832u_props, "Compro VideoMate U620F", NULL) },
+	{ DVB_USB_DEVICE(USB_VID_KWORLD_2, 0xd394,
+		&rtl2832u_props, "MaxMedia HU394-T", NULL) },
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, rtl28xxu_id_table);
diff --git a/drivers/media/usb/dvb-usb-v2/rtl28xxu.h b/drivers/media/usb/dvb-usb-v2/rtl28xxu.h
index 2f3af2d3b6ce..533a33127289 100644
--- a/drivers/media/usb/dvb-usb-v2/rtl28xxu.h
+++ b/drivers/media/usb/dvb-usb-v2/rtl28xxu.h
@@ -82,6 +82,7 @@ enum rtl28xxu_tuner {
 	TUNER_RTL2832_E4000,
 	TUNER_RTL2832_TDA18272,
 	TUNER_RTL2832_FC0013,
+	TUNER_RTL2832_R820T,
 };
 
 struct rtl28xxu_req {
diff --git a/drivers/media/usb/dvb-usb-v2/usb_urb.c b/drivers/media/usb/dvb-usb-v2/usb_urb.c
index 7346f85f3f2f..ca8f3c2b1082 100644
--- a/drivers/media/usb/dvb-usb-v2/usb_urb.c
+++ b/drivers/media/usb/dvb-usb-v2/usb_urb.c
@@ -22,8 +22,8 @@ static void usb_urb_complete(struct urb *urb)
 	int i;
 	u8 *b;
 
-	dev_dbg_ratelimited(&stream->udev->dev, "%s: %s urb completed " \
-			"status=%d length=%d/%d pack_num=%d errors=%d\n",
+	dev_dbg_ratelimited(&stream->udev->dev,
+			"%s: %s urb completed status=%d length=%d/%d pack_num=%d errors=%d\n",
 			__func__, ptype == PIPE_ISOCHRONOUS ? "isoc" : "bulk",
 			urb->status, urb->actual_length,
 			urb->transfer_buffer_length,
@@ -49,8 +49,8 @@ static void usb_urb_complete(struct urb *urb)
 	case PIPE_ISOCHRONOUS:
 		for (i = 0; i < urb->number_of_packets; i++) {
 			if (urb->iso_frame_desc[i].status != 0)
-				dev_dbg(&stream->udev->dev, "%s: iso frame " \
-						"descriptor has an error=%d\n",
+				dev_dbg(&stream->udev->dev,
+						"%s: iso frame descriptor has an error=%d\n",
 						__func__,
 						urb->iso_frame_desc[i].status);
 			else if (urb->iso_frame_desc[i].actual_length > 0)
@@ -67,8 +67,9 @@ static void usb_urb_complete(struct urb *urb)
 			stream->complete(stream, b, urb->actual_length);
 		break;
 	default:
-		dev_err(&stream->udev->dev, "%s: unknown endpoint type in " \
-				"completition handler\n", KBUILD_MODNAME);
+		dev_err(&stream->udev->dev,
+				"%s: unknown endpoint type in completition handler\n",
+				KBUILD_MODNAME);
 		return;
 	}
 	usb_submit_urb(urb, GFP_ATOMIC);
@@ -101,8 +102,8 @@ int usb_urb_submitv2(struct usb_data_stream *stream,
 		dev_dbg(&stream->udev->dev, "%s: submit urb=%d\n", __func__, i);
 		ret = usb_submit_urb(stream->urb_list[i], GFP_ATOMIC);
 		if (ret) {
-			dev_err(&stream->udev->dev, "%s: could not submit " \
-					"urb no. %d - get them all back\n",
+			dev_err(&stream->udev->dev,
+					"%s: could not submit urb no. %d - get them all back\n",
 					KBUILD_MODNAME, i);
 			usb_urb_killv2(stream);
 			return ret;
@@ -229,8 +230,9 @@ static int usb_alloc_stream_buffers(struct usb_data_stream *stream, int num,
 	stream->buf_num = 0;
 	stream->buf_size = size;
 
-	dev_dbg(&stream->udev->dev, "%s: all in all I will use %lu bytes for " \
-			"streaming\n", __func__,  num * size);
+	dev_dbg(&stream->udev->dev,
+			"%s: all in all I will use %lu bytes for streaming\n",
+			__func__,  num * size);
 
 	for (stream->buf_num = 0; stream->buf_num < num; stream->buf_num++) {
 		stream->buf_list[stream->buf_num] = usb_alloc_coherent(
@@ -274,8 +276,8 @@ int usb_urb_reconfig(struct usb_data_stream *stream,
 	}
 
 	if (stream->buf_num < props->count || stream->buf_size < buf_size) {
-		dev_err(&stream->udev->dev, "%s: cannot reconfigure as " \
-				"allocated buffers are too small\n",
+		dev_err(&stream->udev->dev,
+				"%s: cannot reconfigure as allocated buffers are too small\n",
 				KBUILD_MODNAME);
 		return -EINVAL;
 	}
@@ -321,8 +323,9 @@ int usb_urb_initv2(struct usb_data_stream *stream,
 	memcpy(&stream->props, props, sizeof(*props));
 
 	if (!stream->complete) {
-		dev_err(&stream->udev->dev, "%s: there is no data callback - " \
-				"this doesn't make sense\n", KBUILD_MODNAME);
+		dev_err(&stream->udev->dev,
+				"%s: there is no data callback - this doesn't make sense\n",
+				KBUILD_MODNAME);
 		return -EINVAL;
 	}
 
@@ -343,8 +346,9 @@ int usb_urb_initv2(struct usb_data_stream *stream,
 
 		return usb_urb_alloc_isoc_urbs(stream);
 	default:
-		dev_err(&stream->udev->dev, "%s: unknown urb-type for data " \
-				"transfer\n", KBUILD_MODNAME);
+		dev_err(&stream->udev->dev,
+				"%s: unknown urb-type for data transfer\n",
+				KBUILD_MODNAME);
 		return -EINVAL;
 	}
 }
diff --git a/drivers/media/usb/dvb-usb/cinergyT2-fe.c b/drivers/media/usb/dvb-usb/cinergyT2-fe.c
index 1efc028a76c9..c890fe46acd3 100644
--- a/drivers/media/usb/dvb-usb/cinergyT2-fe.c
+++ b/drivers/media/usb/dvb-usb/cinergyT2-fe.c
@@ -300,8 +300,7 @@ static int cinergyt2_fe_set_frontend(struct dvb_frontend *fe)
 static void cinergyt2_fe_release(struct dvb_frontend *fe)
 {
 	struct cinergyt2_fe_state *state = fe->demodulator_priv;
-	if (state != NULL)
-		kfree(state);
+	kfree(state);
 }
 
 static struct dvb_frontend_ops cinergyt2_fe_ops;
diff --git a/drivers/media/usb/dvb-usb/dib0700_devices.c b/drivers/media/usb/dvb-usb/dib0700_devices.c
index 11798426fa88..f08136052f9c 100644
--- a/drivers/media/usb/dvb-usb/dib0700_devices.c
+++ b/drivers/media/usb/dvb-usb/dib0700_devices.c
@@ -1431,13 +1431,22 @@ static int dib8090_get_adc_power(struct dvb_frontend *fe)
 	return dib8000_get_adc_power(fe, 1);
 }
 
+static void dib8090_agc_control(struct dvb_frontend *fe, u8 restart)
+{
+	deb_info("AGC control callback: %i\n", restart);
+	dib0090_dcc_freq(fe, restart);
+
+	if (restart == 0) /* before AGC startup */
+		dib0090_set_dc_servo(fe, 1);
+}
+
 static struct dib8000_config dib809x_dib8000_config[2] = {
 	{
 	.output_mpeg2_in_188_bytes = 1,
 
 	.agc_config_count = 2,
 	.agc = dib8090_agc_config,
-	.agc_control = dib0090_dcc_freq,
+	.agc_control = dib8090_agc_control,
 	.pll = &dib8090_pll_config_12mhz,
 	.tuner_is_baseband = 1,
 
@@ -1456,7 +1465,7 @@ static struct dib8000_config dib809x_dib8000_config[2] = {
 
 	.agc_config_count = 2,
 	.agc = dib8090_agc_config,
-	.agc_control = dib0090_dcc_freq,
+	.agc_control = dib8090_agc_control,
 	.pll = &dib8090_pll_config_12mhz,
 	.tuner_is_baseband = 1,
 
@@ -1504,28 +1513,89 @@ static struct dib0090_config dib809x_dib0090_config = {
 	.fref_clock_ratio = 6,
 };
 
+static u8 dib8090_compute_pll_parameters(struct dvb_frontend *fe)
+{
+	u8 optimal_pll_ratio = 20;
+	u32 freq_adc, ratio, rest, max = 0;
+	u8 pll_ratio;
+
+	for (pll_ratio = 17; pll_ratio <= 20; pll_ratio++) {
+		freq_adc = 12 * pll_ratio * (1 << 8) / 16;
+		ratio = ((fe->dtv_property_cache.frequency / 1000) * (1 << 8) / 1000) / freq_adc;
+		rest = ((fe->dtv_property_cache.frequency / 1000) * (1 << 8) / 1000) - ratio * freq_adc;
+
+		if (rest > freq_adc / 2)
+			rest = freq_adc - rest;
+		deb_info("PLL ratio=%i rest=%i\n", pll_ratio, rest);
+		if ((rest > max) && (rest > 717)) {
+			optimal_pll_ratio = pll_ratio;
+			max = rest;
+		}
+	}
+	deb_info("optimal PLL ratio=%i\n", optimal_pll_ratio);
+
+	return optimal_pll_ratio;
+}
+
 static int dib8096_set_param_override(struct dvb_frontend *fe)
 {
-	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
 	struct dvb_usb_adapter *adap = fe->dvb->priv;
 	struct dib0700_adapter_state *state = adap->priv;
-	u8 band = BAND_OF_FREQUENCY(p->frequency/1000);
-	u16 target;
+	u8 pll_ratio, band = BAND_OF_FREQUENCY(fe->dtv_property_cache.frequency / 1000);
+	u16 target, ltgain, rf_gain_limit;
+	u32 timf;
 	int ret = 0;
 	enum frontend_tune_state tune_state = CT_SHUTDOWN;
-	u16 ltgain, rf_gain_limit;
+
+	switch (band) {
+	default:
+			deb_info("Warning : Rf frequency  (%iHz) is not in the supported range, using VHF switch ", fe->dtv_property_cache.frequency);
+	case BAND_VHF:
+			dib8000_set_gpio(fe, 3, 0, 1);
+			break;
+	case BAND_UHF:
+			dib8000_set_gpio(fe, 3, 0, 0);
+			break;
+	}
 
 	ret = state->set_param_save(fe);
 	if (ret < 0)
 		return ret;
 
-	target = (dib0090_get_wbd_target(fe) * 8 * 18 / 33 + 1) / 2;
-	dib8000_set_wbd_ref(fe, target);
+	if (fe->dtv_property_cache.bandwidth_hz != 6000000) {
+		deb_info("only 6MHz bandwidth is supported\n");
+		return -EINVAL;
+	}
+
+	/** Update PLL if needed ratio **/
+	dib8000_update_pll(fe, &dib8090_pll_config_12mhz, fe->dtv_property_cache.bandwidth_hz / 1000, 0);
+
+	/** Get optimize PLL ratio to remove spurious **/
+	pll_ratio = dib8090_compute_pll_parameters(fe);
+	if (pll_ratio == 17)
+		timf = 21387946;
+	else if (pll_ratio == 18)
+		timf = 20199727;
+	else if (pll_ratio == 19)
+		timf = 19136583;
+	else
+		timf = 18179756;
+
+	/** Update ratio **/
+	dib8000_update_pll(fe, &dib8090_pll_config_12mhz, fe->dtv_property_cache.bandwidth_hz / 1000, pll_ratio);
 
+	dib8000_ctrl_timf(fe, DEMOD_TIMF_SET, timf);
+
+	if (band != BAND_CBAND) {
+		/* dib0090_get_wbd_target is returning any possible temperature compensated wbd-target */
+		target = (dib0090_get_wbd_target(fe) * 8 * 18 / 33 + 1) / 2;
+		dib8000_set_wbd_ref(fe, target);
+	}
 
 	if (band == BAND_CBAND) {
 		deb_info("tuning in CBAND - soft-AGC startup\n");
 		dib0090_set_tune_state(fe, CT_AGC_START);
+
 		do {
 			ret = dib0090_gain_control(fe);
 			msleep(ret);
@@ -1534,14 +1604,17 @@ static int dib8096_set_param_override(struct dvb_frontend *fe)
 				dib8000_set_gpio(fe, 6, 0, 1);
 			else if (tune_state == CT_AGC_STEP_1) {
 				dib0090_get_current_gain(fe, NULL, NULL, &rf_gain_limit, &ltgain);
-				if (rf_gain_limit == 0)
+				if (rf_gain_limit < 2000) /* activate the external attenuator in case of very high input power */
 					dib8000_set_gpio(fe, 6, 0, 0);
 			}
 		} while (tune_state < CT_AGC_STOP);
+
+		deb_info("switching to PWM AGC\n");
 		dib0090_pwm_gain_reset(fe);
 		dib8000_pwm_agc_reset(fe);
 		dib8000_set_tune_state(fe, CT_DEMOD_START);
 	} else {
+		/* for everything else than CBAND we are using standard AGC */
 		deb_info("not tuning in CBAND - standard AGC startup\n");
 		dib0090_pwm_gain_reset(fe);
 	}
@@ -1814,21 +1887,92 @@ struct dibx090p_adc {
 	u32 pll_prediv;		/* New loopdiv */
 };
 
-struct dibx090p_adc dib8090p_adc_tab[] = {
-	{ 50000, 17043521, 16, 3}, /* 64 MHz */
-	{878000, 20199729, 9, 1}, /* 60 MHz */
-	{0xffffffff, 0, 0, 0}, /* 60 MHz */
+struct dibx090p_best_adc {
+	u32 timf;
+	u32 pll_loopdiv;
+	u32 pll_prediv;
 };
 
+static int dib8096p_get_best_sampling(struct dvb_frontend *fe, struct dibx090p_best_adc *adc)
+{
+	u8 spur = 0, prediv = 0, loopdiv = 0, min_prediv = 1, max_prediv = 1;
+	u16 xtal = 12000;
+	u16 fcp_min = 1900;  /* PLL, Minimum Frequency of phase comparator (KHz) */
+	u16 fcp_max = 20000; /* PLL, Maximum Frequency of phase comparator (KHz) */
+	u32 fmem_max = 140000; /* 140MHz max SDRAM freq */
+	u32 fdem_min = 66000;
+	u32 fcp = 0, fs = 0, fdem = 0, fmem = 0;
+	u32 harmonic_id = 0;
+
+	adc->timf = 0;
+	adc->pll_loopdiv = loopdiv;
+	adc->pll_prediv = prediv;
+
+	deb_info("bandwidth = %d", fe->dtv_property_cache.bandwidth_hz);
+
+	/* Find Min and Max prediv */
+	while ((xtal / max_prediv) >= fcp_min)
+		max_prediv++;
+
+	max_prediv--;
+	min_prediv = max_prediv;
+	while ((xtal / min_prediv) <= fcp_max) {
+		min_prediv--;
+		if (min_prediv == 1)
+			break;
+	}
+	deb_info("MIN prediv = %d : MAX prediv = %d", min_prediv, max_prediv);
+
+	min_prediv = 1;
+
+	for (prediv = min_prediv; prediv < max_prediv; prediv++) {
+		fcp = xtal / prediv;
+		if (fcp > fcp_min && fcp < fcp_max) {
+			for (loopdiv = 1; loopdiv < 64; loopdiv++) {
+				fmem = ((xtal/prediv) * loopdiv);
+				fdem = fmem / 2;
+				fs   = fdem / 4;
+
+				/* test min/max system restrictions */
+				if ((fdem >= fdem_min) && (fmem <= fmem_max) && (fs >= fe->dtv_property_cache.bandwidth_hz / 1000)) {
+					spur = 0;
+					/* test fs harmonics positions */
+					for (harmonic_id = (fe->dtv_property_cache.frequency / (1000 * fs));  harmonic_id <= ((fe->dtv_property_cache.frequency / (1000 * fs)) + 1); harmonic_id++) {
+						if (((fs * harmonic_id) >= (fe->dtv_property_cache.frequency / 1000 - (fe->dtv_property_cache.bandwidth_hz / 2000))) &&  ((fs * harmonic_id) <= (fe->dtv_property_cache.frequency / 1000 + (fe->dtv_property_cache.bandwidth_hz / 2000)))) {
+							spur = 1;
+							break;
+						}
+					}
+
+					if (!spur) {
+						adc->pll_loopdiv = loopdiv;
+						adc->pll_prediv = prediv;
+						adc->timf = (4260880253U / fdem) * (1 << 8);
+						adc->timf += ((4260880253U % fdem) << 8) / fdem;
+
+						deb_info("RF %6d; BW %6d; Xtal %6d; Fmem %6d; Fdem %6d; Fs %6d; Prediv %2d; Loopdiv %2d; Timf %8d;", fe->dtv_property_cache.frequency, fe->dtv_property_cache.bandwidth_hz, xtal, fmem, fdem, fs, prediv, loopdiv, adc->timf);
+						break;
+					}
+				}
+			}
+		}
+		if (!spur)
+			break;
+	}
+
+	if (adc->pll_loopdiv == 0 && adc->pll_prediv == 0)
+		return -EINVAL;
+	return 0;
+}
+
 static int dib8096p_agc_startup(struct dvb_frontend *fe)
 {
-	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
 	struct dvb_usb_adapter *adap = fe->dvb->priv;
 	struct dib0700_adapter_state *state = adap->priv;
 	struct dibx000_bandwidth_config pll;
+	struct dibx090p_best_adc adc;
 	u16 target;
-	int better_sampling_freq = 0, ret;
-	struct dibx090p_adc *adc_table = &dib8090p_adc_tab[0];
+	int ret;
 
 	ret = state->set_param_save(fe);
 	if (ret < 0)
@@ -1841,23 +1985,27 @@ static int dib8096p_agc_startup(struct dvb_frontend *fe)
 	target = (dib0090_get_wbd_target(fe) * 8  + 1) / 2;
 	dib8000_set_wbd_ref(fe, target);
 
+	if (dib8096p_get_best_sampling(fe, &adc) == 0) {
+		pll.pll_ratio  = adc.pll_loopdiv;
+		pll.pll_prediv = adc.pll_prediv;
 
-	while (p->frequency / 1000 > adc_table->freq) {
-		better_sampling_freq = 1;
-		adc_table++;
-	}
-
-	if ((adc_table->freq != 0xffffffff) && better_sampling_freq) {
-		pll.pll_ratio  = adc_table->pll_loopdiv;
-		pll.pll_prediv = adc_table->pll_prediv;
-		dib8000_update_pll(fe, &pll);
-		dib8000_ctrl_timf(fe, DEMOD_TIMF_SET, adc_table->timf);
+		dib0700_set_i2c_speed(adap->dev, 200);
+		dib8000_update_pll(fe, &pll, fe->dtv_property_cache.bandwidth_hz / 1000, 0);
+		dib8000_ctrl_timf(fe, DEMOD_TIMF_SET, adc.timf);
+		dib0700_set_i2c_speed(adap->dev, 1000);
 	}
 	return 0;
 }
 
 static int tfe8096p_frontend_attach(struct dvb_usb_adapter *adap)
 {
+	struct dib0700_state *st = adap->dev->priv;
+	u32 fw_version;
+
+	dib0700_get_version(adap->dev, NULL, NULL, &fw_version, NULL);
+	if (fw_version >= 0x10200)
+		st->fw_use_new_i2c_api = 1;
+
 	dib0700_set_gpio(adap->dev, GPIO6, GPIO_OUT, 1);
 	msleep(20);
 	dib0700_set_gpio(adap->dev, GPIO9, GPIO_OUT, 1);
@@ -2242,13 +2390,7 @@ static int nim9090md_tuner_attach(struct dvb_usb_adapter *adap)
 }
 
 /* NIM7090 */
-struct dib7090p_best_adc {
-	u32 timf;
-	u32 pll_loopdiv;
-	u32 pll_prediv;
-};
-
-static int dib7090p_get_best_sampling(struct dvb_frontend *fe , struct dib7090p_best_adc *adc)
+static int dib7090p_get_best_sampling(struct dvb_frontend *fe , struct dibx090p_best_adc *adc)
 {
 	u8 spur = 0, prediv = 0, loopdiv = 0, min_prediv = 1, max_prediv = 1;
 
@@ -2327,7 +2469,7 @@ static int dib7090_agc_startup(struct dvb_frontend *fe)
 	struct dib0700_adapter_state *state = adap->priv;
 	struct dibx000_bandwidth_config pll;
 	u16 target;
-	struct dib7090p_best_adc adc;
+	struct dibx090p_best_adc adc;
 	int ret;
 
 	ret = state->set_param_save(fe);
@@ -2357,36 +2499,16 @@ static int dib7090_agc_restart(struct dvb_frontend *fe, u8 restart)
 	return 0;
 }
 
-static int dib7090e_update_lna(struct dvb_frontend *fe, u16 agc_global)
+static int tfe7790p_update_lna(struct dvb_frontend *fe, u16 agc_global)
 {
-	u16 agc1 = 0, agc2, wbd = 0, wbd_target, wbd_offset, threshold_agc1;
-	s16 wbd_delta;
+	deb_info("update LNA: agc global=%i", agc_global);
 
-	if ((fe->dtv_property_cache.frequency) < 400000000)
-		threshold_agc1 = 25000;
-	else
-		threshold_agc1 = 30000;
-
-	wbd_target = (dib0090_get_wbd_target(fe)*8+1)/2;
-	wbd_offset = dib0090_get_wbd_offset(fe);
-	dib7000p_get_agc_values(fe, NULL, &agc1, &agc2, &wbd);
-	wbd_delta = (s16)wbd - (((s16)wbd_offset+10)*4) ;
-
-	deb_info("update lna, agc_global=%d agc1=%d agc2=%d",
-			agc_global, agc1, agc2);
-	deb_info("update lna, wbd=%d wbd target=%d wbd offset=%d wbd delta=%d",
-			wbd, wbd_target, wbd_offset, wbd_delta);
-
-	if ((agc1 < threshold_agc1) && (wbd_delta > 0)) {
-		dib0090_set_switch(fe, 1, 1, 1);
-		dib0090_set_vga(fe, 0);
-		dib0090_update_rframp_7090(fe, 0);
-		dib0090_update_tuning_table_7090(fe, 0);
+	if (agc_global < 25000) {
+		dib7000p_set_gpio(fe, 8, 0, 0);
+		dib7000p_set_agc1_min(fe, 0);
 	} else {
-		dib0090_set_vga(fe, 1);
-		dib0090_update_rframp_7090(fe, 1);
-		dib0090_update_tuning_table_7090(fe, 1);
-		dib0090_set_switch(fe, 0, 0, 0);
+		dib7000p_set_gpio(fe, 8, 0, 1);
+		dib7000p_set_agc1_min(fe, 32768);
 	}
 
 	return 0;
@@ -2400,15 +2522,6 @@ static struct dib0090_wbd_slope dib7090_wbd_table[] = {
 	{ 0xFFFF, 0,   0, 0,   0,   0},
 };
 
-static struct dib0090_wbd_slope dib7090e_wbd_table[] = {
-	{ 380,   81, 850, 64, 540,	4},
-	{ 700,   51, 866, 21,  320,	4},
-	{ 860,   48, 666, 18,  330,	6},
-	{1700,    0, 250, 0,   100, 6},
-	{2600,    0, 250, 0,   100, 6},
-	{ 0xFFFF, 0,   0, 0,   0,	0},
-};
-
 static struct dibx000_agc_config dib7090_agc_config[2] = {
 	{
 		.band_caps      = BAND_UHF,
@@ -2428,7 +2541,7 @@ static struct dibx000_agc_config dib7090_agc_config[2] = {
 		.wbd_alpha      = 5,
 
 		.agc1_max       = 65535,
-		.agc1_min       = 0,
+		.agc1_min       = 32768,
 
 		.agc2_max       = 65535,
 		.agc2_min       = 0,
@@ -2505,7 +2618,7 @@ static struct dib7000p_config nim7090_dib7000p_config = {
 	.output_mpeg2_in_188_bytes  = 1,
 	.hostbus_diversity			= 1,
 	.tuner_is_baseband			= 1,
-	.update_lna					= NULL,
+	.update_lna					= tfe7790p_update_lna, /* GPIO used is the same as TFE7790 */
 
 	.agc_config_count			= 2,
 	.agc						= dib7090_agc_config,
@@ -2529,12 +2642,26 @@ static struct dib7000p_config nim7090_dib7000p_config = {
 	.enMpegOutput				= 1,
 };
 
+static int tfe7090p_pvr_update_lna(struct dvb_frontend *fe, u16 agc_global)
+{
+	deb_info("TFE7090P-PVR update LNA: agc global=%i", agc_global);
+	if (agc_global < 25000) {
+		dib7000p_set_gpio(fe, 5, 0, 0);
+		dib7000p_set_agc1_min(fe, 0);
+	} else {
+		dib7000p_set_gpio(fe, 5, 0, 1);
+		dib7000p_set_agc1_min(fe, 32768);
+	}
+
+	return 0;
+}
+
 static struct dib7000p_config tfe7090pvr_dib7000p_config[2] = {
 	{
 		.output_mpeg2_in_188_bytes  = 1,
 		.hostbus_diversity			= 1,
 		.tuner_is_baseband			= 1,
-		.update_lna					= NULL,
+		.update_lna					= tfe7090p_pvr_update_lna,
 
 		.agc_config_count			= 2,
 		.agc						= dib7090_agc_config,
@@ -2561,7 +2688,7 @@ static struct dib7000p_config tfe7090pvr_dib7000p_config[2] = {
 		.output_mpeg2_in_188_bytes  = 1,
 		.hostbus_diversity			= 1,
 		.tuner_is_baseband			= 1,
-		.update_lna					= NULL,
+		.update_lna					= tfe7090p_pvr_update_lna,
 
 		.agc_config_count			= 2,
 		.agc						= dib7090_agc_config,
@@ -2587,34 +2714,6 @@ static struct dib7000p_config tfe7090pvr_dib7000p_config[2] = {
 	}
 };
 
-static struct dib7000p_config tfe7090e_dib7000p_config = {
-	.output_mpeg2_in_188_bytes  = 1,
-	.hostbus_diversity			= 1,
-	.tuner_is_baseband			= 1,
-	.update_lna					= dib7090e_update_lna,
-
-	.agc_config_count			= 2,
-	.agc						= dib7090_agc_config,
-
-	.bw							= &dib7090_clock_config_12_mhz,
-
-	.gpio_dir					= DIB7000P_GPIO_DEFAULT_DIRECTIONS,
-	.gpio_val					= DIB7000P_GPIO_DEFAULT_VALUES,
-	.gpio_pwm_pos				= DIB7000P_GPIO_DEFAULT_PWM_POS,
-
-	.pwm_freq_div				= 0,
-
-	.agc_control				= dib7090_agc_restart,
-
-	.spur_protect				= 0,
-	.disable_sample_and_hold	= 0,
-	.enable_current_mirror		= 0,
-	.diversity_delay			= 0,
-
-	.output_mode				= OUTMODE_MPEG2_FIFO,
-	.enMpegOutput				= 1,
-};
-
 static const struct dib0090_config nim7090_dib0090_config = {
 	.io.clock_khz = 12000,
 	.io.pll_bypass = 0,
@@ -2649,47 +2748,11 @@ static const struct dib0090_config nim7090_dib0090_config = {
 	.in_soc = 1,
 };
 
-static const struct dib0090_config tfe7090e_dib0090_config = {
-	.io.clock_khz = 12000,
-	.io.pll_bypass = 0,
-	.io.pll_range = 0,
-	.io.pll_prediv = 3,
-	.io.pll_loopdiv = 6,
-	.io.adc_clock_ratio = 0,
-	.io.pll_int_loop_filt = 0,
-	.reset = dib7090_tuner_sleep,
-	.sleep = dib7090_tuner_sleep,
-
-	.freq_offset_khz_uhf = 0,
-	.freq_offset_khz_vhf = 0,
-
-	.get_adc_power = dib7090_get_adc_power,
-
-	.clkouttobamse = 1,
-	.analog_output = 0,
-
-	.wbd_vhf_offset = 0,
-	.wbd_cband_offset = 0,
-	.use_pwm_agc = 1,
-	.clkoutdrive = 0,
-
-	.fref_clock_ratio = 0,
-
-	.wbd = dib7090e_wbd_table,
-
-	.ls_cfg_pad_drv = 0,
-	.data_tx_drv = 0,
-	.low_if = NULL,
-	.in_soc = 1,
-	.force_cband_input = 1,
-	.is_dib7090e = 1,
-};
-
-static struct dib7000p_config tfe7790e_dib7000p_config = {
+static struct dib7000p_config tfe7790p_dib7000p_config = {
 	.output_mpeg2_in_188_bytes  = 1,
 	.hostbus_diversity			= 1,
 	.tuner_is_baseband			= 1,
-	.update_lna					= dib7090e_update_lna,
+	.update_lna					= tfe7790p_update_lna,
 
 	.agc_config_count			= 2,
 	.agc						= dib7090_agc_config,
@@ -2713,7 +2776,7 @@ static struct dib7000p_config tfe7790e_dib7000p_config = {
 	.enMpegOutput				= 1,
 };
 
-static const struct dib0090_config tfe7790e_dib0090_config = {
+static const struct dib0090_config tfe7790p_dib0090_config = {
 	.io.clock_khz = 12000,
 	.io.pll_bypass = 0,
 	.io.pll_range = 0,
@@ -2739,14 +2802,14 @@ static const struct dib0090_config tfe7790e_dib0090_config = {
 
 	.fref_clock_ratio = 0,
 
-	.wbd = dib7090e_wbd_table,
+	.wbd = dib7090_wbd_table,
 
 	.ls_cfg_pad_drv = 0,
 	.data_tx_drv = 0,
 	.low_if = NULL,
 	.in_soc = 1,
-	.force_cband_input = 1,
-	.is_dib7090e = 1,
+	.force_cband_input = 0,
+	.is_dib7090e = 0,
 	.force_crystal_mode = 1,
 };
 
@@ -2942,37 +3005,11 @@ static int tfe7090pvr_tuner1_attach(struct dvb_usb_adapter *adap)
 	return 0;
 }
 
-static int tfe7090e_frontend_attach(struct dvb_usb_adapter *adap)
-{
-	dib0700_set_gpio(adap->dev, GPIO6, GPIO_OUT, 1);
-	msleep(20);
-	dib0700_set_gpio(adap->dev, GPIO9, GPIO_OUT, 1);
-	dib0700_set_gpio(adap->dev, GPIO4, GPIO_OUT, 1);
-	dib0700_set_gpio(adap->dev, GPIO7, GPIO_OUT, 1);
-	dib0700_set_gpio(adap->dev, GPIO10, GPIO_OUT, 0);
-
-	msleep(20);
-	dib0700_set_gpio(adap->dev, GPIO10, GPIO_OUT, 1);
-	msleep(20);
-	dib0700_set_gpio(adap->dev, GPIO0, GPIO_OUT, 1);
-
-	if (dib7000p_i2c_enumeration(&adap->dev->i2c_adap,
-				1, 0x10, &tfe7090e_dib7000p_config) != 0) {
-		err("%s: dib7000p_i2c_enumeration failed.  Cannot continue\n",
-				__func__);
-		return -ENODEV;
-	}
-	adap->fe_adap[0].fe = dvb_attach(dib7000p_attach, &adap->dev->i2c_adap,
-			0x80, &tfe7090e_dib7000p_config);
-
-	return adap->fe_adap[0].fe == NULL ?  -ENODEV : 0;
-}
-
-static int tfe7790e_frontend_attach(struct dvb_usb_adapter *adap)
+static int tfe7790p_frontend_attach(struct dvb_usb_adapter *adap)
 {
 	struct dib0700_state *st = adap->dev->priv;
 
-	/* The TFE7790E requires the dib0700 to not be in master mode */
+	/* The TFE7790P requires the dib0700 to not be in master mode */
 	st->disable_streaming_master_mode = 1;
 
 	dib0700_set_gpio(adap->dev, GPIO6, GPIO_OUT, 1);
@@ -2988,42 +3025,25 @@ static int tfe7790e_frontend_attach(struct dvb_usb_adapter *adap)
 	dib0700_set_gpio(adap->dev, GPIO0, GPIO_OUT, 1);
 
 	if (dib7000p_i2c_enumeration(&adap->dev->i2c_adap,
-				1, 0x10, &tfe7790e_dib7000p_config) != 0) {
+				1, 0x10, &tfe7790p_dib7000p_config) != 0) {
 		err("%s: dib7000p_i2c_enumeration failed.  Cannot continue\n",
 				__func__);
 		return -ENODEV;
 	}
 	adap->fe_adap[0].fe = dvb_attach(dib7000p_attach, &adap->dev->i2c_adap,
-			0x80, &tfe7790e_dib7000p_config);
+			0x80, &tfe7790p_dib7000p_config);
 
 	return adap->fe_adap[0].fe == NULL ?  -ENODEV : 0;
 }
 
-static int tfe7790e_tuner_attach(struct dvb_usb_adapter *adap)
-{
-	struct dib0700_adapter_state *st = adap->priv;
-	struct i2c_adapter *tun_i2c =
-		dib7090_get_i2c_tuner(adap->fe_adap[0].fe);
-
-	if (dvb_attach(dib0090_register, adap->fe_adap[0].fe, tun_i2c,
-				&tfe7790e_dib0090_config) == NULL)
-		return -ENODEV;
-
-	dib7000p_set_gpio(adap->fe_adap[0].fe, 8, 0, 1);
-
-	st->set_param_save = adap->fe_adap[0].fe->ops.tuner_ops.set_params;
-	adap->fe_adap[0].fe->ops.tuner_ops.set_params = dib7090_agc_startup;
-	return 0;
-}
-
-static int tfe7090e_tuner_attach(struct dvb_usb_adapter *adap)
+static int tfe7790p_tuner_attach(struct dvb_usb_adapter *adap)
 {
 	struct dib0700_adapter_state *st = adap->priv;
 	struct i2c_adapter *tun_i2c =
 		dib7090_get_i2c_tuner(adap->fe_adap[0].fe);
 
 	if (dvb_attach(dib0090_register, adap->fe_adap[0].fe, tun_i2c,
-				&tfe7090e_dib0090_config) == NULL)
+				&tfe7790p_dib0090_config) == NULL)
 		return -ENODEV;
 
 	dib7000p_set_gpio(adap->fe_adap[0].fe, 8, 0, 1);
@@ -3566,10 +3586,9 @@ struct usb_device_id dib0700_usb_id_table[] = {
 /* 75 */{ USB_DEVICE(USB_VID_MEDION,    USB_PID_CREATIX_CTX1921) },
 	{ USB_DEVICE(USB_VID_PINNACLE,  USB_PID_PINNACLE_PCTV340E) },
 	{ USB_DEVICE(USB_VID_PINNACLE,  USB_PID_PINNACLE_PCTV340E_SE) },
-	{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_TFE7090E) },
-	{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_TFE7790E) },
-/* 80 */{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_TFE8096P) },
-	{ USB_DEVICE(USB_VID_ELGATO,	USB_PID_ELGATO_EYETV_DTT_2) },
+	{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_TFE7790P) },
+	{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_TFE8096P) },
+/* 80 */{ USB_DEVICE(USB_VID_ELGATO,	USB_PID_ELGATO_EYETV_DTT_2) },
 	{ 0 }		/* Terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, dib0700_usb_id_table);
@@ -3880,7 +3899,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 				{ NULL },
 			},
 			{   "Elgato EyeTV DTT rev. 2",
-				{ &dib0700_usb_id_table[81], NULL },
+				{ &dib0700_usb_id_table[80], NULL },
 				{ NULL },
 			},
 		},
@@ -4697,48 +4716,8 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 					.pid_filter_count = 32,
 					.pid_filter = stk70x0p_pid_filter,
 					.pid_filter_ctrl = stk70x0p_pid_filter_ctrl,
-					.frontend_attach  = tfe7090e_frontend_attach,
-					.tuner_attach     = tfe7090e_tuner_attach,
-
-					DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
-				} },
-
-				.size_of_priv =
-					sizeof(struct dib0700_adapter_state),
-			},
-		},
-
-		.num_device_descs = 1,
-		.devices = {
-			{   "DiBcom TFE7090E reference design",
-				{ &dib0700_usb_id_table[78], NULL },
-				{ NULL },
-			},
-		},
-
-		.rc.core = {
-			.rc_interval      = DEFAULT_RC_INTERVAL,
-			.rc_codes         = RC_MAP_DIB0700_RC5_TABLE,
-			.module_name	  = "dib0700",
-			.rc_query         = dib0700_rc_query_old_firmware,
-			.allowed_protos   = RC_BIT_RC5 |
-					    RC_BIT_RC6_MCE |
-					    RC_BIT_NEC,
-			.change_protocol  = dib0700_change_protocol,
-		},
-	}, { DIB0700_DEFAULT_DEVICE_PROPERTIES,
-		.num_adapters = 1,
-		.adapter = {
-			{
-				.num_frontends = 1,
-				.fe = {{
-					.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
-						DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
-					.pid_filter_count = 32,
-					.pid_filter = stk70x0p_pid_filter,
-					.pid_filter_ctrl = stk70x0p_pid_filter_ctrl,
-					.frontend_attach  = tfe7790e_frontend_attach,
-					.tuner_attach     = tfe7790e_tuner_attach,
+					.frontend_attach  = tfe7790p_frontend_attach,
+					.tuner_attach     = tfe7790p_tuner_attach,
 
 					DIB0700_DEFAULT_STREAMING_CONFIG(0x03),
 				} },
@@ -4750,8 +4729,8 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 		.num_device_descs = 1,
 		.devices = {
-			{   "DiBcom TFE7790E reference design",
-				{ &dib0700_usb_id_table[79], NULL },
+			{   "DiBcom TFE7790P reference design",
+				{ &dib0700_usb_id_table[78], NULL },
 				{ NULL },
 			},
 		},
@@ -4792,7 +4771,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_device_descs = 1,
 		.devices = {
 			{   "DiBcom TFE8096P reference design",
-				{ &dib0700_usb_id_table[80], NULL },
+				{ &dib0700_usb_id_table[79], NULL },
 				{ NULL },
 			},
 		},
diff --git a/drivers/media/usb/dvb-usb/dibusb-common.c b/drivers/media/usb/dvb-usb/dibusb-common.c
index af0d4321845b..c2dded92f1d3 100644
--- a/drivers/media/usb/dvb-usb/dibusb-common.c
+++ b/drivers/media/usb/dvb-usb/dibusb-common.c
@@ -8,6 +8,8 @@
  *
  * see Documentation/dvb/README.dvb-usb for more information
  */
+
+#include <linux/kconfig.h>
 #include "dibusb.h"
 
 static int debug;
@@ -232,8 +234,7 @@ static struct dibx000_agc_config dib3000p_panasonic_agc_config = {
 	.agc2_slope2 = 0x1e,
 };
 
-#if defined(CONFIG_DVB_DIB3000MC) || 					\
-	(defined(CONFIG_DVB_DIB3000MC_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_DVB_DIB3000MC)
 
 static struct dib3000mc_config mod3000p_dib3000p_config = {
 	&dib3000p_panasonic_agc_config,
diff --git a/drivers/media/usb/dvb-usb/dw2102.c b/drivers/media/usb/dvb-usb/dw2102.c
index 9578a6761f1b..6e237b6dd0a8 100644
--- a/drivers/media/usb/dvb-usb/dw2102.c
+++ b/drivers/media/usb/dvb-usb/dw2102.c
@@ -29,7 +29,6 @@
 #include "stb6100.h"
 #include "stb6100_proc.h"
 #include "m88rs2000.h"
-#include "ts2020.h"
 
 #ifndef USB_PID_DW2102
 #define USB_PID_DW2102 0x2102
@@ -79,6 +78,10 @@
 #define USB_PID_TEVII_S632 0xd632
 #endif
 
+#ifndef USB_PID_GOTVIEW_SAT_HD
+#define USB_PID_GOTVIEW_SAT_HD 0x5456
+#endif
+
 #define DW210X_READ_MSG 0
 #define DW210X_WRITE_MSG 1
 
@@ -1548,6 +1551,8 @@ enum dw2102_table_entry {
 	X3M_SPC1400HD,
 	TEVII_S421,
 	TEVII_S632,
+	TERRATEC_CINERGY_S2_R2,
+	GOTVIEW_SAT_HD,
 };
 
 static struct usb_device_id dw2102_table[] = {
@@ -1568,6 +1573,8 @@ static struct usb_device_id dw2102_table[] = {
 	[X3M_SPC1400HD] = {USB_DEVICE(0x1f4d, 0x3100)},
 	[TEVII_S421] = {USB_DEVICE(0x9022, USB_PID_TEVII_S421)},
 	[TEVII_S632] = {USB_DEVICE(0x9022, USB_PID_TEVII_S632)},
+	[TERRATEC_CINERGY_S2_R2] = {USB_DEVICE(USB_VID_TERRATEC, 0x00b0)},
+	[GOTVIEW_SAT_HD] = {USB_DEVICE(0x1FE1, USB_PID_GOTVIEW_SAT_HD)},
 	{ }
 };
 
@@ -1968,7 +1975,7 @@ static struct dvb_usb_device_properties su3000_properties = {
 		}},
 		}
 	},
-	.num_device_descs = 3,
+	.num_device_descs = 5,
 	.devices = {
 		{ "SU3000HD DVB-S USB2.0",
 			{ &dw2102_table[GENIATECH_SU3000], NULL },
@@ -1982,6 +1989,14 @@ static struct dvb_usb_device_properties su3000_properties = {
 			{ &dw2102_table[X3M_SPC1400HD], NULL },
 			{ NULL },
 		},
+		{ "Terratec Cinergy S2 USB HD Rev.2",
+			{ &dw2102_table[TERRATEC_CINERGY_S2_R2], NULL },
+			{ NULL },
+		},
+		{ "GOTVIEW Satellite HD",
+			{ &dw2102_table[GOTVIEW_SAT_HD], NULL },
+			{ NULL },
+		},
 	}
 };
 
diff --git a/drivers/media/usb/dvb-usb/m920x.c b/drivers/media/usb/dvb-usb/m920x.c
index 92afeb20650f..c2b635d6a17a 100644
--- a/drivers/media/usb/dvb-usb/m920x.c
+++ b/drivers/media/usb/dvb-usb/m920x.c
@@ -68,20 +68,20 @@ static inline int m920x_write_seq(struct usb_device *udev, u8 request,
 				  struct m920x_inits *seq)
 {
 	int ret;
-	while (seq->address) {
+	do {
 		ret = m920x_write(udev, request, seq->data, seq->address);
 		if (ret != 0)
 			return ret;
 
 		seq++;
-	}
+	} while (seq->address);
 
-	return ret;
+	return 0;
 }
 
 static int m920x_init(struct dvb_usb_device *d, struct m920x_inits *rc_seq)
 {
-	int ret = 0, i, epi, flags = 0;
+	int ret, i, epi, flags = 0;
 	int adap_enabled[M9206_MAX_ADAPTERS] = { 0 };
 
 	/* Remote controller init. */
@@ -124,7 +124,7 @@ static int m920x_init(struct dvb_usb_device *d, struct m920x_inits *rc_seq)
 		}
 	}
 
-	return ret;
+	return 0;
 }
 
 static int m920x_init_ep(struct usb_interface *intf)
diff --git a/drivers/media/usb/em28xx/Kconfig b/drivers/media/usb/em28xx/Kconfig
index c754a80a8d8b..ca5ee6aceb62 100644
--- a/drivers/media/usb/em28xx/Kconfig
+++ b/drivers/media/usb/em28xx/Kconfig
@@ -46,6 +46,7 @@ config VIDEO_EM28XX_DVB
 	select DVB_A8293 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_MT352 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_S5H1409 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_MB86A20S if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_QT1010 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_TDA18271 if MEDIA_SUBDRV_AUTOSELECT
 	---help---
diff --git a/drivers/media/usb/em28xx/Makefile b/drivers/media/usb/em28xx/Makefile
index 634fb920dd39..ad6d48557940 100644
--- a/drivers/media/usb/em28xx/Makefile
+++ b/drivers/media/usb/em28xx/Makefile
@@ -1,5 +1,5 @@
 em28xx-y +=	em28xx-video.o em28xx-i2c.o em28xx-cards.o
-em28xx-y +=	em28xx-core.o  em28xx-vbi.o
+em28xx-y +=	em28xx-core.o  em28xx-vbi.o em28xx-camera.o
 
 em28xx-alsa-objs := em28xx-audio.o
 em28xx-rc-objs := em28xx-input.o
diff --git a/drivers/media/usb/em28xx/em28xx-camera.c b/drivers/media/usb/em28xx/em28xx-camera.c
new file mode 100644
index 000000000000..73cc50afa5e1
--- /dev/null
+++ b/drivers/media/usb/em28xx/em28xx-camera.c
@@ -0,0 +1,434 @@
+/*
+   em28xx-camera.c - driver for Empia EM25xx/27xx/28xx USB video capture devices
+
+   Copyright (C) 2009 Mauro Carvalho Chehab <mchehab@infradead.org>
+   Copyright (C) 2013 Frank Schäfer <fschaefer.oss@googlemail.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., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+
+#include <linux/i2c.h>
+#include <media/soc_camera.h>
+#include <media/mt9v011.h>
+#include <media/v4l2-common.h>
+
+#include "em28xx.h"
+
+
+/* Possible i2c addresses of Micron sensors */
+static unsigned short micron_sensor_addrs[] = {
+	0xb8 >> 1,   /* MT9V111, MT9V403 */
+	0xba >> 1,   /* MT9M001/011/111/112, MT9V011/012/112, MT9D011 */
+	0x90 >> 1,   /* MT9V012/112, MT9D011 (alternative address) */
+	I2C_CLIENT_END
+};
+
+/* Possible i2c addresses of Omnivision sensors */
+static unsigned short omnivision_sensor_addrs[] = {
+	0x42 >> 1,   /* OV7725, OV7670/60/48 */
+	0x60 >> 1,   /* OV2640, OV9650/53/55 */
+	I2C_CLIENT_END
+};
+
+
+static struct soc_camera_link camlink = {
+	.bus_id = 0,
+	.flags = 0,
+	.module_name = "em28xx",
+};
+
+
+/* FIXME: Should be replaced by a proper mt9m111 driver */
+static int em28xx_initialize_mt9m111(struct em28xx *dev)
+{
+	int i;
+	unsigned char regs[][3] = {
+		{ 0x0d, 0x00, 0x01, },  /* reset and use defaults */
+		{ 0x0d, 0x00, 0x00, },
+		{ 0x0a, 0x00, 0x21, },
+		{ 0x21, 0x04, 0x00, },  /* full readout speed, no row/col skipping */
+	};
+
+	for (i = 0; i < ARRAY_SIZE(regs); i++)
+		i2c_master_send(&dev->i2c_client[dev->def_i2c_bus],
+				&regs[i][0], 3);
+
+	return 0;
+}
+
+
+/* FIXME: Should be replaced by a proper mt9m001 driver */
+static int em28xx_initialize_mt9m001(struct em28xx *dev)
+{
+	int i;
+	unsigned char regs[][3] = {
+		{ 0x0d, 0x00, 0x01, },
+		{ 0x0d, 0x00, 0x00, },
+		{ 0x04, 0x05, 0x00, },	/* hres = 1280 */
+		{ 0x03, 0x04, 0x00, },  /* vres = 1024 */
+		{ 0x20, 0x11, 0x00, },
+		{ 0x06, 0x00, 0x10, },
+		{ 0x2b, 0x00, 0x24, },
+		{ 0x2e, 0x00, 0x24, },
+		{ 0x35, 0x00, 0x24, },
+		{ 0x2d, 0x00, 0x20, },
+		{ 0x2c, 0x00, 0x20, },
+		{ 0x09, 0x0a, 0xd4, },
+		{ 0x35, 0x00, 0x57, },
+	};
+
+	for (i = 0; i < ARRAY_SIZE(regs); i++)
+		i2c_master_send(&dev->i2c_client[dev->def_i2c_bus],
+				&regs[i][0], 3);
+
+	return 0;
+}
+
+
+/*
+ * Probes Micron sensors with 8 bit address and 16 bit register width
+ */
+static int em28xx_probe_sensor_micron(struct em28xx *dev)
+{
+	int ret, i;
+	char *name;
+	u8 reg;
+	__be16 id_be;
+	u16 id;
+
+	struct i2c_client client = dev->i2c_client[dev->def_i2c_bus];
+
+	dev->em28xx_sensor = EM28XX_NOSENSOR;
+	for (i = 0; micron_sensor_addrs[i] != I2C_CLIENT_END; i++) {
+		client.addr = micron_sensor_addrs[i];
+		/* NOTE: i2c_smbus_read_word_data() doesn't work with BE data */
+		/* Read chip ID from register 0x00 */
+		reg = 0x00;
+		ret = i2c_master_send(&client, &reg, 1);
+		if (ret < 0) {
+			if (ret != -ENODEV)
+				em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+					      client.addr << 1, ret);
+			continue;
+		}
+		ret = i2c_master_recv(&client, (u8 *)&id_be, 2);
+		if (ret < 0) {
+			em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+				      client.addr << 1, ret);
+			continue;
+		}
+		id = be16_to_cpu(id_be);
+		/* Read chip ID from register 0xff */
+		reg = 0xff;
+		ret = i2c_master_send(&client, &reg, 1);
+		if (ret < 0) {
+			em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+				      client.addr << 1, ret);
+			continue;
+		}
+		ret = i2c_master_recv(&client, (u8 *)&id_be, 2);
+		if (ret < 0) {
+			em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+				      client.addr << 1, ret);
+			continue;
+		}
+		/* Validate chip ID to be sure we have a Micron device */
+		if (id != be16_to_cpu(id_be))
+			continue;
+		/* Check chip ID */
+		id = be16_to_cpu(id_be);
+		switch (id) {
+		case 0x1222:
+			name = "MT9V012"; /* MI370 */ /* 640x480 */
+			break;
+		case 0x1229:
+			name = "MT9V112"; /* 640x480 */
+			break;
+		case 0x1433:
+			name = "MT9M011"; /* 1280x1024 */
+			break;
+		case 0x143a:    /* found in the ECS G200 */
+			name = "MT9M111"; /* MI1310 */ /* 1280x1024 */
+			dev->em28xx_sensor = EM28XX_MT9M111;
+			break;
+		case 0x148c:
+			name = "MT9M112"; /* MI1320 */ /* 1280x1024 */
+			break;
+		case 0x1511:
+			name = "MT9D011"; /* MI2010 */ /* 1600x1200 */
+			break;
+		case 0x8232:
+		case 0x8243:	/* rev B */
+			name = "MT9V011"; /* MI360 */ /* 640x480 */
+			dev->em28xx_sensor = EM28XX_MT9V011;
+			break;
+		case 0x8431:
+			name = "MT9M001"; /* 1280x1024 */
+			dev->em28xx_sensor = EM28XX_MT9M001;
+			break;
+		default:
+			em28xx_info("unknown Micron sensor detected: 0x%04x\n",
+				    id);
+			return 0;
+		}
+
+		if (dev->em28xx_sensor == EM28XX_NOSENSOR)
+			em28xx_info("unsupported sensor detected: %s\n", name);
+		else
+			em28xx_info("sensor %s detected\n", name);
+
+		dev->i2c_client[dev->def_i2c_bus].addr = client.addr;
+		return 0;
+	}
+
+	return -ENODEV;
+}
+
+/*
+ * Probes Omnivision sensors with 8 bit address and register width
+ */
+static int em28xx_probe_sensor_omnivision(struct em28xx *dev)
+{
+	int ret, i;
+	char *name;
+	u8 reg;
+	u16 id;
+	struct i2c_client client = dev->i2c_client[dev->def_i2c_bus];
+
+	dev->em28xx_sensor = EM28XX_NOSENSOR;
+	/* NOTE: these devices have the register auto incrementation disabled
+	 * by default, so we have to use single byte reads !              */
+	for (i = 0; omnivision_sensor_addrs[i] != I2C_CLIENT_END; i++) {
+		client.addr = omnivision_sensor_addrs[i];
+		/* Read manufacturer ID from registers 0x1c-0x1d (BE) */
+		reg = 0x1c;
+		ret = i2c_smbus_read_byte_data(&client, reg);
+		if (ret < 0) {
+			if (ret != -ENODEV)
+				em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+					      client.addr << 1, ret);
+			continue;
+		}
+		id = ret << 8;
+		reg = 0x1d;
+		ret = i2c_smbus_read_byte_data(&client, reg);
+		if (ret < 0) {
+			em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+				      client.addr << 1, ret);
+			continue;
+		}
+		id += ret;
+		/* Check manufacturer ID */
+		if (id != 0x7fa2)
+			continue;
+		/* Read product ID from registers 0x0a-0x0b (BE) */
+		reg = 0x0a;
+		ret = i2c_smbus_read_byte_data(&client, reg);
+		if (ret < 0) {
+			em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+				      client.addr << 1, ret);
+			continue;
+		}
+		id = ret << 8;
+		reg = 0x0b;
+		ret = i2c_smbus_read_byte_data(&client, reg);
+		if (ret < 0) {
+			em28xx_errdev("couldn't read from i2c device 0x%02x: error %i\n",
+				      client.addr << 1, ret);
+			continue;
+		}
+		id += ret;
+		/* Check product ID */
+		switch (id) {
+		case 0x2642:
+			name = "OV2640";
+			dev->em28xx_sensor = EM28XX_OV2640;
+			break;
+		case 0x7648:
+			name = "OV7648";
+			break;
+		case 0x7660:
+			name = "OV7660";
+			break;
+		case 0x7673:
+			name = "OV7670";
+			break;
+		case 0x7720:
+			name = "OV7720";
+			break;
+		case 0x7721:
+			name = "OV7725";
+			break;
+		case 0x9648: /* Rev 2 */
+		case 0x9649: /* Rev 3 */
+			name = "OV9640";
+			break;
+		case 0x9650:
+		case 0x9652: /* OV9653 */
+			name = "OV9650";
+			break;
+		case 0x9656: /* Rev 4 */
+		case 0x9657: /* Rev 5 */
+			name = "OV9655";
+			break;
+		default:
+			em28xx_info("unknown OmniVision sensor detected: 0x%04x\n",
+				    id);
+			return 0;
+		}
+
+		if (dev->em28xx_sensor == EM28XX_NOSENSOR)
+			em28xx_info("unsupported sensor detected: %s\n", name);
+		else
+			em28xx_info("sensor %s detected\n", name);
+
+		dev->i2c_client[dev->def_i2c_bus].addr = client.addr;
+		return 0;
+	}
+
+	return -ENODEV;
+}
+
+int em28xx_detect_sensor(struct em28xx *dev)
+{
+	int ret;
+
+	ret = em28xx_probe_sensor_micron(dev);
+
+	if (dev->em28xx_sensor == EM28XX_NOSENSOR && ret < 0)
+		ret = em28xx_probe_sensor_omnivision(dev);
+
+	/*
+	 * NOTE: the Windows driver also probes i2c addresses
+	 *       0x22 (Samsung ?) and 0x66 (Kodak ?)
+	 */
+
+	if (dev->em28xx_sensor == EM28XX_NOSENSOR && ret < 0) {
+		em28xx_info("No sensor detected\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+int em28xx_init_camera(struct em28xx *dev)
+{
+	switch (dev->em28xx_sensor) {
+	case EM28XX_MT9V011:
+	{
+		struct mt9v011_platform_data pdata;
+		struct i2c_board_info mt9v011_info = {
+			.type = "mt9v011",
+			.addr = dev->i2c_client[dev->def_i2c_bus].addr,
+			.platform_data = &pdata,
+		};
+
+		dev->sensor_xres = 640;
+		dev->sensor_yres = 480;
+
+		/*
+		 * FIXME: mt9v011 uses I2S speed as xtal clk - at least with
+		 * the Silvercrest cam I have here for testing - for higher
+		 * resolutions, a high clock cause horizontal artifacts, so we
+		 * need to use a lower xclk frequency.
+		 * Yet, it would be possible to adjust xclk depending on the
+		 * desired resolution, since this affects directly the
+		 * frame rate.
+		 */
+		dev->board.xclk = EM28XX_XCLK_FREQUENCY_4_3MHZ;
+		em28xx_write_reg(dev, EM28XX_R0F_XCLK, dev->board.xclk);
+		dev->sensor_xtal = 4300000;
+		pdata.xtal = dev->sensor_xtal;
+		if (NULL ==
+		    v4l2_i2c_new_subdev_board(&dev->v4l2_dev,
+					      &dev->i2c_adap[dev->def_i2c_bus],
+					      &mt9v011_info, NULL))
+			return -ENODEV;
+		/* probably means GRGB 16 bit bayer */
+		dev->vinmode = 0x0d;
+		dev->vinctl = 0x00;
+
+		break;
+	}
+	case EM28XX_MT9M001:
+		dev->sensor_xres = 1280;
+		dev->sensor_yres = 1024;
+
+		em28xx_initialize_mt9m001(dev);
+
+		/* probably means BGGR 16 bit bayer */
+		dev->vinmode = 0x0c;
+		dev->vinctl = 0x00;
+
+		break;
+	case EM28XX_MT9M111:
+		dev->sensor_xres = 640;
+		dev->sensor_yres = 512;
+
+		dev->board.xclk = EM28XX_XCLK_FREQUENCY_48MHZ;
+		em28xx_write_reg(dev, EM28XX_R0F_XCLK, dev->board.xclk);
+		em28xx_initialize_mt9m111(dev);
+
+		dev->vinmode = 0x0a;
+		dev->vinctl = 0x00;
+
+		break;
+	case EM28XX_OV2640:
+	{
+		struct v4l2_subdev *subdev;
+		struct i2c_board_info ov2640_info = {
+			.type = "ov2640",
+			.flags = I2C_CLIENT_SCCB,
+			.addr = dev->i2c_client[dev->def_i2c_bus].addr,
+			.platform_data = &camlink,
+		};
+		struct v4l2_mbus_framefmt fmt;
+
+		/*
+		 * FIXME: sensor supports resolutions up to 1600x1200, but
+		 * resolution setting/switching needs to be modified to
+		 * - switch sensor output resolution (including further
+		 *   configuration changes)
+		 * - adjust bridge xclk
+		 * - disable 16 bit (12 bit) output formats on high resolutions
+		 */
+		dev->sensor_xres = 640;
+		dev->sensor_yres = 480;
+
+		subdev =
+		     v4l2_i2c_new_subdev_board(&dev->v4l2_dev,
+					       &dev->i2c_adap[dev->def_i2c_bus],
+					       &ov2640_info, NULL);
+
+		fmt.code = V4L2_MBUS_FMT_YUYV8_2X8;
+		fmt.width = 640;
+		fmt.height = 480;
+		v4l2_subdev_call(subdev, video, s_mbus_fmt, &fmt);
+
+		/* NOTE: for UXGA=1600x1200 switch to 12MHz */
+		dev->board.xclk = EM28XX_XCLK_FREQUENCY_24MHZ;
+		em28xx_write_reg(dev, EM28XX_R0F_XCLK, dev->board.xclk);
+		dev->vinmode = 0x08;
+		dev->vinctl = 0x00;
+
+		break;
+	}
+	case EM28XX_NOSENSOR:
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
diff --git a/drivers/media/usb/em28xx/em28xx-cards.c b/drivers/media/usb/em28xx/em28xx-cards.c
index 54a03b20de6e..83bfbe4c980f 100644
--- a/drivers/media/usb/em28xx/em28xx-cards.c
+++ b/drivers/media/usb/em28xx/em28xx-cards.c
@@ -34,7 +34,6 @@
 #include <media/saa7115.h>
 #include <media/tvp5150.h>
 #include <media/tvaudio.h>
-#include <media/mt9v011.h>
 #include <media/i2c-addr.h>
 #include <media/tveeprom.h>
 #include <media/v4l2-common.h>
@@ -345,6 +344,18 @@ static struct em28xx_reg_seq pctv_460e[] = {
 	{             -1,   -1,   -1,  -1},
 };
 
+static struct em28xx_reg_seq c3tech_digital_duo_digital[] = {
+	{EM2874_R80_GPIO,	0xff,	0xff,	10},
+	{EM2874_R80_GPIO,	0xfd,	0xff,	10}, /* xc5000 reset */
+	{EM2874_R80_GPIO,	0xf9,	0xff,	35},
+	{EM2874_R80_GPIO,	0xfd,	0xff,	10},
+	{EM2874_R80_GPIO,	0xff,	0xff,	10},
+	{EM2874_R80_GPIO,	0xfe,	0xff,	10},
+	{EM2874_R80_GPIO,	0xbe,	0xff,	10},
+	{EM2874_R80_GPIO,	0xfe,	0xff,	20},
+	{ -1,			-1,	-1,	-1},
+};
+
 #if 0
 static struct em28xx_reg_seq hauppauge_930c_gpio[] = {
 	{EM2874_R80_GPIO,	0x6f,	0xff,	10},
@@ -958,8 +969,8 @@ struct em28xx_board em28xx_boards[] = {
 #else
 		.tuner_type   = TUNER_ABSENT,
 #endif
-		.i2c_speed    = EM2874_I2C_SECONDARY_BUS_SELECT |
-				EM28XX_I2C_CLK_WAIT_ENABLE |
+		.def_i2c_bus  = 1,
+		.i2c_speed    = EM28XX_I2C_CLK_WAIT_ENABLE |
 				EM28XX_I2C_FREQ_400_KHZ,
 	},
 	[EM2884_BOARD_HAUPPAUGE_WINTV_HVR_930C] = {
@@ -974,17 +985,27 @@ struct em28xx_board em28xx_boards[] = {
 		.tuner_type   = TUNER_ABSENT,
 #endif
 		.ir_codes     = RC_MAP_HAUPPAUGE,
-		.i2c_speed    = EM2874_I2C_SECONDARY_BUS_SELECT |
-				EM28XX_I2C_CLK_WAIT_ENABLE |
+		.def_i2c_bus  = 1,
+		.i2c_speed    = EM28XX_I2C_CLK_WAIT_ENABLE |
 				EM28XX_I2C_FREQ_400_KHZ,
 	},
+	[EM2884_BOARD_C3TECH_DIGITAL_DUO] = {
+		.name         = "C3 Tech Digital Duo HDTV/SDTV USB",
+		.has_dvb      = 1,
+		/* FIXME: Add analog support - need a saa7136 driver */
+		.tuner_type = TUNER_ABSENT,	/* Digital-only TDA18271HD */
+		.ir_codes     = RC_MAP_EMPTY,
+		.def_i2c_bus  = 1,
+		.i2c_speed    = EM28XX_I2C_CLK_WAIT_ENABLE,
+		.dvb_gpio     = c3tech_digital_duo_digital,
+	},
 	[EM2884_BOARD_CINERGY_HTC_STICK] = {
 		.name         = "Terratec Cinergy HTC Stick",
 		.has_dvb      = 1,
 		.ir_codes     = RC_MAP_NEC_TERRATEC_CINERGY_XS,
 		.tuner_type   = TUNER_ABSENT,
-		.i2c_speed    = EM2874_I2C_SECONDARY_BUS_SELECT |
-				EM28XX_I2C_CLK_WAIT_ENABLE |
+		.def_i2c_bus  = 1,
+		.i2c_speed    = EM28XX_I2C_CLK_WAIT_ENABLE |
 				EM28XX_I2C_FREQ_400_KHZ,
 	},
 	[EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900] = {
@@ -1404,8 +1425,8 @@ struct em28xx_board em28xx_boards[] = {
 	},
 
 	[EM2874_BOARD_LEADERSHIP_ISDBT] = {
-		.i2c_speed      = EM2874_I2C_SECONDARY_BUS_SELECT |
-				  EM28XX_I2C_CLK_WAIT_ENABLE |
+		.def_i2c_bus	= 1,
+		.i2c_speed      = EM28XX_I2C_CLK_WAIT_ENABLE |
 				  EM28XX_I2C_FREQ_100_KHZ,
 		.xclk		= EM28XX_XCLK_FREQUENCY_10MHZ,
 		.name		= "EM2874 Leadership ISDBT",
@@ -1917,8 +1938,8 @@ struct em28xx_board em28xx_boards[] = {
 	 * Empia EM28174, Sony CXD2820R and NXP TDA18271HD/C2 */
 	[EM28174_BOARD_PCTV_290E] = {
 		.name          = "PCTV nanoStick T2 290e",
-		.i2c_speed      = EM2874_I2C_SECONDARY_BUS_SELECT |
-			EM28XX_I2C_CLK_WAIT_ENABLE | EM28XX_I2C_FREQ_100_KHZ,
+		.def_i2c_bus   = 1,
+		.i2c_speed     = EM28XX_I2C_CLK_WAIT_ENABLE | EM28XX_I2C_FREQ_100_KHZ,
 		.tuner_type    = TUNER_ABSENT,
 		.tuner_gpio    = pctv_290e,
 		.has_dvb       = 1,
@@ -1927,8 +1948,8 @@ struct em28xx_board em28xx_boards[] = {
 	/* 2013:024f PCTV DVB-S2 Stick 460e
 	 * Empia EM28174, NXP TDA10071, Conexant CX24118A and Allegro A8293 */
 	[EM28174_BOARD_PCTV_460E] = {
-		.i2c_speed     = EM2874_I2C_SECONDARY_BUS_SELECT |
-			EM28XX_I2C_CLK_WAIT_ENABLE | EM28XX_I2C_FREQ_400_KHZ,
+		.def_i2c_bus   = 1,
+		.i2c_speed     = EM28XX_I2C_CLK_WAIT_ENABLE | EM28XX_I2C_FREQ_400_KHZ,
 		.name          = "PCTV DVB-S2 Stick (460e)",
 		.tuner_type    = TUNER_ABSENT,
 		.tuner_gpio    = pctv_460e,
@@ -1958,8 +1979,9 @@ struct em28xx_board em28xx_boards[] = {
 		.tuner_type    = TUNER_ABSENT,
 		.tuner_gpio    = maxmedia_ub425_tc,
 		.has_dvb       = 1,
-		.i2c_speed     = EM2874_I2C_SECONDARY_BUS_SELECT |
-				EM28XX_I2C_CLK_WAIT_ENABLE |
+		.ir_codes      = RC_MAP_REDDO,
+		.def_i2c_bus   = 1,
+		.i2c_speed     = EM28XX_I2C_CLK_WAIT_ENABLE |
 				EM28XX_I2C_FREQ_400_KHZ,
 	},
 	/* 2304:0242 PCTV QuatroStick (510e)
@@ -1970,8 +1992,8 @@ struct em28xx_board em28xx_boards[] = {
 		.tuner_gpio    = pctv_510e,
 		.has_dvb       = 1,
 		.ir_codes      = RC_MAP_PINNACLE_PCTV_HD,
-		.i2c_speed     = EM2874_I2C_SECONDARY_BUS_SELECT |
-				EM28XX_I2C_CLK_WAIT_ENABLE |
+		.def_i2c_bus   = 1,
+		.i2c_speed     = EM28XX_I2C_CLK_WAIT_ENABLE |
 				EM28XX_I2C_FREQ_400_KHZ,
 	},
 	/* 2013:0251 PCTV QuatroStick nano (520e)
@@ -1982,8 +2004,8 @@ struct em28xx_board em28xx_boards[] = {
 		.tuner_gpio    = pctv_520e,
 		.has_dvb       = 1,
 		.ir_codes      = RC_MAP_PINNACLE_PCTV_HD,
-		.i2c_speed     = EM2874_I2C_SECONDARY_BUS_SELECT |
-				EM28XX_I2C_CLK_WAIT_ENABLE |
+		.def_i2c_bus   = 1,
+		.i2c_speed     = EM28XX_I2C_CLK_WAIT_ENABLE |
 				EM28XX_I2C_FREQ_400_KHZ,
 	},
 	[EM2884_BOARD_TERRATEC_HTC_USB_XS] = {
@@ -1991,8 +2013,8 @@ struct em28xx_board em28xx_boards[] = {
 		.has_dvb      = 1,
 		.ir_codes     = RC_MAP_NEC_TERRATEC_CINERGY_XS,
 		.tuner_type   = TUNER_ABSENT,
-		.i2c_speed    = EM2874_I2C_SECONDARY_BUS_SELECT |
-				EM28XX_I2C_CLK_WAIT_ENABLE |
+		.def_i2c_bus  = 1,
+		.i2c_speed    = EM28XX_I2C_CLK_WAIT_ENABLE |
 				EM28XX_I2C_FREQ_400_KHZ,
 	},
 };
@@ -2144,6 +2166,8 @@ struct usb_device_id em28xx_id_table[] = {
 			.driver_info = EM28174_BOARD_PCTV_460E },
 	{ USB_DEVICE(0x2040, 0x1605),
 			.driver_info = EM2884_BOARD_HAUPPAUGE_WINTV_HVR_930C },
+	{ USB_DEVICE(0x1b80, 0xe755),
+			.driver_info = EM2884_BOARD_C3TECH_DIGITAL_DUO },
 	{ USB_DEVICE(0xeb1a, 0x5006),
 			.driver_info = EM2860_BOARD_HT_VIDBOX_NW03 },
 	{ USB_DEVICE(0x1b80, 0xe309), /* Sveon STV40 */
@@ -2183,6 +2207,7 @@ static struct em28xx_hash_table em28xx_i2c_hash[] = {
 	{0x4ba50080, EM2861_BOARD_GADMEI_UTV330PLUS, TUNER_TNF_5335MF},
 	{0x6b800080, EM2874_BOARD_LEADERSHIP_ISDBT, TUNER_ABSENT},
 };
+/* NOTE: introduce a separate hash table for devices with 16 bit eeproms */
 
 /* I2C possible address to saa7115, tvp5150, msp3400, tvaudio */
 static unsigned short saa711x_addrs[] = {
@@ -2204,8 +2229,9 @@ static unsigned short msp3400_addrs[] = {
 
 int em28xx_tuner_callback(void *ptr, int component, int command, int arg)
 {
+	struct em28xx_i2c_bus *i2c_bus = ptr;
+	struct em28xx *dev = i2c_bus->dev;
 	int rc = 0;
-	struct em28xx *dev = ptr;
 
 	if (dev->tuner_type != TUNER_XC2028 && dev->tuner_type != TUNER_XC5000)
 		return 0;
@@ -2233,145 +2259,9 @@ static inline void em28xx_set_model(struct em28xx *dev)
 	if (!dev->board.i2c_speed)
 		dev->board.i2c_speed = EM28XX_I2C_CLK_WAIT_ENABLE |
 				       EM28XX_I2C_FREQ_100_KHZ;
-}
-
-
-/* FIXME: Should be replaced by a proper mt9m111 driver */
-static int em28xx_initialize_mt9m111(struct em28xx *dev)
-{
-	int i;
-	unsigned char regs[][3] = {
-		{ 0x0d, 0x00, 0x01, },  /* reset and use defaults */
-		{ 0x0d, 0x00, 0x00, },
-		{ 0x0a, 0x00, 0x21, },
-		{ 0x21, 0x04, 0x00, },  /* full readout speed, no row/col skipping */
-	};
-
-	for (i = 0; i < ARRAY_SIZE(regs); i++)
-		i2c_master_send(&dev->i2c_client, &regs[i][0], 3);
-
-	return 0;
-}
-
-
-/* FIXME: Should be replaced by a proper mt9m001 driver */
-static int em28xx_initialize_mt9m001(struct em28xx *dev)
-{
-	int i;
-	unsigned char regs[][3] = {
-		{ 0x0d, 0x00, 0x01, },
-		{ 0x0d, 0x00, 0x00, },
-		{ 0x04, 0x05, 0x00, },	/* hres = 1280 */
-		{ 0x03, 0x04, 0x00, },  /* vres = 1024 */
-		{ 0x20, 0x11, 0x00, },
-		{ 0x06, 0x00, 0x10, },
-		{ 0x2b, 0x00, 0x24, },
-		{ 0x2e, 0x00, 0x24, },
-		{ 0x35, 0x00, 0x24, },
-		{ 0x2d, 0x00, 0x20, },
-		{ 0x2c, 0x00, 0x20, },
-		{ 0x09, 0x0a, 0xd4, },
-		{ 0x35, 0x00, 0x57, },
-	};
-
-	for (i = 0; i < ARRAY_SIZE(regs); i++)
-		i2c_master_send(&dev->i2c_client, &regs[i][0], 3);
-
-	return 0;
-}
-
-/* HINT method: webcam I2C chips
- *
- * This method works for webcams with Micron sensors
- */
-static int em28xx_hint_sensor(struct em28xx *dev)
-{
-	int rc;
-	char *sensor_name;
-	unsigned char cmd;
-	__be16 version_be;
-	u16 version;
-
-	/* Micron sensor detection */
-	dev->i2c_client.addr = 0xba >> 1;
-	cmd = 0;
-	i2c_master_send(&dev->i2c_client, &cmd, 1);
-	rc = i2c_master_recv(&dev->i2c_client, (char *)&version_be, 2);
-	if (rc != 2)
-		return -EINVAL;
-
-	version = be16_to_cpu(version_be);
-	switch (version) {
-	case 0x8232:		/* mt9v011 640x480 1.3 Mpix sensor */
-	case 0x8243:		/* mt9v011 rev B 640x480 1.3 Mpix sensor */
-		dev->model = EM2820_BOARD_SILVERCREST_WEBCAM;
-		em28xx_set_model(dev);
-
-		sensor_name = "mt9v011";
-		dev->em28xx_sensor = EM28XX_MT9V011;
-		dev->sensor_xres = 640;
-		dev->sensor_yres = 480;
-		/*
-		 * FIXME: mt9v011 uses I2S speed as xtal clk - at least with
-		 * the Silvercrest cam I have here for testing - for higher
-		 * resolutions, a high clock cause horizontal artifacts, so we
-		 * need to use a lower xclk frequency.
-		 * Yet, it would be possible to adjust xclk depending on the
-		 * desired resolution, since this affects directly the
-		 * frame rate.
-		 */
-		dev->board.xclk = EM28XX_XCLK_FREQUENCY_4_3MHZ;
-		dev->sensor_xtal = 4300000;
-
-		/* probably means GRGB 16 bit bayer */
-		dev->vinmode = 0x0d;
-		dev->vinctl = 0x00;
-
-		break;
-
-	case 0x143a:    /* MT9M111 as found in the ECS G200 */
-		dev->model = EM2750_BOARD_UNKNOWN;
-		em28xx_set_model(dev);
-
-		sensor_name = "mt9m111";
-		dev->board.xclk = EM28XX_XCLK_FREQUENCY_48MHZ;
-		dev->em28xx_sensor = EM28XX_MT9M111;
-		em28xx_initialize_mt9m111(dev);
-		dev->sensor_xres = 640;
-		dev->sensor_yres = 512;
-
-		dev->vinmode = 0x0a;
-		dev->vinctl = 0x00;
-
-		break;
-
-	case 0x8431:
-		dev->model = EM2750_BOARD_UNKNOWN;
-		em28xx_set_model(dev);
-
-		sensor_name = "mt9m001";
-		dev->em28xx_sensor = EM28XX_MT9M001;
-		em28xx_initialize_mt9m001(dev);
-		dev->sensor_xres = 1280;
-		dev->sensor_yres = 1024;
-
-		/* probably means BGGR 16 bit bayer */
-		dev->vinmode = 0x0c;
-		dev->vinctl = 0x00;
-
-		break;
-	default:
-		printk("Unknown Micron Sensor 0x%04x\n", version);
-		return -EINVAL;
-	}
-
-	/* Setup webcam defaults */
-	em28xx_pre_card_setup(dev);
 
-	em28xx_errdev("Sensor is %s, using model %s entry.\n",
-		      sensor_name, em28xx_boards[dev->model].name);
-
-	return 0;
+	/* Should be initialized early, for I2C to work */
+	dev->def_i2c_bus = dev->board.def_i2c_bus;
 }
 
 /* Since em28xx_pre_card_setup() requires a proper dev->model,
@@ -2599,6 +2489,18 @@ static int em28xx_hint_board(struct em28xx *dev)
 {
 	int i;
 
+	if (dev->board.is_webcam) {
+		if (dev->em28xx_sensor == EM28XX_MT9V011) {
+			dev->model = EM2820_BOARD_SILVERCREST_WEBCAM;
+		} else if (dev->em28xx_sensor == EM28XX_MT9M001 ||
+			   dev->em28xx_sensor == EM28XX_MT9M111) {
+			dev->model = EM2750_BOARD_UNKNOWN;
+		}
+		/* FIXME: IMPROVE ! */
+
+		return 0;
+	}
+
 	/* HINT method: EEPROM
 	 *
 	 * This method works only for boards with eeprom.
@@ -2638,7 +2540,7 @@ static int em28xx_hint_board(struct em28xx *dev)
 
 	/* user did not request i2c scanning => do it now */
 	if (!dev->i2c_hash)
-		em28xx_do_i2c_scan(dev);
+		em28xx_do_i2c_scan(dev, dev->def_i2c_bus);
 
 	for (i = 0; i < ARRAY_SIZE(em28xx_i2c_hash); i++) {
 		if (dev->i2c_hash == em28xx_i2c_hash[i].hash) {
@@ -2684,16 +2586,16 @@ static void em28xx_card_setup(struct em28xx *dev)
 	 * If sensor is not found, then it isn't a webcam.
 	 */
 	if (dev->board.is_webcam) {
-		if (em28xx_hint_sensor(dev) < 0)
+		if (em28xx_detect_sensor(dev) < 0)
 			dev->board.is_webcam = 0;
 		else
 			dev->progressive = 1;
 	}
 
-	if (!dev->board.is_webcam) {
-		switch (dev->model) {
-		case EM2820_BOARD_UNKNOWN:
-		case EM2800_BOARD_UNKNOWN:
+	switch (dev->model) {
+	case EM2750_BOARD_UNKNOWN:
+	case EM2820_BOARD_UNKNOWN:
+	case EM2800_BOARD_UNKNOWN:
 		/*
 		 * The K-WORLD DVB-T 310U is detected as an MSI Digivox AD.
 		 *
@@ -2714,9 +2616,8 @@ static void em28xx_card_setup(struct em28xx *dev)
 			em28xx_pre_card_setup(dev);
 		}
 		break;
-		default:
-			em28xx_set_model(dev);
-		}
+	default:
+		em28xx_set_model(dev);
 	}
 
 	em28xx_info("Identified as %s (card=%d)\n",
@@ -2736,15 +2637,19 @@ static void em28xx_card_setup(struct em28xx *dev)
 	case EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900_R2:
 	case EM2883_BOARD_HAUPPAUGE_WINTV_HVR_850:
 	case EM2883_BOARD_HAUPPAUGE_WINTV_HVR_950:
+	case EM2884_BOARD_HAUPPAUGE_WINTV_HVR_930C:
 	{
 		struct tveeprom tv;
+
+		if (dev->eedata == NULL)
+			break;
 #if defined(CONFIG_MODULES) && defined(MODULE)
 		request_module("tveeprom");
 #endif
 		/* Call first TVeeprom */
 
-		dev->i2c_client.addr = 0xa0 >> 1;
-		tveeprom_hauppauge_analog(&dev->i2c_client, &tv, dev->eedata);
+		dev->i2c_client[dev->def_i2c_bus].addr = 0xa0 >> 1;
+		tveeprom_hauppauge_analog(&dev->i2c_client[dev->def_i2c_bus], &tv, dev->eedata);
 
 		dev->tuner_type = tv.tuner_type;
 
@@ -2791,7 +2696,7 @@ static void em28xx_card_setup(struct em28xx *dev)
 		em28xx_set_mode(dev, EM28XX_ANALOG_MODE);
 		break;
 
-/*
+		/*
 		 * The Dikom DK300 is detected as an Kworld VS-DVB-T 323UR.
 		 *
 		 * This occurs because they share identical USB vendor and
@@ -2826,51 +2731,41 @@ static void em28xx_card_setup(struct em28xx *dev)
 				"addresses)\n\n");
 	}
 
+	/* Free eeprom data memory */
+	kfree(dev->eedata);
+	dev->eedata = NULL;
+
 	/* Allow override tuner type by a module parameter */
 	if (tuner >= 0)
 		dev->tuner_type = tuner;
 
 	/* request some modules */
 	if (dev->board.has_msp34xx)
-		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
+		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
 			"msp3400", 0, msp3400_addrs);
 
 	if (dev->board.decoder == EM28XX_SAA711X)
-		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
+		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
 			"saa7115_auto", 0, saa711x_addrs);
 
 	if (dev->board.decoder == EM28XX_TVP5150)
-		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
+		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
 			"tvp5150", 0, tvp5150_addrs);
 
-	if (dev->em28xx_sensor == EM28XX_MT9V011) {
-		struct mt9v011_platform_data pdata;
-		struct i2c_board_info mt9v011_info = {
-			.type = "mt9v011",
-			.addr = 0xba >> 1,
-			.platform_data = &pdata,
-		};
-
-		pdata.xtal = dev->sensor_xtal;
-		v4l2_i2c_new_subdev_board(&dev->v4l2_dev, &dev->i2c_adap,
-				&mt9v011_info, NULL);
-	}
-
-
 	if (dev->board.adecoder == EM28XX_TVAUDIO)
-		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
+		v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
 			"tvaudio", dev->board.tvaudio_addr, NULL);
 
 	if (dev->board.tuner_type != TUNER_ABSENT) {
 		int has_demod = (dev->tda9887_conf & TDA9887_PRESENT);
 
 		if (dev->board.radio.type)
-			v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
+			v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
 				"tuner", dev->board.radio_addr, NULL);
 
 		if (has_demod)
 			v4l2_i2c_new_subdev(&dev->v4l2_dev,
-				&dev->i2c_adap, "tuner",
+				&dev->i2c_adap[dev->def_i2c_bus], "tuner",
 				0, v4l2_i2c_tuner_addrs(ADDRS_DEMOD));
 		if (dev->tuner_addr == 0) {
 			enum v4l2_i2c_tuner_type type =
@@ -2878,18 +2773,20 @@ static void em28xx_card_setup(struct em28xx *dev)
 			struct v4l2_subdev *sd;
 
 			sd = v4l2_i2c_new_subdev(&dev->v4l2_dev,
-				&dev->i2c_adap, "tuner",
+				&dev->i2c_adap[dev->def_i2c_bus], "tuner",
 				0, v4l2_i2c_tuner_addrs(type));
 
 			if (sd)
 				dev->tuner_addr = v4l2_i2c_subdev_addr(sd);
 		} else {
-			v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
+			v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap[dev->def_i2c_bus],
 				"tuner", dev->tuner_addr, NULL);
 		}
 	}
 
 	em28xx_tuner_setup(dev);
+
+	em28xx_init_camera(dev);
 }
 
 
@@ -2914,7 +2811,8 @@ static void request_module_async(struct work_struct *work)
 
 	if (dev->board.has_dvb)
 		request_module("em28xx-dvb");
-	if ((dev->board.ir_codes || dev->board.has_ir_i2c) && !disable_ir)
+	if (dev->board.has_snapshot_button ||
+	    ((dev->board.ir_codes || dev->board.has_ir_i2c) && !disable_ir))
 		request_module("em28xx-rc");
 #endif /* CONFIG_MODULES */
 }
@@ -2941,7 +2839,9 @@ void em28xx_release_resources(struct em28xx *dev)
 
 	em28xx_release_analog_resources(dev);
 
-	em28xx_i2c_unregister(dev);
+	if (dev->def_i2c_bus)
+		em28xx_i2c_unregister(dev, 1);
+	em28xx_i2c_unregister(dev, 0);
 
 	v4l2_ctrl_handler_free(&dev->ctrl_handler);
 
@@ -3002,8 +2902,23 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 		case CHIP_ID_EM2750:
 			chip_name = "em2750";
 			break;
+		case CHIP_ID_EM2765:
+			chip_name = "em2765";
+			dev->wait_after_write = 0;
+			dev->is_em25xx = 1;
+			dev->eeprom_addrwidth_16bit = 1;
+			break;
 		case CHIP_ID_EM2820:
 			chip_name = "em2710/2820";
+			if (le16_to_cpu(dev->udev->descriptor.idVendor)
+								    == 0xeb1a) {
+				__le16 idProd = dev->udev->descriptor.idProduct;
+				if (le16_to_cpu(idProd) == 0x2710)
+					chip_name = "em2710";
+				else if (le16_to_cpu(idProd) == 0x2820)
+					chip_name = "em2820";
+			}
+			/* NOTE: the em2820 is used in webcams, too ! */
 			break;
 		case CHIP_ID_EM2840:
 			chip_name = "em2840";
@@ -3019,11 +2934,13 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 			chip_name = "em2874";
 			dev->reg_gpio_num = EM2874_R80_GPIO;
 			dev->wait_after_write = 0;
+			dev->eeprom_addrwidth_16bit = 1;
 			break;
 		case CHIP_ID_EM28174:
 			chip_name = "em28174";
 			dev->reg_gpio_num = EM2874_R80_GPIO;
 			dev->wait_after_write = 0;
+			dev->eeprom_addrwidth_16bit = 1;
 			break;
 		case CHIP_ID_EM2883:
 			chip_name = "em2882/3";
@@ -3033,6 +2950,7 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 			chip_name = "em2884";
 			dev->reg_gpio_num = EM2874_R80_GPIO;
 			dev->wait_after_write = 0;
+			dev->eeprom_addrwidth_16bit = 1;
 			break;
 		default:
 			printk(KERN_INFO DRIVER_NAME
@@ -3066,14 +2984,6 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 
 	em28xx_pre_card_setup(dev);
 
-	if (dev->chip_id == CHIP_ID_EM2820) {
-		if (dev->board.is_webcam)
-			chip_name = "em2710";
-		else
-			chip_name = "em2820";
-		snprintf(dev->name, sizeof(dev->name), "%s #%d", chip_name, dev->devno);
-	}
-
 	if (!dev->board.is_em2800) {
 		/* Resets I2C speed */
 		retval = em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, dev->board.i2c_speed);
@@ -3091,17 +3001,37 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 		return retval;
 	}
 
-	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_handler_init(hdl, 8);
 	dev->v4l2_dev.ctrl_handler = hdl;
 
-	/* register i2c bus */
-	retval = em28xx_i2c_register(dev);
+	rt_mutex_init(&dev->i2c_bus_lock);
+
+	/* register i2c bus 0 */
+	if (dev->board.is_em2800)
+		retval = em28xx_i2c_register(dev, 0, EM28XX_I2C_ALGO_EM2800);
+	else
+		retval = em28xx_i2c_register(dev, 0, EM28XX_I2C_ALGO_EM28XX);
 	if (retval < 0) {
-		em28xx_errdev("%s: em28xx_i2c_register - error [%d]!\n",
+		em28xx_errdev("%s: em28xx_i2c_register bus 0 - error [%d]!\n",
 			__func__, retval);
 		goto unregister_dev;
 	}
 
+	/* register i2c bus 1 */
+	if (dev->def_i2c_bus) {
+		if (dev->is_em25xx)
+			retval = em28xx_i2c_register(dev, 1,
+						  EM28XX_I2C_ALGO_EM25XX_BUS_B);
+		else
+			retval = em28xx_i2c_register(dev, 1,
+							EM28XX_I2C_ALGO_EM28XX);
+		if (retval < 0) {
+			em28xx_errdev("%s: em28xx_i2c_register bus 1 - error [%d]!\n",
+				__func__, retval);
+			goto unregister_dev;
+		}
+	}
+
 	/*
 	 * Default format, used for tvp5150 or saa711x output formats
 	 */
@@ -3160,11 +3090,6 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 		msleep(3);
 	}
 
-	v4l2_ctrl_handler_setup(&dev->ctrl_handler);
-	retval = dev->ctrl_handler.error;
-	if (retval)
-		goto fail;
-
 	retval = em28xx_register_analog_devices(dev);
 	if (retval < 0) {
 		goto fail;
@@ -3176,7 +3101,9 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 	return 0;
 
 fail:
-	em28xx_i2c_unregister(dev);
+	if (dev->def_i2c_bus)
+		em28xx_i2c_unregister(dev, 1);
+	em28xx_i2c_unregister(dev, 0);
 	v4l2_ctrl_handler_free(&dev->ctrl_handler);
 
 unregister_dev:
@@ -3292,14 +3219,15 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 						dev->analog_ep_bulk =
 							    e->bEndpointAddress;
 					} else {
-						has_dvb = true;
 						if (usb_endpoint_xfer_isoc(e)) {
-							dev->dvb_ep_isoc = e->bEndpointAddress;
 							if (size > dev->dvb_max_pkt_size_isoc) {
+								has_dvb = true; /* see NOTE (~) */
+								dev->dvb_ep_isoc = e->bEndpointAddress;
 								dev->dvb_max_pkt_size_isoc = size;
 								dev->dvb_alt_isoc = i;
 							}
 						} else {
+							has_dvb = true;
 							dev->dvb_ep_bulk = e->bEndpointAddress;
 						}
 					}
@@ -3326,6 +3254,12 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 			 * so far. But there might be devices for which this
 			 * logic is not sufficient...
 			 */
+			/*
+			 * NOTE (~): some manufacturers (e.g. Terratec) disable
+			 * endpoints by setting wMaxPacketSize to 0 bytes for
+			 * all alt settings. So far, we've seen this for
+			 * DVB isoc endpoints only.
+			 */
 		}
 	}
 
diff --git a/drivers/media/usb/em28xx/em28xx-core.c b/drivers/media/usb/em28xx/em28xx-core.c
index aaedd11791f2..a802128ce9c5 100644
--- a/drivers/media/usb/em28xx/em28xx-core.c
+++ b/drivers/media/usb/em28xx/em28xx-core.c
@@ -607,12 +607,12 @@ EXPORT_SYMBOL_GPL(em28xx_audio_setup);
 
 int em28xx_colorlevels_set_default(struct em28xx *dev)
 {
-	em28xx_write_reg(dev, EM28XX_R20_YGAIN, 0x10);	/* contrast */
-	em28xx_write_reg(dev, EM28XX_R21_YOFFSET, 0x00);	/* brightness */
-	em28xx_write_reg(dev, EM28XX_R22_UVGAIN, 0x10);	/* saturation */
-	em28xx_write_reg(dev, EM28XX_R23_UOFFSET, 0x00);
-	em28xx_write_reg(dev, EM28XX_R24_VOFFSET, 0x00);
-	em28xx_write_reg(dev, EM28XX_R25_SHARPNESS, 0x00);
+	em28xx_write_reg(dev, EM28XX_R20_YGAIN, CONTRAST_DEFAULT);
+	em28xx_write_reg(dev, EM28XX_R21_YOFFSET, BRIGHTNESS_DEFAULT);
+	em28xx_write_reg(dev, EM28XX_R22_UVGAIN, SATURATION_DEFAULT);
+	em28xx_write_reg(dev, EM28XX_R23_UOFFSET, BLUE_BALANCE_DEFAULT);
+	em28xx_write_reg(dev, EM28XX_R24_VOFFSET, RED_BALANCE_DEFAULT);
+	em28xx_write_reg(dev, EM28XX_R25_SHARPNESS, SHARPNESS_DEFAULT);
 
 	em28xx_write_reg(dev, EM28XX_R14_GAMMA, 0x20);
 	em28xx_write_reg(dev, EM28XX_R15_RGAIN, 0x20);
@@ -681,6 +681,11 @@ int em28xx_vbi_supported(struct em28xx *dev)
 	if (disable_vbi == 1)
 		return 0;
 
+	if (dev->board.is_webcam)
+		return 0;
+
+	/* FIXME: check subdevices for VBI support */
+
 	if (dev->chip_id == CHIP_ID_EM2860 ||
 	    dev->chip_id == CHIP_ID_EM2883)
 		return 1;
@@ -692,12 +697,23 @@ int em28xx_vbi_supported(struct em28xx *dev)
 int em28xx_set_outfmt(struct em28xx *dev)
 {
 	int ret;
-	u8 vinctrl;
-
-	ret = em28xx_write_reg_bits(dev, EM28XX_R27_OUTFMT,
-				dev->format->reg | 0x20, 0xff);
+	u8 fmt, vinctrl;
+
+	fmt = dev->format->reg;
+	if (!dev->is_em25xx)
+		fmt |= 0x20;
+	/*
+	 * NOTE: it's not clear if this is really needed !
+	 * The datasheets say bit 5 is a reserved bit and devices seem to work
+	 * fine without it. But the Windows driver sets it for em2710/50+em28xx
+	 * devices and we've always been setting it, too.
+	 *
+	 * em2765 (em25xx, em276x/7x/8x) devices do NOT work with this bit set,
+	 * it's likely used for an additional (compressed ?) format there.
+	 */
+	ret = em28xx_write_reg(dev, EM28XX_R27_OUTFMT, fmt);
 	if (ret < 0)
-			return ret;
+		return ret;
 
 	ret = em28xx_write_reg(dev, EM28XX_R10_VINMODE, dev->vinmode);
 	if (ret < 0)
@@ -751,6 +767,13 @@ static void em28xx_capture_area_set(struct em28xx *dev, u8 hstart, u8 vstart,
 	em28xx_write_regs(dev, EM28XX_R1E_CWIDTH, &cwidth, 1);
 	em28xx_write_regs(dev, EM28XX_R1F_CHEIGHT, &cheight, 1);
 	em28xx_write_regs(dev, EM28XX_R1B_OFLOW, &overflow, 1);
+
+	/* FIXME: function/meaning of these registers ? */
+	/* FIXME: align width+height to multiples of 4 ?! */
+	if (dev->is_em25xx) {
+		em28xx_write_reg(dev, 0x34, width >> 4);
+		em28xx_write_reg(dev, 0x35, height >> 4);
+	}
 }
 
 static int em28xx_scaler_set(struct em28xx *dev, u16 h, u16 v)
diff --git a/drivers/media/usb/em28xx/em28xx-dvb.c b/drivers/media/usb/em28xx/em28xx-dvb.c
index a81ec2e8cc9b..b22f8fed8127 100644
--- a/drivers/media/usb/em28xx/em28xx-dvb.c
+++ b/drivers/media/usb/em28xx/em28xx-dvb.c
@@ -50,6 +50,7 @@
 #include "tda10071.h"
 #include "a8293.h"
 #include "qt1010.h"
+#include "mb86a20s.h"
 
 MODULE_DESCRIPTION("driver for em28xx based DVB cards");
 MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@infradead.org>");
@@ -177,7 +178,8 @@ static inline int em28xx_dvb_urb_data_copy(struct em28xx *dev, struct urb *urb)
 static int em28xx_start_streaming(struct em28xx_dvb *dvb)
 {
 	int rc;
-	struct em28xx *dev = dvb->adapter.priv;
+	struct em28xx_i2c_bus *i2c_bus = dvb->adapter.priv;
+	struct em28xx *dev = i2c_bus->dev;
 	int dvb_max_packet_size, packet_multiplier, dvb_alt;
 
 	if (dev->dvb_xfer_bulk) {
@@ -216,12 +218,11 @@ static int em28xx_start_streaming(struct em28xx_dvb *dvb)
 
 static int em28xx_stop_streaming(struct em28xx_dvb *dvb)
 {
-	struct em28xx *dev = dvb->adapter.priv;
+	struct em28xx_i2c_bus *i2c_bus = dvb->adapter.priv;
+	struct em28xx *dev = i2c_bus->dev;
 
 	em28xx_stop_urbs(dev);
 
-	em28xx_set_mode(dev, EM28XX_SUSPEND);
-
 	return 0;
 }
 
@@ -269,7 +270,8 @@ static int em28xx_stop_feed(struct dvb_demux_feed *feed)
 /* ------------------------------------------------------------------ */
 static int em28xx_dvb_bus_ctrl(struct dvb_frontend *fe, int acquire)
 {
-	struct em28xx *dev = fe->dvb->priv;
+	struct em28xx_i2c_bus *i2c_bus = fe->dvb->priv;
+        struct em28xx *dev = i2c_bus->dev;
 
 	if (acquire)
 		return em28xx_set_mode(dev, EM28XX_DIGITAL_MODE);
@@ -465,10 +467,10 @@ static void hauppauge_hvr930c_init(struct em28xx *dev)
 	em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, 0x44);
 	msleep(10);
 
-	dev->i2c_client.addr = 0x82 >> 1;
+	dev->i2c_client[dev->def_i2c_bus].addr = 0x82 >> 1;
 
 	for (i = 0; i < ARRAY_SIZE(regs); i++)
-		i2c_master_send(&dev->i2c_client, regs[i].r, regs[i].len);
+		i2c_master_send(&dev->i2c_client[dev->def_i2c_bus], regs[i].r, regs[i].len);
 	em28xx_gpio_set(dev, hauppauge_hvr930c_end);
 
 	msleep(100);
@@ -522,10 +524,10 @@ static void terratec_h5_init(struct em28xx *dev)
 	em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, 0x45);
 	msleep(10);
 
-	dev->i2c_client.addr = 0x82 >> 1;
+	dev->i2c_client[dev->def_i2c_bus].addr = 0x82 >> 1;
 
 	for (i = 0; i < ARRAY_SIZE(regs); i++)
-		i2c_master_send(&dev->i2c_client, regs[i].r, regs[i].len);
+		i2c_master_send(&dev->i2c_client[dev->def_i2c_bus], regs[i].r, regs[i].len);
 	em28xx_gpio_set(dev, terratec_h5_end);
 };
 
@@ -575,10 +577,10 @@ static void terratec_htc_stick_init(struct em28xx *dev)
 	em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, 0x44);
 	msleep(10);
 
-	dev->i2c_client.addr = 0x82 >> 1;
+	dev->i2c_client[dev->def_i2c_bus].addr = 0x82 >> 1;
 
 	for (i = 0; i < ARRAY_SIZE(regs); i++)
-		i2c_master_send(&dev->i2c_client, regs[i].r, regs[i].len);
+		i2c_master_send(&dev->i2c_client[dev->def_i2c_bus], regs[i].r, regs[i].len);
 
 	em28xx_gpio_set(dev, terratec_htc_stick_end);
 };
@@ -633,10 +635,10 @@ static void terratec_htc_usb_xs_init(struct em28xx *dev)
 	em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, 0x44);
 	msleep(10);
 
-	dev->i2c_client.addr = 0x82 >> 1;
+	dev->i2c_client[dev->def_i2c_bus].addr = 0x82 >> 1;
 
 	for (i = 0; i < ARRAY_SIZE(regs); i++)
-		i2c_master_send(&dev->i2c_client, regs[i].r, regs[i].len);
+		i2c_master_send(&dev->i2c_client[dev->def_i2c_bus], regs[i].r, regs[i].len);
 
 	em28xx_gpio_set(dev, terratec_htc_usb_xs_end);
 };
@@ -662,10 +664,10 @@ static void pctv_520e_init(struct em28xx *dev)
 		{{ 0x01, 0x00, 0x73, 0xaf }, 4},
 	};
 
-	dev->i2c_client.addr = 0x82 >> 1; /* 0x41 */
+	dev->i2c_client[dev->def_i2c_bus].addr = 0x82 >> 1; /* 0x41 */
 
 	for (i = 0; i < ARRAY_SIZE(regs); i++)
-		i2c_master_send(&dev->i2c_client, regs[i].r, regs[i].len);
+		i2c_master_send(&dev->i2c_client[dev->def_i2c_bus], regs[i].r, regs[i].len);
 };
 
 static int em28xx_pctv_290e_set_lna(struct dvb_frontend *fe)
@@ -768,9 +770,25 @@ static struct zl10353_config em28xx_zl10353_no_i2c_gate_dev = {
 };
 static struct qt1010_config em28xx_qt1010_config = {
 	.i2c_address = 0x62
+};
 
+static const struct mb86a20s_config c3tech_duo_mb86a20s_config = {
+	.demod_address = 0x10,
+	.is_serial = true,
+};
+
+static struct tda18271_std_map mb86a20s_tda18271_config = {
+	.dvbt_6   = { .if_freq = 4000, .agc_mode = 3, .std = 4,
+		      .if_lvl = 1, .rfagc_top = 0x37, },
 };
 
+static struct tda18271_config c3tech_duo_tda18271_config = {
+	.std_map = &mb86a20s_tda18271_config,
+	.gate    = TDA18271_GATE_DIGITAL,
+	.small_i2c = TDA18271_03_BYTE_CHUNK_INIT,
+};
+
+
 /* ------------------------------------------------------------------ */
 
 static int em28xx_attach_xc3028(u8 addr, struct em28xx *dev)
@@ -779,7 +797,7 @@ static int em28xx_attach_xc3028(u8 addr, struct em28xx *dev)
 	struct xc2028_config cfg;
 
 	memset(&cfg, 0, sizeof(cfg));
-	cfg.i2c_adap  = &dev->i2c_adap;
+	cfg.i2c_adap  = &dev->i2c_adap[dev->def_i2c_bus];
 	cfg.i2c_addr  = addr;
 
 	if (!dev->dvb->fe[0]) {
@@ -824,7 +842,7 @@ static int em28xx_register_dvb(struct em28xx_dvb *dvb, struct module *module,
 	if (dvb->fe[1])
 		dvb->fe[1]->ops.ts_bus_ctrl = em28xx_dvb_bus_ctrl;
 
-	dvb->adapter.priv = dev;
+	dvb->adapter.priv = &dev->i2c_bus[dev->def_i2c_bus];
 
 	/* register frontend */
 	result = dvb_register_frontend(&dvb->adapter, dvb->fe[0]);
@@ -962,7 +980,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	switch (dev->model) {
 	case EM2874_BOARD_LEADERSHIP_ISDBT:
 		dvb->fe[0] = dvb_attach(s921_attach,
-				&sharp_isdbt, &dev->i2c_adap);
+				&sharp_isdbt, &dev->i2c_adap[dev->def_i2c_bus]);
 
 		if (!dvb->fe[0]) {
 			result = -EINVAL;
@@ -976,7 +994,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2880_BOARD_AMD_ATI_TV_WONDER_HD_600:
 		dvb->fe[0] = dvb_attach(lgdt330x_attach,
 					   &em2880_lgdt3303_dev,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (em28xx_attach_xc3028(0x61, dev) < 0) {
 			result = -EINVAL;
 			goto out_free;
@@ -985,7 +1003,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2880_BOARD_KWORLD_DVB_310U:
 		dvb->fe[0] = dvb_attach(zl10353_attach,
 					   &em28xx_zl10353_with_xc3028,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (em28xx_attach_xc3028(0x61, dev) < 0) {
 			result = -EINVAL;
 			goto out_free;
@@ -996,7 +1014,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2880_BOARD_EMPIRE_DUAL_TV:
 		dvb->fe[0] = dvb_attach(zl10353_attach,
 					   &em28xx_zl10353_xc3028_no_i2c_gate,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (em28xx_attach_xc3028(0x61, dev) < 0) {
 			result = -EINVAL;
 			goto out_free;
@@ -1009,13 +1027,13 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2882_BOARD_KWORLD_VS_DVBT:
 		dvb->fe[0] = dvb_attach(zl10353_attach,
 					   &em28xx_zl10353_xc3028_no_i2c_gate,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (dvb->fe[0] == NULL) {
 			/* This board could have either a zl10353 or a mt352.
 			   If the chip id isn't for zl10353, try mt352 */
 			dvb->fe[0] = dvb_attach(mt352_attach,
 						   &terratec_xs_mt352_cfg,
-						   &dev->i2c_adap);
+						   &dev->i2c_adap[dev->def_i2c_bus]);
 		}
 
 		if (em28xx_attach_xc3028(0x61, dev) < 0) {
@@ -1026,16 +1044,16 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2870_BOARD_KWORLD_355U:
 		dvb->fe[0] = dvb_attach(zl10353_attach,
 					   &em28xx_zl10353_no_i2c_gate_dev,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (dvb->fe[0] != NULL)
 			dvb_attach(qt1010_attach, dvb->fe[0],
-				   &dev->i2c_adap, &em28xx_qt1010_config);
+				   &dev->i2c_adap[dev->def_i2c_bus], &em28xx_qt1010_config);
 		break;
 	case EM2883_BOARD_KWORLD_HYBRID_330U:
 	case EM2882_BOARD_EVGA_INDTUBE:
 		dvb->fe[0] = dvb_attach(s5h1409_attach,
 					   &em28xx_s5h1409_with_xc3028,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (em28xx_attach_xc3028(0x61, dev) < 0) {
 			result = -EINVAL;
 			goto out_free;
@@ -1044,10 +1062,10 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2882_BOARD_KWORLD_ATSC_315U:
 		dvb->fe[0] = dvb_attach(lgdt330x_attach,
 					   &em2880_lgdt3303_dev,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (dvb->fe[0] != NULL) {
 			if (!dvb_attach(simple_tuner_attach, dvb->fe[0],
-				&dev->i2c_adap, 0x61, TUNER_THOMSON_DTT761X)) {
+				&dev->i2c_adap[dev->def_i2c_bus], 0x61, TUNER_THOMSON_DTT761X)) {
 				result = -EINVAL;
 				goto out_free;
 			}
@@ -1056,7 +1074,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900_R2:
 	case EM2882_BOARD_PINNACLE_HYBRID_PRO_330E:
 		dvb->fe[0] = dvb_attach(drxd_attach, &em28xx_drxd, NULL,
-					   &dev->i2c_adap, &dev->udev->dev);
+					   &dev->i2c_adap[dev->def_i2c_bus], &dev->udev->dev);
 		if (em28xx_attach_xc3028(0x61, dev) < 0) {
 			result = -EINVAL;
 			goto out_free;
@@ -1066,10 +1084,10 @@ static int em28xx_dvb_init(struct em28xx *dev)
 		/* Philips CU1216L NIM (Philips TDA10023 + Infineon TUA6034) */
 		dvb->fe[0] = dvb_attach(tda10023_attach,
 			&em28xx_tda10023_config,
-			&dev->i2c_adap, 0x48);
+			&dev->i2c_adap[dev->def_i2c_bus], 0x48);
 		if (dvb->fe[0]) {
 			if (!dvb_attach(simple_tuner_attach, dvb->fe[0],
-				&dev->i2c_adap, 0x60, TUNER_PHILIPS_CU1216L)) {
+				&dev->i2c_adap[dev->def_i2c_bus], 0x60, TUNER_PHILIPS_CU1216L)) {
 				result = -EINVAL;
 				goto out_free;
 			}
@@ -1078,10 +1096,10 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2870_BOARD_KWORLD_A340:
 		dvb->fe[0] = dvb_attach(lgdt3305_attach,
 					   &em2870_lgdt3304_dev,
-					   &dev->i2c_adap);
+					   &dev->i2c_adap[dev->def_i2c_bus]);
 		if (dvb->fe[0] != NULL)
 			dvb_attach(tda18271_attach, dvb->fe[0], 0x60,
-				   &dev->i2c_adap, &kworld_a340_config);
+				   &dev->i2c_adap[dev->def_i2c_bus], &kworld_a340_config);
 		break;
 	case EM28174_BOARD_PCTV_290E:
 		/* set default GPIO0 for LNA, used if GPIOLIB is undefined */
@@ -1089,14 +1107,14 @@ static int em28xx_dvb_init(struct em28xx *dev)
 				CXD2820R_GPIO_L;
 		dvb->fe[0] = dvb_attach(cxd2820r_attach,
 					&em28xx_cxd2820r_config,
-					&dev->i2c_adap,
+					&dev->i2c_adap[dev->def_i2c_bus],
 					&dvb->lna_gpio);
 		if (dvb->fe[0]) {
 			/* FE 0 attach tuner */
 			if (!dvb_attach(tda18271_attach,
 					dvb->fe[0],
 					0x60,
-					&dev->i2c_adap,
+					&dev->i2c_adap[dev->def_i2c_bus],
 					&em28xx_cxd2820r_tda18271_config)) {
 
 				dvb_frontend_detach(dvb->fe[0]);
@@ -1126,7 +1144,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 		hauppauge_hvr930c_init(dev);
 
 		dvb->fe[0] = dvb_attach(drxk_attach,
-					&hauppauge_930c_drxk, &dev->i2c_adap);
+					&hauppauge_930c_drxk, &dev->i2c_adap[dev->def_i2c_bus]);
 		if (!dvb->fe[0]) {
 			result = -EINVAL;
 			goto out_free;
@@ -1144,7 +1162,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 
 		if (dvb->fe[0]->ops.i2c_gate_ctrl)
 			dvb->fe[0]->ops.i2c_gate_ctrl(dvb->fe[0], 1);
-		if (!dvb_attach(xc5000_attach, dvb->fe[0], &dev->i2c_adap,
+		if (!dvb_attach(xc5000_attach, dvb->fe[0], &dev->i2c_adap[dev->def_i2c_bus],
 				&cfg)) {
 			result = -EINVAL;
 			goto out_free;
@@ -1157,7 +1175,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	case EM2884_BOARD_TERRATEC_H5:
 		terratec_h5_init(dev);
 
-		dvb->fe[0] = dvb_attach(drxk_attach, &terratec_h5_drxk, &dev->i2c_adap);
+		dvb->fe[0] = dvb_attach(drxk_attach, &terratec_h5_drxk, &dev->i2c_adap[dev->def_i2c_bus]);
 		if (!dvb->fe[0]) {
 			result = -EINVAL;
 			goto out_free;
@@ -1171,7 +1189,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 		/* Attach tda18271 to DVB-C frontend */
 		if (dvb->fe[0]->ops.i2c_gate_ctrl)
 			dvb->fe[0]->ops.i2c_gate_ctrl(dvb->fe[0], 1);
-		if (!dvb_attach(tda18271c2dd_attach, dvb->fe[0], &dev->i2c_adap, 0x60)) {
+		if (!dvb_attach(tda18271c2dd_attach, dvb->fe[0], &dev->i2c_adap[dev->def_i2c_bus], 0x60)) {
 			result = -EINVAL;
 			goto out_free;
 		}
@@ -1179,20 +1197,29 @@ static int em28xx_dvb_init(struct em28xx *dev)
 			dvb->fe[0]->ops.i2c_gate_ctrl(dvb->fe[0], 0);
 
 		break;
+	case EM2884_BOARD_C3TECH_DIGITAL_DUO:
+		dvb->fe[0] = dvb_attach(mb86a20s_attach,
+					   &c3tech_duo_mb86a20s_config,
+					   &dev->i2c_adap[dev->def_i2c_bus]);
+		if (dvb->fe[0] != NULL)
+			dvb_attach(tda18271_attach, dvb->fe[0], 0x60,
+				   &dev->i2c_adap[dev->def_i2c_bus],
+				   &c3tech_duo_tda18271_config);
+		break;
 	case EM28174_BOARD_PCTV_460E:
 		/* attach demod */
 		dvb->fe[0] = dvb_attach(tda10071_attach,
-			&em28xx_tda10071_config, &dev->i2c_adap);
+			&em28xx_tda10071_config, &dev->i2c_adap[dev->def_i2c_bus]);
 
 		/* attach SEC */
 		if (dvb->fe[0])
-			dvb_attach(a8293_attach, dvb->fe[0], &dev->i2c_adap,
+			dvb_attach(a8293_attach, dvb->fe[0], &dev->i2c_adap[dev->def_i2c_bus],
 				&em28xx_a8293_config);
 		break;
 	case EM2874_BOARD_MAXMEDIA_UB425_TC:
 		/* attach demodulator */
 		dvb->fe[0] = dvb_attach(drxk_attach, &maxmedia_ub425_tc_drxk,
-				&dev->i2c_adap);
+				&dev->i2c_adap[dev->def_i2c_bus]);
 
 		if (dvb->fe[0]) {
 			/* disable I2C-gate */
@@ -1200,7 +1227,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 
 			/* attach tuner */
 			if (!dvb_attach(tda18271c2dd_attach, dvb->fe[0],
-					&dev->i2c_adap, 0x60)) {
+					&dev->i2c_adap[dev->def_i2c_bus], 0x60)) {
 				dvb_frontend_detach(dvb->fe[0]);
 				result = -EINVAL;
 				goto out_free;
@@ -1218,12 +1245,12 @@ static int em28xx_dvb_init(struct em28xx *dev)
 
 		/* attach demodulator */
 		dvb->fe[0] = dvb_attach(drxk_attach, &pctv_520e_drxk,
-				&dev->i2c_adap);
+				&dev->i2c_adap[dev->def_i2c_bus]);
 
 		if (dvb->fe[0]) {
 			/* attach tuner */
 			if (!dvb_attach(tda18271_attach, dvb->fe[0], 0x60,
-					&dev->i2c_adap,
+					&dev->i2c_adap[dev->def_i2c_bus],
 					&em28xx_cxd2820r_tda18271_config)) {
 				dvb_frontend_detach(dvb->fe[0]);
 				result = -EINVAL;
@@ -1236,7 +1263,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 
 		/* attach demodulator */
 		dvb->fe[0] = dvb_attach(drxk_attach, &terratec_htc_stick_drxk,
-					&dev->i2c_adap);
+					&dev->i2c_adap[dev->def_i2c_bus]);
 		if (!dvb->fe[0]) {
 			result = -EINVAL;
 			goto out_free;
@@ -1244,7 +1271,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 
 		/* Attach the demodulator. */
 		if (!dvb_attach(tda18271_attach, dvb->fe[0], 0x60,
-				&dev->i2c_adap,
+				&dev->i2c_adap[dev->def_i2c_bus],
 				&em28xx_cxd2820r_tda18271_config)) {
 			result = -EINVAL;
 			goto out_free;
@@ -1255,7 +1282,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 
 		/* attach demodulator */
 		dvb->fe[0] = dvb_attach(drxk_attach, &terratec_htc_stick_drxk,
-					&dev->i2c_adap);
+					&dev->i2c_adap[dev->def_i2c_bus]);
 		if (!dvb->fe[0]) {
 			result = -EINVAL;
 			goto out_free;
@@ -1263,7 +1290,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 
 		/* Attach the demodulator. */
 		if (!dvb_attach(tda18271_attach, dvb->fe[0], 0x60,
-				&dev->i2c_adap,
+				&dev->i2c_adap[dev->def_i2c_bus],
 				&em28xx_cxd2820r_tda18271_config)) {
 			result = -EINVAL;
 			goto out_free;
diff --git a/drivers/media/usb/em28xx/em28xx-i2c.c b/drivers/media/usb/em28xx/em28xx-i2c.c
index 8532c1d4fd46..4851cc2e4a4d 100644
--- a/drivers/media/usb/em28xx/em28xx-i2c.c
+++ b/drivers/media/usb/em28xx/em28xx-i2c.c
@@ -5,6 +5,7 @@
 		      Markus Rechberger <mrechberger@gmail.com>
 		      Mauro Carvalho Chehab <mchehab@infradead.org>
 		      Sascha Sommer <saschasommer@freenet.de>
+   Copyright (C) 2013 Frank Schäfer <fschaefer.oss@googlemail.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
@@ -41,14 +42,6 @@ static unsigned int i2c_debug;
 module_param(i2c_debug, int, 0644);
 MODULE_PARM_DESC(i2c_debug, "enable debug messages [i2c]");
 
-#define dprintk2(lvl, fmt, args...)			\
-do {							\
-	if (i2c_debug >= lvl) {				\
-		printk(KERN_DEBUG "%s at %s: " fmt,	\
-		       dev->name, __func__ , ##args);	\
-      } 						\
-} while (0)
-
 /*
  * em2800_i2c_send_bytes()
  * send up to 4 bytes to the em2800 i2c device
@@ -76,8 +69,8 @@ static int em2800_i2c_send_bytes(struct em28xx *dev, u8 addr, u8 *buf, u16 len)
 	/* trigger write */
 	ret = dev->em28xx_write_regs(dev, 4 - len, &b2[4 - len], 2 + len);
 	if (ret != 2 + len) {
-		em28xx_warn("failed to trigger write to i2c address 0x%x "
-			    "(error=%i)\n", addr, ret);
+		em28xx_warn("failed to trigger write to i2c address 0x%x (error=%i)\n",
+			    addr, ret);
 		return (ret < 0) ? ret : -EIO;
 	}
 	/* wait for completion */
@@ -89,8 +82,8 @@ static int em2800_i2c_send_bytes(struct em28xx *dev, u8 addr, u8 *buf, u16 len)
 		} else if (ret == 0x94 + len - 1) {
 			return -ENODEV;
 		} else if (ret < 0) {
-			em28xx_warn("failed to get i2c transfer status from "
-				    "bridge register (error=%i)\n", ret);
+			em28xx_warn("failed to get i2c transfer status from bridge register (error=%i)\n",
+				    ret);
 			return ret;
 		}
 		msleep(5);
@@ -118,8 +111,8 @@ static int em2800_i2c_recv_bytes(struct em28xx *dev, u8 addr, u8 *buf, u16 len)
 	buf2[0] = addr;
 	ret = dev->em28xx_write_regs(dev, 0x04, buf2, 2);
 	if (ret != 2) {
-		em28xx_warn("failed to trigger read from i2c address 0x%x "
-			    "(error=%i)\n", addr, ret);
+		em28xx_warn("failed to trigger read from i2c address 0x%x (error=%i)\n",
+			    addr, ret);
 		return (ret < 0) ? ret : -EIO;
 	}
 
@@ -132,8 +125,8 @@ static int em2800_i2c_recv_bytes(struct em28xx *dev, u8 addr, u8 *buf, u16 len)
 		} else if (ret == 0x94 + len - 1) {
 			return -ENODEV;
 		} else if (ret < 0) {
-			em28xx_warn("failed to get i2c transfer status from "
-				    "bridge register (error=%i)\n", ret);
+			em28xx_warn("failed to get i2c transfer status from bridge register (error=%i)\n",
+				    ret);
 			return ret;
 		}
 		msleep(5);
@@ -144,9 +137,8 @@ static int em2800_i2c_recv_bytes(struct em28xx *dev, u8 addr, u8 *buf, u16 len)
 	/* get the received message */
 	ret = dev->em28xx_read_reg_req_len(dev, 0x00, 4-len, buf2, len);
 	if (ret != len) {
-		em28xx_warn("reading from i2c device at 0x%x failed: "
-			    "couldn't get the received message from the bridge "
-			    "(error=%i)\n", addr, ret);
+		em28xx_warn("reading from i2c device at 0x%x failed: couldn't get the received message from the bridge (error=%i)\n",
+			    addr, ret);
 		return (ret < 0) ? ret : -EIO;
 	}
 	for (i = 0; i < len; i++)
@@ -180,19 +172,20 @@ static int em28xx_i2c_send_bytes(struct em28xx *dev, u16 addr, u8 *buf,
 
 	if (len < 1 || len > 64)
 		return -EOPNOTSUPP;
-	/* NOTE: limited by the USB ctrl message constraints
-	 * Zero length reads always succeed, even if no device is connected */
+	/*
+	 * NOTE: limited by the USB ctrl message constraints
+	 * Zero length reads always succeed, even if no device is connected
+	 */
 
 	/* Write to i2c device */
 	ret = dev->em28xx_write_regs_req(dev, stop ? 2 : 3, addr, buf, len);
 	if (ret != len) {
 		if (ret < 0) {
-			em28xx_warn("writing to i2c device at 0x%x failed "
-				    "(error=%i)\n", addr, ret);
+			em28xx_warn("writing to i2c device at 0x%x failed (error=%i)\n",
+				    addr, ret);
 			return ret;
 		} else {
-			em28xx_warn("%i bytes write to i2c device at 0x%x "
-				    "requested, but %i bytes written\n",
+			em28xx_warn("%i bytes write to i2c device at 0x%x requested, but %i bytes written\n",
 				    len, addr, ret);
 			return -EIO;
 		}
@@ -207,14 +200,16 @@ static int em28xx_i2c_send_bytes(struct em28xx *dev, u16 addr, u8 *buf,
 		} else if (ret == 0x10) {
 			return -ENODEV;
 		} else if (ret < 0) {
-			em28xx_warn("failed to read i2c transfer status from "
-				    "bridge (error=%i)\n", ret);
+			em28xx_warn("failed to read i2c transfer status from bridge (error=%i)\n",
+				    ret);
 			return ret;
 		}
 		msleep(5);
-		/* NOTE: do we really have to wait for success ?
-		   Never seen anything else than 0x00 or 0x10
-		   (even with high payload) ...			*/
+		/*
+		 * NOTE: do we really have to wait for success ?
+		 * Never seen anything else than 0x00 or 0x10
+		 * (even with high payload) ...
+		 */
 	}
 	em28xx_warn("write to i2c device at 0x%x timed out\n", addr);
 	return -EIO;
@@ -230,29 +225,32 @@ static int em28xx_i2c_recv_bytes(struct em28xx *dev, u16 addr, u8 *buf, u16 len)
 
 	if (len < 1 || len > 64)
 		return -EOPNOTSUPP;
-	/* NOTE: limited by the USB ctrl message constraints
-	 * Zero length reads always succeed, even if no device is connected */
+	/*
+	 * NOTE: limited by the USB ctrl message constraints
+	 * Zero length reads always succeed, even if no device is connected
+	 */
 
 	/* Read data from i2c device */
 	ret = dev->em28xx_read_reg_req_len(dev, 2, addr, buf, len);
-	if (ret != len) {
-		if (ret < 0) {
-			em28xx_warn("reading from i2c device at 0x%x failed "
-				    "(error=%i)\n", addr, ret);
-			return ret;
-		} else {
-			em28xx_warn("%i bytes requested from i2c device at "
-				    "0x%x, but %i bytes received\n",
-				    len, addr, ret);
-			return -EIO;
-		}
+	if (ret < 0) {
+		em28xx_warn("reading from i2c device at 0x%x failed (error=%i)\n",
+			    addr, ret);
+		return ret;
 	}
+	/*
+	 * NOTE: some devices with two i2c busses have the bad habit to return 0
+	 * bytes if we are on bus B AND there was no write attempt to the
+	 * specified slave address before AND no device is present at the
+	 * requested slave address.
+	 * Anyway, the next check will fail with -ENODEV in this case, so avoid
+	 * spamming the system log on device probing and do nothing here.
+	 */
 
 	/* Check success of the i2c operation */
 	ret = dev->em28xx_read_reg(dev, 0x05);
 	if (ret < 0) {
-		em28xx_warn("failed to read i2c transfer status from "
-			    "bridge (error=%i)\n", ret);
+		em28xx_warn("failed to read i2c transfer status from bridge (error=%i)\n",
+			    ret);
 		return ret;
 	}
 	if (ret > 0) {
@@ -282,77 +280,254 @@ static int em28xx_i2c_check_for_device(struct em28xx *dev, u16 addr)
 }
 
 /*
+ * em25xx_bus_B_send_bytes
+ * write bytes to the i2c device
+ */
+static int em25xx_bus_B_send_bytes(struct em28xx *dev, u16 addr, u8 *buf,
+				   u16 len)
+{
+	int ret;
+
+	if (len < 1 || len > 64)
+		return -EOPNOTSUPP;
+	/*
+	 * NOTE: limited by the USB ctrl message constraints
+	 * Zero length reads always succeed, even if no device is connected
+	 */
+
+	/* Set register and write value */
+	ret = dev->em28xx_write_regs_req(dev, 0x06, addr, buf, len);
+	if (ret != len) {
+		if (ret < 0) {
+			em28xx_warn("writing to i2c device at 0x%x failed (error=%i)\n",
+				    addr, ret);
+			return ret;
+		} else {
+			em28xx_warn("%i bytes write to i2c device at 0x%x requested, but %i bytes written\n",
+				    len, addr, ret);
+			return -EIO;
+		}
+	}
+	/* Check success */
+	ret = dev->em28xx_read_reg_req(dev, 0x08, 0x0000);
+	/*
+	 * NOTE: the only error we've seen so far is
+	 * 0x01 when the slave device is not present
+	 */
+	if (!ret)
+		return len;
+	else if (ret > 0)
+		return -ENODEV;
+
+	return ret;
+	/*
+	 * NOTE: With chip types (other chip IDs) which actually don't support
+	 * this operation, it seems to succeed ALWAYS ! (even if there is no
+	 * slave device or even no second i2c bus provided)
+	 */
+}
+
+/*
+ * em25xx_bus_B_recv_bytes
+ * read bytes from the i2c device
+ */
+static int em25xx_bus_B_recv_bytes(struct em28xx *dev, u16 addr, u8 *buf,
+				   u16 len)
+{
+	int ret;
+
+	if (len < 1 || len > 64)
+		return -EOPNOTSUPP;
+	/*
+	 * NOTE: limited by the USB ctrl message constraints
+	 * Zero length reads always succeed, even if no device is connected
+	 */
+
+	/* Read value */
+	ret = dev->em28xx_read_reg_req_len(dev, 0x06, addr, buf, len);
+	if (ret < 0) {
+		em28xx_warn("reading from i2c device at 0x%x failed (error=%i)\n",
+			    addr, ret);
+		return ret;
+	}
+	/*
+	 * NOTE: some devices with two i2c busses have the bad habit to return 0
+	 * bytes if we are on bus B AND there was no write attempt to the
+	 * specified slave address before AND no device is present at the
+	 * requested slave address.
+	 * Anyway, the next check will fail with -ENODEV in this case, so avoid
+	 * spamming the system log on device probing and do nothing here.
+	 */
+
+	/* Check success */
+	ret = dev->em28xx_read_reg_req(dev, 0x08, 0x0000);
+	/*
+	 * NOTE: the only error we've seen so far is
+	 * 0x01 when the slave device is not present
+	 */
+	if (!ret)
+		return len;
+	else if (ret > 0)
+		return -ENODEV;
+
+	return ret;
+	/*
+	 * NOTE: With chip types (other chip IDs) which actually don't support
+	 * this operation, it seems to succeed ALWAYS ! (even if there is no
+	 * slave device or even no second i2c bus provided)
+	 */
+}
+
+/*
+ * em25xx_bus_B_check_for_device()
+ * check if there is a i2c device at the supplied address
+ */
+static int em25xx_bus_B_check_for_device(struct em28xx *dev, u16 addr)
+{
+	u8 buf;
+	int ret;
+
+	ret = em25xx_bus_B_recv_bytes(dev, addr, &buf, 1);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+	/*
+	 * NOTE: With chips which do not support this operation,
+	 * it seems to succeed ALWAYS ! (even if no device connected)
+	 */
+}
+
+static inline int i2c_check_for_device(struct em28xx_i2c_bus *i2c_bus, u16 addr)
+{
+	struct em28xx *dev = i2c_bus->dev;
+	int rc = -EOPNOTSUPP;
+
+	if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM28XX)
+		rc = em28xx_i2c_check_for_device(dev, addr);
+	else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM2800)
+		rc = em2800_i2c_check_for_device(dev, addr);
+	else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM25XX_BUS_B)
+		rc = em25xx_bus_B_check_for_device(dev, addr);
+	if (rc == -ENODEV) {
+		if (i2c_debug)
+			printk(" no device\n");
+	}
+	return rc;
+}
+
+static inline int i2c_recv_bytes(struct em28xx_i2c_bus *i2c_bus,
+				 struct i2c_msg msg)
+{
+	struct em28xx *dev = i2c_bus->dev;
+	u16 addr = msg.addr << 1;
+	int byte, rc = -EOPNOTSUPP;
+
+	if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM28XX)
+		rc = em28xx_i2c_recv_bytes(dev, addr, msg.buf, msg.len);
+	else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM2800)
+		rc = em2800_i2c_recv_bytes(dev, addr, msg.buf, msg.len);
+	else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM25XX_BUS_B)
+		rc = em25xx_bus_B_recv_bytes(dev, addr, msg.buf, msg.len);
+	if (i2c_debug) {
+		for (byte = 0; byte < msg.len; byte++)
+			printk(" %02x", msg.buf[byte]);
+	}
+	return rc;
+}
+
+static inline int i2c_send_bytes(struct em28xx_i2c_bus *i2c_bus,
+				 struct i2c_msg msg, int stop)
+{
+	struct em28xx *dev = i2c_bus->dev;
+	u16 addr = msg.addr << 1;
+	int byte, rc = -EOPNOTSUPP;
+
+	if (i2c_debug) {
+		for (byte = 0; byte < msg.len; byte++)
+			printk(" %02x", msg.buf[byte]);
+	}
+	if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM28XX)
+		rc = em28xx_i2c_send_bytes(dev, addr, msg.buf, msg.len, stop);
+	else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM2800)
+		rc = em2800_i2c_send_bytes(dev, addr, msg.buf, msg.len);
+	else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM25XX_BUS_B)
+		rc = em25xx_bus_B_send_bytes(dev, addr, msg.buf, msg.len);
+	return rc;
+}
+
+/*
  * em28xx_i2c_xfer()
  * the main i2c transfer function
  */
 static int em28xx_i2c_xfer(struct i2c_adapter *i2c_adap,
 			   struct i2c_msg msgs[], int num)
 {
-	struct em28xx *dev = i2c_adap->algo_data;
-	int addr, rc, i, byte;
+	struct em28xx_i2c_bus *i2c_bus = i2c_adap->algo_data;
+	struct em28xx *dev = i2c_bus->dev;
+	unsigned bus = i2c_bus->bus;
+	int addr, rc, i;
+	u8 reg;
+
+	rc = rt_mutex_trylock(&dev->i2c_bus_lock);
+	if (rc < 0)
+		return rc;
+
+	/* Switch I2C bus if needed */
+	if (bus != dev->cur_i2c_bus &&
+	    i2c_bus->algo_type == EM28XX_I2C_ALGO_EM28XX) {
+		if (bus == 1)
+			reg = EM2874_I2C_SECONDARY_BUS_SELECT;
+		else
+			reg = 0;
+		em28xx_write_reg_bits(dev, EM28XX_R06_I2C_CLK, reg,
+				      EM2874_I2C_SECONDARY_BUS_SELECT);
+		dev->cur_i2c_bus = bus;
+	}
 
-	if (num <= 0)
+	if (num <= 0) {
+		rt_mutex_unlock(&dev->i2c_bus_lock);
 		return 0;
+	}
 	for (i = 0; i < num; i++) {
 		addr = msgs[i].addr << 1;
-		dprintk2(2, "%s %s addr=%x len=%d:",
-			 (msgs[i].flags & I2C_M_RD) ? "read" : "write",
-			 i == num - 1 ? "stop" : "nonstop", addr, msgs[i].len);
+		if (i2c_debug)
+			printk(KERN_DEBUG "%s at %s: %s %s addr=%02x len=%d:",
+			       dev->name, __func__ ,
+			       (msgs[i].flags & I2C_M_RD) ? "read" : "write",
+			       i == num - 1 ? "stop" : "nonstop",
+			       addr, msgs[i].len);
 		if (!msgs[i].len) { /* no len: check only for device presence */
-			if (dev->board.is_em2800)
-				rc = em2800_i2c_check_for_device(dev, addr);
-			else
-				rc = em28xx_i2c_check_for_device(dev, addr);
+			rc = i2c_check_for_device(i2c_bus, addr);
 			if (rc == -ENODEV) {
-				if (i2c_debug >= 2)
-					printk(" no device\n");
+				rt_mutex_unlock(&dev->i2c_bus_lock);
 				return rc;
 			}
 		} else if (msgs[i].flags & I2C_M_RD) {
 			/* read bytes */
-			if (dev->board.is_em2800)
-				rc = em2800_i2c_recv_bytes(dev, addr,
-							   msgs[i].buf,
-							   msgs[i].len);
-			else
-				rc = em28xx_i2c_recv_bytes(dev, addr,
-							   msgs[i].buf,
-							   msgs[i].len);
-			if (i2c_debug >= 2) {
-				for (byte = 0; byte < msgs[i].len; byte++)
-					printk(" %02x", msgs[i].buf[byte]);
-			}
+			rc = i2c_recv_bytes(i2c_bus, msgs[i]);
 		} else {
 			/* write bytes */
-			if (i2c_debug >= 2) {
-				for (byte = 0; byte < msgs[i].len; byte++)
-					printk(" %02x", msgs[i].buf[byte]);
-			}
-			if (dev->board.is_em2800)
-				rc = em2800_i2c_send_bytes(dev, addr,
-							   msgs[i].buf,
-							   msgs[i].len);
-			else
-				rc = em28xx_i2c_send_bytes(dev, addr,
-							   msgs[i].buf,
-							   msgs[i].len,
-							   i == num - 1);
+			rc = i2c_send_bytes(i2c_bus, msgs[i], i == num - 1);
 		}
 		if (rc < 0) {
-			if (i2c_debug >= 2)
+			if (i2c_debug)
 				printk(" ERROR: %i\n", rc);
+			rt_mutex_unlock(&dev->i2c_bus_lock);
 			return rc;
 		}
-		if (i2c_debug >= 2)
+		if (i2c_debug)
 			printk("\n");
 	}
 
+	rt_mutex_unlock(&dev->i2c_bus_lock);
 	return num;
 }
 
-/* based on linux/sunrpc/svcauth.h and linux/hash.h
+/*
+ * based on linux/sunrpc/svcauth.h and linux/hash.h
  * The original hash function returns a different value, if arch is x86_64
- *  or i386.
+ * or i386.
  */
 static inline unsigned long em28xx_hash_mem(char *buf, int length, int bits)
 {
@@ -375,127 +550,230 @@ static inline unsigned long em28xx_hash_mem(char *buf, int length, int bits)
 	return (hash >> (32 - bits)) & 0xffffffffUL;
 }
 
-static int em28xx_i2c_eeprom(struct em28xx *dev, unsigned char *eedata, int len)
+/*
+ * Helper function to read data blocks from i2c clients with 8 or 16 bit
+ * address width, 8 bit register width and auto incrementation been activated
+ */
+static int em28xx_i2c_read_block(struct em28xx *dev, unsigned bus, u16 addr,
+				 bool addr_w16, u16 len, u8 *data)
 {
-	unsigned char buf, *p = eedata;
-	struct em28xx_eeprom *em_eeprom = (void *)eedata;
-	int i, err, size = len, block, block_max;
-
-	if (dev->chip_id == CHIP_ID_EM2874 ||
-	    dev->chip_id == CHIP_ID_EM28174 ||
-	    dev->chip_id == CHIP_ID_EM2884) {
-		/* Empia switched to a 16-bit addressable eeprom in newer
-		   devices.  While we could certainly write a routine to read
-		   the eeprom, there is nothing of use in there that cannot be
-		   accessed through registers, and there is the risk that we
-		   could corrupt the eeprom (since a 16-bit read call is
-		   interpreted as a write call by 8-bit eeproms).
-		*/
-		return 0;
+	int remain = len, rsize, rsize_max, ret;
+	u8 buf[2];
+
+	/* Sanity check */
+	if (addr + remain > (addr_w16 * 0xff00 + 0xff + 1))
+		return -EINVAL;
+	/* Select address */
+	buf[0] = addr >> 8;
+	buf[1] = addr & 0xff;
+	ret = i2c_master_send(&dev->i2c_client[bus], buf + !addr_w16, 1 + addr_w16);
+	if (ret < 0)
+		return ret;
+	/* Read data */
+	if (dev->board.is_em2800)
+		rsize_max = 4;
+	else
+		rsize_max = 64;
+	while (remain > 0) {
+		if (remain > rsize_max)
+			rsize = rsize_max;
+		else
+			rsize = remain;
+
+		ret = i2c_master_recv(&dev->i2c_client[bus], data, rsize);
+		if (ret < 0)
+			return ret;
+
+		remain -= rsize;
+		data += rsize;
 	}
 
-	dev->i2c_client.addr = 0xa0 >> 1;
+	return len;
+}
+
+static int em28xx_i2c_eeprom(struct em28xx *dev, unsigned bus,
+			     u8 **eedata, u16 *eedata_len)
+{
+	const u16 len = 256;
+	/*
+	 * FIXME common length/size for bytes to read, to display, hash
+	 * calculation and returned device dataset. Simplifies the code a lot,
+	 * but we might have to deal with multiple sizes in the future !
+	 */
+	int i, err;
+	struct em28xx_eeprom *dev_config;
+	u8 buf, *data;
+
+	*eedata = NULL;
+	*eedata_len = 0;
+
+	/* EEPROM is always on i2c bus 0 on all known devices. */
+
+	dev->i2c_client[bus].addr = 0xa0 >> 1;
 
 	/* Check if board has eeprom */
-	err = i2c_master_recv(&dev->i2c_client, &buf, 0);
+	err = i2c_master_recv(&dev->i2c_client[bus], &buf, 0);
 	if (err < 0) {
-		em28xx_errdev("board has no eeprom\n");
-		memset(eedata, 0, len);
+		em28xx_info("board has no eeprom\n");
 		return -ENODEV;
 	}
 
-	buf = 0;
-
-	err = i2c_master_send(&dev->i2c_client, &buf, 1);
-	if (err != 1) {
-		printk(KERN_INFO "%s: Huh, no eeprom present (err=%d)?\n",
-		       dev->name, err);
-		return err;
+	data = kzalloc(len, GFP_KERNEL);
+	if (data == NULL)
+		return -ENOMEM;
+
+	/* Read EEPROM content */
+	err = em28xx_i2c_read_block(dev, bus, 0x0000,
+				    dev->eeprom_addrwidth_16bit,
+				    len, data);
+	if (err != len) {
+		em28xx_errdev("failed to read eeprom (err=%d)\n", err);
+		goto error;
 	}
 
-	if (dev->board.is_em2800)
-		block_max = 4;
-	else
-		block_max = 64;
-
-	while (size > 0) {
-		if (size > block_max)
-			block = block_max;
-		else
-			block = size;
-
-		if (block !=
-		    (err = i2c_master_recv(&dev->i2c_client, p, block))) {
-			printk(KERN_WARNING
-			       "%s: i2c eeprom read error (err=%d)\n",
-			       dev->name, err);
-			return err;
-		}
-		size -= block;
-		p += block;
-	}
+	/* Display eeprom content */
 	for (i = 0; i < len; i++) {
-		if (0 == (i % 16))
-			printk(KERN_INFO "%s: i2c eeprom %02x:", dev->name, i);
-		printk(" %02x", eedata[i]);
+		if (0 == (i % 16)) {
+			if (dev->eeprom_addrwidth_16bit)
+				em28xx_info("i2c eeprom %04x:", i);
+			else
+				em28xx_info("i2c eeprom %02x:", i);
+		}
+		printk(" %02x", data[i]);
 		if (15 == (i % 16))
 			printk("\n");
 	}
+	if (dev->eeprom_addrwidth_16bit)
+		em28xx_info("i2c eeprom %04x: ... (skipped)\n", i);
+
+	if (dev->eeprom_addrwidth_16bit &&
+	    data[0] == 0x26 && data[3] == 0x00) {
+		/* new eeprom format; size 4-64kb */
+		u16 mc_start;
+		u16 hwconf_offset;
+
+		dev->hash = em28xx_hash_mem(data, len, 32);
+		mc_start = (data[1] << 8) + 4;	/* usually 0x0004 */
+
+		em28xx_info("EEPROM ID = %02x %02x %02x %02x, EEPROM hash = 0x%08lx\n",
+			    data[0], data[1], data[2], data[3], dev->hash);
+		em28xx_info("EEPROM info:\n");
+		em28xx_info("\tmicrocode start address = 0x%04x, boot configuration = 0x%02x\n",
+			    mc_start, data[2]);
+		/*
+		 * boot configuration (address 0x0002):
+		 * [0]   microcode download speed: 1 = 400 kHz; 0 = 100 kHz
+		 * [1]   always selects 12 kb RAM
+		 * [2]   USB device speed: 1 = force Full Speed; 0 = auto detect
+		 * [4]   1 = force fast mode and no suspend for device testing
+		 * [5:7] USB PHY tuning registers; determined by device
+		 *       characterization
+		 */
+
+		/*
+		 * Read hardware config dataset offset from address
+		 * (microcode start + 46)
+		 */
+		err = em28xx_i2c_read_block(dev, bus, mc_start + 46, 1, 2,
+					    data);
+		if (err != 2) {
+			em28xx_errdev("failed to read hardware configuration data from eeprom (err=%d)\n",
+				      err);
+			goto error;
+		}
 
-	if (em_eeprom->id == 0x9567eb1a)
-		dev->hash = em28xx_hash_mem(eedata, len, 32);
+		/* Calculate hardware config dataset start address */
+		hwconf_offset = mc_start + data[0] + (data[1] << 8);
+
+		/* Read hardware config dataset */
+		/*
+		 * NOTE: the microcode copy can be multiple pages long, but
+		 * we assume the hardware config dataset is the same as in
+		 * the old eeprom and not longer than 256 bytes.
+		 * tveeprom is currently also limited to 256 bytes.
+		 */
+		err = em28xx_i2c_read_block(dev, bus, hwconf_offset, 1, len,
+					    data);
+		if (err != len) {
+			em28xx_errdev("failed to read hardware configuration data from eeprom (err=%d)\n",
+				      err);
+			goto error;
+		}
 
-	printk(KERN_INFO "%s: EEPROM ID= 0x%08x, EEPROM hash = 0x%08lx\n",
-	       dev->name, em_eeprom->id, dev->hash);
+		/* Verify hardware config dataset */
+		/* NOTE: not all devices provide this type of dataset */
+		if (data[0] != 0x1a || data[1] != 0xeb ||
+		    data[2] != 0x67 || data[3] != 0x95) {
+			em28xx_info("\tno hardware configuration dataset found in eeprom\n");
+			kfree(data);
+			return 0;
+		}
 
-	printk(KERN_INFO "%s: EEPROM info:\n", dev->name);
+		/* TODO: decrypt eeprom data for camera bridges (em25xx, em276x+) */
+
+	} else if (!dev->eeprom_addrwidth_16bit &&
+		   data[0] == 0x1a && data[1] == 0xeb &&
+		   data[2] == 0x67 && data[3] == 0x95) {
+		dev->hash = em28xx_hash_mem(data, len, 32);
+		em28xx_info("EEPROM ID = %02x %02x %02x %02x, EEPROM hash = 0x%08lx\n",
+			    data[0], data[1], data[2], data[3], dev->hash);
+		em28xx_info("EEPROM info:\n");
+	} else {
+		em28xx_info("unknown eeprom format or eeprom corrupted !\n");
+		err = -ENODEV;
+		goto error;
+	}
 
-	switch (em_eeprom->chip_conf >> 4 & 0x3) {
+	*eedata = data;
+	*eedata_len = len;
+	dev_config = (void *)eedata;
+
+	switch (le16_to_cpu(dev_config->chip_conf) >> 4 & 0x3) {
 	case 0:
-		printk(KERN_INFO "%s:\tNo audio on board.\n", dev->name);
+		em28xx_info("\tNo audio on board.\n");
 		break;
 	case 1:
-		printk(KERN_INFO "%s:\tAC97 audio (5 sample rates)\n",
-				 dev->name);
+		em28xx_info("\tAC97 audio (5 sample rates)\n");
 		break;
 	case 2:
-		printk(KERN_INFO "%s:\tI2S audio, sample rate=32k\n",
-				 dev->name);
+		em28xx_info("\tI2S audio, sample rate=32k\n");
 		break;
 	case 3:
-		printk(KERN_INFO "%s:\tI2S audio, 3 sample rates\n",
-				 dev->name);
+		em28xx_info("\tI2S audio, 3 sample rates\n");
 		break;
 	}
 
-	if (em_eeprom->chip_conf & 1 << 3)
-		printk(KERN_INFO "%s:\tUSB Remote wakeup capable\n", dev->name);
+	if (le16_to_cpu(dev_config->chip_conf) & 1 << 3)
+		em28xx_info("\tUSB Remote wakeup capable\n");
 
-	if (em_eeprom->chip_conf & 1 << 2)
-		printk(KERN_INFO "%s:\tUSB Self power capable\n", dev->name);
+	if (le16_to_cpu(dev_config->chip_conf) & 1 << 2)
+		em28xx_info("\tUSB Self power capable\n");
 
-	switch (em_eeprom->chip_conf & 0x3) {
+	switch (le16_to_cpu(dev_config->chip_conf) & 0x3) {
 	case 0:
-		printk(KERN_INFO "%s:\t500mA max power\n", dev->name);
+		em28xx_info("\t500mA max power\n");
 		break;
 	case 1:
-		printk(KERN_INFO "%s:\t400mA max power\n", dev->name);
+		em28xx_info("\t400mA max power\n");
 		break;
 	case 2:
-		printk(KERN_INFO "%s:\t300mA max power\n", dev->name);
+		em28xx_info("\t300mA max power\n");
 		break;
 	case 3:
-		printk(KERN_INFO "%s:\t200mA max power\n", dev->name);
+		em28xx_info("\t200mA max power\n");
 		break;
 	}
-	printk(KERN_INFO "%s:\tTable at 0x%02x, strings=0x%04x, 0x%04x, 0x%04x\n",
-				dev->name,
-				em_eeprom->string_idx_table,
-				em_eeprom->string1,
-				em_eeprom->string2,
-				em_eeprom->string3);
+	em28xx_info("\tTable at offset 0x%02x, strings=0x%04x, 0x%04x, 0x%04x\n",
+		    dev_config->string_idx_table,
+		    le16_to_cpu(dev_config->string1),
+		    le16_to_cpu(dev_config->string2),
+		    le16_to_cpu(dev_config->string3));
 
 	return 0;
+
+error:
+	kfree(data);
+	return err;
 }
 
 /* ----------------------------------------------------------- */
@@ -503,13 +781,20 @@ static int em28xx_i2c_eeprom(struct em28xx *dev, unsigned char *eedata, int len)
 /*
  * functionality()
  */
-static u32 functionality(struct i2c_adapter *adap)
+static u32 functionality(struct i2c_adapter *i2c_adap)
 {
-	struct em28xx *dev = adap->algo_data;
-	u32 func_flags = I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
-	if (dev->board.is_em2800)
-		func_flags &= ~I2C_FUNC_SMBUS_WRITE_BLOCK_DATA;
-	return func_flags;
+	struct em28xx_i2c_bus *i2c_bus = i2c_adap->algo_data;
+
+	if ((i2c_bus->algo_type == EM28XX_I2C_ALGO_EM28XX) ||
+	    (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM25XX_BUS_B)) {
+		return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+	} else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM2800)  {
+		return (I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL) &
+			~I2C_FUNC_SMBUS_WRITE_BLOCK_DATA;
+	}
+
+	WARN(1, "Unknown i2c bus algorithm.\n");
+	return 0;
 }
 
 static struct i2c_algorithm em28xx_algo = {
@@ -556,7 +841,7 @@ static char *i2c_devs[128] = {
  * do_i2c_scan()
  * check i2c address range for devices
  */
-void em28xx_do_i2c_scan(struct em28xx *dev)
+void em28xx_do_i2c_scan(struct em28xx *dev, unsigned bus)
 {
 	u8 i2c_devicelist[128];
 	unsigned char buf;
@@ -565,55 +850,68 @@ void em28xx_do_i2c_scan(struct em28xx *dev)
 	memset(i2c_devicelist, 0, ARRAY_SIZE(i2c_devicelist));
 
 	for (i = 0; i < ARRAY_SIZE(i2c_devs); i++) {
-		dev->i2c_client.addr = i;
-		rc = i2c_master_recv(&dev->i2c_client, &buf, 0);
+		dev->i2c_client[bus].addr = i;
+		rc = i2c_master_recv(&dev->i2c_client[bus], &buf, 0);
 		if (rc < 0)
 			continue;
 		i2c_devicelist[i] = i;
-		printk(KERN_INFO "%s: found i2c device @ 0x%x [%s]\n",
-		       dev->name, i << 1, i2c_devs[i] ? i2c_devs[i] : "???");
+		em28xx_info("found i2c device @ 0x%x on bus %d [%s]\n",
+			    i << 1, bus, i2c_devs[i] ? i2c_devs[i] : "???");
 	}
 
-	dev->i2c_hash = em28xx_hash_mem(i2c_devicelist,
-					ARRAY_SIZE(i2c_devicelist), 32);
+	if (bus == dev->def_i2c_bus)
+		dev->i2c_hash = em28xx_hash_mem(i2c_devicelist,
+						ARRAY_SIZE(i2c_devicelist), 32);
 }
 
 /*
  * em28xx_i2c_register()
  * register i2c bus
  */
-int em28xx_i2c_register(struct em28xx *dev)
+int em28xx_i2c_register(struct em28xx *dev, unsigned bus,
+			enum em28xx_i2c_algo_type algo_type)
 {
 	int retval;
 
 	BUG_ON(!dev->em28xx_write_regs || !dev->em28xx_read_reg);
 	BUG_ON(!dev->em28xx_write_regs_req || !dev->em28xx_read_reg_req);
-	dev->i2c_adap = em28xx_adap_template;
-	dev->i2c_adap.dev.parent = &dev->udev->dev;
-	strcpy(dev->i2c_adap.name, dev->name);
-	dev->i2c_adap.algo_data = dev;
-	i2c_set_adapdata(&dev->i2c_adap, &dev->v4l2_dev);
 
-	retval = i2c_add_adapter(&dev->i2c_adap);
+	if (bus >= NUM_I2C_BUSES)
+		return -ENODEV;
+
+	dev->i2c_adap[bus] = em28xx_adap_template;
+	dev->i2c_adap[bus].dev.parent = &dev->udev->dev;
+	strcpy(dev->i2c_adap[bus].name, dev->name);
+
+	dev->i2c_bus[bus].bus = bus;
+	dev->i2c_bus[bus].algo_type = algo_type;
+	dev->i2c_bus[bus].dev = dev;
+	dev->i2c_adap[bus].algo_data = &dev->i2c_bus[bus];
+	i2c_set_adapdata(&dev->i2c_adap[bus], &dev->v4l2_dev);
+
+	retval = i2c_add_adapter(&dev->i2c_adap[bus]);
 	if (retval < 0) {
 		em28xx_errdev("%s: i2c_add_adapter failed! retval [%d]\n",
 			__func__, retval);
 		return retval;
 	}
 
-	dev->i2c_client = em28xx_client_template;
-	dev->i2c_client.adapter = &dev->i2c_adap;
+	dev->i2c_client[bus] = em28xx_client_template;
+	dev->i2c_client[bus].adapter = &dev->i2c_adap[bus];
 
-	retval = em28xx_i2c_eeprom(dev, dev->eedata, sizeof(dev->eedata));
-	if ((retval < 0) && (retval != -ENODEV)) {
-		em28xx_errdev("%s: em28xx_i2_eeprom failed! retval [%d]\n",
-			__func__, retval);
+	/* Up to now, all eeproms are at bus 0 */
+	if (!bus) {
+		retval = em28xx_i2c_eeprom(dev, bus, &dev->eedata, &dev->eedata_len);
+		if ((retval < 0) && (retval != -ENODEV)) {
+			em28xx_errdev("%s: em28xx_i2_eeprom failed! retval [%d]\n",
+				__func__, retval);
 
-		return retval;
+			return retval;
+		}
 	}
 
 	if (i2c_scan)
-		em28xx_do_i2c_scan(dev);
+		em28xx_do_i2c_scan(dev, bus);
 
 	return 0;
 }
@@ -622,8 +920,11 @@ int em28xx_i2c_register(struct em28xx *dev)
  * em28xx_i2c_unregister()
  * unregister i2c_bus
  */
-int em28xx_i2c_unregister(struct em28xx *dev)
+int em28xx_i2c_unregister(struct em28xx *dev, unsigned bus)
 {
-	i2c_del_adapter(&dev->i2c_adap);
+	if (bus >= NUM_I2C_BUSES)
+		return -ENODEV;
+
+	i2c_del_adapter(&dev->i2c_adap[bus]);
 	return 0;
 }
diff --git a/drivers/media/usb/em28xx/em28xx-input.c b/drivers/media/usb/em28xx/em28xx-input.c
index 1bef990b3f18..466b19d0d767 100644
--- a/drivers/media/usb/em28xx/em28xx-input.c
+++ b/drivers/media/usb/em28xx/em28xx-input.c
@@ -280,11 +280,12 @@ static int em2874_polling_getkey(struct em28xx_IR *ir,
 
 static int em28xx_i2c_ir_handle_key(struct em28xx_IR *ir)
 {
+	struct em28xx *dev = ir->dev;
 	static u32 ir_key;
 	int rc;
 	struct i2c_client client;
 
-	client.adapter = &ir->dev->i2c_adap;
+	client.adapter = &ir->dev->i2c_adap[dev->def_i2c_bus];
 	client.addr = ir->i2c_dev_addr;
 
 	rc = ir->get_key_i2c(&client, &ir_key);
@@ -461,7 +462,7 @@ static int em28xx_probe_i2c_ir(struct em28xx *dev)
 	};
 
 	while (addr_list[i] != I2C_CLIENT_END) {
-		if (i2c_probe_func_quick_read(&dev->i2c_adap, addr_list[i]) == 1)
+		if (i2c_probe_func_quick_read(&dev->i2c_adap[dev->def_i2c_bus], addr_list[i]) == 1)
 			return addr_list[i];
 		i++;
 	}
diff --git a/drivers/media/usb/em28xx/em28xx-reg.h b/drivers/media/usb/em28xx/em28xx-reg.h
index 885089e22bcd..622871db04aa 100644
--- a/drivers/media/usb/em28xx/em28xx-reg.h
+++ b/drivers/media/usb/em28xx/em28xx-reg.h
@@ -48,7 +48,7 @@
 #define EM28XX_CHIPCFG2_TS_PACKETSIZE_752	0x03
 
 
-	/* GPIO/GPO registers */
+/* GPIO/GPO registers */
 #define EM2880_R04_GPO	0x04    /* em2880-em2883 only */
 #define EM28XX_R08_GPIO	0x08	/* em2820 or upper */
 
@@ -120,12 +120,23 @@
 #define EM28XX_R1E_CWIDTH	0x1e
 #define EM28XX_R1F_CHEIGHT	0x1f
 
-#define EM28XX_R20_YGAIN	0x20
-#define EM28XX_R21_YOFFSET	0x21
-#define EM28XX_R22_UVGAIN	0x22
-#define EM28XX_R23_UOFFSET	0x23
-#define EM28XX_R24_VOFFSET	0x24
-#define EM28XX_R25_SHARPNESS	0x25
+#define EM28XX_R20_YGAIN	0x20 /* contrast [0:4]   */
+#define   CONTRAST_DEFAULT	0x10
+
+#define EM28XX_R21_YOFFSET	0x21 /* brightness       */	/* signed */
+#define   BRIGHTNESS_DEFAULT	0x00
+
+#define EM28XX_R22_UVGAIN	0x22 /* saturation [0:4] */
+#define   SATURATION_DEFAULT	0x10
+
+#define EM28XX_R23_UOFFSET	0x23 /* blue balance     */	/* signed */
+#define   BLUE_BALANCE_DEFAULT	0x00
+
+#define EM28XX_R24_VOFFSET	0x24 /* red balance      */	/* signed */
+#define   RED_BALANCE_DEFAULT	0x00
+
+#define EM28XX_R25_SHARPNESS	0x25 /* sharpness [0:4]  */
+#define   SHARPNESS_DEFAULT	0x00
 
 #define EM28XX_R26_COMPR	0x26
 #define EM28XX_R27_OUTFMT	0x27
@@ -152,8 +163,17 @@
 #define EM28XX_R31_HSCALEHIGH	0x31
 #define EM28XX_R32_VSCALELOW	0x32
 #define EM28XX_R33_VSCALEHIGH	0x33
+#define   EM28XX_HVSCALE_MAX	0x3fff /* => 20% */
+
 #define EM28XX_R34_VBI_START_H	0x34
 #define EM28XX_R35_VBI_START_V	0x35
+/*
+ * NOTE: the EM276x (and EM25xx, EM277x/8x ?) (camera bridges) use these
+ * registers for a different unknown purpose.
+ *   => register 0x34 is set to capture width / 16
+ *   => register 0x35 is set to capture height / 16
+ */
+
 #define EM28XX_R36_VBI_WIDTH	0x36
 #define EM28XX_R37_VBI_HEIGHT	0x37
 
@@ -206,6 +226,7 @@ enum em28xx_chip_id {
 	CHIP_ID_EM2860 = 34,
 	CHIP_ID_EM2870 = 35,
 	CHIP_ID_EM2883 = 36,
+	CHIP_ID_EM2765 = 54,
 	CHIP_ID_EM2874 = 65,
 	CHIP_ID_EM2884 = 68,
 	CHIP_ID_EM28174 = 113,
diff --git a/drivers/media/usb/em28xx/em28xx-video.c b/drivers/media/usb/em28xx/em28xx-video.c
index 32bd7de5dec1..32d60e5546bc 100644
--- a/drivers/media/usb/em28xx/em28xx-video.c
+++ b/drivers/media/usb/em28xx/em28xx-video.c
@@ -52,7 +52,7 @@
 
 #define DRIVER_DESC         "Empia em28xx based USB video device driver"
 
-#define EM28XX_VERSION "0.1.3"
+#define EM28XX_VERSION "0.2.0"
 
 #define em28xx_videodbg(fmt, arg...) do {\
 	if (video_debug) \
@@ -76,6 +76,16 @@ MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
 MODULE_VERSION(EM28XX_VERSION);
 
+
+#define EM25XX_FRMDATAHDR_BYTE1			0x02
+#define EM25XX_FRMDATAHDR_BYTE2_STILL_IMAGE	0x20
+#define EM25XX_FRMDATAHDR_BYTE2_FRAME_END	0x02
+#define EM25XX_FRMDATAHDR_BYTE2_FRAME_ID	0x01
+#define EM25XX_FRMDATAHDR_BYTE2_MASK	(EM25XX_FRMDATAHDR_BYTE2_STILL_IMAGE | \
+					 EM25XX_FRMDATAHDR_BYTE2_FRAME_END |   \
+					 EM25XX_FRMDATAHDR_BYTE2_FRAME_ID)
+
+
 static unsigned int video_nr[] = {[0 ... (EM28XX_MAXBOARDS - 1)] = -1U };
 static unsigned int vbi_nr[]   = {[0 ... (EM28XX_MAXBOARDS - 1)] = -1U };
 static unsigned int radio_nr[] = {[0 ... (EM28XX_MAXBOARDS - 1)] = -1U };
@@ -408,6 +418,62 @@ static inline void process_frame_data_em28xx(struct em28xx *dev,
 		em28xx_copy_video(dev, buf, data_pkt, data_len);
 }
 
+/*
+ * Process data packet according to the em25xx/em276x/7x/8x frame data format
+ */
+static inline void process_frame_data_em25xx(struct em28xx *dev,
+					     unsigned char *data_pkt,
+					     unsigned int  data_len)
+{
+	struct em28xx_buffer    *buf = dev->usb_ctl.vid_buf;
+	struct em28xx_dmaqueue  *dmaq = &dev->vidq;
+	bool frame_end = 0;
+
+	/* Check for header */
+	/* NOTE: at least with bulk transfers, only the first packet
+	 * has a header and has always set the FRAME_END bit         */
+	if (data_len >= 2) {	/* em25xx header is only 2 bytes long */
+		if ((data_pkt[0] == EM25XX_FRMDATAHDR_BYTE1) &&
+		    ((data_pkt[1] & ~EM25XX_FRMDATAHDR_BYTE2_MASK) == 0x00)) {
+			dev->top_field = !(data_pkt[1] &
+					   EM25XX_FRMDATAHDR_BYTE2_FRAME_ID);
+			frame_end = data_pkt[1] &
+				    EM25XX_FRMDATAHDR_BYTE2_FRAME_END;
+			data_pkt += 2;
+			data_len -= 2;
+		}
+
+		/* Finish field and prepare next (BULK only) */
+		if (dev->analog_xfer_bulk && frame_end) {
+			buf = finish_field_prepare_next(dev, buf, dmaq);
+			dev->usb_ctl.vid_buf = buf;
+		}
+		/* NOTE: in ISOC mode when a new frame starts and buf==NULL,
+		 * we COULD already prepare a buffer here to avoid skipping the
+		 * first frame.
+		 */
+	}
+
+	/* Copy data */
+	if (buf != NULL && data_len > 0)
+		em28xx_copy_video(dev, buf, data_pkt, data_len);
+
+	/* Finish frame (ISOC only) => avoids lag of 1 frame */
+	if (!dev->analog_xfer_bulk && frame_end) {
+		buf = finish_field_prepare_next(dev, buf, dmaq);
+		dev->usb_ctl.vid_buf = buf;
+	}
+
+	/* NOTE: Tested with USB bulk transfers only !
+	 * The wording in the datasheet suggests that isoc might work different.
+	 * The current code assumes that with isoc transfers each packet has a
+	 * header like with the other em28xx devices.
+	 */
+	/* NOTE: Support for interlaced mode is pure theory. It has not been
+	 * tested and it is unknown if these devices actually support it. */
+	/* NOTE: No VBI support yet (these chips likely do not support VBI). */
+}
+
 /* Processes and copies the URB data content (video and VBI data) */
 static inline int em28xx_urb_data_copy(struct em28xx *dev, struct urb *urb)
 {
@@ -460,7 +526,13 @@ static inline int em28xx_urb_data_copy(struct em28xx *dev, struct urb *urb)
 			continue;
 		}
 
-		process_frame_data_em28xx(dev, usb_data_pkt, usb_data_len);
+		if (dev->is_em25xx)
+			process_frame_data_em25xx(dev,
+						  usb_data_pkt, usb_data_len);
+		else
+			process_frame_data_em28xx(dev,
+						  usb_data_pkt, usb_data_len);
+
 	}
 	return 1;
 }
@@ -700,6 +772,7 @@ int em28xx_vb2_setup(struct em28xx *dev)
 	q = &dev->vb_vidq;
 	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	q->io_modes = VB2_READ | VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	q->drv_priv = dev;
 	q->buf_struct_size = sizeof(struct em28xx_buffer);
 	q->ops = &em28xx_video_qops;
@@ -713,6 +786,7 @@ int em28xx_vb2_setup(struct em28xx *dev)
 	q = &dev->vb_vbiq;
 	q->type = V4L2_BUF_TYPE_VBI_CAPTURE;
 	q->io_modes = VB2_READ | VB2_MMAP | VB2_USERPTR;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	q->drv_priv = dev;
 	q->buf_struct_size = sizeof(struct em28xx_buffer);
 	q->ops = &em28xx_vbi_qops;
@@ -782,33 +856,45 @@ void em28xx_ctrl_notify(struct v4l2_ctrl *ctrl, void *priv)
 static int em28xx_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct em28xx *dev = container_of(ctrl->handler, struct em28xx, ctrl_handler);
+	int ret = -EINVAL;
 
 	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_MUTE:
 		dev->mute = ctrl->val;
+		ret = em28xx_audio_analog_set(dev);
 		break;
 	case V4L2_CID_AUDIO_VOLUME:
 		dev->volume = ctrl->val;
+		ret = em28xx_audio_analog_set(dev);
+		break;
+	case V4L2_CID_CONTRAST:
+		ret = em28xx_write_reg(dev, EM28XX_R20_YGAIN, ctrl->val);
+		break;
+	case V4L2_CID_BRIGHTNESS:
+		ret = em28xx_write_reg(dev, EM28XX_R21_YOFFSET, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		ret = em28xx_write_reg(dev, EM28XX_R22_UVGAIN, ctrl->val);
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		ret = em28xx_write_reg(dev, EM28XX_R23_UOFFSET, ctrl->val);
+		break;
+	case V4L2_CID_RED_BALANCE:
+		ret = em28xx_write_reg(dev, EM28XX_R24_VOFFSET, ctrl->val);
+		break;
+	case V4L2_CID_SHARPNESS:
+		ret = em28xx_write_reg(dev, EM28XX_R25_SHARPNESS, ctrl->val);
 		break;
 	}
 
-	return em28xx_audio_analog_set(dev);
+	return (ret < 0) ? ret : 0;
 }
 
 const struct v4l2_ctrl_ops em28xx_ctrl_ops = {
 	.s_ctrl = em28xx_s_ctrl,
 };
 
-static int check_dev(struct em28xx *dev)
-{
-	if (dev->disconnected) {
-		em28xx_errdev("v4l2 ioctl: device not present\n");
-		return -ENODEV;
-	}
-	return 0;
-}
-
-static void get_scale(struct em28xx *dev,
+static void size_to_scale(struct em28xx *dev,
 			unsigned int width, unsigned int height,
 			unsigned int *hscale, unsigned int *vscale)
 {
@@ -816,12 +902,23 @@ static void get_scale(struct em28xx *dev,
 	unsigned int          maxh = norm_maxh(dev);
 
 	*hscale = (((unsigned long)maxw) << 12) / width - 4096L;
-	if (*hscale >= 0x4000)
-		*hscale = 0x3fff;
+	if (*hscale > EM28XX_HVSCALE_MAX)
+		*hscale = EM28XX_HVSCALE_MAX;
 
 	*vscale = (((unsigned long)maxh) << 12) / height - 4096L;
-	if (*vscale >= 0x4000)
-		*vscale = 0x3fff;
+	if (*vscale > EM28XX_HVSCALE_MAX)
+		*vscale = EM28XX_HVSCALE_MAX;
+}
+
+static void scale_to_size(struct em28xx *dev,
+			  unsigned int hscale, unsigned int vscale,
+			  unsigned int *width, unsigned int *height)
+{
+	unsigned int          maxw = norm_maxw(dev);
+	unsigned int          maxh = norm_maxh(dev);
+
+	*width = (((unsigned long)maxw) << 12) / (hscale + 4096L);
+	*height = (((unsigned long)maxh) << 12) / (vscale + 4096L);
 }
 
 /* ------------------------------------------------------------------
@@ -898,10 +995,8 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 				      1, 0);
 	}
 
-	get_scale(dev, width, height, &hscale, &vscale);
-
-	width = (((unsigned long)maxw) << 12) / (hscale + 4096L);
-	height = (((unsigned long)maxh) << 12) / (vscale + 4096L);
+	size_to_scale(dev, width, height, &hscale, &vscale);
+	scale_to_size(dev, hscale, vscale, &width, &height);
 
 	f->fmt.pix.width = width;
 	f->fmt.pix.height = height;
@@ -932,7 +1027,7 @@ static int em28xx_set_video_format(struct em28xx *dev, unsigned int fourcc,
 	dev->height = height;
 
 	/* set new image size */
-	get_scale(dev, dev->width, dev->height, &dev->hscale, &dev->vscale);
+	size_to_scale(dev, dev->width, dev->height, &dev->hscale, &dev->vscale);
 
 	em28xx_resolution_set(dev);
 
@@ -957,13 +1052,6 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *norm)
 {
 	struct em28xx_fh   *fh  = priv;
 	struct em28xx      *dev = fh->dev;
-	int                rc;
-
-	if (dev->board.is_webcam)
-		return -ENOTTY;
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
 
 	*norm = dev->norm;
 
@@ -974,48 +1062,35 @@ static int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *norm)
 {
 	struct em28xx_fh   *fh  = priv;
 	struct em28xx      *dev = fh->dev;
-	int                rc;
-
-	if (dev->board.is_webcam)
-		return -ENOTTY;
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
 
 	v4l2_device_call_all(&dev->v4l2_dev, 0, video, querystd, norm);
 
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *norm)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 {
 	struct em28xx_fh   *fh  = priv;
 	struct em28xx      *dev = fh->dev;
 	struct v4l2_format f;
-	int                rc;
 
-	if (dev->board.is_webcam)
-		return -ENOTTY;
-	if (*norm == dev->norm)
+	if (norm == dev->norm)
 		return 0;
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
 
 	if (dev->streaming_users > 0)
 		return -EBUSY;
 
-	dev->norm = *norm;
+	dev->norm = norm;
 
 	/* Adjusts width/height, if needed */
 	f.fmt.pix.width = 720;
-	f.fmt.pix.height = (*norm & V4L2_STD_525_60) ? 480 : 576;
+	f.fmt.pix.height = (norm & V4L2_STD_525_60) ? 480 : 576;
 	vidioc_try_fmt_vid_cap(file, priv, &f);
 
 	/* set new image size */
 	dev->width = f.fmt.pix.width;
 	dev->height = f.fmt.pix.height;
-	get_scale(dev, dev->width, dev->height, &dev->hscale, &dev->vscale);
+	size_to_scale(dev, dev->width, dev->height, &dev->hscale, &dev->vscale);
 
 	em28xx_resolution_set(dev);
 	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_std, dev->norm);
@@ -1030,9 +1105,6 @@ static int vidioc_g_parm(struct file *file, void *priv,
 	struct em28xx      *dev = fh->dev;
 	int rc = 0;
 
-	if (p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
 	p->parm.capture.readbuffers = EM28XX_MIN_BUF;
 	if (dev->board.is_webcam)
 		rc = v4l2_device_call_until_err(&dev->v4l2_dev, 0,
@@ -1050,12 +1122,6 @@ static int vidioc_s_parm(struct file *file, void *priv,
 	struct em28xx_fh   *fh  = priv;
 	struct em28xx      *dev = fh->dev;
 
-	if (!dev->board.is_webcam)
-		return -ENOTTY;
-
-	if (p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
 	p->parm.capture.readbuffers = EM28XX_MIN_BUF;
 	return v4l2_device_call_until_err(&dev->v4l2_dev, 0, video, s_parm, p);
 }
@@ -1116,11 +1182,6 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 {
 	struct em28xx_fh   *fh  = priv;
 	struct em28xx      *dev = fh->dev;
-	int                rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
 
 	if (i >= MAX_EM28XX_INPUT)
 		return -EINVAL;
@@ -1136,9 +1197,6 @@ static int vidioc_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
 	struct em28xx_fh   *fh    = priv;
 	struct em28xx      *dev   = fh->dev;
 
-	if (!dev->audio_mode.has_audio)
-		return -EINVAL;
-
 	switch (a->index) {
 	case EM28XX_AMUX_VIDEO:
 		strcpy(a->name, "Television");
@@ -1179,10 +1237,6 @@ static int vidioc_s_audio(struct file *file, void *priv, const struct v4l2_audio
 	struct em28xx_fh   *fh  = priv;
 	struct em28xx      *dev = fh->dev;
 
-
-	if (!dev->audio_mode.has_audio)
-		return -EINVAL;
-
 	if (a->index >= MAX_EM28XX_INPUT)
 		return -EINVAL;
 	if (0 == INPUT(a->index)->type)
@@ -1202,11 +1256,6 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 {
 	struct em28xx_fh      *fh  = priv;
 	struct em28xx         *dev = fh->dev;
-	int                   rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
 
 	if (0 != t->index)
 		return -EINVAL;
@@ -1218,15 +1267,10 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct em28xx_fh      *fh  = priv;
 	struct em28xx         *dev = fh->dev;
-	int                   rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
 
 	if (0 != t->index)
 		return -EINVAL;
@@ -1249,39 +1293,22 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
+	struct v4l2_frequency new_freq = *f;
 	struct em28xx_fh      *fh  = priv;
 	struct em28xx         *dev = fh->dev;
-	int                   rc;
-
-	rc = check_dev(dev);
-	if (rc < 0)
-		return rc;
 
 	if (0 != f->tuner)
 		return -EINVAL;
 
 	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_frequency, f);
-	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_frequency, f);
-	dev->ctl_freq = f->frequency;
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_frequency, &new_freq);
+	dev->ctl_freq = new_freq.frequency;
 
 	return 0;
 }
 
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-static int em28xx_reg_len(int reg)
-{
-	switch (reg) {
-	case EM28XX_R40_AC97LSB:
-	case EM28XX_R30_HSCALELOW:
-	case EM28XX_R32_VSCALELOW:
-		return 2;
-	default:
-		return 1;
-	}
-}
-
 static int vidioc_g_chip_ident(struct file *file, void *priv,
 	       struct v4l2_dbg_chip_ident *chip)
 {
@@ -1290,9 +1317,9 @@ static int vidioc_g_chip_ident(struct file *file, void *priv,
 
 	chip->ident = V4L2_IDENT_NONE;
 	chip->revision = 0;
-	if (chip->match.type == V4L2_CHIP_MATCH_HOST) {
-		if (v4l2_chip_match_host(&chip->match))
-			chip->ident = V4L2_IDENT_NONE;
+	if (chip->match.type == V4L2_CHIP_MATCH_BRIDGE) {
+		if (chip->match.addr > 1)
+			return -EINVAL;
 		return 0;
 	}
 	if (chip->match.type != V4L2_CHIP_MATCH_I2C_DRIVER &&
@@ -1304,6 +1331,33 @@ static int vidioc_g_chip_ident(struct file *file, void *priv,
 	return 0;
 }
 
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int vidioc_g_chip_info(struct file *file, void *priv,
+	       struct v4l2_dbg_chip_info *chip)
+{
+	struct em28xx_fh      *fh  = priv;
+	struct em28xx         *dev = fh->dev;
+
+	if (chip->match.addr > 1)
+		return -EINVAL;
+	if (chip->match.addr == 1)
+		strlcpy(chip->name, "ac97", sizeof(chip->name));
+	else
+		strlcpy(chip->name, dev->v4l2_dev.name, sizeof(chip->name));
+	return 0;
+}
+
+static int em28xx_reg_len(int reg)
+{
+	switch (reg) {
+	case EM28XX_R40_AC97LSB:
+	case EM28XX_R30_HSCALELOW:
+	case EM28XX_R32_VSCALELOW:
+		return 2;
+	default:
+		return 1;
+	}
+}
 
 static int vidioc_g_register(struct file *file, void *priv,
 			     struct v4l2_dbg_register *reg)
@@ -1313,6 +1367,12 @@ static int vidioc_g_register(struct file *file, void *priv,
 	int ret;
 
 	switch (reg->match.type) {
+	case V4L2_CHIP_MATCH_BRIDGE:
+		if (reg->match.addr > 1)
+			return -EINVAL;
+		if (!reg->match.addr)
+			break;
+		/* fall-through */
 	case V4L2_CHIP_MATCH_AC97:
 		ret = em28xx_read_ac97(dev, reg->reg);
 		if (ret < 0)
@@ -1329,8 +1389,7 @@ static int vidioc_g_register(struct file *file, void *priv,
 		v4l2_device_call_all(&dev->v4l2_dev, 0, core, g_register, reg);
 		return 0;
 	default:
-		if (!v4l2_chip_match_host(&reg->match))
-			return -EINVAL;
+		return -EINVAL;
 	}
 
 	/* Match host */
@@ -1356,13 +1415,19 @@ static int vidioc_g_register(struct file *file, void *priv,
 }
 
 static int vidioc_s_register(struct file *file, void *priv,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct em28xx_fh      *fh  = priv;
 	struct em28xx         *dev = fh->dev;
 	__le16 buf;
 
 	switch (reg->match.type) {
+	case V4L2_CHIP_MATCH_BRIDGE:
+		if (reg->match.addr > 1)
+			return -EINVAL;
+		if (!reg->match.addr)
+			break;
+		/* fall-through */
 	case V4L2_CHIP_MATCH_AC97:
 		return em28xx_write_ac97(dev, reg->reg, reg->val);
 	case V4L2_CHIP_MATCH_I2C_DRIVER:
@@ -1373,8 +1438,7 @@ static int vidioc_s_register(struct file *file, void *priv,
 		v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_register, reg);
 		return 0;
 	default:
-		if (!v4l2_chip_match_host(&reg->match))
-			return -EINVAL;
+		return -EINVAL;
 	}
 
 	/* Match host */
@@ -1386,26 +1450,6 @@ static int vidioc_s_register(struct file *file, void *priv,
 #endif
 
 
-static int vidioc_cropcap(struct file *file, void *priv,
-					struct v4l2_cropcap *cc)
-{
-	struct em28xx_fh      *fh  = priv;
-	struct em28xx         *dev = fh->dev;
-
-	if (cc->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	cc->bounds.left = 0;
-	cc->bounds.top = 0;
-	cc->bounds.width = dev->width;
-	cc->bounds.height = dev->height;
-	cc->defrect = cc->bounds;
-	cc->pixelaspect.numerator = 54;	/* 4:3 FIXME: remove magic numbers */
-	cc->pixelaspect.denominator = 59;
-
-	return 0;
-}
-
 static int vidioc_querycap(struct file *file, void  *priv,
 					struct v4l2_capability *cap)
 {
@@ -1482,8 +1526,12 @@ static int vidioc_enum_framesizes(struct file *file, void *priv,
 
 	/* Report a continuous range */
 	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
-	fsize->stepwise.min_width = 48;
-	fsize->stepwise.min_height = 32;
+	scale_to_size(dev, EM28XX_HVSCALE_MAX, EM28XX_HVSCALE_MAX,
+		      &fsize->stepwise.min_width, &fsize->stepwise.min_height);
+	if (fsize->stepwise.min_width < 48)
+		fsize->stepwise.min_width = 48;
+	if (fsize->stepwise.min_height < 38)
+		fsize->stepwise.min_height = 38;
 	fsize->stepwise.max_width = maxw;
 	fsize->stepwise.max_height = maxh;
 	fsize->stepwise.step_width = 1;
@@ -1522,35 +1570,6 @@ static int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
 	return 0;
 }
 
-static int vidioc_s_fmt_vbi_cap(struct file *file, void *priv,
-				struct v4l2_format *format)
-{
-	struct em28xx_fh      *fh  = priv;
-	struct em28xx         *dev = fh->dev;
-
-	format->fmt.vbi.samples_per_line = dev->vbi_width;
-	format->fmt.vbi.sample_format = V4L2_PIX_FMT_GREY;
-	format->fmt.vbi.offset = 0;
-	format->fmt.vbi.flags = 0;
-	format->fmt.vbi.sampling_rate = 6750000 * 4 / 2;
-	format->fmt.vbi.count[0] = dev->vbi_height;
-	format->fmt.vbi.count[1] = dev->vbi_height;
-	memset(format->fmt.vbi.reserved, 0, sizeof(format->fmt.vbi.reserved));
-
-	/* Varies by video standard (NTSC, PAL, etc.) */
-	if (dev->norm & V4L2_STD_525_60) {
-		/* NTSC */
-		format->fmt.vbi.start[0] = 10;
-		format->fmt.vbi.start[1] = 273;
-	} else if (dev->norm & V4L2_STD_625_50) {
-		/* PAL */
-		format->fmt.vbi.start[0] = 6;
-		format->fmt.vbi.start[1] = 318;
-	}
-
-	return 0;
-}
-
 /* ----------------------------------------------------------- */
 /* RADIO ESPECIFIC IOCTLS                                      */
 /* ----------------------------------------------------------- */
@@ -1564,7 +1583,6 @@ static int radio_g_tuner(struct file *file, void *priv,
 		return -EINVAL;
 
 	strcpy(t->name, "Radio");
-	t->type = V4L2_TUNER_RADIO;
 
 	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_tuner, t);
 
@@ -1572,7 +1590,7 @@ static int radio_g_tuner(struct file *file, void *priv,
 }
 
 static int radio_s_tuner(struct file *file, void *priv,
-			 struct v4l2_tuner *t)
+			 const struct v4l2_tuner *t)
 {
 	struct em28xx *dev = ((struct em28xx_fh *)priv)->dev;
 
@@ -1749,11 +1767,10 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_s_fmt_vid_cap       = vidioc_s_fmt_vid_cap,
 	.vidioc_g_fmt_vbi_cap       = vidioc_g_fmt_vbi_cap,
 	.vidioc_try_fmt_vbi_cap     = vidioc_g_fmt_vbi_cap,
-	.vidioc_s_fmt_vbi_cap       = vidioc_s_fmt_vbi_cap,
+	.vidioc_s_fmt_vbi_cap       = vidioc_g_fmt_vbi_cap,
 	.vidioc_enum_framesizes     = vidioc_enum_framesizes,
 	.vidioc_g_audio             = vidioc_g_audio,
 	.vidioc_s_audio             = vidioc_s_audio,
-	.vidioc_cropcap             = vidioc_cropcap,
 
 	.vidioc_reqbufs             = vb2_ioctl_reqbufs,
 	.vidioc_create_bufs         = vb2_ioctl_create_bufs,
@@ -1778,10 +1795,11 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_s_frequency         = vidioc_s_frequency,
 	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+	.vidioc_g_chip_ident        = vidioc_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_chip_info         = vidioc_g_chip_info,
 	.vidioc_g_register          = vidioc_g_register,
 	.vidioc_s_register          = vidioc_s_register,
-	.vidioc_g_chip_ident        = vidioc_g_chip_ident,
 #endif
 };
 
@@ -1808,7 +1826,9 @@ static const struct v4l2_ioctl_ops radio_ioctl_ops = {
 	.vidioc_s_frequency   = vidioc_s_frequency,
 	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+	.vidioc_g_chip_ident  = vidioc_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_chip_info   = vidioc_g_chip_info,
 	.vidioc_g_register    = vidioc_g_register,
 	.vidioc_s_register    = vidioc_s_register,
 #endif
@@ -1887,9 +1907,42 @@ int em28xx_register_analog_devices(struct em28xx *dev)
 			 (EM28XX_XCLK_AUDIO_UNMUTE | val));
 
 	em28xx_set_outfmt(dev);
-	em28xx_colorlevels_set_default(dev);
 	em28xx_compression_disable(dev);
 
+	/* Add image controls */
+	/* NOTE: at this point, the subdevices are already registered, so bridge
+	 * controls are only added/enabled when no subdevice provides them */
+	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_CONTRAST))
+		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+				  V4L2_CID_CONTRAST,
+				  0, 0x1f, 1, CONTRAST_DEFAULT);
+	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_BRIGHTNESS))
+		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+				  V4L2_CID_BRIGHTNESS,
+				  -0x80, 0x7f, 1, BRIGHTNESS_DEFAULT);
+	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_SATURATION))
+		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+				  V4L2_CID_SATURATION,
+				  0, 0x1f, 1, SATURATION_DEFAULT);
+	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_BLUE_BALANCE))
+		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+				  V4L2_CID_BLUE_BALANCE,
+				  -0x30, 0x30, 1, BLUE_BALANCE_DEFAULT);
+	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_RED_BALANCE))
+		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+				  V4L2_CID_RED_BALANCE,
+				  -0x30, 0x30, 1, RED_BALANCE_DEFAULT);
+	if (NULL == v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_SHARPNESS))
+		v4l2_ctrl_new_std(&dev->ctrl_handler, &em28xx_ctrl_ops,
+				  V4L2_CID_SHARPNESS,
+				  0, 0x0f, 1, SHARPNESS_DEFAULT);
+
+	/* Reset image controls */
+	em28xx_colorlevels_set_default(dev);
+	v4l2_ctrl_handler_setup(&dev->ctrl_handler);
+	if (dev->ctrl_handler.error)
+		return dev->ctrl_handler.error;
+
 	/* allocate and fill video video_device struct */
 	dev->vdev = em28xx_vdev_init(dev, &em28xx_video_template, "video");
 	if (!dev->vdev) {
@@ -1899,6 +1952,25 @@ int em28xx_register_analog_devices(struct em28xx *dev)
 	dev->vdev->queue = &dev->vb_vidq;
 	dev->vdev->queue->lock = &dev->vb_queue_lock;
 
+	/* disable inapplicable ioctls */
+	if (dev->board.is_webcam) {
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_QUERYSTD);
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_G_STD);
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_STD);
+	} else {
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_PARM);
+	}
+	if (dev->tuner_type == TUNER_ABSENT) {
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_G_TUNER);
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_TUNER);
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_G_FREQUENCY);
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_FREQUENCY);
+	}
+	if (!dev->audio_mode.has_audio) {
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_G_AUDIO);
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_AUDIO);
+	}
+
 	/* register v4l2 video video_device */
 	ret = video_register_device(dev->vdev, VFL_TYPE_GRABBER,
 				       video_nr[dev->devno]);
@@ -1916,6 +1988,19 @@ int em28xx_register_analog_devices(struct em28xx *dev)
 		dev->vbi_dev->queue = &dev->vb_vbiq;
 		dev->vbi_dev->queue->lock = &dev->vb_vbi_queue_lock;
 
+		/* disable inapplicable ioctls */
+		v4l2_disable_ioctl(dev->vdev, VIDIOC_S_PARM);
+		if (dev->tuner_type == TUNER_ABSENT) {
+			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_G_TUNER);
+			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_S_TUNER);
+			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_G_FREQUENCY);
+			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_S_FREQUENCY);
+		}
+		if (!dev->audio_mode.has_audio) {
+			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_G_AUDIO);
+			v4l2_disable_ioctl(dev->vbi_dev, VIDIOC_S_AUDIO);
+		}
+
 		/* register v4l2 vbi video_device */
 		ret = video_register_device(dev->vbi_dev, VFL_TYPE_VBI,
 					    vbi_nr[dev->devno]);
diff --git a/drivers/media/usb/em28xx/em28xx.h b/drivers/media/usb/em28xx/em28xx.h
index 5f0b2c59e846..a9323b63d8e5 100644
--- a/drivers/media/usb/em28xx/em28xx.h
+++ b/drivers/media/usb/em28xx/em28xx.h
@@ -42,28 +42,28 @@
 #include "em28xx-reg.h"
 
 /* Boards supported by driver */
-#define EM2800_BOARD_UNKNOWN			0
-#define EM2820_BOARD_UNKNOWN			1
-#define EM2820_BOARD_TERRATEC_CINERGY_250	2
-#define EM2820_BOARD_PINNACLE_USB_2		3
-#define EM2820_BOARD_HAUPPAUGE_WINTV_USB_2      4
-#define EM2820_BOARD_MSI_VOX_USB_2              5
-#define EM2800_BOARD_TERRATEC_CINERGY_200       6
-#define EM2800_BOARD_LEADTEK_WINFAST_USBII      7
-#define EM2800_BOARD_KWORLD_USB2800             8
-#define EM2820_BOARD_PINNACLE_DVC_90		9
-#define EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900	10
-#define EM2880_BOARD_TERRATEC_HYBRID_XS		11
-#define EM2820_BOARD_KWORLD_PVRTV2800RF		12
-#define EM2880_BOARD_TERRATEC_PRODIGY_XS	13
-#define EM2820_BOARD_PROLINK_PLAYTV_USB2	14
-#define EM2800_BOARD_VGEAR_POCKETTV             15
-#define EM2883_BOARD_HAUPPAUGE_WINTV_HVR_950	16
-#define EM2880_BOARD_PINNACLE_PCTV_HD_PRO	17
-#define EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900_R2	18
-#define EM2860_BOARD_SAA711X_REFERENCE_DESIGN	19
-#define EM2880_BOARD_AMD_ATI_TV_WONDER_HD_600   20
-#define EM2800_BOARD_GRABBEEX_USB2800           21
+#define EM2800_BOARD_UNKNOWN			  0
+#define EM2820_BOARD_UNKNOWN			  1
+#define EM2820_BOARD_TERRATEC_CINERGY_250	  2
+#define EM2820_BOARD_PINNACLE_USB_2		  3
+#define EM2820_BOARD_HAUPPAUGE_WINTV_USB_2	  4
+#define EM2820_BOARD_MSI_VOX_USB_2		  5
+#define EM2800_BOARD_TERRATEC_CINERGY_200	  6
+#define EM2800_BOARD_LEADTEK_WINFAST_USBII	  7
+#define EM2800_BOARD_KWORLD_USB2800		  8
+#define EM2820_BOARD_PINNACLE_DVC_90		  9
+#define EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900	  10
+#define EM2880_BOARD_TERRATEC_HYBRID_XS		  11
+#define EM2820_BOARD_KWORLD_PVRTV2800RF		  12
+#define EM2880_BOARD_TERRATEC_PRODIGY_XS	  13
+#define EM2820_BOARD_PROLINK_PLAYTV_USB2	  14
+#define EM2800_BOARD_VGEAR_POCKETTV		  15
+#define EM2883_BOARD_HAUPPAUGE_WINTV_HVR_950	  16
+#define EM2880_BOARD_PINNACLE_PCTV_HD_PRO	  17
+#define EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900_R2	  18
+#define EM2860_BOARD_SAA711X_REFERENCE_DESIGN	  19
+#define EM2880_BOARD_AMD_ATI_TV_WONDER_HD_600	  20
+#define EM2800_BOARD_GRABBEEX_USB2800		  21
 #define EM2750_BOARD_UNKNOWN			  22
 #define EM2750_BOARD_DLCW_130			  23
 #define EM2820_BOARD_DLINK_USB_TV		  24
@@ -99,36 +99,37 @@
 #define EM2882_BOARD_KWORLD_VS_DVBT		  54
 #define EM2882_BOARD_TERRATEC_HYBRID_XS		  55
 #define EM2882_BOARD_PINNACLE_HYBRID_PRO_330E	  56
-#define EM2883_BOARD_KWORLD_HYBRID_330U                  57
+#define EM2883_BOARD_KWORLD_HYBRID_330U		  57
 #define EM2820_BOARD_COMPRO_VIDEOMATE_FORYOU	  58
 #define EM2883_BOARD_HAUPPAUGE_WINTV_HVR_850	  60
 #define EM2820_BOARD_PROLINK_PLAYTV_BOX4_USB2	  61
 #define EM2820_BOARD_GADMEI_TVR200		  62
-#define EM2860_BOARD_KAIOMY_TVNPC_U2              63
-#define EM2860_BOARD_EASYCAP                      64
+#define EM2860_BOARD_KAIOMY_TVNPC_U2		  63
+#define EM2860_BOARD_EASYCAP			  64
 #define EM2820_BOARD_IODATA_GVMVP_SZ		  65
 #define EM2880_BOARD_EMPIRE_DUAL_TV		  66
 #define EM2860_BOARD_TERRATEC_GRABBY		  67
 #define EM2860_BOARD_TERRATEC_AV350		  68
 #define EM2882_BOARD_KWORLD_ATSC_315U		  69
 #define EM2882_BOARD_EVGA_INDTUBE		  70
-#define EM2820_BOARD_SILVERCREST_WEBCAM           71
-#define EM2861_BOARD_GADMEI_UTV330PLUS           72
-#define EM2870_BOARD_REDDO_DVB_C_USB_BOX          73
+#define EM2820_BOARD_SILVERCREST_WEBCAM		  71
+#define EM2861_BOARD_GADMEI_UTV330PLUS		  72
+#define EM2870_BOARD_REDDO_DVB_C_USB_BOX	  73
 #define EM2800_BOARD_VC211A			  74
 #define EM2882_BOARD_DIKOM_DK300		  75
 #define EM2870_BOARD_KWORLD_A340		  76
 #define EM2874_BOARD_LEADERSHIP_ISDBT		  77
-#define EM28174_BOARD_PCTV_290E                   78
+#define EM28174_BOARD_PCTV_290E			  78
 #define EM2884_BOARD_TERRATEC_H5		  79
-#define EM28174_BOARD_PCTV_460E                   80
+#define EM28174_BOARD_PCTV_460E			  80
 #define EM2884_BOARD_HAUPPAUGE_WINTV_HVR_930C	  81
 #define EM2884_BOARD_CINERGY_HTC_STICK		  82
-#define EM2860_BOARD_HT_VIDBOX_NW03 		  83
-#define EM2874_BOARD_MAXMEDIA_UB425_TC            84
-#define EM2884_BOARD_PCTV_510E                    85
-#define EM2884_BOARD_PCTV_520E                    86
+#define EM2860_BOARD_HT_VIDBOX_NW03		  83
+#define EM2874_BOARD_MAXMEDIA_UB425_TC		  84
+#define EM2884_BOARD_PCTV_510E			  85
+#define EM2884_BOARD_PCTV_520E			  86
 #define EM2884_BOARD_TERRATEC_HTC_USB_XS	  87
+#define EM2884_BOARD_C3TECH_DIGITAL_DUO		  88
 
 /* Limits minimum and default number of buffers */
 #define EM28XX_MIN_BUF 4
@@ -157,6 +158,9 @@
 #define EM28XX_NUM_BUFS 5
 #define EM28XX_DVB_NUM_BUFS 5
 
+/* max number of I2C buses on em28xx devices */
+#define NUM_I2C_BUSES	2
+
 /* isoc transfers: number of packets for each buffer
    windows requests only 64 packets .. so we better do the same
    this is what I found out for all alternate numbers there!
@@ -172,27 +176,6 @@
 
 #define EM28XX_INTERLACED_DEFAULT 1
 
-/*
-#define (use usbview if you want to get the other alternate number infos)
-#define
-#define alternate number 2
-#define 			Endpoint Address: 82
-			Direction: in
-			Attribute: 1
-			Type: Isoc
-			Max Packet Size: 1448
-			Interval: 125us
-
-  alternate number 7
-
-			Endpoint Address: 82
-			Direction: in
-			Attribute: 1
-			Type: Isoc
-			Max Packet Size: 3072
-			Interval: 125us
-*/
-
 /* time in msecs to wait for i2c writes to finish */
 #define EM2800_I2C_XFER_TIMEOUT		20
 
@@ -381,6 +364,7 @@ enum em28xx_sensor {
 	EM28XX_MT9V011,
 	EM28XX_MT9M001,
 	EM28XX_MT9M111,
+	EM28XX_OV2640,
 };
 
 enum em28xx_adecoder {
@@ -393,6 +377,7 @@ struct em28xx_board {
 	int vchannels;
 	int tuner_type;
 	int tuner_addr;
+	unsigned def_i2c_bus;	/* Default I2C bus */
 
 	/* i2c flags */
 	unsigned int tda9887_conf;
@@ -426,15 +411,15 @@ struct em28xx_board {
 };
 
 struct em28xx_eeprom {
-	u32 id;			/* 0x9567eb1a */
-	u16 vendor_ID;
-	u16 product_ID;
+	u8 id[4];			/* 1a eb 67 95 */
+	__le16 vendor_ID;
+	__le16 product_ID;
 
-	u16 chip_conf;
+	__le16 chip_conf;
 
-	u16 board_conf;
+	__le16 board_conf;
 
-	u16 string1, string2, string3;
+	__le16 string1, string2, string3;
 
 	u8 string_idx_table;
 };
@@ -477,6 +462,20 @@ struct em28xx_fh {
 	enum v4l2_buf_type           type;
 };
 
+enum em28xx_i2c_algo_type {
+	EM28XX_I2C_ALGO_EM28XX = 0,
+	EM28XX_I2C_ALGO_EM2800,
+	EM28XX_I2C_ALGO_EM25XX_BUS_B,
+};
+
+struct em28xx_i2c_bus {
+	struct em28xx *dev;
+
+	unsigned bus;
+	enum em28xx_i2c_algo_type algo_type;
+};
+
+
 /* main device struct */
 struct em28xx {
 	/* generic device properties */
@@ -484,6 +483,7 @@ struct em28xx {
 	int model;		/* index in the device_data struct */
 	int devno;		/* marks the number of this device */
 	enum em28xx_chip_id chip_id;
+	unsigned int is_em25xx:1;	/* em25xx/em276x/7x/8x family bridge */
 
 	unsigned char disconnected:1;	/* device has been diconnected */
 
@@ -491,8 +491,6 @@ struct em28xx {
 
 	struct v4l2_device v4l2_dev;
 	struct v4l2_ctrl_handler ctrl_handler;
-	/* provides ac97 mute and volume overrides */
-	struct v4l2_ctrl_handler ac97_ctrl_handler;
 	struct em28xx_board board;
 
 	/* Webcam specific fields */
@@ -511,8 +509,8 @@ struct em28xx {
 	unsigned int is_audio_only:1;
 
 	/* Controls audio streaming */
-	struct work_struct wq_trigger;              /* Trigger to start/stop audio for alsa module */
-	 atomic_t       stream_started;      /* stream should be running if true */
+	struct work_struct wq_trigger;	/* Trigger to start/stop audio for alsa module */
+	atomic_t       stream_started;	/* stream should be running if true */
 
 	struct em28xx_fmt *format;
 
@@ -530,9 +528,17 @@ struct em28xx {
 	int tuner_type;		/* type of the tuner */
 	int tuner_addr;		/* tuner address */
 	int tda9887_conf;
+
 	/* i2c i/o */
-	struct i2c_adapter i2c_adap;
-	struct i2c_client i2c_client;
+	struct i2c_adapter i2c_adap[NUM_I2C_BUSES];
+	struct i2c_client i2c_client[NUM_I2C_BUSES];
+	struct em28xx_i2c_bus i2c_bus[NUM_I2C_BUSES];
+
+	unsigned char eeprom_addrwidth_16bit:1;
+	unsigned def_i2c_bus;	/* Default I2C bus */
+	unsigned cur_i2c_bus;	/* Current I2C bus */
+	struct rt_mutex i2c_bus_lock;
+
 	/* video for linux */
 	int users;		/* user count for exclusive use */
 	int streaming_users;    /* Number of actively streaming users */
@@ -584,7 +590,9 @@ struct em28xx {
 	/* resources in use */
 	unsigned int resources;
 
-	unsigned char eedata[256];
+	/* eeprom content */
+	u8 *eedata;
+	u16 eedata_len;
 
 	/* Isoc control struct */
 	struct em28xx_dmaqueue vidq;
@@ -600,7 +608,7 @@ struct em28xx {
 	u8 analog_ep_isoc;	/* address of isoc endpoint for analog */
 	u8 analog_ep_bulk;	/* address of bulk endpoint for analog */
 	u8 dvb_ep_isoc;		/* address of isoc endpoint for DVB */
-	u8 dvb_ep_bulk;		/* address of bulk endpoint for DVC */
+	u8 dvb_ep_bulk;		/* address of bulk endpoint for DVB */
 	int alt;		/* alternate setting */
 	int max_pkt_size;	/* max packet size of the selected ep at alt */
 	int packet_multiplier;	/* multiplier for wMaxPacketSize, used for
@@ -651,16 +659,12 @@ struct em28xx_ops {
 };
 
 /* Provided by em28xx-i2c.c */
-void em28xx_do_i2c_scan(struct em28xx *dev);
-int  em28xx_i2c_register(struct em28xx *dev);
-int  em28xx_i2c_unregister(struct em28xx *dev);
+void em28xx_do_i2c_scan(struct em28xx *dev, unsigned bus);
+int  em28xx_i2c_register(struct em28xx *dev, unsigned bus,
+			 enum em28xx_i2c_algo_type algo_type);
+int  em28xx_i2c_unregister(struct em28xx *dev, unsigned bus);
 
 /* Provided by em28xx-core.c */
-
-u32 em28xx_request_buffers(struct em28xx *dev, u32 count);
-void em28xx_queue_unusedframes(struct em28xx *dev);
-void em28xx_release_buffers(struct em28xx *dev);
-
 int em28xx_read_reg_req_len(struct em28xx *dev, u8 req, u16 reg,
 			    char *buf, int len);
 int em28xx_read_reg_req(struct em28xx *dev, u8 req, u16 reg);
@@ -693,7 +697,6 @@ int em28xx_init_usb_xfer(struct em28xx *dev, enum em28xx_mode mode,
 					(struct em28xx *dev, struct urb *urb));
 void em28xx_uninit_usb_xfer(struct em28xx *dev, enum em28xx_mode mode);
 void em28xx_stop_urbs(struct em28xx *dev);
-int em28xx_isoc_dvb_max_packetsize(struct em28xx *dev);
 int em28xx_set_mode(struct em28xx *dev, enum em28xx_mode set_mode);
 int em28xx_gpio_set(struct em28xx *dev, struct em28xx_reg_seq *gpio);
 void em28xx_wake_i2c(struct em28xx *dev);
@@ -712,16 +715,18 @@ int em28xx_stop_vbi_streaming(struct vb2_queue *vq);
 extern const struct v4l2_ctrl_ops em28xx_ctrl_ops;
 
 /* Provided by em28xx-cards.c */
-extern int em2800_variant_detect(struct usb_device *udev, int model);
 extern struct em28xx_board em28xx_boards[];
 extern struct usb_device_id em28xx_id_table[];
-extern const unsigned int em28xx_bcount;
 int em28xx_tuner_callback(void *ptr, int component, int command, int arg);
 void em28xx_release_resources(struct em28xx *dev);
 
 /* Provided by em28xx-vbi.c */
 extern struct vb2_ops em28xx_vbi_qops;
 
+/* Provided by em28xx-camera.c */
+int em28xx_detect_sensor(struct em28xx *dev);
+int em28xx_init_camera(struct em28xx *dev);
+
 /* printk macros */
 
 #define em28xx_err(fmt, arg...) do {\
@@ -744,72 +749,6 @@ static inline int em28xx_compression_disable(struct em28xx *dev)
 	return em28xx_write_reg(dev, EM28XX_R26_COMPR, 0x00);
 }
 
-static inline int em28xx_contrast_get(struct em28xx *dev)
-{
-	return em28xx_read_reg(dev, EM28XX_R20_YGAIN) & 0x1f;
-}
-
-static inline int em28xx_brightness_get(struct em28xx *dev)
-{
-	return em28xx_read_reg(dev, EM28XX_R21_YOFFSET);
-}
-
-static inline int em28xx_saturation_get(struct em28xx *dev)
-{
-	return em28xx_read_reg(dev, EM28XX_R22_UVGAIN) & 0x1f;
-}
-
-static inline int em28xx_u_balance_get(struct em28xx *dev)
-{
-	return em28xx_read_reg(dev, EM28XX_R23_UOFFSET);
-}
-
-static inline int em28xx_v_balance_get(struct em28xx *dev)
-{
-	return em28xx_read_reg(dev, EM28XX_R24_VOFFSET);
-}
-
-static inline int em28xx_gamma_get(struct em28xx *dev)
-{
-	return em28xx_read_reg(dev, EM28XX_R14_GAMMA) & 0x3f;
-}
-
-static inline int em28xx_contrast_set(struct em28xx *dev, s32 val)
-{
-	u8 tmp = (u8) val;
-	return em28xx_write_regs(dev, EM28XX_R20_YGAIN, &tmp, 1);
-}
-
-static inline int em28xx_brightness_set(struct em28xx *dev, s32 val)
-{
-	u8 tmp = (u8) val;
-	return em28xx_write_regs(dev, EM28XX_R21_YOFFSET, &tmp, 1);
-}
-
-static inline int em28xx_saturation_set(struct em28xx *dev, s32 val)
-{
-	u8 tmp = (u8) val;
-	return em28xx_write_regs(dev, EM28XX_R22_UVGAIN, &tmp, 1);
-}
-
-static inline int em28xx_u_balance_set(struct em28xx *dev, s32 val)
-{
-	u8 tmp = (u8) val;
-	return em28xx_write_regs(dev, EM28XX_R23_UOFFSET, &tmp, 1);
-}
-
-static inline int em28xx_v_balance_set(struct em28xx *dev, s32 val)
-{
-	u8 tmp = (u8) val;
-	return em28xx_write_regs(dev, EM28XX_R24_VOFFSET, &tmp, 1);
-}
-
-static inline int em28xx_gamma_set(struct em28xx *dev, s32 val)
-{
-	u8 tmp = (u8) val;
-	return em28xx_write_regs(dev, EM28XX_R14_GAMMA, &tmp, 1);
-}
-
 /*FIXME: maxw should be dependent of alt mode */
 static inline unsigned int norm_maxw(struct em28xx *dev)
 {
diff --git a/drivers/media/usb/gspca/autogain_functions.h b/drivers/media/usb/gspca/autogain_functions.h
deleted file mode 100644
index d625eafe63eb..000000000000
--- a/drivers/media/usb/gspca/autogain_functions.h
+++ /dev/null
@@ -1,183 +0,0 @@
-/*
- * Functions for auto gain.
- *
- * Copyright (C) 2010-2011 Hans de Goede <hdegoede@redhat.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
- */
-
-#ifdef WANT_REGULAR_AUTOGAIN
-/* auto gain and exposure algorithm based on the knee algorithm described here:
-   http://ytse.tricolour.net/docs/LowLightOptimization.html
-
-   Returns 0 if no changes were made, 1 if the gain and or exposure settings
-   where changed. */
-static inline int auto_gain_n_exposure(
-			struct gspca_dev *gspca_dev,
-			int avg_lum,
-			int desired_avg_lum,
-			int deadzone,
-			int gain_knee,
-			int exposure_knee)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	int i, steps, gain, orig_gain, exposure, orig_exposure;
-	int retval = 0;
-
-	orig_gain = gain = sd->ctrls[GAIN].val;
-	orig_exposure = exposure = sd->ctrls[EXPOSURE].val;
-
-	/* If we are of a multiple of deadzone, do multiple steps to reach the
-	   desired lumination fast (with the risc of a slight overshoot) */
-	steps = abs(desired_avg_lum - avg_lum) / deadzone;
-
-	PDEBUG(D_FRAM, "autogain: lum: %d, desired: %d, steps: %d",
-		avg_lum, desired_avg_lum, steps);
-
-	for (i = 0; i < steps; i++) {
-		if (avg_lum > desired_avg_lum) {
-			if (gain > gain_knee)
-				gain--;
-			else if (exposure > exposure_knee)
-				exposure--;
-			else if (gain > sd->ctrls[GAIN].def)
-				gain--;
-			else if (exposure > sd->ctrls[EXPOSURE].min)
-				exposure--;
-			else if (gain > sd->ctrls[GAIN].min)
-				gain--;
-			else
-				break;
-		} else {
-			if (gain < sd->ctrls[GAIN].def)
-				gain++;
-			else if (exposure < exposure_knee)
-				exposure++;
-			else if (gain < gain_knee)
-				gain++;
-			else if (exposure < sd->ctrls[EXPOSURE].max)
-				exposure++;
-			else if (gain < sd->ctrls[GAIN].max)
-				gain++;
-			else
-				break;
-		}
-	}
-
-	if (gain != orig_gain) {
-		sd->ctrls[GAIN].val = gain;
-		setgain(gspca_dev);
-		retval = 1;
-	}
-	if (exposure != orig_exposure) {
-		sd->ctrls[EXPOSURE].val = exposure;
-		setexposure(gspca_dev);
-		retval = 1;
-	}
-
-	if (retval)
-		PDEBUG(D_FRAM, "autogain: changed gain: %d, expo: %d",
-			gain, exposure);
-	return retval;
-}
-#endif
-
-#ifdef WANT_COARSE_EXPO_AUTOGAIN
-/* Autogain + exposure algorithm for cameras with a coarse exposure control
-   (usually this means we can only control the clockdiv to change exposure)
-   As changing the clockdiv so that the fps drops from 30 to 15 fps for
-   example, will lead to a huge exposure change (it effectively doubles),
-   this algorithm normally tries to only adjust the gain (between 40 and
-   80 %) and if that does not help, only then changes exposure. This leads
-   to a much more stable image then using the knee algorithm which at
-   certain points of the knee graph will only try to adjust exposure,
-   which leads to oscilating as one exposure step is huge.
-
-   Note this assumes that the sd struct for the cam in question has
-   exp_too_low_cnt and exp_too_high_cnt int members for use by this function.
-
-   Returns 0 if no changes were made, 1 if the gain and or exposure settings
-   where changed. */
-static inline int coarse_grained_expo_autogain(
-			struct gspca_dev *gspca_dev,
-			int avg_lum,
-			int desired_avg_lum,
-			int deadzone)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	int steps, gain, orig_gain, exposure, orig_exposure;
-	int gain_low, gain_high;
-	int retval = 0;
-
-	orig_gain = gain = sd->ctrls[GAIN].val;
-	orig_exposure = exposure = sd->ctrls[EXPOSURE].val;
-
-	gain_low = (sd->ctrls[GAIN].max - sd->ctrls[GAIN].min) / 5 * 2;
-	gain_low += sd->ctrls[GAIN].min;
-	gain_high = (sd->ctrls[GAIN].max - sd->ctrls[GAIN].min) / 5 * 4;
-	gain_high += sd->ctrls[GAIN].min;
-
-	/* If we are of a multiple of deadzone, do multiple steps to reach the
-	   desired lumination fast (with the risc of a slight overshoot) */
-	steps = (desired_avg_lum - avg_lum) / deadzone;
-
-	PDEBUG(D_FRAM, "autogain: lum: %d, desired: %d, steps: %d",
-		avg_lum, desired_avg_lum, steps);
-
-	if ((gain + steps) > gain_high &&
-	    exposure < sd->ctrls[EXPOSURE].max) {
-		gain = gain_high;
-		sd->exp_too_low_cnt++;
-		sd->exp_too_high_cnt = 0;
-	} else if ((gain + steps) < gain_low &&
-		   exposure > sd->ctrls[EXPOSURE].min) {
-		gain = gain_low;
-		sd->exp_too_high_cnt++;
-		sd->exp_too_low_cnt = 0;
-	} else {
-		gain += steps;
-		if (gain > sd->ctrls[GAIN].max)
-			gain = sd->ctrls[GAIN].max;
-		else if (gain < sd->ctrls[GAIN].min)
-			gain = sd->ctrls[GAIN].min;
-		sd->exp_too_high_cnt = 0;
-		sd->exp_too_low_cnt = 0;
-	}
-
-	if (sd->exp_too_high_cnt > 3) {
-		exposure--;
-		sd->exp_too_high_cnt = 0;
-	} else if (sd->exp_too_low_cnt > 3) {
-		exposure++;
-		sd->exp_too_low_cnt = 0;
-	}
-
-	if (gain != orig_gain) {
-		sd->ctrls[GAIN].val = gain;
-		setgain(gspca_dev);
-		retval = 1;
-	}
-	if (exposure != orig_exposure) {
-		sd->ctrls[EXPOSURE].val = exposure;
-		setexposure(gspca_dev);
-		retval = 1;
-	}
-
-	if (retval)
-		PDEBUG(D_FRAM, "autogain: changed gain: %d, expo: %d",
-			gain, exposure);
-	return retval;
-}
-#endif
diff --git a/drivers/media/usb/gspca/benq.c b/drivers/media/usb/gspca/benq.c
index 352f32190e68..05f406deae13 100644
--- a/drivers/media/usb/gspca/benq.c
+++ b/drivers/media/usb/gspca/benq.c
@@ -186,7 +186,7 @@ static void sd_isoc_irq(struct urb *urb)
 		/* check the packet status and length */
 		if (urb0->iso_frame_desc[i].actual_length != SD_PKT_SZ
 		    || urb->iso_frame_desc[i].actual_length != SD_PKT_SZ) {
-			PDEBUG(D_ERR, "ISOC bad lengths %d / %d",
+			PERR("ISOC bad lengths %d / %d",
 				urb0->iso_frame_desc[i].actual_length,
 				urb->iso_frame_desc[i].actual_length);
 			gspca_dev->last_packet_type = DISCARD_PACKET;
diff --git a/drivers/media/usb/gspca/conex.c b/drivers/media/usb/gspca/conex.c
index c9052f20435e..38714df31ac4 100644
--- a/drivers/media/usb/gspca/conex.c
+++ b/drivers/media/usb/gspca/conex.c
@@ -73,12 +73,11 @@ static void reg_r(struct gspca_dev *gspca_dev,
 {
 	struct usb_device *dev = gspca_dev->dev;
 
-#ifdef GSPCA_DEBUG
 	if (len > USB_BUF_SZ) {
-		pr_err("reg_r: buffer overflow\n");
+		PERR("reg_r: buffer overflow\n");
 		return;
 	}
-#endif
+
 	usb_control_msg(dev,
 			usb_rcvctrlpipe(dev, 0),
 			0,
@@ -113,13 +112,12 @@ static void reg_w(struct gspca_dev *gspca_dev,
 {
 	struct usb_device *dev = gspca_dev->dev;
 
-#ifdef GSPCA_DEBUG
 	if (len > USB_BUF_SZ) {
-		pr_err("reg_w: buffer overflow\n");
+		PERR("reg_w: buffer overflow\n");
 		return;
 	}
 	PDEBUG(D_USBO, "reg write [%02x] = %02x..", index, *buffer);
-#endif
+
 	memcpy(gspca_dev->usb_buf, buffer, len);
 	usb_control_msg(dev,
 			usb_sndctrlpipe(dev, 0),
@@ -689,7 +687,7 @@ static void cx11646_jpeg(struct gspca_dev*gspca_dev)
 		reg_w_val(gspca_dev, 0x0053, 0x00);
 	} while (--retry);
 	if (retry == 0)
-		PDEBUG(D_ERR, "Damned Errors sending jpeg Table");
+		PERR("Damned Errors sending jpeg Table");
 	/* send the qtable now */
 	reg_r(gspca_dev, 0x0001, 1);		/* -> 0x18 */
 	length = 8;
diff --git a/drivers/media/usb/gspca/cpia1.c b/drivers/media/usb/gspca/cpia1.c
index 1dcdd9f95f1c..064b53043b15 100644
--- a/drivers/media/usb/gspca/cpia1.c
+++ b/drivers/media/usb/gspca/cpia1.c
@@ -421,8 +421,7 @@ static int cpia_usb_transferCmd(struct gspca_dev *gspca_dev, u8 *command)
 		pipe = usb_sndctrlpipe(gspca_dev->dev, 0);
 		requesttype = USB_TYPE_VENDOR | USB_RECIP_DEVICE;
 	} else {
-		PDEBUG(D_ERR, "Unexpected first byte of command: %x",
-		       command[0]);
+		PERR("Unexpected first byte of command: %x", command[0]);
 		return -EINVAL;
 	}
 
@@ -701,7 +700,7 @@ static void reset_camera_params(struct gspca_dev *gspca_dev)
 	params->qx3.cradled = 0;
 }
 
-static void printstatus(struct cam_params *params)
+static void printstatus(struct gspca_dev *gspca_dev, struct cam_params *params)
 {
 	PDEBUG(D_PROBE, "status: %02x %02x %02x %02x %02x %02x %02x %02x",
 	       params->status.systemState, params->status.grabState,
@@ -725,10 +724,9 @@ static int goto_low_power(struct gspca_dev *gspca_dev)
 
 	if (sd->params.status.systemState != LO_POWER_STATE) {
 		if (sd->params.status.systemState != WARM_BOOT_STATE) {
-			PDEBUG(D_ERR,
-			       "unexpected state after lo power cmd: %02x",
-			       sd->params.status.systemState);
-			printstatus(&sd->params);
+			PERR("unexpected state after lo power cmd: %02x",
+			     sd->params.status.systemState);
+			printstatus(gspca_dev, &sd->params);
 		}
 		return -EIO;
 	}
@@ -756,9 +754,9 @@ static int goto_high_power(struct gspca_dev *gspca_dev)
 		return ret;
 
 	if (sd->params.status.systemState != HI_POWER_STATE) {
-		PDEBUG(D_ERR, "unexpected state after hi power cmd: %02x",
-			       sd->params.status.systemState);
-		printstatus(&sd->params);
+		PERR("unexpected state after hi power cmd: %02x",
+		     sd->params.status.systemState);
+		printstatus(gspca_dev, &sd->params);
 		return -EIO;
 	}
 
@@ -1449,8 +1447,8 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	sd->params.version.firmwareVersion = 0;
 	get_version_information(gspca_dev);
 	if (sd->params.version.firmwareVersion != 1) {
-		PDEBUG(D_ERR, "only firmware version 1 is supported (got: %d)",
-		       sd->params.version.firmwareVersion);
+		PERR("only firmware version 1 is supported (got: %d)",
+		     sd->params.version.firmwareVersion);
 		return -ENODEV;
 	}
 
@@ -1475,9 +1473,9 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	/* Start the camera in low power mode */
 	if (goto_low_power(gspca_dev)) {
 		if (sd->params.status.systemState != WARM_BOOT_STATE) {
-			PDEBUG(D_ERR, "unexpected systemstate: %02x",
-			       sd->params.status.systemState);
-			printstatus(&sd->params);
+			PERR("unexpected systemstate: %02x",
+			     sd->params.status.systemState);
+			printstatus(gspca_dev, &sd->params);
 			return -ENODEV;
 		}
 
@@ -1523,9 +1521,8 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		return ret;
 
 	if (sd->params.status.fatalError) {
-		PDEBUG(D_ERR, "fatal_error: %04x, vp_status: %04x",
-		       sd->params.status.fatalError,
-		       sd->params.status.vpStatus);
+		PERR("fatal_error: %04x, vp_status: %04x",
+		     sd->params.status.fatalError, sd->params.status.vpStatus);
 		return -EIO;
 	}
 
diff --git a/drivers/media/usb/gspca/etoms.c b/drivers/media/usb/gspca/etoms.c
index 38f68e11c3a2..26c9ee1f1045 100644
--- a/drivers/media/usb/gspca/etoms.c
+++ b/drivers/media/usb/gspca/etoms.c
@@ -163,12 +163,11 @@ static void reg_r(struct gspca_dev *gspca_dev,
 {
 	struct usb_device *dev = gspca_dev->dev;
 
-#ifdef GSPCA_DEBUG
 	if (len > USB_BUF_SZ) {
-		pr_err("reg_r: buffer overflow\n");
+		PERR("reg_r: buffer overflow\n");
 		return;
 	}
-#endif
+
 	usb_control_msg(dev,
 			usb_rcvctrlpipe(dev, 0),
 			0,
@@ -201,13 +200,12 @@ static void reg_w(struct gspca_dev *gspca_dev,
 {
 	struct usb_device *dev = gspca_dev->dev;
 
-#ifdef GSPCA_DEBUG
 	if (len > USB_BUF_SZ) {
 		pr_err("reg_w: buffer overflow\n");
 		return;
 	}
 	PDEBUG(D_USBO, "reg write [%02x] = %02x..", index, *buffer);
-#endif
+
 	memcpy(gspca_dev->usb_buf, buffer, len);
 	usb_control_msg(dev,
 			usb_sndctrlpipe(dev, 0),
@@ -274,7 +272,7 @@ static int et_video(struct gspca_dev *gspca_dev,
 		     : 0);		/* stopvideo */
 	ret = Et_WaitStatus(gspca_dev);
 	if (ret != 0)
-		PDEBUG(D_ERR, "timeout video on/off");
+		PERR("timeout video on/off");
 	return ret;
 }
 
@@ -768,9 +766,7 @@ static const struct sd_desc sd_desc = {
 /* -- module initialisation -- */
 static const struct usb_device_id device_table[] = {
 	{USB_DEVICE(0x102c, 0x6151), .driver_info = SENSOR_PAS106},
-#if !defined CONFIG_USB_ET61X251 && !defined CONFIG_USB_ET61X251_MODULE
 	{USB_DEVICE(0x102c, 0x6251), .driver_info = SENSOR_TAS5130CXX},
-#endif
 	{}
 };
 
diff --git a/drivers/media/usb/gspca/gl860/gl860.c b/drivers/media/usb/gspca/gl860/gl860.c
index ced3b71f14e5..cb1e64ca59c9 100644
--- a/drivers/media/usb/gspca/gl860/gl860.c
+++ b/drivers/media/usb/gspca/gl860/gl860.c
@@ -58,115 +58,135 @@ MODULE_PARM_DESC(sensor,
 
 /*============================ webcam controls =============================*/
 
-/* Functions to get and set a control value */
-#define SD_SETGET(thename) \
-static int sd_set_##thename(struct gspca_dev *gspca_dev, s32 val)\
-{\
-	struct sd *sd = (struct sd *) gspca_dev;\
-\
-	sd->vcur.thename = val;\
-	if (gspca_dev->streaming)\
-		sd->waitSet = 1;\
-	return 0;\
-} \
-static int sd_get_##thename(struct gspca_dev *gspca_dev, s32 *val)\
-{\
-	struct sd *sd = (struct sd *) gspca_dev;\
-\
-	*val = sd->vcur.thename;\
-	return 0;\
-}
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *) gspca_dev;
 
-SD_SETGET(mirror)
-SD_SETGET(flip)
-SD_SETGET(AC50Hz)
-SD_SETGET(backlight)
-SD_SETGET(brightness)
-SD_SETGET(gamma)
-SD_SETGET(hue)
-SD_SETGET(saturation)
-SD_SETGET(sharpness)
-SD_SETGET(whitebal)
-SD_SETGET(contrast)
-
-#define GL860_NCTRLS 11
-
-/* control table */
-static struct ctrl sd_ctrls_mi1320[GL860_NCTRLS];
-static struct ctrl sd_ctrls_mi2020[GL860_NCTRLS];
-static struct ctrl sd_ctrls_ov2640[GL860_NCTRLS];
-static struct ctrl sd_ctrls_ov9655[GL860_NCTRLS];
-
-#define SET_MY_CTRL(theid, \
-	thetype, thelabel, thename) \
-	if (sd->vmax.thename != 0) {\
-		sd_ctrls[nCtrls].qctrl.id   = theid;\
-		sd_ctrls[nCtrls].qctrl.type = thetype;\
-		strcpy(sd_ctrls[nCtrls].qctrl.name, thelabel);\
-		sd_ctrls[nCtrls].qctrl.minimum = 0;\
-		sd_ctrls[nCtrls].qctrl.maximum = sd->vmax.thename;\
-		sd_ctrls[nCtrls].qctrl.default_value = sd->vcur.thename;\
-		sd_ctrls[nCtrls].qctrl.step = \
-			(sd->vmax.thename < 16) ? 1 : sd->vmax.thename/16;\
-		sd_ctrls[nCtrls].set = sd_set_##thename;\
-		sd_ctrls[nCtrls].get = sd_get_##thename;\
-		nCtrls++;\
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		sd->vcur.brightness = ctrl->val;
+		break;
+	case V4L2_CID_CONTRAST:
+		sd->vcur.contrast = ctrl->val;
+		break;
+	case V4L2_CID_SATURATION:
+		sd->vcur.saturation = ctrl->val;
+		break;
+	case V4L2_CID_HUE:
+		sd->vcur.hue = ctrl->val;
+		break;
+	case V4L2_CID_GAMMA:
+		sd->vcur.gamma = ctrl->val;
+		break;
+	case V4L2_CID_HFLIP:
+		sd->vcur.mirror = ctrl->val;
+		break;
+	case V4L2_CID_VFLIP:
+		sd->vcur.flip = ctrl->val;
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		sd->vcur.AC50Hz = ctrl->val;
+		break;
+	case V4L2_CID_WHITE_BALANCE_TEMPERATURE:
+		sd->vcur.whitebal = ctrl->val;
+		break;
+	case V4L2_CID_SHARPNESS:
+		sd->vcur.sharpness = ctrl->val;
+		break;
+	case V4L2_CID_BACKLIGHT_COMPENSATION:
+		sd->vcur.backlight = ctrl->val;
+		break;
+	default:
+		return -EINVAL;
 	}
 
-static int gl860_build_control_table(struct gspca_dev *gspca_dev)
+	if (gspca_dev->streaming)
+		sd->waitSet = 1;
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
+
+static int sd_init_controls(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct ctrl *sd_ctrls;
-	int nCtrls = 0;
-
-	if (_MI1320_)
-		sd_ctrls = sd_ctrls_mi1320;
-	else if (_MI2020_)
-		sd_ctrls = sd_ctrls_mi2020;
-	else if (_OV2640_)
-		sd_ctrls = sd_ctrls_ov2640;
-	else if (_OV9655_)
-		sd_ctrls = sd_ctrls_ov9655;
-	else
-		return 0;
-
-	memset(sd_ctrls, 0, GL860_NCTRLS * sizeof(struct ctrl));
-
-	SET_MY_CTRL(V4L2_CID_BRIGHTNESS,
-		V4L2_CTRL_TYPE_INTEGER, "Brightness", brightness)
-	SET_MY_CTRL(V4L2_CID_SHARPNESS,
-		V4L2_CTRL_TYPE_INTEGER, "Sharpness", sharpness)
-	SET_MY_CTRL(V4L2_CID_CONTRAST,
-		V4L2_CTRL_TYPE_INTEGER, "Contrast", contrast)
-	SET_MY_CTRL(V4L2_CID_GAMMA,
-		V4L2_CTRL_TYPE_INTEGER, "Gamma", gamma)
-	SET_MY_CTRL(V4L2_CID_HUE,
-		V4L2_CTRL_TYPE_INTEGER, "Palette", hue)
-	SET_MY_CTRL(V4L2_CID_SATURATION,
-		V4L2_CTRL_TYPE_INTEGER, "Saturation", saturation)
-	SET_MY_CTRL(V4L2_CID_WHITE_BALANCE_TEMPERATURE,
-		V4L2_CTRL_TYPE_INTEGER, "White Bal.", whitebal)
-	SET_MY_CTRL(V4L2_CID_BACKLIGHT_COMPENSATION,
-		V4L2_CTRL_TYPE_INTEGER, "Backlight" , backlight)
-
-	SET_MY_CTRL(V4L2_CID_HFLIP,
-		V4L2_CTRL_TYPE_BOOLEAN, "Mirror", mirror)
-	SET_MY_CTRL(V4L2_CID_VFLIP,
-		V4L2_CTRL_TYPE_BOOLEAN, "Flip", flip)
-	SET_MY_CTRL(V4L2_CID_POWER_LINE_FREQUENCY,
-		V4L2_CTRL_TYPE_BOOLEAN, "AC power 50Hz", AC50Hz)
-
-	return nCtrls;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 11);
+
+	if (sd->vmax.brightness)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_BRIGHTNESS,
+				  0, sd->vmax.brightness, 1,
+				  sd->vcur.brightness);
+
+	if (sd->vmax.contrast)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_CONTRAST,
+				  0, sd->vmax.contrast, 1,
+				  sd->vcur.contrast);
+
+	if (sd->vmax.saturation)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_SATURATION,
+				  0, sd->vmax.saturation, 1,
+				  sd->vcur.saturation);
+
+	if (sd->vmax.hue)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_HUE,
+				  0, sd->vmax.hue, 1, sd->vcur.hue);
+
+	if (sd->vmax.gamma)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_GAMMA,
+				  0, sd->vmax.gamma, 1, sd->vcur.gamma);
+
+	if (sd->vmax.mirror)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_HFLIP,
+				  0, sd->vmax.mirror, 1, sd->vcur.mirror);
+
+	if (sd->vmax.flip)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_VFLIP,
+				  0, sd->vmax.flip, 1, sd->vcur.flip);
+
+	if (sd->vmax.AC50Hz)
+		v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
+				  V4L2_CID_POWER_LINE_FREQUENCY,
+				  sd->vmax.AC50Hz, 0, sd->vcur.AC50Hz);
+
+	if (sd->vmax.whitebal)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				  V4L2_CID_WHITE_BALANCE_TEMPERATURE,
+				  0, sd->vmax.whitebal, 1, sd->vcur.whitebal);
+
+	if (sd->vmax.sharpness)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_SHARPNESS,
+				  0, sd->vmax.sharpness, 1,
+				  sd->vcur.sharpness);
+
+	if (sd->vmax.backlight)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				  V4L2_CID_BACKLIGHT_COMPENSATION,
+				  0, sd->vmax.backlight, 1,
+				  sd->vcur.backlight);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+
+	return 0;
 }
 
 /*==================== sud-driver structure initialisation =================*/
 
 static const struct sd_desc sd_desc_mi1320 = {
 	.name        = MODULE_NAME,
-	.ctrls       = sd_ctrls_mi1320,
-	.nctrls      = GL860_NCTRLS,
 	.config      = sd_config,
 	.init        = sd_init,
+	.init_controls = sd_init_controls,
 	.isoc_init   = sd_isoc_init,
 	.start       = sd_start,
 	.stop0       = sd_stop0,
@@ -176,10 +196,9 @@ static const struct sd_desc sd_desc_mi1320 = {
 
 static const struct sd_desc sd_desc_mi2020 = {
 	.name        = MODULE_NAME,
-	.ctrls       = sd_ctrls_mi2020,
-	.nctrls      = GL860_NCTRLS,
 	.config      = sd_config,
 	.init        = sd_init,
+	.init_controls = sd_init_controls,
 	.isoc_init   = sd_isoc_init,
 	.start       = sd_start,
 	.stop0       = sd_stop0,
@@ -189,10 +208,9 @@ static const struct sd_desc sd_desc_mi2020 = {
 
 static const struct sd_desc sd_desc_ov2640 = {
 	.name        = MODULE_NAME,
-	.ctrls       = sd_ctrls_ov2640,
-	.nctrls      = GL860_NCTRLS,
 	.config      = sd_config,
 	.init        = sd_init,
+	.init_controls = sd_init_controls,
 	.isoc_init   = sd_isoc_init,
 	.start       = sd_start,
 	.stop0       = sd_stop0,
@@ -202,10 +220,9 @@ static const struct sd_desc sd_desc_ov2640 = {
 
 static const struct sd_desc sd_desc_ov9655 = {
 	.name        = MODULE_NAME,
-	.ctrls       = sd_ctrls_ov9655,
-	.nctrls      = GL860_NCTRLS,
 	.config      = sd_config,
 	.init        = sd_init,
+	.init_controls = sd_init_controls,
 	.isoc_init   = sd_isoc_init,
 	.start       = sd_start,
 	.stop0       = sd_stop0,
@@ -371,7 +388,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	dev_init_settings(gspca_dev);
 	if (AC50Hz != 0xff)
 		((struct sd *) gspca_dev)->vcur.AC50Hz = AC50Hz;
-	gl860_build_control_table(gspca_dev);
 
 	return 0;
 }
@@ -566,7 +582,7 @@ int gl860_RTx(struct gspca_dev *gspca_dev,
 		pr_err("ctrl transfer failed %4d [p%02x r%d v%04x i%04x len%d]\n",
 		       r, pref, req, val, index, len);
 	else if (len > 1 && r < len)
-		PDEBUG(D_ERR, "short ctrl transfer %d/%d", r, len);
+		PERR("short ctrl transfer %d/%d", r, len);
 
 	msleep(1);
 
diff --git a/drivers/media/usb/gspca/gspca.c b/drivers/media/usb/gspca/gspca.c
index 3564bdbb2ea3..5995ec4de6bc 100644
--- a/drivers/media/usb/gspca/gspca.c
+++ b/drivers/media/usb/gspca/gspca.c
@@ -60,14 +60,14 @@ MODULE_DESCRIPTION("GSPCA USB Camera Driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(GSPCA_VERSION);
 
-#ifdef GSPCA_DEBUG
-int gspca_debug = D_ERR | D_PROBE;
+int gspca_debug;
 EXPORT_SYMBOL(gspca_debug);
 
-static void PDEBUG_MODE(char *txt, __u32 pixfmt, int w, int h)
+static void PDEBUG_MODE(struct gspca_dev *gspca_dev, int debug, char *txt,
+			__u32 pixfmt, int w, int h)
 {
 	if ((pixfmt >> 24) >= '0' && (pixfmt >> 24) <= 'z') {
-		PDEBUG(D_CONF|D_STREAM, "%s %c%c%c%c %dx%d",
+		PDEBUG(debug, "%s %c%c%c%c %dx%d",
 			txt,
 			pixfmt & 0xff,
 			(pixfmt >> 8) & 0xff,
@@ -75,15 +75,12 @@ static void PDEBUG_MODE(char *txt, __u32 pixfmt, int w, int h)
 			pixfmt >> 24,
 			w, h);
 	} else {
-		PDEBUG(D_CONF|D_STREAM, "%s 0x%08x %dx%d",
+		PDEBUG(debug, "%s 0x%08x %dx%d",
 			txt,
 			pixfmt,
 			w, h);
 	}
 }
-#else
-#define PDEBUG_MODE(txt, pixfmt, w, h)
-#endif
 
 /* specific memory types - !! should be different from V4L2_MEMORY_xxx */
 #define GSPCA_MEMORY_NO 0	/* V4L2_MEMORY_xxx starts from 1 */
@@ -129,7 +126,7 @@ static void int_irq(struct urb *urb)
 	case 0:
 		if (gspca_dev->sd_desc->int_pkt_scan(gspca_dev,
 		    urb->transfer_buffer, urb->actual_length) < 0) {
-			PDEBUG(D_ERR, "Unknown packet received");
+			PERR("Unknown packet received");
 		}
 		break;
 
@@ -143,7 +140,7 @@ static void int_irq(struct urb *urb)
 		break;
 
 	default:
-		PDEBUG(D_ERR, "URB error %i, resubmitting", urb->status);
+		PERR("URB error %i, resubmitting", urb->status);
 		urb->status = 0;
 		ret = 0;
 	}
@@ -229,7 +226,7 @@ static int alloc_and_submit_int_urb(struct gspca_dev *gspca_dev,
 	urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
 	ret = usb_submit_urb(urb, GFP_KERNEL);
 	if (ret < 0) {
-		PDEBUG(D_ERR, "submit int URB failed with error %i", ret);
+		PERR("submit int URB failed with error %i", ret);
 		goto error_submit;
 	}
 	gspca_dev->int_urb = urb;
@@ -315,7 +312,7 @@ static void fill_frame(struct gspca_dev *gspca_dev,
 		if (gspca_dev->frozen)
 			return;
 #endif
-		PDEBUG(D_ERR|D_PACK, "urb status: %d", urb->status);
+		PERR("urb status: %d", urb->status);
 		urb->status = 0;
 		goto resubmit;
 	}
@@ -388,7 +385,7 @@ static void bulk_irq(struct urb *urb)
 		if (gspca_dev->frozen)
 			return;
 #endif
-		PDEBUG(D_ERR|D_PACK, "urb status: %d", urb->status);
+		PERR("urb status: %d", urb->status);
 		urb->status = 0;
 		goto resubmit;
 	}
@@ -460,7 +457,7 @@ void gspca_frame_add(struct gspca_dev *gspca_dev,
 	/* append the packet to the frame buffer */
 	if (len > 0) {
 		if (gspca_dev->image_len + len > gspca_dev->frsz) {
-			PDEBUG(D_ERR|D_PACK, "frame overflow %d > %d",
+			PERR("frame overflow %d > %d",
 				gspca_dev->image_len + len,
 				gspca_dev->frsz);
 			packet_type = DISCARD_PACKET;
@@ -570,11 +567,10 @@ static void destroy_urbs(struct gspca_dev *gspca_dev)
 
 		gspca_dev->urb[i] = NULL;
 		usb_kill_urb(urb);
-		if (urb->transfer_buffer != NULL)
-			usb_free_coherent(gspca_dev->dev,
-					  urb->transfer_buffer_length,
-					  urb->transfer_buffer,
-					  urb->transfer_dma);
+		usb_free_coherent(gspca_dev->dev,
+				  urb->transfer_buffer_length,
+				  urb->transfer_buffer,
+				  urb->transfer_dma);
 		usb_free_urb(urb);
 	}
 }
@@ -960,9 +956,7 @@ static int gspca_init_transfer(struct gspca_dev *gspca_dev)
 		/* the bandwidth is not wide enough
 		 * negotiate or try a lower alternate setting */
 retry:
-		PDEBUG(D_ERR|D_STREAM,
-			"alt %d - bandwidth not wide enough - trying again",
-			alt);
+		PERR("alt %d - bandwidth not wide enough, trying again", alt);
 		msleep(20);	/* wait for kill complete */
 		if (gspca_dev->sd_desc->isoc_nego) {
 			ret = gspca_dev->sd_desc->isoc_nego(gspca_dev);
@@ -984,7 +978,6 @@ out:
 
 static void gspca_set_default_mode(struct gspca_dev *gspca_dev)
 {
-	struct gspca_ctrl *ctrl;
 	int i;
 
 	i = gspca_dev->cam.nmodes - 1;	/* take the highest mode */
@@ -993,17 +986,8 @@ static void gspca_set_default_mode(struct gspca_dev *gspca_dev)
 	gspca_dev->height = gspca_dev->cam.cam_mode[i].height;
 	gspca_dev->pixfmt = gspca_dev->cam.cam_mode[i].pixelformat;
 
-	/* set the current control values to their default values
-	 * which may have changed in sd_init() */
 	/* does nothing if ctrl_handler == NULL */
 	v4l2_ctrl_handler_setup(gspca_dev->vdev.ctrl_handler);
-	ctrl = gspca_dev->cam.ctrls;
-	if (ctrl != NULL) {
-		for (i = 0;
-		     i < gspca_dev->sd_desc->nctrls;
-		     i++, ctrl++)
-			ctrl->val = ctrl->def;
-	}
 }
 
 static int wxh_to_mode(struct gspca_dev *gspca_dev,
@@ -1055,7 +1039,7 @@ static int vidioc_g_register(struct file *file, void *priv,
 }
 
 static int vidioc_s_register(struct file *file, void *priv,
-			struct v4l2_dbg_register *reg)
+			const struct v4l2_dbg_register *reg)
 {
 	struct gspca_dev *gspca_dev = video_drvdata(file);
 
@@ -1137,10 +1121,9 @@ static int try_fmt_vid_cap(struct gspca_dev *gspca_dev,
 	w = fmt->fmt.pix.width;
 	h = fmt->fmt.pix.height;
 
-#ifdef GSPCA_DEBUG
-	if (gspca_debug & D_CONF)
-		PDEBUG_MODE("try fmt cap", fmt->fmt.pix.pixelformat, w, h);
-#endif
+	PDEBUG_MODE(gspca_dev, D_CONF, "try fmt cap",
+		    fmt->fmt.pix.pixelformat, w, h);
+
 	/* search the closest mode for width and height */
 	mode = wxh_to_mode(gspca_dev, w, h);
 
@@ -1153,8 +1136,6 @@ static int try_fmt_vid_cap(struct gspca_dev *gspca_dev,
 					fmt->fmt.pix.pixelformat);
 		if (mode2 >= 0)
 			mode = mode2;
-/*		else
-			;		 * no chance, return this mode */
 	}
 	fmt->fmt.pix = gspca_dev->cam.cam_mode[mode];
 	/* some drivers use priv internally, zero it before giving it to
@@ -1290,15 +1271,6 @@ static int dev_open(struct file *file)
 	if (!try_module_get(gspca_dev->module))
 		return -ENODEV;
 
-#ifdef GSPCA_DEBUG
-	/* activate the v4l2 debug */
-	if (gspca_debug & D_V4L2)
-		gspca_dev->vdev.debug |= V4L2_DEBUG_IOCTL
-					| V4L2_DEBUG_IOCTL_ARG;
-	else
-		gspca_dev->vdev.debug &= ~(V4L2_DEBUG_IOCTL
-					| V4L2_DEBUG_IOCTL_ARG);
-#endif
 	return v4l2_fh_open(file);
 }
 
@@ -1357,134 +1329,6 @@ static int vidioc_querycap(struct file *file, void  *priv,
 	return 0;
 }
 
-static int get_ctrl(struct gspca_dev *gspca_dev,
-				   int id)
-{
-	const struct ctrl *ctrls;
-	int i;
-
-	for (i = 0, ctrls = gspca_dev->sd_desc->ctrls;
-	     i < gspca_dev->sd_desc->nctrls;
-	     i++, ctrls++) {
-		if (gspca_dev->ctrl_dis & (1 << i))
-			continue;
-		if (id == ctrls->qctrl.id)
-			return i;
-	}
-	return -1;
-}
-
-static int vidioc_queryctrl(struct file *file, void *priv,
-			   struct v4l2_queryctrl *q_ctrl)
-{
-	struct gspca_dev *gspca_dev = video_drvdata(file);
-	const struct ctrl *ctrls;
-	struct gspca_ctrl *gspca_ctrl;
-	int i, idx;
-	u32 id;
-
-	id = q_ctrl->id;
-	if (id & V4L2_CTRL_FLAG_NEXT_CTRL) {
-		id &= V4L2_CTRL_ID_MASK;
-		id++;
-		idx = -1;
-		for (i = 0; i < gspca_dev->sd_desc->nctrls; i++) {
-			if (gspca_dev->ctrl_dis & (1 << i))
-				continue;
-			if (gspca_dev->sd_desc->ctrls[i].qctrl.id < id)
-				continue;
-			if (idx >= 0
-			 && gspca_dev->sd_desc->ctrls[i].qctrl.id
-				    > gspca_dev->sd_desc->ctrls[idx].qctrl.id)
-				continue;
-			idx = i;
-		}
-	} else {
-		idx = get_ctrl(gspca_dev, id);
-	}
-	if (idx < 0)
-		return -EINVAL;
-	ctrls = &gspca_dev->sd_desc->ctrls[idx];
-	memcpy(q_ctrl, &ctrls->qctrl, sizeof *q_ctrl);
-	if (gspca_dev->cam.ctrls != NULL) {
-		gspca_ctrl = &gspca_dev->cam.ctrls[idx];
-		q_ctrl->default_value = gspca_ctrl->def;
-		q_ctrl->minimum = gspca_ctrl->min;
-		q_ctrl->maximum = gspca_ctrl->max;
-	}
-	if (gspca_dev->ctrl_inac & (1 << idx))
-		q_ctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
-	return 0;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
-{
-	struct gspca_dev *gspca_dev = video_drvdata(file);
-	const struct ctrl *ctrls;
-	struct gspca_ctrl *gspca_ctrl;
-	int idx;
-
-	idx = get_ctrl(gspca_dev, ctrl->id);
-	if (idx < 0)
-		return -EINVAL;
-	if (gspca_dev->ctrl_inac & (1 << idx))
-		return -EINVAL;
-	ctrls = &gspca_dev->sd_desc->ctrls[idx];
-	if (gspca_dev->cam.ctrls != NULL) {
-		gspca_ctrl = &gspca_dev->cam.ctrls[idx];
-		if (ctrl->value < gspca_ctrl->min
-		    || ctrl->value > gspca_ctrl->max)
-			return -ERANGE;
-	} else {
-		gspca_ctrl = NULL;
-		if (ctrl->value < ctrls->qctrl.minimum
-		    || ctrl->value > ctrls->qctrl.maximum)
-			return -ERANGE;
-	}
-	PDEBUG(D_CONF, "set ctrl [%08x] = %d", ctrl->id, ctrl->value);
-	gspca_dev->usb_err = 0;
-	if (ctrls->set != NULL)
-		return ctrls->set(gspca_dev, ctrl->value);
-	if (gspca_ctrl != NULL) {
-		gspca_ctrl->val = ctrl->value;
-		if (ctrls->set_control != NULL
-		 && gspca_dev->streaming)
-			ctrls->set_control(gspca_dev);
-	}
-	return gspca_dev->usb_err;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
-{
-	struct gspca_dev *gspca_dev = video_drvdata(file);
-	const struct ctrl *ctrls;
-	int idx;
-
-	idx = get_ctrl(gspca_dev, ctrl->id);
-	if (idx < 0)
-		return -EINVAL;
-	ctrls = &gspca_dev->sd_desc->ctrls[idx];
-
-	gspca_dev->usb_err = 0;
-	if (ctrls->get != NULL)
-		return ctrls->get(gspca_dev, &ctrl->value);
-	if (gspca_dev->cam.ctrls != NULL)
-		ctrl->value = gspca_dev->cam.ctrls[idx].val;
-	return 0;
-}
-
-static int vidioc_querymenu(struct file *file, void *priv,
-			    struct v4l2_querymenu *qmenu)
-{
-	struct gspca_dev *gspca_dev = video_drvdata(file);
-
-	if (!gspca_dev->sd_desc->querymenu)
-		return -ENOTTY;
-	return gspca_dev->sd_desc->querymenu(gspca_dev, qmenu);
-}
-
 static int vidioc_enum_input(struct file *file, void *priv,
 				struct v4l2_input *input)
 {
@@ -1621,14 +1465,8 @@ static int vidioc_streamon(struct file *file, void *priv,
 		if (ret < 0)
 			goto out;
 	}
-#ifdef GSPCA_DEBUG
-	if (gspca_debug & D_STREAM) {
-		PDEBUG_MODE("stream on OK",
-			gspca_dev->pixfmt,
-			gspca_dev->width,
-			gspca_dev->height);
-	}
-#endif
+	PDEBUG_MODE(gspca_dev, D_STREAM, "stream on OK", gspca_dev->pixfmt,
+		    gspca_dev->width, gspca_dev->height);
 	ret = 0;
 out:
 	mutex_unlock(&gspca_dev->queue_lock);
@@ -1879,8 +1717,7 @@ static int vidioc_dqbuf(struct file *file, void *priv,
 		if (copy_to_user((__u8 __user *) frame->v4l2_buf.m.userptr,
 				 frame->data,
 				 frame->v4l2_buf.bytesused)) {
-			PDEBUG(D_ERR|D_STREAM,
-				"dqbuf cp to user failed");
+			PERR("dqbuf cp to user failed");
 			ret = -EFAULT;
 		}
 	}
@@ -2092,8 +1929,7 @@ static ssize_t dev_read(struct file *file, char __user *data,
 		count = frame->v4l2_buf.bytesused;
 	ret = copy_to_user(data, frame->data, count);
 	if (ret != 0) {
-		PDEBUG(D_ERR|D_STREAM,
-			"read cp to user lack %d / %zd", ret, count);
+		PERR("read cp to user lack %d / %zd", ret, count);
 		ret = -EFAULT;
 		goto out;
 	}
@@ -2125,10 +1961,6 @@ static const struct v4l2_ioctl_ops dev_ioctl_ops = {
 	.vidioc_g_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap	= vidioc_s_fmt_vid_cap,
 	.vidioc_streamon	= vidioc_streamon,
-	.vidioc_queryctrl	= vidioc_queryctrl,
-	.vidioc_g_ctrl		= vidioc_g_ctrl,
-	.vidioc_s_ctrl		= vidioc_s_ctrl,
-	.vidioc_querymenu	= vidioc_querymenu,
 	.vidioc_enum_input	= vidioc_enum_input,
 	.vidioc_g_input		= vidioc_g_input,
 	.vidioc_s_input		= vidioc_s_input,
@@ -2157,22 +1989,6 @@ static const struct video_device gspca_template = {
 	.release = video_device_release_empty, /* We use v4l2_dev.release */
 };
 
-/* initialize the controls */
-static void ctrls_init(struct gspca_dev *gspca_dev)
-{
-	struct gspca_ctrl *ctrl;
-	int i;
-
-	for (i = 0, ctrl = gspca_dev->cam.ctrls;
-	     i < gspca_dev->sd_desc->nctrls;
-	     i++, ctrl++) {
-		ctrl->def = gspca_dev->sd_desc->ctrls[i].qctrl.default_value;
-		ctrl->val = ctrl->def;
-		ctrl->min = gspca_dev->sd_desc->ctrls[i].qctrl.minimum;
-		ctrl->max = gspca_dev->sd_desc->ctrls[i].qctrl.maximum;
-	}
-}
-
 /*
  * probe and create a new gspca device
  *
@@ -2249,8 +2065,6 @@ int gspca_dev_probe2(struct usb_interface *intf,
 	ret = sd_desc->config(gspca_dev, id);
 	if (ret < 0)
 		goto out;
-	if (gspca_dev->cam.ctrls != NULL)
-		ctrls_init(gspca_dev);
 	ret = sd_desc->init(gspca_dev);
 	if (ret < 0)
 		goto out;
@@ -2450,10 +2264,6 @@ static void __exit gspca_exit(void)
 module_init(gspca_init);
 module_exit(gspca_exit);
 
-#ifdef GSPCA_DEBUG
 module_param_named(debug, gspca_debug, int, 0644);
 MODULE_PARM_DESC(debug,
-		"Debug (bit) 0x01:error 0x02:probe 0x04:config"
-		" 0x08:stream 0x10:frame 0x20:packet"
-		" 0x0100: v4l2");
-#endif
+		"1:probe 2:config 3:stream 4:frame 5:packet 6:usbi 7:usbo");
diff --git a/drivers/media/usb/gspca/gspca.h b/drivers/media/usb/gspca/gspca.h
index 5559932bf2f5..ef8efeb80070 100644
--- a/drivers/media/usb/gspca/gspca.h
+++ b/drivers/media/usb/gspca/gspca.h
@@ -10,30 +10,26 @@
 #include <media/v4l2-device.h>
 #include <linux/mutex.h>
 
-/* compilation option */
-/*#define GSPCA_DEBUG 1*/
 
-#ifdef GSPCA_DEBUG
-/* GSPCA our debug messages */
+
+/* GSPCA debug codes */
+
+#define D_PROBE  1
+#define D_CONF   2
+#define D_STREAM 3
+#define D_FRAM   4
+#define D_PACK   5
+#define D_USBI   6
+#define D_USBO   7
+
 extern int gspca_debug;
-#define PDEBUG(level, fmt, ...)					\
-do {								\
-	if (gspca_debug & (level))				\
-		pr_info(fmt, ##__VA_ARGS__);			\
-} while (0)
 
-#define D_ERR  0x01
-#define D_PROBE 0x02
-#define D_CONF 0x04
-#define D_STREAM 0x08
-#define D_FRAM 0x10
-#define D_PACK 0x20
-#define D_USBI 0x00
-#define D_USBO 0x00
-#define D_V4L2 0x0100
-#else
-#define PDEBUG(level, fmt, ...) do {} while(0)
-#endif
+
+#define PDEBUG(level, fmt, ...) \
+	v4l2_dbg(level, gspca_debug, &gspca_dev->v4l2_dev, fmt, ##__VA_ARGS__)
+
+#define PERR(fmt, ...) \
+	v4l2_err(&gspca_dev->v4l2_dev, fmt, ##__VA_ARGS__)
 
 #define GSPCA_MAX_FRAMES 16	/* maximum number of video frame buffers */
 /* image transfers */
@@ -46,20 +42,11 @@ struct framerates {
 	int nrates;
 };
 
-/* control definition */
-struct gspca_ctrl {
-	s16 val;	/* current value */
-	s16 def;	/* default value */
-	s16 min, max;	/* minimum and maximum values */
-};
-
 /* device information - set at probe time */
 struct cam {
 	const struct v4l2_pix_format *cam_mode;	/* size nmodes */
 	const struct framerates *mode_framerates; /* must have size nmodes,
 						   * just like cam_mode */
-	struct gspca_ctrl *ctrls;	/* control table - size nctrls */
-					/* may be NULL */
 	u32 bulk_size;		/* buffer size when image transfer by bulk */
 	u32 input_flags;	/* value for ENUM_INPUT status flags */
 	u8 nmodes;		/* size of cam_mode */
@@ -87,14 +74,14 @@ typedef int (*cam_get_jpg_op) (struct gspca_dev *,
 				struct v4l2_jpegcompression *);
 typedef int (*cam_set_jpg_op) (struct gspca_dev *,
 				const struct v4l2_jpegcompression *);
-typedef int (*cam_reg_op) (struct gspca_dev *,
+typedef int (*cam_get_reg_op) (struct gspca_dev *,
 				struct v4l2_dbg_register *);
+typedef int (*cam_set_reg_op) (struct gspca_dev *,
+				const struct v4l2_dbg_register *);
 typedef int (*cam_ident_op) (struct gspca_dev *,
 				struct v4l2_dbg_chip_ident *);
 typedef void (*cam_streamparm_op) (struct gspca_dev *,
 				  struct v4l2_streamparm *);
-typedef int (*cam_qmnu_op) (struct gspca_dev *,
-			struct v4l2_querymenu *);
 typedef void (*cam_pkt_op) (struct gspca_dev *gspca_dev,
 				u8 *data,
 				int len);
@@ -102,20 +89,10 @@ typedef int (*cam_int_pkt_op) (struct gspca_dev *gspca_dev,
 				u8 *data,
 				int len);
 
-struct ctrl {
-	struct v4l2_queryctrl qctrl;
-	int (*set)(struct gspca_dev *, __s32);
-	int (*get)(struct gspca_dev *, __s32 *);
-	cam_v_op set_control;
-};
-
 /* subdriver description */
 struct sd_desc {
 /* information */
 	const char *name;	/* sub-driver name */
-/* controls */
-	const struct ctrl *ctrls;	/* static control definition */
-	int nctrls;
 /* mandatory operations */
 	cam_cf_op config;	/* called on probe */
 	cam_op init;		/* called on probe and resume */
@@ -130,12 +107,11 @@ struct sd_desc {
 	cam_v_op dq_callback;	/* called when a frame has been dequeued */
 	cam_get_jpg_op get_jcomp;
 	cam_set_jpg_op set_jcomp;
-	cam_qmnu_op querymenu;
 	cam_streamparm_op get_streamparm;
 	cam_streamparm_op set_streamparm;
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-	cam_reg_op set_register;
-	cam_reg_op get_register;
+	cam_set_reg_op set_register;
+	cam_get_reg_op get_register;
 #endif
 	cam_ident_op get_chip_ident;
 #if IS_ENABLED(CONFIG_INPUT)
@@ -174,8 +150,6 @@ struct gspca_dev {
 
 	struct cam cam;				/* device information */
 	const struct sd_desc *sd_desc;		/* subdriver description */
-	unsigned ctrl_dis;		/* disabled controls (bit map) */
-	unsigned ctrl_inac;		/* inactive controls (bit map) */
 	struct v4l2_ctrl_handler ctrl_handler;
 
 	/* autogain and exposure or gain control cluster, these are global as
diff --git a/drivers/media/usb/gspca/jeilinj.c b/drivers/media/usb/gspca/jeilinj.c
index 1ba29fe7fada..8da3dde38385 100644
--- a/drivers/media/usb/gspca/jeilinj.c
+++ b/drivers/media/usb/gspca/jeilinj.c
@@ -266,7 +266,7 @@ static int jlj_start(struct gspca_dev *gspca_dev)
 	msleep(2);
 	setfreq(gspca_dev, v4l2_ctrl_g_ctrl(sd->freq));
 	if (gspca_dev->usb_err < 0)
-		PDEBUG(D_ERR, "Start streaming command failed");
+		PERR("Start streaming command failed");
 	return gspca_dev->usb_err;
 }
 
diff --git a/drivers/media/usb/gspca/konica.c b/drivers/media/usb/gspca/konica.c
index 61e25dbf2447..39c96bb4c985 100644
--- a/drivers/media/usb/gspca/konica.c
+++ b/drivers/media/usb/gspca/konica.c
@@ -277,7 +277,7 @@ static void sd_isoc_irq(struct urb *urb)
 		if (gspca_dev->frozen)
 			return;
 #endif
-		PDEBUG(D_ERR, "urb status: %d", urb->status);
+		PERR("urb status: %d", urb->status);
 		st = usb_submit_urb(urb, GFP_ATOMIC);
 		if (st < 0)
 			pr_err("resubmit urb error %d\n", st);
@@ -295,33 +295,30 @@ static void sd_isoc_irq(struct urb *urb)
 	sd->last_data_urb = NULL;
 
 	if (!data_urb || data_urb->start_frame != status_urb->start_frame) {
-		PDEBUG(D_ERR|D_PACK, "lost sync on frames");
+		PERR("lost sync on frames");
 		goto resubmit;
 	}
 
 	if (data_urb->number_of_packets != status_urb->number_of_packets) {
-		PDEBUG(D_ERR|D_PACK,
-		       "no packets does not match, data: %d, status: %d",
-		       data_urb->number_of_packets,
-		       status_urb->number_of_packets);
+		PERR("no packets does not match, data: %d, status: %d",
+		     data_urb->number_of_packets,
+		     status_urb->number_of_packets);
 		goto resubmit;
 	}
 
 	for (i = 0; i < status_urb->number_of_packets; i++) {
 		if (data_urb->iso_frame_desc[i].status ||
 		    status_urb->iso_frame_desc[i].status) {
-			PDEBUG(D_ERR|D_PACK,
-			       "pkt %d data-status %d, status-status %d", i,
-			       data_urb->iso_frame_desc[i].status,
-			       status_urb->iso_frame_desc[i].status);
+			PERR("pkt %d data-status %d, status-status %d", i,
+			     data_urb->iso_frame_desc[i].status,
+			     status_urb->iso_frame_desc[i].status);
 			gspca_dev->last_packet_type = DISCARD_PACKET;
 			continue;
 		}
 
 		if (status_urb->iso_frame_desc[i].actual_length != 1) {
-			PDEBUG(D_ERR|D_PACK,
-			       "bad status packet length %d",
-			       status_urb->iso_frame_desc[i].actual_length);
+			PERR("bad status packet length %d",
+			     status_urb->iso_frame_desc[i].actual_length);
 			gspca_dev->last_packet_type = DISCARD_PACKET;
 			continue;
 		}
@@ -366,12 +363,11 @@ resubmit:
 	if (data_urb) {
 		st = usb_submit_urb(data_urb, GFP_ATOMIC);
 		if (st < 0)
-			PDEBUG(D_ERR|D_PACK,
-			       "usb_submit_urb(data_urb) ret %d", st);
+			PERR("usb_submit_urb(data_urb) ret %d", st);
 	}
 	st = usb_submit_urb(status_urb, GFP_ATOMIC);
 	if (st < 0)
-		pr_err("usb_submit_urb(status_urb) ret %d\n", st);
+		PERR("usb_submit_urb(status_urb) ret %d\n", st);
 }
 
 static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
diff --git a/drivers/media/usb/gspca/m5602/m5602_bridge.h b/drivers/media/usb/gspca/m5602/m5602_bridge.h
index 51af3ee3ab85..19eb1a64f9d6 100644
--- a/drivers/media/usb/gspca/m5602/m5602_bridge.h
+++ b/drivers/media/usb/gspca/m5602/m5602_bridge.h
@@ -136,16 +136,33 @@ struct sd {
 	/* A pointer to the currently connected sensor */
 	const struct m5602_sensor *sensor;
 
-	struct sd_desc *desc;
-
-	/* Sensor private data */
-	void *sensor_priv;
-
 	/* The current frame's id, used to detect frame boundaries */
 	u8 frame_id;
 
 	/* The current frame count */
 	u32 frame_count;
+
+	/* Camera rotation polling thread for "flipable" cams */
+	struct task_struct *rotation_thread;
+
+	struct { /* auto-white-bal + green/red/blue balance control cluster */
+		struct v4l2_ctrl *auto_white_bal;
+		struct v4l2_ctrl *red_bal;
+		struct v4l2_ctrl *blue_bal;
+		struct v4l2_ctrl *green_bal;
+	};
+	struct { /* autoexpo / expo cluster */
+		struct v4l2_ctrl *autoexpo;
+		struct v4l2_ctrl *expo;
+	};
+	struct { /* autogain / gain cluster */
+		struct v4l2_ctrl *autogain;
+		struct v4l2_ctrl *gain;
+	};
+	struct { /* hflip/vflip cluster */
+		struct v4l2_ctrl *hflip;
+		struct v4l2_ctrl *vflip;
+	};
 };
 
 int m5602_read_bridge(
diff --git a/drivers/media/usb/gspca/m5602/m5602_core.c b/drivers/media/usb/gspca/m5602/m5602_core.c
index ed22638978ce..d926e62cb80b 100644
--- a/drivers/media/usb/gspca/m5602/m5602_core.c
+++ b/drivers/media/usb/gspca/m5602/m5602_core.c
@@ -41,6 +41,7 @@ MODULE_DEVICE_TABLE(usb, m5602_table);
 int m5602_read_bridge(struct sd *sd, const u8 address, u8 *i2c_data)
 {
 	int err;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *) sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
 
@@ -62,6 +63,7 @@ int m5602_read_bridge(struct sd *sd, const u8 address, u8 *i2c_data)
 int m5602_write_bridge(struct sd *sd, const u8 address, const u8 i2c_data)
 {
 	int err;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *) sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
 
@@ -98,6 +100,7 @@ int m5602_read_sensor(struct sd *sd, const u8 address,
 		       u8 *i2c_data, const u8 len)
 {
 	int err, i;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *) sd;
 
 	if (!len || len > sd->sensor->i2c_regW)
 		return -EINVAL;
@@ -147,6 +150,7 @@ int m5602_write_sensor(struct sd *sd, const u8 address,
 {
 	int err, i;
 	u8 *p;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *) sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
 
@@ -252,6 +256,16 @@ static int m5602_init(struct gspca_dev *gspca_dev)
 	return err;
 }
 
+static int m5602_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	if (!sd->sensor->init_controls)
+		return 0;
+
+	return sd->sensor->init_controls(sd);
+}
+
 static int m5602_start_transfer(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -336,11 +350,12 @@ static void m5602_stop_transfer(struct gspca_dev *gspca_dev)
 		sd->sensor->stop(sd);
 }
 
-/* sub-driver description, the ctrl and nctrl is filled at probe time */
-static struct sd_desc sd_desc = {
+/* sub-driver description */
+static const struct sd_desc sd_desc = {
 	.name		= MODULE_NAME,
 	.config		= m5602_configure,
 	.init		= m5602_init,
+	.init_controls	= m5602_init_controls,
 	.start		= m5602_start_transfer,
 	.stopN		= m5602_stop_transfer,
 	.pkt_scan	= m5602_urb_complete
@@ -355,7 +370,6 @@ static int m5602_configure(struct gspca_dev *gspca_dev,
 	int err;
 
 	cam = &gspca_dev->cam;
-	sd->desc = &sd_desc;
 
 	if (dump_bridge)
 		m5602_dump_bridge(sd);
@@ -368,7 +382,7 @@ static int m5602_configure(struct gspca_dev *gspca_dev,
 	return 0;
 
 fail:
-	PDEBUG(D_ERR, "ALi m5602 webcam failed");
+	PERR("ALi m5602 webcam failed");
 	cam->cam_mode = NULL;
 	cam->nmodes = 0;
 
diff --git a/drivers/media/usb/gspca/m5602/m5602_mt9m111.c b/drivers/media/usb/gspca/m5602/m5602_mt9m111.c
index 6268aa24ec5d..cfa4663f8934 100644
--- a/drivers/media/usb/gspca/m5602/m5602_mt9m111.c
+++ b/drivers/media/usb/gspca/m5602/m5602_mt9m111.c
@@ -20,22 +20,8 @@
 
 #include "m5602_mt9m111.h"
 
-static int mt9m111_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
-static int mt9m111_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int mt9m111_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int mt9m111_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
-static int mt9m111_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int mt9m111_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int mt9m111_set_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 val);
-static int mt9m111_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					  __s32 *val);
-static int mt9m111_get_green_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int mt9m111_set_green_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int mt9m111_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int mt9m111_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int mt9m111_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int mt9m111_set_red_balance(struct gspca_dev *gspca_dev, __s32 val);
+static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl);
+static void mt9m111_dump_registers(struct sd *sd);
 
 static struct v4l2_pix_format mt9m111_modes[] = {
 	{
@@ -50,118 +36,27 @@ static struct v4l2_pix_format mt9m111_modes[] = {
 	}
 };
 
-static const struct ctrl mt9m111_ctrls[] = {
-#define VFLIP_IDX 0
-	{
-		{
-			.id		= V4L2_CID_VFLIP,
-			.type           = V4L2_CTRL_TYPE_BOOLEAN,
-			.name           = "vertical flip",
-			.minimum        = 0,
-			.maximum        = 1,
-			.step           = 1,
-			.default_value  = 0
-		},
-		.set = mt9m111_set_vflip,
-		.get = mt9m111_get_vflip
-	},
-#define HFLIP_IDX 1
-	{
-		{
-			.id             = V4L2_CID_HFLIP,
-			.type           = V4L2_CTRL_TYPE_BOOLEAN,
-			.name           = "horizontal flip",
-			.minimum        = 0,
-			.maximum        = 1,
-			.step           = 1,
-			.default_value  = 0
-		},
-		.set = mt9m111_set_hflip,
-		.get = mt9m111_get_hflip
-	},
-#define GAIN_IDX 2
-	{
-		{
-			.id             = V4L2_CID_GAIN,
-			.type           = V4L2_CTRL_TYPE_INTEGER,
-			.name           = "gain",
-			.minimum        = 0,
-			.maximum        = (INITIAL_MAX_GAIN - 1) * 2 * 2 * 2,
-			.step           = 1,
-			.default_value  = MT9M111_DEFAULT_GAIN,
-			.flags          = V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = mt9m111_set_gain,
-		.get = mt9m111_get_gain
-	},
-#define AUTO_WHITE_BALANCE_IDX 3
-	{
-		{
-			.id             = V4L2_CID_AUTO_WHITE_BALANCE,
-			.type           = V4L2_CTRL_TYPE_BOOLEAN,
-			.name           = "auto white balance",
-			.minimum        = 0,
-			.maximum        = 1,
-			.step           = 1,
-			.default_value  = 0,
-		},
-		.set = mt9m111_set_auto_white_balance,
-		.get = mt9m111_get_auto_white_balance
-	},
-#define GREEN_BALANCE_IDX 4
-	{
-		{
-			.id		= M5602_V4L2_CID_GREEN_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "green balance",
-			.minimum	= 0x00,
-			.maximum	= 0x7ff,
-			.step		= 0x1,
-			.default_value	= MT9M111_GREEN_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = mt9m111_set_green_balance,
-		.get = mt9m111_get_green_balance
-	},
-#define BLUE_BALANCE_IDX 5
-	{
-		{
-			.id		= V4L2_CID_BLUE_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "blue balance",
-			.minimum	= 0x00,
-			.maximum	= 0x7ff,
-			.step		= 0x1,
-			.default_value	= MT9M111_BLUE_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = mt9m111_set_blue_balance,
-		.get = mt9m111_get_blue_balance
-	},
-#define RED_BALANCE_IDX 5
-	{
-		{
-			.id		= V4L2_CID_RED_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "red balance",
-			.minimum	= 0x00,
-			.maximum	= 0x7ff,
-			.step		= 0x1,
-			.default_value	= MT9M111_RED_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = mt9m111_set_red_balance,
-		.get = mt9m111_get_red_balance
-	},
+static const struct v4l2_ctrl_ops mt9m111_ctrl_ops = {
+	.s_ctrl = mt9m111_s_ctrl,
 };
 
-static void mt9m111_dump_registers(struct sd *sd);
+static const struct v4l2_ctrl_config mt9m111_greenbal_cfg = {
+	.ops	= &mt9m111_ctrl_ops,
+	.id	= M5602_V4L2_CID_GREEN_BALANCE,
+	.name	= "Green Balance",
+	.type	= V4L2_CTRL_TYPE_INTEGER,
+	.min	= 0,
+	.max	= 0x7ff,
+	.step	= 1,
+	.def	= MT9M111_GREEN_GAIN_DEFAULT,
+	.flags	= V4L2_CTRL_FLAG_SLIDER,
+};
 
 int mt9m111_probe(struct sd *sd)
 {
 	u8 data[2] = {0x00, 0x00};
 	int i;
-	s32 *sensor_settings;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	if (force_sensor) {
 		if (force_sensor == MT9M111_SENSOR) {
@@ -200,19 +95,8 @@ int mt9m111_probe(struct sd *sd)
 	return -ENODEV;
 
 sensor_found:
-	sensor_settings = kmalloc(ARRAY_SIZE(mt9m111_ctrls) * sizeof(s32),
-				  GFP_KERNEL);
-	if (!sensor_settings)
-		return -ENOMEM;
-
 	sd->gspca_dev.cam.cam_mode = mt9m111_modes;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(mt9m111_modes);
-	sd->desc->ctrls = mt9m111_ctrls;
-	sd->desc->nctrls = ARRAY_SIZE(mt9m111_ctrls);
-
-	for (i = 0; i < ARRAY_SIZE(mt9m111_ctrls); i++)
-		sensor_settings[i] = mt9m111_ctrls[i].qctrl.default_value;
-	sd->sensor_priv = sensor_settings;
 
 	return 0;
 }
@@ -220,7 +104,6 @@ sensor_found:
 int mt9m111_init(struct sd *sd)
 {
 	int i, err = 0;
-	s32 *sensor_settings = sd->sensor_priv;
 
 	/* Init the sensor */
 	for (i = 0; i < ARRAY_SIZE(init_mt9m111) && !err; i++) {
@@ -241,30 +124,45 @@ int mt9m111_init(struct sd *sd)
 	if (dump_sensor)
 		mt9m111_dump_registers(sd);
 
-	err = mt9m111_set_vflip(&sd->gspca_dev, sensor_settings[VFLIP_IDX]);
-	if (err < 0)
-		return err;
-
-	err = mt9m111_set_hflip(&sd->gspca_dev, sensor_settings[HFLIP_IDX]);
-	if (err < 0)
-		return err;
-
-	err = mt9m111_set_green_balance(&sd->gspca_dev,
-					 sensor_settings[GREEN_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+	return 0;
+}
 
-	err = mt9m111_set_blue_balance(&sd->gspca_dev,
-					 sensor_settings[BLUE_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+int mt9m111_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+
+	sd->gspca_dev.vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 7);
+
+	sd->auto_white_bal = v4l2_ctrl_new_std(hdl, &mt9m111_ctrl_ops,
+					       V4L2_CID_AUTO_WHITE_BALANCE,
+					       0, 1, 1, 0);
+	sd->green_bal = v4l2_ctrl_new_custom(hdl, &mt9m111_greenbal_cfg, NULL);
+	sd->red_bal = v4l2_ctrl_new_std(hdl, &mt9m111_ctrl_ops,
+					V4L2_CID_RED_BALANCE, 0, 0x7ff, 1,
+					MT9M111_RED_GAIN_DEFAULT);
+	sd->blue_bal = v4l2_ctrl_new_std(hdl, &mt9m111_ctrl_ops,
+					V4L2_CID_BLUE_BALANCE, 0, 0x7ff, 1,
+					MT9M111_BLUE_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std(hdl, &mt9m111_ctrl_ops, V4L2_CID_GAIN, 0,
+			  (INITIAL_MAX_GAIN - 1) * 2 * 2 * 2, 1,
+			  MT9M111_DEFAULT_GAIN);
+
+	sd->hflip = v4l2_ctrl_new_std(hdl, &mt9m111_ctrl_ops, V4L2_CID_HFLIP,
+				      0, 1, 1, 0);
+	sd->vflip = v4l2_ctrl_new_std(hdl, &mt9m111_ctrl_ops, V4L2_CID_VFLIP,
+				      0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 
-	err = mt9m111_set_red_balance(&sd->gspca_dev,
-					sensor_settings[RED_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+	v4l2_ctrl_auto_cluster(4, &sd->auto_white_bal, 0, false);
+	v4l2_ctrl_cluster(2, &sd->hflip);
 
-	return mt9m111_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
+	return 0;
 }
 
 int mt9m111_start(struct sd *sd)
@@ -272,7 +170,7 @@ int mt9m111_start(struct sd *sd)
 	int i, err = 0;
 	u8 data[2];
 	struct cam *cam = &sd->gspca_dev.cam;
-	s32 *sensor_settings = sd->sensor_priv;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	int width = cam->cam_mode[sd->gspca_dev.curr_mode].width - 1;
 	int height = cam->cam_mode[sd->gspca_dev.curr_mode].height;
@@ -333,26 +231,11 @@ int mt9m111_start(struct sd *sd)
 
 	switch (width) {
 	case 640:
-		PDEBUG(D_V4L2, "Configuring camera for VGA mode");
-		data[0] = MT9M111_RMB_OVER_SIZED;
-		data[1] = MT9M111_RMB_ROW_SKIP_2X |
-			  MT9M111_RMB_COLUMN_SKIP_2X |
-			  (sensor_settings[VFLIP_IDX] << 0) |
-			  (sensor_settings[HFLIP_IDX] << 1);
-
-		err = m5602_write_sensor(sd,
-					 MT9M111_SC_R_MODE_CONTEXT_B, data, 2);
+		PDEBUG(D_CONF, "Configuring camera for VGA mode");
 		break;
 
 	case 320:
-		PDEBUG(D_V4L2, "Configuring camera for QVGA mode");
-		data[0] = MT9M111_RMB_OVER_SIZED;
-		data[1] = MT9M111_RMB_ROW_SKIP_4X |
-				MT9M111_RMB_COLUMN_SKIP_4X |
-				(sensor_settings[VFLIP_IDX] << 0) |
-				(sensor_settings[HFLIP_IDX] << 1);
-		err = m5602_write_sensor(sd,
-					 MT9M111_SC_R_MODE_CONTEXT_B, data, 2);
+		PDEBUG(D_CONF, "Configuring camera for QVGA mode");
 		break;
 	}
 	return err;
@@ -361,105 +244,46 @@ int mt9m111_start(struct sd *sd)
 void mt9m111_disconnect(struct sd *sd)
 {
 	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
-}
-
-static int mt9m111_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[VFLIP_IDX];
-	PDEBUG(D_V4L2, "Read vertical flip %d", *val);
-
-	return 0;
-}
-
-static int mt9m111_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	int err;
-	u8 data[2] = {0x00, 0x00};
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	PDEBUG(D_V4L2, "Set vertical flip to %d", val);
-
-	sensor_settings[VFLIP_IDX] = val;
-
-	/* The mt9m111 is flipped by default */
-	val = !val;
-
-	/* Set the correct page map */
-	err = m5602_write_sensor(sd, MT9M111_PAGE_MAP, data, 2);
-	if (err < 0)
-		return err;
-
-	err = m5602_read_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B, data, 2);
-	if (err < 0)
-		return err;
-
-	data[1] = (data[1] & 0xfe) | val;
-	err = m5602_write_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
-				   data, 2);
-	return err;
-}
-
-static int mt9m111_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[HFLIP_IDX];
-	PDEBUG(D_V4L2, "Read horizontal flip %d", *val);
-
-	return 0;
 }
 
-static int mt9m111_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
+static int mt9m111_set_hvflip(struct gspca_dev *gspca_dev)
 {
 	int err;
 	u8 data[2] = {0x00, 0x00};
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	int hflip;
+	int vflip;
 
-	PDEBUG(D_V4L2, "Set horizontal flip to %d", val);
-
-	sensor_settings[HFLIP_IDX] = val;
+	PDEBUG(D_CONF, "Set hvflip to %d %d", sd->hflip->val, sd->vflip->val);
 
 	/* The mt9m111 is flipped by default */
-	val = !val;
+	hflip = !sd->hflip->val;
+	vflip = !sd->vflip->val;
 
 	/* Set the correct page map */
 	err = m5602_write_sensor(sd, MT9M111_PAGE_MAP, data, 2);
 	if (err < 0)
 		return err;
 
-	err = m5602_read_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B, data, 2);
-	if (err < 0)
-		return err;
-
-	data[1] = (data[1] & 0xfd) | ((val << 1) & 0x02);
+	data[0] = MT9M111_RMB_OVER_SIZED;
+	if (gspca_dev->width == 640) {
+		data[1] = MT9M111_RMB_ROW_SKIP_2X |
+			  MT9M111_RMB_COLUMN_SKIP_2X |
+			  (hflip << 1) | vflip;
+	} else {
+		data[1] = MT9M111_RMB_ROW_SKIP_4X |
+			  MT9M111_RMB_COLUMN_SKIP_4X |
+			  (hflip << 1) | vflip;
+	}
 	err = m5602_write_sensor(sd, MT9M111_SC_R_MODE_CONTEXT_B,
 					data, 2);
 	return err;
 }
 
-static int mt9m111_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[GAIN_IDX];
-	PDEBUG(D_V4L2, "Read gain %d", *val);
-
-	return 0;
-}
-
 static int mt9m111_set_auto_white_balance(struct gspca_dev *gspca_dev,
 					  __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	int err;
 	u8 data[2];
 
@@ -467,33 +291,19 @@ static int mt9m111_set_auto_white_balance(struct gspca_dev *gspca_dev,
 	if (err < 0)
 		return err;
 
-	sensor_settings[AUTO_WHITE_BALANCE_IDX] = val & 0x01;
 	data[1] = ((data[1] & 0xfd) | ((val & 0x01) << 1));
 
 	err = m5602_write_sensor(sd, MT9M111_CP_OPERATING_MODE_CTL, data, 2);
 
-	PDEBUG(D_V4L2, "Set auto white balance %d", val);
+	PDEBUG(D_CONF, "Set auto white balance %d", val);
 	return err;
 }
 
-static int mt9m111_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					  __s32 *val) {
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_WHITE_BALANCE_IDX];
-	PDEBUG(D_V4L2, "Read auto white balance %d", *val);
-	return 0;
-}
-
 static int mt9m111_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err, tmp;
 	u8 data[2] = {0x00, 0x00};
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	sensor_settings[GAIN_IDX] = val;
 
 	/* Set the correct page map */
 	err = m5602_write_sensor(sd, MT9M111_PAGE_MAP, data, 2);
@@ -518,7 +328,7 @@ static int mt9m111_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 
 	data[1] = (tmp & 0xff);
 	data[0] = (tmp & 0xff00) >> 8;
-	PDEBUG(D_V4L2, "tmp=%d, data[1]=%d, data[0]=%d", tmp,
+	PDEBUG(D_CONF, "tmp=%d, data[1]=%d, data[0]=%d", tmp,
 	       data[1], data[0]);
 
 	err = m5602_write_sensor(sd, MT9M111_SC_GLOBAL_GAIN,
@@ -532,13 +342,11 @@ static int mt9m111_set_green_balance(struct gspca_dev *gspca_dev, __s32 val)
 	int err;
 	u8 data[2];
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	sensor_settings[GREEN_BALANCE_IDX] = val;
 	data[1] = (val & 0xff);
 	data[0] = (val & 0xff00) >> 8;
 
-	PDEBUG(D_V4L2, "Set green balance %d", val);
+	PDEBUG(D_CONF, "Set green balance %d", val);
 	err = m5602_write_sensor(sd, MT9M111_SC_GREEN_1_GAIN,
 				 data, 2);
 	if (err < 0)
@@ -548,66 +356,68 @@ static int mt9m111_set_green_balance(struct gspca_dev *gspca_dev, __s32 val)
 				  data, 2);
 }
 
-static int mt9m111_get_green_balance(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[GREEN_BALANCE_IDX];
-	PDEBUG(D_V4L2, "Read green balance %d", *val);
-	return 0;
-}
-
 static int mt9m111_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	u8 data[2];
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	sensor_settings[BLUE_BALANCE_IDX] = val;
 	data[1] = (val & 0xff);
 	data[0] = (val & 0xff00) >> 8;
 
-	PDEBUG(D_V4L2, "Set blue balance %d", val);
+	PDEBUG(D_CONF, "Set blue balance %d", val);
 
 	return m5602_write_sensor(sd, MT9M111_SC_BLUE_GAIN,
 				  data, 2);
 }
 
-static int mt9m111_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[BLUE_BALANCE_IDX];
-	PDEBUG(D_V4L2, "Read blue balance %d", *val);
-	return 0;
-}
-
 static int mt9m111_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	u8 data[2];
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	sensor_settings[RED_BALANCE_IDX] = val;
 	data[1] = (val & 0xff);
 	data[0] = (val & 0xff00) >> 8;
 
-	PDEBUG(D_V4L2, "Set red balance %d", val);
+	PDEBUG(D_CONF, "Set red balance %d", val);
 
 	return m5602_write_sensor(sd, MT9M111_SC_RED_GAIN,
 				  data, 2);
 }
 
-static int mt9m111_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val)
+static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	int err;
 
-	*val = sensor_settings[RED_BALANCE_IDX];
-	PDEBUG(D_V4L2, "Read red balance %d", *val);
-	return 0;
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		err = mt9m111_set_auto_white_balance(gspca_dev, ctrl->val);
+		if (err || ctrl->val)
+			return err;
+		err = mt9m111_set_green_balance(gspca_dev, sd->green_bal->val);
+		if (err)
+			return err;
+		err = mt9m111_set_red_balance(gspca_dev, sd->red_bal->val);
+		if (err)
+			return err;
+		err = mt9m111_set_blue_balance(gspca_dev, sd->blue_bal->val);
+		break;
+	case V4L2_CID_GAIN:
+		err = mt9m111_set_gain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		err = mt9m111_set_hvflip(gspca_dev);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return err;
 }
 
 static void mt9m111_dump_registers(struct sd *sd)
diff --git a/drivers/media/usb/gspca/m5602/m5602_mt9m111.h b/drivers/media/usb/gspca/m5602/m5602_mt9m111.h
index 8c672b5c8c6a..07448d35e3cd 100644
--- a/drivers/media/usb/gspca/m5602/m5602_mt9m111.h
+++ b/drivers/media/usb/gspca/m5602/m5602_mt9m111.h
@@ -110,6 +110,7 @@ extern bool dump_sensor;
 
 int mt9m111_probe(struct sd *sd);
 int mt9m111_init(struct sd *sd);
+int mt9m111_init_controls(struct sd *sd);
 int mt9m111_start(struct sd *sd);
 void mt9m111_disconnect(struct sd *sd);
 
@@ -121,6 +122,7 @@ static const struct m5602_sensor mt9m111 = {
 
 	.probe = mt9m111_probe,
 	.init = mt9m111_init,
+	.init_controls = mt9m111_init_controls,
 	.disconnect = mt9m111_disconnect,
 	.start = mt9m111_start,
 };
diff --git a/drivers/media/usb/gspca/m5602/m5602_ov7660.c b/drivers/media/usb/gspca/m5602/m5602_ov7660.c
index 9a14835c128f..64b3b03a9141 100644
--- a/drivers/media/usb/gspca/m5602/m5602_ov7660.c
+++ b/drivers/media/usb/gspca/m5602/m5602_ov7660.c
@@ -20,111 +20,8 @@
 
 #include "m5602_ov7660.h"
 
-static int ov7660_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov7660_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int ov7660_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 *val);
-static int ov7660_set_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 val);
-static int ov7660_get_auto_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov7660_set_auto_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int ov7660_get_auto_exposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov7660_set_auto_exposure(struct gspca_dev *gspca_dev, __s32 val);
-static int ov7660_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov7660_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
-static int ov7660_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov7660_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
-
-static const struct ctrl ov7660_ctrls[] = {
-#define GAIN_IDX 1
-	{
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "gain",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= OV7660_DEFAULT_GAIN,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = ov7660_set_gain,
-		.get = ov7660_get_gain
-	},
-#define BLUE_BALANCE_IDX 2
-#define RED_BALANCE_IDX 3
-#define AUTO_WHITE_BALANCE_IDX 4
-	{
-		{
-			.id		= V4L2_CID_AUTO_WHITE_BALANCE,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto white balance",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 1
-		},
-		.set = ov7660_set_auto_white_balance,
-		.get = ov7660_get_auto_white_balance
-	},
-#define AUTO_GAIN_CTRL_IDX 5
-	{
-		{
-			.id		= V4L2_CID_AUTOGAIN,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto gain control",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 1
-		},
-		.set = ov7660_set_auto_gain,
-		.get = ov7660_get_auto_gain
-	},
-#define AUTO_EXPOSURE_IDX 6
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE_AUTO,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto exposure",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 1
-		},
-		.set = ov7660_set_auto_exposure,
-		.get = ov7660_get_auto_exposure
-	},
-#define HFLIP_IDX 7
-	{
-		{
-			.id		= V4L2_CID_HFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "horizontal flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = ov7660_set_hflip,
-		.get = ov7660_get_hflip
-	},
-#define VFLIP_IDX 8
-	{
-		{
-			.id		= V4L2_CID_VFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "vertical flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = ov7660_set_vflip,
-		.get = ov7660_get_vflip
-	},
-
-};
+static int ov7660_s_ctrl(struct v4l2_ctrl *ctrl);
+static void ov7660_dump_registers(struct sd *sd);
 
 static struct v4l2_pix_format ov7660_modes[] = {
 	{
@@ -140,15 +37,15 @@ static struct v4l2_pix_format ov7660_modes[] = {
 	}
 };
 
-static void ov7660_dump_registers(struct sd *sd);
+static const struct v4l2_ctrl_ops ov7660_ctrl_ops = {
+	.s_ctrl = ov7660_s_ctrl,
+};
 
 int ov7660_probe(struct sd *sd)
 {
 	int err = 0, i;
 	u8 prod_id = 0, ver_id = 0;
 
-	s32 *sensor_settings;
-
 	if (force_sensor) {
 		if (force_sensor == OV7660_SENSOR) {
 			pr_info("Forcing an %s sensor\n", ov7660.name);
@@ -191,27 +88,15 @@ int ov7660_probe(struct sd *sd)
 	return -ENODEV;
 
 sensor_found:
-	sensor_settings = kmalloc(
-		ARRAY_SIZE(ov7660_ctrls) * sizeof(s32), GFP_KERNEL);
-	if (!sensor_settings)
-		return -ENOMEM;
-
 	sd->gspca_dev.cam.cam_mode = ov7660_modes;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(ov7660_modes);
-	sd->desc->ctrls = ov7660_ctrls;
-	sd->desc->nctrls = ARRAY_SIZE(ov7660_ctrls);
-
-	for (i = 0; i < ARRAY_SIZE(ov7660_ctrls); i++)
-		sensor_settings[i] = ov7660_ctrls[i].qctrl.default_value;
-	sd->sensor_priv = sensor_settings;
 
 	return 0;
 }
 
 int ov7660_init(struct sd *sd)
 {
-	int i, err = 0;
-	s32 *sensor_settings = sd->sensor_priv;
+	int i, err;
 
 	/* Init the sensor */
 	for (i = 0; i < ARRAY_SIZE(init_ov7660); i++) {
@@ -226,38 +111,47 @@ int ov7660_init(struct sd *sd)
 			err = m5602_write_sensor(sd,
 				init_ov7660[i][1], data, 1);
 		}
+		if (err < 0)
+			return err;
 	}
 
 	if (dump_sensor)
 		ov7660_dump_registers(sd);
 
-	err = ov7660_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
-	if (err < 0)
-		return err;
+	return 0;
+}
 
-	err = ov7660_set_auto_white_balance(&sd->gspca_dev,
-		sensor_settings[AUTO_WHITE_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+int ov7660_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
 
-	err = ov7660_set_auto_gain(&sd->gspca_dev,
-		sensor_settings[AUTO_GAIN_CTRL_IDX]);
-	if (err < 0)
-		return err;
+	sd->gspca_dev.vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 6);
 
-	err = ov7660_set_auto_exposure(&sd->gspca_dev,
-		sensor_settings[AUTO_EXPOSURE_IDX]);
-	if (err < 0)
-		return err;
-	err = ov7660_set_hflip(&sd->gspca_dev,
-		sensor_settings[HFLIP_IDX]);
-	if (err < 0)
-		return err;
+	v4l2_ctrl_new_std(hdl, &ov7660_ctrl_ops, V4L2_CID_AUTO_WHITE_BALANCE,
+			  0, 1, 1, 1);
+	v4l2_ctrl_new_std_menu(hdl, &ov7660_ctrl_ops,
+			  V4L2_CID_EXPOSURE_AUTO, 1, 0, V4L2_EXPOSURE_AUTO);
 
-	err = ov7660_set_vflip(&sd->gspca_dev,
-		sensor_settings[VFLIP_IDX]);
+	sd->autogain = v4l2_ctrl_new_std(hdl, &ov7660_ctrl_ops,
+					 V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	sd->gain = v4l2_ctrl_new_std(hdl, &ov7660_ctrl_ops, V4L2_CID_GAIN, 0,
+				     255, 1, OV7660_DEFAULT_GAIN);
 
-	return err;
+	sd->hflip = v4l2_ctrl_new_std(hdl, &ov7660_ctrl_ops, V4L2_CID_HFLIP,
+				      0, 1, 1, 0);
+	sd->vflip = v4l2_ctrl_new_std(hdl, &ov7660_ctrl_ops, V4L2_CID_VFLIP,
+				      0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+
+	v4l2_ctrl_auto_cluster(2, &sd->autogain, 0, false);
+	v4l2_ctrl_cluster(2, &sd->hflip);
+
+	return 0;
 }
 
 int ov7660_start(struct sd *sd)
@@ -275,56 +169,29 @@ void ov7660_disconnect(struct sd *sd)
 	ov7660_stop(sd);
 
 	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
-}
-
-static int ov7660_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[GAIN_IDX];
-	PDEBUG(D_V4L2, "Read gain %d", *val);
-	return 0;
 }
 
 static int ov7660_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
-	u8 i2c_data;
+	u8 i2c_data = val;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Setting gain to %d", val);
-
-	sensor_settings[GAIN_IDX] = val;
+	PDEBUG(D_CONF, "Setting gain to %d", val);
 
 	err = m5602_write_sensor(sd, OV7660_GAIN, &i2c_data, 1);
 	return err;
 }
 
-
-static int ov7660_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_WHITE_BALANCE_IDX];
-	return 0;
-}
-
 static int ov7660_set_auto_white_balance(struct gspca_dev *gspca_dev,
 					 __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set auto white balance to %d", val);
+	PDEBUG(D_CONF, "Set auto white balance to %d", val);
 
-	sensor_settings[AUTO_WHITE_BALANCE_IDX] = val;
 	err = m5602_read_sensor(sd, OV7660_COM8, &i2c_data, 1);
 	if (err < 0)
 		return err;
@@ -335,26 +202,14 @@ static int ov7660_set_auto_white_balance(struct gspca_dev *gspca_dev,
 	return err;
 }
 
-static int ov7660_get_auto_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_GAIN_CTRL_IDX];
-	PDEBUG(D_V4L2, "Read auto gain control %d", *val);
-	return 0;
-}
-
 static int ov7660_set_auto_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set auto gain control to %d", val);
+	PDEBUG(D_CONF, "Set auto gain control to %d", val);
 
-	sensor_settings[AUTO_GAIN_CTRL_IDX] = val;
 	err = m5602_read_sensor(sd, OV7660_COM8, &i2c_data, 1);
 	if (err < 0)
 		return err;
@@ -364,94 +219,69 @@ static int ov7660_set_auto_gain(struct gspca_dev *gspca_dev, __s32 val)
 	return m5602_write_sensor(sd, OV7660_COM8, &i2c_data, 1);
 }
 
-static int ov7660_get_auto_exposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_EXPOSURE_IDX];
-	PDEBUG(D_V4L2, "Read auto exposure control %d", *val);
-	return 0;
-}
-
 static int ov7660_set_auto_exposure(struct gspca_dev *gspca_dev,
 				    __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set auto exposure control to %d", val);
+	PDEBUG(D_CONF, "Set auto exposure control to %d", val);
 
-	sensor_settings[AUTO_EXPOSURE_IDX] = val;
 	err = m5602_read_sensor(sd, OV7660_COM8, &i2c_data, 1);
 	if (err < 0)
 		return err;
 
+	val = (val == V4L2_EXPOSURE_AUTO);
 	i2c_data = ((i2c_data & 0xfe) | ((val & 0x01) << 0));
 
 	return m5602_write_sensor(sd, OV7660_COM8, &i2c_data, 1);
 }
 
-static int ov7660_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[HFLIP_IDX];
-	PDEBUG(D_V4L2, "Read horizontal flip %d", *val);
-	return 0;
-}
-
-static int ov7660_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
+static int ov7660_set_hvflip(struct gspca_dev *gspca_dev)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set horizontal flip to %d", val);
+	PDEBUG(D_CONF, "Set hvflip to %d, %d", sd->hflip->val, sd->vflip->val);
 
-	sensor_settings[HFLIP_IDX] = val;
-
-	i2c_data = ((val & 0x01) << 5) |
-		(sensor_settings[VFLIP_IDX] << 4);
+	i2c_data = (sd->hflip->val << 5) | (sd->vflip->val << 4);
 
 	err = m5602_write_sensor(sd, OV7660_MVFP, &i2c_data, 1);
 
 	return err;
 }
 
-static int ov7660_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
+static int ov7660_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[VFLIP_IDX];
-	PDEBUG(D_V4L2, "Read vertical flip %d", *val);
-
-	return 0;
-}
-
-static int ov7660_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
-{
 	int err;
-	u8 i2c_data;
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	PDEBUG(D_V4L2, "Set vertical flip to %d", val);
-	sensor_settings[VFLIP_IDX] = val;
 
-	i2c_data = ((val & 0x01) << 4) | (sensor_settings[VFLIP_IDX] << 5);
-	err = m5602_write_sensor(sd, OV7660_MVFP, &i2c_data, 1);
-	if (err < 0)
-		return err;
-
-	/* When vflip is toggled we need to readjust the bridge hsync/vsync */
-	if (gspca_dev->streaming)
-		err = ov7660_start(sd);
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		err = ov7660_set_auto_white_balance(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		err = ov7660_set_auto_exposure(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		err = ov7660_set_auto_gain(gspca_dev, ctrl->val);
+		if (err || ctrl->val)
+			return err;
+		err = ov7660_set_gain(gspca_dev, sd->gain->val);
+		break;
+	case V4L2_CID_HFLIP:
+		err = ov7660_set_hvflip(gspca_dev);
+		break;
+	default:
+		return -EINVAL;
+	}
 
 	return err;
 }
diff --git a/drivers/media/usb/gspca/m5602/m5602_ov7660.h b/drivers/media/usb/gspca/m5602/m5602_ov7660.h
index 2b6a13b508f7..6fece1ce1232 100644
--- a/drivers/media/usb/gspca/m5602/m5602_ov7660.h
+++ b/drivers/media/usb/gspca/m5602/m5602_ov7660.h
@@ -90,6 +90,8 @@ extern bool dump_sensor;
 
 int ov7660_probe(struct sd *sd);
 int ov7660_init(struct sd *sd);
+int ov7660_init(struct sd *sd);
+int ov7660_init_controls(struct sd *sd);
 int ov7660_start(struct sd *sd);
 int ov7660_stop(struct sd *sd);
 void ov7660_disconnect(struct sd *sd);
@@ -100,6 +102,7 @@ static const struct m5602_sensor ov7660 = {
 	.i2c_regW = 1,
 	.probe = ov7660_probe,
 	.init = ov7660_init,
+	.init_controls = ov7660_init_controls,
 	.start = ov7660_start,
 	.stop = ov7660_stop,
 	.disconnect = ov7660_disconnect,
diff --git a/drivers/media/usb/gspca/m5602/m5602_ov9650.c b/drivers/media/usb/gspca/m5602/m5602_ov9650.c
index 2114a8b90ec9..59bc62bfae26 100644
--- a/drivers/media/usb/gspca/m5602/m5602_ov9650.c
+++ b/drivers/media/usb/gspca/m5602/m5602_ov9650.c
@@ -20,26 +20,8 @@
 
 #include "m5602_ov9650.h"
 
-static int ov9650_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
-static int ov9650_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int ov9650_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_set_red_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int ov9650_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int ov9650_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
-static int ov9650_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
-static int ov9650_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 *val);
-static int ov9650_set_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 val);
-static int ov9650_get_auto_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_set_auto_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int ov9650_get_auto_exposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int ov9650_set_auto_exposure(struct gspca_dev *gspca_dev, __s32 val);
+static int ov9650_s_ctrl(struct v4l2_ctrl *ctrl);
+static void ov9650_dump_registers(struct sd *sd);
 
 /* Vertically and horizontally flips the image if matched, needed for machines
    where the sensor is mounted upside down */
@@ -113,140 +95,6 @@ static
 	{}
 };
 
-static const struct ctrl ov9650_ctrls[] = {
-#define EXPOSURE_IDX 0
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "exposure",
-			.minimum	= 0x00,
-			.maximum	= 0x1ff,
-			.step		= 0x4,
-			.default_value	= EXPOSURE_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = ov9650_set_exposure,
-		.get = ov9650_get_exposure
-	},
-#define GAIN_IDX 1
-	{
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "gain",
-			.minimum	= 0x00,
-			.maximum	= 0x3ff,
-			.step		= 0x1,
-			.default_value	= GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = ov9650_set_gain,
-		.get = ov9650_get_gain
-	},
-#define RED_BALANCE_IDX 2
-	{
-		{
-			.id		= V4L2_CID_RED_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "red balance",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= RED_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = ov9650_set_red_balance,
-		.get = ov9650_get_red_balance
-	},
-#define BLUE_BALANCE_IDX 3
-	{
-		{
-			.id		= V4L2_CID_BLUE_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "blue balance",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= BLUE_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = ov9650_set_blue_balance,
-		.get = ov9650_get_blue_balance
-	},
-#define HFLIP_IDX 4
-	{
-		{
-			.id		= V4L2_CID_HFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "horizontal flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = ov9650_set_hflip,
-		.get = ov9650_get_hflip
-	},
-#define VFLIP_IDX 5
-	{
-		{
-			.id		= V4L2_CID_VFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "vertical flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = ov9650_set_vflip,
-		.get = ov9650_get_vflip
-	},
-#define AUTO_WHITE_BALANCE_IDX 6
-	{
-		{
-			.id		= V4L2_CID_AUTO_WHITE_BALANCE,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto white balance",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 1
-		},
-		.set = ov9650_set_auto_white_balance,
-		.get = ov9650_get_auto_white_balance
-	},
-#define AUTO_GAIN_CTRL_IDX 7
-	{
-		{
-			.id		= V4L2_CID_AUTOGAIN,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto gain control",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 1
-		},
-		.set = ov9650_set_auto_gain,
-		.get = ov9650_get_auto_gain
-	},
-#define AUTO_EXPOSURE_IDX 8
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE_AUTO,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto exposure",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 1
-		},
-		.set = ov9650_set_auto_exposure,
-		.get = ov9650_get_auto_exposure
-	}
-
-};
-
 static struct v4l2_pix_format ov9650_modes[] = {
 	{
 		176,
@@ -291,13 +139,15 @@ static struct v4l2_pix_format ov9650_modes[] = {
 	}
 };
 
-static void ov9650_dump_registers(struct sd *sd);
+static const struct v4l2_ctrl_ops ov9650_ctrl_ops = {
+	.s_ctrl = ov9650_s_ctrl,
+};
 
 int ov9650_probe(struct sd *sd)
 {
 	int err = 0;
 	u8 prod_id = 0, ver_id = 0, i;
-	s32 *sensor_settings;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	if (force_sensor) {
 		if (force_sensor == OV9650_SENSOR) {
@@ -338,19 +188,9 @@ int ov9650_probe(struct sd *sd)
 	return -ENODEV;
 
 sensor_found:
-	sensor_settings = kmalloc(
-		ARRAY_SIZE(ov9650_ctrls) * sizeof(s32), GFP_KERNEL);
-	if (!sensor_settings)
-		return -ENOMEM;
-
 	sd->gspca_dev.cam.cam_mode = ov9650_modes;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(ov9650_modes);
-	sd->desc->ctrls = ov9650_ctrls;
-	sd->desc->nctrls = ARRAY_SIZE(ov9650_ctrls);
 
-	for (i = 0; i < ARRAY_SIZE(ov9650_ctrls); i++)
-		sensor_settings[i] = ov9650_ctrls[i].qctrl.default_value;
-	sd->sensor_priv = sensor_settings;
 	return 0;
 }
 
@@ -358,7 +198,6 @@ int ov9650_init(struct sd *sd)
 {
 	int i, err = 0;
 	u8 data;
-	s32 *sensor_settings = sd->sensor_priv;
 
 	if (dump_sensor)
 		ov9650_dump_registers(sd);
@@ -372,46 +211,52 @@ int ov9650_init(struct sd *sd)
 			err = m5602_write_bridge(sd, init_ov9650[i][1], data);
 	}
 
-	err = ov9650_set_exposure(&sd->gspca_dev,
-				   sensor_settings[EXPOSURE_IDX]);
-	if (err < 0)
-		return err;
-
-	err = ov9650_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
-	if (err < 0)
-		return err;
-
-	err = ov9650_set_red_balance(&sd->gspca_dev,
-				      sensor_settings[RED_BALANCE_IDX]);
-	if (err < 0)
-		return err;
-
-	err = ov9650_set_blue_balance(&sd->gspca_dev,
-				       sensor_settings[BLUE_BALANCE_IDX]);
-	if (err < 0)
-		return err;
-
-	err = ov9650_set_hflip(&sd->gspca_dev, sensor_settings[HFLIP_IDX]);
-	if (err < 0)
-		return err;
-
-	err = ov9650_set_vflip(&sd->gspca_dev, sensor_settings[VFLIP_IDX]);
-	if (err < 0)
-		return err;
+	return 0;
+}
 
-	err = ov9650_set_auto_exposure(&sd->gspca_dev,
-				sensor_settings[AUTO_EXPOSURE_IDX]);
-	if (err < 0)
-		return err;
+int ov9650_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+
+	sd->gspca_dev.vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 9);
+
+	sd->auto_white_bal = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops,
+					       V4L2_CID_AUTO_WHITE_BALANCE,
+					       0, 1, 1, 1);
+	sd->red_bal = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops,
+					V4L2_CID_RED_BALANCE, 0, 255, 1,
+					RED_GAIN_DEFAULT);
+	sd->blue_bal = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops,
+					V4L2_CID_BLUE_BALANCE, 0, 255, 1,
+					BLUE_GAIN_DEFAULT);
+
+	sd->autoexpo = v4l2_ctrl_new_std_menu(hdl, &ov9650_ctrl_ops,
+			  V4L2_CID_EXPOSURE_AUTO, 1, 0, V4L2_EXPOSURE_AUTO);
+	sd->expo = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops, V4L2_CID_EXPOSURE,
+			  0, 0x1ff, 4, EXPOSURE_DEFAULT);
+
+	sd->autogain = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops,
+					 V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	sd->gain = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops, V4L2_CID_GAIN, 0,
+				     0x3ff, 1, GAIN_DEFAULT);
+
+	sd->hflip = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops, V4L2_CID_HFLIP,
+				      0, 1, 1, 0);
+	sd->vflip = v4l2_ctrl_new_std(hdl, &ov9650_ctrl_ops, V4L2_CID_VFLIP,
+				      0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 
-	err = ov9650_set_auto_white_balance(&sd->gspca_dev,
-				sensor_settings[AUTO_WHITE_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+	v4l2_ctrl_auto_cluster(3, &sd->auto_white_bal, 0, false);
+	v4l2_ctrl_auto_cluster(2, &sd->autoexpo, 0, false);
+	v4l2_ctrl_auto_cluster(2, &sd->autogain, 0, false);
+	v4l2_ctrl_cluster(2, &sd->hflip);
 
-	err = ov9650_set_auto_gain(&sd->gspca_dev,
-				sensor_settings[AUTO_GAIN_CTRL_IDX]);
-	return err;
+	return 0;
 }
 
 int ov9650_start(struct sd *sd)
@@ -419,17 +264,17 @@ int ov9650_start(struct sd *sd)
 	u8 data;
 	int i, err = 0;
 	struct cam *cam = &sd->gspca_dev.cam;
-	s32 *sensor_settings = sd->sensor_priv;
 
 	int width = cam->cam_mode[sd->gspca_dev.curr_mode].width;
 	int height = cam->cam_mode[sd->gspca_dev.curr_mode].height;
 	int ver_offs = cam->cam_mode[sd->gspca_dev.curr_mode].priv;
 	int hor_offs = OV9650_LEFT_OFFSET;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	if ((!dmi_check_system(ov9650_flip_dmi_table) &&
-		sensor_settings[VFLIP_IDX]) ||
+		sd->vflip->val) ||
 		(dmi_check_system(ov9650_flip_dmi_table) &&
-		!sensor_settings[VFLIP_IDX]))
+		!sd->vflip->val))
 		ver_offs--;
 
 	if (width <= 320)
@@ -508,7 +353,7 @@ int ov9650_start(struct sd *sd)
 
 	switch (width) {
 	case 640:
-		PDEBUG(D_V4L2, "Configuring camera for VGA mode");
+		PDEBUG(D_CONF, "Configuring camera for VGA mode");
 
 		data = OV9650_VGA_SELECT | OV9650_RGB_SELECT |
 		       OV9650_RAW_RGB_SELECT;
@@ -516,7 +361,7 @@ int ov9650_start(struct sd *sd)
 		break;
 
 	case 352:
-		PDEBUG(D_V4L2, "Configuring camera for CIF mode");
+		PDEBUG(D_CONF, "Configuring camera for CIF mode");
 
 		data = OV9650_CIF_SELECT | OV9650_RGB_SELECT |
 				OV9650_RAW_RGB_SELECT;
@@ -524,7 +369,7 @@ int ov9650_start(struct sd *sd)
 		break;
 
 	case 320:
-		PDEBUG(D_V4L2, "Configuring camera for QVGA mode");
+		PDEBUG(D_CONF, "Configuring camera for QVGA mode");
 
 		data = OV9650_QVGA_SELECT | OV9650_RGB_SELECT |
 				OV9650_RAW_RGB_SELECT;
@@ -532,7 +377,7 @@ int ov9650_start(struct sd *sd)
 		break;
 
 	case 176:
-		PDEBUG(D_V4L2, "Configuring camera for QCIF mode");
+		PDEBUG(D_CONF, "Configuring camera for QCIF mode");
 
 		data = OV9650_QCIF_SELECT | OV9650_RGB_SELECT |
 			OV9650_RAW_RGB_SELECT;
@@ -553,29 +398,16 @@ void ov9650_disconnect(struct sd *sd)
 	ov9650_stop(sd);
 
 	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
-}
-
-static int ov9650_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[EXPOSURE_IDX];
-	PDEBUG(D_V4L2, "Read exposure %d", *val);
-	return 0;
 }
 
 static int ov9650_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	PDEBUG(D_V4L2, "Set exposure to %d", val);
+	PDEBUG(D_CONF, "Set exposure to %d", val);
 
-	sensor_settings[EXPOSURE_IDX] = val;
 	/* The 6 MSBs */
 	i2c_data = (val >> 10) & 0x3f;
 	err = m5602_write_sensor(sd, OV9650_AECHM,
@@ -596,26 +428,13 @@ static int ov9650_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int ov9650_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[GAIN_IDX];
-	PDEBUG(D_V4L2, "Read gain %d", *val);
-	return 0;
-}
-
 static int ov9650_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Setting gain to %d", val);
-
-	sensor_settings[GAIN_IDX] = val;
+	PDEBUG(D_CONF, "Setting gain to %d", val);
 
 	/* The 2 MSB */
 	/* Read the OV9650_VREF register first to avoid
@@ -637,117 +456,46 @@ static int ov9650_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int ov9650_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[RED_BALANCE_IDX];
-	PDEBUG(D_V4L2, "Read red gain %d", *val);
-	return 0;
-}
-
 static int ov9650_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set red gain to %d", val);
-
-	sensor_settings[RED_BALANCE_IDX] = val;
+	PDEBUG(D_CONF, "Set red gain to %d", val);
 
 	i2c_data = val & 0xff;
 	err = m5602_write_sensor(sd, OV9650_RED, &i2c_data, 1);
 	return err;
 }
 
-static int ov9650_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[BLUE_BALANCE_IDX];
-	PDEBUG(D_V4L2, "Read blue gain %d", *val);
-
-	return 0;
-}
-
 static int ov9650_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	PDEBUG(D_V4L2, "Set blue gain to %d", val);
 
-	sensor_settings[BLUE_BALANCE_IDX] = val;
+	PDEBUG(D_CONF, "Set blue gain to %d", val);
 
 	i2c_data = val & 0xff;
 	err = m5602_write_sensor(sd, OV9650_BLUE, &i2c_data, 1);
 	return err;
 }
 
-static int ov9650_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[HFLIP_IDX];
-	PDEBUG(D_V4L2, "Read horizontal flip %d", *val);
-	return 0;
-}
-
-static int ov9650_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
+static int ov9650_set_hvflip(struct gspca_dev *gspca_dev)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	int hflip = sd->hflip->val;
+	int vflip = sd->vflip->val;
 
-	PDEBUG(D_V4L2, "Set horizontal flip to %d", val);
-
-	sensor_settings[HFLIP_IDX] = val;
-
-	if (!dmi_check_system(ov9650_flip_dmi_table))
-		i2c_data = ((val & 0x01) << 5) |
-				(sensor_settings[VFLIP_IDX] << 4);
-	else
-		i2c_data = ((val & 0x01) << 5) |
-				(!sensor_settings[VFLIP_IDX] << 4);
-
-	err = m5602_write_sensor(sd, OV9650_MVFP, &i2c_data, 1);
-
-	return err;
-}
-
-static int ov9650_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[VFLIP_IDX];
-	PDEBUG(D_V4L2, "Read vertical flip %d", *val);
-
-	return 0;
-}
-
-static int ov9650_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	int err;
-	u8 i2c_data;
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	PDEBUG(D_V4L2, "Set vertical flip to %d", val);
-	sensor_settings[VFLIP_IDX] = val;
+	PDEBUG(D_CONF, "Set hvflip to %d %d", hflip, vflip);
 
 	if (dmi_check_system(ov9650_flip_dmi_table))
-		val = !val;
+		vflip = !vflip;
 
-	i2c_data = ((val & 0x01) << 4) | (sensor_settings[VFLIP_IDX] << 5);
+	i2c_data = (hflip << 5) | (vflip << 4);
 	err = m5602_write_sensor(sd, OV9650_MVFP, &i2c_data, 1);
 	if (err < 0)
 		return err;
@@ -759,57 +507,34 @@ static int ov9650_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int ov9650_get_auto_exposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_EXPOSURE_IDX];
-	PDEBUG(D_V4L2, "Read auto exposure control %d", *val);
-	return 0;
-}
-
 static int ov9650_set_auto_exposure(struct gspca_dev *gspca_dev,
 				    __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set auto exposure control to %d", val);
+	PDEBUG(D_CONF, "Set auto exposure control to %d", val);
 
-	sensor_settings[AUTO_EXPOSURE_IDX] = val;
 	err = m5602_read_sensor(sd, OV9650_COM8, &i2c_data, 1);
 	if (err < 0)
 		return err;
 
+	val = (val == V4L2_EXPOSURE_AUTO);
 	i2c_data = ((i2c_data & 0xfe) | ((val & 0x01) << 0));
 
 	return m5602_write_sensor(sd, OV9650_COM8, &i2c_data, 1);
 }
 
-static int ov9650_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_WHITE_BALANCE_IDX];
-	return 0;
-}
-
 static int ov9650_set_auto_white_balance(struct gspca_dev *gspca_dev,
 					 __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set auto white balance to %d", val);
+	PDEBUG(D_CONF, "Set auto white balance to %d", val);
 
-	sensor_settings[AUTO_WHITE_BALANCE_IDX] = val;
 	err = m5602_read_sensor(sd, OV9650_COM8, &i2c_data, 1);
 	if (err < 0)
 		return err;
@@ -820,26 +545,14 @@ static int ov9650_set_auto_white_balance(struct gspca_dev *gspca_dev,
 	return err;
 }
 
-static int ov9650_get_auto_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_GAIN_CTRL_IDX];
-	PDEBUG(D_V4L2, "Read auto gain control %d", *val);
-	return 0;
-}
-
 static int ov9650_set_auto_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u8 i2c_data;
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 
-	PDEBUG(D_V4L2, "Set auto gain control to %d", val);
+	PDEBUG(D_CONF, "Set auto gain control to %d", val);
 
-	sensor_settings[AUTO_GAIN_CTRL_IDX] = val;
 	err = m5602_read_sensor(sd, OV9650_COM8, &i2c_data, 1);
 	if (err < 0)
 		return err;
@@ -849,6 +562,48 @@ static int ov9650_set_auto_gain(struct gspca_dev *gspca_dev, __s32 val)
 	return m5602_write_sensor(sd, OV9650_COM8, &i2c_data, 1);
 }
 
+static int ov9650_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *) gspca_dev;
+	int err;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		err = ov9650_set_auto_white_balance(gspca_dev, ctrl->val);
+		if (err || ctrl->val)
+			return err;
+		err = ov9650_set_red_balance(gspca_dev, sd->red_bal->val);
+		if (err)
+			return err;
+		err = ov9650_set_blue_balance(gspca_dev, sd->blue_bal->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		err = ov9650_set_auto_exposure(gspca_dev, ctrl->val);
+		if (err || ctrl->val == V4L2_EXPOSURE_AUTO)
+			return err;
+		err = ov9650_set_exposure(gspca_dev, sd->expo->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		err = ov9650_set_auto_gain(gspca_dev, ctrl->val);
+		if (err || ctrl->val)
+			return err;
+		err = ov9650_set_gain(gspca_dev, sd->gain->val);
+		break;
+	case V4L2_CID_HFLIP:
+		err = ov9650_set_hvflip(gspca_dev);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return err;
+}
+
 static void ov9650_dump_registers(struct sd *sd)
 {
 	int address;
diff --git a/drivers/media/usb/gspca/m5602/m5602_ov9650.h b/drivers/media/usb/gspca/m5602/m5602_ov9650.h
index f7aa5bf68983..f9f5870da60f 100644
--- a/drivers/media/usb/gspca/m5602/m5602_ov9650.h
+++ b/drivers/media/usb/gspca/m5602/m5602_ov9650.h
@@ -139,6 +139,7 @@ extern bool dump_sensor;
 
 int ov9650_probe(struct sd *sd);
 int ov9650_init(struct sd *sd);
+int ov9650_init_controls(struct sd *sd);
 int ov9650_start(struct sd *sd);
 int ov9650_stop(struct sd *sd);
 void ov9650_disconnect(struct sd *sd);
@@ -149,6 +150,7 @@ static const struct m5602_sensor ov9650 = {
 	.i2c_regW = 1,
 	.probe = ov9650_probe,
 	.init = ov9650_init,
+	.init_controls = ov9650_init_controls,
 	.start = ov9650_start,
 	.stop = ov9650_stop,
 	.disconnect = ov9650_disconnect,
diff --git a/drivers/media/usb/gspca/m5602/m5602_po1030.c b/drivers/media/usb/gspca/m5602/m5602_po1030.c
index b8771698cbcb..4bf5c43424b7 100644
--- a/drivers/media/usb/gspca/m5602/m5602_po1030.c
+++ b/drivers/media/usb/gspca/m5602/m5602_po1030.c
@@ -20,28 +20,8 @@
 
 #include "m5602_po1030.h"
 
-static int po1030_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
-static int po1030_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int po1030_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int po1030_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int po1030_set_red_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int po1030_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int po1030_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int po1030_get_green_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int po1030_set_green_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int po1030_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int po1030_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
-static int po1030_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int po1030_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
-static int po1030_set_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 val);
-static int po1030_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 *val);
-static int po1030_set_auto_exposure(struct gspca_dev *gspca_dev,
-					 __s32 val);
-static int po1030_get_auto_exposure(struct gspca_dev *gspca_dev,
-					 __s32 *val);
+static int po1030_s_ctrl(struct v4l2_ctrl *ctrl);
+static void po1030_dump_registers(struct sd *sd);
 
 static struct v4l2_pix_format po1030_modes[] = {
 	{
@@ -56,146 +36,26 @@ static struct v4l2_pix_format po1030_modes[] = {
 	}
 };
 
-static const struct ctrl po1030_ctrls[] = {
-#define GAIN_IDX 0
-	{
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "gain",
-			.minimum	= 0x00,
-			.maximum	= 0x4f,
-			.step		= 0x1,
-			.default_value	= PO1030_GLOBAL_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = po1030_set_gain,
-		.get = po1030_get_gain
-	},
-#define EXPOSURE_IDX 1
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "exposure",
-			.minimum	= 0x00,
-			.maximum	= 0x02ff,
-			.step		= 0x1,
-			.default_value	= PO1030_EXPOSURE_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = po1030_set_exposure,
-		.get = po1030_get_exposure
-	},
-#define RED_BALANCE_IDX 2
-	{
-		{
-			.id		= V4L2_CID_RED_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "red balance",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= PO1030_RED_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = po1030_set_red_balance,
-		.get = po1030_get_red_balance
-	},
-#define BLUE_BALANCE_IDX 3
-	{
-		{
-			.id		= V4L2_CID_BLUE_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "blue balance",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= PO1030_BLUE_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = po1030_set_blue_balance,
-		.get = po1030_get_blue_balance
-	},
-#define HFLIP_IDX 4
-	{
-		{
-			.id		= V4L2_CID_HFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "horizontal flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0,
-		},
-		.set = po1030_set_hflip,
-		.get = po1030_get_hflip
-	},
-#define VFLIP_IDX 5
-	{
-		{
-			.id		= V4L2_CID_VFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "vertical flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0,
-		},
-		.set = po1030_set_vflip,
-		.get = po1030_get_vflip
-	},
-#define AUTO_WHITE_BALANCE_IDX 6
-	{
-		{
-			.id		= V4L2_CID_AUTO_WHITE_BALANCE,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto white balance",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0,
-		},
-		.set = po1030_set_auto_white_balance,
-		.get = po1030_get_auto_white_balance
-	},
-#define AUTO_EXPOSURE_IDX 7
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE_AUTO,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "auto exposure",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0,
-		},
-		.set = po1030_set_auto_exposure,
-		.get = po1030_get_auto_exposure
-	},
-#define GREEN_BALANCE_IDX 8
-	{
-		{
-			.id		= M5602_V4L2_CID_GREEN_BALANCE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "green balance",
-			.minimum	= 0x00,
-			.maximum	= 0xff,
-			.step		= 0x1,
-			.default_value	= PO1030_GREEN_GAIN_DEFAULT,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = po1030_set_green_balance,
-		.get = po1030_get_green_balance
-	},
+static const struct v4l2_ctrl_ops po1030_ctrl_ops = {
+	.s_ctrl = po1030_s_ctrl,
 };
 
-static void po1030_dump_registers(struct sd *sd);
+static const struct v4l2_ctrl_config po1030_greenbal_cfg = {
+	.ops	= &po1030_ctrl_ops,
+	.id	= M5602_V4L2_CID_GREEN_BALANCE,
+	.name	= "Green Balance",
+	.type	= V4L2_CTRL_TYPE_INTEGER,
+	.min	= 0,
+	.max	= 255,
+	.step	= 1,
+	.def	= PO1030_GREEN_GAIN_DEFAULT,
+	.flags	= V4L2_CTRL_FLAG_SLIDER,
+};
 
 int po1030_probe(struct sd *sd)
 {
 	u8 dev_id_h = 0, i;
-	s32 *sensor_settings;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	if (force_sensor) {
 		if (force_sensor == PO1030_SENSOR) {
@@ -229,26 +89,14 @@ int po1030_probe(struct sd *sd)
 	return -ENODEV;
 
 sensor_found:
-	sensor_settings = kmalloc(
-		ARRAY_SIZE(po1030_ctrls) * sizeof(s32), GFP_KERNEL);
-	if (!sensor_settings)
-		return -ENOMEM;
-
 	sd->gspca_dev.cam.cam_mode = po1030_modes;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(po1030_modes);
-	sd->desc->ctrls = po1030_ctrls;
-	sd->desc->nctrls = ARRAY_SIZE(po1030_ctrls);
-
-	for (i = 0; i < ARRAY_SIZE(po1030_ctrls); i++)
-		sensor_settings[i] = po1030_ctrls[i].qctrl.default_value;
-	sd->sensor_priv = sensor_settings;
 
 	return 0;
 }
 
 int po1030_init(struct sd *sd)
 {
-	s32 *sensor_settings = sd->sensor_priv;
 	int i, err = 0;
 
 	/* Init the sensor */
@@ -279,46 +127,50 @@ int po1030_init(struct sd *sd)
 	if (dump_sensor)
 		po1030_dump_registers(sd);
 
-	err = po1030_set_exposure(&sd->gspca_dev,
-				   sensor_settings[EXPOSURE_IDX]);
-	if (err < 0)
-		return err;
-
-	err = po1030_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
-	if (err < 0)
-		return err;
-
-	err = po1030_set_hflip(&sd->gspca_dev, sensor_settings[HFLIP_IDX]);
-	if (err < 0)
-		return err;
-
-	err = po1030_set_vflip(&sd->gspca_dev, sensor_settings[VFLIP_IDX]);
-	if (err < 0)
-		return err;
-
-	err = po1030_set_red_balance(&sd->gspca_dev,
-				      sensor_settings[RED_BALANCE_IDX]);
-	if (err < 0)
-		return err;
-
-	err = po1030_set_blue_balance(&sd->gspca_dev,
-				      sensor_settings[BLUE_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+	return 0;
+}
 
-	err = po1030_set_green_balance(&sd->gspca_dev,
-				       sensor_settings[GREEN_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+int po1030_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+
+	sd->gspca_dev.vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 9);
+
+	sd->auto_white_bal = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops,
+					       V4L2_CID_AUTO_WHITE_BALANCE,
+					       0, 1, 1, 0);
+	sd->green_bal = v4l2_ctrl_new_custom(hdl, &po1030_greenbal_cfg, NULL);
+	sd->red_bal = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops,
+					V4L2_CID_RED_BALANCE, 0, 255, 1,
+					PO1030_RED_GAIN_DEFAULT);
+	sd->blue_bal = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops,
+					V4L2_CID_BLUE_BALANCE, 0, 255, 1,
+					PO1030_BLUE_GAIN_DEFAULT);
+
+	sd->autoexpo = v4l2_ctrl_new_std_menu(hdl, &po1030_ctrl_ops,
+			  V4L2_CID_EXPOSURE_AUTO, 1, 0, V4L2_EXPOSURE_MANUAL);
+	sd->expo = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_EXPOSURE,
+			  0, 0x2ff, 1, PO1030_EXPOSURE_DEFAULT);
+
+	sd->gain = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_GAIN, 0,
+				     0x4f, 1, PO1030_GLOBAL_GAIN_DEFAULT);
+
+	sd->hflip = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_HFLIP,
+				      0, 1, 1, 0);
+	sd->vflip = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_VFLIP,
+				      0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 
-	err = po1030_set_auto_white_balance(&sd->gspca_dev,
-				sensor_settings[AUTO_WHITE_BALANCE_IDX]);
-	if (err < 0)
-		return err;
+	v4l2_ctrl_auto_cluster(4, &sd->auto_white_bal, 0, false);
+	v4l2_ctrl_auto_cluster(2, &sd->autoexpo, 0, false);
+	v4l2_ctrl_cluster(2, &sd->hflip);
 
-	err = po1030_set_auto_exposure(&sd->gspca_dev,
-				sensor_settings[AUTO_EXPOSURE_IDX]);
-	return err;
+	return 0;
 }
 
 int po1030_start(struct sd *sd)
@@ -448,28 +300,16 @@ int po1030_start(struct sd *sd)
 	return err;
 }
 
-static int po1030_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[EXPOSURE_IDX];
-	PDEBUG(D_V4L2, "Exposure read as %d", *val);
-	return 0;
-}
-
 static int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[EXPOSURE_IDX] = val;
-	PDEBUG(D_V4L2, "Set exposure to %d", val & 0xffff);
+	PDEBUG(D_CONF, "Set exposure to %d", val & 0xffff);
 
 	i2c_data = ((val & 0xff00) >> 8);
-	PDEBUG(D_V4L2, "Set exposure to high byte to 0x%x",
+	PDEBUG(D_CONF, "Set exposure to high byte to 0x%x",
 	       i2c_data);
 
 	err = m5602_write_sensor(sd, PO1030_INTEGLINES_H,
@@ -478,7 +318,7 @@ static int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 		return err;
 
 	i2c_data = (val & 0xff);
-	PDEBUG(D_V4L2, "Set exposure to low byte to 0x%x",
+	PDEBUG(D_CONF, "Set exposure to low byte to 0x%x",
 	       i2c_data);
 	err = m5602_write_sensor(sd, PO1030_INTEGLINES_M,
 				  &i2c_data, 1);
@@ -486,91 +326,32 @@ static int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int po1030_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[GAIN_IDX];
-	PDEBUG(D_V4L2, "Read global gain %d", *val);
-	return 0;
-}
-
 static int po1030_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[GAIN_IDX] = val;
-
 	i2c_data = val & 0xff;
-	PDEBUG(D_V4L2, "Set global gain to %d", i2c_data);
+	PDEBUG(D_CONF, "Set global gain to %d", i2c_data);
 	err = m5602_write_sensor(sd, PO1030_GLOBALGAIN,
 				 &i2c_data, 1);
 	return err;
 }
 
-static int po1030_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[HFLIP_IDX];
-	PDEBUG(D_V4L2, "Read hflip %d", *val);
-
-	return 0;
-}
-
-static int po1030_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-	u8 i2c_data;
-	int err;
-
-	sensor_settings[HFLIP_IDX] = val;
-
-	PDEBUG(D_V4L2, "Set hflip %d", val);
-	err = m5602_read_sensor(sd, PO1030_CONTROL2, &i2c_data, 1);
-	if (err < 0)
-		return err;
-
-	i2c_data = (0x7f & i2c_data) | ((val & 0x01) << 7);
-
-	err = m5602_write_sensor(sd, PO1030_CONTROL2,
-				 &i2c_data, 1);
-
-	return err;
-}
-
-static int po1030_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
+static int po1030_set_hvflip(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[VFLIP_IDX];
-	PDEBUG(D_V4L2, "Read vflip %d", *val);
-
-	return 0;
-}
-
-static int po1030_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[VFLIP_IDX] = val;
-
-	PDEBUG(D_V4L2, "Set vflip %d", val);
+	PDEBUG(D_CONF, "Set hvflip %d %d", sd->hflip->val, sd->vflip->val);
 	err = m5602_read_sensor(sd, PO1030_CONTROL2, &i2c_data, 1);
 	if (err < 0)
 		return err;
 
-	i2c_data = (i2c_data & 0xbf) | ((val & 0x01) << 6);
+	i2c_data = (0x3f & i2c_data) | (sd->hflip->val << 7) |
+		   (sd->vflip->val << 6);
 
 	err = m5602_write_sensor(sd, PO1030_CONTROL2,
 				 &i2c_data, 1);
@@ -578,81 +359,41 @@ static int po1030_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int po1030_get_red_balance(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[RED_BALANCE_IDX];
-	PDEBUG(D_V4L2, "Read red gain %d", *val);
-	return 0;
-}
-
 static int po1030_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[RED_BALANCE_IDX] = val;
-
 	i2c_data = val & 0xff;
-	PDEBUG(D_V4L2, "Set red gain to %d", i2c_data);
+	PDEBUG(D_CONF, "Set red gain to %d", i2c_data);
 	err = m5602_write_sensor(sd, PO1030_RED_GAIN,
 				  &i2c_data, 1);
 	return err;
 }
 
-static int po1030_get_blue_balance(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[BLUE_BALANCE_IDX];
-	PDEBUG(D_V4L2, "Read blue gain %d", *val);
-
-	return 0;
-}
-
 static int po1030_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[BLUE_BALANCE_IDX] = val;
-
 	i2c_data = val & 0xff;
-	PDEBUG(D_V4L2, "Set blue gain to %d", i2c_data);
+	PDEBUG(D_CONF, "Set blue gain to %d", i2c_data);
 	err = m5602_write_sensor(sd, PO1030_BLUE_GAIN,
 				  &i2c_data, 1);
 
 	return err;
 }
 
-static int po1030_get_green_balance(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[GREEN_BALANCE_IDX];
-	PDEBUG(D_V4L2, "Read green gain %d", *val);
-
-	return 0;
-}
-
 static int po1030_set_green_balance(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[GREEN_BALANCE_IDX] = val;
 	i2c_data = val & 0xff;
-	PDEBUG(D_V4L2, "Set green gain to %d", i2c_data);
+	PDEBUG(D_CONF, "Set green gain to %d", i2c_data);
 
 	err = m5602_write_sensor(sd, PO1030_GREEN_1_GAIN,
 			   &i2c_data, 1);
@@ -663,63 +404,36 @@ static int po1030_set_green_balance(struct gspca_dev *gspca_dev, __s32 val)
 				 &i2c_data, 1);
 }
 
-static int po1030_get_auto_white_balance(struct gspca_dev *gspca_dev,
-					 __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_WHITE_BALANCE_IDX];
-	PDEBUG(D_V4L2, "Auto white balancing is %d", *val);
-
-	return 0;
-}
-
 static int po1030_set_auto_white_balance(struct gspca_dev *gspca_dev,
 					 __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[AUTO_WHITE_BALANCE_IDX] = val;
-
 	err = m5602_read_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 	if (err < 0)
 		return err;
 
-	PDEBUG(D_V4L2, "Set auto white balance to %d", val);
+	PDEBUG(D_CONF, "Set auto white balance to %d", val);
 	i2c_data = (i2c_data & 0xfe) | (val & 0x01);
 	err = m5602_write_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 	return err;
 }
 
-static int po1030_get_auto_exposure(struct gspca_dev *gspca_dev,
-				    __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[AUTO_EXPOSURE_IDX];
-	PDEBUG(D_V4L2, "Auto exposure is %d", *val);
-	return 0;
-}
-
 static int po1030_set_auto_exposure(struct gspca_dev *gspca_dev,
 				    __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 i2c_data;
 	int err;
 
-	sensor_settings[AUTO_EXPOSURE_IDX] = val;
 	err = m5602_read_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 	if (err < 0)
 		return err;
 
-	PDEBUG(D_V4L2, "Set auto exposure to %d", val);
+	PDEBUG(D_CONF, "Set auto exposure to %d", val);
+	val = (val == V4L2_EXPOSURE_AUTO);
 	i2c_data = (i2c_data & 0xfd) | ((val & 0x01) << 1);
 	return m5602_write_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
 }
@@ -727,7 +441,48 @@ static int po1030_set_auto_exposure(struct gspca_dev *gspca_dev,
 void po1030_disconnect(struct sd *sd)
 {
 	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
+}
+
+static int po1030_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *) gspca_dev;
+	int err;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		err = po1030_set_auto_white_balance(gspca_dev, ctrl->val);
+		if (err || ctrl->val)
+			return err;
+		err = po1030_set_green_balance(gspca_dev, sd->green_bal->val);
+		if (err)
+			return err;
+		err = po1030_set_red_balance(gspca_dev, sd->red_bal->val);
+		if (err)
+			return err;
+		err = po1030_set_blue_balance(gspca_dev, sd->blue_bal->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		err = po1030_set_auto_exposure(gspca_dev, ctrl->val);
+		if (err || ctrl->val == V4L2_EXPOSURE_AUTO)
+			return err;
+		err = po1030_set_exposure(gspca_dev, sd->expo->val);
+		break;
+	case V4L2_CID_GAIN:
+		err = po1030_set_gain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		err = po1030_set_hvflip(gspca_dev);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return err;
 }
 
 static void po1030_dump_registers(struct sd *sd)
diff --git a/drivers/media/usb/gspca/m5602/m5602_po1030.h b/drivers/media/usb/gspca/m5602/m5602_po1030.h
index 81a2bcb88fe3..a6ab76149bd0 100644
--- a/drivers/media/usb/gspca/m5602/m5602_po1030.h
+++ b/drivers/media/usb/gspca/m5602/m5602_po1030.h
@@ -151,6 +151,7 @@ extern bool dump_sensor;
 
 int po1030_probe(struct sd *sd);
 int po1030_init(struct sd *sd);
+int po1030_init_controls(struct sd *sd);
 int po1030_start(struct sd *sd);
 void po1030_disconnect(struct sd *sd);
 
@@ -162,6 +163,7 @@ static const struct m5602_sensor po1030 = {
 
 	.probe = po1030_probe,
 	.init = po1030_init,
+	.init_controls = po1030_init_controls,
 	.start = po1030_start,
 	.disconnect = po1030_disconnect,
 };
diff --git a/drivers/media/usb/gspca/m5602/m5602_s5k4aa.c b/drivers/media/usb/gspca/m5602/m5602_s5k4aa.c
index c8e1572eb502..7d12599458e2 100644
--- a/drivers/media/usb/gspca/m5602/m5602_s5k4aa.c
+++ b/drivers/media/usb/gspca/m5602/m5602_s5k4aa.c
@@ -20,18 +20,12 @@
 
 #include "m5602_s5k4aa.h"
 
-static int s5k4aa_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k4aa_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k4aa_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k4aa_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k4aa_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k4aa_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k4aa_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k4aa_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k4aa_get_noise(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k4aa_set_noise(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k4aa_get_brightness(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k4aa_set_brightness(struct gspca_dev *gspca_dev, __s32 val);
+static int s5k4aa_s_ctrl(struct v4l2_ctrl *ctrl);
+static void s5k4aa_dump_registers(struct sd *sd);
+
+static const struct v4l2_ctrl_ops s5k4aa_ctrl_ops = {
+	.s_ctrl = s5k4aa_s_ctrl,
+};
 
 static
     const
@@ -147,104 +141,12 @@ static struct v4l2_pix_format s5k4aa_modes[] = {
 	}
 };
 
-static const struct ctrl s5k4aa_ctrls[] = {
-#define VFLIP_IDX 0
-	{
-		{
-			.id		= V4L2_CID_VFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "vertical flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = s5k4aa_set_vflip,
-		.get = s5k4aa_get_vflip
-	},
-#define HFLIP_IDX 1
-	{
-		{
-			.id		= V4L2_CID_HFLIP,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "horizontal flip",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 0
-		},
-		.set = s5k4aa_set_hflip,
-		.get = s5k4aa_get_hflip
-	},
-#define GAIN_IDX 2
-	{
-		{
-			.id		= V4L2_CID_GAIN,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Gain",
-			.minimum	= 0,
-			.maximum	= 127,
-			.step		= 1,
-			.default_value	= S5K4AA_DEFAULT_GAIN,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = s5k4aa_set_gain,
-		.get = s5k4aa_get_gain
-	},
-#define EXPOSURE_IDX 3
-	{
-		{
-			.id		= V4L2_CID_EXPOSURE,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Exposure",
-			.minimum	= 13,
-			.maximum	= 0xfff,
-			.step		= 1,
-			.default_value	= 0x100,
-			.flags		= V4L2_CTRL_FLAG_SLIDER
-		},
-		.set = s5k4aa_set_exposure,
-		.get = s5k4aa_get_exposure
-	},
-#define NOISE_SUPP_IDX 4
-	{
-		{
-			.id		= V4L2_CID_PRIVATE_BASE,
-			.type		= V4L2_CTRL_TYPE_BOOLEAN,
-			.name		= "Noise suppression (smoothing)",
-			.minimum	= 0,
-			.maximum	= 1,
-			.step		= 1,
-			.default_value	= 1,
-		},
-			.set = s5k4aa_set_noise,
-			.get = s5k4aa_get_noise
-	},
-#define BRIGHTNESS_IDX 5
-	{
-		{
-			.id		= V4L2_CID_BRIGHTNESS,
-			.type		= V4L2_CTRL_TYPE_INTEGER,
-			.name		= "Brightness",
-			.minimum	= 0,
-			.maximum	= 0x1f,
-			.step		= 1,
-			.default_value	= S5K4AA_DEFAULT_BRIGHTNESS,
-		},
-			.set = s5k4aa_set_brightness,
-			.get = s5k4aa_get_brightness
-	},
-
-};
-
-static void s5k4aa_dump_registers(struct sd *sd);
-
 int s5k4aa_probe(struct sd *sd)
 {
 	u8 prod_id[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
 	const u8 expected_prod_id[6] = {0x00, 0x10, 0x00, 0x4b, 0x33, 0x75};
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int i, err = 0;
-	s32 *sensor_settings;
 
 	if (force_sensor) {
 		if (force_sensor == S5K4AA_SENSOR) {
@@ -303,19 +205,8 @@ int s5k4aa_probe(struct sd *sd)
 		pr_info("Detected a s5k4aa sensor\n");
 
 sensor_found:
-	sensor_settings = kmalloc(
-		ARRAY_SIZE(s5k4aa_ctrls) * sizeof(s32), GFP_KERNEL);
-	if (!sensor_settings)
-		return -ENOMEM;
-
 	sd->gspca_dev.cam.cam_mode = s5k4aa_modes;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(s5k4aa_modes);
-	sd->desc->ctrls = s5k4aa_ctrls;
-	sd->desc->nctrls = ARRAY_SIZE(s5k4aa_ctrls);
-
-	for (i = 0; i < ARRAY_SIZE(s5k4aa_ctrls); i++)
-		sensor_settings[i] = s5k4aa_ctrls[i].qctrl.default_value;
-	sd->sensor_priv = sensor_settings;
 
 	return 0;
 }
@@ -325,11 +216,11 @@ int s5k4aa_start(struct sd *sd)
 	int i, err = 0;
 	u8 data[2];
 	struct cam *cam = &sd->gspca_dev.cam;
-	s32 *sensor_settings = sd->sensor_priv;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	switch (cam->cam_mode[sd->gspca_dev.curr_mode].width) {
 	case 1280:
-		PDEBUG(D_V4L2, "Configuring camera for SXGA mode");
+		PDEBUG(D_CONF, "Configuring camera for SXGA mode");
 
 		for (i = 0; i < ARRAY_SIZE(SXGA_s5k4aa); i++) {
 			switch (SXGA_s5k4aa[i][0]) {
@@ -359,13 +250,10 @@ int s5k4aa_start(struct sd *sd)
 				return -EINVAL;
 			}
 		}
-		err = s5k4aa_set_noise(&sd->gspca_dev, 0);
-		if (err < 0)
-			return err;
 		break;
 
 	case 640:
-		PDEBUG(D_V4L2, "Configuring camera for VGA mode");
+		PDEBUG(D_CONF, "Configuring camera for VGA mode");
 
 		for (i = 0; i < ARRAY_SIZE(VGA_s5k4aa); i++) {
 			switch (VGA_s5k4aa[i][0]) {
@@ -395,37 +283,12 @@ int s5k4aa_start(struct sd *sd)
 				return -EINVAL;
 			}
 		}
-		err = s5k4aa_set_noise(&sd->gspca_dev, 1);
-		if (err < 0)
-			return err;
 		break;
 	}
 	if (err < 0)
 		return err;
 
-	err = s5k4aa_set_exposure(&sd->gspca_dev,
-				   sensor_settings[EXPOSURE_IDX]);
-	if (err < 0)
-		return err;
-
-	err = s5k4aa_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
-	if (err < 0)
-		return err;
-
-	err = s5k4aa_set_brightness(&sd->gspca_dev,
-				     sensor_settings[BRIGHTNESS_IDX]);
-	if (err < 0)
-		return err;
-
-	err = s5k4aa_set_noise(&sd->gspca_dev, sensor_settings[NOISE_SUPP_IDX]);
-	if (err < 0)
-		return err;
-
-	err = s5k4aa_set_vflip(&sd->gspca_dev, sensor_settings[VFLIP_IDX]);
-	if (err < 0)
-		return err;
-
-	return s5k4aa_set_hflip(&sd->gspca_dev, sensor_settings[HFLIP_IDX]);
+	return 0;
 }
 
 int s5k4aa_init(struct sd *sd)
@@ -466,13 +329,36 @@ int s5k4aa_init(struct sd *sd)
 	return err;
 }
 
-static int s5k4aa_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
+int s5k4aa_init_controls(struct sd *sd)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
+
+	sd->gspca_dev.vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 6);
 
-	*val = sensor_settings[EXPOSURE_IDX];
-	PDEBUG(D_V4L2, "Read exposure %d", *val);
+	v4l2_ctrl_new_std(hdl, &s5k4aa_ctrl_ops, V4L2_CID_BRIGHTNESS,
+			  0, 0x1f, 1, S5K4AA_DEFAULT_BRIGHTNESS);
+
+	v4l2_ctrl_new_std(hdl, &s5k4aa_ctrl_ops, V4L2_CID_EXPOSURE,
+			  13, 0xfff, 1, 0x100);
+
+	v4l2_ctrl_new_std(hdl, &s5k4aa_ctrl_ops, V4L2_CID_GAIN,
+			  0, 127, 1, S5K4AA_DEFAULT_GAIN);
+
+	v4l2_ctrl_new_std(hdl, &s5k4aa_ctrl_ops, V4L2_CID_SHARPNESS,
+			  0, 1, 1, 1);
+
+	sd->hflip = v4l2_ctrl_new_std(hdl, &s5k4aa_ctrl_ops, V4L2_CID_HFLIP,
+				      0, 1, 1, 0);
+	sd->vflip = v4l2_ctrl_new_std(hdl, &s5k4aa_ctrl_ops, V4L2_CID_VFLIP,
+				      0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+
+	v4l2_ctrl_cluster(2, &sd->hflip);
 
 	return 0;
 }
@@ -480,12 +366,10 @@ static int s5k4aa_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
 static int s5k4aa_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 data = S5K4AA_PAGE_MAP_2;
 	int err;
 
-	sensor_settings[EXPOSURE_IDX] = val;
-	PDEBUG(D_V4L2, "Set exposure to %d", val);
+	PDEBUG(D_CONF, "Set exposure to %d", val);
 	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		return err;
@@ -499,27 +383,15 @@ static int s5k4aa_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int s5k4aa_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
+static int s5k4aa_set_hvflip(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[VFLIP_IDX];
-	PDEBUG(D_V4L2, "Read vertical flip %d", *val);
-
-	return 0;
-}
-
-static int s5k4aa_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 data = S5K4AA_PAGE_MAP_2;
 	int err;
+	int hflip = sd->hflip->val;
+	int vflip = sd->vflip->val;
 
-	sensor_settings[VFLIP_IDX] = val;
-
-	PDEBUG(D_V4L2, "Set vertical flip to %d", val);
+	PDEBUG(D_CONF, "Set hvflip %d %d", hflip, vflip);
 	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		return err;
@@ -528,93 +400,48 @@ static int s5k4aa_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 	if (err < 0)
 		return err;
 
-	if (dmi_check_system(s5k4aa_vflip_dmi_table))
-		val = !val;
+	if (dmi_check_system(s5k4aa_vflip_dmi_table)) {
+		hflip = !hflip;
+		vflip = !vflip;
+	}
 
-	data = ((data & ~S5K4AA_RM_V_FLIP) | ((val & 0x01) << 7));
+	data = (data & 0x7f) | (vflip << 7) | (hflip << 6);
 	err = m5602_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
 	if (err < 0)
 		return err;
 
-	err = m5602_read_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
+	err = m5602_read_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
 	if (err < 0)
 		return err;
-	if (val)
+	if (hflip)
 		data &= 0xfe;
 	else
 		data |= 0x01;
-	err = m5602_write_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
-	return err;
-}
-
-static int s5k4aa_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[HFLIP_IDX];
-	PDEBUG(D_V4L2, "Read horizontal flip %d", *val);
-
-	return 0;
-}
-
-static int s5k4aa_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-	u8 data = S5K4AA_PAGE_MAP_2;
-	int err;
-
-	sensor_settings[HFLIP_IDX] = val;
-
-	PDEBUG(D_V4L2, "Set horizontal flip to %d", val);
-	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
-	if (err < 0)
-		return err;
-
-	err = m5602_read_sensor(sd, S5K4AA_READ_MODE, &data, 1);
-	if (err < 0)
-		return err;
-
-	if (dmi_check_system(s5k4aa_vflip_dmi_table))
-		val = !val;
-
-	data = ((data & ~S5K4AA_RM_H_FLIP) | ((val & 0x01) << 6));
-	err = m5602_write_sensor(sd, S5K4AA_READ_MODE, &data, 1);
+	err = m5602_write_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
 	if (err < 0)
 		return err;
 
-	err = m5602_read_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
+	err = m5602_read_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
 	if (err < 0)
 		return err;
-	if (val)
+	if (vflip)
 		data &= 0xfe;
 	else
 		data |= 0x01;
-	err = m5602_write_sensor(sd, S5K4AA_COLSTART_LO, &data, 1);
-	return err;
-}
-
-static int s5k4aa_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
+	err = m5602_write_sensor(sd, S5K4AA_ROWSTART_LO, &data, 1);
+	if (err < 0)
+		return err;
 
-	*val = sensor_settings[GAIN_IDX];
-	PDEBUG(D_V4L2, "Read gain %d", *val);
 	return 0;
 }
 
 static int s5k4aa_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 data = S5K4AA_PAGE_MAP_2;
 	int err;
 
-	sensor_settings[GAIN_IDX] = val;
-
-	PDEBUG(D_V4L2, "Set gain to %d", val);
+	PDEBUG(D_CONF, "Set gain to %d", val);
 	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		return err;
@@ -625,26 +452,13 @@ static int s5k4aa_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int s5k4aa_get_brightness(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[BRIGHTNESS_IDX];
-	PDEBUG(D_V4L2, "Read brightness %d", *val);
-	return 0;
-}
-
 static int s5k4aa_set_brightness(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 data = S5K4AA_PAGE_MAP_2;
 	int err;
 
-	sensor_settings[BRIGHTNESS_IDX] = val;
-
-	PDEBUG(D_V4L2, "Set brightness to %d", val);
+	PDEBUG(D_CONF, "Set brightness to %d", val);
 	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		return err;
@@ -653,26 +467,13 @@ static int s5k4aa_set_brightness(struct gspca_dev *gspca_dev, __s32 val)
 	return m5602_write_sensor(sd, S5K4AA_BRIGHTNESS, &data, 1);
 }
 
-static int s5k4aa_get_noise(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
-
-	*val = sensor_settings[NOISE_SUPP_IDX];
-	PDEBUG(D_V4L2, "Read noise %d", *val);
-	return 0;
-}
-
 static int s5k4aa_set_noise(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s32 *sensor_settings = sd->sensor_priv;
 	u8 data = S5K4AA_PAGE_MAP_2;
 	int err;
 
-	sensor_settings[NOISE_SUPP_IDX] = val;
-
-	PDEBUG(D_V4L2, "Set noise to %d", val);
+	PDEBUG(D_CONF, "Set noise to %d", val);
 	err = m5602_write_sensor(sd, S5K4AA_PAGE_MAP, &data, 1);
 	if (err < 0)
 		return err;
@@ -681,10 +482,41 @@ static int s5k4aa_set_noise(struct gspca_dev *gspca_dev, __s32 val)
 	return m5602_write_sensor(sd, S5K4AA_NOISE_SUPP, &data, 1);
 }
 
+static int s5k4aa_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	int err;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		err = s5k4aa_set_brightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		err = s5k4aa_set_exposure(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_GAIN:
+		err = s5k4aa_set_gain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SHARPNESS:
+		err = s5k4aa_set_noise(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		err = s5k4aa_set_hvflip(gspca_dev);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return err;
+}
+
 void s5k4aa_disconnect(struct sd *sd)
 {
 	sd->sensor = NULL;
-	kfree(sd->sensor_priv);
 }
 
 static void s5k4aa_dump_registers(struct sd *sd)
diff --git a/drivers/media/usb/gspca/m5602/m5602_s5k4aa.h b/drivers/media/usb/gspca/m5602/m5602_s5k4aa.h
index 8e0035e731c7..9953e9766954 100644
--- a/drivers/media/usb/gspca/m5602/m5602_s5k4aa.h
+++ b/drivers/media/usb/gspca/m5602/m5602_s5k4aa.h
@@ -69,6 +69,7 @@ extern bool dump_sensor;
 
 int s5k4aa_probe(struct sd *sd);
 int s5k4aa_init(struct sd *sd);
+int s5k4aa_init_controls(struct sd *sd);
 int s5k4aa_start(struct sd *sd);
 void s5k4aa_disconnect(struct sd *sd);
 
@@ -79,6 +80,7 @@ static const struct m5602_sensor s5k4aa = {
 
 	.probe = s5k4aa_probe,
 	.init = s5k4aa_init,
+	.init_controls = s5k4aa_init_controls,
 	.start = s5k4aa_start,
 	.disconnect = s5k4aa_disconnect,
 };
diff --git a/drivers/media/usb/gspca/m5602/m5602_s5k83a.c b/drivers/media/usb/gspca/m5602/m5602_s5k83a.c
index 1de743a02b02..7cbc3a00bda8 100644
--- a/drivers/media/usb/gspca/m5602/m5602_s5k83a.c
+++ b/drivers/media/usb/gspca/m5602/m5602_s5k83a.c
@@ -21,16 +21,11 @@
 #include <linux/kthread.h>
 #include "m5602_s5k83a.h"
 
-static int s5k83a_set_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k83a_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k83a_set_brightness(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k83a_get_brightness(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k83a_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k83a_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k83a_get_vflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k83a_set_vflip(struct gspca_dev *gspca_dev, __s32 val);
-static int s5k83a_get_hflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int s5k83a_set_hflip(struct gspca_dev *gspca_dev, __s32 val);
+static int s5k83a_s_ctrl(struct v4l2_ctrl *ctrl);
+
+static const struct v4l2_ctrl_ops s5k83a_ctrl_ops = {
+	.s_ctrl = s5k83a_s_ctrl,
+};
 
 static struct v4l2_pix_format s5k83a_modes[] = {
 	{
@@ -46,83 +41,6 @@ static struct v4l2_pix_format s5k83a_modes[] = {
 	}
 };
 
-static const struct ctrl s5k83a_ctrls[] = {
-#define GAIN_IDX 0
-	{
-		{
-			.id = V4L2_CID_GAIN,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "gain",
-			.minimum = 0x00,
-			.maximum = 0xff,
-			.step = 0x01,
-			.default_value = S5K83A_DEFAULT_GAIN,
-			.flags = V4L2_CTRL_FLAG_SLIDER
-		},
-			.set = s5k83a_set_gain,
-			.get = s5k83a_get_gain
-
-	},
-#define BRIGHTNESS_IDX 1
-	{
-		{
-			.id = V4L2_CID_BRIGHTNESS,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "brightness",
-			.minimum = 0x00,
-			.maximum = 0xff,
-			.step = 0x01,
-			.default_value = S5K83A_DEFAULT_BRIGHTNESS,
-			.flags = V4L2_CTRL_FLAG_SLIDER
-		},
-			.set = s5k83a_set_brightness,
-			.get = s5k83a_get_brightness,
-	},
-#define EXPOSURE_IDX 2
-	{
-		{
-			.id = V4L2_CID_EXPOSURE,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "exposure",
-			.minimum = 0x00,
-			.maximum = S5K83A_MAXIMUM_EXPOSURE,
-			.step = 0x01,
-			.default_value = S5K83A_DEFAULT_EXPOSURE,
-			.flags = V4L2_CTRL_FLAG_SLIDER
-		},
-			.set = s5k83a_set_exposure,
-			.get = s5k83a_get_exposure
-	},
-#define HFLIP_IDX 3
-	{
-		{
-			.id = V4L2_CID_HFLIP,
-			.type = V4L2_CTRL_TYPE_BOOLEAN,
-			.name = "horizontal flip",
-			.minimum = 0,
-			.maximum = 1,
-			.step = 1,
-			.default_value = 0
-		},
-			.set = s5k83a_set_hflip,
-			.get = s5k83a_get_hflip
-	},
-#define VFLIP_IDX 4
-	{
-		{
-			.id = V4L2_CID_VFLIP,
-			.type = V4L2_CTRL_TYPE_BOOLEAN,
-			.name = "vertical flip",
-			.minimum = 0,
-			.maximum = 1,
-			.step = 1,
-			.default_value = 0
-		},
-		.set = s5k83a_set_vflip,
-		.get = s5k83a_get_vflip
-	}
-};
-
 static void s5k83a_dump_registers(struct sd *sd);
 static int s5k83a_get_rotation(struct sd *sd, u8 *reg_data);
 static int s5k83a_set_led_indication(struct sd *sd, u8 val);
@@ -131,9 +49,9 @@ static int s5k83a_set_flip_real(struct gspca_dev *gspca_dev,
 
 int s5k83a_probe(struct sd *sd)
 {
-	struct s5k83a_priv *sens_priv;
 	u8 prod_id = 0, ver_id = 0;
 	int i, err = 0;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	if (force_sensor) {
 		if (force_sensor == S5K83A_SENSOR) {
@@ -173,38 +91,18 @@ int s5k83a_probe(struct sd *sd)
 		pr_info("Detected a s5k83a sensor\n");
 
 sensor_found:
-	sens_priv = kmalloc(
-		sizeof(struct s5k83a_priv), GFP_KERNEL);
-	if (!sens_priv)
-		return -ENOMEM;
-
-	sens_priv->settings =
-	kmalloc(sizeof(s32)*ARRAY_SIZE(s5k83a_ctrls), GFP_KERNEL);
-	if (!sens_priv->settings) {
-		kfree(sens_priv);
-		return -ENOMEM;
-	}
-
 	sd->gspca_dev.cam.cam_mode = s5k83a_modes;
 	sd->gspca_dev.cam.nmodes = ARRAY_SIZE(s5k83a_modes);
-	sd->desc->ctrls = s5k83a_ctrls;
-	sd->desc->nctrls = ARRAY_SIZE(s5k83a_ctrls);
 
 	/* null the pointer! thread is't running now */
-	sens_priv->rotation_thread = NULL;
-
-	for (i = 0; i < ARRAY_SIZE(s5k83a_ctrls); i++)
-		sens_priv->settings[i] = s5k83a_ctrls[i].qctrl.default_value;
+	sd->rotation_thread = NULL;
 
-	sd->sensor_priv = sens_priv;
 	return 0;
 }
 
 int s5k83a_init(struct sd *sd)
 {
 	int i, err = 0;
-	s32 *sensor_settings =
-			((struct s5k83a_priv *) sd->sensor_priv)->settings;
 
 	for (i = 0; i < ARRAY_SIZE(init_s5k83a) && !err; i++) {
 		u8 data[2] = {0x00, 0x00};
@@ -237,33 +135,44 @@ int s5k83a_init(struct sd *sd)
 	if (dump_sensor)
 		s5k83a_dump_registers(sd);
 
-	err = s5k83a_set_gain(&sd->gspca_dev, sensor_settings[GAIN_IDX]);
-	if (err < 0)
-		return err;
+	return err;
+}
 
-	err = s5k83a_set_brightness(&sd->gspca_dev,
-				     sensor_settings[BRIGHTNESS_IDX]);
-	if (err < 0)
-		return err;
+int s5k83a_init_controls(struct sd *sd)
+{
+	struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
 
-	err = s5k83a_set_exposure(&sd->gspca_dev,
-				   sensor_settings[EXPOSURE_IDX]);
-	if (err < 0)
-		return err;
+	sd->gspca_dev.vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 6);
 
-	err = s5k83a_set_hflip(&sd->gspca_dev, sensor_settings[HFLIP_IDX]);
-	if (err < 0)
-		return err;
+	v4l2_ctrl_new_std(hdl, &s5k83a_ctrl_ops, V4L2_CID_BRIGHTNESS,
+			  0, 255, 1, S5K83A_DEFAULT_BRIGHTNESS);
 
-	err = s5k83a_set_vflip(&sd->gspca_dev, sensor_settings[VFLIP_IDX]);
+	v4l2_ctrl_new_std(hdl, &s5k83a_ctrl_ops, V4L2_CID_EXPOSURE,
+			  0, S5K83A_MAXIMUM_EXPOSURE, 1,
+			  S5K83A_DEFAULT_EXPOSURE);
 
-	return err;
+	v4l2_ctrl_new_std(hdl, &s5k83a_ctrl_ops, V4L2_CID_GAIN,
+			  0, 255, 1, S5K83A_DEFAULT_GAIN);
+
+	sd->hflip = v4l2_ctrl_new_std(hdl, &s5k83a_ctrl_ops, V4L2_CID_HFLIP,
+				      0, 1, 1, 0);
+	sd->vflip = v4l2_ctrl_new_std(hdl, &s5k83a_ctrl_ops, V4L2_CID_VFLIP,
+				      0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+
+	v4l2_ctrl_cluster(2, &sd->hflip);
+
+	return 0;
 }
 
 static int rotation_thread_function(void *data)
 {
 	struct sd *sd = (struct sd *) data;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
 	u8 reg, previous_rotation = 0;
 	__s32 vflip, hflip;
 
@@ -277,8 +186,8 @@ static int rotation_thread_function(void *data)
 			previous_rotation = reg;
 			pr_info("Camera was flipped\n");
 
-			s5k83a_get_vflip((struct gspca_dev *) sd, &vflip);
-			s5k83a_get_hflip((struct gspca_dev *) sd, &hflip);
+			hflip = sd->hflip->val;
+			vflip = sd->vflip->val;
 
 			if (reg) {
 				vflip = !vflip;
@@ -294,26 +203,25 @@ static int rotation_thread_function(void *data)
 
 	/* return to "front" flip */
 	if (previous_rotation) {
-		s5k83a_get_vflip((struct gspca_dev *) sd, &vflip);
-		s5k83a_get_hflip((struct gspca_dev *) sd, &hflip);
+		hflip = sd->hflip->val;
+		vflip = sd->vflip->val;
 		s5k83a_set_flip_real((struct gspca_dev *) sd, vflip, hflip);
 	}
 
-	sens_priv->rotation_thread = NULL;
+	sd->rotation_thread = NULL;
 	return 0;
 }
 
 int s5k83a_start(struct sd *sd)
 {
 	int i, err = 0;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
 
 	/* Create another thread, polling the GPIO ports of the camera to check
 	   if it got rotated. This is how the windows driver does it so we have
 	   to assume that there is no better way of accomplishing this */
-	sens_priv->rotation_thread = kthread_create(rotation_thread_function,
-						    sd, "rotation thread");
-	wake_up_process(sens_priv->rotation_thread);
+	sd->rotation_thread = kthread_create(rotation_thread_function,
+					     sd, "rotation thread");
+	wake_up_process(sd->rotation_thread);
 
 	/* Preinit the sensor */
 	for (i = 0; i < ARRAY_SIZE(start_s5k83a) && !err; i++) {
@@ -333,32 +241,17 @@ int s5k83a_start(struct sd *sd)
 
 int s5k83a_stop(struct sd *sd)
 {
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	if (sens_priv->rotation_thread)
-		kthread_stop(sens_priv->rotation_thread);
+	if (sd->rotation_thread)
+		kthread_stop(sd->rotation_thread);
 
 	return s5k83a_set_led_indication(sd, 0);
 }
 
 void s5k83a_disconnect(struct sd *sd)
 {
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
 	s5k83a_stop(sd);
 
 	sd->sensor = NULL;
-	kfree(sens_priv->settings);
-	kfree(sens_priv);
-}
-
-static int s5k83a_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	*val = sens_priv->settings[GAIN_IDX];
-	return 0;
 }
 
 static int s5k83a_set_gain(struct gspca_dev *gspca_dev, __s32 val)
@@ -366,9 +259,6 @@ static int s5k83a_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 	int err;
 	u8 data[2];
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	sens_priv->settings[GAIN_IDX] = val;
 
 	data[0] = 0x00;
 	data[1] = 0x20;
@@ -391,60 +281,29 @@ static int s5k83a_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 	return err;
 }
 
-static int s5k83a_get_brightness(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	*val = sens_priv->settings[BRIGHTNESS_IDX];
-	return 0;
-}
-
 static int s5k83a_set_brightness(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u8 data[1];
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
 
-	sens_priv->settings[BRIGHTNESS_IDX] = val;
 	data[0] = val;
 	err = m5602_write_sensor(sd, S5K83A_BRIGHTNESS, data, 1);
 	return err;
 }
 
-static int s5k83a_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	*val = sens_priv->settings[EXPOSURE_IDX];
-	return 0;
-}
-
 static int s5k83a_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 {
 	int err;
 	u8 data[2];
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
 
-	sens_priv->settings[EXPOSURE_IDX] = val;
 	data[0] = 0;
 	data[1] = val;
 	err = m5602_write_sensor(sd, S5K83A_EXPOSURE, data, 2);
 	return err;
 }
 
-static int s5k83a_get_vflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	*val = sens_priv->settings[VFLIP_IDX];
-	return 0;
-}
-
 static int s5k83a_set_flip_real(struct gspca_dev *gspca_dev,
 				__s32 vflip, __s32 hflip)
 {
@@ -476,60 +335,52 @@ static int s5k83a_set_flip_real(struct gspca_dev *gspca_dev,
 	return err;
 }
 
-static int s5k83a_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
+static int s5k83a_set_hvflip(struct gspca_dev *gspca_dev)
 {
 	int err;
 	u8 reg;
-	__s32 hflip;
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	sens_priv->settings[VFLIP_IDX] = val;
-
-	s5k83a_get_hflip(gspca_dev, &hflip);
+	int hflip = sd->hflip->val;
+	int vflip = sd->vflip->val;
 
 	err = s5k83a_get_rotation(sd, &reg);
 	if (err < 0)
 		return err;
 	if (reg) {
-		val = !val;
 		hflip = !hflip;
+		vflip = !vflip;
 	}
 
-	err = s5k83a_set_flip_real(gspca_dev, val, hflip);
+	err = s5k83a_set_flip_real(gspca_dev, vflip, hflip);
 	return err;
 }
 
-static int s5k83a_get_hflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	*val = sens_priv->settings[HFLIP_IDX];
-	return 0;
-}
-
-static int s5k83a_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
+static int s5k83a_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 	int err;
-	u8 reg;
-	__s32 vflip;
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct s5k83a_priv *sens_priv = sd->sensor_priv;
-
-	sens_priv->settings[HFLIP_IDX] = val;
 
-	s5k83a_get_vflip(gspca_dev, &vflip);
-
-	err = s5k83a_get_rotation(sd, &reg);
-	if (err < 0)
-		return err;
-	if (reg) {
-		val = !val;
-		vflip = !vflip;
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		err = s5k83a_set_brightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		err = s5k83a_set_exposure(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_GAIN:
+		err = s5k83a_set_gain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		err = s5k83a_set_hvflip(gspca_dev);
+		break;
+	default:
+		return -EINVAL;
 	}
 
-	err = s5k83a_set_flip_real(gspca_dev, vflip, val);
 	return err;
 }
 
diff --git a/drivers/media/usb/gspca/m5602/m5602_s5k83a.h b/drivers/media/usb/gspca/m5602/m5602_s5k83a.h
index 79952247b534..d61b918228df 100644
--- a/drivers/media/usb/gspca/m5602/m5602_s5k83a.h
+++ b/drivers/media/usb/gspca/m5602/m5602_s5k83a.h
@@ -45,6 +45,7 @@ extern bool dump_sensor;
 
 int s5k83a_probe(struct sd *sd);
 int s5k83a_init(struct sd *sd);
+int s5k83a_init_controls(struct sd *sd);
 int s5k83a_start(struct sd *sd);
 int s5k83a_stop(struct sd *sd);
 void s5k83a_disconnect(struct sd *sd);
@@ -53,6 +54,7 @@ static const struct m5602_sensor s5k83a = {
 	.name = "S5K83A",
 	.probe = s5k83a_probe,
 	.init = s5k83a_init,
+	.init_controls = s5k83a_init_controls,
 	.start = s5k83a_start,
 	.stop = s5k83a_stop,
 	.disconnect = s5k83a_disconnect,
@@ -60,13 +62,6 @@ static const struct m5602_sensor s5k83a = {
 	.i2c_regW = 2,
 };
 
-struct s5k83a_priv {
-	/* We use another thread periodically
-	   probing the orientation of the camera */
-	struct task_struct *rotation_thread;
-	s32 *settings;
-};
-
 static const unsigned char preinit_s5k83a[][4] = {
 	{BRIDGE, M5602_XB_MCU_CLK_DIV, 0x02, 0x00},
 	{BRIDGE, M5602_XB_MCU_CLK_CTRL, 0xb0, 0x00},
diff --git a/drivers/media/usb/gspca/m5602/m5602_sensor.h b/drivers/media/usb/gspca/m5602/m5602_sensor.h
index edff4f1f586f..48341b4d607b 100644
--- a/drivers/media/usb/gspca/m5602/m5602_sensor.h
+++ b/drivers/media/usb/gspca/m5602/m5602_sensor.h
@@ -57,6 +57,9 @@ struct m5602_sensor {
 	/* Performs a initialization sequence */
 	int (*init)(struct sd *sd);
 
+	/* Controls initialization, maybe NULL */
+	int (*init_controls)(struct sd *sd);
+
 	/* Executed when the camera starts to send data */
 	int (*start)(struct sd *sd);
 
diff --git a/drivers/media/usb/gspca/mr97310a.c b/drivers/media/usb/gspca/mr97310a.c
index 8f4714df5990..68bb2f359666 100644
--- a/drivers/media/usb/gspca/mr97310a.c
+++ b/drivers/media/usb/gspca/mr97310a.c
@@ -289,7 +289,7 @@ static int zero_the_pointer(struct gspca_dev *gspca_dev)
 			return err_code;
 	}
 	if (status != 0x0a)
-		PDEBUG(D_ERR, "status is %02x", status);
+		PERR("status is %02x", status);
 
 	tries = 0;
 	while (tries < 4) {
@@ -330,7 +330,7 @@ static void stream_stop(struct gspca_dev *gspca_dev)
 	gspca_dev->usb_buf[0] = 0x01;
 	gspca_dev->usb_buf[1] = 0x00;
 	if (mr_write(gspca_dev, 2) < 0)
-		PDEBUG(D_ERR, "Stream Stop failed");
+		PERR("Stream Stop failed");
 }
 
 static void lcd_stop(struct gspca_dev *gspca_dev)
@@ -338,7 +338,7 @@ static void lcd_stop(struct gspca_dev *gspca_dev)
 	gspca_dev->usb_buf[0] = 0x19;
 	gspca_dev->usb_buf[1] = 0x54;
 	if (mr_write(gspca_dev, 2) < 0)
-		PDEBUG(D_ERR, "LCD Stop failed");
+		PERR("LCD Stop failed");
 }
 
 static int isoc_enable(struct gspca_dev *gspca_dev)
@@ -1026,7 +1026,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	struct sd *sd = (struct sd *) gspca_dev;
 	unsigned char *sof;
 
-	sof = pac_find_sof(&sd->sof_read, data, len);
+	sof = pac_find_sof(gspca_dev, &sd->sof_read, data, len);
 	if (sof) {
 		int n;
 
diff --git a/drivers/media/usb/gspca/ov519.c b/drivers/media/usb/gspca/ov519.c
index 9ad19a7ef81b..a3958ee86816 100644
--- a/drivers/media/usb/gspca/ov519.c
+++ b/drivers/media/usb/gspca/ov519.c
@@ -2034,6 +2034,7 @@ static unsigned char ov7670_abs_to_sm(unsigned char v)
 /* Write a OV519 register */
 static void reg_w(struct sd *sd, u16 index, u16 value)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int ret, req = 0;
 
 	if (sd->gspca_dev.usb_err < 0)
@@ -2071,7 +2072,7 @@ static void reg_w(struct sd *sd, u16 index, u16 value)
 			sd->gspca_dev.usb_buf, 1, 500);
 leave:
 	if (ret < 0) {
-		pr_err("reg_w %02x failed %d\n", index, ret);
+		PERR("reg_w %02x failed %d\n", index, ret);
 		sd->gspca_dev.usb_err = ret;
 		return;
 	}
@@ -2081,6 +2082,7 @@ leave:
 /* returns: negative is error, pos or zero is data */
 static int reg_r(struct sd *sd, u16 index)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int ret;
 	int req;
 
@@ -2110,7 +2112,7 @@ static int reg_r(struct sd *sd, u16 index)
 		PDEBUG(D_USBI, "GET %02x 0000 %04x %02x",
 			req, index, ret);
 	} else {
-		pr_err("reg_r %02x failed %d\n", index, ret);
+		PERR("reg_r %02x failed %d\n", index, ret);
 		sd->gspca_dev.usb_err = ret;
 	}
 
@@ -2121,6 +2123,7 @@ static int reg_r(struct sd *sd, u16 index)
 static int reg_r8(struct sd *sd,
 		  u16 index)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int ret;
 
 	if (sd->gspca_dev.usb_err < 0)
@@ -2135,7 +2138,7 @@ static int reg_r8(struct sd *sd,
 	if (ret >= 0) {
 		ret = sd->gspca_dev.usb_buf[0];
 	} else {
-		pr_err("reg_r8 %02x failed %d\n", index, ret);
+		PERR("reg_r8 %02x failed %d\n", index, ret);
 		sd->gspca_dev.usb_err = ret;
 	}
 
@@ -2174,6 +2177,7 @@ static void reg_w_mask(struct sd *sd,
  */
 static void ov518_reg_w32(struct sd *sd, u16 index, u32 value, int n)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int ret;
 
 	if (sd->gspca_dev.usb_err < 0)
@@ -2188,13 +2192,14 @@ static void ov518_reg_w32(struct sd *sd, u16 index, u32 value, int n)
 			0, index,
 			sd->gspca_dev.usb_buf, n, 500);
 	if (ret < 0) {
-		pr_err("reg_w32 %02x failed %d\n", index, ret);
+		PERR("reg_w32 %02x failed %d\n", index, ret);
 		sd->gspca_dev.usb_err = ret;
 	}
 }
 
 static void ov511_i2c_w(struct sd *sd, u8 reg, u8 value)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int rc, retries;
 
 	PDEBUG(D_USBO, "ov511_i2c_w %02x %02x", reg, value);
@@ -2228,6 +2233,7 @@ static void ov511_i2c_w(struct sd *sd, u8 reg, u8 value)
 
 static int ov511_i2c_r(struct sd *sd, u8 reg)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int rc, value, retries;
 
 	/* Two byte write cycle */
@@ -2300,6 +2306,8 @@ static void ov518_i2c_w(struct sd *sd,
 		u8 reg,
 		u8 value)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
+
 	PDEBUG(D_USBO, "ov518_i2c_w %02x %02x", reg, value);
 
 	/* Select camera register */
@@ -2325,6 +2333,7 @@ static void ov518_i2c_w(struct sd *sd,
  */
 static int ov518_i2c_r(struct sd *sd, u8 reg)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int value;
 
 	/* Select camera register */
@@ -2345,6 +2354,7 @@ static int ov518_i2c_r(struct sd *sd, u8 reg)
 
 static void ovfx2_i2c_w(struct sd *sd, u8 reg, u8 value)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int ret;
 
 	if (sd->gspca_dev.usb_err < 0)
@@ -2357,7 +2367,7 @@ static void ovfx2_i2c_w(struct sd *sd, u8 reg, u8 value)
 			(u16) value, (u16) reg, NULL, 0, 500);
 
 	if (ret < 0) {
-		pr_err("ovfx2_i2c_w %02x failed %d\n", reg, ret);
+		PERR("ovfx2_i2c_w %02x failed %d\n", reg, ret);
 		sd->gspca_dev.usb_err = ret;
 	}
 
@@ -2366,6 +2376,7 @@ static void ovfx2_i2c_w(struct sd *sd, u8 reg, u8 value)
 
 static int ovfx2_i2c_r(struct sd *sd, u8 reg)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int ret;
 
 	if (sd->gspca_dev.usb_err < 0)
@@ -2381,7 +2392,7 @@ static int ovfx2_i2c_r(struct sd *sd, u8 reg)
 		ret = sd->gspca_dev.usb_buf[0];
 		PDEBUG(D_USBI, "ovfx2_i2c_r %02x %02x", reg, ret);
 	} else {
-		pr_err("ovfx2_i2c_r %02x failed %d\n", reg, ret);
+		PERR("ovfx2_i2c_r %02x failed %d\n", reg, ret);
 		sd->gspca_dev.usb_err = ret;
 	}
 
@@ -2478,6 +2489,8 @@ static void i2c_w_mask(struct sd *sd,
  * registers while the camera is streaming */
 static inline void ov51x_stop(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
+
 	PDEBUG(D_STREAM, "stopping");
 	sd->stopped = 1;
 	switch (sd->bridge) {
@@ -2507,6 +2520,8 @@ static inline void ov51x_stop(struct sd *sd)
  * actually stopped (for performance). */
 static inline void ov51x_restart(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
+
 	PDEBUG(D_STREAM, "restarting");
 	if (!sd->stopped)
 		return;
@@ -2545,6 +2560,7 @@ static void ov51x_set_slave_ids(struct sd *sd, u8 slave);
 static int init_ov_sensor(struct sd *sd, u8 slave)
 {
 	int i;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 
 	ov51x_set_slave_ids(sd, slave);
 
@@ -2624,10 +2640,11 @@ static void write_i2c_regvals(struct sd *sd,
 /* This initializes the OV2x10 / OV3610 / OV3620 / OV9600 */
 static void ov_hires_configure(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int high, low;
 
 	if (sd->bridge != BRIDGE_OVFX2) {
-		pr_err("error hires sensors only supported with ovfx2\n");
+		PERR("error hires sensors only supported with ovfx2\n");
 		return;
 	}
 
@@ -2662,7 +2679,7 @@ static void ov_hires_configure(struct sd *sd)
 		}
 		break;
 	}
-	pr_err("Error unknown sensor type: %02x%02x\n", high, low);
+	PERR("Error unknown sensor type: %02x%02x\n", high, low);
 }
 
 /* This initializes the OV8110, OV8610 sensor. The OV8110 uses
@@ -2670,6 +2687,7 @@ static void ov_hires_configure(struct sd *sd)
  */
 static void ov8xx0_configure(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int rc;
 
 	PDEBUG(D_PROBE, "starting ov8xx0 configuration");
@@ -2677,13 +2695,13 @@ static void ov8xx0_configure(struct sd *sd)
 	/* Detect sensor (sub)type */
 	rc = i2c_r(sd, OV7610_REG_COM_I);
 	if (rc < 0) {
-		PDEBUG(D_ERR, "Error detecting sensor type");
+		PERR("Error detecting sensor type");
 		return;
 	}
 	if ((rc & 3) == 1)
 		sd->sensor = SEN_OV8610;
 	else
-		pr_err("Unknown image sensor version: %d\n", rc & 3);
+		PERR("Unknown image sensor version: %d\n", rc & 3);
 }
 
 /* This initializes the OV7610, OV7620, or OV76BE sensor. The OV76BE uses
@@ -2691,6 +2709,7 @@ static void ov8xx0_configure(struct sd *sd)
  */
 static void ov7xx0_configure(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int rc, high, low;
 
 	PDEBUG(D_PROBE, "starting OV7xx0 configuration");
@@ -2701,7 +2720,7 @@ static void ov7xx0_configure(struct sd *sd)
 	/* add OV7670 here
 	 * it appears to be wrongly detected as a 7610 by default */
 	if (rc < 0) {
-		pr_err("Error detecting sensor type\n");
+		PERR("Error detecting sensor type\n");
 		return;
 	}
 	if ((rc & 3) == 3) {
@@ -2729,19 +2748,19 @@ static void ov7xx0_configure(struct sd *sd)
 		/* try to read product id registers */
 		high = i2c_r(sd, 0x0a);
 		if (high < 0) {
-			pr_err("Error detecting camera chip PID\n");
+			PERR("Error detecting camera chip PID\n");
 			return;
 		}
 		low = i2c_r(sd, 0x0b);
 		if (low < 0) {
-			pr_err("Error detecting camera chip VER\n");
+			PERR("Error detecting camera chip VER\n");
 			return;
 		}
 		if (high == 0x76) {
 			switch (low) {
 			case 0x30:
-				pr_err("Sensor is an OV7630/OV7635\n");
-				pr_err("7630 is not supported by this driver\n");
+				PERR("Sensor is an OV7630/OV7635\n");
+				PERR("7630 is not supported by this driver\n");
 				return;
 			case 0x40:
 				PDEBUG(D_PROBE, "Sensor is an OV7645");
@@ -2760,7 +2779,7 @@ static void ov7xx0_configure(struct sd *sd)
 				sd->sensor = SEN_OV7660;
 				break;
 			default:
-				pr_err("Unknown sensor: 0x76%02x\n", low);
+				PERR("Unknown sensor: 0x76%02x\n", low);
 				return;
 			}
 		} else {
@@ -2768,20 +2787,22 @@ static void ov7xx0_configure(struct sd *sd)
 			sd->sensor = SEN_OV7620;
 		}
 	} else {
-		pr_err("Unknown image sensor version: %d\n", rc & 3);
+		PERR("Unknown image sensor version: %d\n", rc & 3);
 	}
 }
 
 /* This initializes the OV6620, OV6630, OV6630AE, or OV6630AF sensor. */
 static void ov6xx0_configure(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int rc;
+
 	PDEBUG(D_PROBE, "starting OV6xx0 configuration");
 
 	/* Detect sensor (sub)type */
 	rc = i2c_r(sd, OV7610_REG_COM_I);
 	if (rc < 0) {
-		pr_err("Error detecting sensor type\n");
+		PERR("Error detecting sensor type\n");
 		return;
 	}
 
@@ -2810,7 +2831,7 @@ static void ov6xx0_configure(struct sd *sd)
 		pr_warn("WARNING: Sensor is an OV66307. Your camera may have been misdetected in previous driver versions.\n");
 		break;
 	default:
-		pr_err("FATAL: Unknown sensor version: 0x%02x\n", rc);
+		PERR("FATAL: Unknown sensor version: 0x%02x\n", rc);
 		return;
 	}
 
@@ -2907,6 +2928,7 @@ static void ov51x_upload_quan_tables(struct sd *sd)
 		7, 7, 7, 7, 7, 7, 8, 8
 	};
 
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	const unsigned char *pYTable, *pUVTable;
 	unsigned char val0, val1;
 	int i, size, reg = R51x_COMP_LUT_BEGIN;
@@ -3300,7 +3322,7 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	} else if (init_ov_sensor(sd, OV_HIRES_SID) >= 0) {
 		ov_hires_configure(sd);
 	} else {
-		pr_err("Can't determine sensor slave IDs\n");
+		PERR("Can't determine sensor slave IDs\n");
 		goto error;
 	}
 
@@ -3433,7 +3455,7 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	}
 	return gspca_dev->usb_err;
 error:
-	PDEBUG(D_ERR, "OV519 Config failed");
+	PERR("OV519 Config failed");
 	return -EINVAL;
 }
 
@@ -3459,6 +3481,7 @@ static int sd_isoc_init(struct gspca_dev *gspca_dev)
  */
 static void ov511_mode_init_regs(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int hsegs, vsegs, packet_size, fps, needed;
 	int interlaced = 0;
 	struct usb_host_interface *alt;
@@ -3467,7 +3490,7 @@ static void ov511_mode_init_regs(struct sd *sd)
 	intf = usb_ifnum_to_if(sd->gspca_dev.dev, sd->gspca_dev.iface);
 	alt = usb_altnum_to_altsetting(intf, sd->gspca_dev.alt);
 	if (!alt) {
-		pr_err("Couldn't get altsetting\n");
+		PERR("Couldn't get altsetting\n");
 		sd->gspca_dev.usb_err = -EIO;
 		return;
 	}
@@ -3583,6 +3606,7 @@ static void ov511_mode_init_regs(struct sd *sd)
  */
 static void ov518_mode_init_regs(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int hsegs, vsegs, packet_size;
 	struct usb_host_interface *alt;
 	struct usb_interface *intf;
@@ -3590,7 +3614,7 @@ static void ov518_mode_init_regs(struct sd *sd)
 	intf = usb_ifnum_to_if(sd->gspca_dev.dev, sd->gspca_dev.iface);
 	alt = usb_altnum_to_altsetting(intf, sd->gspca_dev.alt);
 	if (!alt) {
-		pr_err("Couldn't get altsetting\n");
+		PERR("Couldn't get altsetting\n");
 		sd->gspca_dev.usb_err = -EIO;
 		return;
 	}
@@ -3750,6 +3774,8 @@ static void ov519_mode_init_regs(struct sd *sd)
 		/* windows reads 0x55 at this point, why? */
 	};
 
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
+
 	/******** Set the mode ********/
 	switch (sd->sensor) {
 	default:
@@ -3865,11 +3891,10 @@ static void ov519_mode_init_regs(struct sd *sd)
 
 static void mode_init_ov_sensor_regs(struct sd *sd)
 {
-	struct gspca_dev *gspca_dev;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int qvga, xstart, xend, ystart, yend;
 	u8 v;
 
-	gspca_dev = &sd->gspca_dev;
 	qvga = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].priv & 1;
 
 	/******** Mode (VGA/QVGA) and sensor specific regs ********/
@@ -4304,7 +4329,7 @@ static void ov511_pkt_scan(struct gspca_dev *gspca_dev,
 			/* Frame end */
 			if ((in[9] + 1) * 8 != gspca_dev->width ||
 			    (in[10] + 1) * 8 != gspca_dev->height) {
-				PDEBUG(D_ERR, "Invalid frame size, got: %dx%d,"
+				PERR("Invalid frame size, got: %dx%d,"
 					" requested: %dx%d\n",
 					(in[9] + 1) * 8, (in[10] + 1) * 8,
 					gspca_dev->width, gspca_dev->height);
@@ -4355,7 +4380,7 @@ static void ov518_pkt_scan(struct gspca_dev *gspca_dev,
 		   except that they may contain part of the footer), are
 		   numbered 0 */
 		else if (sd->packet_nr == 0 || data[len]) {
-			PDEBUG(D_ERR, "Invalid packet nr: %d (expect: %d)",
+			PERR("Invalid packet nr: %d (expect: %d)",
 				(int)data[len], (int)sd->packet_nr);
 			gspca_dev->last_packet_type = DISCARD_PACKET;
 			return;
@@ -4898,7 +4923,7 @@ static int sd_init_controls(struct gspca_dev *gspca_dev)
 			QUALITY_MIN, QUALITY_MAX, 1, QUALITY_DEF);
 
 	if (hdl->error) {
-		pr_err("Could not initialize controls\n");
+		PERR("Could not initialize controls\n");
 		return hdl->error;
 	}
 	if (gspca_dev->autogain)
diff --git a/drivers/media/usb/gspca/ov534.c b/drivers/media/usb/gspca/ov534.c
index bb09d7884b89..2e28c81a03ab 100644
--- a/drivers/media/usb/gspca/ov534.c
+++ b/drivers/media/usb/gspca/ov534.c
@@ -690,7 +690,7 @@ static int sccb_check_status(struct gspca_dev *gspca_dev)
 		case 0x03:
 			break;
 		default:
-			PDEBUG(D_ERR, "sccb status 0x%02x, attempt %d/5",
+			PERR("sccb status 0x%02x, attempt %d/5",
 			       data, i + 1);
 		}
 	}
diff --git a/drivers/media/usb/gspca/pac207.c b/drivers/media/usb/gspca/pac207.c
index 3b75097dd34e..83519be94e58 100644
--- a/drivers/media/usb/gspca/pac207.c
+++ b/drivers/media/usb/gspca/pac207.c
@@ -373,7 +373,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	struct sd *sd = (struct sd *) gspca_dev;
 	unsigned char *sof;
 
-	sof = pac_find_sof(&sd->sof_read, data, len);
+	sof = pac_find_sof(gspca_dev, &sd->sof_read, data, len);
 	if (sof) {
 		int n;
 
diff --git a/drivers/media/usb/gspca/pac7302.c b/drivers/media/usb/gspca/pac7302.c
index add6f725ba50..6008c8d546a3 100644
--- a/drivers/media/usb/gspca/pac7302.c
+++ b/drivers/media/usb/gspca/pac7302.c
@@ -344,13 +344,10 @@ static void reg_w_var(struct gspca_dev *gspca_dev,
 			reg_w_page(gspca_dev, page3, page3_len);
 			break;
 		default:
-#ifdef GSPCA_DEBUG
 			if (len > USB_BUF_SZ) {
-				PDEBUG(D_ERR|D_STREAM,
-					"Incorrect variable sequence");
+				PERR("Incorrect variable sequence");
 				return;
 			}
-#endif
 			while (len > 0) {
 				if (len < 8) {
 					reg_w_buf(gspca_dev,
@@ -795,7 +792,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	u8 *image;
 	u8 *sof;
 
-	sof = pac_find_sof(&sd->sof_read, data, len);
+	sof = pac_find_sof(gspca_dev, &sd->sof_read, data, len);
 	if (sof) {
 		int n, lum_offset, footer_length;
 
@@ -843,7 +840,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 static int sd_dbg_s_register(struct gspca_dev *gspca_dev,
-			struct v4l2_dbg_register *reg)
+			const struct v4l2_dbg_register *reg)
 {
 	u8 index;
 	u8 value;
diff --git a/drivers/media/usb/gspca/pac7311.c b/drivers/media/usb/gspca/pac7311.c
index a12dfbf6e051..1a5bdc853a80 100644
--- a/drivers/media/usb/gspca/pac7311.c
+++ b/drivers/media/usb/gspca/pac7311.c
@@ -262,8 +262,7 @@ static void reg_w_var(struct gspca_dev *gspca_dev,
 			break;
 		default:
 			if (len > USB_BUF_SZ) {
-				PDEBUG(D_ERR|D_STREAM,
-					"Incorrect variable sequence");
+				PERR("Incorrect variable sequence");
 				return;
 			}
 			while (len > 0) {
@@ -575,7 +574,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	u8 *image;
 	unsigned char *sof;
 
-	sof = pac_find_sof(&sd->sof_read, data, len);
+	sof = pac_find_sof(gspca_dev, &sd->sof_read, data, len);
 	if (sof) {
 		int n, lum_offset, footer_length;
 
diff --git a/drivers/media/usb/gspca/pac_common.h b/drivers/media/usb/gspca/pac_common.h
index 8462a7c1a338..fbc5e226c3e4 100644
--- a/drivers/media/usb/gspca/pac_common.h
+++ b/drivers/media/usb/gspca/pac_common.h
@@ -71,7 +71,7 @@ static const unsigned char pac_sof_marker[5] =
 	   +----------+
 */
 
-static unsigned char *pac_find_sof(u8 *sof_read,
+static unsigned char *pac_find_sof(struct gspca_dev *gspca_dev, u8 *sof_read,
 					unsigned char *m, int len)
 {
 	int i;
diff --git a/drivers/media/usb/gspca/sn9c2028.c b/drivers/media/usb/gspca/sn9c2028.c
index 03fa3fd940b4..39b6b2e02963 100644
--- a/drivers/media/usb/gspca/sn9c2028.c
+++ b/drivers/media/usb/gspca/sn9c2028.c
@@ -650,13 +650,13 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
 
 	result = sn9c2028_read1(gspca_dev);
 	if (result < 0)
-		PDEBUG(D_ERR, "Camera Stop read failed");
+		PERR("Camera Stop read failed");
 
 	memset(data, 0, 6);
 	data[0] = 0x14;
 	result = sn9c2028_command(gspca_dev, data);
 	if (result < 0)
-		PDEBUG(D_ERR, "Camera Stop command failed");
+		PERR("Camera Stop command failed");
 }
 
 /* Include sn9c2028 sof detection functions */
diff --git a/drivers/media/usb/gspca/sn9c20x.c b/drivers/media/usb/gspca/sn9c20x.c
index 4ec544f4a845..ead9a1f58513 100644
--- a/drivers/media/usb/gspca/sn9c20x.c
+++ b/drivers/media/usb/gspca/sn9c20x.c
@@ -1598,7 +1598,7 @@ static int sd_dbg_g_register(struct gspca_dev *gspca_dev,
 }
 
 static int sd_dbg_s_register(struct gspca_dev *gspca_dev,
-			struct v4l2_dbg_register *reg)
+			const struct v4l2_dbg_register *reg)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
diff --git a/drivers/media/usb/gspca/sonixb.c b/drivers/media/usb/gspca/sonixb.c
index 104ae25275b4..3fe207e038c7 100644
--- a/drivers/media/usb/gspca/sonixb.c
+++ b/drivers/media/usb/gspca/sonixb.c
@@ -1379,27 +1379,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	}
 }
 
-static int sd_querymenu(struct gspca_dev *gspca_dev,
-			struct v4l2_querymenu *menu)
-{
-	switch (menu->id) {
-	case V4L2_CID_POWER_LINE_FREQUENCY:
-		switch (menu->index) {
-		case 0:		/* V4L2_CID_POWER_LINE_FREQUENCY_DISABLED */
-			strcpy((char *) menu->name, "NoFliker");
-			return 0;
-		case 1:		/* V4L2_CID_POWER_LINE_FREQUENCY_50HZ */
-			strcpy((char *) menu->name, "50 Hz");
-			return 0;
-		case 2:		/* V4L2_CID_POWER_LINE_FREQUENCY_60HZ */
-			strcpy((char *) menu->name, "60 Hz");
-			return 0;
-		}
-		break;
-	}
-	return -EINVAL;
-}
-
 #if IS_ENABLED(CONFIG_INPUT)
 static int sd_int_pkt_scan(struct gspca_dev *gspca_dev,
 			u8 *data,		/* interrupt packet data */
@@ -1428,7 +1407,6 @@ static const struct sd_desc sd_desc = {
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
-	.querymenu = sd_querymenu,
 	.dq_callback = do_autogain,
 #if IS_ENABLED(CONFIG_INPUT)
 	.int_pkt_scan = sd_int_pkt_scan,
diff --git a/drivers/media/usb/gspca/sonixj.c b/drivers/media/usb/gspca/sonixj.c
index 671d0c6dece3..3b5ccb1c4cdf 100644
--- a/drivers/media/usb/gspca/sonixj.c
+++ b/drivers/media/usb/gspca/sonixj.c
@@ -31,32 +31,26 @@ MODULE_AUTHOR("Jean-François Moine <http://moinejf.free.fr>");
 MODULE_DESCRIPTION("GSPCA/SONIX JPEG USB Camera Driver");
 MODULE_LICENSE("GPL");
 
-/* controls */
-enum e_ctrl {
-	BRIGHTNESS,
-	CONTRAST,
-	COLORS,
-	BLUE,
-	RED,
-	GAMMA,
-	EXPOSURE,
-	AUTOGAIN,
-	GAIN,
-	HFLIP,
-	VFLIP,
-	SHARPNESS,
-	ILLUM,
-	FREQ,
-	NCTRLS		/* number of controls */
-};
-
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	struct gspca_ctrl ctrls[NCTRLS];
-
 	atomic_t avg_lum;
+	struct v4l2_ctrl *brightness;
+	struct v4l2_ctrl *contrast;
+	struct v4l2_ctrl *saturation;
+	struct { /* red/blue balance control cluster */
+		struct v4l2_ctrl *red_bal;
+		struct v4l2_ctrl *blue_bal;
+	};
+	struct { /* hflip/vflip control cluster */
+		struct v4l2_ctrl *vflip;
+		struct v4l2_ctrl *hflip;
+	};
+	struct v4l2_ctrl *gamma;
+	struct v4l2_ctrl *illum;
+	struct v4l2_ctrl *sharpness;
+	struct v4l2_ctrl *freq;
 	u32 exposure;
 
 	struct work_struct work;
@@ -127,283 +121,6 @@ static void qual_upd(struct work_struct *work);
 #define SEN_CLK_EN	0x20	/* enable sensor clock */
 #define DEF_EN		0x80	/* defect pixel by 0: soft, 1: hard */
 
-/* V4L2 controls supported by the driver */
-static void setbrightness(struct gspca_dev *gspca_dev);
-static void setcontrast(struct gspca_dev *gspca_dev);
-static void setcolors(struct gspca_dev *gspca_dev);
-static void setredblue(struct gspca_dev *gspca_dev);
-static void setgamma(struct gspca_dev *gspca_dev);
-static void setexposure(struct gspca_dev *gspca_dev);
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val);
-static void setgain(struct gspca_dev *gspca_dev);
-static void sethvflip(struct gspca_dev *gspca_dev);
-static void setsharpness(struct gspca_dev *gspca_dev);
-static void setillum(struct gspca_dev *gspca_dev);
-static void setfreq(struct gspca_dev *gspca_dev);
-
-static const struct ctrl sd_ctrls[NCTRLS] = {
-[BRIGHTNESS] = {
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 0xff,
-		.step    = 1,
-		.default_value = 0x80,
-	    },
-	    .set_control = setbrightness
-	},
-[CONTRAST] = {
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-#define CONTRAST_MAX 127
-		.maximum = CONTRAST_MAX,
-		.step    = 1,
-		.default_value = 20,
-	    },
-	    .set_control = setcontrast
-	},
-[COLORS] = {
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Saturation",
-		.minimum = 0,
-		.maximum = 40,
-		.step    = 1,
-#define COLORS_DEF 25
-		.default_value = COLORS_DEF,
-	    },
-	    .set_control = setcolors
-	},
-[BLUE] = {
-	    {
-		.id      = V4L2_CID_BLUE_BALANCE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Blue Balance",
-		.minimum = 24,
-		.maximum = 40,
-		.step    = 1,
-		.default_value = 32,
-	    },
-	    .set_control = setredblue
-	},
-[RED] = {
-	    {
-		.id      = V4L2_CID_RED_BALANCE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Red Balance",
-		.minimum = 24,
-		.maximum = 40,
-		.step    = 1,
-		.default_value = 32,
-	    },
-	    .set_control = setredblue
-	},
-[GAMMA] = {
-	    {
-		.id      = V4L2_CID_GAMMA,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gamma",
-		.minimum = 0,
-		.maximum = 40,
-		.step    = 1,
-#define GAMMA_DEF 20
-		.default_value = GAMMA_DEF,
-	    },
-	    .set_control = setgamma
-	},
-[EXPOSURE] = {
-	    {
-		.id      = V4L2_CID_EXPOSURE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Exposure",
-		.minimum = 500,
-		.maximum = 1500,
-		.step    = 1,
-		.default_value = 1024
-	    },
-	    .set_control = setexposure
-	},
-[AUTOGAIN] = {
-	    {
-		.id      = V4L2_CID_AUTOGAIN,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Auto Gain",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 1
-	    },
-	    .set = sd_setautogain,
-	},
-[GAIN] = {
-	    {
-		.id      = V4L2_CID_GAIN,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gain",
-		.minimum = 4,
-		.maximum = 49,
-		.step    = 1,
-		.default_value = 15
-	    },
-	    .set_control = setgain
-	},
-[HFLIP] = {
-	    {
-		.id      = V4L2_CID_HFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Mirror",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = sethvflip
-	},
-[VFLIP] = {
-	    {
-		.id      = V4L2_CID_VFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Vflip",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = sethvflip
-	},
-[SHARPNESS] = {
-	    {
-		.id	 = V4L2_CID_SHARPNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Sharpness",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 90,
-	    },
-	    .set_control = setsharpness
-	},
-[ILLUM] = {
-	    {
-		.id      = V4L2_CID_ILLUMINATORS_1,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Illuminator / infrared",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = setillum
-	},
-/* ov7630/ov7648/ov7660 only */
-[FREQ] = {
-	    {
-		.id	 = V4L2_CID_POWER_LINE_FREQUENCY,
-		.type    = V4L2_CTRL_TYPE_MENU,
-		.name    = "Light frequency filter",
-		.minimum = 0,
-		.maximum = 2,	/* 0: 0, 1: 50Hz, 2:60Hz */
-		.step    = 1,
-		.default_value = 1,
-	    },
-	    .set_control = setfreq
-	},
-};
-
-/* table of the disabled controls */
-static const __u32 ctrl_dis[] = {
-[SENSOR_ADCM1700] =	(1 << EXPOSURE) |
-			(1 << AUTOGAIN) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_GC0307] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_HV7131R] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << FREQ),
-
-[SENSOR_MI0360] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_MI0360B] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_MO4000] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_MT9V111] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_OM6802] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_OV7630] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP),
-
-[SENSOR_OV7648] =	(1 << EXPOSURE) |
-			(1 << GAIN) |
-			(1 << HFLIP),
-
-[SENSOR_OV7660] =	(1 << EXPOSURE) |
-			(1 << AUTOGAIN) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP),
-
-[SENSOR_PO1030] =	(1 << EXPOSURE) |
-			(1 << AUTOGAIN) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_PO2030N] =	(1 << FREQ),
-
-[SENSOR_SOI768] =	(1 << EXPOSURE) |
-			(1 << AUTOGAIN) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-
-[SENSOR_SP80708] =	(1 << EXPOSURE) |
-			(1 << AUTOGAIN) |
-			(1 << GAIN) |
-			(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << FREQ),
-};
-
 static const struct v4l2_pix_format cif_mode[] = {
 	{352, 288, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 352,
@@ -1442,12 +1159,11 @@ static void reg_r(struct gspca_dev *gspca_dev,
 
 	if (gspca_dev->usb_err < 0)
 		return;
-#ifdef GSPCA_DEBUG
 	if (len > USB_BUF_SZ) {
-		pr_err("reg_r: buffer overflow\n");
+		PERR("reg_r: buffer overflow\n");
 		return;
 	}
-#endif
+
 	ret = usb_control_msg(gspca_dev->dev,
 			usb_rcvctrlpipe(gspca_dev->dev, 0),
 			0,
@@ -1496,12 +1212,12 @@ static void reg_w(struct gspca_dev *gspca_dev,
 		return;
 	PDEBUG(D_USBO, "reg_w [%04x] = %02x %02x ..",
 		value, buffer[0], buffer[1]);
-#ifdef GSPCA_DEBUG
+
 	if (len > USB_BUF_SZ) {
-		pr_err("reg_w: buffer overflow\n");
+		PERR("reg_w: buffer overflow\n");
 		return;
 	}
-#endif
+
 	memcpy(gspca_dev->usb_buf, buffer, len);
 	ret = usb_control_msg(gspca_dev->dev,
 			usb_sndctrlpipe(gspca_dev->dev, 0),
@@ -1822,7 +1538,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 		cam->nmodes = ARRAY_SIZE(vga_mode);
 	}
 	cam->npkt = 24;			/* 24 packets per ISOC message */
-	cam->ctrls = sd->ctrls;
 
 	sd->ag_cnt = -1;
 	sd->quality = QUALITY_DEF;
@@ -1888,9 +1603,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		break;
 	}
 
-	if (sd->sensor == SENSOR_OM6802)
-		sd->ctrls[SHARPNESS].def = 0x10;
-
 	/* Note we do not disable the sensor clock here (power saving mode),
 	   as that also disables the button on the cam. */
 	reg_w1(gspca_dev, 0xf1, 0x00);
@@ -1899,13 +1611,92 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	sn9c1xx = sn_tb[sd->sensor];
 	sd->i2c_addr = sn9c1xx[9];
 
-	gspca_dev->ctrl_dis = ctrl_dis[sd->sensor];
-	if (!(sd->flags & F_ILLUM))
-		gspca_dev->ctrl_dis |= (1 << ILLUM);
-
 	return gspca_dev->usb_err;
 }
 
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl);
+
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
+
+/* this function is called at probe time */
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 14);
+
+	sd->brightness = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+#define CONTRAST_MAX 127
+	sd->contrast = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, CONTRAST_MAX, 1, 20);
+#define COLORS_DEF 25
+	sd->saturation = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 40, 1, COLORS_DEF);
+	sd->red_bal = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_RED_BALANCE, 24, 40, 1, 32);
+	sd->blue_bal = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BLUE_BALANCE, 24, 40, 1, 32);
+#define GAMMA_DEF 20
+	sd->gamma = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAMMA, 0, 40, 1, GAMMA_DEF);
+
+	if (sd->sensor == SENSOR_OM6802)
+		sd->sharpness = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0, 255, 1, 16);
+	else
+		sd->sharpness = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0, 255, 1, 90);
+
+	if (sd->flags & F_ILLUM)
+		sd->illum = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_ILLUMINATORS_1, 0, 1, 1, 0);
+
+	if (sd->sensor == SENSOR_PO2030N) {
+		gspca_dev->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_EXPOSURE, 500, 1500, 1, 1024);
+		gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 4, 49, 1, 15);
+		sd->hflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	}
+
+	if (sd->sensor != SENSOR_ADCM1700 && sd->sensor != SENSOR_OV7660 &&
+	    sd->sensor != SENSOR_PO1030 && sd->sensor != SENSOR_SOI768 &&
+	    sd->sensor != SENSOR_SP80708)
+		gspca_dev->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+
+	if (sd->sensor == SENSOR_HV7131R || sd->sensor == SENSOR_OV7630 ||
+	    sd->sensor == SENSOR_OV7648 || sd->sensor == SENSOR_PO2030N)
+		sd->vflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	if (sd->sensor == SENSOR_OV7630 || sd->sensor == SENSOR_OV7648 ||
+	    sd->sensor == SENSOR_OV7660)
+		sd->freq = v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
+			V4L2_CID_POWER_LINE_FREQUENCY,
+			V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 0,
+			V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+
+	v4l2_ctrl_cluster(2, &sd->red_bal);
+	if (sd->sensor == SENSOR_PO2030N) {
+		v4l2_ctrl_cluster(2, &sd->vflip);
+		v4l2_ctrl_auto_cluster(3, &gspca_dev->autogain, 0, false);
+	}
+
+	return 0;
+}
+
 static u32 expo_adjust(struct gspca_dev *gspca_dev,
 			u32 expo)
 {
@@ -2014,10 +1805,9 @@ static void setbrightness(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	unsigned int expo;
-	int brightness;
+	int brightness = sd->brightness->val;
 	u8 k2;
 
-	brightness = sd->ctrls[BRIGHTNESS].val;
 	k2 = (brightness - 0x80) >> 2;
 	switch (sd->sensor) {
 	case SENSOR_ADCM1700:
@@ -2064,7 +1854,7 @@ static void setcontrast(struct gspca_dev *gspca_dev)
 	u8 k2;
 	u8 contrast[6];
 
-	k2 = sd->ctrls[CONTRAST].val * 37 / (CONTRAST_MAX + 1)
+	k2 = sd->contrast->val * 37 / (CONTRAST_MAX + 1)
 				+ 37;		/* 37..73 */
 	contrast[0] = (k2 + 1) / 2;		/* red */
 	contrast[1] = 0;
@@ -2090,7 +1880,7 @@ static void setcolors(struct gspca_dev *gspca_dev)
 		 60, -51, -9		/* VR VG VB */
 	};
 
-	colors = sd->ctrls[COLORS].val;
+	colors = sd->saturation->val;
 	if (sd->sensor == SENSOR_MI0360B)
 		uv = uv_mi0360b;
 	else
@@ -2112,14 +1902,14 @@ static void setredblue(struct gspca_dev *gspca_dev)
 			{0xc1, 0x6e, 0x16, 0x00, 0x40, 0x00, 0x00, 0x10};
 
 		/* 0x40 = normal value = gain x 1 */
-		rg1b[3] = sd->ctrls[RED].val * 2;
-		rg1b[5] = sd->ctrls[BLUE].val * 2;
+		rg1b[3] = sd->red_bal->val * 2;
+		rg1b[5] = sd->blue_bal->val * 2;
 		i2c_w8(gspca_dev, rg1b);
 		return;
 	}
-	reg_w1(gspca_dev, 0x05, sd->ctrls[RED].val);
+	reg_w1(gspca_dev, 0x05, sd->red_bal->val);
 /*	reg_w1(gspca_dev, 0x07, 32); */
-	reg_w1(gspca_dev, 0x06, sd->ctrls[BLUE].val);
+	reg_w1(gspca_dev, 0x06, sd->blue_bal->val);
 }
 
 static void setgamma(struct gspca_dev *gspca_dev)
@@ -2153,7 +1943,7 @@ static void setgamma(struct gspca_dev *gspca_dev)
 		break;
 	}
 
-	val = sd->ctrls[GAMMA].val;
+	val = sd->gamma->val;
 	for (i = 0; i < sizeof gamma; i++)
 		gamma[i] = gamma_base[i]
 			+ delta[i] * (val - GAMMA_DEF) / 32;
@@ -2168,11 +1958,11 @@ static void setexposure(struct gspca_dev *gspca_dev)
 		u8 rexpo[] =		/* 1a: expo H, 1b: expo M */
 			{0xa1, 0x6e, 0x1a, 0x00, 0x40, 0x00, 0x00, 0x10};
 
-		rexpo[3] = sd->ctrls[EXPOSURE].val >> 8;
+		rexpo[3] = gspca_dev->exposure->val >> 8;
 		i2c_w8(gspca_dev, rexpo);
 		msleep(6);
 		rexpo[2] = 0x1b;
-		rexpo[3] = sd->ctrls[EXPOSURE].val;
+		rexpo[3] = gspca_dev->exposure->val;
 		i2c_w8(gspca_dev, rexpo);
 	}
 }
@@ -2181,8 +1971,6 @@ static void setautogain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	if (gspca_dev->ctrl_dis & (1 << AUTOGAIN))
-		return;
 	switch (sd->sensor) {
 	case SENSOR_OV7630:
 	case SENSOR_OV7648: {
@@ -2192,13 +1980,13 @@ static void setautogain(struct gspca_dev *gspca_dev)
 			comb = 0xc0;
 		else
 			comb = 0xa0;
-		if (sd->ctrls[AUTOGAIN].val)
+		if (gspca_dev->autogain->val)
 			comb |= 0x03;
 		i2c_w1(&sd->gspca_dev, 0x13, comb);
 		return;
 	    }
 	}
-	if (sd->ctrls[AUTOGAIN].val)
+	if (gspca_dev->autogain->val)
 		sd->ag_cnt = AG_CNT_START;
 	else
 		sd->ag_cnt = -1;
@@ -2212,7 +2000,7 @@ static void setgain(struct gspca_dev *gspca_dev)
 		u8 rgain[] =		/* 15: gain */
 			{0xa1, 0x6e, 0x15, 0x00, 0x40, 0x00, 0x00, 0x15};
 
-		rgain[3] = sd->ctrls[GAIN].val;
+		rgain[3] = gspca_dev->gain->val;
 		i2c_w8(gspca_dev, rgain);
 	}
 }
@@ -2225,19 +2013,19 @@ static void sethvflip(struct gspca_dev *gspca_dev)
 	switch (sd->sensor) {
 	case SENSOR_HV7131R:
 		comn = 0x18;			/* clkdiv = 1, ablcen = 1 */
-		if (sd->ctrls[VFLIP].val)
+		if (sd->vflip->val)
 			comn |= 0x01;
 		i2c_w1(gspca_dev, 0x01, comn);	/* sctra */
 		break;
 	case SENSOR_OV7630:
 		comn = 0x02;
-		if (!sd->ctrls[VFLIP].val)
+		if (!sd->vflip->val)
 			comn |= 0x80;
 		i2c_w1(gspca_dev, 0x75, comn);
 		break;
 	case SENSOR_OV7648:
 		comn = 0x06;
-		if (sd->ctrls[VFLIP].val)
+		if (sd->vflip->val)
 			comn |= 0x80;
 		i2c_w1(gspca_dev, 0x75, comn);
 		break;
@@ -2251,9 +2039,9 @@ static void sethvflip(struct gspca_dev *gspca_dev)
 		 * bit3-0: X
 		 */
 		comn = 0x0a;
-		if (sd->ctrls[HFLIP].val)
+		if (sd->hflip->val)
 			comn |= 0x80;
-		if (sd->ctrls[VFLIP].val)
+		if (sd->vflip->val)
 			comn |= 0x40;
 		i2c_w1(&sd->gspca_dev, 0x1e, comn);
 		break;
@@ -2264,23 +2052,21 @@ static void setsharpness(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	reg_w1(gspca_dev, 0x99, sd->ctrls[SHARPNESS].val);
+	reg_w1(gspca_dev, 0x99, sd->sharpness->val);
 }
 
 static void setillum(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	if (gspca_dev->ctrl_dis & (1 << ILLUM))
-		return;
 	switch (sd->sensor) {
 	case SENSOR_ADCM1700:
 		reg_w1(gspca_dev, 0x02,				/* gpio */
-			sd->ctrls[ILLUM].val ? 0x64 : 0x60);
+			sd->illum->val ? 0x64 : 0x60);
 		break;
 	case SENSOR_MT9V111:
 		reg_w1(gspca_dev, 0x02,
-			sd->ctrls[ILLUM].val ? 0x77 : 0x74);
+			sd->illum->val ? 0x77 : 0x74);
 /* should have been: */
 /*						0x55 : 0x54);	* 370i */
 /*						0x66 : 0x64);	* Clip */
@@ -2292,13 +2078,11 @@ static void setfreq(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	if (gspca_dev->ctrl_dis & (1 << FREQ))
-		return;
 	if (sd->sensor == SENSOR_OV7660) {
 		u8 com8;
 
 		com8 = 0xdf;		/* auto gain/wb/expo */
-		switch (sd->ctrls[FREQ].val) {
+		switch (sd->freq->val) {
 		case 0: /* Banding filter disabled */
 			i2c_w1(gspca_dev, 0x13, com8 | 0x20);
 			break;
@@ -2326,7 +2110,7 @@ static void setfreq(struct gspca_dev *gspca_dev)
 			break;
 		}
 
-		switch (sd->ctrls[FREQ].val) {
+		switch (sd->freq->val) {
 		case 0: /* Banding filter disabled */
 			break;
 		case 1: /* 50 hz (filter on and framerate adj) */
@@ -2698,17 +2482,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	sd->reg01 = reg01;
 	sd->reg17 = reg17;
 
-	sethvflip(gspca_dev);
-	setbrightness(gspca_dev);
-	setcontrast(gspca_dev);
-	setcolors(gspca_dev);
-	setautogain(gspca_dev);
-	if (!(gspca_dev->ctrl_inac & ((1 << EXPOSURE) | (1 << GAIN)))) {
-		setexposure(gspca_dev);
-		setgain(gspca_dev);
-	}
-	setfreq(gspca_dev);
-
 	sd->pktsz = sd->npkt = 0;
 	sd->nchg = sd->short_mark = 0;
 	sd->work_thread = create_singlethread_workqueue(MODULE_NAME);
@@ -2803,9 +2576,6 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 	}
 }
 
-#define WANT_REGULAR_AUTOGAIN
-#include "autogain_functions.h"
-
 static void do_autogain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -2825,7 +2595,7 @@ static void do_autogain(struct gspca_dev *gspca_dev)
 	PDEBUG(D_FRAM, "mean lum %d", delta);
 
 	if (sd->sensor == SENSOR_PO2030N) {
-		auto_gain_n_exposure(gspca_dev, delta, luma_mean, luma_delta,
+		gspca_expo_autogain(gspca_dev, delta, luma_mean, luma_delta,
 					15, 1024);
 		return;
 	}
@@ -3042,39 +2812,53 @@ marker_found:
 	}
 }
 
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 
-	sd->ctrls[AUTOGAIN].val = val;
-	if (val)
-		gspca_dev->ctrl_inac |= (1 << EXPOSURE) | (1 << GAIN);
-	else
-		gspca_dev->ctrl_inac &= ~(1 << EXPOSURE) & ~(1 << GAIN);
-	if (gspca_dev->streaming)
-		setautogain(gspca_dev);
-	return gspca_dev->usb_err;
-}
+	gspca_dev->usb_err = 0;
 
-static int sd_querymenu(struct gspca_dev *gspca_dev,
-			struct v4l2_querymenu *menu)
-{
-	switch (menu->id) {
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev);
+		break;
+	case V4L2_CID_SATURATION:
+		setcolors(gspca_dev);
+		break;
+	case V4L2_CID_RED_BALANCE:
+		setredblue(gspca_dev);
+		break;
+	case V4L2_CID_GAMMA:
+		setgamma(gspca_dev);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		setautogain(gspca_dev);
+		setexposure(gspca_dev);
+		setgain(gspca_dev);
+		break;
+	case V4L2_CID_VFLIP:
+		sethvflip(gspca_dev);
+		break;
+	case V4L2_CID_SHARPNESS:
+		setsharpness(gspca_dev);
+		break;
+	case V4L2_CID_ILLUMINATORS_1:
+		setillum(gspca_dev);
+		break;
 	case V4L2_CID_POWER_LINE_FREQUENCY:
-		switch (menu->index) {
-		case 0:		/* V4L2_CID_POWER_LINE_FREQUENCY_DISABLED */
-			strcpy((char *) menu->name, "NoFliker");
-			return 0;
-		case 1:		/* V4L2_CID_POWER_LINE_FREQUENCY_50HZ */
-			strcpy((char *) menu->name, "50 Hz");
-			return 0;
-		case 2:		/* V4L2_CID_POWER_LINE_FREQUENCY_60HZ */
-			strcpy((char *) menu->name, "60 Hz");
-			return 0;
-		}
+		setfreq(gspca_dev);
 		break;
+	default:
+		return -EINVAL;
 	}
-	return -EINVAL;
+	return gspca_dev->usb_err;
 }
 
 #if IS_ENABLED(CONFIG_INPUT)
@@ -3099,16 +2883,14 @@ static int sd_int_pkt_scan(struct gspca_dev *gspca_dev,
 /* sub-driver description */
 static const struct sd_desc sd_desc = {
 	.name = MODULE_NAME,
-	.ctrls = sd_ctrls,
-	.nctrls = NCTRLS,
 	.config = sd_config,
 	.init = sd_init,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.stop0 = sd_stop0,
 	.pkt_scan = sd_pkt_scan,
 	.dq_callback = do_autogain,
-	.querymenu = sd_querymenu,
 #if IS_ENABLED(CONFIG_INPUT)
 	.int_pkt_scan = sd_int_pkt_scan,
 #endif
diff --git a/drivers/media/usb/gspca/spca1528.c b/drivers/media/usb/gspca/spca1528.c
index 14d635277d71..688592b289ea 100644
--- a/drivers/media/usb/gspca/spca1528.c
+++ b/drivers/media/usb/gspca/spca1528.c
@@ -146,7 +146,7 @@ static void wait_status_0(struct gspca_dev *gspca_dev)
 		w += 15;
 		msleep(w);
 	} while (--i > 0);
-	PDEBUG(D_ERR, "wait_status_0 timeout");
+	PERR("wait_status_0 timeout");
 	gspca_dev->usb_err = -ETIME;
 }
 
@@ -164,7 +164,7 @@ static void wait_status_1(struct gspca_dev *gspca_dev)
 			return;
 		}
 	} while (--i > 0);
-	PDEBUG(D_ERR, "wait_status_1 timeout");
+	PERR("wait_status_1 timeout");
 	gspca_dev->usb_err = -ETIME;
 }
 
diff --git a/drivers/media/usb/gspca/spca500.c b/drivers/media/usb/gspca/spca500.c
index 25cb68d0556d..9f8bf51fd64b 100644
--- a/drivers/media/usb/gspca/spca500.c
+++ b/drivers/media/usb/gspca/spca500.c
@@ -489,7 +489,7 @@ static int spca500_full_reset(struct gspca_dev *gspca_dev)
 		return err;
 	err = reg_r_wait(gspca_dev, 0x06, 0, 0);
 	if (err < 0) {
-		PDEBUG(D_ERR, "reg_r_wait() failed");
+		PERR("reg_r_wait() failed");
 		return err;
 	}
 	/* all ok */
@@ -505,7 +505,7 @@ static int spca500_full_reset(struct gspca_dev *gspca_dev)
 static int spca500_synch310(struct gspca_dev *gspca_dev)
 {
 	if (usb_set_interface(gspca_dev->dev, gspca_dev->iface, 0) < 0) {
-		PDEBUG(D_ERR, "Set packet size: set interface error");
+		PERR("Set packet size: set interface error");
 		goto error;
 	}
 	spca500_ping310(gspca_dev);
@@ -519,7 +519,7 @@ static int spca500_synch310(struct gspca_dev *gspca_dev)
 	if (usb_set_interface(gspca_dev->dev,
 				gspca_dev->iface,
 				gspca_dev->alt) < 0) {
-		PDEBUG(D_ERR, "Set packet size: set interface error");
+		PERR("Set packet size: set interface error");
 		goto error;
 	}
 	return 0;
@@ -544,7 +544,7 @@ static void spca500_reinit(struct gspca_dev *gspca_dev)
 	err = spca50x_setup_qtable(gspca_dev, 0x00, 0x8800, 0x8840,
 				 qtable_pocketdv);
 	if (err < 0)
-		PDEBUG(D_ERR|D_STREAM, "spca50x_setup_qtable failed on init");
+		PERR("spca50x_setup_qtable failed on init");
 
 	/* set qtable index */
 	reg_w(gspca_dev, 0x00, 0x8880, 2);
@@ -639,7 +639,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 					   0x00, 0x8800, 0x8840,
 					   qtable_creative_pccam);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca50x_setup_qtable failed");
+			PERR("spca50x_setup_qtable failed");
 		/* Init SDRAM - needed for SDRAM access */
 		reg_w(gspca_dev, 0x00, 0x870a, 0x04);
 
@@ -647,7 +647,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, 0x00, 0x8000, 0x0004);
 		msleep(500);
 		if (reg_r_wait(gspca_dev, 0, 0x8000, 0x44) != 0)
-			PDEBUG(D_ERR, "reg_r_wait() failed");
+			PERR("reg_r_wait() failed");
 
 		reg_r(gspca_dev, 0x816b, 1);
 		Data = gspca_dev->usb_buf[0];
@@ -660,13 +660,13 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		/* enable drop packet */
 		err = reg_w(gspca_dev, 0x00, 0x850a, 0x0001);
 		if (err < 0)
-			PDEBUG(D_ERR, "failed to enable drop packet");
+			PERR("failed to enable drop packet");
 		reg_w(gspca_dev, 0x00, 0x8880, 3);
 		err = spca50x_setup_qtable(gspca_dev,
 					   0x00, 0x8800, 0x8840,
 					   qtable_creative_pccam);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca50x_setup_qtable failed");
+			PERR("spca50x_setup_qtable failed");
 
 		/* Init SDRAM - needed for SDRAM access */
 		reg_w(gspca_dev, 0x00, 0x870a, 0x04);
@@ -675,7 +675,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, 0x00, 0x8000, 0x0004);
 
 		if (reg_r_wait(gspca_dev, 0, 0x8000, 0x44) != 0)
-			PDEBUG(D_ERR, "reg_r_wait() failed");
+			PERR("reg_r_wait() failed");
 
 		reg_r(gspca_dev, 0x816b, 1);
 		Data = gspca_dev->usb_buf[0];
@@ -689,18 +689,18 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		/* do a full reset */
 		err = spca500_full_reset(gspca_dev);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca500_full_reset failed");
+			PERR("spca500_full_reset failed");
 
 		/* enable drop packet */
 		err = reg_w(gspca_dev, 0x00, 0x850a, 0x0001);
 		if (err < 0)
-			PDEBUG(D_ERR, "failed to enable drop packet");
+			PERR("failed to enable drop packet");
 		reg_w(gspca_dev, 0x00, 0x8880, 3);
 		err = spca50x_setup_qtable(gspca_dev,
 					   0x00, 0x8800, 0x8840,
 					   qtable_creative_pccam);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca50x_setup_qtable failed");
+			PERR("spca50x_setup_qtable failed");
 
 		spca500_setmode(gspca_dev, xmult, ymult);
 		reg_w(gspca_dev, 0x20, 0x0001, 0x0004);
@@ -709,7 +709,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, 0x00, 0x8000, 0x0004);
 
 		if (reg_r_wait(gspca_dev, 0, 0x8000, 0x44) != 0)
-			PDEBUG(D_ERR, "reg_r_wait() failed");
+			PERR("reg_r_wait() failed");
 
 		reg_r(gspca_dev, 0x816b, 1);
 		Data = gspca_dev->usb_buf[0];
@@ -722,7 +722,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		/* do a full reset */
 		err = spca500_full_reset(gspca_dev);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca500_full_reset failed");
+			PERR("spca500_full_reset failed");
 		/* enable drop packet */
 		reg_w(gspca_dev, 0x00, 0x850a, 0x0001);
 		reg_w(gspca_dev, 0x00, 0x8880, 0);
@@ -730,7 +730,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 					   0x00, 0x8800, 0x8840,
 					   qtable_kodak_ez200);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca50x_setup_qtable failed");
+			PERR("spca50x_setup_qtable failed");
 		spca500_setmode(gspca_dev, xmult, ymult);
 
 		reg_w(gspca_dev, 0x20, 0x0001, 0x0004);
@@ -739,7 +739,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, 0x00, 0x8000, 0x0004);
 
 		if (reg_r_wait(gspca_dev, 0, 0x8000, 0x44) != 0)
-			PDEBUG(D_ERR, "reg_r_wait() failed");
+			PERR("reg_r_wait() failed");
 
 		reg_r(gspca_dev, 0x816b, 1);
 		Data = gspca_dev->usb_buf[0];
@@ -765,7 +765,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		err = spca50x_setup_qtable(gspca_dev,
 				   0x00, 0x8800, 0x8840, qtable_pocketdv);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca50x_setup_qtable failed");
+			PERR("spca50x_setup_qtable failed");
 		reg_w(gspca_dev, 0x00, 0x8880, 2);
 
 		/* familycam Quicksmart pocketDV stuff */
@@ -795,7 +795,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 					0x00, 0x8800,
 					0x8840, qtable_creative_pccam);
 		if (err < 0)
-			PDEBUG(D_ERR, "spca50x_setup_qtable failed");
+			PERR("spca50x_setup_qtable failed");
 		reg_w(gspca_dev, 0x00, 0x8880, 3);
 		reg_w(gspca_dev, 0x00, 0x800a, 0x00);
 		/* Init SDRAM - needed for SDRAM access */
diff --git a/drivers/media/usb/gspca/spca501.c b/drivers/media/usb/gspca/spca501.c
index 3b7f777785b4..d92fd17d6701 100644
--- a/drivers/media/usb/gspca/spca501.c
+++ b/drivers/media/usb/gspca/spca501.c
@@ -1756,10 +1756,11 @@ static const __u16 spca501c_mysterious_init_data[][3] = {
 	{}
 };
 
-static int reg_write(struct usb_device *dev,
-		     __u16 req, __u16 index, __u16 value)
+static int reg_write(struct gspca_dev *gspca_dev,
+					__u16 req, __u16 index, __u16 value)
 {
 	int ret;
+	struct usb_device *dev = gspca_dev->dev;
 
 	ret = usb_control_msg(dev,
 			usb_sndctrlpipe(dev, 0),
@@ -1774,17 +1775,15 @@ static int reg_write(struct usb_device *dev,
 }
 
 
-static int write_vector(struct gspca_dev *gspca_dev,
-			const __u16 data[][3])
+static int write_vector(struct gspca_dev *gspca_dev, const __u16 data[][3])
 {
-	struct usb_device *dev = gspca_dev->dev;
 	int ret, i = 0;
 
 	while (data[i][0] != 0 || data[i][1] != 0 || data[i][2] != 0) {
-		ret = reg_write(dev, data[i][0], data[i][2], data[i][1]);
+		ret = reg_write(gspca_dev, data[i][0], data[i][2],
+								data[i][1]);
 		if (ret < 0) {
-			PDEBUG(D_ERR,
-				"Reg write failed for 0x%02x,0x%02x,0x%02x",
+			PERR("Reg write failed for 0x%02x,0x%02x,0x%02x",
 				data[i][0], data[i][1], data[i][2]);
 			return ret;
 		}
@@ -1795,30 +1794,28 @@ static int write_vector(struct gspca_dev *gspca_dev,
 
 static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
-	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x12, val);
+	reg_write(gspca_dev, SPCA501_REG_CCDSP, 0x12, val);
 }
 
 static void setcontrast(struct gspca_dev *gspca_dev, s32 val)
 {
-	reg_write(gspca_dev->dev, 0x00, 0x00,
-				  (val >> 8) & 0xff);
-	reg_write(gspca_dev->dev, 0x00, 0x01,
-				  val & 0xff);
+	reg_write(gspca_dev, 0x00, 0x00, (val >> 8) & 0xff);
+	reg_write(gspca_dev, 0x00, 0x01, val & 0xff);
 }
 
 static void setcolors(struct gspca_dev *gspca_dev, s32 val)
 {
-	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x0c, val);
+	reg_write(gspca_dev, SPCA501_REG_CCDSP, 0x0c, val);
 }
 
 static void setblue_balance(struct gspca_dev *gspca_dev, s32 val)
 {
-	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x11, val);
+	reg_write(gspca_dev, SPCA501_REG_CCDSP, 0x11, val);
 }
 
 static void setred_balance(struct gspca_dev *gspca_dev, s32 val)
 {
-	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x13, val);
+	reg_write(gspca_dev, SPCA501_REG_CCDSP, 0x13, val);
 }
 
 /* this function is called at probe time */
@@ -1868,7 +1865,6 @@ error:
 static int sd_start(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct usb_device *dev = gspca_dev->dev;
 	int mode;
 
 	switch (sd->subtype) {
@@ -1895,20 +1891,20 @@ static int sd_start(struct gspca_dev *gspca_dev)
 
 	/* Enable ISO packet machine CTRL reg=2,
 	 * index=1 bitmask=0x2 (bit ordinal 1) */
-	reg_write(dev, SPCA50X_REG_USB, 0x6, 0x94);
+	reg_write(gspca_dev, SPCA50X_REG_USB, 0x6, 0x94);
 	switch (mode) {
 	case 0: /* 640x480 */
-		reg_write(dev, SPCA50X_REG_USB, 0x07, 0x004a);
+		reg_write(gspca_dev, SPCA50X_REG_USB, 0x07, 0x004a);
 		break;
 	case 1: /* 320x240 */
-		reg_write(dev, SPCA50X_REG_USB, 0x07, 0x104a);
+		reg_write(gspca_dev, SPCA50X_REG_USB, 0x07, 0x104a);
 		break;
 	default:
 /*	case 2:  * 160x120 */
-		reg_write(dev, SPCA50X_REG_USB, 0x07, 0x204a);
+		reg_write(gspca_dev, SPCA50X_REG_USB, 0x07, 0x204a);
 		break;
 	}
-	reg_write(dev, SPCA501_REG_CTLRL, 0x01, 0x02);
+	reg_write(gspca_dev, SPCA501_REG_CTLRL, 0x01, 0x02);
 
 	return 0;
 }
@@ -1917,7 +1913,7 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
 {
 	/* Disable ISO packet
 	 * machine CTRL reg=2, index=1 bitmask=0x0 (bit ordinal 1) */
-	reg_write(gspca_dev->dev, SPCA501_REG_CTLRL, 0x01, 0x00);
+	reg_write(gspca_dev, SPCA501_REG_CTLRL, 0x01, 0x00);
 }
 
 /* called on streamoff with alt 0 and on disconnect */
@@ -1925,7 +1921,7 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 {
 	if (!gspca_dev->present)
 		return;
-	reg_write(gspca_dev->dev, SPCA501_REG_CTLRL, 0x05, 0x00);
+	reg_write(gspca_dev, SPCA501_REG_CTLRL, 0x05, 0x00);
 }
 
 static void sd_pkt_scan(struct gspca_dev *gspca_dev,
diff --git a/drivers/media/usb/gspca/spca505.c b/drivers/media/usb/gspca/spca505.c
index bc7d67c3cb04..232b330d2dd3 100644
--- a/drivers/media/usb/gspca/spca505.c
+++ b/drivers/media/usb/gspca/spca505.c
@@ -544,10 +544,11 @@ static const u8 spca505b_open_data_ccd[][3] = {
 	{}
 };
 
-static int reg_write(struct usb_device *dev,
+static int reg_write(struct gspca_dev *gspca_dev,
 		     u16 req, u16 index, u16 value)
 {
 	int ret;
+	struct usb_device *dev = gspca_dev->dev;
 
 	ret = usb_control_msg(dev,
 			usb_sndctrlpipe(dev, 0),
@@ -584,11 +585,11 @@ static int reg_read(struct gspca_dev *gspca_dev,
 static int write_vector(struct gspca_dev *gspca_dev,
 			const u8 data[][3])
 {
-	struct usb_device *dev = gspca_dev->dev;
 	int ret, i = 0;
 
 	while (data[i][0] != 0) {
-		ret = reg_write(dev, data[i][0], data[i][2], data[i][1]);
+		ret = reg_write(gspca_dev, data[i][0], data[i][2],
+								data[i][1]);
 		if (ret < 0)
 			return ret;
 		i++;
@@ -629,14 +630,13 @@ static int sd_init(struct gspca_dev *gspca_dev)
 
 static void setbrightness(struct gspca_dev *gspca_dev, s32 brightness)
 {
-	reg_write(gspca_dev->dev, 0x05, 0x00, (255 - brightness) >> 6);
-	reg_write(gspca_dev->dev, 0x05, 0x01, (255 - brightness) << 2);
+	reg_write(gspca_dev, 0x05, 0x00, (255 - brightness) >> 6);
+	reg_write(gspca_dev, 0x05, 0x01, (255 - brightness) << 2);
 }
 
 static int sd_start(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct usb_device *dev = gspca_dev->dev;
 	int ret, mode;
 	static u8 mode_tb[][3] = {
 	/*	  r00   r06   r07	*/
@@ -654,9 +654,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	ret = reg_read(gspca_dev, 0x06, 0x16);
 
 	if (ret < 0) {
-		PDEBUG(D_ERR|D_CONF,
-		       "register read failed err: %d",
-		       ret);
+		PERR("register read failed err: %d", ret);
 		return ret;
 	}
 	if (ret != 0x0101) {
@@ -664,22 +662,22 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		       ret);
 	}
 
-	ret = reg_write(gspca_dev->dev, 0x06, 0x16, 0x0a);
+	ret = reg_write(gspca_dev, 0x06, 0x16, 0x0a);
 	if (ret < 0)
 		return ret;
-	reg_write(gspca_dev->dev, 0x05, 0xc2, 0x12);
+	reg_write(gspca_dev, 0x05, 0xc2, 0x12);
 
 	/* necessary because without it we can see stream
 	 * only once after loading module */
 	/* stopping usb registers Tomasz change */
-	reg_write(dev, 0x02, 0x00, 0x00);
+	reg_write(gspca_dev, 0x02, 0x00, 0x00);
 
 	mode = gspca_dev->cam.cam_mode[(int) gspca_dev->curr_mode].priv;
-	reg_write(dev, SPCA50X_REG_COMPRESS, 0x00, mode_tb[mode][0]);
-	reg_write(dev, SPCA50X_REG_COMPRESS, 0x06, mode_tb[mode][1]);
-	reg_write(dev, SPCA50X_REG_COMPRESS, 0x07, mode_tb[mode][2]);
+	reg_write(gspca_dev, SPCA50X_REG_COMPRESS, 0x00, mode_tb[mode][0]);
+	reg_write(gspca_dev, SPCA50X_REG_COMPRESS, 0x06, mode_tb[mode][1]);
+	reg_write(gspca_dev, SPCA50X_REG_COMPRESS, 0x07, mode_tb[mode][2]);
 
-	return reg_write(dev, SPCA50X_REG_USB,
+	return reg_write(gspca_dev, SPCA50X_REG_USB,
 			 SPCA50X_USB_CTRL,
 			 SPCA50X_CUSB_ENABLE);
 }
@@ -687,7 +685,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 static void sd_stopN(struct gspca_dev *gspca_dev)
 {
 	/* Disable ISO packet machine */
-	reg_write(gspca_dev->dev, 0x02, 0x00, 0x00);
+	reg_write(gspca_dev, 0x02, 0x00, 0x00);
 }
 
 /* called on streamoff with alt 0 and on disconnect */
@@ -697,11 +695,11 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 		return;
 
 	/* This maybe reset or power control */
-	reg_write(gspca_dev->dev, 0x03, 0x03, 0x20);
-	reg_write(gspca_dev->dev, 0x03, 0x01, 0x00);
-	reg_write(gspca_dev->dev, 0x03, 0x00, 0x01);
-	reg_write(gspca_dev->dev, 0x05, 0x10, 0x01);
-	reg_write(gspca_dev->dev, 0x05, 0x11, 0x0f);
+	reg_write(gspca_dev, 0x03, 0x03, 0x20);
+	reg_write(gspca_dev, 0x03, 0x01, 0x00);
+	reg_write(gspca_dev, 0x03, 0x00, 0x01);
+	reg_write(gspca_dev, 0x05, 0x10, 0x01);
+	reg_write(gspca_dev, 0x05, 0x11, 0x0f);
 }
 
 static void sd_pkt_scan(struct gspca_dev *gspca_dev,
diff --git a/drivers/media/usb/gspca/spca508.c b/drivers/media/usb/gspca/spca508.c
index 1286b4170b88..75f2beb2ea5a 100644
--- a/drivers/media/usb/gspca/spca508.c
+++ b/drivers/media/usb/gspca/spca508.c
@@ -1241,10 +1241,10 @@ static const u16 spca508_vista_init_data[][2] = {
 	{}
 };
 
-static int reg_write(struct usb_device *dev,
-			u16 index, u16 value)
+static int reg_write(struct gspca_dev *gspca_dev, u16 index, u16 value)
 {
 	int ret;
+	struct usb_device *dev = gspca_dev->dev;
 
 	ret = usb_control_msg(dev,
 			usb_sndctrlpipe(dev, 0),
@@ -1286,22 +1286,21 @@ static int reg_read(struct gspca_dev *gspca_dev,
 static int ssi_w(struct gspca_dev *gspca_dev,
 		u16 reg, u16 val)
 {
-	struct usb_device *dev = gspca_dev->dev;
 	int ret, retry;
 
-	ret = reg_write(dev, 0x8802, reg >> 8);
+	ret = reg_write(gspca_dev, 0x8802, reg >> 8);
 	if (ret < 0)
 		goto out;
-	ret = reg_write(dev, 0x8801, reg & 0x00ff);
+	ret = reg_write(gspca_dev, 0x8801, reg & 0x00ff);
 	if (ret < 0)
 		goto out;
 	if ((reg & 0xff00) == 0x1000) {		/* if 2 bytes */
-		ret = reg_write(dev, 0x8805, val & 0x00ff);
+		ret = reg_write(gspca_dev, 0x8805, val & 0x00ff);
 		if (ret < 0)
 			goto out;
 		val >>= 8;
 	}
-	ret = reg_write(dev, 0x8800, val);
+	ret = reg_write(gspca_dev, 0x8800, val);
 	if (ret < 0)
 		goto out;
 
@@ -1314,8 +1313,7 @@ static int ssi_w(struct gspca_dev *gspca_dev,
 		if (gspca_dev->usb_buf[0] == 0)
 			break;
 		if (--retry <= 0) {
-			PDEBUG(D_ERR, "ssi_w busy %02x",
-					gspca_dev->usb_buf[0]);
+			PERR("ssi_w busy %02x", gspca_dev->usb_buf[0]);
 			ret = -1;
 			break;
 		}
@@ -1329,7 +1327,6 @@ out:
 static int write_vector(struct gspca_dev *gspca_dev,
 			const u16 (*data)[2])
 {
-	struct usb_device *dev = gspca_dev->dev;
 	int ret = 0;
 
 	while ((*data)[1] != 0) {
@@ -1337,7 +1334,8 @@ static int write_vector(struct gspca_dev *gspca_dev,
 			if ((*data)[1] == 0xdd00)	/* delay */
 				msleep((*data)[0]);
 			else
-				ret = reg_write(dev, (*data)[1], (*data)[0]);
+				ret = reg_write(gspca_dev, (*data)[1],
+								(*data)[0]);
 		} else {
 			ret = ssi_w(gspca_dev, (*data)[1], (*data)[0]);
 		}
@@ -1363,8 +1361,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 		spca508cs110_init_data,		/* MicroInnovationIC200 4 */
 		spca508_init_data,		/* ViewQuestVQ110 5 */
 	};
-
-#ifdef GSPCA_DEBUG
 	int data1, data2;
 
 	/* Read from global register the USB product and vendor IDs, just to
@@ -1381,7 +1377,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 
 	data1 = reg_read(gspca_dev, 0x8621);
 	PDEBUG(D_PROBE, "Window 1 average luminance: %d", data1);
-#endif
 
 	cam = &gspca_dev->cam;
 	cam->cam_mode = sif_mode;
@@ -1404,26 +1399,26 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	int mode;
 
 	mode = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].priv;
-	reg_write(gspca_dev->dev, 0x8500, mode);
+	reg_write(gspca_dev, 0x8500, mode);
 	switch (mode) {
 	case 0:
 	case 1:
-		reg_write(gspca_dev->dev, 0x8700, 0x28);	/* clock */
+		reg_write(gspca_dev, 0x8700, 0x28); /* clock */
 		break;
 	default:
 /*	case 2: */
 /*	case 3: */
-		reg_write(gspca_dev->dev, 0x8700, 0x23);	/* clock */
+		reg_write(gspca_dev, 0x8700, 0x23); /* clock */
 		break;
 	}
-	reg_write(gspca_dev->dev, 0x8112, 0x10 | 0x20);
+	reg_write(gspca_dev, 0x8112, 0x10 | 0x20);
 	return 0;
 }
 
 static void sd_stopN(struct gspca_dev *gspca_dev)
 {
 	/* Video ISO disable, Video Drop Packet enable: */
-	reg_write(gspca_dev->dev, 0x8112, 0x20);
+	reg_write(gspca_dev, 0x8112, 0x20);
 }
 
 static void sd_pkt_scan(struct gspca_dev *gspca_dev,
@@ -1450,10 +1445,10 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 static void setbrightness(struct gspca_dev *gspca_dev, s32 brightness)
 {
 	/* MX seem contrast */
-	reg_write(gspca_dev->dev, 0x8651, brightness);
-	reg_write(gspca_dev->dev, 0x8652, brightness);
-	reg_write(gspca_dev->dev, 0x8653, brightness);
-	reg_write(gspca_dev->dev, 0x8654, brightness);
+	reg_write(gspca_dev, 0x8651, brightness);
+	reg_write(gspca_dev, 0x8652, brightness);
+	reg_write(gspca_dev, 0x8653, brightness);
+	reg_write(gspca_dev, 0x8654, brightness);
 }
 
 static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
diff --git a/drivers/media/usb/gspca/spca561.c b/drivers/media/usb/gspca/spca561.c
index d1db3d8f6522..403d71cd65d9 100644
--- a/drivers/media/usb/gspca/spca561.c
+++ b/drivers/media/usb/gspca/spca561.c
@@ -285,9 +285,10 @@ static const __u16 spca561_161rev12A_data2[][2] = {
 	{}
 };
 
-static void reg_w_val(struct usb_device *dev, __u16 index, __u8 value)
+static void reg_w_val(struct gspca_dev *gspca_dev, __u16 index, __u8 value)
 {
 	int ret;
+	struct usb_device *dev = gspca_dev->dev;
 
 	ret = usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
 			      0,		/* request */
@@ -301,12 +302,11 @@ static void reg_w_val(struct usb_device *dev, __u16 index, __u8 value)
 static void write_vector(struct gspca_dev *gspca_dev,
 			const __u16 data[][2])
 {
-	struct usb_device *dev = gspca_dev->dev;
 	int i;
 
 	i = 0;
 	while (data[i][1] != 0) {
-		reg_w_val(dev, data[i][1], data[i][0]);
+		reg_w_val(gspca_dev, data[i][1], data[i][0]);
 		i++;
 	}
 }
@@ -339,9 +339,9 @@ static void i2c_write(struct gspca_dev *gspca_dev, __u16 value, __u16 reg)
 {
 	int retry = 60;
 
-	reg_w_val(gspca_dev->dev, 0x8801, reg);
-	reg_w_val(gspca_dev->dev, 0x8805, value);
-	reg_w_val(gspca_dev->dev, 0x8800, value >> 8);
+	reg_w_val(gspca_dev, 0x8801, reg);
+	reg_w_val(gspca_dev, 0x8805, value);
+	reg_w_val(gspca_dev, 0x8800, value >> 8);
 	do {
 		reg_r(gspca_dev, 0x8803, 1);
 		if (!gspca_dev->usb_buf[0])
@@ -355,9 +355,9 @@ static int i2c_read(struct gspca_dev *gspca_dev, __u16 reg, __u8 mode)
 	int retry = 60;
 	__u8 value;
 
-	reg_w_val(gspca_dev->dev, 0x8804, 0x92);
-	reg_w_val(gspca_dev->dev, 0x8801, reg);
-	reg_w_val(gspca_dev->dev, 0x8802, mode | 0x01);
+	reg_w_val(gspca_dev, 0x8804, 0x92);
+	reg_w_val(gspca_dev, 0x8801, reg);
+	reg_w_val(gspca_dev, 0x8802, mode | 0x01);
 	do {
 		reg_r(gspca_dev, 0x8803, 1);
 		if (!gspca_dev->usb_buf[0]) {
@@ -459,14 +459,13 @@ static int sd_init_72a(struct gspca_dev *gspca_dev)
 	write_sensor_72a(gspca_dev, rev72a_init_sensor1);
 	write_vector(gspca_dev, rev72a_init_data2);
 	write_sensor_72a(gspca_dev, rev72a_init_sensor2);
-	reg_w_val(gspca_dev->dev, 0x8112, 0x30);
+	reg_w_val(gspca_dev, 0x8112, 0x30);
 	return 0;
 }
 
 static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct usb_device *dev = gspca_dev->dev;
 	__u16 reg;
 
 	if (sd->chip_revision == Rev012A)
@@ -474,16 +473,15 @@ static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 	else
 		reg = 0x8611;
 
-	reg_w_val(dev, reg + 0, val);		/* R */
-	reg_w_val(dev, reg + 1, val);		/* Gr */
-	reg_w_val(dev, reg + 2, val);		/* B */
-	reg_w_val(dev, reg + 3, val);		/* Gb */
+	reg_w_val(gspca_dev, reg + 0, val);		/* R */
+	reg_w_val(gspca_dev, reg + 1, val);		/* Gr */
+	reg_w_val(gspca_dev, reg + 2, val);		/* B */
+	reg_w_val(gspca_dev, reg + 3, val);		/* Gb */
 }
 
 static void setwhite(struct gspca_dev *gspca_dev, s32 white, s32 contrast)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct usb_device *dev = gspca_dev->dev;
 	__u8 blue, red;
 	__u16 reg;
 
@@ -496,11 +494,11 @@ static void setwhite(struct gspca_dev *gspca_dev, s32 white, s32 contrast)
 		reg = 0x8651;
 		red += contrast - 0x20;
 		blue += contrast - 0x20;
-		reg_w_val(dev, 0x8652, contrast + 0x20); /* Gr */
-		reg_w_val(dev, 0x8654, contrast + 0x20); /* Gb */
+		reg_w_val(gspca_dev, 0x8652, contrast + 0x20); /* Gr */
+		reg_w_val(gspca_dev, 0x8654, contrast + 0x20); /* Gb */
 	}
-	reg_w_val(dev, reg, red);
-	reg_w_val(dev, reg + 2, blue);
+	reg_w_val(gspca_dev, reg, red);
+	reg_w_val(gspca_dev, reg + 2, blue);
 }
 
 /* rev 12a only */
@@ -570,7 +568,6 @@ static void setautogain(struct gspca_dev *gspca_dev, s32 val)
 
 static int sd_start_12a(struct gspca_dev *gspca_dev)
 {
-	struct usb_device *dev = gspca_dev->dev;
 	int mode;
 	static const __u8 Reg8391[8] =
 		{0x92, 0x30, 0x20, 0x00, 0x0c, 0x00, 0x00, 0x00};
@@ -578,34 +575,33 @@ static int sd_start_12a(struct gspca_dev *gspca_dev)
 	mode = gspca_dev->cam.cam_mode[(int) gspca_dev->curr_mode].priv;
 	if (mode <= 1) {
 		/* Use compression on 320x240 and above */
-		reg_w_val(dev, 0x8500, 0x10 | mode);
+		reg_w_val(gspca_dev, 0x8500, 0x10 | mode);
 	} else {
 		/* I couldn't get the compression to work below 320x240
 		 * Fortunately at these resolutions the bandwidth
 		 * is sufficient to push raw frames at ~20fps */
-		reg_w_val(dev, 0x8500, mode);
+		reg_w_val(gspca_dev, 0x8500, mode);
 	}		/* -- qq@kuku.eu.org */
 
 	gspca_dev->usb_buf[0] = 0xaa;
 	gspca_dev->usb_buf[1] = 0x00;
 	reg_w_buf(gspca_dev, 0x8307, 2);
 	/* clock - lower 0x8X values lead to fps > 30 */
-	reg_w_val(gspca_dev->dev, 0x8700, 0x8a);
+	reg_w_val(gspca_dev, 0x8700, 0x8a);
 					/* 0x8f 0x85 0x27 clock */
-	reg_w_val(gspca_dev->dev, 0x8112, 0x1e | 0x20);
-	reg_w_val(gspca_dev->dev, 0x850b, 0x03);
+	reg_w_val(gspca_dev, 0x8112, 0x1e | 0x20);
+	reg_w_val(gspca_dev, 0x850b, 0x03);
 	memcpy(gspca_dev->usb_buf, Reg8391, 8);
 	reg_w_buf(gspca_dev, 0x8391, 8);
 	reg_w_buf(gspca_dev, 0x8390, 8);
 
 	/* Led ON (bit 3 -> 0 */
-	reg_w_val(gspca_dev->dev, 0x8114, 0x00);
+	reg_w_val(gspca_dev, 0x8114, 0x00);
 	return 0;
 }
 static int sd_start_72a(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	struct usb_device *dev = gspca_dev->dev;
 	int Clck;
 	int mode;
 
@@ -630,15 +626,15 @@ static int sd_start_72a(struct gspca_dev *gspca_dev)
 		Clck = 0x21;
 		break;
 	}
-	reg_w_val(dev, 0x8700, Clck);	/* 0x27 clock */
-	reg_w_val(dev, 0x8702, 0x81);
-	reg_w_val(dev, 0x8500, mode);	/* mode */
+	reg_w_val(gspca_dev, 0x8700, Clck);	/* 0x27 clock */
+	reg_w_val(gspca_dev, 0x8702, 0x81);
+	reg_w_val(gspca_dev, 0x8500, mode);	/* mode */
 	write_sensor_72a(gspca_dev, rev72a_init_sensor2);
 	setwhite(gspca_dev, v4l2_ctrl_g_ctrl(sd->hue),
 			v4l2_ctrl_g_ctrl(sd->contrast));
 /*	setbrightness(gspca_dev);	 * fixme: bad values */
 	setautogain(gspca_dev, v4l2_ctrl_g_ctrl(sd->autogain));
-	reg_w_val(dev, 0x8112, 0x10 | 0x20);
+	reg_w_val(gspca_dev, 0x8112, 0x10 | 0x20);
 	return 0;
 }
 
@@ -647,12 +643,12 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	if (sd->chip_revision == Rev012A) {
-		reg_w_val(gspca_dev->dev, 0x8112, 0x0e);
+		reg_w_val(gspca_dev, 0x8112, 0x0e);
 		/* Led Off (bit 3 -> 1 */
-		reg_w_val(gspca_dev->dev, 0x8114, 0x08);
+		reg_w_val(gspca_dev, 0x8114, 0x08);
 	} else {
-		reg_w_val(gspca_dev->dev, 0x8112, 0x20);
-/*		reg_w_val(gspca_dev->dev, 0x8102, 0x00); ?? */
+		reg_w_val(gspca_dev, 0x8112, 0x20);
+/*		reg_w_val(gspca_dev, 0x8102, 0x00); ?? */
 	}
 }
 
@@ -736,7 +732,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 
 		/* This should never happen */
 		if (len < 2) {
-			PDEBUG(D_ERR, "Short SOF packet, ignoring");
+			PERR("Short SOF packet, ignoring");
 			gspca_dev->last_packet_type = DISCARD_PACKET;
 			return;
 		}
diff --git a/drivers/media/usb/gspca/sq905.c b/drivers/media/usb/gspca/sq905.c
index 1d99f10a3e19..a7ae0ec9fa91 100644
--- a/drivers/media/usb/gspca/sq905.c
+++ b/drivers/media/usb/gspca/sq905.c
@@ -387,7 +387,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	}
 
 	if (ret < 0) {
-		PDEBUG(D_ERR, "Start streaming command failed");
+		PERR("Start streaming command failed");
 		return ret;
 	}
 	/* Start the workqueue function to do the streaming */
diff --git a/drivers/media/usb/gspca/sq905c.c b/drivers/media/usb/gspca/sq905c.c
index 410cdcbb55d4..acb19fb9a3df 100644
--- a/drivers/media/usb/gspca/sq905c.c
+++ b/drivers/media/usb/gspca/sq905c.c
@@ -215,13 +215,13 @@ static int sd_config(struct gspca_dev *gspca_dev,
 
 	ret = sq905c_command(gspca_dev, SQ905C_GET_ID, 0);
 	if (ret < 0) {
-		PDEBUG(D_ERR, "Get version command failed");
+		PERR("Get version command failed");
 		return ret;
 	}
 
 	ret = sq905c_read(gspca_dev, 0xf5, 0, 20);
 	if (ret < 0) {
-		PDEBUG(D_ERR, "Reading version command failed");
+		PERR("Reading version command failed");
 		return ret;
 	}
 	/* Note we leave out the usb id and the manufacturing date */
@@ -286,7 +286,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	}
 
 	if (ret < 0) {
-		PDEBUG(D_ERR, "Start streaming command failed");
+		PERR("Start streaming command failed");
 		return ret;
 	}
 	/* Start the workqueue function to do the streaming */
diff --git a/drivers/media/usb/gspca/sq930x.c b/drivers/media/usb/gspca/sq930x.c
index 7e8748b31e85..b10d0821111c 100644
--- a/drivers/media/usb/gspca/sq930x.c
+++ b/drivers/media/usb/gspca/sq930x.c
@@ -541,13 +541,11 @@ static void ucbus_write(struct gspca_dev *gspca_dev,
 	if (gspca_dev->usb_err < 0)
 		return;
 
-#ifdef GSPCA_DEBUG
 	if ((batchsize - 1) * 3 > USB_BUF_SZ) {
-		pr_err("Bug: usb_buf overflow\n");
+		PERR("Bug: usb_buf overflow\n");
 		gspca_dev->usb_err = -ENOMEM;
 		return;
 	}
-#endif
 
 	for (;;) {
 		len = ncmds;
diff --git a/drivers/media/usb/gspca/stv0680.c b/drivers/media/usb/gspca/stv0680.c
index 67605272aaa8..9c0827631b9c 100644
--- a/drivers/media/usb/gspca/stv0680.c
+++ b/drivers/media/usb/gspca/stv0680.c
@@ -86,7 +86,7 @@ static int stv_sndctrl(struct gspca_dev *gspca_dev, int set, u8 req, u16 val,
 static int stv0680_handle_error(struct gspca_dev *gspca_dev, int ret)
 {
 	stv_sndctrl(gspca_dev, 0, 0x80, 0, 0x02); /* Get Last Error */
-	PDEBUG(D_ERR, "last error: %i,  command = 0x%x",
+	PERR("last error: %i,  command = 0x%x",
 	       gspca_dev->usb_buf[0], gspca_dev->usb_buf[1]);
 	return ret;
 }
@@ -98,7 +98,7 @@ static int stv0680_get_video_mode(struct gspca_dev *gspca_dev)
 	gspca_dev->usb_buf[0] = 0x0f;
 
 	if (stv_sndctrl(gspca_dev, 0, 0x87, 0, 0x08) != 0x08) {
-		PDEBUG(D_ERR, "Get_Camera_Mode failed");
+		PERR("Get_Camera_Mode failed");
 		return stv0680_handle_error(gspca_dev, -EIO);
 	}
 
@@ -116,13 +116,13 @@ static int stv0680_set_video_mode(struct gspca_dev *gspca_dev, u8 mode)
 	gspca_dev->usb_buf[0] = mode;
 
 	if (stv_sndctrl(gspca_dev, 3, 0x07, 0x0100, 0x08) != 0x08) {
-		PDEBUG(D_ERR, "Set_Camera_Mode failed");
+		PERR("Set_Camera_Mode failed");
 		return stv0680_handle_error(gspca_dev, -EIO);
 	}
 
 	/* Verify we got what we've asked for */
 	if (stv0680_get_video_mode(gspca_dev) != mode) {
-		PDEBUG(D_ERR, "Error setting camera video mode!");
+		PERR("Error setting camera video mode!");
 		return -EIO;
 	}
 
@@ -146,7 +146,7 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	/* ping camera to be sure STV0680 is present */
 	if (stv_sndctrl(gspca_dev, 0, 0x88, 0x5678, 0x02) != 0x02 ||
 	    gspca_dev->usb_buf[0] != 0x56 || gspca_dev->usb_buf[1] != 0x78) {
-		PDEBUG(D_ERR, "STV(e): camera ping failed!!");
+		PERR("STV(e): camera ping failed!!");
 		return stv0680_handle_error(gspca_dev, -ENODEV);
 	}
 
@@ -156,7 +156,7 @@ static int sd_config(struct gspca_dev *gspca_dev,
 
 	if (stv_sndctrl(gspca_dev, 2, 0x06, 0x0200, 0x22) != 0x22 ||
 	    gspca_dev->usb_buf[7] != 0xa0 || gspca_dev->usb_buf[8] != 0x23) {
-		PDEBUG(D_ERR, "Could not get descriptor 0200.");
+		PERR("Could not get descriptor 0200.");
 		return stv0680_handle_error(gspca_dev, -ENODEV);
 	}
 	if (stv_sndctrl(gspca_dev, 0, 0x8a, 0, 0x02) != 0x02)
@@ -167,7 +167,7 @@ static int sd_config(struct gspca_dev *gspca_dev,
 		return stv0680_handle_error(gspca_dev, -ENODEV);
 
 	if (!(gspca_dev->usb_buf[7] & 0x09)) {
-		PDEBUG(D_ERR, "Camera supports neither CIF nor QVGA mode");
+		PERR("Camera supports neither CIF nor QVGA mode");
 		return -ENODEV;
 	}
 	if (gspca_dev->usb_buf[7] & 0x01)
diff --git a/drivers/media/usb/gspca/stv06xx/stv06xx.c b/drivers/media/usb/gspca/stv06xx/stv06xx.c
index 657160b4a1f7..55ee7a61c67f 100644
--- a/drivers/media/usb/gspca/stv06xx/stv06xx.c
+++ b/drivers/media/usb/gspca/stv06xx/stv06xx.c
@@ -42,8 +42,10 @@ static bool dump_sensor;
 int stv06xx_write_bridge(struct sd *sd, u16 address, u16 i2c_data)
 {
 	int err;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
+
 	u8 len = (i2c_data > 0xff) ? 2 : 1;
 
 	buf[0] = i2c_data & 0xff;
@@ -62,6 +64,7 @@ int stv06xx_write_bridge(struct sd *sd, u16 address, u16 i2c_data)
 int stv06xx_read_bridge(struct sd *sd, u16 address, u8 *i2c_data)
 {
 	int err;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
 
@@ -110,6 +113,7 @@ static int stv06xx_write_sensor_finish(struct sd *sd)
 int stv06xx_write_sensor_bytes(struct sd *sd, const u8 *data, u8 len)
 {
 	int err, i, j;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
 
@@ -139,6 +143,7 @@ int stv06xx_write_sensor_bytes(struct sd *sd, const u8 *data, u8 len)
 int stv06xx_write_sensor_words(struct sd *sd, const u16 *data, u8 len)
 {
 	int err, i, j;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
 
@@ -170,6 +175,7 @@ int stv06xx_write_sensor_words(struct sd *sd, const u16 *data, u8 len)
 int stv06xx_read_sensor(struct sd *sd, const u8 address, u16 *value)
 {
 	int err;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	struct usb_device *udev = sd->gspca_dev.dev;
 	__u8 *buf = sd->gspca_dev.usb_buf;
 
@@ -283,7 +289,7 @@ static int stv06xx_start(struct gspca_dev *gspca_dev)
 	intf = usb_ifnum_to_if(sd->gspca_dev.dev, sd->gspca_dev.iface);
 	alt = usb_altnum_to_altsetting(intf, sd->gspca_dev.alt);
 	if (!alt) {
-		PDEBUG(D_ERR, "Couldn't get altsetting");
+		PERR("Couldn't get altsetting");
 		return -EIO;
 	}
 
@@ -341,7 +347,7 @@ static int stv06xx_isoc_nego(struct gspca_dev *gspca_dev)
 
 	ret = usb_set_interface(gspca_dev->dev, gspca_dev->iface, 1);
 	if (ret < 0)
-		PDEBUG(D_ERR|D_STREAM, "set alt 1 err %d", ret);
+		PERR("set alt 1 err %d", ret);
 
 	return ret;
 }
@@ -406,7 +412,7 @@ static void stv06xx_pkt_scan(struct gspca_dev *gspca_dev,
 		len -= 4;
 
 		if (len < chunk_len) {
-			PDEBUG(D_ERR, "URB packet length is smaller"
+			PERR("URB packet length is smaller"
 				" than the specified chunk length");
 			gspca_dev->last_packet_type = DISCARD_PACKET;
 			return;
@@ -449,7 +455,7 @@ frame_data:
 				sd->to_skip = gspca_dev->width * 4;
 
 			if (chunk_len)
-				PDEBUG(D_ERR, "Chunk length is "
+				PERR("Chunk length is "
 					      "non-zero on a SOF");
 			break;
 
@@ -463,7 +469,7 @@ frame_data:
 					NULL, 0);
 
 			if (chunk_len)
-				PDEBUG(D_ERR, "Chunk length is "
+				PERR("Chunk length is "
 					      "non-zero on a EOF");
 			break;
 
@@ -596,7 +602,6 @@ MODULE_DEVICE_TABLE(usb, device_table);
 static int sd_probe(struct usb_interface *intf,
 			const struct usb_device_id *id)
 {
-	PDEBUG(D_PROBE, "Probing for a stv06xx device");
 	return gspca_dev_probe(intf, id, &sd_desc, sizeof(struct sd),
 			       THIS_MODULE);
 }
diff --git a/drivers/media/usb/gspca/stv06xx/stv06xx_hdcs.c b/drivers/media/usb/gspca/stv06xx/stv06xx_hdcs.c
index 06fa54c5efb2..2220b70d47e6 100644
--- a/drivers/media/usb/gspca/stv06xx/stv06xx_hdcs.c
+++ b/drivers/media/usb/gspca/stv06xx/stv06xx_hdcs.c
@@ -255,7 +255,7 @@ static int hdcs_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 		if (err < 0)
 			return err;
 	}
-	PDEBUG(D_V4L2, "Writing exposure %d, rowexp %d, srowexp %d",
+	PDEBUG(D_CONF, "Writing exposure %d, rowexp %d, srowexp %d",
 	       val, rowexp, srowexp);
 	return err;
 }
@@ -280,7 +280,7 @@ static int hdcs_set_gains(struct sd *sd, u8 g)
 
 static int hdcs_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 {
-	PDEBUG(D_V4L2, "Writing gain %d", val);
+	PDEBUG(D_CONF, "Writing gain %d", val);
 	return hdcs_set_gains((struct sd *) gspca_dev,
 			       val & 0xff);
 }
@@ -467,6 +467,8 @@ static int hdcs_probe_1020(struct sd *sd)
 
 static int hdcs_start(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
+
 	PDEBUG(D_STREAM, "Starting stream");
 
 	return hdcs_set_state(sd, HDCS_STATE_RUN);
@@ -474,6 +476,8 @@ static int hdcs_start(struct sd *sd)
 
 static int hdcs_stop(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
+
 	PDEBUG(D_STREAM, "Halting stream");
 
 	return hdcs_set_state(sd, HDCS_STATE_SLEEP);
diff --git a/drivers/media/usb/gspca/stv06xx/stv06xx_pb0100.c b/drivers/media/usb/gspca/stv06xx/stv06xx_pb0100.c
index cdfc3d05ab6b..8206b7743300 100644
--- a/drivers/media/usb/gspca/stv06xx/stv06xx_pb0100.c
+++ b/drivers/media/usb/gspca/stv06xx/stv06xx_pb0100.c
@@ -190,6 +190,7 @@ static int pb0100_start(struct sd *sd)
 	int err, packet_size, max_packet_size;
 	struct usb_host_interface *alt;
 	struct usb_interface *intf;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	struct cam *cam = &sd->gspca_dev.cam;
 	u32 mode = cam->cam_mode[sd->gspca_dev.curr_mode].priv;
 
@@ -239,6 +240,7 @@ static int pb0100_start(struct sd *sd)
 
 static int pb0100_stop(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int err;
 
 	err = stv06xx_write_sensor(sd, PB_ABORTFRAME, 1);
@@ -334,7 +336,7 @@ static int pb0100_set_gain(struct gspca_dev *gspca_dev, __s32 val)
 	err = stv06xx_write_sensor(sd, PB_G1GAIN, val);
 	if (!err)
 		err = stv06xx_write_sensor(sd, PB_G2GAIN, val);
-	PDEBUG(D_V4L2, "Set green gain to %d, status: %d", val, err);
+	PDEBUG(D_CONF, "Set green gain to %d, status: %d", val, err);
 
 	if (!err)
 		err = pb0100_set_red_balance(gspca_dev, ctrls->red->val);
@@ -357,7 +359,7 @@ static int pb0100_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
 		val = 255;
 
 	err = stv06xx_write_sensor(sd, PB_RGAIN, val);
-	PDEBUG(D_V4L2, "Set red gain to %d, status: %d", val, err);
+	PDEBUG(D_CONF, "Set red gain to %d, status: %d", val, err);
 
 	return err;
 }
@@ -375,7 +377,7 @@ static int pb0100_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
 		val = 255;
 
 	err = stv06xx_write_sensor(sd, PB_BGAIN, val);
-	PDEBUG(D_V4L2, "Set blue gain to %d, status: %d", val, err);
+	PDEBUG(D_CONF, "Set blue gain to %d, status: %d", val, err);
 
 	return err;
 }
@@ -386,7 +388,7 @@ static int pb0100_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	int err;
 
 	err = stv06xx_write_sensor(sd, PB_RINTTIME, val);
-	PDEBUG(D_V4L2, "Set exposure to %d, status: %d", val, err);
+	PDEBUG(D_CONF, "Set exposure to %d, status: %d", val, err);
 
 	return err;
 }
@@ -406,7 +408,7 @@ static int pb0100_set_autogain(struct gspca_dev *gspca_dev, __s32 val)
 		val = 0;
 
 	err = stv06xx_write_sensor(sd, PB_EXPGAIN, val);
-	PDEBUG(D_V4L2, "Set autogain to %d (natural: %d), status: %d",
+	PDEBUG(D_CONF, "Set autogain to %d (natural: %d), status: %d",
 	       val, ctrls->natural->val, err);
 
 	return err;
@@ -428,7 +430,7 @@ static int pb0100_set_autogain_target(struct gspca_dev *gspca_dev, __s32 val)
 	if (!err)
 		err = stv06xx_write_sensor(sd, PB_R22, darkpixels);
 
-	PDEBUG(D_V4L2, "Set autogain target to %d, status: %d", val, err);
+	PDEBUG(D_CONF, "Set autogain target to %d, status: %d", val, err);
 
 	return err;
 }
diff --git a/drivers/media/usb/gspca/stv06xx/stv06xx_st6422.c b/drivers/media/usb/gspca/stv06xx/stv06xx_st6422.c
index 8a57990dfe0f..515a9e121653 100644
--- a/drivers/media/usb/gspca/stv06xx/stv06xx_st6422.c
+++ b/drivers/media/usb/gspca/stv06xx/stv06xx_st6422.c
@@ -279,6 +279,8 @@ static int st6422_start(struct sd *sd)
 
 static int st6422_stop(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
+
 	PDEBUG(D_STREAM, "Halting stream");
 
 	return 0;
diff --git a/drivers/media/usb/gspca/stv06xx/stv06xx_vv6410.c b/drivers/media/usb/gspca/stv06xx/stv06xx_vv6410.c
index e95fa8997d22..bf3e5c317a26 100644
--- a/drivers/media/usb/gspca/stv06xx/stv06xx_vv6410.c
+++ b/drivers/media/usb/gspca/stv06xx/stv06xx_vv6410.c
@@ -131,6 +131,7 @@ static int vv6410_init(struct sd *sd)
 static int vv6410_start(struct sd *sd)
 {
 	int err;
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	struct cam *cam = &sd->gspca_dev.cam;
 	u32 priv = cam->cam_mode[sd->gspca_dev.curr_mode].priv;
 
@@ -163,6 +164,7 @@ static int vv6410_start(struct sd *sd)
 
 static int vv6410_stop(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int err;
 
 	/* Turn off LED */
@@ -208,7 +210,7 @@ static int vv6410_set_hflip(struct gspca_dev *gspca_dev, __s32 val)
 	else
 		i2c_data &= ~VV6410_HFLIP;
 
-	PDEBUG(D_V4L2, "Set horizontal flip to %d", val);
+	PDEBUG(D_CONF, "Set horizontal flip to %d", val);
 	err = stv06xx_write_sensor(sd, VV6410_DATAFORMAT, i2c_data);
 
 	return (err < 0) ? err : 0;
@@ -229,7 +231,7 @@ static int vv6410_set_vflip(struct gspca_dev *gspca_dev, __s32 val)
 	else
 		i2c_data &= ~VV6410_VFLIP;
 
-	PDEBUG(D_V4L2, "Set vertical flip to %d", val);
+	PDEBUG(D_CONF, "Set vertical flip to %d", val);
 	err = stv06xx_write_sensor(sd, VV6410_DATAFORMAT, i2c_data);
 
 	return (err < 0) ? err : 0;
@@ -240,7 +242,7 @@ static int vv6410_set_analog_gain(struct gspca_dev *gspca_dev, __s32 val)
 	int err;
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	PDEBUG(D_V4L2, "Set analog gain to %d", val);
+	PDEBUG(D_CONF, "Set analog gain to %d", val);
 	err = stv06xx_write_sensor(sd, VV6410_ANALOGGAIN, 0xf0 | (val & 0xf));
 
 	return (err < 0) ? err : 0;
@@ -257,7 +259,7 @@ static int vv6410_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	fine = val % VV6410_CIF_LINELENGTH;
 	coarse = min(512, val / VV6410_CIF_LINELENGTH);
 
-	PDEBUG(D_V4L2, "Set coarse exposure to %d, fine expsure to %d",
+	PDEBUG(D_CONF, "Set coarse exposure to %d, fine expsure to %d",
 	       coarse, fine);
 
 	err = stv06xx_write_sensor(sd, VV6410_FINEH, fine >> 8);
diff --git a/drivers/media/usb/gspca/sunplus.c b/drivers/media/usb/gspca/sunplus.c
index 9ccfcb1c6479..af8767a9bd4c 100644
--- a/drivers/media/usb/gspca/sunplus.c
+++ b/drivers/media/usb/gspca/sunplus.c
@@ -251,12 +251,10 @@ static void reg_r(struct gspca_dev *gspca_dev,
 {
 	int ret;
 
-#ifdef GSPCA_DEBUG
 	if (len > USB_BUF_SZ) {
-		pr_err("reg_r: buffer overflow\n");
+		PERR("reg_r: buffer overflow\n");
 		return;
 	}
-#endif
 	if (gspca_dev->usb_err < 0)
 		return;
 	ret = usb_control_msg(gspca_dev->dev,
@@ -357,12 +355,14 @@ static void spca504_acknowledged_command(struct gspca_dev *gspca_dev,
 	PDEBUG(D_FRAM, "after wait 0x%04x", gspca_dev->usb_buf[0]);
 }
 
-#ifdef GSPCA_DEBUG
 static void spca504_read_info(struct gspca_dev *gspca_dev)
 {
 	int i;
 	u8 info[6];
 
+	if (gspca_debug < D_STREAM)
+		return;
+
 	for (i = 0; i < 6; i++) {
 		reg_r(gspca_dev, 0, i, 1);
 		info[i] = gspca_dev->usb_buf[0];
@@ -373,7 +373,6 @@ static void spca504_read_info(struct gspca_dev *gspca_dev)
 		info[0], info[1], info[2],
 		info[3], info[4], info[5]);
 }
-#endif
 
 static void spca504A_acknowledged_command(struct gspca_dev *gspca_dev,
 			u8 req,
@@ -432,11 +431,13 @@ static void spca504B_WaitCmdStatus(struct gspca_dev *gspca_dev)
 	}
 }
 
-#ifdef GSPCA_DEBUG
 static void spca50x_GetFirmware(struct gspca_dev *gspca_dev)
 {
 	u8 *data;
 
+	if (gspca_debug < D_STREAM)
+		return;
+
 	data = gspca_dev->usb_buf;
 	reg_r(gspca_dev, 0x20, 0, 5);
 	PDEBUG(D_STREAM, "FirmWare: %d %d %d %d %d",
@@ -444,7 +445,6 @@ static void spca50x_GetFirmware(struct gspca_dev *gspca_dev)
 	reg_r(gspca_dev, 0x23, 0, 64);
 	reg_r(gspca_dev, 0x23, 1, 64);
 }
-#endif
 
 static void spca504B_SetSizeType(struct gspca_dev *gspca_dev)
 {
@@ -457,9 +457,8 @@ static void spca504B_SetSizeType(struct gspca_dev *gspca_dev)
 		reg_w_riv(gspca_dev, 0x31, 0, 0);
 		spca504B_WaitCmdStatus(gspca_dev);
 		spca504B_PollingDataReady(gspca_dev);
-#ifdef GSPCA_DEBUG
 		spca50x_GetFirmware(gspca_dev);
-#endif
+
 		reg_w_1(gspca_dev, 0x24, 0, 8, 2);		/* type */
 		reg_r(gspca_dev, 0x24, 8, 1);
 
@@ -645,14 +644,10 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		/* fall thru */
 	case BRIDGE_SPCA533:
 		spca504B_PollingDataReady(gspca_dev);
-#ifdef GSPCA_DEBUG
 		spca50x_GetFirmware(gspca_dev);
-#endif
 		break;
 	case BRIDGE_SPCA536:
-#ifdef GSPCA_DEBUG
 		spca50x_GetFirmware(gspca_dev);
-#endif
 		reg_r(gspca_dev, 0x00, 0x5002, 1);
 		reg_w_1(gspca_dev, 0x24, 0, 0, 0);
 		reg_r(gspca_dev, 0x24, 0, 1);
@@ -678,9 +673,7 @@ static int sd_init(struct gspca_dev *gspca_dev)
 /*	case BRIDGE_SPCA504: */
 		PDEBUG(D_STREAM, "Opening SPCA504");
 		if (sd->subtype == AiptekMiniPenCam13) {
-#ifdef GSPCA_DEBUG
 			spca504_read_info(gspca_dev);
-#endif
 
 			/* Set AE AWB Banding Type 3-> 50Hz 2-> 60Hz */
 			spca504A_acknowledged_command(gspca_dev, 0x24,
@@ -752,9 +745,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		break;
 	case BRIDGE_SPCA504:
 		if (sd->subtype == AiptekMiniPenCam13) {
-#ifdef GSPCA_DEBUG
 			spca504_read_info(gspca_dev);
-#endif
 
 			/* Set AE AWB Banding Type 3-> 50Hz 2-> 60Hz */
 			spca504A_acknowledged_command(gspca_dev, 0x24,
@@ -766,9 +757,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 							0, 0, 0x9d, 1);
 		} else {
 			spca504_acknowledged_command(gspca_dev, 0x24, 8, 3);
-#ifdef GSPCA_DEBUG
 			spca504_read_info(gspca_dev);
-#endif
 			spca504_acknowledged_command(gspca_dev, 0x24, 8, 3);
 			spca504_acknowledged_command(gspca_dev, 0x24, 0, 0);
 		}
diff --git a/drivers/media/usb/gspca/vc032x.c b/drivers/media/usb/gspca/vc032x.c
index e50079503d96..c00ac57de510 100644
--- a/drivers/media/usb/gspca/vc032x.c
+++ b/drivers/media/usb/gspca/vc032x.c
@@ -2927,7 +2927,6 @@ static void reg_r(struct gspca_dev *gspca_dev,
 		  u16 len)
 {
 	reg_r_i(gspca_dev, req, index, len);
-#ifdef GSPCA_DEBUG
 	if (gspca_dev->usb_err < 0)
 		return;
 	if (len == 1)
@@ -2936,7 +2935,6 @@ static void reg_r(struct gspca_dev *gspca_dev,
 	else
 		PDEBUG(D_USBI, "GET %02x 0001 %04x %*ph",
 				req, index, 3, gspca_dev->usb_buf);
-#endif
 }
 
 static void reg_w_i(struct gspca_dev *gspca_dev,
@@ -2964,11 +2962,9 @@ static void reg_w(struct gspca_dev *gspca_dev,
 			    u16 value,
 			    u16 index)
 {
-#ifdef GSPCA_DEBUG
 	if (gspca_dev->usb_err < 0)
 		return;
 	PDEBUG(D_USBO, "SET %02x %04x %04x", req, value, index);
-#endif
 	reg_w_i(gspca_dev, req, value, index);
 }
 
@@ -3044,8 +3040,7 @@ static int vc032x_probe_sensor(struct gspca_dev *gspca_dev)
 		if (value == 0 && ptsensor_info->IdAdd == 0x82)
 			value = read_sensor_register(gspca_dev, 0x83);
 		if (value != 0) {
-			PDEBUG(D_ERR|D_PROBE, "Sensor ID %04x (%d)",
-				value, i);
+			PDEBUG(D_PROBE, "Sensor ID %04x (%d)", value, i);
 			if (value == ptsensor_info->VpId)
 				return ptsensor_info->sensorId;
 
@@ -3069,14 +3064,12 @@ static void i2c_write(struct gspca_dev *gspca_dev,
 {
 	int retry;
 
-#ifdef GSPCA_DEBUG
 	if (gspca_dev->usb_err < 0)
 		return;
 	if (size == 1)
 		PDEBUG(D_USBO, "i2c_w %02x %02x", reg, *val);
 	else
 		PDEBUG(D_USBO, "i2c_w %02x %02x%02x", reg, *val, val[1]);
-#endif
 	reg_r_i(gspca_dev, 0xa1, 0xb33f, 1);
 /*fixme:should check if (!(gspca_dev->usb_buf[0] & 0x02)) error*/
 	reg_w_i(gspca_dev, 0xa0, size, 0xb334);
diff --git a/drivers/media/usb/gspca/w996Xcf.c b/drivers/media/usb/gspca/w996Xcf.c
index 9e3a909e0a00..2165da0c7ce1 100644
--- a/drivers/media/usb/gspca/w996Xcf.c
+++ b/drivers/media/usb/gspca/w996Xcf.c
@@ -232,6 +232,7 @@ static void w9968cf_smbus_write_nack(struct sd *sd)
 
 static void w9968cf_smbus_read_ack(struct sd *sd)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int sda;
 
 	/* Ensure SDA is high before raising clock to avoid a spurious stop */
@@ -248,6 +249,7 @@ static void w9968cf_smbus_read_ack(struct sd *sd)
 /* SMBus protocol: S Addr Wr [A] Subaddr [A] Value [A] P */
 static void w9968cf_i2c_w(struct sd *sd, u8 reg, u8 value)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	u16* data = (u16 *)sd->gspca_dev.usb_buf;
 
 	data[0] = 0x082f | ((sd->sensor_addr & 0x80) ? 0x1500 : 0x0);
@@ -297,6 +299,7 @@ static void w9968cf_i2c_w(struct sd *sd, u8 reg, u8 value)
 /* SMBus protocol: S Addr Wr [A] Subaddr [A] P S Addr+1 Rd [A] [Value] NA P */
 static int w9968cf_i2c_r(struct sd *sd, u8 reg)
 {
+	struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
 	int ret = 0;
 	u8 value;
 
@@ -326,7 +329,7 @@ static int w9968cf_i2c_r(struct sd *sd, u8 reg)
 		ret = value;
 		PDEBUG(D_USBI, "i2c [0x%02X] -> 0x%02X", reg, value);
 	} else
-		PDEBUG(D_ERR, "i2c read [0x%02x] failed", reg);
+		PERR("i2c read [0x%02x] failed", reg);
 
 	return ret;
 }
diff --git a/drivers/media/usb/gspca/zc3xx.c b/drivers/media/usb/gspca/zc3xx.c
index a8dc421f9f1f..cbfc2f921427 100644
--- a/drivers/media/usb/gspca/zc3xx.c
+++ b/drivers/media/usb/gspca/zc3xx.c
@@ -6259,12 +6259,11 @@ static int vga_3wr_probe(struct gspca_dev *gspca_dev)
 	retword |= i2c_read(gspca_dev, 0x01);		/* ID 1 */
 	PDEBUG(D_PROBE, "probe 3wr vga 2 0x%04x", retword);
 	if (retword == 0x2030) {
-#ifdef GSPCA_DEBUG
 		u8 retbyte;
 
 		retbyte = i2c_read(gspca_dev, 0x02);	/* revision number */
 		PDEBUG(D_PROBE, "sensor PO2030 rev 0x%02x", retbyte);
-#endif
+
 		send_unknown(gspca_dev, SENSOR_PO2030);
 		return retword;
 	}
diff --git a/drivers/media/usb/hdpvr/hdpvr-core.c b/drivers/media/usb/hdpvr/hdpvr-core.c
index 5c6193536399..8247c19d6260 100644
--- a/drivers/media/usb/hdpvr/hdpvr-core.c
+++ b/drivers/media/usb/hdpvr/hdpvr-core.c
@@ -174,6 +174,7 @@ static int device_authorization(struct hdpvr_device *dev)
 	case HDPVR_FIRMWARE_VERSION_AC3:
 	case HDPVR_FIRMWARE_VERSION_0X12:
 	case HDPVR_FIRMWARE_VERSION_0X15:
+	case HDPVR_FIRMWARE_VERSION_0X1E:
 		dev->flags |= HDPVR_FLAG_AC3_CAP;
 		break;
 	default:
@@ -196,6 +197,7 @@ static int device_authorization(struct hdpvr_device *dev)
 	hex_dump_to_buffer(response, 8, 16, 1, print_buf, 5*buf_size+1, 0);
 	v4l2_dbg(MSG_INFO, hdpvr_debug, &dev->v4l2_dev, " response: %s\n",
 		 print_buf);
+	kfree(print_buf);
 #endif
 
 	msleep(100);
@@ -385,12 +387,6 @@ static int hdpvr_probe(struct usb_interface *interface,
 	}
 	mutex_unlock(&dev->io_mutex);
 
-	if (hdpvr_register_videodev(dev, &interface->dev,
-				    video_nr[atomic_inc_return(&dev_nr)])) {
-		v4l2_err(&dev->v4l2_dev, "registering videodev failed\n");
-		goto error;
-	}
-
 #if IS_ENABLED(CONFIG_I2C)
 	retval = hdpvr_register_i2c_adapter(dev);
 	if (retval < 0) {
@@ -413,6 +409,13 @@ static int hdpvr_probe(struct usb_interface *interface,
 	}
 #endif
 
+	retval = hdpvr_register_videodev(dev, &interface->dev,
+				    video_nr[atomic_inc_return(&dev_nr)]);
+	if (retval < 0) {
+		v4l2_err(&dev->v4l2_dev, "registering videodev failed\n");
+		goto error;
+	}
+
 	/* let the user know what node this device is now attached to */
 	v4l2_info(&dev->v4l2_dev, "device now attached to %s\n",
 		  video_device_node_name(dev->video_dev));
diff --git a/drivers/media/usb/hdpvr/hdpvr-video.c b/drivers/media/usb/hdpvr/hdpvr-video.c
index da6b77912222..774ba0e820be 100644
--- a/drivers/media/usb/hdpvr/hdpvr-video.c
+++ b/drivers/media/usb/hdpvr/hdpvr-video.c
@@ -10,6 +10,7 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/kconfig.h>
 #include <linux/errno.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -20,9 +21,11 @@
 #include <linux/workqueue.h>
 
 #include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
 #include <media/v4l2-dev.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
 #include "hdpvr.h"
 
 #define BULK_URB_TIMEOUT   90 /* 0.09 seconds */
@@ -34,8 +37,23 @@
 			 list_size(&dev->free_buff_list),		\
 			 list_size(&dev->rec_buff_list)); }
 
+static const struct v4l2_dv_timings hdpvr_dv_timings[] = {
+	V4L2_DV_BT_CEA_720X480I59_94,
+	V4L2_DV_BT_CEA_720X576I50,
+	V4L2_DV_BT_CEA_720X480P59_94,
+	V4L2_DV_BT_CEA_720X576P50,
+	V4L2_DV_BT_CEA_1280X720P50,
+	V4L2_DV_BT_CEA_1280X720P60,
+	V4L2_DV_BT_CEA_1920X1080I50,
+	V4L2_DV_BT_CEA_1920X1080I60,
+};
+
+/* Use 480i59 as the default timings */
+#define HDPVR_DEF_DV_TIMINGS_IDX (0)
+
 struct hdpvr_fh {
-	struct hdpvr_device	*dev;
+	struct v4l2_fh fh;
+	bool legacy_mode;
 };
 
 static uint list_size(struct list_head *list)
@@ -359,53 +377,29 @@ static int hdpvr_stop_streaming(struct hdpvr_device *dev)
 
 static int hdpvr_open(struct file *file)
 {
-	struct hdpvr_device *dev;
-	struct hdpvr_fh *fh;
-	int retval = -ENOMEM;
-
-	dev = (struct hdpvr_device *)video_get_drvdata(video_devdata(file));
-	if (!dev) {
-		pr_err("open failing with with ENODEV\n");
-		retval = -ENODEV;
-		goto err;
-	}
-
-	fh = kzalloc(sizeof(struct hdpvr_fh), GFP_KERNEL);
-	if (!fh) {
-		v4l2_err(&dev->v4l2_dev, "Out of memory\n");
-		goto err;
-	}
-	/* lock the device to allow correctly handling errors
-	 * in resumption */
-	mutex_lock(&dev->io_mutex);
-	dev->open_count++;
-	mutex_unlock(&dev->io_mutex);
+	struct hdpvr_fh *fh = kzalloc(sizeof(*fh), GFP_KERNEL);
 
-	fh->dev = dev;
-
-	/* save our object in the file's private structure */
+	if (fh == NULL)
+		return -ENOMEM;
+	fh->legacy_mode = true;
+	v4l2_fh_init(&fh->fh, video_devdata(file));
+	v4l2_fh_add(&fh->fh);
 	file->private_data = fh;
-
-	retval = 0;
-err:
-	return retval;
+	return 0;
 }
 
 static int hdpvr_release(struct file *file)
 {
-	struct hdpvr_fh		*fh  = file->private_data;
-	struct hdpvr_device	*dev = fh->dev;
-
-	if (!dev)
-		return -ENODEV;
+	struct hdpvr_device *dev = video_drvdata(file);
 
 	mutex_lock(&dev->io_mutex);
-	if (!(--dev->open_count) && dev->status == STATUS_STREAMING)
+	if (file->private_data == dev->owner) {
 		hdpvr_stop_streaming(dev);
-
+		dev->owner = NULL;
+	}
 	mutex_unlock(&dev->io_mutex);
 
-	return 0;
+	return v4l2_fh_release(file);
 }
 
 /*
@@ -415,8 +409,7 @@ static int hdpvr_release(struct file *file)
 static ssize_t hdpvr_read(struct file *file, char __user *buffer, size_t count,
 			  loff_t *pos)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
+	struct hdpvr_device *dev = video_drvdata(file);
 	struct hdpvr_buffer *buf = NULL;
 	struct urb *urb;
 	unsigned int ret = 0;
@@ -425,9 +418,6 @@ static ssize_t hdpvr_read(struct file *file, char __user *buffer, size_t count,
 	if (*pos)
 		return -ESPIPE;
 
-	if (!dev)
-		return -ENODEV;
-
 	mutex_lock(&dev->io_mutex);
 	if (dev->status == STATUS_IDLE) {
 		if (hdpvr_start_streaming(dev)) {
@@ -439,6 +429,7 @@ static ssize_t hdpvr_read(struct file *file, char __user *buffer, size_t count,
 			mutex_unlock(&dev->io_mutex);
 			goto err;
 		}
+		dev->owner = file->private_data;
 		print_buffer_status();
 	}
 	mutex_unlock(&dev->io_mutex);
@@ -516,23 +507,23 @@ err:
 
 static unsigned int hdpvr_poll(struct file *filp, poll_table *wait)
 {
+	unsigned long req_events = poll_requested_events(wait);
 	struct hdpvr_buffer *buf = NULL;
-	struct hdpvr_fh *fh = filp->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	unsigned int mask = 0;
+	struct hdpvr_device *dev = video_drvdata(filp);
+	unsigned int mask = v4l2_ctrl_poll(filp, wait);
 
-	mutex_lock(&dev->io_mutex);
+	if (!(req_events & (POLLIN | POLLRDNORM)))
+		return mask;
 
-	if (!video_is_registered(dev->video_dev)) {
-		mutex_unlock(&dev->io_mutex);
-		return -EIO;
-	}
+	mutex_lock(&dev->io_mutex);
 
 	if (dev->status == STATUS_IDLE) {
 		if (hdpvr_start_streaming(dev)) {
 			v4l2_dbg(MSG_BUFFER, hdpvr_debug, &dev->v4l2_dev,
 				 "start_streaming failed\n");
 			dev->status = STATUS_IDLE;
+		} else {
+			dev->owner = filp->private_data;
 		}
 
 		print_buffer_status();
@@ -574,36 +565,188 @@ static int vidioc_querycap(struct file *file, void  *priv,
 	strcpy(cap->driver, "hdpvr");
 	strcpy(cap->card, "Hauppauge HD PVR");
 	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
-	cap->capabilities =     V4L2_CAP_VIDEO_CAPTURE |
-				V4L2_CAP_AUDIO         |
-				V4L2_CAP_READWRITE;
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_AUDIO |
+			    V4L2_CAP_READWRITE;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *private_data,
-			v4l2_std_id *std)
+static int vidioc_s_std(struct file *file, void *_fh,
+			v4l2_std_id std)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
 	u8 std_type = 1;
 
-	if (*std & (V4L2_STD_NTSC | V4L2_STD_PAL_60))
+	if (!fh->legacy_mode && dev->options.video_input == HDPVR_COMPONENT)
+		return -ENODATA;
+	if (dev->status != STATUS_IDLE)
+		return -EBUSY;
+	if (std & V4L2_STD_525_60)
 		std_type = 0;
+	dev->cur_std = std;
+	dev->width = 720;
+	dev->height = std_type ? 576 : 480;
 
 	return hdpvr_config_call(dev, CTRL_VIDEO_STD_TYPE, std_type);
 }
 
+static int vidioc_g_std(struct file *file, void *_fh,
+			v4l2_std_id *std)
+{
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
+
+	if (!fh->legacy_mode && dev->options.video_input == HDPVR_COMPONENT)
+		return -ENODATA;
+	*std = dev->cur_std;
+	return 0;
+}
+
+static int vidioc_querystd(struct file *file, void *_fh, v4l2_std_id *a)
+{
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_video_info *vid_info;
+	struct hdpvr_fh *fh = _fh;
+
+	*a = V4L2_STD_ALL;
+	if (dev->options.video_input == HDPVR_COMPONENT)
+		return fh->legacy_mode ? 0 : -ENODATA;
+	vid_info = get_video_info(dev);
+	if (vid_info == NULL)
+		return 0;
+	if (vid_info->width == 720 &&
+	    (vid_info->height == 480 || vid_info->height == 576)) {
+		*a = (vid_info->height == 480) ?
+			V4L2_STD_525_60 : V4L2_STD_625_50;
+	}
+	kfree(vid_info);
+	return 0;
+}
+
+static int vidioc_s_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
+	int i;
+
+	fh->legacy_mode = false;
+	if (dev->options.video_input)
+		return -ENODATA;
+	if (dev->status != STATUS_IDLE)
+		return -EBUSY;
+	for (i = 0; i < ARRAY_SIZE(hdpvr_dv_timings); i++)
+		if (v4l_match_dv_timings(timings, hdpvr_dv_timings + i, 0))
+			break;
+	if (i == ARRAY_SIZE(hdpvr_dv_timings))
+		return -EINVAL;
+	dev->cur_dv_timings = hdpvr_dv_timings[i];
+	dev->width = hdpvr_dv_timings[i].bt.width;
+	dev->height = hdpvr_dv_timings[i].bt.height;
+	return 0;
+}
+
+static int vidioc_g_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
+
+	fh->legacy_mode = false;
+	if (dev->options.video_input)
+		return -ENODATA;
+	*timings = dev->cur_dv_timings;
+	return 0;
+}
+
+static int vidioc_query_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
+	struct hdpvr_video_info *vid_info;
+	bool interlaced;
+	int ret = 0;
+	int i;
+
+	fh->legacy_mode = false;
+	if (dev->options.video_input)
+		return -ENODATA;
+	vid_info = get_video_info(dev);
+	if (vid_info == NULL)
+		return -ENOLCK;
+	interlaced = vid_info->fps <= 30;
+	for (i = 0; i < ARRAY_SIZE(hdpvr_dv_timings); i++) {
+		const struct v4l2_bt_timings *bt = &hdpvr_dv_timings[i].bt;
+		unsigned hsize;
+		unsigned vsize;
+		unsigned fps;
+
+		hsize = bt->hfrontporch + bt->hsync + bt->hbackporch + bt->width;
+		vsize = bt->vfrontporch + bt->vsync + bt->vbackporch +
+			bt->il_vfrontporch + bt->il_vsync + bt->il_vbackporch +
+			bt->height;
+		fps = (unsigned)bt->pixelclock / (hsize * vsize);
+		if (bt->width != vid_info->width ||
+		    bt->height != vid_info->height ||
+		    bt->interlaced != interlaced ||
+		    (fps != vid_info->fps && fps + 1 != vid_info->fps))
+			continue;
+		*timings = hdpvr_dv_timings[i];
+		break;
+	}
+	if (i == ARRAY_SIZE(hdpvr_dv_timings))
+		ret = -ERANGE;
+	kfree(vid_info);
+	return ret;
+}
+
+static int vidioc_enum_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_enum_dv_timings *timings)
+{
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
+
+	fh->legacy_mode = false;
+	memset(timings->reserved, 0, sizeof(timings->reserved));
+	if (dev->options.video_input)
+		return -ENODATA;
+	if (timings->index >= ARRAY_SIZE(hdpvr_dv_timings))
+		return -EINVAL;
+	timings->timings = hdpvr_dv_timings[timings->index];
+	return 0;
+}
+
+static int vidioc_dv_timings_cap(struct file *file, void *_fh,
+				    struct v4l2_dv_timings_cap *cap)
+{
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
+
+	fh->legacy_mode = false;
+	if (dev->options.video_input)
+		return -ENODATA;
+	cap->type = V4L2_DV_BT_656_1120;
+	cap->bt.min_width = 720;
+	cap->bt.max_width = 1920;
+	cap->bt.min_height = 480;
+	cap->bt.max_height = 1080;
+	cap->bt.min_pixelclock = 27000000;
+	cap->bt.max_pixelclock = 74250000;
+	cap->bt.standards = V4L2_DV_BT_STD_CEA861;
+	cap->bt.capabilities = V4L2_DV_BT_CAP_INTERLACED | V4L2_DV_BT_CAP_PROGRESSIVE;
+	return 0;
+}
+
 static const char *iname[] = {
 	[HDPVR_COMPONENT] = "Component",
 	[HDPVR_SVIDEO]    = "S-Video",
 	[HDPVR_COMPOSITE] = "Composite",
 };
 
-static int vidioc_enum_input(struct file *file, void *priv,
-				struct v4l2_input *i)
+static int vidioc_enum_input(struct file *file, void *_fh, struct v4l2_input *i)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
 	unsigned int n;
 
 	n = i->index;
@@ -617,27 +760,42 @@ static int vidioc_enum_input(struct file *file, void *priv,
 
 	i->audioset = 1<<HDPVR_RCA_FRONT | 1<<HDPVR_RCA_BACK | 1<<HDPVR_SPDIF;
 
-	i->std = dev->video_dev->tvnorms;
+	i->capabilities = n ? V4L2_IN_CAP_STD : V4L2_IN_CAP_DV_TIMINGS;
+	i->std = n ? V4L2_STD_ALL : 0;
 
 	return 0;
 }
 
-static int vidioc_s_input(struct file *file, void *private_data,
+static int vidioc_s_input(struct file *file, void *_fh,
 			  unsigned int index)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
+	struct hdpvr_device *dev = video_drvdata(file);
 	int retval;
 
 	if (index >= HDPVR_VIDEO_INPUTS)
 		return -EINVAL;
 
 	if (dev->status != STATUS_IDLE)
-		return -EAGAIN;
+		return -EBUSY;
 
 	retval = hdpvr_config_call(dev, CTRL_VIDEO_INPUT_VALUE, index+1);
-	if (!retval)
+	if (!retval) {
 		dev->options.video_input = index;
+		/*
+		 * Unfortunately gstreamer calls ENUMSTD and bails out if it
+		 * won't find any formats, even though component input is
+		 * selected. This means that we have to leave tvnorms at
+		 * V4L2_STD_ALL. We cannot use the 'legacy' trick since
+		 * tvnorms is set at the device node level and not at the
+		 * filehandle level.
+		 *
+		 * Comment this out for now, but if the legacy mode can be
+		 * removed in the future, then this code should be enabled
+		 * again.
+		dev->video_dev->tvnorms =
+			(index != HDPVR_COMPONENT) ? V4L2_STD_ALL : 0;
+		 */
+	}
 
 	return retval;
 }
@@ -645,8 +803,7 @@ static int vidioc_s_input(struct file *file, void *private_data,
 static int vidioc_g_input(struct file *file, void *private_data,
 			  unsigned int *index)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
+	struct hdpvr_device *dev = video_drvdata(file);
 
 	*index = dev->options.video_input;
 	return 0;
@@ -679,15 +836,14 @@ static int vidioc_enumaudio(struct file *file, void *priv,
 static int vidioc_s_audio(struct file *file, void *private_data,
 			  const struct v4l2_audio *audio)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
+	struct hdpvr_device *dev = video_drvdata(file);
 	int retval;
 
 	if (audio->index >= HDPVR_AUDIO_INPUTS)
 		return -EINVAL;
 
 	if (dev->status != STATUS_IDLE)
-		return -EAGAIN;
+		return -EBUSY;
 
 	retval = hdpvr_set_audio(dev, audio->index+1, dev->options.audio_codec);
 	if (!retval)
@@ -699,8 +855,7 @@ static int vidioc_s_audio(struct file *file, void *private_data,
 static int vidioc_g_audio(struct file *file, void *private_data,
 			  struct v4l2_audio *audio)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
+	struct hdpvr_device *dev = video_drvdata(file);
 
 	audio->index = dev->options.audio_input;
 	audio->capability = V4L2_AUDCAP_STEREO;
@@ -709,335 +864,69 @@ static int vidioc_g_audio(struct file *file, void *private_data,
 	return 0;
 }
 
-static const s32 supported_v4l2_ctrls[] = {
-	V4L2_CID_BRIGHTNESS,
-	V4L2_CID_CONTRAST,
-	V4L2_CID_SATURATION,
-	V4L2_CID_HUE,
-	V4L2_CID_SHARPNESS,
-	V4L2_CID_MPEG_AUDIO_ENCODING,
-	V4L2_CID_MPEG_VIDEO_ENCODING,
-	V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
-	V4L2_CID_MPEG_VIDEO_BITRATE,
-	V4L2_CID_MPEG_VIDEO_BITRATE_PEAK,
-};
-
-static int fill_queryctrl(struct hdpvr_options *opt, struct v4l2_queryctrl *qc,
-			  int ac3, int fw_ver)
-{
-	int err;
-
-	if (fw_ver > 0x15) {
-		switch (qc->id) {
-		case V4L2_CID_BRIGHTNESS:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x80);
-		case V4L2_CID_CONTRAST:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x40);
-		case V4L2_CID_SATURATION:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x40);
-		case V4L2_CID_HUE:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0x1e, 1, 0xf);
-		case V4L2_CID_SHARPNESS:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x80);
-		}
-	} else {
-		switch (qc->id) {
-		case V4L2_CID_BRIGHTNESS:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x86);
-		case V4L2_CID_CONTRAST:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x80);
-		case V4L2_CID_SATURATION:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x80);
-		case V4L2_CID_HUE:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x80);
-		case V4L2_CID_SHARPNESS:
-			return v4l2_ctrl_query_fill(qc, 0x0, 0xff, 1, 0x80);
-		}
-	}
-
-	switch (qc->id) {
-	case V4L2_CID_MPEG_AUDIO_ENCODING:
-		return v4l2_ctrl_query_fill(
-			qc, V4L2_MPEG_AUDIO_ENCODING_AAC,
-			ac3 ? V4L2_MPEG_AUDIO_ENCODING_AC3
-			: V4L2_MPEG_AUDIO_ENCODING_AAC,
-			1, V4L2_MPEG_AUDIO_ENCODING_AAC);
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		return v4l2_ctrl_query_fill(
-			qc, V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC,
-			V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC, 1,
-			V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC);
-
-/* 	case V4L2_CID_MPEG_VIDEO_? maybe keyframe interval: */
-/* 		return v4l2_ctrl_query_fill(qc, 0, 128, 128, 0); */
-	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
-		return v4l2_ctrl_query_fill(
-			qc, V4L2_MPEG_VIDEO_BITRATE_MODE_VBR,
-			V4L2_MPEG_VIDEO_BITRATE_MODE_CBR, 1,
-			V4L2_MPEG_VIDEO_BITRATE_MODE_CBR);
-
-	case V4L2_CID_MPEG_VIDEO_BITRATE:
-		return v4l2_ctrl_query_fill(qc, 1000000, 13500000, 100000,
-					    6500000);
-	case V4L2_CID_MPEG_VIDEO_BITRATE_PEAK:
-		err = v4l2_ctrl_query_fill(qc, 1100000, 20200000, 100000,
-					   9000000);
-		if (!err && opt->bitrate_mode == HDPVR_CONSTANT)
-			qc->flags |= V4L2_CTRL_FLAG_INACTIVE;
-		return err;
-	default:
-		return -EINVAL;
-	}
-}
-
-static int vidioc_queryctrl(struct file *file, void *private_data,
-			    struct v4l2_queryctrl *qc)
-{
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	int i, next;
-	u32 id = qc->id;
-
-	memset(qc, 0, sizeof(*qc));
-
-	next = !!(id &  V4L2_CTRL_FLAG_NEXT_CTRL);
-	qc->id = id & ~V4L2_CTRL_FLAG_NEXT_CTRL;
-
-	for (i = 0; i < ARRAY_SIZE(supported_v4l2_ctrls); i++) {
-		if (next) {
-			if (qc->id < supported_v4l2_ctrls[i])
-				qc->id = supported_v4l2_ctrls[i];
-			else
-				continue;
-		}
-
-		if (qc->id == supported_v4l2_ctrls[i])
-			return fill_queryctrl(&dev->options, qc,
-					      dev->flags & HDPVR_FLAG_AC3_CAP,
-					      dev->fw_ver);
-
-		if (qc->id < supported_v4l2_ctrls[i])
-			break;
-	}
-
-	return -EINVAL;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *private_data,
-			 struct v4l2_control *ctrl)
+static int hdpvr_try_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
+	struct hdpvr_device *dev =
+		container_of(ctrl->handler, struct hdpvr_device, hdl);
 
 	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = dev->options.brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = dev->options.contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = dev->options.saturation;
-		break;
-	case V4L2_CID_HUE:
-		ctrl->value = dev->options.hue;
-		break;
-	case V4L2_CID_SHARPNESS:
-		ctrl->value = dev->options.sharpness;
+	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
+		if (ctrl->val == V4L2_MPEG_VIDEO_BITRATE_MODE_VBR &&
+		    dev->video_bitrate->val >= dev->video_bitrate_peak->val)
+			dev->video_bitrate_peak->val =
+					dev->video_bitrate->val + 100000;
 		break;
-	default:
-		return -EINVAL;
 	}
 	return 0;
 }
 
-static int vidioc_s_ctrl(struct file *file, void *private_data,
-			 struct v4l2_control *ctrl)
+static int hdpvr_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	int retval;
+	struct hdpvr_device *dev =
+		container_of(ctrl->handler, struct hdpvr_device, hdl);
+	struct hdpvr_options *opt = &dev->options;
+	int ret = -EINVAL;
 
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		retval = hdpvr_config_call(dev, CTRL_BRIGHTNESS, ctrl->value);
-		if (!retval)
-			dev->options.brightness = ctrl->value;
-		break;
+		ret = hdpvr_config_call(dev, CTRL_BRIGHTNESS, ctrl->val);
+		if (ret)
+			break;
+		dev->options.brightness = ctrl->val;
+		return 0;
 	case V4L2_CID_CONTRAST:
-		retval = hdpvr_config_call(dev, CTRL_CONTRAST, ctrl->value);
-		if (!retval)
-			dev->options.contrast = ctrl->value;
-		break;
+		ret = hdpvr_config_call(dev, CTRL_CONTRAST, ctrl->val);
+		if (ret)
+			break;
+		dev->options.contrast = ctrl->val;
+		return 0;
 	case V4L2_CID_SATURATION:
-		retval = hdpvr_config_call(dev, CTRL_SATURATION, ctrl->value);
-		if (!retval)
-			dev->options.saturation = ctrl->value;
-		break;
+		ret = hdpvr_config_call(dev, CTRL_SATURATION, ctrl->val);
+		if (ret)
+			break;
+		dev->options.saturation = ctrl->val;
+		return 0;
 	case V4L2_CID_HUE:
-		retval = hdpvr_config_call(dev, CTRL_HUE, ctrl->value);
-		if (!retval)
-			dev->options.hue = ctrl->value;
-		break;
+		ret = hdpvr_config_call(dev, CTRL_HUE, ctrl->val);
+		if (ret)
+			break;
+		dev->options.hue = ctrl->val;
+		return 0;
 	case V4L2_CID_SHARPNESS:
-		retval = hdpvr_config_call(dev, CTRL_SHARPNESS, ctrl->value);
-		if (!retval)
-			dev->options.sharpness = ctrl->value;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return retval;
-}
-
-
-static int hdpvr_get_ctrl(struct hdpvr_options *opt,
-			  struct v4l2_ext_control *ctrl)
-{
-	switch (ctrl->id) {
-	case V4L2_CID_MPEG_AUDIO_ENCODING:
-		ctrl->value = opt->audio_codec;
-		break;
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC;
-		break;
-/* 	case V4L2_CID_MPEG_VIDEO_B_FRAMES: */
-/* 		ctrl->value = (opt->gop_mode & 0x2) ? 0 : 128; */
-/* 		break; */
-	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
-		ctrl->value = opt->bitrate_mode == HDPVR_CONSTANT
-			? V4L2_MPEG_VIDEO_BITRATE_MODE_CBR
-			: V4L2_MPEG_VIDEO_BITRATE_MODE_VBR;
-		break;
-	case V4L2_CID_MPEG_VIDEO_BITRATE:
-		ctrl->value = opt->bitrate * 100000;
-		break;
-	case V4L2_CID_MPEG_VIDEO_BITRATE_PEAK:
-		ctrl->value = opt->peak_bitrate * 100000;
-		break;
-	case V4L2_CID_MPEG_STREAM_TYPE:
-		ctrl->value = V4L2_MPEG_STREAM_TYPE_MPEG2_TS;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int vidioc_g_ext_ctrls(struct file *file, void *priv,
-			      struct v4l2_ext_controls *ctrls)
-{
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	int i, err = 0;
-
-	if (ctrls->ctrl_class == V4L2_CTRL_CLASS_MPEG) {
-		for (i = 0; i < ctrls->count; i++) {
-			struct v4l2_ext_control *ctrl = ctrls->controls + i;
-
-			err = hdpvr_get_ctrl(&dev->options, ctrl);
-			if (err) {
-				ctrls->error_idx = i;
-				break;
-			}
-		}
-		return err;
-
-	}
-
-	return -EINVAL;
-}
-
-
-static int hdpvr_try_ctrl(struct v4l2_ext_control *ctrl, int ac3)
-{
-	int ret = -EINVAL;
-
-	switch (ctrl->id) {
-	case V4L2_CID_MPEG_AUDIO_ENCODING:
-		if (ctrl->value == V4L2_MPEG_AUDIO_ENCODING_AAC ||
-		    (ac3 && ctrl->value == V4L2_MPEG_AUDIO_ENCODING_AC3))
-			ret = 0;
-		break;
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		if (ctrl->value == V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC)
-			ret = 0;
-		break;
-/* 	case V4L2_CID_MPEG_VIDEO_B_FRAMES: */
-/* 		if (ctrl->value == 0 || ctrl->value == 128) */
-/* 			ret = 0; */
-/* 		break; */
-	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
-		if (ctrl->value == V4L2_MPEG_VIDEO_BITRATE_MODE_CBR ||
-		    ctrl->value == V4L2_MPEG_VIDEO_BITRATE_MODE_VBR)
-			ret = 0;
-		break;
-	case V4L2_CID_MPEG_VIDEO_BITRATE:
-	{
-		uint bitrate = ctrl->value / 100000;
-		if (bitrate >= 10 && bitrate <= 135)
-			ret = 0;
-		break;
-	}
-	case V4L2_CID_MPEG_VIDEO_BITRATE_PEAK:
-	{
-		uint peak_bitrate = ctrl->value / 100000;
-		if (peak_bitrate >= 10 && peak_bitrate <= 202)
-			ret = 0;
-		break;
-	}
-	case V4L2_CID_MPEG_STREAM_TYPE:
-		if (ctrl->value == V4L2_MPEG_STREAM_TYPE_MPEG2_TS)
-			ret = 0;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return ret;
-}
-
-static int vidioc_try_ext_ctrls(struct file *file, void *priv,
-				struct v4l2_ext_controls *ctrls)
-{
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	int i, err = 0;
-
-	if (ctrls->ctrl_class == V4L2_CTRL_CLASS_MPEG) {
-		for (i = 0; i < ctrls->count; i++) {
-			struct v4l2_ext_control *ctrl = ctrls->controls + i;
-
-			err = hdpvr_try_ctrl(ctrl,
-					     dev->flags & HDPVR_FLAG_AC3_CAP);
-			if (err) {
-				ctrls->error_idx = i;
-				break;
-			}
-		}
-		return err;
-	}
-
-	return -EINVAL;
-}
-
-
-static int hdpvr_set_ctrl(struct hdpvr_device *dev,
-			  struct v4l2_ext_control *ctrl)
-{
-	struct hdpvr_options *opt = &dev->options;
-	int ret = 0;
-
-	switch (ctrl->id) {
+		ret = hdpvr_config_call(dev, CTRL_SHARPNESS, ctrl->val);
+		if (ret)
+			break;
+		dev->options.sharpness = ctrl->val;
+		return 0;
 	case V4L2_CID_MPEG_AUDIO_ENCODING:
 		if (dev->flags & HDPVR_FLAG_AC3_CAP) {
-			opt->audio_codec = ctrl->value;
-			ret = hdpvr_set_audio(dev, opt->audio_input,
+			opt->audio_codec = ctrl->val;
+			return hdpvr_set_audio(dev, opt->audio_input,
 					      opt->audio_codec);
 		}
-		break;
+		return 0;
 	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		break;
+		return 0;
 /* 	case V4L2_CID_MPEG_VIDEO_B_FRAMES: */
 /* 		if (ctrl->value == 0 && !(opt->gop_mode & 0x2)) { */
 /* 			opt->gop_mode |= 0x2; */
@@ -1050,86 +939,41 @@ static int hdpvr_set_ctrl(struct hdpvr_device *dev,
 /* 					  opt->gop_mode); */
 /* 		} */
 /* 		break; */
-	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
-		if (ctrl->value == V4L2_MPEG_VIDEO_BITRATE_MODE_CBR &&
-		    opt->bitrate_mode != HDPVR_CONSTANT) {
-			opt->bitrate_mode = HDPVR_CONSTANT;
-			hdpvr_config_call(dev, CTRL_BITRATE_MODE_VALUE,
-					  opt->bitrate_mode);
-		}
-		if (ctrl->value == V4L2_MPEG_VIDEO_BITRATE_MODE_VBR &&
-		    opt->bitrate_mode == HDPVR_CONSTANT) {
-			opt->bitrate_mode = HDPVR_VARIABLE_AVERAGE;
+	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE: {
+		uint peak_bitrate = dev->video_bitrate_peak->val / 100000;
+		uint bitrate = dev->video_bitrate->val / 100000;
+
+		if (ctrl->is_new) {
+			if (ctrl->val == V4L2_MPEG_VIDEO_BITRATE_MODE_CBR)
+				opt->bitrate_mode = HDPVR_CONSTANT;
+			else
+				opt->bitrate_mode = HDPVR_VARIABLE_AVERAGE;
 			hdpvr_config_call(dev, CTRL_BITRATE_MODE_VALUE,
 					  opt->bitrate_mode);
+			v4l2_ctrl_activate(dev->video_bitrate_peak,
+				ctrl->val != V4L2_MPEG_VIDEO_BITRATE_MODE_CBR);
 		}
-		break;
-	case V4L2_CID_MPEG_VIDEO_BITRATE: {
-		uint bitrate = ctrl->value / 100000;
-
-		opt->bitrate = bitrate;
-		if (bitrate >= opt->peak_bitrate)
-			opt->peak_bitrate = bitrate+1;
-
-		hdpvr_set_bitrate(dev);
-		break;
-	}
-	case V4L2_CID_MPEG_VIDEO_BITRATE_PEAK: {
-		uint peak_bitrate = ctrl->value / 100000;
-
-		if (opt->bitrate_mode == HDPVR_CONSTANT)
-			break;
 
-		if (opt->bitrate < peak_bitrate) {
+		if (dev->video_bitrate_peak->is_new ||
+		    dev->video_bitrate->is_new) {
+			opt->bitrate = bitrate;
 			opt->peak_bitrate = peak_bitrate;
 			hdpvr_set_bitrate(dev);
-		} else
-			ret = -EINVAL;
-		break;
+		}
+		return 0;
 	}
 	case V4L2_CID_MPEG_STREAM_TYPE:
-		break;
+		return 0;
 	default:
-		return -EINVAL;
+		break;
 	}
 	return ret;
 }
 
-static int vidioc_s_ext_ctrls(struct file *file, void *priv,
-			      struct v4l2_ext_controls *ctrls)
-{
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	int i, err = 0;
-
-	if (ctrls->ctrl_class == V4L2_CTRL_CLASS_MPEG) {
-		for (i = 0; i < ctrls->count; i++) {
-			struct v4l2_ext_control *ctrl = ctrls->controls + i;
-
-			err = hdpvr_try_ctrl(ctrl,
-					     dev->flags & HDPVR_FLAG_AC3_CAP);
-			if (err) {
-				ctrls->error_idx = i;
-				break;
-			}
-			err = hdpvr_set_ctrl(dev, ctrl);
-			if (err) {
-				ctrls->error_idx = i;
-				break;
-			}
-		}
-		return err;
-
-	}
-
-	return -EINVAL;
-}
-
 static int vidioc_enum_fmt_vid_cap(struct file *file, void *private_data,
 				    struct v4l2_fmtdesc *f)
 {
-
-	if (f->index != 0 || f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (f->index != 0)
 		return -EINVAL;
 
 	f->flags = V4L2_FMT_FLAG_COMPRESSED;
@@ -1139,56 +983,92 @@ static int vidioc_enum_fmt_vid_cap(struct file *file, void *private_data,
 	return 0;
 }
 
-static int vidioc_g_fmt_vid_cap(struct file *file, void *private_data,
+static int vidioc_g_fmt_vid_cap(struct file *file, void *_fh,
 				struct v4l2_format *f)
 {
-	struct hdpvr_fh *fh = file->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	struct hdpvr_video_info *vid_info;
-
-	if (!dev)
-		return -ENODEV;
-
-	vid_info = get_video_info(dev);
-	if (!vid_info)
-		return -EFAULT;
-
-	f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	struct hdpvr_device *dev = video_drvdata(file);
+	struct hdpvr_fh *fh = _fh;
+
+	/*
+	 * The original driver would always returns the current detected
+	 * resolution as the format (and EFAULT if it couldn't be detected).
+	 * With the introduction of VIDIOC_QUERY_DV_TIMINGS there is now a
+	 * better way of doing this, but to stay compatible with existing
+	 * applications we assume legacy mode every time an application opens
+	 * the device. Only if one of the new DV_TIMINGS ioctls is called
+	 * will the filehandle go into 'normal' mode where g_fmt returns the
+	 * last set format.
+	 */
+	if (fh->legacy_mode) {
+		struct hdpvr_video_info *vid_info;
+
+		vid_info = get_video_info(dev);
+		if (!vid_info)
+			return -EFAULT;
+		f->fmt.pix.width = vid_info->width;
+		f->fmt.pix.height = vid_info->height;
+		kfree(vid_info);
+	} else {
+		f->fmt.pix.width = dev->width;
+		f->fmt.pix.height = dev->height;
+	}
 	f->fmt.pix.pixelformat	= V4L2_PIX_FMT_MPEG;
-	f->fmt.pix.width	= vid_info->width;
-	f->fmt.pix.height	= vid_info->height;
 	f->fmt.pix.sizeimage	= dev->bulk_in_size;
-	f->fmt.pix.colorspace	= 0;
 	f->fmt.pix.bytesperline	= 0;
-	f->fmt.pix.field	= V4L2_FIELD_ANY;
-
-	kfree(vid_info);
+	f->fmt.pix.priv		= 0;
+	if (f->fmt.pix.width == 720) {
+		/* SDTV formats */
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+		f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	} else {
+		/* HDTV formats */
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE240M;
+		f->fmt.pix.field = V4L2_FIELD_NONE;
+	}
 	return 0;
 }
 
 static int vidioc_encoder_cmd(struct file *filp, void *priv,
 			       struct v4l2_encoder_cmd *a)
 {
-	struct hdpvr_fh *fh = filp->private_data;
-	struct hdpvr_device *dev = fh->dev;
-	int res;
+	struct hdpvr_device *dev = video_drvdata(filp);
+	int res = 0;
 
 	mutex_lock(&dev->io_mutex);
+	a->flags = 0;
 
-	memset(&a->raw, 0, sizeof(a->raw));
 	switch (a->cmd) {
 	case V4L2_ENC_CMD_START:
-		a->flags = 0;
+		if (dev->owner && filp->private_data != dev->owner) {
+			res = -EBUSY;
+			break;
+		}
+		if (dev->status == STATUS_STREAMING)
+			break;
 		res = hdpvr_start_streaming(dev);
+		if (!res)
+			dev->owner = filp->private_data;
+		else
+			dev->status = STATUS_IDLE;
 		break;
 	case V4L2_ENC_CMD_STOP:
+		if (dev->owner && filp->private_data != dev->owner) {
+			res = -EBUSY;
+			break;
+		}
+		if (dev->status == STATUS_IDLE)
+			break;
 		res = hdpvr_stop_streaming(dev);
+		if (!res)
+			dev->owner = NULL;
 		break;
 	default:
 		v4l2_dbg(MSG_INFO, hdpvr_debug, &dev->v4l2_dev,
 			 "Unsupported encoder cmd %d\n", a->cmd);
 		res = -EINVAL;
+		break;
 	}
+
 	mutex_unlock(&dev->io_mutex);
 	return res;
 }
@@ -1196,6 +1076,7 @@ static int vidioc_encoder_cmd(struct file *filp, void *priv,
 static int vidioc_try_encoder_cmd(struct file *filp, void *priv,
 					struct v4l2_encoder_cmd *a)
 {
+	a->flags = 0;
 	switch (a->cmd) {
 	case V4L2_ENC_CMD_START:
 	case V4L2_ENC_CMD_STOP:
@@ -1208,22 +1089,28 @@ static int vidioc_try_encoder_cmd(struct file *filp, void *priv,
 static const struct v4l2_ioctl_ops hdpvr_ioctl_ops = {
 	.vidioc_querycap	= vidioc_querycap,
 	.vidioc_s_std		= vidioc_s_std,
+	.vidioc_g_std		= vidioc_g_std,
+	.vidioc_querystd	= vidioc_querystd,
+	.vidioc_s_dv_timings	= vidioc_s_dv_timings,
+	.vidioc_g_dv_timings	= vidioc_g_dv_timings,
+	.vidioc_query_dv_timings= vidioc_query_dv_timings,
+	.vidioc_enum_dv_timings	= vidioc_enum_dv_timings,
+	.vidioc_dv_timings_cap	= vidioc_dv_timings_cap,
 	.vidioc_enum_input	= vidioc_enum_input,
 	.vidioc_g_input		= vidioc_g_input,
 	.vidioc_s_input		= vidioc_s_input,
 	.vidioc_enumaudio	= vidioc_enumaudio,
 	.vidioc_g_audio		= vidioc_g_audio,
 	.vidioc_s_audio		= vidioc_s_audio,
-	.vidioc_queryctrl	= vidioc_queryctrl,
-	.vidioc_g_ctrl		= vidioc_g_ctrl,
-	.vidioc_s_ctrl		= vidioc_s_ctrl,
-	.vidioc_g_ext_ctrls	= vidioc_g_ext_ctrls,
-	.vidioc_s_ext_ctrls	= vidioc_s_ext_ctrls,
-	.vidioc_try_ext_ctrls	= vidioc_try_ext_ctrls,
-	.vidioc_enum_fmt_vid_cap	= vidioc_enum_fmt_vid_cap,
-	.vidioc_g_fmt_vid_cap		= vidioc_g_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_cap= vidioc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
 	.vidioc_encoder_cmd	= vidioc_encoder_cmd,
 	.vidioc_try_encoder_cmd	= vidioc_try_encoder_cmd,
+	.vidioc_log_status	= v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static void hdpvr_device_release(struct video_device *vdev)
@@ -1236,9 +1123,10 @@ static void hdpvr_device_release(struct video_device *vdev)
 	mutex_unlock(&dev->io_mutex);
 
 	v4l2_device_unregister(&dev->v4l2_dev);
+	v4l2_ctrl_handler_free(&dev->hdl);
 
 	/* deregister I2C adapter */
-#if defined(CONFIG_I2C) || (CONFIG_I2C_MODULE)
+#if IS_ENABLED(CONFIG_I2C)
 	mutex_lock(&dev->i2c_mutex);
 	i2c_del_adapter(&dev->i2c_adapter);
 	mutex_unlock(&dev->i2c_mutex);
@@ -1249,41 +1137,112 @@ static void hdpvr_device_release(struct video_device *vdev)
 }
 
 static const struct video_device hdpvr_video_template = {
-/* 	.type			= VFL_TYPE_GRABBER, */
-/* 	.type2			= VID_TYPE_CAPTURE | VID_TYPE_MPEG_ENCODER, */
 	.fops			= &hdpvr_fops,
 	.release		= hdpvr_device_release,
 	.ioctl_ops 		= &hdpvr_ioctl_ops,
-	.tvnorms 		=
-		V4L2_STD_NTSC  | V4L2_STD_SECAM | V4L2_STD_PAL_B |
-		V4L2_STD_PAL_G | V4L2_STD_PAL_H | V4L2_STD_PAL_I |
-		V4L2_STD_PAL_D | V4L2_STD_PAL_M | V4L2_STD_PAL_N |
-		V4L2_STD_PAL_60,
-	.current_norm 		= V4L2_STD_NTSC | V4L2_STD_PAL_M |
-		V4L2_STD_PAL_60,
+	.tvnorms		= V4L2_STD_ALL,
+};
+
+static const struct v4l2_ctrl_ops hdpvr_ctrl_ops = {
+	.try_ctrl = hdpvr_try_ctrl,
+	.s_ctrl = hdpvr_s_ctrl,
 };
 
 int hdpvr_register_videodev(struct hdpvr_device *dev, struct device *parent,
 			    int devnum)
 {
+	struct v4l2_ctrl_handler *hdl = &dev->hdl;
+	bool ac3 = dev->flags & HDPVR_FLAG_AC3_CAP;
+	int res;
+
+	dev->cur_std = V4L2_STD_525_60;
+	dev->width = 720;
+	dev->height = 480;
+	dev->cur_dv_timings = hdpvr_dv_timings[HDPVR_DEF_DV_TIMINGS_IDX];
+	v4l2_ctrl_handler_init(hdl, 11);
+	if (dev->fw_ver > 0x15) {
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0x0, 0xff, 1, 0x80);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_CONTRAST, 0x0, 0xff, 1, 0x40);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_SATURATION, 0x0, 0xff, 1, 0x40);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_HUE, 0x0, 0x1e, 1, 0xf);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0x0, 0xff, 1, 0x80);
+	} else {
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0x0, 0xff, 1, 0x86);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_CONTRAST, 0x0, 0xff, 1, 0x80);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_SATURATION, 0x0, 0xff, 1, 0x80);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_HUE, 0x0, 0xff, 1, 0x80);
+		v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0x0, 0xff, 1, 0x80);
+	}
+
+	v4l2_ctrl_new_std_menu(hdl, &hdpvr_ctrl_ops,
+		V4L2_CID_MPEG_STREAM_TYPE,
+		V4L2_MPEG_STREAM_TYPE_MPEG2_TS,
+		0x1, V4L2_MPEG_STREAM_TYPE_MPEG2_TS);
+	v4l2_ctrl_new_std_menu(hdl, &hdpvr_ctrl_ops,
+		V4L2_CID_MPEG_AUDIO_ENCODING,
+		ac3 ? V4L2_MPEG_AUDIO_ENCODING_AC3 : V4L2_MPEG_AUDIO_ENCODING_AAC,
+		0x7, V4L2_MPEG_AUDIO_ENCODING_AAC);
+	v4l2_ctrl_new_std_menu(hdl, &hdpvr_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_ENCODING,
+		V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC, 0x3,
+		V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC);
+
+	dev->video_mode = v4l2_ctrl_new_std_menu(hdl, &hdpvr_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
+		V4L2_MPEG_VIDEO_BITRATE_MODE_CBR, 0,
+		V4L2_MPEG_VIDEO_BITRATE_MODE_CBR);
+
+	dev->video_bitrate = v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_BITRATE,
+		1000000, 13500000, 100000, 6500000);
+	dev->video_bitrate_peak = v4l2_ctrl_new_std(hdl, &hdpvr_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_BITRATE_PEAK,
+		1100000, 20200000, 100000, 9000000);
+	dev->v4l2_dev.ctrl_handler = hdl;
+	if (hdl->error) {
+		res = hdl->error;
+		v4l2_err(&dev->v4l2_dev, "Could not register controls\n");
+		goto error;
+	}
+	v4l2_ctrl_cluster(3, &dev->video_mode);
+	res = v4l2_ctrl_handler_setup(hdl);
+	if (res < 0) {
+		v4l2_err(&dev->v4l2_dev, "Could not setup controls\n");
+		goto error;
+	}
+
 	/* setup and register video device */
 	dev->video_dev = video_device_alloc();
 	if (!dev->video_dev) {
 		v4l2_err(&dev->v4l2_dev, "video_device_alloc() failed\n");
+		res = -ENOMEM;
 		goto error;
 	}
 
-	*(dev->video_dev) = hdpvr_video_template;
+	*dev->video_dev = hdpvr_video_template;
 	strcpy(dev->video_dev->name, "Hauppauge HD PVR");
-	dev->video_dev->parent = parent;
+	dev->video_dev->v4l2_dev = &dev->v4l2_dev;
 	video_set_drvdata(dev->video_dev, dev);
+	set_bit(V4L2_FL_USE_FH_PRIO, &dev->video_dev->flags);
 
-	if (video_register_device(dev->video_dev, VFL_TYPE_GRABBER, devnum)) {
+	res = video_register_device(dev->video_dev, VFL_TYPE_GRABBER, devnum);
+	if (res < 0) {
 		v4l2_err(&dev->v4l2_dev, "video_device registration failed\n");
 		goto error;
 	}
 
 	return 0;
 error:
-	return -ENOMEM;
+	v4l2_ctrl_handler_free(hdl);
+	return res;
 }
diff --git a/drivers/media/usb/hdpvr/hdpvr.h b/drivers/media/usb/hdpvr/hdpvr.h
index fea3c6926997..1478f3d57630 100644
--- a/drivers/media/usb/hdpvr/hdpvr.h
+++ b/drivers/media/usb/hdpvr/hdpvr.h
@@ -16,6 +16,7 @@
 #include <linux/videodev2.h>
 
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
 #include <media/ir-kbd-i2c.h>
 
 #define HDPVR_MAX 8
@@ -37,6 +38,7 @@
 #define HDPVR_FIRMWARE_VERSION_AC3	0x0d
 #define HDPVR_FIRMWARE_VERSION_0X12	0x12
 #define HDPVR_FIRMWARE_VERSION_0X15	0x15
+#define HDPVR_FIRMWARE_VERSION_0X1E	0x1e
 
 /* #define HDPVR_DEBUG */
 
@@ -65,10 +67,19 @@ struct hdpvr_options {
 struct hdpvr_device {
 	/* the v4l device for this device */
 	struct video_device	*video_dev;
+	/* the control handler for this device */
+	struct v4l2_ctrl_handler hdl;
 	/* the usb device for this device */
 	struct usb_device	*udev;
 	/* v4l2-device unused */
 	struct v4l2_device	v4l2_dev;
+	struct { /* video mode/bitrate control cluster */
+		struct v4l2_ctrl *video_mode;
+		struct v4l2_ctrl *video_bitrate;
+		struct v4l2_ctrl *video_bitrate_peak;
+	};
+	/* v4l2 format */
+	uint width, height;
 
 	/* the max packet size of the bulk endpoint */
 	size_t			bulk_in_size;
@@ -77,11 +88,11 @@ struct hdpvr_device {
 
 	/* holds the current device status */
 	__u8			status;
-	/* count the number of openers */
-	uint			open_count;
 
-	/* holds the cureent set options */
+	/* holds the current set options */
 	struct hdpvr_options	options;
+	v4l2_std_id		cur_std;
+	struct v4l2_dv_timings	cur_dv_timings;
 
 	uint			flags;
 
@@ -99,6 +110,8 @@ struct hdpvr_device {
 	struct workqueue_struct	*workqueue;
 	/**/
 	struct work_struct	worker;
+	/* current stream owner */
+	struct v4l2_fh		*owner;
 
 	/* I2C adapter */
 	struct i2c_adapter	i2c_adapter;
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-hdw.c b/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
index 299751a8b06b..e11267f35d87 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
@@ -5165,7 +5165,7 @@ static int pvr2_hdw_get_eeprom_addr(struct pvr2_hdw *hdw)
 
 
 int pvr2_hdw_register_access(struct pvr2_hdw *hdw,
-			     struct v4l2_dbg_match *match, u64 reg_id,
+			     const struct v4l2_dbg_match *match, u64 reg_id,
 			     int setFl, u64 *val_ptr)
 {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-hdw.h b/drivers/media/usb/pvrusb2/pvrusb2-hdw.h
index 8060fc666eeb..91bae930cd79 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-hdw.h
+++ b/drivers/media/usb/pvrusb2/pvrusb2-hdw.h
@@ -240,7 +240,7 @@ void pvr2_hdw_v4l_store_minor_number(struct pvr2_hdw *,
    setFl   - true to set the register, false to read it
    val_ptr - storage location for source / result. */
 int pvr2_hdw_register_access(struct pvr2_hdw *,
-			     struct v4l2_dbg_match *match, u64 reg_id,
+			     const struct v4l2_dbg_match *match, u64 reg_id,
 			     int setFl, u64 *val_ptr);
 
 /* The following entry points are all lower level things you normally don't
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c b/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c
index 34c3b6e80e86..a8a65fa57930 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c
@@ -196,13 +196,13 @@ static int pvr2_g_std(struct file *file, void *priv, v4l2_std_id *std)
 	return ret;
 }
 
-static int pvr2_s_std(struct file *file, void *priv, v4l2_std_id *std)
+static int pvr2_s_std(struct file *file, void *priv, v4l2_std_id std)
 {
 	struct pvr2_v4l2_fh *fh = file->private_data;
 	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
 
 	return pvr2_ctrl_set_value(
-		pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_STDCUR), *std);
+		pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_STDCUR), std);
 }
 
 static int pvr2_querystd(struct file *file, void *priv, v4l2_std_id *std)
@@ -352,7 +352,7 @@ static int pvr2_g_tuner(struct file *file, void *priv, struct v4l2_tuner *vt)
 	return pvr2_hdw_get_tuner_status(hdw, vt);
 }
 
-static int pvr2_s_tuner(struct file *file, void *priv, struct v4l2_tuner *vt)
+static int pvr2_s_tuner(struct file *file, void *priv, const struct v4l2_tuner *vt)
 {
 	struct pvr2_v4l2_fh *fh = file->private_data;
 	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
@@ -365,7 +365,7 @@ static int pvr2_s_tuner(struct file *file, void *priv, struct v4l2_tuner *vt)
 			vt->audmode);
 }
 
-static int pvr2_s_frequency(struct file *file, void *priv, struct v4l2_frequency *vf)
+static int pvr2_s_frequency(struct file *file, void *priv, const struct v4l2_frequency *vf)
 {
 	struct pvr2_v4l2_fh *fh = file->private_data;
 	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
@@ -815,7 +815,7 @@ static int pvr2_g_register(struct file *file, void *priv, struct v4l2_dbg_regist
 	return ret;
 }
 
-static int pvr2_s_register(struct file *file, void *priv, struct v4l2_dbg_register *req)
+static int pvr2_s_register(struct file *file, void *priv, const struct v4l2_dbg_register *req)
 {
 	struct pvr2_v4l2_fh *fh = file->private_data;
 	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
diff --git a/drivers/media/usb/pwc/pwc-if.c b/drivers/media/usb/pwc/pwc-if.c
index 5ec15cb1ed26..77bbf7889659 100644
--- a/drivers/media/usb/pwc/pwc-if.c
+++ b/drivers/media/usb/pwc/pwc-if.c
@@ -1001,6 +1001,7 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
 	pdev->vb_queue.buf_struct_size = sizeof(struct pwc_frame_buf);
 	pdev->vb_queue.ops = &pwc_vb_queue_ops;
 	pdev->vb_queue.mem_ops = &vb2_vmalloc_memops;
+	pdev->vb_queue.timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	rc = vb2_queue_init(&pdev->vb_queue);
 	if (rc < 0) {
 		PWC_ERROR("Oops, could not initialize vb2 queue.\n");
diff --git a/drivers/media/usb/s2255/s2255drv.c b/drivers/media/usb/s2255/s2255drv.c
index 498c57ea5d32..ab97e7d0b4f2 100644
--- a/drivers/media/usb/s2255/s2255drv.c
+++ b/drivers/media/usb/s2255/s2255drv.c
@@ -43,12 +43,14 @@
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 #include <linux/mm.h>
+#include <linux/vmalloc.h>
+#include <linux/usb.h>
 #include <media/videobuf-vmalloc.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
-#include <linux/vmalloc.h>
-#include <linux/usb.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 
 #define S2255_VERSION		"1.22.1"
 #define FIRMWARE_FILE_NAME "f2255usb.bin"
@@ -217,12 +219,15 @@ struct s2255_dev;
 
 struct s2255_channel {
 	struct video_device	vdev;
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_ctrl	*jpegqual_ctrl;
 	int			resources;
 	struct s2255_dmaqueue	vidq;
 	struct s2255_bufferi	buffer;
 	struct s2255_mode	mode;
+	v4l2_std_id		std;
 	/* jpeg compression */
-	struct v4l2_jpegcompression jc;
+	unsigned		jpegqual;
 	/* capture parameters (for high quality mode full size) */
 	struct v4l2_captureparm cap_parm;
 	int			cur_frame;
@@ -292,6 +297,8 @@ struct s2255_buffer {
 };
 
 struct s2255_fh {
+	/* this must be the first field in this struct */
+	struct v4l2_fh		fh;
 	struct s2255_dev	*dev;
 	struct videobuf_queue	vb_vidq;
 	enum v4l2_buf_type	type;
@@ -306,7 +313,7 @@ struct s2255_fh {
 /* Need DSP version 5+ for video status feature */
 #define S2255_MIN_DSP_STATUS      5
 #define S2255_MIN_DSP_COLORFILTER 8
-#define S2255_NORMS		(V4L2_STD_PAL | V4L2_STD_NTSC)
+#define S2255_NORMS		(V4L2_STD_ALL)
 
 /* private V4L2 controls */
 
@@ -336,7 +343,7 @@ struct s2255_fh {
  */
 #define S2255_V4L2_YC_ON  1
 #define S2255_V4L2_YC_OFF 0
-#define V4L2_CID_PRIVATE_COLORFILTER (V4L2_CID_PRIVATE_BASE + 0)
+#define V4L2_CID_S2255_COLORFILTER (V4L2_CID_USER_S2255_BASE + 0)
 
 /* frame prefix size (sent once every frame) */
 #define PREFIX_SIZE		512
@@ -409,11 +416,6 @@ MODULE_DEVICE_TABLE(usb, s2255_table);
 /* JPEG formats must be defined last to support jpeg_enable parameter */
 static const struct s2255_fmt formats[] = {
 	{
-		.name = "4:2:2, planar, YUV422P",
-		.fourcc = V4L2_PIX_FMT_YUV422P,
-		.depth = 16
-
-	}, {
 		.name = "4:2:2, packed, YUYV",
 		.fourcc = V4L2_PIX_FMT_YUYV,
 		.depth = 16
@@ -423,6 +425,11 @@ static const struct s2255_fmt formats[] = {
 		.fourcc = V4L2_PIX_FMT_UYVY,
 		.depth = 16
 	}, {
+		.name = "4:2:2, planar, YUV422P",
+		.fourcc = V4L2_PIX_FMT_YUV422P,
+		.depth = 16
+
+	}, {
 		.name = "8bpp GREY",
 		.fourcc = V4L2_PIX_FMT_GREY,
 		.depth = 8
@@ -437,27 +444,27 @@ static const struct s2255_fmt formats[] = {
 	}
 };
 
-static int norm_maxw(struct video_device *vdev)
+static int norm_maxw(struct s2255_channel *channel)
 {
-	return (vdev->current_norm & V4L2_STD_NTSC) ?
+	return (channel->std & V4L2_STD_525_60) ?
 	    LINE_SZ_4CIFS_NTSC : LINE_SZ_4CIFS_PAL;
 }
 
-static int norm_maxh(struct video_device *vdev)
+static int norm_maxh(struct s2255_channel *channel)
 {
-	return (vdev->current_norm & V4L2_STD_NTSC) ?
+	return (channel->std & V4L2_STD_525_60) ?
 	    (NUM_LINES_1CIFS_NTSC * 2) : (NUM_LINES_1CIFS_PAL * 2);
 }
 
-static int norm_minw(struct video_device *vdev)
+static int norm_minw(struct s2255_channel *channel)
 {
-	return (vdev->current_norm & V4L2_STD_NTSC) ?
+	return (channel->std & V4L2_STD_525_60) ?
 	    LINE_SZ_1CIFS_NTSC : LINE_SZ_1CIFS_PAL;
 }
 
-static int norm_minh(struct video_device *vdev)
+static int norm_minh(struct s2255_channel *channel)
 {
-	return (vdev->current_norm & V4L2_STD_NTSC) ?
+	return (channel->std & V4L2_STD_525_60) ?
 	    (NUM_LINES_1CIFS_NTSC) : (NUM_LINES_1CIFS_PAL);
 }
 
@@ -515,7 +522,7 @@ static void s2255_timer(unsigned long user_data)
 
 
 /* this loads the firmware asynchronously.
-   Originally this was done synchroously in probe.
+   Originally this was done synchronously in probe.
    But it is better to load it asynchronously here than block
    inside the probe function. Blocking inside probe affects boot time.
    FW loading is triggered by the timer in the probe function
@@ -719,10 +726,10 @@ static int buffer_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb,
 	if (channel->fmt == NULL)
 		return -EINVAL;
 
-	if ((w < norm_minw(&channel->vdev)) ||
-	    (w > norm_maxw(&channel->vdev)) ||
-	    (h < norm_minh(&channel->vdev)) ||
-	    (h > norm_maxh(&channel->vdev))) {
+	if ((w < norm_minw(channel)) ||
+	    (w > norm_maxw(channel)) ||
+	    (h < norm_minh(channel)) ||
+	    (h > norm_maxh(channel))) {
 		dprintk(4, "invalid buffer prepare\n");
 		return -EINVAL;
 	}
@@ -810,37 +817,17 @@ static void res_free(struct s2255_fh *fh)
 	dprintk(1, "res: put\n");
 }
 
-static int vidioc_querymenu(struct file *file, void *priv,
-			    struct v4l2_querymenu *qmenu)
-{
-	static const char *colorfilter[] = {
-		"Off",
-		"On",
-		NULL
-	};
-	if (qmenu->id == V4L2_CID_PRIVATE_COLORFILTER) {
-		int i;
-		const char **menu_items = colorfilter;
-		for (i = 0; i < qmenu->index && menu_items[i]; i++)
-			; /* do nothing (from v4l2-common.c) */
-		if (menu_items[i] == NULL || menu_items[i][0] == '\0')
-			return -EINVAL;
-		strlcpy(qmenu->name, menu_items[qmenu->index],
-			sizeof(qmenu->name));
-		return 0;
-	}
-	return v4l2_ctrl_query_menu(qmenu, NULL, NULL);
-}
-
 static int vidioc_querycap(struct file *file, void *priv,
 			   struct v4l2_capability *cap)
 {
 	struct s2255_fh *fh = file->private_data;
 	struct s2255_dev *dev = fh->dev;
+
 	strlcpy(cap->driver, "s2255", sizeof(cap->driver));
 	strlcpy(cap->card, "s2255", sizeof(cap->card));
 	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -865,13 +852,20 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 {
 	struct s2255_fh *fh = priv;
 	struct s2255_channel *channel = fh->channel;
+	int is_ntsc = channel->std & V4L2_STD_525_60;
 
 	f->fmt.pix.width = channel->width;
 	f->fmt.pix.height = channel->height;
-	f->fmt.pix.field = fh->vb_vidq.field;
+	if (f->fmt.pix.height >=
+	    (is_ntsc ? NUM_LINES_1CIFS_NTSC : NUM_LINES_1CIFS_PAL) * 2)
+		f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	else
+		f->fmt.pix.field = V4L2_FIELD_TOP;
 	f->fmt.pix.pixelformat = channel->fmt->fourcc;
 	f->fmt.pix.bytesperline = f->fmt.pix.width * (channel->fmt->depth >> 3);
 	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
 	return 0;
 }
 
@@ -880,12 +874,9 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 {
 	const struct s2255_fmt *fmt;
 	enum v4l2_field field;
-	int  b_any_field = 0;
 	struct s2255_fh *fh = priv;
 	struct s2255_channel *channel = fh->channel;
-	int is_ntsc;
-	is_ntsc =
-		(channel->vdev.current_norm & V4L2_STD_NTSC) ? 1 : 0;
+	int is_ntsc = channel->std & V4L2_STD_525_60;
 
 	fmt = format_by_fourcc(f->fmt.pix.pixelformat);
 
@@ -893,8 +884,6 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 		return -EINVAL;
 
 	field = f->fmt.pix.field;
-	if (field == V4L2_FIELD_ANY)
-		b_any_field = 1;
 
 	dprintk(50, "%s NTSC: %d suggested width: %d, height: %d\n",
 		__func__, is_ntsc, f->fmt.pix.width, f->fmt.pix.height);
@@ -902,24 +891,10 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 		/* NTSC */
 		if (f->fmt.pix.height >= NUM_LINES_1CIFS_NTSC * 2) {
 			f->fmt.pix.height = NUM_LINES_1CIFS_NTSC * 2;
-			if (b_any_field) {
-				field = V4L2_FIELD_SEQ_TB;
-			} else if (!((field == V4L2_FIELD_INTERLACED) ||
-				      (field == V4L2_FIELD_SEQ_TB) ||
-				      (field == V4L2_FIELD_INTERLACED_TB))) {
-				dprintk(1, "unsupported field setting\n");
-				return -EINVAL;
-			}
+			field = V4L2_FIELD_INTERLACED;
 		} else {
 			f->fmt.pix.height = NUM_LINES_1CIFS_NTSC;
-			if (b_any_field) {
-				field = V4L2_FIELD_TOP;
-			} else if (!((field == V4L2_FIELD_TOP) ||
-				      (field == V4L2_FIELD_BOTTOM))) {
-				dprintk(1, "unsupported field setting\n");
-				return -EINVAL;
-			}
-
+			field = V4L2_FIELD_TOP;
 		}
 		if (f->fmt.pix.width >= LINE_SZ_4CIFS_NTSC)
 			f->fmt.pix.width = LINE_SZ_4CIFS_NTSC;
@@ -933,41 +908,25 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 		/* PAL */
 		if (f->fmt.pix.height >= NUM_LINES_1CIFS_PAL * 2) {
 			f->fmt.pix.height = NUM_LINES_1CIFS_PAL * 2;
-			if (b_any_field) {
-				field = V4L2_FIELD_SEQ_TB;
-			} else if (!((field == V4L2_FIELD_INTERLACED) ||
-				      (field == V4L2_FIELD_SEQ_TB) ||
-				      (field == V4L2_FIELD_INTERLACED_TB))) {
-				dprintk(1, "unsupported field setting\n");
-				return -EINVAL;
-			}
+			field = V4L2_FIELD_INTERLACED;
 		} else {
 			f->fmt.pix.height = NUM_LINES_1CIFS_PAL;
-			if (b_any_field) {
-				field = V4L2_FIELD_TOP;
-			} else if (!((field == V4L2_FIELD_TOP) ||
-				     (field == V4L2_FIELD_BOTTOM))) {
-				dprintk(1, "unsupported field setting\n");
-				return -EINVAL;
-			}
+			field = V4L2_FIELD_TOP;
 		}
-		if (f->fmt.pix.width >= LINE_SZ_4CIFS_PAL) {
+		if (f->fmt.pix.width >= LINE_SZ_4CIFS_PAL)
 			f->fmt.pix.width = LINE_SZ_4CIFS_PAL;
-			field = V4L2_FIELD_SEQ_TB;
-		} else if (f->fmt.pix.width >= LINE_SZ_2CIFS_PAL) {
+		else if (f->fmt.pix.width >= LINE_SZ_2CIFS_PAL)
 			f->fmt.pix.width = LINE_SZ_2CIFS_PAL;
-			field = V4L2_FIELD_TOP;
-		} else if (f->fmt.pix.width >= LINE_SZ_1CIFS_PAL) {
+		else if (f->fmt.pix.width >= LINE_SZ_1CIFS_PAL)
 			f->fmt.pix.width = LINE_SZ_1CIFS_PAL;
-			field = V4L2_FIELD_TOP;
-		} else {
+		else
 			f->fmt.pix.width = LINE_SZ_1CIFS_PAL;
-			field = V4L2_FIELD_TOP;
-		}
 	}
 	f->fmt.pix.field = field;
 	f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
 	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
 	dprintk(50, "%s: set width %d height %d field %d\n", __func__,
 		f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.field);
 	return 0;
@@ -1012,8 +971,8 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	channel->height = f->fmt.pix.height;
 	fh->vb_vidq.field = f->fmt.pix.field;
 	fh->type = f->type;
-	if (channel->width > norm_minw(&channel->vdev)) {
-		if (channel->height > norm_minh(&channel->vdev)) {
+	if (channel->width > norm_minw(channel)) {
+		if (channel->height > norm_minh(channel)) {
 			if (channel->cap_parm.capturemode &
 			    V4L2_MODE_HIGHQUALITY)
 				mode.scale = SCALE_4CIFSI;
@@ -1035,7 +994,7 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	case V4L2_PIX_FMT_MJPEG:
 		mode.color &= ~MASK_COLOR;
 		mode.color |= COLOR_JPG;
-		mode.color |= (channel->jc.quality << 8);
+		mode.color |= (channel->jpegqual << 8);
 		break;
 	case V4L2_PIX_FMT_YUV422P:
 		mode.color &= ~MASK_COLOR;
@@ -1198,6 +1157,8 @@ static int s2255_set_mode(struct s2255_channel *channel,
 	__le32 *buffer;
 	unsigned long chn_rev;
 	struct s2255_dev *dev = to_s2255_dev(channel->vdev.v4l2_dev);
+	int i;
+
 	chn_rev = G_chnmap[channel->idx];
 	dprintk(3, "%s channel: %d\n", __func__, channel->idx);
 	/* if JPEG, set the quality */
@@ -1205,7 +1166,7 @@ static int s2255_set_mode(struct s2255_channel *channel,
 		mode->color &= ~MASK_COLOR;
 		mode->color |= COLOR_JPG;
 		mode->color &= ~MASK_JPG_QUALITY;
-		mode->color |= (channel->jc.quality << 8);
+		mode->color |= (channel->jpegqual << 8);
 	}
 	/* save the mode */
 	channel->mode = *mode;
@@ -1220,7 +1181,8 @@ static int s2255_set_mode(struct s2255_channel *channel,
 	buffer[0] = IN_DATA_TOKEN;
 	buffer[1] = (__le32) cpu_to_le32(chn_rev);
 	buffer[2] = CMD_SET_MODE;
-	memcpy(&buffer[3], &channel->mode, sizeof(struct s2255_mode));
+	for (i = 0; i < sizeof(struct s2255_mode) / sizeof(u32); i++)
+		buffer[3 + i] = cpu_to_le32(((u32 *)&channel->mode)[i]);
 	channel->setmode_ready = 0;
 	res = s2255_write_config(dev->udev, (unsigned char *)buffer, 512);
 	if (debug)
@@ -1332,12 +1294,14 @@ static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *i)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id i)
 {
 	struct s2255_fh *fh = priv;
 	struct s2255_mode mode;
 	struct videobuf_queue *q = &fh->vb_vidq;
+	struct s2255_channel *channel = fh->channel;
 	int ret = 0;
+
 	mutex_lock(&q->vb_lock);
 	if (videobuf_queue_is_busy(q)) {
 		dprintk(1, "queue busy\n");
@@ -1350,24 +1314,30 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *i)
 		goto out_s_std;
 	}
 	mode = fh->channel->mode;
-	if (*i & V4L2_STD_NTSC) {
-		dprintk(4, "%s NTSC\n", __func__);
+	if (i & V4L2_STD_525_60) {
+		dprintk(4, "%s 60 Hz\n", __func__);
 		/* if changing format, reset frame decimation/intervals */
 		if (mode.format != FORMAT_NTSC) {
 			mode.restart = 1;
 			mode.format = FORMAT_NTSC;
 			mode.fdec = FDEC_1;
+			channel->width = LINE_SZ_4CIFS_NTSC;
+			channel->height = NUM_LINES_4CIFS_NTSC * 2;
 		}
-	} else if (*i & V4L2_STD_PAL) {
-		dprintk(4, "%s PAL\n", __func__);
+	} else if (i & V4L2_STD_625_50) {
+		dprintk(4, "%s 50 Hz\n", __func__);
 		if (mode.format != FORMAT_PAL) {
 			mode.restart = 1;
 			mode.format = FORMAT_PAL;
 			mode.fdec = FDEC_1;
+			channel->width = LINE_SZ_4CIFS_PAL;
+			channel->height = NUM_LINES_4CIFS_PAL * 2;
 		}
 	} else {
 		ret = -EINVAL;
+		goto out_s_std;
 	}
+	fh->channel->std = i;
 	if (mode.restart)
 		s2255_set_mode(fh->channel, &mode);
 out_s_std:
@@ -1375,6 +1345,14 @@ out_s_std:
 	return ret;
 }
 
+static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *i)
+{
+	struct s2255_fh *fh = priv;
+
+	*i = fh->channel->std;
+	return 0;
+}
+
 /* Sensoray 2255 is a multiple channel capture device.
    It does not have a "crossbar" of inputs.
    We use one V4L device per channel. The user must
@@ -1427,110 +1405,36 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 	return 0;
 }
 
-/* --- controls ---------------------------------------------- */
-static int vidioc_queryctrl(struct file *file, void *priv,
-			    struct v4l2_queryctrl *qc)
-{
-	struct s2255_fh *fh = priv;
-	struct s2255_channel *channel = fh->channel;
-	struct s2255_dev *dev = fh->dev;
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		v4l2_ctrl_query_fill(qc, -127, 127, 1, DEF_BRIGHT);
-		break;
-	case V4L2_CID_CONTRAST:
-		v4l2_ctrl_query_fill(qc, 0, 255, 1, DEF_CONTRAST);
-		break;
-	case V4L2_CID_SATURATION:
-		v4l2_ctrl_query_fill(qc, 0, 255, 1, DEF_SATURATION);
-		break;
-	case V4L2_CID_HUE:
-		v4l2_ctrl_query_fill(qc, 0, 255, 1, DEF_HUE);
-		break;
-	case V4L2_CID_PRIVATE_COLORFILTER:
-		if (dev->dsp_fw_ver < S2255_MIN_DSP_COLORFILTER)
-			return -EINVAL;
-		if ((dev->pid == 0x2257) && (channel->idx > 1))
-			return -EINVAL;
-		strlcpy(qc->name, "Color Filter", sizeof(qc->name));
-		qc->type = V4L2_CTRL_TYPE_MENU;
-		qc->minimum = 0;
-		qc->maximum = 1;
-		qc->step = 1;
-		qc->default_value = 1;
-		qc->flags = 0;
-		break;
-	default:
-		return -EINVAL;
-	}
-	dprintk(4, "%s, id %d\n", __func__, qc->id);
-	return 0;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
-{
-	struct s2255_fh *fh = priv;
-	struct s2255_dev *dev = fh->dev;
-	struct s2255_channel *channel = fh->channel;
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = channel->mode.bright;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = channel->mode.contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = channel->mode.saturation;
-		break;
-	case V4L2_CID_HUE:
-		ctrl->value = channel->mode.hue;
-		break;
-	case V4L2_CID_PRIVATE_COLORFILTER:
-		if (dev->dsp_fw_ver < S2255_MIN_DSP_COLORFILTER)
-			return -EINVAL;
-		if ((dev->pid == 0x2257) && (channel->idx > 1))
-			return -EINVAL;
-		ctrl->value = !((channel->mode.color & MASK_INPUT_TYPE) >> 16);
-		break;
-	default:
-		return -EINVAL;
-	}
-	dprintk(4, "%s, id %d val %d\n", __func__, ctrl->id, ctrl->value);
-	return 0;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
+static int s2255_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct s2255_fh *fh = priv;
-	struct s2255_channel *channel = fh->channel;
-	struct s2255_dev *dev = to_s2255_dev(channel->vdev.v4l2_dev);
+	struct s2255_channel *channel =
+		container_of(ctrl->handler, struct s2255_channel, hdl);
 	struct s2255_mode mode;
+
 	mode = channel->mode;
 	dprintk(4, "%s\n", __func__);
+
 	/* update the mode to the corresponding value */
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		mode.bright = ctrl->value;
+		mode.bright = ctrl->val;
 		break;
 	case V4L2_CID_CONTRAST:
-		mode.contrast = ctrl->value;
+		mode.contrast = ctrl->val;
 		break;
 	case V4L2_CID_HUE:
-		mode.hue = ctrl->value;
+		mode.hue = ctrl->val;
 		break;
 	case V4L2_CID_SATURATION:
-		mode.saturation = ctrl->value;
+		mode.saturation = ctrl->val;
 		break;
-	case V4L2_CID_PRIVATE_COLORFILTER:
-		if (dev->dsp_fw_ver < S2255_MIN_DSP_COLORFILTER)
-			return -EINVAL;
-		if ((dev->pid == 0x2257) && (channel->idx > 1))
-			return -EINVAL;
+	case V4L2_CID_S2255_COLORFILTER:
 		mode.color &= ~MASK_INPUT_TYPE;
-		mode.color |= ((ctrl->value ? 0 : 1) << 16);
+		mode.color |= !ctrl->val << 16;
 		break;
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		channel->jpegqual = ctrl->val;
+		return 0;
 	default:
 		return -EINVAL;
 	}
@@ -1539,7 +1443,7 @@ static int vidioc_s_ctrl(struct file *file, void *priv,
 	   some V4L programs restart stream unnecessarily
 	   after a s_crtl.
 	*/
-	s2255_set_mode(fh->channel, &mode);
+	s2255_set_mode(channel, &mode);
 	return 0;
 }
 
@@ -1548,7 +1452,9 @@ static int vidioc_g_jpegcomp(struct file *file, void *priv,
 {
 	struct s2255_fh *fh = priv;
 	struct s2255_channel *channel = fh->channel;
-	*jc = channel->jc;
+
+	memset(jc, 0, sizeof(*jc));
+	jc->quality = channel->jpegqual;
 	dprintk(2, "%s: quality %d\n", __func__, jc->quality);
 	return 0;
 }
@@ -1560,7 +1466,7 @@ static int vidioc_s_jpegcomp(struct file *file, void *priv,
 	struct s2255_channel *channel = fh->channel;
 	if (jc->quality < 0 || jc->quality > 100)
 		return -EINVAL;
-	channel->jc.quality = jc->quality;
+	v4l2_ctrl_s_ctrl(channel->jpegqual_ctrl, jc->quality);
 	dprintk(2, "%s: quality %d\n", __func__, jc->quality);
 	return 0;
 }
@@ -1573,7 +1479,6 @@ static int vidioc_g_parm(struct file *file, void *priv,
 	struct s2255_channel *channel = fh->channel;
 	if (sp->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
-	memset(sp, 0, sizeof(struct v4l2_streamparm));
 	sp->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
 	sp->parm.capture.capturemode = channel->cap_parm.capturemode;
 	def_num = (channel->mode.format == FORMAT_NTSC) ? 1001 : 1000;
@@ -1643,36 +1548,64 @@ static int vidioc_s_parm(struct file *file, void *priv,
 	return 0;
 }
 
+#define NUM_SIZE_ENUMS 3
+static const struct v4l2_frmsize_discrete ntsc_sizes[] = {
+	{ 640, 480 },
+	{ 640, 240 },
+	{ 320, 240 },
+};
+static const struct v4l2_frmsize_discrete pal_sizes[] = {
+	{ 704, 576 },
+	{ 704, 288 },
+	{ 352, 288 },
+};
+
+static int vidioc_enum_framesizes(struct file *file, void *priv,
+			    struct v4l2_frmsizeenum *fe)
+{
+	struct s2255_fh *fh = priv;
+	struct s2255_channel *channel = fh->channel;
+	int is_ntsc = channel->std & V4L2_STD_525_60;
+	const struct s2255_fmt *fmt;
+
+	if (fe->index >= NUM_SIZE_ENUMS)
+		return -EINVAL;
+
+	fmt = format_by_fourcc(fe->pixel_format);
+	if (fmt == NULL)
+		return -EINVAL;
+	fe->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fe->discrete = is_ntsc ?  ntsc_sizes[fe->index] : pal_sizes[fe->index];
+	return 0;
+}
+
 static int vidioc_enum_frameintervals(struct file *file, void *priv,
 			    struct v4l2_frmivalenum *fe)
 {
-	int is_ntsc = 0;
+	struct s2255_fh *fh = priv;
+	struct s2255_channel *channel = fh->channel;
+	const struct s2255_fmt *fmt;
+	const struct v4l2_frmsize_discrete *sizes;
+	int is_ntsc = channel->std & V4L2_STD_525_60;
 #define NUM_FRAME_ENUMS 4
 	int frm_dec[NUM_FRAME_ENUMS] = {1, 2, 3, 5};
+	int i;
+
 	if (fe->index >= NUM_FRAME_ENUMS)
 		return -EINVAL;
-	switch (fe->width) {
-	case 640:
-		if (fe->height != 240 && fe->height != 480)
-			return -EINVAL;
-		is_ntsc = 1;
-		break;
-	case 320:
-		if (fe->height != 240)
-			return -EINVAL;
-		is_ntsc = 1;
-		break;
-	case 704:
-		if (fe->height != 288 && fe->height != 576)
-			return -EINVAL;
-		break;
-	case 352:
-		if (fe->height != 288)
-			return -EINVAL;
-		break;
-	default:
+
+	fmt = format_by_fourcc(fe->pixel_format);
+	if (fmt == NULL)
 		return -EINVAL;
-	}
+
+	sizes = is_ntsc ? ntsc_sizes : pal_sizes;
+	for (i = 0; i < NUM_SIZE_ENUMS; i++, sizes++)
+		if (fe->width == sizes->width &&
+		    fe->height == sizes->height)
+			break;
+	if (i == NUM_SIZE_ENUMS)
+		return -EINVAL;
+
 	fe->type = V4L2_FRMIVAL_TYPE_DISCRETE;
 	fe->discrete.denominator = is_ntsc ? 30000 : 25000;
 	fe->discrete.numerator = (is_ntsc ? 1001 : 1000) * frm_dec[fe->index];
@@ -1757,7 +1690,9 @@ static int __s2255_open(struct file *file)
 	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
 	if (NULL == fh)
 		return -ENOMEM;
-	file->private_data = fh;
+	v4l2_fh_init(&fh->fh, vdev);
+	v4l2_fh_add(&fh->fh);
+	file->private_data = &fh->fh;
 	fh->dev = dev;
 	fh->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	fh->channel = channel;
@@ -1800,12 +1735,13 @@ static unsigned int s2255_poll(struct file *file,
 {
 	struct s2255_fh *fh = file->private_data;
 	struct s2255_dev *dev = fh->dev;
-	int rc;
+	int rc = v4l2_ctrl_poll(file, wait);
+
 	dprintk(100, "%s\n", __func__);
 	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != fh->type)
 		return POLLERR;
 	mutex_lock(&dev->lock);
-	rc = videobuf_poll_stream(file, &fh->vb_vidq, wait);
+	rc |= videobuf_poll_stream(file, &fh->vb_vidq, wait);
 	mutex_unlock(&dev->lock);
 	return rc;
 }
@@ -1852,6 +1788,8 @@ static int s2255_release(struct file *file)
 	videobuf_mmap_free(&fh->vb_vidq);
 	mutex_unlock(&dev->lock);
 	dprintk(1, "%s (dev=%s)\n", __func__, video_device_node_name(vdev));
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
 	kfree(fh);
 	return 0;
 }
@@ -1886,7 +1824,6 @@ static const struct v4l2_file_operations s2255_fops_v4l = {
 };
 
 static const struct v4l2_ioctl_ops s2255_ioctl_ops = {
-	.vidioc_querymenu = vidioc_querymenu,
 	.vidioc_querycap = vidioc_querycap,
 	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
@@ -1897,26 +1834,33 @@ static const struct v4l2_ioctl_ops s2255_ioctl_ops = {
 	.vidioc_qbuf = vidioc_qbuf,
 	.vidioc_dqbuf = vidioc_dqbuf,
 	.vidioc_s_std = vidioc_s_std,
+	.vidioc_g_std = vidioc_g_std,
 	.vidioc_enum_input = vidioc_enum_input,
 	.vidioc_g_input = vidioc_g_input,
 	.vidioc_s_input = vidioc_s_input,
-	.vidioc_queryctrl = vidioc_queryctrl,
-	.vidioc_g_ctrl = vidioc_g_ctrl,
-	.vidioc_s_ctrl = vidioc_s_ctrl,
 	.vidioc_streamon = vidioc_streamon,
 	.vidioc_streamoff = vidioc_streamoff,
 	.vidioc_s_jpegcomp = vidioc_s_jpegcomp,
 	.vidioc_g_jpegcomp = vidioc_g_jpegcomp,
 	.vidioc_s_parm = vidioc_s_parm,
 	.vidioc_g_parm = vidioc_g_parm,
+	.vidioc_enum_framesizes = vidioc_enum_framesizes,
 	.vidioc_enum_frameintervals = vidioc_enum_frameintervals,
+	.vidioc_log_status  = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static void s2255_video_device_release(struct video_device *vdev)
 {
 	struct s2255_dev *dev = to_s2255_dev(vdev->v4l2_dev);
-	dprintk(4, "%s, chnls: %d \n", __func__,
+	struct s2255_channel *channel =
+		container_of(vdev, struct s2255_channel, vdev);
+
+	v4l2_ctrl_handler_free(&channel->hdl);
+	dprintk(4, "%s, chnls: %d\n", __func__,
 		atomic_read(&dev->num_channels));
+
 	if (atomic_dec_and_test(&dev->num_channels))
 		s2255_destroy(dev);
 	return;
@@ -1928,7 +1872,20 @@ static struct video_device template = {
 	.ioctl_ops = &s2255_ioctl_ops,
 	.release = s2255_video_device_release,
 	.tvnorms = S2255_NORMS,
-	.current_norm = V4L2_STD_NTSC_M,
+};
+
+static const struct v4l2_ctrl_ops s2255_ctrl_ops = {
+	.s_ctrl = s2255_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config color_filter_ctrl = {
+	.ops = &s2255_ctrl_ops,
+	.name = "Color Filter",
+	.id = V4L2_CID_S2255_COLORFILTER,
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+	.def = 1,
 };
 
 static int s2255_probe_v4l(struct s2255_dev *dev)
@@ -1945,11 +1902,36 @@ static int s2255_probe_v4l(struct s2255_dev *dev)
 	for (i = 0; i < MAX_CHANNELS; i++) {
 		channel = &dev->channel[i];
 		INIT_LIST_HEAD(&channel->vidq.active);
+
+		v4l2_ctrl_handler_init(&channel->hdl, 6);
+		v4l2_ctrl_new_std(&channel->hdl, &s2255_ctrl_ops,
+				V4L2_CID_BRIGHTNESS, -127, 127, 1, DEF_BRIGHT);
+		v4l2_ctrl_new_std(&channel->hdl, &s2255_ctrl_ops,
+				V4L2_CID_CONTRAST, 0, 255, 1, DEF_CONTRAST);
+		v4l2_ctrl_new_std(&channel->hdl, &s2255_ctrl_ops,
+				V4L2_CID_SATURATION, 0, 255, 1, DEF_SATURATION);
+		v4l2_ctrl_new_std(&channel->hdl, &s2255_ctrl_ops,
+				V4L2_CID_HUE, 0, 255, 1, DEF_HUE);
+		channel->jpegqual_ctrl = v4l2_ctrl_new_std(&channel->hdl,
+				&s2255_ctrl_ops,
+				V4L2_CID_JPEG_COMPRESSION_QUALITY,
+				0, 100, 1, S2255_DEF_JPEG_QUAL);
+		if (dev->dsp_fw_ver >= S2255_MIN_DSP_COLORFILTER &&
+		    (dev->pid != 0x2257 || channel->idx <= 1))
+			v4l2_ctrl_new_custom(&channel->hdl, &color_filter_ctrl, NULL);
+		if (channel->hdl.error) {
+			ret = channel->hdl.error;
+			v4l2_ctrl_handler_free(&channel->hdl);
+			dev_err(&dev->udev->dev, "couldn't register control\n");
+			break;
+		}
 		channel->vidq.dev = dev;
 		/* register 4 video devices */
 		channel->vdev = template;
+		channel->vdev.ctrl_handler = &channel->hdl;
 		channel->vdev.lock = &dev->lock;
 		channel->vdev.v4l2_dev = &dev->v4l2_dev;
+		set_bit(V4L2_FL_USE_FH_PRIO, &channel->vdev.flags);
 		video_set_drvdata(&channel->vdev, channel);
 		if (video_nr == -1)
 			ret = video_register_device(&channel->vdev,
@@ -2300,9 +2282,10 @@ static int s2255_board_init(struct s2255_dev *dev)
 		channel->mode = mode_def;
 		if (dev->pid == 0x2257 && j > 1)
 			channel->mode.color |= (1 << 16);
-		channel->jc.quality = S2255_DEF_JPEG_QUAL;
+		channel->jpegqual = S2255_DEF_JPEG_QUAL;
 		channel->width = LINE_SZ_4CIFS_NTSC;
 		channel->height = NUM_LINES_4CIFS_NTSC * 2;
+		channel->std = V4L2_STD_NTSC_M;
 		channel->fmt = &formats[0];
 		channel->mode.restart = 1;
 		channel->req_image_size = get_transfer_size(&mode_def);
@@ -2531,7 +2514,7 @@ static int s2255_probe(struct usb_interface *interface,
 		return -ENOMEM;
 	}
 	atomic_set(&dev->num_channels, 0);
-	dev->pid = id->idProduct;
+	dev->pid = le16_to_cpu(id->idProduct);
 	dev->fw_data = kzalloc(sizeof(struct s2255_fw), GFP_KERNEL);
 	if (!dev->fw_data)
 		goto errorFWDATA1;
@@ -2601,7 +2584,7 @@ static int s2255_probe(struct usb_interface *interface,
 		/* make sure firmware is the latest */
 		__le32 *pRel;
 		pRel = (__le32 *) &dev->fw_data->fw->data[fw_size - 4];
-		printk(KERN_INFO "s2255 dsp fw version %x\n", *pRel);
+		printk(KERN_INFO "s2255 dsp fw version %x\n", le32_to_cpu(*pRel));
 		dev->dsp_fw_ver = le32_to_cpu(*pRel);
 		if (dev->dsp_fw_ver < S2255_CUR_DSP_FWVER)
 			printk(KERN_INFO "s2255: f2255usb.bin out of date.\n");
diff --git a/drivers/media/usb/siano/smsusb.c b/drivers/media/usb/siano/smsusb.c
index de2c10289eec..03761c6f472f 100644
--- a/drivers/media/usb/siano/smsusb.c
+++ b/drivers/media/usb/siano/smsusb.c
@@ -35,16 +35,23 @@ module_param_named(debug, sms_dbg, int, 0644);
 MODULE_PARM_DESC(debug, "set debug level (info=1, adv=2 (or-able))");
 
 #define USB1_BUFFER_SIZE		0x1000
-#define USB2_BUFFER_SIZE		0x4000
+#define USB2_BUFFER_SIZE		0x2000
 
 #define MAX_BUFFERS		50
 #define MAX_URBS		10
 
 struct smsusb_device_t;
 
+enum smsusb_state {
+	SMSUSB_DISCONNECTED,
+	SMSUSB_SUSPENDED,
+	SMSUSB_ACTIVE
+};
+
 struct smsusb_urb_t {
+	struct list_head entry;
 	struct smscore_buffer_t *cb;
-	struct smsusb_device_t	*dev;
+	struct smsusb_device_t *dev;
 
 	struct urb urb;
 };
@@ -57,11 +64,23 @@ struct smsusb_device_t {
 
 	int		response_alignment;
 	int		buffer_size;
+
+	unsigned char in_ep;
+	unsigned char out_ep;
+	enum smsusb_state state;
 };
 
 static int smsusb_submit_urb(struct smsusb_device_t *dev,
 			     struct smsusb_urb_t *surb);
 
+/**
+ * Completing URB's callback handler - top half (interrupt context)
+ * adds completing sms urb to the global surbs list and activtes the worker
+ * thread the surb
+ * IMPORTANT - blocking functions must not be called from here !!!
+
+ * @param urb pointer to a completing urb object
+ */
 static void smsusb_onresponse(struct urb *urb)
 {
 	struct smsusb_urb_t *surb = (struct smsusb_urb_t *) urb->context;
@@ -74,26 +93,26 @@ static void smsusb_onresponse(struct urb *urb)
 	}
 
 	if ((urb->actual_length > 0) && (urb->status == 0)) {
-		struct SmsMsgHdr_ST *phdr = (struct SmsMsgHdr_ST *)surb->cb->p;
+		struct sms_msg_hdr *phdr = (struct sms_msg_hdr *)surb->cb->p;
 
 		smsendian_handle_message_header(phdr);
-		if (urb->actual_length >= phdr->msgLength) {
-			surb->cb->size = phdr->msgLength;
+		if (urb->actual_length >= phdr->msg_length) {
+			surb->cb->size = phdr->msg_length;
 
 			if (dev->response_alignment &&
-			    (phdr->msgFlags & MSG_HDR_FLAG_SPLIT_MSG)) {
+			    (phdr->msg_flags & MSG_HDR_FLAG_SPLIT_MSG)) {
 
 				surb->cb->offset =
 					dev->response_alignment +
-					((phdr->msgFlags >> 8) & 3);
+					((phdr->msg_flags >> 8) & 3);
 
 				/* sanity check */
-				if (((int) phdr->msgLength +
+				if (((int) phdr->msg_length +
 				     surb->cb->offset) > urb->actual_length) {
 					sms_err("invalid response "
 						"msglen %d offset %d "
 						"size %d",
-						phdr->msgLength,
+						phdr->msg_length,
 						surb->cb->offset,
 						urb->actual_length);
 					goto exit_and_resubmit;
@@ -102,16 +121,22 @@ static void smsusb_onresponse(struct urb *urb)
 				/* move buffer pointer and
 				 * copy header to its new location */
 				memcpy((char *) phdr + surb->cb->offset,
-				       phdr, sizeof(struct SmsMsgHdr_ST));
+				       phdr, sizeof(struct sms_msg_hdr));
 			} else
 				surb->cb->offset = 0;
 
+			sms_debug("received %s(%d) size: %d",
+				  smscore_translate_msg(phdr->msg_type),
+				  phdr->msg_type, phdr->msg_length);
+
+			smsendian_handle_rx_message((struct sms_msg_data *) phdr);
+
 			smscore_onresponse(dev->coredev, surb->cb);
 			surb->cb = NULL;
 		} else {
 			sms_err("invalid response "
 				"msglen %d actual %d",
-				phdr->msgLength, urb->actual_length);
+				phdr->msg_length, urb->actual_length);
 		}
 	} else
 		sms_err("error, urb status %d, %d bytes",
@@ -136,7 +161,7 @@ static int smsusb_submit_urb(struct smsusb_device_t *dev,
 	usb_fill_bulk_urb(
 		&surb->urb,
 		dev->udev,
-		usb_rcvbulkpipe(dev->udev, 0x81),
+		usb_rcvbulkpipe(dev->udev, dev->in_ep),
 		surb->cb->p,
 		dev->buffer_size,
 		smsusb_onresponse,
@@ -181,9 +206,18 @@ static int smsusb_start_streaming(struct smsusb_device_t *dev)
 static int smsusb_sendrequest(void *context, void *buffer, size_t size)
 {
 	struct smsusb_device_t *dev = (struct smsusb_device_t *) context;
+	struct sms_msg_hdr *phdr = (struct sms_msg_hdr *) buffer;
 	int dummy;
 
-	smsendian_handle_message_header((struct SmsMsgHdr_ST *)buffer);
+	if (dev->state != SMSUSB_ACTIVE)
+		return -ENOENT;
+
+	sms_debug("sending %s(%d) size: %d",
+		  smscore_translate_msg(phdr->msg_type), phdr->msg_type,
+		  phdr->msg_length);
+
+	smsendian_handle_tx_message((struct sms_msg_data *) phdr);
+	smsendian_handle_message_header((struct sms_msg_hdr *)buffer);
 	return usb_bulk_msg(dev->udev, usb_sndbulkpipe(dev->udev, 2),
 			    buffer, size, &dummy, 1000);
 }
@@ -276,15 +310,15 @@ static void smsusb1_detectmode(void *context, int *mode)
 
 static int smsusb1_setmode(void *context, int mode)
 {
-	struct SmsMsgHdr_ST Msg = { MSG_SW_RELOAD_REQ, 0, HIF_TASK,
-			     sizeof(struct SmsMsgHdr_ST), 0 };
+	struct sms_msg_hdr msg = { MSG_SW_RELOAD_REQ, 0, HIF_TASK,
+			     sizeof(struct sms_msg_hdr), 0 };
 
 	if (mode < DEVICE_MODE_DVBT || mode > DEVICE_MODE_DVBT_BDA) {
 		sms_err("invalid firmware id specified %d", mode);
 		return -EINVAL;
 	}
 
-	return smsusb_sendrequest(context, &Msg, sizeof(Msg));
+	return smsusb_sendrequest(context, &msg, sizeof(msg));
 }
 
 static void smsusb_term_device(struct usb_interface *intf)
@@ -292,13 +326,15 @@ static void smsusb_term_device(struct usb_interface *intf)
 	struct smsusb_device_t *dev = usb_get_intfdata(intf);
 
 	if (dev) {
+		dev->state = SMSUSB_DISCONNECTED;
+
 		smsusb_stop_streaming(dev);
 
 		/* unregister from smscore */
 		if (dev->coredev)
 			smscore_unregister_device(dev->coredev);
 
-		sms_info("device %p destroyed", dev);
+		sms_info("device 0x%p destroyed", dev);
 		kfree(dev);
 	}
 
@@ -321,6 +357,7 @@ static int smsusb_init_device(struct usb_interface *intf, int board_id)
 	memset(&params, 0, sizeof(params));
 	usb_set_intfdata(intf, dev);
 	dev->udev = interface_to_usbdev(intf);
+	dev->state = SMSUSB_DISCONNECTED;
 
 	params.device_type = sms_get_board(board_id)->type;
 
@@ -331,21 +368,29 @@ static int smsusb_init_device(struct usb_interface *intf, int board_id)
 		params.setmode_handler = smsusb1_setmode;
 		params.detectmode_handler = smsusb1_detectmode;
 		break;
-	default:
+	case SMS_UNKNOWN_TYPE:
 		sms_err("Unspecified sms device type!");
 		/* fall-thru */
-	case SMS_NOVA_A0:
-	case SMS_NOVA_B0:
-	case SMS_VEGA:
+	default:
 		dev->buffer_size = USB2_BUFFER_SIZE;
 		dev->response_alignment =
 		    le16_to_cpu(dev->udev->ep_in[1]->desc.wMaxPacketSize) -
-		    sizeof(struct SmsMsgHdr_ST);
+		    sizeof(struct sms_msg_hdr);
 
 		params.flags |= SMS_DEVICE_FAMILY2;
 		break;
 	}
 
+	for (i = 0; i < intf->cur_altsetting->desc.bNumEndpoints; i++) {
+		if (intf->cur_altsetting->endpoint[i].desc. bEndpointAddress & USB_DIR_IN)
+			dev->in_ep = intf->cur_altsetting->endpoint[i].desc.bEndpointAddress;
+		else
+			dev->out_ep = intf->cur_altsetting->endpoint[i].desc.bEndpointAddress;
+	}
+
+	sms_info("in_ep = %02x, out_ep = %02x",
+		dev->in_ep, dev->out_ep);
+
 	params.device = &dev->udev->dev;
 	params.buffer_size = dev->buffer_size;
 	params.num_buffers = MAX_BUFFERS;
@@ -363,6 +408,8 @@ static int smsusb_init_device(struct usb_interface *intf, int board_id)
 
 	smscore_set_board_id(dev->coredev, board_id);
 
+	dev->coredev->is_usb_device = true;
+
 	/* initialize urbs */
 	for (i = 0; i < MAX_URBS; i++) {
 		dev->surbs[i].dev = dev;
@@ -377,6 +424,8 @@ static int smsusb_init_device(struct usb_interface *intf, int board_id)
 		return rc;
 	}
 
+	dev->state = SMSUSB_ACTIVE;
+
 	rc = smscore_start_device(dev->coredev);
 	if (rc < 0) {
 		sms_err("smscore_start_device(...) failed");
@@ -384,7 +433,7 @@ static int smsusb_init_device(struct usb_interface *intf, int board_id)
 		return rc;
 	}
 
-	sms_info("device %p created", dev);
+	sms_info("device 0x%p created", dev);
 
 	return rc;
 }
@@ -396,12 +445,21 @@ static int smsusb_probe(struct usb_interface *intf,
 	char devpath[32];
 	int i, rc;
 
-	rc = usb_clear_halt(udev, usb_rcvbulkpipe(udev, 0x81));
-	rc = usb_clear_halt(udev, usb_rcvbulkpipe(udev, 0x02));
+	sms_info("interface number %d",
+		 intf->cur_altsetting->desc.bInterfaceNumber);
 
-	if (intf->num_altsetting > 0) {
-		rc = usb_set_interface(
-			udev, intf->cur_altsetting->desc.bInterfaceNumber, 0);
+	if (sms_get_board(id->driver_info)->intf_num !=
+	    intf->cur_altsetting->desc.bInterfaceNumber) {
+		sms_err("interface number is %d expecting %d",
+			sms_get_board(id->driver_info)->intf_num,
+			intf->cur_altsetting->desc.bInterfaceNumber);
+		return -ENODEV;
+	}
+
+	if (intf->num_altsetting > 1) {
+		rc = usb_set_interface(udev,
+				       intf->cur_altsetting->desc.bInterfaceNumber,
+				       0);
 		if (rc < 0) {
 			sms_err("usb_set_interface failed, rc %d", rc);
 			return rc;
@@ -410,19 +468,27 @@ static int smsusb_probe(struct usb_interface *intf,
 
 	sms_info("smsusb_probe %d",
 	       intf->cur_altsetting->desc.bInterfaceNumber);
-	for (i = 0; i < intf->cur_altsetting->desc.bNumEndpoints; i++)
+	for (i = 0; i < intf->cur_altsetting->desc.bNumEndpoints; i++) {
 		sms_info("endpoint %d %02x %02x %d", i,
 		       intf->cur_altsetting->endpoint[i].desc.bEndpointAddress,
 		       intf->cur_altsetting->endpoint[i].desc.bmAttributes,
 		       intf->cur_altsetting->endpoint[i].desc.wMaxPacketSize);
-
+		if (intf->cur_altsetting->endpoint[i].desc.bEndpointAddress &
+		    USB_DIR_IN)
+			rc = usb_clear_halt(udev, usb_rcvbulkpipe(udev,
+				intf->cur_altsetting->endpoint[i].desc.bEndpointAddress));
+		else
+			rc = usb_clear_halt(udev, usb_sndbulkpipe(udev,
+				intf->cur_altsetting->endpoint[i].desc.bEndpointAddress));
+	}
 	if ((udev->actconfig->desc.bNumInterfaces == 2) &&
 	    (intf->cur_altsetting->desc.bInterfaceNumber == 0)) {
 		sms_err("rom interface 0 is not used");
 		return -ENODEV;
 	}
 
-	if (intf->cur_altsetting->desc.bInterfaceNumber == 1) {
+	if (id->driver_info == SMS1XXX_BOARD_SIANO_STELLAR_ROM) {
+		sms_info("stellar device was found.");
 		snprintf(devpath, sizeof(devpath), "usb\\%d-%s",
 			 udev->bus->busnum, udev->devpath);
 		sms_info("stellar device was found.");
@@ -445,7 +511,9 @@ static void smsusb_disconnect(struct usb_interface *intf)
 static int smsusb_suspend(struct usb_interface *intf, pm_message_t msg)
 {
 	struct smsusb_device_t *dev = usb_get_intfdata(intf);
-	printk(KERN_INFO "%s: Entering status %d.\n", __func__, msg.event);
+	printk(KERN_INFO "%s  Entering status %d.\n", __func__, msg.event);
+	dev->state = SMSUSB_SUSPENDED;
+	/*smscore_set_power_mode(dev, SMS_POWER_MODE_SUSPENDED);*/
 	smsusb_stop_streaming(dev);
 	return 0;
 }
@@ -456,9 +524,9 @@ static int smsusb_resume(struct usb_interface *intf)
 	struct smsusb_device_t *dev = usb_get_intfdata(intf);
 	struct usb_device *udev = interface_to_usbdev(intf);
 
-	printk(KERN_INFO "%s: Entering.\n", __func__);
-	usb_clear_halt(udev, usb_rcvbulkpipe(udev, 0x81));
-	usb_clear_halt(udev, usb_rcvbulkpipe(udev, 0x02));
+	printk(KERN_INFO "%s  Entering.\n", __func__);
+	usb_clear_halt(udev, usb_rcvbulkpipe(udev, dev->in_ep));
+	usb_clear_halt(udev, usb_sndbulkpipe(udev, dev->out_ep));
 
 	for (i = 0; i < intf->cur_altsetting->desc.bNumEndpoints; i++)
 		printk(KERN_INFO "endpoint %d %02x %02x %d\n", i,
@@ -546,6 +614,26 @@ static const struct usb_device_id smsusb_id_table[] = {
 		.driver_info = SMS1XXX_BOARD_HAUPPAUGE_WINDHAM },
 	{ USB_DEVICE(0x2040, 0xf5a0),
 		.driver_info = SMS1XXX_BOARD_HAUPPAUGE_WINDHAM },
+	{ USB_DEVICE(0x187f, 0x0202),
+		.driver_info = SMS1XXX_BOARD_SIANO_NICE },
+	{ USB_DEVICE(0x187f, 0x0301),
+		.driver_info = SMS1XXX_BOARD_SIANO_VENICE },
+	{ USB_DEVICE(0x187f, 0x0302),
+		.driver_info = SMS1XXX_BOARD_SIANO_VENICE },
+	{ USB_DEVICE(0x187f, 0x0310),
+		.driver_info = SMS1XXX_BOARD_SIANO_MING },
+	{ USB_DEVICE(0x187f, 0x0500),
+		.driver_info = SMS1XXX_BOARD_SIANO_PELE },
+	{ USB_DEVICE(0x187f, 0x0600),
+		.driver_info = SMS1XXX_BOARD_SIANO_RIO },
+	{ USB_DEVICE(0x187f, 0x0700),
+		.driver_info = SMS1XXX_BOARD_SIANO_DENVER_2160 },
+	{ USB_DEVICE(0x187f, 0x0800),
+		.driver_info = SMS1XXX_BOARD_SIANO_DENVER_1530 },
+	{ USB_DEVICE(0x19D2, 0x0086),
+		.driver_info = SMS1XXX_BOARD_ZTE_DVB_DATA_CARD },
+	{ USB_DEVICE(0x19D2, 0x0078),
+		.driver_info = SMS1XXX_BOARD_ONDA_MDTV_DATA_CARD },
 	{ } /* Terminating entry */
 	};
 
diff --git a/drivers/media/usb/stk1160/stk1160-v4l.c b/drivers/media/usb/stk1160/stk1160-v4l.c
index 6694f9e2ca57..a59153d2f8bf 100644
--- a/drivers/media/usb/stk1160/stk1160-v4l.c
+++ b/drivers/media/usb/stk1160/stk1160-v4l.c
@@ -375,7 +375,7 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *norm)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *norm)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 {
 	struct stk1160 *dev = video_drvdata(file);
 	struct vb2_queue *q = &dev->vb_vidq;
@@ -388,7 +388,7 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *norm)
 		return -ENODEV;
 
 	/* We need to set this now, before we call stk1160_set_std */
-	dev->norm = *norm;
+	dev->norm = norm;
 
 	/* This is taken from saa7115 video decoder */
 	if (dev->norm & V4L2_STD_525_60) {
@@ -458,7 +458,7 @@ static int vidioc_g_chip_ident(struct file *file, void *priv,
 	       struct v4l2_dbg_chip_ident *chip)
 {
 	switch (chip->match.type) {
-	case V4L2_CHIP_MATCH_HOST:
+	case V4L2_CHIP_MATCH_BRIDGE:
 		chip->ident = V4L2_IDENT_NONE;
 		chip->revision = 0;
 		return 0;
@@ -476,9 +476,6 @@ static int vidioc_g_register(struct file *file, void *priv,
 	u8 val;
 
 	switch (reg->match.type) {
-	case V4L2_CHIP_MATCH_AC97:
-		/* TODO: Support me please :-( */
-		return -EINVAL;
 	case V4L2_CHIP_MATCH_I2C_DRIVER:
 		v4l2_device_call_all(&dev->v4l2_dev, 0, core, g_register, reg);
 		return 0;
@@ -500,13 +497,11 @@ static int vidioc_g_register(struct file *file, void *priv,
 }
 
 static int vidioc_s_register(struct file *file, void *priv,
-			     struct v4l2_dbg_register *reg)
+			     const struct v4l2_dbg_register *reg)
 {
 	struct stk1160 *dev = video_drvdata(file);
 
 	switch (reg->match.type) {
-	case V4L2_CHIP_MATCH_AC97:
-		return -EINVAL;
 	case V4L2_CHIP_MATCH_I2C_DRIVER:
 		v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_register, reg);
 		return 0;
@@ -687,6 +682,7 @@ int stk1160_vb2_setup(struct stk1160 *dev)
 	q->buf_struct_size = sizeof(struct stk1160_buffer);
 	q->ops = &stk1160_video_qops;
 	q->mem_ops = &vb2_vmalloc_memops;
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	rc = vb2_queue_init(q);
 	if (rc < 0)
diff --git a/drivers/media/usb/stkwebcam/stk-webcam.c b/drivers/media/usb/stkwebcam/stk-webcam.c
index 4cbab085e348..c43c8d32be40 100644
--- a/drivers/media/usb/stkwebcam/stk-webcam.c
+++ b/drivers/media/usb/stkwebcam/stk-webcam.c
@@ -35,6 +35,7 @@
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
 
 #include "stk-webcam.h"
 
@@ -63,7 +64,39 @@ static struct usb_device_id stkwebcam_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, stkwebcam_table);
 
-/* The stk webcam laptop module is mounted upside down in some laptops :( */
+/*
+ * The stk webcam laptop module is mounted upside down in some laptops :(
+ *
+ * Some background information (thanks to Hans de Goede for providing this):
+ *
+ * 1) Once upon a time the stkwebcam driver was written
+ *
+ * 2) The webcam in question was used mostly in Asus laptop models, including
+ * the laptop of the original author of the driver, and in these models, in
+ * typical Asus fashion (see the long long list for uvc cams inside v4l-utils),
+ * they mounted the webcam-module the wrong way up. So the hflip and vflip
+ * module options were given a default value of 1 (the correct value for
+ * upside down mounted models)
+ *
+ * 3) Years later I got a bug report from a user with a laptop with stkwebcam,
+ * where the module was actually mounted the right way up, and thus showed
+ * upside down under Linux. So now I was facing the choice of 2 options:
+ *
+ * a) Add a not-upside-down list to stkwebcam, which overrules the default.
+ *
+ * b) Do it like all the other drivers do, and make the default right for
+ *    cams mounted the proper way and add an upside-down model list, with
+ *    models where we need to flip-by-default.
+ *
+ * Despite knowing that going b) would cause a period of pain where we were
+ * building the table I opted to go for option b), since a) is just too ugly,
+ * and worse different from how every other driver does it leading to
+ * confusion in the long run. This change was made in kernel 3.6.
+ *
+ * So for any user report about upside-down images since kernel 3.6 ask them
+ * to provide the output of 'sudo dmidecode' so the laptop can be added in
+ * the table below.
+ */
 static const struct dmi_system_id stk_upside_down_dmi_table[] = {
 	{
 		.ident = "ASUS G1",
@@ -71,6 +104,12 @@ static const struct dmi_system_id stk_upside_down_dmi_table[] = {
 			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK Computer Inc."),
 			DMI_MATCH(DMI_PRODUCT_NAME, "G1")
 		}
+	}, {
+		.ident = "ASUS F3JC",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK Computer Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "F3JC")
+		}
 	},
 	{}
 };
@@ -565,31 +604,31 @@ static void stk_free_buffers(struct stk_camera *dev)
 
 static int v4l_stk_open(struct file *fp)
 {
-	static int first_init = 1; /* webcam LED management */
-	struct stk_camera *dev;
-	struct video_device *vdev;
-
-	vdev = video_devdata(fp);
-	dev = vdev_to_camera(vdev);
+	struct stk_camera *dev = video_drvdata(fp);
+	int err;
 
 	if (dev == NULL || !is_present(dev))
 		return -ENXIO;
 
-	if (!first_init)
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	if (!dev->first_init)
 		stk_camera_write_reg(dev, 0x0, 0x24);
 	else
-		first_init = 0;
-
-	fp->private_data = dev;
-	usb_autopm_get_interface(dev->interface);
+		dev->first_init = 0;
 
-	return 0;
+	err = v4l2_fh_open(fp);
+	if (!err)
+		usb_autopm_get_interface(dev->interface);
+	mutex_unlock(&dev->lock);
+	return err;
 }
 
 static int v4l_stk_release(struct file *fp)
 {
-	struct stk_camera *dev = fp->private_data;
+	struct stk_camera *dev = video_drvdata(fp);
 
+	mutex_lock(&dev->lock);
 	if (dev->owner == fp) {
 		stk_stop_stream(dev);
 		stk_free_buffers(dev);
@@ -600,22 +639,22 @@ static int v4l_stk_release(struct file *fp)
 
 	if (is_present(dev))
 		usb_autopm_put_interface(dev->interface);
-
-	return 0;
+	mutex_unlock(&dev->lock);
+	return v4l2_fh_release(fp);
 }
 
-static ssize_t v4l_stk_read(struct file *fp, char __user *buf,
+static ssize_t stk_read(struct file *fp, char __user *buf,
 		size_t count, loff_t *f_pos)
 {
 	int i;
 	int ret;
 	unsigned long flags;
 	struct stk_sio_buffer *sbuf;
-	struct stk_camera *dev = fp->private_data;
+	struct stk_camera *dev = video_drvdata(fp);
 
 	if (!is_present(dev))
 		return -EIO;
-	if (dev->owner && dev->owner != fp)
+	if (dev->owner && (!dev->reading || dev->owner != fp))
 		return -EBUSY;
 	dev->owner = fp;
 	if (!is_streaming(dev)) {
@@ -623,6 +662,7 @@ static ssize_t v4l_stk_read(struct file *fp, char __user *buf,
 			|| stk_allocate_buffers(dev, 3)
 			|| stk_start_stream(dev))
 			return -ENOMEM;
+		dev->reading = 1;
 		spin_lock_irqsave(&dev->spinlock, flags);
 		for (i = 0; i < dev->n_sbufs; i++) {
 			list_add_tail(&dev->sio_bufs[i].list, &dev->sio_avail);
@@ -665,9 +705,23 @@ static ssize_t v4l_stk_read(struct file *fp, char __user *buf,
 	return count;
 }
 
+static ssize_t v4l_stk_read(struct file *fp, char __user *buf,
+		size_t count, loff_t *f_pos)
+{
+	struct stk_camera *dev = video_drvdata(fp);
+	int ret;
+
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	ret = stk_read(fp, buf, count, f_pos);
+	mutex_unlock(&dev->lock);
+	return ret;
+}
+
 static unsigned int v4l_stk_poll(struct file *fp, poll_table *wait)
 {
-	struct stk_camera *dev = fp->private_data;
+	struct stk_camera *dev = video_drvdata(fp);
+	unsigned res = v4l2_ctrl_poll(fp, wait);
 
 	poll_wait(fp, &dev->wait_frame, wait);
 
@@ -675,9 +729,9 @@ static unsigned int v4l_stk_poll(struct file *fp, poll_table *wait)
 		return POLLERR;
 
 	if (!list_empty(&dev->sio_full))
-		return POLLIN | POLLRDNORM;
+		return res | POLLIN | POLLRDNORM;
 
-	return 0;
+	return res;
 }
 
 
@@ -703,7 +757,7 @@ static int v4l_stk_mmap(struct file *fp, struct vm_area_struct *vma)
 	unsigned int i;
 	int ret;
 	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
-	struct stk_camera *dev = fp->private_data;
+	struct stk_camera *dev = video_drvdata(fp);
 	struct stk_sio_buffer *sbuf = NULL;
 
 	if (!(vma->vm_flags & VM_WRITE) || !(vma->vm_flags & VM_SHARED))
@@ -733,12 +787,15 @@ static int v4l_stk_mmap(struct file *fp, struct vm_area_struct *vma)
 static int stk_vidioc_querycap(struct file *filp,
 		void *priv, struct v4l2_capability *cap)
 {
+	struct stk_camera *dev = video_drvdata(filp);
+
 	strcpy(cap->driver, "stk");
 	strcpy(cap->card, "stk");
-	cap->version = DRIVER_VERSION_NUM;
+	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
 
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE
 		| V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -762,111 +819,28 @@ static int stk_vidioc_g_input(struct file *filp, void *priv, unsigned int *i)
 
 static int stk_vidioc_s_input(struct file *filp, void *priv, unsigned int i)
 {
-	if (i != 0)
-		return -EINVAL;
-	else
-		return 0;
-}
-
-/* from vivi.c */
-static int stk_vidioc_s_std(struct file *filp, void *priv, v4l2_std_id *a)
-{
-	return 0;
-}
-
-/* List of all V4Lv2 controls supported by the driver */
-static struct v4l2_queryctrl stk_controls[] = {
-	{
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 0xffff,
-		.step    = 0x0100,
-		.default_value = 0x6000,
-	},
-	{
-		.id      = V4L2_CID_HFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Horizontal Flip",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 1,
-	},
-	{
-		.id      = V4L2_CID_VFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Vertical Flip",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 1,
-	},
-};
-
-static int stk_vidioc_queryctrl(struct file *filp,
-		void *priv, struct v4l2_queryctrl *c)
-{
-	int i;
-	int nbr;
-	nbr = ARRAY_SIZE(stk_controls);
-
-	for (i = 0; i < nbr; i++) {
-		if (stk_controls[i].id == c->id) {
-			memcpy(c, &stk_controls[i],
-				sizeof(struct v4l2_queryctrl));
-			return 0;
-		}
-	}
-	return -EINVAL;
+	return i ? -EINVAL : 0;
 }
 
-static int stk_vidioc_g_ctrl(struct file *filp,
-		void *priv, struct v4l2_control *c)
+static int stk_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct stk_camera *dev = priv;
-	switch (c->id) {
-	case V4L2_CID_BRIGHTNESS:
-		c->value = dev->vsettings.brightness;
-		break;
-	case V4L2_CID_HFLIP:
-		if (dmi_check_system(stk_upside_down_dmi_table))
-			c->value = !dev->vsettings.hflip;
-		else
-			c->value = dev->vsettings.hflip;
-		break;
-	case V4L2_CID_VFLIP:
-		if (dmi_check_system(stk_upside_down_dmi_table))
-			c->value = !dev->vsettings.vflip;
-		else
-			c->value = dev->vsettings.vflip;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
+	struct stk_camera *dev =
+		container_of(ctrl->handler, struct stk_camera, hdl);
 
-static int stk_vidioc_s_ctrl(struct file *filp,
-		void *priv, struct v4l2_control *c)
-{
-	struct stk_camera *dev = priv;
-	switch (c->id) {
+	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		dev->vsettings.brightness = c->value;
-		return stk_sensor_set_brightness(dev, c->value >> 8);
+		return stk_sensor_set_brightness(dev, ctrl->val);
 	case V4L2_CID_HFLIP:
 		if (dmi_check_system(stk_upside_down_dmi_table))
-			dev->vsettings.hflip = !c->value;
+			dev->vsettings.hflip = !ctrl->val;
 		else
-			dev->vsettings.hflip = c->value;
+			dev->vsettings.hflip = ctrl->val;
 		return 0;
 	case V4L2_CID_VFLIP:
 		if (dmi_check_system(stk_upside_down_dmi_table))
-			dev->vsettings.vflip = !c->value;
+			dev->vsettings.vflip = !ctrl->val;
 		else
-			dev->vsettings.vflip = c->value;
+			dev->vsettings.vflip = ctrl->val;
 		return 0;
 	default:
 		return -EINVAL;
@@ -921,7 +895,7 @@ static int stk_vidioc_g_fmt_vid_cap(struct file *filp,
 		void *priv, struct v4l2_format *f)
 {
 	struct v4l2_pix_format *pix_format = &f->fmt.pix;
-	struct stk_camera *dev = priv;
+	struct stk_camera *dev = video_drvdata(filp);
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(stk_sizes) &&
@@ -942,11 +916,12 @@ static int stk_vidioc_g_fmt_vid_cap(struct file *filp,
 		pix_format->bytesperline = 2 * pix_format->width;
 	pix_format->sizeimage = pix_format->bytesperline
 				* pix_format->height;
+	pix_format->priv = 0;
 	return 0;
 }
 
-static int stk_vidioc_try_fmt_vid_cap(struct file *filp,
-		void *priv, struct v4l2_format *fmtd)
+static int stk_try_fmt_vid_cap(struct file *filp,
+		struct v4l2_format *fmtd, int *idx)
 {
 	int i;
 	switch (fmtd->fmt.pix.pixelformat) {
@@ -968,11 +943,13 @@ static int stk_vidioc_try_fmt_vid_cap(struct file *filp,
 			< abs(fmtd->fmt.pix.width - stk_sizes[i].w))) {
 		fmtd->fmt.pix.height = stk_sizes[i-1].h;
 		fmtd->fmt.pix.width = stk_sizes[i-1].w;
-		fmtd->fmt.pix.priv = i - 1;
+		if (idx)
+			*idx = i - 1;
 	} else {
 		fmtd->fmt.pix.height = stk_sizes[i].h;
 		fmtd->fmt.pix.width = stk_sizes[i].w;
-		fmtd->fmt.pix.priv = i;
+		if (idx)
+			*idx = i;
 	}
 
 	fmtd->fmt.pix.field = V4L2_FIELD_NONE;
@@ -983,9 +960,16 @@ static int stk_vidioc_try_fmt_vid_cap(struct file *filp,
 		fmtd->fmt.pix.bytesperline = 2 * fmtd->fmt.pix.width;
 	fmtd->fmt.pix.sizeimage = fmtd->fmt.pix.bytesperline
 		* fmtd->fmt.pix.height;
+	fmtd->fmt.pix.priv = 0;
 	return 0;
 }
 
+static int stk_vidioc_try_fmt_vid_cap(struct file *filp,
+		void *priv, struct v4l2_format *fmtd)
+{
+	return stk_try_fmt_vid_cap(filp, fmtd, NULL);
+}
+
 static int stk_setup_format(struct stk_camera *dev)
 {
 	int i = 0;
@@ -1026,7 +1010,8 @@ static int stk_vidioc_s_fmt_vid_cap(struct file *filp,
 		void *priv, struct v4l2_format *fmtd)
 {
 	int ret;
-	struct stk_camera *dev = priv;
+	int idx;
+	struct stk_camera *dev = video_drvdata(filp);
 
 	if (dev == NULL)
 		return -ENODEV;
@@ -1034,17 +1019,16 @@ static int stk_vidioc_s_fmt_vid_cap(struct file *filp,
 		return -ENODEV;
 	if (is_streaming(dev))
 		return -EBUSY;
-	if (dev->owner && dev->owner != filp)
+	if (dev->owner)
 		return -EBUSY;
-	ret = stk_vidioc_try_fmt_vid_cap(filp, priv, fmtd);
+	ret = stk_try_fmt_vid_cap(filp, fmtd, &idx);
 	if (ret)
 		return ret;
-	dev->owner = filp;
 
 	dev->vsettings.palette = fmtd->fmt.pix.pixelformat;
 	stk_free_buffers(dev);
 	dev->frame_size = fmtd->fmt.pix.sizeimage;
-	dev->vsettings.mode = stk_sizes[fmtd->fmt.pix.priv].m;
+	dev->vsettings.mode = stk_sizes[idx].m;
 
 	stk_initialise(dev);
 	return stk_setup_format(dev);
@@ -1053,7 +1037,7 @@ static int stk_vidioc_s_fmt_vid_cap(struct file *filp,
 static int stk_vidioc_reqbufs(struct file *filp,
 		void *priv, struct v4l2_requestbuffers *rb)
 {
-	struct stk_camera *dev = priv;
+	struct stk_camera *dev = video_drvdata(filp);
 
 	if (dev == NULL)
 		return -ENODEV;
@@ -1062,6 +1046,13 @@ static int stk_vidioc_reqbufs(struct file *filp,
 	if (is_streaming(dev)
 		|| (dev->owner && dev->owner != filp))
 		return -EBUSY;
+	stk_free_buffers(dev);
+	if (rb->count == 0) {
+		stk_camera_write_reg(dev, 0x0, 0x49); /* turn off the LED */
+		unset_initialised(dev);
+		dev->owner = NULL;
+		return 0;
+	}
 	dev->owner = filp;
 
 	/*FIXME If they ask for zero, we must stop streaming and free */
@@ -1079,7 +1070,7 @@ static int stk_vidioc_reqbufs(struct file *filp,
 static int stk_vidioc_querybuf(struct file *filp,
 		void *priv, struct v4l2_buffer *buf)
 {
-	struct stk_camera *dev = priv;
+	struct stk_camera *dev = video_drvdata(filp);
 	struct stk_sio_buffer *sbuf;
 
 	if (buf->index >= dev->n_sbufs)
@@ -1092,7 +1083,7 @@ static int stk_vidioc_querybuf(struct file *filp,
 static int stk_vidioc_qbuf(struct file *filp,
 		void *priv, struct v4l2_buffer *buf)
 {
-	struct stk_camera *dev = priv;
+	struct stk_camera *dev = video_drvdata(filp);
 	struct stk_sio_buffer *sbuf;
 	unsigned long flags;
 
@@ -1116,7 +1107,7 @@ static int stk_vidioc_qbuf(struct file *filp,
 static int stk_vidioc_dqbuf(struct file *filp,
 		void *priv, struct v4l2_buffer *buf)
 {
-	struct stk_camera *dev = priv;
+	struct stk_camera *dev = video_drvdata(filp);
 	struct stk_sio_buffer *sbuf;
 	unsigned long flags;
 	int ret;
@@ -1149,7 +1140,7 @@ static int stk_vidioc_dqbuf(struct file *filp,
 static int stk_vidioc_streamon(struct file *filp,
 		void *priv, enum v4l2_buf_type type)
 {
-	struct stk_camera *dev = priv;
+	struct stk_camera *dev = video_drvdata(filp);
 	if (is_streaming(dev))
 		return 0;
 	if (dev->sio_bufs == NULL)
@@ -1161,7 +1152,7 @@ static int stk_vidioc_streamon(struct file *filp,
 static int stk_vidioc_streamoff(struct file *filp,
 		void *priv, enum v4l2_buf_type type)
 {
-	struct stk_camera *dev = priv;
+	struct stk_camera *dev = video_drvdata(filp);
 	unsigned long flags;
 	int i;
 	stk_stop_stream(dev);
@@ -1206,6 +1197,10 @@ static int stk_vidioc_enum_framesizes(struct file *filp,
 	}
 }
 
+static const struct v4l2_ctrl_ops stk_ctrl_ops = {
+	.s_ctrl = stk_s_ctrl,
+};
+
 static struct v4l2_file_operations v4l_stk_fops = {
 	.owner = THIS_MODULE,
 	.open = v4l_stk_open,
@@ -1213,7 +1208,7 @@ static struct v4l2_file_operations v4l_stk_fops = {
 	.read = v4l_stk_read,
 	.poll = v4l_stk_poll,
 	.mmap = v4l_stk_mmap,
-	.ioctl = video_ioctl2,
+	.unlocked_ioctl = video_ioctl2,
 };
 
 static const struct v4l2_ioctl_ops v4l_stk_ioctl_ops = {
@@ -1225,18 +1220,17 @@ static const struct v4l2_ioctl_ops v4l_stk_ioctl_ops = {
 	.vidioc_enum_input = stk_vidioc_enum_input,
 	.vidioc_s_input = stk_vidioc_s_input,
 	.vidioc_g_input = stk_vidioc_g_input,
-	.vidioc_s_std = stk_vidioc_s_std,
 	.vidioc_reqbufs = stk_vidioc_reqbufs,
 	.vidioc_querybuf = stk_vidioc_querybuf,
 	.vidioc_qbuf = stk_vidioc_qbuf,
 	.vidioc_dqbuf = stk_vidioc_dqbuf,
 	.vidioc_streamon = stk_vidioc_streamon,
 	.vidioc_streamoff = stk_vidioc_streamoff,
-	.vidioc_queryctrl = stk_vidioc_queryctrl,
-	.vidioc_g_ctrl = stk_vidioc_g_ctrl,
-	.vidioc_s_ctrl = stk_vidioc_s_ctrl,
 	.vidioc_g_parm = stk_vidioc_g_parm,
 	.vidioc_enum_framesizes = stk_vidioc_enum_framesizes,
+	.vidioc_log_status = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static void stk_v4l_dev_release(struct video_device *vd)
@@ -1251,8 +1245,6 @@ static void stk_v4l_dev_release(struct video_device *vd)
 
 static struct video_device stk_v4l_data = {
 	.name = "stkwebcam",
-	.tvnorms = V4L2_STD_UNKNOWN,
-	.current_norm = V4L2_STD_UNKNOWN,
 	.fops = &v4l_stk_fops,
 	.ioctl_ops = &v4l_stk_ioctl_ops,
 	.release = stk_v4l_dev_release,
@@ -1264,8 +1256,11 @@ static int stk_register_video_device(struct stk_camera *dev)
 	int err;
 
 	dev->vdev = stk_v4l_data;
+	dev->vdev.lock = &dev->lock;
 	dev->vdev.debug = debug;
-	dev->vdev.parent = &dev->interface->dev;
+	dev->vdev.v4l2_dev = &dev->v4l2_dev;
+	set_bit(V4L2_FL_USE_FH_PRIO, &dev->vdev.flags);
+	video_set_drvdata(&dev->vdev, dev);
 	err = video_register_device(&dev->vdev, VFL_TYPE_GRABBER, -1);
 	if (err)
 		STK_ERROR("v4l registration failed\n");
@@ -1281,8 +1276,9 @@ static int stk_register_video_device(struct stk_camera *dev)
 static int stk_camera_probe(struct usb_interface *interface,
 		const struct usb_device_id *id)
 {
-	int i;
+	struct v4l2_ctrl_handler *hdl;
 	int err = 0;
+	int i;
 
 	struct stk_camera *dev = NULL;
 	struct usb_device *udev = interface_to_usbdev(interface);
@@ -1294,9 +1290,31 @@ static int stk_camera_probe(struct usb_interface *interface,
 		STK_ERROR("Out of memory !\n");
 		return -ENOMEM;
 	}
+	err = v4l2_device_register(&interface->dev, &dev->v4l2_dev);
+	if (err < 0) {
+		dev_err(&udev->dev, "couldn't register v4l2_device\n");
+		kfree(dev);
+		return err;
+	}
+	hdl = &dev->hdl;
+	v4l2_ctrl_handler_init(hdl, 3);
+	v4l2_ctrl_new_std(hdl, &stk_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, 0, 0xff, 0x1, 0x60);
+	v4l2_ctrl_new_std(hdl, &stk_ctrl_ops,
+			  V4L2_CID_HFLIP, 0, 1, 1, 1);
+	v4l2_ctrl_new_std(hdl, &stk_ctrl_ops,
+			  V4L2_CID_VFLIP, 0, 1, 1, 1);
+	if (hdl->error) {
+		err = hdl->error;
+		dev_err(&udev->dev, "couldn't register control\n");
+		goto error;
+	}
+	dev->v4l2_dev.ctrl_handler = hdl;
 
 	spin_lock_init(&dev->spinlock);
+	mutex_init(&dev->lock);
 	init_waitqueue_head(&dev->wait_frame);
+	dev->first_init = 1; /* webcam LED management */
 
 	dev->udev = udev;
 	dev->interface = interface;
@@ -1337,7 +1355,6 @@ static int stk_camera_probe(struct usb_interface *interface,
 		err = -ENODEV;
 		goto error;
 	}
-	dev->vsettings.brightness = 0x7fff;
 	dev->vsettings.palette = V4L2_PIX_FMT_RGB565;
 	dev->vsettings.mode = MODE_VGA;
 	dev->frame_size = 640 * 480 * 2;
@@ -1354,6 +1371,8 @@ static int stk_camera_probe(struct usb_interface *interface,
 	return 0;
 
 error:
+	v4l2_ctrl_handler_free(hdl);
+	v4l2_device_unregister(&dev->v4l2_dev);
 	kfree(dev);
 	return err;
 }
@@ -1371,6 +1390,8 @@ static void stk_camera_disconnect(struct usb_interface *interface)
 		 video_device_node_name(&dev->vdev));
 
 	video_unregister_device(&dev->vdev);
+	v4l2_ctrl_handler_free(&dev->hdl);
+	v4l2_device_unregister(&dev->v4l2_dev);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/media/usb/stkwebcam/stk-webcam.h b/drivers/media/usb/stkwebcam/stk-webcam.h
index 9f6736637571..9bbfa3d9bfdd 100644
--- a/drivers/media/usb/stkwebcam/stk-webcam.h
+++ b/drivers/media/usb/stkwebcam/stk-webcam.h
@@ -23,6 +23,8 @@
 #define STKWEBCAM_H
 
 #include <linux/usb.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
 #include <media/v4l2-common.h>
 
 #define DRIVER_VERSION		"v0.0.1"
@@ -59,7 +61,6 @@ enum stk_mode {MODE_VGA, MODE_SXGA, MODE_CIF, MODE_QVGA, MODE_QCIF};
 
 struct stk_video {
 	enum stk_mode mode;
-	int brightness;
 	__u32 palette;
 	int hflip;
 	int vflip;
@@ -91,11 +92,15 @@ struct regval {
 };
 
 struct stk_camera {
+	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler hdl;
 	struct video_device vdev;
 	struct usb_device *udev;
 	struct usb_interface *interface;
 	int webcam_model;
 	struct file *owner;
+	struct mutex lock;
+	int first_init;
 
 	u8 isoc_ep;
 
@@ -113,6 +118,7 @@ struct stk_camera {
 
 	int frame_size;
 	/* Streaming buffers */
+	int reading;
 	unsigned int n_sbufs;
 	struct stk_sio_buffer *sio_bufs;
 	struct list_head sio_avail;
diff --git a/drivers/media/usb/tlg2300/pd-common.h b/drivers/media/usb/tlg2300/pd-common.h
index 5dd73b7857d1..9e23ad32d2fe 100644
--- a/drivers/media/usb/tlg2300/pd-common.h
+++ b/drivers/media/usb/tlg2300/pd-common.h
@@ -10,6 +10,7 @@
 #include <linux/poll.h>
 #include <media/videobuf-vmalloc.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
 
 #include "dvb_frontend.h"
 #include "dvbdev.h"
@@ -25,7 +26,6 @@
 #define POSEIDON_STATE_ANALOG		(0x0001)
 #define POSEIDON_STATE_FM		(0x0002)
 #define POSEIDON_STATE_DVBT		(0x0004)
-#define POSEIDON_STATE_VBI		(0x0008)
 #define POSEIDON_STATE_DISCONNECT	(0x0080)
 
 #define PM_SUSPEND_DELAY	3
@@ -35,11 +35,11 @@
 #define V4L_PAL_VBI_FRAMESIZE	(V4L_PAL_VBI_LINES * 1440 * 2)
 #define V4L_NTSC_VBI_FRAMESIZE	(V4L_NTSC_VBI_LINES * 1440 * 2)
 
-#define TUNER_FREQ_MIN		(45000000)
-#define TUNER_FREQ_MAX		(862000000)
+#define TUNER_FREQ_MIN		(45000000U)
+#define TUNER_FREQ_MAX		(862000000U)
 
 struct vbi_data {
-	struct video_device	*v_dev;
+	struct video_device	v_dev;
 	struct video_data	*video;
 	struct front_face	*front;
 
@@ -62,7 +62,8 @@ struct running_context {
 
 struct video_data {
 	/* v4l2 video device */
-	struct video_device	*v_dev;
+	struct video_device	v_dev;
+	struct v4l2_ctrl_handler ctrl_handler;
 
 	/* the working context */
 	struct running_context	context;
@@ -115,10 +116,10 @@ struct poseidon_audio {
 
 struct radio_data {
 	__u32		fm_freq;
-	int		users;
 	unsigned int	is_radio_streaming;
 	int		pre_emphasis;
-	struct video_device *fm_dev;
+	struct video_device fm_dev;
+	struct v4l2_ctrl_handler ctrl_handler;
 };
 
 #define DVB_SBUF_NUM		4
@@ -233,7 +234,6 @@ void dvb_stop_streaming(struct pd_dvb_adapter *);
 /* FM */
 int poseidon_fm_init(struct poseidon *);
 int poseidon_fm_exit(struct poseidon *);
-struct video_device *vdev_init(struct poseidon *, struct video_device *);
 
 /* vendor command ops */
 int send_set_req(struct poseidon*, u8, s32, s32*);
@@ -249,7 +249,6 @@ void free_all_urb_generic(struct urb **urb_array, int num);
 
 /* misc */
 void poseidon_delete(struct kref *kref);
-void destroy_video_device(struct video_device **v_dev);
 extern int debug_mode;
 void set_debug_mode(struct video_device *vfd, int debug_mode);
 
@@ -269,13 +268,4 @@ void set_debug_mode(struct video_device *vfd, int debug_mode);
 				log();\
 		} while (0)
 
-#define logs(f) do { \
-			if ((debug_mode & 0x4) && \
-				(f)->type == V4L2_BUF_TYPE_VBI_CAPTURE) \
-					log("type : VBI");\
-								\
-			if ((debug_mode & 0x8) && \
-				(f)->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) \
-					log("type : VIDEO");\
-		} while (0)
 #endif
diff --git a/drivers/media/usb/tlg2300/pd-main.c b/drivers/media/usb/tlg2300/pd-main.c
index 7b1f6ebd0e2c..e07e4c699cc2 100644
--- a/drivers/media/usb/tlg2300/pd-main.c
+++ b/drivers/media/usb/tlg2300/pd-main.c
@@ -55,7 +55,6 @@ MODULE_PARM_DESC(debug_mode, "0 = disable, 1 = enable, 2 = verbose");
 
 #define TLG2300_FIRMWARE "tlg2300_firmware.bin"
 static const char *firmware_name = TLG2300_FIRMWARE;
-static struct usb_driver poseidon_driver;
 static LIST_HEAD(pd_device_list);
 
 /*
@@ -268,7 +267,8 @@ static inline void set_map_flags(struct poseidon *pd, struct usb_device *udev)
 static inline int get_autopm_ref(struct poseidon *pd)
 {
 	return  pd->video_data.users + pd->vbi_data.users + pd->audio.users
-		+ atomic_read(&pd->dvb_data.users) + pd->radio_data.users;
+		+ atomic_read(&pd->dvb_data.users) +
+		!list_empty(&pd->radio_data.fm_dev.fh_list);
 }
 
 /* fixup something for poseidon */
@@ -316,7 +316,7 @@ static int poseidon_suspend(struct usb_interface *intf, pm_message_t msg)
 		if (get_pm_count(pd) <= 0 && !in_hibernation(pd)) {
 			pd->msg.event = PM_EVENT_AUTO_SUSPEND;
 			pd->pm_resume = NULL; /*  a good guard */
-			printk(KERN_DEBUG "\n\t+ TLG2300 auto suspend +\n\n");
+			printk(KERN_DEBUG "TLG2300 auto suspend\n");
 		}
 		return 0;
 	}
@@ -331,7 +331,7 @@ static int poseidon_resume(struct usb_interface *intf)
 
 	if (!pd)
 		return 0;
-	printk(KERN_DEBUG "\n\t ++ TLG2300 resume ++\n\n");
+	printk(KERN_DEBUG "TLG2300 resume\n");
 
 	if (!is_working(pd)) {
 		if (PM_EVENT_AUTO_SUSPEND == pd->msg.event)
@@ -431,15 +431,11 @@ static int poseidon_probe(struct usb_interface *interface,
 	usb_set_intfdata(interface, pd);
 
 	if (new_one) {
-		struct device *dev = &interface->dev;
-
 		logpm(pd);
 		mutex_init(&pd->lock);
 
 		/* register v4l2 device */
-		snprintf(pd->v4l2_dev.name, sizeof(pd->v4l2_dev.name), "%s %s",
-			dev->driver->name, dev_name(dev));
-		ret = v4l2_device_register(NULL, &pd->v4l2_dev);
+		ret = v4l2_device_register(&interface->dev, &pd->v4l2_dev);
 
 		/* register devices in directory /dev */
 		ret = pd_video_init(pd);
@@ -530,7 +526,7 @@ module_init(poseidon_init);
 module_exit(poseidon_exit);
 
 MODULE_AUTHOR("Telegent Systems");
-MODULE_DESCRIPTION("For tlg2300-based USB device ");
+MODULE_DESCRIPTION("For tlg2300-based USB device");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("0.0.2");
 MODULE_FIRMWARE(TLG2300_FIRMWARE);
diff --git a/drivers/media/usb/tlg2300/pd-radio.c b/drivers/media/usb/tlg2300/pd-radio.c
index 25eeb166aa0b..ea6070ba835e 100644
--- a/drivers/media/usb/tlg2300/pd-radio.c
+++ b/drivers/media/usb/tlg2300/pd-radio.c
@@ -9,6 +9,8 @@
 #include <linux/mm.h>
 #include <linux/mutex.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fh.h>
 #include <linux/sched.h>
 
 #include "pd-common.h"
@@ -18,8 +20,8 @@ static int set_frequency(struct poseidon *p, __u32 frequency);
 static int poseidon_fm_close(struct file *filp);
 static int poseidon_fm_open(struct file *filp);
 
-#define TUNER_FREQ_MIN_FM 76000000
-#define TUNER_FREQ_MAX_FM 108000000
+#define TUNER_FREQ_MIN_FM 76000000U
+#define TUNER_FREQ_MAX_FM 108000000U
 
 #define MAX_PREEMPHASIS (V4L2_PREEMPHASIS_75_uS + 1)
 static int preemphasis[MAX_PREEMPHASIS] = {
@@ -77,13 +79,9 @@ static int pm_fm_resume(struct poseidon *p)
 
 static int poseidon_fm_open(struct file *filp)
 {
-	struct video_device *vfd = video_devdata(filp);
-	struct poseidon *p = video_get_drvdata(vfd);
+	struct poseidon *p = video_drvdata(filp);
 	int ret = 0;
 
-	if (!p)
-		return -1;
-
 	mutex_lock(&p->lock);
 	if (p->state & POSEIDON_STATE_DISCONNECT) {
 		ret = -ENODEV;
@@ -94,9 +92,14 @@ static int poseidon_fm_open(struct file *filp)
 		ret = -EBUSY;
 		goto out;
 	}
+	ret = v4l2_fh_open(filp);
+	if (ret)
+		goto out;
 
 	usb_autopm_get_interface(p->interface);
 	if (0 == p->state) {
+		struct video_device *vfd = &p->radio_data.fm_dev;
+
 		/* default pre-emphasis */
 		if (p->radio_data.pre_emphasis == 0)
 			p->radio_data.pre_emphasis = TLG_TUNE_ASTD_FM_EUR;
@@ -109,9 +112,7 @@ static int poseidon_fm_open(struct file *filp)
 		}
 		p->state |= POSEIDON_STATE_FM;
 	}
-	p->radio_data.users++;
 	kref_get(&p->kref);
-	filp->private_data = p;
 out:
 	mutex_unlock(&p->lock);
 	return ret;
@@ -119,13 +120,12 @@ out:
 
 static int poseidon_fm_close(struct file *filp)
 {
-	struct poseidon *p = filp->private_data;
+	struct poseidon *p = video_drvdata(filp);
 	struct radio_data *fm = &p->radio_data;
 	uint32_t status;
 
 	mutex_lock(&p->lock);
-	fm->users--;
-	if (0 == fm->users)
+	if (v4l2_fh_is_singular_file(filp))
 		p->state &= ~POSEIDON_STATE_FM;
 
 	if (fm->is_radio_streaming && filp == p->file_for_stream) {
@@ -136,19 +136,23 @@ static int poseidon_fm_close(struct file *filp)
 	mutex_unlock(&p->lock);
 
 	kref_put(&p->kref, poseidon_delete);
-	filp->private_data = NULL;
-	return 0;
+	return v4l2_fh_release(filp);
 }
 
 static int vidioc_querycap(struct file *file, void *priv,
 			struct v4l2_capability *v)
 {
-	struct poseidon *p = file->private_data;
+	struct poseidon *p = video_drvdata(file);
 
 	strlcpy(v->driver, "tele-radio", sizeof(v->driver));
 	strlcpy(v->card, "Telegent Poseidon", sizeof(v->card));
 	usb_make_path(p->udev, v->bus_info, sizeof(v->bus_info));
-	v->capabilities = V4L2_CAP_TUNER | V4L2_CAP_RADIO;
+	v->device_caps = V4L2_CAP_TUNER | V4L2_CAP_RADIO;
+	/* Report all capabilities of the USB device */
+	v->capabilities = v->device_caps | V4L2_CAP_DEVICE_CAPS |
+			V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VBI_CAPTURE |
+			V4L2_CAP_AUDIO | V4L2_CAP_STREAMING |
+			V4L2_CAP_READWRITE;
 	return 0;
 }
 
@@ -156,27 +160,29 @@ static const struct v4l2_file_operations poseidon_fm_fops = {
 	.owner         = THIS_MODULE,
 	.open          = poseidon_fm_open,
 	.release       = poseidon_fm_close,
-	.ioctl	       = video_ioctl2,
+	.poll		= v4l2_ctrl_poll,
+	.unlocked_ioctl = video_ioctl2,
 };
 
 static int tlg_fm_vidioc_g_tuner(struct file *file, void *priv,
 				 struct v4l2_tuner *vt)
 {
+	struct poseidon *p = video_drvdata(file);
 	struct tuner_fm_sig_stat_s fm_stat = {};
 	int ret, status, count = 5;
-	struct poseidon *p = file->private_data;
 
 	if (vt->index != 0)
 		return -EINVAL;
 
 	vt->type	= V4L2_TUNER_RADIO;
-	vt->capability	= V4L2_TUNER_CAP_STEREO;
-	vt->rangelow	= TUNER_FREQ_MIN_FM / 62500;
-	vt->rangehigh	= TUNER_FREQ_MAX_FM / 62500;
+	vt->capability	= V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_LOW;
+	vt->rangelow	= TUNER_FREQ_MIN_FM * 2 / 125;
+	vt->rangehigh	= TUNER_FREQ_MAX_FM * 2 / 125;
 	vt->rxsubchans	= V4L2_TUNER_SUB_STEREO;
 	vt->audmode	= V4L2_TUNER_MODE_STEREO;
 	vt->signal	= 0;
 	vt->afc 	= 0;
+	strlcpy(vt->name, "Radio", sizeof(vt->name));
 
 	mutex_lock(&p->lock);
 	ret = send_get_req(p, TUNER_STATUS, TLG_MODE_FM_RADIO,
@@ -205,8 +211,10 @@ static int tlg_fm_vidioc_g_tuner(struct file *file, void *priv,
 static int fm_get_freq(struct file *file, void *priv,
 		       struct v4l2_frequency *argp)
 {
-	struct poseidon *p = file->private_data;
+	struct poseidon *p = video_drvdata(file);
 
+	if (argp->tuner)
+		return -EINVAL;
 	argp->frequency = p->radio_data.fm_freq;
 	return 0;
 }
@@ -221,11 +229,8 @@ static int set_frequency(struct poseidon *p, __u32 frequency)
 	ret = send_set_req(p, TUNER_AUD_ANA_STD,
 				p->radio_data.pre_emphasis, &status);
 
-	freq =  (frequency * 125) * 500 / 1000;/* kHZ */
-	if (freq < TUNER_FREQ_MIN_FM/1000 || freq > TUNER_FREQ_MAX_FM/1000) {
-		ret = -EINVAL;
-		goto error;
-	}
+	freq = (frequency * 125) / 2; /* Hz */
+	freq = clamp(freq, TUNER_FREQ_MIN_FM, TUNER_FREQ_MAX_FM);
 
 	ret = send_set_req(p, TUNE_FREQ_SELECT, freq, &status);
 	if (ret < 0)
@@ -240,18 +245,20 @@ static int set_frequency(struct poseidon *p, __u32 frequency)
 				TLG_TUNE_PLAY_SVC_START, &status);
 		p->radio_data.is_radio_streaming = 1;
 	}
-	p->radio_data.fm_freq = frequency;
+	p->radio_data.fm_freq = freq * 2 / 125;
 error:
 	mutex_unlock(&p->lock);
 	return ret;
 }
 
 static int fm_set_freq(struct file *file, void *priv,
-		       struct v4l2_frequency *argp)
+		       const struct v4l2_frequency *argp)
 {
-	struct poseidon *p = file->private_data;
+	struct poseidon *p = video_drvdata(file);
 
-	p->file_for_stream  = file;
+	if (argp->tuner)
+		return -EINVAL;
+	p->file_for_stream = file;
 #ifdef CONFIG_PM
 	p->pm_suspend = pm_fm_suspend;
 	p->pm_resume  = pm_fm_resume;
@@ -259,163 +266,75 @@ static int fm_set_freq(struct file *file, void *priv,
 	return set_frequency(p, argp->frequency);
 }
 
-static int tlg_fm_vidioc_g_ctrl(struct file *file, void *priv,
-		struct v4l2_control *arg)
-{
-	return 0;
-}
-
-static int tlg_fm_vidioc_g_exts_ctrl(struct file *file, void *fh,
-				struct v4l2_ext_controls *ctrls)
-{
-	struct poseidon *p = file->private_data;
-	int i;
-
-	if (ctrls->ctrl_class != V4L2_CTRL_CLASS_FM_TX)
-		return -EINVAL;
-
-	for (i = 0; i < ctrls->count; i++) {
-		struct v4l2_ext_control *ctrl = ctrls->controls + i;
-
-		if (ctrl->id != V4L2_CID_TUNE_PREEMPHASIS)
-			continue;
-
-		if (i < MAX_PREEMPHASIS)
-			ctrl->value = p->radio_data.pre_emphasis;
-	}
-	return 0;
-}
-
-static int tlg_fm_vidioc_s_exts_ctrl(struct file *file, void *fh,
-			struct v4l2_ext_controls *ctrls)
-{
-	int i;
-
-	if (ctrls->ctrl_class != V4L2_CTRL_CLASS_FM_TX)
-		return -EINVAL;
-
-	for (i = 0; i < ctrls->count; i++) {
-		struct v4l2_ext_control *ctrl = ctrls->controls + i;
-
-		if (ctrl->id != V4L2_CID_TUNE_PREEMPHASIS)
-			continue;
-
-		if (ctrl->value >= 0 && ctrl->value < MAX_PREEMPHASIS) {
-			struct poseidon *p = file->private_data;
-			int pre_emphasis = preemphasis[ctrl->value];
-			u32 status;
-
-			send_set_req(p, TUNER_AUD_ANA_STD,
-						pre_emphasis, &status);
-			p->radio_data.pre_emphasis = pre_emphasis;
-		}
-	}
-	return 0;
-}
-
-static int tlg_fm_vidioc_s_ctrl(struct file *file, void *priv,
-		struct v4l2_control *ctrl)
+static int tlg_fm_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	return 0;
-}
-
-static int tlg_fm_vidioc_queryctrl(struct file *file, void *priv,
-		struct v4l2_queryctrl *ctrl)
-{
-	if (!(ctrl->id & V4L2_CTRL_FLAG_NEXT_CTRL))
-		return -EINVAL;
+	struct poseidon *p = container_of(ctrl->handler, struct poseidon,
+						radio_data.ctrl_handler);
+	int pre_emphasis;
+	u32 status;
 
-	ctrl->id &= ~V4L2_CTRL_FLAG_NEXT_CTRL;
-	if (ctrl->id != V4L2_CID_TUNE_PREEMPHASIS) {
-		/* return the next supported control */
-		ctrl->id = V4L2_CID_TUNE_PREEMPHASIS;
-		v4l2_ctrl_query_fill(ctrl, V4L2_PREEMPHASIS_DISABLED,
-					V4L2_PREEMPHASIS_75_uS, 1,
-					V4L2_PREEMPHASIS_50_uS);
-		ctrl->flags = V4L2_CTRL_FLAG_UPDATE;
+	switch (ctrl->id) {
+	case V4L2_CID_TUNE_PREEMPHASIS:
+		pre_emphasis = preemphasis[ctrl->val];
+		send_set_req(p, TUNER_AUD_ANA_STD, pre_emphasis, &status);
+		p->radio_data.pre_emphasis = pre_emphasis;
 		return 0;
 	}
 	return -EINVAL;
 }
 
-static int tlg_fm_vidioc_querymenu(struct file *file, void *fh,
-				struct v4l2_querymenu *qmenu)
-{
-	return v4l2_ctrl_query_menu(qmenu, NULL, NULL);
-}
-
-static int vidioc_s_tuner(struct file *file, void *priv, struct v4l2_tuner *vt)
+static int vidioc_s_tuner(struct file *file, void *priv, const struct v4l2_tuner *vt)
 {
 	return vt->index > 0 ? -EINVAL : 0;
 }
-static int vidioc_s_audio(struct file *file, void *priv, const struct v4l2_audio *va)
-{
-	return (va->index != 0) ? -EINVAL : 0;
-}
-
-static int vidioc_g_audio(struct file *file, void *priv, struct v4l2_audio *a)
-{
-	a->index    = 0;
-	a->mode    = 0;
-	a->capability = V4L2_AUDCAP_STEREO;
-	strcpy(a->name, "Radio");
-	return 0;
-}
-
-static int vidioc_s_input(struct file *filp, void *priv, u32 i)
-{
-	return (i != 0) ? -EINVAL : 0;
-}
 
-static int vidioc_g_input(struct file *filp, void *priv, u32 *i)
-{
-	return (*i != 0) ? -EINVAL : 0;
-}
+static const struct v4l2_ctrl_ops tlg_fm_ctrl_ops = {
+	.s_ctrl = tlg_fm_s_ctrl,
+};
 
 static const struct v4l2_ioctl_ops poseidon_fm_ioctl_ops = {
 	.vidioc_querycap    = vidioc_querycap,
-	.vidioc_g_audio     = vidioc_g_audio,
-	.vidioc_s_audio     = vidioc_s_audio,
-	.vidioc_g_input     = vidioc_g_input,
-	.vidioc_s_input     = vidioc_s_input,
-	.vidioc_queryctrl   = tlg_fm_vidioc_queryctrl,
-	.vidioc_querymenu   = tlg_fm_vidioc_querymenu,
-	.vidioc_g_ctrl      = tlg_fm_vidioc_g_ctrl,
-	.vidioc_s_ctrl      = tlg_fm_vidioc_s_ctrl,
-	.vidioc_s_ext_ctrls = tlg_fm_vidioc_s_exts_ctrl,
-	.vidioc_g_ext_ctrls = tlg_fm_vidioc_g_exts_ctrl,
 	.vidioc_s_tuner     = vidioc_s_tuner,
 	.vidioc_g_tuner     = tlg_fm_vidioc_g_tuner,
 	.vidioc_g_frequency = fm_get_freq,
 	.vidioc_s_frequency = fm_set_freq,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static struct video_device poseidon_fm_template = {
 	.name       = "Telegent-Radio",
 	.fops       = &poseidon_fm_fops,
 	.minor      = -1,
-	.release    = video_device_release,
+	.release    = video_device_release_empty,
 	.ioctl_ops  = &poseidon_fm_ioctl_ops,
 };
 
 int poseidon_fm_init(struct poseidon *p)
 {
-	struct video_device *fm_dev;
-
-	fm_dev = vdev_init(p, &poseidon_fm_template);
-	if (fm_dev == NULL)
-		return -1;
-
-	if (video_register_device(fm_dev, VFL_TYPE_RADIO, -1) < 0) {
-		video_device_release(fm_dev);
-		return -1;
+	struct video_device *vfd = &p->radio_data.fm_dev;
+	struct v4l2_ctrl_handler *hdl = &p->radio_data.ctrl_handler;
+
+	*vfd = poseidon_fm_template;
+
+	set_frequency(p, TUNER_FREQ_MIN_FM);
+	v4l2_ctrl_handler_init(hdl, 1);
+	v4l2_ctrl_new_std_menu(hdl, &tlg_fm_ctrl_ops, V4L2_CID_TUNE_PREEMPHASIS,
+			V4L2_PREEMPHASIS_75_uS, 0, V4L2_PREEMPHASIS_50_uS);
+	if (hdl->error) {
+		v4l2_ctrl_handler_free(hdl);
+		return hdl->error;
 	}
-	p->radio_data.fm_dev = fm_dev;
-	return 0;
+	vfd->v4l2_dev = &p->v4l2_dev;
+	vfd->ctrl_handler = hdl;
+	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
+	video_set_drvdata(vfd, p);
+	return video_register_device(vfd, VFL_TYPE_RADIO, -1);
 }
 
 int poseidon_fm_exit(struct poseidon *p)
 {
-	destroy_video_device(&p->radio_data.fm_dev);
+	video_unregister_device(&p->radio_data.fm_dev);
+	v4l2_ctrl_handler_free(&p->radio_data.ctrl_handler);
 	return 0;
 }
diff --git a/drivers/media/usb/tlg2300/pd-video.c b/drivers/media/usb/tlg2300/pd-video.c
index 21723378bb8f..8df668d06552 100644
--- a/drivers/media/usb/tlg2300/pd-video.c
+++ b/drivers/media/usb/tlg2300/pd-video.c
@@ -8,6 +8,7 @@
 
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-dev.h>
+#include <media/v4l2-ctrls.h>
 
 #include "pd-common.h"
 #include "vendorcmds.h"
@@ -82,31 +83,6 @@ static const struct pd_input pd_inputs[] = {
 };
 static const unsigned int POSEIDON_INPUTS = ARRAY_SIZE(pd_inputs);
 
-struct poseidon_control {
-	struct v4l2_queryctrl v4l2_ctrl;
-	enum cmd_custom_param_id vc_id;
-};
-
-static struct poseidon_control controls[] = {
-	{
-		{ V4L2_CID_BRIGHTNESS, V4L2_CTRL_TYPE_INTEGER,
-			"brightness", 0, 10000, 1, 100, 0, },
-		CUST_PARM_ID_BRIGHTNESS_CTRL
-	}, {
-		{ V4L2_CID_CONTRAST, V4L2_CTRL_TYPE_INTEGER,
-			"contrast", 0, 10000, 1, 100, 0, },
-		CUST_PARM_ID_CONTRAST_CTRL,
-	}, {
-		{ V4L2_CID_HUE, V4L2_CTRL_TYPE_INTEGER,
-			"hue", 0, 10000, 1, 100, 0, },
-		CUST_PARM_ID_HUE_CTRL,
-	}, {
-		{ V4L2_CID_SATURATION, V4L2_CTRL_TYPE_INTEGER,
-			"saturation", 0, 10000, 1, 100, 0, },
-		CUST_PARM_ID_SATURATION_CTRL,
-	},
-};
-
 struct video_std_to_audio_std {
 	v4l2_std_id	video_std;
 	int 		audio_std;
@@ -142,17 +118,20 @@ static int get_audio_std(v4l2_std_id v4l2_std)
 static int vidioc_querycap(struct file *file, void *fh,
 			struct v4l2_capability *cap)
 {
-	struct front_face *front = fh;
-	struct poseidon *p = front->pd;
-
-	logs(front);
+	struct video_device *vdev = video_devdata(file);
+	struct poseidon *p = video_get_drvdata(vdev);
 
 	strcpy(cap->driver, "tele-video");
 	strcpy(cap->card, "Telegent Poseidon");
 	usb_make_path(p->udev, cap->bus_info, sizeof(cap->bus_info));
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_TUNER |
-				V4L2_CAP_AUDIO | V4L2_CAP_STREAMING |
-				V4L2_CAP_READWRITE | V4L2_CAP_VBI_CAPTURE;
+	cap->device_caps = V4L2_CAP_TUNER | V4L2_CAP_AUDIO |
+			V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+	if (vdev->vfl_type == VFL_TYPE_VBI)
+		cap->device_caps |= V4L2_CAP_VBI_CAPTURE;
+	else
+		cap->device_caps |= V4L2_CAP_VIDEO_CAPTURE;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS |
+		V4L2_CAP_RADIO | V4L2_CAP_VBI_CAPTURE | V4L2_CAP_VIDEO_CAPTURE;
 	return 0;
 }
 
@@ -223,7 +202,6 @@ static void submit_frame(struct front_face *front)
  */
 static void end_field(struct video_data *video)
 {
-	/* logs(video->front); */
 	if (1 == video->field_count)
 		submit_frame(video->front);
 	else
@@ -718,17 +696,10 @@ static int vidioc_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
 	struct front_face *front = fh;
 	struct poseidon *pd = front->pd;
 
-	logs(front);
 	f->fmt.pix = pd->video_data.context.pix;
 	return 0;
 }
 
-static int vidioc_try_fmt(struct file *file, void *fh,
-		struct v4l2_format *f)
-{
-	return 0;
-}
-
 /*
  * VLC calls VIDIOC_S_STD before VIDIOC_S_FMT, while
  * Mplayer calls them in the reverse order.
@@ -787,7 +758,6 @@ static int vidioc_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
 	struct front_face *front	= fh;
 	struct poseidon *pd		= front->pd;
 
-	logs(front);
 	/* stop VBI here */
 	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != f->type)
 		return -EINVAL;
@@ -828,11 +798,10 @@ static int vidioc_g_fmt_vbi(struct file *file, void *fh,
 		vbi_fmt->count[1] = V4L_PAL_VBI_LINES;
 	}
 	vbi_fmt->flags = V4L2_VBI_UNSYNC;
-	logs(front);
 	return 0;
 }
 
-static int set_std(struct poseidon *pd, v4l2_std_id *norm)
+static int set_std(struct poseidon *pd, v4l2_std_id norm)
 {
 	struct video_data *video = &pd->video_data;
 	struct vbi_data *vbi	= &pd->vbi_data;
@@ -842,7 +811,7 @@ static int set_std(struct poseidon *pd, v4l2_std_id *norm)
 	int height;
 
 	for (i = 0; i < POSEIDON_TVNORMS; i++) {
-		if (*norm & poseidon_tvnorms[i].v4l2_id) {
+		if (norm & poseidon_tvnorms[i].v4l2_id) {
 			param = poseidon_tvnorms[i].tlg_tvnorm;
 			log("name : %s", poseidon_tvnorms[i].name);
 			goto found;
@@ -877,17 +846,23 @@ out:
 	return ret;
 }
 
-static int vidioc_s_std(struct file *file, void *fh, v4l2_std_id *norm)
+static int vidioc_s_std(struct file *file, void *fh, v4l2_std_id norm)
 {
 	struct front_face *front = fh;
-	logs(front);
+
 	return set_std(front->pd, norm);
 }
 
-static int vidioc_enum_input(struct file *file, void *fh, struct v4l2_input *in)
+static int vidioc_g_std(struct file *file, void *fh, v4l2_std_id *norm)
 {
 	struct front_face *front = fh;
 
+	*norm = front->pd->video_data.context.tvnormid;
+	return 0;
+}
+
+static int vidioc_enum_input(struct file *file, void *fh, struct v4l2_input *in)
+{
 	if (in->index >= POSEIDON_INPUTS)
 		return -EINVAL;
 	strcpy(in->name, pd_inputs[in->index].name);
@@ -897,11 +872,10 @@ static int vidioc_enum_input(struct file *file, void *fh, struct v4l2_input *in)
 	 * the audio input index mixed with this video input,
 	 * Poseidon only have one audio/video, set to "0"
 	 */
-	in->audioset	= 0;
+	in->audioset	= 1;
 	in->tuner	= 0;
 	in->std		= V4L2_STD_ALL;
 	in->status	= 0;
-	logs(front);
 	return 0;
 }
 
@@ -911,7 +885,6 @@ static int vidioc_g_input(struct file *file, void *fh, unsigned int *i)
 	struct poseidon *pd = front->pd;
 	struct running_context *context = &pd->video_data.context;
 
-	logs(front);
 	*i = context->sig_index;
 	return 0;
 }
@@ -934,68 +907,28 @@ static int vidioc_s_input(struct file *file, void *fh, unsigned int i)
 	return 0;
 }
 
-static struct poseidon_control *check_control_id(__u32 id)
-{
-	struct poseidon_control *control = &controls[0];
-	int array_size = ARRAY_SIZE(controls);
-
-	for (; control < &controls[array_size]; control++)
-		if (control->v4l2_ctrl.id  == id)
-			return control;
-	return NULL;
-}
-
-static int vidioc_queryctrl(struct file *file, void *fh,
-			struct v4l2_queryctrl *a)
-{
-	struct poseidon_control *control = NULL;
-
-	control = check_control_id(a->id);
-	if (!control)
-		return -EINVAL;
-
-	*a = control->v4l2_ctrl;
-	return 0;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *fh, struct v4l2_control *ctrl)
-{
-	struct front_face *front = fh;
-	struct poseidon *pd = front->pd;
-	struct poseidon_control *control = NULL;
-	struct tuner_custom_parameter_s tuner_param;
-	s32 ret = 0, cmd_status;
-
-	control = check_control_id(ctrl->id);
-	if (!control)
-		return -EINVAL;
-
-	mutex_lock(&pd->lock);
-	ret = send_get_req(pd, TUNER_CUSTOM_PARAMETER, control->vc_id,
-			&tuner_param, &cmd_status, sizeof(tuner_param));
-	mutex_unlock(&pd->lock);
-
-	if (ret || cmd_status)
-		return -1;
-
-	ctrl->value = tuner_param.param_value;
-	return 0;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *fh, struct v4l2_control *a)
+static int tlg_s_ctrl(struct v4l2_ctrl *c)
 {
+	struct poseidon *pd = container_of(c->handler, struct poseidon,
+						video_data.ctrl_handler);
 	struct tuner_custom_parameter_s param = {0};
-	struct poseidon_control *control = NULL;
-	struct front_face *front	= fh;
-	struct poseidon *pd		= front->pd;
 	s32 ret = 0, cmd_status, params;
 
-	control = check_control_id(a->id);
-	if (!control)
-		return -EINVAL;
-
-	param.param_value = a->value;
-	param.param_id	= control->vc_id;
+	switch (c->id) {
+	case V4L2_CID_BRIGHTNESS:
+		param.param_id = CUST_PARM_ID_BRIGHTNESS_CTRL;
+		break;
+	case V4L2_CID_CONTRAST:
+		param.param_id = CUST_PARM_ID_CONTRAST_CTRL;
+		break;
+	case V4L2_CID_HUE:
+		param.param_id = CUST_PARM_ID_HUE_CTRL;
+		break;
+	case V4L2_CID_SATURATION:
+		param.param_id = CUST_PARM_ID_SATURATION_CTRL;
+		break;
+	}
+	param.param_value = c->val;
 	params = *(s32 *)&param; /* temp code */
 
 	mutex_lock(&pd->lock);
@@ -1079,7 +1012,6 @@ static int vidioc_g_tuner(struct file *file, void *fh, struct v4l2_tuner *tuner)
 	tuner->rxsubchans = pd_audio_modes[index].v4l2_audio_sub;
 	tuner->audmode = pd_audio_modes[index].v4l2_audio_mode;
 	tuner->afc = 0;
-	logs(front);
 	return 0;
 }
 
@@ -1099,7 +1031,7 @@ static int pd_vidioc_s_tuner(struct poseidon *pd, int index)
 	return ret;
 }
 
-static int vidioc_s_tuner(struct file *file, void *fh, struct v4l2_tuner *a)
+static int vidioc_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *a)
 {
 	struct front_face *front	= fh;
 	struct poseidon *pd		= front->pd;
@@ -1107,7 +1039,6 @@ static int vidioc_s_tuner(struct file *file, void *fh, struct v4l2_tuner *a)
 
 	if (0 != a->index)
 		return -EINVAL;
-	logs(front);
 	for (index = 0; index < POSEIDON_AUDIOMODS; index++)
 		if (a->audmode == pd_audio_modes[index].v4l2_audio_mode)
 			return pd_vidioc_s_tuner(pd, index);
@@ -1128,51 +1059,51 @@ static int vidioc_g_frequency(struct file *file, void *fh,
 	return 0;
 }
 
-static int set_frequency(struct poseidon *pd, __u32 frequency)
+static int set_frequency(struct poseidon *pd, u32 *frequency)
 {
 	s32 ret = 0, param, cmd_status;
 	struct running_context *context = &pd->video_data.context;
 
-	param = frequency * 62500 / 1000;
-	if (param < TUNER_FREQ_MIN/1000 || param > TUNER_FREQ_MAX / 1000)
-		return -EINVAL;
+	*frequency = clamp(*frequency,
+			TUNER_FREQ_MIN / 62500, TUNER_FREQ_MAX / 62500);
+	param = (*frequency) * 62500 / 1000;
 
 	mutex_lock(&pd->lock);
 	ret = send_set_req(pd, TUNE_FREQ_SELECT, param, &cmd_status);
 	ret = send_set_req(pd, TAKE_REQUEST, 0, &cmd_status);
 
 	msleep(250); /* wait for a while until the hardware is ready. */
-	context->freq = frequency;
+	context->freq = *frequency;
 	mutex_unlock(&pd->lock);
 	return ret;
 }
 
 static int vidioc_s_frequency(struct file *file, void *fh,
-				struct v4l2_frequency *freq)
+				const struct v4l2_frequency *freq)
 {
 	struct front_face *front = fh;
 	struct poseidon *pd = front->pd;
+	u32 frequency = freq->frequency;
 
-	logs(front);
+	if (freq->tuner)
+		return -EINVAL;
 #ifdef CONFIG_PM
 	pd->pm_suspend = pm_video_suspend;
 	pd->pm_resume = pm_video_resume;
 #endif
-	return set_frequency(pd, freq->frequency);
+	return set_frequency(pd, &frequency);
 }
 
 static int vidioc_reqbufs(struct file *file, void *fh,
 				struct v4l2_requestbuffers *b)
 {
 	struct front_face *front = file->private_data;
-	logs(front);
 	return videobuf_reqbufs(&front->q, b);
 }
 
 static int vidioc_querybuf(struct file *file, void *fh, struct v4l2_buffer *b)
 {
 	struct front_face *front = file->private_data;
-	logs(front);
 	return videobuf_querybuf(&front->q, b);
 }
 
@@ -1261,7 +1192,6 @@ static int vidioc_streamon(struct file *file, void *fh,
 {
 	struct front_face *front = fh;
 
-	logs(front);
 	if (unlikely(type != front->type))
 		return -EINVAL;
 	return videobuf_streamon(&front->q);
@@ -1272,7 +1202,6 @@ static int vidioc_streamoff(struct file *file, void *fh,
 {
 	struct front_face *front = file->private_data;
 
-	logs(front);
 	if (unlikely(type != front->type))
 		return -EINVAL;
 	return videobuf_streamoff(&front->q);
@@ -1341,11 +1270,11 @@ static int restore_v4l2_context(struct poseidon *pd,
 
 	pd_video_checkmode(pd);
 
-	set_std(pd, &context->tvnormid);
+	set_std(pd, context->tvnormid);
 	vidioc_s_input(NULL, front, context->sig_index);
 	pd_vidioc_s_tuner(pd, context->audio_idx);
 	pd_vidioc_s_fmt(pd, &context->pix);
-	set_frequency(pd, context->freq);
+	set_frequency(pd, &context->freq);
 	return 0;
 }
 
@@ -1406,12 +1335,14 @@ static int pd_video_open(struct file *file)
 	mutex_lock(&pd->lock);
 	usb_autopm_get_interface(pd->interface);
 
-	if (vfd->vfl_type == VFL_TYPE_GRABBER
-		&& !(pd->state & POSEIDON_STATE_ANALOG)) {
-		front = kzalloc(sizeof(struct front_face), GFP_KERNEL);
-		if (!front)
-			goto out;
-
+	if (pd->state && !(pd->state & POSEIDON_STATE_ANALOG)) {
+		ret = -EBUSY;
+		goto out;
+	}
+	front = kzalloc(sizeof(struct front_face), GFP_KERNEL);
+	if (!front)
+		goto out;
+	if (vfd->vfl_type == VFL_TYPE_GRABBER) {
 		pd->cur_transfer_mode	= usb_transfer_mode;/* bulk or iso */
 		init_video_context(&pd->video_data.context);
 
@@ -1422,7 +1353,6 @@ static int pd_video_open(struct file *file)
 			goto out;
 		}
 
-		pd->state		|= POSEIDON_STATE_ANALOG;
 		front->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
 		pd->video_data.users++;
 		set_debug_mode(vfd, debug_mode);
@@ -1433,13 +1363,7 @@ static int pd_video_open(struct file *file)
 				V4L2_FIELD_INTERLACED,/* video is interlacd */
 				sizeof(struct videobuf_buffer),/*it's enough*/
 				front, NULL);
-	} else if (vfd->vfl_type == VFL_TYPE_VBI
-		&& !(pd->state & POSEIDON_STATE_VBI)) {
-		front = kzalloc(sizeof(struct front_face), GFP_KERNEL);
-		if (!front)
-			goto out;
-
-		pd->state	|= POSEIDON_STATE_VBI;
+	} else {
 		front->type	= V4L2_BUF_TYPE_VBI_CAPTURE;
 		pd->vbi_data.front = front;
 		pd->vbi_data.users++;
@@ -1450,19 +1374,15 @@ static int pd_video_open(struct file *file)
 				V4L2_FIELD_NONE, /* vbi is NONE mode */
 				sizeof(struct videobuf_buffer),
 				front, NULL);
-	} else {
-		/* maybe add FM support here */
-		log("other ");
-		ret = -EINVAL;
-		goto out;
 	}
 
-	front->pd		= pd;
-	front->curr_frame	= NULL;
+	pd->state |= POSEIDON_STATE_ANALOG;
+	front->pd = pd;
+	front->curr_frame = NULL;
 	INIT_LIST_HEAD(&front->active);
 	spin_lock_init(&front->queue_lock);
 
-	file->private_data	= front;
+	file->private_data = front;
 	kref_get(&pd->kref);
 
 	mutex_unlock(&pd->lock);
@@ -1479,12 +1399,9 @@ static int pd_video_release(struct file *file)
 	struct poseidon *pd = front->pd;
 	s32 cmd_status = 0;
 
-	logs(front);
 	mutex_lock(&pd->lock);
 
 	if (front->type	== V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-		pd->state &= ~POSEIDON_STATE_ANALOG;
-
 		/* stop the device, and free the URBs */
 		usb_transfer_stop(&pd->video_data);
 		free_all_urb(&pd->video_data);
@@ -1496,10 +1413,11 @@ static int pd_video_release(struct file *file)
 		pd->file_for_stream = NULL;
 		pd->video_data.users--;
 	} else if (front->type	== V4L2_BUF_TYPE_VBI_CAPTURE) {
-		pd->state &= ~POSEIDON_STATE_VBI;
 		pd->vbi_data.front = NULL;
 		pd->vbi_data.users--;
 	}
+	if (!pd->vbi_data.users && !pd->video_data.users)
+		pd->state &= ~POSEIDON_STATE_ANALOG;
 	videobuf_stop(&front->q);
 	videobuf_mmap_free(&front->q);
 
@@ -1551,7 +1469,6 @@ static const struct v4l2_ioctl_ops pd_video_ioctl_ops = {
 	.vidioc_enum_fmt_vid_cap	= vidioc_enum_fmt,
 	.vidioc_s_fmt_vid_cap	= vidioc_s_fmt,
 	.vidioc_g_fmt_vbi_cap	= vidioc_g_fmt_vbi, /* VBI */
-	.vidioc_try_fmt_vid_cap = vidioc_try_fmt,
 
 	/* Input */
 	.vidioc_g_input		= vidioc_g_input,
@@ -1566,6 +1483,7 @@ static const struct v4l2_ioctl_ops pd_video_ioctl_ops = {
 	/* Tuner ioctls */
 	.vidioc_g_tuner		= vidioc_g_tuner,
 	.vidioc_s_tuner		= vidioc_s_tuner,
+	.vidioc_g_std		= vidioc_g_std,
 	.vidioc_s_std		= vidioc_s_std,
 	.vidioc_g_frequency	= vidioc_g_frequency,
 	.vidioc_s_frequency	= vidioc_s_frequency,
@@ -1579,59 +1497,29 @@ static const struct v4l2_ioctl_ops pd_video_ioctl_ops = {
 	/* Stream on/off */
 	.vidioc_streamon	= vidioc_streamon,
 	.vidioc_streamoff	= vidioc_streamoff,
-
-	/* Control handling */
-	.vidioc_queryctrl	= vidioc_queryctrl,
-	.vidioc_g_ctrl		= vidioc_g_ctrl,
-	.vidioc_s_ctrl		= vidioc_s_ctrl,
 };
 
 static struct video_device pd_video_template = {
 	.name = "Telegent-Video",
 	.fops = &pd_video_fops,
 	.minor = -1,
-	.release = video_device_release,
+	.release = video_device_release_empty,
 	.tvnorms = V4L2_STD_ALL,
 	.ioctl_ops = &pd_video_ioctl_ops,
 };
 
-struct video_device *vdev_init(struct poseidon *pd, struct video_device *tmp)
-{
-	struct video_device *vfd;
-
-	vfd = video_device_alloc();
-	if (vfd == NULL)
-		return NULL;
-	*vfd		= *tmp;
-	vfd->minor	= -1;
-	vfd->v4l2_dev	= &pd->v4l2_dev;
-	/*vfd->parent	= &(pd->udev->dev); */
-	vfd->release	= video_device_release;
-	video_set_drvdata(vfd, pd);
-	return vfd;
-}
-
-void destroy_video_device(struct video_device **v_dev)
-{
-	struct video_device *dev = *v_dev;
-
-	if (dev == NULL)
-		return;
-
-	if (video_is_registered(dev))
-		video_unregister_device(dev);
-	else
-		video_device_release(dev);
-	*v_dev = NULL;
-}
+static const struct v4l2_ctrl_ops tlg_ctrl_ops = {
+	.s_ctrl = tlg_s_ctrl,
+};
 
 void pd_video_exit(struct poseidon *pd)
 {
 	struct video_data *video = &pd->video_data;
 	struct vbi_data *vbi = &pd->vbi_data;
 
-	destroy_video_device(&video->v_dev);
-	destroy_video_device(&vbi->v_dev);
+	video_unregister_device(&video->v_dev);
+	video_unregister_device(&vbi->v_dev);
+	v4l2_ctrl_handler_free(&video->ctrl_handler);
 	log();
 }
 
@@ -1639,23 +1527,39 @@ int pd_video_init(struct poseidon *pd)
 {
 	struct video_data *video = &pd->video_data;
 	struct vbi_data *vbi	= &pd->vbi_data;
+	struct v4l2_ctrl_handler *hdl = &video->ctrl_handler;
+	u32 freq = TUNER_FREQ_MIN / 62500;
 	int ret = -ENOMEM;
 
-	video->v_dev = vdev_init(pd, &pd_video_template);
-	if (video->v_dev == NULL)
-		goto out;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &tlg_ctrl_ops, V4L2_CID_BRIGHTNESS,
+			0, 10000, 1, 100);
+	v4l2_ctrl_new_std(hdl, &tlg_ctrl_ops, V4L2_CID_CONTRAST,
+			0, 10000, 1, 100);
+	v4l2_ctrl_new_std(hdl, &tlg_ctrl_ops, V4L2_CID_HUE,
+			0, 10000, 1, 100);
+	v4l2_ctrl_new_std(hdl, &tlg_ctrl_ops, V4L2_CID_SATURATION,
+			0, 10000, 1, 100);
+	if (hdl->error) {
+		v4l2_ctrl_handler_free(hdl);
+		return hdl->error;
+	}
+	set_frequency(pd, &freq);
+	video->v_dev = pd_video_template;
+	video->v_dev.v4l2_dev = &pd->v4l2_dev;
+	video->v_dev.ctrl_handler = hdl;
+	video_set_drvdata(&video->v_dev, pd);
 
-	ret = video_register_device(video->v_dev, VFL_TYPE_GRABBER, -1);
+	ret = video_register_device(&video->v_dev, VFL_TYPE_GRABBER, -1);
 	if (ret != 0)
 		goto out;
 
 	/* VBI uses the same template as video */
-	vbi->v_dev = vdev_init(pd, &pd_video_template);
-	if (vbi->v_dev == NULL) {
-		ret = -ENOMEM;
-		goto out;
-	}
-	ret = video_register_device(vbi->v_dev, VFL_TYPE_VBI, -1);
+	vbi->v_dev = pd_video_template;
+	vbi->v_dev.v4l2_dev = &pd->v4l2_dev;
+	vbi->v_dev.ctrl_handler = hdl;
+	video_set_drvdata(&vbi->v_dev, pd);
+	ret = video_register_device(&vbi->v_dev, VFL_TYPE_VBI, -1);
 	if (ret != 0)
 		goto out;
 	log("register VIDEO/VBI devices");
@@ -1665,4 +1569,3 @@ out:
 	pd_video_exit(pd);
 	return ret;
 }
-
diff --git a/drivers/media/usb/tm6000/tm6000-video.c b/drivers/media/usb/tm6000/tm6000-video.c
index 1a6857929c15..a78de1d1bc9e 100644
--- a/drivers/media/usb/tm6000/tm6000-video.c
+++ b/drivers/media/usb/tm6000/tm6000-video.c
@@ -1056,13 +1056,13 @@ static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *norm)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 {
 	int rc = 0;
 	struct tm6000_fh *fh = priv;
 	struct tm6000_core *dev = fh->dev;
 
-	dev->norm = *norm;
+	dev->norm = norm;
 	rc = tm6000_init_analog_mode(dev);
 
 	fh->width  = dev->width;
@@ -1134,7 +1134,7 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 
 	dev->input = i;
 
-	rc = vidioc_s_std(file, priv, &dev->vfd->current_norm);
+	rc = vidioc_s_std(file, priv, dev->vfd->current_norm);
 
 	return rc;
 }
@@ -1215,7 +1215,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
+				const struct v4l2_tuner *t)
 {
 	struct tm6000_fh   *fh  = priv;
 	struct tm6000_core *dev = fh->dev;
@@ -1255,7 +1255,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+				const struct v4l2_frequency *f)
 {
 	struct tm6000_fh   *fh  = priv;
 	struct tm6000_core *dev = fh->dev;
@@ -1293,18 +1293,14 @@ static int radio_g_tuner(struct file *file, void *priv,
 }
 
 static int radio_s_tuner(struct file *file, void *priv,
-					struct v4l2_tuner *t)
+					const struct v4l2_tuner *t)
 {
 	struct tm6000_fh *fh = file->private_data;
 	struct tm6000_core *dev = fh->dev;
 
 	if (0 != t->index)
 		return -EINVAL;
-	if (t->audmode > V4L2_TUNER_MODE_STEREO)
-		t->audmode = V4L2_TUNER_MODE_STEREO;
-
 	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_tuner, t);
-
 	return 0;
 }
 
diff --git a/drivers/media/usb/ttusb-budget/dvb-ttusb-budget.c b/drivers/media/usb/ttusb-budget/dvb-ttusb-budget.c
index e40718552850..21b9049c7b3f 100644
--- a/drivers/media/usb/ttusb-budget/dvb-ttusb-budget.c
+++ b/drivers/media/usb/ttusb-budget/dvb-ttusb-budget.c
@@ -930,11 +930,11 @@ static int ttusb_start_feed(struct dvb_demux_feed *dvbdmxfeed)
 
 	if (dvbdmxfeed->type == DMX_TYPE_TS) {
 		switch (dvbdmxfeed->pes_type) {
-		case DMX_TS_PES_VIDEO:
-		case DMX_TS_PES_AUDIO:
-		case DMX_TS_PES_TELETEXT:
-		case DMX_TS_PES_PCR:
-		case DMX_TS_PES_OTHER:
+		case DMX_PES_VIDEO:
+		case DMX_PES_AUDIO:
+		case DMX_PES_TELETEXT:
+		case DMX_PES_PCR:
+		case DMX_PES_OTHER:
 			break;
 		default:
 			return -EINVAL;
diff --git a/drivers/media/usb/ttusb-dec/ttusb_dec.c b/drivers/media/usb/ttusb-dec/ttusb_dec.c
index 504c81230339..e52c3b97f304 100644
--- a/drivers/media/usb/ttusb-dec/ttusb_dec.c
+++ b/drivers/media/usb/ttusb-dec/ttusb_dec.c
@@ -951,34 +951,34 @@ static int ttusb_dec_start_ts_feed(struct dvb_demux_feed *dvbdmxfeed)
 
 	switch (dvbdmxfeed->pes_type) {
 
-	case DMX_TS_PES_VIDEO:
-		dprintk("  pes_type: DMX_TS_PES_VIDEO\n");
+	case DMX_PES_VIDEO:
+		dprintk("  pes_type: DMX_PES_VIDEO\n");
 		dec->pid[DMX_PES_PCR] = dvbdmxfeed->pid;
 		dec->pid[DMX_PES_VIDEO] = dvbdmxfeed->pid;
 		dec->video_filter = dvbdmxfeed->filter;
 		ttusb_dec_set_pids(dec);
 		break;
 
-	case DMX_TS_PES_AUDIO:
-		dprintk("  pes_type: DMX_TS_PES_AUDIO\n");
+	case DMX_PES_AUDIO:
+		dprintk("  pes_type: DMX_PES_AUDIO\n");
 		dec->pid[DMX_PES_AUDIO] = dvbdmxfeed->pid;
 		dec->audio_filter = dvbdmxfeed->filter;
 		ttusb_dec_set_pids(dec);
 		break;
 
-	case DMX_TS_PES_TELETEXT:
+	case DMX_PES_TELETEXT:
 		dec->pid[DMX_PES_TELETEXT] = dvbdmxfeed->pid;
-		dprintk("  pes_type: DMX_TS_PES_TELETEXT(not supported)\n");
+		dprintk("  pes_type: DMX_PES_TELETEXT(not supported)\n");
 		return -ENOSYS;
 
-	case DMX_TS_PES_PCR:
-		dprintk("  pes_type: DMX_TS_PES_PCR\n");
+	case DMX_PES_PCR:
+		dprintk("  pes_type: DMX_PES_PCR\n");
 		dec->pid[DMX_PES_PCR] = dvbdmxfeed->pid;
 		ttusb_dec_set_pids(dec);
 		break;
 
-	case DMX_TS_PES_OTHER:
-		dprintk("  pes_type: DMX_TS_PES_OTHER(not supported)\n");
+	case DMX_PES_OTHER:
+		dprintk("  pes_type: DMX_PES_OTHER(not supported)\n");
 		return -ENOSYS;
 
 	default:
diff --git a/drivers/media/usb/usbvision/usbvision-video.c b/drivers/media/usb/usbvision/usbvision-video.c
index cd1fe78a5532..d34c2afe2c24 100644
--- a/drivers/media/usb/usbvision/usbvision-video.c
+++ b/drivers/media/usb/usbvision/usbvision-video.c
@@ -483,7 +483,7 @@ static int vidioc_g_register(struct file *file, void *priv,
 }
 
 static int vidioc_s_register(struct file *file, void *priv,
-				struct v4l2_dbg_register *reg)
+				const struct v4l2_dbg_register *reg)
 {
 	struct usb_usbvision *usbvision = video_drvdata(file);
 	int err_code;
@@ -595,11 +595,11 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int input)
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *id)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct usb_usbvision *usbvision = video_drvdata(file);
 
-	usbvision->tvnorm_id = *id;
+	usbvision->tvnorm_id = id;
 
 	call_all(usbvision, core, s_std, usbvision->tvnorm_id);
 	/* propagate the change to the decoder */
@@ -628,7 +628,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *vt)
+				const struct v4l2_tuner *vt)
 {
 	struct usb_usbvision *usbvision = video_drvdata(file);
 
@@ -657,7 +657,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *freq)
+				const struct v4l2_frequency *freq)
 {
 	struct usb_usbvision *usbvision = video_drvdata(file);
 
diff --git a/drivers/media/usb/uvc/uvc_ctrl.c b/drivers/media/usb/uvc/uvc_ctrl.c
index 61e28dec991d..a2f4501c23ca 100644
--- a/drivers/media/usb/uvc/uvc_ctrl.c
+++ b/drivers/media/usb/uvc/uvc_ctrl.c
@@ -1487,7 +1487,7 @@ int uvc_ctrl_set(struct uvc_video_chain *chain,
 			step = mapping->get(mapping, UVC_GET_RES,
 					uvc_ctrl_data(ctrl, UVC_CTRL_DATA_RES));
 			if (!(step & value))
-				return -ERANGE;
+				return -EINVAL;
 		}
 
 		break;
diff --git a/drivers/media/usb/uvc/uvc_queue.c b/drivers/media/usb/uvc/uvc_queue.c
index 6c233a54ce40..cd962be860ca 100644
--- a/drivers/media/usb/uvc/uvc_queue.c
+++ b/drivers/media/usb/uvc/uvc_queue.c
@@ -149,6 +149,7 @@ int uvc_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type,
 	queue->queue.buf_struct_size = sizeof(struct uvc_buffer);
 	queue->queue.ops = &uvc_queue_qops;
 	queue->queue.mem_ops = &vb2_vmalloc_memops;
+	queue->queue.timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	ret = vb2_queue_init(&queue->queue);
 	if (ret)
 		return ret;
diff --git a/drivers/media/v4l2-core/Kconfig b/drivers/media/v4l2-core/Kconfig
index 976d029e9925..8c05565a240e 100644
--- a/drivers/media/v4l2-core/Kconfig
+++ b/drivers/media/v4l2-core/Kconfig
@@ -67,6 +67,7 @@ config VIDEOBUF2_MEMOPS
 
 config VIDEOBUF2_DMA_CONTIG
 	tristate
+	depends on HAS_DMA
 	select VIDEOBUF2_CORE
 	select VIDEOBUF2_MEMOPS
 	select DMA_SHARED_BUFFER
diff --git a/drivers/media/v4l2-core/Makefile b/drivers/media/v4l2-core/Makefile
index 768aaf62d5dc..aa50c46314b7 100644
--- a/drivers/media/v4l2-core/Makefile
+++ b/drivers/media/v4l2-core/Makefile
@@ -9,6 +9,9 @@ videodev-objs	:=	v4l2-dev.o v4l2-ioctl.o v4l2-device.o v4l2-fh.o \
 ifeq ($(CONFIG_COMPAT),y)
   videodev-objs += v4l2-compat-ioctl32.o
 endif
+ifeq ($(CONFIG_OF),y)
+  videodev-objs += v4l2-of.o
+endif
 
 obj-$(CONFIG_VIDEO_V4L2) += videodev.o
 obj-$(CONFIG_VIDEO_V4L2_INT_DEVICE) += v4l2-int-device.o
diff --git a/drivers/media/v4l2-core/tuner-core.c b/drivers/media/v4l2-core/tuner-core.c
index b5a8aac2e126..ddc9379eb276 100644
--- a/drivers/media/v4l2-core/tuner-core.c
+++ b/drivers/media/v4l2-core/tuner-core.c
@@ -132,7 +132,7 @@ struct tuner {
 	bool                standby;	/* Standby mode */
 
 	unsigned int        type; /* chip type id */
-	unsigned int        config;
+	void                *config;
 	const char          *name;
 };
 
@@ -218,26 +218,6 @@ static void fe_standby(struct dvb_frontend *fe)
 		fe_tuner_ops->sleep(fe);
 }
 
-static int fe_has_signal(struct dvb_frontend *fe)
-{
-	u16 strength = 0;
-
-	if (fe->ops.tuner_ops.get_rf_strength)
-		fe->ops.tuner_ops.get_rf_strength(fe, &strength);
-
-	return strength;
-}
-
-static int fe_get_afc(struct dvb_frontend *fe)
-{
-	s32 afc = 0;
-
-	if (fe->ops.tuner_ops.get_afc)
-		fe->ops.tuner_ops.get_afc(fe, &afc);
-
-	return 0;
-}
-
 static int fe_set_config(struct dvb_frontend *fe, void *priv_cfg)
 {
 	struct dvb_tuner_ops *fe_tuner_ops = &fe->ops.tuner_ops;
@@ -253,11 +233,9 @@ static int fe_set_config(struct dvb_frontend *fe, void *priv_cfg)
 
 static void tuner_status(struct dvb_frontend *fe);
 
-static struct analog_demod_ops tuner_analog_ops = {
+static const struct analog_demod_ops tuner_analog_ops = {
 	.set_params     = fe_set_params,
 	.standby        = fe_standby,
-	.has_signal     = fe_has_signal,
-	.get_afc        = fe_get_afc,
 	.set_config     = fe_set_config,
 	.tuner_status   = tuner_status
 };
@@ -272,9 +250,8 @@ static struct analog_demod_ops tuner_analog_ops = {
  * @c:			i2c_client descriptoy
  * @type:		type of the tuner (e. g. tuner number)
  * @new_mode_mask:	Indicates if tuner supports TV and/or Radio
- * @new_config:		an optional parameter ranging from 0-255 used by
-			a few tuners to adjust an internal parameter,
-			like LNA mode
+ * @new_config:		an optional parameter used by a few tuners to adjust
+			internal parameters, like LNA mode
  * @tuner_callback:	an optional function to be called when switching
  *			to analog mode
  *
@@ -282,7 +259,7 @@ static struct analog_demod_ops tuner_analog_ops = {
  * by tun_setup structure. It contains several per-tuner initialization "magic"
  */
 static void set_type(struct i2c_client *c, unsigned int type,
-		     unsigned int new_mode_mask, unsigned int new_config,
+		     unsigned int new_mode_mask, void *new_config,
 		     int (*tuner_callback) (void *dev, int component, int cmd, int arg))
 {
 	struct tuner *t = to_tuner(i2c_get_clientdata(c));
@@ -297,8 +274,7 @@ static void set_type(struct i2c_client *c, unsigned int type,
 	}
 
 	t->type = type;
-	/* prevent invalid config values */
-	t->config = new_config < 256 ? new_config : 0;
+	t->config = new_config;
 	if (tuner_callback != NULL) {
 		tuner_dbg("defining GPIO callback\n");
 		t->fe.callback = tuner_callback;
@@ -316,11 +292,8 @@ static void set_type(struct i2c_client *c, unsigned int type,
 		break;
 	case TUNER_PHILIPS_TDA8290:
 	{
-		struct tda829x_config cfg = {
-			.lna_cfg        = t->config,
-		};
 		if (!dvb_attach(tda829x_attach, &t->fe, t->i2c->adapter,
-				t->i2c->addr, &cfg))
+				t->i2c->addr, t->config))
 			goto attach_failed;
 		break;
 	}
@@ -409,7 +382,6 @@ static void set_type(struct i2c_client *c, unsigned int type,
 	case TUNER_NXP_TDA18271:
 	{
 		struct tda18271_config cfg = {
-			.config = t->config,
 			.small_i2c = TDA18271_03_BYTE_CHUNK_INIT,
 		};
 
@@ -453,6 +425,11 @@ static void set_type(struct i2c_client *c, unsigned int type,
 		memcpy(analog_ops, &tuner_analog_ops,
 		       sizeof(struct analog_demod_ops));
 
+		if (fe_tuner_ops->get_rf_strength)
+			analog_ops->has_signal = fe_tuner_ops->get_rf_strength;
+		if (fe_tuner_ops->get_afc)
+			analog_ops->get_afc = fe_tuner_ops->get_afc;
+
 	} else {
 		t->name = analog_ops->info.name;
 	}
@@ -506,7 +483,7 @@ static int tuner_s_type_addr(struct v4l2_subdev *sd,
 	struct tuner *t = to_tuner(sd);
 	struct i2c_client *c = v4l2_get_subdevdata(sd);
 
-	tuner_dbg("Calling set_type_addr for type=%d, addr=0x%02x, mode=0x%02x, config=0x%02x\n",
+	tuner_dbg("Calling set_type_addr for type=%d, addr=0x%02x, mode=0x%02x, config=%p\n",
 			tun_setup->type,
 			tun_setup->addr,
 			tun_setup->mode_mask,
@@ -1073,9 +1050,12 @@ static void tuner_status(struct dvb_frontend *fe)
 		if (tuner_status & TUNER_STATUS_STEREO)
 			tuner_info("Stereo:          yes\n");
 	}
-	if (analog_ops->has_signal)
-		tuner_info("Signal strength: %d\n",
-			   analog_ops->has_signal(fe));
+	if (analog_ops->has_signal) {
+		u16 signal;
+
+		if (!analog_ops->has_signal(fe, &signal))
+			tuner_info("Signal strength: %hu\n", signal);
+	}
 }
 
 /*
@@ -1134,7 +1114,7 @@ static int tuner_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
 	return 0;
 }
 
-static int tuner_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
+static int tuner_s_frequency(struct v4l2_subdev *sd, const struct v4l2_frequency *f)
 {
 	struct tuner *t = to_tuner(sd);
 
@@ -1193,9 +1173,13 @@ static int tuner_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	if (check_mode(t, vt->type) == -EINVAL)
 		return 0;
 	if (vt->type == t->mode && analog_ops->get_afc)
-		vt->afc = analog_ops->get_afc(&t->fe);
-	if (analog_ops->has_signal)
-		vt->signal = analog_ops->has_signal(&t->fe);
+		analog_ops->get_afc(&t->fe, &vt->afc);
+	if (vt->type == t->mode && analog_ops->has_signal) {
+		u16 signal = (u16)vt->signal;
+
+		if (!analog_ops->has_signal(&t->fe, &signal))
+			vt->signal = signal;
+	}
 	if (vt->type != V4L2_TUNER_RADIO) {
 		vt->capability |= V4L2_TUNER_CAP_NORM;
 		vt->rangelow = tv_range[0] * 16;
@@ -1233,7 +1217,7 @@ static int tuner_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
  * Note: vt->type should be initialized before calling it.
  * This is done by either video_ioctl2 or by the bridge driver.
  */
-static int tuner_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+static int tuner_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
 {
 	struct tuner *t = to_tuner(sd);
 
diff --git a/drivers/media/v4l2-core/v4l2-common.c b/drivers/media/v4l2-core/v4l2-common.c
index aa044f491666..3fed63f4e026 100644
--- a/drivers/media/v4l2-core/v4l2-common.c
+++ b/drivers/media/v4l2-core/v4l2-common.c
@@ -230,7 +230,7 @@ EXPORT_SYMBOL(v4l2_ctrl_next);
 int v4l2_chip_match_host(const struct v4l2_dbg_match *match)
 {
 	switch (match->type) {
-	case V4L2_CHIP_MATCH_HOST:
+	case V4L2_CHIP_MATCH_BRIDGE:
 		return match->addr == 0;
 	default:
 		return 0;
@@ -251,12 +251,11 @@ int v4l2_chip_match_i2c_client(struct i2c_client *c, const struct v4l2_dbg_match
 		if (c->driver == NULL || c->driver->driver.name == NULL)
 			return 0;
 		len = strlen(c->driver->driver.name);
-		/* legacy drivers have a ' suffix, don't try to match that */
-		if (len && c->driver->driver.name[len - 1] == '\'')
-			len--;
 		return len && !strncmp(c->driver->driver.name, match->name, len);
 	case V4L2_CHIP_MATCH_I2C_ADDR:
 		return c->addr == match->addr;
+	case V4L2_CHIP_MATCH_SUBDEV:
+		return 1;
 	default:
 		return 0;
 	}
@@ -551,53 +550,6 @@ void v4l_bound_align_image(u32 *w, unsigned int wmin, unsigned int wmax,
 EXPORT_SYMBOL_GPL(v4l_bound_align_image);
 
 /**
- * v4l_fill_dv_preset_info - fill description of a digital video preset
- * @preset - preset value
- * @info - pointer to struct v4l2_dv_enum_preset
- *
- * drivers can use this helper function to fill description of dv preset
- * in info.
- */
-int v4l_fill_dv_preset_info(u32 preset, struct v4l2_dv_enum_preset *info)
-{
-	static const struct v4l2_dv_preset_info {
-		u16 width;
-		u16 height;
-		const char *name;
-	} dv_presets[] = {
-		{ 0, 0, "Invalid" },		/* V4L2_DV_INVALID */
-		{ 720,  480, "480p@59.94" },	/* V4L2_DV_480P59_94 */
-		{ 720,  576, "576p@50" },	/* V4L2_DV_576P50 */
-		{ 1280, 720, "720p@24" },	/* V4L2_DV_720P24 */
-		{ 1280, 720, "720p@25" },	/* V4L2_DV_720P25 */
-		{ 1280, 720, "720p@30" },	/* V4L2_DV_720P30 */
-		{ 1280, 720, "720p@50" },	/* V4L2_DV_720P50 */
-		{ 1280, 720, "720p@59.94" },	/* V4L2_DV_720P59_94 */
-		{ 1280, 720, "720p@60" },	/* V4L2_DV_720P60 */
-		{ 1920, 1080, "1080i@29.97" },	/* V4L2_DV_1080I29_97 */
-		{ 1920, 1080, "1080i@30" },	/* V4L2_DV_1080I30 */
-		{ 1920, 1080, "1080i@25" },	/* V4L2_DV_1080I25 */
-		{ 1920, 1080, "1080i@50" },	/* V4L2_DV_1080I50 */
-		{ 1920, 1080, "1080i@60" },	/* V4L2_DV_1080I60 */
-		{ 1920, 1080, "1080p@24" },	/* V4L2_DV_1080P24 */
-		{ 1920, 1080, "1080p@25" },	/* V4L2_DV_1080P25 */
-		{ 1920, 1080, "1080p@30" },	/* V4L2_DV_1080P30 */
-		{ 1920, 1080, "1080p@50" },	/* V4L2_DV_1080P50 */
-		{ 1920, 1080, "1080p@60" },	/* V4L2_DV_1080P60 */
-	};
-
-	if (info == NULL || preset >= ARRAY_SIZE(dv_presets))
-		return -EINVAL;
-
-	info->preset = preset;
-	info->width = dv_presets[preset].width;
-	info->height = dv_presets[preset].height;
-	strlcpy(info->name, dv_presets[preset].name, sizeof(info->name));
-	return 0;
-}
-EXPORT_SYMBOL_GPL(v4l_fill_dv_preset_info);
-
-/**
  * v4l_match_dv_timings - check if two timings match
  * @t1 - compare this v4l2_dv_timings struct...
  * @t2 - with this struct.
diff --git a/drivers/media/v4l2-core/v4l2-compat-ioctl32.c b/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
index 7157af301b14..f1295519f285 100644
--- a/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
+++ b/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
@@ -1076,10 +1076,6 @@ long v4l2_compat_ioctl32(struct file *file, unsigned int cmd, unsigned long arg)
 	case VIDIOC_DBG_G_REGISTER:
 	case VIDIOC_DBG_G_CHIP_IDENT:
 	case VIDIOC_S_HW_FREQ_SEEK:
-	case VIDIOC_ENUM_DV_PRESETS:
-	case VIDIOC_S_DV_PRESET:
-	case VIDIOC_G_DV_PRESET:
-	case VIDIOC_QUERY_DV_PRESET:
 	case VIDIOC_S_DV_TIMINGS:
 	case VIDIOC_G_DV_TIMINGS:
 	case VIDIOC_DQEVENT:
diff --git a/drivers/media/v4l2-core/v4l2-ctrls.c b/drivers/media/v4l2-core/v4l2-ctrls.c
index 6b28b5800500..ebb8e48619a2 100644
--- a/drivers/media/v4l2-core/v4l2-ctrls.c
+++ b/drivers/media/v4l2-core/v4l2-ctrls.c
@@ -234,6 +234,7 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
 		"Average",
 		"Center Weighted",
 		"Spot",
+		"Matrix",
 		NULL
 	};
 	static const char * const camera_auto_focus_range[] = {
@@ -297,8 +298,8 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
 		"Text",
 		NULL
 	};
-	static const char * const tune_preemphasis[] = {
-		"No Preemphasis",
+	static const char * const tune_emphasis[] = {
+		"None",
 		"50 Microseconds",
 		"75 Microseconds",
 		NULL,
@@ -508,7 +509,9 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
 	case V4L2_CID_SCENE_MODE:
 		return scene_mode;
 	case V4L2_CID_TUNE_PREEMPHASIS:
-		return tune_preemphasis;
+		return tune_emphasis;
+	case V4L2_CID_TUNE_DEEMPHASIS:
+		return tune_emphasis;
 	case V4L2_CID_FLASH_LED_MODE:
 		return flash_led_mode;
 	case V4L2_CID_FLASH_STROBE_SOURCE:
@@ -695,6 +698,7 @@ const char *v4l2_ctrl_get_name(u32 id)
 	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";
+	case V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER:		return "Repeat Sequence Header";
 
 	/* CAMERA controls */
 	/* Keep the order of the 'case's the same as in videodev2.h! */
@@ -799,6 +803,9 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_DV_RX_POWER_PRESENT:	return "Power Present";
 	case V4L2_CID_DV_RX_RGB_RANGE:		return "Rx RGB Quantization Range";
 
+	case V4L2_CID_FM_RX_CLASS:		return "FM Radio Receiver Controls";
+	case V4L2_CID_TUNE_DEEMPHASIS:		return "De-Emphasis";
+	case V4L2_CID_RDS_RECEPTION:		return "RDS Reception";
 	default:
 		return NULL;
 	}
@@ -844,8 +851,10 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 	case V4L2_CID_MPEG_VIDEO_H264_8X8_TRANSFORM:
 	case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_ENABLE:
 	case V4L2_CID_MPEG_VIDEO_MPEG4_QPEL:
+	case V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER:
 	case V4L2_CID_WIDE_DYNAMIC_RANGE:
 	case V4L2_CID_IMAGE_STABILIZATION:
+	case V4L2_CID_RDS_RECEPTION:
 		*type = V4L2_CTRL_TYPE_BOOLEAN;
 		*min = 0;
 		*max = *step = 1;
@@ -904,6 +913,7 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 	case V4L2_CID_DV_TX_RGB_RANGE:
 	case V4L2_CID_DV_RX_RGB_RANGE:
 	case V4L2_CID_TEST_PATTERN:
+	case V4L2_CID_TUNE_DEEMPHASIS:
 		*type = V4L2_CTRL_TYPE_MENU;
 		break;
 	case V4L2_CID_LINK_FREQ:
@@ -926,6 +936,7 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 	case V4L2_CID_IMAGE_SOURCE_CLASS:
 	case V4L2_CID_IMAGE_PROC_CLASS:
 	case V4L2_CID_DV_CLASS:
+	case V4L2_CID_FM_RX_CLASS:
 		*type = V4L2_CTRL_TYPE_CTRL_CLASS;
 		/* You can neither read not write these */
 		*flags |= V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY;
@@ -1362,11 +1373,13 @@ static inline int handler_set_err(struct v4l2_ctrl_handler *hdl, int err)
 }
 
 /* Initialize the handler */
-int v4l2_ctrl_handler_init(struct v4l2_ctrl_handler *hdl,
-			   unsigned nr_of_controls_hint)
+int v4l2_ctrl_handler_init_class(struct v4l2_ctrl_handler *hdl,
+				 unsigned nr_of_controls_hint,
+				 struct lock_class_key *key, const char *name)
 {
 	hdl->lock = &hdl->_lock;
 	mutex_init(hdl->lock);
+	lockdep_set_class_and_name(hdl->lock, key, name);
 	INIT_LIST_HEAD(&hdl->ctrls);
 	INIT_LIST_HEAD(&hdl->ctrl_refs);
 	hdl->nr_of_buckets = 1 + nr_of_controls_hint / 8;
@@ -1375,7 +1388,7 @@ int v4l2_ctrl_handler_init(struct v4l2_ctrl_handler *hdl,
 	hdl->error = hdl->buckets ? 0 : -ENOMEM;
 	return hdl->error;
 }
-EXPORT_SYMBOL(v4l2_ctrl_handler_init);
+EXPORT_SYMBOL(v4l2_ctrl_handler_init_class);
 
 /* Free all controls and control refs */
 void v4l2_ctrl_handler_free(struct v4l2_ctrl_handler *hdl)
diff --git a/drivers/media/v4l2-core/v4l2-dev.c b/drivers/media/v4l2-core/v4l2-dev.c
index de1e9ab7db99..5923c5dfacd5 100644
--- a/drivers/media/v4l2-core/v4l2-dev.c
+++ b/drivers/media/v4l2-core/v4l2-dev.c
@@ -592,8 +592,9 @@ static void determine_valid_ioctls(struct video_device *vdev)
 	SET_VALID_IOCTL(ops, VIDIOC_S_FREQUENCY, vidioc_s_frequency);
 	SET_VALID_IOCTL(ops, VIDIOC_LOG_STATUS, vidioc_log_status);
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-	SET_VALID_IOCTL(ops, VIDIOC_DBG_G_REGISTER, vidioc_g_register);
-	SET_VALID_IOCTL(ops, VIDIOC_DBG_S_REGISTER, vidioc_s_register);
+	set_bit(_IOC_NR(VIDIOC_DBG_G_CHIP_INFO), valid_ioctls);
+	set_bit(_IOC_NR(VIDIOC_DBG_G_REGISTER), valid_ioctls);
+	set_bit(_IOC_NR(VIDIOC_DBG_S_REGISTER), valid_ioctls);
 #endif
 	SET_VALID_IOCTL(ops, VIDIOC_DBG_G_CHIP_IDENT, vidioc_g_chip_ident);
 	/* yes, really vidioc_subscribe_event */
@@ -685,7 +686,6 @@ static void determine_valid_ioctls(struct video_device *vdev)
 			SET_VALID_IOCTL(ops, VIDIOC_ENUMAUDIO, vidioc_enumaudio);
 			SET_VALID_IOCTL(ops, VIDIOC_G_AUDIO, vidioc_g_audio);
 			SET_VALID_IOCTL(ops, VIDIOC_S_AUDIO, vidioc_s_audio);
-			SET_VALID_IOCTL(ops, VIDIOC_QUERY_DV_PRESET, vidioc_query_dv_preset);
 			SET_VALID_IOCTL(ops, VIDIOC_QUERY_DV_TIMINGS, vidioc_query_dv_timings);
 		}
 		if (is_tx) {
@@ -708,9 +708,6 @@ static void determine_valid_ioctls(struct video_device *vdev)
 					(ops->vidioc_g_std || vdev->current_norm)))
 			set_bit(_IOC_NR(VIDIOC_G_PARM), valid_ioctls);
 		SET_VALID_IOCTL(ops, VIDIOC_S_PARM, vidioc_s_parm);
-		SET_VALID_IOCTL(ops, VIDIOC_ENUM_DV_PRESETS, vidioc_enum_dv_presets);
-		SET_VALID_IOCTL(ops, VIDIOC_S_DV_PRESET, vidioc_s_dv_preset);
-		SET_VALID_IOCTL(ops, VIDIOC_G_DV_PRESET, vidioc_g_dv_preset);
 		SET_VALID_IOCTL(ops, VIDIOC_S_DV_TIMINGS, vidioc_s_dv_timings);
 		SET_VALID_IOCTL(ops, VIDIOC_G_DV_TIMINGS, vidioc_g_dv_timings);
 		SET_VALID_IOCTL(ops, VIDIOC_ENUM_DV_TIMINGS, vidioc_enum_dv_timings);
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index aa6e7c788db2..f81bda1a48ec 100644
--- a/drivers/media/v4l2-core/v4l2-ioctl.c
+++ b/drivers/media/v4l2-core/v4l2-ioctl.c
@@ -35,6 +35,8 @@
 	memset((u8 *)(p) + offsetof(typeof(*(p)), field) + sizeof((p)->field), \
 	0, sizeof(*(p)) - offsetof(typeof(*(p)), field) - sizeof((p)->field))
 
+#define is_valid_ioctl(vfd, cmd) test_bit(_IOC_NR(cmd), (vfd)->valid_ioctls)
+
 struct std_descr {
 	v4l2_std_id std;
 	const char *descr;
@@ -167,9 +169,11 @@ static void v4l_print_querycap(const void *arg, bool write_only)
 {
 	const struct v4l2_capability *p = arg;
 
-	pr_cont("driver=%s, card=%s, bus=%s, version=0x%08x, "
+	pr_cont("driver=%.*s, card=%.*s, bus=%.*s, version=0x%08x, "
 		"capabilities=0x%08x, device_caps=0x%08x\n",
-		p->driver, p->card, p->bus_info,
+		(int)sizeof(p->driver), p->driver,
+		(int)sizeof(p->card), p->card,
+		(int)sizeof(p->bus_info), p->bus_info,
 		p->version, p->capabilities, p->device_caps);
 }
 
@@ -177,20 +181,21 @@ static void v4l_print_enuminput(const void *arg, bool write_only)
 {
 	const struct v4l2_input *p = arg;
 
-	pr_cont("index=%u, name=%s, type=%u, audioset=0x%x, tuner=%u, "
+	pr_cont("index=%u, name=%.*s, type=%u, audioset=0x%x, tuner=%u, "
 		"std=0x%08Lx, status=0x%x, capabilities=0x%x\n",
-		p->index, p->name, p->type, p->audioset, p->tuner,
-		(unsigned long long)p->std, p->status, p->capabilities);
+		p->index, (int)sizeof(p->name), p->name, p->type, p->audioset,
+		p->tuner, (unsigned long long)p->std, p->status,
+		p->capabilities);
 }
 
 static void v4l_print_enumoutput(const void *arg, bool write_only)
 {
 	const struct v4l2_output *p = arg;
 
-	pr_cont("index=%u, name=%s, type=%u, audioset=0x%x, "
+	pr_cont("index=%u, name=%.*s, type=%u, audioset=0x%x, "
 		"modulator=%u, std=0x%08Lx, capabilities=0x%x\n",
-		p->index, p->name, p->type, p->audioset, p->modulator,
-		(unsigned long long)p->std, p->capabilities);
+		p->index, (int)sizeof(p->name), p->name, p->type, p->audioset,
+		p->modulator, (unsigned long long)p->std, p->capabilities);
 }
 
 static void v4l_print_audio(const void *arg, bool write_only)
@@ -200,8 +205,9 @@ static void v4l_print_audio(const void *arg, bool write_only)
 	if (write_only)
 		pr_cont("index=%u, mode=0x%x\n", p->index, p->mode);
 	else
-		pr_cont("index=%u, name=%s, capability=0x%x, mode=0x%x\n",
-			p->index, p->name, p->capability, p->mode);
+		pr_cont("index=%u, name=%.*s, capability=0x%x, mode=0x%x\n",
+			p->index, (int)sizeof(p->name), p->name,
+			p->capability, p->mode);
 }
 
 static void v4l_print_audioout(const void *arg, bool write_only)
@@ -211,21 +217,22 @@ static void v4l_print_audioout(const void *arg, bool write_only)
 	if (write_only)
 		pr_cont("index=%u\n", p->index);
 	else
-		pr_cont("index=%u, name=%s, capability=0x%x, mode=0x%x\n",
-			p->index, p->name, p->capability, p->mode);
+		pr_cont("index=%u, name=%.*s, capability=0x%x, mode=0x%x\n",
+			p->index, (int)sizeof(p->name), p->name,
+			p->capability, p->mode);
 }
 
 static void v4l_print_fmtdesc(const void *arg, bool write_only)
 {
 	const struct v4l2_fmtdesc *p = arg;
 
-	pr_cont("index=%u, type=%s, flags=0x%x, pixelformat=%c%c%c%c, description='%s'\n",
+	pr_cont("index=%u, type=%s, flags=0x%x, pixelformat=%c%c%c%c, description='%.*s'\n",
 		p->index, prt_names(p->type, v4l2_type_names),
 		p->flags, (p->pixelformat & 0xff),
 		(p->pixelformat >>  8) & 0xff,
 		(p->pixelformat >> 16) & 0xff,
 		(p->pixelformat >> 24) & 0xff,
-		p->description);
+		(int)sizeof(p->description), p->description);
 }
 
 static void v4l_print_format(const void *arg, bool write_only)
@@ -348,9 +355,9 @@ static void v4l_print_modulator(const void *arg, bool write_only)
 	if (write_only)
 		pr_cont("index=%u, txsubchans=0x%x", p->index, p->txsubchans);
 	else
-		pr_cont("index=%u, name=%s, capability=0x%x, "
+		pr_cont("index=%u, name=%.*s, capability=0x%x, "
 			"rangelow=%u, rangehigh=%u, txsubchans=0x%x\n",
-			p->index, p->name, p->capability,
+			p->index, (int)sizeof(p->name), p->name, p->capability,
 			p->rangelow, p->rangehigh, p->txsubchans);
 }
 
@@ -361,10 +368,10 @@ static void v4l_print_tuner(const void *arg, bool write_only)
 	if (write_only)
 		pr_cont("index=%u, audmode=%u\n", p->index, p->audmode);
 	else
-		pr_cont("index=%u, name=%s, type=%u, capability=0x%x, "
+		pr_cont("index=%u, name=%.*s, type=%u, capability=0x%x, "
 			"rangelow=%u, rangehigh=%u, signal=%u, afc=%d, "
 			"rxsubchans=0x%x, audmode=%u\n",
-			p->index, p->name, p->type,
+			p->index, (int)sizeof(p->name), p->name, p->type,
 			p->capability, p->rangelow,
 			p->rangehigh, p->signal, p->afc,
 			p->rxsubchans, p->audmode);
@@ -382,9 +389,9 @@ static void v4l_print_standard(const void *arg, bool write_only)
 {
 	const struct v4l2_standard *p = arg;
 
-	pr_cont("index=%u, id=0x%Lx, name=%s, fps=%u/%u, "
+	pr_cont("index=%u, id=0x%Lx, name=%.*s, fps=%u/%u, "
 		"framelines=%u\n", p->index,
-		(unsigned long long)p->id, p->name,
+		(unsigned long long)p->id, (int)sizeof(p->name), p->name,
 		p->frameperiod.numerator,
 		p->frameperiod.denominator,
 		p->framelines);
@@ -504,9 +511,9 @@ static void v4l_print_queryctrl(const void *arg, bool write_only)
 {
 	const struct v4l2_queryctrl *p = arg;
 
-	pr_cont("id=0x%x, type=%d, name=%s, min/max=%d/%d, "
+	pr_cont("id=0x%x, type=%d, name=%.*s, min/max=%d/%d, "
 		"step=%d, default=%d, flags=0x%08x\n",
-			p->id, p->type, p->name,
+			p->id, p->type, (int)sizeof(p->name), p->name,
 			p->minimum, p->maximum,
 			p->step, p->default_value, p->flags);
 }
@@ -623,39 +630,39 @@ static void v4l_print_dbg_chip_ident(const void *arg, bool write_only)
 
 	pr_cont("type=%u, ", p->match.type);
 	if (p->match.type == V4L2_CHIP_MATCH_I2C_DRIVER)
-		pr_cont("name=%s, ", p->match.name);
+		pr_cont("name=%.*s, ",
+				(int)sizeof(p->match.name), p->match.name);
 	else
 		pr_cont("addr=%u, ", p->match.addr);
 	pr_cont("chip_ident=%u, revision=0x%x\n",
 			p->ident, p->revision);
 }
 
-static void v4l_print_dbg_register(const void *arg, bool write_only)
+static void v4l_print_dbg_chip_info(const void *arg, bool write_only)
 {
-	const struct v4l2_dbg_register *p = arg;
+	const struct v4l2_dbg_chip_info *p = arg;
 
 	pr_cont("type=%u, ", p->match.type);
 	if (p->match.type == V4L2_CHIP_MATCH_I2C_DRIVER)
-		pr_cont("name=%s, ", p->match.name);
+		pr_cont("name=%.*s, ",
+				(int)sizeof(p->match.name), p->match.name);
 	else
 		pr_cont("addr=%u, ", p->match.addr);
-	pr_cont("reg=0x%llx, val=0x%llx\n",
-			p->reg, p->val);
+	pr_cont("name=%.*s\n", (int)sizeof(p->name), p->name);
 }
 
-static void v4l_print_dv_enum_presets(const void *arg, bool write_only)
-{
-	const struct v4l2_dv_enum_preset *p = arg;
-
-	pr_cont("index=%u, preset=%u, name=%s, width=%u, height=%u\n",
-			p->index, p->preset, p->name, p->width, p->height);
-}
-
-static void v4l_print_dv_preset(const void *arg, bool write_only)
+static void v4l_print_dbg_register(const void *arg, bool write_only)
 {
-	const struct v4l2_dv_preset *p = arg;
+	const struct v4l2_dbg_register *p = arg;
 
-	pr_cont("preset=%u\n", p->preset);
+	pr_cont("type=%u, ", p->match.type);
+	if (p->match.type == V4L2_CHIP_MATCH_I2C_DRIVER)
+		pr_cont("name=%.*s, ",
+				(int)sizeof(p->match.name), p->match.name);
+	else
+		pr_cont("addr=%u, ", p->match.addr);
+	pr_cont("reg=0x%llx, val=0x%llx\n",
+			p->reg, p->val);
 }
 
 static void v4l_print_dv_timings(const void *arg, bool write_only)
@@ -997,20 +1004,17 @@ static int v4l_s_priority(const struct v4l2_ioctl_ops *ops,
 static int v4l_enuminput(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
+	struct video_device *vfd = video_devdata(file);
 	struct v4l2_input *p = arg;
 
 	/*
-	 * We set the flags for CAP_PRESETS, CAP_DV_TIMINGS &
+	 * We set the flags for CAP_DV_TIMINGS &
 	 * CAP_STD here based on ioctl handler provided by the
 	 * driver. If the driver doesn't support these
 	 * for a specific input, it must override these flags.
 	 */
-	if (ops->vidioc_s_std)
+	if (is_valid_ioctl(vfd, VIDIOC_S_STD))
 		p->capabilities |= V4L2_IN_CAP_STD;
-	if (ops->vidioc_s_dv_preset)
-		p->capabilities |= V4L2_IN_CAP_PRESETS;
-	if (ops->vidioc_s_dv_timings)
-		p->capabilities |= V4L2_IN_CAP_DV_TIMINGS;
 
 	return ops->vidioc_enum_input(file, fh, p);
 }
@@ -1018,20 +1022,17 @@ static int v4l_enuminput(const struct v4l2_ioctl_ops *ops,
 static int v4l_enumoutput(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
+	struct video_device *vfd = video_devdata(file);
 	struct v4l2_output *p = arg;
 
 	/*
-	 * We set the flags for CAP_PRESETS, CAP_DV_TIMINGS &
+	 * We set the flags for CAP_DV_TIMINGS &
 	 * CAP_STD here based on ioctl handler provided by the
 	 * driver. If the driver doesn't support these
 	 * for a specific output, it must override these flags.
 	 */
-	if (ops->vidioc_s_std)
+	if (is_valid_ioctl(vfd, VIDIOC_S_STD))
 		p->capabilities |= V4L2_OUT_CAP_STD;
-	if (ops->vidioc_s_dv_preset)
-		p->capabilities |= V4L2_OUT_CAP_PRESETS;
-	if (ops->vidioc_s_dv_timings)
-		p->capabilities |= V4L2_OUT_CAP_DV_TIMINGS;
 
 	return ops->vidioc_enum_output(file, fh, p);
 }
@@ -1316,7 +1317,7 @@ static int v4l_s_frequency(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
 	struct video_device *vfd = video_devdata(file);
-	struct v4l2_frequency *p = arg;
+	const struct v4l2_frequency *p = arg;
 	enum v4l2_tuner_type type;
 
 	type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
@@ -1383,15 +1384,15 @@ static int v4l_s_std(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
 	struct video_device *vfd = video_devdata(file);
-	v4l2_std_id *id = arg, norm;
+	v4l2_std_id id = *(v4l2_std_id *)arg, norm;
 	int ret;
 
-	norm = (*id) & vfd->tvnorms;
+	norm = id & vfd->tvnorms;
 	if (vfd->tvnorms && !norm)	/* Check if std is supported */
 		return -EINVAL;
 
 	/* Calls the specific handler */
-	ret = ops->vidioc_s_std(file, fh, &norm);
+	ret = ops->vidioc_s_std(file, fh, norm);
 
 	/* Updates standard information */
 	if (ret >= 0)
@@ -1513,7 +1514,7 @@ static int v4l_g_parm(const struct v4l2_ioctl_ops *ops,
 	    p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 		return -EINVAL;
 	p->parm.capture.readbuffers = 2;
-	if (ops->vidioc_g_std)
+	if (is_valid_ioctl(vfd, VIDIOC_G_STD) && ops->vidioc_g_std)
 		ret = ops->vidioc_g_std(file, fh, &std);
 	if (ret == 0)
 		v4l2_video_std_frame_period(std,
@@ -1792,10 +1793,23 @@ static int v4l_dbg_g_register(const struct v4l2_ioctl_ops *ops,
 {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	struct v4l2_dbg_register *p = arg;
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_subdev *sd;
+	int idx = 0;
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
-	return ops->vidioc_g_register(file, fh, p);
+	if (p->match.type == V4L2_CHIP_MATCH_SUBDEV) {
+		if (vfd->v4l2_dev == NULL)
+			return -EINVAL;
+		v4l2_device_for_each_subdev(sd, vfd->v4l2_dev)
+			if (p->match.addr == idx++)
+				return v4l2_subdev_call(sd, core, g_register, p);
+		return -EINVAL;
+	}
+	if (ops->vidioc_g_register)
+		return ops->vidioc_g_register(file, fh, p);
+	return -EINVAL;
 #else
 	return -ENOTTY;
 #endif
@@ -1805,11 +1819,24 @@ static int v4l_dbg_s_register(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
-	struct v4l2_dbg_register *p = arg;
+	const struct v4l2_dbg_register *p = arg;
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_subdev *sd;
+	int idx = 0;
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
-	return ops->vidioc_s_register(file, fh, p);
+	if (p->match.type == V4L2_CHIP_MATCH_SUBDEV) {
+		if (vfd->v4l2_dev == NULL)
+			return -EINVAL;
+		v4l2_device_for_each_subdev(sd, vfd->v4l2_dev)
+			if (p->match.addr == idx++)
+				return v4l2_subdev_call(sd, core, s_register, p);
+		return -EINVAL;
+	}
+	if (ops->vidioc_s_register)
+		return ops->vidioc_s_register(file, fh, p);
+	return -EINVAL;
 #else
 	return -ENOTTY;
 #endif
@@ -1822,9 +1849,59 @@ static int v4l_dbg_g_chip_ident(const struct v4l2_ioctl_ops *ops,
 
 	p->ident = V4L2_IDENT_NONE;
 	p->revision = 0;
+	if (p->match.type == V4L2_CHIP_MATCH_SUBDEV)
+		return -EINVAL;
 	return ops->vidioc_g_chip_ident(file, fh, p);
 }
 
+static int v4l_dbg_g_chip_info(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_dbg_chip_info *p = arg;
+	struct v4l2_subdev *sd;
+	int idx = 0;
+
+	switch (p->match.type) {
+	case V4L2_CHIP_MATCH_BRIDGE:
+		if (ops->vidioc_s_register)
+			p->flags |= V4L2_CHIP_FL_WRITABLE;
+		if (ops->vidioc_g_register)
+			p->flags |= V4L2_CHIP_FL_READABLE;
+		if (vfd->v4l2_dev)
+			strlcpy(p->name, vfd->v4l2_dev->name, sizeof(p->name));
+		else if (vfd->parent)
+			strlcpy(p->name, vfd->parent->driver->name, sizeof(p->name));
+		else
+			strlcpy(p->name, "bridge", sizeof(p->name));
+		if (ops->vidioc_g_chip_info)
+			return ops->vidioc_g_chip_info(file, fh, arg);
+		if (p->match.addr)
+			return -EINVAL;
+		return 0;
+
+	case V4L2_CHIP_MATCH_SUBDEV:
+		if (vfd->v4l2_dev == NULL)
+			break;
+		v4l2_device_for_each_subdev(sd, vfd->v4l2_dev) {
+			if (p->match.addr != idx++)
+				continue;
+			if (sd->ops->core && sd->ops->core->s_register)
+				p->flags |= V4L2_CHIP_FL_WRITABLE;
+			if (sd->ops->core && sd->ops->core->g_register)
+				p->flags |= V4L2_CHIP_FL_READABLE;
+			strlcpy(p->name, sd->name, sizeof(p->name));
+			return 0;
+		}
+		break;
+	}
+	return -EINVAL;
+#else
+	return -ENOTTY;
+#endif
+}
+
 static int v4l_dqevent(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
@@ -1873,7 +1950,7 @@ static int v4l_enum_freq_bands(const struct v4l2_ioctl_ops *ops,
 		return -EINVAL;
 	if (ops->vidioc_enum_freq_bands)
 		return ops->vidioc_enum_freq_bands(file, fh, p);
-	if (ops->vidioc_g_tuner) {
+	if (is_valid_ioctl(vfd, VIDIOC_G_TUNER)) {
 		struct v4l2_tuner t = {
 			.index = p->tuner,
 			.type = type,
@@ -1891,7 +1968,7 @@ static int v4l_enum_freq_bands(const struct v4l2_ioctl_ops *ops,
 			V4L2_BAND_MODULATION_FM : V4L2_BAND_MODULATION_VSB;
 		return 0;
 	}
-	if (ops->vidioc_g_modulator) {
+	if (is_valid_ioctl(vfd, VIDIOC_G_MODULATOR)) {
 		struct v4l2_modulator m = {
 			.index = p->tuner,
 		};
@@ -2028,10 +2105,6 @@ static struct v4l2_ioctl_info v4l2_ioctls[] = {
 	IOCTL_INFO_FNC(VIDIOC_DBG_G_REGISTER, v4l_dbg_g_register, v4l_print_dbg_register, 0),
 	IOCTL_INFO_FNC(VIDIOC_DBG_G_CHIP_IDENT, v4l_dbg_g_chip_ident, v4l_print_dbg_chip_ident, 0),
 	IOCTL_INFO_FNC(VIDIOC_S_HW_FREQ_SEEK, v4l_s_hw_freq_seek, v4l_print_hw_freq_seek, INFO_FL_PRIO),
-	IOCTL_INFO_STD(VIDIOC_ENUM_DV_PRESETS, vidioc_enum_dv_presets, v4l_print_dv_enum_presets, 0),
-	IOCTL_INFO_STD(VIDIOC_S_DV_PRESET, vidioc_s_dv_preset, v4l_print_dv_preset, INFO_FL_PRIO),
-	IOCTL_INFO_STD(VIDIOC_G_DV_PRESET, vidioc_g_dv_preset, v4l_print_dv_preset, 0),
-	IOCTL_INFO_STD(VIDIOC_QUERY_DV_PRESET, vidioc_query_dv_preset, v4l_print_dv_preset, 0),
 	IOCTL_INFO_STD(VIDIOC_S_DV_TIMINGS, vidioc_s_dv_timings, v4l_print_dv_timings, INFO_FL_PRIO),
 	IOCTL_INFO_STD(VIDIOC_G_DV_TIMINGS, vidioc_g_dv_timings, v4l_print_dv_timings, 0),
 	IOCTL_INFO_FNC(VIDIOC_DQEVENT, v4l_dqevent, v4l_print_event, 0),
@@ -2043,6 +2116,7 @@ static struct v4l2_ioctl_info v4l2_ioctls[] = {
 	IOCTL_INFO_STD(VIDIOC_QUERY_DV_TIMINGS, vidioc_query_dv_timings, v4l_print_dv_timings, 0),
 	IOCTL_INFO_STD(VIDIOC_DV_TIMINGS_CAP, vidioc_dv_timings_cap, v4l_print_dv_timings_cap, INFO_FL_CLEAR(v4l2_dv_timings_cap, type)),
 	IOCTL_INFO_FNC(VIDIOC_ENUM_FREQ_BANDS, v4l_enum_freq_bands, v4l_print_freq_band, 0),
+	IOCTL_INFO_FNC(VIDIOC_DBG_G_CHIP_INFO, v4l_dbg_g_chip_info, v4l_print_dbg_chip_info, INFO_FL_CLEAR(v4l2_dbg_chip_info, match)),
 };
 #define V4L2_IOCTLS ARRAY_SIZE(v4l2_ioctls)
 
@@ -2147,11 +2221,6 @@ static long __video_do_ioctl(struct file *file,
 	}
 
 	write_only = _IOC_DIR(cmd) == _IOC_WRITE;
-	if (write_only && debug > V4L2_DEBUG_IOCTL) {
-		v4l_printk_ioctl(video_device_node_name(vfd), cmd);
-		pr_cont(": ");
-		info->debug(arg, write_only);
-	}
 	if (info->flags & INFO_FL_STD) {
 		typedef int (*vidioc_op)(struct file *file, void *fh, void *p);
 		const void *p = vfd->ioctl_ops;
@@ -2170,16 +2239,10 @@ static long __video_do_ioctl(struct file *file,
 
 done:
 	if (debug) {
-		if (write_only && debug > V4L2_DEBUG_IOCTL) {
-			if (ret < 0)
-				printk(KERN_DEBUG "%s: error %ld\n",
-					video_device_node_name(vfd), ret);
-			return ret;
-		}
 		v4l_printk_ioctl(video_device_node_name(vfd), cmd);
 		if (ret < 0)
-			pr_cont(": error %ld\n", ret);
-		else if (debug == V4L2_DEBUG_IOCTL)
+			pr_cont(": error %ld", ret);
+		if (debug == V4L2_DEBUG_IOCTL)
 			pr_cont("\n");
 		else if (_IOC_DIR(cmd) == _IOC_NONE)
 			info->debug(arg, write_only);
diff --git a/drivers/media/v4l2-core/v4l2-mem2mem.c b/drivers/media/v4l2-core/v4l2-mem2mem.c
index da99cf727162..66f599fcb829 100644
--- a/drivers/media/v4l2-core/v4l2-mem2mem.c
+++ b/drivers/media/v4l2-core/v4l2-mem2mem.c
@@ -230,12 +230,15 @@ static void v4l2_m2m_try_schedule(struct v4l2_m2m_ctx *m2m_ctx)
 		dprintk("No input buffers available\n");
 		return;
 	}
+	spin_lock_irqsave(&m2m_ctx->cap_q_ctx.rdy_spinlock, flags);
 	if (list_empty(&m2m_ctx->cap_q_ctx.rdy_queue)) {
+		spin_unlock_irqrestore(&m2m_ctx->cap_q_ctx.rdy_spinlock, flags);
 		spin_unlock_irqrestore(&m2m_ctx->out_q_ctx.rdy_spinlock, flags);
 		spin_unlock_irqrestore(&m2m_dev->job_spinlock, flags_job);
 		dprintk("No output buffers available\n");
 		return;
 	}
+	spin_unlock_irqrestore(&m2m_ctx->cap_q_ctx.rdy_spinlock, flags);
 	spin_unlock_irqrestore(&m2m_ctx->out_q_ctx.rdy_spinlock, flags);
 
 	if (m2m_dev->m2m_ops->job_ready
@@ -405,10 +408,35 @@ EXPORT_SYMBOL_GPL(v4l2_m2m_streamon);
 int v4l2_m2m_streamoff(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		       enum v4l2_buf_type type)
 {
-	struct vb2_queue *vq;
+	struct v4l2_m2m_dev *m2m_dev;
+	struct v4l2_m2m_queue_ctx *q_ctx;
+	unsigned long flags_job, flags;
+	int ret;
 
-	vq = v4l2_m2m_get_vq(m2m_ctx, type);
-	return vb2_streamoff(vq, type);
+	q_ctx = get_queue_ctx(m2m_ctx, type);
+	ret = vb2_streamoff(&q_ctx->q, type);
+	if (ret)
+		return ret;
+
+	m2m_dev = m2m_ctx->m2m_dev;
+	spin_lock_irqsave(&m2m_dev->job_spinlock, flags_job);
+	/* We should not be scheduled anymore, since we're dropping a queue. */
+	INIT_LIST_HEAD(&m2m_ctx->queue);
+	m2m_ctx->job_flags = 0;
+
+	spin_lock_irqsave(&q_ctx->rdy_spinlock, flags);
+	/* Drop queue, since streamoff returns device to the same state as after
+	 * calling reqbufs. */
+	INIT_LIST_HEAD(&q_ctx->rdy_queue);
+	spin_unlock_irqrestore(&q_ctx->rdy_spinlock, flags);
+
+	if (m2m_dev->curr_ctx == m2m_ctx) {
+		m2m_dev->curr_ctx = NULL;
+		wake_up(&m2m_ctx->finished);
+	}
+	spin_unlock_irqrestore(&m2m_dev->job_spinlock, flags_job);
+
+	return 0;
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_streamoff);
 
diff --git a/drivers/media/v4l2-core/v4l2-of.c b/drivers/media/v4l2-core/v4l2-of.c
new file mode 100644
index 000000000000..aa59639d013c
--- /dev/null
+++ b/drivers/media/v4l2-core/v4l2-of.c
@@ -0,0 +1,266 @@
+/*
+ * V4L2 OF binding parsing library
+ *
+ * Copyright (C) 2012 - 2013 Samsung Electronics Co., Ltd.
+ * Author: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * Copyright (C) 2012 Renesas Electronics Corp.
+ * Author: Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/string.h>
+#include <linux/types.h>
+
+#include <media/v4l2-of.h>
+
+static void v4l2_of_parse_csi_bus(const struct device_node *node,
+				  struct v4l2_of_endpoint *endpoint)
+{
+	struct v4l2_of_bus_mipi_csi2 *bus = &endpoint->bus.mipi_csi2;
+	u32 data_lanes[ARRAY_SIZE(bus->data_lanes)];
+	struct property *prop;
+	bool have_clk_lane = false;
+	unsigned int flags = 0;
+	u32 v;
+
+	prop = of_find_property(node, "data-lanes", NULL);
+	if (prop) {
+		const __be32 *lane = NULL;
+		int i;
+
+		for (i = 0; i < ARRAY_SIZE(data_lanes); i++) {
+			lane = of_prop_next_u32(prop, lane, &data_lanes[i]);
+			if (!lane)
+				break;
+		}
+		bus->num_data_lanes = i;
+		while (i--)
+			bus->data_lanes[i] = data_lanes[i];
+	}
+
+	if (!of_property_read_u32(node, "clock-lanes", &v)) {
+		bus->clock_lane = v;
+		have_clk_lane = true;
+	}
+
+	if (of_get_property(node, "clock-noncontinuous", &v))
+		flags |= V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
+	else if (have_clk_lane || bus->num_data_lanes > 0)
+		flags |= V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
+
+	bus->flags = flags;
+	endpoint->bus_type = V4L2_MBUS_CSI2;
+}
+
+static void v4l2_of_parse_parallel_bus(const struct device_node *node,
+				       struct v4l2_of_endpoint *endpoint)
+{
+	struct v4l2_of_bus_parallel *bus = &endpoint->bus.parallel;
+	unsigned int flags = 0;
+	u32 v;
+
+	if (!of_property_read_u32(node, "hsync-active", &v))
+		flags |= v ? V4L2_MBUS_HSYNC_ACTIVE_HIGH :
+			V4L2_MBUS_HSYNC_ACTIVE_LOW;
+
+	if (!of_property_read_u32(node, "vsync-active", &v))
+		flags |= v ? V4L2_MBUS_VSYNC_ACTIVE_HIGH :
+			V4L2_MBUS_VSYNC_ACTIVE_LOW;
+
+	if (!of_property_read_u32(node, "pclk-sample", &v))
+		flags |= v ? V4L2_MBUS_PCLK_SAMPLE_RISING :
+			V4L2_MBUS_PCLK_SAMPLE_FALLING;
+
+	if (!of_property_read_u32(node, "field-even-active", &v))
+		flags |= v ? V4L2_MBUS_FIELD_EVEN_HIGH :
+			V4L2_MBUS_FIELD_EVEN_LOW;
+	if (flags)
+		endpoint->bus_type = V4L2_MBUS_PARALLEL;
+	else
+		endpoint->bus_type = V4L2_MBUS_BT656;
+
+	if (!of_property_read_u32(node, "data-active", &v))
+		flags |= v ? V4L2_MBUS_DATA_ACTIVE_HIGH :
+			V4L2_MBUS_DATA_ACTIVE_LOW;
+
+	if (of_get_property(node, "slave-mode", &v))
+		flags |= V4L2_MBUS_SLAVE;
+	else
+		flags |= V4L2_MBUS_MASTER;
+
+	if (!of_property_read_u32(node, "bus-width", &v))
+		bus->bus_width = v;
+
+	if (!of_property_read_u32(node, "data-shift", &v))
+		bus->data_shift = v;
+
+	bus->flags = flags;
+
+}
+
+/**
+ * v4l2_of_parse_endpoint() - parse all endpoint node properties
+ * @node: pointer to endpoint device_node
+ * @endpoint: pointer to the V4L2 OF endpoint data structure
+ *
+ * All properties are optional. If none are found, we don't set any flags.
+ * This means the port has a static configuration and no properties have
+ * to be specified explicitly.
+ * If any properties that identify the bus as parallel are found and
+ * slave-mode isn't set, we set V4L2_MBUS_MASTER. Similarly, if we recognise
+ * the bus as serial CSI-2 and clock-noncontinuous isn't set, we set the
+ * V4L2_MBUS_CSI2_CONTINUOUS_CLOCK flag.
+ * The caller should hold a reference to @node.
+ */
+void v4l2_of_parse_endpoint(const struct device_node *node,
+			    struct v4l2_of_endpoint *endpoint)
+{
+	struct device_node *port_node = of_get_parent(node);
+
+	memset(endpoint, 0, offsetof(struct v4l2_of_endpoint, head));
+
+	endpoint->local_node = node;
+	/*
+	 * It doesn't matter whether the two calls below succeed.
+	 * If they don't then the default value 0 is used.
+	 */
+	of_property_read_u32(port_node, "reg", &endpoint->port);
+	of_property_read_u32(node, "reg", &endpoint->id);
+
+	v4l2_of_parse_csi_bus(node, endpoint);
+	/*
+	 * Parse the parallel video bus properties only if none
+	 * of the MIPI CSI-2 specific properties were found.
+	 */
+	if (endpoint->bus.mipi_csi2.flags == 0)
+		v4l2_of_parse_parallel_bus(node, endpoint);
+
+	of_node_put(port_node);
+}
+EXPORT_SYMBOL(v4l2_of_parse_endpoint);
+
+/**
+ * v4l2_of_get_next_endpoint() - get next endpoint node
+ * @parent: pointer to the parent device node
+ * @prev: previous endpoint node, or NULL to get first
+ *
+ * Return: An 'endpoint' node pointer with refcount incremented. Refcount
+ * of the passed @prev node is not decremented, the caller have to use
+ * of_node_put() on it when done.
+ */
+struct device_node *v4l2_of_get_next_endpoint(const struct device_node *parent,
+					struct device_node *prev)
+{
+	struct device_node *endpoint;
+	struct device_node *port = NULL;
+
+	if (!parent)
+		return NULL;
+
+	if (!prev) {
+		struct device_node *node;
+		/*
+		 * It's the first call, we have to find a port subnode
+		 * within this node or within an optional 'ports' node.
+		 */
+		node = of_get_child_by_name(parent, "ports");
+		if (node)
+			parent = node;
+
+		for_each_child_of_node(parent, node) {
+			if (!of_node_cmp(node->name, "port")) {
+				port = node;
+				break;
+			}
+		}
+		if (port) {
+			/* Found a port, get an endpoint. */
+			endpoint = of_get_next_child(port, NULL);
+			of_node_put(port);
+		} else {
+			endpoint = NULL;
+		}
+
+		if (!endpoint)
+			pr_err("%s(): no endpoint nodes specified for %s\n",
+			       __func__, parent->full_name);
+	} else {
+		port = of_get_parent(prev);
+		if (!port)
+			/* Hm, has someone given us the root node ?... */
+			return NULL;
+
+		/* Avoid dropping prev node refcount to 0. */
+		of_node_get(prev);
+		endpoint = of_get_next_child(port, prev);
+		if (endpoint) {
+			of_node_put(port);
+			return endpoint;
+		}
+
+		/* No more endpoints under this port, try the next one. */
+		do {
+			port = of_get_next_child(parent, port);
+			if (!port)
+				return NULL;
+		} while (of_node_cmp(port->name, "port"));
+
+		/* Pick up the first endpoint in this port. */
+		endpoint = of_get_next_child(port, NULL);
+		of_node_put(port);
+	}
+
+	return endpoint;
+}
+EXPORT_SYMBOL(v4l2_of_get_next_endpoint);
+
+/**
+ * v4l2_of_get_remote_port_parent() - get remote port's parent node
+ * @node: pointer to a local endpoint device_node
+ *
+ * Return: Remote device node associated with remote endpoint node linked
+ *	   to @node. Use of_node_put() on it when done.
+ */
+struct device_node *v4l2_of_get_remote_port_parent(
+			       const struct device_node *node)
+{
+	struct device_node *np;
+	unsigned int depth;
+
+	/* Get remote endpoint node. */
+	np = of_parse_phandle(node, "remote-endpoint", 0);
+
+	/* Walk 3 levels up only if there is 'ports' node. */
+	for (depth = 3; depth && np; depth--) {
+		np = of_get_next_parent(np);
+		if (depth == 2 && of_node_cmp(np->name, "ports"))
+			break;
+	}
+	return np;
+}
+EXPORT_SYMBOL(v4l2_of_get_remote_port_parent);
+
+/**
+ * v4l2_of_get_remote_port() - get remote port node
+ * @node: pointer to a local endpoint device_node
+ *
+ * Return: Remote port node associated with remote endpoint node linked
+ *	   to @node. Use of_node_put() on it when done.
+ */
+struct device_node *v4l2_of_get_remote_port(const struct device_node *node)
+{
+	struct device_node *np;
+
+	/* Get remote endpoint node. */
+	np = of_parse_phandle(node, "remote-endpoint", 0);
+	if (!np)
+		return NULL;
+	return of_get_parent(np);
+}
+EXPORT_SYMBOL(v4l2_of_get_remote_port);
diff --git a/drivers/media/v4l2-core/videobuf-dma-contig.c b/drivers/media/v4l2-core/videobuf-dma-contig.c
index 3a43ba0959bf..67f572c3fba2 100644
--- a/drivers/media/v4l2-core/videobuf-dma-contig.c
+++ b/drivers/media/v4l2-core/videobuf-dma-contig.c
@@ -27,7 +27,6 @@ struct videobuf_dma_contig_memory {
 	u32 magic;
 	void *vaddr;
 	dma_addr_t dma_handle;
-	bool cached;
 	unsigned long size;
 };
 
@@ -43,26 +42,8 @@ static int __videobuf_dc_alloc(struct device *dev,
 			       unsigned long size, gfp_t flags)
 {
 	mem->size = size;
-	if (mem->cached) {
-		mem->vaddr = alloc_pages_exact(mem->size, flags | GFP_DMA);
-		if (mem->vaddr) {
-			int err;
-
-			mem->dma_handle = dma_map_single(dev, mem->vaddr,
-							 mem->size,
-							 DMA_FROM_DEVICE);
-			err = dma_mapping_error(dev, mem->dma_handle);
-			if (err) {
-				dev_err(dev, "dma_map_single failed\n");
-
-				free_pages_exact(mem->vaddr, mem->size);
-				mem->vaddr = NULL;
-				return err;
-			}
-		}
-	} else
-		mem->vaddr = dma_alloc_coherent(dev, mem->size,
-						&mem->dma_handle, flags);
+	mem->vaddr = dma_alloc_coherent(dev, mem->size,
+					&mem->dma_handle, flags);
 
 	if (!mem->vaddr) {
 		dev_err(dev, "memory alloc size %ld failed\n", mem->size);
@@ -77,14 +58,7 @@ static int __videobuf_dc_alloc(struct device *dev,
 static void __videobuf_dc_free(struct device *dev,
 			       struct videobuf_dma_contig_memory *mem)
 {
-	if (mem->cached) {
-		if (!mem->vaddr)
-			return;
-		dma_unmap_single(dev, mem->dma_handle, mem->size,
-				 DMA_FROM_DEVICE);
-		free_pages_exact(mem->vaddr, mem->size);
-	} else
-		dma_free_coherent(dev, mem->size, mem->vaddr, mem->dma_handle);
+	dma_free_coherent(dev, mem->size, mem->vaddr, mem->dma_handle);
 
 	mem->vaddr = NULL;
 }
@@ -234,7 +208,7 @@ out_up:
 	return ret;
 }
 
-static struct videobuf_buffer *__videobuf_alloc_vb(size_t size, bool cached)
+static struct videobuf_buffer *__videobuf_alloc(size_t size)
 {
 	struct videobuf_dma_contig_memory *mem;
 	struct videobuf_buffer *vb;
@@ -244,22 +218,11 @@ static struct videobuf_buffer *__videobuf_alloc_vb(size_t size, bool cached)
 		vb->priv = ((char *)vb) + size;
 		mem = vb->priv;
 		mem->magic = MAGIC_DC_MEM;
-		mem->cached = cached;
 	}
 
 	return vb;
 }
 
-static struct videobuf_buffer *__videobuf_alloc_uncached(size_t size)
-{
-	return __videobuf_alloc_vb(size, false);
-}
-
-static struct videobuf_buffer *__videobuf_alloc_cached(size_t size)
-{
-	return __videobuf_alloc_vb(size, true);
-}
-
 static void *__videobuf_to_vaddr(struct videobuf_buffer *buf)
 {
 	struct videobuf_dma_contig_memory *mem = buf->priv;
@@ -310,19 +273,6 @@ static int __videobuf_iolock(struct videobuf_queue *q,
 	return 0;
 }
 
-static int __videobuf_sync(struct videobuf_queue *q,
-			   struct videobuf_buffer *buf)
-{
-	struct videobuf_dma_contig_memory *mem = buf->priv;
-	BUG_ON(!mem);
-	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
-
-	dma_sync_single_for_cpu(q->dev, mem->dma_handle, mem->size,
-				DMA_FROM_DEVICE);
-
-	return 0;
-}
-
 static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 				  struct videobuf_buffer *buf,
 				  struct vm_area_struct *vma)
@@ -331,8 +281,6 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	struct videobuf_mapping *map;
 	int retval;
 	unsigned long size;
-	unsigned long pos, start = vma->vm_start;
-	struct page *page;
 
 	dev_dbg(q->dev, "%s\n", __func__);
 
@@ -359,43 +307,16 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	size = vma->vm_end - vma->vm_start;
 	size = (size < mem->size) ? size : mem->size;
 
-	if (!mem->cached) {
-		vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
-		retval = remap_pfn_range(vma, vma->vm_start,
-			 mem->dma_handle >> PAGE_SHIFT,
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	retval = remap_pfn_range(vma, vma->vm_start,
+				 mem->dma_handle >> PAGE_SHIFT,
 				 size, vma->vm_page_prot);
-		if (retval) {
-			dev_err(q->dev, "mmap: remap failed with error %d. ",
-								retval);
-			dma_free_coherent(q->dev, mem->size,
-					mem->vaddr, mem->dma_handle);
-			goto error;
-		}
-	} else {
-		pos = (unsigned long)mem->vaddr;
-
-		while (size > 0) {
-			page = virt_to_page((void *)pos);
-			if (NULL == page) {
-				dev_err(q->dev, "mmap: virt_to_page failed\n");
-				__videobuf_dc_free(q->dev, mem);
-				goto error;
-			}
-			retval = vm_insert_page(vma, start, page);
-			if (retval) {
-				dev_err(q->dev, "mmap: insert failed with error %d\n",
-					retval);
-				__videobuf_dc_free(q->dev, mem);
-				goto error;
-			}
-			start += PAGE_SIZE;
-			pos += PAGE_SIZE;
-
-			if (size > PAGE_SIZE)
-				size -= PAGE_SIZE;
-			else
-				size = 0;
-		}
+	if (retval) {
+		dev_err(q->dev, "mmap: remap failed with error %d. ",
+			retval);
+		dma_free_coherent(q->dev, mem->size,
+				  mem->vaddr, mem->dma_handle);
+		goto error;
 	}
 
 	vma->vm_ops = &videobuf_vm_ops;
@@ -417,17 +338,8 @@ error:
 
 static struct videobuf_qtype_ops qops = {
 	.magic		= MAGIC_QTYPE_OPS,
-	.alloc_vb	= __videobuf_alloc_uncached,
-	.iolock		= __videobuf_iolock,
-	.mmap_mapper	= __videobuf_mmap_mapper,
-	.vaddr		= __videobuf_to_vaddr,
-};
-
-static struct videobuf_qtype_ops qops_cached = {
-	.magic		= MAGIC_QTYPE_OPS,
-	.alloc_vb	= __videobuf_alloc_cached,
+	.alloc_vb	= __videobuf_alloc,
 	.iolock		= __videobuf_iolock,
-	.sync		= __videobuf_sync,
 	.mmap_mapper	= __videobuf_mmap_mapper,
 	.vaddr		= __videobuf_to_vaddr,
 };
@@ -447,20 +359,6 @@ void videobuf_queue_dma_contig_init(struct videobuf_queue *q,
 }
 EXPORT_SYMBOL_GPL(videobuf_queue_dma_contig_init);
 
-void videobuf_queue_dma_contig_init_cached(struct videobuf_queue *q,
-					   const struct videobuf_queue_ops *ops,
-					   struct device *dev,
-					   spinlock_t *irqlock,
-					   enum v4l2_buf_type type,
-					   enum v4l2_field field,
-					   unsigned int msize,
-					   void *priv, struct mutex *ext_lock)
-{
-	videobuf_queue_core_init(q, ops, dev, irqlock, type, field, msize,
-				 priv, &qops_cached, ext_lock);
-}
-EXPORT_SYMBOL_GPL(videobuf_queue_dma_contig_init_cached);
-
 dma_addr_t videobuf_to_dma_contig(struct videobuf_buffer *buf)
 {
 	struct videobuf_dma_contig_memory *mem = buf->priv;
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index db1235dcb328..7d833eefaf4e 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -54,10 +54,15 @@ static int __vb2_buf_mem_alloc(struct vb2_buffer *vb)
 	void *mem_priv;
 	int plane;
 
-	/* Allocate memory for all planes in this buffer */
+	/*
+	 * Allocate memory for all planes in this buffer
+	 * NOTE: mmapped areas should be page aligned
+	 */
 	for (plane = 0; plane < vb->num_planes; ++plane) {
+		unsigned long size = PAGE_ALIGN(q->plane_sizes[plane]);
+
 		mem_priv = call_memop(q, alloc, q->alloc_ctx[plane],
-				      q->plane_sizes[plane]);
+				      size, q->gfp_flags);
 		if (IS_ERR_OR_NULL(mem_priv))
 			goto free;
 
@@ -403,7 +408,7 @@ static void __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 	 * Clear any buffer state related flags.
 	 */
 	b->flags &= ~V4L2_BUFFER_MASK_FLAGS;
-	b->flags |= V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	b->flags |= q->timestamp_type;
 
 	switch (vb->state) {
 	case VB2_BUF_STATE_QUEUED:
@@ -855,7 +860,7 @@ void vb2_buffer_done(struct vb2_buffer *vb, enum vb2_buffer_state state)
 		return;
 
 	dprintk(4, "Done processing on buffer %d, state: %d\n",
-			vb->v4l2_buf.index, vb->state);
+			vb->v4l2_buf.index, state);
 
 	/* sync buffers */
 	for (plane = 0; plane < vb->num_planes; ++plane)
@@ -1852,6 +1857,7 @@ int vb2_mmap(struct vb2_queue *q, struct vm_area_struct *vma)
 	struct vb2_buffer *vb;
 	unsigned int buffer, plane;
 	int ret;
+	unsigned long length;
 
 	if (q->memory != V4L2_MEMORY_MMAP) {
 		dprintk(1, "Queue is not currently set up for mmap\n");
@@ -1886,6 +1892,18 @@ int vb2_mmap(struct vb2_queue *q, struct vm_area_struct *vma)
 
 	vb = q->bufs[buffer];
 
+	/*
+	 * MMAP requires page_aligned buffers.
+	 * The buffer length was page_aligned at __vb2_buf_mem_alloc(),
+	 * so, we need to do the same here.
+	 */
+	length = PAGE_ALIGN(vb->v4l2_planes[plane].length);
+	if (length < (vma->vm_end - vma->vm_start)) {
+		dprintk(1,
+			"MMAP invalid, as it would overflow buffer length\n");
+		return -EINVAL;
+	}
+
 	ret = call_memop(q, mmap, vb->planes[plane].mem_priv, vma);
 	if (ret)
 		return ret;
@@ -2039,9 +2057,13 @@ int vb2_queue_init(struct vb2_queue *q)
 	    WARN_ON(!q->type)		  ||
 	    WARN_ON(!q->io_modes)	  ||
 	    WARN_ON(!q->ops->queue_setup) ||
-	    WARN_ON(!q->ops->buf_queue))
+	    WARN_ON(!q->ops->buf_queue)   ||
+	    WARN_ON(q->timestamp_type & ~V4L2_BUF_FLAG_TIMESTAMP_MASK))
 		return -EINVAL;
 
+	/* Warn that the driver should choose an appropriate timestamp type */
+	WARN_ON(q->timestamp_type == V4L2_BUF_FLAG_TIMESTAMP_UNKNOWN);
+
 	INIT_LIST_HEAD(&q->queued_list);
 	INIT_LIST_HEAD(&q->done_list);
 	spin_lock_init(&q->done_lock);
diff --git a/drivers/media/v4l2-core/videobuf2-dma-contig.c b/drivers/media/v4l2-core/videobuf2-dma-contig.c
index 10beaee7f0ae..fd56f2563201 100644
--- a/drivers/media/v4l2-core/videobuf2-dma-contig.c
+++ b/drivers/media/v4l2-core/videobuf2-dma-contig.c
@@ -152,7 +152,7 @@ static void vb2_dc_put(void *buf_priv)
 	kfree(buf);
 }
 
-static void *vb2_dc_alloc(void *alloc_ctx, unsigned long size)
+static void *vb2_dc_alloc(void *alloc_ctx, unsigned long size, gfp_t gfp_flags)
 {
 	struct vb2_dc_conf *conf = alloc_ctx;
 	struct device *dev = conf->dev;
@@ -162,10 +162,8 @@ static void *vb2_dc_alloc(void *alloc_ctx, unsigned long size)
 	if (!buf)
 		return ERR_PTR(-ENOMEM);
 
-	/* align image size to PAGE_SIZE */
-	size = PAGE_ALIGN(size);
-
-	buf->vaddr = dma_alloc_coherent(dev, size, &buf->dma_addr, GFP_KERNEL);
+	buf->vaddr = dma_alloc_coherent(dev, size, &buf->dma_addr,
+						GFP_KERNEL | gfp_flags);
 	if (!buf->vaddr) {
 		dev_err(dev, "dma_alloc_coherent of size %ld failed\n", size);
 		kfree(buf);
diff --git a/drivers/media/v4l2-core/videobuf2-dma-sg.c b/drivers/media/v4l2-core/videobuf2-dma-sg.c
index 25c3b360e1ad..16ae3dcc7e29 100644
--- a/drivers/media/v4l2-core/videobuf2-dma-sg.c
+++ b/drivers/media/v4l2-core/videobuf2-dma-sg.c
@@ -21,6 +21,15 @@
 #include <media/videobuf2-memops.h>
 #include <media/videobuf2-dma-sg.h>
 
+static int debug;
+module_param(debug, int, 0644);
+
+#define dprintk(level, fmt, arg...)					\
+	do {								\
+		if (debug >= level)					\
+			printk(KERN_DEBUG "vb2-dma-sg: " fmt, ## arg);	\
+	} while (0)
+
 struct vb2_dma_sg_buf {
 	void				*vaddr;
 	struct page			**pages;
@@ -33,7 +42,7 @@ struct vb2_dma_sg_buf {
 
 static void vb2_dma_sg_put(void *buf_priv);
 
-static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size)
+static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size, gfp_t gfp_flags)
 {
 	struct vb2_dma_sg_buf *buf;
 	int i;
@@ -46,7 +55,8 @@ static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size)
 	buf->write = 0;
 	buf->offset = 0;
 	buf->sg_desc.size = size;
-	buf->sg_desc.num_pages = (size + PAGE_SIZE - 1) >> PAGE_SHIFT;
+	/* size is already page aligned */
+	buf->sg_desc.num_pages = size >> PAGE_SHIFT;
 
 	buf->sg_desc.sglist = vzalloc(buf->sg_desc.num_pages *
 				      sizeof(*buf->sg_desc.sglist));
@@ -60,7 +70,8 @@ static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size)
 		goto fail_pages_array_alloc;
 
 	for (i = 0; i < buf->sg_desc.num_pages; ++i) {
-		buf->pages[i] = alloc_page(GFP_KERNEL | __GFP_ZERO | __GFP_NOWARN);
+		buf->pages[i] = alloc_page(GFP_KERNEL | __GFP_ZERO |
+					   __GFP_NOWARN | gfp_flags);
 		if (NULL == buf->pages[i])
 			goto fail_pages_alloc;
 		sg_set_page(&buf->sg_desc.sglist[i],
@@ -73,7 +84,7 @@ static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size)
 
 	atomic_inc(&buf->refcount);
 
-	printk(KERN_DEBUG "%s: Allocated buffer of %d pages\n",
+	dprintk(1, "%s: Allocated buffer of %d pages\n",
 		__func__, buf->sg_desc.num_pages);
 	return buf;
 
@@ -96,7 +107,7 @@ static void vb2_dma_sg_put(void *buf_priv)
 	int i = buf->sg_desc.num_pages;
 
 	if (atomic_dec_and_test(&buf->refcount)) {
-		printk(KERN_DEBUG "%s: Freeing buffer of %d pages\n", __func__,
+		dprintk(1, "%s: Freeing buffer of %d pages\n", __func__,
 			buf->sg_desc.num_pages);
 		if (buf->vaddr)
 			vm_unmap_ram(buf->vaddr, buf->sg_desc.num_pages);
@@ -162,7 +173,7 @@ static void *vb2_dma_sg_get_userptr(void *alloc_ctx, unsigned long vaddr,
 	return buf;
 
 userptr_fail_get_user_pages:
-	printk(KERN_DEBUG "get_user_pages requested/got: %d/%d]\n",
+	dprintk(1, "get_user_pages requested/got: %d/%d]\n",
 	       num_pages_from_user, buf->sg_desc.num_pages);
 	while (--num_pages_from_user >= 0)
 		put_page(buf->pages[num_pages_from_user]);
@@ -185,7 +196,7 @@ static void vb2_dma_sg_put_userptr(void *buf_priv)
 	struct vb2_dma_sg_buf *buf = buf_priv;
 	int i = buf->sg_desc.num_pages;
 
-	printk(KERN_DEBUG "%s: Releasing userspace buffer of %d pages\n",
+	dprintk(1, "%s: Releasing userspace buffer of %d pages\n",
 	       __func__, buf->sg_desc.num_pages);
 	if (buf->vaddr)
 		vm_unmap_ram(buf->vaddr, buf->sg_desc.num_pages);
diff --git a/drivers/media/v4l2-core/videobuf2-vmalloc.c b/drivers/media/v4l2-core/videobuf2-vmalloc.c
index a47fd4f589a1..313d9771b2bc 100644
--- a/drivers/media/v4l2-core/videobuf2-vmalloc.c
+++ b/drivers/media/v4l2-core/videobuf2-vmalloc.c
@@ -35,11 +35,11 @@ struct vb2_vmalloc_buf {
 
 static void vb2_vmalloc_put(void *buf_priv);
 
-static void *vb2_vmalloc_alloc(void *alloc_ctx, unsigned long size)
+static void *vb2_vmalloc_alloc(void *alloc_ctx, unsigned long size, gfp_t gfp_flags)
 {
 	struct vb2_vmalloc_buf *buf;
 
-	buf = kzalloc(sizeof(*buf), GFP_KERNEL);
+	buf = kzalloc(sizeof(*buf), GFP_KERNEL | gfp_flags);
 	if (!buf)
 		return NULL;
 
diff --git a/drivers/memstick/core/mspro_block.c b/drivers/memstick/core/mspro_block.c
index f12b78dbce04..f4176ca3a794 100644
--- a/drivers/memstick/core/mspro_block.c
+++ b/drivers/memstick/core/mspro_block.c
@@ -204,7 +204,7 @@ static int mspro_block_bd_open(struct block_device *bdev, fmode_t mode)
 }
 
 
-static int mspro_block_disk_release(struct gendisk *disk)
+static void mspro_block_disk_release(struct gendisk *disk)
 {
 	struct mspro_block_data *msb = disk->private_data;
 	int disk_id = MINOR(disk_devt(disk)) >> MSPRO_BLOCK_PART_SHIFT;
@@ -224,13 +224,11 @@ static int mspro_block_disk_release(struct gendisk *disk)
 	}
 
 	mutex_unlock(&mspro_block_disk_lock);
-
-	return 0;
 }
 
-static int mspro_block_bd_release(struct gendisk *disk, fmode_t mode)
+static void mspro_block_bd_release(struct gendisk *disk, fmode_t mode)
 {
-	return mspro_block_disk_release(disk);
+	mspro_block_disk_release(disk);
 }
 
 static int mspro_block_bd_getgeo(struct block_device *bdev,
diff --git a/drivers/memstick/host/r592.c b/drivers/memstick/host/r592.c
index a7c5b31c0d50..9718661c1fb6 100644
--- a/drivers/memstick/host/r592.c
+++ b/drivers/memstick/host/r592.c
@@ -847,7 +847,7 @@ static void r592_remove(struct pci_dev *pdev)
 			dev->dummy_dma_page_physical_address);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int r592_suspend(struct device *core_dev)
 {
 	struct pci_dev *pdev = to_pci_dev(core_dev);
@@ -870,10 +870,10 @@ static int r592_resume(struct device *core_dev)
 	r592_update_card_detect(dev);
 	return 0;
 }
-
-SIMPLE_DEV_PM_OPS(r592_pm_ops, r592_suspend, r592_resume);
 #endif
 
+static SIMPLE_DEV_PM_OPS(r592_pm_ops, r592_suspend, r592_resume);
+
 MODULE_DEVICE_TABLE(pci, r592_pci_id_tbl);
 
 static struct pci_driver r852_pci_driver = {
@@ -881,9 +881,7 @@ static struct pci_driver r852_pci_driver = {
 	.id_table	= r592_pci_id_tbl,
 	.probe		= r592_probe,
 	.remove		= r592_remove,
-#ifdef CONFIG_PM
 	.driver.pm	= &r592_pm_ops,
-#endif
 };
 
 static __init int r592_module_init(void)
diff --git a/drivers/message/fusion/mptbase.c b/drivers/message/fusion/mptbase.c
index fb69baa06ca8..767ff4d839f4 100644
--- a/drivers/message/fusion/mptbase.c
+++ b/drivers/message/fusion/mptbase.c
@@ -6656,7 +6656,7 @@ static int mpt_summary_proc_show(struct seq_file *m, void *v)
 
 static int mpt_summary_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, mpt_summary_proc_show, PDE(inode)->data);
+	return single_open(file, mpt_summary_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations mpt_summary_proc_fops = {
@@ -6805,7 +6805,7 @@ static int mpt_iocinfo_proc_show(struct seq_file *m, void *v)
 
 static int mpt_iocinfo_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, mpt_iocinfo_proc_show, PDE(inode)->data);
+	return single_open(file, mpt_iocinfo_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations mpt_iocinfo_proc_fops = {
diff --git a/drivers/message/fusion/mptctl.c b/drivers/message/fusion/mptctl.c
index b383b6961e59..dcc8385adeb3 100644
--- a/drivers/message/fusion/mptctl.c
+++ b/drivers/message/fusion/mptctl.c
@@ -597,13 +597,6 @@ mptctl_event_process(MPT_ADAPTER *ioc, EventNotificationReply_t *pEvReply)
 }
 
 static int
-mptctl_release(struct inode *inode, struct file *filep)
-{
-	fasync_helper(-1, filep, 0, &async_queue);
-	return 0;
-}
-
-static int
 mptctl_fasync(int fd, struct file *filep, int mode)
 {
 	MPT_ADAPTER	*ioc;
@@ -2822,7 +2815,6 @@ static const struct file_operations mptctl_fops = {
 	.llseek =	no_llseek,
 	.fasync = 	mptctl_fasync,
 	.unlocked_ioctl = mptctl_ioctl,
-	.release =	mptctl_release,
 #ifdef CONFIG_COMPAT
 	.compat_ioctl = compat_mpctl_ioctl,
 #endif
diff --git a/drivers/message/fusion/mptfc.c b/drivers/message/fusion/mptfc.c
index c13cd9bc590b..fd75108c355e 100644
--- a/drivers/message/fusion/mptfc.c
+++ b/drivers/message/fusion/mptfc.c
@@ -109,7 +109,7 @@ static int mptfc_host_reset(struct scsi_cmnd *SCpnt);
 static struct scsi_host_template mptfc_driver_template = {
 	.module				= THIS_MODULE,
 	.proc_name			= "mptfc",
-	.proc_info			= mptscsih_proc_info,
+	.show_info			= mptscsih_show_info,
 	.name				= "MPT FC Host",
 	.info				= mptscsih_info,
 	.queuecommand			= mptfc_qcmd,
diff --git a/drivers/message/fusion/mptsas.c b/drivers/message/fusion/mptsas.c
index fa43c391c8ed..dd239bdbfcb4 100644
--- a/drivers/message/fusion/mptsas.c
+++ b/drivers/message/fusion/mptsas.c
@@ -1977,7 +1977,7 @@ done:
 static struct scsi_host_template mptsas_driver_template = {
 	.module				= THIS_MODULE,
 	.proc_name			= "mptsas",
-	.proc_info			= mptscsih_proc_info,
+	.show_info			= mptscsih_show_info,
 	.name				= "MPT SAS Host",
 	.info				= mptscsih_info,
 	.queuecommand			= mptsas_qcmd,
@@ -2235,10 +2235,10 @@ static int mptsas_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	}
 
 	/* do we need to support multiple segments? */
-	if (req->bio->bi_vcnt > 1 || rsp->bio->bi_vcnt > 1) {
+	if (bio_segments(req->bio) > 1 || bio_segments(rsp->bio) > 1) {
 		printk(MYIOC_s_ERR_FMT "%s: multiple segments req %u %u, rsp %u %u\n",
-		    ioc->name, __func__, req->bio->bi_vcnt, blk_rq_bytes(req),
-		    rsp->bio->bi_vcnt, blk_rq_bytes(rsp));
+		    ioc->name, __func__, bio_segments(req->bio), blk_rq_bytes(req),
+		    bio_segments(rsp->bio), blk_rq_bytes(rsp));
 		return -EINVAL;
 	}
 
diff --git a/drivers/message/fusion/mptscsih.c b/drivers/message/fusion/mptscsih.c
index 164afa71bba7..727819cc7034 100644
--- a/drivers/message/fusion/mptscsih.c
+++ b/drivers/message/fusion/mptscsih.c
@@ -1284,101 +1284,17 @@ mptscsih_info(struct Scsi_Host *SChost)
 	return h->info_kbuf;
 }
 
-struct info_str {
-	char *buffer;
-	int   length;
-	int   offset;
-	int   pos;
-};
-
-static void
-mptscsih_copy_mem_info(struct info_str *info, char *data, int len)
-{
-	if (info->pos + len > info->length)
-		len = info->length - info->pos;
-
-	if (info->pos + len < info->offset) {
-		info->pos += len;
-		return;
-	}
-
-	if (info->pos < info->offset) {
-	        data += (info->offset - info->pos);
-	        len  -= (info->offset - info->pos);
-	}
-
-	if (len > 0) {
-                memcpy(info->buffer + info->pos, data, len);
-                info->pos += len;
-	}
-}
-
-static int
-mptscsih_copy_info(struct info_str *info, char *fmt, ...)
-{
-	va_list args;
-	char buf[81];
-	int len;
-
-	va_start(args, fmt);
-	len = vsprintf(buf, fmt, args);
-	va_end(args);
-
-	mptscsih_copy_mem_info(info, buf, len);
-	return len;
-}
-
-static int
-mptscsih_host_info(MPT_ADAPTER *ioc, char *pbuf, off_t offset, int len)
-{
-	struct info_str info;
-
-	info.buffer	= pbuf;
-	info.length	= len;
-	info.offset	= offset;
-	info.pos	= 0;
-
-	mptscsih_copy_info(&info, "%s: %s, ", ioc->name, ioc->prod_name);
-	mptscsih_copy_info(&info, "%s%08xh, ", MPT_FW_REV_MAGIC_ID_STRING, ioc->facts.FWVersion.Word);
-	mptscsih_copy_info(&info, "Ports=%d, ", ioc->facts.NumberOfPorts);
-	mptscsih_copy_info(&info, "MaxQ=%d\n", ioc->req_depth);
-
-	return ((info.pos > info.offset) ? info.pos - info.offset : 0);
-}
-
-/*=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
-/**
- *	mptscsih_proc_info - Return information about MPT adapter
- * 	@host:   scsi host struct
- * 	@buffer: if write, user data; if read, buffer for user
- *	@start: returns the buffer address
- * 	@offset: if write, 0; if read, the current offset into the buffer from
- * 		 the previous read.
- * 	@length: if write, return length;
- *	@func:   write = 1; read = 0
- *
- *	(linux scsi_host_template.info routine)
- */
-int
-mptscsih_proc_info(struct Scsi_Host *host, char *buffer, char **start, off_t offset,
-			int length, int func)
+int mptscsih_show_info(struct seq_file *m, struct Scsi_Host *host)
 {
 	MPT_SCSI_HOST	*hd = shost_priv(host);
 	MPT_ADAPTER	*ioc = hd->ioc;
-	int size = 0;
 
-	if (func) {
-		/*
-		 * write is not supported
-		 */
-	} else {
-		if (start)
-			*start = buffer;
-
-		size = mptscsih_host_info(ioc, buffer, offset, length);
-	}
+	seq_printf(m, "%s: %s, ", ioc->name, ioc->prod_name);
+	seq_printf(m, "%s%08xh, ", MPT_FW_REV_MAGIC_ID_STRING, ioc->facts.FWVersion.Word);
+	seq_printf(m, "Ports=%d, ", ioc->facts.NumberOfPorts);
+	seq_printf(m, "MaxQ=%d\n", ioc->req_depth);
 
-	return size;
+	return 0;
 }
 
 /*=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
@@ -3348,7 +3264,7 @@ EXPORT_SYMBOL(mptscsih_shutdown);
 EXPORT_SYMBOL(mptscsih_suspend);
 EXPORT_SYMBOL(mptscsih_resume);
 #endif
-EXPORT_SYMBOL(mptscsih_proc_info);
+EXPORT_SYMBOL(mptscsih_show_info);
 EXPORT_SYMBOL(mptscsih_info);
 EXPORT_SYMBOL(mptscsih_qcmd);
 EXPORT_SYMBOL(mptscsih_slave_destroy);
diff --git a/drivers/message/fusion/mptscsih.h b/drivers/message/fusion/mptscsih.h
index 43e75ff39921..83f503162f7a 100644
--- a/drivers/message/fusion/mptscsih.h
+++ b/drivers/message/fusion/mptscsih.h
@@ -111,7 +111,7 @@ extern void mptscsih_shutdown(struct pci_dev *);
 extern int mptscsih_suspend(struct pci_dev *pdev, pm_message_t state);
 extern int mptscsih_resume(struct pci_dev *pdev);
 #endif
-extern int mptscsih_proc_info(struct Scsi_Host *host, char *buffer, char **start, off_t offset, int length, int func);
+extern int mptscsih_show_info(struct seq_file *, struct Scsi_Host *);
 extern const char * mptscsih_info(struct Scsi_Host *SChost);
 extern int mptscsih_qcmd(struct scsi_cmnd *SCpnt, void (*done)(struct scsi_cmnd *));
 extern int mptscsih_IssueTaskMgmt(MPT_SCSI_HOST *hd, u8 type, u8 channel,
diff --git a/drivers/message/fusion/mptspi.c b/drivers/message/fusion/mptspi.c
index c3aabde2dc4f..5653e505f91f 100644
--- a/drivers/message/fusion/mptspi.c
+++ b/drivers/message/fusion/mptspi.c
@@ -831,7 +831,7 @@ static void mptspi_slave_destroy(struct scsi_device *sdev)
 static struct scsi_host_template mptspi_driver_template = {
 	.module				= THIS_MODULE,
 	.proc_name			= "mptspi",
-	.proc_info			= mptscsih_proc_info,
+	.show_info			= mptscsih_show_info,
 	.name				= "MPT SPI Host",
 	.info				= mptscsih_info,
 	.queuecommand			= mptspi_qcmd,
diff --git a/drivers/message/i2o/i2o_block.c b/drivers/message/i2o/i2o_block.c
index 49e86aed2bc4..6fc3866965df 100644
--- a/drivers/message/i2o/i2o_block.c
+++ b/drivers/message/i2o/i2o_block.c
@@ -600,10 +600,8 @@ static int i2o_block_open(struct block_device *bdev, fmode_t mode)
  *
  *	Unlock and unmount the media, and power down the device. Gets called if
  *	the block device is closed.
- *
- *	Returns 0 on success or negative error code on failure.
  */
-static int i2o_block_release(struct gendisk *disk, fmode_t mode)
+static void i2o_block_release(struct gendisk *disk, fmode_t mode)
 {
 	struct i2o_block_device *dev = disk->private_data;
 	u8 operation;
@@ -617,7 +615,7 @@ static int i2o_block_release(struct gendisk *disk, fmode_t mode)
 	 * the TID no longer exists.
 	 */
 	if (!dev->i2o_dev)
-		return 0;
+		return;
 
 	mutex_lock(&i2o_block_mutex);
 	i2o_block_device_flush(dev->i2o_dev);
@@ -631,8 +629,6 @@ static int i2o_block_release(struct gendisk *disk, fmode_t mode)
 
 	i2o_block_device_power(dev, operation);
 	mutex_unlock(&i2o_block_mutex);
-
-	return 0;
 }
 
 static int i2o_block_getgeo(struct block_device *bdev, struct hd_geometry *geo)
diff --git a/drivers/message/i2o/i2o_config.c b/drivers/message/i2o/i2o_config.c
index 5451beff183f..a60c188c2bd9 100644
--- a/drivers/message/i2o/i2o_config.c
+++ b/drivers/message/i2o/i2o_config.c
@@ -687,6 +687,11 @@ static int i2o_cfg_passthru32(struct file *file, unsigned cmnd,
 		}
 		size = size >> 16;
 		size *= 4;
+		if (size > sizeof(rmsg)) {
+			rcode = -EINVAL;
+			goto sg_list_cleanup;
+		}
+
 		/* Copy in the user's I2O command */
 		if (copy_from_user(rmsg, user_msg, size)) {
 			rcode = -EFAULT;
@@ -922,6 +927,11 @@ static int i2o_cfg_passthru(unsigned long arg)
 		}
 		size = size >> 16;
 		size *= 4;
+		if (size > sizeof(rmsg)) {
+			rcode = -EFAULT;
+			goto sg_list_cleanup;
+		}
+
 		/* Copy in the user's I2O command */
 		if (copy_from_user(rmsg, user_msg, size)) {
 			rcode = -EFAULT;
diff --git a/drivers/message/i2o/i2o_proc.c b/drivers/message/i2o/i2o_proc.c
index 8001aa6bfb48..b7d87cd227a9 100644
--- a/drivers/message/i2o/i2o_proc.c
+++ b/drivers/message/i2o/i2o_proc.c
@@ -1599,98 +1599,98 @@ static int i2o_seq_show_sensors(struct seq_file *seq, void *v)
 
 static int i2o_seq_open_hrt(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_hrt, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_hrt, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_lct(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_lct, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_lct, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_status(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_status, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_status, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_hw(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_hw, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_hw, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_ddm_table(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_ddm_table, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_ddm_table, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_driver_store(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_driver_store, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_driver_store, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_drivers_stored(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_drivers_stored, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_drivers_stored, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_groups(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_groups, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_groups, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_phys_device(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_phys_device, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_phys_device, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_claimed(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_claimed, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_claimed, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_users(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_users, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_users, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_priv_msgs(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_priv_msgs, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_priv_msgs, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_authorized_users(struct inode *inode, struct file *file)
 {
 	return single_open(file, i2o_seq_show_authorized_users,
-			   PDE(inode)->data);
+			   PDE_DATA(inode));
 };
 
 static int i2o_seq_open_dev_identity(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_dev_identity, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_dev_identity, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_ddm_identity(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_ddm_identity, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_ddm_identity, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_uinfo(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_uinfo, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_uinfo, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_sgl_limits(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_sgl_limits, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_sgl_limits, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_sensors(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_sensors, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_sensors, PDE_DATA(inode));
 };
 
 static int i2o_seq_open_dev_name(struct inode *inode, struct file *file)
 {
-	return single_open(file, i2o_seq_show_dev_name, PDE(inode)->data);
+	return single_open(file, i2o_seq_show_dev_name, PDE_DATA(inode));
 };
 
 static const struct file_operations i2o_seq_fops_lct = {
@@ -1895,25 +1895,6 @@ static int i2o_proc_create_entries(struct proc_dir_entry *dir,
 }
 
 /**
- *	i2o_proc_subdir_remove - Remove child entries from a proc entry
- *	@dir: proc dir entry from which the childs should be removed
- *
- *	Iterate over each i2o proc entry under dir and remove it. If the child
- *	also has entries, remove them too.
- */
-static void i2o_proc_subdir_remove(struct proc_dir_entry *dir)
-{
-	struct proc_dir_entry *pe, *tmp;
-	pe = dir->subdir;
-	while (pe) {
-		tmp = pe->next;
-		i2o_proc_subdir_remove(pe);
-		remove_proc_entry(pe->name, dir);
-		pe = tmp;
-	}
-};
-
-/**
  *	i2o_proc_device_add - Add an I2O device to the proc dir
  *	@dir: proc dir entry to which the device should be added
  *	@dev: I2O device which should be added
@@ -1932,14 +1913,12 @@ static void i2o_proc_device_add(struct proc_dir_entry *dir,
 
 	osm_debug("adding device /proc/i2o/%s/%s\n", dev->iop->name, buff);
 
-	devdir = proc_mkdir(buff, dir);
+	devdir = proc_mkdir_data(buff, 0, dir, dev);
 	if (!devdir) {
 		osm_warn("Could not allocate procdir!\n");
 		return;
 	}
 
-	devdir->data = dev;
-
 	i2o_proc_create_entries(devdir, generic_dev_entries, dev);
 
 	/* Inform core that we want updates about this device's status */
@@ -1973,12 +1952,10 @@ static int i2o_proc_iop_add(struct proc_dir_entry *dir,
 
 	osm_debug("adding IOP /proc/i2o/%s\n", c->name);
 
-	iopdir = proc_mkdir(c->name, dir);
+	iopdir = proc_mkdir_data(c->name, 0, dir, c);
 	if (!iopdir)
 		return -1;
 
-	iopdir->data = c;
-
 	i2o_proc_create_entries(iopdir, i2o_proc_generic_iop_entries, c);
 
 	list_for_each_entry(dev, &c->devices, list)
@@ -1988,31 +1965,6 @@ static int i2o_proc_iop_add(struct proc_dir_entry *dir,
 }
 
 /**
- *	i2o_proc_iop_remove - Removes an I2O controller from the i2o proc tree
- *	@dir: parent proc dir entry
- *	@c: I2O controller which should be removed
- *
- *	Iterate over each i2o proc entry and search controller c. If it is found
- *	remove it from the tree.
- */
-static void i2o_proc_iop_remove(struct proc_dir_entry *dir,
-				struct i2o_controller *c)
-{
-	struct proc_dir_entry *pe, *tmp;
-
-	pe = dir->subdir;
-	while (pe) {
-		tmp = pe->next;
-		if (pe->data == c) {
-			i2o_proc_subdir_remove(pe);
-			remove_proc_entry(pe->name, dir);
-		}
-		osm_debug("removing IOP /proc/i2o/%s\n", c->name);
-		pe = tmp;
-	}
-}
-
-/**
  *	i2o_proc_fs_create - Create the i2o proc fs.
  *
  *	Iterate over each I2O controller and create the entries for it.
@@ -2042,12 +1994,7 @@ static int __init i2o_proc_fs_create(void)
  */
 static int __exit i2o_proc_fs_destroy(void)
 {
-	struct i2o_controller *c;
-
-	list_for_each_entry(c, &i2o_controllers, list)
-	    i2o_proc_iop_remove(i2o_proc_dir_root, c);
-
-	remove_proc_entry("i2o", NULL);
+	remove_proc_subtree("i2o", NULL);
 
 	return 0;
 };
diff --git a/drivers/mfd/88pm860x-core.c b/drivers/mfd/88pm860x-core.c
index 893fc1ba6ead..31ca55548ef9 100644
--- a/drivers/mfd/88pm860x-core.c
+++ b/drivers/mfd/88pm860x-core.c
@@ -1144,17 +1144,15 @@ static int pm860x_probe(struct i2c_client *client,
 			return -ENOMEM;
 		ret = pm860x_dt_init(node, &client->dev, pdata);
 		if (ret)
-			goto err;
+			return ret;
 	} else if (!pdata) {
 		pr_info("No platform data in %s!\n", __func__);
 		return -EINVAL;
 	}
 
 	chip = kzalloc(sizeof(struct pm860x_chip), GFP_KERNEL);
-	if (chip == NULL) {
-		ret = -ENOMEM;
-		goto err;
-	}
+	if (chip == NULL)
+		return -ENOMEM;
 
 	chip->id = verify_addr(client);
 	chip->regmap = regmap_init_i2c(client, &pm860x_regmap_config);
@@ -1194,10 +1192,6 @@ static int pm860x_probe(struct i2c_client *client,
 
 	pm860x_device_init(chip, pdata);
 	return 0;
-err:
-	if (node)
-		devm_kfree(&client->dev, pdata);
-	return ret;
 }
 
 static int pm860x_remove(struct i2c_client *client)
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index ca86581d02ce..d9aed1593e5d 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -10,19 +10,240 @@ config MFD_CORE
 	select IRQ_DOMAIN
 	default n
 
-config MFD_88PM860X
-	bool "Support Marvell 88PM8606/88PM8607"
+config MFD_CS5535
+	tristate "AMD CS5535 and CS5536 southbridge core functions"
+	select MFD_CORE
+	depends on PCI && X86
+	---help---
+	  This is the core driver for CS5535/CS5536 MFD functions.  This is
+          necessary for using the board's GPIO and MFGPT functionality.
+
+config MFD_AS3711
+	bool "AMS AS3711"
+	select MFD_CORE
+	select REGMAP_I2C
+	select REGMAP_IRQ
 	depends on I2C=y && GENERIC_HARDIRQS
+	help
+	  Support for the AS3711 PMIC from AMS
+
+config PMIC_ADP5520
+	bool "Analog Devices ADP5520/01 MFD PMIC Core Support"
+	depends on I2C=y
+	help
+	  Say yes here to add support for Analog Devices AD5520 and ADP5501,
+	  Multifunction Power Management IC. This includes
+	  the I2C driver and the core APIs _only_, you have to select
+	  individual components like LCD backlight, LEDs, GPIOs and Kepad
+	  under the corresponding menus.
+
+config MFD_AAT2870_CORE
+	bool "AnalogicTech AAT2870"
+	select MFD_CORE
+	depends on I2C=y && GPIOLIB && GENERIC_HARDIRQS
+	help
+	  If you say yes here you get support for the AAT2870.
+	  This driver provides common support for accessing the device,
+	  additional drivers must be enabled in order to use the
+	  functionality of the device.
+
+config MFD_CROS_EC
+	tristate "ChromeOS Embedded Controller"
+	select MFD_CORE
+	help
+	  If you say Y here you get support for the ChromeOS Embedded
+	  Controller (EC) providing keyboard, battery and power services.
+	  You also ned to enable the driver for the bus you are using. The
+	  protocol for talking to the EC is defined by the bus driver.
+
+config MFD_CROS_EC_I2C
+	tristate "ChromeOS Embedded Controller (I2C)"
+	depends on MFD_CROS_EC && I2C
+
+	help
+	  If you say Y here, you get support for talking to the ChromeOS
+	  EC through an I2C bus. This uses a simple byte-level protocol with
+	  a checksum. Failing accesses will be retried three times to
+	  improve reliability.
+
+config MFD_CROS_EC_SPI
+	tristate "ChromeOS Embedded Controller (SPI)"
+	depends on MFD_CROS_EC && SPI
+
+	---help---
+	  If you say Y here, you get support for talking to the ChromeOS EC
+	  through a SPI bus, using a byte-level protocol. Since the EC's
+	  response time cannot be guaranteed, we support ignoring
+	  'pre-amble' bytes before the response actually starts.
+
+config MFD_ASIC3
+	bool "Compaq ASIC3"
+	depends on GENERIC_HARDIRQS && GPIOLIB && ARM
+	select MFD_CORE
+	 ---help---
+	  This driver supports the ASIC3 multifunction chip found on many
+	  PDAs (mainly iPAQ and HTC based ones)
+
+config PMIC_DA903X
+	bool "Dialog Semiconductor DA9030/DA9034 PMIC Support"
+	depends on I2C=y
+	help
+	  Say yes here to support for Dialog Semiconductor DA9030 (a.k.a
+	  ARAVA) and DA9034 (a.k.a MICCO), these are Power Management IC
+	  usually found on PXA processors-based platforms. This includes
+	  the I2C driver and the core APIs _only_, you have to select
+	  individual components like LCD backlight, voltage regulators,
+	  LEDs and battery-charger under the corresponding menus.
+
+config PMIC_DA9052
+	bool
+	select MFD_CORE
+
+config MFD_DA9052_SPI
+	bool "Dialog Semiconductor DA9052/53 PMIC variants with SPI"
+	select REGMAP_SPI
+	select REGMAP_IRQ
+	select PMIC_DA9052
+	depends on SPI_MASTER=y && GENERIC_HARDIRQS
+	help
+	  Support for the Dialog Semiconductor DA9052 PMIC
+	  when controlled using SPI. This driver provides common support
+	  for accessing the device, additional drivers must be enabled in
+	  order to use the functionality of the device.
+
+config MFD_DA9052_I2C
+	bool "Dialog Semiconductor DA9052/53 PMIC variants with I2C"
 	select REGMAP_I2C
+	select REGMAP_IRQ
+	select PMIC_DA9052
+	depends on I2C=y && GENERIC_HARDIRQS
+	help
+	  Support for the Dialog Semiconductor DA9052 PMIC
+	  when controlled using I2C. This driver provides common support
+	  for accessing the device, additional drivers must be enabled in
+	  order to use the functionality of the device.
+
+config MFD_DA9055
+	bool "Dialog Semiconductor DA9055 PMIC Support"
+	select REGMAP_I2C
+	select REGMAP_IRQ
 	select MFD_CORE
+	depends on I2C=y && GENERIC_HARDIRQS
 	help
-	  This supports for Marvell 88PM8606/88PM8607 Power Management IC.
-	  This includes the I2C driver and the core APIs _only_, you have to
-	  select individual components like voltage regulators, RTC and
-	  battery-charger under the corresponding menus.
+	  Say yes here for support of Dialog Semiconductor DA9055. This is
+	  a Power Management IC. This driver provides common support for
+	  accessing the device as well as the I2C interface to the chip itself.
+	  Additional drivers must be enabled in order to use the functionality
+	  of the device.
+
+	  This driver can be built as a module. If built as a module it will be
+	  called "da9055"
+
+config MFD_MC13783
+	tristate
+
+config MFD_MC13XXX
+	tristate
+	depends on (SPI_MASTER || I2C) && GENERIC_HARDIRQS
+	select MFD_CORE
+	select MFD_MC13783
+	help
+	  Enable support for the Freescale MC13783 and MC13892 PMICs.
+	  This driver provides common support for accessing the device,
+	  additional drivers must be enabled in order to use the
+	  functionality of the device.
+
+config MFD_MC13XXX_SPI
+	tristate "Freescale MC13783 and MC13892 SPI interface"
+	depends on SPI_MASTER && GENERIC_HARDIRQS
+	select REGMAP_SPI
+	select MFD_MC13XXX
+	help
+	  Select this if your MC13xxx is connected via an SPI bus.
+
+config MFD_MC13XXX_I2C
+	tristate "Freescale MC13892 I2C interface"
+	depends on I2C && GENERIC_HARDIRQS
+	select REGMAP_I2C
+	select MFD_MC13XXX
+	help
+	  Select this if your MC13xxx is connected via an I2C bus.
+
+config HTC_EGPIO
+	bool "HTC EGPIO support"
+	depends on GENERIC_HARDIRQS && GPIOLIB && ARM
+	help
+	    This driver supports the CPLD egpio chip present on
+	    several HTC phones.  It provides basic support for input
+	    pins, output pins, and irqs.
+
+config HTC_PASIC3
+	tristate "HTC PASIC3 LED/DS1WM chip support"
+	select MFD_CORE
+	depends on GENERIC_HARDIRQS
+	help
+	  This core driver provides register access for the LED/DS1WM
+	  chips labeled "AIC2" and "AIC3", found on HTC Blueangel and
+	  HTC Magician devices, respectively. Actual functionality is
+	  handled by the leds-pasic3 and ds1wm drivers.
+
+config HTC_I2CPLD
+	bool "HTC I2C PLD chip support"
+	depends on I2C=y && GPIOLIB
+	help
+	  If you say yes here you get support for the supposed CPLD
+	  found on omap850 HTC devices like the HTC Wizard and HTC Herald.
+	  This device provides input and output GPIOs through an I2C
+	  interface to one or more sub-chips.
+
+config LPC_ICH
+	tristate "Intel ICH LPC"
+	depends on PCI && GENERIC_HARDIRQS
+	select MFD_CORE
+	help
+	  The LPC bridge function of the Intel ICH provides support for
+	  many functional units. This driver provides needed support for
+	  other drivers to control these functions, currently GPIO and
+	  watchdog.
+
+config LPC_SCH
+	tristate "Intel SCH LPC"
+	depends on PCI && GENERIC_HARDIRQS
+	select MFD_CORE
+	help
+	  LPC bridge function of the Intel SCH provides support for
+	  System Management Bus and General Purpose I/O.
+
+config MFD_INTEL_MSIC
+	bool "Intel MSIC"
+	depends on INTEL_SCU_IPC
+	select MFD_CORE
+	help
+	  Select this option to enable access to Intel MSIC (Avatele
+	  Passage) chip. This chip embeds audio, battery, GPIO, etc.
+	  devices used in Intel Medfield platforms.
+
+config MFD_JANZ_CMODIO
+	tristate "Janz CMOD-IO PCI MODULbus Carrier Board"
+	select MFD_CORE
+	depends on PCI && GENERIC_HARDIRQS
+	help
+	  This is the core driver for the Janz CMOD-IO PCI MODULbus
+	  carrier board. This device is a PCI to MODULbus bridge which may
+	  host many different types of MODULbus daughterboards, including
+	  CAN and GPIO controllers.
+
+config MFD_JZ4740_ADC
+	bool "Janz JZ4740 ADC core"
+	select MFD_CORE
+	select GENERIC_IRQ_CHIP
+	depends on MACH_JZ4740
+	help
+	  Say yes here if you want support for the ADC unit in the JZ4740 SoC.
+	  This driver is necessary for jz4740-battery and jz4740-hwmon driver.
 
 config MFD_88PM800
-	tristate "Support Marvell 88PM800"
+	tristate "Marvell 88PM800"
 	depends on I2C=y && GENERIC_HARDIRQS
 	select REGMAP_I2C
 	select REGMAP_IRQ
@@ -34,7 +255,7 @@ config MFD_88PM800
 	  battery-charger under the corresponding menus.
 
 config MFD_88PM805
-	tristate "Support Marvell 88PM805"
+	tristate "Marvell 88PM805"
 	depends on I2C=y && GENERIC_HARDIRQS
 	select REGMAP_I2C
 	select REGMAP_IRQ
@@ -45,8 +266,242 @@ config MFD_88PM805
 	  components like codec device, headset/Mic device under the
 	  corresponding menus.
 
+config MFD_88PM860X
+	bool "Marvell 88PM8606/88PM8607"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select REGMAP_I2C
+	select MFD_CORE
+	help
+	  This supports for Marvell 88PM8606/88PM8607 Power Management IC.
+	  This includes the I2C driver and the core APIs _only_, you have to
+	  select individual components like voltage regulators, RTC and
+	  battery-charger under the corresponding menus.
+
+config MFD_MAX77686
+	bool "Maxim Semiconductor MAX77686 PMIC Support"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select MFD_CORE
+	select REGMAP_I2C
+	select IRQ_DOMAIN
+	help
+	  Say yes here to support for Maxim Semiconductor MAX77686.
+	  This is a Power Management IC with RTC on chip.
+	  This driver provides common support for accessing the device;
+	  additional drivers must be enabled in order to use the functionality
+	  of the device.
+
+config MFD_MAX77693
+	bool "Maxim Semiconductor MAX77693 PMIC Support"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select MFD_CORE
+	select REGMAP_I2C
+	help
+	  Say yes here to support for Maxim Semiconductor MAX77693.
+	  This is a companion Power Management IC with Flash, Haptic, Charger,
+	  and MUIC(Micro USB Interface Controller) controls on chip.
+	  This driver provides common support for accessing the device;
+	  additional drivers must be enabled in order to use the functionality
+	  of the device.
+
+config MFD_MAX8907
+	tristate "Maxim Semiconductor MAX8907 PMIC Support"
+	select MFD_CORE
+	depends on I2C=y && GENERIC_HARDIRQS
+	select REGMAP_I2C
+	select REGMAP_IRQ
+	help
+	  Say yes here to support for Maxim Semiconductor MAX8907. This is
+	  a Power Management IC. This driver provides common support for
+	  accessing the device; additional drivers must be enabled in order
+	  to use the functionality of the device.
+
+config MFD_MAX8925
+	bool "Maxim Semiconductor MAX8925 PMIC Support"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select MFD_CORE
+	help
+	  Say yes here to support for Maxim Semiconductor MAX8925. This is
+	  a Power Management IC. This driver provides common support for
+	  accessing the device, additional drivers must be enabled in order
+	  to use the functionality of the device.
+
+config MFD_MAX8997
+	bool "Maxim Semiconductor MAX8997/8966 PMIC Support"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select MFD_CORE
+	select IRQ_DOMAIN
+	help
+	  Say yes here to support for Maxim Semiconductor MAX8997/8966.
+	  This is a Power Management IC with RTC, Flash, Fuel Gauge, Haptic,
+	  MUIC controls on chip.
+	  This driver provides common support for accessing the device;
+	  additional drivers must be enabled in order to use the functionality
+	  of the device.
+
+config MFD_MAX8998
+	bool "Maxim Semiconductor MAX8998/National LP3974 PMIC Support"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select MFD_CORE
+	help
+	  Say yes here to support for Maxim Semiconductor MAX8998 and
+	  National Semiconductor LP3974. This is a Power Management IC.
+	  This driver provides common support for accessing the device,
+	  additional drivers must be enabled in order to use the functionality
+	  of the device.
+
+config EZX_PCAP
+	bool "Motorola EZXPCAP Support"
+	depends on GENERIC_HARDIRQS && SPI_MASTER
+	help
+	  This enables the PCAP ASIC present on EZX Phones. This is
+	  needed for MMC, TouchScreen, Sound, USB, etc..
+
+config MFD_VIPERBOARD
+        tristate "Nano River Technologies Viperboard"
+	select MFD_CORE
+	depends on USB && GENERIC_HARDIRQS
+	default n
+	help
+	  Say yes here if you want support for Nano River Technologies
+	  Viperboard.
+	  There are mfd cell drivers available for i2c master, adc and
+	  both gpios found on the board. The spi part does not yet
+	  have a driver.
+	  You need to select the mfd cell drivers separately.
+	  The drivers do not support all features the board exposes.
+
+config MFD_RETU
+	tristate "Nokia Retu and Tahvo multi-function device"
+	select MFD_CORE
+	depends on I2C && GENERIC_HARDIRQS
+	select REGMAP_IRQ
+	help
+	  Retu and Tahvo are a multi-function devices found on Nokia
+	  Internet Tablets (770, N800 and N810).
+
+config MFD_PCF50633
+	tristate "NXP PCF50633"
+	depends on I2C
+	select REGMAP_I2C
+	help
+	  Say yes here if you have NXP PCF50633 chip on your board.
+	  This core driver provides register access and IRQ handling
+	  facilities, and registers devices for the various functions
+	  so that function-specific drivers can bind to them.
+
+config PCF50633_ADC
+	tristate "NXP PCF50633 ADC"
+	depends on MFD_PCF50633
+	help
+	 Say yes here if you want to include support for ADC in the
+	 NXP PCF50633 chip.
+
+config PCF50633_GPIO
+	tristate "NXP PCF50633 GPIO"
+	depends on MFD_PCF50633
+	help
+	 Say yes here if you want to include support GPIO for pins on
+	 the PCF50633 chip.
+
+config UCB1400_CORE
+	tristate "Philips UCB1400 Core driver"
+	depends on AC97_BUS
+	depends on GPIOLIB
+	help
+	  This enables support for the Philips UCB1400 core functions.
+	  The UCB1400 is an AC97 audio codec.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ucb1400_core.
+
+config MFD_PM8XXX
+	tristate
+
+config MFD_PM8921_CORE
+	tristate "Qualcomm PM8921 PMIC chip"
+	depends on SSBI && BROKEN
+	select MFD_CORE
+	select MFD_PM8XXX
+	help
+	  If you say yes to this option, support will be included for the
+	  built-in PM8921 PMIC chip.
+
+	  This is required if your board has a PM8921 and uses its features,
+	  such as: MPPs, GPIOs, regulators, interrupts, and PWM.
+
+	  Say M here if you want to include support for PM8921 chip as a module.
+	  This will build a module called "pm8921-core".
+
+config MFD_PM8XXX_IRQ
+	bool "Qualcomm PM8xxx IRQ features"
+	depends on MFD_PM8XXX
+	default y if MFD_PM8XXX
+	help
+	  This is the IRQ driver for Qualcomm PM 8xxx PMIC chips.
+
+	  This is required to use certain other PM 8xxx features, such as GPIO
+	  and MPP.
+
+config MFD_RDC321X
+	tristate "RDC R-321x southbridge"
+	select MFD_CORE
+	depends on PCI && GENERIC_HARDIRQS
+	help
+	  Say yes here if you want to have support for the RDC R-321x SoC
+	  southbridge which provides access to GPIOs and Watchdog using the
+	  southbridge PCI device configuration space.
+
+config MFD_RTSX_PCI
+	tristate "Realtek PCI-E card reader"
+	depends on PCI && GENERIC_HARDIRQS
+	select MFD_CORE
+	help
+	  This supports for Realtek PCI-Express card reader including rts5209,
+	  rts5229, rtl8411, etc. Realtek card reader supports access to many
+	  types of memory cards, such as Memory Stick, Memory Stick Pro,
+	  Secure Digital and MultiMediaCard.
+
+config MFD_RC5T583
+	bool "Ricoh RC5T583 Power Management system device"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select MFD_CORE
+	select REGMAP_I2C
+	help
+	  Select this option to get support for the RICOH583 Power
+	  Management system device.
+	  This driver provides common support for accessing the device
+	  through i2c interface. The device supports multiple sub-devices
+	  like GPIO, interrupts, RTC, LDO and DCDC regulators, onkey.
+	  Additional drivers must be enabled in order to use the
+	  different functionality of the device.
+
+config MFD_SEC_CORE
+	bool "SAMSUNG Electronics PMIC Series Support"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select MFD_CORE
+	select REGMAP_I2C
+	select REGMAP_IRQ
+	help
+	 Support for the Samsung Electronics MFD series.
+	 This driver provides common support for accessing the device,
+	 additional drivers must be enabled in order to use the functionality
+	 of the device
+
+config MFD_SI476X_CORE
+	tristate "Silicon Laboratories 4761/64/68 AM/FM radio."
+	depends on I2C
+	select MFD_CORE
+	select REGMAP_I2C
+	help
+	  This is the core driver for the SI476x series of AM/FM
+	  radio. This MFD driver connects the radio-si476x V4L2 module
+	  and the si476x audio codec.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called si476x-core.
+
 config MFD_SM501
-	tristate "Support for Silicon Motion SM501"
+	tristate "Silicon Motion SM501"
 	 ---help---
 	  This is the core driver for the Silicon Motion SM501 multimedia
 	  companion chip. This device is a multifunction device which may
@@ -63,46 +518,147 @@ config MFD_SM501_GPIO
 	 lines on the SM501. The platform data is used to supply the
 	 base number for the first GPIO line to register.
 
-config MFD_RTSX_PCI
-	tristate "Support for Realtek PCI-E card reader"
-	depends on PCI && GENERIC_HARDIRQS
+config MFD_SMSC
+       bool "SMSC ECE1099 series chips"
+       depends on I2C=y && GENERIC_HARDIRQS
+       select MFD_CORE
+       select REGMAP_I2C
+       help
+        If you say yes here you get support for the
+        ece1099 chips from SMSC.
+
+        To compile this driver as a module, choose M here: the
+        module will be called smsc.
+
+config ABX500_CORE
+	bool "ST-Ericsson ABX500 Mixed Signal Circuit register functions"
+	default y if ARCH_U300 || ARCH_U8500
+	help
+	  Say yes here if you have the ABX500 Mixed Signal IC family
+	  chips. This core driver expose register access functions.
+	  Functionality specific drivers using these functions can
+	  remain unchanged when IC changes. Binding of the functions to
+	  actual register access is done by the IC core driver.
+
+config AB3100_CORE
+	bool "ST-Ericsson AB3100 Mixed Signal Circuit core functions"
+	depends on I2C=y && ABX500_CORE && GENERIC_HARDIRQS
 	select MFD_CORE
+	default y if ARCH_U300
 	help
-	  This supports for Realtek PCI-Express card reader including rts5209,
-	  rts5229, rtl8411, etc. Realtek card reader supports access to many
-	  types of memory cards, such as Memory Stick, Memory Stick Pro,
-	  Secure Digital and MultiMediaCard.
+	  Select this to enable the AB3100 Mixed Signal IC core
+	  functionality. This connects to a AB3100 on the I2C bus
+	  and expose a number of symbols needed for dependent devices
+	  to read and write registers and subscribe to events from
+	  this multi-functional IC. This is needed to use other features
+	  of the AB3100 such as battery-backed RTC, charging control,
+	  LEDs, vibrator, system power and temperature, power management
+	  and ALSA sound.
 
-config MFD_ASIC3
-	bool "Support for Compaq ASIC3"
-	depends on GENERIC_HARDIRQS && GPIOLIB && ARM
+config AB3100_OTP
+	tristate "ST-Ericsson AB3100 OTP functions"
+	depends on AB3100_CORE
+	default y if AB3100_CORE
+	help
+	  Select this to enable the AB3100 Mixed Signal IC OTP (one-time
+	  programmable memory) support. This exposes a sysfs file to read
+	  out OTP values.
+
+config AB8500_CORE
+	bool "ST-Ericsson AB8500 Mixed Signal Power Management chip"
+	depends on GENERIC_HARDIRQS && ABX500_CORE && MFD_DB8500_PRCMU
+	select POWER_SUPPLY
 	select MFD_CORE
-	 ---help---
-	  This driver supports the ASIC3 multifunction chip found on many
-	  PDAs (mainly iPAQ and HTC based ones)
+	select IRQ_DOMAIN
+	help
+	  Select this option to enable access to AB8500 power management
+	  chip. This connects to U8500 either on the SSP/SPI bus (deprecated
+	  since hardware version v1.0) or the I2C bus via PRCMU. It also adds
+	  the irq_chip parts for handling the Mixed Signal chip events.
+	  This chip embeds various other multimedia funtionalities as well.
 
-config MFD_DAVINCI_VOICECODEC
-	tristate
+config AB8500_DEBUG
+       bool "Enable debug info via debugfs"
+       depends on AB8500_CORE && DEBUG_FS
+       default y if DEBUG_FS
+       help
+         Select this option if you want debug information using the debug
+         filesystem, debugfs.
+
+config AB8500_GPADC
+	bool "ST-Ericsson AB8500 GPADC driver"
+	depends on AB8500_CORE && REGULATOR_AB8500
+	default y
+	help
+	  AB8500 GPADC driver used to convert Acc and battery/ac/usb voltage
+
+config MFD_DB8500_PRCMU
+	bool "ST-Ericsson DB8500 Power Reset Control Management Unit"
+	depends on UX500_SOC_DB8500
 	select MFD_CORE
+	help
+	  Select this option to enable support for the DB8500 Power Reset
+	  and Control Management Unit. This is basically an autonomous
+	  system controller running an XP70 microprocessor, which is accessed
+	  through a register map.
 
-config MFD_DM355EVM_MSP
-	bool "DaVinci DM355 EVM microcontroller"
-	depends on I2C=y && MACH_DAVINCI_DM355_EVM
+config MFD_STMPE
+	bool "STMicroelectronics STMPE"
+	depends on (I2C=y || SPI_MASTER=y) && GENERIC_HARDIRQS
+	select MFD_CORE
 	help
-	  This driver supports the MSP430 microcontroller used on these
-	  boards.  MSP430 firmware manages resets and power sequencing,
-	  inputs from buttons and the IR remote, LEDs, an RTC, and more.
+	  Support for the STMPE family of I/O Expanders from
+	  STMicroelectronics.
 
-config MFD_TI_SSP
-	tristate "TI Sequencer Serial Port support"
-	depends on ARCH_DAVINCI_TNETV107X && GENERIC_HARDIRQS
+	  Currently supported devices are:
+
+		STMPE811: GPIO, Touchscreen
+		STMPE1601: GPIO, Keypad
+		STMPE1801: GPIO, Keypad
+		STMPE2401: GPIO, Keypad
+		STMPE2403: GPIO, Keypad
+
+	  This driver provides common support for accessing the device,
+	  additional drivers must be enabled in order to use the functionality
+	  of the device.  Currently available sub drivers are:
+
+		GPIO: stmpe-gpio
+		Keypad: stmpe-keypad
+		Touchscreen: stmpe-ts
+
+menu "STMicroelectronics STMPE Interface Drivers"
+depends on MFD_STMPE
+
+config STMPE_I2C
+	bool "STMicroelectronics STMPE I2C Inteface"
+	depends on I2C=y
+	default y
+	help
+	  This is used to enable I2C interface of STMPE
+
+config STMPE_SPI
+	bool "STMicroelectronics STMPE SPI Inteface"
+	depends on SPI_MASTER
+	help
+	  This is used to enable SPI interface of STMPE
+endmenu
+
+config MFD_STA2X11
+	bool "STMicroelectronics STA2X11"
+	depends on STA2X11 && GENERIC_HARDIRQS
 	select MFD_CORE
-	---help---
-	  Say Y here if you want support for the Sequencer Serial Port
-	  in a Texas Instruments TNETV107X SoC.
+	select REGMAP_MMIO
 
-	  To compile this driver as a module, choose M here: the
-	  module will be called ti-ssp.
+config MFD_SYSCON
+	bool "System Controller Register R/W Based on Regmap"
+	select REGMAP_MMIO
+	help
+	  Select this option to enable accessing system control registers
+	  via regmap.
+
+config MFD_DAVINCI_VOICECODEC
+	tristate
+	select MFD_CORE
 
 config MFD_TI_AM335X_TSCADC
 	tristate "TI ADC / Touch Screen chip support"
@@ -116,60 +672,56 @@ config MFD_TI_AM335X_TSCADC
 	  To compile this driver as a module, choose M here: the
 	  module will be called ti_am335x_tscadc.
 
-config HTC_EGPIO
-	bool "HTC EGPIO support"
-	depends on GENERIC_HARDIRQS && GPIOLIB && ARM
+config MFD_DM355EVM_MSP
+	bool "TI DaVinci DM355 EVM microcontroller"
+	depends on I2C=y && MACH_DAVINCI_DM355_EVM
 	help
-	    This driver supports the CPLD egpio chip present on
-	    several HTC phones.  It provides basic support for input
-	    pins, output pins, and irqs.
+	  This driver supports the MSP430 microcontroller used on these
+	  boards.  MSP430 firmware manages resets and power sequencing,
+	  inputs from buttons and the IR remote, LEDs, an RTC, and more.
 
-config HTC_PASIC3
-	tristate "HTC PASIC3 LED/DS1WM chip support"
+config MFD_LP8788
+	bool "TI LP8788 Power Management Unit Driver"
+	depends on I2C=y && GENERIC_HARDIRQS
 	select MFD_CORE
-	depends on GENERIC_HARDIRQS
-	help
-	  This core driver provides register access for the LED/DS1WM
-	  chips labeled "AIC2" and "AIC3", found on HTC Blueangel and
-	  HTC Magician devices, respectively. Actual functionality is
-	  handled by the leds-pasic3 and ds1wm drivers.
-
-config HTC_I2CPLD
-	bool "HTC I2C PLD chip support"
-	depends on I2C=y && GPIOLIB
+	select REGMAP_I2C
+	select IRQ_DOMAIN
 	help
-	  If you say yes here you get support for the supposed CPLD
-	  found on omap850 HTC devices like the HTC Wizard and HTC Herald.
-	  This device provides input and output GPIOs through an I2C
-	  interface to one or more sub-chips.
+	  TI LP8788 PMU supports regulators, battery charger, RTC,
+	  ADC, backlight driver and current sinks.
 
-config UCB1400_CORE
-	tristate "Philips UCB1400 Core driver"
-	depends on AC97_BUS
-	depends on GPIOLIB
+config MFD_OMAP_USB_HOST
+	bool "TI OMAP USBHS core and TLL driver"
+	depends on USB_EHCI_HCD_OMAP || USB_OHCI_HCD_OMAP3
+	default y
 	help
-	  This enables support for the Philips UCB1400 core functions.
-	  The UCB1400 is an AC97 audio codec.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called ucb1400_core.
+	  This is the core driver for the OAMP EHCI and OHCI drivers.
+	  This MFD driver does the required setup functionalities for
+	  OMAP USB Host drivers.
 
-config MFD_LM3533
-	tristate "LM3533 Lighting Power chip"
-	depends on I2C
+config MFD_PALMAS
+	bool "TI Palmas series chips"
 	select MFD_CORE
 	select REGMAP_I2C
-	depends on GENERIC_HARDIRQS
+	select REGMAP_IRQ
+	depends on I2C=y && GENERIC_HARDIRQS
 	help
-	  Say yes here to enable support for National Semiconductor / TI
-	  LM3533 Lighting Power chips.
+	  If you say yes here you get support for the Palmas
+	  series of PMIC chips from Texas Instruments.
 
-	  This driver provides common support for accessing the device;
-	  additional drivers must be enabled in order to use the LED,
-	  backlight or ambient-light-sensor functionality of the device.
+config MFD_TI_SSP
+	tristate "TI Sequencer Serial Port support"
+	depends on ARCH_DAVINCI_TNETV107X && GENERIC_HARDIRQS
+	select MFD_CORE
+	---help---
+	  Say Y here if you want support for the Sequencer Serial Port
+	  in a Texas Instruments TNETV107X SoC.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ti-ssp.
 
 config TPS6105X
-	tristate "TPS61050/61052 Boost Converters"
+	tristate "TI TPS61050/61052 Boost Converters"
 	depends on I2C
 	select REGULATOR
 	select MFD_CORE
@@ -182,7 +734,7 @@ config TPS6105X
 	  also contains a GPIO pin.
 
 config TPS65010
-	tristate "TPS6501x Power Management chips"
+	tristate "TI TPS6501x Power Management chips"
 	depends on I2C && GPIOLIB
 	default y if MACH_OMAP_H2 || MACH_OMAP_H3 || MACH_OMAP_OSK
 	help
@@ -195,7 +747,7 @@ config TPS65010
 	  will be called tps65010.
 
 config TPS6507X
-	tristate "TPS6507x Power Management / Touch Screen chips"
+	tristate "TI TPS6507x Power Management / Touch Screen chips"
 	select MFD_CORE
 	depends on I2C && GENERIC_HARDIRQS
 	help
@@ -206,8 +758,24 @@ config TPS6507X
 	  This driver can also be built as a module.  If so, the module
 	  will be called tps6507x.
 
+config TPS65911_COMPARATOR
+	tristate
+
+config MFD_TPS65090
+	bool "TI TPS65090 Power Management chips"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select MFD_CORE
+	select REGMAP_I2C
+	select REGMAP_IRQ
+	help
+	  If you say yes here you get support for the TPS65090 series of
+	  Power Management chips.
+	  This driver provides common support for accessing the device,
+	  additional drivers must be enabled in order to use the
+	  functionality of the device.
+
 config MFD_TPS65217
-	tristate "TPS65217 Power Management / White LED chips"
+	tristate "TI TPS65217 Power Management / White LED chips"
 	depends on I2C && GENERIC_HARDIRQS
 	select MFD_CORE
 	select REGMAP_I2C
@@ -222,7 +790,7 @@ config MFD_TPS65217
 	  will be called tps65217.
 
 config MFD_TPS6586X
-	bool "TPS6586x Power Management chips"
+	bool "TI TPS6586x Power Management chips"
 	depends on I2C=y && GENERIC_HARDIRQS
 	select MFD_CORE
 	select REGMAP_I2C
@@ -237,7 +805,7 @@ config MFD_TPS6586X
 	  will be called tps6586x.
 
 config MFD_TPS65910
-	bool "TPS65910 Power Management chip"
+	bool "TI TPS65910 Power Management chip"
 	depends on I2C=y && GPIOLIB && GENERIC_HARDIRQS
 	select MFD_CORE
 	select REGMAP_I2C
@@ -248,11 +816,14 @@ config MFD_TPS65910
 	  Power Management chips.
 
 config MFD_TPS65912
-	bool
+	bool "TI TPS65912 Power Management chip"
 	depends on GPIOLIB
+	help
+	  If you say yes here you get support for the TPS65912 series of
+	  PM chips.
 
 config MFD_TPS65912_I2C
-	bool "TPS65912 Power Management chip with I2C"
+	bool "TI TPS65912 Power Management chip with I2C"
 	select MFD_CORE
 	select MFD_TPS65912
 	depends on I2C=y && GPIOLIB && GENERIC_HARDIRQS
@@ -261,7 +832,7 @@ config MFD_TPS65912_I2C
 	  PM chips with I2C interface.
 
 config MFD_TPS65912_SPI
-	bool "TPS65912 Power Management chip with SPI"
+	bool "TI TPS65912 Power Management chip with SPI"
 	select MFD_CORE
 	select MFD_TPS65912
 	depends on SPI_MASTER && GPIOLIB && GENERIC_HARDIRQS
@@ -283,18 +854,8 @@ config MFD_TPS80031
 	  ADC, RTC, 2 PWM, System Voltage Regulator/Battery Charger with
 	  Power Path from USB, 32K clock generator.
 
-config MENELAUS
-	bool "Texas Instruments TWL92330/Menelaus PM chip"
-	depends on I2C=y && ARCH_OMAP2
-	help
-	  If you say yes here you get support for the Texas Instruments
-	  TWL92330/Menelaus Power Management chip. This include voltage
-	  regulators, Dual slot memory card transceivers, real-time clock
-	  and other features that are often used in portable devices like
-	  cell phones and PDAs.
-
 config TWL4030_CORE
-	bool "Texas Instruments TWL4030/TWL5030/TWL6030/TPS659x0 Support"
+	bool "TI TWL4030/TWL5030/TWL6030/TPS659x0 Support"
 	depends on I2C=y && GENERIC_HARDIRQS
 	select IRQ_DOMAIN
 	select REGMAP_I2C
@@ -310,7 +871,7 @@ config TWL4030_CORE
 	  versions) and many other features.
 
 config TWL4030_MADC
-	tristate "Texas Instruments TWL4030 MADC"
+	tristate "TI TWL4030 MADC"
 	depends on TWL4030_CORE
 	help
 	This driver provides support for triton TWL4030-MADC. The
@@ -320,7 +881,7 @@ config TWL4030_MADC
 	named twl4030-madc
 
 config TWL4030_POWER
-	bool "Support power resources on TWL4030 family chips"
+	bool "TI TWL4030 power resources"
 	depends on TWL4030_CORE && ARM
 	help
 	  Say yes here if you want to use the power resources on the
@@ -333,13 +894,13 @@ config TWL4030_POWER
 	  or reset when a sleep, wakeup or warm reset event occurs.
 
 config MFD_TWL4030_AUDIO
-	bool
+	bool "TI TWL4030 Audio"
 	depends on TWL4030_CORE && GENERIC_HARDIRQS
 	select MFD_CORE
 	default n
 
 config TWL6040_CORE
-	bool "Support for TWL6040 audio codec"
+	bool "TI TWL6040 audio codec"
 	depends on I2C=y && GENERIC_HARDIRQS
 	select MFD_CORE
 	select REGMAP_I2C
@@ -352,48 +913,53 @@ config TWL6040_CORE
 	  additional drivers must be enabled in order to use the
 	  functionality of the device (audio, vibra).
 
-config MFD_STMPE
-	bool "Support STMicroelectronics STMPE"
-	depends on (I2C=y || SPI_MASTER=y) && GENERIC_HARDIRQS
-	select MFD_CORE
+config MENELAUS
+	bool "TI TWL92330/Menelaus PM chip"
+	depends on I2C=y && ARCH_OMAP2
 	help
-	  Support for the STMPE family of I/O Expanders from
-	  STMicroelectronics.
-
-	  Currently supported devices are:
-
-		STMPE811: GPIO, Touchscreen
-		STMPE1601: GPIO, Keypad
-		STMPE2401: GPIO, Keypad
-		STMPE2403: GPIO, Keypad
+	  If you say yes here you get support for the Texas Instruments
+	  TWL92330/Menelaus Power Management chip. This include voltage
+	  regulators, Dual slot memory card transceivers, real-time clock
+	  and other features that are often used in portable devices like
+	  cell phones and PDAs.
 
-	  This driver provides common support for accessing the device,
-	  additional drivers must be enabled in order to use the functionality
-	  of the device.  Currently available sub drivers are:
+config MFD_WL1273_CORE
+	tristate "TI WL1273 FM radio"
+	depends on I2C && GENERIC_HARDIRQS
+	select MFD_CORE
+	default n
+	help
+	  This is the core driver for the TI WL1273 FM radio. This MFD
+	  driver connects the radio-wl1273 V4L2 module and the wl1273
+	  audio codec.
 
-		GPIO: stmpe-gpio
-		Keypad: stmpe-keypad
-		Touchscreen: stmpe-ts
+config MFD_LM3533
+	tristate "TI/National Semiconductor LM3533 Lighting Power chip"
+	depends on I2C
+	select MFD_CORE
+	select REGMAP_I2C
+	depends on GENERIC_HARDIRQS
+	help
+	  Say yes here to enable support for National Semiconductor / TI
+	  LM3533 Lighting Power chips.
 
-menu "STMPE Interface Drivers"
-depends on MFD_STMPE
+	  This driver provides common support for accessing the device;
+	  additional drivers must be enabled in order to use the LED,
+	  backlight or ambient-light-sensor functionality of the device.
 
-config STMPE_I2C
-	bool "STMPE I2C Inteface"
-	depends on I2C=y
-	default y
-	help
-	  This is used to enable I2C interface of STMPE
+config MFD_TIMBERDALE
+	tristate "Timberdale FPGA"
+	select MFD_CORE
+	depends on PCI && GPIOLIB
+	---help---
+	This is the core driver for the timberdale FPGA. This device is a
+	multifunction device which exposes numerous platform devices.
 
-config STMPE_SPI
-	bool "STMPE SPI Inteface"
-	depends on SPI_MASTER
-	help
-	  This is used to enable SPI interface of STMPE
-endmenu
+	The timberdale FPGA can be found on the Intel Atom development board
+	for in-vehicle infontainment, called Russellville.
 
 config MFD_TC3589X
-	bool "Support Toshiba TC35892 and variants"
+	bool "Toshiba TC35892 and variants"
 	depends on I2C=y && GENERIC_HARDIRQS
 	select MFD_CORE
 	help
@@ -408,27 +974,15 @@ config MFD_TMIO
 	default n
 
 config MFD_T7L66XB
-	bool "Support Toshiba T7L66XB"
+	bool "Toshiba T7L66XB"
 	depends on ARM && HAVE_CLK && GENERIC_HARDIRQS
 	select MFD_CORE
 	select MFD_TMIO
 	help
 	  Support for Toshiba Mobile IO Controller T7L66XB
 
-config MFD_SMSC
-       bool "Support for the SMSC ECE1099 series chips"
-       depends on I2C=y && GENERIC_HARDIRQS
-       select MFD_CORE
-       select REGMAP_I2C
-       help
-        If you say yes here you get support for the
-        ece1099 chips from SMSC.
-
-        To compile this driver as a module, choose M here: the
-        module will be called smsc.
-
 config MFD_TC6387XB
-	bool "Support Toshiba TC6387XB"
+	bool "Toshiba TC6387XB"
 	depends on ARM && HAVE_CLK
 	select MFD_CORE
 	select MFD_TMIO
@@ -436,7 +990,7 @@ config MFD_TC6387XB
 	  Support for Toshiba Mobile IO Controller TC6387XB
 
 config MFD_TC6393XB
-	bool "Support Toshiba TC6393XB"
+	bool "Toshiba TC6393XB"
 	depends on ARM && HAVE_CLK
 	select GPIOLIB
 	select MFD_CORE
@@ -444,165 +998,14 @@ config MFD_TC6393XB
 	help
 	  Support for Toshiba Mobile IO Controller TC6393XB
 
-config PMIC_DA903X
-	bool "Dialog Semiconductor DA9030/DA9034 PMIC Support"
-	depends on I2C=y
-	help
-	  Say yes here to support for Dialog Semiconductor DA9030 (a.k.a
-	  ARAVA) and DA9034 (a.k.a MICCO), these are Power Management IC
-	  usually found on PXA processors-based platforms. This includes
-	  the I2C driver and the core APIs _only_, you have to select
-	  individual components like LCD backlight, voltage regulators,
-	  LEDs and battery-charger under the corresponding menus.
-
-config PMIC_DA9052
-	bool
-	select MFD_CORE
-
-config MFD_DA9052_SPI
-	bool "Support Dialog Semiconductor DA9052/53 PMIC variants with SPI"
-	select REGMAP_SPI
-	select REGMAP_IRQ
-	select PMIC_DA9052
-	depends on SPI_MASTER=y && GENERIC_HARDIRQS
-	help
-	  Support for the Dialog Semiconductor DA9052 PMIC
-	  when controlled using SPI. This driver provides common support
-	  for accessing the device, additional drivers must be enabled in
-	  order to use the functionality of the device.
-
-config MFD_DA9052_I2C
-	bool "Support Dialog Semiconductor DA9052/53 PMIC variants with I2C"
-	select REGMAP_I2C
-	select REGMAP_IRQ
-	select PMIC_DA9052
-	depends on I2C=y && GENERIC_HARDIRQS
-	help
-	  Support for the Dialog Semiconductor DA9052 PMIC
-	  when controlled using I2C. This driver provides common support
-	  for accessing the device, additional drivers must be enabled in
-	  order to use the functionality of the device.
-
-config MFD_DA9055
-	bool "Dialog Semiconductor DA9055 PMIC Support"
-	select REGMAP_I2C
-	select REGMAP_IRQ
-	select PMIC_DA9055
-	select MFD_CORE
-	depends on I2C=y && GENERIC_HARDIRQS
-	help
-	  Say yes here for support of Dialog Semiconductor DA9055. This is
-	  a Power Management IC. This driver provides common support for
-	  accessing the device as well as the I2C interface to the chip itself.
-	  Additional drivers must be enabled in order to use the functionality
-	  of the device.
-
-	  This driver can be built as a module. If built as a module it will be
-	  called "da9055"
-
-config PMIC_ADP5520
-	bool "Analog Devices ADP5520/01 MFD PMIC Core Support"
-	depends on I2C=y
-	help
-	  Say yes here to add support for Analog Devices AD5520 and ADP5501,
-	  Multifunction Power Management IC. This includes
-	  the I2C driver and the core APIs _only_, you have to select
-	  individual components like LCD backlight, LEDs, GPIOs and Kepad
-	  under the corresponding menus.
-
-config MFD_LP8788
-	bool "Texas Instruments LP8788 Power Management Unit Driver"
-	depends on I2C=y && GENERIC_HARDIRQS
-	select MFD_CORE
-	select REGMAP_I2C
-	select IRQ_DOMAIN
-	help
-	  TI LP8788 PMU supports regulators, battery charger, RTC,
-	  ADC, backlight driver and current sinks.
-
-config MFD_MAX77686
-	bool "Maxim Semiconductor MAX77686 PMIC Support"
-	depends on I2C=y && GENERIC_HARDIRQS
-	select MFD_CORE
-	select REGMAP_I2C
-	select IRQ_DOMAIN
-	help
-	  Say yes here to support for Maxim Semiconductor MAX77686.
-	  This is a Power Management IC with RTC on chip.
-	  This driver provides common support for accessing the device;
-	  additional drivers must be enabled in order to use the functionality
-	  of the device.
-
-config MFD_MAX77693
-	bool "Maxim Semiconductor MAX77693 PMIC Support"
-	depends on I2C=y && GENERIC_HARDIRQS
-	select MFD_CORE
-	select REGMAP_I2C
-	help
-	  Say yes here to support for Maxim Semiconductor MAX77693.
-	  This is a companion Power Management IC with Flash, Haptic, Charger,
-	  and MUIC(Micro USB Interface Controller) controls on chip.
-	  This driver provides common support for accessing the device;
-	  additional drivers must be enabled in order to use the functionality
-	  of the device.
-
-config MFD_MAX8907
-	tristate "Maxim Semiconductor MAX8907 PMIC Support"
-	select MFD_CORE
-	depends on I2C=y && GENERIC_HARDIRQS
-	select REGMAP_I2C
-	select REGMAP_IRQ
-	help
-	  Say yes here to support for Maxim Semiconductor MAX8907. This is
-	  a Power Management IC. This driver provides common support for
-	  accessing the device; additional drivers must be enabled in order
-	  to use the functionality of the device.
-
-config MFD_MAX8925
-	bool "Maxim Semiconductor MAX8925 PMIC Support"
-	depends on I2C=y && GENERIC_HARDIRQS
-	select MFD_CORE
-	help
-	  Say yes here to support for Maxim Semiconductor MAX8925. This is
-	  a Power Management IC. This driver provides common support for
-	  accessing the device, additional drivers must be enabled in order
-	  to use the functionality of the device.
-
-config MFD_MAX8997
-	bool "Maxim Semiconductor MAX8997/8966 PMIC Support"
-	depends on I2C=y && GENERIC_HARDIRQS
-	select MFD_CORE
-	select IRQ_DOMAIN
-	help
-	  Say yes here to support for Maxim Semiconductor MAX8997/8966.
-	  This is a Power Management IC with RTC, Flash, Fuel Gauge, Haptic,
-	  MUIC controls on chip.
-	  This driver provides common support for accessing the device;
-	  additional drivers must be enabled in order to use the functionality
-	  of the device.
-
-config MFD_MAX8998
-	bool "Maxim Semiconductor MAX8998/National LP3974 PMIC Support"
-	depends on I2C=y && GENERIC_HARDIRQS
-	select MFD_CORE
-	help
-	  Say yes here to support for Maxim Semiconductor MAX8998 and
-	  National Semiconductor LP3974. This is a Power Management IC.
-	  This driver provides common support for accessing the device,
-	  additional drivers must be enabled in order to use the functionality
-	  of the device.
-
-config MFD_SEC_CORE
-	bool "SAMSUNG Electronics PMIC Series Support"
-	depends on I2C=y && GENERIC_HARDIRQS
+config MFD_VX855
+	tristate "VIA VX855/VX875 integrated south bridge"
+	depends on PCI && GENERIC_HARDIRQS
 	select MFD_CORE
-	select REGMAP_I2C
-	select REGMAP_IRQ
 	help
-	 Support for the Samsung Electronics MFD series.
-	 This driver provides common support for accessing the device,
-	 additional drivers must be enabled in order to use the functionality
-	 of the device
+	  Say yes here to enable support for various functions of the
+	  VIA VX855/VX875 south bridge. You will need to enable the vx855_spi
+	  and/or vx855_gpio drivers for this to do anything useful.
 
 config MFD_ARIZONA
 	select REGMAP
@@ -611,7 +1014,7 @@ config MFD_ARIZONA
 	bool
 
 config MFD_ARIZONA_I2C
-	tristate "Support Wolfson Microelectronics Arizona platform with I2C"
+	tristate "Wolfson Microelectronics Arizona platform with I2C"
 	select MFD_ARIZONA
 	select MFD_CORE
 	select REGMAP_I2C
@@ -621,7 +1024,7 @@ config MFD_ARIZONA_I2C
 	  core functionality controlled via I2C.
 
 config MFD_ARIZONA_SPI
-	tristate "Support Wolfson Microelectronics Arizona platform with SPI"
+	tristate "Wolfson Microelectronics Arizona platform with SPI"
 	select MFD_ARIZONA
 	select MFD_CORE
 	select REGMAP_SPI
@@ -631,19 +1034,19 @@ config MFD_ARIZONA_SPI
 	  core functionality controlled via I2C.
 
 config MFD_WM5102
-	bool "Support Wolfson Microelectronics WM5102"
+	bool "Wolfson Microelectronics WM5102"
 	depends on MFD_ARIZONA
 	help
 	  Support for Wolfson Microelectronics WM5102 low power audio SoC
 
 config MFD_WM5110
-	bool "Support Wolfson Microelectronics WM5110"
+	bool "Wolfson Microelectronics WM5110"
 	depends on MFD_ARIZONA
 	help
 	  Support for Wolfson Microelectronics WM5110 low power audio SoC
 
 config MFD_WM8400
-	bool "Support Wolfson Microelectronics WM8400"
+	bool "Wolfson Microelectronics WM8400"
 	select MFD_CORE
 	depends on I2C=y && GENERIC_HARDIRQS
 	select REGMAP_I2C
@@ -658,7 +1061,7 @@ config MFD_WM831X
 	depends on GENERIC_HARDIRQS
 
 config MFD_WM831X_I2C
-	bool "Support Wolfson Microelectronics WM831x/2x PMICs with I2C"
+	bool "Wolfson Microelectronics WM831x/2x PMICs with I2C"
 	select MFD_CORE
 	select MFD_WM831X
 	select REGMAP_I2C
@@ -671,7 +1074,7 @@ config MFD_WM831X_I2C
 	  order to use the functionality of the device.
 
 config MFD_WM831X_SPI
-	bool "Support Wolfson Microelectronics WM831x/2x PMICs with SPI"
+	bool "Wolfson Microelectronics WM831x/2x PMICs with SPI"
 	select MFD_CORE
 	select MFD_WM831X
 	select REGMAP_SPI
@@ -687,56 +1090,8 @@ config MFD_WM8350
 	bool
 	depends on GENERIC_HARDIRQS
 
-config MFD_WM8350_CONFIG_MODE_0
-	bool
-	depends on MFD_WM8350
-
-config MFD_WM8350_CONFIG_MODE_1
-	bool
-	depends on MFD_WM8350
-
-config MFD_WM8350_CONFIG_MODE_2
-	bool
-	depends on MFD_WM8350
-
-config MFD_WM8350_CONFIG_MODE_3
-	bool
-	depends on MFD_WM8350
-
-config MFD_WM8351_CONFIG_MODE_0
-	bool
-	depends on MFD_WM8350
-
-config MFD_WM8351_CONFIG_MODE_1
-	bool
-	depends on MFD_WM8350
-
-config MFD_WM8351_CONFIG_MODE_2
-	bool
-	depends on MFD_WM8350
-
-config MFD_WM8351_CONFIG_MODE_3
-	bool
-	depends on MFD_WM8350
-
-config MFD_WM8352_CONFIG_MODE_0
-	bool
-	depends on MFD_WM8350
-
-config MFD_WM8352_CONFIG_MODE_1
-	bool
-	depends on MFD_WM8350
-
-config MFD_WM8352_CONFIG_MODE_2
-	bool
-	depends on MFD_WM8350
-
-config MFD_WM8352_CONFIG_MODE_3
-	bool
-	depends on MFD_WM8350
-
 config MFD_WM8350_I2C
-	bool "Support Wolfson Microelectronics WM8350 with I2C"
+	bool "Wolfson Microelectronics WM8350 with I2C"
 	select MFD_WM8350
 	depends on I2C=y && GENERIC_HARDIRQS
 	help
@@ -747,7 +1102,7 @@ config MFD_WM8350_I2C
 	  selected to enable support for the functionality of the chip.
 
 config MFD_WM8994
-	bool "Support Wolfson Microelectronics WM8994"
+	bool "Wolfson Microelectronics WM8994"
 	select MFD_CORE
 	select REGMAP_I2C
 	select REGMAP_IRQ
@@ -760,365 +1115,6 @@ config MFD_WM8994
 	  core support for the WM8994, in order to use the actual
 	  functionaltiy of the device other drivers must be enabled.
 
-config MFD_PCF50633
-	tristate "Support for NXP PCF50633"
-	depends on I2C
-	select REGMAP_I2C
-	help
-	  Say yes here if you have NXP PCF50633 chip on your board.
-	  This core driver provides register access and IRQ handling
-	  facilities, and registers devices for the various functions
-	  so that function-specific drivers can bind to them.
-
-config PCF50633_ADC
-	tristate "Support for NXP PCF50633 ADC"
-	depends on MFD_PCF50633
-	help
-	 Say yes here if you want to include support for ADC in the
-	 NXP PCF50633 chip.
-
-config PCF50633_GPIO
-	tristate "Support for NXP PCF50633 GPIO"
-	depends on MFD_PCF50633
-	help
-	 Say yes here if you want to include support GPIO for pins on
-	 the PCF50633 chip.
-
-config MFD_MC13783
-	tristate
-
-config MFD_MC13XXX
-	tristate
-	depends on (SPI_MASTER || I2C) && GENERIC_HARDIRQS
-	select MFD_CORE
-	select MFD_MC13783
-	help
-	  Enable support for the Freescale MC13783 and MC13892 PMICs.
-	  This driver provides common support for accessing the device,
-	  additional drivers must be enabled in order to use the
-	  functionality of the device.
-
-config MFD_MC13XXX_SPI
-	tristate "Freescale MC13783 and MC13892 SPI interface"
-	depends on SPI_MASTER && GENERIC_HARDIRQS
-	select REGMAP_SPI
-	select MFD_MC13XXX
-	help
-	  Select this if your MC13xxx is connected via an SPI bus.
-
-config MFD_MC13XXX_I2C
-	tristate "Freescale MC13892 I2C interface"
-	depends on I2C && GENERIC_HARDIRQS
-	select REGMAP_I2C
-	select MFD_MC13XXX
-	help
-	  Select this if your MC13xxx is connected via an I2C bus.
-
-config ABX500_CORE
-	bool "ST-Ericsson ABX500 Mixed Signal Circuit register functions"
-	default y if ARCH_U300 || ARCH_U8500
-	help
-	  Say yes here if you have the ABX500 Mixed Signal IC family
-	  chips. This core driver expose register access functions.
-	  Functionality specific drivers using these functions can
-	  remain unchanged when IC changes. Binding of the functions to
-	  actual register access is done by the IC core driver.
-
-config AB3100_CORE
-	bool "ST-Ericsson AB3100 Mixed Signal Circuit core functions"
-	depends on I2C=y && ABX500_CORE && GENERIC_HARDIRQS
-	select MFD_CORE
-	default y if ARCH_U300
-	help
-	  Select this to enable the AB3100 Mixed Signal IC core
-	  functionality. This connects to a AB3100 on the I2C bus
-	  and expose a number of symbols needed for dependent devices
-	  to read and write registers and subscribe to events from
-	  this multi-functional IC. This is needed to use other features
-	  of the AB3100 such as battery-backed RTC, charging control,
-	  LEDs, vibrator, system power and temperature, power management
-	  and ALSA sound.
-
-config AB3100_OTP
-	tristate "ST-Ericsson AB3100 OTP functions"
-	depends on AB3100_CORE
-	default y if AB3100_CORE
-	help
-	  Select this to enable the AB3100 Mixed Signal IC OTP (one-time
-	  programmable memory) support. This exposes a sysfs file to read
-	  out OTP values.
-
-config EZX_PCAP
-	bool "PCAP Support"
-	depends on GENERIC_HARDIRQS && SPI_MASTER
-	help
-	  This enables the PCAP ASIC present on EZX Phones. This is
-	  needed for MMC, TouchScreen, Sound, USB, etc..
-
-config AB8500_CORE
-	bool "ST-Ericsson AB8500 Mixed Signal Power Management chip"
-	depends on GENERIC_HARDIRQS && ABX500_CORE && MFD_DB8500_PRCMU
-	select POWER_SUPPLY
-	select MFD_CORE
-	select IRQ_DOMAIN
-	help
-	  Select this option to enable access to AB8500 power management
-	  chip. This connects to U8500 either on the SSP/SPI bus (deprecated
-	  since hardware version v1.0) or the I2C bus via PRCMU. It also adds
-	  the irq_chip parts for handling the Mixed Signal chip events.
-	  This chip embeds various other multimedia funtionalities as well.
-
-config AB8500_DEBUG
-       bool "Enable debug info via debugfs"
-       depends on AB8500_CORE && DEBUG_FS
-       default y if DEBUG_FS
-       help
-         Select this option if you want debug information using the debug
-         filesystem, debugfs.
-
-config AB8500_GPADC
-	bool "AB8500 GPADC driver"
-	depends on AB8500_CORE && REGULATOR_AB8500
-	default y
-	help
-	  AB8500 GPADC driver used to convert Acc and battery/ac/usb voltage
-
-config MFD_DB8500_PRCMU
-	bool "ST-Ericsson DB8500 Power Reset Control Management Unit"
-	depends on UX500_SOC_DB8500
-	select MFD_CORE
-	help
-	  Select this option to enable support for the DB8500 Power Reset
-	  and Control Management Unit. This is basically an autonomous
-	  system controller running an XP70 microprocessor, which is accessed
-	  through a register map.
-
-config MFD_CS5535
-	tristate "Support for CS5535 and CS5536 southbridge core functions"
-	select MFD_CORE
-	depends on PCI && X86
-	---help---
-	  This is the core driver for CS5535/CS5536 MFD functions.  This is
-          necessary for using the board's GPIO and MFGPT functionality.
-
-config MFD_TIMBERDALE
-	tristate "Support for the Timberdale FPGA"
-	select MFD_CORE
-	depends on PCI && GPIOLIB
-	---help---
-	This is the core driver for the timberdale FPGA. This device is a
-	multifunction device which exposes numerous platform devices.
-
-	The timberdale FPGA can be found on the Intel Atom development board
-	for in-vehicle infontainment, called Russellville.
-
-config LPC_SCH
-	tristate "Intel SCH LPC"
-	depends on PCI && GENERIC_HARDIRQS
-	select MFD_CORE
-	help
-	  LPC bridge function of the Intel SCH provides support for
-	  System Management Bus and General Purpose I/O.
-
-config LPC_ICH
-	tristate "Intel ICH LPC"
-	depends on PCI && GENERIC_HARDIRQS
-	select MFD_CORE
-	help
-	  The LPC bridge function of the Intel ICH provides support for
-	  many functional units. This driver provides needed support for
-	  other drivers to control these functions, currently GPIO and
-	  watchdog.
-
-config MFD_RDC321X
-	tristate "Support for RDC-R321x southbridge"
-	select MFD_CORE
-	depends on PCI && GENERIC_HARDIRQS
-	help
-	  Say yes here if you want to have support for the RDC R-321x SoC
-	  southbridge which provides access to GPIOs and Watchdog using the
-	  southbridge PCI device configuration space.
-
-config MFD_JANZ_CMODIO
-	tristate "Support for Janz CMOD-IO PCI MODULbus Carrier Board"
-	select MFD_CORE
-	depends on PCI && GENERIC_HARDIRQS
-	help
-	  This is the core driver for the Janz CMOD-IO PCI MODULbus
-	  carrier board. This device is a PCI to MODULbus bridge which may
-	  host many different types of MODULbus daughterboards, including
-	  CAN and GPIO controllers.
-
-config MFD_JZ4740_ADC
-	bool "Support for the JZ4740 SoC ADC core"
-	select MFD_CORE
-	select GENERIC_IRQ_CHIP
-	depends on MACH_JZ4740
-	help
-	  Say yes here if you want support for the ADC unit in the JZ4740 SoC.
-	  This driver is necessary for jz4740-battery and jz4740-hwmon driver.
-
-config MFD_VX855
-	tristate "Support for VIA VX855/VX875 integrated south bridge"
-	depends on PCI && GENERIC_HARDIRQS
-	select MFD_CORE
-	help
-	  Say yes here to enable support for various functions of the
-	  VIA VX855/VX875 south bridge. You will need to enable the vx855_spi
-	  and/or vx855_gpio drivers for this to do anything useful.
-
-config MFD_WL1273_CORE
-	tristate "Support for TI WL1273 FM radio."
-	depends on I2C && GENERIC_HARDIRQS
-	select MFD_CORE
-	default n
-	help
-	  This is the core driver for the TI WL1273 FM radio. This MFD
-	  driver connects the radio-wl1273 V4L2 module and the wl1273
-	  audio codec.
-
-config MFD_OMAP_USB_HOST
-	bool "Support OMAP USBHS core and TLL driver"
-	depends on USB_EHCI_HCD_OMAP || USB_OHCI_HCD_OMAP3
-	default y
-	help
-	  This is the core driver for the OAMP EHCI and OHCI drivers.
-	  This MFD driver does the required setup functionalities for
-	  OMAP USB Host drivers.
-
-config MFD_PM8XXX
-	tristate
-
-config MFD_PM8921_CORE
-	tristate "Qualcomm PM8921 PMIC chip"
-	depends on SSBI && BROKEN
-	select MFD_CORE
-	select MFD_PM8XXX
-	help
-	  If you say yes to this option, support will be included for the
-	  built-in PM8921 PMIC chip.
-
-	  This is required if your board has a PM8921 and uses its features,
-	  such as: MPPs, GPIOs, regulators, interrupts, and PWM.
-
-	  Say M here if you want to include support for PM8921 chip as a module.
-	  This will build a module called "pm8921-core".
-
-config MFD_PM8XXX_IRQ
-	bool "Support for Qualcomm PM8xxx IRQ features"
-	depends on MFD_PM8XXX
-	default y if MFD_PM8XXX
-	help
-	  This is the IRQ driver for Qualcomm PM 8xxx PMIC chips.
-
-	  This is required to use certain other PM 8xxx features, such as GPIO
-	  and MPP.
-
-config TPS65911_COMPARATOR
-	tristate
-
-config MFD_TPS65090
-	bool "TPS65090 Power Management chips"
-	depends on I2C=y && GENERIC_HARDIRQS
-	select MFD_CORE
-	select REGMAP_I2C
-	select REGMAP_IRQ
-	help
-	  If you say yes here you get support for the TPS65090 series of
-	  Power Management chips.
-	  This driver provides common support for accessing the device,
-	  additional drivers must be enabled in order to use the
-	  functionality of the device.
-
-config MFD_AAT2870_CORE
-	bool "Support for the AnalogicTech AAT2870"
-	select MFD_CORE
-	depends on I2C=y && GPIOLIB && GENERIC_HARDIRQS
-	help
-	  If you say yes here you get support for the AAT2870.
-	  This driver provides common support for accessing the device,
-	  additional drivers must be enabled in order to use the
-	  functionality of the device.
-
-config MFD_INTEL_MSIC
-	bool "Support for Intel MSIC"
-	depends on INTEL_SCU_IPC
-	select MFD_CORE
-	help
-	  Select this option to enable access to Intel MSIC (Avatele
-	  Passage) chip. This chip embeds audio, battery, GPIO, etc.
-	  devices used in Intel Medfield platforms.
-
-config MFD_RC5T583
-	bool "Ricoh RC5T583 Power Management system device"
-	depends on I2C=y && GENERIC_HARDIRQS
-	select MFD_CORE
-	select REGMAP_I2C
-	help
-	  Select this option to get support for the RICOH583 Power
-	  Management system device.
-	  This driver provides common support for accessing the device
-	  through i2c interface. The device supports multiple sub-devices
-	  like GPIO, interrupts, RTC, LDO and DCDC regulators, onkey.
-	  Additional drivers must be enabled in order to use the
-	  different functionality of the device.
-
-config MFD_STA2X11
-	bool "STA2X11 multi function device support"
-	depends on STA2X11 && GENERIC_HARDIRQS
-	select MFD_CORE
-	select REGMAP_MMIO
-
-config MFD_SYSCON
-	bool "System Controller Register R/W Based on Regmap"
-	depends on OF
-	select REGMAP_MMIO
-	help
-	  Select this option to enable accessing system control registers
-	  via regmap.
-
-config MFD_PALMAS
-	bool "Support for the TI Palmas series chips"
-	select MFD_CORE
-	select REGMAP_I2C
-	select REGMAP_IRQ
-	depends on I2C=y && GENERIC_HARDIRQS
-	help
-	  If you say yes here you get support for the Palmas
-	  series of PMIC chips from Texas Instruments.
-
-config MFD_VIPERBOARD
-        tristate "Support for Nano River Technologies Viperboard"
-	select MFD_CORE
-	depends on USB && GENERIC_HARDIRQS
-	default n
-	help
-	  Say yes here if you want support for Nano River Technologies
-	  Viperboard.
-	  There are mfd cell drivers available for i2c master, adc and
-	  both gpios found on the board. The spi part does not yet
-	  have a driver.
-	  You need to select the mfd cell drivers separately.
-	  The drivers do not support all features the board exposes.
-
-config MFD_RETU
-	tristate "Support for Retu multi-function device"
-	select MFD_CORE
-	depends on I2C && GENERIC_HARDIRQS
-	select REGMAP_IRQ
-	help
-	  Retu is a multi-function device found on Nokia Internet Tablets
-	  (770, N800 and N810).
-
-config MFD_AS3711
-	bool "Support for AS3711"
-	select MFD_CORE
-	select REGMAP_I2C
-	select REGMAP_IRQ
-	depends on I2C=y && GENERIC_HARDIRQS
-	help
-	  Support for the AS3711 PMIC from AMS
-
 endmenu
 endif
 
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index b90409c23664..718e94a2a9a7 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -8,8 +8,11 @@ obj-$(CONFIG_MFD_88PM800)	+= 88pm800.o 88pm80x.o
 obj-$(CONFIG_MFD_88PM805)	+= 88pm805.o 88pm80x.o
 obj-$(CONFIG_MFD_SM501)		+= sm501.o
 obj-$(CONFIG_MFD_ASIC3)		+= asic3.o tmio_core.o
+obj-$(CONFIG_MFD_CROS_EC)	+= cros_ec.o
+obj-$(CONFIG_MFD_CROS_EC_I2C)	+= cros_ec_i2c.o
+obj-$(CONFIG_MFD_CROS_EC_SPI)	+= cros_ec_spi.o
 
-rtsx_pci-objs			:= rtsx_pcr.o rts5209.o rts5229.o rtl8411.o rts5227.o
+rtsx_pci-objs			:= rtsx_pcr.o rts5209.o rts5229.o rtl8411.o rts5227.o rts5249.o
 obj-$(CONFIG_MFD_RTSX_PCI)	+= rtsx_pci.o
 
 obj-$(CONFIG_HTC_EGPIO)		+= htc-egpio.o
@@ -131,6 +134,10 @@ obj-$(CONFIG_MFD_JZ4740_ADC)	+= jz4740-adc.o
 obj-$(CONFIG_MFD_TPS6586X)	+= tps6586x.o
 obj-$(CONFIG_MFD_VX855)		+= vx855.o
 obj-$(CONFIG_MFD_WL1273_CORE)	+= wl1273-core.o
+
+si476x-core-y := si476x-cmd.o si476x-prop.o si476x-i2c.o
+obj-$(CONFIG_MFD_SI476X_CORE)	+= si476x-core.o
+
 obj-$(CONFIG_MFD_CS5535)	+= cs5535-mfd.o
 obj-$(CONFIG_MFD_OMAP_USB_HOST)	+= omap-usb-host.o omap-usb-tll.o
 obj-$(CONFIG_MFD_PM8921_CORE) 	+= pm8921-core.o
diff --git a/drivers/mfd/aat2870-core.c b/drivers/mfd/aat2870-core.c
index f1beb4971f87..dfdb0a2b6835 100644
--- a/drivers/mfd/aat2870-core.c
+++ b/drivers/mfd/aat2870-core.c
@@ -367,12 +367,12 @@ static int aat2870_i2c_probe(struct i2c_client *client,
 	int i, j;
 	int ret = 0;
 
-	aat2870 = kzalloc(sizeof(struct aat2870_data), GFP_KERNEL);
+	aat2870 = devm_kzalloc(&client->dev, sizeof(struct aat2870_data),
+				GFP_KERNEL);
 	if (!aat2870) {
 		dev_err(&client->dev,
 			"Failed to allocate memory for aat2870\n");
-		ret = -ENOMEM;
-		goto out;
+		return -ENOMEM;
 	}
 
 	aat2870->dev = &client->dev;
@@ -400,12 +400,12 @@ static int aat2870_i2c_probe(struct i2c_client *client,
 		aat2870->init(aat2870);
 
 	if (aat2870->en_pin >= 0) {
-		ret = gpio_request_one(aat2870->en_pin, GPIOF_OUT_INIT_HIGH,
-				       "aat2870-en");
+		ret = devm_gpio_request_one(&client->dev, aat2870->en_pin,
+					GPIOF_OUT_INIT_HIGH, "aat2870-en");
 		if (ret < 0) {
 			dev_err(&client->dev,
 				"Failed to request GPIO %d\n", aat2870->en_pin);
-			goto out_kfree;
+			return ret;
 		}
 	}
 
@@ -436,11 +436,6 @@ static int aat2870_i2c_probe(struct i2c_client *client,
 
 out_disable:
 	aat2870_disable(aat2870);
-	if (aat2870->en_pin >= 0)
-		gpio_free(aat2870->en_pin);
-out_kfree:
-	kfree(aat2870);
-out:
 	return ret;
 }
 
@@ -452,11 +447,8 @@ static int aat2870_i2c_remove(struct i2c_client *client)
 
 	mfd_remove_devices(aat2870->dev);
 	aat2870_disable(aat2870);
-	if (aat2870->en_pin >= 0)
-		gpio_free(aat2870->en_pin);
 	if (aat2870->uninit)
 		aat2870->uninit(aat2870);
-	kfree(aat2870);
 
 	return 0;
 }
diff --git a/drivers/mfd/ab3100-otp.c b/drivers/mfd/ab3100-otp.c
index 8440010eb2b8..d7ce016029fa 100644
--- a/drivers/mfd/ab3100-otp.c
+++ b/drivers/mfd/ab3100-otp.c
@@ -248,19 +248,7 @@ static struct platform_driver ab3100_otp_driver = {
 	.remove	 = __exit_p(ab3100_otp_remove),
 };
 
-static int __init ab3100_otp_init(void)
-{
-	return platform_driver_probe(&ab3100_otp_driver,
-				     ab3100_otp_probe);
-}
-
-static void __exit ab3100_otp_exit(void)
-{
-	platform_driver_unregister(&ab3100_otp_driver);
-}
-
-module_init(ab3100_otp_init);
-module_exit(ab3100_otp_exit);
+module_platform_driver_probe(ab3100_otp_driver, ab3100_otp_probe);
 
 MODULE_AUTHOR("Linus Walleij <linus.walleij@stericsson.com>");
 MODULE_DESCRIPTION("AB3100 OTP Readout Driver");
diff --git a/drivers/mfd/ab8500-core.c b/drivers/mfd/ab8500-core.c
index 7c84ced2e01b..8e8a016effe9 100644
--- a/drivers/mfd/ab8500-core.c
+++ b/drivers/mfd/ab8500-core.c
@@ -95,6 +95,7 @@
 #define AB8500_IT_MASK22_REG		0x55
 #define AB8500_IT_MASK23_REG		0x56
 #define AB8500_IT_MASK24_REG		0x57
+#define AB8500_IT_MASK25_REG		0x58
 
 /*
  * latch hierarchy registers
@@ -102,15 +103,25 @@
 #define AB8500_IT_LATCHHIER1_REG	0x60
 #define AB8500_IT_LATCHHIER2_REG	0x61
 #define AB8500_IT_LATCHHIER3_REG	0x62
+#define AB8540_IT_LATCHHIER4_REG	0x63
 
 #define AB8500_IT_LATCHHIER_NUM		3
+#define AB8540_IT_LATCHHIER_NUM		4
 
 #define AB8500_REV_REG			0x80
 #define AB8500_IC_NAME_REG		0x82
 #define AB8500_SWITCH_OFF_STATUS	0x00
 
 #define AB8500_TURN_ON_STATUS		0x00
+#define AB8505_TURN_ON_STATUS_2	0x04
 
+#define AB8500_CH_USBCH_STAT1_REG	0x02
+#define VBUS_DET_DBNC100		0x02
+#define VBUS_DET_DBNC1			0x01
+
+static DEFINE_SPINLOCK(on_stat_lock);
+static u8 turn_on_stat_mask = 0xFF;
+static u8 turn_on_stat_set;
 static bool no_bm; /* No battery management */
 module_param(no_bm, bool, S_IRUGO);
 
@@ -130,9 +141,15 @@ static const int ab8500_irq_regoffset[AB8500_NUM_IRQ_REGS] = {
 	0, 1, 2, 3, 4, 6, 7, 8, 9, 11, 18, 19, 20, 21,
 };
 
-/* AB9540 support */
+/* AB9540 / AB8505 support */
 static const int ab9540_irq_regoffset[AB9540_NUM_IRQ_REGS] = {
-	0, 1, 2, 3, 4, 6, 7, 8, 9, 11, 18, 19, 20, 21, 12, 13, 24,
+	0, 1, 2, 3, 4, 6, 7, 8, 9, 11, 18, 19, 20, 21, 12, 13, 24, 5, 22, 23
+};
+
+/* AB8540 support */
+static const int ab8540_irq_regoffset[AB8540_NUM_IRQ_REGS] = {
+	0, 1, 2, 3, 4, -1, -1, -1, -1, 11, 18, 19, 20, 21, 12, 13, 24, 5, 22, 23,
+	25, 26, 27, 28, 29, 30, 31,
 };
 
 static const char ab8500_version_str[][7] = {
@@ -352,6 +369,9 @@ static void ab8500_irq_sync_unlock(struct irq_data *data)
 			is_ab8500_1p1_or_earlier(ab8500))
 			continue;
 
+		if (ab8500->irq_reg_offset[i] < 0)
+			continue;
+
 		ab8500->oldmask[i] = new;
 
 		reg = AB8500_IT_MASK1_REG + ab8500->irq_reg_offset[i];
@@ -423,25 +443,38 @@ static struct irq_chip ab8500_irq_chip = {
 	.irq_set_type		= ab8500_irq_set_type,
 };
 
+static void update_latch_offset(u8 *offset, int i)
+{
+	/* Fix inconsistent ITFromLatch25 bit mapping... */
+	if (unlikely(*offset == 17))
+			*offset = 24;
+	/* Fix inconsistent ab8540 bit mapping... */
+	if (unlikely(*offset == 16))
+			*offset = 25;
+	if ((i==3) && (*offset >= 24))
+			*offset += 2;
+}
+
 static int ab8500_handle_hierarchical_line(struct ab8500 *ab8500,
 					int latch_offset, u8 latch_val)
 {
-	int int_bit = __ffs(latch_val);
-	int line, i;
+	int int_bit, line, i;
 
-	do {
-		int_bit = __ffs(latch_val);
+	for (i = 0; i < ab8500->mask_size; i++)
+		if (ab8500->irq_reg_offset[i] == latch_offset)
+			break;
 
-		for (i = 0; i < ab8500->mask_size; i++)
-			if (ab8500->irq_reg_offset[i] == latch_offset)
-				break;
+	if (i >= ab8500->mask_size) {
+		dev_err(ab8500->dev, "Register offset 0x%2x not declared\n",
+				latch_offset);
+		return -ENXIO;
+	}
 
-		if (i >= ab8500->mask_size) {
-			dev_err(ab8500->dev, "Register offset 0x%2x not declared\n",
-					latch_offset);
-			return -ENXIO;
-		}
+	/* ignore masked out interrupts */
+	latch_val &= ~ab8500->mask[i];
 
+	while (latch_val) {
+		int_bit = __ffs(latch_val);
 		line = (i << 3) + int_bit;
 		latch_val &= ~(1 << int_bit);
 
@@ -459,7 +492,7 @@ static int ab8500_handle_hierarchical_line(struct ab8500 *ab8500,
 			line += 1;
 
 		handle_nested_irq(ab8500->irq_base + line);
-	} while (latch_val);
+	}
 
 	return 0;
 }
@@ -474,9 +507,7 @@ static int ab8500_handle_hierarchical_latch(struct ab8500 *ab8500,
 		latch_bit = __ffs(hier_val);
 		latch_offset = (hier_offset << 3) + latch_bit;
 
-		/* Fix inconsistent ITFromLatch25 bit mapping... */
-		if (unlikely(latch_offset == 17))
-			latch_offset = 24;
+		update_latch_offset(&latch_offset, hier_offset);
 
 		status = get_register_interruptible(ab8500,
 				AB8500_INTERRUPT,
@@ -504,7 +535,7 @@ static irqreturn_t ab8500_hierarchical_irq(int irq, void *dev)
 	dev_vdbg(ab8500->dev, "interrupt\n");
 
 	/*  Hierarchical interrupt version */
-	for (i = 0; i < AB8500_IT_LATCHHIER_NUM; i++) {
+	for (i = 0; i < (ab8500->it_latchhier_num); i++) {
 		int status;
 		u8 hier_val;
 
@@ -520,63 +551,6 @@ static irqreturn_t ab8500_hierarchical_irq(int irq, void *dev)
 	return IRQ_HANDLED;
 }
 
-/**
- * ab8500_irq_get_virq(): Map an interrupt on a chip to a virtual IRQ
- *
- * @ab8500: ab8500_irq controller to operate on.
- * @irq: index of the interrupt requested in the chip IRQs
- *
- * Useful for drivers to request their own IRQs.
- */
-static int ab8500_irq_get_virq(struct ab8500 *ab8500, int irq)
-{
-	if (!ab8500)
-		return -EINVAL;
-
-	return irq_create_mapping(ab8500->domain, irq);
-}
-
-static irqreturn_t ab8500_irq(int irq, void *dev)
-{
-	struct ab8500 *ab8500 = dev;
-	int i;
-
-	dev_vdbg(ab8500->dev, "interrupt\n");
-
-	atomic_inc(&ab8500->transfer_ongoing);
-
-	for (i = 0; i < ab8500->mask_size; i++) {
-		int regoffset = ab8500->irq_reg_offset[i];
-		int status;
-		u8 value;
-
-		/*
-		 * Interrupt register 12 doesn't exist prior to AB8500 version
-		 * 2.0
-		 */
-		if (regoffset == 11 && is_ab8500_1p1_or_earlier(ab8500))
-			continue;
-
-		status = get_register_interruptible(ab8500, AB8500_INTERRUPT,
-			AB8500_IT_LATCH1_REG + regoffset, &value);
-		if (status < 0 || value == 0)
-			continue;
-
-		do {
-			int bit = __ffs(value);
-			int line = i * 8 + bit;
-			int virq = ab8500_irq_get_virq(ab8500, line);
-
-			handle_nested_irq(virq);
-			ab8500_debug_register_interrupt(line);
-			value &= ~(1 << bit);
-
-		} while (value);
-	}
-	atomic_dec(&ab8500->transfer_ongoing);
-	return IRQ_HANDLED;
-}
-
 static int ab8500_irq_map(struct irq_domain *d, unsigned int virq,
 				irq_hw_number_t hwirq)
 {
@@ -607,7 +581,9 @@ static int ab8500_irq_init(struct ab8500 *ab8500, struct device_node *np)
 {
 	int num_irqs;
 
-	if (is_ab9540(ab8500))
+	if (is_ab8540(ab8500))
+		num_irqs = AB8540_NR_IRQS;
+	else if (is_ab9540(ab8500))
 		num_irqs = AB9540_NR_IRQS;
 	else if (is_ab8505(ab8500))
 		num_irqs = AB8505_NR_IRQS;
@@ -650,6 +626,15 @@ static struct resource ab8500_gpadc_resources[] = {
 	},
 };
 
+static struct resource ab8505_gpadc_resources[] = {
+	{
+		.name	= "SW_CONV_END",
+		.start	= AB8500_INT_GP_SW_ADC_CONV_END,
+		.end	= AB8500_INT_GP_SW_ADC_CONV_END,
+		.flags	= IORESOURCE_IRQ,
+	},
+};
+
 static struct resource ab8500_rtc_resources[] = {
 	{
 		.name	= "60S",
@@ -973,6 +958,30 @@ static struct resource ab8505_iddet_resources[] = {
 		.end   = AB8505_INT_KEYSTUCK,
 		.flags = IORESOURCE_IRQ,
 	},
+	{
+		.name = "VBUS_DET_R",
+		.start = AB8500_INT_VBUS_DET_R,
+		.end = AB8500_INT_VBUS_DET_R,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "VBUS_DET_F",
+		.start = AB8500_INT_VBUS_DET_F,
+		.end = AB8500_INT_VBUS_DET_F,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "ID_DET_PLUGR",
+		.start = AB8500_INT_ID_DET_PLUGR,
+		.end = AB8500_INT_ID_DET_PLUGR,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "ID_DET_PLUGF",
+		.start = AB8500_INT_ID_DET_PLUGF,
+		.end = AB8500_INT_ID_DET_PLUGF,
+		.flags = IORESOURCE_IRQ,
+	},
 };
 
 static struct resource ab8500_temp_resources[] = {
@@ -984,7 +993,42 @@ static struct resource ab8500_temp_resources[] = {
 	},
 };
 
-static struct mfd_cell abx500_common_devs[] = {
+static struct mfd_cell ab8500_bm_devs[] = {
+	{
+		.name = "ab8500-charger",
+		.of_compatible = "stericsson,ab8500-charger",
+		.num_resources = ARRAY_SIZE(ab8500_charger_resources),
+		.resources = ab8500_charger_resources,
+		.platform_data = &ab8500_bm_data,
+		.pdata_size = sizeof(ab8500_bm_data),
+	},
+	{
+		.name = "ab8500-btemp",
+		.of_compatible = "stericsson,ab8500-btemp",
+		.num_resources = ARRAY_SIZE(ab8500_btemp_resources),
+		.resources = ab8500_btemp_resources,
+		.platform_data = &ab8500_bm_data,
+		.pdata_size = sizeof(ab8500_bm_data),
+	},
+	{
+		.name = "ab8500-fg",
+		.of_compatible = "stericsson,ab8500-fg",
+		.num_resources = ARRAY_SIZE(ab8500_fg_resources),
+		.resources = ab8500_fg_resources,
+		.platform_data = &ab8500_bm_data,
+		.pdata_size = sizeof(ab8500_bm_data),
+	},
+	{
+		.name = "ab8500-chargalg",
+		.of_compatible = "stericsson,ab8500-chargalg",
+		.num_resources = ARRAY_SIZE(ab8500_chargalg_resources),
+		.resources = ab8500_chargalg_resources,
+		.platform_data = &ab8500_bm_data,
+		.pdata_size = sizeof(ab8500_bm_data),
+	},
+};
+
+static struct mfd_cell ab8500_devs[] = {
 #ifdef CONFIG_DEBUG_FS
 	{
 		.name = "ab8500-debug",
@@ -1007,7 +1051,6 @@ static struct mfd_cell abx500_common_devs[] = {
 	},
 	{
 		.name = "ab8500-gpadc",
-		.of_compatible = "stericsson,ab8500-gpadc",
 		.num_resources = ARRAY_SIZE(ab8500_gpadc_resources),
 		.resources = ab8500_gpadc_resources,
 	},
@@ -1024,6 +1067,7 @@ static struct mfd_cell abx500_common_devs[] = {
 		.resources = ab8500_av_acc_detect_resources,
 	},
 	{
+
 		.name = "ab8500-poweron-key",
 		.of_compatible = "stericsson,ab8500-poweron-key",
 		.num_resources = ARRAY_SIZE(ab8500_poweronkey_db_resources),
@@ -1053,82 +1097,221 @@ static struct mfd_cell abx500_common_devs[] = {
 		.of_compatible = "stericsson,ab8500-denc",
 	},
 	{
+		.name = "ab8500-gpio",
+		.of_compatible = "stericsson,ab8500-gpio",
+	},
+	{
 		.name = "abx500-temp",
 		.of_compatible = "stericsson,abx500-temp",
 		.num_resources = ARRAY_SIZE(ab8500_temp_resources),
 		.resources = ab8500_temp_resources,
 	},
+	{
+		.name = "ab8500-usb",
+		.of_compatible = "stericsson,ab8500-usb",
+		.num_resources = ARRAY_SIZE(ab8500_usb_resources),
+		.resources = ab8500_usb_resources,
+	},
+	{
+		.name = "ab8500-codec",
+	},
 };
 
-static struct mfd_cell ab8500_bm_devs[] = {
+static struct mfd_cell ab9540_devs[] = {
+#ifdef CONFIG_DEBUG_FS
 	{
-		.name = "ab8500-charger",
-		.of_compatible = "stericsson,ab8500-charger",
-		.num_resources = ARRAY_SIZE(ab8500_charger_resources),
-		.resources = ab8500_charger_resources,
-		.platform_data = &ab8500_bm_data,
-		.pdata_size = sizeof(ab8500_bm_data),
+		.name = "ab8500-debug",
+		.num_resources = ARRAY_SIZE(ab8500_debug_resources),
+		.resources = ab8500_debug_resources,
 	},
+#endif
 	{
-		.name = "ab8500-btemp",
-		.of_compatible = "stericsson,ab8500-btemp",
-		.num_resources = ARRAY_SIZE(ab8500_btemp_resources),
-		.resources = ab8500_btemp_resources,
-		.platform_data = &ab8500_bm_data,
-		.pdata_size = sizeof(ab8500_bm_data),
+		.name = "ab8500-sysctrl",
 	},
 	{
-		.name = "ab8500-fg",
-		.of_compatible = "stericsson,ab8500-fg",
-		.num_resources = ARRAY_SIZE(ab8500_fg_resources),
-		.resources = ab8500_fg_resources,
-		.platform_data = &ab8500_bm_data,
-		.pdata_size = sizeof(ab8500_bm_data),
+		.name = "ab8500-regulator",
 	},
 	{
-		.name = "ab8500-chargalg",
-		.of_compatible = "stericsson,ab8500-chargalg",
-		.num_resources = ARRAY_SIZE(ab8500_chargalg_resources),
-		.resources = ab8500_chargalg_resources,
-		.platform_data = &ab8500_bm_data,
-		.pdata_size = sizeof(ab8500_bm_data),
+		.name = "abx500-clk",
+		.of_compatible = "stericsson,abx500-clk",
+	},
+	{
+		.name = "ab8500-gpadc",
+		.of_compatible = "stericsson,ab8500-gpadc",
+		.num_resources = ARRAY_SIZE(ab8500_gpadc_resources),
+		.resources = ab8500_gpadc_resources,
+	},
+	{
+		.name = "ab8500-rtc",
+		.num_resources = ARRAY_SIZE(ab8500_rtc_resources),
+		.resources = ab8500_rtc_resources,
+	},
+	{
+		.name = "ab8500-acc-det",
+		.num_resources = ARRAY_SIZE(ab8500_av_acc_detect_resources),
+		.resources = ab8500_av_acc_detect_resources,
+	},
+	{
+		.name = "ab8500-poweron-key",
+		.num_resources = ARRAY_SIZE(ab8500_poweronkey_db_resources),
+		.resources = ab8500_poweronkey_db_resources,
+	},
+	{
+		.name = "ab8500-pwm",
+		.id = 1,
+	},
+	{
+		.name = "ab8500-leds",
+	},
+	{
+		.name = "abx500-temp",
+		.num_resources = ARRAY_SIZE(ab8500_temp_resources),
+		.resources = ab8500_temp_resources,
+	},
+	{
+		.name = "pinctrl-ab9540",
+		.of_compatible = "stericsson,ab9540-gpio",
+	},
+	{
+		.name = "ab9540-usb",
+		.num_resources = ARRAY_SIZE(ab8500_usb_resources),
+		.resources = ab8500_usb_resources,
+	},
+	{
+		.name = "ab9540-codec",
+	},
+	{
+		.name = "ab-iddet",
+		.num_resources = ARRAY_SIZE(ab8505_iddet_resources),
+		.resources = ab8505_iddet_resources,
 	},
 };
 
-static struct mfd_cell ab8500_devs[] = {
+/* Device list for ab8505  */
+static struct mfd_cell ab8505_devs[] = {
+#ifdef CONFIG_DEBUG_FS
 	{
-		.name = "pinctrl-ab8500",
-		.of_compatible = "stericsson,ab8500-gpio",
+		.name = "ab8500-debug",
+		.num_resources = ARRAY_SIZE(ab8500_debug_resources),
+		.resources = ab8500_debug_resources,
+	},
+#endif
+	{
+		.name = "ab8500-sysctrl",
+	},
+	{
+		.name = "ab8500-regulator",
+	},
+	{
+		.name = "abx500-clk",
+		.of_compatible = "stericsson,abx500-clk",
+	},
+	{
+		.name = "ab8500-gpadc",
+		.num_resources = ARRAY_SIZE(ab8505_gpadc_resources),
+		.resources = ab8505_gpadc_resources,
+	},
+	{
+		.name = "ab8500-rtc",
+		.num_resources = ARRAY_SIZE(ab8500_rtc_resources),
+		.resources = ab8500_rtc_resources,
+	},
+	{
+		.name = "ab8500-acc-det",
+		.num_resources = ARRAY_SIZE(ab8500_av_acc_detect_resources),
+		.resources = ab8500_av_acc_detect_resources,
+	},
+	{
+		.name = "ab8500-poweron-key",
+		.num_resources = ARRAY_SIZE(ab8500_poweronkey_db_resources),
+		.resources = ab8500_poweronkey_db_resources,
+	},
+	{
+		.name = "ab8500-pwm",
+		.id = 1,
+	},
+	{
+		.name = "ab8500-leds",
+	},
+	{
+		.name = "ab8500-gpio",
 	},
 	{
 		.name = "ab8500-usb",
-		.of_compatible = "stericsson,ab8500-usb",
 		.num_resources = ARRAY_SIZE(ab8500_usb_resources),
 		.resources = ab8500_usb_resources,
 	},
 	{
 		.name = "ab8500-codec",
-		.of_compatible = "stericsson,ab8500-codec",
+	},
+	{
+		.name = "ab-iddet",
+		.num_resources = ARRAY_SIZE(ab8505_iddet_resources),
+		.resources = ab8505_iddet_resources,
 	},
 };
 
-static struct mfd_cell ab9540_devs[] = {
+static struct mfd_cell ab8540_devs[] = {
+#ifdef CONFIG_DEBUG_FS
 	{
-		.name = "pinctrl-ab9540",
-		.of_compatible = "stericsson,ab9540-gpio",
+		.name = "ab8500-debug",
+		.num_resources = ARRAY_SIZE(ab8500_debug_resources),
+		.resources = ab8500_debug_resources,
 	},
+#endif
 	{
-		.name = "ab9540-usb",
+		.name = "ab8500-sysctrl",
+	},
+	{
+		.name = "ab8500-regulator",
+	},
+	{
+		.name = "abx500-clk",
+		.of_compatible = "stericsson,abx500-clk",
+	},
+	{
+		.name = "ab8500-gpadc",
+		.num_resources = ARRAY_SIZE(ab8505_gpadc_resources),
+		.resources = ab8505_gpadc_resources,
+	},
+	{
+		.name = "ab8500-rtc",
+		.num_resources = ARRAY_SIZE(ab8500_rtc_resources),
+		.resources = ab8500_rtc_resources,
+	},
+	{
+		.name = "ab8500-acc-det",
+		.num_resources = ARRAY_SIZE(ab8500_av_acc_detect_resources),
+		.resources = ab8500_av_acc_detect_resources,
+	},
+	{
+		.name = "ab8500-poweron-key",
+		.num_resources = ARRAY_SIZE(ab8500_poweronkey_db_resources),
+		.resources = ab8500_poweronkey_db_resources,
+	},
+	{
+		.name = "ab8500-pwm",
+		.id = 1,
+	},
+	{
+		.name = "ab8500-leds",
+	},
+	{
+		.name = "abx500-temp",
+		.num_resources = ARRAY_SIZE(ab8500_temp_resources),
+		.resources = ab8500_temp_resources,
+	},
+	{
+		.name = "ab8500-gpio",
+	},
+	{
+		.name = "ab8540-usb",
 		.num_resources = ARRAY_SIZE(ab8500_usb_resources),
 		.resources = ab8500_usb_resources,
 	},
 	{
-		.name = "ab9540-codec",
+		.name = "ab8540-codec",
 	},
-};
-
-/* Device list common to ab9540 and ab8505 */
-static struct mfd_cell ab9540_ab8505_devs[] = {
 	{
 		.name = "ab-iddet",
 		.num_resources = ARRAY_SIZE(ab8505_iddet_resources),
@@ -1142,6 +1325,7 @@ static ssize_t show_chip_id(struct device *dev,
 	struct ab8500 *ab8500;
 
 	ab8500 = dev_get_drvdata(dev);
+
 	return sprintf(buf, "%#x\n", ab8500 ? ab8500->chip_id : -EINVAL);
 }
 
@@ -1171,6 +1355,15 @@ static ssize_t show_switch_off_status(struct device *dev,
 	return sprintf(buf, "%#x\n", value);
 }
 
+/* use mask and set to override the register turn_on_stat value */
+void ab8500_override_turn_on_stat(u8 mask, u8 set)
+{
+	spin_lock(&on_stat_lock);
+	turn_on_stat_mask = mask;
+	turn_on_stat_set = set;
+	spin_unlock(&on_stat_lock);
+}
+
 /*
  * ab8500 has turned on due to (TURN_ON_STATUS):
  * 0x01 PORnVbat
@@ -1194,9 +1387,38 @@ static ssize_t show_turn_on_status(struct device *dev,
 		AB8500_TURN_ON_STATUS, &value);
 	if (ret < 0)
 		return ret;
+
+	/*
+	 * In L9540, turn_on_status register is not updated correctly if
+	 * the device is rebooted with AC/USB charger connected. Due to
+	 * this, the device boots android instead of entering into charge
+	 * only mode. Read the AC/USB status register to detect the charger
+	 * presence and update the turn on status manually.
+	 */
+	if (is_ab9540(ab8500)) {
+		spin_lock(&on_stat_lock);
+		value = (value & turn_on_stat_mask) | turn_on_stat_set;
+		spin_unlock(&on_stat_lock);
+	}
+
 	return sprintf(buf, "%#x\n", value);
 }
 
+static ssize_t show_turn_on_status_2(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	int ret;
+	u8 value;
+	struct ab8500 *ab8500;
+
+	ab8500 = dev_get_drvdata(dev);
+	ret = get_register_interruptible(ab8500, AB8500_SYS_CTRL1_BLOCK,
+		AB8505_TURN_ON_STATUS_2, &value);
+	if (ret < 0)
+		return ret;
+	return sprintf(buf, "%#x\n", (value & 0x1));
+}
+
 static ssize_t show_ab9540_dbbrstn(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
@@ -1253,6 +1475,7 @@ exit:
 static DEVICE_ATTR(chip_id, S_IRUGO, show_chip_id, NULL);
 static DEVICE_ATTR(switch_off_status, S_IRUGO, show_switch_off_status, NULL);
 static DEVICE_ATTR(turn_on_status, S_IRUGO, show_turn_on_status, NULL);
+static DEVICE_ATTR(turn_on_status_2, S_IRUGO, show_turn_on_status_2, NULL);
 static DEVICE_ATTR(dbbrstn, S_IRUGO | S_IWUSR,
 			show_ab9540_dbbrstn, store_ab9540_dbbrstn);
 
@@ -1263,6 +1486,11 @@ static struct attribute *ab8500_sysfs_entries[] = {
 	NULL,
 };
 
+static struct attribute *ab8505_sysfs_entries[] = {
+	&dev_attr_turn_on_status_2.attr,
+	NULL,
+};
+
 static struct attribute *ab9540_sysfs_entries[] = {
 	&dev_attr_chip_id.attr,
 	&dev_attr_switch_off_status.attr,
@@ -1275,6 +1503,10 @@ static struct attribute_group ab8500_attr_group = {
 	.attrs	= ab8500_sysfs_entries,
 };
 
+static struct attribute_group ab8505_attr_group = {
+	.attrs	= ab8505_sysfs_entries,
+};
+
 static struct attribute_group ab9540_attr_group = {
 	.attrs	= ab9540_sysfs_entries,
 };
@@ -1290,6 +1522,15 @@ static int ab8500_probe(struct platform_device *pdev)
 		"Battery level lower than power on reset threshold",
 		"Power on key 1 pressed longer than 10 seconds",
 		"DB8500 thermal shutdown"};
+	static char *turn_on_status[] = {
+		"Battery rising (Vbat)",
+		"Power On Key 1 dbF",
+		"Power On Key 2 dbF",
+		"RTC Alarm",
+		"Main Charger Detect",
+		"Vbus Detect (USB)",
+		"USB ID Detect",
+		"UART Factory Mode Detect"};
 	struct ab8500_platform_data *plat = dev_get_platdata(&pdev->dev);
 	const struct platform_device_id *platid = platform_get_device_id(pdev);
 	enum ab8500_version version = AB8500_VERSION_UNDEFINED;
@@ -1351,13 +1592,20 @@ static int ab8500_probe(struct platform_device *pdev)
 			ab8500->chip_id >> 4,
 			ab8500->chip_id & 0x0F);
 
-	/* Configure AB8500 or AB9540 IRQ */
-	if (is_ab9540(ab8500) || is_ab8505(ab8500)) {
+	/* Configure AB8540 */
+	if (is_ab8540(ab8500)) {
+		ab8500->mask_size = AB8540_NUM_IRQ_REGS;
+		ab8500->irq_reg_offset = ab8540_irq_regoffset;
+		ab8500->it_latchhier_num = AB8540_IT_LATCHHIER_NUM;
+	}/* Configure AB8500 or AB9540 IRQ */
+	else if (is_ab9540(ab8500) || is_ab8505(ab8500)) {
 		ab8500->mask_size = AB9540_NUM_IRQ_REGS;
 		ab8500->irq_reg_offset = ab9540_irq_regoffset;
+		ab8500->it_latchhier_num = AB8500_IT_LATCHHIER_NUM;
 	} else {
 		ab8500->mask_size = AB8500_NUM_IRQ_REGS;
 		ab8500->irq_reg_offset = ab8500_irq_regoffset;
+		ab8500->it_latchhier_num = AB8500_IT_LATCHHIER_NUM;
 	}
 	ab8500->mask = devm_kzalloc(&pdev->dev, ab8500->mask_size, GFP_KERNEL);
 	if (!ab8500->mask)
@@ -1396,10 +1644,36 @@ static int ab8500_probe(struct platform_device *pdev)
 	} else {
 		printk(KERN_CONT " None\n");
 	}
+	ret = get_register_interruptible(ab8500, AB8500_SYS_CTRL1_BLOCK,
+		AB8500_TURN_ON_STATUS, &value);
+	if (ret < 0)
+		return ret;
+	dev_info(ab8500->dev, "turn on reason(s) (%#x): ", value);
+
+	if (value) {
+		for (i = 0; i < ARRAY_SIZE(turn_on_status); i++) {
+			if (value & 1)
+				printk("\"%s\" ", turn_on_status[i]);
+			value = value >> 1;
+		}
+		printk("\n");
+	} else {
+		printk("None\n");
+	}
 
 	if (plat && plat->init)
 		plat->init(ab8500);
 
+	if (is_ab9540(ab8500)) {
+		ret = get_register_interruptible(ab8500, AB8500_CHARGER,
+			AB8500_CH_USBCH_STAT1_REG, &value);
+		if (ret < 0)
+			return ret;
+		if ((value & VBUS_DET_DBNC1) && (value & VBUS_DET_DBNC100))
+			ab8500_override_turn_on_stat(~AB8500_POW_KEY_1_ON,
+						     AB8500_VBUS_DET);
+	}
+
 	/* Clear and mask all interrupts */
 	for (i = 0; i < ab8500->mask_size; i++) {
 		/*
@@ -1410,6 +1684,9 @@ static int ab8500_probe(struct platform_device *pdev)
 				is_ab8500_1p1_or_earlier(ab8500))
 			continue;
 
+		if (ab8500->irq_reg_offset[i] < 0)
+			continue;
+
 		get_register_interruptible(ab8500, AB8500_INTERRUPT,
 			AB8500_IT_LATCH1_REG + ab8500->irq_reg_offset[i],
 			&value);
@@ -1428,26 +1705,10 @@ static int ab8500_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	/*  Activate this feature only in ab9540 */
-	/*  till tests are done on ab8500 1p2 or later*/
-	if (is_ab9540(ab8500)) {
-		ret = devm_request_threaded_irq(&pdev->dev, ab8500->irq, NULL,
-						ab8500_hierarchical_irq,
-						IRQF_ONESHOT | IRQF_NO_SUSPEND,
-						"ab8500", ab8500);
-	}
-	else {
-		ret = devm_request_threaded_irq(&pdev->dev, ab8500->irq, NULL,
-						ab8500_irq,
-						IRQF_ONESHOT | IRQF_NO_SUSPEND,
-						"ab8500", ab8500);
-		if (ret)
-			return ret;
-	}
-
-	ret = mfd_add_devices(ab8500->dev, 0, abx500_common_devs,
-			ARRAY_SIZE(abx500_common_devs), NULL,
-			ab8500->irq_base, ab8500->domain);
+	ret = devm_request_threaded_irq(&pdev->dev, ab8500->irq, NULL,
+			ab8500_hierarchical_irq,
+			IRQF_ONESHOT | IRQF_NO_SUSPEND,
+			"ab8500", ab8500);
 	if (ret)
 		return ret;
 
@@ -1455,6 +1716,14 @@ static int ab8500_probe(struct platform_device *pdev)
 		ret = mfd_add_devices(ab8500->dev, 0, ab9540_devs,
 				ARRAY_SIZE(ab9540_devs), NULL,
 				ab8500->irq_base, ab8500->domain);
+	else if (is_ab8540(ab8500))
+		ret = mfd_add_devices(ab8500->dev, 0, ab8540_devs,
+			      ARRAY_SIZE(ab8540_devs), NULL,
+			      ab8500->irq_base, ab8500->domain);
+	else if (is_ab8505(ab8500))
+		ret = mfd_add_devices(ab8500->dev, 0, ab8505_devs,
+			      ARRAY_SIZE(ab8505_devs), NULL,
+			      ab8500->irq_base, ab8500->domain);
 	else
 		ret = mfd_add_devices(ab8500->dev, 0, ab8500_devs,
 				ARRAY_SIZE(ab8500_devs), NULL,
@@ -1462,13 +1731,6 @@ static int ab8500_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	if (is_ab9540(ab8500) || is_ab8505(ab8500))
-		ret = mfd_add_devices(ab8500->dev, 0, ab9540_ab8505_devs,
-				ARRAY_SIZE(ab9540_ab8505_devs), NULL,
-				ab8500->irq_base, ab8500->domain);
-	if (ret)
-		return ret;
-
 	if (!no_bm) {
 		/* Add battery management devices */
 		ret = mfd_add_devices(ab8500->dev, 0, ab8500_bm_devs,
@@ -1478,12 +1740,19 @@ static int ab8500_probe(struct platform_device *pdev)
 			dev_err(ab8500->dev, "error adding bm devices\n");
 	}
 
-	if (is_ab9540(ab8500))
+	if (((is_ab8505(ab8500) || is_ab9540(ab8500)) &&
+			ab8500->chip_id >= AB8500_CUT2P0) || is_ab8540(ab8500))
 		ret = sysfs_create_group(&ab8500->dev->kobj,
 					&ab9540_attr_group);
 	else
 		ret = sysfs_create_group(&ab8500->dev->kobj,
 					&ab8500_attr_group);
+
+	if ((is_ab8505(ab8500) || is_ab9540(ab8500)) &&
+			ab8500->chip_id >= AB8500_CUT2P0)
+		ret = sysfs_create_group(&ab8500->dev->kobj,
+					 &ab8505_attr_group);
+
 	if (ret)
 		dev_err(ab8500->dev, "error creating sysfs entries\n");
 
@@ -1494,11 +1763,16 @@ static int ab8500_remove(struct platform_device *pdev)
 {
 	struct ab8500 *ab8500 = platform_get_drvdata(pdev);
 
-	if (is_ab9540(ab8500))
+	if (((is_ab8505(ab8500) || is_ab9540(ab8500)) &&
+			ab8500->chip_id >= AB8500_CUT2P0) || is_ab8540(ab8500))
 		sysfs_remove_group(&ab8500->dev->kobj, &ab9540_attr_group);
 	else
 		sysfs_remove_group(&ab8500->dev->kobj, &ab8500_attr_group);
 
+	if ((is_ab8505(ab8500) || is_ab9540(ab8500)) &&
+			ab8500->chip_id >= AB8500_CUT2P0)
+		sysfs_remove_group(&ab8500->dev->kobj, &ab8505_attr_group);
+
 	mfd_remove_devices(ab8500->dev);
 
 	return 0;
diff --git a/drivers/mfd/ab8500-debugfs.c b/drivers/mfd/ab8500-debugfs.c
index 45fe3c50eb03..b88bbbc15f1e 100644
--- a/drivers/mfd/ab8500-debugfs.c
+++ b/drivers/mfd/ab8500-debugfs.c
@@ -80,6 +80,7 @@
 #include <linux/interrupt.h>
 #include <linux/kobject.h>
 #include <linux/slab.h>
+#include <linux/irq.h>
 
 #include <linux/mfd/abx500.h>
 #include <linux/mfd/abx500/ab8500.h>
@@ -90,6 +91,9 @@
 #include <linux/ctype.h>
 #endif
 
+/* TODO: this file should not reference IRQ_DB8500_AB8500! */
+#include <mach/irqs.h>
+
 static u32 debug_bank;
 static u32 debug_address;
 
@@ -101,6 +105,11 @@ static int num_irqs;
 static struct device_attribute **dev_attr;
 static char **event_name;
 
+static u8 avg_sample = SAMPLE_16;
+static u8 trig_edge = RISING_EDGE;
+static u8 conv_type = ADC_SW;
+static u8 trig_timer;
+
 /**
  * struct ab8500_reg_range
  * @first: the first address of the range
@@ -150,7 +159,9 @@ static struct hwreg_cfg hwreg_cfg = {
 
 #define AB8500_REV_REG 0x80
 
-static struct ab8500_prcmu_ranges debug_ranges[AB8500_NUM_BANKS] = {
+static struct ab8500_prcmu_ranges *debug_ranges;
+
+struct ab8500_prcmu_ranges ab8500_debug_ranges[AB8500_NUM_BANKS] = {
 	[0x0] = {
 		.num_ranges = 0,
 		.range = NULL,
@@ -354,7 +365,7 @@ static struct ab8500_prcmu_ranges debug_ranges[AB8500_NUM_BANKS] = {
 			},
 			{
 				.first = 0xf5,
-				.last =	0xf6,
+				.last = 0xf6,
 			},
 		},
 	},
@@ -479,6 +490,781 @@ static struct ab8500_prcmu_ranges debug_ranges[AB8500_NUM_BANKS] = {
 	},
 };
 
+struct ab8500_prcmu_ranges ab8505_debug_ranges[AB8500_NUM_BANKS] = {
+	[0x0] = {
+		.num_ranges = 0,
+		.range = NULL,
+	},
+	[AB8500_SYS_CTRL1_BLOCK] = {
+		.num_ranges = 5,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x04,
+			},
+			{
+				.first = 0x42,
+				.last = 0x42,
+			},
+			{
+				.first = 0x52,
+				.last = 0x52,
+			},
+			{
+				.first = 0x54,
+				.last = 0x57,
+			},
+			{
+				.first = 0x80,
+				.last = 0x83,
+			},
+		},
+	},
+	[AB8500_SYS_CTRL2_BLOCK] = {
+		.num_ranges = 5,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x0D,
+			},
+			{
+				.first = 0x0F,
+				.last = 0x17,
+			},
+			{
+				.first = 0x20,
+				.last = 0x20,
+			},
+			{
+				.first = 0x30,
+				.last = 0x30,
+			},
+			{
+				.first = 0x32,
+				.last = 0x3A,
+			},
+		},
+	},
+	[AB8500_REGU_CTRL1] = {
+		.num_ranges = 3,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x00,
+			},
+			{
+				.first = 0x03,
+				.last = 0x11,
+			},
+			{
+				.first = 0x80,
+				.last = 0x86,
+			},
+		},
+	},
+	[AB8500_REGU_CTRL2] = {
+		.num_ranges = 6,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x06,
+			},
+			{
+				.first = 0x08,
+				.last = 0x15,
+			},
+			{
+				.first = 0x17,
+				.last = 0x19,
+			},
+			{
+				.first = 0x1B,
+				.last = 0x1D,
+			},
+			{
+				.first = 0x1F,
+				.last = 0x30,
+			},
+			{
+				.first = 0x40,
+				.last = 0x48,
+			},
+			/* 0x80-0x8B is SIM registers and should
+			 * not be accessed from here */
+		},
+	},
+	[AB8500_USB] = {
+		.num_ranges = 3,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x80,
+				.last = 0x83,
+			},
+			{
+				.first = 0x87,
+				.last = 0x8A,
+			},
+			{
+				.first = 0x91,
+				.last = 0x94,
+			},
+		},
+	},
+	[AB8500_TVOUT] = {
+		.num_ranges = 0,
+		.range = NULL,
+	},
+	[AB8500_DBI] = {
+		.num_ranges = 0,
+		.range = NULL,
+	},
+	[AB8500_ECI_AV_ACC] = {
+		.num_ranges = 1,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x80,
+				.last = 0x82,
+			},
+		},
+	},
+	[AB8500_RESERVED] = {
+		.num_ranges = 0,
+		.range = NULL,
+	},
+	[AB8500_GPADC] = {
+		.num_ranges = 1,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x08,
+			},
+		},
+	},
+	[AB8500_CHARGER] = {
+		.num_ranges = 9,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x02,
+				.last = 0x03,
+			},
+			{
+				.first = 0x05,
+				.last = 0x05,
+			},
+			{
+				.first = 0x40,
+				.last = 0x44,
+			},
+			{
+				.first = 0x50,
+				.last = 0x57,
+			},
+			{
+				.first = 0x60,
+				.last = 0x60,
+			},
+			{
+				.first = 0xA0,
+				.last = 0xA7,
+			},
+			{
+				.first = 0xAF,
+				.last = 0xB2,
+			},
+			{
+				.first = 0xC0,
+				.last = 0xC2,
+			},
+			{
+				.first = 0xF5,
+				.last = 0xF5,
+			},
+		},
+	},
+	[AB8500_GAS_GAUGE] = {
+		.num_ranges = 3,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x00,
+			},
+			{
+				.first = 0x07,
+				.last = 0x0A,
+			},
+			{
+				.first = 0x10,
+				.last = 0x14,
+			},
+		},
+	},
+	[AB8500_AUDIO] = {
+		.num_ranges = 1,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x83,
+			},
+		},
+	},
+	[AB8500_INTERRUPT] = {
+		.num_ranges = 11,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x04,
+			},
+			{
+				.first = 0x06,
+				.last = 0x07,
+			},
+			{
+				.first = 0x09,
+				.last = 0x09,
+			},
+			{
+				.first = 0x0B,
+				.last = 0x0C,
+			},
+			{
+				.first = 0x12,
+				.last = 0x15,
+			},
+			{
+				.first = 0x18,
+				.last = 0x18,
+			},
+			/* Latch registers should not be read here */
+			{
+				.first = 0x40,
+				.last = 0x44,
+			},
+			{
+				.first = 0x46,
+				.last = 0x49,
+			},
+			{
+				.first = 0x4B,
+				.last = 0x4D,
+			},
+			{
+				.first = 0x52,
+				.last = 0x55,
+			},
+			{
+				.first = 0x58,
+				.last = 0x58,
+			},
+			/* LatchHier registers should not be read here */
+		},
+	},
+	[AB8500_RTC] = {
+		.num_ranges = 2,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x14,
+			},
+			{
+				.first = 0x16,
+				.last = 0x17,
+			},
+		},
+	},
+	[AB8500_MISC] = {
+		.num_ranges = 8,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x06,
+			},
+			{
+				.first = 0x10,
+				.last = 0x16,
+			},
+			{
+				.first = 0x20,
+				.last = 0x26,
+			},
+			{
+				.first = 0x30,
+				.last = 0x36,
+			},
+			{
+				.first = 0x40,
+				.last = 0x46,
+			},
+			{
+				.first = 0x50,
+				.last = 0x50,
+			},
+			{
+				.first = 0x60,
+				.last = 0x6B,
+			},
+			{
+				.first = 0x80,
+				.last = 0x82,
+			},
+		},
+	},
+	[AB8500_DEVELOPMENT] = {
+		.num_ranges = 2,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x00,
+			},
+			{
+				.first = 0x05,
+				.last = 0x05,
+			},
+		},
+	},
+	[AB8500_DEBUG] = {
+		.num_ranges = 1,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x05,
+				.last = 0x07,
+			},
+		},
+	},
+	[AB8500_PROD_TEST] = {
+		.num_ranges = 0,
+		.range = NULL,
+	},
+	[AB8500_STE_TEST] = {
+		.num_ranges = 0,
+		.range = NULL,
+	},
+	[AB8500_OTP_EMUL] = {
+		.num_ranges = 1,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x01,
+				.last = 0x15,
+			},
+		},
+	},
+};
+
+struct ab8500_prcmu_ranges ab8540_debug_ranges[AB8500_NUM_BANKS] = {
+	[AB8500_M_FSM_RANK] = {
+		.num_ranges = 1,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x0B,
+			},
+		},
+	},
+	[AB8500_SYS_CTRL1_BLOCK] = {
+		.num_ranges = 6,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x04,
+			},
+			{
+				.first = 0x42,
+				.last = 0x42,
+			},
+			{
+				.first = 0x50,
+				.last = 0x54,
+			},
+			{
+				.first = 0x57,
+				.last = 0x57,
+			},
+			{
+				.first = 0x80,
+				.last = 0x83,
+			},
+			{
+				.first = 0x90,
+				.last = 0x90,
+			},
+		},
+	},
+	[AB8500_SYS_CTRL2_BLOCK] = {
+		.num_ranges = 5,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x0D,
+			},
+			{
+				.first = 0x0F,
+				.last = 0x10,
+			},
+			{
+				.first = 0x20,
+				.last = 0x21,
+			},
+			{
+				.first = 0x32,
+				.last = 0x3C,
+			},
+			{
+				.first = 0x40,
+				.last = 0x42,
+			},
+		},
+	},
+	[AB8500_REGU_CTRL1] = {
+		.num_ranges = 4,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x03,
+				.last = 0x15,
+			},
+			{
+				.first = 0x20,
+				.last = 0x20,
+			},
+			{
+				.first = 0x80,
+				.last = 0x85,
+			},
+			{
+				.first = 0x87,
+				.last = 0x88,
+			},
+		},
+	},
+	[AB8500_REGU_CTRL2] = {
+		.num_ranges = 8,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x06,
+			},
+			{
+				.first = 0x08,
+				.last = 0x15,
+			},
+			{
+				.first = 0x17,
+				.last = 0x19,
+			},
+			{
+				.first = 0x1B,
+				.last = 0x1D,
+			},
+			{
+				.first = 0x1F,
+				.last = 0x2F,
+			},
+			{
+				.first = 0x31,
+				.last = 0x3A,
+			},
+			{
+				.first = 0x43,
+				.last = 0x44,
+			},
+			{
+				.first = 0x48,
+				.last = 0x49,
+			},
+		},
+	},
+	[AB8500_USB] = {
+		.num_ranges = 3,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x80,
+				.last = 0x83,
+			},
+			{
+				.first = 0x87,
+				.last = 0x8A,
+			},
+			{
+				.first = 0x91,
+				.last = 0x94,
+			},
+		},
+	},
+	[AB8500_TVOUT] = {
+		.num_ranges = 0,
+		.range = NULL
+	},
+	[AB8500_DBI] = {
+		.num_ranges = 4,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x07,
+			},
+			{
+				.first = 0x10,
+				.last = 0x11,
+			},
+			{
+				.first = 0x20,
+				.last = 0x21,
+			},
+			{
+				.first = 0x30,
+				.last = 0x43,
+			},
+		},
+	},
+	[AB8500_ECI_AV_ACC] = {
+		.num_ranges = 2,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x03,
+			},
+			{
+				.first = 0x80,
+				.last = 0x82,
+			},
+		},
+	},
+	[AB8500_RESERVED] = {
+		.num_ranges = 0,
+		.range = NULL,
+	},
+	[AB8500_GPADC] = {
+		.num_ranges = 4,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x01,
+			},
+			{
+				.first = 0x04,
+				.last = 0x06,
+			},
+			{
+				.first = 0x09,
+				.last = 0x0A,
+			},
+			{
+				.first = 0x10,
+				.last = 0x14,
+			},
+		},
+	},
+	[AB8500_CHARGER] = {
+		.num_ranges = 10,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x00,
+			},
+			{
+				.first = 0x02,
+				.last = 0x05,
+			},
+			{
+				.first = 0x40,
+				.last = 0x44,
+			},
+			{
+				.first = 0x50,
+				.last = 0x57,
+			},
+			{
+				.first = 0x60,
+				.last = 0x60,
+			},
+			{
+				.first = 0x70,
+				.last = 0x70,
+			},
+			{
+				.first = 0xA0,
+				.last = 0xA9,
+			},
+			{
+				.first = 0xAF,
+				.last = 0xB2,
+			},
+			{
+				.first = 0xC0,
+				.last = 0xC6,
+			},
+			{
+				.first = 0xF5,
+				.last = 0xF5,
+			},
+		},
+	},
+	[AB8500_GAS_GAUGE] = {
+		.num_ranges = 3,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x00,
+			},
+			{
+				.first = 0x07,
+				.last = 0x0A,
+			},
+			{
+				.first = 0x10,
+				.last = 0x14,
+			},
+		},
+	},
+	[AB8500_AUDIO] = {
+		.num_ranges = 1,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x9f,
+			},
+		},
+	},
+	[AB8500_INTERRUPT] = {
+		.num_ranges = 6,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x05,
+			},
+			{
+				.first = 0x0B,
+				.last = 0x0D,
+			},
+			{
+				.first = 0x12,
+				.last = 0x20,
+			},
+			/* Latch registers should not be read here */
+			{
+				.first = 0x40,
+				.last = 0x45,
+			},
+			{
+				.first = 0x4B,
+				.last = 0x4D,
+			},
+			{
+				.first = 0x52,
+				.last = 0x60,
+			},
+			/* LatchHier registers should not be read here */
+		},
+	},
+	[AB8500_RTC] = {
+		.num_ranges = 3,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x07,
+			},
+			{
+				.first = 0x0B,
+				.last = 0x18,
+			},
+			{
+				.first = 0x20,
+				.last = 0x25,
+			},
+		},
+	},
+	[AB8500_MISC] = {
+		.num_ranges = 9,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x06,
+			},
+			{
+				.first = 0x10,
+				.last = 0x16,
+			},
+			{
+				.first = 0x20,
+				.last = 0x26,
+			},
+			{
+				.first = 0x30,
+				.last = 0x36,
+			},
+			{
+				.first = 0x40,
+				.last = 0x49,
+			},
+			{
+				.first = 0x50,
+				.last = 0x50,
+			},
+			{
+				.first = 0x60,
+				.last = 0x6B,
+			},
+			{
+				.first = 0x70,
+				.last = 0x74,
+			},
+			{
+				.first = 0x80,
+				.last = 0x82,
+			},
+		},
+	},
+	[AB8500_DEVELOPMENT] = {
+		.num_ranges = 3,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x01,
+			},
+			{
+				.first = 0x06,
+				.last = 0x06,
+			},
+			{
+				.first = 0x10,
+				.last = 0x21,
+			},
+		},
+	},
+	[AB8500_DEBUG] = {
+		.num_ranges = 3,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x01,
+				.last = 0x0C,
+			},
+			{
+				.first = 0x0E,
+				.last = 0x11,
+			},
+			{
+				.first = 0x80,
+				.last = 0x81,
+			},
+		},
+	},
+	[AB8500_PROD_TEST] = {
+		.num_ranges = 0,
+		.range = NULL,
+	},
+	[AB8500_STE_TEST] = {
+		.num_ranges = 0,
+		.range = NULL,
+	},
+	[AB8500_OTP_EMUL] = {
+		.num_ranges = 1,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x3F,
+			},
+		},
+	},
+};
+
+
 static irqreturn_t ab8500_debug_handler(int irq, void *data)
 {
 	char buf[16];
@@ -520,19 +1306,16 @@ static int ab8500_registers_print(struct device *dev, u32 bank,
 			}
 
 			if (s) {
-				err = seq_printf(s, "  [%u/0x%02X]: 0x%02X\n",
+				err = seq_printf(s, "  [0x%02X/0x%02X]: 0x%02X\n",
 					bank, reg, value);
 				if (err < 0) {
-					dev_err(dev,
-					"seq_printf overflow bank=%d reg=%d\n",
-						bank, reg);
 					/* Error is not returned here since
 					 * the output is wanted in any case */
 					return 0;
 				}
 			} else {
-				printk(KERN_INFO" [%u/0x%02X]: 0x%02X\n", bank,
-					reg, value);
+				printk(KERN_INFO" [0x%02X/0x%02X]: 0x%02X\n",
+					bank, reg, value);
 			}
 		}
 	}
@@ -546,7 +1329,7 @@ static int ab8500_print_bank_registers(struct seq_file *s, void *p)
 
 	seq_printf(s, AB8500_NAME_STRING " register values:\n");
 
-	seq_printf(s, " bank %u:\n", bank);
+	seq_printf(s, " bank 0x%02X:\n", bank);
 
 	ab8500_registers_print(dev, bank, s);
 	return 0;
@@ -573,10 +1356,8 @@ static int ab8500_print_all_banks(struct seq_file *s, void *p)
 
 	seq_printf(s, AB8500_NAME_STRING " register values:\n");
 
-	for (i = 1; i < AB8500_NUM_BANKS; i++) {
-		err = seq_printf(s, " bank %u:\n", i);
-		if (err < 0)
-			dev_err(dev, "seq_printf overflow, bank=%d\n", i);
+	for (i = 0; i < AB8500_NUM_BANKS; i++) {
+		err = seq_printf(s, " bank 0x%02X:\n", i);
 
 		ab8500_registers_print(dev, i, s);
 	}
@@ -591,11 +1372,68 @@ void ab8500_dump_all_banks(struct device *dev)
 	printk(KERN_INFO"ab8500 register values:\n");
 
 	for (i = 1; i < AB8500_NUM_BANKS; i++) {
-		printk(KERN_INFO" bank %u:\n", i);
+		printk(KERN_INFO" bank 0x%02X:\n", i);
 		ab8500_registers_print(dev, i, NULL);
 	}
 }
 
+/* Space for 500 registers. */
+#define DUMP_MAX_REGS 700
+struct ab8500_register_dump
+{
+	u8 bank;
+	u8 reg;
+	u8 value;
+} ab8500_complete_register_dump[DUMP_MAX_REGS];
+
+extern int prcmu_abb_read(u8 slave, u8 reg, u8 *value, u8 size);
+
+/* This shall only be called upon kernel panic! */
+void ab8500_dump_all_banks_to_mem(void)
+{
+	int i, r = 0;
+	u8 bank;
+	int err = 0;
+
+	pr_info("Saving all ABB registers at \"ab8500_complete_register_dump\" "
+		"for crash analyze.\n");
+
+	for (bank = 0; bank < AB8500_NUM_BANKS; bank++) {
+		for (i = 0; i < debug_ranges[bank].num_ranges; i++) {
+			u8 reg;
+
+			for (reg = debug_ranges[bank].range[i].first;
+			     reg <= debug_ranges[bank].range[i].last;
+			     reg++) {
+				u8 value;
+
+				err = prcmu_abb_read(bank, reg, &value, 1);
+
+				if (err < 0)
+					goto out;
+
+				ab8500_complete_register_dump[r].bank = bank;
+				ab8500_complete_register_dump[r].reg = reg;
+				ab8500_complete_register_dump[r].value = value;
+
+				r++;
+
+				if (r >= DUMP_MAX_REGS) {
+					pr_err("%s: too many register to dump!\n",
+						__func__);
+					err = -EINVAL;
+					goto out;
+				}
+			}
+		}
+	}
+out:
+	if (err >= 0)
+		pr_info("Saved all ABB registers.\n");
+	else
+		pr_info("Failed to save all ABB registers.\n");
+}
+
 static int ab8500_all_banks_open(struct inode *inode, struct file *file)
 {
 	struct seq_file *s;
@@ -625,7 +1463,7 @@ static const struct file_operations ab8500_all_banks_fops = {
 
 static int ab8500_bank_print(struct seq_file *s, void *p)
 {
-	return seq_printf(s, "%d\n", debug_bank);
+	return seq_printf(s, "0x%02X\n", debug_bank);
 }
 
 static int ab8500_bank_open(struct inode *inode, struct file *file)
@@ -641,7 +1479,6 @@ static ssize_t ab8500_bank_write(struct file *file,
 	unsigned long user_bank;
 	int err;
 
-	/* Get userspace string and assure termination */
 	err = kstrtoul_from_user(user_buf, count, 0, &user_bank);
 	if (err)
 		return err;
@@ -667,14 +1504,13 @@ static int ab8500_address_open(struct inode *inode, struct file *file)
 }
 
 static ssize_t ab8500_address_write(struct file *file,
-	const char __user *user_buf,
-	size_t count, loff_t *ppos)
+				    const char __user *user_buf,
+				    size_t count, loff_t *ppos)
 {
 	struct device *dev = ((struct seq_file *)(file->private_data))->private;
 	unsigned long user_address;
 	int err;
 
-	/* Get userspace string and assure termination */
 	err = kstrtoul_from_user(user_buf, count, 0, &user_address);
 	if (err)
 		return err;
@@ -684,6 +1520,7 @@ static ssize_t ab8500_address_write(struct file *file,
 		return -EINVAL;
 	}
 	debug_address = user_address;
+
 	return count;
 }
 
@@ -711,14 +1548,13 @@ static int ab8500_val_open(struct inode *inode, struct file *file)
 }
 
 static ssize_t ab8500_val_write(struct file *file,
-	const char __user *user_buf,
-	size_t count, loff_t *ppos)
+				const char __user *user_buf,
+				size_t count, loff_t *ppos)
 {
 	struct device *dev = ((struct seq_file *)(file->private_data))->private;
 	unsigned long user_val;
 	int err;
 
-	/* Get userspace string and assure termination */
 	err = kstrtoul_from_user(user_buf, count, 0, &user_val);
 	if (err)
 		return err;
@@ -741,22 +1577,46 @@ static ssize_t ab8500_val_write(struct file *file,
  * Interrupt status
  */
 static u32 num_interrupts[AB8500_MAX_NR_IRQS];
+static u32 num_wake_interrupts[AB8500_MAX_NR_IRQS];
 static int num_interrupt_lines;
 
+bool __attribute__((weak)) suspend_test_wake_cause_interrupt_is_mine(u32 my_int)
+{
+	return false;
+}
+
 void ab8500_debug_register_interrupt(int line)
 {
-	if (line < num_interrupt_lines)
+	if (line < num_interrupt_lines) {
 		num_interrupts[line]++;
+		if (suspend_test_wake_cause_interrupt_is_mine(IRQ_DB8500_AB8500))
+			num_wake_interrupts[line]++;
+	}
 }
 
 static int ab8500_interrupts_print(struct seq_file *s, void *p)
 {
 	int line;
 
-	seq_printf(s, "irq:  number of\n");
+	seq_printf(s, "name: number:  number of: wake:\n");
 
-	for (line = 0; line < num_interrupt_lines; line++)
-		seq_printf(s, "%3i:  %6i\n", line, num_interrupts[line]);
+	for (line = 0; line < num_interrupt_lines; line++) {
+		struct irq_desc *desc = irq_to_desc(line + irq_first);
+		struct irqaction *action = desc->action;
+
+		seq_printf(s, "%3i:  %6i %4i", line,
+			   num_interrupts[line],
+			   num_wake_interrupts[line]);
+
+		if (desc && desc->name)
+			seq_printf(s, "-%-8s", desc->name);
+		if (action) {
+			seq_printf(s, "  %s", action->name);
+			while ((action = action->next) != NULL)
+				seq_printf(s, ", %s", action->name);
+		}
+		seq_putc(s, '\n');
+	}
 
 	return 0;
 }
@@ -801,6 +1661,79 @@ static int ab8500_hwreg_open(struct inode *inode, struct file *file)
 	return single_open(file, ab8500_hwreg_print, inode->i_private);
 }
 
+#define AB8500_SUPPLY_CONTROL_CONFIG_1 0x01
+#define AB8500_SUPPLY_CONTROL_REG 0x00
+#define AB8500_FIRST_SIM_REG 0x80
+#define AB8500_LAST_SIM_REG 0x8B
+#define AB8505_LAST_SIM_REG 0x8C
+
+static int ab8500_print_modem_registers(struct seq_file *s, void *p)
+{
+	struct device *dev = s->private;
+	struct ab8500 *ab8500;
+	int err;
+	u8 value;
+	u8 orig_value;
+	u32 bank = AB8500_REGU_CTRL2;
+	u32 last_sim_reg = AB8500_LAST_SIM_REG;
+	u32 reg;
+
+	ab8500 = dev_get_drvdata(dev->parent);
+	dev_warn(dev, "WARNING! This operation can interfer with modem side\n"
+		"and should only be done with care\n");
+
+	err = abx500_get_register_interruptible(dev,
+		AB8500_REGU_CTRL1, AB8500_SUPPLY_CONTROL_REG, &orig_value);
+	if (err < 0) {
+		dev_err(dev, "ab->read fail %d\n", err);
+		return err;
+	}
+	/* Config 1 will allow APE side to read SIM registers */
+	err = abx500_set_register_interruptible(dev,
+		AB8500_REGU_CTRL1, AB8500_SUPPLY_CONTROL_REG,
+		AB8500_SUPPLY_CONTROL_CONFIG_1);
+	if (err < 0) {
+		dev_err(dev, "ab->write fail %d\n", err);
+		return err;
+	}
+
+	seq_printf(s, " bank 0x%02X:\n", bank);
+
+	if (is_ab9540(ab8500) || is_ab8505(ab8500))
+		last_sim_reg = AB8505_LAST_SIM_REG;
+
+	for (reg = AB8500_FIRST_SIM_REG; reg <= last_sim_reg; reg++) {
+		err = abx500_get_register_interruptible(dev,
+			bank, reg, &value);
+		if (err < 0) {
+			dev_err(dev, "ab->read fail %d\n", err);
+			return err;
+		}
+		err = seq_printf(s, "  [0x%02X/0x%02X]: 0x%02X\n",
+			bank, reg, value);
+	}
+	err = abx500_set_register_interruptible(dev,
+		AB8500_REGU_CTRL1, AB8500_SUPPLY_CONTROL_REG, orig_value);
+	if (err < 0) {
+		dev_err(dev, "ab->write fail %d\n", err);
+		return err;
+	}
+	return 0;
+}
+
+static int ab8500_modem_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_print_modem_registers, inode->i_private);
+}
+
+static const struct file_operations ab8500_modem_fops = {
+	.open = ab8500_modem_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
 static int ab8500_gpadc_bat_ctrl_print(struct seq_file *s, void *p)
 {
 	int bat_ctrl_raw;
@@ -808,12 +1741,13 @@ static int ab8500_gpadc_bat_ctrl_print(struct seq_file *s, void *p)
 	struct ab8500_gpadc *gpadc;
 
 	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
-	bat_ctrl_raw = ab8500_gpadc_read_raw(gpadc, BAT_CTRL);
+	bat_ctrl_raw = ab8500_gpadc_read_raw(gpadc, BAT_CTRL,
+		avg_sample, trig_edge, trig_timer, conv_type);
 	bat_ctrl_convert = ab8500_gpadc_ad_to_voltage(gpadc,
-			BAT_CTRL, bat_ctrl_raw);
+		BAT_CTRL, bat_ctrl_raw);
 
 	return seq_printf(s, "%d,0x%X\n",
-			bat_ctrl_convert, bat_ctrl_raw);
+		bat_ctrl_convert, bat_ctrl_raw);
 }
 
 static int ab8500_gpadc_bat_ctrl_open(struct inode *inode, struct file *file)
@@ -836,16 +1770,17 @@ static int ab8500_gpadc_btemp_ball_print(struct seq_file *s, void *p)
 	struct ab8500_gpadc *gpadc;
 
 	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
-	btemp_ball_raw = ab8500_gpadc_read_raw(gpadc, BTEMP_BALL);
+	btemp_ball_raw = ab8500_gpadc_read_raw(gpadc, BTEMP_BALL,
+		avg_sample, trig_edge, trig_timer, conv_type);
 	btemp_ball_convert = ab8500_gpadc_ad_to_voltage(gpadc, BTEMP_BALL,
-			btemp_ball_raw);
+		btemp_ball_raw);
 
 	return seq_printf(s,
-			"%d,0x%X\n", btemp_ball_convert, btemp_ball_raw);
+		"%d,0x%X\n", btemp_ball_convert, btemp_ball_raw);
 }
 
 static int ab8500_gpadc_btemp_ball_open(struct inode *inode,
-		struct file *file)
+					struct file *file)
 {
 	return single_open(file, ab8500_gpadc_btemp_ball_print, inode->i_private);
 }
@@ -865,19 +1800,20 @@ static int ab8500_gpadc_main_charger_v_print(struct seq_file *s, void *p)
 	struct ab8500_gpadc *gpadc;
 
 	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
-	main_charger_v_raw = ab8500_gpadc_read_raw(gpadc, MAIN_CHARGER_V);
+	main_charger_v_raw = ab8500_gpadc_read_raw(gpadc, MAIN_CHARGER_V,
+		avg_sample, trig_edge, trig_timer, conv_type);
 	main_charger_v_convert = ab8500_gpadc_ad_to_voltage(gpadc,
-			MAIN_CHARGER_V, main_charger_v_raw);
+		MAIN_CHARGER_V, main_charger_v_raw);
 
 	return seq_printf(s, "%d,0x%X\n",
 			main_charger_v_convert, main_charger_v_raw);
 }
 
 static int ab8500_gpadc_main_charger_v_open(struct inode *inode,
-		struct file *file)
+					    struct file *file)
 {
 	return single_open(file, ab8500_gpadc_main_charger_v_print,
-			inode->i_private);
+		inode->i_private);
 }
 
 static const struct file_operations ab8500_gpadc_main_charger_v_fops = {
@@ -895,19 +1831,20 @@ static int ab8500_gpadc_acc_detect1_print(struct seq_file *s, void *p)
 	struct ab8500_gpadc *gpadc;
 
 	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
-	acc_detect1_raw = ab8500_gpadc_read_raw(gpadc, ACC_DETECT1);
+	acc_detect1_raw = ab8500_gpadc_read_raw(gpadc, ACC_DETECT1,
+		avg_sample, trig_edge, trig_timer, conv_type);
 	acc_detect1_convert = ab8500_gpadc_ad_to_voltage(gpadc, ACC_DETECT1,
-			acc_detect1_raw);
+		acc_detect1_raw);
 
 	return seq_printf(s, "%d,0x%X\n",
-			acc_detect1_convert, acc_detect1_raw);
+		acc_detect1_convert, acc_detect1_raw);
 }
 
 static int ab8500_gpadc_acc_detect1_open(struct inode *inode,
-		struct file *file)
+					 struct file *file)
 {
 	return single_open(file, ab8500_gpadc_acc_detect1_print,
-			inode->i_private);
+		inode->i_private);
 }
 
 static const struct file_operations ab8500_gpadc_acc_detect1_fops = {
@@ -925,19 +1862,20 @@ static int ab8500_gpadc_acc_detect2_print(struct seq_file *s, void *p)
 	struct ab8500_gpadc *gpadc;
 
 	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
-	acc_detect2_raw = ab8500_gpadc_read_raw(gpadc, ACC_DETECT2);
+	acc_detect2_raw = ab8500_gpadc_read_raw(gpadc, ACC_DETECT2,
+		avg_sample, trig_edge, trig_timer, conv_type);
 	acc_detect2_convert = ab8500_gpadc_ad_to_voltage(gpadc,
-	    ACC_DETECT2, acc_detect2_raw);
+		ACC_DETECT2, acc_detect2_raw);
 
 	return seq_printf(s, "%d,0x%X\n",
-			acc_detect2_convert, acc_detect2_raw);
+		acc_detect2_convert, acc_detect2_raw);
 }
 
 static int ab8500_gpadc_acc_detect2_open(struct inode *inode,
 		struct file *file)
 {
 	return single_open(file, ab8500_gpadc_acc_detect2_print,
-	    inode->i_private);
+		inode->i_private);
 }
 
 static const struct file_operations ab8500_gpadc_acc_detect2_fops = {
@@ -955,12 +1893,13 @@ static int ab8500_gpadc_aux1_print(struct seq_file *s, void *p)
 	struct ab8500_gpadc *gpadc;
 
 	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
-	aux1_raw = ab8500_gpadc_read_raw(gpadc, ADC_AUX1);
+	aux1_raw = ab8500_gpadc_read_raw(gpadc, ADC_AUX1,
+		avg_sample, trig_edge, trig_timer, conv_type);
 	aux1_convert = ab8500_gpadc_ad_to_voltage(gpadc, ADC_AUX1,
-			aux1_raw);
+		aux1_raw);
 
 	return seq_printf(s, "%d,0x%X\n",
-			aux1_convert, aux1_raw);
+		aux1_convert, aux1_raw);
 }
 
 static int ab8500_gpadc_aux1_open(struct inode *inode, struct file *file)
@@ -983,9 +1922,10 @@ static int ab8500_gpadc_aux2_print(struct seq_file *s, void *p)
 	struct ab8500_gpadc *gpadc;
 
 	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
-	aux2_raw = ab8500_gpadc_read_raw(gpadc, ADC_AUX2);
+	aux2_raw = ab8500_gpadc_read_raw(gpadc, ADC_AUX2,
+		avg_sample, trig_edge, trig_timer, conv_type);
 	aux2_convert = ab8500_gpadc_ad_to_voltage(gpadc, ADC_AUX2,
-			aux2_raw);
+		aux2_raw);
 
 	return seq_printf(s, "%d,0x%X\n",
 			aux2_convert, aux2_raw);
@@ -1011,16 +1951,17 @@ static int ab8500_gpadc_main_bat_v_print(struct seq_file *s, void *p)
 	struct ab8500_gpadc *gpadc;
 
 	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
-	main_bat_v_raw = ab8500_gpadc_read_raw(gpadc, MAIN_BAT_V);
+	main_bat_v_raw = ab8500_gpadc_read_raw(gpadc, MAIN_BAT_V,
+		avg_sample, trig_edge, trig_timer, conv_type);
 	main_bat_v_convert = ab8500_gpadc_ad_to_voltage(gpadc, MAIN_BAT_V,
-			main_bat_v_raw);
+		main_bat_v_raw);
 
 	return seq_printf(s, "%d,0x%X\n",
-			main_bat_v_convert, main_bat_v_raw);
+		main_bat_v_convert, main_bat_v_raw);
 }
 
 static int ab8500_gpadc_main_bat_v_open(struct inode *inode,
-		struct file *file)
+					struct file *file)
 {
 	return single_open(file, ab8500_gpadc_main_bat_v_print, inode->i_private);
 }
@@ -1040,12 +1981,13 @@ static int ab8500_gpadc_vbus_v_print(struct seq_file *s, void *p)
 	struct ab8500_gpadc *gpadc;
 
 	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
-	vbus_v_raw = ab8500_gpadc_read_raw(gpadc, VBUS_V);
+	vbus_v_raw =  ab8500_gpadc_read_raw(gpadc, VBUS_V,
+		avg_sample, trig_edge, trig_timer, conv_type);
 	vbus_v_convert = ab8500_gpadc_ad_to_voltage(gpadc, VBUS_V,
-			vbus_v_raw);
+		vbus_v_raw);
 
 	return seq_printf(s, "%d,0x%X\n",
-			vbus_v_convert, vbus_v_raw);
+		vbus_v_convert, vbus_v_raw);
 }
 
 static int ab8500_gpadc_vbus_v_open(struct inode *inode, struct file *file)
@@ -1068,19 +2010,20 @@ static int ab8500_gpadc_main_charger_c_print(struct seq_file *s, void *p)
 	struct ab8500_gpadc *gpadc;
 
 	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
-	main_charger_c_raw = ab8500_gpadc_read_raw(gpadc, MAIN_CHARGER_C);
+	main_charger_c_raw = ab8500_gpadc_read_raw(gpadc, MAIN_CHARGER_C,
+		avg_sample, trig_edge, trig_timer, conv_type);
 	main_charger_c_convert = ab8500_gpadc_ad_to_voltage(gpadc,
-			MAIN_CHARGER_C, main_charger_c_raw);
+		MAIN_CHARGER_C, main_charger_c_raw);
 
 	return seq_printf(s, "%d,0x%X\n",
-			main_charger_c_convert, main_charger_c_raw);
+		main_charger_c_convert, main_charger_c_raw);
 }
 
 static int ab8500_gpadc_main_charger_c_open(struct inode *inode,
 		struct file *file)
 {
 	return single_open(file, ab8500_gpadc_main_charger_c_print,
-			inode->i_private);
+		inode->i_private);
 }
 
 static const struct file_operations ab8500_gpadc_main_charger_c_fops = {
@@ -1098,19 +2041,20 @@ static int ab8500_gpadc_usb_charger_c_print(struct seq_file *s, void *p)
 	struct ab8500_gpadc *gpadc;
 
 	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
-	usb_charger_c_raw = ab8500_gpadc_read_raw(gpadc, USB_CHARGER_C);
+	usb_charger_c_raw = ab8500_gpadc_read_raw(gpadc, USB_CHARGER_C,
+		avg_sample, trig_edge, trig_timer, conv_type);
 	usb_charger_c_convert = ab8500_gpadc_ad_to_voltage(gpadc,
-	    USB_CHARGER_C, usb_charger_c_raw);
+		USB_CHARGER_C, usb_charger_c_raw);
 
 	return seq_printf(s, "%d,0x%X\n",
-			usb_charger_c_convert, usb_charger_c_raw);
+		usb_charger_c_convert, usb_charger_c_raw);
 }
 
 static int ab8500_gpadc_usb_charger_c_open(struct inode *inode,
 		struct file *file)
 {
 	return single_open(file, ab8500_gpadc_usb_charger_c_print,
-	    inode->i_private);
+		inode->i_private);
 }
 
 static const struct file_operations ab8500_gpadc_usb_charger_c_fops = {
@@ -1128,12 +2072,13 @@ static int ab8500_gpadc_bk_bat_v_print(struct seq_file *s, void *p)
 	struct ab8500_gpadc *gpadc;
 
 	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
-	bk_bat_v_raw = ab8500_gpadc_read_raw(gpadc, BK_BAT_V);
+	bk_bat_v_raw = ab8500_gpadc_read_raw(gpadc, BK_BAT_V,
+		avg_sample, trig_edge, trig_timer, conv_type);
 	bk_bat_v_convert = ab8500_gpadc_ad_to_voltage(gpadc,
-			BK_BAT_V, bk_bat_v_raw);
+		BK_BAT_V, bk_bat_v_raw);
 
 	return seq_printf(s, "%d,0x%X\n",
-			bk_bat_v_convert, bk_bat_v_raw);
+		bk_bat_v_convert, bk_bat_v_raw);
 }
 
 static int ab8500_gpadc_bk_bat_v_open(struct inode *inode, struct file *file)
@@ -1156,12 +2101,13 @@ static int ab8500_gpadc_die_temp_print(struct seq_file *s, void *p)
 	struct ab8500_gpadc *gpadc;
 
 	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
-	die_temp_raw = ab8500_gpadc_read_raw(gpadc, DIE_TEMP);
+	die_temp_raw = ab8500_gpadc_read_raw(gpadc, DIE_TEMP,
+		avg_sample, trig_edge, trig_timer, conv_type);
 	die_temp_convert = ab8500_gpadc_ad_to_voltage(gpadc, DIE_TEMP,
-			die_temp_raw);
+		die_temp_raw);
 
 	return seq_printf(s, "%d,0x%X\n",
-			die_temp_convert, die_temp_raw);
+		die_temp_convert, die_temp_raw);
 }
 
 static int ab8500_gpadc_die_temp_open(struct inode *inode, struct file *file)
@@ -1177,6 +2123,453 @@ static const struct file_operations ab8500_gpadc_die_temp_fops = {
 	.owner = THIS_MODULE,
 };
 
+static int ab8500_gpadc_usb_id_print(struct seq_file *s, void *p)
+{
+	int usb_id_raw;
+	int usb_id_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	usb_id_raw = ab8500_gpadc_read_raw(gpadc, USB_ID,
+		avg_sample, trig_edge, trig_timer, conv_type);
+	usb_id_convert = ab8500_gpadc_ad_to_voltage(gpadc, USB_ID,
+		usb_id_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+		usb_id_convert, usb_id_raw);
+}
+
+static int ab8500_gpadc_usb_id_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_usb_id_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_usb_id_fops = {
+	.open = ab8500_gpadc_usb_id_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8540_gpadc_xtal_temp_print(struct seq_file *s, void *p)
+{
+	int xtal_temp_raw;
+	int xtal_temp_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	xtal_temp_raw = ab8500_gpadc_read_raw(gpadc, XTAL_TEMP,
+		avg_sample, trig_edge, trig_timer, conv_type);
+	xtal_temp_convert = ab8500_gpadc_ad_to_voltage(gpadc, XTAL_TEMP,
+		xtal_temp_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+		xtal_temp_convert, xtal_temp_raw);
+}
+
+static int ab8540_gpadc_xtal_temp_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8540_gpadc_xtal_temp_print,
+		inode->i_private);
+}
+
+static const struct file_operations ab8540_gpadc_xtal_temp_fops = {
+	.open = ab8540_gpadc_xtal_temp_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8540_gpadc_vbat_true_meas_print(struct seq_file *s, void *p)
+{
+	int vbat_true_meas_raw;
+	int vbat_true_meas_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	vbat_true_meas_raw = ab8500_gpadc_read_raw(gpadc, VBAT_TRUE_MEAS,
+		avg_sample, trig_edge, trig_timer, conv_type);
+	vbat_true_meas_convert = ab8500_gpadc_ad_to_voltage(gpadc, VBAT_TRUE_MEAS,
+		vbat_true_meas_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+		vbat_true_meas_convert, vbat_true_meas_raw);
+}
+
+static int ab8540_gpadc_vbat_true_meas_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8540_gpadc_vbat_true_meas_print,
+		inode->i_private);
+}
+
+static const struct file_operations ab8540_gpadc_vbat_true_meas_fops = {
+	.open = ab8540_gpadc_vbat_true_meas_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8540_gpadc_bat_ctrl_and_ibat_print(struct seq_file *s, void *p)
+{
+	int bat_ctrl_raw;
+	int bat_ctrl_convert;
+	int ibat_raw;
+	int ibat_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	bat_ctrl_raw = ab8500_gpadc_double_read_raw(gpadc, BAT_CTRL_AND_IBAT,
+		avg_sample, trig_edge, trig_timer, conv_type, &ibat_raw);
+
+	bat_ctrl_convert = ab8500_gpadc_ad_to_voltage(gpadc, BAT_CTRL,
+		bat_ctrl_raw);
+	ibat_convert = ab8500_gpadc_ad_to_voltage(gpadc, IBAT_VIRTUAL_CHANNEL,
+		ibat_raw);
+
+	return seq_printf(s, "%d,0x%X\n"  "%d,0x%X\n",
+		bat_ctrl_convert, bat_ctrl_raw,
+		ibat_convert, ibat_raw);
+}
+
+static int ab8540_gpadc_bat_ctrl_and_ibat_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8540_gpadc_bat_ctrl_and_ibat_print,
+		inode->i_private);
+}
+
+static const struct file_operations ab8540_gpadc_bat_ctrl_and_ibat_fops = {
+	.open = ab8540_gpadc_bat_ctrl_and_ibat_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8540_gpadc_vbat_meas_and_ibat_print(struct seq_file *s, void *p)
+{
+	int vbat_meas_raw;
+	int vbat_meas_convert;
+	int ibat_raw;
+	int ibat_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	vbat_meas_raw = ab8500_gpadc_double_read_raw(gpadc, VBAT_MEAS_AND_IBAT,
+		avg_sample, trig_edge, trig_timer, conv_type, &ibat_raw);
+	vbat_meas_convert = ab8500_gpadc_ad_to_voltage(gpadc, MAIN_BAT_V,
+		vbat_meas_raw);
+	ibat_convert = ab8500_gpadc_ad_to_voltage(gpadc, IBAT_VIRTUAL_CHANNEL,
+		ibat_raw);
+
+	return seq_printf(s, "%d,0x%X\n"  "%d,0x%X\n",
+		vbat_meas_convert, vbat_meas_raw,
+		ibat_convert, ibat_raw);
+}
+
+static int ab8540_gpadc_vbat_meas_and_ibat_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8540_gpadc_vbat_meas_and_ibat_print,
+		inode->i_private);
+}
+
+static const struct file_operations ab8540_gpadc_vbat_meas_and_ibat_fops = {
+	.open = ab8540_gpadc_vbat_meas_and_ibat_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8540_gpadc_vbat_true_meas_and_ibat_print(struct seq_file *s, void *p)
+{
+	int vbat_true_meas_raw;
+	int vbat_true_meas_convert;
+	int ibat_raw;
+	int ibat_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	vbat_true_meas_raw = ab8500_gpadc_double_read_raw(gpadc,
+			VBAT_TRUE_MEAS_AND_IBAT, avg_sample, trig_edge,
+			trig_timer, conv_type, &ibat_raw);
+	vbat_true_meas_convert = ab8500_gpadc_ad_to_voltage(gpadc,
+			VBAT_TRUE_MEAS, vbat_true_meas_raw);
+	ibat_convert = ab8500_gpadc_ad_to_voltage(gpadc, IBAT_VIRTUAL_CHANNEL,
+		ibat_raw);
+
+	return seq_printf(s, "%d,0x%X\n"  "%d,0x%X\n",
+		vbat_true_meas_convert, vbat_true_meas_raw,
+		ibat_convert, ibat_raw);
+}
+
+static int ab8540_gpadc_vbat_true_meas_and_ibat_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8540_gpadc_vbat_true_meas_and_ibat_print,
+		inode->i_private);
+}
+
+static const struct file_operations ab8540_gpadc_vbat_true_meas_and_ibat_fops = {
+	.open = ab8540_gpadc_vbat_true_meas_and_ibat_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8540_gpadc_bat_temp_and_ibat_print(struct seq_file *s, void *p)
+{
+	int bat_temp_raw;
+	int bat_temp_convert;
+	int ibat_raw;
+	int ibat_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	bat_temp_raw = ab8500_gpadc_double_read_raw(gpadc, BAT_TEMP_AND_IBAT,
+		avg_sample, trig_edge, trig_timer, conv_type, &ibat_raw);
+	bat_temp_convert = ab8500_gpadc_ad_to_voltage(gpadc, BTEMP_BALL,
+		bat_temp_raw);
+	ibat_convert = ab8500_gpadc_ad_to_voltage(gpadc, IBAT_VIRTUAL_CHANNEL,
+		ibat_raw);
+
+	return seq_printf(s, "%d,0x%X\n"  "%d,0x%X\n",
+		bat_temp_convert, bat_temp_raw,
+		ibat_convert, ibat_raw);
+}
+
+static int ab8540_gpadc_bat_temp_and_ibat_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8540_gpadc_bat_temp_and_ibat_print,
+		inode->i_private);
+}
+
+static const struct file_operations ab8540_gpadc_bat_temp_and_ibat_fops = {
+	.open = ab8540_gpadc_bat_temp_and_ibat_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8540_gpadc_otp_cal_print(struct seq_file *s, void *p)
+{
+	struct ab8500_gpadc *gpadc;
+	u16 vmain_l, vmain_h, btemp_l, btemp_h;
+	u16 vbat_l, vbat_h, ibat_l, ibat_h;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	ab8540_gpadc_get_otp(gpadc, &vmain_l, &vmain_h, &btemp_l, &btemp_h,
+			&vbat_l, &vbat_h, &ibat_l, &ibat_h);
+	return seq_printf(s, "VMAIN_L:0x%X\n"
+		"VMAIN_H:0x%X\n"
+		"BTEMP_L:0x%X\n"
+		"BTEMP_H:0x%X\n"
+		"VBAT_L:0x%X\n"
+		"VBAT_H:0x%X\n"
+		"IBAT_L:0x%X\n"
+		"IBAT_H:0x%X\n",
+		vmain_l, vmain_h, btemp_l, btemp_h, vbat_l, vbat_h, ibat_l, ibat_h);
+}
+
+static int ab8540_gpadc_otp_cal_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8540_gpadc_otp_cal_print, inode->i_private);
+}
+
+static const struct file_operations ab8540_gpadc_otp_calib_fops = {
+	.open = ab8540_gpadc_otp_cal_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_avg_sample_print(struct seq_file *s, void *p)
+{
+	return seq_printf(s, "%d\n", avg_sample);
+}
+
+static int ab8500_gpadc_avg_sample_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_avg_sample_print,
+		inode->i_private);
+}
+
+static ssize_t ab8500_gpadc_avg_sample_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	struct device *dev = ((struct seq_file *)(file->private_data))->private;
+	unsigned long user_avg_sample;
+	int err;
+
+	err = kstrtoul_from_user(user_buf, count, 0, &user_avg_sample);
+	if (err)
+		return err;
+
+	if ((user_avg_sample == SAMPLE_1) || (user_avg_sample == SAMPLE_4)
+			|| (user_avg_sample == SAMPLE_8)
+			|| (user_avg_sample == SAMPLE_16)) {
+		avg_sample = (u8) user_avg_sample;
+	} else {
+		dev_err(dev, "debugfs error input: "
+			"should be egal to 1, 4, 8 or 16\n");
+		return -EINVAL;
+	}
+
+	return count;
+}
+
+static const struct file_operations ab8500_gpadc_avg_sample_fops = {
+	.open = ab8500_gpadc_avg_sample_open,
+	.read = seq_read,
+	.write = ab8500_gpadc_avg_sample_write,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_trig_edge_print(struct seq_file *s, void *p)
+{
+	return seq_printf(s, "%d\n", trig_edge);
+}
+
+static int ab8500_gpadc_trig_edge_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_trig_edge_print,
+		inode->i_private);
+}
+
+static ssize_t ab8500_gpadc_trig_edge_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	struct device *dev = ((struct seq_file *)(file->private_data))->private;
+	unsigned long user_trig_edge;
+	int err;
+
+	err = kstrtoul_from_user(user_buf, count, 0, &user_trig_edge);
+	if (err)
+		return err;
+
+	if ((user_trig_edge == RISING_EDGE)
+			|| (user_trig_edge == FALLING_EDGE)) {
+		trig_edge = (u8) user_trig_edge;
+	} else {
+		dev_err(dev, "Wrong input:\n"
+			"Enter 0. Rising edge\n"
+			"Enter 1. Falling edge\n");
+		return -EINVAL;
+	}
+
+	return count;
+}
+
+static const struct file_operations ab8500_gpadc_trig_edge_fops = {
+	.open = ab8500_gpadc_trig_edge_open,
+	.read = seq_read,
+	.write = ab8500_gpadc_trig_edge_write,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_trig_timer_print(struct seq_file *s, void *p)
+{
+	return seq_printf(s, "%d\n", trig_timer);
+}
+
+static int ab8500_gpadc_trig_timer_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_trig_timer_print,
+		inode->i_private);
+}
+
+static ssize_t ab8500_gpadc_trig_timer_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	struct device *dev = ((struct seq_file *)(file->private_data))->private;
+	unsigned long user_trig_timer;
+	int err;
+
+	err = kstrtoul_from_user(user_buf, count, 0, &user_trig_timer);
+	if (err)
+		return err;
+
+	if ((user_trig_timer >= 0) && (user_trig_timer <= 255)) {
+		trig_timer = (u8) user_trig_timer;
+	} else {
+		dev_err(dev, "debugfs error input: "
+			"should be beetween 0 to 255\n");
+		return -EINVAL;
+	}
+
+	return count;
+}
+
+static const struct file_operations ab8500_gpadc_trig_timer_fops = {
+	.open = ab8500_gpadc_trig_timer_open,
+	.read = seq_read,
+	.write = ab8500_gpadc_trig_timer_write,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_conv_type_print(struct seq_file *s, void *p)
+{
+	return seq_printf(s, "%d\n", conv_type);
+}
+
+static int ab8500_gpadc_conv_type_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_conv_type_print,
+		inode->i_private);
+}
+
+static ssize_t ab8500_gpadc_conv_type_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	struct device *dev = ((struct seq_file *)(file->private_data))->private;
+	unsigned long user_conv_type;
+	int err;
+
+	err = kstrtoul_from_user(user_buf, count, 0, &user_conv_type);
+	if (err)
+		return err;
+
+	if ((user_conv_type == ADC_SW)
+			|| (user_conv_type == ADC_HW)) {
+		conv_type = (u8) user_conv_type;
+	} else {
+		dev_err(dev, "Wrong input:\n"
+			"Enter 0. ADC SW conversion\n"
+			"Enter 1. ADC HW conversion\n");
+		return -EINVAL;
+	}
+
+	return count;
+}
+
+static const struct file_operations ab8500_gpadc_conv_type_fops = {
+	.open = ab8500_gpadc_conv_type_open,
+	.read = seq_read,
+	.write = ab8500_gpadc_conv_type_write,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
 /*
  * return length of an ASCII numerical value, 0 is string is not a
  * numerical value.
@@ -1352,7 +2745,7 @@ static int ab8500_subscribe_unsubscribe_open(struct inode *inode,
 					     struct file *file)
 {
 	return single_open(file, ab8500_subscribe_unsubscribe_print,
-			   inode->i_private);
+		inode->i_private);
 }
 
 /*
@@ -1382,21 +2775,14 @@ static ssize_t ab8500_subscribe_write(struct file *file,
 				      size_t count, loff_t *ppos)
 {
 	struct device *dev = ((struct seq_file *)(file->private_data))->private;
-	char buf[32];
-	int buf_size;
 	unsigned long user_val;
 	int err;
 	unsigned int irq_index;
 
-	/* Get userspace string and assure termination */
-	buf_size = min(count, (sizeof(buf)-1));
-	if (copy_from_user(buf, user_buf, buf_size))
-		return -EFAULT;
-	buf[buf_size] = 0;
-
-	err = strict_strtoul(buf, 0, &user_val);
+	err = kstrtoul_from_user(user_buf, count, 0, &user_val);
 	if (err)
-		return -EINVAL;
+		return err;
+
 	if (user_val < irq_first) {
 		dev_err(dev, "debugfs error input < %d\n", irq_first);
 		return -EINVAL;
@@ -1416,7 +2802,7 @@ static ssize_t ab8500_subscribe_write(struct file *file,
 	 */
 	dev_attr[irq_index] = kmalloc(sizeof(struct device_attribute),
 		GFP_KERNEL);
-	event_name[irq_index] = kmalloc(buf_size, GFP_KERNEL);
+	event_name[irq_index] = kmalloc(count, GFP_KERNEL);
 	sprintf(event_name[irq_index], "%lu", user_val);
 	dev_attr[irq_index]->show = show_irq;
 	dev_attr[irq_index]->store = NULL;
@@ -1438,7 +2824,7 @@ static ssize_t ab8500_subscribe_write(struct file *file,
 		return err;
 	}
 
-	return buf_size;
+	return count;
 }
 
 static ssize_t ab8500_unsubscribe_write(struct file *file,
@@ -1446,21 +2832,14 @@ static ssize_t ab8500_unsubscribe_write(struct file *file,
 					size_t count, loff_t *ppos)
 {
 	struct device *dev = ((struct seq_file *)(file->private_data))->private;
-	char buf[32];
-	int buf_size;
 	unsigned long user_val;
 	int err;
 	unsigned int irq_index;
 
-	/* Get userspace string and assure termination */
-	buf_size = min(count, (sizeof(buf)-1));
-	if (copy_from_user(buf, user_buf, buf_size))
-		return -EFAULT;
-	buf[buf_size] = 0;
-
-	err = strict_strtoul(buf, 0, &user_val);
+	err = kstrtoul_from_user(user_buf, count, 0, &user_val);
 	if (err)
-		return -EINVAL;
+		return err;
+
 	if (user_val < irq_first) {
 		dev_err(dev, "debugfs error input < %d\n", irq_first);
 		return -EINVAL;
@@ -1485,7 +2864,7 @@ static ssize_t ab8500_unsubscribe_write(struct file *file,
 	kfree(event_name[irq_index]);
 	kfree(dev_attr[irq_index]);
 
-	return buf_size;
+	return count;
 }
 
 /*
@@ -1583,7 +2962,7 @@ static int ab8500_debug_probe(struct platform_device *plf)
 	irq_first = platform_get_irq_byname(plf, "IRQ_FIRST");
 	if (irq_first < 0) {
 		dev_err(&plf->dev, "First irq not found, err %d\n",
-				irq_first);
+			irq_first);
 		ret = irq_first;
 		goto out_freeevent_name;
 	}
@@ -1591,9 +2970,9 @@ static int ab8500_debug_probe(struct platform_device *plf)
 	irq_last = platform_get_irq_byname(plf, "IRQ_LAST");
 	if (irq_last < 0) {
 		dev_err(&plf->dev, "Last irq not found, err %d\n",
-				irq_last);
+			irq_last);
 		ret = irq_last;
-                goto out_freeevent_name;
+		goto out_freeevent_name;
 	}
 
 	ab8500_dir = debugfs_create_dir(AB8500_NAME_STRING, NULL);
@@ -1601,124 +2980,198 @@ static int ab8500_debug_probe(struct platform_device *plf)
 		goto err;
 
 	ab8500_gpadc_dir = debugfs_create_dir(AB8500_ADC_NAME_STRING,
-	    ab8500_dir);
+		ab8500_dir);
 	if (!ab8500_gpadc_dir)
 		goto err;
 
 	file = debugfs_create_file("all-bank-registers", S_IRUGO,
-	    ab8500_dir, &plf->dev, &ab8500_registers_fops);
+		ab8500_dir, &plf->dev, &ab8500_registers_fops);
 	if (!file)
 		goto err;
 
 	file = debugfs_create_file("all-banks", S_IRUGO,
-	    ab8500_dir, &plf->dev, &ab8500_all_banks_fops);
+		ab8500_dir, &plf->dev, &ab8500_all_banks_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("register-bank", (S_IRUGO | S_IWUSR),
-	    ab8500_dir, &plf->dev, &ab8500_bank_fops);
+	file = debugfs_create_file("register-bank", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_dir, &plf->dev, &ab8500_bank_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("register-address", (S_IRUGO | S_IWUSR),
-	    ab8500_dir, &plf->dev, &ab8500_address_fops);
+	file = debugfs_create_file("register-address", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_dir, &plf->dev, &ab8500_address_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("register-value", (S_IRUGO | S_IWUSR),
-	    ab8500_dir, &plf->dev, &ab8500_val_fops);
+	file = debugfs_create_file("register-value", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_dir, &plf->dev, &ab8500_val_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("irq-subscribe", (S_IRUGO | S_IWUSR),
-	    ab8500_dir, &plf->dev, &ab8500_subscribe_fops);
+	file = debugfs_create_file("irq-subscribe", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_dir, &plf->dev, &ab8500_subscribe_fops);
 	if (!file)
 		goto err;
 
-	if (is_ab8500(ab8500))
+	if (is_ab8500(ab8500)) {
+		debug_ranges = ab8500_debug_ranges;
 		num_interrupt_lines = AB8500_NR_IRQS;
-	else if (is_ab8505(ab8500))
+	} else if (is_ab8505(ab8500)) {
+		debug_ranges = ab8505_debug_ranges;
 		num_interrupt_lines = AB8505_NR_IRQS;
-	else if (is_ab9540(ab8500))
+	} else if (is_ab9540(ab8500)) {
+		debug_ranges = ab8505_debug_ranges;
 		num_interrupt_lines = AB9540_NR_IRQS;
+	} else if (is_ab8540(ab8500)) {
+		debug_ranges = ab8540_debug_ranges;
+		num_interrupt_lines = AB8540_NR_IRQS;
+	}
 
 	file = debugfs_create_file("interrupts", (S_IRUGO),
-	    ab8500_dir, &plf->dev, &ab8500_interrupts_fops);
+		ab8500_dir, &plf->dev, &ab8500_interrupts_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("irq-unsubscribe", (S_IRUGO | S_IWUSR),
-	    ab8500_dir, &plf->dev, &ab8500_unsubscribe_fops);
+	file = debugfs_create_file("irq-unsubscribe", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_dir, &plf->dev, &ab8500_unsubscribe_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("hwreg", (S_IRUGO | S_IWUSR),
-	    ab8500_dir, &plf->dev, &ab8500_hwreg_fops);
+	file = debugfs_create_file("hwreg", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_dir, &plf->dev, &ab8500_hwreg_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("bat_ctrl", (S_IRUGO | S_IWUSR),
-	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_bat_ctrl_fops);
+	file = debugfs_create_file("all-modem-registers", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_dir, &plf->dev, &ab8500_modem_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("btemp_ball", (S_IRUGO | S_IWUSR),
-	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_btemp_ball_fops);
+	file = debugfs_create_file("bat_ctrl", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_bat_ctrl_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("main_charger_v", (S_IRUGO | S_IWUSR),
-	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_main_charger_v_fops);
+	file = debugfs_create_file("btemp_ball", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_btemp_ball_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("acc_detect1", (S_IRUGO | S_IWUSR),
-	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_acc_detect1_fops);
+	file = debugfs_create_file("main_charger_v", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_main_charger_v_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("acc_detect2", (S_IRUGO | S_IWUSR),
-	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_acc_detect2_fops);
+	file = debugfs_create_file("acc_detect1", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_acc_detect1_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("adc_aux1", (S_IRUGO | S_IWUSR),
-	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_aux1_fops);
+	file = debugfs_create_file("acc_detect2", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_acc_detect2_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("adc_aux2", (S_IRUGO | S_IWUSR),
-	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_aux2_fops);
+	file = debugfs_create_file("adc_aux1", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_aux1_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("main_bat_v", (S_IRUGO | S_IWUSR),
-	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_main_bat_v_fops);
+	file = debugfs_create_file("adc_aux2", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_aux2_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("vbus_v", (S_IRUGO | S_IWUSR),
-	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_vbus_v_fops);
+	file = debugfs_create_file("main_bat_v", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_main_bat_v_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("main_charger_c", (S_IRUGO | S_IWUSR),
-	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_main_charger_c_fops);
+	file = debugfs_create_file("vbus_v", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_vbus_v_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("main_charger_c", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_main_charger_c_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("usb_charger_c", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_usb_charger_c_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("bk_bat_v", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_bk_bat_v_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("die_temp", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_die_temp_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("usb_id", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_usb_id_fops);
+	if (!file)
+		goto err;
+
+	if (is_ab8540(ab8500)) {
+		file = debugfs_create_file("xtal_temp", (S_IRUGO | S_IWUSR | S_IWGRP),
+			ab8500_gpadc_dir, &plf->dev, &ab8540_gpadc_xtal_temp_fops);
+		if (!file)
+			goto err;
+		file = debugfs_create_file("vbattruemeas", (S_IRUGO | S_IWUSR | S_IWGRP),
+			ab8500_gpadc_dir, &plf->dev,
+			&ab8540_gpadc_vbat_true_meas_fops);
+		if (!file)
+			goto err;
+		file = debugfs_create_file("batctrl_and_ibat",
+			(S_IRUGO | S_IWUGO), ab8500_gpadc_dir,
+			&plf->dev, &ab8540_gpadc_bat_ctrl_and_ibat_fops);
+		if (!file)
+			goto err;
+		file = debugfs_create_file("vbatmeas_and_ibat",
+			(S_IRUGO | S_IWUGO), ab8500_gpadc_dir,
+			&plf->dev,
+			&ab8540_gpadc_vbat_meas_and_ibat_fops);
+		if (!file)
+			goto err;
+		file = debugfs_create_file("vbattruemeas_and_ibat",
+			(S_IRUGO | S_IWUGO), ab8500_gpadc_dir,
+			&plf->dev,
+			&ab8540_gpadc_vbat_true_meas_and_ibat_fops);
+		if (!file)
+			goto err;
+		file = debugfs_create_file("battemp_and_ibat",
+			(S_IRUGO | S_IWUGO), ab8500_gpadc_dir,
+			&plf->dev, &ab8540_gpadc_bat_temp_and_ibat_fops);
+		if (!file)
+			goto err;
+		file = debugfs_create_file("otp_calib", (S_IRUGO | S_IWUSR | S_IWGRP),
+			ab8500_gpadc_dir, &plf->dev, &ab8540_gpadc_otp_calib_fops);
+		if (!file)
+			goto err;
+	}
+	file = debugfs_create_file("avg_sample", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_avg_sample_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("usb_charger_c", (S_IRUGO | S_IWUSR),
-	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_usb_charger_c_fops);
+	file = debugfs_create_file("trig_edge", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_trig_edge_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("bk_bat_v", (S_IRUGO | S_IWUSR),
-	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_bk_bat_v_fops);
+	file = debugfs_create_file("trig_timer", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_trig_timer_fops);
 	if (!file)
 		goto err;
 
-	file = debugfs_create_file("die_temp", (S_IRUGO | S_IWUSR),
-	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_die_temp_fops);
+	file = debugfs_create_file("conv_type", (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_conv_type_fops);
 	if (!file)
 		goto err;
 
diff --git a/drivers/mfd/ab8500-gpadc.c b/drivers/mfd/ab8500-gpadc.c
index 5f341a50ee5a..5e65b28a5d09 100644
--- a/drivers/mfd/ab8500-gpadc.c
+++ b/drivers/mfd/ab8500-gpadc.c
@@ -37,6 +37,13 @@
 #define AB8500_GPADC_AUTODATAL_REG	0x07
 #define AB8500_GPADC_AUTODATAH_REG	0x08
 #define AB8500_GPADC_MUX_CTRL_REG	0x09
+#define AB8540_GPADC_MANDATA2L_REG	0x09
+#define AB8540_GPADC_MANDATA2H_REG	0x0A
+#define AB8540_GPADC_APEAAX_REG		0x10
+#define AB8540_GPADC_APEAAT_REG		0x11
+#define AB8540_GPADC_APEAAM_REG		0x12
+#define AB8540_GPADC_APEAAH_REG		0x13
+#define AB8540_GPADC_APEAAL_REG		0x14
 
 /*
  * OTP register offsets
@@ -49,19 +56,29 @@
 #define AB8500_GPADC_CAL_5		0x13
 #define AB8500_GPADC_CAL_6		0x14
 #define AB8500_GPADC_CAL_7		0x15
+/* New calibration for 8540 */
+#define AB8540_GPADC_OTP4_REG_7	0x38
+#define AB8540_GPADC_OTP4_REG_6	0x39
+#define AB8540_GPADC_OTP4_REG_5	0x3A
 
 /* gpadc constants */
 #define EN_VINTCORE12			0x04
 #define EN_VTVOUT			0x02
 #define EN_GPADC			0x01
 #define DIS_GPADC			0x00
-#define SW_AVG_16			0x60
+#define AVG_1				0x00
+#define AVG_4				0x20
+#define AVG_8				0x40
+#define AVG_16				0x60
 #define ADC_SW_CONV			0x04
 #define EN_ICHAR			0x80
 #define BTEMP_PULL_UP			0x08
 #define EN_BUF				0x40
 #define DIS_ZERO			0x00
 #define GPADC_BUSY			0x01
+#define EN_FALLING			0x10
+#define EN_TRIG_EDGE			0x02
+#define EN_VBIAS_XTAL_TEMP		0x02
 
 /* GPADC constants from AB8500 spec, UM0836 */
 #define ADC_RESOLUTION			1024
@@ -80,8 +97,21 @@
 #define ADC_CH_BKBAT_MIN		0
 #define ADC_CH_BKBAT_MAX		3200
 
+/* GPADC constants from AB8540 spec */
+#define ADC_CH_IBAT_MIN			(-6000) /* mA range measured by ADC for ibat*/
+#define ADC_CH_IBAT_MAX			6000
+#define ADC_CH_IBAT_MIN_V		(-60)	/* mV range measured by ADC for ibat*/
+#define ADC_CH_IBAT_MAX_V		60
+#define IBAT_VDROP_L			(-56)  /* mV */
+#define IBAT_VDROP_H			56
+
 /* This is used to not lose precision when dividing to get gain and offset */
-#define CALIB_SCALE			1000
+#define CALIB_SCALE		1000
+/*
+ * Number of bits shift used to not lose precision
+ * when dividing to get ibat gain.
+ */
+#define CALIB_SHIFT_IBAT	20
 
 /* Time in ms before disabling regulator */
 #define GPADC_AUDOSUSPEND_DELAY		1
@@ -92,6 +122,7 @@ enum cal_channels {
 	ADC_INPUT_VMAIN = 0,
 	ADC_INPUT_BTEMP,
 	ADC_INPUT_VBAT,
+	ADC_INPUT_IBAT,
 	NBR_CAL_INPUTS,
 };
 
@@ -102,8 +133,10 @@ enum cal_channels {
  * @offset:		Offset of the ADC channel
  */
 struct adc_cal_data {
-	u64 gain;
-	u64 offset;
+	s64 gain;
+	s64 offset;
+	u16 otp_calib_hi;
+	u16 otp_calib_lo;
 };
 
 /**
@@ -116,7 +149,10 @@ struct adc_cal_data {
  *				the completion of gpadc conversion
  * @ab8500_gpadc_lock:		structure of type mutex
  * @regu:			pointer to the struct regulator
- * @irq:			interrupt number that is used by gpadc
+ * @irq_sw:			interrupt number that is used by gpadc for Sw
+ *				conversion
+ * @irq_hw:			interrupt number that is used by gpadc for Hw
+ *				conversion
  * @cal_data			array of ADC calibration data structs
  */
 struct ab8500_gpadc {
@@ -126,7 +162,8 @@ struct ab8500_gpadc {
 	struct completion ab8500_gpadc_complete;
 	struct mutex ab8500_gpadc_lock;
 	struct regulator *regu;
-	int irq;
+	int irq_sw;
+	int irq_hw;
 	struct adc_cal_data cal_data[NBR_CAL_INPUTS];
 };
 
@@ -171,6 +208,7 @@ int ab8500_gpadc_ad_to_voltage(struct ab8500_gpadc *gpadc, u8 channel,
 			gpadc->cal_data[ADC_INPUT_VMAIN].offset) / CALIB_SCALE;
 		break;
 
+	case XTAL_TEMP:
 	case BAT_CTRL:
 	case BTEMP_BALL:
 	case ACC_DETECT1:
@@ -189,6 +227,7 @@ int ab8500_gpadc_ad_to_voltage(struct ab8500_gpadc *gpadc, u8 channel,
 		break;
 
 	case MAIN_BAT_V:
+	case VBAT_TRUE_MEAS:
 		/* For some reason we don't have calibrated data */
 		if (!gpadc->cal_data[ADC_INPUT_VBAT].gain) {
 			res = ADC_CH_VBAT_MIN + (ADC_CH_VBAT_MAX -
@@ -232,6 +271,20 @@ int ab8500_gpadc_ad_to_voltage(struct ab8500_gpadc *gpadc, u8 channel,
 			ADC_RESOLUTION;
 		break;
 
+	case IBAT_VIRTUAL_CHANNEL:
+		/* For some reason we don't have calibrated data */
+		if (!gpadc->cal_data[ADC_INPUT_IBAT].gain) {
+			res = ADC_CH_IBAT_MIN + (ADC_CH_IBAT_MAX -
+				ADC_CH_IBAT_MIN) * ad_value /
+				ADC_RESOLUTION;
+			break;
+		}
+		/* Here we can use the calibrated data */
+		res = (int) (ad_value * gpadc->cal_data[ADC_INPUT_IBAT].gain +
+				gpadc->cal_data[ADC_INPUT_IBAT].offset)
+				>> CALIB_SHIFT_IBAT;
+		break;
+
 	default:
 		dev_err(gpadc->dev,
 			"unknown channel, not possible to convert\n");
@@ -244,51 +297,86 @@ int ab8500_gpadc_ad_to_voltage(struct ab8500_gpadc *gpadc, u8 channel,
 EXPORT_SYMBOL(ab8500_gpadc_ad_to_voltage);
 
 /**
- * ab8500_gpadc_convert() - gpadc conversion
+ * ab8500_gpadc_sw_hw_convert() - gpadc conversion
  * @channel:	analog channel to be converted to digital data
+ * @avg_sample:  number of ADC sample to average
+ * @trig_egde:  selected ADC trig edge
+ * @trig_timer: selected ADC trigger delay timer
+ * @conv_type: selected conversion type (HW or SW conversion)
  *
  * This function converts the selected analog i/p to digital
  * data.
  */
-int ab8500_gpadc_convert(struct ab8500_gpadc *gpadc, u8 channel)
+int ab8500_gpadc_sw_hw_convert(struct ab8500_gpadc *gpadc, u8 channel,
+		u8 avg_sample, u8 trig_edge, u8 trig_timer, u8 conv_type)
 {
 	int ad_value;
 	int voltage;
 
-	ad_value = ab8500_gpadc_read_raw(gpadc, channel);
-	if (ad_value < 0) {
-		dev_err(gpadc->dev, "GPADC raw value failed ch: %d\n", channel);
+	ad_value = ab8500_gpadc_read_raw(gpadc, channel, avg_sample,
+			trig_edge, trig_timer, conv_type);
+/* On failure retry a second time */
+	if (ad_value < 0)
+		ad_value = ab8500_gpadc_read_raw(gpadc, channel, avg_sample,
+			trig_edge, trig_timer, conv_type);
+if (ad_value < 0) {
+		dev_err(gpadc->dev, "GPADC raw value failed ch: %d\n",
+				channel);
 		return ad_value;
 	}
 
 	voltage = ab8500_gpadc_ad_to_voltage(gpadc, channel, ad_value);
-
 	if (voltage < 0)
 		dev_err(gpadc->dev, "GPADC to voltage conversion failed ch:"
 			" %d AD: 0x%x\n", channel, ad_value);
 
 	return voltage;
 }
-EXPORT_SYMBOL(ab8500_gpadc_convert);
+EXPORT_SYMBOL(ab8500_gpadc_sw_hw_convert);
 
 /**
  * ab8500_gpadc_read_raw() - gpadc read
  * @channel:	analog channel to be read
+ * @avg_sample:  number of ADC sample to average
+ * @trig_edge:  selected trig edge
+ * @trig_timer: selected ADC trigger delay timer
+ * @conv_type: selected conversion type (HW or SW conversion)
  *
- * This function obtains the raw ADC value, this then needs
- * to be converted by calling ab8500_gpadc_ad_to_voltage()
+ * This function obtains the raw ADC value for an hardware conversion,
+ * this then needs to be converted by calling ab8500_gpadc_ad_to_voltage()
  */
-int ab8500_gpadc_read_raw(struct ab8500_gpadc *gpadc, u8 channel)
+int ab8500_gpadc_read_raw(struct ab8500_gpadc *gpadc, u8 channel,
+		u8 avg_sample, u8 trig_edge, u8 trig_timer, u8 conv_type)
+{
+	int raw_data;
+	raw_data = ab8500_gpadc_double_read_raw(gpadc, channel,
+			avg_sample, trig_edge, trig_timer, conv_type, NULL);
+	return raw_data;
+}
+
+int ab8500_gpadc_double_read_raw(struct ab8500_gpadc *gpadc, u8 channel,
+		u8 avg_sample, u8 trig_edge, u8 trig_timer, u8 conv_type,
+		int *ibat)
 {
 	int ret;
 	int looplimit = 0;
-	u8 val, low_data, high_data;
+	unsigned long completion_timeout;
+	u8 val, low_data, high_data, low_data2, high_data2;
+	u8 val_reg1 = 0;
+	unsigned int delay_min = 0;
+	unsigned int delay_max = 0;
+	u8 data_low_addr, data_high_addr;
 
 	if (!gpadc)
 		return -ENODEV;
 
-	mutex_lock(&gpadc->ab8500_gpadc_lock);
+	/* check if convertion is supported */
+	if ((gpadc->irq_sw < 0) && (conv_type == ADC_SW))
+		return -ENOTSUPP;
+	if ((gpadc->irq_hw < 0) && (conv_type == ADC_HW))
+		return -ENOTSUPP;
 
+	mutex_lock(&gpadc->ab8500_gpadc_lock);
 	/* Enable VTVout LDO this is required for GPADC */
 	pm_runtime_get_sync(gpadc->dev);
 
@@ -309,16 +397,34 @@ int ab8500_gpadc_read_raw(struct ab8500_gpadc *gpadc, u8 channel)
 	}
 
 	/* Enable GPADC */
-	ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
-		AB8500_GPADC, AB8500_GPADC_CTRL1_REG, EN_GPADC, EN_GPADC);
-	if (ret < 0) {
-		dev_err(gpadc->dev, "gpadc_conversion: enable gpadc failed\n");
-		goto out;
+	val_reg1 |= EN_GPADC;
+
+	/* Select the channel source and set average samples */
+	switch (avg_sample) {
+	case SAMPLE_1:
+		val = channel | AVG_1;
+		break;
+	case SAMPLE_4:
+		val = channel | AVG_4;
+		break;
+	case SAMPLE_8:
+		val = channel | AVG_8;
+		break;
+	default:
+		val = channel | AVG_16;
+		break;
 	}
 
-	/* Select the channel source and set average samples to 16 */
-	ret = abx500_set_register_interruptible(gpadc->dev, AB8500_GPADC,
-		AB8500_GPADC_CTRL2_REG, (channel | SW_AVG_16));
+	if (conv_type == ADC_HW) {
+		ret = abx500_set_register_interruptible(gpadc->dev,
+				AB8500_GPADC, AB8500_GPADC_CTRL3_REG, val);
+		val_reg1 |= EN_TRIG_EDGE;
+		if (trig_edge)
+			val_reg1 |= EN_FALLING;
+	}
+	else
+		ret = abx500_set_register_interruptible(gpadc->dev,
+				AB8500_GPADC, AB8500_GPADC_CTRL2_REG, val);
 	if (ret < 0) {
 		dev_err(gpadc->dev,
 			"gpadc_conversion: set avg samples failed\n");
@@ -333,71 +439,129 @@ int ab8500_gpadc_read_raw(struct ab8500_gpadc *gpadc, u8 channel)
 	switch (channel) {
 	case MAIN_CHARGER_C:
 	case USB_CHARGER_C:
-		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
-			AB8500_GPADC, AB8500_GPADC_CTRL1_REG,
-			EN_BUF | EN_ICHAR,
-			EN_BUF | EN_ICHAR);
+		val_reg1 |= EN_BUF | EN_ICHAR;
 		break;
 	case BTEMP_BALL:
 		if (!is_ab8500_2p0_or_earlier(gpadc->parent)) {
-			/* Turn on btemp pull-up on ABB 3.0 */
-			ret = abx500_mask_and_set_register_interruptible(
-				gpadc->dev,
-				AB8500_GPADC, AB8500_GPADC_CTRL1_REG,
-				EN_BUF | BTEMP_PULL_UP,
-				EN_BUF | BTEMP_PULL_UP);
-
-		 /*
-		  * Delay might be needed for ABB8500 cut 3.0, if not, remove
-		  * when hardware will be available
-		  */
-			usleep_range(1000, 1000);
+			val_reg1 |= EN_BUF | BTEMP_PULL_UP;
+			/*
+			* Delay might be needed for ABB8500 cut 3.0, if not,
+			* remove when hardware will be availible
+			*/
+			delay_min = 1000; /* Delay in micro seconds */
+			delay_max = 10000; /* large range to optimise sleep mode */
 			break;
 		}
 		/* Intentional fallthrough */
 	default:
-		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
-			AB8500_GPADC, AB8500_GPADC_CTRL1_REG, EN_BUF, EN_BUF);
+		val_reg1 |= EN_BUF;
 		break;
 	}
+
+	/* Write configuration to register */
+	ret = abx500_set_register_interruptible(gpadc->dev,
+		AB8500_GPADC, AB8500_GPADC_CTRL1_REG, val_reg1);
 	if (ret < 0) {
 		dev_err(gpadc->dev,
-			"gpadc_conversion: select falling edge failed\n");
+			"gpadc_conversion: set Control register failed\n");
 		goto out;
 	}
 
-	ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
-		AB8500_GPADC, AB8500_GPADC_CTRL1_REG, ADC_SW_CONV, ADC_SW_CONV);
-	if (ret < 0) {
-		dev_err(gpadc->dev,
-			"gpadc_conversion: start s/w conversion failed\n");
-		goto out;
+	if (delay_min != 0)
+		usleep_range(delay_min, delay_max);
+
+	if (conv_type == ADC_HW) {
+		/* Set trigger delay timer */
+		ret = abx500_set_register_interruptible(gpadc->dev,
+			AB8500_GPADC, AB8500_GPADC_AUTO_TIMER_REG, trig_timer);
+		if (ret < 0) {
+			dev_err(gpadc->dev,
+				"gpadc_conversion: trig timer failed\n");
+			goto out;
+		}
+		completion_timeout = 2 * HZ;
+		data_low_addr = AB8500_GPADC_AUTODATAL_REG;
+		data_high_addr = AB8500_GPADC_AUTODATAH_REG;
+	} else {
+		/* Start SW conversion */
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB8500_GPADC, AB8500_GPADC_CTRL1_REG,
+			ADC_SW_CONV, ADC_SW_CONV);
+		if (ret < 0) {
+			dev_err(gpadc->dev,
+				"gpadc_conversion: start s/w conv failed\n");
+			goto out;
+		}
+		completion_timeout = msecs_to_jiffies(CONVERSION_TIME);
+		data_low_addr = AB8500_GPADC_MANDATAL_REG;
+		data_high_addr = AB8500_GPADC_MANDATAH_REG;
 	}
+
 	/* wait for completion of conversion */
 	if (!wait_for_completion_timeout(&gpadc->ab8500_gpadc_complete,
-					 msecs_to_jiffies(CONVERSION_TIME))) {
+			completion_timeout)) {
 		dev_err(gpadc->dev,
-			"timeout: didn't receive GPADC conversion interrupt\n");
+			"timeout didn't receive GPADC conv interrupt\n");
 		ret = -EINVAL;
 		goto out;
 	}
 
 	/* Read the converted RAW data */
-	ret = abx500_get_register_interruptible(gpadc->dev, AB8500_GPADC,
-		AB8500_GPADC_MANDATAL_REG, &low_data);
+	ret = abx500_get_register_interruptible(gpadc->dev,
+			AB8500_GPADC, data_low_addr, &low_data);
 	if (ret < 0) {
 		dev_err(gpadc->dev, "gpadc_conversion: read low data failed\n");
 		goto out;
 	}
 
-	ret = abx500_get_register_interruptible(gpadc->dev, AB8500_GPADC,
-		AB8500_GPADC_MANDATAH_REG, &high_data);
+	ret = abx500_get_register_interruptible(gpadc->dev,
+		AB8500_GPADC, data_high_addr, &high_data);
 	if (ret < 0) {
-		dev_err(gpadc->dev,
-			"gpadc_conversion: read high data failed\n");
+		dev_err(gpadc->dev, "gpadc_conversion: read high data failed\n");
 		goto out;
 	}
 
+	/* Check if double convertion is required */
+	if ((channel == BAT_CTRL_AND_IBAT) ||
+			(channel == VBAT_MEAS_AND_IBAT) ||
+			(channel == VBAT_TRUE_MEAS_AND_IBAT) ||
+			(channel == BAT_TEMP_AND_IBAT)) {
+
+		if (conv_type == ADC_HW) {
+			/* not supported */
+			ret = -ENOTSUPP;
+			dev_err(gpadc->dev,
+				"gpadc_conversion: only SW double conversion supported\n");
+			goto out;
+		} else {
+			/* Read the converted RAW data 2 */
+			ret = abx500_get_register_interruptible(gpadc->dev,
+				AB8500_GPADC, AB8540_GPADC_MANDATA2L_REG,
+				&low_data2);
+			if (ret < 0) {
+				dev_err(gpadc->dev,
+					"gpadc_conversion: read sw low data 2 failed\n");
+				goto out;
+			}
+
+			ret = abx500_get_register_interruptible(gpadc->dev,
+				AB8500_GPADC, AB8540_GPADC_MANDATA2H_REG,
+				&high_data2);
+			if (ret < 0) {
+				dev_err(gpadc->dev,
+					"gpadc_conversion: read sw high data 2 failed\n");
+				goto out;
+			}
+			if (ibat != NULL) {
+				*ibat = (high_data2 << 8) | low_data2;
+			} else {
+				dev_warn(gpadc->dev,
+					"gpadc_conversion: ibat not stored\n");
+			}
+
+		}
+	}
+
 	/* Disable GPADC */
 	ret = abx500_set_register_interruptible(gpadc->dev, AB8500_GPADC,
 		AB8500_GPADC_CTRL1_REG, DIS_GPADC);
@@ -406,6 +570,7 @@ int ab8500_gpadc_read_raw(struct ab8500_gpadc *gpadc, u8 channel)
 		goto out;
 	}
 
+	/* Disable VTVout LDO this is required for GPADC */
 	pm_runtime_mark_last_busy(gpadc->dev);
 	pm_runtime_put_autosuspend(gpadc->dev);
 
@@ -422,9 +587,7 @@ out:
 	 */
 	(void) abx500_set_register_interruptible(gpadc->dev, AB8500_GPADC,
 		AB8500_GPADC_CTRL1_REG, DIS_GPADC);
-
 	pm_runtime_put(gpadc->dev);
-
 	mutex_unlock(&gpadc->ab8500_gpadc_lock);
 	dev_err(gpadc->dev,
 		"gpadc_conversion: Failed to AD convert channel %d\n", channel);
@@ -433,16 +596,16 @@ out:
 EXPORT_SYMBOL(ab8500_gpadc_read_raw);
 
 /**
- * ab8500_bm_gpswadcconvend_handler() - isr for s/w gpadc conversion completion
+ * ab8500_bm_gpadcconvend_handler() - isr for gpadc conversion completion
  * @irq:	irq number
  * @data:	pointer to the data passed during request irq
  *
- * This is a interrupt service routine for s/w gpadc conversion completion.
+ * This is a interrupt service routine for gpadc conversion completion.
  * Notifies the gpadc completion is completed and the converted raw value
  * can be read from the registers.
  * Returns IRQ status(IRQ_HANDLED)
  */
-static irqreturn_t ab8500_bm_gpswadcconvend_handler(int irq, void *_gpadc)
+static irqreturn_t ab8500_bm_gpadcconvend_handler(int irq, void *_gpadc)
 {
 	struct ab8500_gpadc *gpadc = _gpadc;
 
@@ -461,15 +624,27 @@ static int otp_cal_regs[] = {
 	AB8500_GPADC_CAL_7,
 };
 
+static int otp4_cal_regs[] = {
+	AB8540_GPADC_OTP4_REG_7,
+	AB8540_GPADC_OTP4_REG_6,
+	AB8540_GPADC_OTP4_REG_5,
+};
+
 static void ab8500_gpadc_read_calibration_data(struct ab8500_gpadc *gpadc)
 {
 	int i;
 	int ret[ARRAY_SIZE(otp_cal_regs)];
 	u8 gpadc_cal[ARRAY_SIZE(otp_cal_regs)];
-
+	int ret_otp4[ARRAY_SIZE(otp4_cal_regs)];
+	u8 gpadc_otp4[ARRAY_SIZE(otp4_cal_regs)];
 	int vmain_high, vmain_low;
 	int btemp_high, btemp_low;
 	int vbat_high, vbat_low;
+	int ibat_high, ibat_low;
+	s64 V_gain, V_offset, V2A_gain, V2A_offset;
+	struct ab8500 *ab8500;
+
+	ab8500 = gpadc->parent;
 
 	/* First we read all OTP registers and store the error code */
 	for (i = 0; i < ARRAY_SIZE(otp_cal_regs); i++) {
@@ -489,7 +664,7 @@ static void ab8500_gpadc_read_calibration_data(struct ab8500_gpadc *gpadc)
 	 * bt_h/l = btemp_high/low
 	 * vb_h/l = vbat_high/low
 	 *
-	 * Data bits:
+	 * Data bits 8500/9540:
 	 * | 7	   | 6	   | 5	   | 4	   | 3	   | 2	   | 1	   | 0
 	 * |.......|.......|.......|.......|.......|.......|.......|.......
 	 * |						   | vm_h9 | vm_h8
@@ -507,6 +682,35 @@ static void ab8500_gpadc_read_calibration_data(struct ab8500_gpadc *gpadc)
 	 * | vb_l5 | vb_l4 | vb_l3 | vb_l2 | vb_l1 | vb_l0 |
 	 * |.......|.......|.......|.......|.......|.......|.......|.......
 	 *
+	 * Data bits 8540:
+	 * OTP2
+	 * | 7	   | 6	   | 5	   | 4	   | 3	   | 2	   | 1	   | 0
+	 * |.......|.......|.......|.......|.......|.......|.......|.......
+	 * |
+	 * |.......|.......|.......|.......|.......|.......|.......|.......
+	 * | vm_h9 | vm_h8 | vm_h7 | vm_h6 | vm_h5 | vm_h4 | vm_h3 | vm_h2
+	 * |.......|.......|.......|.......|.......|.......|.......|.......
+	 * | vm_h1 | vm_h0 | vm_l4 | vm_l3 | vm_l2 | vm_l1 | vm_l0 | bt_h9
+	 * |.......|.......|.......|.......|.......|.......|.......|.......
+	 * | bt_h8 | bt_h7 | bt_h6 | bt_h5 | bt_h4 | bt_h3 | bt_h2 | bt_h1
+	 * |.......|.......|.......|.......|.......|.......|.......|.......
+	 * | bt_h0 | bt_l4 | bt_l3 | bt_l2 | bt_l1 | bt_l0 | vb_h9 | vb_h8
+	 * |.......|.......|.......|.......|.......|.......|.......|.......
+	 * | vb_h7 | vb_h6 | vb_h5 | vb_h4 | vb_h3 | vb_h2 | vb_h1 | vb_h0
+	 * |.......|.......|.......|.......|.......|.......|.......|.......
+	 * | vb_l5 | vb_l4 | vb_l3 | vb_l2 | vb_l1 | vb_l0 |
+	 * |.......|.......|.......|.......|.......|.......|.......|.......
+	 *
+	 * Data bits 8540:
+	 * OTP4
+	 * | 7	   | 6	   | 5	   | 4	   | 3	   | 2	   | 1	   | 0
+	 * |.......|.......|.......|.......|.......|.......|.......|.......
+	 * |					   | ib_h9 | ib_h8 | ib_h7
+	 * |.......|.......|.......|.......|.......|.......|.......|.......
+	 * | ib_h6 | ib_h5 | ib_h4 | ib_h3 | ib_h2 | ib_h1 | ib_h0 | ib_l5
+	 * |.......|.......|.......|.......|.......|.......|.......|.......
+	 * | ib_l4 | ib_l3 | ib_l2 | ib_l1 | ib_l0 |
+	 *
 	 *
 	 * Ideal output ADC codes corresponding to injected input voltages
 	 * during manufacturing is:
@@ -519,38 +723,116 @@ static void ab8500_gpadc_read_calibration_data(struct ab8500_gpadc *gpadc)
 	 * vbat_low:   Vin = 2380mV  / ADC ideal code = 33
 	 */
 
-	/* Calculate gain and offset for VMAIN if all reads succeeded */
-	if (!(ret[0] < 0 || ret[1] < 0 || ret[2] < 0)) {
-		vmain_high = (((gpadc_cal[0] & 0x03) << 8) |
-			((gpadc_cal[1] & 0x3F) << 2) |
-			((gpadc_cal[2] & 0xC0) >> 6));
+	if (is_ab8540(ab8500)) {
+		/* Calculate gain and offset for VMAIN if all reads succeeded*/
+		if (!(ret[1] < 0 || ret[2] < 0)) {
+			vmain_high = (((gpadc_cal[1] & 0xFF) << 2) |
+				((gpadc_cal[2] & 0xC0) >> 6));
+			vmain_low = ((gpadc_cal[2] & 0x3E) >> 1);
+
+			gpadc->cal_data[ADC_INPUT_VMAIN].otp_calib_hi =
+				(u16)vmain_high;
+			gpadc->cal_data[ADC_INPUT_VMAIN].otp_calib_lo =
+				(u16)vmain_low;
+
+			gpadc->cal_data[ADC_INPUT_VMAIN].gain = CALIB_SCALE *
+				(19500 - 315) / (vmain_high - vmain_low);
+			gpadc->cal_data[ADC_INPUT_VMAIN].offset = CALIB_SCALE *
+				19500 - (CALIB_SCALE * (19500 - 315) /
+				(vmain_high - vmain_low)) * vmain_high;
+		} else {
+		gpadc->cal_data[ADC_INPUT_VMAIN].gain = 0;
+		}
 
-		vmain_low = ((gpadc_cal[2] & 0x3E) >> 1);
+		/* Read IBAT calibration Data */
+		for (i = 0; i < ARRAY_SIZE(otp4_cal_regs); i++) {
+			ret_otp4[i] = abx500_get_register_interruptible(
+					gpadc->dev, AB8500_OTP_EMUL,
+					otp4_cal_regs[i],  &gpadc_otp4[i]);
+			if (ret_otp4[i] < 0)
+				dev_err(gpadc->dev,
+					"%s: read otp4 reg 0x%02x failed\n",
+					__func__, otp4_cal_regs[i]);
+		}
 
-		gpadc->cal_data[ADC_INPUT_VMAIN].gain = CALIB_SCALE *
-			(19500 - 315) /	(vmain_high - vmain_low);
+		/* Calculate gain and offset for IBAT if all reads succeeded */
+		if (!(ret_otp4[0] < 0 || ret_otp4[1] < 0 || ret_otp4[2] < 0)) {
+			ibat_high = (((gpadc_otp4[0] & 0x07) << 7) |
+				((gpadc_otp4[1] & 0xFE) >> 1));
+			ibat_low = (((gpadc_otp4[1] & 0x01) << 5) |
+				((gpadc_otp4[2] & 0xF8) >> 3));
+
+			gpadc->cal_data[ADC_INPUT_IBAT].otp_calib_hi =
+				(u16)ibat_high;
+			gpadc->cal_data[ADC_INPUT_IBAT].otp_calib_lo =
+				(u16)ibat_low;
+
+			V_gain = ((IBAT_VDROP_H - IBAT_VDROP_L)
+				<< CALIB_SHIFT_IBAT) / (ibat_high - ibat_low);
+
+			V_offset = (IBAT_VDROP_H << CALIB_SHIFT_IBAT) -
+				(((IBAT_VDROP_H - IBAT_VDROP_L) <<
+				CALIB_SHIFT_IBAT) / (ibat_high - ibat_low))
+				* ibat_high;
+			/*
+			 * Result obtained is in mV (at a scale factor),
+			 * we need to calculate gain and offset to get mA
+			 */
+			V2A_gain = (ADC_CH_IBAT_MAX - ADC_CH_IBAT_MIN)/
+				(ADC_CH_IBAT_MAX_V - ADC_CH_IBAT_MIN_V);
+			V2A_offset = ((ADC_CH_IBAT_MAX_V * ADC_CH_IBAT_MIN -
+				ADC_CH_IBAT_MAX * ADC_CH_IBAT_MIN_V)
+				<< CALIB_SHIFT_IBAT)
+				/ (ADC_CH_IBAT_MAX_V - ADC_CH_IBAT_MIN_V);
+
+			gpadc->cal_data[ADC_INPUT_IBAT].gain = V_gain * V2A_gain;
+			gpadc->cal_data[ADC_INPUT_IBAT].offset = V_offset *
+				V2A_gain + V2A_offset;
+		} else {
+			gpadc->cal_data[ADC_INPUT_IBAT].gain = 0;
+		}
 
-		gpadc->cal_data[ADC_INPUT_VMAIN].offset = CALIB_SCALE * 19500 -
-			(CALIB_SCALE * (19500 - 315) /
-			 (vmain_high - vmain_low)) * vmain_high;
+		dev_dbg(gpadc->dev, "IBAT gain %llu offset %llu\n",
+			gpadc->cal_data[ADC_INPUT_IBAT].gain,
+			gpadc->cal_data[ADC_INPUT_IBAT].offset);
 	} else {
-		gpadc->cal_data[ADC_INPUT_VMAIN].gain = 0;
+		/* Calculate gain and offset for VMAIN if all reads succeeded */
+		if (!(ret[0] < 0 || ret[1] < 0 || ret[2] < 0)) {
+			vmain_high = (((gpadc_cal[0] & 0x03) << 8) |
+				((gpadc_cal[1] & 0x3F) << 2) |
+				((gpadc_cal[2] & 0xC0) >> 6));
+			vmain_low = ((gpadc_cal[2] & 0x3E) >> 1);
+
+			gpadc->cal_data[ADC_INPUT_VMAIN].otp_calib_hi =
+				(u16)vmain_high;
+			gpadc->cal_data[ADC_INPUT_VMAIN].otp_calib_lo =
+				(u16)vmain_low;
+
+			gpadc->cal_data[ADC_INPUT_VMAIN].gain = CALIB_SCALE *
+				(19500 - 315) / (vmain_high - vmain_low);
+
+			gpadc->cal_data[ADC_INPUT_VMAIN].offset = CALIB_SCALE *
+				19500 - (CALIB_SCALE * (19500 - 315) /
+				(vmain_high - vmain_low)) * vmain_high;
+		} else {
+			gpadc->cal_data[ADC_INPUT_VMAIN].gain = 0;
+		}
 	}
 
 	/* Calculate gain and offset for BTEMP if all reads succeeded */
 	if (!(ret[2] < 0 || ret[3] < 0 || ret[4] < 0)) {
 		btemp_high = (((gpadc_cal[2] & 0x01) << 9) |
-			(gpadc_cal[3] << 1) |
-			((gpadc_cal[4] & 0x80) >> 7));
-
+			(gpadc_cal[3] << 1) | ((gpadc_cal[4] & 0x80) >> 7));
 		btemp_low = ((gpadc_cal[4] & 0x7C) >> 2);
 
+		gpadc->cal_data[ADC_INPUT_BTEMP].otp_calib_hi = (u16)btemp_high;
+		gpadc->cal_data[ADC_INPUT_BTEMP].otp_calib_lo = (u16)btemp_low;
+
 		gpadc->cal_data[ADC_INPUT_BTEMP].gain =
 			CALIB_SCALE * (1300 - 21) / (btemp_high - btemp_low);
-
 		gpadc->cal_data[ADC_INPUT_BTEMP].offset = CALIB_SCALE * 1300 -
-			(CALIB_SCALE * (1300 - 21) /
-			(btemp_high - btemp_low)) * btemp_high;
+			(CALIB_SCALE * (1300 - 21) / (btemp_high - btemp_low))
+			* btemp_high;
 	} else {
 		gpadc->cal_data[ADC_INPUT_BTEMP].gain = 0;
 	}
@@ -560,9 +842,11 @@ static void ab8500_gpadc_read_calibration_data(struct ab8500_gpadc *gpadc)
 		vbat_high = (((gpadc_cal[4] & 0x03) << 8) | gpadc_cal[5]);
 		vbat_low = ((gpadc_cal[6] & 0xFC) >> 2);
 
+		gpadc->cal_data[ADC_INPUT_VBAT].otp_calib_hi = (u16)vbat_high;
+		gpadc->cal_data[ADC_INPUT_VBAT].otp_calib_lo = (u16)vbat_low;
+
 		gpadc->cal_data[ADC_INPUT_VBAT].gain = CALIB_SCALE *
 			(4700 - 2380) /	(vbat_high - vbat_low);
-
 		gpadc->cal_data[ADC_INPUT_VBAT].offset = CALIB_SCALE * 4700 -
 			(CALIB_SCALE * (4700 - 2380) /
 			(vbat_high - vbat_low)) * vbat_high;
@@ -608,6 +892,31 @@ static int ab8500_gpadc_runtime_idle(struct device *dev)
 	return 0;
 }
 
+static int ab8500_gpadc_suspend(struct device *dev)
+{
+	struct ab8500_gpadc *gpadc = dev_get_drvdata(dev);
+
+	mutex_lock(&gpadc->ab8500_gpadc_lock);
+
+	pm_runtime_get_sync(dev);
+
+	regulator_disable(gpadc->regu);
+	return 0;
+}
+
+static int ab8500_gpadc_resume(struct device *dev)
+{
+	struct ab8500_gpadc *gpadc = dev_get_drvdata(dev);
+
+	regulator_enable(gpadc->regu);
+
+	pm_runtime_mark_last_busy(gpadc->dev);
+	pm_runtime_put_autosuspend(gpadc->dev);
+
+	mutex_unlock(&gpadc->ab8500_gpadc_lock);
+	return 0;
+}
+
 static int ab8500_gpadc_probe(struct platform_device *pdev)
 {
 	int ret = 0;
@@ -619,13 +928,13 @@ static int ab8500_gpadc_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	gpadc->irq = platform_get_irq_byname(pdev, "SW_CONV_END");
-	if (gpadc->irq < 0) {
-		dev_err(&pdev->dev, "failed to get platform irq-%d\n",
-			gpadc->irq);
-		ret = gpadc->irq;
-		goto fail;
-	}
+	gpadc->irq_sw = platform_get_irq_byname(pdev, "SW_CONV_END");
+	if (gpadc->irq_sw < 0)
+		dev_err(gpadc->dev, "failed to get platform sw_conv_end irq\n");
+
+	gpadc->irq_hw = platform_get_irq_byname(pdev, "HW_CONV_END");
+	if (gpadc->irq_hw < 0)
+		dev_err(gpadc->dev, "failed to get platform hw_conv_end irq\n");
 
 	gpadc->dev = &pdev->dev;
 	gpadc->parent = dev_get_drvdata(pdev->dev.parent);
@@ -634,15 +943,31 @@ static int ab8500_gpadc_probe(struct platform_device *pdev)
 	/* Initialize completion used to notify completion of conversion */
 	init_completion(&gpadc->ab8500_gpadc_complete);
 
-	/* Register interrupt  - SwAdcComplete */
-	ret = request_threaded_irq(gpadc->irq, NULL,
-		ab8500_bm_gpswadcconvend_handler,
-		IRQF_ONESHOT | IRQF_NO_SUSPEND | IRQF_SHARED,
-				"ab8500-gpadc", gpadc);
-	if (ret < 0) {
-		dev_err(gpadc->dev, "Failed to register interrupt, irq: %d\n",
-			gpadc->irq);
-		goto fail;
+	/* Register interrupts */
+	if (gpadc->irq_sw >= 0) {
+		ret = request_threaded_irq(gpadc->irq_sw, NULL,
+			ab8500_bm_gpadcconvend_handler,
+			IRQF_NO_SUSPEND | IRQF_SHARED, "ab8500-gpadc-sw",
+			gpadc);
+		if (ret < 0) {
+			dev_err(gpadc->dev,
+				"Failed to register interrupt irq: %d\n",
+				gpadc->irq_sw);
+			goto fail;
+		}
+	}
+
+	if (gpadc->irq_hw >= 0) {
+		ret = request_threaded_irq(gpadc->irq_hw, NULL,
+			ab8500_bm_gpadcconvend_handler,
+			IRQF_NO_SUSPEND | IRQF_SHARED, "ab8500-gpadc-hw",
+			gpadc);
+		if (ret < 0) {
+			dev_err(gpadc->dev,
+				"Failed to register interrupt irq: %d\n",
+				gpadc->irq_hw);
+			goto fail_irq;
+		}
 	}
 
 	/* VTVout LDO used to power up ab8500-GPADC */
@@ -669,11 +994,13 @@ static int ab8500_gpadc_probe(struct platform_device *pdev)
 	ab8500_gpadc_read_calibration_data(gpadc);
 	list_add_tail(&gpadc->node, &ab8500_gpadc_list);
 	dev_dbg(gpadc->dev, "probe success\n");
+
 	return 0;
 
 fail_enable:
 fail_irq:
-	free_irq(gpadc->irq, gpadc);
+	free_irq(gpadc->irq_sw, gpadc);
+	free_irq(gpadc->irq_hw, gpadc);
 fail:
 	kfree(gpadc);
 	gpadc = NULL;
@@ -687,7 +1014,10 @@ static int ab8500_gpadc_remove(struct platform_device *pdev)
 	/* remove this gpadc entry from the list */
 	list_del(&gpadc->node);
 	/* remove interrupt  - completion of Sw ADC conversion */
-	free_irq(gpadc->irq, gpadc);
+	if (gpadc->irq_sw >= 0)
+		free_irq(gpadc->irq_sw, gpadc);
+	if (gpadc->irq_hw >= 0)
+		free_irq(gpadc->irq_hw, gpadc);
 
 	pm_runtime_get_sync(gpadc->dev);
 	pm_runtime_disable(gpadc->dev);
@@ -707,6 +1037,9 @@ static const struct dev_pm_ops ab8500_gpadc_pm_ops = {
 	SET_RUNTIME_PM_OPS(ab8500_gpadc_runtime_suspend,
 			   ab8500_gpadc_runtime_resume,
 			   ab8500_gpadc_runtime_idle)
+	SET_SYSTEM_SLEEP_PM_OPS(ab8500_gpadc_suspend,
+				ab8500_gpadc_resume)
+
 };
 
 static struct platform_driver ab8500_gpadc_driver = {
@@ -729,10 +1062,30 @@ static void __exit ab8500_gpadc_exit(void)
 	platform_driver_unregister(&ab8500_gpadc_driver);
 }
 
+/**
+ * ab8540_gpadc_get_otp() - returns OTP values
+ *
+ */
+void ab8540_gpadc_get_otp(struct ab8500_gpadc *gpadc,
+			u16 *vmain_l, u16 *vmain_h, u16 *btemp_l, u16 *btemp_h,
+			u16 *vbat_l, u16 *vbat_h, u16 *ibat_l, u16 *ibat_h)
+{
+	*vmain_l = gpadc->cal_data[ADC_INPUT_VMAIN].otp_calib_lo;
+	*vmain_h = gpadc->cal_data[ADC_INPUT_VMAIN].otp_calib_hi;
+	*btemp_l = gpadc->cal_data[ADC_INPUT_BTEMP].otp_calib_lo;
+	*btemp_h = gpadc->cal_data[ADC_INPUT_BTEMP].otp_calib_hi;
+	*vbat_l = gpadc->cal_data[ADC_INPUT_VBAT].otp_calib_lo;
+	*vbat_h = gpadc->cal_data[ADC_INPUT_VBAT].otp_calib_hi;
+	*ibat_l = gpadc->cal_data[ADC_INPUT_IBAT].otp_calib_lo;
+	*ibat_h = gpadc->cal_data[ADC_INPUT_IBAT].otp_calib_hi;
+	return ;
+}
+
 subsys_initcall_sync(ab8500_gpadc_init);
 module_exit(ab8500_gpadc_exit);
 
 MODULE_LICENSE("GPL v2");
-MODULE_AUTHOR("Arun R Murthy, Daniel Willerud, Johan Palsson");
+MODULE_AUTHOR("Arun R Murthy, Daniel Willerud, Johan Palsson,"
+		"M'boumba Cedric Madianga");
 MODULE_ALIAS("platform:ab8500_gpadc");
 MODULE_DESCRIPTION("AB8500 GPADC driver");
diff --git a/drivers/mfd/ab8500-sysctrl.c b/drivers/mfd/ab8500-sysctrl.c
index 108fd86552f0..fbca1ced49fa 100644
--- a/drivers/mfd/ab8500-sysctrl.c
+++ b/drivers/mfd/ab8500-sysctrl.c
@@ -15,19 +15,30 @@
 #include <linux/mfd/abx500/ab8500.h>
 #include <linux/mfd/abx500/ab8500-sysctrl.h>
 
+/* RtcCtrl bits */
+#define AB8500_ALARM_MIN_LOW  0x08
+#define AB8500_ALARM_MIN_MID 0x09
+#define RTC_CTRL 0x0B
+#define RTC_ALARM_ENABLE 0x4
+
 static struct device *sysctrl_dev;
 
 void ab8500_power_off(void)
 {
 	sigset_t old;
 	sigset_t all;
-	static char *pss[] = {"ab8500_ac", "ab8500_usb"};
+	static char *pss[] = {"ab8500_ac", "pm2301", "ab8500_usb"};
 	int i;
 	bool charger_present = false;
 	union power_supply_propval val;
 	struct power_supply *psy;
 	int ret;
 
+	if (sysctrl_dev == NULL) {
+		pr_err("%s: sysctrl not initialized\n", __func__);
+		return;
+	}
+
 	/*
 	 * If we have a charger connected and we're powering off,
 	 * reboot into charge-only mode.
@@ -74,6 +85,63 @@ shutdown:
 	}
 }
 
+/*
+ * Use the AB WD to reset the platform. It will perform a hard
+ * reset instead of a soft reset. Write the reset reason to
+ * the AB before reset, which can be read upon restart.
+ */
+void ab8500_restart(char mode, const char *cmd)
+{
+	struct ab8500_platform_data *plat;
+	struct ab8500_sysctrl_platform_data *pdata;
+	u16 reason = 0;
+	u8 val;
+
+	if (sysctrl_dev == NULL) {
+		pr_err("%s: sysctrl not initialized\n", __func__);
+		return;
+	}
+
+	plat = dev_get_platdata(sysctrl_dev->parent);
+	pdata = plat->sysctrl;
+	if (pdata->reboot_reason_code)
+		reason = pdata->reboot_reason_code(cmd);
+	else
+		pr_warn("[%s] No reboot reason set. Default reason %d\n",
+			__func__, reason);
+
+	/*
+	 * Disable RTC alarm, just a precaution so that no alarm
+	 * is running when WD reset is executed.
+	 */
+	abx500_get_register_interruptible(sysctrl_dev, AB8500_RTC,
+		RTC_CTRL , &val);
+	abx500_set_register_interruptible(sysctrl_dev, AB8500_RTC,
+		RTC_CTRL , (val & ~RTC_ALARM_ENABLE));
+
+	/*
+	 * Android is not using the RTC alarm registers during reboot
+	 * so we borrow them for writing the reason of reset
+	 */
+
+	/* reason[8 LSB] */
+	val = reason & 0xFF;
+	abx500_set_register_interruptible(sysctrl_dev, AB8500_RTC,
+		AB8500_ALARM_MIN_LOW , val);
+
+	/* reason[8 MSB] */
+	val = (reason>>8) & 0xFF;
+	abx500_set_register_interruptible(sysctrl_dev, AB8500_RTC,
+		AB8500_ALARM_MIN_MID , val);
+
+	/* Setting WD timeout to 0 */
+	ab8500_sysctrl_write(AB8500_MAINWDOGTIMER, 0xFF, 0x0);
+
+	/* Setting the parameters to AB8500 WD*/
+	ab8500_sysctrl_write(AB8500_MAINWDOGCTRL, 0xFF, (AB8500_ENABLE_WD |
+		AB8500_WD_RESTART_ON_EXPIRE | AB8500_KICK_WD));
+}
+
 static inline bool valid_bank(u8 bank)
 {
 	return ((bank == AB8500_SYS_CTRL1_BLOCK) ||
@@ -85,7 +153,7 @@ int ab8500_sysctrl_read(u16 reg, u8 *value)
 	u8 bank;
 
 	if (sysctrl_dev == NULL)
-		return -EAGAIN;
+		return -EINVAL;
 
 	bank = (reg >> 8);
 	if (!valid_bank(bank))
@@ -101,7 +169,7 @@ int ab8500_sysctrl_write(u16 reg, u8 mask, u8 value)
 	u8 bank;
 
 	if (sysctrl_dev == NULL)
-		return -EAGAIN;
+		return -EINVAL;
 
 	bank = (reg >> 8);
 	if (!valid_bank(bank))
@@ -114,28 +182,36 @@ EXPORT_SYMBOL(ab8500_sysctrl_write);
 
 static int ab8500_sysctrl_probe(struct platform_device *pdev)
 {
+	struct ab8500 *ab8500 = dev_get_drvdata(pdev->dev.parent);
 	struct ab8500_platform_data *plat;
 	struct ab8500_sysctrl_platform_data *pdata;
 
-	sysctrl_dev = &pdev->dev;
 	plat = dev_get_platdata(pdev->dev.parent);
+
+	if (!(plat && plat->sysctrl))
+		return -EINVAL;
+
 	if (plat->pm_power_off)
 		pm_power_off = ab8500_power_off;
 
 	pdata = plat->sysctrl;
 
 	if (pdata) {
-		int ret, i, j;
+		int last, ret, i, j;
+
+		if (is_ab8505(ab8500))
+			last = AB8500_SYSCLKREQ4RFCLKBUF;
+		else
+			last = AB8500_SYSCLKREQ8RFCLKBUF;
 
-		for (i = AB8500_SYSCLKREQ1RFCLKBUF;
-		     i <= AB8500_SYSCLKREQ8RFCLKBUF; i++) {
+		for (i = AB8500_SYSCLKREQ1RFCLKBUF; i <= last; i++) {
 			j = i - AB8500_SYSCLKREQ1RFCLKBUF;
 			ret = ab8500_sysctrl_write(i, 0xff,
-						   pdata->initial_req_buf_config[j]);
+					pdata->initial_req_buf_config[j]);
 			dev_dbg(&pdev->dev,
-				"Setting SysClkReq%dRfClkBuf 0x%X\n",
-				j + 1,
-				pdata->initial_req_buf_config[j]);
+					"Setting SysClkReq%dRfClkBuf 0x%X\n",
+					j + 1,
+					pdata->initial_req_buf_config[j]);
 			if (ret < 0) {
 				dev_err(&pdev->dev,
 					"unable to set sysClkReq%dRfClkBuf: "
@@ -166,7 +242,7 @@ static int __init ab8500_sysctrl_init(void)
 {
 	return platform_driver_register(&ab8500_sysctrl_driver);
 }
-subsys_initcall(ab8500_sysctrl_init);
+arch_initcall(ab8500_sysctrl_init);
 
 MODULE_AUTHOR("Mattias Nilsson <mattias.i.nilsson@stericsson.com");
 MODULE_DESCRIPTION("AB8500 system control driver");
diff --git a/drivers/mfd/adp5520.c b/drivers/mfd/adp5520.c
index 210dd038bb5a..0d2eba023439 100644
--- a/drivers/mfd/adp5520.c
+++ b/drivers/mfd/adp5520.c
@@ -36,6 +36,7 @@ struct adp5520_chip {
 	struct blocking_notifier_head notifier_list;
 	int irq;
 	unsigned long id;
+	uint8_t mode;
 };
 
 static int __adp5520_read(struct i2c_client *client,
@@ -326,7 +327,10 @@ static int adp5520_suspend(struct device *dev)
 	struct i2c_client *client = to_i2c_client(dev);
 	struct adp5520_chip *chip = dev_get_drvdata(&client->dev);
 
-	adp5520_clr_bits(chip->dev, ADP5520_MODE_STATUS, ADP5520_nSTNBY);
+	adp5520_read(chip->dev, ADP5520_MODE_STATUS, &chip->mode);
+	/* All other bits are W1C */
+	chip->mode &= ADP5520_BL_EN | ADP5520_DIM_EN | ADP5520_nSTNBY;
+	adp5520_write(chip->dev, ADP5520_MODE_STATUS, 0);
 	return 0;
 }
 
@@ -335,7 +339,7 @@ static int adp5520_resume(struct device *dev)
 	struct i2c_client *client = to_i2c_client(dev);
 	struct adp5520_chip *chip = dev_get_drvdata(&client->dev);
 
-	adp5520_set_bits(chip->dev, ADP5520_MODE_STATUS, ADP5520_nSTNBY);
+	adp5520_write(chip->dev, ADP5520_MODE_STATUS, chip->mode);
 	return 0;
 }
 #endif
@@ -360,17 +364,7 @@ static struct i2c_driver adp5520_driver = {
 	.id_table 	= adp5520_id,
 };
 
-static int __init adp5520_init(void)
-{
-	return i2c_add_driver(&adp5520_driver);
-}
-module_init(adp5520_init);
-
-static void __exit adp5520_exit(void)
-{
-	i2c_del_driver(&adp5520_driver);
-}
-module_exit(adp5520_exit);
+module_i2c_driver(adp5520_driver);
 
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("ADP5520(01) PMIC-MFD Driver");
diff --git a/drivers/mfd/arizona-core.c b/drivers/mfd/arizona-core.c
index b562c7bf8a46..6ab03043fd60 100644
--- a/drivers/mfd/arizona-core.c
+++ b/drivers/mfd/arizona-core.c
@@ -39,11 +39,21 @@ int arizona_clk32k_enable(struct arizona *arizona)
 
 	arizona->clk32k_ref++;
 
-	if (arizona->clk32k_ref == 1)
+	if (arizona->clk32k_ref == 1) {
+		switch (arizona->pdata.clk32k_src) {
+		case ARIZONA_32KZ_MCLK1:
+			ret = pm_runtime_get_sync(arizona->dev);
+			if (ret != 0)
+				goto out;
+			break;
+		}
+
 		ret = regmap_update_bits(arizona->regmap, ARIZONA_CLOCK_32K_1,
 					 ARIZONA_CLK_32K_ENA,
 					 ARIZONA_CLK_32K_ENA);
+	}
 
+out:
 	if (ret != 0)
 		arizona->clk32k_ref--;
 
@@ -63,10 +73,17 @@ int arizona_clk32k_disable(struct arizona *arizona)
 
 	arizona->clk32k_ref--;
 
-	if (arizona->clk32k_ref == 0)
+	if (arizona->clk32k_ref == 0) {
 		regmap_update_bits(arizona->regmap, ARIZONA_CLOCK_32K_1,
 				   ARIZONA_CLK_32K_ENA, 0);
 
+		switch (arizona->pdata.clk32k_src) {
+		case ARIZONA_32KZ_MCLK1:
+			pm_runtime_put_sync(arizona->dev);
+			break;
+		}
+	}
+
 	mutex_unlock(&arizona->clk_lock);
 
 	return ret;
@@ -179,42 +196,134 @@ static irqreturn_t arizona_overclocked(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static int arizona_wait_for_boot(struct arizona *arizona)
+static int arizona_poll_reg(struct arizona *arizona,
+			    int timeout, unsigned int reg,
+			    unsigned int mask, unsigned int target)
 {
-	unsigned int reg;
+	unsigned int val = 0;
 	int ret, i;
 
+	for (i = 0; i < timeout; i++) {
+		ret = regmap_read(arizona->regmap, reg, &val);
+		if (ret != 0) {
+			dev_err(arizona->dev, "Failed to read reg %u: %d\n",
+				reg, ret);
+			continue;
+		}
+
+		if ((val & mask) == target)
+			return 0;
+
+		msleep(1);
+	}
+
+	dev_err(arizona->dev, "Polling reg %u timed out: %x\n", reg, val);
+	return -ETIMEDOUT;
+}
+
+static int arizona_wait_for_boot(struct arizona *arizona)
+{
+	int ret;
+
 	/*
 	 * We can't use an interrupt as we need to runtime resume to do so,
 	 * we won't race with the interrupt handler as it'll be blocked on
 	 * runtime resume.
 	 */
-	for (i = 0; i < 5; i++) {
-		msleep(1);
+	ret = arizona_poll_reg(arizona, 5, ARIZONA_INTERRUPT_RAW_STATUS_5,
+			       ARIZONA_BOOT_DONE_STS, ARIZONA_BOOT_DONE_STS);
 
-		ret = regmap_read(arizona->regmap,
-				  ARIZONA_INTERRUPT_RAW_STATUS_5, &reg);
-		if (ret != 0) {
-			dev_err(arizona->dev, "Failed to read boot state: %d\n",
-				ret);
-			continue;
-		}
+	if (!ret)
+		regmap_write(arizona->regmap, ARIZONA_INTERRUPT_STATUS_5,
+			     ARIZONA_BOOT_DONE_STS);
 
-		if (reg & ARIZONA_BOOT_DONE_STS)
-			break;
+	pm_runtime_mark_last_busy(arizona->dev);
+
+	return ret;
+}
+
+static int arizona_apply_hardware_patch(struct arizona* arizona)
+{
+	unsigned int fll, sysclk;
+	int ret, err;
+
+	regcache_cache_bypass(arizona->regmap, true);
+
+	/* Cache existing FLL and SYSCLK settings */
+	ret = regmap_read(arizona->regmap, ARIZONA_FLL1_CONTROL_1, &fll);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to cache FLL settings: %d\n",
+			ret);
+		return ret;
+	}
+	ret = regmap_read(arizona->regmap, ARIZONA_SYSTEM_CLOCK_1, &sysclk);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to cache SYSCLK settings: %d\n",
+			ret);
+		return ret;
 	}
 
-	if (reg & ARIZONA_BOOT_DONE_STS) {
-		regmap_write(arizona->regmap, ARIZONA_INTERRUPT_STATUS_5,
-			     ARIZONA_BOOT_DONE_STS);
-	} else {
-		dev_err(arizona->dev, "Device boot timed out: %x\n", reg);
-		return -ETIMEDOUT;
+	/* Start up SYSCLK using the FLL in free running mode */
+	ret = regmap_write(arizona->regmap, ARIZONA_FLL1_CONTROL_1,
+			ARIZONA_FLL1_ENA | ARIZONA_FLL1_FREERUN);
+	if (ret != 0) {
+		dev_err(arizona->dev,
+			"Failed to start FLL in freerunning mode: %d\n",
+			ret);
+		return ret;
+	}
+	ret = arizona_poll_reg(arizona, 25, ARIZONA_INTERRUPT_RAW_STATUS_5,
+			       ARIZONA_FLL1_CLOCK_OK_STS,
+			       ARIZONA_FLL1_CLOCK_OK_STS);
+	if (ret != 0) {
+		ret = -ETIMEDOUT;
+		goto err_fll;
 	}
 
-	pm_runtime_mark_last_busy(arizona->dev);
+	ret = regmap_write(arizona->regmap, ARIZONA_SYSTEM_CLOCK_1, 0x0144);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to start SYSCLK: %d\n", ret);
+		goto err_fll;
+	}
 
-	return 0;
+	/* Start the write sequencer and wait for it to finish */
+	ret = regmap_write(arizona->regmap, ARIZONA_WRITE_SEQUENCER_CTRL_0,
+			ARIZONA_WSEQ_ENA | ARIZONA_WSEQ_START | 160);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to start write sequencer: %d\n",
+			ret);
+		goto err_sysclk;
+	}
+	ret = arizona_poll_reg(arizona, 5, ARIZONA_WRITE_SEQUENCER_CTRL_1,
+			       ARIZONA_WSEQ_BUSY, 0);
+	if (ret != 0) {
+		regmap_write(arizona->regmap, ARIZONA_WRITE_SEQUENCER_CTRL_0,
+				ARIZONA_WSEQ_ABORT);
+		ret = -ETIMEDOUT;
+	}
+
+err_sysclk:
+	err = regmap_write(arizona->regmap, ARIZONA_SYSTEM_CLOCK_1, sysclk);
+	if (err != 0) {
+		dev_err(arizona->dev,
+			"Failed to re-apply old SYSCLK settings: %d\n",
+			err);
+	}
+
+err_fll:
+	err = regmap_write(arizona->regmap, ARIZONA_FLL1_CONTROL_1, fll);
+	if (err != 0) {
+		dev_err(arizona->dev,
+			"Failed to re-apply old FLL settings: %d\n",
+			err);
+	}
+
+	regcache_cache_bypass(arizona->regmap, false);
+
+	if (ret != 0)
+		return ret;
+	else
+		return err;
 }
 
 #ifdef CONFIG_PM_RUNTIME
@@ -233,20 +342,44 @@ static int arizona_runtime_resume(struct device *dev)
 
 	regcache_cache_only(arizona->regmap, false);
 
-	ret = arizona_wait_for_boot(arizona);
-	if (ret != 0) {
-		regulator_disable(arizona->dcvdd);
-		return ret;
+	switch (arizona->type) {
+	case WM5102:
+		ret = wm5102_patch(arizona);
+		if (ret != 0) {
+			dev_err(arizona->dev, "Failed to apply patch: %d\n",
+				ret);
+			goto err;
+		}
+
+		ret = arizona_apply_hardware_patch(arizona);
+		if (ret != 0) {
+			dev_err(arizona->dev,
+				"Failed to apply hardware patch: %d\n",
+				ret);
+			goto err;
+		}
+		break;
+	default:
+		ret = arizona_wait_for_boot(arizona);
+		if (ret != 0) {
+			goto err;
+		}
+
+		break;
 	}
 
 	ret = regcache_sync(arizona->regmap);
 	if (ret != 0) {
 		dev_err(arizona->dev, "Failed to restore register cache\n");
-		regulator_disable(arizona->dcvdd);
-		return ret;
+		goto err;
 	}
 
 	return 0;
+
+err:
+	regcache_cache_only(arizona->regmap, true);
+	regulator_disable(arizona->dcvdd);
+	return ret;
 }
 
 static int arizona_runtime_suspend(struct device *dev)
@@ -371,6 +504,17 @@ int arizona_dev_init(struct arizona *arizona)
 		goto err_early;
 	}
 
+	if (arizona->pdata.reset) {
+		/* Start out with /RESET low to put the chip into reset */
+		ret = gpio_request_one(arizona->pdata.reset,
+				       GPIOF_DIR_OUT | GPIOF_INIT_LOW,
+				       "arizona /RESET");
+		if (ret != 0) {
+			dev_err(dev, "Failed to request /RESET: %d\n", ret);
+			goto err_early;
+		}
+	}
+
 	ret = regulator_bulk_enable(arizona->num_core_supplies,
 				    arizona->core_supplies);
 	if (ret != 0) {
@@ -386,16 +530,8 @@ int arizona_dev_init(struct arizona *arizona)
 	}
 
 	if (arizona->pdata.reset) {
-		/* Start out with /RESET low to put the chip into reset */
-		ret = gpio_request_one(arizona->pdata.reset,
-				       GPIOF_DIR_OUT | GPIOF_INIT_LOW,
-				       "arizona /RESET");
-		if (ret != 0) {
-			dev_err(dev, "Failed to request /RESET: %d\n", ret);
-			goto err_dcvdd;
-		}
-
 		gpio_set_value_cansleep(arizona->pdata.reset, 1);
+		msleep(1);
 	}
 
 	regcache_cache_only(arizona->regmap, false);
@@ -424,6 +560,7 @@ int arizona_dev_init(struct arizona *arizona)
 			arizona->type = WM5102;
 		}
 		apply_patch = wm5102_patch;
+		arizona->rev &= 0x7;
 		break;
 #endif
 #ifdef CONFIG_MFD_WM5110
@@ -454,6 +591,8 @@ int arizona_dev_init(struct arizona *arizona)
 			goto err_reset;
 		}
 
+		msleep(1);
+
 		ret = regcache_sync(arizona->regmap);
 		if (ret != 0) {
 			dev_err(dev, "Failed to sync device: %d\n", ret);
@@ -461,10 +600,24 @@ int arizona_dev_init(struct arizona *arizona)
 		}
 	}
 
-	ret = arizona_wait_for_boot(arizona);
-	if (ret != 0) {
-		dev_err(arizona->dev, "Device failed initial boot: %d\n", ret);
-		goto err_reset;
+	switch (arizona->type) {
+	case WM5102:
+		ret = regmap_read(arizona->regmap, 0x19, &val);
+		if (ret != 0)
+			dev_err(dev,
+				"Failed to check write sequencer state: %d\n",
+				ret);
+		else if (val & 0x01)
+			break;
+		/* Fall through */
+	default:
+		ret = arizona_wait_for_boot(arizona);
+		if (ret != 0) {
+			dev_err(arizona->dev,
+				"Device failed initial boot: %d\n", ret);
+			goto err_reset;
+		}
+		break;
 	}
 
 	if (apply_patch) {
@@ -474,6 +627,20 @@ int arizona_dev_init(struct arizona *arizona)
 				ret);
 			goto err_reset;
 		}
+
+		switch (arizona->type) {
+		case WM5102:
+			ret = arizona_apply_hardware_patch(arizona);
+			if (ret != 0) {
+				dev_err(arizona->dev,
+					"Failed to apply hardware patch: %d\n",
+					ret);
+				goto err_reset;
+			}
+			break;
+		default:
+			break;
+		}
 	}
 
 	for (i = 0; i < ARRAY_SIZE(arizona->pdata.gpio_defaults); i++) {
@@ -498,6 +665,7 @@ int arizona_dev_init(struct arizona *arizona)
 		regmap_update_bits(arizona->regmap, ARIZONA_CLOCK_32K_1,
 				   ARIZONA_CLK_32K_SRC_MASK,
 				   arizona->pdata.clk32k_src - 1);
+		arizona_clk32k_enable(arizona);
 		break;
 	case ARIZONA_32KZ_NONE:
 		regmap_update_bits(arizona->regmap, ARIZONA_CLOCK_32K_1,
@@ -511,10 +679,16 @@ int arizona_dev_init(struct arizona *arizona)
 	}
 
 	for (i = 0; i < ARIZONA_MAX_MICBIAS; i++) {
-		if (!arizona->pdata.micbias[i].mV)
+		if (!arizona->pdata.micbias[i].mV &&
+		    !arizona->pdata.micbias[i].bypass)
 			continue;
 
+		/* Apply default for bypass mode */
+		if (!arizona->pdata.micbias[i].mV)
+			arizona->pdata.micbias[i].mV = 2800;
+
 		val = (arizona->pdata.micbias[i].mV - 1500) / 100;
+
 		val <<= ARIZONA_MICB1_LVL_SHIFT;
 
 		if (arizona->pdata.micbias[i].ext_cap)
@@ -526,10 +700,14 @@ int arizona_dev_init(struct arizona *arizona)
 		if (arizona->pdata.micbias[i].fast_start)
 			val |= ARIZONA_MICB1_RATE;
 
+		if (arizona->pdata.micbias[i].bypass)
+			val |= ARIZONA_MICB1_BYPASS;
+
 		regmap_update_bits(arizona->regmap,
 				   ARIZONA_MIC_BIAS_CTRL_1 + i,
 				   ARIZONA_MICB1_LVL_MASK |
 				   ARIZONA_MICB1_DISCH |
+				   ARIZONA_MICB1_BYPASS |
 				   ARIZONA_MICB1_RATE, val);
 	}
 
@@ -610,10 +788,9 @@ err_irq:
 	arizona_irq_exit(arizona);
 err_reset:
 	if (arizona->pdata.reset) {
-		gpio_set_value_cansleep(arizona->pdata.reset, 1);
+		gpio_set_value_cansleep(arizona->pdata.reset, 0);
 		gpio_free(arizona->pdata.reset);
 	}
-err_dcvdd:
 	regulator_disable(arizona->dcvdd);
 err_enable:
 	regulator_bulk_disable(arizona->num_core_supplies,
diff --git a/drivers/mfd/arizona-irq.c b/drivers/mfd/arizona-irq.c
index 2bec5f0db3ee..64cd9b6dac92 100644
--- a/drivers/mfd/arizona-irq.c
+++ b/drivers/mfd/arizona-irq.c
@@ -94,6 +94,7 @@ static irqreturn_t arizona_ctrlif_err(int irq, void *data)
 static irqreturn_t arizona_irq_thread(int irq, void *data)
 {
 	struct arizona *arizona = data;
+	bool poll;
 	unsigned int val;
 	int ret;
 
@@ -103,20 +104,39 @@ static irqreturn_t arizona_irq_thread(int irq, void *data)
 		return IRQ_NONE;
 	}
 
-	/* Always handle the AoD domain */
-	handle_nested_irq(irq_find_mapping(arizona->virq, 0));
+	do {
+		poll = false;
+
+		/* Always handle the AoD domain */
+		handle_nested_irq(irq_find_mapping(arizona->virq, 0));
+
+		/*
+		 * Check if one of the main interrupts is asserted and only
+		 * check that domain if it is.
+		 */
+		ret = regmap_read(arizona->regmap, ARIZONA_IRQ_PIN_STATUS,
+				  &val);
+		if (ret == 0 && val & ARIZONA_IRQ1_STS) {
+			handle_nested_irq(irq_find_mapping(arizona->virq, 1));
+		} else if (ret != 0) {
+			dev_err(arizona->dev,
+				"Failed to read main IRQ status: %d\n", ret);
+		}
 
-	/*
-	 * Check if one of the main interrupts is asserted and only
-	 * check that domain if it is.
-	 */
-	ret = regmap_read(arizona->regmap, ARIZONA_IRQ_PIN_STATUS, &val);
-	if (ret == 0 && val & ARIZONA_IRQ1_STS) {
-		handle_nested_irq(irq_find_mapping(arizona->virq, 1));
-	} else if (ret != 0) {
-		dev_err(arizona->dev, "Failed to read main IRQ status: %d\n",
-			ret);
-	}
+		/*
+		 * Poll the IRQ pin status to see if we're really done
+		 * if the interrupt controller can't do it for us.
+		 */
+		if (!arizona->pdata.irq_gpio) {
+			break;
+		} else if (arizona->pdata.irq_flags & IRQF_TRIGGER_RISING &&
+			   gpio_get_value_cansleep(arizona->pdata.irq_gpio)) {
+			poll = true;
+		} else if (arizona->pdata.irq_flags & IRQF_TRIGGER_FALLING &&
+			   !gpio_get_value_cansleep(arizona->pdata.irq_gpio)) {
+			poll = true;
+		}
+	} while (poll);
 
 	pm_runtime_mark_last_busy(arizona->dev);
 	pm_runtime_put_autosuspend(arizona->dev);
@@ -169,6 +189,7 @@ int arizona_irq_init(struct arizona *arizona)
 	int ret, i;
 	const struct regmap_irq_chip *aod, *irq;
 	bool ctrlif_error = true;
+	struct irq_data *irq_data;
 
 	switch (arizona->type) {
 #ifdef CONFIG_MFD_WM5102
@@ -192,7 +213,36 @@ int arizona_irq_init(struct arizona *arizona)
 		return -EINVAL;
 	}
 
-	if (arizona->pdata.irq_active_high) {
+	/* Disable all wake sources by default */
+	regmap_write(arizona->regmap, ARIZONA_WAKE_CONTROL, 0);
+
+	/* Read the flags from the interrupt controller if not specified */
+	if (!arizona->pdata.irq_flags) {
+		irq_data = irq_get_irq_data(arizona->irq);
+		if (!irq_data) {
+			dev_err(arizona->dev, "Invalid IRQ: %d\n",
+				arizona->irq);
+			return -EINVAL;
+		}
+
+		arizona->pdata.irq_flags = irqd_get_trigger_type(irq_data);
+		switch (arizona->pdata.irq_flags) {
+		case IRQF_TRIGGER_LOW:
+		case IRQF_TRIGGER_HIGH:
+		case IRQF_TRIGGER_RISING:
+		case IRQF_TRIGGER_FALLING:
+			break;
+
+		case IRQ_TYPE_NONE:
+		default:
+			/* Device default */
+			arizona->pdata.irq_flags = IRQF_TRIGGER_LOW;
+			break;
+		}
+	}
+
+	if (arizona->pdata.irq_flags & (IRQF_TRIGGER_HIGH |
+					IRQF_TRIGGER_RISING)) {
 		ret = regmap_update_bits(arizona->regmap, ARIZONA_IRQ_CTRL_1,
 					 ARIZONA_IRQ_POL, 0);
 		if (ret != 0) {
@@ -200,12 +250,10 @@ int arizona_irq_init(struct arizona *arizona)
 				ret);
 			goto err;
 		}
-
-		flags |= IRQF_TRIGGER_HIGH;
-	} else {
-		flags |= IRQF_TRIGGER_LOW;
 	}
 
+	flags |= arizona->pdata.irq_flags;
+
 	/* Allocate a virtual IRQ domain to distribute to the regmap domains */
 	arizona->virq = irq_domain_add_linear(NULL, 2, &arizona_domain_ops,
 					      arizona);
@@ -257,11 +305,31 @@ int arizona_irq_init(struct arizona *arizona)
 		}
 	}
 
+	/* Used to emulate edge trigger and to work around broken pinmux */
+	if (arizona->pdata.irq_gpio) {
+		if (gpio_to_irq(arizona->pdata.irq_gpio) != arizona->irq) {
+			dev_warn(arizona->dev, "IRQ %d is not GPIO %d (%d)\n",
+				 arizona->irq, arizona->pdata.irq_gpio,
+				 gpio_to_irq(arizona->pdata.irq_gpio));
+			arizona->irq = gpio_to_irq(arizona->pdata.irq_gpio);
+		}
+
+		ret = devm_gpio_request_one(arizona->dev,
+					    arizona->pdata.irq_gpio,
+					    GPIOF_IN, "arizona IRQ");
+		if (ret != 0) {
+			dev_err(arizona->dev,
+				"Failed to request IRQ GPIO %d:: %d\n",
+				arizona->pdata.irq_gpio, ret);
+			arizona->pdata.irq_gpio = 0;
+		}
+	}
+
 	ret = request_threaded_irq(arizona->irq, NULL, arizona_irq_thread,
 				   flags, "arizona", arizona);
 
 	if (ret != 0) {
-		dev_err(arizona->dev, "Failed to request IRQ %d: %d\n",
+		dev_err(arizona->dev, "Failed to request primary IRQ %d: %d\n",
 			arizona->irq, ret);
 		goto err_main_irq;
 	}
diff --git a/drivers/mfd/arizona-spi.c b/drivers/mfd/arizona-spi.c
index 1b9fdd698b03..b57e642d2b4a 100644
--- a/drivers/mfd/arizona-spi.c
+++ b/drivers/mfd/arizona-spi.c
@@ -67,7 +67,7 @@ static int arizona_spi_probe(struct spi_device *spi)
 
 static int arizona_spi_remove(struct spi_device *spi)
 {
-	struct arizona *arizona = dev_get_drvdata(&spi->dev);
+	struct arizona *arizona = spi_get_drvdata(spi);
 	arizona_dev_exit(arizona);
 	return 0;
 }
diff --git a/drivers/mfd/as3711.c b/drivers/mfd/as3711.c
index e994c9691124..01e414162702 100644
--- a/drivers/mfd/as3711.c
+++ b/drivers/mfd/as3711.c
@@ -112,16 +112,34 @@ static const struct regmap_config as3711_regmap_config = {
 	.cache_type = REGCACHE_RBTREE,
 };
 
+#ifdef CONFIG_OF
+static struct of_device_id as3711_of_match[] = {
+	{.compatible = "ams,as3711",},
+	{}
+};
+MODULE_DEVICE_TABLE(of, as3711_of_match);
+#endif
+
 static int as3711_i2c_probe(struct i2c_client *client,
 			    const struct i2c_device_id *id)
 {
 	struct as3711 *as3711;
-	struct as3711_platform_data *pdata = client->dev.platform_data;
+	struct as3711_platform_data *pdata;
 	unsigned int id1, id2;
 	int ret;
 
-	if (!pdata)
-		dev_dbg(&client->dev, "Platform data not found\n");
+	if (!client->dev.of_node) {
+		pdata = client->dev.platform_data;
+		if (!pdata)
+			dev_dbg(&client->dev, "Platform data not found\n");
+	} else {
+		pdata = devm_kzalloc(&client->dev,
+				     sizeof(*pdata), GFP_KERNEL);
+		if (!pdata) {
+			dev_err(&client->dev, "Failed to allocate pdata\n");
+			return -ENOMEM;
+		}
+	}
 
 	as3711 = devm_kzalloc(&client->dev, sizeof(struct as3711), GFP_KERNEL);
 	if (!as3711) {
@@ -193,7 +211,8 @@ static struct i2c_driver as3711_i2c_driver = {
 	.driver = {
 		   .name = "as3711",
 		   .owner = THIS_MODULE,
-		   },
+		   .of_match_table = of_match_ptr(as3711_of_match),
+	},
 	.probe = as3711_i2c_probe,
 	.remove = as3711_i2c_remove,
 	.id_table = as3711_i2c_id,
diff --git a/drivers/mfd/cros_ec.c b/drivers/mfd/cros_ec.c
new file mode 100644
index 000000000000..10cd14e35eb0
--- /dev/null
+++ b/drivers/mfd/cros_ec.c
@@ -0,0 +1,196 @@
+/*
+ * ChromeOS EC multi-function device
+ *
+ * Copyright (C) 2012 Google, Inc
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ * The ChromeOS EC multi function device is used to mux all the requests
+ * to the EC device for its multiple features: keyboard controller,
+ * battery charging and regulator control, firmware update.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/cros_ec.h>
+#include <linux/mfd/cros_ec_commands.h>
+
+int cros_ec_prepare_tx(struct cros_ec_device *ec_dev,
+		       struct cros_ec_msg *msg)
+{
+	uint8_t *out;
+	int csum, i;
+
+	BUG_ON(msg->out_len > EC_HOST_PARAM_SIZE);
+	out = ec_dev->dout;
+	out[0] = EC_CMD_VERSION0 + msg->version;
+	out[1] = msg->cmd;
+	out[2] = msg->out_len;
+	csum = out[0] + out[1] + out[2];
+	for (i = 0; i < msg->out_len; i++)
+		csum += out[EC_MSG_TX_HEADER_BYTES + i] = msg->out_buf[i];
+	out[EC_MSG_TX_HEADER_BYTES + msg->out_len] = (uint8_t)(csum & 0xff);
+
+	return EC_MSG_TX_PROTO_BYTES + msg->out_len;
+}
+EXPORT_SYMBOL(cros_ec_prepare_tx);
+
+static int cros_ec_command_sendrecv(struct cros_ec_device *ec_dev,
+		uint16_t cmd, void *out_buf, int out_len,
+		void *in_buf, int in_len)
+{
+	struct cros_ec_msg msg;
+
+	msg.version = cmd >> 8;
+	msg.cmd = cmd & 0xff;
+	msg.out_buf = out_buf;
+	msg.out_len = out_len;
+	msg.in_buf = in_buf;
+	msg.in_len = in_len;
+
+	return ec_dev->command_xfer(ec_dev, &msg);
+}
+
+static int cros_ec_command_recv(struct cros_ec_device *ec_dev,
+		uint16_t cmd, void *buf, int buf_len)
+{
+	return cros_ec_command_sendrecv(ec_dev, cmd, NULL, 0, buf, buf_len);
+}
+
+static int cros_ec_command_send(struct cros_ec_device *ec_dev,
+		uint16_t cmd, void *buf, int buf_len)
+{
+	return cros_ec_command_sendrecv(ec_dev, cmd, buf, buf_len, NULL, 0);
+}
+
+static irqreturn_t ec_irq_thread(int irq, void *data)
+{
+	struct cros_ec_device *ec_dev = data;
+
+	if (device_may_wakeup(ec_dev->dev))
+		pm_wakeup_event(ec_dev->dev, 0);
+
+	blocking_notifier_call_chain(&ec_dev->event_notifier, 1, ec_dev);
+
+	return IRQ_HANDLED;
+}
+
+static struct mfd_cell cros_devs[] = {
+	{
+		.name = "cros-ec-keyb",
+		.id = 1,
+		.of_compatible = "google,cros-ec-keyb",
+	},
+};
+
+int cros_ec_register(struct cros_ec_device *ec_dev)
+{
+	struct device *dev = ec_dev->dev;
+	int err = 0;
+
+	BLOCKING_INIT_NOTIFIER_HEAD(&ec_dev->event_notifier);
+
+	ec_dev->command_send = cros_ec_command_send;
+	ec_dev->command_recv = cros_ec_command_recv;
+	ec_dev->command_sendrecv = cros_ec_command_sendrecv;
+
+	if (ec_dev->din_size) {
+		ec_dev->din = kmalloc(ec_dev->din_size, GFP_KERNEL);
+		if (!ec_dev->din) {
+			err = -ENOMEM;
+			goto fail_din;
+		}
+	}
+	if (ec_dev->dout_size) {
+		ec_dev->dout = kmalloc(ec_dev->dout_size, GFP_KERNEL);
+		if (!ec_dev->dout) {
+			err = -ENOMEM;
+			goto fail_dout;
+		}
+	}
+
+	if (!ec_dev->irq) {
+		dev_dbg(dev, "no valid IRQ: %d\n", ec_dev->irq);
+		goto fail_irq;
+	}
+
+	err = request_threaded_irq(ec_dev->irq, NULL, ec_irq_thread,
+				   IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+				   "chromeos-ec", ec_dev);
+	if (err) {
+		dev_err(dev, "request irq %d: error %d\n", ec_dev->irq, err);
+		goto fail_irq;
+	}
+
+	err = mfd_add_devices(dev, 0, cros_devs,
+			      ARRAY_SIZE(cros_devs),
+			      NULL, ec_dev->irq, NULL);
+	if (err) {
+		dev_err(dev, "failed to add mfd devices\n");
+		goto fail_mfd;
+	}
+
+	dev_info(dev, "Chrome EC (%s)\n", ec_dev->name);
+
+	return 0;
+
+fail_mfd:
+	free_irq(ec_dev->irq, ec_dev);
+fail_irq:
+	kfree(ec_dev->dout);
+fail_dout:
+	kfree(ec_dev->din);
+fail_din:
+	return err;
+}
+EXPORT_SYMBOL(cros_ec_register);
+
+int cros_ec_remove(struct cros_ec_device *ec_dev)
+{
+	mfd_remove_devices(ec_dev->dev);
+	free_irq(ec_dev->irq, ec_dev);
+	kfree(ec_dev->dout);
+	kfree(ec_dev->din);
+
+	return 0;
+}
+EXPORT_SYMBOL(cros_ec_remove);
+
+#ifdef CONFIG_PM_SLEEP
+int cros_ec_suspend(struct cros_ec_device *ec_dev)
+{
+	struct device *dev = ec_dev->dev;
+
+	if (device_may_wakeup(dev))
+		ec_dev->wake_enabled = !enable_irq_wake(ec_dev->irq);
+
+	disable_irq(ec_dev->irq);
+	ec_dev->was_wake_device = ec_dev->wake_enabled;
+
+	return 0;
+}
+EXPORT_SYMBOL(cros_ec_suspend);
+
+int cros_ec_resume(struct cros_ec_device *ec_dev)
+{
+	enable_irq(ec_dev->irq);
+
+	if (ec_dev->wake_enabled) {
+		disable_irq_wake(ec_dev->irq);
+		ec_dev->wake_enabled = 0;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(cros_ec_resume);
+
+#endif
diff --git a/drivers/mfd/cros_ec_i2c.c b/drivers/mfd/cros_ec_i2c.c
new file mode 100644
index 000000000000..123044608b63
--- /dev/null
+++ b/drivers/mfd/cros_ec_i2c.c
@@ -0,0 +1,201 @@
+/*
+ * ChromeOS EC multi-function device (I2C)
+ *
+ * Copyright (C) 2012 Google, Inc
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/mfd/cros_ec.h>
+#include <linux/mfd/cros_ec_commands.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+static inline struct cros_ec_device *to_ec_dev(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+
+	return i2c_get_clientdata(client);
+}
+
+static int cros_ec_command_xfer(struct cros_ec_device *ec_dev,
+				struct cros_ec_msg *msg)
+{
+	struct i2c_client *client = ec_dev->priv;
+	int ret = -ENOMEM;
+	int i;
+	int packet_len;
+	u8 *out_buf = NULL;
+	u8 *in_buf = NULL;
+	u8 sum;
+	struct i2c_msg i2c_msg[2];
+
+	i2c_msg[0].addr = client->addr;
+	i2c_msg[0].flags = 0;
+	i2c_msg[1].addr = client->addr;
+	i2c_msg[1].flags = I2C_M_RD;
+
+	/*
+	 * allocate larger packet (one byte for checksum, one byte for
+	 * length, and one for result code)
+	 */
+	packet_len = msg->in_len + 3;
+	in_buf = kzalloc(packet_len, GFP_KERNEL);
+	if (!in_buf)
+		goto done;
+	i2c_msg[1].len = packet_len;
+	i2c_msg[1].buf = (char *)in_buf;
+
+	/*
+	 * allocate larger packet (one byte for checksum, one for
+	 * command code, one for length, and one for command version)
+	 */
+	packet_len = msg->out_len + 4;
+	out_buf = kzalloc(packet_len, GFP_KERNEL);
+	if (!out_buf)
+		goto done;
+	i2c_msg[0].len = packet_len;
+	i2c_msg[0].buf = (char *)out_buf;
+
+	out_buf[0] = EC_CMD_VERSION0 + msg->version;
+	out_buf[1] = msg->cmd;
+	out_buf[2] = msg->out_len;
+
+	/* copy message payload and compute checksum */
+	sum = out_buf[0] + out_buf[1] + out_buf[2];
+	for (i = 0; i < msg->out_len; i++) {
+		out_buf[3 + i] = msg->out_buf[i];
+		sum += out_buf[3 + i];
+	}
+	out_buf[3 + msg->out_len] = sum;
+
+	/* send command to EC and read answer */
+	ret = i2c_transfer(client->adapter, i2c_msg, 2);
+	if (ret < 0) {
+		dev_err(ec_dev->dev, "i2c transfer failed: %d\n", ret);
+		goto done;
+	} else if (ret != 2) {
+		dev_err(ec_dev->dev, "failed to get response: %d\n", ret);
+		ret = -EIO;
+		goto done;
+	}
+
+	/* check response error code */
+	if (i2c_msg[1].buf[0]) {
+		dev_warn(ec_dev->dev, "command 0x%02x returned an error %d\n",
+			 msg->cmd, i2c_msg[1].buf[0]);
+		ret = -EINVAL;
+		goto done;
+	}
+
+	/* copy response packet payload and compute checksum */
+	sum = in_buf[0] + in_buf[1];
+	for (i = 0; i < msg->in_len; i++) {
+		msg->in_buf[i] = in_buf[2 + i];
+		sum += in_buf[2 + i];
+	}
+	dev_dbg(ec_dev->dev, "packet: %*ph, sum = %02x\n",
+		i2c_msg[1].len, in_buf, sum);
+	if (sum != in_buf[2 + msg->in_len]) {
+		dev_err(ec_dev->dev, "bad packet checksum\n");
+		ret = -EBADMSG;
+		goto done;
+	}
+
+	ret = 0;
+ done:
+	kfree(in_buf);
+	kfree(out_buf);
+	return ret;
+}
+
+static int cros_ec_probe_i2c(struct i2c_client *client,
+			     const struct i2c_device_id *dev_id)
+{
+	struct device *dev = &client->dev;
+	struct cros_ec_device *ec_dev = NULL;
+	int err;
+
+ 	ec_dev = devm_kzalloc(dev, sizeof(*ec_dev), GFP_KERNEL);
+	if (!ec_dev)
+		return -ENOMEM;
+
+	i2c_set_clientdata(client, ec_dev);
+	ec_dev->name = "I2C";
+	ec_dev->dev = dev;
+	ec_dev->priv = client;
+	ec_dev->irq = client->irq;
+	ec_dev->command_xfer = cros_ec_command_xfer;
+	ec_dev->ec_name = client->name;
+	ec_dev->phys_name = client->adapter->name;
+	ec_dev->parent = &client->dev;
+
+	err = cros_ec_register(ec_dev);
+	if (err) {
+		dev_err(dev, "cannot register EC\n");
+		return err;
+	}
+
+	return 0;
+}
+
+static int cros_ec_remove_i2c(struct i2c_client *client)
+{
+	struct cros_ec_device *ec_dev = i2c_get_clientdata(client);
+
+	cros_ec_remove(ec_dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int cros_ec_i2c_suspend(struct device *dev)
+{
+	struct cros_ec_device *ec_dev = to_ec_dev(dev);
+
+	return cros_ec_suspend(ec_dev);
+}
+
+static int cros_ec_i2c_resume(struct device *dev)
+{
+	struct cros_ec_device *ec_dev = to_ec_dev(dev);
+
+	return cros_ec_resume(ec_dev);
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(cros_ec_i2c_pm_ops, cros_ec_i2c_suspend,
+			  cros_ec_i2c_resume);
+
+static const struct i2c_device_id cros_ec_i2c_id[] = {
+	{ "cros-ec-i2c", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, cros_ec_i2c_id);
+
+static struct i2c_driver cros_ec_driver = {
+	.driver	= {
+		.name	= "cros-ec-i2c",
+		.owner	= THIS_MODULE,
+		.pm	= &cros_ec_i2c_pm_ops,
+	},
+	.probe		= cros_ec_probe_i2c,
+	.remove		= cros_ec_remove_i2c,
+	.id_table	= cros_ec_i2c_id,
+};
+
+module_i2c_driver(cros_ec_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ChromeOS EC multi function device");
diff --git a/drivers/mfd/cros_ec_spi.c b/drivers/mfd/cros_ec_spi.c
new file mode 100644
index 000000000000..19193cf1e7a1
--- /dev/null
+++ b/drivers/mfd/cros_ec_spi.c
@@ -0,0 +1,375 @@
+/*
+ * ChromeOS EC multi-function device (SPI)
+ *
+ * Copyright (C) 2012 Google, Inc
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ */
+
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mfd/cros_ec.h>
+#include <linux/mfd/cros_ec_commands.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/spi/spi.h>
+
+
+/* The header byte, which follows the preamble */
+#define EC_MSG_HEADER			0xec
+
+/*
+ * Number of EC preamble bytes we read at a time. Since it takes
+ * about 400-500us for the EC to respond there is not a lot of
+ * point in tuning this. If the EC could respond faster then
+ * we could increase this so that might expect the preamble and
+ * message to occur in a single transaction. However, the maximum
+ * SPI transfer size is 256 bytes, so at 5MHz we need a response
+ * time of perhaps <320us (200 bytes / 1600 bits).
+ */
+#define EC_MSG_PREAMBLE_COUNT		32
+
+/*
+  * We must get a response from the EC in 5ms. This is a very long
+  * time, but the flash write command can take 2-3ms. The EC command
+  * processing is currently not very fast (about 500us). We could
+  * look at speeding this up and making the flash write command a
+  * 'slow' command, requiring a GET_STATUS wait loop, like flash
+  * erase.
+  */
+#define EC_MSG_DEADLINE_MS		5
+
+/*
+  * Time between raising the SPI chip select (for the end of a
+  * transaction) and dropping it again (for the next transaction).
+  * If we go too fast, the EC will miss the transaction. It seems
+  * that 50us is enough with the 16MHz STM32 EC.
+  */
+#define EC_SPI_RECOVERY_TIME_NS	(50 * 1000)
+
+/**
+ * struct cros_ec_spi - information about a SPI-connected EC
+ *
+ * @spi: SPI device we are connected to
+ * @last_transfer_ns: time that we last finished a transfer, or 0 if there
+ *	if no record
+ */
+struct cros_ec_spi {
+	struct spi_device *spi;
+	s64 last_transfer_ns;
+};
+
+static void debug_packet(struct device *dev, const char *name, u8 *ptr,
+			  int len)
+{
+#ifdef DEBUG
+	int i;
+
+	dev_dbg(dev, "%s: ", name);
+	for (i = 0; i < len; i++)
+		dev_cont(dev, " %02x", ptr[i]);
+#endif
+}
+
+/**
+ * cros_ec_spi_receive_response - Receive a response from the EC.
+ *
+ * This function has two phases: reading the preamble bytes (since if we read
+ * data from the EC before it is ready to send, we just get preamble) and
+ * reading the actual message.
+ *
+ * The received data is placed into ec_dev->din.
+ *
+ * @ec_dev: ChromeOS EC device
+ * @need_len: Number of message bytes we need to read
+ */
+static int cros_ec_spi_receive_response(struct cros_ec_device *ec_dev,
+					int need_len)
+{
+	struct cros_ec_spi *ec_spi = ec_dev->priv;
+	struct spi_transfer trans;
+	struct spi_message msg;
+	u8 *ptr, *end;
+	int ret;
+	unsigned long deadline;
+	int todo;
+
+	/* Receive data until we see the header byte */
+	deadline = jiffies + msecs_to_jiffies(EC_MSG_DEADLINE_MS);
+	do {
+		memset(&trans, '\0', sizeof(trans));
+		trans.cs_change = 1;
+		trans.rx_buf = ptr = ec_dev->din;
+		trans.len = EC_MSG_PREAMBLE_COUNT;
+
+		spi_message_init(&msg);
+		spi_message_add_tail(&trans, &msg);
+		ret = spi_sync(ec_spi->spi, &msg);
+		if (ret < 0) {
+			dev_err(ec_dev->dev, "spi transfer failed: %d\n", ret);
+			return ret;
+		}
+
+		for (end = ptr + EC_MSG_PREAMBLE_COUNT; ptr != end; ptr++) {
+			if (*ptr == EC_MSG_HEADER) {
+				dev_dbg(ec_dev->dev, "msg found at %ld\n",
+					ptr - ec_dev->din);
+				break;
+			}
+		}
+
+		if (time_after(jiffies, deadline)) {
+			dev_warn(ec_dev->dev, "EC failed to respond in time\n");
+			return -ETIMEDOUT;
+		}
+	} while (ptr == end);
+
+	/*
+	 * ptr now points to the header byte. Copy any valid data to the
+	 * start of our buffer
+	 */
+	todo = end - ++ptr;
+	BUG_ON(todo < 0 || todo > ec_dev->din_size);
+	todo = min(todo, need_len);
+	memmove(ec_dev->din, ptr, todo);
+	ptr = ec_dev->din + todo;
+	dev_dbg(ec_dev->dev, "need %d, got %d bytes from preamble\n",
+		 need_len, todo);
+	need_len -= todo;
+
+	/* Receive data until we have it all */
+	while (need_len > 0) {
+		/*
+		 * We can't support transfers larger than the SPI FIFO size
+		 * unless we have DMA. We don't have DMA on the ISP SPI ports
+		 * for Exynos. We need a way of asking SPI driver for
+		 * maximum-supported transfer size.
+		 */
+		todo = min(need_len, 256);
+		dev_dbg(ec_dev->dev, "loop, todo=%d, need_len=%d, ptr=%ld\n",
+			todo, need_len, ptr - ec_dev->din);
+
+		memset(&trans, '\0', sizeof(trans));
+		trans.cs_change = 1;
+		trans.rx_buf = ptr;
+		trans.len = todo;
+		spi_message_init(&msg);
+		spi_message_add_tail(&trans, &msg);
+
+		/* send command to EC and read answer */
+		BUG_ON((u8 *)trans.rx_buf - ec_dev->din + todo >
+				ec_dev->din_size);
+		ret = spi_sync(ec_spi->spi, &msg);
+		if (ret < 0) {
+			dev_err(ec_dev->dev, "spi transfer failed: %d\n", ret);
+			return ret;
+		}
+
+		debug_packet(ec_dev->dev, "interim", ptr, todo);
+		ptr += todo;
+		need_len -= todo;
+	}
+
+	dev_dbg(ec_dev->dev, "loop done, ptr=%ld\n", ptr - ec_dev->din);
+
+	return 0;
+}
+
+/**
+ * cros_ec_command_spi_xfer - Transfer a message over SPI and receive the reply
+ *
+ * @ec_dev: ChromeOS EC device
+ * @ec_msg: Message to transfer
+ */
+static int cros_ec_command_spi_xfer(struct cros_ec_device *ec_dev,
+				    struct cros_ec_msg *ec_msg)
+{
+	struct cros_ec_spi *ec_spi = ec_dev->priv;
+	struct spi_transfer trans;
+	struct spi_message msg;
+	int i, len;
+	u8 *ptr;
+	int sum;
+	int ret = 0, final_ret;
+	struct timespec ts;
+
+	len = cros_ec_prepare_tx(ec_dev, ec_msg);
+	dev_dbg(ec_dev->dev, "prepared, len=%d\n", len);
+
+	/* If it's too soon to do another transaction, wait */
+	if (ec_spi->last_transfer_ns) {
+		struct timespec ts;
+		unsigned long delay;	/* The delay completed so far */
+
+		ktime_get_ts(&ts);
+		delay = timespec_to_ns(&ts) - ec_spi->last_transfer_ns;
+		if (delay < EC_SPI_RECOVERY_TIME_NS)
+			ndelay(delay);
+	}
+
+	/* Transmit phase - send our message */
+	debug_packet(ec_dev->dev, "out", ec_dev->dout, len);
+	memset(&trans, '\0', sizeof(trans));
+	trans.tx_buf = ec_dev->dout;
+	trans.len = len;
+	trans.cs_change = 1;
+	spi_message_init(&msg);
+	spi_message_add_tail(&trans, &msg);
+	ret = spi_sync(ec_spi->spi, &msg);
+
+	/* Get the response */
+	if (!ret) {
+		ret = cros_ec_spi_receive_response(ec_dev,
+				ec_msg->in_len + EC_MSG_TX_PROTO_BYTES);
+	} else {
+		dev_err(ec_dev->dev, "spi transfer failed: %d\n", ret);
+	}
+
+	/* turn off CS */
+	spi_message_init(&msg);
+	final_ret = spi_sync(ec_spi->spi, &msg);
+	ktime_get_ts(&ts);
+	ec_spi->last_transfer_ns = timespec_to_ns(&ts);
+	if (!ret)
+		ret = final_ret;
+	if (ret < 0) {
+		dev_err(ec_dev->dev, "spi transfer failed: %d\n", ret);
+		return ret;
+	}
+
+	/* check response error code */
+	ptr = ec_dev->din;
+	if (ptr[0]) {
+		dev_warn(ec_dev->dev, "command 0x%02x returned an error %d\n",
+			 ec_msg->cmd, ptr[0]);
+		debug_packet(ec_dev->dev, "in_err", ptr, len);
+		return -EINVAL;
+	}
+	len = ptr[1];
+	sum = ptr[0] + ptr[1];
+	if (len > ec_msg->in_len) {
+		dev_err(ec_dev->dev, "packet too long (%d bytes, expected %d)",
+			len, ec_msg->in_len);
+		return -ENOSPC;
+	}
+
+	/* copy response packet payload and compute checksum */
+	for (i = 0; i < len; i++) {
+		sum += ptr[i + 2];
+		if (ec_msg->in_len)
+			ec_msg->in_buf[i] = ptr[i + 2];
+	}
+	sum &= 0xff;
+
+	debug_packet(ec_dev->dev, "in", ptr, len + 3);
+
+	if (sum != ptr[len + 2]) {
+		dev_err(ec_dev->dev,
+			"bad packet checksum, expected %02x, got %02x\n",
+			sum, ptr[len + 2]);
+		return -EBADMSG;
+	}
+
+	return 0;
+}
+
+static int cros_ec_probe_spi(struct spi_device *spi)
+{
+	struct device *dev = &spi->dev;
+	struct cros_ec_device *ec_dev;
+	struct cros_ec_spi *ec_spi;
+	int err;
+
+	spi->bits_per_word = 8;
+	spi->mode = SPI_MODE_0;
+	err = spi_setup(spi);
+	if (err < 0)
+		return err;
+
+	ec_spi = devm_kzalloc(dev, sizeof(*ec_spi), GFP_KERNEL);
+	if (ec_spi == NULL)
+		return -ENOMEM;
+	ec_spi->spi = spi;
+	ec_dev = devm_kzalloc(dev, sizeof(*ec_dev), GFP_KERNEL);
+	if (!ec_dev)
+		return -ENOMEM;
+
+	spi_set_drvdata(spi, ec_dev);
+	ec_dev->name = "SPI";
+	ec_dev->dev = dev;
+	ec_dev->priv = ec_spi;
+	ec_dev->irq = spi->irq;
+	ec_dev->command_xfer = cros_ec_command_spi_xfer;
+	ec_dev->ec_name = ec_spi->spi->modalias;
+	ec_dev->phys_name = dev_name(&ec_spi->spi->dev);
+	ec_dev->parent = &ec_spi->spi->dev;
+	ec_dev->din_size = EC_MSG_BYTES + EC_MSG_PREAMBLE_COUNT;
+	ec_dev->dout_size = EC_MSG_BYTES;
+
+	err = cros_ec_register(ec_dev);
+	if (err) {
+		dev_err(dev, "cannot register EC\n");
+		return err;
+	}
+
+	return 0;
+}
+
+static int cros_ec_remove_spi(struct spi_device *spi)
+{
+	struct cros_ec_device *ec_dev;
+
+	ec_dev = spi_get_drvdata(spi);
+	cros_ec_remove(ec_dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int cros_ec_spi_suspend(struct device *dev)
+{
+	struct cros_ec_device *ec_dev = dev_get_drvdata(dev);
+
+	return cros_ec_suspend(ec_dev);
+}
+
+static int cros_ec_spi_resume(struct device *dev)
+{
+	struct cros_ec_device *ec_dev = dev_get_drvdata(dev);
+
+	return cros_ec_resume(ec_dev);
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(cros_ec_spi_pm_ops, cros_ec_spi_suspend,
+			 cros_ec_spi_resume);
+
+static const struct spi_device_id cros_ec_spi_id[] = {
+	{ "cros-ec-spi", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(spi, cros_ec_spi_id);
+
+static struct spi_driver cros_ec_driver_spi = {
+	.driver	= {
+		.name	= "cros-ec-spi",
+		.owner	= THIS_MODULE,
+		.pm	= &cros_ec_spi_pm_ops,
+	},
+	.probe		= cros_ec_probe_spi,
+	.remove		= cros_ec_remove_spi,
+	.id_table	= cros_ec_spi_id,
+};
+
+module_spi_driver(cros_ec_driver_spi);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ChromeOS EC multi function device (SPI)");
diff --git a/drivers/mfd/da903x.c b/drivers/mfd/da903x.c
index 05176cd2862b..f1a316e0d6a6 100644
--- a/drivers/mfd/da903x.c
+++ b/drivers/mfd/da903x.c
@@ -499,7 +499,8 @@ static int da903x_probe(struct i2c_client *client,
 	unsigned int tmp;
 	int ret;
 
-	chip = kzalloc(sizeof(struct da903x_chip), GFP_KERNEL);
+	chip = devm_kzalloc(&client->dev, sizeof(struct da903x_chip),
+				GFP_KERNEL);
 	if (chip == NULL)
 		return -ENOMEM;
 
@@ -515,33 +516,27 @@ static int da903x_probe(struct i2c_client *client,
 
 	ret = chip->ops->init_chip(chip);
 	if (ret)
-		goto out_free_chip;
+		return ret;
 
 	/* mask and clear all IRQs */
 	chip->events_mask = 0xffffffff;
 	chip->ops->mask_events(chip, chip->events_mask);
 	chip->ops->read_events(chip, &tmp);
 
-	ret = request_irq(client->irq, da903x_irq_handler,
+	ret = devm_request_irq(&client->dev, client->irq, da903x_irq_handler,
 			IRQF_TRIGGER_FALLING,
 			"da903x", chip);
 	if (ret) {
 		dev_err(&client->dev, "failed to request irq %d\n",
 				client->irq);
-		goto out_free_chip;
+		return ret;
 	}
 
 	ret = da903x_add_subdevs(chip, pdata);
 	if (ret)
-		goto out_free_irq;
+		return ret;
 
 	return 0;
-
-out_free_irq:
-	free_irq(client->irq, chip);
-out_free_chip:
-	kfree(chip);
-	return ret;
 }
 
 static int da903x_remove(struct i2c_client *client)
@@ -549,8 +544,6 @@ static int da903x_remove(struct i2c_client *client)
 	struct da903x_chip *chip = i2c_get_clientdata(client);
 
 	da903x_remove_subdevs(chip);
-	free_irq(client->irq, chip);
-	kfree(chip);
 	return 0;
 }
 
diff --git a/drivers/mfd/da9052-spi.c b/drivers/mfd/da9052-spi.c
index 61d63b93576c..0680bcbc53de 100644
--- a/drivers/mfd/da9052-spi.c
+++ b/drivers/mfd/da9052-spi.c
@@ -38,7 +38,7 @@ static int da9052_spi_probe(struct spi_device *spi)
 	da9052->dev = &spi->dev;
 	da9052->chip_irq = spi->irq;
 
-	dev_set_drvdata(&spi->dev, da9052);
+	spi_set_drvdata(spi, da9052);
 
 	da9052_regmap_config.read_flag_mask = 1;
 	da9052_regmap_config.write_flag_mask = 0;
@@ -60,7 +60,7 @@ static int da9052_spi_probe(struct spi_device *spi)
 
 static int da9052_spi_remove(struct spi_device *spi)
 {
-	struct da9052 *da9052 = dev_get_drvdata(&spi->dev);
+	struct da9052 *da9052 = spi_get_drvdata(spi);
 
 	da9052_device_exit(da9052);
 	return 0;
diff --git a/drivers/mfd/da9055-core.c b/drivers/mfd/da9055-core.c
index f56a1a9f7777..49cb23d37469 100644
--- a/drivers/mfd/da9055-core.c
+++ b/drivers/mfd/da9055-core.c
@@ -391,7 +391,7 @@ int da9055_device_init(struct da9055 *da9055)
 		da9055->irq_base = pdata->irq_base;
 
 	ret = regmap_add_irq_chip(da9055->regmap, da9055->chip_irq,
-				  IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
+				  IRQF_TRIGGER_LOW | IRQF_ONESHOT,
 				  da9055->irq_base, &da9055_regmap_irq_chip,
 				  &da9055->irq_data);
 	if (ret < 0)
diff --git a/drivers/mfd/davinci_voicecodec.c b/drivers/mfd/davinci_voicecodec.c
index c0bcc872af4e..c60ab0c3c4db 100644
--- a/drivers/mfd/davinci_voicecodec.c
+++ b/drivers/mfd/davinci_voicecodec.c
@@ -177,17 +177,7 @@ static struct platform_driver davinci_vc_driver = {
 	.remove	= davinci_vc_remove,
 };
 
-static int __init davinci_vc_init(void)
-{
-	return platform_driver_probe(&davinci_vc_driver, davinci_vc_probe);
-}
-module_init(davinci_vc_init);
-
-static void __exit davinci_vc_exit(void)
-{
-	platform_driver_unregister(&davinci_vc_driver);
-}
-module_exit(davinci_vc_exit);
+module_platform_driver_probe(davinci_vc_driver, davinci_vc_probe);
 
 MODULE_AUTHOR("Miguel Aguilar");
 MODULE_DESCRIPTION("Texas Instruments DaVinci Voice Codec Core Interface");
diff --git a/drivers/mfd/db8500-prcmu.c b/drivers/mfd/db8500-prcmu.c
index 21f261bf9e95..319b8abe742b 100644
--- a/drivers/mfd/db8500-prcmu.c
+++ b/drivers/mfd/db8500-prcmu.c
@@ -24,9 +24,9 @@
 #include <linux/jiffies.h>
 #include <linux/bitops.h>
 #include <linux/fs.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/uaccess.h>
-#include <linux/irqchip/arm-gic.h>
 #include <linux/mfd/core.h>
 #include <linux/mfd/dbx500-prcmu.h>
 #include <linux/mfd/abx500/ab8500.h>
@@ -34,9 +34,7 @@
 #include <linux/regulator/machine.h>
 #include <linux/cpufreq.h>
 #include <linux/platform_data/ux500_wdt.h>
-#include <mach/hardware.h>
-#include <mach/irqs.h>
-#include <mach/db8500-regs.h>
+#include <linux/platform_data/db8500_thermal.h>
 #include "dbx500-prcmu-regs.h"
 
 /* Index of different voltages to be used when accessing AVSData */
@@ -276,8 +274,34 @@ static struct irq_domain *db8500_irq_domain;
  * the bits in the bit field are not. (The bits also have a tendency to move
  * around, to further complicate matters.)
  */
-#define IRQ_INDEX(_name) ((IRQ_PRCMU_##_name) - IRQ_PRCMU_BASE)
+#define IRQ_INDEX(_name) ((IRQ_PRCMU_##_name))
 #define IRQ_ENTRY(_name)[IRQ_INDEX(_name)] = (WAKEUP_BIT_##_name)
+
+#define IRQ_PRCMU_RTC 0
+#define IRQ_PRCMU_RTT0 1
+#define IRQ_PRCMU_RTT1 2
+#define IRQ_PRCMU_HSI0 3
+#define IRQ_PRCMU_HSI1 4
+#define IRQ_PRCMU_CA_WAKE 5
+#define IRQ_PRCMU_USB 6
+#define IRQ_PRCMU_ABB 7
+#define IRQ_PRCMU_ABB_FIFO 8
+#define IRQ_PRCMU_ARM 9
+#define IRQ_PRCMU_MODEM_SW_RESET_REQ 10
+#define IRQ_PRCMU_GPIO0 11
+#define IRQ_PRCMU_GPIO1 12
+#define IRQ_PRCMU_GPIO2 13
+#define IRQ_PRCMU_GPIO3 14
+#define IRQ_PRCMU_GPIO4 15
+#define IRQ_PRCMU_GPIO5 16
+#define IRQ_PRCMU_GPIO6 17
+#define IRQ_PRCMU_GPIO7 18
+#define IRQ_PRCMU_GPIO8 19
+#define IRQ_PRCMU_CA_SLEEP 20
+#define IRQ_PRCMU_HOTMON_LOW 21
+#define IRQ_PRCMU_HOTMON_HIGH 22
+#define NUM_PRCMU_WAKEUPS 23
+
 static u32 prcmu_irq_bit[NUM_PRCMU_WAKEUPS] = {
 	IRQ_ENTRY(RTC),
 	IRQ_ENTRY(RTT0),
@@ -422,9 +446,10 @@ static DEFINE_SPINLOCK(clkout_lock);
 
 /* Global var to runtime determine TCDM base for v2 or v1 */
 static __iomem void *tcdm_base;
+static __iomem void *prcmu_base;
 
 struct clk_mgt {
-	void __iomem *reg;
+	u32 offset;
 	u32 pllsw;
 	int branch;
 	bool clk38div;
@@ -599,9 +624,9 @@ int db8500_prcmu_set_display_clocks(void)
 	while ((readl(PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
 		cpu_relax();
 
-	writel(PRCMU_DSI_CLOCK_SETTING, PRCM_HDMICLK_MGT);
-	writel(PRCMU_DSI_LP_CLOCK_SETTING, PRCM_TVCLK_MGT);
-	writel(PRCMU_DPI_CLOCK_SETTING, PRCM_LCDCLK_MGT);
+	writel(PRCMU_DSI_CLOCK_SETTING, prcmu_base + PRCM_HDMICLK_MGT);
+	writel(PRCMU_DSI_LP_CLOCK_SETTING, prcmu_base + PRCM_TVCLK_MGT);
+	writel(PRCMU_DPI_CLOCK_SETTING, prcmu_base + PRCM_LCDCLK_MGT);
 
 	/* Release the HW semaphore. */
 	writel(0, PRCM_SEM);
@@ -613,7 +638,7 @@ int db8500_prcmu_set_display_clocks(void)
 
 u32 db8500_prcmu_read(unsigned int reg)
 {
-	return readl(_PRCMU_BASE + reg);
+	return readl(prcmu_base + reg);
 }
 
 void db8500_prcmu_write(unsigned int reg, u32 value)
@@ -621,7 +646,7 @@ void db8500_prcmu_write(unsigned int reg, u32 value)
 	unsigned long flags;
 
 	spin_lock_irqsave(&prcmu_lock, flags);
-	writel(value, (_PRCMU_BASE + reg));
+	writel(value, (prcmu_base + reg));
 	spin_unlock_irqrestore(&prcmu_lock, flags);
 }
 
@@ -631,9 +656,9 @@ void db8500_prcmu_write_masked(unsigned int reg, u32 mask, u32 value)
 	unsigned long flags;
 
 	spin_lock_irqsave(&prcmu_lock, flags);
-	val = readl(_PRCMU_BASE + reg);
+	val = readl(prcmu_base + reg);
 	val = ((val & ~mask) | (value & mask));
-	writel(val, (_PRCMU_BASE + reg));
+	writel(val, (prcmu_base + reg));
 	spin_unlock_irqrestore(&prcmu_lock, flags);
 }
 
@@ -793,119 +818,6 @@ u8 db8500_prcmu_get_power_state_result(void)
 	return readb(tcdm_base + PRCM_ACK_MB0_AP_PWRSTTR_STATUS);
 }
 
-/* This function decouple the gic from the prcmu */
-int db8500_prcmu_gic_decouple(void)
-{
-	u32 val = readl(PRCM_A9_MASK_REQ);
-
-	/* Set bit 0 register value to 1 */
-	writel(val | PRCM_A9_MASK_REQ_PRCM_A9_MASK_REQ,
-	       PRCM_A9_MASK_REQ);
-
-	/* Make sure the register is updated */
-	readl(PRCM_A9_MASK_REQ);
-
-	/* Wait a few cycles for the gic mask completion */
-	udelay(1);
-
-	return 0;
-}
-
-/* This function recouple the gic with the prcmu */
-int db8500_prcmu_gic_recouple(void)
-{
-	u32 val = readl(PRCM_A9_MASK_REQ);
-
-	/* Set bit 0 register value to 0 */
-	writel(val & ~PRCM_A9_MASK_REQ_PRCM_A9_MASK_REQ, PRCM_A9_MASK_REQ);
-
-	return 0;
-}
-
-#define PRCMU_GIC_NUMBER_REGS 5
-
-/*
- * This function checks if there are pending irq on the gic. It only
- * makes sense if the gic has been decoupled before with the
- * db8500_prcmu_gic_decouple function. Disabling an interrupt only
- * disables the forwarding of the interrupt to any CPU interface. It
- * does not prevent the interrupt from changing state, for example
- * becoming pending, or active and pending if it is already
- * active. Hence, we have to check the interrupt is pending *and* is
- * active.
- */
-bool db8500_prcmu_gic_pending_irq(void)
-{
-	u32 pr; /* Pending register */
-	u32 er; /* Enable register */
-	void __iomem *dist_base = __io_address(U8500_GIC_DIST_BASE);
-	int i;
-
-        /* 5 registers. STI & PPI not skipped */
-	for (i = 0; i < PRCMU_GIC_NUMBER_REGS; i++) {
-
-		pr = readl_relaxed(dist_base + GIC_DIST_PENDING_SET + i * 4);
-		er = readl_relaxed(dist_base + GIC_DIST_ENABLE_SET + i * 4);
-
-		if (pr & er)
-			return true; /* There is a pending interrupt */
-	}
-
-	return false;
-}
-
-/*
- * This function checks if there are pending interrupt on the
- * prcmu which has been delegated to monitor the irqs with the
- * db8500_prcmu_copy_gic_settings function.
- */
-bool db8500_prcmu_pending_irq(void)
-{
-	u32 it, im;
-	int i;
-
-	for (i = 0; i < PRCMU_GIC_NUMBER_REGS - 1; i++) {
-		it = readl(PRCM_ARMITVAL31TO0 + i * 4);
-		im = readl(PRCM_ARMITMSK31TO0 + i * 4);
-		if (it & im)
-			return true; /* There is a pending interrupt */
-	}
-
-	return false;
-}
-
-/*
- * This function checks if the specified cpu is in in WFI. It's usage
- * makes sense only if the gic is decoupled with the db8500_prcmu_gic_decouple
- * function. Of course passing smp_processor_id() to this function will
- * always return false...
- */
-bool db8500_prcmu_is_cpu_in_wfi(int cpu)
-{
-	return readl(PRCM_ARM_WFI_STANDBY) & cpu ? PRCM_ARM_WFI_STANDBY_WFI1 :
-		     PRCM_ARM_WFI_STANDBY_WFI0;
-}
-
-/*
- * This function copies the gic SPI settings to the prcmu in order to
- * monitor them and abort/finish the retention/off sequence or state.
- */
-int db8500_prcmu_copy_gic_settings(void)
-{
-	u32 er; /* Enable register */
-	void __iomem *dist_base = __io_address(U8500_GIC_DIST_BASE);
-	int i;
-
-        /* We skip the STI and PPI */
-	for (i = 0; i < PRCMU_GIC_NUMBER_REGS - 1; i++) {
-		er = readl_relaxed(dist_base +
-				   GIC_DIST_ENABLE_SET + (i + 1) * 4);
-		writel(er, PRCM_ARMITMSK31TO0 + i * 4);
-	}
-
-	return 0;
-}
-
 /* This function should only be called while mb0_transfer.lock is held. */
 static void config_wakeups(void)
 {
@@ -1059,7 +971,7 @@ int db8500_prcmu_set_ddr_opp(u8 opp)
 /* Divide the frequency of certain clocks by 2 for APE_50_PARTLY_25_OPP. */
 static void request_even_slower_clocks(bool enable)
 {
-	void __iomem *clock_reg[] = {
+	u32 clock_reg[] = {
 		PRCM_ACLK_MGT,
 		PRCM_DMACLK_MGT
 	};
@@ -1076,7 +988,7 @@ static void request_even_slower_clocks(bool enable)
 		u32 val;
 		u32 div;
 
-		val = readl(clock_reg[i]);
+		val = readl(prcmu_base + clock_reg[i]);
 		div = (val & PRCM_CLK_MGT_CLKPLLDIV_MASK);
 		if (enable) {
 			if ((div <= 1) || (div > 15)) {
@@ -1092,7 +1004,7 @@ static void request_even_slower_clocks(bool enable)
 		}
 		val = ((val & ~PRCM_CLK_MGT_CLKPLLDIV_MASK) |
 			(div & PRCM_CLK_MGT_CLKPLLDIV_MASK));
-		writel(val, clock_reg[i]);
+		writel(val, prcmu_base + clock_reg[i]);
 	}
 
 unlock_and_return:
@@ -1446,14 +1358,14 @@ static int request_clock(u8 clock, bool enable)
 	while ((readl(PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
 		cpu_relax();
 
-	val = readl(clk_mgt[clock].reg);
+	val = readl(prcmu_base + clk_mgt[clock].offset);
 	if (enable) {
 		val |= (PRCM_CLK_MGT_CLKEN | clk_mgt[clock].pllsw);
 	} else {
 		clk_mgt[clock].pllsw = (val & PRCM_CLK_MGT_CLKPLLSW_MASK);
 		val &= ~(PRCM_CLK_MGT_CLKEN | PRCM_CLK_MGT_CLKPLLSW_MASK);
 	}
-	writel(val, clk_mgt[clock].reg);
+	writel(val, prcmu_base + clk_mgt[clock].offset);
 
 	/* Release the HW semaphore. */
 	writel(0, PRCM_SEM);
@@ -1629,7 +1541,7 @@ static unsigned long clock_rate(u8 clock)
 	u32 pllsw;
 	unsigned long rate = ROOT_CLOCK_RATE;
 
-	val = readl(clk_mgt[clock].reg);
+	val = readl(prcmu_base + clk_mgt[clock].offset);
 
 	if (val & PRCM_CLK_MGT_CLK38) {
 		if (clk_mgt[clock].clk38div && (val & PRCM_CLK_MGT_CLK38DIV))
@@ -1785,7 +1697,7 @@ static long round_clock_rate(u8 clock, unsigned long rate)
 	unsigned long src_rate;
 	long rounded_rate;
 
-	val = readl(clk_mgt[clock].reg);
+	val = readl(prcmu_base + clk_mgt[clock].offset);
 	src_rate = clock_source_rate((val | clk_mgt[clock].pllsw),
 		clk_mgt[clock].branch);
 	div = clock_divider(src_rate, rate);
@@ -1933,7 +1845,7 @@ static void set_clock_rate(u8 clock, unsigned long rate)
 	while ((readl(PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
 		cpu_relax();
 
-	val = readl(clk_mgt[clock].reg);
+	val = readl(prcmu_base + clk_mgt[clock].offset);
 	src_rate = clock_source_rate((val | clk_mgt[clock].pllsw),
 		clk_mgt[clock].branch);
 	div = clock_divider(src_rate, rate);
@@ -1961,7 +1873,7 @@ static void set_clock_rate(u8 clock, unsigned long rate)
 		val &= ~PRCM_CLK_MGT_CLKPLLDIV_MASK;
 		val |= min(div, (u32)31);
 	}
-	writel(val, clk_mgt[clock].reg);
+	writel(val, prcmu_base + clk_mgt[clock].offset);
 
 	/* Release the HW semaphore. */
 	writel(0, PRCM_SEM);
@@ -2764,14 +2676,13 @@ static struct irq_domain_ops db8500_irq_ops = {
 	.xlate  = irq_domain_xlate_twocell,
 };
 
-static int db8500_irq_init(struct device_node *np)
+static int db8500_irq_init(struct device_node *np, int irq_base)
 {
-	int irq_base = 0;
 	int i;
 
 	/* In the device tree case, just take some IRQs */
-	if (!np)
-		irq_base = IRQ_PRCMU_BASE;
+	if (np)
+		irq_base = 0;
 
 	db8500_irq_domain = irq_domain_add_simple(
 		np, NUM_PRCMU_WAKEUPS, irq_base,
@@ -2794,6 +2705,7 @@ static void dbx500_fw_version_init(struct platform_device *pdev,
 {
 	struct resource *res;
 	void __iomem *tcpm_base;
+	u32 version;
 
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
 					   "prcmu-tcpm");
@@ -2803,30 +2715,42 @@ static void dbx500_fw_version_init(struct platform_device *pdev,
 		return;
 	}
 	tcpm_base = ioremap(res->start, resource_size(res));
-	if (tcpm_base != NULL) {
-		u32 version;
-
-		version = readl(tcpm_base + version_offset);
-		fw_info.version.project = (version & 0xFF);
-		fw_info.version.api_version = (version >> 8) & 0xFF;
-		fw_info.version.func_version = (version >> 16) & 0xFF;
-		fw_info.version.errata = (version >> 24) & 0xFF;
-		strncpy(fw_info.version.project_name,
-			fw_project_name(fw_info.version.project),
-			PRCMU_FW_PROJECT_NAME_LEN);
-		fw_info.valid = true;
-		pr_info("PRCMU firmware: %s(%d), version %d.%d.%d\n",
-			fw_info.version.project_name,
-			fw_info.version.project,
-			fw_info.version.api_version,
-			fw_info.version.func_version,
-			fw_info.version.errata);
-		iounmap(tcpm_base);
+	if (!tcpm_base) {
+		dev_err(&pdev->dev, "no prcmu tcpm mem region provided\n");
+		return;
 	}
-}
 
-void __init db8500_prcmu_early_init(void)
+	version = readl(tcpm_base + version_offset);
+	fw_info.version.project = (version & 0xFF);
+	fw_info.version.api_version = (version >> 8) & 0xFF;
+	fw_info.version.func_version = (version >> 16) & 0xFF;
+	fw_info.version.errata = (version >> 24) & 0xFF;
+	strncpy(fw_info.version.project_name,
+		fw_project_name(fw_info.version.project),
+		PRCMU_FW_PROJECT_NAME_LEN);
+	fw_info.valid = true;
+	pr_info("PRCMU firmware: %s(%d), version %d.%d.%d\n",
+		fw_info.version.project_name,
+		fw_info.version.project,
+		fw_info.version.api_version,
+		fw_info.version.func_version,
+		fw_info.version.errata);
+	iounmap(tcpm_base);
+}
+
+void __init db8500_prcmu_early_init(u32 phy_base, u32 size)
 {
+	/*
+	 * This is a temporary remap to bring up the clocks. It is
+	 * subsequently replaces with a real remap. After the merge of
+	 * the mailbox subsystem all of this early code goes away, and the
+	 * clock driver can probe independently. An early initcall will
+	 * still be needed, but it can be diverted into drivers/clk/ux500.
+	 */
+	prcmu_base = ioremap(phy_base, size);
+	if (!prcmu_base)
+		pr_err("%s: ioremap() of prcmu registers failed!\n", __func__);
+
 	spin_lock_init(&mb0_transfer.lock);
 	spin_lock_init(&mb0_transfer.dbb_irqs_lock);
 	mutex_init(&mb0_transfer.ac_wake_lock);
@@ -3092,18 +3016,66 @@ static struct regulator_init_data db8500_regulators[DB8500_NUM_REGULATORS] = {
 	},
 };
 
-static struct resource ab8500_resources[] = {
-	[0] = {
-		.start	= IRQ_DB8500_AB8500,
-		.end	= IRQ_DB8500_AB8500,
-		.flags	= IORESOURCE_IRQ
-	}
-};
-
 static struct ux500_wdt_data db8500_wdt_pdata = {
 	.timeout = 600, /* 10 minutes */
 	.has_28_bits_resolution = true,
 };
+/*
+ * Thermal Sensor
+ */
+
+static struct resource db8500_thsens_resources[] = {
+	{
+		.name = "IRQ_HOTMON_LOW",
+		.start  = IRQ_PRCMU_HOTMON_LOW,
+		.end    = IRQ_PRCMU_HOTMON_LOW,
+		.flags  = IORESOURCE_IRQ,
+	},
+	{
+		.name = "IRQ_HOTMON_HIGH",
+		.start  = IRQ_PRCMU_HOTMON_HIGH,
+		.end    = IRQ_PRCMU_HOTMON_HIGH,
+		.flags  = IORESOURCE_IRQ,
+	},
+};
+
+static struct db8500_thsens_platform_data db8500_thsens_data = {
+	.trip_points[0] = {
+		.temp = 70000,
+		.type = THERMAL_TRIP_ACTIVE,
+		.cdev_name = {
+			[0] = "thermal-cpufreq-0",
+		},
+	},
+	.trip_points[1] = {
+		.temp = 75000,
+		.type = THERMAL_TRIP_ACTIVE,
+		.cdev_name = {
+			[0] = "thermal-cpufreq-0",
+		},
+	},
+	.trip_points[2] = {
+		.temp = 80000,
+		.type = THERMAL_TRIP_ACTIVE,
+		.cdev_name = {
+			[0] = "thermal-cpufreq-0",
+		},
+	},
+	.trip_points[3] = {
+		.temp = 85000,
+		.type = THERMAL_TRIP_CRITICAL,
+	},
+	.num_trips = 4,
+};
+
+static struct mfd_cell common_prcmu_devs[] = {
+	{
+		.name = "ux500_wdt",
+		.platform_data = &db8500_wdt_pdata,
+		.pdata_size = sizeof(db8500_wdt_pdata),
+		.id = -1,
+	},
+};
 
 static struct mfd_cell db8500_prcmu_devs[] = {
 	{
@@ -3119,17 +3091,10 @@ static struct mfd_cell db8500_prcmu_devs[] = {
 		.pdata_size = sizeof(db8500_cpufreq_table),
 	},
 	{
-		.name = "ux500_wdt",
-		.platform_data = &db8500_wdt_pdata,
-		.pdata_size = sizeof(db8500_wdt_pdata),
-		.id = -1,
-	},
-	{
-		.name = "ab8500-core",
-		.of_compatible = "stericsson,ab8500",
-		.num_resources = ARRAY_SIZE(ab8500_resources),
-		.resources = ab8500_resources,
-		.id = AB8500_VERSION_AB8500,
+		.name = "db8500-thermal",
+		.num_resources = ARRAY_SIZE(db8500_thsens_resources),
+		.resources = db8500_thsens_resources,
+		.platform_data = &db8500_thsens_data,
 	},
 };
 
@@ -3141,6 +3106,24 @@ static void db8500_prcmu_update_cpufreq(void)
 	}
 }
 
+static int db8500_prcmu_register_ab8500(struct device *parent,
+					struct ab8500_platform_data *pdata,
+					int irq)
+{
+	struct resource ab8500_resource = DEFINE_RES_IRQ(irq);
+	struct mfd_cell ab8500_cell = {
+		.name = "ab8500-core",
+		.of_compatible = "stericsson,ab8500",
+		.id = AB8500_VERSION_AB8500,
+		.platform_data = pdata,
+		.pdata_size = sizeof(struct ab8500_platform_data),
+		.resources = &ab8500_resource,
+		.num_resources = 1,
+	};
+
+	return mfd_add_devices(parent, 0, &ab8500_cell, 1, NULL, 0, NULL);
+}
+
 /**
  * prcmu_fw_init - arch init call for the Linux PRCMU fw init logic
  *
@@ -3149,11 +3132,21 @@ static int db8500_prcmu_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
 	struct prcmu_pdata *pdata = dev_get_platdata(&pdev->dev);
-	int irq = 0, err = 0, i;
+	int irq = 0, err = 0;
 	struct resource *res;
 
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "prcmu");
+	if (!res) {
+		dev_err(&pdev->dev, "no prcmu memory region provided\n");
+		return -ENOENT;
+	}
+	prcmu_base = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	if (!prcmu_base) {
+		dev_err(&pdev->dev,
+			"failed to ioremap prcmu register memory\n");
+		return -ENOENT;
+	}
 	init_prcm_registers();
-
 	dbx500_fw_version_init(pdev, pdata->version_offset);
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "prcmu-tcdm");
 	if (!res) {
@@ -3180,26 +3173,39 @@ static int db8500_prcmu_probe(struct platform_device *pdev)
 		goto no_irq_return;
 	}
 
-	db8500_irq_init(np);
-
-	for (i = 0; i < ARRAY_SIZE(db8500_prcmu_devs); i++) {
-		if (!strcmp(db8500_prcmu_devs[i].name, "ab8500-core")) {
-			db8500_prcmu_devs[i].platform_data = pdata->ab_platdata;
-			db8500_prcmu_devs[i].pdata_size = sizeof(struct ab8500_platform_data);
-		}
-	}
+	db8500_irq_init(np, pdata->irq_base);
 
 	prcmu_config_esram0_deep_sleep(ESRAM0_DEEP_SLEEP_STATE_RET);
 
 	db8500_prcmu_update_cpufreq();
 
-	err = mfd_add_devices(&pdev->dev, 0, db8500_prcmu_devs,
-			      ARRAY_SIZE(db8500_prcmu_devs), NULL, 0, NULL);
+	err = mfd_add_devices(&pdev->dev, 0, common_prcmu_devs,
+			      ARRAY_SIZE(common_prcmu_devs), NULL, 0, db8500_irq_domain);
 	if (err) {
 		pr_err("prcmu: Failed to add subdevices\n");
 		return err;
 	}
 
+	/* TODO: Remove restriction when clk definitions are available. */
+	if (!of_machine_is_compatible("st-ericsson,u8540")) {
+		err = mfd_add_devices(&pdev->dev, 0, db8500_prcmu_devs,
+				      ARRAY_SIZE(db8500_prcmu_devs), NULL, 0,
+				      db8500_irq_domain);
+		if (err) {
+			mfd_remove_devices(&pdev->dev);
+			pr_err("prcmu: Failed to add subdevices\n");
+			goto no_irq_return;
+		}
+	}
+
+	err = db8500_prcmu_register_ab8500(&pdev->dev, pdata->ab_platdata,
+					   pdata->ab_irq);
+	if (err) {
+		mfd_remove_devices(&pdev->dev);
+		pr_err("prcmu: Failed to add ab8500 subdevice\n");
+		goto no_irq_return;
+	}
+
 	pr_info("DB8500 PRCMU initialized\n");
 
 no_irq_return:
diff --git a/drivers/mfd/dbx500-prcmu-regs.h b/drivers/mfd/dbx500-prcmu-regs.h
index 79c76ebdba52..d14836ed2114 100644
--- a/drivers/mfd/dbx500-prcmu-regs.h
+++ b/drivers/mfd/dbx500-prcmu-regs.h
@@ -13,136 +13,110 @@
 #ifndef __DB8500_PRCMU_REGS_H
 #define __DB8500_PRCMU_REGS_H
 
-#include <mach/hardware.h>
-
 #define BITS(_start, _end) ((BIT(_end) - BIT(_start)) + BIT(_end))
 
-#define PRCM_CLK_MGT(_offset) (void __iomem *)(IO_ADDRESS(U8500_PRCMU_BASE) \
-	+ _offset)
-#define PRCM_ACLK_MGT		PRCM_CLK_MGT(0x004)
-#define PRCM_SVACLK_MGT		PRCM_CLK_MGT(0x008)
-#define PRCM_SIACLK_MGT		PRCM_CLK_MGT(0x00C)
-#define PRCM_SGACLK_MGT		PRCM_CLK_MGT(0x014)
-#define PRCM_UARTCLK_MGT	PRCM_CLK_MGT(0x018)
-#define PRCM_MSP02CLK_MGT	PRCM_CLK_MGT(0x01C)
-#define PRCM_I2CCLK_MGT		PRCM_CLK_MGT(0x020)
-#define PRCM_SDMMCCLK_MGT	PRCM_CLK_MGT(0x024)
-#define PRCM_SLIMCLK_MGT	PRCM_CLK_MGT(0x028)
-#define PRCM_PER1CLK_MGT	PRCM_CLK_MGT(0x02C)
-#define PRCM_PER2CLK_MGT	PRCM_CLK_MGT(0x030)
-#define PRCM_PER3CLK_MGT	PRCM_CLK_MGT(0x034)
-#define PRCM_PER5CLK_MGT	PRCM_CLK_MGT(0x038)
-#define PRCM_PER6CLK_MGT	PRCM_CLK_MGT(0x03C)
-#define PRCM_PER7CLK_MGT	PRCM_CLK_MGT(0x040)
-#define PRCM_LCDCLK_MGT		PRCM_CLK_MGT(0x044)
-#define PRCM_BMLCLK_MGT		PRCM_CLK_MGT(0x04C)
-#define PRCM_HSITXCLK_MGT	PRCM_CLK_MGT(0x050)
-#define PRCM_HSIRXCLK_MGT	PRCM_CLK_MGT(0x054)
-#define PRCM_HDMICLK_MGT	PRCM_CLK_MGT(0x058)
-#define PRCM_APEATCLK_MGT	PRCM_CLK_MGT(0x05C)
-#define PRCM_APETRACECLK_MGT	PRCM_CLK_MGT(0x060)
-#define PRCM_MCDECLK_MGT	PRCM_CLK_MGT(0x064)
-#define PRCM_IPI2CCLK_MGT	PRCM_CLK_MGT(0x068)
-#define PRCM_DSIALTCLK_MGT	PRCM_CLK_MGT(0x06C)
-#define PRCM_DMACLK_MGT		PRCM_CLK_MGT(0x074)
-#define PRCM_B2R2CLK_MGT	PRCM_CLK_MGT(0x078)
-#define PRCM_TVCLK_MGT		PRCM_CLK_MGT(0x07C)
-#define PRCM_UNIPROCLK_MGT	PRCM_CLK_MGT(0x278)
-#define PRCM_SSPCLK_MGT		PRCM_CLK_MGT(0x280)
-#define PRCM_RNGCLK_MGT		PRCM_CLK_MGT(0x284)
-#define PRCM_UICCCLK_MGT	PRCM_CLK_MGT(0x27C)
-#define PRCM_MSP1CLK_MGT	PRCM_CLK_MGT(0x288)
-
-#define PRCM_ARM_PLLDIVPS	(_PRCMU_BASE + 0x118)
+#define PRCM_ACLK_MGT		(0x004)
+#define PRCM_SVACLK_MGT		(0x008)
+#define PRCM_SIACLK_MGT		(0x00C)
+#define PRCM_SGACLK_MGT		(0x014)
+#define PRCM_UARTCLK_MGT	(0x018)
+#define PRCM_MSP02CLK_MGT	(0x01C)
+#define PRCM_I2CCLK_MGT		(0x020)
+#define PRCM_SDMMCCLK_MGT	(0x024)
+#define PRCM_SLIMCLK_MGT	(0x028)
+#define PRCM_PER1CLK_MGT	(0x02C)
+#define PRCM_PER2CLK_MGT	(0x030)
+#define PRCM_PER3CLK_MGT	(0x034)
+#define PRCM_PER5CLK_MGT	(0x038)
+#define PRCM_PER6CLK_MGT	(0x03C)
+#define PRCM_PER7CLK_MGT	(0x040)
+#define PRCM_LCDCLK_MGT		(0x044)
+#define PRCM_BMLCLK_MGT		(0x04C)
+#define PRCM_HSITXCLK_MGT	(0x050)
+#define PRCM_HSIRXCLK_MGT	(0x054)
+#define PRCM_HDMICLK_MGT	(0x058)
+#define PRCM_APEATCLK_MGT	(0x05C)
+#define PRCM_APETRACECLK_MGT	(0x060)
+#define PRCM_MCDECLK_MGT	(0x064)
+#define PRCM_IPI2CCLK_MGT	(0x068)
+#define PRCM_DSIALTCLK_MGT	(0x06C)
+#define PRCM_DMACLK_MGT		(0x074)
+#define PRCM_B2R2CLK_MGT	(0x078)
+#define PRCM_TVCLK_MGT		(0x07C)
+#define PRCM_UNIPROCLK_MGT	(0x278)
+#define PRCM_SSPCLK_MGT		(0x280)
+#define PRCM_RNGCLK_MGT		(0x284)
+#define PRCM_UICCCLK_MGT	(0x27C)
+#define PRCM_MSP1CLK_MGT	(0x288)
+
+#define PRCM_ARM_PLLDIVPS	(prcmu_base + 0x118)
 #define PRCM_ARM_PLLDIVPS_ARM_BRM_RATE		0x3f
 #define PRCM_ARM_PLLDIVPS_MAX_MASK		0xf
 
-#define PRCM_PLLARM_LOCKP       (_PRCMU_BASE + 0x0a8)
+#define PRCM_PLLARM_LOCKP       (prcmu_base + 0x0a8)
 #define PRCM_PLLARM_LOCKP_PRCM_PLLARM_LOCKP3	0x2
 
-#define PRCM_ARM_CHGCLKREQ	(_PRCMU_BASE + 0x114)
+#define PRCM_ARM_CHGCLKREQ	(prcmu_base + 0x114)
 #define PRCM_ARM_CHGCLKREQ_PRCM_ARM_CHGCLKREQ	BIT(0)
 #define PRCM_ARM_CHGCLKREQ_PRCM_ARM_DIVSEL	BIT(16)
 
-#define PRCM_PLLARM_ENABLE	(_PRCMU_BASE + 0x98)
+#define PRCM_PLLARM_ENABLE	(prcmu_base + 0x98)
 #define PRCM_PLLARM_ENABLE_PRCM_PLLARM_ENABLE	0x1
 #define PRCM_PLLARM_ENABLE_PRCM_PLLARM_COUNTON	0x100
 
-#define PRCM_ARMCLKFIX_MGT	(_PRCMU_BASE + 0x0)
-#define PRCM_A9PL_FORCE_CLKEN	(_PRCMU_BASE + 0x19C)
-#define PRCM_A9_RESETN_CLR	(_PRCMU_BASE + 0x1f4)
-#define PRCM_A9_RESETN_SET	(_PRCMU_BASE + 0x1f0)
-#define PRCM_ARM_LS_CLAMP	(_PRCMU_BASE + 0x30c)
-#define PRCM_SRAM_A9		(_PRCMU_BASE + 0x308)
+#define PRCM_ARMCLKFIX_MGT	(prcmu_base + 0x0)
+#define PRCM_A9PL_FORCE_CLKEN	(prcmu_base + 0x19C)
+#define PRCM_A9_RESETN_CLR	(prcmu_base + 0x1f4)
+#define PRCM_A9_RESETN_SET	(prcmu_base + 0x1f0)
+#define PRCM_ARM_LS_CLAMP	(prcmu_base + 0x30c)
+#define PRCM_SRAM_A9		(prcmu_base + 0x308)
 
 #define PRCM_A9PL_FORCE_CLKEN_PRCM_A9PL_FORCE_CLKEN BIT(0)
 #define PRCM_A9PL_FORCE_CLKEN_PRCM_A9AXI_FORCE_CLKEN BIT(1)
 
-/* ARM WFI Standby signal register */
-#define PRCM_ARM_WFI_STANDBY    (_PRCMU_BASE + 0x130)
-#define PRCM_ARM_WFI_STANDBY_WFI0               0x08
-#define PRCM_ARM_WFI_STANDBY_WFI1               0x10
-#define PRCM_IOCR		(_PRCMU_BASE + 0x310)
-#define PRCM_IOCR_IOFORCE			0x1
-
 /* CPU mailbox registers */
-#define PRCM_MBOX_CPU_VAL	(_PRCMU_BASE + 0x0fc)
-#define PRCM_MBOX_CPU_SET	(_PRCMU_BASE + 0x100)
-#define PRCM_MBOX_CPU_CLR	(_PRCMU_BASE + 0x104)
-
-/* Dual A9 core interrupt management unit registers */
-#define PRCM_A9_MASK_REQ	(_PRCMU_BASE + 0x328)
-#define PRCM_A9_MASK_REQ_PRCM_A9_MASK_REQ	0x1
-
-#define PRCM_A9_MASK_ACK	(_PRCMU_BASE + 0x32c)
-#define PRCM_ARMITMSK31TO0	(_PRCMU_BASE + 0x11c)
-#define PRCM_ARMITMSK63TO32	(_PRCMU_BASE + 0x120)
-#define PRCM_ARMITMSK95TO64	(_PRCMU_BASE + 0x124)
-#define PRCM_ARMITMSK127TO96	(_PRCMU_BASE + 0x128)
-#define PRCM_POWER_STATE_VAL	(_PRCMU_BASE + 0x25C)
-#define PRCM_ARMITVAL31TO0	(_PRCMU_BASE + 0x260)
-#define PRCM_ARMITVAL63TO32	(_PRCMU_BASE + 0x264)
-#define PRCM_ARMITVAL95TO64	(_PRCMU_BASE + 0x268)
-#define PRCM_ARMITVAL127TO96	(_PRCMU_BASE + 0x26C)
-
-#define PRCM_HOSTACCESS_REQ	(_PRCMU_BASE + 0x334)
+#define PRCM_MBOX_CPU_VAL	(prcmu_base + 0x0fc)
+#define PRCM_MBOX_CPU_SET	(prcmu_base + 0x100)
+#define PRCM_MBOX_CPU_CLR	(prcmu_base + 0x104)
+
+#define PRCM_HOSTACCESS_REQ	(prcmu_base + 0x334)
 #define PRCM_HOSTACCESS_REQ_HOSTACCESS_REQ 0x1
 #define PRCM_HOSTACCESS_REQ_WAKE_REQ	BIT(16)
 #define ARM_WAKEUP_MODEM	0x1
 
-#define PRCM_ARM_IT1_CLR	(_PRCMU_BASE + 0x48C)
-#define PRCM_ARM_IT1_VAL	(_PRCMU_BASE + 0x494)
-#define PRCM_HOLD_EVT		(_PRCMU_BASE + 0x174)
+#define PRCM_ARM_IT1_CLR	(prcmu_base + 0x48C)
+#define PRCM_ARM_IT1_VAL	(prcmu_base + 0x494)
+#define PRCM_HOLD_EVT		(prcmu_base + 0x174)
 
-#define PRCM_MOD_AWAKE_STATUS	(_PRCMU_BASE + 0x4A0)
+#define PRCM_MOD_AWAKE_STATUS	(prcmu_base + 0x4A0)
 #define PRCM_MOD_AWAKE_STATUS_PRCM_MOD_COREPD_AWAKE	BIT(0)
 #define PRCM_MOD_AWAKE_STATUS_PRCM_MOD_AAPD_AWAKE	BIT(1)
 #define PRCM_MOD_AWAKE_STATUS_PRCM_MOD_VMODEM_OFF_ISO	BIT(2)
 
-#define PRCM_ITSTATUS0		(_PRCMU_BASE + 0x148)
-#define PRCM_ITSTATUS1		(_PRCMU_BASE + 0x150)
-#define PRCM_ITSTATUS2		(_PRCMU_BASE + 0x158)
-#define PRCM_ITSTATUS3		(_PRCMU_BASE + 0x160)
-#define PRCM_ITSTATUS4		(_PRCMU_BASE + 0x168)
-#define PRCM_ITSTATUS5		(_PRCMU_BASE + 0x484)
-#define PRCM_ITCLEAR5		(_PRCMU_BASE + 0x488)
-#define PRCM_ARMIT_MASKXP70_IT	(_PRCMU_BASE + 0x1018)
+#define PRCM_ITSTATUS0		(prcmu_base + 0x148)
+#define PRCM_ITSTATUS1		(prcmu_base + 0x150)
+#define PRCM_ITSTATUS2		(prcmu_base + 0x158)
+#define PRCM_ITSTATUS3		(prcmu_base + 0x160)
+#define PRCM_ITSTATUS4		(prcmu_base + 0x168)
+#define PRCM_ITSTATUS5		(prcmu_base + 0x484)
+#define PRCM_ITCLEAR5		(prcmu_base + 0x488)
+#define PRCM_ARMIT_MASKXP70_IT	(prcmu_base + 0x1018)
 
 /* System reset register */
-#define PRCM_APE_SOFTRST	(_PRCMU_BASE + 0x228)
+#define PRCM_APE_SOFTRST	(prcmu_base + 0x228)
 
 /* Level shifter and clamp control registers */
-#define PRCM_MMIP_LS_CLAMP_SET     (_PRCMU_BASE + 0x420)
-#define PRCM_MMIP_LS_CLAMP_CLR     (_PRCMU_BASE + 0x424)
+#define PRCM_MMIP_LS_CLAMP_SET     (prcmu_base + 0x420)
+#define PRCM_MMIP_LS_CLAMP_CLR     (prcmu_base + 0x424)
 
 #define PRCM_MMIP_LS_CLAMP_DSIPLL_CLAMP		BIT(11)
 #define PRCM_MMIP_LS_CLAMP_DSIPLL_CLAMPI	BIT(22)
 
 /* PRCMU clock/PLL/reset registers */
-#define PRCM_PLLSOC0_FREQ	   (_PRCMU_BASE + 0x080)
-#define PRCM_PLLSOC1_FREQ	   (_PRCMU_BASE + 0x084)
-#define PRCM_PLLARM_FREQ	   (_PRCMU_BASE + 0x088)
-#define PRCM_PLLDDR_FREQ	   (_PRCMU_BASE + 0x08C)
+#define PRCM_PLLSOC0_FREQ	   (prcmu_base + 0x080)
+#define PRCM_PLLSOC1_FREQ	   (prcmu_base + 0x084)
+#define PRCM_PLLARM_FREQ	   (prcmu_base + 0x088)
+#define PRCM_PLLDDR_FREQ	   (prcmu_base + 0x08C)
 #define PRCM_PLL_FREQ_D_SHIFT	0
 #define PRCM_PLL_FREQ_D_MASK	BITS(0, 7)
 #define PRCM_PLL_FREQ_N_SHIFT	8
@@ -152,14 +126,14 @@
 #define PRCM_PLL_FREQ_SELDIV2	BIT(24)
 #define PRCM_PLL_FREQ_DIV2EN	BIT(25)
 
-#define PRCM_PLLDSI_FREQ           (_PRCMU_BASE + 0x500)
-#define PRCM_PLLDSI_ENABLE         (_PRCMU_BASE + 0x504)
-#define PRCM_PLLDSI_LOCKP          (_PRCMU_BASE + 0x508)
-#define PRCM_DSI_PLLOUT_SEL        (_PRCMU_BASE + 0x530)
-#define PRCM_DSITVCLK_DIV          (_PRCMU_BASE + 0x52C)
-#define PRCM_PLLDSI_LOCKP          (_PRCMU_BASE + 0x508)
-#define PRCM_APE_RESETN_SET        (_PRCMU_BASE + 0x1E4)
-#define PRCM_APE_RESETN_CLR        (_PRCMU_BASE + 0x1E8)
+#define PRCM_PLLDSI_FREQ           (prcmu_base + 0x500)
+#define PRCM_PLLDSI_ENABLE         (prcmu_base + 0x504)
+#define PRCM_PLLDSI_LOCKP          (prcmu_base + 0x508)
+#define PRCM_DSI_PLLOUT_SEL        (prcmu_base + 0x530)
+#define PRCM_DSITVCLK_DIV          (prcmu_base + 0x52C)
+#define PRCM_PLLDSI_LOCKP          (prcmu_base + 0x508)
+#define PRCM_APE_RESETN_SET        (prcmu_base + 0x1E4)
+#define PRCM_APE_RESETN_CLR        (prcmu_base + 0x1E8)
 
 #define PRCM_PLLDSI_ENABLE_PRCM_PLLDSI_ENABLE BIT(0)
 
@@ -188,30 +162,30 @@
 
 #define PRCM_APE_RESETN_DSIPLL_RESETN BIT(14)
 
-#define PRCM_CLKOCR		   (_PRCMU_BASE + 0x1CC)
+#define PRCM_CLKOCR		   (prcmu_base + 0x1CC)
 #define PRCM_CLKOCR_CLKOUT0_REF_CLK	(1 << 0)
 #define PRCM_CLKOCR_CLKOUT0_MASK	BITS(0, 13)
 #define PRCM_CLKOCR_CLKOUT1_REF_CLK	(1 << 16)
 #define PRCM_CLKOCR_CLKOUT1_MASK	BITS(16, 29)
 
 /* ePOD and memory power signal control registers */
-#define PRCM_EPOD_C_SET            (_PRCMU_BASE + 0x410)
-#define PRCM_SRAM_LS_SLEEP         (_PRCMU_BASE + 0x304)
+#define PRCM_EPOD_C_SET            (prcmu_base + 0x410)
+#define PRCM_SRAM_LS_SLEEP         (prcmu_base + 0x304)
 
 /* Debug power control unit registers */
-#define PRCM_POWER_STATE_SET       (_PRCMU_BASE + 0x254)
+#define PRCM_POWER_STATE_SET       (prcmu_base + 0x254)
 
 /* Miscellaneous unit registers */
-#define PRCM_DSI_SW_RESET          (_PRCMU_BASE + 0x324)
-#define PRCM_GPIOCR                (_PRCMU_BASE + 0x138)
+#define PRCM_DSI_SW_RESET          (prcmu_base + 0x324)
+#define PRCM_GPIOCR                (prcmu_base + 0x138)
 #define PRCM_GPIOCR_DBG_STM_MOD_CMD1            0x800
 #define PRCM_GPIOCR_DBG_UARTMOD_CMD0            0x1
 
 /* PRCMU HW semaphore */
-#define PRCM_SEM                   (_PRCMU_BASE + 0x400)
+#define PRCM_SEM                   (prcmu_base + 0x400)
 #define PRCM_SEM_PRCM_SEM BIT(0)
 
-#define PRCM_TCR                   (_PRCMU_BASE + 0x1C8)
+#define PRCM_TCR                   (prcmu_base + 0x1C8)
 #define PRCM_TCR_TENSEL_MASK       BITS(0, 7)
 #define PRCM_TCR_STOP_TIMERS       BIT(16)
 #define PRCM_TCR_DOZE_MODE         BIT(17)
@@ -239,15 +213,15 @@
 /* GPIOCR register */
 #define PRCM_GPIOCR_SPI2_SELECT BIT(23)
 
-#define PRCM_DDR_SUBSYS_APE_MINBW	(_PRCMU_BASE + 0x438)
-#define PRCM_CGATING_BYPASS		(_PRCMU_BASE + 0x134)
+#define PRCM_DDR_SUBSYS_APE_MINBW	(prcmu_base + 0x438)
+#define PRCM_CGATING_BYPASS		(prcmu_base + 0x134)
 #define PRCM_CGATING_BYPASS_ICN2	BIT(6)
 
 /* Miscellaneous unit registers */
-#define PRCM_RESOUTN_SET		(_PRCMU_BASE + 0x214)
-#define PRCM_RESOUTN_CLR		(_PRCMU_BASE + 0x218)
+#define PRCM_RESOUTN_SET		(prcmu_base + 0x214)
+#define PRCM_RESOUTN_CLR		(prcmu_base + 0x218)
 
 /* System reset register */
-#define PRCM_APE_SOFTRST		(_PRCMU_BASE + 0x228)
+#define PRCM_APE_SOFTRST		(prcmu_base + 0x228)
 
 #endif /* __DB8500_PRCMU_REGS_H */
diff --git a/drivers/mfd/ezx-pcap.c b/drivers/mfd/ezx-pcap.c
index b7a61f0f27a4..5502106ad515 100644
--- a/drivers/mfd/ezx-pcap.c
+++ b/drivers/mfd/ezx-pcap.c
@@ -393,7 +393,7 @@ static int pcap_add_subdev(struct pcap_chip *pcap,
 
 static int ezx_pcap_remove(struct spi_device *spi)
 {
-	struct pcap_chip *pcap = dev_get_drvdata(&spi->dev);
+	struct pcap_chip *pcap = spi_get_drvdata(spi);
 	struct pcap_platform_data *pdata = spi->dev.platform_data;
 	int i, adc_irq;
 
@@ -403,7 +403,7 @@ static int ezx_pcap_remove(struct spi_device *spi)
 	/* cleanup ADC */
 	adc_irq = pcap_to_irq(pcap, (pdata->config & PCAP_SECOND_PORT) ?
 				PCAP_IRQ_ADCDONE2 : PCAP_IRQ_ADCDONE);
-	free_irq(adc_irq, pcap);
+	devm_free_irq(&spi->dev, adc_irq, pcap);
 	mutex_lock(&pcap->adc_mutex);
 	for (i = 0; i < PCAP_ADC_MAXQ; i++)
 		kfree(pcap->adc_queue[i]);
@@ -415,8 +415,6 @@ static int ezx_pcap_remove(struct spi_device *spi)
 
 	destroy_workqueue(pcap->workqueue);
 
-	kfree(pcap);
-
 	return 0;
 }
 
@@ -431,7 +429,7 @@ static int ezx_pcap_probe(struct spi_device *spi)
 	if (!pdata)
 		goto ret;
 
-	pcap = kzalloc(sizeof(*pcap), GFP_KERNEL);
+	pcap = devm_kzalloc(&spi->dev, sizeof(*pcap), GFP_KERNEL);
 	if (!pcap) {
 		ret = -ENOMEM;
 		goto ret;
@@ -441,14 +439,14 @@ static int ezx_pcap_probe(struct spi_device *spi)
 	mutex_init(&pcap->adc_mutex);
 	INIT_WORK(&pcap->isr_work, pcap_isr_work);
 	INIT_WORK(&pcap->msr_work, pcap_msr_work);
-	dev_set_drvdata(&spi->dev, pcap);
+	spi_set_drvdata(spi, pcap);
 
 	/* setup spi */
 	spi->bits_per_word = 32;
 	spi->mode = SPI_MODE_0 | (pdata->config & PCAP_CS_AH ? SPI_CS_HIGH : 0);
 	ret = spi_setup(spi);
 	if (ret)
-		goto free_pcap;
+		goto ret;
 
 	pcap->spi = spi;
 
@@ -458,7 +456,7 @@ static int ezx_pcap_probe(struct spi_device *spi)
 	if (!pcap->workqueue) {
 		ret = -ENOMEM;
 		dev_err(&spi->dev, "can't create pcap thread\n");
-		goto free_pcap;
+		goto ret;
 	}
 
 	/* redirect interrupts to AP, except adcdone2 */
@@ -491,7 +489,8 @@ static int ezx_pcap_probe(struct spi_device *spi)
 	adc_irq = pcap_to_irq(pcap, (pdata->config & PCAP_SECOND_PORT) ?
 					PCAP_IRQ_ADCDONE2 : PCAP_IRQ_ADCDONE);
 
-	ret = request_irq(adc_irq, pcap_adc_irq, 0, "ADC", pcap);
+	ret = devm_request_irq(&spi->dev, adc_irq, pcap_adc_irq, 0, "ADC",
+				pcap);
 	if (ret)
 		goto free_irqchip;
 
@@ -511,14 +510,12 @@ static int ezx_pcap_probe(struct spi_device *spi)
 remove_subdevs:
 	device_for_each_child(&spi->dev, NULL, pcap_remove_subdev);
 /* free_adc: */
-	free_irq(adc_irq, pcap);
+	devm_free_irq(&spi->dev, adc_irq, pcap);
 free_irqchip:
 	for (i = pcap->irq_base; i < (pcap->irq_base + PCAP_NIRQS); i++)
 		irq_set_chip_and_handler(i, NULL, NULL);
 /* destroy_workqueue: */
 	destroy_workqueue(pcap->workqueue);
-free_pcap:
-	kfree(pcap);
 ret:
 	return ret;
 }
diff --git a/drivers/mfd/htc-pasic3.c b/drivers/mfd/htc-pasic3.c
index 9e5453d21a68..0285fceb99a6 100644
--- a/drivers/mfd/htc-pasic3.c
+++ b/drivers/mfd/htc-pasic3.c
@@ -208,18 +208,7 @@ static struct platform_driver pasic3_driver = {
 	.remove		= pasic3_remove,
 };
 
-static int __init pasic3_base_init(void)
-{
-	return platform_driver_probe(&pasic3_driver, pasic3_probe);
-}
-
-static void __exit pasic3_base_exit(void)
-{
-	platform_driver_unregister(&pasic3_driver);
-}
-
-module_init(pasic3_base_init);
-module_exit(pasic3_base_exit);
+module_platform_driver_probe(pasic3_driver, pasic3_probe);
 
 MODULE_AUTHOR("Philipp Zabel <philipp.zabel@gmail.com>");
 MODULE_DESCRIPTION("Core driver for HTC PASIC3");
diff --git a/drivers/mfd/intel_msic.c b/drivers/mfd/intel_msic.c
index 1804331bd52c..5be3b5e13855 100644
--- a/drivers/mfd/intel_msic.c
+++ b/drivers/mfd/intel_msic.c
@@ -323,7 +323,8 @@ static int intel_msic_init_devices(struct intel_msic *msic)
 	if (pdata->ocd) {
 		unsigned gpio = pdata->ocd->gpio;
 
-		ret = gpio_request_one(gpio, GPIOF_IN, "ocd_gpio");
+		ret = devm_gpio_request_one(&pdev->dev, gpio,
+					GPIOF_IN, "ocd_gpio");
 		if (ret) {
 			dev_err(&pdev->dev, "failed to register OCD GPIO\n");
 			return ret;
@@ -332,7 +333,6 @@ static int intel_msic_init_devices(struct intel_msic *msic)
 		ret = gpio_to_irq(gpio);
 		if (ret < 0) {
 			dev_err(&pdev->dev, "no IRQ number for OCD GPIO\n");
-			gpio_free(gpio);
 			return ret;
 		}
 
@@ -359,8 +359,6 @@ static int intel_msic_init_devices(struct intel_msic *msic)
 
 fail:
 	mfd_remove_devices(&pdev->dev);
-	if (pdata->ocd)
-		gpio_free(pdata->ocd->gpio);
 
 	return ret;
 }
@@ -368,12 +366,8 @@ fail:
 static void intel_msic_remove_devices(struct intel_msic *msic)
 {
 	struct platform_device *pdev = msic->pdev;
-	struct intel_msic_platform_data *pdata = pdev->dev.platform_data;
 
 	mfd_remove_devices(&pdev->dev);
-
-	if (pdata->ocd)
-		gpio_free(pdata->ocd->gpio);
 }
 
 static int intel_msic_probe(struct platform_device *pdev)
diff --git a/drivers/mfd/lm3533-core.c b/drivers/mfd/lm3533-core.c
index ceebf2c1ea97..4b7e6dac1de8 100644
--- a/drivers/mfd/lm3533-core.c
+++ b/drivers/mfd/lm3533-core.c
@@ -496,8 +496,8 @@ static int lm3533_device_init(struct lm3533 *lm3533)
 	dev_set_drvdata(lm3533->dev, lm3533);
 
 	if (gpio_is_valid(lm3533->gpio_hwen)) {
-		ret = gpio_request_one(lm3533->gpio_hwen, GPIOF_OUT_INIT_LOW,
-								"lm3533-hwen");
+		ret = devm_gpio_request_one(lm3533->dev, lm3533->gpio_hwen,
+					GPIOF_OUT_INIT_LOW, "lm3533-hwen");
 		if (ret < 0) {
 			dev_err(lm3533->dev,
 				"failed to request HWEN GPIO %d\n",
@@ -528,8 +528,6 @@ err_unregister:
 	mfd_remove_devices(lm3533->dev);
 err_disable:
 	lm3533_disable(lm3533);
-	if (gpio_is_valid(lm3533->gpio_hwen))
-		gpio_free(lm3533->gpio_hwen);
 
 	return ret;
 }
@@ -542,8 +540,6 @@ static void lm3533_device_exit(struct lm3533 *lm3533)
 
 	mfd_remove_devices(lm3533->dev);
 	lm3533_disable(lm3533);
-	if (gpio_is_valid(lm3533->gpio_hwen))
-		gpio_free(lm3533->gpio_hwen);
 }
 
 static bool lm3533_readable_register(struct device *dev, unsigned int reg)
diff --git a/drivers/mfd/max77686.c b/drivers/mfd/max77686.c
index 4d73963cd8f0..1cbb17609c8b 100644
--- a/drivers/mfd/max77686.c
+++ b/drivers/mfd/max77686.c
@@ -46,7 +46,7 @@ static struct regmap_config max77686_regmap_config = {
 
 #ifdef CONFIG_OF
 static struct of_device_id max77686_pmic_dt_match[] = {
-	{.compatible = "maxim,max77686",        .data = 0},
+	{.compatible = "maxim,max77686", .data = NULL},
 	{},
 };
 
diff --git a/drivers/mfd/mc13xxx-spi.c b/drivers/mfd/mc13xxx-spi.c
index 3032bae20b62..77189daadf1e 100644
--- a/drivers/mfd/mc13xxx-spi.c
+++ b/drivers/mfd/mc13xxx-spi.c
@@ -131,7 +131,7 @@ static int mc13xxx_spi_probe(struct spi_device *spi)
 	if (!mc13xxx)
 		return -ENOMEM;
 
-	dev_set_drvdata(&spi->dev, mc13xxx);
+	spi_set_drvdata(spi, mc13xxx);
 	spi->mode = SPI_MODE_0 | SPI_CS_HIGH;
 
 	mc13xxx->dev = &spi->dev;
@@ -144,7 +144,7 @@ static int mc13xxx_spi_probe(struct spi_device *spi)
 		ret = PTR_ERR(mc13xxx->regmap);
 		dev_err(mc13xxx->dev, "Failed to initialize register map: %d\n",
 				ret);
-		dev_set_drvdata(&spi->dev, NULL);
+		spi_set_drvdata(spi, NULL);
 		return ret;
 	}
 
@@ -164,7 +164,7 @@ static int mc13xxx_spi_probe(struct spi_device *spi)
 
 static int mc13xxx_spi_remove(struct spi_device *spi)
 {
-	struct mc13xxx *mc13xxx = dev_get_drvdata(&spi->dev);
+	struct mc13xxx *mc13xxx = spi_get_drvdata(spi);
 
 	mc13xxx_common_cleanup(mc13xxx);
 
diff --git a/drivers/mfd/omap-usb-host.c b/drivers/mfd/omap-usb-host.c
index 4febc5c7fdee..759fae3ca7fb 100644
--- a/drivers/mfd/omap-usb-host.c
+++ b/drivers/mfd/omap-usb-host.c
@@ -1,8 +1,9 @@
 /**
  * omap-usb-host.c - The USBHS core driver for OMAP EHCI & OHCI
  *
- * Copyright (C) 2011 Texas Instruments Incorporated - http://www.ti.com
+ * Copyright (C) 2011-2013 Texas Instruments Incorporated - http://www.ti.com
  * Author: Keshava Munegowda <keshava_mgowda@ti.com>
+ * Author: Roger Quadros <rogerq@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  of
@@ -27,6 +28,9 @@
 #include <linux/platform_device.h>
 #include <linux/platform_data/usb-omap.h>
 #include <linux/pm_runtime.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/err.h>
 
 #include "omap-usb.h"
 
@@ -137,6 +141,49 @@ static inline u8 usbhs_readb(void __iomem *base, u8 reg)
 
 /*-------------------------------------------------------------------------*/
 
+/**
+ * Map 'enum usbhs_omap_port_mode' found in <linux/platform_data/usb-omap.h>
+ * to the device tree binding portN-mode found in
+ * 'Documentation/devicetree/bindings/mfd/omap-usb-host.txt'
+ */
+static const char * const port_modes[] = {
+	[OMAP_USBHS_PORT_MODE_UNUSED]	= "",
+	[OMAP_EHCI_PORT_MODE_PHY]	= "ehci-phy",
+	[OMAP_EHCI_PORT_MODE_TLL]	= "ehci-tll",
+	[OMAP_EHCI_PORT_MODE_HSIC]	= "ehci-hsic",
+	[OMAP_OHCI_PORT_MODE_PHY_6PIN_DATSE0]	= "ohci-phy-6pin-datse0",
+	[OMAP_OHCI_PORT_MODE_PHY_6PIN_DPDM]	= "ohci-phy-6pin-dpdm",
+	[OMAP_OHCI_PORT_MODE_PHY_3PIN_DATSE0]	= "ohci-phy-3pin-datse0",
+	[OMAP_OHCI_PORT_MODE_PHY_4PIN_DPDM]	= "ohci-phy-4pin-dpdm",
+	[OMAP_OHCI_PORT_MODE_TLL_6PIN_DATSE0]	= "ohci-tll-6pin-datse0",
+	[OMAP_OHCI_PORT_MODE_TLL_6PIN_DPDM]	= "ohci-tll-6pin-dpdm",
+	[OMAP_OHCI_PORT_MODE_TLL_3PIN_DATSE0]	= "ohci-tll-3pin-datse0",
+	[OMAP_OHCI_PORT_MODE_TLL_4PIN_DPDM]	= "ohci-tll-4pin-dpdm",
+	[OMAP_OHCI_PORT_MODE_TLL_2PIN_DATSE0]	= "ohci-tll-2pin-datse0",
+	[OMAP_OHCI_PORT_MODE_TLL_2PIN_DPDM]	= "ohci-tll-2pin-dpdm",
+};
+
+/**
+ * omap_usbhs_get_dt_port_mode - Get the 'enum usbhs_omap_port_mode'
+ * from the port mode string.
+ * @mode: The port mode string, usually obtained from device tree.
+ *
+ * The function returns the 'enum usbhs_omap_port_mode' that matches the
+ * provided port mode string as per the port_modes table.
+ * If no match is found it returns -ENODEV
+ */
+static const int omap_usbhs_get_dt_port_mode(const char *mode)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(port_modes); i++) {
+		if (!strcmp(mode, port_modes[i]))
+			return i;
+	}
+
+	return -ENODEV;
+}
+
 static struct platform_device *omap_usbhs_alloc_child(const char *name,
 			struct resource	*res, int num_resources, void *pdata,
 			size_t pdata_size, struct device *dev)
@@ -278,7 +325,7 @@ static int usbhs_runtime_resume(struct device *dev)
 
 	dev_dbg(dev, "usbhs_runtime_resume\n");
 
-	omap_tll_enable();
+	omap_tll_enable(pdata);
 
 	if (!IS_ERR(omap->ehci_logic_fck))
 		clk_enable(omap->ehci_logic_fck);
@@ -353,7 +400,7 @@ static int usbhs_runtime_suspend(struct device *dev)
 	if (!IS_ERR(omap->ehci_logic_fck))
 		clk_disable(omap->ehci_logic_fck);
 
-	omap_tll_disable();
+	omap_tll_disable(pdata);
 
 	return 0;
 }
@@ -430,24 +477,10 @@ static unsigned omap_usbhs_rev2_hostconfig(struct usbhs_hcd_omap *omap,
 static void omap_usbhs_init(struct device *dev)
 {
 	struct usbhs_hcd_omap		*omap = dev_get_drvdata(dev);
-	struct usbhs_omap_platform_data	*pdata = omap->pdata;
 	unsigned			reg;
 
 	dev_dbg(dev, "starting TI HSUSB Controller\n");
 
-	if (pdata->phy_reset) {
-		if (gpio_is_valid(pdata->reset_gpio_port[0]))
-			gpio_request_one(pdata->reset_gpio_port[0],
-					 GPIOF_OUT_INIT_LOW, "USB1 PHY reset");
-
-		if (gpio_is_valid(pdata->reset_gpio_port[1]))
-			gpio_request_one(pdata->reset_gpio_port[1],
-					 GPIOF_OUT_INIT_LOW, "USB2 PHY reset");
-
-		/* Hold the PHY in RESET for enough time till DIR is high */
-		udelay(10);
-	}
-
 	pm_runtime_get_sync(dev);
 
 	reg = usbhs_read(omap->uhh_base, OMAP_UHH_HOSTCONFIG);
@@ -476,36 +509,59 @@ static void omap_usbhs_init(struct device *dev)
 	dev_dbg(dev, "UHH setup done, uhh_hostconfig=%x\n", reg);
 
 	pm_runtime_put_sync(dev);
-	if (pdata->phy_reset) {
-		/* Hold the PHY in RESET for enough time till
-		 * PHY is settled and ready
-		 */
-		udelay(10);
+}
+
+static int usbhs_omap_get_dt_pdata(struct device *dev,
+					struct usbhs_omap_platform_data *pdata)
+{
+	int ret, i;
+	struct device_node *node = dev->of_node;
 
-		if (gpio_is_valid(pdata->reset_gpio_port[0]))
-			gpio_set_value_cansleep
-				(pdata->reset_gpio_port[0], 1);
+	ret = of_property_read_u32(node, "num-ports", &pdata->nports);
+	if (ret)
+		pdata->nports = 0;
 
-		if (gpio_is_valid(pdata->reset_gpio_port[1]))
-			gpio_set_value_cansleep
-				(pdata->reset_gpio_port[1], 1);
+	if (pdata->nports > OMAP3_HS_USB_PORTS) {
+		dev_warn(dev, "Too many num_ports <%d> in device tree. Max %d\n",
+				pdata->nports, OMAP3_HS_USB_PORTS);
+		return -ENODEV;
 	}
-}
 
-static void omap_usbhs_deinit(struct device *dev)
-{
-	struct usbhs_hcd_omap		*omap = dev_get_drvdata(dev);
-	struct usbhs_omap_platform_data	*pdata = omap->pdata;
+	/* get port modes */
+	for (i = 0; i < OMAP3_HS_USB_PORTS; i++) {
+		char prop[11];
+		const char *mode;
 
-	if (pdata->phy_reset) {
-		if (gpio_is_valid(pdata->reset_gpio_port[0]))
-			gpio_free(pdata->reset_gpio_port[0]);
+		pdata->port_mode[i] = OMAP_USBHS_PORT_MODE_UNUSED;
+
+		snprintf(prop, sizeof(prop), "port%d-mode", i + 1);
+		ret = of_property_read_string(node, prop, &mode);
+		if (ret < 0)
+			continue;
+
+		ret = omap_usbhs_get_dt_port_mode(mode);
+		if (ret < 0) {
+			dev_warn(dev, "Invalid port%d-mode \"%s\" in device tree\n",
+					i, mode);
+			return -ENODEV;
+		}
 
-		if (gpio_is_valid(pdata->reset_gpio_port[1]))
-			gpio_free(pdata->reset_gpio_port[1]);
+		dev_dbg(dev, "port%d-mode: %s -> %d\n", i, mode, ret);
+		pdata->port_mode[i] = ret;
 	}
+
+	/* get flags */
+	pdata->single_ulpi_bypass = of_property_read_bool(node,
+						"single-ulpi-bypass");
+
+	return 0;
 }
 
+static struct of_device_id usbhs_child_match_table[] = {
+	{ .compatible = "ti,omap-ehci", },
+	{ .compatible = "ti,omap-ohci", },
+	{ }
+};
 
 /**
  * usbhs_omap_probe - initialize TI-based HCDs
@@ -522,26 +578,46 @@ static int usbhs_omap_probe(struct platform_device *pdev)
 	int				i;
 	bool				need_logic_fck;
 
+	if (dev->of_node) {
+		/* For DT boot we populate platform data from OF node */
+		pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+		if (!pdata)
+			return -ENOMEM;
+
+		ret = usbhs_omap_get_dt_pdata(dev, pdata);
+		if (ret)
+			return ret;
+
+		dev->platform_data = pdata;
+	}
+
 	if (!pdata) {
 		dev_err(dev, "Missing platform data\n");
 		return -ENODEV;
 	}
 
+	if (pdata->nports > OMAP3_HS_USB_PORTS) {
+		dev_info(dev, "Too many num_ports <%d> in platform_data. Max %d\n",
+				pdata->nports, OMAP3_HS_USB_PORTS);
+		return -ENODEV;
+	}
+
 	omap = devm_kzalloc(dev, sizeof(*omap), GFP_KERNEL);
 	if (!omap) {
 		dev_err(dev, "Memory allocation failed\n");
 		return -ENOMEM;
 	}
 
-	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "uhh");
-	omap->uhh_base = devm_request_and_ioremap(dev, res);
-	if (!omap->uhh_base) {
-		dev_err(dev, "Resource request/ioremap failed\n");
-		return -EADDRNOTAVAIL;
-	}
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	omap->uhh_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(omap->uhh_base))
+		return PTR_ERR(omap->uhh_base);
 
 	omap->pdata = pdata;
 
+	/* Initialize the TLL subsystem */
+	omap_tll_init(pdata);
+
 	pm_runtime_enable(dev);
 
 	platform_set_drvdata(pdev, omap);
@@ -575,6 +651,7 @@ static int usbhs_omap_probe(struct platform_device *pdev)
 			 omap->usbhs_rev, omap->nports);
 			break;
 		}
+		pdata->nports = omap->nports;
 	}
 
 	i = sizeof(struct clk *) * omap->nports;
@@ -700,17 +777,28 @@ static int usbhs_omap_probe(struct platform_device *pdev)
 	}
 
 	omap_usbhs_init(dev);
-	ret = omap_usbhs_alloc_children(pdev);
-	if (ret) {
-		dev_err(dev, "omap_usbhs_alloc_children failed\n");
-		goto err_alloc;
+
+	if (dev->of_node) {
+		ret = of_platform_populate(dev->of_node,
+				usbhs_child_match_table, NULL, dev);
+
+		if (ret) {
+			dev_err(dev, "Failed to create DT children: %d\n", ret);
+			goto err_alloc;
+		}
+
+	} else {
+		ret = omap_usbhs_alloc_children(pdev);
+		if (ret) {
+			dev_err(dev, "omap_usbhs_alloc_children failed: %d\n",
+						ret);
+			goto err_alloc;
+		}
 	}
 
 	return 0;
 
 err_alloc:
-	omap_usbhs_deinit(&pdev->dev);
-
 	for (i = 0; i < omap->nports; i++) {
 		if (!IS_ERR(omap->utmi_clk[i]))
 			clk_put(omap->utmi_clk[i]);
@@ -744,6 +832,13 @@ err_mem:
 	return ret;
 }
 
+static int usbhs_omap_remove_child(struct device *dev, void *data)
+{
+	dev_info(dev, "unregistering\n");
+	platform_device_unregister(to_platform_device(dev));
+	return 0;
+}
+
 /**
  * usbhs_omap_remove - shutdown processing for UHH & TLL HCDs
  * @pdev: USB Host Controller being removed
@@ -755,8 +850,6 @@ static int usbhs_omap_remove(struct platform_device *pdev)
 	struct usbhs_hcd_omap *omap = platform_get_drvdata(pdev);
 	int i;
 
-	omap_usbhs_deinit(&pdev->dev);
-
 	for (i = 0; i < omap->nports; i++) {
 		if (!IS_ERR(omap->utmi_clk[i]))
 			clk_put(omap->utmi_clk[i]);
@@ -777,6 +870,8 @@ static int usbhs_omap_remove(struct platform_device *pdev)
 
 	pm_runtime_disable(&pdev->dev);
 
+	/* remove children */
+	device_for_each_child(&pdev->dev, NULL, usbhs_omap_remove_child);
 	return 0;
 }
 
@@ -785,16 +880,26 @@ static const struct dev_pm_ops usbhsomap_dev_pm_ops = {
 	.runtime_resume		= usbhs_runtime_resume,
 };
 
+static const struct of_device_id usbhs_omap_dt_ids[] = {
+	{ .compatible = "ti,usbhs-host" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, usbhs_omap_dt_ids);
+
+
 static struct platform_driver usbhs_omap_driver = {
 	.driver = {
 		.name		= (char *)usbhs_driver_name,
 		.owner		= THIS_MODULE,
 		.pm		= &usbhsomap_dev_pm_ops,
+		.of_match_table = of_match_ptr(usbhs_omap_dt_ids),
 	},
 	.remove		= usbhs_omap_remove,
 };
 
 MODULE_AUTHOR("Keshava Munegowda <keshava_mgowda@ti.com>");
+MODULE_AUTHOR("Roger Quadros <rogerq@ti.com>");
 MODULE_ALIAS("platform:" USBHS_DRIVER_NAME);
 MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("usb host common core driver for omap EHCI and OHCI");
diff --git a/drivers/mfd/omap-usb-tll.c b/drivers/mfd/omap-usb-tll.c
index 0aef1a768880..e59ac4cbac96 100644
--- a/drivers/mfd/omap-usb-tll.c
+++ b/drivers/mfd/omap-usb-tll.c
@@ -1,8 +1,9 @@
 /**
  * omap-usb-tll.c - The USB TLL driver for OMAP EHCI & OHCI
  *
- * Copyright (C) 2012 Texas Instruments Incorporated - http://www.ti.com
+ * Copyright (C) 2012-2013 Texas Instruments Incorporated - http://www.ti.com
  * Author: Keshava Munegowda <keshava_mgowda@ti.com>
+ * Author: Roger Quadros <rogerq@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  of
@@ -27,6 +28,7 @@
 #include <linux/err.h>
 #include <linux/pm_runtime.h>
 #include <linux/platform_data/usb-omap.h>
+#include <linux/of.h>
 
 #define USBTLL_DRIVER_NAME	"usbhs_tll"
 
@@ -105,8 +107,8 @@
 
 struct usbtll_omap {
 	int					nch;	/* num. of channels */
-	struct usbhs_omap_platform_data		*pdata;
 	struct clk				**ch_clk;
+	void __iomem				*base;
 };
 
 /*-------------------------------------------------------------------------*/
@@ -210,14 +212,10 @@ static unsigned ohci_omap3_fslsmode(enum usbhs_omap_port_mode mode)
 static int usbtll_omap_probe(struct platform_device *pdev)
 {
 	struct device				*dev =  &pdev->dev;
-	struct usbhs_omap_platform_data		*pdata = dev->platform_data;
-	void __iomem				*base;
 	struct resource				*res;
 	struct usbtll_omap			*tll;
-	unsigned				reg;
 	int					ret = 0;
 	int					i, ver;
-	bool needs_tll;
 
 	dev_dbg(dev, "starting TI HSUSB TLL Controller\n");
 
@@ -227,26 +225,16 @@ static int usbtll_omap_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	if (!pdata) {
-		dev_err(dev, "Platform data missing\n");
-		return -ENODEV;
-	}
-
-	tll->pdata = pdata;
-
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	base = devm_request_and_ioremap(dev, res);
-	if (!base) {
-		ret = -EADDRNOTAVAIL;
-		dev_err(dev, "Resource request/ioremap failed:%d\n", ret);
-		return ret;
-	}
+	tll->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(tll->base))
+		return PTR_ERR(tll->base);
 
 	platform_set_drvdata(pdev, tll);
 	pm_runtime_enable(dev);
 	pm_runtime_get_sync(dev);
 
-	ver =  usbtll_read(base, OMAP_USBTLL_REVISION);
+	ver =  usbtll_read(tll->base, OMAP_USBTLL_REVISION);
 	switch (ver) {
 	case OMAP_USBTLL_REV1:
 	case OMAP_USBTLL_REV4:
@@ -283,11 +271,85 @@ static int usbtll_omap_probe(struct platform_device *pdev)
 			dev_dbg(dev, "can't get clock : %s\n", clkname);
 	}
 
+	pm_runtime_put_sync(dev);
+	/* only after this can omap_tll_enable/disable work */
+	spin_lock(&tll_lock);
+	tll_dev = dev;
+	spin_unlock(&tll_lock);
+
+	return 0;
+
+err_clk_alloc:
+	pm_runtime_put_sync(dev);
+	pm_runtime_disable(dev);
+
+	return ret;
+}
+
+/**
+ * usbtll_omap_remove - shutdown processing for UHH & TLL HCDs
+ * @pdev: USB Host Controller being removed
+ *
+ * Reverses the effect of usbtll_omap_probe().
+ */
+static int usbtll_omap_remove(struct platform_device *pdev)
+{
+	struct usbtll_omap *tll = platform_get_drvdata(pdev);
+	int i;
+
+	spin_lock(&tll_lock);
+	tll_dev = NULL;
+	spin_unlock(&tll_lock);
+
+	for (i = 0; i < tll->nch; i++)
+		if (!IS_ERR(tll->ch_clk[i]))
+			clk_put(tll->ch_clk[i]);
+
+	pm_runtime_disable(&pdev->dev);
+	return 0;
+}
+
+static const struct of_device_id usbtll_omap_dt_ids[] = {
+	{ .compatible = "ti,usbhs-tll" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, usbtll_omap_dt_ids);
+
+static struct platform_driver usbtll_omap_driver = {
+	.driver = {
+		.name		= (char *)usbtll_driver_name,
+		.owner		= THIS_MODULE,
+		.of_match_table = of_match_ptr(usbtll_omap_dt_ids),
+	},
+	.probe		= usbtll_omap_probe,
+	.remove		= usbtll_omap_remove,
+};
+
+int omap_tll_init(struct usbhs_omap_platform_data *pdata)
+{
+	int i;
+	bool needs_tll;
+	unsigned reg;
+	struct usbtll_omap *tll;
+
+	spin_lock(&tll_lock);
+
+	if (!tll_dev) {
+		spin_unlock(&tll_lock);
+		return -ENODEV;
+	}
+
+	tll = dev_get_drvdata(tll_dev);
+
 	needs_tll = false;
 	for (i = 0; i < tll->nch; i++)
 		needs_tll |= omap_usb_mode_needs_tll(pdata->port_mode[i]);
 
+	pm_runtime_get_sync(tll_dev);
+
 	if (needs_tll) {
+		void __iomem *base = tll->base;
 
 		/* Program Common TLL register */
 		reg = usbtll_read(base, OMAP_TLL_SHARED_CONF);
@@ -336,51 +398,29 @@ static int usbtll_omap_probe(struct platform_device *pdev)
 		}
 	}
 
-	pm_runtime_put_sync(dev);
-	/* only after this can omap_tll_enable/disable work */
-	spin_lock(&tll_lock);
-	tll_dev = dev;
+	pm_runtime_put_sync(tll_dev);
+
 	spin_unlock(&tll_lock);
 
 	return 0;
-
-err_clk_alloc:
-	pm_runtime_put_sync(dev);
-	pm_runtime_disable(dev);
-
-	return ret;
 }
+EXPORT_SYMBOL_GPL(omap_tll_init);
 
-/**
- * usbtll_omap_remove - shutdown processing for UHH & TLL HCDs
- * @pdev: USB Host Controller being removed
- *
- * Reverses the effect of usbtll_omap_probe().
- */
-static int usbtll_omap_remove(struct platform_device *pdev)
+int omap_tll_enable(struct usbhs_omap_platform_data *pdata)
 {
-	struct usbtll_omap *tll = platform_get_drvdata(pdev);
 	int i;
+	struct usbtll_omap *tll;
 
 	spin_lock(&tll_lock);
-	tll_dev = NULL;
-	spin_unlock(&tll_lock);
 
-	for (i = 0; i < tll->nch; i++)
-		if (!IS_ERR(tll->ch_clk[i]))
-			clk_put(tll->ch_clk[i]);
-
-	pm_runtime_disable(&pdev->dev);
-	return 0;
-}
+	if (!tll_dev) {
+		spin_unlock(&tll_lock);
+		return -ENODEV;
+	}
 
-static int usbtll_runtime_resume(struct device *dev)
-{
-	struct usbtll_omap			*tll = dev_get_drvdata(dev);
-	struct usbhs_omap_platform_data		*pdata = tll->pdata;
-	int i;
+	tll = dev_get_drvdata(tll_dev);
 
-	dev_dbg(dev, "usbtll_runtime_resume\n");
+	pm_runtime_get_sync(tll_dev);
 
 	for (i = 0; i < tll->nch; i++) {
 		if (omap_usb_mode_needs_tll(pdata->port_mode[i])) {
@@ -391,22 +431,31 @@ static int usbtll_runtime_resume(struct device *dev)
 
 			r = clk_enable(tll->ch_clk[i]);
 			if (r) {
-				dev_err(dev,
+				dev_err(tll_dev,
 				 "Error enabling ch %d clock: %d\n", i, r);
 			}
 		}
 	}
 
+	spin_unlock(&tll_lock);
+
 	return 0;
 }
+EXPORT_SYMBOL_GPL(omap_tll_enable);
 
-static int usbtll_runtime_suspend(struct device *dev)
+int omap_tll_disable(struct usbhs_omap_platform_data *pdata)
 {
-	struct usbtll_omap			*tll = dev_get_drvdata(dev);
-	struct usbhs_omap_platform_data		*pdata = tll->pdata;
 	int i;
+	struct usbtll_omap *tll;
 
-	dev_dbg(dev, "usbtll_runtime_suspend\n");
+	spin_lock(&tll_lock);
+
+	if (!tll_dev) {
+		spin_unlock(&tll_lock);
+		return -ENODEV;
+	}
+
+	tll = dev_get_drvdata(tll_dev);
 
 	for (i = 0; i < tll->nch; i++) {
 		if (omap_usb_mode_needs_tll(pdata->port_mode[i])) {
@@ -415,64 +464,16 @@ static int usbtll_runtime_suspend(struct device *dev)
 		}
 	}
 
-	return 0;
-}
-
-static const struct dev_pm_ops usbtllomap_dev_pm_ops = {
-	SET_RUNTIME_PM_OPS(usbtll_runtime_suspend,
-			   usbtll_runtime_resume,
-			   NULL)
-};
-
-static struct platform_driver usbtll_omap_driver = {
-	.driver = {
-		.name		= (char *)usbtll_driver_name,
-		.owner		= THIS_MODULE,
-		.pm		= &usbtllomap_dev_pm_ops,
-	},
-	.probe		= usbtll_omap_probe,
-	.remove		= usbtll_omap_remove,
-};
-
-int omap_tll_enable(void)
-{
-	int ret;
-
-	spin_lock(&tll_lock);
-
-	if (!tll_dev) {
-		pr_err("%s: OMAP USB TLL not initialized\n", __func__);
-		ret = -ENODEV;
-	} else {
-		ret = pm_runtime_get_sync(tll_dev);
-	}
-
-	spin_unlock(&tll_lock);
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(omap_tll_enable);
-
-int omap_tll_disable(void)
-{
-	int ret;
-
-	spin_lock(&tll_lock);
-
-	if (!tll_dev) {
-		pr_err("%s: OMAP USB TLL not initialized\n", __func__);
-		ret = -ENODEV;
-	} else {
-		ret = pm_runtime_put_sync(tll_dev);
-	}
+	pm_runtime_put_sync(tll_dev);
 
 	spin_unlock(&tll_lock);
 
-	return ret;
+	return 0;
 }
 EXPORT_SYMBOL_GPL(omap_tll_disable);
 
 MODULE_AUTHOR("Keshava Munegowda <keshava_mgowda@ti.com>");
+MODULE_AUTHOR("Roger Quadros <rogerq@ti.com>");
 MODULE_ALIAS("platform:" USBHS_DRIVER_NAME);
 MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("usb tll driver for TI OMAP EHCI and OHCI controllers");
diff --git a/drivers/mfd/omap-usb.h b/drivers/mfd/omap-usb.h
index 972aa961b064..2a508b6aeac8 100644
--- a/drivers/mfd/omap-usb.h
+++ b/drivers/mfd/omap-usb.h
@@ -1,2 +1,3 @@
-extern int omap_tll_enable(void);
-extern int omap_tll_disable(void);
+extern int omap_tll_init(struct usbhs_omap_platform_data *pdata);
+extern int omap_tll_enable(struct usbhs_omap_platform_data *pdata);
+extern int omap_tll_disable(struct usbhs_omap_platform_data *pdata);
diff --git a/drivers/mfd/palmas.c b/drivers/mfd/palmas.c
index 73bf76df1044..53e9fe638d32 100644
--- a/drivers/mfd/palmas.c
+++ b/drivers/mfd/palmas.c
@@ -278,20 +278,20 @@ static void palmas_dt_to_pdata(struct i2c_client *i2c,
 	int ret;
 	u32 prop;
 
-	ret = of_property_read_u32(node, "ti,mux_pad1", &prop);
+	ret = of_property_read_u32(node, "ti,mux-pad1", &prop);
 	if (!ret) {
 		pdata->mux_from_pdata = 1;
 		pdata->pad1 = prop;
 	}
 
-	ret = of_property_read_u32(node, "ti,mux_pad2", &prop);
+	ret = of_property_read_u32(node, "ti,mux-pad2", &prop);
 	if (!ret) {
 		pdata->mux_from_pdata = 1;
 		pdata->pad2 = prop;
 	}
 
 	/* The default for this register is all masked */
-	ret = of_property_read_u32(node, "ti,power_ctrl", &prop);
+	ret = of_property_read_u32(node, "ti,power-ctrl", &prop);
 	if (!ret)
 		pdata->power_ctrl = prop;
 	else
@@ -349,6 +349,7 @@ static int palmas_i2c_probe(struct i2c_client *i2c,
 				ret = -ENOMEM;
 				goto err;
 			}
+			palmas->i2c_clients[i]->dev.of_node = of_node_get(node);
 		}
 		palmas->regmap[i] = devm_regmap_init_i2c(palmas->i2c_clients[i],
 				&palmas_regmap_config[i]);
diff --git a/drivers/mfd/retu-mfd.c b/drivers/mfd/retu-mfd.c
index 3ba048655bf3..a1830986eeb7 100644
--- a/drivers/mfd/retu-mfd.c
+++ b/drivers/mfd/retu-mfd.c
@@ -1,5 +1,5 @@
 /*
- * Retu MFD driver
+ * Retu/Tahvo MFD driver
  *
  * Copyright (C) 2004, 2005 Nokia Corporation
  *
@@ -33,7 +33,8 @@
 #define RETU_REG_ASICR		0x00		/* ASIC ID and revision */
 #define RETU_REG_ASICR_VILMA	(1 << 7)	/* Bit indicating Vilma */
 #define RETU_REG_IDR		0x01		/* Interrupt ID */
-#define RETU_REG_IMR		0x02		/* Interrupt mask */
+#define RETU_REG_IMR		0x02		/* Interrupt mask (Retu) */
+#define TAHVO_REG_IMR		0x03		/* Interrupt mask (Tahvo) */
 
 /* Interrupt sources */
 #define RETU_INT_PWR		0		/* Power button */
@@ -84,6 +85,62 @@ static struct regmap_irq_chip retu_irq_chip = {
 /* Retu device registered for the power off. */
 static struct retu_dev *retu_pm_power_off;
 
+static struct resource tahvo_usb_res[] = {
+	{
+		.name	= "tahvo-usb",
+		.start	= TAHVO_INT_VBUS,
+		.end	= TAHVO_INT_VBUS,
+		.flags	= IORESOURCE_IRQ,
+	},
+};
+
+static struct mfd_cell tahvo_devs[] = {
+	{
+		.name		= "tahvo-usb",
+		.resources	= tahvo_usb_res,
+		.num_resources	= ARRAY_SIZE(tahvo_usb_res),
+	},
+};
+
+static struct regmap_irq tahvo_irqs[] = {
+	[TAHVO_INT_VBUS] = {
+		.mask = 1 << TAHVO_INT_VBUS,
+	}
+};
+
+static struct regmap_irq_chip tahvo_irq_chip = {
+	.name		= "TAHVO",
+	.irqs		= tahvo_irqs,
+	.num_irqs	= ARRAY_SIZE(tahvo_irqs),
+	.num_regs	= 1,
+	.status_base	= RETU_REG_IDR,
+	.mask_base	= TAHVO_REG_IMR,
+	.ack_base	= RETU_REG_IDR,
+};
+
+static const struct retu_data {
+	char			*chip_name;
+	char			*companion_name;
+	struct regmap_irq_chip	*irq_chip;
+	struct mfd_cell		*children;
+	int			nchildren;
+} retu_data[] = {
+	[0] = {
+		.chip_name	= "Retu",
+		.companion_name	= "Vilma",
+		.irq_chip	= &retu_irq_chip,
+		.children	= retu_devs,
+		.nchildren	= ARRAY_SIZE(retu_devs),
+	},
+	[1] = {
+		.chip_name	= "Tahvo",
+		.companion_name	= "Betty",
+		.irq_chip	= &tahvo_irq_chip,
+		.children	= tahvo_devs,
+		.nchildren	= ARRAY_SIZE(tahvo_devs),
+	}
+};
+
 int retu_read(struct retu_dev *rdev, u8 reg)
 {
 	int ret;
@@ -173,9 +230,14 @@ static struct regmap_config retu_config = {
 
 static int retu_probe(struct i2c_client *i2c, const struct i2c_device_id *id)
 {
+	struct retu_data const *rdat;
 	struct retu_dev *rdev;
 	int ret;
 
+	if (i2c->addr > ARRAY_SIZE(retu_data))
+		return -ENODEV;
+	rdat = &retu_data[i2c->addr - 1];
+
 	rdev = devm_kzalloc(&i2c->dev, sizeof(*rdev), GFP_KERNEL);
 	if (rdev == NULL)
 		return -ENOMEM;
@@ -190,25 +252,27 @@ static int retu_probe(struct i2c_client *i2c, const struct i2c_device_id *id)
 
 	ret = retu_read(rdev, RETU_REG_ASICR);
 	if (ret < 0) {
-		dev_err(rdev->dev, "could not read Retu revision: %d\n", ret);
+		dev_err(rdev->dev, "could not read %s revision: %d\n",
+			rdat->chip_name, ret);
 		return ret;
 	}
 
-	dev_info(rdev->dev, "Retu%s v%d.%d found\n",
-		 (ret & RETU_REG_ASICR_VILMA) ? " & Vilma" : "",
+	dev_info(rdev->dev, "%s%s%s v%d.%d found\n", rdat->chip_name,
+		 (ret & RETU_REG_ASICR_VILMA) ? " & " : "",
+		 (ret & RETU_REG_ASICR_VILMA) ? rdat->companion_name : "",
 		 (ret >> 4) & 0x7, ret & 0xf);
 
-	/* Mask all RETU interrupts. */
-	ret = retu_write(rdev, RETU_REG_IMR, 0xffff);
+	/* Mask all interrupts. */
+	ret = retu_write(rdev, rdat->irq_chip->mask_base, 0xffff);
 	if (ret < 0)
 		return ret;
 
 	ret = regmap_add_irq_chip(rdev->regmap, i2c->irq, IRQF_ONESHOT, -1,
-				  &retu_irq_chip, &rdev->irq_data);
+				  rdat->irq_chip, &rdev->irq_data);
 	if (ret < 0)
 		return ret;
 
-	ret = mfd_add_devices(rdev->dev, -1, retu_devs, ARRAY_SIZE(retu_devs),
+	ret = mfd_add_devices(rdev->dev, -1, rdat->children, rdat->nchildren,
 			      NULL, regmap_irq_chip_get_base(rdev->irq_data),
 			      NULL);
 	if (ret < 0) {
@@ -216,7 +280,7 @@ static int retu_probe(struct i2c_client *i2c, const struct i2c_device_id *id)
 		return ret;
 	}
 
-	if (!pm_power_off) {
+	if (i2c->addr == 1 && !pm_power_off) {
 		retu_pm_power_off = rdev;
 		pm_power_off	  = retu_power_off;
 	}
@@ -240,6 +304,7 @@ static int retu_remove(struct i2c_client *i2c)
 
 static const struct i2c_device_id retu_id[] = {
 	{ "retu-mfd", 0 },
+	{ "tahvo-mfd", 0 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, retu_id);
diff --git a/drivers/mfd/rts5249.c b/drivers/mfd/rts5249.c
new file mode 100644
index 000000000000..15dc848bc081
--- /dev/null
+++ b/drivers/mfd/rts5249.c
@@ -0,0 +1,241 @@
+/* Driver for Realtek PCI-Express card reader
+ *
+ * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2, 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, see <http://www.gnu.org/licenses/>.
+ *
+ * Author:
+ *   Wei WANG <wei_wang@realsil.com.cn>
+ *   No. 128, West Shenhu Road, Suzhou Industry Park, Suzhou, China
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/mfd/rtsx_pci.h>
+
+#include "rtsx_pcr.h"
+
+static u8 rts5249_get_ic_version(struct rtsx_pcr *pcr)
+{
+	u8 val;
+
+	rtsx_pci_read_register(pcr, DUMMY_REG_RESET_0, &val);
+	return val & 0x0F;
+}
+
+static int rts5249_extra_init_hw(struct rtsx_pcr *pcr)
+{
+	rtsx_pci_init_cmd(pcr);
+
+	/* Configure GPIO as output */
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, GPIO_CTL, 0x02, 0x02);
+	/* Switch LDO3318 source from DV33 to card_3v3 */
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, LDO_PWR_SEL, 0x03, 0x00);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, LDO_PWR_SEL, 0x03, 0x01);
+	/* LED shine disabled, set initial shine cycle period */
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, OLT_LED_CTL, 0x0F, 0x02);
+	/* Correct driving */
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD,
+			SD30_CLK_DRIVE_SEL, 0xFF, 0x99);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD,
+			SD30_CMD_DRIVE_SEL, 0xFF, 0x99);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD,
+			SD30_DAT_DRIVE_SEL, 0xFF, 0x92);
+
+	return rtsx_pci_send_cmd(pcr, 100);
+}
+
+static int rts5249_optimize_phy(struct rtsx_pcr *pcr)
+{
+	int err;
+
+	err = rtsx_pci_write_phy_register(pcr, PHY_REG_REV, 0xFE46);
+	if (err < 0)
+		return err;
+
+	msleep(1);
+
+	return rtsx_pci_write_phy_register(pcr, PHY_BPCR, 0x05C0);
+}
+
+static int rts5249_turn_on_led(struct rtsx_pcr *pcr)
+{
+	return rtsx_pci_write_register(pcr, GPIO_CTL, 0x02, 0x02);
+}
+
+static int rts5249_turn_off_led(struct rtsx_pcr *pcr)
+{
+	return rtsx_pci_write_register(pcr, GPIO_CTL, 0x02, 0x00);
+}
+
+static int rts5249_enable_auto_blink(struct rtsx_pcr *pcr)
+{
+	return rtsx_pci_write_register(pcr, OLT_LED_CTL, 0x08, 0x08);
+}
+
+static int rts5249_disable_auto_blink(struct rtsx_pcr *pcr)
+{
+	return rtsx_pci_write_register(pcr, OLT_LED_CTL, 0x08, 0x00);
+}
+
+static int rts5249_card_power_on(struct rtsx_pcr *pcr, int card)
+{
+	int err;
+
+	rtsx_pci_init_cmd(pcr);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CARD_PWR_CTL,
+			SD_POWER_MASK, SD_VCC_PARTIAL_POWER_ON);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, PWR_GATE_CTRL,
+			LDO3318_PWR_MASK, 0x02);
+	err = rtsx_pci_send_cmd(pcr, 100);
+	if (err < 0)
+		return err;
+
+	msleep(5);
+
+	rtsx_pci_init_cmd(pcr);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CARD_PWR_CTL,
+			SD_POWER_MASK, SD_VCC_POWER_ON);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, PWR_GATE_CTRL,
+			LDO3318_PWR_MASK, 0x06);
+	err = rtsx_pci_send_cmd(pcr, 100);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+
+static int rts5249_card_power_off(struct rtsx_pcr *pcr, int card)
+{
+	rtsx_pci_init_cmd(pcr);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CARD_PWR_CTL,
+			SD_POWER_MASK, SD_POWER_OFF);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, PWR_GATE_CTRL,
+			LDO3318_PWR_MASK, 0x00);
+	return rtsx_pci_send_cmd(pcr, 100);
+}
+
+static int rts5249_switch_output_voltage(struct rtsx_pcr *pcr, u8 voltage)
+{
+	int err;
+	u8 clk_drive, cmd_drive, dat_drive;
+
+	if (voltage == OUTPUT_3V3) {
+		err = rtsx_pci_write_phy_register(pcr, PHY_TUNE, 0x4FC0 | 0x24);
+		if (err < 0)
+			return err;
+		clk_drive = 0x99;
+		cmd_drive = 0x99;
+		dat_drive = 0x92;
+	} else if (voltage == OUTPUT_1V8) {
+		err = rtsx_pci_write_phy_register(pcr, PHY_BACR, 0x3C02);
+		if (err < 0)
+			return err;
+		err = rtsx_pci_write_phy_register(pcr, PHY_TUNE, 0x4C40 | 0x24);
+		if (err < 0)
+			return err;
+		clk_drive = 0xb3;
+		cmd_drive = 0xb3;
+		dat_drive = 0xb3;
+	} else {
+		return -EINVAL;
+	}
+
+	/* set pad drive */
+	rtsx_pci_init_cmd(pcr);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, SD30_CLK_DRIVE_SEL,
+			0xFF, clk_drive);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, SD30_CMD_DRIVE_SEL,
+			0xFF, cmd_drive);
+	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, SD30_DAT_DRIVE_SEL,
+			0xFF, dat_drive);
+	return rtsx_pci_send_cmd(pcr, 100);
+}
+
+static const struct pcr_ops rts5249_pcr_ops = {
+	.extra_init_hw = rts5249_extra_init_hw,
+	.optimize_phy = rts5249_optimize_phy,
+	.turn_on_led = rts5249_turn_on_led,
+	.turn_off_led = rts5249_turn_off_led,
+	.enable_auto_blink = rts5249_enable_auto_blink,
+	.disable_auto_blink = rts5249_disable_auto_blink,
+	.card_power_on = rts5249_card_power_on,
+	.card_power_off = rts5249_card_power_off,
+	.switch_output_voltage = rts5249_switch_output_voltage,
+};
+
+/* SD Pull Control Enable:
+ *     SD_DAT[3:0] ==> pull up
+ *     SD_CD       ==> pull up
+ *     SD_WP       ==> pull up
+ *     SD_CMD      ==> pull up
+ *     SD_CLK      ==> pull down
+ */
+static const u32 rts5249_sd_pull_ctl_enable_tbl[] = {
+	RTSX_REG_PAIR(CARD_PULL_CTL1, 0x66),
+	RTSX_REG_PAIR(CARD_PULL_CTL2, 0xAA),
+	RTSX_REG_PAIR(CARD_PULL_CTL3, 0xE9),
+	RTSX_REG_PAIR(CARD_PULL_CTL4, 0xAA),
+	0,
+};
+
+/* SD Pull Control Disable:
+ *     SD_DAT[3:0] ==> pull down
+ *     SD_CD       ==> pull up
+ *     SD_WP       ==> pull down
+ *     SD_CMD      ==> pull down
+ *     SD_CLK      ==> pull down
+ */
+static const u32 rts5249_sd_pull_ctl_disable_tbl[] = {
+	RTSX_REG_PAIR(CARD_PULL_CTL1, 0x66),
+	RTSX_REG_PAIR(CARD_PULL_CTL2, 0x55),
+	RTSX_REG_PAIR(CARD_PULL_CTL3, 0xD5),
+	RTSX_REG_PAIR(CARD_PULL_CTL4, 0x55),
+	0,
+};
+
+/* MS Pull Control Enable:
+ *     MS CD       ==> pull up
+ *     others      ==> pull down
+ */
+static const u32 rts5249_ms_pull_ctl_enable_tbl[] = {
+	RTSX_REG_PAIR(CARD_PULL_CTL4, 0x55),
+	RTSX_REG_PAIR(CARD_PULL_CTL5, 0x55),
+	RTSX_REG_PAIR(CARD_PULL_CTL6, 0x15),
+	0,
+};
+
+/* MS Pull Control Disable:
+ *     MS CD       ==> pull up
+ *     others      ==> pull down
+ */
+static const u32 rts5249_ms_pull_ctl_disable_tbl[] = {
+	RTSX_REG_PAIR(CARD_PULL_CTL4, 0x55),
+	RTSX_REG_PAIR(CARD_PULL_CTL5, 0x55),
+	RTSX_REG_PAIR(CARD_PULL_CTL6, 0x15),
+	0,
+};
+
+void rts5249_init_params(struct rtsx_pcr *pcr)
+{
+	pcr->extra_caps = EXTRA_CAPS_SD_SDR50 | EXTRA_CAPS_SD_SDR104;
+	pcr->num_slots = 2;
+	pcr->ops = &rts5249_pcr_ops;
+
+	pcr->ic_version = rts5249_get_ic_version(pcr);
+	pcr->sd_pull_ctl_enable_tbl = rts5249_sd_pull_ctl_enable_tbl;
+	pcr->sd_pull_ctl_disable_tbl = rts5249_sd_pull_ctl_disable_tbl;
+	pcr->ms_pull_ctl_enable_tbl = rts5249_ms_pull_ctl_enable_tbl;
+	pcr->ms_pull_ctl_disable_tbl = rts5249_ms_pull_ctl_disable_tbl;
+}
diff --git a/drivers/mfd/rtsx_pcr.c b/drivers/mfd/rtsx_pcr.c
index 2f12cc13489a..e968c01ca2ac 100644
--- a/drivers/mfd/rtsx_pcr.c
+++ b/drivers/mfd/rtsx_pcr.c
@@ -56,6 +56,7 @@ static DEFINE_PCI_DEVICE_TABLE(rtsx_pci_ids) = {
 	{ PCI_DEVICE(0x10EC, 0x5229), PCI_CLASS_OTHERS << 16, 0xFF0000 },
 	{ PCI_DEVICE(0x10EC, 0x5289), PCI_CLASS_OTHERS << 16, 0xFF0000 },
 	{ PCI_DEVICE(0x10EC, 0x5227), PCI_CLASS_OTHERS << 16, 0xFF0000 },
+	{ PCI_DEVICE(0x10EC, 0x5249), PCI_CLASS_OTHERS << 16, 0xFF0000 },
 	{ 0, }
 };
 
@@ -1033,6 +1034,10 @@ static int rtsx_pci_init_chip(struct rtsx_pcr *pcr)
 	case 0x5227:
 		rts5227_init_params(pcr);
 		break;
+
+	case 0x5249:
+		rts5249_init_params(pcr);
+		break;
 	}
 
 	dev_dbg(&(pcr->pci->dev), "PID: 0x%04x, IC version: 0x%02x\n",
@@ -1138,7 +1143,7 @@ static int rtsx_pci_probe(struct pci_dev *pcidev,
 
 	ret = rtsx_pci_acquire_irq(pcr);
 	if (ret < 0)
-		goto free_dma;
+		goto disable_msi;
 
 	pci_set_master(pcidev);
 	synchronize_irq(pcr->irq);
@@ -1162,7 +1167,9 @@ static int rtsx_pci_probe(struct pci_dev *pcidev,
 
 disable_irq:
 	free_irq(pcr->irq, (void *)pcr);
-free_dma:
+disable_msi:
+	if (pcr->msi_en)
+		pci_disable_msi(pcr->pci);
 	dma_free_coherent(&(pcr->pci->dev), RTSX_RESV_BUF_LEN,
 			pcr->rtsx_resv_buf, pcr->rtsx_resv_buf_addr);
 unmap:
diff --git a/drivers/mfd/rtsx_pcr.h b/drivers/mfd/rtsx_pcr.h
index 2b3ab8a04823..55fcfc25c4e4 100644
--- a/drivers/mfd/rtsx_pcr.h
+++ b/drivers/mfd/rtsx_pcr.h
@@ -32,5 +32,6 @@ void rts5209_init_params(struct rtsx_pcr *pcr);
 void rts5229_init_params(struct rtsx_pcr *pcr);
 void rtl8411_init_params(struct rtsx_pcr *pcr);
 void rts5227_init_params(struct rtsx_pcr *pcr);
+void rts5249_init_params(struct rtsx_pcr *pcr);
 
 #endif
diff --git a/drivers/mfd/si476x-cmd.c b/drivers/mfd/si476x-cmd.c
new file mode 100644
index 000000000000..de48b4e88450
--- /dev/null
+++ b/drivers/mfd/si476x-cmd.c
@@ -0,0 +1,1553 @@
+/*
+ * drivers/mfd/si476x-cmd.c -- Subroutines implementing command
+ * protocol of si476x series of chips
+ *
+ * Copyright (C) 2012 Innovative Converged Devices(ICD)
+ * Copyright (C) 2013 Andrey Smirnov
+ *
+ * Author: Andrey Smirnov <andrew.smirnov@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; 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.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/atomic.h>
+#include <linux/i2c.h>
+#include <linux/device.h>
+#include <linux/gpio.h>
+#include <linux/videodev2.h>
+
+#include <linux/mfd/si476x-core.h>
+
+#define msb(x)                  ((u8)((u16) x >> 8))
+#define lsb(x)                  ((u8)((u16) x &  0x00FF))
+
+
+
+#define CMD_POWER_UP				0x01
+#define CMD_POWER_UP_A10_NRESP			1
+#define CMD_POWER_UP_A10_NARGS			5
+
+#define CMD_POWER_UP_A20_NRESP			1
+#define CMD_POWER_UP_A20_NARGS			5
+
+#define POWER_UP_DELAY_MS			110
+
+#define CMD_POWER_DOWN				0x11
+#define CMD_POWER_DOWN_A10_NRESP		1
+
+#define CMD_POWER_DOWN_A20_NRESP		1
+#define CMD_POWER_DOWN_A20_NARGS		1
+
+#define CMD_FUNC_INFO				0x12
+#define CMD_FUNC_INFO_NRESP			7
+
+#define CMD_SET_PROPERTY			0x13
+#define CMD_SET_PROPERTY_NARGS			5
+#define CMD_SET_PROPERTY_NRESP			1
+
+#define CMD_GET_PROPERTY			0x14
+#define CMD_GET_PROPERTY_NARGS			3
+#define CMD_GET_PROPERTY_NRESP			4
+
+#define CMD_AGC_STATUS				0x17
+#define CMD_AGC_STATUS_NRESP_A10		2
+#define CMD_AGC_STATUS_NRESP_A20                6
+
+#define PIN_CFG_BYTE(x) (0x7F & (x))
+#define CMD_DIG_AUDIO_PIN_CFG			0x18
+#define CMD_DIG_AUDIO_PIN_CFG_NARGS		4
+#define CMD_DIG_AUDIO_PIN_CFG_NRESP		5
+
+#define CMD_ZIF_PIN_CFG				0x19
+#define CMD_ZIF_PIN_CFG_NARGS			4
+#define CMD_ZIF_PIN_CFG_NRESP			5
+
+#define CMD_IC_LINK_GPO_CTL_PIN_CFG		0x1A
+#define CMD_IC_LINK_GPO_CTL_PIN_CFG_NARGS	4
+#define CMD_IC_LINK_GPO_CTL_PIN_CFG_NRESP	5
+
+#define CMD_ANA_AUDIO_PIN_CFG			0x1B
+#define CMD_ANA_AUDIO_PIN_CFG_NARGS		1
+#define CMD_ANA_AUDIO_PIN_CFG_NRESP		2
+
+#define CMD_INTB_PIN_CFG			0x1C
+#define CMD_INTB_PIN_CFG_NARGS			2
+#define CMD_INTB_PIN_CFG_A10_NRESP		6
+#define CMD_INTB_PIN_CFG_A20_NRESP		3
+
+#define CMD_FM_TUNE_FREQ			0x30
+#define CMD_FM_TUNE_FREQ_A10_NARGS		5
+#define CMD_FM_TUNE_FREQ_A20_NARGS		3
+#define CMD_FM_TUNE_FREQ_NRESP			1
+
+#define CMD_FM_RSQ_STATUS			0x32
+
+#define CMD_FM_RSQ_STATUS_A10_NARGS		1
+#define CMD_FM_RSQ_STATUS_A10_NRESP		17
+#define CMD_FM_RSQ_STATUS_A30_NARGS		1
+#define CMD_FM_RSQ_STATUS_A30_NRESP		23
+
+
+#define CMD_FM_SEEK_START			0x31
+#define CMD_FM_SEEK_START_NARGS			1
+#define CMD_FM_SEEK_START_NRESP			1
+
+#define CMD_FM_RDS_STATUS			0x36
+#define CMD_FM_RDS_STATUS_NARGS			1
+#define CMD_FM_RDS_STATUS_NRESP			16
+
+#define CMD_FM_RDS_BLOCKCOUNT			0x37
+#define CMD_FM_RDS_BLOCKCOUNT_NARGS		1
+#define CMD_FM_RDS_BLOCKCOUNT_NRESP		8
+
+#define CMD_FM_PHASE_DIVERSITY			0x38
+#define CMD_FM_PHASE_DIVERSITY_NARGS		1
+#define CMD_FM_PHASE_DIVERSITY_NRESP		1
+
+#define CMD_FM_PHASE_DIV_STATUS			0x39
+#define CMD_FM_PHASE_DIV_STATUS_NRESP		2
+
+#define CMD_AM_TUNE_FREQ			0x40
+#define CMD_AM_TUNE_FREQ_NARGS			3
+#define CMD_AM_TUNE_FREQ_NRESP			1
+
+#define CMD_AM_RSQ_STATUS			0x42
+#define CMD_AM_RSQ_STATUS_NARGS			1
+#define CMD_AM_RSQ_STATUS_NRESP			13
+
+#define CMD_AM_SEEK_START			0x41
+#define CMD_AM_SEEK_START_NARGS			1
+#define CMD_AM_SEEK_START_NRESP			1
+
+
+#define CMD_AM_ACF_STATUS			0x45
+#define CMD_AM_ACF_STATUS_NRESP			6
+#define CMD_AM_ACF_STATUS_NARGS			1
+
+#define CMD_FM_ACF_STATUS			0x35
+#define CMD_FM_ACF_STATUS_NRESP			8
+#define CMD_FM_ACF_STATUS_NARGS			1
+
+#define CMD_MAX_ARGS_COUNT			(10)
+
+
+enum si476x_acf_status_report_bits {
+	SI476X_ACF_BLEND_INT	= (1 << 4),
+	SI476X_ACF_HIBLEND_INT	= (1 << 3),
+	SI476X_ACF_HICUT_INT	= (1 << 2),
+	SI476X_ACF_CHBW_INT	= (1 << 1),
+	SI476X_ACF_SOFTMUTE_INT	= (1 << 0),
+
+	SI476X_ACF_SMUTE	= (1 << 0),
+	SI476X_ACF_SMATTN	= 0b11111,
+	SI476X_ACF_PILOT	= (1 << 7),
+	SI476X_ACF_STBLEND	= ~SI476X_ACF_PILOT,
+};
+
+enum si476x_agc_status_report_bits {
+	SI476X_AGC_MXHI		= (1 << 5),
+	SI476X_AGC_MXLO		= (1 << 4),
+	SI476X_AGC_LNAHI	= (1 << 3),
+	SI476X_AGC_LNALO	= (1 << 2),
+};
+
+enum si476x_errors {
+	SI476X_ERR_BAD_COMMAND		= 0x10,
+	SI476X_ERR_BAD_ARG1		= 0x11,
+	SI476X_ERR_BAD_ARG2		= 0x12,
+	SI476X_ERR_BAD_ARG3		= 0x13,
+	SI476X_ERR_BAD_ARG4		= 0x14,
+	SI476X_ERR_BUSY			= 0x18,
+	SI476X_ERR_BAD_INTERNAL_MEMORY  = 0x20,
+	SI476X_ERR_BAD_PATCH		= 0x30,
+	SI476X_ERR_BAD_BOOT_MODE	= 0x31,
+	SI476X_ERR_BAD_PROPERTY		= 0x40,
+};
+
+static int si476x_core_parse_and_nag_about_error(struct si476x_core *core)
+{
+	int err;
+	char *cause;
+	u8 buffer[2];
+
+	if (core->revision != SI476X_REVISION_A10) {
+		err = si476x_core_i2c_xfer(core, SI476X_I2C_RECV,
+					   buffer, sizeof(buffer));
+		if (err == sizeof(buffer)) {
+			switch (buffer[1]) {
+			case SI476X_ERR_BAD_COMMAND:
+				cause = "Bad command";
+				err = -EINVAL;
+				break;
+			case SI476X_ERR_BAD_ARG1:
+				cause = "Bad argument #1";
+				err = -EINVAL;
+				break;
+			case SI476X_ERR_BAD_ARG2:
+				cause = "Bad argument #2";
+				err = -EINVAL;
+				break;
+			case SI476X_ERR_BAD_ARG3:
+				cause = "Bad argument #3";
+				err = -EINVAL;
+				break;
+			case SI476X_ERR_BAD_ARG4:
+				cause = "Bad argument #4";
+				err = -EINVAL;
+				break;
+			case SI476X_ERR_BUSY:
+				cause = "Chip is busy";
+				err = -EBUSY;
+				break;
+			case SI476X_ERR_BAD_INTERNAL_MEMORY:
+				cause = "Bad internal memory";
+				err = -EIO;
+				break;
+			case SI476X_ERR_BAD_PATCH:
+				cause = "Bad patch";
+				err = -EINVAL;
+				break;
+			case SI476X_ERR_BAD_BOOT_MODE:
+				cause = "Bad boot mode";
+				err = -EINVAL;
+				break;
+			case SI476X_ERR_BAD_PROPERTY:
+				cause = "Bad property";
+				err = -EINVAL;
+				break;
+			default:
+				cause = "Unknown";
+				err = -EIO;
+			}
+
+			dev_err(&core->client->dev,
+				"[Chip error status]: %s\n", cause);
+		} else {
+			dev_err(&core->client->dev,
+				"Failed to fetch error code\n");
+			err = (err >= 0) ? -EIO : err;
+		}
+	} else {
+		err = -EIO;
+	}
+
+	return err;
+}
+
+/**
+ * si476x_core_send_command() - sends a command to si476x and waits its
+ * response
+ * @core:    si476x_device structure for the device we are
+ *            communicating with
+ * @command:  command id
+ * @args:     command arguments we are sending
+ * @argn:     actual size of @args
+ * @response: buffer to place the expected response from the device
+ * @respn:    actual size of @response
+ * @usecs:    amount of time to wait before reading the response (in
+ *            usecs)
+ *
+ * Function returns 0 on succsess and negative error code on
+ * failure
+ */
+static int si476x_core_send_command(struct si476x_core *core,
+				    const u8 command,
+				    const u8 args[],
+				    const int argn,
+				    u8 resp[],
+				    const int respn,
+				    const int usecs)
+{
+	struct i2c_client *client = core->client;
+	int err;
+	u8  data[CMD_MAX_ARGS_COUNT + 1];
+
+	if (argn > CMD_MAX_ARGS_COUNT) {
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	if (!client->adapter) {
+		err = -ENODEV;
+		goto exit;
+	}
+
+	/* First send the command and its arguments */
+	data[0] = command;
+	memcpy(&data[1], args, argn);
+	dev_dbg(&client->dev, "Command:\n %*ph\n", argn + 1, data);
+
+	err = si476x_core_i2c_xfer(core, SI476X_I2C_SEND,
+				   (char *) data, argn + 1);
+	if (err != argn + 1) {
+		dev_err(&core->client->dev,
+			"Error while sending command 0x%02x\n",
+			command);
+		err = (err >= 0) ? -EIO : err;
+		goto exit;
+	}
+	/* Set CTS to zero only after the command is send to avoid
+	 * possible racing conditions when working in polling mode */
+	atomic_set(&core->cts, 0);
+
+	/* if (unlikely(command == CMD_POWER_DOWN) */
+	if (!wait_event_timeout(core->command,
+				atomic_read(&core->cts),
+				usecs_to_jiffies(usecs) + 1))
+		dev_warn(&core->client->dev,
+			 "(%s) [CMD 0x%02x] Answer timeout.\n",
+			 __func__, command);
+
+	/*
+	  When working in polling mode, for some reason the tuner will
+	  report CTS bit as being set in the first status byte read,
+	  but all the consequtive ones will return zeros until the
+	  tuner is actually completed the POWER_UP command. To
+	  workaround that we wait for second CTS to be reported
+	 */
+	if (unlikely(!core->client->irq && command == CMD_POWER_UP)) {
+		if (!wait_event_timeout(core->command,
+					atomic_read(&core->cts),
+					usecs_to_jiffies(usecs) + 1))
+			dev_warn(&core->client->dev,
+				 "(%s) Power up took too much time.\n",
+				 __func__);
+	}
+
+	/* Then get the response */
+	err = si476x_core_i2c_xfer(core, SI476X_I2C_RECV, resp, respn);
+	if (err != respn) {
+		dev_err(&core->client->dev,
+			"Error while reading response for command 0x%02x\n",
+			command);
+		err = (err >= 0) ? -EIO : err;
+		goto exit;
+	}
+	dev_dbg(&client->dev, "Response:\n %*ph\n", respn, resp);
+
+	err = 0;
+
+	if (resp[0] & SI476X_ERR) {
+		dev_err(&core->client->dev,
+			"[CMD 0x%02x] Chip set error flag\n", command);
+		err = si476x_core_parse_and_nag_about_error(core);
+		goto exit;
+	}
+
+	if (!(resp[0] & SI476X_CTS))
+		err = -EBUSY;
+exit:
+	return err;
+}
+
+static int si476x_cmd_clear_stc(struct si476x_core *core)
+{
+	int err;
+	struct si476x_rsq_status_args args = {
+		.primary	= false,
+		.rsqack		= false,
+		.attune		= false,
+		.cancel		= false,
+		.stcack		= true,
+	};
+
+	switch (core->power_up_parameters.func) {
+	case SI476X_FUNC_FM_RECEIVER:
+		err = si476x_core_cmd_fm_rsq_status(core, &args, NULL);
+		break;
+	case SI476X_FUNC_AM_RECEIVER:
+		err = si476x_core_cmd_am_rsq_status(core, &args, NULL);
+		break;
+	default:
+		err = -EINVAL;
+	}
+
+	return err;
+}
+
+static int si476x_cmd_tune_seek_freq(struct si476x_core *core,
+				     uint8_t cmd,
+				     const uint8_t args[], size_t argn,
+				     uint8_t *resp, size_t respn)
+{
+	int err;
+
+
+	atomic_set(&core->stc, 0);
+	err = si476x_core_send_command(core, cmd, args, argn, resp, respn,
+				       SI476X_TIMEOUT_TUNE);
+	if (!err) {
+		wait_event_killable(core->tuning,
+				    atomic_read(&core->stc));
+		si476x_cmd_clear_stc(core);
+	}
+
+	return err;
+}
+
+/**
+ * si476x_cmd_func_info() - send 'FUNC_INFO' command to the device
+ * @core: device to send the command to
+ * @info:  struct si476x_func_info to fill all the information
+ *         returned by the command
+ *
+ * The command requests the firmware and patch version for currently
+ * loaded firmware (dependent on the function of the device FM/AM/WB)
+ *
+ * Function returns 0 on succsess and negative error code on
+ * failure
+ */
+int si476x_core_cmd_func_info(struct si476x_core *core,
+			      struct si476x_func_info *info)
+{
+	int err;
+	u8  resp[CMD_FUNC_INFO_NRESP];
+
+	err = si476x_core_send_command(core, CMD_FUNC_INFO,
+				       NULL, 0,
+				       resp, ARRAY_SIZE(resp),
+				       SI476X_DEFAULT_TIMEOUT);
+
+	info->firmware.major    = resp[1];
+	info->firmware.minor[0] = resp[2];
+	info->firmware.minor[1] = resp[3];
+
+	info->patch_id = ((u16) resp[4] << 8) | resp[5];
+	info->func     = resp[6];
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_func_info);
+
+/**
+ * si476x_cmd_set_property() - send 'SET_PROPERTY' command to the device
+ * @core:    device to send the command to
+ * @property: property address
+ * @value:    property value
+ *
+ * Function returns 0 on succsess and negative error code on
+ * failure
+ */
+int si476x_core_cmd_set_property(struct si476x_core *core,
+				 u16 property, u16 value)
+{
+	u8       resp[CMD_SET_PROPERTY_NRESP];
+	const u8 args[CMD_SET_PROPERTY_NARGS] = {
+		0x00,
+		msb(property),
+		lsb(property),
+		msb(value),
+		lsb(value),
+	};
+
+	return si476x_core_send_command(core, CMD_SET_PROPERTY,
+					args, ARRAY_SIZE(args),
+					resp, ARRAY_SIZE(resp),
+					SI476X_DEFAULT_TIMEOUT);
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_set_property);
+
+/**
+ * si476x_cmd_get_property() - send 'GET_PROPERTY' command to the device
+ * @core:    device to send the command to
+ * @property: property address
+ *
+ * Function return the value of property as u16 on success or a
+ * negative error on failure
+ */
+int si476x_core_cmd_get_property(struct si476x_core *core, u16 property)
+{
+	int err;
+	u8       resp[CMD_GET_PROPERTY_NRESP];
+	const u8 args[CMD_GET_PROPERTY_NARGS] = {
+		0x00,
+		msb(property),
+		lsb(property),
+	};
+
+	err = si476x_core_send_command(core, CMD_GET_PROPERTY,
+				       args, ARRAY_SIZE(args),
+				       resp, ARRAY_SIZE(resp),
+				       SI476X_DEFAULT_TIMEOUT);
+	if (err < 0)
+		return err;
+	else
+		return be16_to_cpup((__be16 *)(resp + 2));
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_get_property);
+
+/**
+ * si476x_cmd_dig_audio_pin_cfg() - send 'DIG_AUDIO_PIN_CFG' command to
+ * the device
+ * @core: device to send the command to
+ * @dclk:  DCLK pin function configuration:
+ *	   #SI476X_DCLK_NOOP     - do not modify the behaviour
+ *         #SI476X_DCLK_TRISTATE - put the pin in tristate condition,
+ *                                 enable 1MOhm pulldown
+ *         #SI476X_DCLK_DAUDIO   - set the pin to be a part of digital
+ *                                 audio interface
+ * @dfs:   DFS pin function configuration:
+ *         #SI476X_DFS_NOOP      - do not modify the behaviour
+ *         #SI476X_DFS_TRISTATE  - put the pin in tristate condition,
+ *                             enable 1MOhm pulldown
+ *      SI476X_DFS_DAUDIO    - set the pin to be a part of digital
+ *                             audio interface
+ * @dout - DOUT pin function configuration:
+ *      SI476X_DOUT_NOOP       - do not modify the behaviour
+ *      SI476X_DOUT_TRISTATE   - put the pin in tristate condition,
+ *                               enable 1MOhm pulldown
+ *      SI476X_DOUT_I2S_OUTPUT - set this pin to be digital out on I2S
+ *                               port 1
+ *      SI476X_DOUT_I2S_INPUT  - set this pin to be digital in on I2S
+ *                               port 1
+ * @xout - XOUT pin function configuration:
+ *	SI476X_XOUT_NOOP        - do not modify the behaviour
+ *      SI476X_XOUT_TRISTATE    - put the pin in tristate condition,
+ *                                enable 1MOhm pulldown
+ *      SI476X_XOUT_I2S_INPUT   - set this pin to be digital in on I2S
+ *                                port 1
+ *      SI476X_XOUT_MODE_SELECT - set this pin to be the input that
+ *                                selects the mode of the I2S audio
+ *                                combiner (analog or HD)
+ *                                [SI4761/63/65/67 Only]
+ *
+ * Function returns 0 on success and negative error code on failure
+ */
+int si476x_core_cmd_dig_audio_pin_cfg(struct  si476x_core *core,
+				      enum si476x_dclk_config dclk,
+				      enum si476x_dfs_config  dfs,
+				      enum si476x_dout_config dout,
+				      enum si476x_xout_config xout)
+{
+	u8       resp[CMD_DIG_AUDIO_PIN_CFG_NRESP];
+	const u8 args[CMD_DIG_AUDIO_PIN_CFG_NARGS] = {
+		PIN_CFG_BYTE(dclk),
+		PIN_CFG_BYTE(dfs),
+		PIN_CFG_BYTE(dout),
+		PIN_CFG_BYTE(xout),
+	};
+
+	return si476x_core_send_command(core, CMD_DIG_AUDIO_PIN_CFG,
+					args, ARRAY_SIZE(args),
+					resp, ARRAY_SIZE(resp),
+					SI476X_DEFAULT_TIMEOUT);
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_dig_audio_pin_cfg);
+
+/**
+ * si476x_cmd_zif_pin_cfg - send 'ZIF_PIN_CFG_COMMAND'
+ * @core - device to send the command to
+ * @iqclk - IQCL pin function configuration:
+ *       SI476X_IQCLK_NOOP     - do not modify the behaviour
+ *       SI476X_IQCLK_TRISTATE - put the pin in tristate condition,
+ *                               enable 1MOhm pulldown
+ *       SI476X_IQCLK_IQ       - set pin to be a part of I/Q interace
+ *                               in master mode
+ * @iqfs - IQFS pin function configuration:
+ *       SI476X_IQFS_NOOP     - do not modify the behaviour
+ *       SI476X_IQFS_TRISTATE - put the pin in tristate condition,
+ *                              enable 1MOhm pulldown
+ *       SI476X_IQFS_IQ       - set pin to be a part of I/Q interace
+ *                              in master mode
+ * @iout - IOUT pin function configuration:
+ *       SI476X_IOUT_NOOP     - do not modify the behaviour
+ *       SI476X_IOUT_TRISTATE - put the pin in tristate condition,
+ *                              enable 1MOhm pulldown
+ *       SI476X_IOUT_OUTPUT   - set pin to be I out
+ * @qout - QOUT pin function configuration:
+ *       SI476X_QOUT_NOOP     - do not modify the behaviour
+ *       SI476X_QOUT_TRISTATE - put the pin in tristate condition,
+ *                              enable 1MOhm pulldown
+ *       SI476X_QOUT_OUTPUT   - set pin to be Q out
+ *
+ * Function returns 0 on success and negative error code on failure
+ */
+int si476x_core_cmd_zif_pin_cfg(struct si476x_core *core,
+				enum si476x_iqclk_config iqclk,
+				enum si476x_iqfs_config iqfs,
+				enum si476x_iout_config iout,
+				enum si476x_qout_config qout)
+{
+	u8       resp[CMD_ZIF_PIN_CFG_NRESP];
+	const u8 args[CMD_ZIF_PIN_CFG_NARGS] = {
+		PIN_CFG_BYTE(iqclk),
+		PIN_CFG_BYTE(iqfs),
+		PIN_CFG_BYTE(iout),
+		PIN_CFG_BYTE(qout),
+	};
+
+	return si476x_core_send_command(core, CMD_ZIF_PIN_CFG,
+					args, ARRAY_SIZE(args),
+					resp, ARRAY_SIZE(resp),
+					SI476X_DEFAULT_TIMEOUT);
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_zif_pin_cfg);
+
+/**
+ * si476x_cmd_ic_link_gpo_ctl_pin_cfg - send
+ * 'IC_LINK_GPIO_CTL_PIN_CFG' comand to the device
+ * @core - device to send the command to
+ * @icin - ICIN pin function configuration:
+ *      SI476X_ICIN_NOOP      - do not modify the behaviour
+ *      SI476X_ICIN_TRISTATE  - put the pin in tristate condition,
+ *                              enable 1MOhm pulldown
+ *      SI476X_ICIN_GPO1_HIGH - set pin to be an output, drive it high
+ *      SI476X_ICIN_GPO1_LOW  - set pin to be an output, drive it low
+ *      SI476X_ICIN_IC_LINK   - set the pin to be a part of Inter-Chip link
+ * @icip - ICIP pin function configuration:
+ *      SI476X_ICIP_NOOP      - do not modify the behaviour
+ *      SI476X_ICIP_TRISTATE  - put the pin in tristate condition,
+ *                              enable 1MOhm pulldown
+ *      SI476X_ICIP_GPO1_HIGH - set pin to be an output, drive it high
+ *      SI476X_ICIP_GPO1_LOW  - set pin to be an output, drive it low
+ *      SI476X_ICIP_IC_LINK   - set the pin to be a part of Inter-Chip link
+ * @icon - ICON pin function configuration:
+ *      SI476X_ICON_NOOP     - do not modify the behaviour
+ *      SI476X_ICON_TRISTATE - put the pin in tristate condition,
+ *                             enable 1MOhm pulldown
+ *      SI476X_ICON_I2S      - set the pin to be a part of audio
+ *                             interface in slave mode (DCLK)
+ *      SI476X_ICON_IC_LINK  - set the pin to be a part of Inter-Chip link
+ * @icop - ICOP pin function configuration:
+ *      SI476X_ICOP_NOOP     - do not modify the behaviour
+ *      SI476X_ICOP_TRISTATE - put the pin in tristate condition,
+ *                             enable 1MOhm pulldown
+ *      SI476X_ICOP_I2S      - set the pin to be a part of audio
+ *                             interface in slave mode (DOUT)
+ *                             [Si4761/63/65/67 Only]
+ *      SI476X_ICOP_IC_LINK  - set the pin to be a part of Inter-Chip link
+ *
+ * Function returns 0 on success and negative error code on failure
+ */
+int si476x_core_cmd_ic_link_gpo_ctl_pin_cfg(struct si476x_core *core,
+					    enum si476x_icin_config icin,
+					    enum si476x_icip_config icip,
+					    enum si476x_icon_config icon,
+					    enum si476x_icop_config icop)
+{
+	u8       resp[CMD_IC_LINK_GPO_CTL_PIN_CFG_NRESP];
+	const u8 args[CMD_IC_LINK_GPO_CTL_PIN_CFG_NARGS] = {
+		PIN_CFG_BYTE(icin),
+		PIN_CFG_BYTE(icip),
+		PIN_CFG_BYTE(icon),
+		PIN_CFG_BYTE(icop),
+	};
+
+	return si476x_core_send_command(core, CMD_IC_LINK_GPO_CTL_PIN_CFG,
+					args, ARRAY_SIZE(args),
+					resp, ARRAY_SIZE(resp),
+					SI476X_DEFAULT_TIMEOUT);
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_ic_link_gpo_ctl_pin_cfg);
+
+/**
+ * si476x_cmd_ana_audio_pin_cfg - send 'ANA_AUDIO_PIN_CFG' to the
+ * device
+ * @core - device to send the command to
+ * @lrout - LROUT pin function configuration:
+ *       SI476X_LROUT_NOOP     - do not modify the behaviour
+ *       SI476X_LROUT_TRISTATE - put the pin in tristate condition,
+ *                               enable 1MOhm pulldown
+ *       SI476X_LROUT_AUDIO    - set pin to be audio output
+ *       SI476X_LROUT_MPX      - set pin to be MPX output
+ *
+ * Function returns 0 on success and negative error code on failure
+ */
+int si476x_core_cmd_ana_audio_pin_cfg(struct si476x_core *core,
+				      enum si476x_lrout_config lrout)
+{
+	u8       resp[CMD_ANA_AUDIO_PIN_CFG_NRESP];
+	const u8 args[CMD_ANA_AUDIO_PIN_CFG_NARGS] = {
+		PIN_CFG_BYTE(lrout),
+	};
+
+	return si476x_core_send_command(core, CMD_ANA_AUDIO_PIN_CFG,
+					args, ARRAY_SIZE(args),
+					resp, ARRAY_SIZE(resp),
+					SI476X_DEFAULT_TIMEOUT);
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_ana_audio_pin_cfg);
+
+
+/**
+ * si476x_cmd_intb_pin_cfg - send 'INTB_PIN_CFG' command to the device
+ * @core - device to send the command to
+ * @intb - INTB pin function configuration:
+ *      SI476X_INTB_NOOP     - do not modify the behaviour
+ *      SI476X_INTB_TRISTATE - put the pin in tristate condition,
+ *                             enable 1MOhm pulldown
+ *      SI476X_INTB_DAUDIO   - set pin to be a part of digital
+ *                             audio interface in slave mode
+ *      SI476X_INTB_IRQ      - set pin to be an interrupt request line
+ * @a1 - A1 pin function configuration:
+ *      SI476X_A1_NOOP     - do not modify the behaviour
+ *      SI476X_A1_TRISTATE - put the pin in tristate condition,
+ *                           enable 1MOhm pulldown
+ *      SI476X_A1_IRQ      - set pin to be an interrupt request line
+ *
+ * Function returns 0 on success and negative error code on failure
+ */
+static int si476x_core_cmd_intb_pin_cfg_a10(struct si476x_core *core,
+					    enum si476x_intb_config intb,
+					    enum si476x_a1_config a1)
+{
+	u8       resp[CMD_INTB_PIN_CFG_A10_NRESP];
+	const u8 args[CMD_INTB_PIN_CFG_NARGS] = {
+		PIN_CFG_BYTE(intb),
+		PIN_CFG_BYTE(a1),
+	};
+
+	return si476x_core_send_command(core, CMD_INTB_PIN_CFG,
+					args, ARRAY_SIZE(args),
+					resp, ARRAY_SIZE(resp),
+					SI476X_DEFAULT_TIMEOUT);
+}
+
+static int si476x_core_cmd_intb_pin_cfg_a20(struct si476x_core *core,
+					    enum si476x_intb_config intb,
+					    enum si476x_a1_config a1)
+{
+	u8       resp[CMD_INTB_PIN_CFG_A20_NRESP];
+	const u8 args[CMD_INTB_PIN_CFG_NARGS] = {
+		PIN_CFG_BYTE(intb),
+		PIN_CFG_BYTE(a1),
+	};
+
+	return si476x_core_send_command(core, CMD_INTB_PIN_CFG,
+					args, ARRAY_SIZE(args),
+					resp, ARRAY_SIZE(resp),
+					SI476X_DEFAULT_TIMEOUT);
+}
+
+
+
+/**
+ * si476x_cmd_am_rsq_status - send 'AM_RSQ_STATUS' command to the
+ * device
+ * @core  - device to send the command to
+ * @rsqack - if set command clears RSQINT, SNRINT, SNRLINT, RSSIHINT,
+ *           RSSSILINT, BLENDINT, MULTHINT and MULTLINT
+ * @attune - when set the values in the status report are the values
+ *           that were calculated at tune
+ * @cancel - abort ongoing seek/tune opertation
+ * @stcack - clear the STCINT bin in status register
+ * @report - all signal quality information retured by the command
+ *           (if NULL then the output of the command is ignored)
+ *
+ * Function returns 0 on success and negative error code on failure
+ */
+int si476x_core_cmd_am_rsq_status(struct si476x_core *core,
+				  struct si476x_rsq_status_args *rsqargs,
+				  struct si476x_rsq_status_report *report)
+{
+	int err;
+	u8       resp[CMD_AM_RSQ_STATUS_NRESP];
+	const u8 args[CMD_AM_RSQ_STATUS_NARGS] = {
+		rsqargs->rsqack << 3 | rsqargs->attune << 2 |
+		rsqargs->cancel << 1 | rsqargs->stcack,
+	};
+
+	err = si476x_core_send_command(core, CMD_AM_RSQ_STATUS,
+				       args, ARRAY_SIZE(args),
+				       resp, ARRAY_SIZE(resp),
+				       SI476X_DEFAULT_TIMEOUT);
+	/*
+	 * Besides getting received signal quality information this
+	 * command can be used to just acknowledge different interrupt
+	 * flags in those cases it is useless to copy and parse
+	 * received data so user can pass NULL, and thus avoid
+	 * unnecessary copying.
+	 */
+	if (!report)
+		return err;
+
+	report->snrhint		= 0b00001000 & resp[1];
+	report->snrlint		= 0b00000100 & resp[1];
+	report->rssihint	= 0b00000010 & resp[1];
+	report->rssilint	= 0b00000001 & resp[1];
+
+	report->bltf		= 0b10000000 & resp[2];
+	report->snr_ready	= 0b00100000 & resp[2];
+	report->rssiready	= 0b00001000 & resp[2];
+	report->afcrl		= 0b00000010 & resp[2];
+	report->valid		= 0b00000001 & resp[2];
+
+	report->readfreq	= be16_to_cpup((__be16 *)(resp + 3));
+	report->freqoff		= resp[5];
+	report->rssi		= resp[6];
+	report->snr		= resp[7];
+	report->lassi		= resp[9];
+	report->hassi		= resp[10];
+	report->mult		= resp[11];
+	report->dev		= resp[12];
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_am_rsq_status);
+
+int si476x_core_cmd_fm_acf_status(struct si476x_core *core,
+			     struct si476x_acf_status_report *report)
+{
+	int err;
+	u8       resp[CMD_FM_ACF_STATUS_NRESP];
+	const u8 args[CMD_FM_ACF_STATUS_NARGS] = {
+		0x0,
+	};
+
+	if (!report)
+		return -EINVAL;
+
+	err = si476x_core_send_command(core, CMD_FM_ACF_STATUS,
+				       args, ARRAY_SIZE(args),
+				       resp, ARRAY_SIZE(resp),
+				       SI476X_DEFAULT_TIMEOUT);
+	if (err < 0)
+		return err;
+
+	report->blend_int	= resp[1] & SI476X_ACF_BLEND_INT;
+	report->hblend_int	= resp[1] & SI476X_ACF_HIBLEND_INT;
+	report->hicut_int	= resp[1] & SI476X_ACF_HICUT_INT;
+	report->chbw_int	= resp[1] & SI476X_ACF_CHBW_INT;
+	report->softmute_int	= resp[1] & SI476X_ACF_SOFTMUTE_INT;
+	report->smute		= resp[2] & SI476X_ACF_SMUTE;
+	report->smattn		= resp[3] & SI476X_ACF_SMATTN;
+	report->chbw		= resp[4];
+	report->hicut		= resp[5];
+	report->hiblend		= resp[6];
+	report->pilot		= resp[7] & SI476X_ACF_PILOT;
+	report->stblend		= resp[7] & SI476X_ACF_STBLEND;
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_fm_acf_status);
+
+int si476x_core_cmd_am_acf_status(struct si476x_core *core,
+				  struct si476x_acf_status_report *report)
+{
+	int err;
+	u8       resp[CMD_AM_ACF_STATUS_NRESP];
+	const u8 args[CMD_AM_ACF_STATUS_NARGS] = {
+		0x0,
+	};
+
+	if (!report)
+		return -EINVAL;
+
+	err = si476x_core_send_command(core, CMD_AM_ACF_STATUS,
+				       args, ARRAY_SIZE(args),
+				       resp, ARRAY_SIZE(resp),
+				       SI476X_DEFAULT_TIMEOUT);
+	if (err < 0)
+		return err;
+
+	report->blend_int	= resp[1] & SI476X_ACF_BLEND_INT;
+	report->hblend_int	= resp[1] & SI476X_ACF_HIBLEND_INT;
+	report->hicut_int	= resp[1] & SI476X_ACF_HICUT_INT;
+	report->chbw_int	= resp[1] & SI476X_ACF_CHBW_INT;
+	report->softmute_int	= resp[1] & SI476X_ACF_SOFTMUTE_INT;
+	report->smute		= resp[2] & SI476X_ACF_SMUTE;
+	report->smattn		= resp[3] & SI476X_ACF_SMATTN;
+	report->chbw		= resp[4];
+	report->hicut		= resp[5];
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_am_acf_status);
+
+
+/**
+ * si476x_cmd_fm_seek_start - send 'FM_SEEK_START' command to the
+ * device
+ * @core  - device to send the command to
+ * @seekup - if set the direction of the search is 'up'
+ * @wrap   - if set seek wraps when hitting band limit
+ *
+ * This function begins search for a valid station. The station is
+ * considered valid when 'FM_VALID_SNR_THRESHOLD' and
+ * 'FM_VALID_RSSI_THRESHOLD' and 'FM_VALID_MAX_TUNE_ERROR' criteria
+ * are met.
+} *
+ * Function returns 0 on success and negative error code on failure
+ */
+int si476x_core_cmd_fm_seek_start(struct si476x_core *core,
+				  bool seekup, bool wrap)
+{
+	u8       resp[CMD_FM_SEEK_START_NRESP];
+	const u8 args[CMD_FM_SEEK_START_NARGS] = {
+		seekup << 3 | wrap << 2,
+	};
+
+	return si476x_cmd_tune_seek_freq(core, CMD_FM_SEEK_START,
+					 args, sizeof(args),
+					 resp, sizeof(resp));
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_fm_seek_start);
+
+/**
+ * si476x_cmd_fm_rds_status - send 'FM_RDS_STATUS' command to the
+ * device
+ * @core - device to send the command to
+ * @status_only - if set the data is not removed from RDSFIFO,
+ *                RDSFIFOUSED is not decremented and data in all the
+ *                rest RDS data contains the last valid info received
+ * @mtfifo if set the command clears RDS receive FIFO
+ * @intack if set the command clards the RDSINT bit.
+ *
+ * Function returns 0 on success and negative error code on failure
+ */
+int si476x_core_cmd_fm_rds_status(struct si476x_core *core,
+				  bool status_only,
+				  bool mtfifo,
+				  bool intack,
+				  struct si476x_rds_status_report *report)
+{
+	int err;
+	u8       resp[CMD_FM_RDS_STATUS_NRESP];
+	const u8 args[CMD_FM_RDS_STATUS_NARGS] = {
+		status_only << 2 | mtfifo << 1 | intack,
+	};
+
+	err = si476x_core_send_command(core, CMD_FM_RDS_STATUS,
+				       args, ARRAY_SIZE(args),
+				       resp, ARRAY_SIZE(resp),
+				       SI476X_DEFAULT_TIMEOUT);
+	/*
+	 * Besides getting RDS status information this command can be
+	 * used to just acknowledge different interrupt flags in those
+	 * cases it is useless to copy and parse received data so user
+	 * can pass NULL, and thus avoid unnecessary copying.
+	 */
+	if (err < 0 || report == NULL)
+		return err;
+
+	report->rdstpptyint	= 0b00010000 & resp[1];
+	report->rdspiint	= 0b00001000 & resp[1];
+	report->rdssyncint	= 0b00000010 & resp[1];
+	report->rdsfifoint	= 0b00000001 & resp[1];
+
+	report->tpptyvalid	= 0b00010000 & resp[2];
+	report->pivalid		= 0b00001000 & resp[2];
+	report->rdssync		= 0b00000010 & resp[2];
+	report->rdsfifolost	= 0b00000001 & resp[2];
+
+	report->tp		= 0b00100000 & resp[3];
+	report->pty		= 0b00011111 & resp[3];
+
+	report->pi		= be16_to_cpup((__be16 *)(resp + 4));
+	report->rdsfifoused	= resp[6];
+
+	report->ble[V4L2_RDS_BLOCK_A]	= 0b11000000 & resp[7];
+	report->ble[V4L2_RDS_BLOCK_B]	= 0b00110000 & resp[7];
+	report->ble[V4L2_RDS_BLOCK_C]	= 0b00001100 & resp[7];
+	report->ble[V4L2_RDS_BLOCK_D]	= 0b00000011 & resp[7];
+
+	report->rds[V4L2_RDS_BLOCK_A].block = V4L2_RDS_BLOCK_A;
+	report->rds[V4L2_RDS_BLOCK_A].msb = resp[8];
+	report->rds[V4L2_RDS_BLOCK_A].lsb = resp[9];
+
+	report->rds[V4L2_RDS_BLOCK_B].block = V4L2_RDS_BLOCK_B;
+	report->rds[V4L2_RDS_BLOCK_B].msb = resp[10];
+	report->rds[V4L2_RDS_BLOCK_B].lsb = resp[11];
+
+	report->rds[V4L2_RDS_BLOCK_C].block = V4L2_RDS_BLOCK_C;
+	report->rds[V4L2_RDS_BLOCK_C].msb = resp[12];
+	report->rds[V4L2_RDS_BLOCK_C].lsb = resp[13];
+
+	report->rds[V4L2_RDS_BLOCK_D].block = V4L2_RDS_BLOCK_D;
+	report->rds[V4L2_RDS_BLOCK_D].msb = resp[14];
+	report->rds[V4L2_RDS_BLOCK_D].lsb = resp[15];
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_fm_rds_status);
+
+int si476x_core_cmd_fm_rds_blockcount(struct si476x_core *core,
+				bool clear,
+				struct si476x_rds_blockcount_report *report)
+{
+	int err;
+	u8       resp[CMD_FM_RDS_BLOCKCOUNT_NRESP];
+	const u8 args[CMD_FM_RDS_BLOCKCOUNT_NARGS] = {
+		clear,
+	};
+
+	if (!report)
+		return -EINVAL;
+
+	err = si476x_core_send_command(core, CMD_FM_RDS_BLOCKCOUNT,
+				       args, ARRAY_SIZE(args),
+				       resp, ARRAY_SIZE(resp),
+				       SI476X_DEFAULT_TIMEOUT);
+
+	if (!err) {
+		report->expected	= be16_to_cpup((__be16 *)(resp + 2));
+		report->received	= be16_to_cpup((__be16 *)(resp + 4));
+		report->uncorrectable	= be16_to_cpup((__be16 *)(resp + 6));
+	}
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_fm_rds_blockcount);
+
+int si476x_core_cmd_fm_phase_diversity(struct si476x_core *core,
+				       enum si476x_phase_diversity_mode mode)
+{
+	u8       resp[CMD_FM_PHASE_DIVERSITY_NRESP];
+	const u8 args[CMD_FM_PHASE_DIVERSITY_NARGS] = {
+		mode & 0b111,
+	};
+
+	return si476x_core_send_command(core, CMD_FM_PHASE_DIVERSITY,
+					args, ARRAY_SIZE(args),
+					resp, ARRAY_SIZE(resp),
+					SI476X_DEFAULT_TIMEOUT);
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_fm_phase_diversity);
+/**
+ * si476x_core_cmd_fm_phase_div_status() - get the phase diversity
+ * status
+ *
+ * @core: si476x device
+ *
+ * NOTE caller must hold core lock
+ *
+ * Function returns the value of the status bit in case of success and
+ * negative error code in case of failre.
+ */
+int si476x_core_cmd_fm_phase_div_status(struct si476x_core *core)
+{
+	int err;
+	u8 resp[CMD_FM_PHASE_DIV_STATUS_NRESP];
+
+	err = si476x_core_send_command(core, CMD_FM_PHASE_DIV_STATUS,
+				       NULL, 0,
+				       resp, ARRAY_SIZE(resp),
+				       SI476X_DEFAULT_TIMEOUT);
+
+	return (err < 0) ? err : resp[1];
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_fm_phase_div_status);
+
+
+/**
+ * si476x_cmd_am_seek_start - send 'FM_SEEK_START' command to the
+ * device
+ * @core  - device to send the command to
+ * @seekup - if set the direction of the search is 'up'
+ * @wrap   - if set seek wraps when hitting band limit
+ *
+ * This function begins search for a valid station. The station is
+ * considered valid when 'FM_VALID_SNR_THRESHOLD' and
+ * 'FM_VALID_RSSI_THRESHOLD' and 'FM_VALID_MAX_TUNE_ERROR' criteria
+ * are met.
+ *
+ * Function returns 0 on success and negative error code on failure
+ */
+int si476x_core_cmd_am_seek_start(struct si476x_core *core,
+				  bool seekup, bool wrap)
+{
+	u8       resp[CMD_AM_SEEK_START_NRESP];
+	const u8 args[CMD_AM_SEEK_START_NARGS] = {
+		seekup << 3 | wrap << 2,
+	};
+
+	return si476x_cmd_tune_seek_freq(core,  CMD_AM_SEEK_START,
+					 args, sizeof(args),
+					 resp, sizeof(resp));
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_am_seek_start);
+
+
+
+static int si476x_core_cmd_power_up_a10(struct si476x_core *core,
+					struct si476x_power_up_args *puargs)
+{
+	u8       resp[CMD_POWER_UP_A10_NRESP];
+	const bool intsel = (core->pinmux.a1 == SI476X_A1_IRQ);
+	const bool ctsen  = (core->client->irq != 0);
+	const u8 args[CMD_POWER_UP_A10_NARGS] = {
+		0xF7,		/* Reserved, always 0xF7 */
+		0x3F & puargs->xcload,	/* First two bits are reserved to be
+				 * zeros */
+		ctsen << 7 | intsel << 6 | 0x07, /* Last five bits
+						   * are reserved to
+						   * be written as 0x7 */
+		puargs->func << 4 | puargs->freq,
+		0x11,		/* Reserved, always 0x11 */
+	};
+
+	return si476x_core_send_command(core, CMD_POWER_UP,
+					args, ARRAY_SIZE(args),
+					resp, ARRAY_SIZE(resp),
+					SI476X_TIMEOUT_POWER_UP);
+}
+
+static int si476x_core_cmd_power_up_a20(struct si476x_core *core,
+				 struct si476x_power_up_args *puargs)
+{
+	u8       resp[CMD_POWER_UP_A20_NRESP];
+	const bool intsel = (core->pinmux.a1 == SI476X_A1_IRQ);
+	const bool ctsen  = (core->client->irq != 0);
+	const u8 args[CMD_POWER_UP_A20_NARGS] = {
+		puargs->ibias6x << 7 | puargs->xstart,
+		0x3F & puargs->xcload,	/* First two bits are reserved to be
+					 * zeros */
+		ctsen << 7 | intsel << 6 | puargs->fastboot << 5 |
+		puargs->xbiashc << 3 | puargs->xbias,
+		puargs->func << 4 | puargs->freq,
+		0x10 | puargs->xmode,
+	};
+
+	return si476x_core_send_command(core, CMD_POWER_UP,
+					args, ARRAY_SIZE(args),
+					resp, ARRAY_SIZE(resp),
+					SI476X_TIMEOUT_POWER_UP);
+}
+
+static int si476x_core_cmd_power_down_a10(struct si476x_core *core,
+					  struct si476x_power_down_args *pdargs)
+{
+	u8 resp[CMD_POWER_DOWN_A10_NRESP];
+
+	return si476x_core_send_command(core, CMD_POWER_DOWN,
+					NULL, 0,
+					resp, ARRAY_SIZE(resp),
+					SI476X_DEFAULT_TIMEOUT);
+}
+
+static int si476x_core_cmd_power_down_a20(struct si476x_core *core,
+					  struct si476x_power_down_args *pdargs)
+{
+	u8 resp[CMD_POWER_DOWN_A20_NRESP];
+	const u8 args[CMD_POWER_DOWN_A20_NARGS] = {
+		pdargs->xosc,
+	};
+	return si476x_core_send_command(core, CMD_POWER_DOWN,
+					args, ARRAY_SIZE(args),
+					resp, ARRAY_SIZE(resp),
+					SI476X_DEFAULT_TIMEOUT);
+}
+
+static int si476x_core_cmd_am_tune_freq_a10(struct si476x_core *core,
+					struct si476x_tune_freq_args *tuneargs)
+{
+
+	const int am_freq = tuneargs->freq;
+	u8       resp[CMD_AM_TUNE_FREQ_NRESP];
+	const u8 args[CMD_AM_TUNE_FREQ_NARGS] = {
+		(tuneargs->hd << 6),
+		msb(am_freq),
+		lsb(am_freq),
+	};
+
+	return si476x_cmd_tune_seek_freq(core, CMD_AM_TUNE_FREQ, args,
+					 sizeof(args),
+					 resp, sizeof(resp));
+}
+
+static int si476x_core_cmd_am_tune_freq_a20(struct si476x_core *core,
+					struct si476x_tune_freq_args *tuneargs)
+{
+	const int am_freq = tuneargs->freq;
+	u8       resp[CMD_AM_TUNE_FREQ_NRESP];
+	const u8 args[CMD_AM_TUNE_FREQ_NARGS] = {
+		(tuneargs->zifsr << 6) | (tuneargs->injside & 0b11),
+		msb(am_freq),
+		lsb(am_freq),
+	};
+
+	return si476x_cmd_tune_seek_freq(core, CMD_AM_TUNE_FREQ,
+					 args, sizeof(args),
+					 resp, sizeof(resp));
+}
+
+static int si476x_core_cmd_fm_rsq_status_a10(struct si476x_core *core,
+					struct si476x_rsq_status_args *rsqargs,
+					struct si476x_rsq_status_report *report)
+{
+	int err;
+	u8       resp[CMD_FM_RSQ_STATUS_A10_NRESP];
+	const u8 args[CMD_FM_RSQ_STATUS_A10_NARGS] = {
+		rsqargs->rsqack << 3 | rsqargs->attune << 2 |
+		rsqargs->cancel << 1 | rsqargs->stcack,
+	};
+
+	err = si476x_core_send_command(core, CMD_FM_RSQ_STATUS,
+				       args, ARRAY_SIZE(args),
+				       resp, ARRAY_SIZE(resp),
+				       SI476X_DEFAULT_TIMEOUT);
+	/*
+	 * Besides getting received signal quality information this
+	 * command can be used to just acknowledge different interrupt
+	 * flags in those cases it is useless to copy and parse
+	 * received data so user can pass NULL, and thus avoid
+	 * unnecessary copying.
+	 */
+	if (err < 0 || report == NULL)
+		return err;
+
+	report->multhint	= 0b10000000 & resp[1];
+	report->multlint	= 0b01000000 & resp[1];
+	report->snrhint		= 0b00001000 & resp[1];
+	report->snrlint		= 0b00000100 & resp[1];
+	report->rssihint	= 0b00000010 & resp[1];
+	report->rssilint	= 0b00000001 & resp[1];
+
+	report->bltf		= 0b10000000 & resp[2];
+	report->snr_ready	= 0b00100000 & resp[2];
+	report->rssiready	= 0b00001000 & resp[2];
+	report->afcrl		= 0b00000010 & resp[2];
+	report->valid		= 0b00000001 & resp[2];
+
+	report->readfreq	= be16_to_cpup((__be16 *)(resp + 3));
+	report->freqoff		= resp[5];
+	report->rssi		= resp[6];
+	report->snr		= resp[7];
+	report->lassi		= resp[9];
+	report->hassi		= resp[10];
+	report->mult		= resp[11];
+	report->dev		= resp[12];
+	report->readantcap	= be16_to_cpup((__be16 *)(resp + 13));
+	report->assi		= resp[15];
+	report->usn		= resp[16];
+
+	return err;
+}
+
+static int si476x_core_cmd_fm_rsq_status_a20(struct si476x_core *core,
+					     struct si476x_rsq_status_args *rsqargs,
+					     struct si476x_rsq_status_report *report)
+{
+	int err;
+	u8       resp[CMD_FM_RSQ_STATUS_A10_NRESP];
+	const u8 args[CMD_FM_RSQ_STATUS_A30_NARGS] = {
+		rsqargs->primary << 4 | rsqargs->rsqack << 3 |
+		rsqargs->attune  << 2 | rsqargs->cancel << 1 |
+		rsqargs->stcack,
+	};
+
+	err = si476x_core_send_command(core, CMD_FM_RSQ_STATUS,
+				       args, ARRAY_SIZE(args),
+				       resp, ARRAY_SIZE(resp),
+				       SI476X_DEFAULT_TIMEOUT);
+	/*
+	 * Besides getting received signal quality information this
+	 * command can be used to just acknowledge different interrupt
+	 * flags in those cases it is useless to copy and parse
+	 * received data so user can pass NULL, and thus avoid
+	 * unnecessary copying.
+	 */
+	if (err < 0 || report == NULL)
+		return err;
+
+	report->multhint	= 0b10000000 & resp[1];
+	report->multlint	= 0b01000000 & resp[1];
+	report->snrhint		= 0b00001000 & resp[1];
+	report->snrlint		= 0b00000100 & resp[1];
+	report->rssihint	= 0b00000010 & resp[1];
+	report->rssilint	= 0b00000001 & resp[1];
+
+	report->bltf		= 0b10000000 & resp[2];
+	report->snr_ready	= 0b00100000 & resp[2];
+	report->rssiready	= 0b00001000 & resp[2];
+	report->afcrl		= 0b00000010 & resp[2];
+	report->valid		= 0b00000001 & resp[2];
+
+	report->readfreq	= be16_to_cpup((__be16 *)(resp + 3));
+	report->freqoff		= resp[5];
+	report->rssi		= resp[6];
+	report->snr		= resp[7];
+	report->lassi		= resp[9];
+	report->hassi		= resp[10];
+	report->mult		= resp[11];
+	report->dev		= resp[12];
+	report->readantcap	= be16_to_cpup((__be16 *)(resp + 13));
+	report->assi		= resp[15];
+	report->usn		= resp[16];
+
+	return err;
+}
+
+
+static int si476x_core_cmd_fm_rsq_status_a30(struct si476x_core *core,
+					struct si476x_rsq_status_args *rsqargs,
+					struct si476x_rsq_status_report *report)
+{
+	int err;
+	u8       resp[CMD_FM_RSQ_STATUS_A30_NRESP];
+	const u8 args[CMD_FM_RSQ_STATUS_A30_NARGS] = {
+		rsqargs->primary << 4 | rsqargs->rsqack << 3 |
+		rsqargs->attune << 2 | rsqargs->cancel << 1 |
+		rsqargs->stcack,
+	};
+
+	err = si476x_core_send_command(core, CMD_FM_RSQ_STATUS,
+				       args, ARRAY_SIZE(args),
+				       resp, ARRAY_SIZE(resp),
+				       SI476X_DEFAULT_TIMEOUT);
+	/*
+	 * Besides getting received signal quality information this
+	 * command can be used to just acknowledge different interrupt
+	 * flags in those cases it is useless to copy and parse
+	 * received data so user can pass NULL, and thus avoid
+	 * unnecessary copying.
+	 */
+	if (err < 0 || report == NULL)
+		return err;
+
+	report->multhint	= 0b10000000 & resp[1];
+	report->multlint	= 0b01000000 & resp[1];
+	report->snrhint		= 0b00001000 & resp[1];
+	report->snrlint		= 0b00000100 & resp[1];
+	report->rssihint	= 0b00000010 & resp[1];
+	report->rssilint	= 0b00000001 & resp[1];
+
+	report->bltf		= 0b10000000 & resp[2];
+	report->snr_ready	= 0b00100000 & resp[2];
+	report->rssiready	= 0b00001000 & resp[2];
+	report->injside         = 0b00000100 & resp[2];
+	report->afcrl		= 0b00000010 & resp[2];
+	report->valid		= 0b00000001 & resp[2];
+
+	report->readfreq	= be16_to_cpup((__be16 *)(resp + 3));
+	report->freqoff		= resp[5];
+	report->rssi		= resp[6];
+	report->snr		= resp[7];
+	report->issi		= resp[8];
+	report->lassi		= resp[9];
+	report->hassi		= resp[10];
+	report->mult		= resp[11];
+	report->dev		= resp[12];
+	report->readantcap	= be16_to_cpup((__be16 *)(resp + 13));
+	report->assi		= resp[15];
+	report->usn		= resp[16];
+
+	report->pilotdev	= resp[17];
+	report->rdsdev		= resp[18];
+	report->assidev		= resp[19];
+	report->strongdev	= resp[20];
+	report->rdspi		= be16_to_cpup((__be16 *)(resp + 21));
+
+	return err;
+}
+
+static int si476x_core_cmd_fm_tune_freq_a10(struct si476x_core *core,
+					struct si476x_tune_freq_args *tuneargs)
+{
+	u8       resp[CMD_FM_TUNE_FREQ_NRESP];
+	const u8 args[CMD_FM_TUNE_FREQ_A10_NARGS] = {
+		(tuneargs->hd << 6) | (tuneargs->tunemode << 4)
+		| (tuneargs->smoothmetrics << 2),
+		msb(tuneargs->freq),
+		lsb(tuneargs->freq),
+		msb(tuneargs->antcap),
+		lsb(tuneargs->antcap)
+	};
+
+	return si476x_cmd_tune_seek_freq(core, CMD_FM_TUNE_FREQ,
+					 args, sizeof(args),
+					 resp, sizeof(resp));
+}
+
+static int si476x_core_cmd_fm_tune_freq_a20(struct si476x_core *core,
+					struct si476x_tune_freq_args *tuneargs)
+{
+	u8       resp[CMD_FM_TUNE_FREQ_NRESP];
+	const u8 args[CMD_FM_TUNE_FREQ_A20_NARGS] = {
+		(tuneargs->hd << 6) | (tuneargs->tunemode << 4)
+		|  (tuneargs->smoothmetrics << 2) | (tuneargs->injside),
+		msb(tuneargs->freq),
+		lsb(tuneargs->freq),
+	};
+
+	return si476x_cmd_tune_seek_freq(core, CMD_FM_TUNE_FREQ,
+					 args, sizeof(args),
+					 resp, sizeof(resp));
+}
+
+static int si476x_core_cmd_agc_status_a20(struct si476x_core *core,
+					struct si476x_agc_status_report *report)
+{
+	int err;
+	u8 resp[CMD_AGC_STATUS_NRESP_A20];
+
+	if (!report)
+		return -EINVAL;
+
+	err = si476x_core_send_command(core, CMD_AGC_STATUS,
+				       NULL, 0,
+				       resp, ARRAY_SIZE(resp),
+				       SI476X_DEFAULT_TIMEOUT);
+	if (err < 0)
+		return err;
+
+	report->mxhi		= resp[1] & SI476X_AGC_MXHI;
+	report->mxlo		= resp[1] & SI476X_AGC_MXLO;
+	report->lnahi		= resp[1] & SI476X_AGC_LNAHI;
+	report->lnalo		= resp[1] & SI476X_AGC_LNALO;
+	report->fmagc1		= resp[2];
+	report->fmagc2		= resp[3];
+	report->pgagain		= resp[4];
+	report->fmwblang	= resp[5];
+
+	return err;
+}
+
+static int si476x_core_cmd_agc_status_a10(struct si476x_core *core,
+					struct si476x_agc_status_report *report)
+{
+	int err;
+	u8 resp[CMD_AGC_STATUS_NRESP_A10];
+
+	if (!report)
+		return -EINVAL;
+
+	err = si476x_core_send_command(core, CMD_AGC_STATUS,
+				       NULL, 0,
+				       resp, ARRAY_SIZE(resp),
+				       SI476X_DEFAULT_TIMEOUT);
+	if (err < 0)
+		return err;
+
+	report->mxhi	= resp[1] & SI476X_AGC_MXHI;
+	report->mxlo	= resp[1] & SI476X_AGC_MXLO;
+	report->lnahi	= resp[1] & SI476X_AGC_LNAHI;
+	report->lnalo	= resp[1] & SI476X_AGC_LNALO;
+
+	return err;
+}
+
+typedef int (*tune_freq_func_t) (struct si476x_core *core,
+				 struct si476x_tune_freq_args *tuneargs);
+
+static struct {
+	int (*power_up) (struct si476x_core *,
+			 struct si476x_power_up_args *);
+	int (*power_down) (struct si476x_core *,
+			   struct si476x_power_down_args *);
+
+	tune_freq_func_t fm_tune_freq;
+	tune_freq_func_t am_tune_freq;
+
+	int (*fm_rsq_status)(struct si476x_core *,
+			     struct si476x_rsq_status_args *,
+			     struct si476x_rsq_status_report *);
+
+	int (*agc_status)(struct si476x_core *,
+			  struct si476x_agc_status_report *);
+	int (*intb_pin_cfg)(struct si476x_core *core,
+			    enum si476x_intb_config intb,
+			    enum si476x_a1_config a1);
+} si476x_cmds_vtable[] = {
+	[SI476X_REVISION_A10] = {
+		.power_up	= si476x_core_cmd_power_up_a10,
+		.power_down	= si476x_core_cmd_power_down_a10,
+		.fm_tune_freq	= si476x_core_cmd_fm_tune_freq_a10,
+		.am_tune_freq	= si476x_core_cmd_am_tune_freq_a10,
+		.fm_rsq_status	= si476x_core_cmd_fm_rsq_status_a10,
+		.agc_status	= si476x_core_cmd_agc_status_a10,
+		.intb_pin_cfg   = si476x_core_cmd_intb_pin_cfg_a10,
+	},
+	[SI476X_REVISION_A20] = {
+		.power_up	= si476x_core_cmd_power_up_a20,
+		.power_down	= si476x_core_cmd_power_down_a20,
+		.fm_tune_freq	= si476x_core_cmd_fm_tune_freq_a20,
+		.am_tune_freq	= si476x_core_cmd_am_tune_freq_a20,
+		.fm_rsq_status	= si476x_core_cmd_fm_rsq_status_a20,
+		.agc_status	= si476x_core_cmd_agc_status_a20,
+		.intb_pin_cfg   = si476x_core_cmd_intb_pin_cfg_a20,
+	},
+	[SI476X_REVISION_A30] = {
+		.power_up	= si476x_core_cmd_power_up_a20,
+		.power_down	= si476x_core_cmd_power_down_a20,
+		.fm_tune_freq	= si476x_core_cmd_fm_tune_freq_a20,
+		.am_tune_freq	= si476x_core_cmd_am_tune_freq_a20,
+		.fm_rsq_status	= si476x_core_cmd_fm_rsq_status_a30,
+		.agc_status	= si476x_core_cmd_agc_status_a20,
+		.intb_pin_cfg   = si476x_core_cmd_intb_pin_cfg_a20,
+	},
+};
+
+int si476x_core_cmd_power_up(struct si476x_core *core,
+			     struct si476x_power_up_args *args)
+{
+	BUG_ON(core->revision > SI476X_REVISION_A30 ||
+	       core->revision == -1);
+	return si476x_cmds_vtable[core->revision].power_up(core, args);
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_power_up);
+
+int si476x_core_cmd_power_down(struct si476x_core *core,
+			       struct si476x_power_down_args *args)
+{
+	BUG_ON(core->revision > SI476X_REVISION_A30 ||
+	       core->revision == -1);
+	return si476x_cmds_vtable[core->revision].power_down(core, args);
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_power_down);
+
+int si476x_core_cmd_fm_tune_freq(struct si476x_core *core,
+				 struct si476x_tune_freq_args *args)
+{
+	BUG_ON(core->revision > SI476X_REVISION_A30 ||
+	       core->revision == -1);
+	return si476x_cmds_vtable[core->revision].fm_tune_freq(core, args);
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_fm_tune_freq);
+
+int si476x_core_cmd_am_tune_freq(struct si476x_core *core,
+				 struct si476x_tune_freq_args *args)
+{
+	BUG_ON(core->revision > SI476X_REVISION_A30 ||
+	       core->revision == -1);
+	return si476x_cmds_vtable[core->revision].am_tune_freq(core, args);
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_am_tune_freq);
+
+int si476x_core_cmd_fm_rsq_status(struct si476x_core *core,
+				  struct si476x_rsq_status_args *args,
+				  struct si476x_rsq_status_report *report)
+
+{
+	BUG_ON(core->revision > SI476X_REVISION_A30 ||
+	       core->revision == -1);
+	return si476x_cmds_vtable[core->revision].fm_rsq_status(core, args,
+								report);
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_fm_rsq_status);
+
+int si476x_core_cmd_agc_status(struct si476x_core *core,
+				  struct si476x_agc_status_report *report)
+
+{
+	BUG_ON(core->revision > SI476X_REVISION_A30 ||
+	       core->revision == -1);
+	return si476x_cmds_vtable[core->revision].agc_status(core, report);
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_agc_status);
+
+int si476x_core_cmd_intb_pin_cfg(struct si476x_core *core,
+			    enum si476x_intb_config intb,
+			    enum si476x_a1_config a1)
+{
+	BUG_ON(core->revision > SI476X_REVISION_A30 ||
+	       core->revision == -1);
+
+	return si476x_cmds_vtable[core->revision].intb_pin_cfg(core, intb, a1);
+}
+EXPORT_SYMBOL_GPL(si476x_core_cmd_intb_pin_cfg);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Andrey Smirnov <andrew.smirnov@gmail.com>");
+MODULE_DESCRIPTION("API for command exchange for si476x");
diff --git a/drivers/mfd/si476x-i2c.c b/drivers/mfd/si476x-i2c.c
new file mode 100644
index 000000000000..f5bc8e4bd4bf
--- /dev/null
+++ b/drivers/mfd/si476x-i2c.c
@@ -0,0 +1,886 @@
+/*
+ * drivers/mfd/si476x-i2c.c -- Core device driver for si476x MFD
+ * device
+ *
+ * Copyright (C) 2012 Innovative Converged Devices(ICD)
+ * Copyright (C) 2013 Andrey Smirnov
+ *
+ * Author: Andrey Smirnov <andrew.smirnov@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; 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.
+ *
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/regulator/consumer.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+
+#include <linux/mfd/si476x-core.h>
+
+#define SI476X_MAX_IO_ERRORS		10
+#define SI476X_DRIVER_RDS_FIFO_DEPTH	128
+
+/**
+ * si476x_core_config_pinmux() - pin function configuration function
+ *
+ * @core: Core device structure
+ *
+ * Configure the functions of the pins of the radio chip.
+ *
+ * The function returns zero in case of succes or negative error code
+ * otherwise.
+ */
+static int si476x_core_config_pinmux(struct si476x_core *core)
+{
+	int err;
+	dev_dbg(&core->client->dev, "Configuring pinmux\n");
+	err = si476x_core_cmd_dig_audio_pin_cfg(core,
+						core->pinmux.dclk,
+						core->pinmux.dfs,
+						core->pinmux.dout,
+						core->pinmux.xout);
+	if (err < 0) {
+		dev_err(&core->client->dev,
+			"Failed to configure digital audio pins(err = %d)\n",
+			err);
+		return err;
+	}
+
+	err = si476x_core_cmd_zif_pin_cfg(core,
+					  core->pinmux.iqclk,
+					  core->pinmux.iqfs,
+					  core->pinmux.iout,
+					  core->pinmux.qout);
+	if (err < 0) {
+		dev_err(&core->client->dev,
+			"Failed to configure ZIF pins(err = %d)\n",
+			err);
+		return err;
+	}
+
+	err = si476x_core_cmd_ic_link_gpo_ctl_pin_cfg(core,
+						      core->pinmux.icin,
+						      core->pinmux.icip,
+						      core->pinmux.icon,
+						      core->pinmux.icop);
+	if (err < 0) {
+		dev_err(&core->client->dev,
+			"Failed to configure IC-Link/GPO pins(err = %d)\n",
+			err);
+		return err;
+	}
+
+	err = si476x_core_cmd_ana_audio_pin_cfg(core,
+						core->pinmux.lrout);
+	if (err < 0) {
+		dev_err(&core->client->dev,
+			"Failed to configure analog audio pins(err = %d)\n",
+			err);
+		return err;
+	}
+
+	err = si476x_core_cmd_intb_pin_cfg(core,
+					   core->pinmux.intb,
+					   core->pinmux.a1);
+	if (err < 0) {
+		dev_err(&core->client->dev,
+			"Failed to configure interrupt pins(err = %d)\n",
+			err);
+		return err;
+	}
+
+	return 0;
+}
+
+static inline void si476x_core_schedule_polling_work(struct si476x_core *core)
+{
+	schedule_delayed_work(&core->status_monitor,
+			      usecs_to_jiffies(SI476X_STATUS_POLL_US));
+}
+
+/**
+ * si476x_core_start() - early chip startup function
+ * @core: Core device structure
+ * @soft: When set, this flag forces "soft" startup, where "soft"
+ * power down is the one done by sending appropriate command instead
+ * of using reset pin of the tuner
+ *
+ * Perform required startup sequence to correctly power
+ * up the chip and perform initial configuration. It does the
+ * following sequence of actions:
+ *       1. Claims and enables the power supplies VD and VIO1 required
+ *          for I2C interface of the chip operation.
+ *       2. Waits for 100us, pulls the reset line up, enables irq,
+ *          waits for another 100us as it is specified by the
+ *          datasheet.
+ *       3. Sends 'POWER_UP' command to the device with all provided
+ *          information about power-up parameters.
+ *       4. Configures, pin multiplexor, disables digital audio and
+ *          configures interrupt sources.
+ *
+ * The function returns zero in case of succes or negative error code
+ * otherwise.
+ */
+int si476x_core_start(struct si476x_core *core, bool soft)
+{
+	struct i2c_client *client = core->client;
+	int err;
+
+	if (!soft) {
+		if (gpio_is_valid(core->gpio_reset))
+			gpio_set_value_cansleep(core->gpio_reset, 1);
+
+		if (client->irq)
+			enable_irq(client->irq);
+
+		udelay(100);
+
+		if (!client->irq) {
+			atomic_set(&core->is_alive, 1);
+			si476x_core_schedule_polling_work(core);
+		}
+	} else {
+		if (client->irq)
+			enable_irq(client->irq);
+		else {
+			atomic_set(&core->is_alive, 1);
+			si476x_core_schedule_polling_work(core);
+		}
+	}
+
+	err = si476x_core_cmd_power_up(core,
+				       &core->power_up_parameters);
+
+	if (err < 0) {
+		dev_err(&core->client->dev,
+			"Power up failure(err = %d)\n",
+			err);
+		goto disable_irq;
+	}
+
+	if (client->irq)
+		atomic_set(&core->is_alive, 1);
+
+	err = si476x_core_config_pinmux(core);
+	if (err < 0) {
+		dev_err(&core->client->dev,
+			"Failed to configure pinmux(err = %d)\n",
+			err);
+		goto disable_irq;
+	}
+
+	if (client->irq) {
+		err = regmap_write(core->regmap,
+				   SI476X_PROP_INT_CTL_ENABLE,
+				   SI476X_RDSIEN |
+				   SI476X_STCIEN |
+				   SI476X_CTSIEN);
+		if (err < 0) {
+			dev_err(&core->client->dev,
+				"Failed to configure interrupt sources"
+				"(err = %d)\n", err);
+			goto disable_irq;
+		}
+	}
+
+	return 0;
+
+disable_irq:
+	if (err == -ENODEV)
+		atomic_set(&core->is_alive, 0);
+
+	if (client->irq)
+		disable_irq(client->irq);
+	else
+		cancel_delayed_work_sync(&core->status_monitor);
+
+	if (gpio_is_valid(core->gpio_reset))
+		gpio_set_value_cansleep(core->gpio_reset, 0);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(si476x_core_start);
+
+/**
+ * si476x_core_stop() - chip power-down function
+ * @core: Core device structure
+ * @soft: When set, function sends a POWER_DOWN command instead of
+ * bringing reset line low
+ *
+ * Power down the chip by performing following actions:
+ * 1. Disable IRQ or stop the polling worker
+ * 2. Send the POWER_DOWN command if the power down is soft or bring
+ *    reset line low if not.
+ *
+ * The function returns zero in case of succes or negative error code
+ * otherwise.
+ */
+int si476x_core_stop(struct si476x_core *core, bool soft)
+{
+	int err = 0;
+	atomic_set(&core->is_alive, 0);
+
+	if (soft) {
+		/* TODO: This probably shoud be a configurable option,
+		 * so it is possible to have the chips keep their
+		 * oscillators running
+		 */
+		struct si476x_power_down_args args = {
+			.xosc = false,
+		};
+		err = si476x_core_cmd_power_down(core, &args);
+	}
+
+	/* We couldn't disable those before
+	 * 'si476x_core_cmd_power_down' since we expect to get CTS
+	 * interrupt */
+	if (core->client->irq)
+		disable_irq(core->client->irq);
+	else
+		cancel_delayed_work_sync(&core->status_monitor);
+
+	if (!soft) {
+		if (gpio_is_valid(core->gpio_reset))
+			gpio_set_value_cansleep(core->gpio_reset, 0);
+	}
+	return err;
+}
+EXPORT_SYMBOL_GPL(si476x_core_stop);
+
+/**
+ * si476x_core_set_power_state() - set the level at which the power is
+ * supplied for the chip.
+ * @core: Core device structure
+ * @next_state: enum si476x_power_state describing power state to
+ *              switch to.
+ *
+ * Switch on all the required power supplies
+ *
+ * This function returns 0 in case of suvccess and negative error code
+ * otherwise.
+ */
+int si476x_core_set_power_state(struct si476x_core *core,
+				enum si476x_power_state next_state)
+{
+	/*
+	   It is not clear form the datasheet if it is possible to
+	   work with device if not all power domains are operational.
+	   So for now the power-up policy is "power-up all the things!"
+	 */
+	int err = 0;
+
+	if (core->power_state == SI476X_POWER_INCONSISTENT) {
+		dev_err(&core->client->dev,
+			"The device in inconsistent power state\n");
+		return -EINVAL;
+	}
+
+	if (next_state != core->power_state) {
+		switch (next_state) {
+		case SI476X_POWER_UP_FULL:
+			err = regulator_bulk_enable(ARRAY_SIZE(core->supplies),
+						    core->supplies);
+			if (err < 0) {
+				core->power_state = SI476X_POWER_INCONSISTENT;
+				break;
+			}
+			/*
+			 * Startup timing diagram recommends to have a
+			 * 100 us delay between enabling of the power
+			 * supplies and turning the tuner on.
+			 */
+			udelay(100);
+
+			err = si476x_core_start(core, false);
+			if (err < 0)
+				goto disable_regulators;
+
+			core->power_state = next_state;
+			break;
+
+		case SI476X_POWER_DOWN:
+			core->power_state = next_state;
+			err = si476x_core_stop(core, false);
+			if (err < 0)
+				core->power_state = SI476X_POWER_INCONSISTENT;
+disable_regulators:
+			err = regulator_bulk_disable(ARRAY_SIZE(core->supplies),
+						     core->supplies);
+			if (err < 0)
+				core->power_state = SI476X_POWER_INCONSISTENT;
+			break;
+		default:
+			BUG();
+		}
+	}
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(si476x_core_set_power_state);
+
+/**
+ * si476x_core_report_drainer_stop() - mark the completion of the RDS
+ * buffer drain porcess by the worker.
+ *
+ * @core: Core device structure
+ */
+static inline void si476x_core_report_drainer_stop(struct si476x_core *core)
+{
+	mutex_lock(&core->rds_drainer_status_lock);
+	core->rds_drainer_is_working = false;
+	mutex_unlock(&core->rds_drainer_status_lock);
+}
+
+/**
+ * si476x_core_start_rds_drainer_once() - start RDS drainer worker if
+ * ther is none working, do nothing otherwise
+ *
+ * @core: Datastructure corresponding to the chip.
+ */
+static inline void si476x_core_start_rds_drainer_once(struct si476x_core *core)
+{
+	mutex_lock(&core->rds_drainer_status_lock);
+	if (!core->rds_drainer_is_working) {
+		core->rds_drainer_is_working = true;
+		schedule_work(&core->rds_fifo_drainer);
+	}
+	mutex_unlock(&core->rds_drainer_status_lock);
+}
+/**
+ * si476x_drain_rds_fifo() - RDS buffer drainer.
+ * @work: struct work_struct being ppassed to the function by the
+ * kernel.
+ *
+ * Drain the contents of the RDS FIFO of
+ */
+static void si476x_core_drain_rds_fifo(struct work_struct *work)
+{
+	int err;
+
+	struct si476x_core *core = container_of(work, struct si476x_core,
+						rds_fifo_drainer);
+
+	struct si476x_rds_status_report report;
+
+	si476x_core_lock(core);
+	err = si476x_core_cmd_fm_rds_status(core, true, false, false, &report);
+	if (!err) {
+		int i = report.rdsfifoused;
+		dev_dbg(&core->client->dev,
+			"%d elements in RDS FIFO. Draining.\n", i);
+		for (; i > 0; --i) {
+			err = si476x_core_cmd_fm_rds_status(core, false, false,
+							    (i == 1), &report);
+			if (err < 0)
+				goto unlock;
+
+			kfifo_in(&core->rds_fifo, report.rds,
+				 sizeof(report.rds));
+			dev_dbg(&core->client->dev, "RDS data:\n %*ph\n",
+				(int)sizeof(report.rds), report.rds);
+		}
+		dev_dbg(&core->client->dev, "Drrrrained!\n");
+		wake_up_interruptible(&core->rds_read_queue);
+	}
+
+unlock:
+	si476x_core_unlock(core);
+	si476x_core_report_drainer_stop(core);
+}
+
+/**
+ * si476x_core_pronounce_dead()
+ *
+ * @core: Core device structure
+ *
+ * Mark the device as being dead and wake up all potentially waiting
+ * threads of execution.
+ *
+ */
+static void si476x_core_pronounce_dead(struct si476x_core *core)
+{
+	dev_info(&core->client->dev, "Core device is dead.\n");
+
+	atomic_set(&core->is_alive, 0);
+
+	/* Wake up al possible waiting processes */
+	wake_up_interruptible(&core->rds_read_queue);
+
+	atomic_set(&core->cts, 1);
+	wake_up(&core->command);
+
+	atomic_set(&core->stc, 1);
+	wake_up(&core->tuning);
+}
+
+/**
+ * si476x_core_i2c_xfer()
+ *
+ * @core: Core device structure
+ * @type: Transfer type
+ * @buf: Transfer buffer for/with data
+ * @count: Transfer buffer size
+ *
+ * Perfrom and I2C transfer(either read or write) and keep a counter
+ * of I/O errors. If the error counter rises above the threshold
+ * pronounce device dead.
+ *
+ * The function returns zero on succes or negative error code on
+ * failure.
+ */
+int si476x_core_i2c_xfer(struct si476x_core *core,
+		    enum si476x_i2c_type type,
+		    char *buf, int count)
+{
+	static int io_errors_count;
+	int err;
+	if (type == SI476X_I2C_SEND)
+		err = i2c_master_send(core->client, buf, count);
+	else
+		err = i2c_master_recv(core->client, buf, count);
+
+	if (err < 0) {
+		if (io_errors_count++ > SI476X_MAX_IO_ERRORS)
+			si476x_core_pronounce_dead(core);
+	} else {
+		io_errors_count = 0;
+	}
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(si476x_core_i2c_xfer);
+
+/**
+ * si476x_get_status()
+ * @core: Core device structure
+ *
+ * Get the status byte of the core device by berforming one byte I2C
+ * read.
+ *
+ * The function returns a status value or a negative error code on
+ * error.
+ */
+static int si476x_core_get_status(struct si476x_core *core)
+{
+	u8 response;
+	int err = si476x_core_i2c_xfer(core, SI476X_I2C_RECV,
+				  &response, sizeof(response));
+
+	return (err < 0) ? err : response;
+}
+
+/**
+ * si476x_get_and_signal_status() - IRQ dispatcher
+ * @core: Core device structure
+ *
+ * Dispatch the arrived interrupt request based on the value of the
+ * status byte reported by the tuner.
+ *
+ */
+static void si476x_core_get_and_signal_status(struct si476x_core *core)
+{
+	int status = si476x_core_get_status(core);
+	if (status < 0) {
+		dev_err(&core->client->dev, "Failed to get status\n");
+		return;
+	}
+
+	if (status & SI476X_CTS) {
+		/* Unfortunately completions could not be used for
+		 * signalling CTS since this flag cannot be cleared
+		 * in status byte, and therefore once it becomes true
+		 * multiple calls to 'complete' would cause the
+		 * commands following the current one to be completed
+		 * before they actually are */
+		dev_dbg(&core->client->dev, "[interrupt] CTSINT\n");
+		atomic_set(&core->cts, 1);
+		wake_up(&core->command);
+	}
+
+	if (status & SI476X_FM_RDS_INT) {
+		dev_dbg(&core->client->dev, "[interrupt] RDSINT\n");
+		si476x_core_start_rds_drainer_once(core);
+	}
+
+	if (status & SI476X_STC_INT) {
+		dev_dbg(&core->client->dev, "[interrupt] STCINT\n");
+		atomic_set(&core->stc, 1);
+		wake_up(&core->tuning);
+	}
+}
+
+static void si476x_core_poll_loop(struct work_struct *work)
+{
+	struct si476x_core *core = SI476X_WORK_TO_CORE(work);
+
+	si476x_core_get_and_signal_status(core);
+
+	if (atomic_read(&core->is_alive))
+		si476x_core_schedule_polling_work(core);
+}
+
+static irqreturn_t si476x_core_interrupt(int irq, void *dev)
+{
+	struct si476x_core *core = dev;
+
+	si476x_core_get_and_signal_status(core);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * si476x_firmware_version_to_revision()
+ * @core: Core device structure
+ * @major:  Firmware major number
+ * @minor1: Firmware first minor number
+ * @minor2: Firmware second minor number
+ *
+ * Convert a chip's firmware version number into an offset that later
+ * will be used to as offset in "vtable" of tuner functions
+ *
+ * This function returns a positive offset in case of success and a -1
+ * in case of failure.
+ */
+static int si476x_core_fwver_to_revision(struct si476x_core *core,
+					 int func, int major,
+					 int minor1, int minor2)
+{
+	switch (func) {
+	case SI476X_FUNC_FM_RECEIVER:
+		switch (major) {
+		case 5:
+			return SI476X_REVISION_A10;
+		case 8:
+			return SI476X_REVISION_A20;
+		case 10:
+			return SI476X_REVISION_A30;
+		default:
+			goto unknown_revision;
+		}
+	case SI476X_FUNC_AM_RECEIVER:
+		switch (major) {
+		case 5:
+			return SI476X_REVISION_A10;
+		case 7:
+			return SI476X_REVISION_A20;
+		case 9:
+			return SI476X_REVISION_A30;
+		default:
+			goto unknown_revision;
+		}
+	case SI476X_FUNC_WB_RECEIVER:
+		switch (major) {
+		case 3:
+			return SI476X_REVISION_A10;
+		case 5:
+			return SI476X_REVISION_A20;
+		case 7:
+			return SI476X_REVISION_A30;
+		default:
+			goto unknown_revision;
+		}
+	case SI476X_FUNC_BOOTLOADER:
+	default:		/* FALLTHROUG */
+		BUG();
+		return -1;
+	}
+
+unknown_revision:
+	dev_err(&core->client->dev,
+		"Unsupported version of the firmware: %d.%d.%d, "
+		"reverting to A10 comptible functions\n",
+		major, minor1, minor2);
+
+	return SI476X_REVISION_A10;
+}
+
+/**
+ * si476x_get_revision_info()
+ * @core: Core device structure
+ *
+ * Get the firmware version number of the device. It is done in
+ * following three steps:
+ *    1. Power-up the device
+ *    2. Send the 'FUNC_INFO' command
+ *    3. Powering the device down.
+ *
+ * The function return zero on success and a negative error code on
+ * failure.
+ */
+static int si476x_core_get_revision_info(struct si476x_core *core)
+{
+	int rval;
+	struct si476x_func_info info;
+
+	si476x_core_lock(core);
+	rval = si476x_core_set_power_state(core, SI476X_POWER_UP_FULL);
+	if (rval < 0)
+		goto exit;
+
+	rval = si476x_core_cmd_func_info(core, &info);
+	if (rval < 0)
+		goto power_down;
+
+	core->revision = si476x_core_fwver_to_revision(core, info.func,
+						       info.firmware.major,
+						       info.firmware.minor[0],
+						       info.firmware.minor[1]);
+power_down:
+	si476x_core_set_power_state(core, SI476X_POWER_DOWN);
+exit:
+	si476x_core_unlock(core);
+
+	return rval;
+}
+
+bool si476x_core_has_am(struct si476x_core *core)
+{
+	return core->chip_id == SI476X_CHIP_SI4761 ||
+		core->chip_id == SI476X_CHIP_SI4764;
+}
+EXPORT_SYMBOL_GPL(si476x_core_has_am);
+
+bool si476x_core_has_diversity(struct si476x_core *core)
+{
+	return core->chip_id == SI476X_CHIP_SI4764;
+}
+EXPORT_SYMBOL_GPL(si476x_core_has_diversity);
+
+bool si476x_core_is_a_secondary_tuner(struct si476x_core *core)
+{
+	return si476x_core_has_diversity(core) &&
+		(core->diversity_mode == SI476X_PHDIV_SECONDARY_ANTENNA ||
+		 core->diversity_mode == SI476X_PHDIV_SECONDARY_COMBINING);
+}
+EXPORT_SYMBOL_GPL(si476x_core_is_a_secondary_tuner);
+
+bool si476x_core_is_a_primary_tuner(struct si476x_core *core)
+{
+	return si476x_core_has_diversity(core) &&
+		(core->diversity_mode == SI476X_PHDIV_PRIMARY_ANTENNA ||
+		 core->diversity_mode == SI476X_PHDIV_PRIMARY_COMBINING);
+}
+EXPORT_SYMBOL_GPL(si476x_core_is_a_primary_tuner);
+
+bool si476x_core_is_in_am_receiver_mode(struct si476x_core *core)
+{
+	return si476x_core_has_am(core) &&
+		(core->power_up_parameters.func == SI476X_FUNC_AM_RECEIVER);
+}
+EXPORT_SYMBOL_GPL(si476x_core_is_in_am_receiver_mode);
+
+bool si476x_core_is_powered_up(struct si476x_core *core)
+{
+	return core->power_state == SI476X_POWER_UP_FULL;
+}
+EXPORT_SYMBOL_GPL(si476x_core_is_powered_up);
+
+static int si476x_core_probe(struct i2c_client *client,
+			     const struct i2c_device_id *id)
+{
+	int rval;
+	struct si476x_core          *core;
+	struct si476x_platform_data *pdata;
+	struct mfd_cell *cell;
+	int              cell_num;
+
+	core = devm_kzalloc(&client->dev, sizeof(*core), GFP_KERNEL);
+	if (!core) {
+		dev_err(&client->dev,
+			"failed to allocate 'struct si476x_core'\n");
+		return -ENOMEM;
+	}
+	core->client = client;
+
+	core->regmap = devm_regmap_init_si476x(core);
+	if (IS_ERR(core->regmap)) {
+		rval = PTR_ERR(core->regmap);
+		dev_err(&client->dev,
+			"Failed to allocate register map: %d\n",
+			rval);
+		return rval;
+	}
+
+	i2c_set_clientdata(client, core);
+
+	atomic_set(&core->is_alive, 0);
+	core->power_state = SI476X_POWER_DOWN;
+
+	pdata = client->dev.platform_data;
+	if (pdata) {
+		memcpy(&core->power_up_parameters,
+		       &pdata->power_up_parameters,
+		       sizeof(core->power_up_parameters));
+
+		core->gpio_reset = -1;
+		if (gpio_is_valid(pdata->gpio_reset)) {
+			rval = gpio_request(pdata->gpio_reset, "si476x reset");
+			if (rval) {
+				dev_err(&client->dev,
+					"Failed to request gpio: %d\n", rval);
+				return rval;
+			}
+			core->gpio_reset = pdata->gpio_reset;
+			gpio_direction_output(core->gpio_reset, 0);
+		}
+
+		core->diversity_mode = pdata->diversity_mode;
+		memcpy(&core->pinmux, &pdata->pinmux,
+		       sizeof(struct si476x_pinmux));
+	} else {
+		dev_err(&client->dev, "No platform data provided\n");
+		return -EINVAL;
+	}
+
+	core->supplies[0].supply = "vd";
+	core->supplies[1].supply = "va";
+	core->supplies[2].supply = "vio1";
+	core->supplies[3].supply = "vio2";
+
+	rval = devm_regulator_bulk_get(&client->dev,
+				       ARRAY_SIZE(core->supplies),
+				       core->supplies);
+	if (rval) {
+		dev_err(&client->dev, "Failet to gett all of the regulators\n");
+		goto free_gpio;
+	}
+
+	mutex_init(&core->cmd_lock);
+	init_waitqueue_head(&core->command);
+	init_waitqueue_head(&core->tuning);
+
+	rval = kfifo_alloc(&core->rds_fifo,
+			   SI476X_DRIVER_RDS_FIFO_DEPTH *
+			   sizeof(struct v4l2_rds_data),
+			   GFP_KERNEL);
+	if (rval) {
+		dev_err(&client->dev, "Could not alloate the FIFO\n");
+		goto free_gpio;
+	}
+	mutex_init(&core->rds_drainer_status_lock);
+	init_waitqueue_head(&core->rds_read_queue);
+	INIT_WORK(&core->rds_fifo_drainer, si476x_core_drain_rds_fifo);
+
+	if (client->irq) {
+		rval = devm_request_threaded_irq(&client->dev,
+						 client->irq, NULL,
+						 si476x_core_interrupt,
+						 IRQF_TRIGGER_FALLING,
+						 client->name, core);
+		if (rval < 0) {
+			dev_err(&client->dev, "Could not request IRQ %d\n",
+				client->irq);
+			goto free_kfifo;
+		}
+		disable_irq(client->irq);
+		dev_dbg(&client->dev, "IRQ requested.\n");
+
+		core->rds_fifo_depth = 20;
+	} else {
+		INIT_DELAYED_WORK(&core->status_monitor,
+				  si476x_core_poll_loop);
+		dev_info(&client->dev,
+			 "No IRQ number specified, will use polling\n");
+
+		core->rds_fifo_depth = 5;
+	}
+
+	core->chip_id = id->driver_data;
+
+	rval = si476x_core_get_revision_info(core);
+	if (rval < 0) {
+		rval = -ENODEV;
+		goto free_kfifo;
+	}
+
+	cell_num = 0;
+
+	cell = &core->cells[SI476X_RADIO_CELL];
+	cell->name = "si476x-radio";
+	cell_num++;
+
+#ifdef CONFIG_SND_SOC_SI476X
+	if ((core->chip_id == SI476X_CHIP_SI4761 ||
+	     core->chip_id == SI476X_CHIP_SI4764)	&&
+	    core->pinmux.dclk == SI476X_DCLK_DAUDIO     &&
+	    core->pinmux.dfs  == SI476X_DFS_DAUDIO      &&
+	    core->pinmux.dout == SI476X_DOUT_I2S_OUTPUT &&
+	    core->pinmux.xout == SI476X_XOUT_TRISTATE) {
+		cell = &core->cells[SI476X_CODEC_CELL];
+		cell->name          = "si476x-codec";
+		cell_num++;
+	}
+#endif
+	rval = mfd_add_devices(&client->dev,
+			       (client->adapter->nr << 8) + client->addr,
+			       core->cells, cell_num,
+			       NULL, 0, NULL);
+	if (!rval)
+		return 0;
+
+free_kfifo:
+	kfifo_free(&core->rds_fifo);
+
+free_gpio:
+	if (gpio_is_valid(core->gpio_reset))
+		gpio_free(core->gpio_reset);
+
+	return rval;
+}
+
+static int si476x_core_remove(struct i2c_client *client)
+{
+	struct si476x_core *core = i2c_get_clientdata(client);
+
+	si476x_core_pronounce_dead(core);
+	mfd_remove_devices(&client->dev);
+
+	if (client->irq)
+		disable_irq(client->irq);
+	else
+		cancel_delayed_work_sync(&core->status_monitor);
+
+	kfifo_free(&core->rds_fifo);
+
+	if (gpio_is_valid(core->gpio_reset))
+		gpio_free(core->gpio_reset);
+
+	return 0;
+}
+
+
+static const struct i2c_device_id si476x_id[] = {
+	{ "si4761", SI476X_CHIP_SI4761 },
+	{ "si4764", SI476X_CHIP_SI4764 },
+	{ "si4768", SI476X_CHIP_SI4768 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, si476x_id);
+
+static struct i2c_driver si476x_core_driver = {
+	.driver		= {
+		.name	= "si476x-core",
+		.owner  = THIS_MODULE,
+	},
+	.probe		= si476x_core_probe,
+	.remove         = si476x_core_remove,
+	.id_table       = si476x_id,
+};
+module_i2c_driver(si476x_core_driver);
+
+
+MODULE_AUTHOR("Andrey Smirnov <andrew.smirnov@gmail.com>");
+MODULE_DESCRIPTION("Si4761/64/68 AM/FM MFD core device driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/si476x-prop.c b/drivers/mfd/si476x-prop.c
new file mode 100644
index 000000000000..cfeffa6e15d9
--- /dev/null
+++ b/drivers/mfd/si476x-prop.c
@@ -0,0 +1,241 @@
+/*
+ * drivers/mfd/si476x-prop.c -- Subroutines to access
+ * properties of si476x chips
+ *
+ * Copyright (C) 2012 Innovative Converged Devices(ICD)
+ * Copyright (C) 2013 Andrey Smirnov
+ *
+ * Author: Andrey Smirnov <andrew.smirnov@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; 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.
+ */
+#include <linux/module.h>
+
+#include <linux/mfd/si476x-core.h>
+
+struct si476x_property_range {
+	u16 low, high;
+};
+
+static bool si476x_core_element_is_in_array(u16 element,
+					    const u16 array[],
+					    size_t size)
+{
+	int i;
+
+	for (i = 0; i < size; i++)
+		if (element == array[i])
+			return true;
+
+	return false;
+}
+
+static bool si476x_core_element_is_in_range(u16 element,
+					    const struct si476x_property_range range[],
+					    size_t size)
+{
+	int i;
+
+	for (i = 0; i < size; i++)
+		if (element <= range[i].high && element >= range[i].low)
+			return true;
+
+	return false;
+}
+
+static bool si476x_core_is_valid_property_a10(struct si476x_core *core,
+					      u16 property)
+{
+	static const u16 valid_properties[] = {
+		0x0000,
+		0x0500, 0x0501,
+		0x0600,
+		0x0709, 0x070C, 0x070D, 0x70E, 0x710,
+		0x0718,
+		0x1207, 0x1208,
+		0x2007,
+		0x2300,
+	};
+
+	static const struct si476x_property_range valid_ranges[] = {
+		{ 0x0200, 0x0203 },
+		{ 0x0300, 0x0303 },
+		{ 0x0400, 0x0404 },
+		{ 0x0700, 0x0707 },
+		{ 0x1100, 0x1102 },
+		{ 0x1200, 0x1204 },
+		{ 0x1300, 0x1306 },
+		{ 0x2000, 0x2005 },
+		{ 0x2100, 0x2104 },
+		{ 0x2106, 0x2106 },
+		{ 0x2200, 0x220E },
+		{ 0x3100, 0x3104 },
+		{ 0x3207, 0x320F },
+		{ 0x3300, 0x3304 },
+		{ 0x3500, 0x3517 },
+		{ 0x3600, 0x3617 },
+		{ 0x3700, 0x3717 },
+		{ 0x4000, 0x4003 },
+	};
+
+	return	si476x_core_element_is_in_range(property, valid_ranges,
+						ARRAY_SIZE(valid_ranges)) ||
+		si476x_core_element_is_in_array(property, valid_properties,
+						ARRAY_SIZE(valid_properties));
+}
+
+static bool si476x_core_is_valid_property_a20(struct si476x_core *core,
+					      u16 property)
+{
+	static const u16 valid_properties[] = {
+		0x071B,
+		0x1006,
+		0x2210,
+		0x3401,
+	};
+
+	static const struct si476x_property_range valid_ranges[] = {
+		{ 0x2215, 0x2219 },
+	};
+
+	return	si476x_core_is_valid_property_a10(core, property) ||
+		si476x_core_element_is_in_range(property, valid_ranges,
+						ARRAY_SIZE(valid_ranges))  ||
+		si476x_core_element_is_in_array(property, valid_properties,
+						ARRAY_SIZE(valid_properties));
+}
+
+static bool si476x_core_is_valid_property_a30(struct si476x_core *core,
+					      u16 property)
+{
+	static const u16 valid_properties[] = {
+		0x071C, 0x071D,
+		0x1007, 0x1008,
+		0x220F, 0x2214,
+		0x2301,
+		0x3105, 0x3106,
+		0x3402,
+	};
+
+	static const struct si476x_property_range valid_ranges[] = {
+		{ 0x0405, 0x0411 },
+		{ 0x2008, 0x200B },
+		{ 0x2220, 0x2223 },
+		{ 0x3100, 0x3106 },
+	};
+
+	return	si476x_core_is_valid_property_a20(core, property) ||
+		si476x_core_element_is_in_range(property, valid_ranges,
+						ARRAY_SIZE(valid_ranges)) ||
+		si476x_core_element_is_in_array(property, valid_properties,
+						ARRAY_SIZE(valid_properties));
+}
+
+typedef bool (*valid_property_pred_t) (struct si476x_core *, u16);
+
+static bool si476x_core_is_valid_property(struct si476x_core *core,
+					  u16 property)
+{
+	static const valid_property_pred_t is_valid_property[] = {
+		[SI476X_REVISION_A10] = si476x_core_is_valid_property_a10,
+		[SI476X_REVISION_A20] = si476x_core_is_valid_property_a20,
+		[SI476X_REVISION_A30] = si476x_core_is_valid_property_a30,
+	};
+
+	BUG_ON(core->revision > SI476X_REVISION_A30 ||
+	       core->revision == -1);
+	return is_valid_property[core->revision](core, property);
+}
+
+
+static bool si476x_core_is_readonly_property(struct si476x_core *core,
+					     u16 property)
+{
+	BUG_ON(core->revision > SI476X_REVISION_A30 ||
+	       core->revision == -1);
+
+	switch (core->revision) {
+	case SI476X_REVISION_A10:
+		return (property == 0x3200);
+	case SI476X_REVISION_A20:
+		return (property == 0x1006 ||
+			property == 0x2210 ||
+			property == 0x3200);
+	case SI476X_REVISION_A30:
+		return false;
+	}
+
+	return false;
+}
+
+static bool si476x_core_regmap_readable_register(struct device *dev,
+						 unsigned int reg)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct si476x_core *core = i2c_get_clientdata(client);
+
+	return si476x_core_is_valid_property(core, (u16) reg);
+
+}
+
+static bool si476x_core_regmap_writable_register(struct device *dev,
+						 unsigned int reg)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct si476x_core *core = i2c_get_clientdata(client);
+
+	return si476x_core_is_valid_property(core, (u16) reg) &&
+		!si476x_core_is_readonly_property(core, (u16) reg);
+}
+
+
+static int si476x_core_regmap_write(void *context, unsigned int reg,
+				    unsigned int val)
+{
+	return si476x_core_cmd_set_property(context, reg, val);
+}
+
+static int si476x_core_regmap_read(void *context, unsigned int reg,
+				   unsigned *val)
+{
+	struct si476x_core *core = context;
+	int err;
+
+	err = si476x_core_cmd_get_property(core, reg);
+	if (err < 0)
+		return err;
+
+	*val = err;
+
+	return 0;
+}
+
+
+static const struct regmap_config si476x_regmap_config = {
+	.reg_bits = 16,
+	.val_bits = 16,
+
+	.max_register = 0x4003,
+
+	.writeable_reg = si476x_core_regmap_writable_register,
+	.readable_reg = si476x_core_regmap_readable_register,
+
+	.reg_read = si476x_core_regmap_read,
+	.reg_write = si476x_core_regmap_write,
+
+	.cache_type = REGCACHE_RBTREE,
+};
+
+struct regmap *devm_regmap_init_si476x(struct si476x_core *core)
+{
+	return devm_regmap_init(&core->client->dev, NULL,
+				core, &si476x_regmap_config);
+}
+EXPORT_SYMBOL_GPL(devm_regmap_init_si476x);
diff --git a/drivers/mfd/sta2x11-mfd.c b/drivers/mfd/sta2x11-mfd.c
index 9bd33169a111..d70a343078fd 100644
--- a/drivers/mfd/sta2x11-mfd.c
+++ b/drivers/mfd/sta2x11-mfd.c
@@ -98,17 +98,6 @@ static int sta2x11_mfd_add(struct pci_dev *pdev, gfp_t flags)
 	return 0;
 }
 
-static int mfd_remove(struct pci_dev *pdev)
-{
-	struct sta2x11_mfd *mfd = sta2x11_mfd_find(pdev);
-
-	if (!mfd)
-		return -ENODEV;
-	list_del(&mfd->list);
-	kfree(mfd);
-	return 0;
-}
-
 /* This function is exported and is not expected to fail */
 u32 __sta2x11_mfd_mask(struct pci_dev *pdev, u32 reg, u32 mask, u32 val,
 		       enum sta2x11_mfd_plat_dev index)
diff --git a/drivers/mfd/stmpe-i2c.c b/drivers/mfd/stmpe-i2c.c
index fd5fcb630685..0da02e11d58e 100644
--- a/drivers/mfd/stmpe-i2c.c
+++ b/drivers/mfd/stmpe-i2c.c
@@ -75,6 +75,7 @@ static const struct i2c_device_id stmpe_i2c_id[] = {
 	{ "stmpe801", STMPE801 },
 	{ "stmpe811", STMPE811 },
 	{ "stmpe1601", STMPE1601 },
+	{ "stmpe1801", STMPE1801 },
 	{ "stmpe2401", STMPE2401 },
 	{ "stmpe2403", STMPE2403 },
 	{ }
diff --git a/drivers/mfd/stmpe-spi.c b/drivers/mfd/stmpe-spi.c
index 973659f8abd9..a81badbaa917 100644
--- a/drivers/mfd/stmpe-spi.c
+++ b/drivers/mfd/stmpe-spi.c
@@ -103,7 +103,7 @@ stmpe_spi_probe(struct spi_device *spi)
 
 static int stmpe_spi_remove(struct spi_device *spi)
 {
-	struct stmpe *stmpe = dev_get_drvdata(&spi->dev);
+	struct stmpe *stmpe = spi_get_drvdata(spi);
 
 	return stmpe_remove(stmpe);
 }
diff --git a/drivers/mfd/stmpe.c b/drivers/mfd/stmpe.c
index 4b11202061be..bbccd514d3ec 100644
--- a/drivers/mfd/stmpe.c
+++ b/drivers/mfd/stmpe.c
@@ -19,6 +19,7 @@
 #include <linux/pm.h>
 #include <linux/slab.h>
 #include <linux/mfd/core.h>
+#include <linux/delay.h>
 #include "stmpe.h"
 
 static int __stmpe_enable(struct stmpe *stmpe, unsigned int blocks)
@@ -643,6 +644,88 @@ static struct stmpe_variant_info stmpe1601 = {
 };
 
 /*
+ * STMPE1801
+ */
+static const u8 stmpe1801_regs[] = {
+	[STMPE_IDX_CHIP_ID]	= STMPE1801_REG_CHIP_ID,
+	[STMPE_IDX_ICR_LSB]	= STMPE1801_REG_INT_CTRL_LOW,
+	[STMPE_IDX_IER_LSB]	= STMPE1801_REG_INT_EN_MASK_LOW,
+	[STMPE_IDX_ISR_LSB]	= STMPE1801_REG_INT_STA_LOW,
+	[STMPE_IDX_GPMR_LSB]	= STMPE1801_REG_GPIO_MP_LOW,
+	[STMPE_IDX_GPSR_LSB]	= STMPE1801_REG_GPIO_SET_LOW,
+	[STMPE_IDX_GPCR_LSB]	= STMPE1801_REG_GPIO_CLR_LOW,
+	[STMPE_IDX_GPDR_LSB]	= STMPE1801_REG_GPIO_SET_DIR_LOW,
+	[STMPE_IDX_GPRER_LSB]	= STMPE1801_REG_GPIO_RE_LOW,
+	[STMPE_IDX_GPFER_LSB]	= STMPE1801_REG_GPIO_FE_LOW,
+	[STMPE_IDX_IEGPIOR_LSB]	= STMPE1801_REG_INT_EN_GPIO_MASK_LOW,
+	[STMPE_IDX_ISGPIOR_LSB]	= STMPE1801_REG_INT_STA_GPIO_LOW,
+};
+
+static struct stmpe_variant_block stmpe1801_blocks[] = {
+	{
+		.cell	= &stmpe_gpio_cell,
+		.irq	= STMPE1801_IRQ_GPIOC,
+		.block	= STMPE_BLOCK_GPIO,
+	},
+	{
+		.cell	= &stmpe_keypad_cell,
+		.irq	= STMPE1801_IRQ_KEYPAD,
+		.block	= STMPE_BLOCK_KEYPAD,
+	},
+};
+
+static int stmpe1801_enable(struct stmpe *stmpe, unsigned int blocks,
+			    bool enable)
+{
+	unsigned int mask = 0;
+	if (blocks & STMPE_BLOCK_GPIO)
+		mask |= STMPE1801_MSK_INT_EN_GPIO;
+
+	if (blocks & STMPE_BLOCK_KEYPAD)
+		mask |= STMPE1801_MSK_INT_EN_KPC;
+
+	return __stmpe_set_bits(stmpe, STMPE1801_REG_INT_EN_MASK_LOW, mask,
+				enable ? mask : 0);
+}
+
+static int stmpe1801_reset(struct stmpe *stmpe)
+{
+	unsigned long timeout;
+	int ret = 0;
+
+	ret = __stmpe_set_bits(stmpe, STMPE1801_REG_SYS_CTRL,
+		STMPE1801_MSK_SYS_CTRL_RESET, STMPE1801_MSK_SYS_CTRL_RESET);
+	if (ret < 0)
+		return ret;
+
+	timeout = jiffies + msecs_to_jiffies(100);
+	while (time_before(jiffies, timeout)) {
+		ret = __stmpe_reg_read(stmpe, STMPE1801_REG_SYS_CTRL);
+		if (ret < 0)
+			return ret;
+		if (!(ret & STMPE1801_MSK_SYS_CTRL_RESET))
+			return 0;
+		usleep_range(100, 200);
+	};
+	return -EIO;
+}
+
+static struct stmpe_variant_info stmpe1801 = {
+	.name		= "stmpe1801",
+	.id_val		= STMPE1801_ID,
+	.id_mask	= 0xfff0,
+	.num_gpios	= 18,
+	.af_bits	= 0,
+	.regs		= stmpe1801_regs,
+	.blocks		= stmpe1801_blocks,
+	.num_blocks	= ARRAY_SIZE(stmpe1801_blocks),
+	.num_irqs	= STMPE1801_NR_INTERNAL_IRQS,
+	.enable		= stmpe1801_enable,
+	/* stmpe1801 do not have any gpio alternate function */
+	.get_altfunc	= NULL,
+};
+
+/*
  * STMPE24XX
  */
 
@@ -740,6 +823,7 @@ static struct stmpe_variant_info *stmpe_variant_info[STMPE_NBR_PARTS] = {
 	[STMPE801]	= &stmpe801,
 	[STMPE811]	= &stmpe811,
 	[STMPE1601]	= &stmpe1601,
+	[STMPE1801]	= &stmpe1801,
 	[STMPE2401]	= &stmpe2401,
 	[STMPE2403]	= &stmpe2403,
 };
@@ -759,7 +843,7 @@ static irqreturn_t stmpe_irq(int irq, void *data)
 	struct stmpe *stmpe = data;
 	struct stmpe_variant_info *variant = stmpe->variant;
 	int num = DIV_ROUND_UP(variant->num_irqs, 8);
-	u8 israddr = stmpe->regs[STMPE_IDX_ISR_MSB];
+	u8 israddr;
 	u8 isr[num];
 	int ret;
 	int i;
@@ -771,6 +855,11 @@ static irqreturn_t stmpe_irq(int irq, void *data)
 		return IRQ_HANDLED;
 	}
 
+	if (variant->id_val == STMPE1801_ID)
+		israddr = stmpe->regs[STMPE_IDX_ISR_LSB];
+	else
+		israddr = stmpe->regs[STMPE_IDX_ISR_MSB];
+
 	ret = stmpe_block_read(stmpe, israddr, num, isr);
 	if (ret < 0)
 		return IRQ_NONE;
@@ -938,6 +1027,12 @@ static int stmpe_chip_init(struct stmpe *stmpe)
 	if (ret)
 		return ret;
 
+	if (id == STMPE1801_ID)	{
+		ret =  stmpe1801_reset(stmpe);
+		if (ret < 0)
+			return ret;
+	}
+
 	if (stmpe->irq >= 0) {
 		if (id == STMPE801_ID)
 			icr = STMPE801_REG_SYS_CTRL_INT_EN;
@@ -1015,7 +1110,10 @@ void stmpe_of_probe(struct stmpe_platform_data *pdata, struct device_node *np)
 {
 	struct device_node *child;
 
-	pdata->id = -1;
+	pdata->id = of_alias_get_id(np, "stmpe-i2c");
+	if (pdata->id < 0)
+		pdata->id = -1;
+
 	pdata->irq_trigger = IRQF_TRIGGER_NONE;
 
 	of_property_read_u32(np, "st,autosleep-timeout",
@@ -1057,6 +1155,9 @@ int stmpe_probe(struct stmpe_client_info *ci, int partnum)
 			return -ENOMEM;
 
 		stmpe_of_probe(pdata, np);
+
+		if (of_find_property(np, "interrupts", NULL) == NULL)
+			ci->irq = -1;
 	}
 
 	stmpe = devm_kzalloc(ci->dev, sizeof(struct stmpe), GFP_KERNEL);
diff --git a/drivers/mfd/stmpe.h b/drivers/mfd/stmpe.h
index 7b8e13f5b764..ff2b09ba8797 100644
--- a/drivers/mfd/stmpe.h
+++ b/drivers/mfd/stmpe.h
@@ -199,6 +199,55 @@ int stmpe_remove(struct stmpe *stmpe);
 #define STPME1601_AUTOSLEEP_ENABLE		(1 << 3)
 
 /*
+ * STMPE1801
+ */
+#define STMPE1801_ID			0xc110
+#define STMPE1801_NR_INTERNAL_IRQS	5
+#define STMPE1801_IRQ_KEYPAD_COMBI	4
+#define STMPE1801_IRQ_GPIOC		3
+#define STMPE1801_IRQ_KEYPAD_OVER	2
+#define STMPE1801_IRQ_KEYPAD		1
+#define STMPE1801_IRQ_WAKEUP		0
+
+#define STMPE1801_REG_CHIP_ID			0x00
+#define STMPE1801_REG_SYS_CTRL			0x02
+#define STMPE1801_REG_INT_CTRL_LOW		0x04
+#define STMPE1801_REG_INT_EN_MASK_LOW		0x06
+#define STMPE1801_REG_INT_STA_LOW		0x08
+#define STMPE1801_REG_INT_EN_GPIO_MASK_LOW	0x0A
+#define STMPE1801_REG_INT_EN_GPIO_MASK_MID	0x0B
+#define STMPE1801_REG_INT_EN_GPIO_MASK_HIGH	0x0C
+#define STMPE1801_REG_INT_STA_GPIO_LOW		0x0D
+#define STMPE1801_REG_INT_STA_GPIO_MID		0x0E
+#define STMPE1801_REG_INT_STA_GPIO_HIGH		0x0F
+#define STMPE1801_REG_GPIO_SET_LOW		0x10
+#define STMPE1801_REG_GPIO_SET_MID		0x11
+#define STMPE1801_REG_GPIO_SET_HIGH		0x12
+#define STMPE1801_REG_GPIO_CLR_LOW		0x13
+#define STMPE1801_REG_GPIO_CLR_MID		0x14
+#define STMPE1801_REG_GPIO_CLR_HIGH		0x15
+#define STMPE1801_REG_GPIO_MP_LOW		0x16
+#define STMPE1801_REG_GPIO_MP_MID		0x17
+#define STMPE1801_REG_GPIO_MP_HIGH		0x18
+#define STMPE1801_REG_GPIO_SET_DIR_LOW		0x19
+#define STMPE1801_REG_GPIO_SET_DIR_MID		0x1A
+#define STMPE1801_REG_GPIO_SET_DIR_HIGH		0x1B
+#define STMPE1801_REG_GPIO_RE_LOW		0x1C
+#define STMPE1801_REG_GPIO_RE_MID		0x1D
+#define STMPE1801_REG_GPIO_RE_HIGH		0x1E
+#define STMPE1801_REG_GPIO_FE_LOW		0x1F
+#define STMPE1801_REG_GPIO_FE_MID		0x20
+#define STMPE1801_REG_GPIO_FE_HIGH		0x21
+#define STMPE1801_REG_GPIO_PULL_UP_LOW		0x22
+#define STMPE1801_REG_GPIO_PULL_UP_MID		0x23
+#define STMPE1801_REG_GPIO_PULL_UP_HIGH		0x24
+
+#define STMPE1801_MSK_SYS_CTRL_RESET		(1 << 7)
+
+#define STMPE1801_MSK_INT_EN_KPC		(1 << 1)
+#define STMPE1801_MSK_INT_EN_GPIO		(1 << 3)
+
+/*
  * STMPE24xx
  */
 
diff --git a/drivers/mfd/syscon.c b/drivers/mfd/syscon.c
index 61aea6381cdf..962a6e17a01a 100644
--- a/drivers/mfd/syscon.c
+++ b/drivers/mfd/syscon.c
@@ -25,17 +25,15 @@
 static struct platform_driver syscon_driver;
 
 struct syscon {
-	struct device *dev;
 	void __iomem *base;
 	struct regmap *regmap;
 };
 
-static int syscon_match(struct device *dev, void *data)
+static int syscon_match_node(struct device *dev, void *data)
 {
-	struct syscon *syscon = dev_get_drvdata(dev);
 	struct device_node *dn = data;
 
-	return (syscon->dev->of_node == dn) ? 1 : 0;
+	return (dev->of_node == dn) ? 1 : 0;
 }
 
 struct regmap *syscon_node_to_regmap(struct device_node *np)
@@ -44,7 +42,7 @@ struct regmap *syscon_node_to_regmap(struct device_node *np)
 	struct device *dev;
 
 	dev = driver_find_device(&syscon_driver.driver, NULL, np,
-				 syscon_match);
+				 syscon_match_node);
 	if (!dev)
 		return ERR_PTR(-EPROBE_DEFER);
 
@@ -70,6 +68,34 @@ struct regmap *syscon_regmap_lookup_by_compatible(const char *s)
 }
 EXPORT_SYMBOL_GPL(syscon_regmap_lookup_by_compatible);
 
+static int syscon_match_pdevname(struct device *dev, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	const struct platform_device_id *id = platform_get_device_id(pdev);
+
+	if (id)
+		if (!strcmp(id->name, (const char *)data))
+			return 1;
+
+	return !strcmp(dev_name(dev), (const char *)data);
+}
+
+struct regmap *syscon_regmap_lookup_by_pdevname(const char *s)
+{
+	struct device *dev;
+	struct syscon *syscon;
+
+	dev = driver_find_device(&syscon_driver.driver, NULL, (void *)s,
+				 syscon_match_pdevname);
+	if (!dev)
+		return ERR_PTR(-EPROBE_DEFER);
+
+	syscon = dev_get_drvdata(dev);
+
+	return syscon->regmap;
+}
+EXPORT_SYMBOL_GPL(syscon_regmap_lookup_by_pdevname);
+
 struct regmap *syscon_regmap_lookup_by_phandle(struct device_node *np,
 					const char *property)
 {
@@ -101,28 +127,22 @@ static struct regmap_config syscon_regmap_config = {
 static int syscon_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
-	struct device_node *np = dev->of_node;
 	struct syscon *syscon;
-	struct resource res;
-	int ret;
-
-	if (!np)
-		return -ENOENT;
+	struct resource *res;
 
-	syscon = devm_kzalloc(dev, sizeof(struct syscon),
-			    GFP_KERNEL);
+	syscon = devm_kzalloc(dev, sizeof(*syscon), GFP_KERNEL);
 	if (!syscon)
 		return -ENOMEM;
 
-	syscon->base = of_iomap(np, 0);
-	if (!syscon->base)
-		return -EADDRNOTAVAIL;
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENOENT;
 
-	ret = of_address_to_resource(np, 0, &res);
-	if (ret)
-		return ret;
+	syscon->base = devm_ioremap(dev, res->start, resource_size(res));
+	if (!syscon->base)
+		return -ENOMEM;
 
-	syscon_regmap_config.max_register = res.end - res.start - 3;
+	syscon_regmap_config.max_register = res->end - res->start - 3;
 	syscon->regmap = devm_regmap_init_mmio(dev, syscon->base,
 					&syscon_regmap_config);
 	if (IS_ERR(syscon->regmap)) {
@@ -130,25 +150,17 @@ static int syscon_probe(struct platform_device *pdev)
 		return PTR_ERR(syscon->regmap);
 	}
 
-	syscon->dev = dev;
 	platform_set_drvdata(pdev, syscon);
 
-	dev_info(dev, "syscon regmap start 0x%x end 0x%x registered\n",
-		res.start, res.end);
+	dev_info(dev, "regmap %pR registered\n", res);
 
 	return 0;
 }
 
-static int syscon_remove(struct platform_device *pdev)
-{
-	struct syscon *syscon;
-
-	syscon = platform_get_drvdata(pdev);
-	iounmap(syscon->base);
-	platform_set_drvdata(pdev, NULL);
-
-	return 0;
-}
+static const struct platform_device_id syscon_ids[] = {
+	{ "syscon", },
+	{ }
+};
 
 static struct platform_driver syscon_driver = {
 	.driver = {
@@ -157,7 +169,7 @@ static struct platform_driver syscon_driver = {
 		.of_match_table = of_syscon_match,
 	},
 	.probe		= syscon_probe,
-	.remove		= syscon_remove,
+	.id_table	= syscon_ids,
 };
 
 static int __init syscon_init(void)
diff --git a/drivers/mfd/tc3589x.c b/drivers/mfd/tc3589x.c
index ecc092c7f745..4cb92bb2aea2 100644
--- a/drivers/mfd/tc3589x.c
+++ b/drivers/mfd/tc3589x.c
@@ -350,7 +350,8 @@ static int tc3589x_probe(struct i2c_client *i2c,
 				     | I2C_FUNC_SMBUS_I2C_BLOCK))
 		return -EIO;
 
-	tc3589x = kzalloc(sizeof(struct tc3589x), GFP_KERNEL);
+	tc3589x = devm_kzalloc(&i2c->dev, sizeof(struct tc3589x),
+				GFP_KERNEL);
 	if (!tc3589x)
 		return -ENOMEM;
 
@@ -366,33 +367,27 @@ static int tc3589x_probe(struct i2c_client *i2c,
 
 	ret = tc3589x_chip_init(tc3589x);
 	if (ret)
-		goto out_free;
+		return ret;
 
 	ret = tc3589x_irq_init(tc3589x, np);
 	if (ret)
-		goto out_free;
+		return ret;
 
 	ret = request_threaded_irq(tc3589x->i2c->irq, NULL, tc3589x_irq,
 				   IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
 				   "tc3589x", tc3589x);
 	if (ret) {
 		dev_err(tc3589x->dev, "failed to request IRQ: %d\n", ret);
-		goto out_free;
+		return ret;
 	}
 
 	ret = tc3589x_device_init(tc3589x);
 	if (ret) {
 		dev_err(tc3589x->dev, "failed to add child devices\n");
-		goto out_freeirq;
+		return ret;
 	}
 
 	return 0;
-
-out_freeirq:
-	free_irq(tc3589x->i2c->irq, tc3589x);
-out_free:
-	kfree(tc3589x);
-	return ret;
 }
 
 static int tc3589x_remove(struct i2c_client *client)
@@ -401,10 +396,6 @@ static int tc3589x_remove(struct i2c_client *client)
 
 	mfd_remove_devices(tc3589x->dev);
 
-	free_irq(tc3589x->i2c->irq, tc3589x);
-
-	kfree(tc3589x);
-
 	return 0;
 }
 
diff --git a/drivers/mfd/tps65090.c b/drivers/mfd/tps65090.c
index 98edb5be85c6..fbd6ee67b5a5 100644
--- a/drivers/mfd/tps65090.c
+++ b/drivers/mfd/tps65090.c
@@ -56,12 +56,23 @@
 #define TPS65090_INT2_MASK_OVERLOAD_FET6		6
 #define TPS65090_INT2_MASK_OVERLOAD_FET7		7
 
+static struct resource charger_resources[] = {
+	{
+		.start  = TPS65090_IRQ_VAC_STATUS_CHANGE,
+		.end    = TPS65090_IRQ_VAC_STATUS_CHANGE,
+		.flags  = IORESOURCE_IRQ,
+	}
+};
+
 static struct mfd_cell tps65090s[] = {
 	{
 		.name = "tps65090-pmic",
 	},
 	{
 		.name = "tps65090-charger",
+		.num_resources = ARRAY_SIZE(charger_resources),
+		.resources = &charger_resources[0],
+		.of_compatible = "ti,tps65090-charger",
 	},
 };
 
diff --git a/drivers/mfd/twl4030-madc.c b/drivers/mfd/twl4030-madc.c
index 942b666a2a07..42bd3ea5df3c 100644
--- a/drivers/mfd/twl4030-madc.c
+++ b/drivers/mfd/twl4030-madc.c
@@ -211,12 +211,14 @@ static int twl4030battery_current(int raw_volt)
  * @reg_base - Base address of the first channel
  * @Channels - 16 bit bitmap. If the bit is set, channel value is read
  * @buf - The channel values are stored here. if read fails error
+ * @raw - Return raw values without conversion
  * value is stored
  * Returns the number of successfully read channels.
  */
 static int twl4030_madc_read_channels(struct twl4030_madc_data *madc,
 				      u8 reg_base, unsigned
-						long channels, int *buf)
+				      long channels, int *buf,
+				      bool raw)
 {
 	int count = 0, count_req = 0, i;
 	u8 reg;
@@ -230,6 +232,10 @@ static int twl4030_madc_read_channels(struct twl4030_madc_data *madc,
 			count_req++;
 			continue;
 		}
+		if (raw) {
+			count++;
+			continue;
+		}
 		switch (i) {
 		case 10:
 			buf[i] = twl4030battery_current(buf[i]);
@@ -371,7 +377,7 @@ static irqreturn_t twl4030_madc_threaded_irq_handler(int irq, void *_madc)
 		method = &twl4030_conversion_methods[r->method];
 		/* Read results */
 		len = twl4030_madc_read_channels(madc, method->rbase,
-						 r->channels, r->rbuf);
+						 r->channels, r->rbuf, r->raw);
 		/* Return results to caller */
 		if (r->func_cb != NULL) {
 			r->func_cb(len, r->channels, r->rbuf);
@@ -397,7 +403,7 @@ err_i2c:
 		method = &twl4030_conversion_methods[r->method];
 		/* Read results */
 		len = twl4030_madc_read_channels(madc, method->rbase,
-						 r->channels, r->rbuf);
+						 r->channels, r->rbuf, r->raw);
 		/* Return results to caller */
 		if (r->func_cb != NULL) {
 			r->func_cb(len, r->channels, r->rbuf);
@@ -585,7 +591,7 @@ int twl4030_madc_conversion(struct twl4030_madc_request *req)
 		goto out;
 	}
 	ret = twl4030_madc_read_channels(twl4030_madc, method->rbase,
-					 req->channels, req->rbuf);
+					 req->channels, req->rbuf, req->raw);
 	twl4030_madc->requests[req->method].active = 0;
 
 out:
diff --git a/drivers/mfd/twl6040.c b/drivers/mfd/twl6040.c
index f361bf38a0aa..492ee2cd3400 100644
--- a/drivers/mfd/twl6040.c
+++ b/drivers/mfd/twl6040.c
@@ -554,7 +554,7 @@ static int twl6040_probe(struct i2c_client *client,
 
 	twl6040->supplies[0].supply = "vio";
 	twl6040->supplies[1].supply = "v2v1";
-	ret = regulator_bulk_get(&client->dev, TWL6040_NUM_SUPPLIES,
+	ret = devm_regulator_bulk_get(&client->dev, TWL6040_NUM_SUPPLIES,
 				 twl6040->supplies);
 	if (ret != 0) {
 		dev_err(&client->dev, "Failed to get supplies: %d\n", ret);
@@ -564,7 +564,7 @@ static int twl6040_probe(struct i2c_client *client,
 	ret = regulator_bulk_enable(TWL6040_NUM_SUPPLIES, twl6040->supplies);
 	if (ret != 0) {
 		dev_err(&client->dev, "Failed to enable supplies: %d\n", ret);
-		goto power_err;
+		goto regulator_get_err;
 	}
 
 	twl6040->dev = &client->dev;
@@ -586,8 +586,8 @@ static int twl6040_probe(struct i2c_client *client,
 		twl6040->audpwron = -EINVAL;
 
 	if (gpio_is_valid(twl6040->audpwron)) {
-		ret = gpio_request_one(twl6040->audpwron, GPIOF_OUT_INIT_LOW,
-				       "audpwron");
+		ret = devm_gpio_request_one(&client->dev, twl6040->audpwron,
+					GPIOF_OUT_INIT_LOW, "audpwron");
 		if (ret)
 			goto gpio_err;
 	}
@@ -596,14 +596,14 @@ static int twl6040_probe(struct i2c_client *client,
 			IRQF_ONESHOT, 0, &twl6040_irq_chip,
 			&twl6040->irq_data);
 	if (ret < 0)
-		goto irq_init_err;
+		goto gpio_err;
 
 	twl6040->irq_ready = regmap_irq_get_virq(twl6040->irq_data,
 					       TWL6040_IRQ_READY);
 	twl6040->irq_th = regmap_irq_get_virq(twl6040->irq_data,
 					       TWL6040_IRQ_TH);
 
-	ret = request_threaded_irq(twl6040->irq_ready, NULL,
+	ret = devm_request_threaded_irq(twl6040->dev, twl6040->irq_ready, NULL,
 				   twl6040_readyint_handler, IRQF_ONESHOT,
 				   "twl6040_irq_ready", twl6040);
 	if (ret) {
@@ -611,7 +611,7 @@ static int twl6040_probe(struct i2c_client *client,
 		goto readyirq_err;
 	}
 
-	ret = request_threaded_irq(twl6040->irq_th, NULL,
+	ret = devm_request_threaded_irq(twl6040->dev, twl6040->irq_th, NULL,
 				   twl6040_thint_handler, IRQF_ONESHOT,
 				   "twl6040_irq_th", twl6040);
 	if (ret) {
@@ -681,18 +681,13 @@ static int twl6040_probe(struct i2c_client *client,
 	return 0;
 
 mfd_err:
-	free_irq(twl6040->irq_th, twl6040);
+	devm_free_irq(&client->dev, twl6040->irq_th, twl6040);
 thirq_err:
-	free_irq(twl6040->irq_ready, twl6040);
+	devm_free_irq(&client->dev, twl6040->irq_ready, twl6040);
 readyirq_err:
 	regmap_del_irq_chip(twl6040->irq, twl6040->irq_data);
-irq_init_err:
-	if (gpio_is_valid(twl6040->audpwron))
-		gpio_free(twl6040->audpwron);
 gpio_err:
 	regulator_bulk_disable(TWL6040_NUM_SUPPLIES, twl6040->supplies);
-power_err:
-	regulator_bulk_free(TWL6040_NUM_SUPPLIES, twl6040->supplies);
 regulator_get_err:
 	i2c_set_clientdata(client, NULL);
 err:
@@ -706,18 +701,14 @@ static int twl6040_remove(struct i2c_client *client)
 	if (twl6040->power_count)
 		twl6040_power(twl6040, 0);
 
-	if (gpio_is_valid(twl6040->audpwron))
-		gpio_free(twl6040->audpwron);
-
-	free_irq(twl6040->irq_ready, twl6040);
-	free_irq(twl6040->irq_th, twl6040);
+	devm_free_irq(&client->dev, twl6040->irq_ready, twl6040);
+	devm_free_irq(&client->dev, twl6040->irq_th, twl6040);
 	regmap_del_irq_chip(twl6040->irq, twl6040->irq_data);
 
 	mfd_remove_devices(&client->dev);
 	i2c_set_clientdata(client, NULL);
 
 	regulator_bulk_disable(TWL6040_NUM_SUPPLIES, twl6040->supplies);
-	regulator_bulk_free(TWL6040_NUM_SUPPLIES, twl6040->supplies);
 
 	return 0;
 }
diff --git a/drivers/mfd/ucb1400_core.c b/drivers/mfd/ucb1400_core.c
index daf69527ed83..e9031fa9d53d 100644
--- a/drivers/mfd/ucb1400_core.c
+++ b/drivers/mfd/ucb1400_core.c
@@ -75,6 +75,11 @@ static int ucb1400_core_probe(struct device *dev)
 
 	/* GPIO */
 	ucb_gpio.ac97 = ac97;
+	if (pdata) {
+		ucb_gpio.gpio_setup = pdata->gpio_setup;
+		ucb_gpio.gpio_teardown = pdata->gpio_teardown;
+		ucb_gpio.gpio_offset = pdata->gpio_offset;
+	}
 	ucb->ucb1400_gpio = platform_device_alloc("ucb1400_gpio", -1);
 	if (!ucb->ucb1400_gpio) {
 		err = -ENOMEM;
diff --git a/drivers/mfd/vexpress-config.c b/drivers/mfd/vexpress-config.c
index 3c1723aa6225..84ce6b9daa3d 100644
--- a/drivers/mfd/vexpress-config.c
+++ b/drivers/mfd/vexpress-config.c
@@ -184,13 +184,14 @@ static int vexpress_config_schedule(struct vexpress_config_trans *trans)
 
 	spin_lock_irqsave(&bridge->transactions_lock, flags);
 
-	vexpress_config_dump_trans("Executing", trans);
-
-	if (list_empty(&bridge->transactions))
+	if (list_empty(&bridge->transactions)) {
+		vexpress_config_dump_trans("Executing", trans);
 		status = bridge->info->func_exec(trans->func->func,
 				trans->offset, trans->write, trans->data);
-	else
+	} else {
+		vexpress_config_dump_trans("Queuing", trans);
 		status = VEXPRESS_CONFIG_STATUS_WAIT;
+	}
 
 	switch (status) {
 	case VEXPRESS_CONFIG_STATUS_DONE:
@@ -212,25 +213,31 @@ void vexpress_config_complete(struct vexpress_config_bridge *bridge,
 {
 	struct vexpress_config_trans *trans;
 	unsigned long flags;
+	const char *message = "Completed";
 
 	spin_lock_irqsave(&bridge->transactions_lock, flags);
 
 	trans = list_first_entry(&bridge->transactions,
 			struct vexpress_config_trans, list);
-	vexpress_config_dump_trans("Completed", trans);
-
 	trans->status = status;
-	list_del(&trans->list);
 
-	if (!list_empty(&bridge->transactions)) {
-		vexpress_config_dump_trans("Pending", trans);
+	do {
+		vexpress_config_dump_trans(message, trans);
+		list_del(&trans->list);
+		complete(&trans->completion);
 
-		bridge->info->func_exec(trans->func->func, trans->offset,
-				trans->write, trans->data);
-	}
-	spin_unlock_irqrestore(&bridge->transactions_lock, flags);
+		if (list_empty(&bridge->transactions))
+			break;
+
+		trans = list_first_entry(&bridge->transactions,
+				struct vexpress_config_trans, list);
+		vexpress_config_dump_trans("Executing pending", trans);
+		trans->status = bridge->info->func_exec(trans->func->func,
+				trans->offset, trans->write, trans->data);
+		message = "Finished pending";
+	} while (trans->status == VEXPRESS_CONFIG_STATUS_DONE);
 
-	complete(&trans->completion);
+	spin_unlock_irqrestore(&bridge->transactions_lock, flags);
 }
 EXPORT_SYMBOL(vexpress_config_complete);
 
diff --git a/drivers/mfd/vexpress-sysreg.c b/drivers/mfd/vexpress-sysreg.c
index bf75e967a1f3..96a020b1dcd1 100644
--- a/drivers/mfd/vexpress-sysreg.c
+++ b/drivers/mfd/vexpress-sysreg.c
@@ -490,12 +490,12 @@ static int vexpress_sysreg_probe(struct platform_device *pdev)
 		return err;
 	}
 
+	vexpress_sysreg_dev = &pdev->dev;
+
 	platform_device_register_data(vexpress_sysreg_dev, "leds-gpio",
 			PLATFORM_DEVID_AUTO, &vexpress_sysreg_leds_pdata,
 			sizeof(vexpress_sysreg_leds_pdata));
 
-	vexpress_sysreg_dev = &pdev->dev;
-
 	device_create_file(vexpress_sysreg_dev, &dev_attr_sys_id);
 
 	return 0;
diff --git a/drivers/mfd/wm5102-tables.c b/drivers/mfd/wm5102-tables.c
index ca2aed6bc830..155c4a1a6a99 100644
--- a/drivers/mfd/wm5102-tables.c
+++ b/drivers/mfd/wm5102-tables.c
@@ -10,6 +10,7 @@
  * published by the Free Software Foundation.
  */
 
+#include <linux/device.h>
 #include <linux/module.h>
 
 #include <linux/mfd/arizona/core.h>
@@ -57,31 +58,54 @@ static const struct reg_default wm5102_reva_patch[] = {
 };
 
 static const struct reg_default wm5102_revb_patch[] = {
+	{ 0x19, 0x0001 },
 	{ 0x80, 0x0003 },
 	{ 0x081, 0xE022 },
-	{ 0x410, 0x4080 },
-	{ 0x418, 0x4080 },
-	{ 0x420, 0x4080 },
-	{ 0x428, 0xC000 },
+	{ 0x410, 0x6080 },
+	{ 0x418, 0xa080 },
+	{ 0x420, 0xa080 },
+	{ 0x428, 0xe000 },
+	{ 0x443, 0xDC1A },
 	{ 0x4B0, 0x0066 },
 	{ 0x458, 0x000b },
 	{ 0x212, 0x0000 },
+	{ 0x171, 0x0000 },
+	{ 0x35E, 0x000C },
+	{ 0x2D4, 0x0000 },
 	{ 0x80, 0x0000 },
 };
 
 /* We use a function so we can use ARRAY_SIZE() */
 int wm5102_patch(struct arizona *arizona)
 {
+	const struct reg_default *wm5102_patch;
+	int ret = 0;
+	int i, patch_size;
+
 	switch (arizona->rev) {
 	case 0:
-		return regmap_register_patch(arizona->regmap,
-					     wm5102_reva_patch,
-					     ARRAY_SIZE(wm5102_reva_patch));
+		wm5102_patch = wm5102_reva_patch;
+		patch_size = ARRAY_SIZE(wm5102_reva_patch);
 	default:
-		return regmap_register_patch(arizona->regmap,
-					     wm5102_revb_patch,
-					     ARRAY_SIZE(wm5102_revb_patch));
+		wm5102_patch = wm5102_revb_patch;
+		patch_size = ARRAY_SIZE(wm5102_revb_patch);
+	}
+
+	regcache_cache_bypass(arizona->regmap, true);
+
+	for (i = 0; i < patch_size; i++) {
+		ret = regmap_write(arizona->regmap, wm5102_patch[i].reg,
+				   wm5102_patch[i].def);
+		if (ret != 0) {
+			dev_err(arizona->dev, "Failed to write %x = %x: %d\n",
+				wm5102_patch[i].reg, wm5102_patch[i].def, ret);
+			goto out;
+		}
 	}
+
+out:
+	regcache_cache_bypass(arizona->regmap, false);
+	return ret;
 }
 
 static const struct regmap_irq wm5102_aod_irqs[ARIZONA_NUM_IRQ] = {
@@ -282,7 +306,7 @@ static const struct reg_default wm5102_reg_default[] = {
 	{ 0x00000155, 0x0000 },   /* R341   - Rate Estimator 4 */ 
 	{ 0x00000156, 0x0000 },   /* R342   - Rate Estimator 5 */ 
 	{ 0x00000161, 0x0000 },   /* R353   - Dynamic Frequency Scaling 1 */ 
-	{ 0x00000171, 0x0002 },   /* R369   - FLL1 Control 1 */
+	{ 0x00000171, 0x0000 },   /* R369   - FLL1 Control 1 */
 	{ 0x00000172, 0x0008 },   /* R370   - FLL1 Control 2 */ 
 	{ 0x00000173, 0x0018 },   /* R371   - FLL1 Control 3 */ 
 	{ 0x00000174, 0x007D },   /* R372   - FLL1 Control 4 */ 
@@ -290,12 +314,14 @@ static const struct reg_default wm5102_reg_default[] = {
 	{ 0x00000176, 0x0000 },   /* R374   - FLL1 Control 6 */ 
 	{ 0x00000177, 0x0181 },   /* R375   - FLL1 Loop Filter Test 1 */ 
 	{ 0x00000178, 0x0000 },   /* R376   - FLL1 NCO Test 0 */
+	{ 0x00000179, 0x0000 },   /* R377   - FLL1 Control 7 */
 	{ 0x00000181, 0x0000 },   /* R385   - FLL1 Synchroniser 1 */ 
 	{ 0x00000182, 0x0000 },   /* R386   - FLL1 Synchroniser 2 */ 
 	{ 0x00000183, 0x0000 },   /* R387   - FLL1 Synchroniser 3 */ 
 	{ 0x00000184, 0x0000 },   /* R388   - FLL1 Synchroniser 4 */ 
 	{ 0x00000185, 0x0000 },   /* R389   - FLL1 Synchroniser 5 */ 
 	{ 0x00000186, 0x0000 },   /* R390   - FLL1 Synchroniser 6 */ 
+	{ 0x00000187, 0x0001 },   /* R391   - FLL1 Synchroniser 7 */
 	{ 0x00000189, 0x0000 },   /* R393   - FLL1 Spread Spectrum */ 
 	{ 0x0000018A, 0x0004 },   /* R394   - FLL1 GPIO Clock */ 
 	{ 0x00000191, 0x0000 },   /* R401   - FLL2 Control 1 */ 
@@ -306,12 +332,14 @@ static const struct reg_default wm5102_reg_default[] = {
 	{ 0x00000196, 0x0000 },   /* R406   - FLL2 Control 6 */ 
 	{ 0x00000197, 0x0000 },   /* R407   - FLL2 Loop Filter Test 1 */ 
 	{ 0x00000198, 0x0000 },   /* R408   - FLL2 NCO Test 0 */
+	{ 0x00000199, 0x0000 },   /* R409   - FLL2 Control 7 */
 	{ 0x000001A1, 0x0000 },   /* R417   - FLL2 Synchroniser 1 */ 
 	{ 0x000001A2, 0x0000 },   /* R418   - FLL2 Synchroniser 2 */ 
 	{ 0x000001A3, 0x0000 },   /* R419   - FLL2 Synchroniser 3 */ 
 	{ 0x000001A4, 0x0000 },   /* R420   - FLL2 Synchroniser 4 */ 
 	{ 0x000001A5, 0x0000 },   /* R421   - FLL2 Synchroniser 5 */ 
 	{ 0x000001A6, 0x0000 },   /* R422   - FLL2 Synchroniser 6 */ 
+	{ 0x000001A7, 0x0001 },   /* R423   - FLL2 Synchroniser 7 */
 	{ 0x000001A9, 0x0000 },   /* R425   - FLL2 Spread Spectrum */ 
 	{ 0x000001AA, 0x0004 },   /* R426   - FLL2 GPIO Clock */ 
 	{ 0x00000200, 0x0006 },   /* R512   - Mic Charge Pump 1 */ 
@@ -362,7 +390,7 @@ static const struct reg_default wm5102_reg_default[] = {
 	{ 0x00000400, 0x0000 },   /* R1024  - Output Enables 1 */ 
 	{ 0x00000408, 0x0000 },   /* R1032  - Output Rate 1 */ 
 	{ 0x00000409, 0x0022 },   /* R1033  - Output Volume Ramp */ 
-	{ 0x00000410, 0x4080 },   /* R1040  - Output Path Config 1L */
+	{ 0x00000410, 0x6080 },   /* R1040  - Output Path Config 1L */
 	{ 0x00000411, 0x0180 },   /* R1041  - DAC Digital Volume 1L */ 
 	{ 0x00000412, 0x0081 },   /* R1042  - DAC Volume Limit 1L */
 	{ 0x00000413, 0x0001 },   /* R1043  - Noise Gate Select 1L */ 
@@ -370,7 +398,7 @@ static const struct reg_default wm5102_reg_default[] = {
 	{ 0x00000415, 0x0180 },   /* R1045  - DAC Digital Volume 1R */ 
 	{ 0x00000416, 0x0081 },   /* R1046  - DAC Volume Limit 1R */
 	{ 0x00000417, 0x0002 },   /* R1047  - Noise Gate Select 1R */ 
-	{ 0x00000418, 0x4080 },   /* R1048  - Output Path Config 2L */
+	{ 0x00000418, 0xA080 },   /* R1048  - Output Path Config 2L */
 	{ 0x00000419, 0x0180 },   /* R1049  - DAC Digital Volume 2L */ 
 	{ 0x0000041A, 0x0081 },   /* R1050  - DAC Volume Limit 2L */
 	{ 0x0000041B, 0x0004 },   /* R1051  - Noise Gate Select 2L */ 
@@ -378,11 +406,11 @@ static const struct reg_default wm5102_reg_default[] = {
 	{ 0x0000041D, 0x0180 },   /* R1053  - DAC Digital Volume 2R */ 
 	{ 0x0000041E, 0x0081 },   /* R1054  - DAC Volume Limit 2R */
 	{ 0x0000041F, 0x0008 },   /* R1055  - Noise Gate Select 2R */ 
-	{ 0x00000420, 0x4080 },   /* R1056  - Output Path Config 3L */
+	{ 0x00000420, 0xA080 },   /* R1056  - Output Path Config 3L */
 	{ 0x00000421, 0x0180 },   /* R1057  - DAC Digital Volume 3L */ 
 	{ 0x00000422, 0x0081 },   /* R1058  - DAC Volume Limit 3L */
 	{ 0x00000423, 0x0010 },   /* R1059  - Noise Gate Select 3L */ 
-	{ 0x00000428, 0xC000 },   /* R1064  - Output Path Config 4L */
+	{ 0x00000428, 0xE000 },   /* R1064  - Output Path Config 4L */
 	{ 0x00000429, 0x0180 },   /* R1065  - DAC Digital Volume 4L */ 
 	{ 0x0000042A, 0x0081 },   /* R1066  - Out Volume 4L */
 	{ 0x0000042B, 0x0040 },   /* R1067  - Noise Gate Select 4L */ 
@@ -397,7 +425,7 @@ static const struct reg_default wm5102_reg_default[] = {
 	{ 0x00000436, 0x0081 },   /* R1078  - DAC Volume Limit 5R */
 	{ 0x00000437, 0x0200 },   /* R1079  - Noise Gate Select 5R */
 	{ 0x00000450, 0x0000 },   /* R1104  - DAC AEC Control 1 */ 
-	{ 0x00000458, 0x0001 },   /* R1112  - Noise Gate Control */ 
+	{ 0x00000458, 0x000B },   /* R1112  - Noise Gate Control */
 	{ 0x00000490, 0x0069 },   /* R1168  - PDM SPK1 CTRL 1 */ 
 	{ 0x00000491, 0x0000 },   /* R1169  - PDM SPK1 CTRL 2 */ 
 	{ 0x00000500, 0x000C },   /* R1280  - AIF1 BCLK Ctrl */ 
@@ -1055,12 +1083,14 @@ static bool wm5102_readable_register(struct device *dev, unsigned int reg)
 	case ARIZONA_FLL1_CONTROL_6:
 	case ARIZONA_FLL1_LOOP_FILTER_TEST_1:
 	case ARIZONA_FLL1_NCO_TEST_0:
+	case ARIZONA_FLL1_CONTROL_7:
 	case ARIZONA_FLL1_SYNCHRONISER_1:
 	case ARIZONA_FLL1_SYNCHRONISER_2:
 	case ARIZONA_FLL1_SYNCHRONISER_3:
 	case ARIZONA_FLL1_SYNCHRONISER_4:
 	case ARIZONA_FLL1_SYNCHRONISER_5:
 	case ARIZONA_FLL1_SYNCHRONISER_6:
+	case ARIZONA_FLL1_SYNCHRONISER_7:
 	case ARIZONA_FLL1_SPREAD_SPECTRUM:
 	case ARIZONA_FLL1_GPIO_CLOCK:
 	case ARIZONA_FLL2_CONTROL_1:
@@ -1071,12 +1101,14 @@ static bool wm5102_readable_register(struct device *dev, unsigned int reg)
 	case ARIZONA_FLL2_CONTROL_6:
 	case ARIZONA_FLL2_LOOP_FILTER_TEST_1:
 	case ARIZONA_FLL2_NCO_TEST_0:
+	case ARIZONA_FLL2_CONTROL_7:
 	case ARIZONA_FLL2_SYNCHRONISER_1:
 	case ARIZONA_FLL2_SYNCHRONISER_2:
 	case ARIZONA_FLL2_SYNCHRONISER_3:
 	case ARIZONA_FLL2_SYNCHRONISER_4:
 	case ARIZONA_FLL2_SYNCHRONISER_5:
 	case ARIZONA_FLL2_SYNCHRONISER_6:
+	case ARIZONA_FLL2_SYNCHRONISER_7:
 	case ARIZONA_FLL2_SPREAD_SPECTRUM:
 	case ARIZONA_FLL2_GPIO_CLOCK:
 	case ARIZONA_MIC_CHARGE_PUMP_1:
@@ -1169,6 +1201,8 @@ static bool wm5102_readable_register(struct device *dev, unsigned int reg)
 	case ARIZONA_NOISE_GATE_CONTROL:
 	case ARIZONA_PDM_SPK1_CTRL_1:
 	case ARIZONA_PDM_SPK1_CTRL_2:
+	case ARIZONA_SPK_CTRL_2:
+	case ARIZONA_SPK_CTRL_3:
 	case ARIZONA_DAC_COMP_1:
 	case ARIZONA_DAC_COMP_2:
 	case ARIZONA_DAC_COMP_3:
diff --git a/drivers/mfd/wm831x-spi.c b/drivers/mfd/wm831x-spi.c
index 4e70e157a909..e7ed14f661d8 100644
--- a/drivers/mfd/wm831x-spi.c
+++ b/drivers/mfd/wm831x-spi.c
@@ -37,7 +37,7 @@ static int wm831x_spi_probe(struct spi_device *spi)
 	spi->bits_per_word = 16;
 	spi->mode = SPI_MODE_0;
 
-	dev_set_drvdata(&spi->dev, wm831x);
+	spi_set_drvdata(spi, wm831x);
 	wm831x->dev = &spi->dev;
 
 	wm831x->regmap = devm_regmap_init_spi(spi, &wm831x_regmap_config);
@@ -53,7 +53,7 @@ static int wm831x_spi_probe(struct spi_device *spi)
 
 static int wm831x_spi_remove(struct spi_device *spi)
 {
-	struct wm831x *wm831x = dev_get_drvdata(&spi->dev);
+	struct wm831x *wm831x = spi_get_drvdata(spi);
 
 	wm831x_device_exit(wm831x);
 
@@ -69,7 +69,7 @@ static int wm831x_spi_suspend(struct device *dev)
 
 static void wm831x_spi_shutdown(struct spi_device *spi)
 {
-	struct wm831x *wm831x = dev_get_drvdata(&spi->dev);
+	struct wm831x *wm831x = spi_get_drvdata(spi);
 
 	wm831x_device_shutdown(wm831x);
 }
diff --git a/drivers/mfd/wm8994-core.c b/drivers/mfd/wm8994-core.c
index 803e93fae56a..00e4fe2f3c75 100644
--- a/drivers/mfd/wm8994-core.c
+++ b/drivers/mfd/wm8994-core.c
@@ -19,6 +19,9 @@
 #include <linux/err.h>
 #include <linux/delay.h>
 #include <linux/mfd/core.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
 #include <linux/pm_runtime.h>
 #include <linux/regmap.h>
 #include <linux/regulator/consumer.h>
@@ -191,7 +194,7 @@ static const char *wm8958_main_supplies[] = {
 	"SPKVDD2",
 };
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_RUNTIME
 static int wm8994_suspend(struct device *dev)
 {
 	struct wm8994 *wm8994 = dev_get_drvdata(dev);
@@ -396,6 +399,60 @@ static const struct reg_default wm1811_reva_patch[] = {
 	{ 0x102, 0x0 },
 };
 
+#ifdef CONFIG_OF
+static int wm8994_set_pdata_from_of(struct wm8994 *wm8994)
+{
+	struct device_node *np = wm8994->dev->of_node;
+	struct wm8994_pdata *pdata = &wm8994->pdata;
+	int i;
+
+	if (!np)
+		return 0;
+
+	if (of_property_read_u32_array(np, "wlf,gpio-cfg", pdata->gpio_defaults,
+				       ARRAY_SIZE(pdata->gpio_defaults)) >= 0) {
+		for (i = 0; i < ARRAY_SIZE(pdata->gpio_defaults); i++) {
+			if (wm8994->pdata.gpio_defaults[i] == 0)
+				pdata->gpio_defaults[i]
+					= WM8994_CONFIGURE_GPIO;
+		}
+	}
+
+	of_property_read_u32_array(np, "wlf,micbias-cfg", pdata->micbias,
+				   ARRAY_SIZE(pdata->micbias));
+
+	pdata->lineout1_diff = true;
+	pdata->lineout2_diff = true;
+	if (of_find_property(np, "wlf,lineout1-se", NULL))
+		pdata->lineout1_diff = false;
+	if (of_find_property(np, "wlf,lineout2-se", NULL))
+		pdata->lineout2_diff = false;
+
+	if (of_find_property(np, "wlf,lineout1-feedback", NULL))
+		pdata->lineout1fb = true;
+	if (of_find_property(np, "wlf,lineout2-feedback", NULL))
+		pdata->lineout2fb = true;
+
+	if (of_find_property(np, "wlf,ldoena-always-driven", NULL))
+		pdata->lineout2fb = true;
+
+	pdata->ldo[0].enable = of_get_named_gpio(np, "wlf,ldo1ena", 0);
+	if (pdata->ldo[0].enable < 0)
+		pdata->ldo[0].enable = 0;
+
+	pdata->ldo[1].enable = of_get_named_gpio(np, "wlf,ldo2ena", 0);
+	if (pdata->ldo[1].enable < 0)
+		pdata->ldo[1].enable = 0;
+
+	return 0;
+}
+#else
+static int wm8994_set_pdata_from_of(struct wm8994 *wm8994)
+{
+	return 0;
+}
+#endif
+
 /*
  * Instantiate the generic non-control parts of the device.
  */
@@ -405,7 +462,7 @@ static int wm8994_device_init(struct wm8994 *wm8994, int irq)
 	struct regmap_config *regmap_config;
 	const struct reg_default *regmap_patch = NULL;
 	const char *devname;
-	int ret, i, patch_regs;
+	int ret, i, patch_regs = 0;
 	int pulls = 0;
 
 	if (dev_get_platdata(wm8994->dev)) {
@@ -414,6 +471,10 @@ static int wm8994_device_init(struct wm8994 *wm8994, int irq)
 	}
 	pdata = &wm8994->pdata;
 
+	ret = wm8994_set_pdata_from_of(wm8994);
+	if (ret != 0)
+		return ret;
+
 	dev_set_drvdata(wm8994->dev, wm8994);
 
 	/* Add the on-chip regulators first for bootstrapping */
@@ -673,9 +734,9 @@ static void wm8994_device_exit(struct wm8994 *wm8994)
 }
 
 static const struct of_device_id wm8994_of_match[] = {
-	{ .compatible = "wlf,wm1811", },
-	{ .compatible = "wlf,wm8994", },
-	{ .compatible = "wlf,wm8958", },
+	{ .compatible = "wlf,wm1811", .data = (void *)WM1811 },
+	{ .compatible = "wlf,wm8994", .data = (void *)WM8994 },
+	{ .compatible = "wlf,wm8958", .data = (void *)WM8958 },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, wm8994_of_match);
@@ -683,6 +744,7 @@ MODULE_DEVICE_TABLE(of, wm8994_of_match);
 static int wm8994_i2c_probe(struct i2c_client *i2c,
 				      const struct i2c_device_id *id)
 {
+	const struct of_device_id *of_id;
 	struct wm8994 *wm8994;
 	int ret;
 
@@ -693,7 +755,14 @@ static int wm8994_i2c_probe(struct i2c_client *i2c,
 	i2c_set_clientdata(i2c, wm8994);
 	wm8994->dev = &i2c->dev;
 	wm8994->irq = i2c->irq;
-	wm8994->type = id->driver_data;
+
+	if (i2c->dev.of_node) {
+		of_id = of_match_device(wm8994_of_match, &i2c->dev);
+		if (of_id)
+			wm8994->type = (int)of_id->data;
+	} else {
+		wm8994->type = id->driver_data;
+	}
 
 	wm8994->regmap = devm_regmap_init_i2c(i2c, &wm8994_base_regmap_config);
 	if (IS_ERR(wm8994->regmap)) {
@@ -724,15 +793,16 @@ static const struct i2c_device_id wm8994_i2c_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, wm8994_i2c_id);
 
-static UNIVERSAL_DEV_PM_OPS(wm8994_pm_ops, wm8994_suspend, wm8994_resume,
-			    NULL);
+static const struct dev_pm_ops wm8994_pm_ops = {
+	SET_RUNTIME_PM_OPS(wm8994_suspend, wm8994_resume, NULL)
+};
 
 static struct i2c_driver wm8994_i2c_driver = {
 	.driver = {
 		.name = "wm8994",
 		.owner = THIS_MODULE,
 		.pm = &wm8994_pm_ops,
-		.of_match_table = wm8994_of_match,
+		.of_match_table = of_match_ptr(wm8994_of_match),
 	},
 	.probe = wm8994_i2c_probe,
 	.remove = wm8994_i2c_remove,
diff --git a/drivers/misc/lis3lv02d/lis3lv02d.c b/drivers/misc/lis3lv02d/lis3lv02d.c
index 4a87e5c0a320..4cd4a3d2a76a 100644
--- a/drivers/misc/lis3lv02d/lis3lv02d.c
+++ b/drivers/misc/lis3lv02d/lis3lv02d.c
@@ -593,7 +593,6 @@ static int lis3lv02d_misc_release(struct inode *inode, struct file *file)
 	struct lis3lv02d *lis3 = container_of(file->private_data,
 					      struct lis3lv02d, miscdev);
 
-	fasync_helper(-1, file, 0, &lis3->async_queue);
 	clear_bit(0, &lis3->misc_opened); /* release the device */
 	if (lis3->pm_dev)
 		pm_runtime_put(lis3->pm_dev);
diff --git a/drivers/misc/sgi-gru/gruprocfs.c b/drivers/misc/sgi-gru/gruprocfs.c
index 950dbe9ecb36..797d7962cc88 100644
--- a/drivers/misc/sgi-gru/gruprocfs.c
+++ b/drivers/misc/sgi-gru/gruprocfs.c
@@ -355,7 +355,7 @@ static void delete_proc_files(void)
 		for (p = proc_files; p->name; p++)
 			if (p->entry)
 				remove_proc_entry(p->name, proc_gru);
-		remove_proc_entry("gru", proc_gru->parent);
+		proc_remove(proc_gru);
 	}
 }
 
diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index 5bab73b91c20..dd27b0783d52 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -304,14 +304,13 @@ static int mmc_blk_open(struct block_device *bdev, fmode_t mode)
 	return ret;
 }
 
-static int mmc_blk_release(struct gendisk *disk, fmode_t mode)
+static void mmc_blk_release(struct gendisk *disk, fmode_t mode)
 {
 	struct mmc_blk_data *md = disk->private_data;
 
 	mutex_lock(&block_mutex);
 	mmc_blk_put(md);
 	mutex_unlock(&block_mutex);
-	return 0;
 }
 
 static int
@@ -1932,8 +1931,14 @@ static int mmc_blk_issue_rq(struct mmc_queue *mq, struct request *req)
 	}
 
 out:
-	if (!req && !(mq->flags & MMC_QUEUE_NEW_REQUEST))
-		/* release host only when there are no more requests */
+	if ((!req && !(mq->flags & MMC_QUEUE_NEW_REQUEST)) ||
+	     (req && (req->cmd_flags & MMC_REQ_SPECIAL_MASK)))
+		/*
+		 * Release host when there are no more requests
+		 * and after special request(discard, flush) is done.
+		 * In case sepecial request, there is no reentry to
+		 * the 'mmc_blk_issue_rq' with 'mqrq_prev->req'.
+		 */
 		mmc_release_host(card->host);
 	return ret;
 }
diff --git a/drivers/mmc/card/queue.c b/drivers/mmc/card/queue.c
index fa4e44ee7961..9447a0e970d1 100644
--- a/drivers/mmc/card/queue.c
+++ b/drivers/mmc/card/queue.c
@@ -22,9 +22,6 @@
 
 #define MMC_QUEUE_BOUNCESZ	65536
 
-
-#define MMC_REQ_SPECIAL_MASK	(REQ_DISCARD | REQ_FLUSH)
-
 /*
  * Prepare a MMC request. This just filters out odd stuff.
  */
diff --git a/drivers/mmc/card/queue.h b/drivers/mmc/card/queue.h
index 031bf6376c99..5752d50049a3 100644
--- a/drivers/mmc/card/queue.h
+++ b/drivers/mmc/card/queue.h
@@ -1,6 +1,8 @@
 #ifndef MMC_QUEUE_H
 #define MMC_QUEUE_H
 
+#define MMC_REQ_SPECIAL_MASK	(REQ_DISCARD | REQ_FLUSH)
+
 struct request;
 struct task_struct;
 
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index 9290bb51a06a..c40396f23202 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -2289,6 +2289,19 @@ int _mmc_detect_card_removed(struct mmc_host *host)
 		return 1;
 
 	ret = host->bus_ops->alive(host);
+
+	/*
+	 * Card detect status and alive check may be out of sync if card is
+	 * removed slowly, when card detect switch changes while card/slot
+	 * pads are still contacted in hardware (refer to "SD Card Mechanical
+	 * Addendum, Appendix C: Card Detection Switch"). So reschedule a
+	 * detect work 200ms later for this case.
+	 */
+	if (!ret && host->ops->get_cd && !host->ops->get_cd(host)) {
+		mmc_detect_change(host, msecs_to_jiffies(200));
+		pr_debug("%s: card removed too slowly\n", mmc_hostname(host));
+	}
+
 	if (ret) {
 		mmc_card_set_removed(host->card);
 		pr_debug("%s: card remove detected\n", mmc_hostname(host));
@@ -2403,7 +2416,10 @@ void mmc_start_host(struct mmc_host *host)
 {
 	host->f_init = max(freqs[0], host->f_min);
 	host->rescan_disable = 0;
-	mmc_power_up(host);
+	if (host->caps2 & MMC_CAP2_NO_PRESCAN_POWERUP)
+		mmc_power_off(host);
+	else
+		mmc_power_up(host);
 	mmc_detect_change(host, 0);
 }
 
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index c8f3d6e0684e..0cbd1effe960 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -96,6 +96,7 @@ static int mmc_decode_cid(struct mmc_card *card)
 		card->cid.prod_name[3]	= UNSTUFF_BITS(resp, 72, 8);
 		card->cid.prod_name[4]	= UNSTUFF_BITS(resp, 64, 8);
 		card->cid.prod_name[5]	= UNSTUFF_BITS(resp, 56, 8);
+		card->cid.prv		= UNSTUFF_BITS(resp, 48, 8);
 		card->cid.serial	= UNSTUFF_BITS(resp, 16, 32);
 		card->cid.month		= UNSTUFF_BITS(resp, 12, 4);
 		card->cid.year		= UNSTUFF_BITS(resp, 8, 4) + 1997;
@@ -368,13 +369,13 @@ static int mmc_read_ext_csd(struct mmc_card *card, u8 *ext_csd)
 		ext_csd[EXT_CSD_SEC_FEATURE_SUPPORT];
 	card->ext_csd.raw_trim_mult =
 		ext_csd[EXT_CSD_TRIM_MULT];
+	card->ext_csd.raw_partition_support = ext_csd[EXT_CSD_PARTITION_SUPPORT];
 	if (card->ext_csd.rev >= 4) {
 		/*
 		 * Enhanced area feature support -- check whether the eMMC
 		 * card has the Enhanced area enabled.  If so, export enhanced
 		 * area offset and size to user by adding sysfs interface.
 		 */
-		card->ext_csd.raw_partition_support = ext_csd[EXT_CSD_PARTITION_SUPPORT];
 		if ((ext_csd[EXT_CSD_PARTITION_SUPPORT] & 0x2) &&
 		    (ext_csd[EXT_CSD_PARTITION_ATTRIBUTE] & 0x1)) {
 			hc_erase_grp_sz =
@@ -627,6 +628,7 @@ MMC_DEV_ATTR(hwrev, "0x%x\n", card->cid.hwrev);
 MMC_DEV_ATTR(manfid, "0x%06x\n", card->cid.manfid);
 MMC_DEV_ATTR(name, "%s\n", card->cid.prod_name);
 MMC_DEV_ATTR(oemid, "0x%04x\n", card->cid.oemid);
+MMC_DEV_ATTR(prv, "0x%x\n", card->cid.prv);
 MMC_DEV_ATTR(serial, "0x%08x\n", card->cid.serial);
 MMC_DEV_ATTR(enhanced_area_offset, "%llu\n",
 		card->ext_csd.enhanced_area_offset);
@@ -645,6 +647,7 @@ static struct attribute *mmc_std_attrs[] = {
 	&dev_attr_manfid.attr,
 	&dev_attr_name.attr,
 	&dev_attr_oemid.attr,
+	&dev_attr_prv.attr,
 	&dev_attr_serial.attr,
 	&dev_attr_enhanced_area_offset.attr,
 	&dev_attr_enhanced_area_size.attr,
diff --git a/drivers/mmc/core/sdio.c b/drivers/mmc/core/sdio.c
index aa0719a4dfd1..6889a821c1da 100644
--- a/drivers/mmc/core/sdio.c
+++ b/drivers/mmc/core/sdio.c
@@ -861,8 +861,10 @@ static void mmc_sdio_detect(struct mmc_host *host)
 	/* Make sure card is powered before detecting it */
 	if (host->caps & MMC_CAP_POWER_OFF_CARD) {
 		err = pm_runtime_get_sync(&host->card->dev);
-		if (err < 0)
+		if (err < 0) {
+			pm_runtime_put_noidle(&host->card->dev);
 			goto out;
+		}
 	}
 
 	mmc_claim_host(host);
diff --git a/drivers/mmc/core/sdio_bus.c b/drivers/mmc/core/sdio_bus.c
index 5e57048e2c1d..546c67c2bbbf 100644
--- a/drivers/mmc/core/sdio_bus.c
+++ b/drivers/mmc/core/sdio_bus.c
@@ -16,6 +16,7 @@
 #include <linux/export.h>
 #include <linux/slab.h>
 #include <linux/pm_runtime.h>
+#include <linux/acpi.h>
 
 #include <linux/mmc/card.h>
 #include <linux/mmc/host.h>
@@ -137,7 +138,7 @@ static int sdio_bus_probe(struct device *dev)
 	if (func->card->host->caps & MMC_CAP_POWER_OFF_CARD) {
 		ret = pm_runtime_get_sync(dev);
 		if (ret < 0)
-			goto out;
+			goto disable_runtimepm;
 	}
 
 	/* Set the default block size so the driver is sure it's something
@@ -157,7 +158,6 @@ static int sdio_bus_probe(struct device *dev)
 disable_runtimepm:
 	if (func->card->host->caps & MMC_CAP_POWER_OFF_CARD)
 		pm_runtime_put_noidle(dev);
-out:
 	return ret;
 }
 
@@ -299,6 +299,19 @@ struct sdio_func *sdio_alloc_func(struct mmc_card *card)
 	return func;
 }
 
+#ifdef CONFIG_ACPI
+static void sdio_acpi_set_handle(struct sdio_func *func)
+{
+	struct mmc_host *host = func->card->host;
+	u64 addr = (host->slotno << 16) | func->num;
+
+	ACPI_HANDLE_SET(&func->dev,
+			acpi_get_child(ACPI_HANDLE(host->parent), addr));
+}
+#else
+static inline void sdio_acpi_set_handle(struct sdio_func *func) {}
+#endif
+
 /*
  * Register a new SDIO function with the driver model.
  */
@@ -308,9 +321,12 @@ int sdio_add_func(struct sdio_func *func)
 
 	dev_set_name(&func->dev, "%s:%d", mmc_card_id(func->card), func->num);
 
+	sdio_acpi_set_handle(func);
 	ret = device_add(&func->dev);
-	if (ret == 0)
+	if (ret == 0) {
 		sdio_func_set_present(func);
+		acpi_dev_pm_attach(&func->dev, false);
+	}
 
 	return ret;
 }
@@ -326,6 +342,7 @@ void sdio_remove_func(struct sdio_func *func)
 	if (!sdio_func_present(func))
 		return;
 
+	acpi_dev_pm_detach(&func->dev, false);
 	device_del(&func->dev);
 	put_device(&func->dev);
 }
diff --git a/drivers/mmc/host/Kconfig b/drivers/mmc/host/Kconfig
index d88219e1d86e..9ab8f8dee942 100644
--- a/drivers/mmc/host/Kconfig
+++ b/drivers/mmc/host/Kconfig
@@ -190,6 +190,17 @@ config MMC_SDHCI_S3C
 
 	  If unsure, say N.
 
+config MMC_SDHCI_SIRF
+	tristate "SDHCI support on CSR SiRFprimaII and SiRFmarco SoCs"
+	depends on ARCH_SIRF
+	depends on MMC_SDHCI_PLTFM
+	help
+	  This selects the SDHCI support for SiRF System-on-Chip devices.
+
+	  If you have a controller with this interface, say Y or M here.
+
+	  If unsure, say N.
+
 config MMC_SDHCI_PXAV3
 	tristate "Marvell MMP2 SD Host Controller support (PXAV3)"
 	depends on CLKDEV_LOOKUP
@@ -300,16 +311,6 @@ config MMC_ATMELMCI
 
 	  If unsure, say N.
 
-config MMC_ATMELMCI_DMA
-	bool "Atmel MCI DMA support"
-	depends on MMC_ATMELMCI && (AVR32 || ARCH_AT91SAM9G45) && DMA_ENGINE
-	help
-	  Say Y here to have the Atmel MCI driver use a DMA engine to
-	  do data transfers and thus increase the throughput and
-	  reduce the CPU utilization.
-
-	  If unsure, say N.
-
 config MMC_MSM
 	tristate "Qualcomm SDCC Controller Support"
 	depends on MMC && ARCH_MSM
@@ -319,12 +320,12 @@ config MMC_MSM
 	  support for SDIO devices.
 
 config MMC_MXC
-	tristate "Freescale i.MX21/27/31 Multimedia Card Interface support"
-	depends on ARCH_MXC
+	tristate "Freescale i.MX21/27/31 or MPC512x Multimedia Card support"
+	depends on ARCH_MXC || PPC_MPC512x
 	help
-	  This selects the Freescale i.MX21, i.MX27 and i.MX31 Multimedia card
-	  Interface. If you have a i.MX platform with a Multimedia Card slot,
-	  say Y or M here.
+	  This selects the Freescale i.MX21, i.MX27, i.MX31 or MPC512x
+	  Multimedia Card Interface. If you have an i.MX or MPC512x platform
+	  with a Multimedia Card slot, say Y or M here.
 
 	  If unsure, say N.
 
diff --git a/drivers/mmc/host/Makefile b/drivers/mmc/host/Makefile
index c380e3cf0a3b..cd3228075553 100644
--- a/drivers/mmc/host/Makefile
+++ b/drivers/mmc/host/Makefile
@@ -13,6 +13,7 @@ obj-$(CONFIG_MMC_SDHCI_ACPI)	+= sdhci-acpi.o
 obj-$(CONFIG_MMC_SDHCI_PXAV3)	+= sdhci-pxav3.o
 obj-$(CONFIG_MMC_SDHCI_PXAV2)	+= sdhci-pxav2.o
 obj-$(CONFIG_MMC_SDHCI_S3C)	+= sdhci-s3c.o
+obj-$(CONFIG_MMC_SDHCI_SIRF)   	+= sdhci-sirf.o
 obj-$(CONFIG_MMC_SDHCI_SPEAR)	+= sdhci-spear.o
 obj-$(CONFIG_MMC_WBSD)		+= wbsd.o
 obj-$(CONFIG_MMC_AU1X)		+= au1xmmc.o
diff --git a/drivers/mmc/host/android-goldfish.c b/drivers/mmc/host/android-goldfish.c
index ef3aef0f376d..7780c14704c4 100644
--- a/drivers/mmc/host/android-goldfish.c
+++ b/drivers/mmc/host/android-goldfish.c
@@ -476,7 +476,7 @@ static int goldfish_mmc_probe(struct platform_device *pdev)
 	host->mmc = mmc;
 
 	pr_err("mmc: Mapping %lX to %lX\n", (long)res->start, (long)res->end);
-	host->reg_base = ioremap(res->start, res->end - res->start + 1);
+	host->reg_base = ioremap(res->start, resource_size(res));
 	if (host->reg_base == NULL) {
 		ret = -ENOMEM;
 		goto ioremap_failed;
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index 722af1de7967..e75774f72606 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -178,6 +178,7 @@ struct atmel_mci {
 	void __iomem		*regs;
 
 	struct scatterlist	*sg;
+	unsigned int		sg_len;
 	unsigned int		pio_offset;
 	unsigned int		*buffer;
 	unsigned int		buf_size;
@@ -892,6 +893,7 @@ static u32 atmci_prepare_data(struct atmel_mci *host, struct mmc_data *data)
 	data->error = -EINPROGRESS;
 
 	host->sg = data->sg;
+	host->sg_len = data->sg_len;
 	host->data = data;
 	host->data_chan = NULL;
 
@@ -1826,7 +1828,8 @@ static void atmci_read_data_pio(struct atmel_mci *host)
 			if (offset == sg->length) {
 				flush_dcache_page(sg_page(sg));
 				host->sg = sg = sg_next(sg);
-				if (!sg)
+				host->sg_len--;
+				if (!sg || !host->sg_len)
 					goto done;
 
 				offset = 0;
@@ -1839,7 +1842,8 @@ static void atmci_read_data_pio(struct atmel_mci *host)
 
 			flush_dcache_page(sg_page(sg));
 			host->sg = sg = sg_next(sg);
-			if (!sg)
+			host->sg_len--;
+			if (!sg || !host->sg_len)
 				goto done;
 
 			offset = 4 - remaining;
@@ -1890,7 +1894,8 @@ static void atmci_write_data_pio(struct atmel_mci *host)
 			nbytes += 4;
 			if (offset == sg->length) {
 				host->sg = sg = sg_next(sg);
-				if (!sg)
+				host->sg_len--;
+				if (!sg || !host->sg_len)
 					goto done;
 
 				offset = 0;
@@ -1904,7 +1909,8 @@ static void atmci_write_data_pio(struct atmel_mci *host)
 			nbytes += remaining;
 
 			host->sg = sg = sg_next(sg);
-			if (!sg) {
+			host->sg_len--;
+			if (!sg || !host->sg_len) {
 				atmci_writel(host, ATMCI_TDR, value);
 				goto done;
 			}
@@ -2487,10 +2493,8 @@ static int __exit atmci_remove(struct platform_device *pdev)
 	atmci_readl(host, ATMCI_SR);
 	clk_disable(host->mck);
 
-#ifdef CONFIG_MMC_ATMELMCI_DMA
 	if (host->dma.chan)
 		dma_release_channel(host->dma.chan);
-#endif
 
 	free_irq(platform_get_irq(pdev, 0), host);
 	iounmap(host->regs);
diff --git a/drivers/mmc/host/davinci_mmc.c b/drivers/mmc/host/davinci_mmc.c
index 20636772c09b..3946a0eb3a03 100644
--- a/drivers/mmc/host/davinci_mmc.c
+++ b/drivers/mmc/host/davinci_mmc.c
@@ -34,6 +34,8 @@
 #include <linux/dma-mapping.h>
 #include <linux/edma.h>
 #include <linux/mmc/mmc.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 
 #include <linux/platform_data/mmc-davinci.h>
 
@@ -522,14 +524,16 @@ static int __init davinci_acquire_dma_channels(struct mmc_davinci_host *host)
 	dma_cap_set(DMA_SLAVE, mask);
 
 	host->dma_tx =
-		dma_request_channel(mask, edma_filter_fn, &host->txdma);
+		dma_request_slave_channel_compat(mask, edma_filter_fn,
+				&host->txdma, mmc_dev(host->mmc), "tx");
 	if (!host->dma_tx) {
 		dev_err(mmc_dev(host->mmc), "Can't get dma_tx channel\n");
 		return -ENODEV;
 	}
 
 	host->dma_rx =
-		dma_request_channel(mask, edma_filter_fn, &host->rxdma);
+		dma_request_slave_channel_compat(mask, edma_filter_fn,
+				&host->rxdma, mmc_dev(host->mmc), "rx");
 	if (!host->dma_rx) {
 		dev_err(mmc_dev(host->mmc), "Can't get dma_rx channel\n");
 		r = -ENODEV;
@@ -1157,16 +1161,86 @@ static void __init init_mmcsd_host(struct mmc_davinci_host *host)
 	mmc_davinci_reset_ctrl(host, 0);
 }
 
-static int __init davinci_mmcsd_probe(struct platform_device *pdev)
+static struct platform_device_id davinci_mmc_devtype[] = {
+	{
+		.name	= "dm6441-mmc",
+		.driver_data = MMC_CTLR_VERSION_1,
+	}, {
+		.name	= "da830-mmc",
+		.driver_data = MMC_CTLR_VERSION_2,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(platform, davinci_mmc_devtype);
+
+static const struct of_device_id davinci_mmc_dt_ids[] = {
+	{
+		.compatible = "ti,dm6441-mmc",
+		.data = &davinci_mmc_devtype[MMC_CTLR_VERSION_1],
+	},
+	{
+		.compatible = "ti,da830-mmc",
+		.data = &davinci_mmc_devtype[MMC_CTLR_VERSION_2],
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, davinci_mmc_dt_ids);
+
+static struct davinci_mmc_config
+	*mmc_parse_pdata(struct platform_device *pdev)
 {
+	struct device_node *np;
 	struct davinci_mmc_config *pdata = pdev->dev.platform_data;
+	const struct of_device_id *match =
+		of_match_device(of_match_ptr(davinci_mmc_dt_ids), &pdev->dev);
+	u32 data;
+
+	np = pdev->dev.of_node;
+	if (!np)
+		return pdata;
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata) {
+		dev_err(&pdev->dev, "Failed to allocate memory for struct davinci_mmc_config\n");
+		goto nodata;
+	}
+
+	if (match)
+		pdev->id_entry = match->data;
+
+	if (of_property_read_u32(np, "max-frequency", &pdata->max_freq))
+		dev_info(&pdev->dev, "'max-frequency' property not specified, defaulting to 25MHz\n");
+
+	of_property_read_u32(np, "bus-width", &data);
+	switch (data) {
+	case 1:
+	case 4:
+	case 8:
+		pdata->wires = data;
+		break;
+	default:
+		pdata->wires = 1;
+		dev_info(&pdev->dev, "Unsupported buswidth, defaulting to 1 bit\n");
+	}
+nodata:
+	return pdata;
+}
+
+static int __init davinci_mmcsd_probe(struct platform_device *pdev)
+{
+	struct davinci_mmc_config *pdata = NULL;
 	struct mmc_davinci_host *host = NULL;
 	struct mmc_host *mmc = NULL;
 	struct resource *r, *mem = NULL;
 	int ret = 0, irq = 0;
 	size_t mem_size;
+	const struct platform_device_id *id_entry;
 
-	/* REVISIT:  when we're fully converted, fail if pdata is NULL */
+	pdata = mmc_parse_pdata(pdev);
+	if (pdata == NULL) {
+		dev_err(&pdev->dev, "Couldn't get platform data\n");
+		return -ENOENT;
+	}
 
 	ret = -ENODEV;
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -1190,13 +1264,15 @@ static int __init davinci_mmcsd_probe(struct platform_device *pdev)
 
 	r = platform_get_resource(pdev, IORESOURCE_DMA, 0);
 	if (!r)
-		goto out;
-	host->rxdma = r->start;
+		dev_warn(&pdev->dev, "RX DMA resource not specified\n");
+	else
+		host->rxdma = r->start;
 
 	r = platform_get_resource(pdev, IORESOURCE_DMA, 1);
 	if (!r)
-		goto out;
-	host->txdma = r->start;
+		dev_warn(&pdev->dev, "TX DMA resource not specified\n");
+	else
+		host->txdma = r->start;
 
 	host->mem_res = mem;
 	host->base = ioremap(mem->start, mem_size);
@@ -1237,7 +1313,9 @@ static int __init davinci_mmcsd_probe(struct platform_device *pdev)
 	if (pdata && (pdata->wires == 8))
 		mmc->caps |= (MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA);
 
-	host->version = pdata->version;
+	id_entry = platform_get_device_id(pdev);
+	if (id_entry)
+		host->version = id_entry->driver_data;
 
 	mmc->ops = &mmc_davinci_ops;
 	mmc->f_min = 312500;
@@ -1406,22 +1484,13 @@ static struct platform_driver davinci_mmcsd_driver = {
 		.name	= "davinci_mmc",
 		.owner	= THIS_MODULE,
 		.pm	= davinci_mmcsd_pm_ops,
+		.of_match_table = of_match_ptr(davinci_mmc_dt_ids),
 	},
 	.remove		= __exit_p(davinci_mmcsd_remove),
+	.id_table	= davinci_mmc_devtype,
 };
 
-static int __init davinci_mmcsd_init(void)
-{
-	return platform_driver_probe(&davinci_mmcsd_driver,
-				     davinci_mmcsd_probe);
-}
-module_init(davinci_mmcsd_init);
-
-static void __exit davinci_mmcsd_exit(void)
-{
-	platform_driver_unregister(&davinci_mmcsd_driver);
-}
-module_exit(davinci_mmcsd_exit);
+module_platform_driver_probe(davinci_mmcsd_driver, davinci_mmcsd_probe);
 
 MODULE_AUTHOR("Texas Instruments India");
 MODULE_LICENSE("GPL");
diff --git a/drivers/mmc/host/dw_mmc-exynos.c b/drivers/mmc/host/dw_mmc-exynos.c
index 72fd0f2c9013..f013e7e3746b 100644
--- a/drivers/mmc/host/dw_mmc-exynos.c
+++ b/drivers/mmc/host/dw_mmc-exynos.c
@@ -152,45 +152,8 @@ static int dw_mci_exynos_parse_dt(struct dw_mci *host)
 	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;
-		}
-	}
-
-	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] = {
+/* Common capabilities of Exynos4/Exynos5 SoC */
+static unsigned long exynos_dwmmc_caps[4] = {
 	MMC_CAP_UHS_DDR50 | MMC_CAP_1_8V_DDR |
 		MMC_CAP_8_BIT_DATA | MMC_CAP_CMD23,
 	MMC_CAP_CMD23,
@@ -198,24 +161,25 @@ static unsigned long exynos5250_dwmmc_caps[4] = {
 	MMC_CAP_CMD23,
 };
 
-static const struct dw_mci_drv_data exynos5250_drv_data = {
-	.caps			= exynos5250_dwmmc_caps,
+static const struct dw_mci_drv_data exynos_drv_data = {
+	.caps			= exynos_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,exynos4412-dw-mshc",
+			.data = &exynos_drv_data, },
 	{ .compatible = "samsung,exynos5250-dw-mshc",
-			.data = &exynos5250_drv_data, },
+			.data = &exynos_drv_data, },
 	{},
 };
 MODULE_DEVICE_TABLE(of, dw_mci_exynos_match);
 
-int dw_mci_exynos_probe(struct platform_device *pdev)
+static int dw_mci_exynos_probe(struct platform_device *pdev)
 {
 	const struct dw_mci_drv_data *drv_data;
 	const struct of_device_id *match;
@@ -230,7 +194,7 @@ static struct platform_driver dw_mci_exynos_pltfm_driver = {
 	.remove		= __exit_p(dw_mci_pltfm_remove),
 	.driver		= {
 		.name		= "dwmmc_exynos",
-		.of_match_table	= of_match_ptr(dw_mci_exynos_match),
+		.of_match_table	= dw_mci_exynos_match,
 		.pm		= &dw_mci_pltfm_pmops,
 	},
 };
diff --git a/drivers/mmc/host/dw_mmc.c b/drivers/mmc/host/dw_mmc.c
index 98342213ed21..bc3a1bc4940f 100644
--- a/drivers/mmc/host/dw_mmc.c
+++ b/drivers/mmc/host/dw_mmc.c
@@ -795,9 +795,9 @@ static void dw_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 
 	/* DDR mode set */
 	if (ios->timing == MMC_TIMING_UHS_DDR50)
-		regs |= (0x1 << slot->id) << 16;
+		regs |= ((0x1 << slot->id) << 16);
 	else
-		regs &= ~(0x1 << slot->id) << 16;
+		regs &= ~((0x1 << slot->id) << 16);
 
 	mci_writel(slot->host, UHS_REG, regs);
 
@@ -818,6 +818,20 @@ static void dw_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	switch (ios->power_mode) {
 	case MMC_POWER_UP:
 		set_bit(DW_MMC_CARD_NEED_INIT, &slot->flags);
+		/* Power up slot */
+		if (slot->host->pdata->setpower)
+			slot->host->pdata->setpower(slot->id, mmc->ocr_avail);
+		regs = mci_readl(slot->host, PWREN);
+		regs |= (1 << slot->id);
+		mci_writel(slot->host, PWREN, regs);
+		break;
+	case MMC_POWER_OFF:
+		/* Power down slot */
+		if (slot->host->pdata->setpower)
+			slot->host->pdata->setpower(slot->id, 0);
+		regs = mci_readl(slot->host, PWREN);
+		regs &= ~(1 << slot->id);
+		mci_writel(slot->host, PWREN, regs);
 		break;
 	default:
 		break;
@@ -1191,12 +1205,15 @@ static void dw_mci_pull_final_bytes(struct dw_mci *host, void *buf, int cnt)
 
 static void dw_mci_push_data16(struct dw_mci *host, void *buf, int cnt)
 {
+	struct mmc_data *data = host->data;
+	int init_cnt = cnt;
+
 	/* try and push anything in the part_buf */
 	if (unlikely(host->part_buf_count)) {
 		int len = dw_mci_push_part_bytes(host, buf, cnt);
 		buf += len;
 		cnt -= len;
-		if (!sg_next(host->sg) || host->part_buf_count == 2) {
+		if (host->part_buf_count == 2) {
 			mci_writew(host, DATA(host->data_offset),
 					host->part_buf16);
 			host->part_buf_count = 0;
@@ -1229,9 +1246,11 @@ static void dw_mci_push_data16(struct dw_mci *host, void *buf, int cnt)
 	/* put anything remaining in the part_buf */
 	if (cnt) {
 		dw_mci_set_part_bytes(host, buf, cnt);
-		if (!sg_next(host->sg))
+		 /* Push data if we have reached the expected data length */
+		if ((data->bytes_xfered + init_cnt) ==
+		    (data->blksz * data->blocks))
 			mci_writew(host, DATA(host->data_offset),
-					host->part_buf16);
+				   host->part_buf16);
 	}
 }
 
@@ -1269,12 +1288,15 @@ static void dw_mci_pull_data16(struct dw_mci *host, void *buf, int cnt)
 
 static void dw_mci_push_data32(struct dw_mci *host, void *buf, int cnt)
 {
+	struct mmc_data *data = host->data;
+	int init_cnt = cnt;
+
 	/* try and push anything in the part_buf */
 	if (unlikely(host->part_buf_count)) {
 		int len = dw_mci_push_part_bytes(host, buf, cnt);
 		buf += len;
 		cnt -= len;
-		if (!sg_next(host->sg) || host->part_buf_count == 4) {
+		if (host->part_buf_count == 4) {
 			mci_writel(host, DATA(host->data_offset),
 					host->part_buf32);
 			host->part_buf_count = 0;
@@ -1307,9 +1329,11 @@ static void dw_mci_push_data32(struct dw_mci *host, void *buf, int cnt)
 	/* put anything remaining in the part_buf */
 	if (cnt) {
 		dw_mci_set_part_bytes(host, buf, cnt);
-		if (!sg_next(host->sg))
+		 /* Push data if we have reached the expected data length */
+		if ((data->bytes_xfered + init_cnt) ==
+		    (data->blksz * data->blocks))
 			mci_writel(host, DATA(host->data_offset),
-						host->part_buf32);
+				   host->part_buf32);
 	}
 }
 
@@ -1347,13 +1371,17 @@ static void dw_mci_pull_data32(struct dw_mci *host, void *buf, int cnt)
 
 static void dw_mci_push_data64(struct dw_mci *host, void *buf, int cnt)
 {
+	struct mmc_data *data = host->data;
+	int init_cnt = cnt;
+
 	/* try and push anything in the part_buf */
 	if (unlikely(host->part_buf_count)) {
 		int len = dw_mci_push_part_bytes(host, buf, cnt);
 		buf += len;
 		cnt -= len;
-		if (!sg_next(host->sg) || host->part_buf_count == 8) {
-			mci_writew(host, DATA(host->data_offset),
+
+		if (host->part_buf_count == 8) {
+			mci_writeq(host, DATA(host->data_offset),
 					host->part_buf);
 			host->part_buf_count = 0;
 		}
@@ -1385,9 +1413,11 @@ static void dw_mci_push_data64(struct dw_mci *host, void *buf, int cnt)
 	/* put anything remaining in the part_buf */
 	if (cnt) {
 		dw_mci_set_part_bytes(host, buf, cnt);
-		if (!sg_next(host->sg))
+		/* Push data if we have reached the expected data length */
+		if ((data->bytes_xfered + init_cnt) ==
+		    (data->blksz * data->blocks))
 			mci_writeq(host, DATA(host->data_offset),
-					host->part_buf);
+				   host->part_buf);
 	}
 }
 
@@ -1438,7 +1468,7 @@ static void dw_mci_pull_data(struct dw_mci *host, void *buf, int cnt)
 	host->pull_data(host, buf, cnt);
 }
 
-static void dw_mci_read_data_pio(struct dw_mci *host)
+static void dw_mci_read_data_pio(struct dw_mci *host, bool dto)
 {
 	struct sg_mapping_iter *sg_miter = &host->sg_miter;
 	void *buf;
@@ -1446,7 +1476,7 @@ static void dw_mci_read_data_pio(struct dw_mci *host)
 	struct mmc_data	*data = host->data;
 	int shift = host->data_shift;
 	u32 status;
-	unsigned int nbytes = 0, len;
+	unsigned int len;
 	unsigned int remain, fcnt;
 
 	do {
@@ -1465,16 +1495,17 @@ static void dw_mci_read_data_pio(struct dw_mci *host)
 			if (!len)
 				break;
 			dw_mci_pull_data(host, (void *)(buf + offset), len);
+			data->bytes_xfered += len;
 			offset += len;
-			nbytes += len;
 			remain -= len;
 		} while (remain);
 
 		sg_miter->consumed = offset;
 		status = mci_readl(host, MINTSTS);
 		mci_writel(host, RINTSTS, SDMMC_INT_RXDR);
-	} while (status & SDMMC_INT_RXDR); /*if the RXDR is ready read again*/
-	data->bytes_xfered += nbytes;
+	/* if the RXDR is ready read again */
+	} while ((status & SDMMC_INT_RXDR) ||
+		 (dto && SDMMC_GET_FCNT(mci_readl(host, STATUS))));
 
 	if (!remain) {
 		if (!sg_miter_next(sg_miter))
@@ -1485,7 +1516,6 @@ static void dw_mci_read_data_pio(struct dw_mci *host)
 	return;
 
 done:
-	data->bytes_xfered += nbytes;
 	sg_miter_stop(sg_miter);
 	host->sg = NULL;
 	smp_wmb();
@@ -1500,7 +1530,7 @@ static void dw_mci_write_data_pio(struct dw_mci *host)
 	struct mmc_data	*data = host->data;
 	int shift = host->data_shift;
 	u32 status;
-	unsigned int nbytes = 0, len;
+	unsigned int len;
 	unsigned int fifo_depth = host->fifo_depth;
 	unsigned int remain, fcnt;
 
@@ -1521,8 +1551,8 @@ static void dw_mci_write_data_pio(struct dw_mci *host)
 			if (!len)
 				break;
 			host->push_data(host, (void *)(buf + offset), len);
+			data->bytes_xfered += len;
 			offset += len;
-			nbytes += len;
 			remain -= len;
 		} while (remain);
 
@@ -1530,7 +1560,6 @@ static void dw_mci_write_data_pio(struct dw_mci *host)
 		status = mci_readl(host, MINTSTS);
 		mci_writel(host, RINTSTS, SDMMC_INT_TXDR);
 	} while (status & SDMMC_INT_TXDR); /* if TXDR write again */
-	data->bytes_xfered += nbytes;
 
 	if (!remain) {
 		if (!sg_miter_next(sg_miter))
@@ -1541,7 +1570,6 @@ static void dw_mci_write_data_pio(struct dw_mci *host)
 	return;
 
 done:
-	data->bytes_xfered += nbytes;
 	sg_miter_stop(sg_miter);
 	host->sg = NULL;
 	smp_wmb();
@@ -1563,11 +1591,11 @@ static irqreturn_t dw_mci_interrupt(int irq, void *dev_id)
 {
 	struct dw_mci *host = dev_id;
 	u32 pending;
-	unsigned int pass_count = 0;
 	int i;
 
-	do {
-		pending = mci_readl(host, MINTSTS); /* read-only mask reg */
+	pending = mci_readl(host, MINTSTS); /* read-only mask reg */
+
+	if (pending) {
 
 		/*
 		 * DTO fix - version 2.10a and below, and only if internal DMA
@@ -1579,9 +1607,6 @@ static irqreturn_t dw_mci_interrupt(int irq, void *dev_id)
 				pending |= SDMMC_INT_DATA_OVER;
 		}
 
-		if (!pending)
-			break;
-
 		if (pending & DW_MCI_CMD_ERROR_FLAGS) {
 			mci_writel(host, RINTSTS, DW_MCI_CMD_ERROR_FLAGS);
 			host->cmd_status = pending;
@@ -1605,7 +1630,7 @@ static irqreturn_t dw_mci_interrupt(int irq, void *dev_id)
 			smp_wmb();
 			if (host->dir_status == DW_MCI_RECV_STATUS) {
 				if (host->sg != NULL)
-					dw_mci_read_data_pio(host);
+					dw_mci_read_data_pio(host, true);
 			}
 			set_bit(EVENT_DATA_COMPLETE, &host->pending_events);
 			tasklet_schedule(&host->tasklet);
@@ -1614,7 +1639,7 @@ static irqreturn_t dw_mci_interrupt(int irq, void *dev_id)
 		if (pending & SDMMC_INT_RXDR) {
 			mci_writel(host, RINTSTS, SDMMC_INT_RXDR);
 			if (host->dir_status == DW_MCI_RECV_STATUS && host->sg)
-				dw_mci_read_data_pio(host);
+				dw_mci_read_data_pio(host, false);
 		}
 
 		if (pending & SDMMC_INT_TXDR) {
@@ -1642,7 +1667,7 @@ static irqreturn_t dw_mci_interrupt(int irq, void *dev_id)
 			}
 		}
 
-	} while (pass_count++ < 5);
+	}
 
 #ifdef CONFIG_MMC_DW_IDMAC
 	/* Handle DMA interrupts */
@@ -1674,10 +1699,6 @@ static void dw_mci_work_routine_card(struct work_struct *work)
 			dev_dbg(&slot->mmc->class_dev, "card %s\n",
 				present ? "inserted" : "removed");
 
-			/* Power up slot (before spin_lock, may sleep) */
-			if (present != 0 && host->pdata->setpower)
-				host->pdata->setpower(slot->id, mmc->ocr_avail);
-
 			spin_lock_bh(&host->lock);
 
 			/* Card change detected */
@@ -1760,10 +1781,6 @@ static void dw_mci_work_routine_card(struct work_struct *work)
 
 			spin_unlock_bh(&host->lock);
 
-			/* Power down slot (after spin_unlock, may sleep) */
-			if (present == 0 && host->pdata->setpower)
-				host->pdata->setpower(slot->id, 0);
-
 			present = dw_mci_get_cd(mmc);
 		}
 
@@ -1935,14 +1952,6 @@ static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 	else
 		bus_width = 1;
 
-	if (drv_data && drv_data->setup_bus) {
-		struct device_node *slot_np;
-		slot_np = dw_mci_of_find_slot_node(host->dev, slot->id);
-		ret = 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;
@@ -1980,8 +1989,14 @@ static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 	if (IS_ERR(host->vmmc)) {
 		pr_info("%s: no vmmc regulator found\n", mmc_hostname(mmc));
 		host->vmmc = NULL;
-	} else
-		regulator_enable(host->vmmc);
+	} else {
+		ret = regulator_enable(host->vmmc);
+		if (ret) {
+			dev_err(host->dev,
+				"failed to enable regulator: %d\n", ret);
+			goto err_setup_bus;
+		}
+	}
 
 	if (dw_mci_get_cd(mmc))
 		set_bit(DW_MMC_CARD_PRESENT, &slot->flags);
@@ -1990,7 +2005,9 @@ static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 
 	slot->wp_gpio = dw_mci_of_get_wp_gpio(host->dev, slot->id);
 
-	mmc_add_host(mmc);
+	ret = mmc_add_host(mmc);
+	if (ret)
+		goto err_setup_bus;
 
 #if defined(CONFIG_DEBUG_FS)
 	dw_mci_init_debugfs(slot);
@@ -2289,6 +2306,18 @@ int dw_mci_probe(struct dw_mci *host)
 	mci_writel(host, CLKENA, 0);
 	mci_writel(host, CLKSRC, 0);
 
+	/*
+	 * In 2.40a spec, Data offset is changed.
+	 * 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);
+
+	if (host->verid < DW_MMC_240A)
+		host->data_offset = DATA_OFFSET;
+	else
+		host->data_offset = DATA_240A_OFFSET;
+
 	tasklet_init(&host->tasklet, dw_mci_tasklet_func, (unsigned long)host);
 	host->card_workqueue = alloc_workqueue("dw-mci-card",
 			WQ_MEM_RECLAIM | WQ_NON_REENTRANT, 1);
@@ -2337,18 +2366,6 @@ int dw_mci_probe(struct dw_mci *host)
 		goto err_workqueue;
 	}
 
-	/*
-	 * In 2.40a spec, Data offset is changed.
-	 * 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);
-
-	if (host->verid < DW_MMC_240A)
-		host->data_offset = DATA_OFFSET;
-	else
-		host->data_offset = DATA_240A_OFFSET;
-
 	if (host->quirks & DW_MCI_QUIRK_IDMAC_DTO)
 		dev_info(host->dev, "Internal DMAC interrupt fix enabled.\n");
 
@@ -2445,8 +2462,14 @@ int dw_mci_resume(struct dw_mci *host)
 {
 	int i, ret;
 
-	if (host->vmmc)
-		regulator_enable(host->vmmc);
+	if (host->vmmc) {
+		ret = regulator_enable(host->vmmc);
+		if (ret) {
+			dev_err(host->dev,
+				"failed to enable regulator: %d\n", ret);
+			return ret;
+		}
+	}
 
 	if (!mci_wait_reset(host->dev, host)) {
 		ret = -ENODEV;
@@ -2485,7 +2508,7 @@ EXPORT_SYMBOL(dw_mci_resume);
 
 static int __init dw_mci_init(void)
 {
-	printk(KERN_INFO "Synopsys Designware Multimedia Card Interface Driver");
+	pr_info("Synopsys Designware Multimedia Card Interface Driver\n");
 	return 0;
 }
 
diff --git a/drivers/mmc/host/dw_mmc.h b/drivers/mmc/host/dw_mmc.h
index 53b8fd987e47..0b74189e7ee7 100644
--- a/drivers/mmc/host/dw_mmc.h
+++ b/drivers/mmc/host/dw_mmc.h
@@ -190,7 +190,6 @@ extern int dw_mci_resume(struct dw_mci *host);
  * @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
@@ -203,7 +202,5 @@ struct dw_mci_drv_data {
 	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/mmci.c b/drivers/mmc/host/mmci.c
index 372e921389c8..375c109607ff 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -1141,6 +1141,11 @@ static void mmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	case MMC_POWER_OFF:
 		if (!IS_ERR(mmc->supply.vmmc))
 			mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, 0);
+
+		if (!IS_ERR(mmc->supply.vqmmc) &&
+		    regulator_is_enabled(mmc->supply.vqmmc))
+			regulator_disable(mmc->supply.vqmmc);
+
 		break;
 	case MMC_POWER_UP:
 		if (!IS_ERR(mmc->supply.vmmc))
@@ -1155,6 +1160,10 @@ static void mmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 
 		break;
 	case MMC_POWER_ON:
+		if (!IS_ERR(mmc->supply.vqmmc) &&
+		    !regulator_is_enabled(mmc->supply.vqmmc))
+			regulator_enable(mmc->supply.vqmmc);
+
 		pwr |= MCI_PWR_ON;
 		break;
 	}
diff --git a/drivers/mmc/host/msm_sdcc.c b/drivers/mmc/host/msm_sdcc.c
index 7c0af0e80047..0ee4a57fe6b2 100644
--- a/drivers/mmc/host/msm_sdcc.c
+++ b/drivers/mmc/host/msm_sdcc.c
@@ -43,7 +43,6 @@
 #include <asm/sizes.h>
 
 #include <linux/platform_data/mmc-msm_sdcc.h>
-#include <mach/msm_iomap.h>
 #include <mach/dma.h>
 #include <mach/clk.h>
 
diff --git a/drivers/mmc/host/mvsdio.c b/drivers/mmc/host/mvsdio.c
index 145cdaf000d1..8960fc846c77 100644
--- a/drivers/mmc/host/mvsdio.c
+++ b/drivers/mmc/host/mvsdio.c
@@ -119,10 +119,8 @@ static int mvsd_setup_data(struct mvsd_host *host, struct mmc_data *data)
 		host->pio_size = data->blocks * data->blksz;
 		host->pio_ptr = sg_virt(data->sg);
 		if (!nodma)
-			pr_debug("%s: fallback to PIO for data "
-					  "at 0x%p size %d\n",
-					  mmc_hostname(host->mmc),
-					  host->pio_ptr, host->pio_size);
+			dev_dbg(host->dev, "fallback to PIO for data at 0x%p size %d\n",
+				host->pio_ptr, host->pio_size);
 		return 1;
 	} else {
 		dma_addr_t phys_addr;
@@ -473,8 +471,8 @@ static irqreturn_t mvsd_irq(int irq, void *dev)
 		if (mrq->data)
 			err_status = mvsd_finish_data(host, mrq->data, err_status);
 		if (err_status) {
-			pr_err("%s: unhandled error status %#04x\n",
-					mmc_hostname(host->mmc), err_status);
+			dev_err(host->dev, "unhandled error status %#04x\n",
+				err_status);
 			cmd->error = -ENOMSG;
 		}
 
@@ -491,9 +489,8 @@ static irqreturn_t mvsd_irq(int irq, void *dev)
 	if (irq_handled)
 		return IRQ_HANDLED;
 
-	pr_err("%s: unhandled interrupt status=0x%04x en=0x%04x "
-			"pio=%d\n", mmc_hostname(host->mmc), intr_status,
-			host->intr_en, host->pio_size);
+	dev_err(host->dev, "unhandled interrupt status=0x%04x en=0x%04x pio=%d\n",
+		intr_status, host->intr_en, host->pio_size);
 	return IRQ_NONE;
 }
 
@@ -507,13 +504,11 @@ static void mvsd_timeout_timer(unsigned long data)
 	spin_lock_irqsave(&host->lock, flags);
 	mrq = host->mrq;
 	if (mrq) {
-		pr_err("%s: Timeout waiting for hardware interrupt.\n",
-				mmc_hostname(host->mmc));
-		pr_err("%s: hw_state=0x%04x, intr_status=0x%04x "
-				"intr_en=0x%04x\n", mmc_hostname(host->mmc),
-				mvsd_read(MVSD_HW_STATE),
-				mvsd_read(MVSD_NOR_INTR_STATUS),
-				mvsd_read(MVSD_NOR_INTR_EN));
+		dev_err(host->dev, "Timeout waiting for hardware interrupt.\n");
+		dev_err(host->dev, "hw_state=0x%04x, intr_status=0x%04x intr_en=0x%04x\n",
+			mvsd_read(MVSD_HW_STATE),
+			mvsd_read(MVSD_NOR_INTR_STATUS),
+			mvsd_read(MVSD_NOR_INTR_EN));
 
 		host->mrq = NULL;
 
@@ -741,8 +736,8 @@ static int __init mvsd_probe(struct platform_device *pdev)
 			goto out;
 		}
 		host->base_clock = mvsd_data->clock / 2;
-		gpio_card_detect = mvsd_data->gpio_card_detect;
-		gpio_write_protect = mvsd_data->gpio_write_protect;
+		gpio_card_detect = mvsd_data->gpio_card_detect ? : -EINVAL;
+		gpio_write_protect = mvsd_data->gpio_write_protect ? : -EINVAL;
 	}
 
 	mmc->ops = &mvsd_ops;
@@ -778,7 +773,7 @@ static int __init mvsd_probe(struct platform_device *pdev)
 
 	ret = devm_request_irq(&pdev->dev, irq, mvsd_irq, 0, DRIVER_NAME, host);
 	if (ret) {
-		pr_err("%s: cannot assign irq %d\n", DRIVER_NAME, irq);
+		dev_err(&pdev->dev, "cannot assign irq %d\n", irq);
 		goto out;
 	}
 
@@ -797,13 +792,11 @@ static int __init mvsd_probe(struct platform_device *pdev)
 	if (ret)
 		goto out;
 
-	pr_notice("%s: %s driver initialized, ",
-			   mmc_hostname(mmc), DRIVER_NAME);
 	if (!(mmc->caps & MMC_CAP_NEEDS_POLL))
-		printk("using GPIO %d for card detection\n",
-		       gpio_card_detect);
+		dev_notice(&pdev->dev, "using GPIO %d for card detection\n",
+			   gpio_card_detect);
 	else
-		printk("lacking card detect (fall back to polling)\n");
+		dev_notice(&pdev->dev, "lacking card detect (fall back to polling)\n");
 	return 0;
 
 out:
@@ -881,18 +874,7 @@ static struct platform_driver mvsd_driver = {
 	},
 };
 
-static int __init mvsd_init(void)
-{
-	return platform_driver_probe(&mvsd_driver, mvsd_probe);
-}
-
-static void __exit mvsd_exit(void)
-{
-	platform_driver_unregister(&mvsd_driver);
-}
-
-module_init(mvsd_init);
-module_exit(mvsd_exit);
+module_platform_driver_probe(mvsd_driver, mvsd_probe);
 
 /* maximum card clock frequency (default 50MHz) */
 module_param(maxfreq, int, 0);
diff --git a/drivers/mmc/host/mxcmmc.c b/drivers/mmc/host/mxcmmc.c
index a72936eea6fa..d5036353bddc 100644
--- a/drivers/mmc/host/mxcmmc.c
+++ b/drivers/mmc/host/mxcmmc.c
@@ -34,10 +34,14 @@
 #include <linux/regulator/consumer.h>
 #include <linux/dmaengine.h>
 #include <linux/types.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_dma.h>
+#include <linux/of_gpio.h>
+#include <linux/mmc/slot-gpio.h>
 
 #include <asm/dma.h>
 #include <asm/irq.h>
-#include <asm/sizes.h>
 #include <linux/platform_data/mmc-mxcmmc.h>
 
 #include <linux/platform_data/dma-imx.h>
@@ -115,6 +119,7 @@
 enum mxcmci_type {
 	IMX21_MMC,
 	IMX31_MMC,
+	MPC512X_MMC,
 };
 
 struct mxcmci_host {
@@ -160,7 +165,7 @@ struct mxcmci_host {
 	enum mxcmci_type	devtype;
 };
 
-static struct platform_device_id mxcmci_devtype[] = {
+static const struct platform_device_id mxcmci_devtype[] = {
 	{
 		.name = "imx21-mmc",
 		.driver_data = IMX21_MMC,
@@ -168,16 +173,72 @@ static struct platform_device_id mxcmci_devtype[] = {
 		.name = "imx31-mmc",
 		.driver_data = IMX31_MMC,
 	}, {
+		.name = "mpc512x-sdhc",
+		.driver_data = MPC512X_MMC,
+	}, {
 		/* sentinel */
 	}
 };
 MODULE_DEVICE_TABLE(platform, mxcmci_devtype);
 
+static const struct of_device_id mxcmci_of_match[] = {
+	{
+		.compatible = "fsl,imx21-mmc",
+		.data = &mxcmci_devtype[IMX21_MMC],
+	}, {
+		.compatible = "fsl,imx31-mmc",
+		.data = &mxcmci_devtype[IMX31_MMC],
+	}, {
+		.compatible = "fsl,mpc5121-sdhc",
+		.data = &mxcmci_devtype[MPC512X_MMC],
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(of, mxcmci_of_match);
+
 static inline int is_imx31_mmc(struct mxcmci_host *host)
 {
 	return host->devtype == IMX31_MMC;
 }
 
+static inline int is_mpc512x_mmc(struct mxcmci_host *host)
+{
+	return host->devtype == MPC512X_MMC;
+}
+
+static inline u32 mxcmci_readl(struct mxcmci_host *host, int reg)
+{
+	if (IS_ENABLED(CONFIG_PPC_MPC512x))
+		return ioread32be(host->base + reg);
+	else
+		return readl(host->base + reg);
+}
+
+static inline void mxcmci_writel(struct mxcmci_host *host, u32 val, int reg)
+{
+	if (IS_ENABLED(CONFIG_PPC_MPC512x))
+		iowrite32be(val, host->base + reg);
+	else
+		writel(val, host->base + reg);
+}
+
+static inline u16 mxcmci_readw(struct mxcmci_host *host, int reg)
+{
+	if (IS_ENABLED(CONFIG_PPC_MPC512x))
+		return ioread32be(host->base + reg);
+	else
+		return readw(host->base + reg);
+}
+
+static inline void mxcmci_writew(struct mxcmci_host *host, u16 val, int reg)
+{
+	if (IS_ENABLED(CONFIG_PPC_MPC512x))
+		iowrite32be(val, host->base + reg);
+	else
+		writew(val, host->base + reg);
+}
+
 static void mxcmci_set_clk_rate(struct mxcmci_host *host, unsigned int clk_ios);
 
 static inline void mxcmci_init_ocr(struct mxcmci_host *host)
@@ -229,17 +290,40 @@ static void mxcmci_softreset(struct mxcmci_host *host)
 	dev_dbg(mmc_dev(host->mmc), "mxcmci_softreset\n");
 
 	/* reset sequence */
-	writew(STR_STP_CLK_RESET, host->base + MMC_REG_STR_STP_CLK);
-	writew(STR_STP_CLK_RESET | STR_STP_CLK_START_CLK,
-			host->base + MMC_REG_STR_STP_CLK);
+	mxcmci_writew(host, STR_STP_CLK_RESET, MMC_REG_STR_STP_CLK);
+	mxcmci_writew(host, STR_STP_CLK_RESET | STR_STP_CLK_START_CLK,
+			MMC_REG_STR_STP_CLK);
 
 	for (i = 0; i < 8; i++)
-		writew(STR_STP_CLK_START_CLK, host->base + MMC_REG_STR_STP_CLK);
+		mxcmci_writew(host, STR_STP_CLK_START_CLK, MMC_REG_STR_STP_CLK);
 
-	writew(0xff, host->base + MMC_REG_RES_TO);
+	mxcmci_writew(host, 0xff, MMC_REG_RES_TO);
 }
 static int mxcmci_setup_dma(struct mmc_host *mmc);
 
+#if IS_ENABLED(CONFIG_PPC_MPC512x)
+static inline void buffer_swap32(u32 *buf, int len)
+{
+	int i;
+
+	for (i = 0; i < ((len + 3) / 4); i++) {
+		st_le32(buf, *buf);
+		buf++;
+	}
+}
+
+static void mxcmci_swap_buffers(struct mmc_data *data)
+{
+	struct scatterlist *sg;
+	int i;
+
+	for_each_sg(data->sg, sg, data->sg_len, i)
+		buffer_swap32(sg_virt(sg), sg->length);
+}
+#else
+static inline void mxcmci_swap_buffers(struct mmc_data *data) {}
+#endif
+
 static int mxcmci_setup_data(struct mxcmci_host *host, struct mmc_data *data)
 {
 	unsigned int nob = data->blocks;
@@ -255,8 +339,8 @@ static int mxcmci_setup_data(struct mxcmci_host *host, struct mmc_data *data)
 	host->data = data;
 	data->bytes_xfered = 0;
 
-	writew(nob, host->base + MMC_REG_NOB);
-	writew(blksz, host->base + MMC_REG_BLK_LEN);
+	mxcmci_writew(host, nob, MMC_REG_NOB);
+	mxcmci_writew(host, blksz, MMC_REG_BLK_LEN);
 	host->datasize = datasize;
 
 	if (!mxcmci_use_dma(host))
@@ -275,6 +359,8 @@ static int mxcmci_setup_data(struct mxcmci_host *host, struct mmc_data *data)
 	} else {
 		host->dma_dir = DMA_TO_DEVICE;
 		slave_dirn = DMA_MEM_TO_DEV;
+
+		mxcmci_swap_buffers(data);
 	}
 
 	nents = dma_map_sg(host->dma->device->dev, data->sg,
@@ -312,13 +398,13 @@ static void mxcmci_dma_callback(void *data)
 
 	del_timer(&host->watchdog);
 
-	stat = readl(host->base + MMC_REG_STATUS);
-	writel(stat & ~STATUS_DATA_TRANS_DONE, host->base + MMC_REG_STATUS);
+	stat = mxcmci_readl(host, MMC_REG_STATUS);
+	mxcmci_writel(host, stat & ~STATUS_DATA_TRANS_DONE, 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_writel(host, STATUS_READ_OP_DONE, MMC_REG_STATUS);
 
 	mxcmci_data_done(host, stat);
 }
@@ -366,12 +452,12 @@ static int mxcmci_start_cmd(struct mxcmci_host *host, struct mmc_command *cmd,
 	spin_lock_irqsave(&host->lock, flags);
 	if (host->use_sdio)
 		int_cntr |= INT_SDIO_IRQ_EN;
-	writel(int_cntr, host->base + MMC_REG_INT_CNTR);
+	mxcmci_writel(host, int_cntr, MMC_REG_INT_CNTR);
 	spin_unlock_irqrestore(&host->lock, flags);
 
-	writew(cmd->opcode, host->base + MMC_REG_CMD);
-	writel(cmd->arg, host->base + MMC_REG_ARG);
-	writew(cmdat, host->base + MMC_REG_CMD_DAT_CONT);
+	mxcmci_writew(host, cmd->opcode, MMC_REG_CMD);
+	mxcmci_writel(host, cmd->arg, MMC_REG_ARG);
+	mxcmci_writew(host, cmdat, MMC_REG_CMD_DAT_CONT);
 
 	return 0;
 }
@@ -385,7 +471,7 @@ static void mxcmci_finish_request(struct mxcmci_host *host,
 	spin_lock_irqsave(&host->lock, flags);
 	if (host->use_sdio)
 		int_cntr |= INT_SDIO_IRQ_EN;
-	writel(int_cntr, host->base + MMC_REG_INT_CNTR);
+	mxcmci_writel(host, int_cntr, MMC_REG_INT_CNTR);
 	spin_unlock_irqrestore(&host->lock, flags);
 
 	host->req = NULL;
@@ -400,9 +486,11 @@ 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))
+	if (mxcmci_use_dma(host)) {
 		dma_unmap_sg(host->dma->device->dev, data->sg, data->sg_len,
 				host->dma_dir);
+		mxcmci_swap_buffers(data);
+	}
 
 	if (stat & STATUS_ERR_MASK) {
 		dev_dbg(mmc_dev(host->mmc), "request failed. status: 0x%08x\n",
@@ -460,14 +548,14 @@ static void mxcmci_read_response(struct mxcmci_host *host, unsigned int stat)
 	if (cmd->flags & MMC_RSP_PRESENT) {
 		if (cmd->flags & MMC_RSP_136) {
 			for (i = 0; i < 4; i++) {
-				a = readw(host->base + MMC_REG_RES_FIFO);
-				b = readw(host->base + MMC_REG_RES_FIFO);
+				a = mxcmci_readw(host, MMC_REG_RES_FIFO);
+				b = mxcmci_readw(host, MMC_REG_RES_FIFO);
 				cmd->resp[i] = a << 16 | b;
 			}
 		} else {
-			a = readw(host->base + MMC_REG_RES_FIFO);
-			b = readw(host->base + MMC_REG_RES_FIFO);
-			c = readw(host->base + MMC_REG_RES_FIFO);
+			a = mxcmci_readw(host, MMC_REG_RES_FIFO);
+			b = mxcmci_readw(host, MMC_REG_RES_FIFO);
+			c = mxcmci_readw(host, MMC_REG_RES_FIFO);
 			cmd->resp[0] = a << 24 | b << 8 | c >> 8;
 		}
 	}
@@ -479,7 +567,7 @@ static int mxcmci_poll_status(struct mxcmci_host *host, u32 mask)
 	unsigned long timeout = jiffies + HZ;
 
 	do {
-		stat = readl(host->base + MMC_REG_STATUS);
+		stat = mxcmci_readl(host, MMC_REG_STATUS);
 		if (stat & STATUS_ERR_MASK)
 			return stat;
 		if (time_after(jiffies, timeout)) {
@@ -503,7 +591,7 @@ static int mxcmci_pull(struct mxcmci_host *host, void *_buf, int bytes)
 				STATUS_BUF_READ_RDY | STATUS_READ_OP_DONE);
 		if (stat)
 			return stat;
-		*buf++ = readl(host->base + MMC_REG_BUFFER_ACCESS);
+		*buf++ = cpu_to_le32(mxcmci_readl(host, MMC_REG_BUFFER_ACCESS));
 		bytes -= 4;
 	}
 
@@ -515,7 +603,7 @@ static int mxcmci_pull(struct mxcmci_host *host, void *_buf, int bytes)
 				STATUS_BUF_READ_RDY | STATUS_READ_OP_DONE);
 		if (stat)
 			return stat;
-		tmp = readl(host->base + MMC_REG_BUFFER_ACCESS);
+		tmp = cpu_to_le32(mxcmci_readl(host, MMC_REG_BUFFER_ACCESS));
 		memcpy(b, &tmp, bytes);
 	}
 
@@ -531,7 +619,7 @@ static int mxcmci_push(struct mxcmci_host *host, void *_buf, int bytes)
 		stat = mxcmci_poll_status(host, STATUS_BUF_WRITE_RDY);
 		if (stat)
 			return stat;
-		writel(*buf++, host->base + MMC_REG_BUFFER_ACCESS);
+		mxcmci_writel(host, cpu_to_le32(*buf++), MMC_REG_BUFFER_ACCESS);
 		bytes -= 4;
 	}
 
@@ -544,7 +632,7 @@ static int mxcmci_push(struct mxcmci_host *host, void *_buf, int bytes)
 			return stat;
 
 		memcpy(&tmp, b, bytes);
-		writel(tmp, host->base + MMC_REG_BUFFER_ACCESS);
+		mxcmci_writel(host, cpu_to_le32(tmp), MMC_REG_BUFFER_ACCESS);
 	}
 
 	stat = mxcmci_poll_status(host, STATUS_BUF_WRITE_RDY);
@@ -590,8 +678,8 @@ static void mxcmci_datawork(struct work_struct *work)
 						  datawork);
 	int datastat = mxcmci_transfer_data(host);
 
-	writel(STATUS_READ_OP_DONE | STATUS_WRITE_OP_DONE,
-		host->base + MMC_REG_STATUS);
+	mxcmci_writel(host, STATUS_READ_OP_DONE | STATUS_WRITE_OP_DONE,
+		MMC_REG_STATUS);
 	mxcmci_finish_data(host, datastat);
 
 	if (host->req->stop) {
@@ -606,24 +694,40 @@ static void mxcmci_datawork(struct work_struct *work)
 
 static void mxcmci_data_done(struct mxcmci_host *host, unsigned int stat)
 {
-	struct mmc_data *data = host->data;
+	struct mmc_request *req;
 	int data_error;
+	unsigned long flags;
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	if (!host->data) {
+		spin_unlock_irqrestore(&host->lock, flags);
+		return;
+	}
 
-	if (!data)
+	if (!host->req) {
+		spin_unlock_irqrestore(&host->lock, flags);
 		return;
+	}
+
+	req = host->req;
+	if (!req->stop)
+		host->req = NULL; /* we will handle finish req below */
 
 	data_error = mxcmci_finish_data(host, stat);
 
+	spin_unlock_irqrestore(&host->lock, flags);
+
 	mxcmci_read_response(host, stat);
 	host->cmd = NULL;
 
-	if (host->req->stop) {
-		if (mxcmci_start_cmd(host, host->req->stop, 0)) {
-			mxcmci_finish_request(host, host->req);
+	if (req->stop) {
+		if (mxcmci_start_cmd(host, req->stop, 0)) {
+			mxcmci_finish_request(host, req);
 			return;
 		}
 	} else {
-		mxcmci_finish_request(host, host->req);
+		mxcmci_finish_request(host, req);
 	}
 }
 
@@ -653,9 +757,11 @@ static irqreturn_t mxcmci_irq(int irq, void *devid)
 	bool sdio_irq;
 	u32 stat;
 
-	stat = readl(host->base + MMC_REG_STATUS);
-	writel(stat & ~(STATUS_SDIO_INT_ACTIVE | STATUS_DATA_TRANS_DONE |
-			STATUS_WRITE_OP_DONE), host->base + MMC_REG_STATUS);
+	stat = mxcmci_readl(host, MMC_REG_STATUS);
+	mxcmci_writel(host,
+		stat & ~(STATUS_SDIO_INT_ACTIVE | STATUS_DATA_TRANS_DONE |
+			 STATUS_WRITE_OP_DONE),
+		MMC_REG_STATUS);
 
 	dev_dbg(mmc_dev(host->mmc), "%s: 0x%08x\n", __func__, stat);
 
@@ -665,11 +771,11 @@ static irqreturn_t mxcmci_irq(int irq, void *devid)
 
 	if (mxcmci_use_dma(host) &&
 	    (stat & (STATUS_READ_OP_DONE | STATUS_WRITE_OP_DONE)))
-		writel(STATUS_READ_OP_DONE | STATUS_WRITE_OP_DONE,
-			host->base + MMC_REG_STATUS);
+		mxcmci_writel(host, STATUS_READ_OP_DONE | STATUS_WRITE_OP_DONE,
+			MMC_REG_STATUS);
 
 	if (sdio_irq) {
-		writel(STATUS_SDIO_INT_ACTIVE, host->base + MMC_REG_STATUS);
+		mxcmci_writel(host, STATUS_SDIO_INT_ACTIVE, MMC_REG_STATUS);
 		mmc_signal_sdio_irq(host->mmc);
 	}
 
@@ -751,7 +857,7 @@ static void mxcmci_set_clk_rate(struct mxcmci_host *host, unsigned int clk_ios)
 			prescaler <<= 1;
 	}
 
-	writew((prescaler << 4) | divider, host->base + MMC_REG_CLK_RATE);
+	mxcmci_writew(host, (prescaler << 4) | divider, MMC_REG_CLK_RATE);
 
 	dev_dbg(mmc_dev(host->mmc), "scaler: %d divider: %d in: %d out: %d\n",
 			prescaler, divider, clk_in, clk_ios);
@@ -814,9 +920,9 @@ static void mxcmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 
 	if (ios->clock) {
 		mxcmci_set_clk_rate(host, ios->clock);
-		writew(STR_STP_CLK_START_CLK, host->base + MMC_REG_STR_STP_CLK);
+		mxcmci_writew(host, STR_STP_CLK_START_CLK, MMC_REG_STR_STP_CLK);
 	} else {
-		writew(STR_STP_CLK_STOP_CLK, host->base + MMC_REG_STR_STP_CLK);
+		mxcmci_writew(host, STR_STP_CLK_STOP_CLK, MMC_REG_STR_STP_CLK);
 	}
 
 	host->clock = ios->clock;
@@ -839,10 +945,11 @@ static int mxcmci_get_ro(struct mmc_host *mmc)
 	if (host->pdata && host->pdata->get_ro)
 		return !!host->pdata->get_ro(mmc_dev(mmc));
 	/*
-	 * Board doesn't support read only detection; let the mmc core
-	 * decide what to do.
+	 * If board doesn't support read only detection (no mmc_gpio
+	 * context or gpio is invalid), then let the mmc core decide
+	 * what to do.
 	 */
-	return -ENOSYS;
+	return mmc_gpio_get_ro(mmc);
 }
 
 static void mxcmci_enable_sdio_irq(struct mmc_host *mmc, int enable)
@@ -853,14 +960,14 @@ static void mxcmci_enable_sdio_irq(struct mmc_host *mmc, int enable)
 
 	spin_lock_irqsave(&host->lock, flags);
 	host->use_sdio = enable;
-	int_cntr = readl(host->base + MMC_REG_INT_CNTR);
+	int_cntr = mxcmci_readl(host, MMC_REG_INT_CNTR);
 
 	if (enable)
 		int_cntr |= INT_SDIO_IRQ_EN;
 	else
 		int_cntr &= ~INT_SDIO_IRQ_EN;
 
-	writel(int_cntr, host->base + MMC_REG_INT_CNTR);
+	mxcmci_writel(host, int_cntr, MMC_REG_INT_CNTR);
 	spin_unlock_irqrestore(&host->lock, flags);
 }
 
@@ -898,7 +1005,7 @@ 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);
+	unsigned int stat = mxcmci_readl(host, MMC_REG_STATUS);
 
 	if (host->dma_dir == DMA_FROM_DEVICE) {
 		dmaengine_terminate_all(host->dma);
@@ -914,7 +1021,8 @@ static void mxcmci_watchdog(unsigned long data)
 
 	/* Mark transfer as erroneus and inform the upper layers */
 
-	host->data->error = -ETIMEDOUT;
+	if (host->data)
+		host->data->error = -ETIMEDOUT;
 	host->req = NULL;
 	host->cmd = NULL;
 	host->data = NULL;
@@ -935,9 +1043,14 @@ static int mxcmci_probe(struct platform_device *pdev)
 	struct mxcmci_host *host = NULL;
 	struct resource *iores, *r;
 	int ret = 0, irq;
+	bool dat3_card_detect = false;
 	dma_cap_mask_t mask;
+	const struct of_device_id *of_id;
+	struct imxmmc_platform_data *pdata = pdev->dev.platform_data;
 
-	pr_info("i.MX SDHC driver\n");
+	pr_info("i.MX/MPC512x SDHC driver\n");
+
+	of_id = of_match_device(mxcmci_of_match, &pdev->dev);
 
 	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	irq = platform_get_irq(pdev, 0);
@@ -954,11 +1067,16 @@ static int mxcmci_probe(struct platform_device *pdev)
 		goto out_release_mem;
 	}
 
+	mmc_of_parse(mmc);
 	mmc->ops = &mxcmci_ops;
-	mmc->caps = MMC_CAP_4_BIT_DATA | MMC_CAP_SDIO_IRQ;
+
+	/* For devicetree parsing, the bus width is read from devicetree */
+	if (pdata)
+		mmc->caps = MMC_CAP_4_BIT_DATA | MMC_CAP_SDIO_IRQ;
+	else
+		mmc->caps |= MMC_CAP_SDIO_IRQ;
 
 	/* MMC core transfer sizes tunable parameters */
-	mmc->max_segs = 64;
 	mmc->max_blk_size = 2048;
 	mmc->max_blk_count = 65535;
 	mmc->max_req_size = mmc->max_blk_size * mmc->max_blk_count;
@@ -971,14 +1089,30 @@ static int mxcmci_probe(struct platform_device *pdev)
 		goto out_free;
 	}
 
+	if (of_id) {
+		const struct platform_device_id *id_entry = of_id->data;
+		host->devtype = id_entry->driver_data;
+	} else {
+		host->devtype = pdev->id_entry->driver_data;
+	}
+
+	/* adjust max_segs after devtype detection */
+	if (!is_mpc512x_mmc(host))
+		mmc->max_segs = 64;
+
 	host->mmc = mmc;
-	host->pdata = pdev->dev.platform_data;
-	host->devtype = pdev->id_entry->driver_data;
+	host->pdata = pdata;
 	spin_lock_init(&host->lock);
 
+	if (pdata)
+		dat3_card_detect = pdata->dat3_card_detect;
+	else if (!(mmc->caps & MMC_CAP_NONREMOVABLE)
+			&& !of_property_read_bool(pdev->dev.of_node, "cd-gpios"))
+		dat3_card_detect = true;
+
 	mxcmci_init_ocr(host);
 
-	if (host->pdata && host->pdata->dat3_card_detect)
+	if (dat3_card_detect)
 		host->default_irq_mask =
 			INT_CARD_INSERTION_EN | INT_CARD_REMOVAL_EN;
 	else
@@ -1004,7 +1138,7 @@ static int mxcmci_probe(struct platform_device *pdev)
 
 	mxcmci_softreset(host);
 
-	host->rev_no = readw(host->base + MMC_REG_REV_NO);
+	host->rev_no = mxcmci_readw(host, MMC_REG_REV_NO);
 	if (host->rev_no != 0x400) {
 		ret = -ENODEV;
 		dev_err(mmc_dev(host->mmc), "wrong rev.no. 0x%08x. aborting.\n",
@@ -1016,25 +1150,28 @@ static int mxcmci_probe(struct platform_device *pdev)
 	mmc->f_max = clk_get_rate(host->clk_per) >> 1;
 
 	/* recommended in data sheet */
-	writew(0x2db4, host->base + MMC_REG_READ_TO);
-
-	writel(host->default_irq_mask, host->base + MMC_REG_INT_CNTR);
-
-	r = platform_get_resource(pdev, IORESOURCE_DMA, 0);
-	if (r) {
-		host->dmareq = r->start;
-		host->dma_data.peripheral_type = IMX_DMATYPE_SDHC;
-		host->dma_data.priority = DMA_PRIO_LOW;
-		host->dma_data.dma_request = host->dmareq;
-		dma_cap_zero(mask);
-		dma_cap_set(DMA_SLAVE, mask);
-		host->dma = dma_request_channel(mask, filter, host);
-		if (host->dma)
-			mmc->max_seg_size = dma_get_max_seg_size(
-					host->dma->device->dev);
-	}
+	mxcmci_writew(host, 0x2db4, MMC_REG_READ_TO);
+
+	mxcmci_writel(host, host->default_irq_mask, MMC_REG_INT_CNTR);
 
-	if (!host->dma)
+	if (!host->pdata) {
+		host->dma = dma_request_slave_channel(&pdev->dev, "rx-tx");
+	} else {
+		r = platform_get_resource(pdev, IORESOURCE_DMA, 0);
+		if (r) {
+			host->dmareq = r->start;
+			host->dma_data.peripheral_type = IMX_DMATYPE_SDHC;
+			host->dma_data.priority = DMA_PRIO_LOW;
+			host->dma_data.dma_request = host->dmareq;
+			dma_cap_zero(mask);
+			dma_cap_set(DMA_SLAVE, mask);
+			host->dma = dma_request_channel(mask, filter, host);
+		}
+	}
+	if (host->dma)
+		mmc->max_seg_size = dma_get_max_seg_size(
+				host->dma->device->dev);
+	else
 		dev_info(mmc_dev(host->mmc), "dma not available. Using PIO\n");
 
 	INIT_WORK(&host->datawork, mxcmci_datawork);
@@ -1052,12 +1189,12 @@ static int mxcmci_probe(struct platform_device *pdev)
 			goto out_free_irq;
 	}
 
-	mmc_add_host(mmc);
-
 	init_timer(&host->watchdog);
 	host->watchdog.function = &mxcmci_watchdog;
 	host->watchdog.data = (unsigned long)mmc;
 
+	mmc_add_host(mmc);
+
 	return 0;
 
 out_free_irq:
@@ -1153,6 +1290,7 @@ static struct platform_driver mxcmci_driver = {
 #ifdef CONFIG_PM
 		.pm	= &mxcmci_pm_ops,
 #endif
+		.of_match_table	= mxcmci_of_match,
 	}
 };
 
diff --git a/drivers/mmc/host/mxs-mmc.c b/drivers/mmc/host/mxs-mmc.c
index 4efe3021b217..4278a1787d08 100644
--- a/drivers/mmc/host/mxs-mmc.c
+++ b/drivers/mmc/host/mxs-mmc.c
@@ -72,6 +72,9 @@ struct mxs_mmc_host {
 	int				sdio_irq_en;
 	int				wp_gpio;
 	bool				wp_inverted;
+	bool				cd_inverted;
+	bool				broken_cd;
+	bool				non_removable;
 };
 
 static int mxs_mmc_get_ro(struct mmc_host *mmc)
@@ -95,8 +98,9 @@ static int mxs_mmc_get_cd(struct mmc_host *mmc)
 	struct mxs_mmc_host *host = mmc_priv(mmc);
 	struct mxs_ssp *ssp = &host->ssp;
 
-	return !(readl(ssp->base + HW_SSP_STATUS(ssp)) &
-		 BM_SSP_STATUS_CARD_DETECT);
+	return host->non_removable || host->broken_cd ||
+		!(readl(ssp->base + HW_SSP_STATUS(ssp)) &
+		  BM_SSP_STATUS_CARD_DETECT) ^ host->cd_inverted;
 }
 
 static void mxs_mmc_reset(struct mxs_mmc_host *host)
@@ -548,22 +552,6 @@ static const struct mmc_host_ops mxs_mmc_ops = {
 	.enable_sdio_irq = mxs_mmc_enable_sdio_irq,
 };
 
-static bool mxs_mmc_dma_filter(struct dma_chan *chan, void *param)
-{
-	struct mxs_mmc_host *host = param;
-	struct mxs_ssp *ssp = &host->ssp;
-
-	if (!mxs_dma_is_apbh(chan))
-		return false;
-
-	if (chan->chan_id != ssp->dma_channel)
-		return false;
-
-	chan->private = &ssp->dma_data;
-
-	return true;
-}
-
 static struct platform_device_id mxs_ssp_ids[] = {
 	{
 		.name = "imx23-mmc",
@@ -591,20 +579,17 @@ static int mxs_mmc_probe(struct platform_device *pdev)
 	struct device_node *np = pdev->dev.of_node;
 	struct mxs_mmc_host *host;
 	struct mmc_host *mmc;
-	struct resource *iores, *dmares;
+	struct resource *iores;
 	struct pinctrl *pinctrl;
-	int ret = 0, irq_err, irq_dma;
-	dma_cap_mask_t mask;
+	int ret = 0, irq_err;
 	struct regulator *reg_vmmc;
 	enum of_gpio_flags flags;
 	struct mxs_ssp *ssp;
 	u32 bus_width = 0;
 
 	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	dmares = platform_get_resource(pdev, IORESOURCE_DMA, 0);
 	irq_err = platform_get_irq(pdev, 0);
-	irq_dma = platform_get_irq(pdev, 1);
-	if (!iores || irq_err < 0 || irq_dma < 0)
+	if (!iores || irq_err < 0)
 		return -EINVAL;
 
 	mmc = mmc_alloc_host(sizeof(struct mxs_mmc_host), &pdev->dev);
@@ -620,23 +605,7 @@ static int mxs_mmc_probe(struct platform_device *pdev)
 		goto out_mmc_free;
 	}
 
-	if (np) {
-		ssp->devid = (enum mxs_ssp_id) of_id->data;
-		/*
-		 * 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(np, "fsl,ssp-dma-channel",
-					   &ssp->dma_channel);
-		if (ret) {
-			dev_err(mmc_dev(host->mmc),
-				"failed to get dma channel\n");
-			goto out_mmc_free;
-		}
-	} else {
-		ssp->devid = pdev->id_entry->driver_data;
-		ssp->dma_channel = dmares->start;
-	}
+	ssp->devid = (enum mxs_ssp_id) of_id->data;
 
 	host->mmc = mmc;
 	host->sdio_irq_en = 0;
@@ -666,10 +635,7 @@ static int mxs_mmc_probe(struct platform_device *pdev)
 
 	mxs_mmc_reset(host);
 
-	dma_cap_zero(mask);
-	dma_cap_set(DMA_SLAVE, mask);
-	ssp->dma_data.chan_irq = irq_dma;
-	ssp->dmach = dma_request_channel(mask, mxs_mmc_dma_filter, host);
+	ssp->dmach = dma_request_slave_channel(&pdev->dev, "rx-tx");
 	if (!ssp->dmach) {
 		dev_err(mmc_dev(host->mmc),
 			"%s: failed to request dma\n", __func__);
@@ -686,11 +652,16 @@ static int mxs_mmc_probe(struct platform_device *pdev)
 		mmc->caps |= MMC_CAP_4_BIT_DATA;
 	else if (bus_width == 8)
 		mmc->caps |= MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA;
+	host->broken_cd = of_property_read_bool(np, "broken-cd");
+	host->non_removable = of_property_read_bool(np, "non-removable");
+	if (host->non_removable)
+		mmc->caps |= MMC_CAP_NONREMOVABLE;
 	host->wp_gpio = of_get_named_gpio_flags(np, "wp-gpios", 0, &flags);
-
 	if (flags & OF_GPIO_ACTIVE_LOW)
 		host->wp_inverted = 1;
 
+	host->cd_inverted = of_property_read_bool(np, "cd-inverted");
+
 	mmc->f_min = 400000;
 	mmc->f_max = 288000000;
 	mmc->ocr_avail = MMC_VDD_32_33 | MMC_VDD_33_34;
diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index bc5807873b2c..6e44025acf01 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -1717,6 +1717,12 @@ static struct omap_mmc_platform_data *of_get_hsmmc_pdata(struct device *dev)
 	struct omap_mmc_platform_data *pdata;
 	struct device_node *np = dev->of_node;
 	u32 bus_width, max_freq;
+	int cd_gpio, wp_gpio;
+
+	cd_gpio = of_get_named_gpio(np, "cd-gpios", 0);
+	wp_gpio = of_get_named_gpio(np, "wp-gpios", 0);
+	if (cd_gpio == -EPROBE_DEFER || wp_gpio == -EPROBE_DEFER)
+		return ERR_PTR(-EPROBE_DEFER);
 
 	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
@@ -1727,8 +1733,8 @@ static struct omap_mmc_platform_data *of_get_hsmmc_pdata(struct device *dev)
 
 	/* This driver only supports 1 slot */
 	pdata->nr_slots = 1;
-	pdata->slots[0].switch_pin = of_get_named_gpio(np, "cd-gpios", 0);
-	pdata->slots[0].gpio_wp = of_get_named_gpio(np, "wp-gpios", 0);
+	pdata->slots[0].switch_pin = cd_gpio;
+	pdata->slots[0].gpio_wp = wp_gpio;
 
 	if (of_find_property(np, "ti,non-removable", NULL)) {
 		pdata->slots[0].nonremovable = true;
@@ -1774,6 +1780,10 @@ static int omap_hsmmc_probe(struct platform_device *pdev)
 	match = of_match_device(of_match_ptr(omap_mmc_of_match), &pdev->dev);
 	if (match) {
 		pdata = of_get_hsmmc_pdata(&pdev->dev);
+
+		if (IS_ERR(pdata))
+			return PTR_ERR(pdata);
+
 		if (match->data) {
 			const u16 *offsetp = match->data;
 			pdata->reg_offset = *offsetp;
diff --git a/drivers/mmc/host/rtsx_pci_sdmmc.c b/drivers/mmc/host/rtsx_pci_sdmmc.c
index f981f7d1f6e3..ad13f4240c49 100644
--- a/drivers/mmc/host/rtsx_pci_sdmmc.c
+++ b/drivers/mmc/host/rtsx_pci_sdmmc.c
@@ -57,6 +57,9 @@ struct realtek_pci_sdmmc {
 	bool			eject;
 	bool			initial_mode;
 	bool			ddr_mode;
+	int			power_state;
+#define SDMMC_POWER_ON		1
+#define SDMMC_POWER_OFF		0
 };
 
 static inline struct device *sdmmc_dev(struct realtek_pci_sdmmc *host)
@@ -765,6 +768,9 @@ static int sd_power_on(struct realtek_pci_sdmmc *host)
 	struct rtsx_pcr *pcr = host->pcr;
 	int err;
 
+	if (host->power_state == SDMMC_POWER_ON)
+		return 0;
+
 	rtsx_pci_init_cmd(pcr);
 	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CARD_SELECT, 0x07, SD_MOD_SEL);
 	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CARD_SHARE_MODE,
@@ -787,6 +793,7 @@ static int sd_power_on(struct realtek_pci_sdmmc *host)
 	if (err < 0)
 		return err;
 
+	host->power_state = SDMMC_POWER_ON;
 	return 0;
 }
 
@@ -795,6 +802,8 @@ static int sd_power_off(struct realtek_pci_sdmmc *host)
 	struct rtsx_pcr *pcr = host->pcr;
 	int err;
 
+	host->power_state = SDMMC_POWER_OFF;
+
 	rtsx_pci_init_cmd(pcr);
 
 	rtsx_pci_add_cmd(pcr, WRITE_REG_CMD, CARD_CLK_EN, SD_CLK_EN, 0);
@@ -1260,6 +1269,7 @@ static int rtsx_pci_sdmmc_drv_probe(struct platform_device *pdev)
 	host->pcr = pcr;
 	host->mmc = mmc;
 	host->pdev = pdev;
+	host->power_state = SDMMC_POWER_OFF;
 	platform_set_drvdata(pdev, host);
 	pcr->slots[RTSX_SD_CARD].p_dev = pdev;
 	pcr->slots[RTSX_SD_CARD].card_event = rtsx_pci_sdmmc_card_event;
diff --git a/drivers/mmc/host/s3cmci.c b/drivers/mmc/host/s3cmci.c
index 63fb265e0da6..8d6794cdf899 100644
--- a/drivers/mmc/host/s3cmci.c
+++ b/drivers/mmc/host/s3cmci.c
@@ -25,14 +25,93 @@
 
 #include <mach/dma.h>
 
-#include <mach/regs-sdi.h>
-
 #include <linux/platform_data/mmc-s3cmci.h>
 
 #include "s3cmci.h"
 
 #define DRIVER_NAME "s3c-mci"
 
+#define S3C2410_SDICON			(0x00)
+#define S3C2410_SDIPRE			(0x04)
+#define S3C2410_SDICMDARG		(0x08)
+#define S3C2410_SDICMDCON		(0x0C)
+#define S3C2410_SDICMDSTAT		(0x10)
+#define S3C2410_SDIRSP0			(0x14)
+#define S3C2410_SDIRSP1			(0x18)
+#define S3C2410_SDIRSP2			(0x1C)
+#define S3C2410_SDIRSP3			(0x20)
+#define S3C2410_SDITIMER		(0x24)
+#define S3C2410_SDIBSIZE		(0x28)
+#define S3C2410_SDIDCON			(0x2C)
+#define S3C2410_SDIDCNT			(0x30)
+#define S3C2410_SDIDSTA			(0x34)
+#define S3C2410_SDIFSTA			(0x38)
+
+#define S3C2410_SDIDATA			(0x3C)
+#define S3C2410_SDIIMSK			(0x40)
+
+#define S3C2440_SDIDATA			(0x40)
+#define S3C2440_SDIIMSK			(0x3C)
+
+#define S3C2440_SDICON_SDRESET		(1 << 8)
+#define S3C2410_SDICON_SDIOIRQ		(1 << 3)
+#define S3C2410_SDICON_FIFORESET	(1 << 1)
+#define S3C2410_SDICON_CLOCKTYPE	(1 << 0)
+
+#define S3C2410_SDICMDCON_LONGRSP	(1 << 10)
+#define S3C2410_SDICMDCON_WAITRSP	(1 << 9)
+#define S3C2410_SDICMDCON_CMDSTART	(1 << 8)
+#define S3C2410_SDICMDCON_SENDERHOST	(1 << 6)
+#define S3C2410_SDICMDCON_INDEX		(0x3f)
+
+#define S3C2410_SDICMDSTAT_CRCFAIL	(1 << 12)
+#define S3C2410_SDICMDSTAT_CMDSENT	(1 << 11)
+#define S3C2410_SDICMDSTAT_CMDTIMEOUT	(1 << 10)
+#define S3C2410_SDICMDSTAT_RSPFIN	(1 << 9)
+
+#define S3C2440_SDIDCON_DS_WORD		(2 << 22)
+#define S3C2410_SDIDCON_TXAFTERRESP	(1 << 20)
+#define S3C2410_SDIDCON_RXAFTERCMD	(1 << 19)
+#define S3C2410_SDIDCON_BLOCKMODE	(1 << 17)
+#define S3C2410_SDIDCON_WIDEBUS		(1 << 16)
+#define S3C2410_SDIDCON_DMAEN		(1 << 15)
+#define S3C2410_SDIDCON_STOP		(1 << 14)
+#define S3C2440_SDIDCON_DATSTART	(1 << 14)
+
+#define S3C2410_SDIDCON_XFER_RXSTART	(2 << 12)
+#define S3C2410_SDIDCON_XFER_TXSTART	(3 << 12)
+
+#define S3C2410_SDIDCON_BLKNUM_MASK	(0xFFF)
+
+#define S3C2410_SDIDSTA_SDIOIRQDETECT	(1 << 9)
+#define S3C2410_SDIDSTA_FIFOFAIL	(1 << 8)
+#define S3C2410_SDIDSTA_CRCFAIL		(1 << 7)
+#define S3C2410_SDIDSTA_RXCRCFAIL	(1 << 6)
+#define S3C2410_SDIDSTA_DATATIMEOUT	(1 << 5)
+#define S3C2410_SDIDSTA_XFERFINISH	(1 << 4)
+#define S3C2410_SDIDSTA_TXDATAON	(1 << 1)
+#define S3C2410_SDIDSTA_RXDATAON	(1 << 0)
+
+#define S3C2440_SDIFSTA_FIFORESET	(1 << 16)
+#define S3C2440_SDIFSTA_FIFOFAIL	(3 << 14)
+#define S3C2410_SDIFSTA_TFDET		(1 << 13)
+#define S3C2410_SDIFSTA_RFDET		(1 << 12)
+#define S3C2410_SDIFSTA_COUNTMASK	(0x7f)
+
+#define S3C2410_SDIIMSK_RESPONSECRC	(1 << 17)
+#define S3C2410_SDIIMSK_CMDSENT		(1 << 16)
+#define S3C2410_SDIIMSK_CMDTIMEOUT	(1 << 15)
+#define S3C2410_SDIIMSK_RESPONSEND	(1 << 14)
+#define S3C2410_SDIIMSK_SDIOIRQ		(1 << 12)
+#define S3C2410_SDIIMSK_FIFOFAIL	(1 << 11)
+#define S3C2410_SDIIMSK_CRCSTATUS	(1 << 10)
+#define S3C2410_SDIIMSK_DATACRC		(1 << 9)
+#define S3C2410_SDIIMSK_DATATIMEOUT	(1 << 8)
+#define S3C2410_SDIIMSK_DATAFINISH	(1 << 7)
+#define S3C2410_SDIIMSK_TXFIFOHALF	(1 << 4)
+#define S3C2410_SDIIMSK_RXFIFOLAST	(1 << 2)
+#define S3C2410_SDIIMSK_RXFIFOHALF	(1 << 0)
+
 enum dbg_channels {
 	dbg_err   = (1 << 0),
 	dbg_debug = (1 << 1),
diff --git a/drivers/mmc/host/sdhci-acpi.c b/drivers/mmc/host/sdhci-acpi.c
index 2592dddbd965..7bcf74b1a5cd 100644
--- a/drivers/mmc/host/sdhci-acpi.c
+++ b/drivers/mmc/host/sdhci-acpi.c
@@ -195,6 +195,8 @@ static int sdhci_acpi_probe(struct platform_device *pdev)
 		host->mmc->pm_caps  |= c->slot->pm_caps;
 	}
 
+	host->mmc->caps2 |= MMC_CAP2_NO_PRESCAN_POWERUP;
+
 	err = sdhci_add_host(host);
 	if (err)
 		goto err_free;
diff --git a/drivers/mmc/host/sdhci-bcm2835.c b/drivers/mmc/host/sdhci-bcm2835.c
index 8ffea05152c6..d49bc958c8ba 100644
--- a/drivers/mmc/host/sdhci-bcm2835.c
+++ b/drivers/mmc/host/sdhci-bcm2835.c
@@ -124,7 +124,7 @@ unsigned int bcm2835_sdhci_get_min_clock(struct sdhci_host *host)
 	return MIN_FREQ;
 }
 
-static struct sdhci_ops bcm2835_sdhci_ops = {
+static const struct sdhci_ops bcm2835_sdhci_ops = {
 	.write_l = bcm2835_sdhci_writel,
 	.write_w = bcm2835_sdhci_writew,
 	.write_b = bcm2835_sdhci_writeb,
@@ -135,7 +135,7 @@ static struct sdhci_ops bcm2835_sdhci_ops = {
 	.get_min_clock = bcm2835_sdhci_get_min_clock,
 };
 
-static struct sdhci_pltfm_data bcm2835_sdhci_pdata = {
+static const struct sdhci_pltfm_data bcm2835_sdhci_pdata = {
 	.quirks = SDHCI_QUIRK_BROKEN_CARD_DETECTION |
 		  SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK,
 	.ops = &bcm2835_sdhci_ops,
diff --git a/drivers/mmc/host/sdhci-cns3xxx.c b/drivers/mmc/host/sdhci-cns3xxx.c
index 30bfdc4ae52a..8ebb6b650f3f 100644
--- a/drivers/mmc/host/sdhci-cns3xxx.c
+++ b/drivers/mmc/host/sdhci-cns3xxx.c
@@ -16,7 +16,6 @@
 #include <linux/device.h>
 #include <linux/mmc/host.h>
 #include <linux/module.h>
-#include <mach/cns3xxx.h>
 #include "sdhci-pltfm.h"
 
 static unsigned int sdhci_cns3xxx_get_max_clk(struct sdhci_host *host)
@@ -80,12 +79,12 @@ out:
 	host->clock = clock;
 }
 
-static struct sdhci_ops sdhci_cns3xxx_ops = {
+static const struct sdhci_ops sdhci_cns3xxx_ops = {
 	.get_max_clock	= sdhci_cns3xxx_get_max_clk,
 	.set_clock	= sdhci_cns3xxx_set_clock,
 };
 
-static struct sdhci_pltfm_data sdhci_cns3xxx_pdata = {
+static const struct sdhci_pltfm_data sdhci_cns3xxx_pdata = {
 	.ops = &sdhci_cns3xxx_ops,
 	.quirks = SDHCI_QUIRK_BROKEN_DMA |
 		  SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK |
diff --git a/drivers/mmc/host/sdhci-dove.c b/drivers/mmc/host/sdhci-dove.c
index 169fab91778e..15e7803040f1 100644
--- a/drivers/mmc/host/sdhci-dove.c
+++ b/drivers/mmc/host/sdhci-dove.c
@@ -83,12 +83,12 @@ static u32 sdhci_dove_readl(struct sdhci_host *host, int reg)
 	return ret;
 }
 
-static struct sdhci_ops sdhci_dove_ops = {
+static const struct sdhci_ops sdhci_dove_ops = {
 	.read_w	= sdhci_dove_readw,
 	.read_l	= sdhci_dove_readl,
 };
 
-static struct sdhci_pltfm_data sdhci_dove_pdata = {
+static const struct sdhci_pltfm_data sdhci_dove_pdata = {
 	.ops	= &sdhci_dove_ops,
 	.quirks	= SDHCI_QUIRK_NO_SIMULT_VDD_AND_POWER |
 		  SDHCI_QUIRK_NO_BUSY_IRQ |
diff --git a/drivers/mmc/host/sdhci-esdhc-imx.c b/drivers/mmc/host/sdhci-esdhc-imx.c
index 78ac00227c1a..67d6dde2ff19 100644
--- a/drivers/mmc/host/sdhci-esdhc-imx.c
+++ b/drivers/mmc/host/sdhci-esdhc-imx.c
@@ -399,7 +399,7 @@ static int esdhc_pltfm_bus_width(struct sdhci_host *host, int width)
 	return 0;
 }
 
-static struct sdhci_ops sdhci_esdhc_ops = {
+static const struct sdhci_ops sdhci_esdhc_ops = {
 	.read_l = esdhc_readl_le,
 	.read_w = esdhc_readw_le,
 	.write_l = esdhc_writel_le,
@@ -412,7 +412,7 @@ static struct sdhci_ops sdhci_esdhc_ops = {
 	.platform_bus_width = esdhc_pltfm_bus_width,
 };
 
-static struct sdhci_pltfm_data sdhci_esdhc_imx_pdata = {
+static const struct sdhci_pltfm_data sdhci_esdhc_imx_pdata = {
 	.quirks = ESDHC_DEFAULT_QUIRKS | SDHCI_QUIRK_NO_HISPD_BIT
 			| SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC
 			| SDHCI_QUIRK_BROKEN_ADMA_ZEROLEN_DESC
diff --git a/drivers/mmc/host/sdhci-of-esdhc.c b/drivers/mmc/host/sdhci-of-esdhc.c
index f32526d2d966..5e68adc2461e 100644
--- a/drivers/mmc/host/sdhci-of-esdhc.c
+++ b/drivers/mmc/host/sdhci-of-esdhc.c
@@ -230,7 +230,7 @@ static void esdhc_of_platform_init(struct sdhci_host *host)
 		host->quirks &= ~SDHCI_QUIRK_NO_BUSY_IRQ;
 }
 
-static struct sdhci_ops sdhci_esdhc_ops = {
+static const struct sdhci_ops sdhci_esdhc_ops = {
 	.read_l = esdhc_readl,
 	.read_w = esdhc_readw,
 	.read_b = esdhc_readb,
@@ -249,7 +249,7 @@ static struct sdhci_ops sdhci_esdhc_ops = {
 	.adma_workaround = esdhci_of_adma_workaround,
 };
 
-static struct sdhci_pltfm_data sdhci_esdhc_pdata = {
+static const struct sdhci_pltfm_data sdhci_esdhc_pdata = {
 	/*
 	 * card detection could be handled via GPIO
 	 * eSDHC cannot support End Attribute in NOP ADMA descriptor
diff --git a/drivers/mmc/host/sdhci-of-hlwd.c b/drivers/mmc/host/sdhci-of-hlwd.c
index c3d3715ec3d7..200a6a9fa805 100644
--- a/drivers/mmc/host/sdhci-of-hlwd.c
+++ b/drivers/mmc/host/sdhci-of-hlwd.c
@@ -51,7 +51,7 @@ static void sdhci_hlwd_writeb(struct sdhci_host *host, u8 val, int reg)
 	udelay(SDHCI_HLWD_WRITE_DELAY);
 }
 
-static struct sdhci_ops sdhci_hlwd_ops = {
+static const struct sdhci_ops sdhci_hlwd_ops = {
 	.read_l = sdhci_be32bs_readl,
 	.read_w = sdhci_be32bs_readw,
 	.read_b = sdhci_be32bs_readb,
@@ -60,7 +60,7 @@ static struct sdhci_ops sdhci_hlwd_ops = {
 	.write_b = sdhci_hlwd_writeb,
 };
 
-static struct sdhci_pltfm_data sdhci_hlwd_pdata = {
+static const struct sdhci_pltfm_data sdhci_hlwd_pdata = {
 	.quirks = SDHCI_QUIRK_32BIT_DMA_ADDR |
 		  SDHCI_QUIRK_32BIT_DMA_SIZE,
 	.ops = &sdhci_hlwd_ops,
diff --git a/drivers/mmc/host/sdhci-pci.c b/drivers/mmc/host/sdhci-pci.c
index c7ccf3034dad..0012d3fdc999 100644
--- a/drivers/mmc/host/sdhci-pci.c
+++ b/drivers/mmc/host/sdhci-pci.c
@@ -975,7 +975,7 @@ static void sdhci_pci_hw_reset(struct sdhci_host *host)
 	usleep_range(300, 1000);
 }
 
-static struct sdhci_ops sdhci_pci_ops = {
+static const struct sdhci_ops sdhci_pci_ops = {
 	.enable_dma	= sdhci_pci_enable_dma,
 	.platform_bus_width	= sdhci_pci_bus_width,
 	.hw_reset		= sdhci_pci_hw_reset,
@@ -1279,6 +1279,8 @@ static struct sdhci_pci_slot *sdhci_pci_probe_slot(
 	}
 
 	host->mmc->pm_caps = MMC_PM_KEEP_POWER | MMC_PM_WAKE_SDIO_IRQ;
+	host->mmc->slotno = slotno;
+	host->mmc->caps2 |= MMC_CAP2_NO_PRESCAN_POWERUP;
 
 	ret = sdhci_add_host(host);
 	if (ret)
diff --git a/drivers/mmc/host/sdhci-pltfm.c b/drivers/mmc/host/sdhci-pltfm.c
index 3145a780b035..cd0f1f68e261 100644
--- a/drivers/mmc/host/sdhci-pltfm.c
+++ b/drivers/mmc/host/sdhci-pltfm.c
@@ -44,7 +44,7 @@ unsigned int sdhci_pltfm_clk_get_max_clock(struct sdhci_host *host)
 }
 EXPORT_SYMBOL_GPL(sdhci_pltfm_clk_get_max_clock);
 
-static struct sdhci_ops sdhci_pltfm_ops = {
+static const struct sdhci_ops sdhci_pltfm_ops = {
 };
 
 #ifdef CONFIG_OF
@@ -94,6 +94,7 @@ void sdhci_get_of_property(struct platform_device *pdev)
 
 		if (of_device_is_compatible(np, "fsl,p2020-esdhc") ||
 		    of_device_is_compatible(np, "fsl,p1010-esdhc") ||
+		    of_device_is_compatible(np, "fsl,t4240-esdhc") ||
 		    of_device_is_compatible(np, "fsl,mpc8536-esdhc"))
 			host->quirks |= SDHCI_QUIRK_BROKEN_TIMEOUT_VAL;
 
@@ -114,7 +115,7 @@ void sdhci_get_of_property(struct platform_device *pdev) {}
 EXPORT_SYMBOL_GPL(sdhci_get_of_property);
 
 struct sdhci_host *sdhci_pltfm_init(struct platform_device *pdev,
-				    struct sdhci_pltfm_data *pdata)
+				    const struct sdhci_pltfm_data *pdata)
 {
 	struct sdhci_host *host;
 	struct sdhci_pltfm_host *pltfm_host;
@@ -201,7 +202,7 @@ void sdhci_pltfm_free(struct platform_device *pdev)
 EXPORT_SYMBOL_GPL(sdhci_pltfm_free);
 
 int sdhci_pltfm_register(struct platform_device *pdev,
-			 struct sdhci_pltfm_data *pdata)
+			 const struct sdhci_pltfm_data *pdata)
 {
 	struct sdhci_host *host;
 	int ret = 0;
diff --git a/drivers/mmc/host/sdhci-pltfm.h b/drivers/mmc/host/sdhci-pltfm.h
index 153b6c509ebe..1210ed1b0c60 100644
--- a/drivers/mmc/host/sdhci-pltfm.h
+++ b/drivers/mmc/host/sdhci-pltfm.h
@@ -16,7 +16,7 @@
 #include "sdhci.h"
 
 struct sdhci_pltfm_data {
-	struct sdhci_ops *ops;
+	const struct sdhci_ops *ops;
 	unsigned int quirks;
 };
 
@@ -91,11 +91,11 @@ static inline void sdhci_be32bs_writeb(struct sdhci_host *host, u8 val, int reg)
 extern void sdhci_get_of_property(struct platform_device *pdev);
 
 extern struct sdhci_host *sdhci_pltfm_init(struct platform_device *pdev,
-					   struct sdhci_pltfm_data *pdata);
+					  const struct sdhci_pltfm_data *pdata);
 extern void sdhci_pltfm_free(struct platform_device *pdev);
 
 extern int sdhci_pltfm_register(struct platform_device *pdev,
-				struct sdhci_pltfm_data *pdata);
+				const struct sdhci_pltfm_data *pdata);
 extern int sdhci_pltfm_unregister(struct platform_device *pdev);
 
 extern unsigned int sdhci_pltfm_clk_get_max_clock(struct sdhci_host *host);
diff --git a/drivers/mmc/host/sdhci-pxav2.c b/drivers/mmc/host/sdhci-pxav2.c
index eeb7d439db1d..6a3f702a38a6 100644
--- a/drivers/mmc/host/sdhci-pxav2.c
+++ b/drivers/mmc/host/sdhci-pxav2.c
@@ -111,7 +111,7 @@ static int pxav2_mmc_set_width(struct sdhci_host *host, int width)
 	return 0;
 }
 
-static struct sdhci_ops pxav2_sdhci_ops = {
+static const struct sdhci_ops pxav2_sdhci_ops = {
 	.get_max_clock = sdhci_pltfm_clk_get_max_clock,
 	.platform_reset_exit = pxav2_set_private_registers,
 	.platform_bus_width = pxav2_mmc_set_width,
diff --git a/drivers/mmc/host/sdhci-pxav3.c b/drivers/mmc/host/sdhci-pxav3.c
index a0cdbc570a83..1ae358e0662d 100644
--- a/drivers/mmc/host/sdhci-pxav3.c
+++ b/drivers/mmc/host/sdhci-pxav3.c
@@ -167,13 +167,21 @@ static int pxav3_set_uhs_signaling(struct sdhci_host *host, unsigned int uhs)
 	return 0;
 }
 
-static struct sdhci_ops pxav3_sdhci_ops = {
+static const struct sdhci_ops pxav3_sdhci_ops = {
 	.platform_reset_exit = pxav3_set_private_registers,
 	.set_uhs_signaling = pxav3_set_uhs_signaling,
 	.platform_send_init_74_clocks = pxav3_gen_init_74_clocks,
 	.get_max_clock = sdhci_pltfm_clk_get_max_clock,
 };
 
+static struct sdhci_pltfm_data sdhci_pxav3_pdata = {
+	.quirks = SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK
+		| SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC
+		| SDHCI_QUIRK_32BIT_ADMA_SIZE
+		| SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN,
+	.ops = &pxav3_sdhci_ops,
+};
+
 #ifdef CONFIG_OF
 static const struct of_device_id sdhci_pxav3_of_match[] = {
 	{
@@ -187,29 +195,16 @@ static struct sdhci_pxa_platdata *pxav3_get_mmc_pdata(struct device *dev)
 {
 	struct sdhci_pxa_platdata *pdata;
 	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)
 		return NULL;
 
-	if (of_find_property(np, "non-removable", NULL))
-		pdata->flags |= PXA_FLAG_CARD_PERMANENT;
-
-	of_property_read_u32(np, "bus-width", &bus_width);
-	if (bus_width == 8)
-		pdata->flags |= PXA_FLAG_SD_8_BIT_CAPABLE_SLOT;
-
 	of_property_read_u32(np, "mrvl,clk-delay-cycles", &clk_delay_cycles);
 	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
@@ -235,7 +230,7 @@ static int sdhci_pxav3_probe(struct platform_device *pdev)
 	if (!pxa)
 		return -ENOMEM;
 
-	host = sdhci_pltfm_init(pdev, NULL);
+	host = sdhci_pltfm_init(pdev, &sdhci_pxav3_pdata);
 	if (IS_ERR(host)) {
 		kfree(pxa);
 		return PTR_ERR(host);
@@ -252,24 +247,18 @@ static int sdhci_pxav3_probe(struct platform_device *pdev)
 	pltfm_host->clk = clk;
 	clk_prepare_enable(clk);
 
-	host->quirks = SDHCI_QUIRK_BROKEN_TIMEOUT_VAL
-		| SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC
-		| SDHCI_QUIRK_32BIT_ADMA_SIZE
-		| SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN;
-
 	/* enable 1/8V DDR capable */
 	host->mmc->caps |= MMC_CAP_1_8V_DDR;
 
 	match = of_match_device(of_match_ptr(sdhci_pxav3_of_match), &pdev->dev);
-	if (match)
+	if (match) {
+		mmc_of_parse(host->mmc);
+		sdhci_get_of_property(pdev);
 		pdata = pxav3_get_mmc_pdata(dev);
-
-	if (pdata) {
-		if (pdata->flags & PXA_FLAG_CARD_PERMANENT) {
-			/* on-chip device */
-			host->quirks |= SDHCI_QUIRK_BROKEN_CARD_DETECTION;
+	} else if (pdata) {
+		/* on-chip device */
+		if (pdata->flags & PXA_FLAG_CARD_PERMANENT)
 			host->mmc->caps |= MMC_CAP_NONREMOVABLE;
-		}
 
 		/* If slot design supports 8 bit data, indicate this to MMC. */
 		if (pdata->flags & PXA_FLAG_SD_8_BIT_CAPABLE_SLOT)
@@ -296,10 +285,6 @@ static int sdhci_pxav3_probe(struct platform_device *pdev)
 		}
 	}
 
-	host->ops = &pxav3_sdhci_ops;
-
-	sdhci_get_of_property(pdev);
-
 	pm_runtime_set_active(&pdev->dev);
 	pm_runtime_enable(&pdev->dev);
 	pm_runtime_set_autosuspend_delay(&pdev->dev, PXAV3_RPM_DELAY_MS);
@@ -317,7 +302,7 @@ static int sdhci_pxav3_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, host);
 
-	if (pdata->pm_caps & MMC_PM_KEEP_POWER) {
+	if (host->mmc->pm_caps & MMC_PM_KEEP_POWER) {
 		device_init_wakeup(&pdev->dev, 1);
 		host->mmc->pm_flags |= MMC_PM_WAKE_SDIO_IRQ;
 	} else {
diff --git a/drivers/mmc/host/sdhci-s3c-regs.h b/drivers/mmc/host/sdhci-s3c-regs.h
new file mode 100644
index 000000000000..e34049ad44cc
--- /dev/null
+++ b/drivers/mmc/host/sdhci-s3c-regs.h
@@ -0,0 +1,87 @@
+/* linux/arch/arm/plat-s3c/include/plat/regs-sdhci.h
+ *
+ * Copyright 2008 Openmoko, Inc.
+ * Copyright 2008 Simtec Electronics
+ *	http://armlinux.simtec.co.uk/
+ *	Ben Dooks <ben@simtec.co.uk>
+ *
+ * S3C Platform - SDHCI (HSMMC) register definitions
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#ifndef __PLAT_S3C_SDHCI_REGS_H
+#define __PLAT_S3C_SDHCI_REGS_H __FILE__
+
+#define S3C_SDHCI_CONTROL2			(0x80)
+#define S3C_SDHCI_CONTROL3			(0x84)
+#define S3C64XX_SDHCI_CONTROL4			(0x8C)
+
+#define S3C64XX_SDHCI_CTRL2_ENSTAASYNCCLR	(1 << 31)
+#define S3C64XX_SDHCI_CTRL2_ENCMDCNFMSK		(1 << 30)
+#define S3C_SDHCI_CTRL2_CDINVRXD3		(1 << 29)
+#define S3C_SDHCI_CTRL2_SLCARDOUT		(1 << 28)
+
+#define S3C_SDHCI_CTRL2_FLTCLKSEL_MASK		(0xf << 24)
+#define S3C_SDHCI_CTRL2_FLTCLKSEL_SHIFT		(24)
+#define S3C_SDHCI_CTRL2_FLTCLKSEL(_x)		((_x) << 24)
+
+#define S3C_SDHCI_CTRL2_LVLDAT_MASK		(0xff << 16)
+#define S3C_SDHCI_CTRL2_LVLDAT_SHIFT		(16)
+#define S3C_SDHCI_CTRL2_LVLDAT(_x)		((_x) << 16)
+
+#define S3C_SDHCI_CTRL2_ENFBCLKTX		(1 << 15)
+#define S3C_SDHCI_CTRL2_ENFBCLKRX		(1 << 14)
+#define S3C_SDHCI_CTRL2_SDCDSEL			(1 << 13)
+#define S3C_SDHCI_CTRL2_SDSIGPC			(1 << 12)
+#define S3C_SDHCI_CTRL2_ENBUSYCHKTXSTART	(1 << 11)
+
+#define S3C_SDHCI_CTRL2_DFCNT_MASK		(0x3 << 9)
+#define S3C_SDHCI_CTRL2_DFCNT_SHIFT		(9)
+#define S3C_SDHCI_CTRL2_DFCNT_NONE		(0x0 << 9)
+#define S3C_SDHCI_CTRL2_DFCNT_4SDCLK		(0x1 << 9)
+#define S3C_SDHCI_CTRL2_DFCNT_16SDCLK		(0x2 << 9)
+#define S3C_SDHCI_CTRL2_DFCNT_64SDCLK		(0x3 << 9)
+
+#define S3C_SDHCI_CTRL2_ENCLKOUTHOLD		(1 << 8)
+#define S3C_SDHCI_CTRL2_RWAITMODE		(1 << 7)
+#define S3C_SDHCI_CTRL2_DISBUFRD		(1 << 6)
+#define S3C_SDHCI_CTRL2_SELBASECLK_MASK		(0x3 << 4)
+#define S3C_SDHCI_CTRL2_SELBASECLK_SHIFT	(4)
+#define S3C_SDHCI_CTRL2_PWRSYNC			(1 << 3)
+#define S3C_SDHCI_CTRL2_ENCLKOUTMSKCON		(1 << 1)
+#define S3C_SDHCI_CTRL2_HWINITFIN		(1 << 0)
+
+#define S3C_SDHCI_CTRL3_FCSEL3			(1 << 31)
+#define S3C_SDHCI_CTRL3_FCSEL2			(1 << 23)
+#define S3C_SDHCI_CTRL3_FCSEL1			(1 << 15)
+#define S3C_SDHCI_CTRL3_FCSEL0			(1 << 7)
+
+#define S3C_SDHCI_CTRL3_FIA3_MASK		(0x7f << 24)
+#define S3C_SDHCI_CTRL3_FIA3_SHIFT		(24)
+#define S3C_SDHCI_CTRL3_FIA3(_x)		((_x) << 24)
+
+#define S3C_SDHCI_CTRL3_FIA2_MASK		(0x7f << 16)
+#define S3C_SDHCI_CTRL3_FIA2_SHIFT		(16)
+#define S3C_SDHCI_CTRL3_FIA2(_x)		((_x) << 16)
+
+#define S3C_SDHCI_CTRL3_FIA1_MASK		(0x7f << 8)
+#define S3C_SDHCI_CTRL3_FIA1_SHIFT		(8)
+#define S3C_SDHCI_CTRL3_FIA1(_x)		((_x) << 8)
+
+#define S3C_SDHCI_CTRL3_FIA0_MASK		(0x7f << 0)
+#define S3C_SDHCI_CTRL3_FIA0_SHIFT		(0)
+#define S3C_SDHCI_CTRL3_FIA0(_x)		((_x) << 0)
+
+#define S3C64XX_SDHCI_CONTROL4_DRIVE_MASK	(0x3 << 16)
+#define S3C64XX_SDHCI_CONTROL4_DRIVE_SHIFT	(16)
+#define S3C64XX_SDHCI_CONTROL4_DRIVE_2mA	(0x0 << 16)
+#define S3C64XX_SDHCI_CONTROL4_DRIVE_4mA	(0x1 << 16)
+#define S3C64XX_SDHCI_CONTROL4_DRIVE_7mA	(0x2 << 16)
+#define S3C64XX_SDHCI_CONTROL4_DRIVE_9mA	(0x3 << 16)
+
+#define S3C64XX_SDHCI_CONTROL4_BUSY		(1)
+
+#endif /* __PLAT_S3C_SDHCI_REGS_H */
diff --git a/drivers/mmc/host/sdhci-s3c.c b/drivers/mmc/host/sdhci-s3c.c
index 7363efe72287..c6f6246a4933 100644
--- a/drivers/mmc/host/sdhci-s3c.c
+++ b/drivers/mmc/host/sdhci-s3c.c
@@ -15,6 +15,7 @@
 #include <linux/delay.h>
 #include <linux/dma-mapping.h>
 #include <linux/platform_device.h>
+#include <linux/platform_data/mmc-sdhci-s3c.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
 #include <linux/io.h>
@@ -24,13 +25,10 @@
 #include <linux/of_gpio.h>
 #include <linux/pm.h>
 #include <linux/pm_runtime.h>
-#include <linux/pinctrl/consumer.h>
 
 #include <linux/mmc/host.h>
 
-#include <plat/sdhci.h>
-#include <plat/regs-sdhci.h>
-
+#include "sdhci-s3c-regs.h"
 #include "sdhci.h"
 
 #define MAX_BUS_CLK	(4)
@@ -45,7 +43,6 @@
  * @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.
  */
@@ -57,8 +54,6 @@ struct sdhci_s3c {
 	unsigned int		cur_clk;
 	int			ext_cd_irq;
 	int			ext_cd_gpio;
-	int			*gpios;
-	struct pinctrl          *pctrl;
 
 	struct clk		*clk_io;
 	struct clk		*clk_bus[MAX_BUS_CLK];
@@ -447,88 +442,39 @@ static int sdhci_s3c_parse_dt(struct device *dev,
 	struct device_node *node = dev->of_node;
 	struct sdhci_s3c *ourhost = to_s3c(host);
 	u32 max_width;
-	int gpio, cnt, ret;
+	int gpio;
 
 	/* 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", NULL)) {
 		pdata->cd_type = S3C_SDHCI_CD_NONE;
-		goto setup_bus;
+		return 0;
 	}
 
 	if (of_get_property(node, "non-removable", NULL)) {
 		pdata->cd_type = S3C_SDHCI_CD_PERMANENT;
-		goto setup_bus;
+		return 0;
 	}
 
 	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;
-	}
-
-	/* assuming internal card detect that will be configured by pinctrl */
-	pdata->cd_type = S3C_SDHCI_CD_INTERNAL;
-	goto setup_bus;
-
- 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 = devm_gpio_request(dev, gpio, "sdhci-cd");
-		if (ret) {
-			dev_err(dev, "card detect gpio request failed\n");
-			return -EINVAL;
-		}
-		ourhost->ext_cd_gpio = gpio;
-	}
-
- setup_bus:
-	if (!IS_ERR(ourhost->pctrl))
 		return 0;
-
-	/* 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);
-			return -EINVAL;
-		}
-		ourhost->gpios[cnt] = gpio;
-	}
-
-	for (cnt = 0; cnt < NUM_GPIOS(pdata->max_width); cnt++) {
-		ret = devm_gpio_request(dev, ourhost->gpios[cnt], "sdhci-gpio");
-		if (ret) {
-			dev_err(dev, "gpio[%d] request failed\n", cnt);
-			return -EINVAL;
-		}
+	} else if (gpio != -ENOENT) {
+		dev_err(dev, "invalid card detect gpio specified\n");
+		return -EINVAL;
 	}
 
+	/* assuming internal card detect that will be configured by pinctrl */
+	pdata->cd_type = S3C_SDHCI_CD_INTERNAL;
 	return 0;
 }
 #else
@@ -589,8 +535,6 @@ static int sdhci_s3c_probe(struct platform_device *pdev)
 		goto err_pdata_io_clk;
 	}
 
-	sc->pctrl = devm_pinctrl_get_select_default(&pdev->dev);
-
 	if (pdev->dev.of_node) {
 		ret = sdhci_s3c_parse_dt(&pdev->dev, host, pdata);
 		if (ret)
@@ -608,7 +552,7 @@ static int sdhci_s3c_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, host);
 
-	sc->clk_io = clk_get(dev, "hsmmc");
+	sc->clk_io = devm_clk_get(dev, "hsmmc");
 	if (IS_ERR(sc->clk_io)) {
 		dev_err(dev, "failed to get io clock\n");
 		ret = PTR_ERR(sc->clk_io);
@@ -623,7 +567,7 @@ static int sdhci_s3c_probe(struct platform_device *pdev)
 		char name[14];
 
 		snprintf(name, 14, "mmc_busclk.%d", ptr);
-		clk = clk_get(dev, name);
+		clk = devm_clk_get(dev, name);
 		if (IS_ERR(clk))
 			continue;
 
@@ -764,15 +708,9 @@ static int sdhci_s3c_probe(struct platform_device *pdev)
 #ifndef CONFIG_PM_RUNTIME
 	clk_disable_unprepare(sc->clk_bus[sc->cur_clk]);
 #endif
-	for (ptr = 0; ptr < MAX_BUS_CLK; ptr++) {
-		if (sc->clk_bus[ptr]) {
-			clk_put(sc->clk_bus[ptr]);
-		}
-	}
 
  err_no_busclks:
 	clk_disable_unprepare(sc->clk_io);
-	clk_put(sc->clk_io);
 
  err_pdata_io_clk:
 	sdhci_free_host(host);
@@ -785,7 +723,6 @@ static int sdhci_s3c_remove(struct platform_device *pdev)
 	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)
 		pdata->ext_cd_cleanup(&sdhci_s3c_notify_change);
@@ -805,13 +742,7 @@ static int sdhci_s3c_remove(struct platform_device *pdev)
 #ifndef CONFIG_PM_RUNTIME
 	clk_disable_unprepare(sc->clk_bus[sc->cur_clk]);
 #endif
-	for (ptr = 0; ptr < MAX_BUS_CLK; ptr++) {
-		if (sc->clk_bus[ptr]) {
-			clk_put(sc->clk_bus[ptr]);
-		}
-	}
 	clk_disable_unprepare(sc->clk_io);
-	clk_put(sc->clk_io);
 
 	sdhci_free_host(host);
 	platform_set_drvdata(pdev, NULL);
diff --git a/drivers/mmc/host/sdhci-sirf.c b/drivers/mmc/host/sdhci-sirf.c
new file mode 100644
index 000000000000..09805af0526d
--- /dev/null
+++ b/drivers/mmc/host/sdhci-sirf.c
@@ -0,0 +1,193 @@
+/*
+ * SDHCI support for SiRF primaII and marco SoCs
+ *
+ * Copyright (c) 2011 Cambridge Silicon Radio Limited, a CSR plc group company.
+ *
+ * Licensed under GPLv2 or later.
+ */
+
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/mmc/host.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/mmc/slot-gpio.h>
+#include <linux/pinctrl/consumer.h>
+#include "sdhci-pltfm.h"
+
+struct sdhci_sirf_priv {
+	struct clk *clk;
+	int gpio_cd;
+};
+
+static unsigned int sdhci_sirf_get_max_clk(struct sdhci_host *host)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct sdhci_sirf_priv *priv = pltfm_host->priv;
+	return clk_get_rate(priv->clk);
+}
+
+static struct sdhci_ops sdhci_sirf_ops = {
+	.get_max_clock	= sdhci_sirf_get_max_clk,
+};
+
+static struct sdhci_pltfm_data sdhci_sirf_pdata = {
+	.ops = &sdhci_sirf_ops,
+	.quirks = SDHCI_QUIRK_BROKEN_TIMEOUT_VAL |
+		SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK |
+		SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN |
+		SDHCI_QUIRK_INVERTED_WRITE_PROTECT |
+		SDHCI_QUIRK_DELAY_AFTER_POWER,
+};
+
+static int sdhci_sirf_probe(struct platform_device *pdev)
+{
+	struct sdhci_host *host;
+	struct sdhci_pltfm_host *pltfm_host;
+	struct sdhci_sirf_priv *priv;
+	struct pinctrl *pinctrl;
+	int ret;
+
+	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
+	if (IS_ERR(pinctrl)) {
+		dev_err(&pdev->dev, "unable to get pinmux");
+		return PTR_ERR(pinctrl);
+	}
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(struct sdhci_sirf_priv),
+		GFP_KERNEL);
+	if (!priv) {
+		dev_err(&pdev->dev, "unable to allocate private data");
+		return -ENOMEM;
+	}
+
+	priv->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(priv->clk)) {
+		dev_err(&pdev->dev, "unable to get clock");
+		return PTR_ERR(priv->clk);
+	}
+
+	if (pdev->dev.of_node) {
+		priv->gpio_cd = of_get_named_gpio(pdev->dev.of_node,
+			"cd-gpios", 0);
+	} else {
+		priv->gpio_cd = -EINVAL;
+	}
+
+	host = sdhci_pltfm_init(pdev, &sdhci_sirf_pdata);
+	if (IS_ERR(host)) {
+		ret = PTR_ERR(host);
+		goto err_sdhci_pltfm_init;
+	}
+
+	pltfm_host = sdhci_priv(host);
+	pltfm_host->priv = priv;
+
+	sdhci_get_of_property(pdev);
+
+	clk_prepare_enable(priv->clk);
+
+	ret = sdhci_add_host(host);
+	if (ret)
+		goto err_sdhci_add;
+
+	/*
+	 * We must request the IRQ after sdhci_add_host(), as the tasklet only
+	 * gets setup in sdhci_add_host() and we oops.
+	 */
+	if (gpio_is_valid(priv->gpio_cd)) {
+		ret = mmc_gpio_request_cd(host->mmc, priv->gpio_cd);
+		if (ret) {
+			dev_err(&pdev->dev, "card detect irq request failed: %d\n",
+				ret);
+			goto err_request_cd;
+		}
+	}
+
+	return 0;
+
+err_request_cd:
+	sdhci_remove_host(host, 0);
+err_sdhci_add:
+	clk_disable_unprepare(priv->clk);
+	sdhci_pltfm_free(pdev);
+err_sdhci_pltfm_init:
+	return ret;
+}
+
+static int sdhci_sirf_remove(struct platform_device *pdev)
+{
+	struct sdhci_host *host = platform_get_drvdata(pdev);
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct sdhci_sirf_priv *priv = pltfm_host->priv;
+
+	sdhci_pltfm_unregister(pdev);
+
+	if (gpio_is_valid(priv->gpio_cd))
+		mmc_gpio_free_cd(host->mmc);
+
+	clk_disable_unprepare(priv->clk);
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int sdhci_sirf_suspend(struct device *dev)
+{
+	struct sdhci_host *host = dev_get_drvdata(dev);
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct sdhci_sirf_priv *priv = pltfm_host->priv;
+	int ret;
+
+	ret = sdhci_suspend_host(host);
+	if (ret)
+		return ret;
+
+	clk_disable(priv->clk);
+
+	return 0;
+}
+
+static int sdhci_sirf_resume(struct device *dev)
+{
+	struct sdhci_host *host = dev_get_drvdata(dev);
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct sdhci_sirf_priv *priv = pltfm_host->priv;
+	int ret;
+
+	ret = clk_enable(priv->clk);
+	if (ret) {
+		dev_dbg(dev, "Resume: Error enabling clock\n");
+		return ret;
+	}
+
+	return sdhci_resume_host(host);
+}
+
+static SIMPLE_DEV_PM_OPS(sdhci_sirf_pm_ops, sdhci_sirf_suspend, sdhci_sirf_resume);
+#endif
+
+static const struct of_device_id sdhci_sirf_of_match[] = {
+	{ .compatible = "sirf,prima2-sdhc" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, sdhci_sirf_of_match);
+
+static struct platform_driver sdhci_sirf_driver = {
+	.driver		= {
+		.name	= "sdhci-sirf",
+		.owner	= THIS_MODULE,
+		.of_match_table = sdhci_sirf_of_match,
+#ifdef CONFIG_PM_SLEEP
+		.pm	= &sdhci_sirf_pm_ops,
+#endif
+	},
+	.probe		= sdhci_sirf_probe,
+	.remove		= sdhci_sirf_remove,
+};
+
+module_platform_driver(sdhci_sirf_driver);
+
+MODULE_DESCRIPTION("SDHCI driver for SiRFprimaII/SiRFmarco");
+MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mmc/host/sdhci-spear.c b/drivers/mmc/host/sdhci-spear.c
index c6ece0bd03b3..7ae5b3ae7bad 100644
--- a/drivers/mmc/host/sdhci-spear.c
+++ b/drivers/mmc/host/sdhci-spear.c
@@ -36,7 +36,7 @@ struct spear_sdhci {
 };
 
 /* sdhci ops */
-static struct sdhci_ops sdhci_pltfm_ops = {
+static const struct sdhci_ops sdhci_pltfm_ops = {
 	/* Nothing to do for now. */
 };
 
@@ -291,7 +291,7 @@ static int sdhci_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int sdhci_suspend(struct device *dev)
 {
 	struct sdhci_host *host = dev_get_drvdata(dev);
diff --git a/drivers/mmc/host/sdhci-tegra.c b/drivers/mmc/host/sdhci-tegra.c
index 08b06e9a3a21..e0dba74cff98 100644
--- a/drivers/mmc/host/sdhci-tegra.c
+++ b/drivers/mmc/host/sdhci-tegra.c
@@ -24,6 +24,7 @@
 #include <linux/gpio.h>
 #include <linux/mmc/card.h>
 #include <linux/mmc/host.h>
+#include <linux/mmc/slot-gpio.h>
 
 #include <asm/gpio.h>
 
@@ -38,16 +39,13 @@
 #define NVQUIRK_ENABLE_SDHCI_SPEC_300	BIT(2)
 
 struct sdhci_tegra_soc_data {
-	struct sdhci_pltfm_data *pdata;
+	const struct sdhci_pltfm_data *pdata;
 	u32 nvquirks;
 };
 
 struct sdhci_tegra {
 	const struct sdhci_tegra_soc_data *soc_data;
-	int cd_gpio;
-	int wp_gpio;
 	int power_gpio;
-	int is_8bit;
 };
 
 static u32 tegra_sdhci_readl(struct sdhci_host *host, int reg)
@@ -107,23 +105,9 @@ static void tegra_sdhci_writel(struct sdhci_host *host, u32 val, int reg)
 
 static unsigned int tegra_sdhci_get_ro(struct sdhci_host *host)
 {
-	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
-	struct sdhci_tegra *tegra_host = pltfm_host->priv;
-
-	if (!gpio_is_valid(tegra_host->wp_gpio))
-		return -1;
-
-	return gpio_get_value(tegra_host->wp_gpio);
+	return mmc_gpio_get_ro(host->mmc);
 }
 
-static irqreturn_t carddetect_irq(int irq, void *data)
-{
-	struct sdhci_host *sdhost = (struct sdhci_host *)data;
-
-	tasklet_schedule(&sdhost->card_tasklet);
-	return IRQ_HANDLED;
-};
-
 static void tegra_sdhci_reset_exit(struct sdhci_host *host, u8 mask)
 {
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
@@ -145,12 +129,11 @@ static void tegra_sdhci_reset_exit(struct sdhci_host *host, u8 mask)
 
 static int tegra_sdhci_buswidth(struct sdhci_host *host, int bus_width)
 {
-	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
-	struct sdhci_tegra *tegra_host = pltfm_host->priv;
 	u32 ctrl;
 
 	ctrl = sdhci_readb(host, SDHCI_HOST_CONTROL);
-	if (tegra_host->is_8bit && bus_width == MMC_BUS_WIDTH_8) {
+	if ((host->mmc->caps & MMC_CAP_8_BIT_DATA) &&
+	    (bus_width == MMC_BUS_WIDTH_8)) {
 		ctrl &= ~SDHCI_CTRL_4BITBUS;
 		ctrl |= SDHCI_CTRL_8BITBUS;
 	} else {
@@ -164,7 +147,7 @@ static int tegra_sdhci_buswidth(struct sdhci_host *host, int bus_width)
 	return 0;
 }
 
-static struct sdhci_ops tegra_sdhci_ops = {
+static const struct sdhci_ops tegra_sdhci_ops = {
 	.get_ro     = tegra_sdhci_get_ro,
 	.read_l     = tegra_sdhci_readl,
 	.read_w     = tegra_sdhci_readw,
@@ -173,8 +156,7 @@ static struct sdhci_ops tegra_sdhci_ops = {
 	.platform_reset_exit = tegra_sdhci_reset_exit,
 };
 
-#ifdef CONFIG_ARCH_TEGRA_2x_SOC
-static struct sdhci_pltfm_data sdhci_tegra20_pdata = {
+static const struct sdhci_pltfm_data sdhci_tegra20_pdata = {
 	.quirks = SDHCI_QUIRK_BROKEN_TIMEOUT_VAL |
 		  SDHCI_QUIRK_SINGLE_POWER_WRITE |
 		  SDHCI_QUIRK_NO_HISPD_BIT |
@@ -187,10 +169,8 @@ static struct sdhci_tegra_soc_data soc_data_tegra20 = {
 	.nvquirks = NVQUIRK_FORCE_SDHCI_SPEC_200 |
 		    NVQUIRK_ENABLE_BLOCK_GAP_DET,
 };
-#endif
 
-#ifdef CONFIG_ARCH_TEGRA_3x_SOC
-static struct sdhci_pltfm_data sdhci_tegra30_pdata = {
+static const struct sdhci_pltfm_data sdhci_tegra30_pdata = {
 	.quirks = SDHCI_QUIRK_BROKEN_TIMEOUT_VAL |
 		  SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK |
 		  SDHCI_QUIRK_SINGLE_POWER_WRITE |
@@ -203,32 +183,37 @@ static struct sdhci_tegra_soc_data soc_data_tegra30 = {
 	.pdata = &sdhci_tegra30_pdata,
 	.nvquirks = NVQUIRK_ENABLE_SDHCI_SPEC_300,
 };
-#endif
+
+static const struct sdhci_pltfm_data sdhci_tegra114_pdata = {
+	.quirks = SDHCI_QUIRK_BROKEN_TIMEOUT_VAL |
+		  SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK |
+		  SDHCI_QUIRK_SINGLE_POWER_WRITE |
+		  SDHCI_QUIRK_NO_HISPD_BIT |
+		  SDHCI_QUIRK_BROKEN_ADMA_ZEROLEN_DESC,
+	.ops  = &tegra_sdhci_ops,
+};
+
+static struct sdhci_tegra_soc_data soc_data_tegra114 = {
+	.pdata = &sdhci_tegra114_pdata,
+};
 
 static const struct of_device_id sdhci_tegra_dt_match[] = {
-#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+	{ .compatible = "nvidia,tegra114-sdhci", .data = &soc_data_tegra114 },
 	{ .compatible = "nvidia,tegra30-sdhci", .data = &soc_data_tegra30 },
-#endif
-#ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	{ .compatible = "nvidia,tegra20-sdhci", .data = &soc_data_tegra20 },
-#endif
 	{}
 };
-MODULE_DEVICE_TABLE(of, sdhci_dt_ids);
+MODULE_DEVICE_TABLE(of, sdhci_tegra_dt_match);
 
-static void sdhci_tegra_parse_dt(struct device *dev,
-					struct sdhci_tegra *tegra_host)
+static void sdhci_tegra_parse_dt(struct device *dev)
 {
 	struct device_node *np = dev->of_node;
-	u32 bus_width;
+	struct sdhci_host *host = dev_get_drvdata(dev);
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct sdhci_tegra *tegra_host = pltfm_host->priv;
 
-	tegra_host->cd_gpio = of_get_named_gpio(np, "cd-gpios", 0);
-	tegra_host->wp_gpio = of_get_named_gpio(np, "wp-gpios", 0);
 	tegra_host->power_gpio = of_get_named_gpio(np, "power-gpios", 0);
-
-	if (of_property_read_u32(np, "bus-width", &bus_width) == 0 &&
-	    bus_width == 8)
-		tegra_host->is_8bit = 1;
+	mmc_of_parse(host->mmc);
 }
 
 static int sdhci_tegra_probe(struct platform_device *pdev)
@@ -260,7 +245,7 @@ static int sdhci_tegra_probe(struct platform_device *pdev)
 	tegra_host->soc_data = soc_data;
 	pltfm_host->priv = tegra_host;
 
-	sdhci_tegra_parse_dt(&pdev->dev, tegra_host);
+	sdhci_tegra_parse_dt(&pdev->dev);
 
 	if (gpio_is_valid(tegra_host->power_gpio)) {
 		rc = gpio_request(tegra_host->power_gpio, "sdhci_power");
@@ -272,37 +257,6 @@ static int sdhci_tegra_probe(struct platform_device *pdev)
 		gpio_direction_output(tegra_host->power_gpio, 1);
 	}
 
-	if (gpio_is_valid(tegra_host->cd_gpio)) {
-		rc = gpio_request(tegra_host->cd_gpio, "sdhci_cd");
-		if (rc) {
-			dev_err(mmc_dev(host->mmc),
-				"failed to allocate cd gpio\n");
-			goto err_cd_req;
-		}
-		gpio_direction_input(tegra_host->cd_gpio);
-
-		rc = request_irq(gpio_to_irq(tegra_host->cd_gpio),
-				 carddetect_irq,
-				 IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
-				 mmc_hostname(host->mmc), host);
-
-		if (rc)	{
-			dev_err(mmc_dev(host->mmc), "request irq error\n");
-			goto err_cd_irq_req;
-		}
-
-	}
-
-	if (gpio_is_valid(tegra_host->wp_gpio)) {
-		rc = gpio_request(tegra_host->wp_gpio, "sdhci_wp");
-		if (rc) {
-			dev_err(mmc_dev(host->mmc),
-				"failed to allocate wp gpio\n");
-			goto err_wp_req;
-		}
-		gpio_direction_input(tegra_host->wp_gpio);
-	}
-
 	clk = clk_get(mmc_dev(host->mmc), NULL);
 	if (IS_ERR(clk)) {
 		dev_err(mmc_dev(host->mmc), "clk err\n");
@@ -312,9 +266,6 @@ static int sdhci_tegra_probe(struct platform_device *pdev)
 	clk_prepare_enable(clk);
 	pltfm_host->clk = clk;
 
-	if (tegra_host->is_8bit)
-		host->mmc->caps |= MMC_CAP_8_BIT_DATA;
-
 	rc = sdhci_add_host(host);
 	if (rc)
 		goto err_add_host;
@@ -325,15 +276,6 @@ err_add_host:
 	clk_disable_unprepare(pltfm_host->clk);
 	clk_put(pltfm_host->clk);
 err_clk_get:
-	if (gpio_is_valid(tegra_host->wp_gpio))
-		gpio_free(tegra_host->wp_gpio);
-err_wp_req:
-	if (gpio_is_valid(tegra_host->cd_gpio))
-		free_irq(gpio_to_irq(tegra_host->cd_gpio), host);
-err_cd_irq_req:
-	if (gpio_is_valid(tegra_host->cd_gpio))
-		gpio_free(tegra_host->cd_gpio);
-err_cd_req:
 	if (gpio_is_valid(tegra_host->power_gpio))
 		gpio_free(tegra_host->power_gpio);
 err_power_req:
@@ -351,14 +293,6 @@ static int sdhci_tegra_remove(struct platform_device *pdev)
 
 	sdhci_remove_host(host, dead);
 
-	if (gpio_is_valid(tegra_host->wp_gpio))
-		gpio_free(tegra_host->wp_gpio);
-
-	if (gpio_is_valid(tegra_host->cd_gpio)) {
-		free_irq(gpio_to_irq(tegra_host->cd_gpio), host);
-		gpio_free(tegra_host->cd_gpio);
-	}
-
 	if (gpio_is_valid(tegra_host->power_gpio))
 		gpio_free(tegra_host->power_gpio);
 
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index 51bbba486f38..2ea429c27714 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -1581,6 +1581,37 @@ static void sdhci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	sdhci_runtime_pm_put(host);
 }
 
+static int sdhci_do_get_cd(struct sdhci_host *host)
+{
+	int gpio_cd = mmc_gpio_get_cd(host->mmc);
+
+	if (host->flags & SDHCI_DEVICE_DEAD)
+		return 0;
+
+	/* If polling/nonremovable, assume that the card is always present. */
+	if ((host->quirks & SDHCI_QUIRK_BROKEN_CARD_DETECTION) ||
+	    (host->mmc->caps & MMC_CAP_NONREMOVABLE))
+		return 1;
+
+	/* Try slot gpio detect */
+	if (!IS_ERR_VALUE(gpio_cd))
+		return !!gpio_cd;
+
+	/* Host native card detect */
+	return !!(sdhci_readl(host, SDHCI_PRESENT_STATE) & SDHCI_CARD_PRESENT);
+}
+
+static int sdhci_get_cd(struct mmc_host *mmc)
+{
+	struct sdhci_host *host = mmc_priv(mmc);
+	int ret;
+
+	sdhci_runtime_pm_get(host);
+	ret = sdhci_do_get_cd(host);
+	sdhci_runtime_pm_put(host);
+	return ret;
+}
+
 static int sdhci_check_ro(struct sdhci_host *host)
 {
 	unsigned long flags;
@@ -2038,6 +2069,7 @@ static void sdhci_card_event(struct mmc_host *mmc)
 static const struct mmc_host_ops sdhci_ops = {
 	.request	= sdhci_request,
 	.set_ios	= sdhci_set_ios,
+	.get_cd		= sdhci_get_cd,
 	.get_ro		= sdhci_get_ro,
 	.hw_reset	= sdhci_hw_reset,
 	.enable_sdio_irq = sdhci_enable_sdio_irq,
@@ -2907,12 +2939,17 @@ int sdhci_add_host(struct sdhci_host *host)
 			host->vqmmc = NULL;
 		}
 	} else {
-		regulator_enable(host->vqmmc);
+		ret = regulator_enable(host->vqmmc);
 		if (!regulator_is_supported_voltage(host->vqmmc, 1700000,
 			1950000))
 			caps[1] &= ~(SDHCI_SUPPORT_SDR104 |
 					SDHCI_SUPPORT_SDR50 |
 					SDHCI_SUPPORT_DDR50);
+		if (ret) {
+			pr_warn("%s: Failed to enable vqmmc regulator: %d\n",
+				mmc_hostname(mmc), ret);
+			host->vqmmc = NULL;
+		}
 	}
 
 	if (host->quirks2 & SDHCI_QUIRK2_NO_1_8_V)
diff --git a/drivers/mmc/host/wmt-sdmmc.c b/drivers/mmc/host/wmt-sdmmc.c
index c6d001509e5a..442f5766ffca 100644
--- a/drivers/mmc/host/wmt-sdmmc.c
+++ b/drivers/mmc/host/wmt-sdmmc.c
@@ -348,13 +348,11 @@ static void wmt_complete_data_request(struct wmt_mci_priv *priv)
 
 static irqreturn_t wmt_mci_dma_isr(int irq_num, void *data)
 {
-	struct mmc_host *mmc;
 	struct wmt_mci_priv *priv;
 
 	int status;
 
 	priv = (struct wmt_mci_priv *)data;
-	mmc = priv->mmc;
 
 	status = readl(priv->sdmmc_base + SDDMA_CCR) & 0x0F;
 
@@ -925,7 +923,7 @@ static int wmt_mci_remove(struct platform_device *pdev)
 	clk_put(priv->clk_sdmmc);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	release_mem_region(res->start, res->end - res->start + 1);
+	release_mem_region(res->start, resource_size(res));
 
 	mmc_free_host(mmc);
 
diff --git a/drivers/mtd/Kconfig b/drivers/mtd/Kconfig
index 557bec599f4f..5fab4e6e8301 100644
--- a/drivers/mtd/Kconfig
+++ b/drivers/mtd/Kconfig
@@ -157,19 +157,6 @@ config MTD_BCM47XX_PARTS
 
 comment "User Modules And Translation Layers"
 
-config MTD_CHAR
-	tristate "Direct char device access to MTD devices"
-	help
-	  This provides a character device for each MTD device present in
-	  the system, allowing the user to read and write directly to the
-	  memory chips, and also use ioctl() to obtain information about
-	  the device, or to erase parts of it.
-
-config HAVE_MTD_OTP
-	bool
-	help
-	  Enable access to OTP regions using MTD_CHAR.
-
 config MTD_BLKDEVS
 	tristate "Common interface to block layer for MTD 'translation layers'"
 	depends on BLOCK
diff --git a/drivers/mtd/Makefile b/drivers/mtd/Makefile
index 18a38e55b2f0..4cfb31e6c966 100644
--- a/drivers/mtd/Makefile
+++ b/drivers/mtd/Makefile
@@ -4,7 +4,7 @@
 
 # Core functionality.
 obj-$(CONFIG_MTD)		+= mtd.o
-mtd-y				:= mtdcore.o mtdsuper.o mtdconcat.o mtdpart.o
+mtd-y				:= mtdcore.o mtdsuper.o mtdconcat.o mtdpart.o mtdchar.o
 
 obj-$(CONFIG_MTD_OF_PARTS)	+= ofpart.o
 obj-$(CONFIG_MTD_REDBOOT_PARTS) += redboot.o
@@ -15,7 +15,6 @@ obj-$(CONFIG_MTD_BCM63XX_PARTS)	+= bcm63xxpart.o
 obj-$(CONFIG_MTD_BCM47XX_PARTS)	+= bcm47xxpart.o
 
 # 'Users' - code which presents functionality to userspace.
-obj-$(CONFIG_MTD_CHAR)		+= mtdchar.o
 obj-$(CONFIG_MTD_BLKDEVS)	+= mtd_blkdevs.o
 obj-$(CONFIG_MTD_BLOCK)		+= mtdblock.o
 obj-$(CONFIG_MTD_BLOCK_RO)	+= mtdblock_ro.o
diff --git a/drivers/mtd/chips/Kconfig b/drivers/mtd/chips/Kconfig
index c219e3d098d9..e4696b37f3de 100644
--- a/drivers/mtd/chips/Kconfig
+++ b/drivers/mtd/chips/Kconfig
@@ -146,7 +146,6 @@ config MTD_CFI_I8
 config MTD_OTP
 	bool "Protection Registers aka one-time programmable (OTP) bits"
 	depends on MTD_CFI_ADV_OPTIONS
-	select HAVE_MTD_OTP
 	default n
 	help
 	  This enables support for reading, writing and locking so called
diff --git a/drivers/mtd/chips/gen_probe.c b/drivers/mtd/chips/gen_probe.c
index 3b9a2843c5f8..74dbb6bcf488 100644
--- a/drivers/mtd/chips/gen_probe.c
+++ b/drivers/mtd/chips/gen_probe.c
@@ -204,14 +204,16 @@ static inline struct mtd_info *cfi_cmdset_unknown(struct map_info *map,
 	struct cfi_private *cfi = map->fldrv_priv;
 	__u16 type = primary?cfi->cfiq->P_ID:cfi->cfiq->A_ID;
 #ifdef CONFIG_MODULES
-	char probename[16+sizeof(MODULE_SYMBOL_PREFIX)];
+	char probename[sizeof(VMLINUX_SYMBOL_STR(cfi_cmdset_%4.4X))];
 	cfi_cmdset_fn_t *probe_function;
 
-	sprintf(probename, MODULE_SYMBOL_PREFIX "cfi_cmdset_%4.4X", type);
+	sprintf(probename, VMLINUX_SYMBOL_STR(cfi_cmdset_%4.4X), type);
 
 	probe_function = __symbol_get(probename);
 	if (!probe_function) {
-		request_module(probename + sizeof(MODULE_SYMBOL_PREFIX) - 1);
+		char modname[sizeof("cfi_cmdset_%4.4X")];
+		sprintf(modname, "cfi_cmdset_%4.4X", type);
+		request_module(modname);
 		probe_function = __symbol_get(probename);
 	}
 
diff --git a/drivers/mtd/devices/Kconfig b/drivers/mtd/devices/Kconfig
index 12311f506ca1..2a4d55e4b362 100644
--- a/drivers/mtd/devices/Kconfig
+++ b/drivers/mtd/devices/Kconfig
@@ -71,7 +71,6 @@ config MTD_DATAFLASH_WRITE_VERIFY
 config MTD_DATAFLASH_OTP
 	bool "DataFlash OTP support (Security Register)"
 	depends on MTD_DATAFLASH
-	select HAVE_MTD_OTP
 	help
 	  Newer DataFlash chips (revisions C and D) support 128 bytes of
 	  one-time-programmable (OTP) data.  The first half may be written
@@ -205,69 +204,6 @@ config MTD_BLOCK2MTD
 
 comment "Disk-On-Chip Device Drivers"
 
-config MTD_DOC2000
-	tristate "M-Systems Disk-On-Chip 2000 and Millennium (DEPRECATED)"
-	depends on MTD_NAND
-	select MTD_DOCPROBE
-	select MTD_NAND_IDS
-	---help---
-	  This provides an MTD device driver for the M-Systems DiskOnChip
-	  2000 and Millennium devices.  Originally designed for the DiskOnChip
-	  2000, it also now includes support for the DiskOnChip Millennium.
-	  If you have problems with this driver and the DiskOnChip Millennium,
-	  you may wish to try the alternative Millennium driver below. To use
-	  the alternative driver, you will need to undefine DOC_SINGLE_DRIVER
-	  in the <file:drivers/mtd/devices/docprobe.c> source code.
-
-	  If you use this device, you probably also want to enable the NFTL
-	  'NAND Flash Translation Layer' option below, which is used to
-	  emulate a block device by using a kind of file system on the flash
-	  chips.
-
-	  NOTE: This driver is deprecated and will probably be removed soon.
-	  Please try the new DiskOnChip driver under "NAND Flash Device
-	  Drivers".
-
-config MTD_DOC2001
-	tristate "M-Systems Disk-On-Chip Millennium-only alternative driver (DEPRECATED)"
-	depends on MTD_NAND
-	select MTD_DOCPROBE
-	select MTD_NAND_IDS
-	---help---
-	  This provides an alternative MTD device driver for the M-Systems
-	  DiskOnChip Millennium devices.  Use this if you have problems with
-	  the combined DiskOnChip 2000 and Millennium driver above.  To get
-	  the DiskOnChip probe code to load and use this driver instead of
-	  the other one, you will need to undefine DOC_SINGLE_DRIVER near
-	  the beginning of <file:drivers/mtd/devices/docprobe.c>.
-
-	  If you use this device, you probably also want to enable the NFTL
-	  'NAND Flash Translation Layer' option below, which is used to
-	  emulate a block device by using a kind of file system on the flash
-	  chips.
-
-	  NOTE: This driver is deprecated and will probably be removed soon.
-	  Please try the new DiskOnChip driver under "NAND Flash Device
-	  Drivers".
-
-config MTD_DOC2001PLUS
-	tristate "M-Systems Disk-On-Chip Millennium Plus"
-	depends on MTD_NAND
-	select MTD_DOCPROBE
-	select MTD_NAND_IDS
-	---help---
-	  This provides an MTD device driver for the M-Systems DiskOnChip
-	  Millennium Plus devices.
-
-	  If you use this device, you probably also want to enable the INFTL
-	  'Inverse NAND Flash Translation Layer' option below, which is used
-	  to emulate a block device by using a kind of file system on the
-	  flash chips.
-
-	  NOTE: This driver will soon be replaced by the new DiskOnChip driver
-	  under "NAND Flash Device Drivers" (currently that driver does not
-	  support all Millennium Plus devices).
-
 config MTD_DOCG3
 	tristate "M-Systems Disk-On-Chip G3"
 	select BCH
diff --git a/drivers/mtd/devices/Makefile b/drivers/mtd/devices/Makefile
index 369a1943ca25..d83bd73096f6 100644
--- a/drivers/mtd/devices/Makefile
+++ b/drivers/mtd/devices/Makefile
@@ -2,12 +2,7 @@
 # linux/drivers/mtd/devices/Makefile
 #
 
-obj-$(CONFIG_MTD_DOC2000)	+= doc2000.o
-obj-$(CONFIG_MTD_DOC2001)	+= doc2001.o
-obj-$(CONFIG_MTD_DOC2001PLUS)	+= doc2001plus.o
 obj-$(CONFIG_MTD_DOCG3)		+= docg3.o
-obj-$(CONFIG_MTD_DOCPROBE)	+= docprobe.o
-obj-$(CONFIG_MTD_DOCECC)	+= docecc.o
 obj-$(CONFIG_MTD_SLRAM)		+= slram.o
 obj-$(CONFIG_MTD_PHRAM)		+= phram.o
 obj-$(CONFIG_MTD_PMC551)	+= pmc551.o
diff --git a/drivers/mtd/devices/bcm47xxsflash.c b/drivers/mtd/devices/bcm47xxsflash.c
index 95266285acb1..18e7761137a3 100644
--- a/drivers/mtd/devices/bcm47xxsflash.c
+++ b/drivers/mtd/devices/bcm47xxsflash.c
@@ -10,7 +10,7 @@
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Serial flash driver for BCMA bus");
 
-static const char *probes[] = { "bcm47xxpart", NULL };
+static const char * const probes[] = { "bcm47xxpart", NULL };
 
 static int bcm47xxsflash_read(struct mtd_info *mtd, loff_t from, size_t len,
 			      size_t *retlen, u_char *buf)
@@ -61,6 +61,17 @@ static int bcm47xxsflash_bcma_probe(struct platform_device *pdev)
 	}
 	sflash->priv = b47s;
 
+	b47s->bcma_cc = container_of(sflash, struct bcma_drv_cc, sflash);
+
+	switch (b47s->bcma_cc->capabilities & BCMA_CC_CAP_FLASHT) {
+	case BCMA_CC_FLASHT_STSER:
+		b47s->type = BCM47XXSFLASH_TYPE_ST;
+		break;
+	case BCMA_CC_FLASHT_ATSER:
+		b47s->type = BCM47XXSFLASH_TYPE_ATMEL;
+		break;
+	}
+
 	b47s->window = sflash->window;
 	b47s->blocksize = sflash->blocksize;
 	b47s->numblocks = sflash->numblocks;
diff --git a/drivers/mtd/devices/bcm47xxsflash.h b/drivers/mtd/devices/bcm47xxsflash.h
index ebf6f710e23c..f22f8c46dfc0 100644
--- a/drivers/mtd/devices/bcm47xxsflash.h
+++ b/drivers/mtd/devices/bcm47xxsflash.h
@@ -3,7 +3,66 @@
 
 #include <linux/mtd/mtd.h>
 
+/* Used for ST flashes only. */
+#define OPCODE_ST_WREN		0x0006		/* Write Enable */
+#define OPCODE_ST_WRDIS		0x0004		/* Write Disable */
+#define OPCODE_ST_RDSR		0x0105		/* Read Status Register */
+#define OPCODE_ST_WRSR		0x0101		/* Write Status Register */
+#define OPCODE_ST_READ		0x0303		/* Read Data Bytes */
+#define OPCODE_ST_PP		0x0302		/* Page Program */
+#define OPCODE_ST_SE		0x02d8		/* Sector Erase */
+#define OPCODE_ST_BE		0x00c7		/* Bulk Erase */
+#define OPCODE_ST_DP		0x00b9		/* Deep Power-down */
+#define OPCODE_ST_RES		0x03ab		/* Read Electronic Signature */
+#define OPCODE_ST_CSA		0x1000		/* Keep chip select asserted */
+#define OPCODE_ST_SSE		0x0220		/* Sub-sector Erase */
+
+/* Used for Atmel flashes only. */
+#define OPCODE_AT_READ				0x07e8
+#define OPCODE_AT_PAGE_READ			0x07d2
+#define OPCODE_AT_STATUS			0x01d7
+#define OPCODE_AT_BUF1_WRITE			0x0384
+#define OPCODE_AT_BUF2_WRITE			0x0387
+#define OPCODE_AT_BUF1_ERASE_PROGRAM		0x0283
+#define OPCODE_AT_BUF2_ERASE_PROGRAM		0x0286
+#define OPCODE_AT_BUF1_PROGRAM			0x0288
+#define OPCODE_AT_BUF2_PROGRAM			0x0289
+#define OPCODE_AT_PAGE_ERASE			0x0281
+#define OPCODE_AT_BLOCK_ERASE			0x0250
+#define OPCODE_AT_BUF1_WRITE_ERASE_PROGRAM	0x0382
+#define OPCODE_AT_BUF2_WRITE_ERASE_PROGRAM	0x0385
+#define OPCODE_AT_BUF1_LOAD			0x0253
+#define OPCODE_AT_BUF2_LOAD			0x0255
+#define OPCODE_AT_BUF1_COMPARE			0x0260
+#define OPCODE_AT_BUF2_COMPARE			0x0261
+#define OPCODE_AT_BUF1_REPROGRAM		0x0258
+#define OPCODE_AT_BUF2_REPROGRAM		0x0259
+
+/* Status register bits for ST flashes */
+#define SR_ST_WIP		0x01		/* Write In Progress */
+#define SR_ST_WEL		0x02		/* Write Enable Latch */
+#define SR_ST_BP_MASK		0x1c		/* Block Protect */
+#define SR_ST_BP_SHIFT		2
+#define SR_ST_SRWD		0x80		/* Status Register Write Disable */
+
+/* Status register bits for Atmel flashes */
+#define SR_AT_READY		0x80
+#define SR_AT_MISMATCH		0x40
+#define SR_AT_ID_MASK		0x38
+#define SR_AT_ID_SHIFT		3
+
+struct bcma_drv_cc;
+
+enum bcm47xxsflash_type {
+	BCM47XXSFLASH_TYPE_ATMEL,
+	BCM47XXSFLASH_TYPE_ST,
+};
+
 struct bcm47xxsflash {
+	struct bcma_drv_cc *bcma_cc;
+
+	enum bcm47xxsflash_type type;
+
 	u32 window;
 	u32 blocksize;
 	u16 numblocks;
diff --git a/drivers/mtd/devices/doc2000.c b/drivers/mtd/devices/doc2000.c
deleted file mode 100644
index a4eb8b5b85ec..000000000000
--- a/drivers/mtd/devices/doc2000.c
+++ /dev/null
@@ -1,1178 +0,0 @@
-
-/*
- * Linux driver for Disk-On-Chip 2000 and Millennium
- * (c) 1999 Machine Vision Holdings, Inc.
- * (c) 1999, 2000 David Woodhouse <dwmw2@infradead.org>
- */
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <asm/errno.h>
-#include <asm/io.h>
-#include <asm/uaccess.h>
-#include <linux/delay.h>
-#include <linux/slab.h>
-#include <linux/sched.h>
-#include <linux/init.h>
-#include <linux/types.h>
-#include <linux/bitops.h>
-#include <linux/mutex.h>
-
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/nand.h>
-#include <linux/mtd/doc2000.h>
-
-#define DOC_SUPPORT_2000
-#define DOC_SUPPORT_2000TSOP
-#define DOC_SUPPORT_MILLENNIUM
-
-#ifdef DOC_SUPPORT_2000
-#define DoC_is_2000(doc) (doc->ChipID == DOC_ChipID_Doc2k)
-#else
-#define DoC_is_2000(doc) (0)
-#endif
-
-#if defined(DOC_SUPPORT_2000TSOP) || defined(DOC_SUPPORT_MILLENNIUM)
-#define DoC_is_Millennium(doc) (doc->ChipID == DOC_ChipID_DocMil)
-#else
-#define DoC_is_Millennium(doc) (0)
-#endif
-
-/* #define ECC_DEBUG */
-
-/* I have no idea why some DoC chips can not use memcpy_from|to_io().
- * This may be due to the different revisions of the ASIC controller built-in or
- * simplily a QA/Bug issue. Who knows ?? If you have trouble, please uncomment
- * this:
- #undef USE_MEMCPY
-*/
-
-static int doc_read(struct mtd_info *mtd, loff_t from, size_t len,
-		    size_t *retlen, u_char *buf);
-static int doc_write(struct mtd_info *mtd, loff_t to, size_t len,
-		     size_t *retlen, const u_char *buf);
-static int doc_read_oob(struct mtd_info *mtd, loff_t ofs,
-			struct mtd_oob_ops *ops);
-static int doc_write_oob(struct mtd_info *mtd, loff_t ofs,
-			 struct mtd_oob_ops *ops);
-static int doc_write_oob_nolock(struct mtd_info *mtd, loff_t ofs, size_t len,
-			 size_t *retlen, const u_char *buf);
-static int doc_erase (struct mtd_info *mtd, struct erase_info *instr);
-
-static struct mtd_info *doc2klist = NULL;
-
-/* Perform the required delay cycles by reading from the appropriate register */
-static void DoC_Delay(struct DiskOnChip *doc, unsigned short cycles)
-{
-	volatile char dummy;
-	int i;
-
-	for (i = 0; i < cycles; i++) {
-		if (DoC_is_Millennium(doc))
-			dummy = ReadDOC(doc->virtadr, NOP);
-		else
-			dummy = ReadDOC(doc->virtadr, DOCStatus);
-	}
-
-}
-
-/* DOC_WaitReady: Wait for RDY line to be asserted by the flash chip */
-static int _DoC_WaitReady(struct DiskOnChip *doc)
-{
-	void __iomem *docptr = doc->virtadr;
-	unsigned long timeo = jiffies + (HZ * 10);
-
-	pr_debug("_DoC_WaitReady called for out-of-line wait\n");
-
-	/* Out-of-line routine to wait for chip response */
-	while (!(ReadDOC(docptr, CDSNControl) & CDSN_CTRL_FR_B)) {
-		/* issue 2 read from NOP register after reading from CDSNControl register
-	   	see Software Requirement 11.4 item 2. */
-		DoC_Delay(doc, 2);
-
-		if (time_after(jiffies, timeo)) {
-			pr_debug("_DoC_WaitReady timed out.\n");
-			return -EIO;
-		}
-		udelay(1);
-		cond_resched();
-	}
-
-	return 0;
-}
-
-static inline int DoC_WaitReady(struct DiskOnChip *doc)
-{
-	void __iomem *docptr = doc->virtadr;
-
-	/* This is inline, to optimise the common case, where it's ready instantly */
-	int ret = 0;
-
-	/* 4 read form NOP register should be issued in prior to the read from CDSNControl
-	   see Software Requirement 11.4 item 2. */
-	DoC_Delay(doc, 4);
-
-	if (!(ReadDOC(docptr, CDSNControl) & CDSN_CTRL_FR_B))
-		/* Call the out-of-line routine to wait */
-		ret = _DoC_WaitReady(doc);
-
-	/* issue 2 read from NOP register after reading from CDSNControl register
-	   see Software Requirement 11.4 item 2. */
-	DoC_Delay(doc, 2);
-
-	return ret;
-}
-
-/* DoC_Command: Send a flash command to the flash chip through the CDSN Slow IO register to
-   bypass the internal pipeline. Each of 4 delay cycles (read from the NOP register) is
-   required after writing to CDSN Control register, see Software Requirement 11.4 item 3. */
-
-static int DoC_Command(struct DiskOnChip *doc, unsigned char command,
-			      unsigned char xtraflags)
-{
-	void __iomem *docptr = doc->virtadr;
-
-	if (DoC_is_2000(doc))
-		xtraflags |= CDSN_CTRL_FLASH_IO;
-
-	/* Assert the CLE (Command Latch Enable) line to the flash chip */
-	WriteDOC(xtraflags | CDSN_CTRL_CLE | CDSN_CTRL_CE, docptr, CDSNControl);
-	DoC_Delay(doc, 4);	/* Software requirement 11.4.3 for Millennium */
-
-	if (DoC_is_Millennium(doc))
-		WriteDOC(command, docptr, CDSNSlowIO);
-
-	/* Send the command */
-	WriteDOC_(command, docptr, doc->ioreg);
-	if (DoC_is_Millennium(doc))
-		WriteDOC(command, docptr, WritePipeTerm);
-
-	/* Lower the CLE line */
-	WriteDOC(xtraflags | CDSN_CTRL_CE, docptr, CDSNControl);
-	DoC_Delay(doc, 4);	/* Software requirement 11.4.3 for Millennium */
-
-	/* Wait for the chip to respond - Software requirement 11.4.1 (extended for any command) */
-	return DoC_WaitReady(doc);
-}
-
-/* DoC_Address: Set the current address for the flash chip through the CDSN Slow IO register to
-   bypass the internal pipeline. Each of 4 delay cycles (read from the NOP register) is
-   required after writing to CDSN Control register, see Software Requirement 11.4 item 3. */
-
-static int DoC_Address(struct DiskOnChip *doc, int numbytes, unsigned long ofs,
-		       unsigned char xtraflags1, unsigned char xtraflags2)
-{
-	int i;
-	void __iomem *docptr = doc->virtadr;
-
-	if (DoC_is_2000(doc))
-		xtraflags1 |= CDSN_CTRL_FLASH_IO;
-
-	/* Assert the ALE (Address Latch Enable) line to the flash chip */
-	WriteDOC(xtraflags1 | CDSN_CTRL_ALE | CDSN_CTRL_CE, docptr, CDSNControl);
-
-	DoC_Delay(doc, 4);	/* Software requirement 11.4.3 for Millennium */
-
-	/* Send the address */
-	/* Devices with 256-byte page are addressed as:
-	   Column (bits 0-7), Page (bits 8-15, 16-23, 24-31)
-	   * there is no device on the market with page256
-	   and more than 24 bits.
-	   Devices with 512-byte page are addressed as:
-	   Column (bits 0-7), Page (bits 9-16, 17-24, 25-31)
-	   * 25-31 is sent only if the chip support it.
-	   * bit 8 changes the read command to be sent
-	   (NAND_CMD_READ0 or NAND_CMD_READ1).
-	 */
-
-	if (numbytes == ADDR_COLUMN || numbytes == ADDR_COLUMN_PAGE) {
-		if (DoC_is_Millennium(doc))
-			WriteDOC(ofs & 0xff, docptr, CDSNSlowIO);
-		WriteDOC_(ofs & 0xff, docptr, doc->ioreg);
-	}
-
-	if (doc->page256) {
-		ofs = ofs >> 8;
-	} else {
-		ofs = ofs >> 9;
-	}
-
-	if (numbytes == ADDR_PAGE || numbytes == ADDR_COLUMN_PAGE) {
-		for (i = 0; i < doc->pageadrlen; i++, ofs = ofs >> 8) {
-			if (DoC_is_Millennium(doc))
-				WriteDOC(ofs & 0xff, docptr, CDSNSlowIO);
-			WriteDOC_(ofs & 0xff, docptr, doc->ioreg);
-		}
-	}
-
-	if (DoC_is_Millennium(doc))
-		WriteDOC(ofs & 0xff, docptr, WritePipeTerm);
-
-	DoC_Delay(doc, 2);	/* Needed for some slow flash chips. mf. */
-
-	/* FIXME: The SlowIO's for millennium could be replaced by
-	   a single WritePipeTerm here. mf. */
-
-	/* Lower the ALE line */
-	WriteDOC(xtraflags1 | xtraflags2 | CDSN_CTRL_CE, docptr,
-		 CDSNControl);
-
-	DoC_Delay(doc, 4);	/* Software requirement 11.4.3 for Millennium */
-
-	/* Wait for the chip to respond - Software requirement 11.4.1 */
-	return DoC_WaitReady(doc);
-}
-
-/* Read a buffer from DoC, taking care of Millennium odditys */
-static void DoC_ReadBuf(struct DiskOnChip *doc, u_char * buf, int len)
-{
-	volatile int dummy;
-	int modulus = 0xffff;
-	void __iomem *docptr = doc->virtadr;
-	int i;
-
-	if (len <= 0)
-		return;
-
-	if (DoC_is_Millennium(doc)) {
-		/* Read the data via the internal pipeline through CDSN IO register,
-		   see Pipelined Read Operations 11.3 */
-		dummy = ReadDOC(docptr, ReadPipeInit);
-
-		/* Millennium should use the LastDataRead register - Pipeline Reads */
-		len--;
-
-		/* This is needed for correctly ECC calculation */
-		modulus = 0xff;
-	}
-
-	for (i = 0; i < len; i++)
-		buf[i] = ReadDOC_(docptr, doc->ioreg + (i & modulus));
-
-	if (DoC_is_Millennium(doc)) {
-		buf[i] = ReadDOC(docptr, LastDataRead);
-	}
-}
-
-/* Write a buffer to DoC, taking care of Millennium odditys */
-static void DoC_WriteBuf(struct DiskOnChip *doc, const u_char * buf, int len)
-{
-	void __iomem *docptr = doc->virtadr;
-	int i;
-
-	if (len <= 0)
-		return;
-
-	for (i = 0; i < len; i++)
-		WriteDOC_(buf[i], docptr, doc->ioreg + i);
-
-	if (DoC_is_Millennium(doc)) {
-		WriteDOC(0x00, docptr, WritePipeTerm);
-	}
-}
-
-
-/* DoC_SelectChip: Select a given flash chip within the current floor */
-
-static inline int DoC_SelectChip(struct DiskOnChip *doc, int chip)
-{
-	void __iomem *docptr = doc->virtadr;
-
-	/* Software requirement 11.4.4 before writing DeviceSelect */
-	/* Deassert the CE line to eliminate glitches on the FCE# outputs */
-	WriteDOC(CDSN_CTRL_WP, docptr, CDSNControl);
-	DoC_Delay(doc, 4);	/* Software requirement 11.4.3 for Millennium */
-
-	/* Select the individual flash chip requested */
-	WriteDOC(chip, docptr, CDSNDeviceSelect);
-	DoC_Delay(doc, 4);
-
-	/* Reassert the CE line */
-	WriteDOC(CDSN_CTRL_CE | CDSN_CTRL_FLASH_IO | CDSN_CTRL_WP, docptr,
-		 CDSNControl);
-	DoC_Delay(doc, 4);	/* Software requirement 11.4.3 for Millennium */
-
-	/* Wait for it to be ready */
-	return DoC_WaitReady(doc);
-}
-
-/* DoC_SelectFloor: Select a given floor (bank of flash chips) */
-
-static inline int DoC_SelectFloor(struct DiskOnChip *doc, int floor)
-{
-	void __iomem *docptr = doc->virtadr;
-
-	/* Select the floor (bank) of chips required */
-	WriteDOC(floor, docptr, FloorSelect);
-
-	/* Wait for the chip to be ready */
-	return DoC_WaitReady(doc);
-}
-
-/* DoC_IdentChip: Identify a given NAND chip given {floor,chip} */
-
-static int DoC_IdentChip(struct DiskOnChip *doc, int floor, int chip)
-{
-	int mfr, id, i, j;
-	volatile char dummy;
-
-	/* Page in the required floor/chip */
-	DoC_SelectFloor(doc, floor);
-	DoC_SelectChip(doc, chip);
-
-	/* Reset the chip */
-	if (DoC_Command(doc, NAND_CMD_RESET, CDSN_CTRL_WP)) {
-		pr_debug("DoC_Command (reset) for %d,%d returned true\n",
-		      floor, chip);
-		return 0;
-	}
-
-
-	/* Read the NAND chip ID: 1. Send ReadID command */
-	if (DoC_Command(doc, NAND_CMD_READID, CDSN_CTRL_WP)) {
-		pr_debug("DoC_Command (ReadID) for %d,%d returned true\n",
-		      floor, chip);
-		return 0;
-	}
-
-	/* Read the NAND chip ID: 2. Send address byte zero */
-	DoC_Address(doc, ADDR_COLUMN, 0, CDSN_CTRL_WP, 0);
-
-	/* Read the manufacturer and device id codes from the device */
-
-	if (DoC_is_Millennium(doc)) {
-		DoC_Delay(doc, 2);
-		dummy = ReadDOC(doc->virtadr, ReadPipeInit);
-		mfr = ReadDOC(doc->virtadr, LastDataRead);
-
-		DoC_Delay(doc, 2);
-		dummy = ReadDOC(doc->virtadr, ReadPipeInit);
-		id = ReadDOC(doc->virtadr, LastDataRead);
-	} else {
-		/* CDSN Slow IO register see Software Req 11.4 item 5. */
-		dummy = ReadDOC(doc->virtadr, CDSNSlowIO);
-		DoC_Delay(doc, 2);
-		mfr = ReadDOC_(doc->virtadr, doc->ioreg);
-
-		/* CDSN Slow IO register see Software Req 11.4 item 5. */
-		dummy = ReadDOC(doc->virtadr, CDSNSlowIO);
-		DoC_Delay(doc, 2);
-		id = ReadDOC_(doc->virtadr, doc->ioreg);
-	}
-
-	/* No response - return failure */
-	if (mfr == 0xff || mfr == 0)
-		return 0;
-
-	/* Check it's the same as the first chip we identified.
-	 * M-Systems say that any given DiskOnChip device should only
-	 * contain _one_ type of flash part, although that's not a
-	 * hardware restriction. */
-	if (doc->mfr) {
-		if (doc->mfr == mfr && doc->id == id)
-			return 1;	/* This is the same as the first */
-		else
-			printk(KERN_WARNING
-			       "Flash chip at floor %d, chip %d is different:\n",
-			       floor, chip);
-	}
-
-	/* Print and store the manufacturer and ID codes. */
-	for (i = 0; nand_flash_ids[i].name != NULL; i++) {
-		if (id == nand_flash_ids[i].id) {
-			/* Try to identify manufacturer */
-			for (j = 0; nand_manuf_ids[j].id != 0x0; j++) {
-				if (nand_manuf_ids[j].id == mfr)
-					break;
-			}
-			printk(KERN_INFO
-			       "Flash chip found: Manufacturer ID: %2.2X, "
-			       "Chip ID: %2.2X (%s:%s)\n", mfr, id,
-			       nand_manuf_ids[j].name, nand_flash_ids[i].name);
-			if (!doc->mfr) {
-				doc->mfr = mfr;
-				doc->id = id;
-				doc->chipshift =
-					ffs((nand_flash_ids[i].chipsize << 20)) - 1;
-				doc->page256 = (nand_flash_ids[i].pagesize == 256) ? 1 : 0;
-				doc->pageadrlen = doc->chipshift > 25 ? 3 : 2;
-				doc->erasesize =
-				    nand_flash_ids[i].erasesize;
-				return 1;
-			}
-			return 0;
-		}
-	}
-
-
-	/* We haven't fully identified the chip. Print as much as we know. */
-	printk(KERN_WARNING "Unknown flash chip found: %2.2X %2.2X\n",
-	       id, mfr);
-
-	printk(KERN_WARNING "Please report to dwmw2@infradead.org\n");
-	return 0;
-}
-
-/* DoC_ScanChips: Find all NAND chips present in a DiskOnChip, and identify them */
-
-static void DoC_ScanChips(struct DiskOnChip *this, int maxchips)
-{
-	int floor, chip;
-	int numchips[MAX_FLOORS];
-	int ret = 1;
-
-	this->numchips = 0;
-	this->mfr = 0;
-	this->id = 0;
-
-	/* For each floor, find the number of valid chips it contains */
-	for (floor = 0; floor < MAX_FLOORS; floor++) {
-		ret = 1;
-		numchips[floor] = 0;
-		for (chip = 0; chip < maxchips && ret != 0; chip++) {
-
-			ret = DoC_IdentChip(this, floor, chip);
-			if (ret) {
-				numchips[floor]++;
-				this->numchips++;
-			}
-		}
-	}
-
-	/* If there are none at all that we recognise, bail */
-	if (!this->numchips) {
-		printk(KERN_NOTICE "No flash chips recognised.\n");
-		return;
-	}
-
-	/* Allocate an array to hold the information for each chip */
-	this->chips = kmalloc(sizeof(struct Nand) * this->numchips, GFP_KERNEL);
-	if (!this->chips) {
-		printk(KERN_NOTICE "No memory for allocating chip info structures\n");
-		return;
-	}
-
-	ret = 0;
-
-	/* Fill out the chip array with {floor, chipno} for each
-	 * detected chip in the device. */
-	for (floor = 0; floor < MAX_FLOORS; floor++) {
-		for (chip = 0; chip < numchips[floor]; chip++) {
-			this->chips[ret].floor = floor;
-			this->chips[ret].chip = chip;
-			this->chips[ret].curadr = 0;
-			this->chips[ret].curmode = 0x50;
-			ret++;
-		}
-	}
-
-	/* Calculate and print the total size of the device */
-	this->totlen = this->numchips * (1 << this->chipshift);
-
-	printk(KERN_INFO "%d flash chips found. Total DiskOnChip size: %ld MiB\n",
-	       this->numchips, this->totlen >> 20);
-}
-
-static int DoC2k_is_alias(struct DiskOnChip *doc1, struct DiskOnChip *doc2)
-{
-	int tmp1, tmp2, retval;
-	if (doc1->physadr == doc2->physadr)
-		return 1;
-
-	/* Use the alias resolution register which was set aside for this
-	 * purpose. If it's value is the same on both chips, they might
-	 * be the same chip, and we write to one and check for a change in
-	 * the other. It's unclear if this register is usuable in the
-	 * DoC 2000 (it's in the Millennium docs), but it seems to work. */
-	tmp1 = ReadDOC(doc1->virtadr, AliasResolution);
-	tmp2 = ReadDOC(doc2->virtadr, AliasResolution);
-	if (tmp1 != tmp2)
-		return 0;
-
-	WriteDOC((tmp1 + 1) % 0xff, doc1->virtadr, AliasResolution);
-	tmp2 = ReadDOC(doc2->virtadr, AliasResolution);
-	if (tmp2 == (tmp1 + 1) % 0xff)
-		retval = 1;
-	else
-		retval = 0;
-
-	/* Restore register contents.  May not be necessary, but do it just to
-	 * be safe. */
-	WriteDOC(tmp1, doc1->virtadr, AliasResolution);
-
-	return retval;
-}
-
-/* This routine is found from the docprobe code by symbol_get(),
- * which will bump the use count of this module. */
-void DoC2k_init(struct mtd_info *mtd)
-{
-	struct DiskOnChip *this = mtd->priv;
-	struct DiskOnChip *old = NULL;
-	int maxchips;
-
-	/* We must avoid being called twice for the same device. */
-
-	if (doc2klist)
-		old = doc2klist->priv;
-
-	while (old) {
-		if (DoC2k_is_alias(old, this)) {
-			printk(KERN_NOTICE
-			       "Ignoring DiskOnChip 2000 at 0x%lX - already configured\n",
-			       this->physadr);
-			iounmap(this->virtadr);
-			kfree(mtd);
-			return;
-		}
-		if (old->nextdoc)
-			old = old->nextdoc->priv;
-		else
-			old = NULL;
-	}
-
-
-	switch (this->ChipID) {
-	case DOC_ChipID_Doc2kTSOP:
-		mtd->name = "DiskOnChip 2000 TSOP";
-		this->ioreg = DoC_Mil_CDSN_IO;
-		/* Pretend it's a Millennium */
-		this->ChipID = DOC_ChipID_DocMil;
-		maxchips = MAX_CHIPS;
-		break;
-	case DOC_ChipID_Doc2k:
-		mtd->name = "DiskOnChip 2000";
-		this->ioreg = DoC_2k_CDSN_IO;
-		maxchips = MAX_CHIPS;
-		break;
-	case DOC_ChipID_DocMil:
-		mtd->name = "DiskOnChip Millennium";
-		this->ioreg = DoC_Mil_CDSN_IO;
-		maxchips = MAX_CHIPS_MIL;
-		break;
-	default:
-		printk("Unknown ChipID 0x%02x\n", this->ChipID);
-		kfree(mtd);
-		iounmap(this->virtadr);
-		return;
-	}
-
-	printk(KERN_NOTICE "%s found at address 0x%lX\n", mtd->name,
-	       this->physadr);
-
-	mtd->type = MTD_NANDFLASH;
-	mtd->flags = MTD_CAP_NANDFLASH;
-	mtd->writebufsize = mtd->writesize = 512;
-	mtd->oobsize = 16;
-	mtd->ecc_strength = 2;
-	mtd->owner = THIS_MODULE;
-	mtd->_erase = doc_erase;
-	mtd->_read = doc_read;
-	mtd->_write = doc_write;
-	mtd->_read_oob = doc_read_oob;
-	mtd->_write_oob = doc_write_oob;
-	this->curfloor = -1;
-	this->curchip = -1;
-	mutex_init(&this->lock);
-
-	/* Ident all the chips present. */
-	DoC_ScanChips(this, maxchips);
-
-	if (!this->totlen) {
-		kfree(mtd);
-		iounmap(this->virtadr);
-	} else {
-		this->nextdoc = doc2klist;
-		doc2klist = mtd;
-		mtd->size = this->totlen;
-		mtd->erasesize = this->erasesize;
-		mtd_device_register(mtd, NULL, 0);
-		return;
-	}
-}
-EXPORT_SYMBOL_GPL(DoC2k_init);
-
-static int doc_read(struct mtd_info *mtd, loff_t from, size_t len,
-		    size_t * retlen, u_char * buf)
-{
-	struct DiskOnChip *this = mtd->priv;
-	void __iomem *docptr = this->virtadr;
-	struct Nand *mychip;
-	unsigned char syndrome[6], eccbuf[6];
-	volatile char dummy;
-	int i, len256 = 0, ret=0;
-	size_t left = len;
-
-	mutex_lock(&this->lock);
-	while (left) {
-		len = left;
-
-		/* Don't allow a single read to cross a 512-byte block boundary */
-		if (from + len > ((from | 0x1ff) + 1))
-			len = ((from | 0x1ff) + 1) - from;
-
-		/* The ECC will not be calculated correctly if less than 512 is read */
-		if (len != 0x200)
-			printk(KERN_WARNING
-			       "ECC needs a full sector read (adr: %lx size %lx)\n",
-			       (long) from, (long) len);
-
-		/* printk("DoC_Read (adr: %lx size %lx)\n", (long) from, (long) len); */
-
-
-		/* Find the chip which is to be used and select it */
-		mychip = &this->chips[from >> (this->chipshift)];
-
-		if (this->curfloor != mychip->floor) {
-			DoC_SelectFloor(this, mychip->floor);
-			DoC_SelectChip(this, mychip->chip);
-		} else if (this->curchip != mychip->chip) {
-			DoC_SelectChip(this, mychip->chip);
-		}
-
-		this->curfloor = mychip->floor;
-		this->curchip = mychip->chip;
-
-		DoC_Command(this,
-			    (!this->page256
-			     && (from & 0x100)) ? NAND_CMD_READ1 : NAND_CMD_READ0,
-			    CDSN_CTRL_WP);
-		DoC_Address(this, ADDR_COLUMN_PAGE, from, CDSN_CTRL_WP,
-			    CDSN_CTRL_ECC_IO);
-
-		/* Prime the ECC engine */
-		WriteDOC(DOC_ECC_RESET, docptr, ECCConf);
-		WriteDOC(DOC_ECC_EN, docptr, ECCConf);
-
-		/* treat crossing 256-byte sector for 2M x 8bits devices */
-		if (this->page256 && from + len > (from | 0xff) + 1) {
-			len256 = (from | 0xff) + 1 - from;
-			DoC_ReadBuf(this, buf, len256);
-
-			DoC_Command(this, NAND_CMD_READ0, CDSN_CTRL_WP);
-			DoC_Address(this, ADDR_COLUMN_PAGE, from + len256,
-				    CDSN_CTRL_WP, CDSN_CTRL_ECC_IO);
-		}
-
-		DoC_ReadBuf(this, &buf[len256], len - len256);
-
-		/* Let the caller know we completed it */
-		*retlen += len;
-
-		/* Read the ECC data through the DiskOnChip ECC logic */
-		/* Note: this will work even with 2M x 8bit devices as   */
-		/*       they have 8 bytes of OOB per 256 page. mf.      */
-		DoC_ReadBuf(this, eccbuf, 6);
-
-		/* Flush the pipeline */
-		if (DoC_is_Millennium(this)) {
-			dummy = ReadDOC(docptr, ECCConf);
-			dummy = ReadDOC(docptr, ECCConf);
-			i = ReadDOC(docptr, ECCConf);
-		} else {
-			dummy = ReadDOC(docptr, 2k_ECCStatus);
-			dummy = ReadDOC(docptr, 2k_ECCStatus);
-			i = ReadDOC(docptr, 2k_ECCStatus);
-		}
-
-		/* Check the ECC Status */
-		if (i & 0x80) {
-			int nb_errors;
-			/* There was an ECC error */
-#ifdef ECC_DEBUG
-			printk(KERN_ERR "DiskOnChip ECC Error: Read at %lx\n", (long)from);
-#endif
-			/* Read the ECC syndrome through the DiskOnChip ECC
-			   logic.  These syndrome will be all ZERO when there
-			   is no error */
-			for (i = 0; i < 6; i++) {
-				syndrome[i] =
-					ReadDOC(docptr, ECCSyndrome0 + i);
-			}
-			nb_errors = doc_decode_ecc(buf, syndrome);
-
-#ifdef ECC_DEBUG
-			printk(KERN_ERR "Errors corrected: %x\n", nb_errors);
-#endif
-			if (nb_errors < 0) {
-				/* We return error, but have actually done the
-				   read. Not that this can be told to
-				   user-space, via sys_read(), but at least
-				   MTD-aware stuff can know about it by
-				   checking *retlen */
-				ret = -EIO;
-			}
-		}
-
-#ifdef PSYCHO_DEBUG
-		printk(KERN_DEBUG "ECC DATA at %lxB: %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X\n",
-		       (long)from, eccbuf[0], eccbuf[1], eccbuf[2],
-		       eccbuf[3], eccbuf[4], eccbuf[5]);
-#endif
-
-		/* disable the ECC engine */
-		WriteDOC(DOC_ECC_DIS, docptr , ECCConf);
-
-		/* according to 11.4.1, we need to wait for the busy line
-	         * drop if we read to the end of the page.  */
-		if(0 == ((from + len) & 0x1ff))
-		{
-		    DoC_WaitReady(this);
-		}
-
-		from += len;
-		left -= len;
-		buf += len;
-	}
-
-	mutex_unlock(&this->lock);
-
-	return ret;
-}
-
-static int doc_write(struct mtd_info *mtd, loff_t to, size_t len,
-		     size_t * retlen, const u_char * buf)
-{
-	struct DiskOnChip *this = mtd->priv;
-	int di; /* Yes, DI is a hangover from when I was disassembling the binary driver */
-	void __iomem *docptr = this->virtadr;
-	unsigned char eccbuf[6];
-	volatile char dummy;
-	int len256 = 0;
-	struct Nand *mychip;
-	size_t left = len;
-	int status;
-
-	mutex_lock(&this->lock);
-	while (left) {
-		len = left;
-
-		/* Don't allow a single write to cross a 512-byte block boundary */
-		if (to + len > ((to | 0x1ff) + 1))
-			len = ((to | 0x1ff) + 1) - to;
-
-		/* The ECC will not be calculated correctly if less than 512 is written */
-/* DBB-
-		if (len != 0x200 && eccbuf)
-			printk(KERN_WARNING
-			       "ECC needs a full sector write (adr: %lx size %lx)\n",
-			       (long) to, (long) len);
-   -DBB */
-
-		/* printk("DoC_Write (adr: %lx size %lx)\n", (long) to, (long) len); */
-
-		/* Find the chip which is to be used and select it */
-		mychip = &this->chips[to >> (this->chipshift)];
-
-		if (this->curfloor != mychip->floor) {
-			DoC_SelectFloor(this, mychip->floor);
-			DoC_SelectChip(this, mychip->chip);
-		} else if (this->curchip != mychip->chip) {
-			DoC_SelectChip(this, mychip->chip);
-		}
-
-		this->curfloor = mychip->floor;
-		this->curchip = mychip->chip;
-
-		/* Set device to main plane of flash */
-		DoC_Command(this, NAND_CMD_RESET, CDSN_CTRL_WP);
-		DoC_Command(this,
-			    (!this->page256
-			     && (to & 0x100)) ? NAND_CMD_READ1 : NAND_CMD_READ0,
-			    CDSN_CTRL_WP);
-
-		DoC_Command(this, NAND_CMD_SEQIN, 0);
-		DoC_Address(this, ADDR_COLUMN_PAGE, to, 0, CDSN_CTRL_ECC_IO);
-
-		/* Prime the ECC engine */
-		WriteDOC(DOC_ECC_RESET, docptr, ECCConf);
-		WriteDOC(DOC_ECC_EN | DOC_ECC_RW, docptr, ECCConf);
-
-		/* treat crossing 256-byte sector for 2M x 8bits devices */
-		if (this->page256 && to + len > (to | 0xff) + 1) {
-			len256 = (to | 0xff) + 1 - to;
-			DoC_WriteBuf(this, buf, len256);
-
-			DoC_Command(this, NAND_CMD_PAGEPROG, 0);
-
-			DoC_Command(this, NAND_CMD_STATUS, CDSN_CTRL_WP);
-			/* There's an implicit DoC_WaitReady() in DoC_Command */
-
-			dummy = ReadDOC(docptr, CDSNSlowIO);
-			DoC_Delay(this, 2);
-
-			if (ReadDOC_(docptr, this->ioreg) & 1) {
-				printk(KERN_ERR "Error programming flash\n");
-				/* Error in programming */
-				*retlen = 0;
-				mutex_unlock(&this->lock);
-				return -EIO;
-			}
-
-			DoC_Command(this, NAND_CMD_SEQIN, 0);
-			DoC_Address(this, ADDR_COLUMN_PAGE, to + len256, 0,
-				    CDSN_CTRL_ECC_IO);
-		}
-
-		DoC_WriteBuf(this, &buf[len256], len - len256);
-
-		WriteDOC(CDSN_CTRL_ECC_IO | CDSN_CTRL_CE, docptr, CDSNControl);
-
-		if (DoC_is_Millennium(this)) {
-			WriteDOC(0, docptr, NOP);
-			WriteDOC(0, docptr, NOP);
-			WriteDOC(0, docptr, NOP);
-		} else {
-			WriteDOC_(0, docptr, this->ioreg);
-			WriteDOC_(0, docptr, this->ioreg);
-			WriteDOC_(0, docptr, this->ioreg);
-		}
-
-		WriteDOC(CDSN_CTRL_ECC_IO | CDSN_CTRL_FLASH_IO | CDSN_CTRL_CE, docptr,
-			 CDSNControl);
-
-		/* Read the ECC data through the DiskOnChip ECC logic */
-		for (di = 0; di < 6; di++) {
-			eccbuf[di] = ReadDOC(docptr, ECCSyndrome0 + di);
-		}
-
-		/* Reset the ECC engine */
-		WriteDOC(DOC_ECC_DIS, docptr, ECCConf);
-
-#ifdef PSYCHO_DEBUG
-		printk
-			("OOB data at %lx is %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X\n",
-			 (long) to, eccbuf[0], eccbuf[1], eccbuf[2], eccbuf[3],
-			 eccbuf[4], eccbuf[5]);
-#endif
-		DoC_Command(this, NAND_CMD_PAGEPROG, 0);
-
-		DoC_Command(this, NAND_CMD_STATUS, CDSN_CTRL_WP);
-		/* There's an implicit DoC_WaitReady() in DoC_Command */
-
-		if (DoC_is_Millennium(this)) {
-			ReadDOC(docptr, ReadPipeInit);
-			status = ReadDOC(docptr, LastDataRead);
-		} else {
-			dummy = ReadDOC(docptr, CDSNSlowIO);
-			DoC_Delay(this, 2);
-			status = ReadDOC_(docptr, this->ioreg);
-		}
-
-		if (status & 1) {
-			printk(KERN_ERR "Error programming flash\n");
-			/* Error in programming */
-			*retlen = 0;
-			mutex_unlock(&this->lock);
-			return -EIO;
-		}
-
-		/* Let the caller know we completed it */
-		*retlen += len;
-
-		{
-			unsigned char x[8];
-			size_t dummy;
-			int ret;
-
-			/* Write the ECC data to flash */
-			for (di=0; di<6; di++)
-				x[di] = eccbuf[di];
-
-			x[6]=0x55;
-			x[7]=0x55;
-
-			ret = doc_write_oob_nolock(mtd, to, 8, &dummy, x);
-			if (ret) {
-				mutex_unlock(&this->lock);
-				return ret;
-			}
-		}
-
-		to += len;
-		left -= len;
-		buf += len;
-	}
-
-	mutex_unlock(&this->lock);
-	return 0;
-}
-
-static int doc_read_oob(struct mtd_info *mtd, loff_t ofs,
-			struct mtd_oob_ops *ops)
-{
-	struct DiskOnChip *this = mtd->priv;
-	int len256 = 0, ret;
-	struct Nand *mychip;
-	uint8_t *buf = ops->oobbuf;
-	size_t len = ops->len;
-
-	BUG_ON(ops->mode != MTD_OPS_PLACE_OOB);
-
-	ofs += ops->ooboffs;
-
-	mutex_lock(&this->lock);
-
-	mychip = &this->chips[ofs >> this->chipshift];
-
-	if (this->curfloor != mychip->floor) {
-		DoC_SelectFloor(this, mychip->floor);
-		DoC_SelectChip(this, mychip->chip);
-	} else if (this->curchip != mychip->chip) {
-		DoC_SelectChip(this, mychip->chip);
-	}
-	this->curfloor = mychip->floor;
-	this->curchip = mychip->chip;
-
-	/* update address for 2M x 8bit devices. OOB starts on the second */
-	/* page to maintain compatibility with doc_read_ecc. */
-	if (this->page256) {
-		if (!(ofs & 0x8))
-			ofs += 0x100;
-		else
-			ofs -= 0x8;
-	}
-
-	DoC_Command(this, NAND_CMD_READOOB, CDSN_CTRL_WP);
-	DoC_Address(this, ADDR_COLUMN_PAGE, ofs, CDSN_CTRL_WP, 0);
-
-	/* treat crossing 8-byte OOB data for 2M x 8bit devices */
-	/* Note: datasheet says it should automaticaly wrap to the */
-	/*       next OOB block, but it didn't work here. mf.      */
-	if (this->page256 && ofs + len > (ofs | 0x7) + 1) {
-		len256 = (ofs | 0x7) + 1 - ofs;
-		DoC_ReadBuf(this, buf, len256);
-
-		DoC_Command(this, NAND_CMD_READOOB, CDSN_CTRL_WP);
-		DoC_Address(this, ADDR_COLUMN_PAGE, ofs & (~0x1ff),
-			    CDSN_CTRL_WP, 0);
-	}
-
-	DoC_ReadBuf(this, &buf[len256], len - len256);
-
-	ops->retlen = len;
-	/* Reading the full OOB data drops us off of the end of the page,
-         * causing the flash device to go into busy mode, so we need
-         * to wait until ready 11.4.1 and Toshiba TC58256FT docs */
-
-	ret = DoC_WaitReady(this);
-
-	mutex_unlock(&this->lock);
-	return ret;
-
-}
-
-static int doc_write_oob_nolock(struct mtd_info *mtd, loff_t ofs, size_t len,
-				size_t * retlen, const u_char * buf)
-{
-	struct DiskOnChip *this = mtd->priv;
-	int len256 = 0;
-	void __iomem *docptr = this->virtadr;
-	struct Nand *mychip = &this->chips[ofs >> this->chipshift];
-	volatile int dummy;
-	int status;
-
-	//      printk("doc_write_oob(%lx, %d): %2.2X %2.2X %2.2X %2.2X ... %2.2X %2.2X .. %2.2X %2.2X\n",(long)ofs, len,
-	//   buf[0], buf[1], buf[2], buf[3], buf[8], buf[9], buf[14],buf[15]);
-
-	/* Find the chip which is to be used and select it */
-	if (this->curfloor != mychip->floor) {
-		DoC_SelectFloor(this, mychip->floor);
-		DoC_SelectChip(this, mychip->chip);
-	} else if (this->curchip != mychip->chip) {
-		DoC_SelectChip(this, mychip->chip);
-	}
-	this->curfloor = mychip->floor;
-	this->curchip = mychip->chip;
-
-	/* disable the ECC engine */
-	WriteDOC (DOC_ECC_RESET, docptr, ECCConf);
-	WriteDOC (DOC_ECC_DIS, docptr, ECCConf);
-
-	/* Reset the chip, see Software Requirement 11.4 item 1. */
-	DoC_Command(this, NAND_CMD_RESET, CDSN_CTRL_WP);
-
-	/* issue the Read2 command to set the pointer to the Spare Data Area. */
-	DoC_Command(this, NAND_CMD_READOOB, CDSN_CTRL_WP);
-
-	/* update address for 2M x 8bit devices. OOB starts on the second */
-	/* page to maintain compatibility with doc_read_ecc. */
-	if (this->page256) {
-		if (!(ofs & 0x8))
-			ofs += 0x100;
-		else
-			ofs -= 0x8;
-	}
-
-	/* issue the Serial Data In command to initial the Page Program process */
-	DoC_Command(this, NAND_CMD_SEQIN, 0);
-	DoC_Address(this, ADDR_COLUMN_PAGE, ofs, 0, 0);
-
-	/* treat crossing 8-byte OOB data for 2M x 8bit devices */
-	/* Note: datasheet says it should automaticaly wrap to the */
-	/*       next OOB block, but it didn't work here. mf.      */
-	if (this->page256 && ofs + len > (ofs | 0x7) + 1) {
-		len256 = (ofs | 0x7) + 1 - ofs;
-		DoC_WriteBuf(this, buf, len256);
-
-		DoC_Command(this, NAND_CMD_PAGEPROG, 0);
-		DoC_Command(this, NAND_CMD_STATUS, 0);
-		/* DoC_WaitReady() is implicit in DoC_Command */
-
-		if (DoC_is_Millennium(this)) {
-			ReadDOC(docptr, ReadPipeInit);
-			status = ReadDOC(docptr, LastDataRead);
-		} else {
-			dummy = ReadDOC(docptr, CDSNSlowIO);
-			DoC_Delay(this, 2);
-			status = ReadDOC_(docptr, this->ioreg);
-		}
-
-		if (status & 1) {
-			printk(KERN_ERR "Error programming oob data\n");
-			/* There was an error */
-			*retlen = 0;
-			return -EIO;
-		}
-		DoC_Command(this, NAND_CMD_SEQIN, 0);
-		DoC_Address(this, ADDR_COLUMN_PAGE, ofs & (~0x1ff), 0, 0);
-	}
-
-	DoC_WriteBuf(this, &buf[len256], len - len256);
-
-	DoC_Command(this, NAND_CMD_PAGEPROG, 0);
-	DoC_Command(this, NAND_CMD_STATUS, 0);
-	/* DoC_WaitReady() is implicit in DoC_Command */
-
-	if (DoC_is_Millennium(this)) {
-		ReadDOC(docptr, ReadPipeInit);
-		status = ReadDOC(docptr, LastDataRead);
-	} else {
-		dummy = ReadDOC(docptr, CDSNSlowIO);
-		DoC_Delay(this, 2);
-		status = ReadDOC_(docptr, this->ioreg);
-	}
-
-	if (status & 1) {
-		printk(KERN_ERR "Error programming oob data\n");
-		/* There was an error */
-		*retlen = 0;
-		return -EIO;
-	}
-
-	*retlen = len;
-	return 0;
-
-}
-
-static int doc_write_oob(struct mtd_info *mtd, loff_t ofs,
-			 struct mtd_oob_ops *ops)
-{
-	struct DiskOnChip *this = mtd->priv;
-	int ret;
-
-	BUG_ON(ops->mode != MTD_OPS_PLACE_OOB);
-
-	mutex_lock(&this->lock);
-	ret = doc_write_oob_nolock(mtd, ofs + ops->ooboffs, ops->len,
-				   &ops->retlen, ops->oobbuf);
-
-	mutex_unlock(&this->lock);
-	return ret;
-}
-
-static int doc_erase(struct mtd_info *mtd, struct erase_info *instr)
-{
-	struct DiskOnChip *this = mtd->priv;
-	__u32 ofs = instr->addr;
-	__u32 len = instr->len;
-	volatile int dummy;
-	void __iomem *docptr = this->virtadr;
-	struct Nand *mychip;
-	int status;
-
- 	mutex_lock(&this->lock);
-
-	if (ofs & (mtd->erasesize-1) || len & (mtd->erasesize-1)) {
-		mutex_unlock(&this->lock);
-		return -EINVAL;
-	}
-
-	instr->state = MTD_ERASING;
-
-	/* FIXME: Do this in the background. Use timers or schedule_task() */
-	while(len) {
-		mychip = &this->chips[ofs >> this->chipshift];
-
-		if (this->curfloor != mychip->floor) {
-			DoC_SelectFloor(this, mychip->floor);
-			DoC_SelectChip(this, mychip->chip);
-		} else if (this->curchip != mychip->chip) {
-			DoC_SelectChip(this, mychip->chip);
-		}
-		this->curfloor = mychip->floor;
-		this->curchip = mychip->chip;
-
-		DoC_Command(this, NAND_CMD_ERASE1, 0);
-		DoC_Address(this, ADDR_PAGE, ofs, 0, 0);
-		DoC_Command(this, NAND_CMD_ERASE2, 0);
-
-		DoC_Command(this, NAND_CMD_STATUS, CDSN_CTRL_WP);
-
-		if (DoC_is_Millennium(this)) {
-			ReadDOC(docptr, ReadPipeInit);
-			status = ReadDOC(docptr, LastDataRead);
-		} else {
-			dummy = ReadDOC(docptr, CDSNSlowIO);
-			DoC_Delay(this, 2);
-			status = ReadDOC_(docptr, this->ioreg);
-		}
-
-		if (status & 1) {
-			printk(KERN_ERR "Error erasing at 0x%x\n", ofs);
-			/* There was an error */
-			instr->state = MTD_ERASE_FAILED;
-			goto callback;
-		}
-		ofs += mtd->erasesize;
-		len -= mtd->erasesize;
-	}
-	instr->state = MTD_ERASE_DONE;
-
- callback:
-	mtd_erase_callback(instr);
-
-	mutex_unlock(&this->lock);
-	return 0;
-}
-
-
-/****************************************************************************
- *
- * Module stuff
- *
- ****************************************************************************/
-
-static void __exit cleanup_doc2000(void)
-{
-	struct mtd_info *mtd;
-	struct DiskOnChip *this;
-
-	while ((mtd = doc2klist)) {
-		this = mtd->priv;
-		doc2klist = this->nextdoc;
-
-		mtd_device_unregister(mtd);
-
-		iounmap(this->virtadr);
-		kfree(this->chips);
-		kfree(mtd);
-	}
-}
-
-module_exit(cleanup_doc2000);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("David Woodhouse <dwmw2@infradead.org> et al.");
-MODULE_DESCRIPTION("MTD driver for DiskOnChip 2000 and Millennium");
-
diff --git a/drivers/mtd/devices/doc2001.c b/drivers/mtd/devices/doc2001.c
deleted file mode 100644
index f6927955dab0..000000000000
--- a/drivers/mtd/devices/doc2001.c
+++ /dev/null
@@ -1,824 +0,0 @@
-
-/*
- * Linux driver for Disk-On-Chip Millennium
- * (c) 1999 Machine Vision Holdings, Inc.
- * (c) 1999, 2000 David Woodhouse <dwmw2@infradead.org>
- */
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <asm/errno.h>
-#include <asm/io.h>
-#include <asm/uaccess.h>
-#include <linux/delay.h>
-#include <linux/slab.h>
-#include <linux/init.h>
-#include <linux/types.h>
-#include <linux/bitops.h>
-
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/nand.h>
-#include <linux/mtd/doc2000.h>
-
-/* #define ECC_DEBUG */
-
-/* I have no idea why some DoC chips can not use memcop_form|to_io().
- * This may be due to the different revisions of the ASIC controller built-in or
- * simplily a QA/Bug issue. Who knows ?? If you have trouble, please uncomment
- * this:*/
-#undef USE_MEMCPY
-
-static int doc_read(struct mtd_info *mtd, loff_t from, size_t len,
-		    size_t *retlen, u_char *buf);
-static int doc_write(struct mtd_info *mtd, loff_t to, size_t len,
-		     size_t *retlen, const u_char *buf);
-static int doc_read_oob(struct mtd_info *mtd, loff_t ofs,
-			struct mtd_oob_ops *ops);
-static int doc_write_oob(struct mtd_info *mtd, loff_t ofs,
-			 struct mtd_oob_ops *ops);
-static int doc_erase (struct mtd_info *mtd, struct erase_info *instr);
-
-static struct mtd_info *docmillist = NULL;
-
-/* Perform the required delay cycles by reading from the NOP register */
-static void DoC_Delay(void __iomem * docptr, unsigned short cycles)
-{
-	volatile char dummy;
-	int i;
-
-	for (i = 0; i < cycles; i++)
-		dummy = ReadDOC(docptr, NOP);
-}
-
-/* DOC_WaitReady: Wait for RDY line to be asserted by the flash chip */
-static int _DoC_WaitReady(void __iomem * docptr)
-{
-	unsigned short c = 0xffff;
-
-	pr_debug("_DoC_WaitReady called for out-of-line wait\n");
-
-	/* Out-of-line routine to wait for chip response */
-	while (!(ReadDOC(docptr, CDSNControl) & CDSN_CTRL_FR_B) && --c)
-		;
-
-	if (c == 0)
-		pr_debug("_DoC_WaitReady timed out.\n");
-
-	return (c == 0);
-}
-
-static inline int DoC_WaitReady(void __iomem * docptr)
-{
-	/* This is inline, to optimise the common case, where it's ready instantly */
-	int ret = 0;
-
-	/* 4 read form NOP register should be issued in prior to the read from CDSNControl
-	   see Software Requirement 11.4 item 2. */
-	DoC_Delay(docptr, 4);
-
-	if (!(ReadDOC(docptr, CDSNControl) & CDSN_CTRL_FR_B))
-		/* Call the out-of-line routine to wait */
-		ret = _DoC_WaitReady(docptr);
-
-	/* issue 2 read from NOP register after reading from CDSNControl register
-	   see Software Requirement 11.4 item 2. */
-	DoC_Delay(docptr, 2);
-
-	return ret;
-}
-
-/* DoC_Command: Send a flash command to the flash chip through the CDSN IO register
-   with the internal pipeline. Each of 4 delay cycles (read from the NOP register) is
-   required after writing to CDSN Control register, see Software Requirement 11.4 item 3. */
-
-static void DoC_Command(void __iomem * docptr, unsigned char command,
-			       unsigned char xtraflags)
-{
-	/* Assert the CLE (Command Latch Enable) line to the flash chip */
-	WriteDOC(xtraflags | CDSN_CTRL_CLE | CDSN_CTRL_CE, docptr, CDSNControl);
-	DoC_Delay(docptr, 4);
-
-	/* Send the command */
-	WriteDOC(command, docptr, Mil_CDSN_IO);
-	WriteDOC(0x00, docptr, WritePipeTerm);
-
-	/* Lower the CLE line */
-	WriteDOC(xtraflags | CDSN_CTRL_CE, docptr, CDSNControl);
-	DoC_Delay(docptr, 4);
-}
-
-/* DoC_Address: Set the current address for the flash chip through the CDSN IO register
-   with the internal pipeline. Each of 4 delay cycles (read from the NOP register) is
-   required after writing to CDSN Control register, see Software Requirement 11.4 item 3. */
-
-static inline void DoC_Address(void __iomem * docptr, int numbytes, unsigned long ofs,
-			       unsigned char xtraflags1, unsigned char xtraflags2)
-{
-	/* Assert the ALE (Address Latch Enable) line to the flash chip */
-	WriteDOC(xtraflags1 | CDSN_CTRL_ALE | CDSN_CTRL_CE, docptr, CDSNControl);
-	DoC_Delay(docptr, 4);
-
-	/* Send the address */
-	switch (numbytes)
-	    {
-	    case 1:
-		    /* Send single byte, bits 0-7. */
-		    WriteDOC(ofs & 0xff, docptr, Mil_CDSN_IO);
-		    WriteDOC(0x00, docptr, WritePipeTerm);
-		    break;
-	    case 2:
-		    /* Send bits 9-16 followed by 17-23 */
-		    WriteDOC((ofs >> 9)  & 0xff, docptr, Mil_CDSN_IO);
-		    WriteDOC((ofs >> 17) & 0xff, docptr, Mil_CDSN_IO);
-		    WriteDOC(0x00, docptr, WritePipeTerm);
-		break;
-	    case 3:
-		    /* Send 0-7, 9-16, then 17-23 */
-		    WriteDOC(ofs & 0xff, docptr, Mil_CDSN_IO);
-		    WriteDOC((ofs >> 9)  & 0xff, docptr, Mil_CDSN_IO);
-		    WriteDOC((ofs >> 17) & 0xff, docptr, Mil_CDSN_IO);
-		    WriteDOC(0x00, docptr, WritePipeTerm);
-		break;
-	    default:
-		return;
-	    }
-
-	/* Lower the ALE line */
-	WriteDOC(xtraflags1 | xtraflags2 | CDSN_CTRL_CE, docptr, CDSNControl);
-	DoC_Delay(docptr, 4);
-}
-
-/* DoC_SelectChip: Select a given flash chip within the current floor */
-static int DoC_SelectChip(void __iomem * docptr, int chip)
-{
-	/* Select the individual flash chip requested */
-	WriteDOC(chip, docptr, CDSNDeviceSelect);
-	DoC_Delay(docptr, 4);
-
-	/* Wait for it to be ready */
-	return DoC_WaitReady(docptr);
-}
-
-/* DoC_SelectFloor: Select a given floor (bank of flash chips) */
-static int DoC_SelectFloor(void __iomem * docptr, int floor)
-{
-	/* Select the floor (bank) of chips required */
-	WriteDOC(floor, docptr, FloorSelect);
-
-	/* Wait for the chip to be ready */
-	return DoC_WaitReady(docptr);
-}
-
-/* DoC_IdentChip: Identify a given NAND chip given {floor,chip} */
-static int DoC_IdentChip(struct DiskOnChip *doc, int floor, int chip)
-{
-	int mfr, id, i, j;
-	volatile char dummy;
-
-	/* Page in the required floor/chip
-	   FIXME: is this supported by Millennium ?? */
-	DoC_SelectFloor(doc->virtadr, floor);
-	DoC_SelectChip(doc->virtadr, chip);
-
-	/* Reset the chip, see Software Requirement 11.4 item 1. */
-	DoC_Command(doc->virtadr, NAND_CMD_RESET, CDSN_CTRL_WP);
-	DoC_WaitReady(doc->virtadr);
-
-	/* Read the NAND chip ID: 1. Send ReadID command */
-	DoC_Command(doc->virtadr, NAND_CMD_READID, CDSN_CTRL_WP);
-
-	/* Read the NAND chip ID: 2. Send address byte zero */
-	DoC_Address(doc->virtadr, 1, 0x00, CDSN_CTRL_WP, 0x00);
-
-	/* Read the manufacturer and device id codes of the flash device through
-	   CDSN IO register see Software Requirement 11.4 item 5.*/
-	dummy = ReadDOC(doc->virtadr, ReadPipeInit);
-	DoC_Delay(doc->virtadr, 2);
-	mfr = ReadDOC(doc->virtadr, Mil_CDSN_IO);
-
-	DoC_Delay(doc->virtadr, 2);
-	id  = ReadDOC(doc->virtadr, Mil_CDSN_IO);
-	dummy = ReadDOC(doc->virtadr, LastDataRead);
-
-	/* No response - return failure */
-	if (mfr == 0xff || mfr == 0)
-		return 0;
-
-	/* FIXME: to deal with multi-flash on multi-Millennium case more carefully */
-	for (i = 0; nand_flash_ids[i].name != NULL; i++) {
-		if ( id == nand_flash_ids[i].id) {
-			/* Try to identify manufacturer */
-			for (j = 0; nand_manuf_ids[j].id != 0x0; j++) {
-				if (nand_manuf_ids[j].id == mfr)
-					break;
-			}
-			printk(KERN_INFO "Flash chip found: Manufacturer ID: %2.2X, "
-			       "Chip ID: %2.2X (%s:%s)\n",
-			       mfr, id, nand_manuf_ids[j].name, nand_flash_ids[i].name);
-			doc->mfr = mfr;
-			doc->id = id;
-			doc->chipshift = ffs((nand_flash_ids[i].chipsize << 20)) - 1;
-			break;
-		}
-	}
-
-	if (nand_flash_ids[i].name == NULL)
-		return 0;
-	else
-		return 1;
-}
-
-/* DoC_ScanChips: Find all NAND chips present in a DiskOnChip, and identify them */
-static void DoC_ScanChips(struct DiskOnChip *this)
-{
-	int floor, chip;
-	int numchips[MAX_FLOORS_MIL];
-	int ret;
-
-	this->numchips = 0;
-	this->mfr = 0;
-	this->id = 0;
-
-	/* For each floor, find the number of valid chips it contains */
-	for (floor = 0,ret = 1; floor < MAX_FLOORS_MIL; floor++) {
-		numchips[floor] = 0;
-		for (chip = 0; chip < MAX_CHIPS_MIL && ret != 0; chip++) {
-			ret = DoC_IdentChip(this, floor, chip);
-			if (ret) {
-				numchips[floor]++;
-				this->numchips++;
-			}
-		}
-	}
-	/* If there are none at all that we recognise, bail */
-	if (!this->numchips) {
-		printk("No flash chips recognised.\n");
-		return;
-	}
-
-	/* Allocate an array to hold the information for each chip */
-	this->chips = kmalloc(sizeof(struct Nand) * this->numchips, GFP_KERNEL);
-	if (!this->chips){
-		printk("No memory for allocating chip info structures\n");
-		return;
-	}
-
-	/* Fill out the chip array with {floor, chipno} for each
-	 * detected chip in the device. */
-	for (floor = 0, ret = 0; floor < MAX_FLOORS_MIL; floor++) {
-		for (chip = 0 ; chip < numchips[floor] ; chip++) {
-			this->chips[ret].floor = floor;
-			this->chips[ret].chip = chip;
-			this->chips[ret].curadr = 0;
-			this->chips[ret].curmode = 0x50;
-			ret++;
-		}
-	}
-
-	/* Calculate and print the total size of the device */
-	this->totlen = this->numchips * (1 << this->chipshift);
-	printk(KERN_INFO "%d flash chips found. Total DiskOnChip size: %ld MiB\n",
-	       this->numchips ,this->totlen >> 20);
-}
-
-static int DoCMil_is_alias(struct DiskOnChip *doc1, struct DiskOnChip *doc2)
-{
-	int tmp1, tmp2, retval;
-
-	if (doc1->physadr == doc2->physadr)
-		return 1;
-
-	/* Use the alias resolution register which was set aside for this
-	 * purpose. If it's value is the same on both chips, they might
-	 * be the same chip, and we write to one and check for a change in
-	 * the other. It's unclear if this register is usuable in the
-	 * DoC 2000 (it's in the Millenium docs), but it seems to work. */
-	tmp1 = ReadDOC(doc1->virtadr, AliasResolution);
-	tmp2 = ReadDOC(doc2->virtadr, AliasResolution);
-	if (tmp1 != tmp2)
-		return 0;
-
-	WriteDOC((tmp1+1) % 0xff, doc1->virtadr, AliasResolution);
-	tmp2 = ReadDOC(doc2->virtadr, AliasResolution);
-	if (tmp2 == (tmp1+1) % 0xff)
-		retval = 1;
-	else
-		retval = 0;
-
-	/* Restore register contents.  May not be necessary, but do it just to
-	 * be safe. */
-	WriteDOC(tmp1, doc1->virtadr, AliasResolution);
-
-	return retval;
-}
-
-/* This routine is found from the docprobe code by symbol_get(),
- * which will bump the use count of this module. */
-void DoCMil_init(struct mtd_info *mtd)
-{
-	struct DiskOnChip *this = mtd->priv;
-	struct DiskOnChip *old = NULL;
-
-	/* We must avoid being called twice for the same device. */
-	if (docmillist)
-		old = docmillist->priv;
-
-	while (old) {
-		if (DoCMil_is_alias(this, old)) {
-			printk(KERN_NOTICE "Ignoring DiskOnChip Millennium at "
-			       "0x%lX - already configured\n", this->physadr);
-			iounmap(this->virtadr);
-			kfree(mtd);
-			return;
-		}
-		if (old->nextdoc)
-			old = old->nextdoc->priv;
-		else
-			old = NULL;
-	}
-
-	mtd->name = "DiskOnChip Millennium";
-	printk(KERN_NOTICE "DiskOnChip Millennium found at address 0x%lX\n",
-	       this->physadr);
-
-	mtd->type = MTD_NANDFLASH;
-	mtd->flags = MTD_CAP_NANDFLASH;
-
-	/* FIXME: erase size is not always 8KiB */
-	mtd->erasesize = 0x2000;
-	mtd->writebufsize = mtd->writesize = 512;
-	mtd->oobsize = 16;
-	mtd->ecc_strength = 2;
-	mtd->owner = THIS_MODULE;
-	mtd->_erase = doc_erase;
-	mtd->_read = doc_read;
-	mtd->_write = doc_write;
-	mtd->_read_oob = doc_read_oob;
-	mtd->_write_oob = doc_write_oob;
-	this->curfloor = -1;
-	this->curchip = -1;
-
-	/* Ident all the chips present. */
-	DoC_ScanChips(this);
-
-	if (!this->totlen) {
-		kfree(mtd);
-		iounmap(this->virtadr);
-	} else {
-		this->nextdoc = docmillist;
-		docmillist = mtd;
-		mtd->size  = this->totlen;
-		mtd_device_register(mtd, NULL, 0);
-		return;
-	}
-}
-EXPORT_SYMBOL_GPL(DoCMil_init);
-
-static int doc_read (struct mtd_info *mtd, loff_t from, size_t len,
-		     size_t *retlen, u_char *buf)
-{
-	int i, ret;
-	volatile char dummy;
-	unsigned char syndrome[6], eccbuf[6];
-	struct DiskOnChip *this = mtd->priv;
-	void __iomem *docptr = this->virtadr;
-	struct Nand *mychip = &this->chips[from >> (this->chipshift)];
-
-	/* Don't allow a single read to cross a 512-byte block boundary */
-	if (from + len > ((from | 0x1ff) + 1))
-		len = ((from | 0x1ff) + 1) - from;
-
-	/* Find the chip which is to be used and select it */
-	if (this->curfloor != mychip->floor) {
-		DoC_SelectFloor(docptr, mychip->floor);
-		DoC_SelectChip(docptr, mychip->chip);
-	} else if (this->curchip != mychip->chip) {
-		DoC_SelectChip(docptr, mychip->chip);
-	}
-	this->curfloor = mychip->floor;
-	this->curchip = mychip->chip;
-
-	/* issue the Read0 or Read1 command depend on which half of the page
-	   we are accessing. Polling the Flash Ready bit after issue 3 bytes
-	   address in Sequence Read Mode, see Software Requirement 11.4 item 1.*/
-	DoC_Command(docptr, (from >> 8) & 1, CDSN_CTRL_WP);
-	DoC_Address(docptr, 3, from, CDSN_CTRL_WP, 0x00);
-	DoC_WaitReady(docptr);
-
-	/* init the ECC engine, see Reed-Solomon EDC/ECC 11.1 .*/
-	WriteDOC (DOC_ECC_RESET, docptr, ECCConf);
-	WriteDOC (DOC_ECC_EN, docptr, ECCConf);
-
-	/* Read the data via the internal pipeline through CDSN IO register,
-	   see Pipelined Read Operations 11.3 */
-	dummy = ReadDOC(docptr, ReadPipeInit);
-#ifndef USE_MEMCPY
-	for (i = 0; i < len-1; i++) {
-		/* N.B. you have to increase the source address in this way or the
-		   ECC logic will not work properly */
-		buf[i] = ReadDOC(docptr, Mil_CDSN_IO + (i & 0xff));
-	}
-#else
-	memcpy_fromio(buf, docptr + DoC_Mil_CDSN_IO, len - 1);
-#endif
-	buf[len - 1] = ReadDOC(docptr, LastDataRead);
-
-	/* Let the caller know we completed it */
-	*retlen = len;
-        ret = 0;
-
-	/* Read the ECC data from Spare Data Area,
-	   see Reed-Solomon EDC/ECC 11.1 */
-	dummy = ReadDOC(docptr, ReadPipeInit);
-#ifndef USE_MEMCPY
-	for (i = 0; i < 5; i++) {
-		/* N.B. you have to increase the source address in this way or the
-		   ECC logic will not work properly */
-		eccbuf[i] = ReadDOC(docptr, Mil_CDSN_IO + i);
-	}
-#else
-	memcpy_fromio(eccbuf, docptr + DoC_Mil_CDSN_IO, 5);
-#endif
-	eccbuf[5] = ReadDOC(docptr, LastDataRead);
-
-	/* Flush the pipeline */
-	dummy = ReadDOC(docptr, ECCConf);
-	dummy = ReadDOC(docptr, ECCConf);
-
-	/* Check the ECC Status */
-	if (ReadDOC(docptr, ECCConf) & 0x80) {
-		int nb_errors;
-		/* There was an ECC error */
-#ifdef ECC_DEBUG
-		printk("DiskOnChip ECC Error: Read at %lx\n", (long)from);
-#endif
-		/* Read the ECC syndrome through the DiskOnChip ECC logic.
-		   These syndrome will be all ZERO when there is no error */
-		for (i = 0; i < 6; i++) {
-			syndrome[i] = ReadDOC(docptr, ECCSyndrome0 + i);
-		}
-		nb_errors = doc_decode_ecc(buf, syndrome);
-#ifdef ECC_DEBUG
-		printk("ECC Errors corrected: %x\n", nb_errors);
-#endif
-		if (nb_errors < 0) {
-			/* We return error, but have actually done the read. Not that
-			   this can be told to user-space, via sys_read(), but at least
-			   MTD-aware stuff can know about it by checking *retlen */
-			ret = -EIO;
-		}
-	}
-
-#ifdef PSYCHO_DEBUG
-	printk("ECC DATA at %lx: %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X\n",
-	       (long)from, eccbuf[0], eccbuf[1], eccbuf[2], eccbuf[3],
-	       eccbuf[4], eccbuf[5]);
-#endif
-
-	/* disable the ECC engine */
-	WriteDOC(DOC_ECC_DIS, docptr , ECCConf);
-
-	return ret;
-}
-
-static int doc_write (struct mtd_info *mtd, loff_t to, size_t len,
-		      size_t *retlen, const u_char *buf)
-{
-	int i,ret = 0;
-	char eccbuf[6];
-	volatile char dummy;
-	struct DiskOnChip *this = mtd->priv;
-	void __iomem *docptr = this->virtadr;
-	struct Nand *mychip = &this->chips[to >> (this->chipshift)];
-
-#if 0
-	/* Don't allow a single write to cross a 512-byte block boundary */
-	if (to + len > ( (to | 0x1ff) + 1))
-		len = ((to | 0x1ff) + 1) - to;
-#else
-	/* Don't allow writes which aren't exactly one block */
-	if (to & 0x1ff || len != 0x200)
-		return -EINVAL;
-#endif
-
-	/* Find the chip which is to be used and select it */
-	if (this->curfloor != mychip->floor) {
-		DoC_SelectFloor(docptr, mychip->floor);
-		DoC_SelectChip(docptr, mychip->chip);
-	} else if (this->curchip != mychip->chip) {
-		DoC_SelectChip(docptr, mychip->chip);
-	}
-	this->curfloor = mychip->floor;
-	this->curchip = mychip->chip;
-
-	/* Reset the chip, see Software Requirement 11.4 item 1. */
-	DoC_Command(docptr, NAND_CMD_RESET, 0x00);
-	DoC_WaitReady(docptr);
-	/* Set device to main plane of flash */
-	DoC_Command(docptr, NAND_CMD_READ0, 0x00);
-
-	/* issue the Serial Data In command to initial the Page Program process */
-	DoC_Command(docptr, NAND_CMD_SEQIN, 0x00);
-	DoC_Address(docptr, 3, to, 0x00, 0x00);
-	DoC_WaitReady(docptr);
-
-	/* init the ECC engine, see Reed-Solomon EDC/ECC 11.1 .*/
-	WriteDOC (DOC_ECC_RESET, docptr, ECCConf);
-	WriteDOC (DOC_ECC_EN | DOC_ECC_RW, docptr, ECCConf);
-
-	/* Write the data via the internal pipeline through CDSN IO register,
-	   see Pipelined Write Operations 11.2 */
-#ifndef USE_MEMCPY
-	for (i = 0; i < len; i++) {
-		/* N.B. you have to increase the source address in this way or the
-		   ECC logic will not work properly */
-		WriteDOC(buf[i], docptr, Mil_CDSN_IO + i);
-	}
-#else
-	memcpy_toio(docptr + DoC_Mil_CDSN_IO, buf, len);
-#endif
-	WriteDOC(0x00, docptr, WritePipeTerm);
-
-	/* Write ECC data to flash, the ECC info is generated by the DiskOnChip ECC logic
-	   see Reed-Solomon EDC/ECC 11.1 */
-	WriteDOC(0, docptr, NOP);
-	WriteDOC(0, docptr, NOP);
-	WriteDOC(0, docptr, NOP);
-
-	/* Read the ECC data through the DiskOnChip ECC logic */
-	for (i = 0; i < 6; i++) {
-		eccbuf[i] = ReadDOC(docptr, ECCSyndrome0 + i);
-	}
-
-	/* ignore the ECC engine */
-	WriteDOC(DOC_ECC_DIS, docptr , ECCConf);
-
-#ifndef USE_MEMCPY
-	/* Write the ECC data to flash */
-	for (i = 0; i < 6; i++) {
-		/* N.B. you have to increase the source address in this way or the
-		   ECC logic will not work properly */
-		WriteDOC(eccbuf[i], docptr, Mil_CDSN_IO + i);
-	}
-#else
-	memcpy_toio(docptr + DoC_Mil_CDSN_IO, eccbuf, 6);
-#endif
-
-	/* write the block status BLOCK_USED (0x5555) at the end of ECC data
-	   FIXME: this is only a hack for programming the IPL area for LinuxBIOS
-	   and should be replace with proper codes in user space utilities */
-	WriteDOC(0x55, docptr, Mil_CDSN_IO);
-	WriteDOC(0x55, docptr, Mil_CDSN_IO + 1);
-
-	WriteDOC(0x00, docptr, WritePipeTerm);
-
-#ifdef PSYCHO_DEBUG
-	printk("OOB data at %lx is %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X\n",
-	       (long) to, eccbuf[0], eccbuf[1], eccbuf[2], eccbuf[3],
-	       eccbuf[4], eccbuf[5]);
-#endif
-
-	/* Commit the Page Program command and wait for ready
-	   see Software Requirement 11.4 item 1.*/
-	DoC_Command(docptr, NAND_CMD_PAGEPROG, 0x00);
-	DoC_WaitReady(docptr);
-
-	/* Read the status of the flash device through CDSN IO register
-	   see Software Requirement 11.4 item 5.*/
-	DoC_Command(docptr, NAND_CMD_STATUS, CDSN_CTRL_WP);
-	dummy = ReadDOC(docptr, ReadPipeInit);
-	DoC_Delay(docptr, 2);
-	if (ReadDOC(docptr, Mil_CDSN_IO) & 1) {
-		printk("Error programming flash\n");
-		/* Error in programming
-		   FIXME: implement Bad Block Replacement (in nftl.c ??) */
-		ret = -EIO;
-	}
-	dummy = ReadDOC(docptr, LastDataRead);
-
-	/* Let the caller know we completed it */
-	*retlen = len;
-
-	return ret;
-}
-
-static int doc_read_oob(struct mtd_info *mtd, loff_t ofs,
-			struct mtd_oob_ops *ops)
-{
-#ifndef USE_MEMCPY
-	int i;
-#endif
-	volatile char dummy;
-	struct DiskOnChip *this = mtd->priv;
-	void __iomem *docptr = this->virtadr;
-	struct Nand *mychip = &this->chips[ofs >> this->chipshift];
-	uint8_t *buf = ops->oobbuf;
-	size_t len = ops->len;
-
-	BUG_ON(ops->mode != MTD_OPS_PLACE_OOB);
-
-	ofs += ops->ooboffs;
-
-	/* Find the chip which is to be used and select it */
-	if (this->curfloor != mychip->floor) {
-		DoC_SelectFloor(docptr, mychip->floor);
-		DoC_SelectChip(docptr, mychip->chip);
-	} else if (this->curchip != mychip->chip) {
-		DoC_SelectChip(docptr, mychip->chip);
-	}
-	this->curfloor = mychip->floor;
-	this->curchip = mychip->chip;
-
-	/* disable the ECC engine */
-	WriteDOC (DOC_ECC_RESET, docptr, ECCConf);
-	WriteDOC (DOC_ECC_DIS, docptr, ECCConf);
-
-	/* issue the Read2 command to set the pointer to the Spare Data Area.
-	   Polling the Flash Ready bit after issue 3 bytes address in
-	   Sequence Read Mode, see Software Requirement 11.4 item 1.*/
-	DoC_Command(docptr, NAND_CMD_READOOB, CDSN_CTRL_WP);
-	DoC_Address(docptr, 3, ofs, CDSN_CTRL_WP, 0x00);
-	DoC_WaitReady(docptr);
-
-	/* Read the data out via the internal pipeline through CDSN IO register,
-	   see Pipelined Read Operations 11.3 */
-	dummy = ReadDOC(docptr, ReadPipeInit);
-#ifndef USE_MEMCPY
-	for (i = 0; i < len-1; i++) {
-		/* N.B. you have to increase the source address in this way or the
-		   ECC logic will not work properly */
-		buf[i] = ReadDOC(docptr, Mil_CDSN_IO + i);
-	}
-#else
-	memcpy_fromio(buf, docptr + DoC_Mil_CDSN_IO, len - 1);
-#endif
-	buf[len - 1] = ReadDOC(docptr, LastDataRead);
-
-	ops->retlen = len;
-
-	return 0;
-}
-
-static int doc_write_oob(struct mtd_info *mtd, loff_t ofs,
-			 struct mtd_oob_ops *ops)
-{
-#ifndef USE_MEMCPY
-	int i;
-#endif
-	volatile char dummy;
-	int ret = 0;
-	struct DiskOnChip *this = mtd->priv;
-	void __iomem *docptr = this->virtadr;
-	struct Nand *mychip = &this->chips[ofs >> this->chipshift];
-	uint8_t *buf = ops->oobbuf;
-	size_t len = ops->len;
-
-	BUG_ON(ops->mode != MTD_OPS_PLACE_OOB);
-
-	ofs += ops->ooboffs;
-
-	/* Find the chip which is to be used and select it */
-	if (this->curfloor != mychip->floor) {
-		DoC_SelectFloor(docptr, mychip->floor);
-		DoC_SelectChip(docptr, mychip->chip);
-	} else if (this->curchip != mychip->chip) {
-		DoC_SelectChip(docptr, mychip->chip);
-	}
-	this->curfloor = mychip->floor;
-	this->curchip = mychip->chip;
-
-	/* disable the ECC engine */
-	WriteDOC (DOC_ECC_RESET, docptr, ECCConf);
-	WriteDOC (DOC_ECC_DIS, docptr, ECCConf);
-
-	/* Reset the chip, see Software Requirement 11.4 item 1. */
-	DoC_Command(docptr, NAND_CMD_RESET, CDSN_CTRL_WP);
-	DoC_WaitReady(docptr);
-	/* issue the Read2 command to set the pointer to the Spare Data Area. */
-	DoC_Command(docptr, NAND_CMD_READOOB, CDSN_CTRL_WP);
-
-	/* issue the Serial Data In command to initial the Page Program process */
-	DoC_Command(docptr, NAND_CMD_SEQIN, 0x00);
-	DoC_Address(docptr, 3, ofs, 0x00, 0x00);
-
-	/* Write the data via the internal pipeline through CDSN IO register,
-	   see Pipelined Write Operations 11.2 */
-#ifndef USE_MEMCPY
-	for (i = 0; i < len; i++) {
-		/* N.B. you have to increase the source address in this way or the
-		   ECC logic will not work properly */
-		WriteDOC(buf[i], docptr, Mil_CDSN_IO + i);
-	}
-#else
-	memcpy_toio(docptr + DoC_Mil_CDSN_IO, buf, len);
-#endif
-	WriteDOC(0x00, docptr, WritePipeTerm);
-
-	/* Commit the Page Program command and wait for ready
-	   see Software Requirement 11.4 item 1.*/
-	DoC_Command(docptr, NAND_CMD_PAGEPROG, 0x00);
-	DoC_WaitReady(docptr);
-
-	/* Read the status of the flash device through CDSN IO register
-	   see Software Requirement 11.4 item 5.*/
-	DoC_Command(docptr, NAND_CMD_STATUS, 0x00);
-	dummy = ReadDOC(docptr, ReadPipeInit);
-	DoC_Delay(docptr, 2);
-	if (ReadDOC(docptr, Mil_CDSN_IO) & 1) {
-		printk("Error programming oob data\n");
-		/* FIXME: implement Bad Block Replacement (in nftl.c ??) */
-		ops->retlen = 0;
-		ret = -EIO;
-	}
-	dummy = ReadDOC(docptr, LastDataRead);
-
-	ops->retlen = len;
-
-	return ret;
-}
-
-int doc_erase (struct mtd_info *mtd, struct erase_info *instr)
-{
-	volatile char dummy;
-	struct DiskOnChip *this = mtd->priv;
-	__u32 ofs = instr->addr;
-	__u32 len = instr->len;
-	void __iomem *docptr = this->virtadr;
-	struct Nand *mychip = &this->chips[ofs >> this->chipshift];
-
-	if (len != mtd->erasesize)
-		printk(KERN_WARNING "Erase not right size (%x != %x)n",
-		       len, mtd->erasesize);
-
-	/* Find the chip which is to be used and select it */
-	if (this->curfloor != mychip->floor) {
-		DoC_SelectFloor(docptr, mychip->floor);
-		DoC_SelectChip(docptr, mychip->chip);
-	} else if (this->curchip != mychip->chip) {
-		DoC_SelectChip(docptr, mychip->chip);
-	}
-	this->curfloor = mychip->floor;
-	this->curchip = mychip->chip;
-
-	instr->state = MTD_ERASE_PENDING;
-
-	/* issue the Erase Setup command */
-	DoC_Command(docptr, NAND_CMD_ERASE1, 0x00);
-	DoC_Address(docptr, 2, ofs, 0x00, 0x00);
-
-	/* Commit the Erase Start command and wait for ready
-	   see Software Requirement 11.4 item 1.*/
-	DoC_Command(docptr, NAND_CMD_ERASE2, 0x00);
-	DoC_WaitReady(docptr);
-
-	instr->state = MTD_ERASING;
-
-	/* Read the status of the flash device through CDSN IO register
-	   see Software Requirement 11.4 item 5.
-	   FIXME: it seems that we are not wait long enough, some blocks are not
-	   erased fully */
-	DoC_Command(docptr, NAND_CMD_STATUS, CDSN_CTRL_WP);
-	dummy = ReadDOC(docptr, ReadPipeInit);
-	DoC_Delay(docptr, 2);
-	if (ReadDOC(docptr, Mil_CDSN_IO) & 1) {
-		printk("Error Erasing at 0x%x\n", ofs);
-		/* There was an error
-		   FIXME: implement Bad Block Replacement (in nftl.c ??) */
-		instr->state = MTD_ERASE_FAILED;
-	} else
-		instr->state = MTD_ERASE_DONE;
-	dummy = ReadDOC(docptr, LastDataRead);
-
-	mtd_erase_callback(instr);
-
-	return 0;
-}
-
-/****************************************************************************
- *
- * Module stuff
- *
- ****************************************************************************/
-
-static void __exit cleanup_doc2001(void)
-{
-	struct mtd_info *mtd;
-	struct DiskOnChip *this;
-
-	while ((mtd=docmillist)) {
-		this = mtd->priv;
-		docmillist = this->nextdoc;
-
-		mtd_device_unregister(mtd);
-
-		iounmap(this->virtadr);
-		kfree(this->chips);
-		kfree(mtd);
-	}
-}
-
-module_exit(cleanup_doc2001);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("David Woodhouse <dwmw2@infradead.org> et al.");
-MODULE_DESCRIPTION("Alternative driver for DiskOnChip Millennium");
diff --git a/drivers/mtd/devices/doc2001plus.c b/drivers/mtd/devices/doc2001plus.c
deleted file mode 100644
index 4f2220ad8924..000000000000
--- a/drivers/mtd/devices/doc2001plus.c
+++ /dev/null
@@ -1,1080 +0,0 @@
-/*
- * Linux driver for Disk-On-Chip Millennium Plus
- *
- * (c) 2002-2003 Greg Ungerer <gerg@snapgear.com>
- * (c) 2002-2003 SnapGear Inc
- * (c) 1999 Machine Vision Holdings, Inc.
- * (c) 1999, 2000 David Woodhouse <dwmw2@infradead.org>
- *
- * Released under GPL
- */
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <asm/errno.h>
-#include <asm/io.h>
-#include <asm/uaccess.h>
-#include <linux/delay.h>
-#include <linux/slab.h>
-#include <linux/init.h>
-#include <linux/types.h>
-#include <linux/bitops.h>
-
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/nand.h>
-#include <linux/mtd/doc2000.h>
-
-/* #define ECC_DEBUG */
-
-/* I have no idea why some DoC chips can not use memcop_form|to_io().
- * This may be due to the different revisions of the ASIC controller built-in or
- * simplily a QA/Bug issue. Who knows ?? If you have trouble, please uncomment
- * this:*/
-#undef USE_MEMCPY
-
-static int doc_read(struct mtd_info *mtd, loff_t from, size_t len,
-		size_t *retlen, u_char *buf);
-static int doc_write(struct mtd_info *mtd, loff_t to, size_t len,
-		size_t *retlen, const u_char *buf);
-static int doc_read_oob(struct mtd_info *mtd, loff_t ofs,
-			struct mtd_oob_ops *ops);
-static int doc_write_oob(struct mtd_info *mtd, loff_t ofs,
-			 struct mtd_oob_ops *ops);
-static int doc_erase (struct mtd_info *mtd, struct erase_info *instr);
-
-static struct mtd_info *docmilpluslist = NULL;
-
-
-/* Perform the required delay cycles by writing to the NOP register */
-static void DoC_Delay(void __iomem * docptr, int cycles)
-{
-	int i;
-
-	for (i = 0; (i < cycles); i++)
-		WriteDOC(0, docptr, Mplus_NOP);
-}
-
-#define	CDSN_CTRL_FR_B_MASK	(CDSN_CTRL_FR_B0 | CDSN_CTRL_FR_B1)
-
-/* DOC_WaitReady: Wait for RDY line to be asserted by the flash chip */
-static int _DoC_WaitReady(void __iomem * docptr)
-{
-	unsigned int c = 0xffff;
-
-	pr_debug("_DoC_WaitReady called for out-of-line wait\n");
-
-	/* Out-of-line routine to wait for chip response */
-	while (((ReadDOC(docptr, Mplus_FlashControl) & CDSN_CTRL_FR_B_MASK) != CDSN_CTRL_FR_B_MASK) && --c)
-		;
-
-	if (c == 0)
-		pr_debug("_DoC_WaitReady timed out.\n");
-
-	return (c == 0);
-}
-
-static inline int DoC_WaitReady(void __iomem * docptr)
-{
-	/* This is inline, to optimise the common case, where it's ready instantly */
-	int ret = 0;
-
-	/* read form NOP register should be issued prior to the read from CDSNControl
-	   see Software Requirement 11.4 item 2. */
-	DoC_Delay(docptr, 4);
-
-	if ((ReadDOC(docptr, Mplus_FlashControl) & CDSN_CTRL_FR_B_MASK) != CDSN_CTRL_FR_B_MASK)
-		/* Call the out-of-line routine to wait */
-		ret = _DoC_WaitReady(docptr);
-
-	return ret;
-}
-
-/* For some reason the Millennium Plus seems to occasionally put itself
- * into reset mode. For me this happens randomly, with no pattern that I
- * can detect. M-systems suggest always check this on any block level
- * operation and setting to normal mode if in reset mode.
- */
-static inline void DoC_CheckASIC(void __iomem * docptr)
-{
-	/* Make sure the DoC is in normal mode */
-	if ((ReadDOC(docptr, Mplus_DOCControl) & DOC_MODE_NORMAL) == 0) {
-		WriteDOC((DOC_MODE_NORMAL | DOC_MODE_MDWREN), docptr, Mplus_DOCControl);
-		WriteDOC(~(DOC_MODE_NORMAL | DOC_MODE_MDWREN), docptr, Mplus_CtrlConfirm);
-	}
-}
-
-/* DoC_Command: Send a flash command to the flash chip through the Flash
- * command register. Need 2 Write Pipeline Terminates to complete send.
- */
-static void DoC_Command(void __iomem * docptr, unsigned char command,
-			       unsigned char xtraflags)
-{
-	WriteDOC(command, docptr, Mplus_FlashCmd);
-	WriteDOC(command, docptr, Mplus_WritePipeTerm);
-	WriteDOC(command, docptr, Mplus_WritePipeTerm);
-}
-
-/* DoC_Address: Set the current address for the flash chip through the Flash
- * Address register. Need 2 Write Pipeline Terminates to complete send.
- */
-static inline void DoC_Address(struct DiskOnChip *doc, int numbytes,
-			       unsigned long ofs, unsigned char xtraflags1,
-			       unsigned char xtraflags2)
-{
-	void __iomem * docptr = doc->virtadr;
-
-	/* Allow for possible Mill Plus internal flash interleaving */
-	ofs >>= doc->interleave;
-
-	switch (numbytes) {
-	case 1:
-		/* Send single byte, bits 0-7. */
-		WriteDOC(ofs & 0xff, docptr, Mplus_FlashAddress);
-		break;
-	case 2:
-		/* Send bits 9-16 followed by 17-23 */
-		WriteDOC((ofs >> 9)  & 0xff, docptr, Mplus_FlashAddress);
-		WriteDOC((ofs >> 17) & 0xff, docptr, Mplus_FlashAddress);
-		break;
-	case 3:
-		/* Send 0-7, 9-16, then 17-23 */
-		WriteDOC(ofs & 0xff, docptr, Mplus_FlashAddress);
-		WriteDOC((ofs >> 9)  & 0xff, docptr, Mplus_FlashAddress);
-		WriteDOC((ofs >> 17) & 0xff, docptr, Mplus_FlashAddress);
-		break;
-	default:
-		return;
-	}
-
-	WriteDOC(0x00, docptr, Mplus_WritePipeTerm);
-	WriteDOC(0x00, docptr, Mplus_WritePipeTerm);
-}
-
-/* DoC_SelectChip: Select a given flash chip within the current floor */
-static int DoC_SelectChip(void __iomem * docptr, int chip)
-{
-	/* No choice for flash chip on Millennium Plus */
-	return 0;
-}
-
-/* DoC_SelectFloor: Select a given floor (bank of flash chips) */
-static int DoC_SelectFloor(void __iomem * docptr, int floor)
-{
-	WriteDOC((floor & 0x3), docptr, Mplus_DeviceSelect);
-	return 0;
-}
-
-/*
- * Translate the given offset into the appropriate command and offset.
- * This does the mapping using the 16bit interleave layout defined by
- * M-Systems, and looks like this for a sector pair:
- *  +-----------+-------+-------+-------+--------------+---------+-----------+
- *  | 0 --- 511 |512-517|518-519|520-521| 522 --- 1033 |1034-1039|1040 - 1055|
- *  +-----------+-------+-------+-------+--------------+---------+-----------+
- *  | Data 0    | ECC 0 |Flags0 |Flags1 | Data 1       |ECC 1    | OOB 1 + 2 |
- *  +-----------+-------+-------+-------+--------------+---------+-----------+
- */
-/* FIXME: This lives in INFTL not here. Other users of flash devices
-   may not want it */
-static unsigned int DoC_GetDataOffset(struct mtd_info *mtd, loff_t *from)
-{
-	struct DiskOnChip *this = mtd->priv;
-
-	if (this->interleave) {
-		unsigned int ofs = *from & 0x3ff;
-		unsigned int cmd;
-
-		if (ofs < 512) {
-			cmd = NAND_CMD_READ0;
-			ofs &= 0x1ff;
-		} else if (ofs < 1014) {
-			cmd = NAND_CMD_READ1;
-			ofs = (ofs & 0x1ff) + 10;
-		} else {
-			cmd = NAND_CMD_READOOB;
-			ofs = ofs - 1014;
-		}
-
-		*from = (*from & ~0x3ff) | ofs;
-		return cmd;
-	} else {
-		/* No interleave */
-		if ((*from) & 0x100)
-			return NAND_CMD_READ1;
-		return NAND_CMD_READ0;
-	}
-}
-
-static unsigned int DoC_GetECCOffset(struct mtd_info *mtd, loff_t *from)
-{
-	unsigned int ofs, cmd;
-
-	if (*from & 0x200) {
-		cmd = NAND_CMD_READOOB;
-		ofs = 10 + (*from & 0xf);
-	} else {
-		cmd = NAND_CMD_READ1;
-		ofs = (*from & 0xf);
-	}
-
-	*from = (*from & ~0x3ff) | ofs;
-	return cmd;
-}
-
-static unsigned int DoC_GetFlagsOffset(struct mtd_info *mtd, loff_t *from)
-{
-	unsigned int ofs, cmd;
-
-	cmd = NAND_CMD_READ1;
-	ofs = (*from & 0x200) ? 8 : 6;
-	*from = (*from & ~0x3ff) | ofs;
-	return cmd;
-}
-
-static unsigned int DoC_GetHdrOffset(struct mtd_info *mtd, loff_t *from)
-{
-	unsigned int ofs, cmd;
-
-	cmd = NAND_CMD_READOOB;
-	ofs = (*from & 0x200) ? 24 : 16;
-	*from = (*from & ~0x3ff) | ofs;
-	return cmd;
-}
-
-static inline void MemReadDOC(void __iomem * docptr, unsigned char *buf, int len)
-{
-#ifndef USE_MEMCPY
-	int i;
-	for (i = 0; i < len; i++)
-		buf[i] = ReadDOC(docptr, Mil_CDSN_IO + i);
-#else
-	memcpy_fromio(buf, docptr + DoC_Mil_CDSN_IO, len);
-#endif
-}
-
-static inline void MemWriteDOC(void __iomem * docptr, unsigned char *buf, int len)
-{
-#ifndef USE_MEMCPY
-	int i;
-	for (i = 0; i < len; i++)
-		WriteDOC(buf[i], docptr, Mil_CDSN_IO + i);
-#else
-	memcpy_toio(docptr + DoC_Mil_CDSN_IO, buf, len);
-#endif
-}
-
-/* DoC_IdentChip: Identify a given NAND chip given {floor,chip} */
-static int DoC_IdentChip(struct DiskOnChip *doc, int floor, int chip)
-{
-	int mfr, id, i, j;
-	volatile char dummy;
-	void __iomem * docptr = doc->virtadr;
-
-	/* Page in the required floor/chip */
-	DoC_SelectFloor(docptr, floor);
-	DoC_SelectChip(docptr, chip);
-
-	/* Millennium Plus bus cycle sequence as per figure 2, section 2.4 */
-	WriteDOC((DOC_FLASH_CE | DOC_FLASH_WP), docptr, Mplus_FlashSelect);
-
-	/* Reset the chip, see Software Requirement 11.4 item 1. */
-	DoC_Command(docptr, NAND_CMD_RESET, 0);
-	DoC_WaitReady(docptr);
-
-	/* Read the NAND chip ID: 1. Send ReadID command */
-	DoC_Command(docptr, NAND_CMD_READID, 0);
-
-	/* Read the NAND chip ID: 2. Send address byte zero */
-	DoC_Address(doc, 1, 0x00, 0, 0x00);
-
-	WriteDOC(0, docptr, Mplus_FlashControl);
-	DoC_WaitReady(docptr);
-
-	/* Read the manufacturer and device id codes of the flash device through
-	   CDSN IO register see Software Requirement 11.4 item 5.*/
-	dummy = ReadDOC(docptr, Mplus_ReadPipeInit);
-	dummy = ReadDOC(docptr, Mplus_ReadPipeInit);
-
-	mfr = ReadDOC(docptr, Mil_CDSN_IO);
-	if (doc->interleave)
-		dummy = ReadDOC(docptr, Mil_CDSN_IO); /* 2 way interleave */
-
-	id  = ReadDOC(docptr, Mil_CDSN_IO);
-	if (doc->interleave)
-		dummy = ReadDOC(docptr, Mil_CDSN_IO); /* 2 way interleave */
-
-	dummy = ReadDOC(docptr, Mplus_LastDataRead);
-	dummy = ReadDOC(docptr, Mplus_LastDataRead);
-
-	/* Disable flash internally */
-	WriteDOC(0, docptr, Mplus_FlashSelect);
-
-	/* No response - return failure */
-	if (mfr == 0xff || mfr == 0)
-		return 0;
-
-	for (i = 0; nand_flash_ids[i].name != NULL; i++) {
-		if (id == nand_flash_ids[i].id) {
-			/* Try to identify manufacturer */
-			for (j = 0; nand_manuf_ids[j].id != 0x0; j++) {
-				if (nand_manuf_ids[j].id == mfr)
-					break;
-			}
-			printk(KERN_INFO "Flash chip found: Manufacturer ID: %2.2X, "
-			       "Chip ID: %2.2X (%s:%s)\n", mfr, id,
-			       nand_manuf_ids[j].name, nand_flash_ids[i].name);
-			doc->mfr = mfr;
-			doc->id = id;
-			doc->chipshift = ffs((nand_flash_ids[i].chipsize << 20)) - 1;
-			doc->erasesize = nand_flash_ids[i].erasesize << doc->interleave;
-			break;
-		}
-	}
-
-	if (nand_flash_ids[i].name == NULL)
-		return 0;
-	return 1;
-}
-
-/* DoC_ScanChips: Find all NAND chips present in a DiskOnChip, and identify them */
-static void DoC_ScanChips(struct DiskOnChip *this)
-{
-	int floor, chip;
-	int numchips[MAX_FLOORS_MPLUS];
-	int ret;
-
-	this->numchips = 0;
-	this->mfr = 0;
-	this->id = 0;
-
-	/* Work out the intended interleave setting */
-	this->interleave = 0;
-	if (this->ChipID == DOC_ChipID_DocMilPlus32)
-		this->interleave = 1;
-
-	/* Check the ASIC agrees */
-	if ( (this->interleave << 2) !=
-	     (ReadDOC(this->virtadr, Mplus_Configuration) & 4)) {
-		u_char conf = ReadDOC(this->virtadr, Mplus_Configuration);
-		printk(KERN_NOTICE "Setting DiskOnChip Millennium Plus interleave to %s\n",
-		       this->interleave?"on (16-bit)":"off (8-bit)");
-		conf ^= 4;
-		WriteDOC(conf, this->virtadr, Mplus_Configuration);
-	}
-
-	/* For each floor, find the number of valid chips it contains */
-	for (floor = 0,ret = 1; floor < MAX_FLOORS_MPLUS; floor++) {
-		numchips[floor] = 0;
-		for (chip = 0; chip < MAX_CHIPS_MPLUS && ret != 0; chip++) {
-			ret = DoC_IdentChip(this, floor, chip);
-			if (ret) {
-				numchips[floor]++;
-				this->numchips++;
-			}
-		}
-	}
-	/* If there are none at all that we recognise, bail */
-	if (!this->numchips) {
-		printk("No flash chips recognised.\n");
-		return;
-	}
-
-	/* Allocate an array to hold the information for each chip */
-	this->chips = kmalloc(sizeof(struct Nand) * this->numchips, GFP_KERNEL);
-	if (!this->chips){
-		printk("MTD: No memory for allocating chip info structures\n");
-		return;
-	}
-
-	/* Fill out the chip array with {floor, chipno} for each
-	 * detected chip in the device. */
-	for (floor = 0, ret = 0; floor < MAX_FLOORS_MPLUS; floor++) {
-		for (chip = 0 ; chip < numchips[floor] ; chip++) {
-			this->chips[ret].floor = floor;
-			this->chips[ret].chip = chip;
-			this->chips[ret].curadr = 0;
-			this->chips[ret].curmode = 0x50;
-			ret++;
-		}
-	}
-
-	/* Calculate and print the total size of the device */
-	this->totlen = this->numchips * (1 << this->chipshift);
-	printk(KERN_INFO "%d flash chips found. Total DiskOnChip size: %ld MiB\n",
-	       this->numchips ,this->totlen >> 20);
-}
-
-static int DoCMilPlus_is_alias(struct DiskOnChip *doc1, struct DiskOnChip *doc2)
-{
-	int tmp1, tmp2, retval;
-
-	if (doc1->physadr == doc2->physadr)
-		return 1;
-
-	/* Use the alias resolution register which was set aside for this
-	 * purpose. If it's value is the same on both chips, they might
-	 * be the same chip, and we write to one and check for a change in
-	 * the other. It's unclear if this register is usuable in the
-	 * DoC 2000 (it's in the Millennium docs), but it seems to work. */
-	tmp1 = ReadDOC(doc1->virtadr, Mplus_AliasResolution);
-	tmp2 = ReadDOC(doc2->virtadr, Mplus_AliasResolution);
-	if (tmp1 != tmp2)
-		return 0;
-
-	WriteDOC((tmp1+1) % 0xff, doc1->virtadr, Mplus_AliasResolution);
-	tmp2 = ReadDOC(doc2->virtadr, Mplus_AliasResolution);
-	if (tmp2 == (tmp1+1) % 0xff)
-		retval = 1;
-	else
-		retval = 0;
-
-	/* Restore register contents.  May not be necessary, but do it just to
-	 * be safe. */
-	WriteDOC(tmp1, doc1->virtadr, Mplus_AliasResolution);
-
-	return retval;
-}
-
-/* This routine is found from the docprobe code by symbol_get(),
- * which will bump the use count of this module. */
-void DoCMilPlus_init(struct mtd_info *mtd)
-{
-	struct DiskOnChip *this = mtd->priv;
-	struct DiskOnChip *old = NULL;
-
-	/* We must avoid being called twice for the same device. */
-	if (docmilpluslist)
-		old = docmilpluslist->priv;
-
-	while (old) {
-		if (DoCMilPlus_is_alias(this, old)) {
-			printk(KERN_NOTICE "Ignoring DiskOnChip Millennium "
-				"Plus at 0x%lX - already configured\n",
-				this->physadr);
-			iounmap(this->virtadr);
-			kfree(mtd);
-			return;
-		}
-		if (old->nextdoc)
-			old = old->nextdoc->priv;
-		else
-			old = NULL;
-	}
-
-	mtd->name = "DiskOnChip Millennium Plus";
-	printk(KERN_NOTICE "DiskOnChip Millennium Plus found at "
-		"address 0x%lX\n", this->physadr);
-
-	mtd->type = MTD_NANDFLASH;
-	mtd->flags = MTD_CAP_NANDFLASH;
-	mtd->writebufsize = mtd->writesize = 512;
-	mtd->oobsize = 16;
-	mtd->ecc_strength = 2;
-	mtd->owner = THIS_MODULE;
-	mtd->_erase = doc_erase;
-	mtd->_read = doc_read;
-	mtd->_write = doc_write;
-	mtd->_read_oob = doc_read_oob;
-	mtd->_write_oob = doc_write_oob;
-	this->curfloor = -1;
-	this->curchip = -1;
-
-	/* Ident all the chips present. */
-	DoC_ScanChips(this);
-
-	if (!this->totlen) {
-		kfree(mtd);
-		iounmap(this->virtadr);
-	} else {
-		this->nextdoc = docmilpluslist;
-		docmilpluslist = mtd;
-		mtd->size  = this->totlen;
-		mtd->erasesize = this->erasesize;
-		mtd_device_register(mtd, NULL, 0);
-		return;
-	}
-}
-EXPORT_SYMBOL_GPL(DoCMilPlus_init);
-
-#if 0
-static int doc_dumpblk(struct mtd_info *mtd, loff_t from)
-{
-	int i;
-	loff_t fofs;
-	struct DiskOnChip *this = mtd->priv;
-	void __iomem * docptr = this->virtadr;
-	struct Nand *mychip = &this->chips[from >> (this->chipshift)];
-	unsigned char *bp, buf[1056];
-	char c[32];
-
-	from &= ~0x3ff;
-
-	/* Don't allow read past end of device */
-	if (from >= this->totlen)
-		return -EINVAL;
-
-	DoC_CheckASIC(docptr);
-
-	/* Find the chip which is to be used and select it */
-	if (this->curfloor != mychip->floor) {
-		DoC_SelectFloor(docptr, mychip->floor);
-		DoC_SelectChip(docptr, mychip->chip);
-	} else if (this->curchip != mychip->chip) {
-		DoC_SelectChip(docptr, mychip->chip);
-	}
-	this->curfloor = mychip->floor;
-	this->curchip = mychip->chip;
-
-	/* Millennium Plus bus cycle sequence as per figure 2, section 2.4 */
-	WriteDOC((DOC_FLASH_CE | DOC_FLASH_WP), docptr, Mplus_FlashSelect);
-
-	/* Reset the chip, see Software Requirement 11.4 item 1. */
-	DoC_Command(docptr, NAND_CMD_RESET, 0);
-	DoC_WaitReady(docptr);
-
-	fofs = from;
-	DoC_Command(docptr, DoC_GetDataOffset(mtd, &fofs), 0);
-	DoC_Address(this, 3, fofs, 0, 0x00);
-	WriteDOC(0, docptr, Mplus_FlashControl);
-	DoC_WaitReady(docptr);
-
-	/* disable the ECC engine */
-	WriteDOC(DOC_ECC_RESET, docptr, Mplus_ECCConf);
-
-	ReadDOC(docptr, Mplus_ReadPipeInit);
-	ReadDOC(docptr, Mplus_ReadPipeInit);
-
-	/* Read the data via the internal pipeline through CDSN IO
-	   register, see Pipelined Read Operations 11.3 */
-	MemReadDOC(docptr, buf, 1054);
-	buf[1054] = ReadDOC(docptr, Mplus_LastDataRead);
-	buf[1055] = ReadDOC(docptr, Mplus_LastDataRead);
-
-	memset(&c[0], 0, sizeof(c));
-	printk("DUMP OFFSET=%x:\n", (int)from);
-
-        for (i = 0, bp = &buf[0]; (i < 1056); i++) {
-                if ((i % 16) == 0)
-                        printk("%08x: ", i);
-                printk(" %02x", *bp);
-                c[(i & 0xf)] = ((*bp >= 0x20) && (*bp <= 0x7f)) ? *bp : '.';
-                bp++;
-                if (((i + 1) % 16) == 0)
-                        printk("    %s\n", c);
-        }
-	printk("\n");
-
-	/* Disable flash internally */
-	WriteDOC(0, docptr, Mplus_FlashSelect);
-
-	return 0;
-}
-#endif
-
-static int doc_read(struct mtd_info *mtd, loff_t from, size_t len,
-		    size_t *retlen, u_char *buf)
-{
-	int ret, i;
-	volatile char dummy;
-	loff_t fofs;
-	unsigned char syndrome[6], eccbuf[6];
-	struct DiskOnChip *this = mtd->priv;
-	void __iomem * docptr = this->virtadr;
-	struct Nand *mychip = &this->chips[from >> (this->chipshift)];
-
-	/* Don't allow a single read to cross a 512-byte block boundary */
-	if (from + len > ((from | 0x1ff) + 1))
-		len = ((from | 0x1ff) + 1) - from;
-
-	DoC_CheckASIC(docptr);
-
-	/* Find the chip which is to be used and select it */
-	if (this->curfloor != mychip->floor) {
-		DoC_SelectFloor(docptr, mychip->floor);
-		DoC_SelectChip(docptr, mychip->chip);
-	} else if (this->curchip != mychip->chip) {
-		DoC_SelectChip(docptr, mychip->chip);
-	}
-	this->curfloor = mychip->floor;
-	this->curchip = mychip->chip;
-
-	/* Millennium Plus bus cycle sequence as per figure 2, section 2.4 */
-	WriteDOC((DOC_FLASH_CE | DOC_FLASH_WP), docptr, Mplus_FlashSelect);
-
-	/* Reset the chip, see Software Requirement 11.4 item 1. */
-	DoC_Command(docptr, NAND_CMD_RESET, 0);
-	DoC_WaitReady(docptr);
-
-	fofs = from;
-	DoC_Command(docptr, DoC_GetDataOffset(mtd, &fofs), 0);
-	DoC_Address(this, 3, fofs, 0, 0x00);
-	WriteDOC(0, docptr, Mplus_FlashControl);
-	DoC_WaitReady(docptr);
-
-	/* init the ECC engine, see Reed-Solomon EDC/ECC 11.1 .*/
-	WriteDOC(DOC_ECC_RESET, docptr, Mplus_ECCConf);
-	WriteDOC(DOC_ECC_EN, docptr, Mplus_ECCConf);
-
-	/* Let the caller know we completed it */
-	*retlen = len;
-	ret = 0;
-
-	ReadDOC(docptr, Mplus_ReadPipeInit);
-	ReadDOC(docptr, Mplus_ReadPipeInit);
-
-	/* Read the data via the internal pipeline through CDSN IO
-	   register, see Pipelined Read Operations 11.3 */
-	MemReadDOC(docptr, buf, len);
-
-	/* Read the ECC data following raw data */
-	MemReadDOC(docptr, eccbuf, 4);
-	eccbuf[4] = ReadDOC(docptr, Mplus_LastDataRead);
-	eccbuf[5] = ReadDOC(docptr, Mplus_LastDataRead);
-
-	/* Flush the pipeline */
-	dummy = ReadDOC(docptr, Mplus_ECCConf);
-	dummy = ReadDOC(docptr, Mplus_ECCConf);
-
-	/* Check the ECC Status */
-	if (ReadDOC(docptr, Mplus_ECCConf) & 0x80) {
-		int nb_errors;
-		/* There was an ECC error */
-#ifdef ECC_DEBUG
-		printk("DiskOnChip ECC Error: Read at %lx\n", (long)from);
-#endif
-		/* Read the ECC syndrome through the DiskOnChip ECC logic.
-		   These syndrome will be all ZERO when there is no error */
-		for (i = 0; i < 6; i++)
-			syndrome[i] = ReadDOC(docptr, Mplus_ECCSyndrome0 + i);
-
-		nb_errors = doc_decode_ecc(buf, syndrome);
-#ifdef ECC_DEBUG
-		printk("ECC Errors corrected: %x\n", nb_errors);
-#endif
-		if (nb_errors < 0) {
-			/* We return error, but have actually done the
-			   read. Not that this can be told to user-space, via
-			   sys_read(), but at least MTD-aware stuff can know
-			   about it by checking *retlen */
-#ifdef ECC_DEBUG
-			printk("%s(%d): Millennium Plus ECC error (from=0x%x:\n",
-				__FILE__, __LINE__, (int)from);
-			printk("        syndrome= %*phC\n", 6, syndrome);
-			printk("        eccbuf= %*phC\n", 6, eccbuf);
-#endif
-				ret = -EIO;
-		}
-	}
-
-#ifdef PSYCHO_DEBUG
-	printk("ECC DATA at %lx: %*ph\n", (long)from, 6, eccbuf);
-#endif
-	/* disable the ECC engine */
-	WriteDOC(DOC_ECC_DIS, docptr , Mplus_ECCConf);
-
-	/* Disable flash internally */
-	WriteDOC(0, docptr, Mplus_FlashSelect);
-
-	return ret;
-}
-
-static int doc_write(struct mtd_info *mtd, loff_t to, size_t len,
-		     size_t *retlen, const u_char *buf)
-{
-	int i, before, ret = 0;
-	loff_t fto;
-	volatile char dummy;
-	char eccbuf[6];
-	struct DiskOnChip *this = mtd->priv;
-	void __iomem * docptr = this->virtadr;
-	struct Nand *mychip = &this->chips[to >> (this->chipshift)];
-
-	/* Don't allow writes which aren't exactly one block (512 bytes) */
-	if ((to & 0x1ff) || (len != 0x200))
-		return -EINVAL;
-
-	/* Determine position of OOB flags, before or after data */
-	before = (this->interleave && (to & 0x200));
-
-	DoC_CheckASIC(docptr);
-
-	/* Find the chip which is to be used and select it */
-	if (this->curfloor != mychip->floor) {
-		DoC_SelectFloor(docptr, mychip->floor);
-		DoC_SelectChip(docptr, mychip->chip);
-	} else if (this->curchip != mychip->chip) {
-		DoC_SelectChip(docptr, mychip->chip);
-	}
-	this->curfloor = mychip->floor;
-	this->curchip = mychip->chip;
-
-	/* Millennium Plus bus cycle sequence as per figure 2, section 2.4 */
-	WriteDOC(DOC_FLASH_CE, docptr, Mplus_FlashSelect);
-
-	/* Reset the chip, see Software Requirement 11.4 item 1. */
-	DoC_Command(docptr, NAND_CMD_RESET, 0);
-	DoC_WaitReady(docptr);
-
-	/* Set device to appropriate plane of flash */
-	fto = to;
-	WriteDOC(DoC_GetDataOffset(mtd, &fto), docptr, Mplus_FlashCmd);
-
-	/* On interleaved devices the flags for 2nd half 512 are before data */
-	if (before)
-		fto -= 2;
-
-	/* issue the Serial Data In command to initial the Page Program process */
-	DoC_Command(docptr, NAND_CMD_SEQIN, 0x00);
-	DoC_Address(this, 3, fto, 0x00, 0x00);
-
-	/* Disable the ECC engine */
-	WriteDOC(DOC_ECC_RESET, docptr, Mplus_ECCConf);
-
-	if (before) {
-		/* Write the block status BLOCK_USED (0x5555) */
-		WriteDOC(0x55, docptr, Mil_CDSN_IO);
-		WriteDOC(0x55, docptr, Mil_CDSN_IO);
-	}
-
-	/* init the ECC engine, see Reed-Solomon EDC/ECC 11.1 .*/
-	WriteDOC(DOC_ECC_EN | DOC_ECC_RW, docptr, Mplus_ECCConf);
-
-	MemWriteDOC(docptr, (unsigned char *) buf, len);
-
-	/* Write ECC data to flash, the ECC info is generated by
-	   the DiskOnChip ECC logic see Reed-Solomon EDC/ECC 11.1 */
-	DoC_Delay(docptr, 3);
-
-	/* Read the ECC data through the DiskOnChip ECC logic */
-	for (i = 0; i < 6; i++)
-		eccbuf[i] = ReadDOC(docptr, Mplus_ECCSyndrome0 + i);
-
-	/* disable the ECC engine */
-	WriteDOC(DOC_ECC_DIS, docptr, Mplus_ECCConf);
-
-	/* Write the ECC data to flash */
-	MemWriteDOC(docptr, eccbuf, 6);
-
-	if (!before) {
-		/* Write the block status BLOCK_USED (0x5555) */
-		WriteDOC(0x55, docptr, Mil_CDSN_IO+6);
-		WriteDOC(0x55, docptr, Mil_CDSN_IO+7);
-	}
-
-#ifdef PSYCHO_DEBUG
-	printk("OOB data at %lx is %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X\n",
-	       (long) to, eccbuf[0], eccbuf[1], eccbuf[2], eccbuf[3],
-	       eccbuf[4], eccbuf[5]);
-#endif
-
-	WriteDOC(0x00, docptr, Mplus_WritePipeTerm);
-	WriteDOC(0x00, docptr, Mplus_WritePipeTerm);
-
-	/* Commit the Page Program command and wait for ready
-	   see Software Requirement 11.4 item 1.*/
-	DoC_Command(docptr, NAND_CMD_PAGEPROG, 0x00);
-	DoC_WaitReady(docptr);
-
-	/* Read the status of the flash device through CDSN IO register
-	   see Software Requirement 11.4 item 5.*/
-	DoC_Command(docptr, NAND_CMD_STATUS, 0);
-	dummy = ReadDOC(docptr, Mplus_ReadPipeInit);
-	dummy = ReadDOC(docptr, Mplus_ReadPipeInit);
-	DoC_Delay(docptr, 2);
-	if ((dummy = ReadDOC(docptr, Mplus_LastDataRead)) & 1) {
-		printk("MTD: Error 0x%x programming at 0x%x\n", dummy, (int)to);
-		/* Error in programming
-		   FIXME: implement Bad Block Replacement (in nftl.c ??) */
-		ret = -EIO;
-	}
-	dummy = ReadDOC(docptr, Mplus_LastDataRead);
-
-	/* Disable flash internally */
-	WriteDOC(0, docptr, Mplus_FlashSelect);
-
-	/* Let the caller know we completed it */
-	*retlen = len;
-
-	return ret;
-}
-
-static int doc_read_oob(struct mtd_info *mtd, loff_t ofs,
-			struct mtd_oob_ops *ops)
-{
-	loff_t fofs, base;
-	struct DiskOnChip *this = mtd->priv;
-	void __iomem * docptr = this->virtadr;
-	struct Nand *mychip = &this->chips[ofs >> this->chipshift];
-	size_t i, size, got, want;
-	uint8_t *buf = ops->oobbuf;
-	size_t len = ops->len;
-
-	BUG_ON(ops->mode != MTD_OPS_PLACE_OOB);
-
-	ofs += ops->ooboffs;
-
-	DoC_CheckASIC(docptr);
-
-	/* Find the chip which is to be used and select it */
-	if (this->curfloor != mychip->floor) {
-		DoC_SelectFloor(docptr, mychip->floor);
-		DoC_SelectChip(docptr, mychip->chip);
-	} else if (this->curchip != mychip->chip) {
-		DoC_SelectChip(docptr, mychip->chip);
-	}
-	this->curfloor = mychip->floor;
-	this->curchip = mychip->chip;
-
-	/* Millennium Plus bus cycle sequence as per figure 2, section 2.4 */
-	WriteDOC((DOC_FLASH_CE | DOC_FLASH_WP), docptr, Mplus_FlashSelect);
-
-	/* disable the ECC engine */
-	WriteDOC(DOC_ECC_RESET, docptr, Mplus_ECCConf);
-	DoC_WaitReady(docptr);
-
-	/* Maximum of 16 bytes in the OOB region, so limit read to that */
-	if (len > 16)
-		len = 16;
-	got = 0;
-	want = len;
-
-	for (i = 0; ((i < 3) && (want > 0)); i++) {
-		/* Figure out which region we are accessing... */
-		fofs = ofs;
-		base = ofs & 0xf;
-		if (!this->interleave) {
-			DoC_Command(docptr, NAND_CMD_READOOB, 0);
-			size = 16 - base;
-		} else if (base < 6) {
-			DoC_Command(docptr, DoC_GetECCOffset(mtd, &fofs), 0);
-			size = 6 - base;
-		} else if (base < 8) {
-			DoC_Command(docptr, DoC_GetFlagsOffset(mtd, &fofs), 0);
-			size = 8 - base;
-		} else {
-			DoC_Command(docptr, DoC_GetHdrOffset(mtd, &fofs), 0);
-			size = 16 - base;
-		}
-		if (size > want)
-			size = want;
-
-		/* Issue read command */
-		DoC_Address(this, 3, fofs, 0, 0x00);
-		WriteDOC(0, docptr, Mplus_FlashControl);
-		DoC_WaitReady(docptr);
-
-		ReadDOC(docptr, Mplus_ReadPipeInit);
-		ReadDOC(docptr, Mplus_ReadPipeInit);
-		MemReadDOC(docptr, &buf[got], size - 2);
-		buf[got + size - 2] = ReadDOC(docptr, Mplus_LastDataRead);
-		buf[got + size - 1] = ReadDOC(docptr, Mplus_LastDataRead);
-
-		ofs += size;
-		got += size;
-		want -= size;
-	}
-
-	/* Disable flash internally */
-	WriteDOC(0, docptr, Mplus_FlashSelect);
-
-	ops->retlen = len;
-	return 0;
-}
-
-static int doc_write_oob(struct mtd_info *mtd, loff_t ofs,
-			 struct mtd_oob_ops *ops)
-{
-	volatile char dummy;
-	loff_t fofs, base;
-	struct DiskOnChip *this = mtd->priv;
-	void __iomem * docptr = this->virtadr;
-	struct Nand *mychip = &this->chips[ofs >> this->chipshift];
-	size_t i, size, got, want;
-	int ret = 0;
-	uint8_t *buf = ops->oobbuf;
-	size_t len = ops->len;
-
-	BUG_ON(ops->mode != MTD_OPS_PLACE_OOB);
-
-	ofs += ops->ooboffs;
-
-	DoC_CheckASIC(docptr);
-
-	/* Find the chip which is to be used and select it */
-	if (this->curfloor != mychip->floor) {
-		DoC_SelectFloor(docptr, mychip->floor);
-		DoC_SelectChip(docptr, mychip->chip);
-	} else if (this->curchip != mychip->chip) {
-		DoC_SelectChip(docptr, mychip->chip);
-	}
-	this->curfloor = mychip->floor;
-	this->curchip = mychip->chip;
-
-	/* Millennium Plus bus cycle sequence as per figure 2, section 2.4 */
-	WriteDOC(DOC_FLASH_CE, docptr, Mplus_FlashSelect);
-
-
-	/* Maximum of 16 bytes in the OOB region, so limit write to that */
-	if (len > 16)
-		len = 16;
-	got = 0;
-	want = len;
-
-	for (i = 0; ((i < 3) && (want > 0)); i++) {
-		/* Reset the chip, see Software Requirement 11.4 item 1. */
-		DoC_Command(docptr, NAND_CMD_RESET, 0);
-		DoC_WaitReady(docptr);
-
-		/* Figure out which region we are accessing... */
-		fofs = ofs;
-		base = ofs & 0x0f;
-		if (!this->interleave) {
-			WriteDOC(NAND_CMD_READOOB, docptr, Mplus_FlashCmd);
-			size = 16 - base;
-		} else if (base < 6) {
-			WriteDOC(DoC_GetECCOffset(mtd, &fofs), docptr, Mplus_FlashCmd);
-			size = 6 - base;
-		} else if (base < 8) {
-			WriteDOC(DoC_GetFlagsOffset(mtd, &fofs), docptr, Mplus_FlashCmd);
-			size = 8 - base;
-		} else {
-			WriteDOC(DoC_GetHdrOffset(mtd, &fofs), docptr, Mplus_FlashCmd);
-			size = 16 - base;
-		}
-		if (size > want)
-			size = want;
-
-		/* Issue the Serial Data In command to initial the Page Program process */
-		DoC_Command(docptr, NAND_CMD_SEQIN, 0x00);
-		DoC_Address(this, 3, fofs, 0, 0x00);
-
-		/* Disable the ECC engine */
-		WriteDOC(DOC_ECC_RESET, docptr, Mplus_ECCConf);
-
-		/* Write the data via the internal pipeline through CDSN IO
-		   register, see Pipelined Write Operations 11.2 */
-		MemWriteDOC(docptr, (unsigned char *) &buf[got], size);
-		WriteDOC(0x00, docptr, Mplus_WritePipeTerm);
-		WriteDOC(0x00, docptr, Mplus_WritePipeTerm);
-
-		/* Commit the Page Program command and wait for ready
-	 	   see Software Requirement 11.4 item 1.*/
-		DoC_Command(docptr, NAND_CMD_PAGEPROG, 0x00);
-		DoC_WaitReady(docptr);
-
-		/* Read the status of the flash device through CDSN IO register
-		   see Software Requirement 11.4 item 5.*/
-		DoC_Command(docptr, NAND_CMD_STATUS, 0x00);
-		dummy = ReadDOC(docptr, Mplus_ReadPipeInit);
-		dummy = ReadDOC(docptr, Mplus_ReadPipeInit);
-		DoC_Delay(docptr, 2);
-		if ((dummy = ReadDOC(docptr, Mplus_LastDataRead)) & 1) {
-			printk("MTD: Error 0x%x programming oob at 0x%x\n",
-				dummy, (int)ofs);
-			/* FIXME: implement Bad Block Replacement */
-			ops->retlen = 0;
-			ret = -EIO;
-		}
-		dummy = ReadDOC(docptr, Mplus_LastDataRead);
-
-		ofs += size;
-		got += size;
-		want -= size;
-	}
-
-	/* Disable flash internally */
-	WriteDOC(0, docptr, Mplus_FlashSelect);
-
-	ops->retlen = len;
-	return ret;
-}
-
-int doc_erase(struct mtd_info *mtd, struct erase_info *instr)
-{
-	volatile char dummy;
-	struct DiskOnChip *this = mtd->priv;
-	__u32 ofs = instr->addr;
-	__u32 len = instr->len;
-	void __iomem * docptr = this->virtadr;
-	struct Nand *mychip = &this->chips[ofs >> this->chipshift];
-
-	DoC_CheckASIC(docptr);
-
-	if (len != mtd->erasesize)
-		printk(KERN_WARNING "MTD: Erase not right size (%x != %x)n",
-		       len, mtd->erasesize);
-
-	/* Find the chip which is to be used and select it */
-	if (this->curfloor != mychip->floor) {
-		DoC_SelectFloor(docptr, mychip->floor);
-		DoC_SelectChip(docptr, mychip->chip);
-	} else if (this->curchip != mychip->chip) {
-		DoC_SelectChip(docptr, mychip->chip);
-	}
-	this->curfloor = mychip->floor;
-	this->curchip = mychip->chip;
-
-	instr->state = MTD_ERASE_PENDING;
-
-	/* Millennium Plus bus cycle sequence as per figure 2, section 2.4 */
-	WriteDOC(DOC_FLASH_CE, docptr, Mplus_FlashSelect);
-
-	DoC_Command(docptr, NAND_CMD_RESET, 0x00);
-	DoC_WaitReady(docptr);
-
-	DoC_Command(docptr, NAND_CMD_ERASE1, 0);
-	DoC_Address(this, 2, ofs, 0, 0x00);
-	DoC_Command(docptr, NAND_CMD_ERASE2, 0);
-	DoC_WaitReady(docptr);
-	instr->state = MTD_ERASING;
-
-	/* Read the status of the flash device through CDSN IO register
-	   see Software Requirement 11.4 item 5. */
-	DoC_Command(docptr, NAND_CMD_STATUS, 0);
-	dummy = ReadDOC(docptr, Mplus_ReadPipeInit);
-	dummy = ReadDOC(docptr, Mplus_ReadPipeInit);
-	if ((dummy = ReadDOC(docptr, Mplus_LastDataRead)) & 1) {
-		printk("MTD: Error 0x%x erasing at 0x%x\n", dummy, ofs);
-		/* FIXME: implement Bad Block Replacement (in nftl.c ??) */
-		instr->state = MTD_ERASE_FAILED;
-	} else {
-		instr->state = MTD_ERASE_DONE;
-	}
-	dummy = ReadDOC(docptr, Mplus_LastDataRead);
-
-	/* Disable flash internally */
-	WriteDOC(0, docptr, Mplus_FlashSelect);
-
-	mtd_erase_callback(instr);
-
-	return 0;
-}
-
-/****************************************************************************
- *
- * Module stuff
- *
- ****************************************************************************/
-
-static void __exit cleanup_doc2001plus(void)
-{
-	struct mtd_info *mtd;
-	struct DiskOnChip *this;
-
-	while ((mtd=docmilpluslist)) {
-		this = mtd->priv;
-		docmilpluslist = this->nextdoc;
-
-		mtd_device_unregister(mtd);
-
-		iounmap(this->virtadr);
-		kfree(this->chips);
-		kfree(mtd);
-	}
-}
-
-module_exit(cleanup_doc2001plus);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Greg Ungerer <gerg@snapgear.com> et al.");
-MODULE_DESCRIPTION("Driver for DiskOnChip Millennium Plus");
diff --git a/drivers/mtd/devices/docecc.c b/drivers/mtd/devices/docecc.c
deleted file mode 100644
index 4a1c39b6f37d..000000000000
--- a/drivers/mtd/devices/docecc.c
+++ /dev/null
@@ -1,521 +0,0 @@
-/*
- * ECC algorithm for M-systems disk on chip. We use the excellent Reed
- * Solmon code of Phil Karn (karn@ka9q.ampr.org) available under the
- * GNU GPL License. The rest is simply to convert the disk on chip
- * syndrome into a standard syndome.
- *
- * Author: Fabrice Bellard (fabrice.bellard@netgem.com)
- * Copyright (C) 2000 Netgem S.A.
- *
- * 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/kernel.h>
-#include <linux/module.h>
-#include <asm/errno.h>
-#include <asm/io.h>
-#include <asm/uaccess.h>
-#include <linux/delay.h>
-#include <linux/slab.h>
-#include <linux/init.h>
-#include <linux/types.h>
-
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/doc2000.h>
-
-#define DEBUG_ECC 0
-/* need to undef it (from asm/termbits.h) */
-#undef B0
-
-#define MM 10 /* Symbol size in bits */
-#define KK (1023-4) /* Number of data symbols per block */
-#define B0 510 /* First root of generator polynomial, alpha form */
-#define PRIM 1 /* power of alpha used to generate roots of generator poly */
-#define	NN ((1 << MM) - 1)
-
-typedef unsigned short dtype;
-
-/* 1+x^3+x^10 */
-static const int Pp[MM+1] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 };
-
-/* This defines the type used to store an element of the Galois Field
- * used by the code. Make sure this is something larger than a char if
- * if anything larger than GF(256) is used.
- *
- * Note: unsigned char will work up to GF(256) but int seems to run
- * faster on the Pentium.
- */
-typedef int gf;
-
-/* No legal value in index form represents zero, so
- * we need a special value for this purpose
- */
-#define A0	(NN)
-
-/* Compute x % NN, where NN is 2**MM - 1,
- * without a slow divide
- */
-static inline gf
-modnn(int x)
-{
-  while (x >= NN) {
-    x -= NN;
-    x = (x >> MM) + (x & NN);
-  }
-  return x;
-}
-
-#define	CLEAR(a,n) {\
-int ci;\
-for(ci=(n)-1;ci >=0;ci--)\
-(a)[ci] = 0;\
-}
-
-#define	COPY(a,b,n) {\
-int ci;\
-for(ci=(n)-1;ci >=0;ci--)\
-(a)[ci] = (b)[ci];\
-}
-
-#define	COPYDOWN(a,b,n) {\
-int ci;\
-for(ci=(n)-1;ci >=0;ci--)\
-(a)[ci] = (b)[ci];\
-}
-
-#define Ldec 1
-
-/* generate GF(2**m) from the irreducible polynomial p(X) in Pp[0]..Pp[m]
-   lookup tables:  index->polynomial form   alpha_to[] contains j=alpha**i;
-                   polynomial form -> index form  index_of[j=alpha**i] = i
-   alpha=2 is the primitive element of GF(2**m)
-   HARI's COMMENT: (4/13/94) alpha_to[] can be used as follows:
-        Let @ represent the primitive element commonly called "alpha" that
-   is the root of the primitive polynomial p(x). Then in GF(2^m), for any
-   0 <= i <= 2^m-2,
-        @^i = a(0) + a(1) @ + a(2) @^2 + ... + a(m-1) @^(m-1)
-   where the binary vector (a(0),a(1),a(2),...,a(m-1)) is the representation
-   of the integer "alpha_to[i]" with a(0) being the LSB and a(m-1) the MSB. Thus for
-   example the polynomial representation of @^5 would be given by the binary
-   representation of the integer "alpha_to[5]".
-                   Similarly, index_of[] can be used as follows:
-        As above, let @ represent the primitive element of GF(2^m) that is
-   the root of the primitive polynomial p(x). In order to find the power
-   of @ (alpha) that has the polynomial representation
-        a(0) + a(1) @ + a(2) @^2 + ... + a(m-1) @^(m-1)
-   we consider the integer "i" whose binary representation with a(0) being LSB
-   and a(m-1) MSB is (a(0),a(1),...,a(m-1)) and locate the entry
-   "index_of[i]". Now, @^index_of[i] is that element whose polynomial
-    representation is (a(0),a(1),a(2),...,a(m-1)).
-   NOTE:
-        The element alpha_to[2^m-1] = 0 always signifying that the
-   representation of "@^infinity" = 0 is (0,0,0,...,0).
-        Similarly, the element index_of[0] = A0 always signifying
-   that the power of alpha which has the polynomial representation
-   (0,0,...,0) is "infinity".
-
-*/
-
-static void
-generate_gf(dtype Alpha_to[NN + 1], dtype Index_of[NN + 1])
-{
-  register int i, mask;
-
-  mask = 1;
-  Alpha_to[MM] = 0;
-  for (i = 0; i < MM; i++) {
-    Alpha_to[i] = mask;
-    Index_of[Alpha_to[i]] = i;
-    /* If Pp[i] == 1 then, term @^i occurs in poly-repr of @^MM */
-    if (Pp[i] != 0)
-      Alpha_to[MM] ^= mask;	/* Bit-wise EXOR operation */
-    mask <<= 1;	/* single left-shift */
-  }
-  Index_of[Alpha_to[MM]] = MM;
-  /*
-   * Have obtained poly-repr of @^MM. Poly-repr of @^(i+1) is given by
-   * poly-repr of @^i shifted left one-bit and accounting for any @^MM
-   * term that may occur when poly-repr of @^i is shifted.
-   */
-  mask >>= 1;
-  for (i = MM + 1; i < NN; i++) {
-    if (Alpha_to[i - 1] >= mask)
-      Alpha_to[i] = Alpha_to[MM] ^ ((Alpha_to[i - 1] ^ mask) << 1);
-    else
-      Alpha_to[i] = Alpha_to[i - 1] << 1;
-    Index_of[Alpha_to[i]] = i;
-  }
-  Index_of[0] = A0;
-  Alpha_to[NN] = 0;
-}
-
-/*
- * Performs ERRORS+ERASURES decoding of RS codes. bb[] is the content
- * of the feedback shift register after having processed the data and
- * the ECC.
- *
- * Return number of symbols corrected, or -1 if codeword is illegal
- * or uncorrectable. If eras_pos is non-null, the detected error locations
- * are written back. NOTE! This array must be at least NN-KK elements long.
- * The corrected data are written in eras_val[]. They must be xor with the data
- * to retrieve the correct data : data[erase_pos[i]] ^= erase_val[i] .
- *
- * First "no_eras" erasures are declared by the calling program. Then, the
- * maximum # of errors correctable is t_after_eras = floor((NN-KK-no_eras)/2).
- * If the number of channel errors is not greater than "t_after_eras" the
- * transmitted codeword will be recovered. Details of algorithm can be found
- * in R. Blahut's "Theory ... of Error-Correcting Codes".
-
- * Warning: the eras_pos[] array must not contain duplicate entries; decoder failure
- * will result. The decoder *could* check for this condition, but it would involve
- * extra time on every decoding operation.
- * */
-static int
-eras_dec_rs(dtype Alpha_to[NN + 1], dtype Index_of[NN + 1],
-            gf bb[NN - KK + 1], gf eras_val[NN-KK], int eras_pos[NN-KK],
-            int no_eras)
-{
-  int deg_lambda, el, deg_omega;
-  int i, j, r,k;
-  gf u,q,tmp,num1,num2,den,discr_r;
-  gf lambda[NN-KK + 1], s[NN-KK + 1];	/* Err+Eras Locator poly
-					 * and syndrome poly */
-  gf b[NN-KK + 1], t[NN-KK + 1], omega[NN-KK + 1];
-  gf root[NN-KK], reg[NN-KK + 1], loc[NN-KK];
-  int syn_error, count;
-
-  syn_error = 0;
-  for(i=0;i<NN-KK;i++)
-      syn_error |= bb[i];
-
-  if (!syn_error) {
-    /* if remainder is zero, data[] is a codeword and there are no
-     * errors to correct. So return data[] unmodified
-     */
-    count = 0;
-    goto finish;
-  }
-
-  for(i=1;i<=NN-KK;i++){
-    s[i] = bb[0];
-  }
-  for(j=1;j<NN-KK;j++){
-    if(bb[j] == 0)
-      continue;
-    tmp = Index_of[bb[j]];
-
-    for(i=1;i<=NN-KK;i++)
-      s[i] ^= Alpha_to[modnn(tmp + (B0+i-1)*PRIM*j)];
-  }
-
-  /* undo the feedback register implicit multiplication and convert
-     syndromes to index form */
-
-  for(i=1;i<=NN-KK;i++) {
-      tmp = Index_of[s[i]];
-      if (tmp != A0)
-          tmp = modnn(tmp + 2 * KK * (B0+i-1)*PRIM);
-      s[i] = tmp;
-  }
-
-  CLEAR(&lambda[1],NN-KK);
-  lambda[0] = 1;
-
-  if (no_eras > 0) {
-    /* Init lambda to be the erasure locator polynomial */
-    lambda[1] = Alpha_to[modnn(PRIM * eras_pos[0])];
-    for (i = 1; i < no_eras; i++) {
-      u = modnn(PRIM*eras_pos[i]);
-      for (j = i+1; j > 0; j--) {
-	tmp = Index_of[lambda[j - 1]];
-	if(tmp != A0)
-	  lambda[j] ^= Alpha_to[modnn(u + tmp)];
-      }
-    }
-#if DEBUG_ECC >= 1
-    /* Test code that verifies the erasure locator polynomial just constructed
-       Needed only for decoder debugging. */
-
-    /* find roots of the erasure location polynomial */
-    for(i=1;i<=no_eras;i++)
-      reg[i] = Index_of[lambda[i]];
-    count = 0;
-    for (i = 1,k=NN-Ldec; i <= NN; i++,k = modnn(NN+k-Ldec)) {
-      q = 1;
-      for (j = 1; j <= no_eras; j++)
-	if (reg[j] != A0) {
-	  reg[j] = modnn(reg[j] + j);
-	  q ^= Alpha_to[reg[j]];
-	}
-      if (q != 0)
-	continue;
-      /* store root and error location number indices */
-      root[count] = i;
-      loc[count] = k;
-      count++;
-    }
-    if (count != no_eras) {
-      printf("\n lambda(x) is WRONG\n");
-      count = -1;
-      goto finish;
-    }
-#if DEBUG_ECC >= 2
-    printf("\n Erasure positions as determined by roots of Eras Loc Poly:\n");
-    for (i = 0; i < count; i++)
-      printf("%d ", loc[i]);
-    printf("\n");
-#endif
-#endif
-  }
-  for(i=0;i<NN-KK+1;i++)
-    b[i] = Index_of[lambda[i]];
-
-  /*
-   * Begin Berlekamp-Massey algorithm to determine error+erasure
-   * locator polynomial
-   */
-  r = no_eras;
-  el = no_eras;
-  while (++r <= NN-KK) {	/* r is the step number */
-    /* Compute discrepancy at the r-th step in poly-form */
-    discr_r = 0;
-    for (i = 0; i < r; i++){
-      if ((lambda[i] != 0) && (s[r - i] != A0)) {
-	discr_r ^= Alpha_to[modnn(Index_of[lambda[i]] + s[r - i])];
-      }
-    }
-    discr_r = Index_of[discr_r];	/* Index form */
-    if (discr_r == A0) {
-      /* 2 lines below: B(x) <-- x*B(x) */
-      COPYDOWN(&b[1],b,NN-KK);
-      b[0] = A0;
-    } else {
-      /* 7 lines below: T(x) <-- lambda(x) - discr_r*x*b(x) */
-      t[0] = lambda[0];
-      for (i = 0 ; i < NN-KK; i++) {
-	if(b[i] != A0)
-	  t[i+1] = lambda[i+1] ^ Alpha_to[modnn(discr_r + b[i])];
-	else
-	  t[i+1] = lambda[i+1];
-      }
-      if (2 * el <= r + no_eras - 1) {
-	el = r + no_eras - el;
-	/*
-	 * 2 lines below: B(x) <-- inv(discr_r) *
-	 * lambda(x)
-	 */
-	for (i = 0; i <= NN-KK; i++)
-	  b[i] = (lambda[i] == 0) ? A0 : modnn(Index_of[lambda[i]] - discr_r + NN);
-      } else {
-	/* 2 lines below: B(x) <-- x*B(x) */
-	COPYDOWN(&b[1],b,NN-KK);
-	b[0] = A0;
-      }
-      COPY(lambda,t,NN-KK+1);
-    }
-  }
-
-  /* Convert lambda to index form and compute deg(lambda(x)) */
-  deg_lambda = 0;
-  for(i=0;i<NN-KK+1;i++){
-    lambda[i] = Index_of[lambda[i]];
-    if(lambda[i] != A0)
-      deg_lambda = i;
-  }
-  /*
-   * Find roots of the error+erasure locator polynomial by Chien
-   * Search
-   */
-  COPY(&reg[1],&lambda[1],NN-KK);
-  count = 0;		/* Number of roots of lambda(x) */
-  for (i = 1,k=NN-Ldec; i <= NN; i++,k = modnn(NN+k-Ldec)) {
-    q = 1;
-    for (j = deg_lambda; j > 0; j--){
-      if (reg[j] != A0) {
-	reg[j] = modnn(reg[j] + j);
-	q ^= Alpha_to[reg[j]];
-      }
-    }
-    if (q != 0)
-      continue;
-    /* store root (index-form) and error location number */
-    root[count] = i;
-    loc[count] = k;
-    /* If we've already found max possible roots,
-     * abort the search to save time
-     */
-    if(++count == deg_lambda)
-      break;
-  }
-  if (deg_lambda != count) {
-    /*
-     * deg(lambda) unequal to number of roots => uncorrectable
-     * error detected
-     */
-    count = -1;
-    goto finish;
-  }
-  /*
-   * Compute err+eras evaluator poly omega(x) = s(x)*lambda(x) (modulo
-   * x**(NN-KK)). in index form. Also find deg(omega).
-   */
-  deg_omega = 0;
-  for (i = 0; i < NN-KK;i++){
-    tmp = 0;
-    j = (deg_lambda < i) ? deg_lambda : i;
-    for(;j >= 0; j--){
-      if ((s[i + 1 - j] != A0) && (lambda[j] != A0))
-	tmp ^= Alpha_to[modnn(s[i + 1 - j] + lambda[j])];
-    }
-    if(tmp != 0)
-      deg_omega = i;
-    omega[i] = Index_of[tmp];
-  }
-  omega[NN-KK] = A0;
-
-  /*
-   * Compute error values in poly-form. num1 = omega(inv(X(l))), num2 =
-   * inv(X(l))**(B0-1) and den = lambda_pr(inv(X(l))) all in poly-form
-   */
-  for (j = count-1; j >=0; j--) {
-    num1 = 0;
-    for (i = deg_omega; i >= 0; i--) {
-      if (omega[i] != A0)
-	num1  ^= Alpha_to[modnn(omega[i] + i * root[j])];
-    }
-    num2 = Alpha_to[modnn(root[j] * (B0 - 1) + NN)];
-    den = 0;
-
-    /* lambda[i+1] for i even is the formal derivative lambda_pr of lambda[i] */
-    for (i = min(deg_lambda,NN-KK-1) & ~1; i >= 0; i -=2) {
-      if(lambda[i+1] != A0)
-	den ^= Alpha_to[modnn(lambda[i+1] + i * root[j])];
-    }
-    if (den == 0) {
-#if DEBUG_ECC >= 1
-      printf("\n ERROR: denominator = 0\n");
-#endif
-      /* Convert to dual- basis */
-      count = -1;
-      goto finish;
-    }
-    /* Apply error to data */
-    if (num1 != 0) {
-        eras_val[j] = Alpha_to[modnn(Index_of[num1] + Index_of[num2] + NN - Index_of[den])];
-    } else {
-        eras_val[j] = 0;
-    }
-  }
- finish:
-  for(i=0;i<count;i++)
-      eras_pos[i] = loc[i];
-  return count;
-}
-
-/***************************************************************************/
-/* The DOC specific code begins here */
-
-#define SECTOR_SIZE 512
-/* The sector bytes are packed into NB_DATA MM bits words */
-#define NB_DATA (((SECTOR_SIZE + 1) * 8 + 6) / MM)
-
-/*
- * Correct the errors in 'sector[]' by using 'ecc1[]' which is the
- * content of the feedback shift register applyied to the sector and
- * the ECC. Return the number of errors corrected (and correct them in
- * sector), or -1 if error
- */
-int doc_decode_ecc(unsigned char sector[SECTOR_SIZE], unsigned char ecc1[6])
-{
-    int parity, i, nb_errors;
-    gf bb[NN - KK + 1];
-    gf error_val[NN-KK];
-    int error_pos[NN-KK], pos, bitpos, index, val;
-    dtype *Alpha_to, *Index_of;
-
-    /* init log and exp tables here to save memory. However, it is slower */
-    Alpha_to = kmalloc((NN + 1) * sizeof(dtype), GFP_KERNEL);
-    if (!Alpha_to)
-        return -1;
-
-    Index_of = kmalloc((NN + 1) * sizeof(dtype), GFP_KERNEL);
-    if (!Index_of) {
-        kfree(Alpha_to);
-        return -1;
-    }
-
-    generate_gf(Alpha_to, Index_of);
-
-    parity = ecc1[1];
-
-    bb[0] =  (ecc1[4] & 0xff) | ((ecc1[5] & 0x03) << 8);
-    bb[1] = ((ecc1[5] & 0xfc) >> 2) | ((ecc1[2] & 0x0f) << 6);
-    bb[2] = ((ecc1[2] & 0xf0) >> 4) | ((ecc1[3] & 0x3f) << 4);
-    bb[3] = ((ecc1[3] & 0xc0) >> 6) | ((ecc1[0] & 0xff) << 2);
-
-    nb_errors = eras_dec_rs(Alpha_to, Index_of, bb,
-                            error_val, error_pos, 0);
-    if (nb_errors <= 0)
-        goto the_end;
-
-    /* correct the errors */
-    for(i=0;i<nb_errors;i++) {
-        pos = error_pos[i];
-        if (pos >= NB_DATA && pos < KK) {
-            nb_errors = -1;
-            goto the_end;
-        }
-        if (pos < NB_DATA) {
-            /* extract bit position (MSB first) */
-            pos = 10 * (NB_DATA - 1 - pos) - 6;
-            /* now correct the following 10 bits. At most two bytes
-               can be modified since pos is even */
-            index = (pos >> 3) ^ 1;
-            bitpos = pos & 7;
-            if ((index >= 0 && index < SECTOR_SIZE) ||
-                index == (SECTOR_SIZE + 1)) {
-                val = error_val[i] >> (2 + bitpos);
-                parity ^= val;
-                if (index < SECTOR_SIZE)
-                    sector[index] ^= val;
-            }
-            index = ((pos >> 3) + 1) ^ 1;
-            bitpos = (bitpos + 10) & 7;
-            if (bitpos == 0)
-                bitpos = 8;
-            if ((index >= 0 && index < SECTOR_SIZE) ||
-                index == (SECTOR_SIZE + 1)) {
-                val = error_val[i] << (8 - bitpos);
-                parity ^= val;
-                if (index < SECTOR_SIZE)
-                    sector[index] ^= val;
-            }
-        }
-    }
-
-    /* use parity to test extra errors */
-    if ((parity & 0xff) != 0)
-        nb_errors = -1;
-
- the_end:
-    kfree(Alpha_to);
-    kfree(Index_of);
-    return nb_errors;
-}
-
-EXPORT_SYMBOL_GPL(doc_decode_ecc);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Fabrice Bellard <fabrice.bellard@netgem.com>");
-MODULE_DESCRIPTION("ECC code for correcting errors detected by DiskOnChip 2000 and Millennium ECC hardware");
diff --git a/drivers/mtd/devices/docg3.c b/drivers/mtd/devices/docg3.c
index 8510ccb9c6f0..3e1b0a0ef4db 100644
--- a/drivers/mtd/devices/docg3.c
+++ b/drivers/mtd/devices/docg3.c
@@ -123,7 +123,7 @@ static inline void doc_flash_address(struct docg3 *docg3, u8 addr)
 	doc_writeb(docg3, addr, DOC_FLASHADDRESS);
 }
 
-static char const *part_probes[] = { "cmdlinepart", "saftlpart", NULL };
+static char const * const part_probes[] = { "cmdlinepart", "saftlpart", NULL };
 
 static int doc_register_readb(struct docg3 *docg3, int reg)
 {
@@ -2144,18 +2144,7 @@ static struct platform_driver g3_driver = {
 	.remove		= __exit_p(docg3_release),
 };
 
-static int __init docg3_init(void)
-{
-	return platform_driver_probe(&g3_driver, docg3_probe);
-}
-module_init(docg3_init);
-
-
-static void __exit docg3_exit(void)
-{
-	platform_driver_unregister(&g3_driver);
-}
-module_exit(docg3_exit);
+module_platform_driver_probe(g3_driver, docg3_probe);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Robert Jarzmik <robert.jarzmik@free.fr>");
diff --git a/drivers/mtd/devices/docprobe.c b/drivers/mtd/devices/docprobe.c
deleted file mode 100644
index 88b3fd3e18a7..000000000000
--- a/drivers/mtd/devices/docprobe.c
+++ /dev/null
@@ -1,325 +0,0 @@
-
-/* Linux driver for Disk-On-Chip devices			*/
-/* Probe routines common to all DoC devices			*/
-/* (C) 1999 Machine Vision Holdings, Inc.			*/
-/* (C) 1999-2003 David Woodhouse <dwmw2@infradead.org>		*/
-
-
-/* DOC_PASSIVE_PROBE:
-   In order to ensure that the BIOS checksum is correct at boot time, and
-   hence that the onboard BIOS extension gets executed, the DiskOnChip
-   goes into reset mode when it is read sequentially: all registers
-   return 0xff until the chip is woken up again by writing to the
-   DOCControl register.
-
-   Unfortunately, this means that the probe for the DiskOnChip is unsafe,
-   because one of the first things it does is write to where it thinks
-   the DOCControl register should be - which may well be shared memory
-   for another device. I've had machines which lock up when this is
-   attempted. Hence the possibility to do a passive probe, which will fail
-   to detect a chip in reset mode, but is at least guaranteed not to lock
-   the machine.
-
-   If you have this problem, uncomment the following line:
-#define DOC_PASSIVE_PROBE
-*/
-
-
-/* DOC_SINGLE_DRIVER:
-   Millennium driver has been merged into DOC2000 driver.
-
-   The old Millennium-only driver has been retained just in case there
-   are problems with the new code. If the combined driver doesn't work
-   for you, you can try the old one by undefining DOC_SINGLE_DRIVER
-   below and also enabling it in your configuration. If this fixes the
-   problems, please send a report to the MTD mailing list at
-   <linux-mtd@lists.infradead.org>.
-*/
-#define DOC_SINGLE_DRIVER
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <asm/errno.h>
-#include <asm/io.h>
-#include <linux/delay.h>
-#include <linux/slab.h>
-#include <linux/init.h>
-#include <linux/types.h>
-
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/nand.h>
-#include <linux/mtd/doc2000.h>
-
-
-static unsigned long doc_config_location = CONFIG_MTD_DOCPROBE_ADDRESS;
-module_param(doc_config_location, ulong, 0);
-MODULE_PARM_DESC(doc_config_location, "Physical memory address at which to probe for DiskOnChip");
-
-static unsigned long __initdata doc_locations[] = {
-#if defined (__alpha__) || defined(__i386__) || defined(__x86_64__)
-#ifdef CONFIG_MTD_DOCPROBE_HIGH
-	0xfffc8000, 0xfffca000, 0xfffcc000, 0xfffce000,
-	0xfffd0000, 0xfffd2000, 0xfffd4000, 0xfffd6000,
-	0xfffd8000, 0xfffda000, 0xfffdc000, 0xfffde000,
-	0xfffe0000, 0xfffe2000, 0xfffe4000, 0xfffe6000,
-	0xfffe8000, 0xfffea000, 0xfffec000, 0xfffee000,
-#else /*  CONFIG_MTD_DOCPROBE_HIGH */
-	0xc8000, 0xca000, 0xcc000, 0xce000,
-	0xd0000, 0xd2000, 0xd4000, 0xd6000,
-	0xd8000, 0xda000, 0xdc000, 0xde000,
-	0xe0000, 0xe2000, 0xe4000, 0xe6000,
-	0xe8000, 0xea000, 0xec000, 0xee000,
-#endif /*  CONFIG_MTD_DOCPROBE_HIGH */
-#endif
-	0xffffffff };
-
-/* doccheck: Probe a given memory window to see if there's a DiskOnChip present */
-
-static inline int __init doccheck(void __iomem *potential, unsigned long physadr)
-{
-	void __iomem *window=potential;
-	unsigned char tmp, tmpb, tmpc, ChipID;
-#ifndef DOC_PASSIVE_PROBE
-	unsigned char tmp2;
-#endif
-
-	/* Routine copied from the Linux DOC driver */
-
-#ifdef CONFIG_MTD_DOCPROBE_55AA
-	/* Check for 0x55 0xAA signature at beginning of window,
-	   this is no longer true once we remove the IPL (for Millennium */
-	if (ReadDOC(window, Sig1) != 0x55 || ReadDOC(window, Sig2) != 0xaa)
-		return 0;
-#endif /* CONFIG_MTD_DOCPROBE_55AA */
-
-#ifndef DOC_PASSIVE_PROBE
-	/* It's not possible to cleanly detect the DiskOnChip - the
-	 * bootup procedure will put the device into reset mode, and
-	 * it's not possible to talk to it without actually writing
-	 * to the DOCControl register. So we store the current contents
-	 * of the DOCControl register's location, in case we later decide
-	 * that it's not a DiskOnChip, and want to put it back how we
-	 * found it.
-	 */
-	tmp2 = ReadDOC(window, DOCControl);
-
-	/* Reset the DiskOnChip ASIC */
-	WriteDOC(DOC_MODE_CLR_ERR | DOC_MODE_MDWREN | DOC_MODE_RESET,
-		 window, DOCControl);
-	WriteDOC(DOC_MODE_CLR_ERR | DOC_MODE_MDWREN | DOC_MODE_RESET,
-		 window, DOCControl);
-
-	/* Enable the DiskOnChip ASIC */
-	WriteDOC(DOC_MODE_CLR_ERR | DOC_MODE_MDWREN | DOC_MODE_NORMAL,
-		 window, DOCControl);
-	WriteDOC(DOC_MODE_CLR_ERR | DOC_MODE_MDWREN | DOC_MODE_NORMAL,
-		 window, DOCControl);
-#endif /* !DOC_PASSIVE_PROBE */
-
-	/* We need to read the ChipID register four times. For some
-	   newer DiskOnChip 2000 units, the first three reads will
-	   return the DiskOnChip Millennium ident. Don't ask. */
-	ChipID = ReadDOC(window, ChipID);
-
-	switch (ChipID) {
-	case DOC_ChipID_Doc2k:
-		/* Check the TOGGLE bit in the ECC register */
-		tmp  = ReadDOC(window, 2k_ECCStatus) & DOC_TOGGLE_BIT;
-		tmpb = ReadDOC(window, 2k_ECCStatus) & DOC_TOGGLE_BIT;
-		tmpc = ReadDOC(window, 2k_ECCStatus) & DOC_TOGGLE_BIT;
-		if (tmp != tmpb && tmp == tmpc)
-				return ChipID;
-		break;
-
-	case DOC_ChipID_DocMil:
-		/* Check for the new 2000 with Millennium ASIC */
-		ReadDOC(window, ChipID);
-		ReadDOC(window, ChipID);
-		if (ReadDOC(window, ChipID) != DOC_ChipID_DocMil)
-			ChipID = DOC_ChipID_Doc2kTSOP;
-
-		/* Check the TOGGLE bit in the ECC register */
-		tmp  = ReadDOC(window, ECCConf) & DOC_TOGGLE_BIT;
-		tmpb = ReadDOC(window, ECCConf) & DOC_TOGGLE_BIT;
-		tmpc = ReadDOC(window, ECCConf) & DOC_TOGGLE_BIT;
-		if (tmp != tmpb && tmp == tmpc)
-				return ChipID;
-		break;
-
-	case DOC_ChipID_DocMilPlus16:
-	case DOC_ChipID_DocMilPlus32:
-	case 0:
-		/* Possible Millennium+, need to do more checks */
-#ifndef DOC_PASSIVE_PROBE
-		/* Possibly release from power down mode */
-		for (tmp = 0; (tmp < 4); tmp++)
-			ReadDOC(window, Mplus_Power);
-
-		/* Reset the DiskOnChip ASIC */
-		tmp = DOC_MODE_RESET | DOC_MODE_MDWREN | DOC_MODE_RST_LAT |
-			DOC_MODE_BDECT;
-		WriteDOC(tmp, window, Mplus_DOCControl);
-		WriteDOC(~tmp, window, Mplus_CtrlConfirm);
-
-		mdelay(1);
-		/* Enable the DiskOnChip ASIC */
-		tmp = DOC_MODE_NORMAL | DOC_MODE_MDWREN | DOC_MODE_RST_LAT |
-			DOC_MODE_BDECT;
-		WriteDOC(tmp, window, Mplus_DOCControl);
-		WriteDOC(~tmp, window, Mplus_CtrlConfirm);
-		mdelay(1);
-#endif /* !DOC_PASSIVE_PROBE */
-
-		ChipID = ReadDOC(window, ChipID);
-
-		switch (ChipID) {
-		case DOC_ChipID_DocMilPlus16:
-		case DOC_ChipID_DocMilPlus32:
-			/* Check the TOGGLE bit in the toggle register */
-			tmp  = ReadDOC(window, Mplus_Toggle) & DOC_TOGGLE_BIT;
-			tmpb = ReadDOC(window, Mplus_Toggle) & DOC_TOGGLE_BIT;
-			tmpc = ReadDOC(window, Mplus_Toggle) & DOC_TOGGLE_BIT;
-			if (tmp != tmpb && tmp == tmpc)
-					return ChipID;
-		default:
-			break;
-		}
-		/* FALL TRHU */
-
-	default:
-
-#ifdef CONFIG_MTD_DOCPROBE_55AA
-		printk(KERN_DEBUG "Possible DiskOnChip with unknown ChipID %2.2X found at 0x%lx\n",
-		       ChipID, physadr);
-#endif
-#ifndef DOC_PASSIVE_PROBE
-		/* Put back the contents of the DOCControl register, in case it's not
-		 * actually a DiskOnChip.
-		 */
-		WriteDOC(tmp2, window, DOCControl);
-#endif
-		return 0;
-	}
-
-	printk(KERN_WARNING "DiskOnChip failed TOGGLE test, dropping.\n");
-
-#ifndef DOC_PASSIVE_PROBE
-	/* Put back the contents of the DOCControl register: it's not a DiskOnChip */
-	WriteDOC(tmp2, window, DOCControl);
-#endif
-	return 0;
-}
-
-static int docfound;
-
-extern void DoC2k_init(struct mtd_info *);
-extern void DoCMil_init(struct mtd_info *);
-extern void DoCMilPlus_init(struct mtd_info *);
-
-static void __init DoC_Probe(unsigned long physadr)
-{
-	void __iomem *docptr;
-	struct DiskOnChip *this;
-	struct mtd_info *mtd;
-	int ChipID;
-	char namebuf[15];
-	char *name = namebuf;
-	void (*initroutine)(struct mtd_info *) = NULL;
-
-	docptr = ioremap(physadr, DOC_IOREMAP_LEN);
-
-	if (!docptr)
-		return;
-
-	if ((ChipID = doccheck(docptr, physadr))) {
-		if (ChipID == DOC_ChipID_Doc2kTSOP) {
-			/* Remove this at your own peril. The hardware driver works but nothing prevents you from erasing bad blocks */
-			printk(KERN_NOTICE "Refusing to drive DiskOnChip 2000 TSOP until Bad Block Table is correctly supported by INFTL\n");
-			iounmap(docptr);
-			return;
-		}
-		docfound = 1;
-		mtd = kzalloc(sizeof(struct DiskOnChip) + sizeof(struct mtd_info), GFP_KERNEL);
-		if (!mtd) {
-			printk(KERN_WARNING "Cannot allocate memory for data structures. Dropping.\n");
-			iounmap(docptr);
-			return;
-		}
-
-		this = (struct DiskOnChip *)(&mtd[1]);
-		mtd->priv = this;
-		this->virtadr = docptr;
-		this->physadr = physadr;
-		this->ChipID = ChipID;
-		sprintf(namebuf, "with ChipID %2.2X", ChipID);
-
-		switch(ChipID) {
-		case DOC_ChipID_Doc2kTSOP:
-			name="2000 TSOP";
-			initroutine = symbol_request(DoC2k_init);
-			break;
-
-		case DOC_ChipID_Doc2k:
-			name="2000";
-			initroutine = symbol_request(DoC2k_init);
-			break;
-
-		case DOC_ChipID_DocMil:
-			name="Millennium";
-#ifdef DOC_SINGLE_DRIVER
-			initroutine = symbol_request(DoC2k_init);
-#else
-			initroutine = symbol_request(DoCMil_init);
-#endif /* DOC_SINGLE_DRIVER */
-			break;
-
-		case DOC_ChipID_DocMilPlus16:
-		case DOC_ChipID_DocMilPlus32:
-			name="MillenniumPlus";
-			initroutine = symbol_request(DoCMilPlus_init);
-			break;
-		}
-
-		if (initroutine) {
-			(*initroutine)(mtd);
-			symbol_put_addr(initroutine);
-			return;
-		}
-		printk(KERN_NOTICE "Cannot find driver for DiskOnChip %s at 0x%lX\n", name, physadr);
-		kfree(mtd);
-	}
-	iounmap(docptr);
-}
-
-
-/****************************************************************************
- *
- * Module stuff
- *
- ****************************************************************************/
-
-static int __init init_doc(void)
-{
-	int i;
-
-	if (doc_config_location) {
-		printk(KERN_INFO "Using configured DiskOnChip probe address 0x%lx\n", doc_config_location);
-		DoC_Probe(doc_config_location);
-	} else {
-		for (i=0; (doc_locations[i] != 0xffffffff); i++) {
-			DoC_Probe(doc_locations[i]);
-		}
-	}
-	/* No banner message any more. Print a message if no DiskOnChip
-	   found, so the user knows we at least tried. */
-	if (!docfound)
-		printk(KERN_INFO "No recognised DiskOnChip devices found\n");
-	return -EAGAIN;
-}
-
-module_init(init_doc);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("David Woodhouse <dwmw2@infradead.org>");
-MODULE_DESCRIPTION("Probe code for DiskOnChip 2000 and Millennium devices");
-
diff --git a/drivers/mtd/devices/elm.c b/drivers/mtd/devices/elm.c
index 2ec5da9ee248..dccef9fdc1f2 100644
--- a/drivers/mtd/devices/elm.c
+++ b/drivers/mtd/devices/elm.c
@@ -81,14 +81,21 @@ static u32 elm_read_reg(struct elm_info *info, int offset)
  * @dev:	ELM device
  * @bch_type:	Type of BCH ecc
  */
-void elm_config(struct device *dev, enum bch_ecc bch_type)
+int elm_config(struct device *dev, enum bch_ecc bch_type)
 {
 	u32 reg_val;
 	struct elm_info *info = dev_get_drvdata(dev);
 
+	if (!info) {
+		dev_err(dev, "Unable to configure elm - device not probed?\n");
+		return -ENODEV;
+	}
+
 	reg_val = (bch_type & ECC_BCH_LEVEL_MASK) | (ELM_ECC_SIZE << 16);
 	elm_write_reg(info, ELM_LOCATION_CONFIG, reg_val);
 	info->bch_type = bch_type;
+
+	return 0;
 }
 EXPORT_SYMBOL(elm_config);
 
diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index 5b6b0728be21..2f3d2a5ff349 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -681,6 +681,7 @@ struct flash_info {
 	u16		flags;
 #define	SECT_4K		0x01		/* OPCODE_BE_4K works uniformly */
 #define	M25P_NO_ERASE	0x02		/* No erase command needed */
+#define	SST_WRITE	0x04		/* use SST byte programming */
 };
 
 #define INFO(_jedec_id, _ext_id, _sector_size, _n_sectors, _flags)	\
@@ -728,6 +729,7 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "en25q32b", INFO(0x1c3016, 0, 64 * 1024,  64, 0) },
 	{ "en25p64", INFO(0x1c2017, 0, 64 * 1024, 128, 0) },
 	{ "en25q64", INFO(0x1c3017, 0, 64 * 1024, 128, SECT_4K) },
+	{ "en25qh256", INFO(0x1c7019, 0, 64 * 1024, 512, 0) },
 
 	/* Everspin */
 	{ "mr25h256", CAT25_INFO(  32 * 1024, 1, 256, 2) },
@@ -740,7 +742,6 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "160s33b",  INFO(0x898911, 0, 64 * 1024,  32, 0) },
 	{ "320s33b",  INFO(0x898912, 0, 64 * 1024,  64, 0) },
 	{ "640s33b",  INFO(0x898913, 0, 64 * 1024, 128, 0) },
-	{ "n25q064",  INFO(0x20ba17, 0, 64 * 1024, 128, 0) },
 
 	/* Macronix */
 	{ "mx25l2005a",  INFO(0xc22012, 0, 64 * 1024,   4, SECT_4K) },
@@ -753,8 +754,10 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "mx25l12855e", INFO(0xc22618, 0, 64 * 1024, 256, 0) },
 	{ "mx25l25635e", INFO(0xc22019, 0, 64 * 1024, 512, 0) },
 	{ "mx25l25655e", INFO(0xc22619, 0, 64 * 1024, 512, 0) },
+	{ "mx66l51235l", INFO(0xc2201a, 0, 64 * 1024, 1024, 0) },
 
 	/* Micron */
+	{ "n25q064",  INFO(0x20ba17, 0, 64 * 1024, 128, 0) },
 	{ "n25q128a11",  INFO(0x20bb18, 0, 64 * 1024, 256, 0) },
 	{ "n25q128a13",  INFO(0x20ba18, 0, 64 * 1024, 256, 0) },
 	{ "n25q256a", INFO(0x20ba19, 0, 64 * 1024, 512, SECT_4K) },
@@ -781,14 +784,15 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "s25fl064k",  INFO(0xef4017,      0,  64 * 1024, 128, SECT_4K) },
 
 	/* SST -- large erase sizes are "overlays", "sectors" are 4K */
-	{ "sst25vf040b", INFO(0xbf258d, 0, 64 * 1024,  8, SECT_4K) },
-	{ "sst25vf080b", INFO(0xbf258e, 0, 64 * 1024, 16, SECT_4K) },
-	{ "sst25vf016b", INFO(0xbf2541, 0, 64 * 1024, 32, SECT_4K) },
-	{ "sst25vf032b", INFO(0xbf254a, 0, 64 * 1024, 64, SECT_4K) },
-	{ "sst25wf512",  INFO(0xbf2501, 0, 64 * 1024,  1, SECT_4K) },
-	{ "sst25wf010",  INFO(0xbf2502, 0, 64 * 1024,  2, SECT_4K) },
-	{ "sst25wf020",  INFO(0xbf2503, 0, 64 * 1024,  4, SECT_4K) },
-	{ "sst25wf040",  INFO(0xbf2504, 0, 64 * 1024,  8, SECT_4K) },
+	{ "sst25vf040b", INFO(0xbf258d, 0, 64 * 1024,  8, SECT_4K | SST_WRITE) },
+	{ "sst25vf080b", INFO(0xbf258e, 0, 64 * 1024, 16, SECT_4K | SST_WRITE) },
+	{ "sst25vf016b", INFO(0xbf2541, 0, 64 * 1024, 32, SECT_4K | SST_WRITE) },
+	{ "sst25vf032b", INFO(0xbf254a, 0, 64 * 1024, 64, SECT_4K | SST_WRITE) },
+	{ "sst25vf064c", INFO(0xbf254b, 0, 64 * 1024, 128, SECT_4K) },
+	{ "sst25wf512",  INFO(0xbf2501, 0, 64 * 1024,  1, SECT_4K | SST_WRITE) },
+	{ "sst25wf010",  INFO(0xbf2502, 0, 64 * 1024,  2, SECT_4K | SST_WRITE) },
+	{ "sst25wf020",  INFO(0xbf2503, 0, 64 * 1024,  4, SECT_4K | SST_WRITE) },
+	{ "sst25wf040",  INFO(0xbf2504, 0, 64 * 1024,  8, SECT_4K | SST_WRITE) },
 
 	/* ST Microelectronics -- newer production may have feature updates */
 	{ "m25p05",  INFO(0x202010,  0,  32 * 1024,   2, 0) },
@@ -838,6 +842,7 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "w25q64", INFO(0xef4017, 0, 64 * 1024, 128, SECT_4K) },
 	{ "w25q80", INFO(0xef5014, 0, 64 * 1024,  16, SECT_4K) },
 	{ "w25q80bl", INFO(0xef4014, 0, 64 * 1024,  16, SECT_4K) },
+	{ "w25q128", INFO(0xef4018, 0, 64 * 1024, 256, SECT_4K) },
 	{ "w25q256", INFO(0xef4019, 0, 64 * 1024, 512, SECT_4K) },
 
 	/* Catalyst / On Semiconductor -- non-JEDEC */
@@ -1000,7 +1005,7 @@ static int m25p_probe(struct spi_device *spi)
 	}
 
 	/* sst flash chips use AAI word program */
-	if (JEDEC_MFR(info->jedec_id) == CFI_MFR_SST)
+	if (info->flags & SST_WRITE)
 		flash->mtd._write = sst_write;
 	else
 		flash->mtd._write = m25p80_write;
diff --git a/drivers/mtd/devices/mtd_dataflash.c b/drivers/mtd/devices/mtd_dataflash.c
index 945c9f762349..28779b6dfcd9 100644
--- a/drivers/mtd/devices/mtd_dataflash.c
+++ b/drivers/mtd/devices/mtd_dataflash.c
@@ -105,8 +105,6 @@ static const struct of_device_id dataflash_dt_ids[] = {
 	{ .compatible = "atmel,dataflash", },
 	{ /* sentinel */ }
 };
-#else
-#define dataflash_dt_ids NULL
 #endif
 
 /* ......................................................................... */
@@ -914,7 +912,7 @@ static struct spi_driver dataflash_driver = {
 	.driver = {
 		.name		= "mtd_dataflash",
 		.owner		= THIS_MODULE,
-		.of_match_table = dataflash_dt_ids,
+		.of_match_table = of_match_ptr(dataflash_dt_ids),
 	},
 
 	.probe		= dataflash_probe,
diff --git a/drivers/mtd/maps/Kconfig b/drivers/mtd/maps/Kconfig
index 3ed17c4d4358..bed9d58d5741 100644
--- a/drivers/mtd/maps/Kconfig
+++ b/drivers/mtd/maps/Kconfig
@@ -249,22 +249,6 @@ config MTD_LANTIQ
 	help
 	  Support for NOR flash attached to the Lantiq SoC's External Bus Unit.
 
-config MTD_DILNETPC
-	tristate "CFI Flash device mapped on DIL/Net PC"
-	depends on X86 && MTD_CFI_INTELEXT && BROKEN
-	help
-	  MTD map driver for SSV DIL/Net PC Boards "DNP" and "ADNP".
-	  For details, see <http://www.ssv-embedded.de/ssv/pc104/p169.htm>
- 	  and <http://www.ssv-embedded.de/ssv/pc104/p170.htm>
-
-config MTD_DILNETPC_BOOTSIZE
-	hex "Size of DIL/Net PC flash boot partition"
-	depends on MTD_DILNETPC
-	default "0x80000"
-	help
-	  The amount of space taken up by the kernel or Etherboot
-	  on the DIL/Net PC flash chips.
-
 config MTD_L440GX
 	tristate "BIOS flash chip on Intel L440GX boards"
 	depends on X86 && MTD_JEDECPROBE
@@ -274,42 +258,6 @@ config MTD_L440GX
 
 	  BE VERY CAREFUL.
 
-config MTD_TQM8XXL
-	tristate "CFI Flash device mapped on TQM8XXL"
-	depends on MTD_CFI && TQM8xxL
-	help
-	  The TQM8xxL PowerPC board has up to two banks of CFI-compliant
-	  chips, currently uses AMD one. This 'mapping' driver supports
-	  that arrangement, allowing the CFI probe and command set driver
-	  code to communicate with the chips on the TQM8xxL board. More at
-	  <http://www.denx.de/wiki/PPCEmbedded/>.
-
-config MTD_RPXLITE
-	tristate "CFI Flash device mapped on RPX Lite or CLLF"
-	depends on MTD_CFI && (RPXCLASSIC || RPXLITE)
-	help
-	  The RPXLite PowerPC board has CFI-compliant chips mapped in
-	  a strange sparse mapping. This 'mapping' driver supports that
-	  arrangement, allowing the CFI probe and command set driver code
-	  to communicate with the chips on the RPXLite board. More at
-	  <http://www.embeddedplanet.com/>.
-
-config MTD_MBX860
-	tristate "System flash on MBX860 board"
-	depends on MTD_CFI && MBX
-	help
-	  This enables access routines for the flash chips on the Motorola
-	  MBX860 board. If you have one of these boards and would like
-	  to use the flash chips on it, say 'Y'.
-
-config MTD_DBOX2
-	tristate "CFI Flash device mapped on D-Box2"
-	depends on DBOX2 && MTD_CFI_INTELSTD && MTD_CFI_INTELEXT && MTD_CFI_AMDSTD
-	help
-	  This enables access routines for the flash chips on the Nokia/Sagem
-	  D-Box 2 board. If you have one of these boards and would like to use
-	  the flash chips on it, say 'Y'.
-
 config MTD_CFI_FLAGADM
 	tristate "CFI Flash device mapping on FlagaDM"
 	depends on 8xx && MTD_CFI
@@ -349,15 +297,6 @@ config MTD_IXP4XX
 	  IXDP425 and Coyote. If you have an IXP4xx based board and
 	  would like to use the flash chips on it, say 'Y'.
 
-config MTD_IXP2000
-	tristate "CFI Flash device mapped on Intel IXP2000 based systems"
-	depends on MTD_CFI && MTD_COMPLEX_MAPPINGS && ARCH_IXP2000
-	help
-	  This enables MTD access to flash devices on platforms based
-	  on Intel's IXP2000 family of network processors. If you have an
-	  IXP2000 based board and would like to use the flash chips on it,
-	  say 'Y'.
-
 config MTD_AUTCPU12
 	bool "NV-RAM mapping AUTCPU12 board"
 	depends on ARCH_AUTCPU12
@@ -372,13 +311,6 @@ config MTD_IMPA7
 	  This enables access to the NOR Flash on the impA7 board of
 	  implementa GmbH. If you have such a board, say 'Y' here.
 
-config MTD_H720X
-	tristate "Hynix evaluation board mappings"
-	depends on MTD_CFI && ( ARCH_H7201 || ARCH_H7202 )
-	help
-	  This enables access to the flash chips on the Hynix evaluation boards.
-	  If you have such a board, say 'Y'.
-
 # This needs CFI or JEDEC, depending on the cards found.
 config MTD_PCI
 	tristate "PCI MTD driver"
@@ -419,7 +351,7 @@ config MTD_BFIN_ASYNC
 
 config MTD_GPIO_ADDR
 	tristate "GPIO-assisted Flash Chip Support"
-	depends on GENERIC_GPIO || GPIOLIB
+	depends on GPIOLIB
 	depends on MTD_COMPLEX_MAPPINGS
 	help
 	  Map driver which allows flashes to be partially physically addressed
@@ -433,15 +365,6 @@ config MTD_UCLINUX
 	help
 	  Map driver to support image based filesystems for uClinux.
 
-config MTD_DMV182
-        tristate "Map driver for Dy-4 SVME/DMV-182 board."
-        depends on DMV182
-	select MTD_MAP_BANK_WIDTH_32
-	select MTD_CFI_I8
-	select MTD_CFI_AMDSTD
-        help
-          Map driver for Dy-4 SVME/DMV-182 board.
-
 config MTD_INTEL_VR_NOR
 	tristate "NOR flash on Intel Vermilion Range Expansion Bus CS0"
 	depends on PCI
diff --git a/drivers/mtd/maps/Makefile b/drivers/mtd/maps/Makefile
index 4ded28711bc1..395a12444048 100644
--- a/drivers/mtd/maps/Makefile
+++ b/drivers/mtd/maps/Makefile
@@ -9,7 +9,6 @@ endif
 # Chip mappings
 obj-$(CONFIG_MTD_CFI_FLAGADM)	+= cfi_flagadm.o
 obj-$(CONFIG_MTD_DC21285)	+= dc21285.o
-obj-$(CONFIG_MTD_DILNETPC)	+= dilnetpc.o
 obj-$(CONFIG_MTD_L440GX)	+= l440gx.o
 obj-$(CONFIG_MTD_AMD76XROM)	+= amd76xrom.o
 obj-$(CONFIG_MTD_ESB2ROM)	+= esb2rom.o
@@ -17,15 +16,12 @@ obj-$(CONFIG_MTD_ICHXROM)	+= ichxrom.o
 obj-$(CONFIG_MTD_CK804XROM)	+= ck804xrom.o
 obj-$(CONFIG_MTD_TSUNAMI)	+= tsunami_flash.o
 obj-$(CONFIG_MTD_PXA2XX)	+= pxa2xx-flash.o
-obj-$(CONFIG_MTD_MBX860)	+= mbx860.o
 obj-$(CONFIG_MTD_OCTAGON)	+= octagon-5066.o
 obj-$(CONFIG_MTD_PHYSMAP)	+= physmap.o
 obj-$(CONFIG_MTD_PHYSMAP_OF)	+= physmap_of.o
 obj-$(CONFIG_MTD_PISMO)		+= pismo.o
 obj-$(CONFIG_MTD_PMC_MSP_EVM)   += pmcmsp-flash.o
 obj-$(CONFIG_MTD_PCMCIA)	+= pcmciamtd.o
-obj-$(CONFIG_MTD_RPXLITE)	+= rpxlite.o
-obj-$(CONFIG_MTD_TQM8XXL)	+= tqm8xxl.o
 obj-$(CONFIG_MTD_SA1100)	+= sa1100-flash.o
 obj-$(CONFIG_MTD_SBC_GXX)	+= sbc_gxx.o
 obj-$(CONFIG_MTD_SC520CDP)	+= sc520cdp.o
@@ -34,7 +30,6 @@ obj-$(CONFIG_MTD_TS5500)	+= ts5500_flash.o
 obj-$(CONFIG_MTD_SUN_UFLASH)	+= sun_uflash.o
 obj-$(CONFIG_MTD_VMAX)		+= vmax301.o
 obj-$(CONFIG_MTD_SCx200_DOCFLASH)+= scx200_docflash.o
-obj-$(CONFIG_MTD_DBOX2)		+= dbox2-flash.o
 obj-$(CONFIG_MTD_SOLUTIONENGINE)+= solutionengine.o
 obj-$(CONFIG_MTD_PCI)		+= pci.o
 obj-$(CONFIG_MTD_AUTCPU12)	+= autcpu12-nvram.o
@@ -42,10 +37,7 @@ obj-$(CONFIG_MTD_IMPA7)		+= impa7.o
 obj-$(CONFIG_MTD_UCLINUX)	+= uclinux.o
 obj-$(CONFIG_MTD_NETtel)	+= nettel.o
 obj-$(CONFIG_MTD_SCB2_FLASH)	+= scb2_flash.o
-obj-$(CONFIG_MTD_H720X)		+= h720x-flash.o
 obj-$(CONFIG_MTD_IXP4XX)	+= ixp4xx.o
-obj-$(CONFIG_MTD_IXP2000)	+= ixp2000.o
-obj-$(CONFIG_MTD_DMV182)	+= dmv182.o
 obj-$(CONFIG_MTD_PLATRAM)	+= plat-ram.o
 obj-$(CONFIG_MTD_INTEL_VR_NOR)	+= intel_vr_nor.o
 obj-$(CONFIG_MTD_BFIN_ASYNC)	+= bfin-async-flash.o
diff --git a/drivers/mtd/maps/bfin-async-flash.c b/drivers/mtd/maps/bfin-async-flash.c
index f833edfaab79..319b04a6c9d1 100644
--- a/drivers/mtd/maps/bfin-async-flash.c
+++ b/drivers/mtd/maps/bfin-async-flash.c
@@ -122,7 +122,8 @@ static void bfin_flash_copy_to(struct map_info *map, unsigned long to, const voi
 	switch_back(state);
 }
 
-static const char *part_probe_types[] = { "cmdlinepart", "RedBoot", NULL };
+static const char * const part_probe_types[] = {
+	"cmdlinepart", "RedBoot", NULL };
 
 static int bfin_flash_probe(struct platform_device *pdev)
 {
diff --git a/drivers/mtd/maps/ck804xrom.c b/drivers/mtd/maps/ck804xrom.c
index 586a1c77e48a..0455166f05fa 100644
--- a/drivers/mtd/maps/ck804xrom.c
+++ b/drivers/mtd/maps/ck804xrom.c
@@ -308,8 +308,7 @@ static int ck804xrom_init_one(struct pci_dev *pdev,
 
  out:
 	/* Free any left over map structures */
-	if (map)
-		kfree(map);
+	kfree(map);
 
 	/* See if I have any map structures */
 	if (list_empty(&window->maps)) {
diff --git a/drivers/mtd/maps/dbox2-flash.c b/drivers/mtd/maps/dbox2-flash.c
deleted file mode 100644
index 85bdece6ab3f..000000000000
--- a/drivers/mtd/maps/dbox2-flash.c
+++ /dev/null
@@ -1,123 +0,0 @@
-/*
- * D-Box 2 flash driver
- */
-
-#include <linux/module.h>
-#include <linux/types.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <asm/io.h>
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/map.h>
-#include <linux/mtd/partitions.h>
-#include <linux/errno.h>
-
-/* partition_info gives details on the logical partitions that the split the
- * single flash device into. If the size if zero we use up to the end of the
- * device. */
-static struct mtd_partition partition_info[]= {
-	{
-	.name		= "BR bootloader",
-	.size		= 128 * 1024,
-	.offset		= 0,
-	.mask_flags	= MTD_WRITEABLE
-	},
-	{
-	.name		= "FLFS (U-Boot)",
-	.size		= 128 * 1024,
-	.offset		= MTDPART_OFS_APPEND,
-	.mask_flags	= 0
-	},
-	{
-	.name		= "Root (SquashFS)",
-	.size		= 7040 * 1024,
-	.offset		= MTDPART_OFS_APPEND,
-	.mask_flags	= 0
-	},
-	{
-	.name		= "var (JFFS2)",
-	.size		= 896 * 1024,
-	.offset		= MTDPART_OFS_APPEND,
-	.mask_flags	= 0
-	},
-	{
-	.name		= "Flash without bootloader",
-	.size		= MTDPART_SIZ_FULL,
-	.offset		= 128 * 1024,
-	.mask_flags	= 0
-	},
-	{
-	.name		= "Complete Flash",
-	.size		= MTDPART_SIZ_FULL,
-	.offset		= 0,
-	.mask_flags	= MTD_WRITEABLE
-	}
-};
-
-#define NUM_PARTITIONS ARRAY_SIZE(partition_info)
-
-#define WINDOW_ADDR 0x10000000
-#define WINDOW_SIZE 0x800000
-
-static struct mtd_info *mymtd;
-
-
-struct map_info dbox2_flash_map = {
-	.name		= "D-Box 2 flash memory",
-	.size		= WINDOW_SIZE,
-	.bankwidth	= 4,
-	.phys		= WINDOW_ADDR,
-};
-
-static int __init init_dbox2_flash(void)
-{
-       	printk(KERN_NOTICE "D-Box 2 flash driver (size->0x%X mem->0x%X)\n", WINDOW_SIZE, WINDOW_ADDR);
-	dbox2_flash_map.virt = ioremap(WINDOW_ADDR, WINDOW_SIZE);
-
-	if (!dbox2_flash_map.virt) {
-		printk("Failed to ioremap\n");
-		return -EIO;
-	}
-	simple_map_init(&dbox2_flash_map);
-
-	// Probe for dual Intel 28F320 or dual AMD
-	mymtd = do_map_probe("cfi_probe", &dbox2_flash_map);
-	if (!mymtd) {
-	    // Probe for single Intel 28F640
-	    dbox2_flash_map.bankwidth = 2;
-
-	    mymtd = do_map_probe("cfi_probe", &dbox2_flash_map);
-	}
-
-	if (mymtd) {
-		mymtd->owner = THIS_MODULE;
-
-                /* Create MTD devices for each partition. */
-		mtd_device_register(mymtd, partition_info, NUM_PARTITIONS);
-
-		return 0;
-	}
-
-	iounmap((void *)dbox2_flash_map.virt);
-	return -ENXIO;
-}
-
-static void __exit cleanup_dbox2_flash(void)
-{
-	if (mymtd) {
-		mtd_device_unregister(mymtd);
-		map_destroy(mymtd);
-	}
-	if (dbox2_flash_map.virt) {
-		iounmap((void *)dbox2_flash_map.virt);
-		dbox2_flash_map.virt = 0;
-	}
-}
-
-module_init(init_dbox2_flash);
-module_exit(cleanup_dbox2_flash);
-
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Kári Davíðsson <kd@flaga.is>, Bastian Blank <waldi@tuxbox.org>, Alexander Wild <wild@te-elektronik.com>");
-MODULE_DESCRIPTION("MTD map driver for D-Box 2 board");
diff --git a/drivers/mtd/maps/dc21285.c b/drivers/mtd/maps/dc21285.c
index 080f06053bd4..f8a7dd14cee0 100644
--- a/drivers/mtd/maps/dc21285.c
+++ b/drivers/mtd/maps/dc21285.c
@@ -143,9 +143,8 @@ static struct map_info dc21285_map = {
 	.copy_from = dc21285_copy_from,
 };
 
-
 /* Partition stuff */
-static const char *probes[] = { "RedBoot", "cmdlinepart", NULL };
+static const char * const probes[] = { "RedBoot", "cmdlinepart", NULL };
 
 static int __init init_dc21285(void)
 {
diff --git a/drivers/mtd/maps/dilnetpc.c b/drivers/mtd/maps/dilnetpc.c
deleted file mode 100644
index 3e393f0da823..000000000000
--- a/drivers/mtd/maps/dilnetpc.c
+++ /dev/null
@@ -1,496 +0,0 @@
-/* dilnetpc.c -- MTD map driver for SSV DIL/Net PC Boards "DNP" and "ADNP"
- *
- * 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
- *
- * The DIL/Net PC is a tiny embedded PC board made by SSV Embedded Systems
- * featuring the AMD Elan SC410 processor. There are two variants of this
- * board: DNP/1486 and ADNP/1486. The DNP version has 2 megs of flash
- * ROM (Intel 28F016S3) and 8 megs of DRAM, the ADNP version has 4 megs
- * flash and 16 megs of RAM.
- * For details, see http://www.ssv-embedded.de/ssv/pc104/p169.htm
- * and http://www.ssv-embedded.de/ssv/pc104/p170.htm
- */
-
-#include <linux/module.h>
-#include <linux/types.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/string.h>
-
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/map.h>
-#include <linux/mtd/partitions.h>
-#include <linux/mtd/concat.h>
-
-#include <asm/io.h>
-
-/*
-** The DIL/NetPC keeps its BIOS in two distinct flash blocks.
-** Destroying any of these blocks transforms the DNPC into
-** a paperweight (albeit not a very useful one, considering
-** it only weighs a few grams).
-**
-** Therefore, the BIOS blocks must never be erased or written to
-** except by people who know exactly what they are doing (e.g.
-** to install a BIOS update). These partitions are marked read-only
-** by default, but can be made read/write by undefining
-** DNPC_BIOS_BLOCKS_WRITEPROTECTED:
-*/
-#define DNPC_BIOS_BLOCKS_WRITEPROTECTED
-
-/*
-** The ID string (in ROM) is checked to determine whether we
-** are running on a DNP/1486 or ADNP/1486
-*/
-#define BIOSID_BASE	0x000fe100
-
-#define ID_DNPC	"DNP1486"
-#define ID_ADNP	"ADNP1486"
-
-/*
-** Address where the flash should appear in CPU space
-*/
-#define FLASH_BASE	0x2000000
-
-/*
-** Chip Setup and Control (CSC) indexed register space
-*/
-#define CSC_INDEX	0x22
-#define CSC_DATA	0x23
-
-#define CSC_MMSWAR	0x30	/* MMS window C-F attributes register */
-#define CSC_MMSWDSR	0x31	/* MMS window C-F device select register */
-
-#define CSC_RBWR	0xa7	/* GPIO Read-Back/Write Register B */
-
-#define CSC_CR		0xd0	/* internal I/O device disable/Echo */
-				/* Z-bus/configuration register */
-
-#define CSC_PCCMDCR	0xf1	/* PC card mode and DMA control register */
-
-
-/*
-** PC Card indexed register space:
-*/
-
-#define PCC_INDEX	0x3e0
-#define PCC_DATA	0x3e1
-
-#define PCC_AWER_B		0x46	/* Socket B Address Window enable register */
-#define PCC_MWSAR_1_Lo	0x58	/* memory window 1 start address low register */
-#define PCC_MWSAR_1_Hi	0x59	/* memory window 1 start address high register */
-#define PCC_MWEAR_1_Lo	0x5A	/* memory window 1 stop address low register */
-#define PCC_MWEAR_1_Hi	0x5B	/* memory window 1 stop address high register */
-#define PCC_MWAOR_1_Lo	0x5C	/* memory window 1 address offset low register */
-#define PCC_MWAOR_1_Hi	0x5D	/* memory window 1 address offset high register */
-
-
-/*
-** Access to SC4x0's Chip Setup and Control (CSC)
-** and PC Card (PCC) indexed registers:
-*/
-static inline void setcsc(int reg, unsigned char data)
-{
-	outb(reg, CSC_INDEX);
-	outb(data, CSC_DATA);
-}
-
-static inline unsigned char getcsc(int reg)
-{
-	outb(reg, CSC_INDEX);
-	return(inb(CSC_DATA));
-}
-
-static inline void setpcc(int reg, unsigned char data)
-{
-	outb(reg, PCC_INDEX);
-	outb(data, PCC_DATA);
-}
-
-static inline unsigned char getpcc(int reg)
-{
-	outb(reg, PCC_INDEX);
-	return(inb(PCC_DATA));
-}
-
-
-/*
-************************************************************
-** Enable access to DIL/NetPC's flash by mapping it into
-** the SC4x0's MMS Window C.
-************************************************************
-*/
-static void dnpc_map_flash(unsigned long flash_base, unsigned long flash_size)
-{
-	unsigned long flash_end = flash_base + flash_size - 1;
-
-	/*
-	** enable setup of MMS windows C-F:
-	*/
-	/* - enable PC Card indexed register space */
-	setcsc(CSC_CR, getcsc(CSC_CR) | 0x2);
-	/* - set PC Card controller to operate in standard mode */
-	setcsc(CSC_PCCMDCR, getcsc(CSC_PCCMDCR) & ~1);
-
-	/*
-	** Program base address and end address of window
-	** where the flash ROM should appear in CPU address space
-	*/
-	setpcc(PCC_MWSAR_1_Lo, (flash_base >> 12) & 0xff);
-	setpcc(PCC_MWSAR_1_Hi, (flash_base >> 20) & 0x3f);
-	setpcc(PCC_MWEAR_1_Lo, (flash_end >> 12) & 0xff);
-	setpcc(PCC_MWEAR_1_Hi, (flash_end >> 20) & 0x3f);
-
-	/* program offset of first flash location to appear in this window (0) */
-	setpcc(PCC_MWAOR_1_Lo, ((0 - flash_base) >> 12) & 0xff);
-	setpcc(PCC_MWAOR_1_Hi, ((0 - flash_base)>> 20) & 0x3f);
-
-	/* set attributes for MMS window C: non-cacheable, write-enabled */
-	setcsc(CSC_MMSWAR, getcsc(CSC_MMSWAR) & ~0x11);
-
-	/* select physical device ROMCS0 (i.e. flash) for MMS Window C */
-	setcsc(CSC_MMSWDSR, getcsc(CSC_MMSWDSR) & ~0x03);
-
-	/* enable memory window 1 */
-	setpcc(PCC_AWER_B, getpcc(PCC_AWER_B) | 0x02);
-
-	/* now disable PC Card indexed register space again */
-	setcsc(CSC_CR, getcsc(CSC_CR) & ~0x2);
-}
-
-
-/*
-************************************************************
-** Disable access to DIL/NetPC's flash by mapping it into
-** the SC4x0's MMS Window C.
-************************************************************
-*/
-static void dnpc_unmap_flash(void)
-{
-	/* - enable PC Card indexed register space */
-	setcsc(CSC_CR, getcsc(CSC_CR) | 0x2);
-
-	/* disable memory window 1 */
-	setpcc(PCC_AWER_B, getpcc(PCC_AWER_B) & ~0x02);
-
-	/* now disable PC Card indexed register space again */
-	setcsc(CSC_CR, getcsc(CSC_CR) & ~0x2);
-}
-
-
-
-/*
-************************************************************
-** Enable/Disable VPP to write to flash
-************************************************************
-*/
-
-static DEFINE_SPINLOCK(dnpc_spin);
-static int        vpp_counter = 0;
-/*
-** This is what has to be done for the DNP board ..
-*/
-static void dnp_set_vpp(struct map_info *not_used, int on)
-{
-	spin_lock_irq(&dnpc_spin);
-
-	if (on)
-	{
-		if(++vpp_counter == 1)
-			setcsc(CSC_RBWR, getcsc(CSC_RBWR) & ~0x4);
-	}
-	else
-	{
-		if(--vpp_counter == 0)
-			setcsc(CSC_RBWR, getcsc(CSC_RBWR) | 0x4);
-		else
-			BUG_ON(vpp_counter < 0);
-	}
-	spin_unlock_irq(&dnpc_spin);
-}
-
-/*
-** .. and this the ADNP version:
-*/
-static void adnp_set_vpp(struct map_info *not_used, int on)
-{
-	spin_lock_irq(&dnpc_spin);
-
-	if (on)
-	{
-		if(++vpp_counter == 1)
-			setcsc(CSC_RBWR, getcsc(CSC_RBWR) & ~0x8);
-	}
-	else
-	{
-		if(--vpp_counter == 0)
-			setcsc(CSC_RBWR, getcsc(CSC_RBWR) | 0x8);
-		else
-			BUG_ON(vpp_counter < 0);
-	}
-	spin_unlock_irq(&dnpc_spin);
-}
-
-
-
-#define DNP_WINDOW_SIZE		0x00200000	/*  DNP flash size is 2MiB  */
-#define ADNP_WINDOW_SIZE	0x00400000	/* ADNP flash size is 4MiB */
-#define WINDOW_ADDR		FLASH_BASE
-
-static struct map_info dnpc_map = {
-	.name = "ADNP Flash Bank",
-	.size = ADNP_WINDOW_SIZE,
-	.bankwidth = 1,
-	.set_vpp = adnp_set_vpp,
-	.phys = WINDOW_ADDR
-};
-
-/*
-** The layout of the flash is somewhat "strange":
-**
-** 1.  960 KiB (15 blocks) : Space for ROM Bootloader and user data
-** 2.   64 KiB (1 block)   : System BIOS
-** 3.  960 KiB (15 blocks) : User Data (DNP model) or
-** 3. 3008 KiB (47 blocks) : User Data (ADNP model)
-** 4.   64 KiB (1 block)   : System BIOS Entry
-*/
-
-static struct mtd_partition partition_info[]=
-{
-	{
-		.name =		"ADNP boot",
-		.offset =	0,
-		.size =		0xf0000,
-	},
-	{
-		.name =		"ADNP system BIOS",
-		.offset =	MTDPART_OFS_NXTBLK,
-		.size =		0x10000,
-#ifdef DNPC_BIOS_BLOCKS_WRITEPROTECTED
-		.mask_flags =	MTD_WRITEABLE,
-#endif
-	},
-	{
-		.name =		"ADNP file system",
-		.offset =	MTDPART_OFS_NXTBLK,
-		.size =		0x2f0000,
-	},
-	{
-		.name =		"ADNP system BIOS entry",
-		.offset =	MTDPART_OFS_NXTBLK,
-		.size =		MTDPART_SIZ_FULL,
-#ifdef DNPC_BIOS_BLOCKS_WRITEPROTECTED
-		.mask_flags =	MTD_WRITEABLE,
-#endif
-	},
-};
-
-#define NUM_PARTITIONS ARRAY_SIZE(partition_info)
-
-static struct mtd_info *mymtd;
-static struct mtd_info *lowlvl_parts[NUM_PARTITIONS];
-static struct mtd_info *merged_mtd;
-
-/*
-** "Highlevel" partition info:
-**
-** Using the MTD concat layer, we can re-arrange partitions to our
-** liking: we construct a virtual MTD device by concatenating the
-** partitions, specifying the sequence such that the boot block
-** is immediately followed by the filesystem block (i.e. the stupid
-** system BIOS block is mapped to a different place). When re-partitioning
-** this concatenated MTD device, we can set the boot block size to
-** an arbitrary (though erase block aligned) value i.e. not one that
-** is dictated by the flash's physical layout. We can thus set the
-** boot block to be e.g. 64 KB (which is fully sufficient if we want
-** to boot an etherboot image) or to -say- 1.5 MB if we want to boot
-** a large kernel image. In all cases, the remainder of the flash
-** is available as file system space.
-*/
-
-static struct mtd_partition higlvl_partition_info[]=
-{
-	{
-		.name =		"ADNP boot block",
-		.offset =	0,
-		.size =		CONFIG_MTD_DILNETPC_BOOTSIZE,
-	},
-	{
-		.name =		"ADNP file system space",
-		.offset =	MTDPART_OFS_NXTBLK,
-		.size =		ADNP_WINDOW_SIZE-CONFIG_MTD_DILNETPC_BOOTSIZE-0x20000,
-	},
-	{
-		.name =		"ADNP system BIOS + BIOS Entry",
-		.offset =	MTDPART_OFS_NXTBLK,
-		.size =		MTDPART_SIZ_FULL,
-#ifdef DNPC_BIOS_BLOCKS_WRITEPROTECTED
-		.mask_flags =	MTD_WRITEABLE,
-#endif
-	},
-};
-
-#define NUM_HIGHLVL_PARTITIONS ARRAY_SIZE(higlvl_partition_info)
-
-
-static int dnp_adnp_probe(void)
-{
-	char *biosid, rc = -1;
-
-	biosid = (char*)ioremap(BIOSID_BASE, 16);
-	if(biosid)
-	{
-		if(!strcmp(biosid, ID_DNPC))
-			rc = 1;		/* this is a DNPC  */
-		else if(!strcmp(biosid, ID_ADNP))
-			rc = 0;		/* this is a ADNPC */
-	}
-	iounmap((void *)biosid);
-	return(rc);
-}
-
-
-static int __init init_dnpc(void)
-{
-	int is_dnp;
-
-	/*
-	** determine hardware (DNP/ADNP/invalid)
-	*/
-	if((is_dnp = dnp_adnp_probe()) < 0)
-		return -ENXIO;
-
-	/*
-	** Things are set up for ADNP by default
-	** -> modify all that needs to be different for DNP
-	*/
-	if(is_dnp)
-	{	/*
-		** Adjust window size, select correct set_vpp function.
-		** The partitioning scheme is identical on both DNP
-		** and ADNP except for the size of the third partition.
-		*/
-		int i;
-		dnpc_map.size          = DNP_WINDOW_SIZE;
-		dnpc_map.set_vpp       = dnp_set_vpp;
-		partition_info[2].size = 0xf0000;
-
-		/*
-		** increment all string pointers so the leading 'A' gets skipped,
-		** thus turning all occurrences of "ADNP ..." into "DNP ..."
-		*/
-		++dnpc_map.name;
-		for(i = 0; i < NUM_PARTITIONS; i++)
-			++partition_info[i].name;
-		higlvl_partition_info[1].size = DNP_WINDOW_SIZE -
-			CONFIG_MTD_DILNETPC_BOOTSIZE - 0x20000;
-		for(i = 0; i < NUM_HIGHLVL_PARTITIONS; i++)
-			++higlvl_partition_info[i].name;
-	}
-
-	printk(KERN_NOTICE "DIL/Net %s flash: 0x%lx at 0x%llx\n",
-		is_dnp ? "DNPC" : "ADNP", dnpc_map.size, (unsigned long long)dnpc_map.phys);
-
-	dnpc_map.virt = ioremap_nocache(dnpc_map.phys, dnpc_map.size);
-
-	dnpc_map_flash(dnpc_map.phys, dnpc_map.size);
-
-	if (!dnpc_map.virt) {
-		printk("Failed to ioremap_nocache\n");
-		return -EIO;
-	}
-	simple_map_init(&dnpc_map);
-
-	printk("FLASH virtual address: 0x%p\n", dnpc_map.virt);
-
-	mymtd = do_map_probe("jedec_probe", &dnpc_map);
-
-	if (!mymtd)
-		mymtd = do_map_probe("cfi_probe", &dnpc_map);
-
-	/*
-	** If flash probes fail, try to make flashes accessible
-	** at least as ROM. Ajust erasesize in this case since
-	** the default one (128M) will break our partitioning
-	*/
-	if (!mymtd)
-		if((mymtd = do_map_probe("map_rom", &dnpc_map)))
-			mymtd->erasesize = 0x10000;
-
-	if (!mymtd) {
-		iounmap(dnpc_map.virt);
-		return -ENXIO;
-	}
-
-	mymtd->owner = THIS_MODULE;
-
-	/*
-	** Supply pointers to lowlvl_parts[] array to add_mtd_partitions()
-	** -> add_mtd_partitions() will _not_ register MTD devices for
-	** the partitions, but will instead store pointers to the MTD
-	** objects it creates into our lowlvl_parts[] array.
-	** NOTE: we arrange the pointers such that the sequence of the
-	**       partitions gets re-arranged: partition #2 follows
-	**       partition #0.
-	*/
-	partition_info[0].mtdp = &lowlvl_parts[0];
-	partition_info[1].mtdp = &lowlvl_parts[2];
-	partition_info[2].mtdp = &lowlvl_parts[1];
-	partition_info[3].mtdp = &lowlvl_parts[3];
-
-	mtd_device_register(mymtd, partition_info, NUM_PARTITIONS);
-
-	/*
-	** now create a virtual MTD device by concatenating the for partitions
-	** (in the sequence given by the lowlvl_parts[] array.
-	*/
-	merged_mtd = mtd_concat_create(lowlvl_parts, NUM_PARTITIONS, "(A)DNP Flash Concatenated");
-	if(merged_mtd)
-	{	/*
-		** now partition the new device the way we want it. This time,
-		** we do not supply mtd pointers in higlvl_partition_info, so
-		** add_mtd_partitions() will register the devices.
-		*/
-		mtd_device_register(merged_mtd, higlvl_partition_info,
-				    NUM_HIGHLVL_PARTITIONS);
-	}
-
-	return 0;
-}
-
-static void __exit cleanup_dnpc(void)
-{
-	if(merged_mtd) {
-		mtd_device_unregister(merged_mtd);
-		mtd_concat_destroy(merged_mtd);
-	}
-
-	if (mymtd) {
-		mtd_device_unregister(mymtd);
-		map_destroy(mymtd);
-	}
-	if (dnpc_map.virt) {
-		iounmap(dnpc_map.virt);
-		dnpc_unmap_flash();
-		dnpc_map.virt = NULL;
-	}
-}
-
-module_init(init_dnpc);
-module_exit(cleanup_dnpc);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Sysgo Real-Time Solutions GmbH");
-MODULE_DESCRIPTION("MTD map driver for SSV DIL/NetPC DNP & ADNP");
diff --git a/drivers/mtd/maps/dmv182.c b/drivers/mtd/maps/dmv182.c
deleted file mode 100644
index 6538ac675e00..000000000000
--- a/drivers/mtd/maps/dmv182.c
+++ /dev/null
@@ -1,146 +0,0 @@
-
-/*
- * drivers/mtd/maps/dmv182.c
- *
- * Flash map driver for the Dy4 SVME182 board
- *
- * Copyright 2003-2004, TimeSys Corporation
- *
- * Based on the SVME181 flash map, by Tom Nelson, Dot4, Inc. for TimeSys Corp.
- *
- * 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/init.h>
-#include <linux/types.h>
-#include <linux/kernel.h>
-#include <asm/io.h>
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/map.h>
-#include <linux/mtd/partitions.h>
-#include <linux/errno.h>
-
-/*
- * This driver currently handles only the 16MiB user flash bank 1 on the
- * board.  It does not provide access to bank 0 (contains the Dy4 FFW), bank 2
- * (VxWorks boot), or the optional 48MiB expansion flash.
- *
- * scott.wood@timesys.com: On the newer boards with 128MiB flash, it
- * now supports the first 96MiB (the boot flash bank containing FFW
- * is excluded).  The VxWorks loader is in partition 1.
- */
-
-#define FLASH_BASE_ADDR 0xf0000000
-#define FLASH_BANK_SIZE (128*1024*1024)
-
-MODULE_AUTHOR("Scott Wood, TimeSys Corporation <scott.wood@timesys.com>");
-MODULE_DESCRIPTION("User-programmable flash device on the Dy4 SVME182 board");
-MODULE_LICENSE("GPL");
-
-static struct map_info svme182_map = {
-	.name		= "Dy4 SVME182",
-	.bankwidth	= 32,
-	.size		=  128 * 1024 * 1024
-};
-
-#define BOOTIMAGE_PART_SIZE		((6*1024*1024)-RESERVED_PART_SIZE)
-
-// Allow 6MiB for the kernel
-#define NEW_BOOTIMAGE_PART_SIZE  (6 * 1024 * 1024)
-// Allow 1MiB for the bootloader
-#define NEW_BOOTLOADER_PART_SIZE (1024 * 1024)
-// Use the remaining 9MiB at the end of flash for the RFS
-#define NEW_RFS_PART_SIZE        (0x01000000 - NEW_BOOTLOADER_PART_SIZE - \
-                                  NEW_BOOTIMAGE_PART_SIZE)
-
-static struct mtd_partition svme182_partitions[] = {
-	// The Lower PABS is only 128KiB, but the partition code doesn't
-	// like partitions that don't end on the largest erase block
-	// size of the device, even if all of the erase blocks in the
-	// partition are small ones.  The hardware should prevent
-	// writes to the actual PABS areas.
-	{
-		name:       "Lower PABS and CPU 0 bootloader or kernel",
-		size:       6*1024*1024,
-		offset:     0,
-	},
-	{
-		name:       "Root Filesystem",
-		size:       10*1024*1024,
-		offset:     MTDPART_OFS_NXTBLK
-	},
-	{
-		name:       "CPU1 Bootloader",
-		size:       1024*1024,
-		offset:     MTDPART_OFS_NXTBLK,
-	},
-	{
-		name:       "Extra",
-		size:       110*1024*1024,
-		offset:     MTDPART_OFS_NXTBLK
-	},
-	{
-		name:       "Foundation Firmware and Upper PABS",
-		size:       1024*1024,
-		offset:     MTDPART_OFS_NXTBLK,
-		mask_flags: MTD_WRITEABLE // read-only
-	}
-};
-
-static struct mtd_info *this_mtd;
-
-static int __init init_svme182(void)
-{
-	struct mtd_partition *partitions;
-	int num_parts = ARRAY_SIZE(svme182_partitions);
-
-	partitions = svme182_partitions;
-
-	svme182_map.virt = ioremap(FLASH_BASE_ADDR, svme182_map.size);
-
-	if (svme182_map.virt == 0) {
-		printk("Failed to ioremap FLASH memory area.\n");
-		return -EIO;
-	}
-
-	simple_map_init(&svme182_map);
-
-	this_mtd = do_map_probe("cfi_probe", &svme182_map);
-	if (!this_mtd)
-	{
-		iounmap((void *)svme182_map.virt);
-		return -ENXIO;
-	}
-
-	printk(KERN_NOTICE "SVME182 flash device: %dMiB at 0x%08x\n",
-		   this_mtd->size >> 20, FLASH_BASE_ADDR);
-
-	this_mtd->owner = THIS_MODULE;
-	mtd_device_register(this_mtd, partitions, num_parts);
-
-	return 0;
-}
-
-static void __exit cleanup_svme182(void)
-{
-	if (this_mtd)
-	{
-		mtd_device_unregister(this_mtd);
-		map_destroy(this_mtd);
-	}
-
-	if (svme182_map.virt)
-	{
-		iounmap((void *)svme182_map.virt);
-		svme182_map.virt = 0;
-	}
-
-	return;
-}
-
-module_init(init_svme182);
-module_exit(cleanup_svme182);
diff --git a/drivers/mtd/maps/gpio-addr-flash.c b/drivers/mtd/maps/gpio-addr-flash.c
index 7b643de2500b..5ede28294f9e 100644
--- a/drivers/mtd/maps/gpio-addr-flash.c
+++ b/drivers/mtd/maps/gpio-addr-flash.c
@@ -157,7 +157,8 @@ static void gf_copy_to(struct map_info *map, unsigned long to,
 	memcpy_toio(map->virt + (to % state->win_size), from, len);
 }
 
-static const char *part_probe_types[] = { "cmdlinepart", "RedBoot", NULL };
+static const char * const part_probe_types[] = {
+	"cmdlinepart", "RedBoot", NULL };
 
 /**
  * gpio_flash_probe() - setup a mapping for a GPIO assisted flash
diff --git a/drivers/mtd/maps/h720x-flash.c b/drivers/mtd/maps/h720x-flash.c
deleted file mode 100644
index 8ed6cb4529d8..000000000000
--- a/drivers/mtd/maps/h720x-flash.c
+++ /dev/null
@@ -1,120 +0,0 @@
-/*
- * Flash memory access on Hynix GMS30C7201/HMS30C7202 based
- * evaluation boards
- *
- * (C) 2002 Jungjun Kim <jungjun.kim@hynix.com>
- *     2003 Thomas Gleixner <tglx@linutronix.de>
- */
-
-#include <linux/module.h>
-#include <linux/types.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/errno.h>
-#include <linux/slab.h>
-
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/map.h>
-#include <linux/mtd/partitions.h>
-#include <mach/hardware.h>
-#include <asm/io.h>
-
-static struct mtd_info *mymtd;
-
-static struct map_info h720x_map = {
-	.name =		"H720X",
-	.bankwidth =	4,
-	.size =		H720X_FLASH_SIZE,
-	.phys =		H720X_FLASH_PHYS,
-};
-
-static struct mtd_partition h720x_partitions[] = {
-        {
-                .name = "ArMon",
-                .size = 0x00080000,
-                .offset = 0,
-                .mask_flags = MTD_WRITEABLE
-        },{
-                .name = "Env",
-                .size = 0x00040000,
-                .offset = 0x00080000,
-                .mask_flags = MTD_WRITEABLE
-        },{
-                .name = "Kernel",
-                .size = 0x00180000,
-                .offset = 0x000c0000,
-                .mask_flags = MTD_WRITEABLE
-        },{
-                .name = "Ramdisk",
-                .size = 0x00400000,
-                .offset = 0x00240000,
-                .mask_flags = MTD_WRITEABLE
-        },{
-                .name = "jffs2",
-                .size = MTDPART_SIZ_FULL,
-                .offset = MTDPART_OFS_APPEND
-        }
-};
-
-#define NUM_PARTITIONS ARRAY_SIZE(h720x_partitions)
-
-/*
- * Initialize FLASH support
- */
-static int __init h720x_mtd_init(void)
-{
-	h720x_map.virt = ioremap(h720x_map.phys, h720x_map.size);
-
-	if (!h720x_map.virt) {
-		printk(KERN_ERR "H720x-MTD: ioremap failed\n");
-		return -EIO;
-	}
-
-	simple_map_init(&h720x_map);
-
-	// Probe for flash bankwidth 4
-	printk (KERN_INFO "H720x-MTD probing 32bit FLASH\n");
-	mymtd = do_map_probe("cfi_probe", &h720x_map);
-	if (!mymtd) {
-		printk (KERN_INFO "H720x-MTD probing 16bit FLASH\n");
-	    // Probe for bankwidth 2
-	    h720x_map.bankwidth = 2;
-	    mymtd = do_map_probe("cfi_probe", &h720x_map);
-	}
-
-	if (mymtd) {
-		mymtd->owner = THIS_MODULE;
-
-		mtd_device_parse_register(mymtd, NULL, NULL,
-					  h720x_partitions, NUM_PARTITIONS);
-		return 0;
-	}
-
-	iounmap((void *)h720x_map.virt);
-	return -ENXIO;
-}
-
-/*
- * Cleanup
- */
-static void __exit h720x_mtd_cleanup(void)
-{
-
-	if (mymtd) {
-		mtd_device_unregister(mymtd);
-		map_destroy(mymtd);
-	}
-
-	if (h720x_map.virt) {
-		iounmap((void *)h720x_map.virt);
-		h720x_map.virt = 0;
-	}
-}
-
-
-module_init(h720x_mtd_init);
-module_exit(h720x_mtd_cleanup);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Thomas Gleixner <tglx@linutronix.de>");
-MODULE_DESCRIPTION("MTD map driver for Hynix evaluation boards");
diff --git a/drivers/mtd/maps/impa7.c b/drivers/mtd/maps/impa7.c
index 834a06c56f56..49686744d93c 100644
--- a/drivers/mtd/maps/impa7.c
+++ b/drivers/mtd/maps/impa7.c
@@ -24,14 +24,12 @@
 #define NUM_FLASHBANKS 2
 #define BUSWIDTH     4
 
-/* can be { "cfi_probe", "jedec_probe", "map_rom", NULL } */
-#define PROBETYPES { "jedec_probe", NULL }
-
 #define MSG_PREFIX "impA7:"   /* prefix for our printk()'s */
 #define MTDID      "impa7-%d"  /* for mtdparts= partitioning */
 
 static struct mtd_info *impa7_mtd[NUM_FLASHBANKS];
 
+static const char * const rom_probe_types[] = { "jedec_probe", NULL };
 
 static struct map_info impa7_map[NUM_FLASHBANKS] = {
 	{
@@ -60,8 +58,7 @@ static struct mtd_partition partitions[] =
 
 static int __init init_impa7(void)
 {
-	static const char *rom_probe_types[] = PROBETYPES;
-	const char **type;
+	const char * const *type;
 	int i;
 	static struct { u_long addr; u_long size; } pt[NUM_FLASHBANKS] = {
 	  { WINDOW_ADDR0, WINDOW_SIZE0 },
diff --git a/drivers/mtd/maps/intel_vr_nor.c b/drivers/mtd/maps/intel_vr_nor.c
index b14053b25026..f581ac1cf022 100644
--- a/drivers/mtd/maps/intel_vr_nor.c
+++ b/drivers/mtd/maps/intel_vr_nor.c
@@ -82,9 +82,9 @@ static void vr_nor_destroy_mtd_setup(struct vr_nor_mtd *p)
 
 static int vr_nor_mtd_setup(struct vr_nor_mtd *p)
 {
-	static const char *probe_types[] =
+	static const char * const probe_types[] =
 	    { "cfi_probe", "jedec_probe", NULL };
-	const char **type;
+	const char * const *type;
 
 	for (type = probe_types; !p->info && *type; type++)
 		p->info = do_map_probe(*type, &p->map);
diff --git a/drivers/mtd/maps/ixp2000.c b/drivers/mtd/maps/ixp2000.c
deleted file mode 100644
index 4a41ced0f710..000000000000
--- a/drivers/mtd/maps/ixp2000.c
+++ /dev/null
@@ -1,253 +0,0 @@
-/*
- * drivers/mtd/maps/ixp2000.c
- *
- * Mapping for the Intel XScale IXP2000 based systems
- *
- * Copyright (C) 2002 Intel Corp.
- * Copyright (C) 2003-2004 MontaVista Software, Inc.
- *
- * Original Author: Naeem M Afzal <naeem.m.afzal@intel.com>
- * Maintainer: Deepak Saxena <dsaxena@plexity.net>
- *
- * 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/types.h>
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/string.h>
-#include <linux/slab.h>
-#include <linux/ioport.h>
-#include <linux/device.h>
-#include <linux/platform_device.h>
-
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/map.h>
-#include <linux/mtd/partitions.h>
-
-#include <asm/io.h>
-#include <mach/hardware.h>
-#include <asm/mach/flash.h>
-
-#include <linux/reboot.h>
-
-struct ixp2000_flash_info {
-	struct		mtd_info *mtd;
-	struct		map_info map;
-	struct		resource *res;
-};
-
-static inline unsigned long flash_bank_setup(struct map_info *map, unsigned long ofs)
-{
-	unsigned long (*set_bank)(unsigned long) =
-		(unsigned long(*)(unsigned long))map->map_priv_2;
-
-	return (set_bank ? set_bank(ofs) : ofs);
-}
-
-#ifdef __ARMEB__
-/*
- * Rev A0 and A1 of IXP2400 silicon have a broken addressing unit which
- * causes the lower address bits to be XORed with 0x11 on 8 bit accesses
- * and XORed with 0x10 on 16 bit accesses. See the spec update, erratum 44.
- */
-static int erratum44_workaround = 0;
-
-static inline unsigned long address_fix8_write(unsigned long addr)
-{
-	if (erratum44_workaround) {
-		return (addr ^ 3);
-	}
-	return addr;
-}
-#else
-
-#define address_fix8_write(x)	(x)
-#endif
-
-static map_word ixp2000_flash_read8(struct map_info *map, unsigned long ofs)
-{
-	map_word val;
-
-	val.x[0] =  *((u8 *)(map->map_priv_1 + flash_bank_setup(map, ofs)));
-	return val;
-}
-
-/*
- * We can't use the standard memcpy due to the broken SlowPort
- * address translation on rev A0 and A1 silicon and the fact that
- * we have banked flash.
- */
-static void ixp2000_flash_copy_from(struct map_info *map, void *to,
-			      unsigned long from, ssize_t len)
-{
-	from = flash_bank_setup(map, from);
-	while(len--)
-		*(__u8 *) to++ = *(__u8 *)(map->map_priv_1 + from++);
-}
-
-static void ixp2000_flash_write8(struct map_info *map, map_word d, unsigned long ofs)
-{
-	*(__u8 *) (address_fix8_write(map->map_priv_1 +
-				      flash_bank_setup(map, ofs))) = d.x[0];
-}
-
-static void ixp2000_flash_copy_to(struct map_info *map, unsigned long to,
-			    const void *from, ssize_t len)
-{
-	to = flash_bank_setup(map, to);
-	while(len--) {
-		unsigned long tmp = address_fix8_write(map->map_priv_1 + to++);
-		*(__u8 *)(tmp) = *(__u8 *)(from++);
-	}
-}
-
-
-static int ixp2000_flash_remove(struct platform_device *dev)
-{
-	struct flash_platform_data *plat = dev->dev.platform_data;
-	struct ixp2000_flash_info *info = platform_get_drvdata(dev);
-
-	platform_set_drvdata(dev, NULL);
-
-	if(!info)
-		return 0;
-
-	if (info->mtd) {
-		mtd_device_unregister(info->mtd);
-		map_destroy(info->mtd);
-	}
-	if (info->map.map_priv_1)
-		iounmap((void *) info->map.map_priv_1);
-
-	if (info->res) {
-		release_resource(info->res);
-		kfree(info->res);
-	}
-
-	if (plat->exit)
-		plat->exit();
-
-	return 0;
-}
-
-
-static int ixp2000_flash_probe(struct platform_device *dev)
-{
-	static const char *probes[] = { "RedBoot", "cmdlinepart", NULL };
-	struct ixp2000_flash_data *ixp_data = dev->dev.platform_data;
-	struct flash_platform_data *plat;
-	struct ixp2000_flash_info *info;
-	unsigned long window_size;
-	int err = -1;
-
-	if (!ixp_data)
-		return -ENODEV;
-
-	plat = ixp_data->platform_data;
-	if (!plat)
-		return -ENODEV;
-
-	window_size = resource_size(dev->resource);
-	dev_info(&dev->dev, "Probe of IXP2000 flash(%d banks x %dMiB)\n",
-		 ixp_data->nr_banks, ((u32)window_size >> 20));
-
-	if (plat->width != 1) {
-		dev_err(&dev->dev, "IXP2000 MTD map only supports 8-bit mode, asking for %d\n",
-			plat->width * 8);
-		return -EIO;
-	}
-
-	info = kzalloc(sizeof(struct ixp2000_flash_info), GFP_KERNEL);
-	if(!info) {
-		err = -ENOMEM;
-		goto Error;
-	}
-
-	platform_set_drvdata(dev, info);
-
-	/*
-	 * Tell the MTD layer we're not 1:1 mapped so that it does
-	 * not attempt to do a direct access on us.
-	 */
-	info->map.phys = NO_XIP;
-
-	info->map.size = ixp_data->nr_banks * window_size;
-	info->map.bankwidth = 1;
-
-	/*
- 	 * map_priv_2 is used to store a ptr to the bank_setup routine
- 	 */
-	info->map.map_priv_2 = (unsigned long) ixp_data->bank_setup;
-
-	info->map.name = dev_name(&dev->dev);
-	info->map.read = ixp2000_flash_read8;
-	info->map.write = ixp2000_flash_write8;
-	info->map.copy_from = ixp2000_flash_copy_from;
-	info->map.copy_to = ixp2000_flash_copy_to;
-
-	info->res = request_mem_region(dev->resource->start,
-				       resource_size(dev->resource),
-				       dev_name(&dev->dev));
-	if (!info->res) {
-		dev_err(&dev->dev, "Could not reserve memory region\n");
-		err = -ENOMEM;
-		goto Error;
-	}
-
-	info->map.map_priv_1 =
-		(unsigned long)ioremap(dev->resource->start,
-				       resource_size(dev->resource));
-	if (!info->map.map_priv_1) {
-		dev_err(&dev->dev, "Failed to ioremap flash region\n");
-		err = -EIO;
-		goto Error;
-	}
-
-#if defined(__ARMEB__)
-	/*
-	 * Enable erratum 44 workaround for NPUs with broken slowport
-	 */
-
-	erratum44_workaround = ixp2000_has_broken_slowport();
-	dev_info(&dev->dev, "Erratum 44 workaround %s\n",
-	       erratum44_workaround ? "enabled" : "disabled");
-#endif
-
-	info->mtd = do_map_probe(plat->map_name, &info->map);
-	if (!info->mtd) {
-		dev_err(&dev->dev, "map_probe failed\n");
-		err = -ENXIO;
-		goto Error;
-	}
-	info->mtd->owner = THIS_MODULE;
-
-	err = mtd_device_parse_register(info->mtd, probes, NULL, NULL, 0);
-	if (err)
-		goto Error;
-
-	return 0;
-
-Error:
-	ixp2000_flash_remove(dev);
-	return err;
-}
-
-static struct platform_driver ixp2000_flash_driver = {
-	.probe		= ixp2000_flash_probe,
-	.remove		= ixp2000_flash_remove,
-	.driver		= {
-		.name	= "IXP2000-Flash",
-		.owner	= THIS_MODULE,
-	},
-};
-
-module_platform_driver(ixp2000_flash_driver);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Deepak Saxena <dsaxena@plexity.net>");
-MODULE_ALIAS("platform:IXP2000-Flash");
diff --git a/drivers/mtd/maps/ixp4xx.c b/drivers/mtd/maps/ixp4xx.c
index e864fc6c58f9..52b3410a105c 100644
--- a/drivers/mtd/maps/ixp4xx.c
+++ b/drivers/mtd/maps/ixp4xx.c
@@ -148,7 +148,7 @@ struct ixp4xx_flash_info {
 	struct resource *res;
 };
 
-static const char *probes[] = { "RedBoot", "cmdlinepart", NULL };
+static const char * const probes[] = { "RedBoot", "cmdlinepart", NULL };
 
 static int ixp4xx_flash_remove(struct platform_device *dev)
 {
diff --git a/drivers/mtd/maps/lantiq-flash.c b/drivers/mtd/maps/lantiq-flash.c
index d1da6ede3845..d7ac65d1d569 100644
--- a/drivers/mtd/maps/lantiq-flash.c
+++ b/drivers/mtd/maps/lantiq-flash.c
@@ -46,8 +46,7 @@ struct ltq_mtd {
 };
 
 static const char ltq_map_name[] = "ltq_nor";
-static const char *ltq_probe_types[] = {
-					"cmdlinepart", "ofpart", NULL };
+static const char * const ltq_probe_types[] = { "cmdlinepart", "ofpart", NULL };
 
 static map_word
 ltq_read16(struct map_info *map, unsigned long adr)
diff --git a/drivers/mtd/maps/mbx860.c b/drivers/mtd/maps/mbx860.c
deleted file mode 100644
index 93fa56c33003..000000000000
--- a/drivers/mtd/maps/mbx860.c
+++ /dev/null
@@ -1,98 +0,0 @@
-/*
- * Handle mapping of the flash on MBX860 boards
- *
- * Author:	Anton Todorov
- * Copyright:	(C) 2001 Emness Technology
- *
- * 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/types.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <asm/io.h>
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/map.h>
-#include <linux/mtd/partitions.h>
-
-
-#define WINDOW_ADDR 0xfe000000
-#define WINDOW_SIZE 0x00200000
-
-/* Flash / Partition sizing */
-#define MAX_SIZE_KiB              8192
-#define BOOT_PARTITION_SIZE_KiB    512
-#define KERNEL_PARTITION_SIZE_KiB 5632
-#define APP_PARTITION_SIZE_KiB    2048
-
-#define NUM_PARTITIONS 3
-
-/* partition_info gives details on the logical partitions that the split the
- * single flash device into. If the size if zero we use up to the end of the
- * device. */
-static struct mtd_partition partition_info[]={
-	{ .name = "MBX flash BOOT partition",
-	.offset = 0,
-	.size =   BOOT_PARTITION_SIZE_KiB*1024 },
-	{ .name = "MBX flash DATA partition",
-	.offset = BOOT_PARTITION_SIZE_KiB*1024,
-	.size = (KERNEL_PARTITION_SIZE_KiB)*1024 },
-	{ .name = "MBX flash APPLICATION partition",
-	.offset = (BOOT_PARTITION_SIZE_KiB+KERNEL_PARTITION_SIZE_KiB)*1024 }
-};
-
-
-static struct mtd_info *mymtd;
-
-struct map_info mbx_map = {
-	.name = "MBX flash",
-	.size = WINDOW_SIZE,
-	.phys = WINDOW_ADDR,
-	.bankwidth = 4,
-};
-
-static int __init init_mbx(void)
-{
-	printk(KERN_NOTICE "Motorola MBX flash device: 0x%x at 0x%x\n", WINDOW_SIZE*4, WINDOW_ADDR);
-	mbx_map.virt = ioremap(WINDOW_ADDR, WINDOW_SIZE * 4);
-
-	if (!mbx_map.virt) {
-		printk("Failed to ioremap\n");
-		return -EIO;
-	}
-	simple_map_init(&mbx_map);
-
-	mymtd = do_map_probe("jedec_probe", &mbx_map);
-	if (mymtd) {
-		mymtd->owner = THIS_MODULE;
-		mtd_device_register(mymtd, NULL, 0);
-		mtd_device_register(mymtd, partition_info, NUM_PARTITIONS);
-		return 0;
-	}
-
-	iounmap((void *)mbx_map.virt);
-	return -ENXIO;
-}
-
-static void __exit cleanup_mbx(void)
-{
-	if (mymtd) {
-		mtd_device_unregister(mymtd);
-		map_destroy(mymtd);
-	}
-	if (mbx_map.virt) {
-		iounmap((void *)mbx_map.virt);
-		mbx_map.virt = 0;
-	}
-}
-
-module_init(init_mbx);
-module_exit(cleanup_mbx);
-
-MODULE_AUTHOR("Anton Todorov <a.todorov@emness.com>");
-MODULE_DESCRIPTION("MTD map driver for Motorola MBX860 board");
-MODULE_LICENSE("GPL");
diff --git a/drivers/mtd/maps/pci.c b/drivers/mtd/maps/pci.c
index c3aebd5da5d6..c2604f8b2a5e 100644
--- a/drivers/mtd/maps/pci.c
+++ b/drivers/mtd/maps/pci.c
@@ -283,8 +283,7 @@ static int mtd_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	if (err)
 		goto release;
 
-	/* tsk - do_map_probe should take const char * */
-	mtd = do_map_probe((char *)info->map_name, &map->map);
+	mtd = do_map_probe(info->map_name, &map->map);
 	err = -ENODEV;
 	if (!mtd)
 		goto release;
diff --git a/drivers/mtd/maps/physmap.c b/drivers/mtd/maps/physmap.c
index 21b0b713cacb..e7a592c8c765 100644
--- a/drivers/mtd/maps/physmap.c
+++ b/drivers/mtd/maps/physmap.c
@@ -87,21 +87,18 @@ static void physmap_set_vpp(struct map_info *map, int state)
 	spin_unlock_irqrestore(&info->vpp_lock, flags);
 }
 
-static const char *rom_probe_types[] = {
-					"cfi_probe",
-					"jedec_probe",
-					"qinfo_probe",
-					"map_rom",
-					NULL };
-static const char *part_probe_types[] = { "cmdlinepart", "RedBoot", "afs",
-					  NULL };
+static const char * const rom_probe_types[] = {
+	"cfi_probe", "jedec_probe", "qinfo_probe", "map_rom", NULL };
+
+static const char * const part_probe_types[] = {
+	"cmdlinepart", "RedBoot", "afs", NULL };
 
 static int physmap_flash_probe(struct platform_device *dev)
 {
 	struct physmap_flash_data *physmap_data;
 	struct physmap_flash_info *info;
-	const char **probe_type;
-	const char **part_types;
+	const char * const *probe_type;
+	const char * const *part_types;
 	int err = 0;
 	int i;
 	int devices_found = 0;
diff --git a/drivers/mtd/maps/physmap_of.c b/drivers/mtd/maps/physmap_of.c
index 363939dfad05..d11109762ac5 100644
--- a/drivers/mtd/maps/physmap_of.c
+++ b/drivers/mtd/maps/physmap_of.c
@@ -71,6 +71,9 @@ static int of_flash_remove(struct platform_device *dev)
 	return 0;
 }
 
+static const char * const rom_probe_types[] = {
+	"cfi_probe", "jedec_probe", "map_rom" };
+
 /* Helper function to handle probing of the obsolete "direct-mapped"
  * compatible binding, which has an extra "probe-type" property
  * describing the type of flash probe necessary. */
@@ -80,8 +83,6 @@ static struct mtd_info *obsolete_probe(struct platform_device *dev,
 	struct device_node *dp = dev->dev.of_node;
 	const char *of_probe;
 	struct mtd_info *mtd;
-	static const char *rom_probe_types[]
-		= { "cfi_probe", "jedec_probe", "map_rom"};
 	int i;
 
 	dev_warn(&dev->dev, "Device tree uses obsolete \"direct-mapped\" "
@@ -111,9 +112,10 @@ static struct mtd_info *obsolete_probe(struct platform_device *dev,
    specifies the list of partition probers to use. If none is given then the
    default is use. These take precedence over other device tree
    information. */
-static const char *part_probe_types_def[] = { "cmdlinepart", "RedBoot",
-					"ofpart", "ofoldpart", NULL };
-static const char **of_get_probes(struct device_node *dp)
+static const char * const part_probe_types_def[] = {
+	"cmdlinepart", "RedBoot", "ofpart", "ofoldpart", NULL };
+
+static const char * const *of_get_probes(struct device_node *dp)
 {
 	const char *cp;
 	int cplen;
@@ -142,7 +144,7 @@ static const char **of_get_probes(struct device_node *dp)
 	return res;
 }
 
-static void of_free_probes(const char **probes)
+static void of_free_probes(const char * const *probes)
 {
 	if (probes != part_probe_types_def)
 		kfree(probes);
@@ -151,7 +153,7 @@ static void of_free_probes(const char **probes)
 static struct of_device_id of_flash_match[];
 static int of_flash_probe(struct platform_device *dev)
 {
-	const char **part_probe_types;
+	const char * const *part_probe_types;
 	const struct of_device_id *match;
 	struct device_node *dp = dev->dev.of_node;
 	struct resource res;
diff --git a/drivers/mtd/maps/plat-ram.c b/drivers/mtd/maps/plat-ram.c
index 2de66b062f0d..71fdda29594b 100644
--- a/drivers/mtd/maps/plat-ram.c
+++ b/drivers/mtd/maps/plat-ram.c
@@ -199,7 +199,7 @@ static int platram_probe(struct platform_device *pdev)
 	 * supplied by the platform_data struct */
 
 	if (pdata->map_probes) {
-		const char **map_probes = pdata->map_probes;
+		const char * const *map_probes = pdata->map_probes;
 
 		for ( ; !info->mtd && *map_probes; map_probes++)
 			info->mtd = do_map_probe(*map_probes , &info->map);
diff --git a/drivers/mtd/maps/pxa2xx-flash.c b/drivers/mtd/maps/pxa2xx-flash.c
index 43e3dbb976d9..acb1dbcf7ce5 100644
--- a/drivers/mtd/maps/pxa2xx-flash.c
+++ b/drivers/mtd/maps/pxa2xx-flash.c
@@ -45,9 +45,7 @@ struct pxa2xx_flash_info {
 	struct map_info		map;
 };
 
-
-static const char *probes[] = { "RedBoot", "cmdlinepart", NULL };
-
+static const char * const probes[] = { "RedBoot", "cmdlinepart", NULL };
 
 static int pxa2xx_flash_probe(struct platform_device *pdev)
 {
diff --git a/drivers/mtd/maps/rbtx4939-flash.c b/drivers/mtd/maps/rbtx4939-flash.c
index 49c3fe715eee..ac02fbffd6df 100644
--- a/drivers/mtd/maps/rbtx4939-flash.c
+++ b/drivers/mtd/maps/rbtx4939-flash.c
@@ -45,14 +45,15 @@ static int rbtx4939_flash_remove(struct platform_device *dev)
 	return 0;
 }
 
-static const char *rom_probe_types[] = { "cfi_probe", "jedec_probe", NULL };
+static const char * const rom_probe_types[] = {
+	"cfi_probe", "jedec_probe", NULL };
 
 static int rbtx4939_flash_probe(struct platform_device *dev)
 {
 	struct rbtx4939_flash_data *pdata;
 	struct rbtx4939_flash_info *info;
 	struct resource *res;
-	const char **probe_type;
+	const char * const *probe_type;
 	int err = 0;
 	unsigned long size;
 
diff --git a/drivers/mtd/maps/rpxlite.c b/drivers/mtd/maps/rpxlite.c
deleted file mode 100644
index ed88225bf667..000000000000
--- a/drivers/mtd/maps/rpxlite.c
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
- * Handle mapping of the flash on the RPX Lite and CLLF boards
- */
-
-#include <linux/module.h>
-#include <linux/types.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <asm/io.h>
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/map.h>
-
-
-#define WINDOW_ADDR 0xfe000000
-#define WINDOW_SIZE 0x800000
-
-static struct mtd_info *mymtd;
-
-static struct map_info rpxlite_map = {
-	.name = "RPX",
-	.size = WINDOW_SIZE,
-	.bankwidth = 4,
-	.phys = WINDOW_ADDR,
-};
-
-static int __init init_rpxlite(void)
-{
-	printk(KERN_NOTICE "RPX Lite or CLLF flash device: %x at %x\n", WINDOW_SIZE*4, WINDOW_ADDR);
-	rpxlite_map.virt = ioremap(WINDOW_ADDR, WINDOW_SIZE * 4);
-
-	if (!rpxlite_map.virt) {
-		printk("Failed to ioremap\n");
-		return -EIO;
-	}
-	simple_map_init(&rpxlite_map);
-	mymtd = do_map_probe("cfi_probe", &rpxlite_map);
-	if (mymtd) {
-		mymtd->owner = THIS_MODULE;
-		mtd_device_register(mymtd, NULL, 0);
-		return 0;
-	}
-
-	iounmap((void *)rpxlite_map.virt);
-	return -ENXIO;
-}
-
-static void __exit cleanup_rpxlite(void)
-{
-	if (mymtd) {
-		mtd_device_unregister(mymtd);
-		map_destroy(mymtd);
-	}
-	if (rpxlite_map.virt) {
-		iounmap((void *)rpxlite_map.virt);
-		rpxlite_map.virt = 0;
-	}
-}
-
-module_init(init_rpxlite);
-module_exit(cleanup_rpxlite);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Arnold Christensen <AKC@pel.dk>");
-MODULE_DESCRIPTION("MTD map driver for RPX Lite and CLLF boards");
diff --git a/drivers/mtd/maps/sa1100-flash.c b/drivers/mtd/maps/sa1100-flash.c
index f694417cf7e6..29e3dcaa1d90 100644
--- a/drivers/mtd/maps/sa1100-flash.c
+++ b/drivers/mtd/maps/sa1100-flash.c
@@ -244,7 +244,7 @@ static struct sa_info *sa1100_setup_mtd(struct platform_device *pdev,
 	return ERR_PTR(ret);
 }
 
-static const char *part_probes[] = { "cmdlinepart", "RedBoot", NULL };
+static const char * const part_probes[] = { "cmdlinepart", "RedBoot", NULL };
 
 static int sa1100_mtd_probe(struct platform_device *pdev)
 {
diff --git a/drivers/mtd/maps/solutionengine.c b/drivers/mtd/maps/solutionengine.c
index 9d900ada6708..83a7a7091562 100644
--- a/drivers/mtd/maps/solutionengine.c
+++ b/drivers/mtd/maps/solutionengine.c
@@ -31,7 +31,7 @@ struct map_info soleng_flash_map = {
 	.bankwidth = 4,
 };
 
-static const char *probes[] = { "RedBoot", "cmdlinepart", NULL };
+static const char * const probes[] = { "RedBoot", "cmdlinepart", NULL };
 
 #ifdef CONFIG_MTD_SUPERH_RESERVE
 static struct mtd_partition superh_se_partitions[] = {
diff --git a/drivers/mtd/maps/tqm8xxl.c b/drivers/mtd/maps/tqm8xxl.c
deleted file mode 100644
index d78587990e7e..000000000000
--- a/drivers/mtd/maps/tqm8xxl.c
+++ /dev/null
@@ -1,249 +0,0 @@
-/*
- * Handle mapping of the flash memory access routines
- * on TQM8xxL based devices.
- *
- * based on rpxlite.c
- *
- * Copyright(C) 2001 Kirk Lee <kirk@hpc.ee.ntu.edu.tw>
- *
- * This code is GPLed
- *
- */
-
-/*
- * According to TQM8xxL hardware manual, TQM8xxL series have
- * following flash memory organisations:
- *	| capacity |	| chip type |	| bank0 |	| bank1 |
- *	    2MiB	   512Kx16	  2MiB		   0
- *	    4MiB	   1Mx16	  4MiB		   0
- *	    8MiB	   1Mx16	  4MiB		   4MiB
- * Thus, we choose CONFIG_MTD_CFI_I2 & CONFIG_MTD_CFI_B4 at
- * kernel configuration.
- */
-#include <linux/module.h>
-#include <linux/types.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/slab.h>
-
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/map.h>
-#include <linux/mtd/partitions.h>
-
-#include <asm/io.h>
-
-#define FLASH_ADDR 0x40000000
-#define FLASH_SIZE 0x00800000
-#define FLASH_BANK_MAX 4
-
-// trivial struct to describe partition information
-struct mtd_part_def
-{
-	int nums;
-	unsigned char *type;
-	struct mtd_partition* mtd_part;
-};
-
-//static struct mtd_info *mymtd;
-static struct mtd_info* mtd_banks[FLASH_BANK_MAX];
-static struct map_info* map_banks[FLASH_BANK_MAX];
-static struct mtd_part_def part_banks[FLASH_BANK_MAX];
-static unsigned long num_banks;
-static void __iomem *start_scan_addr;
-
-/*
- * Here are partition information for all known TQM8xxL series devices.
- * See include/linux/mtd/partitions.h for definition of the mtd_partition
- * structure.
- *
- * The *_max_flash_size is the maximum possible mapped flash size which
- * is not necessarily the actual flash size.  It must correspond to the
- * value specified in the mapping definition defined by the
- * "struct map_desc *_io_desc" for the corresponding machine.
- */
-
-/* Currently, TQM8xxL has up to 8MiB flash */
-static unsigned long tqm8xxl_max_flash_size = 0x00800000;
-
-/* partition definition for first flash bank
- * (cf. "drivers/char/flash_config.c")
- */
-static struct mtd_partition tqm8xxl_partitions[] = {
-	{
-	  .name = "ppcboot",
-	  .offset = 0x00000000,
-	  .size = 0x00020000,           /* 128KB           */
-	  .mask_flags = MTD_WRITEABLE,  /* force read-only */
-	},
-	{
-	  .name = "kernel",             /* default kernel image */
-	  .offset = 0x00020000,
-	  .size = 0x000e0000,
-	  .mask_flags = MTD_WRITEABLE,  /* force read-only */
-	},
-	{
-	  .name = "user",
-	  .offset = 0x00100000,
-	  .size = 0x00100000,
-	},
-	{
-	  .name = "initrd",
-	  .offset = 0x00200000,
-	  .size = 0x00200000,
-	}
-};
-/* partition definition for second flash bank */
-static struct mtd_partition tqm8xxl_fs_partitions[] = {
-	{
-	  .name = "cramfs",
-	  .offset = 0x00000000,
-	  .size = 0x00200000,
-	},
-	{
-	  .name = "jffs",
-	  .offset = 0x00200000,
-	  .size = 0x00200000,
-	  //.size = MTDPART_SIZ_FULL,
-	}
-};
-
-static int __init init_tqm_mtd(void)
-{
-	int idx = 0, ret = 0;
-	unsigned long flash_addr, flash_size, mtd_size = 0;
-	/* pointer to TQM8xxL board info data */
-	bd_t *bd = (bd_t *)__res;
-
-	flash_addr = bd->bi_flashstart;
-	flash_size = bd->bi_flashsize;
-
-	//request maximum flash size address space
-	start_scan_addr = ioremap(flash_addr, flash_size);
-	if (!start_scan_addr) {
-		printk(KERN_WARNING "%s:Failed to ioremap address:0x%x\n", __func__, flash_addr);
-		return -EIO;
-	}
-
-	for (idx = 0 ; idx < FLASH_BANK_MAX ; idx++) {
-		if(mtd_size >= flash_size)
-			break;
-
-		printk(KERN_INFO "%s: chip probing count %d\n", __func__, idx);
-
-		map_banks[idx] = kzalloc(sizeof(struct map_info), GFP_KERNEL);
-		if(map_banks[idx] == NULL) {
-			ret = -ENOMEM;
-			/* FIXME: What if some MTD devices were probed already? */
-			goto error_mem;
-		}
-
-		map_banks[idx]->name = kmalloc(16, GFP_KERNEL);
-
-		if (!map_banks[idx]->name) {
-			ret = -ENOMEM;
-			/* FIXME: What if some MTD devices were probed already? */
-			goto error_mem;
-		}
-		sprintf(map_banks[idx]->name, "TQM8xxL%d", idx);
-
-		map_banks[idx]->size = flash_size;
-		map_banks[idx]->bankwidth = 4;
-
-		simple_map_init(map_banks[idx]);
-
-		map_banks[idx]->virt = start_scan_addr;
-		map_banks[idx]->phys = flash_addr;
-		/* FIXME: This looks utterly bogus, but I'm trying to
-		   preserve the behaviour of the original (shown here)...
-
-		map_banks[idx]->map_priv_1 =
-		start_scan_addr + ((idx > 0) ?
-		(mtd_banks[idx-1] ? mtd_banks[idx-1]->size : 0) : 0);
-		*/
-
-		if (idx && mtd_banks[idx-1]) {
-			map_banks[idx]->virt += mtd_banks[idx-1]->size;
-			map_banks[idx]->phys += mtd_banks[idx-1]->size;
-		}
-
-		//start to probe flash chips
-		mtd_banks[idx] = do_map_probe("cfi_probe", map_banks[idx]);
-
-		if (mtd_banks[idx]) {
-			mtd_banks[idx]->owner = THIS_MODULE;
-			mtd_size += mtd_banks[idx]->size;
-			num_banks++;
-
-			printk(KERN_INFO "%s: bank%d, name:%s, size:%dbytes \n", __func__, num_banks,
-			mtd_banks[idx]->name, mtd_banks[idx]->size);
-		}
-	}
-
-	/* no supported flash chips found */
-	if (!num_banks) {
-		printk(KERN_NOTICE "TQM8xxL: No support flash chips found!\n");
-		ret = -ENXIO;
-		goto error_mem;
-	}
-
-	/*
-	 * Select Static partition definitions
-	 */
-	part_banks[0].mtd_part = tqm8xxl_partitions;
-	part_banks[0].type = "Static image";
-	part_banks[0].nums = ARRAY_SIZE(tqm8xxl_partitions);
-
-	part_banks[1].mtd_part = tqm8xxl_fs_partitions;
-	part_banks[1].type = "Static file system";
-	part_banks[1].nums = ARRAY_SIZE(tqm8xxl_fs_partitions);
-
-	for(idx = 0; idx < num_banks ; idx++) {
-		if (part_banks[idx].nums == 0)
-			printk(KERN_NOTICE "TQM flash%d: no partition info available, registering whole flash at once\n", idx);
-		else
-			printk(KERN_NOTICE "TQM flash%d: Using %s partition definition\n",
-					idx, part_banks[idx].type);
-		mtd_device_register(mtd_banks[idx], part_banks[idx].mtd_part,
-		part_banks[idx].nums);
-	}
-	return 0;
-error_mem:
-	for(idx = 0 ; idx < FLASH_BANK_MAX ; idx++) {
-		if(map_banks[idx] != NULL) {
-			kfree(map_banks[idx]->name);
-			map_banks[idx]->name = NULL;
-			kfree(map_banks[idx]);
-			map_banks[idx] = NULL;
-		}
-	}
-error:
-	iounmap(start_scan_addr);
-	return ret;
-}
-
-static void __exit cleanup_tqm_mtd(void)
-{
-	unsigned int idx = 0;
-	for(idx = 0 ; idx < num_banks ; idx++) {
-		/* destroy mtd_info previously allocated */
-		if (mtd_banks[idx]) {
-			mtd_device_unregister(mtd_banks[idx]);
-			map_destroy(mtd_banks[idx]);
-		}
-		/* release map_info not used anymore */
-		kfree(map_banks[idx]->name);
-		kfree(map_banks[idx]);
-	}
-
-	if (start_scan_addr) {
-		iounmap(start_scan_addr);
-		start_scan_addr = 0;
-	}
-}
-
-module_init(init_tqm_mtd);
-module_exit(cleanup_tqm_mtd);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Kirk Lee <kirk@hpc.ee.ntu.edu.tw>");
-MODULE_DESCRIPTION("MTD map driver for TQM8xxL boards");
diff --git a/drivers/mtd/maps/tsunami_flash.c b/drivers/mtd/maps/tsunami_flash.c
index 1de390e1c2fb..da2cdb5fd6db 100644
--- a/drivers/mtd/maps/tsunami_flash.c
+++ b/drivers/mtd/maps/tsunami_flash.c
@@ -82,11 +82,12 @@ static void __exit  cleanup_tsunami_flash(void)
 	tsunami_flash_mtd = 0;
 }
 
+static const char * const rom_probe_types[] = {
+	"cfi_probe", "jedec_probe", "map_rom", NULL };
 
 static int __init init_tsunami_flash(void)
 {
-	static const char *rom_probe_types[] = { "cfi_probe", "jedec_probe", "map_rom", NULL };
-	char **type;
+	const char * const *type;
 
 	tsunami_tig_writeb(FLASH_ENABLE_BYTE, FLASH_ENABLE_PORT);
 
diff --git a/drivers/mtd/mtd_blkdevs.c b/drivers/mtd/mtd_blkdevs.c
index 5ad39bb5ab4c..5073cbc796d8 100644
--- a/drivers/mtd/mtd_blkdevs.c
+++ b/drivers/mtd/mtd_blkdevs.c
@@ -237,13 +237,12 @@ error_put:
 	return ret;
 }
 
-static int blktrans_release(struct gendisk *disk, fmode_t mode)
+static void blktrans_release(struct gendisk *disk, fmode_t mode)
 {
 	struct mtd_blktrans_dev *dev = blktrans_dev_get(disk);
-	int ret = 0;
 
 	if (!dev)
-		return ret;
+		return;
 
 	mutex_lock(&dev->lock);
 
@@ -254,13 +253,13 @@ static int blktrans_release(struct gendisk *disk, fmode_t mode)
 	module_put(dev->tr->owner);
 
 	if (dev->mtd) {
-		ret = dev->tr->release ? dev->tr->release(dev) : 0;
+		if (dev->tr->release)
+			dev->tr->release(dev);
 		__put_mtd_device(dev->mtd);
 	}
 unlock:
 	mutex_unlock(&dev->lock);
 	blktrans_dev_put(dev);
-	return ret;
 }
 
 static int blktrans_getgeo(struct block_device *bdev, struct hd_geometry *geo)
diff --git a/drivers/mtd/mtdblock.c b/drivers/mtd/mtdblock.c
index 6c6d80736fad..2aef5dda522b 100644
--- a/drivers/mtd/mtdblock.c
+++ b/drivers/mtd/mtdblock.c
@@ -308,7 +308,7 @@ static int mtdblock_open(struct mtd_blktrans_dev *mbd)
 	return 0;
 }
 
-static int mtdblock_release(struct mtd_blktrans_dev *mbd)
+static void mtdblock_release(struct mtd_blktrans_dev *mbd)
 {
 	struct mtdblk_dev *mtdblk = container_of(mbd, struct mtdblk_dev, mbd);
 
@@ -333,8 +333,6 @@ static int mtdblock_release(struct mtd_blktrans_dev *mbd)
 	mutex_unlock(&mtdblks_lock);
 
 	pr_debug("ok\n");
-
-	return 0;
 }
 
 static int mtdblock_flush(struct mtd_blktrans_dev *dev)
diff --git a/drivers/mtd/mtdchar.c b/drivers/mtd/mtdchar.c
index dc571ebc1aa0..c719879284bd 100644
--- a/drivers/mtd/mtdchar.c
+++ b/drivers/mtd/mtdchar.c
@@ -38,6 +38,8 @@
 
 #include <asm/uaccess.h>
 
+#include "mtdcore.h"
+
 static DEFINE_MUTEX(mtd_mutex);
 
 /*
@@ -365,37 +367,35 @@ static void mtdchar_erase_callback (struct erase_info *instr)
 	wake_up((wait_queue_head_t *)instr->priv);
 }
 
-#ifdef CONFIG_HAVE_MTD_OTP
 static int otp_select_filemode(struct mtd_file_info *mfi, int mode)
 {
 	struct mtd_info *mtd = mfi->mtd;
 	size_t retlen;
-	int ret = 0;
-
-	/*
-	 * Make a fake call to mtd_read_fact_prot_reg() to check if OTP
-	 * operations are supported.
-	 */
-	if (mtd_read_fact_prot_reg(mtd, -1, 0, &retlen, NULL) == -EOPNOTSUPP)
-		return -EOPNOTSUPP;
 
 	switch (mode) {
 	case MTD_OTP_FACTORY:
+		if (mtd_read_fact_prot_reg(mtd, -1, 0, &retlen, NULL) ==
+				-EOPNOTSUPP)
+			return -EOPNOTSUPP;
+
 		mfi->mode = MTD_FILE_MODE_OTP_FACTORY;
 		break;
 	case MTD_OTP_USER:
+		if (mtd_read_user_prot_reg(mtd, -1, 0, &retlen, NULL) ==
+				-EOPNOTSUPP)
+			return -EOPNOTSUPP;
+
 		mfi->mode = MTD_FILE_MODE_OTP_USER;
 		break;
-	default:
-		ret = -EINVAL;
 	case MTD_OTP_OFF:
+		mfi->mode = MTD_FILE_MODE_NORMAL;
 		break;
+	default:
+		return -EINVAL;
 	}
-	return ret;
+
+	return 0;
 }
-#else
-# define otp_select_filemode(f,m)	-EOPNOTSUPP
-#endif
 
 static int mtdchar_writeoob(struct file *file, struct mtd_info *mtd,
 	uint64_t start, uint32_t length, void __user *ptr,
@@ -888,7 +888,6 @@ static int mtdchar_ioctl(struct file *file, u_int cmd, u_long arg)
 		break;
 	}
 
-#ifdef CONFIG_HAVE_MTD_OTP
 	case OTPSELECT:
 	{
 		int mode;
@@ -944,7 +943,6 @@ static int mtdchar_ioctl(struct file *file, u_int cmd, u_long arg)
 		ret = mtd_lock_user_prot_reg(mtd, oinfo.start, oinfo.length);
 		break;
 	}
-#endif
 
 	/* This ioctl is being deprecated - it truncates the ECC layout */
 	case ECCGETLAYOUT:
@@ -1185,23 +1183,25 @@ static struct file_system_type mtd_inodefs_type = {
 };
 MODULE_ALIAS_FS("mtd_inodefs");
 
-static int __init init_mtdchar(void)
+int __init init_mtdchar(void)
 {
 	int ret;
 
 	ret = __register_chrdev(MTD_CHAR_MAJOR, 0, 1 << MINORBITS,
 				   "mtd", &mtd_fops);
 	if (ret < 0) {
-		pr_notice("Can't allocate major number %d for "
-				"Memory Technology Devices.\n", MTD_CHAR_MAJOR);
+		pr_err("Can't allocate major number %d for MTD\n",
+		       MTD_CHAR_MAJOR);
 		return ret;
 	}
 
 	ret = register_filesystem(&mtd_inodefs_type);
 	if (ret) {
-		pr_notice("Can't register mtd_inodefs filesystem: %d\n", ret);
+		pr_err("Can't register mtd_inodefs filesystem, error %d\n",
+		       ret);
 		goto err_unregister_chdev;
 	}
+
 	return ret;
 
 err_unregister_chdev:
@@ -1209,18 +1209,10 @@ err_unregister_chdev:
 	return ret;
 }
 
-static void __exit cleanup_mtdchar(void)
+void __exit cleanup_mtdchar(void)
 {
 	unregister_filesystem(&mtd_inodefs_type);
 	__unregister_chrdev(MTD_CHAR_MAJOR, 0, 1 << MINORBITS, "mtd");
 }
 
-module_init(init_mtdchar);
-module_exit(cleanup_mtdchar);
-
-MODULE_ALIAS_CHARDEV_MAJOR(MTD_CHAR_MAJOR);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("David Woodhouse <dwmw2@infradead.org>");
-MODULE_DESCRIPTION("Direct character-device access to MTD devices");
 MODULE_ALIAS_CHARDEV_MAJOR(MTD_CHAR_MAJOR);
diff --git a/drivers/mtd/mtdcore.c b/drivers/mtd/mtdcore.c
index 61d5f56473e1..c400c57c394a 100644
--- a/drivers/mtd/mtdcore.c
+++ b/drivers/mtd/mtdcore.c
@@ -36,11 +36,13 @@
 #include <linux/idr.h>
 #include <linux/backing-dev.h>
 #include <linux/gfp.h>
+#include <linux/slab.h>
 
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/partitions.h>
 
 #include "mtdcore.h"
+
 /*
  * backing device capabilities for non-mappable devices (such as NAND flash)
  * - permits private mappings, copies are taken of the data
@@ -96,11 +98,7 @@ EXPORT_SYMBOL_GPL(__mtd_next_device);
 static LIST_HEAD(mtd_notifiers);
 
 
-#if defined(CONFIG_MTD_CHAR) || defined(CONFIG_MTD_CHAR_MODULE)
 #define MTD_DEVT(index) MKDEV(MTD_CHAR_MAJOR, (index)*2)
-#else
-#define MTD_DEVT(index) 0
-#endif
 
 /* REVISIT once MTD uses the driver model better, whoever allocates
  * the mtd_info will probably want to use the release() hook...
@@ -492,7 +490,7 @@ out_error:
  *
  * Returns zero in case of success and a negative error code in case of failure.
  */
-int mtd_device_parse_register(struct mtd_info *mtd, const char **types,
+int mtd_device_parse_register(struct mtd_info *mtd, const char * const *types,
 			      struct mtd_part_parser_data *parser_data,
 			      const struct mtd_partition *parts,
 			      int nr_parts)
@@ -1116,8 +1114,6 @@ EXPORT_SYMBOL_GPL(mtd_kmalloc_up_to);
 /*====================================================================*/
 /* Support for /proc/mtd */
 
-static struct proc_dir_entry *proc_mtd;
-
 static int mtd_proc_show(struct seq_file *m, void *v)
 {
 	struct mtd_info *mtd;
@@ -1163,6 +1159,8 @@ static int __init mtd_bdi_init(struct backing_dev_info *bdi, const char *name)
 	return ret;
 }
 
+static struct proc_dir_entry *proc_mtd;
+
 static int __init init_mtd(void)
 {
 	int ret;
@@ -1183,11 +1181,17 @@ static int __init init_mtd(void)
 	if (ret)
 		goto err_bdi3;
 
-#ifdef CONFIG_PROC_FS
 	proc_mtd = proc_create("mtd", 0, NULL, &mtd_proc_ops);
-#endif /* CONFIG_PROC_FS */
+
+	ret = init_mtdchar();
+	if (ret)
+		goto out_procfs;
+
 	return 0;
 
+out_procfs:
+	if (proc_mtd)
+		remove_proc_entry("mtd", NULL);
 err_bdi3:
 	bdi_destroy(&mtd_bdi_ro_mappable);
 err_bdi2:
@@ -1201,10 +1205,9 @@ err_reg:
 
 static void __exit cleanup_mtd(void)
 {
-#ifdef CONFIG_PROC_FS
+	cleanup_mtdchar();
 	if (proc_mtd)
-		remove_proc_entry( "mtd", NULL);
-#endif /* CONFIG_PROC_FS */
+		remove_proc_entry("mtd", NULL);
 	class_unregister(&mtd_class);
 	bdi_destroy(&mtd_bdi_unmappable);
 	bdi_destroy(&mtd_bdi_ro_mappable);
diff --git a/drivers/mtd/mtdcore.h b/drivers/mtd/mtdcore.h
index 961a38408542..7b0353399a10 100644
--- a/drivers/mtd/mtdcore.h
+++ b/drivers/mtd/mtdcore.h
@@ -1,23 +1,21 @@
-/* linux/drivers/mtd/mtdcore.h
- *
- * Header file for driver private mtdcore exports
- *
+/*
+ * These are exported solely for the purpose of mtd_blkdevs.c and mtdchar.c.
+ * You should not use them for _anything_ else.
  */
 
-/* These are exported solely for the purpose of mtd_blkdevs.c. You
-   should not use them for _anything_ else */
-
 extern struct mutex mtd_table_mutex;
-extern struct mtd_info *__mtd_next_device(int i);
 
-extern int add_mtd_device(struct mtd_info *mtd);
-extern int del_mtd_device(struct mtd_info *mtd);
-extern int add_mtd_partitions(struct mtd_info *, const struct mtd_partition *,
-			      int);
-extern int del_mtd_partitions(struct mtd_info *);
-extern int parse_mtd_partitions(struct mtd_info *master, const char **types,
-				struct mtd_partition **pparts,
-				struct mtd_part_parser_data *data);
+struct mtd_info *__mtd_next_device(int i);
+int add_mtd_device(struct mtd_info *mtd);
+int del_mtd_device(struct mtd_info *mtd);
+int add_mtd_partitions(struct mtd_info *, const struct mtd_partition *, int);
+int del_mtd_partitions(struct mtd_info *);
+int parse_mtd_partitions(struct mtd_info *master, const char * const *types,
+			 struct mtd_partition **pparts,
+			 struct mtd_part_parser_data *data);
+
+int __init init_mtdchar(void);
+void __exit cleanup_mtdchar(void);
 
 #define mtd_for_each_device(mtd)			\
 	for ((mtd) = __mtd_next_device(0);		\
diff --git a/drivers/mtd/mtdpart.c b/drivers/mtd/mtdpart.c
index 70fa70a8318f..301493382cd0 100644
--- a/drivers/mtd/mtdpart.c
+++ b/drivers/mtd/mtdpart.c
@@ -694,7 +694,7 @@ EXPORT_SYMBOL_GPL(deregister_mtd_parser);
  * Do not forget to update 'parse_mtd_partitions()' kerneldoc comment if you
  * are changing this array!
  */
-static const char *default_mtd_part_types[] = {
+static const char * const default_mtd_part_types[] = {
 	"cmdlinepart",
 	"ofpart",
 	NULL
@@ -720,7 +720,7 @@ static const char *default_mtd_part_types[] = {
  * o a positive number of found partitions, in which case on exit @pparts will
  *   point to an array containing this number of &struct mtd_info objects.
  */
-int parse_mtd_partitions(struct mtd_info *master, const char **types,
+int parse_mtd_partitions(struct mtd_info *master, const char *const *types,
 			 struct mtd_partition **pparts,
 			 struct mtd_part_parser_data *data)
 {
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index 81bf5e52601e..a60f6c17f57b 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -41,14 +41,6 @@ config MTD_SM_COMMON
 	tristate
 	default n
 
-config MTD_NAND_MUSEUM_IDS
-	bool "Enable chip ids for obsolete ancient NAND devices"
-	default n
-	help
-	  Enable this option only when your board has first generation
-	  NAND chips (page size 256 byte, erase size 4-8KiB). The IDs
-	  of these chips were reused by later, larger chips.
-
 config MTD_NAND_DENALI
         tristate "Support Denali NAND controller"
         help
@@ -81,15 +73,9 @@ config MTD_NAND_DENALI_SCRATCH_REG_ADDR
           scratch register here to enable this feature. On Intel Moorestown
           boards, the scratch register is at 0xFF108018.
 
-config MTD_NAND_H1900
-	tristate "iPAQ H1900 flash"
-	depends on ARCH_PXA && BROKEN
-	help
-	  This enables the driver for the iPAQ h1900 flash.
-
 config MTD_NAND_GPIO
 	tristate "GPIO NAND Flash driver"
-	depends on GENERIC_GPIO && ARM
+	depends on GPIOLIB && ARM
 	help
 	  This enables a GPIO based NAND flash driver.
 
@@ -201,22 +187,6 @@ config MTD_NAND_BF5XX_BOOTROM_ECC
 
 	  If unsure, say N.
 
-config MTD_NAND_RTC_FROM4
-	tristate "Renesas Flash ROM 4-slot interface board (FROM_BOARD4)"
-	depends on SH_SOLUTION_ENGINE
-	select REED_SOLOMON
-	select REED_SOLOMON_DEC8
-	select BITREVERSE
-	help
-	  This enables the driver for the Renesas Technology AG-AND
-	  flash interface board (FROM_BOARD4)
-
-config MTD_NAND_PPCHAMELEONEVB
-	tristate "NAND Flash device on PPChameleonEVB board"
-	depends on PPCHAMELEONEVB && BROKEN
-	help
-	  This enables the NAND flash driver on the PPChameleon EVB Board.
-
 config MTD_NAND_S3C2410
 	tristate "NAND Flash support for Samsung S3C SoCs"
 	depends on ARCH_S3C24XX || ARCH_S3C64XX
diff --git a/drivers/mtd/nand/Makefile b/drivers/mtd/nand/Makefile
index d76d91205691..bb8189172f62 100644
--- a/drivers/mtd/nand/Makefile
+++ b/drivers/mtd/nand/Makefile
@@ -15,14 +15,11 @@ obj-$(CONFIG_MTD_NAND_DENALI_PCI)	+= denali_pci.o
 obj-$(CONFIG_MTD_NAND_DENALI_DT)	+= denali_dt.o
 obj-$(CONFIG_MTD_NAND_AU1550)		+= au1550nd.o
 obj-$(CONFIG_MTD_NAND_BF5XX)		+= bf5xx_nand.o
-obj-$(CONFIG_MTD_NAND_PPCHAMELEONEVB)	+= ppchameleonevb.o
 obj-$(CONFIG_MTD_NAND_S3C2410)		+= s3c2410.o
 obj-$(CONFIG_MTD_NAND_DAVINCI)		+= davinci_nand.o
 obj-$(CONFIG_MTD_NAND_DISKONCHIP)	+= diskonchip.o
 obj-$(CONFIG_MTD_NAND_DOCG4)		+= docg4.o
 obj-$(CONFIG_MTD_NAND_FSMC)		+= fsmc_nand.o
-obj-$(CONFIG_MTD_NAND_H1900)		+= h1910.o
-obj-$(CONFIG_MTD_NAND_RTC_FROM4)	+= rtc_from4.o
 obj-$(CONFIG_MTD_NAND_SHARPSL)		+= sharpsl.o
 obj-$(CONFIG_MTD_NAND_NANDSIM)		+= nandsim.o
 obj-$(CONFIG_MTD_NAND_CS553X)		+= cs553x_nand.o
diff --git a/drivers/mtd/nand/atmel_nand.c b/drivers/mtd/nand/atmel_nand.c
index ffcbcca2fd2d..2d23d2929438 100644
--- a/drivers/mtd/nand/atmel_nand.c
+++ b/drivers/mtd/nand/atmel_nand.c
@@ -1737,20 +1737,7 @@ static struct platform_driver atmel_nand_driver = {
 	},
 };
 
-static int __init atmel_nand_init(void)
-{
-	return platform_driver_probe(&atmel_nand_driver, atmel_nand_probe);
-}
-
-
-static void __exit atmel_nand_exit(void)
-{
-	platform_driver_unregister(&atmel_nand_driver);
-}
-
-
-module_init(atmel_nand_init);
-module_exit(atmel_nand_exit);
+module_platform_driver_probe(atmel_nand_driver, atmel_nand_probe);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Rick Bronson");
diff --git a/drivers/mtd/nand/bf5xx_nand.c b/drivers/mtd/nand/bf5xx_nand.c
index 4271e948d1e2..776df3694f75 100644
--- a/drivers/mtd/nand/bf5xx_nand.c
+++ b/drivers/mtd/nand/bf5xx_nand.c
@@ -874,21 +874,7 @@ static struct platform_driver bf5xx_nand_driver = {
 	},
 };
 
-static int __init bf5xx_nand_init(void)
-{
-	printk(KERN_INFO "%s, Version %s (c) 2007 Analog Devices, Inc.\n",
-		DRV_DESC, DRV_VERSION);
-
-	return platform_driver_register(&bf5xx_nand_driver);
-}
-
-static void __exit bf5xx_nand_exit(void)
-{
-	platform_driver_unregister(&bf5xx_nand_driver);
-}
-
-module_init(bf5xx_nand_init);
-module_exit(bf5xx_nand_exit);
+module_platform_driver(bf5xx_nand_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR(DRV_AUTHOR);
diff --git a/drivers/mtd/nand/cafe_nand.c b/drivers/mtd/nand/cafe_nand.c
index 010d61266536..c34985a55101 100644
--- a/drivers/mtd/nand/cafe_nand.c
+++ b/drivers/mtd/nand/cafe_nand.c
@@ -303,13 +303,7 @@ static void cafe_nand_cmdfunc(struct mtd_info *mtd, unsigned command,
 	case NAND_CMD_SEQIN:
 	case NAND_CMD_RNDIN:
 	case NAND_CMD_STATUS:
-	case NAND_CMD_DEPLETE1:
 	case NAND_CMD_RNDOUT:
-	case NAND_CMD_STATUS_ERROR:
-	case NAND_CMD_STATUS_ERROR0:
-	case NAND_CMD_STATUS_ERROR1:
-	case NAND_CMD_STATUS_ERROR2:
-	case NAND_CMD_STATUS_ERROR3:
 		cafe_writel(cafe, cafe->ctl2, NAND_CTRL2);
 		return;
 	}
@@ -536,8 +530,8 @@ static int cafe_nand_write_page_lowlevel(struct mtd_info *mtd,
 }
 
 static int cafe_nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
-				const uint8_t *buf, int oob_required, int page,
-				int cached, int raw)
+			uint32_t offset, int data_len, const uint8_t *buf,
+			int oob_required, int page, int cached, int raw)
 {
 	int status;
 
diff --git a/drivers/mtd/nand/davinci_nand.c b/drivers/mtd/nand/davinci_nand.c
index 94e17af8e450..c3e15a558173 100644
--- a/drivers/mtd/nand/davinci_nand.c
+++ b/drivers/mtd/nand/davinci_nand.c
@@ -34,6 +34,7 @@
 #include <linux/mtd/partitions.h>
 #include <linux/slab.h>
 #include <linux/of_device.h>
+#include <linux/of.h>
 
 #include <linux/platform_data/mtd-davinci.h>
 #include <linux/platform_data/mtd-davinci-aemif.h>
@@ -577,7 +578,6 @@ static struct davinci_nand_pdata
 	return pdev->dev.platform_data;
 }
 #else
-#define davinci_nand_of_match NULL
 static struct davinci_nand_pdata
 	*nand_davinci_get_pdata(struct platform_device *pdev)
 {
@@ -878,22 +878,12 @@ static struct platform_driver nand_davinci_driver = {
 	.driver		= {
 		.name	= "davinci_nand",
 		.owner	= THIS_MODULE,
-		.of_match_table = davinci_nand_of_match,
+		.of_match_table = of_match_ptr(davinci_nand_of_match),
 	},
 };
 MODULE_ALIAS("platform:davinci_nand");
 
-static int __init nand_davinci_init(void)
-{
-	return platform_driver_probe(&nand_davinci_driver, nand_davinci_probe);
-}
-module_init(nand_davinci_init);
-
-static void __exit nand_davinci_exit(void)
-{
-	platform_driver_unregister(&nand_davinci_driver);
-}
-module_exit(nand_davinci_exit);
+module_platform_driver_probe(nand_davinci_driver, nand_davinci_probe);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Texas Instruments");
diff --git a/drivers/mtd/nand/denali_dt.c b/drivers/mtd/nand/denali_dt.c
index 546f8cb5688d..92530244e2cb 100644
--- a/drivers/mtd/nand/denali_dt.c
+++ b/drivers/mtd/nand/denali_dt.c
@@ -42,7 +42,7 @@ static void __iomem *request_and_map(struct device *dev,
 	}
 
 	ptr = devm_ioremap_nocache(dev, res->start, resource_size(res));
-	if (!res)
+	if (!ptr)
 		dev_err(dev, "ioremap_nocache of %s failed!", res->name);
 
 	return ptr;
@@ -90,7 +90,7 @@ static int denali_dt_probe(struct platform_device *ofdev)
 	denali->irq = platform_get_irq(ofdev, 0);
 	if (denali->irq < 0) {
 		dev_err(&ofdev->dev, "no irq defined\n");
-		return -ENXIO;
+		return denali->irq;
 	}
 
 	denali->flash_reg = request_and_map(&ofdev->dev, denali_reg);
@@ -146,21 +146,11 @@ static struct platform_driver denali_dt_driver = {
 	.driver		= {
 		.name	= "denali-nand-dt",
 		.owner	= THIS_MODULE,
-		.of_match_table	= of_match_ptr(denali_nand_dt_ids),
+		.of_match_table	= denali_nand_dt_ids,
 	},
 };
 
-static int __init denali_init_dt(void)
-{
-	return platform_driver_register(&denali_dt_driver);
-}
-module_init(denali_init_dt);
-
-static void __exit denali_exit_dt(void)
-{
-	platform_driver_unregister(&denali_dt_driver);
-}
-module_exit(denali_exit_dt);
+module_platform_driver(denali_dt_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Jamie Iles");
diff --git a/drivers/mtd/nand/docg4.c b/drivers/mtd/nand/docg4.c
index 18fa4489e52e..fa25e7a08134 100644
--- a/drivers/mtd/nand/docg4.c
+++ b/drivers/mtd/nand/docg4.c
@@ -1397,18 +1397,7 @@ static struct platform_driver docg4_driver = {
 	.remove		= __exit_p(cleanup_docg4),
 };
 
-static int __init docg4_init(void)
-{
-	return platform_driver_probe(&docg4_driver, probe_docg4);
-}
-
-static void __exit docg4_exit(void)
-{
-	platform_driver_unregister(&docg4_driver);
-}
-
-module_init(docg4_init);
-module_exit(docg4_exit);
+module_platform_driver_probe(docg4_driver, probe_docg4);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Mike Dunn");
diff --git a/drivers/mtd/nand/fsmc_nand.c b/drivers/mtd/nand/fsmc_nand.c
index 05ba3f0c2d19..911e2433fe30 100644
--- a/drivers/mtd/nand/fsmc_nand.c
+++ b/drivers/mtd/nand/fsmc_nand.c
@@ -1235,18 +1235,7 @@ static struct platform_driver fsmc_nand_driver = {
 	},
 };
 
-static int __init fsmc_nand_init(void)
-{
-	return platform_driver_probe(&fsmc_nand_driver,
-				     fsmc_nand_probe);
-}
-module_init(fsmc_nand_init);
-
-static void __exit fsmc_nand_exit(void)
-{
-	platform_driver_unregister(&fsmc_nand_driver);
-}
-module_exit(fsmc_nand_exit);
+module_platform_driver_probe(fsmc_nand_driver, fsmc_nand_probe);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Vipin Kumar <vipin.kumar@st.com>, Ashish Priyadarshi");
diff --git a/drivers/mtd/nand/gpio.c b/drivers/mtd/nand/gpio.c
index e789e3f51710..89065dd83d64 100644
--- a/drivers/mtd/nand/gpio.c
+++ b/drivers/mtd/nand/gpio.c
@@ -190,7 +190,6 @@ static struct resource *gpio_nand_get_io_sync_of(struct platform_device *pdev)
 	return r;
 }
 #else /* CONFIG_OF */
-#define gpio_nand_id_table NULL
 static inline int gpio_nand_get_config_of(const struct device *dev,
 					  struct gpio_nand_platdata *plat)
 {
@@ -259,8 +258,6 @@ static int gpio_nand_remove(struct platform_device *dev)
 	if (gpio_is_valid(gpiomtd->plat.gpio_rdy))
 		gpio_free(gpiomtd->plat.gpio_rdy);
 
-	kfree(gpiomtd);
-
 	return 0;
 }
 
@@ -297,7 +294,7 @@ static int gpio_nand_probe(struct platform_device *dev)
 	if (!res0)
 		return -EINVAL;
 
-	gpiomtd = kzalloc(sizeof(*gpiomtd), GFP_KERNEL);
+	gpiomtd = devm_kzalloc(&dev->dev, sizeof(*gpiomtd), GFP_KERNEL);
 	if (gpiomtd == NULL) {
 		dev_err(&dev->dev, "failed to create NAND MTD\n");
 		return -ENOMEM;
@@ -412,7 +409,6 @@ err_sync:
 	iounmap(gpiomtd->nand_chip.IO_ADDR_R);
 	release_mem_region(res0->start, resource_size(res0));
 err_map:
-	kfree(gpiomtd);
 	return ret;
 }
 
@@ -421,7 +417,7 @@ static struct platform_driver gpio_nand_driver = {
 	.remove		= gpio_nand_remove,
 	.driver		= {
 		.name	= "gpio-nand",
-		.of_match_table = gpio_nand_id_table,
+		.of_match_table = of_match_ptr(gpio_nand_id_table),
 	},
 };
 
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
index 717881a3d1b8..25ecfa1822a8 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
@@ -36,7 +36,6 @@
 #define GPMI_NAND_GPMI_REGS_ADDR_RES_NAME  "gpmi-nand"
 #define GPMI_NAND_BCH_REGS_ADDR_RES_NAME   "bch"
 #define GPMI_NAND_BCH_INTERRUPT_RES_NAME   "bch"
-#define GPMI_NAND_DMA_INTERRUPT_RES_NAME   "gpmi-dma"
 
 /* add our owner bbt descriptor */
 static uint8_t scan_ff_pattern[] = { 0xff };
@@ -420,28 +419,6 @@ static void release_bch_irq(struct gpmi_nand_data *this)
 		free_irq(i, this);
 }
 
-static bool gpmi_dma_filter(struct dma_chan *chan, void *param)
-{
-	struct gpmi_nand_data *this = param;
-	int dma_channel = (int)this->private;
-
-	if (!mxs_dma_is_apbh(chan))
-		return false;
-	/*
-	 * only catch the GPMI dma channels :
-	 *	for mx23 :	MX23_DMA_GPMI0 ~ MX23_DMA_GPMI3
-	 *		(These four channels share the same IRQ!)
-	 *
-	 *	for mx28 :	MX28_DMA_GPMI0 ~ MX28_DMA_GPMI7
-	 *		(These eight channels share the same IRQ!)
-	 */
-	if (dma_channel == chan->chan_id) {
-		chan->private = &this->dma_data;
-		return true;
-	}
-	return false;
-}
-
 static void release_dma_channels(struct gpmi_nand_data *this)
 {
 	unsigned int i;
@@ -455,36 +432,10 @@ static void release_dma_channels(struct gpmi_nand_data *this)
 static int acquire_dma_channels(struct gpmi_nand_data *this)
 {
 	struct platform_device *pdev = this->pdev;
-	struct resource *r_dma;
-	struct device_node *dn;
-	u32 dma_channel;
-	int ret;
 	struct dma_chan *dma_chan;
-	dma_cap_mask_t mask;
-
-	/* dma channel, we only use the first one. */
-	dn = pdev->dev.of_node;
-	ret = of_property_read_u32(dn, "fsl,gpmi-dma-channel", &dma_channel);
-	if (ret) {
-		pr_err("unable to get DMA channel from dt.\n");
-		goto acquire_err;
-	}
-	this->private = (void *)dma_channel;
-
-	/* gpmi dma interrupt */
-	r_dma = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
-					GPMI_NAND_DMA_INTERRUPT_RES_NAME);
-	if (!r_dma) {
-		pr_err("Can't get resource for DMA\n");
-		goto acquire_err;
-	}
-	this->dma_data.chan_irq = r_dma->start;
 
 	/* request dma channel */
-	dma_cap_zero(mask);
-	dma_cap_set(DMA_SLAVE, mask);
-
-	dma_chan = dma_request_channel(mask, gpmi_dma_filter, this);
+	dma_chan = dma_request_slave_channel(&pdev->dev, "rx-tx");
 	if (!dma_chan) {
 		pr_err("Failed to request DMA channel.\n");
 		goto acquire_err;
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
index 072947731277..a7685e3a8748 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
@@ -20,7 +20,7 @@
 #include <linux/mtd/nand.h>
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
-#include <linux/fsl/mxs-dma.h>
+#include <linux/dmaengine.h>
 
 #define GPMI_CLK_MAX 5 /* MX6Q needs five clocks */
 struct resources {
@@ -180,7 +180,6 @@ struct gpmi_nand_data {
 	/* DMA channels */
 #define DMA_CHANS		8
 	struct dma_chan		*dma_chans[DMA_CHANS];
-	struct mxs_dma_data	dma_data;
 	enum dma_ops_type	last_dma_type;
 	enum dma_ops_type	dma_type;
 	struct completion	dma_done;
diff --git a/drivers/mtd/nand/h1910.c b/drivers/mtd/nand/h1910.c
deleted file mode 100644
index 50166e93ba96..000000000000
--- a/drivers/mtd/nand/h1910.c
+++ /dev/null
@@ -1,167 +0,0 @@
-/*
- *  drivers/mtd/nand/h1910.c
- *
- *  Copyright (C) 2003 Joshua Wise (joshua@joshuawise.com)
- *
- *  Derived from drivers/mtd/nand/edb7312.c
- *       Copyright (C) 2002 Marius Gröger (mag@sysgo.de)
- *       Copyright (c) 2001 Thomas Gleixner (gleixner@autronix.de)
- *
- * 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.
- *
- *  Overview:
- *   This is a device driver for the NAND flash device found on the
- *   iPAQ h1910 board which utilizes the Samsung K9F2808 part. This is
- *   a 128Mibit (16MiB x 8 bits) NAND flash device.
- */
-
-#include <linux/slab.h>
-#include <linux/init.h>
-#include <linux/module.h>
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/nand.h>
-#include <linux/mtd/partitions.h>
-#include <asm/io.h>
-#include <mach/hardware.h>
-#include <asm/sizes.h>
-#include <mach/h1900-gpio.h>
-#include <mach/ipaq.h>
-
-/*
- * MTD structure for EDB7312 board
- */
-static struct mtd_info *h1910_nand_mtd = NULL;
-
-/*
- * Module stuff
- */
-
-/*
- * Define static partitions for flash device
- */
-static struct mtd_partition partition_info[] = {
-      {name:"h1910 NAND Flash",
-	      offset:0,
-      size:16 * 1024 * 1024}
-};
-
-#define NUM_PARTITIONS 1
-
-/*
- *	hardware specific access to control-lines
- *
- *	NAND_NCE: bit 0 - don't care
- *	NAND_CLE: bit 1 - address bit 2
- *	NAND_ALE: bit 2 - address bit 3
- */
-static void h1910_hwcontrol(struct mtd_info *mtd, int cmd,
-			    unsigned int ctrl)
-{
-	struct nand_chip *chip = mtd->priv;
-
-	if (cmd != NAND_CMD_NONE)
-		writeb(cmd, chip->IO_ADDR_W | ((ctrl & 0x6) << 1));
-}
-
-/*
- *	read device ready pin
- */
-#if 0
-static int h1910_device_ready(struct mtd_info *mtd)
-{
-	return (GPLR(55) & GPIO_bit(55));
-}
-#endif
-
-/*
- * Main initialization routine
- */
-static int __init h1910_init(void)
-{
-	struct nand_chip *this;
-	void __iomem *nandaddr;
-
-	if (!machine_is_h1900())
-		return -ENODEV;
-
-	nandaddr = ioremap(0x08000000, 0x1000);
-	if (!nandaddr) {
-		printk("Failed to ioremap nand flash.\n");
-		return -ENOMEM;
-	}
-
-	/* Allocate memory for MTD device structure and private data */
-	h1910_nand_mtd = kmalloc(sizeof(struct mtd_info) + sizeof(struct nand_chip), GFP_KERNEL);
-	if (!h1910_nand_mtd) {
-		printk("Unable to allocate h1910 NAND MTD device structure.\n");
-		iounmap((void *)nandaddr);
-		return -ENOMEM;
-	}
-
-	/* Get pointer to private data */
-	this = (struct nand_chip *)(&h1910_nand_mtd[1]);
-
-	/* Initialize structures */
-	memset(h1910_nand_mtd, 0, sizeof(struct mtd_info));
-	memset(this, 0, sizeof(struct nand_chip));
-
-	/* Link the private data with the MTD structure */
-	h1910_nand_mtd->priv = this;
-	h1910_nand_mtd->owner = THIS_MODULE;
-
-	/*
-	 * Enable VPEN
-	 */
-	GPSR(37) = GPIO_bit(37);
-
-	/* insert callbacks */
-	this->IO_ADDR_R = nandaddr;
-	this->IO_ADDR_W = nandaddr;
-	this->cmd_ctrl = h1910_hwcontrol;
-	this->dev_ready = NULL;	/* unknown whether that was correct or not so we will just do it like this */
-	/* 15 us command delay time */
-	this->chip_delay = 50;
-	this->ecc.mode = NAND_ECC_SOFT;
-
-	/* Scan to find existence of the device */
-	if (nand_scan(h1910_nand_mtd, 1)) {
-		printk(KERN_NOTICE "No NAND device - returning -ENXIO\n");
-		kfree(h1910_nand_mtd);
-		iounmap((void *)nandaddr);
-		return -ENXIO;
-	}
-
-	/* Register the partitions */
-	mtd_device_parse_register(h1910_nand_mtd, NULL, NULL, partition_info,
-				  NUM_PARTITIONS);
-
-	/* Return happy */
-	return 0;
-}
-
-module_init(h1910_init);
-
-/*
- * Clean up routine
- */
-static void __exit h1910_cleanup(void)
-{
-	struct nand_chip *this = (struct nand_chip *)&h1910_nand_mtd[1];
-
-	/* Release resources, unregister device */
-	nand_release(h1910_nand_mtd);
-
-	/* Release io resource */
-	iounmap((void *)this->IO_ADDR_W);
-
-	/* Free the MTD device structure */
-	kfree(h1910_nand_mtd);
-}
-
-module_exit(h1910_cleanup);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Joshua Wise <joshua at joshuawise dot com>");
-MODULE_DESCRIPTION("NAND flash driver for iPAQ h1910");
diff --git a/drivers/mtd/nand/lpc32xx_mlc.c b/drivers/mtd/nand/lpc32xx_mlc.c
index 0ca22ae9135c..a94facb46e5c 100644
--- a/drivers/mtd/nand/lpc32xx_mlc.c
+++ b/drivers/mtd/nand/lpc32xx_mlc.c
@@ -540,8 +540,8 @@ static int lpc32xx_write_page_lowlevel(struct mtd_info *mtd,
 }
 
 static int lpc32xx_write_page(struct mtd_info *mtd, struct nand_chip *chip,
-			      const uint8_t *buf, int oob_required, int page,
-			      int cached, int raw)
+			uint32_t offset, int data_len, const uint8_t *buf,
+			int oob_required, int page, int cached, int raw)
 {
 	int res;
 
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index 42c63927609d..dfcd0a565c5b 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -4,7 +4,6 @@
  *  Overview:
  *   This is the generic MTD driver for NAND flash devices. It should be
  *   capable of working with almost all NAND chips currently available.
- *   Basic support for AG-AND chips is provided.
  *
  *	Additional technical information is available on
  *	http://www.linux-mtd.infradead.org/doc/nand.html
@@ -22,8 +21,6 @@
  *	Enable cached programming for 2k page size chips
  *	Check, if mtd->ecctype should be set to MTD_ECC_HW
  *	if we have HW ECC support.
- *	The AG-AND chips have nice features for speed improvement,
- *	which are not supported yet. Read / program 4 pages in one go.
  *	BBT table is not serialized, has to be fixed
  *
  * This program is free software; you can redistribute it and/or modify
@@ -515,7 +512,7 @@ EXPORT_SYMBOL_GPL(nand_wait_ready);
  * @page_addr: the page address for this command, -1 if none
  *
  * Send command to NAND device. This function is used for small page devices
- * (256/512 Bytes per page).
+ * (512 Bytes per page).
  */
 static void nand_command(struct mtd_info *mtd, unsigned int command,
 			 int column, int page_addr)
@@ -631,8 +628,7 @@ static void nand_command_lp(struct mtd_info *mtd, unsigned int command,
 	}
 
 	/* Command latch cycle */
-	chip->cmd_ctrl(mtd, command & 0xff,
-		       NAND_NCE | NAND_CLE | NAND_CTRL_CHANGE);
+	chip->cmd_ctrl(mtd, command, NAND_NCE | NAND_CLE | NAND_CTRL_CHANGE);
 
 	if (column != -1 || page_addr != -1) {
 		int ctrl = NAND_CTRL_CHANGE | NAND_NCE | NAND_ALE;
@@ -671,16 +667,6 @@ static void nand_command_lp(struct mtd_info *mtd, unsigned int command,
 	case NAND_CMD_SEQIN:
 	case NAND_CMD_RNDIN:
 	case NAND_CMD_STATUS:
-	case NAND_CMD_DEPLETE1:
-		return;
-
-	case NAND_CMD_STATUS_ERROR:
-	case NAND_CMD_STATUS_ERROR0:
-	case NAND_CMD_STATUS_ERROR1:
-	case NAND_CMD_STATUS_ERROR2:
-	case NAND_CMD_STATUS_ERROR3:
-		/* Read error status commands require only a short delay */
-		udelay(chip->chip_delay);
 		return;
 
 	case NAND_CMD_RESET:
@@ -836,10 +822,7 @@ static int nand_wait(struct mtd_info *mtd, struct nand_chip *chip)
 	 */
 	ndelay(100);
 
-	if ((state == FL_ERASING) && (chip->options & NAND_IS_AND))
-		chip->cmdfunc(mtd, NAND_CMD_STATUS_MULTI, -1, -1);
-	else
-		chip->cmdfunc(mtd, NAND_CMD_STATUS, -1, -1);
+	chip->cmdfunc(mtd, NAND_CMD_STATUS, -1, -1);
 
 	if (in_interrupt() || oops_in_progress)
 		panic_nand_wait(mtd, chip, timeo);
@@ -1127,7 +1110,7 @@ static int nand_read_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
 }
 
 /**
- * nand_read_subpage - [REPLACEABLE] software ECC based sub-page read function
+ * nand_read_subpage - [REPLACEABLE] ECC based sub-page read function
  * @mtd: mtd info structure
  * @chip: nand chip info structure
  * @data_offs: offset of requested data within the page
@@ -1995,6 +1978,67 @@ static int nand_write_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
 	return 0;
 }
 
+
+/**
+ * nand_write_subpage_hwecc - [REPLACABLE] hardware ECC based subpage write
+ * @mtd:	mtd info structure
+ * @chip:	nand chip info structure
+ * @column:	column address of subpage within the page
+ * @data_len:	data length
+ * @oob_required: must write chip->oob_poi to OOB
+ */
+static int nand_write_subpage_hwecc(struct mtd_info *mtd,
+				struct nand_chip *chip, uint32_t offset,
+				uint32_t data_len, const uint8_t *data_buf,
+				int oob_required)
+{
+	uint8_t *oob_buf  = chip->oob_poi;
+	uint8_t *ecc_calc = chip->buffers->ecccalc;
+	int ecc_size      = chip->ecc.size;
+	int ecc_bytes     = chip->ecc.bytes;
+	int ecc_steps     = chip->ecc.steps;
+	uint32_t *eccpos  = chip->ecc.layout->eccpos;
+	uint32_t start_step = offset / ecc_size;
+	uint32_t end_step   = (offset + data_len - 1) / ecc_size;
+	int oob_bytes       = mtd->oobsize / ecc_steps;
+	int step, i;
+
+	for (step = 0; step < ecc_steps; step++) {
+		/* configure controller for WRITE access */
+		chip->ecc.hwctl(mtd, NAND_ECC_WRITE);
+
+		/* write data (untouched subpages already masked by 0xFF) */
+		chip->write_buf(mtd, data_buf, ecc_size);
+
+		/* mask ECC of un-touched subpages by padding 0xFF */
+		if ((step < start_step) || (step > end_step))
+			memset(ecc_calc, 0xff, ecc_bytes);
+		else
+			chip->ecc.calculate(mtd, data_buf, ecc_calc);
+
+		/* mask OOB of un-touched subpages by padding 0xFF */
+		/* if oob_required, preserve OOB metadata of written subpage */
+		if (!oob_required || (step < start_step) || (step > end_step))
+			memset(oob_buf, 0xff, oob_bytes);
+
+		data_buf += ecc_size;
+		ecc_calc += ecc_bytes;
+		oob_buf  += oob_bytes;
+	}
+
+	/* copy calculated ECC for whole page to chip->buffer->oob */
+	/* this include masked-value(0xFF) for unwritten subpages */
+	ecc_calc = chip->buffers->ecccalc;
+	for (i = 0; i < chip->ecc.total; i++)
+		chip->oob_poi[eccpos[i]] = ecc_calc[i];
+
+	/* write OOB buffer to NAND device */
+	chip->write_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	return 0;
+}
+
+
 /**
  * nand_write_page_syndrome - [REPLACEABLE] hardware ECC syndrome based page write
  * @mtd: mtd info structure
@@ -2047,6 +2091,8 @@ static int nand_write_page_syndrome(struct mtd_info *mtd,
  * nand_write_page - [REPLACEABLE] write one page
  * @mtd: MTD device structure
  * @chip: NAND chip descriptor
+ * @offset: address offset within the page
+ * @data_len: length of actual data to be written
  * @buf: the data to write
  * @oob_required: must write chip->oob_poi to OOB
  * @page: page number to write
@@ -2054,15 +2100,25 @@ static int nand_write_page_syndrome(struct mtd_info *mtd,
  * @raw: use _raw version of write_page
  */
 static int nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
-			   const uint8_t *buf, int oob_required, int page,
-			   int cached, int raw)
+		uint32_t offset, int data_len, const uint8_t *buf,
+		int oob_required, int page, int cached, int raw)
 {
-	int status;
+	int status, subpage;
+
+	if (!(chip->options & NAND_NO_SUBPAGE_WRITE) &&
+		chip->ecc.write_subpage)
+		subpage = offset || (data_len < mtd->writesize);
+	else
+		subpage = 0;
 
 	chip->cmdfunc(mtd, NAND_CMD_SEQIN, 0x00, page);
 
 	if (unlikely(raw))
-		status = chip->ecc.write_page_raw(mtd, chip, buf, oob_required);
+		status = chip->ecc.write_page_raw(mtd, chip, buf,
+							oob_required);
+	else if (subpage)
+		status = chip->ecc.write_subpage(mtd, chip, offset, data_len,
+							 buf, oob_required);
 	else
 		status = chip->ecc.write_page(mtd, chip, buf, oob_required);
 
@@ -2075,7 +2131,7 @@ static int nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 	 */
 	cached = 0;
 
-	if (!cached || !(chip->options & NAND_CACHEPRG)) {
+	if (!cached || !NAND_HAS_CACHEPROG(chip)) {
 
 		chip->cmdfunc(mtd, NAND_CMD_PAGEPROG, -1, -1);
 		status = chip->waitfunc(mtd, chip);
@@ -2176,7 +2232,7 @@ static int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 
 	uint8_t *oob = ops->oobbuf;
 	uint8_t *buf = ops->datbuf;
-	int ret, subpage;
+	int ret;
 	int oob_required = oob ? 1 : 0;
 
 	ops->retlen = 0;
@@ -2191,10 +2247,6 @@ static int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 	}
 
 	column = to & (mtd->writesize - 1);
-	subpage = column || (writelen & (mtd->writesize - 1));
-
-	if (subpage && oob)
-		return -EINVAL;
 
 	chipnr = (int)(to >> chip->chip_shift);
 	chip->select_chip(mtd, chipnr);
@@ -2243,9 +2295,9 @@ static int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 			/* We still need to erase leftover OOB data */
 			memset(chip->oob_poi, 0xff, mtd->oobsize);
 		}
-
-		ret = chip->write_page(mtd, chip, wbuf, oob_required, page,
-				       cached, (ops->mode == MTD_OPS_RAW));
+		ret = chip->write_page(mtd, chip, column, bytes, wbuf,
+					oob_required, page, cached,
+					(ops->mode == MTD_OPS_RAW));
 		if (ret)
 			break;
 
@@ -2481,24 +2533,6 @@ static void single_erase_cmd(struct mtd_info *mtd, int page)
 }
 
 /**
- * multi_erase_cmd - [GENERIC] AND specific block erase command function
- * @mtd: MTD device structure
- * @page: the page address of the block which will be erased
- *
- * AND multi block erase command function. Erase 4 consecutive blocks.
- */
-static void multi_erase_cmd(struct mtd_info *mtd, int page)
-{
-	struct nand_chip *chip = mtd->priv;
-	/* Send commands to erase a block */
-	chip->cmdfunc(mtd, NAND_CMD_ERASE1, -1, page++);
-	chip->cmdfunc(mtd, NAND_CMD_ERASE1, -1, page++);
-	chip->cmdfunc(mtd, NAND_CMD_ERASE1, -1, page++);
-	chip->cmdfunc(mtd, NAND_CMD_ERASE1, -1, page);
-	chip->cmdfunc(mtd, NAND_CMD_ERASE2, -1, -1);
-}
-
-/**
  * nand_erase - [MTD Interface] erase block(s)
  * @mtd: MTD device structure
  * @instr: erase instruction
@@ -2510,7 +2544,6 @@ static int nand_erase(struct mtd_info *mtd, struct erase_info *instr)
 	return nand_erase_nand(mtd, instr, 0);
 }
 
-#define BBT_PAGE_MASK	0xffffff3f
 /**
  * nand_erase_nand - [INTERN] erase block(s)
  * @mtd: MTD device structure
@@ -2524,8 +2557,6 @@ int nand_erase_nand(struct mtd_info *mtd, struct erase_info *instr,
 {
 	int page, status, pages_per_block, ret, chipnr;
 	struct nand_chip *chip = mtd->priv;
-	loff_t rewrite_bbt[NAND_MAX_CHIPS] = {0};
-	unsigned int bbt_masked_page = 0xffffffff;
 	loff_t len;
 
 	pr_debug("%s: start = 0x%012llx, len = %llu\n",
@@ -2556,15 +2587,6 @@ int nand_erase_nand(struct mtd_info *mtd, struct erase_info *instr,
 		goto erase_exit;
 	}
 
-	/*
-	 * If BBT requires refresh, set the BBT page mask to see if the BBT
-	 * should be rewritten. Otherwise the mask is set to 0xffffffff which
-	 * can not be matched. This is also done when the bbt is actually
-	 * erased to avoid recursive updates.
-	 */
-	if (chip->options & BBT_AUTO_REFRESH && !allowbbt)
-		bbt_masked_page = chip->bbt_td->pages[chipnr] & BBT_PAGE_MASK;
-
 	/* Loop through the pages */
 	len = instr->len;
 
@@ -2610,15 +2632,6 @@ int nand_erase_nand(struct mtd_info *mtd, struct erase_info *instr,
 			goto erase_exit;
 		}
 
-		/*
-		 * If BBT requires refresh, set the BBT rewrite flag to the
-		 * page being erased.
-		 */
-		if (bbt_masked_page != 0xffffffff &&
-		    (page & BBT_PAGE_MASK) == bbt_masked_page)
-			    rewrite_bbt[chipnr] =
-					((loff_t)page << chip->page_shift);
-
 		/* Increment page address and decrement length */
 		len -= (1 << chip->phys_erase_shift);
 		page += pages_per_block;
@@ -2628,15 +2641,6 @@ int nand_erase_nand(struct mtd_info *mtd, struct erase_info *instr,
 			chipnr++;
 			chip->select_chip(mtd, -1);
 			chip->select_chip(mtd, chipnr);
-
-			/*
-			 * If BBT requires refresh and BBT-PERCHIP, set the BBT
-			 * page mask to see if this BBT should be rewritten.
-			 */
-			if (bbt_masked_page != 0xffffffff &&
-			    (chip->bbt_td->options & NAND_BBT_PERCHIP))
-				bbt_masked_page = chip->bbt_td->pages[chipnr] &
-					BBT_PAGE_MASK;
 		}
 	}
 	instr->state = MTD_ERASE_DONE;
@@ -2653,23 +2657,6 @@ erase_exit:
 	if (!ret)
 		mtd_erase_callback(instr);
 
-	/*
-	 * If BBT requires refresh and erase was successful, rewrite any
-	 * selected bad block tables.
-	 */
-	if (bbt_masked_page == 0xffffffff || ret)
-		return ret;
-
-	for (chipnr = 0; chipnr < chip->numchips; chipnr++) {
-		if (!rewrite_bbt[chipnr])
-			continue;
-		/* Update the BBT for chip */
-		pr_debug("%s: nand_update_bbt (%d:0x%0llx 0x%0x)\n",
-				__func__, chipnr, rewrite_bbt[chipnr],
-				chip->bbt_td->pages[chipnr]);
-		nand_update_bbt(mtd, rewrite_bbt[chipnr]);
-	}
-
 	/* Return more or less happy */
 	return ret;
 }
@@ -2905,8 +2892,6 @@ static int nand_flash_detect_onfi(struct mtd_info *mtd, struct nand_chip *chip,
 		chip->onfi_version = 20;
 	else if (val & (1 << 1))
 		chip->onfi_version = 10;
-	else
-		chip->onfi_version = 0;
 
 	if (!chip->onfi_version) {
 		pr_info("%s: unsupported ONFI version: %d\n", __func__, val);
@@ -3171,6 +3156,30 @@ static void nand_decode_bbm_options(struct mtd_info *mtd,
 		chip->bbt_options |= NAND_BBT_SCAN2NDPAGE;
 }
 
+static inline bool is_full_id_nand(struct nand_flash_dev *type)
+{
+	return type->id_len;
+}
+
+static bool find_full_id_nand(struct mtd_info *mtd, struct nand_chip *chip,
+		   struct nand_flash_dev *type, u8 *id_data, int *busw)
+{
+	if (!strncmp(type->id, id_data, type->id_len)) {
+		mtd->writesize = type->pagesize;
+		mtd->erasesize = type->erasesize;
+		mtd->oobsize = type->oobsize;
+
+		chip->cellinfo = id_data[2];
+		chip->chipsize = (uint64_t)type->chipsize << 20;
+		chip->options |= type->options;
+
+		*busw = type->options & NAND_BUSWIDTH_16;
+
+		return true;
+	}
+	return false;
+}
+
 /*
  * Get the flash and manufacturer id and lookup if the type is supported.
  */
@@ -3222,9 +3231,14 @@ static struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
 	if (!type)
 		type = nand_flash_ids;
 
-	for (; type->name != NULL; type++)
-		if (*dev_id == type->id)
-			break;
+	for (; type->name != NULL; type++) {
+		if (is_full_id_nand(type)) {
+			if (find_full_id_nand(mtd, chip, type, id_data, &busw))
+				goto ident_done;
+		} else if (*dev_id == type->dev_id) {
+				break;
+		}
+	}
 
 	chip->onfi_version = 0;
 	if (!type->name || !type->pagesize) {
@@ -3302,12 +3316,7 @@ ident_done:
 	}
 
 	chip->badblockbits = 8;
-
-	/* Check for AND chips with 4 page planes */
-	if (chip->options & NAND_4PAGE_ARRAY)
-		chip->erase_cmd = multi_erase_cmd;
-	else
-		chip->erase_cmd = single_erase_cmd;
+	chip->erase_cmd = single_erase_cmd;
 
 	/* Do not replace user supplied command function! */
 	if (mtd->writesize > 512 && chip->cmdfunc == nand_command)
@@ -3474,6 +3483,10 @@ int nand_scan_tail(struct mtd_info *mtd)
 			chip->ecc.read_oob = nand_read_oob_std;
 		if (!chip->ecc.write_oob)
 			chip->ecc.write_oob = nand_write_oob_std;
+		if (!chip->ecc.read_subpage)
+			chip->ecc.read_subpage = nand_read_subpage;
+		if (!chip->ecc.write_subpage)
+			chip->ecc.write_subpage = nand_write_subpage_hwecc;
 
 	case NAND_ECC_HW_SYNDROME:
 		if ((!chip->ecc.calculate || !chip->ecc.correct ||
diff --git a/drivers/mtd/nand/nand_bbt.c b/drivers/mtd/nand/nand_bbt.c
index 916d6e9c0ab1..267264320e06 100644
--- a/drivers/mtd/nand/nand_bbt.c
+++ b/drivers/mtd/nand/nand_bbt.c
@@ -1240,15 +1240,6 @@ int nand_update_bbt(struct mtd_info *mtd, loff_t offs)
  */
 static uint8_t scan_ff_pattern[] = { 0xff, 0xff };
 
-static uint8_t scan_agand_pattern[] = { 0x1C, 0x71, 0xC7, 0x1C, 0x71, 0xC7 };
-
-static struct nand_bbt_descr agand_flashbased = {
-	.options = NAND_BBT_SCANEMPTY | NAND_BBT_SCANALLPAGES,
-	.offs = 0x20,
-	.len = 6,
-	.pattern = scan_agand_pattern
-};
-
 /* Generic flash bbt descriptors */
 static uint8_t bbt_pattern[] = {'B', 'b', 't', '0' };
 static uint8_t mirror_pattern[] = {'1', 't', 'b', 'B' };
@@ -1333,22 +1324,6 @@ int nand_default_bbt(struct mtd_info *mtd)
 {
 	struct nand_chip *this = mtd->priv;
 
-	/*
-	 * Default for AG-AND. We must use a flash based bad block table as the
-	 * devices have factory marked _good_ blocks. Erasing those blocks
-	 * leads to loss of the good / bad information, so we _must_ store this
-	 * information in a good / bad table during startup.
-	 */
-	if (this->options & NAND_IS_AND) {
-		/* Use the default pattern descriptors */
-		if (!this->bbt_td) {
-			this->bbt_td = &bbt_main_descr;
-			this->bbt_md = &bbt_mirror_descr;
-		}
-		this->bbt_options |= NAND_BBT_USE_FLASH;
-		return nand_scan_bbt(mtd, &agand_flashbased);
-	}
-
 	/* Is a flash based bad block table requested? */
 	if (this->bbt_options & NAND_BBT_USE_FLASH) {
 		/* Use the default pattern descriptors */
diff --git a/drivers/mtd/nand/nand_ids.c b/drivers/mtd/nand/nand_ids.c
index 9c612388e5de..683813a46a90 100644
--- a/drivers/mtd/nand/nand_ids.c
+++ b/drivers/mtd/nand/nand_ids.c
@@ -10,163 +10,153 @@
  */
 #include <linux/module.h>
 #include <linux/mtd/nand.h>
-/*
-*	Chip ID list
-*
-*	Name. ID code, pagesize, chipsize in MegaByte, eraseblock size,
-*	options
-*
-*	Pagesize; 0, 256, 512
-*	0	get this information from the extended chip ID
-+	256	256 Byte page size
-*	512	512 Byte page size
-*/
-struct nand_flash_dev nand_flash_ids[] = {
+#include <linux/sizes.h>
+
+#define LP_OPTIONS NAND_SAMSUNG_LP_OPTIONS
+#define LP_OPTIONS16 (LP_OPTIONS | NAND_BUSWIDTH_16)
+
 #define SP_OPTIONS NAND_NEED_READRDY
 #define SP_OPTIONS16 (SP_OPTIONS | NAND_BUSWIDTH_16)
 
-#ifdef CONFIG_MTD_NAND_MUSEUM_IDS
-	{"NAND 1MiB 5V 8-bit",		0x6e, 256, 1, 0x1000, SP_OPTIONS},
-	{"NAND 2MiB 5V 8-bit",		0x64, 256, 2, 0x1000, SP_OPTIONS},
-	{"NAND 4MiB 5V 8-bit",		0x6b, 512, 4, 0x2000, SP_OPTIONS},
-	{"NAND 1MiB 3,3V 8-bit",	0xe8, 256, 1, 0x1000, SP_OPTIONS},
-	{"NAND 1MiB 3,3V 8-bit",	0xec, 256, 1, 0x1000, SP_OPTIONS},
-	{"NAND 2MiB 3,3V 8-bit",	0xea, 256, 2, 0x1000, SP_OPTIONS},
-	{"NAND 4MiB 3,3V 8-bit",	0xd5, 512, 4, 0x2000, SP_OPTIONS},
-	{"NAND 4MiB 3,3V 8-bit",	0xe3, 512, 4, 0x2000, SP_OPTIONS},
-	{"NAND 4MiB 3,3V 8-bit",	0xe5, 512, 4, 0x2000, SP_OPTIONS},
-	{"NAND 8MiB 3,3V 8-bit",	0xd6, 512, 8, 0x2000, SP_OPTIONS},
-
-	{"NAND 8MiB 1,8V 8-bit",	0x39, 512, 8, 0x2000, SP_OPTIONS},
-	{"NAND 8MiB 3,3V 8-bit",	0xe6, 512, 8, 0x2000, SP_OPTIONS},
-	{"NAND 8MiB 1,8V 16-bit",	0x49, 512, 8, 0x2000, SP_OPTIONS16},
-	{"NAND 8MiB 3,3V 16-bit",	0x59, 512, 8, 0x2000, SP_OPTIONS16},
-#endif
-
-	{"NAND 16MiB 1,8V 8-bit",	0x33, 512, 16, 0x4000, SP_OPTIONS},
-	{"NAND 16MiB 3,3V 8-bit",	0x73, 512, 16, 0x4000, SP_OPTIONS},
-	{"NAND 16MiB 1,8V 16-bit",	0x43, 512, 16, 0x4000, SP_OPTIONS16},
-	{"NAND 16MiB 3,3V 16-bit",	0x53, 512, 16, 0x4000, SP_OPTIONS16},
-
-	{"NAND 32MiB 1,8V 8-bit",	0x35, 512, 32, 0x4000, SP_OPTIONS},
-	{"NAND 32MiB 3,3V 8-bit",	0x75, 512, 32, 0x4000, SP_OPTIONS},
-	{"NAND 32MiB 1,8V 16-bit",	0x45, 512, 32, 0x4000, SP_OPTIONS16},
-	{"NAND 32MiB 3,3V 16-bit",	0x55, 512, 32, 0x4000, SP_OPTIONS16},
-
-	{"NAND 64MiB 1,8V 8-bit",	0x36, 512, 64, 0x4000, SP_OPTIONS},
-	{"NAND 64MiB 3,3V 8-bit",	0x76, 512, 64, 0x4000, SP_OPTIONS},
-	{"NAND 64MiB 1,8V 16-bit",	0x46, 512, 64, 0x4000, SP_OPTIONS16},
-	{"NAND 64MiB 3,3V 16-bit",	0x56, 512, 64, 0x4000, SP_OPTIONS16},
-
-	{"NAND 128MiB 1,8V 8-bit",	0x78, 512, 128, 0x4000, SP_OPTIONS},
-	{"NAND 128MiB 1,8V 8-bit",	0x39, 512, 128, 0x4000, SP_OPTIONS},
-	{"NAND 128MiB 3,3V 8-bit",	0x79, 512, 128, 0x4000, SP_OPTIONS},
-	{"NAND 128MiB 1,8V 16-bit",	0x72, 512, 128, 0x4000, SP_OPTIONS16},
-	{"NAND 128MiB 1,8V 16-bit",	0x49, 512, 128, 0x4000, SP_OPTIONS16},
-	{"NAND 128MiB 3,3V 16-bit",	0x74, 512, 128, 0x4000, SP_OPTIONS16},
-	{"NAND 128MiB 3,3V 16-bit",	0x59, 512, 128, 0x4000, SP_OPTIONS16},
-
-	{"NAND 256MiB 3,3V 8-bit",	0x71, 512, 256, 0x4000, SP_OPTIONS},
+/*
+ * The chip ID list:
+ *    name, device ID, page size, chip size in MiB, eraseblock size, options
+ *
+ * If page size and eraseblock size are 0, the sizes are taken from the
+ * extended chip ID.
+ */
+struct nand_flash_dev nand_flash_ids[] = {
+	/*
+	 * Some incompatible NAND chips share device ID's and so must be
+	 * listed by full ID. We list them first so that we can easily identify
+	 * the most specific match.
+	 */
+	{"TC58NVG2S0F 4G 3.3V 8-bit",
+		{ .id = {0x98, 0xdc, 0x90, 0x26, 0x76, 0x15, 0x01, 0x08} },
+		  SZ_4K, SZ_512, SZ_256K, 0, 8, 224},
+	{"TC58NVG3S0F 8G 3.3V 8-bit",
+		{ .id = {0x98, 0xd3, 0x90, 0x26, 0x76, 0x15, 0x02, 0x08} },
+		  SZ_4K, SZ_1K, SZ_256K, 0, 8, 232},
+	{"TC58NVG5D2 32G 3.3V 8-bit",
+		{ .id = {0x98, 0xd7, 0x94, 0x32, 0x76, 0x56, 0x09, 0x00} },
+		  SZ_8K, SZ_4K, SZ_1M, 0, 8, 640},
+	{"TC58NVG6D2 64G 3.3V 8-bit",
+		{ .id = {0x98, 0xde, 0x94, 0x82, 0x76, 0x56, 0x04, 0x20} },
+		  SZ_8K, SZ_8K, SZ_2M, 0, 8, 640},
+
+	LEGACY_ID_NAND("NAND 4MiB 5V 8-bit",   0x6B, 4, SZ_8K, SP_OPTIONS),
+	LEGACY_ID_NAND("NAND 4MiB 3,3V 8-bit", 0xE3, 4, SZ_8K, SP_OPTIONS),
+	LEGACY_ID_NAND("NAND 4MiB 3,3V 8-bit", 0xE5, 4, SZ_8K, SP_OPTIONS),
+	LEGACY_ID_NAND("NAND 8MiB 3,3V 8-bit", 0xD6, 8, SZ_8K, SP_OPTIONS),
+	LEGACY_ID_NAND("NAND 8MiB 3,3V 8-bit", 0xE6, 8, SZ_8K, SP_OPTIONS),
+
+	LEGACY_ID_NAND("NAND 16MiB 1,8V 8-bit",  0x33, 16, SZ_16K, SP_OPTIONS),
+	LEGACY_ID_NAND("NAND 16MiB 3,3V 8-bit",  0x73, 16, SZ_16K, SP_OPTIONS),
+	LEGACY_ID_NAND("NAND 16MiB 1,8V 16-bit", 0x43, 16, SZ_16K, SP_OPTIONS16),
+	LEGACY_ID_NAND("NAND 16MiB 3,3V 16-bit", 0x53, 16, SZ_16K, SP_OPTIONS16),
+
+	LEGACY_ID_NAND("NAND 32MiB 1,8V 8-bit",  0x35, 32, SZ_16K, SP_OPTIONS),
+	LEGACY_ID_NAND("NAND 32MiB 3,3V 8-bit",  0x75, 32, SZ_16K, SP_OPTIONS),
+	LEGACY_ID_NAND("NAND 32MiB 1,8V 16-bit", 0x45, 32, SZ_16K, SP_OPTIONS16),
+	LEGACY_ID_NAND("NAND 32MiB 3,3V 16-bit", 0x55, 32, SZ_16K, SP_OPTIONS16),
+
+	LEGACY_ID_NAND("NAND 64MiB 1,8V 8-bit",  0x36, 64, SZ_16K, SP_OPTIONS),
+	LEGACY_ID_NAND("NAND 64MiB 3,3V 8-bit",  0x76, 64, SZ_16K, SP_OPTIONS),
+	LEGACY_ID_NAND("NAND 64MiB 1,8V 16-bit", 0x46, 64, SZ_16K, SP_OPTIONS16),
+	LEGACY_ID_NAND("NAND 64MiB 3,3V 16-bit", 0x56, 64, SZ_16K, SP_OPTIONS16),
+
+	LEGACY_ID_NAND("NAND 128MiB 1,8V 8-bit",  0x78, 128, SZ_16K, SP_OPTIONS),
+	LEGACY_ID_NAND("NAND 128MiB 1,8V 8-bit",  0x39, 128, SZ_16K, SP_OPTIONS),
+	LEGACY_ID_NAND("NAND 128MiB 3,3V 8-bit",  0x79, 128, SZ_16K, SP_OPTIONS),
+	LEGACY_ID_NAND("NAND 128MiB 1,8V 16-bit", 0x72, 128, SZ_16K, SP_OPTIONS16),
+	LEGACY_ID_NAND("NAND 128MiB 1,8V 16-bit", 0x49, 128, SZ_16K, SP_OPTIONS16),
+	LEGACY_ID_NAND("NAND 128MiB 3,3V 16-bit", 0x74, 128, SZ_16K, SP_OPTIONS16),
+	LEGACY_ID_NAND("NAND 128MiB 3,3V 16-bit", 0x59, 128, SZ_16K, SP_OPTIONS16),
+
+	LEGACY_ID_NAND("NAND 256MiB 3,3V 8-bit", 0x71, 256, SZ_16K, SP_OPTIONS),
 
 	/*
-	 * These are the new chips with large page size. The pagesize and the
-	 * erasesize is determined from the extended id bytes
+	 * These are the new chips with large page size. Their page size and
+	 * eraseblock size are determined from the extended ID bytes.
 	 */
-#define LP_OPTIONS NAND_SAMSUNG_LP_OPTIONS
-#define LP_OPTIONS16 (LP_OPTIONS | NAND_BUSWIDTH_16)
 
 	/* 512 Megabit */
-	{"NAND 64MiB 1,8V 8-bit",	0xA2, 0,  64, 0, LP_OPTIONS},
-	{"NAND 64MiB 1,8V 8-bit",	0xA0, 0,  64, 0, LP_OPTIONS},
-	{"NAND 64MiB 3,3V 8-bit",	0xF2, 0,  64, 0, LP_OPTIONS},
-	{"NAND 64MiB 3,3V 8-bit",	0xD0, 0,  64, 0, LP_OPTIONS},
-	{"NAND 64MiB 3,3V 8-bit",	0xF0, 0,  64, 0, LP_OPTIONS},
-	{"NAND 64MiB 1,8V 16-bit",	0xB2, 0,  64, 0, LP_OPTIONS16},
-	{"NAND 64MiB 1,8V 16-bit",	0xB0, 0,  64, 0, LP_OPTIONS16},
-	{"NAND 64MiB 3,3V 16-bit",	0xC2, 0,  64, 0, LP_OPTIONS16},
-	{"NAND 64MiB 3,3V 16-bit",	0xC0, 0,  64, 0, LP_OPTIONS16},
+	EXTENDED_ID_NAND("NAND 64MiB 1,8V 8-bit",  0xA2,  64, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 64MiB 1,8V 8-bit",  0xA0,  64, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 64MiB 3,3V 8-bit",  0xF2,  64, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 64MiB 3,3V 8-bit",  0xD0,  64, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 64MiB 3,3V 8-bit",  0xF0,  64, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 64MiB 1,8V 16-bit", 0xB2,  64, LP_OPTIONS16),
+	EXTENDED_ID_NAND("NAND 64MiB 1,8V 16-bit", 0xB0,  64, LP_OPTIONS16),
+	EXTENDED_ID_NAND("NAND 64MiB 3,3V 16-bit", 0xC2,  64, LP_OPTIONS16),
+	EXTENDED_ID_NAND("NAND 64MiB 3,3V 16-bit", 0xC0,  64, LP_OPTIONS16),
 
 	/* 1 Gigabit */
-	{"NAND 128MiB 1,8V 8-bit",	0xA1, 0, 128, 0, LP_OPTIONS},
-	{"NAND 128MiB 3,3V 8-bit",	0xF1, 0, 128, 0, LP_OPTIONS},
-	{"NAND 128MiB 3,3V 8-bit",	0xD1, 0, 128, 0, LP_OPTIONS},
-	{"NAND 128MiB 1,8V 16-bit",	0xB1, 0, 128, 0, LP_OPTIONS16},
-	{"NAND 128MiB 3,3V 16-bit",	0xC1, 0, 128, 0, LP_OPTIONS16},
-	{"NAND 128MiB 1,8V 16-bit",     0xAD, 0, 128, 0, LP_OPTIONS16},
+	EXTENDED_ID_NAND("NAND 128MiB 1,8V 8-bit",  0xA1, 128, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 128MiB 3,3V 8-bit",  0xF1, 128, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 128MiB 3,3V 8-bit",  0xD1, 128, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 128MiB 1,8V 16-bit", 0xB1, 128, LP_OPTIONS16),
+	EXTENDED_ID_NAND("NAND 128MiB 3,3V 16-bit", 0xC1, 128, LP_OPTIONS16),
+	EXTENDED_ID_NAND("NAND 128MiB 1,8V 16-bit", 0xAD, 128, LP_OPTIONS16),
 
 	/* 2 Gigabit */
-	{"NAND 256MiB 1,8V 8-bit",	0xAA, 0, 256, 0, LP_OPTIONS},
-	{"NAND 256MiB 3,3V 8-bit",	0xDA, 0, 256, 0, LP_OPTIONS},
-	{"NAND 256MiB 1,8V 16-bit",	0xBA, 0, 256, 0, LP_OPTIONS16},
-	{"NAND 256MiB 3,3V 16-bit",	0xCA, 0, 256, 0, LP_OPTIONS16},
+	EXTENDED_ID_NAND("NAND 256MiB 1,8V 8-bit",  0xAA, 256, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 256MiB 3,3V 8-bit",  0xDA, 256, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 256MiB 1,8V 16-bit", 0xBA, 256, LP_OPTIONS16),
+	EXTENDED_ID_NAND("NAND 256MiB 3,3V 16-bit", 0xCA, 256, LP_OPTIONS16),
 
 	/* 4 Gigabit */
-	{"NAND 512MiB 1,8V 8-bit",	0xAC, 0, 512, 0, LP_OPTIONS},
-	{"NAND 512MiB 3,3V 8-bit",	0xDC, 0, 512, 0, LP_OPTIONS},
-	{"NAND 512MiB 1,8V 16-bit",	0xBC, 0, 512, 0, LP_OPTIONS16},
-	{"NAND 512MiB 3,3V 16-bit",	0xCC, 0, 512, 0, LP_OPTIONS16},
+	EXTENDED_ID_NAND("NAND 512MiB 1,8V 8-bit",  0xAC, 512, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 512MiB 3,3V 8-bit",  0xDC, 512, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 512MiB 1,8V 16-bit", 0xBC, 512, LP_OPTIONS16),
+	EXTENDED_ID_NAND("NAND 512MiB 3,3V 16-bit", 0xCC, 512, LP_OPTIONS16),
 
 	/* 8 Gigabit */
-	{"NAND 1GiB 1,8V 8-bit",	0xA3, 0, 1024, 0, LP_OPTIONS},
-	{"NAND 1GiB 3,3V 8-bit",	0xD3, 0, 1024, 0, LP_OPTIONS},
-	{"NAND 1GiB 1,8V 16-bit",	0xB3, 0, 1024, 0, LP_OPTIONS16},
-	{"NAND 1GiB 3,3V 16-bit",	0xC3, 0, 1024, 0, LP_OPTIONS16},
+	EXTENDED_ID_NAND("NAND 1GiB 1,8V 8-bit",  0xA3, 1024, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 1GiB 3,3V 8-bit",  0xD3, 1024, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 1GiB 1,8V 16-bit", 0xB3, 1024, LP_OPTIONS16),
+	EXTENDED_ID_NAND("NAND 1GiB 3,3V 16-bit", 0xC3, 1024, LP_OPTIONS16),
 
 	/* 16 Gigabit */
-	{"NAND 2GiB 1,8V 8-bit",	0xA5, 0, 2048, 0, LP_OPTIONS},
-	{"NAND 2GiB 3,3V 8-bit",	0xD5, 0, 2048, 0, LP_OPTIONS},
-	{"NAND 2GiB 1,8V 16-bit",	0xB5, 0, 2048, 0, LP_OPTIONS16},
-	{"NAND 2GiB 3,3V 16-bit",	0xC5, 0, 2048, 0, LP_OPTIONS16},
+	EXTENDED_ID_NAND("NAND 2GiB 1,8V 8-bit",  0xA5, 2048, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 2GiB 3,3V 8-bit",  0xD5, 2048, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 2GiB 1,8V 16-bit", 0xB5, 2048, LP_OPTIONS16),
+	EXTENDED_ID_NAND("NAND 2GiB 3,3V 16-bit", 0xC5, 2048, LP_OPTIONS16),
 
 	/* 32 Gigabit */
-	{"NAND 4GiB 1,8V 8-bit",	0xA7, 0, 4096, 0, LP_OPTIONS},
-	{"NAND 4GiB 3,3V 8-bit",	0xD7, 0, 4096, 0, LP_OPTIONS},
-	{"NAND 4GiB 1,8V 16-bit",	0xB7, 0, 4096, 0, LP_OPTIONS16},
-	{"NAND 4GiB 3,3V 16-bit",	0xC7, 0, 4096, 0, LP_OPTIONS16},
+	EXTENDED_ID_NAND("NAND 4GiB 1,8V 8-bit",  0xA7, 4096, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 4GiB 3,3V 8-bit",  0xD7, 4096, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 4GiB 1,8V 16-bit", 0xB7, 4096, LP_OPTIONS16),
+	EXTENDED_ID_NAND("NAND 4GiB 3,3V 16-bit", 0xC7, 4096, LP_OPTIONS16),
 
 	/* 64 Gigabit */
-	{"NAND 8GiB 1,8V 8-bit",	0xAE, 0, 8192, 0, LP_OPTIONS},
-	{"NAND 8GiB 3,3V 8-bit",	0xDE, 0, 8192, 0, LP_OPTIONS},
-	{"NAND 8GiB 1,8V 16-bit",	0xBE, 0, 8192, 0, LP_OPTIONS16},
-	{"NAND 8GiB 3,3V 16-bit",	0xCE, 0, 8192, 0, LP_OPTIONS16},
+	EXTENDED_ID_NAND("NAND 8GiB 1,8V 8-bit",  0xAE, 8192, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 8GiB 3,3V 8-bit",  0xDE, 8192, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 8GiB 1,8V 16-bit", 0xBE, 8192, LP_OPTIONS16),
+	EXTENDED_ID_NAND("NAND 8GiB 3,3V 16-bit", 0xCE, 8192, LP_OPTIONS16),
 
 	/* 128 Gigabit */
-	{"NAND 16GiB 1,8V 8-bit",	0x1A, 0, 16384, 0, LP_OPTIONS},
-	{"NAND 16GiB 3,3V 8-bit",	0x3A, 0, 16384, 0, LP_OPTIONS},
-	{"NAND 16GiB 1,8V 16-bit",	0x2A, 0, 16384, 0, LP_OPTIONS16},
-	{"NAND 16GiB 3,3V 16-bit",	0x4A, 0, 16384, 0, LP_OPTIONS16},
+	EXTENDED_ID_NAND("NAND 16GiB 1,8V 8-bit",  0x1A, 16384, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 16GiB 3,3V 8-bit",  0x3A, 16384, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 16GiB 1,8V 16-bit", 0x2A, 16384, LP_OPTIONS16),
+	EXTENDED_ID_NAND("NAND 16GiB 3,3V 16-bit", 0x4A, 16384, LP_OPTIONS16),
 
 	/* 256 Gigabit */
-	{"NAND 32GiB 1,8V 8-bit",	0x1C, 0, 32768, 0, LP_OPTIONS},
-	{"NAND 32GiB 3,3V 8-bit",	0x3C, 0, 32768, 0, LP_OPTIONS},
-	{"NAND 32GiB 1,8V 16-bit",	0x2C, 0, 32768, 0, LP_OPTIONS16},
-	{"NAND 32GiB 3,3V 16-bit",	0x4C, 0, 32768, 0, LP_OPTIONS16},
+	EXTENDED_ID_NAND("NAND 32GiB 1,8V 8-bit",  0x1C, 32768, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 32GiB 3,3V 8-bit",  0x3C, 32768, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 32GiB 1,8V 16-bit", 0x2C, 32768, LP_OPTIONS16),
+	EXTENDED_ID_NAND("NAND 32GiB 3,3V 16-bit", 0x4C, 32768, LP_OPTIONS16),
 
 	/* 512 Gigabit */
-	{"NAND 64GiB 1,8V 8-bit",	0x1E, 0, 65536, 0, LP_OPTIONS},
-	{"NAND 64GiB 3,3V 8-bit",	0x3E, 0, 65536, 0, LP_OPTIONS},
-	{"NAND 64GiB 1,8V 16-bit",	0x2E, 0, 65536, 0, LP_OPTIONS16},
-	{"NAND 64GiB 3,3V 16-bit",	0x4E, 0, 65536, 0, LP_OPTIONS16},
+	EXTENDED_ID_NAND("NAND 64GiB 1,8V 8-bit",  0x1E, 65536, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 64GiB 3,3V 8-bit",  0x3E, 65536, LP_OPTIONS),
+	EXTENDED_ID_NAND("NAND 64GiB 1,8V 16-bit", 0x2E, 65536, LP_OPTIONS16),
+	EXTENDED_ID_NAND("NAND 64GiB 3,3V 16-bit", 0x4E, 65536, LP_OPTIONS16),
 
-	/*
-	 * Renesas AND 1 Gigabit. Those chips do not support extended id and
-	 * have a strange page/block layout !  The chosen minimum erasesize is
-	 * 4 * 2 * 2048 = 16384 Byte, as those chips have an array of 4 page
-	 * planes 1 block = 2 pages, but due to plane arrangement the blocks
-	 * 0-3 consists of page 0 + 4,1 + 5, 2 + 6, 3 + 7 Anyway JFFS2 would
-	 * increase the eraseblock size so we chose a combined one which can be
-	 * erased in one go There are more speed improvements for reads and
-	 * writes possible, but not implemented now
-	 */
-	{"AND 128MiB 3,3V 8-bit",	0x01, 2048, 128, 0x4000,
-	 NAND_IS_AND | NAND_4PAGE_ARRAY | BBT_AUTO_REFRESH},
-
-	{NULL,}
+	{NULL}
 };
 
-/*
-*	Manufacturer ID list
-*/
+/* Manufacturer IDs */
 struct nand_manufacturers nand_manuf_ids[] = {
 	{NAND_MFR_TOSHIBA, "Toshiba"},
 	{NAND_MFR_SAMSUNG, "Samsung"},
diff --git a/drivers/mtd/nand/nandsim.c b/drivers/mtd/nand/nandsim.c
index 891c52a30e6a..cb38f3d94218 100644
--- a/drivers/mtd/nand/nandsim.c
+++ b/drivers/mtd/nand/nandsim.c
@@ -218,7 +218,6 @@ MODULE_PARM_DESC(bch,		 "Enable BCH ecc and set how many bits should "
 #define STATE_CMD_READOOB      0x00000005 /* read OOB area */
 #define STATE_CMD_ERASE1       0x00000006 /* sector erase first command */
 #define STATE_CMD_STATUS       0x00000007 /* read status */
-#define STATE_CMD_STATUS_M     0x00000008 /* read multi-plane status (isn't implemented) */
 #define STATE_CMD_SEQIN        0x00000009 /* sequential data input */
 #define STATE_CMD_READID       0x0000000A /* read ID */
 #define STATE_CMD_ERASE2       0x0000000B /* sector erase second command */
@@ -263,14 +262,13 @@ MODULE_PARM_DESC(bch,		 "Enable BCH ecc and set how many bits should "
 #define NS_OPER_STATES   6  /* Maximum number of states in operation */
 
 #define OPT_ANY          0xFFFFFFFF /* any chip supports this operation */
-#define OPT_PAGE256      0x00000001 /* 256-byte  page chips */
 #define OPT_PAGE512      0x00000002 /* 512-byte  page chips */
 #define OPT_PAGE2048     0x00000008 /* 2048-byte page chips */
 #define OPT_SMARTMEDIA   0x00000010 /* SmartMedia technology chips */
 #define OPT_PAGE512_8BIT 0x00000040 /* 512-byte page chips with 8-bit bus width */
 #define OPT_PAGE4096     0x00000080 /* 4096-byte page chips */
 #define OPT_LARGEPAGE    (OPT_PAGE2048 | OPT_PAGE4096) /* 2048 & 4096-byte page chips */
-#define OPT_SMALLPAGE    (OPT_PAGE256  | OPT_PAGE512)  /* 256 and 512-byte page chips */
+#define OPT_SMALLPAGE    (OPT_PAGE512) /* 512-byte page chips */
 
 /* Remove action bits from state */
 #define NS_STATE(x) ((x) & ~ACTION_MASK)
@@ -406,8 +404,6 @@ static struct nandsim_operations {
 	{OPT_ANY, {STATE_CMD_ERASE1, STATE_ADDR_SEC, STATE_CMD_ERASE2 | ACTION_SECERASE, STATE_READY}},
 	/* Read status */
 	{OPT_ANY, {STATE_CMD_STATUS, STATE_DATAOUT_STATUS, STATE_READY}},
-	/* Read multi-plane status */
-	{OPT_SMARTMEDIA, {STATE_CMD_STATUS_M, STATE_DATAOUT_STATUS_M, STATE_READY}},
 	/* Read ID */
 	{OPT_ANY, {STATE_CMD_READID, STATE_ADDR_ZERO, STATE_DATAOUT_ID, STATE_READY}},
 	/* Large page devices read page */
@@ -699,10 +695,7 @@ static int init_nandsim(struct mtd_info *mtd)
 	ns->geom.secszoob = ns->geom.secsz + ns->geom.oobsz * ns->geom.pgsec;
 	ns->options = 0;
 
-	if (ns->geom.pgsz == 256) {
-		ns->options |= OPT_PAGE256;
-	}
-	else if (ns->geom.pgsz == 512) {
+	if (ns->geom.pgsz == 512) {
 		ns->options |= OPT_PAGE512;
 		if (ns->busw == 8)
 			ns->options |= OPT_PAGE512_8BIT;
@@ -769,9 +762,9 @@ static int init_nandsim(struct mtd_info *mtd)
 	}
 
 	/* Detect how many ID bytes the NAND chip outputs */
-        for (i = 0; nand_flash_ids[i].name != NULL; i++) {
-                if (second_id_byte != nand_flash_ids[i].id)
-                        continue;
+	for (i = 0; nand_flash_ids[i].name != NULL; i++) {
+		if (second_id_byte != nand_flash_ids[i].dev_id)
+			continue;
 	}
 
 	if (ns->busw == 16)
@@ -1079,8 +1072,6 @@ static char *get_state_name(uint32_t state)
 			return "STATE_CMD_ERASE1";
 		case STATE_CMD_STATUS:
 			return "STATE_CMD_STATUS";
-		case STATE_CMD_STATUS_M:
-			return "STATE_CMD_STATUS_M";
 		case STATE_CMD_SEQIN:
 			return "STATE_CMD_SEQIN";
 		case STATE_CMD_READID:
@@ -1145,7 +1136,6 @@ static int check_command(int cmd)
 	case NAND_CMD_RNDOUTSTART:
 		return 0;
 
-	case NAND_CMD_STATUS_MULTI:
 	default:
 		return 1;
 	}
@@ -1171,8 +1161,6 @@ static uint32_t get_state_by_command(unsigned command)
 			return STATE_CMD_ERASE1;
 		case NAND_CMD_STATUS:
 			return STATE_CMD_STATUS;
-		case NAND_CMD_STATUS_MULTI:
-			return STATE_CMD_STATUS_M;
 		case NAND_CMD_SEQIN:
 			return STATE_CMD_SEQIN;
 		case NAND_CMD_READID:
@@ -2306,7 +2294,7 @@ static int __init ns_init_module(void)
 		nand->geom.idbytes = 2;
 	nand->regs.status = NS_STATUS_OK(nand);
 	nand->nxstate = STATE_UNKNOWN;
-	nand->options |= OPT_PAGE256; /* temporary value */
+	nand->options |= OPT_PAGE512; /* temporary value */
 	nand->ids[0] = first_id_byte;
 	nand->ids[1] = second_id_byte;
 	nand->ids[2] = third_id_byte;
diff --git a/drivers/mtd/nand/nuc900_nand.c b/drivers/mtd/nand/nuc900_nand.c
index a6191198d259..cd6be2ed53a8 100644
--- a/drivers/mtd/nand/nuc900_nand.c
+++ b/drivers/mtd/nand/nuc900_nand.c
@@ -177,15 +177,6 @@ static void nuc900_nand_command_lp(struct mtd_info *mtd, unsigned int command,
 	case NAND_CMD_SEQIN:
 	case NAND_CMD_RNDIN:
 	case NAND_CMD_STATUS:
-	case NAND_CMD_DEPLETE1:
-		return;
-
-	case NAND_CMD_STATUS_ERROR:
-	case NAND_CMD_STATUS_ERROR0:
-	case NAND_CMD_STATUS_ERROR1:
-	case NAND_CMD_STATUS_ERROR2:
-	case NAND_CMD_STATUS_ERROR3:
-		udelay(chip->chip_delay);
 		return;
 
 	case NAND_CMD_RESET:
diff --git a/drivers/mtd/nand/omap2.c b/drivers/mtd/nand/omap2.c
index 8e820ddf4e08..81b80af55872 100644
--- a/drivers/mtd/nand/omap2.c
+++ b/drivers/mtd/nand/omap2.c
@@ -1023,9 +1023,9 @@ static int omap_wait(struct mtd_info *mtd, struct nand_chip *chip)
 	int status, state = this->state;
 
 	if (state == FL_ERASING)
-		timeo += (HZ * 400) / 1000;
+		timeo += msecs_to_jiffies(400);
 	else
-		timeo += (HZ * 20) / 1000;
+		timeo += msecs_to_jiffies(20);
 
 	writeb(NAND_CMD_STATUS & 0xFF, info->reg.gpmc_nand_command);
 	while (time_before(jiffies, timeo)) {
@@ -1701,8 +1701,9 @@ static int omap3_init_bch(struct mtd_info *mtd, int ecc_opt)
 		elm_node = of_find_node_by_phandle(be32_to_cpup(parp));
 		pdev = of_find_device_by_node(elm_node);
 		info->elm_dev = &pdev->dev;
-		elm_config(info->elm_dev, bch_type);
-		info->is_elm_used = true;
+
+		if (elm_config(info->elm_dev, bch_type) == 0)
+			info->is_elm_used = true;
 	}
 
 	if (info->is_elm_used && (mtd->writesize <= 4096)) {
diff --git a/drivers/mtd/nand/orion_nand.c b/drivers/mtd/nand/orion_nand.c
index cd72b9299f6b..8fbd00208610 100644
--- a/drivers/mtd/nand/orion_nand.c
+++ b/drivers/mtd/nand/orion_nand.c
@@ -231,18 +231,7 @@ static struct platform_driver orion_nand_driver = {
 	},
 };
 
-static int __init orion_nand_init(void)
-{
-	return platform_driver_probe(&orion_nand_driver, orion_nand_probe);
-}
-
-static void __exit orion_nand_exit(void)
-{
-	platform_driver_unregister(&orion_nand_driver);
-}
-
-module_init(orion_nand_init);
-module_exit(orion_nand_exit);
+module_platform_driver_probe(orion_nand_driver, orion_nand_probe);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Tzachi Perelstein");
diff --git a/drivers/mtd/nand/ppchameleonevb.c b/drivers/mtd/nand/ppchameleonevb.c
deleted file mode 100644
index 0ddd90e5788f..000000000000
--- a/drivers/mtd/nand/ppchameleonevb.c
+++ /dev/null
@@ -1,403 +0,0 @@
-/*
- *  drivers/mtd/nand/ppchameleonevb.c
- *
- *  Copyright (C) 2003 DAVE Srl (info@wawnet.biz)
- *
- *  Derived from drivers/mtd/nand/edb7312.c
- *
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- *  Overview:
- *   This is a device driver for the NAND flash devices found on the
- *   PPChameleon/PPChameleonEVB system.
- *   PPChameleon options (autodetected):
- *   - BA model: no NAND
- *   - ME model: 32MB (Samsung K9F5608U0B)
- *   - HI model: 128MB (Samsung K9F1G08UOM)
- *   PPChameleonEVB options:
- *   - 32MB (Samsung K9F5608U0B)
- */
-
-#include <linux/init.h>
-#include <linux/slab.h>
-#include <linux/module.h>
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/nand.h>
-#include <linux/mtd/partitions.h>
-#include <asm/io.h>
-#include <platforms/PPChameleonEVB.h>
-
-#undef USE_READY_BUSY_PIN
-#define USE_READY_BUSY_PIN
-/* see datasheets (tR) */
-#define NAND_BIG_DELAY_US		25
-#define NAND_SMALL_DELAY_US		10
-
-/* handy sizes */
-#define SZ_4M                           0x00400000
-#define NAND_SMALL_SIZE                 0x02000000
-#define NAND_MTD_NAME		"ppchameleon-nand"
-#define NAND_EVB_MTD_NAME	"ppchameleonevb-nand"
-
-/* GPIO pins used to drive NAND chip mounted on processor module */
-#define NAND_nCE_GPIO_PIN 		(0x80000000 >> 1)
-#define NAND_CLE_GPIO_PIN 		(0x80000000 >> 2)
-#define NAND_ALE_GPIO_PIN 		(0x80000000 >> 3)
-#define NAND_RB_GPIO_PIN 		(0x80000000 >> 4)
-/* GPIO pins used to drive NAND chip mounted on EVB */
-#define NAND_EVB_nCE_GPIO_PIN 	(0x80000000 >> 14)
-#define NAND_EVB_CLE_GPIO_PIN 	(0x80000000 >> 15)
-#define NAND_EVB_ALE_GPIO_PIN 	(0x80000000 >> 16)
-#define NAND_EVB_RB_GPIO_PIN 	(0x80000000 >> 31)
-
-/*
- * MTD structure for PPChameleonEVB board
- */
-static struct mtd_info *ppchameleon_mtd = NULL;
-static struct mtd_info *ppchameleonevb_mtd = NULL;
-
-/*
- * Module stuff
- */
-static unsigned long ppchameleon_fio_pbase = CFG_NAND0_PADDR;
-static unsigned long ppchameleonevb_fio_pbase = CFG_NAND1_PADDR;
-
-#ifdef MODULE
-module_param(ppchameleon_fio_pbase, ulong, 0);
-module_param(ppchameleonevb_fio_pbase, ulong, 0);
-#else
-__setup("ppchameleon_fio_pbase=", ppchameleon_fio_pbase);
-__setup("ppchameleonevb_fio_pbase=", ppchameleonevb_fio_pbase);
-#endif
-
-/*
- * Define static partitions for flash devices
- */
-static struct mtd_partition partition_info_hi[] = {
-      { .name = "PPChameleon HI Nand Flash",
-	.offset = 0,
-	.size = 128 * 1024 * 1024
-      }
-};
-
-static struct mtd_partition partition_info_me[] = {
-      { .name = "PPChameleon ME Nand Flash",
-	.offset = 0,
-	.size = 32 * 1024 * 1024
-      }
-};
-
-static struct mtd_partition partition_info_evb[] = {
-      { .name = "PPChameleonEVB Nand Flash",
-	.offset = 0,
-	.size = 32 * 1024 * 1024
-      }
-};
-
-#define NUM_PARTITIONS 1
-
-/*
- *	hardware specific access to control-lines
- */
-static void ppchameleon_hwcontrol(struct mtd_info *mtdinfo, int cmd,
-				  unsigned int ctrl)
-{
-	struct nand_chip *chip = mtd->priv;
-
-	if (ctrl & NAND_CTRL_CHANGE) {
-#error Missing headerfiles. No way to fix this. -tglx
-		switch (cmd) {
-		case NAND_CTL_SETCLE:
-			MACRO_NAND_CTL_SETCLE((unsigned long)CFG_NAND0_PADDR);
-			break;
-		case NAND_CTL_CLRCLE:
-			MACRO_NAND_CTL_CLRCLE((unsigned long)CFG_NAND0_PADDR);
-			break;
-		case NAND_CTL_SETALE:
-			MACRO_NAND_CTL_SETALE((unsigned long)CFG_NAND0_PADDR);
-			break;
-		case NAND_CTL_CLRALE:
-			MACRO_NAND_CTL_CLRALE((unsigned long)CFG_NAND0_PADDR);
-			break;
-		case NAND_CTL_SETNCE:
-			MACRO_NAND_ENABLE_CE((unsigned long)CFG_NAND0_PADDR);
-			break;
-		case NAND_CTL_CLRNCE:
-			MACRO_NAND_DISABLE_CE((unsigned long)CFG_NAND0_PADDR);
-			break;
-		}
-	}
-	if (cmd != NAND_CMD_NONE)
-		writeb(cmd, chip->IO_ADDR_W);
-}
-
-static void ppchameleonevb_hwcontrol(struct mtd_info *mtdinfo, int cmd,
-				     unsigned int ctrl)
-{
-	struct nand_chip *chip = mtd->priv;
-
-	if (ctrl & NAND_CTRL_CHANGE) {
-#error Missing headerfiles. No way to fix this. -tglx
-		switch (cmd) {
-		case NAND_CTL_SETCLE:
-			MACRO_NAND_CTL_SETCLE((unsigned long)CFG_NAND1_PADDR);
-			break;
-		case NAND_CTL_CLRCLE:
-			MACRO_NAND_CTL_CLRCLE((unsigned long)CFG_NAND1_PADDR);
-			break;
-		case NAND_CTL_SETALE:
-			MACRO_NAND_CTL_SETALE((unsigned long)CFG_NAND1_PADDR);
-			break;
-		case NAND_CTL_CLRALE:
-			MACRO_NAND_CTL_CLRALE((unsigned long)CFG_NAND1_PADDR);
-			break;
-		case NAND_CTL_SETNCE:
-			MACRO_NAND_ENABLE_CE((unsigned long)CFG_NAND1_PADDR);
-			break;
-		case NAND_CTL_CLRNCE:
-			MACRO_NAND_DISABLE_CE((unsigned long)CFG_NAND1_PADDR);
-			break;
-		}
-	}
-	if (cmd != NAND_CMD_NONE)
-		writeb(cmd, chip->IO_ADDR_W);
-}
-
-#ifdef USE_READY_BUSY_PIN
-/*
- *	read device ready pin
- */
-static int ppchameleon_device_ready(struct mtd_info *minfo)
-{
-	if (in_be32((volatile unsigned *)GPIO0_IR) & NAND_RB_GPIO_PIN)
-		return 1;
-	return 0;
-}
-
-static int ppchameleonevb_device_ready(struct mtd_info *minfo)
-{
-	if (in_be32((volatile unsigned *)GPIO0_IR) & NAND_EVB_RB_GPIO_PIN)
-		return 1;
-	return 0;
-}
-#endif
-
-/*
- * Main initialization routine
- */
-static int __init ppchameleonevb_init(void)
-{
-	struct nand_chip *this;
-	void __iomem *ppchameleon_fio_base;
-	void __iomem *ppchameleonevb_fio_base;
-
-	/*********************************
-	* Processor module NAND (if any) *
-	*********************************/
-	/* Allocate memory for MTD device structure and private data */
-	ppchameleon_mtd = kmalloc(sizeof(struct mtd_info) + sizeof(struct nand_chip), GFP_KERNEL);
-	if (!ppchameleon_mtd) {
-		printk("Unable to allocate PPChameleon NAND MTD device structure.\n");
-		return -ENOMEM;
-	}
-
-	/* map physical address */
-	ppchameleon_fio_base = ioremap(ppchameleon_fio_pbase, SZ_4M);
-	if (!ppchameleon_fio_base) {
-		printk("ioremap PPChameleon NAND flash failed\n");
-		kfree(ppchameleon_mtd);
-		return -EIO;
-	}
-
-	/* Get pointer to private data */
-	this = (struct nand_chip *)(&ppchameleon_mtd[1]);
-
-	/* Initialize structures */
-	memset(ppchameleon_mtd, 0, sizeof(struct mtd_info));
-	memset(this, 0, sizeof(struct nand_chip));
-
-	/* Link the private data with the MTD structure */
-	ppchameleon_mtd->priv = this;
-	ppchameleon_mtd->owner = THIS_MODULE;
-
-	/* Initialize GPIOs */
-	/* Pin mapping for NAND chip */
-	/*
-	   CE   GPIO_01
-	   CLE  GPIO_02
-	   ALE  GPIO_03
-	   R/B  GPIO_04
-	 */
-	/* output select */
-	out_be32((volatile unsigned *)GPIO0_OSRH, in_be32((volatile unsigned *)GPIO0_OSRH) & 0xC0FFFFFF);
-	/* three-state select */
-	out_be32((volatile unsigned *)GPIO0_TSRH, in_be32((volatile unsigned *)GPIO0_TSRH) & 0xC0FFFFFF);
-	/* enable output driver */
-	out_be32((volatile unsigned *)GPIO0_TCR,
-		 in_be32((volatile unsigned *)GPIO0_TCR) | NAND_nCE_GPIO_PIN | NAND_CLE_GPIO_PIN | NAND_ALE_GPIO_PIN);
-#ifdef USE_READY_BUSY_PIN
-	/* three-state select */
-	out_be32((volatile unsigned *)GPIO0_TSRH, in_be32((volatile unsigned *)GPIO0_TSRH) & 0xFF3FFFFF);
-	/* high-impedecence */
-	out_be32((volatile unsigned *)GPIO0_TCR, in_be32((volatile unsigned *)GPIO0_TCR) & (~NAND_RB_GPIO_PIN));
-	/* input select */
-	out_be32((volatile unsigned *)GPIO0_ISR1H,
-		 (in_be32((volatile unsigned *)GPIO0_ISR1H) & 0xFF3FFFFF) | 0x00400000);
-#endif
-
-	/* insert callbacks */
-	this->IO_ADDR_R = ppchameleon_fio_base;
-	this->IO_ADDR_W = ppchameleon_fio_base;
-	this->cmd_ctrl = ppchameleon_hwcontrol;
-#ifdef USE_READY_BUSY_PIN
-	this->dev_ready = ppchameleon_device_ready;
-#endif
-	this->chip_delay = NAND_BIG_DELAY_US;
-	/* ECC mode */
-	this->ecc.mode = NAND_ECC_SOFT;
-
-	/* Scan to find existence of the device (it could not be mounted) */
-	if (nand_scan(ppchameleon_mtd, 1)) {
-		iounmap((void *)ppchameleon_fio_base);
-		ppchameleon_fio_base = NULL;
-		kfree(ppchameleon_mtd);
-		goto nand_evb_init;
-	}
-#ifndef USE_READY_BUSY_PIN
-	/* Adjust delay if necessary */
-	if (ppchameleon_mtd->size == NAND_SMALL_SIZE)
-		this->chip_delay = NAND_SMALL_DELAY_US;
-#endif
-
-	ppchameleon_mtd->name = "ppchameleon-nand";
-
-	/* Register the partitions */
-	mtd_device_parse_register(ppchameleon_mtd, NULL, NULL,
-				  ppchameleon_mtd->size == NAND_SMALL_SIZE ?
-					partition_info_me : partition_info_hi,
-				  NUM_PARTITIONS);
-
- nand_evb_init:
-	/****************************
-	* EVB NAND (always present) *
-	****************************/
-	/* Allocate memory for MTD device structure and private data */
-	ppchameleonevb_mtd = kmalloc(sizeof(struct mtd_info) + sizeof(struct nand_chip), GFP_KERNEL);
-	if (!ppchameleonevb_mtd) {
-		printk("Unable to allocate PPChameleonEVB NAND MTD device structure.\n");
-		if (ppchameleon_fio_base)
-			iounmap(ppchameleon_fio_base);
-		return -ENOMEM;
-	}
-
-	/* map physical address */
-	ppchameleonevb_fio_base = ioremap(ppchameleonevb_fio_pbase, SZ_4M);
-	if (!ppchameleonevb_fio_base) {
-		printk("ioremap PPChameleonEVB NAND flash failed\n");
-		kfree(ppchameleonevb_mtd);
-		if (ppchameleon_fio_base)
-			iounmap(ppchameleon_fio_base);
-		return -EIO;
-	}
-
-	/* Get pointer to private data */
-	this = (struct nand_chip *)(&ppchameleonevb_mtd[1]);
-
-	/* Initialize structures */
-	memset(ppchameleonevb_mtd, 0, sizeof(struct mtd_info));
-	memset(this, 0, sizeof(struct nand_chip));
-
-	/* Link the private data with the MTD structure */
-	ppchameleonevb_mtd->priv = this;
-
-	/* Initialize GPIOs */
-	/* Pin mapping for NAND chip */
-	/*
-	   CE   GPIO_14
-	   CLE  GPIO_15
-	   ALE  GPIO_16
-	   R/B  GPIO_31
-	 */
-	/* output select */
-	out_be32((volatile unsigned *)GPIO0_OSRH, in_be32((volatile unsigned *)GPIO0_OSRH) & 0xFFFFFFF0);
-	out_be32((volatile unsigned *)GPIO0_OSRL, in_be32((volatile unsigned *)GPIO0_OSRL) & 0x3FFFFFFF);
-	/* three-state select */
-	out_be32((volatile unsigned *)GPIO0_TSRH, in_be32((volatile unsigned *)GPIO0_TSRH) & 0xFFFFFFF0);
-	out_be32((volatile unsigned *)GPIO0_TSRL, in_be32((volatile unsigned *)GPIO0_TSRL) & 0x3FFFFFFF);
-	/* enable output driver */
-	out_be32((volatile unsigned *)GPIO0_TCR, in_be32((volatile unsigned *)GPIO0_TCR) | NAND_EVB_nCE_GPIO_PIN |
-		 NAND_EVB_CLE_GPIO_PIN | NAND_EVB_ALE_GPIO_PIN);
-#ifdef USE_READY_BUSY_PIN
-	/* three-state select */
-	out_be32((volatile unsigned *)GPIO0_TSRL, in_be32((volatile unsigned *)GPIO0_TSRL) & 0xFFFFFFFC);
-	/* high-impedecence */
-	out_be32((volatile unsigned *)GPIO0_TCR, in_be32((volatile unsigned *)GPIO0_TCR) & (~NAND_EVB_RB_GPIO_PIN));
-	/* input select */
-	out_be32((volatile unsigned *)GPIO0_ISR1L,
-		 (in_be32((volatile unsigned *)GPIO0_ISR1L) & 0xFFFFFFFC) | 0x00000001);
-#endif
-
-	/* insert callbacks */
-	this->IO_ADDR_R = ppchameleonevb_fio_base;
-	this->IO_ADDR_W = ppchameleonevb_fio_base;
-	this->cmd_ctrl = ppchameleonevb_hwcontrol;
-#ifdef USE_READY_BUSY_PIN
-	this->dev_ready = ppchameleonevb_device_ready;
-#endif
-	this->chip_delay = NAND_SMALL_DELAY_US;
-
-	/* ECC mode */
-	this->ecc.mode = NAND_ECC_SOFT;
-
-	/* Scan to find existence of the device */
-	if (nand_scan(ppchameleonevb_mtd, 1)) {
-		iounmap((void *)ppchameleonevb_fio_base);
-		kfree(ppchameleonevb_mtd);
-		if (ppchameleon_fio_base)
-			iounmap(ppchameleon_fio_base);
-		return -ENXIO;
-	}
-
-	ppchameleonevb_mtd->name = NAND_EVB_MTD_NAME;
-
-	/* Register the partitions */
-	mtd_device_parse_register(ppchameleonevb_mtd, NULL, NULL,
-				  ppchameleon_mtd->size == NAND_SMALL_SIZE ?
-				  partition_info_me : partition_info_hi,
-				  NUM_PARTITIONS);
-
-	/* Return happy */
-	return 0;
-}
-
-module_init(ppchameleonevb_init);
-
-/*
- * Clean up routine
- */
-static void __exit ppchameleonevb_cleanup(void)
-{
-	struct nand_chip *this;
-
-	/* Release resources, unregister device(s) */
-	nand_release(ppchameleon_mtd);
-	nand_release(ppchameleonevb_mtd);
-
-	/* Release iomaps */
-	this = (struct nand_chip *) &ppchameleon_mtd[1];
-	iounmap((void *) this->IO_ADDR_R);
-	this = (struct nand_chip *) &ppchameleonevb_mtd[1];
-	iounmap((void *) this->IO_ADDR_R);
-
-	/* Free the MTD device structure */
-	kfree (ppchameleon_mtd);
-	kfree (ppchameleonevb_mtd);
-}
-module_exit(ppchameleonevb_cleanup);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("DAVE Srl <support-ppchameleon@dave-tech.it>");
-MODULE_DESCRIPTION("MTD map driver for DAVE Srl PPChameleonEVB board");
diff --git a/drivers/mtd/nand/pxa3xx_nand.c b/drivers/mtd/nand/pxa3xx_nand.c
index 37ee75c7bacb..dec80ca6a5ce 100644
--- a/drivers/mtd/nand/pxa3xx_nand.c
+++ b/drivers/mtd/nand/pxa3xx_nand.c
@@ -989,7 +989,7 @@ static int pxa3xx_nand_scan(struct mtd_info *mtd)
 	}
 
 	pxa3xx_flash_ids[0].name = f->name;
-	pxa3xx_flash_ids[0].id = (f->chip_id >> 8) & 0xffff;
+	pxa3xx_flash_ids[0].dev_id = (f->chip_id >> 8) & 0xffff;
 	pxa3xx_flash_ids[0].pagesize = f->page_size;
 	chipsize = (uint64_t)f->num_blocks * f->page_per_block * f->page_size;
 	pxa3xx_flash_ids[0].chipsize = chipsize >> 20;
diff --git a/drivers/mtd/nand/rtc_from4.c b/drivers/mtd/nand/rtc_from4.c
deleted file mode 100644
index e55b5cfbe145..000000000000
--- a/drivers/mtd/nand/rtc_from4.c
+++ /dev/null
@@ -1,624 +0,0 @@
-/*
- *  drivers/mtd/nand/rtc_from4.c
- *
- *  Copyright (C) 2004  Red Hat, Inc.
- *
- *  Derived from drivers/mtd/nand/spia.c
- *       Copyright (C) 2000 Steven J. Hill (sjhill@realitydiluted.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.
- *
- * Overview:
- *   This is a device driver for the AG-AND flash device found on the
- *   Renesas Technology Corp. Flash ROM 4-slot interface board (FROM_BOARD4),
- *   which utilizes the Renesas HN29V1G91T-30 part.
- *   This chip is a 1 GBibit (128MiB x 8 bits) AG-AND flash device.
- */
-
-#include <linux/delay.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/slab.h>
-#include <linux/rslib.h>
-#include <linux/bitrev.h>
-#include <linux/module.h>
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/nand.h>
-#include <linux/mtd/partitions.h>
-#include <asm/io.h>
-
-/*
- * MTD structure for Renesas board
- */
-static struct mtd_info *rtc_from4_mtd = NULL;
-
-#define RTC_FROM4_MAX_CHIPS	2
-
-/* HS77x9 processor register defines */
-#define SH77X9_BCR1	((volatile unsigned short *)(0xFFFFFF60))
-#define SH77X9_BCR2	((volatile unsigned short *)(0xFFFFFF62))
-#define SH77X9_WCR1	((volatile unsigned short *)(0xFFFFFF64))
-#define SH77X9_WCR2	((volatile unsigned short *)(0xFFFFFF66))
-#define SH77X9_MCR	((volatile unsigned short *)(0xFFFFFF68))
-#define SH77X9_PCR	((volatile unsigned short *)(0xFFFFFF6C))
-#define SH77X9_FRQCR	((volatile unsigned short *)(0xFFFFFF80))
-
-/*
- * Values specific to the Renesas Technology Corp. FROM_BOARD4 (used with HS77x9 processor)
- */
-/* Address where flash is mapped */
-#define RTC_FROM4_FIO_BASE	0x14000000
-
-/* CLE and ALE are tied to address lines 5 & 4, respectively */
-#define RTC_FROM4_CLE		(1 << 5)
-#define RTC_FROM4_ALE		(1 << 4)
-
-/* address lines A24-A22 used for chip selection */
-#define RTC_FROM4_NAND_ADDR_SLOT3	(0x00800000)
-#define RTC_FROM4_NAND_ADDR_SLOT4	(0x00C00000)
-#define RTC_FROM4_NAND_ADDR_FPGA	(0x01000000)
-/* mask address lines A24-A22 used for chip selection */
-#define RTC_FROM4_NAND_ADDR_MASK	(RTC_FROM4_NAND_ADDR_SLOT3 | RTC_FROM4_NAND_ADDR_SLOT4 | RTC_FROM4_NAND_ADDR_FPGA)
-
-/* FPGA status register for checking device ready (bit zero) */
-#define RTC_FROM4_FPGA_SR		(RTC_FROM4_NAND_ADDR_FPGA | 0x00000002)
-#define RTC_FROM4_DEVICE_READY		0x0001
-
-/* FPGA Reed-Solomon ECC Control register */
-
-#define RTC_FROM4_RS_ECC_CTL		(RTC_FROM4_NAND_ADDR_FPGA | 0x00000050)
-#define RTC_FROM4_RS_ECC_CTL_CLR	(1 << 7)
-#define RTC_FROM4_RS_ECC_CTL_GEN	(1 << 6)
-#define RTC_FROM4_RS_ECC_CTL_FD_E	(1 << 5)
-
-/* FPGA Reed-Solomon ECC code base */
-#define RTC_FROM4_RS_ECC		(RTC_FROM4_NAND_ADDR_FPGA | 0x00000060)
-#define RTC_FROM4_RS_ECCN		(RTC_FROM4_NAND_ADDR_FPGA | 0x00000080)
-
-/* FPGA Reed-Solomon ECC check register */
-#define RTC_FROM4_RS_ECC_CHK		(RTC_FROM4_NAND_ADDR_FPGA | 0x00000070)
-#define RTC_FROM4_RS_ECC_CHK_ERROR	(1 << 7)
-
-#define ERR_STAT_ECC_AVAILABLE		0x20
-
-/* Undefine for software ECC */
-#define RTC_FROM4_HWECC	1
-
-/* Define as 1 for no virtual erase blocks (in JFFS2) */
-#define RTC_FROM4_NO_VIRTBLOCKS	0
-
-/*
- * Module stuff
- */
-static void __iomem *rtc_from4_fio_base = (void *)P2SEGADDR(RTC_FROM4_FIO_BASE);
-
-static const struct mtd_partition partition_info[] = {
-	{
-	 .name = "Renesas flash partition 1",
-	 .offset = 0,
-	 .size = MTDPART_SIZ_FULL},
-};
-
-#define NUM_PARTITIONS 1
-
-/*
- *	hardware specific flash bbt decriptors
- *	Note: this is to allow debugging by disabling
- *		NAND_BBT_CREATE and/or NAND_BBT_WRITE
- *
- */
-static uint8_t bbt_pattern[] = { 'B', 'b', 't', '0' };
-static uint8_t mirror_pattern[] = { '1', 't', 'b', 'B' };
-
-static struct nand_bbt_descr rtc_from4_bbt_main_descr = {
-	.options = NAND_BBT_LASTBLOCK | NAND_BBT_CREATE | NAND_BBT_WRITE
-		| NAND_BBT_2BIT | NAND_BBT_VERSION | NAND_BBT_PERCHIP,
-	.offs = 40,
-	.len = 4,
-	.veroffs = 44,
-	.maxblocks = 4,
-	.pattern = bbt_pattern
-};
-
-static struct nand_bbt_descr rtc_from4_bbt_mirror_descr = {
-	.options = NAND_BBT_LASTBLOCK | NAND_BBT_CREATE | NAND_BBT_WRITE
-		| NAND_BBT_2BIT | NAND_BBT_VERSION | NAND_BBT_PERCHIP,
-	.offs = 40,
-	.len = 4,
-	.veroffs = 44,
-	.maxblocks = 4,
-	.pattern = mirror_pattern
-};
-
-#ifdef RTC_FROM4_HWECC
-
-/* the Reed Solomon control structure */
-static struct rs_control *rs_decoder;
-
-/*
- *      hardware specific Out Of Band information
- */
-static struct nand_ecclayout rtc_from4_nand_oobinfo = {
-	.eccbytes = 32,
-	.eccpos = {
-		   0, 1, 2, 3, 4, 5, 6, 7,
-		   8, 9, 10, 11, 12, 13, 14, 15,
-		   16, 17, 18, 19, 20, 21, 22, 23,
-		   24, 25, 26, 27, 28, 29, 30, 31},
-	.oobfree = {{32, 32}}
-};
-
-#endif
-
-/*
- * rtc_from4_hwcontrol - hardware specific access to control-lines
- * @mtd:	MTD device structure
- * @cmd:	hardware control command
- *
- * Address lines (A5 and A4) are used to control Command and Address Latch
- * Enable on this board, so set the read/write address appropriately.
- *
- * Chip Enable is also controlled by the Chip Select (CS5) and
- * Address lines (A24-A22), so no action is required here.
- *
- */
-static void rtc_from4_hwcontrol(struct mtd_info *mtd, int cmd,
-				unsigned int ctrl)
-{
-	struct nand_chip *chip = (mtd->priv);
-
-	if (cmd == NAND_CMD_NONE)
-		return;
-
-	if (ctrl & NAND_CLE)
-		writeb(cmd, chip->IO_ADDR_W | RTC_FROM4_CLE);
-	else
-		writeb(cmd, chip->IO_ADDR_W | RTC_FROM4_ALE);
-}
-
-/*
- * rtc_from4_nand_select_chip - hardware specific chip select
- * @mtd:	MTD device structure
- * @chip:	Chip to select (0 == slot 3, 1 == slot 4)
- *
- * The chip select is based on address lines A24-A22.
- * This driver uses flash slots 3 and 4 (A23-A22).
- *
- */
-static void rtc_from4_nand_select_chip(struct mtd_info *mtd, int chip)
-{
-	struct nand_chip *this = mtd->priv;
-
-	this->IO_ADDR_R = (void __iomem *)((unsigned long)this->IO_ADDR_R & ~RTC_FROM4_NAND_ADDR_MASK);
-	this->IO_ADDR_W = (void __iomem *)((unsigned long)this->IO_ADDR_W & ~RTC_FROM4_NAND_ADDR_MASK);
-
-	switch (chip) {
-
-	case 0:		/* select slot 3 chip */
-		this->IO_ADDR_R = (void __iomem *)((unsigned long)this->IO_ADDR_R | RTC_FROM4_NAND_ADDR_SLOT3);
-		this->IO_ADDR_W = (void __iomem *)((unsigned long)this->IO_ADDR_W | RTC_FROM4_NAND_ADDR_SLOT3);
-		break;
-	case 1:		/* select slot 4 chip */
-		this->IO_ADDR_R = (void __iomem *)((unsigned long)this->IO_ADDR_R | RTC_FROM4_NAND_ADDR_SLOT4);
-		this->IO_ADDR_W = (void __iomem *)((unsigned long)this->IO_ADDR_W | RTC_FROM4_NAND_ADDR_SLOT4);
-		break;
-
-	}
-}
-
-/*
- * rtc_from4_nand_device_ready - hardware specific ready/busy check
- * @mtd:	MTD device structure
- *
- * This board provides the Ready/Busy state in the status register
- * of the FPGA.  Bit zero indicates the RDY(1)/BSY(0) signal.
- *
- */
-static int rtc_from4_nand_device_ready(struct mtd_info *mtd)
-{
-	unsigned short status;
-
-	status = *((volatile unsigned short *)(rtc_from4_fio_base + RTC_FROM4_FPGA_SR));
-
-	return (status & RTC_FROM4_DEVICE_READY);
-
-}
-
-/*
- * deplete - code to perform device recovery in case there was a power loss
- * @mtd:	MTD device structure
- * @chip:	Chip to select (0 == slot 3, 1 == slot 4)
- *
- * If there was a sudden loss of power during an erase operation, a
- * "device recovery" operation must be performed when power is restored
- * to ensure correct operation.  This routine performs the required steps
- * for the requested chip.
- *
- * See page 86 of the data sheet for details.
- *
- */
-static void deplete(struct mtd_info *mtd, int chip)
-{
-	struct nand_chip *this = mtd->priv;
-
-	/* wait until device is ready */
-	while (!this->dev_ready(mtd)) ;
-
-	this->select_chip(mtd, chip);
-
-	/* Send the commands for device recovery, phase 1 */
-	this->cmdfunc(mtd, NAND_CMD_DEPLETE1, 0x0000, 0x0000);
-	this->cmdfunc(mtd, NAND_CMD_DEPLETE2, -1, -1);
-
-	/* Send the commands for device recovery, phase 2 */
-	this->cmdfunc(mtd, NAND_CMD_DEPLETE1, 0x0000, 0x0004);
-	this->cmdfunc(mtd, NAND_CMD_DEPLETE2, -1, -1);
-
-}
-
-#ifdef RTC_FROM4_HWECC
-/*
- * rtc_from4_enable_hwecc - hardware specific hardware ECC enable function
- * @mtd:	MTD device structure
- * @mode:	I/O mode; read or write
- *
- * enable hardware ECC for data read or write
- *
- */
-static void rtc_from4_enable_hwecc(struct mtd_info *mtd, int mode)
-{
-	volatile unsigned short *rs_ecc_ctl = (volatile unsigned short *)(rtc_from4_fio_base + RTC_FROM4_RS_ECC_CTL);
-	unsigned short status;
-
-	switch (mode) {
-	case NAND_ECC_READ:
-		status = RTC_FROM4_RS_ECC_CTL_CLR | RTC_FROM4_RS_ECC_CTL_FD_E;
-
-		*rs_ecc_ctl = status;
-		break;
-
-	case NAND_ECC_READSYN:
-		status = 0x00;
-
-		*rs_ecc_ctl = status;
-		break;
-
-	case NAND_ECC_WRITE:
-		status = RTC_FROM4_RS_ECC_CTL_CLR | RTC_FROM4_RS_ECC_CTL_GEN | RTC_FROM4_RS_ECC_CTL_FD_E;
-
-		*rs_ecc_ctl = status;
-		break;
-
-	default:
-		BUG();
-		break;
-	}
-
-}
-
-/*
- * rtc_from4_calculate_ecc - hardware specific code to read ECC code
- * @mtd:	MTD device structure
- * @dat:	buffer containing the data to generate ECC codes
- * @ecc_code	ECC codes calculated
- *
- * The ECC code is calculated by the FPGA.  All we have to do is read the values
- * from the FPGA registers.
- *
- * Note: We read from the inverted registers, since data is inverted before
- * the code is calculated. So all 0xff data (blank page) results in all 0xff rs code
- *
- */
-static void rtc_from4_calculate_ecc(struct mtd_info *mtd, const u_char *dat, u_char *ecc_code)
-{
-	volatile unsigned short *rs_eccn = (volatile unsigned short *)(rtc_from4_fio_base + RTC_FROM4_RS_ECCN);
-	unsigned short value;
-	int i;
-
-	for (i = 0; i < 8; i++) {
-		value = *rs_eccn;
-		ecc_code[i] = (unsigned char)value;
-		rs_eccn++;
-	}
-	ecc_code[7] |= 0x0f;	/* set the last four bits (not used) */
-}
-
-/*
- * rtc_from4_correct_data - hardware specific code to correct data using ECC code
- * @mtd:	MTD device structure
- * @buf:	buffer containing the data to generate ECC codes
- * @ecc1	ECC codes read
- * @ecc2	ECC codes calculated
- *
- * The FPGA tells us fast, if there's an error or not. If no, we go back happy
- * else we read the ecc results from the fpga and call the rs library to decode
- * and hopefully correct the error.
- *
- */
-static int rtc_from4_correct_data(struct mtd_info *mtd, const u_char *buf, u_char *ecc1, u_char *ecc2)
-{
-	int i, j, res;
-	unsigned short status;
-	uint16_t par[6], syn[6];
-	uint8_t ecc[8];
-	volatile unsigned short *rs_ecc;
-
-	status = *((volatile unsigned short *)(rtc_from4_fio_base + RTC_FROM4_RS_ECC_CHK));
-
-	if (!(status & RTC_FROM4_RS_ECC_CHK_ERROR)) {
-		return 0;
-	}
-
-	/* Read the syndrome pattern from the FPGA and correct the bitorder */
-	rs_ecc = (volatile unsigned short *)(rtc_from4_fio_base + RTC_FROM4_RS_ECC);
-	for (i = 0; i < 8; i++) {
-		ecc[i] = bitrev8(*rs_ecc);
-		rs_ecc++;
-	}
-
-	/* convert into 6 10bit syndrome fields */
-	par[5] = rs_decoder->index_of[(((uint16_t) ecc[0] >> 0) & 0x0ff) | (((uint16_t) ecc[1] << 8) & 0x300)];
-	par[4] = rs_decoder->index_of[(((uint16_t) ecc[1] >> 2) & 0x03f) | (((uint16_t) ecc[2] << 6) & 0x3c0)];
-	par[3] = rs_decoder->index_of[(((uint16_t) ecc[2] >> 4) & 0x00f) | (((uint16_t) ecc[3] << 4) & 0x3f0)];
-	par[2] = rs_decoder->index_of[(((uint16_t) ecc[3] >> 6) & 0x003) | (((uint16_t) ecc[4] << 2) & 0x3fc)];
-	par[1] = rs_decoder->index_of[(((uint16_t) ecc[5] >> 0) & 0x0ff) | (((uint16_t) ecc[6] << 8) & 0x300)];
-	par[0] = (((uint16_t) ecc[6] >> 2) & 0x03f) | (((uint16_t) ecc[7] << 6) & 0x3c0);
-
-	/* Convert to computable syndrome */
-	for (i = 0; i < 6; i++) {
-		syn[i] = par[0];
-		for (j = 1; j < 6; j++)
-			if (par[j] != rs_decoder->nn)
-				syn[i] ^= rs_decoder->alpha_to[rs_modnn(rs_decoder, par[j] + i * j)];
-
-		/* Convert to index form */
-		syn[i] = rs_decoder->index_of[syn[i]];
-	}
-
-	/* Let the library code do its magic. */
-	res = decode_rs8(rs_decoder, (uint8_t *) buf, par, 512, syn, 0, NULL, 0xff, NULL);
-	if (res > 0) {
-		pr_debug("rtc_from4_correct_data: " "ECC corrected %d errors on read\n", res);
-	}
-	return res;
-}
-
-/**
- * rtc_from4_errstat - perform additional error status checks
- * @mtd:	MTD device structure
- * @this:	NAND chip structure
- * @state:	state or the operation
- * @status:	status code returned from read status
- * @page:	startpage inside the chip, must be called with (page & this->pagemask)
- *
- * Perform additional error status checks on erase and write failures
- * to determine if errors are correctable.  For this device, correctable
- * 1-bit errors on erase and write are considered acceptable.
- *
- * note: see pages 34..37 of data sheet for details.
- *
- */
-static int rtc_from4_errstat(struct mtd_info *mtd, struct nand_chip *this,
-			     int state, int status, int page)
-{
-	int er_stat = 0;
-	int rtn, retlen;
-	size_t len;
-	uint8_t *buf;
-	int i;
-
-	this->cmdfunc(mtd, NAND_CMD_STATUS_CLEAR, -1, -1);
-
-	if (state == FL_ERASING) {
-
-		for (i = 0; i < 4; i++) {
-			if (!(status & 1 << (i + 1)))
-				continue;
-			this->cmdfunc(mtd, (NAND_CMD_STATUS_ERROR + i + 1),
-				      -1, -1);
-			rtn = this->read_byte(mtd);
-			this->cmdfunc(mtd, NAND_CMD_STATUS_RESET, -1, -1);
-
-			/* err_ecc_not_avail */
-			if (!(rtn & ERR_STAT_ECC_AVAILABLE))
-				er_stat |= 1 << (i + 1);
-		}
-
-	} else if (state == FL_WRITING) {
-
-		unsigned long corrected = mtd->ecc_stats.corrected;
-
-		/* single bank write logic */
-		this->cmdfunc(mtd, NAND_CMD_STATUS_ERROR, -1, -1);
-		rtn = this->read_byte(mtd);
-		this->cmdfunc(mtd, NAND_CMD_STATUS_RESET, -1, -1);
-
-		if (!(rtn & ERR_STAT_ECC_AVAILABLE)) {
-			/* err_ecc_not_avail */
-			er_stat |= 1 << 1;
-			goto out;
-		}
-
-		len = mtd->writesize;
-		buf = kmalloc(len, GFP_KERNEL);
-		if (!buf) {
-			er_stat = 1;
-			goto out;
-		}
-
-		/* recovery read */
-		rtn = nand_do_read(mtd, page, len, &retlen, buf);
-
-		/* if read failed or > 1-bit error corrected */
-		if (rtn || (mtd->ecc_stats.corrected - corrected) > 1)
-			er_stat |= 1 << 1;
-		kfree(buf);
-	}
-out:
-	rtn = status;
-	if (er_stat == 0) {	/* if ECC is available   */
-		rtn = (status & ~NAND_STATUS_FAIL);	/*   clear the error bit */
-	}
-
-	return rtn;
-}
-#endif
-
-/*
- * Main initialization routine
- */
-static int __init rtc_from4_init(void)
-{
-	struct nand_chip *this;
-	unsigned short bcr1, bcr2, wcr2;
-	int i;
-	int ret;
-
-	/* Allocate memory for MTD device structure and private data */
-	rtc_from4_mtd = kmalloc(sizeof(struct mtd_info) + sizeof(struct nand_chip), GFP_KERNEL);
-	if (!rtc_from4_mtd) {
-		printk("Unable to allocate Renesas NAND MTD device structure.\n");
-		return -ENOMEM;
-	}
-
-	/* Get pointer to private data */
-	this = (struct nand_chip *)(&rtc_from4_mtd[1]);
-
-	/* Initialize structures */
-	memset(rtc_from4_mtd, 0, sizeof(struct mtd_info));
-	memset(this, 0, sizeof(struct nand_chip));
-
-	/* Link the private data with the MTD structure */
-	rtc_from4_mtd->priv = this;
-	rtc_from4_mtd->owner = THIS_MODULE;
-
-	/* set area 5 as PCMCIA mode to clear the spec of tDH(Data hold time;9ns min) */
-	bcr1 = *SH77X9_BCR1 & ~0x0002;
-	bcr1 |= 0x0002;
-	*SH77X9_BCR1 = bcr1;
-
-	/* set */
-	bcr2 = *SH77X9_BCR2 & ~0x0c00;
-	bcr2 |= 0x0800;
-	*SH77X9_BCR2 = bcr2;
-
-	/* set area 5 wait states */
-	wcr2 = *SH77X9_WCR2 & ~0x1c00;
-	wcr2 |= 0x1c00;
-	*SH77X9_WCR2 = wcr2;
-
-	/* Set address of NAND IO lines */
-	this->IO_ADDR_R = rtc_from4_fio_base;
-	this->IO_ADDR_W = rtc_from4_fio_base;
-	/* Set address of hardware control function */
-	this->cmd_ctrl = rtc_from4_hwcontrol;
-	/* Set address of chip select function */
-	this->select_chip = rtc_from4_nand_select_chip;
-	/* command delay time (in us) */
-	this->chip_delay = 100;
-	/* return the status of the Ready/Busy line */
-	this->dev_ready = rtc_from4_nand_device_ready;
-
-#ifdef RTC_FROM4_HWECC
-	printk(KERN_INFO "rtc_from4_init: using hardware ECC detection.\n");
-
-	this->ecc.mode = NAND_ECC_HW_SYNDROME;
-	this->ecc.size = 512;
-	this->ecc.bytes = 8;
-	this->ecc.strength = 3;
-	/* return the status of extra status and ECC checks */
-	this->errstat = rtc_from4_errstat;
-	/* set the nand_oobinfo to support FPGA H/W error detection */
-	this->ecc.layout = &rtc_from4_nand_oobinfo;
-	this->ecc.hwctl = rtc_from4_enable_hwecc;
-	this->ecc.calculate = rtc_from4_calculate_ecc;
-	this->ecc.correct = rtc_from4_correct_data;
-
-	/* We could create the decoder on demand, if memory is a concern.
-	 * This way we have it handy, if an error happens
-	 *
-	 * Symbolsize is 10 (bits)
-	 * Primitve polynomial is x^10+x^3+1
-	 * first consecutive root is 0
-	 * primitve element to generate roots = 1
-	 * generator polinomial degree = 6
-	 */
-	rs_decoder = init_rs(10, 0x409, 0, 1, 6);
-	if (!rs_decoder) {
-		printk(KERN_ERR "Could not create a RS decoder\n");
-		ret = -ENOMEM;
-		goto err_1;
-	}
-#else
-	printk(KERN_INFO "rtc_from4_init: using software ECC detection.\n");
-
-	this->ecc.mode = NAND_ECC_SOFT;
-#endif
-
-	/* set the bad block tables to support debugging */
-	this->bbt_td = &rtc_from4_bbt_main_descr;
-	this->bbt_md = &rtc_from4_bbt_mirror_descr;
-
-	/* Scan to find existence of the device */
-	if (nand_scan(rtc_from4_mtd, RTC_FROM4_MAX_CHIPS)) {
-		ret = -ENXIO;
-		goto err_2;
-	}
-
-	/* Perform 'device recovery' for each chip in case there was a power loss. */
-	for (i = 0; i < this->numchips; i++) {
-		deplete(rtc_from4_mtd, i);
-	}
-
-#if RTC_FROM4_NO_VIRTBLOCKS
-	/* use a smaller erase block to minimize wasted space when a block is bad */
-	/* note: this uses eight times as much RAM as using the default and makes */
-	/*       mounts take four times as long. */
-	rtc_from4_mtd->flags |= MTD_NO_VIRTBLOCKS;
-#endif
-
-	/* Register the partitions */
-	ret = mtd_device_register(rtc_from4_mtd, partition_info,
-				  NUM_PARTITIONS);
-	if (ret)
-		goto err_3;
-
-	/* Return happy */
-	return 0;
-err_3:
-	nand_release(rtc_from4_mtd);
-err_2:
-	free_rs(rs_decoder);
-err_1:
-	kfree(rtc_from4_mtd);
-	return ret;
-}
-
-module_init(rtc_from4_init);
-
-/*
- * Clean up routine
- */
-static void __exit rtc_from4_cleanup(void)
-{
-	/* Release resource, unregister partitions */
-	nand_release(rtc_from4_mtd);
-
-	/* Free the MTD device structure */
-	kfree(rtc_from4_mtd);
-
-#ifdef RTC_FROM4_HWECC
-	/* Free the reed solomon resources */
-	if (rs_decoder) {
-		free_rs(rs_decoder);
-	}
-#endif
-}
-
-module_exit(rtc_from4_cleanup);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("d.marlin <dmarlin@redhat.com");
-MODULE_DESCRIPTION("Board-specific glue layer for AG-AND flash on Renesas FROM_BOARD4");
diff --git a/drivers/mtd/nand/sh_flctl.c b/drivers/mtd/nand/sh_flctl.c
index 57b3971c9c0a..e57e18e8c289 100644
--- a/drivers/mtd/nand/sh_flctl.c
+++ b/drivers/mtd/nand/sh_flctl.c
@@ -1081,7 +1081,6 @@ static struct sh_flctl_platform_data *flctl_parse_dt(struct device *dev)
 	return pdata;
 }
 #else /* CONFIG_OF */
-#define of_flctl_match NULL
 static struct sh_flctl_platform_data *flctl_parse_dt(struct device *dev)
 {
 	return NULL;
@@ -1219,22 +1218,11 @@ static struct platform_driver flctl_driver = {
 	.driver = {
 		.name	= "sh_flctl",
 		.owner	= THIS_MODULE,
-		.of_match_table = of_flctl_match,
+		.of_match_table = of_match_ptr(of_flctl_match),
 	},
 };
 
-static int __init flctl_nand_init(void)
-{
-	return platform_driver_probe(&flctl_driver, flctl_probe);
-}
-
-static void __exit flctl_nand_cleanup(void)
-{
-	platform_driver_unregister(&flctl_driver);
-}
-
-module_init(flctl_nand_init);
-module_exit(flctl_nand_cleanup);
+module_platform_driver_probe(flctl_driver, flctl_probe);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Yoshihiro Shimoda");
diff --git a/drivers/mtd/nand/sm_common.c b/drivers/mtd/nand/sm_common.c
index 082bcdcd6bcf..e8181edebddd 100644
--- a/drivers/mtd/nand/sm_common.c
+++ b/drivers/mtd/nand/sm_common.c
@@ -9,6 +9,7 @@
 #include <linux/kernel.h>
 #include <linux/mtd/nand.h>
 #include <linux/module.h>
+#include <linux/sizes.h>
 #include "sm_common.h"
 
 static struct nand_ecclayout nand_oob_sm = {
@@ -67,44 +68,37 @@ static int sm_block_markbad(struct mtd_info *mtd, loff_t ofs)
 	return error;
 }
 
-
 static struct nand_flash_dev nand_smartmedia_flash_ids[] = {
-	{"SmartMedia 1MiB 5V",          0x6e, 256, 1, 0x1000, 0},
-	{"SmartMedia 1MiB 3,3V",        0xe8, 256, 1, 0x1000, 0},
-	{"SmartMedia 1MiB 3,3V",        0xec, 256, 1, 0x1000, 0},
-	{"SmartMedia 2MiB 3,3V",        0xea, 256, 2, 0x1000, 0},
-	{"SmartMedia 2MiB 5V",          0x64, 256, 2, 0x1000, 0},
-	{"SmartMedia 2MiB 3,3V ROM",    0x5d, 512, 2, 0x2000, NAND_ROM},
-	{"SmartMedia 4MiB 3,3V",        0xe3, 512, 4, 0x2000, 0},
-	{"SmartMedia 4MiB 3,3/5V",      0xe5, 512, 4, 0x2000, 0},
-	{"SmartMedia 4MiB 5V",          0x6b, 512, 4, 0x2000, 0},
-	{"SmartMedia 4MiB 3,3V ROM",    0xd5, 512, 4, 0x2000, NAND_ROM},
-	{"SmartMedia 8MiB 3,3V",        0xe6, 512, 8, 0x2000, 0},
-	{"SmartMedia 8MiB 3,3V ROM",    0xd6, 512, 8, 0x2000, NAND_ROM},
-	{"SmartMedia 16MiB 3,3V",       0x73, 512, 16, 0x4000, 0},
-	{"SmartMedia 16MiB 3,3V ROM",   0x57, 512, 16, 0x4000, NAND_ROM},
-	{"SmartMedia 32MiB 3,3V",       0x75, 512, 32, 0x4000, 0},
-	{"SmartMedia 32MiB 3,3V ROM",   0x58, 512, 32, 0x4000, NAND_ROM},
-	{"SmartMedia 64MiB 3,3V",       0x76, 512, 64, 0x4000, 0},
-	{"SmartMedia 64MiB 3,3V ROM",   0xd9, 512, 64, 0x4000, NAND_ROM},
-	{"SmartMedia 128MiB 3,3V",      0x79, 512, 128, 0x4000, 0},
-	{"SmartMedia 128MiB 3,3V ROM",  0xda, 512, 128, 0x4000, NAND_ROM},
-	{"SmartMedia 256MiB 3,3V",      0x71, 512, 256, 0x4000 },
-	{"SmartMedia 256MiB 3,3V ROM",  0x5b, 512, 256, 0x4000, NAND_ROM},
-	{NULL,}
+	LEGACY_ID_NAND("SmartMedia 2MiB 3,3V ROM",   0x5d, 2,   SZ_8K, NAND_ROM),
+	LEGACY_ID_NAND("SmartMedia 4MiB 3,3V",       0xe3, 4,   SZ_8K, 0),
+	LEGACY_ID_NAND("SmartMedia 4MiB 3,3/5V",     0xe5, 4,   SZ_8K, 0),
+	LEGACY_ID_NAND("SmartMedia 4MiB 5V",         0x6b, 4,   SZ_8K, 0),
+	LEGACY_ID_NAND("SmartMedia 4MiB 3,3V ROM",   0xd5, 4,   SZ_8K, NAND_ROM),
+	LEGACY_ID_NAND("SmartMedia 8MiB 3,3V",       0xe6, 8,   SZ_8K, 0),
+	LEGACY_ID_NAND("SmartMedia 8MiB 3,3V ROM",   0xd6, 8,   SZ_8K, NAND_ROM),
+	LEGACY_ID_NAND("SmartMedia 16MiB 3,3V",      0x73, 16,  SZ_16K, 0),
+	LEGACY_ID_NAND("SmartMedia 16MiB 3,3V ROM",  0x57, 16,  SZ_16K, NAND_ROM),
+	LEGACY_ID_NAND("SmartMedia 32MiB 3,3V",      0x75, 32,  SZ_16K, 0),
+	LEGACY_ID_NAND("SmartMedia 32MiB 3,3V ROM",  0x58, 32,  SZ_16K, NAND_ROM),
+	LEGACY_ID_NAND("SmartMedia 64MiB 3,3V",      0x76, 64,  SZ_16K, 0),
+	LEGACY_ID_NAND("SmartMedia 64MiB 3,3V ROM",  0xd9, 64,  SZ_16K, NAND_ROM),
+	LEGACY_ID_NAND("SmartMedia 128MiB 3,3V",     0x79, 128, SZ_16K, 0),
+	LEGACY_ID_NAND("SmartMedia 128MiB 3,3V ROM", 0xda, 128, SZ_16K, NAND_ROM),
+	LEGACY_ID_NAND("SmartMedia 256MiB 3, 3V",    0x71, 256, SZ_16K, 0),
+	LEGACY_ID_NAND("SmartMedia 256MiB 3,3V ROM", 0x5b, 256, SZ_16K, NAND_ROM),
+	{NULL}
 };
 
 static struct nand_flash_dev nand_xd_flash_ids[] = {
-
-	{"xD 16MiB 3,3V",    0x73, 512, 16, 0x4000, 0},
-	{"xD 32MiB 3,3V",    0x75, 512, 32, 0x4000, 0},
-	{"xD 64MiB 3,3V",    0x76, 512, 64, 0x4000, 0},
-	{"xD 128MiB 3,3V",   0x79, 512, 128, 0x4000, 0},
-	{"xD 256MiB 3,3V",   0x71, 512, 256, 0x4000, NAND_BROKEN_XD},
-	{"xD 512MiB 3,3V",   0xdc, 512, 512, 0x4000, NAND_BROKEN_XD},
-	{"xD 1GiB 3,3V",     0xd3, 512, 1024, 0x4000, NAND_BROKEN_XD},
-	{"xD 2GiB 3,3V",     0xd5, 512, 2048, 0x4000, NAND_BROKEN_XD},
-	{NULL,}
+	LEGACY_ID_NAND("xD 16MiB 3,3V",  0x73, 16,   SZ_16K, 0),
+	LEGACY_ID_NAND("xD 32MiB 3,3V",  0x75, 32,   SZ_16K, 0),
+	LEGACY_ID_NAND("xD 64MiB 3,3V",  0x76, 64,   SZ_16K, 0),
+	LEGACY_ID_NAND("xD 128MiB 3,3V", 0x79, 128,  SZ_16K, 0),
+	LEGACY_ID_NAND("xD 256MiB 3,3V", 0x71, 256,  SZ_16K, NAND_BROKEN_XD),
+	LEGACY_ID_NAND("xD 512MiB 3,3V", 0xdc, 512,  SZ_16K, NAND_BROKEN_XD),
+	LEGACY_ID_NAND("xD 1GiB 3,3V",   0xd3, 1024, SZ_16K, NAND_BROKEN_XD),
+	LEGACY_ID_NAND("xD 2GiB 3,3V",   0xd5, 2048, SZ_16K, NAND_BROKEN_XD),
+	{NULL}
 };
 
 int sm_register_device(struct mtd_info *mtd, int smartmedia)
diff --git a/drivers/mtd/nand/txx9ndfmc.c b/drivers/mtd/nand/txx9ndfmc.c
index e1e8748aa47b..7ed654c68b08 100644
--- a/drivers/mtd/nand/txx9ndfmc.c
+++ b/drivers/mtd/nand/txx9ndfmc.c
@@ -427,18 +427,7 @@ static struct platform_driver txx9ndfmc_driver = {
 	},
 };
 
-static int __init txx9ndfmc_init(void)
-{
-	return platform_driver_probe(&txx9ndfmc_driver, txx9ndfmc_probe);
-}
-
-static void __exit txx9ndfmc_exit(void)
-{
-	platform_driver_unregister(&txx9ndfmc_driver);
-}
-
-module_init(txx9ndfmc_init);
-module_exit(txx9ndfmc_exit);
+module_platform_driver_probe(txx9ndfmc_driver, txx9ndfmc_probe);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("TXx9 SoC NAND flash controller driver");
diff --git a/drivers/mtd/ofpart.c b/drivers/mtd/ofpart.c
index 30bd907a260a..553d6d6d5603 100644
--- a/drivers/mtd/ofpart.c
+++ b/drivers/mtd/ofpart.c
@@ -55,6 +55,7 @@ static int parse_ofpart_partitions(struct mtd_info *master,
 	while ((pp = of_get_next_child(node, pp))) {
 		const __be32 *reg;
 		int len;
+		int a_cells, s_cells;
 
 		reg = of_get_property(pp, "reg", &len);
 		if (!reg) {
@@ -62,8 +63,10 @@ static int parse_ofpart_partitions(struct mtd_info *master,
 			continue;
 		}
 
-		(*pparts)[i].offset = be32_to_cpu(reg[0]);
-		(*pparts)[i].size = be32_to_cpu(reg[1]);
+		a_cells = of_n_addr_cells(pp);
+		s_cells = of_n_size_cells(pp);
+		(*pparts)[i].offset = of_read_number(reg, a_cells);
+		(*pparts)[i].size = of_read_number(reg + a_cells, s_cells);
 
 		partname = of_get_property(pp, "label", &len);
 		if (!partname)
diff --git a/drivers/mtd/onenand/Kconfig b/drivers/mtd/onenand/Kconfig
index 91467bb03634..ab2607273e80 100644
--- a/drivers/mtd/onenand/Kconfig
+++ b/drivers/mtd/onenand/Kconfig
@@ -40,7 +40,6 @@ config MTD_ONENAND_SAMSUNG
 
 config MTD_ONENAND_OTP
 	bool "OneNAND OTP Support"
-	select HAVE_MTD_OTP
 	help
 	  One Block of the NAND Flash Array memory is reserved as
 	  a One-Time Programmable Block memory area.
@@ -68,10 +67,4 @@ config MTD_ONENAND_2X_PROGRAM
 
 	  And more recent chips
 
-config MTD_ONENAND_SIM
-	tristate "OneNAND simulator support"
-	help
-	  The simulator may simulate various OneNAND flash chips for the
-	  OneNAND MTD layer.
-
 endif # MTD_ONENAND
diff --git a/drivers/mtd/onenand/Makefile b/drivers/mtd/onenand/Makefile
index 2b7884c7577e..9d6540e8b3d2 100644
--- a/drivers/mtd/onenand/Makefile
+++ b/drivers/mtd/onenand/Makefile
@@ -10,7 +10,4 @@ obj-$(CONFIG_MTD_ONENAND_GENERIC)	+= generic.o
 obj-$(CONFIG_MTD_ONENAND_OMAP2)		+= omap2.o
 obj-$(CONFIG_MTD_ONENAND_SAMSUNG)       += samsung.o
 
-# Simulator
-obj-$(CONFIG_MTD_ONENAND_SIM)		+= onenand_sim.o
-
 onenand-objs = onenand_base.o onenand_bbt.o
diff --git a/drivers/mtd/onenand/omap2.c b/drivers/mtd/onenand/omap2.c
index eec2aedb4ab8..d98b198edd53 100644
--- a/drivers/mtd/onenand/omap2.c
+++ b/drivers/mtd/onenand/omap2.c
@@ -832,19 +832,7 @@ static struct platform_driver omap2_onenand_driver = {
 	},
 };
 
-static int __init omap2_onenand_init(void)
-{
-	printk(KERN_INFO "OneNAND driver initializing\n");
-	return platform_driver_register(&omap2_onenand_driver);
-}
-
-static void __exit omap2_onenand_exit(void)
-{
-	platform_driver_unregister(&omap2_onenand_driver);
-}
-
-module_init(omap2_onenand_init);
-module_exit(omap2_onenand_exit);
+module_platform_driver(omap2_onenand_driver);
 
 MODULE_ALIAS("platform:" DRIVER_NAME);
 MODULE_LICENSE("GPL");
diff --git a/drivers/mtd/onenand/onenand_sim.c b/drivers/mtd/onenand/onenand_sim.c
deleted file mode 100644
index 85399e3accda..000000000000
--- a/drivers/mtd/onenand/onenand_sim.c
+++ /dev/null
@@ -1,564 +0,0 @@
-/*
- *  linux/drivers/mtd/onenand/onenand_sim.c
- *
- *  The OneNAND simulator
- *
- *  Copyright © 2005-2007 Samsung Electronics
- *  Kyungmin Park <kyungmin.park@samsung.com>
- *
- *  Vishak G <vishak.g at samsung.com>, Rohit Hagargundgi <h.rohit at samsung.com>
- *  Flex-OneNAND simulator support
- *  Copyright (C) Samsung Electronics, 2008
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/vmalloc.h>
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/partitions.h>
-#include <linux/mtd/onenand.h>
-
-#include <linux/io.h>
-
-#ifndef CONFIG_ONENAND_SIM_MANUFACTURER
-#define CONFIG_ONENAND_SIM_MANUFACTURER         0xec
-#endif
-
-#ifndef CONFIG_ONENAND_SIM_DEVICE_ID
-#define CONFIG_ONENAND_SIM_DEVICE_ID            0x04
-#endif
-
-#define CONFIG_FLEXONENAND ((CONFIG_ONENAND_SIM_DEVICE_ID >> 9) & 1)
-
-#ifndef CONFIG_ONENAND_SIM_VERSION_ID
-#define CONFIG_ONENAND_SIM_VERSION_ID           0x1e
-#endif
-
-#ifndef CONFIG_ONENAND_SIM_TECHNOLOGY_ID
-#define CONFIG_ONENAND_SIM_TECHNOLOGY_ID CONFIG_FLEXONENAND
-#endif
-
-/* Initial boundary values for Flex-OneNAND Simulator */
-#ifndef CONFIG_FLEXONENAND_SIM_DIE0_BOUNDARY
-#define CONFIG_FLEXONENAND_SIM_DIE0_BOUNDARY	0x01
-#endif
-
-#ifndef CONFIG_FLEXONENAND_SIM_DIE1_BOUNDARY
-#define CONFIG_FLEXONENAND_SIM_DIE1_BOUNDARY	0x01
-#endif
-
-static int manuf_id	= CONFIG_ONENAND_SIM_MANUFACTURER;
-static int device_id	= CONFIG_ONENAND_SIM_DEVICE_ID;
-static int version_id	= CONFIG_ONENAND_SIM_VERSION_ID;
-static int technology_id = CONFIG_ONENAND_SIM_TECHNOLOGY_ID;
-static int boundary[] = {
-	CONFIG_FLEXONENAND_SIM_DIE0_BOUNDARY,
-	CONFIG_FLEXONENAND_SIM_DIE1_BOUNDARY,
-};
-
-struct onenand_flash {
-	void __iomem *base;
-	void __iomem *data;
-};
-
-#define ONENAND_CORE(flash)		(flash->data)
-#define ONENAND_CORE_SPARE(flash, this, offset)				\
-	((flash->data) + (this->chipsize) + (offset >> 5))
-
-#define ONENAND_MAIN_AREA(this, offset)					\
-	(this->base + ONENAND_DATARAM + offset)
-
-#define ONENAND_SPARE_AREA(this, offset)				\
-	(this->base + ONENAND_SPARERAM + offset)
-
-#define ONENAND_GET_WP_STATUS(this)					\
-	(readw(this->base + ONENAND_REG_WP_STATUS))
-
-#define ONENAND_SET_WP_STATUS(v, this)					\
-	(writew(v, this->base + ONENAND_REG_WP_STATUS))
-
-/* It has all 0xff chars */
-#define MAX_ONENAND_PAGESIZE		(4096 + 128)
-static unsigned char *ffchars;
-
-#if CONFIG_FLEXONENAND
-#define PARTITION_NAME "Flex-OneNAND simulator partition"
-#else
-#define PARTITION_NAME "OneNAND simulator partition"
-#endif
-
-static struct mtd_partition os_partitions[] = {
-	{
-		.name		= PARTITION_NAME,
-		.offset		= 0,
-		.size		= MTDPART_SIZ_FULL,
-	},
-};
-
-/*
- * OneNAND simulator mtd
- */
-struct onenand_info {
-	struct mtd_info		mtd;
-	struct mtd_partition	*parts;
-	struct onenand_chip	onenand;
-	struct onenand_flash	flash;
-};
-
-static struct onenand_info *info;
-
-#define DPRINTK(format, args...)					\
-do {									\
-	printk(KERN_DEBUG "%s[%d]: " format "\n", __func__,		\
-			   __LINE__, ##args);				\
-} while (0)
-
-/**
- * onenand_lock_handle - Handle Lock scheme
- * @this:		OneNAND device structure
- * @cmd:		The command to be sent
- *
- * Send lock command to OneNAND device.
- * The lock scheme depends on chip type.
- */
-static void onenand_lock_handle(struct onenand_chip *this, int cmd)
-{
-	int block_lock_scheme;
-	int status;
-
-	status = ONENAND_GET_WP_STATUS(this);
-	block_lock_scheme = !(this->options & ONENAND_HAS_CONT_LOCK);
-
-	switch (cmd) {
-	case ONENAND_CMD_UNLOCK:
-	case ONENAND_CMD_UNLOCK_ALL:
-		if (block_lock_scheme)
-			ONENAND_SET_WP_STATUS(ONENAND_WP_US, this);
-		else
-			ONENAND_SET_WP_STATUS(status | ONENAND_WP_US, this);
-		break;
-
-	case ONENAND_CMD_LOCK:
-		if (block_lock_scheme)
-			ONENAND_SET_WP_STATUS(ONENAND_WP_LS, this);
-		else
-			ONENAND_SET_WP_STATUS(status | ONENAND_WP_LS, this);
-		break;
-
-	case ONENAND_CMD_LOCK_TIGHT:
-		if (block_lock_scheme)
-			ONENAND_SET_WP_STATUS(ONENAND_WP_LTS, this);
-		else
-			ONENAND_SET_WP_STATUS(status | ONENAND_WP_LTS, this);
-		break;
-
-	default:
-		break;
-	}
-}
-
-/**
- * onenand_bootram_handle - Handle BootRAM area
- * @this:		OneNAND device structure
- * @cmd:		The command to be sent
- *
- * Emulate BootRAM area. It is possible to do basic operation using BootRAM.
- */
-static void onenand_bootram_handle(struct onenand_chip *this, int cmd)
-{
-	switch (cmd) {
-	case ONENAND_CMD_READID:
-		writew(manuf_id, this->base);
-		writew(device_id, this->base + 2);
-		writew(version_id, this->base + 4);
-		break;
-
-	default:
-		/* REVIST: Handle other commands */
-		break;
-	}
-}
-
-/**
- * onenand_update_interrupt - Set interrupt register
- * @this:         OneNAND device structure
- * @cmd:          The command to be sent
- *
- * Update interrupt register. The status depends on command.
- */
-static void onenand_update_interrupt(struct onenand_chip *this, int cmd)
-{
-	int interrupt = ONENAND_INT_MASTER;
-
-	switch (cmd) {
-	case ONENAND_CMD_READ:
-	case ONENAND_CMD_READOOB:
-		interrupt |= ONENAND_INT_READ;
-		break;
-
-	case ONENAND_CMD_PROG:
-	case ONENAND_CMD_PROGOOB:
-		interrupt |= ONENAND_INT_WRITE;
-		break;
-
-	case ONENAND_CMD_ERASE:
-		interrupt |= ONENAND_INT_ERASE;
-		break;
-
-	case ONENAND_CMD_RESET:
-		interrupt |= ONENAND_INT_RESET;
-		break;
-
-	default:
-		break;
-	}
-
-	writew(interrupt, this->base + ONENAND_REG_INTERRUPT);
-}
-
-/**
- * onenand_check_overwrite - Check if over-write happened
- * @dest:		The destination pointer
- * @src:		The source pointer
- * @count:		The length to be check
- *
- * Returns:		0 on same, otherwise 1
- *
- * Compare the source with destination
- */
-static int onenand_check_overwrite(void *dest, void *src, size_t count)
-{
-	unsigned int *s = (unsigned int *) src;
-	unsigned int *d = (unsigned int *) dest;
-	int i;
-
-	count >>= 2;
-	for (i = 0; i < count; i++)
-		if ((*s++ ^ *d++) != 0)
-			return 1;
-
-	return 0;
-}
-
-/**
- * onenand_data_handle - Handle OneNAND Core and DataRAM
- * @this:		OneNAND device structure
- * @cmd:		The command to be sent
- * @dataram:		Which dataram used
- * @offset:		The offset to OneNAND Core
- *
- * Copy data from OneNAND Core to DataRAM (read)
- * Copy data from DataRAM to OneNAND Core (write)
- * Erase the OneNAND Core (erase)
- */
-static void onenand_data_handle(struct onenand_chip *this, int cmd,
-				int dataram, unsigned int offset)
-{
-	struct mtd_info *mtd = &info->mtd;
-	struct onenand_flash *flash = this->priv;
-	int main_offset, spare_offset, die = 0;
-	void __iomem *src;
-	void __iomem *dest;
-	unsigned int i;
-	static int pi_operation;
-	int erasesize, rgn;
-
-	if (dataram) {
-		main_offset = mtd->writesize;
-		spare_offset = mtd->oobsize;
-	} else {
-		main_offset = 0;
-		spare_offset = 0;
-	}
-
-	if (pi_operation) {
-		die = readw(this->base + ONENAND_REG_START_ADDRESS2);
-		die >>= ONENAND_DDP_SHIFT;
-	}
-
-	switch (cmd) {
-	case FLEXONENAND_CMD_PI_ACCESS:
-		pi_operation = 1;
-		break;
-
-	case ONENAND_CMD_RESET:
-		pi_operation = 0;
-		break;
-
-	case ONENAND_CMD_READ:
-		src = ONENAND_CORE(flash) + offset;
-		dest = ONENAND_MAIN_AREA(this, main_offset);
-		if (pi_operation) {
-			writew(boundary[die], this->base + ONENAND_DATARAM);
-			break;
-		}
-		memcpy(dest, src, mtd->writesize);
-		/* Fall through */
-
-	case ONENAND_CMD_READOOB:
-		src = ONENAND_CORE_SPARE(flash, this, offset);
-		dest = ONENAND_SPARE_AREA(this, spare_offset);
-		memcpy(dest, src, mtd->oobsize);
-		break;
-
-	case ONENAND_CMD_PROG:
-		src = ONENAND_MAIN_AREA(this, main_offset);
-		dest = ONENAND_CORE(flash) + offset;
-		if (pi_operation) {
-			boundary[die] = readw(this->base + ONENAND_DATARAM);
-			break;
-		}
-		/* To handle partial write */
-		for (i = 0; i < (1 << mtd->subpage_sft); i++) {
-			int off = i * this->subpagesize;
-			if (!memcmp(src + off, ffchars, this->subpagesize))
-				continue;
-			if (memcmp(dest + off, ffchars, this->subpagesize) &&
-			    onenand_check_overwrite(dest + off, src + off, this->subpagesize))
-				printk(KERN_ERR "over-write happened at 0x%08x\n", offset);
-			memcpy(dest + off, src + off, this->subpagesize);
-		}
-		/* Fall through */
-
-	case ONENAND_CMD_PROGOOB:
-		src = ONENAND_SPARE_AREA(this, spare_offset);
-		/* Check all data is 0xff chars */
-		if (!memcmp(src, ffchars, mtd->oobsize))
-			break;
-
-		dest = ONENAND_CORE_SPARE(flash, this, offset);
-		if (memcmp(dest, ffchars, mtd->oobsize) &&
-		    onenand_check_overwrite(dest, src, mtd->oobsize))
-			printk(KERN_ERR "OOB: over-write happened at 0x%08x\n",
-			       offset);
-		memcpy(dest, src, mtd->oobsize);
-		break;
-
-	case ONENAND_CMD_ERASE:
-		if (pi_operation)
-			break;
-
-		if (FLEXONENAND(this)) {
-			rgn = flexonenand_region(mtd, offset);
-			erasesize = mtd->eraseregions[rgn].erasesize;
-		} else
-			erasesize = mtd->erasesize;
-
-		memset(ONENAND_CORE(flash) + offset, 0xff, erasesize);
-		memset(ONENAND_CORE_SPARE(flash, this, offset), 0xff,
-		       (erasesize >> 5));
-		break;
-
-	default:
-		break;
-	}
-}
-
-/**
- * onenand_command_handle - Handle command
- * @this:		OneNAND device structure
- * @cmd:		The command to be sent
- *
- * Emulate OneNAND command.
- */
-static void onenand_command_handle(struct onenand_chip *this, int cmd)
-{
-	unsigned long offset = 0;
-	int block = -1, page = -1, bufferram = -1;
-	int dataram = 0;
-
-	switch (cmd) {
-	case ONENAND_CMD_UNLOCK:
-	case ONENAND_CMD_LOCK:
-	case ONENAND_CMD_LOCK_TIGHT:
-	case ONENAND_CMD_UNLOCK_ALL:
-		onenand_lock_handle(this, cmd);
-		break;
-
-	case ONENAND_CMD_BUFFERRAM:
-		/* Do nothing */
-		return;
-
-	default:
-		block = (int) readw(this->base + ONENAND_REG_START_ADDRESS1);
-		if (block & (1 << ONENAND_DDP_SHIFT)) {
-			block &= ~(1 << ONENAND_DDP_SHIFT);
-			/* The half of chip block */
-			block += this->chipsize >> (this->erase_shift + 1);
-		}
-		if (cmd == ONENAND_CMD_ERASE)
-			break;
-
-		page = (int) readw(this->base + ONENAND_REG_START_ADDRESS8);
-		page = (page >> ONENAND_FPA_SHIFT);
-		bufferram = (int) readw(this->base + ONENAND_REG_START_BUFFER);
-		bufferram >>= ONENAND_BSA_SHIFT;
-		bufferram &= ONENAND_BSA_DATARAM1;
-		dataram = (bufferram == ONENAND_BSA_DATARAM1) ? 1 : 0;
-		break;
-	}
-
-	if (block != -1)
-		offset = onenand_addr(this, block);
-
-	if (page != -1)
-		offset += page << this->page_shift;
-
-	onenand_data_handle(this, cmd, dataram, offset);
-
-	onenand_update_interrupt(this, cmd);
-}
-
-/**
- * onenand_writew - [OneNAND Interface] Emulate write operation
- * @value:		value to write
- * @addr:		address to write
- *
- * Write OneNAND register with value
- */
-static void onenand_writew(unsigned short value, void __iomem * addr)
-{
-	struct onenand_chip *this = info->mtd.priv;
-
-	/* BootRAM handling */
-	if (addr < this->base + ONENAND_DATARAM) {
-		onenand_bootram_handle(this, value);
-		return;
-	}
-	/* Command handling */
-	if (addr == this->base + ONENAND_REG_COMMAND)
-		onenand_command_handle(this, value);
-
-	writew(value, addr);
-}
-
-/**
- * flash_init - Initialize OneNAND simulator
- * @flash:		OneNAND simulator data strucutres
- *
- * Initialize OneNAND simulator.
- */
-static int __init flash_init(struct onenand_flash *flash)
-{
-	int density, size;
-	int buffer_size;
-
-	flash->base = kzalloc(131072, GFP_KERNEL);
-	if (!flash->base) {
-		printk(KERN_ERR "Unable to allocate base address.\n");
-		return -ENOMEM;
-	}
-
-	density = device_id >> ONENAND_DEVICE_DENSITY_SHIFT;
-	density &= ONENAND_DEVICE_DENSITY_MASK;
-	size = ((16 << 20) << density);
-
-	ONENAND_CORE(flash) = vmalloc(size + (size >> 5));
-	if (!ONENAND_CORE(flash)) {
-		printk(KERN_ERR "Unable to allocate nand core address.\n");
-		kfree(flash->base);
-		return -ENOMEM;
-	}
-
-	memset(ONENAND_CORE(flash), 0xff, size + (size >> 5));
-
-	/* Setup registers */
-	writew(manuf_id, flash->base + ONENAND_REG_MANUFACTURER_ID);
-	writew(device_id, flash->base + ONENAND_REG_DEVICE_ID);
-	writew(version_id, flash->base + ONENAND_REG_VERSION_ID);
-	writew(technology_id, flash->base + ONENAND_REG_TECHNOLOGY);
-
-	if (density < 2 && (!CONFIG_FLEXONENAND))
-		buffer_size = 0x0400;	/* 1KiB page */
-	else
-		buffer_size = 0x0800;	/* 2KiB page */
-	writew(buffer_size, flash->base + ONENAND_REG_DATA_BUFFER_SIZE);
-
-	return 0;
-}
-
-/**
- * flash_exit - Clean up OneNAND simulator
- * @flash:		OneNAND simulator data structures
- *
- * Clean up OneNAND simulator.
- */
-static void flash_exit(struct onenand_flash *flash)
-{
-	vfree(ONENAND_CORE(flash));
-	kfree(flash->base);
-}
-
-static int __init onenand_sim_init(void)
-{
-	/* Allocate all 0xff chars pointer */
-	ffchars = kmalloc(MAX_ONENAND_PAGESIZE, GFP_KERNEL);
-	if (!ffchars) {
-		printk(KERN_ERR "Unable to allocate ff chars.\n");
-		return -ENOMEM;
-	}
-	memset(ffchars, 0xff, MAX_ONENAND_PAGESIZE);
-
-	/* Allocate OneNAND simulator mtd pointer */
-	info = kzalloc(sizeof(struct onenand_info), GFP_KERNEL);
-	if (!info) {
-		printk(KERN_ERR "Unable to allocate core structures.\n");
-		kfree(ffchars);
-		return -ENOMEM;
-	}
-
-	/* Override write_word function */
-	info->onenand.write_word = onenand_writew;
-
-	if (flash_init(&info->flash)) {
-		printk(KERN_ERR "Unable to allocate flash.\n");
-		kfree(ffchars);
-		kfree(info);
-		return -ENOMEM;
-	}
-
-	info->parts = os_partitions;
-
-	info->onenand.base = info->flash.base;
-	info->onenand.priv = &info->flash;
-
-	info->mtd.name = "OneNAND simulator";
-	info->mtd.priv = &info->onenand;
-	info->mtd.owner = THIS_MODULE;
-
-	if (onenand_scan(&info->mtd, 1)) {
-		flash_exit(&info->flash);
-		kfree(ffchars);
-		kfree(info);
-		return -ENXIO;
-	}
-
-	mtd_device_register(&info->mtd, info->parts,
-			    ARRAY_SIZE(os_partitions));
-
-	return 0;
-}
-
-static void __exit onenand_sim_exit(void)
-{
-	struct onenand_chip *this = info->mtd.priv;
-	struct onenand_flash *flash = this->priv;
-
-	onenand_release(&info->mtd);
-	flash_exit(flash);
-	kfree(ffchars);
-	kfree(info);
-}
-
-module_init(onenand_sim_init);
-module_exit(onenand_sim_exit);
-
-MODULE_AUTHOR("Kyungmin Park <kyungmin.park@samsung.com>");
-MODULE_DESCRIPTION("The OneNAND flash simulator");
-MODULE_LICENSE("GPL");
diff --git a/drivers/mtd/onenand/samsung.c b/drivers/mtd/onenand/samsung.c
index 33f2a8fb8df9..2cf74085f935 100644
--- a/drivers/mtd/onenand/samsung.c
+++ b/drivers/mtd/onenand/samsung.c
@@ -23,11 +23,11 @@
 #include <linux/mtd/partitions.h>
 #include <linux/dma-mapping.h>
 #include <linux/interrupt.h>
+#include <linux/io.h>
 
 #include <asm/mach/flash.h>
-#include <plat/regs-onenand.h>
 
-#include <linux/io.h>
+#include "samsung.h"
 
 enum soc_type {
 	TYPE_S3C6400,
diff --git a/drivers/mtd/onenand/samsung.h b/drivers/mtd/onenand/samsung.h
new file mode 100644
index 000000000000..c4a80e67e438
--- /dev/null
+++ b/drivers/mtd/onenand/samsung.h
@@ -0,0 +1,61 @@
+/*
+ * linux/arch/arm/plat-s3c/include/plat/regs-onenand.h
+ *
+ *  Copyright (C) 2008-2010 Samsung Electronics
+ *  Kyungmin Park <kyungmin.park@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 __SAMSUNG_ONENAND_H__
+#define __SAMSUNG_ONENAND_H__
+
+/*
+ * OneNAND Controller
+ */
+#define MEM_CFG_OFFSET		0x0000
+#define BURST_LEN_OFFSET	0x0010
+#define MEM_RESET_OFFSET	0x0020
+#define INT_ERR_STAT_OFFSET	0x0030
+#define INT_ERR_MASK_OFFSET	0x0040
+#define INT_ERR_ACK_OFFSET	0x0050
+#define ECC_ERR_STAT_OFFSET	0x0060
+#define MANUFACT_ID_OFFSET	0x0070
+#define DEVICE_ID_OFFSET	0x0080
+#define DATA_BUF_SIZE_OFFSET	0x0090
+#define BOOT_BUF_SIZE_OFFSET	0x00A0
+#define BUF_AMOUNT_OFFSET	0x00B0
+#define TECH_OFFSET		0x00C0
+#define FBA_WIDTH_OFFSET	0x00D0
+#define FPA_WIDTH_OFFSET	0x00E0
+#define FSA_WIDTH_OFFSET	0x00F0
+#define TRANS_SPARE_OFFSET	0x0140
+#define DBS_DFS_WIDTH_OFFSET	0x0160
+#define INT_PIN_ENABLE_OFFSET	0x01A0
+#define ACC_CLOCK_OFFSET	0x01C0
+#define FLASH_VER_ID_OFFSET	0x01F0
+#define FLASH_AUX_CNTRL_OFFSET	0x0300		/* s3c64xx only */
+
+#define ONENAND_MEM_RESET_HOT	0x3
+#define ONENAND_MEM_RESET_COLD	0x2
+#define ONENAND_MEM_RESET_WARM	0x1
+
+#define CACHE_OP_ERR		(1 << 13)
+#define RST_CMP			(1 << 12)
+#define RDY_ACT			(1 << 11)
+#define INT_ACT			(1 << 10)
+#define UNSUP_CMD		(1 << 9)
+#define LOCKED_BLK		(1 << 8)
+#define BLK_RW_CMP		(1 << 7)
+#define ERS_CMP			(1 << 6)
+#define PGM_CMP			(1 << 5)
+#define LOAD_CMP		(1 << 4)
+#define ERS_FAIL		(1 << 3)
+#define PGM_FAIL		(1 << 2)
+#define INT_TO			(1 << 1)
+#define LD_FAIL_ECC_ERR		(1 << 0)
+
+#define TSRF			(1 << 0)
+
+#endif
diff --git a/drivers/mtd/sm_ftl.c b/drivers/mtd/sm_ftl.c
index 8dd6ba52404a..f9d5615c5727 100644
--- a/drivers/mtd/sm_ftl.c
+++ b/drivers/mtd/sm_ftl.c
@@ -1107,7 +1107,7 @@ static int sm_flush(struct mtd_blktrans_dev *dev)
 }
 
 /* outside interface: device is released */
-static int sm_release(struct mtd_blktrans_dev *dev)
+static void sm_release(struct mtd_blktrans_dev *dev)
 {
 	struct sm_ftl *ftl = dev->priv;
 
@@ -1116,7 +1116,6 @@ static int sm_release(struct mtd_blktrans_dev *dev)
 	cancel_work_sync(&ftl->flush_work);
 	sm_cache_flush(ftl);
 	mutex_unlock(&ftl->mutex);
-	return 0;
 }
 
 /* outside interface: get geometry */
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index 87f1d39ca551..3835321b8cf3 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -151,6 +151,7 @@ config MACVTAP
 config VXLAN
        tristate "Virtual eXtensible Local Area Network (VXLAN)"
        depends on INET
+       select NET_IP_TUNNEL
        ---help---
 	  This allows one to create vxlan virtual interfaces that provide
 	  Layer 2 Networks over Layer 3 Networks. VXLAN is often used
diff --git a/drivers/net/appletalk/Kconfig b/drivers/net/appletalk/Kconfig
index f5a89164e779..4ce6ca5f3d36 100644
--- a/drivers/net/appletalk/Kconfig
+++ b/drivers/net/appletalk/Kconfig
@@ -106,20 +106,4 @@ config IPDDP_ENCAP
 	  IP packets inside AppleTalk frames; this is useful if your Linux box
 	  is stuck on an AppleTalk network (which hopefully contains a
 	  decapsulator somewhere). Please see
-	  <file:Documentation/networking/ipddp.txt> for more information. If
-	  you said Y to "AppleTalk-IP driver support" above and you say Y
-	  here, then you cannot say Y to "AppleTalk-IP to IP Decapsulation
-	  support", below.
-
-config IPDDP_DECAP
-	bool "Appletalk-IP to IP Decapsulation support"
-	depends on IPDDP
-	help
-	  If you say Y here, the AppleTalk-IP code will be able to decapsulate
-	  AppleTalk-IP frames to IP packets; this is useful if you want your
-	  Linux box to act as an Internet gateway for an AppleTalk network.
-	  Please see <file:Documentation/networking/ipddp.txt> for more
-	  information.  If you said Y to "AppleTalk-IP driver support" above
-	  and you say Y here, then you cannot say Y to "IP to AppleTalk-IP
-	  Encapsulation support", above.
-
+	  <file:Documentation/networking/ipddp.txt> for more information.
diff --git a/drivers/net/bonding/bond_alb.c b/drivers/net/bonding/bond_alb.c
index f5e052723029..e02cc265723a 100644
--- a/drivers/net/bonding/bond_alb.c
+++ b/drivers/net/bonding/bond_alb.c
@@ -514,7 +514,7 @@ static void rlb_update_client(struct rlb_client_info *client_info)
 		skb->dev = client_info->slave->dev;
 
 		if (client_info->tag) {
-			skb = vlan_put_tag(skb, client_info->vlan_id);
+			skb = vlan_put_tag(skb, htons(ETH_P_8021Q), client_info->vlan_id);
 			if (!skb) {
 				pr_err("%s: Error: failed to insert VLAN tag\n",
 				       client_info->slave->bond->dev->name);
@@ -1014,7 +1014,7 @@ static void alb_send_learning_packets(struct slave *slave, u8 mac_addr[])
 				continue;
 			}
 
-			skb = vlan_put_tag(skb, vlan->vlan_id);
+			skb = vlan_put_tag(skb, htons(ETH_P_8021Q), vlan->vlan_id);
 			if (!skb) {
 				pr_err("%s: Error: failed to insert VLAN tag\n",
 				       bond->dev->name);
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index dbbea0eec134..d0aade04e49a 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -428,14 +428,15 @@ int bond_dev_queue_xmit(struct bonding *bond, struct sk_buff *skb,
  * @bond_dev: bonding net device that got called
  * @vid: vlan id being added
  */
-static int bond_vlan_rx_add_vid(struct net_device *bond_dev, uint16_t vid)
+static int bond_vlan_rx_add_vid(struct net_device *bond_dev,
+				__be16 proto, u16 vid)
 {
 	struct bonding *bond = netdev_priv(bond_dev);
 	struct slave *slave, *stop_at;
 	int i, res;
 
 	bond_for_each_slave(bond, slave, i) {
-		res = vlan_vid_add(slave->dev, vid);
+		res = vlan_vid_add(slave->dev, proto, vid);
 		if (res)
 			goto unwind;
 	}
@@ -453,7 +454,7 @@ unwind:
 	/* unwind from head to the slave that failed */
 	stop_at = slave;
 	bond_for_each_slave_from_to(bond, slave, i, bond->first_slave, stop_at)
-		vlan_vid_del(slave->dev, vid);
+		vlan_vid_del(slave->dev, proto, vid);
 
 	return res;
 }
@@ -463,14 +464,15 @@ unwind:
  * @bond_dev: bonding net device that got called
  * @vid: vlan id being removed
  */
-static int bond_vlan_rx_kill_vid(struct net_device *bond_dev, uint16_t vid)
+static int bond_vlan_rx_kill_vid(struct net_device *bond_dev,
+				 __be16 proto, u16 vid)
 {
 	struct bonding *bond = netdev_priv(bond_dev);
 	struct slave *slave;
 	int i, res;
 
 	bond_for_each_slave(bond, slave, i)
-		vlan_vid_del(slave->dev, vid);
+		vlan_vid_del(slave->dev, proto, vid);
 
 	res = bond_del_vlan(bond, vid);
 	if (res) {
@@ -488,7 +490,8 @@ static void bond_add_vlans_on_slave(struct bonding *bond, struct net_device *sla
 	int res;
 
 	list_for_each_entry(vlan, &bond->vlan_list, vlan_list) {
-		res = vlan_vid_add(slave_dev, vlan->vlan_id);
+		res = vlan_vid_add(slave_dev, htons(ETH_P_8021Q),
+				   vlan->vlan_id);
 		if (res)
 			pr_warning("%s: Failed to add vlan id %d to device %s\n",
 				   bond->dev->name, vlan->vlan_id,
@@ -504,7 +507,7 @@ static void bond_del_vlans_from_slave(struct bonding *bond,
 	list_for_each_entry(vlan, &bond->vlan_list, vlan_list) {
 		if (!vlan->vlan_id)
 			continue;
-		vlan_vid_del(slave_dev, vlan->vlan_id);
+		vlan_vid_del(slave_dev, htons(ETH_P_8021Q), vlan->vlan_id);
 	}
 }
 
@@ -779,7 +782,7 @@ static void bond_resend_igmp_join_requests(struct bonding *bond)
 
 	/* rejoin all groups on vlan devices */
 	list_for_each_entry(vlan, &bond->vlan_list, vlan_list) {
-		vlan_dev = __vlan_find_dev_deep(bond_dev,
+		vlan_dev = __vlan_find_dev_deep(bond_dev, htons(ETH_P_8021Q),
 						vlan->vlan_id);
 		if (vlan_dev)
 			__bond_resend_igmp_join_requests(vlan_dev);
@@ -796,9 +799,8 @@ static void bond_resend_igmp_join_requests_delayed(struct work_struct *work)
 {
 	struct bonding *bond = container_of(work, struct bonding,
 					    mcast_work.work);
-	rcu_read_lock();
+
 	bond_resend_igmp_join_requests(bond);
-	rcu_read_unlock();
 }
 
 /*
@@ -1915,14 +1917,16 @@ err_detach:
 	bond_detach_slave(bond, new_slave);
 	if (bond->primary_slave == new_slave)
 		bond->primary_slave = NULL;
-	write_unlock_bh(&bond->lock);
 	if (bond->curr_active_slave == new_slave) {
+		bond_change_active_slave(bond, NULL);
+		write_unlock_bh(&bond->lock);
 		read_lock(&bond->lock);
 		write_lock_bh(&bond->curr_slave_lock);
-		bond_change_active_slave(bond, NULL);
 		bond_select_active_slave(bond);
 		write_unlock_bh(&bond->curr_slave_lock);
 		read_unlock(&bond->lock);
+	} else {
+		write_unlock_bh(&bond->lock);
 	}
 	slave_disable_netpoll(new_slave);
 
@@ -2532,7 +2536,8 @@ static int bond_has_this_ip(struct bonding *bond, __be32 ip)
 
 	list_for_each_entry(vlan, &bond->vlan_list, vlan_list) {
 		rcu_read_lock();
-		vlan_dev = __vlan_find_dev_deep(bond->dev, vlan->vlan_id);
+		vlan_dev = __vlan_find_dev_deep(bond->dev, htons(ETH_P_8021Q),
+						vlan->vlan_id);
 		rcu_read_unlock();
 		if (vlan_dev && ip == bond_confirm_addr(vlan_dev, 0, ip))
 			return 1;
@@ -2561,7 +2566,7 @@ static void bond_arp_send(struct net_device *slave_dev, int arp_op, __be32 dest_
 		return;
 	}
 	if (vlan_id) {
-		skb = vlan_put_tag(skb, vlan_id);
+		skb = vlan_put_tag(skb, htons(ETH_P_8021Q), vlan_id);
 		if (!skb) {
 			pr_err("failed to insert VLAN tag\n");
 			return;
@@ -2623,6 +2628,7 @@ static void bond_arp_send_all(struct bonding *bond, struct slave *slave)
 		list_for_each_entry(vlan, &bond->vlan_list, vlan_list) {
 			rcu_read_lock();
 			vlan_dev = __vlan_find_dev_deep(bond->dev,
+							htons(ETH_P_8021Q),
 							vlan->vlan_id);
 			rcu_read_unlock();
 			if (vlan_dev == rt->dst.dev) {
@@ -4258,6 +4264,37 @@ void bond_set_mode_ops(struct bonding *bond, int mode)
 	}
 }
 
+static int bond_ethtool_get_settings(struct net_device *bond_dev,
+				     struct ethtool_cmd *ecmd)
+{
+	struct bonding *bond = netdev_priv(bond_dev);
+	struct slave *slave;
+	int i;
+	unsigned long speed = 0;
+
+	ecmd->duplex = DUPLEX_UNKNOWN;
+	ecmd->port = PORT_OTHER;
+
+	/* Since SLAVE_IS_OK returns false for all inactive or down slaves, we
+	 * do not need to check mode.  Though link speed might not represent
+	 * the true receive or transmit bandwidth (not all modes are symmetric)
+	 * this is an accurate maximum.
+	 */
+	read_lock(&bond->lock);
+	bond_for_each_slave(bond, slave, i) {
+		if (SLAVE_IS_OK(slave)) {
+			if (slave->speed != SPEED_UNKNOWN)
+				speed += slave->speed;
+			if (ecmd->duplex == DUPLEX_UNKNOWN &&
+			    slave->duplex != DUPLEX_UNKNOWN)
+				ecmd->duplex = slave->duplex;
+		}
+	}
+	ethtool_cmd_speed_set(ecmd, speed ? : SPEED_UNKNOWN);
+	read_unlock(&bond->lock);
+	return 0;
+}
+
 static void bond_ethtool_get_drvinfo(struct net_device *bond_dev,
 				     struct ethtool_drvinfo *drvinfo)
 {
@@ -4269,6 +4306,7 @@ static void bond_ethtool_get_drvinfo(struct net_device *bond_dev,
 
 static const struct ethtool_ops bond_ethtool_ops = {
 	.get_drvinfo		= bond_ethtool_get_drvinfo,
+	.get_settings		= bond_ethtool_get_settings,
 	.get_link		= ethtool_op_get_link,
 };
 
@@ -4359,9 +4397,9 @@ static void bond_setup(struct net_device *bond_dev)
 	 */
 
 	bond_dev->hw_features = BOND_VLAN_FEATURES |
-				NETIF_F_HW_VLAN_TX |
-				NETIF_F_HW_VLAN_RX |
-				NETIF_F_HW_VLAN_FILTER;
+				NETIF_F_HW_VLAN_CTAG_TX |
+				NETIF_F_HW_VLAN_CTAG_RX |
+				NETIF_F_HW_VLAN_CTAG_FILTER;
 
 	bond_dev->hw_features &= ~(NETIF_F_ALL_CSUM & ~NETIF_F_HW_CSUM);
 	bond_dev->features |= bond_dev->hw_features;
diff --git a/drivers/net/bonding/bond_procfs.c b/drivers/net/bonding/bond_procfs.c
index 3cea38d37344..94d06f1307b8 100644
--- a/drivers/net/bonding/bond_procfs.c
+++ b/drivers/net/bonding/bond_procfs.c
@@ -218,15 +218,13 @@ static const struct seq_operations bond_info_seq_ops = {
 static int bond_info_open(struct inode *inode, struct file *file)
 {
 	struct seq_file *seq;
-	struct proc_dir_entry *proc;
 	int res;
 
 	res = seq_open(file, &bond_info_seq_ops);
 	if (!res) {
 		/* recover the pointer buried in proc_dir_entry data */
 		seq = file->private_data;
-		proc = PDE(inode);
-		seq->private = proc->data;
+		seq->private = PDE_DATA(inode);
 	}
 
 	return res;
diff --git a/drivers/net/caif/Kconfig b/drivers/net/caif/Kconfig
index 60c2142373c9..7ffc756131a2 100644
--- a/drivers/net/caif/Kconfig
+++ b/drivers/net/caif/Kconfig
@@ -32,13 +32,6 @@ config CAIF_SPI_SYNC
 	help to synchronize to the next transfer in case of over or under-runs.
 	This option also needs to be enabled on the modem.
 
-config CAIF_SHM
-	tristate "CAIF shared memory protocol driver"
-	depends on CAIF && U5500_MBOX
-	default n
-	---help---
-	The CAIF shared memory protocol driver for the STE UX5500 platform.
-
 config CAIF_HSI
        tristate "CAIF HSI transport driver"
        depends on CAIF
@@ -47,3 +40,17 @@ config CAIF_HSI
        The caif low level driver for CAIF over HSI.
        Be aware that if you enable this then you also need to
        enable a low-level HSI driver.
+
+config CAIF_VIRTIO
+	tristate "CAIF virtio transport driver"
+	depends on CAIF
+	select VHOST_RING
+	select VIRTIO
+	select GENERIC_ALLOCATOR
+	default n
+	---help---
+	The caif driver for CAIF over Virtio.
+
+if CAIF_VIRTIO
+source "drivers/vhost/Kconfig"
+endif
diff --git a/drivers/net/caif/Makefile b/drivers/net/caif/Makefile
index 91dff861560f..9bbd45391f6c 100644
--- a/drivers/net/caif/Makefile
+++ b/drivers/net/caif/Makefile
@@ -7,9 +7,8 @@ obj-$(CONFIG_CAIF_TTY) += caif_serial.o
 cfspi_slave-objs := caif_spi.o caif_spi_slave.o
 obj-$(CONFIG_CAIF_SPI_SLAVE) += cfspi_slave.o
 
-# Shared memory
-caif_shm-objs := caif_shmcore.o caif_shm_u5500.o
-obj-$(CONFIG_CAIF_SHM) += caif_shm.o
-
 # HSI interface
 obj-$(CONFIG_CAIF_HSI) += caif_hsi.o
+
+# Virtio interface
+obj-$(CONFIG_CAIF_VIRTIO) += caif_virtio.o
diff --git a/drivers/net/caif/caif_hsi.c b/drivers/net/caif/caif_hsi.c
index 0def8b3106f4..5e40a8b68cbe 100644
--- a/drivers/net/caif/caif_hsi.c
+++ b/drivers/net/caif/caif_hsi.c
@@ -1,8 +1,7 @@
 /*
  * Copyright (C) ST-Ericsson AB 2010
- * Contact: Sjur Brendeland / sjur.brandeland@stericsson.com
- * Author:  Daniel Martensson / daniel.martensson@stericsson.com
- *	    Dmitry.Tarnyagin  / dmitry.tarnyagin@stericsson.com
+ * Author:  Daniel Martensson
+ *	    Dmitry.Tarnyagin  / dmitry.tarnyagin@lockless.no
  * License terms: GNU General Public License (GPL) version 2.
  */
 
@@ -25,7 +24,7 @@
 #include <net/caif/caif_hsi.h>
 
 MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Daniel Martensson<daniel.martensson@stericsson.com>");
+MODULE_AUTHOR("Daniel Martensson");
 MODULE_DESCRIPTION("CAIF HSI driver");
 
 /* Returns the number of padding bytes for alignment. */
diff --git a/drivers/net/caif/caif_serial.c b/drivers/net/caif/caif_serial.c
index be90debc7cd1..77be3cb0b5fe 100644
--- a/drivers/net/caif/caif_serial.c
+++ b/drivers/net/caif/caif_serial.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (C) ST-Ericsson AB 2010
- * Author:	Sjur Brendeland / sjur.brandeland@stericsson.com
+ * Author:	Sjur Brendeland
  * License terms: GNU General Public License (GPL) version 2
  */
 
@@ -21,7 +21,7 @@
 #include <linux/debugfs.h>
 
 MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Sjur Brendeland<sjur.brandeland@stericsson.com>");
+MODULE_AUTHOR("Sjur Brendeland");
 MODULE_DESCRIPTION("CAIF serial device TTY line discipline");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS_LDISC(N_CAIF);
diff --git a/drivers/net/caif/caif_shm_u5500.c b/drivers/net/caif/caif_shm_u5500.c
deleted file mode 100644
index 89d76b7b325a..000000000000
--- a/drivers/net/caif/caif_shm_u5500.c
+++ /dev/null
@@ -1,128 +0,0 @@
-/*
- * Copyright (C) ST-Ericsson AB 2010
- * Contact: Sjur Brendeland / sjur.brandeland@stericsson.com
- * Author:  Amarnath Revanna / amarnath.bangalore.revanna@stericsson.com
- * License terms: GNU General Public License (GPL) version 2
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ":" fmt
-
-#include <linux/init.h>
-#include <linux/module.h>
-#include <linux/netdevice.h>
-#include <mach/mbox-db5500.h>
-#include <net/caif/caif_shm.h>
-
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("CAIF Shared Memory protocol driver");
-
-#define MAX_SHM_INSTANCES	1
-
-enum {
-	MBX_ACC0,
-	MBX_ACC1,
-	MBX_DSP
-};
-
-static struct shmdev_layer shmdev_lyr[MAX_SHM_INSTANCES];
-
-static unsigned int shm_start;
-static unsigned int shm_size;
-
-module_param(shm_size, uint  , 0440);
-MODULE_PARM_DESC(shm_total_size, "Start of SHM shared memory");
-
-module_param(shm_start, uint  , 0440);
-MODULE_PARM_DESC(shm_total_start, "Total Size of SHM shared memory");
-
-static int shmdev_send_msg(u32 dev_id, u32 mbx_msg)
-{
-	/* Always block until msg is written successfully */
-	mbox_send(shmdev_lyr[dev_id].hmbx, mbx_msg, true);
-	return 0;
-}
-
-static int shmdev_mbx_setup(void *pshmdrv_cb, struct shmdev_layer *pshm_dev,
-							 void *pshm_drv)
-{
-	/*
-	 * For UX5500, we have only 1 SHM instance which uses MBX0
-	 * for communication with the peer modem
-	 */
-	pshm_dev->hmbx = mbox_setup(MBX_ACC0, pshmdrv_cb, pshm_drv);
-
-	if (!pshm_dev->hmbx)
-		return -ENODEV;
-	else
-		return 0;
-}
-
-static int __init caif_shmdev_init(void)
-{
-	int i, result;
-
-	/* Loop is currently overkill, there is only one instance */
-	for (i = 0; i < MAX_SHM_INSTANCES; i++) {
-
-		shmdev_lyr[i].shm_base_addr = shm_start;
-		shmdev_lyr[i].shm_total_sz = shm_size;
-
-		if (((char *)shmdev_lyr[i].shm_base_addr == NULL)
-			       || (shmdev_lyr[i].shm_total_sz <= 0))	{
-			pr_warn("ERROR,"
-				"Shared memory Address and/or Size incorrect"
-				", Bailing out ...\n");
-			result = -EINVAL;
-			goto clean;
-		}
-
-		pr_info("SHM AREA (instance %d) STARTS"
-			" AT %p\n", i, (char *)shmdev_lyr[i].shm_base_addr);
-
-		shmdev_lyr[i].shm_id = i;
-		shmdev_lyr[i].pshmdev_mbxsend = shmdev_send_msg;
-		shmdev_lyr[i].pshmdev_mbxsetup = shmdev_mbx_setup;
-
-		/*
-		 * Finally, CAIF core module is called with details in place:
-		 * 1. SHM base address
-		 * 2. SHM size
-		 * 3. MBX handle
-		 */
-		result = caif_shmcore_probe(&shmdev_lyr[i]);
-		if (result) {
-			pr_warn("ERROR[%d],"
-				"Could not probe SHM core (instance %d)"
-				" Bailing out ...\n", result, i);
-			goto clean;
-		}
-	}
-
-	return 0;
-
-clean:
-	/*
-	 * For now, we assume that even if one instance of SHM fails, we bail
-	 * out of the driver support completely. For this, we need to release
-	 * any memory allocated and unregister any instance of SHM net device.
-	 */
-	for (i = 0; i < MAX_SHM_INSTANCES; i++) {
-		if (shmdev_lyr[i].pshm_netdev)
-			unregister_netdev(shmdev_lyr[i].pshm_netdev);
-	}
-	return result;
-}
-
-static void __exit caif_shmdev_exit(void)
-{
-	int i;
-
-	for (i = 0; i < MAX_SHM_INSTANCES; i++) {
-		caif_shmcore_remove(shmdev_lyr[i].pshm_netdev);
-		kfree((void *)shmdev_lyr[i].shm_base_addr);
-	}
-
-}
-
-module_init(caif_shmdev_init);
-module_exit(caif_shmdev_exit);
diff --git a/drivers/net/caif/caif_shmcore.c b/drivers/net/caif/caif_shmcore.c
deleted file mode 100644
index bce8bac311c9..000000000000
--- a/drivers/net/caif/caif_shmcore.c
+++ /dev/null
@@ -1,747 +0,0 @@
-/*
- * Copyright (C) ST-Ericsson AB 2010
- * Contact: Sjur Brendeland / sjur.brandeland@stericsson.com
- * Authors:  Amarnath Revanna / amarnath.bangalore.revanna@stericsson.com,
- *           Daniel Martensson / daniel.martensson@stericsson.com
- * License terms: GNU General Public License (GPL) version 2
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ":" fmt
-
-#include <linux/spinlock.h>
-#include <linux/sched.h>
-#include <linux/list.h>
-#include <linux/netdevice.h>
-#include <linux/if_arp.h>
-#include <linux/io.h>
-
-#include <net/caif/caif_device.h>
-#include <net/caif/caif_shm.h>
-
-#define NR_TX_BUF		6
-#define NR_RX_BUF		6
-#define TX_BUF_SZ		0x2000
-#define RX_BUF_SZ		0x2000
-
-#define CAIF_NEEDED_HEADROOM	32
-
-#define CAIF_FLOW_ON		1
-#define CAIF_FLOW_OFF		0
-
-#define LOW_WATERMARK		3
-#define HIGH_WATERMARK		4
-
-/* Maximum number of CAIF buffers per shared memory buffer. */
-#define SHM_MAX_FRMS_PER_BUF	10
-
-/*
- * Size in bytes of the descriptor area
- * (With end of descriptor signalling)
- */
-#define SHM_CAIF_DESC_SIZE	((SHM_MAX_FRMS_PER_BUF + 1) * \
-					sizeof(struct shm_pck_desc))
-
-/*
- * Offset to the first CAIF frame within a shared memory buffer.
- * Aligned on 32 bytes.
- */
-#define SHM_CAIF_FRM_OFS	(SHM_CAIF_DESC_SIZE + (SHM_CAIF_DESC_SIZE % 32))
-
-/* Number of bytes for CAIF shared memory header. */
-#define SHM_HDR_LEN		1
-
-/* Number of padding bytes for the complete CAIF frame. */
-#define SHM_FRM_PAD_LEN		4
-
-#define CAIF_MAX_MTU		4096
-
-#define SHM_SET_FULL(x)	(((x+1) & 0x0F) << 0)
-#define SHM_GET_FULL(x)	(((x >> 0) & 0x0F) - 1)
-
-#define SHM_SET_EMPTY(x)	(((x+1) & 0x0F) << 4)
-#define SHM_GET_EMPTY(x)	(((x >> 4) & 0x0F) - 1)
-
-#define SHM_FULL_MASK		(0x0F << 0)
-#define SHM_EMPTY_MASK		(0x0F << 4)
-
-struct shm_pck_desc {
-	/*
-	 * Offset from start of shared memory area to start of
-	 * shared memory CAIF frame.
-	 */
-	u32 frm_ofs;
-	u32 frm_len;
-};
-
-struct buf_list {
-	unsigned char *desc_vptr;
-	u32 phy_addr;
-	u32 index;
-	u32 len;
-	u32 frames;
-	u32 frm_ofs;
-	struct list_head list;
-};
-
-struct shm_caif_frm {
-	/* Number of bytes of padding before the CAIF frame. */
-	u8 hdr_ofs;
-};
-
-struct shmdrv_layer {
-	/* caif_dev_common must always be first in the structure*/
-	struct caif_dev_common cfdev;
-
-	u32 shm_tx_addr;
-	u32 shm_rx_addr;
-	u32 shm_base_addr;
-	u32 tx_empty_available;
-	spinlock_t lock;
-
-	struct list_head tx_empty_list;
-	struct list_head tx_pend_list;
-	struct list_head tx_full_list;
-	struct list_head rx_empty_list;
-	struct list_head rx_pend_list;
-	struct list_head rx_full_list;
-
-	struct workqueue_struct *pshm_tx_workqueue;
-	struct workqueue_struct *pshm_rx_workqueue;
-
-	struct work_struct shm_tx_work;
-	struct work_struct shm_rx_work;
-
-	struct sk_buff_head sk_qhead;
-	struct shmdev_layer *pshm_dev;
-};
-
-static int shm_netdev_open(struct net_device *shm_netdev)
-{
-	netif_wake_queue(shm_netdev);
-	return 0;
-}
-
-static int shm_netdev_close(struct net_device *shm_netdev)
-{
-	netif_stop_queue(shm_netdev);
-	return 0;
-}
-
-int caif_shmdrv_rx_cb(u32 mbx_msg, void *priv)
-{
-	struct buf_list *pbuf;
-	struct shmdrv_layer *pshm_drv;
-	struct list_head *pos;
-	u32 avail_emptybuff = 0;
-	unsigned long flags = 0;
-
-	pshm_drv = priv;
-
-	/* Check for received buffers. */
-	if (mbx_msg & SHM_FULL_MASK) {
-		int idx;
-
-		spin_lock_irqsave(&pshm_drv->lock, flags);
-
-		/* Check whether we have any outstanding buffers. */
-		if (list_empty(&pshm_drv->rx_empty_list)) {
-
-			/* Release spin lock. */
-			spin_unlock_irqrestore(&pshm_drv->lock, flags);
-
-			/* We print even in IRQ context... */
-			pr_warn("No empty Rx buffers to fill: "
-					"mbx_msg:%x\n", mbx_msg);
-
-			/* Bail out. */
-			goto err_sync;
-		}
-
-		pbuf =
-			list_entry(pshm_drv->rx_empty_list.next,
-					struct buf_list, list);
-		idx = pbuf->index;
-
-		/* Check buffer synchronization. */
-		if (idx != SHM_GET_FULL(mbx_msg)) {
-
-			/* We print even in IRQ context... */
-			pr_warn(
-			"phyif_shm_mbx_msg_cb: RX full out of sync:"
-			" idx:%d, msg:%x SHM_GET_FULL(mbx_msg):%x\n",
-				idx, mbx_msg, SHM_GET_FULL(mbx_msg));
-
-			spin_unlock_irqrestore(&pshm_drv->lock, flags);
-
-			/* Bail out. */
-			goto err_sync;
-		}
-
-		list_del_init(&pbuf->list);
-		list_add_tail(&pbuf->list, &pshm_drv->rx_full_list);
-
-		spin_unlock_irqrestore(&pshm_drv->lock, flags);
-
-		/* Schedule RX work queue. */
-		if (!work_pending(&pshm_drv->shm_rx_work))
-			queue_work(pshm_drv->pshm_rx_workqueue,
-						&pshm_drv->shm_rx_work);
-	}
-
-	/* Check for emptied buffers. */
-	if (mbx_msg & SHM_EMPTY_MASK) {
-		int idx;
-
-		spin_lock_irqsave(&pshm_drv->lock, flags);
-
-		/* Check whether we have any outstanding buffers. */
-		if (list_empty(&pshm_drv->tx_full_list)) {
-
-			/* We print even in IRQ context... */
-			pr_warn("No TX to empty: msg:%x\n", mbx_msg);
-
-			spin_unlock_irqrestore(&pshm_drv->lock, flags);
-
-			/* Bail out. */
-			goto err_sync;
-		}
-
-		pbuf =
-			list_entry(pshm_drv->tx_full_list.next,
-					struct buf_list, list);
-		idx = pbuf->index;
-
-		/* Check buffer synchronization. */
-		if (idx != SHM_GET_EMPTY(mbx_msg)) {
-
-			spin_unlock_irqrestore(&pshm_drv->lock, flags);
-
-			/* We print even in IRQ context... */
-			pr_warn("TX empty "
-				"out of sync:idx:%d, msg:%x\n", idx, mbx_msg);
-
-			/* Bail out. */
-			goto err_sync;
-		}
-		list_del_init(&pbuf->list);
-
-		/* Reset buffer parameters. */
-		pbuf->frames = 0;
-		pbuf->frm_ofs = SHM_CAIF_FRM_OFS;
-
-		list_add_tail(&pbuf->list, &pshm_drv->tx_empty_list);
-
-		/* Check the available no. of buffers in the empty list */
-		list_for_each(pos, &pshm_drv->tx_empty_list)
-			avail_emptybuff++;
-
-		/* Check whether we have to wake up the transmitter. */
-		if ((avail_emptybuff > HIGH_WATERMARK) &&
-					(!pshm_drv->tx_empty_available)) {
-			pshm_drv->tx_empty_available = 1;
-			spin_unlock_irqrestore(&pshm_drv->lock, flags);
-			pshm_drv->cfdev.flowctrl
-					(pshm_drv->pshm_dev->pshm_netdev,
-								CAIF_FLOW_ON);
-
-
-			/* Schedule the work queue. if required */
-			if (!work_pending(&pshm_drv->shm_tx_work))
-				queue_work(pshm_drv->pshm_tx_workqueue,
-							&pshm_drv->shm_tx_work);
-		} else
-			spin_unlock_irqrestore(&pshm_drv->lock, flags);
-	}
-
-	return 0;
-
-err_sync:
-	return -EIO;
-}
-
-static void shm_rx_work_func(struct work_struct *rx_work)
-{
-	struct shmdrv_layer *pshm_drv;
-	struct buf_list *pbuf;
-	unsigned long flags = 0;
-	struct sk_buff *skb;
-	char *p;
-	int ret;
-
-	pshm_drv = container_of(rx_work, struct shmdrv_layer, shm_rx_work);
-
-	while (1) {
-
-		struct shm_pck_desc *pck_desc;
-
-		spin_lock_irqsave(&pshm_drv->lock, flags);
-
-		/* Check for received buffers. */
-		if (list_empty(&pshm_drv->rx_full_list)) {
-			spin_unlock_irqrestore(&pshm_drv->lock, flags);
-			break;
-		}
-
-		pbuf =
-			list_entry(pshm_drv->rx_full_list.next, struct buf_list,
-					list);
-		list_del_init(&pbuf->list);
-		spin_unlock_irqrestore(&pshm_drv->lock, flags);
-
-		/* Retrieve pointer to start of the packet descriptor area. */
-		pck_desc = (struct shm_pck_desc *) pbuf->desc_vptr;
-
-		/*
-		 * Check whether descriptor contains a CAIF shared memory
-		 * frame.
-		 */
-		while (pck_desc->frm_ofs) {
-			unsigned int frm_buf_ofs;
-			unsigned int frm_pck_ofs;
-			unsigned int frm_pck_len;
-			/*
-			 * Check whether offset is within buffer limits
-			 * (lower).
-			 */
-			if (pck_desc->frm_ofs <
-				(pbuf->phy_addr - pshm_drv->shm_base_addr))
-				break;
-			/*
-			 * Check whether offset is within buffer limits
-			 * (higher).
-			 */
-			if (pck_desc->frm_ofs >
-				((pbuf->phy_addr - pshm_drv->shm_base_addr) +
-					pbuf->len))
-				break;
-
-			/* Calculate offset from start of buffer. */
-			frm_buf_ofs =
-				pck_desc->frm_ofs - (pbuf->phy_addr -
-						pshm_drv->shm_base_addr);
-
-			/*
-			 * Calculate offset and length of CAIF packet while
-			 * taking care of the shared memory header.
-			 */
-			frm_pck_ofs =
-				frm_buf_ofs + SHM_HDR_LEN +
-				(*(pbuf->desc_vptr + frm_buf_ofs));
-			frm_pck_len =
-				(pck_desc->frm_len - SHM_HDR_LEN -
-				(*(pbuf->desc_vptr + frm_buf_ofs)));
-
-			/* Check whether CAIF packet is within buffer limits */
-			if ((frm_pck_ofs + pck_desc->frm_len) > pbuf->len)
-				break;
-
-			/* Get a suitable CAIF packet and copy in data. */
-			skb = netdev_alloc_skb(pshm_drv->pshm_dev->pshm_netdev,
-							frm_pck_len + 1);
-
-			if (skb == NULL) {
-				pr_info("OOM: Try next frame in descriptor\n");
-				break;
-			}
-
-			p = skb_put(skb, frm_pck_len);
-			memcpy(p, pbuf->desc_vptr + frm_pck_ofs, frm_pck_len);
-
-			skb->protocol = htons(ETH_P_CAIF);
-			skb_reset_mac_header(skb);
-			skb->dev = pshm_drv->pshm_dev->pshm_netdev;
-
-			/* Push received packet up the stack. */
-			ret = netif_rx_ni(skb);
-
-			if (!ret) {
-				pshm_drv->pshm_dev->pshm_netdev->stats.
-								rx_packets++;
-				pshm_drv->pshm_dev->pshm_netdev->stats.
-						rx_bytes += pck_desc->frm_len;
-			} else
-				++pshm_drv->pshm_dev->pshm_netdev->stats.
-								rx_dropped;
-			/* Move to next packet descriptor. */
-			pck_desc++;
-		}
-
-		spin_lock_irqsave(&pshm_drv->lock, flags);
-		list_add_tail(&pbuf->list, &pshm_drv->rx_pend_list);
-
-		spin_unlock_irqrestore(&pshm_drv->lock, flags);
-
-	}
-
-	/* Schedule the work queue. if required */
-	if (!work_pending(&pshm_drv->shm_tx_work))
-		queue_work(pshm_drv->pshm_tx_workqueue, &pshm_drv->shm_tx_work);
-
-}
-
-static void shm_tx_work_func(struct work_struct *tx_work)
-{
-	u32 mbox_msg;
-	unsigned int frmlen, avail_emptybuff, append = 0;
-	unsigned long flags = 0;
-	struct buf_list *pbuf = NULL;
-	struct shmdrv_layer *pshm_drv;
-	struct shm_caif_frm *frm;
-	struct sk_buff *skb;
-	struct shm_pck_desc *pck_desc;
-	struct list_head *pos;
-
-	pshm_drv = container_of(tx_work, struct shmdrv_layer, shm_tx_work);
-
-	do {
-		/* Initialize mailbox message. */
-		mbox_msg = 0x00;
-		avail_emptybuff = 0;
-
-		spin_lock_irqsave(&pshm_drv->lock, flags);
-
-		/* Check for pending receive buffers. */
-		if (!list_empty(&pshm_drv->rx_pend_list)) {
-
-			pbuf = list_entry(pshm_drv->rx_pend_list.next,
-						struct buf_list, list);
-
-			list_del_init(&pbuf->list);
-			list_add_tail(&pbuf->list, &pshm_drv->rx_empty_list);
-			/*
-			 * Value index is never changed,
-			 * so read access should be safe.
-			 */
-			mbox_msg |= SHM_SET_EMPTY(pbuf->index);
-		}
-
-		skb = skb_peek(&pshm_drv->sk_qhead);
-
-		if (skb == NULL)
-			goto send_msg;
-		/* Check the available no. of buffers in the empty list */
-		list_for_each(pos, &pshm_drv->tx_empty_list)
-			avail_emptybuff++;
-
-		if ((avail_emptybuff < LOW_WATERMARK) &&
-					pshm_drv->tx_empty_available) {
-			/* Update blocking condition. */
-			pshm_drv->tx_empty_available = 0;
-			spin_unlock_irqrestore(&pshm_drv->lock, flags);
-			pshm_drv->cfdev.flowctrl
-					(pshm_drv->pshm_dev->pshm_netdev,
-					CAIF_FLOW_OFF);
-			spin_lock_irqsave(&pshm_drv->lock, flags);
-		}
-		/*
-		 * We simply return back to the caller if we do not have space
-		 * either in Tx pending list or Tx empty list. In this case,
-		 * we hold the received skb in the skb list, waiting to
-		 * be transmitted once Tx buffers become available
-		 */
-		if (list_empty(&pshm_drv->tx_empty_list))
-			goto send_msg;
-
-		/* Get the first free Tx buffer. */
-		pbuf = list_entry(pshm_drv->tx_empty_list.next,
-						struct buf_list, list);
-		do {
-			if (append) {
-				skb = skb_peek(&pshm_drv->sk_qhead);
-				if (skb == NULL)
-					break;
-			}
-
-			frm = (struct shm_caif_frm *)
-					(pbuf->desc_vptr + pbuf->frm_ofs);
-
-			frm->hdr_ofs = 0;
-			frmlen = 0;
-			frmlen += SHM_HDR_LEN + frm->hdr_ofs + skb->len;
-
-			/* Add tail padding if needed. */
-			if (frmlen % SHM_FRM_PAD_LEN)
-				frmlen += SHM_FRM_PAD_LEN -
-						(frmlen % SHM_FRM_PAD_LEN);
-
-			/*
-			 * Verify that packet, header and additional padding
-			 * can fit within the buffer frame area.
-			 */
-			if (frmlen >= (pbuf->len - pbuf->frm_ofs))
-				break;
-
-			if (!append) {
-				list_del_init(&pbuf->list);
-				append = 1;
-			}
-
-			skb = skb_dequeue(&pshm_drv->sk_qhead);
-			if (skb == NULL)
-				break;
-			/* Copy in CAIF frame. */
-			skb_copy_bits(skb, 0, pbuf->desc_vptr +
-					pbuf->frm_ofs + SHM_HDR_LEN +
-						frm->hdr_ofs, skb->len);
-
-			pshm_drv->pshm_dev->pshm_netdev->stats.tx_packets++;
-			pshm_drv->pshm_dev->pshm_netdev->stats.tx_bytes +=
-									frmlen;
-			dev_kfree_skb_irq(skb);
-
-			/* Fill in the shared memory packet descriptor area. */
-			pck_desc = (struct shm_pck_desc *) (pbuf->desc_vptr);
-			/* Forward to current frame. */
-			pck_desc += pbuf->frames;
-			pck_desc->frm_ofs = (pbuf->phy_addr -
-						pshm_drv->shm_base_addr) +
-								pbuf->frm_ofs;
-			pck_desc->frm_len = frmlen;
-			/* Terminate packet descriptor area. */
-			pck_desc++;
-			pck_desc->frm_ofs = 0;
-			/* Update buffer parameters. */
-			pbuf->frames++;
-			pbuf->frm_ofs += frmlen + (frmlen % 32);
-
-		} while (pbuf->frames < SHM_MAX_FRMS_PER_BUF);
-
-		/* Assign buffer as full. */
-		list_add_tail(&pbuf->list, &pshm_drv->tx_full_list);
-		append = 0;
-		mbox_msg |= SHM_SET_FULL(pbuf->index);
-send_msg:
-		spin_unlock_irqrestore(&pshm_drv->lock, flags);
-
-		if (mbox_msg)
-			pshm_drv->pshm_dev->pshmdev_mbxsend
-					(pshm_drv->pshm_dev->shm_id, mbox_msg);
-	} while (mbox_msg);
-}
-
-static int shm_netdev_tx(struct sk_buff *skb, struct net_device *shm_netdev)
-{
-	struct shmdrv_layer *pshm_drv;
-
-	pshm_drv = netdev_priv(shm_netdev);
-
-	skb_queue_tail(&pshm_drv->sk_qhead, skb);
-
-	/* Schedule Tx work queue. for deferred processing of skbs*/
-	if (!work_pending(&pshm_drv->shm_tx_work))
-		queue_work(pshm_drv->pshm_tx_workqueue, &pshm_drv->shm_tx_work);
-
-	return 0;
-}
-
-static const struct net_device_ops netdev_ops = {
-	.ndo_open = shm_netdev_open,
-	.ndo_stop = shm_netdev_close,
-	.ndo_start_xmit = shm_netdev_tx,
-};
-
-static void shm_netdev_setup(struct net_device *pshm_netdev)
-{
-	struct shmdrv_layer *pshm_drv;
-	pshm_netdev->netdev_ops = &netdev_ops;
-
-	pshm_netdev->mtu = CAIF_MAX_MTU;
-	pshm_netdev->type = ARPHRD_CAIF;
-	pshm_netdev->hard_header_len = CAIF_NEEDED_HEADROOM;
-	pshm_netdev->tx_queue_len = 0;
-	pshm_netdev->destructor = free_netdev;
-
-	pshm_drv = netdev_priv(pshm_netdev);
-
-	/* Initialize structures in a clean state. */
-	memset(pshm_drv, 0, sizeof(struct shmdrv_layer));
-
-	pshm_drv->cfdev.link_select = CAIF_LINK_LOW_LATENCY;
-}
-
-int caif_shmcore_probe(struct shmdev_layer *pshm_dev)
-{
-	int result, j;
-	struct shmdrv_layer *pshm_drv = NULL;
-
-	pshm_dev->pshm_netdev = alloc_netdev(sizeof(struct shmdrv_layer),
-						"cfshm%d", shm_netdev_setup);
-	if (!pshm_dev->pshm_netdev)
-		return -ENOMEM;
-
-	pshm_drv = netdev_priv(pshm_dev->pshm_netdev);
-	pshm_drv->pshm_dev = pshm_dev;
-
-	/*
-	 * Initialization starts with the verification of the
-	 * availability of MBX driver by calling its setup function.
-	 * MBX driver must be available by this time for proper
-	 * functioning of SHM driver.
-	 */
-	if ((pshm_dev->pshmdev_mbxsetup
-				(caif_shmdrv_rx_cb, pshm_dev, pshm_drv)) != 0) {
-		pr_warn("Could not config. SHM Mailbox,"
-				" Bailing out.....\n");
-		free_netdev(pshm_dev->pshm_netdev);
-		return -ENODEV;
-	}
-
-	skb_queue_head_init(&pshm_drv->sk_qhead);
-
-	pr_info("SHM DEVICE[%d] PROBED BY DRIVER, NEW SHM DRIVER"
-			" INSTANCE AT pshm_drv =0x%p\n",
-			pshm_drv->pshm_dev->shm_id, pshm_drv);
-
-	if (pshm_dev->shm_total_sz <
-			(NR_TX_BUF * TX_BUF_SZ + NR_RX_BUF * RX_BUF_SZ)) {
-
-		pr_warn("ERROR, Amount of available"
-				" Phys. SHM cannot accommodate current SHM "
-				"driver configuration, Bailing out ...\n");
-		free_netdev(pshm_dev->pshm_netdev);
-		return -ENOMEM;
-	}
-
-	pshm_drv->shm_base_addr = pshm_dev->shm_base_addr;
-	pshm_drv->shm_tx_addr = pshm_drv->shm_base_addr;
-
-	if (pshm_dev->shm_loopback)
-		pshm_drv->shm_rx_addr = pshm_drv->shm_tx_addr;
-	else
-		pshm_drv->shm_rx_addr = pshm_dev->shm_base_addr +
-						(NR_TX_BUF * TX_BUF_SZ);
-
-	spin_lock_init(&pshm_drv->lock);
-	INIT_LIST_HEAD(&pshm_drv->tx_empty_list);
-	INIT_LIST_HEAD(&pshm_drv->tx_pend_list);
-	INIT_LIST_HEAD(&pshm_drv->tx_full_list);
-
-	INIT_LIST_HEAD(&pshm_drv->rx_empty_list);
-	INIT_LIST_HEAD(&pshm_drv->rx_pend_list);
-	INIT_LIST_HEAD(&pshm_drv->rx_full_list);
-
-	INIT_WORK(&pshm_drv->shm_tx_work, shm_tx_work_func);
-	INIT_WORK(&pshm_drv->shm_rx_work, shm_rx_work_func);
-
-	pshm_drv->pshm_tx_workqueue =
-				create_singlethread_workqueue("shm_tx_work");
-	pshm_drv->pshm_rx_workqueue =
-				create_singlethread_workqueue("shm_rx_work");
-
-	for (j = 0; j < NR_TX_BUF; j++) {
-		struct buf_list *tx_buf =
-				kmalloc(sizeof(struct buf_list), GFP_KERNEL);
-
-		if (tx_buf == NULL) {
-			free_netdev(pshm_dev->pshm_netdev);
-			return -ENOMEM;
-		}
-		tx_buf->index = j;
-		tx_buf->phy_addr = pshm_drv->shm_tx_addr + (TX_BUF_SZ * j);
-		tx_buf->len = TX_BUF_SZ;
-		tx_buf->frames = 0;
-		tx_buf->frm_ofs = SHM_CAIF_FRM_OFS;
-
-		if (pshm_dev->shm_loopback)
-			tx_buf->desc_vptr = (unsigned char *)tx_buf->phy_addr;
-		else
-			/*
-			 * FIXME: the result of ioremap is not a pointer - arnd
-			 */
-			tx_buf->desc_vptr =
-					ioremap(tx_buf->phy_addr, TX_BUF_SZ);
-
-		list_add_tail(&tx_buf->list, &pshm_drv->tx_empty_list);
-	}
-
-	for (j = 0; j < NR_RX_BUF; j++) {
-		struct buf_list *rx_buf =
-				kmalloc(sizeof(struct buf_list), GFP_KERNEL);
-
-		if (rx_buf == NULL) {
-			free_netdev(pshm_dev->pshm_netdev);
-			return -ENOMEM;
-		}
-		rx_buf->index = j;
-		rx_buf->phy_addr = pshm_drv->shm_rx_addr + (RX_BUF_SZ * j);
-		rx_buf->len = RX_BUF_SZ;
-
-		if (pshm_dev->shm_loopback)
-			rx_buf->desc_vptr = (unsigned char *)rx_buf->phy_addr;
-		else
-			rx_buf->desc_vptr =
-					ioremap(rx_buf->phy_addr, RX_BUF_SZ);
-		list_add_tail(&rx_buf->list, &pshm_drv->rx_empty_list);
-	}
-
-	pshm_drv->tx_empty_available = 1;
-	result = register_netdev(pshm_dev->pshm_netdev);
-	if (result)
-		pr_warn("ERROR[%d], SHM could not, "
-			"register with NW FRMWK Bailing out ...\n", result);
-
-	return result;
-}
-
-void caif_shmcore_remove(struct net_device *pshm_netdev)
-{
-	struct buf_list *pbuf;
-	struct shmdrv_layer *pshm_drv = NULL;
-
-	pshm_drv = netdev_priv(pshm_netdev);
-
-	while (!(list_empty(&pshm_drv->tx_pend_list))) {
-		pbuf =
-			list_entry(pshm_drv->tx_pend_list.next,
-					struct buf_list, list);
-
-		list_del(&pbuf->list);
-		kfree(pbuf);
-	}
-
-	while (!(list_empty(&pshm_drv->tx_full_list))) {
-		pbuf =
-			list_entry(pshm_drv->tx_full_list.next,
-					struct buf_list, list);
-		list_del(&pbuf->list);
-		kfree(pbuf);
-	}
-
-	while (!(list_empty(&pshm_drv->tx_empty_list))) {
-		pbuf =
-			list_entry(pshm_drv->tx_empty_list.next,
-					struct buf_list, list);
-		list_del(&pbuf->list);
-		kfree(pbuf);
-	}
-
-	while (!(list_empty(&pshm_drv->rx_full_list))) {
-		pbuf =
-			list_entry(pshm_drv->tx_full_list.next,
-				struct buf_list, list);
-		list_del(&pbuf->list);
-		kfree(pbuf);
-	}
-
-	while (!(list_empty(&pshm_drv->rx_pend_list))) {
-		pbuf =
-			list_entry(pshm_drv->tx_pend_list.next,
-				struct buf_list, list);
-		list_del(&pbuf->list);
-		kfree(pbuf);
-	}
-
-	while (!(list_empty(&pshm_drv->rx_empty_list))) {
-		pbuf =
-			list_entry(pshm_drv->rx_empty_list.next,
-				struct buf_list, list);
-		list_del(&pbuf->list);
-		kfree(pbuf);
-	}
-
-	/* Destroy work queues. */
-	destroy_workqueue(pshm_drv->pshm_tx_workqueue);
-	destroy_workqueue(pshm_drv->pshm_rx_workqueue);
-
-	unregister_netdev(pshm_netdev);
-}
diff --git a/drivers/net/caif/caif_spi.c b/drivers/net/caif/caif_spi.c
index b71ce9bf0afb..155db68e13ba 100644
--- a/drivers/net/caif/caif_spi.c
+++ b/drivers/net/caif/caif_spi.c
@@ -1,7 +1,6 @@
 /*
  * Copyright (C) ST-Ericsson AB 2010
- * Contact: Sjur Brendeland / sjur.brandeland@stericsson.com
- * Author:  Daniel Martensson / Daniel.Martensson@stericsson.com
+ * Author:  Daniel Martensson
  * License terms: GNU General Public License (GPL) version 2.
  */
 
@@ -29,7 +28,7 @@
 #endif /* CONFIG_CAIF_SPI_SYNC */
 
 MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Daniel Martensson<daniel.martensson@stericsson.com>");
+MODULE_AUTHOR("Daniel Martensson");
 MODULE_DESCRIPTION("CAIF SPI driver");
 
 /* Returns the number of padding bytes for alignment. */
@@ -864,6 +863,7 @@ static int __init cfspi_init_module(void)
 	driver_remove_file(&cfspi_spi_driver.driver,
 			   &driver_attr_up_head_align);
  err_create_up_head_align:
+	platform_driver_unregister(&cfspi_spi_driver);
  err_dev_register:
 	return result;
 }
diff --git a/drivers/net/caif/caif_spi_slave.c b/drivers/net/caif/caif_spi_slave.c
index e139e133fc79..ee92ad5a6cf8 100644
--- a/drivers/net/caif/caif_spi_slave.c
+++ b/drivers/net/caif/caif_spi_slave.c
@@ -1,7 +1,6 @@
 /*
  * Copyright (C) ST-Ericsson AB 2010
- * Contact: Sjur Brendeland / sjur.brandeland@stericsson.com
- * Author:  Daniel Martensson / Daniel.Martensson@stericsson.com
+ * Author:  Daniel Martensson
  * License terms: GNU General Public License (GPL) version 2.
  */
 #include <linux/init.h>
diff --git a/drivers/net/caif/caif_virtio.c b/drivers/net/caif/caif_virtio.c
new file mode 100644
index 000000000000..b9ed1288ce2d
--- /dev/null
+++ b/drivers/net/caif/caif_virtio.c
@@ -0,0 +1,790 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2013
+ * Authors: Vicram Arv
+ *	    Dmitry Tarnyagin <dmitry.tarnyagin@lockless.no>
+ *	    Sjur Brendeland
+ * License terms: GNU General Public License (GPL) version 2
+ */
+#include <linux/module.h>
+#include <linux/if_arp.h>
+#include <linux/virtio.h>
+#include <linux/vringh.h>
+#include <linux/debugfs.h>
+#include <linux/spinlock.h>
+#include <linux/genalloc.h>
+#include <linux/interrupt.h>
+#include <linux/netdevice.h>
+#include <linux/rtnetlink.h>
+#include <linux/virtio_ids.h>
+#include <linux/virtio_caif.h>
+#include <linux/virtio_ring.h>
+#include <linux/dma-mapping.h>
+#include <net/caif/caif_dev.h>
+#include <linux/virtio_config.h>
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Vicram Arv");
+MODULE_AUTHOR("Sjur Brendeland");
+MODULE_DESCRIPTION("Virtio CAIF Driver");
+
+/* NAPI schedule quota */
+#define CFV_DEFAULT_QUOTA 32
+
+/* Defaults used if virtio config space is unavailable */
+#define CFV_DEF_MTU_SIZE 4096
+#define CFV_DEF_HEADROOM 32
+#define CFV_DEF_TAILROOM 32
+
+/* Required IP header alignment */
+#define IP_HDR_ALIGN 4
+
+/* struct cfv_napi_contxt - NAPI context info
+ * @riov: IOV holding data read from the ring. Note that riov may
+ *	  still hold data when cfv_rx_poll() returns.
+ * @head: Last descriptor ID we received from vringh_getdesc_kern.
+ *	  We use this to put descriptor back on the used ring. USHRT_MAX is
+ *	  used to indicate invalid head-id.
+ */
+struct cfv_napi_context {
+	struct vringh_kiov riov;
+	unsigned short head;
+};
+
+/* struct cfv_stats - statistics for debugfs
+ * @rx_napi_complete:	Number of NAPI completions (RX)
+ * @rx_napi_resched:	Number of calls where the full quota was used (RX)
+ * @rx_nomem:		Number of SKB alloc failures (RX)
+ * @rx_kicks:		Number of RX kicks
+ * @tx_full_ring:	Number times TX ring was full
+ * @tx_no_mem:		Number of times TX went out of memory
+ * @tx_flow_on:		Number of flow on (TX)
+ * @tx_kicks:		Number of TX kicks
+ */
+struct cfv_stats {
+	u32 rx_napi_complete;
+	u32 rx_napi_resched;
+	u32 rx_nomem;
+	u32 rx_kicks;
+	u32 tx_full_ring;
+	u32 tx_no_mem;
+	u32 tx_flow_on;
+	u32 tx_kicks;
+};
+
+/* struct cfv_info - Caif Virtio control structure
+ * @cfdev:	caif common header
+ * @vdev:	Associated virtio device
+ * @vr_rx:	rx/downlink host vring
+ * @vq_tx:	tx/uplink virtqueue
+ * @ndev:	CAIF link layer device
+ * @watermark_tx: indicates number of free descriptors we need
+ *		to reopen the tx-queues after overload.
+ * @tx_lock:	protects vq_tx from concurrent use
+ * @tx_release_tasklet: Tasklet for freeing consumed TX buffers
+ * @napi:       Napi context used in cfv_rx_poll()
+ * @ctx:        Context data used in cfv_rx_poll()
+ * @tx_hr:	transmit headroom
+ * @rx_hr:	receive headroom
+ * @tx_tr:	transmit tail room
+ * @rx_tr:	receive tail room
+ * @mtu:	transmit max size
+ * @mru:	receive max size
+ * @allocsz:    size of dma memory reserved for TX buffers
+ * @alloc_addr: virtual address to dma memory for TX buffers
+ * @alloc_dma:  dma address to dma memory for TX buffers
+ * @genpool:    Gen Pool used for allocating TX buffers
+ * @reserved_mem: Pointer to memory reserve allocated from genpool
+ * @reserved_size: Size of memory reserve allocated from genpool
+ * @stats:       Statistics exposed in sysfs
+ * @debugfs:    Debugfs dentry for statistic counters
+ */
+struct cfv_info {
+	struct caif_dev_common cfdev;
+	struct virtio_device *vdev;
+	struct vringh *vr_rx;
+	struct virtqueue *vq_tx;
+	struct net_device *ndev;
+	unsigned int watermark_tx;
+	/* Protect access to vq_tx */
+	spinlock_t tx_lock;
+	struct tasklet_struct tx_release_tasklet;
+	struct napi_struct napi;
+	struct cfv_napi_context ctx;
+	u16 tx_hr;
+	u16 rx_hr;
+	u16 tx_tr;
+	u16 rx_tr;
+	u32 mtu;
+	u32 mru;
+	size_t allocsz;
+	void *alloc_addr;
+	dma_addr_t alloc_dma;
+	struct gen_pool *genpool;
+	unsigned long reserved_mem;
+	size_t reserved_size;
+	struct cfv_stats stats;
+	struct dentry *debugfs;
+};
+
+/* struct buf_info - maintains transmit buffer data handle
+ * @size:	size of transmit buffer
+ * @dma_handle: handle to allocated dma device memory area
+ * @vaddr:	virtual address mapping to allocated memory area
+ */
+struct buf_info {
+	size_t size;
+	u8 *vaddr;
+};
+
+/* Called from virtio device, in IRQ context */
+static void cfv_release_cb(struct virtqueue *vq_tx)
+{
+	struct cfv_info *cfv = vq_tx->vdev->priv;
+
+	++cfv->stats.tx_kicks;
+	tasklet_schedule(&cfv->tx_release_tasklet);
+}
+
+static void free_buf_info(struct cfv_info *cfv, struct buf_info *buf_info)
+{
+	if (!buf_info)
+		return;
+	gen_pool_free(cfv->genpool, (unsigned long) buf_info->vaddr,
+		      buf_info->size);
+	kfree(buf_info);
+}
+
+/* This is invoked whenever the remote processor completed processing
+ * a TX msg we just sent, and the buffer is put back to the used ring.
+ */
+static void cfv_release_used_buf(struct virtqueue *vq_tx)
+{
+	struct cfv_info *cfv = vq_tx->vdev->priv;
+	unsigned long flags;
+
+	BUG_ON(vq_tx != cfv->vq_tx);
+
+	for (;;) {
+		unsigned int len;
+		struct buf_info *buf_info;
+
+		/* Get used buffer from used ring to recycle used descriptors */
+		spin_lock_irqsave(&cfv->tx_lock, flags);
+		buf_info = virtqueue_get_buf(vq_tx, &len);
+		spin_unlock_irqrestore(&cfv->tx_lock, flags);
+
+		/* Stop looping if there are no more buffers to free */
+		if (!buf_info)
+			break;
+
+		free_buf_info(cfv, buf_info);
+
+		/* watermark_tx indicates if we previously stopped the tx
+		 * queues. If we have enough free stots in the virtio ring,
+		 * re-establish memory reserved and open up tx queues.
+		 */
+		if (cfv->vq_tx->num_free <= cfv->watermark_tx)
+			continue;
+
+		/* Re-establish memory reserve */
+		if (cfv->reserved_mem == 0 && cfv->genpool)
+			cfv->reserved_mem =
+				gen_pool_alloc(cfv->genpool,
+					       cfv->reserved_size);
+
+		/* Open up the tx queues */
+		if (cfv->reserved_mem) {
+			cfv->watermark_tx =
+				virtqueue_get_vring_size(cfv->vq_tx);
+			netif_tx_wake_all_queues(cfv->ndev);
+			/* Buffers are recycled in cfv_netdev_tx, so
+			 * disable notifications when queues are opened.
+			 */
+			virtqueue_disable_cb(cfv->vq_tx);
+			++cfv->stats.tx_flow_on;
+		} else {
+			/* if no memory reserve, wait for more free slots */
+			WARN_ON(cfv->watermark_tx >
+			       virtqueue_get_vring_size(cfv->vq_tx));
+			cfv->watermark_tx +=
+				virtqueue_get_vring_size(cfv->vq_tx) / 4;
+		}
+	}
+}
+
+/* Allocate a SKB and copy packet data to it */
+static struct sk_buff *cfv_alloc_and_copy_skb(int *err,
+					      struct cfv_info *cfv,
+					      u8 *frm, u32 frm_len)
+{
+	struct sk_buff *skb;
+	u32 cfpkt_len, pad_len;
+
+	*err = 0;
+	/* Verify that packet size with down-link header and mtu size */
+	if (frm_len > cfv->mru || frm_len <= cfv->rx_hr + cfv->rx_tr) {
+		netdev_err(cfv->ndev,
+			   "Invalid frmlen:%u  mtu:%u hr:%d tr:%d\n",
+			   frm_len, cfv->mru,  cfv->rx_hr,
+			   cfv->rx_tr);
+		*err = -EPROTO;
+		return NULL;
+	}
+
+	cfpkt_len = frm_len - (cfv->rx_hr + cfv->rx_tr);
+	pad_len = (unsigned long)(frm + cfv->rx_hr) & (IP_HDR_ALIGN - 1);
+
+	skb = netdev_alloc_skb(cfv->ndev, frm_len + pad_len);
+	if (!skb) {
+		*err = -ENOMEM;
+		return NULL;
+	}
+
+	skb_reserve(skb, cfv->rx_hr + pad_len);
+
+	memcpy(skb_put(skb, cfpkt_len), frm + cfv->rx_hr, cfpkt_len);
+	return skb;
+}
+
+/* Get packets from the host vring */
+static int cfv_rx_poll(struct napi_struct *napi, int quota)
+{
+	struct cfv_info *cfv = container_of(napi, struct cfv_info, napi);
+	int rxcnt = 0;
+	int err = 0;
+	void *buf;
+	struct sk_buff *skb;
+	struct vringh_kiov *riov = &cfv->ctx.riov;
+	unsigned int skb_len;
+
+again:
+	do {
+		skb = NULL;
+
+		/* Put the previous iovec back on the used ring and
+		 * fetch a new iovec if we have processed all elements.
+		 */
+		if (riov->i == riov->used) {
+			if (cfv->ctx.head != USHRT_MAX) {
+				vringh_complete_kern(cfv->vr_rx,
+						     cfv->ctx.head,
+						     0);
+				cfv->ctx.head = USHRT_MAX;
+			}
+
+			err = vringh_getdesc_kern(
+				cfv->vr_rx,
+				riov,
+				NULL,
+				&cfv->ctx.head,
+				GFP_ATOMIC);
+
+			if (err <= 0)
+				goto exit;
+		}
+
+		buf = phys_to_virt((unsigned long) riov->iov[riov->i].iov_base);
+		/* TODO: Add check on valid buffer address */
+
+		skb = cfv_alloc_and_copy_skb(&err, cfv, buf,
+					     riov->iov[riov->i].iov_len);
+		if (unlikely(err))
+			goto exit;
+
+		/* Push received packet up the stack. */
+		skb_len = skb->len;
+		skb->protocol = htons(ETH_P_CAIF);
+		skb_reset_mac_header(skb);
+		skb->dev = cfv->ndev;
+		err = netif_receive_skb(skb);
+		if (unlikely(err)) {
+			++cfv->ndev->stats.rx_dropped;
+		} else {
+			++cfv->ndev->stats.rx_packets;
+			cfv->ndev->stats.rx_bytes += skb_len;
+		}
+
+		++riov->i;
+		++rxcnt;
+	} while (rxcnt < quota);
+
+	++cfv->stats.rx_napi_resched;
+	goto out;
+
+exit:
+	switch (err) {
+	case 0:
+		++cfv->stats.rx_napi_complete;
+
+		/* Really out of patckets? (stolen from virtio_net)*/
+		napi_complete(napi);
+		if (unlikely(!vringh_notify_enable_kern(cfv->vr_rx)) &&
+		    napi_schedule_prep(napi)) {
+			vringh_notify_disable_kern(cfv->vr_rx);
+			__napi_schedule(napi);
+			goto again;
+		}
+		break;
+
+	case -ENOMEM:
+		++cfv->stats.rx_nomem;
+		dev_kfree_skb(skb);
+		/* Stop NAPI poll on OOM, we hope to be polled later */
+		napi_complete(napi);
+		vringh_notify_enable_kern(cfv->vr_rx);
+		break;
+
+	default:
+		/* We're doomed, any modem fault is fatal */
+		netdev_warn(cfv->ndev, "Bad ring, disable device\n");
+		cfv->ndev->stats.rx_dropped = riov->used - riov->i;
+		napi_complete(napi);
+		vringh_notify_disable_kern(cfv->vr_rx);
+		netif_carrier_off(cfv->ndev);
+		break;
+	}
+out:
+	if (rxcnt && vringh_need_notify_kern(cfv->vr_rx) > 0)
+		vringh_notify(cfv->vr_rx);
+	return rxcnt;
+}
+
+static void cfv_recv(struct virtio_device *vdev, struct vringh *vr_rx)
+{
+	struct cfv_info *cfv = vdev->priv;
+
+	++cfv->stats.rx_kicks;
+	vringh_notify_disable_kern(cfv->vr_rx);
+	napi_schedule(&cfv->napi);
+}
+
+static void cfv_destroy_genpool(struct cfv_info *cfv)
+{
+	if (cfv->alloc_addr)
+		dma_free_coherent(cfv->vdev->dev.parent->parent,
+				  cfv->allocsz, cfv->alloc_addr,
+				  cfv->alloc_dma);
+
+	if (!cfv->genpool)
+		return;
+	gen_pool_free(cfv->genpool,  cfv->reserved_mem,
+		      cfv->reserved_size);
+	gen_pool_destroy(cfv->genpool);
+	cfv->genpool = NULL;
+}
+
+static int cfv_create_genpool(struct cfv_info *cfv)
+{
+	int err;
+
+	/* dma_alloc can only allocate whole pages, and we need a more
+	 * fine graned allocation so we use genpool. We ask for space needed
+	 * by IP and a full ring. If the dma allcoation fails we retry with a
+	 * smaller allocation size.
+	 */
+	err = -ENOMEM;
+	cfv->allocsz = (virtqueue_get_vring_size(cfv->vq_tx) *
+			(ETH_DATA_LEN + cfv->tx_hr + cfv->tx_tr) * 11)/10;
+	if (cfv->allocsz <= (num_possible_cpus() + 1) * cfv->ndev->mtu)
+		return -EINVAL;
+
+	for (;;) {
+		if (cfv->allocsz <= num_possible_cpus() * cfv->ndev->mtu) {
+			netdev_info(cfv->ndev, "Not enough device memory\n");
+			return -ENOMEM;
+		}
+
+		cfv->alloc_addr = dma_alloc_coherent(
+						cfv->vdev->dev.parent->parent,
+						cfv->allocsz, &cfv->alloc_dma,
+						GFP_ATOMIC);
+		if (cfv->alloc_addr)
+			break;
+
+		cfv->allocsz = (cfv->allocsz * 3) >> 2;
+	}
+
+	netdev_dbg(cfv->ndev, "Allocated %zd bytes from dma-memory\n",
+		   cfv->allocsz);
+
+	/* Allocate on 128 bytes boundaries (1 << 7)*/
+	cfv->genpool = gen_pool_create(7, -1);
+	if (!cfv->genpool)
+		goto err;
+
+	err = gen_pool_add_virt(cfv->genpool, (unsigned long)cfv->alloc_addr,
+				(phys_addr_t)virt_to_phys(cfv->alloc_addr),
+				cfv->allocsz, -1);
+	if (err)
+		goto err;
+
+	/* Reserve some memory for low memory situations. If we hit the roof
+	 * in the memory pool, we stop TX flow and release the reserve.
+	 */
+	cfv->reserved_size = num_possible_cpus() * cfv->ndev->mtu;
+	cfv->reserved_mem = gen_pool_alloc(cfv->genpool,
+					   cfv->reserved_size);
+	if (!cfv->reserved_mem) {
+		err = -ENOMEM;
+		goto err;
+	}
+
+	cfv->watermark_tx = virtqueue_get_vring_size(cfv->vq_tx);
+	return 0;
+err:
+	cfv_destroy_genpool(cfv);
+	return err;
+}
+
+/* Enable the CAIF interface and allocate the memory-pool */
+static int cfv_netdev_open(struct net_device *netdev)
+{
+	struct cfv_info *cfv = netdev_priv(netdev);
+
+	if (cfv_create_genpool(cfv))
+		return -ENOMEM;
+
+	netif_carrier_on(netdev);
+	napi_enable(&cfv->napi);
+
+	/* Schedule NAPI to read any pending packets */
+	napi_schedule(&cfv->napi);
+	return 0;
+}
+
+/* Disable the CAIF interface and free the memory-pool */
+static int cfv_netdev_close(struct net_device *netdev)
+{
+	struct cfv_info *cfv = netdev_priv(netdev);
+	unsigned long flags;
+	struct buf_info *buf_info;
+
+	/* Disable interrupts, queues and NAPI polling */
+	netif_carrier_off(netdev);
+	virtqueue_disable_cb(cfv->vq_tx);
+	vringh_notify_disable_kern(cfv->vr_rx);
+	napi_disable(&cfv->napi);
+
+	/* Release any TX buffers on both used and avilable rings */
+	cfv_release_used_buf(cfv->vq_tx);
+	spin_lock_irqsave(&cfv->tx_lock, flags);
+	while ((buf_info = virtqueue_detach_unused_buf(cfv->vq_tx)))
+		free_buf_info(cfv, buf_info);
+	spin_unlock_irqrestore(&cfv->tx_lock, flags);
+
+	/* Release all dma allocated memory and destroy the pool */
+	cfv_destroy_genpool(cfv);
+	return 0;
+}
+
+/* Allocate a buffer in dma-memory and copy skb to it */
+static struct buf_info *cfv_alloc_and_copy_to_shm(struct cfv_info *cfv,
+						       struct sk_buff *skb,
+						       struct scatterlist *sg)
+{
+	struct caif_payload_info *info = (void *)&skb->cb;
+	struct buf_info *buf_info = NULL;
+	u8 pad_len, hdr_ofs;
+
+	if (!cfv->genpool)
+		goto err;
+
+	if (unlikely(cfv->tx_hr + skb->len + cfv->tx_tr > cfv->mtu)) {
+		netdev_warn(cfv->ndev, "Invalid packet len (%d > %d)\n",
+			    cfv->tx_hr + skb->len + cfv->tx_tr, cfv->mtu);
+		goto err;
+	}
+
+	buf_info = kmalloc(sizeof(struct buf_info), GFP_ATOMIC);
+	if (unlikely(!buf_info))
+		goto err;
+
+	/* Make the IP header aligned in tbe buffer */
+	hdr_ofs = cfv->tx_hr + info->hdr_len;
+	pad_len = hdr_ofs & (IP_HDR_ALIGN - 1);
+	buf_info->size = cfv->tx_hr + skb->len + cfv->tx_tr + pad_len;
+
+	/* allocate dma memory buffer */
+	buf_info->vaddr = (void *)gen_pool_alloc(cfv->genpool, buf_info->size);
+	if (unlikely(!buf_info->vaddr))
+		goto err;
+
+	/* copy skbuf contents to send buffer */
+	skb_copy_bits(skb, 0, buf_info->vaddr + cfv->tx_hr + pad_len, skb->len);
+	sg_init_one(sg, buf_info->vaddr + pad_len,
+		    skb->len + cfv->tx_hr + cfv->rx_hr);
+
+	return buf_info;
+err:
+	kfree(buf_info);
+	return NULL;
+}
+
+/* Put the CAIF packet on the virtio ring and kick the receiver */
+static int cfv_netdev_tx(struct sk_buff *skb, struct net_device *netdev)
+{
+	struct cfv_info *cfv = netdev_priv(netdev);
+	struct buf_info *buf_info;
+	struct scatterlist sg;
+	unsigned long flags;
+	bool flow_off = false;
+	int ret;
+
+	/* garbage collect released buffers */
+	cfv_release_used_buf(cfv->vq_tx);
+	spin_lock_irqsave(&cfv->tx_lock, flags);
+
+	/* Flow-off check takes into account number of cpus to make sure
+	 * virtqueue will not be overfilled in any possible smp conditions.
+	 *
+	 * Flow-on is triggered when sufficient buffers are freed
+	 */
+	if (unlikely(cfv->vq_tx->num_free <= num_present_cpus())) {
+		flow_off = true;
+		cfv->stats.tx_full_ring++;
+	}
+
+	/* If we run out of memory, we release the memory reserve and retry
+	 * allocation.
+	 */
+	buf_info = cfv_alloc_and_copy_to_shm(cfv, skb, &sg);
+	if (unlikely(!buf_info)) {
+		cfv->stats.tx_no_mem++;
+		flow_off = true;
+
+		if (cfv->reserved_mem && cfv->genpool) {
+			gen_pool_free(cfv->genpool,  cfv->reserved_mem,
+				      cfv->reserved_size);
+			cfv->reserved_mem = 0;
+			buf_info = cfv_alloc_and_copy_to_shm(cfv, skb, &sg);
+		}
+	}
+
+	if (unlikely(flow_off)) {
+		/* Turn flow on when a 1/4 of the descriptors are released */
+		cfv->watermark_tx = virtqueue_get_vring_size(cfv->vq_tx) / 4;
+		/* Enable notifications of recycled TX buffers */
+		virtqueue_enable_cb(cfv->vq_tx);
+		netif_tx_stop_all_queues(netdev);
+	}
+
+	if (unlikely(!buf_info)) {
+		/* If the memory reserve does it's job, this shouldn't happen */
+		netdev_warn(cfv->ndev, "Out of gen_pool memory\n");
+		goto err;
+	}
+
+	ret = virtqueue_add_outbuf(cfv->vq_tx, &sg, 1, buf_info, GFP_ATOMIC);
+	if (unlikely((ret < 0))) {
+		/* If flow control works, this shouldn't happen */
+		netdev_warn(cfv->ndev, "Failed adding buffer to TX vring:%d\n",
+			    ret);
+		goto err;
+	}
+
+	/* update netdev statistics */
+	cfv->ndev->stats.tx_packets++;
+	cfv->ndev->stats.tx_bytes += skb->len;
+	spin_unlock_irqrestore(&cfv->tx_lock, flags);
+
+	/* tell the remote processor it has a pending message to read */
+	virtqueue_kick(cfv->vq_tx);
+
+	dev_kfree_skb(skb);
+	return NETDEV_TX_OK;
+err:
+	spin_unlock_irqrestore(&cfv->tx_lock, flags);
+	cfv->ndev->stats.tx_dropped++;
+	free_buf_info(cfv, buf_info);
+	dev_kfree_skb(skb);
+	return NETDEV_TX_OK;
+}
+
+static void cfv_tx_release_tasklet(unsigned long drv)
+{
+	struct cfv_info *cfv = (struct cfv_info *)drv;
+	cfv_release_used_buf(cfv->vq_tx);
+}
+
+static const struct net_device_ops cfv_netdev_ops = {
+	.ndo_open = cfv_netdev_open,
+	.ndo_stop = cfv_netdev_close,
+	.ndo_start_xmit = cfv_netdev_tx,
+};
+
+static void cfv_netdev_setup(struct net_device *netdev)
+{
+	netdev->netdev_ops = &cfv_netdev_ops;
+	netdev->type = ARPHRD_CAIF;
+	netdev->tx_queue_len = 100;
+	netdev->flags = IFF_POINTOPOINT | IFF_NOARP;
+	netdev->mtu = CFV_DEF_MTU_SIZE;
+	netdev->destructor = free_netdev;
+}
+
+/* Create debugfs counters for the device */
+static inline void debugfs_init(struct cfv_info *cfv)
+{
+	cfv->debugfs =
+		debugfs_create_dir(netdev_name(cfv->ndev), NULL);
+
+	if (IS_ERR(cfv->debugfs))
+		return;
+
+	debugfs_create_u32("rx-napi-complete", S_IRUSR, cfv->debugfs,
+			   &cfv->stats.rx_napi_complete);
+	debugfs_create_u32("rx-napi-resched", S_IRUSR, cfv->debugfs,
+			   &cfv->stats.rx_napi_resched);
+	debugfs_create_u32("rx-nomem", S_IRUSR, cfv->debugfs,
+			   &cfv->stats.rx_nomem);
+	debugfs_create_u32("rx-kicks", S_IRUSR, cfv->debugfs,
+			   &cfv->stats.rx_kicks);
+	debugfs_create_u32("tx-full-ring", S_IRUSR, cfv->debugfs,
+			   &cfv->stats.tx_full_ring);
+	debugfs_create_u32("tx-no-mem", S_IRUSR, cfv->debugfs,
+			   &cfv->stats.tx_no_mem);
+	debugfs_create_u32("tx-kicks", S_IRUSR, cfv->debugfs,
+			   &cfv->stats.tx_kicks);
+	debugfs_create_u32("tx-flow-on", S_IRUSR, cfv->debugfs,
+			   &cfv->stats.tx_flow_on);
+}
+
+/* Setup CAIF for the a virtio device */
+static int cfv_probe(struct virtio_device *vdev)
+{
+	vq_callback_t *vq_cbs = cfv_release_cb;
+	vrh_callback_t *vrh_cbs = cfv_recv;
+	const char *names =  "output";
+	const char *cfv_netdev_name = "cfvrt";
+	struct net_device *netdev;
+	struct cfv_info *cfv;
+	int err = -EINVAL;
+
+	netdev = alloc_netdev(sizeof(struct cfv_info), cfv_netdev_name,
+			      cfv_netdev_setup);
+	if (!netdev)
+		return -ENOMEM;
+
+	cfv = netdev_priv(netdev);
+	cfv->vdev = vdev;
+	cfv->ndev = netdev;
+
+	spin_lock_init(&cfv->tx_lock);
+
+	/* Get the RX virtio ring. This is a "host side vring". */
+	err = -ENODEV;
+	if (!vdev->vringh_config || !vdev->vringh_config->find_vrhs)
+		goto err;
+
+	err = vdev->vringh_config->find_vrhs(vdev, 1, &cfv->vr_rx, &vrh_cbs);
+	if (err)
+		goto err;
+
+	/* Get the TX virtio ring. This is a "guest side vring". */
+	err = vdev->config->find_vqs(vdev, 1, &cfv->vq_tx, &vq_cbs, &names);
+	if (err)
+		goto err;
+
+	/* Get the CAIF configuration from virtio config space, if available */
+#define GET_VIRTIO_CONFIG_OPS(_v, _var, _f) \
+	((_v)->config->get(_v, offsetof(struct virtio_caif_transf_config, _f), \
+			   &_var, \
+			   FIELD_SIZEOF(struct virtio_caif_transf_config, _f)))
+
+	if (vdev->config->get) {
+		GET_VIRTIO_CONFIG_OPS(vdev, cfv->tx_hr, headroom);
+		GET_VIRTIO_CONFIG_OPS(vdev, cfv->rx_hr, headroom);
+		GET_VIRTIO_CONFIG_OPS(vdev, cfv->tx_tr, tailroom);
+		GET_VIRTIO_CONFIG_OPS(vdev, cfv->rx_tr, tailroom);
+		GET_VIRTIO_CONFIG_OPS(vdev, cfv->mtu, mtu);
+		GET_VIRTIO_CONFIG_OPS(vdev, cfv->mru, mtu);
+	} else {
+		cfv->tx_hr = CFV_DEF_HEADROOM;
+		cfv->rx_hr = CFV_DEF_HEADROOM;
+		cfv->tx_tr = CFV_DEF_TAILROOM;
+		cfv->rx_tr = CFV_DEF_TAILROOM;
+		cfv->mtu = CFV_DEF_MTU_SIZE;
+		cfv->mru = CFV_DEF_MTU_SIZE;
+	}
+
+	netdev->needed_headroom = cfv->tx_hr;
+	netdev->needed_tailroom = cfv->tx_tr;
+
+	/* Disable buffer release interrupts unless we have stopped TX queues */
+	virtqueue_disable_cb(cfv->vq_tx);
+
+	netdev->mtu = cfv->mtu - cfv->tx_tr;
+	vdev->priv = cfv;
+
+	/* Initialize NAPI poll context data */
+	vringh_kiov_init(&cfv->ctx.riov, NULL, 0);
+	cfv->ctx.head = USHRT_MAX;
+	netif_napi_add(netdev, &cfv->napi, cfv_rx_poll, CFV_DEFAULT_QUOTA);
+
+	tasklet_init(&cfv->tx_release_tasklet,
+		     cfv_tx_release_tasklet,
+		     (unsigned long)cfv);
+
+	/* Carrier is off until netdevice is opened */
+	netif_carrier_off(netdev);
+
+	/* register Netdev */
+	err = register_netdev(netdev);
+	if (err) {
+		dev_err(&vdev->dev, "Unable to register netdev (%d)\n", err);
+		goto err;
+	}
+
+	debugfs_init(cfv);
+
+	return 0;
+err:
+	netdev_warn(cfv->ndev, "CAIF Virtio probe failed:%d\n", err);
+
+	if (cfv->vr_rx)
+		vdev->vringh_config->del_vrhs(cfv->vdev);
+	if (cfv->vdev)
+		vdev->config->del_vqs(cfv->vdev);
+	free_netdev(netdev);
+	return err;
+}
+
+static void cfv_remove(struct virtio_device *vdev)
+{
+	struct cfv_info *cfv = vdev->priv;
+
+	rtnl_lock();
+	dev_close(cfv->ndev);
+	rtnl_unlock();
+
+	tasklet_kill(&cfv->tx_release_tasklet);
+	debugfs_remove_recursive(cfv->debugfs);
+
+	vringh_kiov_cleanup(&cfv->ctx.riov);
+	vdev->config->reset(vdev);
+	vdev->vringh_config->del_vrhs(cfv->vdev);
+	cfv->vr_rx = NULL;
+	vdev->config->del_vqs(cfv->vdev);
+	unregister_netdev(cfv->ndev);
+}
+
+static struct virtio_device_id id_table[] = {
+	{ VIRTIO_ID_CAIF, VIRTIO_DEV_ANY_ID },
+	{ 0 },
+};
+
+static unsigned int features[] = {
+};
+
+static struct virtio_driver caif_virtio_driver = {
+	.feature_table		= features,
+	.feature_table_size	= ARRAY_SIZE(features),
+	.driver.name		= KBUILD_MODNAME,
+	.driver.owner		= THIS_MODULE,
+	.id_table		= id_table,
+	.probe			= cfv_probe,
+	.remove			= cfv_remove,
+};
+
+module_virtio_driver(caif_virtio_driver);
+MODULE_DEVICE_TABLE(virtio, id_table);
diff --git a/drivers/net/can/Kconfig b/drivers/net/can/Kconfig
index 9862b2e07644..e456b70933c2 100644
--- a/drivers/net/can/Kconfig
+++ b/drivers/net/can/Kconfig
@@ -65,7 +65,7 @@ config CAN_LEDS
 
 config CAN_AT91
 	tristate "Atmel AT91 onchip CAN controller"
-	depends on ARCH_AT91SAM9263 || ARCH_AT91SAM9X5
+	depends on ARM
 	---help---
 	  This is a driver for the SoC CAN controller in Atmel's AT91SAM9263
 	  and AT91SAM9X5 processors.
diff --git a/drivers/net/can/at91_can.c b/drivers/net/can/at91_can.c
index 44f363792b59..db52f4414def 100644
--- a/drivers/net/can/at91_can.c
+++ b/drivers/net/can/at91_can.c
@@ -27,6 +27,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/netdevice.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/rtnetlink.h>
 #include <linux/skbuff.h>
@@ -155,19 +156,20 @@ struct at91_priv {
 	canid_t mb0_id;
 };
 
-static const struct at91_devtype_data at91_devtype_data[] = {
-	[AT91_DEVTYPE_SAM9263] = {
-		.rx_first = 1,
-		.rx_split = 8,
-		.rx_last = 11,
-		.tx_shift = 2,
-	},
-	[AT91_DEVTYPE_SAM9X5] = {
-		.rx_first = 0,
-		.rx_split = 4,
-		.rx_last = 5,
-		.tx_shift = 1,
-	},
+static const struct at91_devtype_data at91_at91sam9263_data = {
+	.rx_first = 1,
+	.rx_split = 8,
+	.rx_last = 11,
+	.tx_shift = 2,
+	.type = AT91_DEVTYPE_SAM9263,
+};
+
+static const struct at91_devtype_data at91_at91sam9x5_data = {
+	.rx_first = 0,
+	.rx_split = 4,
+	.rx_last = 5,
+	.tx_shift = 1,
+	.type = AT91_DEVTYPE_SAM9X5,
 };
 
 static const struct can_bittiming_const at91_bittiming_const = {
@@ -1249,10 +1251,42 @@ static struct attribute_group at91_sysfs_attr_group = {
 	.attrs = at91_sysfs_attrs,
 };
 
+#if defined(CONFIG_OF)
+static const struct of_device_id at91_can_dt_ids[] = {
+	{
+		.compatible = "atmel,at91sam9x5-can",
+		.data = &at91_at91sam9x5_data,
+	}, {
+		.compatible = "atmel,at91sam9263-can",
+		.data = &at91_at91sam9263_data,
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(of, at91_can_dt_ids);
+#else
+#define at91_can_dt_ids NULL
+#endif
+
+static const struct at91_devtype_data *at91_can_get_driver_data(struct platform_device *pdev)
+{
+	if (pdev->dev.of_node) {
+		const struct of_device_id *match;
+
+		match = of_match_node(at91_can_dt_ids, pdev->dev.of_node);
+		if (!match) {
+			dev_err(&pdev->dev, "no matching node found in dtb\n");
+			return NULL;
+		}
+		return (const struct at91_devtype_data *)match->data;
+	}
+	return (const struct at91_devtype_data *)
+		platform_get_device_id(pdev)->driver_data;
+}
+
 static int at91_can_probe(struct platform_device *pdev)
 {
 	const struct at91_devtype_data *devtype_data;
-	enum at91_devtype devtype;
 	struct net_device *dev;
 	struct at91_priv *priv;
 	struct resource *res;
@@ -1260,8 +1294,12 @@ static int at91_can_probe(struct platform_device *pdev)
 	void __iomem *addr;
 	int err, irq;
 
-	devtype = pdev->id_entry->driver_data;
-	devtype_data = &at91_devtype_data[devtype];
+	devtype_data = at91_can_get_driver_data(pdev);
+	if (!devtype_data) {
+		dev_err(&pdev->dev, "no driver data\n");
+		err = -ENODEV;
+		goto exit;
+	}
 
 	clk = clk_get(&pdev->dev, "can_clk");
 	if (IS_ERR(clk)) {
@@ -1310,7 +1348,6 @@ static int at91_can_probe(struct platform_device *pdev)
 	priv->dev = dev;
 	priv->reg_base = addr;
 	priv->devtype_data = *devtype_data;
-	priv->devtype_data.type = devtype;
 	priv->clk = clk;
 	priv->pdata = pdev->dev.platform_data;
 	priv->mb0_id = 0x7ff;
@@ -1373,10 +1410,10 @@ static int at91_can_remove(struct platform_device *pdev)
 static const struct platform_device_id at91_can_id_table[] = {
 	{
 		.name = "at91_can",
-		.driver_data = AT91_DEVTYPE_SAM9263,
+		.driver_data = (kernel_ulong_t)&at91_at91sam9x5_data,
 	}, {
 		.name = "at91sam9x5_can",
-		.driver_data = AT91_DEVTYPE_SAM9X5,
+		.driver_data = (kernel_ulong_t)&at91_at91sam9263_data,
 	}, {
 		/* sentinel */
 	}
@@ -1389,6 +1426,7 @@ static struct platform_driver at91_can_driver = {
 	.driver = {
 		.name = KBUILD_MODNAME,
 		.owner = THIS_MODULE,
+		.of_match_table = at91_can_dt_ids,
 	},
 	.id_table = at91_can_id_table,
 };
diff --git a/drivers/net/can/bfin_can.c b/drivers/net/can/bfin_can.c
index 6a0532176b69..d4a15e82bfc0 100644
--- a/drivers/net/can/bfin_can.c
+++ b/drivers/net/can/bfin_can.c
@@ -412,7 +412,7 @@ static int bfin_can_err(struct net_device *dev, u16 isrc, u16 status)
 	return 0;
 }
 
-irqreturn_t bfin_can_interrupt(int irq, void *dev_id)
+static irqreturn_t bfin_can_interrupt(int irq, void *dev_id)
 {
 	struct net_device *dev = dev_id;
 	struct bfin_can_priv *priv = netdev_priv(dev);
@@ -504,7 +504,7 @@ static int bfin_can_close(struct net_device *dev)
 	return 0;
 }
 
-struct net_device *alloc_bfin_candev(void)
+static struct net_device *alloc_bfin_candev(void)
 {
 	struct net_device *dev;
 	struct bfin_can_priv *priv;
diff --git a/drivers/net/can/mcp251x.c b/drivers/net/can/mcp251x.c
index 9aa0c64c33c8..8cda23bf0614 100644
--- a/drivers/net/can/mcp251x.c
+++ b/drivers/net/can/mcp251x.c
@@ -269,7 +269,7 @@ struct mcp251x_priv {
 #define MCP251X_IS(_model) \
 static inline int mcp251x_is_##_model(struct spi_device *spi) \
 { \
-	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev); \
+	struct mcp251x_priv *priv = spi_get_drvdata(spi); \
 	return priv->model == CAN_MCP251X_MCP##_model; \
 }
 
@@ -305,7 +305,7 @@ static void mcp251x_clean(struct net_device *net)
  */
 static int mcp251x_spi_trans(struct spi_device *spi, int len)
 {
-	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct mcp251x_priv *priv = spi_get_drvdata(spi);
 	struct spi_transfer t = {
 		.tx_buf = priv->spi_tx_buf,
 		.rx_buf = priv->spi_rx_buf,
@@ -333,7 +333,7 @@ static int mcp251x_spi_trans(struct spi_device *spi, int len)
 
 static u8 mcp251x_read_reg(struct spi_device *spi, uint8_t reg)
 {
-	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct mcp251x_priv *priv = spi_get_drvdata(spi);
 	u8 val = 0;
 
 	priv->spi_tx_buf[0] = INSTRUCTION_READ;
@@ -348,7 +348,7 @@ static u8 mcp251x_read_reg(struct spi_device *spi, uint8_t reg)
 static void mcp251x_read_2regs(struct spi_device *spi, uint8_t reg,
 		uint8_t *v1, uint8_t *v2)
 {
-	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct mcp251x_priv *priv = spi_get_drvdata(spi);
 
 	priv->spi_tx_buf[0] = INSTRUCTION_READ;
 	priv->spi_tx_buf[1] = reg;
@@ -361,7 +361,7 @@ static void mcp251x_read_2regs(struct spi_device *spi, uint8_t reg,
 
 static void mcp251x_write_reg(struct spi_device *spi, u8 reg, uint8_t val)
 {
-	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct mcp251x_priv *priv = spi_get_drvdata(spi);
 
 	priv->spi_tx_buf[0] = INSTRUCTION_WRITE;
 	priv->spi_tx_buf[1] = reg;
@@ -373,7 +373,7 @@ static void mcp251x_write_reg(struct spi_device *spi, u8 reg, uint8_t val)
 static void mcp251x_write_bits(struct spi_device *spi, u8 reg,
 			       u8 mask, uint8_t val)
 {
-	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct mcp251x_priv *priv = spi_get_drvdata(spi);
 
 	priv->spi_tx_buf[0] = INSTRUCTION_BIT_MODIFY;
 	priv->spi_tx_buf[1] = reg;
@@ -386,7 +386,7 @@ static void mcp251x_write_bits(struct spi_device *spi, u8 reg,
 static void mcp251x_hw_tx_frame(struct spi_device *spi, u8 *buf,
 				int len, int tx_buf_idx)
 {
-	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct mcp251x_priv *priv = spi_get_drvdata(spi);
 
 	if (mcp251x_is_2510(spi)) {
 		int i;
@@ -403,7 +403,7 @@ static void mcp251x_hw_tx_frame(struct spi_device *spi, u8 *buf,
 static void mcp251x_hw_tx(struct spi_device *spi, struct can_frame *frame,
 			  int tx_buf_idx)
 {
-	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct mcp251x_priv *priv = spi_get_drvdata(spi);
 	u32 sid, eid, exide, rtr;
 	u8 buf[SPI_TRANSFER_BUF_LEN];
 
@@ -434,7 +434,7 @@ static void mcp251x_hw_tx(struct spi_device *spi, struct can_frame *frame,
 static void mcp251x_hw_rx_frame(struct spi_device *spi, u8 *buf,
 				int buf_idx)
 {
-	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct mcp251x_priv *priv = spi_get_drvdata(spi);
 
 	if (mcp251x_is_2510(spi)) {
 		int i, len;
@@ -454,7 +454,7 @@ static void mcp251x_hw_rx_frame(struct spi_device *spi, u8 *buf,
 
 static void mcp251x_hw_rx(struct spi_device *spi, int buf_idx)
 {
-	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct mcp251x_priv *priv = spi_get_drvdata(spi);
 	struct sk_buff *skb;
 	struct can_frame *frame;
 	u8 buf[SPI_TRANSFER_BUF_LEN];
@@ -550,7 +550,7 @@ static int mcp251x_do_set_mode(struct net_device *net, enum can_mode mode)
 
 static int mcp251x_set_normal_mode(struct spi_device *spi)
 {
-	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct mcp251x_priv *priv = spi_get_drvdata(spi);
 	unsigned long timeout;
 
 	/* Enable interrupts */
@@ -620,7 +620,7 @@ static int mcp251x_setup(struct net_device *net, struct mcp251x_priv *priv,
 
 static int mcp251x_hw_reset(struct spi_device *spi)
 {
-	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct mcp251x_priv *priv = spi_get_drvdata(spi);
 	int ret;
 	unsigned long timeout;
 
@@ -1026,7 +1026,7 @@ static int mcp251x_can_probe(struct spi_device *spi)
 		CAN_CTRLMODE_LOOPBACK | CAN_CTRLMODE_LISTENONLY;
 	priv->model = spi_get_device_id(spi)->driver_data;
 	priv->net = net;
-	dev_set_drvdata(&spi->dev, priv);
+	spi_set_drvdata(spi, priv);
 
 	priv->spi = spi;
 	mutex_init(&priv->mcp_lock);
@@ -1124,7 +1124,7 @@ error_out:
 static int mcp251x_can_remove(struct spi_device *spi)
 {
 	struct mcp251x_platform_data *pdata = spi->dev.platform_data;
-	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct mcp251x_priv *priv = spi_get_drvdata(spi);
 	struct net_device *net = priv->net;
 
 	unregister_candev(net);
@@ -1144,11 +1144,13 @@ static int mcp251x_can_remove(struct spi_device *spi)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int mcp251x_can_suspend(struct spi_device *spi, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+
+static int mcp251x_can_suspend(struct device *dev)
 {
+	struct spi_device *spi = to_spi_device(dev);
 	struct mcp251x_platform_data *pdata = spi->dev.platform_data;
-	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct mcp251x_priv *priv = spi_get_drvdata(spi);
 	struct net_device *net = priv->net;
 
 	priv->force_quit = 1;
@@ -1176,10 +1178,11 @@ static int mcp251x_can_suspend(struct spi_device *spi, pm_message_t state)
 	return 0;
 }
 
-static int mcp251x_can_resume(struct spi_device *spi)
+static int mcp251x_can_resume(struct device *dev)
 {
+	struct spi_device *spi = to_spi_device(dev);
 	struct mcp251x_platform_data *pdata = spi->dev.platform_data;
-	struct mcp251x_priv *priv = dev_get_drvdata(&spi->dev);
+	struct mcp251x_priv *priv = spi_get_drvdata(spi);
 
 	if (priv->after_suspend & AFTER_SUSPEND_POWER) {
 		pdata->power_enable(1);
@@ -1197,11 +1200,11 @@ static int mcp251x_can_resume(struct spi_device *spi)
 	enable_irq(spi->irq);
 	return 0;
 }
-#else
-#define mcp251x_can_suspend NULL
-#define mcp251x_can_resume NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(mcp251x_can_pm_ops, mcp251x_can_suspend,
+	mcp251x_can_resume);
+
 static const struct spi_device_id mcp251x_id_table[] = {
 	{ "mcp2510",	CAN_MCP251X_MCP2510 },
 	{ "mcp2515",	CAN_MCP251X_MCP2515 },
@@ -1213,29 +1216,15 @@ MODULE_DEVICE_TABLE(spi, mcp251x_id_table);
 static struct spi_driver mcp251x_can_driver = {
 	.driver = {
 		.name = DEVICE_NAME,
-		.bus = &spi_bus_type,
 		.owner = THIS_MODULE,
+		.pm = &mcp251x_can_pm_ops,
 	},
 
 	.id_table = mcp251x_id_table,
 	.probe = mcp251x_can_probe,
 	.remove = mcp251x_can_remove,
-	.suspend = mcp251x_can_suspend,
-	.resume = mcp251x_can_resume,
 };
-
-static int __init mcp251x_can_init(void)
-{
-	return spi_register_driver(&mcp251x_can_driver);
-}
-
-static void __exit mcp251x_can_exit(void)
-{
-	spi_unregister_driver(&mcp251x_can_driver);
-}
-
-module_init(mcp251x_can_init);
-module_exit(mcp251x_can_exit);
+module_spi_driver(mcp251x_can_driver);
 
 MODULE_AUTHOR("Chris Elston <celston@katalix.com>, "
 	      "Christian Pellegrin <chripell@evolware.org>");
diff --git a/drivers/net/can/sja1000/ems_pci.c b/drivers/net/can/sja1000/ems_pci.c
index 36d298da2af6..3752342a678a 100644
--- a/drivers/net/can/sja1000/ems_pci.c
+++ b/drivers/net/can/sja1000/ems_pci.c
@@ -168,12 +168,12 @@ static inline int ems_pci_check_chan(const struct sja1000_priv *priv)
 	unsigned char res;
 
 	/* Make sure SJA1000 is in reset mode */
-	priv->write_reg(priv, REG_MOD, 1);
+	priv->write_reg(priv, SJA1000_MOD, 1);
 
-	priv->write_reg(priv, REG_CDR, CDR_PELICAN);
+	priv->write_reg(priv, SJA1000_CDR, CDR_PELICAN);
 
 	/* read reset-values */
-	res = priv->read_reg(priv, REG_CDR);
+	res = priv->read_reg(priv, SJA1000_CDR);
 
 	if (res == CDR_PELICAN)
 		return 1;
diff --git a/drivers/net/can/sja1000/ems_pcmcia.c b/drivers/net/can/sja1000/ems_pcmcia.c
index 321c27e1c7fc..9e535f2ef52b 100644
--- a/drivers/net/can/sja1000/ems_pcmcia.c
+++ b/drivers/net/can/sja1000/ems_pcmcia.c
@@ -126,11 +126,11 @@ static irqreturn_t ems_pcmcia_interrupt(int irq, void *dev_id)
 static inline int ems_pcmcia_check_chan(struct sja1000_priv *priv)
 {
 	/* Make sure SJA1000 is in reset mode */
-	ems_pcmcia_write_reg(priv, REG_MOD, 1);
-	ems_pcmcia_write_reg(priv, REG_CDR, CDR_PELICAN);
+	ems_pcmcia_write_reg(priv, SJA1000_MOD, 1);
+	ems_pcmcia_write_reg(priv, SJA1000_CDR, CDR_PELICAN);
 
 	/* read reset-values */
-	if (ems_pcmcia_read_reg(priv, REG_CDR) == CDR_PELICAN)
+	if (ems_pcmcia_read_reg(priv, SJA1000_CDR) == CDR_PELICAN)
 		return 1;
 
 	return 0;
diff --git a/drivers/net/can/sja1000/kvaser_pci.c b/drivers/net/can/sja1000/kvaser_pci.c
index 37b0381f532e..217585b97cd3 100644
--- a/drivers/net/can/sja1000/kvaser_pci.c
+++ b/drivers/net/can/sja1000/kvaser_pci.c
@@ -159,9 +159,9 @@ static int number_of_sja1000_chip(void __iomem *base_addr)
 	for (i = 0; i < MAX_NO_OF_CHANNELS; i++) {
 		/* reset chip */
 		iowrite8(MOD_RM, base_addr +
-			 (i * KVASER_PCI_PORT_BYTES) + REG_MOD);
+			 (i * KVASER_PCI_PORT_BYTES) + SJA1000_MOD);
 		status = ioread8(base_addr +
-				 (i * KVASER_PCI_PORT_BYTES) + REG_MOD);
+				 (i * KVASER_PCI_PORT_BYTES) + SJA1000_MOD);
 		/* check reset bit */
 		if (!(status & MOD_RM))
 			break;
diff --git a/drivers/net/can/sja1000/peak_pci.c b/drivers/net/can/sja1000/peak_pci.c
index d1e7f1006ddd..6b6f0ad75090 100644
--- a/drivers/net/can/sja1000/peak_pci.c
+++ b/drivers/net/can/sja1000/peak_pci.c
@@ -402,7 +402,7 @@ static void peak_pciec_write_reg(const struct sja1000_priv *priv,
 	int c = (priv->reg_base - card->reg_base) / PEAK_PCI_CHAN_SIZE;
 
 	/* sja1000 register changes control the leds state */
-	if (port == REG_MOD)
+	if (port == SJA1000_MOD)
 		switch (val) {
 		case MOD_RM:
 			/* Reset Mode: set led on */
diff --git a/drivers/net/can/sja1000/peak_pcmcia.c b/drivers/net/can/sja1000/peak_pcmcia.c
index 0a707f70661c..f7ad754dd2aa 100644
--- a/drivers/net/can/sja1000/peak_pcmcia.c
+++ b/drivers/net/can/sja1000/peak_pcmcia.c
@@ -196,7 +196,7 @@ static void pcan_write_canreg(const struct sja1000_priv *priv, int port, u8 v)
 	int c = (priv->reg_base - card->ioport_addr) / PCC_CHAN_SIZE;
 
 	/* sja1000 register changes control the leds state */
-	if (port == REG_MOD)
+	if (port == SJA1000_MOD)
 		switch (v) {
 		case MOD_RM:
 			/* Reset Mode: set led on */
@@ -509,11 +509,11 @@ static void pcan_free_channels(struct pcan_pccard *card)
 static inline int pcan_channel_present(struct sja1000_priv *priv)
 {
 	/* make sure SJA1000 is in reset mode */
-	pcan_write_canreg(priv, REG_MOD, 1);
-	pcan_write_canreg(priv, REG_CDR, CDR_PELICAN);
+	pcan_write_canreg(priv, SJA1000_MOD, 1);
+	pcan_write_canreg(priv, SJA1000_CDR, CDR_PELICAN);
 
 	/* read reset-values */
-	if (pcan_read_canreg(priv, REG_CDR) == CDR_PELICAN)
+	if (pcan_read_canreg(priv, SJA1000_CDR) == CDR_PELICAN)
 		return 1;
 
 	return 0;
diff --git a/drivers/net/can/sja1000/plx_pci.c b/drivers/net/can/sja1000/plx_pci.c
index 3c18d7d000ed..c52c1e96bf90 100644
--- a/drivers/net/can/sja1000/plx_pci.c
+++ b/drivers/net/can/sja1000/plx_pci.c
@@ -348,20 +348,20 @@ static inline int plx_pci_check_sja1000(const struct sja1000_priv *priv)
 	 */
 	if ((priv->read_reg(priv, REG_CR) & REG_CR_BASICCAN_INITIAL_MASK) ==
 	    REG_CR_BASICCAN_INITIAL &&
-	    (priv->read_reg(priv, SJA1000_REG_SR) == REG_SR_BASICCAN_INITIAL) &&
-	    (priv->read_reg(priv, REG_IR) == REG_IR_BASICCAN_INITIAL))
+	    (priv->read_reg(priv, SJA1000_SR) == REG_SR_BASICCAN_INITIAL) &&
+	    (priv->read_reg(priv, SJA1000_IR) == REG_IR_BASICCAN_INITIAL))
 		flag = 1;
 
 	/* Bring the SJA1000 into the PeliCAN mode*/
-	priv->write_reg(priv, REG_CDR, CDR_PELICAN);
+	priv->write_reg(priv, SJA1000_CDR, CDR_PELICAN);
 
 	/*
 	 * Check registers after reset in the PeliCAN mode.
 	 * See states on p. 23 of the Datasheet.
 	 */
-	if (priv->read_reg(priv, REG_MOD) == REG_MOD_PELICAN_INITIAL &&
-	    priv->read_reg(priv, SJA1000_REG_SR) == REG_SR_PELICAN_INITIAL &&
-	    priv->read_reg(priv, REG_IR) == REG_IR_PELICAN_INITIAL)
+	if (priv->read_reg(priv, SJA1000_MOD) == REG_MOD_PELICAN_INITIAL &&
+	    priv->read_reg(priv, SJA1000_SR) == REG_SR_PELICAN_INITIAL &&
+	    priv->read_reg(priv, SJA1000_IR) == REG_IR_PELICAN_INITIAL)
 		return flag;
 
 	return 0;
diff --git a/drivers/net/can/sja1000/sja1000.c b/drivers/net/can/sja1000/sja1000.c
index e4df307eaa90..7164a999f50f 100644
--- a/drivers/net/can/sja1000/sja1000.c
+++ b/drivers/net/can/sja1000/sja1000.c
@@ -91,14 +91,14 @@ static void sja1000_write_cmdreg(struct sja1000_priv *priv, u8 val)
 	 * the write_reg() operation - especially on SMP systems.
 	 */
 	spin_lock_irqsave(&priv->cmdreg_lock, flags);
-	priv->write_reg(priv, REG_CMR, val);
-	priv->read_reg(priv, SJA1000_REG_SR);
+	priv->write_reg(priv, SJA1000_CMR, val);
+	priv->read_reg(priv, SJA1000_SR);
 	spin_unlock_irqrestore(&priv->cmdreg_lock, flags);
 }
 
 static int sja1000_is_absent(struct sja1000_priv *priv)
 {
-	return (priv->read_reg(priv, REG_MOD) == 0xFF);
+	return (priv->read_reg(priv, SJA1000_MOD) == 0xFF);
 }
 
 static int sja1000_probe_chip(struct net_device *dev)
@@ -116,11 +116,11 @@ static int sja1000_probe_chip(struct net_device *dev)
 static void set_reset_mode(struct net_device *dev)
 {
 	struct sja1000_priv *priv = netdev_priv(dev);
-	unsigned char status = priv->read_reg(priv, REG_MOD);
+	unsigned char status = priv->read_reg(priv, SJA1000_MOD);
 	int i;
 
 	/* disable interrupts */
-	priv->write_reg(priv, REG_IER, IRQ_OFF);
+	priv->write_reg(priv, SJA1000_IER, IRQ_OFF);
 
 	for (i = 0; i < 100; i++) {
 		/* check reset bit */
@@ -129,9 +129,10 @@ static void set_reset_mode(struct net_device *dev)
 			return;
 		}
 
-		priv->write_reg(priv, REG_MOD, MOD_RM);	/* reset chip */
+		/* reset chip */
+		priv->write_reg(priv, SJA1000_MOD, MOD_RM);
 		udelay(10);
-		status = priv->read_reg(priv, REG_MOD);
+		status = priv->read_reg(priv, SJA1000_MOD);
 	}
 
 	netdev_err(dev, "setting SJA1000 into reset mode failed!\n");
@@ -140,7 +141,7 @@ static void set_reset_mode(struct net_device *dev)
 static void set_normal_mode(struct net_device *dev)
 {
 	struct sja1000_priv *priv = netdev_priv(dev);
-	unsigned char status = priv->read_reg(priv, REG_MOD);
+	unsigned char status = priv->read_reg(priv, SJA1000_MOD);
 	int i;
 
 	for (i = 0; i < 100; i++) {
@@ -149,22 +150,22 @@ static void set_normal_mode(struct net_device *dev)
 			priv->can.state = CAN_STATE_ERROR_ACTIVE;
 			/* enable interrupts */
 			if (priv->can.ctrlmode & CAN_CTRLMODE_BERR_REPORTING)
-				priv->write_reg(priv, REG_IER, IRQ_ALL);
+				priv->write_reg(priv, SJA1000_IER, IRQ_ALL);
 			else
-				priv->write_reg(priv, REG_IER,
+				priv->write_reg(priv, SJA1000_IER,
 						IRQ_ALL & ~IRQ_BEI);
 			return;
 		}
 
 		/* set chip to normal mode */
 		if (priv->can.ctrlmode & CAN_CTRLMODE_LISTENONLY)
-			priv->write_reg(priv, REG_MOD, MOD_LOM);
+			priv->write_reg(priv, SJA1000_MOD, MOD_LOM);
 		else
-			priv->write_reg(priv, REG_MOD, 0x00);
+			priv->write_reg(priv, SJA1000_MOD, 0x00);
 
 		udelay(10);
 
-		status = priv->read_reg(priv, REG_MOD);
+		status = priv->read_reg(priv, SJA1000_MOD);
 	}
 
 	netdev_err(dev, "setting SJA1000 into normal mode failed!\n");
@@ -179,9 +180,9 @@ static void sja1000_start(struct net_device *dev)
 		set_reset_mode(dev);
 
 	/* Clear error counters and error code capture */
-	priv->write_reg(priv, REG_TXERR, 0x0);
-	priv->write_reg(priv, REG_RXERR, 0x0);
-	priv->read_reg(priv, REG_ECC);
+	priv->write_reg(priv, SJA1000_TXERR, 0x0);
+	priv->write_reg(priv, SJA1000_RXERR, 0x0);
+	priv->read_reg(priv, SJA1000_ECC);
 
 	/* leave reset mode */
 	set_normal_mode(dev);
@@ -217,8 +218,8 @@ static int sja1000_set_bittiming(struct net_device *dev)
 
 	netdev_info(dev, "setting BTR0=0x%02x BTR1=0x%02x\n", btr0, btr1);
 
-	priv->write_reg(priv, REG_BTR0, btr0);
-	priv->write_reg(priv, REG_BTR1, btr1);
+	priv->write_reg(priv, SJA1000_BTR0, btr0);
+	priv->write_reg(priv, SJA1000_BTR1, btr1);
 
 	return 0;
 }
@@ -228,8 +229,8 @@ static int sja1000_get_berr_counter(const struct net_device *dev,
 {
 	struct sja1000_priv *priv = netdev_priv(dev);
 
-	bec->txerr = priv->read_reg(priv, REG_TXERR);
-	bec->rxerr = priv->read_reg(priv, REG_RXERR);
+	bec->txerr = priv->read_reg(priv, SJA1000_TXERR);
+	bec->rxerr = priv->read_reg(priv, SJA1000_RXERR);
 
 	return 0;
 }
@@ -247,20 +248,20 @@ static void chipset_init(struct net_device *dev)
 	struct sja1000_priv *priv = netdev_priv(dev);
 
 	/* set clock divider and output control register */
-	priv->write_reg(priv, REG_CDR, priv->cdr | CDR_PELICAN);
+	priv->write_reg(priv, SJA1000_CDR, priv->cdr | CDR_PELICAN);
 
 	/* set acceptance filter (accept all) */
-	priv->write_reg(priv, REG_ACCC0, 0x00);
-	priv->write_reg(priv, REG_ACCC1, 0x00);
-	priv->write_reg(priv, REG_ACCC2, 0x00);
-	priv->write_reg(priv, REG_ACCC3, 0x00);
+	priv->write_reg(priv, SJA1000_ACCC0, 0x00);
+	priv->write_reg(priv, SJA1000_ACCC1, 0x00);
+	priv->write_reg(priv, SJA1000_ACCC2, 0x00);
+	priv->write_reg(priv, SJA1000_ACCC3, 0x00);
 
-	priv->write_reg(priv, REG_ACCM0, 0xFF);
-	priv->write_reg(priv, REG_ACCM1, 0xFF);
-	priv->write_reg(priv, REG_ACCM2, 0xFF);
-	priv->write_reg(priv, REG_ACCM3, 0xFF);
+	priv->write_reg(priv, SJA1000_ACCM0, 0xFF);
+	priv->write_reg(priv, SJA1000_ACCM1, 0xFF);
+	priv->write_reg(priv, SJA1000_ACCM2, 0xFF);
+	priv->write_reg(priv, SJA1000_ACCM3, 0xFF);
 
-	priv->write_reg(priv, REG_OCR, priv->ocr | OCR_MODE_NORMAL);
+	priv->write_reg(priv, SJA1000_OCR, priv->ocr | OCR_MODE_NORMAL);
 }
 
 /*
@@ -289,21 +290,21 @@ static netdev_tx_t sja1000_start_xmit(struct sk_buff *skb,
 	id = cf->can_id;
 
 	if (id & CAN_RTR_FLAG)
-		fi |= FI_RTR;
+		fi |= SJA1000_FI_RTR;
 
 	if (id & CAN_EFF_FLAG) {
-		fi |= FI_FF;
-		dreg = EFF_BUF;
-		priv->write_reg(priv, REG_FI, fi);
-		priv->write_reg(priv, REG_ID1, (id & 0x1fe00000) >> (5 + 16));
-		priv->write_reg(priv, REG_ID2, (id & 0x001fe000) >> (5 + 8));
-		priv->write_reg(priv, REG_ID3, (id & 0x00001fe0) >> 5);
-		priv->write_reg(priv, REG_ID4, (id & 0x0000001f) << 3);
+		fi |= SJA1000_FI_FF;
+		dreg = SJA1000_EFF_BUF;
+		priv->write_reg(priv, SJA1000_FI, fi);
+		priv->write_reg(priv, SJA1000_ID1, (id & 0x1fe00000) >> 21);
+		priv->write_reg(priv, SJA1000_ID2, (id & 0x001fe000) >> 13);
+		priv->write_reg(priv, SJA1000_ID3, (id & 0x00001fe0) >> 5);
+		priv->write_reg(priv, SJA1000_ID4, (id & 0x0000001f) << 3);
 	} else {
-		dreg = SFF_BUF;
-		priv->write_reg(priv, REG_FI, fi);
-		priv->write_reg(priv, REG_ID1, (id & 0x000007f8) >> 3);
-		priv->write_reg(priv, REG_ID2, (id & 0x00000007) << 5);
+		dreg = SJA1000_SFF_BUF;
+		priv->write_reg(priv, SJA1000_FI, fi);
+		priv->write_reg(priv, SJA1000_ID1, (id & 0x000007f8) >> 3);
+		priv->write_reg(priv, SJA1000_ID2, (id & 0x00000007) << 5);
 	}
 
 	for (i = 0; i < dlc; i++)
@@ -335,25 +336,25 @@ static void sja1000_rx(struct net_device *dev)
 	if (skb == NULL)
 		return;
 
-	fi = priv->read_reg(priv, REG_FI);
+	fi = priv->read_reg(priv, SJA1000_FI);
 
-	if (fi & FI_FF) {
+	if (fi & SJA1000_FI_FF) {
 		/* extended frame format (EFF) */
-		dreg = EFF_BUF;
-		id = (priv->read_reg(priv, REG_ID1) << (5 + 16))
-		    | (priv->read_reg(priv, REG_ID2) << (5 + 8))
-		    | (priv->read_reg(priv, REG_ID3) << 5)
-		    | (priv->read_reg(priv, REG_ID4) >> 3);
+		dreg = SJA1000_EFF_BUF;
+		id = (priv->read_reg(priv, SJA1000_ID1) << 21)
+		    | (priv->read_reg(priv, SJA1000_ID2) << 13)
+		    | (priv->read_reg(priv, SJA1000_ID3) << 5)
+		    | (priv->read_reg(priv, SJA1000_ID4) >> 3);
 		id |= CAN_EFF_FLAG;
 	} else {
 		/* standard frame format (SFF) */
-		dreg = SFF_BUF;
-		id = (priv->read_reg(priv, REG_ID1) << 3)
-		    | (priv->read_reg(priv, REG_ID2) >> 5);
+		dreg = SJA1000_SFF_BUF;
+		id = (priv->read_reg(priv, SJA1000_ID1) << 3)
+		    | (priv->read_reg(priv, SJA1000_ID2) >> 5);
 	}
 
 	cf->can_dlc = get_can_dlc(fi & 0x0F);
-	if (fi & FI_RTR) {
+	if (fi & SJA1000_FI_RTR) {
 		id |= CAN_RTR_FLAG;
 	} else {
 		for (i = 0; i < cf->can_dlc; i++)
@@ -414,7 +415,7 @@ static int sja1000_err(struct net_device *dev, uint8_t isrc, uint8_t status)
 		priv->can.can_stats.bus_error++;
 		stats->rx_errors++;
 
-		ecc = priv->read_reg(priv, REG_ECC);
+		ecc = priv->read_reg(priv, SJA1000_ECC);
 
 		cf->can_id |= CAN_ERR_PROT | CAN_ERR_BUSERROR;
 
@@ -448,7 +449,7 @@ static int sja1000_err(struct net_device *dev, uint8_t isrc, uint8_t status)
 	if (isrc & IRQ_ALI) {
 		/* arbitration lost interrupt */
 		netdev_dbg(dev, "arbitration lost interrupt\n");
-		alc = priv->read_reg(priv, REG_ALC);
+		alc = priv->read_reg(priv, SJA1000_ALC);
 		priv->can.can_stats.arbitration_lost++;
 		stats->tx_errors++;
 		cf->can_id |= CAN_ERR_LOSTARB;
@@ -457,8 +458,8 @@ static int sja1000_err(struct net_device *dev, uint8_t isrc, uint8_t status)
 
 	if (state != priv->can.state && (state == CAN_STATE_ERROR_WARNING ||
 					 state == CAN_STATE_ERROR_PASSIVE)) {
-		uint8_t rxerr = priv->read_reg(priv, REG_RXERR);
-		uint8_t txerr = priv->read_reg(priv, REG_TXERR);
+		uint8_t rxerr = priv->read_reg(priv, SJA1000_RXERR);
+		uint8_t txerr = priv->read_reg(priv, SJA1000_TXERR);
 		cf->can_id |= CAN_ERR_CRTL;
 		if (state == CAN_STATE_ERROR_WARNING) {
 			priv->can.can_stats.error_warning++;
@@ -494,15 +495,16 @@ irqreturn_t sja1000_interrupt(int irq, void *dev_id)
 	int n = 0;
 
 	/* Shared interrupts and IRQ off? */
-	if (priv->read_reg(priv, REG_IER) == IRQ_OFF)
+	if (priv->read_reg(priv, SJA1000_IER) == IRQ_OFF)
 		return IRQ_NONE;
 
 	if (priv->pre_irq)
 		priv->pre_irq(priv);
 
-	while ((isrc = priv->read_reg(priv, REG_IR)) && (n < SJA1000_MAX_IRQ)) {
+	while ((isrc = priv->read_reg(priv, SJA1000_IR)) &&
+	       (n < SJA1000_MAX_IRQ)) {
 		n++;
-		status = priv->read_reg(priv, SJA1000_REG_SR);
+		status = priv->read_reg(priv, SJA1000_SR);
 		/* check for absent controller due to hw unplug */
 		if (status == 0xFF && sja1000_is_absent(priv))
 			return IRQ_NONE;
@@ -519,7 +521,7 @@ irqreturn_t sja1000_interrupt(int irq, void *dev_id)
 			} else {
 				/* transmission complete */
 				stats->tx_bytes +=
-					priv->read_reg(priv, REG_FI) & 0xf;
+					priv->read_reg(priv, SJA1000_FI) & 0xf;
 				stats->tx_packets++;
 				can_get_echo_skb(dev, 0);
 			}
@@ -530,7 +532,7 @@ irqreturn_t sja1000_interrupt(int irq, void *dev_id)
 			/* receive interrupt */
 			while (status & SR_RBS) {
 				sja1000_rx(dev);
-				status = priv->read_reg(priv, SJA1000_REG_SR);
+				status = priv->read_reg(priv, SJA1000_SR);
 				/* check for absent controller */
 				if (status == 0xFF && sja1000_is_absent(priv))
 					return IRQ_NONE;
diff --git a/drivers/net/can/sja1000/sja1000.h b/drivers/net/can/sja1000/sja1000.h
index aa48e053da27..9d46398f8154 100644
--- a/drivers/net/can/sja1000/sja1000.h
+++ b/drivers/net/can/sja1000/sja1000.h
@@ -54,46 +54,46 @@
 #define SJA1000_MAX_IRQ 20	/* max. number of interrupts handled in ISR */
 
 /* SJA1000 registers - manual section 6.4 (Pelican Mode) */
-#define REG_MOD		0x00
-#define REG_CMR		0x01
-#define SJA1000_REG_SR		0x02
-#define REG_IR		0x03
-#define REG_IER		0x04
-#define REG_ALC		0x0B
-#define REG_ECC		0x0C
-#define REG_EWL		0x0D
-#define REG_RXERR	0x0E
-#define REG_TXERR	0x0F
-#define REG_ACCC0	0x10
-#define REG_ACCC1	0x11
-#define REG_ACCC2	0x12
-#define REG_ACCC3	0x13
-#define REG_ACCM0	0x14
-#define REG_ACCM1	0x15
-#define REG_ACCM2	0x16
-#define REG_ACCM3	0x17
-#define REG_RMC		0x1D
-#define REG_RBSA	0x1E
+#define SJA1000_MOD		0x00
+#define SJA1000_CMR		0x01
+#define SJA1000_SR		0x02
+#define SJA1000_IR		0x03
+#define SJA1000_IER		0x04
+#define SJA1000_ALC		0x0B
+#define SJA1000_ECC		0x0C
+#define SJA1000_EWL		0x0D
+#define SJA1000_RXERR		0x0E
+#define SJA1000_TXERR		0x0F
+#define SJA1000_ACCC0		0x10
+#define SJA1000_ACCC1		0x11
+#define SJA1000_ACCC2		0x12
+#define SJA1000_ACCC3		0x13
+#define SJA1000_ACCM0		0x14
+#define SJA1000_ACCM1		0x15
+#define SJA1000_ACCM2		0x16
+#define SJA1000_ACCM3		0x17
+#define SJA1000_RMC		0x1D
+#define SJA1000_RBSA		0x1E
 
 /* Common registers - manual section 6.5 */
-#define REG_BTR0	0x06
-#define REG_BTR1	0x07
-#define REG_OCR		0x08
-#define REG_CDR		0x1F
+#define SJA1000_BTR0		0x06
+#define SJA1000_BTR1		0x07
+#define SJA1000_OCR		0x08
+#define SJA1000_CDR		0x1F
 
-#define REG_FI		0x10
-#define SFF_BUF		0x13
-#define EFF_BUF		0x15
+#define SJA1000_FI		0x10
+#define SJA1000_SFF_BUF		0x13
+#define SJA1000_EFF_BUF		0x15
 
-#define FI_FF		0x80
-#define FI_RTR		0x40
+#define SJA1000_FI_FF		0x80
+#define SJA1000_FI_RTR		0x40
 
-#define REG_ID1		0x11
-#define REG_ID2		0x12
-#define REG_ID3		0x13
-#define REG_ID4		0x14
+#define SJA1000_ID1		0x11
+#define SJA1000_ID2		0x12
+#define SJA1000_ID3		0x13
+#define SJA1000_ID4		0x14
 
-#define CAN_RAM		0x20
+#define SJA1000_CAN_RAM		0x20
 
 /* mode register */
 #define MOD_RM		0x01
diff --git a/drivers/net/ethernet/3com/3c509.c b/drivers/net/ethernet/3com/3c509.c
index f36ff99fd394..adb4bf5eb4b4 100644
--- a/drivers/net/ethernet/3com/3c509.c
+++ b/drivers/net/ethernet/3com/3c509.c
@@ -306,6 +306,7 @@ static int el3_isa_match(struct device *pdev, unsigned int ndev)
 	if (!dev)
 		return -ENOMEM;
 
+	SET_NETDEV_DEV(dev, pdev);
 	netdev_boot_setup_check(dev);
 
 	if (!request_region(ioaddr, EL3_IO_EXTENT, "3c509-isa")) {
@@ -595,6 +596,7 @@ static int __init el3_eisa_probe (struct device *device)
 		return -ENOMEM;
 	}
 
+	SET_NETDEV_DEV(dev, device);
 	netdev_boot_setup_check(dev);
 
 	el3_dev_fill(dev, phys_addr, ioaddr, irq, if_port, EL3_EISA);
diff --git a/drivers/net/ethernet/3com/3c59x.c b/drivers/net/ethernet/3com/3c59x.c
index 1928e2001587..de570a8f8967 100644
--- a/drivers/net/ethernet/3com/3c59x.c
+++ b/drivers/net/ethernet/3com/3c59x.c
@@ -951,7 +951,7 @@ static int vortex_eisa_remove(struct device *device)
 
 	unregister_netdev(dev);
 	iowrite16(TotalReset|0x14, ioaddr + EL3_CMD);
-	release_region(dev->base_addr, VORTEX_TOTAL_SIZE);
+	release_region(edev->base_addr, VORTEX_TOTAL_SIZE);
 
 	free_netdev(dev);
 	return 0;
diff --git a/drivers/net/ethernet/3com/typhoon.c b/drivers/net/ethernet/3com/typhoon.c
index 27aaaf99e73e..144942f6372b 100644
--- a/drivers/net/ethernet/3com/typhoon.c
+++ b/drivers/net/ethernet/3com/typhoon.c
@@ -1690,7 +1690,7 @@ typhoon_rx(struct typhoon *tp, struct basic_ring *rxRing, volatile __le32 * read
 			skb_checksum_none_assert(new_skb);
 
 		if (rx->rxStatus & TYPHOON_RX_VLAN)
-			__vlan_hwaccel_put_tag(new_skb,
+			__vlan_hwaccel_put_tag(new_skb, htons(ETH_P_8021Q),
 					       ntohl(rx->vlanTag) & 0xffff);
 		netif_receive_skb(new_skb);
 
@@ -2445,9 +2445,9 @@ typhoon_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	 * settings -- so we only allow the user to toggle the TX processing.
 	 */
 	dev->hw_features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_TSO |
-		NETIF_F_HW_VLAN_TX;
+		NETIF_F_HW_VLAN_CTAG_TX;
 	dev->features = dev->hw_features |
-		NETIF_F_HW_VLAN_RX | NETIF_F_RXCSUM;
+		NETIF_F_HW_VLAN_CTAG_RX | NETIF_F_RXCSUM;
 
 	if(register_netdev(dev) < 0) {
 		err_msg = "unable to register netdev";
diff --git a/drivers/net/ethernet/adaptec/starfire.c b/drivers/net/ethernet/adaptec/starfire.c
index 549b77500579..8b04bfc20cfb 100644
--- a/drivers/net/ethernet/adaptec/starfire.c
+++ b/drivers/net/ethernet/adaptec/starfire.c
@@ -594,7 +594,8 @@ static const struct ethtool_ops ethtool_ops;
 
 
 #ifdef VLAN_SUPPORT
-static int netdev_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
+static int netdev_vlan_rx_add_vid(struct net_device *dev,
+				  __be16 proto, u16 vid)
 {
 	struct netdev_private *np = netdev_priv(dev);
 
@@ -608,7 +609,8 @@ static int netdev_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
 	return 0;
 }
 
-static int netdev_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
+static int netdev_vlan_rx_kill_vid(struct net_device *dev,
+				   __be16 proto, u16 vid)
 {
 	struct netdev_private *np = netdev_priv(dev);
 
@@ -702,7 +704,7 @@ static int starfire_init_one(struct pci_dev *pdev,
 #endif /* ZEROCOPY */
 
 #ifdef VLAN_SUPPORT
-	dev->features |= NETIF_F_HW_VLAN_RX | NETIF_F_HW_VLAN_FILTER;
+	dev->features |= NETIF_F_HW_VLAN_CTAG_RX | NETIF_F_HW_VLAN_CTAG_FILTER;
 #endif /* VLAN_RX_KILL_VID */
 #ifdef ADDR_64BITS
 	dev->features |= NETIF_F_HIGHDMA;
@@ -1496,7 +1498,7 @@ static int __netdev_rx(struct net_device *dev, int *quota)
 				printk(KERN_DEBUG "  netdev_rx() vlanid = %d\n",
 				       vlid);
 			}
-			__vlan_hwaccel_put_tag(skb, vlid);
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlid);
 		}
 #endif /* VLAN_SUPPORT */
 		netif_receive_skb(skb);
diff --git a/drivers/net/ethernet/adi/bfin_mac.c b/drivers/net/ethernet/adi/bfin_mac.c
index a175d0be1ae1..dada66bfe0d6 100644
--- a/drivers/net/ethernet/adi/bfin_mac.c
+++ b/drivers/net/ethernet/adi/bfin_mac.c
@@ -188,10 +188,9 @@ static int desc_list_init(struct net_device *dev)
 
 		/* allocate a new skb for next time receive */
 		new_skb = netdev_alloc_skb(dev, PKT_BUF_SZ + NET_IP_ALIGN);
-		if (!new_skb) {
-			pr_notice("init: low on mem - packet dropped\n");
+		if (!new_skb)
 			goto init_error;
-		}
+
 		skb_reserve(new_skb, NET_IP_ALIGN);
 		/* Invidate the data cache of skb->data range when it is write back
 		 * cache. It will prevent overwritting the new data from DMA
@@ -1236,7 +1235,6 @@ static void bfin_mac_rx(struct net_device *dev)
 
 	new_skb = netdev_alloc_skb(dev, PKT_BUF_SZ + NET_IP_ALIGN);
 	if (!new_skb) {
-		netdev_notice(dev, "rx: low on mem - packet dropped\n");
 		dev->stats.rx_dropped++;
 		goto out;
 	}
@@ -1702,7 +1700,8 @@ static int bfin_mac_probe(struct platform_device *pdev)
 	}
 
 	bfin_mac_hwtstamp_init(ndev);
-	if (bfin_phc_init(ndev, &pdev->dev)) {
+	rc = bfin_phc_init(ndev, &pdev->dev);
+	if (rc) {
 		dev_err(&pdev->dev, "Cannot register PHC device!\n");
 		goto out_err_phc;
 	}
diff --git a/drivers/net/ethernet/aeroflex/greth.c b/drivers/net/ethernet/aeroflex/greth.c
index 0be2195e5034..269295403fc4 100644
--- a/drivers/net/ethernet/aeroflex/greth.c
+++ b/drivers/net/ethernet/aeroflex/greth.c
@@ -1464,35 +1464,23 @@ static int greth_of_probe(struct platform_device *ofdev)
 	}
 
 	/* Allocate TX descriptor ring in coherent memory */
-	greth->tx_bd_base = (struct greth_bd *) dma_alloc_coherent(greth->dev,
-								   1024,
-								   &greth->tx_bd_base_phys,
-								   GFP_KERNEL);
-
+	greth->tx_bd_base = dma_alloc_coherent(greth->dev, 1024,
+					       &greth->tx_bd_base_phys,
+					       GFP_KERNEL | __GFP_ZERO);
 	if (!greth->tx_bd_base) {
-		if (netif_msg_probe(greth))
-			dev_err(&dev->dev, "could not allocate descriptor memory.\n");
 		err = -ENOMEM;
 		goto error3;
 	}
 
-	memset(greth->tx_bd_base, 0, 1024);
-
 	/* Allocate RX descriptor ring in coherent memory */
-	greth->rx_bd_base = (struct greth_bd *) dma_alloc_coherent(greth->dev,
-								   1024,
-								   &greth->rx_bd_base_phys,
-								   GFP_KERNEL);
-
+	greth->rx_bd_base = dma_alloc_coherent(greth->dev, 1024,
+					       &greth->rx_bd_base_phys,
+					       GFP_KERNEL | __GFP_ZERO);
 	if (!greth->rx_bd_base) {
-		if (netif_msg_probe(greth))
-			dev_err(greth->dev, "could not allocate descriptor memory.\n");
 		err = -ENOMEM;
 		goto error4;
 	}
 
-	memset(greth->rx_bd_base, 0, 1024);
-
 	/* Get MAC address from: module param, OF property or ID prom */
 	for (i = 0; i < 6; i++) {
 		if (macaddr[i] != 0)
diff --git a/drivers/net/ethernet/alteon/acenic.c b/drivers/net/ethernet/alteon/acenic.c
index c0bc41a784ca..b7894f8af9d1 100644
--- a/drivers/net/ethernet/alteon/acenic.c
+++ b/drivers/net/ethernet/alteon/acenic.c
@@ -472,7 +472,7 @@ static int acenic_probe_one(struct pci_dev *pdev,
 	ap->name = pci_name(pdev);
 
 	dev->features |= NETIF_F_SG | NETIF_F_IP_CSUM;
-	dev->features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
+	dev->features |= NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX;
 
 	dev->watchdog_timeo = 5*HZ;
 
@@ -2019,7 +2019,7 @@ static void ace_rx_int(struct net_device *dev, u32 rxretprd, u32 rxretcsm)
 
 		/* send it up */
 		if ((bd_flags & BD_FLG_VLAN_TAG))
-			__vlan_hwaccel_put_tag(skb, retdesc->vlan);
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), retdesc->vlan);
 		netif_rx(skb);
 
 		dev->stats.rx_packets++;
diff --git a/drivers/net/ethernet/amd/7990.c b/drivers/net/ethernet/amd/7990.c
index 6e722dc37db7..65926a956575 100644
--- a/drivers/net/ethernet/amd/7990.c
+++ b/drivers/net/ethernet/amd/7990.c
@@ -318,8 +318,6 @@ static int lance_rx (struct net_device *dev)
 			struct sk_buff *skb = netdev_alloc_skb(dev, len + 2);
 
                         if (!skb) {
-                                printk ("%s: Memory squeeze, deferring packet.\n",
-                                        dev->name);
                                 dev->stats.rx_dropped++;
                                 rd->mblength = 0;
                                 rd->rmd1_bits = LE_R1_OWN;
diff --git a/drivers/net/ethernet/amd/a2065.c b/drivers/net/ethernet/amd/a2065.c
index 3789affbc0e5..0866e7627433 100644
--- a/drivers/net/ethernet/amd/a2065.c
+++ b/drivers/net/ethernet/amd/a2065.c
@@ -293,7 +293,6 @@ static int lance_rx(struct net_device *dev)
 			struct sk_buff *skb = netdev_alloc_skb(dev, len + 2);
 
 			if (!skb) {
-				netdev_warn(dev, "Memory squeeze, deferring packet\n");
 				dev->stats.rx_dropped++;
 				rd->mblength = 0;
 				rd->rmd1_bits = LE_R1_OWN;
diff --git a/drivers/net/ethernet/amd/am79c961a.c b/drivers/net/ethernet/amd/am79c961a.c
index 60e2b701afe7..9793767996a2 100644
--- a/drivers/net/ethernet/amd/am79c961a.c
+++ b/drivers/net/ethernet/amd/am79c961a.c
@@ -528,7 +528,6 @@ am79c961_rx(struct net_device *dev, struct dev_priv *priv)
 			dev->stats.rx_packets++;
 		} else {
 			am_writeword (dev, hdraddr + 2, RMD_OWN);
-			printk (KERN_WARNING "%s: memory squeeze, dropping packet.\n", dev->name);
 			dev->stats.rx_dropped++;
 			break;
 		}
diff --git a/drivers/net/ethernet/amd/amd8111e.c b/drivers/net/ethernet/amd/amd8111e.c
index 42d4e6ad58a5..8e6b665a6726 100644
--- a/drivers/net/ethernet/amd/amd8111e.c
+++ b/drivers/net/ethernet/amd/amd8111e.c
@@ -793,7 +793,7 @@ static int amd8111e_rx_poll(struct napi_struct *napi, int budget)
 #if AMD8111E_VLAN_TAG_USED
 			if (vtag == TT_VLAN_TAGGED){
 				u16 vlan_tag = le16_to_cpu(lp->rx_ring[rx_index].tag_ctrl_info);
-				__vlan_hwaccel_put_tag(skb, vlan_tag);
+				__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_tag);
 			}
 #endif
 			netif_receive_skb(skb);
@@ -1869,7 +1869,7 @@ static int amd8111e_probe_one(struct pci_dev *pdev,
 	SET_NETDEV_DEV(dev, &pdev->dev);
 
 #if AMD8111E_VLAN_TAG_USED
-	dev->features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX ;
+	dev->features |= NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX ;
 #endif
 
 	lp = netdev_priv(dev);
@@ -1907,7 +1907,7 @@ static int amd8111e_probe_one(struct pci_dev *pdev,
 	netif_napi_add(dev, &lp->napi, amd8111e_rx_poll, 32);
 
 #if AMD8111E_VLAN_TAG_USED
-	dev->features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
+	dev->features |= NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX;
 #endif
 	/* Probe the external PHY */
 	amd8111e_probe_ext_phy(dev);
diff --git a/drivers/net/ethernet/amd/ariadne.c b/drivers/net/ethernet/amd/ariadne.c
index 98f4522fd17b..c178eb4c8166 100644
--- a/drivers/net/ethernet/amd/ariadne.c
+++ b/drivers/net/ethernet/amd/ariadne.c
@@ -193,7 +193,6 @@ static int ariadne_rx(struct net_device *dev)
 
 			skb = netdev_alloc_skb(dev, pkt_len + 2);
 			if (skb == NULL) {
-				netdev_warn(dev, "Memory squeeze, deferring packet\n");
 				for (i = 0; i < RX_RING_SIZE; i++)
 					if (lowb(priv->rx_ring[(entry + i) % RX_RING_SIZE]->RMD1) & RF_OWN)
 						break;
diff --git a/drivers/net/ethernet/amd/atarilance.c b/drivers/net/ethernet/amd/atarilance.c
index 84219df72f51..e8d0ef508f48 100644
--- a/drivers/net/ethernet/amd/atarilance.c
+++ b/drivers/net/ethernet/amd/atarilance.c
@@ -996,8 +996,6 @@ static int lance_rx( struct net_device *dev )
 			else {
 				skb = netdev_alloc_skb(dev, pkt_len + 2);
 				if (skb == NULL) {
-					DPRINTK( 1, ( "%s: Memory squeeze, deferring packet.\n",
-								  dev->name ));
 					for( i = 0; i < RX_RING_SIZE; i++ )
 						if (MEM->rx_head[(entry+i) & RX_RING_MOD_MASK].flag &
 							RMD1_OWN_CHIP)
@@ -1149,9 +1147,7 @@ static struct net_device *atarilance_dev;
 static int __init atarilance_module_init(void)
 {
 	atarilance_dev = atarilance_probe(-1);
-	if (IS_ERR(atarilance_dev))
-		return PTR_ERR(atarilance_dev);
-	return 0;
+	return PTR_RET(atarilance_dev);
 }
 
 static void __exit atarilance_module_exit(void)
diff --git a/drivers/net/ethernet/amd/au1000_eth.c b/drivers/net/ethernet/amd/au1000_eth.c
index de774d419144..688aede742c7 100644
--- a/drivers/net/ethernet/amd/au1000_eth.c
+++ b/drivers/net/ethernet/amd/au1000_eth.c
@@ -727,7 +727,6 @@ static int au1000_rx(struct net_device *dev)
 			frmlen -= 4; /* Remove FCS */
 			skb = netdev_alloc_skb(dev, frmlen + 2);
 			if (skb == NULL) {
-				netdev_err(dev, "Memory squeeze, dropping packet.\n");
 				dev->stats.rx_dropped++;
 				continue;
 			}
diff --git a/drivers/net/ethernet/amd/declance.c b/drivers/net/ethernet/amd/declance.c
index baca0bd1b393..3d86ffeb4e15 100644
--- a/drivers/net/ethernet/amd/declance.c
+++ b/drivers/net/ethernet/amd/declance.c
@@ -607,8 +607,6 @@ static int lance_rx(struct net_device *dev)
 			skb = netdev_alloc_skb(dev, len + 2);
 
 			if (skb == 0) {
-				printk("%s: Memory squeeze, deferring packet.\n",
-				       dev->name);
 				dev->stats.rx_dropped++;
 				*rds_ptr(rd, mblength, lp->type) = 0;
 				*rds_ptr(rd, rmd1, lp->type) =
diff --git a/drivers/net/ethernet/amd/mvme147.c b/drivers/net/ethernet/amd/mvme147.c
index 9af3c307862c..a51497c9d2af 100644
--- a/drivers/net/ethernet/amd/mvme147.c
+++ b/drivers/net/ethernet/amd/mvme147.c
@@ -188,9 +188,7 @@ static struct net_device *dev_mvme147_lance;
 int __init init_module(void)
 {
 	dev_mvme147_lance = mvme147lance_probe(-1);
-	if (IS_ERR(dev_mvme147_lance))
-		return PTR_ERR(dev_mvme147_lance);
-	return 0;
+	return PTR_RET(dev_mvme147_lance);
 }
 
 void __exit cleanup_module(void)
diff --git a/drivers/net/ethernet/amd/ni65.c b/drivers/net/ethernet/amd/ni65.c
index 013b65108536..26fc0ce0faa3 100644
--- a/drivers/net/ethernet/amd/ni65.c
+++ b/drivers/net/ethernet/amd/ni65.c
@@ -1238,7 +1238,7 @@ MODULE_PARM_DESC(dma, "ni6510 ISA DMA channel (ignored for some cards)");
 int __init init_module(void)
 {
  	dev_ni65 = ni65_probe(-1);
-	return IS_ERR(dev_ni65) ? PTR_ERR(dev_ni65) : 0;
+	return PTR_RET(dev_ni65);
 }
 
 void __exit cleanup_module(void)
diff --git a/drivers/net/ethernet/amd/pcnet32.c b/drivers/net/ethernet/amd/pcnet32.c
index 797f847edf13..ed2130727643 100644
--- a/drivers/net/ethernet/amd/pcnet32.c
+++ b/drivers/net/ethernet/amd/pcnet32.c
@@ -1166,7 +1166,6 @@ static void pcnet32_rx_entry(struct net_device *dev,
 		skb = netdev_alloc_skb(dev, pkt_len + NET_IP_ALIGN);
 
 	if (skb == NULL) {
-		netif_err(lp, drv, dev, "Memory squeeze, dropping packet\n");
 		dev->stats.rx_dropped++;
 		return;
 	}
diff --git a/drivers/net/ethernet/amd/sun3lance.c b/drivers/net/ethernet/amd/sun3lance.c
index 74b3891b6483..4375abe61da1 100644
--- a/drivers/net/ethernet/amd/sun3lance.c
+++ b/drivers/net/ethernet/amd/sun3lance.c
@@ -812,9 +812,6 @@ static int lance_rx( struct net_device *dev )
 			else {
 				skb = netdev_alloc_skb(dev, pkt_len + 2);
 				if (skb == NULL) {
-					DPRINTK( 1, ( "%s: Memory squeeze, deferring packet.\n",
-						      dev->name ));
-
 					dev->stats.rx_dropped++;
 					head->msg_length = 0;
 					head->flag |= RMD1_OWN_CHIP;
@@ -943,9 +940,7 @@ static struct net_device *sun3lance_dev;
 int __init init_module(void)
 {
 	sun3lance_dev = sun3lance_probe(-1);
-	if (IS_ERR(sun3lance_dev))
-		return PTR_ERR(sun3lance_dev);
-	return 0;
+	return PTR_RET(sun3lance_dev);
 }
 
 void __exit cleanup_module(void)
diff --git a/drivers/net/ethernet/amd/sunlance.c b/drivers/net/ethernet/amd/sunlance.c
index 6a40290d3727..f47b780892e9 100644
--- a/drivers/net/ethernet/amd/sunlance.c
+++ b/drivers/net/ethernet/amd/sunlance.c
@@ -536,8 +536,6 @@ static void lance_rx_dvma(struct net_device *dev)
 			skb = netdev_alloc_skb(dev, len + 2);
 
 			if (skb == NULL) {
-				printk(KERN_INFO "%s: Memory squeeze, deferring packet.\n",
-				       dev->name);
 				dev->stats.rx_dropped++;
 				rd->mblength = 0;
 				rd->rmd1_bits = LE_R1_OWN;
@@ -708,8 +706,6 @@ static void lance_rx_pio(struct net_device *dev)
 			skb = netdev_alloc_skb(dev, len + 2);
 
 			if (skb == NULL) {
-				printk(KERN_INFO "%s: Memory squeeze, deferring packet.\n",
-				       dev->name);
 				dev->stats.rx_dropped++;
 				sbus_writew(0, &rd->mblength);
 				sbus_writeb(LE_R1_OWN, &rd->rmd1_bits);
@@ -1377,10 +1373,9 @@ static int sparc_lance_probe_one(struct platform_device *op,
 			dma_alloc_coherent(&op->dev,
 					   sizeof(struct lance_init_block),
 					   &lp->init_block_dvma, GFP_ATOMIC);
-		if (!lp->init_block_mem) {
-			printk(KERN_ERR "SunLance: Cannot allocate consistent DMA memory.\n");
+		if (!lp->init_block_mem)
 			goto fail;
-		}
+
 		lp->pio_buffer = 0;
 		lp->init_ring = lance_init_ring_dvma;
 		lp->rx = lance_rx_dvma;
diff --git a/drivers/net/ethernet/apple/macmace.c b/drivers/net/ethernet/apple/macmace.c
index a206779c68cf..4ce8ceb62205 100644
--- a/drivers/net/ethernet/apple/macmace.c
+++ b/drivers/net/ethernet/apple/macmace.c
@@ -386,20 +386,16 @@ static int mace_open(struct net_device *dev)
 	/* Allocate the DMA ring buffers */
 
 	mp->tx_ring = dma_alloc_coherent(mp->device,
-			N_TX_RING * MACE_BUFF_SIZE,
-			&mp->tx_ring_phys, GFP_KERNEL);
-	if (mp->tx_ring == NULL) {
-		printk(KERN_ERR "%s: unable to allocate DMA tx buffers\n", dev->name);
+					 N_TX_RING * MACE_BUFF_SIZE,
+					 &mp->tx_ring_phys, GFP_KERNEL);
+	if (mp->tx_ring == NULL)
 		goto out1;
-	}
 
 	mp->rx_ring = dma_alloc_coherent(mp->device,
-			N_RX_RING * MACE_BUFF_SIZE,
-			&mp->rx_ring_phys, GFP_KERNEL);
-	if (mp->rx_ring == NULL) {
-		printk(KERN_ERR "%s: unable to allocate DMA rx buffers\n", dev->name);
+					 N_RX_RING * MACE_BUFF_SIZE,
+					 &mp->rx_ring_phys, GFP_KERNEL);
+	if (mp->rx_ring == NULL)
 		goto out2;
-	}
 
 	mace_dma_off(dev);
 
diff --git a/drivers/net/ethernet/atheros/atl1c/atl1c_main.c b/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
index 1f07fc633ab9..0ba900762b13 100644
--- a/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
+++ b/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
@@ -417,7 +417,7 @@ static void atl1c_set_multi(struct net_device *netdev)
 
 static void __atl1c_vlan_mode(netdev_features_t features, u32 *mac_ctrl_data)
 {
-	if (features & NETIF_F_HW_VLAN_RX) {
+	if (features & NETIF_F_HW_VLAN_CTAG_RX) {
 		/* enable VLAN tag insert/strip */
 		*mac_ctrl_data |= MAC_CTRL_RMV_VLAN;
 	} else {
@@ -494,10 +494,10 @@ static netdev_features_t atl1c_fix_features(struct net_device *netdev,
 	 * Since there is no support for separate rx/tx vlan accel
 	 * enable/disable make sure tx flag is always in same state as rx.
 	 */
-	if (features & NETIF_F_HW_VLAN_RX)
-		features |= NETIF_F_HW_VLAN_TX;
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
+		features |= NETIF_F_HW_VLAN_CTAG_TX;
 	else
-		features &= ~NETIF_F_HW_VLAN_TX;
+		features &= ~NETIF_F_HW_VLAN_CTAG_TX;
 
 	if (netdev->mtu > MAX_TSO_FRAME_SIZE)
 		features &= ~(NETIF_F_TSO | NETIF_F_TSO6);
@@ -510,7 +510,7 @@ static int atl1c_set_features(struct net_device *netdev,
 {
 	netdev_features_t changed = netdev->features ^ features;
 
-	if (changed & NETIF_F_HW_VLAN_RX)
+	if (changed & NETIF_F_HW_VLAN_CTAG_RX)
 		atl1c_vlan_mode(netdev, features);
 
 	return 0;
@@ -1809,7 +1809,7 @@ rrs_checked:
 
 			AT_TAG_TO_VLAN(rrs->vlan_tag, vlan);
 			vlan = le16_to_cpu(vlan);
-			__vlan_hwaccel_put_tag(skb, vlan);
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan);
 		}
 		netif_receive_skb(skb);
 
@@ -2475,13 +2475,13 @@ static int atl1c_init_netdev(struct net_device *netdev, struct pci_dev *pdev)
 	atl1c_set_ethtool_ops(netdev);
 
 	/* TODO: add when ready */
-	netdev->hw_features =	NETIF_F_SG	   |
-				NETIF_F_HW_CSUM	   |
-				NETIF_F_HW_VLAN_RX |
-				NETIF_F_TSO	   |
+	netdev->hw_features =	NETIF_F_SG		|
+				NETIF_F_HW_CSUM		|
+				NETIF_F_HW_VLAN_CTAG_RX	|
+				NETIF_F_TSO		|
 				NETIF_F_TSO6;
-	netdev->features =	netdev->hw_features |
-				NETIF_F_HW_VLAN_TX;
+	netdev->features =	netdev->hw_features	|
+				NETIF_F_HW_VLAN_CTAG_TX;
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/atheros/atl1e/atl1e_main.c b/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
index ac25f05ff68f..0688bb82b442 100644
--- a/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
+++ b/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
@@ -315,7 +315,7 @@ static void atl1e_set_multi(struct net_device *netdev)
 
 static void __atl1e_vlan_mode(netdev_features_t features, u32 *mac_ctrl_data)
 {
-	if (features & NETIF_F_HW_VLAN_RX) {
+	if (features & NETIF_F_HW_VLAN_CTAG_RX) {
 		/* enable VLAN tag insert/strip */
 		*mac_ctrl_data |= MAC_CTRL_RMV_VLAN;
 	} else {
@@ -378,10 +378,10 @@ static netdev_features_t atl1e_fix_features(struct net_device *netdev,
 	 * Since there is no support for separate rx/tx vlan accel
 	 * enable/disable make sure tx flag is always in same state as rx.
 	 */
-	if (features & NETIF_F_HW_VLAN_RX)
-		features |= NETIF_F_HW_VLAN_TX;
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
+		features |= NETIF_F_HW_VLAN_CTAG_TX;
 	else
-		features &= ~NETIF_F_HW_VLAN_TX;
+		features &= ~NETIF_F_HW_VLAN_CTAG_TX;
 
 	return features;
 }
@@ -391,7 +391,7 @@ static int atl1e_set_features(struct net_device *netdev,
 {
 	netdev_features_t changed = netdev->features ^ features;
 
-	if (changed & NETIF_F_HW_VLAN_RX)
+	if (changed & NETIF_F_HW_VLAN_CTAG_RX)
 		atl1e_vlan_mode(netdev, features);
 
 	return 0;
@@ -1420,11 +1420,9 @@ static void atl1e_clean_rx_irq(struct atl1e_adapter *adapter, u8 que,
 			packet_size = ((prrs->word1 >> RRS_PKT_SIZE_SHIFT) &
 					RRS_PKT_SIZE_MASK) - 4; /* CRC */
 			skb = netdev_alloc_skb_ip_align(netdev, packet_size);
-			if (skb == NULL) {
-				netdev_warn(netdev,
-					    "Memory squeeze, deferring packet\n");
+			if (skb == NULL)
 				goto skip_pkt;
-			}
+
 			memcpy(skb->data, (u8 *)(prrs + 1), packet_size);
 			skb_put(skb, packet_size);
 			skb->protocol = eth_type_trans(skb, netdev);
@@ -1437,7 +1435,7 @@ static void atl1e_clean_rx_irq(struct atl1e_adapter *adapter, u8 que,
 				netdev_dbg(netdev,
 					   "RXD VLAN TAG<RRD>=0x%04x\n",
 					   prrs->vtag);
-				__vlan_hwaccel_put_tag(skb, vlan_tag);
+				__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_tag);
 			}
 			netif_receive_skb(skb);
 
@@ -2200,9 +2198,9 @@ static int atl1e_init_netdev(struct net_device *netdev, struct pci_dev *pdev)
 	atl1e_set_ethtool_ops(netdev);
 
 	netdev->hw_features = NETIF_F_SG | NETIF_F_HW_CSUM | NETIF_F_TSO |
-			      NETIF_F_HW_VLAN_RX;
+			      NETIF_F_HW_VLAN_CTAG_RX;
 	netdev->features = netdev->hw_features | NETIF_F_LLTX |
-			   NETIF_F_HW_VLAN_TX;
+			   NETIF_F_HW_VLAN_CTAG_TX;
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/atheros/atlx/atl1.c b/drivers/net/ethernet/atheros/atlx/atl1.c
index 5b0d9931c720..fa0915f3999b 100644
--- a/drivers/net/ethernet/atheros/atlx/atl1.c
+++ b/drivers/net/ethernet/atheros/atlx/atl1.c
@@ -2024,7 +2024,7 @@ rrd_ok:
 					((rrd->vlan_tag & 7) << 13) |
 					((rrd->vlan_tag & 8) << 9);
 
-			__vlan_hwaccel_put_tag(skb, vlan_tag);
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_tag);
 		}
 		netif_receive_skb(skb);
 
@@ -2774,7 +2774,7 @@ static int atl1_close(struct net_device *netdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int atl1_suspend(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
@@ -2876,23 +2876,18 @@ static int atl1_resume(struct device *dev)
 
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(atl1_pm_ops, atl1_suspend, atl1_resume);
-#define ATL1_PM_OPS	(&atl1_pm_ops)
-
-#else
-
-static int atl1_suspend(struct device *dev) { return 0; }
-
-#define ATL1_PM_OPS	NULL
-#endif
 
 static void atl1_shutdown(struct pci_dev *pdev)
 {
 	struct net_device *netdev = pci_get_drvdata(pdev);
 	struct atl1_adapter *adapter = netdev_priv(netdev);
 
+#ifdef CONFIG_PM_SLEEP
 	atl1_suspend(&pdev->dev);
+#endif
 	pci_wake_from_d3(pdev, adapter->wol);
 	pci_set_power_state(pdev, PCI_D3hot);
 }
@@ -3023,10 +3018,10 @@ static int atl1_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	netdev->features = NETIF_F_HW_CSUM;
 	netdev->features |= NETIF_F_SG;
-	netdev->features |= (NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX);
+	netdev->features |= (NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX);
 
 	netdev->hw_features = NETIF_F_HW_CSUM | NETIF_F_SG | NETIF_F_TSO |
-			      NETIF_F_HW_VLAN_RX;
+			      NETIF_F_HW_VLAN_CTAG_RX;
 
 	/* is this valid? see atl1_setup_mac_ctrl() */
 	netdev->features |= NETIF_F_RXCSUM;
@@ -3147,7 +3142,7 @@ static struct pci_driver atl1_driver = {
 	.probe = atl1_probe,
 	.remove = atl1_remove,
 	.shutdown = atl1_shutdown,
-	.driver.pm = ATL1_PM_OPS,
+	.driver.pm = &atl1_pm_ops,
 };
 
 /**
diff --git a/drivers/net/ethernet/atheros/atlx/atl2.c b/drivers/net/ethernet/atheros/atlx/atl2.c
index 1278b47022e0..265ce1b752ed 100644
--- a/drivers/net/ethernet/atheros/atlx/atl2.c
+++ b/drivers/net/ethernet/atheros/atlx/atl2.c
@@ -363,7 +363,7 @@ static inline void atl2_irq_disable(struct atl2_adapter *adapter)
 
 static void __atl2_vlan_mode(netdev_features_t features, u32 *ctrl)
 {
-	if (features & NETIF_F_HW_VLAN_RX) {
+	if (features & NETIF_F_HW_VLAN_CTAG_RX) {
 		/* enable VLAN tag insert/strip */
 		*ctrl |= MAC_CTRL_RMV_VLAN;
 	} else {
@@ -399,10 +399,10 @@ static netdev_features_t atl2_fix_features(struct net_device *netdev,
 	 * Since there is no support for separate rx/tx vlan accel
 	 * enable/disable make sure tx flag is always in same state as rx.
 	 */
-	if (features & NETIF_F_HW_VLAN_RX)
-		features |= NETIF_F_HW_VLAN_TX;
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
+		features |= NETIF_F_HW_VLAN_CTAG_TX;
 	else
-		features &= ~NETIF_F_HW_VLAN_TX;
+		features &= ~NETIF_F_HW_VLAN_CTAG_TX;
 
 	return features;
 }
@@ -412,7 +412,7 @@ static int atl2_set_features(struct net_device *netdev,
 {
 	netdev_features_t changed = netdev->features ^ features;
 
-	if (changed & NETIF_F_HW_VLAN_RX)
+	if (changed & NETIF_F_HW_VLAN_CTAG_RX)
 		atl2_vlan_mode(netdev, features);
 
 	return 0;
@@ -437,9 +437,6 @@ static void atl2_intr_rx(struct atl2_adapter *adapter)
 			/* alloc new buffer */
 			skb = netdev_alloc_skb_ip_align(netdev, rx_size);
 			if (NULL == skb) {
-				printk(KERN_WARNING
-					"%s: Mem squeeze, deferring packet.\n",
-					netdev->name);
 				/*
 				 * Check that some rx space is free. If not,
 				 * free one and mark stats->rx_dropped++.
@@ -455,7 +452,7 @@ static void atl2_intr_rx(struct atl2_adapter *adapter)
 					((rxd->status.vtag&7) << 13) |
 					((rxd->status.vtag&8) << 9);
 
-				__vlan_hwaccel_put_tag(skb, vlan_tag);
+				__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_tag);
 			}
 			netif_rx(skb);
 			netdev->stats.rx_bytes += rx_size;
@@ -890,7 +887,7 @@ static netdev_tx_t atl2_xmit_frame(struct sk_buff *skb,
 			skb->len-copy_len);
 		offset = ((u32)(skb->len-copy_len + 3) & ~3);
 	}
-#ifdef NETIF_F_HW_VLAN_TX
+#ifdef NETIF_F_HW_VLAN_CTAG_TX
 	if (vlan_tx_tag_present(skb)) {
 		u16 vlan_tag = vlan_tx_tag_get(skb);
 		vlan_tag = (vlan_tag << 4) |
@@ -1416,8 +1413,8 @@ static int atl2_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	err = -EIO;
 
-	netdev->hw_features = NETIF_F_SG | NETIF_F_HW_VLAN_RX;
-	netdev->features |= (NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX);
+	netdev->hw_features = NETIF_F_SG | NETIF_F_HW_VLAN_CTAG_RX;
+	netdev->features |= (NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX);
 
 	/* Init PHY as early as possible due to power saving issue  */
 	atl2_phy_init(&adapter->hw);
diff --git a/drivers/net/ethernet/atheros/atlx/atlx.c b/drivers/net/ethernet/atheros/atlx/atlx.c
index f82eb1699464..46a622cceee4 100644
--- a/drivers/net/ethernet/atheros/atlx/atlx.c
+++ b/drivers/net/ethernet/atheros/atlx/atlx.c
@@ -220,7 +220,7 @@ static void atlx_link_chg_task(struct work_struct *work)
 
 static void __atlx_vlan_mode(netdev_features_t features, u32 *ctrl)
 {
-	if (features & NETIF_F_HW_VLAN_RX) {
+	if (features & NETIF_F_HW_VLAN_CTAG_RX) {
 		/* enable VLAN tag insert/strip */
 		*ctrl |= MAC_CTRL_RMV_VLAN;
 	} else {
@@ -257,10 +257,10 @@ static netdev_features_t atlx_fix_features(struct net_device *netdev,
 	 * Since there is no support for separate rx/tx vlan accel
 	 * enable/disable make sure tx flag is always in same state as rx.
 	 */
-	if (features & NETIF_F_HW_VLAN_RX)
-		features |= NETIF_F_HW_VLAN_TX;
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
+		features |= NETIF_F_HW_VLAN_CTAG_TX;
 	else
-		features &= ~NETIF_F_HW_VLAN_TX;
+		features &= ~NETIF_F_HW_VLAN_CTAG_TX;
 
 	return features;
 }
@@ -270,7 +270,7 @@ static int atlx_set_features(struct net_device *netdev,
 {
 	netdev_features_t changed = netdev->features ^ features;
 
-	if (changed & NETIF_F_HW_VLAN_RX)
+	if (changed & NETIF_F_HW_VLAN_CTAG_RX)
 		atlx_vlan_mode(netdev, features);
 
 	return 0;
diff --git a/drivers/net/ethernet/broadcom/bcm63xx_enet.c b/drivers/net/ethernet/broadcom/bcm63xx_enet.c
index 7d81e059e811..0b3e23ec37f7 100644
--- a/drivers/net/ethernet/broadcom/bcm63xx_enet.c
+++ b/drivers/net/ethernet/broadcom/bcm63xx_enet.c
@@ -862,27 +862,25 @@ static int bcm_enet_open(struct net_device *dev)
 
 	/* allocate rx dma ring */
 	size = priv->rx_ring_size * sizeof(struct bcm_enet_desc);
-	p = dma_alloc_coherent(kdev, size, &priv->rx_desc_dma, GFP_KERNEL);
+	p = dma_alloc_coherent(kdev, size, &priv->rx_desc_dma,
+			       GFP_KERNEL | __GFP_ZERO);
 	if (!p) {
-		dev_err(kdev, "cannot allocate rx ring %u\n", size);
 		ret = -ENOMEM;
 		goto out_freeirq_tx;
 	}
 
-	memset(p, 0, size);
 	priv->rx_desc_alloc_size = size;
 	priv->rx_desc_cpu = p;
 
 	/* allocate tx dma ring */
 	size = priv->tx_ring_size * sizeof(struct bcm_enet_desc);
-	p = dma_alloc_coherent(kdev, size, &priv->tx_desc_dma, GFP_KERNEL);
+	p = dma_alloc_coherent(kdev, size, &priv->tx_desc_dma,
+			       GFP_KERNEL | __GFP_ZERO);
 	if (!p) {
-		dev_err(kdev, "cannot allocate tx ring\n");
 		ret = -ENOMEM;
 		goto out_free_rx_ring;
 	}
 
-	memset(p, 0, size);
 	priv->tx_desc_alloc_size = size;
 	priv->tx_desc_cpu = p;
 
@@ -1619,7 +1617,6 @@ static int bcm_enet_probe(struct platform_device *pdev)
 	struct resource *res_mem, *res_irq, *res_irq_rx, *res_irq_tx;
 	struct mii_bus *bus;
 	const char *clk_name;
-	unsigned int iomem_size;
 	int i, ret;
 
 	/* stop if shared driver failed, assume driver->probe will be
@@ -1644,17 +1641,12 @@ static int bcm_enet_probe(struct platform_device *pdev)
 	if (ret)
 		goto out;
 
-	iomem_size = resource_size(res_mem);
-	if (!request_mem_region(res_mem->start, iomem_size, "bcm63xx_enet")) {
-		ret = -EBUSY;
-		goto out;
-	}
-
-	priv->base = ioremap(res_mem->start, iomem_size);
+	priv->base = devm_request_and_ioremap(&pdev->dev, res_mem);
 	if (priv->base == NULL) {
 		ret = -ENOMEM;
-		goto out_release_mem;
+		goto out;
 	}
+
 	dev->irq = priv->irq = res_irq->start;
 	priv->irq_rx = res_irq_rx->start;
 	priv->irq_tx = res_irq_tx->start;
@@ -1674,9 +1666,9 @@ static int bcm_enet_probe(struct platform_device *pdev)
 	priv->mac_clk = clk_get(&pdev->dev, clk_name);
 	if (IS_ERR(priv->mac_clk)) {
 		ret = PTR_ERR(priv->mac_clk);
-		goto out_unmap;
+		goto out;
 	}
-	clk_enable(priv->mac_clk);
+	clk_prepare_enable(priv->mac_clk);
 
 	/* initialize default and fetch platform data */
 	priv->rx_ring_size = BCMENET_DEF_RX_DESC;
@@ -1705,7 +1697,7 @@ static int bcm_enet_probe(struct platform_device *pdev)
 			priv->phy_clk = NULL;
 			goto out_put_clk_mac;
 		}
-		clk_enable(priv->phy_clk);
+		clk_prepare_enable(priv->phy_clk);
 	}
 
 	/* do minimal hardware init to be able to probe mii bus */
@@ -1733,7 +1725,8 @@ static int bcm_enet_probe(struct platform_device *pdev)
 		 * if a slave is not present on hw */
 		bus->phy_mask = ~(1 << priv->phy_id);
 
-		bus->irq = kmalloc(sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
+		bus->irq = devm_kzalloc(&pdev->dev, sizeof(int) * PHY_MAX_ADDR,
+					GFP_KERNEL);
 		if (!bus->irq) {
 			ret = -ENOMEM;
 			goto out_free_mdio;
@@ -1794,10 +1787,8 @@ static int bcm_enet_probe(struct platform_device *pdev)
 	return 0;
 
 out_unregister_mdio:
-	if (priv->mii_bus) {
+	if (priv->mii_bus)
 		mdiobus_unregister(priv->mii_bus);
-		kfree(priv->mii_bus->irq);
-	}
 
 out_free_mdio:
 	if (priv->mii_bus)
@@ -1807,19 +1798,13 @@ out_uninit_hw:
 	/* turn off mdc clock */
 	enet_writel(priv, 0, ENET_MIISC_REG);
 	if (priv->phy_clk) {
-		clk_disable(priv->phy_clk);
+		clk_disable_unprepare(priv->phy_clk);
 		clk_put(priv->phy_clk);
 	}
 
 out_put_clk_mac:
-	clk_disable(priv->mac_clk);
+	clk_disable_unprepare(priv->mac_clk);
 	clk_put(priv->mac_clk);
-
-out_unmap:
-	iounmap(priv->base);
-
-out_release_mem:
-	release_mem_region(res_mem->start, iomem_size);
 out:
 	free_netdev(dev);
 	return ret;
@@ -1833,7 +1818,6 @@ static int bcm_enet_remove(struct platform_device *pdev)
 {
 	struct bcm_enet_priv *priv;
 	struct net_device *dev;
-	struct resource *res;
 
 	/* stop netdevice */
 	dev = platform_get_drvdata(pdev);
@@ -1845,7 +1829,6 @@ static int bcm_enet_remove(struct platform_device *pdev)
 
 	if (priv->has_phy) {
 		mdiobus_unregister(priv->mii_bus);
-		kfree(priv->mii_bus->irq);
 		mdiobus_free(priv->mii_bus);
 	} else {
 		struct bcm63xx_enet_platform_data *pd;
@@ -1856,17 +1839,12 @@ static int bcm_enet_remove(struct platform_device *pdev)
 				       bcm_enet_mdio_write_mii);
 	}
 
-	/* release device resources */
-	iounmap(priv->base);
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	release_mem_region(res->start, resource_size(res));
-
 	/* disable hw block clocks */
 	if (priv->phy_clk) {
-		clk_disable(priv->phy_clk);
+		clk_disable_unprepare(priv->phy_clk);
 		clk_put(priv->phy_clk);
 	}
-	clk_disable(priv->mac_clk);
+	clk_disable_unprepare(priv->mac_clk);
 	clk_put(priv->mac_clk);
 
 	platform_set_drvdata(pdev, NULL);
@@ -1889,31 +1867,20 @@ struct platform_driver bcm63xx_enet_driver = {
 static int bcm_enet_shared_probe(struct platform_device *pdev)
 {
 	struct resource *res;
-	unsigned int iomem_size;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res)
 		return -ENODEV;
 
-	iomem_size = resource_size(res);
-	if (!request_mem_region(res->start, iomem_size, "bcm63xx_enet_dma"))
-		return -EBUSY;
-
-	bcm_enet_shared_base = ioremap(res->start, iomem_size);
-	if (!bcm_enet_shared_base) {
-		release_mem_region(res->start, iomem_size);
+	bcm_enet_shared_base = devm_request_and_ioremap(&pdev->dev, res);
+	if (!bcm_enet_shared_base)
 		return -ENOMEM;
-	}
+
 	return 0;
 }
 
 static int bcm_enet_shared_remove(struct platform_device *pdev)
 {
-	struct resource *res;
-
-	iounmap(bcm_enet_shared_base);
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	release_mem_region(res->start, resource_size(res));
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/broadcom/bgmac.c b/drivers/net/ethernet/broadcom/bgmac.c
index da5f4397f87c..eec0af45b859 100644
--- a/drivers/net/ethernet/broadcom/bgmac.c
+++ b/drivers/net/ethernet/broadcom/bgmac.c
@@ -13,6 +13,7 @@
 #include <linux/delay.h>
 #include <linux/etherdevice.h>
 #include <linux/mii.h>
+#include <linux/phy.h>
 #include <linux/interrupt.h>
 #include <linux/dma-mapping.h>
 #include <bcm47xx_nvram.h>
@@ -244,10 +245,8 @@ static int bgmac_dma_rx_skb_for_slot(struct bgmac *bgmac,
 
 	/* Alloc skb */
 	slot->skb = netdev_alloc_skb(bgmac->net_dev, BGMAC_RX_BUF_SIZE);
-	if (!slot->skb) {
-		bgmac_err(bgmac, "Allocation of skb failed!\n");
+	if (!slot->skb)
 		return -ENOMEM;
-	}
 
 	/* Poison - if everything goes fine, hardware will overwrite it */
 	rx = (struct bgmac_rx_header *)slot->skb->data;
@@ -1313,6 +1312,73 @@ static const struct ethtool_ops bgmac_ethtool_ops = {
 };
 
 /**************************************************
+ * MII
+ **************************************************/
+
+static int bgmac_mii_read(struct mii_bus *bus, int mii_id, int regnum)
+{
+	return bgmac_phy_read(bus->priv, mii_id, regnum);
+}
+
+static int bgmac_mii_write(struct mii_bus *bus, int mii_id, int regnum,
+			   u16 value)
+{
+	return bgmac_phy_write(bus->priv, mii_id, regnum, value);
+}
+
+static int bgmac_mii_register(struct bgmac *bgmac)
+{
+	struct mii_bus *mii_bus;
+	int i, err = 0;
+
+	mii_bus = mdiobus_alloc();
+	if (!mii_bus)
+		return -ENOMEM;
+
+	mii_bus->name = "bgmac mii bus";
+	sprintf(mii_bus->id, "%s-%d-%d", "bgmac", bgmac->core->bus->num,
+		bgmac->core->core_unit);
+	mii_bus->priv = bgmac;
+	mii_bus->read = bgmac_mii_read;
+	mii_bus->write = bgmac_mii_write;
+	mii_bus->parent = &bgmac->core->dev;
+	mii_bus->phy_mask = ~(1 << bgmac->phyaddr);
+
+	mii_bus->irq = kmalloc_array(PHY_MAX_ADDR, sizeof(int), GFP_KERNEL);
+	if (!mii_bus->irq) {
+		err = -ENOMEM;
+		goto err_free_bus;
+	}
+	for (i = 0; i < PHY_MAX_ADDR; i++)
+		mii_bus->irq[i] = PHY_POLL;
+
+	err = mdiobus_register(mii_bus);
+	if (err) {
+		bgmac_err(bgmac, "Registration of mii bus failed\n");
+		goto err_free_irq;
+	}
+
+	bgmac->mii_bus = mii_bus;
+
+	return err;
+
+err_free_irq:
+	kfree(mii_bus->irq);
+err_free_bus:
+	mdiobus_free(mii_bus);
+	return err;
+}
+
+static void bgmac_mii_unregister(struct bgmac *bgmac)
+{
+	struct mii_bus *mii_bus = bgmac->mii_bus;
+
+	mdiobus_unregister(mii_bus);
+	kfree(mii_bus->irq);
+	mdiobus_free(mii_bus);
+}
+
+/**************************************************
  * BCMA bus ops
  **************************************************/
 
@@ -1404,11 +1470,18 @@ static int bgmac_probe(struct bcma_device *core)
 	if (core->bus->sprom.boardflags_lo & BGMAC_BFL_ENETADM)
 		bgmac_warn(bgmac, "Support for ADMtek ethernet switch not implemented\n");
 
+	err = bgmac_mii_register(bgmac);
+	if (err) {
+		bgmac_err(bgmac, "Cannot register MDIO\n");
+		err = -ENOTSUPP;
+		goto err_dma_free;
+	}
+
 	err = register_netdev(bgmac->net_dev);
 	if (err) {
 		bgmac_err(bgmac, "Cannot register net device\n");
 		err = -ENOTSUPP;
-		goto err_dma_free;
+		goto err_mii_unregister;
 	}
 
 	netif_carrier_off(net_dev);
@@ -1417,6 +1490,8 @@ static int bgmac_probe(struct bcma_device *core)
 
 	return 0;
 
+err_mii_unregister:
+	bgmac_mii_unregister(bgmac);
 err_dma_free:
 	bgmac_dma_free(bgmac);
 
@@ -1433,6 +1508,7 @@ static void bgmac_remove(struct bcma_device *core)
 
 	netif_napi_del(&bgmac->napi);
 	unregister_netdev(bgmac->net_dev);
+	bgmac_mii_unregister(bgmac);
 	bgmac_dma_free(bgmac);
 	bcma_set_drvdata(core, NULL);
 	free_netdev(bgmac->net_dev);
diff --git a/drivers/net/ethernet/broadcom/bgmac.h b/drivers/net/ethernet/broadcom/bgmac.h
index 4ede614c81f8..98d4b5fcc070 100644
--- a/drivers/net/ethernet/broadcom/bgmac.h
+++ b/drivers/net/ethernet/broadcom/bgmac.h
@@ -399,6 +399,7 @@ struct bgmac {
 	struct bcma_device *cmn; /* Reference to CMN core for BCM4706 */
 	struct net_device *net_dev;
 	struct napi_struct napi;
+	struct mii_bus *mii_bus;
 
 	/* DMA */
 	struct bgmac_dma_ring tx_ring[BGMAC_MAX_TX_RINGS];
diff --git a/drivers/net/ethernet/broadcom/bnx2.c b/drivers/net/ethernet/broadcom/bnx2.c
index 2f0ba8f2fd6c..5d204492c603 100644
--- a/drivers/net/ethernet/broadcom/bnx2.c
+++ b/drivers/net/ethernet/broadcom/bnx2.c
@@ -416,7 +416,7 @@ static int bnx2_unregister_cnic(struct net_device *dev)
 	return 0;
 }
 
-struct cnic_eth_dev *bnx2_cnic_probe(struct net_device *dev)
+static struct cnic_eth_dev *bnx2_cnic_probe(struct net_device *dev)
 {
 	struct bnx2 *bp = netdev_priv(dev);
 	struct cnic_eth_dev *cp = &bp->cnic_eth_dev;
@@ -854,12 +854,11 @@ bnx2_alloc_mem(struct bnx2 *bp)
 				sizeof(struct statistics_block);
 
 	status_blk = dma_alloc_coherent(&bp->pdev->dev, bp->status_stats_size,
-					&bp->status_blk_mapping, GFP_KERNEL);
+					&bp->status_blk_mapping,
+					GFP_KERNEL | __GFP_ZERO);
 	if (status_blk == NULL)
 		goto alloc_mem_err;
 
-	memset(status_blk, 0, bp->status_stats_size);
-
 	bnapi = &bp->bnx2_napi[0];
 	bnapi->status_blk.msi = status_blk;
 	bnapi->hw_tx_cons_ptr =
@@ -3212,7 +3211,7 @@ bnx2_rx_int(struct bnx2 *bp, struct bnx2_napi *bnapi, int budget)
 		}
 		if ((status & L2_FHDR_STATUS_L2_VLAN_TAG) &&
 		    !(bp->rx_mode & BNX2_EMAC_RX_MODE_KEEP_VLAN_TAG))
-			__vlan_hwaccel_put_tag(skb, rx_hdr->l2_fhdr_vlan_tag);
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), rx_hdr->l2_fhdr_vlan_tag);
 
 		skb->protocol = eth_type_trans(skb, bp->dev);
 
@@ -3554,7 +3553,7 @@ bnx2_set_rx_mode(struct net_device *dev)
 	rx_mode = bp->rx_mode & ~(BNX2_EMAC_RX_MODE_PROMISCUOUS |
 				  BNX2_EMAC_RX_MODE_KEEP_VLAN_TAG);
 	sort_mode = 1 | BNX2_RPM_SORT_USER0_BC_EN;
-	if (!(dev->features & NETIF_F_HW_VLAN_RX) &&
+	if (!(dev->features & NETIF_F_HW_VLAN_CTAG_RX) &&
 	     (bp->flags & BNX2_FLAG_CAN_KEEP_VLAN))
 		rx_mode |= BNX2_EMAC_RX_MODE_KEEP_VLAN_TAG;
 	if (dev->flags & IFF_PROMISC) {
@@ -7696,7 +7695,7 @@ bnx2_fix_features(struct net_device *dev, netdev_features_t features)
 	struct bnx2 *bp = netdev_priv(dev);
 
 	if (!(bp->flags & BNX2_FLAG_CAN_KEEP_VLAN))
-		features |= NETIF_F_HW_VLAN_RX;
+		features |= NETIF_F_HW_VLAN_CTAG_RX;
 
 	return features;
 }
@@ -7707,12 +7706,12 @@ bnx2_set_features(struct net_device *dev, netdev_features_t features)
 	struct bnx2 *bp = netdev_priv(dev);
 
 	/* TSO with VLAN tag won't work with current firmware */
-	if (features & NETIF_F_HW_VLAN_TX)
+	if (features & NETIF_F_HW_VLAN_CTAG_TX)
 		dev->vlan_features |= (dev->hw_features & NETIF_F_ALL_TSO);
 	else
 		dev->vlan_features &= ~NETIF_F_ALL_TSO;
 
-	if ((!!(features & NETIF_F_HW_VLAN_RX) !=
+	if ((!!(features & NETIF_F_HW_VLAN_CTAG_RX) !=
 	    !!(bp->rx_mode & BNX2_EMAC_RX_MODE_KEEP_VLAN_TAG)) &&
 	    netif_running(dev)) {
 		bnx2_netif_stop(bp, false);
@@ -8552,7 +8551,7 @@ bnx2_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		dev->hw_features |= NETIF_F_IPV6_CSUM | NETIF_F_TSO6;
 
 	dev->vlan_features = dev->hw_features;
-	dev->hw_features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
+	dev->hw_features |= NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX;
 	dev->features |= dev->hw_features;
 	dev->priv_flags |= IFF_UNICAST_FLT;
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
index e4605a965084..3dba2a70a00e 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
@@ -26,8 +26,8 @@
  * (you will need to reboot afterwards) */
 /* #define BNX2X_STOP_ON_ERROR */
 
-#define DRV_MODULE_VERSION      "1.78.02-0"
-#define DRV_MODULE_RELDATE      "2013/01/14"
+#define DRV_MODULE_VERSION      "1.78.17-0"
+#define DRV_MODULE_RELDATE      "2013/04/11"
 #define BNX2X_BC_VER            0x040200
 
 #if defined(CONFIG_DCB)
@@ -492,7 +492,6 @@ enum bnx2x_tpa_mode_t {
 struct bnx2x_fastpath {
 	struct bnx2x		*bp; /* parent */
 
-#define BNX2X_NAPI_WEIGHT       128
 	struct napi_struct	napi;
 	union host_hc_status_block	status_blk;
 	/* chip independed shortcuts into sb structure */
@@ -613,9 +612,10 @@ struct bnx2x_fastpath {
  * START_BD		- describes packed
  * START_BD(splitted)	- includes unpaged data segment for GSO
  * PARSING_BD		- for TSO and CSUM data
+ * PARSING_BD2		- for encapsulation data
  * Frag BDs		- decribes pages for frags
  */
-#define BDS_PER_TX_PKT		3
+#define BDS_PER_TX_PKT		4
 #define MAX_BDS_PER_TX_PKT	(MAX_SKB_FRAGS + BDS_PER_TX_PKT)
 /* max BDs per tx packet including next pages */
 #define MAX_DESC_PER_TX_PKT	(MAX_BDS_PER_TX_PKT + \
@@ -730,18 +730,24 @@ struct bnx2x_fastpath {
 #define SKB_CS(skb)		(*(u16 *)(skb_transport_header(skb) + \
 					  skb->csum_offset))
 
-#define pbd_tcp_flags(skb)	(ntohl(tcp_flag_word(tcp_hdr(skb)))>>16 & 0xff)
+#define pbd_tcp_flags(tcp_hdr)	(ntohl(tcp_flag_word(tcp_hdr))>>16 & 0xff)
 
-#define XMIT_PLAIN			0
-#define XMIT_CSUM_V4			0x1
-#define XMIT_CSUM_V6			0x2
-#define XMIT_CSUM_TCP			0x4
-#define XMIT_GSO_V4			0x8
-#define XMIT_GSO_V6			0x10
+#define XMIT_PLAIN		0
+#define XMIT_CSUM_V4		(1 << 0)
+#define XMIT_CSUM_V6		(1 << 1)
+#define XMIT_CSUM_TCP		(1 << 2)
+#define XMIT_GSO_V4		(1 << 3)
+#define XMIT_GSO_V6		(1 << 4)
+#define XMIT_CSUM_ENC_V4	(1 << 5)
+#define XMIT_CSUM_ENC_V6	(1 << 6)
+#define XMIT_GSO_ENC_V4		(1 << 7)
+#define XMIT_GSO_ENC_V6		(1 << 8)
 
-#define XMIT_CSUM			(XMIT_CSUM_V4 | XMIT_CSUM_V6)
-#define XMIT_GSO			(XMIT_GSO_V4 | XMIT_GSO_V6)
+#define XMIT_CSUM_ENC		(XMIT_CSUM_ENC_V4 | XMIT_CSUM_ENC_V6)
+#define XMIT_GSO_ENC		(XMIT_GSO_ENC_V4 | XMIT_GSO_ENC_V6)
 
+#define XMIT_CSUM		(XMIT_CSUM_V4 | XMIT_CSUM_V6 | XMIT_CSUM_ENC)
+#define XMIT_GSO		(XMIT_GSO_V4 | XMIT_GSO_V6 | XMIT_GSO_ENC)
 
 /* stuff added to make the code fit 80Col */
 #define CQE_TYPE(cqe_fp_flags)	 ((cqe_fp_flags) & ETH_FAST_PATH_RX_CQE_TYPE)
@@ -844,6 +850,9 @@ struct bnx2x_common {
 #define CHIP_IS_57840_VF(bp)		(CHIP_NUM(bp) == CHIP_NUM_57840_VF)
 #define CHIP_IS_E1H(bp)			(CHIP_IS_57711(bp) || \
 					 CHIP_IS_57711E(bp))
+#define CHIP_IS_57811xx(bp)		(CHIP_IS_57811(bp) || \
+					 CHIP_IS_57811_MF(bp) || \
+					 CHIP_IS_57811_VF(bp))
 #define CHIP_IS_E2(bp)			(CHIP_IS_57712(bp) || \
 					 CHIP_IS_57712_MF(bp) || \
 					 CHIP_IS_57712_VF(bp))
@@ -853,9 +862,7 @@ struct bnx2x_common {
 					 CHIP_IS_57810(bp) || \
 					 CHIP_IS_57810_MF(bp) || \
 					 CHIP_IS_57810_VF(bp) || \
-					 CHIP_IS_57811(bp) || \
-					 CHIP_IS_57811_MF(bp) || \
-					 CHIP_IS_57811_VF(bp) || \
+					 CHIP_IS_57811xx(bp) || \
 					 CHIP_IS_57840(bp) || \
 					 CHIP_IS_57840_MF(bp) || \
 					 CHIP_IS_57840_VF(bp))
@@ -1215,14 +1222,16 @@ enum {
 	BNX2X_SP_RTNL_ENABLE_SRIOV,
 	BNX2X_SP_RTNL_VFPF_MCAST,
 	BNX2X_SP_RTNL_VFPF_STORM_RX_MODE,
+	BNX2X_SP_RTNL_HYPERVISOR_VLAN,
 };
 
 
 struct bnx2x_prev_path_list {
+	struct list_head list;
 	u8 bus;
 	u8 slot;
 	u8 path;
-	struct list_head list;
+	u8 aer;
 	u8 undi;
 };
 
@@ -1269,6 +1278,8 @@ struct bnx2x {
 #define BP_FW_MB_IDX(bp)		BP_FW_MB_IDX_VN(bp, BP_VN(bp))
 
 #ifdef CONFIG_BNX2X_SRIOV
+	/* protects vf2pf mailbox from simultaneous access */
+	struct mutex		vf2pf_mutex;
 	/* vf pf channel mailbox contains request and response buffers */
 	struct bnx2x_vf_mbx_msg	*vf2pf_mbox;
 	dma_addr_t		vf2pf_mbox_mapping;
@@ -1281,6 +1292,8 @@ struct bnx2x {
 	dma_addr_t		pf2vf_bulletin_mapping;
 
 	struct pf_vf_bulletin_content	old_bulletin;
+
+	u16 requested_nr_virtfn;
 #endif /* CONFIG_BNX2X_SRIOV */
 
 	struct net_device	*dev;
@@ -1944,12 +1957,9 @@ static inline u32 reg_poll(struct bnx2x *bp, u32 reg, u32 expected, int ms,
 void bnx2x_igu_clear_sb_gen(struct bnx2x *bp, u8 func, u8 idu_sb_id,
 			    bool is_pf);
 
-#define BNX2X_ILT_ZALLOC(x, y, size) \
-	do { \
-		x = dma_alloc_coherent(&bp->pdev->dev, size, y, GFP_KERNEL); \
-		if (x) \
-			memset(x, 0, size); \
-	} while (0)
+#define BNX2X_ILT_ZALLOC(x, y, size)				\
+	x = dma_alloc_coherent(&bp->pdev->dev, size, y,		\
+			       GFP_KERNEL | __GFP_ZERO)
 
 #define BNX2X_ILT_FREE(x, y, size) \
 	do { \
@@ -2286,7 +2296,7 @@ static const u32 dmae_reg_go_c[] = {
 	DMAE_REG_GO_C12, DMAE_REG_GO_C13, DMAE_REG_GO_C14, DMAE_REG_GO_C15
 };
 
-void bnx2x_set_ethtool_ops(struct net_device *netdev);
+void bnx2x_set_ethtool_ops(struct bnx2x *bp, struct net_device *netdev);
 void bnx2x_notify_link_changed(struct bnx2x *bp);
 
 #define BNX2X_MF_SD_PROTOCOL(bp) \
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
index 57619dd4a92b..b8fbe266ab68 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
@@ -451,7 +451,8 @@ static void bnx2x_tpa_start(struct bnx2x_fastpath *fp, u16 queue,
  * Compute number of aggregated segments, and gso_type.
  */
 static void bnx2x_set_gro_params(struct sk_buff *skb, u16 parsing_flags,
-				 u16 len_on_bd, unsigned int pkt_len)
+				 u16 len_on_bd, unsigned int pkt_len,
+				 u16 num_of_coalesced_segs)
 {
 	/* TPA aggregation won't have either IP options or TCP options
 	 * other than timestamp or IPv6 extension headers.
@@ -480,8 +481,7 @@ static void bnx2x_set_gro_params(struct sk_buff *skb, u16 parsing_flags,
 	/* tcp_gro_complete() will copy NAPI_GRO_CB(skb)->count
 	 * to skb_shinfo(skb)->gso_segs
 	 */
-	NAPI_GRO_CB(skb)->count = DIV_ROUND_UP(pkt_len - hdrs_len,
-					       skb_shinfo(skb)->gso_size);
+	NAPI_GRO_CB(skb)->count = num_of_coalesced_segs;
 }
 
 static int bnx2x_alloc_rx_sge(struct bnx2x *bp,
@@ -537,7 +537,8 @@ static int bnx2x_fill_frag_skb(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 	/* This is needed in order to enable forwarding support */
 	if (frag_size)
 		bnx2x_set_gro_params(skb, tpa_info->parsing_flags, len_on_bd,
-				     le16_to_cpu(cqe->pkt_len));
+				     le16_to_cpu(cqe->pkt_len),
+				     le16_to_cpu(cqe->num_of_coalesced_segs));
 
 #ifdef BNX2X_STOP_ON_ERROR
 	if (pages > min_t(u32, 8, MAX_SKB_FRAGS) * SGE_PAGES) {
@@ -641,6 +642,14 @@ static void bnx2x_gro_ipv6_csum(struct bnx2x *bp, struct sk_buff *skb)
 	th->check = ~tcp_v6_check(skb->len - skb_transport_offset(skb),
 				  &iph->saddr, &iph->daddr, 0);
 }
+
+static void bnx2x_gro_csum(struct bnx2x *bp, struct sk_buff *skb,
+			    void (*gro_func)(struct bnx2x*, struct sk_buff*))
+{
+	skb_set_network_header(skb, 0);
+	gro_func(bp, skb);
+	tcp_gro_complete(skb);
+}
 #endif
 
 static void bnx2x_gro_receive(struct bnx2x *bp, struct bnx2x_fastpath *fp,
@@ -648,19 +657,17 @@ static void bnx2x_gro_receive(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 {
 #ifdef CONFIG_INET
 	if (skb_shinfo(skb)->gso_size) {
-		skb_set_network_header(skb, 0);
 		switch (be16_to_cpu(skb->protocol)) {
 		case ETH_P_IP:
-			bnx2x_gro_ip_csum(bp, skb);
+			bnx2x_gro_csum(bp, skb, bnx2x_gro_ip_csum);
 			break;
 		case ETH_P_IPV6:
-			bnx2x_gro_ipv6_csum(bp, skb);
+			bnx2x_gro_csum(bp, skb, bnx2x_gro_ipv6_csum);
 			break;
 		default:
-			BNX2X_ERR("FW GRO supports only IPv4/IPv6, not 0x%04x\n",
+			BNX2X_ERR("Error: FW GRO supports only IPv4/IPv6, not 0x%04x\n",
 				  be16_to_cpu(skb->protocol));
 		}
-		tcp_gro_complete(skb);
 	}
 #endif
 	napi_gro_receive(&fp->napi, skb);
@@ -718,7 +725,7 @@ static void bnx2x_tpa_stop(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 		if (!bnx2x_fill_frag_skb(bp, fp, tpa_info, pages,
 					 skb, cqe, cqe_idx)) {
 			if (tpa_info->parsing_flags & PARSING_FLAGS_VLAN)
-				__vlan_hwaccel_put_tag(skb, tpa_info->vlan_tag);
+				__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), tpa_info->vlan_tag);
 			bnx2x_gro_receive(bp, fp, skb);
 		} else {
 			DP(NETIF_MSG_RX_STATUS,
@@ -993,7 +1000,7 @@ reuse_rx:
 
 		if (le16_to_cpu(cqe_fp->pars_flags.flags) &
 		    PARSING_FLAGS_VLAN)
-			__vlan_hwaccel_put_tag(skb,
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
 					       le16_to_cpu(cqe_fp->vlan_tag));
 		napi_gro_receive(&fp->napi, skb);
 
@@ -1037,6 +1044,7 @@ static irqreturn_t bnx2x_msix_fp_int(int irq, void *fp_cookie)
 	DP(NETIF_MSG_INTR,
 	   "got an MSI-X interrupt on IDX:SB [fp %d fw_sd %d igusb %d]\n",
 	   fp->index, fp->fw_sb_id, fp->igu_sb_id);
+
 	bnx2x_ack_sb(bp, fp->igu_sb_id, USTORM_ID, 0, IGU_INT_DISABLE, 0);
 
 #ifdef BNX2X_STOP_ON_ERROR
@@ -1718,7 +1726,7 @@ static int bnx2x_req_irq(struct bnx2x *bp)
 	return request_irq(irq, bnx2x_interrupt, flags, bp->dev->name, bp->dev);
 }
 
-static int bnx2x_setup_irqs(struct bnx2x *bp)
+int bnx2x_setup_irqs(struct bnx2x *bp)
 {
 	int rc = 0;
 	if (bp->flags & USING_MSIX_FLAG &&
@@ -2009,7 +2017,7 @@ static int bnx2x_init_hw(struct bnx2x *bp, u32 load_code)
  * Cleans the object that have internal lists without sending
  * ramrods. Should be run when interrutps are disabled.
  */
-static void bnx2x_squeeze_objects(struct bnx2x *bp)
+void bnx2x_squeeze_objects(struct bnx2x *bp)
 {
 	int rc;
 	unsigned long ramrod_flags = 0, vlan_mac_flags = 0;
@@ -2574,6 +2582,8 @@ int bnx2x_nic_load(struct bnx2x *bp, int load_mode)
 		}
 	}
 
+	bnx2x_pre_irq_nic_init(bp);
+
 	/* Connect to IRQs */
 	rc = bnx2x_setup_irqs(bp);
 	if (rc) {
@@ -2583,11 +2593,11 @@ int bnx2x_nic_load(struct bnx2x *bp, int load_mode)
 		LOAD_ERROR_EXIT(bp, load_error2);
 	}
 
-	/* Setup NIC internals and enable interrupts */
-	bnx2x_nic_init(bp, load_code);
-
 	/* Init per-function objects */
 	if (IS_PF(bp)) {
+		/* Setup NIC internals and enable interrupts */
+		bnx2x_post_irq_nic_init(bp, load_code);
+
 		bnx2x_init_bp_objs(bp);
 		bnx2x_iov_nic_init(bp);
 
@@ -2657,7 +2667,8 @@ int bnx2x_nic_load(struct bnx2x *bp, int load_mode)
 	if (IS_PF(bp))
 		rc = bnx2x_set_eth_mac(bp, true);
 	else /* vf */
-		rc = bnx2x_vfpf_set_mac(bp);
+		rc = bnx2x_vfpf_config_mac(bp, bp->dev->dev_addr, bp->fp->index,
+					   true);
 	if (rc) {
 		BNX2X_ERR("Setting Ethernet MAC failed\n");
 		LOAD_ERROR_EXIT(bp, load_error3);
@@ -2777,7 +2788,7 @@ load_error0:
 #endif /* ! BNX2X_STOP_ON_ERROR */
 }
 
-static int bnx2x_drain_tx_queues(struct bnx2x *bp)
+int bnx2x_drain_tx_queues(struct bnx2x *bp)
 {
 	u8 rc = 0, cos, i;
 
@@ -2926,9 +2937,9 @@ int bnx2x_nic_unload(struct bnx2x *bp, int unload_mode, bool keep_link)
 		bnx2x_free_fp_mem_cnic(bp);
 
 	if (IS_PF(bp)) {
-		bnx2x_free_mem(bp);
 		if (CNIC_LOADED(bp))
 			bnx2x_free_mem_cnic(bp);
+		bnx2x_free_mem(bp);
 	}
 	bp->state = BNX2X_STATE_CLOSED;
 	bp->cnic_loaded = false;
@@ -3089,11 +3100,11 @@ int bnx2x_poll(struct napi_struct *napi, int budget)
  * to ease the pain of our fellow microcode engineers
  * we use one mapping for both BDs
  */
-static noinline u16 bnx2x_tx_split(struct bnx2x *bp,
-				   struct bnx2x_fp_txdata *txdata,
-				   struct sw_tx_bd *tx_buf,
-				   struct eth_tx_start_bd **tx_bd, u16 hlen,
-				   u16 bd_prod, int nbd)
+static u16 bnx2x_tx_split(struct bnx2x *bp,
+			  struct bnx2x_fp_txdata *txdata,
+			  struct sw_tx_bd *tx_buf,
+			  struct eth_tx_start_bd **tx_bd, u16 hlen,
+			  u16 bd_prod)
 {
 	struct eth_tx_start_bd *h_tx_bd = *tx_bd;
 	struct eth_tx_bd *d_tx_bd;
@@ -3101,11 +3112,10 @@ static noinline u16 bnx2x_tx_split(struct bnx2x *bp,
 	int old_len = le16_to_cpu(h_tx_bd->nbytes);
 
 	/* first fix first BD */
-	h_tx_bd->nbd = cpu_to_le16(nbd);
 	h_tx_bd->nbytes = cpu_to_le16(hlen);
 
-	DP(NETIF_MSG_TX_QUEUED,	"TSO split header size is %d (%x:%x) nbd %d\n",
-	   h_tx_bd->nbytes, h_tx_bd->addr_hi, h_tx_bd->addr_lo, h_tx_bd->nbd);
+	DP(NETIF_MSG_TX_QUEUED,	"TSO split header size is %d (%x:%x)\n",
+	   h_tx_bd->nbytes, h_tx_bd->addr_hi, h_tx_bd->addr_lo);
 
 	/* now get a new data BD
 	 * (after the pbd) and fill it */
@@ -3134,7 +3144,7 @@ static noinline u16 bnx2x_tx_split(struct bnx2x *bp,
 
 #define bswab32(b32) ((__force __le32) swab32((__force __u32) (b32)))
 #define bswab16(b16) ((__force __le16) swab16((__force __u16) (b16)))
-static inline __le16 bnx2x_csum_fix(unsigned char *t_header, u16 csum, s8 fix)
+static __le16 bnx2x_csum_fix(unsigned char *t_header, u16 csum, s8 fix)
 {
 	__sum16 tsum = (__force __sum16) csum;
 
@@ -3149,30 +3159,47 @@ static inline __le16 bnx2x_csum_fix(unsigned char *t_header, u16 csum, s8 fix)
 	return bswab16(tsum);
 }
 
-static inline u32 bnx2x_xmit_type(struct bnx2x *bp, struct sk_buff *skb)
+static u32 bnx2x_xmit_type(struct bnx2x *bp, struct sk_buff *skb)
 {
 	u32 rc;
+	__u8 prot = 0;
+	__be16 protocol;
 
 	if (skb->ip_summed != CHECKSUM_PARTIAL)
-		rc = XMIT_PLAIN;
+		return XMIT_PLAIN;
 
-	else {
-		if (vlan_get_protocol(skb) == htons(ETH_P_IPV6)) {
-			rc = XMIT_CSUM_V6;
-			if (ipv6_hdr(skb)->nexthdr == IPPROTO_TCP)
-				rc |= XMIT_CSUM_TCP;
+	protocol = vlan_get_protocol(skb);
+	if (protocol == htons(ETH_P_IPV6)) {
+		rc = XMIT_CSUM_V6;
+		prot = ipv6_hdr(skb)->nexthdr;
+	} else {
+		rc = XMIT_CSUM_V4;
+		prot = ip_hdr(skb)->protocol;
+	}
 
+	if (!CHIP_IS_E1x(bp) && skb->encapsulation) {
+		if (inner_ip_hdr(skb)->version == 6) {
+			rc |= XMIT_CSUM_ENC_V6;
+			if (inner_ipv6_hdr(skb)->nexthdr == IPPROTO_TCP)
+				rc |= XMIT_CSUM_TCP;
 		} else {
-			rc = XMIT_CSUM_V4;
-			if (ip_hdr(skb)->protocol == IPPROTO_TCP)
+			rc |= XMIT_CSUM_ENC_V4;
+			if (inner_ip_hdr(skb)->protocol == IPPROTO_TCP)
 				rc |= XMIT_CSUM_TCP;
 		}
 	}
+	if (prot == IPPROTO_TCP)
+		rc |= XMIT_CSUM_TCP;
 
-	if (skb_is_gso_v6(skb))
-		rc |= XMIT_GSO_V6 | XMIT_CSUM_TCP | XMIT_CSUM_V6;
-	else if (skb_is_gso(skb))
-		rc |= XMIT_GSO_V4 | XMIT_CSUM_V4 | XMIT_CSUM_TCP;
+	if (skb_is_gso_v6(skb)) {
+		rc |= (XMIT_GSO_V6 | XMIT_CSUM_TCP | XMIT_CSUM_V6);
+		if (rc & XMIT_CSUM_ENC)
+			rc |= XMIT_GSO_ENC_V6;
+	} else if (skb_is_gso(skb)) {
+		rc |= (XMIT_GSO_V4 | XMIT_CSUM_V4 | XMIT_CSUM_TCP);
+		if (rc & XMIT_CSUM_ENC)
+			rc |= XMIT_GSO_ENC_V4;
+	}
 
 	return rc;
 }
@@ -3257,14 +3284,23 @@ exit_lbl:
 }
 #endif
 
-static inline void bnx2x_set_pbd_gso_e2(struct sk_buff *skb, u32 *parsing_data,
-					u32 xmit_type)
+static void bnx2x_set_pbd_gso_e2(struct sk_buff *skb, u32 *parsing_data,
+				 u32 xmit_type)
 {
+	struct ipv6hdr *ipv6;
+
 	*parsing_data |= (skb_shinfo(skb)->gso_size <<
 			      ETH_TX_PARSE_BD_E2_LSO_MSS_SHIFT) &
 			      ETH_TX_PARSE_BD_E2_LSO_MSS;
-	if ((xmit_type & XMIT_GSO_V6) &&
-	    (ipv6_hdr(skb)->nexthdr == NEXTHDR_IPV6))
+
+	if (xmit_type & XMIT_GSO_ENC_V6)
+		ipv6 = inner_ipv6_hdr(skb);
+	else if (xmit_type & XMIT_GSO_V6)
+		ipv6 = ipv6_hdr(skb);
+	else
+		ipv6 = NULL;
+
+	if (ipv6 && ipv6->nexthdr == NEXTHDR_IPV6)
 		*parsing_data |= ETH_TX_PARSE_BD_E2_IPV6_WITH_EXT_HDR;
 }
 
@@ -3275,13 +3311,13 @@ static inline void bnx2x_set_pbd_gso_e2(struct sk_buff *skb, u32 *parsing_data,
  * @pbd:	parse BD
  * @xmit_type:	xmit flags
  */
-static inline void bnx2x_set_pbd_gso(struct sk_buff *skb,
-				     struct eth_tx_parse_bd_e1x *pbd,
-				     u32 xmit_type)
+static void bnx2x_set_pbd_gso(struct sk_buff *skb,
+			      struct eth_tx_parse_bd_e1x *pbd,
+			      u32 xmit_type)
 {
 	pbd->lso_mss = cpu_to_le16(skb_shinfo(skb)->gso_size);
 	pbd->tcp_send_seq = bswab32(tcp_hdr(skb)->seq);
-	pbd->tcp_flags = pbd_tcp_flags(skb);
+	pbd->tcp_flags = pbd_tcp_flags(tcp_hdr(skb));
 
 	if (xmit_type & XMIT_GSO_V4) {
 		pbd->ip_id = bswab16(ip_hdr(skb)->id);
@@ -3301,6 +3337,40 @@ static inline void bnx2x_set_pbd_gso(struct sk_buff *skb,
 }
 
 /**
+ * bnx2x_set_pbd_csum_enc - update PBD with checksum and return header length
+ *
+ * @bp:			driver handle
+ * @skb:		packet skb
+ * @parsing_data:	data to be updated
+ * @xmit_type:		xmit flags
+ *
+ * 57712/578xx related, when skb has encapsulation
+ */
+static u8 bnx2x_set_pbd_csum_enc(struct bnx2x *bp, struct sk_buff *skb,
+				 u32 *parsing_data, u32 xmit_type)
+{
+	*parsing_data |=
+		((((u8 *)skb_inner_transport_header(skb) - skb->data) >> 1) <<
+		ETH_TX_PARSE_BD_E2_L4_HDR_START_OFFSET_W_SHIFT) &
+		ETH_TX_PARSE_BD_E2_L4_HDR_START_OFFSET_W;
+
+	if (xmit_type & XMIT_CSUM_TCP) {
+		*parsing_data |= ((inner_tcp_hdrlen(skb) / 4) <<
+			ETH_TX_PARSE_BD_E2_TCP_HDR_LENGTH_DW_SHIFT) &
+			ETH_TX_PARSE_BD_E2_TCP_HDR_LENGTH_DW;
+
+		return skb_inner_transport_header(skb) +
+			inner_tcp_hdrlen(skb) - skb->data;
+	}
+
+	/* We support checksum offload for TCP and UDP only.
+	 * No need to pass the UDP header length - it's a constant.
+	 */
+	return skb_inner_transport_header(skb) +
+		sizeof(struct udphdr) - skb->data;
+}
+
+/**
  * bnx2x_set_pbd_csum_e2 - update PBD with checksum and return header length
  *
  * @bp:			driver handle
@@ -3308,15 +3378,15 @@ static inline void bnx2x_set_pbd_gso(struct sk_buff *skb,
  * @parsing_data:	data to be updated
  * @xmit_type:		xmit flags
  *
- * 57712 related
+ * 57712/578xx related
  */
-static inline  u8 bnx2x_set_pbd_csum_e2(struct bnx2x *bp, struct sk_buff *skb,
-					u32 *parsing_data, u32 xmit_type)
+static u8 bnx2x_set_pbd_csum_e2(struct bnx2x *bp, struct sk_buff *skb,
+				u32 *parsing_data, u32 xmit_type)
 {
 	*parsing_data |=
 		((((u8 *)skb_transport_header(skb) - skb->data) >> 1) <<
-		ETH_TX_PARSE_BD_E2_TCP_HDR_START_OFFSET_W_SHIFT) &
-		ETH_TX_PARSE_BD_E2_TCP_HDR_START_OFFSET_W;
+		ETH_TX_PARSE_BD_E2_L4_HDR_START_OFFSET_W_SHIFT) &
+		ETH_TX_PARSE_BD_E2_L4_HDR_START_OFFSET_W;
 
 	if (xmit_type & XMIT_CSUM_TCP) {
 		*parsing_data |= ((tcp_hdrlen(skb) / 4) <<
@@ -3331,17 +3401,15 @@ static inline  u8 bnx2x_set_pbd_csum_e2(struct bnx2x *bp, struct sk_buff *skb,
 	return skb_transport_header(skb) + sizeof(struct udphdr) - skb->data;
 }
 
-static inline void bnx2x_set_sbd_csum(struct bnx2x *bp, struct sk_buff *skb,
-	struct eth_tx_start_bd *tx_start_bd, u32 xmit_type)
+/* set FW indication according to inner or outer protocols if tunneled */
+static void bnx2x_set_sbd_csum(struct bnx2x *bp, struct sk_buff *skb,
+			       struct eth_tx_start_bd *tx_start_bd,
+			       u32 xmit_type)
 {
 	tx_start_bd->bd_flags.as_bitfield |= ETH_TX_BD_FLAGS_L4_CSUM;
 
-	if (xmit_type & XMIT_CSUM_V4)
-		tx_start_bd->bd_flags.as_bitfield |=
-					ETH_TX_BD_FLAGS_IP_CSUM;
-	else
-		tx_start_bd->bd_flags.as_bitfield |=
-					ETH_TX_BD_FLAGS_IPV6;
+	if (xmit_type & (XMIT_CSUM_ENC_V6 | XMIT_CSUM_V6))
+		tx_start_bd->bd_flags.as_bitfield |= ETH_TX_BD_FLAGS_IPV6;
 
 	if (!(xmit_type & XMIT_CSUM_TCP))
 		tx_start_bd->bd_flags.as_bitfield |= ETH_TX_BD_FLAGS_IS_UDP;
@@ -3355,9 +3423,9 @@ static inline void bnx2x_set_sbd_csum(struct bnx2x *bp, struct sk_buff *skb,
  * @pbd:	parse BD to be updated
  * @xmit_type:	xmit flags
  */
-static inline u8 bnx2x_set_pbd_csum(struct bnx2x *bp, struct sk_buff *skb,
-	struct eth_tx_parse_bd_e1x *pbd,
-	u32 xmit_type)
+static u8 bnx2x_set_pbd_csum(struct bnx2x *bp, struct sk_buff *skb,
+			     struct eth_tx_parse_bd_e1x *pbd,
+			     u32 xmit_type)
 {
 	u8 hlen = (skb_network_header(skb) - skb->data) >> 1;
 
@@ -3403,6 +3471,75 @@ static inline u8 bnx2x_set_pbd_csum(struct bnx2x *bp, struct sk_buff *skb,
 	return hlen;
 }
 
+static void bnx2x_update_pbds_gso_enc(struct sk_buff *skb,
+				      struct eth_tx_parse_bd_e2 *pbd_e2,
+				      struct eth_tx_parse_2nd_bd *pbd2,
+				      u16 *global_data,
+				      u32 xmit_type)
+{
+	u16 hlen_w = 0;
+	u8 outerip_off, outerip_len = 0;
+	/* from outer IP to transport */
+	hlen_w = (skb_inner_transport_header(skb) -
+		  skb_network_header(skb)) >> 1;
+
+	/* transport len */
+	if (xmit_type & XMIT_CSUM_TCP)
+		hlen_w += inner_tcp_hdrlen(skb) >> 1;
+	else
+		hlen_w += sizeof(struct udphdr) >> 1;
+
+	pbd2->fw_ip_hdr_to_payload_w = hlen_w;
+
+	if (xmit_type & XMIT_CSUM_ENC_V4) {
+		struct iphdr *iph = ip_hdr(skb);
+		pbd2->fw_ip_csum_wo_len_flags_frag =
+			bswab16(csum_fold((~iph->check) -
+					  iph->tot_len - iph->frag_off));
+	} else {
+		pbd2->fw_ip_hdr_to_payload_w =
+			hlen_w - ((sizeof(struct ipv6hdr)) >> 1);
+	}
+
+	pbd2->tcp_send_seq = bswab32(inner_tcp_hdr(skb)->seq);
+
+	pbd2->tcp_flags = pbd_tcp_flags(inner_tcp_hdr(skb));
+
+	if (xmit_type & XMIT_GSO_V4) {
+		pbd2->hw_ip_id = bswab16(inner_ip_hdr(skb)->id);
+
+		pbd_e2->data.tunnel_data.pseudo_csum =
+			bswab16(~csum_tcpudp_magic(
+					inner_ip_hdr(skb)->saddr,
+					inner_ip_hdr(skb)->daddr,
+					0, IPPROTO_TCP, 0));
+
+		outerip_len = ip_hdr(skb)->ihl << 1;
+	} else {
+		pbd_e2->data.tunnel_data.pseudo_csum =
+			bswab16(~csum_ipv6_magic(
+					&inner_ipv6_hdr(skb)->saddr,
+					&inner_ipv6_hdr(skb)->daddr,
+					0, IPPROTO_TCP, 0));
+	}
+
+	outerip_off = (skb_network_header(skb) - skb->data) >> 1;
+
+	*global_data |=
+		outerip_off |
+		(!!(xmit_type & XMIT_CSUM_V6) <<
+			ETH_TX_PARSE_2ND_BD_IP_HDR_TYPE_OUTER_SHIFT) |
+		(outerip_len <<
+			ETH_TX_PARSE_2ND_BD_IP_HDR_LEN_OUTER_W_SHIFT) |
+		((skb->protocol == cpu_to_be16(ETH_P_8021Q)) <<
+			ETH_TX_PARSE_2ND_BD_LLC_SNAP_EN_SHIFT);
+
+	if (ip_hdr(skb)->protocol == IPPROTO_UDP) {
+		SET_FLAG(*global_data, ETH_TX_PARSE_2ND_BD_TUNNEL_UDP_EXIST, 1);
+		pbd2->tunnel_udp_hdr_start_w = skb_transport_offset(skb) >> 1;
+	}
+}
+
 /* called with netif_tx_lock
  * bnx2x_tx_int() runs without netif_tx_lock unless it needs to call
  * netif_wake_queue()
@@ -3418,6 +3555,7 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct eth_tx_bd *tx_data_bd, *total_pkt_bd = NULL;
 	struct eth_tx_parse_bd_e1x *pbd_e1x = NULL;
 	struct eth_tx_parse_bd_e2 *pbd_e2 = NULL;
+	struct eth_tx_parse_2nd_bd *pbd2 = NULL;
 	u32 pbd_e2_parsing_data = 0;
 	u16 pkt_prod, bd_prod;
 	int nbd, txq_index;
@@ -3485,7 +3623,7 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 			mac_type = MULTICAST_ADDRESS;
 	}
 
-#if (MAX_SKB_FRAGS >= MAX_FETCH_BD - 3)
+#if (MAX_SKB_FRAGS >= MAX_FETCH_BD - BDS_PER_TX_PKT)
 	/* First, check if we need to linearize the skb (due to FW
 	   restrictions). No need to check fragmentation if page size > 8K
 	   (there will be no violation to FW restrictions) */
@@ -3533,12 +3671,9 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	first_bd = tx_start_bd;
 
 	tx_start_bd->bd_flags.as_bitfield = ETH_TX_BD_FLAGS_START_BD;
-	SET_FLAG(tx_start_bd->general_data,
-		 ETH_TX_START_BD_PARSE_NBDS,
-		 0);
 
-	/* header nbd */
-	SET_FLAG(tx_start_bd->general_data, ETH_TX_START_BD_HDR_NBDS, 1);
+	/* header nbd: indirectly zero other flags! */
+	tx_start_bd->general_data = 1 << ETH_TX_START_BD_HDR_NBDS_SHIFT;
 
 	/* remember the first BD of the packet */
 	tx_buf->first_bd = txdata->tx_bd_prod;
@@ -3558,19 +3693,16 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		/* when transmitting in a vf, start bd must hold the ethertype
 		 * for fw to enforce it
 		 */
-#ifndef BNX2X_STOP_ON_ERROR
-		if (IS_VF(bp)) {
-#endif
+		if (IS_VF(bp))
 			tx_start_bd->vlan_or_ethertype =
 				cpu_to_le16(ntohs(eth->h_proto));
-#ifndef BNX2X_STOP_ON_ERROR
-		} else {
+		else
 			/* used by FW for packet accounting */
 			tx_start_bd->vlan_or_ethertype = cpu_to_le16(pkt_prod);
-		}
-#endif
 	}
 
+	nbd = 2; /* start_bd + pbd + frags (updated when pages are mapped) */
+
 	/* turn on parsing and get a BD */
 	bd_prod = TX_BD(NEXT_TX_IDX(bd_prod));
 
@@ -3580,23 +3712,58 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (!CHIP_IS_E1x(bp)) {
 		pbd_e2 = &txdata->tx_desc_ring[bd_prod].parse_bd_e2;
 		memset(pbd_e2, 0, sizeof(struct eth_tx_parse_bd_e2));
-		/* Set PBD in checksum offload case */
-		if (xmit_type & XMIT_CSUM)
+
+		if (xmit_type & XMIT_CSUM_ENC) {
+			u16 global_data = 0;
+
+			/* Set PBD in enc checksum offload case */
+			hlen = bnx2x_set_pbd_csum_enc(bp, skb,
+						      &pbd_e2_parsing_data,
+						      xmit_type);
+
+			/* turn on 2nd parsing and get a BD */
+			bd_prod = TX_BD(NEXT_TX_IDX(bd_prod));
+
+			pbd2 = &txdata->tx_desc_ring[bd_prod].parse_2nd_bd;
+
+			memset(pbd2, 0, sizeof(*pbd2));
+
+			pbd_e2->data.tunnel_data.ip_hdr_start_inner_w =
+				(skb_inner_network_header(skb) -
+				 skb->data) >> 1;
+
+			if (xmit_type & XMIT_GSO_ENC)
+				bnx2x_update_pbds_gso_enc(skb, pbd_e2, pbd2,
+							  &global_data,
+							  xmit_type);
+
+			pbd2->global_data = cpu_to_le16(global_data);
+
+			/* add addition parse BD indication to start BD */
+			SET_FLAG(tx_start_bd->general_data,
+				 ETH_TX_START_BD_PARSE_NBDS, 1);
+			/* set encapsulation flag in start BD */
+			SET_FLAG(tx_start_bd->general_data,
+				 ETH_TX_START_BD_TUNNEL_EXIST, 1);
+			nbd++;
+		} else if (xmit_type & XMIT_CSUM) {
+			/* Set PBD in checksum offload case w/o encapsulation */
 			hlen = bnx2x_set_pbd_csum_e2(bp, skb,
 						     &pbd_e2_parsing_data,
 						     xmit_type);
+		}
 
-		if (IS_MF_SI(bp) || IS_VF(bp)) {
-			/* fill in the MAC addresses in the PBD - for local
-			 * switching
-			 */
-			bnx2x_set_fw_mac_addr(&pbd_e2->src_mac_addr_hi,
-					      &pbd_e2->src_mac_addr_mid,
-					      &pbd_e2->src_mac_addr_lo,
+		/* Add the macs to the parsing BD this is a vf */
+		if (IS_VF(bp)) {
+			/* override GRE parameters in BD */
+			bnx2x_set_fw_mac_addr(&pbd_e2->data.mac_addr.src_hi,
+					      &pbd_e2->data.mac_addr.src_mid,
+					      &pbd_e2->data.mac_addr.src_lo,
 					      eth->h_source);
-			bnx2x_set_fw_mac_addr(&pbd_e2->dst_mac_addr_hi,
-					      &pbd_e2->dst_mac_addr_mid,
-					      &pbd_e2->dst_mac_addr_lo,
+
+			bnx2x_set_fw_mac_addr(&pbd_e2->data.mac_addr.dst_hi,
+					      &pbd_e2->data.mac_addr.dst_mid,
+					      &pbd_e2->data.mac_addr.dst_lo,
 					      eth->h_dest);
 		}
 
@@ -3618,14 +3785,13 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	/* Setup the data pointer of the first BD of the packet */
 	tx_start_bd->addr_hi = cpu_to_le32(U64_HI(mapping));
 	tx_start_bd->addr_lo = cpu_to_le32(U64_LO(mapping));
-	nbd = 2; /* start_bd + pbd + frags (updated when pages are mapped) */
 	tx_start_bd->nbytes = cpu_to_le16(skb_headlen(skb));
 	pkt_size = tx_start_bd->nbytes;
 
 	DP(NETIF_MSG_TX_QUEUED,
-	   "first bd @%p  addr (%x:%x)  nbd %d  nbytes %d  flags %x  vlan %x\n",
+	   "first bd @%p  addr (%x:%x)  nbytes %d  flags %x  vlan %x\n",
 	   tx_start_bd, tx_start_bd->addr_hi, tx_start_bd->addr_lo,
-	   le16_to_cpu(tx_start_bd->nbd), le16_to_cpu(tx_start_bd->nbytes),
+	   le16_to_cpu(tx_start_bd->nbytes),
 	   tx_start_bd->bd_flags.as_bitfield,
 	   le16_to_cpu(tx_start_bd->vlan_or_ethertype));
 
@@ -3638,10 +3804,12 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 		tx_start_bd->bd_flags.as_bitfield |= ETH_TX_BD_FLAGS_SW_LSO;
 
-		if (unlikely(skb_headlen(skb) > hlen))
+		if (unlikely(skb_headlen(skb) > hlen)) {
+			nbd++;
 			bd_prod = bnx2x_tx_split(bp, txdata, tx_buf,
 						 &tx_start_bd, hlen,
-						 bd_prod, ++nbd);
+						 bd_prod);
+		}
 		if (!CHIP_IS_E1x(bp))
 			bnx2x_set_pbd_gso_e2(skb, &pbd_e2_parsing_data,
 					     xmit_type);
@@ -3731,9 +3899,13 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (pbd_e2)
 		DP(NETIF_MSG_TX_QUEUED,
 		   "PBD (E2) @%p  dst %x %x %x src %x %x %x parsing_data %x\n",
-		   pbd_e2, pbd_e2->dst_mac_addr_hi, pbd_e2->dst_mac_addr_mid,
-		   pbd_e2->dst_mac_addr_lo, pbd_e2->src_mac_addr_hi,
-		   pbd_e2->src_mac_addr_mid, pbd_e2->src_mac_addr_lo,
+		   pbd_e2,
+		   pbd_e2->data.mac_addr.dst_hi,
+		   pbd_e2->data.mac_addr.dst_mid,
+		   pbd_e2->data.mac_addr.dst_lo,
+		   pbd_e2->data.mac_addr.src_hi,
+		   pbd_e2->data.mac_addr.src_mid,
+		   pbd_e2->data.mac_addr.src_lo,
 		   pbd_e2->parsing_data);
 	DP(NETIF_MSG_TX_QUEUED, "doorbell: nbd %d  bd %u\n", nbd, bd_prod);
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
index aee7671ff4c1..151675d66b0d 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
@@ -50,13 +50,13 @@ extern int int_mode;
 		} \
 	} while (0)
 
-#define BNX2X_PCI_ALLOC(x, y, size) \
-	do { \
-		x = dma_alloc_coherent(&bp->pdev->dev, size, y, GFP_KERNEL); \
-		if (x == NULL) \
-			goto alloc_mem_err; \
-		memset((void *)x, 0, size); \
-	} while (0)
+#define BNX2X_PCI_ALLOC(x, y, size)				\
+do {								\
+	x = dma_alloc_coherent(&bp->pdev->dev, size, y,		\
+			       GFP_KERNEL | __GFP_ZERO);	\
+	if (x == NULL)						\
+		goto alloc_mem_err;				\
+} while (0)
 
 #define BNX2X_ALLOC(x, size) \
 	do { \
@@ -295,16 +295,29 @@ void bnx2x_int_disable_sync(struct bnx2x *bp, int disable_hw);
 void bnx2x_nic_init_cnic(struct bnx2x *bp);
 
 /**
- * bnx2x_nic_init - init driver internals.
+ * bnx2x_preirq_nic_init - init driver internals.
  *
  * @bp:		driver handle
  *
  * Initializes:
- *  - rings
+ *  - fastpath object
+ *  - fastpath rings
+ *  etc.
+ */
+void bnx2x_pre_irq_nic_init(struct bnx2x *bp);
+
+/**
+ * bnx2x_postirq_nic_init - init driver internals.
+ *
+ * @bp:		driver handle
+ * @load_code:	COMMON, PORT or FUNCTION
+ *
+ * Initializes:
  *  - status blocks
+ *  - slowpath rings
  *  - etc.
  */
-void bnx2x_nic_init(struct bnx2x *bp, u32 load_code);
+void bnx2x_post_irq_nic_init(struct bnx2x *bp, u32 load_code);
 /**
  * bnx2x_alloc_mem_cnic - allocate driver's memory for cnic.
  *
@@ -496,7 +509,10 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev);
 /* setup_tc callback */
 int bnx2x_setup_tc(struct net_device *dev, u8 num_tc);
 
+int bnx2x_get_vf_config(struct net_device *dev, int vf,
+			struct ifla_vf_info *ivi);
 int bnx2x_set_vf_mac(struct net_device *dev, int queue, u8 *mac);
+int bnx2x_set_vf_vlan(struct net_device *netdev, int vf, u16 vlan, u8 qos);
 
 /* select_queue callback */
 u16 bnx2x_select_queue(struct net_device *dev, struct sk_buff *skb);
@@ -834,7 +850,7 @@ static inline void bnx2x_add_all_napi_cnic(struct bnx2x *bp)
 	/* Add NAPI objects */
 	for_each_rx_queue_cnic(bp, i)
 		netif_napi_add(bp->dev, &bnx2x_fp(bp, i, napi),
-			       bnx2x_poll, BNX2X_NAPI_WEIGHT);
+			       bnx2x_poll, NAPI_POLL_WEIGHT);
 }
 
 static inline void bnx2x_add_all_napi(struct bnx2x *bp)
@@ -844,7 +860,7 @@ static inline void bnx2x_add_all_napi(struct bnx2x *bp)
 	/* Add NAPI objects */
 	for_each_eth_queue(bp, i)
 		netif_napi_add(bp->dev, &bnx2x_fp(bp, i, napi),
-			       bnx2x_poll, BNX2X_NAPI_WEIGHT);
+			       bnx2x_poll, NAPI_POLL_WEIGHT);
 }
 
 static inline void bnx2x_del_all_napi_cnic(struct bnx2x *bp)
@@ -970,6 +986,9 @@ static inline int bnx2x_func_start(struct bnx2x *bp)
 	else /* CHIP_IS_E1X */
 		start_params->network_cos_mode = FW_WRR;
 
+	start_params->gre_tunnel_mode = IPGRE_TUNNEL;
+	start_params->gre_tunnel_rss = GRE_INNER_HEADERS_RSS;
+
 	return bnx2x_func_state_change(bp, &func_params);
 }
 
@@ -1396,4 +1415,8 @@ static inline bool bnx2x_is_valid_ether_addr(struct bnx2x *bp, u8 *addr)
  *
  */
 void bnx2x_fill_fw_str(struct bnx2x *bp, char *buf, size_t buf_len);
+
+int bnx2x_drain_tx_queues(struct bnx2x *bp);
+void bnx2x_squeeze_objects(struct bnx2x *bp);
+
 #endif /* BNX2X_CMN_H */
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
index edfa67adf2f9..ce1a91618677 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
@@ -1364,11 +1364,27 @@ static int bnx2x_nvram_read(struct bnx2x *bp, u32 offset, u8 *ret_buf,
 	return rc;
 }
 
+static int bnx2x_nvram_read32(struct bnx2x *bp, u32 offset, u32 *buf,
+			      int buf_size)
+{
+	int rc;
+
+	rc = bnx2x_nvram_read(bp, offset, (u8 *)buf, buf_size);
+
+	if (!rc) {
+		__be32 *be = (__be32 *)buf;
+
+		while ((buf_size -= 4) >= 0)
+			*buf++ = be32_to_cpu(*be++);
+	}
+
+	return rc;
+}
+
 static int bnx2x_get_eeprom(struct net_device *dev,
 			    struct ethtool_eeprom *eeprom, u8 *eebuf)
 {
 	struct bnx2x *bp = netdev_priv(dev);
-	int rc;
 
 	if (!netif_running(dev)) {
 		DP(BNX2X_MSG_ETHTOOL  | BNX2X_MSG_NVM,
@@ -1383,9 +1399,7 @@ static int bnx2x_get_eeprom(struct net_device *dev,
 
 	/* parameters already validated in ethtool_get_eeprom */
 
-	rc = bnx2x_nvram_read(bp, eeprom->offset, eebuf, eeprom->len);
-
-	return rc;
+	return bnx2x_nvram_read(bp, eeprom->offset, eebuf, eeprom->len);
 }
 
 static int bnx2x_get_module_eeprom(struct net_device *dev,
@@ -1393,10 +1407,9 @@ static int bnx2x_get_module_eeprom(struct net_device *dev,
 				   u8 *data)
 {
 	struct bnx2x *bp = netdev_priv(dev);
-	int rc = 0, phy_idx;
+	int rc = -EINVAL, phy_idx;
 	u8 *user_data = data;
-	int remaining_len = ee->len, xfer_size;
-	unsigned int page_off = ee->offset;
+	unsigned int start_addr = ee->offset, xfer_size = 0;
 
 	if (!netif_running(dev)) {
 		DP(BNX2X_MSG_ETHTOOL | BNX2X_MSG_NVM,
@@ -1405,21 +1418,52 @@ static int bnx2x_get_module_eeprom(struct net_device *dev,
 	}
 
 	phy_idx = bnx2x_get_cur_phy_idx(bp);
-	bnx2x_acquire_phy_lock(bp);
-	while (!rc && remaining_len > 0) {
-		xfer_size = (remaining_len > SFP_EEPROM_PAGE_SIZE) ?
-			SFP_EEPROM_PAGE_SIZE : remaining_len;
+
+	/* Read A0 section */
+	if (start_addr < ETH_MODULE_SFF_8079_LEN) {
+		/* Limit transfer size to the A0 section boundary */
+		if (start_addr + ee->len > ETH_MODULE_SFF_8079_LEN)
+			xfer_size = ETH_MODULE_SFF_8079_LEN - start_addr;
+		else
+			xfer_size = ee->len;
+		bnx2x_acquire_phy_lock(bp);
 		rc = bnx2x_read_sfp_module_eeprom(&bp->link_params.phy[phy_idx],
 						  &bp->link_params,
-						  page_off,
+						  I2C_DEV_ADDR_A0,
+						  start_addr,
 						  xfer_size,
 						  user_data);
-		remaining_len -= xfer_size;
+		bnx2x_release_phy_lock(bp);
+		if (rc) {
+			DP(BNX2X_MSG_ETHTOOL, "Failed reading A0 section\n");
+
+			return -EINVAL;
+		}
 		user_data += xfer_size;
-		page_off += xfer_size;
+		start_addr += xfer_size;
 	}
 
-	bnx2x_release_phy_lock(bp);
+	/* Read A2 section */
+	if ((start_addr >= ETH_MODULE_SFF_8079_LEN) &&
+	    (start_addr < ETH_MODULE_SFF_8472_LEN)) {
+		xfer_size = ee->len - xfer_size;
+		/* Limit transfer size to the A2 section boundary */
+		if (start_addr + xfer_size > ETH_MODULE_SFF_8472_LEN)
+			xfer_size = ETH_MODULE_SFF_8472_LEN - start_addr;
+		start_addr -= ETH_MODULE_SFF_8079_LEN;
+		bnx2x_acquire_phy_lock(bp);
+		rc = bnx2x_read_sfp_module_eeprom(&bp->link_params.phy[phy_idx],
+						  &bp->link_params,
+						  I2C_DEV_ADDR_A2,
+						  start_addr,
+						  xfer_size,
+						  user_data);
+		bnx2x_release_phy_lock(bp);
+		if (rc) {
+			DP(BNX2X_MSG_ETHTOOL, "Failed reading A2 section\n");
+			return -EINVAL;
+		}
+	}
 	return rc;
 }
 
@@ -1427,24 +1471,50 @@ static int bnx2x_get_module_info(struct net_device *dev,
 				 struct ethtool_modinfo *modinfo)
 {
 	struct bnx2x *bp = netdev_priv(dev);
-	int phy_idx;
+	int phy_idx, rc;
+	u8 sff8472_comp, diag_type;
+
 	if (!netif_running(dev)) {
-		DP(BNX2X_MSG_ETHTOOL  | BNX2X_MSG_NVM,
+		DP(BNX2X_MSG_ETHTOOL | BNX2X_MSG_NVM,
 		   "cannot access eeprom when the interface is down\n");
 		return -EAGAIN;
 	}
-
 	phy_idx = bnx2x_get_cur_phy_idx(bp);
-	switch (bp->link_params.phy[phy_idx].media_type) {
-	case ETH_PHY_SFPP_10G_FIBER:
-	case ETH_PHY_SFP_1G_FIBER:
-	case ETH_PHY_DA_TWINAX:
+	bnx2x_acquire_phy_lock(bp);
+	rc = bnx2x_read_sfp_module_eeprom(&bp->link_params.phy[phy_idx],
+					  &bp->link_params,
+					  I2C_DEV_ADDR_A0,
+					  SFP_EEPROM_SFF_8472_COMP_ADDR,
+					  SFP_EEPROM_SFF_8472_COMP_SIZE,
+					  &sff8472_comp);
+	bnx2x_release_phy_lock(bp);
+	if (rc) {
+		DP(BNX2X_MSG_ETHTOOL, "Failed reading SFF-8472 comp field\n");
+		return -EINVAL;
+	}
+
+	bnx2x_acquire_phy_lock(bp);
+	rc = bnx2x_read_sfp_module_eeprom(&bp->link_params.phy[phy_idx],
+					  &bp->link_params,
+					  I2C_DEV_ADDR_A0,
+					  SFP_EEPROM_DIAG_TYPE_ADDR,
+					  SFP_EEPROM_DIAG_TYPE_SIZE,
+					  &diag_type);
+	bnx2x_release_phy_lock(bp);
+	if (rc) {
+		DP(BNX2X_MSG_ETHTOOL, "Failed reading Diag Type field\n");
+		return -EINVAL;
+	}
+
+	if (!sff8472_comp ||
+	    (diag_type & SFP_EEPROM_DIAG_ADDR_CHANGE_REQ)) {
 		modinfo->type = ETH_MODULE_SFF_8079;
 		modinfo->eeprom_len = ETH_MODULE_SFF_8079_LEN;
-		return 0;
-	default:
-		return -EOPNOTSUPP;
+	} else {
+		modinfo->type = ETH_MODULE_SFF_8472;
+		modinfo->eeprom_len = ETH_MODULE_SFF_8472_LEN;
 	}
+	return 0;
 }
 
 static int bnx2x_nvram_write_dword(struct bnx2x *bp, u32 offset, u32 val,
@@ -1496,9 +1566,8 @@ static int bnx2x_nvram_write1(struct bnx2x *bp, u32 offset, u8 *data_buf,
 			      int buf_size)
 {
 	int rc;
-	u32 cmd_flags;
-	u32 align_offset;
-	__be32 val;
+	u32 cmd_flags, align_offset, val;
+	__be32 val_be;
 
 	if (offset + buf_size > bp->common.flash_size) {
 		DP(BNX2X_MSG_ETHTOOL | BNX2X_MSG_NVM,
@@ -1517,16 +1586,16 @@ static int bnx2x_nvram_write1(struct bnx2x *bp, u32 offset, u8 *data_buf,
 
 	cmd_flags = (MCPR_NVM_COMMAND_FIRST | MCPR_NVM_COMMAND_LAST);
 	align_offset = (offset & ~0x03);
-	rc = bnx2x_nvram_read_dword(bp, align_offset, &val, cmd_flags);
+	rc = bnx2x_nvram_read_dword(bp, align_offset, &val_be, cmd_flags);
 
 	if (rc == 0) {
-		val &= ~(0xff << BYTE_OFFSET(offset));
-		val |= (*data_buf << BYTE_OFFSET(offset));
-
 		/* nvram data is returned as an array of bytes
 		 * convert it back to cpu order
 		 */
-		val = be32_to_cpu(val);
+		val = be32_to_cpu(val_be);
+
+		val &= ~le32_to_cpu(0xff << BYTE_OFFSET(offset));
+		val |= le32_to_cpu(*data_buf << BYTE_OFFSET(offset));
 
 		rc = bnx2x_nvram_write_dword(bp, align_offset, val,
 					     cmd_flags);
@@ -1820,12 +1889,15 @@ static int bnx2x_set_pauseparam(struct net_device *dev,
 			bp->link_params.req_flow_ctrl[cfg_idx] =
 				BNX2X_FLOW_CTRL_AUTO;
 		}
-		bp->link_params.req_fc_auto_adv = BNX2X_FLOW_CTRL_NONE;
+		bp->link_params.req_fc_auto_adv = 0;
 		if (epause->rx_pause)
 			bp->link_params.req_fc_auto_adv |= BNX2X_FLOW_CTRL_RX;
 
 		if (epause->tx_pause)
 			bp->link_params.req_fc_auto_adv |= BNX2X_FLOW_CTRL_TX;
+
+		if (!bp->link_params.req_fc_auto_adv)
+			bp->link_params.req_fc_auto_adv |= BNX2X_FLOW_CTRL_NONE;
 	}
 
 	DP(BNX2X_MSG_ETHTOOL,
@@ -2526,14 +2598,168 @@ static int bnx2x_test_ext_loopback(struct bnx2x *bp)
 	return rc;
 }
 
+struct code_entry {
+	u32 sram_start_addr;
+	u32 code_attribute;
+#define CODE_IMAGE_TYPE_MASK			0xf0800003
+#define CODE_IMAGE_VNTAG_PROFILES_DATA		0xd0000003
+#define CODE_IMAGE_LENGTH_MASK			0x007ffffc
+#define CODE_IMAGE_TYPE_EXTENDED_DIR		0xe0000000
+	u32 nvm_start_addr;
+};
+
+#define CODE_ENTRY_MAX			16
+#define CODE_ENTRY_EXTENDED_DIR_IDX	15
+#define MAX_IMAGES_IN_EXTENDED_DIR	64
+#define NVRAM_DIR_OFFSET		0x14
+
+#define EXTENDED_DIR_EXISTS(code)					  \
+	((code & CODE_IMAGE_TYPE_MASK) == CODE_IMAGE_TYPE_EXTENDED_DIR && \
+	 (code & CODE_IMAGE_LENGTH_MASK) != 0)
+
 #define CRC32_RESIDUAL			0xdebb20e3
+#define CRC_BUFF_SIZE			256
+
+static int bnx2x_nvram_crc(struct bnx2x *bp,
+			   int offset,
+			   int size,
+			   u8 *buff)
+{
+	u32 crc = ~0;
+	int rc = 0, done = 0;
+
+	DP(BNX2X_MSG_ETHTOOL | BNX2X_MSG_NVM,
+	   "NVRAM CRC from 0x%08x to 0x%08x\n", offset, offset + size);
+
+	while (done < size) {
+		int count = min_t(int, size - done, CRC_BUFF_SIZE);
+
+		rc = bnx2x_nvram_read(bp, offset + done, buff, count);
+
+		if (rc)
+			return rc;
+
+		crc = crc32_le(crc, buff, count);
+		done += count;
+	}
+
+	if (crc != CRC32_RESIDUAL)
+		rc = -EINVAL;
+
+	return rc;
+}
+
+static int bnx2x_test_nvram_dir(struct bnx2x *bp,
+				struct code_entry *entry,
+				u8 *buff)
+{
+	size_t size = entry->code_attribute & CODE_IMAGE_LENGTH_MASK;
+	u32 type = entry->code_attribute & CODE_IMAGE_TYPE_MASK;
+	int rc;
+
+	/* Zero-length images and AFEX profiles do not have CRC */
+	if (size == 0 || type == CODE_IMAGE_VNTAG_PROFILES_DATA)
+		return 0;
+
+	rc = bnx2x_nvram_crc(bp, entry->nvm_start_addr, size, buff);
+	if (rc)
+		DP(BNX2X_MSG_ETHTOOL | BNX2X_MSG_NVM,
+		   "image %x has failed crc test (rc %d)\n", type, rc);
+
+	return rc;
+}
+
+static int bnx2x_test_dir_entry(struct bnx2x *bp, u32 addr, u8 *buff)
+{
+	int rc;
+	struct code_entry entry;
+
+	rc = bnx2x_nvram_read32(bp, addr, (u32 *)&entry, sizeof(entry));
+	if (rc)
+		return rc;
+
+	return bnx2x_test_nvram_dir(bp, &entry, buff);
+}
+
+static int bnx2x_test_nvram_ext_dirs(struct bnx2x *bp, u8 *buff)
+{
+	u32 rc, cnt, dir_offset = NVRAM_DIR_OFFSET;
+	struct code_entry entry;
+	int i;
+
+	rc = bnx2x_nvram_read32(bp,
+				dir_offset +
+				sizeof(entry) * CODE_ENTRY_EXTENDED_DIR_IDX,
+				(u32 *)&entry, sizeof(entry));
+	if (rc)
+		return rc;
+
+	if (!EXTENDED_DIR_EXISTS(entry.code_attribute))
+		return 0;
+
+	rc = bnx2x_nvram_read32(bp, entry.nvm_start_addr,
+				&cnt, sizeof(u32));
+	if (rc)
+		return rc;
+
+	dir_offset = entry.nvm_start_addr + 8;
+
+	for (i = 0; i < cnt && i < MAX_IMAGES_IN_EXTENDED_DIR; i++) {
+		rc = bnx2x_test_dir_entry(bp, dir_offset +
+					      sizeof(struct code_entry) * i,
+					  buff);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
+}
+
+static int bnx2x_test_nvram_dirs(struct bnx2x *bp, u8 *buff)
+{
+	u32 rc, dir_offset = NVRAM_DIR_OFFSET;
+	int i;
+
+	DP(BNX2X_MSG_ETHTOOL | BNX2X_MSG_NVM, "NVRAM DIRS CRC test-set\n");
+
+	for (i = 0; i < CODE_ENTRY_EXTENDED_DIR_IDX; i++) {
+		rc = bnx2x_test_dir_entry(bp, dir_offset +
+					      sizeof(struct code_entry) * i,
+					  buff);
+		if (rc)
+			return rc;
+	}
+
+	return bnx2x_test_nvram_ext_dirs(bp, buff);
+}
+
+struct crc_pair {
+	int offset;
+	int size;
+};
+
+static int bnx2x_test_nvram_tbl(struct bnx2x *bp,
+				const struct crc_pair *nvram_tbl, u8 *buf)
+{
+	int i;
+
+	for (i = 0; nvram_tbl[i].size; i++) {
+		int rc = bnx2x_nvram_crc(bp, nvram_tbl[i].offset,
+					 nvram_tbl[i].size, buf);
+		if (rc) {
+			DP(BNX2X_MSG_ETHTOOL | BNX2X_MSG_NVM,
+			   "nvram_tbl[%d] has failed crc test (rc %d)\n",
+			   i, rc);
+			return rc;
+		}
+	}
+
+	return 0;
+}
 
 static int bnx2x_test_nvram(struct bnx2x *bp)
 {
-	static const struct {
-		int offset;
-		int size;
-	} nvram_tbl[] = {
+	const struct crc_pair nvram_tbl[] = {
 		{     0,  0x14 }, /* bootstrap */
 		{  0x14,  0xec }, /* dir */
 		{ 0x100, 0x350 }, /* manuf_info */
@@ -2542,30 +2768,33 @@ static int bnx2x_test_nvram(struct bnx2x *bp)
 		{ 0x708,  0x70 }, /* manuf_key_info */
 		{     0,     0 }
 	};
-	__be32 *buf;
-	u8 *data;
-	int i, rc;
-	u32 magic, crc;
+	const struct crc_pair nvram_tbl2[] = {
+		{ 0x7e8, 0x350 }, /* manuf_info2 */
+		{ 0xb38,  0xf0 }, /* feature_info */
+		{     0,     0 }
+	};
+
+	u8 *buf;
+	int rc;
+	u32 magic;
 
 	if (BP_NOMCP(bp))
 		return 0;
 
-	buf = kmalloc(0x350, GFP_KERNEL);
+	buf = kmalloc(CRC_BUFF_SIZE, GFP_KERNEL);
 	if (!buf) {
 		DP(BNX2X_MSG_ETHTOOL | BNX2X_MSG_NVM, "kmalloc failed\n");
 		rc = -ENOMEM;
 		goto test_nvram_exit;
 	}
-	data = (u8 *)buf;
 
-	rc = bnx2x_nvram_read(bp, 0, data, 4);
+	rc = bnx2x_nvram_read32(bp, 0, &magic, sizeof(magic));
 	if (rc) {
 		DP(BNX2X_MSG_ETHTOOL | BNX2X_MSG_NVM,
 		   "magic value read (rc %d)\n", rc);
 		goto test_nvram_exit;
 	}
 
-	magic = be32_to_cpu(buf[0]);
 	if (magic != 0x669955aa) {
 		DP(BNX2X_MSG_ETHTOOL | BNX2X_MSG_NVM,
 		   "wrong magic value (0x%08x)\n", magic);
@@ -2573,25 +2802,26 @@ static int bnx2x_test_nvram(struct bnx2x *bp)
 		goto test_nvram_exit;
 	}
 
-	for (i = 0; nvram_tbl[i].size; i++) {
+	DP(BNX2X_MSG_ETHTOOL | BNX2X_MSG_NVM, "Port 0 CRC test-set\n");
+	rc = bnx2x_test_nvram_tbl(bp, nvram_tbl, buf);
+	if (rc)
+		goto test_nvram_exit;
 
-		rc = bnx2x_nvram_read(bp, nvram_tbl[i].offset, data,
-				      nvram_tbl[i].size);
-		if (rc) {
-			DP(BNX2X_MSG_ETHTOOL | BNX2X_MSG_NVM,
-			   "nvram_tbl[%d] read data (rc %d)\n", i, rc);
-			goto test_nvram_exit;
-		}
+	if (!CHIP_IS_E1x(bp) && !CHIP_IS_57811xx(bp)) {
+		u32 hide = SHMEM_RD(bp, dev_info.shared_hw_config.config2) &
+			   SHARED_HW_CFG_HIDE_PORT1;
 
-		crc = ether_crc_le(nvram_tbl[i].size, data);
-		if (crc != CRC32_RESIDUAL) {
+		if (!hide) {
 			DP(BNX2X_MSG_ETHTOOL | BNX2X_MSG_NVM,
-			   "nvram_tbl[%d] wrong crc value (0x%08x)\n", i, crc);
-			rc = -ENODEV;
-			goto test_nvram_exit;
+			   "Port 1 CRC test-set\n");
+			rc = bnx2x_test_nvram_tbl(bp, nvram_tbl2, buf);
+			if (rc)
+				goto test_nvram_exit;
 		}
 	}
 
+	rc = bnx2x_test_nvram_dirs(bp, buf);
+
 test_nvram_exit:
 	kfree(buf);
 	return rc;
@@ -3232,7 +3462,32 @@ static const struct ethtool_ops bnx2x_ethtool_ops = {
 	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
-void bnx2x_set_ethtool_ops(struct net_device *netdev)
+static const struct ethtool_ops bnx2x_vf_ethtool_ops = {
+	.get_settings		= bnx2x_get_settings,
+	.set_settings		= bnx2x_set_settings,
+	.get_drvinfo		= bnx2x_get_drvinfo,
+	.get_msglevel		= bnx2x_get_msglevel,
+	.set_msglevel		= bnx2x_set_msglevel,
+	.get_link		= bnx2x_get_link,
+	.get_coalesce		= bnx2x_get_coalesce,
+	.get_ringparam		= bnx2x_get_ringparam,
+	.set_ringparam		= bnx2x_set_ringparam,
+	.get_sset_count		= bnx2x_get_sset_count,
+	.get_strings		= bnx2x_get_strings,
+	.get_ethtool_stats	= bnx2x_get_ethtool_stats,
+	.get_rxnfc		= bnx2x_get_rxnfc,
+	.set_rxnfc		= bnx2x_set_rxnfc,
+	.get_rxfh_indir_size	= bnx2x_get_rxfh_indir_size,
+	.get_rxfh_indir		= bnx2x_get_rxfh_indir,
+	.set_rxfh_indir		= bnx2x_set_rxfh_indir,
+	.get_channels		= bnx2x_get_channels,
+	.set_channels		= bnx2x_set_channels,
+};
+
+void bnx2x_set_ethtool_ops(struct bnx2x *bp, struct net_device *netdev)
 {
-	SET_ETHTOOL_OPS(netdev, &bnx2x_ethtool_ops);
+	if (IS_PF(bp))
+		SET_ETHTOOL_OPS(netdev, &bnx2x_ethtool_ops);
+	else /* vf */
+		SET_ETHTOOL_OPS(netdev, &bnx2x_vf_ethtool_ops);
 }
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_fw_defs.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_fw_defs.h
index e5f808377c91..84aecdf06f7a 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_fw_defs.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_fw_defs.h
@@ -30,31 +30,31 @@
 	* IRO[138].m2) + ((sbId) * IRO[138].m3))
 #define CSTORM_IGU_MODE_OFFSET (IRO[157].base)
 #define CSTORM_ISCSI_CQ_SIZE_OFFSET(pfId) \
-	(IRO[316].base + ((pfId) * IRO[316].m1))
-#define CSTORM_ISCSI_CQ_SQN_SIZE_OFFSET(pfId) \
 	(IRO[317].base + ((pfId) * IRO[317].m1))
+#define CSTORM_ISCSI_CQ_SQN_SIZE_OFFSET(pfId) \
+	(IRO[318].base + ((pfId) * IRO[318].m1))
 #define CSTORM_ISCSI_EQ_CONS_OFFSET(pfId, iscsiEqId) \
-	(IRO[309].base + ((pfId) * IRO[309].m1) + ((iscsiEqId) * IRO[309].m2))
+	(IRO[310].base + ((pfId) * IRO[310].m1) + ((iscsiEqId) * IRO[310].m2))
 #define CSTORM_ISCSI_EQ_NEXT_EQE_ADDR_OFFSET(pfId, iscsiEqId) \
-	(IRO[311].base + ((pfId) * IRO[311].m1) + ((iscsiEqId) * IRO[311].m2))
+	(IRO[312].base + ((pfId) * IRO[312].m1) + ((iscsiEqId) * IRO[312].m2))
 #define CSTORM_ISCSI_EQ_NEXT_PAGE_ADDR_OFFSET(pfId, iscsiEqId) \
-	(IRO[310].base + ((pfId) * IRO[310].m1) + ((iscsiEqId) * IRO[310].m2))
+	(IRO[311].base + ((pfId) * IRO[311].m1) + ((iscsiEqId) * IRO[311].m2))
 #define CSTORM_ISCSI_EQ_NEXT_PAGE_ADDR_VALID_OFFSET(pfId, iscsiEqId) \
-	(IRO[312].base + ((pfId) * IRO[312].m1) + ((iscsiEqId) * IRO[312].m2))
+	(IRO[313].base + ((pfId) * IRO[313].m1) + ((iscsiEqId) * IRO[313].m2))
 #define CSTORM_ISCSI_EQ_PROD_OFFSET(pfId, iscsiEqId) \
-	(IRO[308].base + ((pfId) * IRO[308].m1) + ((iscsiEqId) * IRO[308].m2))
+	(IRO[309].base + ((pfId) * IRO[309].m1) + ((iscsiEqId) * IRO[309].m2))
 #define CSTORM_ISCSI_EQ_SB_INDEX_OFFSET(pfId, iscsiEqId) \
-	(IRO[314].base + ((pfId) * IRO[314].m1) + ((iscsiEqId) * IRO[314].m2))
+	(IRO[315].base + ((pfId) * IRO[315].m1) + ((iscsiEqId) * IRO[315].m2))
 #define CSTORM_ISCSI_EQ_SB_NUM_OFFSET(pfId, iscsiEqId) \
-	(IRO[313].base + ((pfId) * IRO[313].m1) + ((iscsiEqId) * IRO[313].m2))
+	(IRO[314].base + ((pfId) * IRO[314].m1) + ((iscsiEqId) * IRO[314].m2))
 #define CSTORM_ISCSI_HQ_SIZE_OFFSET(pfId) \
-	(IRO[315].base + ((pfId) * IRO[315].m1))
+	(IRO[316].base + ((pfId) * IRO[316].m1))
 #define CSTORM_ISCSI_NUM_OF_TASKS_OFFSET(pfId) \
-	(IRO[307].base + ((pfId) * IRO[307].m1))
+	(IRO[308].base + ((pfId) * IRO[308].m1))
 #define CSTORM_ISCSI_PAGE_SIZE_LOG_OFFSET(pfId) \
-	(IRO[306].base + ((pfId) * IRO[306].m1))
+	(IRO[307].base + ((pfId) * IRO[307].m1))
 #define CSTORM_ISCSI_PAGE_SIZE_OFFSET(pfId) \
-	(IRO[305].base + ((pfId) * IRO[305].m1))
+	(IRO[306].base + ((pfId) * IRO[306].m1))
 #define CSTORM_RECORD_SLOW_PATH_OFFSET(funcId) \
 	(IRO[151].base + ((funcId) * IRO[151].m1))
 #define CSTORM_SP_STATUS_BLOCK_DATA_OFFSET(pfId) \
@@ -114,7 +114,7 @@
 #define TSTORM_ISCSI_RQ_SIZE_OFFSET(pfId) \
 	(IRO[268].base + ((pfId) * IRO[268].m1))
 #define TSTORM_ISCSI_TCP_LOCAL_ADV_WND_OFFSET(pfId) \
-	(IRO[277].base + ((pfId) * IRO[277].m1))
+	(IRO[278].base + ((pfId) * IRO[278].m1))
 #define TSTORM_ISCSI_TCP_VARS_FLAGS_OFFSET(pfId) \
 	(IRO[264].base + ((pfId) * IRO[264].m1))
 #define TSTORM_ISCSI_TCP_VARS_LSB_LOCAL_MAC_ADDR_OFFSET(pfId) \
@@ -136,35 +136,32 @@
 #define USTORM_ASSERT_LIST_INDEX_OFFSET	(IRO[177].base)
 #define USTORM_ASSERT_LIST_OFFSET(assertListEntry) \
 	(IRO[176].base + ((assertListEntry) * IRO[176].m1))
-#define USTORM_CQE_PAGE_NEXT_OFFSET(portId, clientId) \
-	(IRO[205].base + ((portId) * IRO[205].m1) + ((clientId) * \
-	IRO[205].m2))
 #define USTORM_ETH_PAUSE_ENABLED_OFFSET(portId) \
 	(IRO[183].base + ((portId) * IRO[183].m1))
 #define USTORM_FCOE_EQ_PROD_OFFSET(pfId) \
-	(IRO[318].base + ((pfId) * IRO[318].m1))
+	(IRO[319].base + ((pfId) * IRO[319].m1))
 #define USTORM_FUNC_EN_OFFSET(funcId) \
 	(IRO[178].base + ((funcId) * IRO[178].m1))
 #define USTORM_ISCSI_CQ_SIZE_OFFSET(pfId) \
-	(IRO[282].base + ((pfId) * IRO[282].m1))
-#define USTORM_ISCSI_CQ_SQN_SIZE_OFFSET(pfId) \
 	(IRO[283].base + ((pfId) * IRO[283].m1))
+#define USTORM_ISCSI_CQ_SQN_SIZE_OFFSET(pfId) \
+	(IRO[284].base + ((pfId) * IRO[284].m1))
 #define USTORM_ISCSI_ERROR_BITMAP_OFFSET(pfId) \
-	(IRO[287].base + ((pfId) * IRO[287].m1))
+	(IRO[288].base + ((pfId) * IRO[288].m1))
 #define USTORM_ISCSI_GLOBAL_BUF_PHYS_ADDR_OFFSET(pfId) \
-	(IRO[284].base + ((pfId) * IRO[284].m1))
+	(IRO[285].base + ((pfId) * IRO[285].m1))
 #define USTORM_ISCSI_NUM_OF_TASKS_OFFSET(pfId) \
-	(IRO[280].base + ((pfId) * IRO[280].m1))
+	(IRO[281].base + ((pfId) * IRO[281].m1))
 #define USTORM_ISCSI_PAGE_SIZE_LOG_OFFSET(pfId) \
-	(IRO[279].base + ((pfId) * IRO[279].m1))
+	(IRO[280].base + ((pfId) * IRO[280].m1))
 #define USTORM_ISCSI_PAGE_SIZE_OFFSET(pfId) \
-	(IRO[278].base + ((pfId) * IRO[278].m1))
+	(IRO[279].base + ((pfId) * IRO[279].m1))
 #define USTORM_ISCSI_R2TQ_SIZE_OFFSET(pfId) \
-	(IRO[281].base + ((pfId) * IRO[281].m1))
+	(IRO[282].base + ((pfId) * IRO[282].m1))
 #define USTORM_ISCSI_RQ_BUFFER_SIZE_OFFSET(pfId) \
-	(IRO[285].base + ((pfId) * IRO[285].m1))
-#define USTORM_ISCSI_RQ_SIZE_OFFSET(pfId) \
 	(IRO[286].base + ((pfId) * IRO[286].m1))
+#define USTORM_ISCSI_RQ_SIZE_OFFSET(pfId) \
+	(IRO[287].base + ((pfId) * IRO[287].m1))
 #define USTORM_MEM_WORKAROUND_ADDRESS_OFFSET(pfId) \
 	(IRO[182].base + ((pfId) * IRO[182].m1))
 #define USTORM_RECORD_SLOW_PATH_OFFSET(funcId) \
@@ -190,39 +187,39 @@
 #define XSTORM_FUNC_EN_OFFSET(funcId) \
 	(IRO[47].base + ((funcId) * IRO[47].m1))
 #define XSTORM_ISCSI_HQ_SIZE_OFFSET(pfId) \
-	(IRO[295].base + ((pfId) * IRO[295].m1))
+	(IRO[296].base + ((pfId) * IRO[296].m1))
 #define XSTORM_ISCSI_LOCAL_MAC_ADDR0_OFFSET(pfId) \
-	(IRO[298].base + ((pfId) * IRO[298].m1))
-#define XSTORM_ISCSI_LOCAL_MAC_ADDR1_OFFSET(pfId) \
 	(IRO[299].base + ((pfId) * IRO[299].m1))
-#define XSTORM_ISCSI_LOCAL_MAC_ADDR2_OFFSET(pfId) \
+#define XSTORM_ISCSI_LOCAL_MAC_ADDR1_OFFSET(pfId) \
 	(IRO[300].base + ((pfId) * IRO[300].m1))
-#define XSTORM_ISCSI_LOCAL_MAC_ADDR3_OFFSET(pfId) \
+#define XSTORM_ISCSI_LOCAL_MAC_ADDR2_OFFSET(pfId) \
 	(IRO[301].base + ((pfId) * IRO[301].m1))
-#define XSTORM_ISCSI_LOCAL_MAC_ADDR4_OFFSET(pfId) \
+#define XSTORM_ISCSI_LOCAL_MAC_ADDR3_OFFSET(pfId) \
 	(IRO[302].base + ((pfId) * IRO[302].m1))
-#define XSTORM_ISCSI_LOCAL_MAC_ADDR5_OFFSET(pfId) \
+#define XSTORM_ISCSI_LOCAL_MAC_ADDR4_OFFSET(pfId) \
 	(IRO[303].base + ((pfId) * IRO[303].m1))
-#define XSTORM_ISCSI_LOCAL_VLAN_OFFSET(pfId) \
+#define XSTORM_ISCSI_LOCAL_MAC_ADDR5_OFFSET(pfId) \
 	(IRO[304].base + ((pfId) * IRO[304].m1))
+#define XSTORM_ISCSI_LOCAL_VLAN_OFFSET(pfId) \
+	(IRO[305].base + ((pfId) * IRO[305].m1))
 #define XSTORM_ISCSI_NUM_OF_TASKS_OFFSET(pfId) \
-	(IRO[294].base + ((pfId) * IRO[294].m1))
+	(IRO[295].base + ((pfId) * IRO[295].m1))
 #define XSTORM_ISCSI_PAGE_SIZE_LOG_OFFSET(pfId) \
-	(IRO[293].base + ((pfId) * IRO[293].m1))
+	(IRO[294].base + ((pfId) * IRO[294].m1))
 #define XSTORM_ISCSI_PAGE_SIZE_OFFSET(pfId) \
-	(IRO[292].base + ((pfId) * IRO[292].m1))
+	(IRO[293].base + ((pfId) * IRO[293].m1))
 #define XSTORM_ISCSI_R2TQ_SIZE_OFFSET(pfId) \
-	(IRO[297].base + ((pfId) * IRO[297].m1))
+	(IRO[298].base + ((pfId) * IRO[298].m1))
 #define XSTORM_ISCSI_SQ_SIZE_OFFSET(pfId) \
-	(IRO[296].base + ((pfId) * IRO[296].m1))
+	(IRO[297].base + ((pfId) * IRO[297].m1))
 #define XSTORM_ISCSI_TCP_VARS_ADV_WND_SCL_OFFSET(pfId) \
-	(IRO[291].base + ((pfId) * IRO[291].m1))
+	(IRO[292].base + ((pfId) * IRO[292].m1))
 #define XSTORM_ISCSI_TCP_VARS_FLAGS_OFFSET(pfId) \
-	(IRO[290].base + ((pfId) * IRO[290].m1))
+	(IRO[291].base + ((pfId) * IRO[291].m1))
 #define XSTORM_ISCSI_TCP_VARS_TOS_OFFSET(pfId) \
-	(IRO[289].base + ((pfId) * IRO[289].m1))
+	(IRO[290].base + ((pfId) * IRO[290].m1))
 #define XSTORM_ISCSI_TCP_VARS_TTL_OFFSET(pfId) \
-	(IRO[288].base + ((pfId) * IRO[288].m1))
+	(IRO[289].base + ((pfId) * IRO[289].m1))
 #define XSTORM_RATE_SHAPING_PER_VN_VARS_OFFSET(pfId) \
 	(IRO[44].base + ((pfId) * IRO[44].m1))
 #define XSTORM_RECORD_SLOW_PATH_OFFSET(funcId) \
@@ -389,4 +386,8 @@
 
 #define UNDEF_IRO 0x80000000
 
+/* used for defining the amount of FCoE tasks supported for PF */
+#define MAX_FCOE_FUNCS_PER_ENGINE 2
+#define MAX_NUM_FCOE_TASKS_PER_ENGINE 4096
+
 #endif /* BNX2X_FW_DEFS_H */
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
index 037860ecc343..12f00a40cdf0 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
@@ -114,6 +114,10 @@ struct license_key {
 #define EPIO_CFG_EPIO30                     0x0000001f
 #define EPIO_CFG_EPIO31                     0x00000020
 
+struct mac_addr {
+	u32 upper;
+	u32 lower;
+};
 
 struct shared_hw_cfg {			 /* NVRAM Offset */
 	/* Up to 16 bytes of NULL-terminated string */
@@ -508,7 +512,22 @@ struct port_hw_cfg {		    /* port 0: 0x12c  port 1: 0x2bc */
 	#define PORT_HW_CFG_PAUSE_ON_HOST_RING_DISABLED               0x00000000
 	#define PORT_HW_CFG_PAUSE_ON_HOST_RING_ENABLED                0x00000001
 
-	u32 reserved0[6];				    /* 0x178 */
+	/* SFP+ Tx Equalization: NIC recommended and tested value is 0xBEB2
+	 * LOM recommended and tested value is 0xBEB2. Using a different
+	 * value means using a value not tested by BRCM
+	 */
+	u32 sfi_tap_values;                                 /* 0x178 */
+	#define PORT_HW_CFG_TX_EQUALIZATION_MASK                      0x0000FFFF
+	#define PORT_HW_CFG_TX_EQUALIZATION_SHIFT                     0
+
+	/* SFP+ Tx driver broadcast IDRIVER: NIC recommended and tested
+	 * value is 0x2. LOM recommended and tested value is 0x2. Using a
+	 * different value means using a value not tested by BRCM
+	 */
+	#define PORT_HW_CFG_TX_DRV_BROADCAST_MASK                     0x000F0000
+	#define PORT_HW_CFG_TX_DRV_BROADCAST_SHIFT                    16
+
+	u32 reserved0[5];				    /* 0x17c */
 
 	u32 aeu_int_mask;				    /* 0x190 */
 
@@ -2821,8 +2840,8 @@ struct afex_stats {
 
 #define BCM_5710_FW_MAJOR_VERSION			7
 #define BCM_5710_FW_MINOR_VERSION			8
-#define BCM_5710_FW_REVISION_VERSION		2
-#define BCM_5710_FW_ENGINEERING_VERSION			0
+#define BCM_5710_FW_REVISION_VERSION		17
+#define BCM_5710_FW_ENGINEERING_VERSION		0
 #define BCM_5710_FW_COMPILE_FLAGS			1
 
 
@@ -3513,11 +3532,14 @@ struct client_init_tx_data {
 #define CLIENT_INIT_TX_DATA_BCAST_ACCEPT_ALL_SHIFT 2
 #define CLIENT_INIT_TX_DATA_ACCEPT_ANY_VLAN (0x1<<3)
 #define CLIENT_INIT_TX_DATA_ACCEPT_ANY_VLAN_SHIFT 3
-#define CLIENT_INIT_TX_DATA_RESERVED1 (0xFFF<<4)
-#define CLIENT_INIT_TX_DATA_RESERVED1_SHIFT 4
+#define CLIENT_INIT_TX_DATA_RESERVED0 (0xFFF<<4)
+#define CLIENT_INIT_TX_DATA_RESERVED0_SHIFT 4
 	u8 default_vlan_flg;
 	u8 force_default_pri_flg;
-	__le32 reserved3;
+	u8 tunnel_lso_inc_ip_id;
+	u8 refuse_outband_vlan_flg;
+	u8 tunnel_non_lso_pcsum_location;
+	u8 reserved1;
 };
 
 /*
@@ -3551,6 +3573,11 @@ struct client_update_ramrod_data {
 	__le16 silent_vlan_mask;
 	u8 silent_vlan_removal_flg;
 	u8 silent_vlan_change_flg;
+	u8 refuse_outband_vlan_flg;
+	u8 refuse_outband_vlan_change_flg;
+	u8 tx_switching_flg;
+	u8 tx_switching_change_flg;
+	__le32 reserved1;
 	__le32 echo;
 };
 
@@ -3620,7 +3647,8 @@ struct eth_classify_header {
  */
 struct eth_classify_mac_cmd {
 	struct eth_classify_cmd_header header;
-	__le32 reserved0;
+	__le16 reserved0;
+	__le16 inner_mac;
 	__le16 mac_lsb;
 	__le16 mac_mid;
 	__le16 mac_msb;
@@ -3633,7 +3661,8 @@ struct eth_classify_mac_cmd {
  */
 struct eth_classify_pair_cmd {
 	struct eth_classify_cmd_header header;
-	__le32 reserved0;
+	__le16 reserved0;
+	__le16 inner_mac;
 	__le16 mac_lsb;
 	__le16 mac_mid;
 	__le16 mac_msb;
@@ -3855,8 +3884,68 @@ struct eth_halt_ramrod_data {
 
 
 /*
- * Command for setting multicast classification for a client
+ * destination and source mac address.
+ */
+struct eth_mac_addresses {
+#if defined(__BIG_ENDIAN)
+	__le16 dst_mid;
+	__le16 dst_lo;
+#elif defined(__LITTLE_ENDIAN)
+	__le16 dst_lo;
+	__le16 dst_mid;
+#endif
+#if defined(__BIG_ENDIAN)
+	__le16 src_lo;
+	__le16 dst_hi;
+#elif defined(__LITTLE_ENDIAN)
+	__le16 dst_hi;
+	__le16 src_lo;
+#endif
+#if defined(__BIG_ENDIAN)
+	__le16 src_hi;
+	__le16 src_mid;
+#elif defined(__LITTLE_ENDIAN)
+	__le16 src_mid;
+	__le16 src_hi;
+#endif
+};
+
+/* tunneling related data */
+struct eth_tunnel_data {
+#if defined(__BIG_ENDIAN)
+	__le16 dst_mid;
+	__le16 dst_lo;
+#elif defined(__LITTLE_ENDIAN)
+	__le16 dst_lo;
+	__le16 dst_mid;
+#endif
+#if defined(__BIG_ENDIAN)
+	__le16 reserved0;
+	__le16 dst_hi;
+#elif defined(__LITTLE_ENDIAN)
+	__le16 dst_hi;
+	__le16 reserved0;
+#endif
+#if defined(__BIG_ENDIAN)
+	u8 reserved1;
+	u8 ip_hdr_start_inner_w;
+	__le16 pseudo_csum;
+#elif defined(__LITTLE_ENDIAN)
+	__le16 pseudo_csum;
+	u8 ip_hdr_start_inner_w;
+	u8 reserved1;
+#endif
+};
+
+/* union for mac addresses and for tunneling data.
+ * considered as tunneling data only if (tunnel_exist == 1).
  */
+union eth_mac_addr_or_tunnel_data {
+	struct eth_mac_addresses mac_addr;
+	struct eth_tunnel_data tunnel_data;
+};
+
+/*Command for setting multicast classification for a client */
 struct eth_multicast_rules_cmd {
 	u8 cmd_general_data;
 #define ETH_MULTICAST_RULES_CMD_RX_CMD (0x1<<0)
@@ -3874,7 +3963,6 @@ struct eth_multicast_rules_cmd {
 	struct regpair reserved3;
 };
 
-
 /*
  * parameters for multicast classification ramrod
  */
@@ -3883,7 +3971,6 @@ struct eth_multicast_rules_ramrod_data {
 	struct eth_multicast_rules_cmd rules[MULTICAST_RULES_COUNT];
 };
 
-
 /*
  * Place holder for ramrods protocol specific data
  */
@@ -3947,11 +4034,14 @@ struct eth_rss_update_ramrod_data {
 #define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_TCP_CAPABILITY_SHIFT 4
 #define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_UDP_CAPABILITY (0x1<<5)
 #define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_UDP_CAPABILITY_SHIFT 5
+#define ETH_RSS_UPDATE_RAMROD_DATA_EN_5_TUPLE_CAPABILITY (0x1<<6)
+#define ETH_RSS_UPDATE_RAMROD_DATA_EN_5_TUPLE_CAPABILITY_SHIFT 6
 #define ETH_RSS_UPDATE_RAMROD_DATA_UPDATE_RSS_KEY (0x1<<7)
 #define ETH_RSS_UPDATE_RAMROD_DATA_UPDATE_RSS_KEY_SHIFT 7
 	u8 rss_result_mask;
 	u8 rss_mode;
-	__le32 __reserved2;
+	__le16 udp_4tuple_dst_port_mask;
+	__le16 udp_4tuple_dst_port_value;
 	u8 indirection_table[T_ETH_INDIRECTION_TABLE_SIZE];
 	__le32 rss_key[T_ETH_RSS_KEY];
 	__le32 echo;
@@ -4115,6 +4205,23 @@ enum eth_tpa_update_command {
 	MAX_ETH_TPA_UPDATE_COMMAND
 };
 
+/* In case of LSO over IPv4 tunnel, whether to increment
+ * IP ID on external IP header or internal IP header
+ */
+enum eth_tunnel_lso_inc_ip_id {
+	EXT_HEADER,
+	INT_HEADER,
+	MAX_ETH_TUNNEL_LSO_INC_IP_ID
+};
+
+/* In case tunnel exist and L4 checksum offload,
+ * the pseudo checksum location, on packet or on BD.
+ */
+enum eth_tunnel_non_lso_pcsum_location {
+	PCSUM_ON_PKT,
+	PCSUM_ON_BD,
+	MAX_ETH_TUNNEL_NON_LSO_PCSUM_LOCATION
+};
 
 /*
  * Tx regular BD structure
@@ -4166,8 +4273,8 @@ struct eth_tx_start_bd {
 #define ETH_TX_START_BD_FORCE_VLAN_MODE_SHIFT 4
 #define ETH_TX_START_BD_PARSE_NBDS (0x3<<5)
 #define ETH_TX_START_BD_PARSE_NBDS_SHIFT 5
-#define ETH_TX_START_BD_RESREVED (0x1<<7)
-#define ETH_TX_START_BD_RESREVED_SHIFT 7
+#define ETH_TX_START_BD_TUNNEL_EXIST (0x1<<7)
+#define ETH_TX_START_BD_TUNNEL_EXIST_SHIFT 7
 };
 
 /*
@@ -4216,15 +4323,10 @@ struct eth_tx_parse_bd_e1x {
  * Tx parsing BD structure for ETH E2
  */
 struct eth_tx_parse_bd_e2 {
-	__le16 dst_mac_addr_lo;
-	__le16 dst_mac_addr_mid;
-	__le16 dst_mac_addr_hi;
-	__le16 src_mac_addr_lo;
-	__le16 src_mac_addr_mid;
-	__le16 src_mac_addr_hi;
+	union eth_mac_addr_or_tunnel_data data;
 	__le32 parsing_data;
-#define ETH_TX_PARSE_BD_E2_TCP_HDR_START_OFFSET_W (0x7FF<<0)
-#define ETH_TX_PARSE_BD_E2_TCP_HDR_START_OFFSET_W_SHIFT 0
+#define ETH_TX_PARSE_BD_E2_L4_HDR_START_OFFSET_W (0x7FF<<0)
+#define ETH_TX_PARSE_BD_E2_L4_HDR_START_OFFSET_W_SHIFT 0
 #define ETH_TX_PARSE_BD_E2_TCP_HDR_LENGTH_DW (0xF<<11)
 #define ETH_TX_PARSE_BD_E2_TCP_HDR_LENGTH_DW_SHIFT 11
 #define ETH_TX_PARSE_BD_E2_IPV6_WITH_EXT_HDR (0x1<<15)
@@ -4236,8 +4338,51 @@ struct eth_tx_parse_bd_e2 {
 };
 
 /*
- * The last BD in the BD memory will hold a pointer to the next BD memory
+ * Tx 2nd parsing BD structure for ETH packet
  */
+struct eth_tx_parse_2nd_bd {
+	__le16 global_data;
+#define ETH_TX_PARSE_2ND_BD_IP_HDR_START_OUTER_W (0xF<<0)
+#define ETH_TX_PARSE_2ND_BD_IP_HDR_START_OUTER_W_SHIFT 0
+#define ETH_TX_PARSE_2ND_BD_IP_HDR_TYPE_OUTER (0x1<<4)
+#define ETH_TX_PARSE_2ND_BD_IP_HDR_TYPE_OUTER_SHIFT 4
+#define ETH_TX_PARSE_2ND_BD_LLC_SNAP_EN (0x1<<5)
+#define ETH_TX_PARSE_2ND_BD_LLC_SNAP_EN_SHIFT 5
+#define ETH_TX_PARSE_2ND_BD_NS_FLG (0x1<<6)
+#define ETH_TX_PARSE_2ND_BD_NS_FLG_SHIFT 6
+#define ETH_TX_PARSE_2ND_BD_TUNNEL_UDP_EXIST (0x1<<7)
+#define ETH_TX_PARSE_2ND_BD_TUNNEL_UDP_EXIST_SHIFT 7
+#define ETH_TX_PARSE_2ND_BD_IP_HDR_LEN_OUTER_W (0x1F<<8)
+#define ETH_TX_PARSE_2ND_BD_IP_HDR_LEN_OUTER_W_SHIFT 8
+#define ETH_TX_PARSE_2ND_BD_RESERVED0 (0x7<<13)
+#define ETH_TX_PARSE_2ND_BD_RESERVED0_SHIFT 13
+	__le16 reserved1;
+	u8 tcp_flags;
+#define ETH_TX_PARSE_2ND_BD_FIN_FLG (0x1<<0)
+#define ETH_TX_PARSE_2ND_BD_FIN_FLG_SHIFT 0
+#define ETH_TX_PARSE_2ND_BD_SYN_FLG (0x1<<1)
+#define ETH_TX_PARSE_2ND_BD_SYN_FLG_SHIFT 1
+#define ETH_TX_PARSE_2ND_BD_RST_FLG (0x1<<2)
+#define ETH_TX_PARSE_2ND_BD_RST_FLG_SHIFT 2
+#define ETH_TX_PARSE_2ND_BD_PSH_FLG (0x1<<3)
+#define ETH_TX_PARSE_2ND_BD_PSH_FLG_SHIFT 3
+#define ETH_TX_PARSE_2ND_BD_ACK_FLG (0x1<<4)
+#define ETH_TX_PARSE_2ND_BD_ACK_FLG_SHIFT 4
+#define ETH_TX_PARSE_2ND_BD_URG_FLG (0x1<<5)
+#define ETH_TX_PARSE_2ND_BD_URG_FLG_SHIFT 5
+#define ETH_TX_PARSE_2ND_BD_ECE_FLG (0x1<<6)
+#define ETH_TX_PARSE_2ND_BD_ECE_FLG_SHIFT 6
+#define ETH_TX_PARSE_2ND_BD_CWR_FLG (0x1<<7)
+#define ETH_TX_PARSE_2ND_BD_CWR_FLG_SHIFT 7
+	u8 reserved2;
+	u8 tunnel_udp_hdr_start_w;
+	u8 fw_ip_hdr_to_payload_w;
+	__le16 fw_ip_csum_wo_len_flags_frag;
+	__le16 hw_ip_id;
+	__le32 tcp_send_seq;
+};
+
+/* The last BD in the BD memory will hold a pointer to the next BD memory */
 struct eth_tx_next_bd {
 	__le32 addr_lo;
 	__le32 addr_hi;
@@ -4252,6 +4397,7 @@ union eth_tx_bd_types {
 	struct eth_tx_bd reg_bd;
 	struct eth_tx_parse_bd_e1x parse_bd_e1x;
 	struct eth_tx_parse_bd_e2 parse_bd_e2;
+	struct eth_tx_parse_2nd_bd parse_2nd_bd;
 	struct eth_tx_next_bd next_bd;
 };
 
@@ -4663,10 +4809,10 @@ enum common_spqe_cmd_id {
 	RAMROD_CMD_ID_COMMON_STOP_TRAFFIC,
 	RAMROD_CMD_ID_COMMON_START_TRAFFIC,
 	RAMROD_CMD_ID_COMMON_AFEX_VIF_LISTS,
+	RAMROD_CMD_ID_COMMON_SET_TIMESYNC,
 	MAX_COMMON_SPQE_CMD_ID
 };
 
-
 /*
  * Per-protocol connection types
  */
@@ -4863,7 +5009,7 @@ struct vf_flr_event_data {
  */
 struct malicious_vf_event_data {
 	u8 vf_id;
-	u8 reserved0;
+	u8 err_id;
 	u16 reserved1;
 	u32 reserved2;
 	u32 reserved3;
@@ -4969,10 +5115,10 @@ enum event_ring_opcode {
 	EVENT_RING_OPCODE_CLASSIFICATION_RULES,
 	EVENT_RING_OPCODE_FILTERS_RULES,
 	EVENT_RING_OPCODE_MULTICAST_RULES,
+	EVENT_RING_OPCODE_SET_TIMESYNC,
 	MAX_EVENT_RING_OPCODE
 };
 
-
 /*
  * Modes for fairness algorithm
  */
@@ -5010,14 +5156,18 @@ struct flow_control_configuration {
  */
 struct function_start_data {
 	u8 function_mode;
-	u8 reserved;
+	u8 allow_npar_tx_switching;
 	__le16 sd_vlan_tag;
 	__le16 vif_id;
 	u8 path_id;
 	u8 network_cos_mode;
+	u8 dmae_cmd_id;
+	u8 gre_tunnel_mode;
+	u8 gre_tunnel_rss;
+	u8 nvgre_clss_en;
+	__le16 reserved1[2];
 };
 
-
 struct function_update_data {
 	u8 vif_id_change_flg;
 	u8 afex_default_vlan_change_flg;
@@ -5027,14 +5177,19 @@ struct function_update_data {
 	__le16 afex_default_vlan;
 	u8 allowed_priorities;
 	u8 network_cos_mode;
+	u8 lb_mode_en_change_flg;
 	u8 lb_mode_en;
 	u8 tx_switch_suspend_change_flg;
 	u8 tx_switch_suspend;
 	u8 echo;
-	__le16 reserved1;
+	u8 reserved1;
+	u8 update_gre_cfg_flg;
+	u8 gre_tunnel_mode;
+	u8 gre_tunnel_rss;
+	u8 nvgre_clss_en;
+	u32 reserved3;
 };
 
-
 /*
  * FW version stored in the Xstorm RAM
  */
@@ -5061,6 +5216,22 @@ struct fw_version {
 #define __FW_VERSION_RESERVED_SHIFT 4
 };
 
+/* GRE RSS Mode */
+enum gre_rss_mode {
+	GRE_OUTER_HEADERS_RSS,
+	GRE_INNER_HEADERS_RSS,
+	NVGRE_KEY_ENTROPY_RSS,
+	MAX_GRE_RSS_MODE
+};
+
+/* GRE Tunnel Mode */
+enum gre_tunnel_type {
+	NO_GRE_TUNNEL,
+	NVGRE_TUNNEL,
+	L2GRE_TUNNEL,
+	IPGRE_TUNNEL,
+	MAX_GRE_TUNNEL_TYPE
+};
 
 /*
  * Dynamic Host-Coalescing - Driver(host) counters
@@ -5224,6 +5395,26 @@ enum ip_ver {
 	MAX_IP_VER
 };
 
+/*
+ * Malicious VF error ID
+ */
+enum malicious_vf_error_id {
+	VF_PF_CHANNEL_NOT_READY,
+	ETH_ILLEGAL_BD_LENGTHS,
+	ETH_PACKET_TOO_SHORT,
+	ETH_PAYLOAD_TOO_BIG,
+	ETH_ILLEGAL_ETH_TYPE,
+	ETH_ILLEGAL_LSO_HDR_LEN,
+	ETH_TOO_MANY_BDS,
+	ETH_ZERO_HDR_NBDS,
+	ETH_START_BD_NOT_SET,
+	ETH_ILLEGAL_PARSE_NBDS,
+	ETH_IPV6_AND_CHECKSUM,
+	ETH_VLAN_FLG_INCORRECT,
+	ETH_ILLEGAL_LSO_MSS,
+	ETH_TUNNEL_NOT_SUPPORTED,
+	MAX_MALICIOUS_VF_ERROR_ID
+};
 
 /*
  * Multi-function modes
@@ -5368,7 +5559,6 @@ struct protocol_common_spe {
 	union protocol_common_specific_data data;
 };
 
-
 /*
  * The send queue element
  */
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
index 0283f343b0d1..9d64b988ab34 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
@@ -27,6 +27,10 @@
 #include "bnx2x.h"
 #include "bnx2x_cmn.h"
 
+typedef int (*read_sfp_module_eeprom_func_p)(struct bnx2x_phy *phy,
+					     struct link_params *params,
+					     u8 dev_addr, u16 addr, u8 byte_cnt,
+					     u8 *o_buf, u8);
 /********************************************************/
 #define ETH_HLEN			14
 /* L2 header size + 2*VLANs (8 bytes) + LLC SNAP (8 bytes) */
@@ -152,6 +156,7 @@
 #define SFP_EEPROM_CON_TYPE_ADDR		0x2
 	#define SFP_EEPROM_CON_TYPE_VAL_LC	0x7
 	#define SFP_EEPROM_CON_TYPE_VAL_COPPER	0x21
+	#define SFP_EEPROM_CON_TYPE_VAL_RJ45	0x22
 
 
 #define SFP_EEPROM_COMP_CODE_ADDR		0x3
@@ -3127,11 +3132,6 @@ static int bnx2x_bsc_read(struct link_params *params,
 	int rc = 0;
 	struct bnx2x *bp = params->bp;
 
-	if ((sl_devid != 0xa0) && (sl_devid != 0xa2)) {
-		DP(NETIF_MSG_LINK, "invalid sl_devid 0x%x\n", sl_devid);
-		return -EINVAL;
-	}
-
 	if (xfer_cnt > 16) {
 		DP(NETIF_MSG_LINK, "invalid xfer_cnt %d. Max is 16 bytes\n",
 					xfer_cnt);
@@ -3426,13 +3426,19 @@ static void bnx2x_calc_ieee_aneg_adv(struct bnx2x_phy *phy,
 
 	switch (phy->req_flow_ctrl) {
 	case BNX2X_FLOW_CTRL_AUTO:
-		if (params->req_fc_auto_adv == BNX2X_FLOW_CTRL_BOTH)
+		switch (params->req_fc_auto_adv) {
+		case BNX2X_FLOW_CTRL_BOTH:
 			*ieee_fc |= MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_BOTH;
-		else
+			break;
+		case BNX2X_FLOW_CTRL_RX:
+		case BNX2X_FLOW_CTRL_TX:
 			*ieee_fc |=
-			MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_ASYMMETRIC;
+				MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_ASYMMETRIC;
+			break;
+		default:
+			break;
+		}
 		break;
-
 	case BNX2X_FLOW_CTRL_TX:
 		*ieee_fc |= MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_ASYMMETRIC;
 		break;
@@ -3629,6 +3635,16 @@ static u8 bnx2x_ext_phy_resolve_fc(struct bnx2x_phy *phy,
  * init configuration, and set/clear SGMII flag. Internal
  * phy init is done purely in phy_init stage.
  */
+#define WC_TX_DRIVER(post2, idriver, ipre) \
+	((post2 << MDIO_WC_REG_TX0_TX_DRIVER_POST2_COEFF_OFFSET) | \
+	 (idriver << MDIO_WC_REG_TX0_TX_DRIVER_IDRIVER_OFFSET) | \
+	 (ipre << MDIO_WC_REG_TX0_TX_DRIVER_IPRE_DRIVER_OFFSET))
+
+#define WC_TX_FIR(post, main, pre) \
+	((post << MDIO_WC_REG_TX_FIR_TAP_POST_TAP_OFFSET) | \
+	 (main << MDIO_WC_REG_TX_FIR_TAP_MAIN_TAP_OFFSET) | \
+	 (pre << MDIO_WC_REG_TX_FIR_TAP_PRE_TAP_OFFSET))
+
 static void bnx2x_warpcore_enable_AN_KR2(struct bnx2x_phy *phy,
 					 struct link_params *params,
 					 struct link_vars *vars)
@@ -3728,7 +3744,7 @@ static void bnx2x_warpcore_enable_AN_KR(struct bnx2x_phy *phy,
 	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;
+		u16 addr = MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X2;
 		an_adv |= (1<<5);
 
 		/* Enable CL37 1G Parallel Detect */
@@ -3753,20 +3769,13 @@ static void bnx2x_warpcore_enable_AN_KR(struct bnx2x_phy *phy,
 	/* Set Transmit PMD settings */
 	lane = bnx2x_get_warpcore_lane(phy, params);
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-		      MDIO_WC_REG_TX0_TX_DRIVER + 0x10*lane,
-		     ((0x02 << MDIO_WC_REG_TX0_TX_DRIVER_POST2_COEFF_OFFSET) |
-		      (0x06 << MDIO_WC_REG_TX0_TX_DRIVER_IDRIVER_OFFSET) |
-		      (0x09 << MDIO_WC_REG_TX0_TX_DRIVER_IPRE_DRIVER_OFFSET)));
+			 MDIO_WC_REG_TX0_TX_DRIVER + 0x10*lane,
+			 WC_TX_DRIVER(0x02, 0x06, 0x09));
 	/* Configure the next lane if dual mode */
 	if (phy->flags & FLAGS_WC_DUAL_MODE)
 		bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
 				 MDIO_WC_REG_TX0_TX_DRIVER + 0x10*(lane+1),
-				 ((0x02 <<
-				 MDIO_WC_REG_TX0_TX_DRIVER_POST2_COEFF_OFFSET) |
-				  (0x06 <<
-				   MDIO_WC_REG_TX0_TX_DRIVER_IDRIVER_OFFSET) |
-				  (0x09 <<
-				MDIO_WC_REG_TX0_TX_DRIVER_IPRE_DRIVER_OFFSET)));
+				 WC_TX_DRIVER(0x02, 0x06, 0x09));
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
 			 MDIO_WC_REG_CL72_USERB0_CL72_OS_DEF_CTRL,
 			 0x03f0);
@@ -3909,6 +3918,8 @@ static void bnx2x_warpcore_set_10G_XFI(struct bnx2x_phy *phy,
 {
 	struct bnx2x *bp = params->bp;
 	u16 misc1_val, tap_val, tx_driver_val, lane, val;
+	u32 cfg_tap_val, tx_drv_brdct, tx_equal;
+
 	/* Hold rxSeqStart */
 	bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
 				 MDIO_WC_REG_DSC2B0_DSC_MISC_CTRL0, 0x8000);
@@ -3952,23 +3963,33 @@ static void bnx2x_warpcore_set_10G_XFI(struct bnx2x_phy *phy,
 
 	if (is_xfi) {
 		misc1_val |= 0x5;
-		tap_val = ((0x08 << MDIO_WC_REG_TX_FIR_TAP_POST_TAP_OFFSET) |
-			   (0x37 << MDIO_WC_REG_TX_FIR_TAP_MAIN_TAP_OFFSET) |
-			   (0x00 << MDIO_WC_REG_TX_FIR_TAP_PRE_TAP_OFFSET));
-		tx_driver_val =
-		      ((0x00 << MDIO_WC_REG_TX0_TX_DRIVER_POST2_COEFF_OFFSET) |
-		       (0x02 << MDIO_WC_REG_TX0_TX_DRIVER_IDRIVER_OFFSET) |
-		       (0x03 << MDIO_WC_REG_TX0_TX_DRIVER_IPRE_DRIVER_OFFSET));
-
+		tap_val = WC_TX_FIR(0x08, 0x37, 0x00);
+		tx_driver_val = WC_TX_DRIVER(0x00, 0x02, 0x03);
 	} else {
+		cfg_tap_val = REG_RD(bp, params->shmem_base +
+				     offsetof(struct shmem_region, dev_info.
+					      port_hw_config[params->port].
+					      sfi_tap_values));
+
+		tx_equal = cfg_tap_val & PORT_HW_CFG_TX_EQUALIZATION_MASK;
+
+		tx_drv_brdct = (cfg_tap_val &
+				PORT_HW_CFG_TX_DRV_BROADCAST_MASK) >>
+			       PORT_HW_CFG_TX_DRV_BROADCAST_SHIFT;
+
 		misc1_val |= 0x9;
-		tap_val = ((0x0f << MDIO_WC_REG_TX_FIR_TAP_POST_TAP_OFFSET) |
-			   (0x2b << MDIO_WC_REG_TX_FIR_TAP_MAIN_TAP_OFFSET) |
-			   (0x02 << MDIO_WC_REG_TX_FIR_TAP_PRE_TAP_OFFSET));
-		tx_driver_val =
-		      ((0x03 << MDIO_WC_REG_TX0_TX_DRIVER_POST2_COEFF_OFFSET) |
-		       (0x02 << MDIO_WC_REG_TX0_TX_DRIVER_IDRIVER_OFFSET) |
-		       (0x06 << MDIO_WC_REG_TX0_TX_DRIVER_IPRE_DRIVER_OFFSET));
+
+		/* TAP values are controlled by nvram, if value there isn't 0 */
+		if (tx_equal)
+			tap_val = (u16)tx_equal;
+		else
+			tap_val = WC_TX_FIR(0x0f, 0x2b, 0x02);
+
+		if (tx_drv_brdct)
+			tx_driver_val = WC_TX_DRIVER(0x03, (u16)tx_drv_brdct,
+						     0x06);
+		else
+			tx_driver_val = WC_TX_DRIVER(0x03, 0x02, 0x06);
 	}
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
 			 MDIO_WC_REG_SERDESDIGITAL_MISC1, misc1_val);
@@ -4105,15 +4126,11 @@ static void bnx2x_warpcore_set_20G_DXGXS(struct bnx2x *bp,
 	/* Set Transmit PMD settings */
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
 			 MDIO_WC_REG_TX_FIR_TAP,
-			((0x12 << MDIO_WC_REG_TX_FIR_TAP_POST_TAP_OFFSET) |
-			 (0x2d << MDIO_WC_REG_TX_FIR_TAP_MAIN_TAP_OFFSET) |
-			 (0x00 << MDIO_WC_REG_TX_FIR_TAP_PRE_TAP_OFFSET) |
-			 MDIO_WC_REG_TX_FIR_TAP_ENABLE));
+			 (WC_TX_FIR(0x12, 0x2d, 0x00) |
+			  MDIO_WC_REG_TX_FIR_TAP_ENABLE));
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-		      MDIO_WC_REG_TX0_TX_DRIVER + 0x10*lane,
-		     ((0x02 << MDIO_WC_REG_TX0_TX_DRIVER_POST2_COEFF_OFFSET) |
-		      (0x02 << MDIO_WC_REG_TX0_TX_DRIVER_IDRIVER_OFFSET) |
-		      (0x02 << MDIO_WC_REG_TX0_TX_DRIVER_IPRE_DRIVER_OFFSET)));
+			 MDIO_WC_REG_TX0_TX_DRIVER + 0x10*lane,
+			 WC_TX_DRIVER(0x02, 0x02, 0x02));
 }
 
 static void bnx2x_warpcore_set_sgmii_speed(struct bnx2x_phy *phy,
@@ -4750,8 +4767,8 @@ void bnx2x_link_status_update(struct link_params *params,
 					    port_mb[port].link_status));
 
 	/* Force link UP in non LOOPBACK_EXT loopback mode(s) */
-	if (bp->link_params.loopback_mode != LOOPBACK_NONE &&
-	    bp->link_params.loopback_mode != LOOPBACK_EXT)
+	if (params->loopback_mode != LOOPBACK_NONE &&
+	    params->loopback_mode != LOOPBACK_EXT)
 		vars->link_status |= LINK_STATUS_LINK_UP;
 
 	if (bnx2x_eee_has_cap(params))
@@ -7758,7 +7775,8 @@ static void bnx2x_sfp_set_transmitter(struct link_params *params,
 
 static int bnx2x_8726_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 					     struct link_params *params,
-					     u16 addr, u8 byte_cnt, u8 *o_buf)
+					     u8 dev_addr, u16 addr, u8 byte_cnt,
+					     u8 *o_buf, u8 is_init)
 {
 	struct bnx2x *bp = params->bp;
 	u16 val = 0;
@@ -7771,7 +7789,7 @@ static int bnx2x_8726_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 	/* Set the read command byte count */
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD, MDIO_PMA_REG_SFP_TWO_WIRE_BYTE_CNT,
-			 (byte_cnt | 0xa000));
+			 (byte_cnt | (dev_addr << 8)));
 
 	/* Set the read command address */
 	bnx2x_cl45_write(bp, phy,
@@ -7845,6 +7863,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,
+						 u8 dev_addr,
 						 u16 addr, u8 byte_cnt,
 						 u8 *o_buf, u8 is_init)
 {
@@ -7869,7 +7888,7 @@ static int bnx2x_warpcore_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 			usleep_range(1000, 2000);
 			bnx2x_warpcore_power_module(params, 1);
 		}
-		rc = bnx2x_bsc_read(params, phy, 0xa0, addr32, 0, byte_cnt,
+		rc = bnx2x_bsc_read(params, phy, dev_addr, addr32, 0, byte_cnt,
 				    data_array);
 	} while ((rc != 0) && (++cnt < I2C_WA_RETRY_CNT));
 
@@ -7885,7 +7904,8 @@ static int bnx2x_warpcore_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 
 static int bnx2x_8727_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 					     struct link_params *params,
-					     u16 addr, u8 byte_cnt, u8 *o_buf)
+					     u8 dev_addr, u16 addr, u8 byte_cnt,
+					     u8 *o_buf, u8 is_init)
 {
 	struct bnx2x *bp = params->bp;
 	u16 val, i;
@@ -7896,6 +7916,15 @@ static int bnx2x_8727_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 		return -EINVAL;
 	}
 
+	/* Set 2-wire transfer rate of SFP+ module EEPROM
+	 * to 100Khz since some DACs(direct attached cables) do
+	 * not work at 400Khz.
+	 */
+	bnx2x_cl45_write(bp, phy,
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_8727_TWO_WIRE_SLAVE_ADDR,
+			 ((dev_addr << 8) | 1));
+
 	/* Need to read from 1.8000 to clear it */
 	bnx2x_cl45_read(bp, phy,
 			MDIO_PMA_DEVAD,
@@ -7968,26 +7997,44 @@ static int bnx2x_8727_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 
 	return -EINVAL;
 }
-
 int bnx2x_read_sfp_module_eeprom(struct bnx2x_phy *phy,
-				 struct link_params *params, u16 addr,
-				 u8 byte_cnt, u8 *o_buf)
+				 struct link_params *params, u8 dev_addr,
+				 u16 addr, u16 byte_cnt, u8 *o_buf)
 {
-	int rc = -EOPNOTSUPP;
+	int rc = 0;
+	struct bnx2x *bp = params->bp;
+	u8 xfer_size;
+	u8 *user_data = o_buf;
+	read_sfp_module_eeprom_func_p read_func;
+
+	if ((dev_addr != 0xa0) && (dev_addr != 0xa2)) {
+		DP(NETIF_MSG_LINK, "invalid dev_addr 0x%x\n", dev_addr);
+		return -EINVAL;
+	}
+
 	switch (phy->type) {
 	case PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8726:
-		rc = bnx2x_8726_read_sfp_module_eeprom(phy, params, addr,
-						       byte_cnt, o_buf);
-	break;
+		read_func = bnx2x_8726_read_sfp_module_eeprom;
+		break;
 	case PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8727:
 	case PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8722:
-		rc = bnx2x_8727_read_sfp_module_eeprom(phy, params, addr,
-						       byte_cnt, o_buf);
-	break;
+		read_func = bnx2x_8727_read_sfp_module_eeprom;
+		break;
 	case PORT_HW_CFG_XGXS_EXT_PHY_TYPE_DIRECT:
-		rc = bnx2x_warpcore_read_sfp_module_eeprom(phy, params, addr,
-							   byte_cnt, o_buf, 0);
-	break;
+		read_func = bnx2x_warpcore_read_sfp_module_eeprom;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	while (!rc && (byte_cnt > 0)) {
+		xfer_size = (byte_cnt > SFP_EEPROM_PAGE_SIZE) ?
+			SFP_EEPROM_PAGE_SIZE : byte_cnt;
+		rc = read_func(phy, params, dev_addr, addr, xfer_size,
+			       user_data, 0);
+		byte_cnt -= xfer_size;
+		user_data += xfer_size;
+		addr += xfer_size;
 	}
 	return rc;
 }
@@ -8004,6 +8051,7 @@ static int bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 	/* First check for copper cable */
 	if (bnx2x_read_sfp_module_eeprom(phy,
 					 params,
+					 I2C_DEV_ADDR_A0,
 					 SFP_EEPROM_CON_TYPE_ADDR,
 					 2,
 					 (u8 *)val) != 0) {
@@ -8021,6 +8069,7 @@ static int bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 		 */
 		if (bnx2x_read_sfp_module_eeprom(phy,
 					       params,
+					       I2C_DEV_ADDR_A0,
 					       SFP_EEPROM_FC_TX_TECH_ADDR,
 					       1,
 					       &copper_module_type) != 0) {
@@ -8049,20 +8098,24 @@ static int bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 		break;
 	}
 	case SFP_EEPROM_CON_TYPE_VAL_LC:
+	case SFP_EEPROM_CON_TYPE_VAL_RJ45:
 		check_limiting_mode = 1;
 		if ((val[1] & (SFP_EEPROM_COMP_CODE_SR_MASK |
 			       SFP_EEPROM_COMP_CODE_LR_MASK |
 			       SFP_EEPROM_COMP_CODE_LRM_MASK)) == 0) {
-			DP(NETIF_MSG_LINK, "1G Optic module detected\n");
+			DP(NETIF_MSG_LINK, "1G SFP module detected\n");
 			gport = params->port;
 			phy->media_type = ETH_PHY_SFP_1G_FIBER;
-			phy->req_line_speed = SPEED_1000;
-			if (!CHIP_IS_E1x(bp))
-				gport = BP_PATH(bp) + (params->port << 1);
-			netdev_err(bp->dev, "Warning: Link speed was forced to 1000Mbps."
-			      " Current SFP module in port %d is not"
-			      " compliant with 10G Ethernet\n",
-			 gport);
+			if (phy->req_line_speed != SPEED_1000) {
+				phy->req_line_speed = SPEED_1000;
+				if (!CHIP_IS_E1x(bp)) {
+					gport = BP_PATH(bp) +
+					(params->port << 1);
+				}
+				netdev_err(bp->dev,
+					   "Warning: Link speed was forced to 1000Mbps. Current SFP module in port %d is not compliant with 10G Ethernet\n",
+					   gport);
+			}
 		} else {
 			int idx, cfg_idx = 0;
 			DP(NETIF_MSG_LINK, "10G Optic module detected\n");
@@ -8101,6 +8154,7 @@ static int bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 		u8 options[SFP_EEPROM_OPTIONS_SIZE];
 		if (bnx2x_read_sfp_module_eeprom(phy,
 						 params,
+						 I2C_DEV_ADDR_A0,
 						 SFP_EEPROM_OPTIONS_ADDR,
 						 SFP_EEPROM_OPTIONS_SIZE,
 						 options) != 0) {
@@ -8167,6 +8221,7 @@ static int bnx2x_verify_sfp_module(struct bnx2x_phy *phy,
 	/* Format the warning message */
 	if (bnx2x_read_sfp_module_eeprom(phy,
 					 params,
+					 I2C_DEV_ADDR_A0,
 					 SFP_EEPROM_VENDOR_NAME_ADDR,
 					 SFP_EEPROM_VENDOR_NAME_SIZE,
 					 (u8 *)vendor_name))
@@ -8175,6 +8230,7 @@ static int bnx2x_verify_sfp_module(struct bnx2x_phy *phy,
 		vendor_name[SFP_EEPROM_VENDOR_NAME_SIZE] = '\0';
 	if (bnx2x_read_sfp_module_eeprom(phy,
 					 params,
+					 I2C_DEV_ADDR_A0,
 					 SFP_EEPROM_PART_NO_ADDR,
 					 SFP_EEPROM_PART_NO_SIZE,
 					 (u8 *)vendor_pn))
@@ -8205,12 +8261,13 @@ static int bnx2x_wait_for_sfp_module_initialized(struct bnx2x_phy *phy,
 
 	for (timeout = 0; timeout < 60; timeout++) {
 		if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_DIRECT)
-			rc = bnx2x_warpcore_read_sfp_module_eeprom(phy,
-								   params, 1,
-								   1, &val, 1);
+			rc = bnx2x_warpcore_read_sfp_module_eeprom(
+				phy, params, I2C_DEV_ADDR_A0, 1, 1, &val,
+				1);
 		else
-			rc = bnx2x_read_sfp_module_eeprom(phy, params, 1, 1,
-							  &val);
+			rc = bnx2x_read_sfp_module_eeprom(phy, params,
+							  I2C_DEV_ADDR_A0,
+							  1, 1, &val);
 		if (rc == 0) {
 			DP(NETIF_MSG_LINK,
 			   "SFP+ module initialization took %d ms\n",
@@ -8219,7 +8276,8 @@ static int bnx2x_wait_for_sfp_module_initialized(struct bnx2x_phy *phy,
 		}
 		usleep_range(5000, 10000);
 	}
-	rc = bnx2x_read_sfp_module_eeprom(phy, params, 1, 1, &val);
+	rc = bnx2x_read_sfp_module_eeprom(phy, params, I2C_DEV_ADDR_A0,
+					  1, 1, &val);
 	return rc;
 }
 
@@ -8376,15 +8434,6 @@ static void bnx2x_8727_specific_func(struct bnx2x_phy *phy,
 		bnx2x_cl45_write(bp, phy,
 				 MDIO_PMA_DEVAD, MDIO_PMA_REG_8727_PCS_OPT_CTRL,
 				 val);
-
-		/* Set 2-wire transfer rate of SFP+ module EEPROM
-		 * to 100Khz since some DACs(direct attached cables) do
-		 * not work at 400Khz.
-		 */
-		bnx2x_cl45_write(bp, phy,
-				 MDIO_PMA_DEVAD,
-				 MDIO_PMA_REG_8727_TWO_WIRE_SLAVE_ADDR,
-				 0xa001);
 		break;
 	default:
 		DP(NETIF_MSG_LINK, "Function 0x%x not supported by 8727\n",
@@ -9528,8 +9577,7 @@ static void bnx2x_save_848xx_spirom_version(struct bnx2x_phy *phy,
 	} else {
 		/* For 32-bit registers in 848xx, access via MDIO2ARM i/f. */
 		/* (1) set reg 0xc200_0014(SPI_BRIDGE_CTRL_2) to 0x03000000 */
-		for (i = 0; i < ARRAY_SIZE(reg_set);
-		      i++)
+		for (i = 0; i < ARRAY_SIZE(reg_set); i++)
 			bnx2x_cl45_write(bp, phy, reg_set[i].devad,
 					 reg_set[i].reg, reg_set[i].val);
 
@@ -10281,7 +10329,8 @@ static u8 bnx2x_848xx_read_status(struct bnx2x_phy *phy,
 				LINK_STATUS_LINK_PARTNER_10GXFD_CAPABLE;
 
 		/* Determine if EEE was negotiated */
-		if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833)
+		if ((phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833) ||
+		    (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84834))
 			bnx2x_eee_an_resolve(phy, params, vars);
 	}
 
@@ -12242,7 +12291,7 @@ static void bnx2x_init_bmac_loopback(struct link_params *params,
 
 		bnx2x_xgxs_deassert(params);
 
-		/* set bmac loopback */
+		/* Set bmac loopback */
 		bnx2x_bmac_enable(params, vars, 1, 1);
 
 		REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE + params->port*4, 0);
@@ -12261,7 +12310,7 @@ static void bnx2x_init_emac_loopback(struct link_params *params,
 		vars->phy_flags = PHY_XGXS_FLAG;
 
 		bnx2x_xgxs_deassert(params);
-		/* set bmac loopback */
+		/* Set bmac loopback */
 		bnx2x_emac_enable(params, vars, 1);
 		bnx2x_emac_program(params, vars);
 		REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE + params->port*4, 0);
@@ -12521,6 +12570,7 @@ int bnx2x_phy_init(struct link_params *params, struct link_vars *vars)
 		   params->req_line_speed[0], params->req_flow_ctrl[0]);
 	DP(NETIF_MSG_LINK, "(2) req_speed %d, req_flowctrl %d\n",
 		   params->req_line_speed[1], params->req_flow_ctrl[1]);
+	DP(NETIF_MSG_LINK, "req_adv_flow_ctrl 0x%x\n", params->req_fc_auto_adv);
 	vars->link_status = 0;
 	vars->phy_link_up = 0;
 	vars->link_up = 0;
@@ -13440,8 +13490,8 @@ static void bnx2x_check_kr2_wa(struct link_params *params,
 	int sigdet;
 
 	/* Once KR2 was disabled, wait 5 seconds before checking KR2 recovery
-	 * since some switches tend to reinit the AN process and clear the
-	 * advertised BP/NP after ~2 seconds causing the KR2 to be disabled
+	 * Since some switches tend to reinit the AN process and clear the
+	 * the advertised BP/NP after ~2 seconds causing the KR2 to be disabled
 	 * and recovered many times
 	 */
 	if (vars->check_kr2_recovery_cnt > 0) {
@@ -13469,8 +13519,10 @@ static void bnx2x_check_kr2_wa(struct link_params *params,
 
 	/* CL73 has not begun yet */
 	if (base_page == 0) {
-		if (!(vars->link_attr_sync & LINK_ATTR_SYNC_KR2_ENABLE))
+		if (!(vars->link_attr_sync & LINK_ATTR_SYNC_KR2_ENABLE)) {
 			bnx2x_kr2_recovery(params, vars, phy);
+			DP(NETIF_MSG_LINK, "No BP\n");
+		}
 		return;
 	}
 
@@ -13486,7 +13538,7 @@ static void bnx2x_check_kr2_wa(struct link_params *params,
 	if (!(vars->link_attr_sync & LINK_ATTR_SYNC_KR2_ENABLE)) {
 		if (!not_kr2_device) {
 			DP(NETIF_MSG_LINK, "BP=0x%x, NP=0x%x\n", base_page,
-				       next_page);
+			   next_page);
 			bnx2x_kr2_recovery(params, vars, phy);
 		}
 		return;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h
index 56c2aae4e2c8..4df45234fdc0 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h
@@ -41,6 +41,9 @@
 #define SPEED_AUTO_NEG		0
 #define SPEED_20000		20000
 
+#define I2C_DEV_ADDR_A0			0xa0
+#define I2C_DEV_ADDR_A2			0xa2
+
 #define SFP_EEPROM_PAGE_SIZE			16
 #define SFP_EEPROM_VENDOR_NAME_ADDR		0x14
 #define SFP_EEPROM_VENDOR_NAME_SIZE		16
@@ -54,6 +57,15 @@
 #define SFP_EEPROM_SERIAL_SIZE			16
 #define SFP_EEPROM_DATE_ADDR			0x54 /* ASCII YYMMDD */
 #define SFP_EEPROM_DATE_SIZE			6
+#define SFP_EEPROM_DIAG_TYPE_ADDR		0x5c
+#define SFP_EEPROM_DIAG_TYPE_SIZE		1
+#define SFP_EEPROM_DIAG_ADDR_CHANGE_REQ		(1<<2)
+#define SFP_EEPROM_SFF_8472_COMP_ADDR		0x5e
+#define SFP_EEPROM_SFF_8472_COMP_SIZE		1
+
+#define SFP_EEPROM_A2_CHECKSUM_RANGE		0x5e
+#define SFP_EEPROM_A2_CC_DMI_ADDR		0x5f
+
 #define PWR_FLT_ERR_MSG_LEN			250
 
 #define XGXS_EXT_PHY_TYPE(ext_phy_config) \
@@ -420,8 +432,8 @@ void bnx2x_sfx7101_sp_sw_reset(struct bnx2x *bp, struct bnx2x_phy *phy);
 
 /* Read "byte_cnt" bytes from address "addr" from the SFP+ EEPROM */
 int bnx2x_read_sfp_module_eeprom(struct bnx2x_phy *phy,
-				 struct link_params *params, u16 addr,
-				 u8 byte_cnt, u8 *o_buf);
+				 struct link_params *params, u8 dev_addr,
+				 u16 addr, u16 byte_cnt, u8 *o_buf);
 
 void bnx2x_hw_reset_phy(struct link_params *params);
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index c50696b396f1..b4c9dea93a53 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
@@ -75,8 +75,6 @@
 #define FW_FILE_NAME_E1H	"bnx2x/bnx2x-e1h-" FW_FILE_VERSION ".fw"
 #define FW_FILE_NAME_E2		"bnx2x/bnx2x-e2-" FW_FILE_VERSION ".fw"
 
-#define MAC_LEADING_ZERO_CNT (ALIGN(ETH_ALEN, sizeof(u32)) - ETH_ALEN)
-
 /* Time in jiffies before concluding the transmitter is hung */
 #define TX_TIMEOUT		(5*HZ)
 
@@ -2955,14 +2953,16 @@ static unsigned long bnx2x_get_common_flags(struct bnx2x *bp,
 	__set_bit(BNX2X_Q_FLG_ACTIVE, &flags);
 
 	/* tx only connections collect statistics (on the same index as the
-	 *  parent connection). The statistics are zeroed when the parent
-	 *  connection is initialized.
+	 * parent connection). The statistics are zeroed when the parent
+	 * connection is initialized.
 	 */
 
 	__set_bit(BNX2X_Q_FLG_STATS, &flags);
 	if (zero_stats)
 		__set_bit(BNX2X_Q_FLG_ZERO_STATS, &flags);
 
+	__set_bit(BNX2X_Q_FLG_PCSUM_ON_PKT, &flags);
+	__set_bit(BNX2X_Q_FLG_TUN_INC_INNER_IP_ID, &flags);
 
 #ifdef BNX2X_STOP_ON_ERROR
 	__set_bit(BNX2X_Q_FLG_TX_SEC, &flags);
@@ -3227,16 +3227,29 @@ static void bnx2x_drv_info_ether_stat(struct bnx2x *bp)
 {
 	struct eth_stats_info *ether_stat =
 		&bp->slowpath->drv_info_to_mcp.ether_stat;
+	struct bnx2x_vlan_mac_obj *mac_obj =
+		&bp->sp_objs->mac_obj;
+	int i;
 
 	strlcpy(ether_stat->version, DRV_MODULE_VERSION,
 		ETH_STAT_INFO_VERSION_LEN);
 
-	bp->sp_objs[0].mac_obj.get_n_elements(bp, &bp->sp_objs[0].mac_obj,
-					DRV_INFO_ETH_STAT_NUM_MACS_REQUIRED,
-					ether_stat->mac_local);
-
+	/* get DRV_INFO_ETH_STAT_NUM_MACS_REQUIRED macs, placing them in the
+	 * mac_local field in ether_stat struct. The base address is offset by 2
+	 * bytes to account for the field being 8 bytes but a mac address is
+	 * only 6 bytes. Likewise, the stride for the get_n_elements function is
+	 * 2 bytes to compensate from the 6 bytes of a mac to the 8 bytes
+	 * allocated by the ether_stat struct, so the macs will land in their
+	 * proper positions.
+	 */
+	for (i = 0; i < DRV_INFO_ETH_STAT_NUM_MACS_REQUIRED; i++)
+		memset(ether_stat->mac_local + i, 0,
+		       sizeof(ether_stat->mac_local[0]));
+	mac_obj->get_n_elements(bp, &bp->sp_objs[0].mac_obj,
+				DRV_INFO_ETH_STAT_NUM_MACS_REQUIRED,
+				ether_stat->mac_local + MAC_PAD, MAC_PAD,
+				ETH_ALEN);
 	ether_stat->mtu_size = bp->dev->mtu;
-
 	if (bp->dev->features & NETIF_F_RXCSUM)
 		ether_stat->feature_flags |= FEATURE_ETH_CHKSUM_OFFLOAD_MASK;
 	if (bp->dev->features & NETIF_F_TSO)
@@ -3258,8 +3271,7 @@ static void bnx2x_drv_info_fcoe_stat(struct bnx2x *bp)
 	if (!CNIC_LOADED(bp))
 		return;
 
-	memcpy(fcoe_stat->mac_local + MAC_LEADING_ZERO_CNT,
-	       bp->fip_mac, ETH_ALEN);
+	memcpy(fcoe_stat->mac_local + MAC_PAD, bp->fip_mac, ETH_ALEN);
 
 	fcoe_stat->qos_priority =
 		app->traffic_type_priority[LLFC_TRAFFIC_TYPE_FCOE];
@@ -3361,8 +3373,8 @@ static void bnx2x_drv_info_iscsi_stat(struct bnx2x *bp)
 	if (!CNIC_LOADED(bp))
 		return;
 
-	memcpy(iscsi_stat->mac_local + MAC_LEADING_ZERO_CNT,
-	       bp->cnic_eth_dev.iscsi_mac, ETH_ALEN);
+	memcpy(iscsi_stat->mac_local + MAC_PAD, bp->cnic_eth_dev.iscsi_mac,
+	       ETH_ALEN);
 
 	iscsi_stat->qos_priority =
 		app->traffic_type_priority[LLFC_TRAFFIC_TYPE_ISCSI];
@@ -6018,10 +6030,11 @@ void bnx2x_nic_init_cnic(struct bnx2x *bp)
 	mmiowb();
 }
 
-void bnx2x_nic_init(struct bnx2x *bp, u32 load_code)
+void bnx2x_pre_irq_nic_init(struct bnx2x *bp)
 {
 	int i;
 
+	/* Setup NIC internals and enable interrupts */
 	for_each_eth_queue(bp, i)
 		bnx2x_init_eth_fp(bp, i);
 
@@ -6030,17 +6043,26 @@ void bnx2x_nic_init(struct bnx2x *bp, u32 load_code)
 	bnx2x_init_rx_rings(bp);
 	bnx2x_init_tx_rings(bp);
 
-	if (IS_VF(bp))
+	if (IS_VF(bp)) {
+		bnx2x_memset_stats(bp);
 		return;
+	}
 
-	/* Initialize MOD_ABS interrupts */
-	bnx2x_init_mod_abs_int(bp, &bp->link_vars, bp->common.chip_id,
-			       bp->common.shmem_base, bp->common.shmem2_base,
-			       BP_PORT(bp));
+	if (IS_PF(bp)) {
+		/* Initialize MOD_ABS interrupts */
+		bnx2x_init_mod_abs_int(bp, &bp->link_vars, bp->common.chip_id,
+				       bp->common.shmem_base,
+				       bp->common.shmem2_base, BP_PORT(bp));
 
-	bnx2x_init_def_sb(bp);
-	bnx2x_update_dsb_idx(bp);
-	bnx2x_init_sp_ring(bp);
+		/* initialize the default status block and sp ring */
+		bnx2x_init_def_sb(bp);
+		bnx2x_update_dsb_idx(bp);
+		bnx2x_init_sp_ring(bp);
+	}
+}
+
+void bnx2x_post_irq_nic_init(struct bnx2x *bp, u32 load_code)
+{
 	bnx2x_init_eq_ring(bp);
 	bnx2x_init_internal(bp, load_code);
 	bnx2x_pf_init(bp);
@@ -6058,12 +6080,7 @@ void bnx2x_nic_init(struct bnx2x *bp, u32 load_code)
 				   AEU_INPUTS_ATTN_BITS_SPIO5);
 }
 
-/* end of nic init */
-
-/*
- * gzip service functions
- */
-
+/* gzip service functions */
 static int bnx2x_gunzip_init(struct bnx2x *bp)
 {
 	bp->gunzip_buf = dma_alloc_coherent(&bp->pdev->dev, FW_BUF_SIZE,
@@ -7757,6 +7774,8 @@ void bnx2x_free_mem(struct bnx2x *bp)
 	BNX2X_PCI_FREE(bp->eq_ring, bp->eq_mapping,
 		       BCM_PAGE_SIZE * NUM_EQ_PAGES);
 
+	BNX2X_PCI_FREE(bp->t2, bp->t2_mapping, SRC_T2_SZ);
+
 	bnx2x_iov_free_mem(bp);
 }
 
@@ -7773,7 +7792,7 @@ int bnx2x_alloc_mem_cnic(struct bnx2x *bp)
 				sizeof(struct
 				       host_hc_status_block_e1x));
 
-	if (CONFIGURE_NIC_MODE(bp))
+	if (CONFIGURE_NIC_MODE(bp) && !bp->t2)
 		/* allocate searcher T2 table, as it wan't allocated before */
 		BNX2X_PCI_ALLOC(bp->t2, &bp->t2_mapping, SRC_T2_SZ);
 
@@ -7796,7 +7815,7 @@ int bnx2x_alloc_mem(struct bnx2x *bp)
 {
 	int i, allocated, context_size;
 
-	if (!CONFIGURE_NIC_MODE(bp))
+	if (!CONFIGURE_NIC_MODE(bp) && !bp->t2)
 		/* allocate searcher T2 table */
 		BNX2X_PCI_ALLOC(bp->t2, &bp->t2_mapping, SRC_T2_SZ);
 
@@ -7917,8 +7936,6 @@ int bnx2x_del_all_macs(struct bnx2x *bp,
 
 int bnx2x_set_eth_mac(struct bnx2x *bp, bool set)
 {
-	unsigned long ramrod_flags = 0;
-
 	if (is_zero_ether_addr(bp->dev->dev_addr) &&
 	    (IS_MF_STORAGE_SD(bp) || IS_MF_FCOE_AFEX(bp))) {
 		DP(NETIF_MSG_IFUP | NETIF_MSG_IFDOWN,
@@ -7926,12 +7943,18 @@ int bnx2x_set_eth_mac(struct bnx2x *bp, bool set)
 		return 0;
 	}
 
-	DP(NETIF_MSG_IFUP, "Adding Eth MAC\n");
+	if (IS_PF(bp)) {
+		unsigned long ramrod_flags = 0;
 
-	__set_bit(RAMROD_COMP_WAIT, &ramrod_flags);
-	/* Eth MAC is set on RSS leading client (fp[0]) */
-	return bnx2x_set_mac_one(bp, bp->dev->dev_addr, &bp->sp_objs->mac_obj,
-				 set, BNX2X_ETH_MAC, &ramrod_flags);
+		DP(NETIF_MSG_IFUP, "Adding Eth MAC\n");
+		__set_bit(RAMROD_COMP_WAIT, &ramrod_flags);
+		return bnx2x_set_mac_one(bp, bp->dev->dev_addr,
+					 &bp->sp_objs->mac_obj, set,
+					 BNX2X_ETH_MAC, &ramrod_flags);
+	} else { /* vf */
+		return bnx2x_vfpf_config_mac(bp, bp->dev->dev_addr,
+					     bp->fp->index, true);
+	}
 }
 
 int bnx2x_setup_leading(struct bnx2x *bp)
@@ -9525,6 +9548,10 @@ sp_rtnl_not_reset:
 		bnx2x_vfpf_storm_rx_mode(bp);
 	}
 
+	if (test_and_clear_bit(BNX2X_SP_RTNL_HYPERVISOR_VLAN,
+			       &bp->sp_rtnl_state))
+		bnx2x_pf_set_vfs_vlan(bp);
+
 	/* work which needs rtnl lock not-taken (as it takes the lock itself and
 	 * can be called from other contexts as well)
 	 */
@@ -9532,8 +9559,10 @@ sp_rtnl_not_reset:
 
 	/* enable SR-IOV if applicable */
 	if (IS_SRIOV(bp) && test_and_clear_bit(BNX2X_SP_RTNL_ENABLE_SRIOV,
-					       &bp->sp_rtnl_state))
+					       &bp->sp_rtnl_state)) {
+		bnx2x_disable_sriov(bp);
 		bnx2x_enable_sriov(bp);
+	}
 }
 
 static void bnx2x_period_task(struct work_struct *work)
@@ -9701,6 +9730,31 @@ static struct bnx2x_prev_path_list *
 	return NULL;
 }
 
+static int bnx2x_prev_path_mark_eeh(struct bnx2x *bp)
+{
+	struct bnx2x_prev_path_list *tmp_list;
+	int rc;
+
+	rc = down_interruptible(&bnx2x_prev_sem);
+	if (rc) {
+		BNX2X_ERR("Received %d when tried to take lock\n", rc);
+		return rc;
+	}
+
+	tmp_list = bnx2x_prev_path_get_entry(bp);
+	if (tmp_list) {
+		tmp_list->aer = 1;
+		rc = 0;
+	} else {
+		BNX2X_ERR("path %d: Entry does not exist for eeh; Flow occurs before initial insmod is over ?\n",
+			  BP_PATH(bp));
+	}
+
+	up(&bnx2x_prev_sem);
+
+	return rc;
+}
+
 static bool bnx2x_prev_is_path_marked(struct bnx2x *bp)
 {
 	struct bnx2x_prev_path_list *tmp_list;
@@ -9709,14 +9763,15 @@ static bool bnx2x_prev_is_path_marked(struct bnx2x *bp)
 	if (down_trylock(&bnx2x_prev_sem))
 		return false;
 
-	list_for_each_entry(tmp_list, &bnx2x_prev_list, list) {
-		if (PCI_SLOT(bp->pdev->devfn) == tmp_list->slot &&
-		    bp->pdev->bus->number == tmp_list->bus &&
-		    BP_PATH(bp) == tmp_list->path) {
+	tmp_list = bnx2x_prev_path_get_entry(bp);
+	if (tmp_list) {
+		if (tmp_list->aer) {
+			DP(NETIF_MSG_HW, "Path %d was marked by AER\n",
+			   BP_PATH(bp));
+		} else {
 			rc = true;
 			BNX2X_DEV_INFO("Path %d was already cleaned from previous drivers\n",
 				       BP_PATH(bp));
-			break;
 		}
 	}
 
@@ -9730,6 +9785,28 @@ static int bnx2x_prev_mark_path(struct bnx2x *bp, bool after_undi)
 	struct bnx2x_prev_path_list *tmp_list;
 	int rc;
 
+	rc = down_interruptible(&bnx2x_prev_sem);
+	if (rc) {
+		BNX2X_ERR("Received %d when tried to take lock\n", rc);
+		return rc;
+	}
+
+	/* Check whether the entry for this path already exists */
+	tmp_list = bnx2x_prev_path_get_entry(bp);
+	if (tmp_list) {
+		if (!tmp_list->aer) {
+			BNX2X_ERR("Re-Marking the path.\n");
+		} else {
+			DP(NETIF_MSG_HW, "Removing AER indication from path %d\n",
+			   BP_PATH(bp));
+			tmp_list->aer = 0;
+		}
+		up(&bnx2x_prev_sem);
+		return 0;
+	}
+	up(&bnx2x_prev_sem);
+
+	/* Create an entry for this path and add it */
 	tmp_list = kmalloc(sizeof(struct bnx2x_prev_path_list), GFP_KERNEL);
 	if (!tmp_list) {
 		BNX2X_ERR("Failed to allocate 'bnx2x_prev_path_list'\n");
@@ -9739,6 +9816,7 @@ static int bnx2x_prev_mark_path(struct bnx2x *bp, bool after_undi)
 	tmp_list->bus = bp->pdev->bus->number;
 	tmp_list->slot = PCI_SLOT(bp->pdev->devfn);
 	tmp_list->path = BP_PATH(bp);
+	tmp_list->aer = 0;
 	tmp_list->undi = after_undi ? (1 << BP_PORT(bp)) : 0;
 
 	rc = down_interruptible(&bnx2x_prev_sem);
@@ -9746,8 +9824,8 @@ static int bnx2x_prev_mark_path(struct bnx2x *bp, bool after_undi)
 		BNX2X_ERR("Received %d when tried to take lock\n", rc);
 		kfree(tmp_list);
 	} else {
-		BNX2X_DEV_INFO("Marked path [%d] - finished previous unload\n",
-				BP_PATH(bp));
+		DP(NETIF_MSG_HW, "Marked path [%d] - finished previous unload\n",
+		   BP_PATH(bp));
 		list_add(&tmp_list->list, &bnx2x_prev_list);
 		up(&bnx2x_prev_sem);
 	}
@@ -9990,6 +10068,7 @@ static int bnx2x_prev_unload(struct bnx2x *bp)
 	}
 
 	do {
+		int aer = 0;
 		/* Lock MCP using an unload request */
 		fw = bnx2x_fw_command(bp, DRV_MSG_CODE_UNLOAD_REQ_WOL_DIS, 0);
 		if (!fw) {
@@ -9998,7 +10077,18 @@ static int bnx2x_prev_unload(struct bnx2x *bp)
 			break;
 		}
 
-		if (fw == FW_MSG_CODE_DRV_UNLOAD_COMMON) {
+		rc = down_interruptible(&bnx2x_prev_sem);
+		if (rc) {
+			BNX2X_ERR("Cannot check for AER; Received %d when tried to take lock\n",
+				  rc);
+		} else {
+			/* If Path is marked by EEH, ignore unload status */
+			aer = !!(bnx2x_prev_path_get_entry(bp) &&
+				 bnx2x_prev_path_get_entry(bp)->aer);
+			up(&bnx2x_prev_sem);
+		}
+
+		if (fw == FW_MSG_CODE_DRV_UNLOAD_COMMON || aer) {
 			rc = bnx2x_prev_unload_common(bp);
 			break;
 		}
@@ -10038,8 +10128,12 @@ static void bnx2x_get_common_hwinfo(struct bnx2x *bp)
 	id = ((val & 0xffff) << 16);
 	val = REG_RD(bp, MISC_REG_CHIP_REV);
 	id |= ((val & 0xf) << 12);
-	val = REG_RD(bp, MISC_REG_CHIP_METAL);
-	id |= ((val & 0xff) << 4);
+
+	/* Metal is read from PCI regs, but we can't access >=0x400 from
+	 * the configuration space (so we need to reg_rd)
+	 */
+	val = REG_RD(bp, PCICFG_OFFSET + PCI_ID_VAL3);
+	id |= (((val >> 24) & 0xf) << 4);
 	val = REG_RD(bp, MISC_REG_BOND_ID);
 	id |= (val & 0xf);
 	bp->common.chip_id = id;
@@ -10575,10 +10669,12 @@ static void bnx2x_get_port_hwinfo(struct bnx2x *bp)
 
 	bp->link_params.speed_cap_mask[0] =
 		SHMEM_RD(bp,
-			 dev_info.port_hw_config[port].speed_capability_mask);
+			 dev_info.port_hw_config[port].speed_capability_mask) &
+		PORT_HW_CFG_SPEED_CAPABILITY_D0_MASK;
 	bp->link_params.speed_cap_mask[1] =
 		SHMEM_RD(bp,
-			 dev_info.port_hw_config[port].speed_capability_mask2);
+			 dev_info.port_hw_config[port].speed_capability_mask2) &
+		PORT_HW_CFG_SPEED_CAPABILITY_D0_MASK;
 	bp->port.link_config[0] =
 		SHMEM_RD(bp, dev_info.port_feature_config[port].link_config);
 
@@ -10703,6 +10799,12 @@ static void bnx2x_get_fcoe_info(struct bnx2x *bp)
 		(max_fcoe_conn & BNX2X_MAX_FCOE_INIT_CONN_MASK) >>
 		BNX2X_MAX_FCOE_INIT_CONN_SHIFT;
 
+	/* Calculate the number of maximum allowed FCoE tasks */
+	bp->cnic_eth_dev.max_fcoe_exchanges = MAX_NUM_FCOE_TASKS_PER_ENGINE;
+	if (IS_MF(bp) || CHIP_MODE_IS_4_PORT(bp))
+		bp->cnic_eth_dev.max_fcoe_exchanges /=
+						MAX_FCOE_FUNCS_PER_ENGINE;
+
 	/* Read the WWN: */
 	if (!IS_MF(bp)) {
 		/* Port info */
@@ -10816,14 +10918,12 @@ static void bnx2x_get_cnic_mac_hwinfo(struct bnx2x *bp)
 			}
 		}
 
-		if (IS_MF_STORAGE_SD(bp))
-			/* Zero primary MAC configuration */
-			memset(bp->dev->dev_addr, 0, ETH_ALEN);
-
-		if (IS_MF_FCOE_AFEX(bp) || IS_MF_FCOE_SD(bp))
-			/* use FIP MAC as primary MAC */
+		/* If this is a storage-only interface, use SAN mac as
+		 * primary MAC. Notice that for SD this is already the case,
+		 * as the SAN mac was copied from the primary MAC.
+		 */
+		if (IS_MF_FCOE_AFEX(bp))
 			memcpy(bp->dev->dev_addr, fip_mac, ETH_ALEN);
-
 	} else {
 		val2 = SHMEM_RD(bp, dev_info.port_hw_config[port].
 				iscsi_mac_upper);
@@ -11060,6 +11160,9 @@ static int bnx2x_get_hwinfo(struct bnx2x *bp)
 				} else
 					BNX2X_DEV_INFO("illegal OV for SD\n");
 				break;
+			case SHARED_FEAT_CFG_FORCE_SF_MODE_FORCED_SF:
+				bp->mf_config[vn] = 0;
+				break;
 			default:
 				/* Unknown configuration: reset mf_config */
 				bp->mf_config[vn] = 0;
@@ -11406,26 +11509,6 @@ static int bnx2x_init_bp(struct bnx2x *bp)
  * net_device service functions
  */
 
-static int bnx2x_open_epilog(struct bnx2x *bp)
-{
-	/* Enable sriov via delayed work. This must be done via delayed work
-	 * because it causes the probe of the vf devices to be run, which invoke
-	 * register_netdevice which must have rtnl lock taken. As we are holding
-	 * the lock right now, that could only work if the probe would not take
-	 * the lock. However, as the probe of the vf may be called from other
-	 * contexts as well (such as passthrough to vm failes) it can't assume
-	 * the lock is being held for it. Using delayed work here allows the
-	 * probe code to simply take the lock (i.e. wait for it to be released
-	 * if it is being held).
-	 */
-	smp_mb__before_clear_bit();
-	set_bit(BNX2X_SP_RTNL_ENABLE_SRIOV, &bp->sp_rtnl_state);
-	smp_mb__after_clear_bit();
-	schedule_delayed_work(&bp->sp_rtnl_task, 0);
-
-	return 0;
-}
-
 /* called with rtnl_lock */
 static int bnx2x_open(struct net_device *dev)
 {
@@ -11795,6 +11878,8 @@ static const struct net_device_ops bnx2x_netdev_ops = {
 	.ndo_setup_tc		= bnx2x_setup_tc,
 #ifdef CONFIG_BNX2X_SRIOV
 	.ndo_set_vf_mac		= bnx2x_set_vf_mac,
+	.ndo_set_vf_vlan        = bnx2x_set_vf_vlan,
+	.ndo_get_vf_config	= bnx2x_get_vf_config,
 #endif
 #ifdef NETDEV_FCOE_WWNN
 	.ndo_fcoe_get_wwn	= bnx2x_fcoe_get_wwn,
@@ -11957,19 +12042,26 @@ static int bnx2x_init_dev(struct bnx2x *bp, struct pci_dev *pdev,
 	dev->watchdog_timeo = TX_TIMEOUT;
 
 	dev->netdev_ops = &bnx2x_netdev_ops;
-	bnx2x_set_ethtool_ops(dev);
+	bnx2x_set_ethtool_ops(bp, dev);
 
 	dev->priv_flags |= IFF_UNICAST_FLT;
 
 	dev->hw_features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
 		NETIF_F_TSO | NETIF_F_TSO_ECN | NETIF_F_TSO6 |
 		NETIF_F_RXCSUM | NETIF_F_LRO | NETIF_F_GRO |
-		NETIF_F_RXHASH | NETIF_F_HW_VLAN_TX;
+		NETIF_F_RXHASH | NETIF_F_HW_VLAN_CTAG_TX;
+	if (!CHIP_IS_E1x(bp)) {
+		dev->hw_features |= NETIF_F_GSO_GRE | NETIF_F_GSO_UDP_TUNNEL;
+		dev->hw_enc_features =
+			NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM | NETIF_F_SG |
+			NETIF_F_TSO | NETIF_F_TSO_ECN | NETIF_F_TSO6 |
+			NETIF_F_GSO_GRE | NETIF_F_GSO_UDP_TUNNEL;
+	}
 
 	dev->vlan_features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
 		NETIF_F_TSO | NETIF_F_TSO_ECN | NETIF_F_TSO6 | NETIF_F_HIGHDMA;
 
-	dev->features |= dev->hw_features | NETIF_F_HW_VLAN_RX;
+	dev->features |= dev->hw_features | NETIF_F_HW_VLAN_CTAG_RX;
 	if (bp->flags & USING_DAC_FLAG)
 		dev->features |= NETIF_F_HIGHDMA;
 
@@ -12451,7 +12543,7 @@ static int bnx2x_init_one(struct pci_dev *pdev,
 	 * l2 connections.
 	 */
 	if (IS_VF(bp)) {
-		bnx2x_vf_map_doorbells(bp);
+		bp->doorbells = bnx2x_vf_doorbells(bp);
 		rc = bnx2x_vf_pci_alloc(bp);
 		if (rc)
 			goto init_one_exit;
@@ -12479,13 +12571,8 @@ static int bnx2x_init_one(struct pci_dev *pdev,
 			goto init_one_exit;
 	}
 
-	/* Enable SRIOV if capability found in configuration space.
-	 * Once the generic SR-IOV framework makes it in from the
-	 * pci tree this will be revised, to allow dynamic control
-	 * over the number of VFs. Right now, change the num of vfs
-	 * param below to enable SR-IOV.
-	 */
-	rc = bnx2x_iov_init_one(bp, int_mode, 0/*num vfs*/);
+	/* Enable SRIOV if capability found in configuration space */
+	rc = bnx2x_iov_init_one(bp, int_mode, BNX2X_MAX_NUM_OF_VFS);
 	if (rc)
 		goto init_one_exit;
 
@@ -12497,16 +12584,6 @@ static int bnx2x_init_one(struct pci_dev *pdev,
 	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;
-	}
-
 	/* Set bp->num_queues for MSI-X mode*/
 	bnx2x_set_num_queues(bp);
 
@@ -12640,9 +12717,7 @@ static void bnx2x_remove_one(struct pci_dev *pdev)
 
 static int bnx2x_eeh_nic_unload(struct bnx2x *bp)
 {
-	int i;
-
-	bp->state = BNX2X_STATE_ERROR;
+	bp->state = BNX2X_STATE_CLOSING_WAIT4_HALT;
 
 	bp->rx_mode = BNX2X_RX_MODE_NONE;
 
@@ -12651,29 +12726,21 @@ static int bnx2x_eeh_nic_unload(struct bnx2x *bp)
 
 	/* Stop Tx */
 	bnx2x_tx_disable(bp);
-
-	bnx2x_netif_stop(bp, 0);
 	/* Delete all NAPI objects */
 	bnx2x_del_all_napi(bp);
 	if (CNIC_LOADED(bp))
 		bnx2x_del_all_napi_cnic(bp);
+	netdev_reset_tc(bp->dev);
 
 	del_timer_sync(&bp->timer);
+	cancel_delayed_work(&bp->sp_task);
+	cancel_delayed_work(&bp->period_task);
 
-	bnx2x_stats_handle(bp, STATS_EVENT_STOP);
-
-	/* Release IRQs */
-	bnx2x_free_irq(bp);
-
-	/* Free SKBs, SGEs, TPA pool and driver internals */
-	bnx2x_free_skbs(bp);
-
-	for_each_rx_queue(bp, i)
-		bnx2x_free_rx_sge_range(bp, bp->fp + i, NUM_RX_SGE);
-
-	bnx2x_free_mem(bp);
+	spin_lock_bh(&bp->stats_lock);
+	bp->stats_state = STATS_STATE_DISABLED;
+	spin_unlock_bh(&bp->stats_lock);
 
-	bp->state = BNX2X_STATE_CLOSED;
+	bnx2x_save_statistics(bp);
 
 	netif_carrier_off(bp->dev);
 
@@ -12709,6 +12776,8 @@ static pci_ers_result_t bnx2x_io_error_detected(struct pci_dev *pdev,
 
 	rtnl_lock();
 
+	BNX2X_ERR("IO error detected\n");
+
 	netif_device_detach(dev);
 
 	if (state == pci_channel_io_perm_failure) {
@@ -12719,6 +12788,8 @@ static pci_ers_result_t bnx2x_io_error_detected(struct pci_dev *pdev,
 	if (netif_running(dev))
 		bnx2x_eeh_nic_unload(bp);
 
+	bnx2x_prev_path_mark_eeh(bp);
+
 	pci_disable_device(pdev);
 
 	rtnl_unlock();
@@ -12737,9 +12808,10 @@ static pci_ers_result_t bnx2x_io_slot_reset(struct pci_dev *pdev)
 {
 	struct net_device *dev = pci_get_drvdata(pdev);
 	struct bnx2x *bp = netdev_priv(dev);
+	int i;
 
 	rtnl_lock();
-
+	BNX2X_ERR("IO slot reset initializing...\n");
 	if (pci_enable_device(pdev)) {
 		dev_err(&pdev->dev,
 			"Cannot re-enable PCI device after reset\n");
@@ -12749,10 +12821,47 @@ static pci_ers_result_t bnx2x_io_slot_reset(struct pci_dev *pdev)
 
 	pci_set_master(pdev);
 	pci_restore_state(pdev);
+	pci_save_state(pdev);
 
 	if (netif_running(dev))
 		bnx2x_set_power_state(bp, PCI_D0);
 
+	if (netif_running(dev)) {
+		BNX2X_ERR("IO slot reset --> driver unload\n");
+		if (IS_PF(bp) && SHMEM2_HAS(bp, drv_capabilities_flag)) {
+			u32 v;
+
+			v = SHMEM2_RD(bp,
+				      drv_capabilities_flag[BP_FW_MB_IDX(bp)]);
+			SHMEM2_WR(bp, drv_capabilities_flag[BP_FW_MB_IDX(bp)],
+				  v & ~DRV_FLAGS_CAPABILITIES_LOADED_L2);
+		}
+		bnx2x_drain_tx_queues(bp);
+		bnx2x_send_unload_req(bp, UNLOAD_RECOVERY);
+		bnx2x_netif_stop(bp, 1);
+		bnx2x_free_irq(bp);
+
+		/* Report UNLOAD_DONE to MCP */
+		bnx2x_send_unload_done(bp, true);
+
+		bp->sp_state = 0;
+		bp->port.pmf = 0;
+
+		bnx2x_prev_unload(bp);
+
+		/* We should have resetted the engine, so It's fair to
+		 * assume the FW will no longer write to the bnx2x driver.
+		 */
+		bnx2x_squeeze_objects(bp);
+		bnx2x_free_skbs(bp);
+		for_each_rx_queue(bp, i)
+			bnx2x_free_rx_sge_range(bp, bp->fp + i, NUM_RX_SGE);
+		bnx2x_free_fp_mem(bp);
+		bnx2x_free_mem(bp);
+
+		bp->state = BNX2X_STATE_CLOSED;
+	}
+
 	rtnl_unlock();
 
 	return PCI_ERS_RESULT_RECOVERED;
@@ -12779,6 +12888,9 @@ static void bnx2x_io_resume(struct pci_dev *pdev)
 
 	bnx2x_eeh_recover(bp);
 
+	bp->fw_seq = SHMEM_RD(bp, func_mb[BP_FW_MB_IDX(bp)].drv_mb_header) &
+							DRV_MSG_SEQ_NUMBER_MASK;
+
 	if (netif_running(dev))
 		bnx2x_nic_load(bp, LOAD_NORMAL);
 
@@ -12801,6 +12913,9 @@ static struct pci_driver bnx2x_pci_driver = {
 	.suspend     = bnx2x_suspend,
 	.resume      = bnx2x_resume,
 	.err_handler = &bnx2x_err_handler,
+#ifdef CONFIG_BNX2X_SRIOV
+	.sriov_configure = bnx2x_sriov_configure,
+#endif
 };
 
 static int __init bnx2x_init(void)
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
index 791eb2d53011..d22bc40091ec 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
@@ -1491,10 +1491,6 @@
 /* [R 4] This field indicates the type of the device. '0' - 2 Ports; '1' - 1
    Port. */
 #define MISC_REG_BOND_ID					 0xa400
-/* [R 8] These bits indicate the metal revision of the chip. This value
-   starts at 0x00 for each all-layer tape-out and increments by one for each
-   tape-out. */
-#define MISC_REG_CHIP_METAL					 0xa404
 /* [R 16] These bits indicate the part number for the chip. */
 #define MISC_REG_CHIP_NUM					 0xa408
 /* [R 4] These bits indicate the base revision of the chip. This value
@@ -6331,6 +6327,8 @@
 #define PCI_PM_DATA_B					0x414
 #define PCI_ID_VAL1					0x434
 #define PCI_ID_VAL2					0x438
+#define PCI_ID_VAL3					0x43c
+
 #define GRC_CONFIG_REG_PF_INIT_VF		0x624
 #define GRC_CR_PF_INIT_VF_PF_FIRST_VF_NUM_MASK	0xf
 /* First VF_NUM for PF is encoded in this register.
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
index 7306416bc90d..32a9609cc98b 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
@@ -30,8 +30,6 @@
 
 #define BNX2X_MAX_EMUL_MULTI		16
 
-#define MAC_LEADING_ZERO_CNT (ALIGN(ETH_ALEN, sizeof(u32)) - ETH_ALEN)
-
 /**** Exe Queue interfaces ****/
 
 /**
@@ -444,30 +442,21 @@ static bool bnx2x_put_credit_vlan_mac(struct bnx2x_vlan_mac_obj *o)
 }
 
 static int bnx2x_get_n_elements(struct bnx2x *bp, struct bnx2x_vlan_mac_obj *o,
-				int n, u8 *buf)
+				int n, u8 *base, u8 stride, u8 size)
 {
 	struct bnx2x_vlan_mac_registry_elem *pos;
-	u8 *next = buf;
+	u8 *next = base;
 	int counter = 0;
 
 	/* traverse list */
 	list_for_each_entry(pos, &o->head, link) {
 		if (counter < n) {
-			/* place leading zeroes in buffer */
-			memset(next, 0, MAC_LEADING_ZERO_CNT);
-
-			/* place mac after leading zeroes*/
-			memcpy(next + MAC_LEADING_ZERO_CNT, pos->u.mac.mac,
-			       ETH_ALEN);
-
-			/* calculate address of next element and
-			 * advance counter
-			 */
+			memcpy(next, &pos->u, size);
 			counter++;
-			next = buf + counter * ALIGN(ETH_ALEN, sizeof(u32));
+			DP(BNX2X_MSG_SP, "copied element number %d to address %p element was:\n",
+			   counter, next);
+			next += stride + size;
 
-			DP(BNX2X_MSG_SP, "copied element number %d to address %p element was %pM\n",
-			   counter, next, pos->u.mac.mac);
 		}
 	}
 	return counter * ETH_ALEN;
@@ -487,7 +476,8 @@ static int bnx2x_check_mac_add(struct bnx2x *bp,
 
 	/* Check if a requested MAC already exists */
 	list_for_each_entry(pos, &o->head, link)
-		if (!memcmp(data->mac.mac, pos->u.mac.mac, ETH_ALEN))
+		if (!memcmp(data->mac.mac, pos->u.mac.mac, ETH_ALEN) &&
+		    (data->mac.is_inner_mac == pos->u.mac.is_inner_mac))
 			return -EEXIST;
 
 	return 0;
@@ -520,7 +510,9 @@ static int bnx2x_check_vlan_mac_add(struct bnx2x *bp,
 	list_for_each_entry(pos, &o->head, link)
 		if ((data->vlan_mac.vlan == pos->u.vlan_mac.vlan) &&
 		    (!memcmp(data->vlan_mac.mac, pos->u.vlan_mac.mac,
-			     ETH_ALEN)))
+				  ETH_ALEN)) &&
+		    (data->vlan_mac.is_inner_mac ==
+		     pos->u.vlan_mac.is_inner_mac))
 			return -EEXIST;
 
 	return 0;
@@ -538,7 +530,8 @@ static struct bnx2x_vlan_mac_registry_elem *
 	DP(BNX2X_MSG_SP, "Checking MAC %pM for DEL command\n", data->mac.mac);
 
 	list_for_each_entry(pos, &o->head, link)
-		if (!memcmp(data->mac.mac, pos->u.mac.mac, ETH_ALEN))
+		if ((!memcmp(data->mac.mac, pos->u.mac.mac, ETH_ALEN)) &&
+		    (data->mac.is_inner_mac == pos->u.mac.is_inner_mac))
 			return pos;
 
 	return NULL;
@@ -573,7 +566,9 @@ static struct bnx2x_vlan_mac_registry_elem *
 	list_for_each_entry(pos, &o->head, link)
 		if ((data->vlan_mac.vlan == pos->u.vlan_mac.vlan) &&
 		    (!memcmp(data->vlan_mac.mac, pos->u.vlan_mac.mac,
-			     ETH_ALEN)))
+			     ETH_ALEN)) &&
+		    (data->vlan_mac.is_inner_mac ==
+		     pos->u.vlan_mac.is_inner_mac))
 			return pos;
 
 	return NULL;
@@ -770,6 +765,8 @@ static void bnx2x_set_one_mac_e2(struct bnx2x *bp,
 	bnx2x_set_fw_mac_addr(&rule_entry->mac.mac_msb,
 			      &rule_entry->mac.mac_mid,
 			      &rule_entry->mac.mac_lsb, mac);
+	rule_entry->mac.inner_mac =
+		cpu_to_le16(elem->cmd_data.vlan_mac.u.mac.is_inner_mac);
 
 	/* MOVE: Add a rule that will add this MAC to the target Queue */
 	if (cmd == BNX2X_VLAN_MAC_MOVE) {
@@ -786,6 +783,9 @@ static void bnx2x_set_one_mac_e2(struct bnx2x *bp,
 		bnx2x_set_fw_mac_addr(&rule_entry->mac.mac_msb,
 				      &rule_entry->mac.mac_mid,
 				      &rule_entry->mac.mac_lsb, mac);
+		rule_entry->mac.inner_mac =
+			cpu_to_le16(elem->cmd_data.vlan_mac.
+						u.mac.is_inner_mac);
 	}
 
 	/* Set the ramrod data header */
@@ -974,7 +974,8 @@ static void bnx2x_set_one_vlan_mac_e2(struct bnx2x *bp,
 	bnx2x_set_fw_mac_addr(&rule_entry->pair.mac_msb,
 			      &rule_entry->pair.mac_mid,
 			      &rule_entry->pair.mac_lsb, mac);
-
+	rule_entry->pair.inner_mac =
+		cpu_to_le16(elem->cmd_data.vlan_mac.u.vlan_mac.is_inner_mac);
 	/* MOVE: Add a rule that will add this MAC to the target Queue */
 	if (cmd == BNX2X_VLAN_MAC_MOVE) {
 		rule_entry++;
@@ -991,6 +992,9 @@ static void bnx2x_set_one_vlan_mac_e2(struct bnx2x *bp,
 		bnx2x_set_fw_mac_addr(&rule_entry->pair.mac_msb,
 				      &rule_entry->pair.mac_mid,
 				      &rule_entry->pair.mac_lsb, mac);
+		rule_entry->pair.inner_mac =
+			cpu_to_le16(elem->cmd_data.vlan_mac.u.
+						vlan_mac.is_inner_mac);
 	}
 
 	/* Set the ramrod data header */
@@ -1854,6 +1858,7 @@ static int bnx2x_vlan_mac_del_all(struct bnx2x *bp,
 				return rc;
 			}
 			list_del(&exeq_pos->link);
+			bnx2x_exe_queue_free_elem(bp, exeq_pos);
 		}
 	}
 
@@ -2012,6 +2017,7 @@ void bnx2x_init_vlan_obj(struct bnx2x *bp,
 		vlan_obj->check_move        = bnx2x_check_move;
 		vlan_obj->ramrod_cmd        =
 			RAMROD_CMD_ID_ETH_CLASSIFICATION_RULES;
+		vlan_obj->get_n_elements    = bnx2x_get_n_elements;
 
 		/* Exe Queue */
 		bnx2x_exe_queue_init(bp,
@@ -4426,6 +4432,12 @@ static void bnx2x_q_fill_init_tx_data(struct bnx2x_queue_sp_obj *o,
 	tx_data->force_default_pri_flg =
 		test_bit(BNX2X_Q_FLG_FORCE_DEFAULT_PRI, flags);
 
+	tx_data->tunnel_lso_inc_ip_id =
+		test_bit(BNX2X_Q_FLG_TUN_INC_INNER_IP_ID, flags);
+	tx_data->tunnel_non_lso_pcsum_location =
+		test_bit(BNX2X_Q_FLG_PCSUM_ON_PKT, flags) ? PCSUM_ON_PKT :
+								  PCSUM_ON_BD;
+
 	tx_data->tx_status_block_id = params->fw_sb_id;
 	tx_data->tx_sb_index_number = params->sb_cq_index;
 	tx_data->tss_leading_client_id = params->tss_leading_cl_id;
@@ -5669,17 +5681,18 @@ static inline int bnx2x_func_send_start(struct bnx2x *bp,
 	memset(rdata, 0, sizeof(*rdata));
 
 	/* Fill the ramrod data with provided parameters */
-	rdata->function_mode    = (u8)start_params->mf_mode;
-	rdata->sd_vlan_tag      = cpu_to_le16(start_params->sd_vlan_tag);
-	rdata->path_id          = BP_PATH(bp);
-	rdata->network_cos_mode = start_params->network_cos_mode;
-
-	/*
-	 *  No need for an explicit memory barrier here as long we would
-	 *  need to ensure the ordering of writing to the SPQ element
-	 *  and updating of the SPQ producer which involves a memory
-	 *  read and we will have to put a full memory barrier there
-	 *  (inside bnx2x_sp_post()).
+	rdata->function_mode	= (u8)start_params->mf_mode;
+	rdata->sd_vlan_tag	= cpu_to_le16(start_params->sd_vlan_tag);
+	rdata->path_id		= BP_PATH(bp);
+	rdata->network_cos_mode	= start_params->network_cos_mode;
+	rdata->gre_tunnel_mode	= start_params->gre_tunnel_mode;
+	rdata->gre_tunnel_rss	= start_params->gre_tunnel_rss;
+
+	/* No need for an explicit memory barrier here as long we would
+	 * need to ensure the ordering of writing to the SPQ element
+	 * and updating of the SPQ producer which involves a memory
+	 * read and we will have to put a full memory barrier there
+	 * (inside bnx2x_sp_post()).
 	 */
 
 	return bnx2x_sp_post(bp, RAMROD_CMD_ID_COMMON_FUNCTION_START, 0,
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
index ff907609b9fc..43c00bc84a08 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
@@ -100,6 +100,7 @@ struct bnx2x_raw_obj {
 /************************* VLAN-MAC commands related parameters ***************/
 struct bnx2x_mac_ramrod_data {
 	u8 mac[ETH_ALEN];
+	u8 is_inner_mac;
 };
 
 struct bnx2x_vlan_ramrod_data {
@@ -108,6 +109,7 @@ struct bnx2x_vlan_ramrod_data {
 
 struct bnx2x_vlan_mac_ramrod_data {
 	u8 mac[ETH_ALEN];
+	u8 is_inner_mac;
 	u16 vlan;
 };
 
@@ -313,8 +315,9 @@ struct bnx2x_vlan_mac_obj {
 	 *
 	 * @return number of copied bytes
 	 */
-	int (*get_n_elements)(struct bnx2x *bp, struct bnx2x_vlan_mac_obj *o,
-			      int n, u8 *buf);
+	int (*get_n_elements)(struct bnx2x *bp,
+			      struct bnx2x_vlan_mac_obj *o, int n, u8 *base,
+			      u8 stride, u8 size);
 
 	/**
 	 * Checks if ADD-ramrod with the given params may be performed.
@@ -824,7 +827,9 @@ enum {
 	BNX2X_Q_FLG_TX_SEC,
 	BNX2X_Q_FLG_ANTI_SPOOF,
 	BNX2X_Q_FLG_SILENT_VLAN_REM,
-	BNX2X_Q_FLG_FORCE_DEFAULT_PRI
+	BNX2X_Q_FLG_FORCE_DEFAULT_PRI,
+	BNX2X_Q_FLG_PCSUM_ON_PKT,
+	BNX2X_Q_FLG_TUN_INC_INNER_IP_ID
 };
 
 /* Queue type options: queue type may be a compination of below. */
@@ -842,6 +847,7 @@ enum bnx2x_q_type {
 #define BNX2X_MULTI_TX_COS_E3B0			3
 #define BNX2X_MULTI_TX_COS			3 /* Maximum possible */
 
+#define MAC_PAD (ALIGN(ETH_ALEN, sizeof(u32)) - ETH_ALEN)
 
 struct bnx2x_queue_init_params {
 	struct {
@@ -1118,6 +1124,15 @@ struct bnx2x_func_start_params {
 
 	/* Function cos mode */
 	u8 network_cos_mode;
+
+	/* NVGRE classification enablement */
+	u8 nvgre_clss_en;
+
+	/* NO_GRE_TUNNEL/NVGRE_TUNNEL/L2GRE_TUNNEL/IPGRE_TUNNEL */
+	u8 gre_tunnel_mode;
+
+	/* GRE_OUTER_HEADERS_RSS/GRE_INNER_HEADERS_RSS/NVGRE_KEY_ENTROPY_RSS */
+	u8 gre_tunnel_rss;
 };
 
 struct bnx2x_func_switch_update_params {
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
index 6adfa2093581..2ce7c7471367 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
@@ -20,7 +20,9 @@
 #include "bnx2x.h"
 #include "bnx2x_init.h"
 #include "bnx2x_cmn.h"
+#include "bnx2x_sp.h"
 #include <linux/crc32.h>
+#include <linux/if_vlan.h>
 
 /* General service functions */
 static void storm_memset_vf_to_pf(struct bnx2x *bp, u16 abs_fid,
@@ -555,8 +557,7 @@ static int bnx2x_vfop_config_list(struct bnx2x *bp,
 		rc = bnx2x_config_vlan_mac(bp, vlan_mac);
 		if (rc >= 0) {
 			cnt += pos->add ? 1 : -1;
-			list_del(&pos->link);
-			list_add(&pos->link, &rollback_list);
+			list_move(&pos->link, &rollback_list);
 			rc = 0;
 		} else if (rc == -EEXIST) {
 			rc = 0;
@@ -958,6 +959,12 @@ op_err:
 	BNX2X_ERR("QSETUP[%d:%d] error: rc %d\n", vf->abs_vfid, qid, vfop->rc);
 op_done:
 	case BNX2X_VFOP_QSETUP_DONE:
+		vf->cfg_flags |= VF_CFG_VLAN;
+		smp_mb__before_clear_bit();
+		set_bit(BNX2X_SP_RTNL_HYPERVISOR_VLAN,
+			&bp->sp_rtnl_state);
+		smp_mb__after_clear_bit();
+		schedule_delayed_work(&bp->sp_rtnl_task, 0);
 		bnx2x_vfop_end(bp, vf, vfop);
 		return;
 	default:
@@ -1459,7 +1466,6 @@ static u8 bnx2x_vf_is_pcie_pending(struct bnx2x *bp, u8 abs_vfid)
 		return bnx2x_is_pcie_pending(dev);
 
 unknown_dev:
-	BNX2X_ERR("Unknown device\n");
 	return false;
 }
 
@@ -1926,20 +1932,22 @@ int bnx2x_iov_init_one(struct bnx2x *bp, int int_mode_param,
 
 	/* SRIOV can be enabled only with MSIX */
 	if (int_mode_param == BNX2X_INT_MODE_MSI ||
-	    int_mode_param == BNX2X_INT_MODE_INTX)
+	    int_mode_param == BNX2X_INT_MODE_INTX) {
 		BNX2X_ERR("Forced MSI/INTx mode is incompatible with SRIOV\n");
+		return 0;
+	}
 
 	err = -EIO;
 	/* verify ari is enabled */
 	if (!bnx2x_ari_enabled(bp->pdev)) {
-		BNX2X_ERR("ARI not supported, SRIOV can not be enabled\n");
-		return err;
+		BNX2X_ERR("ARI not supported (check pci bridge ARI forwarding), SRIOV can not be enabled\n");
+		return 0;
 	}
 
 	/* verify igu is in normal mode */
 	if (CHIP_INT_MODE_IS_BC(bp)) {
 		BNX2X_ERR("IGU not normal mode,  SRIOV can not be enabled\n");
-		return err;
+		return 0;
 	}
 
 	/* allocate the vfs database */
@@ -1964,8 +1972,10 @@ int bnx2x_iov_init_one(struct bnx2x *bp, int int_mode_param,
 	if (iov->total == 0)
 		goto failed;
 
-	/* calculate the actual number of VFs */
-	iov->nr_virtfn = min_t(u16, iov->total, (u16)num_vfs_param);
+	iov->nr_virtfn = min_t(u16, iov->total, num_vfs_param);
+
+	DP(BNX2X_MSG_IOV, "num_vfs_param was %d, nr_virtfn was %d\n",
+	   num_vfs_param, iov->nr_virtfn);
 
 	/* allocate the vf array */
 	bp->vfdb->vfs = kzalloc(sizeof(struct bnx2x_virtf) *
@@ -2378,8 +2388,8 @@ int bnx2x_iov_eq_sp_event(struct bnx2x *bp, union event_ring_elem *elem)
 		goto get_vf;
 	case EVENT_RING_OPCODE_MALICIOUS_VF:
 		abs_vfid = elem->message.data.malicious_vf_event.vf_id;
-		DP(BNX2X_MSG_IOV, "Got VF MALICIOUS notification abs_vfid=%d\n",
-		   abs_vfid);
+		DP(BNX2X_MSG_IOV, "Got VF MALICIOUS notification abs_vfid=%d err_id=0x%x\n",
+		   abs_vfid, elem->message.data.malicious_vf_event.err_id);
 		goto get_vf;
 	default:
 		return 1;
@@ -2436,8 +2446,8 @@ get_vf:
 		/* Do nothing for now */
 		break;
 	case EVENT_RING_OPCODE_MALICIOUS_VF:
-		DP(BNX2X_MSG_IOV, "got VF [%d] MALICIOUS notification\n",
-		   vf->abs_vfid);
+		DP(BNX2X_MSG_IOV, "Got VF MALICIOUS notification abs_vfid=%d error id %x\n",
+		   abs_vfid, elem->message.data.malicious_vf_event.err_id);
 		/* Do nothing for now */
 		break;
 	}
@@ -3012,21 +3022,138 @@ void bnx2x_unlock_vf_pf_channel(struct bnx2x *bp, struct bnx2x_virtf *vf,
 	vf->op_current = CHANNEL_TLV_NONE;
 }
 
-void bnx2x_enable_sriov(struct bnx2x *bp)
+int bnx2x_sriov_configure(struct pci_dev *dev, int num_vfs_param)
 {
-	int rc = 0;
 
-	/* disbale sriov in case it is still enabled */
+	struct bnx2x *bp = netdev_priv(pci_get_drvdata(dev));
+
+	DP(BNX2X_MSG_IOV, "bnx2x_sriov_configure called with %d, BNX2X_NR_VIRTFN(bp) was %d\n",
+	   num_vfs_param, BNX2X_NR_VIRTFN(bp));
+
+	/* HW channel is only operational when PF is up */
+	if (bp->state != BNX2X_STATE_OPEN) {
+		BNX2X_ERR("VF num configurtion via sysfs not supported while PF is down");
+		return -EINVAL;
+	}
+
+	/* we are always bound by the total_vfs in the configuration space */
+	if (num_vfs_param > BNX2X_NR_VIRTFN(bp)) {
+		BNX2X_ERR("truncating requested number of VFs (%d) down to maximum allowed (%d)\n",
+			  num_vfs_param, BNX2X_NR_VIRTFN(bp));
+		num_vfs_param = BNX2X_NR_VIRTFN(bp);
+	}
+
+	bp->requested_nr_virtfn = num_vfs_param;
+	if (num_vfs_param == 0) {
+		pci_disable_sriov(dev);
+		return 0;
+	} else {
+		return bnx2x_enable_sriov(bp);
+	}
+}
+
+int bnx2x_enable_sriov(struct bnx2x *bp)
+{
+	int rc = 0, req_vfs = bp->requested_nr_virtfn;
+
+	rc = pci_enable_sriov(bp->pdev, req_vfs);
+	if (rc) {
+		BNX2X_ERR("pci_enable_sriov failed with %d\n", rc);
+		return rc;
+	}
+	DP(BNX2X_MSG_IOV, "sriov enabled (%d vfs)\n", req_vfs);
+	return req_vfs;
+}
+
+void bnx2x_pf_set_vfs_vlan(struct bnx2x *bp)
+{
+	int vfidx;
+	struct pf_vf_bulletin_content *bulletin;
+
+	DP(BNX2X_MSG_IOV, "configuring vlan for VFs from sp-task\n");
+	for_each_vf(bp, vfidx) {
+	bulletin = BP_VF_BULLETIN(bp, vfidx);
+		if (BP_VF(bp, vfidx)->cfg_flags & VF_CFG_VLAN)
+			bnx2x_set_vf_vlan(bp->dev, vfidx, bulletin->vlan, 0);
+	}
+}
+
+void bnx2x_disable_sriov(struct bnx2x *bp)
+{
 	pci_disable_sriov(bp->pdev);
-	DP(BNX2X_MSG_IOV, "sriov disabled\n");
+}
+
+static int bnx2x_vf_ndo_sanity(struct bnx2x *bp, int vfidx,
+			       struct bnx2x_virtf *vf)
+{
+	if (!IS_SRIOV(bp)) {
+		BNX2X_ERR("vf ndo called though sriov is disabled\n");
+		return -EINVAL;
+	}
+
+	if (vfidx >= BNX2X_NR_VIRTFN(bp)) {
+		BNX2X_ERR("vf ndo called for uninitialized VF. vfidx was %d BNX2X_NR_VIRTFN was %d\n",
+			  vfidx, BNX2X_NR_VIRTFN(bp));
+		return -EINVAL;
+	}
+
+	if (!vf) {
+		BNX2X_ERR("vf ndo called but vf was null. vfidx was %d\n",
+			  vfidx);
+		return -EINVAL;
+	}
 
-	/* enable sriov */
-	DP(BNX2X_MSG_IOV, "vf num (%d)\n", (bp->vfdb->sriov.nr_virtfn));
-	rc = pci_enable_sriov(bp->pdev, (bp->vfdb->sriov.nr_virtfn));
+	return 0;
+}
+
+int bnx2x_get_vf_config(struct net_device *dev, int vfidx,
+			struct ifla_vf_info *ivi)
+{
+	struct bnx2x *bp = netdev_priv(dev);
+	struct bnx2x_virtf *vf = BP_VF(bp, vfidx);
+	struct bnx2x_vlan_mac_obj *mac_obj = &bnx2x_vfq(vf, 0, mac_obj);
+	struct bnx2x_vlan_mac_obj *vlan_obj = &bnx2x_vfq(vf, 0, vlan_obj);
+	struct pf_vf_bulletin_content *bulletin = BP_VF_BULLETIN(bp, vfidx);
+	int rc;
+
+	/* sanity */
+	rc = bnx2x_vf_ndo_sanity(bp, vfidx, vf);
 	if (rc)
-		BNX2X_ERR("pci_enable_sriov failed with %d\n", rc);
-	else
-		DP(BNX2X_MSG_IOV, "sriov enabled\n");
+		return rc;
+	if (!mac_obj || !vlan_obj || !bulletin) {
+		BNX2X_ERR("VF partially initialized\n");
+		return -EINVAL;
+	}
+
+	ivi->vf = vfidx;
+	ivi->qos = 0;
+	ivi->tx_rate = 10000; /* always 10G. TBA take from link struct */
+	ivi->spoofchk = 1; /*always enabled */
+	if (vf->state == VF_ENABLED) {
+		/* mac and vlan are in vlan_mac objects */
+		mac_obj->get_n_elements(bp, mac_obj, 1, (u8 *)&ivi->mac,
+					0, ETH_ALEN);
+		vlan_obj->get_n_elements(bp, vlan_obj, 1, (u8 *)&ivi->vlan,
+					 0, VLAN_HLEN);
+	} else {
+		/* mac */
+		if (bulletin->valid_bitmap & (1 << MAC_ADDR_VALID))
+			/* mac configured by ndo so its in bulletin board */
+			memcpy(&ivi->mac, bulletin->mac, ETH_ALEN);
+		else
+			/* funtion has not been loaded yet. Show mac as 0s */
+			memset(&ivi->mac, 0, ETH_ALEN);
+
+		/* vlan */
+		if (bulletin->valid_bitmap & (1 << VLAN_VALID))
+			/* vlan configured by ndo so its in bulletin board */
+			memcpy(&ivi->vlan, &bulletin->vlan, VLAN_HLEN);
+		else
+			/* funtion has not been loaded yet. Show vlans as 0s */
+			memset(&ivi->vlan, 0, VLAN_HLEN);
+	}
+
+	return 0;
 }
 
 /* New mac for VF. Consider these cases:
@@ -3044,23 +3171,19 @@ void bnx2x_enable_sriov(struct bnx2x *bp)
  * VF to configure any mac for itself except for this mac. In case of a race
  * where the VF fails to see the new post on its bulletin board before sending a
  * mac configuration request, the PF will simply fail the request and VF can try
- * again after consulting its bulletin board
+ * again after consulting its bulletin board.
  */
-int bnx2x_set_vf_mac(struct net_device *dev, int queue, u8 *mac)
+int bnx2x_set_vf_mac(struct net_device *dev, int vfidx, u8 *mac)
 {
 	struct bnx2x *bp = netdev_priv(dev);
-	int rc, q_logical_state, vfidx = queue;
+	int rc, q_logical_state;
 	struct bnx2x_virtf *vf = BP_VF(bp, vfidx);
 	struct pf_vf_bulletin_content *bulletin = BP_VF_BULLETIN(bp, vfidx);
 
-	/* if SRIOV is disabled there is nothing to do (and somewhere, someone
-	 * has erred).
-	 */
-	if (!IS_SRIOV(bp)) {
-		BNX2X_ERR("bnx2x_set_vf_mac called though sriov is disabled\n");
-		return -EINVAL;
-	}
-
+	/* sanity */
+	rc = bnx2x_vf_ndo_sanity(bp, vfidx, vf);
+	if (rc)
+		return rc;
 	if (!is_valid_ether_addr(mac)) {
 		BNX2X_ERR("mac address invalid\n");
 		return -EINVAL;
@@ -3085,7 +3208,7 @@ int bnx2x_set_vf_mac(struct net_device *dev, int queue, u8 *mac)
 	if (vf->state == VF_ENABLED &&
 	    q_logical_state == BNX2X_Q_LOGICAL_STATE_ACTIVE) {
 		/* configure the mac in device on this vf's queue */
-		unsigned long flags = 0;
+		unsigned long ramrod_flags = 0;
 		struct bnx2x_vlan_mac_obj *mac_obj = &bnx2x_vfq(vf, 0, mac_obj);
 
 		/* must lock vfpf channel to protect against vf flows */
@@ -3106,14 +3229,133 @@ int bnx2x_set_vf_mac(struct net_device *dev, int queue, u8 *mac)
 		}
 
 		/* configure the new mac to device */
-		__set_bit(RAMROD_COMP_WAIT, &flags);
+		__set_bit(RAMROD_COMP_WAIT, &ramrod_flags);
 		bnx2x_set_mac_one(bp, (u8 *)&bulletin->mac, mac_obj, true,
-				  BNX2X_ETH_MAC, &flags);
+				  BNX2X_ETH_MAC, &ramrod_flags);
 
 		bnx2x_unlock_vf_pf_channel(bp, vf, CHANNEL_TLV_PF_SET_MAC);
 	}
 
-	return rc;
+	return 0;
+}
+
+int bnx2x_set_vf_vlan(struct net_device *dev, int vfidx, u16 vlan, u8 qos)
+{
+	struct bnx2x *bp = netdev_priv(dev);
+	int rc, q_logical_state;
+	struct bnx2x_virtf *vf = BP_VF(bp, vfidx);
+	struct pf_vf_bulletin_content *bulletin = BP_VF_BULLETIN(bp, vfidx);
+
+	/* sanity */
+	rc = bnx2x_vf_ndo_sanity(bp, vfidx, vf);
+	if (rc)
+		return rc;
+
+	if (vlan > 4095) {
+		BNX2X_ERR("illegal vlan value %d\n", vlan);
+		return -EINVAL;
+	}
+
+	DP(BNX2X_MSG_IOV, "configuring VF %d with VLAN %d qos %d\n",
+	   vfidx, vlan, 0);
+
+	/* update PF's copy of the VF's bulletin. No point in posting the vlan
+	 * to the VF since it doesn't have anything to do with it. But it useful
+	 * to store it here in case the VF is not up yet and we can only
+	 * configure the vlan later when it does.
+	 */
+	bulletin->valid_bitmap |= 1 << VLAN_VALID;
+	bulletin->vlan = vlan;
+
+	/* is vf initialized and queue set up? */
+	q_logical_state =
+		bnx2x_get_q_logical_state(bp, &bnx2x_vfq(vf, 0, sp_obj));
+	if (vf->state == VF_ENABLED &&
+	    q_logical_state == BNX2X_Q_LOGICAL_STATE_ACTIVE) {
+		/* configure the vlan in device on this vf's queue */
+		unsigned long ramrod_flags = 0;
+		unsigned long vlan_mac_flags = 0;
+		struct bnx2x_vlan_mac_obj *vlan_obj =
+			&bnx2x_vfq(vf, 0, vlan_obj);
+		struct bnx2x_vlan_mac_ramrod_params ramrod_param;
+		struct bnx2x_queue_state_params q_params = {NULL};
+		struct bnx2x_queue_update_params *update_params;
+
+		memset(&ramrod_param, 0, sizeof(ramrod_param));
+
+		/* must lock vfpf channel to protect against vf flows */
+		bnx2x_lock_vf_pf_channel(bp, vf, CHANNEL_TLV_PF_SET_VLAN);
+
+		/* remove existing vlans */
+		__set_bit(RAMROD_COMP_WAIT, &ramrod_flags);
+		rc = vlan_obj->delete_all(bp, vlan_obj, &vlan_mac_flags,
+					  &ramrod_flags);
+		if (rc) {
+			BNX2X_ERR("failed to delete vlans\n");
+			return -EINVAL;
+		}
+
+		/* send queue update ramrod to configure default vlan and silent
+		 * vlan removal
+		 */
+		__set_bit(RAMROD_COMP_WAIT, &q_params.ramrod_flags);
+		q_params.cmd = BNX2X_Q_CMD_UPDATE;
+		q_params.q_obj = &bnx2x_vfq(vf, 0, sp_obj);
+		update_params = &q_params.params.update;
+		__set_bit(BNX2X_Q_UPDATE_DEF_VLAN_EN_CHNG,
+			  &update_params->update_flags);
+		__set_bit(BNX2X_Q_UPDATE_SILENT_VLAN_REM_CHNG,
+			  &update_params->update_flags);
+
+		if (vlan == 0) {
+			/* if vlan is 0 then we want to leave the VF traffic
+			 * untagged, and leave the incoming traffic untouched
+			 * (i.e. do not remove any vlan tags).
+			 */
+			__clear_bit(BNX2X_Q_UPDATE_DEF_VLAN_EN,
+				    &update_params->update_flags);
+			__clear_bit(BNX2X_Q_UPDATE_SILENT_VLAN_REM,
+				    &update_params->update_flags);
+		} else {
+			/* configure the new vlan to device */
+			__set_bit(RAMROD_COMP_WAIT, &ramrod_flags);
+			ramrod_param.vlan_mac_obj = vlan_obj;
+			ramrod_param.ramrod_flags = ramrod_flags;
+			ramrod_param.user_req.u.vlan.vlan = vlan;
+			ramrod_param.user_req.cmd = BNX2X_VLAN_MAC_ADD;
+			rc = bnx2x_config_vlan_mac(bp, &ramrod_param);
+			if (rc) {
+				BNX2X_ERR("failed to configure vlan\n");
+				return -EINVAL;
+			}
+
+			/* configure default vlan to vf queue and set silent
+			 * vlan removal (the vf remains unaware of this vlan).
+			 */
+			update_params = &q_params.params.update;
+			__set_bit(BNX2X_Q_UPDATE_DEF_VLAN_EN,
+				  &update_params->update_flags);
+			__set_bit(BNX2X_Q_UPDATE_SILENT_VLAN_REM,
+				  &update_params->update_flags);
+			update_params->def_vlan = vlan;
+		}
+
+		/* Update the Queue state */
+		rc = bnx2x_queue_state_change(bp, &q_params);
+		if (rc) {
+			BNX2X_ERR("Failed to configure default VLAN\n");
+			return rc;
+		}
+
+		/* clear the flag indicating that this VF needs its vlan
+		 * (will only be set if the HV configured th Vlan before vf was
+		 * and we were called because the VF came up later
+		 */
+		vf->cfg_flags &= ~VF_CFG_VLAN;
+
+		bnx2x_unlock_vf_pf_channel(bp, vf, CHANNEL_TLV_PF_SET_VLAN);
+	}
+	return 0;
 }
 
 /* crc is the first field in the bulletin board. compute the crc over the
@@ -3165,20 +3407,26 @@ enum sample_bulletin_result bnx2x_sample_bulletin(struct bnx2x *bp)
 		memcpy(bp->dev->dev_addr, bulletin.mac, ETH_ALEN);
 	}
 
+	/* the vlan in bulletin board is valid and is new */
+	if (bulletin.valid_bitmap & 1 << VLAN_VALID)
+		memcpy(&bulletin.vlan, &bp->old_bulletin.vlan, VLAN_HLEN);
+
 	/* copy new bulletin board to bp */
 	bp->old_bulletin = bulletin;
 
 	return PFVF_BULLETIN_UPDATED;
 }
 
-void bnx2x_vf_map_doorbells(struct bnx2x *bp)
+void __iomem *bnx2x_vf_doorbells(struct bnx2x *bp)
 {
 	/* vf doorbells are embedded within the regview */
-	bp->doorbells = bp->regview + PXP_VF_ADDR_DB_START;
+	return bp->regview + PXP_VF_ADDR_DB_START;
 }
 
 int bnx2x_vf_pci_alloc(struct bnx2x *bp)
 {
+	mutex_init(&bp->vf2pf_mutex);
+
 	/* allocate vf2pf mailbox for vf to pf channel */
 	BNX2X_PCI_ALLOC(bp->vf2pf_mbox, &bp->vf2pf_mbox_mapping,
 			sizeof(struct bnx2x_vf_mbx_msg));
@@ -3196,3 +3444,26 @@ alloc_mem_err:
 		       sizeof(union pf_vf_bulletin));
 	return -ENOMEM;
 }
+
+int bnx2x_open_epilog(struct bnx2x *bp)
+{
+	/* Enable sriov via delayed work. This must be done via delayed work
+	 * because it causes the probe of the vf devices to be run, which invoke
+	 * register_netdevice which must have rtnl lock taken. As we are holding
+	 * the lock right now, that could only work if the probe would not take
+	 * the lock. However, as the probe of the vf may be called from other
+	 * contexts as well (such as passthrough to vm failes) it can't assume
+	 * the lock is being held for it. Using delayed work here allows the
+	 * probe code to simply take the lock (i.e. wait for it to be released
+	 * if it is being held). We only want to do this if the number of VFs
+	 * was set before PF driver was loaded.
+	 */
+	if (IS_SRIOV(bp) && BNX2X_NR_VIRTFN(bp)) {
+		smp_mb__before_clear_bit();
+		set_bit(BNX2X_SP_RTNL_ENABLE_SRIOV, &bp->sp_rtnl_state);
+		smp_mb__after_clear_bit();
+		schedule_delayed_work(&bp->sp_rtnl_task, 0);
+	}
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h
index b4050173add9..d67ddc554c0f 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h
@@ -193,6 +193,7 @@ struct bnx2x_virtf {
 #define VF_CFG_TPA		0x0004
 #define VF_CFG_INT_SIMD		0x0008
 #define VF_CACHE_LINE		0x0010
+#define VF_CFG_VLAN		0x0020
 
 	u8 state;
 #define VF_FREE		0	/* VF ready to be acquired holds no resc */
@@ -712,6 +713,7 @@ void bnx2x_add_tlv(struct bnx2x *bp, void *tlvs_list, u16 offset, u16 type,
 		   u16 length);
 void bnx2x_vfpf_prep(struct bnx2x *bp, struct vfpf_first_tlv *first_tlv,
 		     u16 type, u16 length);
+void bnx2x_vfpf_finalize(struct bnx2x *bp, struct vfpf_first_tlv *first_tlv);
 void bnx2x_dp_tlv_list(struct bnx2x *bp, void *tlvs_list);
 
 bool bnx2x_tlv_supported(u16 tlvtype);
@@ -731,7 +733,7 @@ int bnx2x_vfpf_init(struct bnx2x *bp);
 void bnx2x_vfpf_close_vf(struct bnx2x *bp);
 int bnx2x_vfpf_setup_q(struct bnx2x *bp, int fp_idx);
 int bnx2x_vfpf_teardown_queue(struct bnx2x *bp, int qidx);
-int bnx2x_vfpf_set_mac(struct bnx2x *bp);
+int bnx2x_vfpf_config_mac(struct bnx2x *bp, u8 *addr, u8 vf_qid, bool set);
 int bnx2x_vfpf_set_mcast(struct net_device *dev);
 int bnx2x_vfpf_storm_rx_mode(struct bnx2x *bp);
 
@@ -750,13 +752,17 @@ static inline int bnx2x_vf_ustorm_prods_offset(struct bnx2x *bp,
 }
 
 enum sample_bulletin_result bnx2x_sample_bulletin(struct bnx2x *bp);
-void bnx2x_vf_map_doorbells(struct bnx2x *bp);
+void __iomem *bnx2x_vf_doorbells(struct bnx2x *bp);
 int bnx2x_vf_pci_alloc(struct bnx2x *bp);
-void bnx2x_enable_sriov(struct bnx2x *bp);
+int bnx2x_enable_sriov(struct bnx2x *bp);
+void bnx2x_disable_sriov(struct bnx2x *bp);
 static inline int bnx2x_vf_headroom(struct bnx2x *bp)
 {
 	return bp->vfdb->sriov.nr_virtfn * BNX2X_CLIENTS_PER_VF;
 }
+void bnx2x_pf_set_vfs_vlan(struct bnx2x *bp);
+int bnx2x_sriov_configure(struct pci_dev *dev, int num_vfs);
+int bnx2x_open_epilog(struct bnx2x *bp);
 
 #else /* CONFIG_BNX2X_SRIOV */
 
@@ -779,7 +785,8 @@ static inline void bnx2x_iov_init_dmae(struct bnx2x *bp) {}
 static inline int bnx2x_iov_init_one(struct bnx2x *bp, int int_mode_param,
 				     int num_vfs_param) {return 0; }
 static inline void bnx2x_iov_remove_one(struct bnx2x *bp) {}
-static inline void bnx2x_enable_sriov(struct bnx2x *bp) {}
+static inline int bnx2x_enable_sriov(struct bnx2x *bp) {return 0; }
+static inline void bnx2x_disable_sriov(struct bnx2x *bp) {}
 static inline int bnx2x_vfpf_acquire(struct bnx2x *bp,
 				     u8 tx_count, u8 rx_count) {return 0; }
 static inline int bnx2x_vfpf_release(struct bnx2x *bp) {return 0; }
@@ -787,7 +794,8 @@ static inline int bnx2x_vfpf_init(struct bnx2x *bp) {return 0; }
 static inline void bnx2x_vfpf_close_vf(struct bnx2x *bp) {}
 static inline int bnx2x_vfpf_setup_q(struct bnx2x *bp, int fp_idx) {return 0; }
 static inline int bnx2x_vfpf_teardown_queue(struct bnx2x *bp, int qidx) {return 0; }
-static inline int bnx2x_vfpf_set_mac(struct bnx2x *bp) {return 0; }
+static inline int bnx2x_vfpf_config_mac(struct bnx2x *bp, u8 *addr,
+					u8 vf_qid, bool set) {return 0; }
 static inline int bnx2x_vfpf_set_mcast(struct net_device *dev) {return 0; }
 static inline int bnx2x_vfpf_storm_rx_mode(struct bnx2x *bp) {return 0; }
 static inline int bnx2x_iov_nic_init(struct bnx2x *bp) {return 0; }
@@ -802,8 +810,15 @@ static inline enum sample_bulletin_result bnx2x_sample_bulletin(struct bnx2x *bp
 	return PFVF_BULLETIN_UNCHANGED;
 }
 
-static inline int bnx2x_vf_map_doorbells(struct bnx2x *bp) {return 0; }
+static inline void __iomem *bnx2x_vf_doorbells(struct bnx2x *bp)
+{
+	return NULL;
+}
+
 static inline int bnx2x_vf_pci_alloc(struct bnx2x *bp) {return 0; }
+static inline void bnx2x_pf_set_vfs_vlan(struct bnx2x *bp) {}
+static inline int bnx2x_sriov_configure(struct pci_dev *dev, int num_vfs) {return 0; }
+static inline int bnx2x_open_epilog(struct bnx2x *bp) {return 0; }
 
 #endif /* CONFIG_BNX2X_SRIOV */
 #endif /* bnx2x_sriov.h */
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
index 4397f8b76f2e..2ca3d94fcec2 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
@@ -1547,11 +1547,51 @@ static void bnx2x_prep_fw_stats_req(struct bnx2x *bp)
 	}
 }
 
+void bnx2x_memset_stats(struct bnx2x *bp)
+{
+	int i;
+
+	/* function stats */
+	for_each_queue(bp, i) {
+		struct bnx2x_fp_stats *fp_stats = &bp->fp_stats[i];
+
+		memset(&fp_stats->old_tclient, 0,
+		       sizeof(fp_stats->old_tclient));
+		memset(&fp_stats->old_uclient, 0,
+		       sizeof(fp_stats->old_uclient));
+		memset(&fp_stats->old_xclient, 0,
+		       sizeof(fp_stats->old_xclient));
+		if (bp->stats_init) {
+			memset(&fp_stats->eth_q_stats, 0,
+			       sizeof(fp_stats->eth_q_stats));
+			memset(&fp_stats->eth_q_stats_old, 0,
+			       sizeof(fp_stats->eth_q_stats_old));
+		}
+	}
+
+	memset(&bp->dev->stats, 0, sizeof(bp->dev->stats));
+
+	if (bp->stats_init) {
+		memset(&bp->net_stats_old, 0, sizeof(bp->net_stats_old));
+		memset(&bp->fw_stats_old, 0, sizeof(bp->fw_stats_old));
+		memset(&bp->eth_stats_old, 0, sizeof(bp->eth_stats_old));
+		memset(&bp->eth_stats, 0, sizeof(bp->eth_stats));
+		memset(&bp->func_stats, 0, sizeof(bp->func_stats));
+	}
+
+	bp->stats_state = STATS_STATE_DISABLED;
+
+	if (bp->port.pmf && bp->port.port_stx)
+		bnx2x_port_stats_base_init(bp);
+
+	/* mark the end of statistics initializiation */
+	bp->stats_init = false;
+}
+
 void bnx2x_stats_init(struct bnx2x *bp)
 {
 	int /*abs*/port = BP_PORT(bp);
 	int mb_idx = BP_FW_MB_IDX(bp);
-	int i;
 
 	bp->stats_pending = 0;
 	bp->executer_idx = 0;
@@ -1587,36 +1627,11 @@ void bnx2x_stats_init(struct bnx2x *bp)
 			    &(bp->port.old_nig_stats.egress_mac_pkt1_lo), 2);
 	}
 
-	/* function stats */
-	for_each_queue(bp, i) {
-		struct bnx2x_fp_stats *fp_stats = &bp->fp_stats[i];
-
-		memset(&fp_stats->old_tclient, 0,
-		       sizeof(fp_stats->old_tclient));
-		memset(&fp_stats->old_uclient, 0,
-		       sizeof(fp_stats->old_uclient));
-		memset(&fp_stats->old_xclient, 0,
-		       sizeof(fp_stats->old_xclient));
-		if (bp->stats_init) {
-			memset(&fp_stats->eth_q_stats, 0,
-			       sizeof(fp_stats->eth_q_stats));
-			memset(&fp_stats->eth_q_stats_old, 0,
-			       sizeof(fp_stats->eth_q_stats_old));
-		}
-	}
-
 	/* Prepare statistics ramrod data */
 	bnx2x_prep_fw_stats_req(bp);
 
-	memset(&bp->dev->stats, 0, sizeof(bp->dev->stats));
+	/* Clean SP from previous statistics */
 	if (bp->stats_init) {
-		memset(&bp->net_stats_old, 0, sizeof(bp->net_stats_old));
-		memset(&bp->fw_stats_old, 0, sizeof(bp->fw_stats_old));
-		memset(&bp->eth_stats_old, 0, sizeof(bp->eth_stats_old));
-		memset(&bp->eth_stats, 0, sizeof(bp->eth_stats));
-		memset(&bp->func_stats, 0, sizeof(bp->func_stats));
-
-		/* Clean SP from previous statistics */
 		if (bp->func_stx) {
 			memset(bnx2x_sp(bp, func_stats), 0,
 			       sizeof(struct host_func_stats));
@@ -1626,13 +1641,7 @@ void bnx2x_stats_init(struct bnx2x *bp)
 		}
 	}
 
-	bp->stats_state = STATS_STATE_DISABLED;
-
-	if (bp->port.pmf && bp->port.port_stx)
-		bnx2x_port_stats_base_init(bp);
-
-	/* mark the end of statistics initializiation */
-	bp->stats_init = false;
+	bnx2x_memset_stats(bp);
 }
 
 void bnx2x_save_statistics(struct bnx2x *bp)
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h
index 198f6f1c9ad5..d117f472816c 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h
@@ -540,8 +540,8 @@ struct bnx2x_fw_port_stats_old {
 /* forward */
 struct bnx2x;
 
+void bnx2x_memset_stats(struct bnx2x *bp);
 void bnx2x_stats_init(struct bnx2x *bp);
-
 void bnx2x_stats_handle(struct bnx2x *bp, enum bnx2x_stats_event event);
 
 /**
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
index 531eebf40d60..928b074d7d80 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
@@ -36,6 +36,8 @@ void bnx2x_add_tlv(struct bnx2x *bp, void *tlvs_list, u16 offset, u16 type,
 void bnx2x_vfpf_prep(struct bnx2x *bp, struct vfpf_first_tlv *first_tlv,
 		     u16 type, u16 length)
 {
+	mutex_lock(&bp->vf2pf_mutex);
+
 	DP(BNX2X_MSG_IOV, "preparing to send %d tlv over vf pf channel\n",
 	   type);
 
@@ -49,6 +51,15 @@ void bnx2x_vfpf_prep(struct bnx2x *bp, struct vfpf_first_tlv *first_tlv,
 	first_tlv->resp_msg_offset = sizeof(bp->vf2pf_mbox->req);
 }
 
+/* releases the mailbox */
+void bnx2x_vfpf_finalize(struct bnx2x *bp, struct vfpf_first_tlv *first_tlv)
+{
+	DP(BNX2X_MSG_IOV, "done sending [%d] tlv over vf pf channel\n",
+	   first_tlv->tl.type);
+
+	mutex_unlock(&bp->vf2pf_mutex);
+}
+
 /* list the types and lengths of the tlvs on the buffer */
 void bnx2x_dp_tlv_list(struct bnx2x *bp, void *tlvs_list)
 {
@@ -181,8 +192,10 @@ int bnx2x_vfpf_acquire(struct bnx2x *bp, u8 tx_count, u8 rx_count)
 	/* clear mailbox and prep first tlv */
 	bnx2x_vfpf_prep(bp, &req->first_tlv, CHANNEL_TLV_ACQUIRE, sizeof(*req));
 
-	if (bnx2x_get_vf_id(bp, &vf_id))
-		return -EAGAIN;
+	if (bnx2x_get_vf_id(bp, &vf_id)) {
+		rc = -EAGAIN;
+		goto out;
+	}
 
 	req->vfdev_info.vf_id = vf_id;
 	req->vfdev_info.vf_os = 0;
@@ -213,7 +226,7 @@ int bnx2x_vfpf_acquire(struct bnx2x *bp, u8 tx_count, u8 rx_count)
 
 		/* PF timeout */
 		if (rc)
-			return rc;
+			goto out;
 
 		/* copy acquire response from buffer to bp */
 		memcpy(&bp->acquire_resp, resp, sizeof(bp->acquire_resp));
@@ -253,7 +266,8 @@ int bnx2x_vfpf_acquire(struct bnx2x *bp, u8 tx_count, u8 rx_count)
 			/* PF reports error */
 			BNX2X_ERR("Failed to get the requested amount of resources: %d. Breaking...\n",
 				  bp->acquire_resp.hdr.status);
-			return -EAGAIN;
+			rc = -EAGAIN;
+			goto out;
 		}
 	}
 
@@ -279,20 +293,24 @@ int bnx2x_vfpf_acquire(struct bnx2x *bp, u8 tx_count, u8 rx_count)
 		       bp->acquire_resp.resc.current_mac_addr,
 		       ETH_ALEN);
 
-	return 0;
+out:
+	bnx2x_vfpf_finalize(bp, &req->first_tlv);
+	return rc;
 }
 
 int bnx2x_vfpf_release(struct bnx2x *bp)
 {
 	struct vfpf_release_tlv *req = &bp->vf2pf_mbox->req.release;
 	struct pfvf_general_resp_tlv *resp = &bp->vf2pf_mbox->resp.general_resp;
-	u32 rc = 0, vf_id;
+	u32 rc, vf_id;
 
 	/* clear mailbox and prep first tlv */
 	bnx2x_vfpf_prep(bp, &req->first_tlv, CHANNEL_TLV_RELEASE, sizeof(*req));
 
-	if (bnx2x_get_vf_id(bp, &vf_id))
-		return -EAGAIN;
+	if (bnx2x_get_vf_id(bp, &vf_id)) {
+		rc = -EAGAIN;
+		goto out;
+	}
 
 	req->vf_id = vf_id;
 
@@ -308,7 +326,8 @@ int bnx2x_vfpf_release(struct bnx2x *bp)
 
 	if (rc)
 		/* PF timeout */
-		return rc;
+		goto out;
+
 	if (resp->hdr.status == PFVF_STATUS_SUCCESS) {
 		/* PF released us */
 		DP(BNX2X_MSG_SP, "vf released\n");
@@ -316,10 +335,13 @@ int bnx2x_vfpf_release(struct bnx2x *bp)
 		/* PF reports error */
 		BNX2X_ERR("PF failed our release request - are we out of sync? response status: %d\n",
 			  resp->hdr.status);
-		return -EAGAIN;
+		rc = -EAGAIN;
+		goto out;
 	}
+out:
+	bnx2x_vfpf_finalize(bp, &req->first_tlv);
 
-	return 0;
+	return rc;
 }
 
 /* Tell PF about SB addresses */
@@ -350,16 +372,20 @@ int bnx2x_vfpf_init(struct bnx2x *bp)
 
 	rc = bnx2x_send_msg2pf(bp, &resp->hdr.status, bp->vf2pf_mbox_mapping);
 	if (rc)
-		return rc;
+		goto out;
 
 	if (resp->hdr.status != PFVF_STATUS_SUCCESS) {
 		BNX2X_ERR("INIT VF failed: %d. Breaking...\n",
 			  resp->hdr.status);
-		return -EAGAIN;
+		rc = -EAGAIN;
+		goto out;
 	}
 
 	DP(BNX2X_MSG_SP, "INIT VF Succeeded\n");
-	return 0;
+out:
+	bnx2x_vfpf_finalize(bp, &req->first_tlv);
+
+	return rc;
 }
 
 /* CLOSE VF - opposite to INIT_VF */
@@ -380,6 +406,9 @@ void bnx2x_vfpf_close_vf(struct bnx2x *bp)
 	for_each_queue(bp, i)
 		bnx2x_vfpf_teardown_queue(bp, i);
 
+	/* remove mac */
+	bnx2x_vfpf_config_mac(bp, bp->dev->dev_addr, bp->fp->index, false);
+
 	/* clear mailbox and prep first tlv */
 	bnx2x_vfpf_prep(bp, &req->first_tlv, CHANNEL_TLV_CLOSE, sizeof(*req));
 
@@ -401,6 +430,8 @@ void bnx2x_vfpf_close_vf(struct bnx2x *bp)
 		BNX2X_ERR("Sending CLOSE failed: pf response was %d\n",
 			  resp->hdr.status);
 
+	bnx2x_vfpf_finalize(bp, &req->first_tlv);
+
 free_irq:
 	/* Disable HW interrupts, NAPI */
 	bnx2x_netif_stop(bp, 0);
@@ -435,7 +466,6 @@ int bnx2x_vfpf_setup_q(struct bnx2x *bp, int fp_idx)
 	/* calculate queue flags */
 	flags |= VFPF_QUEUE_FLG_STATS;
 	flags |= VFPF_QUEUE_FLG_CACHE_ALIGN;
-	flags |= IS_MF_SD(bp) ? VFPF_QUEUE_FLG_OV : 0;
 	flags |= VFPF_QUEUE_FLG_VLAN;
 	DP(NETIF_MSG_IFUP, "vlan removal enabled\n");
 
@@ -486,8 +516,11 @@ int bnx2x_vfpf_setup_q(struct bnx2x *bp, int fp_idx)
 	if (resp->hdr.status != PFVF_STATUS_SUCCESS) {
 		BNX2X_ERR("Status of SETUP_Q for queue[%d] is %d\n",
 			  fp_idx, resp->hdr.status);
-		return -EINVAL;
+		rc = -EINVAL;
 	}
+
+	bnx2x_vfpf_finalize(bp, &req->first_tlv);
+
 	return rc;
 }
 
@@ -515,41 +548,46 @@ int bnx2x_vfpf_teardown_queue(struct bnx2x *bp, int qidx)
 	if (rc) {
 		BNX2X_ERR("Sending TEARDOWN for queue %d failed: %d\n", qidx,
 			  rc);
-		return rc;
+		goto out;
 	}
 
 	/* PF failed the transaction */
 	if (resp->hdr.status != PFVF_STATUS_SUCCESS) {
 		BNX2X_ERR("TEARDOWN for queue %d failed: %d\n", qidx,
 			  resp->hdr.status);
-		return -EINVAL;
+		rc = -EINVAL;
 	}
 
-	return 0;
+out:
+	bnx2x_vfpf_finalize(bp, &req->first_tlv);
+	return rc;
 }
 
 /* request pf to add a mac for the vf */
-int bnx2x_vfpf_set_mac(struct bnx2x *bp)
+int bnx2x_vfpf_config_mac(struct bnx2x *bp, u8 *addr, u8 vf_qid, bool set)
 {
 	struct vfpf_set_q_filters_tlv *req = &bp->vf2pf_mbox->req.set_q_filters;
 	struct pfvf_general_resp_tlv *resp = &bp->vf2pf_mbox->resp.general_resp;
-	int rc;
+	struct pf_vf_bulletin_content bulletin = bp->pf2vf_bulletin->content;
+	int rc = 0;
 
 	/* clear mailbox and prep first tlv */
 	bnx2x_vfpf_prep(bp, &req->first_tlv, CHANNEL_TLV_SET_Q_FILTERS,
 			sizeof(*req));
 
 	req->flags = VFPF_SET_Q_FILTERS_MAC_VLAN_CHANGED;
-	req->vf_qid = 0;
+	req->vf_qid = vf_qid;
 	req->n_mac_vlan_filters = 1;
-	req->filters[0].flags =
-		VFPF_Q_FILTER_DEST_MAC_VALID | VFPF_Q_FILTER_SET_MAC;
+
+	req->filters[0].flags = VFPF_Q_FILTER_DEST_MAC_VALID;
+	if (set)
+		req->filters[0].flags |= VFPF_Q_FILTER_SET_MAC;
 
 	/* sample bulletin board for new mac */
 	bnx2x_sample_bulletin(bp);
 
 	/* copy mac from device to request */
-	memcpy(req->filters[0].mac, bp->dev->dev_addr, ETH_ALEN);
+	memcpy(req->filters[0].mac, addr, ETH_ALEN);
 
 	/* add list termination tlv */
 	bnx2x_add_tlv(bp, req, req->first_tlv.tl.length, CHANNEL_TLV_LIST_END,
@@ -562,7 +600,7 @@ int bnx2x_vfpf_set_mac(struct bnx2x *bp)
 	rc = bnx2x_send_msg2pf(bp, &resp->hdr.status, bp->vf2pf_mbox_mapping);
 	if (rc) {
 		BNX2X_ERR("failed to send message to pf. rc was %d\n", rc);
-		return rc;
+		goto out;
 	}
 
 	/* failure may mean PF was configured with a new mac for us */
@@ -570,6 +608,9 @@ int bnx2x_vfpf_set_mac(struct bnx2x *bp)
 		DP(BNX2X_MSG_IOV,
 		   "vfpf SET MAC failed. Check bulletin board for new posts\n");
 
+		/* copy mac from bulletin to device */
+		memcpy(bp->dev->dev_addr, bulletin.mac, ETH_ALEN);
+
 		/* check if bulletin board was updated */
 		if (bnx2x_sample_bulletin(bp) == PFVF_BULLETIN_UPDATED) {
 			/* copy mac from device to request */
@@ -587,8 +628,10 @@ int bnx2x_vfpf_set_mac(struct bnx2x *bp)
 
 	if (resp->hdr.status != PFVF_STATUS_SUCCESS) {
 		BNX2X_ERR("vfpf SET MAC failed: %d\n", resp->hdr.status);
-		return -EINVAL;
+		rc = -EINVAL;
 	}
+out:
+	bnx2x_vfpf_finalize(bp, &req->first_tlv);
 
 	return 0;
 }
@@ -643,14 +686,16 @@ int bnx2x_vfpf_set_mcast(struct net_device *dev)
 	rc = bnx2x_send_msg2pf(bp, &resp->hdr.status, bp->vf2pf_mbox_mapping);
 	if (rc) {
 		BNX2X_ERR("Sending a message failed: %d\n", rc);
-		return rc;
+		goto out;
 	}
 
 	if (resp->hdr.status != PFVF_STATUS_SUCCESS) {
 		BNX2X_ERR("Set Rx mode/multicast failed: %d\n",
 			  resp->hdr.status);
-		return -EINVAL;
+		rc = -EINVAL;
 	}
+out:
+	bnx2x_vfpf_finalize(bp, &req->first_tlv);
 
 	return 0;
 }
@@ -689,7 +734,8 @@ int bnx2x_vfpf_storm_rx_mode(struct bnx2x *bp)
 		break;
 	default:
 		BNX2X_ERR("BAD rx mode (%d)\n", mode);
-		return -EINVAL;
+		rc = -EINVAL;
+		goto out;
 	}
 
 	req->flags |= VFPF_SET_Q_FILTERS_RX_MASK_CHANGED;
@@ -708,8 +754,10 @@ int bnx2x_vfpf_storm_rx_mode(struct bnx2x *bp)
 
 	if (resp->hdr.status != PFVF_STATUS_SUCCESS) {
 		BNX2X_ERR("Set Rx mode failed: %d\n", resp->hdr.status);
-		return -EINVAL;
+		rc = -EINVAL;
 	}
+out:
+	bnx2x_vfpf_finalize(bp, &req->first_tlv);
 
 	return rc;
 }
@@ -1004,7 +1052,7 @@ static void bnx2x_vf_mbx_init_vf(struct bnx2x *bp, struct bnx2x_virtf *vf,
 }
 
 /* convert MBX queue-flags to standard SP queue-flags */
-static void bnx2x_vf_mbx_set_q_flags(u32 mbx_q_flags,
+static void bnx2x_vf_mbx_set_q_flags(struct bnx2x *bp, u32 mbx_q_flags,
 				     unsigned long *sp_q_flags)
 {
 	if (mbx_q_flags & VFPF_QUEUE_FLG_TPA)
@@ -1015,8 +1063,6 @@ static void bnx2x_vf_mbx_set_q_flags(u32 mbx_q_flags,
 		__set_bit(BNX2X_Q_FLG_TPA_GRO, sp_q_flags);
 	if (mbx_q_flags & VFPF_QUEUE_FLG_STATS)
 		__set_bit(BNX2X_Q_FLG_STATS, sp_q_flags);
-	if (mbx_q_flags & VFPF_QUEUE_FLG_OV)
-		__set_bit(BNX2X_Q_FLG_OV, sp_q_flags);
 	if (mbx_q_flags & VFPF_QUEUE_FLG_VLAN)
 		__set_bit(BNX2X_Q_FLG_VLAN, sp_q_flags);
 	if (mbx_q_flags & VFPF_QUEUE_FLG_COS)
@@ -1025,6 +1071,10 @@ static void bnx2x_vf_mbx_set_q_flags(u32 mbx_q_flags,
 		__set_bit(BNX2X_Q_FLG_HC, sp_q_flags);
 	if (mbx_q_flags & VFPF_QUEUE_FLG_DHC)
 		__set_bit(BNX2X_Q_FLG_DHC, sp_q_flags);
+
+	/* outer vlan removal is set according to the PF's multi fuction mode */
+	if (IS_MF_SD(bp))
+		__set_bit(BNX2X_Q_FLG_OV, sp_q_flags);
 }
 
 static void bnx2x_vf_mbx_setup_q(struct bnx2x *bp, struct bnx2x_virtf *vf,
@@ -1075,11 +1125,11 @@ static void bnx2x_vf_mbx_setup_q(struct bnx2x *bp, struct bnx2x_virtf *vf,
 			init_p->tx.hc_rate = setup_q->txq.hc_rate;
 			init_p->tx.sb_cq_index = setup_q->txq.sb_index;
 
-			bnx2x_vf_mbx_set_q_flags(setup_q->txq.flags,
+			bnx2x_vf_mbx_set_q_flags(bp, setup_q->txq.flags,
 						 &init_p->tx.flags);
 
 			/* tx setup - flags */
-			bnx2x_vf_mbx_set_q_flags(setup_q->txq.flags,
+			bnx2x_vf_mbx_set_q_flags(bp, setup_q->txq.flags,
 						 &setup_p->flags);
 
 			/* tx setup - general, nothing */
@@ -1107,11 +1157,11 @@ static void bnx2x_vf_mbx_setup_q(struct bnx2x *bp, struct bnx2x_virtf *vf,
 			/* rx init */
 			init_p->rx.hc_rate = setup_q->rxq.hc_rate;
 			init_p->rx.sb_cq_index = setup_q->rxq.sb_index;
-			bnx2x_vf_mbx_set_q_flags(setup_q->rxq.flags,
+			bnx2x_vf_mbx_set_q_flags(bp, setup_q->rxq.flags,
 						 &init_p->rx.flags);
 
 			/* rx setup - flags */
-			bnx2x_vf_mbx_set_q_flags(setup_q->rxq.flags,
+			bnx2x_vf_mbx_set_q_flags(bp, setup_q->rxq.flags,
 						 &setup_p->flags);
 
 			/* rx setup - general */
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.h
index bfc80baec00d..41708faab575 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.h
@@ -328,9 +328,15 @@ struct pf_vf_bulletin_content {
 #define MAC_ADDR_VALID		0	/* alert the vf that a new mac address
 					 * is available for it
 					 */
+#define VLAN_VALID		1	/* when set, the vf should not access
+					 * the vfpf channel
+					 */
 
 	u8 mac[ETH_ALEN];
-	u8 padding[2];
+	u8 mac_padding[2];
+
+	u16 vlan;
+	u8 vlan_padding[6];
 };
 
 union pf_vf_bulletin {
@@ -353,6 +359,7 @@ enum channel_tlvs {
 	CHANNEL_TLV_LIST_END,
 	CHANNEL_TLV_FLR,
 	CHANNEL_TLV_PF_SET_MAC,
+	CHANNEL_TLV_PF_SET_VLAN,
 	CHANNEL_TLV_MAX
 };
 
diff --git a/drivers/net/ethernet/broadcom/cnic.c b/drivers/net/ethernet/broadcom/cnic.c
index 149a3a038491..6b0dc131b20e 100644
--- a/drivers/net/ethernet/broadcom/cnic.c
+++ b/drivers/net/ethernet/broadcom/cnic.c
@@ -4085,7 +4085,7 @@ static int cnic_cm_alloc_mem(struct cnic_dev *dev)
 	if (!cp->csk_tbl)
 		return -ENOMEM;
 
-	port_id = random32();
+	port_id = prandom_u32();
 	port_id %= CNIC_LOCAL_PORT_RANGE;
 	if (cnic_init_id_tbl(&cp->csk_port_tbl, CNIC_LOCAL_PORT_RANGE,
 			     CNIC_LOCAL_PORT_MIN, port_id)) {
@@ -4145,7 +4145,7 @@ static int cnic_cm_init_bnx2_hw(struct cnic_dev *dev)
 {
 	u32 seed;
 
-	seed = random32();
+	seed = prandom_u32();
 	cnic_ctx_wr(dev, 45, 0, seed);
 	return 0;
 }
@@ -5544,8 +5544,10 @@ static struct cnic_dev *init_bnx2x_cnic(struct net_device *dev)
 
 	if (!(ethdev->drv_state & CNIC_DRV_STATE_NO_ISCSI))
 		cdev->max_iscsi_conn = ethdev->max_iscsi_conn;
-	if (CNIC_SUPPORTS_FCOE(cp))
+	if (CNIC_SUPPORTS_FCOE(cp)) {
 		cdev->max_fcoe_conn = ethdev->max_fcoe_conn;
+		cdev->max_fcoe_exchanges = ethdev->max_fcoe_exchanges;
+	}
 
 	if (cdev->max_fcoe_conn > BNX2X_FCOE_NUM_CONNECTIONS)
 		cdev->max_fcoe_conn = BNX2X_FCOE_NUM_CONNECTIONS;
diff --git a/drivers/net/ethernet/broadcom/cnic_if.h b/drivers/net/ethernet/broadcom/cnic_if.h
index 0c9367a0f57d..ec9bb9ad4bb3 100644
--- a/drivers/net/ethernet/broadcom/cnic_if.h
+++ b/drivers/net/ethernet/broadcom/cnic_if.h
@@ -195,6 +195,7 @@ struct cnic_eth_dev {
 	u32		max_fcoe_conn;
 	u32		max_rdma_conn;
 	u32		fcoe_init_cid;
+	u32		max_fcoe_exchanges;
 	u32		fcoe_wwn_port_name_hi;
 	u32		fcoe_wwn_port_name_lo;
 	u32		fcoe_wwn_node_name_hi;
@@ -313,6 +314,8 @@ struct cnic_dev {
 	int		max_fcoe_conn;
 	int		max_rdma_conn;
 
+	int		max_fcoe_exchanges;
+
 	union drv_info_to_mcp	*stats_addr;
 	struct fcoe_capabilities	*fcoe_cap;
 
diff --git a/drivers/net/ethernet/broadcom/sb1250-mac.c b/drivers/net/ethernet/broadcom/sb1250-mac.c
index e9b35da375cb..e80bfb60c3ef 100644
--- a/drivers/net/ethernet/broadcom/sb1250-mac.c
+++ b/drivers/net/ethernet/broadcom/sb1250-mac.c
@@ -831,11 +831,8 @@ static int sbdma_add_rcvbuffer(struct sbmac_softc *sc, struct sbmacdma *d,
 		sb_new = netdev_alloc_skb(dev, ENET_PACKET_SIZE +
 					       SMP_CACHE_BYTES * 2 +
 					       NET_IP_ALIGN);
-		if (sb_new == NULL) {
-			pr_info("%s: sk_buff allocation failed\n",
-			       d->sbdma_eth->sbm_dev->name);
+		if (sb_new == NULL)
 			return -ENOBUFS;
-		}
 
 		sbdma_align_skb(sb_new, SMP_CACHE_BYTES, NET_IP_ALIGN);
 	}
diff --git a/drivers/net/ethernet/broadcom/tg3.c b/drivers/net/ethernet/broadcom/tg3.c
index 17a972734ba7..728d42ab2a76 100644
--- a/drivers/net/ethernet/broadcom/tg3.c
+++ b/drivers/net/ethernet/broadcom/tg3.c
@@ -94,10 +94,10 @@ static inline void _tg3_flag_clear(enum TG3_FLAGS flag, unsigned long *bits)
 
 #define DRV_MODULE_NAME		"tg3"
 #define TG3_MAJ_NUM			3
-#define TG3_MIN_NUM			130
+#define TG3_MIN_NUM			131
 #define DRV_MODULE_VERSION	\
 	__stringify(TG3_MAJ_NUM) "." __stringify(TG3_MIN_NUM)
-#define DRV_MODULE_RELDATE	"February 14, 2013"
+#define DRV_MODULE_RELDATE	"April 09, 2013"
 
 #define RESET_KIND_SHUTDOWN	0
 #define RESET_KIND_INIT		1
@@ -212,6 +212,7 @@ static inline void _tg3_flag_clear(enum TG3_FLAGS flag, unsigned long *bits)
 #define TG3_FW_UPDATE_FREQ_SEC		(TG3_FW_UPDATE_TIMEOUT_SEC / 2)
 
 #define FIRMWARE_TG3		"tigon/tg3.bin"
+#define FIRMWARE_TG357766	"tigon/tg357766.bin"
 #define FIRMWARE_TG3TSO		"tigon/tg3_tso.bin"
 #define FIRMWARE_TG3TSO5	"tigon/tg3_tso5.bin"
 
@@ -1873,6 +1874,20 @@ static void tg3_link_report(struct tg3 *tp)
 	tp->link_up = netif_carrier_ok(tp->dev);
 }
 
+static u32 tg3_decode_flowctrl_1000T(u32 adv)
+{
+	u32 flowctrl = 0;
+
+	if (adv & ADVERTISE_PAUSE_CAP) {
+		flowctrl |= FLOW_CTRL_RX;
+		if (!(adv & ADVERTISE_PAUSE_ASYM))
+			flowctrl |= FLOW_CTRL_TX;
+	} else if (adv & ADVERTISE_PAUSE_ASYM)
+		flowctrl |= FLOW_CTRL_TX;
+
+	return flowctrl;
+}
+
 static u16 tg3_advert_flowctrl_1000X(u8 flow_ctrl)
 {
 	u16 miireg;
@@ -1889,6 +1904,20 @@ static u16 tg3_advert_flowctrl_1000X(u8 flow_ctrl)
 	return miireg;
 }
 
+static u32 tg3_decode_flowctrl_1000X(u32 adv)
+{
+	u32 flowctrl = 0;
+
+	if (adv & ADVERTISE_1000XPAUSE) {
+		flowctrl |= FLOW_CTRL_RX;
+		if (!(adv & ADVERTISE_1000XPSE_ASYM))
+			flowctrl |= FLOW_CTRL_TX;
+	} else if (adv & ADVERTISE_1000XPSE_ASYM)
+		flowctrl |= FLOW_CTRL_TX;
+
+	return flowctrl;
+}
+
 static u8 tg3_resolve_flowctrl_1000X(u16 lcladv, u16 rmtadv)
 {
 	u8 cap = 0;
@@ -2199,7 +2228,7 @@ static void tg3_phy_toggle_apd(struct tg3 *tp, bool enable)
 	tg3_writephy(tp, MII_TG3_MISC_SHDW, reg);
 }
 
-static void tg3_phy_toggle_automdix(struct tg3 *tp, int enable)
+static void tg3_phy_toggle_automdix(struct tg3 *tp, bool enable)
 {
 	u32 phy;
 
@@ -2291,7 +2320,7 @@ static void tg3_phy_apply_otp(struct tg3 *tp)
 	tg3_phy_toggle_auxctl_smdsp(tp, false);
 }
 
-static void tg3_phy_eee_adjust(struct tg3 *tp, u32 current_link_up)
+static void tg3_phy_eee_adjust(struct tg3 *tp, bool current_link_up)
 {
 	u32 val;
 
@@ -2301,7 +2330,7 @@ static void tg3_phy_eee_adjust(struct tg3 *tp, u32 current_link_up)
 	tp->setlpicnt = 0;
 
 	if (tp->link_config.autoneg == AUTONEG_ENABLE &&
-	    current_link_up == 1 &&
+	    current_link_up &&
 	    tp->link_config.active_duplex == DUPLEX_FULL &&
 	    (tp->link_config.active_speed == SPEED_100 ||
 	     tp->link_config.active_speed == SPEED_1000)) {
@@ -2323,7 +2352,7 @@ static void tg3_phy_eee_adjust(struct tg3 *tp, u32 current_link_up)
 	}
 
 	if (!tp->setlpicnt) {
-		if (current_link_up == 1 &&
+		if (current_link_up &&
 		   !tg3_phy_toggle_auxctl_smdsp(tp, true)) {
 			tg3_phydsp_write(tp, MII_TG3_DSP_TAP26, 0x0000);
 			tg3_phy_toggle_auxctl_smdsp(tp, false);
@@ -2530,6 +2559,13 @@ static void tg3_carrier_off(struct tg3 *tp)
 	tp->link_up = false;
 }
 
+static void tg3_warn_mgmt_link_flap(struct tg3 *tp)
+{
+	if (tg3_flag(tp, ENABLE_ASF))
+		netdev_warn(tp->dev,
+			    "Management side-band traffic will be interrupted during phy settings change\n");
+}
+
 /* This will reset the tigon3 PHY if there is no valid
  * link unless the FORCE argument is non-zero.
  */
@@ -2669,7 +2705,7 @@ out:
 	if (tg3_chip_rev_id(tp) == CHIPREV_ID_5762_A0)
 		tg3_phydsp_write(tp, 0xffb, 0x4000);
 
-	tg3_phy_toggle_automdix(tp, 1);
+	tg3_phy_toggle_automdix(tp, true);
 	tg3_phy_set_wirespeed(tp);
 	return 0;
 }
@@ -2925,6 +2961,9 @@ static void tg3_power_down_phy(struct tg3 *tp, bool do_low_power)
 {
 	u32 val;
 
+	if (tp->phy_flags & TG3_PHYFLG_KEEP_LINK_ON_PWRDN)
+		return;
+
 	if (tp->phy_flags & TG3_PHYFLG_PHY_SERDES) {
 		if (tg3_asic_rev(tp) == ASIC_REV_5704) {
 			u32 sg_dig_ctrl = tr32(SG_DIG_CTRL);
@@ -3448,11 +3487,58 @@ static int tg3_nvram_write_block(struct tg3 *tp, u32 offset, u32 len, u8 *buf)
 #define TX_CPU_SCRATCH_SIZE	0x04000
 
 /* tp->lock is held. */
-static int tg3_halt_cpu(struct tg3 *tp, u32 offset)
+static int tg3_pause_cpu(struct tg3 *tp, u32 cpu_base)
 {
 	int i;
+	const int iters = 10000;
+
+	for (i = 0; i < iters; i++) {
+		tw32(cpu_base + CPU_STATE, 0xffffffff);
+		tw32(cpu_base + CPU_MODE,  CPU_MODE_HALT);
+		if (tr32(cpu_base + CPU_MODE) & CPU_MODE_HALT)
+			break;
+	}
+
+	return (i == iters) ? -EBUSY : 0;
+}
+
+/* tp->lock is held. */
+static int tg3_rxcpu_pause(struct tg3 *tp)
+{
+	int rc = tg3_pause_cpu(tp, RX_CPU_BASE);
+
+	tw32(RX_CPU_BASE + CPU_STATE, 0xffffffff);
+	tw32_f(RX_CPU_BASE + CPU_MODE,  CPU_MODE_HALT);
+	udelay(10);
+
+	return rc;
+}
+
+/* tp->lock is held. */
+static int tg3_txcpu_pause(struct tg3 *tp)
+{
+	return tg3_pause_cpu(tp, TX_CPU_BASE);
+}
+
+/* tp->lock is held. */
+static void tg3_resume_cpu(struct tg3 *tp, u32 cpu_base)
+{
+	tw32(cpu_base + CPU_STATE, 0xffffffff);
+	tw32_f(cpu_base + CPU_MODE,  0x00000000);
+}
 
-	BUG_ON(offset == TX_CPU_BASE && tg3_flag(tp, 5705_PLUS));
+/* tp->lock is held. */
+static void tg3_rxcpu_resume(struct tg3 *tp)
+{
+	tg3_resume_cpu(tp, RX_CPU_BASE);
+}
+
+/* tp->lock is held. */
+static int tg3_halt_cpu(struct tg3 *tp, u32 cpu_base)
+{
+	int rc;
+
+	BUG_ON(cpu_base == TX_CPU_BASE && tg3_flag(tp, 5705_PLUS));
 
 	if (tg3_asic_rev(tp) == ASIC_REV_5906) {
 		u32 val = tr32(GRC_VCPU_EXT_CTRL);
@@ -3460,17 +3546,8 @@ static int tg3_halt_cpu(struct tg3 *tp, u32 offset)
 		tw32(GRC_VCPU_EXT_CTRL, val | GRC_VCPU_EXT_CTRL_HALT_CPU);
 		return 0;
 	}
-	if (offset == RX_CPU_BASE) {
-		for (i = 0; i < 10000; i++) {
-			tw32(offset + CPU_STATE, 0xffffffff);
-			tw32(offset + CPU_MODE,  CPU_MODE_HALT);
-			if (tr32(offset + CPU_MODE) & CPU_MODE_HALT)
-				break;
-		}
-
-		tw32(offset + CPU_STATE, 0xffffffff);
-		tw32_f(offset + CPU_MODE,  CPU_MODE_HALT);
-		udelay(10);
+	if (cpu_base == RX_CPU_BASE) {
+		rc = tg3_rxcpu_pause(tp);
 	} else {
 		/*
 		 * There is only an Rx CPU for the 5750 derivative in the
@@ -3479,17 +3556,12 @@ static int tg3_halt_cpu(struct tg3 *tp, u32 offset)
 		if (tg3_flag(tp, IS_SSB_CORE))
 			return 0;
 
-		for (i = 0; i < 10000; i++) {
-			tw32(offset + CPU_STATE, 0xffffffff);
-			tw32(offset + CPU_MODE,  CPU_MODE_HALT);
-			if (tr32(offset + CPU_MODE) & CPU_MODE_HALT)
-				break;
-		}
+		rc = tg3_txcpu_pause(tp);
 	}
 
-	if (i >= 10000) {
+	if (rc) {
 		netdev_err(tp->dev, "%s timed out, %s CPU\n",
-			   __func__, offset == RX_CPU_BASE ? "RX" : "TX");
+			   __func__, cpu_base == RX_CPU_BASE ? "RX" : "TX");
 		return -ENODEV;
 	}
 
@@ -3499,19 +3571,41 @@ static int tg3_halt_cpu(struct tg3 *tp, u32 offset)
 	return 0;
 }
 
-struct fw_info {
-	unsigned int fw_base;
-	unsigned int fw_len;
-	const __be32 *fw_data;
-};
+static int tg3_fw_data_len(struct tg3 *tp,
+			   const struct tg3_firmware_hdr *fw_hdr)
+{
+	int fw_len;
+
+	/* Non fragmented firmware have one firmware header followed by a
+	 * contiguous chunk of data to be written. The length field in that
+	 * header is not the length of data to be written but the complete
+	 * length of the bss. The data length is determined based on
+	 * tp->fw->size minus headers.
+	 *
+	 * Fragmented firmware have a main header followed by multiple
+	 * fragments. Each fragment is identical to non fragmented firmware
+	 * with a firmware header followed by a contiguous chunk of data. In
+	 * the main header, the length field is unused and set to 0xffffffff.
+	 * In each fragment header the length is the entire size of that
+	 * fragment i.e. fragment data + header length. Data length is
+	 * therefore length field in the header minus TG3_FW_HDR_LEN.
+	 */
+	if (tp->fw_len == 0xffffffff)
+		fw_len = be32_to_cpu(fw_hdr->len);
+	else
+		fw_len = tp->fw->size;
+
+	return (fw_len - TG3_FW_HDR_LEN) / sizeof(u32);
+}
 
 /* tp->lock is held. */
 static int tg3_load_firmware_cpu(struct tg3 *tp, u32 cpu_base,
 				 u32 cpu_scratch_base, int cpu_scratch_size,
-				 struct fw_info *info)
+				 const struct tg3_firmware_hdr *fw_hdr)
 {
-	int err, lock_err, i;
+	int err, i;
 	void (*write_op)(struct tg3 *, u32, u32);
+	int total_len = tp->fw->size;
 
 	if (cpu_base == TX_CPU_BASE && tg3_flag(tp, 5705_PLUS)) {
 		netdev_err(tp->dev,
@@ -3520,30 +3614,49 @@ static int tg3_load_firmware_cpu(struct tg3 *tp, u32 cpu_base,
 		return -EINVAL;
 	}
 
-	if (tg3_flag(tp, 5705_PLUS))
+	if (tg3_flag(tp, 5705_PLUS) && tg3_asic_rev(tp) != ASIC_REV_57766)
 		write_op = tg3_write_mem;
 	else
 		write_op = tg3_write_indirect_reg32;
 
-	/* It is possible that bootcode is still loading at this point.
-	 * Get the nvram lock first before halting the cpu.
-	 */
-	lock_err = tg3_nvram_lock(tp);
-	err = tg3_halt_cpu(tp, cpu_base);
-	if (!lock_err)
-		tg3_nvram_unlock(tp);
-	if (err)
-		goto out;
+	if (tg3_asic_rev(tp) != ASIC_REV_57766) {
+		/* It is possible that bootcode is still loading at this point.
+		 * Get the nvram lock first before halting the cpu.
+		 */
+		int lock_err = tg3_nvram_lock(tp);
+		err = tg3_halt_cpu(tp, cpu_base);
+		if (!lock_err)
+			tg3_nvram_unlock(tp);
+		if (err)
+			goto out;
 
-	for (i = 0; i < cpu_scratch_size; i += sizeof(u32))
-		write_op(tp, cpu_scratch_base + i, 0);
-	tw32(cpu_base + CPU_STATE, 0xffffffff);
-	tw32(cpu_base + CPU_MODE, tr32(cpu_base+CPU_MODE)|CPU_MODE_HALT);
-	for (i = 0; i < (info->fw_len / sizeof(u32)); i++)
-		write_op(tp, (cpu_scratch_base +
-			      (info->fw_base & 0xffff) +
-			      (i * sizeof(u32))),
-			      be32_to_cpu(info->fw_data[i]));
+		for (i = 0; i < cpu_scratch_size; i += sizeof(u32))
+			write_op(tp, cpu_scratch_base + i, 0);
+		tw32(cpu_base + CPU_STATE, 0xffffffff);
+		tw32(cpu_base + CPU_MODE,
+		     tr32(cpu_base + CPU_MODE) | CPU_MODE_HALT);
+	} else {
+		/* Subtract additional main header for fragmented firmware and
+		 * advance to the first fragment
+		 */
+		total_len -= TG3_FW_HDR_LEN;
+		fw_hdr++;
+	}
+
+	do {
+		u32 *fw_data = (u32 *)(fw_hdr + 1);
+		for (i = 0; i < tg3_fw_data_len(tp, fw_hdr); i++)
+			write_op(tp, cpu_scratch_base +
+				     (be32_to_cpu(fw_hdr->base_addr) & 0xffff) +
+				     (i * sizeof(u32)),
+				 be32_to_cpu(fw_data[i]));
+
+		total_len -= be32_to_cpu(fw_hdr->len);
+
+		/* Advance to next fragment */
+		fw_hdr = (struct tg3_firmware_hdr *)
+			 ((void *)fw_hdr + be32_to_cpu(fw_hdr->len));
+	} while (total_len > 0);
 
 	err = 0;
 
@@ -3552,13 +3665,33 @@ out:
 }
 
 /* tp->lock is held. */
+static int tg3_pause_cpu_and_set_pc(struct tg3 *tp, u32 cpu_base, u32 pc)
+{
+	int i;
+	const int iters = 5;
+
+	tw32(cpu_base + CPU_STATE, 0xffffffff);
+	tw32_f(cpu_base + CPU_PC, pc);
+
+	for (i = 0; i < iters; i++) {
+		if (tr32(cpu_base + CPU_PC) == pc)
+			break;
+		tw32(cpu_base + CPU_STATE, 0xffffffff);
+		tw32(cpu_base + CPU_MODE,  CPU_MODE_HALT);
+		tw32_f(cpu_base + CPU_PC, pc);
+		udelay(1000);
+	}
+
+	return (i == iters) ? -EBUSY : 0;
+}
+
+/* tp->lock is held. */
 static int tg3_load_5701_a0_firmware_fix(struct tg3 *tp)
 {
-	struct fw_info info;
-	const __be32 *fw_data;
-	int err, i;
+	const struct tg3_firmware_hdr *fw_hdr;
+	int err;
 
-	fw_data = (void *)tp->fw->data;
+	fw_hdr = (struct tg3_firmware_hdr *)tp->fw->data;
 
 	/* Firmware blob starts with version numbers, followed by
 	   start address and length. We are setting complete length.
@@ -3566,60 +3699,117 @@ static int tg3_load_5701_a0_firmware_fix(struct tg3 *tp)
 	   Remainder is the blob to be loaded contiguously
 	   from start address. */
 
-	info.fw_base = be32_to_cpu(fw_data[1]);
-	info.fw_len = tp->fw->size - 12;
-	info.fw_data = &fw_data[3];
-
 	err = tg3_load_firmware_cpu(tp, RX_CPU_BASE,
 				    RX_CPU_SCRATCH_BASE, RX_CPU_SCRATCH_SIZE,
-				    &info);
+				    fw_hdr);
 	if (err)
 		return err;
 
 	err = tg3_load_firmware_cpu(tp, TX_CPU_BASE,
 				    TX_CPU_SCRATCH_BASE, TX_CPU_SCRATCH_SIZE,
-				    &info);
+				    fw_hdr);
 	if (err)
 		return err;
 
 	/* Now startup only the RX cpu. */
-	tw32(RX_CPU_BASE + CPU_STATE, 0xffffffff);
-	tw32_f(RX_CPU_BASE + CPU_PC, info.fw_base);
-
-	for (i = 0; i < 5; i++) {
-		if (tr32(RX_CPU_BASE + CPU_PC) == info.fw_base)
-			break;
-		tw32(RX_CPU_BASE + CPU_STATE, 0xffffffff);
-		tw32(RX_CPU_BASE + CPU_MODE,  CPU_MODE_HALT);
-		tw32_f(RX_CPU_BASE + CPU_PC, info.fw_base);
-		udelay(1000);
-	}
-	if (i >= 5) {
+	err = tg3_pause_cpu_and_set_pc(tp, RX_CPU_BASE,
+				       be32_to_cpu(fw_hdr->base_addr));
+	if (err) {
 		netdev_err(tp->dev, "%s fails to set RX CPU PC, is %08x "
 			   "should be %08x\n", __func__,
-			   tr32(RX_CPU_BASE + CPU_PC), info.fw_base);
+			   tr32(RX_CPU_BASE + CPU_PC),
+				be32_to_cpu(fw_hdr->base_addr));
 		return -ENODEV;
 	}
-	tw32(RX_CPU_BASE + CPU_STATE, 0xffffffff);
-	tw32_f(RX_CPU_BASE + CPU_MODE,  0x00000000);
+
+	tg3_rxcpu_resume(tp);
 
 	return 0;
 }
 
+static int tg3_validate_rxcpu_state(struct tg3 *tp)
+{
+	const int iters = 1000;
+	int i;
+	u32 val;
+
+	/* Wait for boot code to complete initialization and enter service
+	 * loop. It is then safe to download service patches
+	 */
+	for (i = 0; i < iters; i++) {
+		if (tr32(RX_CPU_HWBKPT) == TG3_SBROM_IN_SERVICE_LOOP)
+			break;
+
+		udelay(10);
+	}
+
+	if (i == iters) {
+		netdev_err(tp->dev, "Boot code not ready for service patches\n");
+		return -EBUSY;
+	}
+
+	val = tg3_read_indirect_reg32(tp, TG3_57766_FW_HANDSHAKE);
+	if (val & 0xff) {
+		netdev_warn(tp->dev,
+			    "Other patches exist. Not downloading EEE patch\n");
+		return -EEXIST;
+	}
+
+	return 0;
+}
+
+/* tp->lock is held. */
+static void tg3_load_57766_firmware(struct tg3 *tp)
+{
+	struct tg3_firmware_hdr *fw_hdr;
+
+	if (!tg3_flag(tp, NO_NVRAM))
+		return;
+
+	if (tg3_validate_rxcpu_state(tp))
+		return;
+
+	if (!tp->fw)
+		return;
+
+	/* This firmware blob has a different format than older firmware
+	 * releases as given below. The main difference is we have fragmented
+	 * data to be written to non-contiguous locations.
+	 *
+	 * In the beginning we have a firmware header identical to other
+	 * firmware which consists of version, base addr and length. The length
+	 * here is unused and set to 0xffffffff.
+	 *
+	 * This is followed by a series of firmware fragments which are
+	 * individually identical to previous firmware. i.e. they have the
+	 * firmware header and followed by data for that fragment. The version
+	 * field of the individual fragment header is unused.
+	 */
+
+	fw_hdr = (struct tg3_firmware_hdr *)tp->fw->data;
+	if (be32_to_cpu(fw_hdr->base_addr) != TG3_57766_FW_BASE_ADDR)
+		return;
+
+	if (tg3_rxcpu_pause(tp))
+		return;
+
+	/* tg3_load_firmware_cpu() will always succeed for the 57766 */
+	tg3_load_firmware_cpu(tp, 0, TG3_57766_FW_BASE_ADDR, 0, fw_hdr);
+
+	tg3_rxcpu_resume(tp);
+}
+
 /* tp->lock is held. */
 static int tg3_load_tso_firmware(struct tg3 *tp)
 {
-	struct fw_info info;
-	const __be32 *fw_data;
+	const struct tg3_firmware_hdr *fw_hdr;
 	unsigned long cpu_base, cpu_scratch_base, cpu_scratch_size;
-	int err, i;
+	int err;
 
-	if (tg3_flag(tp, HW_TSO_1) ||
-	    tg3_flag(tp, HW_TSO_2) ||
-	    tg3_flag(tp, HW_TSO_3))
+	if (!tg3_flag(tp, FW_TSO))
 		return 0;
 
-	fw_data = (void *)tp->fw->data;
+	fw_hdr = (struct tg3_firmware_hdr *)tp->fw->data;
 
 	/* Firmware blob starts with version numbers, followed by
 	   start address and length. We are setting complete length.
@@ -3627,10 +3817,7 @@ static int tg3_load_tso_firmware(struct tg3 *tp)
 	   Remainder is the blob to be loaded contiguously
 	   from start address. */
 
-	info.fw_base = be32_to_cpu(fw_data[1]);
 	cpu_scratch_size = tp->fw_len;
-	info.fw_len = tp->fw->size - 12;
-	info.fw_data = &fw_data[3];
 
 	if (tg3_asic_rev(tp) == ASIC_REV_5705) {
 		cpu_base = RX_CPU_BASE;
@@ -3643,36 +3830,28 @@ static int tg3_load_tso_firmware(struct tg3 *tp)
 
 	err = tg3_load_firmware_cpu(tp, cpu_base,
 				    cpu_scratch_base, cpu_scratch_size,
-				    &info);
+				    fw_hdr);
 	if (err)
 		return err;
 
 	/* Now startup the cpu. */
-	tw32(cpu_base + CPU_STATE, 0xffffffff);
-	tw32_f(cpu_base + CPU_PC, info.fw_base);
-
-	for (i = 0; i < 5; i++) {
-		if (tr32(cpu_base + CPU_PC) == info.fw_base)
-			break;
-		tw32(cpu_base + CPU_STATE, 0xffffffff);
-		tw32(cpu_base + CPU_MODE,  CPU_MODE_HALT);
-		tw32_f(cpu_base + CPU_PC, info.fw_base);
-		udelay(1000);
-	}
-	if (i >= 5) {
+	err = tg3_pause_cpu_and_set_pc(tp, cpu_base,
+				       be32_to_cpu(fw_hdr->base_addr));
+	if (err) {
 		netdev_err(tp->dev,
 			   "%s fails to set CPU PC, is %08x should be %08x\n",
-			   __func__, tr32(cpu_base + CPU_PC), info.fw_base);
+			   __func__, tr32(cpu_base + CPU_PC),
+			   be32_to_cpu(fw_hdr->base_addr));
 		return -ENODEV;
 	}
-	tw32(cpu_base + CPU_STATE, 0xffffffff);
-	tw32_f(cpu_base + CPU_MODE,  0x00000000);
+
+	tg3_resume_cpu(tp, cpu_base);
 	return 0;
 }
 
 
 /* tp->lock is held. */
-static void __tg3_set_mac_addr(struct tg3 *tp, int skip_mac_1)
+static void __tg3_set_mac_addr(struct tg3 *tp, bool skip_mac_1)
 {
 	u32 addr_high, addr_low;
 	int i;
@@ -3735,7 +3914,7 @@ static int tg3_power_up(struct tg3 *tp)
 	return err;
 }
 
-static int tg3_setup_phy(struct tg3 *, int);
+static int tg3_setup_phy(struct tg3 *, bool);
 
 static int tg3_power_down_prepare(struct tg3 *tp)
 {
@@ -3807,7 +3986,7 @@ static int tg3_power_down_prepare(struct tg3 *tp)
 			tp->phy_flags |= TG3_PHYFLG_IS_LOW_POWER;
 
 		if (!(tp->phy_flags & TG3_PHYFLG_ANY_SERDES))
-			tg3_setup_phy(tp, 0);
+			tg3_setup_phy(tp, false);
 	}
 
 	if (tg3_asic_rev(tp) == ASIC_REV_5906) {
@@ -3848,7 +4027,13 @@ static int tg3_power_down_prepare(struct tg3 *tp)
 
 			if (tp->phy_flags & TG3_PHYFLG_MII_SERDES)
 				mac_mode = MAC_MODE_PORT_MODE_GMII;
-			else
+			else if (tp->phy_flags &
+				 TG3_PHYFLG_KEEP_LINK_ON_PWRDN) {
+				if (tp->link_config.active_speed == SPEED_1000)
+					mac_mode = MAC_MODE_PORT_MODE_GMII;
+				else
+					mac_mode = MAC_MODE_PORT_MODE_MII;
+			} else
 				mac_mode = MAC_MODE_PORT_MODE_MII;
 
 			mac_mode |= tp->mac_mode & MAC_MODE_LINK_POLARITY;
@@ -4102,12 +4287,16 @@ static void tg3_phy_copper_begin(struct tg3 *tp)
 	    (tp->phy_flags & TG3_PHYFLG_IS_LOW_POWER)) {
 		u32 adv, fc;
 
-		if (tp->phy_flags & TG3_PHYFLG_IS_LOW_POWER) {
+		if ((tp->phy_flags & TG3_PHYFLG_IS_LOW_POWER) &&
+		    !(tp->phy_flags & TG3_PHYFLG_KEEP_LINK_ON_PWRDN)) {
 			adv = ADVERTISED_10baseT_Half |
 			      ADVERTISED_10baseT_Full;
 			if (tg3_flag(tp, WOL_SPEED_100MB))
 				adv |= ADVERTISED_100baseT_Half |
 				       ADVERTISED_100baseT_Full;
+			if (tp->phy_flags & TG3_PHYFLG_1G_ON_VAUX_OK)
+				adv |= ADVERTISED_1000baseT_Half |
+				       ADVERTISED_1000baseT_Full;
 
 			fc = FLOW_CTRL_TX | FLOW_CTRL_RX;
 		} else {
@@ -4121,6 +4310,15 @@ static void tg3_phy_copper_begin(struct tg3 *tp)
 
 		tg3_phy_autoneg_cfg(tp, adv, fc);
 
+		if ((tp->phy_flags & TG3_PHYFLG_IS_LOW_POWER) &&
+		    (tp->phy_flags & TG3_PHYFLG_KEEP_LINK_ON_PWRDN)) {
+			/* Normally during power down we want to autonegotiate
+			 * the lowest possible speed for WOL. However, to avoid
+			 * link flap, we leave it untouched.
+			 */
+			return;
+		}
+
 		tg3_writephy(tp, MII_BMCR,
 			     BMCR_ANENABLE | BMCR_ANRESTART);
 	} else {
@@ -4177,6 +4375,103 @@ static void tg3_phy_copper_begin(struct tg3 *tp)
 	}
 }
 
+static int tg3_phy_pull_config(struct tg3 *tp)
+{
+	int err;
+	u32 val;
+
+	err = tg3_readphy(tp, MII_BMCR, &val);
+	if (err)
+		goto done;
+
+	if (!(val & BMCR_ANENABLE)) {
+		tp->link_config.autoneg = AUTONEG_DISABLE;
+		tp->link_config.advertising = 0;
+		tg3_flag_clear(tp, PAUSE_AUTONEG);
+
+		err = -EIO;
+
+		switch (val & (BMCR_SPEED1000 | BMCR_SPEED100)) {
+		case 0:
+			if (tp->phy_flags & TG3_PHYFLG_ANY_SERDES)
+				goto done;
+
+			tp->link_config.speed = SPEED_10;
+			break;
+		case BMCR_SPEED100:
+			if (tp->phy_flags & TG3_PHYFLG_ANY_SERDES)
+				goto done;
+
+			tp->link_config.speed = SPEED_100;
+			break;
+		case BMCR_SPEED1000:
+			if (!(tp->phy_flags & TG3_PHYFLG_10_100_ONLY)) {
+				tp->link_config.speed = SPEED_1000;
+				break;
+			}
+			/* Fall through */
+		default:
+			goto done;
+		}
+
+		if (val & BMCR_FULLDPLX)
+			tp->link_config.duplex = DUPLEX_FULL;
+		else
+			tp->link_config.duplex = DUPLEX_HALF;
+
+		tp->link_config.flowctrl = FLOW_CTRL_RX | FLOW_CTRL_TX;
+
+		err = 0;
+		goto done;
+	}
+
+	tp->link_config.autoneg = AUTONEG_ENABLE;
+	tp->link_config.advertising = ADVERTISED_Autoneg;
+	tg3_flag_set(tp, PAUSE_AUTONEG);
+
+	if (!(tp->phy_flags & TG3_PHYFLG_ANY_SERDES)) {
+		u32 adv;
+
+		err = tg3_readphy(tp, MII_ADVERTISE, &val);
+		if (err)
+			goto done;
+
+		adv = mii_adv_to_ethtool_adv_t(val & ADVERTISE_ALL);
+		tp->link_config.advertising |= adv | ADVERTISED_TP;
+
+		tp->link_config.flowctrl = tg3_decode_flowctrl_1000T(val);
+	} else {
+		tp->link_config.advertising |= ADVERTISED_FIBRE;
+	}
+
+	if (!(tp->phy_flags & TG3_PHYFLG_10_100_ONLY)) {
+		u32 adv;
+
+		if (!(tp->phy_flags & TG3_PHYFLG_ANY_SERDES)) {
+			err = tg3_readphy(tp, MII_CTRL1000, &val);
+			if (err)
+				goto done;
+
+			adv = mii_ctrl1000_to_ethtool_adv_t(val);
+		} else {
+			err = tg3_readphy(tp, MII_ADVERTISE, &val);
+			if (err)
+				goto done;
+
+			adv = tg3_decode_flowctrl_1000X(val);
+			tp->link_config.flowctrl = adv;
+
+			val &= (ADVERTISE_1000XHALF | ADVERTISE_1000XFULL);
+			adv = mii_adv_to_ethtool_adv_x(val);
+		}
+
+		tp->link_config.advertising |= adv;
+	}
+
+done:
+	return err;
+}
+
 static int tg3_init_5401phy_dsp(struct tg3 *tp)
 {
 	int err;
@@ -4196,6 +4491,32 @@ static int tg3_init_5401phy_dsp(struct tg3 *tp)
 	return err;
 }
 
+static bool tg3_phy_eee_config_ok(struct tg3 *tp)
+{
+	u32 val;
+	u32 tgtadv = 0;
+	u32 advertising = tp->link_config.advertising;
+
+	if (!(tp->phy_flags & TG3_PHYFLG_EEE_CAP))
+		return true;
+
+	if (tg3_phy_cl45_read(tp, MDIO_MMD_AN, MDIO_AN_EEE_ADV, &val))
+		return false;
+
+	val &= (MDIO_AN_EEE_ADV_100TX | MDIO_AN_EEE_ADV_1000T);
+
+
+	if (advertising & ADVERTISED_100baseT_Full)
+		tgtadv |= MDIO_AN_EEE_ADV_100TX;
+	if (advertising & ADVERTISED_1000baseT_Full)
+		tgtadv |= MDIO_AN_EEE_ADV_1000T;
+
+	if (val != tgtadv)
+		return false;
+
+	return true;
+}
+
 static bool tg3_phy_copper_an_config_ok(struct tg3 *tp, u32 *lcladv)
 {
 	u32 advmsk, tgtadv, advertising;
@@ -4262,7 +4583,7 @@ static bool tg3_phy_copper_fetch_rmtadv(struct tg3 *tp, u32 *rmtadv)
 	return true;
 }
 
-static bool tg3_test_and_report_link_chg(struct tg3 *tp, int curr_link_up)
+static bool tg3_test_and_report_link_chg(struct tg3 *tp, bool curr_link_up)
 {
 	if (curr_link_up != tp->link_up) {
 		if (curr_link_up) {
@@ -4280,23 +4601,28 @@ static bool tg3_test_and_report_link_chg(struct tg3 *tp, int curr_link_up)
 	return false;
 }
 
-static int tg3_setup_copper_phy(struct tg3 *tp, int force_reset)
+static void tg3_clear_mac_status(struct tg3 *tp)
 {
-	int current_link_up;
+	tw32(MAC_EVENT, 0);
+
+	tw32_f(MAC_STATUS,
+	       MAC_STATUS_SYNC_CHANGED |
+	       MAC_STATUS_CFG_CHANGED |
+	       MAC_STATUS_MI_COMPLETION |
+	       MAC_STATUS_LNKSTATE_CHANGED);
+	udelay(40);
+}
+
+static int tg3_setup_copper_phy(struct tg3 *tp, bool force_reset)
+{
+	bool current_link_up;
 	u32 bmsr, val;
 	u32 lcl_adv, rmt_adv;
 	u16 current_speed;
 	u8 current_duplex;
 	int i, err;
 
-	tw32(MAC_EVENT, 0);
-
-	tw32_f(MAC_STATUS,
-	     (MAC_STATUS_SYNC_CHANGED |
-	      MAC_STATUS_CFG_CHANGED |
-	      MAC_STATUS_MI_COMPLETION |
-	      MAC_STATUS_LNKSTATE_CHANGED));
-	udelay(40);
+	tg3_clear_mac_status(tp);
 
 	if ((tp->mi_mode & MAC_MI_MODE_AUTO_POLL) != 0) {
 		tw32_f(MAC_MI_MODE,
@@ -4316,7 +4642,7 @@ static int tg3_setup_copper_phy(struct tg3 *tp, int force_reset)
 		tg3_readphy(tp, MII_BMSR, &bmsr);
 		if (!tg3_readphy(tp, MII_BMSR, &bmsr) &&
 		    !(bmsr & BMSR_LSTATUS))
-			force_reset = 1;
+			force_reset = true;
 	}
 	if (force_reset)
 		tg3_phy_reset(tp);
@@ -4380,7 +4706,7 @@ static int tg3_setup_copper_phy(struct tg3 *tp, int force_reset)
 			tg3_writephy(tp, MII_TG3_EXT_CTRL, 0);
 	}
 
-	current_link_up = 0;
+	current_link_up = false;
 	current_speed = SPEED_UNKNOWN;
 	current_duplex = DUPLEX_UNKNOWN;
 	tp->phy_flags &= ~TG3_PHYFLG_MDIX_STATE;
@@ -4439,21 +4765,31 @@ static int tg3_setup_copper_phy(struct tg3 *tp, int force_reset)
 		tp->link_config.active_duplex = current_duplex;
 
 		if (tp->link_config.autoneg == AUTONEG_ENABLE) {
+			bool eee_config_ok = tg3_phy_eee_config_ok(tp);
+
 			if ((bmcr & BMCR_ANENABLE) &&
+			    eee_config_ok &&
 			    tg3_phy_copper_an_config_ok(tp, &lcl_adv) &&
 			    tg3_phy_copper_fetch_rmtadv(tp, &rmt_adv))
-				current_link_up = 1;
+				current_link_up = true;
+
+			/* EEE settings changes take effect only after a phy
+			 * reset.  If we have skipped a reset due to Link Flap
+			 * Avoidance being enabled, do it now.
+			 */
+			if (!eee_config_ok &&
+			    (tp->phy_flags & TG3_PHYFLG_KEEP_LINK_ON_PWRDN) &&
+			    !force_reset)
+				tg3_phy_reset(tp);
 		} else {
 			if (!(bmcr & BMCR_ANENABLE) &&
 			    tp->link_config.speed == current_speed &&
-			    tp->link_config.duplex == current_duplex &&
-			    tp->link_config.flowctrl ==
-			    tp->link_config.active_flowctrl) {
-				current_link_up = 1;
+			    tp->link_config.duplex == current_duplex) {
+				current_link_up = true;
 			}
 		}
 
-		if (current_link_up == 1 &&
+		if (current_link_up &&
 		    tp->link_config.active_duplex == DUPLEX_FULL) {
 			u32 reg, bit;
 
@@ -4473,11 +4809,11 @@ static int tg3_setup_copper_phy(struct tg3 *tp, int force_reset)
 	}
 
 relink:
-	if (current_link_up == 0 || (tp->phy_flags & TG3_PHYFLG_IS_LOW_POWER)) {
+	if (!current_link_up || (tp->phy_flags & TG3_PHYFLG_IS_LOW_POWER)) {
 		tg3_phy_copper_begin(tp);
 
 		if (tg3_flag(tp, ROBOSWITCH)) {
-			current_link_up = 1;
+			current_link_up = true;
 			/* FIXME: when BCM5325 switch is used use 100 MBit/s */
 			current_speed = SPEED_1000;
 			current_duplex = DUPLEX_FULL;
@@ -4488,11 +4824,11 @@ relink:
 		tg3_readphy(tp, MII_BMSR, &bmsr);
 		if ((!tg3_readphy(tp, MII_BMSR, &bmsr) && (bmsr & BMSR_LSTATUS)) ||
 		    (tp->mac_mode & MAC_MODE_PORT_INT_LPBACK))
-			current_link_up = 1;
+			current_link_up = true;
 	}
 
 	tp->mac_mode &= ~MAC_MODE_PORT_MODE_MASK;
-	if (current_link_up == 1) {
+	if (current_link_up) {
 		if (tp->link_config.active_speed == SPEED_100 ||
 		    tp->link_config.active_speed == SPEED_10)
 			tp->mac_mode |= MAC_MODE_PORT_MODE_MII;
@@ -4528,7 +4864,7 @@ relink:
 		tp->mac_mode |= MAC_MODE_HALF_DUPLEX;
 
 	if (tg3_asic_rev(tp) == ASIC_REV_5700) {
-		if (current_link_up == 1 &&
+		if (current_link_up &&
 		    tg3_5700_link_polarity(tp, tp->link_config.active_speed))
 			tp->mac_mode |= MAC_MODE_LINK_POLARITY;
 		else
@@ -4559,7 +4895,7 @@ relink:
 	udelay(40);
 
 	if (tg3_asic_rev(tp) == ASIC_REV_5700 &&
-	    current_link_up == 1 &&
+	    current_link_up &&
 	    tp->link_config.active_speed == SPEED_1000 &&
 	    (tg3_flag(tp, PCIX_MODE) || tg3_flag(tp, PCI_HIGH_SPEED))) {
 		udelay(120);
@@ -4999,19 +5335,19 @@ static void tg3_init_bcm8002(struct tg3 *tp)
 	tg3_writephy(tp, 0x10, 0x8011);
 }
 
-static int tg3_setup_fiber_hw_autoneg(struct tg3 *tp, u32 mac_status)
+static bool tg3_setup_fiber_hw_autoneg(struct tg3 *tp, u32 mac_status)
 {
 	u16 flowctrl;
+	bool current_link_up;
 	u32 sg_dig_ctrl, sg_dig_status;
 	u32 serdes_cfg, expected_sg_dig_ctrl;
 	int workaround, port_a;
-	int current_link_up;
 
 	serdes_cfg = 0;
 	expected_sg_dig_ctrl = 0;
 	workaround = 0;
 	port_a = 1;
-	current_link_up = 0;
+	current_link_up = false;
 
 	if (tg3_chip_rev_id(tp) != CHIPREV_ID_5704_A0 &&
 	    tg3_chip_rev_id(tp) != CHIPREV_ID_5704_A1) {
@@ -5042,7 +5378,7 @@ static int tg3_setup_fiber_hw_autoneg(struct tg3 *tp, u32 mac_status)
 		}
 		if (mac_status & MAC_STATUS_PCS_SYNCED) {
 			tg3_setup_flow_control(tp, 0, 0);
-			current_link_up = 1;
+			current_link_up = true;
 		}
 		goto out;
 	}
@@ -5063,7 +5399,7 @@ static int tg3_setup_fiber_hw_autoneg(struct tg3 *tp, u32 mac_status)
 				    MAC_STATUS_RCVD_CFG)) ==
 		     MAC_STATUS_PCS_SYNCED)) {
 			tp->serdes_counter--;
-			current_link_up = 1;
+			current_link_up = true;
 			goto out;
 		}
 restart_autoneg:
@@ -5098,7 +5434,7 @@ restart_autoneg:
 					   mii_adv_to_ethtool_adv_x(remote_adv);
 
 			tg3_setup_flow_control(tp, local_adv, remote_adv);
-			current_link_up = 1;
+			current_link_up = true;
 			tp->serdes_counter = 0;
 			tp->phy_flags &= ~TG3_PHYFLG_PARALLEL_DETECT;
 		} else if (!(sg_dig_status & SG_DIG_AUTONEG_COMPLETE)) {
@@ -5126,7 +5462,7 @@ restart_autoneg:
 				if ((mac_status & MAC_STATUS_PCS_SYNCED) &&
 				    !(mac_status & MAC_STATUS_RCVD_CFG)) {
 					tg3_setup_flow_control(tp, 0, 0);
-					current_link_up = 1;
+					current_link_up = true;
 					tp->phy_flags |=
 						TG3_PHYFLG_PARALLEL_DETECT;
 					tp->serdes_counter =
@@ -5144,9 +5480,9 @@ out:
 	return current_link_up;
 }
 
-static int tg3_setup_fiber_by_hand(struct tg3 *tp, u32 mac_status)
+static bool tg3_setup_fiber_by_hand(struct tg3 *tp, u32 mac_status)
 {
-	int current_link_up = 0;
+	bool current_link_up = false;
 
 	if (!(mac_status & MAC_STATUS_PCS_SYNCED))
 		goto out;
@@ -5173,7 +5509,7 @@ static int tg3_setup_fiber_by_hand(struct tg3 *tp, u32 mac_status)
 
 			tg3_setup_flow_control(tp, local_adv, remote_adv);
 
-			current_link_up = 1;
+			current_link_up = true;
 		}
 		for (i = 0; i < 30; i++) {
 			udelay(20);
@@ -5188,15 +5524,15 @@ static int tg3_setup_fiber_by_hand(struct tg3 *tp, u32 mac_status)
 		}
 
 		mac_status = tr32(MAC_STATUS);
-		if (current_link_up == 0 &&
+		if (!current_link_up &&
 		    (mac_status & MAC_STATUS_PCS_SYNCED) &&
 		    !(mac_status & MAC_STATUS_RCVD_CFG))
-			current_link_up = 1;
+			current_link_up = true;
 	} else {
 		tg3_setup_flow_control(tp, 0, 0);
 
 		/* Forcing 1000FD link up. */
-		current_link_up = 1;
+		current_link_up = true;
 
 		tw32_f(MAC_MODE, (tp->mac_mode | MAC_MODE_SEND_CONFIGS));
 		udelay(40);
@@ -5209,13 +5545,13 @@ out:
 	return current_link_up;
 }
 
-static int tg3_setup_fiber_phy(struct tg3 *tp, int force_reset)
+static int tg3_setup_fiber_phy(struct tg3 *tp, bool force_reset)
 {
 	u32 orig_pause_cfg;
 	u16 orig_active_speed;
 	u8 orig_active_duplex;
 	u32 mac_status;
-	int current_link_up;
+	bool current_link_up;
 	int i;
 
 	orig_pause_cfg = tp->link_config.active_flowctrl;
@@ -5252,7 +5588,7 @@ static int tg3_setup_fiber_phy(struct tg3 *tp, int force_reset)
 	tw32_f(MAC_EVENT, MAC_EVENT_LNKSTATE_CHANGED);
 	udelay(40);
 
-	current_link_up = 0;
+	current_link_up = false;
 	tp->link_config.rmt_adv = 0;
 	mac_status = tr32(MAC_STATUS);
 
@@ -5277,7 +5613,7 @@ static int tg3_setup_fiber_phy(struct tg3 *tp, int force_reset)
 
 	mac_status = tr32(MAC_STATUS);
 	if ((mac_status & MAC_STATUS_PCS_SYNCED) == 0) {
-		current_link_up = 0;
+		current_link_up = false;
 		if (tp->link_config.autoneg == AUTONEG_ENABLE &&
 		    tp->serdes_counter == 0) {
 			tw32_f(MAC_MODE, (tp->mac_mode |
@@ -5287,7 +5623,7 @@ static int tg3_setup_fiber_phy(struct tg3 *tp, int force_reset)
 		}
 	}
 
-	if (current_link_up == 1) {
+	if (current_link_up) {
 		tp->link_config.active_speed = SPEED_1000;
 		tp->link_config.active_duplex = DUPLEX_FULL;
 		tw32(MAC_LED_CTRL, (tp->led_ctrl |
@@ -5312,33 +5648,63 @@ static int tg3_setup_fiber_phy(struct tg3 *tp, int force_reset)
 	return 0;
 }
 
-static int tg3_setup_fiber_mii_phy(struct tg3 *tp, int force_reset)
+static int tg3_setup_fiber_mii_phy(struct tg3 *tp, bool force_reset)
 {
-	int current_link_up, err = 0;
+	int err = 0;
 	u32 bmsr, bmcr;
-	u16 current_speed;
-	u8 current_duplex;
-	u32 local_adv, remote_adv;
+	u16 current_speed = SPEED_UNKNOWN;
+	u8 current_duplex = DUPLEX_UNKNOWN;
+	bool current_link_up = false;
+	u32 local_adv, remote_adv, sgsr;
+
+	if ((tg3_asic_rev(tp) == ASIC_REV_5719 ||
+	     tg3_asic_rev(tp) == ASIC_REV_5720) &&
+	     !tg3_readphy(tp, SERDES_TG3_1000X_STATUS, &sgsr) &&
+	     (sgsr & SERDES_TG3_SGMII_MODE)) {
+
+		if (force_reset)
+			tg3_phy_reset(tp);
+
+		tp->mac_mode &= ~MAC_MODE_PORT_MODE_MASK;
+
+		if (!(sgsr & SERDES_TG3_LINK_UP)) {
+			tp->mac_mode |= MAC_MODE_PORT_MODE_GMII;
+		} else {
+			current_link_up = true;
+			if (sgsr & SERDES_TG3_SPEED_1000) {
+				current_speed = SPEED_1000;
+				tp->mac_mode |= MAC_MODE_PORT_MODE_GMII;
+			} else if (sgsr & SERDES_TG3_SPEED_100) {
+				current_speed = SPEED_100;
+				tp->mac_mode |= MAC_MODE_PORT_MODE_MII;
+			} else {
+				current_speed = SPEED_10;
+				tp->mac_mode |= MAC_MODE_PORT_MODE_MII;
+			}
+
+			if (sgsr & SERDES_TG3_FULL_DUPLEX)
+				current_duplex = DUPLEX_FULL;
+			else
+				current_duplex = DUPLEX_HALF;
+		}
+
+		tw32_f(MAC_MODE, tp->mac_mode);
+		udelay(40);
+
+		tg3_clear_mac_status(tp);
+
+		goto fiber_setup_done;
+	}
 
 	tp->mac_mode |= MAC_MODE_PORT_MODE_GMII;
 	tw32_f(MAC_MODE, tp->mac_mode);
 	udelay(40);
 
-	tw32(MAC_EVENT, 0);
-
-	tw32_f(MAC_STATUS,
-	     (MAC_STATUS_SYNC_CHANGED |
-	      MAC_STATUS_CFG_CHANGED |
-	      MAC_STATUS_MI_COMPLETION |
-	      MAC_STATUS_LNKSTATE_CHANGED));
-	udelay(40);
+	tg3_clear_mac_status(tp);
 
 	if (force_reset)
 		tg3_phy_reset(tp);
 
-	current_link_up = 0;
-	current_speed = SPEED_UNKNOWN;
-	current_duplex = DUPLEX_UNKNOWN;
 	tp->link_config.rmt_adv = 0;
 
 	err |= tg3_readphy(tp, MII_BMSR, &bmsr);
@@ -5424,7 +5790,7 @@ static int tg3_setup_fiber_mii_phy(struct tg3 *tp, int force_reset)
 
 	if (bmsr & BMSR_LSTATUS) {
 		current_speed = SPEED_1000;
-		current_link_up = 1;
+		current_link_up = true;
 		if (bmcr & BMCR_FULLDPLX)
 			current_duplex = DUPLEX_FULL;
 		else
@@ -5451,12 +5817,13 @@ static int tg3_setup_fiber_mii_phy(struct tg3 *tp, int force_reset)
 			} else if (!tg3_flag(tp, 5780_CLASS)) {
 				/* Link is up via parallel detect */
 			} else {
-				current_link_up = 0;
+				current_link_up = false;
 			}
 		}
 	}
 
-	if (current_link_up == 1 && current_duplex == DUPLEX_FULL)
+fiber_setup_done:
+	if (current_link_up && current_duplex == DUPLEX_FULL)
 		tg3_setup_flow_control(tp, local_adv, remote_adv);
 
 	tp->mac_mode &= ~MAC_MODE_HALF_DUPLEX;
@@ -5535,7 +5902,7 @@ static void tg3_serdes_parallel_detect(struct tg3 *tp)
 	}
 }
 
-static int tg3_setup_phy(struct tg3 *tp, int force_reset)
+static int tg3_setup_phy(struct tg3 *tp, bool force_reset)
 {
 	u32 val;
 	int err;
@@ -5625,10 +5992,13 @@ static int tg3_get_ts_info(struct net_device *dev, struct ethtool_ts_info *info)
 
 	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;
+				SOF_TIMESTAMPING_SOFTWARE;
+
+	if (tg3_flag(tp, PTP_CAPABLE)) {
+		info->so_timestamping |= SOF_TIMESTAMPING_TX_HARDWARE |
+					SOF_TIMESTAMPING_RX_HARDWARE |
+					SOF_TIMESTAMPING_RAW_HARDWARE;
+	}
 
 	if (tp->ptp_clock)
 		info->phc_index = ptp_clock_index(tp->ptp_clock);
@@ -6348,7 +6718,7 @@ static int tg3_rx(struct tg3_napi *tnapi, int budget)
 
 		if (desc->type_flags & RXD_FLAG_VLAN &&
 		    !(tp->rx_mode & RX_MODE_KEEP_VLAN_TAG))
-			__vlan_hwaccel_put_tag(skb,
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
 					       desc->err_vlan & RXD_VLAN_MASK);
 
 		napi_gro_receive(&tnapi->napi, skb);
@@ -6436,7 +6806,7 @@ static void tg3_poll_link(struct tg3 *tp)
 				      MAC_STATUS_LNKSTATE_CHANGED));
 				udelay(40);
 			} else
-				tg3_setup_phy(tp, 0);
+				tg3_setup_phy(tp, false);
 			spin_unlock(&tp->lock);
 		}
 	}
@@ -7533,7 +7903,7 @@ static int tg3_phy_lpbk_set(struct tg3 *tp, u32 speed, bool extlpbk)
 	u32 val, bmcr, mac_mode, ptest = 0;
 
 	tg3_phy_toggle_apd(tp, false);
-	tg3_phy_toggle_automdix(tp, 0);
+	tg3_phy_toggle_automdix(tp, false);
 
 	if (extlpbk && tg3_phy_set_extloopbk(tp))
 		return -EIO;
@@ -7641,7 +8011,7 @@ static void tg3_set_loopback(struct net_device *dev, netdev_features_t features)
 		spin_lock_bh(&tp->lock);
 		tg3_mac_loopback(tp, false);
 		/* Force link status check */
-		tg3_setup_phy(tp, 1);
+		tg3_setup_phy(tp, true);
 		spin_unlock_bh(&tp->lock);
 		netdev_info(dev, "Internal MAC loopback mode disabled.\n");
 	}
@@ -8039,11 +8409,9 @@ static int tg3_mem_rx_acquire(struct tg3 *tp)
 		tnapi->rx_rcb = dma_alloc_coherent(&tp->pdev->dev,
 						   TG3_RX_RCB_RING_BYTES(tp),
 						   &tnapi->rx_rcb_mapping,
-						   GFP_KERNEL);
+						   GFP_KERNEL | __GFP_ZERO);
 		if (!tnapi->rx_rcb)
 			goto err_out;
-
-		memset(tnapi->rx_rcb, 0, TG3_RX_RCB_RING_BYTES(tp));
 	}
 
 	return 0;
@@ -8093,12 +8461,10 @@ static int tg3_alloc_consistent(struct tg3 *tp)
 	tp->hw_stats = dma_alloc_coherent(&tp->pdev->dev,
 					  sizeof(struct tg3_hw_stats),
 					  &tp->stats_mapping,
-					  GFP_KERNEL);
+					  GFP_KERNEL | __GFP_ZERO);
 	if (!tp->hw_stats)
 		goto err_out;
 
-	memset(tp->hw_stats, 0, sizeof(struct tg3_hw_stats));
-
 	for (i = 0; i < tp->irq_cnt; i++) {
 		struct tg3_napi *tnapi = &tp->napi[i];
 		struct tg3_hw_status *sblk;
@@ -8106,11 +8472,10 @@ static int tg3_alloc_consistent(struct tg3 *tp)
 		tnapi->hw_status = dma_alloc_coherent(&tp->pdev->dev,
 						      TG3_HW_STATUS_SIZE,
 						      &tnapi->status_mapping,
-						      GFP_KERNEL);
+						      GFP_KERNEL | __GFP_ZERO);
 		if (!tnapi->hw_status)
 			goto err_out;
 
-		memset(tnapi->hw_status, 0, TG3_HW_STATUS_SIZE);
 		sblk = tnapi->hw_status;
 
 		if (tg3_flag(tp, ENABLE_RSS)) {
@@ -8157,7 +8522,7 @@ err_out:
 /* To stop a block, clear the enable bit and poll till it
  * clears.  tp->lock is held.
  */
-static int tg3_stop_block(struct tg3 *tp, unsigned long ofs, u32 enable_bit, int silent)
+static int tg3_stop_block(struct tg3 *tp, unsigned long ofs, u32 enable_bit, bool silent)
 {
 	unsigned int i;
 	u32 val;
@@ -8201,7 +8566,7 @@ static int tg3_stop_block(struct tg3 *tp, unsigned long ofs, u32 enable_bit, int
 }
 
 /* tp->lock is held. */
-static int tg3_abort_hw(struct tg3 *tp, int silent)
+static int tg3_abort_hw(struct tg3 *tp, bool silent)
 {
 	int i, err;
 
@@ -8561,6 +8926,9 @@ static int tg3_chip_reset(struct tg3 *tp)
 
 	/* Reprobe ASF enable state.  */
 	tg3_flag_clear(tp, ENABLE_ASF);
+	tp->phy_flags &= ~(TG3_PHYFLG_1G_ON_VAUX_OK |
+			   TG3_PHYFLG_KEEP_LINK_ON_PWRDN);
+
 	tg3_flag_clear(tp, ASF_NEW_HANDSHAKE);
 	tg3_read_mem(tp, NIC_SRAM_DATA_SIG, &val);
 	if (val == NIC_SRAM_DATA_SIG_MAGIC) {
@@ -8572,6 +8940,12 @@ static int tg3_chip_reset(struct tg3 *tp)
 			tp->last_event_jiffies = jiffies;
 			if (tg3_flag(tp, 5750_PLUS))
 				tg3_flag_set(tp, ASF_NEW_HANDSHAKE);
+
+			tg3_read_mem(tp, NIC_SRAM_DATA_CFG_3, &nic_cfg);
+			if (nic_cfg & NIC_SRAM_1G_ON_VAUX_OK)
+				tp->phy_flags |= TG3_PHYFLG_1G_ON_VAUX_OK;
+			if (nic_cfg & NIC_SRAM_LNK_FLAP_AVOID)
+				tp->phy_flags |= TG3_PHYFLG_KEEP_LINK_ON_PWRDN;
 		}
 	}
 
@@ -8582,7 +8956,7 @@ static void tg3_get_nstats(struct tg3 *, struct rtnl_link_stats64 *);
 static void tg3_get_estats(struct tg3 *, struct tg3_ethtool_stats *);
 
 /* tp->lock is held. */
-static int tg3_halt(struct tg3 *tp, int kind, int silent)
+static int tg3_halt(struct tg3 *tp, int kind, bool silent)
 {
 	int err;
 
@@ -8593,7 +8967,7 @@ static int tg3_halt(struct tg3 *tp, int kind, int silent)
 	tg3_abort_hw(tp, silent);
 	err = tg3_chip_reset(tp);
 
-	__tg3_set_mac_addr(tp, 0);
+	__tg3_set_mac_addr(tp, false);
 
 	tg3_write_sig_legacy(tp, kind);
 	tg3_write_sig_post_reset(tp, kind);
@@ -8617,7 +8991,8 @@ static int tg3_set_mac_addr(struct net_device *dev, void *p)
 {
 	struct tg3 *tp = netdev_priv(dev);
 	struct sockaddr *addr = p;
-	int err = 0, skip_mac_1 = 0;
+	int err = 0;
+	bool skip_mac_1 = false;
 
 	if (!is_valid_ether_addr(addr->sa_data))
 		return -EADDRNOTAVAIL;
@@ -8638,7 +9013,7 @@ static int tg3_set_mac_addr(struct net_device *dev, void *p)
 		/* Skip MAC addr 1 if ASF is using it. */
 		if ((addr0_high != addr1_high || addr0_low != addr1_low) &&
 		    !(addr1_high == 0 && addr1_low == 0))
-			skip_mac_1 = 1;
+			skip_mac_1 = true;
 	}
 	spin_lock_bh(&tp->lock);
 	__tg3_set_mac_addr(tp, skip_mac_1);
@@ -9057,7 +9432,7 @@ static void tg3_rss_write_indir_tbl(struct tg3 *tp)
 }
 
 /* tp->lock is held. */
-static int tg3_reset_hw(struct tg3 *tp, int reset_phy)
+static int tg3_reset_hw(struct tg3 *tp, bool reset_phy)
 {
 	u32 val, rdmac_mode;
 	int i, err, limit;
@@ -9106,6 +9481,12 @@ static int tg3_reset_hw(struct tg3 *tp, int reset_phy)
 		       TG3_CPMU_DBTMR2_TXIDXEQ_2047US);
 	}
 
+	if ((tp->phy_flags & TG3_PHYFLG_KEEP_LINK_ON_PWRDN) &&
+	    !(tp->phy_flags & TG3_PHYFLG_USER_CONFIGURED)) {
+		tg3_phy_pull_config(tp);
+		tp->phy_flags |= TG3_PHYFLG_USER_CONFIGURED;
+	}
+
 	if (reset_phy)
 		tg3_phy_reset(tp);
 
@@ -9444,7 +9825,7 @@ static int tg3_reset_hw(struct tg3 *tp, int reset_phy)
 	tg3_rings_reset(tp);
 
 	/* Initialize MAC address and backoff seed. */
-	__tg3_set_mac_addr(tp, 0);
+	__tg3_set_mac_addr(tp, false);
 
 	/* MTU + ethernet header + FCS + optional VLAN tag */
 	tw32(MAC_RX_MTU_SIZE,
@@ -9781,6 +10162,13 @@ static int tg3_reset_hw(struct tg3 *tp, int reset_phy)
 			return err;
 	}
 
+	if (tg3_asic_rev(tp) == ASIC_REV_57766) {
+		/* Ignore any errors for the firmware download. If download
+		 * fails, the device will operate with EEE disabled
+		 */
+		tg3_load_57766_firmware(tp);
+	}
+
 	if (tg3_flag(tp, TSO_CAPABLE)) {
 		err = tg3_load_tso_firmware(tp);
 		if (err)
@@ -9888,7 +10276,7 @@ static int tg3_reset_hw(struct tg3 *tp, int reset_phy)
 		if (tp->phy_flags & TG3_PHYFLG_IS_LOW_POWER)
 			tp->phy_flags &= ~TG3_PHYFLG_IS_LOW_POWER;
 
-		err = tg3_setup_phy(tp, 0);
+		err = tg3_setup_phy(tp, false);
 		if (err)
 			return err;
 
@@ -9968,7 +10356,7 @@ static int tg3_reset_hw(struct tg3 *tp, int reset_phy)
 /* Called at device open time to get the chip ready for
  * packet processing.  Invoked with tp->lock held.
  */
-static int tg3_init_hw(struct tg3 *tp, int reset_phy)
+static int tg3_init_hw(struct tg3 *tp, bool reset_phy)
 {
 	tg3_switch_clocks(tp);
 
@@ -10229,7 +10617,7 @@ static void tg3_timer(unsigned long __opaque)
 				phy_event = 1;
 
 			if (phy_event)
-				tg3_setup_phy(tp, 0);
+				tg3_setup_phy(tp, false);
 		} else if (tg3_flag(tp, POLL_SERDES)) {
 			u32 mac_stat = tr32(MAC_STATUS);
 			int need_setup = 0;
@@ -10252,7 +10640,7 @@ static void tg3_timer(unsigned long __opaque)
 					tw32_f(MAC_MODE, tp->mac_mode);
 					udelay(40);
 				}
-				tg3_setup_phy(tp, 0);
+				tg3_setup_phy(tp, false);
 			}
 		} else if ((tp->phy_flags & TG3_PHYFLG_MII_SERDES) &&
 			   tg3_flag(tp, 5780_CLASS)) {
@@ -10338,7 +10726,7 @@ static void tg3_timer_stop(struct tg3 *tp)
 /* Restart hardware after configuration changes, self-test, etc.
  * Invoked with tp->lock held.
  */
-static int tg3_restart_hw(struct tg3 *tp, int reset_phy)
+static int tg3_restart_hw(struct tg3 *tp, bool reset_phy)
 	__releases(tp->lock)
 	__acquires(tp->lock)
 {
@@ -10388,7 +10776,7 @@ static void tg3_reset_task(struct work_struct *work)
 	}
 
 	tg3_halt(tp, RESET_KIND_SHUTDOWN, 0);
-	err = tg3_init_hw(tp, 1);
+	err = tg3_init_hw(tp, true);
 	if (err)
 		goto out;
 
@@ -10558,7 +10946,7 @@ static int tg3_test_msi(struct tg3 *tp)
 	tg3_full_lock(tp, 1);
 
 	tg3_halt(tp, RESET_KIND_SHUTDOWN, 1);
-	err = tg3_init_hw(tp, 1);
+	err = tg3_init_hw(tp, true);
 
 	tg3_full_unlock(tp);
 
@@ -10570,7 +10958,7 @@ static int tg3_test_msi(struct tg3 *tp)
 
 static int tg3_request_firmware(struct tg3 *tp)
 {
-	const __be32 *fw_data;
+	const struct tg3_firmware_hdr *fw_hdr;
 
 	if (request_firmware(&tp->fw, tp->fw_needed, &tp->pdev->dev)) {
 		netdev_err(tp->dev, "Failed to load firmware \"%s\"\n",
@@ -10578,15 +10966,15 @@ static int tg3_request_firmware(struct tg3 *tp)
 		return -ENOENT;
 	}
 
-	fw_data = (void *)tp->fw->data;
+	fw_hdr = (struct tg3_firmware_hdr *)tp->fw->data;
 
 	/* Firmware blob starts with version numbers, followed by
 	 * start address and _full_ length including BSS sections
 	 * (which must be longer than the actual data, of course
 	 */
 
-	tp->fw_len = be32_to_cpu(fw_data[2]);	/* includes bss */
-	if (tp->fw_len < (tp->fw->size - 12)) {
+	tp->fw_len = be32_to_cpu(fw_hdr->len);	/* includes bss */
+	if (tp->fw_len < (tp->fw->size - TG3_FW_HDR_LEN)) {
 		netdev_err(tp->dev, "bogus length %d in \"%s\"\n",
 			   tp->fw_len, tp->fw_needed);
 		release_firmware(tp->fw);
@@ -10885,7 +11273,15 @@ static int tg3_open(struct net_device *dev)
 
 	if (tp->fw_needed) {
 		err = tg3_request_firmware(tp);
-		if (tg3_chip_rev_id(tp) == CHIPREV_ID_5701_A0) {
+		if (tg3_asic_rev(tp) == ASIC_REV_57766) {
+			if (err) {
+				netdev_warn(tp->dev, "EEE capability disabled\n");
+				tp->phy_flags &= ~TG3_PHYFLG_EEE_CAP;
+			} else if (!(tp->phy_flags & TG3_PHYFLG_EEE_CAP)) {
+				netdev_warn(tp->dev, "EEE capability restored\n");
+				tp->phy_flags |= TG3_PHYFLG_EEE_CAP;
+			}
+		} else if (tg3_chip_rev_id(tp) == CHIPREV_ID_5701_A0) {
 			if (err)
 				return err;
 		} else if (err) {
@@ -10910,7 +11306,9 @@ static int tg3_open(struct net_device *dev)
 
 	tg3_full_unlock(tp);
 
-	err = tg3_start(tp, true, true, true);
+	err = tg3_start(tp,
+			!(tp->phy_flags & TG3_PHYFLG_KEEP_LINK_ON_PWRDN),
+			true, true);
 	if (err) {
 		tg3_frob_aux_power(tp, false);
 		pci_set_power_state(tp->pdev, PCI_D3hot);
@@ -11416,8 +11814,12 @@ static int tg3_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 		tp->link_config.duplex = cmd->duplex;
 	}
 
+	tp->phy_flags |= TG3_PHYFLG_USER_CONFIGURED;
+
+	tg3_warn_mgmt_link_flap(tp);
+
 	if (netif_running(dev))
-		tg3_setup_phy(tp, 1);
+		tg3_setup_phy(tp, true);
 
 	tg3_full_unlock(tp);
 
@@ -11494,6 +11896,8 @@ static int tg3_nway_reset(struct net_device *dev)
 	if (tp->phy_flags & TG3_PHYFLG_PHY_SERDES)
 		return -EINVAL;
 
+	tg3_warn_mgmt_link_flap(tp);
+
 	if (tg3_flag(tp, USE_PHYLIB)) {
 		if (!(tp->phy_flags & TG3_PHYFLG_IS_CONNECTED))
 			return -EAGAIN;
@@ -11571,7 +11975,7 @@ static int tg3_set_ringparam(struct net_device *dev, struct ethtool_ringparam *e
 
 	if (netif_running(dev)) {
 		tg3_halt(tp, RESET_KIND_SHUTDOWN, 1);
-		err = tg3_restart_hw(tp, 1);
+		err = tg3_restart_hw(tp, false);
 		if (!err)
 			tg3_netif_start(tp);
 	}
@@ -11606,6 +12010,9 @@ static int tg3_set_pauseparam(struct net_device *dev, struct ethtool_pauseparam
 	struct tg3 *tp = netdev_priv(dev);
 	int err = 0;
 
+	if (tp->link_config.autoneg == AUTONEG_ENABLE)
+		tg3_warn_mgmt_link_flap(tp);
+
 	if (tg3_flag(tp, USE_PHYLIB)) {
 		u32 newadv;
 		struct phy_device *phydev;
@@ -11692,7 +12099,7 @@ static int tg3_set_pauseparam(struct net_device *dev, struct ethtool_pauseparam
 
 		if (netif_running(dev)) {
 			tg3_halt(tp, RESET_KIND_SHUTDOWN, 1);
-			err = tg3_restart_hw(tp, 1);
+			err = tg3_restart_hw(tp, false);
 			if (!err)
 				tg3_netif_start(tp);
 		}
@@ -11700,6 +12107,8 @@ static int tg3_set_pauseparam(struct net_device *dev, struct ethtool_pauseparam
 		tg3_full_unlock(tp);
 	}
 
+	tp->phy_flags |= TG3_PHYFLG_USER_CONFIGURED;
+
 	return err;
 }
 
@@ -12760,7 +13169,7 @@ static int tg3_test_loopback(struct tg3 *tp, u64 *data, bool do_extlpbk)
 		goto done;
 	}
 
-	err = tg3_reset_hw(tp, 1);
+	err = tg3_reset_hw(tp, true);
 	if (err) {
 		data[TG3_MAC_LOOPB_TEST] = TG3_LOOPBACK_FAILED;
 		data[TG3_PHY_LOOPB_TEST] = TG3_LOOPBACK_FAILED;
@@ -12927,7 +13336,7 @@ static void tg3_self_test(struct net_device *dev, struct ethtool_test *etest,
 		tg3_halt(tp, RESET_KIND_SHUTDOWN, 1);
 		if (netif_running(dev)) {
 			tg3_flag_set(tp, INIT_COMPLETE);
-			err2 = tg3_restart_hw(tp, 1);
+			err2 = tg3_restart_hw(tp, true);
 			if (!err2)
 				tg3_netif_start(tp);
 		}
@@ -13244,7 +13653,8 @@ static inline void tg3_set_mtu(struct net_device *dev, struct tg3 *tp,
 static int tg3_change_mtu(struct net_device *dev, int new_mtu)
 {
 	struct tg3 *tp = netdev_priv(dev);
-	int err, reset_phy = 0;
+	int err;
+	bool reset_phy = false;
 
 	if (new_mtu < TG3_MIN_MTU || new_mtu > TG3_MAX_MTU(tp))
 		return -EINVAL;
@@ -13271,7 +13681,7 @@ static int tg3_change_mtu(struct net_device *dev, int new_mtu)
 	 * breaks all requests to 256 bytes.
 	 */
 	if (tg3_asic_rev(tp) == ASIC_REV_57766)
-		reset_phy = 1;
+		reset_phy = true;
 
 	err = tg3_restart_hw(tp, reset_phy);
 
@@ -13837,6 +14247,12 @@ static void tg3_get_5720_nvram_info(struct tg3 *tp)
 		case FLASH_5762_EEPROM_LD:
 			nvmpinstrp = FLASH_5720_EEPROM_LD;
 			break;
+		case FLASH_5720VENDOR_M_ST_M45PE20:
+			/* This pinstrap supports multiple sizes, so force it
+			 * to read the actual size from location 0xf0.
+			 */
+			nvmpinstrp = FLASH_5720VENDOR_ST_45USPT;
+			break;
 		}
 	}
 
@@ -14289,14 +14705,18 @@ static void tg3_get_eeprom_hw_cfg(struct tg3 *tp)
 		    (cfg2 & NIC_SRAM_DATA_CFG_2_APD_EN))
 			tp->phy_flags |= TG3_PHYFLG_ENABLE_APD;
 
-		if (tg3_flag(tp, PCI_EXPRESS) &&
-		    tg3_asic_rev(tp) != ASIC_REV_5785 &&
-		    !tg3_flag(tp, 57765_PLUS)) {
+		if (tg3_flag(tp, PCI_EXPRESS)) {
 			u32 cfg3;
 
 			tg3_read_mem(tp, NIC_SRAM_DATA_CFG_3, &cfg3);
-			if (cfg3 & NIC_SRAM_ASPM_DEBOUNCE)
+			if (tg3_asic_rev(tp) != ASIC_REV_5785 &&
+			    !tg3_flag(tp, 57765_PLUS) &&
+			    (cfg3 & NIC_SRAM_ASPM_DEBOUNCE))
 				tg3_flag_set(tp, ASPM_WORKAROUND);
+			if (cfg3 & NIC_SRAM_LNK_FLAP_AVOID)
+				tp->phy_flags |= TG3_PHYFLG_KEEP_LINK_ON_PWRDN;
+			if (cfg3 & NIC_SRAM_1G_ON_VAUX_OK)
+				tp->phy_flags |= TG3_PHYFLG_1G_ON_VAUX_OK;
 		}
 
 		if (cfg4 & NIC_SRAM_RGMII_INBAND_DISABLE)
@@ -14450,6 +14870,12 @@ static int tg3_phy_probe(struct tg3 *tp)
 		}
 	}
 
+	if (!tg3_flag(tp, ENABLE_ASF) &&
+	    !(tp->phy_flags & TG3_PHYFLG_ANY_SERDES) &&
+	    !(tp->phy_flags & TG3_PHYFLG_10_100_ONLY))
+		tp->phy_flags &= ~(TG3_PHYFLG_1G_ON_VAUX_OK |
+				   TG3_PHYFLG_KEEP_LINK_ON_PWRDN);
+
 	if (tg3_flag(tp, USE_PHYLIB))
 		return tg3_phy_init(tp);
 
@@ -14515,6 +14941,7 @@ static int tg3_phy_probe(struct tg3 *tp)
 	if (!(tp->phy_flags & TG3_PHYFLG_ANY_SERDES) &&
 	    (tg3_asic_rev(tp) == ASIC_REV_5719 ||
 	     tg3_asic_rev(tp) == ASIC_REV_5720 ||
+	     tg3_asic_rev(tp) == ASIC_REV_57766 ||
 	     tg3_asic_rev(tp) == ASIC_REV_5762 ||
 	     (tg3_asic_rev(tp) == ASIC_REV_5717 &&
 	      tg3_chip_rev_id(tp) != CHIPREV_ID_5717_A0) ||
@@ -14524,7 +14951,8 @@ static int tg3_phy_probe(struct tg3 *tp)
 
 	tg3_phy_init_link_config(tp);
 
-	if (!(tp->phy_flags & TG3_PHYFLG_ANY_SERDES) &&
+	if (!(tp->phy_flags & TG3_PHYFLG_KEEP_LINK_ON_PWRDN) &&
+	    !(tp->phy_flags & TG3_PHYFLG_ANY_SERDES) &&
 	    !tg3_flag(tp, ENABLE_APE) &&
 	    !tg3_flag(tp, ENABLE_ASF)) {
 		u32 bmsr, dummy;
@@ -15300,7 +15728,8 @@ static int tg3_get_invariants(struct tg3 *tp, const struct pci_device_id *ent)
 	} else if (tg3_asic_rev(tp) != ASIC_REV_5700 &&
 		   tg3_asic_rev(tp) != ASIC_REV_5701 &&
 		   tg3_chip_rev_id(tp) != CHIPREV_ID_5705_A0) {
-			tg3_flag_set(tp, TSO_BUG);
+		tg3_flag_set(tp, FW_TSO);
+		tg3_flag_set(tp, TSO_BUG);
 		if (tg3_asic_rev(tp) == ASIC_REV_5705)
 			tp->fw_needed = FIRMWARE_TG3TSO5;
 		else
@@ -15311,7 +15740,7 @@ static int tg3_get_invariants(struct tg3 *tp, const struct pci_device_id *ent)
 	if (tg3_flag(tp, HW_TSO_1) ||
 	    tg3_flag(tp, HW_TSO_2) ||
 	    tg3_flag(tp, HW_TSO_3) ||
-	    tp->fw_needed) {
+	    tg3_flag(tp, FW_TSO)) {
 		/* For firmware TSO, assume ASF is disabled.
 		 * We'll disable TSO later if we discover ASF
 		 * is enabled in tg3_get_eeprom_hw_cfg().
@@ -15326,6 +15755,9 @@ static int tg3_get_invariants(struct tg3 *tp, const struct pci_device_id *ent)
 	if (tg3_chip_rev_id(tp) == CHIPREV_ID_5701_A0)
 		tp->fw_needed = FIRMWARE_TG3;
 
+	if (tg3_asic_rev(tp) == ASIC_REV_57766)
+		tp->fw_needed = FIRMWARE_TG357766;
+
 	tp->irq_max = 1;
 
 	if (tg3_flag(tp, 5750_PLUS)) {
@@ -15598,7 +16030,7 @@ static int tg3_get_invariants(struct tg3 *tp, const struct pci_device_id *ent)
 	 */
 	tg3_get_eeprom_hw_cfg(tp);
 
-	if (tp->fw_needed && tg3_flag(tp, ENABLE_ASF)) {
+	if (tg3_flag(tp, FW_TSO) && tg3_flag(tp, ENABLE_ASF)) {
 		tg3_flag_clear(tp, TSO_CAPABLE);
 		tg3_flag_clear(tp, TSO_BUG);
 		tp->fw_needed = NULL;
@@ -15786,6 +16218,11 @@ static int tg3_get_invariants(struct tg3 *tp, const struct pci_device_id *ent)
 	udelay(50);
 	tg3_nvram_init(tp);
 
+	/* If the device has an NVRAM, no need to load patch firmware */
+	if (tg3_asic_rev(tp) == ASIC_REV_57766 &&
+	    !tg3_flag(tp, NO_NVRAM))
+		tp->fw_needed = NULL;
+
 	grc_misc_cfg = tr32(GRC_MISC_CFG);
 	grc_misc_cfg &= GRC_MISC_CFG_BOARD_ID_MASK;
 
@@ -16144,7 +16581,7 @@ out:
 }
 
 static int tg3_do_test_dma(struct tg3 *tp, u32 *buf, dma_addr_t buf_dma,
-			   int size, int to_device)
+			   int size, bool to_device)
 {
 	struct tg3_internal_buffer_desc test_desc;
 	u32 sram_dma_descs;
@@ -16344,7 +16781,7 @@ static int tg3_test_dma(struct tg3 *tp)
 			p[i] = i;
 
 		/* Send the buffer to the chip. */
-		ret = tg3_do_test_dma(tp, buf, buf_dma, TEST_BUFFER_SIZE, 1);
+		ret = tg3_do_test_dma(tp, buf, buf_dma, TEST_BUFFER_SIZE, true);
 		if (ret) {
 			dev_err(&tp->pdev->dev,
 				"%s: Buffer write failed. err = %d\n",
@@ -16367,7 +16804,7 @@ static int tg3_test_dma(struct tg3 *tp)
 		}
 #endif
 		/* Now read it back. */
-		ret = tg3_do_test_dma(tp, buf, buf_dma, TEST_BUFFER_SIZE, 0);
+		ret = tg3_do_test_dma(tp, buf, buf_dma, TEST_BUFFER_SIZE, false);
 		if (ret) {
 			dev_err(&tp->pdev->dev, "%s: Buffer read failed. "
 				"err = %d\n", __func__, ret);
@@ -16763,7 +17200,7 @@ static int tg3_init_one(struct pci_dev *pdev,
 
 	tg3_init_bufmgr_config(tp);
 
-	features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
+	features |= NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX;
 
 	/* 5700 B0 chips do not support checksumming correctly due
 	 * to hardware bugs.
@@ -17048,7 +17485,7 @@ static int tg3_suspend(struct device *device)
 		tg3_full_lock(tp, 0);
 
 		tg3_flag_set(tp, INIT_COMPLETE);
-		err2 = tg3_restart_hw(tp, 1);
+		err2 = tg3_restart_hw(tp, true);
 		if (err2)
 			goto out;
 
@@ -17082,7 +17519,8 @@ static int tg3_resume(struct device *device)
 	tg3_full_lock(tp, 0);
 
 	tg3_flag_set(tp, INIT_COMPLETE);
-	err = tg3_restart_hw(tp, 1);
+	err = tg3_restart_hw(tp,
+			     !(tp->phy_flags & TG3_PHYFLG_KEEP_LINK_ON_PWRDN));
 	if (err)
 		goto out;
 
@@ -17098,15 +17536,9 @@ out:
 
 	return err;
 }
+#endif /* CONFIG_PM_SLEEP */
 
 static SIMPLE_DEV_PM_OPS(tg3_pm_ops, tg3_suspend, tg3_resume);
-#define TG3_PM_OPS (&tg3_pm_ops)
-
-#else
-
-#define TG3_PM_OPS NULL
-
-#endif /* CONFIG_PM_SLEEP */
 
 /**
  * tg3_io_error_detected - called when PCI error is detected
@@ -17221,7 +17653,7 @@ static void tg3_io_resume(struct pci_dev *pdev)
 
 	tg3_full_lock(tp, 0);
 	tg3_flag_set(tp, INIT_COMPLETE);
-	err = tg3_restart_hw(tp, 1);
+	err = tg3_restart_hw(tp, true);
 	if (err) {
 		tg3_full_unlock(tp);
 		netdev_err(netdev, "Cannot restart hardware after reset.\n");
@@ -17254,7 +17686,7 @@ static struct pci_driver tg3_driver = {
 	.probe		= tg3_init_one,
 	.remove		= tg3_remove_one,
 	.err_handler	= &tg3_err_handler,
-	.driver.pm	= TG3_PM_OPS,
+	.driver.pm	= &tg3_pm_ops,
 };
 
 static int __init tg3_init(void)
diff --git a/drivers/net/ethernet/broadcom/tg3.h b/drivers/net/ethernet/broadcom/tg3.h
index 8d7d4c2ab5d6..9b2d3ac2474a 100644
--- a/drivers/net/ethernet/broadcom/tg3.h
+++ b/drivers/net/ethernet/broadcom/tg3.h
@@ -2198,6 +2198,8 @@
 
 #define NIC_SRAM_DATA_CFG_3		0x00000d3c
 #define  NIC_SRAM_ASPM_DEBOUNCE		 0x00000002
+#define  NIC_SRAM_LNK_FLAP_AVOID	 0x00400000
+#define  NIC_SRAM_1G_ON_VAUX_OK		 0x00800000
 
 #define NIC_SRAM_DATA_CFG_4		0x00000d60
 #define  NIC_SRAM_GMII_MODE		 0x00000002
@@ -2222,6 +2224,12 @@
 #define  NIC_SRAM_MBUF_POOL_BASE5705	0x00010000
 #define  NIC_SRAM_MBUF_POOL_SIZE5705	0x0000e000
 
+#define TG3_SRAM_RXCPU_SCRATCH_BASE_57766	0x00030000
+#define  TG3_SRAM_RXCPU_SCRATCH_SIZE_57766	 0x00010000
+#define TG3_57766_FW_BASE_ADDR			0x00030000
+#define TG3_57766_FW_HANDSHAKE			0x0003fccc
+#define TG3_SBROM_IN_SERVICE_LOOP		0x51
+
 #define TG3_SRAM_RX_STD_BDCACHE_SIZE_5700	128
 #define TG3_SRAM_RX_STD_BDCACHE_SIZE_5755	64
 #define TG3_SRAM_RX_STD_BDCACHE_SIZE_5906	32
@@ -2365,6 +2373,13 @@
 #define MII_TG3_FET_SHDW_AUXSTAT2	0x1b
 #define  MII_TG3_FET_SHDW_AUXSTAT2_APD	0x0020
 
+/* Serdes PHY Register Definitions */
+#define SERDES_TG3_1000X_STATUS		0x14
+#define  SERDES_TG3_SGMII_MODE		 0x0001
+#define  SERDES_TG3_LINK_UP		 0x0002
+#define  SERDES_TG3_FULL_DUPLEX		 0x0004
+#define  SERDES_TG3_SPEED_100		 0x0008
+#define  SERDES_TG3_SPEED_1000		 0x0010
 
 /* APE registers.  Accessible through BAR1 */
 #define TG3_APE_GPIO_MSG		0x0008
@@ -3009,17 +3024,18 @@ enum TG3_FLAGS {
 	TG3_FLAG_JUMBO_CAPABLE,
 	TG3_FLAG_CHIP_RESETTING,
 	TG3_FLAG_INIT_COMPLETE,
-	TG3_FLAG_TSO_BUG,
 	TG3_FLAG_MAX_RXPEND_64,
-	TG3_FLAG_TSO_CAPABLE,
 	TG3_FLAG_PCI_EXPRESS, /* BCM5785 + pci_is_pcie() */
 	TG3_FLAG_ASF_NEW_HANDSHAKE,
 	TG3_FLAG_HW_AUTONEG,
 	TG3_FLAG_IS_NIC,
 	TG3_FLAG_FLASH,
+	TG3_FLAG_FW_TSO,
 	TG3_FLAG_HW_TSO_1,
 	TG3_FLAG_HW_TSO_2,
 	TG3_FLAG_HW_TSO_3,
+	TG3_FLAG_TSO_CAPABLE,
+	TG3_FLAG_TSO_BUG,
 	TG3_FLAG_ICH_WORKAROUND,
 	TG3_FLAG_1SHOT_MSI,
 	TG3_FLAG_NO_FWARE_REPORTED,
@@ -3064,6 +3080,13 @@ enum TG3_FLAGS {
 	TG3_FLAG_NUMBER_OF_FLAGS,	/* Last entry in enum TG3_FLAGS */
 };
 
+struct tg3_firmware_hdr {
+	__be32 version; /* unused for fragments */
+	__be32 base_addr;
+	__be32 len;
+};
+#define TG3_FW_HDR_LEN         (sizeof(struct tg3_firmware_hdr))
+
 struct tg3 {
 	/* begin "general, frequently-used members" cacheline section */
 
@@ -3267,6 +3290,7 @@ struct tg3 {
 #define TG3_PHYFLG_IS_LOW_POWER		0x00000001
 #define TG3_PHYFLG_IS_CONNECTED		0x00000002
 #define TG3_PHYFLG_USE_MI_INTERRUPT	0x00000004
+#define TG3_PHYFLG_USER_CONFIGURED	0x00000008
 #define TG3_PHYFLG_PHY_SERDES		0x00000010
 #define TG3_PHYFLG_MII_SERDES		0x00000020
 #define TG3_PHYFLG_ANY_SERDES		(TG3_PHYFLG_PHY_SERDES |	\
@@ -3284,6 +3308,8 @@ struct tg3 {
 #define TG3_PHYFLG_SERDES_PREEMPHASIS	0x00010000
 #define TG3_PHYFLG_PARALLEL_DETECT	0x00020000
 #define TG3_PHYFLG_EEE_CAP		0x00040000
+#define TG3_PHYFLG_1G_ON_VAUX_OK	0x00080000
+#define TG3_PHYFLG_KEEP_LINK_ON_PWRDN	0x00100000
 #define TG3_PHYFLG_MDIX_STATE		0x00200000
 
 	u32				led_ctrl;
diff --git a/drivers/net/ethernet/brocade/bna/bfa_ioc.c b/drivers/net/ethernet/brocade/bna/bfa_ioc.c
index 3227fdde521b..f2b73ffa9122 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_ioc.c
+++ b/drivers/net/ethernet/brocade/bna/bfa_ioc.c
@@ -76,7 +76,7 @@ static void bfa_ioc_pf_disabled(struct bfa_ioc *ioc);
 static void bfa_ioc_pf_failed(struct bfa_ioc *ioc);
 static void bfa_ioc_pf_hwfailed(struct bfa_ioc *ioc);
 static void bfa_ioc_pf_fwmismatch(struct bfa_ioc *ioc);
-static void bfa_ioc_boot(struct bfa_ioc *ioc, u32 boot_type,
+static void bfa_ioc_boot(struct bfa_ioc *ioc, enum bfi_fwboot_type boot_type,
 			 u32 boot_param);
 static u32 bfa_ioc_smem_pgnum(struct bfa_ioc *ioc, u32 fmaddr);
 static void bfa_ioc_get_adapter_serial_num(struct bfa_ioc *ioc,
diff --git a/drivers/net/ethernet/brocade/bna/bnad.c b/drivers/net/ethernet/brocade/bna/bnad.c
index 7cce42dc2f20..ce4a030d3d0c 100644
--- a/drivers/net/ethernet/brocade/bna/bnad.c
+++ b/drivers/net/ethernet/brocade/bna/bnad.c
@@ -610,7 +610,7 @@ bnad_cq_process(struct bnad *bnad, struct bna_ccb *ccb, int budget)
 		rcb->rxq->rx_bytes += length;
 
 		if (flags & BNA_CQ_EF_VLAN)
-			__vlan_hwaccel_put_tag(skb, ntohs(cmpl->vlan_tag));
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), ntohs(cmpl->vlan_tag));
 
 		if (BNAD_RXBUF_IS_PAGE(unmap_q->type))
 			napi_gro_frags(&rx_ctrl->napi);
@@ -1264,9 +1264,8 @@ bnad_mem_alloc(struct bnad *bnad,
 			mem_info->mdl[i].len = mem_info->len;
 			mem_info->mdl[i].kva =
 				dma_alloc_coherent(&bnad->pcidev->dev,
-						mem_info->len, &dma_pa,
-						GFP_KERNEL);
-
+						   mem_info->len, &dma_pa,
+						   GFP_KERNEL);
 			if (mem_info->mdl[i].kva == NULL)
 				goto err_return;
 
@@ -3069,8 +3068,7 @@ bnad_change_mtu(struct net_device *netdev, int new_mtu)
 }
 
 static int
-bnad_vlan_rx_add_vid(struct net_device *netdev,
-				 unsigned short vid)
+bnad_vlan_rx_add_vid(struct net_device *netdev, __be16 proto, u16 vid)
 {
 	struct bnad *bnad = netdev_priv(netdev);
 	unsigned long flags;
@@ -3091,8 +3089,7 @@ bnad_vlan_rx_add_vid(struct net_device *netdev,
 }
 
 static int
-bnad_vlan_rx_kill_vid(struct net_device *netdev,
-				  unsigned short vid)
+bnad_vlan_rx_kill_vid(struct net_device *netdev, __be16 proto, u16 vid)
 {
 	struct bnad *bnad = netdev_priv(netdev);
 	unsigned long flags;
@@ -3171,14 +3168,14 @@ bnad_netdev_init(struct bnad *bnad, bool using_dac)
 
 	netdev->hw_features = NETIF_F_SG | NETIF_F_RXCSUM |
 		NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
-		NETIF_F_TSO | NETIF_F_TSO6 | NETIF_F_HW_VLAN_TX;
+		NETIF_F_TSO | NETIF_F_TSO6 | NETIF_F_HW_VLAN_CTAG_TX;
 
 	netdev->vlan_features = NETIF_F_SG | NETIF_F_HIGHDMA |
 		NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
 		NETIF_F_TSO | NETIF_F_TSO6;
 
 	netdev->features |= netdev->hw_features |
-		NETIF_F_HW_VLAN_RX | NETIF_F_HW_VLAN_FILTER;
+		NETIF_F_HW_VLAN_CTAG_RX | NETIF_F_HW_VLAN_CTAG_FILTER;
 
 	if (using_dac)
 		netdev->features |= NETIF_F_HIGHDMA;
diff --git a/drivers/net/ethernet/cadence/at91_ether.c b/drivers/net/ethernet/cadence/at91_ether.c
index 3becdb2deb46..cc9a185f0abb 100644
--- a/drivers/net/ethernet/cadence/at91_ether.c
+++ b/drivers/net/ethernet/cadence/at91_ether.c
@@ -47,22 +47,19 @@ static int at91ether_start(struct net_device *dev)
 	int i;
 
 	lp->rx_ring = dma_alloc_coherent(&lp->pdev->dev,
-					MAX_RX_DESCR * sizeof(struct macb_dma_desc),
-					&lp->rx_ring_dma, GFP_KERNEL);
-	if (!lp->rx_ring) {
-		netdev_err(dev, "unable to alloc rx ring DMA buffer\n");
+					 (MAX_RX_DESCR *
+					  sizeof(struct macb_dma_desc)),
+					 &lp->rx_ring_dma, GFP_KERNEL);
+	if (!lp->rx_ring)
 		return -ENOMEM;
-	}
 
 	lp->rx_buffers = dma_alloc_coherent(&lp->pdev->dev,
-					MAX_RX_DESCR * MAX_RBUFF_SZ,
-					&lp->rx_buffers_dma, GFP_KERNEL);
+					    MAX_RX_DESCR * MAX_RBUFF_SZ,
+					    &lp->rx_buffers_dma, GFP_KERNEL);
 	if (!lp->rx_buffers) {
-		netdev_err(dev, "unable to alloc rx data DMA buffer\n");
-
 		dma_free_coherent(&lp->pdev->dev,
-					MAX_RX_DESCR * sizeof(struct macb_dma_desc),
-					lp->rx_ring, lp->rx_ring_dma);
+				  MAX_RX_DESCR * sizeof(struct macb_dma_desc),
+				  lp->rx_ring, lp->rx_ring_dma);
 		lp->rx_ring = NULL;
 		return -ENOMEM;
 	}
@@ -209,7 +206,6 @@ static void at91ether_rx(struct net_device *dev)
 			netif_rx(skb);
 		} else {
 			lp->stats.rx_dropped++;
-			netdev_notice(dev, "Memory squeeze, dropping packet.\n");
 		}
 
 		if (lp->rx_ring[lp->rx_tail].ctrl & MACB_BIT(RX_MHASH_MATCH))
@@ -303,42 +299,7 @@ static const struct of_device_id at91ether_dt_ids[] = {
 	{ .compatible = "cdns,emac" },
 	{ /* sentinel */ }
 };
-
 MODULE_DEVICE_TABLE(of, at91ether_dt_ids);
-
-static int at91ether_get_phy_mode_dt(struct platform_device *pdev)
-{
-	struct device_node *np = pdev->dev.of_node;
-
-	if (np)
-		return of_get_phy_mode(np);
-
-	return -ENODEV;
-}
-
-static int at91ether_get_hwaddr_dt(struct macb *bp)
-{
-	struct device_node *np = bp->pdev->dev.of_node;
-
-	if (np) {
-		const char *mac = of_get_mac_address(np);
-		if (mac) {
-			memcpy(bp->dev->dev_addr, mac, ETH_ALEN);
-			return 0;
-		}
-	}
-
-	return -ENODEV;
-}
-#else
-static int at91ether_get_phy_mode_dt(struct platform_device *pdev)
-{
-	return -ENODEV;
-}
-static int at91ether_get_hwaddr_dt(struct macb *bp)
-{
-	return -ENODEV;
-}
 #endif
 
 /* Detect MAC & PHY and perform ethernet interface initialization */
@@ -352,6 +313,7 @@ static int __init at91ether_probe(struct platform_device *pdev)
 	struct macb *lp;
 	int res;
 	u32 reg;
+	const char *mac;
 
 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!regs)
@@ -403,11 +365,13 @@ static int __init at91ether_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, dev);
 	SET_NETDEV_DEV(dev, &pdev->dev);
 
-	res = at91ether_get_hwaddr_dt(lp);
-	if (res < 0)
+	mac = of_get_mac_address(pdev->dev.of_node);
+	if (mac)
+		memcpy(lp->dev->dev_addr, mac, ETH_ALEN);
+	else
 		macb_get_hwaddr(lp);
 
-	res = at91ether_get_phy_mode_dt(pdev);
+	res = of_get_phy_mode(pdev->dev.of_node);
 	if (res < 0) {
 		if (board_data && board_data->is_rmii)
 			lp->phy_interface = PHY_INTERFACE_MODE_RMII;
@@ -430,7 +394,8 @@ static int __init at91ether_probe(struct platform_device *pdev)
 	if (res)
 		goto err_disable_clock;
 
-	if (macb_mii_init(lp) != 0)
+	res = macb_mii_init(lp);
+	if (res)
 		goto err_out_unregister_netdev;
 
 	/* will be enabled in open() */
@@ -519,18 +484,7 @@ static struct platform_driver at91ether_driver = {
 	},
 };
 
-static int __init at91ether_init(void)
-{
-	return platform_driver_probe(&at91ether_driver, at91ether_probe);
-}
-
-static void __exit at91ether_exit(void)
-{
-	platform_driver_unregister(&at91ether_driver);
-}
-
-module_init(at91ether_init)
-module_exit(at91ether_exit)
+module_platform_driver_probe(at91ether_driver, at91ether_probe);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("AT91RM9200 EMAC Ethernet driver");
diff --git a/drivers/net/ethernet/cadence/macb.c b/drivers/net/ethernet/cadence/macb.c
index 79039439bfdc..6be513deb17f 100644
--- a/drivers/net/ethernet/cadence/macb.c
+++ b/drivers/net/ethernet/cadence/macb.c
@@ -485,6 +485,8 @@ static void macb_tx_interrupt(struct macb *bp)
 	status = macb_readl(bp, TSR);
 	macb_writel(bp, TSR, status);
 
+	macb_writel(bp, ISR, MACB_BIT(TCOMP));
+
 	netdev_vdbg(bp->dev, "macb_tx_interrupt status = 0x%03lx\n",
 		(unsigned long)status);
 
@@ -736,6 +738,7 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
 			 * now.
 			 */
 			macb_writel(bp, IDR, MACB_RX_INT_FLAGS);
+			macb_writel(bp, ISR, MACB_BIT(RCOMP));
 
 			if (napi_schedule_prep(&bp->napi)) {
 				netdev_vdbg(bp->dev, "scheduling RX softirq\n");
@@ -1054,6 +1057,7 @@ static void macb_configure_dma(struct macb *bp)
 		dmacfg |= GEM_BF(RXBS, RX_BUFFER_SIZE / 64);
 		dmacfg |= GEM_BF(FBLDO, 16);
 		dmacfg |= GEM_BIT(TXPBMS) | GEM_BF(RXBMS, -1L);
+		dmacfg &= ~GEM_BIT(ENDIA);
 		gem_writel(bp, DMACFG, dmacfg);
 	}
 }
@@ -1472,41 +1476,7 @@ static const struct of_device_id macb_dt_ids[] = {
 	{ .compatible = "cdns,gem" },
 	{ /* sentinel */ }
 };
-
 MODULE_DEVICE_TABLE(of, macb_dt_ids);
-
-static int macb_get_phy_mode_dt(struct platform_device *pdev)
-{
-	struct device_node *np = pdev->dev.of_node;
-
-	if (np)
-		return of_get_phy_mode(np);
-
-	return -ENODEV;
-}
-
-static int macb_get_hwaddr_dt(struct macb *bp)
-{
-	struct device_node *np = bp->pdev->dev.of_node;
-	if (np) {
-		const char *mac = of_get_mac_address(np);
-		if (mac) {
-			memcpy(bp->dev->dev_addr, mac, ETH_ALEN);
-			return 0;
-		}
-	}
-
-	return -ENODEV;
-}
-#else
-static int macb_get_phy_mode_dt(struct platform_device *pdev)
-{
-	return -ENODEV;
-}
-static int macb_get_hwaddr_dt(struct macb *bp)
-{
-	return -ENODEV;
-}
 #endif
 
 static int __init macb_probe(struct platform_device *pdev)
@@ -1519,6 +1489,7 @@ static int __init macb_probe(struct platform_device *pdev)
 	u32 config;
 	int err = -ENXIO;
 	struct pinctrl *pinctrl;
+	const char *mac;
 
 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!regs) {
@@ -1557,14 +1528,14 @@ static int __init macb_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to get macb_clk\n");
 		goto err_out_free_dev;
 	}
-	clk_enable(bp->pclk);
+	clk_prepare_enable(bp->pclk);
 
 	bp->hclk = clk_get(&pdev->dev, "hclk");
 	if (IS_ERR(bp->hclk)) {
 		dev_err(&pdev->dev, "failed to get hclk\n");
 		goto err_out_put_pclk;
 	}
-	clk_enable(bp->hclk);
+	clk_prepare_enable(bp->hclk);
 
 	bp->regs = ioremap(regs->start, resource_size(regs));
 	if (!bp->regs) {
@@ -1592,11 +1563,13 @@ static int __init macb_probe(struct platform_device *pdev)
 	config |= macb_dbw(bp);
 	macb_writel(bp, NCFGR, config);
 
-	err = macb_get_hwaddr_dt(bp);
-	if (err < 0)
+	mac = of_get_mac_address(pdev->dev.of_node);
+	if (mac)
+		memcpy(bp->dev->dev_addr, mac, ETH_ALEN);
+	else
 		macb_get_hwaddr(bp);
 
-	err = macb_get_phy_mode_dt(pdev);
+	err = of_get_phy_mode(pdev->dev.of_node);
 	if (err < 0) {
 		pdata = pdev->dev.platform_data;
 		if (pdata && pdata->is_rmii)
@@ -1629,9 +1602,9 @@ static int __init macb_probe(struct platform_device *pdev)
 		goto err_out_free_irq;
 	}
 
-	if (macb_mii_init(bp) != 0) {
+	err = macb_mii_init(bp);
+	if (err)
 		goto err_out_unregister_netdev;
-	}
 
 	platform_set_drvdata(pdev, dev);
 
@@ -1654,9 +1627,9 @@ err_out_free_irq:
 err_out_iounmap:
 	iounmap(bp->regs);
 err_out_disable_clocks:
-	clk_disable(bp->hclk);
+	clk_disable_unprepare(bp->hclk);
 	clk_put(bp->hclk);
-	clk_disable(bp->pclk);
+	clk_disable_unprepare(bp->pclk);
 err_out_put_pclk:
 	clk_put(bp->pclk);
 err_out_free_dev:
@@ -1683,9 +1656,9 @@ static int __exit macb_remove(struct platform_device *pdev)
 		unregister_netdev(dev);
 		free_irq(dev->irq, dev);
 		iounmap(bp->regs);
-		clk_disable(bp->hclk);
+		clk_disable_unprepare(bp->hclk);
 		clk_put(bp->hclk);
-		clk_disable(bp->pclk);
+		clk_disable_unprepare(bp->pclk);
 		clk_put(bp->pclk);
 		free_netdev(dev);
 		platform_set_drvdata(pdev, NULL);
@@ -1703,8 +1676,8 @@ static int macb_suspend(struct platform_device *pdev, pm_message_t state)
 	netif_carrier_off(netdev);
 	netif_device_detach(netdev);
 
-	clk_disable(bp->hclk);
-	clk_disable(bp->pclk);
+	clk_disable_unprepare(bp->hclk);
+	clk_disable_unprepare(bp->pclk);
 
 	return 0;
 }
@@ -1714,8 +1687,8 @@ static int macb_resume(struct platform_device *pdev)
 	struct net_device *netdev = platform_get_drvdata(pdev);
 	struct macb *bp = netdev_priv(netdev);
 
-	clk_enable(bp->pclk);
-	clk_enable(bp->hclk);
+	clk_prepare_enable(bp->pclk);
+	clk_prepare_enable(bp->hclk);
 
 	netif_device_attach(netdev);
 
@@ -1737,18 +1710,7 @@ static struct platform_driver macb_driver = {
 	},
 };
 
-static int __init macb_init(void)
-{
-	return platform_driver_probe(&macb_driver, macb_probe);
-}
-
-static void __exit macb_exit(void)
-{
-	platform_driver_unregister(&macb_driver);
-}
-
-module_init(macb_init);
-module_exit(macb_exit);
+module_platform_driver_probe(macb_driver, macb_probe);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Cadence MACB/GEM Ethernet driver");
diff --git a/drivers/net/ethernet/cadence/macb.h b/drivers/net/ethernet/cadence/macb.h
index 570908b93578..993d70380688 100644
--- a/drivers/net/ethernet/cadence/macb.h
+++ b/drivers/net/ethernet/cadence/macb.h
@@ -173,6 +173,8 @@
 /* Bitfields in DMACFG. */
 #define GEM_FBLDO_OFFSET			0
 #define GEM_FBLDO_SIZE				5
+#define GEM_ENDIA_OFFSET			7
+#define GEM_ENDIA_SIZE				1
 #define GEM_RXBMS_OFFSET			8
 #define GEM_RXBMS_SIZE				2
 #define GEM_TXPBMS_OFFSET			10
diff --git a/drivers/net/ethernet/calxeda/xgmac.c b/drivers/net/ethernet/calxeda/xgmac.c
index b0ebc9f6d55e..4a1f2fa812ab 100644
--- a/drivers/net/ethernet/calxeda/xgmac.c
+++ b/drivers/net/ethernet/calxeda/xgmac.c
@@ -1482,7 +1482,7 @@ static int xgmac_set_features(struct net_device *dev, netdev_features_t features
 	u32 ctrl;
 	struct xgmac_priv *priv = netdev_priv(dev);
 	void __iomem *ioaddr = priv->base;
-	u32 changed = dev->features ^ features;
+	netdev_features_t changed = dev->features ^ features;
 
 	if (!(changed & NETIF_F_RXCSUM))
 		return 0;
@@ -1886,12 +1886,9 @@ static int xgmac_resume(struct device *dev)
 
 	return 0;
 }
+#endif /* CONFIG_PM_SLEEP */
 
 static SIMPLE_DEV_PM_OPS(xgmac_pm_ops, xgmac_suspend, xgmac_resume);
-#define XGMAC_PM_OPS (&xgmac_pm_ops)
-#else
-#define XGMAC_PM_OPS NULL
-#endif /* CONFIG_PM_SLEEP */
 
 static const struct of_device_id xgmac_of_match[] = {
 	{ .compatible = "calxeda,hb-xgmac", },
@@ -1906,7 +1903,7 @@ static struct platform_driver xgmac_driver = {
 	},
 	.probe = xgmac_probe,
 	.remove = xgmac_remove,
-	.driver.pm = XGMAC_PM_OPS,
+	.driver.pm = &xgmac_pm_ops,
 };
 
 module_platform_driver(xgmac_driver);
diff --git a/drivers/net/ethernet/chelsio/cxgb/cxgb2.c b/drivers/net/ethernet/chelsio/cxgb/cxgb2.c
index 20d2085f61c5..9624cfe7df57 100644
--- a/drivers/net/ethernet/chelsio/cxgb/cxgb2.c
+++ b/drivers/net/ethernet/chelsio/cxgb/cxgb2.c
@@ -856,10 +856,10 @@ static netdev_features_t t1_fix_features(struct net_device *dev,
 	 * Since there is no support for separate rx/tx vlan accel
 	 * enable/disable make sure tx flag is always in same state as rx.
 	 */
-	if (features & NETIF_F_HW_VLAN_RX)
-		features |= NETIF_F_HW_VLAN_TX;
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
+		features |= NETIF_F_HW_VLAN_CTAG_TX;
 	else
-		features &= ~NETIF_F_HW_VLAN_TX;
+		features &= ~NETIF_F_HW_VLAN_CTAG_TX;
 
 	return features;
 }
@@ -869,7 +869,7 @@ static int t1_set_features(struct net_device *dev, netdev_features_t features)
 	netdev_features_t changed = dev->features ^ features;
 	struct adapter *adapter = dev->ml_priv;
 
-	if (changed & NETIF_F_HW_VLAN_RX)
+	if (changed & NETIF_F_HW_VLAN_CTAG_RX)
 		t1_vlan_mode(adapter, features);
 
 	return 0;
@@ -1085,8 +1085,9 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 			netdev->features |= NETIF_F_HIGHDMA;
 		if (vlan_tso_capable(adapter)) {
 			netdev->features |=
-				NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
-			netdev->hw_features |= NETIF_F_HW_VLAN_RX;
+				NETIF_F_HW_VLAN_CTAG_TX |
+				NETIF_F_HW_VLAN_CTAG_RX;
+			netdev->hw_features |= NETIF_F_HW_VLAN_CTAG_RX;
 
 			/* T204: disable TSO */
 			if (!(is_T2(adapter)) || bi->port_number != 4) {
diff --git a/drivers/net/ethernet/chelsio/cxgb/sge.c b/drivers/net/ethernet/chelsio/cxgb/sge.c
index 482976925154..8061fb0ef7ed 100644
--- a/drivers/net/ethernet/chelsio/cxgb/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb/sge.c
@@ -734,7 +734,7 @@ void t1_vlan_mode(struct adapter *adapter, netdev_features_t features)
 {
 	struct sge *sge = adapter->sge;
 
-	if (features & NETIF_F_HW_VLAN_RX)
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
 		sge->sge_control |= F_VLAN_XTRACT;
 	else
 		sge->sge_control &= ~F_VLAN_XTRACT;
@@ -835,7 +835,7 @@ static void refill_free_list(struct sge *sge, struct freelQ *q)
 		struct sk_buff *skb;
 		dma_addr_t mapping;
 
-		skb = alloc_skb(q->rx_buffer_size, GFP_ATOMIC);
+		skb = dev_alloc_skb(q->rx_buffer_size);
 		if (!skb)
 			break;
 
@@ -1046,11 +1046,10 @@ static inline struct sk_buff *get_packet(struct pci_dev *pdev,
 	const struct freelQ_ce *ce = &fl->centries[fl->cidx];
 
 	if (len < copybreak) {
-		skb = alloc_skb(len + 2, GFP_ATOMIC);
+		skb = netdev_alloc_skb_ip_align(NULL, len);
 		if (!skb)
 			goto use_orig_buf;
 
-		skb_reserve(skb, 2);	/* align IP header */
 		skb_put(skb, len);
 		pci_dma_sync_single_for_cpu(pdev,
 					    dma_unmap_addr(ce, dma_addr),
@@ -1387,7 +1386,7 @@ static void sge_rx(struct sge *sge, struct freelQ *fl, unsigned int len)
 
 	if (p->vlan_valid) {
 		st->vlan_xtract++;
-		__vlan_hwaccel_put_tag(skb, ntohs(p->vlan));
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), ntohs(p->vlan));
 	}
 	netif_receive_skb(skb);
 }
diff --git a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
index 2b5e62193cea..71497e835f42 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
@@ -1181,14 +1181,15 @@ static void cxgb_vlan_mode(struct net_device *dev, netdev_features_t features)
 
 	if (adapter->params.rev > 0) {
 		t3_set_vlan_accel(adapter, 1 << pi->port_id,
-				  features & NETIF_F_HW_VLAN_RX);
+				  features & NETIF_F_HW_VLAN_CTAG_RX);
 	} else {
 		/* single control for all ports */
-		unsigned int i, have_vlans = features & NETIF_F_HW_VLAN_RX;
+		unsigned int i, have_vlans = features & NETIF_F_HW_VLAN_CTAG_RX;
 
 		for_each_port(adapter, i)
 			have_vlans |=
-				adapter->port[i]->features & NETIF_F_HW_VLAN_RX;
+				adapter->port[i]->features &
+				NETIF_F_HW_VLAN_CTAG_RX;
 
 		t3_set_vlan_accel(adapter, 1, have_vlans);
 	}
@@ -2563,10 +2564,10 @@ static netdev_features_t cxgb_fix_features(struct net_device *dev,
 	 * Since there is no support for separate rx/tx vlan accel
 	 * enable/disable make sure tx flag is always in same state as rx.
 	 */
-	if (features & NETIF_F_HW_VLAN_RX)
-		features |= NETIF_F_HW_VLAN_TX;
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
+		features |= NETIF_F_HW_VLAN_CTAG_TX;
 	else
-		features &= ~NETIF_F_HW_VLAN_TX;
+		features &= ~NETIF_F_HW_VLAN_CTAG_TX;
 
 	return features;
 }
@@ -2575,7 +2576,7 @@ static int cxgb_set_features(struct net_device *dev, netdev_features_t features)
 {
 	netdev_features_t changed = dev->features ^ features;
 
-	if (changed & NETIF_F_HW_VLAN_RX)
+	if (changed & NETIF_F_HW_VLAN_CTAG_RX)
 		cxgb_vlan_mode(dev, features);
 
 	return 0;
@@ -3288,8 +3289,9 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		netdev->mem_start = mmio_start;
 		netdev->mem_end = mmio_start + mmio_len - 1;
 		netdev->hw_features = NETIF_F_SG | NETIF_F_IP_CSUM |
-			NETIF_F_TSO | NETIF_F_RXCSUM | NETIF_F_HW_VLAN_RX;
-		netdev->features |= netdev->hw_features | NETIF_F_HW_VLAN_TX;
+			NETIF_F_TSO | NETIF_F_RXCSUM | NETIF_F_HW_VLAN_CTAG_RX;
+		netdev->features |= netdev->hw_features |
+				    NETIF_F_HW_VLAN_CTAG_TX;
 		netdev->vlan_features |= netdev->features & VLAN_FEAT;
 		if (pci_using_dac)
 			netdev->features |= NETIF_F_HIGHDMA;
diff --git a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c
index 4232767862b5..0c96e5fe99cc 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c
@@ -185,7 +185,7 @@ static struct net_device *get_iff_from_mac(struct adapter *adapter,
 		if (!memcmp(dev->dev_addr, mac, ETH_ALEN)) {
 			rcu_read_lock();
 			if (vlan && vlan != VLAN_VID_MASK) {
-				dev = __vlan_find_dev_deep(dev, vlan);
+				dev = __vlan_find_dev_deep(dev, htons(ETH_P_8021Q), vlan);
 			} else if (netif_is_bond_slave(dev)) {
 				struct net_device *upper_dev;
 
diff --git a/drivers/net/ethernet/chelsio/cxgb3/sge.c b/drivers/net/ethernet/chelsio/cxgb3/sge.c
index 9d67eb794c4b..f12e6b85a653 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/sge.c
@@ -2030,7 +2030,7 @@ static void rx_eth(struct adapter *adap, struct sge_rspq *rq,
 
 	if (p->vlan_valid) {
 		qs->port_stats[SGE_PSTAT_VLANEX]++;
-		__vlan_hwaccel_put_tag(skb, ntohs(p->vlan));
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), ntohs(p->vlan));
 	}
 	if (rq->polling) {
 		if (lro)
@@ -2132,7 +2132,7 @@ static void lro_add_page(struct adapter *adap, struct sge_qset *qs,
 
 	if (cpl->vlan_valid) {
 		qs->port_stats[SGE_PSTAT_VLANEX]++;
-		__vlan_hwaccel_put_tag(skb, ntohs(cpl->vlan));
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), ntohs(cpl->vlan));
 	}
 	napi_gro_frags(&qs->napi);
 }
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
index 6db997c78a5f..681804b30a3f 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
@@ -54,6 +54,10 @@
 #define FW_VERSION_MINOR 1
 #define FW_VERSION_MICRO 0
 
+#define FW_VERSION_MAJOR_T5 0
+#define FW_VERSION_MINOR_T5 0
+#define FW_VERSION_MICRO_T5 0
+
 #define CH_WARN(adap, fmt, ...) dev_warn(adap->pdev_dev, fmt, ## __VA_ARGS__)
 
 enum {
@@ -66,7 +70,9 @@ enum {
 enum {
 	MEM_EDC0,
 	MEM_EDC1,
-	MEM_MC
+	MEM_MC,
+	MEM_MC0 = MEM_MC,
+	MEM_MC1
 };
 
 enum {
@@ -74,8 +80,10 @@ enum {
 	MEMWIN0_BASE     = 0x1b800,
 	MEMWIN1_APERTURE = 32768,
 	MEMWIN1_BASE     = 0x28000,
+	MEMWIN1_BASE_T5  = 0x52000,
 	MEMWIN2_APERTURE = 65536,
 	MEMWIN2_BASE     = 0x30000,
+	MEMWIN2_BASE_T5  = 0x54000,
 };
 
 enum dev_master {
@@ -431,6 +439,7 @@ struct sge_txq {
 	spinlock_t db_lock;
 	int db_disabled;
 	unsigned short db_pidx;
+	u64 udb;
 };
 
 struct sge_eth_txq {                /* state for an SGE Ethernet Tx queue */
@@ -504,13 +513,44 @@ struct sge {
 
 struct l2t_data;
 
+#define CHELSIO_CHIP_CODE(version, revision) (((version) << 4) | (revision))
+#define CHELSIO_CHIP_VERSION(code) ((code) >> 4)
+#define CHELSIO_CHIP_RELEASE(code) ((code) & 0xf)
+
+#define CHELSIO_T4		0x4
+#define CHELSIO_T5		0x5
+
+enum chip_type {
+	T4_A1 = CHELSIO_CHIP_CODE(CHELSIO_T4, 0),
+	T4_A2 = CHELSIO_CHIP_CODE(CHELSIO_T4, 1),
+	T4_A3 = CHELSIO_CHIP_CODE(CHELSIO_T4, 2),
+	T4_FIRST_REV	= T4_A1,
+	T4_LAST_REV	= T4_A3,
+
+	T5_A1 = CHELSIO_CHIP_CODE(CHELSIO_T5, 0),
+	T5_FIRST_REV	= T5_A1,
+	T5_LAST_REV	= T5_A1,
+};
+
+#ifdef CONFIG_PCI_IOV
+
+/* T4 supports SRIOV on PF0-3 and T5 on PF0-7.  However, the Serial
+ * Configuration initialization for T5 only has SR-IOV functionality enabled
+ * on PF0-3 in order to simplify everything.
+ */
+#define NUM_OF_PF_WITH_SRIOV 4
+
+#endif
+
 struct adapter {
 	void __iomem *regs;
+	void __iomem *bar2;
 	struct pci_dev *pdev;
 	struct device *pdev_dev;
 	unsigned int mbox;
 	unsigned int fn;
 	unsigned int flags;
+	enum chip_type chip;
 
 	int msg_enable;
 
@@ -673,6 +713,16 @@ enum {
 	VLAN_REWRITE
 };
 
+static inline int is_t5(enum chip_type chip)
+{
+	return (chip >= T5_FIRST_REV && chip <= T5_LAST_REV);
+}
+
+static inline int is_t4(enum chip_type chip)
+{
+	return (chip >= T4_FIRST_REV && chip <= T4_LAST_REV);
+}
+
 static inline u32 t4_read_reg(struct adapter *adap, u32 reg_addr)
 {
 	return readl(adap->regs + reg_addr);
@@ -858,7 +908,8 @@ int t4_config_rss_range(struct adapter *adapter, int mbox, unsigned int viid,
 			int start, int n, const u16 *rspq, unsigned int nrspq);
 int t4_config_glbl_rss(struct adapter *adapter, int mbox, unsigned int mode,
 		       unsigned int flags);
-int t4_mc_read(struct adapter *adap, u32 addr, __be32 *data, u64 *parity);
+int t4_mc_read(struct adapter *adap, int idx, u32 addr, __be32 *data,
+	       u64 *parity);
 int t4_edc_read(struct adapter *adap, int idx, u32 addr, __be32 *data,
 		u64 *parity);
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index e707e31abd81..3cd397d60434 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -68,8 +68,8 @@
 #include "t4fw_api.h"
 #include "l2t.h"
 
-#define DRV_VERSION "1.3.0-ko"
-#define DRV_DESC "Chelsio T4 Network Driver"
+#define DRV_VERSION "2.0.0-ko"
+#define DRV_DESC "Chelsio T4/T5 Network Driver"
 
 /*
  * Max interrupt hold-off timer value in us.  Queues fall back to this value
@@ -229,11 +229,51 @@ static DEFINE_PCI_DEVICE_TABLE(cxgb4_pci_tbl) = {
 	CH_DEVICE(0x440a, 4),
 	CH_DEVICE(0x440d, 4),
 	CH_DEVICE(0x440e, 4),
+	CH_DEVICE(0x5001, 4),
+	CH_DEVICE(0x5002, 4),
+	CH_DEVICE(0x5003, 4),
+	CH_DEVICE(0x5004, 4),
+	CH_DEVICE(0x5005, 4),
+	CH_DEVICE(0x5006, 4),
+	CH_DEVICE(0x5007, 4),
+	CH_DEVICE(0x5008, 4),
+	CH_DEVICE(0x5009, 4),
+	CH_DEVICE(0x500A, 4),
+	CH_DEVICE(0x500B, 4),
+	CH_DEVICE(0x500C, 4),
+	CH_DEVICE(0x500D, 4),
+	CH_DEVICE(0x500E, 4),
+	CH_DEVICE(0x500F, 4),
+	CH_DEVICE(0x5010, 4),
+	CH_DEVICE(0x5011, 4),
+	CH_DEVICE(0x5012, 4),
+	CH_DEVICE(0x5013, 4),
+	CH_DEVICE(0x5401, 4),
+	CH_DEVICE(0x5402, 4),
+	CH_DEVICE(0x5403, 4),
+	CH_DEVICE(0x5404, 4),
+	CH_DEVICE(0x5405, 4),
+	CH_DEVICE(0x5406, 4),
+	CH_DEVICE(0x5407, 4),
+	CH_DEVICE(0x5408, 4),
+	CH_DEVICE(0x5409, 4),
+	CH_DEVICE(0x540A, 4),
+	CH_DEVICE(0x540B, 4),
+	CH_DEVICE(0x540C, 4),
+	CH_DEVICE(0x540D, 4),
+	CH_DEVICE(0x540E, 4),
+	CH_DEVICE(0x540F, 4),
+	CH_DEVICE(0x5410, 4),
+	CH_DEVICE(0x5411, 4),
+	CH_DEVICE(0x5412, 4),
+	CH_DEVICE(0x5413, 4),
 	{ 0, }
 };
 
 #define FW_FNAME "cxgb4/t4fw.bin"
+#define FW5_FNAME "cxgb4/t5fw.bin"
 #define FW_CFNAME "cxgb4/t4-config.txt"
+#define FW5_CFNAME "cxgb4/t5-config.txt"
 
 MODULE_DESCRIPTION(DRV_DESC);
 MODULE_AUTHOR("Chelsio Communications");
@@ -241,6 +281,7 @@ MODULE_LICENSE("Dual BSD/GPL");
 MODULE_VERSION(DRV_VERSION);
 MODULE_DEVICE_TABLE(pci, cxgb4_pci_tbl);
 MODULE_FIRMWARE(FW_FNAME);
+MODULE_FIRMWARE(FW5_FNAME);
 
 /*
  * Normally we're willing to become the firmware's Master PF but will be happy
@@ -319,7 +360,10 @@ static bool vf_acls;
 module_param(vf_acls, bool, 0644);
 MODULE_PARM_DESC(vf_acls, "if set enable virtualization L2 ACL enforcement");
 
-static unsigned int num_vf[4];
+/* Configure the number of PCI-E Virtual Function which are to be instantiated
+ * on SR-IOV Capable Physical Functions.
+ */
+static unsigned int num_vf[NUM_OF_PF_WITH_SRIOV];
 
 module_param_array(num_vf, uint, NULL, 0644);
 MODULE_PARM_DESC(num_vf, "number of VFs for each of PFs 0-3");
@@ -515,7 +559,7 @@ static int link_start(struct net_device *dev)
 	 * that step explicitly.
 	 */
 	ret = t4_set_rxmode(pi->adapter, mb, pi->viid, dev->mtu, -1, -1, -1,
-			    !!(dev->features & NETIF_F_HW_VLAN_RX), true);
+			    !!(dev->features & NETIF_F_HW_VLAN_CTAG_RX), true);
 	if (ret == 0) {
 		ret = t4_change_mac(pi->adapter, mb, pi->viid,
 				    pi->xact_addr_filt, dev->dev_addr, true,
@@ -601,6 +645,21 @@ static int fwevtq_handler(struct sge_rspq *q, const __be64 *rsp,
 	u8 opcode = ((const struct rss_header *)rsp)->opcode;
 
 	rsp++;                                          /* skip RSS header */
+
+	/* FW can send EGR_UPDATEs encapsulated in a CPL_FW4_MSG.
+	 */
+	if (unlikely(opcode == CPL_FW4_MSG &&
+	   ((const struct cpl_fw4_msg *)rsp)->type == FW_TYPE_RSSCPL)) {
+		rsp++;
+		opcode = ((const struct rss_header *)rsp)->opcode;
+		rsp++;
+		if (opcode != CPL_SGE_EGR_UPDATE) {
+			dev_err(q->adap->pdev_dev, "unexpected FW4/CPL %#x on FW event queue\n"
+				, opcode);
+			goto out;
+		}
+	}
+
 	if (likely(opcode == CPL_SGE_EGR_UPDATE)) {
 		const struct cpl_sge_egr_update *p = (void *)rsp;
 		unsigned int qid = EGR_QID(ntohl(p->opcode_qid));
@@ -635,6 +694,7 @@ static int fwevtq_handler(struct sge_rspq *q, const __be64 *rsp,
 	} else
 		dev_err(q->adap->pdev_dev,
 			"unexpected CPL %#x on FW event queue\n", opcode);
+out:
 	return 0;
 }
 
@@ -652,6 +712,12 @@ static int uldrx_handler(struct sge_rspq *q, const __be64 *rsp,
 {
 	struct sge_ofld_rxq *rxq = container_of(q, struct sge_ofld_rxq, rspq);
 
+	/* FW can send CPLs encapsulated in a CPL_FW4_MSG.
+	 */
+	if (((const struct rss_header *)rsp)->opcode == CPL_FW4_MSG &&
+	    ((const struct cpl_fw4_msg *)(rsp + 1))->type == FW_TYPE_RSSCPL)
+		rsp += 2;
+
 	if (ulds[q->uld].rx_handler(q->adap->uld_handle[q->uld], rsp, gl)) {
 		rxq->stats.nomem++;
 		return -1;
@@ -1002,21 +1068,36 @@ freeout:	t4_free_sge_resources(adap);
 static int upgrade_fw(struct adapter *adap)
 {
 	int ret;
-	u32 vers;
+	u32 vers, exp_major;
 	const struct fw_hdr *hdr;
 	const struct firmware *fw;
 	struct device *dev = adap->pdev_dev;
+	char *fw_file_name;
 
-	ret = request_firmware(&fw, FW_FNAME, dev);
+	switch (CHELSIO_CHIP_VERSION(adap->chip)) {
+	case CHELSIO_T4:
+		fw_file_name = FW_FNAME;
+		exp_major = FW_VERSION_MAJOR;
+		break;
+	case CHELSIO_T5:
+		fw_file_name = FW5_FNAME;
+		exp_major = FW_VERSION_MAJOR_T5;
+		break;
+	default:
+		dev_err(dev, "Unsupported chip type, %x\n", adap->chip);
+		return -EINVAL;
+	}
+
+	ret = request_firmware(&fw, fw_file_name, dev);
 	if (ret < 0) {
-		dev_err(dev, "unable to load firmware image " FW_FNAME
-			", error %d\n", ret);
+		dev_err(dev, "unable to load firmware image %s, error %d\n",
+			fw_file_name, ret);
 		return ret;
 	}
 
 	hdr = (const struct fw_hdr *)fw->data;
 	vers = ntohl(hdr->fw_ver);
-	if (FW_HDR_FW_VER_MAJOR_GET(vers) != FW_VERSION_MAJOR) {
+	if (FW_HDR_FW_VER_MAJOR_GET(vers) != exp_major) {
 		ret = -EINVAL;              /* wrong major version, won't do */
 		goto out;
 	}
@@ -1024,18 +1105,15 @@ static int upgrade_fw(struct adapter *adap)
 	/*
 	 * If the flash FW is unusable or we found something newer, load it.
 	 */
-	if (FW_HDR_FW_VER_MAJOR_GET(adap->params.fw_vers) != FW_VERSION_MAJOR ||
+	if (FW_HDR_FW_VER_MAJOR_GET(adap->params.fw_vers) != exp_major ||
 	    vers > adap->params.fw_vers) {
 		dev_info(dev, "upgrading firmware ...\n");
 		ret = t4_fw_upgrade(adap, adap->mbox, fw->data, fw->size,
 				    /*force=*/false);
 		if (!ret)
-			dev_info(dev, "firmware successfully upgraded to "
-				 FW_FNAME " (%d.%d.%d.%d)\n",
-				 FW_HDR_FW_VER_MAJOR_GET(vers),
-				 FW_HDR_FW_VER_MINOR_GET(vers),
-				 FW_HDR_FW_VER_MICRO_GET(vers),
-				 FW_HDR_FW_VER_BUILD_GET(vers));
+			dev_info(dev,
+				 "firmware upgraded to version %pI4 from %s\n",
+				 &hdr->fw_ver, fw_file_name);
 		else
 			dev_err(dev, "firmware upgrade failed! err=%d\n", -ret);
 	} else {
@@ -1308,6 +1386,8 @@ static char stats_strings[][ETH_GSTRING_LEN] = {
 	"VLANinsertions     ",
 	"GROpackets         ",
 	"GROmerged          ",
+	"WriteCoalSuccess   ",
+	"WriteCoalFail      ",
 };
 
 static int get_sset_count(struct net_device *dev, int sset)
@@ -1321,10 +1401,15 @@ static int get_sset_count(struct net_device *dev, int sset)
 }
 
 #define T4_REGMAP_SIZE (160 * 1024)
+#define T5_REGMAP_SIZE (332 * 1024)
 
 static int get_regs_len(struct net_device *dev)
 {
-	return T4_REGMAP_SIZE;
+	struct adapter *adap = netdev2adap(dev);
+	if (is_t4(adap->chip))
+		return T4_REGMAP_SIZE;
+	else
+		return T5_REGMAP_SIZE;
 }
 
 static int get_eeprom_len(struct net_device *dev)
@@ -1398,11 +1483,25 @@ static void get_stats(struct net_device *dev, struct ethtool_stats *stats,
 {
 	struct port_info *pi = netdev_priv(dev);
 	struct adapter *adapter = pi->adapter;
+	u32 val1, val2;
 
 	t4_get_port_stats(adapter, pi->tx_chan, (struct port_stats *)data);
 
 	data += sizeof(struct port_stats) / sizeof(u64);
 	collect_sge_port_stats(adapter, pi, (struct queue_port_stats *)data);
+	data += sizeof(struct queue_port_stats) / sizeof(u64);
+	if (!is_t4(adapter->chip)) {
+		t4_write_reg(adapter, SGE_STAT_CFG, STATSOURCE_T5(7));
+		val1 = t4_read_reg(adapter, SGE_STAT_TOTAL);
+		val2 = t4_read_reg(adapter, SGE_STAT_MATCH);
+		*data = val1 - val2;
+		data++;
+		*data = val2;
+		data++;
+	} else {
+		memset(data, 0, 2 * sizeof(u64));
+		*data += 2;
+	}
 }
 
 /*
@@ -1413,7 +1512,8 @@ static void get_stats(struct net_device *dev, struct ethtool_stats *stats,
  */
 static inline unsigned int mk_adap_vers(const struct adapter *ap)
 {
-	return 4 | (ap->params.rev << 10) | (1 << 16);
+	return CHELSIO_CHIP_VERSION(ap->chip) |
+		(CHELSIO_CHIP_RELEASE(ap->chip) << 10) | (1 << 16);
 }
 
 static void reg_block_dump(struct adapter *ap, void *buf, unsigned int start,
@@ -1428,7 +1528,7 @@ static void reg_block_dump(struct adapter *ap, void *buf, unsigned int start,
 static void get_regs(struct net_device *dev, struct ethtool_regs *regs,
 		     void *buf)
 {
-	static const unsigned int reg_ranges[] = {
+	static const unsigned int t4_reg_ranges[] = {
 		0x1008, 0x1108,
 		0x1180, 0x11b4,
 		0x11fc, 0x123c,
@@ -1648,13 +1748,452 @@ static void get_regs(struct net_device *dev, struct ethtool_regs *regs,
 		0x27e00, 0x27e04
 	};
 
+	static const unsigned int t5_reg_ranges[] = {
+		0x1008, 0x1148,
+		0x1180, 0x11b4,
+		0x11fc, 0x123c,
+		0x1280, 0x173c,
+		0x1800, 0x18fc,
+		0x3000, 0x3028,
+		0x3060, 0x30d8,
+		0x30e0, 0x30fc,
+		0x3140, 0x357c,
+		0x35a8, 0x35cc,
+		0x35ec, 0x35ec,
+		0x3600, 0x5624,
+		0x56cc, 0x575c,
+		0x580c, 0x5814,
+		0x5890, 0x58bc,
+		0x5940, 0x59dc,
+		0x59fc, 0x5a18,
+		0x5a60, 0x5a9c,
+		0x5b9c, 0x5bfc,
+		0x6000, 0x6040,
+		0x6058, 0x614c,
+		0x7700, 0x7798,
+		0x77c0, 0x78fc,
+		0x7b00, 0x7c54,
+		0x7d00, 0x7efc,
+		0x8dc0, 0x8de0,
+		0x8df8, 0x8e84,
+		0x8ea0, 0x8f84,
+		0x8fc0, 0x90f8,
+		0x9400, 0x9470,
+		0x9600, 0x96f4,
+		0x9800, 0x9808,
+		0x9820, 0x983c,
+		0x9850, 0x9864,
+		0x9c00, 0x9c6c,
+		0x9c80, 0x9cec,
+		0x9d00, 0x9d6c,
+		0x9d80, 0x9dec,
+		0x9e00, 0x9e6c,
+		0x9e80, 0x9eec,
+		0x9f00, 0x9f6c,
+		0x9f80, 0xa020,
+		0xd004, 0xd03c,
+		0xdfc0, 0xdfe0,
+		0xe000, 0x11088,
+		0x1109c, 0x1117c,
+		0x11190, 0x11204,
+		0x19040, 0x1906c,
+		0x19078, 0x19080,
+		0x1908c, 0x19124,
+		0x19150, 0x191b0,
+		0x191d0, 0x191e8,
+		0x19238, 0x19290,
+		0x193f8, 0x19474,
+		0x19490, 0x194cc,
+		0x194f0, 0x194f8,
+		0x19c00, 0x19c60,
+		0x19c94, 0x19e10,
+		0x19e50, 0x19f34,
+		0x19f40, 0x19f50,
+		0x19f90, 0x19fe4,
+		0x1a000, 0x1a06c,
+		0x1a0b0, 0x1a120,
+		0x1a128, 0x1a138,
+		0x1a190, 0x1a1c4,
+		0x1a1fc, 0x1a1fc,
+		0x1e008, 0x1e00c,
+		0x1e040, 0x1e04c,
+		0x1e284, 0x1e290,
+		0x1e2c0, 0x1e2c0,
+		0x1e2e0, 0x1e2e0,
+		0x1e300, 0x1e384,
+		0x1e3c0, 0x1e3c8,
+		0x1e408, 0x1e40c,
+		0x1e440, 0x1e44c,
+		0x1e684, 0x1e690,
+		0x1e6c0, 0x1e6c0,
+		0x1e6e0, 0x1e6e0,
+		0x1e700, 0x1e784,
+		0x1e7c0, 0x1e7c8,
+		0x1e808, 0x1e80c,
+		0x1e840, 0x1e84c,
+		0x1ea84, 0x1ea90,
+		0x1eac0, 0x1eac0,
+		0x1eae0, 0x1eae0,
+		0x1eb00, 0x1eb84,
+		0x1ebc0, 0x1ebc8,
+		0x1ec08, 0x1ec0c,
+		0x1ec40, 0x1ec4c,
+		0x1ee84, 0x1ee90,
+		0x1eec0, 0x1eec0,
+		0x1eee0, 0x1eee0,
+		0x1ef00, 0x1ef84,
+		0x1efc0, 0x1efc8,
+		0x1f008, 0x1f00c,
+		0x1f040, 0x1f04c,
+		0x1f284, 0x1f290,
+		0x1f2c0, 0x1f2c0,
+		0x1f2e0, 0x1f2e0,
+		0x1f300, 0x1f384,
+		0x1f3c0, 0x1f3c8,
+		0x1f408, 0x1f40c,
+		0x1f440, 0x1f44c,
+		0x1f684, 0x1f690,
+		0x1f6c0, 0x1f6c0,
+		0x1f6e0, 0x1f6e0,
+		0x1f700, 0x1f784,
+		0x1f7c0, 0x1f7c8,
+		0x1f808, 0x1f80c,
+		0x1f840, 0x1f84c,
+		0x1fa84, 0x1fa90,
+		0x1fac0, 0x1fac0,
+		0x1fae0, 0x1fae0,
+		0x1fb00, 0x1fb84,
+		0x1fbc0, 0x1fbc8,
+		0x1fc08, 0x1fc0c,
+		0x1fc40, 0x1fc4c,
+		0x1fe84, 0x1fe90,
+		0x1fec0, 0x1fec0,
+		0x1fee0, 0x1fee0,
+		0x1ff00, 0x1ff84,
+		0x1ffc0, 0x1ffc8,
+		0x30000, 0x30030,
+		0x30100, 0x30144,
+		0x30190, 0x301d0,
+		0x30200, 0x30318,
+		0x30400, 0x3052c,
+		0x30540, 0x3061c,
+		0x30800, 0x30834,
+		0x308c0, 0x30908,
+		0x30910, 0x309ac,
+		0x30a00, 0x30a04,
+		0x30a0c, 0x30a2c,
+		0x30a44, 0x30a50,
+		0x30a74, 0x30c24,
+		0x30d08, 0x30d14,
+		0x30d1c, 0x30d20,
+		0x30d3c, 0x30d50,
+		0x31200, 0x3120c,
+		0x31220, 0x31220,
+		0x31240, 0x31240,
+		0x31600, 0x31600,
+		0x31608, 0x3160c,
+		0x31a00, 0x31a1c,
+		0x31e04, 0x31e20,
+		0x31e38, 0x31e3c,
+		0x31e80, 0x31e80,
+		0x31e88, 0x31ea8,
+		0x31eb0, 0x31eb4,
+		0x31ec8, 0x31ed4,
+		0x31fb8, 0x32004,
+		0x32208, 0x3223c,
+		0x32600, 0x32630,
+		0x32a00, 0x32abc,
+		0x32b00, 0x32b70,
+		0x33000, 0x33048,
+		0x33060, 0x3309c,
+		0x330f0, 0x33148,
+		0x33160, 0x3319c,
+		0x331f0, 0x332e4,
+		0x332f8, 0x333e4,
+		0x333f8, 0x33448,
+		0x33460, 0x3349c,
+		0x334f0, 0x33548,
+		0x33560, 0x3359c,
+		0x335f0, 0x336e4,
+		0x336f8, 0x337e4,
+		0x337f8, 0x337fc,
+		0x33814, 0x33814,
+		0x3382c, 0x3382c,
+		0x33880, 0x3388c,
+		0x338e8, 0x338ec,
+		0x33900, 0x33948,
+		0x33960, 0x3399c,
+		0x339f0, 0x33ae4,
+		0x33af8, 0x33b10,
+		0x33b28, 0x33b28,
+		0x33b3c, 0x33b50,
+		0x33bf0, 0x33c10,
+		0x33c28, 0x33c28,
+		0x33c3c, 0x33c50,
+		0x33cf0, 0x33cfc,
+		0x34000, 0x34030,
+		0x34100, 0x34144,
+		0x34190, 0x341d0,
+		0x34200, 0x34318,
+		0x34400, 0x3452c,
+		0x34540, 0x3461c,
+		0x34800, 0x34834,
+		0x348c0, 0x34908,
+		0x34910, 0x349ac,
+		0x34a00, 0x34a04,
+		0x34a0c, 0x34a2c,
+		0x34a44, 0x34a50,
+		0x34a74, 0x34c24,
+		0x34d08, 0x34d14,
+		0x34d1c, 0x34d20,
+		0x34d3c, 0x34d50,
+		0x35200, 0x3520c,
+		0x35220, 0x35220,
+		0x35240, 0x35240,
+		0x35600, 0x35600,
+		0x35608, 0x3560c,
+		0x35a00, 0x35a1c,
+		0x35e04, 0x35e20,
+		0x35e38, 0x35e3c,
+		0x35e80, 0x35e80,
+		0x35e88, 0x35ea8,
+		0x35eb0, 0x35eb4,
+		0x35ec8, 0x35ed4,
+		0x35fb8, 0x36004,
+		0x36208, 0x3623c,
+		0x36600, 0x36630,
+		0x36a00, 0x36abc,
+		0x36b00, 0x36b70,
+		0x37000, 0x37048,
+		0x37060, 0x3709c,
+		0x370f0, 0x37148,
+		0x37160, 0x3719c,
+		0x371f0, 0x372e4,
+		0x372f8, 0x373e4,
+		0x373f8, 0x37448,
+		0x37460, 0x3749c,
+		0x374f0, 0x37548,
+		0x37560, 0x3759c,
+		0x375f0, 0x376e4,
+		0x376f8, 0x377e4,
+		0x377f8, 0x377fc,
+		0x37814, 0x37814,
+		0x3782c, 0x3782c,
+		0x37880, 0x3788c,
+		0x378e8, 0x378ec,
+		0x37900, 0x37948,
+		0x37960, 0x3799c,
+		0x379f0, 0x37ae4,
+		0x37af8, 0x37b10,
+		0x37b28, 0x37b28,
+		0x37b3c, 0x37b50,
+		0x37bf0, 0x37c10,
+		0x37c28, 0x37c28,
+		0x37c3c, 0x37c50,
+		0x37cf0, 0x37cfc,
+		0x38000, 0x38030,
+		0x38100, 0x38144,
+		0x38190, 0x381d0,
+		0x38200, 0x38318,
+		0x38400, 0x3852c,
+		0x38540, 0x3861c,
+		0x38800, 0x38834,
+		0x388c0, 0x38908,
+		0x38910, 0x389ac,
+		0x38a00, 0x38a04,
+		0x38a0c, 0x38a2c,
+		0x38a44, 0x38a50,
+		0x38a74, 0x38c24,
+		0x38d08, 0x38d14,
+		0x38d1c, 0x38d20,
+		0x38d3c, 0x38d50,
+		0x39200, 0x3920c,
+		0x39220, 0x39220,
+		0x39240, 0x39240,
+		0x39600, 0x39600,
+		0x39608, 0x3960c,
+		0x39a00, 0x39a1c,
+		0x39e04, 0x39e20,
+		0x39e38, 0x39e3c,
+		0x39e80, 0x39e80,
+		0x39e88, 0x39ea8,
+		0x39eb0, 0x39eb4,
+		0x39ec8, 0x39ed4,
+		0x39fb8, 0x3a004,
+		0x3a208, 0x3a23c,
+		0x3a600, 0x3a630,
+		0x3aa00, 0x3aabc,
+		0x3ab00, 0x3ab70,
+		0x3b000, 0x3b048,
+		0x3b060, 0x3b09c,
+		0x3b0f0, 0x3b148,
+		0x3b160, 0x3b19c,
+		0x3b1f0, 0x3b2e4,
+		0x3b2f8, 0x3b3e4,
+		0x3b3f8, 0x3b448,
+		0x3b460, 0x3b49c,
+		0x3b4f0, 0x3b548,
+		0x3b560, 0x3b59c,
+		0x3b5f0, 0x3b6e4,
+		0x3b6f8, 0x3b7e4,
+		0x3b7f8, 0x3b7fc,
+		0x3b814, 0x3b814,
+		0x3b82c, 0x3b82c,
+		0x3b880, 0x3b88c,
+		0x3b8e8, 0x3b8ec,
+		0x3b900, 0x3b948,
+		0x3b960, 0x3b99c,
+		0x3b9f0, 0x3bae4,
+		0x3baf8, 0x3bb10,
+		0x3bb28, 0x3bb28,
+		0x3bb3c, 0x3bb50,
+		0x3bbf0, 0x3bc10,
+		0x3bc28, 0x3bc28,
+		0x3bc3c, 0x3bc50,
+		0x3bcf0, 0x3bcfc,
+		0x3c000, 0x3c030,
+		0x3c100, 0x3c144,
+		0x3c190, 0x3c1d0,
+		0x3c200, 0x3c318,
+		0x3c400, 0x3c52c,
+		0x3c540, 0x3c61c,
+		0x3c800, 0x3c834,
+		0x3c8c0, 0x3c908,
+		0x3c910, 0x3c9ac,
+		0x3ca00, 0x3ca04,
+		0x3ca0c, 0x3ca2c,
+		0x3ca44, 0x3ca50,
+		0x3ca74, 0x3cc24,
+		0x3cd08, 0x3cd14,
+		0x3cd1c, 0x3cd20,
+		0x3cd3c, 0x3cd50,
+		0x3d200, 0x3d20c,
+		0x3d220, 0x3d220,
+		0x3d240, 0x3d240,
+		0x3d600, 0x3d600,
+		0x3d608, 0x3d60c,
+		0x3da00, 0x3da1c,
+		0x3de04, 0x3de20,
+		0x3de38, 0x3de3c,
+		0x3de80, 0x3de80,
+		0x3de88, 0x3dea8,
+		0x3deb0, 0x3deb4,
+		0x3dec8, 0x3ded4,
+		0x3dfb8, 0x3e004,
+		0x3e208, 0x3e23c,
+		0x3e600, 0x3e630,
+		0x3ea00, 0x3eabc,
+		0x3eb00, 0x3eb70,
+		0x3f000, 0x3f048,
+		0x3f060, 0x3f09c,
+		0x3f0f0, 0x3f148,
+		0x3f160, 0x3f19c,
+		0x3f1f0, 0x3f2e4,
+		0x3f2f8, 0x3f3e4,
+		0x3f3f8, 0x3f448,
+		0x3f460, 0x3f49c,
+		0x3f4f0, 0x3f548,
+		0x3f560, 0x3f59c,
+		0x3f5f0, 0x3f6e4,
+		0x3f6f8, 0x3f7e4,
+		0x3f7f8, 0x3f7fc,
+		0x3f814, 0x3f814,
+		0x3f82c, 0x3f82c,
+		0x3f880, 0x3f88c,
+		0x3f8e8, 0x3f8ec,
+		0x3f900, 0x3f948,
+		0x3f960, 0x3f99c,
+		0x3f9f0, 0x3fae4,
+		0x3faf8, 0x3fb10,
+		0x3fb28, 0x3fb28,
+		0x3fb3c, 0x3fb50,
+		0x3fbf0, 0x3fc10,
+		0x3fc28, 0x3fc28,
+		0x3fc3c, 0x3fc50,
+		0x3fcf0, 0x3fcfc,
+		0x40000, 0x4000c,
+		0x40040, 0x40068,
+		0x40080, 0x40144,
+		0x40180, 0x4018c,
+		0x40200, 0x40298,
+		0x402ac, 0x4033c,
+		0x403f8, 0x403fc,
+		0x41300, 0x413c4,
+		0x41400, 0x4141c,
+		0x41480, 0x414d0,
+		0x44000, 0x44078,
+		0x440c0, 0x44278,
+		0x442c0, 0x44478,
+		0x444c0, 0x44678,
+		0x446c0, 0x44878,
+		0x448c0, 0x449fc,
+		0x45000, 0x45068,
+		0x45080, 0x45084,
+		0x450a0, 0x450b0,
+		0x45200, 0x45268,
+		0x45280, 0x45284,
+		0x452a0, 0x452b0,
+		0x460c0, 0x460e4,
+		0x47000, 0x4708c,
+		0x47200, 0x47250,
+		0x47400, 0x47420,
+		0x47600, 0x47618,
+		0x47800, 0x47814,
+		0x48000, 0x4800c,
+		0x48040, 0x48068,
+		0x48080, 0x48144,
+		0x48180, 0x4818c,
+		0x48200, 0x48298,
+		0x482ac, 0x4833c,
+		0x483f8, 0x483fc,
+		0x49300, 0x493c4,
+		0x49400, 0x4941c,
+		0x49480, 0x494d0,
+		0x4c000, 0x4c078,
+		0x4c0c0, 0x4c278,
+		0x4c2c0, 0x4c478,
+		0x4c4c0, 0x4c678,
+		0x4c6c0, 0x4c878,
+		0x4c8c0, 0x4c9fc,
+		0x4d000, 0x4d068,
+		0x4d080, 0x4d084,
+		0x4d0a0, 0x4d0b0,
+		0x4d200, 0x4d268,
+		0x4d280, 0x4d284,
+		0x4d2a0, 0x4d2b0,
+		0x4e0c0, 0x4e0e4,
+		0x4f000, 0x4f08c,
+		0x4f200, 0x4f250,
+		0x4f400, 0x4f420,
+		0x4f600, 0x4f618,
+		0x4f800, 0x4f814,
+		0x50000, 0x500cc,
+		0x50400, 0x50400,
+		0x50800, 0x508cc,
+		0x50c00, 0x50c00,
+		0x51000, 0x5101c,
+		0x51300, 0x51308,
+	};
+
 	int i;
 	struct adapter *ap = netdev2adap(dev);
+	static const unsigned int *reg_ranges;
+	int arr_size = 0, buf_size = 0;
+
+	if (is_t4(ap->chip)) {
+		reg_ranges = &t4_reg_ranges[0];
+		arr_size = ARRAY_SIZE(t4_reg_ranges);
+		buf_size = T4_REGMAP_SIZE;
+	} else {
+		reg_ranges = &t5_reg_ranges[0];
+		arr_size = ARRAY_SIZE(t5_reg_ranges);
+		buf_size = T5_REGMAP_SIZE;
+	}
 
 	regs->version = mk_adap_vers(ap);
 
-	memset(buf, 0, T4_REGMAP_SIZE);
-	for (i = 0; i < ARRAY_SIZE(reg_ranges); i += 2)
+	memset(buf, 0, buf_size);
+	for (i = 0; i < arr_size; i += 2)
 		reg_block_dump(ap, buf, reg_ranges[i], reg_ranges[i + 1]);
 }
 
@@ -2205,14 +2744,14 @@ static int cxgb_set_features(struct net_device *dev, netdev_features_t features)
 	netdev_features_t changed = dev->features ^ features;
 	int err;
 
-	if (!(changed & NETIF_F_HW_VLAN_RX))
+	if (!(changed & NETIF_F_HW_VLAN_CTAG_RX))
 		return 0;
 
 	err = t4_set_rxmode(pi->adapter, pi->adapter->fn, pi->viid, -1,
 			    -1, -1, -1,
-			    !!(features & NETIF_F_HW_VLAN_RX), true);
+			    !!(features & NETIF_F_HW_VLAN_CTAG_RX), true);
 	if (unlikely(err))
-		dev->features = features ^ NETIF_F_HW_VLAN_RX;
+		dev->features = features ^ NETIF_F_HW_VLAN_CTAG_RX;
 	return err;
 }
 
@@ -2363,8 +2902,8 @@ static ssize_t mem_read(struct file *file, char __user *buf, size_t count,
 		int ret, ofst;
 		__be32 data[16];
 
-		if (mem == MEM_MC)
-			ret = t4_mc_read(adap, pos, data, NULL);
+		if ((mem == MEM_MC) || (mem == MEM_MC1))
+			ret = t4_mc_read(adap, mem % MEM_MC, pos, data, NULL);
 		else
 			ret = t4_edc_read(adap, mem, pos, data, NULL);
 		if (ret)
@@ -2405,18 +2944,37 @@ static void add_debugfs_mem(struct adapter *adap, const char *name,
 static int setup_debugfs(struct adapter *adap)
 {
 	int i;
+	u32 size;
 
 	if (IS_ERR_OR_NULL(adap->debugfs_root))
 		return -1;
 
 	i = t4_read_reg(adap, MA_TARGET_MEM_ENABLE);
-	if (i & EDRAM0_ENABLE)
-		add_debugfs_mem(adap, "edc0", MEM_EDC0, 5);
-	if (i & EDRAM1_ENABLE)
-		add_debugfs_mem(adap, "edc1", MEM_EDC1, 5);
-	if (i & EXT_MEM_ENABLE)
-		add_debugfs_mem(adap, "mc", MEM_MC,
-			EXT_MEM_SIZE_GET(t4_read_reg(adap, MA_EXT_MEMORY_BAR)));
+	if (i & EDRAM0_ENABLE) {
+		size = t4_read_reg(adap, MA_EDRAM0_BAR);
+		add_debugfs_mem(adap, "edc0", MEM_EDC0,	EDRAM_SIZE_GET(size));
+	}
+	if (i & EDRAM1_ENABLE) {
+		size = t4_read_reg(adap, MA_EDRAM1_BAR);
+		add_debugfs_mem(adap, "edc1", MEM_EDC1, EDRAM_SIZE_GET(size));
+	}
+	if (is_t4(adap->chip)) {
+		size = t4_read_reg(adap, MA_EXT_MEMORY_BAR);
+		if (i & EXT_MEM_ENABLE)
+			add_debugfs_mem(adap, "mc", MEM_MC,
+					EXT_MEM_SIZE_GET(size));
+	} else {
+		if (i & EXT_MEM_ENABLE) {
+			size = t4_read_reg(adap, MA_EXT_MEMORY_BAR);
+			add_debugfs_mem(adap, "mc0", MEM_MC0,
+					EXT_MEM_SIZE_GET(size));
+		}
+		if (i & EXT_MEM1_ENABLE) {
+			size = t4_read_reg(adap, MA_EXT_MEMORY1_BAR);
+			add_debugfs_mem(adap, "mc1", MEM_MC1,
+					EXT_MEM_SIZE_GET(size));
+		}
+	}
 	if (adap->l2t)
 		debugfs_create_file("l2t", S_IRUSR, adap->debugfs_root, adap,
 				    &t4_l2t_fops);
@@ -2747,10 +3305,18 @@ EXPORT_SYMBOL(cxgb4_port_chan);
 unsigned int cxgb4_dbfifo_count(const struct net_device *dev, int lpfifo)
 {
 	struct adapter *adap = netdev2adap(dev);
-	u32 v;
+	u32 v1, v2, lp_count, hp_count;
 
-	v = t4_read_reg(adap, A_SGE_DBFIFO_STATUS);
-	return lpfifo ? G_LP_COUNT(v) : G_HP_COUNT(v);
+	v1 = t4_read_reg(adap, A_SGE_DBFIFO_STATUS);
+	v2 = t4_read_reg(adap, SGE_DBFIFO_STATUS2);
+	if (is_t4(adap->chip)) {
+		lp_count = G_LP_COUNT(v1);
+		hp_count = G_HP_COUNT(v1);
+	} else {
+		lp_count = G_LP_COUNT_T5(v1);
+		hp_count = G_HP_COUNT_T5(v2);
+	}
+	return lpfifo ? lp_count : hp_count;
 }
 EXPORT_SYMBOL(cxgb4_dbfifo_count);
 
@@ -2853,6 +3419,25 @@ out:
 }
 EXPORT_SYMBOL(cxgb4_sync_txq_pidx);
 
+void cxgb4_disable_db_coalescing(struct net_device *dev)
+{
+	struct adapter *adap;
+
+	adap = netdev2adap(dev);
+	t4_set_reg_field(adap, A_SGE_DOORBELL_CONTROL, F_NOCOALESCE,
+			 F_NOCOALESCE);
+}
+EXPORT_SYMBOL(cxgb4_disable_db_coalescing);
+
+void cxgb4_enable_db_coalescing(struct net_device *dev)
+{
+	struct adapter *adap;
+
+	adap = netdev2adap(dev);
+	t4_set_reg_field(adap, A_SGE_DOORBELL_CONTROL, F_NOCOALESCE, 0);
+}
+EXPORT_SYMBOL(cxgb4_enable_db_coalescing);
+
 static struct pci_driver cxgb4_driver;
 
 static void check_neigh_update(struct neighbour *neigh)
@@ -2888,14 +3473,23 @@ static struct notifier_block cxgb4_netevent_nb = {
 
 static void drain_db_fifo(struct adapter *adap, int usecs)
 {
-	u32 v;
+	u32 v1, v2, lp_count, hp_count;
 
 	do {
+		v1 = t4_read_reg(adap, A_SGE_DBFIFO_STATUS);
+		v2 = t4_read_reg(adap, SGE_DBFIFO_STATUS2);
+		if (is_t4(adap->chip)) {
+			lp_count = G_LP_COUNT(v1);
+			hp_count = G_HP_COUNT(v1);
+		} else {
+			lp_count = G_LP_COUNT_T5(v1);
+			hp_count = G_HP_COUNT_T5(v2);
+		}
+
+		if (lp_count == 0 && hp_count == 0)
+			break;
 		set_current_state(TASK_UNINTERRUPTIBLE);
 		schedule_timeout(usecs_to_jiffies(usecs));
-		v = t4_read_reg(adap, A_SGE_DBFIFO_STATUS);
-		if (G_LP_COUNT(v) == 0 && G_HP_COUNT(v) == 0)
-			break;
 	} while (1);
 }
 
@@ -3004,24 +3598,62 @@ static void process_db_drop(struct work_struct *work)
 
 	adap = container_of(work, struct adapter, db_drop_task);
 
+	if (is_t4(adap->chip)) {
+		disable_dbs(adap);
+		notify_rdma_uld(adap, CXGB4_CONTROL_DB_DROP);
+		drain_db_fifo(adap, 1);
+		recover_all_queues(adap);
+		enable_dbs(adap);
+	} else {
+		u32 dropped_db = t4_read_reg(adap, 0x010ac);
+		u16 qid = (dropped_db >> 15) & 0x1ffff;
+		u16 pidx_inc = dropped_db & 0x1fff;
+		unsigned int s_qpp;
+		unsigned short udb_density;
+		unsigned long qpshift;
+		int page;
+		u32 udb;
+
+		dev_warn(adap->pdev_dev,
+			 "Dropped DB 0x%x qid %d bar2 %d coalesce %d pidx %d\n",
+			 dropped_db, qid,
+			 (dropped_db >> 14) & 1,
+			 (dropped_db >> 13) & 1,
+			 pidx_inc);
+
+		drain_db_fifo(adap, 1);
+
+		s_qpp = QUEUESPERPAGEPF1 * adap->fn;
+		udb_density = 1 << QUEUESPERPAGEPF0_GET(t4_read_reg(adap,
+				SGE_EGRESS_QUEUES_PER_PAGE_PF) >> s_qpp);
+		qpshift = PAGE_SHIFT - ilog2(udb_density);
+		udb = qid << qpshift;
+		udb &= PAGE_MASK;
+		page = udb / PAGE_SIZE;
+		udb += (qid - (page * udb_density)) * 128;
+
+		writel(PIDX(pidx_inc),  adap->bar2 + udb + 8);
+
+		/* Re-enable BAR2 WC */
+		t4_set_reg_field(adap, 0x10b0, 1<<15, 1<<15);
+	}
+
 	t4_set_reg_field(adap, A_SGE_DOORBELL_CONTROL, F_DROPPED_DB, 0);
-	disable_dbs(adap);
-	notify_rdma_uld(adap, CXGB4_CONTROL_DB_DROP);
-	drain_db_fifo(adap, 1);
-	recover_all_queues(adap);
-	enable_dbs(adap);
 }
 
 void t4_db_full(struct adapter *adap)
 {
-	t4_set_reg_field(adap, SGE_INT_ENABLE3,
-			 DBFIFO_HP_INT | DBFIFO_LP_INT, 0);
-	queue_work(workq, &adap->db_full_task);
+	if (is_t4(adap->chip)) {
+		t4_set_reg_field(adap, SGE_INT_ENABLE3,
+				 DBFIFO_HP_INT | DBFIFO_LP_INT, 0);
+		queue_work(workq, &adap->db_full_task);
+	}
 }
 
 void t4_db_dropped(struct adapter *adap)
 {
-	queue_work(workq, &adap->db_drop_task);
+	if (is_t4(adap->chip))
+		queue_work(workq, &adap->db_drop_task);
 }
 
 static void uld_attach(struct adapter *adap, unsigned int uld)
@@ -3566,17 +4198,27 @@ void t4_fatal_err(struct adapter *adap)
 
 static void setup_memwin(struct adapter *adap)
 {
-	u32 bar0;
+	u32 bar0, mem_win0_base, mem_win1_base, mem_win2_base;
 
 	bar0 = pci_resource_start(adap->pdev, 0);  /* truncation intentional */
+	if (is_t4(adap->chip)) {
+		mem_win0_base = bar0 + MEMWIN0_BASE;
+		mem_win1_base = bar0 + MEMWIN1_BASE;
+		mem_win2_base = bar0 + MEMWIN2_BASE;
+	} else {
+		/* For T5, only relative offset inside the PCIe BAR is passed */
+		mem_win0_base = MEMWIN0_BASE;
+		mem_win1_base = MEMWIN1_BASE_T5;
+		mem_win2_base = MEMWIN2_BASE_T5;
+	}
 	t4_write_reg(adap, PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN, 0),
-		     (bar0 + MEMWIN0_BASE) | BIR(0) |
+		     mem_win0_base | BIR(0) |
 		     WINDOW(ilog2(MEMWIN0_APERTURE) - 10));
 	t4_write_reg(adap, PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN, 1),
-		     (bar0 + MEMWIN1_BASE) | BIR(0) |
+		     mem_win1_base | BIR(0) |
 		     WINDOW(ilog2(MEMWIN1_APERTURE) - 10));
 	t4_write_reg(adap, PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN, 2),
-		     (bar0 + MEMWIN2_BASE) | BIR(0) |
+		     mem_win2_base | BIR(0) |
 		     WINDOW(ilog2(MEMWIN2_APERTURE) - 10));
 }
 
@@ -3745,6 +4387,7 @@ static int adap_init0_config(struct adapter *adapter, int reset)
 	unsigned long mtype = 0, maddr = 0;
 	u32 finiver, finicsum, cfcsum;
 	int ret, using_flash;
+	char *fw_config_file, fw_config_file_path[256];
 
 	/*
 	 * Reset device if necessary.
@@ -3761,7 +4404,21 @@ static int adap_init0_config(struct adapter *adapter, int reset)
 	 * then use that.  Otherwise, use the configuration file stored
 	 * in the adapter flash ...
 	 */
-	ret = request_firmware(&cf, FW_CFNAME, adapter->pdev_dev);
+	switch (CHELSIO_CHIP_VERSION(adapter->chip)) {
+	case CHELSIO_T4:
+		fw_config_file = FW_CFNAME;
+		break;
+	case CHELSIO_T5:
+		fw_config_file = FW5_CFNAME;
+		break;
+	default:
+		dev_err(adapter->pdev_dev, "Device %d is not supported\n",
+		       adapter->pdev->device);
+		ret = -EINVAL;
+		goto bye;
+	}
+
+	ret = request_firmware(&cf, fw_config_file, adapter->pdev_dev);
 	if (ret < 0) {
 		using_flash = 1;
 		mtype = FW_MEMTYPE_CF_FLASH;
@@ -3877,6 +4534,7 @@ static int adap_init0_config(struct adapter *adapter, int reset)
 	if (ret < 0)
 		goto bye;
 
+	sprintf(fw_config_file_path, "/lib/firmware/%s", fw_config_file);
 	/*
 	 * Return successfully and note that we're operating with parameters
 	 * not supplied by the driver, rather than from hard-wired
@@ -3887,7 +4545,7 @@ static int adap_init0_config(struct adapter *adapter, int reset)
 		 "Configuration File %s, version %#x, computed checksum %#x\n",
 		 (using_flash
 		  ? "in device FLASH"
-		  : "/lib/firmware/" FW_CFNAME),
+		  : fw_config_file_path),
 		 finiver, cfcsum);
 	return 0;
 
@@ -4354,6 +5012,15 @@ static int adap_init0(struct adapter *adap)
 		adap->tids.aftid_end = val[1];
 	}
 
+	/* If we're running on newer firmware, let it know that we're
+	 * prepared to deal with encapsulated CPL messages.  Older
+	 * firmware won't understand this and we'll just get
+	 * unencapsulated messages ...
+	 */
+	params[0] = FW_PARAM_PFVF(CPLFW4MSG_ENCAP);
+	val[0] = 1;
+	(void) t4_set_params(adap, adap->mbox, adap->fn, 0, 1, params, val);
+
 	/*
 	 * Get device capabilities so we can determine what resources we need
 	 * to manage.
@@ -4537,7 +5204,7 @@ static pci_ers_result_t eeh_slot_reset(struct pci_dev *pdev)
 
 	if (t4_wait_dev_ready(adap) < 0)
 		return PCI_ERS_RESULT_DISCONNECT;
-	if (t4_fw_hello(adap, adap->fn, adap->fn, MASTER_MUST, NULL))
+	if (t4_fw_hello(adap, adap->fn, adap->fn, MASTER_MUST, NULL) < 0)
 		return PCI_ERS_RESULT_DISCONNECT;
 	adap->flags |= FW_OK;
 	if (adap_init1(adap, &c))
@@ -4814,7 +5481,8 @@ static void print_port_info(const struct net_device *dev)
 	sprintf(bufp, "BASE-%s", base[pi->port_type]);
 
 	netdev_info(dev, "Chelsio %s rev %d %s %sNIC PCIe x%d%s%s\n",
-		    adap->params.vpd.id, adap->params.rev, buf,
+		    adap->params.vpd.id,
+		    CHELSIO_CHIP_RELEASE(adap->params.rev), buf,
 		    is_offload(adap) ? "R" : "", adap->params.pci.width, spd,
 		    (adap->flags & USING_MSIX) ? " MSI-X" :
 		    (adap->flags & USING_MSI) ? " MSI" : "");
@@ -4854,10 +5522,11 @@ static void free_some_resources(struct adapter *adapter)
 #define TSO_FLAGS (NETIF_F_TSO | NETIF_F_TSO6 | NETIF_F_TSO_ECN)
 #define VLAN_FEAT (NETIF_F_SG | NETIF_F_IP_CSUM | TSO_FLAGS | \
 		   NETIF_F_IPV6_CSUM | NETIF_F_HIGHDMA)
+#define SEGMENT_SIZE 128
 
 static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
-	int func, i, err;
+	int func, i, err, s_qpp, qpp, num_seg;
 	struct port_info *pi;
 	bool highdma = false;
 	struct adapter *adapter = NULL;
@@ -4934,7 +5603,34 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	err = t4_prep_adapter(adapter);
 	if (err)
-		goto out_unmap_bar;
+		goto out_unmap_bar0;
+
+	if (!is_t4(adapter->chip)) {
+		s_qpp = QUEUESPERPAGEPF1 * adapter->fn;
+		qpp = 1 << QUEUESPERPAGEPF0_GET(t4_read_reg(adapter,
+		      SGE_EGRESS_QUEUES_PER_PAGE_PF) >> s_qpp);
+		num_seg = PAGE_SIZE / SEGMENT_SIZE;
+
+		/* Each segment size is 128B. Write coalescing is enabled only
+		 * when SGE_EGRESS_QUEUES_PER_PAGE_PF reg value for the
+		 * queue is less no of segments that can be accommodated in
+		 * a page size.
+		 */
+		if (qpp > num_seg) {
+			dev_err(&pdev->dev,
+				"Incorrect number of egress queues per page\n");
+			err = -EINVAL;
+			goto out_unmap_bar0;
+		}
+		adapter->bar2 = ioremap_wc(pci_resource_start(pdev, 2),
+		pci_resource_len(pdev, 2));
+		if (!adapter->bar2) {
+			dev_err(&pdev->dev, "cannot map device bar2 region\n");
+			err = -ENOMEM;
+			goto out_unmap_bar0;
+		}
+	}
+
 	setup_memwin(adapter);
 	err = adap_init0(adapter);
 	setup_memwin_rdma(adapter);
@@ -4963,7 +5659,7 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		netdev->hw_features = NETIF_F_SG | TSO_FLAGS |
 			NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
 			NETIF_F_RXCSUM | NETIF_F_RXHASH |
-			NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
+			NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX;
 		if (highdma)
 			netdev->hw_features |= NETIF_F_HIGHDMA;
 		netdev->features |= netdev->hw_features;
@@ -5063,6 +5759,9 @@ sriov:
  out_free_dev:
 	free_some_resources(adapter);
  out_unmap_bar:
+	if (!is_t4(adapter->chip))
+		iounmap(adapter->bar2);
+ out_unmap_bar0:
 	iounmap(adapter->regs);
  out_free_adapter:
 	kfree(adapter);
@@ -5113,6 +5812,8 @@ static void remove_one(struct pci_dev *pdev)
 
 		free_some_resources(adapter);
 		iounmap(adapter->regs);
+		if (!is_t4(adapter->chip))
+			iounmap(adapter->bar2);
 		kfree(adapter);
 		pci_disable_pcie_error_reporting(pdev);
 		pci_disable_device(pdev);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
index e2bbc7f3e2de..4faf4d067ee7 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
@@ -269,4 +269,7 @@ struct sk_buff *cxgb4_pktgl_to_skb(const struct pkt_gl *gl,
 				   unsigned int skb_len, unsigned int pull_len);
 int cxgb4_sync_txq_pidx(struct net_device *dev, u16 qid, u16 pidx, u16 size);
 int cxgb4_flush_eq_cache(struct net_device *dev);
+void cxgb4_disable_db_coalescing(struct net_device *dev);
+void cxgb4_enable_db_coalescing(struct net_device *dev);
+
 #endif  /* !__CXGB4_OFLD_H */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/sge.c b/drivers/net/ethernet/chelsio/cxgb4/sge.c
index fe9a2ea3588b..2bfbb206b35a 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/sge.c
@@ -506,10 +506,14 @@ static void unmap_rx_buf(struct adapter *adap, struct sge_fl *q)
 
 static inline void ring_fl_db(struct adapter *adap, struct sge_fl *q)
 {
+	u32 val;
 	if (q->pend_cred >= 8) {
+		val = PIDX(q->pend_cred / 8);
+		if (!is_t4(adap->chip))
+			val |= DBTYPE(1);
 		wmb();
 		t4_write_reg(adap, MYPF_REG(SGE_PF_KDOORBELL), DBPRIO(1) |
-			     QID(q->cntxt_id) | PIDX(q->pend_cred / 8));
+			     QID(q->cntxt_id) | val);
 		q->pend_cred &= 7;
 	}
 }
@@ -812,6 +816,22 @@ static void write_sgl(const struct sk_buff *skb, struct sge_txq *q,
 		*end = 0;
 }
 
+/* This function copies 64 byte coalesced work request to
+ * memory mapped BAR2 space(user space writes).
+ * For coalesced WR SGE, fetches data from the FIFO instead of from Host.
+ */
+static void cxgb_pio_copy(u64 __iomem *dst, u64 *src)
+{
+	int count = 8;
+
+	while (count) {
+		writeq(*src, dst);
+		src++;
+		dst++;
+		count--;
+	}
+}
+
 /**
  *	ring_tx_db - check and potentially ring a Tx queue's doorbell
  *	@adap: the adapter
@@ -822,11 +842,25 @@ static void write_sgl(const struct sk_buff *skb, struct sge_txq *q,
  */
 static inline void ring_tx_db(struct adapter *adap, struct sge_txq *q, int n)
 {
+	unsigned int *wr, index;
+
 	wmb();            /* write descriptors before telling HW */
 	spin_lock(&q->db_lock);
 	if (!q->db_disabled) {
-		t4_write_reg(adap, MYPF_REG(SGE_PF_KDOORBELL),
-			     QID(q->cntxt_id) | PIDX(n));
+		if (is_t4(adap->chip)) {
+			t4_write_reg(adap, MYPF_REG(SGE_PF_KDOORBELL),
+				     QID(q->cntxt_id) | PIDX(n));
+		} else {
+			if (n == 1) {
+				index = q->pidx ? (q->pidx - 1) : (q->size - 1);
+				wr = (unsigned int *)&q->desc[index];
+				cxgb_pio_copy((u64 __iomem *)
+					      (adap->bar2 + q->udb + 64),
+					      (u64 *)wr);
+			} else
+				writel(n,  adap->bar2 + q->udb + 8);
+			wmb();
+		}
 	}
 	q->db_pidx = q->pidx;
 	spin_unlock(&q->db_lock);
@@ -1555,7 +1589,6 @@ static noinline int handle_trace_pkt(struct adapter *adap,
 				     const struct pkt_gl *gl)
 {
 	struct sk_buff *skb;
-	struct cpl_trace_pkt *p;
 
 	skb = cxgb4_pktgl_to_skb(gl, RX_PULL_LEN, RX_PULL_LEN);
 	if (unlikely(!skb)) {
@@ -1563,8 +1596,11 @@ static noinline int handle_trace_pkt(struct adapter *adap,
 		return 0;
 	}
 
-	p = (struct cpl_trace_pkt *)skb->data;
-	__skb_pull(skb, sizeof(*p));
+	if (is_t4(adap->chip))
+		__skb_pull(skb, sizeof(struct cpl_trace_pkt));
+	else
+		__skb_pull(skb, sizeof(struct cpl_t5_trace_pkt));
+
 	skb_reset_mac_header(skb);
 	skb->protocol = htons(0xffff);
 	skb->dev = adap->port[0];
@@ -1597,7 +1633,7 @@ static void do_gro(struct sge_eth_rxq *rxq, const struct pkt_gl *gl,
 		skb->rxhash = (__force u32)pkt->rsshdr.hash_val;
 
 	if (unlikely(pkt->vlan_ex)) {
-		__vlan_hwaccel_put_tag(skb, ntohs(pkt->vlan));
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), ntohs(pkt->vlan));
 		rxq->stats.vlan_ex++;
 	}
 	ret = napi_gro_frags(&rxq->rspq.napi);
@@ -1625,8 +1661,10 @@ int t4_ethrx_handler(struct sge_rspq *q, const __be64 *rsp,
 	const struct cpl_rx_pkt *pkt;
 	struct sge_eth_rxq *rxq = container_of(q, struct sge_eth_rxq, rspq);
 	struct sge *s = &q->adap->sge;
+	int cpl_trace_pkt = is_t4(q->adap->chip) ?
+			    CPL_TRACE_PKT : CPL_TRACE_PKT_T5;
 
-	if (unlikely(*(u8 *)rsp == CPL_TRACE_PKT))
+	if (unlikely(*(u8 *)rsp == cpl_trace_pkt))
 		return handle_trace_pkt(q->adap, si);
 
 	pkt = (const struct cpl_rx_pkt *)rsp;
@@ -1667,7 +1705,7 @@ int t4_ethrx_handler(struct sge_rspq *q, const __be64 *rsp,
 		skb_checksum_none_assert(skb);
 
 	if (unlikely(pkt->vlan_ex)) {
-		__vlan_hwaccel_put_tag(skb, ntohs(pkt->vlan));
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), ntohs(pkt->vlan));
 		rxq->stats.vlan_ex++;
 	}
 	netif_receive_skb(skb);
@@ -2143,11 +2181,27 @@ err:
 
 static void init_txq(struct adapter *adap, struct sge_txq *q, unsigned int id)
 {
+	q->cntxt_id = id;
+	if (!is_t4(adap->chip)) {
+		unsigned int s_qpp;
+		unsigned short udb_density;
+		unsigned long qpshift;
+		int page;
+
+		s_qpp = QUEUESPERPAGEPF1 * adap->fn;
+		udb_density = 1 << QUEUESPERPAGEPF0_GET((t4_read_reg(adap,
+				SGE_EGRESS_QUEUES_PER_PAGE_PF) >> s_qpp));
+		qpshift = PAGE_SHIFT - ilog2(udb_density);
+		q->udb = q->cntxt_id << qpshift;
+		q->udb &= PAGE_MASK;
+		page = q->udb / PAGE_SIZE;
+		q->udb += (q->cntxt_id - (page * udb_density)) * 128;
+	}
+
 	q->in_use = 0;
 	q->cidx = q->pidx = 0;
 	q->stops = q->restarts = 0;
 	q->stat = (void *)&q->desc[q->size];
-	q->cntxt_id = id;
 	spin_lock_init(&q->db_lock);
 	adap->sge.egr_map[id - adap->sge.egr_start] = q;
 }
@@ -2587,11 +2641,20 @@ static int t4_sge_init_hard(struct adapter *adap)
 	 * Set up to drop DOORBELL writes when the DOORBELL FIFO overflows
 	 * and generate an interrupt when this occurs so we can recover.
 	 */
-	t4_set_reg_field(adap, A_SGE_DBFIFO_STATUS,
-			V_HP_INT_THRESH(M_HP_INT_THRESH) |
-			V_LP_INT_THRESH(M_LP_INT_THRESH),
-			V_HP_INT_THRESH(dbfifo_int_thresh) |
-			V_LP_INT_THRESH(dbfifo_int_thresh));
+	if (is_t4(adap->chip)) {
+		t4_set_reg_field(adap, A_SGE_DBFIFO_STATUS,
+				 V_HP_INT_THRESH(M_HP_INT_THRESH) |
+				 V_LP_INT_THRESH(M_LP_INT_THRESH),
+				 V_HP_INT_THRESH(dbfifo_int_thresh) |
+				 V_LP_INT_THRESH(dbfifo_int_thresh));
+	} else {
+		t4_set_reg_field(adap, A_SGE_DBFIFO_STATUS,
+				 V_LP_INT_THRESH_T5(M_LP_INT_THRESH_T5),
+				 V_LP_INT_THRESH_T5(dbfifo_int_thresh));
+		t4_set_reg_field(adap, SGE_DBFIFO_STATUS2,
+				 V_HP_INT_THRESH_T5(M_HP_INT_THRESH_T5),
+				 V_HP_INT_THRESH_T5(dbfifo_int_thresh));
+	}
 	t4_set_reg_field(adap, A_SGE_DOORBELL_CONTROL, F_ENABLE_DROP,
 			F_ENABLE_DROP);
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
index 8049268ce0f2..d02d4e8c4417 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
@@ -282,6 +282,7 @@ int t4_wr_mbox_meat(struct adapter *adap, int mbox, const void *cmd, int size,
  *	t4_mc_read - read from MC through backdoor accesses
  *	@adap: the adapter
  *	@addr: address of first byte requested
+ *	@idx: which MC to access
  *	@data: 64 bytes of data containing the requested address
  *	@ecc: where to store the corresponding 64-bit ECC word
  *
@@ -289,22 +290,38 @@ int t4_wr_mbox_meat(struct adapter *adap, int mbox, const void *cmd, int size,
  *	that covers the requested address @addr.  If @parity is not %NULL it
  *	is assigned the 64-bit ECC word for the read data.
  */
-int t4_mc_read(struct adapter *adap, u32 addr, __be32 *data, u64 *ecc)
+int t4_mc_read(struct adapter *adap, int idx, u32 addr, __be32 *data, u64 *ecc)
 {
 	int i;
+	u32 mc_bist_cmd, mc_bist_cmd_addr, mc_bist_cmd_len;
+	u32 mc_bist_status_rdata, mc_bist_data_pattern;
+
+	if (is_t4(adap->chip)) {
+		mc_bist_cmd = MC_BIST_CMD;
+		mc_bist_cmd_addr = MC_BIST_CMD_ADDR;
+		mc_bist_cmd_len = MC_BIST_CMD_LEN;
+		mc_bist_status_rdata = MC_BIST_STATUS_RDATA;
+		mc_bist_data_pattern = MC_BIST_DATA_PATTERN;
+	} else {
+		mc_bist_cmd = MC_REG(MC_P_BIST_CMD, idx);
+		mc_bist_cmd_addr = MC_REG(MC_P_BIST_CMD_ADDR, idx);
+		mc_bist_cmd_len = MC_REG(MC_P_BIST_CMD_LEN, idx);
+		mc_bist_status_rdata = MC_REG(MC_P_BIST_STATUS_RDATA, idx);
+		mc_bist_data_pattern = MC_REG(MC_P_BIST_DATA_PATTERN, idx);
+	}
 
-	if (t4_read_reg(adap, MC_BIST_CMD) & START_BIST)
+	if (t4_read_reg(adap, mc_bist_cmd) & START_BIST)
 		return -EBUSY;
-	t4_write_reg(adap, MC_BIST_CMD_ADDR, addr & ~0x3fU);
-	t4_write_reg(adap, MC_BIST_CMD_LEN, 64);
-	t4_write_reg(adap, MC_BIST_DATA_PATTERN, 0xc);
-	t4_write_reg(adap, MC_BIST_CMD, BIST_OPCODE(1) | START_BIST |
+	t4_write_reg(adap, mc_bist_cmd_addr, addr & ~0x3fU);
+	t4_write_reg(adap, mc_bist_cmd_len, 64);
+	t4_write_reg(adap, mc_bist_data_pattern, 0xc);
+	t4_write_reg(adap, mc_bist_cmd, BIST_OPCODE(1) | START_BIST |
 		     BIST_CMD_GAP(1));
-	i = t4_wait_op_done(adap, MC_BIST_CMD, START_BIST, 0, 10, 1);
+	i = t4_wait_op_done(adap, mc_bist_cmd, START_BIST, 0, 10, 1);
 	if (i)
 		return i;
 
-#define MC_DATA(i) MC_BIST_STATUS_REG(MC_BIST_STATUS_RDATA, i)
+#define MC_DATA(i) MC_BIST_STATUS_REG(mc_bist_status_rdata, i)
 
 	for (i = 15; i >= 0; i--)
 		*data++ = htonl(t4_read_reg(adap, MC_DATA(i)));
@@ -329,20 +346,39 @@ int t4_mc_read(struct adapter *adap, u32 addr, __be32 *data, u64 *ecc)
 int t4_edc_read(struct adapter *adap, int idx, u32 addr, __be32 *data, u64 *ecc)
 {
 	int i;
+	u32 edc_bist_cmd, edc_bist_cmd_addr, edc_bist_cmd_len;
+	u32 edc_bist_cmd_data_pattern, edc_bist_status_rdata;
+
+	if (is_t4(adap->chip)) {
+		edc_bist_cmd = EDC_REG(EDC_BIST_CMD, idx);
+		edc_bist_cmd_addr = EDC_REG(EDC_BIST_CMD_ADDR, idx);
+		edc_bist_cmd_len = EDC_REG(EDC_BIST_CMD_LEN, idx);
+		edc_bist_cmd_data_pattern = EDC_REG(EDC_BIST_DATA_PATTERN,
+						    idx);
+		edc_bist_status_rdata = EDC_REG(EDC_BIST_STATUS_RDATA,
+						    idx);
+	} else {
+		edc_bist_cmd = EDC_REG_T5(EDC_H_BIST_CMD, idx);
+		edc_bist_cmd_addr = EDC_REG_T5(EDC_H_BIST_CMD_ADDR, idx);
+		edc_bist_cmd_len = EDC_REG_T5(EDC_H_BIST_CMD_LEN, idx);
+		edc_bist_cmd_data_pattern =
+			EDC_REG_T5(EDC_H_BIST_DATA_PATTERN, idx);
+		edc_bist_status_rdata =
+			 EDC_REG_T5(EDC_H_BIST_STATUS_RDATA, idx);
+	}
 
-	idx *= EDC_STRIDE;
-	if (t4_read_reg(adap, EDC_BIST_CMD + idx) & START_BIST)
+	if (t4_read_reg(adap, edc_bist_cmd) & START_BIST)
 		return -EBUSY;
-	t4_write_reg(adap, EDC_BIST_CMD_ADDR + idx, addr & ~0x3fU);
-	t4_write_reg(adap, EDC_BIST_CMD_LEN + idx, 64);
-	t4_write_reg(adap, EDC_BIST_DATA_PATTERN + idx, 0xc);
-	t4_write_reg(adap, EDC_BIST_CMD + idx,
+	t4_write_reg(adap, edc_bist_cmd_addr, addr & ~0x3fU);
+	t4_write_reg(adap, edc_bist_cmd_len, 64);
+	t4_write_reg(adap, edc_bist_cmd_data_pattern, 0xc);
+	t4_write_reg(adap, edc_bist_cmd,
 		     BIST_OPCODE(1) | BIST_CMD_GAP(1) | START_BIST);
-	i = t4_wait_op_done(adap, EDC_BIST_CMD + idx, START_BIST, 0, 10, 1);
+	i = t4_wait_op_done(adap, edc_bist_cmd, START_BIST, 0, 10, 1);
 	if (i)
 		return i;
 
-#define EDC_DATA(i) (EDC_BIST_STATUS_REG(EDC_BIST_STATUS_RDATA, i) + idx)
+#define EDC_DATA(i) (EDC_BIST_STATUS_REG(edc_bist_status_rdata, i))
 
 	for (i = 15; i >= 0; i--)
 		*data++ = htonl(t4_read_reg(adap, EDC_DATA(i)));
@@ -366,6 +402,7 @@ int t4_edc_read(struct adapter *adap, int idx, u32 addr, __be32 *data, u64 *ecc)
 static int t4_mem_win_rw(struct adapter *adap, u32 addr, __be32 *data, int dir)
 {
 	int i;
+	u32 win_pf = is_t4(adap->chip) ? 0 : V_PFNUM(adap->fn);
 
 	/*
 	 * Setup offset into PCIE memory window.  Address must be a
@@ -374,7 +411,7 @@ static int t4_mem_win_rw(struct adapter *adap, u32 addr, __be32 *data, int dir)
 	 * values.)
 	 */
 	t4_write_reg(adap, PCIE_MEM_ACCESS_OFFSET,
-		     addr & ~(MEMWIN0_APERTURE - 1));
+		     (addr & ~(MEMWIN0_APERTURE - 1)) | win_pf);
 	t4_read_reg(adap, PCIE_MEM_ACCESS_OFFSET);
 
 	/* Collecting data 4 bytes at a time upto MEMWIN0_APERTURE */
@@ -410,6 +447,7 @@ static int t4_memory_rw(struct adapter *adap, int mtype, u32 addr, u32 len,
 			__be32 *buf, int dir)
 {
 	u32 pos, start, end, offset, memoffset;
+	u32 edc_size, mc_size;
 	int ret = 0;
 	__be32 *data;
 
@@ -423,13 +461,21 @@ static int t4_memory_rw(struct adapter *adap, int mtype, u32 addr, u32 len,
 	if (!data)
 		return -ENOMEM;
 
-	/*
-	 * Offset into the region of memory which is being accessed
+	/* Offset into the region of memory which is being accessed
 	 * MEM_EDC0 = 0
 	 * MEM_EDC1 = 1
-	 * MEM_MC   = 2
+	 * MEM_MC   = 2 -- T4
+	 * MEM_MC0  = 2 -- For T5
+	 * MEM_MC1  = 3 -- For T5
 	 */
-	memoffset = (mtype * (5 * 1024 * 1024));
+	edc_size  = EDRAM_SIZE_GET(t4_read_reg(adap, MA_EDRAM0_BAR));
+	if (mtype != MEM_MC1)
+		memoffset = (mtype * (edc_size * 1024 * 1024));
+	else {
+		mc_size = EXT_MEM_SIZE_GET(t4_read_reg(adap,
+						       MA_EXT_MEMORY_BAR));
+		memoffset = (MEM_MC0 * edc_size + mc_size) * 1024 * 1024;
+	}
 
 	/* Determine the PCIE_MEM_ACCESS_OFFSET */
 	addr = addr + memoffset;
@@ -497,9 +543,9 @@ int t4_memory_write(struct adapter *adap, int mtype, u32 addr, u32 len,
 }
 
 #define EEPROM_STAT_ADDR   0x7bfc
-#define VPD_LEN            512
 #define VPD_BASE           0x400
 #define VPD_BASE_OLD       0
+#define VPD_LEN            1024
 
 /**
  *	t4_seeprom_wp - enable/disable EEPROM write protection
@@ -856,6 +902,7 @@ int t4_check_fw_version(struct adapter *adapter)
 {
 	u32 api_vers[2];
 	int ret, major, minor, micro;
+	int exp_major, exp_minor, exp_micro;
 
 	ret = get_fw_version(adapter, &adapter->params.fw_vers);
 	if (!ret)
@@ -870,17 +917,35 @@ int t4_check_fw_version(struct adapter *adapter)
 	major = FW_HDR_FW_VER_MAJOR_GET(adapter->params.fw_vers);
 	minor = FW_HDR_FW_VER_MINOR_GET(adapter->params.fw_vers);
 	micro = FW_HDR_FW_VER_MICRO_GET(adapter->params.fw_vers);
+
+	switch (CHELSIO_CHIP_VERSION(adapter->chip)) {
+	case CHELSIO_T4:
+		exp_major = FW_VERSION_MAJOR;
+		exp_minor = FW_VERSION_MINOR;
+		exp_micro = FW_VERSION_MICRO;
+		break;
+	case CHELSIO_T5:
+		exp_major = FW_VERSION_MAJOR_T5;
+		exp_minor = FW_VERSION_MINOR_T5;
+		exp_micro = FW_VERSION_MICRO_T5;
+		break;
+	default:
+		dev_err(adapter->pdev_dev, "Unsupported chip type, %x\n",
+			adapter->chip);
+		return -EINVAL;
+	}
+
 	memcpy(adapter->params.api_vers, api_vers,
 	       sizeof(adapter->params.api_vers));
 
-	if (major != FW_VERSION_MAJOR) {            /* major mismatch - fail */
+	if (major != exp_major) {            /* major mismatch - fail */
 		dev_err(adapter->pdev_dev,
 			"card FW has major version %u, driver wants %u\n",
-			major, FW_VERSION_MAJOR);
+			major, exp_major);
 		return -EINVAL;
 	}
 
-	if (minor == FW_VERSION_MINOR && micro == FW_VERSION_MICRO)
+	if (minor == exp_minor && micro == exp_micro)
 		return 0;                                   /* perfect match */
 
 	/* Minor/micro version mismatch.  Report it but often it's OK. */
@@ -1246,6 +1311,45 @@ static void pcie_intr_handler(struct adapter *adapter)
 		{ 0 }
 	};
 
+	static struct intr_info t5_pcie_intr_info[] = {
+		{ MSTGRPPERR, "Master Response Read Queue parity error",
+		  -1, 1 },
+		{ MSTTIMEOUTPERR, "Master Timeout FIFO parity error", -1, 1 },
+		{ MSIXSTIPERR, "MSI-X STI SRAM parity error", -1, 1 },
+		{ MSIXADDRLPERR, "MSI-X AddrL parity error", -1, 1 },
+		{ MSIXADDRHPERR, "MSI-X AddrH parity error", -1, 1 },
+		{ MSIXDATAPERR, "MSI-X data parity error", -1, 1 },
+		{ MSIXDIPERR, "MSI-X DI parity error", -1, 1 },
+		{ PIOCPLGRPPERR, "PCI PIO completion Group FIFO parity error",
+		  -1, 1 },
+		{ PIOREQGRPPERR, "PCI PIO request Group FIFO parity error",
+		  -1, 1 },
+		{ TARTAGPERR, "PCI PCI target tag FIFO parity error", -1, 1 },
+		{ MSTTAGQPERR, "PCI master tag queue parity error", -1, 1 },
+		{ CREQPERR, "PCI CMD channel request parity error", -1, 1 },
+		{ CRSPPERR, "PCI CMD channel response parity error", -1, 1 },
+		{ DREQWRPERR, "PCI DMA channel write request parity error",
+		  -1, 1 },
+		{ DREQPERR, "PCI DMA channel request parity error", -1, 1 },
+		{ DRSPPERR, "PCI DMA channel response parity error", -1, 1 },
+		{ HREQWRPERR, "PCI HMA channel count parity error", -1, 1 },
+		{ HREQPERR, "PCI HMA channel request parity error", -1, 1 },
+		{ HRSPPERR, "PCI HMA channel response parity error", -1, 1 },
+		{ CFGSNPPERR, "PCI config snoop FIFO parity error", -1, 1 },
+		{ FIDPERR, "PCI FID parity error", -1, 1 },
+		{ VFIDPERR, "PCI INTx clear parity error", -1, 1 },
+		{ MAGRPPERR, "PCI MA group FIFO parity error", -1, 1 },
+		{ PIOTAGPERR, "PCI PIO tag parity error", -1, 1 },
+		{ IPRXHDRGRPPERR, "PCI IP Rx header group parity error",
+		  -1, 1 },
+		{ IPRXDATAGRPPERR, "PCI IP Rx data group parity error", -1, 1 },
+		{ RPLPERR, "PCI IP replay buffer parity error", -1, 1 },
+		{ IPSOTPERR, "PCI IP SOT buffer parity error", -1, 1 },
+		{ TRGT1GRPPERR, "PCI TRGT1 group FIFOs parity error", -1, 1 },
+		{ READRSPERR, "Outbound read error", -1, 0 },
+		{ 0 }
+	};
+
 	int fat;
 
 	fat = t4_handle_intr_status(adapter,
@@ -1254,7 +1358,10 @@ static void pcie_intr_handler(struct adapter *adapter)
 	      t4_handle_intr_status(adapter,
 				    PCIE_CORE_UTL_PCI_EXPRESS_PORT_STATUS,
 				    pcie_port_intr_info) +
-	      t4_handle_intr_status(adapter, PCIE_INT_CAUSE, pcie_intr_info);
+	      t4_handle_intr_status(adapter, PCIE_INT_CAUSE,
+				    is_t4(adapter->chip) ?
+				    pcie_intr_info : t5_pcie_intr_info);
+
 	if (fat)
 		t4_fatal_err(adapter);
 }
@@ -1664,7 +1771,14 @@ static void ncsi_intr_handler(struct adapter *adap)
  */
 static void xgmac_intr_handler(struct adapter *adap, int port)
 {
-	u32 v = t4_read_reg(adap, PORT_REG(port, XGMAC_PORT_INT_CAUSE));
+	u32 v, int_cause_reg;
+
+	if (is_t4(adap->chip))
+		int_cause_reg = PORT_REG(port, XGMAC_PORT_INT_CAUSE);
+	else
+		int_cause_reg = T5_PORT_REG(port, MAC_PORT_INT_CAUSE);
+
+	v = t4_read_reg(adap, int_cause_reg);
 
 	v &= TXFIFO_PRTY_ERR | RXFIFO_PRTY_ERR;
 	if (!v)
@@ -2126,7 +2240,9 @@ void t4_get_port_stats(struct adapter *adap, int idx, struct port_stats *p)
 	u32 bgmap = get_mps_bg_map(adap, idx);
 
 #define GET_STAT(name) \
-	t4_read_reg64(adap, PORT_REG(idx, MPS_PORT_STAT_##name##_L))
+	t4_read_reg64(adap, \
+	(is_t4(adap->chip) ? PORT_REG(idx, MPS_PORT_STAT_##name##_L) : \
+	T5_PORT_REG(idx, MPS_PORT_STAT_##name##_L)))
 #define GET_STAT_COM(name) t4_read_reg64(adap, MPS_STAT_##name##_L)
 
 	p->tx_octets           = GET_STAT(TX_PORT_BYTES);
@@ -2205,14 +2321,26 @@ void t4_get_port_stats(struct adapter *adap, int idx, struct port_stats *p)
 void t4_wol_magic_enable(struct adapter *adap, unsigned int port,
 			 const u8 *addr)
 {
+	u32 mag_id_reg_l, mag_id_reg_h, port_cfg_reg;
+
+	if (is_t4(adap->chip)) {
+		mag_id_reg_l = PORT_REG(port, XGMAC_PORT_MAGIC_MACID_LO);
+		mag_id_reg_h = PORT_REG(port, XGMAC_PORT_MAGIC_MACID_HI);
+		port_cfg_reg = PORT_REG(port, XGMAC_PORT_CFG2);
+	} else {
+		mag_id_reg_l = T5_PORT_REG(port, MAC_PORT_MAGIC_MACID_LO);
+		mag_id_reg_h = T5_PORT_REG(port, MAC_PORT_MAGIC_MACID_HI);
+		port_cfg_reg = T5_PORT_REG(port, MAC_PORT_CFG2);
+	}
+
 	if (addr) {
-		t4_write_reg(adap, PORT_REG(port, XGMAC_PORT_MAGIC_MACID_LO),
+		t4_write_reg(adap, mag_id_reg_l,
 			     (addr[2] << 24) | (addr[3] << 16) |
 			     (addr[4] << 8) | addr[5]);
-		t4_write_reg(adap, PORT_REG(port, XGMAC_PORT_MAGIC_MACID_HI),
+		t4_write_reg(adap, mag_id_reg_h,
 			     (addr[0] << 8) | addr[1]);
 	}
-	t4_set_reg_field(adap, PORT_REG(port, XGMAC_PORT_CFG2), MAGICEN,
+	t4_set_reg_field(adap, port_cfg_reg, MAGICEN,
 			 addr ? MAGICEN : 0);
 }
 
@@ -2235,16 +2363,23 @@ int t4_wol_pat_enable(struct adapter *adap, unsigned int port, unsigned int map,
 		      u64 mask0, u64 mask1, unsigned int crc, bool enable)
 {
 	int i;
+	u32 port_cfg_reg;
+
+	if (is_t4(adap->chip))
+		port_cfg_reg = PORT_REG(port, XGMAC_PORT_CFG2);
+	else
+		port_cfg_reg = T5_PORT_REG(port, MAC_PORT_CFG2);
 
 	if (!enable) {
-		t4_set_reg_field(adap, PORT_REG(port, XGMAC_PORT_CFG2),
-				 PATEN, 0);
+		t4_set_reg_field(adap, port_cfg_reg, PATEN, 0);
 		return 0;
 	}
 	if (map > 0xff)
 		return -EINVAL;
 
-#define EPIO_REG(name) PORT_REG(port, XGMAC_PORT_EPIO_##name)
+#define EPIO_REG(name) \
+	(is_t4(adap->chip) ? PORT_REG(port, XGMAC_PORT_EPIO_##name) : \
+	T5_PORT_REG(port, MAC_PORT_EPIO_##name))
 
 	t4_write_reg(adap, EPIO_REG(DATA1), mask0 >> 32);
 	t4_write_reg(adap, EPIO_REG(DATA2), mask1);
@@ -2322,24 +2457,24 @@ int t4_fwaddrspace_write(struct adapter *adap, unsigned int mbox,
  *     @addr: address of first byte requested aligned on 32b.
  *     @data: len bytes to hold the data read
  *     @len: amount of data to read from window.  Must be <=
- *            MEMWIN0_APERATURE after adjusting for 16B alignment
- *            requirements of the the memory window.
+ *            MEMWIN0_APERATURE after adjusting for 16B for T4 and
+ *            128B for T5 alignment requirements of the the memory window.
  *
  *     Read len bytes of data from MC starting at @addr.
  */
 int t4_mem_win_read_len(struct adapter *adap, u32 addr, __be32 *data, int len)
 {
-	int i;
-	int off;
+	int i, off;
+	u32 win_pf = is_t4(adap->chip) ? 0 : V_PFNUM(adap->fn);
 
-	/*
-	 * Align on a 16B boundary.
+	/* Align on a 2KB boundary.
 	 */
-	off = addr & 15;
+	off = addr & MEMWIN0_APERTURE;
 	if ((addr & 3) || (len + off) > MEMWIN0_APERTURE)
 		return -EINVAL;
 
-	t4_write_reg(adap, PCIE_MEM_ACCESS_OFFSET, addr & ~15);
+	t4_write_reg(adap, PCIE_MEM_ACCESS_OFFSET,
+		     (addr & ~MEMWIN0_APERTURE) | win_pf);
 	t4_read_reg(adap, PCIE_MEM_ACCESS_OFFSET);
 
 	for (i = 0; i < len; i += 4)
@@ -3162,6 +3297,9 @@ int t4_alloc_mac_filt(struct adapter *adap, unsigned int mbox,
 	int i, ret;
 	struct fw_vi_mac_cmd c;
 	struct fw_vi_mac_exact *p;
+	unsigned int max_naddr = is_t4(adap->chip) ?
+				       NUM_MPS_CLS_SRAM_L_INSTANCES :
+				       NUM_MPS_T5_CLS_SRAM_L_INSTANCES;
 
 	if (naddr > 7)
 		return -EINVAL;
@@ -3187,8 +3325,8 @@ int t4_alloc_mac_filt(struct adapter *adap, unsigned int mbox,
 		u16 index = FW_VI_MAC_CMD_IDX_GET(ntohs(p->valid_to_idx));
 
 		if (idx)
-			idx[i] = index >= NEXACT_MAC ? 0xffff : index;
-		if (index < NEXACT_MAC)
+			idx[i] = index >= max_naddr ? 0xffff : index;
+		if (index < max_naddr)
 			ret++;
 		else if (hash)
 			*hash |= (1ULL << hash_mac_addr(addr[i]));
@@ -3221,6 +3359,9 @@ int t4_change_mac(struct adapter *adap, unsigned int mbox, unsigned int viid,
 	int ret, mode;
 	struct fw_vi_mac_cmd c;
 	struct fw_vi_mac_exact *p = c.u.exact;
+	unsigned int max_mac_addr = is_t4(adap->chip) ?
+				    NUM_MPS_CLS_SRAM_L_INSTANCES :
+				    NUM_MPS_T5_CLS_SRAM_L_INSTANCES;
 
 	if (idx < 0)                             /* new allocation */
 		idx = persist ? FW_VI_MAC_ADD_PERSIST_MAC : FW_VI_MAC_ADD_MAC;
@@ -3238,7 +3379,7 @@ int t4_change_mac(struct adapter *adap, unsigned int mbox, unsigned int viid,
 	ret = t4_wr_mbox(adap, mbox, &c, sizeof(c), &c);
 	if (ret == 0) {
 		ret = FW_VI_MAC_CMD_IDX_GET(ntohs(p->valid_to_idx));
-		if (ret >= NEXACT_MAC)
+		if (ret >= max_mac_addr)
 			ret = -ENOMEM;
 	}
 	return ret;
@@ -3547,7 +3688,8 @@ static int get_flash_params(struct adapter *adap)
  */
 int t4_prep_adapter(struct adapter *adapter)
 {
-	int ret;
+	int ret, ver;
+	uint16_t device_id;
 
 	ret = t4_wait_dev_ready(adapter);
 	if (ret < 0)
@@ -3562,6 +3704,28 @@ int t4_prep_adapter(struct adapter *adapter)
 		return ret;
 	}
 
+	/* Retrieve adapter's device ID
+	 */
+	pci_read_config_word(adapter->pdev, PCI_DEVICE_ID, &device_id);
+	ver = device_id >> 12;
+	switch (ver) {
+	case CHELSIO_T4:
+		adapter->chip = CHELSIO_CHIP_CODE(CHELSIO_T4,
+						  adapter->params.rev);
+		break;
+	case CHELSIO_T5:
+		adapter->chip = CHELSIO_CHIP_CODE(CHELSIO_T5,
+						  adapter->params.rev);
+		break;
+	default:
+		dev_err(adapter->pdev_dev, "Device %d is not supported\n",
+			device_id);
+		return -EINVAL;
+	}
+
+	/* Reassign the updated revision field */
+	adapter->params.rev = adapter->chip;
+
 	init_cong_ctrl(adapter->params.a_wnd, adapter->params.b_wnd);
 
 	/*
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h
index f534ed7e10e9..1d1623be9f1e 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h
@@ -47,7 +47,6 @@ enum {
 	TCB_SIZE       = 128,   /* TCB size */
 	NMTUS          = 16,    /* size of MTU table */
 	NCCTRL_WIN     = 32,    /* # of congestion control windows */
-	NEXACT_MAC     = 336,   /* # of exact MAC address filters */
 	L2T_SIZE       = 4096,  /* # of L2T entries */
 	MBOX_LEN       = 64,    /* mailbox size in bytes */
 	TRACE_LEN      = 112,   /* length of trace data and mask */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h b/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
index 261d17703adc..01d484441200 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
@@ -74,6 +74,7 @@ enum {
 	CPL_PASS_ESTABLISH    = 0x41,
 	CPL_RX_DATA_DDP       = 0x42,
 	CPL_PASS_ACCEPT_REQ   = 0x44,
+	CPL_TRACE_PKT_T5      = 0x48,
 
 	CPL_RDMA_READ_REQ     = 0x60,
 
@@ -157,6 +158,7 @@ union opcode_tid {
 };
 
 #define CPL_OPCODE(x) ((x) << 24)
+#define G_CPL_OPCODE(x) (((x) >> 24) & 0xFF)
 #define MK_OPCODE_TID(opcode, tid) (CPL_OPCODE(opcode) | (tid))
 #define OPCODE_TID(cmd) ((cmd)->ot.opcode_tid)
 #define GET_TID(cmd) (ntohl(OPCODE_TID(cmd)) & 0xFFFFFF)
@@ -287,6 +289,23 @@ struct cpl_act_open_req {
 	__be32 opt2;
 };
 
+#define S_FILTER_TUPLE  24
+#define M_FILTER_TUPLE  0xFFFFFFFFFF
+#define V_FILTER_TUPLE(x) ((x) << S_FILTER_TUPLE)
+#define G_FILTER_TUPLE(x) (((x) >> S_FILTER_TUPLE) & M_FILTER_TUPLE)
+struct cpl_t5_act_open_req {
+	WR_HDR;
+	union opcode_tid ot;
+	__be16 local_port;
+	__be16 peer_port;
+	__be32 local_ip;
+	__be32 peer_ip;
+	__be64 opt0;
+	__be32 rsvd;
+	__be32 opt2;
+	__be64 params;
+};
+
 struct cpl_act_open_req6 {
 	WR_HDR;
 	union opcode_tid ot;
@@ -566,6 +585,11 @@ struct cpl_rx_pkt {
 #define V_RX_ETHHDR_LEN(x) ((x) << S_RX_ETHHDR_LEN)
 #define G_RX_ETHHDR_LEN(x) (((x) >> S_RX_ETHHDR_LEN) & M_RX_ETHHDR_LEN)
 
+#define S_RX_T5_ETHHDR_LEN    0
+#define M_RX_T5_ETHHDR_LEN    0x3F
+#define V_RX_T5_ETHHDR_LEN(x) ((x) << S_RX_T5_ETHHDR_LEN)
+#define G_RX_T5_ETHHDR_LEN(x) (((x) >> S_RX_T5_ETHHDR_LEN) & M_RX_T5_ETHHDR_LEN)
+
 #define S_RX_MACIDX    8
 #define M_RX_MACIDX    0x1FF
 #define V_RX_MACIDX(x) ((x) << S_RX_MACIDX)
@@ -612,6 +636,28 @@ struct cpl_trace_pkt {
 	__be64 tstamp;
 };
 
+struct cpl_t5_trace_pkt {
+	__u8 opcode;
+	__u8 intf;
+#if defined(__LITTLE_ENDIAN_BITFIELD)
+	__u8 runt:4;
+	__u8 filter_hit:4;
+	__u8:6;
+	__u8 err:1;
+	__u8 trunc:1;
+#else
+	__u8 filter_hit:4;
+	__u8 runt:4;
+	__u8 trunc:1;
+	__u8 err:1;
+	__u8:6;
+#endif
+	__be16 rsvd;
+	__be16 len;
+	__be64 tstamp;
+	__be64 rsvd1;
+};
+
 struct cpl_l2t_write_req {
 	WR_HDR;
 	union opcode_tid ot;
@@ -643,6 +689,15 @@ struct cpl_sge_egr_update {
 	__be16 pidx;
 };
 
+/* cpl_fw*.type values */
+enum {
+	FW_TYPE_CMD_RPL = 0,
+	FW_TYPE_WR_RPL = 1,
+	FW_TYPE_CQE = 2,
+	FW_TYPE_OFLD_CONNECTION_WR_RPL = 3,
+	FW_TYPE_RSSCPL = 4,
+};
+
 struct cpl_fw4_pld {
 	u8 opcode;
 	u8 rsvd0[3];
@@ -692,6 +747,7 @@ enum {
 	FW6_TYPE_WR_RPL = 1,
 	FW6_TYPE_CQE = 2,
 	FW6_TYPE_OFLD_CONNECTION_WR_RPL = 3,
+	FW6_TYPE_RSSCPL = FW_TYPE_RSSCPL,
 };
 
 struct cpl_fw6_msg_ofld_connection_wr_rpl {
@@ -742,4 +798,12 @@ struct ulp_mem_io {
 #define ULP_MEMIO_LOCK(x) ((x) << 31)
 };
 
+#define S_T5_ULP_MEMIO_IMM    23
+#define V_T5_ULP_MEMIO_IMM(x) ((x) << S_T5_ULP_MEMIO_IMM)
+#define F_T5_ULP_MEMIO_IMM    V_T5_ULP_MEMIO_IMM(1U)
+
+#define S_T5_ULP_MEMIO_ORDER    22
+#define V_T5_ULP_MEMIO_ORDER(x) ((x) << S_T5_ULP_MEMIO_ORDER)
+#define F_T5_ULP_MEMIO_ORDER    V_T5_ULP_MEMIO_ORDER(1U)
+
 #endif  /* __T4_MSG_H */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
index 83ec5f7844ac..ef146c0ba481 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
@@ -68,9 +68,14 @@
 #define  QID_SHIFT   15
 #define  QID(x)      ((x) << QID_SHIFT)
 #define  DBPRIO(x)   ((x) << 14)
+#define  DBTYPE(x)   ((x) << 13)
 #define  PIDX_MASK   0x00003fffU
 #define  PIDX_SHIFT  0
 #define  PIDX(x)     ((x) << PIDX_SHIFT)
+#define  S_PIDX_T5   0
+#define  M_PIDX_T5   0x1fffU
+#define  PIDX_T5(x)  (((x) >> S_PIDX_T5) & M_PIDX_T5)
+
 
 #define SGE_PF_GTS 0x4
 #define  INGRESSQID_MASK   0xffff0000U
@@ -152,6 +157,8 @@
 #define  QUEUESPERPAGEPF0_MASK   0x0000000fU
 #define  QUEUESPERPAGEPF0_GET(x) ((x) & QUEUESPERPAGEPF0_MASK)
 
+#define QUEUESPERPAGEPF1    4
+
 #define SGE_INT_CAUSE1 0x1024
 #define SGE_INT_CAUSE2 0x1030
 #define SGE_INT_CAUSE3 0x103c
@@ -234,6 +241,10 @@
 #define SGE_DOORBELL_CONTROL 0x10a8
 #define  ENABLE_DROP        (1 << 13)
 
+#define S_NOCOALESCE    26
+#define V_NOCOALESCE(x) ((x) << S_NOCOALESCE)
+#define F_NOCOALESCE    V_NOCOALESCE(1U)
+
 #define SGE_TIMER_VALUE_0_AND_1 0x10b8
 #define  TIMERVALUE0_MASK   0xffff0000U
 #define  TIMERVALUE0_SHIFT  16
@@ -272,17 +283,36 @@
 #define S_HP_INT_THRESH    28
 #define M_HP_INT_THRESH 0xfU
 #define V_HP_INT_THRESH(x) ((x) << S_HP_INT_THRESH)
+#define S_LP_INT_THRESH_T5    18
+#define V_LP_INT_THRESH_T5(x) ((x) << S_LP_INT_THRESH_T5)
+#define M_LP_COUNT_T5    0x3ffffU
+#define G_LP_COUNT_T5(x) (((x) >> S_LP_COUNT) & M_LP_COUNT_T5)
 #define M_HP_COUNT 0x7ffU
 #define S_HP_COUNT 16
 #define G_HP_COUNT(x) (((x) >> S_HP_COUNT) & M_HP_COUNT)
 #define S_LP_INT_THRESH    12
 #define M_LP_INT_THRESH 0xfU
+#define M_LP_INT_THRESH_T5    0xfffU
 #define V_LP_INT_THRESH(x) ((x) << S_LP_INT_THRESH)
 #define M_LP_COUNT 0x7ffU
 #define S_LP_COUNT 0
 #define G_LP_COUNT(x) (((x) >> S_LP_COUNT) & M_LP_COUNT)
 #define A_SGE_DBFIFO_STATUS 0x10a4
 
+#define SGE_STAT_TOTAL 0x10e4
+#define SGE_STAT_MATCH 0x10e8
+
+#define SGE_STAT_CFG   0x10ec
+#define S_STATSOURCE_T5    9
+#define STATSOURCE_T5(x) ((x) << S_STATSOURCE_T5)
+
+#define SGE_DBFIFO_STATUS2 0x1118
+#define M_HP_COUNT_T5    0x3ffU
+#define G_HP_COUNT_T5(x) ((x)  & M_HP_COUNT_T5)
+#define S_HP_INT_THRESH_T5    10
+#define M_HP_INT_THRESH_T5    0xfU
+#define V_HP_INT_THRESH_T5(x) ((x) << S_HP_INT_THRESH_T5)
+
 #define S_ENABLE_DROP    13
 #define V_ENABLE_DROP(x) ((x) << S_ENABLE_DROP)
 #define F_ENABLE_DROP    V_ENABLE_DROP(1U)
@@ -331,8 +361,27 @@
 #define  MSIADDRHPERR  0x00000002U
 #define  MSIADDRLPERR  0x00000001U
 
+#define  READRSPERR      0x20000000U
+#define  TRGT1GRPPERR    0x10000000U
+#define  IPSOTPERR       0x08000000U
+#define  IPRXDATAGRPPERR 0x02000000U
+#define  IPRXHDRGRPPERR  0x01000000U
+#define  MAGRPPERR       0x00400000U
+#define  VFIDPERR        0x00200000U
+#define  HREQWRPERR      0x00010000U
+#define  DREQWRPERR      0x00002000U
+#define  MSTTAGQPERR     0x00000400U
+#define  PIOREQGRPPERR   0x00000100U
+#define  PIOCPLGRPPERR   0x00000080U
+#define  MSIXSTIPERR     0x00000004U
+#define  MSTTIMEOUTPERR  0x00000002U
+#define  MSTGRPPERR      0x00000001U
+
 #define PCIE_NONFAT_ERR 0x3010
 #define PCIE_MEM_ACCESS_BASE_WIN 0x3068
+#define S_PCIEOFST       10
+#define M_PCIEOFST       0x3fffffU
+#define GET_PCIEOFST(x)  (((x) >> S_PCIEOFST) & M_PCIEOFST)
 #define  PCIEOFST_MASK   0xfffffc00U
 #define  BIR_MASK        0x00000300U
 #define  BIR_SHIFT       8
@@ -342,6 +391,9 @@
 #define  WINDOW(x)       ((x) << WINDOW_SHIFT)
 #define PCIE_MEM_ACCESS_OFFSET 0x306c
 
+#define S_PFNUM    0
+#define V_PFNUM(x) ((x) << S_PFNUM)
+
 #define PCIE_FW 0x30b8
 #define  PCIE_FW_ERR		0x80000000U
 #define  PCIE_FW_INIT		0x40000000U
@@ -407,12 +459,18 @@
 
 #define MC_BIST_STATUS_RDATA 0x7688
 
+#define MA_EDRAM0_BAR 0x77c0
+#define MA_EDRAM1_BAR 0x77c4
+#define EDRAM_SIZE_MASK   0xfffU
+#define EDRAM_SIZE_GET(x) ((x) & EDRAM_SIZE_MASK)
+
 #define MA_EXT_MEMORY_BAR 0x77c8
 #define  EXT_MEM_SIZE_MASK   0x00000fffU
 #define  EXT_MEM_SIZE_SHIFT  0
 #define  EXT_MEM_SIZE_GET(x) (((x) & EXT_MEM_SIZE_MASK) >> EXT_MEM_SIZE_SHIFT)
 
 #define MA_TARGET_MEM_ENABLE 0x77d8
+#define  EXT_MEM1_ENABLE 0x00000010U
 #define  EXT_MEM_ENABLE 0x00000004U
 #define  EDRAM1_ENABLE  0x00000002U
 #define  EDRAM0_ENABLE  0x00000001U
@@ -431,6 +489,7 @@
 #define MA_PCIE_FW 0x30b8
 #define MA_PARITY_ERROR_STATUS 0x77f4
 
+#define MA_EXT_MEMORY1_BAR 0x7808
 #define EDC_0_BASE_ADDR 0x7900
 
 #define EDC_BIST_CMD 0x7904
@@ -801,6 +860,15 @@
 #define MPS_PORT_STAT_RX_PORT_PPP7_H 0x60c
 #define MPS_PORT_STAT_RX_PORT_LESS_64B_L 0x610
 #define MPS_PORT_STAT_RX_PORT_LESS_64B_H 0x614
+#define MAC_PORT_CFG2 0x818
+#define MAC_PORT_MAGIC_MACID_LO 0x824
+#define MAC_PORT_MAGIC_MACID_HI 0x828
+#define MAC_PORT_EPIO_DATA0 0x8c0
+#define MAC_PORT_EPIO_DATA1 0x8c4
+#define MAC_PORT_EPIO_DATA2 0x8c8
+#define MAC_PORT_EPIO_DATA3 0x8cc
+#define MAC_PORT_EPIO_OP 0x8d0
+
 #define MPS_CMN_CTL 0x9000
 #define  NUMPORTS_MASK   0x00000003U
 #define  NUMPORTS_SHIFT  0
@@ -1063,6 +1131,7 @@
 #define  ADDRESS_SHIFT  0
 #define  ADDRESS(x)     ((x) << ADDRESS_SHIFT)
 
+#define MAC_PORT_INT_CAUSE 0x8dc
 #define XGMAC_PORT_INT_CAUSE 0x10dc
 
 #define A_TP_TX_MOD_QUEUE_REQ_MAP 0x7e28
@@ -1101,4 +1170,33 @@
 #define V_PORT(x) ((x) << S_PORT)
 #define F_PORT    V_PORT(1U)
 
+#define NUM_MPS_CLS_SRAM_L_INSTANCES 336
+#define NUM_MPS_T5_CLS_SRAM_L_INSTANCES 512
+
+#define T5_PORT0_BASE 0x30000
+#define T5_PORT_STRIDE 0x4000
+#define T5_PORT_BASE(idx) (T5_PORT0_BASE + (idx) * T5_PORT_STRIDE)
+#define T5_PORT_REG(idx, reg) (T5_PORT_BASE(idx) + (reg))
+
+#define MC_0_BASE_ADDR 0x40000
+#define MC_1_BASE_ADDR 0x48000
+#define MC_STRIDE (MC_1_BASE_ADDR - MC_0_BASE_ADDR)
+#define MC_REG(reg, idx) (reg + MC_STRIDE * idx)
+
+#define MC_P_BIST_CMD 0x41400
+#define MC_P_BIST_CMD_ADDR 0x41404
+#define MC_P_BIST_CMD_LEN 0x41408
+#define MC_P_BIST_DATA_PATTERN 0x4140c
+#define MC_P_BIST_STATUS_RDATA 0x41488
+#define EDC_T50_BASE_ADDR 0x50000
+#define EDC_H_BIST_CMD 0x50004
+#define EDC_H_BIST_CMD_ADDR 0x50008
+#define EDC_H_BIST_CMD_LEN 0x5000c
+#define EDC_H_BIST_DATA_PATTERN 0x50010
+#define EDC_H_BIST_STATUS_RDATA 0x50028
+
+#define EDC_T51_BASE_ADDR 0x50800
+#define EDC_STRIDE_T5 (EDC_T51_BASE_ADDR - EDC_T50_BASE_ADDR)
+#define EDC_REG_T5(reg, idx) (reg + EDC_STRIDE_T5 * idx)
+
 #endif /* __T4_REGS_H */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
index a0dcccd846c9..d1c755f78aaf 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
@@ -574,7 +574,7 @@ struct fw_eth_tx_pkt_vm_wr {
 	__be16 vlantci;
 };
 
-#define FW_CMD_MAX_TIMEOUT 3000
+#define FW_CMD_MAX_TIMEOUT 10000
 
 /*
  * If a host driver does a HELLO and discovers that there's already a MASTER
@@ -973,7 +973,9 @@ enum fw_params_param_pfvf {
 	FW_PARAMS_PARAM_PFVF_EQ_START	= 0x2B,
 	FW_PARAMS_PARAM_PFVF_EQ_END	= 0x2C,
 	FW_PARAMS_PARAM_PFVF_ACTIVE_FILTER_START = 0x2D,
-	FW_PARAMS_PARAM_PFVF_ACTIVE_FILTER_END = 0x2E
+	FW_PARAMS_PARAM_PFVF_ACTIVE_FILTER_END = 0x2E,
+	FW_PARAMS_PARAM_PFVF_ETHOFLD_END = 0x30,
+	FW_PARAMS_PARAM_PFVF_CPLFW4MSG_ENCAP = 0x31
 };
 
 /*
@@ -1758,6 +1760,25 @@ enum fw_port_module_type {
 	FW_PORT_MOD_TYPE_NONE = FW_PORT_CMD_MODTYPE_MASK
 };
 
+enum fw_port_mod_sub_type {
+	FW_PORT_MOD_SUB_TYPE_NA,
+	FW_PORT_MOD_SUB_TYPE_MV88E114X = 0x1,
+	FW_PORT_MOD_SUB_TYPE_TN8022 = 0x2,
+	FW_PORT_MOD_SUB_TYPE_AQ1202 = 0x3,
+	FW_PORT_MOD_SUB_TYPE_88x3120 = 0x4,
+	FW_PORT_MOD_SUB_TYPE_BCM84834 = 0x5,
+	FW_PORT_MOD_SUB_TYPE_BT_VSC8634 = 0x8,
+
+	/* The following will never been in the VPD.  They are TWINAX cable
+	 * lengths decoded from SFP+ module i2c PROMs.  These should
+	 * almost certainly go somewhere else ...
+	 */
+	FW_PORT_MOD_SUB_TYPE_TWINAX_1 = 0x9,
+	FW_PORT_MOD_SUB_TYPE_TWINAX_3 = 0xA,
+	FW_PORT_MOD_SUB_TYPE_TWINAX_5 = 0xB,
+	FW_PORT_MOD_SUB_TYPE_TWINAX_7 = 0xC,
+};
+
 /* port stats */
 #define FW_NUM_PORT_STATS 50
 #define FW_NUM_PORT_TX_STATS 23
@@ -2123,11 +2144,11 @@ struct fw_hdr {
 	u8 intfver_ri;
 	u8 intfver_iscsipdu;
 	u8 intfver_iscsi;
+	u8 intfver_fcoepdu;
 	u8 intfver_fcoe;
-	u8 reserved2;
+	__u32   reserved2;
 	__u32   reserved3;
 	__u32   reserved4;
-	__u32   reserved5;
 	__be32  flags;
 	__be32  reserved6[23];
 };
@@ -2137,6 +2158,17 @@ struct fw_hdr {
 #define FW_HDR_FW_VER_MICRO_GET(x) (((x) >> 8) & 0xff)
 #define FW_HDR_FW_VER_BUILD_GET(x) (((x) >> 0) & 0xff)
 
+enum fw_hdr_intfver {
+	FW_HDR_INTFVER_NIC      = 0x00,
+	FW_HDR_INTFVER_VNIC     = 0x00,
+	FW_HDR_INTFVER_OFLD     = 0x00,
+	FW_HDR_INTFVER_RI       = 0x00,
+	FW_HDR_INTFVER_ISCSIPDU = 0x00,
+	FW_HDR_INTFVER_ISCSI    = 0x00,
+	FW_HDR_INTFVER_FCOEPDU  = 0x00,
+	FW_HDR_INTFVER_FCOE     = 0x00,
+};
+
 enum fw_hdr_flags {
 	FW_HDR_FLAGS_RESET_HALT = 0x00000001,
 };
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/adapter.h b/drivers/net/ethernet/chelsio/cxgb4vf/adapter.h
index 68eaa9c88c7d..be5c7ef6ca93 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/adapter.h
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/adapter.h
@@ -344,6 +344,7 @@ struct adapter {
 	unsigned long registered_device_map;
 	unsigned long open_device_map;
 	unsigned long flags;
+	enum chip_type chip;
 	struct adapter_params params;
 
 	/* queue and interrupt resources */
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c b/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
index 56b46ab2d4c5..40c22e7de15c 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
@@ -54,8 +54,8 @@
 /*
  * Generic information about the driver.
  */
-#define DRV_VERSION "1.0.0"
-#define DRV_DESC "Chelsio T4 Virtual Function (VF) Network Driver"
+#define DRV_VERSION "2.0.0-ko"
+#define DRV_DESC "Chelsio T4/T5 Virtual Function (VF) Network Driver"
 
 /*
  * Module Parameters.
@@ -409,6 +409,20 @@ static int fwevtq_handler(struct sge_rspq *rspq, const __be64 *rsp,
 		break;
 	}
 
+	case CPL_FW4_MSG: {
+		/* FW can send EGR_UPDATEs encapsulated in a CPL_FW4_MSG.
+		 */
+		const struct cpl_sge_egr_update *p = (void *)(rsp + 3);
+		opcode = G_CPL_OPCODE(ntohl(p->opcode_qid));
+		if (opcode != CPL_SGE_EGR_UPDATE) {
+			dev_err(adapter->pdev_dev, "unexpected FW4/CPL %#x on FW event queue\n"
+				, opcode);
+			break;
+		}
+		cpl = (void *)p;
+		/*FALLTHROUGH*/
+	}
+
 	case CPL_SGE_EGR_UPDATE: {
 		/*
 		 * We've received an Egress Queue Status Update message.  We
@@ -1050,7 +1064,7 @@ static inline unsigned int mk_adap_vers(const struct adapter *adapter)
 	/*
 	 * Chip version 4, revision 0x3f (cxgb4vf).
 	 */
-	return 4 | (0x3f << 10);
+	return CHELSIO_CHIP_VERSION(adapter->chip) | (0x3f << 10);
 }
 
 /*
@@ -1100,10 +1114,10 @@ static netdev_features_t cxgb4vf_fix_features(struct net_device *dev,
 	 * Since there is no support for separate rx/tx vlan accel
 	 * enable/disable make sure tx flag is always in same state as rx.
 	 */
-	if (features & NETIF_F_HW_VLAN_RX)
-		features |= NETIF_F_HW_VLAN_TX;
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
+		features |= NETIF_F_HW_VLAN_CTAG_TX;
 	else
-		features &= ~NETIF_F_HW_VLAN_TX;
+		features &= ~NETIF_F_HW_VLAN_CTAG_TX;
 
 	return features;
 }
@@ -1114,9 +1128,9 @@ static int cxgb4vf_set_features(struct net_device *dev,
 	struct port_info *pi = netdev_priv(dev);
 	netdev_features_t changed = dev->features ^ features;
 
-	if (changed & NETIF_F_HW_VLAN_RX)
+	if (changed & NETIF_F_HW_VLAN_CTAG_RX)
 		t4vf_set_rxmode(pi->adapter, pi->viid, -1, -1, -1, -1,
-				features & NETIF_F_HW_VLAN_TX, 0);
+				features & NETIF_F_HW_VLAN_CTAG_TX, 0);
 
 	return 0;
 }
@@ -2072,6 +2086,7 @@ static int adap_init0(struct adapter *adapter)
 	struct sge *s = &adapter->sge;
 	unsigned int ethqsets;
 	int err;
+	u32 param, val = 0;
 
 	/*
 	 * Wait for the device to become ready before proceeding ...
@@ -2099,6 +2114,15 @@ static int adap_init0(struct adapter *adapter)
 		return err;
 	}
 
+	switch (adapter->pdev->device >> 12) {
+	case CHELSIO_T4:
+		adapter->chip = CHELSIO_CHIP_CODE(CHELSIO_T4, 0);
+		break;
+	case CHELSIO_T5:
+		adapter->chip = CHELSIO_CHIP_CODE(CHELSIO_T5, 0);
+		break;
+	}
+
 	/*
 	 * Grab basic operational parameters.  These will predominantly have
 	 * been set up by the Physical Function Driver or will be hard coded
@@ -2144,6 +2168,16 @@ static int adap_init0(struct adapter *adapter)
 		return err;
 	}
 
+	/* If we're running on newer firmware, let it know that we're
+	 * prepared to deal with encapsulated CPL messages.  Older
+	 * firmware won't understand this and we'll just get
+	 * unencapsulated messages ...
+	 */
+	param = FW_PARAMS_MNEM(FW_PARAMS_MNEM_PFVF) |
+		FW_PARAMS_PARAM_X(FW_PARAMS_PARAM_PFVF_CPLFW4MSG_ENCAP);
+	val = 1;
+	(void) t4vf_set_params(adapter, 1, &param, &val);
+
 	/*
 	 * Retrieve our RX interrupt holdoff timer values and counter
 	 * threshold values from the SGE parameters.
@@ -2614,11 +2648,12 @@ static int cxgb4vf_pci_probe(struct pci_dev *pdev,
 
 		netdev->hw_features = NETIF_F_SG | TSO_FLAGS |
 			NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
-			NETIF_F_HW_VLAN_RX | NETIF_F_RXCSUM;
+			NETIF_F_HW_VLAN_CTAG_RX | NETIF_F_RXCSUM;
 		netdev->vlan_features = NETIF_F_SG | TSO_FLAGS |
 			NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
 			NETIF_F_HIGHDMA;
-		netdev->features = netdev->hw_features | NETIF_F_HW_VLAN_TX;
+		netdev->features = netdev->hw_features |
+				   NETIF_F_HW_VLAN_CTAG_TX;
 		if (pci_using_dac)
 			netdev->features |= NETIF_F_HIGHDMA;
 
@@ -2888,6 +2923,26 @@ static struct pci_device_id cxgb4vf_pci_tbl[] = {
 	CH_DEVICE(0x480a, 0),   /* T404-bt */
 	CH_DEVICE(0x480d, 0),   /* T480-cr */
 	CH_DEVICE(0x480e, 0),   /* T440-lp-cr */
+	CH_DEVICE(0x5800, 0),	/* T580-dbg */
+	CH_DEVICE(0x5801, 0),	/* T520-cr */
+	CH_DEVICE(0x5802, 0),	/* T522-cr */
+	CH_DEVICE(0x5803, 0),	/* T540-cr */
+	CH_DEVICE(0x5804, 0),	/* T520-bch */
+	CH_DEVICE(0x5805, 0),   /* T540-bch */
+	CH_DEVICE(0x5806, 0),	/* T540-ch */
+	CH_DEVICE(0x5807, 0),	/* T520-so */
+	CH_DEVICE(0x5808, 0),	/* T520-cx */
+	CH_DEVICE(0x5809, 0),	/* T520-bt */
+	CH_DEVICE(0x580a, 0),   /* T504-bt */
+	CH_DEVICE(0x580b, 0),   /* T520-sr */
+	CH_DEVICE(0x580c, 0),   /* T504-bt */
+	CH_DEVICE(0x580d, 0),   /* T580-cr */
+	CH_DEVICE(0x580e, 0),   /* T540-lp-cr */
+	CH_DEVICE(0x580f, 0),   /* Amsterdam */
+	CH_DEVICE(0x5810, 0),   /* T580-lp-cr */
+	CH_DEVICE(0x5811, 0),   /* T520-lp-cr */
+	CH_DEVICE(0x5812, 0),   /* T560-cr */
+	CH_DEVICE(0x5813, 0),   /* T580-cr */
 	{ 0, }
 };
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
index 9488032d6d2d..df296af20bd5 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
@@ -528,17 +528,21 @@ static void unmap_rx_buf(struct adapter *adapter, struct sge_fl *fl)
  */
 static inline void ring_fl_db(struct adapter *adapter, struct sge_fl *fl)
 {
+	u32 val;
+
 	/*
 	 * The SGE keeps track of its Producer and Consumer Indices in terms
 	 * of Egress Queue Units so we can only tell it about integral numbers
 	 * of multiples of Free List Entries per Egress Queue Units ...
 	 */
 	if (fl->pend_cred >= FL_PER_EQ_UNIT) {
+		val = PIDX(fl->pend_cred / FL_PER_EQ_UNIT);
+		if (!is_t4(adapter->chip))
+			val |= DBTYPE(1);
 		wmb();
 		t4_write_reg(adapter, T4VF_SGE_BASE_ADDR + SGE_VF_KDOORBELL,
 			     DBPRIO(1) |
-			     QID(fl->cntxt_id) |
-			     PIDX(fl->pend_cred / FL_PER_EQ_UNIT));
+			     QID(fl->cntxt_id) | val);
 		fl->pend_cred %= FL_PER_EQ_UNIT;
 	}
 }
@@ -1478,7 +1482,8 @@ static void do_gro(struct sge_eth_rxq *rxq, const struct pkt_gl *gl,
 	skb_record_rx_queue(skb, rxq->rspq.idx);
 
 	if (pkt->vlan_ex) {
-		__vlan_hwaccel_put_tag(skb, be16_to_cpu(pkt->vlan));
+		__vlan_hwaccel_put_tag(skb, cpu_to_be16(ETH_P_8021Q),
+					be16_to_cpu(pkt->vlan));
 		rxq->stats.vlan_ex++;
 	}
 	ret = napi_gro_frags(&rxq->rspq.napi);
@@ -1547,7 +1552,7 @@ int t4vf_ethrx_handler(struct sge_rspq *rspq, const __be64 *rsp,
 
 	if (pkt->vlan_ex) {
 		rxq->stats.vlan_ex++;
-		__vlan_hwaccel_put_tag(skb, be16_to_cpu(pkt->vlan));
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), be16_to_cpu(pkt->vlan));
 	}
 
 	netif_receive_skb(skb);
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h
index 283f9d0d37fd..53cbfed21d0b 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h
@@ -38,6 +38,25 @@
 
 #include "../cxgb4/t4fw_api.h"
 
+#define CHELSIO_CHIP_CODE(version, revision) (((version) << 4) | (revision))
+#define CHELSIO_CHIP_VERSION(code) ((code) >> 4)
+#define CHELSIO_CHIP_RELEASE(code) ((code) & 0xf)
+
+#define CHELSIO_T4		0x4
+#define CHELSIO_T5		0x5
+
+enum chip_type {
+	T4_A1 = CHELSIO_CHIP_CODE(CHELSIO_T4, 0),
+	T4_A2 = CHELSIO_CHIP_CODE(CHELSIO_T4, 1),
+	T4_A3 = CHELSIO_CHIP_CODE(CHELSIO_T4, 2),
+	T4_FIRST_REV	= T4_A1,
+	T4_LAST_REV	= T4_A3,
+
+	T5_A1 = CHELSIO_CHIP_CODE(CHELSIO_T5, 0),
+	T5_FIRST_REV	= T5_A1,
+	T5_LAST_REV	= T5_A1,
+};
+
 /*
  * The "len16" field of a Firmware Command Structure ...
  */
@@ -232,6 +251,11 @@ static inline int t4vf_wr_mbox_ns(struct adapter *adapter, const void *cmd,
 	return t4vf_wr_mbox_core(adapter, cmd, size, rpl, false);
 }
 
+static inline int is_t4(enum chip_type chip)
+{
+	return (chip >= T4_FIRST_REV && chip <= T4_LAST_REV);
+}
+
 int t4vf_wait_dev_ready(struct adapter *);
 int t4vf_port_init(struct adapter *, int);
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
index 7127c7b9efde..9f96dc3bb112 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
@@ -1027,8 +1027,11 @@ int t4vf_alloc_mac_filt(struct adapter *adapter, unsigned int viid, bool free,
 	unsigned nfilters = 0;
 	unsigned int rem = naddr;
 	struct fw_vi_mac_cmd cmd, rpl;
+	unsigned int max_naddr = is_t4(adapter->chip) ?
+				 NUM_MPS_CLS_SRAM_L_INSTANCES :
+				 NUM_MPS_T5_CLS_SRAM_L_INSTANCES;
 
-	if (naddr > FW_CLS_TCAM_NUM_ENTRIES)
+	if (naddr > max_naddr)
 		return -EINVAL;
 
 	for (offset = 0; offset < naddr; /**/) {
@@ -1069,10 +1072,10 @@ int t4vf_alloc_mac_filt(struct adapter *adapter, unsigned int viid, bool free,
 
 			if (idx)
 				idx[offset+i] =
-					(index >= FW_CLS_TCAM_NUM_ENTRIES
+					(index >= max_naddr
 					 ? 0xffff
 					 : index);
-			if (index < FW_CLS_TCAM_NUM_ENTRIES)
+			if (index < max_naddr)
 				nfilters++;
 			else if (hash)
 				*hash |= (1ULL << hash_mac_addr(addr[offset+i]));
@@ -1118,6 +1121,9 @@ int t4vf_change_mac(struct adapter *adapter, unsigned int viid,
 	struct fw_vi_mac_exact *p = &cmd.u.exact[0];
 	size_t len16 = DIV_ROUND_UP(offsetof(struct fw_vi_mac_cmd,
 					     u.exact[1]), 16);
+	unsigned int max_naddr = is_t4(adapter->chip) ?
+				 NUM_MPS_CLS_SRAM_L_INSTANCES :
+				 NUM_MPS_T5_CLS_SRAM_L_INSTANCES;
 
 	/*
 	 * If this is a new allocation, determine whether it should be
@@ -1140,7 +1146,7 @@ int t4vf_change_mac(struct adapter *adapter, unsigned int viid,
 	if (ret == 0) {
 		p = &rpl.u.exact[0];
 		ret = FW_VI_MAC_CMD_IDX_GET(be16_to_cpu(p->valid_to_idx));
-		if (ret >= FW_CLS_TCAM_NUM_ENTRIES)
+		if (ret >= max_naddr)
 			ret = -ENOMEM;
 	}
 	return ret;
diff --git a/drivers/net/ethernet/cirrus/cs89x0.c b/drivers/net/ethernet/cirrus/cs89x0.c
index 138446957786..19f642a45f40 100644
--- a/drivers/net/ethernet/cirrus/cs89x0.c
+++ b/drivers/net/ethernet/cirrus/cs89x0.c
@@ -101,23 +101,6 @@ static char version[] __initdata =
  * them to system IRQ numbers. This mapping is card specific and is set to
  * the configuration of the Cirrus Eval board for this chip.
  */
-#if defined(CONFIG_MACH_IXDP2351)
-#define CS89x0_NONISA_IRQ
-static unsigned int netcard_portlist[] __used __initdata = {
-	IXDP2351_VIRT_CS8900_BASE, 0
-};
-static unsigned int cs8900_irq_map[] = {
-	IRQ_IXDP2351_CS8900, 0, 0, 0
-};
-#elif defined(CONFIG_ARCH_IXDP2X01)
-#define CS89x0_NONISA_IRQ
-static unsigned int netcard_portlist[] __used __initdata = {
-	IXDP2X01_CS8900_VIRT_BASE, 0
-};
-static unsigned int cs8900_irq_map[] = {
-	IRQ_IXDP2X01_CS8900, 0, 0, 0
-};
-#else
 #ifndef CONFIG_CS89x0_PLATFORM
 static unsigned int netcard_portlist[] __used __initdata = {
 	0x300, 0x320, 0x340, 0x360, 0x200, 0x220, 0x240,
@@ -127,7 +110,6 @@ static unsigned int cs8900_irq_map[] = {
 	10, 11, 12, 5
 };
 #endif
-#endif
 
 #if DEBUGGING
 static unsigned int net_debug = DEBUGGING;
@@ -210,32 +192,6 @@ static int __init media_fn(char *str)
 __setup("cs89x0_media=", media_fn);
 #endif
 
-#if defined(CONFIG_MACH_IXDP2351)
-static u16
-readword(unsigned long base_addr, int portno)
-{
-	return __raw_readw(base_addr + (portno << 1));
-}
-
-static void
-writeword(unsigned long base_addr, int portno, u16 value)
-{
-	__raw_writew(value, base_addr + (portno << 1));
-}
-#elif defined(CONFIG_ARCH_IXDP2X01)
-static u16
-readword(unsigned long base_addr, int portno)
-{
-	return __raw_readl(base_addr + (portno << 1));
-}
-
-static void
-writeword(unsigned long base_addr, int portno, u16 value)
-{
-	__raw_writel(value, base_addr + (portno << 1));
-}
-#endif
-
 static void readwords(struct net_local *lp, int portno, void *buf, int length)
 {
 	u8 *buf8 = (u8 *)buf;
@@ -478,9 +434,6 @@ dma_rx(struct net_device *dev)
 	/* Malloc up new buffer. */
 	skb = netdev_alloc_skb(dev, length + 2);
 	if (skb == NULL) {
-		/* I don't think we want to do this to a stressed system */
-		cs89_dbg(0, err, "%s: Memory squeeze, dropping packet\n",
-			 dev->name);
 		dev->stats.rx_dropped++;
 
 		/* AKPM: advance bp to the next frame */
@@ -731,9 +684,6 @@ net_rx(struct net_device *dev)
 	/* Malloc up new buffer. */
 	skb = netdev_alloc_skb(dev, length + 2);
 	if (skb == NULL) {
-#if 0		/* Again, this seems a cruel thing to do */
-		pr_warn("%s: Memory squeeze, dropping packet\n", dev->name);
-#endif
 		dev->stats.rx_dropped++;
 		return;
 	}
@@ -908,7 +858,7 @@ net_open(struct net_device *dev)
 			goto bad_out;
 		}
 	} else {
-#if !defined(CS89x0_NONISA_IRQ) && !defined(CONFIG_CS89x0_PLATFORM)
+#if !defined(CONFIG_CS89x0_PLATFORM)
 		if (((1 << dev->irq) & lp->irq_map) == 0) {
 			pr_err("%s: IRQ %d is not in our map of allowable IRQs, which is %x\n",
 			       dev->name, dev->irq, lp->irq_map);
@@ -1321,9 +1271,7 @@ static const struct net_device_ops net_ops = {
 static void __init reset_chip(struct net_device *dev)
 {
 #if !defined(CONFIG_MACH_MX31ADS)
-#if !defined(CS89x0_NONISA_IRQ)
 	struct net_local *lp = netdev_priv(dev);
-#endif /* CS89x0_NONISA_IRQ */
 	int reset_start_time;
 
 	writereg(dev, PP_SelfCTL, readreg(dev, PP_SelfCTL) | POWER_ON_RESET);
@@ -1331,7 +1279,6 @@ static void __init reset_chip(struct net_device *dev)
 	/* wait 30 ms */
 	msleep(30);
 
-#if !defined(CS89x0_NONISA_IRQ)
 	if (lp->chip_type != CS8900) {
 		/* Hardware problem requires PNP registers to be reconfigured after a reset */
 		iowrite16(PP_CS8920_ISAINT, lp->virt_addr + ADD_PORT);
@@ -1344,7 +1291,6 @@ static void __init reset_chip(struct net_device *dev)
 		iowrite8((dev->mem_start >> 8) & 0xff,
 			 lp->virt_addr + DATA_PORT + 1);
 	}
-#endif /* CS89x0_NONISA_IRQ */
 
 	/* Wait until the chip is reset */
 	reset_start_time = jiffies;
@@ -1579,9 +1525,6 @@ cs89x0_probe1(struct net_device *dev, void __iomem *ioaddr, int modular)
 		i = lp->isa_config & INT_NO_MASK;
 #ifndef CONFIG_CS89x0_PLATFORM
 		if (lp->chip_type == CS8900) {
-#ifdef CS89x0_NONISA_IRQ
-			i = cs8900_irq_map[0];
-#else
 			/* Translate the IRQ using the IRQ mapping table. */
 			if (i >= ARRAY_SIZE(cs8900_irq_map))
 				pr_err("invalid ISA interrupt number %d\n", i);
@@ -1599,7 +1542,6 @@ cs89x0_probe1(struct net_device *dev, void __iomem *ioaddr, int modular)
 					lp->irq_map = ((irq_map_buff[0] >> 8) |
 						       (irq_map_buff[1] << 8));
 			}
-#endif
 		}
 #endif
 		if (!dev->irq)
@@ -1978,18 +1920,6 @@ static struct platform_driver cs89x0_driver = {
 	.remove	= cs89x0_platform_remove,
 };
 
-static int __init cs89x0_init(void)
-{
-	return platform_driver_probe(&cs89x0_driver, cs89x0_platform_probe);
-}
-
-module_init(cs89x0_init);
-
-static void __exit cs89x0_cleanup(void)
-{
-	platform_driver_unregister(&cs89x0_driver);
-}
-
-module_exit(cs89x0_cleanup);
+module_platform_driver_probe(cs89x0_driver, cs89x0_platform_probe);
 
 #endif /* CONFIG_CS89x0_PLATFORM */
diff --git a/drivers/net/ethernet/cirrus/ep93xx_eth.c b/drivers/net/ethernet/cirrus/ep93xx_eth.c
index 354cbb78ed50..67b0388b6e68 100644
--- a/drivers/net/ethernet/cirrus/ep93xx_eth.c
+++ b/drivers/net/ethernet/cirrus/ep93xx_eth.c
@@ -887,18 +887,7 @@ static struct platform_driver ep93xx_eth_driver = {
 	},
 };
 
-static int __init ep93xx_eth_init_module(void)
-{
-	printk(KERN_INFO DRV_MODULE_NAME " version " DRV_MODULE_VERSION " loading\n");
-	return platform_driver_register(&ep93xx_eth_driver);
-}
-
-static void __exit ep93xx_eth_cleanup_module(void)
-{
-	platform_driver_unregister(&ep93xx_eth_driver);
-}
+module_platform_driver(ep93xx_eth_driver);
 
-module_init(ep93xx_eth_init_module);
-module_exit(ep93xx_eth_cleanup_module);
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:ep93xx-eth");
diff --git a/drivers/net/ethernet/cisco/enic/enic_dev.c b/drivers/net/ethernet/cisco/enic/enic_dev.c
index bf0fc56dba19..4b6e5695b263 100644
--- a/drivers/net/ethernet/cisco/enic/enic_dev.c
+++ b/drivers/net/ethernet/cisco/enic/enic_dev.c
@@ -212,7 +212,7 @@ int enic_dev_deinit_done(struct enic *enic, int *status)
 }
 
 /* rtnl lock is held */
-int enic_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
+int enic_vlan_rx_add_vid(struct net_device *netdev, __be16 proto, u16 vid)
 {
 	struct enic *enic = netdev_priv(netdev);
 	int err;
@@ -225,7 +225,7 @@ int enic_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
 }
 
 /* rtnl lock is held */
-int enic_vlan_rx_kill_vid(struct net_device *netdev, u16 vid)
+int enic_vlan_rx_kill_vid(struct net_device *netdev, __be16 proto, u16 vid)
 {
 	struct enic *enic = netdev_priv(netdev);
 	int err;
diff --git a/drivers/net/ethernet/cisco/enic/enic_dev.h b/drivers/net/ethernet/cisco/enic/enic_dev.h
index da1cba3c410e..08bded051b93 100644
--- a/drivers/net/ethernet/cisco/enic/enic_dev.h
+++ b/drivers/net/ethernet/cisco/enic/enic_dev.h
@@ -46,8 +46,8 @@ int enic_dev_packet_filter(struct enic *enic, int directed, int multicast,
 	int broadcast, int promisc, int allmulti);
 int enic_dev_add_addr(struct enic *enic, u8 *addr);
 int enic_dev_del_addr(struct enic *enic, u8 *addr);
-int enic_vlan_rx_add_vid(struct net_device *netdev, u16 vid);
-int enic_vlan_rx_kill_vid(struct net_device *netdev, u16 vid);
+int enic_vlan_rx_add_vid(struct net_device *netdev, __be16 proto, u16 vid);
+int enic_vlan_rx_kill_vid(struct net_device *netdev, __be16 proto, u16 vid);
 int enic_dev_notify_unset(struct enic *enic);
 int enic_dev_hang_notify(struct enic *enic);
 int enic_dev_set_ig_vlan_rewrite_mode(struct enic *enic);
diff --git a/drivers/net/ethernet/cisco/enic/enic_main.c b/drivers/net/ethernet/cisco/enic/enic_main.c
index ec1a233622c6..635f55992d7e 100644
--- a/drivers/net/ethernet/cisco/enic/enic_main.c
+++ b/drivers/net/ethernet/cisco/enic/enic_main.c
@@ -1300,7 +1300,7 @@ static void enic_rq_indicate_buf(struct vnic_rq *rq,
 		}
 
 		if (vlan_stripped)
-			__vlan_hwaccel_put_tag(skb, vlan_tci);
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_tci);
 
 		if (netdev->features & NETIF_F_GRO)
 			napi_gro_receive(&enic->napi[q_number], skb);
@@ -2496,9 +2496,9 @@ static int enic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	netdev->watchdog_timeo = 2 * HZ;
 	netdev->ethtool_ops = &enic_ethtool_ops;
 
-	netdev->features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
+	netdev->features |= NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX;
 	if (ENIC_SETTING(enic, LOOP)) {
-		netdev->features &= ~NETIF_F_HW_VLAN_TX;
+		netdev->features &= ~NETIF_F_HW_VLAN_CTAG_TX;
 		enic->loop_enable = 1;
 		enic->loop_tag = enic->config.loop_tag;
 		dev_info(dev, "loopback tag=0x%04x\n", enic->loop_tag);
diff --git a/drivers/net/ethernet/cisco/enic/vnic_dev.c b/drivers/net/ethernet/cisco/enic/vnic_dev.c
index 605b22283be1..97455c573db5 100644
--- a/drivers/net/ethernet/cisco/enic/vnic_dev.c
+++ b/drivers/net/ethernet/cisco/enic/vnic_dev.c
@@ -308,6 +308,9 @@ static int _vnic_dev_cmd(struct vnic_dev *vdev, enum vnic_devcmd_cmd cmd,
 
 			if (status & STAT_ERROR) {
 				err = (int)readq(&devcmd->args[0]);
+				if (err == ERR_EINVAL &&
+				    cmd == CMD_CAPABILITY)
+					return err;
 				if (err != ERR_ECMDUNKNOWN ||
 				    cmd != CMD_CAPABILITY)
 					pr_err("Error %d devcmd %d\n",
diff --git a/drivers/net/ethernet/davicom/dm9000.c b/drivers/net/ethernet/davicom/dm9000.c
index 9eada8e86078..9105465b2a1a 100644
--- a/drivers/net/ethernet/davicom/dm9000.c
+++ b/drivers/net/ethernet/davicom/dm9000.c
@@ -1693,22 +1693,7 @@ static struct platform_driver dm9000_driver = {
 	.remove  = dm9000_drv_remove,
 };
 
-static int __init
-dm9000_init(void)
-{
-	printk(KERN_INFO "%s Ethernet Driver, V%s\n", CARDNAME, DRV_VERSION);
-
-	return platform_driver_register(&dm9000_driver);
-}
-
-static void __exit
-dm9000_cleanup(void)
-{
-	platform_driver_unregister(&dm9000_driver);
-}
-
-module_init(dm9000_init);
-module_exit(dm9000_cleanup);
+module_platform_driver(dm9000_driver);
 
 MODULE_AUTHOR("Sascha Hauer, Ben Dooks");
 MODULE_DESCRIPTION("Davicom DM9000 network driver");
diff --git a/drivers/net/ethernet/dec/tulip/xircom_cb.c b/drivers/net/ethernet/dec/tulip/xircom_cb.c
index 88feced9a629..cdbcd1643141 100644
--- a/drivers/net/ethernet/dec/tulip/xircom_cb.c
+++ b/drivers/net/ethernet/dec/tulip/xircom_cb.c
@@ -236,17 +236,14 @@ static int xircom_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	private->rx_buffer = dma_alloc_coherent(d, 8192,
 						&private->rx_dma_handle,
 						GFP_KERNEL);
-	if (private->rx_buffer == NULL) {
-		pr_err("%s: no memory for rx buffer\n", __func__);
+	if (private->rx_buffer == NULL)
 		goto rx_buf_fail;
-	}
+
 	private->tx_buffer = dma_alloc_coherent(d, 8192,
 						&private->tx_dma_handle,
 						GFP_KERNEL);
-	if (private->tx_buffer == NULL) {
-		pr_err("%s: no memory for tx buffer\n", __func__);
+	if (private->tx_buffer == NULL)
 		goto tx_buf_fail;
-	}
 
 	SET_NETDEV_DEV(dev, &pdev->dev);
 
diff --git a/drivers/net/ethernet/dlink/dl2k.c b/drivers/net/ethernet/dlink/dl2k.c
index 110d26f4c602..afa8e3af2c4d 100644
--- a/drivers/net/ethernet/dlink/dl2k.c
+++ b/drivers/net/ethernet/dlink/dl2k.c
@@ -580,12 +580,9 @@ alloc_list (struct net_device *dev)
 
 		skb = netdev_alloc_skb_ip_align(dev, np->rx_buf_sz);
 		np->rx_skbuff[i] = skb;
-		if (skb == NULL) {
-			printk (KERN_ERR
-				"%s: alloc_list: allocate Rx buffer error! ",
-				dev->name);
+		if (skb == NULL)
 			break;
-		}
+
 		/* Rubicon now supports 40 bits of addressing space. */
 		np->rx_ring[i].fraginfo =
 		    cpu_to_le64 ( pci_map_single (
diff --git a/drivers/net/ethernet/emulex/benet/be.h b/drivers/net/ethernet/emulex/benet/be.h
index 29aff55f2eea..f544b297c9ab 100644
--- a/drivers/net/ethernet/emulex/benet/be.h
+++ b/drivers/net/ethernet/emulex/benet/be.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2011 Emulex
+ * Copyright (C) 2005 - 2013 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -214,6 +214,7 @@ struct be_tx_stats {
 };
 
 struct be_tx_obj {
+	u32 db_offset;
 	struct be_queue_info q;
 	struct be_queue_info cq;
 	/* Remember the skbs that were transmitted */
@@ -292,7 +293,7 @@ struct be_drv_stats {
 	u32 rx_in_range_errors;
 	u32 rx_out_range_errors;
 	u32 rx_frame_too_long;
-	u32 rx_address_mismatch_drops;
+	u32 rx_address_filtered;
 	u32 rx_dropped_too_small;
 	u32 rx_dropped_too_short;
 	u32 rx_dropped_header_too_small;
@@ -326,8 +327,10 @@ enum vf_state {
 
 #define BE_FLAGS_LINK_STATUS_INIT		1
 #define BE_FLAGS_WORKER_SCHEDULED		(1 << 3)
+#define BE_FLAGS_NAPI_ENABLED			(1 << 9)
 #define BE_UC_PMAC_COUNT		30
 #define BE_VF_UC_PMAC_COUNT		2
+#define BE_FLAGS_QNQ_ASYNC_EVT_RCVD		(1 << 11)
 
 struct phy_info {
 	u8 transceiver;
@@ -434,6 +437,8 @@ struct be_adapter {
 	u8 wol_cap;
 	bool wol;
 	u32 uc_macs;		/* Count of secondary UC MAC programmed */
+	u16 asic_rev;
+	u16 qnq_vid;
 	u32 msg_enable;
 	int be_get_temp_freq;
 	u16 max_mcast_mac;
@@ -445,6 +450,7 @@ struct be_adapter {
 	u16 max_event_queues;
 	u32 if_cap_flags;
 	u8 pf_number;
+	u64 rss_flags;
 };
 
 #define be_physfn(adapter)		(!adapter->virtfn)
@@ -648,6 +654,11 @@ static inline bool be_is_wol_excluded(struct be_adapter *adapter)
 	}
 }
 
+static inline int qnq_async_evt_rcvd(struct be_adapter *adapter)
+{
+	return adapter->flags & BE_FLAGS_QNQ_ASYNC_EVT_RCVD;
+}
+
 extern void be_cq_notify(struct be_adapter *adapter, u16 qid, bool arm,
 		u16 num_popped);
 extern void be_link_status_update(struct be_adapter *adapter, u8 link_status);
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.c b/drivers/net/ethernet/emulex/benet/be_cmds.c
index 3c9b4f12e3e5..fd7b547698ab 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.c
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2011 Emulex
+ * Copyright (C) 2005 - 2013 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -263,6 +263,27 @@ static void be_async_grp5_evt_process(struct be_adapter *adapter,
 	}
 }
 
+static void be_async_dbg_evt_process(struct be_adapter *adapter,
+		u32 trailer, struct be_mcc_compl *cmp)
+{
+	u8 event_type = 0;
+	struct be_async_event_qnq *evt = (struct be_async_event_qnq *) cmp;
+
+	event_type = (trailer >> ASYNC_TRAILER_EVENT_TYPE_SHIFT) &
+		ASYNC_TRAILER_EVENT_TYPE_MASK;
+
+	switch (event_type) {
+	case ASYNC_DEBUG_EVENT_TYPE_QNQ:
+		if (evt->valid)
+			adapter->qnq_vid = le16_to_cpu(evt->vlan_tag);
+		adapter->flags |= BE_FLAGS_QNQ_ASYNC_EVT_RCVD;
+	break;
+	default:
+		dev_warn(&adapter->pdev->dev, "Unknown debug event\n");
+	break;
+	}
+}
+
 static inline bool is_link_state_evt(u32 trailer)
 {
 	return ((trailer >> ASYNC_TRAILER_EVENT_CODE_SHIFT) &
@@ -277,6 +298,13 @@ static inline bool is_grp5_evt(u32 trailer)
 				ASYNC_EVENT_CODE_GRP_5);
 }
 
+static inline bool is_dbg_evt(u32 trailer)
+{
+	return (((trailer >> ASYNC_TRAILER_EVENT_CODE_SHIFT) &
+		ASYNC_TRAILER_EVENT_CODE_MASK) ==
+				ASYNC_EVENT_CODE_QNQ);
+}
+
 static struct be_mcc_compl *be_mcc_compl_get(struct be_adapter *adapter)
 {
 	struct be_queue_info *mcc_cq = &adapter->mcc_obj.cq;
@@ -325,6 +353,9 @@ int be_process_mcc(struct be_adapter *adapter)
 			else if (is_grp5_evt(compl->flags))
 				be_async_grp5_evt_process(adapter,
 				compl->flags, compl);
+			else if (is_dbg_evt(compl->flags))
+				be_async_dbg_evt_process(adapter,
+				compl->flags, compl);
 		} else if (compl->flags & CQE_FLAGS_COMPLETED_MASK) {
 				status = be_mcc_compl_process(adapter, compl);
 				atomic_dec(&mcc_obj->q.used);
@@ -687,10 +718,8 @@ static struct be_mcc_wrb *wrb_from_mccq(struct be_adapter *adapter)
 	if (!mccq->created)
 		return NULL;
 
-	if (atomic_read(&mccq->used) >= mccq->len) {
-		dev_err(&adapter->pdev->dev, "Out of MCCQ wrbs\n");
+	if (atomic_read(&mccq->used) >= mccq->len)
 		return NULL;
-	}
 
 	wrb = queue_head_node(mccq);
 	queue_head_inc(mccq);
@@ -932,19 +961,8 @@ int be_cmd_cq_create(struct be_adapter *adapter, struct be_queue_info *cq,
 		OPCODE_COMMON_CQ_CREATE, sizeof(*req), wrb, NULL);
 
 	req->num_pages =  cpu_to_le16(PAGES_4K_SPANNED(q_mem->va, q_mem->size));
-	if (lancer_chip(adapter)) {
-		req->hdr.version = 2;
-		req->page_size = 1; /* 1 for 4K */
-		AMAP_SET_BITS(struct amap_cq_context_lancer, nodelay, ctxt,
-								no_delay);
-		AMAP_SET_BITS(struct amap_cq_context_lancer, count, ctxt,
-						__ilog2_u32(cq->len/256));
-		AMAP_SET_BITS(struct amap_cq_context_lancer, valid, ctxt, 1);
-		AMAP_SET_BITS(struct amap_cq_context_lancer, eventable,
-								ctxt, 1);
-		AMAP_SET_BITS(struct amap_cq_context_lancer, eqid,
-								ctxt, eq->id);
-	} else {
+
+	if (BEx_chip(adapter)) {
 		AMAP_SET_BITS(struct amap_cq_context_be, coalescwm, ctxt,
 								coalesce_wm);
 		AMAP_SET_BITS(struct amap_cq_context_be, nodelay,
@@ -954,6 +972,18 @@ int be_cmd_cq_create(struct be_adapter *adapter, struct be_queue_info *cq,
 		AMAP_SET_BITS(struct amap_cq_context_be, valid, ctxt, 1);
 		AMAP_SET_BITS(struct amap_cq_context_be, eventable, ctxt, 1);
 		AMAP_SET_BITS(struct amap_cq_context_be, eqid, ctxt, eq->id);
+	} else {
+		req->hdr.version = 2;
+		req->page_size = 1; /* 1 for 4K */
+		AMAP_SET_BITS(struct amap_cq_context_v2, nodelay, ctxt,
+								no_delay);
+		AMAP_SET_BITS(struct amap_cq_context_v2, count, ctxt,
+						__ilog2_u32(cq->len/256));
+		AMAP_SET_BITS(struct amap_cq_context_v2, valid, ctxt, 1);
+		AMAP_SET_BITS(struct amap_cq_context_v2, eventable,
+								ctxt, 1);
+		AMAP_SET_BITS(struct amap_cq_context_v2, eqid,
+								ctxt, eq->id);
 	}
 
 	be_dws_cpu_to_le(ctxt, sizeof(req->context));
@@ -1022,6 +1052,7 @@ int be_cmd_mccq_ext_create(struct be_adapter *adapter,
 
 	/* Subscribe to Link State and Group 5 Events(bits 1 and 5 set) */
 	req->async_event_bitmap[0] = cpu_to_le32(0x00000022);
+	req->async_event_bitmap[0] |= cpu_to_le32(1 << ASYNC_EVENT_CODE_QNQ);
 	be_dws_cpu_to_le(ctxt, sizeof(req->context));
 
 	be_cmd_page_addrs_prepare(req->pages, ARRAY_SIZE(req->pages), q_mem);
@@ -1095,15 +1126,14 @@ int be_cmd_mccq_create(struct be_adapter *adapter,
 	return status;
 }
 
-int be_cmd_txq_create(struct be_adapter *adapter,
-			struct be_queue_info *txq,
-			struct be_queue_info *cq)
+int be_cmd_txq_create(struct be_adapter *adapter, struct be_tx_obj *txo)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_eth_tx_create *req;
+	struct be_queue_info *txq = &txo->q;
+	struct be_queue_info *cq = &txo->cq;
 	struct be_dma_mem *q_mem = &txq->dma_mem;
-	void *ctxt;
-	int status;
+	int status, ver = 0;
 
 	spin_lock_bh(&adapter->mcc_lock);
 
@@ -1114,34 +1144,37 @@ int be_cmd_txq_create(struct be_adapter *adapter,
 	}
 
 	req = embedded_payload(wrb);
-	ctxt = &req->context;
 
 	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ETH,
 		OPCODE_ETH_TX_CREATE, sizeof(*req), wrb, NULL);
 
 	if (lancer_chip(adapter)) {
 		req->hdr.version = 1;
-		AMAP_SET_BITS(struct amap_tx_context, if_id, ctxt,
-					adapter->if_handle);
+		req->if_id = cpu_to_le16(adapter->if_handle);
+	} else if (BEx_chip(adapter)) {
+		if (adapter->function_caps & BE_FUNCTION_CAPS_SUPER_NIC)
+			req->hdr.version = 2;
+	} else { /* For SH */
+		req->hdr.version = 2;
 	}
 
 	req->num_pages = PAGES_4K_SPANNED(q_mem->va, q_mem->size);
 	req->ulp_num = BE_ULP1_NUM;
 	req->type = BE_ETH_TX_RING_TYPE_STANDARD;
-
-	AMAP_SET_BITS(struct amap_tx_context, tx_ring_size, ctxt,
-		be_encoded_q_len(txq->len));
-	AMAP_SET_BITS(struct amap_tx_context, ctx_valid, ctxt, 1);
-	AMAP_SET_BITS(struct amap_tx_context, cq_id_send, ctxt, cq->id);
-
-	be_dws_cpu_to_le(ctxt, sizeof(req->context));
-
+	req->cq_id = cpu_to_le16(cq->id);
+	req->queue_size = be_encoded_q_len(txq->len);
 	be_cmd_page_addrs_prepare(req->pages, ARRAY_SIZE(req->pages), q_mem);
 
+	ver = req->hdr.version;
+
 	status = be_mcc_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_eth_tx_create *resp = embedded_payload(wrb);
 		txq->id = le16_to_cpu(resp->cid);
+		if (ver == 2)
+			txo->db_offset = le32_to_cpu(resp->db_offset);
+		else
+			txo->db_offset = DB_TXULP1_OFFSET;
 		txq->created = true;
 	}
 
@@ -1731,10 +1764,12 @@ int be_cmd_rx_filter(struct be_adapter *adapter, u32 flags, u32 value)
 	req->if_id = cpu_to_le32(adapter->if_handle);
 	if (flags & IFF_PROMISC) {
 		req->if_flags_mask = cpu_to_le32(BE_IF_FLAGS_PROMISCUOUS |
-					BE_IF_FLAGS_VLAN_PROMISCUOUS);
+					BE_IF_FLAGS_VLAN_PROMISCUOUS |
+					BE_IF_FLAGS_MCAST_PROMISCUOUS);
 		if (value == ON)
 			req->if_flags = cpu_to_le32(BE_IF_FLAGS_PROMISCUOUS |
-						BE_IF_FLAGS_VLAN_PROMISCUOUS);
+						BE_IF_FLAGS_VLAN_PROMISCUOUS |
+						BE_IF_FLAGS_MCAST_PROMISCUOUS);
 	} else if (flags & IFF_ALLMULTI) {
 		req->if_flags_mask = req->if_flags =
 				cpu_to_le32(BE_IF_FLAGS_MCAST_PROMISCUOUS);
@@ -1834,7 +1869,7 @@ err:
 
 /* Uses mbox */
 int be_cmd_query_fw_cfg(struct be_adapter *adapter, u32 *port_num,
-		u32 *mode, u32 *caps)
+			u32 *mode, u32 *caps, u16 *asic_rev)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_query_fw_cfg *req;
@@ -1855,6 +1890,7 @@ int be_cmd_query_fw_cfg(struct be_adapter *adapter, u32 *port_num,
 		*port_num = le32_to_cpu(resp->phys_port);
 		*mode = le32_to_cpu(resp->function_mode);
 		*caps = le32_to_cpu(resp->function_caps);
+		*asic_rev = le32_to_cpu(resp->asic_revision) & 0xFF;
 	}
 
 	mutex_unlock(&adapter->mbox_lock);
@@ -1897,7 +1933,8 @@ int be_cmd_reset_function(struct be_adapter *adapter)
 	return status;
 }
 
-int be_cmd_rss_config(struct be_adapter *adapter, u8 *rsstable, u16 table_size)
+int be_cmd_rss_config(struct be_adapter *adapter, u8 *rsstable,
+			u32 rss_hash_opts, u16 table_size)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_rss_config *req;
@@ -1916,16 +1953,12 @@ int be_cmd_rss_config(struct be_adapter *adapter, u8 *rsstable, u16 table_size)
 		OPCODE_ETH_RSS_CONFIG, sizeof(*req), wrb, NULL);
 
 	req->if_id = cpu_to_le32(adapter->if_handle);
-	req->enable_rss = cpu_to_le16(RSS_ENABLE_TCP_IPV4 | RSS_ENABLE_IPV4 |
-				      RSS_ENABLE_TCP_IPV6 | RSS_ENABLE_IPV6);
+	req->enable_rss = cpu_to_le16(rss_hash_opts);
+	req->cpu_table_size_log2 = cpu_to_le16(fls(table_size) - 1);
 
-	if (lancer_chip(adapter) || skyhawk_chip(adapter)) {
+	if (lancer_chip(adapter) || skyhawk_chip(adapter))
 		req->hdr.version = 1;
-		req->enable_rss |= cpu_to_le16(RSS_ENABLE_UDP_IPV4 |
-					       RSS_ENABLE_UDP_IPV6);
-	}
 
-	req->cpu_table_size_log2 = cpu_to_le16(fls(table_size) - 1);
 	memcpy(req->cpu_table, rsstable, table_size);
 	memcpy(req->hash, myhash, sizeof(myhash));
 	be_dws_cpu_to_le(req->hash, sizeof(req->hash));
@@ -2054,7 +2087,7 @@ int lancer_cmd_write_object(struct be_adapter *adapter, struct be_dma_mem *cmd,
 	spin_unlock_bh(&adapter->mcc_lock);
 
 	if (!wait_for_completion_timeout(&adapter->flash_compl,
-					 msecs_to_jiffies(30000)))
+					 msecs_to_jiffies(60000)))
 		status = -1;
 	else
 		status = adapter->flash_status;
@@ -2343,7 +2376,6 @@ int be_cmd_get_seeprom_data(struct be_adapter *adapter,
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_seeprom_read *req;
-	struct be_sge *sge;
 	int status;
 
 	spin_lock_bh(&adapter->mcc_lock);
@@ -2354,7 +2386,6 @@ int be_cmd_get_seeprom_data(struct be_adapter *adapter,
 		goto err;
 	}
 	req = nonemb_cmd->va;
-	sge = nonembedded_sgl(wrb);
 
 	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 			OPCODE_COMMON_SEEPROM_READ, sizeof(*req), wrb,
@@ -2461,6 +2492,9 @@ int be_cmd_get_cntl_attributes(struct be_adapter *adapter)
 	struct mgmt_controller_attrib *attribs;
 	struct be_dma_mem attribs_cmd;
 
+	if (mutex_lock_interruptible(&adapter->mbox_lock))
+		return -1;
+
 	memset(&attribs_cmd, 0, sizeof(struct be_dma_mem));
 	attribs_cmd.size = sizeof(struct be_cmd_resp_cntl_attribs);
 	attribs_cmd.va = pci_alloc_consistent(adapter->pdev, attribs_cmd.size,
@@ -2468,12 +2502,10 @@ int be_cmd_get_cntl_attributes(struct be_adapter *adapter)
 	if (!attribs_cmd.va) {
 		dev_err(&adapter->pdev->dev,
 				"Memory allocation failure\n");
-		return -ENOMEM;
+		status = -ENOMEM;
+		goto err;
 	}
 
-	if (mutex_lock_interruptible(&adapter->mbox_lock))
-		return -1;
-
 	wrb = wrb_from_mbox(adapter);
 	if (!wrb) {
 		status = -EBUSY;
@@ -2493,8 +2525,9 @@ int be_cmd_get_cntl_attributes(struct be_adapter *adapter)
 
 err:
 	mutex_unlock(&adapter->mbox_lock);
-	pci_free_consistent(adapter->pdev, attribs_cmd.size, attribs_cmd.va,
-					attribs_cmd.dma);
+	if (attribs_cmd.va)
+		pci_free_consistent(adapter->pdev, attribs_cmd.size,
+				    attribs_cmd.va, attribs_cmd.dma);
 	return status;
 }
 
@@ -2607,9 +2640,8 @@ int be_cmd_get_mac_from_list(struct be_adapter *adapter, u8 *mac,
 	req = get_mac_list_cmd.va;
 
 	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
-				OPCODE_COMMON_GET_MAC_LIST, sizeof(*req),
-				wrb, &get_mac_list_cmd);
-
+			       OPCODE_COMMON_GET_MAC_LIST,
+			       get_mac_list_cmd.size, wrb, &get_mac_list_cmd);
 	req->hdr.domain = domain;
 	req->mac_type = MAC_ADDRESS_TYPE_NETWORK;
 	req->perm_override = 1;
@@ -2667,10 +2699,8 @@ int be_cmd_set_mac_list(struct be_adapter *adapter, u8 *mac_array,
 	cmd.size = sizeof(struct be_cmd_req_set_mac_list);
 	cmd.va = dma_alloc_coherent(&adapter->pdev->dev, cmd.size,
 			&cmd.dma, GFP_KERNEL);
-	if (!cmd.va) {
-		dev_err(&adapter->pdev->dev, "Memory alloc failure\n");
+	if (!cmd.va)
 		return -ENOMEM;
-	}
 
 	spin_lock_bh(&adapter->mcc_lock);
 
@@ -2794,6 +2824,9 @@ int be_cmd_get_acpi_wol_cap(struct be_adapter *adapter)
 			    CMD_SUBSYSTEM_ETH))
 		return -EPERM;
 
+	if (mutex_lock_interruptible(&adapter->mbox_lock))
+		return -1;
+
 	memset(&cmd, 0, sizeof(struct be_dma_mem));
 	cmd.size = sizeof(struct be_cmd_resp_acpi_wol_magic_config_v1);
 	cmd.va = pci_alloc_consistent(adapter->pdev, cmd.size,
@@ -2801,12 +2834,10 @@ int be_cmd_get_acpi_wol_cap(struct be_adapter *adapter)
 	if (!cmd.va) {
 		dev_err(&adapter->pdev->dev,
 				"Memory allocation failure\n");
-		return -ENOMEM;
+		status = -ENOMEM;
+		goto err;
 	}
 
-	if (mutex_lock_interruptible(&adapter->mbox_lock))
-		return -1;
-
 	wrb = wrb_from_mbox(adapter);
 	if (!wrb) {
 		status = -EBUSY;
@@ -2837,7 +2868,8 @@ int be_cmd_get_acpi_wol_cap(struct be_adapter *adapter)
 	}
 err:
 	mutex_unlock(&adapter->mbox_lock);
-	pci_free_consistent(adapter->pdev, cmd.size, cmd.va, cmd.dma);
+	if (cmd.va)
+		pci_free_consistent(adapter->pdev, cmd.size, cmd.va, cmd.dma);
 	return status;
 
 }
@@ -2942,14 +2974,15 @@ static struct be_nic_resource_desc *be_get_nic_desc(u8 *buf, u32 desc_count,
 	int i;
 
 	for (i = 0; i < desc_count; i++) {
-		desc->desc_len = RESOURCE_DESC_SIZE;
+		desc->desc_len = desc->desc_len ? : RESOURCE_DESC_SIZE;
 		if (((void *)desc + desc->desc_len) >
 		    (void *)(buf + max_buf_size)) {
 			desc = NULL;
 			break;
 		}
 
-		if (desc->desc_type == NIC_RESOURCE_DESC_TYPE_ID)
+		if (desc->desc_type == NIC_RESOURCE_DESC_TYPE_V0 ||
+		    desc->desc_type == NIC_RESOURCE_DESC_TYPE_V1)
 			break;
 
 		desc = (void *)desc + desc->desc_len;
@@ -2969,16 +3002,18 @@ int be_cmd_get_func_config(struct be_adapter *adapter)
 	int status;
 	struct be_dma_mem cmd;
 
+	if (mutex_lock_interruptible(&adapter->mbox_lock))
+		return -1;
+
 	memset(&cmd, 0, sizeof(struct be_dma_mem));
 	cmd.size = sizeof(struct be_cmd_resp_get_func_config);
 	cmd.va = pci_alloc_consistent(adapter->pdev, cmd.size,
 				      &cmd.dma);
 	if (!cmd.va) {
 		dev_err(&adapter->pdev->dev, "Memory alloc failure\n");
-		return -ENOMEM;
+		status = -ENOMEM;
+		goto err;
 	}
-	if (mutex_lock_interruptible(&adapter->mbox_lock))
-		return -1;
 
 	wrb = wrb_from_mbox(adapter);
 	if (!wrb) {
@@ -2992,6 +3027,9 @@ int be_cmd_get_func_config(struct be_adapter *adapter)
 			       OPCODE_COMMON_GET_FUNC_CONFIG,
 			       cmd.size, wrb, &cmd);
 
+	if (skyhawk_chip(adapter))
+		req->hdr.version = 1;
+
 	status = be_mbox_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_get_func_config *resp = cmd.va;
@@ -3018,28 +3056,46 @@ int be_cmd_get_func_config(struct be_adapter *adapter)
 	}
 err:
 	mutex_unlock(&adapter->mbox_lock);
-	pci_free_consistent(adapter->pdev, cmd.size,
-			    cmd.va, cmd.dma);
+	if (cmd.va)
+		pci_free_consistent(adapter->pdev, cmd.size, cmd.va, cmd.dma);
 	return status;
 }
 
- /* Uses sync mcc */
-int be_cmd_get_profile_config(struct be_adapter *adapter, u32 *cap_flags,
-			      u8 domain)
+/* Uses mbox */
+int be_cmd_get_profile_config_mbox(struct be_adapter *adapter,
+				   u8 domain, struct be_dma_mem *cmd)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_get_profile_config *req;
 	int status;
-	struct be_dma_mem cmd;
 
-	memset(&cmd, 0, sizeof(struct be_dma_mem));
-	cmd.size = sizeof(struct be_cmd_resp_get_profile_config);
-	cmd.va = pci_alloc_consistent(adapter->pdev, cmd.size,
-				      &cmd.dma);
-	if (!cmd.va) {
-		dev_err(&adapter->pdev->dev, "Memory alloc failure\n");
-		return -ENOMEM;
-	}
+	if (mutex_lock_interruptible(&adapter->mbox_lock))
+		return -1;
+	wrb = wrb_from_mbox(adapter);
+
+	req = cmd->va;
+	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+			       OPCODE_COMMON_GET_PROFILE_CONFIG,
+			       cmd->size, wrb, cmd);
+
+	req->type = ACTIVE_PROFILE_TYPE;
+	req->hdr.domain = domain;
+	if (!lancer_chip(adapter))
+		req->hdr.version = 1;
+
+	status = be_mbox_notify_wait(adapter);
+
+	mutex_unlock(&adapter->mbox_lock);
+	return status;
+}
+
+/* Uses sync mcc */
+int be_cmd_get_profile_config_mccq(struct be_adapter *adapter,
+				   u8 domain, struct be_dma_mem *cmd)
+{
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_get_profile_config *req;
+	int status;
 
 	spin_lock_bh(&adapter->mcc_lock);
 
@@ -3049,16 +3105,47 @@ int be_cmd_get_profile_config(struct be_adapter *adapter, u32 *cap_flags,
 		goto err;
 	}
 
-	req = cmd.va;
-
+	req = cmd->va;
 	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 			       OPCODE_COMMON_GET_PROFILE_CONFIG,
-			       cmd.size, wrb, &cmd);
+			       cmd->size, wrb, cmd);
 
 	req->type = ACTIVE_PROFILE_TYPE;
 	req->hdr.domain = domain;
+	if (!lancer_chip(adapter))
+		req->hdr.version = 1;
 
 	status = be_mcc_notify_wait(adapter);
+
+err:
+	spin_unlock_bh(&adapter->mcc_lock);
+	return status;
+}
+
+/* Uses sync mcc, if MCCQ is already created otherwise mbox */
+int be_cmd_get_profile_config(struct be_adapter *adapter, u32 *cap_flags,
+			      u16 *txq_count, u8 domain)
+{
+	struct be_queue_info *mccq = &adapter->mcc_obj.q;
+	struct be_dma_mem cmd;
+	int status;
+
+	memset(&cmd, 0, sizeof(struct be_dma_mem));
+	if (!lancer_chip(adapter))
+		cmd.size = sizeof(struct be_cmd_resp_get_profile_config_v1);
+	else
+		cmd.size = sizeof(struct be_cmd_resp_get_profile_config);
+	cmd.va = pci_alloc_consistent(adapter->pdev, cmd.size,
+				      &cmd.dma);
+	if (!cmd.va) {
+		dev_err(&adapter->pdev->dev, "Memory alloc failure\n");
+		return -ENOMEM;
+	}
+
+	if (!mccq->created)
+		status = be_cmd_get_profile_config_mbox(adapter, domain, &cmd);
+	else
+		status = be_cmd_get_profile_config_mccq(adapter, domain, &cmd);
 	if (!status) {
 		struct be_cmd_resp_get_profile_config *resp = cmd.va;
 		u32 desc_count = le32_to_cpu(resp->desc_count);
@@ -3071,12 +3158,15 @@ int be_cmd_get_profile_config(struct be_adapter *adapter, u32 *cap_flags,
 			status = -EINVAL;
 			goto err;
 		}
-		*cap_flags = le32_to_cpu(desc->cap_flags);
+		if (cap_flags)
+			*cap_flags = le32_to_cpu(desc->cap_flags);
+		if (txq_count)
+			*txq_count = le32_to_cpu(desc->txq_count);
 	}
 err:
-	spin_unlock_bh(&adapter->mcc_lock);
-	pci_free_consistent(adapter->pdev, cmd.size,
-			    cmd.va, cmd.dma);
+	if (cmd.va)
+		pci_free_consistent(adapter->pdev, cmd.size,
+				    cmd.va, cmd.dma);
 	return status;
 }
 
@@ -3105,7 +3195,7 @@ int be_cmd_set_profile_config(struct be_adapter *adapter, u32 bps,
 	req->hdr.domain = domain;
 	req->desc_count = cpu_to_le32(1);
 
-	req->nic_desc.desc_type = NIC_RESOURCE_DESC_TYPE_ID;
+	req->nic_desc.desc_type = NIC_RESOURCE_DESC_TYPE_V0;
 	req->nic_desc.desc_len = RESOURCE_DESC_SIZE;
 	req->nic_desc.flags = (1 << QUN) | (1 << IMM) | (1 << NOSV);
 	req->nic_desc.pf_num = adapter->pf_number;
@@ -3202,6 +3292,31 @@ err:
 	return status;
 }
 
+int be_cmd_intr_set(struct be_adapter *adapter, bool intr_enable)
+{
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_intr_set *req;
+	int status;
+
+	if (mutex_lock_interruptible(&adapter->mbox_lock))
+		return -1;
+
+	wrb = wrb_from_mbox(adapter);
+
+	req = embedded_payload(wrb);
+
+	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+			       OPCODE_COMMON_SET_INTERRUPT_ENABLE, sizeof(*req),
+			       wrb, NULL);
+
+	req->intr_enabled = intr_enable;
+
+	status = be_mbox_notify_wait(adapter);
+
+	mutex_unlock(&adapter->mbox_lock);
+	return status;
+}
+
 int be_roce_mcc_cmd(void *netdev_handle, void *wrb_payload,
 			int wrb_payload_size, u16 *cmd_status, u16 *ext_status)
 {
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.h b/drivers/net/ethernet/emulex/benet/be_cmds.h
index 96970860c915..025bdb0d1764 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.h
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2011 Emulex
+ * Copyright (C) 2005 - 2013 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -84,6 +84,9 @@ struct be_mcc_compl {
 #define ASYNC_EVENT_QOS_SPEED		0x1
 #define ASYNC_EVENT_COS_PRIORITY	0x2
 #define ASYNC_EVENT_PVID_STATE		0x3
+#define ASYNC_EVENT_CODE_QNQ		0x6
+#define ASYNC_DEBUG_EVENT_TYPE_QNQ	1
+
 struct be_async_event_trailer {
 	u32 code;
 };
@@ -144,6 +147,16 @@ struct be_async_event_grp5_pvid_state {
 	struct be_async_event_trailer trailer;
 } __packed;
 
+/* async event indicating outer VLAN tag in QnQ */
+struct be_async_event_qnq {
+	u8 valid;	/* Indicates if outer VLAN is valid */
+	u8 rsvd0;
+	u16 vlan_tag;
+	u32 event_tag;
+	u8 rsvd1[4];
+	struct be_async_event_trailer trailer;
+} __packed;
+
 struct be_mcc_mailbox {
 	struct be_mcc_wrb wrb;
 	struct be_mcc_compl compl;
@@ -188,6 +201,7 @@ struct be_mcc_mailbox {
 #define OPCODE_COMMON_GET_BEACON_STATE			70
 #define OPCODE_COMMON_READ_TRANSRECV_DATA		73
 #define OPCODE_COMMON_GET_PORT_NAME			77
+#define OPCODE_COMMON_SET_INTERRUPT_ENABLE		89
 #define OPCODE_COMMON_GET_PHY_DETAILS			102
 #define OPCODE_COMMON_SET_DRIVER_FUNCTION_CAP		103
 #define OPCODE_COMMON_GET_CNTL_ADDITIONAL_ATTRIBUTES	121
@@ -367,7 +381,7 @@ struct amap_cq_context_be {
 	u8 rsvd5[32];		/* dword 3*/
 } __packed;
 
-struct amap_cq_context_lancer {
+struct amap_cq_context_v2 {
 	u8 rsvd0[12];		/* dword 0*/
 	u8 coalescwm[2];	/* dword 0*/
 	u8 nodelay;		/* dword 0*/
@@ -473,46 +487,27 @@ struct be_cmd_resp_mcc_create {
 #define BE_ETH_TX_RING_TYPE_STANDARD    	2
 #define BE_ULP1_NUM				1
 
-/* Pseudo amap definition in which each bit of the actual structure is defined
- * as a byte: used to calculate offset/shift/mask of each field */
-struct amap_tx_context {
-	u8 if_id[16];		/* dword 0 */
-	u8 tx_ring_size[4];	/* dword 0 */
-	u8 rsvd1[26];		/* dword 0 */
-	u8 pci_func_id[8];	/* dword 1 */
-	u8 rsvd2[9];		/* dword 1 */
-	u8 ctx_valid;		/* dword 1 */
-	u8 cq_id_send[16];	/* dword 2 */
-	u8 rsvd3[16];		/* dword 2 */
-	u8 rsvd4[32];		/* dword 3 */
-	u8 rsvd5[32];		/* dword 4 */
-	u8 rsvd6[32];		/* dword 5 */
-	u8 rsvd7[32];		/* dword 6 */
-	u8 rsvd8[32];		/* dword 7 */
-	u8 rsvd9[32];		/* dword 8 */
-	u8 rsvd10[32];		/* dword 9 */
-	u8 rsvd11[32];		/* dword 10 */
-	u8 rsvd12[32];		/* dword 11 */
-	u8 rsvd13[32];		/* dword 12 */
-	u8 rsvd14[32];		/* dword 13 */
-	u8 rsvd15[32];		/* dword 14 */
-	u8 rsvd16[32];		/* dword 15 */
-} __packed;
-
 struct be_cmd_req_eth_tx_create {
 	struct be_cmd_req_hdr hdr;
 	u8 num_pages;
 	u8 ulp_num;
-	u8 type;
-	u8 bound_port;
-	u8 context[sizeof(struct amap_tx_context) / 8];
+	u16 type;
+	u16 if_id;
+	u8 queue_size;
+	u8 rsvd0;
+	u32 rsvd1;
+	u16 cq_id;
+	u16 rsvd2;
+	u32 rsvd3[13];
 	struct phys_addr pages[8];
 } __packed;
 
 struct be_cmd_resp_eth_tx_create {
 	struct be_cmd_resp_hdr hdr;
 	u16 cid;
-	u16 rsvd0;
+	u16 rid;
+	u32 db_offset;
+	u32 rsvd0[4];
 } __packed;
 
 /******************** Create RxQ ***************************/
@@ -608,8 +603,8 @@ struct be_port_rxf_stats_v0 {
 	u32 rx_in_range_errors;	/* dword 10*/
 	u32 rx_out_range_errors;	/* dword 11*/
 	u32 rx_frame_too_long;	/* dword 12*/
-	u32 rx_address_mismatch_drops;	/* dword 13*/
-	u32 rx_vlan_mismatch_drops;	/* dword 14*/
+	u32 rx_address_filtered;	/* dword 13*/
+	u32 rx_vlan_filtered;	/* dword 14*/
 	u32 rx_dropped_too_small;	/* dword 15*/
 	u32 rx_dropped_too_short;	/* dword 16*/
 	u32 rx_dropped_header_too_small;	/* dword 17*/
@@ -815,8 +810,8 @@ struct lancer_pport_stats {
 	u32 rx_control_frames_unknown_opcode_hi;
 	u32 rx_in_range_errors;
 	u32 rx_out_of_range_errors;
-	u32 rx_address_mismatch_drops;
-	u32 rx_vlan_mismatch_drops;
+	u32 rx_address_filtered;
+	u32 rx_vlan_filtered;
 	u32 rx_dropped_too_small;
 	u32 rx_dropped_too_short;
 	u32 rx_dropped_header_too_small;
@@ -1066,7 +1061,6 @@ struct be_cmd_resp_modify_eq_delay {
 } __packed;
 
 /******************** Get FW Config *******************/
-#define BE_FUNCTION_CAPS_RSS			0x2
 /* The HW can come up in either of the following multi-channel modes
  * based on the skew/IPL.
  */
@@ -1109,6 +1103,9 @@ struct be_cmd_resp_query_fw_cfg {
 #define RSS_ENABLE_UDP_IPV4			0x10
 #define RSS_ENABLE_UDP_IPV6			0x20
 
+#define L3_RSS_FLAGS				(RXH_IP_DST | RXH_IP_SRC)
+#define L4_RSS_FLAGS				(RXH_L4_B_0_1 | RXH_L4_B_2_3)
+
 struct be_cmd_req_rss_config {
 	struct be_cmd_req_hdr hdr;
 	u32 if_id;
@@ -1592,7 +1589,7 @@ struct be_port_rxf_stats_v1 {
 	u32 rx_in_range_errors;
 	u32 rx_out_range_errors;
 	u32 rx_frame_too_long;
-	u32 rx_address_mismatch_drops;
+	u32 rx_address_filtered;
 	u32 rx_dropped_too_small;
 	u32 rx_dropped_too_short;
 	u32 rx_dropped_header_too_small;
@@ -1706,9 +1703,11 @@ struct be_cmd_req_set_ext_fat_caps {
 	struct be_fat_conf_params set_params;
 };
 
-#define RESOURCE_DESC_SIZE			72
-#define NIC_RESOURCE_DESC_TYPE_ID		0x41
+#define RESOURCE_DESC_SIZE			88
+#define NIC_RESOURCE_DESC_TYPE_V0		0x41
+#define NIC_RESOURCE_DESC_TYPE_V1		0x51
 #define MAX_RESOURCE_DESC			4
+#define MAX_RESOURCE_DESC_V1			32
 
 /* QOS unit number */
 #define QUN					4
@@ -1755,7 +1754,7 @@ struct be_cmd_req_get_func_config {
 };
 
 struct be_cmd_resp_get_func_config {
-	struct be_cmd_req_hdr hdr;
+	struct be_cmd_resp_hdr hdr;
 	u32 desc_count;
 	u8 func_param[MAX_RESOURCE_DESC * RESOURCE_DESC_SIZE];
 };
@@ -1774,6 +1773,12 @@ struct be_cmd_resp_get_profile_config {
 	u8 func_param[MAX_RESOURCE_DESC * RESOURCE_DESC_SIZE];
 };
 
+struct be_cmd_resp_get_profile_config_v1 {
+	struct be_cmd_req_hdr hdr;
+	u32 desc_count;
+	u8 func_param[MAX_RESOURCE_DESC_V1 * RESOURCE_DESC_SIZE];
+};
+
 struct be_cmd_req_set_profile_config {
 	struct be_cmd_req_hdr hdr;
 	u32 rsvd;
@@ -1791,6 +1796,12 @@ struct be_cmd_enable_disable_vf {
 	u8 rsvd[3];
 };
 
+struct be_cmd_req_intr_set {
+	struct be_cmd_req_hdr hdr;
+	u8 intr_enabled;
+	u8 rsvd[3];
+};
+
 static inline bool check_privilege(struct be_adapter *adapter, u32 flags)
 {
 	return flags & adapter->cmd_privileges ? true : false;
@@ -1834,8 +1845,7 @@ extern int be_cmd_mccq_create(struct be_adapter *adapter,
 			struct be_queue_info *mccq,
 			struct be_queue_info *cq);
 extern int be_cmd_txq_create(struct be_adapter *adapter,
-			struct be_queue_info *txq,
-			struct be_queue_info *cq);
+			struct be_tx_obj *txo);
 extern int be_cmd_rxq_create(struct be_adapter *adapter,
 			struct be_queue_info *rxq, u16 cq_id,
 			u16 frag_size, u32 if_id, u32 rss, u8 *rss_id);
@@ -1862,11 +1872,11 @@ extern int be_cmd_set_flow_control(struct be_adapter *adapter,
 			u32 tx_fc, u32 rx_fc);
 extern int be_cmd_get_flow_control(struct be_adapter *adapter,
 			u32 *tx_fc, u32 *rx_fc);
-extern int be_cmd_query_fw_cfg(struct be_adapter *adapter,
-			u32 *port_num, u32 *function_mode, u32 *function_caps);
+extern int be_cmd_query_fw_cfg(struct be_adapter *adapter, u32 *port_num,
+			u32 *function_mode, u32 *function_caps, u16 *asic_rev);
 extern int be_cmd_reset_function(struct be_adapter *adapter);
 extern int be_cmd_rss_config(struct be_adapter *adapter, u8 *rsstable,
-			u16 table_size);
+			     u32 rss_hash_opts, u16 table_size);
 extern int be_process_mcc(struct be_adapter *adapter);
 extern int be_cmd_set_beacon_state(struct be_adapter *adapter,
 			u8 port_num, u8 beacon, u8 status, u8 state);
@@ -1931,10 +1941,11 @@ extern int lancer_test_and_set_rdy_state(struct be_adapter *adapter);
 extern int be_cmd_query_port_name(struct be_adapter *adapter, u8 *port_name);
 extern int be_cmd_get_func_config(struct be_adapter *adapter);
 extern int be_cmd_get_profile_config(struct be_adapter *adapter, u32 *cap_flags,
-				     u8 domain);
+				     u16 *txq_count, u8 domain);
 
 extern int be_cmd_set_profile_config(struct be_adapter *adapter, u32 bps,
 				     u8 domain);
 extern int be_cmd_get_if_id(struct be_adapter *adapter,
 			    struct be_vf_cfg *vf_cfg, int vf_num);
 extern int be_cmd_enable_vf(struct be_adapter *adapter, u8 domain);
+extern int be_cmd_intr_set(struct be_adapter *adapter, bool intr_enable);
diff --git a/drivers/net/ethernet/emulex/benet/be_ethtool.c b/drivers/net/ethernet/emulex/benet/be_ethtool.c
index 76b302f30c87..3d4461adb3b4 100644
--- a/drivers/net/ethernet/emulex/benet/be_ethtool.c
+++ b/drivers/net/ethernet/emulex/benet/be_ethtool.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2011 Emulex
+ * Copyright (C) 2005 - 2013 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -54,7 +54,7 @@ static const struct be_ethtool_stat et_stats[] = {
 	/* Received packets dropped when they don't pass the unicast or
 	 * multicast address filtering.
 	 */
-	{DRVSTAT_INFO(rx_address_mismatch_drops)},
+	{DRVSTAT_INFO(rx_address_filtered)},
 	/* Received packets dropped when IP packet length field is less than
 	 * the IP header length field.
 	 */
@@ -85,6 +85,7 @@ static const struct be_ethtool_stat et_stats[] = {
 	{DRVSTAT_INFO(tx_pauseframes)},
 	{DRVSTAT_INFO(tx_controlframes)},
 	{DRVSTAT_INFO(rx_priority_pause_frames)},
+	{DRVSTAT_INFO(tx_priority_pauseframes)},
 	/* Received packets dropped when an internal fifo going into
 	 * main packet buffer tank (PMEM) overflows.
 	 */
@@ -680,7 +681,8 @@ be_get_wol(struct net_device *netdev, struct ethtool_wolinfo *wol)
 
 	if (be_is_wol_supported(adapter)) {
 		wol->supported |= WAKE_MAGIC;
-		wol->wolopts |= WAKE_MAGIC;
+		if (adapter->wol)
+			wol->wolopts |= WAKE_MAGIC;
 	} else
 		wol->wolopts = 0;
 	memset(&wol->sopass, 0, sizeof(wol->sopass));
@@ -719,10 +721,8 @@ be_test_ddr_dma(struct be_adapter *adapter)
 	ddrdma_cmd.size = sizeof(struct be_cmd_req_ddrdma_test);
 	ddrdma_cmd.va = dma_alloc_coherent(&adapter->pdev->dev, ddrdma_cmd.size,
 					   &ddrdma_cmd.dma, GFP_KERNEL);
-	if (!ddrdma_cmd.va) {
-		dev_err(&adapter->pdev->dev, "Memory allocation failure\n");
+	if (!ddrdma_cmd.va)
 		return -ENOMEM;
-	}
 
 	for (i = 0; i < 2; i++) {
 		ret = be_cmd_ddr_dma_test(adapter, pattern[i],
@@ -757,6 +757,12 @@ be_self_test(struct net_device *netdev, struct ethtool_test *test, u64 *data)
 	int status;
 	u8 link_status = 0;
 
+	if (adapter->function_caps & BE_FUNCTION_CAPS_SUPER_NIC) {
+		dev_err(&adapter->pdev->dev, "Self test not supported\n");
+		test->flags |= ETH_TEST_FL_FAILED;
+		return;
+	}
+
 	memset(data, 0, sizeof(u64) * ETHTOOL_TESTS_NUM);
 
 	if (test->flags & ETH_TEST_FL_OFFLINE) {
@@ -845,11 +851,8 @@ be_read_eeprom(struct net_device *netdev, struct ethtool_eeprom *eeprom,
 	eeprom_cmd.va = dma_alloc_coherent(&adapter->pdev->dev, eeprom_cmd.size,
 					   &eeprom_cmd.dma, GFP_KERNEL);
 
-	if (!eeprom_cmd.va) {
-		dev_err(&adapter->pdev->dev,
-			"Memory allocation failure. Could not read eeprom\n");
+	if (!eeprom_cmd.va)
 		return -ENOMEM;
-	}
 
 	status = be_cmd_get_seeprom_data(adapter, &eeprom_cmd);
 
@@ -939,6 +942,159 @@ static void be_set_msg_level(struct net_device *netdev, u32 level)
 	return;
 }
 
+static u64 be_get_rss_hash_opts(struct be_adapter *adapter, u64 flow_type)
+{
+	u64 data = 0;
+
+	switch (flow_type) {
+	case TCP_V4_FLOW:
+		if (adapter->rss_flags & RSS_ENABLE_IPV4)
+			data |= RXH_IP_DST | RXH_IP_SRC;
+		if (adapter->rss_flags & RSS_ENABLE_TCP_IPV4)
+			data |= RXH_L4_B_0_1 | RXH_L4_B_2_3;
+		break;
+	case UDP_V4_FLOW:
+		if (adapter->rss_flags & RSS_ENABLE_IPV4)
+			data |= RXH_IP_DST | RXH_IP_SRC;
+		if (adapter->rss_flags & RSS_ENABLE_UDP_IPV4)
+			data |= RXH_L4_B_0_1 | RXH_L4_B_2_3;
+		break;
+	case TCP_V6_FLOW:
+		if (adapter->rss_flags & RSS_ENABLE_IPV6)
+			data |= RXH_IP_DST | RXH_IP_SRC;
+		if (adapter->rss_flags & RSS_ENABLE_TCP_IPV6)
+			data |= RXH_L4_B_0_1 | RXH_L4_B_2_3;
+		break;
+	case UDP_V6_FLOW:
+		if (adapter->rss_flags & RSS_ENABLE_IPV6)
+			data |= RXH_IP_DST | RXH_IP_SRC;
+		if (adapter->rss_flags & RSS_ENABLE_UDP_IPV6)
+			data |= RXH_L4_B_0_1 | RXH_L4_B_2_3;
+		break;
+	}
+
+	return data;
+}
+
+static int be_get_rxnfc(struct net_device *netdev, struct ethtool_rxnfc *cmd,
+		      u32 *rule_locs)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+
+	if (!be_multi_rxq(adapter)) {
+		dev_info(&adapter->pdev->dev,
+			 "ethtool::get_rxnfc: RX flow hashing is disabled\n");
+		return -EINVAL;
+	}
+
+	switch (cmd->cmd) {
+	case ETHTOOL_GRXFH:
+		cmd->data = be_get_rss_hash_opts(adapter, cmd->flow_type);
+		break;
+	case ETHTOOL_GRXRINGS:
+		cmd->data = adapter->num_rx_qs - 1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int be_set_rss_hash_opts(struct be_adapter *adapter,
+				struct ethtool_rxnfc *cmd)
+{
+	struct be_rx_obj *rxo;
+	int status = 0, i, j;
+	u8 rsstable[128];
+	u32 rss_flags = adapter->rss_flags;
+
+	if (cmd->data != L3_RSS_FLAGS &&
+	    cmd->data != (L3_RSS_FLAGS | L4_RSS_FLAGS))
+		return -EINVAL;
+
+	switch (cmd->flow_type) {
+	case TCP_V4_FLOW:
+		if (cmd->data == L3_RSS_FLAGS)
+			rss_flags &= ~RSS_ENABLE_TCP_IPV4;
+		else if (cmd->data == (L3_RSS_FLAGS | L4_RSS_FLAGS))
+			rss_flags |= RSS_ENABLE_IPV4 |
+					RSS_ENABLE_TCP_IPV4;
+		break;
+	case TCP_V6_FLOW:
+		if (cmd->data == L3_RSS_FLAGS)
+			rss_flags &= ~RSS_ENABLE_TCP_IPV6;
+		else if (cmd->data == (L3_RSS_FLAGS | L4_RSS_FLAGS))
+			rss_flags |= RSS_ENABLE_IPV6 |
+					RSS_ENABLE_TCP_IPV6;
+		break;
+	case UDP_V4_FLOW:
+		if ((cmd->data == (L3_RSS_FLAGS | L4_RSS_FLAGS)) &&
+		    BEx_chip(adapter))
+			return -EINVAL;
+
+		if (cmd->data == L3_RSS_FLAGS)
+			rss_flags &= ~RSS_ENABLE_UDP_IPV4;
+		else if (cmd->data == (L3_RSS_FLAGS | L4_RSS_FLAGS))
+			rss_flags |= RSS_ENABLE_IPV4 |
+					RSS_ENABLE_UDP_IPV4;
+		break;
+	case UDP_V6_FLOW:
+		if ((cmd->data == (L3_RSS_FLAGS | L4_RSS_FLAGS)) &&
+		    BEx_chip(adapter))
+			return -EINVAL;
+
+		if (cmd->data == L3_RSS_FLAGS)
+			rss_flags &= ~RSS_ENABLE_UDP_IPV6;
+		else if (cmd->data == (L3_RSS_FLAGS | L4_RSS_FLAGS))
+			rss_flags |= RSS_ENABLE_IPV6 |
+					RSS_ENABLE_UDP_IPV6;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (rss_flags == adapter->rss_flags)
+		return status;
+
+	if (be_multi_rxq(adapter)) {
+		for (j = 0; j < 128; j += adapter->num_rx_qs - 1) {
+			for_all_rss_queues(adapter, rxo, i) {
+				if ((j + i) >= 128)
+					break;
+				rsstable[j + i] = rxo->rss_id;
+			}
+		}
+	}
+	status = be_cmd_rss_config(adapter, rsstable, rss_flags, 128);
+	if (!status)
+		adapter->rss_flags = rss_flags;
+
+	return status;
+}
+
+static int be_set_rxnfc(struct net_device *netdev, struct ethtool_rxnfc *cmd)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+	int status = 0;
+
+	if (!be_multi_rxq(adapter)) {
+		dev_err(&adapter->pdev->dev,
+			"ethtool::set_rxnfc: RX flow hashing is disabled\n");
+		return -EINVAL;
+	}
+
+	switch (cmd->cmd) {
+	case ETHTOOL_SRXFH:
+		status = be_set_rss_hash_opts(adapter, cmd);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return status;
+}
+
 const struct ethtool_ops be_ethtool_ops = {
 	.get_settings = be_get_settings,
 	.get_drvinfo = be_get_drvinfo,
@@ -962,4 +1118,6 @@ const struct ethtool_ops be_ethtool_ops = {
 	.get_regs = be_get_regs,
 	.flash_device = be_do_flash,
 	.self_test = be_self_test,
+	.get_rxnfc = be_get_rxnfc,
+	.set_rxnfc = be_set_rxnfc,
 };
diff --git a/drivers/net/ethernet/emulex/benet/be_hw.h b/drivers/net/ethernet/emulex/benet/be_hw.h
index 62dc220695f7..3c1099b47f2a 100644
--- a/drivers/net/ethernet/emulex/benet/be_hw.h
+++ b/drivers/net/ethernet/emulex/benet/be_hw.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2011 Emulex
+ * Copyright (C) 2005 - 2013 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -72,6 +72,10 @@
  */
 #define MEMBAR_CTRL_INT_CTRL_HOSTINTR_MASK	(1 << 29) /* bit 29 */
 
+/********* PCI Function Capability *********/
+#define BE_FUNCTION_CAPS_RSS			0x2
+#define BE_FUNCTION_CAPS_SUPER_NIC		0x40
+
 /********* Power management (WOL) **********/
 #define PCICFG_PM_CONTROL_OFFSET		0x44
 #define PCICFG_PM_CONTROL_MASK			0x108	/* bits 3 & 8 */
@@ -495,7 +499,8 @@ struct flash_file_hdr_g3 {
 	u32 antidote;
 	u32 num_imgs;
 	u8 build[24];
-	u8 rsvd[32];
+	u8 asic_type_rev;
+	u8 rsvd[31];
 };
 
 struct flash_section_hdr {
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index 2886c9b63f90..a444110b060f 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2011 Emulex
+ * Copyright (C) 2005 - 2013 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -146,20 +146,16 @@ static int be_queue_alloc(struct be_adapter *adapter, struct be_queue_info *q,
 	q->entry_size = entry_size;
 	mem->size = len * entry_size;
 	mem->va = dma_alloc_coherent(&adapter->pdev->dev, mem->size, &mem->dma,
-				     GFP_KERNEL);
+				     GFP_KERNEL | __GFP_ZERO);
 	if (!mem->va)
 		return -ENOMEM;
-	memset(mem->va, 0, mem->size);
 	return 0;
 }
 
-static void be_intr_set(struct be_adapter *adapter, bool enable)
+static void be_reg_intr_set(struct be_adapter *adapter, bool enable)
 {
 	u32 reg, enabled;
 
-	if (adapter->eeh_error)
-		return;
-
 	pci_read_config_dword(adapter->pdev, PCICFG_MEMBAR_CTRL_INT_CTRL_OFFSET,
 				&reg);
 	enabled = reg & MEMBAR_CTRL_INT_CTRL_HOSTINTR_MASK;
@@ -175,6 +171,22 @@ static void be_intr_set(struct be_adapter *adapter, bool enable)
 			PCICFG_MEMBAR_CTRL_INT_CTRL_OFFSET, reg);
 }
 
+static void be_intr_set(struct be_adapter *adapter, bool enable)
+{
+	int status = 0;
+
+	/* On lancer interrupts can't be controlled via this register */
+	if (lancer_chip(adapter))
+		return;
+
+	if (adapter->eeh_error)
+		return;
+
+	status = be_cmd_intr_set(adapter, enable);
+	if (status)
+		be_reg_intr_set(adapter, enable);
+}
+
 static void be_rxq_notify(struct be_adapter *adapter, u16 qid, u16 posted)
 {
 	u32 val = 0;
@@ -185,14 +197,15 @@ static void be_rxq_notify(struct be_adapter *adapter, u16 qid, u16 posted)
 	iowrite32(val, adapter->db + DB_RQ_OFFSET);
 }
 
-static void be_txq_notify(struct be_adapter *adapter, u16 qid, u16 posted)
+static void be_txq_notify(struct be_adapter *adapter, struct be_tx_obj *txo,
+			  u16 posted)
 {
 	u32 val = 0;
-	val |= qid & DB_TXULP_RING_ID_MASK;
+	val |= txo->q.id & DB_TXULP_RING_ID_MASK;
 	val |= (posted & DB_TXULP_NUM_POSTED_MASK) << DB_TXULP_NUM_POSTED_SHIFT;
 
 	wmb();
-	iowrite32(val, adapter->db + DB_TXULP1_OFFSET);
+	iowrite32(val, adapter->db + txo->db_offset);
 }
 
 static void be_eq_notify(struct be_adapter *adapter, u16 qid,
@@ -340,9 +353,9 @@ static void populate_be_v0_stats(struct be_adapter *adapter)
 	drvs->rx_input_fifo_overflow_drop = port_stats->rx_input_fifo_overflow;
 	drvs->rx_dropped_header_too_small =
 		port_stats->rx_dropped_header_too_small;
-	drvs->rx_address_mismatch_drops =
-					port_stats->rx_address_mismatch_drops +
-					port_stats->rx_vlan_mismatch_drops;
+	drvs->rx_address_filtered =
+					port_stats->rx_address_filtered +
+					port_stats->rx_vlan_filtered;
 	drvs->rx_alignment_symbol_errors =
 		port_stats->rx_alignment_symbol_errors;
 
@@ -391,12 +404,13 @@ static void populate_be_v1_stats(struct be_adapter *adapter)
 		port_stats->rx_dropped_header_too_small;
 	drvs->rx_input_fifo_overflow_drop =
 		port_stats->rx_input_fifo_overflow_drop;
-	drvs->rx_address_mismatch_drops = port_stats->rx_address_mismatch_drops;
+	drvs->rx_address_filtered = port_stats->rx_address_filtered;
 	drvs->rx_alignment_symbol_errors =
 		port_stats->rx_alignment_symbol_errors;
 	drvs->rxpp_fifo_overflow_drop = port_stats->rxpp_fifo_overflow_drop;
 	drvs->tx_pauseframes = port_stats->tx_pauseframes;
 	drvs->tx_controlframes = port_stats->tx_controlframes;
+	drvs->tx_priority_pauseframes = port_stats->tx_priority_pauseframes;
 	drvs->jabber_events = port_stats->jabber_events;
 	drvs->rx_drops_no_pbuf = rxf_stats->rx_drops_no_pbuf;
 	drvs->rx_drops_no_erx_descr = rxf_stats->rx_drops_no_erx_descr;
@@ -432,9 +446,9 @@ static void populate_lancer_stats(struct be_adapter *adapter)
 	drvs->rx_dropped_header_too_small =
 				pport_stats->rx_dropped_header_too_small;
 	drvs->rx_input_fifo_overflow_drop = pport_stats->rx_fifo_overflow;
-	drvs->rx_address_mismatch_drops =
-					pport_stats->rx_address_mismatch_drops +
-					pport_stats->rx_vlan_mismatch_drops;
+	drvs->rx_address_filtered =
+					pport_stats->rx_address_filtered +
+					pport_stats->rx_vlan_filtered;
 	drvs->rx_alignment_symbol_errors = pport_stats->rx_symbol_errors_lo;
 	drvs->rxpp_fifo_overflow_drop = pport_stats->rx_fifo_overflow;
 	drvs->tx_pauseframes = pport_stats->tx_pause_frames_lo;
@@ -458,11 +472,26 @@ static void accumulate_16bit_val(u32 *acc, u16 val)
 	ACCESS_ONCE(*acc) = newacc;
 }
 
+void populate_erx_stats(struct be_adapter *adapter,
+			struct be_rx_obj *rxo,
+			u32 erx_stat)
+{
+	if (!BEx_chip(adapter))
+		rx_stats(rxo)->rx_drops_no_frags = erx_stat;
+	else
+		/* below erx HW counter can actually wrap around after
+		 * 65535. Driver accumulates a 32-bit value
+		 */
+		accumulate_16bit_val(&rx_stats(rxo)->rx_drops_no_frags,
+				     (u16)erx_stat);
+}
+
 void be_parse_stats(struct be_adapter *adapter)
 {
 	struct be_erx_stats_v1 *erx = be_erx_stats_from_cmd(adapter);
 	struct be_rx_obj *rxo;
 	int i;
+	u32 erx_stat;
 
 	if (lancer_chip(adapter)) {
 		populate_lancer_stats(adapter);
@@ -475,12 +504,8 @@ void be_parse_stats(struct be_adapter *adapter)
 
 		/* as erx_v1 is longer than v0, ok to use v1 for v0 access */
 		for_all_rx_queues(adapter, rxo, i) {
-			/* below erx HW counter can actually wrap around after
-			 * 65535. Driver accumulates a 32-bit value
-			 */
-			accumulate_16bit_val(&rx_stats(rxo)->rx_drops_no_frags,
-					     (u16)erx->rx_drops_no_fragments \
-					     [rxo->q.id]);
+			erx_stat = erx->rx_drops_no_fragments[rxo->q.id];
+			populate_erx_stats(adapter, rxo, erx_stat);
 		}
 	}
 }
@@ -626,13 +651,8 @@ static inline u16 be_get_tx_vlan_tag(struct be_adapter *adapter,
 	return vlan_tag;
 }
 
-static int be_vlan_tag_chk(struct be_adapter *adapter, struct sk_buff *skb)
-{
-	return vlan_tx_tag_present(skb) || adapter->pvid;
-}
-
 static void wrb_fill_hdr(struct be_adapter *adapter, struct be_eth_hdr_wrb *hdr,
-		struct sk_buff *skb, u32 wrb_cnt, u32 len)
+		struct sk_buff *skb, u32 wrb_cnt, u32 len, bool skip_hw_vlan)
 {
 	u16 vlan_tag;
 
@@ -659,8 +679,9 @@ static void wrb_fill_hdr(struct be_adapter *adapter, struct be_eth_hdr_wrb *hdr,
 		AMAP_SET_BITS(struct amap_eth_hdr_wrb, vlan_tag, hdr, vlan_tag);
 	}
 
+	/* To skip HW VLAN tagging: evt = 1, compl = 0 */
+	AMAP_SET_BITS(struct amap_eth_hdr_wrb, complete, hdr, !skip_hw_vlan);
 	AMAP_SET_BITS(struct amap_eth_hdr_wrb, event, hdr, 1);
-	AMAP_SET_BITS(struct amap_eth_hdr_wrb, complete, hdr, 1);
 	AMAP_SET_BITS(struct amap_eth_hdr_wrb, num_wrb, hdr, wrb_cnt);
 	AMAP_SET_BITS(struct amap_eth_hdr_wrb, len, hdr, len);
 }
@@ -683,7 +704,8 @@ static void unmap_tx_frag(struct device *dev, struct be_eth_wrb *wrb,
 }
 
 static int make_tx_wrbs(struct be_adapter *adapter, struct be_queue_info *txq,
-		struct sk_buff *skb, u32 wrb_cnt, bool dummy_wrb)
+		struct sk_buff *skb, u32 wrb_cnt, bool dummy_wrb,
+		bool skip_hw_vlan)
 {
 	dma_addr_t busaddr;
 	int i, copied = 0;
@@ -732,7 +754,7 @@ static int make_tx_wrbs(struct be_adapter *adapter, struct be_queue_info *txq,
 		queue_head_inc(txq);
 	}
 
-	wrb_fill_hdr(adapter, hdr, first_skb, wrb_cnt, copied);
+	wrb_fill_hdr(adapter, hdr, first_skb, wrb_cnt, copied, skip_hw_vlan);
 	be_dws_cpu_to_le(hdr, sizeof(*hdr));
 
 	return copied;
@@ -749,7 +771,8 @@ dma_err:
 }
 
 static struct sk_buff *be_insert_vlan_in_pkt(struct be_adapter *adapter,
-					     struct sk_buff *skb)
+					     struct sk_buff *skb,
+					     bool *skip_hw_vlan)
 {
 	u16 vlan_tag = 0;
 
@@ -759,14 +782,72 @@ static struct sk_buff *be_insert_vlan_in_pkt(struct be_adapter *adapter,
 
 	if (vlan_tx_tag_present(skb)) {
 		vlan_tag = be_get_tx_vlan_tag(adapter, skb);
-		skb = __vlan_put_tag(skb, vlan_tag);
+		skb = __vlan_put_tag(skb, htons(ETH_P_8021Q), vlan_tag);
 		if (skb)
 			skb->vlan_tci = 0;
 	}
 
+	if (qnq_async_evt_rcvd(adapter) && adapter->pvid) {
+		if (!vlan_tag)
+			vlan_tag = adapter->pvid;
+		if (skip_hw_vlan)
+			*skip_hw_vlan = true;
+	}
+
+	if (vlan_tag) {
+		skb = __vlan_put_tag(skb, htons(ETH_P_8021Q), vlan_tag);
+		if (unlikely(!skb))
+			return skb;
+
+		skb->vlan_tci = 0;
+	}
+
+	/* Insert the outer VLAN, if any */
+	if (adapter->qnq_vid) {
+		vlan_tag = adapter->qnq_vid;
+		skb = __vlan_put_tag(skb, htons(ETH_P_8021Q), vlan_tag);
+		if (unlikely(!skb))
+			return skb;
+		if (skip_hw_vlan)
+			*skip_hw_vlan = true;
+	}
+
 	return skb;
 }
 
+static bool be_ipv6_exthdr_check(struct sk_buff *skb)
+{
+	struct ethhdr *eh = (struct ethhdr *)skb->data;
+	u16 offset = ETH_HLEN;
+
+	if (eh->h_proto == htons(ETH_P_IPV6)) {
+		struct ipv6hdr *ip6h = (struct ipv6hdr *)(skb->data + offset);
+
+		offset += sizeof(struct ipv6hdr);
+		if (ip6h->nexthdr != NEXTHDR_TCP &&
+		    ip6h->nexthdr != NEXTHDR_UDP) {
+			struct ipv6_opt_hdr *ehdr =
+				(struct ipv6_opt_hdr *) (skb->data + offset);
+
+			/* offending pkt: 2nd byte following IPv6 hdr is 0xff */
+			if (ehdr->hdrlen == 0xff)
+				return true;
+		}
+	}
+	return false;
+}
+
+static int be_vlan_tag_tx_chk(struct be_adapter *adapter, struct sk_buff *skb)
+{
+	return vlan_tx_tag_present(skb) || adapter->pvid || adapter->qnq_vid;
+}
+
+static int be_ipv6_tx_stall_chk(struct be_adapter *adapter, struct sk_buff *skb)
+{
+	return BE3_chip(adapter) &&
+		be_ipv6_exthdr_check(skb);
+}
+
 static netdev_tx_t be_xmit(struct sk_buff *skb,
 			struct net_device *netdev)
 {
@@ -777,33 +858,64 @@ static netdev_tx_t be_xmit(struct sk_buff *skb,
 	u32 wrb_cnt = 0, copied = 0;
 	u32 start = txq->head, eth_hdr_len;
 	bool dummy_wrb, stopped = false;
+	bool skip_hw_vlan = false;
+	struct vlan_ethhdr *veh = (struct vlan_ethhdr *)skb->data;
 
 	eth_hdr_len = ntohs(skb->protocol) == ETH_P_8021Q ?
 		VLAN_ETH_HLEN : ETH_HLEN;
 
-	/* HW has a bug which considers padding bytes as legal
-	 * and modifies the IPv4 hdr's 'tot_len' field
+	/* For padded packets, BE HW modifies tot_len field in IP header
+	 * incorrecly when VLAN tag is inserted by HW.
 	 */
-	if (skb->len <= 60 && be_vlan_tag_chk(adapter, skb) &&
-			is_ipv4_pkt(skb)) {
+	if (skb->len <= 60 && vlan_tx_tag_present(skb) && is_ipv4_pkt(skb)) {
 		ip = (struct iphdr *)ip_hdr(skb);
 		pskb_trim(skb, eth_hdr_len + ntohs(ip->tot_len));
 	}
 
+	/* If vlan tag is already inlined in the packet, skip HW VLAN
+	 * tagging in UMC mode
+	 */
+	if ((adapter->function_mode & UMC_ENABLED) &&
+	    veh->h_vlan_proto == htons(ETH_P_8021Q))
+			skip_hw_vlan = true;
+
 	/* HW has a bug wherein it will calculate CSUM for VLAN
 	 * pkts even though it is disabled.
 	 * Manually insert VLAN in pkt.
 	 */
 	if (skb->ip_summed != CHECKSUM_PARTIAL &&
-			be_vlan_tag_chk(adapter, skb)) {
-		skb = be_insert_vlan_in_pkt(adapter, skb);
+			vlan_tx_tag_present(skb)) {
+		skb = be_insert_vlan_in_pkt(adapter, skb, &skip_hw_vlan);
+		if (unlikely(!skb))
+			goto tx_drop;
+	}
+
+	/* HW may lockup when VLAN HW tagging is requested on
+	 * certain ipv6 packets. Drop such pkts if the HW workaround to
+	 * skip HW tagging is not enabled by FW.
+	 */
+	if (unlikely(be_ipv6_tx_stall_chk(adapter, skb) &&
+		     (adapter->pvid || adapter->qnq_vid) &&
+		     !qnq_async_evt_rcvd(adapter)))
+		goto tx_drop;
+
+	/* Manual VLAN tag insertion to prevent:
+	 * ASIC lockup when the ASIC inserts VLAN tag into
+	 * certain ipv6 packets. Insert VLAN tags in driver,
+	 * and set event, completion, vlan bits accordingly
+	 * in the Tx WRB.
+	 */
+	if (be_ipv6_tx_stall_chk(adapter, skb) &&
+	    be_vlan_tag_tx_chk(adapter, skb)) {
+		skb = be_insert_vlan_in_pkt(adapter, skb, &skip_hw_vlan);
 		if (unlikely(!skb))
 			goto tx_drop;
 	}
 
 	wrb_cnt = wrb_cnt_for_skb(adapter, skb, &dummy_wrb);
 
-	copied = make_tx_wrbs(adapter, txq, skb, wrb_cnt, dummy_wrb);
+	copied = make_tx_wrbs(adapter, txq, skb, wrb_cnt, dummy_wrb,
+			      skip_hw_vlan);
 	if (copied) {
 		int gso_segs = skb_shinfo(skb)->gso_segs;
 
@@ -822,7 +934,7 @@ static netdev_tx_t be_xmit(struct sk_buff *skb,
 			stopped = true;
 		}
 
-		be_txq_notify(adapter, txq->id, wrb_cnt);
+		be_txq_notify(adapter, txo, wrb_cnt);
 
 		be_tx_stats_update(txo, wrb_cnt, copied, gso_segs, stopped);
 	} else {
@@ -891,7 +1003,7 @@ set_vlan_promisc:
 	return status;
 }
 
-static int be_vlan_add_vid(struct net_device *netdev, u16 vid)
+static int be_vlan_add_vid(struct net_device *netdev, __be16 proto, u16 vid)
 {
 	struct be_adapter *adapter = netdev_priv(netdev);
 	int status = 0;
@@ -917,7 +1029,7 @@ ret:
 	return status;
 }
 
-static int be_vlan_rem_vid(struct net_device *netdev, u16 vid)
+static int be_vlan_rem_vid(struct net_device *netdev, __be16 proto, u16 vid)
 {
 	struct be_adapter *adapter = netdev_priv(netdev);
 	int status = 0;
@@ -1372,7 +1484,7 @@ static void be_rx_compl_process(struct be_rx_obj *rxo,
 
 
 	if (rxcp->vlanf)
-		__vlan_hwaccel_put_tag(skb, rxcp->vlan_tag);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), rxcp->vlan_tag);
 
 	netif_receive_skb(skb);
 }
@@ -1428,7 +1540,7 @@ void be_rx_compl_process_gro(struct be_rx_obj *rxo, struct napi_struct *napi,
 		skb->rxhash = rxcp->rss_hash;
 
 	if (rxcp->vlanf)
-		__vlan_hwaccel_put_tag(skb, rxcp->vlan_tag);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), rxcp->vlan_tag);
 
 	napi_gro_frags(napi);
 }
@@ -1715,7 +1827,7 @@ static void be_rx_cq_clean(struct be_rx_obj *rxo)
 			mdelay(1);
 		} else {
 			be_rx_compl_discard(rxo, rxcp);
-			be_cq_notify(adapter, rx_cq->id, true, 1);
+			be_cq_notify(adapter, rx_cq->id, false, 1);
 			if (rxcp->num_rcvd == 0)
 				break;
 		}
@@ -1958,7 +2070,7 @@ static int be_tx_qs_create(struct be_adapter *adapter)
 		if (status)
 			return status;
 
-		status = be_cmd_txq_create(adapter, &txo->q, &txo->cq);
+		status = be_cmd_txq_create(adapter, txo);
 		if (status)
 			return status;
 	}
@@ -2278,7 +2390,7 @@ static uint be_num_rss_want(struct be_adapter *adapter)
 	return num;
 }
 
-static void be_msix_enable(struct be_adapter *adapter)
+static int be_msix_enable(struct be_adapter *adapter)
 {
 #define BE_MIN_MSIX_VECTORS		1
 	int i, status, num_vec, num_roce_vec = 0;
@@ -2303,13 +2415,17 @@ static void be_msix_enable(struct be_adapter *adapter)
 		goto done;
 	} else if (status >= BE_MIN_MSIX_VECTORS) {
 		num_vec = status;
-		if (pci_enable_msix(adapter->pdev, adapter->msix_entries,
-				num_vec) == 0)
+		status = pci_enable_msix(adapter->pdev, adapter->msix_entries,
+					 num_vec);
+		if (!status)
 			goto done;
 	}
 
 	dev_warn(dev, "MSIx enable failed\n");
-	return;
+	/* INTx is not supported in VFs, so fail probe if enable_msix fails */
+	if (!be_physfn(adapter))
+		return status;
+	return 0;
 done:
 	if (be_roce_supported(adapter)) {
 		if (num_vec > num_roce_vec) {
@@ -2323,7 +2439,7 @@ done:
 	} else
 		adapter->num_msix_vec = num_vec;
 	dev_info(dev, "enabled %d MSI-x vector(s)\n", adapter->num_msix_vec);
-	return;
+	return 0;
 }
 
 static inline int be_msix_vec_get(struct be_adapter *adapter,
@@ -2417,11 +2533,6 @@ static void be_rx_qs_destroy(struct be_adapter *adapter)
 		q = &rxo->q;
 		if (q->created) {
 			be_cmd_rxq_destroy(adapter, q);
-			/* After the rxq is invalidated, wait for a grace time
-			 * of 1ms for all dma to end and the flush compl to
-			 * arrive
-			 */
-			mdelay(1);
 			be_rx_cq_clean(rxo);
 		}
 		be_queue_free(adapter, q);
@@ -2436,11 +2547,11 @@ static int be_close(struct net_device *netdev)
 
 	be_roce_dev_close(adapter);
 
-	if (!lancer_chip(adapter))
-		be_intr_set(adapter, false);
-
-	for_all_evt_queues(adapter, eqo, i)
-		napi_disable(&eqo->napi);
+	if (adapter->flags & BE_FLAGS_NAPI_ENABLED) {
+		for_all_evt_queues(adapter, eqo, i)
+			napi_disable(&eqo->napi);
+		adapter->flags &= ~BE_FLAGS_NAPI_ENABLED;
+	}
 
 	be_async_mcc_disable(adapter);
 
@@ -2448,6 +2559,7 @@ static int be_close(struct net_device *netdev)
 	 * all tx skbs are freed.
 	 */
 	be_tx_compl_clean(adapter);
+	netif_tx_disable(netdev);
 
 	be_rx_qs_destroy(adapter);
 
@@ -2500,9 +2612,19 @@ static int be_rx_qs_create(struct be_adapter *adapter)
 				rsstable[j + i] = rxo->rss_id;
 			}
 		}
-		rc = be_cmd_rss_config(adapter, rsstable, 128);
-		if (rc)
+		adapter->rss_flags = RSS_ENABLE_TCP_IPV4 | RSS_ENABLE_IPV4 |
+					RSS_ENABLE_TCP_IPV6 | RSS_ENABLE_IPV6;
+
+		if (!BEx_chip(adapter))
+			adapter->rss_flags |= RSS_ENABLE_UDP_IPV4 |
+						RSS_ENABLE_UDP_IPV6;
+
+		rc = be_cmd_rss_config(adapter, rsstable, adapter->rss_flags,
+				       128);
+		if (rc) {
+			adapter->rss_flags = 0;
 			return rc;
+		}
 	}
 
 	/* First time posting */
@@ -2524,10 +2646,9 @@ static int be_open(struct net_device *netdev)
 	if (status)
 		goto err;
 
-	be_irq_register(adapter);
-
-	if (!lancer_chip(adapter))
-		be_intr_set(adapter, true);
+	status = be_irq_register(adapter);
+	if (status)
+		goto err;
 
 	for_all_rx_queues(adapter, rxo, i)
 		be_cq_notify(adapter, rxo->cq.id, true, 0);
@@ -2541,11 +2662,13 @@ static int be_open(struct net_device *netdev)
 		napi_enable(&eqo->napi);
 		be_eq_notify(adapter, eqo->q.id, true, false, 0);
 	}
+	adapter->flags |= BE_FLAGS_NAPI_ENABLED;
 
 	status = be_cmd_link_status_query(adapter, NULL, &link_status, 0);
 	if (!status)
 		be_link_status_update(adapter, link_status);
 
+	netif_tx_start_all_queues(netdev);
 	be_roce_dev_open(adapter);
 	return 0;
 err:
@@ -2563,10 +2686,9 @@ static int be_setup_wol(struct be_adapter *adapter, bool enable)
 
 	cmd.size = sizeof(struct be_cmd_req_acpi_wol_magic_config);
 	cmd.va = dma_alloc_coherent(&adapter->pdev->dev, cmd.size, &cmd.dma,
-				    GFP_KERNEL);
+				    GFP_KERNEL | __GFP_ZERO);
 	if (cmd.va == NULL)
 		return -1;
-	memset(cmd.va, 0, cmd.size);
 
 	if (enable) {
 		status = pci_write_config_dword(adapter->pdev,
@@ -2658,6 +2780,8 @@ static void be_vf_clear(struct be_adapter *adapter)
 		goto done;
 	}
 
+	pci_disable_sriov(adapter->pdev);
+
 	for_all_vfs(adapter, vf_cfg, vf) {
 		if (lancer_chip(adapter))
 			be_cmd_set_mac_list(adapter, NULL, 0, vf + 1);
@@ -2667,7 +2791,6 @@ static void be_vf_clear(struct be_adapter *adapter)
 
 		be_cmd_if_destroy(adapter, vf_cfg->if_handle, vf + 1);
 	}
-	pci_disable_sriov(adapter->pdev);
 done:
 	kfree(adapter->vf_cfg);
 	adapter->num_vfs = 0;
@@ -2714,7 +2837,8 @@ static int be_vfs_if_create(struct be_adapter *adapter)
 
 	for_all_vfs(adapter, vf_cfg, vf) {
 		if (!BE3_chip(adapter))
-			be_cmd_get_profile_config(adapter, &cap_flags, vf + 1);
+			be_cmd_get_profile_config(adapter, &cap_flags,
+						  NULL, vf + 1);
 
 		/* If a FW profile exists, then cap_flags are updated */
 		en_flags = cap_flags & (BE_IF_FLAGS_UNTAGGED |
@@ -2763,13 +2887,8 @@ static int be_vf_setup(struct be_adapter *adapter)
 			dev_info(dev, "Device supports %d VFs and not %d\n",
 				 adapter->dev_num_vfs, num_vfs);
 		adapter->num_vfs = min_t(u16, num_vfs, adapter->dev_num_vfs);
-
-		status = pci_enable_sriov(adapter->pdev, num_vfs);
-		if (status) {
-			dev_err(dev, "SRIOV enable failed\n");
-			adapter->num_vfs = 0;
+		if (!adapter->num_vfs)
 			return 0;
-		}
 	}
 
 	status = be_vf_setup_init(adapter);
@@ -2818,6 +2937,15 @@ static int be_vf_setup(struct be_adapter *adapter)
 
 		be_cmd_enable_vf(adapter, vf + 1);
 	}
+
+	if (!old_vfs) {
+		status = pci_enable_sriov(adapter->pdev, adapter->num_vfs);
+		if (status) {
+			dev_err(dev, "SRIOV enable failed\n");
+			adapter->num_vfs = 0;
+			goto err;
+		}
+	}
 	return 0;
 err:
 	dev_err(dev, "VF setup failed\n");
@@ -2878,11 +3006,14 @@ static void be_get_resources(struct be_adapter *adapter)
 	u16 dev_num_vfs;
 	int pos, status;
 	bool profile_present = false;
+	u16 txq_count = 0;
 
 	if (!BEx_chip(adapter)) {
 		status = be_cmd_get_func_config(adapter);
 		if (!status)
 			profile_present = true;
+	} else if (BE3_chip(adapter) && be_physfn(adapter)) {
+		be_cmd_get_profile_config(adapter, NULL, &txq_count, 0);
 	}
 
 	if (profile_present) {
@@ -2920,7 +3051,9 @@ static void be_get_resources(struct be_adapter *adapter)
 			adapter->max_vlans = BE_NUM_VLANS_SUPPORTED;
 
 		adapter->max_mcast_mac = BE_MAX_MC;
-		adapter->max_tx_queues = MAX_TX_QS;
+		adapter->max_tx_queues = txq_count ? txq_count : MAX_TX_QS;
+		adapter->max_tx_queues = min_t(u16, adapter->max_tx_queues,
+					       MAX_TX_QS);
 		adapter->max_rss_queues = (adapter->be3_native) ?
 					   BE3_MAX_RSS_QS : BE2_MAX_RSS_QS;
 		adapter->max_event_queues = BE3_MAX_RSS_QS;
@@ -2954,7 +3087,8 @@ static int be_get_config(struct be_adapter *adapter)
 
 	status = be_cmd_query_fw_cfg(adapter, &adapter->port_num,
 				     &adapter->function_mode,
-				     &adapter->function_caps);
+				     &adapter->function_caps,
+				     &adapter->asic_rev);
 	if (status)
 		goto err;
 
@@ -2990,7 +3124,9 @@ static int be_setup(struct be_adapter *adapter)
 	if (status)
 		goto err;
 
-	be_msix_enable(adapter);
+	status = be_msix_enable(adapter);
+	if (status)
+		goto err;
 
 	status = be_evt_queues_create(adapter);
 	if (status)
@@ -3064,7 +3200,7 @@ static int be_setup(struct be_adapter *adapter)
 		be_cmd_set_flow_control(adapter, adapter->tx_fc,
 					adapter->rx_fc);
 
-	if (be_physfn(adapter) && num_vfs) {
+	if (be_physfn(adapter)) {
 		if (adapter->dev_num_vfs)
 			be_vf_setup(adapter);
 		else
@@ -3215,7 +3351,7 @@ static int be_flash(struct be_adapter *adapter, const u8 *img,
 	return 0;
 }
 
-/* For BE2 and BE3 */
+/* For BE2, BE3 and BE3-R */
 static int be_flash_BEx(struct be_adapter *adapter,
 			 const struct firmware *fw,
 			 struct be_dma_mem *flash_cmd,
@@ -3458,11 +3594,9 @@ static int lancer_fw_download(struct be_adapter *adapter,
 	flash_cmd.size = sizeof(struct lancer_cmd_req_write_object)
 				+ LANCER_FW_DOWNLOAD_CHUNK;
 	flash_cmd.va = dma_alloc_coherent(&adapter->pdev->dev, flash_cmd.size,
-						&flash_cmd.dma, GFP_KERNEL);
+					  &flash_cmd.dma, GFP_KERNEL);
 	if (!flash_cmd.va) {
 		status = -ENOMEM;
-		dev_err(&adapter->pdev->dev,
-			"Memory allocation failure while flashing\n");
 		goto lancer_fw_exit;
 	}
 
@@ -3530,18 +3664,22 @@ lancer_fw_exit:
 
 #define UFI_TYPE2		2
 #define UFI_TYPE3		3
+#define UFI_TYPE3R		10
 #define UFI_TYPE4		4
 static int be_get_ufi_type(struct be_adapter *adapter,
-			   struct flash_file_hdr_g2 *fhdr)
+			   struct flash_file_hdr_g3 *fhdr)
 {
 	if (fhdr == NULL)
 		goto be_get_ufi_exit;
 
 	if (skyhawk_chip(adapter) && fhdr->build[0] == '4')
 		return UFI_TYPE4;
-	else if (BE3_chip(adapter) && fhdr->build[0] == '3')
-		return UFI_TYPE3;
-	else if (BE2_chip(adapter) && fhdr->build[0] == '2')
+	else if (BE3_chip(adapter) && fhdr->build[0] == '3') {
+		if (fhdr->asic_type_rev == 0x10)
+			return UFI_TYPE3R;
+		else
+			return UFI_TYPE3;
+	} else if (BE2_chip(adapter) && fhdr->build[0] == '2')
 		return UFI_TYPE2;
 
 be_get_ufi_exit:
@@ -3552,7 +3690,6 @@ be_get_ufi_exit:
 
 static int be_fw_download(struct be_adapter *adapter, const struct firmware* fw)
 {
-	struct flash_file_hdr_g2 *fhdr;
 	struct flash_file_hdr_g3 *fhdr3;
 	struct image_hdr *img_hdr_ptr = NULL;
 	struct be_dma_mem flash_cmd;
@@ -3564,29 +3701,41 @@ static int be_fw_download(struct be_adapter *adapter, const struct firmware* fw)
 					  &flash_cmd.dma, GFP_KERNEL);
 	if (!flash_cmd.va) {
 		status = -ENOMEM;
-		dev_err(&adapter->pdev->dev,
-			"Memory allocation failure while flashing\n");
 		goto be_fw_exit;
 	}
 
 	p = fw->data;
-	fhdr = (struct flash_file_hdr_g2 *)p;
+	fhdr3 = (struct flash_file_hdr_g3 *)p;
 
-	ufi_type = be_get_ufi_type(adapter, fhdr);
+	ufi_type = be_get_ufi_type(adapter, fhdr3);
 
-	fhdr3 = (struct flash_file_hdr_g3 *)fw->data;
 	num_imgs = le32_to_cpu(fhdr3->num_imgs);
 	for (i = 0; i < num_imgs; i++) {
 		img_hdr_ptr = (struct image_hdr *)(fw->data +
 				(sizeof(struct flash_file_hdr_g3) +
 				 i * sizeof(struct image_hdr)));
 		if (le32_to_cpu(img_hdr_ptr->imageid) == 1) {
-			if (ufi_type == UFI_TYPE4)
+			switch (ufi_type) {
+			case UFI_TYPE4:
 				status = be_flash_skyhawk(adapter, fw,
 							&flash_cmd, num_imgs);
-			else if (ufi_type == UFI_TYPE3)
+				break;
+			case UFI_TYPE3R:
 				status = be_flash_BEx(adapter, fw, &flash_cmd,
 						      num_imgs);
+				break;
+			case UFI_TYPE3:
+				/* Do not flash this ufi on BE3-R cards */
+				if (adapter->asic_rev < 0x10)
+					status = be_flash_BEx(adapter, fw,
+							      &flash_cmd,
+							      num_imgs);
+				else {
+					status = -1;
+					dev_err(&adapter->pdev->dev,
+						"Can't load BE3 UFI on BE3R\n");
+				}
+			}
 		}
 	}
 
@@ -3663,12 +3812,12 @@ static void be_netdev_init(struct net_device *netdev)
 
 	netdev->hw_features |= NETIF_F_SG | NETIF_F_TSO | NETIF_F_TSO6 |
 		NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM | NETIF_F_RXCSUM |
-		NETIF_F_HW_VLAN_TX;
+		NETIF_F_HW_VLAN_CTAG_TX;
 	if (be_multi_rxq(adapter))
 		netdev->hw_features |= NETIF_F_RXHASH;
 
 	netdev->features |= netdev->hw_features |
-		NETIF_F_HW_VLAN_RX | NETIF_F_HW_VLAN_FILTER;
+		NETIF_F_HW_VLAN_CTAG_RX | NETIF_F_HW_VLAN_CTAG_FILTER;
 
 	netdev->vlan_features |= NETIF_F_SG | NETIF_F_TSO | NETIF_F_TSO6 |
 		NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM;
@@ -3792,12 +3941,13 @@ static int be_ctrl_init(struct be_adapter *adapter)
 
 	rx_filter->size = sizeof(struct be_cmd_req_rx_filter);
 	rx_filter->va = dma_alloc_coherent(&adapter->pdev->dev, rx_filter->size,
-					&rx_filter->dma, GFP_KERNEL);
+					   &rx_filter->dma,
+					   GFP_KERNEL | __GFP_ZERO);
 	if (rx_filter->va == NULL) {
 		status = -ENOMEM;
 		goto free_mbox;
 	}
-	memset(rx_filter->va, 0, rx_filter->size);
+
 	mutex_init(&adapter->mbox_lock);
 	spin_lock_init(&adapter->mcc_lock);
 	spin_lock_init(&adapter->mcc_cq_lock);
@@ -3839,10 +3989,9 @@ static int be_stats_init(struct be_adapter *adapter)
 		cmd->size = sizeof(struct be_cmd_req_get_stats_v1);
 
 	cmd->va = dma_alloc_coherent(&adapter->pdev->dev, cmd->size, &cmd->dma,
-				     GFP_KERNEL);
+				     GFP_KERNEL | __GFP_ZERO);
 	if (cmd->va == NULL)
 		return -1;
-	memset(cmd->va, 0, cmd->size);
 	return 0;
 }
 
@@ -3854,6 +4003,7 @@ static void be_remove(struct pci_dev *pdev)
 		return;
 
 	be_roce_dev_remove(adapter);
+	be_intr_set(adapter, false);
 
 	cancel_delayed_work_sync(&adapter->func_recovery_work);
 
@@ -4108,6 +4258,11 @@ static int be_probe(struct pci_dev *pdev, const struct pci_device_id *pdev_id)
 
 	status = dma_set_mask(&pdev->dev, DMA_BIT_MASK(64));
 	if (!status) {
+		status = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(64));
+		if (status < 0) {
+			dev_err(&pdev->dev, "dma_set_coherent_mask failed\n");
+			goto free_netdev;
+		}
 		netdev->features |= NETIF_F_HIGHDMA;
 	} else {
 		status = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
@@ -4132,22 +4287,22 @@ static int be_probe(struct pci_dev *pdev, const struct pci_device_id *pdev_id)
 			goto ctrl_clean;
 	}
 
-	/* tell fw we're ready to fire cmds */
-	status = be_cmd_fw_init(adapter);
-	if (status)
-		goto ctrl_clean;
-
 	if (be_reset_required(adapter)) {
 		status = be_cmd_reset_function(adapter);
 		if (status)
 			goto ctrl_clean;
+
+		/* Wait for interrupts to quiesce after an FLR */
+		msleep(100);
 	}
 
-	/* The INTR bit may be set in the card when probed by a kdump kernel
-	 * after a crash.
-	 */
-	if (!lancer_chip(adapter))
-		be_intr_set(adapter, false);
+	/* Allow interrupts for other ULPs running on NIC function */
+	be_intr_set(adapter, true);
+
+	/* tell fw we're ready to fire cmds */
+	status = be_cmd_fw_init(adapter);
+	if (status)
+		goto ctrl_clean;
 
 	status = be_stats_init(adapter);
 	if (status)
@@ -4358,12 +4513,12 @@ static void be_eeh_resume(struct pci_dev *pdev)
 
 	pci_save_state(pdev);
 
-	/* tell fw we're ready to fire cmds */
-	status = be_cmd_fw_init(adapter);
+	status = be_cmd_reset_function(adapter);
 	if (status)
 		goto err;
 
-	status = be_cmd_reset_function(adapter);
+	/* tell fw we're ready to fire cmds */
+	status = be_cmd_fw_init(adapter);
 	if (status)
 		goto err;
 
diff --git a/drivers/net/ethernet/emulex/benet/be_roce.c b/drivers/net/ethernet/emulex/benet/be_roce.c
index 55d32aa0a093..f3d126dcc104 100644
--- a/drivers/net/ethernet/emulex/benet/be_roce.c
+++ b/drivers/net/ethernet/emulex/benet/be_roce.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2011 Emulex
+ * Copyright (C) 2005 - 2013 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/net/ethernet/emulex/benet/be_roce.h b/drivers/net/ethernet/emulex/benet/be_roce.h
index db4ea8081c07..276572998463 100644
--- a/drivers/net/ethernet/emulex/benet/be_roce.h
+++ b/drivers/net/ethernet/emulex/benet/be_roce.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2011 Emulex
+ * Copyright (C) 2005 - 2013 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/net/ethernet/faraday/ftgmac100.c b/drivers/net/ethernet/faraday/ftgmac100.c
index 7c361d1db94c..21b85fb7d05f 100644
--- a/drivers/net/ethernet/faraday/ftgmac100.c
+++ b/drivers/net/ethernet/faraday/ftgmac100.c
@@ -780,12 +780,11 @@ static int ftgmac100_alloc_buffers(struct ftgmac100 *priv)
 
 	priv->descs = dma_alloc_coherent(priv->dev,
 					 sizeof(struct ftgmac100_descs),
-					 &priv->descs_dma_addr, GFP_KERNEL);
+					 &priv->descs_dma_addr,
+					 GFP_KERNEL | __GFP_ZERO);
 	if (!priv->descs)
 		return -ENOMEM;
 
-	memset(priv->descs, 0, sizeof(struct ftgmac100_descs));
-
 	/* initialize RX ring */
 	ftgmac100_rxdes_set_end_of_ring(&priv->descs->rxdes[RX_QUEUE_ENTRIES - 1]);
 
@@ -1350,22 +1349,7 @@ static struct platform_driver ftgmac100_driver = {
 	},
 };
 
-/******************************************************************************
- * initialization / finalization
- *****************************************************************************/
-static int __init ftgmac100_init(void)
-{
-	pr_info("Loading version " DRV_VERSION " ...\n");
-	return platform_driver_register(&ftgmac100_driver);
-}
-
-static void __exit ftgmac100_exit(void)
-{
-	platform_driver_unregister(&ftgmac100_driver);
-}
-
-module_init(ftgmac100_init);
-module_exit(ftgmac100_exit);
+module_platform_driver(ftgmac100_driver);
 
 MODULE_AUTHOR("Po-Yu Chuang <ratbert@faraday-tech.com>");
 MODULE_DESCRIPTION("FTGMAC100 driver");
diff --git a/drivers/net/ethernet/faraday/ftmac100.c b/drivers/net/ethernet/faraday/ftmac100.c
index b5ea8fbd8a76..a6eda8d83138 100644
--- a/drivers/net/ethernet/faraday/ftmac100.c
+++ b/drivers/net/ethernet/faraday/ftmac100.c
@@ -732,13 +732,13 @@ static int ftmac100_alloc_buffers(struct ftmac100 *priv)
 {
 	int i;
 
-	priv->descs = dma_alloc_coherent(priv->dev, sizeof(struct ftmac100_descs),
-					 &priv->descs_dma_addr, GFP_KERNEL);
+	priv->descs = dma_alloc_coherent(priv->dev,
+					 sizeof(struct ftmac100_descs),
+					 &priv->descs_dma_addr,
+					 GFP_KERNEL | __GFP_ZERO);
 	if (!priv->descs)
 		return -ENOMEM;
 
-	memset(priv->descs, 0, sizeof(struct ftmac100_descs));
-
 	/* initialize RX ring */
 	ftmac100_rxdes_set_end_of_ring(&priv->descs->rxdes[RX_QUEUE_ENTRIES - 1]);
 
diff --git a/drivers/net/ethernet/freescale/Makefile b/drivers/net/ethernet/freescale/Makefile
index b7d58fe6f531..549ce13b92ac 100644
--- a/drivers/net/ethernet/freescale/Makefile
+++ b/drivers/net/ethernet/freescale/Makefile
@@ -2,7 +2,8 @@
 # Makefile for the Freescale network device drivers.
 #
 
-obj-$(CONFIG_FEC) += fec.o fec_ptp.o
+obj-$(CONFIG_FEC) += fec.o
+fec-objs :=fec_main.o fec_ptp.o
 obj-$(CONFIG_FEC_MPC52xx) += fec_mpc52xx.o
 ifeq ($(CONFIG_FEC_MPC52xx_MDIO),y)
 	obj-$(CONFIG_FEC_MPC52xx) += fec_mpc52xx_phy.o
diff --git a/drivers/net/ethernet/freescale/fec.h b/drivers/net/ethernet/freescale/fec.h
index eb4372962839..9ce5b7185fda 100644
--- a/drivers/net/ethernet/freescale/fec.h
+++ b/drivers/net/ethernet/freescale/fec.h
@@ -52,6 +52,7 @@
 #define FEC_R_FIFO_RSEM		0x194 /* Receive FIFO section empty threshold */
 #define FEC_R_FIFO_RAEM		0x198 /* Receive FIFO almost empty threshold */
 #define FEC_R_FIFO_RAFL		0x19c /* Receive FIFO almost full threshold */
+#define FEC_RACC		0x1C4 /* Receive Accelerator function */
 #define FEC_MIIGSK_CFGR		0x300 /* MIIGSK Configuration reg */
 #define FEC_MIIGSK_ENR		0x308 /* MIIGSK Enable reg */
 
@@ -164,9 +165,11 @@ struct bufdesc_ex {
 #define BD_ENET_TX_CSL          ((ushort)0x0001)
 #define BD_ENET_TX_STATS        ((ushort)0x03ff)        /* All status bits */
 
-/*enhanced buffer desciptor control/status used by Ethernet transmit*/
+/*enhanced buffer descriptor control/status used by Ethernet transmit*/
 #define BD_ENET_TX_INT          0x40000000
 #define BD_ENET_TX_TS           0x20000000
+#define BD_ENET_TX_PINS         0x10000000
+#define BD_ENET_TX_IINS         0x08000000
 
 
 /* This device has up to three irqs on some platforms */
@@ -190,6 +193,15 @@ struct bufdesc_ex {
 
 #define BD_ENET_RX_INT          0x00800000
 #define BD_ENET_RX_PTP          ((ushort)0x0400)
+#define BD_ENET_RX_ICE		0x00000020
+#define BD_ENET_RX_PCR		0x00000010
+#define FLAG_RX_CSUM_ENABLED	(BD_ENET_RX_ICE | BD_ENET_RX_PCR)
+#define FLAG_RX_CSUM_ERROR	(BD_ENET_RX_ICE | BD_ENET_RX_PCR)
+
+struct fec_enet_delayed_work {
+	struct delayed_work delay_work;
+	bool timeout;
+};
 
 /* The FEC buffer descriptors track the ring buffers.  The rx_bd_base and
  * tx_bd_base always point to the base of the buffer descriptors.  The
@@ -207,6 +219,7 @@ struct fec_enet_private {
 
 	struct clk *clk_ipg;
 	struct clk *clk_ahb;
+	struct clk *clk_enet_out;
 	struct clk *clk_ptp;
 
 	/* The saved address of a sent-in-place packet/buffer, for skfree(). */
@@ -224,9 +237,6 @@ struct fec_enet_private {
 	/* The ring entries to be free()ed */
 	struct bufdesc	*dirty_tx;
 
-	/* hold while accessing the HW like ringbuffer for tx/rx but not MAC */
-	spinlock_t hw_lock;
-
 	struct	platform_device *pdev;
 
 	int	opened;
@@ -247,6 +257,7 @@ struct fec_enet_private {
 	int	pause_flag;
 
 	struct	napi_struct napi;
+	int	csum_flags;
 
 	struct ptp_clock *ptp_clock;
 	struct ptp_clock_info ptp_caps;
@@ -260,7 +271,7 @@ struct fec_enet_private {
 	int hwts_rx_en;
 	int hwts_tx_en;
 	struct timer_list time_keep;
-
+	struct fec_enet_delayed_work delay_work;
 };
 
 void fec_ptp_init(struct net_device *ndev, struct platform_device *pdev);
diff --git a/drivers/net/ethernet/freescale/fec.c b/drivers/net/ethernet/freescale/fec_main.c
index 73195f643c9c..aff0310a778b 100644
--- a/drivers/net/ethernet/freescale/fec.c
+++ b/drivers/net/ethernet/freescale/fec_main.c
@@ -29,12 +29,17 @@
 #include <linux/ioport.h>
 #include <linux/slab.h>
 #include <linux/interrupt.h>
-#include <linux/pci.h>
 #include <linux/init.h>
 #include <linux/delay.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/skbuff.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <net/ip.h>
+#include <linux/tcp.h>
+#include <linux/udp.h>
+#include <linux/icmp.h>
 #include <linux/spinlock.h>
 #include <linux/workqueue.h>
 #include <linux/bitops.h>
@@ -53,11 +58,6 @@
 
 #include <asm/cacheflush.h>
 
-#ifndef CONFIG_ARM
-#include <asm/coldfire.h>
-#include <asm/mcfsim.h>
-#endif
-
 #include "fec.h"
 
 #if defined(CONFIG_ARM)
@@ -107,6 +107,9 @@ static struct platform_device_id fec_devtype[] = {
 		.driver_data = FEC_QUIRK_ENET_MAC | FEC_QUIRK_HAS_GBIT |
 				FEC_QUIRK_HAS_BUFDESC_EX,
 	}, {
+		.name = "mvf-fec",
+		.driver_data = FEC_QUIRK_ENET_MAC,
+	}, {
 		/* sentinel */
 	}
 };
@@ -117,6 +120,7 @@ enum imx_fec_type {
 	IMX27_FEC,	/* runs on i.mx27/35/51 */
 	IMX28_FEC,
 	IMX6Q_FEC,
+	MVF_FEC,
 };
 
 static const struct of_device_id fec_dt_ids[] = {
@@ -124,6 +128,7 @@ static const struct of_device_id fec_dt_ids[] = {
 	{ .compatible = "fsl,imx27-fec", .data = &fec_devtype[IMX27_FEC], },
 	{ .compatible = "fsl,imx28-fec", .data = &fec_devtype[IMX28_FEC], },
 	{ .compatible = "fsl,imx6q-fec", .data = &fec_devtype[IMX6Q_FEC], },
+	{ .compatible = "fsl,mvf-fec", .data = &fec_devtype[MVF_FEC], },
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, fec_dt_ids);
@@ -177,6 +182,11 @@ MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
 #define PKT_MINBUF_SIZE		64
 #define PKT_MAXBLR_SIZE		1520
 
+/* FEC receive acceleration */
+#define FEC_RACC_IPDIS		(1 << 1)
+#define FEC_RACC_PRODIS		(1 << 2)
+#define FEC_RACC_OPTIONS	(FEC_RACC_IPDIS | FEC_RACC_PRODIS)
+
 /*
  * The 5270/5271/5280/5282/532x RX control register also contains maximum frame
  * size bits. Other FEC hardware does not, so we need to take that into
@@ -237,6 +247,21 @@ static void *swap_buffer(void *bufaddr, int len)
 	return bufaddr;
 }
 
+static int
+fec_enet_clear_csum(struct sk_buff *skb, struct net_device *ndev)
+{
+	/* Only run for packets requiring a checksum. */
+	if (skb->ip_summed != CHECKSUM_PARTIAL)
+		return 0;
+
+	if (unlikely(skb_cow_head(skb, 0)))
+		return -1;
+
+	*(__sum16 *)(skb->head + skb->csum_start + skb->csum_offset) = 0;
+
+	return 0;
+}
+
 static netdev_tx_t
 fec_enet_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 {
@@ -249,7 +274,7 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	unsigned int index;
 
 	if (!fep->link) {
-		/* Link is down or autonegotiation is in progress. */
+		/* Link is down or auto-negotiation is in progress. */
 		return NETDEV_TX_BUSY;
 	}
 
@@ -262,10 +287,16 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 		/* Ooops.  All transmit buffers are full.  Bail out.
 		 * This should not happen, since ndev->tbusy should be set.
 		 */
-		printk("%s: tx queue full!.\n", ndev->name);
+		netdev_err(ndev, "tx queue full!\n");
 		return NETDEV_TX_BUSY;
 	}
 
+	/* Protocol checksum off-load for TCP and UDP. */
+	if (fec_enet_clear_csum(skb, ndev)) {
+		kfree_skb(skb);
+		return NETDEV_TX_OK;
+	}
+
 	/* Clear all of the status flags */
 	status &= ~BD_ENET_TX_STATS;
 
@@ -322,8 +353,14 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 			ebdp->cbd_esc = (BD_ENET_TX_TS | BD_ENET_TX_INT);
 			skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 		} else {
-
 			ebdp->cbd_esc = BD_ENET_TX_INT;
+
+			/* Enable protocol checksum flags
+			 * We do not bother with the IP Checksum bits as they
+			 * are done by the kernel
+			 */
+			if (skb->ip_summed == CHECKSUM_PARTIAL)
+				ebdp->cbd_esc |= BD_ENET_TX_PINS;
 		}
 	}
 	/* If this was the last BD in the ring, start at the beginning again. */
@@ -403,10 +440,18 @@ fec_restart(struct net_device *ndev, int duplex)
 	const struct platform_device_id *id_entry =
 				platform_get_device_id(fep->pdev);
 	int i;
+	u32 val;
 	u32 temp_mac[2];
 	u32 rcntl = OPT_FRAME_SIZE | 0x04;
 	u32 ecntl = 0x2; /* ETHEREN */
 
+	if (netif_running(ndev)) {
+		netif_device_detach(ndev);
+		napi_disable(&fep->napi);
+		netif_stop_queue(ndev);
+		netif_tx_lock(ndev);
+	}
+
 	/* Whack a reset.  We should wait for this. */
 	writel(1, fep->hwp + FEC_ECNTRL);
 	udelay(10);
@@ -469,6 +514,14 @@ fec_restart(struct net_device *ndev, int duplex)
 	/* Set MII speed */
 	writel(fep->phy_speed, fep->hwp + FEC_MII_SPEED);
 
+	/* set RX checksum */
+	val = readl(fep->hwp + FEC_RACC);
+	if (fep->csum_flags & FLAG_RX_CSUM_ENABLED)
+		val |= FEC_RACC_OPTIONS;
+	else
+		val &= ~FEC_RACC_OPTIONS;
+	writel(val, fep->hwp + FEC_RACC);
+
 	/*
 	 * The phy interface and speed need to get configured
 	 * differently on enet-mac.
@@ -526,7 +579,7 @@ fec_restart(struct net_device *ndev, int duplex)
 	     fep->phy_dev && fep->phy_dev->pause)) {
 		rcntl |= FEC_ENET_FCE;
 
-		/* set FIFO thresh hold parameter to reduce overrun */
+		/* set FIFO threshold parameter to reduce overrun */
 		writel(FEC_ENET_RSEM_V, fep->hwp + FEC_R_FIFO_RSEM);
 		writel(FEC_ENET_RSFL_V, fep->hwp + FEC_R_FIFO_RSFL);
 		writel(FEC_ENET_RAEM_V, fep->hwp + FEC_R_FIFO_RAEM);
@@ -559,6 +612,13 @@ fec_restart(struct net_device *ndev, int duplex)
 
 	/* Enable interrupts we wish to service */
 	writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
+
+	if (netif_running(ndev)) {
+		netif_device_attach(ndev);
+		napi_enable(&fep->napi);
+		netif_wake_queue(ndev);
+		netif_tx_unlock(ndev);
+	}
 }
 
 static void
@@ -574,7 +634,7 @@ fec_stop(struct net_device *ndev)
 		writel(1, fep->hwp + FEC_X_CNTRL); /* Graceful transmit stop */
 		udelay(10);
 		if (!(readl(fep->hwp + FEC_IEVENT) & FEC_ENET_GRA))
-			printk("fec_stop : Graceful transmit stop did not complete !\n");
+			netdev_err(ndev, "Graceful transmit stop did not complete!\n");
 	}
 
 	/* Whack a reset.  We should wait for this. */
@@ -598,8 +658,22 @@ fec_timeout(struct net_device *ndev)
 
 	ndev->stats.tx_errors++;
 
-	fec_restart(ndev, fep->full_duplex);
-	netif_wake_queue(ndev);
+	fep->delay_work.timeout = true;
+	schedule_delayed_work(&(fep->delay_work.delay_work), 0);
+}
+
+static void fec_enet_work(struct work_struct *work)
+{
+	struct fec_enet_private *fep =
+		container_of(work,
+			     struct fec_enet_private,
+			     delay_work.delay_work.work);
+
+	if (fep->delay_work.timeout) {
+		fep->delay_work.timeout = false;
+		fec_restart(fep->netdev, fep->full_duplex);
+		netif_wake_queue(fep->netdev);
+	}
 }
 
 static void
@@ -672,7 +746,7 @@ fec_enet_tx(struct net_device *ndev)
 		}
 
 		if (status & BD_ENET_TX_READY)
-			printk("HEY! Enet xmit interrupt and TX_READY.\n");
+			netdev_err(ndev, "HEY! Enet xmit interrupt and TX_READY\n");
 
 		/* Deferred means some collisions occurred during transmit,
 		 * but we eventually sent the packet OK.
@@ -740,7 +814,7 @@ fec_enet_rx(struct net_device *ndev, int budget)
 		 * the last indicator should be set.
 		 */
 		if ((status & BD_ENET_RX_LAST) == 0)
-			printk("FEC ENET: rcv is not +last\n");
+			netdev_err(ndev, "rcv is not +last\n");
 
 		if (!fep->opened)
 			goto rx_processing_done;
@@ -791,8 +865,6 @@ fec_enet_rx(struct net_device *ndev, int budget)
 		skb = netdev_alloc_skb(ndev, pkt_len - 4 + NET_IP_ALIGN);
 
 		if (unlikely(!skb)) {
-			printk("%s: Memory squeeze, dropping packet.\n",
-					ndev->name);
 			ndev->stats.rx_dropped++;
 		} else {
 			skb_reserve(skb, NET_IP_ALIGN);
@@ -816,6 +888,18 @@ fec_enet_rx(struct net_device *ndev, int budget)
 				spin_unlock_irqrestore(&fep->tmreg_lock, flags);
 			}
 
+			if (fep->bufdesc_ex &&
+				(fep->csum_flags & FLAG_RX_CSUM_ENABLED)) {
+				struct bufdesc_ex *ebdp =
+					(struct bufdesc_ex *)bdp;
+				if (!(ebdp->cbd_esc & FLAG_RX_CSUM_ERROR)) {
+					/* don't check it */
+					skb->ip_summed = CHECKSUM_UNNECESSARY;
+				} else {
+					skb_checksum_none_assert(skb);
+				}
+			}
+
 			if (!skb_defer_rx_timestamp(skb))
 				napi_gro_receive(&fep->napi, skb);
 		}
@@ -916,7 +1000,6 @@ static void fec_get_mac(struct net_device *ndev)
 	 */
 	iap = macaddr;
 
-#ifdef CONFIG_OF
 	/*
 	 * 2) from device tree data
 	 */
@@ -928,7 +1011,6 @@ static void fec_get_mac(struct net_device *ndev)
 				iap = (unsigned char *) mac;
 		}
 	}
-#endif
 
 	/*
 	 * 3) from flash or fuse (via platform data)
@@ -970,16 +1052,12 @@ static void fec_enet_adjust_link(struct net_device *ndev)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
 	struct phy_device *phy_dev = fep->phy_dev;
-	unsigned long flags;
-
 	int status_change = 0;
 
-	spin_lock_irqsave(&fep->hw_lock, flags);
-
 	/* Prevent a state halted on mii error */
 	if (fep->mii_timeout && phy_dev->state == PHY_HALTED) {
 		phy_dev->state = PHY_RESUMING;
-		goto spin_unlock;
+		return;
 	}
 
 	if (phy_dev->link) {
@@ -1007,9 +1085,6 @@ static void fec_enet_adjust_link(struct net_device *ndev)
 		}
 	}
 
-spin_unlock:
-	spin_unlock_irqrestore(&fep->hw_lock, flags);
-
 	if (status_change)
 		phy_print_status(phy_dev);
 }
@@ -1032,7 +1107,7 @@ static int fec_enet_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
 			usecs_to_jiffies(FEC_MII_TIMEOUT));
 	if (time_left == 0) {
 		fep->mii_timeout = 1;
-		printk(KERN_ERR "FEC: MDIO read timeout\n");
+		netdev_err(fep->netdev, "MDIO read timeout\n");
 		return -ETIMEDOUT;
 	}
 
@@ -1060,7 +1135,7 @@ static int fec_enet_mdio_write(struct mii_bus *bus, int mii_id, int regnum,
 			usecs_to_jiffies(FEC_MII_TIMEOUT));
 	if (time_left == 0) {
 		fep->mii_timeout = 1;
-		printk(KERN_ERR "FEC: MDIO write timeout\n");
+		netdev_err(fep->netdev, "MDIO write timeout\n");
 		return -ETIMEDOUT;
 	}
 
@@ -1100,9 +1175,7 @@ static int fec_enet_mii_probe(struct net_device *ndev)
 	}
 
 	if (phy_id >= PHY_MAX_ADDR) {
-		printk(KERN_INFO
-			"%s: no PHY, assuming direct connection to switch\n",
-			ndev->name);
+		netdev_info(ndev, "no PHY, assuming direct connection to switch\n");
 		strncpy(mdio_bus_id, "fixed-0", MII_BUS_ID_SIZE);
 		phy_id = 0;
 	}
@@ -1111,7 +1184,7 @@ static int fec_enet_mii_probe(struct net_device *ndev)
 	phy_dev = phy_connect(ndev, phy_name, &fec_enet_adjust_link,
 			      fep->phy_interface);
 	if (IS_ERR(phy_dev)) {
-		printk(KERN_ERR "%s: could not attach to PHY\n", ndev->name);
+		netdev_err(ndev, "could not attach to PHY\n");
 		return PTR_ERR(phy_dev);
 	}
 
@@ -1129,11 +1202,9 @@ static int fec_enet_mii_probe(struct net_device *ndev)
 	fep->link = 0;
 	fep->full_duplex = 0;
 
-	printk(KERN_INFO
-		"%s: Freescale FEC PHY driver [%s] (mii_bus:phy_addr=%s, irq=%d)\n",
-		ndev->name,
-		fep->phy_dev->drv->name, dev_name(&fep->phy_dev->dev),
-		fep->phy_dev->irq);
+	netdev_info(ndev, "Freescale FEC PHY driver [%s] (mii_bus:phy_addr=%s, irq=%d)\n",
+		    fep->phy_dev->drv->name, dev_name(&fep->phy_dev->dev),
+		    fep->phy_dev->irq);
 
 	return 0;
 }
@@ -1443,7 +1514,7 @@ static int fec_enet_alloc_buffers(struct net_device *ndev)
 
 		if (fep->bufdesc_ex) {
 			struct bufdesc_ex *ebdp = (struct bufdesc_ex *)bdp;
-			ebdp->cbd_esc = BD_ENET_RX_INT;
+			ebdp->cbd_esc = BD_ENET_TX_INT;
 		}
 
 		bdp = fec_enet_get_nextdesc(bdp, fep->bufdesc_ex);
@@ -1608,7 +1679,7 @@ fec_set_mac_address(struct net_device *ndev, void *p)
  * Polled functionality used by netconsole and others in non interrupt mode
  *
  */
-void fec_poll_controller(struct net_device *dev)
+static void fec_poll_controller(struct net_device *dev)
 {
 	int i;
 	struct fec_enet_private *fep = netdev_priv(dev);
@@ -1623,6 +1694,33 @@ void fec_poll_controller(struct net_device *dev)
 }
 #endif
 
+static int fec_set_features(struct net_device *netdev,
+	netdev_features_t features)
+{
+	struct fec_enet_private *fep = netdev_priv(netdev);
+	netdev_features_t changed = features ^ netdev->features;
+
+	netdev->features = features;
+
+	/* Receive checksum has been changed */
+	if (changed & NETIF_F_RXCSUM) {
+		if (features & NETIF_F_RXCSUM)
+			fep->csum_flags |= FLAG_RX_CSUM_ENABLED;
+		else
+			fep->csum_flags &= ~FLAG_RX_CSUM_ENABLED;
+
+		if (netif_running(netdev)) {
+			fec_stop(netdev);
+			fec_restart(netdev, fep->phy_dev->duplex);
+			netif_wake_queue(netdev);
+		} else {
+			fec_restart(netdev, fep->phy_dev->duplex);
+		}
+	}
+
+	return 0;
+}
+
 static const struct net_device_ops fec_netdev_ops = {
 	.ndo_open		= fec_enet_open,
 	.ndo_stop		= fec_enet_close,
@@ -1636,6 +1734,7 @@ static const struct net_device_ops fec_netdev_ops = {
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller	= fec_poll_controller,
 #endif
+	.ndo_set_features	= fec_set_features,
 };
 
  /*
@@ -1649,14 +1748,11 @@ static int fec_enet_init(struct net_device *ndev)
 
 	/* Allocate memory for buffer descriptors. */
 	cbd_base = dma_alloc_coherent(NULL, PAGE_SIZE, &fep->bd_dma,
-			GFP_KERNEL);
-	if (!cbd_base) {
-		printk("FEC: allocate descriptor memory failed?\n");
+				      GFP_KERNEL);
+	if (!cbd_base)
 		return -ENOMEM;
-	}
 
 	memset(cbd_base, 0, PAGE_SIZE);
-	spin_lock_init(&fep->hw_lock);
 
 	fep->netdev = ndev;
 
@@ -1679,22 +1775,19 @@ static int fec_enet_init(struct net_device *ndev)
 	writel(FEC_RX_DISABLED_IMASK, fep->hwp + FEC_IMASK);
 	netif_napi_add(ndev, &fep->napi, fec_enet_rx_napi, FEC_NAPI_WEIGHT);
 
+	/* enable hw accelerator */
+	ndev->features |= (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM
+			| NETIF_F_RXCSUM);
+	ndev->hw_features |= (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM
+			| NETIF_F_RXCSUM);
+	fep->csum_flags |= FLAG_RX_CSUM_ENABLED;
+
 	fec_restart(ndev, 0);
 
 	return 0;
 }
 
 #ifdef CONFIG_OF
-static int fec_get_phy_mode_dt(struct platform_device *pdev)
-{
-	struct device_node *np = pdev->dev.of_node;
-
-	if (np)
-		return of_get_phy_mode(np);
-
-	return -ENODEV;
-}
-
 static void fec_reset_phy(struct platform_device *pdev)
 {
 	int err, phy_reset;
@@ -1723,11 +1816,6 @@ static void fec_reset_phy(struct platform_device *pdev)
 	gpio_set_value(phy_reset, 1);
 }
 #else /* CONFIG_OF */
-static int fec_get_phy_mode_dt(struct platform_device *pdev)
-{
-	return -ENODEV;
-}
-
 static void fec_reset_phy(struct platform_device *pdev)
 {
 	/*
@@ -1758,16 +1846,10 @@ fec_probe(struct platform_device *pdev)
 	if (!r)
 		return -ENXIO;
 
-	r = request_mem_region(r->start, resource_size(r), pdev->name);
-	if (!r)
-		return -EBUSY;
-
 	/* Init network device */
 	ndev = alloc_etherdev(sizeof(struct fec_enet_private));
-	if (!ndev) {
-		ret = -ENOMEM;
-		goto failed_alloc_etherdev;
-	}
+	if (!ndev)
+		return -ENOMEM;
 
 	SET_NETDEV_DEV(ndev, &pdev->dev);
 
@@ -1779,7 +1861,7 @@ fec_probe(struct platform_device *pdev)
 	    (pdev->id_entry->driver_data & FEC_QUIRK_HAS_GBIT))
 		fep->pause_flag |= FEC_PAUSE_FLAG_AUTONEG;
 
-	fep->hwp = ioremap(r->start, resource_size(r));
+	fep->hwp = devm_request_and_ioremap(&pdev->dev, r);
 	fep->pdev = pdev;
 	fep->dev_id = dev_id++;
 
@@ -1792,7 +1874,7 @@ fec_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, ndev);
 
-	ret = fec_get_phy_mode_dt(pdev);
+	ret = of_get_phy_mode(pdev->dev.of_node);
 	if (ret < 0) {
 		pdata = pdev->dev.platform_data;
 		if (pdata)
@@ -1821,18 +1903,23 @@ fec_probe(struct platform_device *pdev)
 		goto failed_clk;
 	}
 
+	/* enet_out is optional, depends on board */
+	fep->clk_enet_out = devm_clk_get(&pdev->dev, "enet_out");
+	if (IS_ERR(fep->clk_enet_out))
+		fep->clk_enet_out = NULL;
+
 	fep->clk_ptp = devm_clk_get(&pdev->dev, "ptp");
 	fep->bufdesc_ex =
 		pdev->id_entry->driver_data & FEC_QUIRK_HAS_BUFDESC_EX;
 	if (IS_ERR(fep->clk_ptp)) {
-		ret = PTR_ERR(fep->clk_ptp);
+		fep->clk_ptp = NULL;
 		fep->bufdesc_ex = 0;
 	}
 
 	clk_prepare_enable(fep->clk_ahb);
 	clk_prepare_enable(fep->clk_ipg);
-	if (!IS_ERR(fep->clk_ptp))
-		clk_prepare_enable(fep->clk_ptp);
+	clk_prepare_enable(fep->clk_enet_out);
+	clk_prepare_enable(fep->clk_ptp);
 
 	reg_phy = devm_regulator_get(&pdev->dev, "phy");
 	if (!IS_ERR(reg_phy)) {
@@ -1882,6 +1969,10 @@ fec_probe(struct platform_device *pdev)
 	if (ret)
 		goto failed_register;
 
+	if (fep->bufdesc_ex && fep->ptp_clock)
+		netdev_info(ndev, "registered PHC device %d\n", fep->dev_id);
+
+	INIT_DELAYED_WORK(&(fep->delay_work.delay_work), fec_enet_work);
 	return 0;
 
 failed_register:
@@ -1897,15 +1988,12 @@ failed_irq:
 failed_regulator:
 	clk_disable_unprepare(fep->clk_ahb);
 	clk_disable_unprepare(fep->clk_ipg);
-	if (!IS_ERR(fep->clk_ptp))
-		clk_disable_unprepare(fep->clk_ptp);
+	clk_disable_unprepare(fep->clk_enet_out);
+	clk_disable_unprepare(fep->clk_ptp);
 failed_pin:
 failed_clk:
-	iounmap(fep->hwp);
 failed_ioremap:
 	free_netdev(ndev);
-failed_alloc_etherdev:
-	release_mem_region(r->start, resource_size(r));
 
 	return ret;
 }
@@ -1915,15 +2003,16 @@ fec_drv_remove(struct platform_device *pdev)
 {
 	struct net_device *ndev = platform_get_drvdata(pdev);
 	struct fec_enet_private *fep = netdev_priv(ndev);
-	struct resource *r;
 	int i;
 
+	cancel_delayed_work_sync(&(fep->delay_work.delay_work));
 	unregister_netdev(ndev);
 	fec_enet_mii_remove(fep);
 	del_timer_sync(&fep->time_keep);
 	clk_disable_unprepare(fep->clk_ptp);
 	if (fep->ptp_clock)
 		ptp_clock_unregister(fep->ptp_clock);
+	clk_disable_unprepare(fep->clk_enet_out);
 	clk_disable_unprepare(fep->clk_ahb);
 	clk_disable_unprepare(fep->clk_ipg);
 	for (i = 0; i < FEC_IRQ_NUM; i++) {
@@ -1931,19 +2020,14 @@ fec_drv_remove(struct platform_device *pdev)
 		if (irq > 0)
 			free_irq(irq, ndev);
 	}
-	iounmap(fep->hwp);
 	free_netdev(ndev);
 
-	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	BUG_ON(!r);
-	release_mem_region(r->start, resource_size(r));
-
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int
 fec_suspend(struct device *dev)
 {
@@ -1954,6 +2038,7 @@ fec_suspend(struct device *dev)
 		fec_stop(ndev);
 		netif_device_detach(ndev);
 	}
+	clk_disable_unprepare(fep->clk_enet_out);
 	clk_disable_unprepare(fep->clk_ahb);
 	clk_disable_unprepare(fep->clk_ipg);
 
@@ -1966,6 +2051,7 @@ fec_resume(struct device *dev)
 	struct net_device *ndev = dev_get_drvdata(dev);
 	struct fec_enet_private *fep = netdev_priv(ndev);
 
+	clk_prepare_enable(fep->clk_enet_out);
 	clk_prepare_enable(fep->clk_ahb);
 	clk_prepare_enable(fep->clk_ipg);
 	if (netif_running(ndev)) {
@@ -1975,24 +2061,15 @@ fec_resume(struct device *dev)
 
 	return 0;
 }
+#endif /* CONFIG_PM_SLEEP */
 
-static const struct dev_pm_ops fec_pm_ops = {
-	.suspend	= fec_suspend,
-	.resume		= fec_resume,
-	.freeze		= fec_suspend,
-	.thaw		= fec_resume,
-	.poweroff	= fec_suspend,
-	.restore	= fec_resume,
-};
-#endif
+static SIMPLE_DEV_PM_OPS(fec_pm_ops, fec_suspend, fec_resume);
 
 static struct platform_driver fec_driver = {
 	.driver	= {
 		.name	= DRIVER_NAME,
 		.owner	= THIS_MODULE,
-#ifdef CONFIG_PM
 		.pm	= &fec_pm_ops,
-#endif
 		.of_match_table = fec_dt_ids,
 	},
 	.id_table = fec_devtype,
diff --git a/drivers/net/ethernet/freescale/fec_mpc52xx.c b/drivers/net/ethernet/freescale/fec_mpc52xx.c
index 77943a6a1b8c..9bc15e2365bb 100644
--- a/drivers/net/ethernet/freescale/fec_mpc52xx.c
+++ b/drivers/net/ethernet/freescale/fec_mpc52xx.c
@@ -14,6 +14,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/dma-mapping.h>
 #include <linux/module.h>
 
@@ -858,13 +860,11 @@ static int mpc52xx_fec_probe(struct platform_device *op)
 	/* Reserve FEC control zone */
 	rv = of_address_to_resource(np, 0, &mem);
 	if (rv) {
-		printk(KERN_ERR DRIVER_NAME ": "
-				"Error while parsing device node resource\n" );
+		pr_err("Error while parsing device node resource\n");
 		goto err_netdev;
 	}
 	if (resource_size(&mem) < sizeof(struct mpc52xx_fec)) {
-		printk(KERN_ERR DRIVER_NAME
-		       " - invalid resource size (%lx < %x), check mpc52xx_devices.c\n",
+		pr_err("invalid resource size (%lx < %x), check mpc52xx_devices.c\n",
 		       (unsigned long)resource_size(&mem),
 		       sizeof(struct mpc52xx_fec));
 		rv = -EINVAL;
@@ -902,7 +902,7 @@ static int mpc52xx_fec_probe(struct platform_device *op)
 	priv->tx_dmatsk = bcom_fec_tx_init(FEC_TX_NUM_BD, tx_fifo);
 
 	if (!priv->rx_dmatsk || !priv->tx_dmatsk) {
-		printk(KERN_ERR DRIVER_NAME ": Can not init SDMA tasks\n" );
+		pr_err("Can not init SDMA tasks\n");
 		rv = -ENOMEM;
 		goto err_rx_tx_dmatsk;
 	}
@@ -982,8 +982,8 @@ static int mpc52xx_fec_probe(struct platform_device *op)
 
 	/* We're done ! */
 	dev_set_drvdata(&op->dev, ndev);
-	printk(KERN_INFO "%s: %s MAC %pM\n",
-	       ndev->name, op->dev.of_node->full_name, ndev->dev_addr);
+	netdev_info(ndev, "%s MAC %pM\n",
+		    op->dev.of_node->full_name, ndev->dev_addr);
 
 	return 0;
 
@@ -1094,7 +1094,7 @@ mpc52xx_fec_init(void)
 	int ret;
 	ret = platform_driver_register(&mpc52xx_fec_mdio_driver);
 	if (ret) {
-		printk(KERN_ERR DRIVER_NAME ": failed to register mdio driver\n");
+		pr_err("failed to register mdio driver\n");
 		return ret;
 	}
 #endif
diff --git a/drivers/net/ethernet/freescale/fec_ptp.c b/drivers/net/ethernet/freescale/fec_ptp.c
index 0d8df400a479..25fc960cbf0e 100644
--- a/drivers/net/ethernet/freescale/fec_ptp.c
+++ b/drivers/net/ethernet/freescale/fec_ptp.c
@@ -17,6 +17,8 @@
  * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/string.h>
@@ -128,7 +130,6 @@ void fec_ptp_start_cyclecounter(struct net_device *ndev)
 
 	spin_unlock_irqrestore(&fep->tmreg_lock, flags);
 }
-EXPORT_SYMBOL(fec_ptp_start_cyclecounter);
 
 /**
  * fec_ptp_adjfreq - adjust ptp cycle frequency
@@ -319,7 +320,6 @@ int fec_ptp_ioctl(struct net_device *ndev, struct ifreq *ifr, int cmd)
 	return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
 	    -EFAULT : 0;
 }
-EXPORT_SYMBOL(fec_ptp_ioctl);
 
 /**
  * fec_time_keep - call timecounter_read every second to avoid timer overrun
@@ -381,8 +381,5 @@ void fec_ptp_init(struct net_device *ndev, struct platform_device *pdev)
 	if (IS_ERR(fep->ptp_clock)) {
 		fep->ptp_clock = NULL;
 		pr_err("ptp_clock_register failed\n");
-	} else {
-		pr_info("registered PHC device on %s\n", ndev->name);
 	}
 }
-EXPORT_SYMBOL(fec_ptp_init);
diff --git a/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c b/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
index 46df28893c10..edc120094c34 100644
--- a/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
+++ b/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
@@ -177,8 +177,6 @@ static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
 				received++;
 				netif_receive_skb(skb);
 			} else {
-				dev_warn(fep->dev,
-					 "Memory squeeze, dropping packet.\n");
 				fep->stats.rx_dropped++;
 				skbn = skb;
 			}
@@ -309,8 +307,6 @@ static int fs_enet_rx_non_napi(struct net_device *dev)
 				received++;
 				netif_rx(skb);
 			} else {
-				dev_warn(fep->dev,
-					 "Memory squeeze, dropping packet.\n");
 				fep->stats.rx_dropped++;
 				skbn = skb;
 			}
@@ -505,11 +501,9 @@ void fs_init_bds(struct net_device *dev)
 	 */
 	for (i = 0, bdp = fep->rx_bd_base; i < fep->rx_ring; i++, bdp++) {
 		skb = netdev_alloc_skb(dev, ENET_RX_FRSIZE);
-		if (skb == NULL) {
-			dev_warn(fep->dev,
-				 "Memory squeeze, unable to allocate skb\n");
+		if (skb == NULL)
 			break;
-		}
+
 		skb_align(skb, ENET_RX_ALIGN);
 		fep->rx_skbuff[i] = skb;
 		CBDW_BUFADDR(bdp,
@@ -593,13 +587,8 @@ static struct sk_buff *tx_skb_align_workaround(struct net_device *dev,
 
 	/* Alloc new skb */
 	new_skb = netdev_alloc_skb(dev, skb->len + 4);
-	if (!new_skb) {
-		if (net_ratelimit()) {
-			dev_warn(fep->dev,
-				 "Memory squeeze, dropping tx packet.\n");
-		}
+	if (!new_skb)
 		return NULL;
-	}
 
 	/* Make sure new skb is properly aligned */
 	skb_align(new_skb, 4);
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index d2c5441d1bf0..2375a01715a0 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -132,7 +132,7 @@ static int gfar_poll(struct napi_struct *napi, int budget);
 static void gfar_netpoll(struct net_device *dev);
 #endif
 int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue, int rx_work_limit);
-static int gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue);
+static void gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue);
 static void gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
 			       int amount_pull, struct napi_struct *napi);
 void gfar_halt(struct net_device *dev);
@@ -245,14 +245,13 @@ static int gfar_alloc_skb_resources(struct net_device *ndev)
 
 	/* Allocate memory for the buffer descriptors */
 	vaddr = dma_alloc_coherent(dev,
-			sizeof(struct txbd8) * priv->total_tx_ring_size +
-			sizeof(struct rxbd8) * priv->total_rx_ring_size,
-			&addr, GFP_KERNEL);
-	if (!vaddr) {
-		netif_err(priv, ifup, ndev,
-			  "Could not allocate buffer descriptors!\n");
+				   (priv->total_tx_ring_size *
+				    sizeof(struct txbd8)) +
+				   (priv->total_rx_ring_size *
+				    sizeof(struct rxbd8)),
+				   &addr, GFP_KERNEL);
+	if (!vaddr)
 		return -ENOMEM;
-	}
 
 	for (i = 0; i < priv->num_tx_queues; i++) {
 		tx_queue = priv->tx_queue[i];
@@ -342,7 +341,7 @@ static void gfar_init_mac(struct net_device *ndev)
 	gfar_init_tx_rx_base(priv);
 
 	/* Configure the coalescing support */
-	gfar_configure_coalescing(priv, 0xFF, 0xFF);
+	gfar_configure_coalescing_all(priv);
 
 	/* set this when rx hw offload (TOE) functions are being used */
 	priv->uses_rxfcb = 0;
@@ -387,7 +386,7 @@ static void gfar_init_mac(struct net_device *ndev)
 		priv->uses_rxfcb = 1;
 	}
 
-	if (ndev->features & NETIF_F_HW_VLAN_RX) {
+	if (ndev->features & NETIF_F_HW_VLAN_CTAG_RX) {
 		rctrl |= RCTRL_VLEX | RCTRL_PRSDEP_INIT;
 		priv->uses_rxfcb = 1;
 	}
@@ -691,7 +690,7 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 	}
 
 	for (i = 0; i < priv->num_tx_queues; i++)
-	       priv->tx_queue[i] = NULL;
+		priv->tx_queue[i] = NULL;
 	for (i = 0; i < priv->num_rx_queues; i++)
 		priv->rx_queue[i] = NULL;
 
@@ -1051,8 +1050,9 @@ static int gfar_probe(struct platform_device *ofdev)
 	}
 
 	if (priv->device_flags & FSL_GIANFAR_DEV_HAS_VLAN) {
-		dev->hw_features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
-		dev->features |= NETIF_F_HW_VLAN_RX;
+		dev->hw_features |= NETIF_F_HW_VLAN_CTAG_TX |
+				    NETIF_F_HW_VLAN_CTAG_RX;
+		dev->features |= NETIF_F_HW_VLAN_CTAG_RX;
 	}
 
 	if (priv->device_flags & FSL_GIANFAR_DEV_HAS_EXTENDED_HASH) {
@@ -1817,25 +1817,15 @@ void gfar_start(struct net_device *dev)
 	dev->trans_start = jiffies; /* prevent tx timeout */
 }
 
-void gfar_configure_coalescing(struct gfar_private *priv,
+static void gfar_configure_coalescing(struct gfar_private *priv,
 			       unsigned long tx_mask, unsigned long rx_mask)
 {
 	struct gfar __iomem *regs = priv->gfargrp[0].regs;
 	u32 __iomem *baddr;
-	int i = 0;
-
-	/* Backward compatible case ---- even if we enable
-	 * multiple queues, there's only single reg to program
-	 */
-	gfar_write(&regs->txic, 0);
-	if (likely(priv->tx_queue[0]->txcoalescing))
-		gfar_write(&regs->txic, priv->tx_queue[0]->txic);
-
-	gfar_write(&regs->rxic, 0);
-	if (unlikely(priv->rx_queue[0]->rxcoalescing))
-		gfar_write(&regs->rxic, priv->rx_queue[0]->rxic);
 
 	if (priv->mode == MQ_MG_MODE) {
+		int i = 0;
+
 		baddr = &regs->txic0;
 		for_each_set_bit(i, &tx_mask, priv->num_tx_queues) {
 			gfar_write(baddr + i, 0);
@@ -1849,9 +1839,25 @@ void gfar_configure_coalescing(struct gfar_private *priv,
 			if (likely(priv->rx_queue[i]->rxcoalescing))
 				gfar_write(baddr + i, priv->rx_queue[i]->rxic);
 		}
+	} else {
+		/* Backward compatible case -- even if we enable
+		 * multiple queues, there's only single reg to program
+		 */
+		gfar_write(&regs->txic, 0);
+		if (likely(priv->tx_queue[0]->txcoalescing))
+			gfar_write(&regs->txic, priv->tx_queue[0]->txic);
+
+		gfar_write(&regs->rxic, 0);
+		if (unlikely(priv->rx_queue[0]->rxcoalescing))
+			gfar_write(&regs->rxic, priv->rx_queue[0]->rxic);
 	}
 }
 
+void gfar_configure_coalescing_all(struct gfar_private *priv)
+{
+	gfar_configure_coalescing(priv, 0xFF, 0xFF);
+}
+
 static int register_grp_irqs(struct gfar_priv_grp *grp)
 {
 	struct gfar_private *priv = grp->priv;
@@ -1941,7 +1947,7 @@ int startup_gfar(struct net_device *ndev)
 
 	phy_start(priv->phydev);
 
-	gfar_configure_coalescing(priv, 0xFF, 0xFF);
+	gfar_configure_coalescing_all(priv);
 
 	return 0;
 
@@ -2343,7 +2349,7 @@ void gfar_vlan_mode(struct net_device *dev, netdev_features_t features)
 	local_irq_save(flags);
 	lock_rx_qs(priv);
 
-	if (features & NETIF_F_HW_VLAN_TX) {
+	if (features & NETIF_F_HW_VLAN_CTAG_TX) {
 		/* Enable VLAN tag insertion */
 		tempval = gfar_read(&regs->tctrl);
 		tempval |= TCTRL_VLINS;
@@ -2355,7 +2361,7 @@ void gfar_vlan_mode(struct net_device *dev, netdev_features_t features)
 		gfar_write(&regs->tctrl, tempval);
 	}
 
-	if (features & NETIF_F_HW_VLAN_RX) {
+	if (features & NETIF_F_HW_VLAN_CTAG_RX) {
 		/* Enable VLAN tag extraction */
 		tempval = gfar_read(&regs->rctrl);
 		tempval |= (RCTRL_VLEX | RCTRL_PRSDEP_INIT);
@@ -2469,12 +2475,11 @@ static void gfar_align_skb(struct sk_buff *skb)
 }
 
 /* Interrupt Handler for Transmit complete */
-static int gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
+static void gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 {
 	struct net_device *dev = tx_queue->dev;
 	struct netdev_queue *txq;
 	struct gfar_private *priv = netdev_priv(dev);
-	struct gfar_priv_rx_q *rx_queue = NULL;
 	struct txbd8 *bdp, *next = NULL;
 	struct txbd8 *lbdp = NULL;
 	struct txbd8 *base = tx_queue->tx_bd_base;
@@ -2489,7 +2494,6 @@ static int gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 	u32 lstatus;
 	size_t buflen;
 
-	rx_queue = priv->rx_queue[tqi];
 	txq = netdev_get_tx_queue(dev, tqi);
 	bdp = tx_queue->dirty_tx;
 	skb_dirtytx = tx_queue->skb_dirtytx;
@@ -2571,8 +2575,6 @@ static int gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 	tx_queue->dirty_tx = bdp;
 
 	netdev_tx_completed_queue(txq, howmany, bytes_sent);
-
-	return howmany;
 }
 
 static void gfar_schedule_cleanup(struct gfar_priv_grp *gfargrp)
@@ -2694,8 +2696,6 @@ static void gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
 	struct gfar_private *priv = netdev_priv(dev);
 	struct rxfcb *fcb = NULL;
 
-	gro_result_t ret;
-
 	/* fcb is at the beginning if exists */
 	fcb = (struct rxfcb *)skb->data;
 
@@ -2725,19 +2725,17 @@ static void gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
 	/* Tell the skb what kind of packet this is */
 	skb->protocol = eth_type_trans(skb, dev);
 
-	/* There's need to check for NETIF_F_HW_VLAN_RX here.
+	/* There's need to check for NETIF_F_HW_VLAN_CTAG_RX here.
 	 * Even if vlan rx accel is disabled, on some chips
 	 * RXFCB_VLN is pseudo randomly set.
 	 */
-	if (dev->features & NETIF_F_HW_VLAN_RX &&
+	if (dev->features & NETIF_F_HW_VLAN_CTAG_RX &&
 	    fcb->flags & RXFCB_VLN)
-		__vlan_hwaccel_put_tag(skb, fcb->vlctl);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), fcb->vlctl);
 
 	/* Send the packet up the stack */
-	ret = napi_gro_receive(napi, skb);
+	napi_gro_receive(napi, skb);
 
-	if (unlikely(GRO_DROP == ret))
-		atomic64_inc(&priv->extra_stats.kernel_dropped);
 }
 
 /* gfar_clean_rx_ring() -- Processes each frame in the rx ring
@@ -2835,62 +2833,82 @@ static int gfar_poll(struct napi_struct *napi, int budget)
 	struct gfar __iomem *regs = gfargrp->regs;
 	struct gfar_priv_tx_q *tx_queue = NULL;
 	struct gfar_priv_rx_q *rx_queue = NULL;
-	int rx_cleaned = 0, budget_per_queue = 0, rx_cleaned_per_queue = 0;
-	int tx_cleaned = 0, i, left_over_budget = budget;
-	unsigned long serviced_queues = 0;
-	int num_queues = 0;
-
-	num_queues = gfargrp->num_rx_queues;
-	budget_per_queue = budget/num_queues;
+	int work_done = 0, work_done_per_q = 0;
+	int i, budget_per_q = 0;
+	int has_tx_work;
+	unsigned long rstat_rxf;
+	int num_act_queues;
 
 	/* Clear IEVENT, so interrupts aren't called again
 	 * because of the packets that have already arrived
 	 */
 	gfar_write(&regs->ievent, IEVENT_RTX_MASK);
 
-	while (num_queues && left_over_budget) {
-		budget_per_queue = left_over_budget/num_queues;
-		left_over_budget = 0;
+	rstat_rxf = gfar_read(&regs->rstat) & RSTAT_RXF_MASK;
+
+	num_act_queues = bitmap_weight(&rstat_rxf, MAX_RX_QS);
+	if (num_act_queues)
+		budget_per_q = budget/num_act_queues;
+
+	while (1) {
+		has_tx_work = 0;
+		for_each_set_bit(i, &gfargrp->tx_bit_map, priv->num_tx_queues) {
+			tx_queue = priv->tx_queue[i];
+			/* run Tx cleanup to completion */
+			if (tx_queue->tx_skbuff[tx_queue->skb_dirtytx]) {
+				gfar_clean_tx_ring(tx_queue);
+				has_tx_work = 1;
+			}
+		}
 
 		for_each_set_bit(i, &gfargrp->rx_bit_map, priv->num_rx_queues) {
-			if (test_bit(i, &serviced_queues))
+			/* skip queue if not active */
+			if (!(rstat_rxf & (RSTAT_CLEAR_RXF0 >> i)))
 				continue;
+
 			rx_queue = priv->rx_queue[i];
-			tx_queue = priv->tx_queue[rx_queue->qindex];
-
-			tx_cleaned += gfar_clean_tx_ring(tx_queue);
-			rx_cleaned_per_queue =
-				gfar_clean_rx_ring(rx_queue, budget_per_queue);
-			rx_cleaned += rx_cleaned_per_queue;
-			if (rx_cleaned_per_queue < budget_per_queue) {
-				left_over_budget = left_over_budget +
-					(budget_per_queue -
-					 rx_cleaned_per_queue);
-				set_bit(i, &serviced_queues);
-				num_queues--;
+			work_done_per_q =
+				gfar_clean_rx_ring(rx_queue, budget_per_q);
+			work_done += work_done_per_q;
+
+			/* finished processing this queue */
+			if (work_done_per_q < budget_per_q) {
+				/* clear active queue hw indication */
+				gfar_write(&regs->rstat,
+					   RSTAT_CLEAR_RXF0 >> i);
+				rstat_rxf &= ~(RSTAT_CLEAR_RXF0 >> i);
+				num_act_queues--;
+
+				if (!num_act_queues)
+					break;
+				/* recompute budget per Rx queue */
+				budget_per_q =
+					(budget - work_done) / num_act_queues;
 			}
 		}
-	}
 
-	if (tx_cleaned)
-		return budget;
+		if (work_done >= budget)
+			break;
 
-	if (rx_cleaned < budget) {
-		napi_complete(napi);
+		if (!num_act_queues && !has_tx_work) {
 
-		/* Clear the halt bit in RSTAT */
-		gfar_write(&regs->rstat, gfargrp->rstat);
+			napi_complete(napi);
 
-		gfar_write(&regs->imask, IMASK_DEFAULT);
+			/* Clear the halt bit in RSTAT */
+			gfar_write(&regs->rstat, gfargrp->rstat);
 
-		/* If we are coalescing interrupts, update the timer
-		 * Otherwise, clear it
-		 */
-		gfar_configure_coalescing(priv, gfargrp->rx_bit_map,
-					  gfargrp->tx_bit_map);
+			gfar_write(&regs->imask, IMASK_DEFAULT);
+
+			/* If we are coalescing interrupts, update the timer
+			 * Otherwise, clear it
+			 */
+			gfar_configure_coalescing(priv, gfargrp->rx_bit_map,
+						  gfargrp->tx_bit_map);
+			break;
+		}
 	}
 
-	return rx_cleaned;
+	return work_done;
 }
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
diff --git a/drivers/net/ethernet/freescale/gianfar.h b/drivers/net/ethernet/freescale/gianfar.h
index 63a28d294e20..04b552cd419d 100644
--- a/drivers/net/ethernet/freescale/gianfar.h
+++ b/drivers/net/ethernet/freescale/gianfar.h
@@ -291,7 +291,9 @@ extern const char gfar_driver_version[];
 #define RCTRL_PADDING(x)	((x << 16) & RCTRL_PAL_MASK)
 
 
-#define RSTAT_CLEAR_RHALT       0x00800000
+#define RSTAT_CLEAR_RHALT	0x00800000
+#define RSTAT_CLEAR_RXF0	0x00000080
+#define RSTAT_RXF_MASK		0x000000ff
 
 #define TCTRL_IPCSEN		0x00004000
 #define TCTRL_TUCSEN		0x00002000
@@ -627,7 +629,6 @@ struct rmon_mib
 };
 
 struct gfar_extra_stats {
-	atomic64_t kernel_dropped;
 	atomic64_t rx_large;
 	atomic64_t rx_short;
 	atomic64_t rx_nonoctet;
@@ -1180,8 +1181,7 @@ extern void stop_gfar(struct net_device *dev);
 extern void gfar_halt(struct net_device *dev);
 extern void gfar_phy_test(struct mii_bus *bus, struct phy_device *phydev,
 		int enable, u32 regnum, u32 read);
-extern void gfar_configure_coalescing(struct gfar_private *priv,
-		unsigned long tx_mask, unsigned long rx_mask);
+extern void gfar_configure_coalescing_all(struct gfar_private *priv);
 void gfar_init_sysfs(struct net_device *dev);
 int gfar_set_features(struct net_device *dev, netdev_features_t features);
 extern void gfar_check_rx_parser_mode(struct gfar_private *priv);
diff --git a/drivers/net/ethernet/freescale/gianfar_ethtool.c b/drivers/net/ethernet/freescale/gianfar_ethtool.c
index 75e89acf4912..21cd88124ca9 100644
--- a/drivers/net/ethernet/freescale/gianfar_ethtool.c
+++ b/drivers/net/ethernet/freescale/gianfar_ethtool.c
@@ -66,7 +66,6 @@ static void gfar_gdrvinfo(struct net_device *dev,
 			  struct ethtool_drvinfo *drvinfo);
 
 static const char stat_gstrings[][ETH_GSTRING_LEN] = {
-	"rx-dropped-by-kernel",
 	"rx-large-frame-errors",
 	"rx-short-frame-errors",
 	"rx-non-octet-errors",
@@ -390,14 +389,14 @@ static int gfar_scoalesce(struct net_device *dev,
 
 	/* Check the bounds of the values */
 	if (cvals->rx_coalesce_usecs > GFAR_MAX_COAL_USECS) {
-		pr_info("Coalescing is limited to %d microseconds\n",
-			GFAR_MAX_COAL_USECS);
+		netdev_info(dev, "Coalescing is limited to %d microseconds\n",
+			    GFAR_MAX_COAL_USECS);
 		return -EINVAL;
 	}
 
 	if (cvals->rx_max_coalesced_frames > GFAR_MAX_COAL_FRAMES) {
-		pr_info("Coalescing is limited to %d frames\n",
-			GFAR_MAX_COAL_FRAMES);
+		netdev_info(dev, "Coalescing is limited to %d frames\n",
+			    GFAR_MAX_COAL_FRAMES);
 		return -EINVAL;
 	}
 
@@ -419,14 +418,14 @@ static int gfar_scoalesce(struct net_device *dev,
 
 	/* Check the bounds of the values */
 	if (cvals->tx_coalesce_usecs > GFAR_MAX_COAL_USECS) {
-		pr_info("Coalescing is limited to %d microseconds\n",
-			GFAR_MAX_COAL_USECS);
+		netdev_info(dev, "Coalescing is limited to %d microseconds\n",
+			    GFAR_MAX_COAL_USECS);
 		return -EINVAL;
 	}
 
 	if (cvals->tx_max_coalesced_frames > GFAR_MAX_COAL_FRAMES) {
-		pr_info("Coalescing is limited to %d frames\n",
-			GFAR_MAX_COAL_FRAMES);
+		netdev_info(dev, "Coalescing is limited to %d frames\n",
+			    GFAR_MAX_COAL_FRAMES);
 		return -EINVAL;
 	}
 
@@ -436,7 +435,7 @@ static int gfar_scoalesce(struct net_device *dev,
 			gfar_usecs2ticks(priv, cvals->tx_coalesce_usecs));
 	}
 
-	gfar_configure_coalescing(priv, 0xFF, 0xFF);
+	gfar_configure_coalescing_all(priv);
 
 	return 0;
 }
@@ -543,7 +542,7 @@ int gfar_set_features(struct net_device *dev, netdev_features_t features)
 	int err = 0, i = 0;
 	netdev_features_t changed = dev->features ^ features;
 
-	if (changed & (NETIF_F_HW_VLAN_TX|NETIF_F_HW_VLAN_RX))
+	if (changed & (NETIF_F_HW_VLAN_CTAG_TX|NETIF_F_HW_VLAN_CTAG_RX))
 		gfar_vlan_mode(dev, features);
 
 	if (!(changed & NETIF_F_RXCSUM))
@@ -736,7 +735,8 @@ static int gfar_ethflow_to_filer_table(struct gfar_private *priv, u64 ethflow,
 		cmp_rqfpr = RQFPR_IPV6 |RQFPR_UDP;
 		break;
 	default:
-		pr_err("Right now this class is not supported\n");
+		netdev_err(priv->ndev,
+			   "Right now this class is not supported\n");
 		ret = 0;
 		goto err;
 	}
@@ -752,7 +752,8 @@ static int gfar_ethflow_to_filer_table(struct gfar_private *priv, u64 ethflow,
 	}
 
 	if (i == MAX_FILER_IDX + 1) {
-		pr_err("No parse rule found, can't create hash rules\n");
+		netdev_err(priv->ndev,
+			   "No parse rule found, can't create hash rules\n");
 		ret = 0;
 		goto err;
 	}
@@ -1569,7 +1570,7 @@ static int gfar_process_filer_changes(struct gfar_private *priv)
 	gfar_cluster_filer(tab);
 	gfar_optimize_filer_masks(tab);
 
-	pr_debug("\n\tSummary:\n"
+	pr_debug("\tSummary:\n"
 		 "\tData on hardware: %d\n"
 		 "\tCompression rate: %d%%\n",
 		 tab->index, 100 - (100 * tab->index) / i);
diff --git a/drivers/net/ethernet/freescale/gianfar_ptp.c b/drivers/net/ethernet/freescale/gianfar_ptp.c
index 2e5daee0438a..576e4b858fce 100644
--- a/drivers/net/ethernet/freescale/gianfar_ptp.c
+++ b/drivers/net/ethernet/freescale/gianfar_ptp.c
@@ -17,6 +17,9 @@
  *  along with this program; if not, write to the Free Software
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/device.h>
 #include <linux/hrtimer.h>
 #include <linux/init.h>
@@ -127,7 +130,6 @@ struct gianfar_ptp_registers {
 
 #define DRIVER		"gianfar_ptp"
 #define DEFAULT_CKSEL	1
-#define N_ALARM		1 /* first alarm is used internally to reset fipers */
 #define N_EXT_TS	2
 #define REG_SIZE	sizeof(struct gianfar_ptp_registers)
 
@@ -410,7 +412,7 @@ static struct ptp_clock_info ptp_gianfar_caps = {
 	.owner		= THIS_MODULE,
 	.name		= "gianfar clock",
 	.max_adj	= 512000,
-	.n_alarm	= N_ALARM,
+	.n_alarm	= 0,
 	.n_ext_ts	= N_EXT_TS,
 	.n_per_out	= 0,
 	.pps		= 1,
diff --git a/drivers/net/ethernet/freescale/gianfar_sysfs.c b/drivers/net/ethernet/freescale/gianfar_sysfs.c
index cd14a4d449c2..acb55af7e3f3 100644
--- a/drivers/net/ethernet/freescale/gianfar_sysfs.c
+++ b/drivers/net/ethernet/freescale/gianfar_sysfs.c
@@ -337,5 +337,5 @@ void gfar_init_sysfs(struct net_device *dev)
 	rc |= device_create_file(&dev->dev, &dev_attr_fifo_starve);
 	rc |= device_create_file(&dev->dev, &dev_attr_fifo_starve_off);
 	if (rc)
-		dev_err(&dev->dev, "Error creating gianfar sysfs files.\n");
+		dev_err(&dev->dev, "Error creating gianfar sysfs files\n");
 }
diff --git a/drivers/net/ethernet/freescale/ucc_geth.c b/drivers/net/ethernet/freescale/ucc_geth.c
index 0a70bb55d1b0..e04c59818f60 100644
--- a/drivers/net/ethernet/freescale/ucc_geth.c
+++ b/drivers/net/ethernet/freescale/ucc_geth.c
@@ -12,6 +12,9 @@
  * Free Software Foundation;  either version 2 of the  License, or (at your
  * option) any later version.
  */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/errno.h>
@@ -50,12 +53,6 @@
 
 #define ugeth_dbg(format, arg...)            \
         ugeth_printk(KERN_DEBUG , format , ## arg)
-#define ugeth_err(format, arg...)            \
-        ugeth_printk(KERN_ERR , format , ## arg)
-#define ugeth_info(format, arg...)           \
-        ugeth_printk(KERN_INFO , format , ## arg)
-#define ugeth_warn(format, arg...)           \
-        ugeth_printk(KERN_WARNING , format , ## arg)
 
 #ifdef UGETH_VERBOSE_DEBUG
 #define ugeth_vdbg ugeth_dbg
@@ -281,7 +278,7 @@ static int fill_init_enet_entries(struct ucc_geth_private *ugeth,
 	for (i = 0; i < num_entries; i++) {
 		if ((snum = qe_get_snum()) < 0) {
 			if (netif_msg_ifup(ugeth))
-				ugeth_err("fill_init_enet_entries: Can not get SNUM.");
+				pr_err("Can not get SNUM\n");
 			return snum;
 		}
 		if ((i == 0) && skip_page_for_first_entry)
@@ -292,7 +289,7 @@ static int fill_init_enet_entries(struct ucc_geth_private *ugeth,
 			    qe_muram_alloc(thread_size, thread_alignment);
 			if (IS_ERR_VALUE(init_enet_offset)) {
 				if (netif_msg_ifup(ugeth))
-					ugeth_err("fill_init_enet_entries: Can not allocate DPRAM memory.");
+					pr_err("Can not allocate DPRAM memory\n");
 				qe_put_snum((u8) snum);
 				return -ENOMEM;
 			}
@@ -365,10 +362,9 @@ static int dump_init_enet_entries(struct ucc_geth_private *ugeth,
 				init_enet_offset =
 				    (in_be32(p_start) &
 				     ENET_INIT_PARAM_PTR_MASK);
-				ugeth_info("Init enet entry %d:", i);
-				ugeth_info("Base address: 0x%08x",
-					   (u32)
-					   qe_muram_addr(init_enet_offset));
+				pr_info("Init enet entry %d:\n", i);
+				pr_info("Base address: 0x%08x\n",
+					(u32)qe_muram_addr(init_enet_offset));
 				mem_disp(qe_muram_addr(init_enet_offset),
 					 thread_size);
 			}
@@ -396,8 +392,8 @@ static int hw_clear_addr_in_paddr(struct ucc_geth_private *ugeth, u8 paddr_num)
 {
 	struct ucc_geth_82xx_address_filtering_pram __iomem *p_82xx_addr_filt;
 
-	if (!(paddr_num < NUM_OF_PADDRS)) {
-		ugeth_warn("%s: Illagel paddr_num.", __func__);
+	if (paddr_num >= NUM_OF_PADDRS) {
+		pr_warn("%s: Invalid paddr_num: %u\n", __func__, paddr_num);
 		return -EINVAL;
 	}
 
@@ -573,7 +569,7 @@ static void dump_bds(struct ucc_geth_private *ugeth)
 			length =
 			    (ugeth->ug_info->bdRingLenTx[i] *
 			     sizeof(struct qe_bd));
-			ugeth_info("TX BDs[%d]", i);
+			pr_info("TX BDs[%d]\n", i);
 			mem_disp(ugeth->p_tx_bd_ring[i], length);
 		}
 	}
@@ -582,7 +578,7 @@ static void dump_bds(struct ucc_geth_private *ugeth)
 			length =
 			    (ugeth->ug_info->bdRingLenRx[i] *
 			     sizeof(struct qe_bd));
-			ugeth_info("RX BDs[%d]", i);
+			pr_info("RX BDs[%d]\n", i);
 			mem_disp(ugeth->p_rx_bd_ring[i], length);
 		}
 	}
@@ -592,93 +588,93 @@ static void dump_regs(struct ucc_geth_private *ugeth)
 {
 	int i;
 
-	ugeth_info("UCC%d Geth registers:", ugeth->ug_info->uf_info.ucc_num + 1);
-	ugeth_info("Base address: 0x%08x", (u32) ugeth->ug_regs);
-
-	ugeth_info("maccfg1    : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->maccfg1,
-		   in_be32(&ugeth->ug_regs->maccfg1));
-	ugeth_info("maccfg2    : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->maccfg2,
-		   in_be32(&ugeth->ug_regs->maccfg2));
-	ugeth_info("ipgifg     : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->ipgifg,
-		   in_be32(&ugeth->ug_regs->ipgifg));
-	ugeth_info("hafdup     : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->hafdup,
-		   in_be32(&ugeth->ug_regs->hafdup));
-	ugeth_info("ifctl      : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->ifctl,
-		   in_be32(&ugeth->ug_regs->ifctl));
-	ugeth_info("ifstat     : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->ifstat,
-		   in_be32(&ugeth->ug_regs->ifstat));
-	ugeth_info("macstnaddr1: addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->macstnaddr1,
-		   in_be32(&ugeth->ug_regs->macstnaddr1));
-	ugeth_info("macstnaddr2: addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->macstnaddr2,
-		   in_be32(&ugeth->ug_regs->macstnaddr2));
-	ugeth_info("uempr      : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->uempr,
-		   in_be32(&ugeth->ug_regs->uempr));
-	ugeth_info("utbipar    : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->utbipar,
-		   in_be32(&ugeth->ug_regs->utbipar));
-	ugeth_info("uescr      : addr - 0x%08x, val - 0x%04x",
-		   (u32) & ugeth->ug_regs->uescr,
-		   in_be16(&ugeth->ug_regs->uescr));
-	ugeth_info("tx64       : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->tx64,
-		   in_be32(&ugeth->ug_regs->tx64));
-	ugeth_info("tx127      : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->tx127,
-		   in_be32(&ugeth->ug_regs->tx127));
-	ugeth_info("tx255      : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->tx255,
-		   in_be32(&ugeth->ug_regs->tx255));
-	ugeth_info("rx64       : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->rx64,
-		   in_be32(&ugeth->ug_regs->rx64));
-	ugeth_info("rx127      : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->rx127,
-		   in_be32(&ugeth->ug_regs->rx127));
-	ugeth_info("rx255      : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->rx255,
-		   in_be32(&ugeth->ug_regs->rx255));
-	ugeth_info("txok       : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->txok,
-		   in_be32(&ugeth->ug_regs->txok));
-	ugeth_info("txcf       : addr - 0x%08x, val - 0x%04x",
-		   (u32) & ugeth->ug_regs->txcf,
-		   in_be16(&ugeth->ug_regs->txcf));
-	ugeth_info("tmca       : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->tmca,
-		   in_be32(&ugeth->ug_regs->tmca));
-	ugeth_info("tbca       : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->tbca,
-		   in_be32(&ugeth->ug_regs->tbca));
-	ugeth_info("rxfok      : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->rxfok,
-		   in_be32(&ugeth->ug_regs->rxfok));
-	ugeth_info("rxbok      : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->rxbok,
-		   in_be32(&ugeth->ug_regs->rxbok));
-	ugeth_info("rbyt       : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->rbyt,
-		   in_be32(&ugeth->ug_regs->rbyt));
-	ugeth_info("rmca       : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->rmca,
-		   in_be32(&ugeth->ug_regs->rmca));
-	ugeth_info("rbca       : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->rbca,
-		   in_be32(&ugeth->ug_regs->rbca));
-	ugeth_info("scar       : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->scar,
-		   in_be32(&ugeth->ug_regs->scar));
-	ugeth_info("scam       : addr - 0x%08x, val - 0x%08x",
-		   (u32) & ugeth->ug_regs->scam,
-		   in_be32(&ugeth->ug_regs->scam));
+	pr_info("UCC%d Geth registers:\n", ugeth->ug_info->uf_info.ucc_num + 1);
+	pr_info("Base address: 0x%08x\n", (u32)ugeth->ug_regs);
+
+	pr_info("maccfg1    : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->maccfg1,
+		in_be32(&ugeth->ug_regs->maccfg1));
+	pr_info("maccfg2    : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->maccfg2,
+		in_be32(&ugeth->ug_regs->maccfg2));
+	pr_info("ipgifg     : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->ipgifg,
+		in_be32(&ugeth->ug_regs->ipgifg));
+	pr_info("hafdup     : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->hafdup,
+		in_be32(&ugeth->ug_regs->hafdup));
+	pr_info("ifctl      : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->ifctl,
+		in_be32(&ugeth->ug_regs->ifctl));
+	pr_info("ifstat     : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->ifstat,
+		in_be32(&ugeth->ug_regs->ifstat));
+	pr_info("macstnaddr1: addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->macstnaddr1,
+		in_be32(&ugeth->ug_regs->macstnaddr1));
+	pr_info("macstnaddr2: addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->macstnaddr2,
+		in_be32(&ugeth->ug_regs->macstnaddr2));
+	pr_info("uempr      : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->uempr,
+		in_be32(&ugeth->ug_regs->uempr));
+	pr_info("utbipar    : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->utbipar,
+		in_be32(&ugeth->ug_regs->utbipar));
+	pr_info("uescr      : addr - 0x%08x, val - 0x%04x\n",
+		(u32)&ugeth->ug_regs->uescr,
+		in_be16(&ugeth->ug_regs->uescr));
+	pr_info("tx64       : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->tx64,
+		in_be32(&ugeth->ug_regs->tx64));
+	pr_info("tx127      : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->tx127,
+		in_be32(&ugeth->ug_regs->tx127));
+	pr_info("tx255      : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->tx255,
+		in_be32(&ugeth->ug_regs->tx255));
+	pr_info("rx64       : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->rx64,
+		in_be32(&ugeth->ug_regs->rx64));
+	pr_info("rx127      : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->rx127,
+		in_be32(&ugeth->ug_regs->rx127));
+	pr_info("rx255      : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->rx255,
+		in_be32(&ugeth->ug_regs->rx255));
+	pr_info("txok       : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->txok,
+		in_be32(&ugeth->ug_regs->txok));
+	pr_info("txcf       : addr - 0x%08x, val - 0x%04x\n",
+		(u32)&ugeth->ug_regs->txcf,
+		in_be16(&ugeth->ug_regs->txcf));
+	pr_info("tmca       : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->tmca,
+		in_be32(&ugeth->ug_regs->tmca));
+	pr_info("tbca       : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->tbca,
+		in_be32(&ugeth->ug_regs->tbca));
+	pr_info("rxfok      : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->rxfok,
+		in_be32(&ugeth->ug_regs->rxfok));
+	pr_info("rxbok      : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->rxbok,
+		in_be32(&ugeth->ug_regs->rxbok));
+	pr_info("rbyt       : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->rbyt,
+		in_be32(&ugeth->ug_regs->rbyt));
+	pr_info("rmca       : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->rmca,
+		in_be32(&ugeth->ug_regs->rmca));
+	pr_info("rbca       : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->rbca,
+		in_be32(&ugeth->ug_regs->rbca));
+	pr_info("scar       : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->scar,
+		in_be32(&ugeth->ug_regs->scar));
+	pr_info("scam       : addr - 0x%08x, val - 0x%08x\n",
+		(u32)&ugeth->ug_regs->scam,
+		in_be32(&ugeth->ug_regs->scam));
 
 	if (ugeth->p_thread_data_tx) {
 		int numThreadsTxNumerical;
@@ -703,13 +699,13 @@ static void dump_regs(struct ucc_geth_private *ugeth)
 			break;
 		}
 
-		ugeth_info("Thread data TXs:");
-		ugeth_info("Base address: 0x%08x",
-			   (u32) ugeth->p_thread_data_tx);
+		pr_info("Thread data TXs:\n");
+		pr_info("Base address: 0x%08x\n",
+			(u32)ugeth->p_thread_data_tx);
 		for (i = 0; i < numThreadsTxNumerical; i++) {
-			ugeth_info("Thread data TX[%d]:", i);
-			ugeth_info("Base address: 0x%08x",
-				   (u32) & ugeth->p_thread_data_tx[i]);
+			pr_info("Thread data TX[%d]:\n", i);
+			pr_info("Base address: 0x%08x\n",
+				(u32)&ugeth->p_thread_data_tx[i]);
 			mem_disp((u8 *) & ugeth->p_thread_data_tx[i],
 				 sizeof(struct ucc_geth_thread_data_tx));
 		}
@@ -737,270 +733,260 @@ static void dump_regs(struct ucc_geth_private *ugeth)
 			break;
 		}
 
-		ugeth_info("Thread data RX:");
-		ugeth_info("Base address: 0x%08x",
-			   (u32) ugeth->p_thread_data_rx);
+		pr_info("Thread data RX:\n");
+		pr_info("Base address: 0x%08x\n",
+			(u32)ugeth->p_thread_data_rx);
 		for (i = 0; i < numThreadsRxNumerical; i++) {
-			ugeth_info("Thread data RX[%d]:", i);
-			ugeth_info("Base address: 0x%08x",
-				   (u32) & ugeth->p_thread_data_rx[i]);
+			pr_info("Thread data RX[%d]:\n", i);
+			pr_info("Base address: 0x%08x\n",
+				(u32)&ugeth->p_thread_data_rx[i]);
 			mem_disp((u8 *) & ugeth->p_thread_data_rx[i],
 				 sizeof(struct ucc_geth_thread_data_rx));
 		}
 	}
 	if (ugeth->p_exf_glbl_param) {
-		ugeth_info("EXF global param:");
-		ugeth_info("Base address: 0x%08x",
-			   (u32) ugeth->p_exf_glbl_param);
+		pr_info("EXF global param:\n");
+		pr_info("Base address: 0x%08x\n",
+			(u32)ugeth->p_exf_glbl_param);
 		mem_disp((u8 *) ugeth->p_exf_glbl_param,
 			 sizeof(*ugeth->p_exf_glbl_param));
 	}
 	if (ugeth->p_tx_glbl_pram) {
-		ugeth_info("TX global param:");
-		ugeth_info("Base address: 0x%08x", (u32) ugeth->p_tx_glbl_pram);
-		ugeth_info("temoder      : addr - 0x%08x, val - 0x%04x",
-			   (u32) & ugeth->p_tx_glbl_pram->temoder,
-			   in_be16(&ugeth->p_tx_glbl_pram->temoder));
-		ugeth_info("sqptr        : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_tx_glbl_pram->sqptr,
-			   in_be32(&ugeth->p_tx_glbl_pram->sqptr));
-		ugeth_info("schedulerbasepointer: addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_tx_glbl_pram->schedulerbasepointer,
-			   in_be32(&ugeth->p_tx_glbl_pram->
-				   schedulerbasepointer));
-		ugeth_info("txrmonbaseptr: addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_tx_glbl_pram->txrmonbaseptr,
-			   in_be32(&ugeth->p_tx_glbl_pram->txrmonbaseptr));
-		ugeth_info("tstate       : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_tx_glbl_pram->tstate,
-			   in_be32(&ugeth->p_tx_glbl_pram->tstate));
-		ugeth_info("iphoffset[0] : addr - 0x%08x, val - 0x%02x",
-			   (u32) & ugeth->p_tx_glbl_pram->iphoffset[0],
-			   ugeth->p_tx_glbl_pram->iphoffset[0]);
-		ugeth_info("iphoffset[1] : addr - 0x%08x, val - 0x%02x",
-			   (u32) & ugeth->p_tx_glbl_pram->iphoffset[1],
-			   ugeth->p_tx_glbl_pram->iphoffset[1]);
-		ugeth_info("iphoffset[2] : addr - 0x%08x, val - 0x%02x",
-			   (u32) & ugeth->p_tx_glbl_pram->iphoffset[2],
-			   ugeth->p_tx_glbl_pram->iphoffset[2]);
-		ugeth_info("iphoffset[3] : addr - 0x%08x, val - 0x%02x",
-			   (u32) & ugeth->p_tx_glbl_pram->iphoffset[3],
-			   ugeth->p_tx_glbl_pram->iphoffset[3]);
-		ugeth_info("iphoffset[4] : addr - 0x%08x, val - 0x%02x",
-			   (u32) & ugeth->p_tx_glbl_pram->iphoffset[4],
-			   ugeth->p_tx_glbl_pram->iphoffset[4]);
-		ugeth_info("iphoffset[5] : addr - 0x%08x, val - 0x%02x",
-			   (u32) & ugeth->p_tx_glbl_pram->iphoffset[5],
-			   ugeth->p_tx_glbl_pram->iphoffset[5]);
-		ugeth_info("iphoffset[6] : addr - 0x%08x, val - 0x%02x",
-			   (u32) & ugeth->p_tx_glbl_pram->iphoffset[6],
-			   ugeth->p_tx_glbl_pram->iphoffset[6]);
-		ugeth_info("iphoffset[7] : addr - 0x%08x, val - 0x%02x",
-			   (u32) & ugeth->p_tx_glbl_pram->iphoffset[7],
-			   ugeth->p_tx_glbl_pram->iphoffset[7]);
-		ugeth_info("vtagtable[0] : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_tx_glbl_pram->vtagtable[0],
-			   in_be32(&ugeth->p_tx_glbl_pram->vtagtable[0]));
-		ugeth_info("vtagtable[1] : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_tx_glbl_pram->vtagtable[1],
-			   in_be32(&ugeth->p_tx_glbl_pram->vtagtable[1]));
-		ugeth_info("vtagtable[2] : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_tx_glbl_pram->vtagtable[2],
-			   in_be32(&ugeth->p_tx_glbl_pram->vtagtable[2]));
-		ugeth_info("vtagtable[3] : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_tx_glbl_pram->vtagtable[3],
-			   in_be32(&ugeth->p_tx_glbl_pram->vtagtable[3]));
-		ugeth_info("vtagtable[4] : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_tx_glbl_pram->vtagtable[4],
-			   in_be32(&ugeth->p_tx_glbl_pram->vtagtable[4]));
-		ugeth_info("vtagtable[5] : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_tx_glbl_pram->vtagtable[5],
-			   in_be32(&ugeth->p_tx_glbl_pram->vtagtable[5]));
-		ugeth_info("vtagtable[6] : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_tx_glbl_pram->vtagtable[6],
-			   in_be32(&ugeth->p_tx_glbl_pram->vtagtable[6]));
-		ugeth_info("vtagtable[7] : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_tx_glbl_pram->vtagtable[7],
-			   in_be32(&ugeth->p_tx_glbl_pram->vtagtable[7]));
-		ugeth_info("tqptr        : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_tx_glbl_pram->tqptr,
-			   in_be32(&ugeth->p_tx_glbl_pram->tqptr));
+		pr_info("TX global param:\n");
+		pr_info("Base address: 0x%08x\n", (u32)ugeth->p_tx_glbl_pram);
+		pr_info("temoder      : addr - 0x%08x, val - 0x%04x\n",
+			(u32)&ugeth->p_tx_glbl_pram->temoder,
+			in_be16(&ugeth->p_tx_glbl_pram->temoder));
+	       pr_info("sqptr        : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_tx_glbl_pram->sqptr,
+			in_be32(&ugeth->p_tx_glbl_pram->sqptr));
+		pr_info("schedulerbasepointer: addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_tx_glbl_pram->schedulerbasepointer,
+			in_be32(&ugeth->p_tx_glbl_pram->schedulerbasepointer));
+		pr_info("txrmonbaseptr: addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_tx_glbl_pram->txrmonbaseptr,
+			in_be32(&ugeth->p_tx_glbl_pram->txrmonbaseptr));
+		pr_info("tstate       : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_tx_glbl_pram->tstate,
+			in_be32(&ugeth->p_tx_glbl_pram->tstate));
+		pr_info("iphoffset[0] : addr - 0x%08x, val - 0x%02x\n",
+			(u32)&ugeth->p_tx_glbl_pram->iphoffset[0],
+			ugeth->p_tx_glbl_pram->iphoffset[0]);
+		pr_info("iphoffset[1] : addr - 0x%08x, val - 0x%02x\n",
+			(u32)&ugeth->p_tx_glbl_pram->iphoffset[1],
+			ugeth->p_tx_glbl_pram->iphoffset[1]);
+		pr_info("iphoffset[2] : addr - 0x%08x, val - 0x%02x\n",
+			(u32)&ugeth->p_tx_glbl_pram->iphoffset[2],
+			ugeth->p_tx_glbl_pram->iphoffset[2]);
+		pr_info("iphoffset[3] : addr - 0x%08x, val - 0x%02x\n",
+			(u32)&ugeth->p_tx_glbl_pram->iphoffset[3],
+			ugeth->p_tx_glbl_pram->iphoffset[3]);
+		pr_info("iphoffset[4] : addr - 0x%08x, val - 0x%02x\n",
+			(u32)&ugeth->p_tx_glbl_pram->iphoffset[4],
+			ugeth->p_tx_glbl_pram->iphoffset[4]);
+		pr_info("iphoffset[5] : addr - 0x%08x, val - 0x%02x\n",
+			(u32)&ugeth->p_tx_glbl_pram->iphoffset[5],
+			ugeth->p_tx_glbl_pram->iphoffset[5]);
+		pr_info("iphoffset[6] : addr - 0x%08x, val - 0x%02x\n",
+			(u32)&ugeth->p_tx_glbl_pram->iphoffset[6],
+			ugeth->p_tx_glbl_pram->iphoffset[6]);
+		pr_info("iphoffset[7] : addr - 0x%08x, val - 0x%02x\n",
+			(u32)&ugeth->p_tx_glbl_pram->iphoffset[7],
+			ugeth->p_tx_glbl_pram->iphoffset[7]);
+		pr_info("vtagtable[0] : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_tx_glbl_pram->vtagtable[0],
+			in_be32(&ugeth->p_tx_glbl_pram->vtagtable[0]));
+		pr_info("vtagtable[1] : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_tx_glbl_pram->vtagtable[1],
+			in_be32(&ugeth->p_tx_glbl_pram->vtagtable[1]));
+		pr_info("vtagtable[2] : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_tx_glbl_pram->vtagtable[2],
+			in_be32(&ugeth->p_tx_glbl_pram->vtagtable[2]));
+		pr_info("vtagtable[3] : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_tx_glbl_pram->vtagtable[3],
+			in_be32(&ugeth->p_tx_glbl_pram->vtagtable[3]));
+		pr_info("vtagtable[4] : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_tx_glbl_pram->vtagtable[4],
+			in_be32(&ugeth->p_tx_glbl_pram->vtagtable[4]));
+		pr_info("vtagtable[5] : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_tx_glbl_pram->vtagtable[5],
+			in_be32(&ugeth->p_tx_glbl_pram->vtagtable[5]));
+		pr_info("vtagtable[6] : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_tx_glbl_pram->vtagtable[6],
+			in_be32(&ugeth->p_tx_glbl_pram->vtagtable[6]));
+		pr_info("vtagtable[7] : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_tx_glbl_pram->vtagtable[7],
+			in_be32(&ugeth->p_tx_glbl_pram->vtagtable[7]));
+		pr_info("tqptr        : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_tx_glbl_pram->tqptr,
+			in_be32(&ugeth->p_tx_glbl_pram->tqptr));
 	}
 	if (ugeth->p_rx_glbl_pram) {
-		ugeth_info("RX global param:");
-		ugeth_info("Base address: 0x%08x", (u32) ugeth->p_rx_glbl_pram);
-		ugeth_info("remoder         : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->remoder,
-			   in_be32(&ugeth->p_rx_glbl_pram->remoder));
-		ugeth_info("rqptr           : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->rqptr,
-			   in_be32(&ugeth->p_rx_glbl_pram->rqptr));
-		ugeth_info("typeorlen       : addr - 0x%08x, val - 0x%04x",
-			   (u32) & ugeth->p_rx_glbl_pram->typeorlen,
-			   in_be16(&ugeth->p_rx_glbl_pram->typeorlen));
-		ugeth_info("rxgstpack       : addr - 0x%08x, val - 0x%02x",
-			   (u32) & ugeth->p_rx_glbl_pram->rxgstpack,
-			   ugeth->p_rx_glbl_pram->rxgstpack);
-		ugeth_info("rxrmonbaseptr   : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->rxrmonbaseptr,
-			   in_be32(&ugeth->p_rx_glbl_pram->rxrmonbaseptr));
-		ugeth_info("intcoalescingptr: addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->intcoalescingptr,
-			   in_be32(&ugeth->p_rx_glbl_pram->intcoalescingptr));
-		ugeth_info("rstate          : addr - 0x%08x, val - 0x%02x",
-			   (u32) & ugeth->p_rx_glbl_pram->rstate,
-			   ugeth->p_rx_glbl_pram->rstate);
-		ugeth_info("mrblr           : addr - 0x%08x, val - 0x%04x",
-			   (u32) & ugeth->p_rx_glbl_pram->mrblr,
-			   in_be16(&ugeth->p_rx_glbl_pram->mrblr));
-		ugeth_info("rbdqptr         : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->rbdqptr,
-			   in_be32(&ugeth->p_rx_glbl_pram->rbdqptr));
-		ugeth_info("mflr            : addr - 0x%08x, val - 0x%04x",
-			   (u32) & ugeth->p_rx_glbl_pram->mflr,
-			   in_be16(&ugeth->p_rx_glbl_pram->mflr));
-		ugeth_info("minflr          : addr - 0x%08x, val - 0x%04x",
-			   (u32) & ugeth->p_rx_glbl_pram->minflr,
-			   in_be16(&ugeth->p_rx_glbl_pram->minflr));
-		ugeth_info("maxd1           : addr - 0x%08x, val - 0x%04x",
-			   (u32) & ugeth->p_rx_glbl_pram->maxd1,
-			   in_be16(&ugeth->p_rx_glbl_pram->maxd1));
-		ugeth_info("maxd2           : addr - 0x%08x, val - 0x%04x",
-			   (u32) & ugeth->p_rx_glbl_pram->maxd2,
-			   in_be16(&ugeth->p_rx_glbl_pram->maxd2));
-		ugeth_info("ecamptr         : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->ecamptr,
-			   in_be32(&ugeth->p_rx_glbl_pram->ecamptr));
-		ugeth_info("l2qt            : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->l2qt,
-			   in_be32(&ugeth->p_rx_glbl_pram->l2qt));
-		ugeth_info("l3qt[0]         : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->l3qt[0],
-			   in_be32(&ugeth->p_rx_glbl_pram->l3qt[0]));
-		ugeth_info("l3qt[1]         : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->l3qt[1],
-			   in_be32(&ugeth->p_rx_glbl_pram->l3qt[1]));
-		ugeth_info("l3qt[2]         : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->l3qt[2],
-			   in_be32(&ugeth->p_rx_glbl_pram->l3qt[2]));
-		ugeth_info("l3qt[3]         : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->l3qt[3],
-			   in_be32(&ugeth->p_rx_glbl_pram->l3qt[3]));
-		ugeth_info("l3qt[4]         : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->l3qt[4],
-			   in_be32(&ugeth->p_rx_glbl_pram->l3qt[4]));
-		ugeth_info("l3qt[5]         : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->l3qt[5],
-			   in_be32(&ugeth->p_rx_glbl_pram->l3qt[5]));
-		ugeth_info("l3qt[6]         : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->l3qt[6],
-			   in_be32(&ugeth->p_rx_glbl_pram->l3qt[6]));
-		ugeth_info("l3qt[7]         : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->l3qt[7],
-			   in_be32(&ugeth->p_rx_glbl_pram->l3qt[7]));
-		ugeth_info("vlantype        : addr - 0x%08x, val - 0x%04x",
-			   (u32) & ugeth->p_rx_glbl_pram->vlantype,
-			   in_be16(&ugeth->p_rx_glbl_pram->vlantype));
-		ugeth_info("vlantci         : addr - 0x%08x, val - 0x%04x",
-			   (u32) & ugeth->p_rx_glbl_pram->vlantci,
-			   in_be16(&ugeth->p_rx_glbl_pram->vlantci));
+		pr_info("RX global param:\n");
+		pr_info("Base address: 0x%08x\n", (u32)ugeth->p_rx_glbl_pram);
+		pr_info("remoder         : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->remoder,
+			in_be32(&ugeth->p_rx_glbl_pram->remoder));
+		pr_info("rqptr           : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->rqptr,
+			in_be32(&ugeth->p_rx_glbl_pram->rqptr));
+		pr_info("typeorlen       : addr - 0x%08x, val - 0x%04x\n",
+			(u32)&ugeth->p_rx_glbl_pram->typeorlen,
+			in_be16(&ugeth->p_rx_glbl_pram->typeorlen));
+		pr_info("rxgstpack       : addr - 0x%08x, val - 0x%02x\n",
+			(u32)&ugeth->p_rx_glbl_pram->rxgstpack,
+			ugeth->p_rx_glbl_pram->rxgstpack);
+		pr_info("rxrmonbaseptr   : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->rxrmonbaseptr,
+			in_be32(&ugeth->p_rx_glbl_pram->rxrmonbaseptr));
+		pr_info("intcoalescingptr: addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->intcoalescingptr,
+			in_be32(&ugeth->p_rx_glbl_pram->intcoalescingptr));
+		pr_info("rstate          : addr - 0x%08x, val - 0x%02x\n",
+			(u32)&ugeth->p_rx_glbl_pram->rstate,
+			ugeth->p_rx_glbl_pram->rstate);
+		pr_info("mrblr           : addr - 0x%08x, val - 0x%04x\n",
+			(u32)&ugeth->p_rx_glbl_pram->mrblr,
+			in_be16(&ugeth->p_rx_glbl_pram->mrblr));
+		pr_info("rbdqptr         : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->rbdqptr,
+			in_be32(&ugeth->p_rx_glbl_pram->rbdqptr));
+		pr_info("mflr            : addr - 0x%08x, val - 0x%04x\n",
+			(u32)&ugeth->p_rx_glbl_pram->mflr,
+			in_be16(&ugeth->p_rx_glbl_pram->mflr));
+		pr_info("minflr          : addr - 0x%08x, val - 0x%04x\n",
+			(u32)&ugeth->p_rx_glbl_pram->minflr,
+			in_be16(&ugeth->p_rx_glbl_pram->minflr));
+		pr_info("maxd1           : addr - 0x%08x, val - 0x%04x\n",
+			(u32)&ugeth->p_rx_glbl_pram->maxd1,
+			in_be16(&ugeth->p_rx_glbl_pram->maxd1));
+		pr_info("maxd2           : addr - 0x%08x, val - 0x%04x\n",
+			(u32)&ugeth->p_rx_glbl_pram->maxd2,
+			in_be16(&ugeth->p_rx_glbl_pram->maxd2));
+		pr_info("ecamptr         : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->ecamptr,
+			in_be32(&ugeth->p_rx_glbl_pram->ecamptr));
+		pr_info("l2qt            : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->l2qt,
+			in_be32(&ugeth->p_rx_glbl_pram->l2qt));
+		pr_info("l3qt[0]         : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->l3qt[0],
+			in_be32(&ugeth->p_rx_glbl_pram->l3qt[0]));
+		pr_info("l3qt[1]         : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->l3qt[1],
+			in_be32(&ugeth->p_rx_glbl_pram->l3qt[1]));
+		pr_info("l3qt[2]         : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->l3qt[2],
+			in_be32(&ugeth->p_rx_glbl_pram->l3qt[2]));
+		pr_info("l3qt[3]         : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->l3qt[3],
+			in_be32(&ugeth->p_rx_glbl_pram->l3qt[3]));
+		pr_info("l3qt[4]         : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->l3qt[4],
+			in_be32(&ugeth->p_rx_glbl_pram->l3qt[4]));
+		pr_info("l3qt[5]         : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->l3qt[5],
+			in_be32(&ugeth->p_rx_glbl_pram->l3qt[5]));
+		pr_info("l3qt[6]         : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->l3qt[6],
+			in_be32(&ugeth->p_rx_glbl_pram->l3qt[6]));
+		pr_info("l3qt[7]         : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->l3qt[7],
+			in_be32(&ugeth->p_rx_glbl_pram->l3qt[7]));
+		pr_info("vlantype        : addr - 0x%08x, val - 0x%04x\n",
+			(u32)&ugeth->p_rx_glbl_pram->vlantype,
+			in_be16(&ugeth->p_rx_glbl_pram->vlantype));
+		pr_info("vlantci         : addr - 0x%08x, val - 0x%04x\n",
+			(u32)&ugeth->p_rx_glbl_pram->vlantci,
+			in_be16(&ugeth->p_rx_glbl_pram->vlantci));
 		for (i = 0; i < 64; i++)
-			ugeth_info
-		    ("addressfiltering[%d]: addr - 0x%08x, val - 0x%02x",
-			     i,
-			     (u32) & ugeth->p_rx_glbl_pram->addressfiltering[i],
-			     ugeth->p_rx_glbl_pram->addressfiltering[i]);
-		ugeth_info("exfGlobalParam  : addr - 0x%08x, val - 0x%08x",
-			   (u32) & ugeth->p_rx_glbl_pram->exfGlobalParam,
-			   in_be32(&ugeth->p_rx_glbl_pram->exfGlobalParam));
+			pr_info("addressfiltering[%d]: addr - 0x%08x, val - 0x%02x\n",
+				i,
+				(u32)&ugeth->p_rx_glbl_pram->addressfiltering[i],
+				ugeth->p_rx_glbl_pram->addressfiltering[i]);
+		pr_info("exfGlobalParam  : addr - 0x%08x, val - 0x%08x\n",
+			(u32)&ugeth->p_rx_glbl_pram->exfGlobalParam,
+			in_be32(&ugeth->p_rx_glbl_pram->exfGlobalParam));
 	}
 	if (ugeth->p_send_q_mem_reg) {
-		ugeth_info("Send Q memory registers:");
-		ugeth_info("Base address: 0x%08x",
-			   (u32) ugeth->p_send_q_mem_reg);
+		pr_info("Send Q memory registers:\n");
+		pr_info("Base address: 0x%08x\n", (u32)ugeth->p_send_q_mem_reg);
 		for (i = 0; i < ugeth->ug_info->numQueuesTx; i++) {
-			ugeth_info("SQQD[%d]:", i);
-			ugeth_info("Base address: 0x%08x",
-				   (u32) & ugeth->p_send_q_mem_reg->sqqd[i]);
+			pr_info("SQQD[%d]:\n", i);
+			pr_info("Base address: 0x%08x\n",
+				(u32)&ugeth->p_send_q_mem_reg->sqqd[i]);
 			mem_disp((u8 *) & ugeth->p_send_q_mem_reg->sqqd[i],
 				 sizeof(struct ucc_geth_send_queue_qd));
 		}
 	}
 	if (ugeth->p_scheduler) {
-		ugeth_info("Scheduler:");
-		ugeth_info("Base address: 0x%08x", (u32) ugeth->p_scheduler);
+		pr_info("Scheduler:\n");
+		pr_info("Base address: 0x%08x\n", (u32)ugeth->p_scheduler);
 		mem_disp((u8 *) ugeth->p_scheduler,
 			 sizeof(*ugeth->p_scheduler));
 	}
 	if (ugeth->p_tx_fw_statistics_pram) {
-		ugeth_info("TX FW statistics pram:");
-		ugeth_info("Base address: 0x%08x",
-			   (u32) ugeth->p_tx_fw_statistics_pram);
+		pr_info("TX FW statistics pram:\n");
+		pr_info("Base address: 0x%08x\n",
+			(u32)ugeth->p_tx_fw_statistics_pram);
 		mem_disp((u8 *) ugeth->p_tx_fw_statistics_pram,
 			 sizeof(*ugeth->p_tx_fw_statistics_pram));
 	}
 	if (ugeth->p_rx_fw_statistics_pram) {
-		ugeth_info("RX FW statistics pram:");
-		ugeth_info("Base address: 0x%08x",
-			   (u32) ugeth->p_rx_fw_statistics_pram);
+		pr_info("RX FW statistics pram:\n");
+		pr_info("Base address: 0x%08x\n",
+			(u32)ugeth->p_rx_fw_statistics_pram);
 		mem_disp((u8 *) ugeth->p_rx_fw_statistics_pram,
 			 sizeof(*ugeth->p_rx_fw_statistics_pram));
 	}
 	if (ugeth->p_rx_irq_coalescing_tbl) {
-		ugeth_info("RX IRQ coalescing tables:");
-		ugeth_info("Base address: 0x%08x",
-			   (u32) ugeth->p_rx_irq_coalescing_tbl);
+		pr_info("RX IRQ coalescing tables:\n");
+		pr_info("Base address: 0x%08x\n",
+			(u32)ugeth->p_rx_irq_coalescing_tbl);
 		for (i = 0; i < ugeth->ug_info->numQueuesRx; i++) {
-			ugeth_info("RX IRQ coalescing table entry[%d]:", i);
-			ugeth_info("Base address: 0x%08x",
-				   (u32) & ugeth->p_rx_irq_coalescing_tbl->
-				   coalescingentry[i]);
-			ugeth_info
-		("interruptcoalescingmaxvalue: addr - 0x%08x, val - 0x%08x",
-			     (u32) & ugeth->p_rx_irq_coalescing_tbl->
-			     coalescingentry[i].interruptcoalescingmaxvalue,
-			     in_be32(&ugeth->p_rx_irq_coalescing_tbl->
-				     coalescingentry[i].
-				     interruptcoalescingmaxvalue));
-			ugeth_info
-		("interruptcoalescingcounter : addr - 0x%08x, val - 0x%08x",
-			     (u32) & ugeth->p_rx_irq_coalescing_tbl->
-			     coalescingentry[i].interruptcoalescingcounter,
-			     in_be32(&ugeth->p_rx_irq_coalescing_tbl->
-				     coalescingentry[i].
-				     interruptcoalescingcounter));
+			pr_info("RX IRQ coalescing table entry[%d]:\n", i);
+			pr_info("Base address: 0x%08x\n",
+				(u32)&ugeth->p_rx_irq_coalescing_tbl->
+				coalescingentry[i]);
+			pr_info("interruptcoalescingmaxvalue: addr - 0x%08x, val - 0x%08x\n",
+				(u32)&ugeth->p_rx_irq_coalescing_tbl->
+				coalescingentry[i].interruptcoalescingmaxvalue,
+				in_be32(&ugeth->p_rx_irq_coalescing_tbl->
+					coalescingentry[i].
+					interruptcoalescingmaxvalue));
+			pr_info("interruptcoalescingcounter : addr - 0x%08x, val - 0x%08x\n",
+				(u32)&ugeth->p_rx_irq_coalescing_tbl->
+				coalescingentry[i].interruptcoalescingcounter,
+				in_be32(&ugeth->p_rx_irq_coalescing_tbl->
+					coalescingentry[i].
+					interruptcoalescingcounter));
 		}
 	}
 	if (ugeth->p_rx_bd_qs_tbl) {
-		ugeth_info("RX BD QS tables:");
-		ugeth_info("Base address: 0x%08x", (u32) ugeth->p_rx_bd_qs_tbl);
+		pr_info("RX BD QS tables:\n");
+		pr_info("Base address: 0x%08x\n", (u32)ugeth->p_rx_bd_qs_tbl);
 		for (i = 0; i < ugeth->ug_info->numQueuesRx; i++) {
-			ugeth_info("RX BD QS table[%d]:", i);
-			ugeth_info("Base address: 0x%08x",
-				   (u32) & ugeth->p_rx_bd_qs_tbl[i]);
-			ugeth_info
-			    ("bdbaseptr        : addr - 0x%08x, val - 0x%08x",
-			     (u32) & ugeth->p_rx_bd_qs_tbl[i].bdbaseptr,
-			     in_be32(&ugeth->p_rx_bd_qs_tbl[i].bdbaseptr));
-			ugeth_info
-			    ("bdptr            : addr - 0x%08x, val - 0x%08x",
-			     (u32) & ugeth->p_rx_bd_qs_tbl[i].bdptr,
-			     in_be32(&ugeth->p_rx_bd_qs_tbl[i].bdptr));
-			ugeth_info
-			    ("externalbdbaseptr: addr - 0x%08x, val - 0x%08x",
-			     (u32) & ugeth->p_rx_bd_qs_tbl[i].externalbdbaseptr,
-			     in_be32(&ugeth->p_rx_bd_qs_tbl[i].
-				     externalbdbaseptr));
-			ugeth_info
-			    ("externalbdptr    : addr - 0x%08x, val - 0x%08x",
-			     (u32) & ugeth->p_rx_bd_qs_tbl[i].externalbdptr,
-			     in_be32(&ugeth->p_rx_bd_qs_tbl[i].externalbdptr));
-			ugeth_info("ucode RX Prefetched BDs:");
-			ugeth_info("Base address: 0x%08x",
-				   (u32)
-				   qe_muram_addr(in_be32
-						 (&ugeth->p_rx_bd_qs_tbl[i].
-						  bdbaseptr)));
+			pr_info("RX BD QS table[%d]:\n", i);
+			pr_info("Base address: 0x%08x\n",
+				(u32)&ugeth->p_rx_bd_qs_tbl[i]);
+			pr_info("bdbaseptr        : addr - 0x%08x, val - 0x%08x\n",
+				(u32)&ugeth->p_rx_bd_qs_tbl[i].bdbaseptr,
+				in_be32(&ugeth->p_rx_bd_qs_tbl[i].bdbaseptr));
+			pr_info("bdptr            : addr - 0x%08x, val - 0x%08x\n",
+				(u32)&ugeth->p_rx_bd_qs_tbl[i].bdptr,
+				in_be32(&ugeth->p_rx_bd_qs_tbl[i].bdptr));
+			pr_info("externalbdbaseptr: addr - 0x%08x, val - 0x%08x\n",
+				(u32)&ugeth->p_rx_bd_qs_tbl[i].externalbdbaseptr,
+				in_be32(&ugeth->p_rx_bd_qs_tbl[i].
+					externalbdbaseptr));
+			pr_info("externalbdptr    : addr - 0x%08x, val - 0x%08x\n",
+				(u32)&ugeth->p_rx_bd_qs_tbl[i].externalbdptr,
+				in_be32(&ugeth->p_rx_bd_qs_tbl[i].externalbdptr));
+			pr_info("ucode RX Prefetched BDs:\n");
+			pr_info("Base address: 0x%08x\n",
+				(u32)qe_muram_addr(in_be32
+						   (&ugeth->p_rx_bd_qs_tbl[i].
+						    bdbaseptr)));
 			mem_disp((u8 *)
 				 qe_muram_addr(in_be32
 					       (&ugeth->p_rx_bd_qs_tbl[i].
@@ -1010,9 +996,9 @@ static void dump_regs(struct ucc_geth_private *ugeth)
 	}
 	if (ugeth->p_init_enet_param_shadow) {
 		int size;
-		ugeth_info("Init enet param shadow:");
-		ugeth_info("Base address: 0x%08x",
-			   (u32) ugeth->p_init_enet_param_shadow);
+		pr_info("Init enet param shadow:\n");
+		pr_info("Base address: 0x%08x\n",
+			(u32) ugeth->p_init_enet_param_shadow);
 		mem_disp((u8 *) ugeth->p_init_enet_param_shadow,
 			 sizeof(*ugeth->p_init_enet_param_shadow));
 
@@ -1392,12 +1378,11 @@ static int adjust_enet_interface(struct ucc_geth_private *ugeth)
 		struct phy_device *tbiphy;
 
 		if (!ug_info->tbi_node)
-			ugeth_warn("TBI mode requires that the device "
-				"tree specify a tbi-handle\n");
+			pr_warn("TBI mode requires that the device tree specify a tbi-handle\n");
 
 		tbiphy = of_phy_find_device(ug_info->tbi_node);
 		if (!tbiphy)
-			ugeth_warn("Could not get TBI device\n");
+			pr_warn("Could not get TBI device\n");
 
 		value = phy_read(tbiphy, ENET_TBI_MII_CR);
 		value &= ~0x1000;	/* Turn off autonegotiation */
@@ -1409,8 +1394,7 @@ static int adjust_enet_interface(struct ucc_geth_private *ugeth)
 	ret_val = init_preamble_length(ug_info->prel, &ug_regs->maccfg2);
 	if (ret_val != 0) {
 		if (netif_msg_probe(ugeth))
-			ugeth_err("%s: Preamble length must be between 3 and 7 inclusive.",
-			     __func__);
+			pr_err("Preamble length must be between 3 and 7 inclusive\n");
 		return ret_val;
 	}
 
@@ -1520,7 +1504,7 @@ static int ugeth_enable(struct ucc_geth_private *ugeth, enum comm_dir mode)
 	/* check if the UCC number is in range. */
 	if (ugeth->ug_info->uf_info.ucc_num >= UCC_MAX_NUM) {
 		if (netif_msg_probe(ugeth))
-			ugeth_err("%s: ucc_num out of range.", __func__);
+			pr_err("ucc_num out of range\n");
 		return -EINVAL;
 	}
 
@@ -1549,7 +1533,7 @@ static int ugeth_disable(struct ucc_geth_private *ugeth, enum comm_dir mode)
 	/* check if the UCC number is in range. */
 	if (ugeth->ug_info->uf_info.ucc_num >= UCC_MAX_NUM) {
 		if (netif_msg_probe(ugeth))
-			ugeth_err("%s: ucc_num out of range.", __func__);
+			pr_err("ucc_num out of range\n");
 		return -EINVAL;
 	}
 
@@ -1648,7 +1632,7 @@ static void adjust_link(struct net_device *dev)
 				break;
 			default:
 				if (netif_msg_link(ugeth))
-					ugeth_warn(
+					pr_warn(
 						"%s: Ack!  Speed (%d) is not 10/100/1000!",
 						dev->name, phydev->speed);
 				break;
@@ -2103,8 +2087,7 @@ static int ucc_struct_init(struct ucc_geth_private *ugeth)
 	if (!((uf_info->bd_mem_part == MEM_PART_SYSTEM) ||
 	      (uf_info->bd_mem_part == MEM_PART_MURAM))) {
 		if (netif_msg_probe(ugeth))
-			ugeth_err("%s: Bad memory partition value.",
-					__func__);
+			pr_err("Bad memory partition value\n");
 		return -EINVAL;
 	}
 
@@ -2114,9 +2097,7 @@ static int ucc_struct_init(struct ucc_geth_private *ugeth)
 		    (ug_info->bdRingLenRx[i] %
 		     UCC_GETH_RX_BD_RING_SIZE_ALIGNMENT)) {
 			if (netif_msg_probe(ugeth))
-				ugeth_err
-				    ("%s: Rx BD ring length must be multiple of 4, no smaller than 8.",
-					__func__);
+				pr_err("Rx BD ring length must be multiple of 4, no smaller than 8\n");
 			return -EINVAL;
 		}
 	}
@@ -2125,9 +2106,7 @@ static int ucc_struct_init(struct ucc_geth_private *ugeth)
 	for (i = 0; i < ug_info->numQueuesTx; i++) {
 		if (ug_info->bdRingLenTx[i] < UCC_GETH_TX_BD_RING_SIZE_MIN) {
 			if (netif_msg_probe(ugeth))
-				ugeth_err
-				    ("%s: Tx BD ring length must be no smaller than 2.",
-				     __func__);
+				pr_err("Tx BD ring length must be no smaller than 2\n");
 			return -EINVAL;
 		}
 	}
@@ -2136,23 +2115,21 @@ static int ucc_struct_init(struct ucc_geth_private *ugeth)
 	if ((uf_info->max_rx_buf_length == 0) ||
 	    (uf_info->max_rx_buf_length % UCC_GETH_MRBLR_ALIGNMENT)) {
 		if (netif_msg_probe(ugeth))
-			ugeth_err
-			    ("%s: max_rx_buf_length must be non-zero multiple of 128.",
-			     __func__);
+			pr_err("max_rx_buf_length must be non-zero multiple of 128\n");
 		return -EINVAL;
 	}
 
 	/* num Tx queues */
 	if (ug_info->numQueuesTx > NUM_TX_QUEUES) {
 		if (netif_msg_probe(ugeth))
-			ugeth_err("%s: number of tx queues too large.", __func__);
+			pr_err("number of tx queues too large\n");
 		return -EINVAL;
 	}
 
 	/* num Rx queues */
 	if (ug_info->numQueuesRx > NUM_RX_QUEUES) {
 		if (netif_msg_probe(ugeth))
-			ugeth_err("%s: number of rx queues too large.", __func__);
+			pr_err("number of rx queues too large\n");
 		return -EINVAL;
 	}
 
@@ -2160,10 +2137,7 @@ static int ucc_struct_init(struct ucc_geth_private *ugeth)
 	for (i = 0; i < UCC_GETH_VLAN_PRIORITY_MAX; i++) {
 		if (ug_info->l2qt[i] >= ug_info->numQueuesRx) {
 			if (netif_msg_probe(ugeth))
-				ugeth_err
-				    ("%s: VLAN priority table entry must not be"
-					" larger than number of Rx queues.",
-				     __func__);
+				pr_err("VLAN priority table entry must not be larger than number of Rx queues\n");
 			return -EINVAL;
 		}
 	}
@@ -2172,18 +2146,14 @@ static int ucc_struct_init(struct ucc_geth_private *ugeth)
 	for (i = 0; i < UCC_GETH_IP_PRIORITY_MAX; i++) {
 		if (ug_info->l3qt[i] >= ug_info->numQueuesRx) {
 			if (netif_msg_probe(ugeth))
-				ugeth_err
-				    ("%s: IP priority table entry must not be"
-					" larger than number of Rx queues.",
-				     __func__);
+				pr_err("IP priority table entry must not be larger than number of Rx queues\n");
 			return -EINVAL;
 		}
 	}
 
 	if (ug_info->cam && !ug_info->ecamptr) {
 		if (netif_msg_probe(ugeth))
-			ugeth_err("%s: If cam mode is chosen, must supply cam ptr.",
-				  __func__);
+			pr_err("If cam mode is chosen, must supply cam ptr\n");
 		return -EINVAL;
 	}
 
@@ -2191,9 +2161,7 @@ static int ucc_struct_init(struct ucc_geth_private *ugeth)
 	     UCC_GETH_NUM_OF_STATION_ADDRESSES_1) &&
 	    ug_info->rxExtendedFiltering) {
 		if (netif_msg_probe(ugeth))
-			ugeth_err("%s: Number of station addresses greater than 1 "
-				  "not allowed in extended parsing mode.",
-				  __func__);
+			pr_err("Number of station addresses greater than 1 not allowed in extended parsing mode\n");
 		return -EINVAL;
 	}
 
@@ -2207,7 +2175,7 @@ static int ucc_struct_init(struct ucc_geth_private *ugeth)
 	/* Initialize the general fast UCC block. */
 	if (ucc_fast_init(uf_info, &ugeth->uccf)) {
 		if (netif_msg_probe(ugeth))
-			ugeth_err("%s: Failed to init uccf.", __func__);
+			pr_err("Failed to init uccf\n");
 		return -ENOMEM;
 	}
 
@@ -2222,7 +2190,7 @@ static int ucc_struct_init(struct ucc_geth_private *ugeth)
 	ugeth->ug_regs = ioremap(uf_info->regs, sizeof(*ugeth->ug_regs));
 	if (!ugeth->ug_regs) {
 		if (netif_msg_probe(ugeth))
-			ugeth_err("%s: Failed to ioremap regs.", __func__);
+			pr_err("Failed to ioremap regs\n");
 		return -ENOMEM;
 	}
 
@@ -2273,9 +2241,7 @@ static int ucc_geth_alloc_tx(struct ucc_geth_private *ugeth)
 		}
 		if (!ugeth->p_tx_bd_ring[j]) {
 			if (netif_msg_ifup(ugeth))
-				ugeth_err
-				    ("%s: Can not allocate memory for Tx bd rings.",
-				     __func__);
+				pr_err("Can not allocate memory for Tx bd rings\n");
 			return -ENOMEM;
 		}
 		/* Zero unused end of bd ring, according to spec */
@@ -2293,8 +2259,7 @@ static int ucc_geth_alloc_tx(struct ucc_geth_private *ugeth)
 
 		if (ugeth->tx_skbuff[j] == NULL) {
 			if (netif_msg_ifup(ugeth))
-				ugeth_err("%s: Could not allocate tx_skbuff",
-					  __func__);
+				pr_err("Could not allocate tx_skbuff\n");
 			return -ENOMEM;
 		}
 
@@ -2353,9 +2318,7 @@ static int ucc_geth_alloc_rx(struct ucc_geth_private *ugeth)
 		}
 		if (!ugeth->p_rx_bd_ring[j]) {
 			if (netif_msg_ifup(ugeth))
-				ugeth_err
-				    ("%s: Can not allocate memory for Rx bd rings.",
-				     __func__);
+				pr_err("Can not allocate memory for Rx bd rings\n");
 			return -ENOMEM;
 		}
 	}
@@ -2369,8 +2332,7 @@ static int ucc_geth_alloc_rx(struct ucc_geth_private *ugeth)
 
 		if (ugeth->rx_skbuff[j] == NULL) {
 			if (netif_msg_ifup(ugeth))
-				ugeth_err("%s: Could not allocate rx_skbuff",
-					  __func__);
+				pr_err("Could not allocate rx_skbuff\n");
 			return -ENOMEM;
 		}
 
@@ -2438,8 +2400,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 		break;
 	default:
 		if (netif_msg_ifup(ugeth))
-			ugeth_err("%s: Bad number of Rx threads value.",
-				       	__func__);
+			pr_err("Bad number of Rx threads value\n");
 		return -EINVAL;
 		break;
 	}
@@ -2462,8 +2423,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 		break;
 	default:
 		if (netif_msg_ifup(ugeth))
-			ugeth_err("%s: Bad number of Tx threads value.",
-				       	__func__);
+			pr_err("Bad number of Tx threads value\n");
 		return -EINVAL;
 		break;
 	}
@@ -2512,8 +2472,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 					      &ug_regs->ipgifg);
 	if (ret_val != 0) {
 		if (netif_msg_ifup(ugeth))
-			ugeth_err("%s: IPGIFG initialization parameter too large.",
-				  __func__);
+			pr_err("IPGIFG initialization parameter too large\n");
 		return ret_val;
 	}
 
@@ -2529,8 +2488,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 					  &ug_regs->hafdup);
 	if (ret_val != 0) {
 		if (netif_msg_ifup(ugeth))
-			ugeth_err("%s: Half Duplex initialization parameter too large.",
-			  __func__);
+			pr_err("Half Duplex initialization parameter too large\n");
 		return ret_val;
 	}
 
@@ -2567,9 +2525,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 			   UCC_GETH_TX_GLOBAL_PRAM_ALIGNMENT);
 	if (IS_ERR_VALUE(ugeth->tx_glbl_pram_offset)) {
 		if (netif_msg_ifup(ugeth))
-			ugeth_err
-			    ("%s: Can not allocate DPRAM memory for p_tx_glbl_pram.",
-			     __func__);
+			pr_err("Can not allocate DPRAM memory for p_tx_glbl_pram\n");
 		return -ENOMEM;
 	}
 	ugeth->p_tx_glbl_pram =
@@ -2589,9 +2545,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 			   UCC_GETH_THREAD_DATA_ALIGNMENT);
 	if (IS_ERR_VALUE(ugeth->thread_dat_tx_offset)) {
 		if (netif_msg_ifup(ugeth))
-			ugeth_err
-			    ("%s: Can not allocate DPRAM memory for p_thread_data_tx.",
-			     __func__);
+			pr_err("Can not allocate DPRAM memory for p_thread_data_tx\n");
 		return -ENOMEM;
 	}
 
@@ -2618,9 +2572,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 			   UCC_GETH_SEND_QUEUE_QUEUE_DESCRIPTOR_ALIGNMENT);
 	if (IS_ERR_VALUE(ugeth->send_q_mem_reg_offset)) {
 		if (netif_msg_ifup(ugeth))
-			ugeth_err
-			    ("%s: Can not allocate DPRAM memory for p_send_q_mem_reg.",
-			     __func__);
+			pr_err("Can not allocate DPRAM memory for p_send_q_mem_reg\n");
 		return -ENOMEM;
 	}
 
@@ -2661,9 +2613,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 				   UCC_GETH_SCHEDULER_ALIGNMENT);
 		if (IS_ERR_VALUE(ugeth->scheduler_offset)) {
 			if (netif_msg_ifup(ugeth))
-				ugeth_err
-				 ("%s: Can not allocate DPRAM memory for p_scheduler.",
-				     __func__);
+				pr_err("Can not allocate DPRAM memory for p_scheduler\n");
 			return -ENOMEM;
 		}
 
@@ -2710,10 +2660,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 				   UCC_GETH_TX_STATISTICS_ALIGNMENT);
 		if (IS_ERR_VALUE(ugeth->tx_fw_statistics_pram_offset)) {
 			if (netif_msg_ifup(ugeth))
-				ugeth_err
-				    ("%s: Can not allocate DPRAM memory for"
-					" p_tx_fw_statistics_pram.",
-				       	__func__);
+				pr_err("Can not allocate DPRAM memory for p_tx_fw_statistics_pram\n");
 			return -ENOMEM;
 		}
 		ugeth->p_tx_fw_statistics_pram =
@@ -2750,9 +2697,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 			   UCC_GETH_RX_GLOBAL_PRAM_ALIGNMENT);
 	if (IS_ERR_VALUE(ugeth->rx_glbl_pram_offset)) {
 		if (netif_msg_ifup(ugeth))
-			ugeth_err
-			    ("%s: Can not allocate DPRAM memory for p_rx_glbl_pram.",
-			     __func__);
+			pr_err("Can not allocate DPRAM memory for p_rx_glbl_pram\n");
 		return -ENOMEM;
 	}
 	ugeth->p_rx_glbl_pram =
@@ -2771,9 +2716,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 			   UCC_GETH_THREAD_DATA_ALIGNMENT);
 	if (IS_ERR_VALUE(ugeth->thread_dat_rx_offset)) {
 		if (netif_msg_ifup(ugeth))
-			ugeth_err
-			    ("%s: Can not allocate DPRAM memory for p_thread_data_rx.",
-			     __func__);
+			pr_err("Can not allocate DPRAM memory for p_thread_data_rx\n");
 		return -ENOMEM;
 	}
 
@@ -2794,9 +2737,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 				   UCC_GETH_RX_STATISTICS_ALIGNMENT);
 		if (IS_ERR_VALUE(ugeth->rx_fw_statistics_pram_offset)) {
 			if (netif_msg_ifup(ugeth))
-				ugeth_err
-					("%s: Can not allocate DPRAM memory for"
-					" p_rx_fw_statistics_pram.", __func__);
+				pr_err("Can not allocate DPRAM memory for p_rx_fw_statistics_pram\n");
 			return -ENOMEM;
 		}
 		ugeth->p_rx_fw_statistics_pram =
@@ -2816,9 +2757,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 			   + 4, UCC_GETH_RX_INTERRUPT_COALESCING_ALIGNMENT);
 	if (IS_ERR_VALUE(ugeth->rx_irq_coalescing_tbl_offset)) {
 		if (netif_msg_ifup(ugeth))
-			ugeth_err
-			    ("%s: Can not allocate DPRAM memory for"
-				" p_rx_irq_coalescing_tbl.", __func__);
+			pr_err("Can not allocate DPRAM memory for p_rx_irq_coalescing_tbl\n");
 		return -ENOMEM;
 	}
 
@@ -2884,9 +2823,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 			   UCC_GETH_RX_BD_QUEUES_ALIGNMENT);
 	if (IS_ERR_VALUE(ugeth->rx_bd_qs_tbl_offset)) {
 		if (netif_msg_ifup(ugeth))
-			ugeth_err
-			    ("%s: Can not allocate DPRAM memory for p_rx_bd_qs_tbl.",
-			     __func__);
+			pr_err("Can not allocate DPRAM memory for p_rx_bd_qs_tbl\n");
 		return -ENOMEM;
 	}
 
@@ -2961,8 +2898,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 	if (ug_info->rxExtendedFiltering) {
 		if (!ug_info->extendedFilteringChainPointer) {
 			if (netif_msg_ifup(ugeth))
-				ugeth_err("%s: Null Extended Filtering Chain Pointer.",
-					  __func__);
+				pr_err("Null Extended Filtering Chain Pointer\n");
 			return -EINVAL;
 		}
 
@@ -2973,9 +2909,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 		UCC_GETH_RX_EXTENDED_FILTERING_GLOBAL_PARAMETERS_ALIGNMENT);
 		if (IS_ERR_VALUE(ugeth->exf_glbl_param_offset)) {
 			if (netif_msg_ifup(ugeth))
-				ugeth_err
-					("%s: Can not allocate DPRAM memory for"
-					" p_exf_glbl_param.", __func__);
+				pr_err("Can not allocate DPRAM memory for p_exf_glbl_param\n");
 			return -ENOMEM;
 		}
 
@@ -3020,9 +2954,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 	if (!(ugeth->p_init_enet_param_shadow =
 	      kmalloc(sizeof(struct ucc_geth_init_pram), GFP_KERNEL))) {
 		if (netif_msg_ifup(ugeth))
-			ugeth_err
-			    ("%s: Can not allocate memory for"
-				" p_UccInitEnetParamShadows.", __func__);
+			pr_err("Can not allocate memory for p_UccInitEnetParamShadows\n");
 		return -ENOMEM;
 	}
 	/* Zero out *p_init_enet_param_shadow */
@@ -3055,8 +2987,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 	    (ug_info->largestexternallookupkeysize !=
 	     QE_FLTR_LARGEST_EXTERNAL_TABLE_LOOKUP_KEY_SIZE_16_BYTES)) {
 		if (netif_msg_ifup(ugeth))
-			ugeth_err("%s: Invalid largest External Lookup Key Size.",
-				  __func__);
+			pr_err("Invalid largest External Lookup Key Size\n");
 		return -EINVAL;
 	}
 	ugeth->p_init_enet_param_shadow->largestexternallookupkeysize =
@@ -3081,8 +3012,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 		, size, UCC_GETH_THREAD_RX_PRAM_ALIGNMENT,
 		ug_info->riscRx, 1)) != 0) {
 		if (netif_msg_ifup(ugeth))
-				ugeth_err("%s: Can not fill p_init_enet_param_shadow.",
-					__func__);
+			pr_err("Can not fill p_init_enet_param_shadow\n");
 		return ret_val;
 	}
 
@@ -3096,8 +3026,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 				    UCC_GETH_THREAD_TX_PRAM_ALIGNMENT,
 				    ug_info->riscTx, 0)) != 0) {
 		if (netif_msg_ifup(ugeth))
-			ugeth_err("%s: Can not fill p_init_enet_param_shadow.",
-				  __func__);
+			pr_err("Can not fill p_init_enet_param_shadow\n");
 		return ret_val;
 	}
 
@@ -3105,8 +3034,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 	for (i = 0; i < ug_info->numQueuesRx; i++) {
 		if ((ret_val = rx_bd_buffer_set(ugeth, (u8) i)) != 0) {
 			if (netif_msg_ifup(ugeth))
-				ugeth_err("%s: Can not fill Rx bds with buffers.",
-					  __func__);
+				pr_err("Can not fill Rx bds with buffers\n");
 			return ret_val;
 		}
 	}
@@ -3115,9 +3043,7 @@ static int ucc_geth_startup(struct ucc_geth_private *ugeth)
 	init_enet_pram_offset = qe_muram_alloc(sizeof(struct ucc_geth_init_pram), 4);
 	if (IS_ERR_VALUE(init_enet_pram_offset)) {
 		if (netif_msg_ifup(ugeth))
-			ugeth_err
-			    ("%s: Can not allocate DPRAM memory for p_init_enet_pram.",
-			     __func__);
+			pr_err("Can not allocate DPRAM memory for p_init_enet_pram\n");
 		return -ENOMEM;
 	}
 	p_init_enet_pram =
@@ -3266,8 +3192,8 @@ static int ucc_geth_rx(struct ucc_geth_private *ugeth, u8 rxQ, int rx_work_limit
 		    (!(bd_status & (R_F | R_L))) ||
 		    (bd_status & R_ERRORS_FATAL)) {
 			if (netif_msg_rx_err(ugeth))
-				ugeth_err("%s, %d: ERROR!!! skb - 0x%08x",
-					   __func__, __LINE__, (u32) skb);
+				pr_err("%d: ERROR!!! skb - 0x%08x\n",
+				       __LINE__, (u32)skb);
 			dev_kfree_skb(skb);
 
 			ugeth->rx_skbuff[rxQ][ugeth->skb_currx[rxQ]] = NULL;
@@ -3290,7 +3216,7 @@ static int ucc_geth_rx(struct ucc_geth_private *ugeth, u8 rxQ, int rx_work_limit
 		skb = get_new_skb(ugeth, bd);
 		if (!skb) {
 			if (netif_msg_rx_err(ugeth))
-				ugeth_warn("%s: No Rx Data Buffer", __func__);
+				pr_warn("No Rx Data Buffer\n");
 			dev->stats.rx_dropped++;
 			break;
 		}
@@ -3481,25 +3407,19 @@ static int ucc_geth_init_mac(struct ucc_geth_private *ugeth)
 
 	err = ucc_struct_init(ugeth);
 	if (err) {
-		if (netif_msg_ifup(ugeth))
-			ugeth_err("%s: Cannot configure internal struct, "
-				  "aborting.", dev->name);
+		netif_err(ugeth, ifup, dev, "Cannot configure internal struct, aborting\n");
 		goto err;
 	}
 
 	err = ucc_geth_startup(ugeth);
 	if (err) {
-		if (netif_msg_ifup(ugeth))
-			ugeth_err("%s: Cannot configure net device, aborting.",
-				  dev->name);
+		netif_err(ugeth, ifup, dev, "Cannot configure net device, aborting\n");
 		goto err;
 	}
 
 	err = adjust_enet_interface(ugeth);
 	if (err) {
-		if (netif_msg_ifup(ugeth))
-			ugeth_err("%s: Cannot configure net device, aborting.",
-				  dev->name);
+		netif_err(ugeth, ifup, dev, "Cannot configure net device, aborting\n");
 		goto err;
 	}
 
@@ -3516,8 +3436,7 @@ static int ucc_geth_init_mac(struct ucc_geth_private *ugeth)
 
 	err = ugeth_enable(ugeth, COMM_DIR_RX_AND_TX);
 	if (err) {
-		if (netif_msg_ifup(ugeth))
-			ugeth_err("%s: Cannot enable net device, aborting.", dev->name);
+		netif_err(ugeth, ifup, dev, "Cannot enable net device, aborting\n");
 		goto err;
 	}
 
@@ -3538,35 +3457,27 @@ static int ucc_geth_open(struct net_device *dev)
 
 	/* Test station address */
 	if (dev->dev_addr[0] & ENET_GROUP_ADDR) {
-		if (netif_msg_ifup(ugeth))
-			ugeth_err("%s: Multicast address used for station "
-				  "address - is this what you wanted?",
-				  __func__);
+		netif_err(ugeth, ifup, dev,
+			  "Multicast address used for station address - is this what you wanted?\n");
 		return -EINVAL;
 	}
 
 	err = init_phy(dev);
 	if (err) {
-		if (netif_msg_ifup(ugeth))
-			ugeth_err("%s: Cannot initialize PHY, aborting.",
-				  dev->name);
+		netif_err(ugeth, ifup, dev, "Cannot initialize PHY, aborting\n");
 		return err;
 	}
 
 	err = ucc_geth_init_mac(ugeth);
 	if (err) {
-		if (netif_msg_ifup(ugeth))
-			ugeth_err("%s: Cannot initialize MAC, aborting.",
-				  dev->name);
+		netif_err(ugeth, ifup, dev, "Cannot initialize MAC, aborting\n");
 		goto err;
 	}
 
 	err = request_irq(ugeth->ug_info->uf_info.irq, ucc_geth_irq_handler,
 			  0, "UCC Geth", dev);
 	if (err) {
-		if (netif_msg_ifup(ugeth))
-			ugeth_err("%s: Cannot get IRQ for net device, aborting.",
-				  dev->name);
+		netif_err(ugeth, ifup, dev, "Cannot get IRQ for net device, aborting\n");
 		goto err;
 	}
 
@@ -3704,8 +3615,7 @@ static int ucc_geth_resume(struct platform_device *ofdev)
 
 		err = ucc_geth_init_mac(ugeth);
 		if (err) {
-			ugeth_err("%s: Cannot initialize MAC, aborting.",
-				  ndev->name);
+			netdev_err(ndev, "Cannot initialize MAC, aborting\n");
 			return err;
 		}
 	}
@@ -3825,8 +3735,7 @@ static int ucc_geth_probe(struct platform_device* ofdev)
 	ug_info = &ugeth_info[ucc_num];
 	if (ug_info == NULL) {
 		if (netif_msg_probe(&debug))
-			ugeth_err("%s: [%d] Missing additional data!",
-				       	__func__, ucc_num);
+			pr_err("[%d] Missing additional data!\n", ucc_num);
 		return -ENODEV;
 	}
 
@@ -3837,8 +3746,7 @@ static int ucc_geth_probe(struct platform_device* ofdev)
 		ug_info->uf_info.rx_clock = qe_clock_source(sprop);
 		if ((ug_info->uf_info.rx_clock < QE_CLK_NONE) ||
 		    (ug_info->uf_info.rx_clock > QE_CLK24)) {
-			printk(KERN_ERR
-				"ucc_geth: invalid rx-clock-name property\n");
+			pr_err("invalid rx-clock-name property\n");
 			return -EINVAL;
 		}
 	} else {
@@ -3846,13 +3754,11 @@ static int ucc_geth_probe(struct platform_device* ofdev)
 		if (!prop) {
 			/* If both rx-clock-name and rx-clock are missing,
 			   we want to tell people to use rx-clock-name. */
-			printk(KERN_ERR
-				"ucc_geth: missing rx-clock-name property\n");
+			pr_err("missing rx-clock-name property\n");
 			return -EINVAL;
 		}
 		if ((*prop < QE_CLK_NONE) || (*prop > QE_CLK24)) {
-			printk(KERN_ERR
-				"ucc_geth: invalid rx-clock propperty\n");
+			pr_err("invalid rx-clock propperty\n");
 			return -EINVAL;
 		}
 		ug_info->uf_info.rx_clock = *prop;
@@ -3863,20 +3769,17 @@ static int ucc_geth_probe(struct platform_device* ofdev)
 		ug_info->uf_info.tx_clock = qe_clock_source(sprop);
 		if ((ug_info->uf_info.tx_clock < QE_CLK_NONE) ||
 		    (ug_info->uf_info.tx_clock > QE_CLK24)) {
-			printk(KERN_ERR
-				"ucc_geth: invalid tx-clock-name property\n");
+			pr_err("invalid tx-clock-name property\n");
 			return -EINVAL;
 		}
 	} else {
 		prop = of_get_property(np, "tx-clock", NULL);
 		if (!prop) {
-			printk(KERN_ERR
-				"ucc_geth: missing tx-clock-name property\n");
+			pr_err("missing tx-clock-name property\n");
 			return -EINVAL;
 		}
 		if ((*prop < QE_CLK_NONE) || (*prop > QE_CLK24)) {
-			printk(KERN_ERR
-				"ucc_geth: invalid tx-clock property\n");
+			pr_err("invalid tx-clock property\n");
 			return -EINVAL;
 		}
 		ug_info->uf_info.tx_clock = *prop;
@@ -3949,7 +3852,7 @@ static int ucc_geth_probe(struct platform_device* ofdev)
 	}
 
 	if (netif_msg_probe(&debug))
-		printk(KERN_INFO "ucc_geth: UCC%1d at 0x%8x (irq = %d)\n",
+		pr_info("UCC%1d at 0x%8x (irq = %d)\n",
 			ug_info->uf_info.ucc_num + 1, ug_info->uf_info.regs,
 			ug_info->uf_info.irq);
 
@@ -3988,8 +3891,8 @@ static int ucc_geth_probe(struct platform_device* ofdev)
 	err = register_netdev(dev);
 	if (err) {
 		if (netif_msg_probe(ugeth))
-			ugeth_err("%s: Cannot register net device, aborting.",
-				  dev->name);
+			pr_err("%s: Cannot register net device, aborting\n",
+			       dev->name);
 		free_netdev(dev);
 		return err;
 	}
@@ -4047,7 +3950,7 @@ static int __init ucc_geth_init(void)
 	int i, ret;
 
 	if (netif_msg_drv(&debug))
-		printk(KERN_INFO "ucc_geth: " DRV_DESC "\n");
+		pr_info(DRV_DESC "\n");
 	for (i = 0; i < 8; i++)
 		memcpy(&(ugeth_info[i]), &ugeth_primary_info,
 		       sizeof(ugeth_primary_info));
diff --git a/drivers/net/ethernet/freescale/ucc_geth_ethtool.c b/drivers/net/ethernet/freescale/ucc_geth_ethtool.c
index 1ebf7128ec04..e79aaf9ae52a 100644
--- a/drivers/net/ethernet/freescale/ucc_geth_ethtool.c
+++ b/drivers/net/ethernet/freescale/ucc_geth_ethtool.c
@@ -38,7 +38,7 @@
 
 #include "ucc_geth.h"
 
-static char hw_stat_gstrings[][ETH_GSTRING_LEN] = {
+static const char hw_stat_gstrings[][ETH_GSTRING_LEN] = {
 	"tx-64-frames",
 	"tx-65-127-frames",
 	"tx-128-255-frames",
@@ -59,7 +59,7 @@ static char hw_stat_gstrings[][ETH_GSTRING_LEN] = {
 	"rx-dropped-frames",
 };
 
-static char tx_fw_stat_gstrings[][ETH_GSTRING_LEN] = {
+static const char tx_fw_stat_gstrings[][ETH_GSTRING_LEN] = {
 	"tx-single-collision",
 	"tx-multiple-collision",
 	"tx-late-collsion",
@@ -74,7 +74,7 @@ static char tx_fw_stat_gstrings[][ETH_GSTRING_LEN] = {
 	"tx-jumbo-frames",
 };
 
-static char rx_fw_stat_gstrings[][ETH_GSTRING_LEN] = {
+static const char rx_fw_stat_gstrings[][ETH_GSTRING_LEN] = {
 	"rx-crc-errors",
 	"rx-alignment-errors",
 	"rx-in-range-length-errors",
@@ -160,8 +160,7 @@ uec_set_pauseparam(struct net_device *netdev,
 	if (ugeth->phydev->autoneg) {
 		if (netif_running(netdev)) {
 			/* FIXME: automatically restart */
-			printk(KERN_INFO
-				"Please re-open the interface.\n");
+			netdev_info(netdev, "Please re-open the interface\n");
 		}
 	} else {
 		struct ucc_geth_info *ug_info = ugeth->ug_info;
@@ -240,18 +239,18 @@ uec_set_ringparam(struct net_device *netdev,
 	int queue = 0, ret = 0;
 
 	if (ring->rx_pending < UCC_GETH_RX_BD_RING_SIZE_MIN) {
-		printk("%s: RxBD ring size must be no smaller than %d.\n",
-			       	netdev->name, UCC_GETH_RX_BD_RING_SIZE_MIN);
+		netdev_info(netdev, "RxBD ring size must be no smaller than %d\n",
+			    UCC_GETH_RX_BD_RING_SIZE_MIN);
 		return -EINVAL;
 	}
 	if (ring->rx_pending % UCC_GETH_RX_BD_RING_SIZE_ALIGNMENT) {
-		printk("%s: RxBD ring size must be multiple of %d.\n",
-			netdev->name, UCC_GETH_RX_BD_RING_SIZE_ALIGNMENT);
+		netdev_info(netdev, "RxBD ring size must be multiple of %d\n",
+			    UCC_GETH_RX_BD_RING_SIZE_ALIGNMENT);
 		return -EINVAL;
 	}
 	if (ring->tx_pending < UCC_GETH_TX_BD_RING_SIZE_MIN) {
-		printk("%s: TxBD ring size must be no smaller than %d.\n",
-				netdev->name, UCC_GETH_TX_BD_RING_SIZE_MIN);
+		netdev_info(netdev, "TxBD ring size must be no smaller than %d\n",
+			    UCC_GETH_TX_BD_RING_SIZE_MIN);
 		return -EINVAL;
 	}
 
@@ -260,8 +259,7 @@ uec_set_ringparam(struct net_device *netdev,
 
 	if (netif_running(netdev)) {
 		/* FIXME: restart automatically */
-		printk(KERN_INFO
-			"Please re-open the interface.\n");
+		netdev_info(netdev, "Please re-open the interface\n");
 	}
 
 	return ret;
diff --git a/drivers/net/ethernet/fujitsu/fmvj18x_cs.c b/drivers/net/ethernet/fujitsu/fmvj18x_cs.c
index ab98b77df309..ef46b58cb4e9 100644
--- a/drivers/net/ethernet/fujitsu/fmvj18x_cs.c
+++ b/drivers/net/ethernet/fujitsu/fmvj18x_cs.c
@@ -991,8 +991,6 @@ static void fjn_rx(struct net_device *dev)
 	    }
 	    skb = netdev_alloc_skb(dev, pkt_len + 2);
 	    if (skb == NULL) {
-		netdev_notice(dev, "Memory squeeze, dropping packet (len %d)\n",
-			      pkt_len);
 		outb(F_SKP_PKT, ioaddr + RX_SKIP);
 		dev->stats.rx_dropped++;
 		break;
diff --git a/drivers/net/ethernet/i825xx/82596.c b/drivers/net/ethernet/i825xx/82596.c
index 1c54e229e3cc..e38816145395 100644
--- a/drivers/net/ethernet/i825xx/82596.c
+++ b/drivers/net/ethernet/i825xx/82596.c
@@ -798,16 +798,14 @@ static inline int i596_rx(struct net_device *dev)
 #ifdef __mc68000__
 				cache_clear(virt_to_phys(newskb->data), PKT_BUF_SZ);
 #endif
-			}
-			else
+			} else {
 				skb = netdev_alloc_skb(dev, pkt_len + 2);
+			}
 memory_squeeze:
 			if (skb == NULL) {
 				/* XXX tulip.c can defer packets here!! */
-				printk(KERN_WARNING "%s: i596_rx Memory squeeze, dropping packet.\n", dev->name);
 				dev->stats.rx_dropped++;
-			}
-			else {
+			} else {
 				if (!rx_in_place) {
 					/* 16 byte align the data fields */
 					skb_reserve(skb, 2);
diff --git a/drivers/net/ethernet/i825xx/lib82596.c b/drivers/net/ethernet/i825xx/lib82596.c
index f045ea4dc514..d653bac4cfc4 100644
--- a/drivers/net/ethernet/i825xx/lib82596.c
+++ b/drivers/net/ethernet/i825xx/lib82596.c
@@ -715,14 +715,12 @@ static inline int i596_rx(struct net_device *dev)
 				rbd->v_data = newskb->data;
 				rbd->b_data = SWAP32(dma_addr);
 				DMA_WBACK_INV(dev, rbd, sizeof(struct i596_rbd));
-			} else
+			} else {
 				skb = netdev_alloc_skb_ip_align(dev, pkt_len);
+			}
 memory_squeeze:
 			if (skb == NULL) {
 				/* XXX tulip.c can defer packets here!! */
-				printk(KERN_ERR
-				       "%s: i596_rx Memory squeeze, dropping packet.\n",
-				       dev->name);
 				dev->stats.rx_dropped++;
 			} else {
 				if (!rx_in_place) {
diff --git a/drivers/net/ethernet/ibm/ehea/ehea_main.c b/drivers/net/ethernet/ibm/ehea/ehea_main.c
index 328f47c92e26..90ea0b1673ca 100644
--- a/drivers/net/ethernet/ibm/ehea/ehea_main.c
+++ b/drivers/net/ethernet/ibm/ehea/ehea_main.c
@@ -402,7 +402,6 @@ static void ehea_refill_rq1(struct ehea_port_res *pr, int index, int nr_of_wqes)
 			skb_arr_rq1[index] = netdev_alloc_skb(dev,
 							      EHEA_L_PKT_SIZE);
 			if (!skb_arr_rq1[index]) {
-				netdev_info(dev, "Unable to allocate enough skb in the array\n");
 				pr->rq1_skba.os_skbs = fill_wqes - i;
 				break;
 			}
@@ -432,10 +431,8 @@ static void ehea_init_fill_rq1(struct ehea_port_res *pr, int nr_rq1a)
 
 	for (i = 0; i < nr_rq1a; i++) {
 		skb_arr_rq1[i] = netdev_alloc_skb(dev, EHEA_L_PKT_SIZE);
-		if (!skb_arr_rq1[i]) {
-			netdev_info(dev, "Not enough memory to allocate skb array\n");
+		if (!skb_arr_rq1[i])
 			break;
-		}
 	}
 	/* Ring doorbell */
 	ehea_update_rq1a(pr->qp, i - 1);
@@ -695,10 +692,8 @@ static int ehea_proc_rwqes(struct net_device *dev,
 
 					skb = netdev_alloc_skb(dev,
 							       EHEA_L_PKT_SIZE);
-					if (!skb) {
-						netdev_err(dev, "Not enough memory to allocate skb\n");
+					if (!skb)
 						break;
-					}
 				}
 				skb_copy_to_linear_data(skb, ((char *)cqe) + 64,
 						 cqe->num_bytes_transfered - 4);
@@ -730,7 +725,8 @@ static int ehea_proc_rwqes(struct net_device *dev,
 			processed_bytes += skb->len;
 
 			if (cqe->status & EHEA_CQE_VLAN_TAG_XTRACT)
-				__vlan_hwaccel_put_tag(skb, cqe->vlan_tag);
+				__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
+						       cqe->vlan_tag);
 
 			napi_gro_receive(&pr->napi, skb);
 		} else {
@@ -2115,7 +2111,7 @@ static int ehea_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	return NETDEV_TX_OK;
 }
 
-static int ehea_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
+static int ehea_vlan_rx_add_vid(struct net_device *dev, __be16 proto, u16 vid)
 {
 	struct ehea_port *port = netdev_priv(dev);
 	struct ehea_adapter *adapter = port->adapter;
@@ -2153,7 +2149,7 @@ out:
 	return err;
 }
 
-static int ehea_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
+static int ehea_vlan_rx_kill_vid(struct net_device *dev, __be16 proto, u16 vid)
 {
 	struct ehea_port *port = netdev_priv(dev);
 	struct ehea_adapter *adapter = port->adapter;
@@ -3025,12 +3021,12 @@ static struct ehea_port *ehea_setup_single_port(struct ehea_adapter *adapter,
 	dev->netdev_ops = &ehea_netdev_ops;
 	ehea_set_ethtool_ops(dev);
 
-	dev->hw_features = NETIF_F_SG | NETIF_F_TSO
-		      | NETIF_F_IP_CSUM | NETIF_F_HW_VLAN_TX;
-	dev->features = NETIF_F_SG | NETIF_F_FRAGLIST | NETIF_F_TSO
-		      | NETIF_F_HIGHDMA | NETIF_F_IP_CSUM | NETIF_F_HW_VLAN_TX
-		      | NETIF_F_HW_VLAN_RX | NETIF_F_HW_VLAN_FILTER
-		      | NETIF_F_RXCSUM;
+	dev->hw_features = NETIF_F_SG | NETIF_F_TSO |
+		      NETIF_F_IP_CSUM | NETIF_F_HW_VLAN_CTAG_TX;
+	dev->features = NETIF_F_SG | NETIF_F_FRAGLIST | NETIF_F_TSO |
+		      NETIF_F_HIGHDMA | NETIF_F_IP_CSUM |
+		      NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX |
+		      NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_RXCSUM;
 	dev->vlan_features = NETIF_F_SG | NETIF_F_TSO | NETIF_F_HIGHDMA |
 			NETIF_F_IP_CSUM;
 	dev->watchdog_timeo = EHEA_WATCH_DOG_TIMEOUT;
diff --git a/drivers/net/ethernet/ibm/emac/debug.c b/drivers/net/ethernet/ibm/emac/debug.c
index b16b4828b64d..a559f326bf63 100644
--- a/drivers/net/ethernet/ibm/emac/debug.c
+++ b/drivers/net/ethernet/ibm/emac/debug.c
@@ -245,7 +245,7 @@ static void emac_sysrq_handler(int key)
 
 static struct sysrq_key_op emac_sysrq_op = {
 	.handler = emac_sysrq_handler,
-	.help_msg = "emaC",
+	.help_msg = "emac(c)",
 	.action_msg = "Show EMAC(s) status",
 };
 
diff --git a/drivers/net/ethernet/ibm/emac/mal.c b/drivers/net/ethernet/ibm/emac/mal.c
index 1f7ecf57181e..610ed223d1db 100644
--- a/drivers/net/ethernet/ibm/emac/mal.c
+++ b/drivers/net/ethernet/ibm/emac/mal.c
@@ -637,17 +637,12 @@ static int mal_probe(struct platform_device *ofdev)
 	bd_size = sizeof(struct mal_descriptor) *
 		(NUM_TX_BUFF * mal->num_tx_chans +
 		 NUM_RX_BUFF * mal->num_rx_chans);
-	mal->bd_virt =
-		dma_alloc_coherent(&ofdev->dev, bd_size, &mal->bd_dma,
-				   GFP_KERNEL);
+	mal->bd_virt = dma_alloc_coherent(&ofdev->dev, bd_size, &mal->bd_dma,
+					  GFP_KERNEL | __GFP_ZERO);
 	if (mal->bd_virt == NULL) {
-		printk(KERN_ERR
-		       "mal%d: out of memory allocating RX/TX descriptors!\n",
-		       index);
 		err = -ENOMEM;
 		goto fail_unmap;
 	}
-	memset(mal->bd_virt, 0, bd_size);
 
 	for (i = 0; i < mal->num_tx_chans; ++i)
 		set_mal_dcrn(mal, MAL_TXCTPR(i), mal->bd_dma +
diff --git a/drivers/net/ethernet/ibm/ibmveth.c b/drivers/net/ethernet/ibm/ibmveth.c
index c859771a9902..70fd55968844 100644
--- a/drivers/net/ethernet/ibm/ibmveth.c
+++ b/drivers/net/ethernet/ibm/ibmveth.c
@@ -556,11 +556,9 @@ static int ibmveth_open(struct net_device *netdev)
 	adapter->rx_queue.queue_len = sizeof(struct ibmveth_rx_q_entry) *
 						rxq_entries;
 	adapter->rx_queue.queue_addr =
-	    dma_alloc_coherent(dev, adapter->rx_queue.queue_len,
-			       &adapter->rx_queue.queue_dma, GFP_KERNEL);
-
+		dma_alloc_coherent(dev, adapter->rx_queue.queue_len,
+				   &adapter->rx_queue.queue_dma, GFP_KERNEL);
 	if (!adapter->rx_queue.queue_addr) {
-		netdev_err(netdev, "unable to allocate rx queue pages\n");
 		rc = -ENOMEM;
 		goto err_out;
 	}
@@ -1324,7 +1322,7 @@ static const struct net_device_ops ibmveth_netdev_ops = {
 
 static int ibmveth_probe(struct vio_dev *dev, const struct vio_device_id *id)
 {
-	int rc, i;
+	int rc, i, mac_len;
 	struct net_device *netdev;
 	struct ibmveth_adapter *adapter;
 	unsigned char *mac_addr_p;
@@ -1334,11 +1332,19 @@ static int ibmveth_probe(struct vio_dev *dev, const struct vio_device_id *id)
 		dev->unit_address);
 
 	mac_addr_p = (unsigned char *)vio_get_attribute(dev, VETH_MAC_ADDR,
-							NULL);
+							&mac_len);
 	if (!mac_addr_p) {
 		dev_err(&dev->dev, "Can't find VETH_MAC_ADDR attribute\n");
 		return -EINVAL;
 	}
+	/* Workaround for old/broken pHyp */
+	if (mac_len == 8)
+		mac_addr_p += 2;
+	else if (mac_len != 6) {
+		dev_err(&dev->dev, "VETH_MAC_ADDR attribute wrong len %d\n",
+			mac_len);
+		return -EINVAL;
+	}
 
 	mcastFilterSize_p = (unsigned int *)vio_get_attribute(dev,
 						VETH_MCAST_FILTER_SIZE, NULL);
@@ -1363,17 +1369,6 @@ static int ibmveth_probe(struct vio_dev *dev, const struct vio_device_id *id)
 
 	netif_napi_add(netdev, &adapter->napi, ibmveth_poll, 16);
 
-	/*
-	 * Some older boxes running PHYP non-natively have an OF that returns
-	 * a 8-byte local-mac-address field (and the first 2 bytes have to be
-	 * ignored) while newer boxes' OF return a 6-byte field. Note that
-	 * IEEE 1275 specifies that local-mac-address must be a 6-byte field.
-	 * The RPA doc specifies that the first byte must be 10b, so we'll
-	 * just look for it to solve this 8 vs. 6 byte field issue
-	 */
-	if ((*mac_addr_p & 0x3) != 0x02)
-		mac_addr_p += 2;
-
 	adapter->mac_addr = 0;
 	memcpy(&adapter->mac_addr, mac_addr_p, 6);
 
diff --git a/drivers/net/ethernet/intel/e1000/e1000_ethtool.c b/drivers/net/ethernet/intel/e1000/e1000_ethtool.c
index ffd287196bf8..82a967c95598 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_ethtool.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_ethtool.c
@@ -1020,12 +1020,11 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 	txdr->size = txdr->count * sizeof(struct e1000_tx_desc);
 	txdr->size = ALIGN(txdr->size, 4096);
 	txdr->desc = dma_alloc_coherent(&pdev->dev, txdr->size, &txdr->dma,
-					GFP_KERNEL);
+					GFP_KERNEL | __GFP_ZERO);
 	if (!txdr->desc) {
 		ret_val = 2;
 		goto err_nomem;
 	}
-	memset(txdr->desc, 0, txdr->size);
 	txdr->next_to_use = txdr->next_to_clean = 0;
 
 	ew32(TDBAL, ((u64)txdr->dma & 0x00000000FFFFFFFF));
@@ -1079,12 +1078,11 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 
 	rxdr->size = rxdr->count * sizeof(struct e1000_rx_desc);
 	rxdr->desc = dma_alloc_coherent(&pdev->dev, rxdr->size, &rxdr->dma,
-					GFP_KERNEL);
+					GFP_KERNEL | __GFP_ZERO);
 	if (!rxdr->desc) {
 		ret_val = 6;
 		goto err_nomem;
 	}
-	memset(rxdr->desc, 0, rxdr->size);
 	rxdr->next_to_use = rxdr->next_to_clean = 0;
 
 	rctl = er32(RCTL);
diff --git a/drivers/net/ethernet/intel/e1000/e1000_main.c b/drivers/net/ethernet/intel/e1000/e1000_main.c
index 8502c625dbef..59ad007dd5aa 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_main.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_main.c
@@ -166,8 +166,10 @@ static void e1000_vlan_mode(struct net_device *netdev,
 			    netdev_features_t features);
 static void e1000_vlan_filter_on_off(struct e1000_adapter *adapter,
 				     bool filter_on);
-static int e1000_vlan_rx_add_vid(struct net_device *netdev, u16 vid);
-static int e1000_vlan_rx_kill_vid(struct net_device *netdev, u16 vid);
+static int e1000_vlan_rx_add_vid(struct net_device *netdev,
+				 __be16 proto, u16 vid);
+static int e1000_vlan_rx_kill_vid(struct net_device *netdev,
+				  __be16 proto, u16 vid);
 static void e1000_restore_vlan(struct e1000_adapter *adapter);
 
 #ifdef CONFIG_PM
@@ -333,7 +335,7 @@ static void e1000_update_mng_vlan(struct e1000_adapter *adapter)
 	if (!test_bit(vid, adapter->active_vlans)) {
 		if (hw->mng_cookie.status &
 		    E1000_MNG_DHCP_COOKIE_STATUS_VLAN_SUPPORT) {
-			e1000_vlan_rx_add_vid(netdev, vid);
+			e1000_vlan_rx_add_vid(netdev, htons(ETH_P_8021Q), vid);
 			adapter->mng_vlan_id = vid;
 		} else {
 			adapter->mng_vlan_id = E1000_MNG_VLAN_NONE;
@@ -341,7 +343,8 @@ static void e1000_update_mng_vlan(struct e1000_adapter *adapter)
 		if ((old_vid != (u16)E1000_MNG_VLAN_NONE) &&
 		    (vid != old_vid) &&
 		    !test_bit(old_vid, adapter->active_vlans))
-			e1000_vlan_rx_kill_vid(netdev, old_vid);
+			e1000_vlan_rx_kill_vid(netdev, htons(ETH_P_8021Q),
+					       old_vid);
 	} else {
 		adapter->mng_vlan_id = vid;
 	}
@@ -809,10 +812,10 @@ static netdev_features_t e1000_fix_features(struct net_device *netdev,
 	/* Since there is no support for separate Rx/Tx vlan accel
 	 * enable/disable make sure Tx flag is always in same state as Rx.
 	 */
-	if (features & NETIF_F_HW_VLAN_RX)
-		features |= NETIF_F_HW_VLAN_TX;
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
+		features |= NETIF_F_HW_VLAN_CTAG_TX;
 	else
-		features &= ~NETIF_F_HW_VLAN_TX;
+		features &= ~NETIF_F_HW_VLAN_CTAG_TX;
 
 	return features;
 }
@@ -823,7 +826,7 @@ static int e1000_set_features(struct net_device *netdev,
 	struct e1000_adapter *adapter = netdev_priv(netdev);
 	netdev_features_t changed = features ^ netdev->features;
 
-	if (changed & NETIF_F_HW_VLAN_RX)
+	if (changed & NETIF_F_HW_VLAN_CTAG_RX)
 		e1000_vlan_mode(netdev, features);
 
 	if (!(changed & (NETIF_F_RXCSUM | NETIF_F_RXALL)))
@@ -1058,9 +1061,9 @@ static int e1000_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (hw->mac_type >= e1000_82543) {
 		netdev->hw_features = NETIF_F_SG |
 				   NETIF_F_HW_CSUM |
-				   NETIF_F_HW_VLAN_RX;
-		netdev->features = NETIF_F_HW_VLAN_TX |
-				   NETIF_F_HW_VLAN_FILTER;
+				   NETIF_F_HW_VLAN_CTAG_RX;
+		netdev->features = NETIF_F_HW_VLAN_CTAG_TX |
+				   NETIF_F_HW_VLAN_CTAG_FILTER;
 	}
 
 	if ((hw->mac_type >= e1000_82544) &&
@@ -1457,7 +1460,8 @@ static int e1000_close(struct net_device *netdev)
 	if ((hw->mng_cookie.status &
 	     E1000_MNG_DHCP_COOKIE_STATUS_VLAN_SUPPORT) &&
 	    !test_bit(adapter->mng_vlan_id, adapter->active_vlans)) {
-		e1000_vlan_rx_kill_vid(netdev, adapter->mng_vlan_id);
+		e1000_vlan_rx_kill_vid(netdev, htons(ETH_P_8021Q),
+				       adapter->mng_vlan_id);
 	}
 
 	return 0;
@@ -1516,8 +1520,6 @@ static int e1000_setup_tx_resources(struct e1000_adapter *adapter,
 	if (!txdr->desc) {
 setup_tx_desc_die:
 		vfree(txdr->buffer_info);
-		e_err(probe, "Unable to allocate memory for the Tx descriptor "
-		      "ring\n");
 		return -ENOMEM;
 	}
 
@@ -1707,10 +1709,7 @@ static int e1000_setup_rx_resources(struct e1000_adapter *adapter,
 
 	rxdr->desc = dma_alloc_coherent(&pdev->dev, rxdr->size, &rxdr->dma,
 					GFP_KERNEL);
-
 	if (!rxdr->desc) {
-		e_err(probe, "Unable to allocate memory for the Rx descriptor "
-		      "ring\n");
 setup_rx_desc_die:
 		vfree(rxdr->buffer_info);
 		return -ENOMEM;
@@ -1729,8 +1728,6 @@ setup_rx_desc_die:
 		if (!rxdr->desc) {
 			dma_free_coherent(&pdev->dev, rxdr->size, olddesc,
 					  olddma);
-			e_err(probe, "Unable to allocate memory for the Rx "
-			      "descriptor ring\n");
 			goto setup_rx_desc_die;
 		}
 
@@ -4006,7 +4003,7 @@ static void e1000_receive_skb(struct e1000_adapter *adapter, u8 status,
 	if (status & E1000_RXD_STAT_VP) {
 		u16 vid = le16_to_cpu(vlan) & E1000_RXD_SPC_VLAN_MASK;
 
-		__vlan_hwaccel_put_tag(skb, vid);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
 	}
 	napi_gro_receive(&adapter->napi, skb);
 }
@@ -4792,7 +4789,7 @@ static void __e1000_vlan_mode(struct e1000_adapter *adapter,
 	u32 ctrl;
 
 	ctrl = er32(CTRL);
-	if (features & NETIF_F_HW_VLAN_RX) {
+	if (features & NETIF_F_HW_VLAN_CTAG_RX) {
 		/* enable VLAN tag insert/strip */
 		ctrl |= E1000_CTRL_VME;
 	} else {
@@ -4844,7 +4841,8 @@ static void e1000_vlan_mode(struct net_device *netdev,
 		e1000_irq_enable(adapter);
 }
 
-static int e1000_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
+static int e1000_vlan_rx_add_vid(struct net_device *netdev,
+				 __be16 proto, u16 vid)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
@@ -4869,7 +4867,8 @@ static int e1000_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
 	return 0;
 }
 
-static int e1000_vlan_rx_kill_vid(struct net_device *netdev, u16 vid)
+static int e1000_vlan_rx_kill_vid(struct net_device *netdev,
+				  __be16 proto, u16 vid)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
@@ -4903,7 +4902,7 @@ static void e1000_restore_vlan(struct e1000_adapter *adapter)
 
 	e1000_vlan_filter_on_off(adapter, true);
 	for_each_set_bit(vid, adapter->active_vlans, VLAN_N_VID)
-		e1000_vlan_rx_add_vid(adapter->netdev, vid);
+		e1000_vlan_rx_add_vid(adapter->netdev, htons(ETH_P_8021Q), vid);
 }
 
 int e1000_set_spd_dplx(struct e1000_adapter *adapter, u32 spd, u8 dplx)
diff --git a/drivers/net/ethernet/intel/e1000e/80003es2lan.c b/drivers/net/ethernet/intel/e1000e/80003es2lan.c
index e0991388664c..b71c8502a2b3 100644
--- a/drivers/net/ethernet/intel/e1000e/80003es2lan.c
+++ b/drivers/net/ethernet/intel/e1000e/80003es2lan.c
@@ -37,7 +37,9 @@
  * "index + 5".
  */
 static const u16 e1000_gg82563_cable_length_table[] = {
-	 0, 60, 115, 150, 150, 60, 115, 150, 180, 180, 0xFF };
+	0, 60, 115, 150, 150, 60, 115, 150, 180, 180, 0xFF
+};
+
 #define GG82563_CABLE_LENGTH_TABLE_SIZE \
 		ARRAY_SIZE(e1000_gg82563_cable_length_table)
 
@@ -116,7 +118,7 @@ static s32 e1000_init_nvm_params_80003es2lan(struct e1000_hw *hw)
 	nvm->type = e1000_nvm_eeprom_spi;
 
 	size = (u16)((eecd & E1000_EECD_SIZE_EX_MASK) >>
-			  E1000_EECD_SIZE_EX_SHIFT);
+		     E1000_EECD_SIZE_EX_SHIFT);
 
 	/* Added to a constant, "size" becomes the left-shift value
 	 * for setting word_size.
@@ -393,7 +395,7 @@ static s32 e1000_read_phy_reg_gg82563_80003es2lan(struct e1000_hw *hw,
 		 * before the device has completed the "Page Select" MDI
 		 * transaction.  So we wait 200us after each MDI command...
 		 */
-		udelay(200);
+		usleep_range(200, 400);
 
 		/* ...and verify the command was successful. */
 		ret_val = e1000e_read_phy_reg_mdic(hw, page_select, &temp);
@@ -403,17 +405,17 @@ static s32 e1000_read_phy_reg_gg82563_80003es2lan(struct e1000_hw *hw,
 			return -E1000_ERR_PHY;
 		}
 
-		udelay(200);
+		usleep_range(200, 400);
 
 		ret_val = e1000e_read_phy_reg_mdic(hw,
-		                                  MAX_PHY_REG_ADDRESS & offset,
-		                                  data);
+						   MAX_PHY_REG_ADDRESS & offset,
+						   data);
 
-		udelay(200);
+		usleep_range(200, 400);
 	} else {
 		ret_val = e1000e_read_phy_reg_mdic(hw,
-		                                  MAX_PHY_REG_ADDRESS & offset,
-		                                  data);
+						   MAX_PHY_REG_ADDRESS & offset,
+						   data);
 	}
 
 	e1000_release_phy_80003es2lan(hw);
@@ -462,7 +464,7 @@ static s32 e1000_write_phy_reg_gg82563_80003es2lan(struct e1000_hw *hw,
 		 * before the device has completed the "Page Select" MDI
 		 * transaction.  So we wait 200us after each MDI command...
 		 */
-		udelay(200);
+		usleep_range(200, 400);
 
 		/* ...and verify the command was successful. */
 		ret_val = e1000e_read_phy_reg_mdic(hw, page_select, &temp);
@@ -472,17 +474,17 @@ static s32 e1000_write_phy_reg_gg82563_80003es2lan(struct e1000_hw *hw,
 			return -E1000_ERR_PHY;
 		}
 
-		udelay(200);
+		usleep_range(200, 400);
 
 		ret_val = e1000e_write_phy_reg_mdic(hw,
-		                                  MAX_PHY_REG_ADDRESS & offset,
-		                                  data);
+						    MAX_PHY_REG_ADDRESS &
+						    offset, data);
 
-		udelay(200);
+		usleep_range(200, 400);
 	} else {
 		ret_val = e1000e_write_phy_reg_mdic(hw,
-		                                  MAX_PHY_REG_ADDRESS & offset,
-		                                  data);
+						    MAX_PHY_REG_ADDRESS &
+						    offset, data);
 	}
 
 	e1000_release_phy_80003es2lan(hw);
@@ -580,7 +582,7 @@ static s32 e1000_phy_force_speed_duplex_80003es2lan(struct e1000_hw *hw)
 		e_dbg("Waiting for forced speed/duplex link on GG82563 phy.\n");
 
 		ret_val = e1000e_phy_has_link_generic(hw, PHY_FORCE_LIMIT,
-						     100000, &link);
+						      100000, &link);
 		if (ret_val)
 			return ret_val;
 
@@ -595,7 +597,7 @@ static s32 e1000_phy_force_speed_duplex_80003es2lan(struct e1000_hw *hw)
 
 		/* Try once more */
 		ret_val = e1000e_phy_has_link_generic(hw, PHY_FORCE_LIMIT,
-						     100000, &link);
+						      100000, &link);
 		if (ret_val)
 			return ret_val;
 	}
@@ -666,14 +668,12 @@ static s32 e1000_get_link_up_info_80003es2lan(struct e1000_hw *hw, u16 *speed,
 	s32 ret_val;
 
 	if (hw->phy.media_type == e1000_media_type_copper) {
-		ret_val = e1000e_get_speed_and_duplex_copper(hw,
-								    speed,
-								    duplex);
+		ret_val = e1000e_get_speed_and_duplex_copper(hw, speed, duplex);
 		hw->phy.ops.cfg_on_link_up(hw);
 	} else {
 		ret_val = e1000e_get_speed_and_duplex_fiber_serdes(hw,
-								  speed,
-								  duplex);
+								   speed,
+								   duplex);
 	}
 
 	return ret_val;
@@ -754,9 +754,9 @@ static s32 e1000_init_hw_80003es2lan(struct e1000_hw *hw)
 
 	/* Initialize identification LED */
 	ret_val = mac->ops.id_led_init(hw);
+	/* An error is not fatal and we should not stop init due to this */
 	if (ret_val)
 		e_dbg("Error initializing identification LED\n");
-		/* This is not fatal and we should not stop init due to this */
 
 	/* Disabling VLAN filtering */
 	e_dbg("Initializing the IEEE VLAN\n");
@@ -784,14 +784,14 @@ static s32 e1000_init_hw_80003es2lan(struct e1000_hw *hw)
 
 	/* Set the transmit descriptor write-back policy */
 	reg_data = er32(TXDCTL(0));
-	reg_data = (reg_data & ~E1000_TXDCTL_WTHRESH) |
-		   E1000_TXDCTL_FULL_TX_DESC_WB | E1000_TXDCTL_COUNT_DESC;
+	reg_data = ((reg_data & ~E1000_TXDCTL_WTHRESH) |
+		    E1000_TXDCTL_FULL_TX_DESC_WB | E1000_TXDCTL_COUNT_DESC);
 	ew32(TXDCTL(0), reg_data);
 
 	/* ...for both queues. */
 	reg_data = er32(TXDCTL(1));
-	reg_data = (reg_data & ~E1000_TXDCTL_WTHRESH) |
-		   E1000_TXDCTL_FULL_TX_DESC_WB | E1000_TXDCTL_COUNT_DESC;
+	reg_data = ((reg_data & ~E1000_TXDCTL_WTHRESH) |
+		    E1000_TXDCTL_FULL_TX_DESC_WB | E1000_TXDCTL_COUNT_DESC);
 	ew32(TXDCTL(1), reg_data);
 
 	/* Enable retransmit on late collisions */
@@ -818,13 +818,12 @@ static s32 e1000_init_hw_80003es2lan(struct e1000_hw *hw)
 	/* default to true to enable the MDIC W/A */
 	hw->dev_spec.e80003es2lan.mdic_wa_enable = true;
 
-	ret_val = e1000_read_kmrn_reg_80003es2lan(hw,
-	                              E1000_KMRNCTRLSTA_OFFSET >>
-	                              E1000_KMRNCTRLSTA_OFFSET_SHIFT,
-	                              &i);
+	ret_val =
+	    e1000_read_kmrn_reg_80003es2lan(hw, E1000_KMRNCTRLSTA_OFFSET >>
+					    E1000_KMRNCTRLSTA_OFFSET_SHIFT, &i);
 	if (!ret_val) {
 		if ((i & E1000_KMRNCTRLSTA_OPMODE_MASK) ==
-		     E1000_KMRNCTRLSTA_OPMODE_INBAND_MDIO)
+		    E1000_KMRNCTRLSTA_OPMODE_INBAND_MDIO)
 			hw->dev_spec.e80003es2lan.mdic_wa_enable = false;
 	}
 
@@ -891,7 +890,7 @@ static s32 e1000_copper_link_setup_gg82563_80003es2lan(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
-	u32 ctrl_ext;
+	u32 reg;
 	u16 data;
 
 	ret_val = e1e_rphy(hw, GG82563_PHY_MAC_SPEC_CTRL, &data);
@@ -954,22 +953,19 @@ static s32 e1000_copper_link_setup_gg82563_80003es2lan(struct e1000_hw *hw)
 	}
 
 	/* Bypass Rx and Tx FIFO's */
-	ret_val = e1000_write_kmrn_reg_80003es2lan(hw,
-					E1000_KMRNCTRLSTA_OFFSET_FIFO_CTRL,
-					E1000_KMRNCTRLSTA_FIFO_CTRL_RX_BYPASS |
-					E1000_KMRNCTRLSTA_FIFO_CTRL_TX_BYPASS);
+	reg = E1000_KMRNCTRLSTA_OFFSET_FIFO_CTRL;
+	data = (E1000_KMRNCTRLSTA_FIFO_CTRL_RX_BYPASS |
+		E1000_KMRNCTRLSTA_FIFO_CTRL_TX_BYPASS);
+	ret_val = e1000_write_kmrn_reg_80003es2lan(hw, reg, data);
 	if (ret_val)
 		return ret_val;
 
-	ret_val = e1000_read_kmrn_reg_80003es2lan(hw,
-				       E1000_KMRNCTRLSTA_OFFSET_MAC2PHY_OPMODE,
-				       &data);
+	reg = E1000_KMRNCTRLSTA_OFFSET_MAC2PHY_OPMODE;
+	ret_val = e1000_read_kmrn_reg_80003es2lan(hw, reg, &data);
 	if (ret_val)
 		return ret_val;
 	data |= E1000_KMRNCTRLSTA_OPMODE_E_IDLE;
-	ret_val = e1000_write_kmrn_reg_80003es2lan(hw,
-					E1000_KMRNCTRLSTA_OFFSET_MAC2PHY_OPMODE,
-					data);
+	ret_val = e1000_write_kmrn_reg_80003es2lan(hw, reg, data);
 	if (ret_val)
 		return ret_val;
 
@@ -982,9 +978,9 @@ static s32 e1000_copper_link_setup_gg82563_80003es2lan(struct e1000_hw *hw)
 	if (ret_val)
 		return ret_val;
 
-	ctrl_ext = er32(CTRL_EXT);
-	ctrl_ext &= ~(E1000_CTRL_EXT_LINK_MODE_MASK);
-	ew32(CTRL_EXT, ctrl_ext);
+	reg = er32(CTRL_EXT);
+	reg &= ~E1000_CTRL_EXT_LINK_MODE_MASK;
+	ew32(CTRL_EXT, reg);
 
 	ret_val = e1e_rphy(hw, GG82563_PHY_PWR_MGMT_CTRL, &data);
 	if (ret_val)
@@ -1049,27 +1045,29 @@ static s32 e1000_setup_copper_link_80003es2lan(struct e1000_hw *hw)
 	 * polling the phy; this fixes erroneous timeouts at 10Mbps.
 	 */
 	ret_val = e1000_write_kmrn_reg_80003es2lan(hw, GG82563_REG(0x34, 4),
-	                                           0xFFFF);
+						   0xFFFF);
 	if (ret_val)
 		return ret_val;
 	ret_val = e1000_read_kmrn_reg_80003es2lan(hw, GG82563_REG(0x34, 9),
-	                                          &reg_data);
+						  &reg_data);
 	if (ret_val)
 		return ret_val;
 	reg_data |= 0x3F;
 	ret_val = e1000_write_kmrn_reg_80003es2lan(hw, GG82563_REG(0x34, 9),
-	                                           reg_data);
+						   reg_data);
 	if (ret_val)
 		return ret_val;
-	ret_val = e1000_read_kmrn_reg_80003es2lan(hw,
-				      E1000_KMRNCTRLSTA_OFFSET_INB_CTRL,
-				      &reg_data);
+	ret_val =
+	    e1000_read_kmrn_reg_80003es2lan(hw,
+					    E1000_KMRNCTRLSTA_OFFSET_INB_CTRL,
+					    &reg_data);
 	if (ret_val)
 		return ret_val;
 	reg_data |= E1000_KMRNCTRLSTA_INB_CTRL_DIS_PADDING;
-	ret_val = e1000_write_kmrn_reg_80003es2lan(hw,
-					E1000_KMRNCTRLSTA_OFFSET_INB_CTRL,
-					reg_data);
+	ret_val =
+	    e1000_write_kmrn_reg_80003es2lan(hw,
+					     E1000_KMRNCTRLSTA_OFFSET_INB_CTRL,
+					     reg_data);
 	if (ret_val)
 		return ret_val;
 
@@ -1096,7 +1094,7 @@ static s32 e1000_cfg_on_link_up_80003es2lan(struct e1000_hw *hw)
 
 	if (hw->phy.media_type == e1000_media_type_copper) {
 		ret_val = e1000e_get_speed_and_duplex_copper(hw, &speed,
-		                                             &duplex);
+							     &duplex);
 		if (ret_val)
 			return ret_val;
 
@@ -1125,9 +1123,10 @@ static s32 e1000_cfg_kmrn_10_100_80003es2lan(struct e1000_hw *hw, u16 duplex)
 	u16 reg_data, reg_data2;
 
 	reg_data = E1000_KMRNCTRLSTA_HD_CTRL_10_100_DEFAULT;
-	ret_val = e1000_write_kmrn_reg_80003es2lan(hw,
-	                               E1000_KMRNCTRLSTA_OFFSET_HD_CTRL,
-	                               reg_data);
+	ret_val =
+	    e1000_write_kmrn_reg_80003es2lan(hw,
+					     E1000_KMRNCTRLSTA_OFFSET_HD_CTRL,
+					     reg_data);
 	if (ret_val)
 		return ret_val;
 
@@ -1171,9 +1170,10 @@ static s32 e1000_cfg_kmrn_1000_80003es2lan(struct e1000_hw *hw)
 	u32 i = 0;
 
 	reg_data = E1000_KMRNCTRLSTA_HD_CTRL_1000_DEFAULT;
-	ret_val = e1000_write_kmrn_reg_80003es2lan(hw,
-	                               E1000_KMRNCTRLSTA_OFFSET_HD_CTRL,
-	                               reg_data);
+	ret_val =
+	    e1000_write_kmrn_reg_80003es2lan(hw,
+					     E1000_KMRNCTRLSTA_OFFSET_HD_CTRL,
+					     reg_data);
 	if (ret_val)
 		return ret_val;
 
@@ -1220,7 +1220,7 @@ static s32 e1000_read_kmrn_reg_80003es2lan(struct e1000_hw *hw, u32 offset,
 		return ret_val;
 
 	kmrnctrlsta = ((offset << E1000_KMRNCTRLSTA_OFFSET_SHIFT) &
-	               E1000_KMRNCTRLSTA_OFFSET) | E1000_KMRNCTRLSTA_REN;
+		       E1000_KMRNCTRLSTA_OFFSET) | E1000_KMRNCTRLSTA_REN;
 	ew32(KMRNCTRLSTA, kmrnctrlsta);
 	e1e_flush();
 
@@ -1255,7 +1255,7 @@ static s32 e1000_write_kmrn_reg_80003es2lan(struct e1000_hw *hw, u32 offset,
 		return ret_val;
 
 	kmrnctrlsta = ((offset << E1000_KMRNCTRLSTA_OFFSET_SHIFT) &
-	               E1000_KMRNCTRLSTA_OFFSET) | data;
+		       E1000_KMRNCTRLSTA_OFFSET) | data;
 	ew32(KMRNCTRLSTA, kmrnctrlsta);
 	e1e_flush();
 
@@ -1419,4 +1419,3 @@ const struct e1000_info e1000_es2_info = {
 	.phy_ops		= &es2_phy_ops,
 	.nvm_ops		= &es2_nvm_ops,
 };
-
diff --git a/drivers/net/ethernet/intel/e1000e/82571.c b/drivers/net/ethernet/intel/e1000e/82571.c
index 2faffbde179e..7380442a3829 100644
--- a/drivers/net/ethernet/intel/e1000e/82571.c
+++ b/drivers/net/ethernet/intel/e1000e/82571.c
@@ -184,7 +184,7 @@ static s32 e1000_init_nvm_params_82571(struct e1000_hw *hw)
 	default:
 		nvm->type = e1000_nvm_eeprom_spi;
 		size = (u16)((eecd & E1000_EECD_SIZE_EX_MASK) >>
-				  E1000_EECD_SIZE_EX_SHIFT);
+			     E1000_EECD_SIZE_EX_SHIFT);
 		/* Added to a constant, "size" becomes the left-shift value
 		 * for setting word_size.
 		 */
@@ -437,7 +437,7 @@ static s32 e1000_get_phy_id_82571(struct e1000_hw *hw)
 			return ret_val;
 
 		phy->id = (u32)(phy_id << 16);
-		udelay(20);
+		usleep_range(20, 40);
 		ret_val = e1e_rphy(hw, MII_PHYSID2, &phy_id);
 		if (ret_val)
 			return ret_val;
@@ -482,7 +482,7 @@ static s32 e1000_get_hw_semaphore_82571(struct e1000_hw *hw)
 		if (!(swsm & E1000_SWSM_SMBI))
 			break;
 
-		udelay(50);
+		usleep_range(50, 100);
 		i++;
 	}
 
@@ -499,7 +499,7 @@ static s32 e1000_get_hw_semaphore_82571(struct e1000_hw *hw)
 		if (er32(SWSM) & E1000_SWSM_SWESMBI)
 			break;
 
-		udelay(50);
+		usleep_range(50, 100);
 	}
 
 	if (i == fw_timeout) {
@@ -526,6 +526,7 @@ static void e1000_put_hw_semaphore_82571(struct e1000_hw *hw)
 	swsm &= ~(E1000_SWSM_SMBI | E1000_SWSM_SWESMBI);
 	ew32(SWSM, swsm);
 }
+
 /**
  *  e1000_get_hw_semaphore_82573 - Acquire hardware semaphore
  *  @hw: pointer to the HW structure
@@ -846,9 +847,9 @@ static s32 e1000_write_nvm_eewr_82571(struct e1000_hw *hw, u16 offset,
 	}
 
 	for (i = 0; i < words; i++) {
-		eewr = (data[i] << E1000_NVM_RW_REG_DATA) |
-		       ((offset+i) << E1000_NVM_RW_ADDR_SHIFT) |
-		       E1000_NVM_RW_REG_START;
+		eewr = ((data[i] << E1000_NVM_RW_REG_DATA) |
+			((offset + i) << E1000_NVM_RW_ADDR_SHIFT) |
+			E1000_NVM_RW_REG_START);
 
 		ret_val = e1000e_poll_eerd_eewr_done(hw, E1000_NVM_POLL_WRITE);
 		if (ret_val)
@@ -875,8 +876,7 @@ static s32 e1000_get_cfg_done_82571(struct e1000_hw *hw)
 	s32 timeout = PHY_CFG_TIMEOUT;
 
 	while (timeout) {
-		if (er32(EEMNGCTL) &
-		    E1000_NVM_CFG_DONE_PORT_0)
+		if (er32(EEMNGCTL) & E1000_NVM_CFG_DONE_PORT_0)
 			break;
 		usleep_range(1000, 2000);
 		timeout--;
@@ -1022,7 +1022,7 @@ static s32 e1000_reset_hw_82571(struct e1000_hw *hw)
 	}
 
 	if (hw->nvm.type == e1000_nvm_flash_hw) {
-		udelay(10);
+		usleep_range(10, 20);
 		ctrl_ext = er32(CTRL_EXT);
 		ctrl_ext |= E1000_CTRL_EXT_EE_RST;
 		ew32(CTRL_EXT, ctrl_ext);
@@ -1095,9 +1095,9 @@ static s32 e1000_init_hw_82571(struct e1000_hw *hw)
 
 	/* Initialize identification LED */
 	ret_val = mac->ops.id_led_init(hw);
+	/* An error is not fatal and we should not stop init due to this */
 	if (ret_val)
 		e_dbg("Error initializing identification LED\n");
-		/* This is not fatal and we should not stop init due to this */
 
 	/* Disabling VLAN filtering */
 	e_dbg("Initializing the IEEE VLAN\n");
@@ -1122,9 +1122,8 @@ static s32 e1000_init_hw_82571(struct e1000_hw *hw)
 
 	/* Set the transmit descriptor write-back policy */
 	reg_data = er32(TXDCTL(0));
-	reg_data = (reg_data & ~E1000_TXDCTL_WTHRESH) |
-		   E1000_TXDCTL_FULL_TX_DESC_WB |
-		   E1000_TXDCTL_COUNT_DESC;
+	reg_data = ((reg_data & ~E1000_TXDCTL_WTHRESH) |
+		    E1000_TXDCTL_FULL_TX_DESC_WB | E1000_TXDCTL_COUNT_DESC);
 	ew32(TXDCTL(0), reg_data);
 
 	/* ...for both queues. */
@@ -1140,9 +1139,9 @@ static s32 e1000_init_hw_82571(struct e1000_hw *hw)
 		break;
 	default:
 		reg_data = er32(TXDCTL(1));
-		reg_data = (reg_data & ~E1000_TXDCTL_WTHRESH) |
-			   E1000_TXDCTL_FULL_TX_DESC_WB |
-			   E1000_TXDCTL_COUNT_DESC;
+		reg_data = ((reg_data & ~E1000_TXDCTL_WTHRESH) |
+			    E1000_TXDCTL_FULL_TX_DESC_WB |
+			    E1000_TXDCTL_COUNT_DESC);
 		ew32(TXDCTL(1), reg_data);
 		break;
 	}
@@ -1530,7 +1529,7 @@ static s32 e1000_check_for_serdes_link_82571(struct e1000_hw *hw)
 	status = er32(STATUS);
 	er32(RXCW);
 	/* SYNCH bit and IV bit are sticky */
-	udelay(10);
+	usleep_range(10, 20);
 	rxcw = er32(RXCW);
 
 	if ((rxcw & E1000_RXCW_SYNCH) && !(rxcw & E1000_RXCW_IV)) {
@@ -1633,7 +1632,7 @@ static s32 e1000_check_for_serdes_link_82571(struct e1000_hw *hw)
 			 * the IV bit and restart Autoneg
 			 */
 			for (i = 0; i < AN_RETRY_COUNT; i++) {
-				udelay(10);
+				usleep_range(10, 20);
 				rxcw = er32(RXCW);
 				if ((rxcw & E1000_RXCW_SYNCH) &&
 				    (rxcw & E1000_RXCW_C))
@@ -2066,4 +2065,3 @@ const struct e1000_info e1000_82583_info = {
 	.phy_ops		= &e82_phy_ops_bm,
 	.nvm_ops		= &e82571_nvm_ops,
 };
-
diff --git a/drivers/net/ethernet/intel/e1000e/82571.h b/drivers/net/ethernet/intel/e1000e/82571.h
index 85cb1a3b7cd4..08e24dc3dc0e 100644
--- a/drivers/net/ethernet/intel/e1000e/82571.h
+++ b/drivers/net/ethernet/intel/e1000e/82571.h
@@ -44,6 +44,8 @@
 #define E1000_EIAC_82574	0x000DC	/* Ext. Interrupt Auto Clear - RW */
 #define E1000_EIAC_MASK_82574	0x01F00000
 
+#define E1000_IVAR_INT_ALLOC_VALID	0x8
+
 /* Manageability Operation Mode mask */
 #define E1000_NVM_INIT_CTRL2_MNGM	0x6000
 
diff --git a/drivers/net/ethernet/intel/e1000e/defines.h b/drivers/net/ethernet/intel/e1000e/defines.h
index fc3a4fe1ac71..351c94a0cf74 100644
--- a/drivers/net/ethernet/intel/e1000e/defines.h
+++ b/drivers/net/ethernet/intel/e1000e/defines.h
@@ -66,7 +66,7 @@
 #define E1000_CTRL_EXT_LINK_MODE_PCIE_SERDES  0x00C00000
 #define E1000_CTRL_EXT_EIAME          0x01000000
 #define E1000_CTRL_EXT_DRV_LOAD       0x10000000 /* Driver loaded bit for FW */
-#define E1000_CTRL_EXT_IAME           0x08000000 /* Interrupt acknowledge Auto-mask */
+#define E1000_CTRL_EXT_IAME		0x08000000 /* Int ACK Auto-mask */
 #define E1000_CTRL_EXT_PBA_CLR        0x80000000 /* PBA Clear */
 #define E1000_CTRL_EXT_LSECCK         0x00001000
 #define E1000_CTRL_EXT_PHYPDEN        0x00100000
@@ -216,6 +216,8 @@
 #define E1000_CTRL_MEHE     0x00080000  /* Memory Error Handling Enable */
 #define E1000_CTRL_SWDPIN0  0x00040000  /* SWDPIN 0 value */
 #define E1000_CTRL_SWDPIN1  0x00080000  /* SWDPIN 1 value */
+#define E1000_CTRL_ADVD3WUC 0x00100000  /* D3 WUC */
+#define E1000_CTRL_EN_PHY_PWR_MGMT 0x00200000 /* PHY PM enable */
 #define E1000_CTRL_SWDPIO0  0x00400000  /* SWDPIN 0 Input or output */
 #define E1000_CTRL_RST      0x04000000  /* Global reset */
 #define E1000_CTRL_RFCE     0x08000000  /* Receive Flow Control enable */
@@ -234,17 +236,17 @@
 #define E1000_STATUS_FUNC_SHIFT 2
 #define E1000_STATUS_FUNC_1     0x00000004      /* Function 1 */
 #define E1000_STATUS_TXOFF      0x00000010      /* transmission paused */
+#define E1000_STATUS_SPEED_MASK 0x000000C0
 #define E1000_STATUS_SPEED_10   0x00000000      /* Speed 10Mb/s */
 #define E1000_STATUS_SPEED_100  0x00000040      /* Speed 100Mb/s */
 #define E1000_STATUS_SPEED_1000 0x00000080      /* Speed 1000Mb/s */
 #define E1000_STATUS_LAN_INIT_DONE 0x00000200   /* Lan Init Completion by NVM */
 #define E1000_STATUS_PHYRA      0x00000400      /* PHY Reset Asserted */
-#define E1000_STATUS_GIO_MASTER_ENABLE 0x00080000 /* Status of Master requests. */
+#define E1000_STATUS_GIO_MASTER_ENABLE	0x00080000	/* Master Req status */
 
 #define HALF_DUPLEX 1
 #define FULL_DUPLEX 2
 
-
 #define ADVERTISE_10_HALF                 0x0001
 #define ADVERTISE_10_FULL                 0x0002
 #define ADVERTISE_100_HALF                0x0004
@@ -311,6 +313,7 @@
 
 /* SerDes Control */
 #define E1000_SCTL_DISABLE_SERDES_LOOPBACK 0x0400
+#define E1000_SCTL_ENABLE_SERDES_LOOPBACK	0x0410
 
 /* Receive Checksum Control */
 #define E1000_RXCSUM_TUOFL     0x00000200   /* TCP / UDP checksum offload */
@@ -400,7 +403,8 @@
 #define E1000_ICR_RXDMT0        0x00000010 /* Rx desc min. threshold (0) */
 #define E1000_ICR_RXT0          0x00000080 /* Rx timer intr (ring 0) */
 #define E1000_ICR_ECCER         0x00400000 /* Uncorrectable ECC Error */
-#define E1000_ICR_INT_ASSERTED  0x80000000 /* If this bit asserted, the driver should claim the interrupt */
+/* If this bit asserted, the driver should claim the interrupt */
+#define E1000_ICR_INT_ASSERTED	0x80000000
 #define E1000_ICR_RXQ0          0x00100000 /* Rx Queue 0 Interrupt */
 #define E1000_ICR_RXQ1          0x00200000 /* Rx Queue 1 Interrupt */
 #define E1000_ICR_TXQ0          0x00400000 /* Tx Queue 0 Interrupt */
@@ -583,13 +587,13 @@
 #define E1000_EECD_SEC1VAL   0x00400000 /* Sector One Valid */
 #define E1000_EECD_SEC1VAL_VALID_MASK (E1000_EECD_AUTO_RD | E1000_EECD_PRES)
 
-#define E1000_NVM_RW_REG_DATA   16   /* Offset to data in NVM read/write registers */
-#define E1000_NVM_RW_REG_DONE   2    /* Offset to READ/WRITE done bit */
-#define E1000_NVM_RW_REG_START  1    /* Start operation */
-#define E1000_NVM_RW_ADDR_SHIFT 2    /* Shift to the address bits */
-#define E1000_NVM_POLL_WRITE    1    /* Flag for polling for write complete */
-#define E1000_NVM_POLL_READ     0    /* Flag for polling for read complete */
-#define E1000_FLASH_UPDATES  2000
+#define E1000_NVM_RW_REG_DATA	16	/* Offset to data in NVM r/w regs */
+#define E1000_NVM_RW_REG_DONE	2	/* Offset to READ/WRITE done bit */
+#define E1000_NVM_RW_REG_START	1	/* Start operation */
+#define E1000_NVM_RW_ADDR_SHIFT	2	/* Shift to the address bits */
+#define E1000_NVM_POLL_WRITE	1	/* Flag for polling write complete */
+#define E1000_NVM_POLL_READ	0	/* Flag for polling read complete */
+#define E1000_FLASH_UPDATES	2000
 
 /* NVM Word Offsets */
 #define NVM_COMPAT                 0x0003
@@ -785,6 +789,7 @@
 	GG82563_REG(194, 18) /* Inband Control */
 
 /* MDI Control */
+#define E1000_MDIC_REG_MASK	0x001F0000
 #define E1000_MDIC_REG_SHIFT 16
 #define E1000_MDIC_PHY_SHIFT 21
 #define E1000_MDIC_OP_WRITE  0x04000000
diff --git a/drivers/net/ethernet/intel/e1000e/e1000.h b/drivers/net/ethernet/intel/e1000e/e1000.h
index fcc758138b8a..ffbc08f56c40 100644
--- a/drivers/net/ethernet/intel/e1000e/e1000.h
+++ b/drivers/net/ethernet/intel/e1000e/e1000.h
@@ -46,6 +46,7 @@
 #include <linux/ptp_clock_kernel.h>
 #include <linux/ptp_classify.h>
 #include <linux/mii.h>
+#include <linux/mdio.h>
 #include "hw.h"
 
 struct e1000_info;
@@ -61,7 +62,6 @@ struct e1000_info;
 #define e_notice(format, arg...) \
 	netdev_notice(adapter->netdev, format, ## arg)
 
-
 /* Interrupt modes, as used by the IntMode parameter */
 #define E1000E_INT_MODE_LEGACY		0
 #define E1000E_INT_MODE_MSI		1
@@ -239,9 +239,8 @@ struct e1000_adapter {
 	u16 tx_itr;
 	u16 rx_itr;
 
-	/* Tx */
-	struct e1000_ring *tx_ring /* One per active queue */
-						____cacheline_aligned_in_smp;
+	/* Tx - one ring per active queue */
+	struct e1000_ring *tx_ring ____cacheline_aligned_in_smp;
 	u32 tx_fifo_limit;
 
 	struct napi_struct napi;
@@ -352,6 +351,8 @@ struct e1000_adapter {
 	struct timecounter tc;
 	struct ptp_clock *ptp_clock;
 	struct ptp_clock_info ptp_clock_info;
+
+	u16 eee_advert;
 };
 
 struct e1000_info {
@@ -487,8 +488,8 @@ extern int e1000e_setup_tx_resources(struct e1000_ring *ring);
 extern void e1000e_free_rx_resources(struct e1000_ring *ring);
 extern void e1000e_free_tx_resources(struct e1000_ring *ring);
 extern struct rtnl_link_stats64 *e1000e_get_stats64(struct net_device *netdev,
-                                                    struct rtnl_link_stats64
-                                                    *stats);
+						    struct rtnl_link_stats64
+						    *stats);
 extern void e1000e_set_interrupt_capability(struct e1000_adapter *adapter);
 extern void e1000e_reset_interrupt_capability(struct e1000_adapter *adapter);
 extern void e1000e_get_hw_control(struct e1000_adapter *adapter);
@@ -558,12 +559,14 @@ static inline s32 e1000e_update_nvm_checksum(struct e1000_hw *hw)
 	return hw->nvm.ops.update(hw);
 }
 
-static inline s32 e1000_read_nvm(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
+static inline s32 e1000_read_nvm(struct e1000_hw *hw, u16 offset, u16 words,
+				 u16 *data)
 {
 	return hw->nvm.ops.read(hw, offset, words, data);
 }
 
-static inline s32 e1000_write_nvm(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
+static inline s32 e1000_write_nvm(struct e1000_hw *hw, u16 offset, u16 words,
+				  u16 *data)
 {
 	return hw->nvm.ops.write(hw, offset, words, data);
 }
diff --git a/drivers/net/ethernet/intel/e1000e/ethtool.c b/drivers/net/ethernet/intel/e1000e/ethtool.c
index f91a8f3f9d48..7c8ca658d553 100644
--- a/drivers/net/ethernet/intel/e1000e/ethtool.c
+++ b/drivers/net/ethernet/intel/e1000e/ethtool.c
@@ -35,12 +35,11 @@
 #include <linux/slab.h>
 #include <linux/delay.h>
 #include <linux/vmalloc.h>
-#include <linux/mdio.h>
 #include <linux/pm_runtime.h>
 
 #include "e1000.h"
 
-enum {NETDEV_STATS, E1000_STATS};
+enum { NETDEV_STATS, E1000_STATS };
 
 struct e1000_stats {
 	char stat_string[ETH_GSTRING_LEN];
@@ -121,6 +120,7 @@ static const char e1000_gstrings_test[][ETH_GSTRING_LEN] = {
 	"Interrupt test (offline)", "Loopback test  (offline)",
 	"Link test   (on/offline)"
 };
+
 #define E1000_TEST_LEN ARRAY_SIZE(e1000_gstrings_test)
 
 static int e1000_get_settings(struct net_device *netdev,
@@ -197,8 +197,7 @@ static int e1000_get_settings(struct net_device *netdev,
 	/* MDI-X => 2; MDI =>1; Invalid =>0 */
 	if ((hw->phy.media_type == e1000_media_type_copper) &&
 	    netif_carrier_ok(netdev))
-		ecmd->eth_tp_mdix = hw->phy.is_mdix ? ETH_TP_MDI_X :
-		                                      ETH_TP_MDI;
+		ecmd->eth_tp_mdix = hw->phy.is_mdix ? ETH_TP_MDI_X : ETH_TP_MDI;
 	else
 		ecmd->eth_tp_mdix = ETH_TP_MDI_INVALID;
 
@@ -224,8 +223,7 @@ static int e1000_set_spd_dplx(struct e1000_adapter *adapter, u32 spd, u8 dplx)
 
 	/* Fiber NICs only allow 1000 gbps Full duplex */
 	if ((adapter->hw.phy.media_type == e1000_media_type_fiber) &&
-	    spd != SPEED_1000 &&
-	    dplx != DUPLEX_FULL) {
+	    (spd != SPEED_1000) && (dplx != DUPLEX_FULL)) {
 		goto err_inval;
 	}
 
@@ -298,12 +296,10 @@ static int e1000_set_settings(struct net_device *netdev,
 		hw->mac.autoneg = 1;
 		if (hw->phy.media_type == e1000_media_type_fiber)
 			hw->phy.autoneg_advertised = ADVERTISED_1000baseT_Full |
-						     ADVERTISED_FIBRE |
-						     ADVERTISED_Autoneg;
+			    ADVERTISED_FIBRE | ADVERTISED_Autoneg;
 		else
 			hw->phy.autoneg_advertised = ecmd->advertising |
-						     ADVERTISED_TP |
-						     ADVERTISED_Autoneg;
+			    ADVERTISED_TP | ADVERTISED_Autoneg;
 		ecmd->advertising = hw->phy.autoneg_advertised;
 		if (adapter->fc_autoneg)
 			hw->fc.requested_mode = e1000_fc_default;
@@ -346,7 +342,7 @@ static void e1000_get_pauseparam(struct net_device *netdev,
 	struct e1000_hw *hw = &adapter->hw;
 
 	pause->autoneg =
-		(adapter->fc_autoneg ? AUTONEG_ENABLE : AUTONEG_DISABLE);
+	    (adapter->fc_autoneg ? AUTONEG_ENABLE : AUTONEG_DISABLE);
 
 	if (hw->fc.current_mode == e1000_fc_rx_pause) {
 		pause->rx_pause = 1;
@@ -435,7 +431,7 @@ static void e1000_get_regs(struct net_device *netdev,
 	memset(p, 0, E1000_REGS_LEN * sizeof(u32));
 
 	regs->version = (1 << 24) | (adapter->pdev->revision << 16) |
-			adapter->pdev->device;
+	    adapter->pdev->device;
 
 	regs_buff[0]  = er32(CTRL);
 	regs_buff[1]  = er32(STATUS);
@@ -503,8 +499,8 @@ static int e1000_get_eeprom(struct net_device *netdev,
 	first_word = eeprom->offset >> 1;
 	last_word = (eeprom->offset + eeprom->len - 1) >> 1;
 
-	eeprom_buff = kmalloc(sizeof(u16) *
-			(last_word - first_word + 1), GFP_KERNEL);
+	eeprom_buff = kmalloc(sizeof(u16) * (last_word - first_word + 1),
+			      GFP_KERNEL);
 	if (!eeprom_buff)
 		return -ENOMEM;
 
@@ -515,7 +511,7 @@ static int e1000_get_eeprom(struct net_device *netdev,
 	} else {
 		for (i = 0; i < last_word - first_word + 1; i++) {
 			ret_val = e1000_read_nvm(hw, first_word + i, 1,
-						      &eeprom_buff[i]);
+						 &eeprom_buff[i]);
 			if (ret_val)
 				break;
 		}
@@ -553,7 +549,8 @@ static int e1000_set_eeprom(struct net_device *netdev,
 	if (eeprom->len == 0)
 		return -EOPNOTSUPP;
 
-	if (eeprom->magic != (adapter->pdev->vendor | (adapter->pdev->device << 16)))
+	if (eeprom->magic !=
+	    (adapter->pdev->vendor | (adapter->pdev->device << 16)))
 		return -EFAULT;
 
 	if (adapter->flags & FLAG_READ_ONLY_NVM)
@@ -579,7 +576,7 @@ static int e1000_set_eeprom(struct net_device *netdev,
 		/* need read/modify/write of last changed EEPROM word */
 		/* only the first byte of the word is being modified */
 		ret_val = e1000_read_nvm(hw, last_word, 1,
-				  &eeprom_buff[last_word - first_word]);
+					 &eeprom_buff[last_word - first_word]);
 
 	if (ret_val)
 		goto out;
@@ -618,8 +615,7 @@ static void e1000_get_drvinfo(struct net_device *netdev,
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
 
-	strlcpy(drvinfo->driver,  e1000e_driver_name,
-		sizeof(drvinfo->driver));
+	strlcpy(drvinfo->driver, e1000e_driver_name, sizeof(drvinfo->driver));
 	strlcpy(drvinfo->version, e1000e_driver_version,
 		sizeof(drvinfo->version));
 
@@ -627,10 +623,10 @@ static void e1000_get_drvinfo(struct net_device *netdev,
 	 * PCI-E controllers
 	 */
 	snprintf(drvinfo->fw_version, sizeof(drvinfo->fw_version),
-		"%d.%d-%d",
-		(adapter->eeprom_vers & 0xF000) >> 12,
-		(adapter->eeprom_vers & 0x0FF0) >> 4,
-		(adapter->eeprom_vers & 0x000F));
+		 "%d.%d-%d",
+		 (adapter->eeprom_vers & 0xF000) >> 12,
+		 (adapter->eeprom_vers & 0x0FF0) >> 4,
+		 (adapter->eeprom_vers & 0x000F));
 
 	strlcpy(drvinfo->bus_info, pci_name(adapter->pdev),
 		sizeof(drvinfo->bus_info));
@@ -756,7 +752,8 @@ static bool reg_pattern_test(struct e1000_adapter *adapter, u64 *data,
 {
 	u32 pat, val;
 	static const u32 test[] = {
-		0x5A5A5A5A, 0xA5A5A5A5, 0x00000000, 0xFFFFFFFF};
+		0x5A5A5A5A, 0xA5A5A5A5, 0x00000000, 0xFFFFFFFF
+	};
 	for (pat = 0; pat < ARRAY_SIZE(test); pat++) {
 		E1000_WRITE_REG_ARRAY(&adapter->hw, reg, offset,
 				      (test[pat] & write));
@@ -786,6 +783,7 @@ static bool reg_set_and_check(struct e1000_adapter *adapter, u64 *data,
 	}
 	return 0;
 }
+
 #define REG_PATTERN_TEST_ARRAY(reg, offset, mask, write)                       \
 	do {                                                                   \
 		if (reg_pattern_test(adapter, data, reg, offset, mask, write)) \
@@ -813,16 +811,16 @@ static int e1000_reg_test(struct e1000_adapter *adapter, u64 *data)
 	u32 wlock_mac = 0;
 
 	/* The status register is Read Only, so a write should fail.
-	 * Some bits that get toggled are ignored.
+	 * Some bits that get toggled are ignored.  There are several bits
+	 * on newer hardware that are r/w.
 	 */
 	switch (mac->type) {
-	/* there are several bits on newer hardware that are r/w */
 	case e1000_82571:
 	case e1000_82572:
 	case e1000_80003es2lan:
 		toggle = 0x7FFFF3FF;
 		break;
-        default:
+	default:
 		toggle = 0x7FFFF033;
 		break;
 	}
@@ -928,7 +926,7 @@ static int e1000_eeprom_test(struct e1000_adapter *adapter, u64 *data)
 	}
 
 	/* If Checksum is not Correct return error else test passed */
-	if ((checksum != (u16) NVM_SUM) && !(*data))
+	if ((checksum != (u16)NVM_SUM) && !(*data))
 		*data = 2;
 
 	return *data;
@@ -936,7 +934,7 @@ static int e1000_eeprom_test(struct e1000_adapter *adapter, u64 *data)
 
 static irqreturn_t e1000_test_intr(int __always_unused irq, void *data)
 {
-	struct net_device *netdev = (struct net_device *) data;
+	struct net_device *netdev = (struct net_device *)data;
 	struct e1000_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
 
@@ -969,8 +967,8 @@ static int e1000_intr_test(struct e1000_adapter *adapter, u64 *data)
 	if (!request_irq(irq, e1000_test_intr, IRQF_PROBE_SHARED, netdev->name,
 			 netdev)) {
 		shared_int = 0;
-	} else if (request_irq(irq, e1000_test_intr, IRQF_SHARED,
-		 netdev->name, netdev)) {
+	} else if (request_irq(irq, e1000_test_intr, IRQF_SHARED, netdev->name,
+			       netdev)) {
 		*data = 1;
 		ret_val = -1;
 		goto out;
@@ -1080,28 +1078,33 @@ static void e1000_free_desc_rings(struct e1000_adapter *adapter)
 	struct e1000_ring *tx_ring = &adapter->test_tx_ring;
 	struct e1000_ring *rx_ring = &adapter->test_rx_ring;
 	struct pci_dev *pdev = adapter->pdev;
+	struct e1000_buffer *buffer_info;
 	int i;
 
 	if (tx_ring->desc && tx_ring->buffer_info) {
 		for (i = 0; i < tx_ring->count; i++) {
-			if (tx_ring->buffer_info[i].dma)
+			buffer_info = &tx_ring->buffer_info[i];
+
+			if (buffer_info->dma)
 				dma_unmap_single(&pdev->dev,
-					tx_ring->buffer_info[i].dma,
-					tx_ring->buffer_info[i].length,
-					DMA_TO_DEVICE);
-			if (tx_ring->buffer_info[i].skb)
-				dev_kfree_skb(tx_ring->buffer_info[i].skb);
+						 buffer_info->dma,
+						 buffer_info->length,
+						 DMA_TO_DEVICE);
+			if (buffer_info->skb)
+				dev_kfree_skb(buffer_info->skb);
 		}
 	}
 
 	if (rx_ring->desc && rx_ring->buffer_info) {
 		for (i = 0; i < rx_ring->count; i++) {
-			if (rx_ring->buffer_info[i].dma)
+			buffer_info = &rx_ring->buffer_info[i];
+
+			if (buffer_info->dma)
 				dma_unmap_single(&pdev->dev,
-					rx_ring->buffer_info[i].dma,
-					2048, DMA_FROM_DEVICE);
-			if (rx_ring->buffer_info[i].skb)
-				dev_kfree_skb(rx_ring->buffer_info[i].skb);
+						 buffer_info->dma,
+						 2048, DMA_FROM_DEVICE);
+			if (buffer_info->skb)
+				dev_kfree_skb(buffer_info->skb);
 		}
 	}
 
@@ -1138,8 +1141,7 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 		tx_ring->count = E1000_DEFAULT_TXD;
 
 	tx_ring->buffer_info = kcalloc(tx_ring->count,
-				       sizeof(struct e1000_buffer),
-				       GFP_KERNEL);
+				       sizeof(struct e1000_buffer), GFP_KERNEL);
 	if (!tx_ring->buffer_info) {
 		ret_val = 1;
 		goto err_nomem;
@@ -1156,8 +1158,8 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 	tx_ring->next_to_use = 0;
 	tx_ring->next_to_clean = 0;
 
-	ew32(TDBAL(0), ((u64) tx_ring->dma & 0x00000000FFFFFFFF));
-	ew32(TDBAH(0), ((u64) tx_ring->dma >> 32));
+	ew32(TDBAL(0), ((u64)tx_ring->dma & 0x00000000FFFFFFFF));
+	ew32(TDBAH(0), ((u64)tx_ring->dma >> 32));
 	ew32(TDLEN(0), tx_ring->count * sizeof(struct e1000_tx_desc));
 	ew32(TDH(0), 0);
 	ew32(TDT(0), 0);
@@ -1179,8 +1181,8 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 		tx_ring->buffer_info[i].skb = skb;
 		tx_ring->buffer_info[i].length = skb->len;
 		tx_ring->buffer_info[i].dma =
-			dma_map_single(&pdev->dev, skb->data, skb->len,
-				       DMA_TO_DEVICE);
+		    dma_map_single(&pdev->dev, skb->data, skb->len,
+				   DMA_TO_DEVICE);
 		if (dma_mapping_error(&pdev->dev,
 				      tx_ring->buffer_info[i].dma)) {
 			ret_val = 4;
@@ -1200,8 +1202,7 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 		rx_ring->count = E1000_DEFAULT_RXD;
 
 	rx_ring->buffer_info = kcalloc(rx_ring->count,
-				       sizeof(struct e1000_buffer),
-				       GFP_KERNEL);
+				       sizeof(struct e1000_buffer), GFP_KERNEL);
 	if (!rx_ring->buffer_info) {
 		ret_val = 5;
 		goto err_nomem;
@@ -1220,16 +1221,16 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 	rctl = er32(RCTL);
 	if (!(adapter->flags2 & FLAG2_NO_DISABLE_RX))
 		ew32(RCTL, rctl & ~E1000_RCTL_EN);
-	ew32(RDBAL(0), ((u64) rx_ring->dma & 0xFFFFFFFF));
-	ew32(RDBAH(0), ((u64) rx_ring->dma >> 32));
+	ew32(RDBAL(0), ((u64)rx_ring->dma & 0xFFFFFFFF));
+	ew32(RDBAH(0), ((u64)rx_ring->dma >> 32));
 	ew32(RDLEN(0), rx_ring->size);
 	ew32(RDH(0), 0);
 	ew32(RDT(0), 0);
 	rctl = E1000_RCTL_EN | E1000_RCTL_BAM | E1000_RCTL_SZ_2048 |
-		E1000_RCTL_UPE | E1000_RCTL_MPE | E1000_RCTL_LPE |
-		E1000_RCTL_SBP | E1000_RCTL_SECRC |
-		E1000_RCTL_LBM_NO | E1000_RCTL_RDMTS_HALF |
-		(adapter->hw.mac.mc_filter_type << E1000_RCTL_MO_SHIFT);
+	    E1000_RCTL_UPE | E1000_RCTL_MPE | E1000_RCTL_LPE |
+	    E1000_RCTL_SBP | E1000_RCTL_SECRC |
+	    E1000_RCTL_LBM_NO | E1000_RCTL_RDMTS_HALF |
+	    (adapter->hw.mac.mc_filter_type << E1000_RCTL_MO_SHIFT);
 	ew32(RCTL, rctl);
 
 	for (i = 0; i < rx_ring->count; i++) {
@@ -1244,8 +1245,8 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 		skb_reserve(skb, NET_IP_ALIGN);
 		rx_ring->buffer_info[i].skb = skb;
 		rx_ring->buffer_info[i].dma =
-			dma_map_single(&pdev->dev, skb->data, 2048,
-				       DMA_FROM_DEVICE);
+		    dma_map_single(&pdev->dev, skb->data, 2048,
+				   DMA_FROM_DEVICE);
 		if (dma_mapping_error(&pdev->dev,
 				      rx_ring->buffer_info[i].dma)) {
 			ret_val = 8;
@@ -1296,7 +1297,7 @@ static int e1000_integrated_phy_loopback(struct e1000_adapter *adapter)
 
 		ew32(CTRL, ctrl_reg);
 		e1e_flush();
-		udelay(500);
+		usleep_range(500, 1000);
 
 		return 0;
 	}
@@ -1322,7 +1323,7 @@ static int e1000_integrated_phy_loopback(struct e1000_adapter *adapter)
 		e1e_wphy(hw, PHY_REG(2, 21), phy_reg);
 		/* Assert SW reset for above settings to take effect */
 		hw->phy.ops.commit(hw);
-		mdelay(1);
+		usleep_range(1000, 2000);
 		/* Force Full Duplex */
 		e1e_rphy(hw, PHY_REG(769, 16), &phy_reg);
 		e1e_wphy(hw, PHY_REG(769, 16), phy_reg | 0x000C);
@@ -1363,7 +1364,7 @@ static int e1000_integrated_phy_loopback(struct e1000_adapter *adapter)
 
 	/* force 1000, set loopback */
 	e1e_wphy(hw, MII_BMCR, 0x4140);
-	mdelay(250);
+	msleep(250);
 
 	/* Now set up the MAC to the same speed/duplex as the PHY. */
 	ctrl_reg = er32(CTRL);
@@ -1395,7 +1396,7 @@ static int e1000_integrated_phy_loopback(struct e1000_adapter *adapter)
 	if (hw->phy.type == e1000_phy_m88)
 		e1000_phy_disable_receiver(adapter);
 
-	udelay(500);
+	usleep_range(500, 1000);
 
 	return 0;
 }
@@ -1431,8 +1432,7 @@ static int e1000_set_82571_fiber_loopback(struct e1000_adapter *adapter)
 	/* special write to serdes control register to enable SerDes analog
 	 * loopback
 	 */
-#define E1000_SERDES_LB_ON 0x410
-	ew32(SCTL, E1000_SERDES_LB_ON);
+	ew32(SCTL, E1000_SCTL_ENABLE_SERDES_LOOPBACK);
 	e1e_flush();
 	usleep_range(10000, 20000);
 
@@ -1526,8 +1526,7 @@ static void e1000_loopback_cleanup(struct e1000_adapter *adapter)
 	case e1000_82572:
 		if (hw->phy.media_type == e1000_media_type_fiber ||
 		    hw->phy.media_type == e1000_media_type_internal_serdes) {
-#define E1000_SERDES_LB_OFF 0x400
-			ew32(SCTL, E1000_SERDES_LB_OFF);
+			ew32(SCTL, E1000_SCTL_DISABLE_SERDES_LOOPBACK);
 			e1e_flush();
 			usleep_range(10000, 20000);
 			break;
@@ -1564,7 +1563,7 @@ static int e1000_check_lbtest_frame(struct sk_buff *skb,
 	frame_size &= ~1;
 	if (*(skb->data + 3) == 0xFF)
 		if ((*(skb->data + frame_size / 2 + 10) == 0xBE) &&
-		   (*(skb->data + frame_size / 2 + 12) == 0xAF))
+		    (*(skb->data + frame_size / 2 + 12) == 0xAF))
 			return 0;
 	return 13;
 }
@@ -1575,6 +1574,7 @@ static int e1000_run_loopback_test(struct e1000_adapter *adapter)
 	struct e1000_ring *rx_ring = &adapter->test_rx_ring;
 	struct pci_dev *pdev = adapter->pdev;
 	struct e1000_hw *hw = &adapter->hw;
+	struct e1000_buffer *buffer_info;
 	int i, j, k, l;
 	int lc;
 	int good_cnt;
@@ -1595,14 +1595,17 @@ static int e1000_run_loopback_test(struct e1000_adapter *adapter)
 
 	k = 0;
 	l = 0;
-	for (j = 0; j <= lc; j++) { /* loop count loop */
-		for (i = 0; i < 64; i++) { /* send the packets */
-			e1000_create_lbtest_frame(tx_ring->buffer_info[k].skb,
-						  1024);
+	/* loop count loop */
+	for (j = 0; j <= lc; j++) {
+		/* send the packets */
+		for (i = 0; i < 64; i++) {
+			buffer_info = &tx_ring->buffer_info[k];
+
+			e1000_create_lbtest_frame(buffer_info->skb, 1024);
 			dma_sync_single_for_device(&pdev->dev,
-					tx_ring->buffer_info[k].dma,
-					tx_ring->buffer_info[k].length,
-					DMA_TO_DEVICE);
+						   buffer_info->dma,
+						   buffer_info->length,
+						   DMA_TO_DEVICE);
 			k++;
 			if (k == tx_ring->count)
 				k = 0;
@@ -1612,13 +1615,16 @@ static int e1000_run_loopback_test(struct e1000_adapter *adapter)
 		msleep(200);
 		time = jiffies; /* set the start time for the receive */
 		good_cnt = 0;
-		do { /* receive the sent packets */
+		/* receive the sent packets */
+		do {
+			buffer_info = &rx_ring->buffer_info[l];
+
 			dma_sync_single_for_cpu(&pdev->dev,
-					rx_ring->buffer_info[l].dma, 2048,
-					DMA_FROM_DEVICE);
+						buffer_info->dma, 2048,
+						DMA_FROM_DEVICE);
 
-			ret_val = e1000_check_lbtest_frame(
-					rx_ring->buffer_info[l].skb, 1024);
+			ret_val = e1000_check_lbtest_frame(buffer_info->skb,
+							   1024);
 			if (!ret_val)
 				good_cnt++;
 			l++;
@@ -1637,7 +1643,7 @@ static int e1000_run_loopback_test(struct e1000_adapter *adapter)
 			ret_val = 14; /* error code for time out error */
 			break;
 		}
-	} /* end loop count loop */
+	}
 	return ret_val;
 }
 
@@ -1696,7 +1702,7 @@ static int e1000_link_test(struct e1000_adapter *adapter, u64 *data)
 			/* On some Phy/switch combinations, link establishment
 			 * can take a few seconds more than expected.
 			 */
-			msleep(5000);
+			msleep_interruptible(5000);
 
 		if (!(er32(STATUS) & E1000_STATUS_LU))
 			*data = 1;
@@ -1980,12 +1986,12 @@ static void e1000_get_ethtool_stats(struct net_device *netdev,
 	for (i = 0; i < E1000_GLOBAL_STATS_LEN; i++) {
 		switch (e1000_gstrings_stats[i].type) {
 		case NETDEV_STATS:
-			p = (char *) &net_stats +
-					e1000_gstrings_stats[i].stat_offset;
+			p = (char *)&net_stats +
+			    e1000_gstrings_stats[i].stat_offset;
 			break;
 		case E1000_STATS:
-			p = (char *) adapter +
-					e1000_gstrings_stats[i].stat_offset;
+			p = (char *)adapter +
+			    e1000_gstrings_stats[i].stat_offset;
 			break;
 		default:
 			data[i] = 0;
@@ -1993,7 +1999,7 @@ static void e1000_get_ethtool_stats(struct net_device *netdev,
 		}
 
 		data[i] = (e1000_gstrings_stats[i].sizeof_stat ==
-			sizeof(u64)) ? *(u64 *)p : *(u32 *)p;
+			   sizeof(u64)) ? *(u64 *)p : *(u32 *)p;
 	}
 }
 
@@ -2069,23 +2075,20 @@ static int e1000e_get_eee(struct net_device *netdev, struct ethtool_eee *edata)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
-	u16 cap_addr, adv_addr, lpa_addr, pcs_stat_addr, phy_data, lpi_ctrl;
-	u32 status, ret_val;
+	u16 cap_addr, lpa_addr, pcs_stat_addr, phy_data;
+	u32 ret_val;
 
-	if (!(adapter->flags & FLAG_IS_ICH) ||
-	    !(adapter->flags2 & FLAG2_HAS_EEE))
+	if (!(adapter->flags2 & FLAG2_HAS_EEE))
 		return -EOPNOTSUPP;
 
 	switch (hw->phy.type) {
 	case e1000_phy_82579:
 		cap_addr = I82579_EEE_CAPABILITY;
-		adv_addr = I82579_EEE_ADVERTISEMENT;
 		lpa_addr = I82579_EEE_LP_ABILITY;
 		pcs_stat_addr = I82579_EEE_PCS_STATUS;
 		break;
 	case e1000_phy_i217:
 		cap_addr = I217_EEE_CAPABILITY;
-		adv_addr = I217_EEE_ADVERTISEMENT;
 		lpa_addr = I217_EEE_LP_ABILITY;
 		pcs_stat_addr = I217_EEE_PCS_STATUS;
 		break;
@@ -2104,10 +2107,7 @@ static int e1000e_get_eee(struct net_device *netdev, struct ethtool_eee *edata)
 	edata->supported = mmd_eee_cap_to_ethtool_sup_t(phy_data);
 
 	/* EEE Advertised */
-	ret_val = e1000_read_emi_reg_locked(hw, adv_addr, &phy_data);
-	if (ret_val)
-		goto release;
-	edata->advertised = mmd_eee_adv_to_ethtool_adv_t(phy_data);
+	edata->advertised = mmd_eee_adv_to_ethtool_adv_t(adapter->eee_advert);
 
 	/* EEE Link Partner Advertised */
 	ret_val = e1000_read_emi_reg_locked(hw, lpa_addr, &phy_data);
@@ -2125,25 +2125,11 @@ release:
 	if (ret_val)
 		return -ENODATA;
 
-	e1e_rphy(hw, I82579_LPI_CTRL, &lpi_ctrl);
-	status = er32(STATUS);
-
 	/* Result of the EEE auto negotiation - there is no register that
 	 * has the status of the EEE negotiation so do a best-guess based
-	 * on whether both Tx and Rx LPI indications have been received or
-	 * base it on the link speed, the EEE advertised speeds on both ends
-	 * and the speeds on which EEE is enabled locally.
+	 * on whether Tx or Rx LPI indications have been received.
 	 */
-	if (((phy_data & E1000_EEE_TX_LPI_RCVD) &&
-	     (phy_data & E1000_EEE_RX_LPI_RCVD)) ||
-	    ((status & E1000_STATUS_SPEED_100) &&
-	     (edata->advertised & ADVERTISED_100baseT_Full) &&
-	     (edata->lp_advertised & ADVERTISED_100baseT_Full) &&
-	     (lpi_ctrl & I82579_LPI_CTRL_100_ENABLE)) ||
-	    ((status & E1000_STATUS_SPEED_1000) &&
-	     (edata->advertised & ADVERTISED_1000baseT_Full) &&
-	     (edata->lp_advertised & ADVERTISED_1000baseT_Full) &&
-	     (lpi_ctrl & I82579_LPI_CTRL_1000_ENABLE)))
+	if (phy_data & (E1000_EEE_TX_LPI_RCVD | E1000_EEE_RX_LPI_RCVD))
 		edata->eee_active = true;
 
 	edata->eee_enabled = !hw->dev_spec.ich8lan.eee_disable;
@@ -2160,19 +2146,10 @@ static int e1000e_set_eee(struct net_device *netdev, struct ethtool_eee *edata)
 	struct ethtool_eee eee_curr;
 	s32 ret_val;
 
-	if (!(adapter->flags & FLAG_IS_ICH) ||
-	    !(adapter->flags2 & FLAG2_HAS_EEE))
-		return -EOPNOTSUPP;
-
 	ret_val = e1000e_get_eee(netdev, &eee_curr);
 	if (ret_val)
 		return ret_val;
 
-	if (eee_curr.advertised != edata->advertised) {
-		e_err("Setting EEE advertisement is not supported\n");
-		return -EINVAL;
-	}
-
 	if (eee_curr.tx_lpi_enabled != edata->tx_lpi_enabled) {
 		e_err("Setting EEE tx-lpi is not supported\n");
 		return -EINVAL;
@@ -2183,16 +2160,21 @@ static int e1000e_set_eee(struct net_device *netdev, struct ethtool_eee *edata)
 		return -EINVAL;
 	}
 
-	if (hw->dev_spec.ich8lan.eee_disable != !edata->eee_enabled) {
-		hw->dev_spec.ich8lan.eee_disable = !edata->eee_enabled;
-
-		/* reset the link */
-		if (netif_running(netdev))
-			e1000e_reinit_locked(adapter);
-		else
-			e1000e_reset(adapter);
+	if (edata->advertised & ~(ADVERTISE_100_FULL | ADVERTISE_1000_FULL)) {
+		e_err("EEE advertisement supports only 100TX and/or 1000T full-duplex\n");
+		return -EINVAL;
 	}
 
+	adapter->eee_advert = ethtool_adv_to_mmd_eee_adv_t(edata->advertised);
+
+	hw->dev_spec.ich8lan.eee_disable = !edata->eee_enabled;
+
+	/* reset the link */
+	if (netif_running(netdev))
+		e1000e_reinit_locked(adapter);
+	else
+		e1000e_reset(adapter);
+
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/intel/e1000e/hw.h b/drivers/net/ethernet/intel/e1000e/hw.h
index 1e6b889aee87..84850f7a23e4 100644
--- a/drivers/net/ethernet/intel/e1000e/hw.h
+++ b/drivers/net/ethernet/intel/e1000e/hw.h
@@ -167,7 +167,7 @@ enum e1000_1000t_rx_status {
 	e1000_1000t_rx_status_undefined = 0xFF
 };
 
-enum e1000_rev_polarity{
+enum e1000_rev_polarity {
 	e1000_rev_polarity_normal = 0,
 	e1000_rev_polarity_reversed,
 	e1000_rev_polarity_undefined = 0xFF
@@ -545,7 +545,7 @@ struct e1000_mac_info {
 	u16 mta_reg_count;
 
 	/* Maximum size of the MTA register table in all supported adapters */
-	#define MAX_MTA_REG 128
+#define MAX_MTA_REG 128
 	u32 mta_shadow[MAX_MTA_REG];
 	u16 rar_entry_count;
 
diff --git a/drivers/net/ethernet/intel/e1000e/ich8lan.c b/drivers/net/ethernet/intel/e1000e/ich8lan.c
index 121a865c7fbd..ad9d8f2dd868 100644
--- a/drivers/net/ethernet/intel/e1000e/ich8lan.c
+++ b/drivers/net/ethernet/intel/e1000e/ich8lan.c
@@ -61,15 +61,15 @@
 /* Offset 04h HSFSTS */
 union ich8_hws_flash_status {
 	struct ich8_hsfsts {
-		u16 flcdone    :1; /* bit 0 Flash Cycle Done */
-		u16 flcerr     :1; /* bit 1 Flash Cycle Error */
-		u16 dael       :1; /* bit 2 Direct Access error Log */
-		u16 berasesz   :2; /* bit 4:3 Sector Erase Size */
-		u16 flcinprog  :1; /* bit 5 flash cycle in Progress */
-		u16 reserved1  :2; /* bit 13:6 Reserved */
-		u16 reserved2  :6; /* bit 13:6 Reserved */
-		u16 fldesvalid :1; /* bit 14 Flash Descriptor Valid */
-		u16 flockdn    :1; /* bit 15 Flash Config Lock-Down */
+		u16 flcdone:1;	/* bit 0 Flash Cycle Done */
+		u16 flcerr:1;	/* bit 1 Flash Cycle Error */
+		u16 dael:1;	/* bit 2 Direct Access error Log */
+		u16 berasesz:2;	/* bit 4:3 Sector Erase Size */
+		u16 flcinprog:1;	/* bit 5 flash cycle in Progress */
+		u16 reserved1:2;	/* bit 13:6 Reserved */
+		u16 reserved2:6;	/* bit 13:6 Reserved */
+		u16 fldesvalid:1;	/* bit 14 Flash Descriptor Valid */
+		u16 flockdn:1;	/* bit 15 Flash Config Lock-Down */
 	} hsf_status;
 	u16 regval;
 };
@@ -78,11 +78,11 @@ union ich8_hws_flash_status {
 /* Offset 06h FLCTL */
 union ich8_hws_flash_ctrl {
 	struct ich8_hsflctl {
-		u16 flcgo      :1;   /* 0 Flash Cycle Go */
-		u16 flcycle    :2;   /* 2:1 Flash Cycle */
-		u16 reserved   :5;   /* 7:3 Reserved  */
-		u16 fldbcount  :2;   /* 9:8 Flash Data Byte Count */
-		u16 flockdn    :6;   /* 15:10 Reserved */
+		u16 flcgo:1;	/* 0 Flash Cycle Go */
+		u16 flcycle:2;	/* 2:1 Flash Cycle */
+		u16 reserved:5;	/* 7:3 Reserved  */
+		u16 fldbcount:2;	/* 9:8 Flash Data Byte Count */
+		u16 flockdn:6;	/* 15:10 Reserved */
 	} hsf_ctrl;
 	u16 regval;
 };
@@ -90,10 +90,10 @@ union ich8_hws_flash_ctrl {
 /* ICH Flash Region Access Permissions */
 union ich8_hws_flash_regacc {
 	struct ich8_flracc {
-		u32 grra      :8; /* 0:7 GbE region Read Access */
-		u32 grwa      :8; /* 8:15 GbE region Write Access */
-		u32 gmrag     :8; /* 23:16 GbE Master Read Access Grant */
-		u32 gmwag     :8; /* 31:24 GbE Master Write Access Grant */
+		u32 grra:8;	/* 0:7 GbE region Read Access */
+		u32 grwa:8;	/* 8:15 GbE region Write Access */
+		u32 gmrag:8;	/* 23:16 GbE Master Read Access Grant */
+		u32 gmwag:8;	/* 31:24 GbE Master Write Access Grant */
 	} hsf_flregacc;
 	u16 regval;
 };
@@ -142,6 +142,7 @@ static void e1000_rar_set_pch2lan(struct e1000_hw *hw, u8 *addr, u32 index);
 static void e1000_rar_set_pch_lpt(struct e1000_hw *hw, u8 *addr, u32 index);
 static s32 e1000_k1_workaround_lv(struct e1000_hw *hw);
 static void e1000_gate_hw_phy_config_ich8lan(struct e1000_hw *hw, bool gate);
+static s32 e1000_setup_copper_link_pch_lpt(struct e1000_hw *hw);
 
 static inline u16 __er16flash(struct e1000_hw *hw, unsigned long reg)
 {
@@ -312,7 +313,7 @@ static s32 e1000_init_phy_workarounds_pchlan(struct e1000_hw *hw)
 		mac_reg &= ~E1000_CTRL_LANPHYPC_VALUE;
 		ew32(CTRL, mac_reg);
 		e1e_flush();
-		udelay(10);
+		usleep_range(10, 20);
 		mac_reg &= ~E1000_CTRL_LANPHYPC_OVERRIDE;
 		ew32(CTRL, mac_reg);
 		e1e_flush();
@@ -548,8 +549,8 @@ static s32 e1000_init_nvm_params_ich8lan(struct e1000_hw *hw)
 	/* find total size of the NVM, then cut in half since the total
 	 * size represents two separate NVM banks.
 	 */
-	nvm->flash_bank_size = (sector_end_addr - sector_base_addr)
-				<< FLASH_SECTOR_ADDR_SHIFT;
+	nvm->flash_bank_size = ((sector_end_addr - sector_base_addr)
+				<< FLASH_SECTOR_ADDR_SHIFT);
 	nvm->flash_bank_size /= 2;
 	/* Adjust to word count */
 	nvm->flash_bank_size /= sizeof(u16);
@@ -636,6 +637,8 @@ static s32 e1000_init_mac_params_ich8lan(struct e1000_hw *hw)
 	if (mac->type == e1000_pch_lpt) {
 		mac->rar_entry_count = E1000_PCH_LPT_RAR_ENTRIES;
 		mac->ops.rar_set = e1000_rar_set_pch_lpt;
+		mac->ops.setup_physical_interface =
+		    e1000_setup_copper_link_pch_lpt;
 	}
 
 	/* Enable PCS Lock-loss workaround for ICH8 */
@@ -692,7 +695,7 @@ s32 e1000_read_emi_reg_locked(struct e1000_hw *hw, u16 addr, u16 *data)
  *
  *  Assumes the SW/FW/HW Semaphore is already acquired.
  **/
-static s32 e1000_write_emi_reg_locked(struct e1000_hw *hw, u16 addr, u16 data)
+s32 e1000_write_emi_reg_locked(struct e1000_hw *hw, u16 addr, u16 data)
 {
 	return __e1000_access_emi_reg_locked(hw, addr, &data, false);
 }
@@ -709,11 +712,22 @@ static s32 e1000_set_eee_pchlan(struct e1000_hw *hw)
 {
 	struct e1000_dev_spec_ich8lan *dev_spec = &hw->dev_spec.ich8lan;
 	s32 ret_val;
-	u16 lpi_ctrl;
+	u16 lpa, pcs_status, adv, adv_addr, lpi_ctrl, data;
 
-	if ((hw->phy.type != e1000_phy_82579) &&
-	    (hw->phy.type != e1000_phy_i217))
+	switch (hw->phy.type) {
+	case e1000_phy_82579:
+		lpa = I82579_EEE_LP_ABILITY;
+		pcs_status = I82579_EEE_PCS_STATUS;
+		adv_addr = I82579_EEE_ADVERTISEMENT;
+		break;
+	case e1000_phy_i217:
+		lpa = I217_EEE_LP_ABILITY;
+		pcs_status = I217_EEE_PCS_STATUS;
+		adv_addr = I217_EEE_ADVERTISEMENT;
+		break;
+	default:
 		return 0;
+	}
 
 	ret_val = hw->phy.ops.acquire(hw);
 	if (ret_val)
@@ -728,34 +742,24 @@ static s32 e1000_set_eee_pchlan(struct e1000_hw *hw)
 
 	/* Enable EEE if not disabled by user */
 	if (!dev_spec->eee_disable) {
-		u16 lpa, pcs_status, data;
-
 		/* Save off link partner's EEE ability */
-		switch (hw->phy.type) {
-		case e1000_phy_82579:
-			lpa = I82579_EEE_LP_ABILITY;
-			pcs_status = I82579_EEE_PCS_STATUS;
-			break;
-		case e1000_phy_i217:
-			lpa = I217_EEE_LP_ABILITY;
-			pcs_status = I217_EEE_PCS_STATUS;
-			break;
-		default:
-			ret_val = -E1000_ERR_PHY;
-			goto release;
-		}
 		ret_val = e1000_read_emi_reg_locked(hw, lpa,
 						    &dev_spec->eee_lp_ability);
 		if (ret_val)
 			goto release;
 
+		/* Read EEE advertisement */
+		ret_val = e1000_read_emi_reg_locked(hw, adv_addr, &adv);
+		if (ret_val)
+			goto release;
+
 		/* Enable EEE only for speeds in which the link partner is
-		 * EEE capable.
+		 * EEE capable and for which we advertise EEE.
 		 */
-		if (dev_spec->eee_lp_ability & I82579_EEE_1000_SUPPORTED)
+		if (adv & dev_spec->eee_lp_ability & I82579_EEE_1000_SUPPORTED)
 			lpi_ctrl |= I82579_LPI_CTRL_1000_ENABLE;
 
-		if (dev_spec->eee_lp_ability & I82579_EEE_100_SUPPORTED) {
+		if (adv & dev_spec->eee_lp_ability & I82579_EEE_100_SUPPORTED) {
 			e1e_rphy_locked(hw, MII_LPA, &data);
 			if (data & LPA_100FULL)
 				lpi_ctrl |= I82579_LPI_CTRL_100_ENABLE;
@@ -767,13 +771,13 @@ static s32 e1000_set_eee_pchlan(struct e1000_hw *hw)
 				dev_spec->eee_lp_ability &=
 				    ~I82579_EEE_100_SUPPORTED;
 		}
-
-		/* R/Clr IEEE MMD 3.1 bits 11:10 - Tx/Rx LPI Received */
-		ret_val = e1000_read_emi_reg_locked(hw, pcs_status, &data);
-		if (ret_val)
-			goto release;
 	}
 
+	/* R/Clr IEEE MMD 3.1 bits 11:10 - Tx/Rx LPI Received */
+	ret_val = e1000_read_emi_reg_locked(hw, pcs_status, &data);
+	if (ret_val)
+		goto release;
+
 	ret_val = e1e_wphy_locked(hw, I82579_LPI_CTRL, lpi_ctrl);
 release:
 	hw->phy.ops.release(hw);
@@ -835,6 +839,94 @@ release:
 }
 
 /**
+ *  e1000_platform_pm_pch_lpt - Set platform power management values
+ *  @hw: pointer to the HW structure
+ *  @link: bool indicating link status
+ *
+ *  Set the Latency Tolerance Reporting (LTR) values for the "PCIe-like"
+ *  GbE MAC in the Lynx Point PCH based on Rx buffer size and link speed
+ *  when link is up (which must not exceed the maximum latency supported
+ *  by the platform), otherwise specify there is no LTR requirement.
+ *  Unlike true-PCIe devices which set the LTR maximum snoop/no-snoop
+ *  latencies in the LTR Extended Capability Structure in the PCIe Extended
+ *  Capability register set, on this device LTR is set by writing the
+ *  equivalent snoop/no-snoop latencies in the LTRV register in the MAC and
+ *  set the SEND bit to send an Intel On-chip System Fabric sideband (IOSF-SB)
+ *  message to the PMC.
+ **/
+static s32 e1000_platform_pm_pch_lpt(struct e1000_hw *hw, bool link)
+{
+	u32 reg = link << (E1000_LTRV_REQ_SHIFT + E1000_LTRV_NOSNOOP_SHIFT) |
+	    link << E1000_LTRV_REQ_SHIFT | E1000_LTRV_SEND;
+	u16 lat_enc = 0;	/* latency encoded */
+
+	if (link) {
+		u16 speed, duplex, scale = 0;
+		u16 max_snoop, max_nosnoop;
+		u16 max_ltr_enc;	/* max LTR latency encoded */
+		s64 lat_ns;	/* latency (ns) */
+		s64 value;
+		u32 rxa;
+
+		if (!hw->adapter->max_frame_size) {
+			e_dbg("max_frame_size not set.\n");
+			return -E1000_ERR_CONFIG;
+		}
+
+		hw->mac.ops.get_link_up_info(hw, &speed, &duplex);
+		if (!speed) {
+			e_dbg("Speed not set.\n");
+			return -E1000_ERR_CONFIG;
+		}
+
+		/* Rx Packet Buffer Allocation size (KB) */
+		rxa = er32(PBA) & E1000_PBA_RXA_MASK;
+
+		/* Determine the maximum latency tolerated by the device.
+		 *
+		 * Per the PCIe spec, the tolerated latencies are encoded as
+		 * a 3-bit encoded scale (only 0-5 are valid) multiplied by
+		 * a 10-bit value (0-1023) to provide a range from 1 ns to
+		 * 2^25*(2^10-1) ns.  The scale is encoded as 0=2^0ns,
+		 * 1=2^5ns, 2=2^10ns,...5=2^25ns.
+		 */
+		lat_ns = ((s64)rxa * 1024 -
+			  (2 * (s64)hw->adapter->max_frame_size)) * 8 * 1000;
+		if (lat_ns < 0)
+			lat_ns = 0;
+		else
+			do_div(lat_ns, speed);
+
+		value = lat_ns;
+		while (value > PCI_LTR_VALUE_MASK) {
+			scale++;
+			value = DIV_ROUND_UP(value, (1 << 5));
+		}
+		if (scale > E1000_LTRV_SCALE_MAX) {
+			e_dbg("Invalid LTR latency scale %d\n", scale);
+			return -E1000_ERR_CONFIG;
+		}
+		lat_enc = (u16)((scale << PCI_LTR_SCALE_SHIFT) | value);
+
+		/* Determine the maximum latency tolerated by the platform */
+		pci_read_config_word(hw->adapter->pdev, E1000_PCI_LTR_CAP_LPT,
+				     &max_snoop);
+		pci_read_config_word(hw->adapter->pdev,
+				     E1000_PCI_LTR_CAP_LPT + 2, &max_nosnoop);
+		max_ltr_enc = max_t(u16, max_snoop, max_nosnoop);
+
+		if (lat_enc > max_ltr_enc)
+			lat_enc = max_ltr_enc;
+	}
+
+	/* Set Snoop and No-Snoop latencies the same */
+	reg |= lat_enc | (lat_enc << E1000_LTRV_NOSNOOP_SHIFT);
+	ew32(LTRV, reg);
+
+	return 0;
+}
+
+/**
  *  e1000_check_for_copper_link_ich8lan - Check for link (Copper)
  *  @hw: pointer to the HW structure
  *
@@ -871,6 +963,34 @@ static s32 e1000_check_for_copper_link_ich8lan(struct e1000_hw *hw)
 			return ret_val;
 	}
 
+	/* When connected at 10Mbps half-duplex, 82579 parts are excessively
+	 * aggressive resulting in many collisions. To avoid this, increase
+	 * the IPG and reduce Rx latency in the PHY.
+	 */
+	if ((hw->mac.type == e1000_pch2lan) && link) {
+		u32 reg;
+		reg = er32(STATUS);
+		if (!(reg & (E1000_STATUS_FD | E1000_STATUS_SPEED_MASK))) {
+			reg = er32(TIPG);
+			reg &= ~E1000_TIPG_IPGT_MASK;
+			reg |= 0xFF;
+			ew32(TIPG, reg);
+
+			/* Reduce Rx latency in analog PHY */
+			ret_val = hw->phy.ops.acquire(hw);
+			if (ret_val)
+				return ret_val;
+
+			ret_val =
+			    e1000_write_emi_reg_locked(hw, I82579_RX_CONFIG, 0);
+
+			hw->phy.ops.release(hw);
+
+			if (ret_val)
+				return ret_val;
+		}
+	}
+
 	/* Work-around I218 hang issue */
 	if ((hw->adapter->pdev->device == E1000_DEV_ID_PCH_LPTLP_I218_LM) ||
 	    (hw->adapter->pdev->device == E1000_DEV_ID_PCH_LPTLP_I218_V)) {
@@ -879,6 +999,15 @@ static s32 e1000_check_for_copper_link_ich8lan(struct e1000_hw *hw)
 			return ret_val;
 	}
 
+	if (hw->mac.type == e1000_pch_lpt) {
+		/* Set platform power management values for
+		 * Latency Tolerance Reporting (LTR)
+		 */
+		ret_val = e1000_platform_pm_pch_lpt(hw, link);
+		if (ret_val)
+			return ret_val;
+	}
+
 	/* Clear link partner's EEE ability */
 	hw->dev_spec.ich8lan.eee_lp_ability = 0;
 
@@ -1002,10 +1131,6 @@ static s32 e1000_get_variants_ich8lan(struct e1000_adapter *adapter)
 	    (er32(FWSM) & E1000_ICH_FWSM_FW_VALID))
 		adapter->flags2 |= FLAG2_PCIM2PCI_ARBITER_WA;
 
-	/* Disable EEE by default until IEEE802.3az spec is finalized */
-	if (adapter->flags2 & FLAG2_HAS_EEE)
-		adapter->hw.dev_spec.ich8lan.eee_disable = true;
-
 	return 0;
 }
 
@@ -1134,9 +1259,9 @@ static bool e1000_check_mng_mode_ich8lan(struct e1000_hw *hw)
 	u32 fwsm;
 
 	fwsm = er32(FWSM);
-	return (fwsm & E1000_ICH_FWSM_FW_VALID) &&
-	       ((fwsm & E1000_FWSM_MODE_MASK) ==
-		(E1000_ICH_MNG_IAMT_MODE << E1000_FWSM_MODE_SHIFT));
+	return ((fwsm & E1000_ICH_FWSM_FW_VALID) &&
+		((fwsm & E1000_FWSM_MODE_MASK) ==
+		 (E1000_ICH_MNG_IAMT_MODE << E1000_FWSM_MODE_SHIFT)));
 }
 
 /**
@@ -1153,7 +1278,7 @@ static bool e1000_check_mng_mode_pchlan(struct e1000_hw *hw)
 
 	fwsm = er32(FWSM);
 	return (fwsm & E1000_ICH_FWSM_FW_VALID) &&
-	       (fwsm & (E1000_ICH_MNG_IAMT_MODE << E1000_FWSM_MODE_SHIFT));
+	    (fwsm & (E1000_ICH_MNG_IAMT_MODE << E1000_FWSM_MODE_SHIFT));
 }
 
 /**
@@ -1440,8 +1565,7 @@ static s32 e1000_sw_lcd_config_ich8lan(struct e1000_hw *hw)
 	word_addr = (u16)(cnf_base_addr << 1);
 
 	for (i = 0; i < cnf_size; i++) {
-		ret_val = e1000_read_nvm(hw, (word_addr + i * 2), 1,
-					 &reg_data);
+		ret_val = e1000_read_nvm(hw, (word_addr + i * 2), 1, &reg_data);
 		if (ret_val)
 			goto release;
 
@@ -1501,13 +1625,13 @@ static s32 e1000_k1_gig_workaround_hv(struct e1000_hw *hw, bool link)
 			if (ret_val)
 				goto release;
 
-			status_reg &= BM_CS_STATUS_LINK_UP |
-			              BM_CS_STATUS_RESOLVED |
-			              BM_CS_STATUS_SPEED_MASK;
+			status_reg &= (BM_CS_STATUS_LINK_UP |
+				       BM_CS_STATUS_RESOLVED |
+				       BM_CS_STATUS_SPEED_MASK);
 
 			if (status_reg == (BM_CS_STATUS_LINK_UP |
-			                   BM_CS_STATUS_RESOLVED |
-			                   BM_CS_STATUS_SPEED_1000))
+					   BM_CS_STATUS_RESOLVED |
+					   BM_CS_STATUS_SPEED_1000))
 				k1_enable = false;
 		}
 
@@ -1516,13 +1640,13 @@ static s32 e1000_k1_gig_workaround_hv(struct e1000_hw *hw, bool link)
 			if (ret_val)
 				goto release;
 
-			status_reg &= HV_M_STATUS_LINK_UP |
-			              HV_M_STATUS_AUTONEG_COMPLETE |
-			              HV_M_STATUS_SPEED_MASK;
+			status_reg &= (HV_M_STATUS_LINK_UP |
+				       HV_M_STATUS_AUTONEG_COMPLETE |
+				       HV_M_STATUS_SPEED_MASK);
 
 			if (status_reg == (HV_M_STATUS_LINK_UP |
-			                   HV_M_STATUS_AUTONEG_COMPLETE |
-			                   HV_M_STATUS_SPEED_1000))
+					   HV_M_STATUS_AUTONEG_COMPLETE |
+					   HV_M_STATUS_SPEED_1000))
 				k1_enable = false;
 		}
 
@@ -1579,7 +1703,7 @@ s32 e1000_configure_k1_ich8lan(struct e1000_hw *hw, bool k1_enable)
 	if (ret_val)
 		return ret_val;
 
-	udelay(20);
+	usleep_range(20, 40);
 	ctrl_ext = er32(CTRL_EXT);
 	ctrl_reg = er32(CTRL);
 
@@ -1589,11 +1713,11 @@ s32 e1000_configure_k1_ich8lan(struct e1000_hw *hw, bool k1_enable)
 
 	ew32(CTRL_EXT, ctrl_ext | E1000_CTRL_EXT_SPD_BYPS);
 	e1e_flush();
-	udelay(20);
+	usleep_range(20, 40);
 	ew32(CTRL, ctrl_reg);
 	ew32(CTRL_EXT, ctrl_ext);
 	e1e_flush();
-	udelay(20);
+	usleep_range(20, 40);
 
 	return 0;
 }
@@ -1667,7 +1791,6 @@ release:
 	return ret_val;
 }
 
-
 /**
  *  e1000_set_mdio_slow_mode_hv - Set slow MDIO access mode
  *  @hw:   pointer to the HW structure
@@ -1834,7 +1957,7 @@ s32 e1000_lv_jumbo_workaround_ich8lan(struct e1000_hw *hw, bool enable)
 		 * SHRAL/H) and initial CRC values to the MAC
 		 */
 		for (i = 0; i < (hw->mac.rar_entry_count + 4); i++) {
-			u8 mac_addr[ETH_ALEN] = {0};
+			u8 mac_addr[ETH_ALEN] = { 0 };
 			u32 addr_high, addr_low;
 
 			addr_high = er32(RAH(i));
@@ -1865,8 +1988,8 @@ s32 e1000_lv_jumbo_workaround_ich8lan(struct e1000_hw *hw, bool enable)
 		ew32(RCTL, mac_reg);
 
 		ret_val = e1000e_read_kmrn_reg(hw,
-						E1000_KMRNCTRLSTA_CTRL_OFFSET,
-						&data);
+					       E1000_KMRNCTRLSTA_CTRL_OFFSET,
+					       &data);
 		if (ret_val)
 			return ret_val;
 		ret_val = e1000e_write_kmrn_reg(hw,
@@ -1875,8 +1998,8 @@ s32 e1000_lv_jumbo_workaround_ich8lan(struct e1000_hw *hw, bool enable)
 		if (ret_val)
 			return ret_val;
 		ret_val = e1000e_read_kmrn_reg(hw,
-						E1000_KMRNCTRLSTA_HD_CTRL,
-						&data);
+					       E1000_KMRNCTRLSTA_HD_CTRL,
+					       &data);
 		if (ret_val)
 			return ret_val;
 		data &= ~(0xF << 8);
@@ -1923,8 +2046,8 @@ s32 e1000_lv_jumbo_workaround_ich8lan(struct e1000_hw *hw, bool enable)
 		ew32(RCTL, mac_reg);
 
 		ret_val = e1000e_read_kmrn_reg(hw,
-						E1000_KMRNCTRLSTA_CTRL_OFFSET,
-						&data);
+					       E1000_KMRNCTRLSTA_CTRL_OFFSET,
+					       &data);
 		if (ret_val)
 			return ret_val;
 		ret_val = e1000e_write_kmrn_reg(hw,
@@ -1933,8 +2056,8 @@ s32 e1000_lv_jumbo_workaround_ich8lan(struct e1000_hw *hw, bool enable)
 		if (ret_val)
 			return ret_val;
 		ret_val = e1000e_read_kmrn_reg(hw,
-						E1000_KMRNCTRLSTA_HD_CTRL,
-						&data);
+					       E1000_KMRNCTRLSTA_HD_CTRL,
+					       &data);
 		if (ret_val)
 			return ret_val;
 		data &= ~(0xF << 8);
@@ -2100,7 +2223,7 @@ static void e1000_lan_init_done_ich8lan(struct e1000_hw *hw)
 	do {
 		data = er32(STATUS);
 		data &= E1000_STATUS_LAN_INIT_DONE;
-		udelay(100);
+		usleep_range(100, 200);
 	} while ((!data) && --loop);
 
 	/* If basic configuration is incomplete before the above loop
@@ -2445,7 +2568,7 @@ static s32 e1000_valid_nvm_bank_detect_ich8lan(struct e1000_hw *hw, u32 *bank)
 
 		/* Check bank 0 */
 		ret_val = e1000_read_flash_byte_ich8lan(hw, act_offset,
-		                                        &sig_byte);
+							&sig_byte);
 		if (ret_val)
 			return ret_val;
 		if ((sig_byte & E1000_ICH_NVM_VALID_SIG_MASK) ==
@@ -2456,8 +2579,8 @@ static s32 e1000_valid_nvm_bank_detect_ich8lan(struct e1000_hw *hw, u32 *bank)
 
 		/* Check bank 1 */
 		ret_val = e1000_read_flash_byte_ich8lan(hw, act_offset +
-		                                        bank1_offset,
-		                                        &sig_byte);
+							bank1_offset,
+							&sig_byte);
 		if (ret_val)
 			return ret_val;
 		if ((sig_byte & E1000_ICH_NVM_VALID_SIG_MASK) ==
@@ -2510,8 +2633,8 @@ static s32 e1000_read_nvm_ich8lan(struct e1000_hw *hw, u16 offset, u16 words,
 
 	ret_val = 0;
 	for (i = 0; i < words; i++) {
-		if (dev_spec->shadow_ram[offset+i].modified) {
-			data[i] = dev_spec->shadow_ram[offset+i].value;
+		if (dev_spec->shadow_ram[offset + i].modified) {
+			data[i] = dev_spec->shadow_ram[offset + i].value;
 		} else {
 			ret_val = e1000_read_flash_word_ich8lan(hw,
 								act_offset + i,
@@ -2696,8 +2819,8 @@ static s32 e1000_read_flash_data_ich8lan(struct e1000_hw *hw, u32 offset,
 	if (size < 1  || size > 2 || offset > ICH_FLASH_LINEAR_ADDR_MASK)
 		return -E1000_ERR_NVM;
 
-	flash_linear_addr = (ICH_FLASH_LINEAR_ADDR_MASK & offset) +
-			    hw->nvm.flash_base_addr;
+	flash_linear_addr = ((ICH_FLASH_LINEAR_ADDR_MASK & offset) +
+			     hw->nvm.flash_base_addr);
 
 	do {
 		udelay(1);
@@ -2714,8 +2837,9 @@ static s32 e1000_read_flash_data_ich8lan(struct e1000_hw *hw, u32 offset,
 
 		ew32flash(ICH_FLASH_FADDR, flash_linear_addr);
 
-		ret_val = e1000_flash_cycle_ich8lan(hw,
-						ICH_FLASH_READ_COMMAND_TIMEOUT);
+		ret_val =
+		    e1000_flash_cycle_ich8lan(hw,
+					      ICH_FLASH_READ_COMMAND_TIMEOUT);
 
 		/* Check if FCERR is set to 1, if set to 1, clear it
 		 * and try the whole sequence a few more times, else
@@ -2774,8 +2898,8 @@ static s32 e1000_write_nvm_ich8lan(struct e1000_hw *hw, u16 offset, u16 words,
 	nvm->ops.acquire(hw);
 
 	for (i = 0; i < words; i++) {
-		dev_spec->shadow_ram[offset+i].modified = true;
-		dev_spec->shadow_ram[offset+i].value = data[i];
+		dev_spec->shadow_ram[offset + i].modified = true;
+		dev_spec->shadow_ram[offset + i].value = data[i];
 	}
 
 	nvm->ops.release(hw);
@@ -2844,8 +2968,8 @@ static s32 e1000_update_nvm_checksum_ich8lan(struct e1000_hw *hw)
 			data = dev_spec->shadow_ram[i].value;
 		} else {
 			ret_val = e1000_read_flash_word_ich8lan(hw, i +
-			                                        old_bank_offset,
-			                                        &data);
+								old_bank_offset,
+								&data);
 			if (ret_val)
 				break;
 		}
@@ -2863,7 +2987,7 @@ static s32 e1000_update_nvm_checksum_ich8lan(struct e1000_hw *hw)
 		/* Convert offset to bytes. */
 		act_offset = (i + new_bank_offset) << 1;
 
-		udelay(100);
+		usleep_range(100, 200);
 		/* Write the bytes to the new bank. */
 		ret_val = e1000_retry_write_flash_byte_ich8lan(hw,
 							       act_offset,
@@ -2871,10 +2995,10 @@ static s32 e1000_update_nvm_checksum_ich8lan(struct e1000_hw *hw)
 		if (ret_val)
 			break;
 
-		udelay(100);
+		usleep_range(100, 200);
 		ret_val = e1000_retry_write_flash_byte_ich8lan(hw,
-							  act_offset + 1,
-							  (u8)(data >> 8));
+							       act_offset + 1,
+							       (u8)(data >> 8));
 		if (ret_val)
 			break;
 	}
@@ -3050,8 +3174,8 @@ static s32 e1000_write_flash_data_ich8lan(struct e1000_hw *hw, u32 offset,
 	    offset > ICH_FLASH_LINEAR_ADDR_MASK)
 		return -E1000_ERR_NVM;
 
-	flash_linear_addr = (ICH_FLASH_LINEAR_ADDR_MASK & offset) +
-			    hw->nvm.flash_base_addr;
+	flash_linear_addr = ((ICH_FLASH_LINEAR_ADDR_MASK & offset) +
+			     hw->nvm.flash_base_addr);
 
 	do {
 		udelay(1);
@@ -3062,7 +3186,7 @@ static s32 e1000_write_flash_data_ich8lan(struct e1000_hw *hw, u32 offset,
 
 		hsflctl.regval = er16flash(ICH_FLASH_HSFCTL);
 		/* 0b/1b corresponds to 1 or 2 byte size, respectively. */
-		hsflctl.hsf_ctrl.fldbcount = size -1;
+		hsflctl.hsf_ctrl.fldbcount = size - 1;
 		hsflctl.hsf_ctrl.flcycle = ICH_CYCLE_WRITE;
 		ew16flash(ICH_FLASH_HSFCTL, hsflctl.regval);
 
@@ -3078,8 +3202,9 @@ static s32 e1000_write_flash_data_ich8lan(struct e1000_hw *hw, u32 offset,
 		/* check if FCERR is set to 1 , if set to 1, clear it
 		 * and try the whole sequence a few more times else done
 		 */
-		ret_val = e1000_flash_cycle_ich8lan(hw,
-					       ICH_FLASH_WRITE_COMMAND_TIMEOUT);
+		ret_val =
+		    e1000_flash_cycle_ich8lan(hw,
+					      ICH_FLASH_WRITE_COMMAND_TIMEOUT);
 		if (!ret_val)
 			break;
 
@@ -3138,7 +3263,7 @@ static s32 e1000_retry_write_flash_byte_ich8lan(struct e1000_hw *hw,
 
 	for (program_retries = 0; program_retries < 100; program_retries++) {
 		e_dbg("Retrying Byte %2.2X at offset %u\n", byte, offset);
-		udelay(100);
+		usleep_range(100, 200);
 		ret_val = e1000_write_flash_byte_ich8lan(hw, offset, byte);
 		if (!ret_val)
 			break;
@@ -3209,8 +3334,10 @@ static s32 e1000_erase_flash_bank_ich8lan(struct e1000_hw *hw, u32 bank)
 	flash_linear_addr = hw->nvm.flash_base_addr;
 	flash_linear_addr += (bank) ? flash_bank_size : 0;
 
-	for (j = 0; j < iteration ; j++) {
+	for (j = 0; j < iteration; j++) {
 		do {
+			u32 timeout = ICH_FLASH_ERASE_COMMAND_TIMEOUT;
+
 			/* Steps */
 			ret_val = e1000_flash_cycle_init_ich8lan(hw);
 			if (ret_val)
@@ -3230,8 +3357,7 @@ static s32 e1000_erase_flash_bank_ich8lan(struct e1000_hw *hw, u32 bank)
 			flash_linear_addr += (j * sector_size);
 			ew32flash(ICH_FLASH_FADDR, flash_linear_addr);
 
-			ret_val = e1000_flash_cycle_ich8lan(hw,
-					       ICH_FLASH_ERASE_COMMAND_TIMEOUT);
+			ret_val = e1000_flash_cycle_ich8lan(hw, timeout);
 			if (!ret_val)
 				break;
 
@@ -3270,8 +3396,7 @@ static s32 e1000_valid_led_default_ich8lan(struct e1000_hw *hw, u16 *data)
 		return ret_val;
 	}
 
-	if (*data == ID_LED_RESERVED_0000 ||
-	    *data == ID_LED_RESERVED_FFFF)
+	if (*data == ID_LED_RESERVED_0000 || *data == ID_LED_RESERVED_FFFF)
 		*data = ID_LED_DEFAULT_ICH8LAN;
 
 	return 0;
@@ -3511,9 +3636,9 @@ static s32 e1000_init_hw_ich8lan(struct e1000_hw *hw)
 
 	/* Initialize identification LED */
 	ret_val = mac->ops.id_led_init(hw);
+	/* An error is not fatal and we should not stop init due to this */
 	if (ret_val)
 		e_dbg("Error initializing identification LED\n");
-		/* This is not fatal and we should not stop init due to this */
 
 	/* Setup the receive address. */
 	e1000e_init_rx_addrs(hw, mac->rar_entry_count);
@@ -3541,16 +3666,16 @@ static s32 e1000_init_hw_ich8lan(struct e1000_hw *hw)
 
 	/* Set the transmit descriptor write-back policy for both queues */
 	txdctl = er32(TXDCTL(0));
-	txdctl = (txdctl & ~E1000_TXDCTL_WTHRESH) |
-		 E1000_TXDCTL_FULL_TX_DESC_WB;
-	txdctl = (txdctl & ~E1000_TXDCTL_PTHRESH) |
-		 E1000_TXDCTL_MAX_TX_DESC_PREFETCH;
+	txdctl = ((txdctl & ~E1000_TXDCTL_WTHRESH) |
+		  E1000_TXDCTL_FULL_TX_DESC_WB);
+	txdctl = ((txdctl & ~E1000_TXDCTL_PTHRESH) |
+		  E1000_TXDCTL_MAX_TX_DESC_PREFETCH);
 	ew32(TXDCTL(0), txdctl);
 	txdctl = er32(TXDCTL(1));
-	txdctl = (txdctl & ~E1000_TXDCTL_WTHRESH) |
-		 E1000_TXDCTL_FULL_TX_DESC_WB;
-	txdctl = (txdctl & ~E1000_TXDCTL_PTHRESH) |
-		 E1000_TXDCTL_MAX_TX_DESC_PREFETCH;
+	txdctl = ((txdctl & ~E1000_TXDCTL_WTHRESH) |
+		  E1000_TXDCTL_FULL_TX_DESC_WB);
+	txdctl = ((txdctl & ~E1000_TXDCTL_PTHRESH) |
+		  E1000_TXDCTL_MAX_TX_DESC_PREFETCH);
 	ew32(TXDCTL(1), txdctl);
 
 	/* ICH8 has opposite polarity of no_snoop bits.
@@ -3559,7 +3684,7 @@ static s32 e1000_init_hw_ich8lan(struct e1000_hw *hw)
 	if (mac->type == e1000_ich8lan)
 		snoop = PCIE_ICH8_SNOOP_ALL;
 	else
-		snoop = (u32) ~(PCIE_NO_SNOOP_ALL);
+		snoop = (u32)~(PCIE_NO_SNOOP_ALL);
 	e1000e_set_pcie_no_snoop(hw, snoop);
 
 	ctrl_ext = er32(CTRL_EXT);
@@ -3575,6 +3700,7 @@ static s32 e1000_init_hw_ich8lan(struct e1000_hw *hw)
 
 	return ret_val;
 }
+
 /**
  *  e1000_initialize_hw_bits_ich8lan - Initialize required hardware bits
  *  @hw: pointer to the HW structure
@@ -3686,8 +3812,7 @@ static s32 e1000_setup_link_ich8lan(struct e1000_hw *hw)
 	 */
 	hw->fc.current_mode = hw->fc.requested_mode;
 
-	e_dbg("After fix-ups FlowControl is now = %x\n",
-		hw->fc.current_mode);
+	e_dbg("After fix-ups FlowControl is now = %x\n", hw->fc.current_mode);
 
 	/* Continue to configure the copper link. */
 	ret_val = hw->mac.ops.setup_physical_interface(hw);
@@ -3737,12 +3862,12 @@ static s32 e1000_setup_copper_link_ich8lan(struct e1000_hw *hw)
 	if (ret_val)
 		return ret_val;
 	ret_val = e1000e_read_kmrn_reg(hw, E1000_KMRNCTRLSTA_INBAND_PARAM,
-	                               &reg_data);
+				       &reg_data);
 	if (ret_val)
 		return ret_val;
 	reg_data |= 0x3F;
 	ret_val = e1000e_write_kmrn_reg(hw, E1000_KMRNCTRLSTA_INBAND_PARAM,
-	                                reg_data);
+					reg_data);
 	if (ret_val)
 		return ret_val;
 
@@ -3760,7 +3885,6 @@ static s32 e1000_setup_copper_link_ich8lan(struct e1000_hw *hw)
 		break;
 	case e1000_phy_82577:
 	case e1000_phy_82579:
-	case e1000_phy_i217:
 		ret_val = e1000_copper_link_setup_82577(hw);
 		if (ret_val)
 			return ret_val;
@@ -3796,6 +3920,31 @@ static s32 e1000_setup_copper_link_ich8lan(struct e1000_hw *hw)
 }
 
 /**
+ *  e1000_setup_copper_link_pch_lpt - Configure MAC/PHY interface
+ *  @hw: pointer to the HW structure
+ *
+ *  Calls the PHY specific link setup function and then calls the
+ *  generic setup_copper_link to finish configuring the link for
+ *  Lynxpoint PCH devices
+ **/
+static s32 e1000_setup_copper_link_pch_lpt(struct e1000_hw *hw)
+{
+	u32 ctrl;
+	s32 ret_val;
+
+	ctrl = er32(CTRL);
+	ctrl |= E1000_CTRL_SLU;
+	ctrl &= ~(E1000_CTRL_FRCSPD | E1000_CTRL_FRCDPX);
+	ew32(CTRL, ctrl);
+
+	ret_val = e1000_copper_link_setup_82577(hw);
+	if (ret_val)
+		return ret_val;
+
+	return e1000e_setup_copper_link(hw);
+}
+
+/**
  *  e1000_get_link_up_info_ich8lan - Get current link speed and duplex
  *  @hw: pointer to the HW structure
  *  @speed: pointer to store current link speed
@@ -3815,8 +3964,7 @@ static s32 e1000_get_link_up_info_ich8lan(struct e1000_hw *hw, u16 *speed,
 		return ret_val;
 
 	if ((hw->mac.type == e1000_ich8lan) &&
-	    (hw->phy.type == e1000_phy_igp_3) &&
-	    (*speed == SPEED_1000)) {
+	    (hw->phy.type == e1000_phy_igp_3) && (*speed == SPEED_1000)) {
 		ret_val = e1000_kmrn_lock_loss_workaround_ich8lan(hw);
 	}
 
@@ -3899,7 +4047,7 @@ static s32 e1000_kmrn_lock_loss_workaround_ich8lan(struct e1000_hw *hw)
  *  /disabled - false).
  **/
 void e1000e_set_kmrn_lock_loss_workaround_ich8lan(struct e1000_hw *hw,
-						 bool state)
+						  bool state)
 {
 	struct e1000_dev_spec_ich8lan *dev_spec = &hw->dev_spec.ich8lan;
 
@@ -3981,12 +4129,12 @@ void e1000e_gig_downshift_workaround_ich8lan(struct e1000_hw *hw)
 		return;
 
 	ret_val = e1000e_read_kmrn_reg(hw, E1000_KMRNCTRLSTA_DIAG_OFFSET,
-				      &reg_data);
+				       &reg_data);
 	if (ret_val)
 		return;
 	reg_data |= E1000_KMRNCTRLSTA_DIAG_NELPBK;
 	ret_val = e1000e_write_kmrn_reg(hw, E1000_KMRNCTRLSTA_DIAG_OFFSET,
-				       reg_data);
+					reg_data);
 	if (ret_val)
 		return;
 	reg_data &= ~E1000_KMRNCTRLSTA_DIAG_NELPBK;
diff --git a/drivers/net/ethernet/intel/e1000e/ich8lan.h b/drivers/net/ethernet/intel/e1000e/ich8lan.h
index 8bf4655c2e17..80034a2b297c 100644
--- a/drivers/net/ethernet/intel/e1000e/ich8lan.h
+++ b/drivers/net/ethernet/intel/e1000e/ich8lan.h
@@ -211,7 +211,8 @@
 #define I82579_MSE_THRESHOLD	0x084F	/* 82579 Mean Square Error Threshold */
 #define I82577_MSE_THRESHOLD	0x0887	/* 82577 Mean Square Error Threshold */
 #define I82579_MSE_LINK_DOWN	0x2411	/* MSE count before dropping link */
-#define I82579_EEE_PCS_STATUS		0x182D	/* IEEE MMD Register 3.1 >> 8 */
+#define I82579_RX_CONFIG		0x3412	/* Receive configuration */
+#define I82579_EEE_PCS_STATUS		0x182E	/* IEEE MMD Register 3.1 >> 8 */
 #define I82579_EEE_CAPABILITY		0x0410	/* IEEE MMD Register 3.20 */
 #define I82579_EEE_ADVERTISEMENT	0x040E	/* IEEE MMD Register 7.60 */
 #define I82579_EEE_LP_ABILITY		0x040F	/* IEEE MMD Register 7.61 */
@@ -249,13 +250,6 @@
 /* Proprietary Latency Tolerance Reporting PCI Capability */
 #define E1000_PCI_LTR_CAP_LPT		0xA8
 
-/* OBFF Control & Threshold Defines */
-#define E1000_SVCR_OFF_EN		0x00000001
-#define E1000_SVCR_OFF_MASKINT		0x00001000
-#define E1000_SVCR_OFF_TIMER_MASK	0xFFFF0000
-#define E1000_SVCR_OFF_TIMER_SHIFT	16
-#define E1000_SVT_OFF_HWM_MASK		0x0000001F
-
 void e1000e_write_protect_nvm_ich8lan(struct e1000_hw *hw);
 void e1000e_set_kmrn_lock_loss_workaround_ich8lan(struct e1000_hw *hw,
 						  bool state);
@@ -267,4 +261,5 @@ s32 e1000_configure_k1_ich8lan(struct e1000_hw *hw, bool k1_enable);
 void e1000_copy_rx_addrs_to_phy_ich8lan(struct e1000_hw *hw);
 s32 e1000_lv_jumbo_workaround_ich8lan(struct e1000_hw *hw, bool enable);
 s32 e1000_read_emi_reg_locked(struct e1000_hw *hw, u16 addr, u16 *data);
+s32 e1000_write_emi_reg_locked(struct e1000_hw *hw, u16 addr, u16 data);
 #endif /* _E1000E_ICH8LAN_H_ */
diff --git a/drivers/net/ethernet/intel/e1000e/mac.c b/drivers/net/ethernet/intel/e1000e/mac.c
index b78e02174601..2480c1091873 100644
--- a/drivers/net/ethernet/intel/e1000e/mac.c
+++ b/drivers/net/ethernet/intel/e1000e/mac.c
@@ -596,7 +596,7 @@ s32 e1000e_check_for_serdes_link(struct e1000_hw *hw)
 		 * serdes media type.
 		 */
 		/* SYNCH bit and IV bit are sticky. */
-		udelay(10);
+		usleep_range(10, 20);
 		rxcw = er32(RXCW);
 		if (rxcw & E1000_RXCW_SYNCH) {
 			if (!(rxcw & E1000_RXCW_IV)) {
@@ -613,7 +613,7 @@ s32 e1000e_check_for_serdes_link(struct e1000_hw *hw)
 		status = er32(STATUS);
 		if (status & E1000_STATUS_LU) {
 			/* SYNCH bit and IV bit are sticky, so reread rxcw. */
-			udelay(10);
+			usleep_range(10, 20);
 			rxcw = er32(RXCW);
 			if (rxcw & E1000_RXCW_SYNCH) {
 				if (!(rxcw & E1000_RXCW_IV)) {
@@ -1382,7 +1382,7 @@ s32 e1000e_get_hw_semaphore(struct e1000_hw *hw)
 		if (!(swsm & E1000_SWSM_SMBI))
 			break;
 
-		udelay(50);
+		usleep_range(50, 100);
 		i++;
 	}
 
@@ -1400,7 +1400,7 @@ s32 e1000e_get_hw_semaphore(struct e1000_hw *hw)
 		if (er32(SWSM) & E1000_SWSM_SWESMBI)
 			break;
 
-		udelay(50);
+		usleep_range(50, 100);
 	}
 
 	if (i == timeout) {
@@ -1600,15 +1600,28 @@ s32 e1000e_blink_led_generic(struct e1000_hw *hw)
 		ledctl_blink = E1000_LEDCTL_LED0_BLINK |
 		    (E1000_LEDCTL_MODE_LED_ON << E1000_LEDCTL_LED0_MODE_SHIFT);
 	} else {
-		/* set the blink bit for each LED that's "on" (0x0E)
-		 * in ledctl_mode2
+		/* Set the blink bit for each LED that's "on" (0x0E)
+		 * (or "off" if inverted) in ledctl_mode2.  The blink
+		 * logic in hardware only works when mode is set to "on"
+		 * so it must be changed accordingly when the mode is
+		 * "off" and inverted.
 		 */
 		ledctl_blink = hw->mac.ledctl_mode2;
-		for (i = 0; i < 4; i++)
-			if (((hw->mac.ledctl_mode2 >> (i * 8)) & 0xFF) ==
-			    E1000_LEDCTL_MODE_LED_ON)
-				ledctl_blink |= (E1000_LEDCTL_LED0_BLINK <<
-						 (i * 8));
+		for (i = 0; i < 32; i += 8) {
+			u32 mode = (hw->mac.ledctl_mode2 >> i) &
+			    E1000_LEDCTL_LED0_MODE_MASK;
+			u32 led_default = hw->mac.ledctl_default >> i;
+
+			if ((!(led_default & E1000_LEDCTL_LED0_IVRT) &&
+			     (mode == E1000_LEDCTL_MODE_LED_ON)) ||
+			    ((led_default & E1000_LEDCTL_LED0_IVRT) &&
+			     (mode == E1000_LEDCTL_MODE_LED_OFF))) {
+				ledctl_blink &=
+				    ~(E1000_LEDCTL_LED0_MODE_MASK << i);
+				ledctl_blink |= (E1000_LEDCTL_LED0_BLINK |
+						 E1000_LEDCTL_MODE_LED_ON) << i;
+			}
+		}
 	}
 
 	ew32(LEDCTL, ledctl_blink);
@@ -1712,7 +1725,7 @@ s32 e1000e_disable_pcie_master(struct e1000_hw *hw)
 	while (timeout) {
 		if (!(er32(STATUS) & E1000_STATUS_GIO_MASTER_ENABLE))
 			break;
-		udelay(100);
+		usleep_range(100, 200);
 		timeout--;
 	}
 
diff --git a/drivers/net/ethernet/intel/e1000e/netdev.c b/drivers/net/ethernet/intel/e1000e/netdev.c
index 7e615e2bf7e6..a27e3bcc3249 100644
--- a/drivers/net/ethernet/intel/e1000e/netdev.c
+++ b/drivers/net/ethernet/intel/e1000e/netdev.c
@@ -55,7 +55,7 @@
 
 #define DRV_EXTRAVERSION "-k"
 
-#define DRV_VERSION "2.2.14" DRV_EXTRAVERSION
+#define DRV_VERSION "2.3.2" DRV_EXTRAVERSION
 char e1000e_driver_name[] = "e1000e";
 const char e1000e_driver_version[] = DRV_VERSION;
 
@@ -219,9 +219,8 @@ static void e1000e_dump(struct e1000_adapter *adapter)
 	if (netdev) {
 		dev_info(&adapter->pdev->dev, "Net device Info\n");
 		pr_info("Device Name     state            trans_start      last_rx\n");
-		pr_info("%-15s %016lX %016lX %016lX\n",
-			netdev->name, netdev->state, netdev->trans_start,
-			netdev->last_rx);
+		pr_info("%-15s %016lX %016lX %016lX\n", netdev->name,
+			netdev->state, netdev->trans_start, netdev->last_rx);
 	}
 
 	/* Print Registers */
@@ -555,7 +554,7 @@ static void e1000_receive_skb(struct e1000_adapter *adapter,
 	skb->protocol = eth_type_trans(skb, netdev);
 
 	if (staterr & E1000_RXD_STAT_VP)
-		__vlan_hwaccel_put_tag(skb, tag);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), tag);
 
 	napi_gro_receive(&adapter->napi, skb);
 }
@@ -755,8 +754,7 @@ static void e1000_alloc_rx_buffers_ps(struct e1000_ring *rx_ring,
 			    cpu_to_le64(ps_page->dma);
 		}
 
-		skb = __netdev_alloc_skb_ip_align(netdev,
-						  adapter->rx_ps_bsize0,
+		skb = __netdev_alloc_skb_ip_align(netdev, adapter->rx_ps_bsize0,
 						  gfp);
 
 		if (!skb) {
@@ -850,8 +848,8 @@ check_page:
 
 		if (!buffer_info->dma) {
 			buffer_info->dma = dma_map_page(&pdev->dev,
-			                                buffer_info->page, 0,
-			                                PAGE_SIZE,
+							buffer_info->page, 0,
+							PAGE_SIZE,
 							DMA_FROM_DEVICE);
 			if (dma_mapping_error(&pdev->dev, buffer_info->dma)) {
 				adapter->alloc_rx_buff_failed++;
@@ -942,10 +940,8 @@ static bool e1000_clean_rx_irq(struct e1000_ring *rx_ring, int *work_done,
 
 		cleaned = true;
 		cleaned_count++;
-		dma_unmap_single(&pdev->dev,
-				 buffer_info->dma,
-				 adapter->rx_buffer_len,
-				 DMA_FROM_DEVICE);
+		dma_unmap_single(&pdev->dev, buffer_info->dma,
+				 adapter->rx_buffer_len, DMA_FROM_DEVICE);
 		buffer_info->dma = 0;
 
 		length = le16_to_cpu(rx_desc->wb.upper.length);
@@ -1073,8 +1069,8 @@ static void e1000_put_txbuf(struct e1000_ring *tx_ring,
 static void e1000_print_hw_hang(struct work_struct *work)
 {
 	struct e1000_adapter *adapter = container_of(work,
-	                                             struct e1000_adapter,
-	                                             print_hang_task);
+						     struct e1000_adapter,
+						     print_hang_task);
 	struct net_device *netdev = adapter->netdev;
 	struct e1000_ring *tx_ring = adapter->tx_ring;
 	unsigned int i = tx_ring->next_to_clean;
@@ -1087,8 +1083,7 @@ static void e1000_print_hw_hang(struct work_struct *work)
 	if (test_bit(__E1000_DOWN, &adapter->state))
 		return;
 
-	if (!adapter->tx_hang_recheck &&
-	    (adapter->flags2 & FLAG2_DMA_BURST)) {
+	if (!adapter->tx_hang_recheck && (adapter->flags2 & FLAG2_DMA_BURST)) {
 		/* May be block on write-back, flush and detect again
 		 * flush pending descriptor writebacks to memory
 		 */
@@ -1130,19 +1125,10 @@ static void e1000_print_hw_hang(struct work_struct *work)
 	      "PHY 1000BASE-T Status  <%x>\n"
 	      "PHY Extended Status    <%x>\n"
 	      "PCI Status             <%x>\n",
-	      readl(tx_ring->head),
-	      readl(tx_ring->tail),
-	      tx_ring->next_to_use,
-	      tx_ring->next_to_clean,
-	      tx_ring->buffer_info[eop].time_stamp,
-	      eop,
-	      jiffies,
-	      eop_desc->upper.fields.status,
-	      er32(STATUS),
-	      phy_status,
-	      phy_1000t_status,
-	      phy_ext_status,
-	      pci_status);
+	      readl(tx_ring->head), readl(tx_ring->tail), tx_ring->next_to_use,
+	      tx_ring->next_to_clean, tx_ring->buffer_info[eop].time_stamp,
+	      eop, jiffies, eop_desc->upper.fields.status, er32(STATUS),
+	      phy_status, phy_1000t_status, phy_ext_status, pci_status);
 
 	/* Suggest workaround for known h/w issue */
 	if ((hw->mac.type == e1000_pchlan) && (er32(CTRL) & E1000_CTRL_TFCE))
@@ -1435,7 +1421,7 @@ copydone:
 		e1000_rx_hash(netdev, rx_desc->wb.lower.hi_dword.rss, skb);
 
 		if (rx_desc->wb.upper.header_status &
-			   cpu_to_le16(E1000_RXDPS_HDRSTAT_HDRSP))
+		    cpu_to_le16(E1000_RXDPS_HDRSTAT_HDRSP))
 			adapter->rx_hdr_split++;
 
 		e1000_receive_skb(adapter, netdev, skb, staterr,
@@ -1473,7 +1459,7 @@ next_desc:
  * e1000_consume_page - helper function
  **/
 static void e1000_consume_page(struct e1000_buffer *bi, struct sk_buff *skb,
-                               u16 length)
+			       u16 length)
 {
 	bi->page = NULL;
 	skb->len += length;
@@ -1500,7 +1486,8 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_ring *rx_ring, int *work_done,
 	unsigned int i;
 	int cleaned_count = 0;
 	bool cleaned = false;
-	unsigned int total_rx_bytes=0, total_rx_packets=0;
+	unsigned int total_rx_bytes = 0, total_rx_packets = 0;
+	struct skb_shared_info *shinfo;
 
 	i = rx_ring->next_to_clean;
 	rx_desc = E1000_RX_DESC_EXT(*rx_ring, i);
@@ -1546,7 +1533,6 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_ring *rx_ring, int *work_done,
 			rx_ring->rx_skb_top = NULL;
 			goto next_desc;
 		}
-
 #define rxtop (rx_ring->rx_skb_top)
 		if (!(staterr & E1000_RXD_STAT_EOP)) {
 			/* this descriptor is only the beginning (or middle) */
@@ -1554,12 +1540,13 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_ring *rx_ring, int *work_done,
 				/* this is the beginning of a chain */
 				rxtop = skb;
 				skb_fill_page_desc(rxtop, 0, buffer_info->page,
-				                   0, length);
+						   0, length);
 			} else {
 				/* this is the middle of a chain */
-				skb_fill_page_desc(rxtop,
-				    skb_shinfo(rxtop)->nr_frags,
-				    buffer_info->page, 0, length);
+				shinfo = skb_shinfo(rxtop);
+				skb_fill_page_desc(rxtop, shinfo->nr_frags,
+						   buffer_info->page, 0,
+						   length);
 				/* re-use the skb, only consumed the page */
 				buffer_info->skb = skb;
 			}
@@ -1568,9 +1555,10 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_ring *rx_ring, int *work_done,
 		} else {
 			if (rxtop) {
 				/* end of the chain */
-				skb_fill_page_desc(rxtop,
-				    skb_shinfo(rxtop)->nr_frags,
-				    buffer_info->page, 0, length);
+				shinfo = skb_shinfo(rxtop);
+				skb_fill_page_desc(rxtop, shinfo->nr_frags,
+						   buffer_info->page, 0,
+						   length);
 				/* re-use the current skb, we only consumed the
 				 * page
 				 */
@@ -1595,10 +1583,10 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_ring *rx_ring, int *work_done,
 					skb_put(skb, length);
 				} else {
 					skb_fill_page_desc(skb, 0,
-					                   buffer_info->page, 0,
-				                           length);
+							   buffer_info->page, 0,
+							   length);
 					e1000_consume_page(buffer_info, skb,
-					                   length);
+							   length);
 				}
 			}
 		}
@@ -1671,8 +1659,7 @@ static void e1000_clean_rx_ring(struct e1000_ring *rx_ring)
 						 DMA_FROM_DEVICE);
 			else if (adapter->clean_rx == e1000_clean_jumbo_rx_irq)
 				dma_unmap_page(&pdev->dev, buffer_info->dma,
-				               PAGE_SIZE,
-					       DMA_FROM_DEVICE);
+					       PAGE_SIZE, DMA_FROM_DEVICE);
 			else if (adapter->clean_rx == e1000_clean_rx_irq_ps)
 				dma_unmap_single(&pdev->dev, buffer_info->dma,
 						 adapter->rx_ps_bsize0,
@@ -1725,7 +1712,8 @@ static void e1000_clean_rx_ring(struct e1000_ring *rx_ring)
 static void e1000e_downshift_workaround(struct work_struct *work)
 {
 	struct e1000_adapter *adapter = container_of(work,
-					struct e1000_adapter, downshift_task);
+						     struct e1000_adapter,
+						     downshift_task);
 
 	if (test_bit(__E1000_DOWN, &adapter->state))
 		return;
@@ -1918,7 +1906,6 @@ static irqreturn_t e1000_intr_msix_tx(int __always_unused irq, void *data)
 	struct e1000_hw *hw = &adapter->hw;
 	struct e1000_ring *tx_ring = adapter->tx_ring;
 
-
 	adapter->total_tx_bytes = 0;
 	adapter->total_tx_packets = 0;
 
@@ -1975,7 +1962,6 @@ static void e1000_configure_msix(struct e1000_adapter *adapter)
 		ew32(RFCTL, rfctl);
 	}
 
-#define E1000_IVAR_INT_ALLOC_VALID	0x8
 	/* Configure Rx vector */
 	rx_ring->ims_val = E1000_IMS_RXQ0;
 	adapter->eiac_mask |= rx_ring->ims_val;
@@ -2050,8 +2036,9 @@ void e1000e_set_interrupt_capability(struct e1000_adapter *adapter)
 		if (adapter->flags & FLAG_HAS_MSIX) {
 			adapter->num_vectors = 3; /* RxQ0, TxQ0 and other */
 			adapter->msix_entries = kcalloc(adapter->num_vectors,
-						      sizeof(struct msix_entry),
-						      GFP_KERNEL);
+							sizeof(struct
+							       msix_entry),
+							GFP_KERNEL);
 			if (adapter->msix_entries) {
 				for (i = 0; i < adapter->num_vectors; i++)
 					adapter->msix_entries[i].entry = i;
@@ -2495,7 +2482,7 @@ static unsigned int e1000_update_itr(u16 itr_setting, int packets, int bytes)
 	switch (itr_setting) {
 	case lowest_latency:
 		/* handle TSO and jumbo frames */
-		if (bytes/packets > 8000)
+		if (bytes / packets > 8000)
 			retval = bulk_latency;
 		else if ((packets < 5) && (bytes > 512))
 			retval = low_latency;
@@ -2503,13 +2490,13 @@ static unsigned int e1000_update_itr(u16 itr_setting, int packets, int bytes)
 	case low_latency:  /* 50 usec aka 20000 ints/s */
 		if (bytes > 10000) {
 			/* this if handles the TSO accounting */
-			if (bytes/packets > 8000)
+			if (bytes / packets > 8000)
 				retval = bulk_latency;
-			else if ((packets < 10) || ((bytes/packets) > 1200))
+			else if ((packets < 10) || ((bytes / packets) > 1200))
 				retval = bulk_latency;
 			else if ((packets > 35))
 				retval = lowest_latency;
-		} else if (bytes/packets > 2000) {
+		} else if (bytes / packets > 2000) {
 			retval = bulk_latency;
 		} else if (packets <= 2 && bytes < 512) {
 			retval = lowest_latency;
@@ -2561,8 +2548,8 @@ static void e1000_set_itr(struct e1000_adapter *adapter)
 
 	current_itr = max(adapter->rx_itr, adapter->tx_itr);
 
-	switch (current_itr) {
 	/* counts and packets in update_itr are dependent on these numbers */
+	switch (current_itr) {
 	case lowest_latency:
 		new_itr = 70000;
 		break;
@@ -2583,8 +2570,7 @@ set_itr_now:
 		 * increasing
 		 */
 		new_itr = new_itr > adapter->itr ?
-			     min(adapter->itr + (new_itr >> 2), new_itr) :
-			     new_itr;
+		    min(adapter->itr + (new_itr >> 2), new_itr) : new_itr;
 		adapter->itr = new_itr;
 		adapter->rx_ring->itr_val = new_itr;
 		if (adapter->msix_entries)
@@ -2686,7 +2672,8 @@ static int e1000e_poll(struct napi_struct *napi, int weight)
 	return work_done;
 }
 
-static int e1000_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
+static int e1000_vlan_rx_add_vid(struct net_device *netdev,
+				 __be16 proto, u16 vid)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
@@ -2711,7 +2698,8 @@ static int e1000_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
 	return 0;
 }
 
-static int e1000_vlan_rx_kill_vid(struct net_device *netdev, u16 vid)
+static int e1000_vlan_rx_kill_vid(struct net_device *netdev,
+				  __be16 proto, u16 vid)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
@@ -2755,7 +2743,8 @@ static void e1000e_vlan_filter_disable(struct e1000_adapter *adapter)
 		ew32(RCTL, rctl);
 
 		if (adapter->mng_vlan_id != (u16)E1000_MNG_VLAN_NONE) {
-			e1000_vlan_rx_kill_vid(netdev, adapter->mng_vlan_id);
+			e1000_vlan_rx_kill_vid(netdev, htons(ETH_P_8021Q),
+					       adapter->mng_vlan_id);
 			adapter->mng_vlan_id = E1000_MNG_VLAN_NONE;
 		}
 	}
@@ -2815,24 +2804,23 @@ static void e1000_update_mng_vlan(struct e1000_adapter *adapter)
 	u16 vid = adapter->hw.mng_cookie.vlan_id;
 	u16 old_vid = adapter->mng_vlan_id;
 
-	if (adapter->hw.mng_cookie.status &
-	    E1000_MNG_DHCP_COOKIE_STATUS_VLAN) {
-		e1000_vlan_rx_add_vid(netdev, vid);
+	if (adapter->hw.mng_cookie.status & E1000_MNG_DHCP_COOKIE_STATUS_VLAN) {
+		e1000_vlan_rx_add_vid(netdev, htons(ETH_P_8021Q), vid);
 		adapter->mng_vlan_id = vid;
 	}
 
 	if ((old_vid != (u16)E1000_MNG_VLAN_NONE) && (vid != old_vid))
-		e1000_vlan_rx_kill_vid(netdev, old_vid);
+		e1000_vlan_rx_kill_vid(netdev, htons(ETH_P_8021Q), old_vid);
 }
 
 static void e1000_restore_vlan(struct e1000_adapter *adapter)
 {
 	u16 vid;
 
-	e1000_vlan_rx_add_vid(adapter->netdev, 0);
+	e1000_vlan_rx_add_vid(adapter->netdev, htons(ETH_P_8021Q), 0);
 
 	for_each_set_bit(vid, adapter->active_vlans, VLAN_N_VID)
-		e1000_vlan_rx_add_vid(adapter->netdev, vid);
+	    e1000_vlan_rx_add_vid(adapter->netdev, htons(ETH_P_8021Q), vid);
 }
 
 static void e1000_init_manageability_pt(struct e1000_adapter *adapter)
@@ -3007,8 +2995,8 @@ static void e1000_setup_rctl(struct e1000_adapter *adapter)
 	rctl = er32(RCTL);
 	rctl &= ~(3 << E1000_RCTL_MO_SHIFT);
 	rctl |= E1000_RCTL_EN | E1000_RCTL_BAM |
-		E1000_RCTL_LBM_NO | E1000_RCTL_RDMTS_HALF |
-		(adapter->hw.mac.mc_filter_type << E1000_RCTL_MO_SHIFT);
+	    E1000_RCTL_LBM_NO | E1000_RCTL_RDMTS_HALF |
+	    (adapter->hw.mac.mc_filter_type << E1000_RCTL_MO_SHIFT);
 
 	/* Do not Store bad packets */
 	rctl &= ~E1000_RCTL_SBP;
@@ -3094,19 +3082,17 @@ static void e1000_setup_rctl(struct e1000_adapter *adapter)
 		/* Enable Packet split descriptors */
 		rctl |= E1000_RCTL_DTYP_PS;
 
-		psrctl |= adapter->rx_ps_bsize0 >>
-			E1000_PSRCTL_BSIZE0_SHIFT;
+		psrctl |= adapter->rx_ps_bsize0 >> E1000_PSRCTL_BSIZE0_SHIFT;
 
 		switch (adapter->rx_ps_pages) {
 		case 3:
-			psrctl |= PAGE_SIZE <<
-				E1000_PSRCTL_BSIZE3_SHIFT;
+			psrctl |= PAGE_SIZE << E1000_PSRCTL_BSIZE3_SHIFT;
+			/* fall-through */
 		case 2:
-			psrctl |= PAGE_SIZE <<
-				E1000_PSRCTL_BSIZE2_SHIFT;
+			psrctl |= PAGE_SIZE << E1000_PSRCTL_BSIZE2_SHIFT;
+			/* fall-through */
 		case 1:
-			psrctl |= PAGE_SIZE >>
-				E1000_PSRCTL_BSIZE1_SHIFT;
+			psrctl |= PAGE_SIZE >> E1000_PSRCTL_BSIZE1_SHIFT;
 			break;
 		}
 
@@ -3280,7 +3266,7 @@ static int e1000e_write_mc_addr_list(struct net_device *netdev)
 	/* update_mc_addr_list expects a packed array of only addresses. */
 	i = 0;
 	netdev_for_each_mc_addr(ha, netdev)
-		memcpy(mta_list + (i++ * ETH_ALEN), ha->addr, ETH_ALEN);
+	    memcpy(mta_list + (i++ * ETH_ALEN), ha->addr, ETH_ALEN);
 
 	hw->mac.ops.update_mc_addr_list(hw, mta_list, i);
 	kfree(mta_list);
@@ -3390,7 +3376,7 @@ static void e1000e_set_rx_mode(struct net_device *netdev)
 
 	ew32(RCTL, rctl);
 
-	if (netdev->features & NETIF_F_HW_VLAN_RX)
+	if (netdev->features & NETIF_F_HW_VLAN_CTAG_RX)
 		e1000e_vlan_strip_enable(adapter);
 	else
 		e1000e_vlan_strip_disable(adapter);
@@ -3757,8 +3743,7 @@ void e1000e_reset(struct e1000_adapter *adapter)
 		 * but don't include ethernet FCS because hardware appends it
 		 */
 		min_tx_space = (adapter->max_frame_size +
-				sizeof(struct e1000_tx_desc) -
-				ETH_FCS_LEN) * 2;
+				sizeof(struct e1000_tx_desc) - ETH_FCS_LEN) * 2;
 		min_tx_space = ALIGN(min_tx_space, 1024);
 		min_tx_space >>= 10;
 		/* software strips receive CRC, so leave room for it */
@@ -3861,13 +3846,13 @@ void e1000e_reset(struct e1000_adapter *adapter)
 		if ((adapter->max_frame_size * 2) > (pba << 10)) {
 			if (!(adapter->flags2 & FLAG2_DISABLE_AIM)) {
 				dev_info(&adapter->pdev->dev,
-					"Interrupt Throttle Rate turned off\n");
+					 "Interrupt Throttle Rate off\n");
 				adapter->flags2 |= FLAG2_DISABLE_AIM;
 				e1000e_write_itr(adapter, 0);
 			}
 		} else if (adapter->flags2 & FLAG2_DISABLE_AIM) {
 			dev_info(&adapter->pdev->dev,
-				 "Interrupt Throttle Rate turned on\n");
+				 "Interrupt Throttle Rate on\n");
 			adapter->flags2 &= ~FLAG2_DISABLE_AIM;
 			adapter->itr = 20000;
 			e1000e_write_itr(adapter, adapter->itr);
@@ -3898,6 +3883,38 @@ void e1000e_reset(struct e1000_adapter *adapter)
 	/* initialize systim and reset the ns time counter */
 	e1000e_config_hwtstamp(adapter);
 
+	/* Set EEE advertisement as appropriate */
+	if (adapter->flags2 & FLAG2_HAS_EEE) {
+		s32 ret_val;
+		u16 adv_addr;
+
+		switch (hw->phy.type) {
+		case e1000_phy_82579:
+			adv_addr = I82579_EEE_ADVERTISEMENT;
+			break;
+		case e1000_phy_i217:
+			adv_addr = I217_EEE_ADVERTISEMENT;
+			break;
+		default:
+			dev_err(&adapter->pdev->dev,
+				"Invalid PHY type setting EEE advertisement\n");
+			return;
+		}
+
+		ret_val = hw->phy.ops.acquire(hw);
+		if (ret_val) {
+			dev_err(&adapter->pdev->dev,
+				"EEE advertisement - unable to acquire PHY\n");
+			return;
+		}
+
+		e1000_write_emi_reg_locked(hw, adv_addr,
+					   hw->dev_spec.ich8lan.eee_disable ?
+					   0 : adapter->eee_advert);
+
+		hw->phy.ops.release(hw);
+	}
+
 	if (!netif_running(adapter->netdev) &&
 	    !test_bit(__E1000_TESTING, &adapter->state)) {
 		e1000_power_down_phy(adapter);
@@ -3999,6 +4016,8 @@ void e1000e_down(struct e1000_adapter *adapter)
 
 	e1000_irq_disable(adapter);
 
+	napi_synchronize(&adapter->napi);
+
 	del_timer_sync(&adapter->watchdog_timer);
 	del_timer_sync(&adapter->phy_info_timer);
 
@@ -4266,8 +4285,7 @@ static int e1000_open(struct net_device *netdev)
 	e1000e_power_up_phy(adapter);
 
 	adapter->mng_vlan_id = E1000_MNG_VLAN_NONE;
-	if ((adapter->hw.mng_cookie.status &
-	     E1000_MNG_DHCP_COOKIE_STATUS_VLAN))
+	if ((adapter->hw.mng_cookie.status & E1000_MNG_DHCP_COOKIE_STATUS_VLAN))
 		e1000_update_mng_vlan(adapter);
 
 	/* DMA latency requirement to workaround jumbo issue */
@@ -4356,12 +4374,13 @@ static int e1000_close(struct net_device *netdev)
 
 	pm_runtime_get_sync(&pdev->dev);
 
-	napi_disable(&adapter->napi);
-
 	if (!test_bit(__E1000_DOWN, &adapter->state)) {
 		e1000e_down(adapter);
 		e1000_free_irq(adapter);
 	}
+
+	napi_disable(&adapter->napi);
+
 	e1000_power_down_phy(adapter);
 
 	e1000e_free_tx_resources(adapter->tx_ring);
@@ -4370,9 +4389,9 @@ static int e1000_close(struct net_device *netdev)
 	/* kill manageability vlan ID if supported, but not if a vlan with
 	 * the same ID is registered on the host OS (let 8021q kill it)
 	 */
-	if (adapter->hw.mng_cookie.status &
-	    E1000_MNG_DHCP_COOKIE_STATUS_VLAN)
-		e1000_vlan_rx_kill_vid(netdev, adapter->mng_vlan_id);
+	if (adapter->hw.mng_cookie.status & E1000_MNG_DHCP_COOKIE_STATUS_VLAN)
+		e1000_vlan_rx_kill_vid(netdev, htons(ETH_P_8021Q),
+				       adapter->mng_vlan_id);
 
 	/* If AMT is enabled, let the firmware know that the network
 	 * interface is now closed
@@ -4387,6 +4406,7 @@ static int e1000_close(struct net_device *netdev)
 
 	return 0;
 }
+
 /**
  * e1000_set_mac - Change the Ethernet Address of the NIC
  * @netdev: network interface device structure
@@ -4437,7 +4457,8 @@ static int e1000_set_mac(struct net_device *netdev, void *p)
 static void e1000e_update_phy_task(struct work_struct *work)
 {
 	struct e1000_adapter *adapter = container_of(work,
-					struct e1000_adapter, update_phy_task);
+						     struct e1000_adapter,
+						     update_phy_task);
 
 	if (test_bit(__E1000_DOWN, &adapter->state))
 		return;
@@ -4454,7 +4475,7 @@ static void e1000e_update_phy_task(struct work_struct *work)
  **/
 static void e1000_update_phy_info(unsigned long data)
 {
-	struct e1000_adapter *adapter = (struct e1000_adapter *) data;
+	struct e1000_adapter *adapter = (struct e1000_adapter *)data;
 
 	if (test_bit(__E1000_DOWN, &adapter->state))
 		return;
@@ -4621,18 +4642,16 @@ static void e1000e_update_stats(struct e1000_adapter *adapter)
 	 * our own version based on RUC and ROC
 	 */
 	netdev->stats.rx_errors = adapter->stats.rxerrc +
-		adapter->stats.crcerrs + adapter->stats.algnerrc +
-		adapter->stats.ruc + adapter->stats.roc +
-		adapter->stats.cexterr;
+	    adapter->stats.crcerrs + adapter->stats.algnerrc +
+	    adapter->stats.ruc + adapter->stats.roc + adapter->stats.cexterr;
 	netdev->stats.rx_length_errors = adapter->stats.ruc +
-					      adapter->stats.roc;
+	    adapter->stats.roc;
 	netdev->stats.rx_crc_errors = adapter->stats.crcerrs;
 	netdev->stats.rx_frame_errors = adapter->stats.algnerrc;
 	netdev->stats.rx_missed_errors = adapter->stats.mpc;
 
 	/* Tx Errors */
-	netdev->stats.tx_errors = adapter->stats.ecol +
-				       adapter->stats.latecol;
+	netdev->stats.tx_errors = adapter->stats.ecol + adapter->stats.latecol;
 	netdev->stats.tx_aborted_errors = adapter->stats.ecol;
 	netdev->stats.tx_window_errors = adapter->stats.latecol;
 	netdev->stats.tx_carrier_errors = adapter->stats.tncrs;
@@ -4790,7 +4809,7 @@ static void e1000e_check_82574_phy_workaround(struct e1000_adapter *adapter)
  **/
 static void e1000_watchdog(unsigned long data)
 {
-	struct e1000_adapter *adapter = (struct e1000_adapter *) data;
+	struct e1000_adapter *adapter = (struct e1000_adapter *)data;
 
 	/* Do the rest outside of interrupt context */
 	schedule_work(&adapter->watchdog_task);
@@ -4801,7 +4820,8 @@ static void e1000_watchdog(unsigned long data)
 static void e1000_watchdog_task(struct work_struct *work)
 {
 	struct e1000_adapter *adapter = container_of(work,
-					struct e1000_adapter, watchdog_task);
+						     struct e1000_adapter,
+						     watchdog_task);
 	struct net_device *netdev = adapter->netdev;
 	struct e1000_mac_info *mac = &adapter->hw.mac;
 	struct e1000_phy_info *phy = &adapter->hw.phy;
@@ -4835,8 +4855,8 @@ static void e1000_watchdog_task(struct work_struct *work)
 			/* update snapshot of PHY registers on LSC */
 			e1000_phy_read_status(adapter);
 			mac->ops.get_link_up_info(&adapter->hw,
-						   &adapter->link_speed,
-						   &adapter->link_duplex);
+						  &adapter->link_speed,
+						  &adapter->link_duplex);
 			e1000_print_link_info(adapter);
 
 			/* check if SmartSpeed worked */
@@ -4949,7 +4969,7 @@ static void e1000_watchdog_task(struct work_struct *work)
 				adapter->flags |= FLAG_RESTART_NOW;
 			else
 				pm_schedule_suspend(netdev->dev.parent,
-							LINK_TIMEOUT);
+						    LINK_TIMEOUT);
 		}
 	}
 
@@ -4984,8 +5004,8 @@ link_up:
 		 */
 		u32 goc = (adapter->gotc + adapter->gorc) / 10000;
 		u32 dif = (adapter->gotc > adapter->gorc ?
-			    adapter->gotc - adapter->gorc :
-			    adapter->gorc - adapter->gotc) / 10000;
+			   adapter->gotc - adapter->gorc :
+			   adapter->gorc - adapter->gotc) / 10000;
 		u32 itr = goc > 0 ? (dif * 6000 / goc + 2000) : 8000;
 
 		e1000e_write_itr(adapter, itr);
@@ -5064,14 +5084,14 @@ static int e1000_tso(struct e1000_ring *tx_ring, struct sk_buff *skb)
 		iph->tot_len = 0;
 		iph->check = 0;
 		tcp_hdr(skb)->check = ~csum_tcpudp_magic(iph->saddr, iph->daddr,
-		                                         0, IPPROTO_TCP, 0);
+							 0, IPPROTO_TCP, 0);
 		cmd_length = E1000_TXD_CMD_IP;
 		ipcse = skb_transport_offset(skb) - 1;
 	} else if (skb_is_gso_v6(skb)) {
 		ipv6_hdr(skb)->payload_len = 0;
 		tcp_hdr(skb)->check = ~csum_ipv6_magic(&ipv6_hdr(skb)->saddr,
-		                                       &ipv6_hdr(skb)->daddr,
-		                                       0, IPPROTO_TCP, 0);
+						       &ipv6_hdr(skb)->daddr,
+						       0, IPPROTO_TCP, 0);
 		ipcse = 0;
 	}
 	ipcss = skb_network_offset(skb);
@@ -5080,7 +5100,7 @@ static int e1000_tso(struct e1000_ring *tx_ring, struct sk_buff *skb)
 	tucso = (void *)&(tcp_hdr(skb)->check) - (void *)skb->data;
 
 	cmd_length |= (E1000_TXD_CMD_DEXT | E1000_TXD_CMD_TSE |
-	               E1000_TXD_CMD_TCP | (skb->len - (hdr_len)));
+		       E1000_TXD_CMD_TCP | (skb->len - (hdr_len)));
 
 	i = tx_ring->next_to_use;
 	context_desc = E1000_CONTEXT_DESC(*tx_ring, i);
@@ -5150,8 +5170,7 @@ static bool e1000_tx_csum(struct e1000_ring *tx_ring, struct sk_buff *skb)
 
 	context_desc->lower_setup.ip_config = 0;
 	context_desc->upper_setup.tcp_fields.tucss = css;
-	context_desc->upper_setup.tcp_fields.tucso =
-				css + skb->csum_offset;
+	context_desc->upper_setup.tcp_fields.tucso = css + skb->csum_offset;
 	context_desc->upper_setup.tcp_fields.tucse = 0;
 	context_desc->tcp_seg_setup.data = 0;
 	context_desc->cmd_and_length = cpu_to_le32(cmd_len);
@@ -5224,7 +5243,8 @@ static int e1000_tx_map(struct e1000_ring *tx_ring, struct sk_buff *skb,
 			buffer_info->time_stamp = jiffies;
 			buffer_info->next_to_watch = i;
 			buffer_info->dma = skb_frag_dma_map(&pdev->dev, frag,
-						offset, size, DMA_TO_DEVICE);
+							    offset, size,
+							    DMA_TO_DEVICE);
 			buffer_info->mapped_as_page = true;
 			if (dma_mapping_error(&pdev->dev, buffer_info->dma))
 				goto dma_error;
@@ -5273,7 +5293,7 @@ static void e1000_tx_queue(struct e1000_ring *tx_ring, int tx_flags, int count)
 
 	if (tx_flags & E1000_TX_FLAGS_TSO) {
 		txd_lower |= E1000_TXD_CMD_DEXT | E1000_TXD_DTYP_D |
-			     E1000_TXD_CMD_TSE;
+		    E1000_TXD_CMD_TSE;
 		txd_upper |= E1000_TXD_POPTS_TXSM << 8;
 
 		if (tx_flags & E1000_TX_FLAGS_IPV4)
@@ -5304,8 +5324,8 @@ static void e1000_tx_queue(struct e1000_ring *tx_ring, int tx_flags, int count)
 		buffer_info = &tx_ring->buffer_info[i];
 		tx_desc = E1000_TX_DESC(*tx_ring, i);
 		tx_desc->buffer_addr = cpu_to_le64(buffer_info->dma);
-		tx_desc->lower.data =
-			cpu_to_le32(txd_lower | buffer_info->length);
+		tx_desc->lower.data = cpu_to_le32(txd_lower |
+						  buffer_info->length);
 		tx_desc->upper.data = cpu_to_le32(txd_upper);
 
 		i++;
@@ -5355,11 +5375,11 @@ static int e1000_transfer_dhcp_info(struct e1000_adapter *adapter,
 	if (skb->len <= MINIMUM_DHCP_PACKET_SIZE)
 		return 0;
 
-	if (((struct ethhdr *) skb->data)->h_proto != htons(ETH_P_IP))
+	if (((struct ethhdr *)skb->data)->h_proto != htons(ETH_P_IP))
 		return 0;
 
 	{
-		const struct iphdr *ip = (struct iphdr *)((u8 *)skb->data+14);
+		const struct iphdr *ip = (struct iphdr *)((u8 *)skb->data + 14);
 		struct udphdr *udp;
 
 		if (ip->protocol != IPPROTO_UDP)
@@ -5584,7 +5604,7 @@ static void e1000_reset_task(struct work_struct *work)
  * Returns the address of the device statistics structure.
  **/
 struct rtnl_link_stats64 *e1000e_get_stats64(struct net_device *netdev,
-                                             struct rtnl_link_stats64 *stats)
+					     struct rtnl_link_stats64 *stats)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
 
@@ -5605,18 +5625,15 @@ struct rtnl_link_stats64 *e1000e_get_stats64(struct net_device *netdev,
 	 * our own version based on RUC and ROC
 	 */
 	stats->rx_errors = adapter->stats.rxerrc +
-		adapter->stats.crcerrs + adapter->stats.algnerrc +
-		adapter->stats.ruc + adapter->stats.roc +
-		adapter->stats.cexterr;
-	stats->rx_length_errors = adapter->stats.ruc +
-					      adapter->stats.roc;
+	    adapter->stats.crcerrs + adapter->stats.algnerrc +
+	    adapter->stats.ruc + adapter->stats.roc + adapter->stats.cexterr;
+	stats->rx_length_errors = adapter->stats.ruc + adapter->stats.roc;
 	stats->rx_crc_errors = adapter->stats.crcerrs;
 	stats->rx_frame_errors = adapter->stats.algnerrc;
 	stats->rx_missed_errors = adapter->stats.mpc;
 
 	/* Tx Errors */
-	stats->tx_errors = adapter->stats.ecol +
-				       adapter->stats.latecol;
+	stats->tx_errors = adapter->stats.ecol + adapter->stats.latecol;
 	stats->tx_aborted_errors = adapter->stats.ecol;
 	stats->tx_window_errors = adapter->stats.latecol;
 	stats->tx_carrier_errors = adapter->stats.tncrs;
@@ -5685,9 +5702,9 @@ static int e1000_change_mtu(struct net_device *netdev, int new_mtu)
 
 	/* adjust allocation if LPE protects us, and we aren't using SBP */
 	if ((max_frame == ETH_FRAME_LEN + ETH_FCS_LEN) ||
-	     (max_frame == ETH_FRAME_LEN + VLAN_HLEN + ETH_FCS_LEN))
+	    (max_frame == ETH_FRAME_LEN + VLAN_HLEN + ETH_FCS_LEN))
 		adapter->rx_buffer_len = ETH_FRAME_LEN + VLAN_HLEN
-					 + ETH_FCS_LEN;
+		    + ETH_FCS_LEN;
 
 	if (netif_running(netdev))
 		e1000e_up(adapter);
@@ -5866,7 +5883,7 @@ static int e1000_init_phy_wakeup(struct e1000_adapter *adapter, u32 wufc)
 	phy_reg &= ~(BM_RCTL_MO_MASK);
 	if (mac_reg & E1000_RCTL_MO_3)
 		phy_reg |= (((mac_reg & E1000_RCTL_MO_3) >> E1000_RCTL_MO_SHIFT)
-				<< BM_RCTL_MO_SHIFT);
+			    << BM_RCTL_MO_SHIFT);
 	if (mac_reg & E1000_RCTL_BAM)
 		phy_reg |= BM_RCTL_BAM;
 	if (mac_reg & E1000_RCTL_PMCF)
@@ -5935,10 +5952,6 @@ static int __e1000_shutdown(struct pci_dev *pdev, bool runtime)
 		}
 
 		ctrl = er32(CTRL);
-		/* advertise wake from D3Cold */
-		#define E1000_CTRL_ADVD3WUC 0x00100000
-		/* phy power management enable */
-		#define E1000_CTRL_EN_PHY_PWR_MGMT 0x00200000
 		ctrl |= E1000_CTRL_ADVD3WUC;
 		if (!(adapter->flags2 & FLAG2_HAS_PHY_WAKEUP))
 			ctrl |= E1000_CTRL_EN_PHY_PWR_MGMT;
@@ -5982,8 +5995,6 @@ static int __e1000_shutdown(struct pci_dev *pdev, bool runtime)
 	 */
 	e1000e_release_hw_control(adapter);
 
-	pci_clear_master(pdev);
-
 	/* The pci-e switch on some quad port adapters will report a
 	 * correctable error when the MAC transitions from D0 to D3.  To
 	 * prevent this we need to mask off the correctable errors on the
@@ -6082,24 +6093,24 @@ static int __e1000_resume(struct pci_dev *pdev)
 		e1e_rphy(&adapter->hw, BM_WUS, &phy_data);
 		if (phy_data) {
 			e_info("PHY Wakeup cause - %s\n",
-				phy_data & E1000_WUS_EX ? "Unicast Packet" :
-				phy_data & E1000_WUS_MC ? "Multicast Packet" :
-				phy_data & E1000_WUS_BC ? "Broadcast Packet" :
-				phy_data & E1000_WUS_MAG ? "Magic Packet" :
-				phy_data & E1000_WUS_LNKC ?
-				"Link Status Change" : "other");
+			       phy_data & E1000_WUS_EX ? "Unicast Packet" :
+			       phy_data & E1000_WUS_MC ? "Multicast Packet" :
+			       phy_data & E1000_WUS_BC ? "Broadcast Packet" :
+			       phy_data & E1000_WUS_MAG ? "Magic Packet" :
+			       phy_data & E1000_WUS_LNKC ?
+			       "Link Status Change" : "other");
 		}
 		e1e_wphy(&adapter->hw, BM_WUS, ~0);
 	} else {
 		u32 wus = er32(WUS);
 		if (wus) {
 			e_info("MAC Wakeup cause - %s\n",
-				wus & E1000_WUS_EX ? "Unicast Packet" :
-				wus & E1000_WUS_MC ? "Multicast Packet" :
-				wus & E1000_WUS_BC ? "Broadcast Packet" :
-				wus & E1000_WUS_MAG ? "Magic Packet" :
-				wus & E1000_WUS_LNKC ? "Link Status Change" :
-				"other");
+			       wus & E1000_WUS_EX ? "Unicast Packet" :
+			       wus & E1000_WUS_MC ? "Multicast Packet" :
+			       wus & E1000_WUS_BC ? "Broadcast Packet" :
+			       wus & E1000_WUS_MAG ? "Magic Packet" :
+			       wus & E1000_WUS_LNKC ? "Link Status Change" :
+			       "other");
 		}
 		ew32(WUS, ~0);
 	}
@@ -6374,7 +6385,7 @@ static void e1000_print_device_info(struct e1000_adapter *adapter)
 	e_info("(PCI Express:2.5GT/s:%s) %pM\n",
 	       /* bus width */
 	       ((hw->bus.width == e1000_bus_width_pcie_x4) ? "Width x4" :
-	        "Width x1"),
+		"Width x1"),
 	       /* MAC address */
 	       netdev->dev_addr);
 	e_info("Intel(R) PRO/%s Network Connection\n",
@@ -6414,7 +6425,7 @@ static int e1000_set_features(struct net_device *netdev,
 	if (changed & (NETIF_F_TSO | NETIF_F_TSO6))
 		adapter->flags |= FLAG_TSO_FORCE;
 
-	if (!(changed & (NETIF_F_HW_VLAN_RX | NETIF_F_HW_VLAN_TX |
+	if (!(changed & (NETIF_F_HW_VLAN_CTAG_RX | NETIF_F_HW_VLAN_CTAG_TX |
 			 NETIF_F_RXCSUM | NETIF_F_RXHASH | NETIF_F_RXFCS |
 			 NETIF_F_RXALL)))
 		return 0;
@@ -6484,7 +6495,7 @@ static int e1000_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	resource_size_t flash_start, flash_len;
 	static int cards_found;
 	u16 aspm_disable_flag = 0;
-	int i, err, pci_using_dac;
+	int bars, i, err, pci_using_dac;
 	u16 eeprom_data = 0;
 	u16 eeprom_apme_mask = E1000_EEPROM_APME;
 
@@ -6511,15 +6522,16 @@ static int e1000_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 			err = dma_set_coherent_mask(&pdev->dev,
 						    DMA_BIT_MASK(32));
 			if (err) {
-				dev_err(&pdev->dev, "No usable DMA configuration, aborting\n");
+				dev_err(&pdev->dev,
+					"No usable DMA configuration, aborting\n");
 				goto err_dma;
 			}
 		}
 	}
 
-	err = pci_request_selected_regions_exclusive(pdev,
-					  pci_select_bars(pdev, IORESOURCE_MEM),
-					  e1000e_driver_name);
+	bars = pci_select_bars(pdev, IORESOURCE_MEM);
+	err = pci_request_selected_regions_exclusive(pdev, bars,
+						     e1000e_driver_name);
 	if (err)
 		goto err_pci_reg;
 
@@ -6572,6 +6584,10 @@ static int e1000_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 			goto err_flashmap;
 	}
 
+	/* Set default EEE advertisement */
+	if (adapter->flags2 & FLAG2_HAS_EEE)
+		adapter->eee_advert = MDIO_EEE_100TX | MDIO_EEE_1000T;
+
 	/* construct the net_device struct */
 	netdev->netdev_ops		= &e1000e_netdev_ops;
 	e1000e_set_ethtool_ops(netdev);
@@ -6620,8 +6636,8 @@ static int e1000_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	/* Set initial default active device features */
 	netdev->features = (NETIF_F_SG |
-			    NETIF_F_HW_VLAN_RX |
-			    NETIF_F_HW_VLAN_TX |
+			    NETIF_F_HW_VLAN_CTAG_RX |
+			    NETIF_F_HW_VLAN_CTAG_TX |
 			    NETIF_F_TSO |
 			    NETIF_F_TSO6 |
 			    NETIF_F_RXHASH |
@@ -6635,7 +6651,7 @@ static int e1000_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	netdev->hw_features |= NETIF_F_RXALL;
 
 	if (adapter->flags & FLAG_HAS_HW_VLAN_FILTER)
-		netdev->features |= NETIF_F_HW_VLAN_FILTER;
+		netdev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
 
 	netdev->vlan_features |= (NETIF_F_SG |
 				  NETIF_F_TSO |
@@ -6688,11 +6704,11 @@ static int e1000_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	init_timer(&adapter->watchdog_timer);
 	adapter->watchdog_timer.function = e1000_watchdog;
-	adapter->watchdog_timer.data = (unsigned long) adapter;
+	adapter->watchdog_timer.data = (unsigned long)adapter;
 
 	init_timer(&adapter->phy_info_timer);
 	adapter->phy_info_timer.function = e1000_update_phy_info;
-	adapter->phy_info_timer.data = (unsigned long) adapter;
+	adapter->phy_info_timer.data = (unsigned long)adapter;
 
 	INIT_WORK(&adapter->reset_task, e1000_reset_task);
 	INIT_WORK(&adapter->watchdog_task, e1000_watchdog_task);
@@ -6800,7 +6816,7 @@ err_ioremap:
 	free_netdev(netdev);
 err_alloc_etherdev:
 	pci_release_selected_regions(pdev,
-	                             pci_select_bars(pdev, IORESOURCE_MEM));
+				     pci_select_bars(pdev, IORESOURCE_MEM));
 err_pci_reg:
 err_dma:
 	pci_disable_device(pdev);
@@ -6870,7 +6886,7 @@ static void e1000_remove(struct pci_dev *pdev)
 	if (adapter->hw.flash_address)
 		iounmap(adapter->hw.flash_address);
 	pci_release_selected_regions(pdev,
-	                             pci_select_bars(pdev, IORESOURCE_MEM));
+				     pci_select_bars(pdev, IORESOURCE_MEM));
 
 	free_netdev(netdev);
 
@@ -6891,7 +6907,8 @@ static DEFINE_PCI_DEVICE_TABLE(e1000_pci_tbl) = {
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82571EB_COPPER), board_82571 },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82571EB_FIBER), board_82571 },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82571EB_QUAD_COPPER), board_82571 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82571EB_QUAD_COPPER_LP), board_82571 },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82571EB_QUAD_COPPER_LP),
+	  board_82571 },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82571EB_QUAD_FIBER), board_82571 },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82571EB_SERDES), board_82571 },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82571EB_SERDES_DUAL), board_82571 },
@@ -6967,8 +6984,8 @@ MODULE_DEVICE_TABLE(pci, e1000_pci_tbl);
 #ifdef CONFIG_PM
 static const struct dev_pm_ops e1000_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(e1000_suspend, e1000_resume)
-	SET_RUNTIME_PM_OPS(e1000_runtime_suspend,
-				e1000_runtime_resume, e1000_idle)
+	SET_RUNTIME_PM_OPS(e1000_runtime_suspend, e1000_runtime_resume,
+			   e1000_idle)
 };
 #endif
 
diff --git a/drivers/net/ethernet/intel/e1000e/nvm.c b/drivers/net/ethernet/intel/e1000e/nvm.c
index 84fecc268162..44ddc0a0ee0e 100644
--- a/drivers/net/ethernet/intel/e1000e/nvm.c
+++ b/drivers/net/ethernet/intel/e1000e/nvm.c
@@ -630,7 +630,7 @@ void e1000e_reload_nvm_generic(struct e1000_hw *hw)
 {
 	u32 ctrl_ext;
 
-	udelay(10);
+	usleep_range(10, 20);
 	ctrl_ext = er32(CTRL_EXT);
 	ctrl_ext |= E1000_CTRL_EXT_EE_RST;
 	ew32(CTRL_EXT, ctrl_ext);
diff --git a/drivers/net/ethernet/intel/e1000e/param.c b/drivers/net/ethernet/intel/e1000e/param.c
index 98da75dff936..c16bd75b6caa 100644
--- a/drivers/net/ethernet/intel/e1000e/param.c
+++ b/drivers/net/ethernet/intel/e1000e/param.c
@@ -45,7 +45,7 @@
 unsigned int copybreak = COPYBREAK_DEFAULT;
 module_param(copybreak, uint, 0644);
 MODULE_PARM_DESC(copybreak,
-	"Maximum size of packet that is copied to a new buffer on receive");
+		 "Maximum size of packet that is copied to a new buffer on receive");
 
 /* All parameters are treated the same, as an integer array of values.
  * This macro just reduces the need to repeat the same declaration code
@@ -143,7 +143,8 @@ E1000_PARAM(KumeranLockLoss, "Enable Kumeran lock loss workaround");
  *
  * Default Value: 1 (enabled)
  */
-E1000_PARAM(WriteProtectNVM, "Write-protect NVM [WARNING: disabling this can lead to corrupted NVM]");
+E1000_PARAM(WriteProtectNVM,
+	    "Write-protect NVM [WARNING: disabling this can lead to corrupted NVM]");
 
 /* Enable CRC Stripping
  *
@@ -160,13 +161,18 @@ struct e1000_option {
 	const char *err;
 	int def;
 	union {
-		struct { /* range_option info */
+		/* range_option info */
+		struct {
 			int min;
 			int max;
 		} r;
-		struct { /* list_option info */
+		/* list_option info */
+		struct {
 			int nr;
-			struct e1000_opt_list { int i; char *str; } *p;
+			struct e1000_opt_list {
+				int i;
+				char *str;
+			} *p;
 		} l;
 	} arg;
 };
@@ -246,7 +252,8 @@ void e1000e_check_options(struct e1000_adapter *adapter)
 			   "Using defaults for all values\n");
 	}
 
-	{ /* Transmit Interrupt Delay */
+	/* Transmit Interrupt Delay */
+	{
 		static const struct e1000_option opt = {
 			.type = range_option,
 			.name = "Transmit Interrupt Delay",
@@ -265,7 +272,8 @@ void e1000e_check_options(struct e1000_adapter *adapter)
 			adapter->tx_int_delay = opt.def;
 		}
 	}
-	{ /* Transmit Absolute Interrupt Delay */
+	/* Transmit Absolute Interrupt Delay */
+	{
 		static const struct e1000_option opt = {
 			.type = range_option,
 			.name = "Transmit Absolute Interrupt Delay",
@@ -284,7 +292,8 @@ void e1000e_check_options(struct e1000_adapter *adapter)
 			adapter->tx_abs_int_delay = opt.def;
 		}
 	}
-	{ /* Receive Interrupt Delay */
+	/* Receive Interrupt Delay */
+	{
 		static struct e1000_option opt = {
 			.type = range_option,
 			.name = "Receive Interrupt Delay",
@@ -303,7 +312,8 @@ void e1000e_check_options(struct e1000_adapter *adapter)
 			adapter->rx_int_delay = opt.def;
 		}
 	}
-	{ /* Receive Absolute Interrupt Delay */
+	/* Receive Absolute Interrupt Delay */
+	{
 		static const struct e1000_option opt = {
 			.type = range_option,
 			.name = "Receive Absolute Interrupt Delay",
@@ -322,7 +332,8 @@ void e1000e_check_options(struct e1000_adapter *adapter)
 			adapter->rx_abs_int_delay = opt.def;
 		}
 	}
-	{ /* Interrupt Throttling Rate */
+	/* Interrupt Throttling Rate */
+	{
 		static const struct e1000_option opt = {
 			.type = range_option,
 			.name = "Interrupt Throttling Rate (ints/sec)",
@@ -392,7 +403,8 @@ void e1000e_check_options(struct e1000_adapter *adapter)
 			break;
 		}
 	}
-	{ /* Interrupt Mode */
+	/* Interrupt Mode */
+	{
 		static struct e1000_option opt = {
 			.type = range_option,
 			.name = "Interrupt Mode",
@@ -435,7 +447,8 @@ void e1000e_check_options(struct e1000_adapter *adapter)
 		kfree(opt.err);
 #endif
 	}
-	{ /* Smart Power Down */
+	/* Smart Power Down */
+	{
 		static const struct e1000_option opt = {
 			.type = enable_option,
 			.name = "PHY Smart Power Down",
@@ -450,7 +463,8 @@ void e1000e_check_options(struct e1000_adapter *adapter)
 				adapter->flags |= FLAG_SMART_POWER_DOWN;
 		}
 	}
-	{ /* CRC Stripping */
+	/* CRC Stripping */
+	{
 		static const struct e1000_option opt = {
 			.type = enable_option,
 			.name = "CRC Stripping",
@@ -470,27 +484,28 @@ void e1000e_check_options(struct e1000_adapter *adapter)
 			adapter->flags2 |= FLAG2_DFLT_CRC_STRIPPING;
 		}
 	}
-	{ /* Kumeran Lock Loss Workaround */
+	/* Kumeran Lock Loss Workaround */
+	{
 		static const struct e1000_option opt = {
 			.type = enable_option,
 			.name = "Kumeran Lock Loss Workaround",
 			.err  = "defaulting to Enabled",
 			.def  = OPTION_ENABLED
 		};
+		bool enabled = opt.def;
 
 		if (num_KumeranLockLoss > bd) {
 			unsigned int kmrn_lock_loss = KumeranLockLoss[bd];
 			e1000_validate_option(&kmrn_lock_loss, &opt, adapter);
-			if (hw->mac.type == e1000_ich8lan)
-				e1000e_set_kmrn_lock_loss_workaround_ich8lan(hw,
-								kmrn_lock_loss);
-		} else {
-			if (hw->mac.type == e1000_ich8lan)
-				e1000e_set_kmrn_lock_loss_workaround_ich8lan(hw,
-								       opt.def);
+			enabled = kmrn_lock_loss;
 		}
+
+		if (hw->mac.type == e1000_ich8lan)
+			e1000e_set_kmrn_lock_loss_workaround_ich8lan(hw,
+								     enabled);
 	}
-	{ /* Write-protect NVM */
+	/* Write-protect NVM */
+	{
 		static const struct e1000_option opt = {
 			.type = enable_option,
 			.name = "Write-protect NVM",
@@ -500,7 +515,8 @@ void e1000e_check_options(struct e1000_adapter *adapter)
 
 		if (adapter->flags & FLAG_IS_ICH) {
 			if (num_WriteProtectNVM > bd) {
-				unsigned int write_protect_nvm = WriteProtectNVM[bd];
+				unsigned int write_protect_nvm =
+				    WriteProtectNVM[bd];
 				e1000_validate_option(&write_protect_nvm, &opt,
 						      adapter);
 				if (write_protect_nvm)
diff --git a/drivers/net/ethernet/intel/e1000e/phy.c b/drivers/net/ethernet/intel/e1000e/phy.c
index 0930c136aa31..59c76a6815a0 100644
--- a/drivers/net/ethernet/intel/e1000e/phy.c
+++ b/drivers/net/ethernet/intel/e1000e/phy.c
@@ -37,7 +37,9 @@ static s32 e1000_access_phy_debug_regs_hv(struct e1000_hw *hw, u32 offset,
 
 /* Cable length tables */
 static const u16 e1000_m88_cable_length_table[] = {
-	0, 50, 80, 110, 140, 140, E1000_CABLE_LENGTH_UNDEFINED };
+	0, 50, 80, 110, 140, 140, E1000_CABLE_LENGTH_UNDEFINED
+};
+
 #define M88E1000_CABLE_LENGTH_TABLE_SIZE \
 		ARRAY_SIZE(e1000_m88_cable_length_table)
 
@@ -49,7 +51,9 @@ static const u16 e1000_igp_2_cable_length_table[] = {
 	66, 70, 75, 79, 83, 87, 91, 94, 98, 101, 104, 60, 66, 72, 77, 82,
 	87, 92, 96, 100, 104, 108, 111, 114, 117, 119, 121, 83, 89, 95,
 	100, 105, 109, 113, 116, 119, 122, 124, 104, 109, 114, 118, 121,
-	124};
+	124
+};
+
 #define IGP02E1000_CABLE_LENGTH_TABLE_SIZE \
 		ARRAY_SIZE(e1000_igp_2_cable_length_table)
 
@@ -67,8 +71,7 @@ s32 e1000e_check_reset_block_generic(struct e1000_hw *hw)
 
 	manc = er32(MANC);
 
-	return (manc & E1000_MANC_BLK_PHY_RST_ON_IDE) ?
-	       E1000_BLK_PHY_RESET : 0;
+	return (manc & E1000_MANC_BLK_PHY_RST_ON_IDE) ? E1000_BLK_PHY_RESET : 0;
 }
 
 /**
@@ -94,7 +97,7 @@ s32 e1000e_get_phy_id(struct e1000_hw *hw)
 			return ret_val;
 
 		phy->id = (u32)(phy_id << 16);
-		udelay(20);
+		usleep_range(20, 40);
 		ret_val = e1e_rphy(hw, MII_PHYSID2, &phy_id);
 		if (ret_val)
 			return ret_val;
@@ -175,7 +178,13 @@ s32 e1000e_read_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 *data)
 		e_dbg("MDI Error\n");
 		return -E1000_ERR_PHY;
 	}
-	*data = (u16) mdic;
+	if (((mdic & E1000_MDIC_REG_MASK) >> E1000_MDIC_REG_SHIFT) != offset) {
+		e_dbg("MDI Read offset error - requested %d, returned %d\n",
+		      offset,
+		      (mdic & E1000_MDIC_REG_MASK) >> E1000_MDIC_REG_SHIFT);
+		return -E1000_ERR_PHY;
+	}
+	*data = (u16)mdic;
 
 	/* Allow some time after each MDIC transaction to avoid
 	 * reading duplicate data in the next MDIC transaction.
@@ -233,6 +242,12 @@ s32 e1000e_write_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 data)
 		e_dbg("MDI Error\n");
 		return -E1000_ERR_PHY;
 	}
+	if (((mdic & E1000_MDIC_REG_MASK) >> E1000_MDIC_REG_SHIFT) != offset) {
+		e_dbg("MDI Write offset error - requested %d, returned %d\n",
+		      offset,
+		      (mdic & E1000_MDIC_REG_MASK) >> E1000_MDIC_REG_SHIFT);
+		return -E1000_ERR_PHY;
+	}
 
 	/* Allow some time after each MDIC transaction to avoid
 	 * reading duplicate data in the next MDIC transaction.
@@ -324,7 +339,7 @@ s32 e1000_set_page_igp(struct e1000_hw *hw, u16 page)
  *  semaphores before exiting.
  **/
 static s32 __e1000e_read_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 *data,
-                                    bool locked)
+				     bool locked)
 {
 	s32 ret_val = 0;
 
@@ -391,7 +406,7 @@ s32 e1000e_read_phy_reg_igp_locked(struct e1000_hw *hw, u32 offset, u16 *data)
  *  at the offset.  Release any acquired semaphores before exiting.
  **/
 static s32 __e1000e_write_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 data,
-                                     bool locked)
+				      bool locked)
 {
 	s32 ret_val = 0;
 
@@ -410,8 +425,7 @@ static s32 __e1000e_write_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 data,
 						    (u16)offset);
 	if (!ret_val)
 		ret_val = e1000e_write_phy_reg_mdic(hw, MAX_PHY_REG_ADDRESS &
-							offset,
-						    data);
+						    offset, data);
 	if (!locked)
 		hw->phy.ops.release(hw);
 
@@ -458,7 +472,7 @@ s32 e1000e_write_phy_reg_igp_locked(struct e1000_hw *hw, u32 offset, u16 data)
  *  Release any acquired semaphores before exiting.
  **/
 static s32 __e1000_read_kmrn_reg(struct e1000_hw *hw, u32 offset, u16 *data,
-                                 bool locked)
+				 bool locked)
 {
 	u32 kmrnctrlsta;
 
@@ -531,7 +545,7 @@ s32 e1000e_read_kmrn_reg_locked(struct e1000_hw *hw, u32 offset, u16 *data)
  *  before exiting.
  **/
 static s32 __e1000_write_kmrn_reg(struct e1000_hw *hw, u32 offset, u16 data,
-                                  bool locked)
+				  bool locked)
 {
 	u32 kmrnctrlsta;
 
@@ -772,8 +786,7 @@ s32 e1000e_copper_link_setup_m88(struct e1000_hw *hw)
 
 		phy_data |= M88E1000_EPSCR_TX_CLK_25;
 
-		if ((phy->revision == 2) &&
-		    (phy->id == M88E1111_I_PHY_ID)) {
+		if ((phy->revision == 2) && (phy->id == M88E1111_I_PHY_ID)) {
 			/* 82573L PHY - set the downshift counter to 5x. */
 			phy_data &= ~M88EC018_EPSCR_DOWNSHIFT_COUNTER_MASK;
 			phy_data |= M88EC018_EPSCR_DOWNSHIFT_COUNTER_5X;
@@ -1296,7 +1309,7 @@ s32 e1000e_phy_force_speed_duplex_m88(struct e1000_hw *hw)
 		e_dbg("Waiting for forced speed/duplex link on M88 phy.\n");
 
 		ret_val = e1000e_phy_has_link_generic(hw, PHY_FORCE_LIMIT,
-						     100000, &link);
+						      100000, &link);
 		if (ret_val)
 			return ret_val;
 
@@ -1319,7 +1332,7 @@ s32 e1000e_phy_force_speed_duplex_m88(struct e1000_hw *hw)
 
 		/* Try once more */
 		ret_val = e1000e_phy_has_link_generic(hw, PHY_FORCE_LIMIT,
-						     100000, &link);
+						      100000, &link);
 		if (ret_val)
 			return ret_val;
 	}
@@ -1609,9 +1622,9 @@ s32 e1000_check_polarity_m88(struct e1000_hw *hw)
 	ret_val = e1e_rphy(hw, M88E1000_PHY_SPEC_STATUS, &data);
 
 	if (!ret_val)
-		phy->cable_polarity = (data & M88E1000_PSSR_REV_POLARITY)
-				      ? e1000_rev_polarity_reversed
-				      : e1000_rev_polarity_normal;
+		phy->cable_polarity = ((data & M88E1000_PSSR_REV_POLARITY)
+				       ? e1000_rev_polarity_reversed
+				       : e1000_rev_polarity_normal);
 
 	return ret_val;
 }
@@ -1653,9 +1666,9 @@ s32 e1000_check_polarity_igp(struct e1000_hw *hw)
 	ret_val = e1e_rphy(hw, offset, &data);
 
 	if (!ret_val)
-		phy->cable_polarity = (data & mask)
-				      ? e1000_rev_polarity_reversed
-				      : e1000_rev_polarity_normal;
+		phy->cable_polarity = ((data & mask)
+				       ? e1000_rev_polarity_reversed
+				       : e1000_rev_polarity_normal);
 
 	return ret_val;
 }
@@ -1685,9 +1698,9 @@ s32 e1000_check_polarity_ife(struct e1000_hw *hw)
 	ret_val = e1e_rphy(hw, offset, &phy_data);
 
 	if (!ret_val)
-		phy->cable_polarity = (phy_data & mask)
-		                       ? e1000_rev_polarity_reversed
-		                       : e1000_rev_polarity_normal;
+		phy->cable_polarity = ((phy_data & mask)
+				       ? e1000_rev_polarity_reversed
+				       : e1000_rev_polarity_normal);
 
 	return ret_val;
 }
@@ -1733,7 +1746,7 @@ static s32 e1000_wait_autoneg(struct e1000_hw *hw)
  *  Polls the PHY status register for link, 'iterations' number of times.
  **/
 s32 e1000e_phy_has_link_generic(struct e1000_hw *hw, u32 iterations,
-			       u32 usec_interval, bool *success)
+				u32 usec_interval, bool *success)
 {
 	s32 ret_val = 0;
 	u16 i, phy_status;
@@ -1756,7 +1769,7 @@ s32 e1000e_phy_has_link_generic(struct e1000_hw *hw, u32 iterations,
 		if (phy_status & BMSR_LSTATUS)
 			break;
 		if (usec_interval >= 1000)
-			mdelay(usec_interval/1000);
+			mdelay(usec_interval / 1000);
 		else
 			udelay(usec_interval);
 	}
@@ -1791,8 +1804,8 @@ s32 e1000e_get_cable_length_m88(struct e1000_hw *hw)
 	if (ret_val)
 		return ret_val;
 
-	index = (phy_data & M88E1000_PSSR_CABLE_LENGTH) >>
-	        M88E1000_PSSR_CABLE_LENGTH_SHIFT;
+	index = ((phy_data & M88E1000_PSSR_CABLE_LENGTH) >>
+		 M88E1000_PSSR_CABLE_LENGTH_SHIFT);
 
 	if (index >= M88E1000_CABLE_LENGTH_TABLE_SIZE - 1)
 		return -E1000_ERR_PHY;
@@ -1824,10 +1837,10 @@ s32 e1000e_get_cable_length_igp_2(struct e1000_hw *hw)
 	u16 cur_agc_index, max_agc_index = 0;
 	u16 min_agc_index = IGP02E1000_CABLE_LENGTH_TABLE_SIZE - 1;
 	static const u16 agc_reg_array[IGP02E1000_PHY_CHANNEL_NUM] = {
-	       IGP02E1000_PHY_AGC_A,
-	       IGP02E1000_PHY_AGC_B,
-	       IGP02E1000_PHY_AGC_C,
-	       IGP02E1000_PHY_AGC_D
+		IGP02E1000_PHY_AGC_A,
+		IGP02E1000_PHY_AGC_B,
+		IGP02E1000_PHY_AGC_C,
+		IGP02E1000_PHY_AGC_D
 	};
 
 	/* Read the AGC registers for all channels */
@@ -1841,8 +1854,8 @@ s32 e1000e_get_cable_length_igp_2(struct e1000_hw *hw)
 		 * that can be put into the lookup table to obtain the
 		 * approximate cable length.
 		 */
-		cur_agc_index = (phy_data >> IGP02E1000_AGC_LENGTH_SHIFT) &
-				IGP02E1000_AGC_LENGTH_MASK;
+		cur_agc_index = ((phy_data >> IGP02E1000_AGC_LENGTH_SHIFT) &
+				 IGP02E1000_AGC_LENGTH_MASK);
 
 		/* Array index bound check. */
 		if ((cur_agc_index >= IGP02E1000_CABLE_LENGTH_TABLE_SIZE) ||
@@ -1865,8 +1878,8 @@ s32 e1000e_get_cable_length_igp_2(struct e1000_hw *hw)
 	agc_value /= (IGP02E1000_PHY_CHANNEL_NUM - 2);
 
 	/* Calculate cable length with the error range of +/- 10 meters. */
-	phy->min_cable_length = ((agc_value - IGP02E1000_AGC_RANGE) > 0) ?
-				 (agc_value - IGP02E1000_AGC_RANGE) : 0;
+	phy->min_cable_length = (((agc_value - IGP02E1000_AGC_RANGE) > 0) ?
+				 (agc_value - IGP02E1000_AGC_RANGE) : 0);
 	phy->max_cable_length = agc_value + IGP02E1000_AGC_RANGE;
 
 	phy->cable_length = (phy->min_cable_length + phy->max_cable_length) / 2;
@@ -2040,9 +2053,9 @@ s32 e1000_get_phy_info_ife(struct e1000_hw *hw)
 			return ret_val;
 	} else {
 		/* Polarity is forced */
-		phy->cable_polarity = (data & IFE_PSC_FORCE_POLARITY)
-		                      ? e1000_rev_polarity_reversed
-		                      : e1000_rev_polarity_normal;
+		phy->cable_polarity = ((data & IFE_PSC_FORCE_POLARITY)
+				       ? e1000_rev_polarity_reversed
+				       : e1000_rev_polarity_normal);
 	}
 
 	ret_val = e1e_rphy(hw, IFE_PHY_MDIX_CONTROL, &data);
@@ -2119,7 +2132,7 @@ s32 e1000e_phy_hw_reset_generic(struct e1000_hw *hw)
 	ew32(CTRL, ctrl);
 	e1e_flush();
 
-	udelay(150);
+	usleep_range(150, 300);
 
 	phy->ops.release(hw);
 
@@ -2375,13 +2388,13 @@ s32 e1000e_write_phy_reg_bm(struct e1000_hw *hw, u32 offset, u16 data)
 
 		/* Page is shifted left, PHY expects (page x 32) */
 		ret_val = e1000e_write_phy_reg_mdic(hw, page_select,
-		                                    (page << page_shift));
+						    (page << page_shift));
 		if (ret_val)
 			goto release;
 	}
 
 	ret_val = e1000e_write_phy_reg_mdic(hw, MAX_PHY_REG_ADDRESS & offset,
-	                                    data);
+					    data);
 
 release:
 	hw->phy.ops.release(hw);
@@ -2433,13 +2446,13 @@ s32 e1000e_read_phy_reg_bm(struct e1000_hw *hw, u32 offset, u16 *data)
 
 		/* Page is shifted left, PHY expects (page x 32) */
 		ret_val = e1000e_write_phy_reg_mdic(hw, page_select,
-		                                    (page << page_shift));
+						    (page << page_shift));
 		if (ret_val)
 			goto release;
 	}
 
 	ret_val = e1000e_read_phy_reg_mdic(hw, MAX_PHY_REG_ADDRESS & offset,
-	                                   data);
+					   data);
 release:
 	hw->phy.ops.release(hw);
 	return ret_val;
@@ -2674,7 +2687,7 @@ static s32 e1000_access_phy_wakeup_reg_bm(struct e1000_hw *hw, u32 offset,
 	if (read) {
 		/* Read the Wakeup register page value using opcode 0x12 */
 		ret_val = e1000e_read_phy_reg_mdic(hw, BM_WUC_DATA_OPCODE,
-		                                   data);
+						   data);
 	} else {
 		/* Write the Wakeup register page value using opcode 0x12 */
 		ret_val = e1000e_write_phy_reg_mdic(hw, BM_WUC_DATA_OPCODE,
@@ -2763,7 +2776,7 @@ static s32 __e1000_read_phy_reg_hv(struct e1000_hw *hw, u32 offset, u16 *data,
 
 	if (page > 0 && page < HV_INTC_FC_PAGE_START) {
 		ret_val = e1000_access_phy_debug_regs_hv(hw, offset,
-		                                         data, true);
+							 data, true);
 		goto out;
 	}
 
@@ -2786,8 +2799,7 @@ static s32 __e1000_read_phy_reg_hv(struct e1000_hw *hw, u32 offset, u16 *data,
 	e_dbg("reading PHY page %d (or 0x%x shifted) reg 0x%x\n", page,
 	      page << IGP_PAGE_SHIFT, reg);
 
-	ret_val = e1000e_read_phy_reg_mdic(hw, MAX_PHY_REG_ADDRESS & reg,
-	                                  data);
+	ret_val = e1000e_read_phy_reg_mdic(hw, MAX_PHY_REG_ADDRESS & reg, data);
 out:
 	if (!locked)
 		hw->phy.ops.release(hw);
@@ -2871,7 +2883,7 @@ static s32 __e1000_write_phy_reg_hv(struct e1000_hw *hw, u32 offset, u16 data,
 
 	if (page > 0 && page < HV_INTC_FC_PAGE_START) {
 		ret_val = e1000_access_phy_debug_regs_hv(hw, offset,
-		                                         &data, false);
+							 &data, false);
 		goto out;
 	}
 
@@ -2910,7 +2922,7 @@ static s32 __e1000_write_phy_reg_hv(struct e1000_hw *hw, u32 offset, u16 data,
 	      page << IGP_PAGE_SHIFT, reg);
 
 	ret_val = e1000e_write_phy_reg_mdic(hw, MAX_PHY_REG_ADDRESS & reg,
-	                                  data);
+					    data);
 
 out:
 	if (!locked)
@@ -2988,15 +3000,15 @@ static u32 e1000_get_phy_addr_for_hv_page(u32 page)
  *  These accesses done with PHY address 2 and without using pages.
  **/
 static s32 e1000_access_phy_debug_regs_hv(struct e1000_hw *hw, u32 offset,
-                                          u16 *data, bool read)
+					  u16 *data, bool read)
 {
 	s32 ret_val;
 	u32 addr_reg;
 	u32 data_reg;
 
 	/* This takes care of the difference with desktop vs mobile phy */
-	addr_reg = (hw->phy.type == e1000_phy_82578) ?
-	           I82578_ADDR_REG : I82577_ADDR_REG;
+	addr_reg = ((hw->phy.type == e1000_phy_82578) ?
+		    I82578_ADDR_REG : I82577_ADDR_REG);
 	data_reg = addr_reg + 1;
 
 	/* All operations in this function are phy address 2 */
@@ -3050,8 +3062,8 @@ s32 e1000_link_stall_workaround_hv(struct e1000_hw *hw)
 	if (ret_val)
 		return ret_val;
 
-	data &= BM_CS_STATUS_LINK_UP | BM_CS_STATUS_RESOLVED |
-		BM_CS_STATUS_SPEED_MASK;
+	data &= (BM_CS_STATUS_LINK_UP | BM_CS_STATUS_RESOLVED |
+		 BM_CS_STATUS_SPEED_MASK);
 
 	if (data != (BM_CS_STATUS_LINK_UP | BM_CS_STATUS_RESOLVED |
 		     BM_CS_STATUS_SPEED_1000))
@@ -3086,9 +3098,9 @@ s32 e1000_check_polarity_82577(struct e1000_hw *hw)
 	ret_val = e1e_rphy(hw, I82577_PHY_STATUS_2, &data);
 
 	if (!ret_val)
-		phy->cable_polarity = (data & I82577_PHY_STATUS2_REV_POLARITY)
-		                      ? e1000_rev_polarity_reversed
-		                      : e1000_rev_polarity_normal;
+		phy->cable_polarity = ((data & I82577_PHY_STATUS2_REV_POLARITY)
+				       ? e1000_rev_polarity_reversed
+				       : e1000_rev_polarity_normal);
 
 	return ret_val;
 }
@@ -3215,8 +3227,8 @@ s32 e1000_get_cable_length_82577(struct e1000_hw *hw)
 	if (ret_val)
 		return ret_val;
 
-	length = (phy_data & I82577_DSTATUS_CABLE_LENGTH) >>
-	         I82577_DSTATUS_CABLE_LENGTH_SHIFT;
+	length = ((phy_data & I82577_DSTATUS_CABLE_LENGTH) >>
+		  I82577_DSTATUS_CABLE_LENGTH_SHIFT);
 
 	if (length == E1000_CABLE_LENGTH_UNDEFINED)
 		return -E1000_ERR_PHY;
diff --git a/drivers/net/ethernet/intel/e1000e/ptp.c b/drivers/net/ethernet/intel/e1000e/ptp.c
index b477fa53ec94..065f8c80d4f2 100644
--- a/drivers/net/ethernet/intel/e1000e/ptp.c
+++ b/drivers/net/ethernet/intel/e1000e/ptp.c
@@ -145,8 +145,7 @@ static int e1000e_phc_settime(struct ptp_clock_info *ptp,
 	unsigned long flags;
 	u64 ns;
 
-	ns = ts->tv_sec * NSEC_PER_SEC;
-	ns += ts->tv_nsec;
+	ns = timespec_to_ns(ts);
 
 	/* reset the timecounter */
 	spin_lock_irqsave(&adapter->systim_lock, flags);
diff --git a/drivers/net/ethernet/intel/igb/e1000_82575.c b/drivers/net/ethernet/intel/igb/e1000_82575.c
index 12b1d8480808..ff6a17cb1362 100644
--- a/drivers/net/ethernet/intel/igb/e1000_82575.c
+++ b/drivers/net/ethernet/intel/igb/e1000_82575.c
@@ -100,6 +100,7 @@ static bool igb_sgmii_uses_mdio_82575(struct e1000_hw *hw)
 		break;
 	case e1000_82580:
 	case e1000_i350:
+	case e1000_i354:
 	case e1000_i210:
 	case e1000_i211:
 		reg = rd32(E1000_MDICNFG);
@@ -149,6 +150,7 @@ static s32 igb_init_phy_params_82575(struct e1000_hw *hw)
 		switch (hw->mac.type) {
 		case e1000_82580:
 		case e1000_i350:
+		case e1000_i354:
 			phy->ops.read_reg = igb_read_phy_reg_82580;
 			phy->ops.write_reg = igb_write_phy_reg_82580;
 			break;
@@ -174,13 +176,14 @@ static s32 igb_init_phy_params_82575(struct e1000_hw *hw)
 
 	/* Verify phy id and set remaining function pointers */
 	switch (phy->id) {
+	case M88E1545_E_PHY_ID:
 	case I347AT4_E_PHY_ID:
 	case M88E1112_E_PHY_ID:
 	case M88E1111_I_PHY_ID:
 		phy->type		= e1000_phy_m88;
+		phy->ops.check_polarity	= igb_check_polarity_m88;
 		phy->ops.get_phy_info	= igb_get_phy_info_m88;
-		if (phy->id == I347AT4_E_PHY_ID ||
-		    phy->id == M88E1112_E_PHY_ID)
+		if (phy->id != M88E1111_I_PHY_ID)
 			phy->ops.get_cable_length =
 					 igb_get_cable_length_m88_gen2;
 		else
@@ -227,7 +230,7 @@ out:
  *  igb_init_nvm_params_82575 - Init NVM func ptrs.
  *  @hw: pointer to the HW structure
  **/
-s32 igb_init_nvm_params_82575(struct e1000_hw *hw)
+static s32 igb_init_nvm_params_82575(struct e1000_hw *hw)
 {
 	struct e1000_nvm_info *nvm = &hw->nvm;
 	u32 eecd = rd32(E1000_EECD);
@@ -287,6 +290,7 @@ s32 igb_init_nvm_params_82575(struct e1000_hw *hw)
 			nvm->ops.read = igb_read_nvm_spi;
 		nvm->ops.write = igb_write_nvm_spi;
 		break;
+	case e1000_i354:
 	case e1000_i350:
 		nvm->ops.validate = igb_validate_nvm_checksum_i350;
 		nvm->ops.update = igb_update_nvm_checksum_i350;
@@ -352,6 +356,7 @@ static s32 igb_init_mac_params_82575(struct e1000_hw *hw)
 		mac->rar_entry_count = E1000_RAR_ENTRIES_82580;
 		break;
 	case e1000_i350:
+	case e1000_i354:
 		mac->rar_entry_count = E1000_RAR_ENTRIES_I350;
 		break;
 	default:
@@ -384,6 +389,9 @@ static s32 igb_init_mac_params_82575(struct e1000_hw *hw)
 		dev_spec->eee_disable = false;
 	else
 		dev_spec->eee_disable = true;
+	/* Allow a single clear of the SW semaphore on I210 and newer */
+	if (mac->type >= e1000_i210)
+		dev_spec->clear_semaphore_once = true;
 	/* physical interface link setup */
 	mac->ops.setup_physical_interface =
 		(hw->phy.media_type == e1000_media_type_copper)
@@ -435,8 +443,6 @@ static s32 igb_get_invariants_82575(struct e1000_hw *hw)
 		mac->type = e1000_i350;
 		break;
 	case E1000_DEV_ID_I210_COPPER:
-	case E1000_DEV_ID_I210_COPPER_OEM1:
-	case E1000_DEV_ID_I210_COPPER_IT:
 	case E1000_DEV_ID_I210_FIBER:
 	case E1000_DEV_ID_I210_SERDES:
 	case E1000_DEV_ID_I210_SGMII:
@@ -445,14 +451,18 @@ static s32 igb_get_invariants_82575(struct e1000_hw *hw)
 	case E1000_DEV_ID_I211_COPPER:
 		mac->type = e1000_i211;
 		break;
+	case E1000_DEV_ID_I354_BACKPLANE_1GBPS:
+	case E1000_DEV_ID_I354_SGMII:
+	case E1000_DEV_ID_I354_BACKPLANE_2_5GBPS:
+		mac->type = e1000_i354;
+		break;
 	default:
 		return -E1000_ERR_MAC_INIT;
 		break;
 	}
 
 	/* Set media type */
-	/*
-	 * The 82575 uses bits 22:23 for link mode. The mode can be changed
+	/* The 82575 uses bits 22:23 for link mode. The mode can be changed
 	 * based on the EEPROM. We cannot rely upon device ID. There
 	 * is no distinguishable difference between fiber and internal
 	 * SerDes mode on the 82575. There can be an external PHY attached
@@ -621,8 +631,7 @@ static s32 igb_get_phy_id_82575(struct e1000_hw *hw)
 	u32 ctrl_ext;
 	u32 mdic;
 
-	/*
-	 * For SGMII PHYs, we try the list of possible addresses until
+	/* For SGMII PHYs, we try the list of possible addresses until
 	 * we find one that works.  For non-SGMII PHYs
 	 * (e.g. integrated copper PHYs), an address of 1 should
 	 * work.  The result of this function should mean phy->phy_addr
@@ -644,6 +653,7 @@ static s32 igb_get_phy_id_82575(struct e1000_hw *hw)
 			break;
 		case e1000_82580:
 		case e1000_i350:
+		case e1000_i354:
 		case e1000_i210:
 		case e1000_i211:
 			mdic = rd32(E1000_MDICNFG);
@@ -665,8 +675,7 @@ static s32 igb_get_phy_id_82575(struct e1000_hw *hw)
 	wrfl();
 	msleep(300);
 
-	/*
-	 * The address field in the I2CCMD register is 3 bits and 0 is invalid.
+	/* The address field in the I2CCMD register is 3 bits and 0 is invalid.
 	 * Therefore, we need to test 1-7
 	 */
 	for (phy->addr = 1; phy->addr < 8; phy->addr++) {
@@ -674,8 +683,7 @@ static s32 igb_get_phy_id_82575(struct e1000_hw *hw)
 		if (ret_val == 0) {
 			hw_dbg("Vendor ID 0x%08X read at address %u\n",
 			       phy_id, phy->addr);
-			/*
-			 * At the time of this writing, The M88 part is
+			/* At the time of this writing, The M88 part is
 			 * the only supported SGMII PHY product.
 			 */
 			if (phy_id == M88_VENDOR)
@@ -711,15 +719,13 @@ static s32 igb_phy_hw_reset_sgmii_82575(struct e1000_hw *hw)
 {
 	s32 ret_val;
 
-	/*
-	 * This isn't a true "hard" reset, but is the only reset
+	/* This isn't a true "hard" reset, but is the only reset
 	 * available to us at this time.
 	 */
 
 	hw_dbg("Soft resetting SGMII attached PHY...\n");
 
-	/*
-	 * SFP documentation requires the following to configure the SPF module
+	/* SFP documentation requires the following to configure the SPF module
 	 * to work on SGMII.  No further documentation is given.
 	 */
 	ret_val = hw->phy.ops.write_reg(hw, 0x1B, 0x8084);
@@ -774,8 +780,7 @@ static s32 igb_set_d0_lplu_state_82575(struct e1000_hw *hw, bool active)
 		data &= ~IGP02E1000_PM_D0_LPLU;
 		ret_val = phy->ops.write_reg(hw, IGP02E1000_PHY_POWER_MGMT,
 						 data);
-		/*
-		 * LPLU and SmartSpeed are mutually exclusive.  LPLU is used
+		/* LPLU and SmartSpeed are mutually exclusive.  LPLU is used
 		 * during Dx states where the power conservation is most
 		 * important.  During driver activity we should enable
 		 * SmartSpeed, so performance is maintained.
@@ -838,8 +843,7 @@ static s32 igb_set_d0_lplu_state_82580(struct e1000_hw *hw, bool active)
 	} else {
 		data &= ~E1000_82580_PM_D0_LPLU;
 
-		/*
-		 * LPLU and SmartSpeed are mutually exclusive.  LPLU is used
+		/* LPLU and SmartSpeed are mutually exclusive.  LPLU is used
 		 * during Dx states where the power conservation is most
 		 * important.  During driver activity we should enable
 		 * SmartSpeed, so performance is maintained.
@@ -867,7 +871,7 @@ static s32 igb_set_d0_lplu_state_82580(struct e1000_hw *hw, bool active)
  *  During driver activity, SmartSpeed should be enabled so performance is
  *  maintained.
  **/
-s32 igb_set_d3_lplu_state_82580(struct e1000_hw *hw, bool active)
+static s32 igb_set_d3_lplu_state_82580(struct e1000_hw *hw, bool active)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val = 0;
@@ -877,8 +881,7 @@ s32 igb_set_d3_lplu_state_82580(struct e1000_hw *hw, bool active)
 
 	if (!active) {
 		data &= ~E1000_82580_PM_D3_LPLU;
-		/*
-		 * LPLU and SmartSpeed are mutually exclusive.  LPLU is used
+		/* LPLU and SmartSpeed are mutually exclusive.  LPLU is used
 		 * during Dx states where the power conservation is most
 		 * important.  During driver activity we should enable
 		 * SmartSpeed, so performance is maintained.
@@ -964,8 +967,7 @@ static s32 igb_acquire_swfw_sync_82575(struct e1000_hw *hw, u16 mask)
 		if (!(swfw_sync & (fwmask | swmask)))
 			break;
 
-		/*
-		 * Firmware currently using resource (fwmask)
+		/* Firmware currently using resource (fwmask)
 		 * or other software thread using resource (swmask)
 		 */
 		igb_put_hw_semaphore(hw);
@@ -1065,8 +1067,7 @@ static s32 igb_check_for_link_82575(struct e1000_hw *hw)
 	if (hw->phy.media_type != e1000_media_type_copper) {
 		ret_val = igb_get_pcs_speed_and_duplex_82575(hw, &speed,
 		                                             &duplex);
-		/*
-		 * Use this flag to determine if link needs to be checked or
+		/* Use this flag to determine if link needs to be checked or
 		 * not.  If  we have link clear the flag so that we do not
 		 * continue to check for link.
 		 */
@@ -1135,15 +1136,13 @@ static s32 igb_get_pcs_speed_and_duplex_82575(struct e1000_hw *hw, u16 *speed,
 	*speed = 0;
 	*duplex = 0;
 
-	/*
-	 * Read the PCS Status register for link state. For non-copper mode,
+	/* Read the PCS Status register for link state. For non-copper mode,
 	 * the status register is not accurate. The PCS status register is
 	 * used instead.
 	 */
 	pcs = rd32(E1000_PCS_LSTAT);
 
-	/*
-	 * The link up bit determines when link is up on autoneg. The sync ok
+	/* The link up bit determines when link is up on autoneg. The sync ok
 	 * gets set once both sides sync up and agree upon link. Stable link
 	 * can be determined by checking for both link up and link sync ok
 	 */
@@ -1214,8 +1213,7 @@ static s32 igb_reset_hw_82575(struct e1000_hw *hw)
 	u32 ctrl, icr;
 	s32 ret_val;
 
-	/*
-	 * Prevent the PCI-E bus from sticking if there is no TLP connection
+	/* Prevent the PCI-E bus from sticking if there is no TLP connection
 	 * on the last TLP read/write transaction when MAC is reset.
 	 */
 	ret_val = igb_disable_pcie_master(hw);
@@ -1244,8 +1242,7 @@ static s32 igb_reset_hw_82575(struct e1000_hw *hw)
 
 	ret_val = igb_get_auto_rd_done(hw);
 	if (ret_val) {
-		/*
-		 * When auto config read does not complete, do not
+		/* When auto config read does not complete, do not
 		 * return with an error. This can happen in situations
 		 * where there is no eeprom and prevents getting link.
 		 */
@@ -1287,7 +1284,7 @@ static s32 igb_init_hw_82575(struct e1000_hw *hw)
 
 	/* Disabling VLAN filtering */
 	hw_dbg("Initializing the IEEE VLAN\n");
-	if (hw->mac.type == e1000_i350)
+	if ((hw->mac.type == e1000_i350) || (hw->mac.type == e1000_i354))
 		igb_clear_vfta_i350(hw);
 	else
 		igb_clear_vfta(hw);
@@ -1308,8 +1305,7 @@ static s32 igb_init_hw_82575(struct e1000_hw *hw)
 	/* Setup link and flow control */
 	ret_val = igb_setup_link(hw);
 
-	/*
-	 * Clear all of the statistics registers (clear on read).  It is
+	/* Clear all of the statistics registers (clear on read).  It is
 	 * important that we do this after we have tried to establish link
 	 * because the symbol error count will increment wildly if there
 	 * is no link.
@@ -1364,6 +1360,7 @@ static s32 igb_setup_copper_link_82575(struct e1000_hw *hw)
 		switch (hw->phy.id) {
 		case I347AT4_E_PHY_ID:
 		case M88E1112_E_PHY_ID:
+		case M88E1545_E_PHY_ID:
 		case I210_I_PHY_ID:
 			ret_val = igb_copper_link_setup_m88_gen2(hw);
 			break;
@@ -1412,17 +1409,17 @@ static s32 igb_setup_serdes_link_82575(struct e1000_hw *hw)
 		return ret_val;
 
 
-	/*
-	 * On the 82575, SerDes loopback mode persists until it is
+	/* On the 82575, SerDes loopback mode persists until it is
 	 * explicitly turned off or a power cycle is performed.  A read to
 	 * the register does not indicate its status.  Therefore, we ensure
 	 * loopback mode is disabled during initialization.
 	 */
 	wr32(E1000_SCTL, E1000_SCTL_DISABLE_SERDES_LOOPBACK);
 
-	/* power on the sfp cage if present */
+	/* power on the sfp cage if present and turn on I2C */
 	ctrl_ext = rd32(E1000_CTRL_EXT);
 	ctrl_ext &= ~E1000_CTRL_EXT_SDP3_DATA;
+	ctrl_ext |= E1000_CTRL_I2C_ENA;
 	wr32(E1000_CTRL_EXT, ctrl_ext);
 
 	ctrl_reg = rd32(E1000_CTRL);
@@ -1466,8 +1463,7 @@ static s32 igb_setup_serdes_link_82575(struct e1000_hw *hw)
 				pcs_autoneg = false;
 		}
 
-		/*
-		 * non-SGMII modes only supports a speed of 1000/Full for the
+		/* non-SGMII modes only supports a speed of 1000/Full for the
 		 * link so it is best to just force the MAC and let the pcs
 		 * link either autoneg or be forced to 1000/Full
 		 */
@@ -1481,8 +1477,7 @@ static s32 igb_setup_serdes_link_82575(struct e1000_hw *hw)
 
 	wr32(E1000_CTRL, ctrl_reg);
 
-	/*
-	 * New SerDes mode allows for forcing speed or autonegotiating speed
+	/* New SerDes mode allows for forcing speed or autonegotiating speed
 	 * at 1gb. Autoneg should be default set by most drivers. This is the
 	 * mode that will be compatible with older link partners and switches.
 	 * However, both are supported by the hardware and some drivers/tools.
@@ -1592,8 +1587,7 @@ static s32 igb_read_mac_addr_82575(struct e1000_hw *hw)
 {
 	s32 ret_val = 0;
 
-	/*
-	 * If there's an alternate MAC address place it in RAR0
+	/* If there's an alternate MAC address place it in RAR0
 	 * so that it will override the Si installed default perm
 	 * address.
 	 */
@@ -1777,8 +1771,7 @@ static s32 igb_set_pcie_completion_timeout(struct e1000_hw *hw)
 	if (gcr & E1000_GCR_CMPL_TMOUT_MASK)
 		goto out;
 
-	/*
-	 * if capababilities version is type 1 we can write the
+	/* if capabilities version is type 1 we can write the
 	 * timeout of 10ms to 200ms through the GCR register
 	 */
 	if (!(gcr & E1000_GCR_CAP_VER2)) {
@@ -1786,8 +1779,7 @@ static s32 igb_set_pcie_completion_timeout(struct e1000_hw *hw)
 		goto out;
 	}
 
-	/*
-	 * for version 2 capabilities we need to write the config space
+	/* for version 2 capabilities we need to write the config space
 	 * directly in order to set the completion timeout value for
 	 * 16ms to 55ms
 	 */
@@ -1825,6 +1817,7 @@ void igb_vmdq_set_anti_spoofing_pf(struct e1000_hw *hw, bool enable, int pf)
 		reg_offset = E1000_DTXSWC;
 		break;
 	case e1000_i350:
+	case e1000_i354:
 		reg_offset = E1000_TXSWC;
 		break;
 	default:
@@ -1866,6 +1859,7 @@ void igb_vmdq_set_loopback_pf(struct e1000_hw *hw, bool enable)
 			dtxswc &= ~E1000_DTXSWC_VMDQ_LOOPBACK_EN;
 		wr32(E1000_DTXSWC, dtxswc);
 		break;
+	case e1000_i354:
 	case e1000_i350:
 		dtxswc = rd32(E1000_TXSWC);
 		if (enable)
@@ -1879,7 +1873,6 @@ void igb_vmdq_set_loopback_pf(struct e1000_hw *hw, bool enable)
 		break;
 	}
 
-
 }
 
 /**
@@ -1914,7 +1907,6 @@ static s32 igb_read_phy_reg_82580(struct e1000_hw *hw, u32 offset, u16 *data)
 {
 	s32 ret_val;
 
-
 	ret_val = hw->phy.ops.acquire(hw);
 	if (ret_val)
 		goto out;
@@ -2016,8 +2008,7 @@ static s32 igb_reset_hw_82580(struct e1000_hw *hw)
 	/* Get current control state. */
 	ctrl = rd32(E1000_CTRL);
 
-	/*
-	 * Prevent the PCI-E bus from sticking if there is no TLP connection
+	/* Prevent the PCI-E bus from sticking if there is no TLP connection
 	 * on the last TLP read/write transaction when MAC is reset.
 	 */
 	ret_val = igb_disable_pcie_master(hw);
@@ -2052,18 +2043,13 @@ static s32 igb_reset_hw_82580(struct e1000_hw *hw)
 
 	ret_val = igb_get_auto_rd_done(hw);
 	if (ret_val) {
-		/*
-		 * When auto config read does not complete, do not
+		/* When auto config read does not complete, do not
 		 * return with an error. This can happen in situations
 		 * where there is no eeprom and prevents getting link.
 		 */
 		hw_dbg("Auto Read Done did not complete\n");
 	}
 
-	/* If EEPROM is not present, run manual init scripts */
-	if ((rd32(E1000_EECD) & E1000_EECD_PRES) == 0)
-		igb_reset_init_script_82575(hw);
-
 	/* clear global device reset status bit */
 	wr32(E1000_STATUS, E1000_STAT_DEV_RST_SET);
 
@@ -2197,7 +2183,8 @@ static s32 igb_validate_nvm_checksum_82580(struct e1000_hw *hw)
 
 	if (nvm_data & NVM_COMPATIBILITY_BIT_MASK) {
 		/* if checksums compatibility bit is set validate checksums
-		 * for all 4 ports. */
+		 * for all 4 ports.
+		 */
 		eeprom_regions_count = 4;
 	}
 
@@ -2309,6 +2296,41 @@ out:
 }
 
 /**
+ *  __igb_access_emi_reg - Read/write EMI register
+ *  @hw: pointer to the HW structure
+ *  @addr: EMI address to program
+ *  @data: pointer to value to read/write from/to the EMI address
+ *  @read: boolean flag to indicate read or write
+ **/
+static s32 __igb_access_emi_reg(struct e1000_hw *hw, u16 address,
+				  u16 *data, bool read)
+{
+	s32 ret_val = E1000_SUCCESS;
+
+	ret_val = hw->phy.ops.write_reg(hw, E1000_EMIADD, address);
+	if (ret_val)
+		return ret_val;
+
+	if (read)
+		ret_val = hw->phy.ops.read_reg(hw, E1000_EMIDATA, data);
+	else
+		ret_val = hw->phy.ops.write_reg(hw, E1000_EMIDATA, *data);
+
+	return ret_val;
+}
+
+/**
+ *  igb_read_emi_reg - Read Extended Management Interface register
+ *  @hw: pointer to the HW structure
+ *  @addr: EMI address to program
+ *  @data: value to be read from the EMI address
+ **/
+s32 igb_read_emi_reg(struct e1000_hw *hw, u16 addr, u16 *data)
+{
+	return __igb_access_emi_reg(hw, addr, data, true);
+}
+
+/**
  *  igb_set_eee_i350 - Enable/disable EEE support
  *  @hw: pointer to the HW structure
  *
@@ -2338,7 +2360,6 @@ s32 igb_set_eee_i350(struct e1000_hw *hw)
 		if (eee_su & E1000_EEE_SU_LPI_CLK_STP)
 			hw_dbg("LPI Clock Stop Bit should not be set!\n");
 
-
 	} else {
 		ipcnfg &= ~(E1000_IPCNFG_EEE_1G_AN |
 			E1000_IPCNFG_EEE_100M_AN);
@@ -2355,6 +2376,108 @@ out:
 	return ret_val;
 }
 
+/**
+ *  igb_set_eee_i354 - Enable/disable EEE support
+ *  @hw: pointer to the HW structure
+ *
+ *  Enable/disable EEE legacy mode based on setting in dev_spec structure.
+ *
+ **/
+s32 igb_set_eee_i354(struct e1000_hw *hw)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	s32 ret_val = 0;
+	u16 phy_data;
+
+	if ((hw->phy.media_type != e1000_media_type_copper) ||
+	    (phy->id != M88E1545_E_PHY_ID))
+		goto out;
+
+	if (!hw->dev_spec._82575.eee_disable) {
+		/* Switch to PHY page 18. */
+		ret_val = phy->ops.write_reg(hw, E1000_M88E1545_PAGE_ADDR, 18);
+		if (ret_val)
+			goto out;
+
+		ret_val = phy->ops.read_reg(hw, E1000_M88E1545_EEE_CTRL_1,
+					    &phy_data);
+		if (ret_val)
+			goto out;
+
+		phy_data |= E1000_M88E1545_EEE_CTRL_1_MS;
+		ret_val = phy->ops.write_reg(hw, E1000_M88E1545_EEE_CTRL_1,
+					     phy_data);
+		if (ret_val)
+			goto out;
+
+		/* Return the PHY to page 0. */
+		ret_val = phy->ops.write_reg(hw, E1000_M88E1545_PAGE_ADDR, 0);
+		if (ret_val)
+			goto out;
+
+		/* Turn on EEE advertisement. */
+		ret_val = igb_read_xmdio_reg(hw, E1000_EEE_ADV_ADDR_I354,
+					     E1000_EEE_ADV_DEV_I354,
+					     &phy_data);
+		if (ret_val)
+			goto out;
+
+		phy_data |= E1000_EEE_ADV_100_SUPPORTED |
+			    E1000_EEE_ADV_1000_SUPPORTED;
+		ret_val = igb_write_xmdio_reg(hw, E1000_EEE_ADV_ADDR_I354,
+						E1000_EEE_ADV_DEV_I354,
+						phy_data);
+	} else {
+		/* Turn off EEE advertisement. */
+		ret_val = igb_read_xmdio_reg(hw, E1000_EEE_ADV_ADDR_I354,
+					     E1000_EEE_ADV_DEV_I354,
+					     &phy_data);
+		if (ret_val)
+			goto out;
+
+		phy_data &= ~(E1000_EEE_ADV_100_SUPPORTED |
+			      E1000_EEE_ADV_1000_SUPPORTED);
+		ret_val = igb_write_xmdio_reg(hw, E1000_EEE_ADV_ADDR_I354,
+					      E1000_EEE_ADV_DEV_I354,
+					      phy_data);
+	}
+
+out:
+	return ret_val;
+}
+
+/**
+ *  igb_get_eee_status_i354 - Get EEE status
+ *  @hw: pointer to the HW structure
+ *  @status: EEE status
+ *
+ *  Get EEE status by guessing based on whether Tx or Rx LPI indications have
+ *  been received.
+ **/
+s32 igb_get_eee_status_i354(struct e1000_hw *hw, bool *status)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	s32 ret_val = 0;
+	u16 phy_data;
+
+	/* Check if EEE is supported on this device. */
+	if ((hw->phy.media_type != e1000_media_type_copper) ||
+	    (phy->id != M88E1545_E_PHY_ID))
+		goto out;
+
+	ret_val = igb_read_xmdio_reg(hw, E1000_PCS_STATUS_ADDR_I354,
+				     E1000_PCS_STATUS_DEV_I354,
+				     &phy_data);
+	if (ret_val)
+		goto out;
+
+	*status = phy_data & (E1000_PCS_STATUS_TX_LPI_RCVD |
+			      E1000_PCS_STATUS_RX_LPI_RCVD) ? true : false;
+
+out:
+	return ret_val;
+}
+
 static const u8 e1000_emc_temp_data[4] = {
 	E1000_EMC_INTERNAL_DATA,
 	E1000_EMC_DIODE1_DATA,
@@ -2368,11 +2491,12 @@ static const u8 e1000_emc_therm_limit[4] = {
 	E1000_EMC_DIODE3_THERM_LIMIT
 };
 
-/* igb_get_thermal_sensor_data_generic - Gathers thermal sensor data
+/**
+ *  igb_get_thermal_sensor_data_generic - Gathers thermal sensor data
  *  @hw: pointer to hardware structure
  *
  *  Updates the temperatures in mac.thermal_sensor_data
- */
+ **/
 s32 igb_get_thermal_sensor_data_generic(struct e1000_hw *hw)
 {
 	s32 status = E1000_SUCCESS;
@@ -2420,12 +2544,13 @@ s32 igb_get_thermal_sensor_data_generic(struct e1000_hw *hw)
 	return status;
 }
 
-/* igb_init_thermal_sensor_thresh_generic - Sets thermal sensor thresholds
+/**
+ *  igb_init_thermal_sensor_thresh_generic - Sets thermal sensor thresholds
  *  @hw: pointer to hardware structure
  *
  *  Sets the thermal sensor thresholds according to the NVM map
  *  and save off the threshold and location values into mac.thermal_sensor_data
- */
+ **/
 s32 igb_init_thermal_sensor_thresh_generic(struct e1000_hw *hw)
 {
 	s32 status = E1000_SUCCESS;
diff --git a/drivers/net/ethernet/intel/igb/e1000_82575.h b/drivers/net/ethernet/intel/igb/e1000_82575.h
index 73ab41f0e032..74a1506b4235 100644
--- a/drivers/net/ethernet/intel/igb/e1000_82575.h
+++ b/drivers/net/ethernet/intel/igb/e1000_82575.h
@@ -263,7 +263,9 @@ void igb_vmdq_set_anti_spoofing_pf(struct e1000_hw *, bool, int);
 void igb_vmdq_set_loopback_pf(struct e1000_hw *, bool);
 void igb_vmdq_set_replication_pf(struct e1000_hw *, bool);
 u16 igb_rxpbs_adjust_82580(u32 data);
+s32 igb_read_emi_reg(struct e1000_hw *, u16 addr, u16 *data);
 s32 igb_set_eee_i350(struct e1000_hw *);
+s32 igb_set_eee_i354(struct e1000_hw *);
 s32 igb_init_thermal_sensor_thresh_generic(struct e1000_hw *);
 s32 igb_get_thermal_sensor_data_generic(struct e1000_hw *hw);
 
diff --git a/drivers/net/ethernet/intel/igb/e1000_defines.h b/drivers/net/ethernet/intel/igb/e1000_defines.h
index 7e13337d3b9d..31a0f82cc650 100644
--- a/drivers/net/ethernet/intel/igb/e1000_defines.h
+++ b/drivers/net/ethernet/intel/igb/e1000_defines.h
@@ -138,8 +138,7 @@
 #define E1000_RCTL_PMCF           0x00800000    /* pass MAC control frames */
 #define E1000_RCTL_SECRC          0x04000000    /* Strip Ethernet CRC */
 
-/*
- * Use byte values for the following shift parameters
+/* Use byte values for the following shift parameters
  * Usage:
  *     psrctl |= (((ROUNDUP(value0, 128) >> E1000_PSRCTL_BSIZE0_SHIFT) &
  *                  E1000_PSRCTL_BSIZE0_MASK) |
@@ -237,11 +236,14 @@
 #define E1000_STATUS_GIO_MASTER_ENABLE 0x00080000
 /* BMC external code execution disabled */
 
+#define E1000_STATUS_2P5_SKU		0x00001000 /* Val of 2.5GBE SKU strap */
+#define E1000_STATUS_2P5_SKU_OVER	0x00002000 /* Val of 2.5GBE SKU Over */
 /* Constants used to intrepret the masked PCI-X bus speed. */
 
 #define SPEED_10    10
 #define SPEED_100   100
 #define SPEED_1000  1000
+#define SPEED_2500  2500
 #define HALF_DUPLEX 1
 #define FULL_DUPLEX 2
 
@@ -382,8 +384,7 @@
 #define E1000_EICR_OTHER        0x80000000 /* Interrupt Cause Active */
 /* TCP Timer */
 
-/*
- * This defines the bits that are set in the Interrupt Mask
+/* This defines the bits that are set in the Interrupt Mask
  * Set/Read Register.  Each bit is documented below:
  *   o RXT0   = Receiver Timer Interrupt (ring 0)
  *   o TXDW   = Transmit Descriptor Written Back
@@ -440,8 +441,7 @@
 #define E1000_VLAN_FILTER_TBL_SIZE 128  /* VLAN Filter Table (4096 bits) */
 
 /* Receive Address */
-/*
- * Number of high/low register pairs in the RAR. The RAR (Receive Address
+/* Number of high/low register pairs in the RAR. The RAR (Receive Address
  * Registers) holds the directed and multicast addresses that we monitor.
  * Technically, we have 16 spots.  However, we reserve one of these spots
  * (RAR[15]) for our directed address used by controllers with
@@ -760,8 +760,7 @@
 #define MAX_PHY_MULTI_PAGE_REG 0xF
 
 /* Bit definitions for valid PHY IDs. */
-/*
- * I = Integrated
+/* I = Integrated
  * E = External
  */
 #define M88E1111_I_PHY_ID    0x01410CC0
@@ -772,6 +771,7 @@
 #define I350_I_PHY_ID        0x015403B0
 #define M88_VENDOR           0x0141
 #define I210_I_PHY_ID        0x01410C00
+#define M88E1545_E_PHY_ID    0x01410EA0
 
 /* M88E1000 Specific Registers */
 #define M88E1000_PHY_SPEC_CTRL     0x10  /* PHY Specific Control Register */
@@ -791,8 +791,7 @@
 #define M88E1000_PSCR_AUTO_X_1000T     0x0040
 /* Auto crossover enabled all speeds */
 #define M88E1000_PSCR_AUTO_X_MODE      0x0060
-/*
- * 1=Enable Extended 10BASE-T distance (Lower 10BASE-T Rx Threshold
+/* 1=Enable Extended 10BASE-T distance (Lower 10BASE-T Rx Threshold
  * 0=Normal 10BASE-T Rx Threshold
  */
 /* 1=5-bit interface in 100BASE-TX, 0=MII interface in 100BASE-TX */
@@ -802,8 +801,7 @@
 #define M88E1000_PSSR_REV_POLARITY       0x0002 /* 1=Polarity reversed */
 #define M88E1000_PSSR_DOWNSHIFT          0x0020 /* 1=Downshifted */
 #define M88E1000_PSSR_MDIX               0x0040 /* 1=MDIX; 0=MDI */
-/*
- * 0 = <50M
+/* 0 = <50M
  * 1 = 50-80M
  * 2 = 80-110M
  * 3 = 110-140M
@@ -816,20 +814,17 @@
 #define M88E1000_PSSR_CABLE_LENGTH_SHIFT 7
 
 /* M88E1000 Extended PHY Specific Control Register */
-/*
- * 1 = Lost lock detect enabled.
+/* 1 = Lost lock detect enabled.
  * Will assert lost lock and bring
  * link down if idle not seen
  * within 1ms in 1000BASE-T
  */
-/*
- * Number of times we will attempt to autonegotiate before downshifting if we
+/* Number of times we will attempt to autonegotiate before downshifting if we
  * are the master
  */
 #define M88E1000_EPSCR_MASTER_DOWNSHIFT_MASK 0x0C00
 #define M88E1000_EPSCR_MASTER_DOWNSHIFT_1X   0x0000
-/*
- * Number of times we will attempt to autonegotiate before downshifting if we
+/* Number of times we will attempt to autonegotiate before downshifting if we
  * are the slave
  */
 #define M88E1000_EPSCR_SLAVE_DOWNSHIFT_MASK  0x0300
@@ -844,8 +839,7 @@
 
 /* i347-AT4 Extended PHY Specific Control Register */
 
-/*
- *  Number of times we will attempt to autonegotiate before downshifting if we
+/*  Number of times we will attempt to autonegotiate before downshifting if we
  *  are the master
  */
 #define I347AT4_PSCR_DOWNSHIFT_ENABLE 0x0800
@@ -895,6 +889,22 @@
 #define E1000_EEER_LPI_FC            0x00040000  /* EEE Enable on FC */
 #define E1000_EEE_SU_LPI_CLK_STP     0X00800000  /* EEE LPI Clock Stop */
 #define E1000_EEER_EEE_NEG           0x20000000  /* EEE capability nego */
+#define E1000_EEE_LP_ADV_ADDR_I350   0x040F      /* EEE LP Advertisement */
+#define E1000_EEE_LP_ADV_DEV_I210    7           /* EEE LP Adv Device */
+#define E1000_EEE_LP_ADV_ADDR_I210   61          /* EEE LP Adv Register */
+#define E1000_MMDAC_FUNC_DATA        0x4000      /* Data, no post increment */
+#define E1000_M88E1545_PAGE_ADDR	0x16       /* Page Offset Register */
+#define E1000_M88E1545_EEE_CTRL_1	0x0
+#define E1000_M88E1545_EEE_CTRL_1_MS	0x0001     /* EEE Master/Slave */
+#define E1000_EEE_ADV_DEV_I354		7
+#define E1000_EEE_ADV_ADDR_I354		60
+#define E1000_EEE_ADV_100_SUPPORTED	(1 << 1)   /* 100BaseTx EEE Supported */
+#define E1000_EEE_ADV_1000_SUPPORTED	(1 << 2)   /* 1000BaseT EEE Supported */
+#define E1000_PCS_STATUS_DEV_I354	3
+#define E1000_PCS_STATUS_ADDR_I354	1
+#define E1000_PCS_STATUS_TX_LPI_IND	0x0200     /* Tx in LPI state */
+#define E1000_PCS_STATUS_RX_LPI_RCVD	0x0400
+#define E1000_PCS_STATUS_TX_LPI_RCVD	0x0800
 
 /* SerDes Control */
 #define E1000_GEN_CTL_READY             0x80000000
diff --git a/drivers/net/ethernet/intel/igb/e1000_hw.h b/drivers/net/ethernet/intel/igb/e1000_hw.h
index 0d5cf9c63d0d..488abb24a54f 100644
--- a/drivers/net/ethernet/intel/igb/e1000_hw.h
+++ b/drivers/net/ethernet/intel/igb/e1000_hw.h
@@ -38,38 +38,39 @@
 
 struct e1000_hw;
 
-#define E1000_DEV_ID_82576                    0x10C9
-#define E1000_DEV_ID_82576_FIBER              0x10E6
-#define E1000_DEV_ID_82576_SERDES             0x10E7
-#define E1000_DEV_ID_82576_QUAD_COPPER        0x10E8
-#define E1000_DEV_ID_82576_QUAD_COPPER_ET2    0x1526
-#define E1000_DEV_ID_82576_NS                 0x150A
-#define E1000_DEV_ID_82576_NS_SERDES          0x1518
-#define E1000_DEV_ID_82576_SERDES_QUAD        0x150D
-#define E1000_DEV_ID_82575EB_COPPER           0x10A7
-#define E1000_DEV_ID_82575EB_FIBER_SERDES     0x10A9
-#define E1000_DEV_ID_82575GB_QUAD_COPPER      0x10D6
-#define E1000_DEV_ID_82580_COPPER             0x150E
-#define E1000_DEV_ID_82580_FIBER              0x150F
-#define E1000_DEV_ID_82580_SERDES             0x1510
-#define E1000_DEV_ID_82580_SGMII              0x1511
-#define E1000_DEV_ID_82580_COPPER_DUAL        0x1516
-#define E1000_DEV_ID_82580_QUAD_FIBER         0x1527
-#define E1000_DEV_ID_DH89XXCC_SGMII           0x0438
-#define E1000_DEV_ID_DH89XXCC_SERDES          0x043A
-#define E1000_DEV_ID_DH89XXCC_BACKPLANE       0x043C
-#define E1000_DEV_ID_DH89XXCC_SFP             0x0440
-#define E1000_DEV_ID_I350_COPPER              0x1521
-#define E1000_DEV_ID_I350_FIBER               0x1522
-#define E1000_DEV_ID_I350_SERDES              0x1523
-#define E1000_DEV_ID_I350_SGMII               0x1524
+#define E1000_DEV_ID_82576			0x10C9
+#define E1000_DEV_ID_82576_FIBER		0x10E6
+#define E1000_DEV_ID_82576_SERDES		0x10E7
+#define E1000_DEV_ID_82576_QUAD_COPPER		0x10E8
+#define E1000_DEV_ID_82576_QUAD_COPPER_ET2	0x1526
+#define E1000_DEV_ID_82576_NS			0x150A
+#define E1000_DEV_ID_82576_NS_SERDES		0x1518
+#define E1000_DEV_ID_82576_SERDES_QUAD		0x150D
+#define E1000_DEV_ID_82575EB_COPPER		0x10A7
+#define E1000_DEV_ID_82575EB_FIBER_SERDES	0x10A9
+#define E1000_DEV_ID_82575GB_QUAD_COPPER	0x10D6
+#define E1000_DEV_ID_82580_COPPER		0x150E
+#define E1000_DEV_ID_82580_FIBER		0x150F
+#define E1000_DEV_ID_82580_SERDES		0x1510
+#define E1000_DEV_ID_82580_SGMII		0x1511
+#define E1000_DEV_ID_82580_COPPER_DUAL		0x1516
+#define E1000_DEV_ID_82580_QUAD_FIBER		0x1527
+#define E1000_DEV_ID_DH89XXCC_SGMII		0x0438
+#define E1000_DEV_ID_DH89XXCC_SERDES		0x043A
+#define E1000_DEV_ID_DH89XXCC_BACKPLANE		0x043C
+#define E1000_DEV_ID_DH89XXCC_SFP		0x0440
+#define E1000_DEV_ID_I350_COPPER		0x1521
+#define E1000_DEV_ID_I350_FIBER			0x1522
+#define E1000_DEV_ID_I350_SERDES		0x1523
+#define E1000_DEV_ID_I350_SGMII			0x1524
 #define E1000_DEV_ID_I210_COPPER		0x1533
-#define E1000_DEV_ID_I210_COPPER_OEM1		0x1534
-#define E1000_DEV_ID_I210_COPPER_IT		0x1535
 #define E1000_DEV_ID_I210_FIBER			0x1536
 #define E1000_DEV_ID_I210_SERDES		0x1537
 #define E1000_DEV_ID_I210_SGMII			0x1538
 #define E1000_DEV_ID_I211_COPPER		0x1539
+#define E1000_DEV_ID_I354_BACKPLANE_1GBPS	0x1F40
+#define E1000_DEV_ID_I354_SGMII			0x1F41
+#define E1000_DEV_ID_I354_BACKPLANE_2_5GBPS	0x1F45
 
 #define E1000_REVISION_2 2
 #define E1000_REVISION_4 4
@@ -90,6 +91,7 @@ enum e1000_mac_type {
 	e1000_82576,
 	e1000_82580,
 	e1000_i350,
+	e1000_i354,
 	e1000_i210,
 	e1000_i211,
 	e1000_num_macs  /* List is 1-based, so subtract 1 for true count. */
@@ -98,7 +100,8 @@ enum e1000_mac_type {
 enum e1000_media_type {
 	e1000_media_type_unknown = 0,
 	e1000_media_type_copper = 1,
-	e1000_media_type_internal_serdes = 2,
+	e1000_media_type_fiber = 2,
+	e1000_media_type_internal_serdes = 3,
 	e1000_num_media_types
 };
 
@@ -524,6 +527,7 @@ struct e1000_dev_spec_82575 {
 	bool sgmii_active;
 	bool global_device_reset;
 	bool eee_disable;
+	bool clear_semaphore_once;
 };
 
 struct e1000_hw {
diff --git a/drivers/net/ethernet/intel/igb/e1000_i210.c b/drivers/net/ethernet/intel/igb/e1000_i210.c
index 6a42344f24f1..ddb3cf51b9b9 100644
--- a/drivers/net/ethernet/intel/igb/e1000_i210.c
+++ b/drivers/net/ethernet/intel/igb/e1000_i210.c
@@ -44,10 +44,42 @@
 static s32 igb_get_hw_semaphore_i210(struct e1000_hw *hw)
 {
 	u32 swsm;
-	s32 ret_val = E1000_SUCCESS;
 	s32 timeout = hw->nvm.word_size + 1;
 	s32 i = 0;
 
+	/* Get the SW semaphore */
+	while (i < timeout) {
+		swsm = rd32(E1000_SWSM);
+		if (!(swsm & E1000_SWSM_SMBI))
+			break;
+
+		udelay(50);
+		i++;
+	}
+
+	if (i == timeout) {
+		/* In rare circumstances, the SW semaphore may already be held
+		 * unintentionally. Clear the semaphore once before giving up.
+		 */
+		if (hw->dev_spec._82575.clear_semaphore_once) {
+			hw->dev_spec._82575.clear_semaphore_once = false;
+			igb_put_hw_semaphore(hw);
+			for (i = 0; i < timeout; i++) {
+				swsm = rd32(E1000_SWSM);
+				if (!(swsm & E1000_SWSM_SMBI))
+					break;
+
+				udelay(50);
+			}
+		}
+
+		/* If we do not have the semaphore here, we have to give up. */
+		if (i == timeout) {
+			hw_dbg("Driver can't access device - SMBI bit is set.\n");
+			return -E1000_ERR_NVM;
+		}
+	}
+
 	/* Get the FW semaphore. */
 	for (i = 0; i < timeout; i++) {
 		swsm = rd32(E1000_SWSM);
@@ -64,12 +96,10 @@ static s32 igb_get_hw_semaphore_i210(struct e1000_hw *hw)
 		/* Release semaphores */
 		igb_put_hw_semaphore(hw);
 		hw_dbg("Driver can't access the NVM\n");
-		ret_val = -E1000_ERR_NVM;
-		goto out;
+		return -E1000_ERR_NVM;
 	}
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
 /**
@@ -99,23 +129,6 @@ void igb_release_nvm_i210(struct e1000_hw *hw)
 }
 
 /**
- *  igb_put_hw_semaphore_i210 - Release hardware semaphore
- *  @hw: pointer to the HW structure
- *
- *  Release hardware semaphore used to access the PHY or NVM
- */
-static void igb_put_hw_semaphore_i210(struct e1000_hw *hw)
-{
-	u32 swsm;
-
-	swsm = rd32(E1000_SWSM);
-
-	swsm &= ~E1000_SWSM_SWESMBI;
-
-	wr32(E1000_SWSM, swsm);
-}
-
-/**
  *  igb_acquire_swfw_sync_i210 - Acquire SW/FW semaphore
  *  @hw: pointer to the HW structure
  *  @mask: specifies which semaphore to acquire
@@ -138,13 +151,11 @@ s32 igb_acquire_swfw_sync_i210(struct e1000_hw *hw, u16 mask)
 		}
 
 		swfw_sync = rd32(E1000_SW_FW_SYNC);
-		if (!(swfw_sync & fwmask))
+		if (!(swfw_sync & (fwmask | swmask)))
 			break;
 
-		/*
-		 * Firmware currently using resource (fwmask)
-		 */
-		igb_put_hw_semaphore_i210(hw);
+		/* Firmware currently using resource (fwmask) */
+		igb_put_hw_semaphore(hw);
 		mdelay(5);
 		i++;
 	}
@@ -158,7 +169,7 @@ s32 igb_acquire_swfw_sync_i210(struct e1000_hw *hw, u16 mask)
 	swfw_sync |= swmask;
 	wr32(E1000_SW_FW_SYNC, swfw_sync);
 
-	igb_put_hw_semaphore_i210(hw);
+	igb_put_hw_semaphore(hw);
 out:
 	return ret_val;
 }
@@ -182,7 +193,7 @@ void igb_release_swfw_sync_i210(struct e1000_hw *hw, u16 mask)
 	swfw_sync &= ~mask;
 	wr32(E1000_SW_FW_SYNC, swfw_sync);
 
-	igb_put_hw_semaphore_i210(hw);
+	igb_put_hw_semaphore(hw);
 }
 
 /**
@@ -203,7 +214,8 @@ s32 igb_read_nvm_srrd_i210(struct e1000_hw *hw, u16 offset, u16 words,
 
 	/* We cannot hold synchronization semaphores for too long,
 	 * because of forceful takeover procedure. However it is more efficient
-	 * to read in bursts than synchronizing access for each word. */
+	 * to read in bursts than synchronizing access for each word.
+	 */
 	for (i = 0; i < words; i += E1000_EERD_EEWR_MAX_COUNT) {
 		count = (words - i) / E1000_EERD_EEWR_MAX_COUNT > 0 ?
 			E1000_EERD_EEWR_MAX_COUNT : (words - i);
@@ -242,8 +254,7 @@ static s32 igb_write_nvm_srwr(struct e1000_hw *hw, u16 offset, u16 words,
 	u32 attempts = 100000;
 	s32 ret_val = E1000_SUCCESS;
 
-	/*
-	 * A check for invalid values:  offset too large, too many words,
+	/* A check for invalid values:  offset too large, too many words,
 	 * too many words for the offset, and not enough words.
 	 */
 	if ((offset >= nvm->word_size) || (words > (nvm->word_size - offset)) ||
@@ -294,7 +305,7 @@ out:
  *
  *  If error code is returned, data and Shadow RAM may be inconsistent - buffer
  *  partially written.
- */
+ **/
 s32 igb_write_nvm_srwr_i210(struct e1000_hw *hw, u16 offset, u16 words,
 			      u16 *data)
 {
@@ -326,7 +337,7 @@ s32 igb_write_nvm_srwr_i210(struct e1000_hw *hw, u16 offset, u16 words,
 /**
  *  igb_read_nvm_i211 - Read NVM wrapper function for I211
  *  @hw: pointer to the HW structure
- *  @address: the word address (aka eeprom offset) to read
+ *  @words: number of words to read
  *  @data: pointer to the data read
  *
  *  Wrapper function to return data formerly found in the NVM.
@@ -549,8 +560,7 @@ s32 igb_validate_nvm_checksum_i210(struct e1000_hw *hw)
 
 	if (hw->nvm.ops.acquire(hw) == E1000_SUCCESS) {
 
-		/*
-		 * Replace the read function with semaphore grabbing with
+		/* Replace the read function with semaphore grabbing with
 		 * the one that skips this for a while.
 		 * We have semaphore taken already here.
 		 */
@@ -570,7 +580,6 @@ s32 igb_validate_nvm_checksum_i210(struct e1000_hw *hw)
 	return status;
 }
 
-
 /**
  *  igb_update_nvm_checksum_i210 - Update EEPROM checksum
  *  @hw: pointer to the HW structure
@@ -585,8 +594,7 @@ s32 igb_update_nvm_checksum_i210(struct e1000_hw *hw)
 	u16 checksum = 0;
 	u16 i, nvm_data;
 
-	/*
-	 * Read the first word from the EEPROM. If this times out or fails, do
+	/* Read the first word from the EEPROM. If this times out or fails, do
 	 * not continue or we could be in for a very long wait while every
 	 * EEPROM read fails
 	 */
@@ -597,8 +605,7 @@ s32 igb_update_nvm_checksum_i210(struct e1000_hw *hw)
 	}
 
 	if (hw->nvm.ops.acquire(hw) == E1000_SUCCESS) {
-		/*
-		 * Do not use hw->nvm.ops.write, hw->nvm.ops.read
+		/* Do not use hw->nvm.ops.write, hw->nvm.ops.read
 		 * because we do not want to take the synchronization
 		 * semaphores twice here.
 		 */
@@ -635,7 +642,7 @@ out:
  *  igb_pool_flash_update_done_i210 - Pool FLUDONE status.
  *  @hw: pointer to the HW structure
  *
- */
+ **/
 static s32 igb_pool_flash_update_done_i210(struct e1000_hw *hw)
 {
 	s32 ret_val = -E1000_ERR_NVM;
@@ -714,3 +721,68 @@ s32 igb_valid_led_default_i210(struct e1000_hw *hw, u16 *data)
 out:
 	return ret_val;
 }
+
+/**
+ *  __igb_access_xmdio_reg - Read/write XMDIO register
+ *  @hw: pointer to the HW structure
+ *  @address: XMDIO address to program
+ *  @dev_addr: device address to program
+ *  @data: pointer to value to read/write from/to the XMDIO address
+ *  @read: boolean flag to indicate read or write
+ **/
+static s32 __igb_access_xmdio_reg(struct e1000_hw *hw, u16 address,
+				  u8 dev_addr, u16 *data, bool read)
+{
+	s32 ret_val = E1000_SUCCESS;
+
+	ret_val = hw->phy.ops.write_reg(hw, E1000_MMDAC, dev_addr);
+	if (ret_val)
+		return ret_val;
+
+	ret_val = hw->phy.ops.write_reg(hw, E1000_MMDAAD, address);
+	if (ret_val)
+		return ret_val;
+
+	ret_val = hw->phy.ops.write_reg(hw, E1000_MMDAC, E1000_MMDAC_FUNC_DATA |
+							 dev_addr);
+	if (ret_val)
+		return ret_val;
+
+	if (read)
+		ret_val = hw->phy.ops.read_reg(hw, E1000_MMDAAD, data);
+	else
+		ret_val = hw->phy.ops.write_reg(hw, E1000_MMDAAD, *data);
+	if (ret_val)
+		return ret_val;
+
+	/* Recalibrate the device back to 0 */
+	ret_val = hw->phy.ops.write_reg(hw, E1000_MMDAC, 0);
+	if (ret_val)
+		return ret_val;
+
+	return ret_val;
+}
+
+/**
+ *  igb_read_xmdio_reg - Read XMDIO register
+ *  @hw: pointer to the HW structure
+ *  @addr: XMDIO address to program
+ *  @dev_addr: device address to program
+ *  @data: value to be read from the EMI address
+ **/
+s32 igb_read_xmdio_reg(struct e1000_hw *hw, u16 addr, u8 dev_addr, u16 *data)
+{
+	return __igb_access_xmdio_reg(hw, addr, dev_addr, data, true);
+}
+
+/**
+ *  igb_write_xmdio_reg - Write XMDIO register
+ *  @hw: pointer to the HW structure
+ *  @addr: XMDIO address to program
+ *  @dev_addr: device address to program
+ *  @data: value to be written to the XMDIO address
+ **/
+s32 igb_write_xmdio_reg(struct e1000_hw *hw, u16 addr, u8 dev_addr, u16 data)
+{
+	return __igb_access_xmdio_reg(hw, addr, dev_addr, &data, false);
+}
diff --git a/drivers/net/ethernet/intel/igb/e1000_i210.h b/drivers/net/ethernet/intel/igb/e1000_i210.h
index e4e1a73b7c75..bfc08e05c907 100644
--- a/drivers/net/ethernet/intel/igb/e1000_i210.h
+++ b/drivers/net/ethernet/intel/igb/e1000_i210.h
@@ -45,6 +45,10 @@ extern s32 igb_read_nvm_i211(struct e1000_hw *hw, u16 offset, u16 words,
 			       u16 *data);
 extern s32 igb_read_invm_version(struct e1000_hw *hw,
 				 struct e1000_fw_version *invm_ver);
+extern s32 igb_read_xmdio_reg(struct e1000_hw *hw, u16 addr, u8 dev_addr,
+			      u16 *data);
+extern s32 igb_write_xmdio_reg(struct e1000_hw *hw, u16 addr, u8 dev_addr,
+			       u16 data);
 
 #define E1000_STM_OPCODE		0xDB00
 #define E1000_EEPROM_FLASH_SIZE_WORD	0x11
diff --git a/drivers/net/ethernet/intel/igb/e1000_mac.c b/drivers/net/ethernet/intel/igb/e1000_mac.c
index a5c7200b9a71..2559d70a2321 100644
--- a/drivers/net/ethernet/intel/igb/e1000_mac.c
+++ b/drivers/net/ethernet/intel/igb/e1000_mac.c
@@ -214,7 +214,7 @@ s32 igb_vfta_set(struct e1000_hw *hw, u32 vid, bool add)
 		else
 			vfta &= ~mask;
 	}
-	if (hw->mac.type == e1000_i350)
+	if ((hw->mac.type == e1000_i350) || (hw->mac.type == e1000_i354))
 		igb_write_vfta_i350(hw, index, vfta);
 	else
 		igb_write_vfta(hw, index, vfta);
@@ -230,8 +230,8 @@ s32 igb_vfta_set(struct e1000_hw *hw, u32 vid, bool add)
  *  Checks the nvm for an alternate MAC address.  An alternate MAC address
  *  can be setup by pre-boot software and must be treated like a permanent
  *  address and must override the actual permanent MAC address.  If an
- *  alternate MAC address is fopund it is saved in the hw struct and
- *  prgrammed into RAR0 and the cuntion returns success, otherwise the
+ *  alternate MAC address is found it is saved in the hw struct and
+ *  programmed into RAR0 and the function returns success, otherwise the
  *  function returns an error.
  **/
 s32 igb_check_alt_mac_addr(struct e1000_hw *hw)
@@ -241,8 +241,7 @@ s32 igb_check_alt_mac_addr(struct e1000_hw *hw)
 	u16 offset, nvm_alt_mac_addr_offset, nvm_data;
 	u8 alt_mac_addr[ETH_ALEN];
 
-	/*
-	 * Alternate MAC address is handled by the option ROM for 82580
+	/* Alternate MAC address is handled by the option ROM for 82580
 	 * and newer. SW support not required.
 	 */
 	if (hw->mac.type >= e1000_82580)
@@ -285,8 +284,7 @@ s32 igb_check_alt_mac_addr(struct e1000_hw *hw)
 		goto out;
 	}
 
-	/*
-	 * We have a valid alternate MAC address, and we want to treat it the
+	/* We have a valid alternate MAC address, and we want to treat it the
 	 * same as the normal permanent MAC address stored by the HW into the
 	 * RAR. Do this by mapping this address into RAR0.
 	 */
@@ -309,8 +307,7 @@ void igb_rar_set(struct e1000_hw *hw, u8 *addr, u32 index)
 {
 	u32 rar_low, rar_high;
 
-	/*
-	 * HW expects these in little endian so we reverse the byte order
+	/* HW expects these in little endian so we reverse the byte order
 	 * from network order (big endian) to little endian
 	 */
 	rar_low = ((u32) addr[0] |
@@ -323,8 +320,7 @@ void igb_rar_set(struct e1000_hw *hw, u8 *addr, u32 index)
 	if (rar_low || rar_high)
 		rar_high |= E1000_RAH_AV;
 
-	/*
-	 * Some bridges will combine consecutive 32-bit writes into
+	/* Some bridges will combine consecutive 32-bit writes into
 	 * a single burst write, which will malfunction on some parts.
 	 * The flushes avoid this.
 	 */
@@ -348,8 +344,7 @@ void igb_mta_set(struct e1000_hw *hw, u32 hash_value)
 {
 	u32 hash_bit, hash_reg, mta;
 
-	/*
-	 * The MTA is a register array of 32-bit registers. It is
+	/* The MTA is a register array of 32-bit registers. It is
 	 * treated like an array of (32*mta_reg_count) bits.  We want to
 	 * set bit BitArray[hash_value]. So we figure out what register
 	 * the bit is in, read it, OR in the new bit, then write
@@ -386,15 +381,13 @@ static u32 igb_hash_mc_addr(struct e1000_hw *hw, u8 *mc_addr)
 	/* Register count multiplied by bits per register */
 	hash_mask = (hw->mac.mta_reg_count * 32) - 1;
 
-	/*
-	 * For a mc_filter_type of 0, bit_shift is the number of left-shifts
+	/* For a mc_filter_type of 0, bit_shift is the number of left-shifts
 	 * where 0xFF would still fall within the hash mask.
 	 */
 	while (hash_mask >> bit_shift != 0xFF)
 		bit_shift++;
 
-	/*
-	 * The portion of the address that is used for the hash table
+	/* The portion of the address that is used for the hash table
 	 * is determined by the mc_filter_type setting.
 	 * The algorithm is such that there is a total of 8 bits of shifting.
 	 * The bit_shift for a mc_filter_type of 0 represents the number of
@@ -536,8 +529,7 @@ s32 igb_check_for_copper_link(struct e1000_hw *hw)
 	s32 ret_val;
 	bool link;
 
-	/*
-	 * We only want to go out to the PHY registers to see if Auto-Neg
+	/* We only want to go out to the PHY registers to see if Auto-Neg
 	 * has completed and/or if our link status has changed.  The
 	 * get_link_status flag is set upon receiving a Link Status
 	 * Change or Rx Sequence Error interrupt.
@@ -547,8 +539,7 @@ s32 igb_check_for_copper_link(struct e1000_hw *hw)
 		goto out;
 	}
 
-	/*
-	 * First we want to see if the MII Status Register reports
+	/* First we want to see if the MII Status Register reports
 	 * link.  If so, then we want to get the current speed/duplex
 	 * of the PHY.
 	 */
@@ -561,14 +552,12 @@ s32 igb_check_for_copper_link(struct e1000_hw *hw)
 
 	mac->get_link_status = false;
 
-	/*
-	 * Check if there was DownShift, must be checked
+	/* Check if there was DownShift, must be checked
 	 * immediately after link-up
 	 */
 	igb_check_downshift(hw);
 
-	/*
-	 * If we are forcing speed/duplex, then we simply return since
+	/* If we are forcing speed/duplex, then we simply return since
 	 * we have already determined whether we have link or not.
 	 */
 	if (!mac->autoneg) {
@@ -576,15 +565,13 @@ s32 igb_check_for_copper_link(struct e1000_hw *hw)
 		goto out;
 	}
 
-	/*
-	 * Auto-Neg is enabled.  Auto Speed Detection takes care
+	/* Auto-Neg is enabled.  Auto Speed Detection takes care
 	 * of MAC speed/duplex configuration.  So we only need to
 	 * configure Collision Distance in the MAC.
 	 */
 	igb_config_collision_dist(hw);
 
-	/*
-	 * Configure Flow Control now that Auto-Neg has completed.
+	/* Configure Flow Control now that Auto-Neg has completed.
 	 * First, we need to restore the desired flow control
 	 * settings because we may have had to re-autoneg with a
 	 * different link partner.
@@ -611,15 +598,13 @@ s32 igb_setup_link(struct e1000_hw *hw)
 {
 	s32 ret_val = 0;
 
-	/*
-	 * In the case of the phy reset being blocked, we already have a link.
+	/* In the case of the phy reset being blocked, we already have a link.
 	 * We do not need to set it up again.
 	 */
 	if (igb_check_reset_block(hw))
 		goto out;
 
-	/*
-	 * If requested flow control is set to default, set flow control
+	/* If requested flow control is set to default, set flow control
 	 * based on the EEPROM flow control settings.
 	 */
 	if (hw->fc.requested_mode == e1000_fc_default) {
@@ -628,8 +613,7 @@ s32 igb_setup_link(struct e1000_hw *hw)
 			goto out;
 	}
 
-	/*
-	 * We want to save off the original Flow Control configuration just
+	/* We want to save off the original Flow Control configuration just
 	 * in case we get disconnected and then reconnected into a different
 	 * hub or switch with different Flow Control capabilities.
 	 */
@@ -642,8 +626,7 @@ s32 igb_setup_link(struct e1000_hw *hw)
 	if (ret_val)
 		goto out;
 
-	/*
-	 * Initialize the flow control address, type, and PAUSE timer
+	/* Initialize the flow control address, type, and PAUSE timer
 	 * registers to their default values.  This is done even if flow
 	 * control is disabled, because it does not hurt anything to
 	 * initialize these registers.
@@ -696,16 +679,14 @@ static s32 igb_set_fc_watermarks(struct e1000_hw *hw)
 	s32 ret_val = 0;
 	u32 fcrtl = 0, fcrth = 0;
 
-	/*
-	 * Set the flow control receive threshold registers.  Normally,
+	/* Set the flow control receive threshold registers.  Normally,
 	 * these registers will be set to a default threshold that may be
 	 * adjusted later by the driver's runtime code.  However, if the
 	 * ability to transmit pause frames is not enabled, then these
 	 * registers will be set to 0.
 	 */
 	if (hw->fc.current_mode & e1000_fc_tx_pause) {
-		/*
-		 * We need to set up the Receive Threshold high and low water
+		/* We need to set up the Receive Threshold high and low water
 		 * marks as well as (optionally) enabling the transmission of
 		 * XON frames.
 		 */
@@ -733,8 +714,7 @@ static s32 igb_set_default_fc(struct e1000_hw *hw)
 	s32 ret_val = 0;
 	u16 nvm_data;
 
-	/*
-	 * Read and store word 0x0F of the EEPROM. This word contains bits
+	/* Read and store word 0x0F of the EEPROM. This word contains bits
 	 * that determine the hardware's default PAUSE (flow control) mode,
 	 * a bit that determines whether the HW defaults to enabling or
 	 * disabling auto-negotiation, and the direction of the
@@ -778,8 +758,7 @@ s32 igb_force_mac_fc(struct e1000_hw *hw)
 
 	ctrl = rd32(E1000_CTRL);
 
-	/*
-	 * Because we didn't get link via the internal auto-negotiation
+	/* Because we didn't get link via the internal auto-negotiation
 	 * mechanism (we either forced link or we got link via PHY
 	 * auto-neg), we have to manually enable/disable transmit an
 	 * receive flow control.
@@ -843,8 +822,7 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 	u16 mii_status_reg, mii_nway_adv_reg, mii_nway_lp_ability_reg;
 	u16 speed, duplex;
 
-	/*
-	 * Check for the case where we have fiber media and auto-neg failed
+	/* Check for the case where we have fiber media and auto-neg failed
 	 * so we had to force link.  In this case, we need to force the
 	 * configuration of the MAC to match the "fc" parameter.
 	 */
@@ -861,15 +839,13 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 		goto out;
 	}
 
-	/*
-	 * Check for the case where we have copper media and auto-neg is
+	/* Check for the case where we have copper media and auto-neg is
 	 * enabled.  In this case, we need to check and see if Auto-Neg
 	 * has completed, and if so, how the PHY and link partner has
 	 * flow control configured.
 	 */
 	if ((hw->phy.media_type == e1000_media_type_copper) && mac->autoneg) {
-		/*
-		 * Read the MII Status Register and check to see if AutoNeg
+		/* Read the MII Status Register and check to see if AutoNeg
 		 * has completed.  We read this twice because this reg has
 		 * some "sticky" (latched) bits.
 		 */
@@ -888,8 +864,7 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 			goto out;
 		}
 
-		/*
-		 * The AutoNeg process has completed, so we now need to
+		/* The AutoNeg process has completed, so we now need to
 		 * read both the Auto Negotiation Advertisement
 		 * Register (Address 4) and the Auto_Negotiation Base
 		 * Page Ability Register (Address 5) to determine how
@@ -904,8 +879,7 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 		if (ret_val)
 			goto out;
 
-		/*
-		 * Two bits in the Auto Negotiation Advertisement Register
+		/* Two bits in the Auto Negotiation Advertisement Register
 		 * (Address 4) and two bits in the Auto Negotiation Base
 		 * Page Ability Register (Address 5) determine flow control
 		 * for both the PHY and the link partner.  The following
@@ -940,8 +914,7 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 		 */
 		if ((mii_nway_adv_reg & NWAY_AR_PAUSE) &&
 		    (mii_nway_lp_ability_reg & NWAY_LPAR_PAUSE)) {
-			/*
-			 * Now we need to check if the user selected RX ONLY
+			/* Now we need to check if the user selected RX ONLY
 			 * of pause frames.  In this case, we had to advertise
 			 * FULL flow control because we could not advertise RX
 			 * ONLY. Hence, we must now check to see if we need to
@@ -956,8 +929,7 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 				       "RX PAUSE frames only.\r\n");
 			}
 		}
-		/*
-		 * For receiving PAUSE frames ONLY.
+		/* For receiving PAUSE frames ONLY.
 		 *
 		 *   LOCAL DEVICE  |   LINK PARTNER
 		 * PAUSE | ASM_DIR | PAUSE | ASM_DIR | Result
@@ -971,8 +943,7 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 			hw->fc.current_mode = e1000_fc_tx_pause;
 			hw_dbg("Flow Control = TX PAUSE frames only.\r\n");
 		}
-		/*
-		 * For transmitting PAUSE frames ONLY.
+		/* For transmitting PAUSE frames ONLY.
 		 *
 		 *   LOCAL DEVICE  |   LINK PARTNER
 		 * PAUSE | ASM_DIR | PAUSE | ASM_DIR | Result
@@ -986,8 +957,7 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 			hw->fc.current_mode = e1000_fc_rx_pause;
 			hw_dbg("Flow Control = RX PAUSE frames only.\r\n");
 		}
-		/*
-		 * Per the IEEE spec, at this point flow control should be
+		/* Per the IEEE spec, at this point flow control should be
 		 * disabled.  However, we want to consider that we could
 		 * be connected to a legacy switch that doesn't advertise
 		 * desired flow control, but can be forced on the link
@@ -1007,9 +977,9 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 		 * be asked to delay transmission of packets than asking
 		 * our link partner to pause transmission of frames.
 		 */
-		else if ((hw->fc.requested_mode == e1000_fc_none ||
-			  hw->fc.requested_mode == e1000_fc_tx_pause) ||
-			 hw->fc.strict_ieee) {
+		else if ((hw->fc.requested_mode == e1000_fc_none) ||
+			 (hw->fc.requested_mode == e1000_fc_tx_pause) ||
+			 (hw->fc.strict_ieee)) {
 			hw->fc.current_mode = e1000_fc_none;
 			hw_dbg("Flow Control = NONE.\r\n");
 		} else {
@@ -1017,8 +987,7 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 			hw_dbg("Flow Control = RX PAUSE frames only.\r\n");
 		}
 
-		/*
-		 * Now we need to do one last check...  If we auto-
+		/* Now we need to do one last check...  If we auto-
 		 * negotiated to HALF DUPLEX, flow control should not be
 		 * enabled per IEEE 802.3 spec.
 		 */
@@ -1031,8 +1000,7 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 		if (duplex == HALF_DUPLEX)
 			hw->fc.current_mode = e1000_fc_none;
 
-		/*
-		 * Now we call a subroutine to actually force the MAC
+		/* Now we call a subroutine to actually force the MAC
 		 * controller to use the correct flow control settings.
 		 */
 		ret_val = igb_force_mac_fc(hw);
@@ -1203,6 +1171,17 @@ s32 igb_get_speed_and_duplex_copper(struct e1000_hw *hw, u16 *speed,
 		hw_dbg("Half Duplex\n");
 	}
 
+	/* Check if it is an I354 2.5Gb backplane connection. */
+	if (hw->mac.type == e1000_i354) {
+		if ((status & E1000_STATUS_2P5_SKU) &&
+		    !(status & E1000_STATUS_2P5_SKU_OVER)) {
+			*speed = SPEED_2500;
+			*duplex = FULL_DUPLEX;
+			hw_dbg("2500 Mbs, ");
+			hw_dbg("Full Duplex\n");
+		}
+	}
+
 	return 0;
 }
 
@@ -1427,8 +1406,7 @@ s32 igb_blink_led(struct e1000_hw *hw)
 	u32 ledctl_blink = 0;
 	u32 i;
 
-	/*
-	 * set the blink bit for each LED that's "on" (0x0E)
+	/* set the blink bit for each LED that's "on" (0x0E)
 	 * in ledctl_mode2
 	 */
 	ledctl_blink = hw->mac.ledctl_mode2;
@@ -1467,7 +1445,7 @@ s32 igb_led_off(struct e1000_hw *hw)
  *  @hw: pointer to the HW structure
  *
  *  Returns 0 (0) if successful, else returns -10
- *  (-E1000_ERR_MASTER_REQUESTS_PENDING) if master disable bit has not casued
+ *  (-E1000_ERR_MASTER_REQUESTS_PENDING) if master disable bit has not caused
  *  the master requests to be disabled.
  *
  *  Disables PCI-Express master access and verifies there are no pending
diff --git a/drivers/net/ethernet/intel/igb/e1000_mac.h b/drivers/net/ethernet/intel/igb/e1000_mac.h
index e6d6ce433261..5e13e83cc608 100644
--- a/drivers/net/ethernet/intel/igb/e1000_mac.h
+++ b/drivers/net/ethernet/intel/igb/e1000_mac.h
@@ -35,8 +35,7 @@
 #include "e1000_defines.h"
 #include "e1000_i210.h"
 
-/*
- * Functions that should not be called directly from drivers but can be used
+/* Functions that should not be called directly from drivers but can be used
  * by other files in this 'shared code'
  */
 s32  igb_blink_led(struct e1000_hw *hw);
@@ -49,15 +48,15 @@ s32  igb_get_auto_rd_done(struct e1000_hw *hw);
 s32  igb_get_bus_info_pcie(struct e1000_hw *hw);
 s32  igb_get_hw_semaphore(struct e1000_hw *hw);
 s32  igb_get_speed_and_duplex_copper(struct e1000_hw *hw, u16 *speed,
-				       u16 *duplex);
+				     u16 *duplex);
 s32  igb_id_led_init(struct e1000_hw *hw);
 s32  igb_led_off(struct e1000_hw *hw);
 void igb_update_mc_addr_list(struct e1000_hw *hw,
-	                     u8 *mc_addr_list, u32 mc_addr_count);
+			     u8 *mc_addr_list, u32 mc_addr_count);
 s32  igb_setup_link(struct e1000_hw *hw);
 s32  igb_validate_mdi_setting(struct e1000_hw *hw);
 s32  igb_write_8bit_ctrl_reg(struct e1000_hw *hw, u32 reg,
-			       u32 offset, u8 data);
+			     u32 offset, u8 data);
 
 void igb_clear_hw_cntrs_base(struct e1000_hw *hw);
 void igb_clear_vfta(struct e1000_hw *hw);
@@ -80,12 +79,12 @@ enum e1000_mng_mode {
 	e1000_mng_mode_host_if_only
 };
 
-#define E1000_FACTPS_MNGCG    0x20000000
+#define E1000_FACTPS_MNGCG	0x20000000
 
-#define E1000_FWSM_MODE_MASK  0xE
-#define E1000_FWSM_MODE_SHIFT 1
+#define E1000_FWSM_MODE_MASK	0xE
+#define E1000_FWSM_MODE_SHIFT	1
 
-#define E1000_MNG_DHCP_COOKIE_STATUS_VLAN    0x2
+#define E1000_MNG_DHCP_COOKIE_STATUS_VLAN	0x2
 
 extern void e1000_init_function_pointers_82575(struct e1000_hw *hw);
 
diff --git a/drivers/net/ethernet/intel/igb/e1000_mbx.c b/drivers/net/ethernet/intel/igb/e1000_mbx.c
index 38e0df350904..dac1447fabf7 100644
--- a/drivers/net/ethernet/intel/igb/e1000_mbx.c
+++ b/drivers/net/ethernet/intel/igb/e1000_mbx.c
@@ -196,7 +196,8 @@ out:
  *  returns SUCCESS if it successfully received a message notification and
  *  copied it into the receive buffer.
  **/
-static s32 igb_read_posted_mbx(struct e1000_hw *hw, u32 *msg, u16 size, u16 mbx_id)
+static s32 igb_read_posted_mbx(struct e1000_hw *hw, u32 *msg, u16 size,
+			       u16 mbx_id)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
 	s32 ret_val = -E1000_ERR_MBX;
@@ -222,7 +223,8 @@ out:
  *  returns SUCCESS if it successfully copied message into the buffer and
  *  received an ack to that message within delay * timeout period
  **/
-static s32 igb_write_posted_mbx(struct e1000_hw *hw, u32 *msg, u16 size, u16 mbx_id)
+static s32 igb_write_posted_mbx(struct e1000_hw *hw, u32 *msg, u16 size,
+				u16 mbx_id)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
 	s32 ret_val = -E1000_ERR_MBX;
@@ -325,7 +327,6 @@ static s32 igb_obtain_mbx_lock_pf(struct e1000_hw *hw, u16 vf_number)
 	s32 ret_val = -E1000_ERR_MBX;
 	u32 p2v_mailbox;
 
-
 	/* Take ownership of the buffer */
 	wr32(E1000_P2VMAILBOX(vf_number), E1000_P2VMAILBOX_PFU);
 
@@ -347,7 +348,7 @@ static s32 igb_obtain_mbx_lock_pf(struct e1000_hw *hw, u16 vf_number)
  *  returns SUCCESS if it successfully copied message into the buffer
  **/
 static s32 igb_write_mbx_pf(struct e1000_hw *hw, u32 *msg, u16 size,
-                              u16 vf_number)
+			    u16 vf_number)
 {
 	s32 ret_val;
 	u16 i;
@@ -388,7 +389,7 @@ out_no_write:
  *  a message due to a VF request so no polling for message is needed.
  **/
 static s32 igb_read_mbx_pf(struct e1000_hw *hw, u32 *msg, u16 size,
-                             u16 vf_number)
+			   u16 vf_number)
 {
 	s32 ret_val;
 	u16 i;
diff --git a/drivers/net/ethernet/intel/igb/e1000_mbx.h b/drivers/net/ethernet/intel/igb/e1000_mbx.h
index c13b56d9edb2..de9bba41acf3 100644
--- a/drivers/net/ethernet/intel/igb/e1000_mbx.h
+++ b/drivers/net/ethernet/intel/igb/e1000_mbx.h
@@ -30,42 +30,42 @@
 
 #include "e1000_hw.h"
 
-#define E1000_P2VMAILBOX_STS   0x00000001 /* Initiate message send to VF */
-#define E1000_P2VMAILBOX_ACK   0x00000002 /* Ack message recv'd from VF */
-#define E1000_P2VMAILBOX_VFU   0x00000004 /* VF owns the mailbox buffer */
-#define E1000_P2VMAILBOX_PFU   0x00000008 /* PF owns the mailbox buffer */
-#define E1000_P2VMAILBOX_RVFU  0x00000010 /* Reset VFU - used when VF stuck */
+#define E1000_P2VMAILBOX_STS	0x00000001 /* Initiate message send to VF */
+#define E1000_P2VMAILBOX_ACK	0x00000002 /* Ack message recv'd from VF */
+#define E1000_P2VMAILBOX_VFU	0x00000004 /* VF owns the mailbox buffer */
+#define E1000_P2VMAILBOX_PFU	0x00000008 /* PF owns the mailbox buffer */
+#define E1000_P2VMAILBOX_RVFU	0x00000010 /* Reset VFU - used when VF stuck */
 
-#define E1000_MBVFICR_VFREQ_MASK 0x000000FF /* bits for VF messages */
-#define E1000_MBVFICR_VFREQ_VF1  0x00000001 /* bit for VF 1 message */
-#define E1000_MBVFICR_VFACK_MASK 0x00FF0000 /* bits for VF acks */
-#define E1000_MBVFICR_VFACK_VF1  0x00010000 /* bit for VF 1 ack */
+#define E1000_MBVFICR_VFREQ_MASK	0x000000FF /* bits for VF messages */
+#define E1000_MBVFICR_VFREQ_VF1		0x00000001 /* bit for VF 1 message */
+#define E1000_MBVFICR_VFACK_MASK	0x00FF0000 /* bits for VF acks */
+#define E1000_MBVFICR_VFACK_VF1		0x00010000 /* bit for VF 1 ack */
 
-#define E1000_VFMAILBOX_SIZE   16 /* 16 32 bit words - 64 bytes */
+#define E1000_VFMAILBOX_SIZE	16 /* 16 32 bit words - 64 bytes */
 
 /* If it's a E1000_VF_* msg then it originates in the VF and is sent to the
  * PF.  The reverse is true if it is E1000_PF_*.
  * Message ACK's are the value or'd with 0xF0000000
  */
-#define E1000_VT_MSGTYPE_ACK      0x80000000  /* Messages below or'd with
-                                               * this are the ACK */
-#define E1000_VT_MSGTYPE_NACK     0x40000000  /* Messages below or'd with
-                                               * this are the NACK */
-#define E1000_VT_MSGTYPE_CTS      0x20000000  /* Indicates that VF is still
-                                                 clear to send requests */
-#define E1000_VT_MSGINFO_SHIFT    16
+/* Messages below or'd with this are the ACK */
+#define E1000_VT_MSGTYPE_ACK	0x80000000
+/* Messages below or'd with this are the NACK */
+#define E1000_VT_MSGTYPE_NACK	0x40000000
+/* Indicates that VF is still clear to send requests */
+#define E1000_VT_MSGTYPE_CTS	0x20000000
+#define E1000_VT_MSGINFO_SHIFT	16
 /* bits 23:16 are used for exra info for certain messages */
-#define E1000_VT_MSGINFO_MASK     (0xFF << E1000_VT_MSGINFO_SHIFT)
+#define E1000_VT_MSGINFO_MASK	(0xFF << E1000_VT_MSGINFO_SHIFT)
 
-#define E1000_VF_RESET            0x01 /* VF requests reset */
-#define E1000_VF_SET_MAC_ADDR     0x02 /* VF requests to set MAC addr */
-#define E1000_VF_SET_MULTICAST    0x03 /* VF requests to set MC addr */
-#define E1000_VF_SET_VLAN         0x04 /* VF requests to set VLAN */
-#define E1000_VF_SET_LPE          0x05 /* VF requests to set VMOLR.LPE */
-#define E1000_VF_SET_PROMISC      0x06 /*VF requests to clear VMOLR.ROPE/MPME*/
-#define E1000_VF_SET_PROMISC_MULTICAST    (0x02 << E1000_VT_MSGINFO_SHIFT)
+#define E1000_VF_RESET		0x01 /* VF requests reset */
+#define E1000_VF_SET_MAC_ADDR	0x02 /* VF requests to set MAC addr */
+#define E1000_VF_SET_MULTICAST	0x03 /* VF requests to set MC addr */
+#define E1000_VF_SET_VLAN	0x04 /* VF requests to set VLAN */
+#define E1000_VF_SET_LPE	0x05 /* VF requests to set VMOLR.LPE */
+#define E1000_VF_SET_PROMISC	0x06 /*VF requests to clear VMOLR.ROPE/MPME*/
+#define E1000_VF_SET_PROMISC_MULTICAST	(0x02 << E1000_VT_MSGINFO_SHIFT)
 
-#define E1000_PF_CONTROL_MSG      0x0100 /* PF control message */
+#define E1000_PF_CONTROL_MSG	0x0100 /* PF control message */
 
 s32 igb_read_mbx(struct e1000_hw *, u32 *, u16, u16);
 s32 igb_write_mbx(struct e1000_hw *, u32 *, u16, u16);
diff --git a/drivers/net/ethernet/intel/igb/e1000_nvm.c b/drivers/net/ethernet/intel/igb/e1000_nvm.c
index 5b62adbe134d..7f9cd7cbd353 100644
--- a/drivers/net/ethernet/intel/igb/e1000_nvm.c
+++ b/drivers/net/ethernet/intel/igb/e1000_nvm.c
@@ -289,15 +289,14 @@ static s32 igb_ready_nvm_eeprom(struct e1000_hw *hw)
 		udelay(1);
 		timeout = NVM_MAX_RETRY_SPI;
 
-		/*
-		 * Read "Status Register" repeatedly until the LSB is cleared.
+		/* Read "Status Register" repeatedly until the LSB is cleared.
 		 * The EEPROM will signal that the command has been completed
 		 * by clearing bit 0 of the internal status register.  If it's
 		 * not cleared within 'timeout', then error out.
 		 */
 		while (timeout) {
 			igb_shift_out_eec_bits(hw, NVM_RDSR_OPCODE_SPI,
-						 hw->nvm.opcode_bits);
+					       hw->nvm.opcode_bits);
 			spi_stat_reg = (u8)igb_shift_in_eec_bits(hw, 8);
 			if (!(spi_stat_reg & NVM_STATUS_RDY_SPI))
 				break;
@@ -335,8 +334,7 @@ s32 igb_read_nvm_spi(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
 	u16 word_in;
 	u8 read_opcode = NVM_READ_OPCODE_SPI;
 
-	/*
-	 * A check for invalid values:  offset too large, too many words,
+	/* A check for invalid values:  offset too large, too many words,
 	 * and not enough words.
 	 */
 	if ((offset >= nvm->word_size) || (words > (nvm->word_size - offset)) ||
@@ -363,8 +361,7 @@ s32 igb_read_nvm_spi(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
 	igb_shift_out_eec_bits(hw, read_opcode, nvm->opcode_bits);
 	igb_shift_out_eec_bits(hw, (u16)(offset*2), nvm->address_bits);
 
-	/*
-	 * Read the data.  SPI NVMs increment the address with each byte
+	/* Read the data.  SPI NVMs increment the address with each byte
 	 * read and will roll over if reading beyond the end.  This allows
 	 * us to read the whole NVM from any offset
 	 */
@@ -395,8 +392,7 @@ s32 igb_read_nvm_eerd(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
 	u32 i, eerd = 0;
 	s32 ret_val = 0;
 
-	/*
-	 * A check for invalid values:  offset too large, too many words,
+	/* A check for invalid values:  offset too large, too many words,
 	 * and not enough words.
 	 */
 	if ((offset >= nvm->word_size) || (words > (nvm->word_size - offset)) ||
@@ -408,7 +404,7 @@ s32 igb_read_nvm_eerd(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
 
 	for (i = 0; i < words; i++) {
 		eerd = ((offset+i) << E1000_NVM_RW_ADDR_SHIFT) +
-		       E1000_NVM_RW_REG_START;
+			E1000_NVM_RW_REG_START;
 
 		wr32(E1000_EERD, eerd);
 		ret_val = igb_poll_eerd_eewr_done(hw, E1000_NVM_POLL_READ);
@@ -441,8 +437,7 @@ s32 igb_write_nvm_spi(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
 	s32 ret_val = -E1000_ERR_NVM;
 	u16 widx = 0;
 
-	/*
-	 * A check for invalid values:  offset too large, too many words,
+	/* A check for invalid values:  offset too large, too many words,
 	 * and not enough words.
 	 */
 	if ((offset >= nvm->word_size) || (words > (nvm->word_size - offset)) ||
@@ -472,8 +467,7 @@ s32 igb_write_nvm_spi(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
 
 		igb_standby_nvm(hw);
 
-		/*
-		 * Some SPI eeproms use the 8th address bit embedded in the
+		/* Some SPI eeproms use the 8th address bit embedded in the
 		 * opcode
 		 */
 		if ((nvm->address_bits == 8) && (offset >= 128))
@@ -538,8 +532,7 @@ s32 igb_read_part_string(struct e1000_hw *hw, u8 *part_num, u32 part_num_size)
 		goto out;
 	}
 
-	/*
-	 * if nvm_data is not ptr guard the PBA must be in legacy format which
+	/* if nvm_data is not ptr guard the PBA must be in legacy format which
 	 * means pointer is actually our second data word for the PBA number
 	 * and we can decode it into an ascii string
 	 */
@@ -728,6 +721,7 @@ void igb_get_fw_version(struct e1000_hw *hw, struct e1000_fw_version *fw_vers)
 	case e1000_82575:
 	case e1000_82576:
 	case e1000_82580:
+	case e1000_i354:
 	case e1000_i350:
 	case e1000_i210:
 		break;
@@ -746,6 +740,7 @@ void igb_get_fw_version(struct e1000_hw *hw, struct e1000_fw_version *fw_vers)
 
 	switch (hw->mac.type) {
 	case e1000_i210:
+	case e1000_i354:
 	case e1000_i350:
 		/* find combo image version */
 		hw->nvm.ops.read(hw, NVM_COMB_VER_PTR, 1, &comb_offset);
diff --git a/drivers/net/ethernet/intel/igb/e1000_phy.c b/drivers/net/ethernet/intel/igb/e1000_phy.c
index 2918c979b5bb..115b0da6e013 100644
--- a/drivers/net/ethernet/intel/igb/e1000_phy.c
+++ b/drivers/net/ethernet/intel/igb/e1000_phy.c
@@ -33,29 +33,29 @@
 
 static s32  igb_phy_setup_autoneg(struct e1000_hw *hw);
 static void igb_phy_force_speed_duplex_setup(struct e1000_hw *hw,
-					       u16 *phy_ctrl);
+					     u16 *phy_ctrl);
 static s32  igb_wait_autoneg(struct e1000_hw *hw);
 static s32  igb_set_master_slave_mode(struct e1000_hw *hw);
 
 /* Cable length tables */
-static const u16 e1000_m88_cable_length_table[] =
-	{ 0, 50, 80, 110, 140, 140, E1000_CABLE_LENGTH_UNDEFINED };
+static const u16 e1000_m88_cable_length_table[] = {
+	0, 50, 80, 110, 140, 140, E1000_CABLE_LENGTH_UNDEFINED };
 #define M88E1000_CABLE_LENGTH_TABLE_SIZE \
-                (sizeof(e1000_m88_cable_length_table) / \
-                 sizeof(e1000_m88_cable_length_table[0]))
-
-static const u16 e1000_igp_2_cable_length_table[] =
-    { 0, 0, 0, 0, 0, 0, 0, 0, 3, 5, 8, 11, 13, 16, 18, 21,
-      0, 0, 0, 3, 6, 10, 13, 16, 19, 23, 26, 29, 32, 35, 38, 41,
-      6, 10, 14, 18, 22, 26, 30, 33, 37, 41, 44, 48, 51, 54, 58, 61,
-      21, 26, 31, 35, 40, 44, 49, 53, 57, 61, 65, 68, 72, 75, 79, 82,
-      40, 45, 51, 56, 61, 66, 70, 75, 79, 83, 87, 91, 94, 98, 101, 104,
-      60, 66, 72, 77, 82, 87, 92, 96, 100, 104, 108, 111, 114, 117, 119, 121,
-      83, 89, 95, 100, 105, 109, 113, 116, 119, 122, 124,
-      104, 109, 114, 118, 121, 124};
+	(sizeof(e1000_m88_cable_length_table) / \
+	sizeof(e1000_m88_cable_length_table[0]))
+
+static const u16 e1000_igp_2_cable_length_table[] = {
+	0, 0, 0, 0, 0, 0, 0, 0, 3, 5, 8, 11, 13, 16, 18, 21,
+	0, 0, 0, 3, 6, 10, 13, 16, 19, 23, 26, 29, 32, 35, 38, 41,
+	6, 10, 14, 18, 22, 26, 30, 33, 37, 41, 44, 48, 51, 54, 58, 61,
+	21, 26, 31, 35, 40, 44, 49, 53, 57, 61, 65, 68, 72, 75, 79, 82,
+	40, 45, 51, 56, 61, 66, 70, 75, 79, 83, 87, 91, 94, 98, 101, 104,
+	60, 66, 72, 77, 82, 87, 92, 96, 100, 104, 108, 111, 114, 117, 119, 121,
+	83, 89, 95, 100, 105, 109, 113, 116, 119, 122, 124,
+	104, 109, 114, 118, 121, 124};
 #define IGP02E1000_CABLE_LENGTH_TABLE_SIZE \
-		(sizeof(e1000_igp_2_cable_length_table) / \
-		 sizeof(e1000_igp_2_cable_length_table[0]))
+	(sizeof(e1000_igp_2_cable_length_table) / \
+	 sizeof(e1000_igp_2_cable_length_table[0]))
 
 /**
  *  igb_check_reset_block - Check if PHY reset is blocked
@@ -71,8 +71,7 @@ s32 igb_check_reset_block(struct e1000_hw *hw)
 
 	manc = rd32(E1000_MANC);
 
-	return (manc & E1000_MANC_BLK_PHY_RST_ON_IDE) ?
-	       E1000_BLK_PHY_RESET : 0;
+	return (manc & E1000_MANC_BLK_PHY_RST_ON_IDE) ? E1000_BLK_PHY_RESET : 0;
 }
 
 /**
@@ -149,8 +148,7 @@ s32 igb_read_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 *data)
 		goto out;
 	}
 
-	/*
-	 * Set up Op-code, Phy Address, and register offset in the MDI
+	/* Set up Op-code, Phy Address, and register offset in the MDI
 	 * Control register.  The MAC will take care of interfacing with the
 	 * PHY to retrieve the desired data.
 	 */
@@ -160,8 +158,7 @@ s32 igb_read_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 *data)
 
 	wr32(E1000_MDIC, mdic);
 
-	/*
-	 * Poll the ready bit to see if the MDI read completed
+	/* Poll the ready bit to see if the MDI read completed
 	 * Increasing the time out as testing showed failures with
 	 * the lower time out
 	 */
@@ -207,8 +204,7 @@ s32 igb_write_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 data)
 		goto out;
 	}
 
-	/*
-	 * Set up Op-code, Phy Address, and register offset in the MDI
+	/* Set up Op-code, Phy Address, and register offset in the MDI
 	 * Control register.  The MAC will take care of interfacing with the
 	 * PHY to retrieve the desired data.
 	 */
@@ -219,8 +215,7 @@ s32 igb_write_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 data)
 
 	wr32(E1000_MDIC, mdic);
 
-	/*
-	 * Poll the ready bit to see if the MDI read completed
+	/* Poll the ready bit to see if the MDI read completed
 	 * Increasing the time out as testing showed failures with
 	 * the lower time out
 	 */
@@ -259,15 +254,13 @@ s32 igb_read_phy_reg_i2c(struct e1000_hw *hw, u32 offset, u16 *data)
 	struct e1000_phy_info *phy = &hw->phy;
 	u32 i, i2ccmd = 0;
 
-
-	/*
-	 * Set up Op-code, Phy Address, and register address in the I2CCMD
+	/* Set up Op-code, Phy Address, and register address in the I2CCMD
 	 * register.  The MAC will take care of interfacing with the
 	 * PHY to retrieve the desired data.
 	 */
 	i2ccmd = ((offset << E1000_I2CCMD_REG_ADDR_SHIFT) |
-	          (phy->addr << E1000_I2CCMD_PHY_ADDR_SHIFT) |
-	          (E1000_I2CCMD_OPCODE_READ));
+		  (phy->addr << E1000_I2CCMD_PHY_ADDR_SHIFT) |
+		  (E1000_I2CCMD_OPCODE_READ));
 
 	wr32(E1000_I2CCMD, i2ccmd);
 
@@ -317,15 +310,14 @@ s32 igb_write_phy_reg_i2c(struct e1000_hw *hw, u32 offset, u16 data)
 	/* Swap the data bytes for the I2C interface */
 	phy_data_swapped = ((data >> 8) & 0x00FF) | ((data << 8) & 0xFF00);
 
-	/*
-	 * Set up Op-code, Phy Address, and register address in the I2CCMD
+	/* Set up Op-code, Phy Address, and register address in the I2CCMD
 	 * register.  The MAC will take care of interfacing with the
 	 * PHY to retrieve the desired data.
 	 */
 	i2ccmd = ((offset << E1000_I2CCMD_REG_ADDR_SHIFT) |
-	          (phy->addr << E1000_I2CCMD_PHY_ADDR_SHIFT) |
-	          E1000_I2CCMD_OPCODE_WRITE |
-	          phy_data_swapped);
+		  (phy->addr << E1000_I2CCMD_PHY_ADDR_SHIFT) |
+		  E1000_I2CCMD_OPCODE_WRITE |
+		  phy_data_swapped);
 
 	wr32(E1000_I2CCMD, i2ccmd);
 
@@ -371,8 +363,8 @@ s32 igb_read_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 *data)
 
 	if (offset > MAX_PHY_MULTI_PAGE_REG) {
 		ret_val = igb_write_phy_reg_mdic(hw,
-						   IGP01E1000_PHY_PAGE_SELECT,
-						   (u16)offset);
+						 IGP01E1000_PHY_PAGE_SELECT,
+						 (u16)offset);
 		if (ret_val) {
 			hw->phy.ops.release(hw);
 			goto out;
@@ -410,8 +402,8 @@ s32 igb_write_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 data)
 
 	if (offset > MAX_PHY_MULTI_PAGE_REG) {
 		ret_val = igb_write_phy_reg_mdic(hw,
-						   IGP01E1000_PHY_PAGE_SELECT,
-						   (u16)offset);
+						 IGP01E1000_PHY_PAGE_SELECT,
+						 (u16)offset);
 		if (ret_val) {
 			hw->phy.ops.release(hw);
 			goto out;
@@ -419,7 +411,7 @@ s32 igb_write_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 data)
 	}
 
 	ret_val = igb_write_phy_reg_mdic(hw, MAX_PHY_REG_ADDRESS & offset,
-					   data);
+					 data);
 
 	hw->phy.ops.release(hw);
 
@@ -439,7 +431,6 @@ s32 igb_copper_link_setup_82580(struct e1000_hw *hw)
 	s32 ret_val;
 	u16 phy_data;
 
-
 	if (phy->reset_disable) {
 		ret_val = 0;
 		goto out;
@@ -472,8 +463,7 @@ s32 igb_copper_link_setup_82580(struct e1000_hw *hw)
 	if (ret_val)
 		goto out;
 	phy_data &= ~I82580_PHY_CTRL2_MDIX_CFG_MASK;
-	/*
-	 * Options:
+	/* Options:
 	 *   0 - Auto (default)
 	 *   1 - MDI mode
 	 *   2 - MDI-X mode
@@ -520,8 +510,7 @@ s32 igb_copper_link_setup_m88(struct e1000_hw *hw)
 
 	phy_data |= M88E1000_PSCR_ASSERT_CRS_ON_TX;
 
-	/*
-	 * Options:
+	/* Options:
 	 *   MDI/MDI-X = 0 (default)
 	 *   0 - Auto for all speeds
 	 *   1 - MDI mode
@@ -546,8 +535,7 @@ s32 igb_copper_link_setup_m88(struct e1000_hw *hw)
 		break;
 	}
 
-	/*
-	 * Options:
+	/* Options:
 	 *   disable_polarity_correction = 0 (default)
 	 *       Automatic Correction for Reversed Cable Polarity
 	 *   0 - Disabled
@@ -562,12 +550,11 @@ s32 igb_copper_link_setup_m88(struct e1000_hw *hw)
 		goto out;
 
 	if (phy->revision < E1000_REVISION_4) {
-		/*
-		 * Force TX_CLK in the Extended PHY Specific Control Register
+		/* Force TX_CLK in the Extended PHY Specific Control Register
 		 * to 25MHz clock.
 		 */
 		ret_val = phy->ops.read_reg(hw, M88E1000_EXT_PHY_SPEC_CTRL,
-					     &phy_data);
+					    &phy_data);
 		if (ret_val)
 			goto out;
 
@@ -630,8 +617,7 @@ s32 igb_copper_link_setup_m88_gen2(struct e1000_hw *hw)
 	if (ret_val)
 		goto out;
 
-	/*
-	 * Options:
+	/* Options:
 	 *   MDI/MDI-X = 0 (default)
 	 *   0 - Auto for all speeds
 	 *   1 - MDI mode
@@ -659,8 +645,7 @@ s32 igb_copper_link_setup_m88_gen2(struct e1000_hw *hw)
 		break;
 	}
 
-	/*
-	 * Options:
+	/* Options:
 	 *   disable_polarity_correction = 0 (default)
 	 *       Automatic Correction for Reversed Cable Polarity
 	 *   0 - Disabled
@@ -714,14 +699,12 @@ s32 igb_copper_link_setup_igp(struct e1000_hw *hw)
 		goto out;
 	}
 
-	/*
-	 * Wait 100ms for MAC to configure PHY from NVM settings, to avoid
+	/* Wait 100ms for MAC to configure PHY from NVM settings, to avoid
 	 * timeout issues when LFS is enabled.
 	 */
 	msleep(100);
 
-	/*
-	 * The NVM settings will configure LPLU in D3 for
+	/* The NVM settings will configure LPLU in D3 for
 	 * non-IGP1 PHYs.
 	 */
 	if (phy->type == e1000_phy_igp) {
@@ -765,8 +748,7 @@ s32 igb_copper_link_setup_igp(struct e1000_hw *hw)
 
 	/* set auto-master slave resolution settings */
 	if (hw->mac.autoneg) {
-		/*
-		 * when autonegotiation advertisement is only 1000Mbps then we
+		/* when autonegotiation advertisement is only 1000Mbps then we
 		 * should disable SmartSpeed and enable Auto MasterSlave
 		 * resolution as hardware default.
 		 */
@@ -844,14 +826,12 @@ static s32 igb_copper_link_autoneg(struct e1000_hw *hw)
 	s32 ret_val;
 	u16 phy_ctrl;
 
-	/*
-	 * Perform some bounds checking on the autoneg advertisement
+	/* Perform some bounds checking on the autoneg advertisement
 	 * parameter.
 	 */
 	phy->autoneg_advertised &= phy->autoneg_mask;
 
-	/*
-	 * If autoneg_advertised is zero, we assume it was not defaulted
+	/* If autoneg_advertised is zero, we assume it was not defaulted
 	 * by the calling code so we set to advertise full capability.
 	 */
 	if (phy->autoneg_advertised == 0)
@@ -865,8 +845,7 @@ static s32 igb_copper_link_autoneg(struct e1000_hw *hw)
 	}
 	hw_dbg("Restarting Auto-Neg\n");
 
-	/*
-	 * Restart auto-negotiation by setting the Auto Neg Enable bit and
+	/* Restart auto-negotiation by setting the Auto Neg Enable bit and
 	 * the Auto Neg Restart bit in the PHY control register.
 	 */
 	ret_val = phy->ops.read_reg(hw, PHY_CONTROL, &phy_ctrl);
@@ -878,8 +857,7 @@ static s32 igb_copper_link_autoneg(struct e1000_hw *hw)
 	if (ret_val)
 		goto out;
 
-	/*
-	 * Does the user want to wait for Auto-Neg to complete here, or
+	/* Does the user want to wait for Auto-Neg to complete here, or
 	 * check at a later time (for example, callback routine).
 	 */
 	if (phy->autoneg_wait_to_complete) {
@@ -928,16 +906,14 @@ static s32 igb_phy_setup_autoneg(struct e1000_hw *hw)
 			goto out;
 	}
 
-	/*
-	 * Need to parse both autoneg_advertised and fc and set up
+	/* Need to parse both autoneg_advertised and fc and set up
 	 * the appropriate PHY registers.  First we will parse for
 	 * autoneg_advertised software override.  Since we can advertise
 	 * a plethora of combinations, we need to check each bit
 	 * individually.
 	 */
 
-	/*
-	 * First we clear all the 10/100 mb speed bits in the Auto-Neg
+	/* First we clear all the 10/100 mb speed bits in the Auto-Neg
 	 * Advertisement Register (Address 4) and the 1000 mb speed bits in
 	 * the  1000Base-T Control Register (Address 9).
 	 */
@@ -983,8 +959,7 @@ static s32 igb_phy_setup_autoneg(struct e1000_hw *hw)
 		mii_1000t_ctrl_reg |= CR_1000T_FD_CAPS;
 	}
 
-	/*
-	 * Check for a software override of the flow control settings, and
+	/* Check for a software override of the flow control settings, and
 	 * setup the PHY advertisement registers accordingly.  If
 	 * auto-negotiation is enabled, then software will have to set the
 	 * "PAUSE" bits to the correct value in the Auto-Negotiation
@@ -1003,15 +978,13 @@ static s32 igb_phy_setup_autoneg(struct e1000_hw *hw)
 	 */
 	switch (hw->fc.current_mode) {
 	case e1000_fc_none:
-		/*
-		 * Flow control (RX & TX) is completely disabled by a
+		/* Flow control (RX & TX) is completely disabled by a
 		 * software over-ride.
 		 */
 		mii_autoneg_adv_reg &= ~(NWAY_AR_ASM_DIR | NWAY_AR_PAUSE);
 		break;
 	case e1000_fc_rx_pause:
-		/*
-		 * RX Flow control is enabled, and TX Flow control is
+		/* RX Flow control is enabled, and TX Flow control is
 		 * disabled, by a software over-ride.
 		 *
 		 * Since there really isn't a way to advertise that we are
@@ -1023,16 +996,14 @@ static s32 igb_phy_setup_autoneg(struct e1000_hw *hw)
 		mii_autoneg_adv_reg |= (NWAY_AR_ASM_DIR | NWAY_AR_PAUSE);
 		break;
 	case e1000_fc_tx_pause:
-		/*
-		 * TX Flow control is enabled, and RX Flow control is
+		/* TX Flow control is enabled, and RX Flow control is
 		 * disabled, by a software over-ride.
 		 */
 		mii_autoneg_adv_reg |= NWAY_AR_ASM_DIR;
 		mii_autoneg_adv_reg &= ~NWAY_AR_PAUSE;
 		break;
 	case e1000_fc_full:
-		/*
-		 * Flow control (both RX and TX) is enabled by a software
+		/* Flow control (both RX and TX) is enabled by a software
 		 * over-ride.
 		 */
 		mii_autoneg_adv_reg |= (NWAY_AR_ASM_DIR | NWAY_AR_PAUSE);
@@ -1075,18 +1046,15 @@ s32 igb_setup_copper_link(struct e1000_hw *hw)
 	s32 ret_val;
 	bool link;
 
-
 	if (hw->mac.autoneg) {
-		/*
-		 * Setup autoneg and flow control advertisement and perform
+		/* Setup autoneg and flow control advertisement and perform
 		 * autonegotiation.
 		 */
 		ret_val = igb_copper_link_autoneg(hw);
 		if (ret_val)
 			goto out;
 	} else {
-		/*
-		 * PHY will be set to 10H, 10F, 100H or 100F
+		/* PHY will be set to 10H, 10F, 100H or 100F
 		 * depending on user settings.
 		 */
 		hw_dbg("Forcing Speed and Duplex\n");
@@ -1097,14 +1065,10 @@ s32 igb_setup_copper_link(struct e1000_hw *hw)
 		}
 	}
 
-	/*
-	 * Check link status. Wait up to 100 microseconds for link to become
+	/* Check link status. Wait up to 100 microseconds for link to become
 	 * valid.
 	 */
-	ret_val = igb_phy_has_link(hw,
-	                           COPPER_LINK_UP_LIMIT,
-	                           10,
-	                           &link);
+	ret_val = igb_phy_has_link(hw, COPPER_LINK_UP_LIMIT, 10, &link);
 	if (ret_val)
 		goto out;
 
@@ -1145,8 +1109,7 @@ s32 igb_phy_force_speed_duplex_igp(struct e1000_hw *hw)
 	if (ret_val)
 		goto out;
 
-	/*
-	 * Clear Auto-Crossover to force MDI manually.  IGP requires MDI
+	/* Clear Auto-Crossover to force MDI manually.  IGP requires MDI
 	 * forced whenever speed and duplex are forced.
 	 */
 	ret_val = phy->ops.read_reg(hw, IGP01E1000_PHY_PORT_CTRL, &phy_data);
@@ -1167,10 +1130,7 @@ s32 igb_phy_force_speed_duplex_igp(struct e1000_hw *hw)
 	if (phy->autoneg_wait_to_complete) {
 		hw_dbg("Waiting for forced speed/duplex link on IGP phy.\n");
 
-		ret_val = igb_phy_has_link(hw,
-						     PHY_FORCE_LIMIT,
-						     100000,
-						     &link);
+		ret_val = igb_phy_has_link(hw, PHY_FORCE_LIMIT, 10000, &link);
 		if (ret_val)
 			goto out;
 
@@ -1178,10 +1138,7 @@ s32 igb_phy_force_speed_duplex_igp(struct e1000_hw *hw)
 			hw_dbg("Link taking longer than expected.\n");
 
 		/* Try once more */
-		ret_val = igb_phy_has_link(hw,
-						     PHY_FORCE_LIMIT,
-						     100000,
-						     &link);
+		ret_val = igb_phy_has_link(hw, PHY_FORCE_LIMIT, 10000, &link);
 		if (ret_val)
 			goto out;
 	}
@@ -1209,8 +1166,7 @@ s32 igb_phy_force_speed_duplex_m88(struct e1000_hw *hw)
 
 	/* I210 and I211 devices support Auto-Crossover in forced operation. */
 	if (phy->type != e1000_phy_i210) {
-		/*
-		 * Clear Auto-Crossover to force MDI manually.  M88E1000
+		/* Clear Auto-Crossover to force MDI manually.  M88E1000
 		 * requires MDI forced whenever speed and duplex are forced.
 		 */
 		ret_val = phy->ops.read_reg(hw, M88E1000_PHY_SPEC_CTRL,
@@ -1266,13 +1222,12 @@ s32 igb_phy_force_speed_duplex_m88(struct e1000_hw *hw)
 			if (!reset_dsp)
 				hw_dbg("Link taking longer than expected.\n");
 			else {
-				/*
-				 * We didn't get link.
+				/* We didn't get link.
 				 * Reset the DSP and cross our fingers.
 				 */
 				ret_val = phy->ops.write_reg(hw,
-							     M88E1000_PHY_PAGE_SELECT,
-							     0x001d);
+						M88E1000_PHY_PAGE_SELECT,
+						0x001d);
 				if (ret_val)
 					goto out;
 				ret_val = igb_phy_reset_dsp(hw);
@@ -1298,8 +1253,7 @@ s32 igb_phy_force_speed_duplex_m88(struct e1000_hw *hw)
 	if (ret_val)
 		goto out;
 
-	/*
-	 * Resetting the phy means we need to re-force TX_CLK in the
+	/* Resetting the phy means we need to re-force TX_CLK in the
 	 * Extended PHY Specific Control Register to 25MHz clock from
 	 * the reset value of 2.5MHz.
 	 */
@@ -1308,8 +1262,7 @@ s32 igb_phy_force_speed_duplex_m88(struct e1000_hw *hw)
 	if (ret_val)
 		goto out;
 
-	/*
-	 * In addition, we must re-enable CRS on Tx for both half and full
+	/* In addition, we must re-enable CRS on Tx for both half and full
 	 * duplex.
 	 */
 	ret_val = phy->ops.read_reg(hw, M88E1000_PHY_SPEC_CTRL, &phy_data);
@@ -1336,7 +1289,7 @@ out:
  *  take affect.
  **/
 static void igb_phy_force_speed_duplex_setup(struct e1000_hw *hw,
-					       u16 *phy_ctrl)
+					     u16 *phy_ctrl)
 {
 	struct e1000_mac_info *mac = &hw->mac;
 	u32 ctrl;
@@ -1417,8 +1370,7 @@ s32 igb_set_d3_lplu_state(struct e1000_hw *hw, bool active)
 					     data);
 		if (ret_val)
 			goto out;
-		/*
-		 * LPLU and SmartSpeed are mutually exclusive.  LPLU is used
+		/* LPLU and SmartSpeed are mutually exclusive.  LPLU is used
 		 * during Dx states where the power conservation is most
 		 * important.  During driver activity we should enable
 		 * SmartSpeed, so performance is maintained.
@@ -1461,13 +1413,13 @@ s32 igb_set_d3_lplu_state(struct e1000_hw *hw, bool active)
 
 		/* When LPLU is enabled, we should disable SmartSpeed */
 		ret_val = phy->ops.read_reg(hw, IGP01E1000_PHY_PORT_CONFIG,
-					     &data);
+					    &data);
 		if (ret_val)
 			goto out;
 
 		data &= ~IGP01E1000_PSCFR_SMART_SPEED;
 		ret_val = phy->ops.write_reg(hw, IGP01E1000_PHY_PORT_CONFIG,
-					      data);
+					     data);
 	}
 
 out:
@@ -1556,8 +1508,7 @@ static s32 igb_check_polarity_igp(struct e1000_hw *hw)
 	s32 ret_val;
 	u16 data, offset, mask;
 
-	/*
-	 * Polarity is determined based on the speed of
+	/* Polarity is determined based on the speed of
 	 * our connection.
 	 */
 	ret_val = phy->ops.read_reg(hw, IGP01E1000_PHY_PORT_STATUS, &data);
@@ -1569,8 +1520,7 @@ static s32 igb_check_polarity_igp(struct e1000_hw *hw)
 		offset	= IGP01E1000_PHY_PCS_INIT_REG;
 		mask	= IGP01E1000_PHY_POLARITY_MASK;
 	} else {
-		/*
-		 * This really only applies to 10Mbps since
+		/* This really only applies to 10Mbps since
 		 * there is no polarity for 100Mbps (always 0).
 		 */
 		offset	= IGP01E1000_PHY_PORT_STATUS;
@@ -1589,7 +1539,7 @@ out:
 }
 
 /**
- *  igb_wait_autoneg - Wait for auto-neg compeletion
+ *  igb_wait_autoneg - Wait for auto-neg completion
  *  @hw: pointer to the HW structure
  *
  *  Waits for auto-negotiation to complete or for the auto-negotiation time
@@ -1613,8 +1563,7 @@ static s32 igb_wait_autoneg(struct e1000_hw *hw)
 		msleep(100);
 	}
 
-	/*
-	 * PHY_AUTO_NEG_TIME expiration doesn't guarantee auto-negotiation
+	/* PHY_AUTO_NEG_TIME expiration doesn't guarantee auto-negotiation
 	 * has completed.
 	 */
 	return ret_val;
@@ -1630,21 +1579,19 @@ static s32 igb_wait_autoneg(struct e1000_hw *hw)
  *  Polls the PHY status register for link, 'iterations' number of times.
  **/
 s32 igb_phy_has_link(struct e1000_hw *hw, u32 iterations,
-			       u32 usec_interval, bool *success)
+		     u32 usec_interval, bool *success)
 {
 	s32 ret_val = 0;
 	u16 i, phy_status;
 
 	for (i = 0; i < iterations; i++) {
-		/*
-		 * Some PHYs require the PHY_STATUS register to be read
+		/* Some PHYs require the PHY_STATUS register to be read
 		 * twice due to the link bit being sticky.  No harm doing
 		 * it across the board.
 		 */
 		ret_val = hw->phy.ops.read_reg(hw, PHY_STATUS, &phy_status);
-		if (ret_val) {
-			/*
-			 * If the first read fails, another entity may have
+		if (ret_val && usec_interval > 0) {
+			/* If the first read fails, another entity may have
 			 * ownership of the resources, wait and try again to
 			 * see if they have relinquished the resources yet.
 			 */
@@ -1735,6 +1682,7 @@ s32 igb_get_cable_length_m88_gen2(struct e1000_hw *hw)
 		phy->max_cable_length = phy_data / (is_cm ? 100 : 1);
 		phy->cable_length = phy_data / (is_cm ? 100 : 1);
 		break;
+	case M88E1545_E_PHY_ID:
 	case I347AT4_E_PHY_ID:
 		/* Remember the original page select and set it to 7 */
 		ret_val = phy->ops.read_reg(hw, I347AT4_PAGE_SELECT,
@@ -1834,10 +1782,10 @@ s32 igb_get_cable_length_igp_2(struct e1000_hw *hw)
 	u16 cur_agc_index, max_agc_index = 0;
 	u16 min_agc_index = IGP02E1000_CABLE_LENGTH_TABLE_SIZE - 1;
 	static const u16 agc_reg_array[IGP02E1000_PHY_CHANNEL_NUM] = {
-	       IGP02E1000_PHY_AGC_A,
-	       IGP02E1000_PHY_AGC_B,
-	       IGP02E1000_PHY_AGC_C,
-	       IGP02E1000_PHY_AGC_D
+		IGP02E1000_PHY_AGC_A,
+		IGP02E1000_PHY_AGC_B,
+		IGP02E1000_PHY_AGC_C,
+		IGP02E1000_PHY_AGC_D
 	};
 
 	/* Read the AGC registers for all channels */
@@ -1846,8 +1794,7 @@ s32 igb_get_cable_length_igp_2(struct e1000_hw *hw)
 		if (ret_val)
 			goto out;
 
-		/*
-		 * Getting bits 15:9, which represent the combination of
+		/* Getting bits 15:9, which represent the combination of
 		 * coarse and fine gain values.  The result is a number
 		 * that can be put into the lookup table to obtain the
 		 * approximate cable length.
@@ -2167,15 +2114,13 @@ s32 igb_phy_init_script_igp3(struct e1000_hw *hw)
 	hw->phy.ops.write_reg(hw, 0x1796, 0x0008);
 	/* Change cg_icount + enable integbp for channels BCD */
 	hw->phy.ops.write_reg(hw, 0x1798, 0xD008);
-	/*
-	 * Change cg_icount + enable integbp + change prop_factor_master
+	/* Change cg_icount + enable integbp + change prop_factor_master
 	 * to 8 for channel A
 	 */
 	hw->phy.ops.write_reg(hw, 0x1898, 0xD918);
 	/* Disable AHT in Slave mode on channel A */
 	hw->phy.ops.write_reg(hw, 0x187A, 0x0800);
-	/*
-	 * Enable LPLU and disable AN to 1000 in non-D0a states,
+	/* Enable LPLU and disable AN to 1000 in non-D0a states,
 	 * Enable SPD+B2B
 	 */
 	hw->phy.ops.write_reg(hw, 0x0019, 0x008D);
@@ -2257,8 +2202,8 @@ static s32 igb_check_polarity_82580(struct e1000_hw *hw)
 
 	if (!ret_val)
 		phy->cable_polarity = (data & I82580_PHY_STATUS2_REV_POLARITY)
-		                      ? e1000_rev_polarity_reversed
-		                      : e1000_rev_polarity_normal;
+				      ? e1000_rev_polarity_reversed
+				      : e1000_rev_polarity_normal;
 
 	return ret_val;
 }
@@ -2278,7 +2223,6 @@ s32 igb_phy_force_speed_duplex_82580(struct e1000_hw *hw)
 	u16 phy_data;
 	bool link;
 
-
 	ret_val = phy->ops.read_reg(hw, PHY_CONTROL, &phy_data);
 	if (ret_val)
 		goto out;
@@ -2289,8 +2233,7 @@ s32 igb_phy_force_speed_duplex_82580(struct e1000_hw *hw)
 	if (ret_val)
 		goto out;
 
-	/*
-	 * Clear Auto-Crossover to force MDI manually.  82580 requires MDI
+	/* Clear Auto-Crossover to force MDI manually.  82580 requires MDI
 	 * forced whenever speed and duplex are forced.
 	 */
 	ret_val = phy->ops.read_reg(hw, I82580_PHY_CTRL_2, &phy_data);
@@ -2310,10 +2253,7 @@ s32 igb_phy_force_speed_duplex_82580(struct e1000_hw *hw)
 	if (phy->autoneg_wait_to_complete) {
 		hw_dbg("Waiting for forced speed/duplex link on 82580 phy\n");
 
-		ret_val = igb_phy_has_link(hw,
-		                           PHY_FORCE_LIMIT,
-		                           100000,
-		                           &link);
+		ret_val = igb_phy_has_link(hw, PHY_FORCE_LIMIT, 100000, &link);
 		if (ret_val)
 			goto out;
 
@@ -2321,10 +2261,7 @@ s32 igb_phy_force_speed_duplex_82580(struct e1000_hw *hw)
 			hw_dbg("Link taking longer than expected.\n");
 
 		/* Try once more */
-		ret_val = igb_phy_has_link(hw,
-		                           PHY_FORCE_LIMIT,
-		                           100000,
-		                           &link);
+		ret_val = igb_phy_has_link(hw, PHY_FORCE_LIMIT, 100000, &link);
 		if (ret_val)
 			goto out;
 	}
@@ -2349,7 +2286,6 @@ s32 igb_get_phy_info_82580(struct e1000_hw *hw)
 	u16 data;
 	bool link;
 
-
 	ret_val = igb_phy_has_link(hw, 1, 0, &link);
 	if (ret_val)
 		goto out;
@@ -2383,12 +2319,12 @@ s32 igb_get_phy_info_82580(struct e1000_hw *hw)
 			goto out;
 
 		phy->local_rx = (data & SR_1000T_LOCAL_RX_STATUS)
-		                ? e1000_1000t_rx_status_ok
-		                : e1000_1000t_rx_status_not_ok;
+				? e1000_1000t_rx_status_ok
+				: e1000_1000t_rx_status_not_ok;
 
 		phy->remote_rx = (data & SR_1000T_REMOTE_RX_STATUS)
-		                 ? e1000_1000t_rx_status_ok
-		                 : e1000_1000t_rx_status_not_ok;
+				 ? e1000_1000t_rx_status_ok
+				 : e1000_1000t_rx_status_not_ok;
 	} else {
 		phy->cable_length = E1000_CABLE_LENGTH_UNDEFINED;
 		phy->local_rx = e1000_1000t_rx_status_undefined;
@@ -2412,13 +2348,12 @@ s32 igb_get_cable_length_82580(struct e1000_hw *hw)
 	s32 ret_val;
 	u16 phy_data, length;
 
-
 	ret_val = phy->ops.read_reg(hw, I82580_PHY_DIAG_STATUS, &phy_data);
 	if (ret_val)
 		goto out;
 
 	length = (phy_data & I82580_DSTATUS_CABLE_LENGTH) >>
-	         I82580_DSTATUS_CABLE_LENGTH_SHIFT;
+		 I82580_DSTATUS_CABLE_LENGTH_SHIFT;
 
 	if (length == E1000_CABLE_LENGTH_UNDEFINED)
 		ret_val = -E1000_ERR_PHY;
diff --git a/drivers/net/ethernet/intel/igb/e1000_regs.h b/drivers/net/ethernet/intel/igb/e1000_regs.h
index 15343286082e..82632c6c53af 100644
--- a/drivers/net/ethernet/intel/igb/e1000_regs.h
+++ b/drivers/net/ethernet/intel/igb/e1000_regs.h
@@ -65,6 +65,7 @@
 #define E1000_TIPG     0x00410  /* TX Inter-packet gap -RW */
 #define E1000_AIT      0x00458  /* Adaptive Interframe Spacing Throttle - RW */
 #define E1000_LEDCTL   0x00E00  /* LED Control - RW */
+#define E1000_LEDMUX   0x08130  /* LED MUX Control */
 #define E1000_PBA      0x01000  /* Packet Buffer Allocation - RW */
 #define E1000_PBS      0x01008  /* Packet Buffer Size */
 #define E1000_EEMNGCTL 0x01010  /* MNG EEprom Control */
@@ -83,6 +84,9 @@
 #define E1000_I2C_DATA_IN   0x00001000  /* I2C- Data In */
 #define E1000_I2C_CLK_OE_N  0x00002000  /* I2C- Clock Output Enable */
 #define E1000_I2C_CLK_IN    0x00004000  /* I2C- Clock In */
+#define E1000_MPHY_ADDR_CTRL	0x0024 /* GbE MPHY Address Control */
+#define E1000_MPHY_DATA		0x0E10 /* GBE MPHY Data */
+#define E1000_MPHY_STAT		0x0E0C /* GBE MPHY Statistics */
 
 /* IEEE 1588 TIMESYNCH */
 #define E1000_TSYNCRXCTL 0x0B620 /* Rx Time Sync Control register - RW */
@@ -117,21 +121,21 @@
 #define E1000_RQDPC(_n) (0x0C030 + ((_n) * 0x40))
 
 /* DMA Coalescing registers */
-#define E1000_DMACR             0x02508 /* Control Register */
-#define E1000_DMCTXTH           0x03550 /* Transmit Threshold */
-#define E1000_DMCTLX            0x02514 /* Time to Lx Request */
-#define E1000_DMCRTRH           0x05DD0 /* Receive Packet Rate Threshold */
-#define E1000_DMCCNT            0x05DD4 /* Current Rx Count */
-#define E1000_FCRTC             0x02170 /* Flow Control Rx high watermark */
-#define E1000_PCIEMISC          0x05BB8 /* PCIE misc config register */
+#define E1000_DMACR	0x02508 /* Control Register */
+#define E1000_DMCTXTH	0x03550 /* Transmit Threshold */
+#define E1000_DMCTLX	0x02514 /* Time to Lx Request */
+#define E1000_DMCRTRH	0x05DD0 /* Receive Packet Rate Threshold */
+#define E1000_DMCCNT	0x05DD4 /* Current Rx Count */
+#define E1000_FCRTC	0x02170 /* Flow Control Rx high watermark */
+#define E1000_PCIEMISC	0x05BB8 /* PCIE misc config register */
 
 /* TX Rate Limit Registers */
-#define E1000_RTTDQSEL	0x3604	/* Tx Desc Plane Queue Select - WO */
-#define E1000_RTTBCNRM	0x3690	/* Tx BCN Rate-scheduler MMW */
-#define E1000_RTTBCNRC	0x36B0	/* Tx BCN Rate-Scheduler Config - WO */
+#define E1000_RTTDQSEL	0x3604 /* Tx Desc Plane Queue Select - WO */
+#define E1000_RTTBCNRM	0x3690 /* Tx BCN Rate-scheduler MMW */
+#define E1000_RTTBCNRC	0x36B0 /* Tx BCN Rate-Scheduler Config - WO */
 
 /* Split and Replication RX Control - RW */
-#define E1000_RXPBS    0x02404  /* Rx Packet Buffer Size - RW */
+#define E1000_RXPBS	0x02404 /* Rx Packet Buffer Size - RW */
 
 /* Thermal sensor configuration and status registers */
 #define E1000_THMJT	0x08100 /* Junction Temperature */
@@ -140,8 +144,7 @@
 #define E1000_THHIGHTC	0x0810C /* High Threshold Control */
 #define E1000_THSTAT	0x08110 /* Thermal Sensor Status */
 
-/*
- * Convenience macros
+/* Convenience macros
  *
  * Note: "_n" is the queue number of the register to be written to.
  *
@@ -287,7 +290,7 @@
 #define E1000_RFCTL    0x05008  /* Receive Filter Control*/
 #define E1000_MTA      0x05200  /* Multicast Table Array - RW Array */
 #define E1000_RA       0x05400  /* Receive Address - RW Array */
-#define E1000_RA2      0x054E0  /* 2nd half of receive address array - RW Array */
+#define E1000_RA2      0x054E0  /* 2nd half of Rx address array - RW Array */
 #define E1000_PSRTYPE(_i)       (0x05480 + ((_i) * 4))
 #define E1000_RAL(_i)  (((_i) <= 15) ? (0x05400 + ((_i) * 8)) : \
                                        (0x054E0 + ((_i - 16) * 8)))
@@ -360,21 +363,25 @@
 	(readl(hw->hw_addr + reg + ((offset) << 2)))
 
 /* DMA Coalescing registers */
-#define E1000_PCIEMISC          0x05BB8 /* PCIE misc config register */
+#define E1000_PCIEMISC	0x05BB8 /* PCIE misc config register */
 
 /* Energy Efficient Ethernet "EEE" register */
-#define E1000_IPCNFG  0x0E38  /* Internal PHY Configuration */
-#define E1000_EEER    0x0E30  /* Energy Efficient Ethernet */
-#define E1000_EEE_SU  0X0E34  /* EEE Setup */
+#define E1000_IPCNFG	0x0E38 /* Internal PHY Configuration */
+#define E1000_EEER	0x0E30 /* Energy Efficient Ethernet */
+#define E1000_EEE_SU	0X0E34 /* EEE Setup */
+#define E1000_EMIADD	0x10   /* Extended Memory Indirect Address */
+#define E1000_EMIDATA	0x11   /* Extended Memory Indirect Data */
+#define E1000_MMDAC	13     /* MMD Access Control */
+#define E1000_MMDAAD	14     /* MMD Access Address/Data */
 
 /* Thermal Sensor Register */
-#define E1000_THSTAT    0x08110 /* Thermal Sensor Status */
+#define E1000_THSTAT	0x08110 /* Thermal Sensor Status */
 
 /* OS2BMC Registers */
-#define E1000_B2OSPC    0x08FE0 /* BMC2OS packets sent by BMC */
-#define E1000_B2OGPRC   0x04158 /* BMC2OS packets received by host */
-#define E1000_O2BGPTC   0x08FE4 /* OS2BMC packets received by BMC */
-#define E1000_O2BSPC    0x0415C /* OS2BMC packets transmitted by host */
+#define E1000_B2OSPC	0x08FE0 /* BMC2OS packets sent by BMC */
+#define E1000_B2OGPRC	0x04158 /* BMC2OS packets received by host */
+#define E1000_O2BGPTC	0x08FE4 /* OS2BMC packets received by BMC */
+#define E1000_O2BSPC	0x0415C /* OS2BMC packets transmitted by host */
 
 #define E1000_SRWR		0x12018  /* Shadow Ram Write Register - RW */
 #define E1000_I210_FLMNGCTL	0x12038
diff --git a/drivers/net/ethernet/intel/igb/igb.h b/drivers/net/ethernet/intel/igb/igb.h
index ab577a763a20..9d6c075e232d 100644
--- a/drivers/net/ethernet/intel/igb/igb.h
+++ b/drivers/net/ethernet/intel/igb/igb.h
@@ -44,54 +44,54 @@
 
 struct igb_adapter;
 
-#define E1000_PCS_CFG_IGN_SD               1
+#define E1000_PCS_CFG_IGN_SD	1
 
 /* Interrupt defines */
-#define IGB_START_ITR                    648 /* ~6000 ints/sec */
-#define IGB_4K_ITR                       980
-#define IGB_20K_ITR                      196
-#define IGB_70K_ITR                       56
+#define IGB_START_ITR		648 /* ~6000 ints/sec */
+#define IGB_4K_ITR		980
+#define IGB_20K_ITR		196
+#define IGB_70K_ITR		56
 
 /* TX/RX descriptor defines */
-#define IGB_DEFAULT_TXD                  256
-#define IGB_DEFAULT_TX_WORK		 128
-#define IGB_MIN_TXD                       80
-#define IGB_MAX_TXD                     4096
+#define IGB_DEFAULT_TXD		256
+#define IGB_DEFAULT_TX_WORK	128
+#define IGB_MIN_TXD		80
+#define IGB_MAX_TXD		4096
 
-#define IGB_DEFAULT_RXD                  256
-#define IGB_MIN_RXD                       80
-#define IGB_MAX_RXD                     4096
+#define IGB_DEFAULT_RXD		256
+#define IGB_MIN_RXD		80
+#define IGB_MAX_RXD		4096
 
-#define IGB_DEFAULT_ITR                    3 /* dynamic */
-#define IGB_MAX_ITR_USECS              10000
-#define IGB_MIN_ITR_USECS                 10
-#define NON_Q_VECTORS                      1
-#define MAX_Q_VECTORS                      8
+#define IGB_DEFAULT_ITR		3 /* dynamic */
+#define IGB_MAX_ITR_USECS	10000
+#define IGB_MIN_ITR_USECS	10
+#define NON_Q_VECTORS		1
+#define MAX_Q_VECTORS		8
 
 /* Transmit and receive queues */
-#define IGB_MAX_RX_QUEUES                  8
-#define IGB_MAX_RX_QUEUES_82575            4
-#define IGB_MAX_RX_QUEUES_I211             2
-#define IGB_MAX_TX_QUEUES                  8
-#define IGB_MAX_VF_MC_ENTRIES              30
-#define IGB_MAX_VF_FUNCTIONS               8
-#define IGB_MAX_VFTA_ENTRIES               128
-#define IGB_82576_VF_DEV_ID                0x10CA
-#define IGB_I350_VF_DEV_ID                 0x1520
+#define IGB_MAX_RX_QUEUES	8
+#define IGB_MAX_RX_QUEUES_82575	4
+#define IGB_MAX_RX_QUEUES_I211	2
+#define IGB_MAX_TX_QUEUES	8
+#define IGB_MAX_VF_MC_ENTRIES	30
+#define IGB_MAX_VF_FUNCTIONS	8
+#define IGB_MAX_VFTA_ENTRIES	128
+#define IGB_82576_VF_DEV_ID	0x10CA
+#define IGB_I350_VF_DEV_ID	0x1520
 
 /* NVM version defines */
-#define IGB_MAJOR_MASK			0xF000
-#define IGB_MINOR_MASK			0x0FF0
-#define IGB_BUILD_MASK			0x000F
-#define IGB_COMB_VER_MASK		0x00FF
-#define IGB_MAJOR_SHIFT			12
-#define IGB_MINOR_SHIFT			4
-#define IGB_COMB_VER_SHFT		8
-#define IGB_NVM_VER_INVALID		0xFFFF
-#define IGB_ETRACK_SHIFT		16
-#define NVM_ETRACK_WORD			0x0042
-#define NVM_COMB_VER_OFF		0x0083
-#define NVM_COMB_VER_PTR		0x003d
+#define IGB_MAJOR_MASK		0xF000
+#define IGB_MINOR_MASK		0x0FF0
+#define IGB_BUILD_MASK		0x000F
+#define IGB_COMB_VER_MASK	0x00FF
+#define IGB_MAJOR_SHIFT		12
+#define IGB_MINOR_SHIFT		4
+#define IGB_COMB_VER_SHFT	8
+#define IGB_NVM_VER_INVALID	0xFFFF
+#define IGB_ETRACK_SHIFT	16
+#define NVM_ETRACK_WORD		0x0042
+#define NVM_COMB_VER_OFF	0x0083
+#define NVM_COMB_VER_PTR	0x003d
 
 struct vf_data_storage {
 	unsigned char vf_mac_addresses[ETH_ALEN];
@@ -103,6 +103,7 @@ struct vf_data_storage {
 	u16 pf_vlan; /* When set, guest VLAN config not allowed. */
 	u16 pf_qos;
 	u16 tx_rate;
+	bool spoofchk_enabled;
 };
 
 #define IGB_VF_FLAG_CTS            0x00000001 /* VF is clear to send data */
@@ -121,14 +122,14 @@ struct vf_data_storage {
  *           descriptors until either it has this many to write back, or the
  *           ITR timer expires.
  */
-#define IGB_RX_PTHRESH                     8
-#define IGB_RX_HTHRESH                     8
-#define IGB_TX_PTHRESH                     8
-#define IGB_TX_HTHRESH                     1
-#define IGB_RX_WTHRESH                     ((hw->mac.type == e1000_82576 && \
-					     adapter->msix_entries) ? 1 : 4)
-#define IGB_TX_WTHRESH                     ((hw->mac.type == e1000_82576 && \
-					     adapter->msix_entries) ? 1 : 16)
+#define IGB_RX_PTHRESH	((hw->mac.type == e1000_i354) ? 12 : 8)
+#define IGB_RX_HTHRESH	8
+#define IGB_TX_PTHRESH	((hw->mac.type == e1000_i354) ? 20 : 8)
+#define IGB_TX_HTHRESH	1
+#define IGB_RX_WTHRESH	((hw->mac.type == e1000_82576 && \
+			  adapter->msix_entries) ? 1 : 4)
+#define IGB_TX_WTHRESH	((hw->mac.type == e1000_82576 && \
+			  adapter->msix_entries) ? 1 : 16)
 
 /* this is the size past which hardware will drop packets when setting LPE=0 */
 #define MAXIMUM_ETHERNET_VLAN_SIZE 1522
@@ -140,17 +141,17 @@ struct vf_data_storage {
 #define IGB_RX_BUFSZ		IGB_RXBUFFER_2048
 
 /* How many Rx Buffers do we bundle into one write to the hardware ? */
-#define IGB_RX_BUFFER_WRITE	16	/* Must be power of 2 */
+#define IGB_RX_BUFFER_WRITE	16 /* Must be power of 2 */
 
-#define AUTO_ALL_MODES            0
-#define IGB_EEPROM_APME         0x0400
+#define AUTO_ALL_MODES		0
+#define IGB_EEPROM_APME		0x0400
 
 #ifndef IGB_MASTER_SLAVE
 /* Switch to override PHY master/slave setting */
 #define IGB_MASTER_SLAVE	e1000_ms_hw_default
 #endif
 
-#define IGB_MNG_VLAN_NONE -1
+#define IGB_MNG_VLAN_NONE	-1
 
 enum igb_tx_flags {
 	/* cmd_type flags */
@@ -164,11 +165,10 @@ enum igb_tx_flags {
 };
 
 /* VLAN info */
-#define IGB_TX_FLAGS_VLAN_MASK		0xffff0000
+#define IGB_TX_FLAGS_VLAN_MASK	0xffff0000
 #define IGB_TX_FLAGS_VLAN_SHIFT	16
 
-/*
- * The largest size we can write to the descriptor is 65535.  In order to
+/* The largest size we can write to the descriptor is 65535.  In order to
  * maintain a power of two alignment we have to limit ourselves to 32K.
  */
 #define IGB_MAX_TXD_PWR	15
@@ -178,8 +178,17 @@ enum igb_tx_flags {
 #define TXD_USE_COUNT(S) DIV_ROUND_UP((S), IGB_MAX_DATA_PER_TXD)
 #define DESC_NEEDED (MAX_SKB_FRAGS + 4)
 
+/* EEPROM byte offsets */
+#define IGB_SFF_8472_SWAP		0x5C
+#define IGB_SFF_8472_COMP		0x5E
+
+/* Bitmasks */
+#define IGB_SFF_ADDRESSING_MODE		0x4
+#define IGB_SFF_8472_UNSUP		0x00
+
 /* wrapper around a pointer to a socket buffer,
- * so a DMA handle can be stored along with the buffer */
+ * so a DMA handle can be stored along with the buffer
+ */
 struct igb_tx_buffer {
 	union e1000_adv_tx_desc *next_to_watch;
 	unsigned long time_stamp;
@@ -290,11 +299,11 @@ enum e1000_ring_flags_t {
 
 #define IGB_TXD_DCMD (E1000_ADVTXD_DCMD_EOP | E1000_ADVTXD_DCMD_RS)
 
-#define IGB_RX_DESC(R, i)	    \
+#define IGB_RX_DESC(R, i)	\
 	(&(((union e1000_adv_rx_desc *)((R)->desc))[i]))
-#define IGB_TX_DESC(R, i)	    \
+#define IGB_TX_DESC(R, i)	\
 	(&(((union e1000_adv_tx_desc *)((R)->desc))[i]))
-#define IGB_TX_CTXTDESC(R, i)	    \
+#define IGB_TX_CTXTDESC(R, i)	\
 	(&(((struct e1000_adv_tx_context_desc *)((R)->desc))[i]))
 
 /* igb_test_staterr - tests bits within Rx descriptor status and error fields */
@@ -453,12 +462,12 @@ struct igb_adapter {
 #define IGB_FLAG_WOL_SUPPORTED		(1 << 8)
 
 /* DMA Coalescing defines */
-#define IGB_MIN_TXPBSIZE           20408
-#define IGB_TX_BUF_4096            4096
-#define IGB_DMCTLX_DCFLUSH_DIS     0x80000000  /* Disable DMA Coal Flush */
+#define IGB_MIN_TXPBSIZE	20408
+#define IGB_TX_BUF_4096		4096
+#define IGB_DMCTLX_DCFLUSH_DIS	0x80000000  /* Disable DMA Coal Flush */
 
-#define IGB_82576_TSYNC_SHIFT 19
-#define IGB_TS_HDR_LEN        16
+#define IGB_82576_TSYNC_SHIFT	19
+#define IGB_TS_HDR_LEN		16
 enum e1000_state_t {
 	__IGB_TESTING,
 	__IGB_RESETTING,
diff --git a/drivers/net/ethernet/intel/igb/igb_ethtool.c b/drivers/net/ethernet/intel/igb/igb_ethtool.c
index a3830a8ba4c1..7876240fa74e 100644
--- a/drivers/net/ethernet/intel/igb/igb_ethtool.c
+++ b/drivers/net/ethernet/intel/igb/igb_ethtool.c
@@ -38,6 +38,7 @@
 #include <linux/slab.h>
 #include <linux/pm_runtime.h>
 #include <linux/highmem.h>
+#include <linux/mdio.h>
 
 #include "igb.h"
 
@@ -178,44 +179,67 @@ static int igb_get_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
 
 		ecmd->port = PORT_TP;
 		ecmd->phy_address = hw->phy.addr;
+		ecmd->transceiver = XCVR_INTERNAL;
 	} else {
-		ecmd->supported   = (SUPPORTED_1000baseT_Full |
-				     SUPPORTED_FIBRE |
-				     SUPPORTED_Autoneg);
+		ecmd->supported = (SUPPORTED_1000baseT_Full |
+				   SUPPORTED_100baseT_Full |
+				   SUPPORTED_FIBRE |
+				   SUPPORTED_Autoneg |
+				   SUPPORTED_Pause);
+		if (hw->mac.type == e1000_i354)
+				ecmd->supported |= SUPPORTED_2500baseX_Full;
+
+		ecmd->advertising = ADVERTISED_FIBRE;
+
+		switch (adapter->link_speed) {
+		case SPEED_2500:
+			ecmd->advertising = ADVERTISED_2500baseX_Full;
+			break;
+		case SPEED_1000:
+			ecmd->advertising = ADVERTISED_1000baseT_Full;
+			break;
+		case SPEED_100:
+			ecmd->advertising = ADVERTISED_100baseT_Full;
+			break;
+		default:
+			break;
+		}
 
-		ecmd->advertising = (ADVERTISED_1000baseT_Full |
-				     ADVERTISED_FIBRE |
-				     ADVERTISED_Autoneg |
-				     ADVERTISED_Pause);
+		if (hw->mac.autoneg == 1)
+			ecmd->advertising |= ADVERTISED_Autoneg;
 
 		ecmd->port = PORT_FIBRE;
+		ecmd->transceiver = XCVR_EXTERNAL;
 	}
 
-	ecmd->transceiver = XCVR_INTERNAL;
-
 	status = rd32(E1000_STATUS);
 
 	if (status & E1000_STATUS_LU) {
-
-		if ((status & E1000_STATUS_SPEED_1000) ||
-		    hw->phy.media_type != e1000_media_type_copper)
-			ethtool_cmd_speed_set(ecmd, SPEED_1000);
+		if ((hw->mac.type == e1000_i354) &&
+		    (status & E1000_STATUS_2P5_SKU) &&
+		    !(status & E1000_STATUS_2P5_SKU_OVER))
+			ecmd->speed = SPEED_2500;
+		else if (status & E1000_STATUS_SPEED_1000)
+			ecmd->speed = SPEED_1000;
 		else if (status & E1000_STATUS_SPEED_100)
-			ethtool_cmd_speed_set(ecmd, SPEED_100);
+			ecmd->speed = SPEED_100;
 		else
-			ethtool_cmd_speed_set(ecmd, SPEED_10);
-
+			ecmd->speed = SPEED_10;
 		if ((status & E1000_STATUS_FD) ||
 		    hw->phy.media_type != e1000_media_type_copper)
 			ecmd->duplex = DUPLEX_FULL;
 		else
 			ecmd->duplex = DUPLEX_HALF;
 	} else {
-		ethtool_cmd_speed_set(ecmd, -1);
+		ecmd->speed = -1;
 		ecmd->duplex = -1;
 	}
 
-	ecmd->autoneg = hw->mac.autoneg ? AUTONEG_ENABLE : AUTONEG_DISABLE;
+	if ((hw->phy.media_type == e1000_media_type_fiber) ||
+	    hw->mac.autoneg)
+		ecmd->autoneg = AUTONEG_ENABLE;
+	else
+		ecmd->autoneg = AUTONEG_DISABLE;
 
 	/* MDI-X => 2; MDI =>1; Invalid =>0 */
 	if (hw->phy.media_type == e1000_media_type_copper)
@@ -238,15 +262,15 @@ static int igb_set_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
 	struct e1000_hw *hw = &adapter->hw;
 
 	/* When SoL/IDER sessions are active, autoneg/speed/duplex
-	 * cannot be changed */
+	 * cannot be changed
+	 */
 	if (igb_check_reset_block(hw)) {
 		dev_err(&adapter->pdev->dev,
 			"Cannot change link characteristics when SoL/IDER is active.\n");
 		return -EINVAL;
 	}
 
-	/*
-	 * MDI setting is only allowed when autoneg enabled because
+	/* MDI setting is only allowed when autoneg enabled because
 	 * some hardware doesn't allow MDI setting when speed or
 	 * duplex is forced.
 	 */
@@ -266,9 +290,31 @@ static int igb_set_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
 
 	if (ecmd->autoneg == AUTONEG_ENABLE) {
 		hw->mac.autoneg = 1;
-		hw->phy.autoneg_advertised = ecmd->advertising |
-					     ADVERTISED_TP |
-					     ADVERTISED_Autoneg;
+		if (hw->phy.media_type == e1000_media_type_fiber) {
+			hw->phy.autoneg_advertised = ecmd->advertising |
+						     ADVERTISED_FIBRE |
+						     ADVERTISED_Autoneg;
+			switch (adapter->link_speed) {
+			case SPEED_2500:
+				hw->phy.autoneg_advertised =
+					ADVERTISED_2500baseX_Full;
+				break;
+			case SPEED_1000:
+				hw->phy.autoneg_advertised =
+					ADVERTISED_1000baseT_Full;
+				break;
+			case SPEED_100:
+				hw->phy.autoneg_advertised =
+					ADVERTISED_100baseT_Full;
+				break;
+			default:
+				break;
+			}
+		} else {
+			hw->phy.autoneg_advertised = ecmd->advertising |
+						     ADVERTISED_TP |
+						     ADVERTISED_Autoneg;
+		}
 		ecmd->advertising = hw->phy.autoneg_advertised;
 		if (adapter->fc_autoneg)
 			hw->fc.requested_mode = e1000_fc_default;
@@ -283,8 +329,7 @@ static int igb_set_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
 
 	/* MDI-X => 2; MDI => 1; Auto => 3 */
 	if (ecmd->eth_tp_mdix_ctrl) {
-		/*
-		 * fix up the value for auto (3 => 0) as zero is mapped
+		/* fix up the value for auto (3 => 0) as zero is mapped
 		 * internally to auto
 		 */
 		if (ecmd->eth_tp_mdix_ctrl == ETH_TP_MDI_AUTO)
@@ -309,8 +354,7 @@ static u32 igb_get_link(struct net_device *netdev)
 	struct igb_adapter *adapter = netdev_priv(netdev);
 	struct e1000_mac_info *mac = &adapter->hw.mac;
 
-	/*
-	 * If the link is not reported up to netdev, interrupts are disabled,
+	/* If the link is not reported up to netdev, interrupts are disabled,
 	 * and so the physical link state may have changed since we last
 	 * looked. Set get_link_status to make sure that the true link
 	 * state is interrogated, rather than pulling a cached and possibly
@@ -430,7 +474,8 @@ static void igb_get_regs(struct net_device *netdev,
 
 	/* Interrupt */
 	/* Reading EICS for EICR because they read the
-	 * same but EICS does not clear on read */
+	 * same but EICS does not clear on read
+	 */
 	regs_buff[13] = rd32(E1000_EICS);
 	regs_buff[14] = rd32(E1000_EICS);
 	regs_buff[15] = rd32(E1000_EIMS);
@@ -438,7 +483,8 @@ static void igb_get_regs(struct net_device *netdev,
 	regs_buff[17] = rd32(E1000_EIAC);
 	regs_buff[18] = rd32(E1000_EIAM);
 	/* Reading ICS for ICR because they read the
-	 * same but ICS does not clear on read */
+	 * same but ICS does not clear on read
+	 */
 	regs_buff[19] = rd32(E1000_ICS);
 	regs_buff[20] = rd32(E1000_ICS);
 	regs_buff[21] = rd32(E1000_IMS);
@@ -688,12 +734,12 @@ static int igb_get_eeprom(struct net_device *netdev,
 
 	if (hw->nvm.type == e1000_nvm_eeprom_spi)
 		ret_val = hw->nvm.ops.read(hw, first_word,
-					    last_word - first_word + 1,
-					    eeprom_buff);
+					   last_word - first_word + 1,
+					   eeprom_buff);
 	else {
 		for (i = 0; i < last_word - first_word + 1; i++) {
 			ret_val = hw->nvm.ops.read(hw, first_word + i, 1,
-						    &eeprom_buff[i]);
+						   &eeprom_buff[i]);
 			if (ret_val)
 				break;
 		}
@@ -740,15 +786,17 @@ static int igb_set_eeprom(struct net_device *netdev,
 	ptr = (void *)eeprom_buff;
 
 	if (eeprom->offset & 1) {
-		/* need read/modify/write of first changed EEPROM word */
-		/* only the second byte of the word is being modified */
+		/* need read/modify/write of first changed EEPROM word
+		 * only the second byte of the word is being modified
+		 */
 		ret_val = hw->nvm.ops.read(hw, first_word, 1,
 					    &eeprom_buff[0]);
 		ptr++;
 	}
 	if (((eeprom->offset + eeprom->len) & 1) && (ret_val == 0)) {
-		/* need read/modify/write of last changed EEPROM word */
-		/* only the first byte of the word is being modified */
+		/* need read/modify/write of last changed EEPROM word
+		 * only the first byte of the word is being modified
+		 */
 		ret_val = hw->nvm.ops.read(hw, last_word, 1,
 				   &eeprom_buff[last_word - first_word]);
 	}
@@ -763,10 +811,11 @@ static int igb_set_eeprom(struct net_device *netdev,
 		eeprom_buff[i] = cpu_to_le16(eeprom_buff[i]);
 
 	ret_val = hw->nvm.ops.write(hw, first_word,
-				     last_word - first_word + 1, eeprom_buff);
+				    last_word - first_word + 1, eeprom_buff);
 
 	/* Update the checksum over the first part of the EEPROM if needed
-	 * and flush shadow RAM for 82573 controllers */
+	 * and flush shadow RAM for 82573 controllers
+	 */
 	if ((ret_val == 0) && ((first_word <= NVM_CHECKSUM_REG)))
 		hw->nvm.ops.update(hw);
 
@@ -783,8 +832,7 @@ static void igb_get_drvinfo(struct net_device *netdev,
 	strlcpy(drvinfo->driver,  igb_driver_name, sizeof(drvinfo->driver));
 	strlcpy(drvinfo->version, igb_driver_version, sizeof(drvinfo->version));
 
-	/*
-	 * EEPROM image version # is reported as firmware version # for
+	/* EEPROM image version # is reported as firmware version # for
 	 * 82575 controllers
 	 */
 	strlcpy(drvinfo->fw_version, adapter->fw_version,
@@ -847,9 +895,11 @@ static int igb_set_ringparam(struct net_device *netdev,
 	}
 
 	if (adapter->num_tx_queues > adapter->num_rx_queues)
-		temp_ring = vmalloc(adapter->num_tx_queues * sizeof(struct igb_ring));
+		temp_ring = vmalloc(adapter->num_tx_queues *
+				    sizeof(struct igb_ring));
 	else
-		temp_ring = vmalloc(adapter->num_rx_queues * sizeof(struct igb_ring));
+		temp_ring = vmalloc(adapter->num_rx_queues *
+				    sizeof(struct igb_ring));
 
 	if (!temp_ring) {
 		err = -ENOMEM;
@@ -858,10 +908,9 @@ static int igb_set_ringparam(struct net_device *netdev,
 
 	igb_down(adapter);
 
-	/*
-	 * We can't just free everything and then setup again,
+	/* We can't just free everything and then setup again,
 	 * because the ISRs in MSI-X mode get passed pointers
-	 * to the tx and rx ring structs.
+	 * to the Tx and Rx ring structs.
 	 */
 	if (new_tx_count != adapter->tx_ring_count) {
 		for (i = 0; i < adapter->num_tx_queues; i++) {
@@ -1199,6 +1248,7 @@ static int igb_reg_test(struct igb_adapter *adapter, u64 *data)
 
 	switch (adapter->hw.mac.type) {
 	case e1000_i350:
+	case e1000_i354:
 		test = reg_test_i350;
 		toggle = 0x7FEFF3FF;
 		break;
@@ -1361,6 +1411,7 @@ static int igb_intr_test(struct igb_adapter *adapter, u64 *data)
 		ics_mask = 0x77DCFED5;
 		break;
 	case e1000_i350:
+	case e1000_i354:
 	case e1000_i210:
 	case e1000_i211:
 		ics_mask = 0x77DCFED5;
@@ -1627,17 +1678,12 @@ static int igb_setup_loopback_test(struct igb_adapter *adapter)
 		wr32(E1000_CONNSW, reg);
 
 		/* Unset sigdetect for SERDES loopback on
-		 * 82580 and i350 devices.
+		 * 82580 and newer devices.
 		 */
-		switch (hw->mac.type) {
-		case e1000_82580:
-		case e1000_i350:
+		if (hw->mac.type >= e1000_82580) {
 			reg = rd32(E1000_PCS_CFG0);
 			reg |= E1000_PCS_CFG_IGN_SD;
 			wr32(E1000_PCS_CFG0, reg);
-			break;
-		default:
-			break;
 		}
 
 		/* Set PCS register for forced speed */
@@ -1723,8 +1769,8 @@ static int igb_check_lbtest_frame(struct igb_rx_buffer *rx_buffer,
 }
 
 static int igb_clean_test_rings(struct igb_ring *rx_ring,
-                                struct igb_ring *tx_ring,
-                                unsigned int size)
+				struct igb_ring *tx_ring,
+				unsigned int size)
 {
 	union e1000_adv_rx_desc *rx_desc;
 	struct igb_rx_buffer *rx_buffer_info;
@@ -1737,7 +1783,7 @@ static int igb_clean_test_rings(struct igb_ring *rx_ring,
 	rx_desc = IGB_RX_DESC(rx_ring, rx_ntc);
 
 	while (igb_test_staterr(rx_desc, E1000_RXD_STAT_DD)) {
-		/* check rx buffer */
+		/* check Rx buffer */
 		rx_buffer_info = &rx_ring->rx_buffer_info[rx_ntc];
 
 		/* sync Rx buffer for CPU read */
@@ -1756,11 +1802,11 @@ static int igb_clean_test_rings(struct igb_ring *rx_ring,
 					   IGB_RX_BUFSZ,
 					   DMA_FROM_DEVICE);
 
-		/* unmap buffer on tx side */
+		/* unmap buffer on Tx side */
 		tx_buffer_info = &tx_ring->tx_buffer_info[tx_ntc];
 		igb_unmap_and_free_tx_resource(tx_ring, tx_buffer_info);
 
-		/* increment rx/tx next to clean counters */
+		/* increment Rx/Tx next to clean counters */
 		rx_ntc++;
 		if (rx_ntc == rx_ring->count)
 			rx_ntc = 0;
@@ -1801,8 +1847,7 @@ static int igb_run_loopback_test(struct igb_adapter *adapter)
 	igb_create_lbtest_frame(skb, size);
 	skb_put(skb, size);
 
-	/*
-	 * Calculate the loop count based on the largest descriptor ring
+	/* Calculate the loop count based on the largest descriptor ring
 	 * The idea is to wrap the largest ring a number of times using 64
 	 * send/receive pairs during each loop
 	 */
@@ -1829,7 +1874,7 @@ static int igb_run_loopback_test(struct igb_adapter *adapter)
 			break;
 		}
 
-		/* allow 200 milliseconds for packets to go from tx to rx */
+		/* allow 200 milliseconds for packets to go from Tx to Rx */
 		msleep(200);
 
 		good_cnt = igb_clean_test_rings(rx_ring, tx_ring, size);
@@ -1848,13 +1893,21 @@ static int igb_run_loopback_test(struct igb_adapter *adapter)
 static int igb_loopback_test(struct igb_adapter *adapter, u64 *data)
 {
 	/* PHY loopback cannot be performed if SoL/IDER
-	 * sessions are active */
+	 * sessions are active
+	 */
 	if (igb_check_reset_block(&adapter->hw)) {
 		dev_err(&adapter->pdev->dev,
 			"Cannot do PHY loopback test when SoL/IDER is active.\n");
 		*data = 0;
 		goto out;
 	}
+
+	if (adapter->hw.mac.type == e1000_i354) {
+		dev_info(&adapter->pdev->dev,
+			"Loopback test not supported on i354.\n");
+		*data = 0;
+		goto out;
+	}
 	*data = igb_setup_desc_rings(adapter);
 	if (*data)
 		goto out;
@@ -1879,7 +1932,8 @@ static int igb_link_test(struct igb_adapter *adapter, u64 *data)
 		hw->mac.serdes_has_link = false;
 
 		/* On some blade server designs, link establishment
-		 * could take as long as 2-3 minutes */
+		 * could take as long as 2-3 minutes
+		 */
 		do {
 			hw->mac.ops.check_for_link(&adapter->hw);
 			if (hw->mac.serdes_has_link)
@@ -1922,7 +1976,8 @@ static void igb_diag_test(struct net_device *netdev,
 		igb_power_up_link(adapter);
 
 		/* Link test performed before hardware reset so autoneg doesn't
-		 * interfere with test result */
+		 * interfere with test result
+		 */
 		if (igb_link_test(adapter, &data[4]))
 			eth_test->flags |= ETH_TEST_FL_FAILED;
 
@@ -1987,8 +2042,8 @@ static void igb_get_wol(struct net_device *netdev, struct ethtool_wolinfo *wol)
 	struct igb_adapter *adapter = netdev_priv(netdev);
 
 	wol->supported = WAKE_UCAST | WAKE_MCAST |
-	                 WAKE_BCAST | WAKE_MAGIC |
-	                 WAKE_PHY;
+			 WAKE_BCAST | WAKE_MAGIC |
+			 WAKE_PHY;
 	wol->wolopts = 0;
 
 	if (!(adapter->flags & IGB_FLAG_WOL_SUPPORTED))
@@ -2263,7 +2318,7 @@ static void igb_get_strings(struct net_device *netdev, u32 stringset, u8 *data)
 			sprintf(p, "rx_queue_%u_alloc_failed", i);
 			p += ETH_GSTRING_LEN;
 		}
-/*		BUG_ON(p - data != IGB_STATS_LEN * ETH_GSTRING_LEN); */
+		/* BUG_ON(p - data != IGB_STATS_LEN * ETH_GSTRING_LEN); */
 		break;
 	}
 }
@@ -2283,6 +2338,7 @@ static int igb_get_ts_info(struct net_device *dev,
 	case e1000_82576:
 	case e1000_82580:
 	case e1000_i350:
+	case e1000_i354:
 	case e1000_i210:
 	case e1000_i211:
 		info->so_timestamping =
@@ -2362,7 +2418,7 @@ static int igb_get_rss_hash_opts(struct igb_adapter *adapter,
 }
 
 static int igb_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd,
-			   u32 *rule_locs)
+			 u32 *rule_locs)
 {
 	struct igb_adapter *adapter = netdev_priv(dev);
 	int ret = -EOPNOTSUPP;
@@ -2506,7 +2562,8 @@ static int igb_get_eee(struct net_device *netdev, struct ethtool_eee *edata)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
-	u32 ipcnfg, eeer;
+	u32 ipcnfg, eeer, ret_val;
+	u16 phy_data;
 
 	if ((hw->mac.type < e1000_i350) ||
 	    (hw->phy.media_type != e1000_media_type_copper))
@@ -2525,6 +2582,32 @@ static int igb_get_eee(struct net_device *netdev, struct ethtool_eee *edata)
 	if (ipcnfg & E1000_IPCNFG_EEE_100M_AN)
 		edata->advertised |= ADVERTISED_100baseT_Full;
 
+	/* EEE Link Partner Advertised */
+	switch (hw->mac.type) {
+	case e1000_i350:
+		ret_val = igb_read_emi_reg(hw, E1000_EEE_LP_ADV_ADDR_I350,
+					   &phy_data);
+		if (ret_val)
+			return -ENODATA;
+
+		edata->lp_advertised = mmd_eee_adv_to_ethtool_adv_t(phy_data);
+
+		break;
+	case e1000_i210:
+	case e1000_i211:
+		ret_val = igb_read_xmdio_reg(hw, E1000_EEE_LP_ADV_ADDR_I210,
+					     E1000_EEE_LP_ADV_DEV_I210,
+					     &phy_data);
+		if (ret_val)
+			return -ENODATA;
+
+		edata->lp_advertised = mmd_eee_adv_to_ethtool_adv_t(phy_data);
+
+		break;
+	default:
+		break;
+	}
+
 	if (eeer & E1000_EEER_EEE_NEG)
 		edata->eee_active = true;
 
@@ -2600,6 +2683,85 @@ static int igb_set_eee(struct net_device *netdev,
 	return 0;
 }
 
+static int igb_get_module_info(struct net_device *netdev,
+			       struct ethtool_modinfo *modinfo)
+{
+	struct igb_adapter *adapter = netdev_priv(netdev);
+	struct e1000_hw *hw = &adapter->hw;
+	u32 status = E1000_SUCCESS;
+	u16 sff8472_rev, addr_mode;
+	bool page_swap = false;
+
+	if ((hw->phy.media_type == e1000_media_type_copper) ||
+	    (hw->phy.media_type == e1000_media_type_unknown))
+		return -EOPNOTSUPP;
+
+	/* Check whether we support SFF-8472 or not */
+	status = igb_read_phy_reg_i2c(hw, IGB_SFF_8472_COMP, &sff8472_rev);
+	if (status != E1000_SUCCESS)
+		return -EIO;
+
+	/* addressing mode is not supported */
+	status = igb_read_phy_reg_i2c(hw, IGB_SFF_8472_SWAP, &addr_mode);
+	if (status != E1000_SUCCESS)
+		return -EIO;
+
+	/* addressing mode is not supported */
+	if ((addr_mode & 0xFF) & IGB_SFF_ADDRESSING_MODE) {
+		hw_dbg("Address change required to access page 0xA2, but not supported. Please report the module type to the driver maintainers.\n");
+		page_swap = true;
+	}
+
+	if ((sff8472_rev & 0xFF) == IGB_SFF_8472_UNSUP || page_swap) {
+		/* We have an SFP, but it does not support SFF-8472 */
+		modinfo->type = ETH_MODULE_SFF_8079;
+		modinfo->eeprom_len = ETH_MODULE_SFF_8079_LEN;
+	} else {
+		/* We have an SFP which supports a revision of SFF-8472 */
+		modinfo->type = ETH_MODULE_SFF_8472;
+		modinfo->eeprom_len = ETH_MODULE_SFF_8472_LEN;
+	}
+
+	return 0;
+}
+
+static int igb_get_module_eeprom(struct net_device *netdev,
+				 struct ethtool_eeprom *ee, u8 *data)
+{
+	struct igb_adapter *adapter = netdev_priv(netdev);
+	struct e1000_hw *hw = &adapter->hw;
+	u32 status = E1000_SUCCESS;
+	u16 *dataword;
+	u16 first_word, last_word;
+	int i = 0;
+
+	if (ee->len == 0)
+		return -EINVAL;
+
+	first_word = ee->offset >> 1;
+	last_word = (ee->offset + ee->len - 1) >> 1;
+
+	dataword = kmalloc(sizeof(u16) * (last_word - first_word + 1),
+			   GFP_KERNEL);
+	if (!dataword)
+		return -ENOMEM;
+
+	/* Read EEPROM block, SFF-8079/SFF-8472, word at a time */
+	for (i = 0; i < last_word - first_word + 1; i++) {
+		status = igb_read_phy_reg_i2c(hw, first_word + i, &dataword[i]);
+		if (status != E1000_SUCCESS)
+			/* Error occurred while reading module */
+			return -EIO;
+
+		be16_to_cpus(&dataword[i]);
+	}
+
+	memcpy(data, (u8 *)dataword + (ee->offset & 1), ee->len);
+	kfree(dataword);
+
+	return 0;
+}
+
 static int igb_ethtool_begin(struct net_device *netdev)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
@@ -2614,36 +2776,38 @@ static void igb_ethtool_complete(struct net_device *netdev)
 }
 
 static const struct ethtool_ops igb_ethtool_ops = {
-	.get_settings           = igb_get_settings,
-	.set_settings           = igb_set_settings,
-	.get_drvinfo            = igb_get_drvinfo,
-	.get_regs_len           = igb_get_regs_len,
-	.get_regs               = igb_get_regs,
-	.get_wol                = igb_get_wol,
-	.set_wol                = igb_set_wol,
-	.get_msglevel           = igb_get_msglevel,
-	.set_msglevel           = igb_set_msglevel,
-	.nway_reset             = igb_nway_reset,
-	.get_link               = igb_get_link,
-	.get_eeprom_len         = igb_get_eeprom_len,
-	.get_eeprom             = igb_get_eeprom,
-	.set_eeprom             = igb_set_eeprom,
-	.get_ringparam          = igb_get_ringparam,
-	.set_ringparam          = igb_set_ringparam,
-	.get_pauseparam         = igb_get_pauseparam,
-	.set_pauseparam         = igb_set_pauseparam,
-	.self_test              = igb_diag_test,
-	.get_strings            = igb_get_strings,
-	.set_phys_id            = igb_set_phys_id,
-	.get_sset_count         = igb_get_sset_count,
-	.get_ethtool_stats      = igb_get_ethtool_stats,
-	.get_coalesce           = igb_get_coalesce,
-	.set_coalesce           = igb_set_coalesce,
-	.get_ts_info            = igb_get_ts_info,
+	.get_settings		= igb_get_settings,
+	.set_settings		= igb_set_settings,
+	.get_drvinfo		= igb_get_drvinfo,
+	.get_regs_len		= igb_get_regs_len,
+	.get_regs		= igb_get_regs,
+	.get_wol		= igb_get_wol,
+	.set_wol		= igb_set_wol,
+	.get_msglevel		= igb_get_msglevel,
+	.set_msglevel		= igb_set_msglevel,
+	.nway_reset		= igb_nway_reset,
+	.get_link		= igb_get_link,
+	.get_eeprom_len		= igb_get_eeprom_len,
+	.get_eeprom		= igb_get_eeprom,
+	.set_eeprom		= igb_set_eeprom,
+	.get_ringparam		= igb_get_ringparam,
+	.set_ringparam		= igb_set_ringparam,
+	.get_pauseparam		= igb_get_pauseparam,
+	.set_pauseparam		= igb_set_pauseparam,
+	.self_test		= igb_diag_test,
+	.get_strings		= igb_get_strings,
+	.set_phys_id		= igb_set_phys_id,
+	.get_sset_count		= igb_get_sset_count,
+	.get_ethtool_stats	= igb_get_ethtool_stats,
+	.get_coalesce		= igb_get_coalesce,
+	.set_coalesce		= igb_set_coalesce,
+	.get_ts_info		= igb_get_ts_info,
 	.get_rxnfc		= igb_get_rxnfc,
 	.set_rxnfc		= igb_set_rxnfc,
 	.get_eee		= igb_get_eee,
 	.set_eee		= igb_set_eee,
+	.get_module_info	= igb_get_module_info,
+	.get_module_eeprom	= igb_get_module_eeprom,
 	.begin			= igb_ethtool_begin,
 	.complete		= igb_ethtool_complete,
 };
diff --git a/drivers/net/ethernet/intel/igb/igb_hwmon.c b/drivers/net/ethernet/intel/igb/igb_hwmon.c
index 0478a1abe541..58f1ce967aeb 100644
--- a/drivers/net/ethernet/intel/igb/igb_hwmon.c
+++ b/drivers/net/ethernet/intel/igb/igb_hwmon.c
@@ -45,21 +45,21 @@ static struct i2c_board_info i350_sensor_info = {
 
 /* hwmon callback functions */
 static ssize_t igb_hwmon_show_location(struct device *dev,
-					 struct device_attribute *attr,
-					 char *buf)
+				       struct device_attribute *attr,
+				       char *buf)
 {
 	struct hwmon_attr *igb_attr = container_of(attr, struct hwmon_attr,
-						     dev_attr);
+						   dev_attr);
 	return sprintf(buf, "loc%u\n",
 		       igb_attr->sensor->location);
 }
 
 static ssize_t igb_hwmon_show_temp(struct device *dev,
-				     struct device_attribute *attr,
-				     char *buf)
+				   struct device_attribute *attr,
+				   char *buf)
 {
 	struct hwmon_attr *igb_attr = container_of(attr, struct hwmon_attr,
-						     dev_attr);
+						   dev_attr);
 	unsigned int value;
 
 	/* reset the temp field */
@@ -74,11 +74,11 @@ static ssize_t igb_hwmon_show_temp(struct device *dev,
 }
 
 static ssize_t igb_hwmon_show_cautionthresh(struct device *dev,
-				     struct device_attribute *attr,
-				     char *buf)
+					    struct device_attribute *attr,
+					    char *buf)
 {
 	struct hwmon_attr *igb_attr = container_of(attr, struct hwmon_attr,
-						     dev_attr);
+						   dev_attr);
 	unsigned int value = igb_attr->sensor->caution_thresh;
 
 	/* display millidegree */
@@ -88,11 +88,11 @@ static ssize_t igb_hwmon_show_cautionthresh(struct device *dev,
 }
 
 static ssize_t igb_hwmon_show_maxopthresh(struct device *dev,
-				     struct device_attribute *attr,
-				     char *buf)
+					  struct device_attribute *attr,
+					  char *buf)
 {
 	struct hwmon_attr *igb_attr = container_of(attr, struct hwmon_attr,
-						     dev_attr);
+						   dev_attr);
 	unsigned int value = igb_attr->sensor->max_op_thresh;
 
 	/* display millidegree */
@@ -111,7 +111,8 @@ static ssize_t igb_hwmon_show_maxopthresh(struct device *dev,
  * the data structures we need to get the data to display.
  */
 static int igb_add_hwmon_attr(struct igb_adapter *adapter,
-				unsigned int offset, int type) {
+			      unsigned int offset, int type)
+{
 	int rc;
 	unsigned int n_attr;
 	struct hwmon_attr *igb_attr;
@@ -217,7 +218,7 @@ int igb_sysfs_init(struct igb_adapter *adapter)
 	 */
 	n_attrs = E1000_MAX_SENSORS * 4;
 	igb_hwmon->hwmon_list = kcalloc(n_attrs, sizeof(struct hwmon_attr),
-					  GFP_KERNEL);
+					GFP_KERNEL);
 	if (!igb_hwmon->hwmon_list) {
 		rc = -ENOMEM;
 		goto err;
diff --git a/drivers/net/ethernet/intel/igb/igb_main.c b/drivers/net/ethernet/intel/igb/igb_main.c
index 64f75291e3a5..64cbe0dfe043 100644
--- a/drivers/net/ethernet/intel/igb/igb_main.c
+++ b/drivers/net/ethernet/intel/igb/igb_main.c
@@ -60,9 +60,9 @@
 #include <linux/i2c.h>
 #include "igb.h"
 
-#define MAJ 4
-#define MIN 1
-#define BUILD 2
+#define MAJ 5
+#define MIN 0
+#define BUILD 3
 #define DRV_VERSION __stringify(MAJ) "." __stringify(MIN) "." \
 __stringify(BUILD) "-k"
 char igb_driver_name[] = "igb";
@@ -77,6 +77,9 @@ static const struct e1000_info *igb_info_tbl[] = {
 };
 
 static DEFINE_PCI_DEVICE_TABLE(igb_pci_tbl) = {
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I354_BACKPLANE_1GBPS) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I354_SGMII) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I354_BACKPLANE_2_5GBPS) },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I211_COPPER), board_82575 },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I210_COPPER), board_82575 },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I210_FIBER), board_82575 },
@@ -156,8 +159,8 @@ static int igb_ioctl(struct net_device *, struct ifreq *, int cmd);
 static void igb_tx_timeout(struct net_device *);
 static void igb_reset_task(struct work_struct *);
 static void igb_vlan_mode(struct net_device *netdev, netdev_features_t features);
-static int igb_vlan_rx_add_vid(struct net_device *, u16);
-static int igb_vlan_rx_kill_vid(struct net_device *, u16);
+static int igb_vlan_rx_add_vid(struct net_device *, __be16, u16);
+static int igb_vlan_rx_kill_vid(struct net_device *, __be16, u16);
 static void igb_restore_vlan(struct igb_adapter *);
 static void igb_rar_set_qsel(struct igb_adapter *, u8 *, u32 , u8);
 static void igb_ping_all_vfs(struct igb_adapter *);
@@ -169,13 +172,14 @@ static int igb_ndo_set_vf_mac(struct net_device *netdev, int vf, u8 *mac);
 static int igb_ndo_set_vf_vlan(struct net_device *netdev,
 			       int vf, u16 vlan, u8 qos);
 static int igb_ndo_set_vf_bw(struct net_device *netdev, int vf, int tx_rate);
+static int igb_ndo_set_vf_spoofchk(struct net_device *netdev, int vf,
+				   bool setting);
 static int igb_ndo_get_vf_config(struct net_device *netdev, int vf,
 				 struct ifla_vf_info *ivi);
 static void igb_check_vf_rate_limit(struct igb_adapter *);
 
 #ifdef CONFIG_PCI_IOV
 static int igb_vf_configure(struct igb_adapter *adapter, int vf);
-static bool igb_vfs_are_assigned(struct igb_adapter *adapter);
 #endif
 
 #ifdef CONFIG_PM
@@ -292,9 +296,7 @@ static const struct igb_reg_info igb_reg_info_tbl[] = {
 	{}
 };
 
-/*
- * igb_regdump - register printout routine
- */
+/* igb_regdump - register printout routine */
 static void igb_regdump(struct e1000_hw *hw, struct igb_reg_info *reginfo)
 {
 	int n = 0;
@@ -360,9 +362,7 @@ static void igb_regdump(struct e1000_hw *hw, struct igb_reg_info *reginfo)
 		regs[2], regs[3]);
 }
 
-/*
- * igb_dump - Print registers, tx-rings and rx-rings
- */
+/* igb_dump - Print registers, Tx-rings and Rx-rings */
 static void igb_dump(struct igb_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
@@ -569,12 +569,13 @@ exit:
 	return;
 }
 
-/*  igb_get_i2c_data - Reads the I2C SDA data bit
+/**
+ *  igb_get_i2c_data - Reads the I2C SDA data bit
  *  @hw: pointer to hardware structure
  *  @i2cctl: Current value of I2CCTL register
  *
  *  Returns the I2C data bit value
- */
+ **/
 static int igb_get_i2c_data(void *data)
 {
 	struct igb_adapter *adapter = (struct igb_adapter *)data;
@@ -584,12 +585,13 @@ static int igb_get_i2c_data(void *data)
 	return ((i2cctl & E1000_I2C_DATA_IN) != 0);
 }
 
-/* igb_set_i2c_data - Sets the I2C data bit
+/**
+ *  igb_set_i2c_data - Sets the I2C data bit
  *  @data: pointer to hardware structure
  *  @state: I2C data value (0 or 1) to set
  *
  *  Sets the I2C data bit
- */
+ **/
 static void igb_set_i2c_data(void *data, int state)
 {
 	struct igb_adapter *adapter = (struct igb_adapter *)data;
@@ -608,12 +610,13 @@ static void igb_set_i2c_data(void *data, int state)
 
 }
 
-/* igb_set_i2c_clk - Sets the I2C SCL clock
+/**
+ *  igb_set_i2c_clk - Sets the I2C SCL clock
  *  @data: pointer to hardware structure
  *  @state: state to set clock
  *
  *  Sets the I2C clock line to state
- */
+ **/
 static void igb_set_i2c_clk(void *data, int state)
 {
 	struct igb_adapter *adapter = (struct igb_adapter *)data;
@@ -631,11 +634,12 @@ static void igb_set_i2c_clk(void *data, int state)
 	wrfl();
 }
 
-/* igb_get_i2c_clk - Gets the I2C SCL clock state
+/**
+ *  igb_get_i2c_clk - Gets the I2C SCL clock state
  *  @data: pointer to hardware structure
  *
  *  Gets the I2C clock state
- */
+ **/
 static int igb_get_i2c_clk(void *data)
 {
 	struct igb_adapter *adapter = (struct igb_adapter *)data;
@@ -655,8 +659,10 @@ static const struct i2c_algo_bit_data igb_i2c_algo = {
 };
 
 /**
- * igb_get_hw_dev - return device
- * used by hardware layer to print debugging information
+ *  igb_get_hw_dev - return device
+ *  @hw: pointer to hardware structure
+ *
+ *  used by hardware layer to print debugging information
  **/
 struct net_device *igb_get_hw_dev(struct e1000_hw *hw)
 {
@@ -665,10 +671,10 @@ struct net_device *igb_get_hw_dev(struct e1000_hw *hw)
 }
 
 /**
- * igb_init_module - Driver Registration Routine
+ *  igb_init_module - Driver Registration Routine
  *
- * igb_init_module is the first routine called when the driver is
- * loaded. All it does is register with the PCI subsystem.
+ *  igb_init_module is the first routine called when the driver is
+ *  loaded. All it does is register with the PCI subsystem.
  **/
 static int __init igb_init_module(void)
 {
@@ -688,10 +694,10 @@ static int __init igb_init_module(void)
 module_init(igb_init_module);
 
 /**
- * igb_exit_module - Driver Exit Cleanup Routine
+ *  igb_exit_module - Driver Exit Cleanup Routine
  *
- * igb_exit_module is called just before the driver is removed
- * from memory.
+ *  igb_exit_module is called just before the driver is removed
+ *  from memory.
  **/
 static void __exit igb_exit_module(void)
 {
@@ -705,11 +711,11 @@ module_exit(igb_exit_module);
 
 #define Q_IDX_82576(i) (((i & 0x1) << 3) + (i >> 1))
 /**
- * igb_cache_ring_register - Descriptor ring to register mapping
- * @adapter: board private structure to initialize
+ *  igb_cache_ring_register - Descriptor ring to register mapping
+ *  @adapter: board private structure to initialize
  *
- * Once we know the feature-set enabled for the device, we'll cache
- * the register offset the descriptor ring is assigned to.
+ *  Once we know the feature-set enabled for the device, we'll cache
+ *  the register offset the descriptor ring is assigned to.
  **/
 static void igb_cache_ring_register(struct igb_adapter *adapter)
 {
@@ -726,11 +732,12 @@ static void igb_cache_ring_register(struct igb_adapter *adapter)
 		if (adapter->vfs_allocated_count) {
 			for (; i < adapter->rss_queues; i++)
 				adapter->rx_ring[i]->reg_idx = rbase_offset +
-				                               Q_IDX_82576(i);
+							       Q_IDX_82576(i);
 		}
 	case e1000_82575:
 	case e1000_82580:
 	case e1000_i350:
+	case e1000_i354:
 	case e1000_i210:
 	case e1000_i211:
 	default:
@@ -785,9 +792,10 @@ static void igb_assign_vector(struct igb_q_vector *q_vector, int msix_vector)
 	switch (hw->mac.type) {
 	case e1000_82575:
 		/* The 82575 assigns vectors using a bitmask, which matches the
-		   bitmask for the EICR/EIMS/EIMC registers.  To assign one
-		   or more queues to a vector, we write the appropriate bits
-		   into the MSIXBM register for that vector. */
+		 * bitmask for the EICR/EIMS/EIMC registers.  To assign one
+		 * or more queues to a vector, we write the appropriate bits
+		 * into the MSIXBM register for that vector.
+		 */
 		if (rx_queue > IGB_N0_QUEUE)
 			msixbm = E1000_EICR_RX_QUEUE0 << rx_queue;
 		if (tx_queue > IGB_N0_QUEUE)
@@ -798,8 +806,7 @@ static void igb_assign_vector(struct igb_q_vector *q_vector, int msix_vector)
 		q_vector->eims_value = msixbm;
 		break;
 	case e1000_82576:
-		/*
-		 * 82576 uses a table that essentially consists of 2 columns
+		/* 82576 uses a table that essentially consists of 2 columns
 		 * with 8 rows.  The ordering is column-major so we use the
 		 * lower 3 bits as the row index, and the 4th bit as the
 		 * column offset.
@@ -816,10 +823,10 @@ static void igb_assign_vector(struct igb_q_vector *q_vector, int msix_vector)
 		break;
 	case e1000_82580:
 	case e1000_i350:
+	case e1000_i354:
 	case e1000_i210:
 	case e1000_i211:
-		/*
-		 * On 82580 and newer adapters the scheme is similar to 82576
+		/* On 82580 and newer adapters the scheme is similar to 82576
 		 * however instead of ordering column-major we have things
 		 * ordered row-major.  So we traverse the table by using
 		 * bit 0 as the column offset, and the remaining bits as the
@@ -848,10 +855,11 @@ static void igb_assign_vector(struct igb_q_vector *q_vector, int msix_vector)
 }
 
 /**
- * igb_configure_msix - Configure MSI-X hardware
+ *  igb_configure_msix - Configure MSI-X hardware
+ *  @adapter: board private structure to initialize
  *
- * igb_configure_msix sets up the hardware to properly
- * generate MSI-X interrupts.
+ *  igb_configure_msix sets up the hardware to properly
+ *  generate MSI-X interrupts.
  **/
 static void igb_configure_msix(struct igb_adapter *adapter)
 {
@@ -875,8 +883,7 @@ static void igb_configure_msix(struct igb_adapter *adapter)
 		wr32(E1000_CTRL_EXT, tmp);
 
 		/* enable msix_other interrupt */
-		array_wr32(E1000_MSIXBM(0), vector++,
-		                      E1000_EIMS_OTHER);
+		array_wr32(E1000_MSIXBM(0), vector++, E1000_EIMS_OTHER);
 		adapter->eims_other = E1000_EIMS_OTHER;
 
 		break;
@@ -884,13 +891,15 @@ static void igb_configure_msix(struct igb_adapter *adapter)
 	case e1000_82576:
 	case e1000_82580:
 	case e1000_i350:
+	case e1000_i354:
 	case e1000_i210:
 	case e1000_i211:
 		/* Turn on MSI-X capability first, or our settings
-		 * won't stick.  And it will take days to debug. */
+		 * won't stick.  And it will take days to debug.
+		 */
 		wr32(E1000_GPIE, E1000_GPIE_MSIX_MODE |
-		                E1000_GPIE_PBA | E1000_GPIE_EIAME |
-		                E1000_GPIE_NSICR);
+		     E1000_GPIE_PBA | E1000_GPIE_EIAME |
+		     E1000_GPIE_NSICR);
 
 		/* enable msix_other interrupt */
 		adapter->eims_other = 1 << vector;
@@ -912,10 +921,11 @@ static void igb_configure_msix(struct igb_adapter *adapter)
 }
 
 /**
- * igb_request_msix - Initialize MSI-X interrupts
+ *  igb_request_msix - Initialize MSI-X interrupts
+ *  @adapter: board private structure to initialize
  *
- * igb_request_msix allocates MSI-X vectors and requests interrupts from the
- * kernel.
+ *  igb_request_msix allocates MSI-X vectors and requests interrupts from the
+ *  kernel.
  **/
 static int igb_request_msix(struct igb_adapter *adapter)
 {
@@ -924,7 +934,7 @@ static int igb_request_msix(struct igb_adapter *adapter)
 	int i, err = 0, vector = 0, free_vector = 0;
 
 	err = request_irq(adapter->msix_entries[vector].vector,
-	                  igb_msix_other, 0, netdev->name, adapter);
+			  igb_msix_other, 0, netdev->name, adapter);
 	if (err)
 		goto err_out;
 
@@ -948,8 +958,8 @@ static int igb_request_msix(struct igb_adapter *adapter)
 			sprintf(q_vector->name, "%s-unused", netdev->name);
 
 		err = request_irq(adapter->msix_entries[vector].vector,
-		                  igb_msix_ring, 0, q_vector->name,
-		                  q_vector);
+				  igb_msix_ring, 0, q_vector->name,
+				  q_vector);
 		if (err)
 			goto err_free;
 	}
@@ -982,13 +992,13 @@ static void igb_reset_interrupt_capability(struct igb_adapter *adapter)
 }
 
 /**
- * igb_free_q_vector - Free memory allocated for specific interrupt vector
- * @adapter: board private structure to initialize
- * @v_idx: Index of vector to be freed
+ *  igb_free_q_vector - Free memory allocated for specific interrupt vector
+ *  @adapter: board private structure to initialize
+ *  @v_idx: Index of vector to be freed
  *
- * This function frees the memory allocated to the q_vector.  In addition if
- * NAPI is enabled it will delete any references to the NAPI struct prior
- * to freeing the q_vector.
+ *  This function frees the memory allocated to the q_vector.  In addition if
+ *  NAPI is enabled it will delete any references to the NAPI struct prior
+ *  to freeing the q_vector.
  **/
 static void igb_free_q_vector(struct igb_adapter *adapter, int v_idx)
 {
@@ -1003,20 +1013,19 @@ static void igb_free_q_vector(struct igb_adapter *adapter, int v_idx)
 	adapter->q_vector[v_idx] = NULL;
 	netif_napi_del(&q_vector->napi);
 
-	/*
-	 * ixgbe_get_stats64() might access the rings on this vector,
+	/* ixgbe_get_stats64() might access the rings on this vector,
 	 * we must wait a grace period before freeing it.
 	 */
 	kfree_rcu(q_vector, rcu);
 }
 
 /**
- * igb_free_q_vectors - Free memory allocated for interrupt vectors
- * @adapter: board private structure to initialize
+ *  igb_free_q_vectors - Free memory allocated for interrupt vectors
+ *  @adapter: board private structure to initialize
  *
- * This function frees the memory allocated to the q_vectors.  In addition if
- * NAPI is enabled it will delete any references to the NAPI struct prior
- * to freeing the q_vector.
+ *  This function frees the memory allocated to the q_vectors.  In addition if
+ *  NAPI is enabled it will delete any references to the NAPI struct prior
+ *  to freeing the q_vector.
  **/
 static void igb_free_q_vectors(struct igb_adapter *adapter)
 {
@@ -1031,10 +1040,11 @@ static void igb_free_q_vectors(struct igb_adapter *adapter)
 }
 
 /**
- * igb_clear_interrupt_scheme - reset the device to a state of no interrupts
+ *  igb_clear_interrupt_scheme - reset the device to a state of no interrupts
+ *  @adapter: board private structure to initialize
  *
- * This function resets the device so that it has 0 rx queues, tx queues, and
- * MSI-X interrupts allocated.
+ *  This function resets the device so that it has 0 Rx queues, Tx queues, and
+ *  MSI-X interrupts allocated.
  */
 static void igb_clear_interrupt_scheme(struct igb_adapter *adapter)
 {
@@ -1043,10 +1053,12 @@ static void igb_clear_interrupt_scheme(struct igb_adapter *adapter)
 }
 
 /**
- * igb_set_interrupt_capability - set MSI or MSI-X if supported
+ *  igb_set_interrupt_capability - set MSI or MSI-X if supported
+ *  @adapter: board private structure to initialize
+ *  @msix: boolean value of MSIX capability
  *
- * Attempt to configure interrupts using the best available
- * capabilities of the hardware and kernel.
+ *  Attempt to configure interrupts using the best available
+ *  capabilities of the hardware and kernel.
  **/
 static void igb_set_interrupt_capability(struct igb_adapter *adapter, bool msix)
 {
@@ -1063,10 +1075,10 @@ static void igb_set_interrupt_capability(struct igb_adapter *adapter, bool msix)
 	else
 		adapter->num_tx_queues = adapter->rss_queues;
 
-	/* start with one vector for every rx queue */
+	/* start with one vector for every Rx queue */
 	numvecs = adapter->num_rx_queues;
 
-	/* if tx handler is separate add 1 for every tx queue */
+	/* if Tx handler is separate add 1 for every Tx queue */
 	if (!(adapter->flags & IGB_FLAG_QUEUE_PAIRS))
 		numvecs += adapter->num_tx_queues;
 
@@ -1128,16 +1140,16 @@ static void igb_add_ring(struct igb_ring *ring,
 }
 
 /**
- * igb_alloc_q_vector - Allocate memory for a single interrupt vector
- * @adapter: board private structure to initialize
- * @v_count: q_vectors allocated on adapter, used for ring interleaving
- * @v_idx: index of vector in adapter struct
- * @txr_count: total number of Tx rings to allocate
- * @txr_idx: index of first Tx ring to allocate
- * @rxr_count: total number of Rx rings to allocate
- * @rxr_idx: index of first Rx ring to allocate
+ *  igb_alloc_q_vector - Allocate memory for a single interrupt vector
+ *  @adapter: board private structure to initialize
+ *  @v_count: q_vectors allocated on adapter, used for ring interleaving
+ *  @v_idx: index of vector in adapter struct
+ *  @txr_count: total number of Tx rings to allocate
+ *  @txr_idx: index of first Tx ring to allocate
+ *  @rxr_count: total number of Rx rings to allocate
+ *  @rxr_idx: index of first Rx ring to allocate
  *
- * We allocate one q_vector.  If allocation fails we return -ENOMEM.
+ *  We allocate one q_vector.  If allocation fails we return -ENOMEM.
  **/
 static int igb_alloc_q_vector(struct igb_adapter *adapter,
 			      int v_count, int v_idx,
@@ -1179,6 +1191,17 @@ static int igb_alloc_q_vector(struct igb_adapter *adapter,
 	/* initialize pointer to rings */
 	ring = q_vector->ring;
 
+	/* intialize ITR */
+	if (rxr_count) {
+		/* rx or rx/tx vector */
+		if (!adapter->rx_itr_setting || adapter->rx_itr_setting > 3)
+			q_vector->itr_val = adapter->rx_itr_setting;
+	} else {
+		/* tx only vector */
+		if (!adapter->tx_itr_setting || adapter->tx_itr_setting > 3)
+			q_vector->itr_val = adapter->tx_itr_setting;
+	}
+
 	if (txr_count) {
 		/* assign generic ring traits */
 		ring->dev = &adapter->pdev->dev;
@@ -1221,9 +1244,9 @@ static int igb_alloc_q_vector(struct igb_adapter *adapter,
 			set_bit(IGB_RING_FLAG_RX_SCTP_CSUM, &ring->flags);
 
 		/*
-		 * On i350, i210, and i211, loopback VLAN packets
+		 * On i350, i354, i210, and i211, loopback VLAN packets
 		 * have the tag byte-swapped.
-		 * */
+		 */
 		if (adapter->hw.mac.type >= e1000_i350)
 			set_bit(IGB_RING_FLAG_RX_LB_VLAN_BSWAP, &ring->flags);
 
@@ -1240,11 +1263,11 @@ static int igb_alloc_q_vector(struct igb_adapter *adapter,
 
 
 /**
- * igb_alloc_q_vectors - Allocate memory for interrupt vectors
- * @adapter: board private structure to initialize
+ *  igb_alloc_q_vectors - Allocate memory for interrupt vectors
+ *  @adapter: board private structure to initialize
  *
- * We allocate one q_vector per queue interrupt.  If allocation fails we
- * return -ENOMEM.
+ *  We allocate one q_vector per queue interrupt.  If allocation fails we
+ *  return -ENOMEM.
  **/
 static int igb_alloc_q_vectors(struct igb_adapter *adapter)
 {
@@ -1298,9 +1321,11 @@ err_out:
 }
 
 /**
- * igb_init_interrupt_scheme - initialize interrupts, allocate queues/vectors
+ *  igb_init_interrupt_scheme - initialize interrupts, allocate queues/vectors
+ *  @adapter: board private structure to initialize
+ *  @msix: boolean value of MSIX capability
  *
- * This function initializes the interrupts and allocates all of the queues.
+ *  This function initializes the interrupts and allocates all of the queues.
  **/
 static int igb_init_interrupt_scheme(struct igb_adapter *adapter, bool msix)
 {
@@ -1325,10 +1350,11 @@ err_alloc_q_vectors:
 }
 
 /**
- * igb_request_irq - initialize interrupts
+ *  igb_request_irq - initialize interrupts
+ *  @adapter: board private structure to initialize
  *
- * Attempts to configure interrupts using the best available
- * capabilities of the hardware and kernel.
+ *  Attempts to configure interrupts using the best available
+ *  capabilities of the hardware and kernel.
  **/
 static int igb_request_irq(struct igb_adapter *adapter)
 {
@@ -1394,15 +1420,14 @@ static void igb_free_irq(struct igb_adapter *adapter)
 }
 
 /**
- * igb_irq_disable - Mask off interrupt generation on the NIC
- * @adapter: board private structure
+ *  igb_irq_disable - Mask off interrupt generation on the NIC
+ *  @adapter: board private structure
  **/
 static void igb_irq_disable(struct igb_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
 
-	/*
-	 * we need to be careful when disabling interrupts.  The VFs are also
+	/* we need to be careful when disabling interrupts.  The VFs are also
 	 * mapped into these registers and so clearing the bits can cause
 	 * issues on the VF drivers so we only need to clear what we set
 	 */
@@ -1427,8 +1452,8 @@ static void igb_irq_disable(struct igb_adapter *adapter)
 }
 
 /**
- * igb_irq_enable - Enable default interrupt generation settings
- * @adapter: board private structure
+ *  igb_irq_enable - Enable default interrupt generation settings
+ *  @adapter: board private structure
  **/
 static void igb_irq_enable(struct igb_adapter *adapter)
 {
@@ -1477,13 +1502,12 @@ static void igb_update_mng_vlan(struct igb_adapter *adapter)
 }
 
 /**
- * igb_release_hw_control - release control of the h/w to f/w
- * @adapter: address of board private structure
- *
- * igb_release_hw_control resets CTRL_EXT:DRV_LOAD bit.
- * For ASF and Pass Through versions of f/w this means that the
- * driver is no longer loaded.
+ *  igb_release_hw_control - release control of the h/w to f/w
+ *  @adapter: address of board private structure
  *
+ *  igb_release_hw_control resets CTRL_EXT:DRV_LOAD bit.
+ *  For ASF and Pass Through versions of f/w this means that the
+ *  driver is no longer loaded.
  **/
 static void igb_release_hw_control(struct igb_adapter *adapter)
 {
@@ -1497,13 +1521,12 @@ static void igb_release_hw_control(struct igb_adapter *adapter)
 }
 
 /**
- * igb_get_hw_control - get control of the h/w from f/w
- * @adapter: address of board private structure
- *
- * igb_get_hw_control sets CTRL_EXT:DRV_LOAD bit.
- * For ASF and Pass Through versions of f/w this means that
- * the driver is loaded.
+ *  igb_get_hw_control - get control of the h/w from f/w
+ *  @adapter: address of board private structure
  *
+ *  igb_get_hw_control sets CTRL_EXT:DRV_LOAD bit.
+ *  For ASF and Pass Through versions of f/w this means that
+ *  the driver is loaded.
  **/
 static void igb_get_hw_control(struct igb_adapter *adapter)
 {
@@ -1517,8 +1540,8 @@ static void igb_get_hw_control(struct igb_adapter *adapter)
 }
 
 /**
- * igb_configure - configure the hardware for RX and TX
- * @adapter: private board structure
+ *  igb_configure - configure the hardware for RX and TX
+ *  @adapter: private board structure
  **/
 static void igb_configure(struct igb_adapter *adapter)
 {
@@ -1541,7 +1564,8 @@ static void igb_configure(struct igb_adapter *adapter)
 
 	/* call igb_desc_unused which always leaves
 	 * at least 1 descriptor unused to make sure
-	 * next_to_use != next_to_clean */
+	 * next_to_use != next_to_clean
+	 */
 	for (i = 0; i < adapter->num_rx_queues; i++) {
 		struct igb_ring *ring = adapter->rx_ring[i];
 		igb_alloc_rx_buffers(ring, igb_desc_unused(ring));
@@ -1549,8 +1573,8 @@ static void igb_configure(struct igb_adapter *adapter)
 }
 
 /**
- * igb_power_up_link - Power up the phy/serdes link
- * @adapter: address of board private structure
+ *  igb_power_up_link - Power up the phy/serdes link
+ *  @adapter: address of board private structure
  **/
 void igb_power_up_link(struct igb_adapter *adapter)
 {
@@ -1563,8 +1587,8 @@ void igb_power_up_link(struct igb_adapter *adapter)
 }
 
 /**
- * igb_power_down_link - Power down the phy/serdes link
- * @adapter: address of board private structure
+ *  igb_power_down_link - Power down the phy/serdes link
+ *  @adapter: address of board private structure
  */
 static void igb_power_down_link(struct igb_adapter *adapter)
 {
@@ -1575,8 +1599,8 @@ static void igb_power_down_link(struct igb_adapter *adapter)
 }
 
 /**
- * igb_up - Open the interface and prepare it to handle traffic
- * @adapter: board private structure
+ *  igb_up - Open the interface and prepare it to handle traffic
+ *  @adapter: board private structure
  **/
 int igb_up(struct igb_adapter *adapter)
 {
@@ -1624,7 +1648,8 @@ void igb_down(struct igb_adapter *adapter)
 	int i;
 
 	/* signal that we're down so the interrupt handler does not
-	 * reschedule our watchdog timer */
+	 * reschedule our watchdog timer
+	 */
 	set_bit(__IGB_DOWN, &adapter->state);
 
 	/* disable receives in the hardware */
@@ -1694,6 +1719,7 @@ void igb_reset(struct igb_adapter *adapter)
 	 */
 	switch (mac->type) {
 	case e1000_i350:
+	case e1000_i354:
 	case e1000_82580:
 		pba = rd32(E1000_RXPBS);
 		pba = igb_rxpbs_adjust_82580(pba);
@@ -1720,14 +1746,16 @@ void igb_reset(struct igb_adapter *adapter)
 		 * rounded up to the next 1KB and expressed in KB.  Likewise,
 		 * the Rx FIFO should be large enough to accommodate at least
 		 * one full receive packet and is similarly rounded up and
-		 * expressed in KB. */
+		 * expressed in KB.
+		 */
 		pba = rd32(E1000_PBA);
 		/* upper 16 bits has Tx packet buffer allocation size in KB */
 		tx_space = pba >> 16;
 		/* lower 16 bits has Rx packet buffer allocation size in KB */
 		pba &= 0xffff;
-		/* the tx fifo also stores 16 bytes of information about the tx
-		 * but don't include ethernet FCS because hardware appends it */
+		/* the Tx fifo also stores 16 bytes of information about the Tx
+		 * but don't include ethernet FCS because hardware appends it
+		 */
 		min_tx_space = (adapter->max_frame_size +
 				sizeof(union e1000_adv_tx_desc) -
 				ETH_FCS_LEN) * 2;
@@ -1740,13 +1768,15 @@ void igb_reset(struct igb_adapter *adapter)
 
 		/* If current Tx allocation is less than the min Tx FIFO size,
 		 * and the min Tx FIFO size is less than the current Rx FIFO
-		 * allocation, take space away from current Rx allocation */
+		 * allocation, take space away from current Rx allocation
+		 */
 		if (tx_space < min_tx_space &&
 		    ((min_tx_space - tx_space) < pba)) {
 			pba = pba - (min_tx_space - tx_space);
 
-			/* if short on rx space, rx wins and must trump tx
-			 * adjustment */
+			/* if short on Rx space, Rx wins and must trump Tx
+			 * adjustment
+			 */
 			if (pba < min_rx_space)
 				pba = min_rx_space;
 		}
@@ -1758,7 +1788,8 @@ void igb_reset(struct igb_adapter *adapter)
 	 * (or the size used for early receive) above it in the Rx FIFO.
 	 * Set it to the lower of:
 	 * - 90% of the Rx FIFO size, or
-	 * - the full Rx FIFO size minus one full frame */
+	 * - the full Rx FIFO size minus one full frame
+	 */
 	hwm = min(((pba << 10) * 9 / 10),
 			((pba << 10) - 2 * adapter->max_frame_size));
 
@@ -1789,8 +1820,7 @@ void igb_reset(struct igb_adapter *adapter)
 	if (hw->mac.ops.init_hw(hw))
 		dev_err(&pdev->dev, "Hardware Error\n");
 
-	/*
-	 * Flow control settings reset on hardware reset, so guarantee flow
+	/* Flow control settings reset on hardware reset, so guarantee flow
 	 * control is off when forcing speed.
 	 */
 	if (!hw->mac.autoneg)
@@ -1826,14 +1856,13 @@ void igb_reset(struct igb_adapter *adapter)
 static netdev_features_t igb_fix_features(struct net_device *netdev,
 	netdev_features_t features)
 {
-	/*
-	 * Since there is no support for separate rx/tx vlan accel
-	 * enable/disable make sure tx flag is always in same state as rx.
+	/* Since there is no support for separate Rx/Tx vlan accel
+	 * enable/disable make sure Tx flag is always in same state as Rx.
 	 */
-	if (features & NETIF_F_HW_VLAN_RX)
-		features |= NETIF_F_HW_VLAN_TX;
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
+		features |= NETIF_F_HW_VLAN_CTAG_TX;
 	else
-		features &= ~NETIF_F_HW_VLAN_TX;
+		features &= ~NETIF_F_HW_VLAN_CTAG_TX;
 
 	return features;
 }
@@ -1844,7 +1873,7 @@ static int igb_set_features(struct net_device *netdev,
 	netdev_features_t changed = netdev->features ^ features;
 	struct igb_adapter *adapter = netdev_priv(netdev);
 
-	if (changed & NETIF_F_HW_VLAN_RX)
+	if (changed & NETIF_F_HW_VLAN_CTAG_RX)
 		igb_vlan_mode(netdev, features);
 
 	if (!(changed & NETIF_F_RXALL))
@@ -1876,6 +1905,7 @@ static const struct net_device_ops igb_netdev_ops = {
 	.ndo_set_vf_mac		= igb_ndo_set_vf_mac,
 	.ndo_set_vf_vlan	= igb_ndo_set_vf_vlan,
 	.ndo_set_vf_tx_rate	= igb_ndo_set_vf_bw,
+	.ndo_set_vf_spoofchk	= igb_ndo_set_vf_spoofchk,
 	.ndo_get_vf_config	= igb_ndo_get_vf_config,
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller	= igb_netpoll,
@@ -1887,7 +1917,6 @@ static const struct net_device_ops igb_netdev_ops = {
 /**
  * igb_set_fw_version - Configure version string for ethtool
  * @adapter: adapter struct
- *
  **/
 void igb_set_fw_version(struct igb_adapter *adapter)
 {
@@ -1923,10 +1952,10 @@ void igb_set_fw_version(struct igb_adapter *adapter)
 	return;
 }
 
-/*  igb_init_i2c - Init I2C interface
+/**
+ *  igb_init_i2c - Init I2C interface
  *  @adapter: pointer to adapter structure
- *
- */
+ **/
 static s32 igb_init_i2c(struct igb_adapter *adapter)
 {
 	s32 status = E1000_SUCCESS;
@@ -1951,15 +1980,15 @@ static s32 igb_init_i2c(struct igb_adapter *adapter)
 }
 
 /**
- * igb_probe - Device Initialization Routine
- * @pdev: PCI device information struct
- * @ent: entry in igb_pci_tbl
+ *  igb_probe - Device Initialization Routine
+ *  @pdev: PCI device information struct
+ *  @ent: entry in igb_pci_tbl
  *
- * Returns 0 on success, negative on failure
+ *  Returns 0 on success, negative on failure
  *
- * igb_probe initializes an adapter identified by a pci_dev structure.
- * The OS initialization, configuring of the adapter private structure,
- * and a hardware reset occur.
+ *  igb_probe initializes an adapter identified by a pci_dev structure.
+ *  The OS initialization, configuring of the adapter private structure,
+ *  and a hardware reset occur.
  **/
 static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
@@ -1996,18 +2025,19 @@ static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	} else {
 		err = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
 		if (err) {
-			err = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
+			err = dma_set_coherent_mask(&pdev->dev,
+						    DMA_BIT_MASK(32));
 			if (err) {
-				dev_err(&pdev->dev, "No usable DMA "
-					"configuration, aborting\n");
+				dev_err(&pdev->dev,
+					"No usable DMA configuration, aborting\n");
 				goto err_dma;
 			}
 		}
 	}
 
 	err = pci_request_selected_regions(pdev, pci_select_bars(pdev,
-	                                   IORESOURCE_MEM),
-	                                   igb_driver_name);
+					   IORESOURCE_MEM),
+					   igb_driver_name);
 	if (err)
 		goto err_pci_reg;
 
@@ -2085,8 +2115,7 @@ static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		dev_info(&pdev->dev,
 			"PHY reset is blocked due to SOL/IDER session.\n");
 
-	/*
-	 * features is initialized to 0 in allocation, it might have bits
+	/* features is initialized to 0 in allocation, it might have bits
 	 * set by igb_sw_init so we should use an or instead of an
 	 * assignment.
 	 */
@@ -2097,15 +2126,15 @@ static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 			    NETIF_F_TSO6 |
 			    NETIF_F_RXHASH |
 			    NETIF_F_RXCSUM |
-			    NETIF_F_HW_VLAN_RX |
-			    NETIF_F_HW_VLAN_TX;
+			    NETIF_F_HW_VLAN_CTAG_RX |
+			    NETIF_F_HW_VLAN_CTAG_TX;
 
 	/* copy netdev features into list of user selectable features */
 	netdev->hw_features |= netdev->features;
 	netdev->hw_features |= NETIF_F_RXALL;
 
 	/* set this bit last since it cannot be part of hw_features */
-	netdev->features |= NETIF_F_HW_VLAN_FILTER;
+	netdev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
 
 	netdev->vlan_features |= NETIF_F_TSO |
 				 NETIF_F_TSO6 |
@@ -2130,11 +2159,11 @@ static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	adapter->en_mng_pt = igb_enable_mng_pass_thru(hw);
 
 	/* before reading the NVM, reset the controller to put the device in a
-	 * known good starting state */
+	 * known good starting state
+	 */
 	hw->mac.ops.reset_hw(hw);
 
-	/*
-	 * make sure the NVM is good , i211 parts have special NVM that
+	/* make sure the NVM is good , i211 parts have special NVM that
 	 * doesn't contain a checksum
 	 */
 	if (hw->mac.type != e1000_i211) {
@@ -2161,9 +2190,9 @@ static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	igb_set_fw_version(adapter);
 
 	setup_timer(&adapter->watchdog_timer, igb_watchdog,
-	            (unsigned long) adapter);
+		    (unsigned long) adapter);
 	setup_timer(&adapter->phy_info_timer, igb_update_phy_info,
-	            (unsigned long) adapter);
+		    (unsigned long) adapter);
 
 	INIT_WORK(&adapter->reset_task, igb_reset_task);
 	INIT_WORK(&adapter->watchdog_task, igb_watchdog_task);
@@ -2185,8 +2214,8 @@ static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	/* Check the NVM for wake support on non-port A ports */
 	if (hw->mac.type >= e1000_82580)
 		hw->nvm.ops.read(hw, NVM_INIT_CONTROL3_PORT_A +
-		                 NVM_82580_LAN_FUNC_OFFSET(hw->bus.func), 1,
-		                 &eeprom_data);
+				 NVM_82580_LAN_FUNC_OFFSET(hw->bus.func), 1,
+				 &eeprom_data);
 	else if (hw->bus.func == 1)
 		hw->nvm.ops.read(hw, NVM_INIT_CONTROL3_PORT_B, 1, &eeprom_data);
 
@@ -2195,7 +2224,8 @@ static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	/* now that we have the eeprom settings, apply the special cases where
 	 * the eeprom may be wrong or the board simply won't support wake on
-	 * lan on a particular port */
+	 * lan on a particular port
+	 */
 	switch (pdev->device) {
 	case E1000_DEV_ID_82575GB_QUAD_COPPER:
 		adapter->flags &= ~IGB_FLAG_WOL_SUPPORTED;
@@ -2204,7 +2234,8 @@ static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	case E1000_DEV_ID_82576_FIBER:
 	case E1000_DEV_ID_82576_SERDES:
 		/* Wake events only supported on port A for dual fiber
-		 * regardless of eeprom setting */
+		 * regardless of eeprom setting
+		 */
 		if (rd32(E1000_STATUS) & E1000_STATUS_FUNC_1)
 			adapter->flags &= ~IGB_FLAG_WOL_SUPPORTED;
 		break;
@@ -2274,8 +2305,7 @@ static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (hw->mac.type == e1000_i350 && hw->bus.func == 0) {
 		u16 ets_word;
 
-		/*
-		 * Read the NVM to determine if this i350 device supports an
+		/* Read the NVM to determine if this i350 device supports an
 		 * external thermal sensor.
 		 */
 		hw->nvm.ops.read(hw, NVM_ETS_CFG, 1, &ets_word);
@@ -2294,17 +2324,20 @@ static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	igb_ptp_init(adapter);
 
 	dev_info(&pdev->dev, "Intel(R) Gigabit Ethernet Network Connection\n");
-	/* print bus type/speed/width info */
-	dev_info(&pdev->dev, "%s: (PCIe:%s:%s) %pM\n",
-		 netdev->name,
-		 ((hw->bus.speed == e1000_bus_speed_2500) ? "2.5Gb/s" :
-		  (hw->bus.speed == e1000_bus_speed_5000) ? "5.0Gb/s" :
-		                                            "unknown"),
-		 ((hw->bus.width == e1000_bus_width_pcie_x4) ? "Width x4" :
-		  (hw->bus.width == e1000_bus_width_pcie_x2) ? "Width x2" :
-		  (hw->bus.width == e1000_bus_width_pcie_x1) ? "Width x1" :
-		   "unknown"),
-		 netdev->dev_addr);
+	/* print bus type/speed/width info, not applicable to i354 */
+	if (hw->mac.type != e1000_i354) {
+		dev_info(&pdev->dev, "%s: (PCIe:%s:%s) %pM\n",
+			 netdev->name,
+			 ((hw->bus.speed == e1000_bus_speed_2500) ? "2.5Gb/s" :
+			  (hw->bus.speed == e1000_bus_speed_5000) ? "5.0Gb/s" :
+			   "unknown"),
+			 ((hw->bus.width == e1000_bus_width_pcie_x4) ?
+			  "Width x4" :
+			  (hw->bus.width == e1000_bus_width_pcie_x2) ?
+			  "Width x2" :
+			  (hw->bus.width == e1000_bus_width_pcie_x1) ?
+			  "Width x1" : "unknown"), netdev->dev_addr);
+	}
 
 	ret_val = igb_read_part_string(hw, part_str, E1000_PBANUM_LENGTH);
 	if (ret_val)
@@ -2321,6 +2354,13 @@ static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	case e1000_i211:
 		igb_set_eee_i350(hw);
 		break;
+	case e1000_i354:
+		if (hw->phy.media_type == e1000_media_type_copper) {
+			if ((rd32(E1000_CTRL_EXT) &
+			    E1000_CTRL_EXT_LINK_MODE_SGMII))
+				igb_set_eee_i354(hw);
+		}
+		break;
 	default:
 		break;
 	}
@@ -2344,7 +2384,7 @@ err_ioremap:
 	free_netdev(netdev);
 err_alloc_etherdev:
 	pci_release_selected_regions(pdev,
-	                             pci_select_bars(pdev, IORESOURCE_MEM));
+				     pci_select_bars(pdev, IORESOURCE_MEM));
 err_pci_reg:
 err_dma:
 	pci_disable_device(pdev);
@@ -2361,7 +2401,7 @@ static int  igb_disable_sriov(struct pci_dev *pdev)
 	/* reclaim resources allocated to VFs */
 	if (adapter->vf_data) {
 		/* disable iov and allow time for transactions to clear */
-		if (igb_vfs_are_assigned(adapter)) {
+		if (pci_vfs_assigned(pdev)) {
 			dev_warn(&pdev->dev,
 				 "Cannot deallocate SR-IOV virtual functions while they are assigned - VFs will not be deallocated\n");
 			return -EPERM;
@@ -2444,26 +2484,24 @@ out:
 }
 
 #endif
-/*
+/**
  *  igb_remove_i2c - Cleanup  I2C interface
  *  @adapter: pointer to adapter structure
- *
- */
+ **/
 static void igb_remove_i2c(struct igb_adapter *adapter)
 {
-
 	/* free the adapter bus structure */
 	i2c_del_adapter(&adapter->i2c_adap);
 }
 
 /**
- * igb_remove - Device Removal Routine
- * @pdev: PCI device information struct
+ *  igb_remove - Device Removal Routine
+ *  @pdev: PCI device information struct
  *
- * igb_remove is called by the PCI subsystem to alert the driver
- * that it should release a PCI device.  The could be caused by a
- * Hot-Plug event, or because the driver is going to be removed from
- * memory.
+ *  igb_remove is called by the PCI subsystem to alert the driver
+ *  that it should release a PCI device.  The could be caused by a
+ *  Hot-Plug event, or because the driver is going to be removed from
+ *  memory.
  **/
 static void igb_remove(struct pci_dev *pdev)
 {
@@ -2477,8 +2515,7 @@ static void igb_remove(struct pci_dev *pdev)
 #endif
 	igb_remove_i2c(adapter);
 	igb_ptp_stop(adapter);
-	/*
-	 * The watchdog timer may be rescheduled, so explicitly
+	/* The watchdog timer may be rescheduled, so explicitly
 	 * disable watchdog from being rescheduled.
 	 */
 	set_bit(__IGB_DOWN, &adapter->state);
@@ -2498,7 +2535,8 @@ static void igb_remove(struct pci_dev *pdev)
 #endif
 
 	/* Release control of h/w to f/w.  If f/w is AMT enabled, this
-	 * would have already happened in close and is redundant. */
+	 * would have already happened in close and is redundant.
+	 */
 	igb_release_hw_control(adapter);
 
 	unregister_netdev(netdev);
@@ -2513,7 +2551,7 @@ static void igb_remove(struct pci_dev *pdev)
 	if (hw->flash_address)
 		iounmap(hw->flash_address);
 	pci_release_selected_regions(pdev,
-	                             pci_select_bars(pdev, IORESOURCE_MEM));
+				     pci_select_bars(pdev, IORESOURCE_MEM));
 
 	kfree(adapter->shadow_vfta);
 	free_netdev(netdev);
@@ -2524,13 +2562,13 @@ static void igb_remove(struct pci_dev *pdev)
 }
 
 /**
- * igb_probe_vfs - Initialize vf data storage and add VFs to pci config space
- * @adapter: board private structure to initialize
+ *  igb_probe_vfs - Initialize vf data storage and add VFs to pci config space
+ *  @adapter: board private structure to initialize
  *
- * This function initializes the vf specific data storage and then attempts to
- * allocate the VFs.  The reason for ordering it this way is because it is much
- * mor expensive time wise to disable SR-IOV than it is to allocate and free
- * the memory for the VFs.
+ *  This function initializes the vf specific data storage and then attempts to
+ *  allocate the VFs.  The reason for ordering it this way is because it is much
+ *  mor expensive time wise to disable SR-IOV than it is to allocate and free
+ *  the memory for the VFs.
  **/
 static void igb_probe_vfs(struct igb_adapter *adapter)
 {
@@ -2576,6 +2614,7 @@ static void igb_init_queue_configuration(struct igb_adapter *adapter)
 		}
 		/* fall through */
 	case e1000_82580:
+	case e1000_i354:
 	default:
 		max_rss_queues = IGB_MAX_RX_QUEUES;
 		break;
@@ -2590,8 +2629,7 @@ static void igb_init_queue_configuration(struct igb_adapter *adapter)
 		/* Device supports enough interrupts without queue pairing. */
 		break;
 	case e1000_82576:
-		/*
-		 * If VFs are going to be allocated with RSS queues then we
+		/* If VFs are going to be allocated with RSS queues then we
 		 * should pair the queues in order to conserve interrupts due
 		 * to limited supply.
 		 */
@@ -2601,10 +2639,10 @@ static void igb_init_queue_configuration(struct igb_adapter *adapter)
 		/* fall through */
 	case e1000_82580:
 	case e1000_i350:
+	case e1000_i354:
 	case e1000_i210:
 	default:
-		/*
-		 * If rss_queues > half of max_rss_queues, pair the queues in
+		/* If rss_queues > half of max_rss_queues, pair the queues in
 		 * order to conserve interrupts due to limited supply.
 		 */
 		if (adapter->rss_queues > (max_rss_queues / 2))
@@ -2614,12 +2652,12 @@ static void igb_init_queue_configuration(struct igb_adapter *adapter)
 }
 
 /**
- * igb_sw_init - Initialize general software structures (struct igb_adapter)
- * @adapter: board private structure to initialize
+ *  igb_sw_init - Initialize general software structures (struct igb_adapter)
+ *  @adapter: board private structure to initialize
  *
- * igb_sw_init initializes the Adapter private data structure.
- * Fields are initialized based on PCI device information and
- * OS network device settings (MTU size).
+ *  igb_sw_init initializes the Adapter private data structure.
+ *  Fields are initialized based on PCI device information and
+ *  OS network device settings (MTU size).
  **/
 static int igb_sw_init(struct igb_adapter *adapter)
 {
@@ -2689,16 +2727,16 @@ static int igb_sw_init(struct igb_adapter *adapter)
 }
 
 /**
- * igb_open - Called when a network interface is made active
- * @netdev: network interface device structure
+ *  igb_open - Called when a network interface is made active
+ *  @netdev: network interface device structure
  *
- * Returns 0 on success, negative value on failure
+ *  Returns 0 on success, negative value on failure
  *
- * The open entry point is called when a network interface is made
- * active by the system (IFF_UP).  At this point all resources needed
- * for transmit and receive operations are allocated, the interrupt
- * handler is registered with the OS, the watchdog timer is started,
- * and the stack is notified that the interface is ready.
+ *  The open entry point is called when a network interface is made
+ *  active by the system (IFF_UP).  At this point all resources needed
+ *  for transmit and receive operations are allocated, the interrupt
+ *  handler is registered with the OS, the watchdog timer is started,
+ *  and the stack is notified that the interface is ready.
  **/
 static int __igb_open(struct net_device *netdev, bool resuming)
 {
@@ -2734,7 +2772,8 @@ static int __igb_open(struct net_device *netdev, bool resuming)
 	/* before we allocate an interrupt, we must be ready to handle it.
 	 * Setting DEBUG_SHIRQ in the kernel makes it fire an interrupt
 	 * as soon as we call pci_request_irq, so we have to setup our
-	 * clean_rx handler before we do so.  */
+	 * clean_rx handler before we do so.
+	 */
 	igb_configure(adapter);
 
 	err = igb_request_irq(adapter);
@@ -2803,15 +2842,15 @@ static int igb_open(struct net_device *netdev)
 }
 
 /**
- * igb_close - Disables a network interface
- * @netdev: network interface device structure
+ *  igb_close - Disables a network interface
+ *  @netdev: network interface device structure
  *
- * Returns 0, this is not allowed to fail
+ *  Returns 0, this is not allowed to fail
  *
- * The close entry point is called when an interface is de-activated
- * by the OS.  The hardware is still under the driver's control, but
- * needs to be disabled.  A global MAC reset is issued to stop the
- * hardware, and all transmit and receive resources are freed.
+ *  The close entry point is called when an interface is de-activated
+ *  by the OS.  The hardware is still under the driver's control, but
+ *  needs to be disabled.  A global MAC reset is issued to stop the
+ *  hardware, and all transmit and receive resources are freed.
  **/
 static int __igb_close(struct net_device *netdev, bool suspending)
 {
@@ -2840,10 +2879,10 @@ static int igb_close(struct net_device *netdev)
 }
 
 /**
- * igb_setup_tx_resources - allocate Tx resources (Descriptors)
- * @tx_ring: tx descriptor ring (for a specific queue) to setup
+ *  igb_setup_tx_resources - allocate Tx resources (Descriptors)
+ *  @tx_ring: tx descriptor ring (for a specific queue) to setup
  *
- * Return 0 on success, negative on failure
+ *  Return 0 on success, negative on failure
  **/
 int igb_setup_tx_resources(struct igb_ring *tx_ring)
 {
@@ -2878,11 +2917,11 @@ err:
 }
 
 /**
- * igb_setup_all_tx_resources - wrapper to allocate Tx resources
- *				  (Descriptors) for all queues
- * @adapter: board private structure
+ *  igb_setup_all_tx_resources - wrapper to allocate Tx resources
+ *				 (Descriptors) for all queues
+ *  @adapter: board private structure
  *
- * Return 0 on success, negative on failure
+ *  Return 0 on success, negative on failure
  **/
 static int igb_setup_all_tx_resources(struct igb_adapter *adapter)
 {
@@ -2904,8 +2943,8 @@ static int igb_setup_all_tx_resources(struct igb_adapter *adapter)
 }
 
 /**
- * igb_setup_tctl - configure the transmit control registers
- * @adapter: Board private structure
+ *  igb_setup_tctl - configure the transmit control registers
+ *  @adapter: Board private structure
  **/
 void igb_setup_tctl(struct igb_adapter *adapter)
 {
@@ -2930,11 +2969,11 @@ void igb_setup_tctl(struct igb_adapter *adapter)
 }
 
 /**
- * igb_configure_tx_ring - Configure transmit ring after Reset
- * @adapter: board private structure
- * @ring: tx ring to configure
+ *  igb_configure_tx_ring - Configure transmit ring after Reset
+ *  @adapter: board private structure
+ *  @ring: tx ring to configure
  *
- * Configure a transmit ring after a reset.
+ *  Configure a transmit ring after a reset.
  **/
 void igb_configure_tx_ring(struct igb_adapter *adapter,
                            struct igb_ring *ring)
@@ -2950,9 +2989,9 @@ void igb_configure_tx_ring(struct igb_adapter *adapter,
 	mdelay(10);
 
 	wr32(E1000_TDLEN(reg_idx),
-	                ring->count * sizeof(union e1000_adv_tx_desc));
+	     ring->count * sizeof(union e1000_adv_tx_desc));
 	wr32(E1000_TDBAL(reg_idx),
-	                tdba & 0x00000000ffffffffULL);
+	     tdba & 0x00000000ffffffffULL);
 	wr32(E1000_TDBAH(reg_idx), tdba >> 32);
 
 	ring->tail = hw->hw_addr + E1000_TDT(reg_idx);
@@ -2968,10 +3007,10 @@ void igb_configure_tx_ring(struct igb_adapter *adapter,
 }
 
 /**
- * igb_configure_tx - Configure transmit Unit after Reset
- * @adapter: board private structure
+ *  igb_configure_tx - Configure transmit Unit after Reset
+ *  @adapter: board private structure
  *
- * Configure the Tx unit of the MAC after a reset.
+ *  Configure the Tx unit of the MAC after a reset.
  **/
 static void igb_configure_tx(struct igb_adapter *adapter)
 {
@@ -2982,10 +3021,10 @@ static void igb_configure_tx(struct igb_adapter *adapter)
 }
 
 /**
- * igb_setup_rx_resources - allocate Rx resources (Descriptors)
- * @rx_ring:    rx descriptor ring (for a specific queue) to setup
+ *  igb_setup_rx_resources - allocate Rx resources (Descriptors)
+ *  @rx_ring: Rx descriptor ring (for a specific queue) to setup
  *
- * Returns 0 on success, negative on failure
+ *  Returns 0 on success, negative on failure
  **/
 int igb_setup_rx_resources(struct igb_ring *rx_ring)
 {
@@ -3021,11 +3060,11 @@ err:
 }
 
 /**
- * igb_setup_all_rx_resources - wrapper to allocate Rx resources
- *				  (Descriptors) for all queues
- * @adapter: board private structure
+ *  igb_setup_all_rx_resources - wrapper to allocate Rx resources
+ *				 (Descriptors) for all queues
+ *  @adapter: board private structure
  *
- * Return 0 on success, negative on failure
+ *  Return 0 on success, negative on failure
  **/
 static int igb_setup_all_rx_resources(struct igb_adapter *adapter)
 {
@@ -3047,8 +3086,8 @@ static int igb_setup_all_rx_resources(struct igb_adapter *adapter)
 }
 
 /**
- * igb_setup_mrqc - configure the multiple receive queue control registers
- * @adapter: Board private structure
+ *  igb_setup_mrqc - configure the multiple receive queue control registers
+ *  @adapter: Board private structure
  **/
 static void igb_setup_mrqc(struct igb_adapter *adapter)
 {
@@ -3081,8 +3120,7 @@ static void igb_setup_mrqc(struct igb_adapter *adapter)
 		break;
 	}
 
-	/*
-	 * Populate the indirection table 4 entries at a time.  To do this
+	/* Populate the indirection table 4 entries at a time.  To do this
 	 * we are generating the results for n and n+2 and then interleaving
 	 * those with the results with n+1 and n+3.
 	 */
@@ -3098,8 +3136,7 @@ static void igb_setup_mrqc(struct igb_adapter *adapter)
 		wr32(E1000_RETA(j), reta);
 	}
 
-	/*
-	 * Disable raw packet checksumming so that RSS hash is placed in
+	/* Disable raw packet checksumming so that RSS hash is placed in
 	 * descriptor on writeback.  No need to enable TCP/UDP/IP checksum
 	 * offloads as they are enabled by default
 	 */
@@ -3129,7 +3166,8 @@ static void igb_setup_mrqc(struct igb_adapter *adapter)
 
 	/* If VMDq is enabled then we set the appropriate mode for that, else
 	 * we default to RSS so that an RSS hash is calculated per packet even
-	 * if we are only using one queue */
+	 * if we are only using one queue
+	 */
 	if (adapter->vfs_allocated_count) {
 		if (hw->mac.type > e1000_82575) {
 			/* Set the default pool for the PF's first queue */
@@ -3154,8 +3192,8 @@ static void igb_setup_mrqc(struct igb_adapter *adapter)
 }
 
 /**
- * igb_setup_rctl - configure the receive control registers
- * @adapter: Board private structure
+ *  igb_setup_rctl - configure the receive control registers
+ *  @adapter: Board private structure
  **/
 void igb_setup_rctl(struct igb_adapter *adapter)
 {
@@ -3170,8 +3208,7 @@ void igb_setup_rctl(struct igb_adapter *adapter)
 	rctl |= E1000_RCTL_EN | E1000_RCTL_BAM | E1000_RCTL_RDMTS_HALF |
 		(hw->mac.mc_filter_type << E1000_RCTL_MO_SHIFT);
 
-	/*
-	 * enable stripping of CRC. It's unlikely this will break BMC
+	/* enable stripping of CRC. It's unlikely this will break BMC
 	 * redirection as it did with e1000. Newer features require
 	 * that the HW strips the CRC.
 	 */
@@ -3198,7 +3235,8 @@ void igb_setup_rctl(struct igb_adapter *adapter)
 	/* This is useful for sniffing bad packets. */
 	if (adapter->netdev->features & NETIF_F_RXALL) {
 		/* UPE and MPE will be handled by normal PROMISC logic
-		 * in e1000e_set_rx_mode */
+		 * in e1000e_set_rx_mode
+		 */
 		rctl |= (E1000_RCTL_SBP | /* Receive bad packets */
 			 E1000_RCTL_BAM | /* RX All Bcast Pkts */
 			 E1000_RCTL_PMCF); /* RX All MAC Ctrl Pkts */
@@ -3221,7 +3259,8 @@ static inline int igb_set_vf_rlpml(struct igb_adapter *adapter, int size,
 	u32 vmolr;
 
 	/* if it isn't the PF check to see if VFs are enabled and
-	 * increase the size to support vlan tags */
+	 * increase the size to support vlan tags
+	 */
 	if (vfn < adapter->vfs_allocated_count &&
 	    adapter->vf_data[vfn].vlans_enabled)
 		size += VLAN_TAG_SIZE;
@@ -3235,10 +3274,10 @@ static inline int igb_set_vf_rlpml(struct igb_adapter *adapter, int size,
 }
 
 /**
- * igb_rlpml_set - set maximum receive packet size
- * @adapter: board private structure
+ *  igb_rlpml_set - set maximum receive packet size
+ *  @adapter: board private structure
  *
- * Configure maximum receivable packet size.
+ *  Configure maximum receivable packet size.
  **/
 static void igb_rlpml_set(struct igb_adapter *adapter)
 {
@@ -3248,8 +3287,7 @@ static void igb_rlpml_set(struct igb_adapter *adapter)
 
 	if (pf_id) {
 		igb_set_vf_rlpml(adapter, max_frame_size, pf_id);
-		/*
-		 * If we're in VMDQ or SR-IOV mode, then set global RLPML
+		/* If we're in VMDQ or SR-IOV mode, then set global RLPML
 		 * to our max jumbo frame size, in case we need to enable
 		 * jumbo frames on one of the rings later.
 		 * This will not pass over-length frames into the default
@@ -3267,17 +3305,16 @@ static inline void igb_set_vmolr(struct igb_adapter *adapter,
 	struct e1000_hw *hw = &adapter->hw;
 	u32 vmolr;
 
-	/*
-	 * This register exists only on 82576 and newer so if we are older then
+	/* This register exists only on 82576 and newer so if we are older then
 	 * we should exit and do nothing
 	 */
 	if (hw->mac.type < e1000_82576)
 		return;
 
 	vmolr = rd32(E1000_VMOLR(vfn));
-	vmolr |= E1000_VMOLR_STRVLAN;      /* Strip vlan tags */
+	vmolr |= E1000_VMOLR_STRVLAN; /* Strip vlan tags */
 	if (aupe)
-		vmolr |= E1000_VMOLR_AUPE;        /* Accept untagged packets */
+		vmolr |= E1000_VMOLR_AUPE; /* Accept untagged packets */
 	else
 		vmolr &= ~(E1000_VMOLR_AUPE); /* Tagged packets ONLY */
 
@@ -3286,25 +3323,24 @@ static inline void igb_set_vmolr(struct igb_adapter *adapter,
 
 	if (adapter->rss_queues > 1 && vfn == adapter->vfs_allocated_count)
 		vmolr |= E1000_VMOLR_RSSE; /* enable RSS */
-	/*
-	 * for VMDq only allow the VFs and pool 0 to accept broadcast and
+	/* for VMDq only allow the VFs and pool 0 to accept broadcast and
 	 * multicast packets
 	 */
 	if (vfn <= adapter->vfs_allocated_count)
-		vmolr |= E1000_VMOLR_BAM;	   /* Accept broadcast */
+		vmolr |= E1000_VMOLR_BAM; /* Accept broadcast */
 
 	wr32(E1000_VMOLR(vfn), vmolr);
 }
 
 /**
- * igb_configure_rx_ring - Configure a receive ring after Reset
- * @adapter: board private structure
- * @ring: receive ring to be configured
+ *  igb_configure_rx_ring - Configure a receive ring after Reset
+ *  @adapter: board private structure
+ *  @ring: receive ring to be configured
  *
- * Configure the Rx unit of the MAC after a reset.
+ *  Configure the Rx unit of the MAC after a reset.
  **/
 void igb_configure_rx_ring(struct igb_adapter *adapter,
-                           struct igb_ring *ring)
+			   struct igb_ring *ring)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	u64 rdba = ring->dma;
@@ -3319,7 +3355,7 @@ void igb_configure_rx_ring(struct igb_adapter *adapter,
 	     rdba & 0x00000000ffffffffULL);
 	wr32(E1000_RDBAH(reg_idx), rdba >> 32);
 	wr32(E1000_RDLEN(reg_idx),
-	               ring->count * sizeof(union e1000_adv_rx_desc));
+	     ring->count * sizeof(union e1000_adv_rx_desc));
 
 	/* initialize head and tail */
 	ring->tail = hw->hw_addr + E1000_RDT(reg_idx);
@@ -3351,10 +3387,10 @@ void igb_configure_rx_ring(struct igb_adapter *adapter,
 }
 
 /**
- * igb_configure_rx - Configure receive Unit after Reset
- * @adapter: board private structure
+ *  igb_configure_rx - Configure receive Unit after Reset
+ *  @adapter: board private structure
  *
- * Configure the Rx unit of the MAC after a reset.
+ *  Configure the Rx unit of the MAC after a reset.
  **/
 static void igb_configure_rx(struct igb_adapter *adapter)
 {
@@ -3365,19 +3401,20 @@ static void igb_configure_rx(struct igb_adapter *adapter)
 
 	/* set the correct pool for the PF default MAC address in entry 0 */
 	igb_rar_set_qsel(adapter, adapter->hw.mac.addr, 0,
-	                 adapter->vfs_allocated_count);
+			 adapter->vfs_allocated_count);
 
 	/* Setup the HW Rx Head and Tail Descriptor Pointers and
-	 * the Base and Length of the Rx Descriptor Ring */
+	 * the Base and Length of the Rx Descriptor Ring
+	 */
 	for (i = 0; i < adapter->num_rx_queues; i++)
 		igb_configure_rx_ring(adapter, adapter->rx_ring[i]);
 }
 
 /**
- * igb_free_tx_resources - Free Tx Resources per Queue
- * @tx_ring: Tx descriptor ring for a specific queue
+ *  igb_free_tx_resources - Free Tx Resources per Queue
+ *  @tx_ring: Tx descriptor ring for a specific queue
  *
- * Free all transmit software resources
+ *  Free all transmit software resources
  **/
 void igb_free_tx_resources(struct igb_ring *tx_ring)
 {
@@ -3397,10 +3434,10 @@ void igb_free_tx_resources(struct igb_ring *tx_ring)
 }
 
 /**
- * igb_free_all_tx_resources - Free Tx Resources for All Queues
- * @adapter: board private structure
+ *  igb_free_all_tx_resources - Free Tx Resources for All Queues
+ *  @adapter: board private structure
  *
- * Free all transmit software resources
+ *  Free all transmit software resources
  **/
 static void igb_free_all_tx_resources(struct igb_adapter *adapter)
 {
@@ -3433,8 +3470,8 @@ void igb_unmap_and_free_tx_resource(struct igb_ring *ring,
 }
 
 /**
- * igb_clean_tx_ring - Free Tx Buffers
- * @tx_ring: ring to be cleaned
+ *  igb_clean_tx_ring - Free Tx Buffers
+ *  @tx_ring: ring to be cleaned
  **/
 static void igb_clean_tx_ring(struct igb_ring *tx_ring)
 {
@@ -3464,8 +3501,8 @@ static void igb_clean_tx_ring(struct igb_ring *tx_ring)
 }
 
 /**
- * igb_clean_all_tx_rings - Free Tx Buffers for all queues
- * @adapter: board private structure
+ *  igb_clean_all_tx_rings - Free Tx Buffers for all queues
+ *  @adapter: board private structure
  **/
 static void igb_clean_all_tx_rings(struct igb_adapter *adapter)
 {
@@ -3476,10 +3513,10 @@ static void igb_clean_all_tx_rings(struct igb_adapter *adapter)
 }
 
 /**
- * igb_free_rx_resources - Free Rx Resources
- * @rx_ring: ring to clean the resources from
+ *  igb_free_rx_resources - Free Rx Resources
+ *  @rx_ring: ring to clean the resources from
  *
- * Free all receive software resources
+ *  Free all receive software resources
  **/
 void igb_free_rx_resources(struct igb_ring *rx_ring)
 {
@@ -3499,10 +3536,10 @@ void igb_free_rx_resources(struct igb_ring *rx_ring)
 }
 
 /**
- * igb_free_all_rx_resources - Free Rx Resources for All Queues
- * @adapter: board private structure
+ *  igb_free_all_rx_resources - Free Rx Resources for All Queues
+ *  @adapter: board private structure
  *
- * Free all receive software resources
+ *  Free all receive software resources
  **/
 static void igb_free_all_rx_resources(struct igb_adapter *adapter)
 {
@@ -3513,8 +3550,8 @@ static void igb_free_all_rx_resources(struct igb_adapter *adapter)
 }
 
 /**
- * igb_clean_rx_ring - Free Rx Buffers per Queue
- * @rx_ring: ring to free buffers from
+ *  igb_clean_rx_ring - Free Rx Buffers per Queue
+ *  @rx_ring: ring to free buffers from
  **/
 static void igb_clean_rx_ring(struct igb_ring *rx_ring)
 {
@@ -3556,8 +3593,8 @@ static void igb_clean_rx_ring(struct igb_ring *rx_ring)
 }
 
 /**
- * igb_clean_all_rx_rings - Free Rx Buffers for all queues
- * @adapter: board private structure
+ *  igb_clean_all_rx_rings - Free Rx Buffers for all queues
+ *  @adapter: board private structure
  **/
 static void igb_clean_all_rx_rings(struct igb_adapter *adapter)
 {
@@ -3568,11 +3605,11 @@ static void igb_clean_all_rx_rings(struct igb_adapter *adapter)
 }
 
 /**
- * igb_set_mac - Change the Ethernet Address of the NIC
- * @netdev: network interface device structure
- * @p: pointer to an address structure
+ *  igb_set_mac - Change the Ethernet Address of the NIC
+ *  @netdev: network interface device structure
+ *  @p: pointer to an address structure
  *
- * Returns 0 on success, negative on failure
+ *  Returns 0 on success, negative on failure
  **/
 static int igb_set_mac(struct net_device *netdev, void *p)
 {
@@ -3588,19 +3625,19 @@ static int igb_set_mac(struct net_device *netdev, void *p)
 
 	/* set the correct pool for the new PF MAC address in entry 0 */
 	igb_rar_set_qsel(adapter, hw->mac.addr, 0,
-	                 adapter->vfs_allocated_count);
+			 adapter->vfs_allocated_count);
 
 	return 0;
 }
 
 /**
- * igb_write_mc_addr_list - write multicast addresses to MTA
- * @netdev: network interface device structure
+ *  igb_write_mc_addr_list - write multicast addresses to MTA
+ *  @netdev: network interface device structure
  *
- * Writes multicast address list to the MTA hash table.
- * Returns: -ENOMEM on failure
- *                0 on no addresses written
- *                X on writing X addresses to MTA
+ *  Writes multicast address list to the MTA hash table.
+ *  Returns: -ENOMEM on failure
+ *           0 on no addresses written
+ *           X on writing X addresses to MTA
  **/
 static int igb_write_mc_addr_list(struct net_device *netdev)
 {
@@ -3633,13 +3670,13 @@ static int igb_write_mc_addr_list(struct net_device *netdev)
 }
 
 /**
- * igb_write_uc_addr_list - write unicast addresses to RAR table
- * @netdev: network interface device structure
+ *  igb_write_uc_addr_list - write unicast addresses to RAR table
+ *  @netdev: network interface device structure
  *
- * Writes unicast address list to the RAR table.
- * Returns: -ENOMEM on failure/insufficient address space
- *                0 on no addresses written
- *                X on writing X addresses to the RAR table
+ *  Writes unicast address list to the RAR table.
+ *  Returns: -ENOMEM on failure/insufficient address space
+ *           0 on no addresses written
+ *           X on writing X addresses to the RAR table
  **/
 static int igb_write_uc_addr_list(struct net_device *netdev)
 {
@@ -3660,8 +3697,8 @@ static int igb_write_uc_addr_list(struct net_device *netdev)
 			if (!rar_entries)
 				break;
 			igb_rar_set_qsel(adapter, ha->addr,
-			                 rar_entries--,
-			                 vfn);
+					 rar_entries--,
+					 vfn);
 			count++;
 		}
 	}
@@ -3676,13 +3713,13 @@ static int igb_write_uc_addr_list(struct net_device *netdev)
 }
 
 /**
- * igb_set_rx_mode - Secondary Unicast, Multicast and Promiscuous mode set
- * @netdev: network interface device structure
+ *  igb_set_rx_mode - Secondary Unicast, Multicast and Promiscuous mode set
+ *  @netdev: network interface device structure
  *
- * The set_rx_mode entry point is called whenever the unicast or multicast
- * address lists or the network interface flags are updated.  This routine is
- * responsible for configuring the hardware for proper unicast, multicast,
- * promiscuous mode, and all-multi behavior.
+ *  The set_rx_mode entry point is called whenever the unicast or multicast
+ *  address lists or the network interface flags are updated.  This routine is
+ *  responsible for configuring the hardware for proper unicast, multicast,
+ *  promiscuous mode, and all-multi behavior.
  **/
 static void igb_set_rx_mode(struct net_device *netdev)
 {
@@ -3699,6 +3736,10 @@ static void igb_set_rx_mode(struct net_device *netdev)
 	rctl &= ~(E1000_RCTL_UPE | E1000_RCTL_MPE | E1000_RCTL_VFE);
 
 	if (netdev->flags & IFF_PROMISC) {
+		u32 mrqc = rd32(E1000_MRQC);
+		/* retain VLAN HW filtering if in VT mode */
+		if (mrqc & E1000_MRQC_ENABLE_VMDQ)
+			rctl |= E1000_RCTL_VFE;
 		rctl |= (E1000_RCTL_UPE | E1000_RCTL_MPE);
 		vmolr |= (E1000_VMOLR_ROPE | E1000_VMOLR_MPME);
 	} else {
@@ -3706,8 +3747,7 @@ static void igb_set_rx_mode(struct net_device *netdev)
 			rctl |= E1000_RCTL_MPE;
 			vmolr |= E1000_VMOLR_MPME;
 		} else {
-			/*
-			 * Write addresses to the MTA, if the attempt fails
+			/* Write addresses to the MTA, if the attempt fails
 			 * then we should just turn on promiscuous mode so
 			 * that we can at least receive multicast traffic
 			 */
@@ -3719,8 +3759,7 @@ static void igb_set_rx_mode(struct net_device *netdev)
 				vmolr |= E1000_VMOLR_ROMPE;
 			}
 		}
-		/*
-		 * Write addresses to available RAR registers, if there is not
+		/* Write addresses to available RAR registers, if there is not
 		 * sufficient space to store all the addresses then enable
 		 * unicast promiscuous mode
 		 */
@@ -3733,8 +3772,7 @@ static void igb_set_rx_mode(struct net_device *netdev)
 	}
 	wr32(E1000_RCTL, rctl);
 
-	/*
-	 * In order to support SR-IOV and eventually VMDq it is necessary to set
+	/* In order to support SR-IOV and eventually VMDq it is necessary to set
 	 * the VMOLR to enable the appropriate modes.  Without this workaround
 	 * we will have issues with VLAN tag stripping not being done for frames
 	 * that are only arriving because we are the default pool
@@ -3743,7 +3781,7 @@ static void igb_set_rx_mode(struct net_device *netdev)
 		return;
 
 	vmolr |= rd32(E1000_VMOLR(vfn)) &
-	         ~(E1000_VMOLR_ROPE | E1000_VMOLR_MPME | E1000_VMOLR_ROMPE);
+		 ~(E1000_VMOLR_ROPE | E1000_VMOLR_MPME | E1000_VMOLR_ROMPE);
 	wr32(E1000_VMOLR(vfn), vmolr);
 	igb_restore_vf_multicasts(adapter);
 }
@@ -3788,7 +3826,8 @@ static void igb_spoof_check(struct igb_adapter *adapter)
 }
 
 /* Need to wait a few seconds after link up to get diagnostic information from
- * the phy */
+ * the phy
+ */
 static void igb_update_phy_info(unsigned long data)
 {
 	struct igb_adapter *adapter = (struct igb_adapter *) data;
@@ -3796,8 +3835,8 @@ static void igb_update_phy_info(unsigned long data)
 }
 
 /**
- * igb_has_link - check shared code for link and determine up/down
- * @adapter: pointer to driver private info
+ *  igb_has_link - check shared code for link and determine up/down
+ *  @adapter: pointer to driver private info
  **/
 bool igb_has_link(struct igb_adapter *adapter)
 {
@@ -3842,17 +3881,16 @@ static bool igb_thermal_sensor_event(struct e1000_hw *hw, u32 event)
 		ctrl_ext = rd32(E1000_CTRL_EXT);
 
 		if ((hw->phy.media_type == e1000_media_type_copper) &&
-		    !(ctrl_ext & E1000_CTRL_EXT_LINK_MODE_SGMII)) {
+		    !(ctrl_ext & E1000_CTRL_EXT_LINK_MODE_SGMII))
 			ret = !!(thstat & event);
-		}
 	}
 
 	return ret;
 }
 
 /**
- * igb_watchdog - Timer Call-back
- * @data: pointer to adapter cast into an unsigned long
+ *  igb_watchdog - Timer Call-back
+ *  @data: pointer to adapter cast into an unsigned long
  **/
 static void igb_watchdog(unsigned long data)
 {
@@ -3864,9 +3902,10 @@ static void igb_watchdog(unsigned long data)
 static void igb_watchdog_task(struct work_struct *work)
 {
 	struct igb_adapter *adapter = container_of(work,
-	                                           struct igb_adapter,
-                                                   watchdog_task);
+						   struct igb_adapter,
+						   watchdog_task);
 	struct e1000_hw *hw = &adapter->hw;
+	struct e1000_phy_info *phy = &hw->phy;
 	struct net_device *netdev = adapter->netdev;
 	u32 link;
 	int i;
@@ -3879,8 +3918,8 @@ static void igb_watchdog_task(struct work_struct *work)
 		if (!netif_carrier_ok(netdev)) {
 			u32 ctrl;
 			hw->mac.ops.get_speed_and_duplex(hw,
-			                                 &adapter->link_speed,
-			                                 &adapter->link_duplex);
+							 &adapter->link_speed,
+							 &adapter->link_duplex);
 
 			ctrl = rd32(E1000_CTRL);
 			/* Links status message must follow this format */
@@ -3895,6 +3934,11 @@ static void igb_watchdog_task(struct work_struct *work)
 			       (ctrl & E1000_CTRL_RFCE) ?  "RX" :
 			       (ctrl & E1000_CTRL_TFCE) ?  "TX" : "None");
 
+			/* check if SmartSpeed worked */
+			igb_check_downshift(hw);
+			if (phy->speed_downgraded)
+				netdev_warn(netdev, "Link Speed was downgraded by SmartSpeed\n");
+
 			/* check for thermal sensor event */
 			if (igb_thermal_sensor_event(hw,
 			    E1000_THSTAT_LINK_THROTTLE)) {
@@ -3963,7 +4007,8 @@ static void igb_watchdog_task(struct work_struct *work)
 			/* We've lost link, so the controller stops DMA,
 			 * but we've got queued Tx work that's never going
 			 * to get done, so reset controller to flush Tx.
-			 * (Do the reset outside of interrupt context). */
+			 * (Do the reset outside of interrupt context).
+			 */
 			if (igb_desc_unused(tx_ring) + 1 < tx_ring->count) {
 				adapter->tx_timeout_count++;
 				schedule_work(&adapter->reset_task);
@@ -3976,7 +4021,7 @@ static void igb_watchdog_task(struct work_struct *work)
 		set_bit(IGB_RING_FLAG_TX_DETECT_HANG, &tx_ring->flags);
 	}
 
-	/* Cause software interrupt to ensure rx ring is cleaned */
+	/* Cause software interrupt to ensure Rx ring is cleaned */
 	if (adapter->msix_entries) {
 		u32 eics = 0;
 		for (i = 0; i < adapter->num_q_vectors; i++)
@@ -4003,20 +4048,20 @@ enum latency_range {
 };
 
 /**
- * igb_update_ring_itr - update the dynamic ITR value based on packet size
+ *  igb_update_ring_itr - update the dynamic ITR value based on packet size
+ *  @q_vector: pointer to q_vector
  *
- *      Stores a new ITR value based on strictly on packet size.  This
- *      algorithm is less sophisticated than that used in igb_update_itr,
- *      due to the difficulty of synchronizing statistics across multiple
- *      receive rings.  The divisors and thresholds used by this function
- *      were determined based on theoretical maximum wire speed and testing
- *      data, in order to minimize response time while increasing bulk
- *      throughput.
- *      This functionality is controlled by the InterruptThrottleRate module
- *      parameter (see igb_param.c)
- *      NOTE:  This function is called only when operating in a multiqueue
- *             receive environment.
- * @q_vector: pointer to q_vector
+ *  Stores a new ITR value based on strictly on packet size.  This
+ *  algorithm is less sophisticated than that used in igb_update_itr,
+ *  due to the difficulty of synchronizing statistics across multiple
+ *  receive rings.  The divisors and thresholds used by this function
+ *  were determined based on theoretical maximum wire speed and testing
+ *  data, in order to minimize response time while increasing bulk
+ *  throughput.
+ *  This functionality is controlled by the InterruptThrottleRate module
+ *  parameter (see igb_param.c)
+ *  NOTE:  This function is called only when operating in a multiqueue
+ *         receive environment.
  **/
 static void igb_update_ring_itr(struct igb_q_vector *q_vector)
 {
@@ -4077,20 +4122,21 @@ clear_counts:
 }
 
 /**
- * igb_update_itr - update the dynamic ITR value based on statistics
- *      Stores a new ITR value based on packets and byte
- *      counts during the last interrupt.  The advantage of per interrupt
- *      computation is faster updates and more accurate ITR for the current
- *      traffic pattern.  Constants in this function were computed
- *      based on theoretical maximum wire speed and thresholds were set based
- *      on testing data as well as attempting to minimize response time
- *      while increasing bulk throughput.
- *      this functionality is controlled by the InterruptThrottleRate module
- *      parameter (see igb_param.c)
- *      NOTE:  These calculations are only valid when operating in a single-
- *             queue environment.
- * @q_vector: pointer to q_vector
- * @ring_container: ring info to update the itr for
+ *  igb_update_itr - update the dynamic ITR value based on statistics
+ *  @q_vector: pointer to q_vector
+ *  @ring_container: ring info to update the itr for
+ *
+ *  Stores a new ITR value based on packets and byte
+ *  counts during the last interrupt.  The advantage of per interrupt
+ *  computation is faster updates and more accurate ITR for the current
+ *  traffic pattern.  Constants in this function were computed
+ *  based on theoretical maximum wire speed and thresholds were set based
+ *  on testing data as well as attempting to minimize response time
+ *  while increasing bulk throughput.
+ *  this functionality is controlled by the InterruptThrottleRate module
+ *  parameter (see igb_param.c)
+ *  NOTE:  These calculations are only valid when operating in a single-
+ *         queue environment.
  **/
 static void igb_update_itr(struct igb_q_vector *q_vector,
 			   struct igb_ring_container *ring_container)
@@ -4188,12 +4234,12 @@ set_itr_now:
 	if (new_itr != q_vector->itr_val) {
 		/* this attempts to bias the interrupt rate towards Bulk
 		 * by adding intermediate steps when interrupt rate is
-		 * increasing */
+		 * increasing
+		 */
 		new_itr = new_itr > q_vector->itr_val ?
-		             max((new_itr * q_vector->itr_val) /
-		                 (new_itr + (q_vector->itr_val >> 2)),
-				 new_itr) :
-			     new_itr;
+			  max((new_itr * q_vector->itr_val) /
+			  (new_itr + (q_vector->itr_val >> 2)),
+			  new_itr) : new_itr;
 		/* Don't write the value here; it resets the adapter's
 		 * internal timer, and causes us to delay far longer than
 		 * we should between interrupts.  Instead, we write the ITR
@@ -4320,8 +4366,8 @@ static void igb_tx_csum(struct igb_ring *tx_ring, struct igb_tx_buffer *first)
 		default:
 			if (unlikely(net_ratelimit())) {
 				dev_warn(tx_ring->dev,
-				 "partial checksum but proto=%x!\n",
-				 first->protocol);
+					 "partial checksum but proto=%x!\n",
+					 first->protocol);
 			}
 			break;
 		}
@@ -4344,8 +4390,8 @@ static void igb_tx_csum(struct igb_ring *tx_ring, struct igb_tx_buffer *first)
 		default:
 			if (unlikely(net_ratelimit())) {
 				dev_warn(tx_ring->dev,
-				 "partial checksum but l4 proto=%x!\n",
-				 l4_hdr);
+					 "partial checksum but l4 proto=%x!\n",
+					 l4_hdr);
 			}
 			break;
 		}
@@ -4497,8 +4543,7 @@ static void igb_tx_map(struct igb_ring *tx_ring,
 	/* set the timestamp */
 	first->time_stamp = jiffies;
 
-	/*
-	 * Force memory writes to complete before letting h/w know there
+	/* Force memory writes to complete before letting h/w know there
 	 * are new descriptors to fetch.  (Only applicable for weak-ordered
 	 * memory model archs, such as IA-64).
 	 *
@@ -4519,7 +4564,8 @@ static void igb_tx_map(struct igb_ring *tx_ring,
 	writel(i, tx_ring->tail);
 
 	/* we need this if more than one processor can write to our tail
-	 * at a time, it syncronizes IO on IA64/Altix systems */
+	 * at a time, it synchronizes IO on IA64/Altix systems
+	 */
 	mmiowb();
 
 	return;
@@ -4549,11 +4595,13 @@ static int __igb_maybe_stop_tx(struct igb_ring *tx_ring, const u16 size)
 
 	/* Herbert's original patch had:
 	 *  smp_mb__after_netif_stop_queue();
-	 * but since that doesn't exist yet, just open code it. */
+	 * but since that doesn't exist yet, just open code it.
+	 */
 	smp_mb();
 
 	/* We need to check again in a case another CPU has just
-	 * made room available. */
+	 * made room available.
+	 */
 	if (igb_desc_unused(tx_ring) < size)
 		return -EBUSY;
 
@@ -4577,7 +4625,6 @@ static inline int igb_maybe_stop_tx(struct igb_ring *tx_ring, const u16 size)
 netdev_tx_t igb_xmit_frame_ring(struct sk_buff *skb,
 				struct igb_ring *tx_ring)
 {
-	struct igb_adapter *adapter = netdev_priv(tx_ring->netdev);
 	struct igb_tx_buffer *first;
 	int tso;
 	u32 tx_flags = 0;
@@ -4612,15 +4659,18 @@ netdev_tx_t igb_xmit_frame_ring(struct sk_buff *skb,
 
 	skb_tx_timestamp(skb);
 
-	if (unlikely((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
-		     !(adapter->ptp_tx_skb))) {
-		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
-		tx_flags |= IGB_TX_FLAGS_TSTAMP;
+	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {
+		struct igb_adapter *adapter = netdev_priv(tx_ring->netdev);
 
-		adapter->ptp_tx_skb = skb_get(skb);
-		adapter->ptp_tx_start = jiffies;
-		if (adapter->hw.mac.type == e1000_82576)
-			schedule_work(&adapter->ptp_tx_work);
+		if (!(adapter->ptp_tx_skb)) {
+			skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+			tx_flags |= IGB_TX_FLAGS_TSTAMP;
+
+			adapter->ptp_tx_skb = skb_get(skb);
+			adapter->ptp_tx_start = jiffies;
+			if (adapter->hw.mac.type == e1000_82576)
+				schedule_work(&adapter->ptp_tx_work);
+		}
 	}
 
 	if (vlan_tx_tag_present(skb)) {
@@ -4677,8 +4727,7 @@ static netdev_tx_t igb_xmit_frame(struct sk_buff *skb,
 		return NETDEV_TX_OK;
 	}
 
-	/*
-	 * The minimum packet size with TCTL.PSP set is 17 so pad the skb
+	/* The minimum packet size with TCTL.PSP set is 17 so pad the skb
 	 * in order to meet this minimum size requirement.
 	 */
 	if (unlikely(skb->len < 17)) {
@@ -4692,8 +4741,8 @@ static netdev_tx_t igb_xmit_frame(struct sk_buff *skb,
 }
 
 /**
- * igb_tx_timeout - Respond to a Tx Hang
- * @netdev: network interface device structure
+ *  igb_tx_timeout - Respond to a Tx Hang
+ *  @netdev: network interface device structure
  **/
 static void igb_tx_timeout(struct net_device *netdev)
 {
@@ -4722,13 +4771,12 @@ static void igb_reset_task(struct work_struct *work)
 }
 
 /**
- * igb_get_stats64 - Get System Network Statistics
- * @netdev: network interface device structure
- * @stats: rtnl_link_stats64 pointer
- *
+ *  igb_get_stats64 - Get System Network Statistics
+ *  @netdev: network interface device structure
+ *  @stats: rtnl_link_stats64 pointer
  **/
 static struct rtnl_link_stats64 *igb_get_stats64(struct net_device *netdev,
-						 struct rtnl_link_stats64 *stats)
+						struct rtnl_link_stats64 *stats)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
 
@@ -4741,11 +4789,11 @@ static struct rtnl_link_stats64 *igb_get_stats64(struct net_device *netdev,
 }
 
 /**
- * igb_change_mtu - Change the Maximum Transfer Unit
- * @netdev: network interface device structure
- * @new_mtu: new value for maximum frame size
+ *  igb_change_mtu - Change the Maximum Transfer Unit
+ *  @netdev: network interface device structure
+ *  @new_mtu: new value for maximum frame size
  *
- * Returns 0 on success, negative on failure
+ *  Returns 0 on success, negative on failure
  **/
 static int igb_change_mtu(struct net_device *netdev, int new_mtu)
 {
@@ -4788,10 +4836,9 @@ static int igb_change_mtu(struct net_device *netdev, int new_mtu)
 }
 
 /**
- * igb_update_stats - Update the board statistics counters
- * @adapter: board private structure
+ *  igb_update_stats - Update the board statistics counters
+ *  @adapter: board private structure
  **/
-
 void igb_update_stats(struct igb_adapter *adapter,
 		      struct rtnl_link_stats64 *net_stats)
 {
@@ -4806,8 +4853,7 @@ void igb_update_stats(struct igb_adapter *adapter,
 
 #define PHY_IDLE_ERROR_COUNT_MASK 0x00FF
 
-	/*
-	 * Prevent stats update while adapter is being reset, or if the pci
+	/* Prevent stats update while adapter is being reset, or if the pci
 	 * connection is down.
 	 */
 	if (adapter->link_speed == 0)
@@ -4941,7 +4987,8 @@ void igb_update_stats(struct igb_adapter *adapter,
 	/* Rx Errors */
 
 	/* RLEC on some newer hardware can be incorrect so build
-	 * our own version based on RUC and ROC */
+	 * our own version based on RUC and ROC
+	 */
 	net_stats->rx_errors = adapter->stats.rxerrc +
 		adapter->stats.crcerrs + adapter->stats.algnerrc +
 		adapter->stats.ruc + adapter->stats.roc +
@@ -5000,7 +5047,8 @@ static irqreturn_t igb_msix_other(int irq, void *data)
 		adapter->stats.doosync++;
 		/* The DMA Out of Sync is also indication of a spoof event
 		 * in IOV mode. Check the Wrong VM Behavior register to
-		 * see if it is really a spoof event. */
+		 * see if it is really a spoof event.
+		 */
 		igb_check_wvbr(adapter);
 	}
 
@@ -5074,8 +5122,7 @@ static void igb_update_tx_dca(struct igb_adapter *adapter,
 	if (hw->mac.type != e1000_82575)
 		txctrl <<= E1000_DCA_TXCTRL_CPUID_SHIFT;
 
-	/*
-	 * We can enable relaxed ordering for reads, but not writes when
+	/* We can enable relaxed ordering for reads, but not writes when
 	 * DCA is enabled.  This is due to a known issue in some chipsets
 	 * which will cause the DCA tag to be cleared.
 	 */
@@ -5096,8 +5143,7 @@ static void igb_update_rx_dca(struct igb_adapter *adapter,
 	if (hw->mac.type != e1000_82575)
 		rxctrl <<= E1000_DCA_RXCTRL_CPUID_SHIFT;
 
-	/*
-	 * We can enable relaxed ordering for reads, but not writes when
+	/* We can enable relaxed ordering for reads, but not writes when
 	 * DCA is enabled.  This is due to a known issue in some chipsets
 	 * which will cause the DCA tag to be cleared.
 	 */
@@ -5166,7 +5212,8 @@ static int __igb_notify_dca(struct device *dev, void *data)
 	case DCA_PROVIDER_REMOVE:
 		if (adapter->flags & IGB_FLAG_DCA_ENABLED) {
 			/* without this a class_device is left
-			 * hanging around in the sysfs model */
+			 * hanging around in the sysfs model
+			 */
 			dca_remove_requester(dev);
 			dev_info(&pdev->dev, "DCA disabled\n");
 			adapter->flags &= ~IGB_FLAG_DCA_ENABLED;
@@ -5179,12 +5226,12 @@ static int __igb_notify_dca(struct device *dev, void *data)
 }
 
 static int igb_notify_dca(struct notifier_block *nb, unsigned long event,
-                          void *p)
+			  void *p)
 {
 	int ret_val;
 
 	ret_val = driver_for_each_device(&igb_driver.driver, NULL, &event,
-	                                 __igb_notify_dca);
+					 __igb_notify_dca);
 
 	return ret_val ? NOTIFY_BAD : NOTIFY_DONE;
 }
@@ -5198,40 +5245,10 @@ static int igb_vf_configure(struct igb_adapter *adapter, int vf)
 	eth_zero_addr(mac_addr);
 	igb_set_vf_mac(adapter, vf, mac_addr);
 
-	return 0;
-}
-
-static bool igb_vfs_are_assigned(struct igb_adapter *adapter)
-{
-	struct pci_dev *pdev = adapter->pdev;
-	struct pci_dev *vfdev;
-	int dev_id;
-
-	switch (adapter->hw.mac.type) {
-	case e1000_82576:
-		dev_id = IGB_82576_VF_DEV_ID;
-		break;
-	case e1000_i350:
-		dev_id = IGB_I350_VF_DEV_ID;
-		break;
-	default:
-		return false;
-	}
-
-	/* loop through all the VFs to see if we own any that are assigned */
-	vfdev = pci_get_device(PCI_VENDOR_ID_INTEL, dev_id, NULL);
-	while (vfdev) {
-		/* if we don't own it we don't care */
-		if (vfdev->is_virtfn && vfdev->physfn == pdev) {
-			/* if it is assigned we cannot release it */
-			if (vfdev->dev_flags & PCI_DEV_FLAGS_ASSIGNED)
-				return true;
-		}
-
-		vfdev = pci_get_device(PCI_VENDOR_ID_INTEL, dev_id, vfdev);
-	}
+	/* By default spoof check is enabled for all VFs */
+	adapter->vf_data[vf].spoofchk_enabled = true;
 
-	return false;
+	return 0;
 }
 
 #endif
@@ -5256,7 +5273,7 @@ static int igb_set_vf_promisc(struct igb_adapter *adapter, u32 *msgbuf, u32 vf)
 	struct vf_data_storage *vf_data = &adapter->vf_data[vf];
 
 	vf_data->flags &= ~(IGB_VF_FLAG_UNI_PROMISC |
-	                    IGB_VF_FLAG_MULTI_PROMISC);
+			    IGB_VF_FLAG_MULTI_PROMISC);
 	vmolr &= ~(E1000_VMOLR_ROPE | E1000_VMOLR_ROMPE | E1000_VMOLR_MPME);
 
 	if (*msgbuf & E1000_VF_SET_PROMISC_MULTICAST) {
@@ -5264,8 +5281,7 @@ static int igb_set_vf_promisc(struct igb_adapter *adapter, u32 *msgbuf, u32 vf)
 		vf_data->flags |= IGB_VF_FLAG_MULTI_PROMISC;
 		*msgbuf &= ~E1000_VF_SET_PROMISC_MULTICAST;
 	} else {
-		/*
-		 * if we have hashes and we are clearing a multicast promisc
+		/* if we have hashes and we are clearing a multicast promisc
 		 * flag we need to write the hashes to the MTA as this step
 		 * was previously skipped
 		 */
@@ -5286,7 +5302,6 @@ static int igb_set_vf_promisc(struct igb_adapter *adapter, u32 *msgbuf, u32 vf)
 		return -EINVAL;
 
 	return 0;
-
 }
 
 static int igb_set_vf_multicasts(struct igb_adapter *adapter,
@@ -5493,30 +5508,91 @@ static int igb_ndo_set_vf_vlan(struct net_device *netdev,
 			 "Setting VLAN %d, QOS 0x%x on VF %d\n", vlan, qos, vf);
 		if (test_bit(__IGB_DOWN, &adapter->state)) {
 			dev_warn(&adapter->pdev->dev,
-				 "The VF VLAN has been set,"
-				 " but the PF device is not up.\n");
+				 "The VF VLAN has been set, but the PF device is not up.\n");
 			dev_warn(&adapter->pdev->dev,
-				 "Bring the PF device up before"
-				 " attempting to use the VF device.\n");
+				 "Bring the PF device up before attempting to use the VF device.\n");
 		}
 	} else {
 		igb_vlvf_set(adapter, adapter->vf_data[vf].pf_vlan,
-				   false, vf);
+			     false, vf);
 		igb_set_vmvir(adapter, vlan, vf);
 		igb_set_vmolr(adapter, vf, true);
 		adapter->vf_data[vf].pf_vlan = 0;
 		adapter->vf_data[vf].pf_qos = 0;
-       }
+	}
 out:
-       return err;
+	return err;
+}
+
+static int igb_find_vlvf_entry(struct igb_adapter *adapter, int vid)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	int i;
+	u32 reg;
+
+	/* Find the vlan filter for this id */
+	for (i = 0; i < E1000_VLVF_ARRAY_SIZE; i++) {
+		reg = rd32(E1000_VLVF(i));
+		if ((reg & E1000_VLVF_VLANID_ENABLE) &&
+		    vid == (reg & E1000_VLVF_VLANID_MASK))
+			break;
+	}
+
+	if (i >= E1000_VLVF_ARRAY_SIZE)
+		i = -1;
+
+	return i;
 }
 
 static int igb_set_vf_vlan(struct igb_adapter *adapter, u32 *msgbuf, u32 vf)
 {
+	struct e1000_hw *hw = &adapter->hw;
 	int add = (msgbuf[0] & E1000_VT_MSGINFO_MASK) >> E1000_VT_MSGINFO_SHIFT;
 	int vid = (msgbuf[1] & E1000_VLVF_VLANID_MASK);
+	int err = 0;
+
+	/* If in promiscuous mode we need to make sure the PF also has
+	 * the VLAN filter set.
+	 */
+	if (add && (adapter->netdev->flags & IFF_PROMISC))
+		err = igb_vlvf_set(adapter, vid, add,
+				   adapter->vfs_allocated_count);
+	if (err)
+		goto out;
 
-	return igb_vlvf_set(adapter, vid, add, vf);
+	err = igb_vlvf_set(adapter, vid, add, vf);
+
+	if (err)
+		goto out;
+
+	/* Go through all the checks to see if the VLAN filter should
+	 * be wiped completely.
+	 */
+	if (!add && (adapter->netdev->flags & IFF_PROMISC)) {
+		u32 vlvf, bits;
+
+		int regndx = igb_find_vlvf_entry(adapter, vid);
+		if (regndx < 0)
+			goto out;
+		/* See if any other pools are set for this VLAN filter
+		 * entry other than the PF.
+		 */
+		vlvf = bits = rd32(E1000_VLVF(regndx));
+		bits &= 1 << (E1000_VLVF_POOLSEL_SHIFT +
+			      adapter->vfs_allocated_count);
+		/* If the filter was removed then ensure PF pool bit
+		 * is cleared if the PF only added itself to the pool
+		 * because the PF is in promiscuous mode.
+		 */
+		if ((vlvf & VLAN_VID_MASK) == vid &&
+		    !test_bit(vid, adapter->active_vlans) &&
+		    !bits)
+			igb_vlvf_set(adapter, vid, add,
+				     adapter->vfs_allocated_count);
+	}
+
+out:
+	return err;
 }
 
 static inline void igb_vf_reset(struct igb_adapter *adapter, u32 vf)
@@ -5586,8 +5662,7 @@ static void igb_vf_reset_msg(struct igb_adapter *adapter, u32 vf)
 
 static int igb_set_vf_mac_addr(struct igb_adapter *adapter, u32 *msg, int vf)
 {
-	/*
-	 * The VF MAC Address is stored in a packed array of bytes
+	/* The VF MAC Address is stored in a packed array of bytes
 	 * starting at the second 32 bit word of the msg array
 	 */
 	unsigned char *addr = (char *)&msg[1];
@@ -5636,11 +5711,9 @@ static void igb_rcv_msg_from_vf(struct igb_adapter *adapter, u32 vf)
 	if (msgbuf[0] & (E1000_VT_MSGTYPE_ACK | E1000_VT_MSGTYPE_NACK))
 		return;
 
-	/*
-	 * until the vf completes a reset it should not be
+	/* until the vf completes a reset it should not be
 	 * allowed to start any configuration.
 	 */
-
 	if (msgbuf[0] == E1000_VF_RESET) {
 		igb_vf_reset_msg(adapter, vf);
 		return;
@@ -5660,9 +5733,8 @@ static void igb_rcv_msg_from_vf(struct igb_adapter *adapter, u32 vf)
 			retval = igb_set_vf_mac_addr(adapter, msgbuf, vf);
 		else
 			dev_warn(&pdev->dev,
-				 "VF %d attempted to override administratively "
-				 "set MAC address\nReload the VF driver to "
-				 "resume operations\n", vf);
+				 "VF %d attempted to override administratively set MAC address\nReload the VF driver to resume operations\n",
+				 vf);
 		break;
 	case E1000_VF_SET_PROMISC:
 		retval = igb_set_vf_promisc(adapter, msgbuf, vf);
@@ -5677,9 +5749,8 @@ static void igb_rcv_msg_from_vf(struct igb_adapter *adapter, u32 vf)
 		retval = -1;
 		if (vf_data->pf_vlan)
 			dev_warn(&pdev->dev,
-				 "VF %d attempted to override administratively "
-				 "set VLAN tag\nReload the VF driver to "
-				 "resume operations\n", vf);
+				 "VF %d attempted to override administratively set VLAN tag\nReload the VF driver to resume operations\n",
+				 vf);
 		else
 			retval = igb_set_vf_vlan(adapter, msgbuf, vf);
 		break;
@@ -5748,9 +5819,9 @@ static void igb_set_uta(struct igb_adapter *adapter)
 }
 
 /**
- * igb_intr_msi - Interrupt Handler
- * @irq: interrupt number
- * @data: pointer to a network interface device structure
+ *  igb_intr_msi - Interrupt Handler
+ *  @irq: interrupt number
+ *  @data: pointer to a network interface device structure
  **/
 static irqreturn_t igb_intr_msi(int irq, void *data)
 {
@@ -5793,9 +5864,9 @@ static irqreturn_t igb_intr_msi(int irq, void *data)
 }
 
 /**
- * igb_intr - Legacy Interrupt Handler
- * @irq: interrupt number
- * @data: pointer to a network interface device structure
+ *  igb_intr - Legacy Interrupt Handler
+ *  @irq: interrupt number
+ *  @data: pointer to a network interface device structure
  **/
 static irqreturn_t igb_intr(int irq, void *data)
 {
@@ -5803,11 +5874,13 @@ static irqreturn_t igb_intr(int irq, void *data)
 	struct igb_q_vector *q_vector = adapter->q_vector[0];
 	struct e1000_hw *hw = &adapter->hw;
 	/* Interrupt Auto-Mask...upon reading ICR, interrupts are masked.  No
-	 * need for the IMC write */
+	 * need for the IMC write
+	 */
 	u32 icr = rd32(E1000_ICR);
 
 	/* IMS will not auto-mask if INT_ASSERTED is not set, and if it is
-	 * not set, then the adapter didn't send an interrupt */
+	 * not set, then the adapter didn't send an interrupt
+	 */
 	if (!(icr & E1000_ICR_INT_ASSERTED))
 		return IRQ_NONE;
 
@@ -5866,15 +5939,15 @@ static void igb_ring_irq_enable(struct igb_q_vector *q_vector)
 }
 
 /**
- * igb_poll - NAPI Rx polling callback
- * @napi: napi polling structure
- * @budget: count of how many packets we should handle
+ *  igb_poll - NAPI Rx polling callback
+ *  @napi: napi polling structure
+ *  @budget: count of how many packets we should handle
  **/
 static int igb_poll(struct napi_struct *napi, int budget)
 {
 	struct igb_q_vector *q_vector = container_of(napi,
-	                                             struct igb_q_vector,
-	                                             napi);
+						     struct igb_q_vector,
+						     napi);
 	bool clean_complete = true;
 
 #ifdef CONFIG_IGB_DCA
@@ -5899,10 +5972,10 @@ static int igb_poll(struct napi_struct *napi, int budget)
 }
 
 /**
- * igb_clean_tx_irq - Reclaim resources after transmit completes
- * @q_vector: pointer to q_vector containing needed info
+ *  igb_clean_tx_irq - Reclaim resources after transmit completes
+ *  @q_vector: pointer to q_vector containing needed info
  *
- * returns true if ring is completely cleaned
+ *  returns true if ring is completely cleaned
  **/
 static bool igb_clean_tx_irq(struct igb_q_vector *q_vector)
 {
@@ -6008,7 +6081,8 @@ static bool igb_clean_tx_irq(struct igb_q_vector *q_vector)
 		struct e1000_hw *hw = &adapter->hw;
 
 		/* Detect a transmit hang in hardware, this serializes the
-		 * check with the clearing of time_stamp and movement of i */
+		 * check with the clearing of time_stamp and movement of i
+		 */
 		clear_bit(IGB_RING_FLAG_TX_DETECT_HANG, &tx_ring->flags);
 		if (tx_buffer->next_to_watch &&
 		    time_after(jiffies, tx_buffer->time_stamp +
@@ -6047,8 +6121,8 @@ static bool igb_clean_tx_irq(struct igb_q_vector *q_vector)
 
 #define TX_WAKE_THRESHOLD (DESC_NEEDED * 2)
 	if (unlikely(total_packets &&
-		     netif_carrier_ok(tx_ring->netdev) &&
-		     igb_desc_unused(tx_ring) >= TX_WAKE_THRESHOLD)) {
+	    netif_carrier_ok(tx_ring->netdev) &&
+	    igb_desc_unused(tx_ring) >= TX_WAKE_THRESHOLD)) {
 		/* Make sure that anybody stopping the queue after this
 		 * sees the new next_to_clean.
 		 */
@@ -6069,11 +6143,11 @@ static bool igb_clean_tx_irq(struct igb_q_vector *q_vector)
 }
 
 /**
- * igb_reuse_rx_page - page flip buffer and store it back on the ring
- * @rx_ring: rx descriptor ring to store buffers on
- * @old_buff: donor buffer to have page reused
+ *  igb_reuse_rx_page - page flip buffer and store it back on the ring
+ *  @rx_ring: rx descriptor ring to store buffers on
+ *  @old_buff: donor buffer to have page reused
  *
- * Synchronizes page for reuse by the adapter
+ *  Synchronizes page for reuse by the adapter
  **/
 static void igb_reuse_rx_page(struct igb_ring *rx_ring,
 			      struct igb_rx_buffer *old_buff)
@@ -6133,19 +6207,19 @@ static bool igb_can_reuse_rx_page(struct igb_rx_buffer *rx_buffer,
 }
 
 /**
- * igb_add_rx_frag - Add contents of Rx buffer to sk_buff
- * @rx_ring: rx descriptor ring to transact packets on
- * @rx_buffer: buffer containing page to add
- * @rx_desc: descriptor containing length of buffer written by hardware
- * @skb: sk_buff to place the data into
+ *  igb_add_rx_frag - Add contents of Rx buffer to sk_buff
+ *  @rx_ring: rx descriptor ring to transact packets on
+ *  @rx_buffer: buffer containing page to add
+ *  @rx_desc: descriptor containing length of buffer written by hardware
+ *  @skb: sk_buff to place the data into
  *
- * This function will add the data contained in rx_buffer->page to the skb.
- * This is done either through a direct copy if the data in the buffer is
- * less than the skb header size, otherwise it will just attach the page as
- * a frag to the skb.
+ *  This function will add the data contained in rx_buffer->page to the skb.
+ *  This is done either through a direct copy if the data in the buffer is
+ *  less than the skb header size, otherwise it will just attach the page as
+ *  a frag to the skb.
  *
- * The function will then update the page offset if necessary and return
- * true if the buffer can be reused by the adapter.
+ *  The function will then update the page offset if necessary and return
+ *  true if the buffer can be reused by the adapter.
  **/
 static bool igb_add_rx_frag(struct igb_ring *rx_ring,
 			    struct igb_rx_buffer *rx_buffer,
@@ -6216,8 +6290,7 @@ static struct sk_buff *igb_fetch_rx_buffer(struct igb_ring *rx_ring,
 			return NULL;
 		}
 
-		/*
-		 * we will be copying header into skb->data in
+		/* we will be copying header into skb->data in
 		 * pskb_may_pull so it is in our interest to prefetch
 		 * it now to avoid a possible cache miss
 		 */
@@ -6265,8 +6338,7 @@ static inline void igb_rx_checksum(struct igb_ring *ring,
 	if (igb_test_staterr(rx_desc,
 			     E1000_RXDEXT_STATERR_TCPE |
 			     E1000_RXDEXT_STATERR_IPE)) {
-		/*
-		 * work around errata with sctp packets where the TCPE aka
+		/* work around errata with sctp packets where the TCPE aka
 		 * L4E bit is set incorrectly on 64 byte (60 byte w/o crc)
 		 * packets, (aka let the stack check the crc32c)
 		 */
@@ -6297,15 +6369,15 @@ static inline void igb_rx_hash(struct igb_ring *ring,
 }
 
 /**
- * igb_is_non_eop - process handling of non-EOP buffers
- * @rx_ring: Rx ring being processed
- * @rx_desc: Rx descriptor for current buffer
- * @skb: current socket buffer containing buffer in progress
+ *  igb_is_non_eop - process handling of non-EOP buffers
+ *  @rx_ring: Rx ring being processed
+ *  @rx_desc: Rx descriptor for current buffer
+ *  @skb: current socket buffer containing buffer in progress
  *
- * This function updates next to clean.  If the buffer is an EOP buffer
- * this function exits returning false, otherwise it will place the
- * sk_buff in the next buffer to be chained and return true indicating
- * that this is in fact a non-EOP buffer.
+ *  This function updates next to clean.  If the buffer is an EOP buffer
+ *  this function exits returning false, otherwise it will place the
+ *  sk_buff in the next buffer to be chained and return true indicating
+ *  that this is in fact a non-EOP buffer.
  **/
 static bool igb_is_non_eop(struct igb_ring *rx_ring,
 			   union e1000_adv_rx_desc *rx_desc)
@@ -6325,15 +6397,15 @@ static bool igb_is_non_eop(struct igb_ring *rx_ring,
 }
 
 /**
- * igb_get_headlen - determine size of header for LRO/GRO
- * @data: pointer to the start of the headers
- * @max_len: total length of section to find headers in
+ *  igb_get_headlen - determine size of header for LRO/GRO
+ *  @data: pointer to the start of the headers
+ *  @max_len: total length of section to find headers in
  *
- * This function is meant to determine the length of headers that will
- * be recognized by hardware for LRO, and GRO offloads.  The main
- * motivation of doing this is to only perform one pull for IPv4 TCP
- * packets so that we can do basic things like calculating the gso_size
- * based on the average data per packet.
+ *  This function is meant to determine the length of headers that will
+ *  be recognized by hardware for LRO, and GRO offloads.  The main
+ *  motivation of doing this is to only perform one pull for IPv4 TCP
+ *  packets so that we can do basic things like calculating the gso_size
+ *  based on the average data per packet.
  **/
 static unsigned int igb_get_headlen(unsigned char *data,
 				    unsigned int max_len)
@@ -6384,7 +6456,7 @@ static unsigned int igb_get_headlen(unsigned char *data,
 			return hdr.network - data;
 
 		/* record next protocol if header is present */
-		if (!hdr.ipv4->frag_off)
+		if (!(hdr.ipv4->frag_off & htons(IP_OFFSET)))
 			nexthdr = hdr.ipv4->protocol;
 	} else if (protocol == __constant_htons(ETH_P_IPV6)) {
 		if ((hdr.network - data) > (max_len - sizeof(struct ipv6hdr)))
@@ -6420,8 +6492,7 @@ static unsigned int igb_get_headlen(unsigned char *data,
 		hdr.network += sizeof(struct udphdr);
 	}
 
-	/*
-	 * If everything has gone correctly hdr.network should be the
+	/* If everything has gone correctly hdr.network should be the
 	 * data section of the packet and will be the end of the header.
 	 * If not then it probably represents the end of the last recognized
 	 * header.
@@ -6433,17 +6504,17 @@ static unsigned int igb_get_headlen(unsigned char *data,
 }
 
 /**
- * igb_pull_tail - igb specific version of skb_pull_tail
- * @rx_ring: rx descriptor ring packet is being transacted on
- * @rx_desc: pointer to the EOP Rx descriptor
- * @skb: pointer to current skb being adjusted
+ *  igb_pull_tail - igb specific version of skb_pull_tail
+ *  @rx_ring: rx descriptor ring packet is being transacted on
+ *  @rx_desc: pointer to the EOP Rx descriptor
+ *  @skb: pointer to current skb being adjusted
  *
- * This function is an igb specific version of __pskb_pull_tail.  The
- * main difference between this version and the original function is that
- * this function can make several assumptions about the state of things
- * that allow for significant optimizations versus the standard function.
- * As a result we can do things like drop a frag and maintain an accurate
- * truesize for the skb.
+ *  This function is an igb specific version of __pskb_pull_tail.  The
+ *  main difference between this version and the original function is that
+ *  this function can make several assumptions about the state of things
+ *  that allow for significant optimizations versus the standard function.
+ *  As a result we can do things like drop a frag and maintain an accurate
+ *  truesize for the skb.
  */
 static void igb_pull_tail(struct igb_ring *rx_ring,
 			  union e1000_adv_rx_desc *rx_desc,
@@ -6453,8 +6524,7 @@ static void igb_pull_tail(struct igb_ring *rx_ring,
 	unsigned char *va;
 	unsigned int pull_len;
 
-	/*
-	 * it is valid to use page_address instead of kmap since we are
+	/* it is valid to use page_address instead of kmap since we are
 	 * working with pages allocated out of the lomem pool per
 	 * alloc_page(GFP_ATOMIC)
 	 */
@@ -6474,8 +6544,7 @@ static void igb_pull_tail(struct igb_ring *rx_ring,
 		va += IGB_TS_HDR_LEN;
 	}
 
-	/*
-	 * we need the header to contain the greater of either ETH_HLEN or
+	/* we need the header to contain the greater of either ETH_HLEN or
 	 * 60 bytes if the skb->len is less than 60 for skb_pad.
 	 */
 	pull_len = igb_get_headlen(va, IGB_RX_HDR_LEN);
@@ -6491,24 +6560,23 @@ static void igb_pull_tail(struct igb_ring *rx_ring,
 }
 
 /**
- * igb_cleanup_headers - Correct corrupted or empty headers
- * @rx_ring: rx descriptor ring packet is being transacted on
- * @rx_desc: pointer to the EOP Rx descriptor
- * @skb: pointer to current skb being fixed
+ *  igb_cleanup_headers - Correct corrupted or empty headers
+ *  @rx_ring: rx descriptor ring packet is being transacted on
+ *  @rx_desc: pointer to the EOP Rx descriptor
+ *  @skb: pointer to current skb being fixed
  *
- * Address the case where we are pulling data in on pages only
- * and as such no data is present in the skb header.
+ *  Address the case where we are pulling data in on pages only
+ *  and as such no data is present in the skb header.
  *
- * In addition if skb is not at least 60 bytes we need to pad it so that
- * it is large enough to qualify as a valid Ethernet frame.
+ *  In addition if skb is not at least 60 bytes we need to pad it so that
+ *  it is large enough to qualify as a valid Ethernet frame.
  *
- * Returns true if an error was encountered and skb was freed.
+ *  Returns true if an error was encountered and skb was freed.
  **/
 static bool igb_cleanup_headers(struct igb_ring *rx_ring,
 				union e1000_adv_rx_desc *rx_desc,
 				struct sk_buff *skb)
 {
-
 	if (unlikely((igb_test_staterr(rx_desc,
 				       E1000_RXDEXT_ERR_FRAME_ERR_MASK)))) {
 		struct net_device *netdev = rx_ring->netdev;
@@ -6535,14 +6603,14 @@ static bool igb_cleanup_headers(struct igb_ring *rx_ring,
 }
 
 /**
- * igb_process_skb_fields - Populate skb header fields from Rx descriptor
- * @rx_ring: rx descriptor ring packet is being transacted on
- * @rx_desc: pointer to the EOP Rx descriptor
- * @skb: pointer to current skb being populated
+ *  igb_process_skb_fields - Populate skb header fields from Rx descriptor
+ *  @rx_ring: rx descriptor ring packet is being transacted on
+ *  @rx_desc: pointer to the EOP Rx descriptor
+ *  @skb: pointer to current skb being populated
  *
- * This function checks the ring, descriptor, and packet information in
- * order to populate the hash, checksum, VLAN, timestamp, protocol, and
- * other fields within the skb.
+ *  This function checks the ring, descriptor, and packet information in
+ *  order to populate the hash, checksum, VLAN, timestamp, protocol, and
+ *  other fields within the skb.
  **/
 static void igb_process_skb_fields(struct igb_ring *rx_ring,
 				   union e1000_adv_rx_desc *rx_desc,
@@ -6556,7 +6624,7 @@ static void igb_process_skb_fields(struct igb_ring *rx_ring,
 
 	igb_ptp_rx_hwtstamp(rx_ring->q_vector, rx_desc, skb);
 
-	if ((dev->features & NETIF_F_HW_VLAN_RX) &&
+	if ((dev->features & NETIF_F_HW_VLAN_CTAG_RX) &&
 	    igb_test_staterr(rx_desc, E1000_RXD_STAT_VP)) {
 		u16 vid;
 		if (igb_test_staterr(rx_desc, E1000_RXDEXT_STATERR_LB) &&
@@ -6565,7 +6633,7 @@ static void igb_process_skb_fields(struct igb_ring *rx_ring,
 		else
 			vid = le16_to_cpu(rx_desc->wb.upper.vlan);
 
-		__vlan_hwaccel_put_tag(skb, vid);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
 	}
 
 	skb_record_rx_queue(skb, rx_ring->queue_index);
@@ -6670,8 +6738,7 @@ static bool igb_alloc_mapped_page(struct igb_ring *rx_ring,
 	/* map page for use */
 	dma = dma_map_page(rx_ring->dev, page, 0, PAGE_SIZE, DMA_FROM_DEVICE);
 
-	/*
-	 * if mapping failed free memory back to system since
+	/* if mapping failed free memory back to system since
 	 * there isn't much point in holding memory we can't use
 	 */
 	if (dma_mapping_error(rx_ring->dev, dma)) {
@@ -6689,8 +6756,8 @@ static bool igb_alloc_mapped_page(struct igb_ring *rx_ring,
 }
 
 /**
- * igb_alloc_rx_buffers - Replace used receive buffers; packet split
- * @adapter: address of board private structure
+ *  igb_alloc_rx_buffers - Replace used receive buffers; packet split
+ *  @adapter: address of board private structure
  **/
 void igb_alloc_rx_buffers(struct igb_ring *rx_ring, u16 cleaned_count)
 {
@@ -6710,8 +6777,7 @@ void igb_alloc_rx_buffers(struct igb_ring *rx_ring, u16 cleaned_count)
 		if (!igb_alloc_mapped_page(rx_ring, bi))
 			break;
 
-		/*
-		 * Refresh the desc even if buffer_addrs didn't change
+		/* Refresh the desc even if buffer_addrs didn't change
 		 * because each write-back erases this info.
 		 */
 		rx_desc->read.pkt_addr = cpu_to_le64(bi->dma + bi->page_offset);
@@ -6740,8 +6806,7 @@ void igb_alloc_rx_buffers(struct igb_ring *rx_ring, u16 cleaned_count)
 		/* update next to alloc since we have filled the ring */
 		rx_ring->next_to_alloc = i;
 
-		/*
-		 * Force memory writes to complete before letting h/w
+		/* Force memory writes to complete before letting h/w
 		 * know there are new descriptors to fetch.  (Only
 		 * applicable for weak-ordered memory model archs,
 		 * such as IA-64).
@@ -6826,7 +6891,7 @@ static void igb_vlan_mode(struct net_device *netdev, netdev_features_t features)
 	struct igb_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
 	u32 ctrl, rctl;
-	bool enable = !!(features & NETIF_F_HW_VLAN_RX);
+	bool enable = !!(features & NETIF_F_HW_VLAN_CTAG_RX);
 
 	if (enable) {
 		/* enable VLAN tag insert/strip */
@@ -6848,7 +6913,8 @@ static void igb_vlan_mode(struct net_device *netdev, netdev_features_t features)
 	igb_rlpml_set(adapter);
 }
 
-static int igb_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
+static int igb_vlan_rx_add_vid(struct net_device *netdev,
+			       __be16 proto, u16 vid)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
@@ -6865,7 +6931,8 @@ static int igb_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
 	return 0;
 }
 
-static int igb_vlan_rx_kill_vid(struct net_device *netdev, u16 vid)
+static int igb_vlan_rx_kill_vid(struct net_device *netdev,
+				__be16 proto, u16 vid)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
@@ -6891,7 +6958,7 @@ static void igb_restore_vlan(struct igb_adapter *adapter)
 	igb_vlan_mode(adapter->netdev, adapter->netdev->features);
 
 	for_each_set_bit(vid, adapter->active_vlans, VLAN_N_VID)
-		igb_vlan_rx_add_vid(adapter->netdev, vid);
+		igb_vlan_rx_add_vid(adapter->netdev, htons(ETH_P_8021Q), vid);
 }
 
 int igb_set_spd_dplx(struct igb_adapter *adapter, u32 spd, u8 dplx)
@@ -6902,15 +6969,24 @@ int igb_set_spd_dplx(struct igb_adapter *adapter, u32 spd, u8 dplx)
 	mac->autoneg = 0;
 
 	/* Make sure dplx is at most 1 bit and lsb of speed is not set
-	 * for the switch() below to work */
+	 * for the switch() below to work
+	 */
 	if ((spd & 1) || (dplx & ~1))
 		goto err_inval;
 
-	/* Fiber NIC's only allow 1000 Gbps Full duplex */
-	if ((adapter->hw.phy.media_type == e1000_media_type_internal_serdes) &&
-	    spd != SPEED_1000 &&
-	    dplx != DUPLEX_FULL)
-		goto err_inval;
+	/* Fiber NIC's only allow 1000 gbps Full duplex
+	 * and 100Mbps Full duplex for 100baseFx sfp
+	 */
+	if (adapter->hw.phy.media_type == e1000_media_type_internal_serdes) {
+		switch (spd + dplx) {
+		case SPEED_10 + DUPLEX_HALF:
+		case SPEED_10 + DUPLEX_FULL:
+		case SPEED_100 + DUPLEX_HALF:
+			goto err_inval;
+		default:
+			break;
+		}
+	}
 
 	switch (spd + dplx) {
 	case SPEED_10 + DUPLEX_HALF:
@@ -7009,7 +7085,8 @@ static int __igb_shutdown(struct pci_dev *pdev, bool *enable_wake,
 		igb_power_up_link(adapter);
 
 	/* Release control of h/w to f/w.  If f/w is AMT enabled, this
-	 * would have already happened in close and is redundant. */
+	 * would have already happened in close and is redundant.
+	 */
 	igb_release_hw_control(adapter);
 
 	pci_disable_device(pdev);
@@ -7071,7 +7148,8 @@ static int igb_resume(struct device *dev)
 	igb_reset(adapter);
 
 	/* let the f/w know that the h/w is now under the control of the
-	 * driver. */
+	 * driver.
+	 */
 	igb_get_hw_control(adapter);
 
 	wr32(E1000_WUS, ~0);
@@ -7207,8 +7285,7 @@ static int igb_pci_sriov_configure(struct pci_dev *dev, int num_vfs)
 }
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
-/*
- * Polling 'interrupt' - used by things like netconsole to send skbs
+/* Polling 'interrupt' - used by things like netconsole to send skbs
  * without having to re-enable interrupts. It's not called while
  * the interrupt routine is executing.
  */
@@ -7231,13 +7308,13 @@ static void igb_netpoll(struct net_device *netdev)
 #endif /* CONFIG_NET_POLL_CONTROLLER */
 
 /**
- * igb_io_error_detected - called when PCI error is detected
- * @pdev: Pointer to PCI device
- * @state: The current pci connection state
+ *  igb_io_error_detected - called when PCI error is detected
+ *  @pdev: Pointer to PCI device
+ *  @state: The current pci connection state
  *
- * This function is called after a PCI bus error affecting
- * this device has been detected.
- */
+ *  This function is called after a PCI bus error affecting
+ *  this device has been detected.
+ **/
 static pci_ers_result_t igb_io_error_detected(struct pci_dev *pdev,
 					      pci_channel_state_t state)
 {
@@ -7258,12 +7335,12 @@ static pci_ers_result_t igb_io_error_detected(struct pci_dev *pdev,
 }
 
 /**
- * igb_io_slot_reset - called after the pci bus has been reset.
- * @pdev: Pointer to PCI device
+ *  igb_io_slot_reset - called after the pci bus has been reset.
+ *  @pdev: Pointer to PCI device
  *
- * Restart the card from scratch, as if from a cold-boot. Implementation
- * resembles the first-half of the igb_resume routine.
- */
+ *  Restart the card from scratch, as if from a cold-boot. Implementation
+ *  resembles the first-half of the igb_resume routine.
+ **/
 static pci_ers_result_t igb_io_slot_reset(struct pci_dev *pdev)
 {
 	struct net_device *netdev = pci_get_drvdata(pdev);
@@ -7291,8 +7368,9 @@ static pci_ers_result_t igb_io_slot_reset(struct pci_dev *pdev)
 
 	err = pci_cleanup_aer_uncorrect_error_status(pdev);
 	if (err) {
-		dev_err(&pdev->dev, "pci_cleanup_aer_uncorrect_error_status "
-		        "failed 0x%0x\n", err);
+		dev_err(&pdev->dev,
+			"pci_cleanup_aer_uncorrect_error_status failed 0x%0x\n",
+			err);
 		/* non-fatal, continue */
 	}
 
@@ -7300,12 +7378,12 @@ static pci_ers_result_t igb_io_slot_reset(struct pci_dev *pdev)
 }
 
 /**
- * igb_io_resume - called when traffic can start flowing again.
- * @pdev: Pointer to PCI device
+ *  igb_io_resume - called when traffic can start flowing again.
+ *  @pdev: Pointer to PCI device
  *
- * This callback is called when the error recovery driver tells us that
- * its OK to resume normal operation. Implementation resembles the
- * second-half of the igb_resume routine.
+ *  This callback is called when the error recovery driver tells us that
+ *  its OK to resume normal operation. Implementation resembles the
+ *  second-half of the igb_resume routine.
  */
 static void igb_io_resume(struct pci_dev *pdev)
 {
@@ -7322,12 +7400,13 @@ static void igb_io_resume(struct pci_dev *pdev)
 	netif_device_attach(netdev);
 
 	/* let the f/w know that the h/w is now under the control of the
-	 * driver. */
+	 * driver.
+	 */
 	igb_get_hw_control(adapter);
 }
 
 static void igb_rar_set_qsel(struct igb_adapter *adapter, u8 *addr, u32 index,
-                             u8 qsel)
+			     u8 qsel)
 {
 	u32 rar_low, rar_high;
 	struct e1000_hw *hw = &adapter->hw;
@@ -7336,7 +7415,7 @@ static void igb_rar_set_qsel(struct igb_adapter *adapter, u8 *addr, u32 index,
 	 * from network order (big endian) to little endian
 	 */
 	rar_low = ((u32) addr[0] | ((u32) addr[1] << 8) |
-	          ((u32) addr[2] << 16) | ((u32) addr[3] << 24));
+		   ((u32) addr[2] << 16) | ((u32) addr[3] << 24));
 	rar_high = ((u32) addr[4] | ((u32) addr[5] << 8));
 
 	/* Indicate to hardware the Address is Valid. */
@@ -7354,11 +7433,12 @@ static void igb_rar_set_qsel(struct igb_adapter *adapter, u8 *addr, u32 index,
 }
 
 static int igb_set_vf_mac(struct igb_adapter *adapter,
-                          int vf, unsigned char *mac_addr)
+			  int vf, unsigned char *mac_addr)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	/* VF MAC addresses start at end of receive addresses and moves
-	 * torwards the first, as a result a collision should not be possible */
+	 * towards the first, as a result a collision should not be possible
+	 */
 	int rar_entry = hw->mac.rar_entry_count - (vf + 1);
 
 	memcpy(adapter->vf_data[vf].vf_mac_addresses, mac_addr, ETH_ALEN);
@@ -7375,13 +7455,13 @@ static int igb_ndo_set_vf_mac(struct net_device *netdev, int vf, u8 *mac)
 		return -EINVAL;
 	adapter->vf_data[vf].flags |= IGB_VF_FLAG_PF_SET_MAC;
 	dev_info(&adapter->pdev->dev, "setting MAC %pM on VF %d\n", mac, vf);
-	dev_info(&adapter->pdev->dev, "Reload the VF driver to make this"
-				      " change effective.");
+	dev_info(&adapter->pdev->dev,
+		 "Reload the VF driver to make this change effective.");
 	if (test_bit(__IGB_DOWN, &adapter->state)) {
-		dev_warn(&adapter->pdev->dev, "The VF MAC address has been set,"
-			 " but the PF device is not up.\n");
-		dev_warn(&adapter->pdev->dev, "Bring the PF device up before"
-			 " attempting to use the VF device.\n");
+		dev_warn(&adapter->pdev->dev,
+			 "The VF MAC address has been set, but the PF device is not up.\n");
+		dev_warn(&adapter->pdev->dev,
+			 "Bring the PF device up before attempting to use the VF device.\n");
 	}
 	return igb_set_vf_mac(adapter, vf, mac);
 }
@@ -7408,19 +7488,19 @@ static void igb_set_vf_rate_limit(struct e1000_hw *hw, int vf, int tx_rate,
 		/* Calculate the rate factor values to set */
 		rf_int = link_speed / tx_rate;
 		rf_dec = (link_speed - (rf_int * tx_rate));
-		rf_dec = (rf_dec * (1<<E1000_RTTBCNRC_RF_INT_SHIFT)) / tx_rate;
+		rf_dec = (rf_dec * (1 << E1000_RTTBCNRC_RF_INT_SHIFT)) /
+			 tx_rate;
 
 		bcnrc_val = E1000_RTTBCNRC_RS_ENA;
-		bcnrc_val |= ((rf_int<<E1000_RTTBCNRC_RF_INT_SHIFT) &
-		               E1000_RTTBCNRC_RF_INT_MASK);
+		bcnrc_val |= ((rf_int << E1000_RTTBCNRC_RF_INT_SHIFT) &
+			      E1000_RTTBCNRC_RF_INT_MASK);
 		bcnrc_val |= (rf_dec & E1000_RTTBCNRC_RF_DEC_MASK);
 	} else {
 		bcnrc_val = 0;
 	}
 
 	wr32(E1000_RTTDQSEL, vf); /* vf X uses queue X */
-	/*
-	 * Set global transmit compensation time to the MMW_SIZE in RTTBCNRM
+	/* Set global transmit compensation time to the MMW_SIZE in RTTBCNRM
 	 * register. MMW_SIZE=0x014 if 9728-byte jumbo is supported.
 	 */
 	wr32(E1000_RTTBCNRM, 0x14);
@@ -7442,8 +7522,7 @@ static void igb_check_vf_rate_limit(struct igb_adapter *adapter)
 		reset_rate = true;
 		adapter->vf_rate_link_speed = 0;
 		dev_info(&adapter->pdev->dev,
-		         "Link speed has been changed. VF Transmit "
-		         "rate is disabled\n");
+			 "Link speed has been changed. VF Transmit rate is disabled\n");
 	}
 
 	for (i = 0; i < adapter->vfs_allocated_count; i++) {
@@ -7451,8 +7530,8 @@ static void igb_check_vf_rate_limit(struct igb_adapter *adapter)
 			adapter->vf_data[i].tx_rate = 0;
 
 		igb_set_vf_rate_limit(&adapter->hw, i,
-		                      adapter->vf_data[i].tx_rate,
-		                      actual_link_speed);
+				      adapter->vf_data[i].tx_rate,
+				      actual_link_speed);
 	}
 }
 
@@ -7478,6 +7557,33 @@ static int igb_ndo_set_vf_bw(struct net_device *netdev, int vf, int tx_rate)
 	return 0;
 }
 
+static int igb_ndo_set_vf_spoofchk(struct net_device *netdev, int vf,
+				   bool setting)
+{
+	struct igb_adapter *adapter = netdev_priv(netdev);
+	struct e1000_hw *hw = &adapter->hw;
+	u32 reg_val, reg_offset;
+
+	if (!adapter->vfs_allocated_count)
+		return -EOPNOTSUPP;
+
+	if (vf >= adapter->vfs_allocated_count)
+		return -EINVAL;
+
+	reg_offset = (hw->mac.type == e1000_82576) ? E1000_DTXSWC : E1000_TXSWC;
+	reg_val = rd32(reg_offset);
+	if (setting)
+		reg_val |= ((1 << vf) |
+			    (1 << (vf + E1000_DTXSWC_VLAN_SPOOF_SHIFT)));
+	else
+		reg_val &= ~((1 << vf) |
+			     (1 << (vf + E1000_DTXSWC_VLAN_SPOOF_SHIFT)));
+	wr32(reg_offset, reg_val);
+
+	adapter->vf_data[vf].spoofchk_enabled = setting;
+	return E1000_SUCCESS;
+}
+
 static int igb_ndo_get_vf_config(struct net_device *netdev,
 				 int vf, struct ifla_vf_info *ivi)
 {
@@ -7489,6 +7595,7 @@ static int igb_ndo_get_vf_config(struct net_device *netdev,
 	ivi->tx_rate = adapter->vf_data[vf].tx_rate;
 	ivi->vlan = adapter->vf_data[vf].pf_vlan;
 	ivi->qos = adapter->vf_data[vf].pf_qos;
+	ivi->spoofchk = adapter->vf_data[vf].spoofchk_enabled;
 	return 0;
 }
 
@@ -7501,6 +7608,7 @@ static void igb_vmm_control(struct igb_adapter *adapter)
 	case e1000_82575:
 	case e1000_i210:
 	case e1000_i211:
+	case e1000_i354:
 	default:
 		/* replication is not supported for 82575 */
 		return;
@@ -7523,7 +7631,7 @@ static void igb_vmm_control(struct igb_adapter *adapter)
 		igb_vmdq_set_loopback_pf(hw, true);
 		igb_vmdq_set_replication_pf(hw, true);
 		igb_vmdq_set_anti_spoofing_pf(hw, true,
-						adapter->vfs_allocated_count);
+					      adapter->vfs_allocated_count);
 	} else {
 		igb_vmdq_set_loopback_pf(hw, false);
 		igb_vmdq_set_replication_pf(hw, false);
@@ -7543,8 +7651,7 @@ static void igb_init_dmac(struct igb_adapter *adapter, u32 pba)
 			/* force threshold to 0. */
 			wr32(E1000_DMCTXTH, 0);
 
-			/*
-			 * DMA Coalescing high water mark needs to be greater
+			/* DMA Coalescing high water mark needs to be greater
 			 * than the Rx threshold. Set hwm to PBA - max frame
 			 * size in 16B units, capping it at PBA - 6KB.
 			 */
@@ -7557,8 +7664,7 @@ static void igb_init_dmac(struct igb_adapter *adapter, u32 pba)
 				& E1000_FCRTC_RTH_COAL_MASK);
 			wr32(E1000_FCRTC, reg);
 
-			/*
-			 * Set the DMA Coalescing Rx threshold to PBA - 2 * max
+			/* Set the DMA Coalescing Rx threshold to PBA - 2 * max
 			 * frame size, capping it at PBA - 10KB.
 			 */
 			dmac_thr = pba - adapter->max_frame_size / 512;
@@ -7576,11 +7682,12 @@ static void igb_init_dmac(struct igb_adapter *adapter, u32 pba)
 			reg |= (1000 >> 5);
 
 			/* Disable BMC-to-OS Watchdog Enable */
-			reg &= ~E1000_DMACR_DC_BMC2OSW_EN;
+			if (hw->mac.type != e1000_i354)
+				reg &= ~E1000_DMACR_DC_BMC2OSW_EN;
+
 			wr32(E1000_DMACR, reg);
 
-			/*
-			 * no lower threshold to disable
+			/* no lower threshold to disable
 			 * coalescing(smart fifb)-UTRESH=0
 			 */
 			wr32(E1000_DMCRTRH, 0);
@@ -7589,15 +7696,13 @@ static void igb_init_dmac(struct igb_adapter *adapter, u32 pba)
 
 			wr32(E1000_DMCTLX, reg);
 
-			/*
-			 * free space in tx packet buffer to wake from
+			/* free space in tx packet buffer to wake from
 			 * DMA coal
 			 */
 			wr32(E1000_DMCTXTH, (IGB_MIN_TXPBSIZE -
 			     (IGB_TX_BUF_4096 + adapter->max_frame_size)) >> 6);
 
-			/*
-			 * make low power state decision controlled
+			/* make low power state decision controlled
 			 * by DMA coal
 			 */
 			reg = rd32(E1000_PCIEMISC);
@@ -7611,7 +7716,8 @@ static void igb_init_dmac(struct igb_adapter *adapter, u32 pba)
 	}
 }
 
-/*  igb_read_i2c_byte - Reads 8 bit word over I2C
+/**
+ *  igb_read_i2c_byte - Reads 8 bit word over I2C
  *  @hw: pointer to hardware structure
  *  @byte_offset: byte offset to read
  *  @dev_addr: device address
@@ -7619,9 +7725,9 @@ static void igb_init_dmac(struct igb_adapter *adapter, u32 pba)
  *
  *  Performs byte read operation over I2C interface at
  *  a specified device address.
- */
+ **/
 s32 igb_read_i2c_byte(struct e1000_hw *hw, u8 byte_offset,
-				u8 dev_addr, u8 *data)
+		      u8 dev_addr, u8 *data)
 {
 	struct igb_adapter *adapter = container_of(hw, struct igb_adapter, hw);
 	struct i2c_client *this_client = adapter->i2c_client;
@@ -7648,7 +7754,8 @@ s32 igb_read_i2c_byte(struct e1000_hw *hw, u8 byte_offset,
 	}
 }
 
-/*  igb_write_i2c_byte - Writes 8 bit word over I2C
+/**
+ *  igb_write_i2c_byte - Writes 8 bit word over I2C
  *  @hw: pointer to hardware structure
  *  @byte_offset: byte offset to write
  *  @dev_addr: device address
@@ -7656,9 +7763,9 @@ s32 igb_read_i2c_byte(struct e1000_hw *hw, u8 byte_offset,
  *
  *  Performs byte write operation over I2C interface at
  *  a specified device address.
- */
+ **/
 s32 igb_write_i2c_byte(struct e1000_hw *hw, u8 byte_offset,
-				 u8 dev_addr, u8 data)
+		       u8 dev_addr, u8 data)
 {
 	struct igb_adapter *adapter = container_of(hw, struct igb_adapter, hw);
 	struct i2c_client *this_client = adapter->i2c_client;
diff --git a/drivers/net/ethernet/intel/igb/igb_ptp.c b/drivers/net/ethernet/intel/igb/igb_ptp.c
index 0a237507ee85..7e8c477b0ab9 100644
--- a/drivers/net/ethernet/intel/igb/igb_ptp.c
+++ b/drivers/net/ethernet/intel/igb/igb_ptp.c
@@ -1,5 +1,4 @@
-/*
- * PTP Hardware Clock (PHC) driver for the Intel 82576 and 82580
+/* PTP Hardware Clock (PHC) driver for the Intel 82576 and 82580
  *
  * Copyright (C) 2011 Richard Cochran <richardcochran@gmail.com>
  *
@@ -27,8 +26,7 @@
 #define INCVALUE_MASK		0x7fffffff
 #define ISGN			0x80000000
 
-/*
- * The 82580 timesync updates the system timer every 8ns by 8ns,
+/* The 82580 timesync updates the system timer every 8ns by 8ns,
  * and this update value cannot be reprogrammed.
  *
  * Neither the 82576 nor the 82580 offer registers wide enough to hold
@@ -77,10 +75,7 @@
 #define INCVALUE_82576			(16 << IGB_82576_TSYNC_SHIFT)
 #define IGB_NBITS_82580			40
 
-/*
- * SYSTIM read access for the 82576
- */
-
+/* SYSTIM read access for the 82576 */
 static cycle_t igb_ptp_read_82576(const struct cyclecounter *cc)
 {
 	struct igb_adapter *igb = container_of(cc, struct igb_adapter, cc);
@@ -97,10 +92,7 @@ static cycle_t igb_ptp_read_82576(const struct cyclecounter *cc)
 	return val;
 }
 
-/*
- * SYSTIM read access for the 82580
- */
-
+/* SYSTIM read access for the 82580 */
 static cycle_t igb_ptp_read_82580(const struct cyclecounter *cc)
 {
 	struct igb_adapter *igb = container_of(cc, struct igb_adapter, cc);
@@ -108,8 +100,7 @@ static cycle_t igb_ptp_read_82580(const struct cyclecounter *cc)
 	u64 val;
 	u32 lo, hi, jk;
 
-	/*
-	 * The timestamp latches on lowest register read. For the 82580
+	/* The timestamp latches on lowest register read. For the 82580
 	 * the lowest register is SYSTIMR instead of SYSTIML.  However we only
 	 * need to provide nanosecond resolution, so we just ignore it.
 	 */
@@ -123,17 +114,13 @@ static cycle_t igb_ptp_read_82580(const struct cyclecounter *cc)
 	return val;
 }
 
-/*
- * SYSTIM read access for I210/I211
- */
-
+/* SYSTIM read access for I210/I211 */
 static void igb_ptp_read_i210(struct igb_adapter *adapter, struct timespec *ts)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	u32 sec, nsec, jk;
 
-	/*
-	 * The timestamp latches on lowest register read. For I210/I211, the
+	/* The timestamp latches on lowest register read. For I210/I211, the
 	 * lowest register is SYSTIMR. Since we only need to provide nanosecond
 	 * resolution, we can ignore it.
 	 */
@@ -150,8 +137,7 @@ static void igb_ptp_write_i210(struct igb_adapter *adapter,
 {
 	struct e1000_hw *hw = &adapter->hw;
 
-	/*
-	 * Writing the SYSTIMR register is not necessary as it only provides
+	/* Writing the SYSTIMR register is not necessary as it only provides
 	 * sub-nanosecond resolution.
 	 */
 	wr32(E1000_SYSTIML, ts->tv_nsec);
@@ -185,6 +171,7 @@ static void igb_ptp_systim_to_hwtstamp(struct igb_adapter *adapter,
 	switch (adapter->hw.mac.type) {
 	case e1000_82576:
 	case e1000_82580:
+	case e1000_i354:
 	case e1000_i350:
 		spin_lock_irqsave(&adapter->tmreg_lock, flags);
 
@@ -207,10 +194,7 @@ static void igb_ptp_systim_to_hwtstamp(struct igb_adapter *adapter,
 	}
 }
 
-/*
- * PTP clock operations
- */
-
+/* PTP clock operations */
 static int igb_ptp_adjfreq_82576(struct ptp_clock_info *ptp, s32 ppb)
 {
 	struct igb_adapter *igb = container_of(ptp, struct igb_adapter,
@@ -387,7 +371,7 @@ static int igb_ptp_enable(struct ptp_clock_info *ptp,
  *
  * This work function polls the TSYNCTXCTL valid bit to determine when a
  * timestamp has been taken for the current stored skb.
- */
+ **/
 void igb_ptp_tx_work(struct work_struct *work)
 {
 	struct igb_adapter *adapter = container_of(work, struct igb_adapter,
@@ -437,7 +421,7 @@ static void igb_ptp_overflow_check(struct work_struct *work)
  * dropped an Rx packet that was timestamped when the ring is full. The
  * particular error is rare but leaves the device in a state unable to timestamp
  * any future packets.
- */
+ **/
 void igb_ptp_rx_hang(struct igb_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
@@ -481,7 +465,7 @@ void igb_ptp_rx_hang(struct igb_adapter *adapter)
  * If we were asked to do hardware stamping and such a time stamp is
  * available, then it must have been for this skb here because we only
  * allow only one such packet into the queue.
- */
+ **/
 void igb_ptp_tx_hwtstamp(struct igb_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
@@ -506,15 +490,14 @@ void igb_ptp_tx_hwtstamp(struct igb_adapter *adapter)
  * This function is meant to retrieve a timestamp from the first buffer of an
  * incoming frame.  The value is stored in little endian format starting on
  * byte 8.
- */
+ **/
 void igb_ptp_rx_pktstamp(struct igb_q_vector *q_vector,
 			 unsigned char *va,
 			 struct sk_buff *skb)
 {
 	__le64 *regval = (__le64 *)va;
 
-	/*
-	 * The timestamp is recorded in little endian format.
+	/* The timestamp is recorded in little endian format.
 	 * DWORD: 0        1        2        3
 	 * Field: Reserved Reserved SYSTIML  SYSTIMH
 	 */
@@ -529,7 +512,7 @@ void igb_ptp_rx_pktstamp(struct igb_q_vector *q_vector,
  *
  * This function is meant to retrieve a timestamp from the internal registers
  * of the adapter and store it in the skb.
- */
+ **/
 void igb_ptp_rx_rgtstamp(struct igb_q_vector *q_vector,
 			 struct sk_buff *skb)
 {
@@ -537,8 +520,7 @@ void igb_ptp_rx_rgtstamp(struct igb_q_vector *q_vector,
 	struct e1000_hw *hw = &adapter->hw;
 	u64 regval;
 
-	/*
-	 * If this bit is set, then the RX registers contain the time stamp. No
+	/* If this bit is set, then the RX registers contain the time stamp. No
 	 * other packet will be time stamped until we read these registers, so
 	 * read the registers to make them available again. Because only one
 	 * packet can be time stamped at a time, we know that the register
@@ -574,7 +556,6 @@ void igb_ptp_rx_rgtstamp(struct igb_q_vector *q_vector,
  * type has to be specified. Matching the kind of event packet is
  * not supported, with the exception of "all V2 events regardless of
  * level 2 or 4".
- *
  **/
 int igb_ptp_hwtstamp_ioctl(struct net_device *netdev,
 			   struct ifreq *ifr, int cmd)
@@ -655,10 +636,9 @@ int igb_ptp_hwtstamp_ioctl(struct net_device *netdev,
 		return 0;
 	}
 
-	/*
-	 * Per-packet timestamping only works if all packets are
+	/* Per-packet timestamping only works if all packets are
 	 * timestamped, so enable timestamping in all packets as
-	 * long as one rx filter was configured.
+	 * long as one Rx filter was configured.
 	 */
 	if ((hw->mac.type >= e1000_82580) && tsync_rx_ctl) {
 		tsync_rx_ctl = E1000_TSYNCRXCTL_ENABLED;
@@ -756,6 +736,7 @@ void igb_ptp_init(struct igb_adapter *adapter)
 		wr32(E1000_TIMINCA, INCPERIOD_82576 | INCVALUE_82576);
 		break;
 	case e1000_82580:
+	case e1000_i354:
 	case e1000_i350:
 		snprintf(adapter->ptp_caps.name, 16, "%pm", netdev->dev_addr);
 		adapter->ptp_caps.owner = THIS_MODULE;
@@ -844,6 +825,7 @@ void igb_ptp_stop(struct igb_adapter *adapter)
 	switch (adapter->hw.mac.type) {
 	case e1000_82576:
 	case e1000_82580:
+	case e1000_i354:
 	case e1000_i350:
 		cancel_delayed_work_sync(&adapter->ptp_overflow_work);
 		break;
@@ -888,6 +870,7 @@ void igb_ptp_reset(struct igb_adapter *adapter)
 		wr32(E1000_TIMINCA, INCPERIOD_82576 | INCVALUE_82576);
 		break;
 	case e1000_82580:
+	case e1000_i354:
 	case e1000_i350:
 	case e1000_i210:
 	case e1000_i211:
diff --git a/drivers/net/ethernet/intel/igbvf/netdev.c b/drivers/net/ethernet/intel/igbvf/netdev.c
index d60cd4393415..93eb7ee06d3e 100644
--- a/drivers/net/ethernet/intel/igbvf/netdev.c
+++ b/drivers/net/ethernet/intel/igbvf/netdev.c
@@ -116,7 +116,7 @@ static void igbvf_receive_skb(struct igbvf_adapter *adapter,
 		else
 			vid = le16_to_cpu(vlan) & E1000_RXD_SPC_VLAN_MASK;
 		if (test_bit(vid, adapter->active_vlans))
-			__vlan_hwaccel_put_tag(skb, vid);
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
 	}
 
 	napi_gro_receive(&adapter->rx_ring->napi, skb);
@@ -447,7 +447,6 @@ int igbvf_setup_tx_resources(struct igbvf_adapter *adapter,
 
 	tx_ring->desc = dma_alloc_coherent(&pdev->dev, tx_ring->size,
 					   &tx_ring->dma, GFP_KERNEL);
-
 	if (!tx_ring->desc)
 		goto err;
 
@@ -488,7 +487,6 @@ int igbvf_setup_rx_resources(struct igbvf_adapter *adapter,
 
 	rx_ring->desc = dma_alloc_coherent(&pdev->dev, rx_ring->size,
 					   &rx_ring->dma, GFP_KERNEL);
-
 	if (!rx_ring->desc)
 		goto err;
 
@@ -1232,7 +1230,8 @@ static void igbvf_set_rlpml(struct igbvf_adapter *adapter)
 	e1000_rlpml_set_vf(hw, max_frame_size);
 }
 
-static int igbvf_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
+static int igbvf_vlan_rx_add_vid(struct net_device *netdev,
+				 __be16 proto, u16 vid)
 {
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
@@ -1245,7 +1244,8 @@ static int igbvf_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
 	return 0;
 }
 
-static int igbvf_vlan_rx_kill_vid(struct net_device *netdev, u16 vid)
+static int igbvf_vlan_rx_kill_vid(struct net_device *netdev,
+				  __be16 proto, u16 vid)
 {
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
@@ -1264,7 +1264,7 @@ static void igbvf_restore_vlan(struct igbvf_adapter *adapter)
 	u16 vid;
 
 	for_each_set_bit(vid, adapter->active_vlans, VLAN_N_VID)
-		igbvf_vlan_rx_add_vid(adapter->netdev, vid);
+		igbvf_vlan_rx_add_vid(adapter->netdev, htons(ETH_P_8021Q), vid);
 }
 
 /**
@@ -2724,9 +2724,9 @@ static int igbvf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 			   NETIF_F_RXCSUM;
 
 	netdev->features = netdev->hw_features |
-	                   NETIF_F_HW_VLAN_TX |
-	                   NETIF_F_HW_VLAN_RX |
-	                   NETIF_F_HW_VLAN_FILTER;
+	                   NETIF_F_HW_VLAN_CTAG_TX |
+	                   NETIF_F_HW_VLAN_CTAG_RX |
+	                   NETIF_F_HW_VLAN_CTAG_FILTER;
 
 	if (pci_using_dac)
 		netdev->features |= NETIF_F_HIGHDMA;
diff --git a/drivers/net/ethernet/intel/ixgb/ixgb_main.c b/drivers/net/ethernet/intel/ixgb/ixgb_main.c
index b5f94abe3cff..fce3e92f9d11 100644
--- a/drivers/net/ethernet/intel/ixgb/ixgb_main.c
+++ b/drivers/net/ethernet/intel/ixgb/ixgb_main.c
@@ -101,8 +101,10 @@ static void ixgb_tx_timeout_task(struct work_struct *work);
 
 static void ixgb_vlan_strip_enable(struct ixgb_adapter *adapter);
 static void ixgb_vlan_strip_disable(struct ixgb_adapter *adapter);
-static int ixgb_vlan_rx_add_vid(struct net_device *netdev, u16 vid);
-static int ixgb_vlan_rx_kill_vid(struct net_device *netdev, u16 vid);
+static int ixgb_vlan_rx_add_vid(struct net_device *netdev,
+				__be16 proto, u16 vid);
+static int ixgb_vlan_rx_kill_vid(struct net_device *netdev,
+				 __be16 proto, u16 vid);
 static void ixgb_restore_vlan(struct ixgb_adapter *adapter);
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
@@ -332,8 +334,8 @@ ixgb_fix_features(struct net_device *netdev, netdev_features_t features)
 	 * Tx VLAN insertion does not work per HW design when Rx stripping is
 	 * disabled.
 	 */
-	if (!(features & NETIF_F_HW_VLAN_RX))
-		features &= ~NETIF_F_HW_VLAN_TX;
+	if (!(features & NETIF_F_HW_VLAN_CTAG_RX))
+		features &= ~NETIF_F_HW_VLAN_CTAG_TX;
 
 	return features;
 }
@@ -344,7 +346,7 @@ ixgb_set_features(struct net_device *netdev, netdev_features_t features)
 	struct ixgb_adapter *adapter = netdev_priv(netdev);
 	netdev_features_t changed = features ^ netdev->features;
 
-	if (!(changed & (NETIF_F_RXCSUM|NETIF_F_HW_VLAN_RX)))
+	if (!(changed & (NETIF_F_RXCSUM|NETIF_F_HW_VLAN_CTAG_RX)))
 		return 0;
 
 	adapter->rx_csum = !!(features & NETIF_F_RXCSUM);
@@ -479,10 +481,10 @@ ixgb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	netdev->hw_features = NETIF_F_SG |
 			   NETIF_F_TSO |
 			   NETIF_F_HW_CSUM |
-			   NETIF_F_HW_VLAN_TX |
-			   NETIF_F_HW_VLAN_RX;
+			   NETIF_F_HW_VLAN_CTAG_TX |
+			   NETIF_F_HW_VLAN_CTAG_RX;
 	netdev->features = netdev->hw_features |
-			   NETIF_F_HW_VLAN_FILTER;
+			   NETIF_F_HW_VLAN_CTAG_FILTER;
 	netdev->hw_features |= NETIF_F_RXCSUM;
 
 	if (pci_using_dac) {
@@ -717,14 +719,11 @@ ixgb_setup_tx_resources(struct ixgb_adapter *adapter)
 	txdr->size = ALIGN(txdr->size, 4096);
 
 	txdr->desc = dma_alloc_coherent(&pdev->dev, txdr->size, &txdr->dma,
-					GFP_KERNEL);
+					GFP_KERNEL | __GFP_ZERO);
 	if (!txdr->desc) {
 		vfree(txdr->buffer_info);
-		netif_err(adapter, probe, adapter->netdev,
-			  "Unable to allocate transmit descriptor memory\n");
 		return -ENOMEM;
 	}
-	memset(txdr->desc, 0, txdr->size);
 
 	txdr->next_to_use = 0;
 	txdr->next_to_clean = 0;
@@ -807,8 +806,6 @@ ixgb_setup_rx_resources(struct ixgb_adapter *adapter)
 
 	if (!rxdr->desc) {
 		vfree(rxdr->buffer_info);
-		netif_err(adapter, probe, adapter->netdev,
-			  "Unable to allocate receive descriptors\n");
 		return -ENOMEM;
 	}
 	memset(rxdr->desc, 0, rxdr->size);
@@ -1145,7 +1142,7 @@ ixgb_set_multi(struct net_device *netdev)
 	}
 
 alloc_failed:
-	if (netdev->features & NETIF_F_HW_VLAN_RX)
+	if (netdev->features & NETIF_F_HW_VLAN_CTAG_RX)
 		ixgb_vlan_strip_enable(adapter);
 	else
 		ixgb_vlan_strip_disable(adapter);
@@ -2085,8 +2082,8 @@ ixgb_clean_rx_irq(struct ixgb_adapter *adapter, int *work_done, int work_to_do)
 
 		skb->protocol = eth_type_trans(skb, netdev);
 		if (status & IXGB_RX_DESC_STATUS_VP)
-			__vlan_hwaccel_put_tag(skb,
-					       le16_to_cpu(rx_desc->special));
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
+				       le16_to_cpu(rx_desc->special));
 
 		netif_receive_skb(skb);
 
@@ -2214,7 +2211,7 @@ ixgb_vlan_strip_disable(struct ixgb_adapter *adapter)
 }
 
 static int
-ixgb_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
+ixgb_vlan_rx_add_vid(struct net_device *netdev, __be16 proto, u16 vid)
 {
 	struct ixgb_adapter *adapter = netdev_priv(netdev);
 	u32 vfta, index;
@@ -2231,7 +2228,7 @@ ixgb_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
 }
 
 static int
-ixgb_vlan_rx_kill_vid(struct net_device *netdev, u16 vid)
+ixgb_vlan_rx_kill_vid(struct net_device *netdev, __be16 proto, u16 vid)
 {
 	struct ixgb_adapter *adapter = netdev_priv(netdev);
 	u32 vfta, index;
@@ -2253,7 +2250,7 @@ ixgb_restore_vlan(struct ixgb_adapter *adapter)
 	u16 vid;
 
 	for_each_set_bit(vid, adapter->active_vlans, VLAN_N_VID)
-		ixgb_vlan_rx_add_vid(adapter->netdev, vid);
+		ixgb_vlan_rx_add_vid(adapter->netdev, htons(ETH_P_8021Q), vid);
 }
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe.h b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
index a8e10cff7a89..ca932387a80f 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
@@ -740,6 +740,11 @@ extern void ixgbe_dbg_adapter_init(struct ixgbe_adapter *adapter);
 extern void ixgbe_dbg_adapter_exit(struct ixgbe_adapter *adapter);
 extern void ixgbe_dbg_init(void);
 extern void ixgbe_dbg_exit(void);
+#else
+static inline void ixgbe_dbg_adapter_init(struct ixgbe_adapter *adapter) {}
+static inline void ixgbe_dbg_adapter_exit(struct ixgbe_adapter *adapter) {}
+static inline void ixgbe_dbg_init(void) {}
+static inline void ixgbe_dbg_exit(void) {}
 #endif /* CONFIG_DEBUG_FS */
 static inline struct netdev_queue *txring_txq(const struct ixgbe_ring *ring)
 {
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c
index d0113fc97b6f..4a5bfb6b3af0 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c
@@ -1305,6 +1305,7 @@ static struct ixgbe_mac_operations mac_ops_82598 = {
 	.release_swfw_sync      = &ixgbe_release_swfw_sync,
 	.get_thermal_sensor_data = NULL,
 	.init_thermal_sensor_thresh = NULL,
+	.mng_fw_enabled		= NULL,
 };
 
 static struct ixgbe_eeprom_operations eeprom_ops_82598 = {
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
index 203a00c24330..0b82d38bc97d 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
@@ -59,12 +59,34 @@ static s32 ixgbe_setup_copper_link_82599(struct ixgbe_hw *hw,
                                          bool autoneg_wait_to_complete);
 static s32 ixgbe_verify_fw_version_82599(struct ixgbe_hw *hw);
 
+static bool ixgbe_mng_enabled(struct ixgbe_hw *hw)
+{
+	u32 fwsm, manc, factps;
+
+	fwsm = IXGBE_READ_REG(hw, IXGBE_FWSM);
+	if ((fwsm & IXGBE_FWSM_MODE_MASK) != IXGBE_FWSM_FW_MODE_PT)
+		return false;
+
+	manc = IXGBE_READ_REG(hw, IXGBE_MANC);
+	if (!(manc & IXGBE_MANC_RCV_TCO_EN))
+		return false;
+
+	factps = IXGBE_READ_REG(hw, IXGBE_FACTPS);
+	if (factps & IXGBE_FACTPS_MNGCG)
+		return false;
+
+	return true;
+}
+
 static void ixgbe_init_mac_link_ops_82599(struct ixgbe_hw *hw)
 {
 	struct ixgbe_mac_info *mac = &hw->mac;
 
-	/* enable the laser control functions for SFP+ fiber */
-	if (mac->ops.get_media_type(hw) == ixgbe_media_type_fiber) {
+	/* enable the laser control functions for SFP+ fiber
+	 * and MNG not enabled
+	 */
+	if ((mac->ops.get_media_type(hw) == ixgbe_media_type_fiber) &&
+	    !hw->mng_fw_enabled) {
 		mac->ops.disable_tx_laser =
 		                       &ixgbe_disable_tx_laser_multispeed_fiber;
 		mac->ops.enable_tx_laser =
@@ -145,9 +167,9 @@ static s32 ixgbe_setup_sfp_modules_82599(struct ixgbe_hw *hw)
 		}
 
 		/* Restart DSP and set SFI mode */
-		IXGBE_WRITE_REG(hw, IXGBE_AUTOC, (IXGBE_READ_REG(hw,
-				IXGBE_AUTOC) | IXGBE_AUTOC_LMS_10G_SERIAL));
-
+		IXGBE_WRITE_REG(hw, IXGBE_AUTOC, ((hw->mac.orig_autoc) |
+				IXGBE_AUTOC_LMS_10G_SERIAL));
+		hw->mac.cached_autoc = IXGBE_READ_REG(hw, IXGBE_AUTOC);
 		ret_val = ixgbe_reset_pipeline_82599(hw);
 
 		if (got_lock) {
@@ -244,6 +266,8 @@ static s32 ixgbe_get_link_capabilities_82599(struct ixgbe_hw *hw,
 	/* Determine 1G link capabilities off of SFP+ type */
 	if (hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core0 ||
 	    hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core1 ||
+	    hw->phy.sfp_type == ixgbe_sfp_type_1g_lx_core0 ||
+	    hw->phy.sfp_type == ixgbe_sfp_type_1g_lx_core1 ||
 	    hw->phy.sfp_type == ixgbe_sfp_type_1g_sx_core0 ||
 	    hw->phy.sfp_type == ixgbe_sfp_type_1g_sx_core1) {
 		*speed = IXGBE_LINK_SPEED_1GB_FULL;
@@ -563,7 +587,8 @@ static s32 ixgbe_setup_mac_link_multispeed_fiber(struct ixgbe_hw *hw,
 			return status;
 
 		/* Flap the tx laser if it has not already been done */
-		hw->mac.ops.flap_tx_laser(hw);
+		if (hw->mac.ops.flap_tx_laser)
+			hw->mac.ops.flap_tx_laser(hw);
 
 		/*
 		 * Wait for the controller to acquire link.  Per IEEE 802.3ap,
@@ -615,7 +640,8 @@ static s32 ixgbe_setup_mac_link_multispeed_fiber(struct ixgbe_hw *hw,
 			return status;
 
 		/* Flap the tx laser if it has not already been done */
-		hw->mac.ops.flap_tx_laser(hw);
+		if (hw->mac.ops.flap_tx_laser)
+			hw->mac.ops.flap_tx_laser(hw);
 
 		/* Wait for the link partner to also set speed */
 		msleep(100);
@@ -777,12 +803,9 @@ static s32 ixgbe_setup_mac_link_82599(struct ixgbe_hw *hw,
 				      bool autoneg_wait_to_complete)
 {
 	s32 status = 0;
-	u32 autoc = IXGBE_READ_REG(hw, IXGBE_AUTOC);
+	u32 autoc, pma_pmd_1g, link_mode, start_autoc;
 	u32 autoc2 = IXGBE_READ_REG(hw, IXGBE_AUTOC2);
-	u32 start_autoc = autoc;
 	u32 orig_autoc = 0;
-	u32 link_mode = autoc & IXGBE_AUTOC_LMS_MASK;
-	u32 pma_pmd_1g = autoc & IXGBE_AUTOC_1G_PMA_PMD_MASK;
 	u32 pma_pmd_10g_serial = autoc2 & IXGBE_AUTOC2_10G_SERIAL_PMA_PMD_MASK;
 	u32 links_reg;
 	u32 i;
@@ -805,9 +828,14 @@ static s32 ixgbe_setup_mac_link_82599(struct ixgbe_hw *hw,
 
 	/* Use stored value (EEPROM defaults) of AUTOC to find KR/KX4 support*/
 	if (hw->mac.orig_link_settings_stored)
-		orig_autoc = hw->mac.orig_autoc;
+		autoc = hw->mac.orig_autoc;
 	else
-		orig_autoc = autoc;
+		autoc = IXGBE_READ_REG(hw, IXGBE_AUTOC);
+
+	orig_autoc = autoc;
+	start_autoc = hw->mac.cached_autoc;
+	link_mode = autoc & IXGBE_AUTOC_LMS_MASK;
+	pma_pmd_1g = autoc & IXGBE_AUTOC_1G_PMA_PMD_MASK;
 
 	if (link_mode == IXGBE_AUTOC_LMS_KX4_KX_KR ||
 	    link_mode == IXGBE_AUTOC_LMS_KX4_KX_KR_1G_AN ||
@@ -861,6 +889,7 @@ static s32 ixgbe_setup_mac_link_82599(struct ixgbe_hw *hw,
 
 		/* Restart link */
 		IXGBE_WRITE_REG(hw, IXGBE_AUTOC, autoc);
+		hw->mac.cached_autoc = autoc;
 		ixgbe_reset_pipeline_82599(hw);
 
 		if (got_lock)
@@ -932,7 +961,8 @@ static s32 ixgbe_reset_hw_82599(struct ixgbe_hw *hw)
 {
 	ixgbe_link_speed link_speed;
 	s32 status;
-	u32 ctrl, i, autoc, autoc2;
+	u32 ctrl, i, autoc2;
+	u32 curr_lms;
 	bool link_up = false;
 
 	/* Call adapter stop to disable tx/rx and clear interrupts */
@@ -964,6 +994,13 @@ static s32 ixgbe_reset_hw_82599(struct ixgbe_hw *hw)
 	if (hw->phy.reset_disable == false && hw->phy.ops.reset != NULL)
 		hw->phy.ops.reset(hw);
 
+	/* remember AUTOC from before we reset */
+	if (hw->mac.cached_autoc)
+		curr_lms = hw->mac.cached_autoc & IXGBE_AUTOC_LMS_MASK;
+	else
+		curr_lms = IXGBE_READ_REG(hw, IXGBE_AUTOC) &
+			   IXGBE_AUTOC_LMS_MASK;
+
 mac_reset_top:
 	/*
 	 * Issue global reset to the MAC. Needs to be SW reset if link is up.
@@ -1012,14 +1049,35 @@ mac_reset_top:
 	 * stored off yet.  Otherwise restore the stored original
 	 * values since the reset operation sets back to defaults.
 	 */
-	autoc = IXGBE_READ_REG(hw, IXGBE_AUTOC);
+	hw->mac.cached_autoc = IXGBE_READ_REG(hw, IXGBE_AUTOC);
 	autoc2 = IXGBE_READ_REG(hw, IXGBE_AUTOC2);
+
+	/* Enable link if disabled in NVM */
+	if (autoc2 & IXGBE_AUTOC2_LINK_DISABLE_MASK) {
+		autoc2 &= ~IXGBE_AUTOC2_LINK_DISABLE_MASK;
+		IXGBE_WRITE_REG(hw, IXGBE_AUTOC2, autoc2);
+		IXGBE_WRITE_FLUSH(hw);
+	}
+
 	if (hw->mac.orig_link_settings_stored == false) {
-		hw->mac.orig_autoc = autoc;
+		hw->mac.orig_autoc = hw->mac.cached_autoc;
 		hw->mac.orig_autoc2 = autoc2;
 		hw->mac.orig_link_settings_stored = true;
 	} else {
-		if (autoc != hw->mac.orig_autoc) {
+
+		/* If MNG FW is running on a multi-speed device that
+		 * doesn't autoneg with out driver support we need to
+		 * leave LMS in the state it was before we MAC reset.
+		 * Likewise if we support WoL we don't want change the
+		 * LMS state either.
+		 */
+		if ((hw->phy.multispeed_fiber && hw->mng_fw_enabled) ||
+		    hw->wol_enabled)
+			hw->mac.orig_autoc =
+				(hw->mac.orig_autoc & ~IXGBE_AUTOC_LMS_MASK) |
+				curr_lms;
+
+		if (hw->mac.cached_autoc != hw->mac.orig_autoc) {
 			/* Need SW/FW semaphore around AUTOC writes if LESM is
 			 * on, likewise reset_pipeline requires us to hold
 			 * this lock as it also writes to AUTOC.
@@ -1035,6 +1093,7 @@ mac_reset_top:
 			}
 
 			IXGBE_WRITE_REG(hw, IXGBE_AUTOC, hw->mac.orig_autoc);
+			hw->mac.cached_autoc = hw->mac.orig_autoc;
 			ixgbe_reset_pipeline_82599(hw);
 
 			if (got_lock)
@@ -2135,10 +2194,19 @@ static s32 ixgbe_read_eeprom_82599(struct ixgbe_hw *hw,
  **/
 s32 ixgbe_reset_pipeline_82599(struct ixgbe_hw *hw)
 {
-	s32 i, autoc_reg, ret_val;
-	s32 anlp1_reg = 0;
+	s32 ret_val;
+	u32 anlp1_reg = 0;
+	u32 i, autoc_reg, autoc2_reg;
+
+	/* Enable link if disabled in NVM */
+	autoc2_reg = IXGBE_READ_REG(hw, IXGBE_AUTOC2);
+	if (autoc2_reg & IXGBE_AUTOC2_LINK_DISABLE_MASK) {
+		autoc2_reg &= ~IXGBE_AUTOC2_LINK_DISABLE_MASK;
+		IXGBE_WRITE_REG(hw, IXGBE_AUTOC2, autoc2_reg);
+		IXGBE_WRITE_FLUSH(hw);
+	}
 
-	autoc_reg = IXGBE_READ_REG(hw, IXGBE_AUTOC);
+	autoc_reg = hw->mac.cached_autoc;
 	autoc_reg |= IXGBE_AUTOC_AN_RESTART;
 
 	/* Write AUTOC register with toggled LMS[2] bit and Restart_AN */
@@ -2216,7 +2284,7 @@ static struct ixgbe_mac_operations mac_ops_82599 = {
 	.release_swfw_sync      = &ixgbe_release_swfw_sync,
 	.get_thermal_sensor_data = &ixgbe_get_thermal_sensor_data_generic,
 	.init_thermal_sensor_thresh = &ixgbe_init_thermal_sensor_thresh_generic,
-
+	.mng_fw_enabled		= &ixgbe_mng_enabled,
 };
 
 static struct ixgbe_eeprom_operations eeprom_ops_82599 = {
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
index 99e472ebaa75..9bcdeb89af5a 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
@@ -592,6 +592,36 @@ s32 ixgbe_get_mac_addr_generic(struct ixgbe_hw *hw, u8 *mac_addr)
 	return 0;
 }
 
+enum ixgbe_bus_width ixgbe_convert_bus_width(u16 link_status)
+{
+	switch (link_status & IXGBE_PCI_LINK_WIDTH) {
+	case IXGBE_PCI_LINK_WIDTH_1:
+		return ixgbe_bus_width_pcie_x1;
+	case IXGBE_PCI_LINK_WIDTH_2:
+		return ixgbe_bus_width_pcie_x2;
+	case IXGBE_PCI_LINK_WIDTH_4:
+		return ixgbe_bus_width_pcie_x4;
+	case IXGBE_PCI_LINK_WIDTH_8:
+		return ixgbe_bus_width_pcie_x8;
+	default:
+		return ixgbe_bus_width_unknown;
+	}
+}
+
+enum ixgbe_bus_speed ixgbe_convert_bus_speed(u16 link_status)
+{
+	switch (link_status & IXGBE_PCI_LINK_SPEED) {
+	case IXGBE_PCI_LINK_SPEED_2500:
+		return ixgbe_bus_speed_2500;
+	case IXGBE_PCI_LINK_SPEED_5000:
+		return ixgbe_bus_speed_5000;
+	case IXGBE_PCI_LINK_SPEED_8000:
+		return ixgbe_bus_speed_8000;
+	default:
+		return ixgbe_bus_speed_unknown;
+	}
+}
+
 /**
  *  ixgbe_get_bus_info_generic - Generic set PCI bus info
  *  @hw: pointer to hardware structure
@@ -610,35 +640,8 @@ s32 ixgbe_get_bus_info_generic(struct ixgbe_hw *hw)
 	pci_read_config_word(adapter->pdev, IXGBE_PCI_LINK_STATUS,
 	                     &link_status);
 
-	switch (link_status & IXGBE_PCI_LINK_WIDTH) {
-	case IXGBE_PCI_LINK_WIDTH_1:
-		hw->bus.width = ixgbe_bus_width_pcie_x1;
-		break;
-	case IXGBE_PCI_LINK_WIDTH_2:
-		hw->bus.width = ixgbe_bus_width_pcie_x2;
-		break;
-	case IXGBE_PCI_LINK_WIDTH_4:
-		hw->bus.width = ixgbe_bus_width_pcie_x4;
-		break;
-	case IXGBE_PCI_LINK_WIDTH_8:
-		hw->bus.width = ixgbe_bus_width_pcie_x8;
-		break;
-	default:
-		hw->bus.width = ixgbe_bus_width_unknown;
-		break;
-	}
-
-	switch (link_status & IXGBE_PCI_LINK_SPEED) {
-	case IXGBE_PCI_LINK_SPEED_2500:
-		hw->bus.speed = ixgbe_bus_speed_2500;
-		break;
-	case IXGBE_PCI_LINK_SPEED_5000:
-		hw->bus.speed = ixgbe_bus_speed_5000;
-		break;
-	default:
-		hw->bus.speed = ixgbe_bus_speed_unknown;
-		break;
-	}
+	hw->bus.width = ixgbe_convert_bus_width(link_status);
+	hw->bus.speed = ixgbe_convert_bus_speed(link_status);
 
 	mac->ops.set_lan_id(hw);
 
@@ -1125,7 +1128,7 @@ s32 ixgbe_read_eerd_buffer_generic(struct ixgbe_hw *hw, u16 offset,
 	}
 
 	for (i = 0; i < words; i++) {
-		eerd = ((offset + i) << IXGBE_EEPROM_RW_ADDR_SHIFT) +
+		eerd = ((offset + i) << IXGBE_EEPROM_RW_ADDR_SHIFT) |
 		       IXGBE_EEPROM_RW_REG_START;
 
 		IXGBE_WRITE_REG(hw, IXGBE_EERD, eerd);
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
index bc3948ead6e0..22eee38868f1 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
@@ -40,6 +40,8 @@ s32 ixgbe_clear_hw_cntrs_generic(struct ixgbe_hw *hw);
 s32 ixgbe_read_pba_string_generic(struct ixgbe_hw *hw, u8 *pba_num,
                                   u32 pba_num_size);
 s32 ixgbe_get_mac_addr_generic(struct ixgbe_hw *hw, u8 *mac_addr);
+enum ixgbe_bus_width ixgbe_convert_bus_width(u16 link_status);
+enum ixgbe_bus_speed ixgbe_convert_bus_speed(u16 link_status);
 s32 ixgbe_get_bus_info_generic(struct ixgbe_hw *hw);
 void ixgbe_set_lan_id_multi_port_pcie(struct ixgbe_hw *hw);
 s32 ixgbe_stop_adapter_generic(struct ixgbe_hw *hw);
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
index c3f1afd86906..d3754722adb4 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
@@ -231,6 +231,10 @@ static int ixgbe_get_settings(struct net_device *netdev,
 		case ixgbe_sfp_type_lr:
 		case ixgbe_sfp_type_srlr_core0:
 		case ixgbe_sfp_type_srlr_core1:
+		case ixgbe_sfp_type_1g_sx_core0:
+		case ixgbe_sfp_type_1g_sx_core1:
+		case ixgbe_sfp_type_1g_lx_core0:
+		case ixgbe_sfp_type_1g_lx_core1:
 			ecmd->supported |= SUPPORTED_FIBRE;
 			ecmd->advertising |= ADVERTISED_FIBRE;
 			ecmd->port = PORT_FIBRE;
@@ -246,12 +250,6 @@ static int ixgbe_get_settings(struct net_device *netdev,
 			ecmd->advertising |= ADVERTISED_TP;
 			ecmd->port = PORT_TP;
 			break;
-		case ixgbe_sfp_type_1g_sx_core0:
-		case ixgbe_sfp_type_1g_sx_core1:
-			ecmd->supported |= SUPPORTED_FIBRE;
-			ecmd->advertising |= ADVERTISED_FIBRE;
-			ecmd->port = PORT_FIBRE;
-			break;
 		case ixgbe_sfp_type_unknown:
 		default:
 			ecmd->supported |= SUPPORTED_FIBRE;
@@ -442,7 +440,8 @@ static void ixgbe_get_regs(struct net_device *netdev,
 
 	memset(p, 0, IXGBE_REGS_LEN * sizeof(u32));
 
-	regs->version = (1 << 24) | hw->revision_id << 16 | hw->device_id;
+	regs->version = hw->mac.type << 24 | hw->revision_id << 16 |
+			hw->device_id;
 
 	/* General Registers */
 	regs_buff[0] = IXGBE_READ_REG(hw, IXGBE_CTRL);
@@ -1611,16 +1610,9 @@ static int ixgbe_setup_loopback_test(struct ixgbe_adapter *adapter)
 	struct ixgbe_hw *hw = &adapter->hw;
 	u32 reg_data;
 
-	/* X540 needs to set the MACC.FLU bit to force link up */
-	if (adapter->hw.mac.type == ixgbe_mac_X540) {
-		reg_data = IXGBE_READ_REG(hw, IXGBE_MACC);
-		reg_data |= IXGBE_MACC_FLU;
-		IXGBE_WRITE_REG(hw, IXGBE_MACC, reg_data);
-	}
 
-	/* right now we only support MAC loopback in the driver */
-	reg_data = IXGBE_READ_REG(hw, IXGBE_HLREG0);
 	/* Setup MAC loopback */
+	reg_data = IXGBE_READ_REG(hw, IXGBE_HLREG0);
 	reg_data |= IXGBE_HLREG0_LPBK;
 	IXGBE_WRITE_REG(hw, IXGBE_HLREG0, reg_data);
 
@@ -1628,10 +1620,19 @@ static int ixgbe_setup_loopback_test(struct ixgbe_adapter *adapter)
 	reg_data |= IXGBE_FCTRL_BAM | IXGBE_FCTRL_SBP | IXGBE_FCTRL_MPE;
 	IXGBE_WRITE_REG(hw, IXGBE_FCTRL, reg_data);
 
-	reg_data = IXGBE_READ_REG(hw, IXGBE_AUTOC);
-	reg_data &= ~IXGBE_AUTOC_LMS_MASK;
-	reg_data |= IXGBE_AUTOC_LMS_10G_LINK_NO_AN | IXGBE_AUTOC_FLU;
-	IXGBE_WRITE_REG(hw, IXGBE_AUTOC, reg_data);
+	/* X540 needs to set the MACC.FLU bit to force link up */
+	if (adapter->hw.mac.type == ixgbe_mac_X540) {
+		reg_data = IXGBE_READ_REG(hw, IXGBE_MACC);
+		reg_data |= IXGBE_MACC_FLU;
+		IXGBE_WRITE_REG(hw, IXGBE_MACC, reg_data);
+	} else {
+		if (hw->mac.orig_autoc) {
+			reg_data = hw->mac.orig_autoc | IXGBE_AUTOC_FLU;
+			IXGBE_WRITE_REG(hw, IXGBE_AUTOC, reg_data);
+		} else {
+			return 10;
+		}
+	}
 	IXGBE_WRITE_FLUSH(hw);
 	usleep_range(10000, 20000);
 
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
index 79f4a26ea6cc..d30fbdd81fca 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
@@ -63,7 +63,7 @@ char ixgbe_default_device_descr[] =
 static char ixgbe_default_device_descr[] =
 			      "Intel(R) 10 Gigabit Network Connection";
 #endif
-#define DRV_VERSION "3.11.33-k"
+#define DRV_VERSION "3.13.10-k"
 const char ixgbe_driver_version[] = DRV_VERSION;
 static const char ixgbe_copyright[] =
 				"Copyright (c) 1999-2013 Intel Corporation.";
@@ -149,6 +149,52 @@ MODULE_DESCRIPTION("Intel(R) 10 Gigabit PCI Express Network Driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_VERSION);
 
+static int ixgbe_read_pci_cfg_word_parent(struct ixgbe_adapter *adapter,
+					  u32 reg, u16 *value)
+{
+	int pos = 0;
+	struct pci_dev *parent_dev;
+	struct pci_bus *parent_bus;
+
+	parent_bus = adapter->pdev->bus->parent;
+	if (!parent_bus)
+		return -1;
+
+	parent_dev = parent_bus->self;
+	if (!parent_dev)
+		return -1;
+
+	pos = pci_find_capability(parent_dev, PCI_CAP_ID_EXP);
+	if (!pos)
+		return -1;
+
+	pci_read_config_word(parent_dev, pos + reg, value);
+	return 0;
+}
+
+static s32 ixgbe_get_parent_bus_info(struct ixgbe_adapter *adapter)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	u16 link_status = 0;
+	int err;
+
+	hw->bus.type = ixgbe_bus_type_pci_express;
+
+	/* Get the negotiated link width and speed from PCI config space of the
+	 * parent, as this device is behind a switch
+	 */
+	err = ixgbe_read_pci_cfg_word_parent(adapter, 18, &link_status);
+
+	/* assume caller will handle error case */
+	if (err)
+		return err;
+
+	hw->bus.width = ixgbe_convert_bus_width(link_status);
+	hw->bus.speed = ixgbe_convert_bus_speed(link_status);
+
+	return 0;
+}
+
 static void ixgbe_service_event_schedule(struct ixgbe_adapter *adapter)
 {
 	if (!test_bit(__IXGBE_DOWN, &adapter->state) &&
@@ -1337,7 +1383,7 @@ static unsigned int ixgbe_get_headlen(unsigned char *data,
 			return hdr.network - data;
 
 		/* record next protocol if header is present */
-		if (!hdr.ipv4->frag_off)
+		if (!(hdr.ipv4->frag_off & htons(IP_OFFSET)))
 			nexthdr = hdr.ipv4->protocol;
 	} else if (protocol == __constant_htons(ETH_P_IPV6)) {
 		if ((hdr.network - data) > (max_len - sizeof(struct ipv6hdr)))
@@ -1442,10 +1488,10 @@ static void ixgbe_process_skb_fields(struct ixgbe_ring *rx_ring,
 
 	ixgbe_ptp_rx_hwtstamp(rx_ring, rx_desc, skb);
 
-	if ((dev->features & NETIF_F_HW_VLAN_RX) &&
+	if ((dev->features & NETIF_F_HW_VLAN_CTAG_RX) &&
 	    ixgbe_test_staterr(rx_desc, IXGBE_RXD_STAT_VP)) {
 		u16 vid = le16_to_cpu(rx_desc->wb.upper.vlan);
-		__vlan_hwaccel_put_tag(skb, vid);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
 	}
 
 	skb_record_rx_queue(skb, rx_ring->queue_index);
@@ -2049,6 +2095,9 @@ static void ixgbe_update_itr(struct ixgbe_q_vector *q_vector,
 	 */
 	/* what was last interrupt timeslice? */
 	timepassed_us = q_vector->itr >> 2;
+	if (timepassed_us == 0)
+		return;
+
 	bytes_perint = bytes / timepassed_us; /* bytes/usec */
 
 	switch (itr_setting) {
@@ -2405,6 +2454,16 @@ static irqreturn_t ixgbe_msix_other(int irq, void *data)
 	 * with the write to EICR.
 	 */
 	eicr = IXGBE_READ_REG(hw, IXGBE_EICS);
+
+	/* The lower 16bits of the EICR register are for the queue interrupts
+	 * which should be masked here in order to not accidently clear them if
+	 * the bits are high when ixgbe_msix_other is called. There is a race
+	 * condition otherwise which results in possible performance loss
+	 * especially if the ixgbe_msix_other interrupt is triggering
+	 * consistently (as it would when PPS is turned on for the X540 device)
+	 */
+	eicr &= 0xFFFF0000;
+
 	IXGBE_WRITE_REG(hw, IXGBE_EICR, eicr);
 
 	if (eicr & IXGBE_EICR_LSC)
@@ -3421,7 +3480,8 @@ static void ixgbe_configure_rx(struct ixgbe_adapter *adapter)
 	hw->mac.ops.enable_rx_dma(hw, rxctrl);
 }
 
-static int ixgbe_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
+static int ixgbe_vlan_rx_add_vid(struct net_device *netdev,
+				 __be16 proto, u16 vid)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
@@ -3433,7 +3493,8 @@ static int ixgbe_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
 	return 0;
 }
 
-static int ixgbe_vlan_rx_kill_vid(struct net_device *netdev, u16 vid)
+static int ixgbe_vlan_rx_kill_vid(struct net_device *netdev,
+				  __be16 proto, u16 vid)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
@@ -3538,10 +3599,10 @@ static void ixgbe_restore_vlan(struct ixgbe_adapter *adapter)
 {
 	u16 vid;
 
-	ixgbe_vlan_rx_add_vid(adapter->netdev, 0);
+	ixgbe_vlan_rx_add_vid(adapter->netdev, htons(ETH_P_8021Q), 0);
 
 	for_each_set_bit(vid, adapter->active_vlans, VLAN_N_VID)
-		ixgbe_vlan_rx_add_vid(adapter->netdev, vid);
+		ixgbe_vlan_rx_add_vid(adapter->netdev, htons(ETH_P_8021Q), vid);
 }
 
 /**
@@ -3676,7 +3737,7 @@ void ixgbe_set_rx_mode(struct net_device *netdev)
 
 	IXGBE_WRITE_REG(hw, IXGBE_FCTRL, fctrl);
 
-	if (netdev->features & NETIF_F_HW_VLAN_RX)
+	if (netdev->features & NETIF_F_HW_VLAN_CTAG_RX)
 		ixgbe_vlan_strip_enable(adapter);
 	else
 		ixgbe_vlan_strip_disable(adapter);
@@ -5077,14 +5138,14 @@ static int __ixgbe_shutdown(struct pci_dev *pdev, bool *enable_wake)
 
 	netif_device_detach(netdev);
 
+	rtnl_lock();
 	if (netif_running(netdev)) {
-		rtnl_lock();
 		ixgbe_down(adapter);
 		ixgbe_free_irq(adapter);
 		ixgbe_free_all_tx_resources(adapter);
 		ixgbe_free_all_rx_resources(adapter);
-		rtnl_unlock();
 	}
+	rtnl_unlock();
 
 	ixgbe_clear_interrupt_scheme(adapter);
 
@@ -6425,9 +6486,7 @@ netdev_tx_t ixgbe_xmit_frame_ring(struct sk_buff *skb,
 	struct ixgbe_tx_buffer *first;
 	int tso;
 	u32 tx_flags = 0;
-#if PAGE_SIZE > IXGBE_MAX_DATA_PER_TXD
 	unsigned short f;
-#endif
 	u16 count = TXD_USE_COUNT(skb_headlen(skb));
 	__be16 protocol = skb->protocol;
 	u8 hdr_len = 0;
@@ -6439,12 +6498,9 @@ netdev_tx_t ixgbe_xmit_frame_ring(struct sk_buff *skb,
 	 *       + 1 desc for context descriptor,
 	 * otherwise try next time
 	 */
-#if PAGE_SIZE > IXGBE_MAX_DATA_PER_TXD
 	for (f = 0; f < skb_shinfo(skb)->nr_frags; f++)
 		count += TXD_USE_COUNT(skb_shinfo(skb)->frags[f].size);
-#else
-	count += skb_shinfo(skb)->nr_frags;
-#endif
+
 	if (ixgbe_maybe_stop_tx(tx_ring, count + 3)) {
 		tx_ring->tx_stats.tx_busy++;
 		return NETDEV_TX_BUSY;
@@ -6983,7 +7039,7 @@ static int ixgbe_set_features(struct net_device *netdev,
 		break;
 	}
 
-	if (features & NETIF_F_HW_VLAN_RX)
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
 		ixgbe_vlan_strip_enable(adapter);
 	else
 		ixgbe_vlan_strip_disable(adapter);
@@ -7007,7 +7063,7 @@ static int ixgbe_ndo_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 	int err;
 
 	if (!(adapter->flags & IXGBE_FLAG_SRIOV_ENABLED))
-		return -EOPNOTSUPP;
+		return ndo_dflt_fdb_add(ndm, tb, dev, addr, flags);
 
 	/* Hardware does not support aging addresses so if a
 	 * ndm_state is given only allow permanent addresses
@@ -7038,44 +7094,6 @@ static int ixgbe_ndo_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 	return err;
 }
 
-static int ixgbe_ndo_fdb_del(struct ndmsg *ndm, struct nlattr *tb[],
-			     struct net_device *dev,
-			     const unsigned char *addr)
-{
-	struct ixgbe_adapter *adapter = netdev_priv(dev);
-	int err = -EOPNOTSUPP;
-
-	if (ndm->ndm_state & NUD_PERMANENT) {
-		pr_info("%s: FDB only supports static addresses\n",
-			ixgbe_driver_name);
-		return -EINVAL;
-	}
-
-	if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) {
-		if (is_unicast_ether_addr(addr))
-			err = dev_uc_del(dev, addr);
-		else if (is_multicast_ether_addr(addr))
-			err = dev_mc_del(dev, addr);
-		else
-			err = -EINVAL;
-	}
-
-	return err;
-}
-
-static int ixgbe_ndo_fdb_dump(struct sk_buff *skb,
-			      struct netlink_callback *cb,
-			      struct net_device *dev,
-			      int idx)
-{
-	struct ixgbe_adapter *adapter = netdev_priv(dev);
-
-	if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED)
-		idx = ndo_dflt_fdb_dump(skb, cb, dev, idx);
-
-	return idx;
-}
-
 static int ixgbe_ndo_bridge_setlink(struct net_device *dev,
 				    struct nlmsghdr *nlh)
 {
@@ -7171,8 +7189,6 @@ static const struct net_device_ops ixgbe_netdev_ops = {
 	.ndo_set_features = ixgbe_set_features,
 	.ndo_fix_features = ixgbe_fix_features,
 	.ndo_fdb_add		= ixgbe_ndo_fdb_add,
-	.ndo_fdb_del		= ixgbe_ndo_fdb_del,
-	.ndo_fdb_dump		= ixgbe_ndo_fdb_dump,
 	.ndo_bridge_setlink	= ixgbe_ndo_bridge_setlink,
 	.ndo_bridge_getlink	= ixgbe_ndo_bridge_getlink,
 };
@@ -7202,9 +7218,19 @@ int ixgbe_wol_supported(struct ixgbe_adapter *adapter, u16 device_id,
 			/* only support first port */
 			if (hw->bus.func != 0)
 				break;
+		case IXGBE_SUBDEV_ID_82599_SP_560FLR:
 		case IXGBE_SUBDEV_ID_82599_SFP:
 		case IXGBE_SUBDEV_ID_82599_RNDC:
 		case IXGBE_SUBDEV_ID_82599_ECNA_DP:
+		case IXGBE_SUBDEV_ID_82599_LOM_SFP:
+			is_wol_supported = 1;
+			break;
+		}
+		break;
+	case IXGBE_DEV_ID_82599EN_SFP:
+		/* Only this subdevice supports WOL */
+		switch (subdevice_id) {
+		case IXGBE_SUBDEV_ID_82599EN_SFP_OCP1:
 			is_wol_supported = 1;
 			break;
 		}
@@ -7369,6 +7395,10 @@ static int ixgbe_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (err)
 		goto err_sw_init;
 
+	/* Cache if MNG FW is up so we don't have to read the REG later */
+	if (hw->mac.ops.mng_fw_enabled)
+		hw->mng_fw_enabled = hw->mac.ops.mng_fw_enabled(hw);
+
 	/* Make it possible the adapter to be woken up via WOL */
 	switch (adapter->hw.mac.type) {
 	case ixgbe_mac_82599EB:
@@ -7425,9 +7455,9 @@ skip_sriov:
 	netdev->features = NETIF_F_SG |
 			   NETIF_F_IP_CSUM |
 			   NETIF_F_IPV6_CSUM |
-			   NETIF_F_HW_VLAN_TX |
-			   NETIF_F_HW_VLAN_RX |
-			   NETIF_F_HW_VLAN_FILTER |
+			   NETIF_F_HW_VLAN_CTAG_TX |
+			   NETIF_F_HW_VLAN_CTAG_RX |
+			   NETIF_F_HW_VLAN_CTAG_FILTER |
 			   NETIF_F_TSO |
 			   NETIF_F_TSO6 |
 			   NETIF_F_RXHASH |
@@ -7521,7 +7551,9 @@ skip_sriov:
 	/* WOL not supported for all devices */
 	adapter->wol = 0;
 	hw->eeprom.ops.read(hw, 0x2c, &adapter->eeprom_cap);
-	if (ixgbe_wol_supported(adapter, pdev->device, pdev->subsystem_device))
+	hw->wol_enabled = ixgbe_wol_supported(adapter, pdev->device,
+						pdev->subsystem_device);
+	if (hw->wol_enabled)
 		adapter->wol = IXGBE_WUFC_MAG;
 
 	device_set_wakeup_enable(&adapter->pdev->dev, adapter->wol);
@@ -7532,10 +7564,13 @@ skip_sriov:
 
 	/* pick up the PCI bus settings for reporting later */
 	hw->mac.ops.get_bus_info(hw);
+	if (hw->device_id == IXGBE_DEV_ID_82599_SFP_SF_QP)
+		ixgbe_get_parent_bus_info(adapter);
 
 	/* print bus type/speed/width info */
 	e_dev_info("(PCI Express:%s:%s) %pM\n",
-		   (hw->bus.speed == ixgbe_bus_speed_5000 ? "5.0GT/s" :
+		   (hw->bus.speed == ixgbe_bus_speed_8000 ? "8.0GT/s" :
+		    hw->bus.speed == ixgbe_bus_speed_5000 ? "5.0GT/s" :
 		    hw->bus.speed == ixgbe_bus_speed_2500 ? "2.5GT/s" :
 		    "Unknown"),
 		   (hw->bus.width == ixgbe_bus_width_pcie_x8 ? "Width x8" :
@@ -7615,9 +7650,13 @@ skip_sriov:
 		e_err(probe, "failed to allocate sysfs resources\n");
 #endif /* CONFIG_IXGBE_HWMON */
 
-#ifdef CONFIG_DEBUG_FS
 	ixgbe_dbg_adapter_init(adapter);
-#endif /* CONFIG_DEBUG_FS */
+
+	/* Need link setup for MNG FW, else wait for IXGBE_UP */
+	if (hw->mng_fw_enabled && hw->mac.ops.setup_link)
+		hw->mac.ops.setup_link(hw,
+			IXGBE_LINK_SPEED_10GB_FULL | IXGBE_LINK_SPEED_1GB_FULL,
+			true);
 
 	return 0;
 
@@ -7653,9 +7692,7 @@ static void ixgbe_remove(struct pci_dev *pdev)
 	struct ixgbe_adapter *adapter = pci_get_drvdata(pdev);
 	struct net_device *netdev = adapter->netdev;
 
-#ifdef CONFIG_DEBUG_FS
 	ixgbe_dbg_adapter_exit(adapter);
-#endif /*CONFIG_DEBUG_FS */
 
 	set_bit(__IXGBE_DOWN, &adapter->state);
 	cancel_work_sync(&adapter->service_task);
@@ -7918,15 +7955,11 @@ static int __init ixgbe_init_module(void)
 	pr_info("%s - version %s\n", ixgbe_driver_string, ixgbe_driver_version);
 	pr_info("%s\n", ixgbe_copyright);
 
-#ifdef CONFIG_DEBUG_FS
 	ixgbe_dbg_init();
-#endif /* CONFIG_DEBUG_FS */
 
 	ret = pci_register_driver(&ixgbe_driver);
 	if (ret) {
-#ifdef CONFIG_DEBUG_FS
 		ixgbe_dbg_exit();
-#endif /* CONFIG_DEBUG_FS */
 		return ret;
 	}
 
@@ -7952,9 +7985,7 @@ static void __exit ixgbe_exit_module(void)
 #endif
 	pci_unregister_driver(&ixgbe_driver);
 
-#ifdef CONFIG_DEBUG_FS
 	ixgbe_dbg_exit();
-#endif /* CONFIG_DEBUG_FS */
 
 	rcu_barrier(); /* Wait for completion of call_rcu()'s */
 }
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
index 060d2ad2ac96..e5691ccbce9d 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
@@ -956,6 +956,13 @@ s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 				else
 					hw->phy.sfp_type =
 						ixgbe_sfp_type_1g_sx_core1;
+			} else if (comp_codes_1g & IXGBE_SFF_1GBASELX_CAPABLE) {
+				if (hw->bus.lan_id == 0)
+					hw->phy.sfp_type =
+						ixgbe_sfp_type_1g_lx_core0;
+				else
+					hw->phy.sfp_type =
+						ixgbe_sfp_type_1g_lx_core1;
 			} else {
 				hw->phy.sfp_type = ixgbe_sfp_type_unknown;
 			}
@@ -1043,6 +1050,8 @@ s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 		if (comp_codes_10g == 0 &&
 		    !(hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core1 ||
 		      hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core0 ||
+		      hw->phy.sfp_type == ixgbe_sfp_type_1g_lx_core0 ||
+		      hw->phy.sfp_type == ixgbe_sfp_type_1g_lx_core1 ||
 		      hw->phy.sfp_type == ixgbe_sfp_type_1g_sx_core0 ||
 		      hw->phy.sfp_type == ixgbe_sfp_type_1g_sx_core1)) {
 			hw->phy.type = ixgbe_phy_sfp_unsupported;
@@ -1058,10 +1067,12 @@ s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 
 		hw->mac.ops.get_device_caps(hw, &enforce_sfp);
 		if (!(enforce_sfp & IXGBE_DEVICE_CAPS_ALLOW_ANY_SFP) &&
-		    !((hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core0) ||
-		      (hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core1) ||
-		      (hw->phy.sfp_type == ixgbe_sfp_type_1g_sx_core0) ||
-		      (hw->phy.sfp_type == ixgbe_sfp_type_1g_sx_core1))) {
+		    !(hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core0 ||
+		      hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core1 ||
+		      hw->phy.sfp_type == ixgbe_sfp_type_1g_lx_core0 ||
+		      hw->phy.sfp_type == ixgbe_sfp_type_1g_lx_core1 ||
+		      hw->phy.sfp_type == ixgbe_sfp_type_1g_sx_core0 ||
+		      hw->phy.sfp_type == ixgbe_sfp_type_1g_sx_core1)) {
 			/* Make sure we're a supported PHY type */
 			if (hw->phy.type == ixgbe_phy_sfp_intel) {
 				status = 0;
@@ -1125,10 +1136,12 @@ s32 ixgbe_get_sfp_init_sequence_offsets(struct ixgbe_hw *hw,
 	 * SR modules
 	 */
 	if (sfp_type == ixgbe_sfp_type_da_act_lmt_core0 ||
+	    sfp_type == ixgbe_sfp_type_1g_lx_core0 ||
 	    sfp_type == ixgbe_sfp_type_1g_cu_core0 ||
 	    sfp_type == ixgbe_sfp_type_1g_sx_core0)
 		sfp_type = ixgbe_sfp_type_srlr_core0;
 	else if (sfp_type == ixgbe_sfp_type_da_act_lmt_core1 ||
+		 sfp_type == ixgbe_sfp_type_1g_lx_core1 ||
 		 sfp_type == ixgbe_sfp_type_1g_cu_core1 ||
 		 sfp_type == ixgbe_sfp_type_1g_sx_core1)
 		sfp_type = ixgbe_sfp_type_srlr_core1;
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
index 97e33669c0b9..1e7d587c4e57 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
@@ -35,7 +35,7 @@
 #include <linux/ip.h>
 #include <linux/tcp.h>
 #include <linux/ipv6.h>
-#ifdef NETIF_F_HW_VLAN_TX
+#ifdef NETIF_F_HW_VLAN_CTAG_TX
 #include <linux/if_vlan.h>
 #endif
 
@@ -661,13 +661,7 @@ int ixgbe_vf_configuration(struct pci_dev *pdev, unsigned int event_mask)
 	bool enable = ((event_mask & 0x10000000U) != 0);
 
 	if (enable) {
-		eth_random_addr(vf_mac_addr);
-		e_info(probe, "IOV: VF %d is enabled MAC %pM\n",
-		       vfn, vf_mac_addr);
-		/*
-		 * Store away the VF "permananet" MAC address, it will ask
-		 * for it later.
-		 */
+		eth_zero_addr(vf_mac_addr);
 		memcpy(adapter->vfinfo[vfn].vf_mac_addresses, vf_mac_addr, 6);
 	}
 
@@ -688,7 +682,8 @@ static int ixgbe_vf_reset_msg(struct ixgbe_adapter *adapter, u32 vf)
 	ixgbe_vf_reset_event(adapter, vf);
 
 	/* set vf mac address */
-	ixgbe_set_vf_mac(adapter, vf, vf_mac);
+	if (!is_zero_ether_addr(vf_mac))
+		ixgbe_set_vf_mac(adapter, vf, vf_mac);
 
 	vf_shift = vf % 32;
 	reg_offset = vf / 32;
@@ -729,8 +724,16 @@ static int ixgbe_vf_reset_msg(struct ixgbe_adapter *adapter, u32 vf)
 	IXGBE_WRITE_REG(hw, IXGBE_VMECM(reg_offset), reg);
 
 	/* reply to reset with ack and vf mac address */
-	msgbuf[0] = IXGBE_VF_RESET | IXGBE_VT_MSGTYPE_ACK;
-	memcpy(addr, vf_mac, ETH_ALEN);
+	msgbuf[0] = IXGBE_VF_RESET;
+	if (!is_zero_ether_addr(vf_mac)) {
+		msgbuf[0] |= IXGBE_VT_MSGTYPE_ACK;
+		memcpy(addr, vf_mac, ETH_ALEN);
+	} else {
+		msgbuf[0] |= IXGBE_VT_MSGTYPE_NACK;
+		dev_warn(&adapter->pdev->dev,
+			 "VF %d has no MAC address assigned, you may have to assign one manually\n",
+			 vf);
+	}
 
 	/*
 	 * Piggyback the multicast filter type so VF can compute the
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
index 6652e96c352d..70c6aa3d3f95 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
@@ -56,10 +56,13 @@
 #define IXGBE_SUBDEV_ID_82599_SFP        0x11A9
 #define IXGBE_SUBDEV_ID_82599_RNDC       0x1F72
 #define IXGBE_SUBDEV_ID_82599_560FLR     0x17D0
+#define IXGBE_SUBDEV_ID_82599_SP_560FLR  0x211B
 #define IXGBE_SUBDEV_ID_82599_ECNA_DP    0x0470
+#define IXGBE_SUBDEV_ID_82599_LOM_SFP    0x8976
 #define IXGBE_DEV_ID_82599_SFP_EM        0x1507
 #define IXGBE_DEV_ID_82599_SFP_SF2       0x154D
 #define IXGBE_DEV_ID_82599EN_SFP         0x1557
+#define IXGBE_SUBDEV_ID_82599EN_SFP_OCP1 0x0001
 #define IXGBE_DEV_ID_82599_XAUI_LOM      0x10FC
 #define IXGBE_DEV_ID_82599_COMBO_BACKPLANE 0x10F8
 #define IXGBE_SUBDEV_ID_82599_KX4_KR_MEZZ  0x000C
@@ -729,6 +732,13 @@ struct ixgbe_thermal_sensor_data {
 #define IXGBE_MDEF_EXT(_i) (0x05160 + ((_i) * 4)) /* 8 of these (0-7) */
 #define IXGBE_LSWFW     0x15014
 
+/* Management Bit Fields and Masks */
+#define IXGBE_MANC_RCV_TCO_EN	0x00020000 /* Rcv TCO packet enable */
+
+/* Firmware Semaphore Register */
+#define IXGBE_FWSM_MODE_MASK	0xE
+#define IXGBE_FWSM_FW_MODE_PT	0x4
+
 /* ARC Subsystem registers */
 #define IXGBE_HICR      0x15F00
 #define IXGBE_FWSTS     0x15F0C
@@ -1019,6 +1029,7 @@ struct ixgbe_thermal_sensor_data {
 #define IXGBE_CTRL_RST_MASK     (IXGBE_CTRL_LNK_RST | IXGBE_CTRL_RST)
 
 /* FACTPS */
+#define IXGBE_FACTPS_MNGCG      0x20000000 /* Manageblility Clock Gated */
 #define IXGBE_FACTPS_LFS        0x40000000 /* LAN Function Select */
 
 /* MHADD Bit Masks */
@@ -1582,6 +1593,7 @@ enum {
 #define IXGBE_AUTOC2_10G_KR  (0x0 << IXGBE_AUTOC2_10G_SERIAL_PMA_PMD_SHIFT)
 #define IXGBE_AUTOC2_10G_XFI (0x1 << IXGBE_AUTOC2_10G_SERIAL_PMA_PMD_SHIFT)
 #define IXGBE_AUTOC2_10G_SFI (0x2 << IXGBE_AUTOC2_10G_SERIAL_PMA_PMD_SHIFT)
+#define IXGBE_AUTOC2_LINK_DISABLE_MASK        0x70000000
 
 #define IXGBE_MACC_FLU       0x00000001
 #define IXGBE_MACC_FSV_10G   0x00030000
@@ -1827,6 +1839,7 @@ enum {
 #define IXGBE_PCI_LINK_SPEED      0xF
 #define IXGBE_PCI_LINK_SPEED_2500 0x1
 #define IXGBE_PCI_LINK_SPEED_5000 0x2
+#define IXGBE_PCI_LINK_SPEED_8000 0x3
 #define IXGBE_PCI_HEADER_TYPE_REGISTER  0x0E
 #define IXGBE_PCI_HEADER_TYPE_MULTIFUNC 0x80
 #define IXGBE_PCI_DEVICE_CONTROL2_16ms  0x0005
@@ -2600,6 +2613,8 @@ enum ixgbe_sfp_type {
 	ixgbe_sfp_type_1g_cu_core1 = 10,
 	ixgbe_sfp_type_1g_sx_core0 = 11,
 	ixgbe_sfp_type_1g_sx_core1 = 12,
+	ixgbe_sfp_type_1g_lx_core0 = 13,
+	ixgbe_sfp_type_1g_lx_core1 = 14,
 	ixgbe_sfp_type_not_present = 0xFFFE,
 	ixgbe_sfp_type_unknown = 0xFFFF
 };
@@ -2650,6 +2665,7 @@ enum ixgbe_bus_speed {
 	ixgbe_bus_speed_133     = 133,
 	ixgbe_bus_speed_2500    = 2500,
 	ixgbe_bus_speed_5000    = 5000,
+	ixgbe_bus_speed_8000    = 8000,
 	ixgbe_bus_speed_reserved
 };
 
@@ -2859,6 +2875,7 @@ struct ixgbe_mac_operations {
 	s32 (*set_fw_drv_ver)(struct ixgbe_hw *, u8, u8, u8, u8);
 	s32 (*get_thermal_sensor_data)(struct ixgbe_hw *);
 	s32 (*init_thermal_sensor_thresh)(struct ixgbe_hw *hw);
+	bool (*mng_fw_enabled)(struct ixgbe_hw *hw);
 };
 
 struct ixgbe_phy_operations {
@@ -2912,6 +2929,7 @@ struct ixgbe_mac_info {
 	u32                             max_tx_queues;
 	u32                             max_rx_queues;
 	u32                             orig_autoc;
+	u32                             cached_autoc;
 	u32                             orig_autoc2;
 	bool                            orig_link_settings_stored;
 	bool                            autotry_restart;
@@ -2986,6 +3004,8 @@ struct ixgbe_hw {
 	bool				adapter_stopped;
 	bool				force_full_reset;
 	bool				allow_unsupported_sfp;
+	bool				mng_fw_enabled;
+	bool				wol_enabled;
 };
 
 struct ixgbe_info {
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
index 66c5e946284e..389324f5929a 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
@@ -854,6 +854,7 @@ static struct ixgbe_mac_operations mac_ops_X540 = {
 	.enable_rx_buff		= &ixgbe_enable_rx_buff_generic,
 	.get_thermal_sensor_data = NULL,
 	.init_thermal_sensor_thresh = NULL,
+	.mng_fw_enabled		= NULL,
 };
 
 static struct ixgbe_eeprom_operations eeprom_ops_X540 = {
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
index fc0af9a3bb35..fff0d9867529 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
@@ -44,8 +44,8 @@ struct ixgbevf_tx_buffer {
 	struct sk_buff *skb;
 	dma_addr_t dma;
 	unsigned long time_stamp;
+	union ixgbe_adv_tx_desc *next_to_watch;
 	u16 length;
-	u16 next_to_watch;
 	u16 mapped_as_page;
 };
 
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
index 2b6cb5ca48ee..1f5166ad6bb5 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
@@ -76,12 +76,9 @@ static const struct ixgbevf_info *ixgbevf_info_tbl[] = {
  * { Vendor ID, Device ID, SubVendor ID, SubDevice ID,
  *   Class, Class Mask, private data (not used) }
  */
-static struct pci_device_id ixgbevf_pci_tbl[] = {
-	{PCI_VDEVICE(INTEL, IXGBE_DEV_ID_82599_VF),
-	board_82599_vf},
-	{PCI_VDEVICE(INTEL, IXGBE_DEV_ID_X540_VF),
-	board_X540_vf},
-
+static DEFINE_PCI_DEVICE_TABLE(ixgbevf_pci_tbl) = {
+	{PCI_VDEVICE(INTEL, IXGBE_DEV_ID_82599_VF), board_82599_vf },
+	{PCI_VDEVICE(INTEL, IXGBE_DEV_ID_X540_VF), board_X540_vf },
 	/* required last entry */
 	{0, }
 };
@@ -190,28 +187,37 @@ static bool ixgbevf_clean_tx_irq(struct ixgbevf_q_vector *q_vector,
 	struct ixgbevf_adapter *adapter = q_vector->adapter;
 	union ixgbe_adv_tx_desc *tx_desc, *eop_desc;
 	struct ixgbevf_tx_buffer *tx_buffer_info;
-	unsigned int i, eop, count = 0;
+	unsigned int i, count = 0;
 	unsigned int total_bytes = 0, total_packets = 0;
 
 	if (test_bit(__IXGBEVF_DOWN, &adapter->state))
 		return true;
 
 	i = tx_ring->next_to_clean;
-	eop = tx_ring->tx_buffer_info[i].next_to_watch;
-	eop_desc = IXGBEVF_TX_DESC(tx_ring, eop);
+	tx_buffer_info = &tx_ring->tx_buffer_info[i];
+	eop_desc = tx_buffer_info->next_to_watch;
 
-	while ((eop_desc->wb.status & cpu_to_le32(IXGBE_TXD_STAT_DD)) &&
-	       (count < tx_ring->count)) {
+	do {
 		bool cleaned = false;
-		rmb(); /* read buffer_info after eop_desc */
-		/* eop could change between read and DD-check */
-		if (unlikely(eop != tx_ring->tx_buffer_info[i].next_to_watch))
-			goto cont_loop;
+
+		/* if next_to_watch is not set then there is no work pending */
+		if (!eop_desc)
+			break;
+
+		/* prevent any other reads prior to eop_desc */
+		read_barrier_depends();
+
+		/* if DD is not set pending work has not been completed */
+		if (!(eop_desc->wb.status & cpu_to_le32(IXGBE_TXD_STAT_DD)))
+			break;
+
+		/* clear next_to_watch to prevent false hangs */
+		tx_buffer_info->next_to_watch = NULL;
+
 		for ( ; !cleaned; count++) {
 			struct sk_buff *skb;
 			tx_desc = IXGBEVF_TX_DESC(tx_ring, i);
-			tx_buffer_info = &tx_ring->tx_buffer_info[i];
-			cleaned = (i == eop);
+			cleaned = (tx_desc == eop_desc);
 			skb = tx_buffer_info->skb;
 
 			if (cleaned && skb) {
@@ -234,12 +240,12 @@ static bool ixgbevf_clean_tx_irq(struct ixgbevf_q_vector *q_vector,
 			i++;
 			if (i == tx_ring->count)
 				i = 0;
+
+			tx_buffer_info = &tx_ring->tx_buffer_info[i];
 		}
 
-cont_loop:
-		eop = tx_ring->tx_buffer_info[i].next_to_watch;
-		eop_desc = IXGBEVF_TX_DESC(tx_ring, eop);
-	}
+		eop_desc = tx_buffer_info->next_to_watch;
+	} while (count < tx_ring->count);
 
 	tx_ring->next_to_clean = i;
 
@@ -285,7 +291,7 @@ static void ixgbevf_receive_skb(struct ixgbevf_q_vector *q_vector,
 	u16 tag = le16_to_cpu(rx_desc->wb.upper.vlan);
 
 	if (is_vlan && test_bit(tag & VLAN_VID_MASK, adapter->active_vlans))
-		__vlan_hwaccel_put_tag(skb, tag);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), tag);
 
 	if (!(adapter->flags & IXGBE_FLAG_IN_NETPOLL))
 		napi_gro_receive(&q_vector->napi, skb);
@@ -1173,7 +1179,8 @@ static void ixgbevf_configure_rx(struct ixgbevf_adapter *adapter)
 	}
 }
 
-static int ixgbevf_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
+static int ixgbevf_vlan_rx_add_vid(struct net_device *netdev,
+				   __be16 proto, u16 vid)
 {
 	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
@@ -1198,7 +1205,8 @@ static int ixgbevf_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
 	return err;
 }
 
-static int ixgbevf_vlan_rx_kill_vid(struct net_device *netdev, u16 vid)
+static int ixgbevf_vlan_rx_kill_vid(struct net_device *netdev,
+				    __be16 proto, u16 vid)
 {
 	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
@@ -1221,7 +1229,8 @@ static void ixgbevf_restore_vlan(struct ixgbevf_adapter *adapter)
 	u16 vid;
 
 	for_each_set_bit(vid, adapter->active_vlans, VLAN_N_VID)
-		ixgbevf_vlan_rx_add_vid(adapter->netdev, vid);
+		ixgbevf_vlan_rx_add_vid(adapter->netdev,
+					htons(ETH_P_8021Q), vid);
 }
 
 static int ixgbevf_write_uc_addr_list(struct net_device *netdev)
@@ -2046,6 +2055,7 @@ static int ixgbevf_sw_init(struct ixgbevf_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
 	struct pci_dev *pdev = adapter->pdev;
+	struct net_device *netdev = adapter->netdev;
 	int err;
 
 	/* PCI config space info */
@@ -2065,18 +2075,26 @@ static int ixgbevf_sw_init(struct ixgbevf_adapter *adapter)
 	err = hw->mac.ops.reset_hw(hw);
 	if (err) {
 		dev_info(&pdev->dev,
-		         "PF still in reset state, assigning new address\n");
-		eth_hw_addr_random(adapter->netdev);
-		memcpy(adapter->hw.mac.addr, adapter->netdev->dev_addr,
-			adapter->netdev->addr_len);
+			 "PF still in reset state.  Is the PF interface up?\n");
 	} else {
 		err = hw->mac.ops.init_hw(hw);
 		if (err) {
 			pr_err("init_shared_code failed: %d\n", err);
 			goto out;
 		}
-		memcpy(adapter->netdev->dev_addr, adapter->hw.mac.addr,
-		       adapter->netdev->addr_len);
+		err = hw->mac.ops.get_mac_addr(hw, hw->mac.addr);
+		if (err)
+			dev_info(&pdev->dev, "Error reading MAC address\n");
+		else if (is_zero_ether_addr(adapter->hw.mac.addr))
+			dev_info(&pdev->dev,
+				 "MAC address not assigned by administrator.\n");
+		memcpy(netdev->dev_addr, hw->mac.addr, netdev->addr_len);
+	}
+
+	if (!is_valid_ether_addr(netdev->dev_addr)) {
+		dev_info(&pdev->dev, "Assigning random MAC address\n");
+		eth_hw_addr_random(netdev);
+		memcpy(hw->mac.addr, netdev->dev_addr, netdev->addr_len);
 	}
 
 	/* lock to protect mailbox accesses */
@@ -2425,9 +2443,6 @@ int ixgbevf_setup_rx_resources(struct ixgbevf_adapter *adapter,
 					   &rx_ring->dma, GFP_KERNEL);
 
 	if (!rx_ring->desc) {
-		hw_dbg(&adapter->hw,
-		       "Unable to allocate memory for "
-		       "the receive descriptor ring\n");
 		vfree(rx_ring->rx_buffer_info);
 		rx_ring->rx_buffer_info = NULL;
 		goto alloc_failed;
@@ -2822,8 +2837,7 @@ static bool ixgbevf_tx_csum(struct ixgbevf_ring *tx_ring,
 }
 
 static int ixgbevf_tx_map(struct ixgbevf_ring *tx_ring,
-			  struct sk_buff *skb, u32 tx_flags,
-			  unsigned int first)
+			  struct sk_buff *skb, u32 tx_flags)
 {
 	struct ixgbevf_tx_buffer *tx_buffer_info;
 	unsigned int len;
@@ -2848,7 +2862,6 @@ static int ixgbevf_tx_map(struct ixgbevf_ring *tx_ring,
 						     size, DMA_TO_DEVICE);
 		if (dma_mapping_error(tx_ring->dev, tx_buffer_info->dma))
 			goto dma_error;
-		tx_buffer_info->next_to_watch = i;
 
 		len -= size;
 		total -= size;
@@ -2878,7 +2891,6 @@ static int ixgbevf_tx_map(struct ixgbevf_ring *tx_ring,
 					      tx_buffer_info->dma))
 				goto dma_error;
 			tx_buffer_info->mapped_as_page = true;
-			tx_buffer_info->next_to_watch = i;
 
 			len -= size;
 			total -= size;
@@ -2897,8 +2909,6 @@ static int ixgbevf_tx_map(struct ixgbevf_ring *tx_ring,
 	else
 		i = i - 1;
 	tx_ring->tx_buffer_info[i].skb = skb;
-	tx_ring->tx_buffer_info[first].next_to_watch = i;
-	tx_ring->tx_buffer_info[first].time_stamp = jiffies;
 
 	return count;
 
@@ -2907,7 +2917,6 @@ dma_error:
 
 	/* clear timestamp and dma mappings for failed tx_buffer_info map */
 	tx_buffer_info->dma = 0;
-	tx_buffer_info->next_to_watch = 0;
 	count--;
 
 	/* clear timestamp and dma mappings for remaining portion of packet */
@@ -2924,7 +2933,8 @@ dma_error:
 }
 
 static void ixgbevf_tx_queue(struct ixgbevf_ring *tx_ring, int tx_flags,
-			     int count, u32 paylen, u8 hdr_len)
+			     int count, unsigned int first, u32 paylen,
+			     u8 hdr_len)
 {
 	union ixgbe_adv_tx_desc *tx_desc = NULL;
 	struct ixgbevf_tx_buffer *tx_buffer_info;
@@ -2975,6 +2985,16 @@ static void ixgbevf_tx_queue(struct ixgbevf_ring *tx_ring, int tx_flags,
 
 	tx_desc->read.cmd_type_len |= cpu_to_le32(txd_cmd);
 
+	tx_ring->tx_buffer_info[first].time_stamp = jiffies;
+
+	/* Force memory writes to complete before letting h/w
+	 * know there are new descriptors to fetch.  (Only
+	 * applicable for weak-ordered memory model archs,
+	 * such as IA-64).
+	 */
+	wmb();
+
+	tx_ring->tx_buffer_info[first].next_to_watch = tx_desc;
 	tx_ring->next_to_use = i;
 }
 
@@ -3066,15 +3086,8 @@ static int ixgbevf_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
 		tx_flags |= IXGBE_TX_FLAGS_CSUM;
 
 	ixgbevf_tx_queue(tx_ring, tx_flags,
-			 ixgbevf_tx_map(tx_ring, skb, tx_flags, first),
-			 skb->len, hdr_len);
-	/*
-	 * Force memory writes to complete before letting h/w
-	 * know there are new descriptors to fetch.  (Only
-	 * applicable for weak-ordered memory model archs,
-	 * such as IA-64).
-	 */
-	wmb();
+			 ixgbevf_tx_map(tx_ring, skb, tx_flags),
+			 first, skb->len, hdr_len);
 
 	writel(tx_ring->next_to_use, adapter->hw.hw_addr + tx_ring->tail);
 
@@ -3400,9 +3413,9 @@ static int ixgbevf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 			   NETIF_F_RXCSUM;
 
 	netdev->features = netdev->hw_features |
-			   NETIF_F_HW_VLAN_TX |
-			   NETIF_F_HW_VLAN_RX |
-			   NETIF_F_HW_VLAN_FILTER;
+			   NETIF_F_HW_VLAN_CTAG_TX |
+			   NETIF_F_HW_VLAN_CTAG_RX |
+			   NETIF_F_HW_VLAN_CTAG_FILTER;
 
 	netdev->vlan_features |= NETIF_F_TSO;
 	netdev->vlan_features |= NETIF_F_TSO6;
diff --git a/drivers/net/ethernet/intel/ixgbevf/vf.c b/drivers/net/ethernet/intel/ixgbevf/vf.c
index 0c94557b53df..387b52635bc0 100644
--- a/drivers/net/ethernet/intel/ixgbevf/vf.c
+++ b/drivers/net/ethernet/intel/ixgbevf/vf.c
@@ -109,7 +109,12 @@ static s32 ixgbevf_reset_hw_vf(struct ixgbe_hw *hw)
 	if (ret_val)
 		return ret_val;
 
-	if (msgbuf[0] != (IXGBE_VF_RESET | IXGBE_VT_MSGTYPE_ACK))
+	/* New versions of the PF may NACK the reset return message
+	 * to indicate that no MAC address has yet been assigned for
+	 * the VF.
+	 */
+	if (msgbuf[0] != (IXGBE_VF_RESET | IXGBE_VT_MSGTYPE_ACK) &&
+	    msgbuf[0] != (IXGBE_VF_RESET | IXGBE_VT_MSGTYPE_NACK))
 		return IXGBE_ERR_INVALID_MAC_ADDR;
 
 	memcpy(hw->mac.perm_addr, addr, ETH_ALEN);
diff --git a/drivers/net/ethernet/jme.c b/drivers/net/ethernet/jme.c
index 0519afa413d2..070a6f1a0577 100644
--- a/drivers/net/ethernet/jme.c
+++ b/drivers/net/ethernet/jme.c
@@ -1059,7 +1059,7 @@ jme_alloc_and_feed_skb(struct jme_adapter *jme, int idx)
 		if (rxdesc->descwb.flags & cpu_to_le16(RXWBFLAG_TAGON)) {
 			u16 vid = le16_to_cpu(rxdesc->descwb.vlan);
 
-			__vlan_hwaccel_put_tag(skb, vid);
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
 			NET_STAT(jme).rx_bytes += 4;
 		}
 		jme->jme_rx(skb);
@@ -3030,8 +3030,8 @@ jme_init_one(struct pci_dev *pdev,
 						NETIF_F_SG |
 						NETIF_F_TSO |
 						NETIF_F_TSO6 |
-						NETIF_F_HW_VLAN_TX |
-						NETIF_F_HW_VLAN_RX;
+						NETIF_F_HW_VLAN_CTAG_TX |
+						NETIF_F_HW_VLAN_CTAG_RX;
 	if (using_dac)
 		netdev->features	|=	NETIF_F_HIGHDMA;
 
diff --git a/drivers/net/ethernet/marvell/Kconfig b/drivers/net/ethernet/marvell/Kconfig
index 434e33c527df..a49e81bdf8e8 100644
--- a/drivers/net/ethernet/marvell/Kconfig
+++ b/drivers/net/ethernet/marvell/Kconfig
@@ -21,8 +21,8 @@ if NET_VENDOR_MARVELL
 config MV643XX_ETH
 	tristate "Marvell Discovery (643XX) and Orion ethernet support"
 	depends on (MV64X60 || PPC32 || PLAT_ORION) && INET
-	select INET_LRO
 	select PHYLIB
+	select MVMDIO
 	---help---
 	  This driver supports the gigabit ethernet MACs in the
 	  Marvell Discovery PPC/MIPS chipset family (MV643XX) and
@@ -39,9 +39,7 @@ config MVMDIO
 	  interface units of the Marvell EBU SoCs (Kirkwood, Orion5x,
 	  Dove, Armada 370 and Armada XP).
 
-	  For now, this driver is only needed for the MVNETA driver
-	  (used on Armada 370 and XP), but it could be used in the
-	  future by the MV643XX_ETH driver.
+	  This driver is used by the MV643XX_ETH and MVNETA drivers.
 
 config MVNETA
 	tristate "Marvell Armada 370/XP network interface support"
diff --git a/drivers/net/ethernet/marvell/Makefile b/drivers/net/ethernet/marvell/Makefile
index 7f63b4aac434..5c4a7765ff0e 100644
--- a/drivers/net/ethernet/marvell/Makefile
+++ b/drivers/net/ethernet/marvell/Makefile
@@ -2,8 +2,8 @@
 # Makefile for the Marvell device drivers.
 #
 
-obj-$(CONFIG_MV643XX_ETH) += mv643xx_eth.o
 obj-$(CONFIG_MVMDIO) += mvmdio.o
+obj-$(CONFIG_MV643XX_ETH) += mv643xx_eth.o
 obj-$(CONFIG_MVNETA) += mvneta.o
 obj-$(CONFIG_PXA168_ETH) += pxa168_eth.o
 obj-$(CONFIG_SKGE) += skge.o
diff --git a/drivers/net/ethernet/marvell/mv643xx_eth.c b/drivers/net/ethernet/marvell/mv643xx_eth.c
index 6562c736a1d8..d0afeea181fb 100644
--- a/drivers/net/ethernet/marvell/mv643xx_eth.c
+++ b/drivers/net/ethernet/marvell/mv643xx_eth.c
@@ -20,6 +20,8 @@
  * Copyright (C) 2007-2008 Marvell Semiconductor
  *			   Lennert Buytenhek <buytenh@marvell.com>
  *
+ * Copyright (C) 2013 Michael Stapelberg <michael@stapelberg.de>
+ *
  * 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
@@ -54,8 +56,8 @@
 #include <linux/phy.h>
 #include <linux/mv643xx_eth.h>
 #include <linux/io.h>
+#include <linux/interrupt.h>
 #include <linux/types.h>
-#include <linux/inet_lro.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
 
@@ -67,14 +69,6 @@ static char mv643xx_eth_driver_version[] = "1.4";
  * Registers shared between all ports.
  */
 #define PHY_ADDR			0x0000
-#define SMI_REG				0x0004
-#define  SMI_BUSY			0x10000000
-#define  SMI_READ_VALID			0x08000000
-#define  SMI_OPCODE_READ		0x04000000
-#define  SMI_OPCODE_WRITE		0x00000000
-#define ERR_INT_CAUSE			0x0080
-#define  ERR_INT_SMI_DONE		0x00000010
-#define ERR_INT_MASK			0x0084
 #define WINDOW_BASE(w)			(0x0200 + ((w) << 3))
 #define WINDOW_SIZE(w)			(0x0204 + ((w) << 3))
 #define WINDOW_REMAP_HIGH(w)		(0x0280 + ((w) << 2))
@@ -264,25 +258,6 @@ struct mv643xx_eth_shared_private {
 	void __iomem *base;
 
 	/*
-	 * Points at the right SMI instance to use.
-	 */
-	struct mv643xx_eth_shared_private *smi;
-
-	/*
-	 * Provides access to local SMI interface.
-	 */
-	struct mii_bus *smi_bus;
-
-	/*
-	 * If we have access to the error interrupt pin (which is
-	 * somewhat misnamed as it not only reflects internal errors
-	 * but also reflects SMI completion), use that to wait for
-	 * SMI access completion instead of polling the SMI busy bit.
-	 */
-	int err_interrupt;
-	wait_queue_head_t smi_busy_wait;
-
-	/*
 	 * Per-port MBUS window access register value.
 	 */
 	u32 win_protect;
@@ -293,7 +268,7 @@ struct mv643xx_eth_shared_private {
 	int extended_rx_coal_limit;
 	int tx_bw_control;
 	int tx_csum_limit;
-
+	struct clk *clk;
 };
 
 #define TX_BW_CONTROL_ABSENT		0
@@ -341,12 +316,6 @@ struct mib_counters {
 	u32 rx_overrun;
 };
 
-struct lro_counters {
-	u32 lro_aggregated;
-	u32 lro_flushed;
-	u32 lro_no_desc;
-};
-
 struct rx_queue {
 	int index;
 
@@ -360,9 +329,6 @@ struct rx_queue {
 	dma_addr_t rx_desc_dma;
 	int rx_desc_area_size;
 	struct sk_buff **rx_skb;
-
-	struct net_lro_mgr lro_mgr;
-	struct net_lro_desc lro_arr[8];
 };
 
 struct tx_queue {
@@ -398,8 +364,6 @@ struct mv643xx_eth_private {
 	spinlock_t mib_counters_lock;
 	struct mib_counters mib_counters;
 
-	struct lro_counters lro_counters;
-
 	struct work_struct tx_timeout_task;
 
 	struct napi_struct napi;
@@ -435,9 +399,7 @@ struct mv643xx_eth_private {
 	/*
 	 * Hardware-specific parameters.
 	 */
-#if defined(CONFIG_HAVE_CLK)
 	struct clk *clk;
-#endif
 	unsigned int t_clk;
 };
 
@@ -530,42 +492,12 @@ static void txq_maybe_wake(struct tx_queue *txq)
 	}
 }
 
-
-/* rx napi ******************************************************************/
-static int
-mv643xx_get_skb_header(struct sk_buff *skb, void **iphdr, void **tcph,
-		       u64 *hdr_flags, void *priv)
-{
-	unsigned long cmd_sts = (unsigned long)priv;
-
-	/*
-	 * Make sure that this packet is Ethernet II, is not VLAN
-	 * tagged, is IPv4, has a valid IP header, and is TCP.
-	 */
-	if ((cmd_sts & (RX_IP_HDR_OK | RX_PKT_IS_IPV4 |
-		       RX_PKT_IS_ETHERNETV2 | RX_PKT_LAYER4_TYPE_MASK |
-		       RX_PKT_IS_VLAN_TAGGED)) !=
-	    (RX_IP_HDR_OK | RX_PKT_IS_IPV4 |
-	     RX_PKT_IS_ETHERNETV2 | RX_PKT_LAYER4_TYPE_TCP_IPV4))
-		return -1;
-
-	skb_reset_network_header(skb);
-	skb_set_transport_header(skb, ip_hdrlen(skb));
-	*iphdr = ip_hdr(skb);
-	*tcph = tcp_hdr(skb);
-	*hdr_flags = LRO_IPV4 | LRO_TCP;
-
-	return 0;
-}
-
 static int rxq_process(struct rx_queue *rxq, int budget)
 {
 	struct mv643xx_eth_private *mp = rxq_to_mp(rxq);
 	struct net_device_stats *stats = &mp->dev->stats;
-	int lro_flush_needed;
 	int rx;
 
-	lro_flush_needed = 0;
 	rx = 0;
 	while (rx < budget && rxq->rx_desc_count) {
 		struct rx_desc *rx_desc;
@@ -626,12 +558,7 @@ static int rxq_process(struct rx_queue *rxq, int budget)
 			skb->ip_summed = CHECKSUM_UNNECESSARY;
 		skb->protocol = eth_type_trans(skb, mp->dev);
 
-		if (skb->dev->features & NETIF_F_LRO &&
-		    skb->ip_summed == CHECKSUM_UNNECESSARY) {
-			lro_receive_skb(&rxq->lro_mgr, skb, (void *)cmd_sts);
-			lro_flush_needed = 1;
-		} else
-			netif_receive_skb(skb);
+		napi_gro_receive(&mp->napi, skb);
 
 		continue;
 
@@ -651,9 +578,6 @@ err:
 		dev_kfree_skb(skb);
 	}
 
-	if (lro_flush_needed)
-		lro_flush_all(&rxq->lro_mgr);
-
 	if (rx < budget)
 		mp->work_rx &= ~(1 << rxq->index);
 
@@ -1120,97 +1044,6 @@ out_write:
 	wrlp(mp, PORT_SERIAL_CONTROL, pscr);
 }
 
-static irqreturn_t mv643xx_eth_err_irq(int irq, void *dev_id)
-{
-	struct mv643xx_eth_shared_private *msp = dev_id;
-
-	if (readl(msp->base + ERR_INT_CAUSE) & ERR_INT_SMI_DONE) {
-		writel(~ERR_INT_SMI_DONE, msp->base + ERR_INT_CAUSE);
-		wake_up(&msp->smi_busy_wait);
-		return IRQ_HANDLED;
-	}
-
-	return IRQ_NONE;
-}
-
-static int smi_is_done(struct mv643xx_eth_shared_private *msp)
-{
-	return !(readl(msp->base + SMI_REG) & SMI_BUSY);
-}
-
-static int smi_wait_ready(struct mv643xx_eth_shared_private *msp)
-{
-	if (msp->err_interrupt == NO_IRQ) {
-		int i;
-
-		for (i = 0; !smi_is_done(msp); i++) {
-			if (i == 10)
-				return -ETIMEDOUT;
-			msleep(10);
-		}
-
-		return 0;
-	}
-
-	if (!smi_is_done(msp)) {
-		wait_event_timeout(msp->smi_busy_wait, smi_is_done(msp),
-				   msecs_to_jiffies(100));
-		if (!smi_is_done(msp))
-			return -ETIMEDOUT;
-	}
-
-	return 0;
-}
-
-static int smi_bus_read(struct mii_bus *bus, int addr, int reg)
-{
-	struct mv643xx_eth_shared_private *msp = bus->priv;
-	void __iomem *smi_reg = msp->base + SMI_REG;
-	int ret;
-
-	if (smi_wait_ready(msp)) {
-		pr_warn("SMI bus busy timeout\n");
-		return -ETIMEDOUT;
-	}
-
-	writel(SMI_OPCODE_READ | (reg << 21) | (addr << 16), smi_reg);
-
-	if (smi_wait_ready(msp)) {
-		pr_warn("SMI bus busy timeout\n");
-		return -ETIMEDOUT;
-	}
-
-	ret = readl(smi_reg);
-	if (!(ret & SMI_READ_VALID)) {
-		pr_warn("SMI bus read not valid\n");
-		return -ENODEV;
-	}
-
-	return ret & 0xffff;
-}
-
-static int smi_bus_write(struct mii_bus *bus, int addr, int reg, u16 val)
-{
-	struct mv643xx_eth_shared_private *msp = bus->priv;
-	void __iomem *smi_reg = msp->base + SMI_REG;
-
-	if (smi_wait_ready(msp)) {
-		pr_warn("SMI bus busy timeout\n");
-		return -ETIMEDOUT;
-	}
-
-	writel(SMI_OPCODE_WRITE | (reg << 21) |
-		(addr << 16) | (val & 0xffff), smi_reg);
-
-	if (smi_wait_ready(msp)) {
-		pr_warn("SMI bus busy timeout\n");
-		return -ETIMEDOUT;
-	}
-
-	return 0;
-}
-
-
 /* statistics ***************************************************************/
 static struct net_device_stats *mv643xx_eth_get_stats(struct net_device *dev)
 {
@@ -1236,26 +1069,6 @@ static struct net_device_stats *mv643xx_eth_get_stats(struct net_device *dev)
 	return stats;
 }
 
-static void mv643xx_eth_grab_lro_stats(struct mv643xx_eth_private *mp)
-{
-	u32 lro_aggregated = 0;
-	u32 lro_flushed = 0;
-	u32 lro_no_desc = 0;
-	int i;
-
-	for (i = 0; i < mp->rxq_count; i++) {
-		struct rx_queue *rxq = mp->rxq + i;
-
-		lro_aggregated += rxq->lro_mgr.stats.aggregated;
-		lro_flushed += rxq->lro_mgr.stats.flushed;
-		lro_no_desc += rxq->lro_mgr.stats.no_desc;
-	}
-
-	mp->lro_counters.lro_aggregated = lro_aggregated;
-	mp->lro_counters.lro_flushed = lro_flushed;
-	mp->lro_counters.lro_no_desc = lro_no_desc;
-}
-
 static inline u32 mib_read(struct mv643xx_eth_private *mp, int offset)
 {
 	return rdl(mp, MIB_COUNTERS(mp->port_num) + offset);
@@ -1419,10 +1232,6 @@ struct mv643xx_eth_stats {
 	{ #m, FIELD_SIZEOF(struct mib_counters, m),		\
 	  -1, offsetof(struct mv643xx_eth_private, mib_counters.m) }
 
-#define LROSTAT(m)						\
-	{ #m, FIELD_SIZEOF(struct lro_counters, m),		\
-	  -1, offsetof(struct mv643xx_eth_private, lro_counters.m) }
-
 static const struct mv643xx_eth_stats mv643xx_eth_stats[] = {
 	SSTAT(rx_packets),
 	SSTAT(tx_packets),
@@ -1464,9 +1273,6 @@ static const struct mv643xx_eth_stats mv643xx_eth_stats[] = {
 	MIBSTAT(late_collision),
 	MIBSTAT(rx_discard),
 	MIBSTAT(rx_overrun),
-	LROSTAT(lro_aggregated),
-	LROSTAT(lro_flushed),
-	LROSTAT(lro_no_desc),
 };
 
 static int
@@ -1523,6 +1329,34 @@ mv643xx_eth_get_settings_phyless(struct mv643xx_eth_private *mp,
 	return 0;
 }
 
+static void
+mv643xx_eth_get_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
+{
+	struct mv643xx_eth_private *mp = netdev_priv(dev);
+	wol->supported = 0;
+	wol->wolopts = 0;
+	if (mp->phy)
+		phy_ethtool_get_wol(mp->phy, wol);
+}
+
+static int
+mv643xx_eth_set_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
+{
+	struct mv643xx_eth_private *mp = netdev_priv(dev);
+	int err;
+
+	if (mp->phy == NULL)
+		return -EOPNOTSUPP;
+
+	err = phy_ethtool_set_wol(mp->phy, wol);
+	/* Given that mv643xx_eth works without the marvell-specific PHY driver,
+	 * this debugging hint is useful to have.
+	 */
+	if (err == -EOPNOTSUPP)
+		netdev_info(dev, "The PHY does not support set_wol, was CONFIG_MARVELL_PHY enabled?\n");
+	return err;
+}
+
 static int
 mv643xx_eth_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 {
@@ -1668,7 +1502,6 @@ static void mv643xx_eth_get_ethtool_stats(struct net_device *dev,
 
 	mv643xx_eth_get_stats(dev);
 	mib_counters_update(mp);
-	mv643xx_eth_grab_lro_stats(mp);
 
 	for (i = 0; i < ARRAY_SIZE(mv643xx_eth_stats); i++) {
 		const struct mv643xx_eth_stats *stat;
@@ -1708,6 +1541,8 @@ static const struct ethtool_ops mv643xx_eth_ethtool_ops = {
 	.get_ethtool_stats	= mv643xx_eth_get_ethtool_stats,
 	.get_sset_count		= mv643xx_eth_get_sset_count,
 	.get_ts_info		= ethtool_op_get_ts_info,
+	.get_wol                = mv643xx_eth_get_wol,
+	.set_wol                = mv643xx_eth_set_wol,
 };
 
 
@@ -1939,19 +1774,6 @@ static int rxq_init(struct mv643xx_eth_private *mp, int index)
 					nexti * sizeof(struct rx_desc);
 	}
 
-	rxq->lro_mgr.dev = mp->dev;
-	memset(&rxq->lro_mgr.stats, 0, sizeof(rxq->lro_mgr.stats));
-	rxq->lro_mgr.features = LRO_F_NAPI;
-	rxq->lro_mgr.ip_summed = CHECKSUM_UNNECESSARY;
-	rxq->lro_mgr.ip_summed_aggr = CHECKSUM_UNNECESSARY;
-	rxq->lro_mgr.max_desc = ARRAY_SIZE(rxq->lro_arr);
-	rxq->lro_mgr.max_aggr = 32;
-	rxq->lro_mgr.frag_align_pad = 0;
-	rxq->lro_mgr.lro_arr = rxq->lro_arr;
-	rxq->lro_mgr.get_skb_header = mv643xx_get_skb_header;
-
-	memset(&rxq->lro_arr, 0, sizeof(rxq->lro_arr));
-
 	return 0;
 
 
@@ -2635,66 +2457,26 @@ static int mv643xx_eth_shared_probe(struct platform_device *pdev)
 	struct mv643xx_eth_shared_private *msp;
 	const struct mbus_dram_target_info *dram;
 	struct resource *res;
-	int ret;
 
 	if (!mv643xx_eth_version_printed++)
 		pr_notice("MV-643xx 10/100/1000 ethernet driver version %s\n",
 			  mv643xx_eth_driver_version);
 
-	ret = -EINVAL;
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (res == NULL)
-		goto out;
+		return -EINVAL;
 
-	ret = -ENOMEM;
-	msp = kzalloc(sizeof(*msp), GFP_KERNEL);
+	msp = devm_kzalloc(&pdev->dev, sizeof(*msp), GFP_KERNEL);
 	if (msp == NULL)
-		goto out;
+		return -ENOMEM;
 
 	msp->base = ioremap(res->start, resource_size(res));
 	if (msp->base == NULL)
-		goto out_free;
-
-	/*
-	 * Set up and register SMI bus.
-	 */
-	if (pd == NULL || pd->shared_smi == NULL) {
-		msp->smi_bus = mdiobus_alloc();
-		if (msp->smi_bus == NULL)
-			goto out_unmap;
-
-		msp->smi_bus->priv = msp;
-		msp->smi_bus->name = "mv643xx_eth smi";
-		msp->smi_bus->read = smi_bus_read;
-		msp->smi_bus->write = smi_bus_write,
-		snprintf(msp->smi_bus->id, MII_BUS_ID_SIZE, "%s-%d",
-			pdev->name, pdev->id);
-		msp->smi_bus->parent = &pdev->dev;
-		msp->smi_bus->phy_mask = 0xffffffff;
-		if (mdiobus_register(msp->smi_bus) < 0)
-			goto out_free_mii_bus;
-		msp->smi = msp;
-	} else {
-		msp->smi = platform_get_drvdata(pd->shared_smi);
-	}
-
-	msp->err_interrupt = NO_IRQ;
-	init_waitqueue_head(&msp->smi_busy_wait);
+		return -ENOMEM;
 
-	/*
-	 * Check whether the error interrupt is hooked up.
-	 */
-	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (res != NULL) {
-		int err;
-
-		err = request_irq(res->start, mv643xx_eth_err_irq,
-				  IRQF_SHARED, "mv643xx_eth", msp);
-		if (!err) {
-			writel(ERR_INT_SMI_DONE, msp->base + ERR_INT_MASK);
-			msp->err_interrupt = res->start;
-		}
-	}
+	msp->clk = devm_clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(msp->clk))
+		clk_prepare_enable(msp->clk);
 
 	/*
 	 * (Re-)program MBUS remapping windows if we are asked to.
@@ -2710,30 +2492,15 @@ static int mv643xx_eth_shared_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, msp);
 
 	return 0;
-
-out_free_mii_bus:
-	mdiobus_free(msp->smi_bus);
-out_unmap:
-	iounmap(msp->base);
-out_free:
-	kfree(msp);
-out:
-	return ret;
 }
 
 static int mv643xx_eth_shared_remove(struct platform_device *pdev)
 {
 	struct mv643xx_eth_shared_private *msp = platform_get_drvdata(pdev);
-	struct mv643xx_eth_shared_platform_data *pd = pdev->dev.platform_data;
 
-	if (pd == NULL || pd->shared_smi == NULL) {
-		mdiobus_unregister(msp->smi_bus);
-		mdiobus_free(msp->smi_bus);
-	}
-	if (msp->err_interrupt != NO_IRQ)
-		free_irq(msp->err_interrupt, msp);
 	iounmap(msp->base);
-	kfree(msp);
+	if (!IS_ERR(msp->clk))
+		clk_disable_unprepare(msp->clk);
 
 	return 0;
 }
@@ -2794,14 +2561,21 @@ static void set_params(struct mv643xx_eth_private *mp,
 	mp->txq_count = pd->tx_queue_count ? : 1;
 }
 
+static void mv643xx_eth_adjust_link(struct net_device *dev)
+{
+	struct mv643xx_eth_private *mp = netdev_priv(dev);
+
+	mv643xx_adjust_pscr(mp);
+}
+
 static struct phy_device *phy_scan(struct mv643xx_eth_private *mp,
 				   int phy_addr)
 {
-	struct mii_bus *bus = mp->shared->smi->smi_bus;
 	struct phy_device *phydev;
 	int start;
 	int num;
 	int i;
+	char phy_id[MII_BUS_ID_SIZE + 3];
 
 	if (phy_addr == MV643XX_ETH_PHY_ADDR_DEFAULT) {
 		start = phy_addr_get(mp) & 0x1f;
@@ -2811,17 +2585,19 @@ static struct phy_device *phy_scan(struct mv643xx_eth_private *mp,
 		num = 1;
 	}
 
-	phydev = NULL;
+	/* Attempt to connect to the PHY using orion-mdio */
+	phydev = ERR_PTR(-ENODEV);
 	for (i = 0; i < num; i++) {
 		int addr = (start + i) & 0x1f;
 
-		if (bus->phy_map[addr] == NULL)
-			mdiobus_scan(bus, addr);
+		snprintf(phy_id, sizeof(phy_id), PHY_ID_FMT,
+				"orion-mdio-mii", addr);
 
-		if (phydev == NULL) {
-			phydev = bus->phy_map[addr];
-			if (phydev != NULL)
-				phy_addr_set(mp, addr);
+		phydev = phy_connect(mp->dev, phy_id, mv643xx_eth_adjust_link,
+				PHY_INTERFACE_MODE_GMII);
+		if (!IS_ERR(phydev)) {
+			phy_addr_set(mp, addr);
+			break;
 		}
 	}
 
@@ -2834,8 +2610,6 @@ static void phy_init(struct mv643xx_eth_private *mp, int speed, int duplex)
 
 	phy_reset(mp);
 
-	phy_attach(mp->dev, dev_name(&phy->dev), PHY_INTERFACE_MODE_GMII);
-
 	if (speed == 0) {
 		phy->autoneg = AUTONEG_ENABLE;
 		phy->speed = 0;
@@ -2932,22 +2706,27 @@ static int mv643xx_eth_probe(struct platform_device *pdev)
 	 * it to override the default.
 	 */
 	mp->t_clk = 133000000;
-#if defined(CONFIG_HAVE_CLK)
-	mp->clk = clk_get(&pdev->dev, (pdev->id ? "1" : "0"));
+	mp->clk = devm_clk_get(&pdev->dev, NULL);
 	if (!IS_ERR(mp->clk)) {
 		clk_prepare_enable(mp->clk);
 		mp->t_clk = clk_get_rate(mp->clk);
 	}
-#endif
+
 	set_params(mp, pd);
 	netif_set_real_num_tx_queues(dev, mp->txq_count);
 	netif_set_real_num_rx_queues(dev, mp->rxq_count);
 
-	if (pd->phy_addr != MV643XX_ETH_PHY_NONE)
+	if (pd->phy_addr != MV643XX_ETH_PHY_NONE) {
 		mp->phy = phy_scan(mp, pd->phy_addr);
 
-	if (mp->phy != NULL)
+		if (IS_ERR(mp->phy)) {
+			err = PTR_ERR(mp->phy);
+			if (err == -ENODEV)
+				err = -EPROBE_DEFER;
+			goto out;
+		}
 		phy_init(mp, pd->speed, pd->duplex);
+	}
 
 	SET_ETHTOOL_OPS(dev, &mv643xx_eth_ethtool_ops);
 
@@ -2982,8 +2761,7 @@ static int mv643xx_eth_probe(struct platform_device *pdev)
 	dev->watchdog_timeo = 2 * HZ;
 	dev->base_addr = 0;
 
-	dev->hw_features = NETIF_F_SG | NETIF_F_IP_CSUM |
-		NETIF_F_RXCSUM | NETIF_F_LRO;
+	dev->hw_features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_RXCSUM;
 	dev->features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_RXCSUM;
 	dev->vlan_features = NETIF_F_SG | NETIF_F_IP_CSUM;
 
@@ -3014,12 +2792,8 @@ static int mv643xx_eth_probe(struct platform_device *pdev)
 	return 0;
 
 out:
-#if defined(CONFIG_HAVE_CLK)
-	if (!IS_ERR(mp->clk)) {
+	if (!IS_ERR(mp->clk))
 		clk_disable_unprepare(mp->clk);
-		clk_put(mp->clk);
-	}
-#endif
 	free_netdev(dev);
 
 	return err;
@@ -3034,12 +2808,8 @@ static int mv643xx_eth_remove(struct platform_device *pdev)
 		phy_detach(mp->phy);
 	cancel_work_sync(&mp->tx_timeout_task);
 
-#if defined(CONFIG_HAVE_CLK)
-	if (!IS_ERR(mp->clk)) {
+	if (!IS_ERR(mp->clk))
 		clk_disable_unprepare(mp->clk);
-		clk_put(mp->clk);
-	}
-#endif
 
 	free_netdev(mp->dev);
 
diff --git a/drivers/net/ethernet/marvell/mvmdio.c b/drivers/net/ethernet/marvell/mvmdio.c
index 77b7c80262f4..e2f662660313 100644
--- a/drivers/net/ethernet/marvell/mvmdio.c
+++ b/drivers/net/ethernet/marvell/mvmdio.c
@@ -24,10 +24,14 @@
 #include <linux/module.h>
 #include <linux/mutex.h>
 #include <linux/phy.h>
-#include <linux/of_address.h>
-#include <linux/of_mdio.h>
+#include <linux/interrupt.h>
 #include <linux/platform_device.h>
 #include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/of_mdio.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
 
 #define MVMDIO_SMI_DATA_SHIFT              0
 #define MVMDIO_SMI_PHY_ADDR_SHIFT          16
@@ -36,33 +40,59 @@
 #define MVMDIO_SMI_WRITE_OPERATION         0
 #define MVMDIO_SMI_READ_VALID              BIT(27)
 #define MVMDIO_SMI_BUSY                    BIT(28)
+#define MVMDIO_ERR_INT_CAUSE		   0x007C
+#define  MVMDIO_ERR_INT_SMI_DONE	   0x00000010
+#define MVMDIO_ERR_INT_MASK		   0x0080
 
 struct orion_mdio_dev {
 	struct mutex lock;
-	void __iomem *smireg;
+	void __iomem *regs;
+	struct clk *clk;
+	/*
+	 * If we have access to the error interrupt pin (which is
+	 * somewhat misnamed as it not only reflects internal errors
+	 * but also reflects SMI completion), use that to wait for
+	 * SMI access completion instead of polling the SMI busy bit.
+	 */
+	int err_interrupt;
+	wait_queue_head_t smi_busy_wait;
 };
 
+static int orion_mdio_smi_is_done(struct orion_mdio_dev *dev)
+{
+	return !(readl(dev->regs) & MVMDIO_SMI_BUSY);
+}
+
 /* Wait for the SMI unit to be ready for another operation
  */
 static int orion_mdio_wait_ready(struct mii_bus *bus)
 {
 	struct orion_mdio_dev *dev = bus->priv;
 	int count;
-	u32 val;
 
-	count = 0;
-	while (1) {
-		val = readl(dev->smireg);
-		if (!(val & MVMDIO_SMI_BUSY))
-			break;
+	if (dev->err_interrupt <= 0) {
+		count = 0;
+		while (1) {
+			if (orion_mdio_smi_is_done(dev))
+				break;
 
-		if (count > 100) {
-			dev_err(bus->parent, "Timeout: SMI busy for too long\n");
-			return -ETIMEDOUT;
-		}
+			if (count > 100) {
+				dev_err(bus->parent,
+					"Timeout: SMI busy for too long\n");
+				return -ETIMEDOUT;
+			}
 
-		udelay(10);
-		count++;
+			udelay(10);
+			count++;
+		}
+	} else {
+		if (!orion_mdio_smi_is_done(dev)) {
+			wait_event_timeout(dev->smi_busy_wait,
+				orion_mdio_smi_is_done(dev),
+				msecs_to_jiffies(100));
+			if (!orion_mdio_smi_is_done(dev))
+				return -ETIMEDOUT;
+		}
 	}
 
 	return 0;
@@ -87,12 +117,12 @@ static int orion_mdio_read(struct mii_bus *bus, int mii_id,
 	writel(((mii_id << MVMDIO_SMI_PHY_ADDR_SHIFT) |
 		(regnum << MVMDIO_SMI_PHY_REG_SHIFT)  |
 		MVMDIO_SMI_READ_OPERATION),
-	       dev->smireg);
+	       dev->regs);
 
 	/* Wait for the value to become available */
 	count = 0;
 	while (1) {
-		val = readl(dev->smireg);
+		val = readl(dev->regs);
 		if (val & MVMDIO_SMI_READ_VALID)
 			break;
 
@@ -129,7 +159,7 @@ static int orion_mdio_write(struct mii_bus *bus, int mii_id,
 		(regnum << MVMDIO_SMI_PHY_REG_SHIFT)  |
 		MVMDIO_SMI_WRITE_OPERATION            |
 		(value << MVMDIO_SMI_DATA_SHIFT)),
-	       dev->smireg);
+	       dev->regs);
 
 	mutex_unlock(&dev->lock);
 
@@ -141,13 +171,34 @@ static int orion_mdio_reset(struct mii_bus *bus)
 	return 0;
 }
 
+static irqreturn_t orion_mdio_err_irq(int irq, void *dev_id)
+{
+	struct orion_mdio_dev *dev = dev_id;
+
+	if (readl(dev->regs + MVMDIO_ERR_INT_CAUSE) &
+			MVMDIO_ERR_INT_SMI_DONE) {
+		writel(~MVMDIO_ERR_INT_SMI_DONE,
+				dev->regs + MVMDIO_ERR_INT_CAUSE);
+		wake_up(&dev->smi_busy_wait);
+		return IRQ_HANDLED;
+	}
+
+	return IRQ_NONE;
+}
+
 static int orion_mdio_probe(struct platform_device *pdev)
 {
-	struct device_node *np = pdev->dev.of_node;
+	struct resource *r;
 	struct mii_bus *bus;
 	struct orion_mdio_dev *dev;
 	int i, ret;
 
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "No SMI register address given\n");
+		return -ENODEV;
+	}
+
 	bus = mdiobus_alloc_size(sizeof(struct orion_mdio_dev));
 	if (!bus) {
 		dev_err(&pdev->dev, "Cannot allocate MDIO bus\n");
@@ -172,36 +223,66 @@ static int orion_mdio_probe(struct platform_device *pdev)
 		bus->irq[i] = PHY_POLL;
 
 	dev = bus->priv;
-	dev->smireg = of_iomap(pdev->dev.of_node, 0);
-	if (!dev->smireg) {
-		dev_err(&pdev->dev, "No SMI register address given in DT\n");
-		kfree(bus->irq);
-		mdiobus_free(bus);
-		return -ENODEV;
+	dev->regs = devm_ioremap(&pdev->dev, r->start, resource_size(r));
+	if (!dev->regs) {
+		dev_err(&pdev->dev, "Unable to remap SMI register\n");
+		ret = -ENODEV;
+		goto out_mdio;
+	}
+
+	init_waitqueue_head(&dev->smi_busy_wait);
+
+	dev->clk = devm_clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(dev->clk))
+		clk_prepare_enable(dev->clk);
+
+	dev->err_interrupt = platform_get_irq(pdev, 0);
+	if (dev->err_interrupt != -ENXIO) {
+		ret = devm_request_irq(&pdev->dev, dev->err_interrupt,
+					orion_mdio_err_irq,
+					IRQF_SHARED, pdev->name, dev);
+		if (ret)
+			goto out_mdio;
+
+		writel(MVMDIO_ERR_INT_SMI_DONE,
+			dev->regs + MVMDIO_ERR_INT_MASK);
 	}
 
 	mutex_init(&dev->lock);
 
-	ret = of_mdiobus_register(bus, np);
+	if (pdev->dev.of_node)
+		ret = of_mdiobus_register(bus, pdev->dev.of_node);
+	else
+		ret = mdiobus_register(bus);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Cannot register MDIO bus (%d)\n", ret);
-		iounmap(dev->smireg);
-		kfree(bus->irq);
-		mdiobus_free(bus);
-		return ret;
+		goto out_mdio;
 	}
 
 	platform_set_drvdata(pdev, bus);
 
 	return 0;
+
+out_mdio:
+	if (!IS_ERR(dev->clk))
+		clk_disable_unprepare(dev->clk);
+	kfree(bus->irq);
+	mdiobus_free(bus);
+	return ret;
 }
 
 static int orion_mdio_remove(struct platform_device *pdev)
 {
 	struct mii_bus *bus = platform_get_drvdata(pdev);
+	struct orion_mdio_dev *dev = bus->priv;
+
+	writel(0, dev->regs + MVMDIO_ERR_INT_MASK);
 	mdiobus_unregister(bus);
 	kfree(bus->irq);
 	mdiobus_free(bus);
+	if (!IS_ERR(dev->clk))
+		clk_disable_unprepare(dev->clk);
+
 	return 0;
 }
 
@@ -225,3 +306,4 @@ module_platform_driver(orion_mdio_driver);
 MODULE_DESCRIPTION("Marvell MDIO interface driver");
 MODULE_AUTHOR("Thomas Petazzoni <thomas.petazzoni@free-electrons.com>");
 MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:orion-mdio");
diff --git a/drivers/net/ethernet/marvell/mvneta.c b/drivers/net/ethernet/marvell/mvneta.c
index a47a097c21e1..c96678555233 100644
--- a/drivers/net/ethernet/marvell/mvneta.c
+++ b/drivers/net/ethernet/marvell/mvneta.c
@@ -1969,13 +1969,8 @@ static int mvneta_rxq_init(struct mvneta_port *pp,
 	rxq->descs = dma_alloc_coherent(pp->dev->dev.parent,
 					rxq->size * MVNETA_DESC_ALIGNED_SIZE,
 					&rxq->descs_phys, GFP_KERNEL);
-	if (rxq->descs == NULL) {
-		netdev_err(pp->dev,
-			   "rxq=%d: Can't allocate %d bytes for %d RX descr\n",
-			   rxq->id, rxq->size * MVNETA_DESC_ALIGNED_SIZE,
-			   rxq->size);
+	if (rxq->descs == NULL)
 		return -ENOMEM;
-	}
 
 	BUG_ON(rxq->descs !=
 	       PTR_ALIGN(rxq->descs, MVNETA_CPU_D_CACHE_LINE_SIZE));
@@ -2029,13 +2024,8 @@ static int mvneta_txq_init(struct mvneta_port *pp,
 	txq->descs = dma_alloc_coherent(pp->dev->dev.parent,
 					txq->size * MVNETA_DESC_ALIGNED_SIZE,
 					&txq->descs_phys, GFP_KERNEL);
-	if (txq->descs == NULL) {
-		netdev_err(pp->dev,
-			   "txQ=%d: Can't allocate %d bytes for %d TX descr\n",
-			   txq->id, txq->size * MVNETA_DESC_ALIGNED_SIZE,
-			   txq->size);
+	if (txq->descs == NULL)
 		return -ENOMEM;
-	}
 
 	/* Make sure descriptor address is cache line size aligned  */
 	BUG_ON(txq->descs !=
diff --git a/drivers/net/ethernet/marvell/pxa168_eth.c b/drivers/net/ethernet/marvell/pxa168_eth.c
index 037ed866c22f..339bb323cb0c 100644
--- a/drivers/net/ethernet/marvell/pxa168_eth.c
+++ b/drivers/net/ethernet/marvell/pxa168_eth.c
@@ -584,12 +584,14 @@ static int init_hash_table(struct pxa168_eth_private *pep)
 	 */
 	if (pep->htpr == NULL) {
 		pep->htpr = dma_alloc_coherent(pep->dev->dev.parent,
-					      HASH_ADDR_TABLE_SIZE,
-					      &pep->htpr_dma, GFP_KERNEL);
+					       HASH_ADDR_TABLE_SIZE,
+					       &pep->htpr_dma,
+					       GFP_KERNEL | __GFP_ZERO);
 		if (pep->htpr == NULL)
 			return -ENOMEM;
+	} else {
+		memset(pep->htpr, 0, HASH_ADDR_TABLE_SIZE);
 	}
-	memset(pep->htpr, 0, HASH_ADDR_TABLE_SIZE);
 	wrl(pep, HTPR, pep->htpr_dma);
 	return 0;
 }
@@ -1023,13 +1025,11 @@ static int rxq_init(struct net_device *dev)
 	size = pep->rx_ring_size * sizeof(struct rx_desc);
 	pep->rx_desc_area_size = size;
 	pep->p_rx_desc_area = dma_alloc_coherent(pep->dev->dev.parent, size,
-						&pep->rx_desc_dma, GFP_KERNEL);
-	if (!pep->p_rx_desc_area) {
-		printk(KERN_ERR "%s: Cannot alloc RX ring (size %d bytes)\n",
-		       dev->name, size);
+						 &pep->rx_desc_dma,
+						 GFP_KERNEL | __GFP_ZERO);
+	if (!pep->p_rx_desc_area)
 		goto out;
-	}
-	memset((void *)pep->p_rx_desc_area, 0, size);
+
 	/* initialize the next_desc_ptr links in the Rx descriptors ring */
 	p_rx_desc = pep->p_rx_desc_area;
 	for (i = 0; i < rx_desc_num; i++) {
@@ -1086,13 +1086,10 @@ static int txq_init(struct net_device *dev)
 	size = pep->tx_ring_size * sizeof(struct tx_desc);
 	pep->tx_desc_area_size = size;
 	pep->p_tx_desc_area = dma_alloc_coherent(pep->dev->dev.parent, size,
-						&pep->tx_desc_dma, GFP_KERNEL);
-	if (!pep->p_tx_desc_area) {
-		printk(KERN_ERR "%s: Cannot allocate Tx Ring (size %d bytes)\n",
-		       dev->name, size);
+						 &pep->tx_desc_dma,
+						 GFP_KERNEL | __GFP_ZERO);
+	if (!pep->p_tx_desc_area)
 		goto out;
-	}
-	memset((void *)pep->p_tx_desc_area, 0, pep->tx_desc_area_size);
 	/* Initialize the next_desc_ptr links in the Tx descriptors ring */
 	p_tx_desc = pep->p_tx_desc_area;
 	for (i = 0; i < tx_desc_num; i++) {
diff --git a/drivers/net/ethernet/marvell/sky2.c b/drivers/net/ethernet/marvell/sky2.c
index 6a0e671fcecd..d175bbd3ffd3 100644
--- a/drivers/net/ethernet/marvell/sky2.c
+++ b/drivers/net/ethernet/marvell/sky2.c
@@ -1421,14 +1421,14 @@ static void sky2_vlan_mode(struct net_device *dev, netdev_features_t features)
 	struct sky2_hw *hw = sky2->hw;
 	u16 port = sky2->port;
 
-	if (features & NETIF_F_HW_VLAN_RX)
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
 		sky2_write32(hw, SK_REG(port, RX_GMF_CTRL_T),
 			     RX_VLAN_STRIP_ON);
 	else
 		sky2_write32(hw, SK_REG(port, RX_GMF_CTRL_T),
 			     RX_VLAN_STRIP_OFF);
 
-	if (features & NETIF_F_HW_VLAN_TX) {
+	if (features & NETIF_F_HW_VLAN_CTAG_TX) {
 		sky2_write32(hw, SK_REG(port, TX_GMF_CTRL_T),
 			     TX_VLAN_TAG_ON);
 
@@ -2496,10 +2496,12 @@ static struct sk_buff *receive_copy(struct sky2_port *sky2,
 		skb->ip_summed = re->skb->ip_summed;
 		skb->csum = re->skb->csum;
 		skb->rxhash = re->skb->rxhash;
+		skb->vlan_proto = re->skb->vlan_proto;
 		skb->vlan_tci = re->skb->vlan_tci;
 
 		pci_dma_sync_single_for_device(sky2->hw->pdev, re->data_addr,
 					       length, PCI_DMA_FROMDEVICE);
+		re->skb->vlan_proto = 0;
 		re->skb->vlan_tci = 0;
 		re->skb->rxhash = 0;
 		re->skb->ip_summed = CHECKSUM_NONE;
@@ -2713,7 +2715,7 @@ static void sky2_rx_tag(struct sky2_port *sky2, u16 length)
 	struct sk_buff *skb;
 
 	skb = sky2->rx_ring[sky2->rx_next].skb;
-	__vlan_hwaccel_put_tag(skb, be16_to_cpu(length));
+	__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), be16_to_cpu(length));
 }
 
 static void sky2_rx_hash(struct sky2_port *sky2, u32 status)
@@ -4406,7 +4408,7 @@ static int sky2_set_features(struct net_device *dev, netdev_features_t features)
 	if (changed & NETIF_F_RXHASH)
 		rx_set_rss(dev, features);
 
-	if (changed & (NETIF_F_HW_VLAN_TX|NETIF_F_HW_VLAN_RX))
+	if (changed & (NETIF_F_HW_VLAN_CTAG_TX|NETIF_F_HW_VLAN_CTAG_RX))
 		sky2_vlan_mode(dev, features);
 
 	return 0;
@@ -4793,7 +4795,8 @@ static struct net_device *sky2_init_netdev(struct sky2_hw *hw, unsigned port,
 		dev->hw_features |= NETIF_F_RXHASH;
 
 	if (!(hw->flags & SKY2_HW_VLAN_BROKEN)) {
-		dev->hw_features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
+		dev->hw_features |= NETIF_F_HW_VLAN_CTAG_TX |
+				    NETIF_F_HW_VLAN_CTAG_RX;
 		dev->vlan_features |= SKY2_VLAN_OFFLOADS;
 	}
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/Makefile b/drivers/net/ethernet/mellanox/mlx4/Makefile
index 293127d28b33..3e9c70f15b42 100644
--- a/drivers/net/ethernet/mellanox/mlx4/Makefile
+++ b/drivers/net/ethernet/mellanox/mlx4/Makefile
@@ -6,5 +6,5 @@ mlx4_core-y :=	alloc.o catas.o cmd.o cq.o eq.o fw.o icm.o intf.o main.o mcg.o \
 obj-$(CONFIG_MLX4_EN)               += mlx4_en.o
 
 mlx4_en-y := 	en_main.o en_tx.o en_rx.o en_ethtool.o en_port.o en_cq.o \
-		en_resources.o en_netdev.o en_selftest.o
+		en_resources.o en_netdev.o en_selftest.o en_clock.o
 mlx4_en-$(CONFIG_MLX4_EN_DCB) += en_dcb_nl.o
diff --git a/drivers/net/ethernet/mellanox/mlx4/cmd.c b/drivers/net/ethernet/mellanox/mlx4/cmd.c
index fdc5f23d8e9f..1df56cc50ee9 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cmd.c
@@ -1490,6 +1490,69 @@ out:
 	return ret;
 }
 
+static int mlx4_master_activate_admin_state(struct mlx4_priv *priv, int slave)
+{
+	int port, err;
+	struct mlx4_vport_state *vp_admin;
+	struct mlx4_vport_oper_state *vp_oper;
+
+	for (port = 1; port <= MLX4_MAX_PORTS; port++) {
+		vp_oper = &priv->mfunc.master.vf_oper[slave].vport[port];
+		vp_admin = &priv->mfunc.master.vf_admin[slave].vport[port];
+		vp_oper->state = *vp_admin;
+		if (MLX4_VGT != vp_admin->default_vlan) {
+			err = __mlx4_register_vlan(&priv->dev, port,
+						   vp_admin->default_vlan, &(vp_oper->vlan_idx));
+			if (err) {
+				vp_oper->vlan_idx = NO_INDX;
+				mlx4_warn((&priv->dev),
+					  "No vlan resorces slave %d, port %d\n",
+					  slave, port);
+				return err;
+			}
+			mlx4_dbg((&(priv->dev)), "alloc vlan %d idx  %d slave %d port %d\n",
+				 (int)(vp_oper->state.default_vlan),
+				 vp_oper->vlan_idx, slave, port);
+		}
+		if (vp_admin->spoofchk) {
+			vp_oper->mac_idx = __mlx4_register_mac(&priv->dev,
+							       port,
+							       vp_admin->mac);
+			if (0 > vp_oper->mac_idx) {
+				err = vp_oper->mac_idx;
+				vp_oper->mac_idx = NO_INDX;
+				mlx4_warn((&priv->dev),
+					  "No mac resorces slave %d, port %d\n",
+					  slave, port);
+				return err;
+			}
+			mlx4_dbg((&(priv->dev)), "alloc mac %llx idx  %d slave %d port %d\n",
+				 vp_oper->state.mac, vp_oper->mac_idx, slave, port);
+		}
+	}
+	return 0;
+}
+
+static void mlx4_master_deactivate_admin_state(struct mlx4_priv *priv, int slave)
+{
+	int port;
+	struct mlx4_vport_oper_state *vp_oper;
+
+	for (port = 1; port <= MLX4_MAX_PORTS; port++) {
+		vp_oper = &priv->mfunc.master.vf_oper[slave].vport[port];
+		if (NO_INDX != vp_oper->vlan_idx) {
+			__mlx4_unregister_vlan(&priv->dev,
+					       port, vp_oper->vlan_idx);
+			vp_oper->vlan_idx = NO_INDX;
+		}
+		if (NO_INDX != vp_oper->mac_idx) {
+			__mlx4_unregister_mac(&priv->dev, port, vp_oper->mac_idx);
+			vp_oper->mac_idx = NO_INDX;
+		}
+	}
+	return;
+}
+
 static void mlx4_master_do_cmd(struct mlx4_dev *dev, int slave, u8 cmd,
 			       u16 param, u8 toggle)
 {
@@ -1510,6 +1573,7 @@ static void mlx4_master_do_cmd(struct mlx4_dev *dev, int slave, u8 cmd,
 	if (cmd == MLX4_COMM_CMD_RESET) {
 		mlx4_warn(dev, "Received reset from slave:%d\n", slave);
 		slave_state[slave].active = false;
+		mlx4_master_deactivate_admin_state(priv, slave);
 		for (i = 0; i < MLX4_EVENT_TYPES_NUM; ++i) {
 				slave_state[slave].event_eq[i].eqn = -1;
 				slave_state[slave].event_eq[i].token = 0;
@@ -1556,6 +1620,8 @@ static void mlx4_master_do_cmd(struct mlx4_dev *dev, int slave, u8 cmd,
 		if (slave_state[slave].last_cmd != MLX4_COMM_CMD_VHCR2)
 			goto reset_slave;
 		slave_state[slave].vhcr_dma |= param;
+		if (mlx4_master_activate_admin_state(priv, slave))
+				goto reset_slave;
 		slave_state[slave].active = true;
 		mlx4_dispatch_event(dev, MLX4_DEV_EVENT_SLAVE_INIT, slave);
 		break;
@@ -1732,6 +1798,18 @@ int mlx4_multi_func_init(struct mlx4_dev *dev)
 		if (!priv->mfunc.master.slave_state)
 			goto err_comm;
 
+		priv->mfunc.master.vf_admin =
+			kzalloc(dev->num_slaves *
+				sizeof(struct mlx4_vf_admin_state), GFP_KERNEL);
+		if (!priv->mfunc.master.vf_admin)
+			goto err_comm_admin;
+
+		priv->mfunc.master.vf_oper =
+			kzalloc(dev->num_slaves *
+				sizeof(struct mlx4_vf_oper_state), GFP_KERNEL);
+		if (!priv->mfunc.master.vf_oper)
+			goto err_comm_oper;
+
 		for (i = 0; i < dev->num_slaves; ++i) {
 			s_state = &priv->mfunc.master.slave_state[i];
 			s_state->last_cmd = MLX4_COMM_CMD_RESET;
@@ -1752,6 +1830,10 @@ int mlx4_multi_func_init(struct mlx4_dev *dev)
 					goto err_slaves;
 				}
 				INIT_LIST_HEAD(&s_state->mcast_filters[port]);
+				priv->mfunc.master.vf_admin[i].vport[port].default_vlan = MLX4_VGT;
+				priv->mfunc.master.vf_oper[i].vport[port].state.default_vlan = MLX4_VGT;
+				priv->mfunc.master.vf_oper[i].vport[port].vlan_idx = NO_INDX;
+				priv->mfunc.master.vf_oper[i].vport[port].mac_idx = NO_INDX;
 			}
 			spin_lock_init(&s_state->lock);
 		}
@@ -1800,6 +1882,10 @@ err_slaves:
 		for (port = 1; port <= MLX4_MAX_PORTS; port++)
 			kfree(priv->mfunc.master.slave_state[i].vlan_filter[port]);
 	}
+	kfree(priv->mfunc.master.vf_oper);
+err_comm_oper:
+	kfree(priv->mfunc.master.vf_admin);
+err_comm_admin:
 	kfree(priv->mfunc.master.slave_state);
 err_comm:
 	iounmap(priv->mfunc.comm);
@@ -1837,10 +1923,8 @@ int mlx4_cmd_init(struct mlx4_dev *dev)
 		priv->mfunc.vhcr = dma_alloc_coherent(&(dev->pdev->dev), PAGE_SIZE,
 						      &priv->mfunc.vhcr_dma,
 						      GFP_KERNEL);
-		if (!priv->mfunc.vhcr) {
-			mlx4_err(dev, "Couldn't allocate VHCR.\n");
+		if (!priv->mfunc.vhcr)
 			goto err_hcr;
-		}
 	}
 
 	priv->cmd.pool = pci_pool_create("mlx4_cmd", dev->pdev,
@@ -1876,6 +1960,8 @@ void mlx4_multi_func_cleanup(struct mlx4_dev *dev)
 				kfree(priv->mfunc.master.slave_state[i].vlan_filter[port]);
 		}
 		kfree(priv->mfunc.master.slave_state);
+		kfree(priv->mfunc.master.vf_admin);
+		kfree(priv->mfunc.master.vf_oper);
 	}
 
 	iounmap(priv->mfunc.comm);
@@ -1986,3 +2072,115 @@ u32 mlx4_comm_get_version(void)
 {
 	 return ((u32) CMD_CHAN_IF_REV << 8) | (u32) CMD_CHAN_VER;
 }
+
+static int mlx4_get_slave_indx(struct mlx4_dev *dev, int vf)
+{
+	if ((vf < 0) || (vf >= dev->num_vfs)) {
+		mlx4_err(dev, "Bad vf number:%d (number of activated vf: %d)\n", vf, dev->num_vfs);
+		return -EINVAL;
+	}
+
+	return vf+1;
+}
+
+int mlx4_set_vf_mac(struct mlx4_dev *dev, int port, int vf, u64 mac)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+	struct mlx4_vport_state *s_info;
+	int slave;
+
+	if (!mlx4_is_master(dev))
+		return -EPROTONOSUPPORT;
+
+	slave = mlx4_get_slave_indx(dev, vf);
+	if (slave < 0)
+		return -EINVAL;
+
+	s_info = &priv->mfunc.master.vf_admin[slave].vport[port];
+	s_info->mac = mac;
+	mlx4_info(dev, "default mac on vf %d port %d to %llX will take afect only after vf restart\n",
+		  vf, port, s_info->mac);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mlx4_set_vf_mac);
+
+int mlx4_set_vf_vlan(struct mlx4_dev *dev, int port, int vf, u16 vlan, u8 qos)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+	struct mlx4_vport_state *s_info;
+	int slave;
+
+	if ((!mlx4_is_master(dev)) ||
+	    !(dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_VLAN_CONTROL))
+		return -EPROTONOSUPPORT;
+
+	if ((vlan > 4095) || (qos > 7))
+		return -EINVAL;
+
+	slave = mlx4_get_slave_indx(dev, vf);
+	if (slave < 0)
+		return -EINVAL;
+
+	s_info = &priv->mfunc.master.vf_admin[slave].vport[port];
+	if ((0 == vlan) && (0 == qos))
+		s_info->default_vlan = MLX4_VGT;
+	else
+		s_info->default_vlan = vlan;
+	s_info->default_qos = qos;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mlx4_set_vf_vlan);
+
+int mlx4_set_vf_spoofchk(struct mlx4_dev *dev, int port, int vf, bool setting)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+	struct mlx4_vport_state *s_info;
+	int slave;
+
+	if ((!mlx4_is_master(dev)) ||
+	    !(dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_FSM))
+		return -EPROTONOSUPPORT;
+
+	slave = mlx4_get_slave_indx(dev, vf);
+	if (slave < 0)
+		return -EINVAL;
+
+	s_info = &priv->mfunc.master.vf_admin[slave].vport[port];
+	s_info->spoofchk = setting;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mlx4_set_vf_spoofchk);
+
+int mlx4_get_vf_config(struct mlx4_dev *dev, int port, int vf, struct ifla_vf_info *ivf)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+	struct mlx4_vport_state *s_info;
+	int slave;
+
+	if (!mlx4_is_master(dev))
+		return -EPROTONOSUPPORT;
+
+	slave = mlx4_get_slave_indx(dev, vf);
+	if (slave < 0)
+		return -EINVAL;
+
+	s_info = &priv->mfunc.master.vf_admin[slave].vport[port];
+	ivf->vf = vf;
+
+	/* need to convert it to a func */
+	ivf->mac[0] = ((s_info->mac >> (5*8)) & 0xff);
+	ivf->mac[1] = ((s_info->mac >> (4*8)) & 0xff);
+	ivf->mac[2] = ((s_info->mac >> (3*8)) & 0xff);
+	ivf->mac[3] = ((s_info->mac >> (2*8)) & 0xff);
+	ivf->mac[4] = ((s_info->mac >> (1*8)) & 0xff);
+	ivf->mac[5] = ((s_info->mac)  & 0xff);
+
+	ivf->vlan	= s_info->default_vlan;
+	ivf->qos	= s_info->default_qos;
+	ivf->tx_rate	= s_info->tx_rate;
+	ivf->spoofchk	= s_info->spoofchk;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mlx4_get_vf_config);
diff --git a/drivers/net/ethernet/mellanox/mlx4/cq.c b/drivers/net/ethernet/mellanox/mlx4/cq.c
index 0706623cfb96..004e4231af67 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cq.c
@@ -240,9 +240,10 @@ static void mlx4_cq_free_icm(struct mlx4_dev *dev, int cqn)
 		__mlx4_cq_free_icm(dev, cqn);
 }
 
-int mlx4_cq_alloc(struct mlx4_dev *dev, int nent, struct mlx4_mtt *mtt,
-		  struct mlx4_uar *uar, u64 db_rec, struct mlx4_cq *cq,
-		  unsigned vector, int collapsed)
+int mlx4_cq_alloc(struct mlx4_dev *dev, int nent,
+		  struct mlx4_mtt *mtt, struct mlx4_uar *uar, u64 db_rec,
+		  struct mlx4_cq *cq, unsigned vector, int collapsed,
+		  int timestamp_en)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	struct mlx4_cq_table *cq_table = &priv->cq_table;
@@ -276,6 +277,9 @@ int mlx4_cq_alloc(struct mlx4_dev *dev, int nent, struct mlx4_mtt *mtt,
 	memset(cq_context, 0, sizeof *cq_context);
 
 	cq_context->flags	    = cpu_to_be32(!!collapsed << 18);
+	if (timestamp_en)
+		cq_context->flags  |= cpu_to_be32(1 << 19);
+
 	cq_context->logsize_usrpage = cpu_to_be32((ilog2(nent) << 24) | uar->index);
 	cq_context->comp_eqn	    = priv->eq_table.eq[vector].eqn;
 	cq_context->log_page_size   = mtt->page_shift - MLX4_ICM_PAGE_SHIFT;
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_clock.c b/drivers/net/ethernet/mellanox/mlx4/en_clock.c
new file mode 100644
index 000000000000..fd6441071319
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx4/en_clock.c
@@ -0,0 +1,151 @@
+/*
+ * Copyright (c) 2012 Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#include <linux/mlx4/device.h>
+
+#include "mlx4_en.h"
+
+int mlx4_en_timestamp_config(struct net_device *dev, int tx_type, int rx_filter)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = priv->mdev;
+	int port_up = 0;
+	int err = 0;
+
+	mutex_lock(&mdev->state_lock);
+	if (priv->port_up) {
+		port_up = 1;
+		mlx4_en_stop_port(dev, 1);
+	}
+
+	mlx4_en_free_resources(priv);
+
+	en_warn(priv, "Changing Time Stamp configuration\n");
+
+	priv->hwtstamp_config.tx_type = tx_type;
+	priv->hwtstamp_config.rx_filter = rx_filter;
+
+	if (rx_filter != HWTSTAMP_FILTER_NONE)
+		dev->features &= ~NETIF_F_HW_VLAN_CTAG_RX;
+	else
+		dev->features |= NETIF_F_HW_VLAN_CTAG_RX;
+
+	err = mlx4_en_alloc_resources(priv);
+	if (err) {
+		en_err(priv, "Failed reallocating port resources\n");
+		goto out;
+	}
+	if (port_up) {
+		err = mlx4_en_start_port(dev);
+		if (err)
+			en_err(priv, "Failed starting port\n");
+	}
+
+out:
+	mutex_unlock(&mdev->state_lock);
+	netdev_features_change(dev);
+	return err;
+}
+
+/* mlx4_en_read_clock - read raw cycle counter (to be used by time counter)
+ */
+static cycle_t mlx4_en_read_clock(const struct cyclecounter *tc)
+{
+	struct mlx4_en_dev *mdev =
+		container_of(tc, struct mlx4_en_dev, cycles);
+	struct mlx4_dev *dev = mdev->dev;
+
+	return mlx4_read_clock(dev) & tc->mask;
+}
+
+u64 mlx4_en_get_cqe_ts(struct mlx4_cqe *cqe)
+{
+	u64 hi, lo;
+	struct mlx4_ts_cqe *ts_cqe = (struct mlx4_ts_cqe *)cqe;
+
+	lo = (u64)be16_to_cpu(ts_cqe->timestamp_lo);
+	hi = ((u64)be32_to_cpu(ts_cqe->timestamp_hi) + !lo) << 16;
+
+	return hi | lo;
+}
+
+void mlx4_en_fill_hwtstamps(struct mlx4_en_dev *mdev,
+			    struct skb_shared_hwtstamps *hwts,
+			    u64 timestamp)
+{
+	u64 nsec;
+
+	nsec = timecounter_cyc2time(&mdev->clock, timestamp);
+
+	memset(hwts, 0, sizeof(struct skb_shared_hwtstamps));
+	hwts->hwtstamp = ns_to_ktime(nsec);
+}
+
+void mlx4_en_init_timestamp(struct mlx4_en_dev *mdev)
+{
+	struct mlx4_dev *dev = mdev->dev;
+	u64 ns;
+
+	memset(&mdev->cycles, 0, sizeof(mdev->cycles));
+	mdev->cycles.read = mlx4_en_read_clock;
+	mdev->cycles.mask = CLOCKSOURCE_MASK(48);
+	/* Using shift to make calculation more accurate. Since current HW
+	 * clock frequency is 427 MHz, and cycles are given using a 48 bits
+	 * register, the biggest shift when calculating using u64, is 14
+	 * (max_cycles * multiplier < 2^64)
+	 */
+	mdev->cycles.shift = 14;
+	mdev->cycles.mult =
+		clocksource_khz2mult(1000 * dev->caps.hca_core_clock, mdev->cycles.shift);
+
+	timecounter_init(&mdev->clock, &mdev->cycles,
+			 ktime_to_ns(ktime_get_real()));
+
+	/* Calculate period in seconds to call the overflow watchdog - to make
+	 * sure counter is checked at least once every wrap around.
+	 */
+	ns = cyclecounter_cyc2ns(&mdev->cycles, mdev->cycles.mask);
+	do_div(ns, NSEC_PER_SEC / 2 / HZ);
+	mdev->overflow_period = ns;
+}
+
+void mlx4_en_ptp_overflow_check(struct mlx4_en_dev *mdev)
+{
+	bool timeout = time_is_before_jiffies(mdev->last_overflow_check +
+					      mdev->overflow_period);
+
+	if (timeout) {
+		timecounter_read(&mdev->clock);
+		mdev->last_overflow_check = jiffies;
+	}
+}
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_cq.c b/drivers/net/ethernet/mellanox/mlx4/en_cq.c
index b8d0854a7ad1..1e6c594d6d04 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_cq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_cq.c
@@ -77,6 +77,7 @@ int mlx4_en_activate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq,
 	struct mlx4_en_dev *mdev = priv->mdev;
 	int err = 0;
 	char name[25];
+	int timestamp_en = 0;
 	struct cpu_rmap *rmap =
 #ifdef CONFIG_RFS_ACCEL
 		priv->dev->rx_cpu_rmap;
@@ -123,8 +124,13 @@ int mlx4_en_activate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq,
 	if (!cq->is_tx)
 		cq->size = priv->rx_ring[cq->ring].actual_size;
 
-	err = mlx4_cq_alloc(mdev->dev, cq->size, &cq->wqres.mtt, &mdev->priv_uar,
-			    cq->wqres.db.dma, &cq->mcq, cq->vector, 0);
+	if ((cq->is_tx && priv->hwtstamp_config.tx_type) ||
+	    (!cq->is_tx && priv->hwtstamp_config.rx_filter))
+		timestamp_en = 1;
+
+	err = mlx4_cq_alloc(mdev->dev, cq->size, &cq->wqres.mtt,
+			    &mdev->priv_uar, cq->wqres.db.dma, &cq->mcq,
+			    cq->vector, 0, timestamp_en);
 	if (err)
 		return err;
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_dcb_nl.c b/drivers/net/ethernet/mellanox/mlx4/en_dcb_nl.c
index b799ab12a291..0f91222ea3d7 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_dcb_nl.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_dcb_nl.c
@@ -186,7 +186,7 @@ static int mlx4_en_dcbnl_ieee_setpfc(struct net_device *dev,
 
 static u8 mlx4_en_dcbnl_getdcbx(struct net_device *dev)
 {
-	return DCB_CAP_DCBX_VER_IEEE;
+	return DCB_CAP_DCBX_HOST | DCB_CAP_DCBX_VER_IEEE;
 }
 
 static u8 mlx4_en_dcbnl_setdcbx(struct net_device *dev, u8 mode)
@@ -253,3 +253,11 @@ const struct dcbnl_rtnl_ops mlx4_en_dcbnl_ops = {
 	.getdcbx	= mlx4_en_dcbnl_getdcbx,
 	.setdcbx	= mlx4_en_dcbnl_setdcbx,
 };
+
+const struct dcbnl_rtnl_ops mlx4_en_dcbnl_pfc_ops = {
+	.ieee_getpfc	= mlx4_en_dcbnl_ieee_getpfc,
+	.ieee_setpfc	= mlx4_en_dcbnl_ieee_setpfc,
+
+	.getdcbx	= mlx4_en_dcbnl_getdcbx,
+	.setdcbx	= mlx4_en_dcbnl_setdcbx,
+};
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c b/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
index 00f25b5f297f..c9e6b62dd000 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
@@ -889,7 +889,7 @@ static int mlx4_en_flow_replace(struct net_device *dev,
 		.queue_mode = MLX4_NET_TRANS_Q_FIFO,
 		.exclusive = 0,
 		.allow_loopback = 1,
-		.promisc_mode = MLX4_FS_PROMISC_NONE,
+		.promisc_mode = MLX4_FS_REGULAR,
 	};
 
 	rule.port = priv->port;
@@ -1147,6 +1147,35 @@ out:
 	return err;
 }
 
+static int mlx4_en_get_ts_info(struct net_device *dev,
+			       struct ethtool_ts_info *info)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = priv->mdev;
+	int ret;
+
+	ret = ethtool_op_get_ts_info(dev, info);
+	if (ret)
+		return ret;
+
+	if (mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_TS) {
+		info->so_timestamping |=
+			SOF_TIMESTAMPING_TX_HARDWARE |
+			SOF_TIMESTAMPING_RX_HARDWARE |
+			SOF_TIMESTAMPING_RAW_HARDWARE;
+
+		info->tx_types =
+			(1 << HWTSTAMP_TX_OFF) |
+			(1 << HWTSTAMP_TX_ON);
+
+		info->rx_filters =
+			(1 << HWTSTAMP_FILTER_NONE) |
+			(1 << HWTSTAMP_FILTER_ALL);
+	}
+
+	return ret;
+}
+
 const struct ethtool_ops mlx4_en_ethtool_ops = {
 	.get_drvinfo = mlx4_en_get_drvinfo,
 	.get_settings = mlx4_en_get_settings,
@@ -1173,6 +1202,7 @@ const struct ethtool_ops mlx4_en_ethtool_ops = {
 	.set_rxfh_indir = mlx4_en_set_rxfh_indir,
 	.get_channels = mlx4_en_get_channels,
 	.set_channels = mlx4_en_set_channels,
+	.get_ts_info = mlx4_en_get_ts_info,
 };
 
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_main.c b/drivers/net/ethernet/mellanox/mlx4/en_main.c
index fc27800e9c38..a5c9df07a7d0 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_main.c
@@ -300,6 +300,11 @@ static void *mlx4_en_add(struct mlx4_dev *dev)
 		if (mlx4_en_init_netdev(mdev, i, &mdev->profile.prof[i]))
 			mdev->pndev[i] = NULL;
 	}
+
+	/* Initialize time stamp mechanism */
+	if (mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_TS)
+		mlx4_en_init_timestamp(mdev);
+
 	return mdev;
 
 err_mr:
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
index 30d78f806dc3..b35f94700093 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
@@ -127,7 +127,7 @@ static void mlx4_en_filter_work(struct work_struct *work)
 		.queue_mode = MLX4_NET_TRANS_Q_LIFO,
 		.exclusive = 1,
 		.allow_loopback = 1,
-		.promisc_mode = MLX4_FS_PROMISC_NONE,
+		.promisc_mode = MLX4_FS_REGULAR,
 		.port = priv->port,
 		.priority = MLX4_DOMAIN_RFS,
 	};
@@ -356,7 +356,8 @@ static void mlx4_en_filter_rfs_expire(struct mlx4_en_priv *priv)
 }
 #endif
 
-static int mlx4_en_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
+static int mlx4_en_vlan_rx_add_vid(struct net_device *dev,
+				   __be16 proto, u16 vid)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 	struct mlx4_en_dev *mdev = priv->mdev;
@@ -381,7 +382,8 @@ static int mlx4_en_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
 	return 0;
 }
 
-static int mlx4_en_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
+static int mlx4_en_vlan_rx_kill_vid(struct net_device *dev,
+				    __be16 proto, u16 vid)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 	struct mlx4_en_dev *mdev = priv->mdev;
@@ -446,7 +448,7 @@ static int mlx4_en_uc_steer_add(struct mlx4_en_priv *priv,
 			.queue_mode = MLX4_NET_TRANS_Q_FIFO,
 			.exclusive = 0,
 			.allow_loopback = 1,
-			.promisc_mode = MLX4_FS_PROMISC_NONE,
+			.promisc_mode = MLX4_FS_REGULAR,
 			.priority = MLX4_DOMAIN_NIC,
 		};
 
@@ -793,7 +795,7 @@ static void mlx4_en_set_promisc_mode(struct mlx4_en_priv *priv,
 			err = mlx4_flow_steer_promisc_add(mdev->dev,
 							  priv->port,
 							  priv->base_qpn,
-							  MLX4_FS_PROMISC_UPLINK);
+							  MLX4_FS_ALL_DEFAULT);
 			if (err)
 				en_err(priv, "Failed enabling promiscuous mode\n");
 			priv->flags |= MLX4_EN_FLAG_MC_PROMISC;
@@ -856,7 +858,7 @@ static void mlx4_en_clear_promisc_mode(struct mlx4_en_priv *priv,
 	case MLX4_STEERING_MODE_DEVICE_MANAGED:
 		err = mlx4_flow_steer_promisc_remove(mdev->dev,
 						     priv->port,
-						     MLX4_FS_PROMISC_UPLINK);
+						     MLX4_FS_ALL_DEFAULT);
 		if (err)
 			en_err(priv, "Failed disabling promiscuous mode\n");
 		priv->flags &= ~MLX4_EN_FLAG_MC_PROMISC;
@@ -917,7 +919,7 @@ static void mlx4_en_do_multicast(struct mlx4_en_priv *priv,
 				err = mlx4_flow_steer_promisc_add(mdev->dev,
 								  priv->port,
 								  priv->base_qpn,
-								  MLX4_FS_PROMISC_ALL_MULTI);
+								  MLX4_FS_MC_DEFAULT);
 				break;
 
 			case MLX4_STEERING_MODE_B0:
@@ -940,7 +942,7 @@ static void mlx4_en_do_multicast(struct mlx4_en_priv *priv,
 			case MLX4_STEERING_MODE_DEVICE_MANAGED:
 				err = mlx4_flow_steer_promisc_remove(mdev->dev,
 								     priv->port,
-								     MLX4_FS_PROMISC_ALL_MULTI);
+								     MLX4_FS_MC_DEFAULT);
 				break;
 
 			case MLX4_STEERING_MODE_B0:
@@ -1359,6 +1361,27 @@ static void mlx4_en_do_get_stats(struct work_struct *work)
 	mutex_unlock(&mdev->state_lock);
 }
 
+/* mlx4_en_service_task - Run service task for tasks that needed to be done
+ * periodically
+ */
+static void mlx4_en_service_task(struct work_struct *work)
+{
+	struct delayed_work *delay = to_delayed_work(work);
+	struct mlx4_en_priv *priv = container_of(delay, struct mlx4_en_priv,
+						 service_task);
+	struct mlx4_en_dev *mdev = priv->mdev;
+
+	mutex_lock(&mdev->state_lock);
+	if (mdev->device_up) {
+		if (mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_TS)
+			mlx4_en_ptp_overflow_check(mdev);
+
+		queue_delayed_work(mdev->workqueue, &priv->service_task,
+				   SERVICE_TASK_DELAY);
+	}
+	mutex_unlock(&mdev->state_lock);
+}
+
 static void mlx4_en_linkstate(struct work_struct *work)
 {
 	struct mlx4_en_priv *priv = container_of(work, struct mlx4_en_priv,
@@ -1598,10 +1621,10 @@ void mlx4_en_stop_port(struct net_device *dev, int detach)
 				 MLX4_EN_FLAG_MC_PROMISC);
 		mlx4_flow_steer_promisc_remove(mdev->dev,
 					       priv->port,
-					       MLX4_FS_PROMISC_UPLINK);
+					       MLX4_FS_ALL_DEFAULT);
 		mlx4_flow_steer_promisc_remove(mdev->dev,
 					       priv->port,
-					       MLX4_FS_PROMISC_ALL_MULTI);
+					       MLX4_FS_MC_DEFAULT);
 	} else if (priv->flags & MLX4_EN_FLAG_PROMISC) {
 		priv->flags &= ~MLX4_EN_FLAG_PROMISC;
 
@@ -1863,6 +1886,7 @@ void mlx4_en_destroy_netdev(struct net_device *dev)
 		mlx4_free_hwq_res(mdev->dev, &priv->res, MLX4_EN_PAGE_SIZE);
 
 	cancel_delayed_work(&priv->stats_task);
+	cancel_delayed_work(&priv->service_task);
 	/* flush any pending task for this netdev */
 	flush_workqueue(mdev->workqueue);
 
@@ -1914,6 +1938,75 @@ static int mlx4_en_change_mtu(struct net_device *dev, int new_mtu)
 	return 0;
 }
 
+static int mlx4_en_hwtstamp_ioctl(struct net_device *dev, struct ifreq *ifr)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = priv->mdev;
+	struct hwtstamp_config config;
+
+	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
+		return -EFAULT;
+
+	/* reserved for future extensions */
+	if (config.flags)
+		return -EINVAL;
+
+	/* device doesn't support time stamping */
+	if (!(mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_TS))
+		return -EINVAL;
+
+	/* TX HW timestamp */
+	switch (config.tx_type) {
+	case HWTSTAMP_TX_OFF:
+	case HWTSTAMP_TX_ON:
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	/* RX HW timestamp */
+	switch (config.rx_filter) {
+	case HWTSTAMP_FILTER_NONE:
+		break;
+	case HWTSTAMP_FILTER_ALL:
+	case HWTSTAMP_FILTER_SOME:
+	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+		config.rx_filter = HWTSTAMP_FILTER_ALL;
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	if (mlx4_en_timestamp_config(dev, config.tx_type, config.rx_filter)) {
+		config.tx_type = HWTSTAMP_TX_OFF;
+		config.rx_filter = HWTSTAMP_FILTER_NONE;
+	}
+
+	return copy_to_user(ifr->ifr_data, &config,
+			    sizeof(config)) ? -EFAULT : 0;
+}
+
+static int mlx4_en_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	switch (cmd) {
+	case SIOCSHWTSTAMP:
+		return mlx4_en_hwtstamp_ioctl(dev, ifr);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
 static int mlx4_en_set_features(struct net_device *netdev,
 		netdev_features_t features)
 {
@@ -1931,77 +2024,40 @@ static int mlx4_en_set_features(struct net_device *netdev,
 
 }
 
-static int mlx4_en_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
-			   struct net_device *dev,
-			   const unsigned char *addr, u16 flags)
+static int mlx4_en_set_vf_mac(struct net_device *dev, int queue, u8 *mac)
 {
-	struct mlx4_en_priv *priv = netdev_priv(dev);
-	struct mlx4_dev *mdev = priv->mdev->dev;
-	int err;
-
-	if (!mlx4_is_mfunc(mdev))
-		return -EOPNOTSUPP;
+	struct mlx4_en_priv *en_priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = en_priv->mdev;
+	u64 mac_u64 = mlx4_en_mac_to_u64(mac);
 
-	/* Hardware does not support aging addresses, allow only
-	 * permanent addresses if ndm_state is given
-	 */
-	if (ndm->ndm_state && !(ndm->ndm_state & NUD_PERMANENT)) {
-		en_info(priv, "Add FDB only supports static addresses\n");
+	if (!is_valid_ether_addr(mac))
 		return -EINVAL;
-	}
 
-	if (is_unicast_ether_addr(addr) || is_link_local_ether_addr(addr))
-		err = dev_uc_add_excl(dev, addr);
-	else if (is_multicast_ether_addr(addr))
-		err = dev_mc_add_excl(dev, addr);
-	else
-		err = -EINVAL;
-
-	/* Only return duplicate errors if NLM_F_EXCL is set */
-	if (err == -EEXIST && !(flags & NLM_F_EXCL))
-		err = 0;
-
-	return err;
+	return mlx4_set_vf_mac(mdev->dev, en_priv->port, queue, mac_u64);
 }
 
-static int mlx4_en_fdb_del(struct ndmsg *ndm,
-			   struct nlattr *tb[],
-			   struct net_device *dev,
-			   const unsigned char *addr)
+static int mlx4_en_set_vf_vlan(struct net_device *dev, int vf, u16 vlan, u8 qos)
 {
-	struct mlx4_en_priv *priv = netdev_priv(dev);
-	struct mlx4_dev *mdev = priv->mdev->dev;
-	int err;
-
-	if (!mlx4_is_mfunc(mdev))
-		return -EOPNOTSUPP;
+	struct mlx4_en_priv *en_priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = en_priv->mdev;
 
-	if (ndm->ndm_state && !(ndm->ndm_state & NUD_PERMANENT)) {
-		en_info(priv, "Del FDB only supports static addresses\n");
-		return -EINVAL;
-	}
+	return mlx4_set_vf_vlan(mdev->dev, en_priv->port, vf, vlan, qos);
+}
 
-	if (is_unicast_ether_addr(addr) || is_link_local_ether_addr(addr))
-		err = dev_uc_del(dev, addr);
-	else if (is_multicast_ether_addr(addr))
-		err = dev_mc_del(dev, addr);
-	else
-		err = -EINVAL;
+static int mlx4_en_set_vf_spoofchk(struct net_device *dev, int vf, bool setting)
+{
+	struct mlx4_en_priv *en_priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = en_priv->mdev;
 
-	return err;
+	return mlx4_set_vf_spoofchk(mdev->dev, en_priv->port, vf, setting);
 }
 
-static int mlx4_en_fdb_dump(struct sk_buff *skb,
-			    struct netlink_callback *cb,
-			    struct net_device *dev, int idx)
+static int mlx4_en_get_vf_config(struct net_device *dev, int vf, struct ifla_vf_info *ivf)
 {
-	struct mlx4_en_priv *priv = netdev_priv(dev);
-	struct mlx4_dev *mdev = priv->mdev->dev;
-
-	if (mlx4_is_mfunc(mdev))
-		idx = ndo_dflt_fdb_dump(skb, cb, dev, idx);
+	struct mlx4_en_priv *en_priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = en_priv->mdev;
 
-	return idx;
+	return mlx4_get_vf_config(mdev->dev, en_priv->port, vf, ivf);
 }
 
 static const struct net_device_ops mlx4_netdev_ops = {
@@ -2014,6 +2070,7 @@ static const struct net_device_ops mlx4_netdev_ops = {
 	.ndo_set_mac_address	= mlx4_en_set_mac,
 	.ndo_validate_addr	= eth_validate_addr,
 	.ndo_change_mtu		= mlx4_en_change_mtu,
+	.ndo_do_ioctl		= mlx4_en_ioctl,
 	.ndo_tx_timeout		= mlx4_en_tx_timeout,
 	.ndo_vlan_rx_add_vid	= mlx4_en_vlan_rx_add_vid,
 	.ndo_vlan_rx_kill_vid	= mlx4_en_vlan_rx_kill_vid,
@@ -2025,9 +2082,33 @@ static const struct net_device_ops mlx4_netdev_ops = {
 #ifdef CONFIG_RFS_ACCEL
 	.ndo_rx_flow_steer	= mlx4_en_filter_rfs,
 #endif
-	.ndo_fdb_add		= mlx4_en_fdb_add,
-	.ndo_fdb_del		= mlx4_en_fdb_del,
-	.ndo_fdb_dump		= mlx4_en_fdb_dump,
+};
+
+static const struct net_device_ops mlx4_netdev_ops_master = {
+	.ndo_open		= mlx4_en_open,
+	.ndo_stop		= mlx4_en_close,
+	.ndo_start_xmit		= mlx4_en_xmit,
+	.ndo_select_queue	= mlx4_en_select_queue,
+	.ndo_get_stats		= mlx4_en_get_stats,
+	.ndo_set_rx_mode	= mlx4_en_set_rx_mode,
+	.ndo_set_mac_address	= mlx4_en_set_mac,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_change_mtu		= mlx4_en_change_mtu,
+	.ndo_tx_timeout		= mlx4_en_tx_timeout,
+	.ndo_vlan_rx_add_vid	= mlx4_en_vlan_rx_add_vid,
+	.ndo_vlan_rx_kill_vid	= mlx4_en_vlan_rx_kill_vid,
+	.ndo_set_vf_mac		= mlx4_en_set_vf_mac,
+	.ndo_set_vf_vlan	= mlx4_en_set_vf_vlan,
+	.ndo_set_vf_spoofchk	= mlx4_en_set_vf_spoofchk,
+	.ndo_get_vf_config	= mlx4_en_get_vf_config,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= mlx4_en_netpoll,
+#endif
+	.ndo_set_features	= mlx4_en_set_features,
+	.ndo_setup_tc		= mlx4_en_setup_tc,
+#ifdef CONFIG_RFS_ACCEL
+	.ndo_rx_flow_steer	= mlx4_en_filter_rfs,
+#endif
 };
 
 int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
@@ -2088,9 +2169,16 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 	INIT_WORK(&priv->watchdog_task, mlx4_en_restart);
 	INIT_WORK(&priv->linkstate_task, mlx4_en_linkstate);
 	INIT_DELAYED_WORK(&priv->stats_task, mlx4_en_do_get_stats);
+	INIT_DELAYED_WORK(&priv->service_task, mlx4_en_service_task);
 #ifdef CONFIG_MLX4_EN_DCB
-	if (!mlx4_is_slave(priv->mdev->dev))
-		dev->dcbnl_ops = &mlx4_en_dcbnl_ops;
+	if (!mlx4_is_slave(priv->mdev->dev)) {
+		if (mdev->dev->caps.flags & MLX4_DEV_CAP_FLAG_SET_ETH_SCHED) {
+			dev->dcbnl_ops = &mlx4_en_dcbnl_ops;
+		} else {
+			en_info(priv, "enabling only PFC DCB ops\n");
+			dev->dcbnl_ops = &mlx4_en_dcbnl_pfc_ops;
+		}
+	}
 #endif
 
 	for (i = 0; i < MLX4_EN_MAC_HASH_SIZE; ++i)
@@ -2122,6 +2210,11 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 	spin_lock_init(&priv->filters_lock);
 #endif
 
+	/* Initialize time stamping config */
+	priv->hwtstamp_config.flags = 0;
+	priv->hwtstamp_config.tx_type = HWTSTAMP_TX_OFF;
+	priv->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_NONE;
+
 	/* Allocate page for receive rings */
 	err = mlx4_alloc_hwq_res(mdev->dev, &priv->res,
 				MLX4_EN_PAGE_SIZE, MLX4_EN_PAGE_SIZE);
@@ -2134,7 +2227,10 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 	/*
 	 * Initialize netdev entry points
 	 */
-	dev->netdev_ops = &mlx4_netdev_ops;
+	if (mlx4_is_master(priv->mdev->dev))
+		dev->netdev_ops = &mlx4_netdev_ops_master;
+	else
+		dev->netdev_ops = &mlx4_netdev_ops;
 	dev->watchdog_timeo = MLX4_EN_WATCHDOG_TIMEOUT;
 	netif_set_real_num_tx_queues(dev, priv->tx_ring_num);
 	netif_set_real_num_rx_queues(dev, priv->rx_ring_num);
@@ -2152,8 +2248,8 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 
 	dev->hw_features |= NETIF_F_RXCSUM | NETIF_F_RXHASH;
 	dev->features = dev->hw_features | NETIF_F_HIGHDMA |
-			NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX |
-			NETIF_F_HW_VLAN_FILTER;
+			NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX |
+			NETIF_F_HW_VLAN_CTAG_FILTER;
 	dev->hw_features |= NETIF_F_LOOPBACK;
 
 	if (mdev->dev->caps.steering_mode ==
@@ -2199,6 +2295,11 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 	}
 	mlx4_en_set_default_moderation(priv);
 	queue_delayed_work(mdev->workqueue, &priv->stats_task, STATS_DELAY);
+
+	if (mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_TS)
+		queue_delayed_work(mdev->workqueue, &priv->service_task,
+				   SERVICE_TASK_DELAY);
+
 	return 0;
 
 out:
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_resources.c b/drivers/net/ethernet/mellanox/mlx4/en_resources.c
index 10c24c784b70..91f2b2c43c12 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_resources.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_resources.c
@@ -42,6 +42,7 @@ void mlx4_en_fill_qp_context(struct mlx4_en_priv *priv, int size, int stride,
 			     int user_prio, struct mlx4_qp_context *context)
 {
 	struct mlx4_en_dev *mdev = priv->mdev;
+	struct net_device *dev = priv->dev;
 
 	memset(context, 0, sizeof *context);
 	context->flags = cpu_to_be32(7 << 16 | rss << MLX4_RSS_QPC_FLAG_OFFSET);
@@ -65,6 +66,8 @@ void mlx4_en_fill_qp_context(struct mlx4_en_priv *priv, int size, int stride,
 	context->cqn_send = cpu_to_be32(cqn);
 	context->cqn_recv = cpu_to_be32(cqn);
 	context->db_rec_addr = cpu_to_be64(priv->res.db.dma << 2);
+	if (!(dev->features & NETIF_F_HW_VLAN_CTAG_RX))
+		context->param3 |= cpu_to_be32(1 << 30);
 }
 
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_rx.c b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
index c7f856308e1a..02aee1ebd203 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_rx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
@@ -320,6 +320,8 @@ int mlx4_en_create_rx_ring(struct mlx4_en_priv *priv,
 	}
 	ring->buf = ring->wqres.buf.direct.buf;
 
+	ring->hwtstamp_rx_filter = priv->hwtstamp_config.rx_filter;
+
 	return 0;
 
 err_hwq:
@@ -554,6 +556,7 @@ static void mlx4_en_refill_rx_buffers(struct mlx4_en_priv *priv,
 int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int budget)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = priv->mdev;
 	struct mlx4_cqe *cqe;
 	struct mlx4_en_rx_ring *ring = &priv->rx_ring[cq->ring];
 	struct mlx4_en_rx_alloc *frags;
@@ -565,6 +568,7 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 	int polled = 0;
 	int ip_summed;
 	int factor = priv->cqe_factor;
+	u64 timestamp;
 
 	if (!priv->port_up)
 		return 0;
@@ -669,19 +673,27 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 					gro_skb->data_len = length;
 					gro_skb->ip_summed = CHECKSUM_UNNECESSARY;
 
-					if (cqe->vlan_my_qpn &
-					    cpu_to_be32(MLX4_CQE_VLAN_PRESENT_MASK)) {
+					if ((cqe->vlan_my_qpn &
+					    cpu_to_be32(MLX4_CQE_VLAN_PRESENT_MASK)) &&
+					    (dev->features & NETIF_F_HW_VLAN_CTAG_RX)) {
 						u16 vid = be16_to_cpu(cqe->sl_vid);
 
-						__vlan_hwaccel_put_tag(gro_skb, vid);
+						__vlan_hwaccel_put_tag(gro_skb, htons(ETH_P_8021Q), vid);
 					}
 
 					if (dev->features & NETIF_F_RXHASH)
 						gro_skb->rxhash = be32_to_cpu(cqe->immed_rss_invalid);
 
 					skb_record_rx_queue(gro_skb, cq->ring);
-					napi_gro_frags(&cq->napi);
 
+					if (ring->hwtstamp_rx_filter == HWTSTAMP_FILTER_ALL) {
+						timestamp = mlx4_en_get_cqe_ts(cqe);
+						mlx4_en_fill_hwtstamps(mdev,
+								       skb_hwtstamps(gro_skb),
+								       timestamp);
+					}
+
+					napi_gro_frags(&cq->napi);
 					goto next;
 				}
 
@@ -714,9 +726,16 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 		if (dev->features & NETIF_F_RXHASH)
 			skb->rxhash = be32_to_cpu(cqe->immed_rss_invalid);
 
-		if (be32_to_cpu(cqe->vlan_my_qpn) &
-		    MLX4_CQE_VLAN_PRESENT_MASK)
-			__vlan_hwaccel_put_tag(skb, be16_to_cpu(cqe->sl_vid));
+		if ((be32_to_cpu(cqe->vlan_my_qpn) &
+		    MLX4_CQE_VLAN_PRESENT_MASK) &&
+		    (dev->features & NETIF_F_HW_VLAN_CTAG_RX))
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), be16_to_cpu(cqe->sl_vid));
+
+		if (ring->hwtstamp_rx_filter == HWTSTAMP_FILTER_ALL) {
+			timestamp = mlx4_en_get_cqe_ts(cqe);
+			mlx4_en_fill_hwtstamps(mdev, skb_hwtstamps(skb),
+					       timestamp);
+		}
 
 		/* Push it up the stack */
 		netif_receive_skb(skb);
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_selftest.c b/drivers/net/ethernet/mellanox/mlx4/en_selftest.c
index 3488c6d9e6b5..2448f0d669e6 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_selftest.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_selftest.c
@@ -58,10 +58,9 @@ static int mlx4_en_test_loopback_xmit(struct mlx4_en_priv *priv)
 
 	/* build the pkt before xmit */
 	skb = netdev_alloc_skb(priv->dev, MLX4_LOOPBACK_TEST_PAYLOAD + ETH_HLEN + NET_IP_ALIGN);
-	if (!skb) {
-		en_err(priv, "-LOOPBACK_TEST_XMIT- failed to create skb for xmit\n");
+	if (!skb)
 		return -ENOMEM;
-	}
+
 	skb_reserve(skb, NET_IP_ALIGN);
 
 	ethh = (struct ethhdr *)skb_put(skb, sizeof(struct ethhdr));
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_tx.c b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
index 49308cc65ee7..4e6877a032a8 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_tx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
@@ -118,6 +118,8 @@ int mlx4_en_create_tx_ring(struct mlx4_en_priv *priv,
 	} else
 		ring->bf_enabled = true;
 
+	ring->hwtstamp_tx_type = priv->hwtstamp_config.tx_type;
+
 	return 0;
 
 err_map:
@@ -192,8 +194,9 @@ void mlx4_en_deactivate_tx_ring(struct mlx4_en_priv *priv,
 
 static u32 mlx4_en_free_tx_desc(struct mlx4_en_priv *priv,
 				struct mlx4_en_tx_ring *ring,
-				int index, u8 owner)
+				int index, u8 owner, u64 timestamp)
 {
+	struct mlx4_en_dev *mdev = priv->mdev;
 	struct mlx4_en_tx_info *tx_info = &ring->tx_info[index];
 	struct mlx4_en_tx_desc *tx_desc = ring->buf + index * TXBB_SIZE;
 	struct mlx4_wqe_data_seg *data = (void *) tx_desc + tx_info->data_offset;
@@ -204,6 +207,12 @@ static u32 mlx4_en_free_tx_desc(struct mlx4_en_priv *priv,
 	int i;
 	__be32 *ptr = (__be32 *)tx_desc;
 	__be32 stamp = cpu_to_be32(STAMP_VAL | (!!owner << STAMP_SHIFT));
+	struct skb_shared_hwtstamps hwts;
+
+	if (timestamp) {
+		mlx4_en_fill_hwtstamps(mdev, &hwts, timestamp);
+		skb_tstamp_tx(skb, &hwts);
+	}
 
 	/* Optimize the common case when there are no wraparounds */
 	if (likely((void *) tx_desc + tx_info->nr_txbb * TXBB_SIZE <= end)) {
@@ -289,7 +298,7 @@ int mlx4_en_free_tx_buf(struct net_device *dev, struct mlx4_en_tx_ring *ring)
 	while (ring->cons != ring->prod) {
 		ring->last_nr_txbb = mlx4_en_free_tx_desc(priv, ring,
 						ring->cons & ring->size_mask,
-						!!(ring->cons & ring->size));
+						!!(ring->cons & ring->size), 0);
 		ring->cons += ring->last_nr_txbb;
 		cnt++;
 	}
@@ -318,6 +327,7 @@ static void mlx4_en_process_tx_cq(struct net_device *dev, struct mlx4_en_cq *cq)
 	u32 packets = 0;
 	u32 bytes = 0;
 	int factor = priv->cqe_factor;
+	u64 timestamp = 0;
 
 	if (!priv->port_up)
 		return;
@@ -341,11 +351,14 @@ static void mlx4_en_process_tx_cq(struct net_device *dev, struct mlx4_en_cq *cq)
 		do {
 			txbbs_skipped += ring->last_nr_txbb;
 			ring_index = (ring_index + ring->last_nr_txbb) & size_mask;
+			if (ring->tx_info[ring_index].ts_requested)
+				timestamp = mlx4_en_get_cqe_ts(cqe);
+
 			/* free next descriptor */
 			ring->last_nr_txbb = mlx4_en_free_tx_desc(
 					priv, ring, ring_index,
 					!!((ring->cons + txbbs_skipped) &
-							ring->size));
+					ring->size), timestamp);
 			packets++;
 			bytes += ring->tx_info[ring_index].nr_bytes;
 		} while (ring_index != new_index);
@@ -629,6 +642,16 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 	tx_info->skb = skb;
 	tx_info->nr_txbb = nr_txbb;
 
+	/*
+	 * For timestamping add flag to skb_shinfo and
+	 * set flag for further reference
+	 */
+	if (ring->hwtstamp_tx_type == HWTSTAMP_TX_ON &&
+	    skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) {
+		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+		tx_info->ts_requested = 1;
+	}
+
 	/* Prepare ctrl segement apart opcode+ownership, which depends on
 	 * whether LSO is used */
 	tx_desc->ctrl.vlan_tag = cpu_to_be16(vlan_tag);
@@ -729,6 +752,8 @@ 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);
 
+	skb_tx_timestamp(skb);
+
 	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);
diff --git a/drivers/net/ethernet/mellanox/mlx4/eq.c b/drivers/net/ethernet/mellanox/mlx4/eq.c
index 8e3123a1df88..6000342f9725 100644
--- a/drivers/net/ethernet/mellanox/mlx4/eq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/eq.c
@@ -497,8 +497,8 @@ static int mlx4_eq_int(struct mlx4_dev *dev, struct mlx4_eq *eq)
 			break;
 
 		case MLX4_EVENT_TYPE_SRQ_LIMIT:
-			mlx4_warn(dev, "%s: MLX4_EVENT_TYPE_SRQ_LIMIT\n",
-				  __func__);
+			mlx4_dbg(dev, "%s: MLX4_EVENT_TYPE_SRQ_LIMIT\n",
+				 __func__);
 		case MLX4_EVENT_TYPE_SRQ_CATAS_ERROR:
 			if (mlx4_is_master(dev)) {
 				/* forward only to slave owning the SRQ */
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.c b/drivers/net/ethernet/mellanox/mlx4/fw.c
index f6245579962d..b147bdd40768 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.c
@@ -91,7 +91,7 @@ static void dump_dev_cap_flags(struct mlx4_dev *dev, u64 flags)
 		[ 8] = "P_Key violation counter",
 		[ 9] = "Q_Key violation counter",
 		[10] = "VMM",
-		[12] = "DPDP",
+		[12] = "Dual Port Different Protocol (DPDP) support",
 		[15] = "Big LSO headers",
 		[16] = "MW support",
 		[17] = "APM support",
@@ -109,6 +109,8 @@ static void dump_dev_cap_flags(struct mlx4_dev *dev, u64 flags)
 		[41] = "Unicast VEP steering support",
 		[42] = "Multicast VEP steering support",
 		[48] = "Counters support",
+		[53] = "Port ETS Scheduler support",
+		[55] = "Port link type sensing support",
 		[59] = "Port management change event support",
 		[61] = "64 byte EQE support",
 		[62] = "64 byte CQE support",
@@ -128,7 +130,8 @@ static void dump_dev_cap_flags2(struct mlx4_dev *dev, u64 flags)
 		[1] = "RSS Toeplitz Hash Function support",
 		[2] = "RSS XOR Hash Function support",
 		[3] = "Device manage flow steering support",
-		[4] = "Automatic mac reassignment support"
+		[4] = "Automatic MAC reassignment support",
+		[5] = "Time stamping support"
 	};
 	int i;
 
@@ -442,6 +445,7 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 #define QUERY_DEV_CAP_MAX_MSG_SZ_OFFSET		0x38
 #define QUERY_DEV_CAP_MAX_GID_OFFSET		0x3b
 #define QUERY_DEV_CAP_RATE_SUPPORT_OFFSET	0x3c
+#define QUERY_DEV_CAP_CQ_TS_SUPPORT_OFFSET	0x3e
 #define QUERY_DEV_CAP_MAX_PKEY_OFFSET		0x3f
 #define QUERY_DEV_CAP_EXT_FLAGS_OFFSET		0x40
 #define QUERY_DEV_CAP_FLAGS_OFFSET		0x44
@@ -464,6 +468,7 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 #define QUERY_DEV_CAP_RSVD_XRC_OFFSET		0x66
 #define QUERY_DEV_CAP_MAX_XRC_OFFSET		0x67
 #define QUERY_DEV_CAP_MAX_COUNTERS_OFFSET	0x68
+#define QUERY_DEV_CAP_EXT_2_FLAGS_OFFSET	0x70
 #define QUERY_DEV_CAP_FLOW_STEERING_RANGE_EN_OFFSET	0x76
 #define QUERY_DEV_CAP_FLOW_STEERING_MAX_QP_OFFSET	0x77
 #define QUERY_DEV_CAP_RDMARC_ENTRY_SZ_OFFSET	0x80
@@ -558,6 +563,9 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	dev_cap->fs_max_num_qp_per_entry = field;
 	MLX4_GET(stat_rate, outbox, QUERY_DEV_CAP_RATE_SUPPORT_OFFSET);
 	dev_cap->stat_rate_support = stat_rate;
+	MLX4_GET(field, outbox, QUERY_DEV_CAP_CQ_TS_SUPPORT_OFFSET);
+	if (field & 0x80)
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_TS;
 	MLX4_GET(ext_flags, outbox, QUERY_DEV_CAP_EXT_FLAGS_OFFSET);
 	MLX4_GET(flags, outbox, QUERY_DEV_CAP_FLAGS_OFFSET);
 	dev_cap->flags = flags | (u64)ext_flags << 32;
@@ -648,6 +656,12 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 		MLX4_GET(dev_cap->max_counters, outbox,
 			 QUERY_DEV_CAP_MAX_COUNTERS_OFFSET);
 
+	MLX4_GET(field32, outbox, QUERY_DEV_CAP_EXT_2_FLAGS_OFFSET);
+	if (field32 & (1 << 26))
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_VLAN_CONTROL;
+	if (field32 & (1 << 20))
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_FSM;
+
 	if (dev->flags & MLX4_FLAG_OLD_PORT_CMDS) {
 		for (i = 1; i <= dev_cap->num_ports; ++i) {
 			MLX4_GET(field, outbox, QUERY_DEV_CAP_VL_PORT_OFFSET);
@@ -777,6 +791,11 @@ int mlx4_QUERY_DEV_CAP_wrapper(struct mlx4_dev *dev, int slave,
 	flags &= ~MLX4_DEV_CAP_FLAG_MEM_WINDOW;
 	MLX4_PUT(outbox->buf, flags, QUERY_DEV_CAP_EXT_FLAGS_OFFSET);
 
+	/* For guests, disable timestamp */
+	MLX4_GET(field, outbox->buf, QUERY_DEV_CAP_CQ_TS_SUPPORT_OFFSET);
+	field &= 0x7f;
+	MLX4_PUT(outbox->buf, field, QUERY_DEV_CAP_CQ_TS_SUPPORT_OFFSET);
+
 	/* For guests, report Blueflame disabled */
 	MLX4_GET(field, outbox->buf, QUERY_DEV_CAP_BF_OFFSET);
 	field &= 0x7f;
@@ -804,6 +823,7 @@ int mlx4_QUERY_PORT_wrapper(struct mlx4_dev *dev, int slave,
 			    struct mlx4_cmd_mailbox *outbox,
 			    struct mlx4_cmd_info *cmd)
 {
+	struct mlx4_priv *priv = mlx4_priv(dev);
 	u64 def_mac;
 	u8 port_type;
 	u16 short_field;
@@ -821,6 +841,9 @@ int mlx4_QUERY_PORT_wrapper(struct mlx4_dev *dev, int slave,
 		/* set slave default_mac address */
 		MLX4_GET(def_mac, outbox->buf, QUERY_PORT_MAC_OFFSET);
 		def_mac += slave << 8;
+		/* if config MAC in DB use it */
+		if (priv->mfunc.master.vf_oper[slave].vport[vhcr->in_modifier].state.mac)
+			def_mac = priv->mfunc.master.vf_oper[slave].vport[vhcr->in_modifier].state.mac;
 		MLX4_PUT(outbox->buf, def_mac, QUERY_PORT_MAC_OFFSET);
 
 		/* get port type - currently only eth is enabled */
@@ -1006,6 +1029,9 @@ int mlx4_QUERY_FW(struct mlx4_dev *dev)
 #define QUERY_FW_COMM_BASE_OFFSET      0x40
 #define QUERY_FW_COMM_BAR_OFFSET       0x48
 
+#define QUERY_FW_CLOCK_OFFSET	       0x50
+#define QUERY_FW_CLOCK_BAR	       0x58
+
 	mailbox = mlx4_alloc_cmd_mailbox(dev);
 	if (IS_ERR(mailbox))
 		return PTR_ERR(mailbox);
@@ -1080,6 +1106,12 @@ int mlx4_QUERY_FW(struct mlx4_dev *dev)
 		 fw->comm_bar, fw->comm_base);
 	mlx4_dbg(dev, "FW size %d KB\n", fw->fw_pages >> 2);
 
+	MLX4_GET(fw->clock_offset, outbox, QUERY_FW_CLOCK_OFFSET);
+	MLX4_GET(fw->clock_bar,    outbox, QUERY_FW_CLOCK_BAR);
+	fw->clock_bar = (fw->clock_bar >> 6) * 2;
+	mlx4_dbg(dev, "Internal clock bar:%d offset:0x%llx\n",
+		 fw->clock_bar, fw->clock_offset);
+
 	/*
 	 * Round up number of system pages needed in case
 	 * MLX4_ICM_PAGE_SIZE < PAGE_SIZE.
@@ -1367,6 +1399,7 @@ int mlx4_QUERY_HCA(struct mlx4_dev *dev,
 	u8 byte_field;
 
 #define QUERY_HCA_GLOBAL_CAPS_OFFSET	0x04
+#define QUERY_HCA_CORE_CLOCK_OFFSET	0x0c
 
 	mailbox = mlx4_alloc_cmd_mailbox(dev);
 	if (IS_ERR(mailbox))
@@ -1381,6 +1414,7 @@ int mlx4_QUERY_HCA(struct mlx4_dev *dev,
 		goto out;
 
 	MLX4_GET(param->global_caps, outbox, QUERY_HCA_GLOBAL_CAPS_OFFSET);
+	MLX4_GET(param->hca_core_clock, outbox, QUERY_HCA_CORE_CLOCK_OFFSET);
 
 	/* QPC/EEC/CQC/EQC/RDMARC attributes */
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.h b/drivers/net/ethernet/mellanox/mlx4/fw.h
index 151c2bb380a6..fdf41665a059 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.h
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.h
@@ -162,6 +162,7 @@ struct mlx4_init_hca_param {
 	u64 global_caps;
 	u16 log_mc_entry_sz;
 	u16 log_mc_hash_sz;
+	u16 hca_core_clock; /* Internal Clock Frequency (in MHz) */
 	u8  log_num_qps;
 	u8  log_num_srqs;
 	u8  log_num_cqs;
diff --git a/drivers/net/ethernet/mellanox/mlx4/main.c b/drivers/net/ethernet/mellanox/mlx4/main.c
index 16abde20e1fc..0d32a82458bf 100644
--- a/drivers/net/ethernet/mellanox/mlx4/main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/main.c
@@ -513,6 +513,8 @@ static int mlx4_slave_cap(struct mlx4_dev *dev)
 
 	mlx4_log_num_mgm_entry_size = hca_param.log_mc_entry_sz;
 
+	dev->caps.hca_core_clock = hca_param.hca_core_clock;
+
 	memset(&dev_cap, 0, sizeof(dev_cap));
 	dev->caps.max_qp_dest_rdma = 1 << hca_param.log_rd_per_qp;
 	err = mlx4_dev_cap(dev, &dev_cap);
@@ -1226,8 +1228,53 @@ static void unmap_bf_area(struct mlx4_dev *dev)
 		io_mapping_free(mlx4_priv(dev)->bf_mapping);
 }
 
+cycle_t mlx4_read_clock(struct mlx4_dev *dev)
+{
+	u32 clockhi, clocklo, clockhi1;
+	cycle_t cycles;
+	int i;
+	struct mlx4_priv *priv = mlx4_priv(dev);
+
+	for (i = 0; i < 10; i++) {
+		clockhi = swab32(readl(priv->clock_mapping));
+		clocklo = swab32(readl(priv->clock_mapping + 4));
+		clockhi1 = swab32(readl(priv->clock_mapping));
+		if (clockhi == clockhi1)
+			break;
+	}
+
+	cycles = (u64) clockhi << 32 | (u64) clocklo;
+
+	return cycles;
+}
+EXPORT_SYMBOL_GPL(mlx4_read_clock);
+
+
+static int map_internal_clock(struct mlx4_dev *dev)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+
+	priv->clock_mapping =
+		ioremap(pci_resource_start(dev->pdev, priv->fw.clock_bar) +
+			priv->fw.clock_offset, MLX4_CLOCK_SIZE);
+
+	if (!priv->clock_mapping)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void unmap_internal_clock(struct mlx4_dev *dev)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+
+	if (priv->clock_mapping)
+		iounmap(priv->clock_mapping);
+}
+
 static void mlx4_close_hca(struct mlx4_dev *dev)
 {
+	unmap_internal_clock(dev);
 	unmap_bf_area(dev);
 	if (mlx4_is_slave(dev))
 		mlx4_slave_exit(dev);
@@ -1445,6 +1492,37 @@ static int mlx4_init_hca(struct mlx4_dev *dev)
 			mlx4_err(dev, "INIT_HCA command failed, aborting.\n");
 			goto err_free_icm;
 		}
+		/*
+		 * If TS is supported by FW
+		 * read HCA frequency by QUERY_HCA command
+		 */
+		if (dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_TS) {
+			memset(&init_hca, 0, sizeof(init_hca));
+			err = mlx4_QUERY_HCA(dev, &init_hca);
+			if (err) {
+				mlx4_err(dev, "QUERY_HCA command failed, disable timestamp.\n");
+				dev->caps.flags2 &= ~MLX4_DEV_CAP_FLAG2_TS;
+			} else {
+				dev->caps.hca_core_clock =
+					init_hca.hca_core_clock;
+			}
+
+			/* In case we got HCA frequency 0 - disable timestamping
+			 * to avoid dividing by zero
+			 */
+			if (!dev->caps.hca_core_clock) {
+				dev->caps.flags2 &= ~MLX4_DEV_CAP_FLAG2_TS;
+				mlx4_err(dev,
+					 "HCA frequency is 0. Timestamping is not supported.");
+			} else if (map_internal_clock(dev)) {
+				/*
+				 * Map internal clock,
+				 * in case of failure disable timestamping
+				 */
+				dev->caps.flags2 &= ~MLX4_DEV_CAP_FLAG2_TS;
+				mlx4_err(dev, "Failed to map internal clock. Timestamping is not supported.\n");
+			}
+		}
 	} else {
 		err = mlx4_init_slave(dev);
 		if (err) {
@@ -1478,6 +1556,7 @@ static int mlx4_init_hca(struct mlx4_dev *dev)
 	return 0;
 
 unmap_bf:
+	unmap_internal_clock(dev);
 	unmap_bf_area(dev);
 
 err_close:
diff --git a/drivers/net/ethernet/mellanox/mlx4/mcg.c b/drivers/net/ethernet/mellanox/mlx4/mcg.c
index 52685524708d..f3e804f2a35f 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mcg.c
+++ b/drivers/net/ethernet/mellanox/mlx4/mcg.c
@@ -645,25 +645,37 @@ static int find_entry(struct mlx4_dev *dev, u8 port,
 	return err;
 }
 
+static const u8 __promisc_mode[] = {
+	[MLX4_FS_REGULAR]   = 0x0,
+	[MLX4_FS_ALL_DEFAULT] = 0x1,
+	[MLX4_FS_MC_DEFAULT] = 0x3,
+	[MLX4_FS_UC_SNIFFER] = 0x4,
+	[MLX4_FS_MC_SNIFFER] = 0x5,
+};
+
+int mlx4_map_sw_to_hw_steering_mode(struct mlx4_dev *dev,
+				    enum mlx4_net_trans_promisc_mode flow_type)
+{
+	if (flow_type >= MLX4_FS_MODE_NUM || flow_type < 0) {
+		mlx4_err(dev, "Invalid flow type. type = %d\n", flow_type);
+		return -EINVAL;
+	}
+	return __promisc_mode[flow_type];
+}
+EXPORT_SYMBOL_GPL(mlx4_map_sw_to_hw_steering_mode);
+
 static void trans_rule_ctrl_to_hw(struct mlx4_net_trans_rule *ctrl,
 				  struct mlx4_net_trans_rule_hw_ctrl *hw)
 {
-	static const u8 __promisc_mode[] = {
-		[MLX4_FS_PROMISC_NONE]   = 0x0,
-		[MLX4_FS_PROMISC_UPLINK] = 0x1,
-		[MLX4_FS_PROMISC_FUNCTION_PORT] = 0x2,
-		[MLX4_FS_PROMISC_ALL_MULTI] = 0x3,
-	};
-
-	u32 dw = 0;
-
-	dw = ctrl->queue_mode == MLX4_NET_TRANS_Q_LIFO ? 1 : 0;
-	dw |= ctrl->exclusive ? (1 << 2) : 0;
-	dw |= ctrl->allow_loopback ? (1 << 3) : 0;
-	dw |= __promisc_mode[ctrl->promisc_mode] << 8;
-	dw |= ctrl->priority << 16;
-
-	hw->ctrl = cpu_to_be32(dw);
+	u8 flags = 0;
+
+	flags = ctrl->queue_mode == MLX4_NET_TRANS_Q_LIFO ? 1 : 0;
+	flags |= ctrl->exclusive ? (1 << 2) : 0;
+	flags |= ctrl->allow_loopback ? (1 << 3) : 0;
+
+	hw->flags = flags;
+	hw->type = __promisc_mode[ctrl->promisc_mode];
+	hw->prio = cpu_to_be16(ctrl->priority);
 	hw->port = ctrl->port;
 	hw->qpn = cpu_to_be32(ctrl->qpn);
 }
@@ -677,29 +689,51 @@ const u16 __sw_id_hw[] = {
 	[MLX4_NET_TRANS_RULE_ID_UDP]     = 0xE006
 };
 
+int mlx4_map_sw_to_hw_steering_id(struct mlx4_dev *dev,
+				  enum mlx4_net_trans_rule_id id)
+{
+	if (id >= MLX4_NET_TRANS_RULE_NUM || id < 0) {
+		mlx4_err(dev, "Invalid network rule id. id = %d\n", id);
+		return -EINVAL;
+	}
+	return __sw_id_hw[id];
+}
+EXPORT_SYMBOL_GPL(mlx4_map_sw_to_hw_steering_id);
+
+static const int __rule_hw_sz[] = {
+	[MLX4_NET_TRANS_RULE_ID_ETH] =
+		sizeof(struct mlx4_net_trans_rule_hw_eth),
+	[MLX4_NET_TRANS_RULE_ID_IB] =
+		sizeof(struct mlx4_net_trans_rule_hw_ib),
+	[MLX4_NET_TRANS_RULE_ID_IPV6] = 0,
+	[MLX4_NET_TRANS_RULE_ID_IPV4] =
+		sizeof(struct mlx4_net_trans_rule_hw_ipv4),
+	[MLX4_NET_TRANS_RULE_ID_TCP] =
+		sizeof(struct mlx4_net_trans_rule_hw_tcp_udp),
+	[MLX4_NET_TRANS_RULE_ID_UDP] =
+		sizeof(struct mlx4_net_trans_rule_hw_tcp_udp)
+};
+
+int mlx4_hw_rule_sz(struct mlx4_dev *dev,
+	       enum mlx4_net_trans_rule_id id)
+{
+	if (id >= MLX4_NET_TRANS_RULE_NUM || id < 0) {
+		mlx4_err(dev, "Invalid network rule id. id = %d\n", id);
+		return -EINVAL;
+	}
+
+	return __rule_hw_sz[id];
+}
+EXPORT_SYMBOL_GPL(mlx4_hw_rule_sz);
+
 static int parse_trans_rule(struct mlx4_dev *dev, struct mlx4_spec_list *spec,
 			    struct _rule_hw *rule_hw)
 {
-	static const size_t __rule_hw_sz[] = {
-		[MLX4_NET_TRANS_RULE_ID_ETH] =
-			sizeof(struct mlx4_net_trans_rule_hw_eth),
-		[MLX4_NET_TRANS_RULE_ID_IB] =
-			sizeof(struct mlx4_net_trans_rule_hw_ib),
-		[MLX4_NET_TRANS_RULE_ID_IPV6] = 0,
-		[MLX4_NET_TRANS_RULE_ID_IPV4] =
-			sizeof(struct mlx4_net_trans_rule_hw_ipv4),
-		[MLX4_NET_TRANS_RULE_ID_TCP] =
-			sizeof(struct mlx4_net_trans_rule_hw_tcp_udp),
-		[MLX4_NET_TRANS_RULE_ID_UDP] =
-			sizeof(struct mlx4_net_trans_rule_hw_tcp_udp)
-	};
-	if (spec->id >= MLX4_NET_TRANS_RULE_NUM) {
-		mlx4_err(dev, "Invalid network rule id. id = %d\n", spec->id);
+	if (mlx4_hw_rule_sz(dev, spec->id) < 0)
 		return -EINVAL;
-	}
-	memset(rule_hw, 0, __rule_hw_sz[spec->id]);
+	memset(rule_hw, 0, mlx4_hw_rule_sz(dev, spec->id));
 	rule_hw->id = cpu_to_be16(__sw_id_hw[spec->id]);
-	rule_hw->size = __rule_hw_sz[spec->id] >> 2;
+	rule_hw->size = mlx4_hw_rule_sz(dev, spec->id) >> 2;
 
 	switch (spec->id) {
 	case MLX4_NET_TRANS_RULE_ID_ETH:
@@ -713,12 +747,12 @@ static int parse_trans_rule(struct mlx4_dev *dev, struct mlx4_spec_list *spec,
 			rule_hw->eth.ether_type_enable = 1;
 			rule_hw->eth.ether_type = spec->eth.ether_type;
 		}
-		rule_hw->eth.vlan_id = spec->eth.vlan_id;
-		rule_hw->eth.vlan_id_msk = spec->eth.vlan_id_msk;
+		rule_hw->eth.vlan_tag = spec->eth.vlan_id;
+		rule_hw->eth.vlan_tag_msk = spec->eth.vlan_id_msk;
 		break;
 
 	case MLX4_NET_TRANS_RULE_ID_IB:
-		rule_hw->ib.qpn = spec->ib.r_qpn;
+		rule_hw->ib.l3_qpn = spec->ib.l3_qpn;
 		rule_hw->ib.qpn_mask = spec->ib.qpn_msk;
 		memcpy(&rule_hw->ib.dst_gid, &spec->ib.dst_gid, 16);
 		memcpy(&rule_hw->ib.dst_gid_msk, &spec->ib.dst_gid_msk, 16);
@@ -1125,35 +1159,18 @@ static int mlx4_QP_ATTACH(struct mlx4_dev *dev, struct mlx4_qp *qp,
 	return err;
 }
 
-int mlx4_multicast_attach(struct mlx4_dev *dev, struct mlx4_qp *qp, u8 gid[16],
-			  u8 port, int block_mcast_loopback,
-			  enum mlx4_protocol prot, u64 *reg_id)
+int mlx4_trans_to_dmfs_attach(struct mlx4_dev *dev, struct mlx4_qp *qp,
+			      u8 gid[16], u8 port,
+			      int block_mcast_loopback,
+			      enum mlx4_protocol prot, u64 *reg_id)
 {
-
-	switch (dev->caps.steering_mode) {
-	case MLX4_STEERING_MODE_A0:
-		if (prot == MLX4_PROT_ETH)
-			return 0;
-
-	case MLX4_STEERING_MODE_B0:
-		if (prot == MLX4_PROT_ETH)
-			gid[7] |= (MLX4_MC_STEER << 1);
-
-		if (mlx4_is_mfunc(dev))
-			return mlx4_QP_ATTACH(dev, qp, gid, 1,
-					      block_mcast_loopback, prot);
-		return mlx4_qp_attach_common(dev, qp, gid,
-					     block_mcast_loopback, prot,
-					     MLX4_MC_STEER);
-
-	case MLX4_STEERING_MODE_DEVICE_MANAGED: {
 		struct mlx4_spec_list spec = { {NULL} };
 		__be64 mac_mask = cpu_to_be64(MLX4_MAC_MASK << 16);
 
 		struct mlx4_net_trans_rule rule = {
 			.queue_mode = MLX4_NET_TRANS_Q_FIFO,
 			.exclusive = 0,
-			.promisc_mode = MLX4_FS_PROMISC_NONE,
+			.promisc_mode = MLX4_FS_REGULAR,
 			.priority = MLX4_DOMAIN_NIC,
 		};
 
@@ -1180,8 +1197,32 @@ int mlx4_multicast_attach(struct mlx4_dev *dev, struct mlx4_qp *qp, u8 gid[16],
 		list_add_tail(&spec.list, &rule.list);
 
 		return mlx4_flow_attach(dev, &rule, reg_id);
-	}
+}
+
+int mlx4_multicast_attach(struct mlx4_dev *dev, struct mlx4_qp *qp, u8 gid[16],
+			  u8 port, int block_mcast_loopback,
+			  enum mlx4_protocol prot, u64 *reg_id)
+{
+	switch (dev->caps.steering_mode) {
+	case MLX4_STEERING_MODE_A0:
+		if (prot == MLX4_PROT_ETH)
+			return 0;
+
+	case MLX4_STEERING_MODE_B0:
+		if (prot == MLX4_PROT_ETH)
+			gid[7] |= (MLX4_MC_STEER << 1);
 
+		if (mlx4_is_mfunc(dev))
+			return mlx4_QP_ATTACH(dev, qp, gid, 1,
+					      block_mcast_loopback, prot);
+		return mlx4_qp_attach_common(dev, qp, gid,
+					     block_mcast_loopback, prot,
+					     MLX4_MC_STEER);
+
+	case MLX4_STEERING_MODE_DEVICE_MANAGED:
+		return mlx4_trans_to_dmfs_attach(dev, qp, gid, port,
+						 block_mcast_loopback,
+						 prot, reg_id);
 	default:
 		return -EINVAL;
 	}
@@ -1222,11 +1263,10 @@ int mlx4_flow_steer_promisc_add(struct mlx4_dev *dev, u8 port,
 	u64 *regid_p;
 
 	switch (mode) {
-	case MLX4_FS_PROMISC_UPLINK:
-	case MLX4_FS_PROMISC_FUNCTION_PORT:
+	case MLX4_FS_ALL_DEFAULT:
 		regid_p = &dev->regid_promisc_array[port];
 		break;
-	case MLX4_FS_PROMISC_ALL_MULTI:
+	case MLX4_FS_MC_DEFAULT:
 		regid_p = &dev->regid_allmulti_array[port];
 		break;
 	default:
@@ -1253,11 +1293,10 @@ int mlx4_flow_steer_promisc_remove(struct mlx4_dev *dev, u8 port,
 	u64 *regid_p;
 
 	switch (mode) {
-	case MLX4_FS_PROMISC_UPLINK:
-	case MLX4_FS_PROMISC_FUNCTION_PORT:
+	case MLX4_FS_ALL_DEFAULT:
 		regid_p = &dev->regid_promisc_array[port];
 		break;
-	case MLX4_FS_PROMISC_ALL_MULTI:
+	case MLX4_FS_MC_DEFAULT:
 		regid_p = &dev->regid_allmulti_array[port];
 		break;
 	default:
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4.h b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
index d738454116a0..df15bb6631cc 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
@@ -87,7 +87,8 @@ enum {
 	MLX4_HCR_SIZE		= 0x0001c,
 	MLX4_CLR_INT_SIZE	= 0x00008,
 	MLX4_SLAVE_COMM_BASE	= 0x0,
-	MLX4_COMM_PAGESIZE	= 0x1000
+	MLX4_COMM_PAGESIZE	= 0x1000,
+	MLX4_CLOCK_SIZE		= 0x00008
 };
 
 enum {
@@ -403,6 +404,7 @@ struct mlx4_fw {
 	u64			clr_int_base;
 	u64			catas_offset;
 	u64			comm_base;
+	u64			clock_offset;
 	struct mlx4_icm	       *fw_icm;
 	struct mlx4_icm	       *aux_icm;
 	u32			catas_size;
@@ -410,6 +412,7 @@ struct mlx4_fw {
 	u8			clr_int_bar;
 	u8			catas_bar;
 	u8			comm_bar;
+	u8			clock_bar;
 };
 
 struct mlx4_comm {
@@ -470,6 +473,30 @@ struct mlx4_slave_state {
 	enum slave_port_state port_state[MLX4_MAX_PORTS + 1];
 };
 
+#define MLX4_VGT 4095
+#define NO_INDX  (-1)
+
+struct mlx4_vport_state {
+	u64 mac;
+	u16 default_vlan;
+	u8  default_qos;
+	u32 tx_rate;
+	bool spoofchk;
+};
+
+struct mlx4_vf_admin_state {
+	struct mlx4_vport_state vport[MLX4_MAX_PORTS + 1];
+};
+
+struct mlx4_vport_oper_state {
+	struct mlx4_vport_state state;
+	int mac_idx;
+	int vlan_idx;
+};
+struct mlx4_vf_oper_state {
+	struct mlx4_vport_oper_state vport[MLX4_MAX_PORTS + 1];
+};
+
 struct slave_list {
 	struct mutex mutex;
 	struct list_head res_list[MLX4_NUM_OF_RESOURCE_TYPE];
@@ -500,6 +527,8 @@ struct mlx4_master_qp0_state {
 
 struct mlx4_mfunc_master_ctx {
 	struct mlx4_slave_state *slave_state;
+	struct mlx4_vf_admin_state *vf_admin;
+	struct mlx4_vf_oper_state *vf_oper;
 	struct mlx4_master_qp0_state qp0_state[MLX4_MAX_PORTS + 1];
 	int			init_port_ref[MLX4_MAX_PORTS + 1];
 	u16			max_mtu[MLX4_MAX_PORTS + 1];
@@ -701,85 +730,6 @@ struct mlx4_steer {
 	struct list_head steer_entries[MLX4_NUM_STEERS];
 };
 
-struct mlx4_net_trans_rule_hw_ctrl {
-	__be32 ctrl;
-	u8 rsvd1;
-	u8 funcid;
-	u8 vep;
-	u8 port;
-	__be32 qpn;
-	__be32 rsvd2;
-};
-
-struct mlx4_net_trans_rule_hw_ib {
-	u8 size;
-	u8 rsvd1;
-	__be16 id;
-	u32 rsvd2;
-	__be32 qpn;
-	__be32 qpn_mask;
-	u8 dst_gid[16];
-	u8 dst_gid_msk[16];
-} __packed;
-
-struct mlx4_net_trans_rule_hw_eth {
-	u8	size;
-	u8	rsvd;
-	__be16	id;
-	u8	rsvd1[6];
-	u8	dst_mac[6];
-	u16	rsvd2;
-	u8	dst_mac_msk[6];
-	u16	rsvd3;
-	u8	src_mac[6];
-	u16	rsvd4;
-	u8	src_mac_msk[6];
-	u8      rsvd5;
-	u8      ether_type_enable;
-	__be16  ether_type;
-	__be16  vlan_id_msk;
-	__be16  vlan_id;
-} __packed;
-
-struct mlx4_net_trans_rule_hw_tcp_udp {
-	u8	size;
-	u8	rsvd;
-	__be16	id;
-	__be16	rsvd1[3];
-	__be16	dst_port;
-	__be16	rsvd2;
-	__be16	dst_port_msk;
-	__be16	rsvd3;
-	__be16	src_port;
-	__be16	rsvd4;
-	__be16	src_port_msk;
-} __packed;
-
-struct mlx4_net_trans_rule_hw_ipv4 {
-	u8	size;
-	u8	rsvd;
-	__be16	id;
-	__be32	rsvd1;
-	__be32	dst_ip;
-	__be32	dst_ip_msk;
-	__be32	src_ip;
-	__be32	src_ip_msk;
-} __packed;
-
-struct _rule_hw {
-	union {
-		struct {
-			u8 size;
-			u8 rsvd;
-			__be16 id;
-		};
-		struct mlx4_net_trans_rule_hw_eth eth;
-		struct mlx4_net_trans_rule_hw_ib ib;
-		struct mlx4_net_trans_rule_hw_ipv4 ipv4;
-		struct mlx4_net_trans_rule_hw_tcp_udp tcp_udp;
-	};
-};
-
 enum {
 	MLX4_PCI_DEV_IS_VF		= 1 << 0,
 	MLX4_PCI_DEV_FORCE_SENSE_PORT	= 1 << 1,
@@ -826,6 +776,7 @@ struct mlx4_priv {
 	struct list_head	bf_list;
 	struct mutex		bf_mutex;
 	struct io_mapping	*bf_mapping;
+	void __iomem            *clock_mapping;
 	int			reserved_mtts;
 	int			fs_hash_mode;
 	u8 virt2phys_pkey[MLX4_MFUNC_MAX][MLX4_MAX_PORTS][MLX4_MAX_PORT_PKEYS];
@@ -1127,6 +1078,8 @@ int mlx4_change_port_types(struct mlx4_dev *dev,
 
 void mlx4_init_mac_table(struct mlx4_dev *dev, struct mlx4_mac_table *table);
 void mlx4_init_vlan_table(struct mlx4_dev *dev, struct mlx4_vlan_table *table);
+void __mlx4_unregister_vlan(struct mlx4_dev *dev, u8 port, int index);
+int __mlx4_register_vlan(struct mlx4_dev *dev, u8 port, u16 vlan, int *index);
 
 int mlx4_SET_PORT(struct mlx4_dev *dev, u8 port, int pkey_tbl_sz);
 /* resource tracker functions*/
@@ -1190,6 +1143,10 @@ int mlx4_qp_detach_common(struct mlx4_dev *dev, struct mlx4_qp *qp, u8 gid[16],
 int mlx4_qp_attach_common(struct mlx4_dev *dev, struct mlx4_qp *qp, u8 gid[16],
 			  int block_mcast_loopback, enum mlx4_protocol prot,
 			  enum mlx4_steer_type steer);
+int mlx4_trans_to_dmfs_attach(struct mlx4_dev *dev, struct mlx4_qp *qp,
+			      u8 gid[16], u8 port,
+			      int block_mcast_loopback,
+			      enum mlx4_protocol prot, u64 *reg_id);
 int mlx4_SET_MCAST_FLTR_wrapper(struct mlx4_dev *dev, int slave,
 				struct mlx4_vhcr *vhcr,
 				struct mlx4_cmd_mailbox *inbox,
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
index f710b7ce0dcb..b1d7657b2bf5 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
@@ -40,6 +40,7 @@
 #include <linux/mutex.h>
 #include <linux/netdevice.h>
 #include <linux/if_vlan.h>
+#include <linux/net_tstamp.h>
 #ifdef CONFIG_MLX4_EN_DCB
 #include <linux/dcbnl.h>
 #endif
@@ -77,6 +78,7 @@
 #define STAMP_SHIFT		31
 #define STAMP_VAL		0x7fffffff
 #define STATS_DELAY		(HZ / 4)
+#define SERVICE_TASK_DELAY	(HZ / 4)
 #define MAX_NUM_OF_FS_RULES	256
 
 #define MLX4_EN_FILTER_HASH_SHIFT 4
@@ -207,6 +209,7 @@ struct mlx4_en_tx_info {
 	u8 linear;
 	u8 data_offset;
 	u8 inl;
+	u8 ts_requested;
 };
 
 
@@ -262,6 +265,7 @@ struct mlx4_en_tx_ring {
 	struct mlx4_bf bf;
 	bool bf_enabled;
 	struct netdev_queue *tx_queue;
+	int hwtstamp_tx_type;
 };
 
 struct mlx4_en_rx_desc {
@@ -288,6 +292,7 @@ struct mlx4_en_rx_ring {
 	unsigned long packets;
 	unsigned long csum_ok;
 	unsigned long csum_none;
+	int hwtstamp_rx_filter;
 };
 
 struct mlx4_en_cq {
@@ -348,6 +353,10 @@ struct mlx4_en_dev {
 	u32                     priv_pdn;
 	spinlock_t              uar_lock;
 	u8			mac_removed[MLX4_MAX_PORTS + 1];
+	struct cyclecounter	cycles;
+	struct timecounter	clock;
+	unsigned long		last_overflow_check;
+	unsigned long		overflow_period;
 };
 
 
@@ -512,6 +521,7 @@ struct mlx4_en_priv {
 	struct work_struct watchdog_task;
 	struct work_struct linkstate_task;
 	struct delayed_work stats_task;
+	struct delayed_work service_task;
 	struct mlx4_en_perf_stats pstats;
 	struct mlx4_en_pkt_stats pkstats;
 	struct mlx4_en_port_stats port_stats;
@@ -525,6 +535,7 @@ struct mlx4_en_priv {
 	struct device *ddev;
 	int base_tx_qpn;
 	struct hlist_head mac_hash[MLX4_EN_MAC_HASH_SIZE];
+	struct hwtstamp_config hwtstamp_config;
 
 #ifdef CONFIG_MLX4_EN_DCB
 	struct ieee_ets ets;
@@ -624,6 +635,7 @@ int mlx4_en_QUERY_PORT(struct mlx4_en_dev *mdev, u8 port);
 
 #ifdef CONFIG_MLX4_EN_DCB
 extern const struct dcbnl_rtnl_ops mlx4_en_dcbnl_ops;
+extern const struct dcbnl_rtnl_ops mlx4_en_dcbnl_pfc_ops;
 #endif
 
 int mlx4_en_setup_tc(struct net_device *dev, u8 up);
@@ -636,9 +648,21 @@ void mlx4_en_cleanup_filters(struct mlx4_en_priv *priv,
 #define MLX4_EN_NUM_SELF_TEST	5
 void mlx4_en_ex_selftest(struct net_device *dev, u32 *flags, u64 *buf);
 u64 mlx4_en_mac_to_u64(u8 *addr);
+void mlx4_en_ptp_overflow_check(struct mlx4_en_dev *mdev);
 
 /*
- * Globals
+ * Functions for time stamping
+ */
+u64 mlx4_en_get_cqe_ts(struct mlx4_cqe *cqe);
+void mlx4_en_fill_hwtstamps(struct mlx4_en_dev *mdev,
+			    struct skb_shared_hwtstamps *hwts,
+			    u64 timestamp);
+void mlx4_en_init_timestamp(struct mlx4_en_dev *mdev);
+int mlx4_en_timestamp_config(struct net_device *dev,
+			     int tx_type,
+			     int rx_filter);
+
+/* Globals
  */
 extern const struct ethtool_ops mlx4_en_ethtool_ops;
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/port.c b/drivers/net/ethernet/mellanox/mlx4/port.c
index 10c57c86388b..946e0af5faef 100644
--- a/drivers/net/ethernet/mellanox/mlx4/port.c
+++ b/drivers/net/ethernet/mellanox/mlx4/port.c
@@ -32,6 +32,7 @@
 
 #include <linux/errno.h>
 #include <linux/if_ether.h>
+#include <linux/if_vlan.h>
 #include <linux/export.h>
 
 #include <linux/mlx4/cmd.h>
@@ -140,8 +141,9 @@ int __mlx4_register_mac(struct mlx4_dev *dev, u8 port, u64 mac)
 		}
 
 		if (mac == (MLX4_MAC_MASK & be64_to_cpu(table->entries[i]))) {
-			/* MAC already registered, Must not have duplicates */
-			err = -EEXIST;
+			/* MAC already registered, increment ref count */
+			err = i;
+			++table->refs[i];
 			goto out;
 		}
 	}
@@ -164,7 +166,7 @@ int __mlx4_register_mac(struct mlx4_dev *dev, u8 port, u64 mac)
 		table->entries[free] = 0;
 		goto out;
 	}
-
+	table->refs[free] = 1;
 	err = free;
 	++table->total;
 out:
@@ -205,12 +207,16 @@ void __mlx4_unregister_mac(struct mlx4_dev *dev, u8 port, u64 mac)
 	struct mlx4_mac_table *table = &info->mac_table;
 	int index;
 
-	index = find_index(dev, table, mac);
-
 	mutex_lock(&table->mutex);
+	index = find_index(dev, table, mac);
 
 	if (validate_index(dev, table, index))
 		goto out;
+	if (--table->refs[index]) {
+		mlx4_dbg(dev, "Have more references for index %d,"
+			 "no need to modify mac table\n", index);
+		goto out;
+	}
 
 	table->entries[index] = 0;
 	mlx4_set_port_mac_table(dev, port, table->entries);
@@ -304,7 +310,7 @@ int mlx4_find_cached_vlan(struct mlx4_dev *dev, u8 port, u16 vid, int *idx)
 }
 EXPORT_SYMBOL_GPL(mlx4_find_cached_vlan);
 
-static int __mlx4_register_vlan(struct mlx4_dev *dev, u8 port, u16 vlan,
+int __mlx4_register_vlan(struct mlx4_dev *dev, u8 port, u16 vlan,
 				int *index)
 {
 	struct mlx4_vlan_table *table = &mlx4_priv(dev)->port[port].vlan_table;
@@ -378,7 +384,7 @@ int mlx4_register_vlan(struct mlx4_dev *dev, u8 port, u16 vlan, int *index)
 }
 EXPORT_SYMBOL_GPL(mlx4_register_vlan);
 
-static void __mlx4_unregister_vlan(struct mlx4_dev *dev, u8 port, int index)
+void __mlx4_unregister_vlan(struct mlx4_dev *dev, u8 port, int index)
 {
 	struct mlx4_vlan_table *table = &mlx4_priv(dev)->port[port].vlan_table;
 
@@ -517,7 +523,8 @@ static int mlx4_common_set_port(struct mlx4_dev *dev, int slave, u32 in_mod,
 			/* Mtu is configured as the max MTU among all the
 			 * the functions on the port. */
 			mtu = be16_to_cpu(gen_context->mtu);
-			mtu = min_t(int, mtu, dev->caps.eth_mtu_cap[port]);
+			mtu = min_t(int, mtu, dev->caps.eth_mtu_cap[port] +
+				    ETH_HLEN + VLAN_HLEN + ETH_FCS_LEN);
 			prev_mtu = slave_st->mtu[port];
 			slave_st->mtu[port] = mtu;
 			if (mtu > master->max_mtu[port])
diff --git a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
index 1391b52f443a..e12e0d2e0ee0 100644
--- a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
+++ b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
@@ -75,6 +75,7 @@ struct res_gid {
 	u8			gid[16];
 	enum mlx4_protocol	prot;
 	enum mlx4_steer_type	steer;
+	u64			reg_id;
 };
 
 enum res_qp_states {
@@ -352,6 +353,47 @@ static void update_gid(struct mlx4_dev *dev, struct mlx4_cmd_mailbox *inbox,
 	}
 }
 
+static int update_vport_qp_param(struct mlx4_dev *dev,
+				 struct mlx4_cmd_mailbox *inbox,
+				 u8 slave)
+{
+	struct mlx4_qp_context	*qpc = inbox->buf + 8;
+	struct mlx4_vport_oper_state *vp_oper;
+	struct mlx4_priv *priv;
+	u32 qp_type;
+	int port;
+
+	port = (qpc->pri_path.sched_queue & 0x40) ? 2 : 1;
+	priv = mlx4_priv(dev);
+	vp_oper = &priv->mfunc.master.vf_oper[slave].vport[port];
+
+	if (MLX4_VGT != vp_oper->state.default_vlan) {
+		qp_type	= (be32_to_cpu(qpc->flags) >> 16) & 0xff;
+		if (MLX4_QP_ST_RC == qp_type)
+			return -EINVAL;
+
+		qpc->pri_path.vlan_index = vp_oper->vlan_idx;
+		qpc->pri_path.fl = (1 << 6) | (1 << 2); /* set cv bit and hide_cqe_vlan bit*/
+		qpc->pri_path.feup |= 1 << 3; /* set fvl bit */
+		qpc->pri_path.sched_queue &= 0xC7;
+		qpc->pri_path.sched_queue |= (vp_oper->state.default_qos) << 3;
+		mlx4_dbg(dev, "qp %d  port %d Q 0x%x set vlan to %d vidx %d feup %x fl %x\n",
+			 be32_to_cpu(qpc->local_qpn) & 0xffffff, port,
+			 (int)(qpc->pri_path.sched_queue), vp_oper->state.default_vlan,
+			 vp_oper->vlan_idx, (int)(qpc->pri_path.feup),
+			 (int)(qpc->pri_path.fl));
+	}
+	if (vp_oper->state.spoofchk) {
+		qpc->pri_path.feup |= 1 << 5; /* set fsm bit */;
+		qpc->pri_path.grh_mylmc = (0x80 & qpc->pri_path.grh_mylmc) + vp_oper->mac_idx;
+		mlx4_dbg(dev, "spoof qp %d  port %d feup  0x%x, myLmc 0x%x mindx %d\n",
+			 be32_to_cpu(qpc->local_qpn) & 0xffffff, port,
+			 (int)qpc->pri_path.feup, (int)qpc->pri_path.grh_mylmc,
+			 vp_oper->mac_idx);
+	}
+	return 0;
+}
+
 static int mpt_mask(struct mlx4_dev *dev)
 {
 	return dev->caps.num_mpts - 1;
@@ -2797,6 +2839,9 @@ int mlx4_INIT2RTR_QP_wrapper(struct mlx4_dev *dev, int slave,
 	update_pkey_index(dev, slave, inbox);
 	update_gid(dev, inbox, (u8)slave);
 	adjust_proxy_tun_qkey(dev, vhcr, qpc);
+	err = update_vport_qp_param(dev, inbox, slave);
+	if (err)
+		return err;
 
 	return mlx4_GEN_QP_wrapper(dev, slave, vhcr, inbox, outbox, cmd);
 }
@@ -2934,7 +2979,7 @@ static struct res_gid *find_gid(struct mlx4_dev *dev, int slave,
 
 static int add_mcg_res(struct mlx4_dev *dev, int slave, struct res_qp *rqp,
 		       u8 *gid, enum mlx4_protocol prot,
-		       enum mlx4_steer_type steer)
+		       enum mlx4_steer_type steer, u64 reg_id)
 {
 	struct res_gid *res;
 	int err;
@@ -2951,6 +2996,7 @@ static int add_mcg_res(struct mlx4_dev *dev, int slave, struct res_qp *rqp,
 		memcpy(res->gid, gid, 16);
 		res->prot = prot;
 		res->steer = steer;
+		res->reg_id = reg_id;
 		list_add_tail(&res->list, &rqp->mcg_list);
 		err = 0;
 	}
@@ -2961,7 +3007,7 @@ static int add_mcg_res(struct mlx4_dev *dev, int slave, struct res_qp *rqp,
 
 static int rem_mcg_res(struct mlx4_dev *dev, int slave, struct res_qp *rqp,
 		       u8 *gid, enum mlx4_protocol prot,
-		       enum mlx4_steer_type steer)
+		       enum mlx4_steer_type steer, u64 *reg_id)
 {
 	struct res_gid *res;
 	int err;
@@ -2971,6 +3017,7 @@ static int rem_mcg_res(struct mlx4_dev *dev, int slave, struct res_qp *rqp,
 	if (!res || res->prot != prot || res->steer != steer)
 		err = -EINVAL;
 	else {
+		*reg_id = res->reg_id;
 		list_del(&res->list);
 		kfree(res);
 		err = 0;
@@ -2980,6 +3027,37 @@ static int rem_mcg_res(struct mlx4_dev *dev, int slave, struct res_qp *rqp,
 	return err;
 }
 
+static int qp_attach(struct mlx4_dev *dev, struct mlx4_qp *qp, u8 gid[16],
+		     int block_loopback, enum mlx4_protocol prot,
+		     enum mlx4_steer_type type, u64 *reg_id)
+{
+	switch (dev->caps.steering_mode) {
+	case MLX4_STEERING_MODE_DEVICE_MANAGED:
+		return mlx4_trans_to_dmfs_attach(dev, qp, gid, gid[5],
+						block_loopback, prot,
+						reg_id);
+	case MLX4_STEERING_MODE_B0:
+		return mlx4_qp_attach_common(dev, qp, gid,
+					    block_loopback, prot, type);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int qp_detach(struct mlx4_dev *dev, struct mlx4_qp *qp, u8 gid[16],
+		     enum mlx4_protocol prot, enum mlx4_steer_type type,
+		     u64 reg_id)
+{
+	switch (dev->caps.steering_mode) {
+	case MLX4_STEERING_MODE_DEVICE_MANAGED:
+		return mlx4_flow_detach(dev, reg_id);
+	case MLX4_STEERING_MODE_B0:
+		return mlx4_qp_detach_common(dev, qp, gid, prot, type);
+	default:
+		return -EINVAL;
+	}
+}
+
 int mlx4_QP_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 			       struct mlx4_vhcr *vhcr,
 			       struct mlx4_cmd_mailbox *inbox,
@@ -2992,14 +3070,12 @@ int mlx4_QP_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 	int err;
 	int qpn;
 	struct res_qp *rqp;
+	u64 reg_id = 0;
 	int attach = vhcr->op_modifier;
 	int block_loopback = vhcr->in_modifier >> 31;
 	u8 steer_type_mask = 2;
 	enum mlx4_steer_type type = (gid[7] & steer_type_mask) >> 1;
 
-	if (dev->caps.steering_mode != MLX4_STEERING_MODE_B0)
-		return -EINVAL;
-
 	qpn = vhcr->in_modifier & 0xffffff;
 	err = get_res(dev, slave, qpn, RES_QP, &rqp);
 	if (err)
@@ -3007,30 +3083,32 @@ int mlx4_QP_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 
 	qp.qpn = qpn;
 	if (attach) {
-		err = add_mcg_res(dev, slave, rqp, gid, prot, type);
-		if (err)
+		err = qp_attach(dev, &qp, gid, block_loopback, prot,
+				type, &reg_id);
+		if (err) {
+			pr_err("Fail to attach rule to qp 0x%x\n", qpn);
 			goto ex_put;
-
-		err = mlx4_qp_attach_common(dev, &qp, gid,
-					    block_loopback, prot, type);
+		}
+		err = add_mcg_res(dev, slave, rqp, gid, prot, type, reg_id);
 		if (err)
-			goto ex_rem;
+			goto ex_detach;
 	} else {
-		err = rem_mcg_res(dev, slave, rqp, gid, prot, type);
+		err = rem_mcg_res(dev, slave, rqp, gid, prot, type, &reg_id);
 		if (err)
 			goto ex_put;
-		err = mlx4_qp_detach_common(dev, &qp, gid, prot, type);
-	}
 
+		err = qp_detach(dev, &qp, gid, prot, type, reg_id);
+		if (err)
+			pr_err("Fail to detach rule from qp 0x%x reg_id = 0x%llx\n",
+			       qpn, reg_id);
+	}
 	put_res(dev, slave, qpn, RES_QP);
-	return 0;
+	return err;
 
-ex_rem:
-	/* ignore error return below, already in error */
-	(void) rem_mcg_res(dev, slave, rqp, gid, prot, type);
+ex_detach:
+	qp_detach(dev, &qp, gid, prot, type, reg_id);
 ex_put:
 	put_res(dev, slave, qpn, RES_QP);
-
 	return err;
 }
 
@@ -3266,9 +3344,16 @@ static void detach_qp(struct mlx4_dev *dev, int slave, struct res_qp *rqp)
 	struct mlx4_qp qp; /* dummy for calling attach/detach */
 
 	list_for_each_entry_safe(rgid, tmp, &rqp->mcg_list, list) {
-		qp.qpn = rqp->local_qpn;
-		(void) mlx4_qp_detach_common(dev, &qp, rgid->gid, rgid->prot,
-					     rgid->steer);
+		switch (dev->caps.steering_mode) {
+		case MLX4_STEERING_MODE_DEVICE_MANAGED:
+			mlx4_flow_detach(dev, rgid->reg_id);
+			break;
+		case MLX4_STEERING_MODE_B0:
+			qp.qpn = rqp->local_qpn;
+			(void) mlx4_qp_detach_common(dev, &qp, rgid->gid,
+						     rgid->prot, rgid->steer);
+			break;
+		}
 		list_del(&rgid->list);
 		kfree(rgid);
 	}
diff --git a/drivers/net/ethernet/mellanox/mlx4/srq.c b/drivers/net/ethernet/mellanox/mlx4/srq.c
index e329fe1f11b7..79fd269e2c54 100644
--- a/drivers/net/ethernet/mellanox/mlx4/srq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/srq.c
@@ -298,3 +298,18 @@ void mlx4_cleanup_srq_table(struct mlx4_dev *dev)
 		return;
 	mlx4_bitmap_cleanup(&mlx4_priv(dev)->srq_table.bitmap);
 }
+
+struct mlx4_srq *mlx4_srq_lookup(struct mlx4_dev *dev, u32 srqn)
+{
+	struct mlx4_srq_table *srq_table = &mlx4_priv(dev)->srq_table;
+	struct mlx4_srq *srq;
+	unsigned long flags;
+
+	spin_lock_irqsave(&srq_table->lock, flags);
+	srq = radix_tree_lookup(&srq_table->tree,
+				srqn & (dev->caps.num_srqs - 1));
+	spin_unlock_irqrestore(&srq_table->lock, flags);
+
+	return srq;
+}
+EXPORT_SYMBOL_GPL(mlx4_srq_lookup);
diff --git a/drivers/net/ethernet/micrel/ks8695net.c b/drivers/net/ethernet/micrel/ks8695net.c
index 07a6ebc47c92..b6c60fdef4ff 100644
--- a/drivers/net/ethernet/micrel/ks8695net.c
+++ b/drivers/net/ethernet/micrel/ks8695net.c
@@ -1622,25 +1622,7 @@ static struct platform_driver ks8695_driver = {
 	.resume		= ks8695_drv_resume,
 };
 
-/* Module interface */
-
-static int __init
-ks8695_init(void)
-{
-	printk(KERN_INFO "%s Ethernet driver, V%s\n",
-	       MODULENAME, MODULEVERSION);
-
-	return platform_driver_register(&ks8695_driver);
-}
-
-static void __exit
-ks8695_cleanup(void)
-{
-	platform_driver_unregister(&ks8695_driver);
-}
-
-module_init(ks8695_init);
-module_exit(ks8695_cleanup);
+module_platform_driver(ks8695_driver);
 
 MODULE_AUTHOR("Simtec Electronics");
 MODULE_DESCRIPTION("Micrel KS8695 (Centaur) Ethernet driver");
diff --git a/drivers/net/ethernet/micrel/ks8851.c b/drivers/net/ethernet/micrel/ks8851.c
index 8fb481252e2c..727b546a9eb8 100644
--- a/drivers/net/ethernet/micrel/ks8851.c
+++ b/drivers/net/ethernet/micrel/ks8851.c
@@ -1364,37 +1364,37 @@ static int ks8851_read_selftest(struct ks8851_net *ks)
 
 /* driver bus management functions */
 
-#ifdef CONFIG_PM
-static int ks8851_suspend(struct spi_device *spi, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+
+static int ks8851_suspend(struct device *dev)
 {
-	struct ks8851_net *ks = dev_get_drvdata(&spi->dev);
-	struct net_device *dev = ks->netdev;
+	struct ks8851_net *ks = dev_get_drvdata(dev);
+	struct net_device *netdev = ks->netdev;
 
-	if (netif_running(dev)) {
-		netif_device_detach(dev);
-		ks8851_net_stop(dev);
+	if (netif_running(netdev)) {
+		netif_device_detach(netdev);
+		ks8851_net_stop(netdev);
 	}
 
 	return 0;
 }
 
-static int ks8851_resume(struct spi_device *spi)
+static int ks8851_resume(struct device *dev)
 {
-	struct ks8851_net *ks = dev_get_drvdata(&spi->dev);
-	struct net_device *dev = ks->netdev;
+	struct ks8851_net *ks = dev_get_drvdata(dev);
+	struct net_device *netdev = ks->netdev;
 
-	if (netif_running(dev)) {
-		ks8851_net_open(dev);
-		netif_device_attach(dev);
+	if (netif_running(netdev)) {
+		ks8851_net_open(netdev);
+		netif_device_attach(netdev);
 	}
 
 	return 0;
 }
-#else
-#define ks8851_suspend NULL
-#define ks8851_resume NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(ks8851_pm_ops, ks8851_suspend, ks8851_resume);
+
 static int ks8851_probe(struct spi_device *spi)
 {
 	struct net_device *ndev;
@@ -1456,7 +1456,7 @@ static int ks8851_probe(struct spi_device *spi)
 	SET_ETHTOOL_OPS(ndev, &ks8851_ethtool_ops);
 	SET_NETDEV_DEV(ndev, &spi->dev);
 
-	dev_set_drvdata(&spi->dev, ks);
+	spi_set_drvdata(spi, ks);
 
 	ndev->if_port = IF_PORT_100BASET;
 	ndev->netdev_ops = &ks8851_netdev_ops;
@@ -1516,7 +1516,7 @@ err_irq:
 
 static int ks8851_remove(struct spi_device *spi)
 {
-	struct ks8851_net *priv = dev_get_drvdata(&spi->dev);
+	struct ks8851_net *priv = spi_get_drvdata(spi);
 
 	if (netif_msg_drv(priv))
 		dev_info(&spi->dev, "remove\n");
@@ -1532,25 +1532,12 @@ static struct spi_driver ks8851_driver = {
 	.driver = {
 		.name = "ks8851",
 		.owner = THIS_MODULE,
+		.pm = &ks8851_pm_ops,
 	},
 	.probe = ks8851_probe,
 	.remove = ks8851_remove,
-	.suspend = ks8851_suspend,
-	.resume = ks8851_resume,
 };
-
-static int __init ks8851_init(void)
-{
-	return spi_register_driver(&ks8851_driver);
-}
-
-static void __exit ks8851_exit(void)
-{
-	spi_unregister_driver(&ks8851_driver);
-}
-
-module_init(ks8851_init);
-module_exit(ks8851_exit);
+module_spi_driver(ks8851_driver);
 
 MODULE_DESCRIPTION("KS8851 Network driver");
 MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>");
diff --git a/drivers/net/ethernet/micrel/ks8851_mll.c b/drivers/net/ethernet/micrel/ks8851_mll.c
index a343066f7b43..ddaf138ce0d4 100644
--- a/drivers/net/ethernet/micrel/ks8851_mll.c
+++ b/drivers/net/ethernet/micrel/ks8851_mll.c
@@ -792,20 +792,35 @@ static void ks_rcv(struct ks_net *ks, struct net_device *netdev)
 
 	frame_hdr = ks->frame_head_info;
 	while (ks->frame_cnt--) {
+		if (unlikely(!(frame_hdr->sts & RXFSHR_RXFV) ||
+			     frame_hdr->len >= RX_BUF_SIZE ||
+			     frame_hdr->len <= 0)) {
+
+			/* discard an invalid packet */
+			ks_wrreg16(ks, KS_RXQCR, (ks->rc_rxqcr | RXQCR_RRXEF));
+			netdev->stats.rx_dropped++;
+			if (!(frame_hdr->sts & RXFSHR_RXFV))
+				netdev->stats.rx_frame_errors++;
+			else
+				netdev->stats.rx_length_errors++;
+			frame_hdr++;
+			continue;
+		}
+
 		skb = netdev_alloc_skb(netdev, frame_hdr->len + 16);
-		if (likely(skb && (frame_hdr->sts & RXFSHR_RXFV) &&
-			(frame_hdr->len < RX_BUF_SIZE) && frame_hdr->len)) {
+		if (likely(skb)) {
 			skb_reserve(skb, 2);
 			/* read data block including CRC 4 bytes */
 			ks_read_qmu(ks, (u16 *)skb->data, frame_hdr->len);
-			skb_put(skb, frame_hdr->len);
+			skb_put(skb, frame_hdr->len - 4);
 			skb->protocol = eth_type_trans(skb, netdev);
 			netif_rx(skb);
+			/* exclude CRC size */
+			netdev->stats.rx_bytes += frame_hdr->len - 4;
+			netdev->stats.rx_packets++;
 		} else {
-			pr_err("%s: err:skb alloc\n", __func__);
 			ks_wrreg16(ks, KS_RXQCR, (ks->rc_rxqcr | RXQCR_RRXEF));
-			if (skb)
-				dev_kfree_skb_irq(skb);
+			netdev->stats.rx_dropped++;
 		}
 		frame_hdr++;
 	}
@@ -877,6 +892,8 @@ static irqreturn_t ks_irq(int irq, void *pw)
 		ks_wrreg16(ks, KS_PMECR, pmecr | PMECR_WKEVT_LINK);
 	}
 
+	if (unlikely(status & IRQ_RXOI))
+		ks->netdev->stats.rx_over_errors++;
 	/* this should be the last in IRQ handler*/
 	ks_restore_cmd_reg(ks);
 	return IRQ_HANDLED;
@@ -1015,6 +1032,9 @@ static int ks_start_xmit(struct sk_buff *skb, struct net_device *netdev)
 
 	if (likely(ks_tx_fifo_space(ks) >= skb->len + 12)) {
 		ks_write_qmu(ks, skb->data, skb->len);
+		/* add tx statistics */
+		netdev->stats.tx_bytes += skb->len;
+		netdev->stats.tx_packets++;
 		dev_kfree_skb(skb);
 	} else
 		retv = NETDEV_TX_BUSY;
diff --git a/drivers/net/ethernet/microchip/enc28j60.c b/drivers/net/ethernet/microchip/enc28j60.c
index 5d98a9f7bfc7..c7b40aa21f22 100644
--- a/drivers/net/ethernet/microchip/enc28j60.c
+++ b/drivers/net/ethernet/microchip/enc28j60.c
@@ -1566,7 +1566,7 @@ static int enc28j60_probe(struct spi_device *spi)
 	INIT_WORK(&priv->setrx_work, enc28j60_setrx_work_handler);
 	INIT_WORK(&priv->irq_work, enc28j60_irq_work_handler);
 	INIT_WORK(&priv->restart_work, enc28j60_restart_work_handler);
-	dev_set_drvdata(&spi->dev, priv);	/* spi to priv reference */
+	spi_set_drvdata(spi, priv);	/* spi to priv reference */
 	SET_NETDEV_DEV(dev, &spi->dev);
 
 	if (!enc28j60_chipset_init(dev)) {
@@ -1618,7 +1618,7 @@ error_alloc:
 
 static int enc28j60_remove(struct spi_device *spi)
 {
-	struct enc28j60_net *priv = dev_get_drvdata(&spi->dev);
+	struct enc28j60_net *priv = spi_get_drvdata(spi);
 
 	if (netif_msg_drv(priv))
 		printk(KERN_DEBUG DRV_NAME ": remove\n");
diff --git a/drivers/net/ethernet/myricom/myri10ge/myri10ge.c b/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
index 4f9937e026e5..7be9788ed0f6 100644
--- a/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
+++ b/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
@@ -1281,7 +1281,8 @@ myri10ge_vlan_rx(struct net_device *dev, void *addr, struct sk_buff *skb)
 	va = addr;
 	va += MXGEFW_PAD;
 	veh = (struct vlan_ethhdr *)va;
-	if ((dev->features & NETIF_F_HW_VLAN_RX) == NETIF_F_HW_VLAN_RX &&
+	if ((dev->features & NETIF_F_HW_VLAN_CTAG_RX) ==
+	    NETIF_F_HW_VLAN_CTAG_RX &&
 	    veh->h_vlan_proto == htons(ETH_P_8021Q)) {
 		/* fixup csum if needed */
 		if (skb->ip_summed == CHECKSUM_COMPLETE) {
@@ -1289,7 +1290,7 @@ myri10ge_vlan_rx(struct net_device *dev, void *addr, struct sk_buff *skb)
 			skb->csum = csum_sub(skb->csum, vsum);
 		}
 		/* pop tag */
-		__vlan_hwaccel_put_tag(skb, ntohs(veh->h_vlan_TCI));
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), ntohs(veh->h_vlan_TCI));
 		memmove(va + VLAN_HLEN, va, 2 * ETH_ALEN);
 		skb->len -= VLAN_HLEN;
 		skb->data_len -= VLAN_HLEN;
@@ -3592,10 +3593,9 @@ static int myri10ge_alloc_slices(struct myri10ge_priv *mgp)
 		bytes = mgp->max_intr_slots * sizeof(*ss->rx_done.entry);
 		ss->rx_done.entry = dma_alloc_coherent(&pdev->dev, bytes,
 						       &ss->rx_done.bus,
-						       GFP_KERNEL);
+						       GFP_KERNEL | __GFP_ZERO);
 		if (ss->rx_done.entry == NULL)
 			goto abort;
-		memset(ss->rx_done.entry, 0, bytes);
 		bytes = sizeof(*ss->fw_stats);
 		ss->fw_stats = dma_alloc_coherent(&pdev->dev, bytes,
 						  &ss->fw_stats_bus,
@@ -3888,8 +3888,8 @@ static int myri10ge_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	netdev->mtu = myri10ge_initial_mtu;
 	netdev->hw_features = mgp->features | NETIF_F_RXCSUM;
 
-	/* fake NETIF_F_HW_VLAN_RX for good GRO performance */
-	netdev->hw_features |= NETIF_F_HW_VLAN_RX;
+	/* fake NETIF_F_HW_VLAN_CTAG_RX for good GRO performance */
+	netdev->hw_features |= NETIF_F_HW_VLAN_CTAG_RX;
 
 	netdev->features = netdev->hw_features;
 
diff --git a/drivers/net/ethernet/natsemi/jazzsonic.c b/drivers/net/ethernet/natsemi/jazzsonic.c
index b0b361546365..c20766c2f65b 100644
--- a/drivers/net/ethernet/natsemi/jazzsonic.c
+++ b/drivers/net/ethernet/natsemi/jazzsonic.c
@@ -175,13 +175,13 @@ static int sonic_probe1(struct net_device *dev)
 
 	/* Allocate the entire chunk of memory for the descriptors.
            Note that this cannot cross a 64K boundary. */
-	if ((lp->descriptors = dma_alloc_coherent(lp->device,
-				SIZEOF_SONIC_DESC * SONIC_BUS_SCALE(lp->dma_bitmode),
-				&lp->descriptors_laddr, GFP_KERNEL)) == NULL) {
-		printk(KERN_ERR "%s: couldn't alloc DMA memory for descriptors.\n",
-		       dev_name(lp->device));
+	lp->descriptors = dma_alloc_coherent(lp->device,
+					     SIZEOF_SONIC_DESC *
+					     SONIC_BUS_SCALE(lp->dma_bitmode),
+					     &lp->descriptors_laddr,
+					     GFP_KERNEL);
+	if (lp->descriptors == NULL)
 		goto out;
-	}
 
 	/* Now set up the pointers to point to the appropriate places */
 	lp->cda = lp->descriptors;
diff --git a/drivers/net/ethernet/natsemi/macsonic.c b/drivers/net/ethernet/natsemi/macsonic.c
index 0ffde69c8d01..346a4e025c34 100644
--- a/drivers/net/ethernet/natsemi/macsonic.c
+++ b/drivers/net/ethernet/natsemi/macsonic.c
@@ -202,13 +202,13 @@ static int macsonic_init(struct net_device *dev)
 
 	/* Allocate the entire chunk of memory for the descriptors.
            Note that this cannot cross a 64K boundary. */
-	if ((lp->descriptors = dma_alloc_coherent(lp->device,
-	            SIZEOF_SONIC_DESC * SONIC_BUS_SCALE(lp->dma_bitmode),
-	            &lp->descriptors_laddr, GFP_KERNEL)) == NULL) {
-		printk(KERN_ERR "%s: couldn't alloc DMA memory for descriptors.\n",
-		       dev_name(lp->device));
+	lp->descriptors = dma_alloc_coherent(lp->device,
+					     SIZEOF_SONIC_DESC *
+					     SONIC_BUS_SCALE(lp->dma_bitmode),
+					     &lp->descriptors_laddr,
+					     GFP_KERNEL);
+	if (lp->descriptors == NULL)
 		return -ENOMEM;
-	}
 
 	/* Now set up the pointers to point to the appropriate places */
 	lp->cda = lp->descriptors;
diff --git a/drivers/net/ethernet/natsemi/ns83820.c b/drivers/net/ethernet/natsemi/ns83820.c
index 77c070de621e..d3b47003a575 100644
--- a/drivers/net/ethernet/natsemi/ns83820.c
+++ b/drivers/net/ethernet/natsemi/ns83820.c
@@ -911,7 +911,7 @@ static void rx_irq(struct net_device *ndev)
 				unsigned short tag;
 
 				tag = ntohs(extsts & EXTSTS_VTG_MASK);
-				__vlan_hwaccel_put_tag(skb, tag);
+				__vlan_hwaccel_put_tag(skb, htons(ETH_P_IPV6), tag);
 			}
 #endif
 			rx_rc = netif_rx(skb);
@@ -2193,7 +2193,7 @@ static int ns83820_init_one(struct pci_dev *pci_dev,
 
 #ifdef NS83820_VLAN_ACCEL_SUPPORT
 	/* We also support hardware vlan acceleration */
-	ndev->features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
+	ndev->features |= NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX;
 #endif
 
 	if (using_dac) {
diff --git a/drivers/net/ethernet/natsemi/sonic.c b/drivers/net/ethernet/natsemi/sonic.c
index 46795e403467..1bd419dbda6d 100644
--- a/drivers/net/ethernet/natsemi/sonic.c
+++ b/drivers/net/ethernet/natsemi/sonic.c
@@ -424,7 +424,6 @@ static void sonic_rx(struct net_device *dev)
 			/* Malloc up new buffer. */
 			new_skb = netdev_alloc_skb(dev, SONIC_RBSIZE + 2);
 			if (new_skb == NULL) {
-				printk(KERN_ERR "%s: Memory squeeze, dropping packet.\n", dev->name);
 				lp->stats.rx_dropped++;
 				break;
 			}
diff --git a/drivers/net/ethernet/natsemi/xtsonic.c b/drivers/net/ethernet/natsemi/xtsonic.c
index 5e4748e855f6..c2e0256fe3df 100644
--- a/drivers/net/ethernet/natsemi/xtsonic.c
+++ b/drivers/net/ethernet/natsemi/xtsonic.c
@@ -197,14 +197,12 @@ static int __init sonic_probe1(struct net_device *dev)
 	 *  We also allocate extra space for a pointer to allow freeing
 	 *  this structure later on (in xtsonic_cleanup_module()).
 	 */
-	lp->descriptors =
-		dma_alloc_coherent(lp->device,
-			SIZEOF_SONIC_DESC * SONIC_BUS_SCALE(lp->dma_bitmode),
-			&lp->descriptors_laddr, GFP_KERNEL);
-
+	lp->descriptors = dma_alloc_coherent(lp->device,
+					     SIZEOF_SONIC_DESC *
+					     SONIC_BUS_SCALE(lp->dma_bitmode),
+					     &lp->descriptors_laddr,
+					     GFP_KERNEL);
 	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/neterion/s2io.c b/drivers/net/ethernet/neterion/s2io.c
index bfd887382e19..51b00941302c 100644
--- a/drivers/net/ethernet/neterion/s2io.c
+++ b/drivers/net/ethernet/neterion/s2io.c
@@ -80,6 +80,7 @@
 #include <linux/slab.h>
 #include <linux/prefetch.h>
 #include <net/tcp.h>
+#include <net/checksum.h>
 
 #include <asm/div64.h>
 #include <asm/irq.h>
@@ -7919,7 +7920,7 @@ s2io_init_nic(struct pci_dev *pdev, const struct pci_device_id *pre)
 		NETIF_F_TSO | NETIF_F_TSO6 |
 		NETIF_F_RXCSUM | NETIF_F_LRO;
 	dev->features |= dev->hw_features |
-		NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
+		NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX;
 	if (sp->device_type & XFRAME_II_DEVICE) {
 		dev->hw_features |= NETIF_F_UFO;
 		if (ufo)
@@ -8337,16 +8338,13 @@ static void update_L3L4_header(struct s2io_nic *sp, struct lro *lro)
 {
 	struct iphdr *ip = lro->iph;
 	struct tcphdr *tcp = lro->tcph;
-	__sum16 nchk;
 	struct swStat *swstats = &sp->mac_control.stats_info->sw_stat;
 
 	DBG_PRINT(INFO_DBG, "%s: Been here...\n", __func__);
 
 	/* Update L3 header */
+	csum_replace2(&ip->check, ip->tot_len, htons(lro->total_len));
 	ip->tot_len = htons(lro->total_len);
-	ip->check = 0;
-	nchk = ip_fast_csum((u8 *)lro->iph, ip->ihl);
-	ip->check = nchk;
 
 	/* Update L4 header */
 	tcp->ack_seq = lro->tcp_ack;
@@ -8557,7 +8555,7 @@ static void queue_rx_frame(struct sk_buff *skb, u16 vlan_tag)
 
 	skb->protocol = eth_type_trans(skb, dev);
 	if (vlan_tag && sp->vlan_strip_flag)
-		__vlan_hwaccel_put_tag(skb, vlan_tag);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_tag);
 	if (sp->config.napi)
 		netif_receive_skb(skb);
 	else
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-main.c b/drivers/net/ethernet/neterion/vxge/vxge-main.c
index 794444e09492..cbfaed5f2f8d 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-main.c
+++ b/drivers/net/ethernet/neterion/vxge/vxge-main.c
@@ -312,7 +312,7 @@ vxge_rx_complete(struct vxge_ring *ring, struct sk_buff *skb, u16 vlan,
 
 	if (ext_info->vlan &&
 	    ring->vlan_tag_strip == VXGE_HW_VPATH_RPA_STRIP_VLAN_TAG_ENABLE)
-		__vlan_hwaccel_put_tag(skb, ext_info->vlan);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), ext_info->vlan);
 	napi_gro_receive(ring->napi_p, skb);
 
 	vxge_debug_entryexit(VXGE_TRACE,
@@ -3300,12 +3300,13 @@ static void vxge_tx_watchdog(struct net_device *dev)
 /**
  * vxge_vlan_rx_add_vid
  * @dev: net device pointer.
+ * @proto: vlan protocol
  * @vid: vid
  *
  * Add the vlan id to the devices vlan id table
  */
 static int
-vxge_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
+vxge_vlan_rx_add_vid(struct net_device *dev, __be16 proto, u16 vid)
 {
 	struct vxgedev *vdev = netdev_priv(dev);
 	struct vxge_vpath *vpath;
@@ -3323,14 +3324,15 @@ vxge_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
 }
 
 /**
- * vxge_vlan_rx_add_vid
+ * vxge_vlan_rx_kill_vid
  * @dev: net device pointer.
+ * @proto: vlan protocol
  * @vid: vid
  *
  * Remove the vlan id from the device's vlan id table
  */
 static int
-vxge_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
+vxge_vlan_rx_kill_vid(struct net_device *dev, __be16 proto, u16 vid)
 {
 	struct vxgedev *vdev = netdev_priv(dev);
 	struct vxge_vpath *vpath;
@@ -3415,12 +3417,12 @@ static int vxge_device_register(struct __vxge_hw_device *hldev,
 	ndev->hw_features = NETIF_F_RXCSUM | NETIF_F_SG |
 		NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
 		NETIF_F_TSO | NETIF_F_TSO6 |
-		NETIF_F_HW_VLAN_TX;
+		NETIF_F_HW_VLAN_CTAG_TX;
 	if (vdev->config.rth_steering != NO_STEERING)
 		ndev->hw_features |= NETIF_F_RXHASH;
 
 	ndev->features |= ndev->hw_features |
-		NETIF_F_HW_VLAN_RX | NETIF_F_HW_VLAN_FILTER;
+		NETIF_F_HW_VLAN_CTAG_RX | NETIF_F_HW_VLAN_CTAG_FILTER;
 
 
 	ndev->netdev_ops = &vxge_netdev_ops;
diff --git a/drivers/net/ethernet/netx-eth.c b/drivers/net/ethernet/netx-eth.c
index 63e7af44366f..cb9e63831500 100644
--- a/drivers/net/ethernet/netx-eth.c
+++ b/drivers/net/ethernet/netx-eth.c
@@ -152,8 +152,6 @@ static void netx_eth_receive(struct net_device *ndev)
 
 	skb = netdev_alloc_skb(ndev, len);
 	if (unlikely(skb == NULL)) {
-		printk(KERN_NOTICE "%s: Low memory, packet dropped.\n",
-			ndev->name);
 		ndev->stats.rx_dropped++;
 		return;
 	}
diff --git a/drivers/net/ethernet/nuvoton/w90p910_ether.c b/drivers/net/ethernet/nuvoton/w90p910_ether.c
index 162da8975b05..3df8287b7452 100644
--- a/drivers/net/ethernet/nuvoton/w90p910_ether.c
+++ b/drivers/net/ethernet/nuvoton/w90p910_ether.c
@@ -287,23 +287,16 @@ static int w90p910_init_desc(struct net_device *dev)
 	ether = netdev_priv(dev);
 	pdev = ether->pdev;
 
-	ether->tdesc = (struct tran_pdesc *)
-		dma_alloc_coherent(&pdev->dev, sizeof(struct tran_pdesc),
-					&ether->tdesc_phys, GFP_KERNEL);
-
-	if (!ether->tdesc) {
-		dev_err(&pdev->dev, "Failed to allocate memory for tx desc\n");
+	ether->tdesc = dma_alloc_coherent(&pdev->dev, sizeof(struct tran_pdesc),
+					  &ether->tdesc_phys, GFP_KERNEL);
+	if (!ether->tdesc)
 		return -ENOMEM;
-	}
-
-	ether->rdesc = (struct recv_pdesc *)
-		dma_alloc_coherent(&pdev->dev, sizeof(struct recv_pdesc),
-					&ether->rdesc_phys, GFP_KERNEL);
 
+	ether->rdesc = dma_alloc_coherent(&pdev->dev, sizeof(struct recv_pdesc),
+					  &ether->rdesc_phys, GFP_KERNEL);
 	if (!ether->rdesc) {
-		dev_err(&pdev->dev, "Failed to allocate memory for rx desc\n");
 		dma_free_coherent(&pdev->dev, sizeof(struct tran_pdesc),
-					ether->tdesc, ether->tdesc_phys);
+				  ether->tdesc, ether->tdesc_phys);
 		return -ENOMEM;
 	}
 
@@ -737,7 +730,6 @@ static void netdev_rx(struct net_device *dev)
 			data = ether->rdesc->recv_buf[ether->cur_rx];
 			skb = netdev_alloc_skb(dev, length + 2);
 			if (!skb) {
-				dev_err(&pdev->dev, "get skb buffer error\n");
 				ether->stats.rx_dropped++;
 				return;
 			}
diff --git a/drivers/net/ethernet/nvidia/forcedeth.c b/drivers/net/ethernet/nvidia/forcedeth.c
index 0b8de12bcbca..b003fe53c8e2 100644
--- a/drivers/net/ethernet/nvidia/forcedeth.c
+++ b/drivers/net/ethernet/nvidia/forcedeth.c
@@ -2200,6 +2200,7 @@ static netdev_tx_t nv_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct ring_desc *start_tx;
 	struct ring_desc *prev_tx;
 	struct nv_skb_map *prev_tx_ctx;
+	struct nv_skb_map *tmp_tx_ctx = NULL, *start_tx_ctx = NULL;
 	unsigned long flags;
 
 	/* add fragments to entries count */
@@ -2261,12 +2262,31 @@ static netdev_tx_t nv_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		do {
 			prev_tx = put_tx;
 			prev_tx_ctx = np->put_tx_ctx;
+			if (!start_tx_ctx)
+				start_tx_ctx = tmp_tx_ctx = np->put_tx_ctx;
+
 			bcnt = (frag_size > NV_TX2_TSO_MAX_SIZE) ? NV_TX2_TSO_MAX_SIZE : frag_size;
 			np->put_tx_ctx->dma = skb_frag_dma_map(
 							&np->pci_dev->dev,
 							frag, offset,
 							bcnt,
 							DMA_TO_DEVICE);
+			if (dma_mapping_error(&np->pci_dev->dev, np->put_tx_ctx->dma)) {
+
+				/* Unwind the mapped fragments */
+				do {
+					nv_unmap_txskb(np, start_tx_ctx);
+					if (unlikely(tmp_tx_ctx++ == np->last_tx_ctx))
+						tmp_tx_ctx = np->first_tx_ctx;
+				} while (tmp_tx_ctx != np->put_tx_ctx);
+				kfree_skb(skb);
+				np->put_tx_ctx = start_tx_ctx;
+				u64_stats_update_begin(&np->swstats_tx_syncp);
+				np->stat_tx_dropped++;
+				u64_stats_update_end(&np->swstats_tx_syncp);
+				return NETDEV_TX_OK;
+			}
+
 			np->put_tx_ctx->dma_len = bcnt;
 			np->put_tx_ctx->dma_single = 0;
 			put_tx->buf = cpu_to_le32(np->put_tx_ctx->dma);
@@ -2327,7 +2347,8 @@ static netdev_tx_t nv_start_xmit_optimized(struct sk_buff *skb,
 	struct ring_desc_ex *start_tx;
 	struct ring_desc_ex *prev_tx;
 	struct nv_skb_map *prev_tx_ctx;
-	struct nv_skb_map *start_tx_ctx;
+	struct nv_skb_map *start_tx_ctx = NULL;
+	struct nv_skb_map *tmp_tx_ctx = NULL;
 	unsigned long flags;
 
 	/* add fragments to entries count */
@@ -2392,11 +2413,29 @@ static netdev_tx_t nv_start_xmit_optimized(struct sk_buff *skb,
 			prev_tx = put_tx;
 			prev_tx_ctx = np->put_tx_ctx;
 			bcnt = (frag_size > NV_TX2_TSO_MAX_SIZE) ? NV_TX2_TSO_MAX_SIZE : frag_size;
+			if (!start_tx_ctx)
+				start_tx_ctx = tmp_tx_ctx = np->put_tx_ctx;
 			np->put_tx_ctx->dma = skb_frag_dma_map(
 							&np->pci_dev->dev,
 							frag, offset,
 							bcnt,
 							DMA_TO_DEVICE);
+
+			if (dma_mapping_error(&np->pci_dev->dev, np->put_tx_ctx->dma)) {
+
+				/* Unwind the mapped fragments */
+				do {
+					nv_unmap_txskb(np, start_tx_ctx);
+					if (unlikely(tmp_tx_ctx++ == np->last_tx_ctx))
+						tmp_tx_ctx = np->first_tx_ctx;
+				} while (tmp_tx_ctx != np->put_tx_ctx);
+				kfree_skb(skb);
+				np->put_tx_ctx = start_tx_ctx;
+				u64_stats_update_begin(&np->swstats_tx_syncp);
+				np->stat_tx_dropped++;
+				u64_stats_update_end(&np->swstats_tx_syncp);
+				return NETDEV_TX_OK;
+			}
 			np->put_tx_ctx->dma_len = bcnt;
 			np->put_tx_ctx->dma_single = 0;
 			put_tx->bufhigh = cpu_to_le32(dma_high(np->put_tx_ctx->dma));
@@ -2922,15 +2961,15 @@ static int nv_rx_process_optimized(struct net_device *dev, int limit)
 			vlanflags = le32_to_cpu(np->get_rx.ex->buflow);
 
 			/*
-			 * There's need to check for NETIF_F_HW_VLAN_RX here.
-			 * Even if vlan rx accel is disabled,
+			 * There's need to check for NETIF_F_HW_VLAN_CTAG_RX
+			 * here. Even if vlan rx accel is disabled,
 			 * NV_RX3_VLAN_TAG_PRESENT is pseudo randomly set.
 			 */
-			if (dev->features & NETIF_F_HW_VLAN_RX &&
+			if (dev->features & NETIF_F_HW_VLAN_CTAG_RX &&
 			    vlanflags & NV_RX3_VLAN_TAG_PRESENT) {
 				u16 vid = vlanflags & NV_RX3_VLAN_TAG_MASK;
 
-				__vlan_hwaccel_put_tag(skb, vid);
+				__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
 			}
 			napi_gro_receive(&np->napi, skb);
 			u64_stats_update_begin(&np->swstats_rx_syncp);
@@ -4777,7 +4816,7 @@ static netdev_features_t nv_fix_features(struct net_device *dev,
 	netdev_features_t features)
 {
 	/* vlan is dependent on rx checksum offload */
-	if (features & (NETIF_F_HW_VLAN_TX|NETIF_F_HW_VLAN_RX))
+	if (features & (NETIF_F_HW_VLAN_CTAG_TX|NETIF_F_HW_VLAN_CTAG_RX))
 		features |= NETIF_F_RXCSUM;
 
 	return features;
@@ -4789,12 +4828,12 @@ static void nv_vlan_mode(struct net_device *dev, netdev_features_t features)
 
 	spin_lock_irq(&np->lock);
 
-	if (features & NETIF_F_HW_VLAN_RX)
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
 		np->txrxctl_bits |= NVREG_TXRXCTL_VLANSTRIP;
 	else
 		np->txrxctl_bits &= ~NVREG_TXRXCTL_VLANSTRIP;
 
-	if (features & NETIF_F_HW_VLAN_TX)
+	if (features & NETIF_F_HW_VLAN_CTAG_TX)
 		np->txrxctl_bits |= NVREG_TXRXCTL_VLANINS;
 	else
 		np->txrxctl_bits &= ~NVREG_TXRXCTL_VLANINS;
@@ -4831,7 +4870,7 @@ static int nv_set_features(struct net_device *dev, netdev_features_t features)
 		spin_unlock_irq(&np->lock);
 	}
 
-	if (changed & (NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX))
+	if (changed & (NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX))
 		nv_vlan_mode(dev, features);
 
 	return 0;
@@ -5025,7 +5064,6 @@ static int nv_loopback_test(struct net_device *dev)
 	pkt_len = ETH_DATA_LEN;
 	tx_skb = netdev_alloc_skb(dev, pkt_len);
 	if (!tx_skb) {
-		netdev_err(dev, "netdev_alloc_skb() failed during loopback test\n");
 		ret = 0;
 		goto out;
 	}
@@ -5667,7 +5705,8 @@ static int nv_probe(struct pci_dev *pci_dev, const struct pci_device_id *id)
 	np->vlanctl_bits = 0;
 	if (id->driver_data & DEV_HAS_VLAN) {
 		np->vlanctl_bits = NVREG_VLANCONTROL_ENABLE;
-		dev->hw_features |= NETIF_F_HW_VLAN_RX | NETIF_F_HW_VLAN_TX;
+		dev->hw_features |= NETIF_F_HW_VLAN_CTAG_RX |
+				    NETIF_F_HW_VLAN_CTAG_TX;
 	}
 
 	dev->features |= dev->hw_features;
@@ -5958,7 +5997,8 @@ static int nv_probe(struct pci_dev *pci_dev, const struct pci_device_id *id)
 		 dev->features & NETIF_F_HIGHDMA ? "highdma " : "",
 		 dev->features & (NETIF_F_IP_CSUM | NETIF_F_SG) ?
 			"csum " : "",
-		 dev->features & (NETIF_F_HW_VLAN_RX | NETIF_F_HW_VLAN_TX) ?
+		 dev->features & (NETIF_F_HW_VLAN_CTAG_RX |
+				  NETIF_F_HW_VLAN_CTAG_TX) ?
 			"vlan " : "",
 		 dev->features & (NETIF_F_LOOPBACK) ?
 			"loopback " : "",
diff --git a/drivers/net/ethernet/nxp/lpc_eth.c b/drivers/net/ethernet/nxp/lpc_eth.c
index efa29b712d5f..55a5548d6add 100644
--- a/drivers/net/ethernet/nxp/lpc_eth.c
+++ b/drivers/net/ethernet/nxp/lpc_eth.c
@@ -1409,9 +1409,7 @@ static int lpc_eth_drv_probe(struct platform_device *pdev)
 			dma_alloc_coherent(&pldat->pdev->dev,
 					   pldat->dma_buff_size, &dma_handle,
 					   GFP_KERNEL);
-
 		if (pldat->dma_buff_base_v == NULL) {
-			dev_err(&pdev->dev, "error getting DMA region.\n");
 			ret = -ENOMEM;
 			goto err_out_free_irq;
 		}
@@ -1434,13 +1432,11 @@ static int lpc_eth_drv_probe(struct platform_device *pdev)
 	/* Get MAC address from current HW setting (POR state is all zeros) */
 	__lpc_get_mac(pldat, ndev->dev_addr);
 
-#ifdef CONFIG_OF_NET
 	if (!is_valid_ether_addr(ndev->dev_addr)) {
 		const char *macaddr = of_get_mac_address(pdev->dev.of_node);
 		if (macaddr)
 			memcpy(ndev->dev_addr, macaddr, ETH_ALEN);
 	}
-#endif
 	if (!is_valid_ether_addr(ndev->dev_addr))
 		eth_hw_addr_random(ndev);
 
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 73ce7dd6b954..0c1c65a9ce5e 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
@@ -1469,13 +1469,11 @@ pch_gbe_alloc_rx_buffers_pool(struct pch_gbe_adapter *adapter,
 
 	size = rx_ring->count * bufsz + PCH_GBE_RESERVE_MEMORY;
 	rx_ring->rx_buff_pool = dma_alloc_coherent(&pdev->dev, size,
-						&rx_ring->rx_buff_pool_logic,
-						GFP_KERNEL);
-	if (!rx_ring->rx_buff_pool) {
-		pr_err("Unable to allocate memory for the receive pool buffer\n");
+						   &rx_ring->rx_buff_pool_logic,
+						   GFP_KERNEL | __GFP_ZERO);
+	if (!rx_ring->rx_buff_pool)
 		return -ENOMEM;
-	}
-	memset(rx_ring->rx_buff_pool, 0, size);
+
 	rx_ring->rx_buff_pool_size = size;
 	for (i = 0; i < rx_ring->count; i++) {
 		buffer_info = &rx_ring->buffer_info[i];
@@ -1774,13 +1772,12 @@ int pch_gbe_setup_tx_resources(struct pch_gbe_adapter *adapter,
 	tx_ring->size = tx_ring->count * (int)sizeof(struct pch_gbe_tx_desc);
 
 	tx_ring->desc = dma_alloc_coherent(&pdev->dev, tx_ring->size,
-					   &tx_ring->dma, GFP_KERNEL);
+					   &tx_ring->dma,
+					   GFP_KERNEL | __GFP_ZERO);
 	if (!tx_ring->desc) {
 		vfree(tx_ring->buffer_info);
-		pr_err("Unable to allocate memory for the transmit descriptor ring\n");
 		return -ENOMEM;
 	}
-	memset(tx_ring->desc, 0, tx_ring->size);
 
 	tx_ring->next_to_use = 0;
 	tx_ring->next_to_clean = 0;
@@ -1820,14 +1817,12 @@ int pch_gbe_setup_rx_resources(struct pch_gbe_adapter *adapter,
 
 	rx_ring->size = rx_ring->count * (int)sizeof(struct pch_gbe_rx_desc);
 	rx_ring->desc =	dma_alloc_coherent(&pdev->dev, rx_ring->size,
-					   &rx_ring->dma, GFP_KERNEL);
-
+					   &rx_ring->dma,
+					   GFP_KERNEL | __GFP_ZERO);
 	if (!rx_ring->desc) {
-		pr_err("Unable to allocate memory for the receive descriptor ring\n");
 		vfree(rx_ring->buffer_info);
 		return -ENOMEM;
 	}
-	memset(rx_ring->desc, 0, rx_ring->size);
 	rx_ring->next_to_clean = 0;
 	rx_ring->next_to_use = 0;
 	for (desNo = 0; desNo < rx_ring->count; desNo++) {
@@ -2268,7 +2263,7 @@ static int pch_gbe_change_mtu(struct net_device *netdev, int new_mtu)
 		if (err) {
 			adapter->rx_buffer_len = old_rx_buffer_len;
 			pch_gbe_up(adapter);
-			return -ENOMEM;
+			return err;
 		} else {
 			netdev->mtu = new_mtu;
 			adapter->hw.mac.max_frame_size = max_frame;
diff --git a/drivers/net/ethernet/pasemi/pasemi_mac.c b/drivers/net/ethernet/pasemi/pasemi_mac.c
index b1cfbb75ff1e..a5f0b5da6149 100644
--- a/drivers/net/ethernet/pasemi/pasemi_mac.c
+++ b/drivers/net/ethernet/pasemi/pasemi_mac.c
@@ -441,12 +441,11 @@ static int pasemi_mac_setup_rx_resources(const struct net_device *dev)
 
 	ring->buffers = dma_alloc_coherent(&mac->dma_pdev->dev,
 					   RX_RING_SIZE * sizeof(u64),
-					   &ring->buf_dma, GFP_KERNEL);
+					   &ring->buf_dma,
+					   GFP_KERNEL | __GFP_ZERO);
 	if (!ring->buffers)
 		goto out_ring_desc;
 
-	memset(ring->buffers, 0, RX_RING_SIZE * sizeof(u64));
-
 	write_dma_reg(PAS_DMA_RXCHAN_BASEL(chno),
 		      PAS_DMA_RXCHAN_BASEL_BRBL(ring->chan.ring_dma));
 
diff --git a/drivers/net/ethernet/qlogic/Kconfig b/drivers/net/ethernet/qlogic/Kconfig
index a8669adecc97..0e1797295a48 100644
--- a/drivers/net/ethernet/qlogic/Kconfig
+++ b/drivers/net/ethernet/qlogic/Kconfig
@@ -35,6 +35,16 @@ config QLCNIC
 	  This driver supports QLogic QLE8240 and QLE8242 Converged Ethernet
 	  devices.
 
+config QLCNIC_SRIOV
+	bool "QLOGIC QLCNIC 83XX family SR-IOV Support"
+	depends on QLCNIC && PCI_IOV
+	default y
+	---help---
+	  This configuration parameter enables Single Root Input Output
+	  Virtualization support for QLE83XX Converged Ethernet devices.
+	  This allows for virtual function acceleration in virtualized
+	  environments.
+
 config QLGE
 	tristate "QLogic QLGE 10Gb Ethernet Driver Support"
 	depends on PCI
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic.h b/drivers/net/ethernet/qlogic/netxen/netxen_nic.h
index eb3dfdbb642b..322a36b76727 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic.h
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic.h
@@ -955,9 +955,10 @@ typedef struct nx_mac_list_s {
 	uint8_t mac_addr[ETH_ALEN+2];
 } nx_mac_list_t;
 
-struct nx_vlan_ip_list {
+struct nx_ip_list {
 	struct list_head list;
 	__be32 ip_addr;
+	bool master;
 };
 
 /*
@@ -1605,7 +1606,7 @@ struct netxen_adapter {
 	struct net_device *netdev;
 	struct pci_dev *pdev;
 	struct list_head mac_list;
-	struct list_head vlan_ip_list;
+	struct list_head ip_list;
 
 	spinlock_t tx_clean_lock;
 
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c
index 4782dcfde736..7692dfd4f262 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c
@@ -27,6 +27,7 @@
 #include <linux/delay.h>
 #include <linux/slab.h>
 #include <linux/if_vlan.h>
+#include <net/checksum.h>
 #include "netxen_nic.h"
 #include "netxen_nic_hw.h"
 
@@ -1641,9 +1642,8 @@ netxen_process_lro(struct netxen_adapter *adapter,
 	th = (struct tcphdr *)((skb->data + vhdr_len) + (iph->ihl << 2));
 
 	length = (iph->ihl << 2) + (th->doff << 2) + lro_length;
+	csum_replace2(&iph->check, iph->tot_len, htons(length));
 	iph->tot_len = htons(length);
-	iph->check = 0;
-	iph->check = ip_fast_csum((unsigned char *)iph, iph->ihl);
 	th->psh = push;
 	th->seq = htonl(seq_number);
 
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c
index 501f49207da5..af951f343ff6 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c
@@ -90,7 +90,7 @@ static irqreturn_t netxen_intr(int irq, void *data);
 static irqreturn_t netxen_msi_intr(int irq, void *data);
 static irqreturn_t netxen_msix_intr(int irq, void *data);
 
-static void netxen_free_vlan_ip_list(struct netxen_adapter *);
+static void netxen_free_ip_list(struct netxen_adapter *, bool);
 static void netxen_restore_indev_addr(struct net_device *dev, unsigned long);
 static struct rtnl_link_stats64 *netxen_nic_get_stats(struct net_device *dev,
 						      struct rtnl_link_stats64 *stats);
@@ -1345,7 +1345,7 @@ netxen_setup_netdev(struct netxen_adapter *adapter,
 	}
 
 	if (adapter->capabilities & NX_FW_CAPABILITY_FVLANTX)
-		netdev->hw_features |= NETIF_F_HW_VLAN_TX;
+		netdev->hw_features |= NETIF_F_HW_VLAN_CTAG_TX;
 
 	if (adapter->capabilities & NX_FW_CAPABILITY_HW_LRO)
 		netdev->hw_features |= NETIF_F_LRO;
@@ -1450,7 +1450,7 @@ netxen_nic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	spin_lock_init(&adapter->tx_clean_lock);
 	INIT_LIST_HEAD(&adapter->mac_list);
-	INIT_LIST_HEAD(&adapter->vlan_ip_list);
+	INIT_LIST_HEAD(&adapter->ip_list);
 
 	err = netxen_setup_pci_map(adapter);
 	if (err)
@@ -1585,7 +1585,7 @@ static void netxen_nic_remove(struct pci_dev *pdev)
 
 	cancel_work_sync(&adapter->tx_timeout_task);
 
-	netxen_free_vlan_ip_list(adapter);
+	netxen_free_ip_list(adapter, false);
 	netxen_nic_detach(adapter);
 
 	nx_decr_dev_ref_cnt(adapter);
@@ -3137,62 +3137,77 @@ netxen_destip_supported(struct netxen_adapter *adapter)
 }
 
 static void
-netxen_free_vlan_ip_list(struct netxen_adapter *adapter)
+netxen_free_ip_list(struct netxen_adapter *adapter, bool master)
 {
-	struct nx_vlan_ip_list  *cur;
-	struct list_head *head = &adapter->vlan_ip_list;
+	struct nx_ip_list  *cur, *tmp_cur;
 
-	while (!list_empty(head)) {
-		cur = list_entry(head->next, struct nx_vlan_ip_list, list);
-		netxen_config_ipaddr(adapter, cur->ip_addr, NX_IP_DOWN);
-		list_del(&cur->list);
-		kfree(cur);
+	list_for_each_entry_safe(cur, tmp_cur, &adapter->ip_list, list) {
+		if (master) {
+			if (cur->master) {
+				netxen_config_ipaddr(adapter, cur->ip_addr,
+						     NX_IP_DOWN);
+				list_del(&cur->list);
+				kfree(cur);
+			}
+		} else {
+			netxen_config_ipaddr(adapter, cur->ip_addr, NX_IP_DOWN);
+			list_del(&cur->list);
+			kfree(cur);
+		}
 	}
-
 }
-static void
-netxen_list_config_vlan_ip(struct netxen_adapter *adapter,
+
+static bool
+netxen_list_config_ip(struct netxen_adapter *adapter,
 		struct in_ifaddr *ifa, unsigned long event)
 {
 	struct net_device *dev;
-	struct nx_vlan_ip_list *cur, *tmp_cur;
+	struct nx_ip_list *cur, *tmp_cur;
 	struct list_head *head;
+	bool ret = false;
 
 	dev = ifa->ifa_dev ? ifa->ifa_dev->dev : NULL;
 
 	if (dev == NULL)
-		return;
-
-	if (!is_vlan_dev(dev))
-		return;
+		goto out;
 
 	switch (event) {
 	case NX_IP_UP:
-		list_for_each(head, &adapter->vlan_ip_list) {
-			cur = list_entry(head, struct nx_vlan_ip_list, list);
+		list_for_each(head, &adapter->ip_list) {
+			cur = list_entry(head, struct nx_ip_list, list);
 
 			if (cur->ip_addr == ifa->ifa_address)
-				return;
+				goto out;
 		}
 
-		cur = kzalloc(sizeof(struct nx_vlan_ip_list), GFP_ATOMIC);
+		cur = kzalloc(sizeof(struct nx_ip_list), GFP_ATOMIC);
 		if (cur == NULL)
-			return;
-
+			goto out;
+		if (dev->priv_flags & IFF_802_1Q_VLAN)
+			dev = vlan_dev_real_dev(dev);
+		cur->master = !!netif_is_bond_master(dev);
 		cur->ip_addr = ifa->ifa_address;
-		list_add_tail(&cur->list, &adapter->vlan_ip_list);
+		list_add_tail(&cur->list, &adapter->ip_list);
+		netxen_config_ipaddr(adapter, ifa->ifa_address, NX_IP_UP);
+		ret = true;
 		break;
 	case NX_IP_DOWN:
 		list_for_each_entry_safe(cur, tmp_cur,
-					&adapter->vlan_ip_list, list) {
+					&adapter->ip_list, list) {
 			if (cur->ip_addr == ifa->ifa_address) {
 				list_del(&cur->list);
 				kfree(cur);
+				netxen_config_ipaddr(adapter, ifa->ifa_address,
+						     NX_IP_DOWN);
+				ret = true;
 				break;
 			}
 		}
 	}
+out:
+	return ret;
 }
+
 static void
 netxen_config_indev_addr(struct netxen_adapter *adapter,
 		struct net_device *dev, unsigned long event)
@@ -3209,14 +3224,10 @@ netxen_config_indev_addr(struct netxen_adapter *adapter,
 	for_ifa(indev) {
 		switch (event) {
 		case NETDEV_UP:
-			netxen_config_ipaddr(adapter,
-					ifa->ifa_address, NX_IP_UP);
-			netxen_list_config_vlan_ip(adapter, ifa, NX_IP_UP);
+			netxen_list_config_ip(adapter, ifa, NX_IP_UP);
 			break;
 		case NETDEV_DOWN:
-			netxen_config_ipaddr(adapter,
-					ifa->ifa_address, NX_IP_DOWN);
-			netxen_list_config_vlan_ip(adapter, ifa, NX_IP_DOWN);
+			netxen_list_config_ip(adapter, ifa, NX_IP_DOWN);
 			break;
 		default:
 			break;
@@ -3231,23 +3242,78 @@ netxen_restore_indev_addr(struct net_device *netdev, unsigned long event)
 
 {
 	struct netxen_adapter *adapter = netdev_priv(netdev);
-	struct nx_vlan_ip_list *pos, *tmp_pos;
+	struct nx_ip_list *pos, *tmp_pos;
 	unsigned long ip_event;
 
 	ip_event = (event == NETDEV_UP) ? NX_IP_UP : NX_IP_DOWN;
 	netxen_config_indev_addr(adapter, netdev, event);
 
-	list_for_each_entry_safe(pos, tmp_pos, &adapter->vlan_ip_list, list) {
+	list_for_each_entry_safe(pos, tmp_pos, &adapter->ip_list, list) {
 		netxen_config_ipaddr(adapter, pos->ip_addr, ip_event);
 	}
 }
 
+static inline bool
+netxen_config_checkdev(struct net_device *dev)
+{
+	struct netxen_adapter *adapter;
+
+	if (!is_netxen_netdev(dev))
+		return false;
+	adapter = netdev_priv(dev);
+	if (!adapter)
+		return false;
+	if (!netxen_destip_supported(adapter))
+		return false;
+	if (adapter->is_up != NETXEN_ADAPTER_UP_MAGIC)
+		return false;
+
+	return true;
+}
+
+/**
+ * netxen_config_master - configure addresses based on master
+ * @dev: netxen device
+ * @event: netdev event
+ */
+static void netxen_config_master(struct net_device *dev, unsigned long event)
+{
+	struct net_device *master, *slave;
+	struct netxen_adapter *adapter = netdev_priv(dev);
+
+	rcu_read_lock();
+	master = netdev_master_upper_dev_get_rcu(dev);
+	/*
+	 * This is the case where the netxen nic is being
+	 * enslaved and is dev_open()ed in bond_enslave()
+	 * Now we should program the bond's (and its vlans')
+	 * addresses in the netxen NIC.
+	 */
+	if (master && netif_is_bond_master(master) &&
+	    !netif_is_bond_slave(dev)) {
+		netxen_config_indev_addr(adapter, master, event);
+		for_each_netdev_rcu(&init_net, slave)
+			if (slave->priv_flags & IFF_802_1Q_VLAN &&
+			    vlan_dev_real_dev(slave) == master)
+				netxen_config_indev_addr(adapter, slave, event);
+	}
+	rcu_read_unlock();
+	/*
+	 * This is the case where the netxen nic is being
+	 * released and is dev_close()ed in bond_release()
+	 * just before IFF_BONDING is stripped.
+	 */
+	if (!master && dev->priv_flags & IFF_BONDING)
+		netxen_free_ip_list(adapter, true);
+}
+
 static int netxen_netdev_event(struct notifier_block *this,
 				 unsigned long event, void *ptr)
 {
 	struct netxen_adapter *adapter;
 	struct net_device *dev = (struct net_device *)ptr;
 	struct net_device *orig_dev = dev;
+	struct net_device *slave;
 
 recheck:
 	if (dev == NULL)
@@ -3257,19 +3323,28 @@ recheck:
 		dev = vlan_dev_real_dev(dev);
 		goto recheck;
 	}
-
-	if (!is_netxen_netdev(dev))
-		goto done;
-
-	adapter = netdev_priv(dev);
-
-	if (!adapter)
-		goto done;
-
-	if (adapter->is_up != NETXEN_ADAPTER_UP_MAGIC)
-		goto done;
-
-	netxen_config_indev_addr(adapter, orig_dev, event);
+	if (event == NETDEV_UP || event == NETDEV_DOWN) {
+		/* If this is a bonding device, look for netxen-based slaves*/
+		if (netif_is_bond_master(dev)) {
+			rcu_read_lock();
+			for_each_netdev_in_bond_rcu(dev, slave) {
+				if (!netxen_config_checkdev(slave))
+					continue;
+				adapter = netdev_priv(slave);
+				netxen_config_indev_addr(adapter,
+							 orig_dev, event);
+			}
+			rcu_read_unlock();
+		} else {
+			if (!netxen_config_checkdev(dev))
+				goto done;
+			adapter = netdev_priv(dev);
+			/* Act only if the actual netxen is the target */
+			if (orig_dev == dev)
+				netxen_config_master(dev, event);
+			netxen_config_indev_addr(adapter, orig_dev, event);
+		}
+	}
 done:
 	return NOTIFY_DONE;
 }
@@ -3279,12 +3354,12 @@ netxen_inetaddr_event(struct notifier_block *this,
 		unsigned long event, void *ptr)
 {
 	struct netxen_adapter *adapter;
-	struct net_device *dev;
-
+	struct net_device *dev, *slave;
 	struct in_ifaddr *ifa = (struct in_ifaddr *)ptr;
+	unsigned long ip_event;
 
 	dev = ifa->ifa_dev ? ifa->ifa_dev->dev : NULL;
-
+	ip_event = (event == NETDEV_UP) ? NX_IP_UP : NX_IP_DOWN;
 recheck:
 	if (dev == NULL)
 		goto done;
@@ -3293,31 +3368,24 @@ recheck:
 		dev = vlan_dev_real_dev(dev);
 		goto recheck;
 	}
-
-	if (!is_netxen_netdev(dev))
-		goto done;
-
-	adapter = netdev_priv(dev);
-
-	if (!adapter || !netxen_destip_supported(adapter))
-		goto done;
-
-	if (adapter->is_up != NETXEN_ADAPTER_UP_MAGIC)
-		goto done;
-
-	switch (event) {
-	case NETDEV_UP:
-		netxen_config_ipaddr(adapter, ifa->ifa_address, NX_IP_UP);
-		netxen_list_config_vlan_ip(adapter, ifa, NX_IP_UP);
-		break;
-	case NETDEV_DOWN:
-		netxen_config_ipaddr(adapter, ifa->ifa_address, NX_IP_DOWN);
-		netxen_list_config_vlan_ip(adapter, ifa, NX_IP_DOWN);
-		break;
-	default:
-		break;
+	if (event == NETDEV_UP || event == NETDEV_DOWN) {
+		/* If this is a bonding device, look for netxen-based slaves*/
+		if (netif_is_bond_master(dev)) {
+			rcu_read_lock();
+			for_each_netdev_in_bond_rcu(dev, slave) {
+				if (!netxen_config_checkdev(slave))
+					continue;
+				adapter = netdev_priv(slave);
+				netxen_list_config_ip(adapter, ifa, ip_event);
+			}
+			rcu_read_unlock();
+		} else {
+			if (!netxen_config_checkdev(dev))
+				goto done;
+			adapter = netdev_priv(dev);
+			netxen_list_config_ip(adapter, ifa, ip_event);
+		}
 	}
-
 done:
 	return NOTIFY_DONE;
 }
@@ -3334,7 +3402,7 @@ static void
 netxen_restore_indev_addr(struct net_device *dev, unsigned long event)
 { }
 static void
-netxen_free_vlan_ip_list(struct netxen_adapter *adapter)
+netxen_free_ip_list(struct netxen_adapter *adapter, bool master)
 { }
 #endif
 
diff --git a/drivers/net/ethernet/qlogic/qla3xxx.c b/drivers/net/ethernet/qlogic/qla3xxx.c
index 8fd38cb6d26a..91a8fcd6c246 100644
--- a/drivers/net/ethernet/qlogic/qla3xxx.c
+++ b/drivers/net/ethernet/qlogic/qla3xxx.c
@@ -312,7 +312,6 @@ static void ql_release_to_lrg_buf_free_list(struct ql3_adapter *qdev,
 		lrg_buf_cb->skb = netdev_alloc_skb(qdev->ndev,
 						   qdev->lrg_buffer_len);
 		if (unlikely(!lrg_buf_cb->skb)) {
-			netdev_err(qdev->ndev, "failed netdev_alloc_skb()\n");
 			qdev->lrg_buf_skb_check++;
 		} else {
 			/*
diff --git a/drivers/net/ethernet/qlogic/qlcnic/Makefile b/drivers/net/ethernet/qlogic/qlcnic/Makefile
index 7722a203e388..4b1fb3faa3b7 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/Makefile
+++ b/drivers/net/ethernet/qlogic/qlcnic/Makefile
@@ -8,4 +8,6 @@ qlcnic-y := qlcnic_hw.o qlcnic_main.o qlcnic_init.o \
 	qlcnic_ethtool.o qlcnic_ctx.o qlcnic_io.o \
 	qlcnic_sysfs.o qlcnic_minidump.o qlcnic_83xx_hw.o \
 	qlcnic_83xx_init.o qlcnic_83xx_vnic.o \
-	qlcnic_minidump.o
+	qlcnic_minidump.o qlcnic_sriov_common.o
+
+qlcnic-$(CONFIG_QLCNIC_SRIOV) += qlcnic_sriov_pf.o
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
index ba3c72fce1f2..90c253b145ef 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
@@ -37,9 +37,9 @@
 #include "qlcnic_83xx_hw.h"
 
 #define _QLCNIC_LINUX_MAJOR 5
-#define _QLCNIC_LINUX_MINOR 1
-#define _QLCNIC_LINUX_SUBVERSION 35
-#define QLCNIC_LINUX_VERSIONID  "5.1.35"
+#define _QLCNIC_LINUX_MINOR 2
+#define _QLCNIC_LINUX_SUBVERSION 42
+#define QLCNIC_LINUX_VERSIONID  "5.2.42"
 #define QLCNIC_DRV_IDC_VER  0x01
 #define QLCNIC_DRIVER_VERSION  ((_QLCNIC_LINUX_MAJOR << 16) |\
 		 (_QLCNIC_LINUX_MINOR << 8) | (_QLCNIC_LINUX_SUBVERSION))
@@ -347,8 +347,14 @@ struct qlcnic_rx_buffer {
  * Interrupt coalescing defaults. The defaults are for 1500 MTU. It is
  * adjusted based on configured MTU.
  */
-#define QLCNIC_DEFAULT_INTR_COALESCE_RX_TIME_US	3
-#define QLCNIC_DEFAULT_INTR_COALESCE_RX_PACKETS	256
+#define QLCNIC_INTR_COAL_TYPE_RX		1
+#define QLCNIC_INTR_COAL_TYPE_TX		2
+
+#define QLCNIC_DEF_INTR_COALESCE_RX_TIME_US	3
+#define QLCNIC_DEF_INTR_COALESCE_RX_PACKETS	256
+
+#define QLCNIC_DEF_INTR_COALESCE_TX_TIME_US	64
+#define QLCNIC_DEF_INTR_COALESCE_TX_PACKETS	64
 
 #define QLCNIC_INTR_DEFAULT			0x04
 #define QLCNIC_CONFIG_INTR_COALESCE		3
@@ -359,6 +365,8 @@ struct qlcnic_nic_intr_coalesce {
 	u8	sts_ring_mask;
 	u16	rx_packets;
 	u16	rx_time_us;
+	u16	tx_packets;
+	u16	tx_time_us;
 	u16	flag;
 	u32	timer_out;
 };
@@ -449,6 +457,7 @@ struct qlcnic_hardware_context {
 	struct qlc_83xx_idc idc;
 	struct qlc_83xx_fw_info fw_info;
 	struct qlcnic_intrpt_config *intr_tbl;
+	struct qlcnic_sriov *sriov;
 	u32 *reg_tbl;
 	u32 *ext_reg_tbl;
 	u32 mbox_aen[QLC_83XX_MBX_AEN_CNT];
@@ -510,13 +519,13 @@ struct qlcnic_host_sds_ring {
 	int irq;
 
 	dma_addr_t phys_addr;
-	char name[IFNAMSIZ+4];
+	char name[IFNAMSIZ + 12];
 } ____cacheline_internodealigned_in_smp;
 
 struct qlcnic_host_tx_ring {
 	int irq;
 	void __iomem *crb_intr_mask;
-	char name[IFNAMSIZ+4];
+	char name[IFNAMSIZ + 12];
 	u16 ctx_id;
 	u32 producer;
 	u32 sw_consumer;
@@ -896,6 +905,7 @@ struct qlcnic_ipaddr {
 #define QLCNIC_FW_RESET_OWNER		0x2000
 #define QLCNIC_FW_HANG			0x4000
 #define QLCNIC_FW_LRO_MSS_CAP		0x8000
+#define QLCNIC_TX_INTR_SHARED		0x10000
 #define QLCNIC_IS_MSI_FAMILY(adapter) \
 	((adapter)->flags & (QLCNIC_MSI_ENABLED | QLCNIC_MSIX_ENABLED))
 
@@ -914,7 +924,10 @@ struct qlcnic_ipaddr {
 #define __QLCNIC_AER			5
 #define __QLCNIC_DIAG_RES_ALLOC		6
 #define __QLCNIC_LED_ENABLE		7
-#define __QLCNIC_ELB_INPROGRESS	8
+#define __QLCNIC_ELB_INPROGRESS		8
+#define __QLCNIC_SRIOV_ENABLE		10
+#define __QLCNIC_SRIOV_CAPABLE		11
+#define __QLCNIC_MBX_POLL_ENABLE	12
 
 #define QLCNIC_INTERRUPT_TEST		1
 #define QLCNIC_LOOPBACK_TEST		2
@@ -935,7 +948,7 @@ struct qlcnic_ipaddr {
 struct qlcnic_filter {
 	struct hlist_node fnode;
 	u8 faddr[ETH_ALEN];
-	__le16 vlan_id;
+	u16 vlan_id;
 	unsigned long ftime;
 };
 
@@ -972,9 +985,11 @@ struct qlcnic_adapter {
 	u8 fw_fail_cnt;
 	u8 tx_timeo_cnt;
 	u8 need_fw_reset;
+	u8 reset_ctx_cnt;
 
 	u16 is_up;
-	u16 pvid;
+	u16 rx_pvid;
+	u16 tx_pvid;
 
 	u32 irq;
 	u32 heartbeat;
@@ -1006,9 +1021,11 @@ struct qlcnic_adapter {
 	struct workqueue_struct *qlcnic_wq;
 	struct delayed_work fw_work;
 	struct delayed_work idc_aen_work;
+	struct delayed_work mbx_poll_work;
 
 	struct qlcnic_filter_hash fhash;
 	struct qlcnic_filter_hash rx_fhash;
+	struct list_head vf_mc_list;
 
 	spinlock_t tx_clean_lock;
 	spinlock_t mac_learn_lock;
@@ -1051,7 +1068,11 @@ struct qlcnic_info_le {
 	u8      total_pf;
 	u8      total_rss_engines;
 	__le16  max_vports;
-	u8      reserved2[64];
+	__le16	linkstate_reg_offset;
+	__le16	bit_offsets;
+	__le16  max_local_ipv6_addrs;
+	__le16  max_remote_ipv6_addrs;
+	u8	reserved2[56];
 } __packed;
 
 struct qlcnic_info {
@@ -1083,6 +1104,10 @@ struct qlcnic_info {
 	u8      total_pf;
 	u8      total_rss_engines;
 	u16	max_vports;
+	u16	linkstate_reg_offset;
+	u16	bit_offsets;
+	u16	max_local_ipv6_addrs;
+	u16	max_remote_ipv6_addrs;
 };
 
 struct qlcnic_pci_info_le {
@@ -1348,6 +1373,7 @@ struct _cdrp_cmd {
 struct qlcnic_cmd_args {
 	struct _cdrp_cmd req;
 	struct _cdrp_cmd rsp;
+	int op_type;
 };
 
 int qlcnic_fw_cmd_get_minidump_temp(struct qlcnic_adapter *adapter);
@@ -1430,9 +1456,10 @@ void qlcnic_post_rx_buffers(struct qlcnic_adapter *adapter,
 		struct qlcnic_host_rds_ring *rds_ring, u8 ring_id);
 int qlcnic_process_rcv_ring(struct qlcnic_host_sds_ring *sds_ring, int max);
 void qlcnic_set_multi(struct net_device *netdev);
-int qlcnic_nic_add_mac(struct qlcnic_adapter *, const u8 *);
+void __qlcnic_set_multi(struct net_device *, u16);
+int qlcnic_nic_add_mac(struct qlcnic_adapter *, const u8 *, u16);
 int qlcnic_nic_del_mac(struct qlcnic_adapter *, const u8 *);
-void qlcnic_free_mac_list(struct qlcnic_adapter *adapter);
+void qlcnic_82xx_free_mac_list(struct qlcnic_adapter *adapter);
 
 int qlcnic_fw_cmd_set_mtu(struct qlcnic_adapter *adapter, int mtu);
 int qlcnic_fw_cmd_set_drv_version(struct qlcnic_adapter *);
@@ -1455,7 +1482,7 @@ void qlcnic_diag_free_res(struct net_device *netdev, int max_sds_rings);
 int qlcnic_diag_alloc_res(struct net_device *netdev, int test);
 netdev_tx_t qlcnic_xmit_frame(struct sk_buff *skb, struct net_device *netdev);
 int qlcnic_set_max_rss(struct qlcnic_adapter *, u8, size_t);
-int qlcnic_validate_max_rss(u8, u8);
+int qlcnic_validate_max_rss(struct qlcnic_adapter *, __u32);
 void qlcnic_alloc_lb_filters_mem(struct qlcnic_adapter *adapter);
 int qlcnic_enable_msix(struct qlcnic_adapter *, u32);
 
@@ -1509,8 +1536,13 @@ int qlcnic_init_pci_info(struct qlcnic_adapter *);
 int qlcnic_set_default_offload_settings(struct qlcnic_adapter *);
 int qlcnic_reset_npar_config(struct qlcnic_adapter *);
 int qlcnic_set_eswitch_port_config(struct qlcnic_adapter *);
-void qlcnic_add_lb_filter(struct qlcnic_adapter *, struct sk_buff *, int,
-			  __le16);
+void qlcnic_add_lb_filter(struct qlcnic_adapter *, struct sk_buff *, int, u16);
+int qlcnic_83xx_configure_opmode(struct qlcnic_adapter *adapter);
+int qlcnic_read_mac_addr(struct qlcnic_adapter *);
+int qlcnic_setup_netdev(struct qlcnic_adapter *, struct net_device *, int);
+void qlcnic_sriov_vf_schedule_multi(struct net_device *);
+void qlcnic_vf_add_mc_list(struct net_device *, u16);
+
 /*
  * QLOGIC Board information
  */
@@ -1567,11 +1599,14 @@ struct qlcnic_hardware_ops {
 	int (*create_rx_ctx) (struct qlcnic_adapter *);
 	int (*create_tx_ctx) (struct qlcnic_adapter *,
 	struct qlcnic_host_tx_ring *, int);
+	void (*del_rx_ctx) (struct qlcnic_adapter *);
+	void (*del_tx_ctx) (struct qlcnic_adapter *,
+			    struct qlcnic_host_tx_ring *);
 	int (*setup_link_event) (struct qlcnic_adapter *, int);
 	int (*get_nic_info) (struct qlcnic_adapter *, struct qlcnic_info *, u8);
 	int (*get_pci_info) (struct qlcnic_adapter *, struct qlcnic_pci_info *);
 	int (*set_nic_info) (struct qlcnic_adapter *, struct qlcnic_info *);
-	int (*change_macvlan) (struct qlcnic_adapter *, u8*, __le16, u8);
+	int (*change_macvlan) (struct qlcnic_adapter *, u8*, u16, u8);
 	void (*napi_enable) (struct qlcnic_adapter *);
 	void (*napi_disable) (struct qlcnic_adapter *);
 	void (*config_intr_coal) (struct qlcnic_adapter *);
@@ -1580,8 +1615,9 @@ struct qlcnic_hardware_ops {
 	int (*config_loopback) (struct qlcnic_adapter *, u8);
 	int (*clear_loopback) (struct qlcnic_adapter *, u8);
 	int (*config_promisc_mode) (struct qlcnic_adapter *, u32);
-	void (*change_l2_filter) (struct qlcnic_adapter *, u64 *, __le16);
+	void (*change_l2_filter) (struct qlcnic_adapter *, u64 *, u16);
 	int (*get_board_info) (struct qlcnic_adapter *);
+	void (*free_mac_list) (struct qlcnic_adapter *);
 };
 
 extern struct qlcnic_nic_template qlcnic_vf_ops;
@@ -1635,7 +1671,10 @@ static inline int qlcnic_alloc_mbx_args(struct qlcnic_cmd_args *mbx,
 static inline int qlcnic_issue_cmd(struct qlcnic_adapter *adapter,
 				   struct qlcnic_cmd_args *cmd)
 {
-	return adapter->ahw->hw_ops->mbx_cmd(adapter, cmd);
+	if (adapter->ahw->hw_ops->mbx_cmd)
+		return adapter->ahw->hw_ops->mbx_cmd(adapter, cmd);
+
+	return -EIO;
 }
 
 static inline void qlcnic_get_func_no(struct qlcnic_adapter *adapter)
@@ -1655,12 +1694,14 @@ static inline void qlcnic_api_unlock(struct qlcnic_adapter *adapter)
 
 static inline void qlcnic_add_sysfs(struct qlcnic_adapter *adapter)
 {
-	adapter->ahw->hw_ops->add_sysfs(adapter);
+	if (adapter->ahw->hw_ops->add_sysfs)
+		adapter->ahw->hw_ops->add_sysfs(adapter);
 }
 
 static inline void qlcnic_remove_sysfs(struct qlcnic_adapter *adapter)
 {
-	adapter->ahw->hw_ops->remove_sysfs(adapter);
+	if (adapter->ahw->hw_ops->remove_sysfs)
+		adapter->ahw->hw_ops->remove_sysfs(adapter);
 }
 
 static inline void
@@ -1681,6 +1722,17 @@ static inline int qlcnic_fw_cmd_create_tx_ctx(struct qlcnic_adapter *adapter,
 	return adapter->ahw->hw_ops->create_tx_ctx(adapter, ptr, ring);
 }
 
+static inline void qlcnic_fw_cmd_del_rx_ctx(struct qlcnic_adapter *adapter)
+{
+	return adapter->ahw->hw_ops->del_rx_ctx(adapter);
+}
+
+static inline void qlcnic_fw_cmd_del_tx_ctx(struct qlcnic_adapter *adapter,
+					    struct qlcnic_host_tx_ring *ptr)
+{
+	return adapter->ahw->hw_ops->del_tx_ctx(adapter, ptr);
+}
+
 static inline int qlcnic_linkevent_request(struct qlcnic_adapter *adapter,
 					   int enable)
 {
@@ -1706,7 +1758,7 @@ static inline int qlcnic_set_nic_info(struct qlcnic_adapter *adapter,
 }
 
 static inline int qlcnic_sre_macaddr_change(struct qlcnic_adapter *adapter,
-					    u8 *addr, __le16 id, u8 cmd)
+					    u8 *addr, u16 id, u8 cmd)
 {
 	return adapter->ahw->hw_ops->change_macvlan(adapter, addr, id, cmd);
 }
@@ -1765,7 +1817,7 @@ static inline int qlcnic_nic_set_promisc(struct qlcnic_adapter *adapter,
 }
 
 static inline void qlcnic_change_filter(struct qlcnic_adapter *adapter,
-					u64 *addr, __le16 id)
+					u64 *addr, u16 id)
 {
 	adapter->ahw->hw_ops->change_l2_filter(adapter, addr, id);
 }
@@ -1775,15 +1827,22 @@ static inline int qlcnic_get_board_info(struct qlcnic_adapter *adapter)
 	return adapter->ahw->hw_ops->get_board_info(adapter);
 }
 
+static inline void qlcnic_free_mac_list(struct qlcnic_adapter *adapter)
+{
+	return adapter->ahw->hw_ops->free_mac_list(adapter);
+}
+
 static inline void qlcnic_dev_request_reset(struct qlcnic_adapter *adapter,
 					    u32 key)
 {
-	adapter->nic_ops->request_reset(adapter, key);
+	if (adapter->nic_ops->request_reset)
+		adapter->nic_ops->request_reset(adapter, key);
 }
 
 static inline void qlcnic_cancel_idc_work(struct qlcnic_adapter *adapter)
 {
-	adapter->nic_ops->cancel_idc_work(adapter);
+	if (adapter->nic_ops->cancel_idc_work)
+		adapter->nic_ops->cancel_idc_work(adapter);
 }
 
 static inline irqreturn_t
@@ -1819,6 +1878,7 @@ static inline void qlcnic_enable_int(struct qlcnic_host_sds_ring *sds_ring)
 		writel(0xfbff, adapter->tgt_mask_reg);
 }
 
+extern const struct ethtool_ops qlcnic_sriov_vf_ethtool_ops;
 extern const struct ethtool_ops qlcnic_ethtool_ops;
 extern const struct ethtool_ops qlcnic_ethtool_failed_ops;
 
@@ -1830,7 +1890,9 @@ extern const struct ethtool_ops qlcnic_ethtool_failed_ops;
 	} while (0)
 
 #define PCI_DEVICE_ID_QLOGIC_QLE834X    0x8030
+#define PCI_DEVICE_ID_QLOGIC_VF_QLE834X	0x8430
 #define PCI_DEVICE_ID_QLOGIC_QLE824X	0x8020
+
 static inline bool qlcnic_82xx_check(struct qlcnic_adapter *adapter)
 {
 	unsigned short device = adapter->pdev->device;
@@ -1840,8 +1902,23 @@ static inline bool qlcnic_82xx_check(struct qlcnic_adapter *adapter)
 static inline bool qlcnic_83xx_check(struct qlcnic_adapter *adapter)
 {
 	unsigned short device = adapter->pdev->device;
-	return (device == PCI_DEVICE_ID_QLOGIC_QLE834X) ? true : false;
+	bool status;
+
+	status = ((device == PCI_DEVICE_ID_QLOGIC_QLE834X) ||
+		  (device == PCI_DEVICE_ID_QLOGIC_VF_QLE834X)) ? true : false;
+
+	return status;
 }
 
+static inline bool qlcnic_sriov_pf_check(struct qlcnic_adapter *adapter)
+{
+	return (adapter->ahw->op_mode == QLCNIC_SRIOV_PF_FUNC) ? true : false;
+}
 
+static inline bool qlcnic_sriov_vf_check(struct qlcnic_adapter *adapter)
+{
+	unsigned short device = adapter->pdev->device;
+
+	return (device == PCI_DEVICE_ID_QLOGIC_VF_QLE834X) ? true : false;
+}
 #endif				/* __QLCNIC_H_ */
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c
index edd63f1230f3..ea790a93ee7c 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c
@@ -6,6 +6,7 @@
  */
 
 #include "qlcnic.h"
+#include "qlcnic_sriov.h"
 #include <linux/if_vlan.h>
 #include <linux/ipv6.h>
 #include <linux/ethtool.h>
@@ -13,100 +14,7 @@
 
 #define QLCNIC_MAX_TX_QUEUES		1
 #define RSS_HASHTYPE_IP_TCP		0x3
-
-/* status descriptor mailbox data
- * @phy_addr: physical address of buffer
- * @sds_ring_size: buffer size
- * @intrpt_id: interrupt id
- * @intrpt_val: source of interrupt
- */
-struct qlcnic_sds_mbx {
-	u64	phy_addr;
-	u8	rsvd1[16];
-	u16	sds_ring_size;
-	u16	rsvd2[3];
-	u16	intrpt_id;
-	u8	intrpt_val;
-	u8	rsvd3[5];
-} __packed;
-
-/* receive descriptor buffer data
- * phy_addr_reg: physical address of regular buffer
- * phy_addr_jmb: physical address of jumbo buffer
- * reg_ring_sz: size of regular buffer
- * reg_ring_len: no. of entries in regular buffer
- * jmb_ring_len: no. of entries in jumbo buffer
- * jmb_ring_sz: size of jumbo buffer
- */
-struct qlcnic_rds_mbx {
-	u64	phy_addr_reg;
-	u64	phy_addr_jmb;
-	u16	reg_ring_sz;
-	u16	reg_ring_len;
-	u16	jmb_ring_sz;
-	u16	jmb_ring_len;
-} __packed;
-
-/* host producers for regular and jumbo rings */
-struct __host_producer_mbx {
-	u32	reg_buf;
-	u32	jmb_buf;
-} __packed;
-
-/* Receive context mailbox data outbox registers
- * @state: state of the context
- * @vport_id: virtual port id
- * @context_id: receive context id
- * @num_pci_func: number of pci functions of the port
- * @phy_port: physical port id
- */
-struct qlcnic_rcv_mbx_out {
-	u8	rcv_num;
-	u8	sts_num;
-	u16	ctx_id;
-	u8	state;
-	u8	num_pci_func;
-	u8	phy_port;
-	u8	vport_id;
-	u32	host_csmr[QLCNIC_MAX_RING_SETS];
-	struct __host_producer_mbx host_prod[QLCNIC_MAX_RING_SETS];
-} __packed;
-
-struct qlcnic_add_rings_mbx_out {
-	u8      rcv_num;
-	u8      sts_num;
-	u16  ctx_id;
-	u32  host_csmr[QLCNIC_MAX_RING_SETS];
-	struct __host_producer_mbx host_prod[QLCNIC_MAX_RING_SETS];
-} __packed;
-
-/* Transmit context mailbox inbox registers
- * @phys_addr: DMA address of the transmit buffer
- * @cnsmr_index: host consumer index
- * @size: legth of transmit buffer ring
- * @intr_id: interrput id
- * @src: src of interrupt
- */
-struct qlcnic_tx_mbx {
-	u64	phys_addr;
-	u64	cnsmr_index;
-	u16	size;
-	u16	intr_id;
-	u8	src;
-	u8	rsvd[3];
-} __packed;
-
-/* Transmit context mailbox outbox registers
- * @host_prod: host producer index
- * @ctx_id: transmit context id
- * @state: state of the transmit context
- */
-struct qlcnic_tx_mbx_out {
-	u32	host_prod;
-	u16	ctx_id;
-	u8	state;
-	u8	rsvd;
-} __packed;
+#define QLC_83XX_FW_MBX_CMD		0
 
 static const struct qlcnic_mailbox_metadata qlcnic_83xx_mbx_tbl[] = {
 	{QLCNIC_CMD_CONFIGURE_IP_ADDR, 6, 1},
@@ -156,9 +64,11 @@ static const struct qlcnic_mailbox_metadata qlcnic_83xx_mbx_tbl[] = {
 	{QLCNIC_CMD_SET_LED_CONFIG, 5, 1},
 	{QLCNIC_CMD_GET_LED_CONFIG, 1, 5},
 	{QLCNIC_CMD_ADD_RCV_RINGS, 130, 26},
+	{QLCNIC_CMD_CONFIG_VPORT, 4, 4},
+	{QLCNIC_CMD_BC_EVENT_SETUP, 2, 1},
 };
 
-static const u32 qlcnic_83xx_ext_reg_tbl[] = {
+const u32 qlcnic_83xx_ext_reg_tbl[] = {
 	0x38CC,		/* Global Reset */
 	0x38F0,		/* Wildcard */
 	0x38FC,		/* Informant */
@@ -204,7 +114,7 @@ static const u32 qlcnic_83xx_ext_reg_tbl[] = {
 	0x34A4,		/* QLC_83XX_ASIC_TEMP */
 };
 
-static const u32 qlcnic_83xx_reg_tbl[] = {
+const u32 qlcnic_83xx_reg_tbl[] = {
 	0x34A8,		/* PEG_HALT_STAT1 */
 	0x34AC,		/* PEG_HALT_STAT2 */
 	0x34B0,		/* FW_HEARTBEAT */
@@ -247,6 +157,8 @@ static struct qlcnic_hardware_ops qlcnic_83xx_hw_ops = {
 	.process_lb_rcv_ring_diag	= qlcnic_83xx_process_rcv_ring_diag,
 	.create_rx_ctx			= qlcnic_83xx_create_rx_ctx,
 	.create_tx_ctx			= qlcnic_83xx_create_tx_ctx,
+	.del_rx_ctx			= qlcnic_83xx_del_rx_ctx,
+	.del_tx_ctx			= qlcnic_83xx_del_tx_ctx,
 	.setup_link_event		= qlcnic_83xx_setup_link_event,
 	.get_nic_info			= qlcnic_83xx_get_nic_info,
 	.get_pci_info			= qlcnic_83xx_get_pci_info,
@@ -260,6 +172,7 @@ static struct qlcnic_hardware_ops qlcnic_83xx_hw_ops = {
 	.config_promisc_mode		= qlcnic_83xx_nic_set_promisc,
 	.change_l2_filter		= qlcnic_83xx_change_l2_filter,
 	.get_board_info			= qlcnic_83xx_get_port_info,
+	.free_mac_list			= qlcnic_82xx_free_mac_list,
 };
 
 static struct qlcnic_nic_template qlcnic_83xx_ops = {
@@ -355,14 +268,20 @@ int qlcnic_83xx_setup_intr(struct qlcnic_adapter *adapter, u8 num_intr)
 					      num_intr));
 	/* account for AEN interrupt MSI-X based interrupts */
 	num_msix += 1;
-	num_msix += adapter->max_drv_tx_rings;
+
+	if (!(adapter->flags & QLCNIC_TX_INTR_SHARED))
+		num_msix += adapter->max_drv_tx_rings;
+
 	err = qlcnic_enable_msix(adapter, num_msix);
 	if (err == -ENOMEM)
 		return err;
 	if (adapter->flags & QLCNIC_MSIX_ENABLED)
 		num_msix = adapter->ahw->num_msix;
-	else
+	else {
+		if (qlcnic_sriov_vf_check(adapter))
+			return -EINVAL;
 		num_msix = 1;
+	}
 	/* setup interrupt mapping table for fw */
 	ahw->intr_tbl = vzalloc(num_msix *
 				sizeof(struct qlcnic_intrpt_config));
@@ -421,12 +340,13 @@ inline void qlcnic_83xx_enable_legacy_msix_mbx_intr(struct qlcnic_adapter
 	writel(0, adapter->ahw->pci_base0 + mask);
 }
 
-inline void qlcnic_83xx_disable_mbx_intr(struct qlcnic_adapter *adapter)
+void qlcnic_83xx_disable_mbx_intr(struct qlcnic_adapter *adapter)
 {
 	u32 mask;
 
 	mask = QLCRDX(adapter->ahw, QLCNIC_DEF_INT_MASK);
 	writel(1, adapter->ahw->pci_base0 + mask);
+	QLCWRX(adapter->ahw, QLCNIC_MBX_INTR_ENBL, 0);
 }
 
 static inline void qlcnic_83xx_get_mbx_data(struct qlcnic_adapter *adapter,
@@ -482,7 +402,8 @@ static void qlcnic_83xx_poll_process_aen(struct qlcnic_adapter *adapter)
 
 	event = readl(QLCNIC_MBX_FW(adapter->ahw, 0));
 	if (event &  QLCNIC_MBX_ASYNC_EVENT)
-		qlcnic_83xx_process_aen(adapter);
+		__qlcnic_83xx_process_aen(adapter);
+
 out:
 	qlcnic_83xx_enable_legacy_msix_mbx_intr(adapter);
 	spin_unlock_irqrestore(&adapter->ahw->mbx_lock, flags);
@@ -535,17 +456,15 @@ done:
 
 void qlcnic_83xx_free_mbx_intr(struct qlcnic_adapter *adapter)
 {
-	u32 val = 0, num_msix = adapter->ahw->num_msix - 1;
+	u32 num_msix;
+
+	qlcnic_83xx_disable_mbx_intr(adapter);
 
 	if (adapter->flags & QLCNIC_MSIX_ENABLED)
 		num_msix = adapter->ahw->num_msix - 1;
 	else
 		num_msix = 0;
 
-	QLCWRX(adapter->ahw, QLCNIC_MBX_INTR_ENBL, val);
-
-	qlcnic_83xx_disable_mbx_intr(adapter);
-
 	msleep(20);
 	synchronize_irq(adapter->msix_entries[num_msix].vector);
 	free_irq(adapter->msix_entries[num_msix].vector, adapter);
@@ -595,7 +514,7 @@ int qlcnic_83xx_setup_mbx_intr(struct qlcnic_adapter *adapter)
 void qlcnic_83xx_get_func_no(struct qlcnic_adapter *adapter)
 {
 	u32 val = QLCRDX(adapter->ahw, QLCNIC_INFORMANT);
-	adapter->ahw->pci_func = val & 0xf;
+	adapter->ahw->pci_func = (val >> 24) & 0xff;
 }
 
 int qlcnic_83xx_cam_lock(struct qlcnic_adapter *adapter)
@@ -707,6 +626,11 @@ void qlcnic_83xx_check_vf(struct qlcnic_adapter *adapter,
 	ahw->fw_hal_version = 2;
 	qlcnic_get_func_no(adapter);
 
+	if (qlcnic_sriov_vf_check(adapter)) {
+		qlcnic_sriov_vf_set_ops(adapter);
+		return;
+	}
+
 	/* Determine function privilege level */
 	op_mode = QLCRDX(adapter->ahw, QLC_83XX_DRV_OP_MODE);
 	if (op_mode == QLC_83XX_DEFAULT_OPMODE)
@@ -722,6 +646,9 @@ void qlcnic_83xx_check_vf(struct qlcnic_adapter *adapter,
 			 ahw->fw_hal_version);
 		adapter->nic_ops = &qlcnic_vf_ops;
 	} else {
+		if (pci_find_ext_capability(adapter->pdev,
+					    PCI_EXT_CAP_ID_SRIOV))
+			set_bit(__QLCNIC_SRIOV_CAPABLE, &adapter->state);
 		adapter->nic_ops = &qlcnic_83xx_ops;
 	}
 }
@@ -755,7 +682,7 @@ static void qlcnic_dump_mbx(struct qlcnic_adapter *adapter,
 }
 
 /* Mailbox response for mac rcode */
-static u32 qlcnic_83xx_mac_rcode(struct qlcnic_adapter *adapter)
+u32 qlcnic_83xx_mac_rcode(struct qlcnic_adapter *adapter)
 {
 	u32 fw_data;
 	u8 mac_cmd_rcode;
@@ -769,7 +696,7 @@ static u32 qlcnic_83xx_mac_rcode(struct qlcnic_adapter *adapter)
 	return 1;
 }
 
-static u32 qlcnic_83xx_mbx_poll(struct qlcnic_adapter *adapter)
+u32 qlcnic_83xx_mbx_poll(struct qlcnic_adapter *adapter)
 {
 	u32 data;
 	unsigned long wait_time = 0;
@@ -832,7 +759,7 @@ poll:
 		/* Get the FW response data */
 		fw_data = readl(QLCNIC_MBX_FW(ahw, 0));
 		if (fw_data &  QLCNIC_MBX_ASYNC_EVENT) {
-			qlcnic_83xx_process_aen(adapter);
+			__qlcnic_83xx_process_aen(adapter);
 			mbx_val = QLCRDX(ahw, QLCNIC_HOST_MBX_CTRL);
 			if (mbx_val)
 				goto poll;
@@ -884,6 +811,7 @@ int qlcnic_83xx_alloc_mbx_args(struct qlcnic_cmd_args *mbx,
 	size = ARRAY_SIZE(qlcnic_83xx_mbx_tbl);
 	for (i = 0; i < size; i++) {
 		if (type == mbx_tbl[i].cmd) {
+			mbx->op_type = QLC_83XX_FW_MBX_CMD;
 			mbx->req.num = mbx_tbl[i].in_args;
 			mbx->rsp.num = mbx_tbl[i].out_args;
 			mbx->req.arg = kcalloc(mbx->req.num, sizeof(u32),
@@ -901,10 +829,10 @@ int qlcnic_83xx_alloc_mbx_args(struct qlcnic_cmd_args *mbx,
 			memset(mbx->rsp.arg, 0, sizeof(u32) * mbx->rsp.num);
 			temp = adapter->ahw->fw_hal_version << 29;
 			mbx->req.arg[0] = (type | (mbx->req.num << 16) | temp);
-			break;
+			return 0;
 		}
 	}
-	return 0;
+	return -EINVAL;
 }
 
 void qlcnic_83xx_idc_aen_work(struct work_struct *work)
@@ -935,7 +863,7 @@ static void qlcnic_83xx_handle_idc_comp_aen(struct qlcnic_adapter *adapter,
 	return;
 }
 
-void qlcnic_83xx_process_aen(struct qlcnic_adapter *adapter)
+void __qlcnic_83xx_process_aen(struct qlcnic_adapter *adapter)
 {
 	u32 event[QLC_83XX_MBX_AEN_CNT];
 	int i;
@@ -960,6 +888,9 @@ void qlcnic_83xx_process_aen(struct qlcnic_adapter *adapter)
 		break;
 	case QLCNIC_MBX_TIME_EXTEND_EVENT:
 		break;
+	case QLCNIC_MBX_BC_EVENT:
+		qlcnic_sriov_handle_bc_event(adapter, event[1]);
+		break;
 	case QLCNIC_MBX_SFP_INSERT_EVENT:
 		dev_info(&adapter->pdev->dev, "SFP+ Insert AEN:0x%x.\n",
 			 QLCNIC_MBX_RSP(event[0]));
@@ -977,6 +908,53 @@ void qlcnic_83xx_process_aen(struct qlcnic_adapter *adapter)
 	QLCWRX(ahw, QLCNIC_FW_MBX_CTRL, QLCNIC_CLR_OWNER);
 }
 
+static void qlcnic_83xx_process_aen(struct qlcnic_adapter *adapter)
+{
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
+	u32 resp, event;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ahw->mbx_lock, flags);
+
+	resp = QLCRDX(ahw, QLCNIC_FW_MBX_CTRL);
+	if (resp & QLCNIC_SET_OWNER) {
+		event = readl(QLCNIC_MBX_FW(ahw, 0));
+		if (event &  QLCNIC_MBX_ASYNC_EVENT)
+			__qlcnic_83xx_process_aen(adapter);
+	}
+
+	spin_unlock_irqrestore(&ahw->mbx_lock, flags);
+}
+
+static void qlcnic_83xx_mbx_poll_work(struct work_struct *work)
+{
+	struct qlcnic_adapter *adapter;
+
+	adapter = container_of(work, struct qlcnic_adapter, mbx_poll_work.work);
+
+	if (!test_bit(__QLCNIC_MBX_POLL_ENABLE, &adapter->state))
+		return;
+
+	qlcnic_83xx_process_aen(adapter);
+	queue_delayed_work(adapter->qlcnic_wq, &adapter->mbx_poll_work,
+			   (HZ / 10));
+}
+
+void qlcnic_83xx_enable_mbx_poll(struct qlcnic_adapter *adapter)
+{
+	if (test_and_set_bit(__QLCNIC_MBX_POLL_ENABLE, &adapter->state))
+		return;
+
+	INIT_DELAYED_WORK(&adapter->mbx_poll_work, qlcnic_83xx_mbx_poll_work);
+}
+
+void qlcnic_83xx_disable_mbx_poll(struct qlcnic_adapter *adapter)
+{
+	if (!test_and_clear_bit(__QLCNIC_MBX_POLL_ENABLE, &adapter->state))
+		return;
+	cancel_delayed_work_sync(&adapter->mbx_poll_work);
+}
+
 static int qlcnic_83xx_add_rings(struct qlcnic_adapter *adapter)
 {
 	int index, i, err, sds_mbx_size;
@@ -1004,7 +982,8 @@ static int qlcnic_83xx_add_rings(struct qlcnic_adapter *adapter)
 		sds = &recv_ctx->sds_rings[i];
 		sds->consumer = 0;
 		memset(sds->desc_head, 0, STATUS_DESC_RINGSIZE(sds));
-		sds_mbx.phy_addr = sds->phys_addr;
+		sds_mbx.phy_addr_low = LSD(sds->phys_addr);
+		sds_mbx.phy_addr_high = MSD(sds->phys_addr);
 		sds_mbx.sds_ring_size = sds->num_desc;
 
 		if (adapter->flags & QLCNIC_MSIX_ENABLED)
@@ -1050,6 +1029,32 @@ out:
 	return err;
 }
 
+void qlcnic_83xx_del_rx_ctx(struct qlcnic_adapter *adapter)
+{
+	int err;
+	u32 temp = 0;
+	struct qlcnic_cmd_args cmd;
+	struct qlcnic_recv_context *recv_ctx = adapter->recv_ctx;
+
+	if (qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_DESTROY_RX_CTX))
+		return;
+
+	if (qlcnic_sriov_pf_check(adapter) || qlcnic_sriov_vf_check(adapter))
+		cmd.req.arg[0] |= (0x3 << 29);
+
+	if (qlcnic_sriov_pf_check(adapter))
+		qlcnic_pf_set_interface_id_del_rx_ctx(adapter, &temp);
+
+	cmd.req.arg[1] = recv_ctx->context_id | temp;
+	err = qlcnic_issue_cmd(adapter, &cmd);
+	if (err)
+		dev_err(&adapter->pdev->dev,
+			"Failed to destroy rx ctx in firmware\n");
+
+	recv_ctx->state = QLCNIC_HOST_CTX_STATE_FREED;
+	qlcnic_free_mbx_args(&cmd);
+}
+
 int qlcnic_83xx_create_rx_ctx(struct qlcnic_adapter *adapter)
 {
 	int i, err, index, sds_mbx_size, rds_mbx_size;
@@ -1080,9 +1085,17 @@ int qlcnic_83xx_create_rx_ctx(struct qlcnic_adapter *adapter)
 	/* set mailbox hdr and capabilities */
 	qlcnic_alloc_mbx_args(&cmd, adapter,
 			      QLCNIC_CMD_CREATE_RX_CTX);
+
+	if (qlcnic_sriov_pf_check(adapter) || qlcnic_sriov_vf_check(adapter))
+		cmd.req.arg[0] |= (0x3 << 29);
+
 	cmd.req.arg[1] = cap;
 	cmd.req.arg[5] = 1 | (num_rds << 5) | (num_sds << 8) |
 			 (QLC_83XX_HOST_RDS_MODE_UNIQUE << 16);
+
+	if (qlcnic_sriov_pf_check(adapter))
+		qlcnic_pf_set_interface_id_create_rx_ctx(adapter,
+							 &cmd.req.arg[6]);
 	/* set up status rings, mbx 8-57/87 */
 	index = QLC_83XX_HOST_SDS_MBX_IDX;
 	for (i = 0; i < num_sds; i++) {
@@ -1090,7 +1103,8 @@ int qlcnic_83xx_create_rx_ctx(struct qlcnic_adapter *adapter)
 		sds = &recv_ctx->sds_rings[i];
 		sds->consumer = 0;
 		memset(sds->desc_head, 0, STATUS_DESC_RINGSIZE(sds));
-		sds_mbx.phy_addr = sds->phys_addr;
+		sds_mbx.phy_addr_low = LSD(sds->phys_addr);
+		sds_mbx.phy_addr_high = MSD(sds->phys_addr);
 		sds_mbx.sds_ring_size = sds->num_desc;
 		if (adapter->flags & QLCNIC_MSIX_ENABLED)
 			intrpt_id = ahw->intr_tbl[i].id;
@@ -1110,13 +1124,15 @@ int qlcnic_83xx_create_rx_ctx(struct qlcnic_adapter *adapter)
 	rds = &recv_ctx->rds_rings[0];
 	rds->producer = 0;
 	memset(&rds_mbx, 0, rds_mbx_size);
-	rds_mbx.phy_addr_reg = rds->phys_addr;
+	rds_mbx.phy_addr_reg_low = LSD(rds->phys_addr);
+	rds_mbx.phy_addr_reg_high = MSD(rds->phys_addr);
 	rds_mbx.reg_ring_sz = rds->dma_size;
 	rds_mbx.reg_ring_len = rds->num_desc;
 	/* Jumbo ring */
 	rds = &recv_ctx->rds_rings[1];
 	rds->producer = 0;
-	rds_mbx.phy_addr_jmb = rds->phys_addr;
+	rds_mbx.phy_addr_jmb_low = LSD(rds->phys_addr);
+	rds_mbx.phy_addr_jmb_high = MSD(rds->phys_addr);
 	rds_mbx.jmb_ring_sz = rds->dma_size;
 	rds_mbx.jmb_ring_len = rds->num_desc;
 	buf = &cmd.req.arg[index];
@@ -1163,16 +1179,39 @@ out:
 	return err;
 }
 
+void qlcnic_83xx_del_tx_ctx(struct qlcnic_adapter *adapter,
+			    struct qlcnic_host_tx_ring *tx_ring)
+{
+	struct qlcnic_cmd_args cmd;
+	u32 temp = 0;
+
+	if (qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_DESTROY_TX_CTX))
+		return;
+
+	if (qlcnic_sriov_pf_check(adapter) || qlcnic_sriov_vf_check(adapter))
+		cmd.req.arg[0] |= (0x3 << 29);
+
+	if (qlcnic_sriov_pf_check(adapter))
+		qlcnic_pf_set_interface_id_del_tx_ctx(adapter, &temp);
+
+	cmd.req.arg[1] = tx_ring->ctx_id | temp;
+	if (qlcnic_issue_cmd(adapter, &cmd))
+		dev_err(&adapter->pdev->dev,
+			"Failed to destroy tx ctx in firmware\n");
+	qlcnic_free_mbx_args(&cmd);
+}
+
 int qlcnic_83xx_create_tx_ctx(struct qlcnic_adapter *adapter,
 			      struct qlcnic_host_tx_ring *tx, int ring)
 {
 	int err;
 	u16 msix_id;
-	u32 *buf, intr_mask;
+	u32 *buf, intr_mask, temp = 0;
 	struct qlcnic_cmd_args cmd;
 	struct qlcnic_tx_mbx mbx;
 	struct qlcnic_tx_mbx_out *mbx_out;
 	struct qlcnic_hardware_context *ahw = adapter->ahw;
+	u32 msix_vector;
 
 	/* Reset host resources */
 	tx->producer = 0;
@@ -1182,13 +1221,21 @@ int qlcnic_83xx_create_tx_ctx(struct qlcnic_adapter *adapter,
 	memset(&mbx, 0, sizeof(struct qlcnic_tx_mbx));
 
 	/* setup mailbox inbox registerss */
-	mbx.phys_addr = tx->phys_addr;
-	mbx.cnsmr_index = tx->hw_cons_phys_addr;
+	mbx.phys_addr_low = LSD(tx->phys_addr);
+	mbx.phys_addr_high = MSD(tx->phys_addr);
+	mbx.cnsmr_index_low = LSD(tx->hw_cons_phys_addr);
+	mbx.cnsmr_index_high = MSD(tx->hw_cons_phys_addr);
 	mbx.size = tx->num_desc;
-	if (adapter->flags & QLCNIC_MSIX_ENABLED)
-		msix_id = ahw->intr_tbl[adapter->max_sds_rings + ring].id;
-	else
+	if (adapter->flags & QLCNIC_MSIX_ENABLED) {
+		if (!(adapter->flags & QLCNIC_TX_INTR_SHARED))
+			msix_vector = adapter->max_sds_rings + ring;
+		else
+			msix_vector = adapter->max_sds_rings - 1;
+		msix_id = ahw->intr_tbl[msix_vector].id;
+	} else {
 		msix_id = QLCRDX(ahw, QLCNIC_DEF_INT_ID);
+	}
+
 	if (adapter->ahw->diag_test != QLCNIC_LOOPBACK_TEST)
 		mbx.intr_id = msix_id;
 	else
@@ -1196,8 +1243,15 @@ int qlcnic_83xx_create_tx_ctx(struct qlcnic_adapter *adapter,
 	mbx.src = 0;
 
 	qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_CREATE_TX_CTX);
+
+	if (qlcnic_sriov_pf_check(adapter) || qlcnic_sriov_vf_check(adapter))
+		cmd.req.arg[0] |= (0x3 << 29);
+
+	if (qlcnic_sriov_pf_check(adapter))
+		qlcnic_pf_set_interface_id_create_tx_ctx(adapter, &temp);
+
 	cmd.req.arg[1] = QLCNIC_CAP0_LEGACY_CONTEXT;
-	cmd.req.arg[5] = QLCNIC_MAX_TX_QUEUES;
+	cmd.req.arg[5] = QLCNIC_MAX_TX_QUEUES | temp;
 	buf = &cmd.req.arg[6];
 	memcpy(buf, &mbx, sizeof(struct qlcnic_tx_mbx));
 	/* send the mailbox command*/
@@ -1210,7 +1264,8 @@ int qlcnic_83xx_create_tx_ctx(struct qlcnic_adapter *adapter,
 	mbx_out = (struct qlcnic_tx_mbx_out *)&cmd.rsp.arg[2];
 	tx->crb_cmd_producer = ahw->pci_base0 + mbx_out->host_prod;
 	tx->ctx_id = mbx_out->ctx_id;
-	if (adapter->flags & QLCNIC_MSIX_ENABLED) {
+	if ((adapter->flags & QLCNIC_MSIX_ENABLED) &&
+	    !(adapter->flags & QLCNIC_TX_INTR_SHARED)) {
 		intr_mask = ahw->intr_tbl[adapter->max_sds_rings + ring].src;
 		tx->crb_intr_mask = ahw->pci_base0 + intr_mask;
 	}
@@ -1267,7 +1322,8 @@ static int qlcnic_83xx_diag_alloc_res(struct net_device *netdev, int test)
 
 	if (adapter->ahw->diag_test == QLCNIC_LOOPBACK_TEST) {
 		/* disable and free mailbox interrupt */
-		qlcnic_83xx_free_mbx_intr(adapter);
+		if (!(adapter->flags & QLCNIC_MSIX_ENABLED))
+			qlcnic_83xx_free_mbx_intr(adapter);
 		adapter->ahw->loopback_state = 0;
 		adapter->ahw->hw_ops->setup_link_event(adapter, 1);
 	}
@@ -1295,12 +1351,14 @@ static void qlcnic_83xx_diag_free_res(struct net_device *netdev,
 	qlcnic_detach(adapter);
 
 	if (adapter->ahw->diag_test == QLCNIC_LOOPBACK_TEST) {
-		err = qlcnic_83xx_setup_mbx_intr(adapter);
-		if (err) {
-			dev_err(&adapter->pdev->dev,
-				"%s: failed to setup mbx interrupt\n",
-				__func__);
-			goto out;
+		if (!(adapter->flags & QLCNIC_MSIX_ENABLED)) {
+			err = qlcnic_83xx_setup_mbx_intr(adapter);
+			if (err) {
+				dev_err(&adapter->pdev->dev,
+					"%s: failed to setup mbx interrupt\n",
+					__func__);
+				goto out;
+			}
 		}
 	}
 	adapter->ahw->diag_test = 0;
@@ -1373,12 +1431,60 @@ mbx_err:
 	}
 }
 
+int  qlcnic_83xx_set_led(struct net_device *netdev,
+			 enum ethtool_phys_id_state state)
+{
+	struct qlcnic_adapter *adapter = netdev_priv(netdev);
+	int err = -EIO, active = 1;
+
+	if (adapter->ahw->op_mode == QLCNIC_NON_PRIV_FUNC) {
+		netdev_warn(netdev,
+			    "LED test is not supported in non-privileged mode\n");
+		return -EOPNOTSUPP;
+	}
+
+	switch (state) {
+	case ETHTOOL_ID_ACTIVE:
+		if (test_and_set_bit(__QLCNIC_LED_ENABLE, &adapter->state))
+			return -EBUSY;
+
+		if (test_bit(__QLCNIC_RESETTING, &adapter->state))
+			break;
+
+		err = qlcnic_83xx_config_led(adapter, active, 0);
+		if (err)
+			netdev_err(netdev, "Failed to set LED blink state\n");
+		break;
+	case ETHTOOL_ID_INACTIVE:
+		active = 0;
+
+		if (test_bit(__QLCNIC_RESETTING, &adapter->state))
+			break;
+
+		err = qlcnic_83xx_config_led(adapter, active, 0);
+		if (err)
+			netdev_err(netdev, "Failed to reset LED blink state\n");
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	if (!active || err)
+		clear_bit(__QLCNIC_LED_ENABLE, &adapter->state);
+
+	return err;
+}
+
 void qlcnic_83xx_register_nic_idc_func(struct qlcnic_adapter *adapter,
 				       int enable)
 {
 	struct qlcnic_cmd_args cmd;
 	int status;
 
+	if (qlcnic_sriov_vf_check(adapter))
+		return;
+
 	if (enable) {
 		qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_INIT_NIC_FUNC);
 		cmd.req.arg[1] = BIT_0 | BIT_31;
@@ -1441,24 +1547,35 @@ int qlcnic_83xx_setup_link_event(struct qlcnic_adapter *adapter, int enable)
 	return err;
 }
 
+static void qlcnic_83xx_set_interface_id_promisc(struct qlcnic_adapter *adapter,
+						 u32 *interface_id)
+{
+	if (qlcnic_sriov_pf_check(adapter)) {
+		qlcnic_pf_set_interface_id_promisc(adapter, interface_id);
+	} else {
+		if (!qlcnic_sriov_vf_check(adapter))
+			*interface_id = adapter->recv_ctx->context_id << 16;
+	}
+}
+
 int qlcnic_83xx_nic_set_promisc(struct qlcnic_adapter *adapter, u32 mode)
 {
 	int err;
-	u32 temp;
+	u32 temp = 0;
 	struct qlcnic_cmd_args cmd;
 
 	if (adapter->recv_ctx->state == QLCNIC_HOST_CTX_STATE_FREED)
 		return -EIO;
 
 	qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_CONFIGURE_MAC_RX_MODE);
-	temp = adapter->recv_ctx->context_id << 16;
+	qlcnic_83xx_set_interface_id_promisc(adapter, &temp);
 	cmd.req.arg[1] = (mode ? 1 : 0) | temp;
 	err = qlcnic_issue_cmd(adapter, &cmd);
 	if (err)
 		dev_info(&adapter->pdev->dev,
 			 "Promiscous mode config failed\n");
-	qlcnic_free_mbx_args(&cmd);
 
+	qlcnic_free_mbx_args(&cmd);
 	return err;
 }
 
@@ -1490,7 +1607,9 @@ int qlcnic_83xx_loopback_test(struct net_device *netdev, u8 mode)
 	/* Poll for link up event before running traffic */
 	do {
 		msleep(500);
-		qlcnic_83xx_process_aen(adapter);
+		if (!(adapter->flags & QLCNIC_MSIX_ENABLED))
+			qlcnic_83xx_process_aen(adapter);
+
 		if (loop++ > QLCNIC_ILB_MAX_RCV_LOOP) {
 			dev_info(&adapter->pdev->dev,
 				 "Firmware didn't sent link up event to loopback request\n");
@@ -1550,7 +1669,9 @@ int qlcnic_83xx_set_lb_mode(struct qlcnic_adapter *adapter, u8 mode)
 	/* Wait for Link and IDC Completion AEN */
 	do {
 		msleep(300);
-		qlcnic_83xx_process_aen(adapter);
+		if (!(adapter->flags & QLCNIC_MSIX_ENABLED))
+			qlcnic_83xx_process_aen(adapter);
+
 		if (loop++ > QLCNIC_ILB_MAX_RCV_LOOP) {
 			dev_err(&adapter->pdev->dev,
 				"FW did not generate IDC completion AEN\n");
@@ -1590,7 +1711,9 @@ int qlcnic_83xx_clear_lb_mode(struct qlcnic_adapter *adapter, u8 mode)
 	/* Wait for Link and IDC Completion AEN */
 	do {
 		msleep(300);
-		qlcnic_83xx_process_aen(adapter);
+		if (!(adapter->flags & QLCNIC_MSIX_ENABLED))
+			qlcnic_83xx_process_aen(adapter);
+
 		if (loop++ > QLCNIC_ILB_MAX_RCV_LOOP) {
 			dev_err(&adapter->pdev->dev,
 				"Firmware didn't sent IDC completion AEN\n");
@@ -1604,21 +1727,31 @@ int qlcnic_83xx_clear_lb_mode(struct qlcnic_adapter *adapter, u8 mode)
 	return status;
 }
 
+static void qlcnic_83xx_set_interface_id_ipaddr(struct qlcnic_adapter *adapter,
+						u32 *interface_id)
+{
+	if (qlcnic_sriov_pf_check(adapter)) {
+		qlcnic_pf_set_interface_id_ipaddr(adapter, interface_id);
+	} else {
+		if (!qlcnic_sriov_vf_check(adapter))
+			*interface_id = adapter->recv_ctx->context_id << 16;
+	}
+}
+
 void qlcnic_83xx_config_ipaddr(struct qlcnic_adapter *adapter, __be32 ip,
 			       int mode)
 {
 	int err;
-	u32 temp, temp_ip;
+	u32 temp = 0, temp_ip;
 	struct qlcnic_cmd_args cmd;
 
 	qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_CONFIGURE_IP_ADDR);
-	if (mode == QLCNIC_IP_UP) {
-		temp = adapter->recv_ctx->context_id << 16;
+	qlcnic_83xx_set_interface_id_ipaddr(adapter, &temp);
+
+	if (mode == QLCNIC_IP_UP)
 		cmd.req.arg[1] = 1 | temp;
-	} else {
-		temp = adapter->recv_ctx->context_id << 16;
+	else
 		cmd.req.arg[1] = 2 | temp;
-	}
 
 	/*
 	 * Adapter needs IP address in network byte order.
@@ -1635,6 +1768,7 @@ void qlcnic_83xx_config_ipaddr(struct qlcnic_adapter *adapter, __be32 ip,
 		dev_err(&adapter->netdev->dev,
 			"could not notify %s IP 0x%x request\n",
 			(mode == QLCNIC_IP_UP) ? "Add" : "Remove", ip);
+
 	qlcnic_free_mbx_args(&cmd);
 }
 
@@ -1701,11 +1835,22 @@ int qlcnic_83xx_config_rss(struct qlcnic_adapter *adapter, int enable)
 
 }
 
+static void qlcnic_83xx_set_interface_id_macaddr(struct qlcnic_adapter *adapter,
+						 u32 *interface_id)
+{
+	if (qlcnic_sriov_pf_check(adapter)) {
+		qlcnic_pf_set_interface_id_macaddr(adapter, interface_id);
+	} else {
+		if (!qlcnic_sriov_vf_check(adapter))
+			*interface_id = adapter->recv_ctx->context_id << 16;
+	}
+}
+
 int qlcnic_83xx_sre_macaddr_change(struct qlcnic_adapter *adapter, u8 *addr,
-				   __le16 vlan_id, u8 op)
+				   u16 vlan_id, u8 op)
 {
 	int err;
-	u32 *buf;
+	u32 *buf, temp = 0;
 	struct qlcnic_cmd_args cmd;
 	struct qlcnic_macvlan_mbx mv;
 
@@ -1715,11 +1860,21 @@ int qlcnic_83xx_sre_macaddr_change(struct qlcnic_adapter *adapter, u8 *addr,
 	err = qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_CONFIG_MAC_VLAN);
 	if (err)
 		return err;
-	cmd.req.arg[1] = op | (1 << 8) |
-			(adapter->recv_ctx->context_id << 16);
 
-	mv.vlan = le16_to_cpu(vlan_id);
-	memcpy(&mv.mac, addr, ETH_ALEN);
+	if (vlan_id)
+		op = (op == QLCNIC_MAC_ADD || op == QLCNIC_MAC_VLAN_ADD) ?
+		     QLCNIC_MAC_VLAN_ADD : QLCNIC_MAC_VLAN_DEL;
+
+	cmd.req.arg[1] = op | (1 << 8);
+	qlcnic_83xx_set_interface_id_macaddr(adapter, &temp);
+	cmd.req.arg[1] |= temp;
+	mv.vlan = vlan_id;
+	mv.mac_addr0 = addr[0];
+	mv.mac_addr1 = addr[1];
+	mv.mac_addr2 = addr[2];
+	mv.mac_addr3 = addr[3];
+	mv.mac_addr4 = addr[4];
+	mv.mac_addr5 = addr[5];
 	buf = &cmd.req.arg[2];
 	memcpy(buf, &mv, sizeof(struct qlcnic_macvlan_mbx));
 	err = qlcnic_issue_cmd(adapter, &cmd);
@@ -1732,7 +1887,7 @@ int qlcnic_83xx_sre_macaddr_change(struct qlcnic_adapter *adapter, u8 *addr,
 }
 
 void qlcnic_83xx_change_l2_filter(struct qlcnic_adapter *adapter, u64 *addr,
-				  __le16 vlan_id)
+				  u16 vlan_id)
 {
 	u8 mac[ETH_ALEN];
 	memcpy(&mac, addr, ETH_ALEN);
@@ -1782,7 +1937,7 @@ int qlcnic_83xx_get_mac_address(struct qlcnic_adapter *adapter, u8 *mac)
 void qlcnic_83xx_config_intr_coal(struct qlcnic_adapter *adapter)
 {
 	int err;
-	u32 temp;
+	u16 temp;
 	struct qlcnic_cmd_args cmd;
 	struct qlcnic_nic_intr_coalesce *coal = &adapter->ahw->coal;
 
@@ -1790,10 +1945,18 @@ void qlcnic_83xx_config_intr_coal(struct qlcnic_adapter *adapter)
 		return;
 
 	qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_CONFIG_INTR_COAL);
-	cmd.req.arg[1] = 1 | (adapter->recv_ctx->context_id << 16);
+	if (coal->type == QLCNIC_INTR_COAL_TYPE_RX) {
+		temp = adapter->recv_ctx->context_id;
+		cmd.req.arg[1] = QLCNIC_INTR_COAL_TYPE_RX | temp << 16;
+		temp = coal->rx_time_us;
+		cmd.req.arg[2] = coal->rx_packets | temp << 16;
+	} else if (coal->type == QLCNIC_INTR_COAL_TYPE_TX) {
+		temp = adapter->tx_ring->ctx_id;
+		cmd.req.arg[1] = QLCNIC_INTR_COAL_TYPE_TX | temp << 16;
+		temp = coal->tx_time_us;
+		cmd.req.arg[2] = coal->tx_packets | temp << 16;
+	}
 	cmd.req.arg[3] = coal->flag;
-	temp = coal->rx_time_us << 16;
-	cmd.req.arg[2] = coal->rx_packets | temp;
 	err = qlcnic_issue_cmd(adapter, &cmd);
 	if (err != QLCNIC_RCODE_SUCCESS)
 		dev_info(&adapter->pdev->dev,
@@ -1832,7 +1995,7 @@ irqreturn_t qlcnic_83xx_handle_aen(int irq, void *data)
 
 	event = readl(QLCNIC_MBX_FW(adapter->ahw, 0));
 	if (event &  QLCNIC_MBX_ASYNC_EVENT)
-		qlcnic_83xx_process_aen(adapter);
+		__qlcnic_83xx_process_aen(adapter);
 out:
 	mask = QLCRDX(adapter->ahw, QLCNIC_DEF_INT_MASK);
 	writel(0, adapter->ahw->pci_base0 + mask);
@@ -2008,14 +2171,17 @@ int qlcnic_83xx_get_pci_info(struct qlcnic_adapter *adapter,
 int qlcnic_83xx_config_intrpt(struct qlcnic_adapter *adapter, bool op_type)
 {
 	int i, index, err;
-	bool type;
 	u8 max_ints;
-	u32 val, temp;
+	u32 val, temp, type;
 	struct qlcnic_cmd_args cmd;
 
 	max_ints = adapter->ahw->num_msix - 1;
 	qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_CONFIG_INTRPT);
 	cmd.req.arg[1] = max_ints;
+
+	if (qlcnic_sriov_vf_check(adapter))
+		cmd.req.arg[1] |= (adapter->ahw->pci_func << 8) | BIT_16;
+
 	for (i = 0, index = 2; i < max_ints; i++) {
 		type = op_type ? QLCNIC_INTRPT_ADD : QLCNIC_INTRPT_DEL;
 		val = type | (adapter->ahw->intr_tbl[i].type << 4);
@@ -2169,7 +2335,7 @@ static int qlcnic_83xx_poll_flash_status_reg(struct qlcnic_adapter *adapter)
 	return 0;
 }
 
-static int qlcnic_83xx_enable_flash_write_op(struct qlcnic_adapter *adapter)
+int qlcnic_83xx_enable_flash_write(struct qlcnic_adapter *adapter)
 {
 	int ret;
 	u32 cmd;
@@ -2187,7 +2353,7 @@ static int qlcnic_83xx_enable_flash_write_op(struct qlcnic_adapter *adapter)
 	return 0;
 }
 
-static int qlcnic_83xx_disable_flash_write_op(struct qlcnic_adapter *adapter)
+int qlcnic_83xx_disable_flash_write(struct qlcnic_adapter *adapter)
 {
 	int ret;
 
@@ -2261,7 +2427,7 @@ int qlcnic_83xx_erase_flash_sector(struct qlcnic_adapter *adapter,
 		return -EIO;
 
 	if (adapter->ahw->fdt.mfg_id == adapter->flash_mfg_id) {
-		ret = qlcnic_83xx_enable_flash_write_op(adapter);
+		ret = qlcnic_83xx_enable_flash_write(adapter);
 		if (ret) {
 			qlcnic_83xx_unlock_flash(adapter);
 			dev_err(&adapter->pdev->dev,
@@ -2303,7 +2469,7 @@ int qlcnic_83xx_erase_flash_sector(struct qlcnic_adapter *adapter,
 	}
 
 	if (adapter->ahw->fdt.mfg_id == adapter->flash_mfg_id) {
-		ret = qlcnic_83xx_disable_flash_write_op(adapter);
+		ret = qlcnic_83xx_disable_flash_write(adapter);
 		if (ret) {
 			qlcnic_83xx_unlock_flash(adapter);
 			dev_err(&adapter->pdev->dev,
@@ -2343,8 +2509,8 @@ int qlcnic_83xx_flash_bulk_write(struct qlcnic_adapter *adapter, u32 addr,
 	u32 temp;
 	int ret = -EIO;
 
-	if ((count < QLC_83XX_FLASH_BULK_WRITE_MIN) ||
-	    (count > QLC_83XX_FLASH_BULK_WRITE_MAX)) {
+	if ((count < QLC_83XX_FLASH_WRITE_MIN) ||
+	    (count > QLC_83XX_FLASH_WRITE_MAX)) {
 		dev_err(&adapter->pdev->dev,
 			"%s: Invalid word count\n", __func__);
 		return -EIO;
@@ -2622,13 +2788,19 @@ int qlcnic_83xx_flash_read32(struct qlcnic_adapter *adapter, u32 flash_addr,
 
 int qlcnic_83xx_test_link(struct qlcnic_adapter *adapter)
 {
+	u8 pci_func;
 	int err;
 	u32 config = 0, state;
 	struct qlcnic_cmd_args cmd;
 	struct qlcnic_hardware_context *ahw = adapter->ahw;
 
-	state = readl(ahw->pci_base0 + QLC_83XX_LINK_STATE(ahw->pci_func));
-	if (!QLC_83xx_FUNC_VAL(state, ahw->pci_func)) {
+	if (qlcnic_sriov_vf_check(adapter))
+		pci_func = adapter->portnum;
+	else
+		pci_func = ahw->pci_func;
+
+	state = readl(ahw->pci_base0 + QLC_83XX_LINK_STATE(pci_func));
+	if (!QLC_83xx_FUNC_VAL(state, pci_func)) {
 		dev_info(&adapter->pdev->dev, "link state down\n");
 		return config;
 	}
@@ -2758,6 +2930,9 @@ static u64 *qlcnic_83xx_fill_stats(struct qlcnic_adapter *adapter,
 		/* fill in MAC rx frame stats */
 		for (k += 6; k < 80; k += 2)
 			data = qlcnic_83xx_copy_stats(cmd, data, k);
+		/* fill in eSwitch stats */
+		for (; k < total_regs; k += 2)
+			data = qlcnic_83xx_copy_stats(cmd, data, k);
 		break;
 	case QLC_83XX_STAT_RX:
 		for (k = 2; k < 8; k += 2)
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h
index 61f81f6c84a9..1f1d85e6f2af 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h
@@ -12,6 +12,8 @@
 #include <linux/etherdevice.h>
 #include "qlcnic_hw.h"
 
+#define QLCNIC_83XX_BAR0_LENGTH 0x4000
+
 /* Directly mapped registers */
 #define QLC_83XX_CRB_WIN_BASE		0x3800
 #define QLC_83XX_CRB_WIN_FUNC(f)	(QLC_83XX_CRB_WIN_BASE+((f)*4))
@@ -86,6 +88,153 @@
 
 #define QLC_83XX_MAX_RESET_SEQ_ENTRIES	16
 
+/* status descriptor mailbox data
+ * @phy_addr_{low|high}: physical address of buffer
+ * @sds_ring_size: buffer size
+ * @intrpt_id: interrupt id
+ * @intrpt_val: source of interrupt
+ */
+struct qlcnic_sds_mbx {
+	u32	phy_addr_low;
+	u32	phy_addr_high;
+	u32	rsvd1[4];
+#if defined(__LITTLE_ENDIAN)
+	u16	sds_ring_size;
+	u16	rsvd2;
+	u16	rsvd3[2];
+	u16	intrpt_id;
+	u8	intrpt_val;
+	u8	rsvd4;
+#elif defined(__BIG_ENDIAN)
+	u16	rsvd2;
+	u16	sds_ring_size;
+	u16	rsvd3[2];
+	u8	rsvd4;
+	u8	intrpt_val;
+	u16	intrpt_id;
+#endif
+	u32	rsvd5;
+} __packed;
+
+/* receive descriptor buffer data
+ * phy_addr_reg_{low|high}: physical address of regular buffer
+ * phy_addr_jmb_{low|high}: physical address of jumbo buffer
+ * reg_ring_sz: size of regular buffer
+ * reg_ring_len: no. of entries in regular buffer
+ * jmb_ring_len: no. of entries in jumbo buffer
+ * jmb_ring_sz: size of jumbo buffer
+ */
+struct qlcnic_rds_mbx {
+	u32	phy_addr_reg_low;
+	u32	phy_addr_reg_high;
+	u32	phy_addr_jmb_low;
+	u32	phy_addr_jmb_high;
+#if defined(__LITTLE_ENDIAN)
+	u16	reg_ring_sz;
+	u16	reg_ring_len;
+	u16	jmb_ring_sz;
+	u16	jmb_ring_len;
+#elif defined(__BIG_ENDIAN)
+	u16	reg_ring_len;
+	u16	reg_ring_sz;
+	u16	jmb_ring_len;
+	u16	jmb_ring_sz;
+#endif
+} __packed;
+
+/* host producers for regular and jumbo rings */
+struct __host_producer_mbx {
+	u32	reg_buf;
+	u32	jmb_buf;
+} __packed;
+
+/* Receive context mailbox data outbox registers
+ * @state: state of the context
+ * @vport_id: virtual port id
+ * @context_id: receive context id
+ * @num_pci_func: number of pci functions of the port
+ * @phy_port: physical port id
+ */
+struct qlcnic_rcv_mbx_out {
+#if defined(__LITTLE_ENDIAN)
+	u8	rcv_num;
+	u8	sts_num;
+	u16	ctx_id;
+	u8	state;
+	u8	num_pci_func;
+	u8	phy_port;
+	u8	vport_id;
+#elif defined(__BIG_ENDIAN)
+	u16	ctx_id;
+	u8	sts_num;
+	u8	rcv_num;
+	u8	vport_id;
+	u8	phy_port;
+	u8	num_pci_func;
+	u8	state;
+#endif
+	u32	host_csmr[QLCNIC_MAX_RING_SETS];
+	struct __host_producer_mbx host_prod[QLCNIC_MAX_RING_SETS];
+} __packed;
+
+struct qlcnic_add_rings_mbx_out {
+#if defined(__LITTLE_ENDIAN)
+	u8      rcv_num;
+	u8      sts_num;
+	u16	ctx_id;
+#elif defined(__BIG_ENDIAN)
+	u16	ctx_id;
+	u8	sts_num;
+	u8	rcv_num;
+#endif
+	u32  host_csmr[QLCNIC_MAX_RING_SETS];
+	struct __host_producer_mbx host_prod[QLCNIC_MAX_RING_SETS];
+} __packed;
+
+/* Transmit context mailbox inbox registers
+ * @phys_addr_{low|high}: DMA address of the transmit buffer
+ * @cnsmr_index_{low|high}: host consumer index
+ * @size: legth of transmit buffer ring
+ * @intr_id: interrput id
+ * @src: src of interrupt
+ */
+struct qlcnic_tx_mbx {
+	u32	phys_addr_low;
+	u32	phys_addr_high;
+	u32	cnsmr_index_low;
+	u32	cnsmr_index_high;
+#if defined(__LITTLE_ENDIAN)
+	u16	size;
+	u16	intr_id;
+	u8	src;
+	u8	rsvd[3];
+#elif defined(__BIG_ENDIAN)
+	u16	intr_id;
+	u16	size;
+	u8	rsvd[3];
+	u8	src;
+#endif
+} __packed;
+
+/* Transmit context mailbox outbox registers
+ * @host_prod: host producer index
+ * @ctx_id: transmit context id
+ * @state: state of the transmit context
+ */
+
+struct qlcnic_tx_mbx_out {
+	u32	host_prod;
+#if defined(__LITTLE_ENDIAN)
+	u16	ctx_id;
+	u8	state;
+	u8	rsvd;
+#elif defined(__BIG_ENDIAN)
+	u8	rsvd;
+	u8	state;
+	u16	ctx_id;
+#endif
+} __packed;
+
 struct qlcnic_intrpt_config {
 	u8	type;
 	u8	enabled;
@@ -94,8 +243,23 @@ struct qlcnic_intrpt_config {
 };
 
 struct qlcnic_macvlan_mbx {
-	u8	mac[ETH_ALEN];
+#if defined(__LITTLE_ENDIAN)
+	u8	mac_addr0;
+	u8	mac_addr1;
+	u8	mac_addr2;
+	u8	mac_addr3;
+	u8	mac_addr4;
+	u8	mac_addr5;
 	u16	vlan;
+#elif defined(__BIG_ENDIAN)
+	u8	mac_addr3;
+	u8	mac_addr2;
+	u8	mac_addr1;
+	u8	mac_addr0;
+	u16	vlan;
+	u8	mac_addr5;
+	u8	mac_addr4;
+#endif
 };
 
 struct qlc_83xx_fw_info {
@@ -153,6 +317,18 @@ struct qlc_83xx_idc {
 	char		**name;
 };
 
+/* Device States */
+enum qlcnic_83xx_states {
+	QLC_83XX_IDC_DEV_UNKNOWN,
+	QLC_83XX_IDC_DEV_COLD,
+	QLC_83XX_IDC_DEV_INIT,
+	QLC_83XX_IDC_DEV_READY,
+	QLC_83XX_IDC_DEV_NEED_RESET,
+	QLC_83XX_IDC_DEV_NEED_QUISCENT,
+	QLC_83XX_IDC_DEV_FAILED,
+	QLC_83XX_IDC_DEV_QUISCENT
+};
+
 #define QLCNIC_MBX_RSP(reg)		LSW(reg)
 #define QLCNIC_MBX_NUM_REGS(reg)	(MSW(reg) & 0x1FF)
 #define QLCNIC_MBX_STATUS(reg)		(((reg) >> 25) & 0x7F)
@@ -205,7 +381,7 @@ struct qlc_83xx_idc {
 #define QLC_83XX_STAT_MAC	1
 #define QLC_83XX_TX_STAT_REGS	14
 #define QLC_83XX_RX_STAT_REGS	40
-#define QLC_83XX_MAC_STAT_REGS	80
+#define QLC_83XX_MAC_STAT_REGS	94
 
 #define QLC_83XX_GET_FUNC_PRIVILEGE(VAL, FN)	(0x3 & ((VAL) >> (FN * 2)))
 #define QLC_83XX_SET_FUNC_OPMODE(VAL, FN)	((VAL) << (FN * 2))
@@ -226,6 +402,7 @@ struct qlc_83xx_idc {
 #define QLC_83XX_GET_FW_LRO_MSS_CAPABILITY(val)	(val & 0x20000)
 #define QLC_83XX_VIRTUAL_NIC_MODE			0xFF
 #define QLC_83XX_DEFAULT_MODE				0x0
+#define QLC_83XX_SRIOV_MODE				0x1
 #define QLCNIC_BRDTYPE_83XX_10G			0x0083
 
 #define QLC_83XX_FLASH_SPI_STATUS		0x2808E010
@@ -242,8 +419,8 @@ struct qlc_83xx_idc {
 #define QLC_83XX_FLASH_BULK_WRITE_CMD		0xcadcadca
 #define QLC_83XX_FLASH_READ_RETRY_COUNT	5000
 #define QLC_83XX_FLASH_STATUS_READY		0x6
-#define QLC_83XX_FLASH_BULK_WRITE_MIN		2
-#define QLC_83XX_FLASH_BULK_WRITE_MAX		64
+#define QLC_83XX_FLASH_WRITE_MIN		2
+#define QLC_83XX_FLASH_WRITE_MAX		64
 #define QLC_83XX_FLASH_STATUS_REG_POLL_DELAY	1
 #define QLC_83XX_ERASE_MODE			1
 #define QLC_83XX_WRITE_MODE			2
@@ -336,7 +513,7 @@ int qlcnic_83xx_clear_lb_mode(struct qlcnic_adapter *, u8);
 int qlcnic_83xx_config_hw_lro(struct qlcnic_adapter *, int);
 int qlcnic_83xx_config_rss(struct qlcnic_adapter *, int);
 int qlcnic_83xx_config_intr_coalesce(struct qlcnic_adapter *);
-void qlcnic_83xx_change_l2_filter(struct qlcnic_adapter *, u64 *, __le16);
+void qlcnic_83xx_change_l2_filter(struct qlcnic_adapter *, u64 *, u16);
 int qlcnic_83xx_get_pci_info(struct qlcnic_adapter *, struct qlcnic_pci_info *);
 int qlcnic_83xx_set_nic_info(struct qlcnic_adapter *, struct qlcnic_info *);
 void qlcnic_83xx_register_nic_idc_func(struct qlcnic_adapter *, int);
@@ -351,11 +528,14 @@ int qlcnic_ind_rd(struct qlcnic_adapter *, u32);
 int qlcnic_83xx_create_rx_ctx(struct qlcnic_adapter *);
 int qlcnic_83xx_create_tx_ctx(struct qlcnic_adapter *,
 			      struct qlcnic_host_tx_ring *, int);
+void qlcnic_83xx_del_rx_ctx(struct qlcnic_adapter *);
+void qlcnic_83xx_del_tx_ctx(struct qlcnic_adapter *,
+			    struct qlcnic_host_tx_ring *);
 int qlcnic_83xx_get_nic_info(struct qlcnic_adapter *, struct qlcnic_info *, u8);
 int qlcnic_83xx_setup_link_event(struct qlcnic_adapter *, int);
 void qlcnic_83xx_process_rcv_ring_diag(struct qlcnic_host_sds_ring *);
 int qlcnic_83xx_config_intrpt(struct qlcnic_adapter *, bool);
-int qlcnic_83xx_sre_macaddr_change(struct qlcnic_adapter *, u8 *, __le16, u8);
+int qlcnic_83xx_sre_macaddr_change(struct qlcnic_adapter *, u8 *, u16, u8);
 int qlcnic_83xx_get_mac_address(struct qlcnic_adapter *, u8 *);
 void qlcnic_83xx_configure_mac(struct qlcnic_adapter *, u8 *, u8,
 			       struct qlcnic_cmd_args *);
@@ -368,6 +548,7 @@ void qlcnic_83xx_config_intr_coal(struct qlcnic_adapter *);
 irqreturn_t qlcnic_83xx_handle_aen(int, void *);
 int qlcnic_83xx_get_port_info(struct qlcnic_adapter *);
 void qlcnic_83xx_enable_mbx_intrpt(struct qlcnic_adapter *);
+void qlcnic_83xx_disable_mbx_intr(struct qlcnic_adapter *);
 irqreturn_t qlcnic_83xx_clear_legacy_intr(struct qlcnic_adapter *);
 irqreturn_t qlcnic_83xx_intr(int, void *);
 irqreturn_t qlcnic_83xx_tmp_intr(int, void *);
@@ -377,7 +558,7 @@ void qlcnic_83xx_disable_intr(struct qlcnic_adapter *,
 			     struct qlcnic_host_sds_ring *);
 void qlcnic_83xx_check_vf(struct qlcnic_adapter *,
 			  const struct pci_device_id *);
-void qlcnic_83xx_process_aen(struct qlcnic_adapter *);
+void __qlcnic_83xx_process_aen(struct qlcnic_adapter *);
 int qlcnic_83xx_get_port_config(struct qlcnic_adapter *);
 int qlcnic_83xx_set_port_config(struct qlcnic_adapter *);
 int qlcnic_enable_eswitch(struct qlcnic_adapter *, u8, u8);
@@ -401,7 +582,7 @@ int qlcnic_83xx_read_flash_descriptor_table(struct qlcnic_adapter *);
 int qlcnic_83xx_flash_read32(struct qlcnic_adapter *, u32, u8 *, int);
 int qlcnic_83xx_lockless_flash_read32(struct qlcnic_adapter *,
 				      u32, u8 *, int);
-int qlcnic_83xx_init(struct qlcnic_adapter *);
+int qlcnic_83xx_init(struct qlcnic_adapter *, int);
 int qlcnic_83xx_idc_ready_state_entry(struct qlcnic_adapter *);
 int qlcnic_83xx_check_hw_status(struct qlcnic_adapter *p_dev);
 void qlcnic_83xx_idc_poll_dev_state(struct work_struct *);
@@ -434,5 +615,12 @@ int qlcnic_83xx_get_regs_len(struct qlcnic_adapter *);
 int qlcnic_83xx_get_registers(struct qlcnic_adapter *, u32 *);
 int qlcnic_83xx_loopback_test(struct net_device *, u8);
 int qlcnic_83xx_interrupt_test(struct net_device *);
+int qlcnic_83xx_set_led(struct net_device *, enum ethtool_phys_id_state);
 int qlcnic_83xx_flash_test(struct qlcnic_adapter *);
+int qlcnic_83xx_enable_flash_write(struct qlcnic_adapter *);
+int qlcnic_83xx_disable_flash_write(struct qlcnic_adapter *);
+u32 qlcnic_83xx_mac_rcode(struct qlcnic_adapter *);
+u32 qlcnic_83xx_mbx_poll(struct qlcnic_adapter *);
+void qlcnic_83xx_enable_mbx_poll(struct qlcnic_adapter *);
+void qlcnic_83xx_disable_mbx_poll(struct qlcnic_adapter *);
 #endif
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_init.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_init.c
index 5c033f268ca5..ab1d8d99cbd5 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_init.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_init.c
@@ -5,6 +5,7 @@
  * See LICENSE.qlcnic for copyright and licensing details.
  */
 
+#include "qlcnic_sriov.h"
 #include "qlcnic.h"
 #include "qlcnic_hw.h"
 
@@ -24,13 +25,24 @@
 #define QLC_83XX_OPCODE_TMPL_END		0x0080
 #define QLC_83XX_OPCODE_POLL_READ_LIST		0x0100
 
+/* EPORT control registers */
+#define QLC_83XX_RESET_CONTROL			0x28084E50
+#define QLC_83XX_RESET_REG			0x28084E60
+#define QLC_83XX_RESET_PORT0			0x28084E70
+#define QLC_83XX_RESET_PORT1			0x28084E80
+#define QLC_83XX_RESET_PORT2			0x28084E90
+#define QLC_83XX_RESET_PORT3			0x28084EA0
+#define QLC_83XX_RESET_SRESHIM			0x28084EB0
+#define QLC_83XX_RESET_EPGSHIM			0x28084EC0
+#define QLC_83XX_RESET_ETHERPCS			0x28084ED0
+
 static int qlcnic_83xx_init_default_driver(struct qlcnic_adapter *adapter);
-static int qlcnic_83xx_configure_opmode(struct qlcnic_adapter *adapter);
 static int qlcnic_83xx_check_heartbeat(struct qlcnic_adapter *p_dev);
 static int qlcnic_83xx_restart_hw(struct qlcnic_adapter *adapter);
 
 /* Template header */
 struct qlc_83xx_reset_hdr {
+#if defined(__LITTLE_ENDIAN)
 	u16	version;
 	u16	signature;
 	u16	size;
@@ -39,14 +51,31 @@ struct qlc_83xx_reset_hdr {
 	u16	checksum;
 	u16	init_offset;
 	u16	start_offset;
+#elif defined(__BIG_ENDIAN)
+	u16	signature;
+	u16	version;
+	u16	entries;
+	u16	size;
+	u16	checksum;
+	u16	hdr_size;
+	u16	start_offset;
+	u16	init_offset;
+#endif
 } __packed;
 
 /* Command entry header. */
 struct qlc_83xx_entry_hdr {
-	u16 cmd;
-	u16 size;
-	u16 count;
-	u16 delay;
+#if defined(__LITTLE_ENDIAN)
+	u16	cmd;
+	u16	size;
+	u16	count;
+	u16	delay;
+#elif defined(__BIG_ENDIAN)
+	u16	size;
+	u16	cmd;
+	u16	delay;
+	u16	count;
+#endif
 } __packed;
 
 /* Generic poll command */
@@ -60,10 +89,17 @@ struct qlc_83xx_rmw {
 	u32	mask;
 	u32	xor_value;
 	u32	or_value;
+#if defined(__LITTLE_ENDIAN)
 	u8	shl;
 	u8	shr;
 	u8	index_a;
 	u8	rsvd;
+#elif defined(__BIG_ENDIAN)
+	u8	rsvd;
+	u8	index_a;
+	u8	shr;
+	u8	shl;
+#endif
 } __packed;
 
 /* Generic command with 2 DWORD */
@@ -90,18 +126,6 @@ static const char *const qlc_83xx_idc_states[] = {
 	"Quiesce"
 };
 
-/* Device States */
-enum qlcnic_83xx_states {
-	QLC_83XX_IDC_DEV_UNKNOWN,
-	QLC_83XX_IDC_DEV_COLD,
-	QLC_83XX_IDC_DEV_INIT,
-	QLC_83XX_IDC_DEV_READY,
-	QLC_83XX_IDC_DEV_NEED_RESET,
-	QLC_83XX_IDC_DEV_NEED_QUISCENT,
-	QLC_83XX_IDC_DEV_FAILED,
-	QLC_83XX_IDC_DEV_QUISCENT
-};
-
 static int
 qlcnic_83xx_idc_check_driver_presence_reg(struct qlcnic_adapter *adapter)
 {
@@ -137,7 +161,8 @@ static int qlcnic_83xx_idc_update_audit_reg(struct qlcnic_adapter *adapter,
 			return -EBUSY;
 	}
 
-	val = adapter->portnum & 0xf;
+	val = QLCRDX(adapter->ahw, QLC_83XX_IDC_DRV_AUDIT);
+	val |= (adapter->portnum & 0xf);
 	val |= mode << 7;
 	if (mode)
 		seconds = jiffies / HZ - adapter->ahw->idc.sec_counter;
@@ -376,14 +401,18 @@ static void qlcnic_83xx_idc_detach_driver(struct qlcnic_adapter *adapter)
 	struct net_device *netdev = adapter->netdev;
 
 	netif_device_detach(netdev);
+
 	/* Disable mailbox interrupt */
-	QLCWRX(adapter->ahw, QLCNIC_MBX_INTR_ENBL, 0);
+	qlcnic_83xx_disable_mbx_intr(adapter);
 	qlcnic_down(adapter, netdev);
 	for (i = 0; i < adapter->ahw->num_msix; i++) {
 		adapter->ahw->intr_tbl[i].id = i;
 		adapter->ahw->intr_tbl[i].enabled = 0;
 		adapter->ahw->intr_tbl[i].src = 0;
 	}
+
+	if (qlcnic_sriov_pf_check(adapter))
+		qlcnic_sriov_pf_reset(adapter);
 }
 
 /**
@@ -585,9 +614,15 @@ static int qlcnic_83xx_idc_check_fan_failure(struct qlcnic_adapter *adapter)
 
 static int qlcnic_83xx_idc_reattach_driver(struct qlcnic_adapter *adapter)
 {
+	int err;
+
 	/* register for NIC IDC AEN Events */
 	qlcnic_83xx_register_nic_idc_func(adapter, 1);
 
+	err = qlcnic_sriov_pf_reinit(adapter);
+	if (err)
+		return err;
+
 	qlcnic_83xx_enable_mbx_intrpt(adapter);
 
 	if (qlcnic_83xx_configure_opmode(adapter)) {
@@ -1350,6 +1385,19 @@ static void qlcnic_83xx_disable_pause_frames(struct qlcnic_adapter *adapter)
 	qlcnic_83xx_unlock_driver(adapter);
 }
 
+static void qlcnic_83xx_take_eport_out_of_reset(struct qlcnic_adapter *adapter)
+{
+	QLCWR32(adapter, QLC_83XX_RESET_REG, 0);
+	QLCWR32(adapter, QLC_83XX_RESET_PORT0, 0);
+	QLCWR32(adapter, QLC_83XX_RESET_PORT1, 0);
+	QLCWR32(adapter, QLC_83XX_RESET_PORT2, 0);
+	QLCWR32(adapter, QLC_83XX_RESET_PORT3, 0);
+	QLCWR32(adapter, QLC_83XX_RESET_SRESHIM, 0);
+	QLCWR32(adapter, QLC_83XX_RESET_EPGSHIM, 0);
+	QLCWR32(adapter, QLC_83XX_RESET_ETHERPCS, 0);
+	QLCWR32(adapter, QLC_83XX_RESET_CONTROL, 1);
+}
+
 static int qlcnic_83xx_check_heartbeat(struct qlcnic_adapter *p_dev)
 {
 	u32 heartbeat, peg_status;
@@ -1371,6 +1419,7 @@ static int qlcnic_83xx_check_heartbeat(struct qlcnic_adapter *p_dev)
 
 	if (ret) {
 		dev_err(&p_dev->pdev->dev, "firmware hang detected\n");
+		qlcnic_83xx_take_eport_out_of_reset(p_dev);
 		qlcnic_83xx_disable_pause_frames(p_dev);
 		peg_status = QLC_SHARED_REG_RD32(p_dev,
 						 QLCNIC_PEG_HALT_STATUS1);
@@ -1893,6 +1942,9 @@ int qlcnic_83xx_config_default_opmode(struct qlcnic_adapter *adapter)
 	qlcnic_get_func_no(adapter);
 	op_mode = QLCRDX(ahw, QLC_83XX_DRV_OP_MODE);
 
+	if (test_bit(__QLCNIC_SRIOV_CAPABLE, &adapter->state))
+		op_mode = QLC_83XX_DEFAULT_OPMODE;
+
 	if (op_mode == QLC_83XX_DEFAULT_OPMODE) {
 		adapter->nic_ops->init_driver = qlcnic_83xx_init_default_driver;
 		ahw->idc.state_entry = qlcnic_83xx_idc_ready_state_entry;
@@ -1922,6 +1974,16 @@ int qlcnic_83xx_get_nic_configuration(struct qlcnic_adapter *adapter)
 	ahw->max_mac_filters = nic_info.max_mac_filters;
 	ahw->max_mtu = nic_info.max_mtu;
 
+	/* VNIC mode is detected by BIT_23 in capabilities. This bit is also
+	 * set in case device is SRIOV capable. VNIC and SRIOV are mutually
+	 * exclusive. So in case of sriov capable device load driver in
+	 * default mode
+	 */
+	if (test_bit(__QLCNIC_SRIOV_CAPABLE, &adapter->state)) {
+		ahw->nic_mode = QLC_83XX_DEFAULT_MODE;
+		return ahw->nic_mode;
+	}
+
 	if (ahw->capabilities & BIT_23)
 		ahw->nic_mode = QLC_83XX_VIRTUAL_NIC_MODE;
 	else
@@ -1930,7 +1992,7 @@ int qlcnic_83xx_get_nic_configuration(struct qlcnic_adapter *adapter)
 	return ahw->nic_mode;
 }
 
-static int qlcnic_83xx_configure_opmode(struct qlcnic_adapter *adapter)
+int qlcnic_83xx_configure_opmode(struct qlcnic_adapter *adapter)
 {
 	int ret;
 
@@ -2008,10 +2070,13 @@ static void qlcnic_83xx_clear_function_resources(struct qlcnic_adapter *adapter)
 	}
 }
 
-int qlcnic_83xx_init(struct qlcnic_adapter *adapter)
+int qlcnic_83xx_init(struct qlcnic_adapter *adapter, int pci_using_dac)
 {
 	struct qlcnic_hardware_context *ahw = adapter->ahw;
 
+	if (qlcnic_sriov_vf_check(adapter))
+		return qlcnic_sriov_vf_init(adapter, pci_using_dac);
+
 	if (qlcnic_83xx_check_hw_status(adapter))
 		return -EIO;
 
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c
index a69097c6b84d..43562c256379 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c
@@ -382,8 +382,7 @@ out_free_rq:
 	return err;
 }
 
-static void
-qlcnic_fw_cmd_destroy_rx_ctx(struct qlcnic_adapter *adapter)
+void qlcnic_82xx_fw_cmd_del_rx_ctx(struct qlcnic_adapter *adapter)
 {
 	int err;
 	struct qlcnic_cmd_args cmd;
@@ -422,22 +421,20 @@ int qlcnic_82xx_fw_cmd_create_tx_ctx(struct qlcnic_adapter *adapter,
 
 	rq_size = SIZEOF_HOSTRQ_TX(struct qlcnic_hostrq_tx_ctx);
 	rq_addr = dma_alloc_coherent(&adapter->pdev->dev, rq_size,
-			&rq_phys_addr, GFP_KERNEL);
+				     &rq_phys_addr, GFP_KERNEL | __GFP_ZERO);
 	if (!rq_addr)
 		return -ENOMEM;
 
 	rsp_size = SIZEOF_CARDRSP_TX(struct qlcnic_cardrsp_tx_ctx);
 	rsp_addr = dma_alloc_coherent(&adapter->pdev->dev, rsp_size,
-			&rsp_phys_addr, GFP_KERNEL);
+				      &rsp_phys_addr, GFP_KERNEL | __GFP_ZERO);
 	if (!rsp_addr) {
 		err = -ENOMEM;
 		goto out_free_rq;
 	}
 
-	memset(rq_addr, 0, rq_size);
 	prq = rq_addr;
 
-	memset(rsp_addr, 0, rsp_size);
 	prsp = rsp_addr;
 
 	prq->host_rsp_dma_addr = cpu_to_le64(rsp_phys_addr);
@@ -486,13 +483,13 @@ out_free_rq:
 	return err;
 }
 
-static void
-qlcnic_fw_cmd_destroy_tx_ctx(struct qlcnic_adapter *adapter,
-			     struct qlcnic_host_tx_ring *tx_ring)
+void qlcnic_82xx_fw_cmd_del_tx_ctx(struct qlcnic_adapter *adapter,
+				   struct qlcnic_host_tx_ring *tx_ring)
 {
 	struct qlcnic_cmd_args cmd;
 
 	qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_DESTROY_TX_CTX);
+
 	cmd.req.arg[1] = tx_ring->ctx_id;
 	if (qlcnic_issue_cmd(adapter, &cmd))
 		dev_err(&adapter->pdev->dev,
@@ -532,20 +529,15 @@ int qlcnic_alloc_hw_resources(struct qlcnic_adapter *adapter)
 		ptr = (__le32 *)dma_alloc_coherent(&pdev->dev, sizeof(u32),
 						   &tx_ring->hw_cons_phys_addr,
 						   GFP_KERNEL);
-
-		if (ptr == NULL) {
-			dev_err(&pdev->dev, "failed to allocate tx consumer\n");
+		if (ptr == NULL)
 			return -ENOMEM;
-		}
+
 		tx_ring->hw_consumer = ptr;
 		/* cmd desc ring */
 		addr = dma_alloc_coherent(&pdev->dev, TX_DESC_RINGSIZE(tx_ring),
 					  &tx_ring->phys_addr,
 					  GFP_KERNEL);
-
 		if (addr == NULL) {
-			dev_err(&pdev->dev,
-				"failed to allocate tx desc ring\n");
 			err = -ENOMEM;
 			goto err_out_free;
 		}
@@ -556,11 +548,9 @@ int qlcnic_alloc_hw_resources(struct qlcnic_adapter *adapter)
 	for (ring = 0; ring < adapter->max_rds_rings; ring++) {
 		rds_ring = &recv_ctx->rds_rings[ring];
 		addr = dma_alloc_coherent(&adapter->pdev->dev,
-				RCV_DESC_RINGSIZE(rds_ring),
-				&rds_ring->phys_addr, GFP_KERNEL);
+					  RCV_DESC_RINGSIZE(rds_ring),
+					  &rds_ring->phys_addr, GFP_KERNEL);
 		if (addr == NULL) {
-			dev_err(&pdev->dev,
-				"failed to allocate rds ring [%d]\n", ring);
 			err = -ENOMEM;
 			goto err_out_free;
 		}
@@ -572,11 +562,9 @@ int qlcnic_alloc_hw_resources(struct qlcnic_adapter *adapter)
 		sds_ring = &recv_ctx->sds_rings[ring];
 
 		addr = dma_alloc_coherent(&adapter->pdev->dev,
-				STATUS_DESC_RINGSIZE(sds_ring),
-				&sds_ring->phys_addr, GFP_KERNEL);
+					  STATUS_DESC_RINGSIZE(sds_ring),
+					  &sds_ring->phys_addr, GFP_KERNEL);
 		if (addr == NULL) {
-			dev_err(&pdev->dev,
-				"failed to allocate sds ring [%d]\n", ring);
 			err = -ENOMEM;
 			goto err_out_free;
 		}
@@ -616,13 +604,12 @@ int qlcnic_fw_create_ctx(struct qlcnic_adapter *dev)
 						  &dev->tx_ring[ring],
 						  ring);
 		if (err) {
-			qlcnic_fw_cmd_destroy_rx_ctx(dev);
+			qlcnic_fw_cmd_del_rx_ctx(dev);
 			if (ring == 0)
 				goto err_out;
 
 			for (i = 0; i < ring; i++)
-				qlcnic_fw_cmd_destroy_tx_ctx(dev,
-							     &dev->tx_ring[i]);
+				qlcnic_fw_cmd_del_tx_ctx(dev, &dev->tx_ring[i]);
 
 			goto err_out;
 		}
@@ -644,10 +631,10 @@ void qlcnic_fw_destroy_ctx(struct qlcnic_adapter *adapter)
 	int ring;
 
 	if (test_and_clear_bit(__QLCNIC_FW_ATTACHED, &adapter->state)) {
-		qlcnic_fw_cmd_destroy_rx_ctx(adapter);
+		qlcnic_fw_cmd_del_rx_ctx(adapter);
 		for (ring = 0; ring < adapter->max_drv_tx_rings; ring++)
-			qlcnic_fw_cmd_destroy_tx_ctx(adapter,
-						     &adapter->tx_ring[ring]);
+			qlcnic_fw_cmd_del_tx_ctx(adapter,
+						 &adapter->tx_ring[ring]);
 
 		if (qlcnic_83xx_check(adapter) &&
 		    (adapter->flags & QLCNIC_MSIX_ENABLED)) {
@@ -655,7 +642,7 @@ void qlcnic_fw_destroy_ctx(struct qlcnic_adapter *adapter)
 				qlcnic_83xx_config_intrpt(adapter, 0);
 		}
 		/* Allow dma queues to drain after context reset */
-		mdelay(20);
+		msleep(20);
 	}
 }
 
@@ -753,10 +740,9 @@ int qlcnic_82xx_get_nic_info(struct qlcnic_adapter *adapter,
 	size_t  nic_size = sizeof(struct qlcnic_info_le);
 
 	nic_info_addr = dma_alloc_coherent(&adapter->pdev->dev, nic_size,
-				&nic_dma_t, GFP_KERNEL);
+					   &nic_dma_t, GFP_KERNEL | __GFP_ZERO);
 	if (!nic_info_addr)
 		return -ENOMEM;
-	memset(nic_info_addr, 0, nic_size);
 
 	nic_info = nic_info_addr;
 
@@ -804,11 +790,10 @@ int qlcnic_82xx_set_nic_info(struct qlcnic_adapter *adapter,
 		return err;
 
 	nic_info_addr = dma_alloc_coherent(&adapter->pdev->dev, nic_size,
-			&nic_dma_t, GFP_KERNEL);
+					   &nic_dma_t, GFP_KERNEL | __GFP_ZERO);
 	if (!nic_info_addr)
 		return -ENOMEM;
 
-	memset(nic_info_addr, 0, nic_size);
 	nic_info = nic_info_addr;
 
 	nic_info->pci_func = cpu_to_le16(nic->pci_func);
@@ -854,10 +839,10 @@ int qlcnic_82xx_get_pci_info(struct qlcnic_adapter *adapter,
 	size_t pci_size = npar_size * QLCNIC_MAX_PCI_FUNC;
 
 	pci_info_addr = dma_alloc_coherent(&adapter->pdev->dev, pci_size,
-			&pci_info_dma_t, GFP_KERNEL);
+					   &pci_info_dma_t,
+					   GFP_KERNEL | __GFP_ZERO);
 	if (!pci_info_addr)
 		return -ENOMEM;
-	memset(pci_info_addr, 0, pci_size);
 
 	npar = pci_info_addr;
 	qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_GET_PCI_INFO);
@@ -949,12 +934,9 @@ int qlcnic_get_port_stats(struct qlcnic_adapter *adapter, const u8 func,
 	}
 
 	stats_addr = dma_alloc_coherent(&adapter->pdev->dev, stats_size,
-			&stats_dma_t, GFP_KERNEL);
-	if (!stats_addr) {
-		dev_err(&adapter->pdev->dev, "Unable to allocate memory\n");
+					&stats_dma_t, GFP_KERNEL | __GFP_ZERO);
+	if (!stats_addr)
 		return -ENOMEM;
-	}
-	memset(stats_addr, 0, stats_size);
 
 	arg1 = func | QLCNIC_STATS_VERSION << 8 | QLCNIC_STATS_PORT << 12;
 	arg1 |= rx_tx << 15 | stats_size << 16;
@@ -1003,13 +985,10 @@ int qlcnic_get_mac_stats(struct qlcnic_adapter *adapter,
 		return -ENOMEM;
 
 	stats_addr = dma_alloc_coherent(&adapter->pdev->dev, stats_size,
-			&stats_dma_t, GFP_KERNEL);
-	if (!stats_addr) {
-		dev_err(&adapter->pdev->dev,
-			"%s: Unable to allocate memory.\n", __func__);
+					&stats_dma_t, GFP_KERNEL | __GFP_ZERO);
+	if (!stats_addr)
 		return -ENOMEM;
-	}
-	memset(stats_addr, 0, stats_size);
+
 	qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_GET_MAC_STATS);
 	cmd.req.arg[1] = stats_size << 16;
 	cmd.req.arg[2] = MSD(stats_dma_t);
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c
index 5641f8ec49ab..08efb4635007 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c
@@ -115,6 +115,13 @@ static const char qlcnic_83xx_mac_stats_strings[][ETH_GSTRING_LEN] = {
 	"mac_rx_dropped",
 	"mac_crc_error",
 	"mac_align_error",
+	"eswitch_frames",
+	"eswitch_bytes",
+	"eswitch_multicast_frames",
+	"eswitch_broadcast_frames",
+	"eswitch_unicast_frames",
+	"eswitch_error_free_frames",
+	"eswitch_error_free_bytes",
 };
 
 #define QLCNIC_STATS_LEN	ARRAY_SIZE(qlcnic_gstrings_stats)
@@ -149,7 +156,8 @@ static const char qlcnic_gstrings_test[][ETH_GSTRING_LEN] = {
 
 static inline int qlcnic_82xx_statistics(void)
 {
-	return QLCNIC_STATS_LEN + ARRAY_SIZE(qlcnic_83xx_mac_stats_strings);
+	return ARRAY_SIZE(qlcnic_device_gstrings_stats) +
+	       ARRAY_SIZE(qlcnic_83xx_mac_stats_strings);
 }
 
 static inline int qlcnic_83xx_statistics(void)
@@ -634,7 +642,7 @@ static int qlcnic_set_channels(struct net_device *dev,
 	    channel->tx_count != channel->max_tx)
 		return -EINVAL;
 
-	err = qlcnic_validate_max_rss(channel->max_rx, channel->rx_count);
+	err = qlcnic_validate_max_rss(adapter, channel->rx_count);
 	if (err)
 		return err;
 
@@ -858,9 +866,11 @@ clear_diag_irq:
 	return ret;
 }
 
-#define QLCNIC_ILB_PKT_SIZE 64
-#define QLCNIC_NUM_ILB_PKT	16
-#define QLCNIC_ILB_MAX_RCV_LOOP 10
+#define QLCNIC_ILB_PKT_SIZE		64
+#define QLCNIC_NUM_ILB_PKT		16
+#define QLCNIC_ILB_MAX_RCV_LOOP		10
+#define QLCNIC_LB_PKT_POLL_DELAY_MSEC	1
+#define QLCNIC_LB_PKT_POLL_COUNT	20
 
 static void qlcnic_create_loopback_buff(unsigned char *data, u8 mac[])
 {
@@ -897,9 +907,9 @@ int qlcnic_do_lb_test(struct qlcnic_adapter *adapter, u8 mode)
 		loop = 0;
 
 		do {
-			msleep(1);
+			msleep(QLCNIC_LB_PKT_POLL_DELAY_MSEC);
 			qlcnic_process_rcv_ring_diag(sds_ring);
-			if (loop++ > QLCNIC_ILB_MAX_RCV_LOOP)
+			if (loop++ > QLCNIC_LB_PKT_POLL_COUNT)
 				break;
 		} while (!adapter->ahw->diag_cnt);
 
@@ -1070,8 +1080,7 @@ qlcnic_get_strings(struct net_device *dev, u32 stringset, u8 *data)
 	}
 }
 
-static void
-qlcnic_fill_stats(u64 *data, void *stats, int type)
+static u64 *qlcnic_fill_stats(u64 *data, void *stats, int type)
 {
 	if (type == QLCNIC_MAC_STATS) {
 		struct qlcnic_mac_statistics *mac_stats =
@@ -1120,6 +1129,7 @@ qlcnic_fill_stats(u64 *data, void *stats, int type)
 		*data++ = QLCNIC_FILL_STATS(esw_stats->local_frames);
 		*data++ = QLCNIC_FILL_STATS(esw_stats->numbytes);
 	}
+	return data;
 }
 
 static void qlcnic_get_ethtool_stats(struct net_device *dev,
@@ -1147,7 +1157,7 @@ static void qlcnic_get_ethtool_stats(struct net_device *dev,
 		/* Retrieve MAC statistics from firmware */
 		memset(&mac_stats, 0, sizeof(struct qlcnic_mac_statistics));
 		qlcnic_get_mac_stats(adapter, &mac_stats);
-		qlcnic_fill_stats(data, &mac_stats, QLCNIC_MAC_STATS);
+		data = qlcnic_fill_stats(data, &mac_stats, QLCNIC_MAC_STATS);
 	}
 
 	if (!(adapter->flags & QLCNIC_ESWITCH_ENABLED))
@@ -1159,7 +1169,7 @@ static void qlcnic_get_ethtool_stats(struct net_device *dev,
 	if (ret)
 		return;
 
-	qlcnic_fill_stats(data, &port_stats.rx, QLCNIC_ESW_STATS);
+	data = qlcnic_fill_stats(data, &port_stats.rx, QLCNIC_ESW_STATS);
 	ret = qlcnic_get_port_stats(adapter, adapter->ahw->pci_func,
 			QLCNIC_QUERY_TX_COUNTER, &port_stats.tx);
 	if (ret)
@@ -1176,7 +1186,8 @@ static int qlcnic_set_led(struct net_device *dev,
 	int err = -EIO, active = 1;
 
 	if (qlcnic_83xx_check(adapter))
-		return -EOPNOTSUPP;
+		return qlcnic_83xx_set_led(dev, state);
+
 	if (adapter->ahw->op_mode == QLCNIC_NON_PRIV_FUNC) {
 		netdev_warn(dev, "LED test not supported for non "
 				"privilege function\n");
@@ -1292,6 +1303,9 @@ static int qlcnic_set_intr_coalesce(struct net_device *netdev,
 			struct ethtool_coalesce *ethcoal)
 {
 	struct qlcnic_adapter *adapter = netdev_priv(netdev);
+	struct qlcnic_nic_intr_coalesce *coal;
+	u32 rx_coalesce_usecs, rx_max_frames;
+	u32 tx_coalesce_usecs, tx_max_frames;
 
 	if (!test_bit(__QLCNIC_DEV_UP, &adapter->state))
 		return -EINVAL;
@@ -1302,8 +1316,8 @@ static int qlcnic_set_intr_coalesce(struct net_device *netdev,
 	*/
 	if (ethcoal->rx_coalesce_usecs > 0xffff ||
 		ethcoal->rx_max_coalesced_frames > 0xffff ||
-		ethcoal->tx_coalesce_usecs ||
-		ethcoal->tx_max_coalesced_frames ||
+		ethcoal->tx_coalesce_usecs > 0xffff ||
+		ethcoal->tx_max_coalesced_frames > 0xffff ||
 		ethcoal->rx_coalesce_usecs_irq ||
 		ethcoal->rx_max_coalesced_frames_irq ||
 		ethcoal->tx_coalesce_usecs_irq ||
@@ -1323,18 +1337,55 @@ static int qlcnic_set_intr_coalesce(struct net_device *netdev,
 		ethcoal->tx_max_coalesced_frames_high)
 		return -EINVAL;
 
-	if (!ethcoal->rx_coalesce_usecs ||
-		!ethcoal->rx_max_coalesced_frames) {
-		adapter->ahw->coal.flag = QLCNIC_INTR_DEFAULT;
-		adapter->ahw->coal.rx_time_us =
-			QLCNIC_DEFAULT_INTR_COALESCE_RX_TIME_US;
-		adapter->ahw->coal.rx_packets =
-			QLCNIC_DEFAULT_INTR_COALESCE_RX_PACKETS;
+	coal = &adapter->ahw->coal;
+
+	if (qlcnic_83xx_check(adapter)) {
+		if (!ethcoal->tx_coalesce_usecs ||
+		    !ethcoal->tx_max_coalesced_frames ||
+		    !ethcoal->rx_coalesce_usecs ||
+		    !ethcoal->rx_max_coalesced_frames) {
+			coal->flag = QLCNIC_INTR_DEFAULT;
+			coal->type = QLCNIC_INTR_COAL_TYPE_RX;
+			coal->rx_time_us = QLCNIC_DEF_INTR_COALESCE_RX_TIME_US;
+			coal->rx_packets = QLCNIC_DEF_INTR_COALESCE_RX_PACKETS;
+			coal->tx_time_us = QLCNIC_DEF_INTR_COALESCE_TX_TIME_US;
+			coal->tx_packets = QLCNIC_DEF_INTR_COALESCE_TX_PACKETS;
+		} else {
+			tx_coalesce_usecs = ethcoal->tx_coalesce_usecs;
+			tx_max_frames = ethcoal->tx_max_coalesced_frames;
+			rx_coalesce_usecs = ethcoal->rx_coalesce_usecs;
+			rx_max_frames = ethcoal->rx_max_coalesced_frames;
+			coal->flag = 0;
+
+			if ((coal->rx_time_us == rx_coalesce_usecs) &&
+			    (coal->rx_packets == rx_max_frames)) {
+				coal->type = QLCNIC_INTR_COAL_TYPE_TX;
+				coal->tx_time_us = tx_coalesce_usecs;
+				coal->tx_packets = tx_max_frames;
+			} else if ((coal->tx_time_us == tx_coalesce_usecs) &&
+				   (coal->tx_packets == tx_max_frames)) {
+				coal->type = QLCNIC_INTR_COAL_TYPE_RX;
+				coal->rx_time_us = rx_coalesce_usecs;
+				coal->rx_packets = rx_max_frames;
+			} else {
+				coal->type = QLCNIC_INTR_COAL_TYPE_RX;
+				coal->rx_time_us = rx_coalesce_usecs;
+				coal->rx_packets = rx_max_frames;
+				coal->tx_time_us = tx_coalesce_usecs;
+				coal->tx_packets = tx_max_frames;
+			}
+		}
 	} else {
-		adapter->ahw->coal.flag = 0;
-		adapter->ahw->coal.rx_time_us = ethcoal->rx_coalesce_usecs;
-		adapter->ahw->coal.rx_packets =
-			ethcoal->rx_max_coalesced_frames;
+		if (!ethcoal->rx_coalesce_usecs ||
+		    !ethcoal->rx_max_coalesced_frames) {
+			coal->flag = QLCNIC_INTR_DEFAULT;
+			coal->rx_time_us = QLCNIC_DEF_INTR_COALESCE_RX_TIME_US;
+			coal->rx_packets = QLCNIC_DEF_INTR_COALESCE_RX_PACKETS;
+		} else {
+			coal->flag = 0;
+			coal->rx_time_us = ethcoal->rx_coalesce_usecs;
+			coal->rx_packets = ethcoal->rx_max_coalesced_frames;
+		}
 	}
 
 	qlcnic_config_intr_coalesce(adapter);
@@ -1352,6 +1403,8 @@ static int qlcnic_get_intr_coalesce(struct net_device *netdev,
 
 	ethcoal->rx_coalesce_usecs = adapter->ahw->coal.rx_time_us;
 	ethcoal->rx_max_coalesced_frames = adapter->ahw->coal.rx_packets;
+	ethcoal->tx_coalesce_usecs = adapter->ahw->coal.tx_time_us;
+	ethcoal->tx_max_coalesced_frames = adapter->ahw->coal.tx_packets;
 
 	return 0;
 }
@@ -1537,3 +1590,25 @@ const struct ethtool_ops qlcnic_ethtool_ops = {
 	.get_dump_data = qlcnic_get_dump_data,
 	.set_dump = qlcnic_set_dump,
 };
+
+const struct ethtool_ops qlcnic_sriov_vf_ethtool_ops = {
+	.get_settings		= qlcnic_get_settings,
+	.get_drvinfo		= qlcnic_get_drvinfo,
+	.get_regs_len		= qlcnic_get_regs_len,
+	.get_regs		= qlcnic_get_regs,
+	.get_link		= ethtool_op_get_link,
+	.get_eeprom_len		= qlcnic_get_eeprom_len,
+	.get_eeprom		= qlcnic_get_eeprom,
+	.get_ringparam		= qlcnic_get_ringparam,
+	.set_ringparam		= qlcnic_set_ringparam,
+	.get_channels		= qlcnic_get_channels,
+	.get_pauseparam		= qlcnic_get_pauseparam,
+	.get_wol		= qlcnic_get_wol,
+	.get_strings		= qlcnic_get_strings,
+	.get_ethtool_stats	= qlcnic_get_ethtool_stats,
+	.get_sset_count		= qlcnic_get_sset_count,
+	.get_coalesce		= qlcnic_get_intr_coalesce,
+	.set_coalesce		= qlcnic_set_intr_coalesce,
+	.set_msglevel		= qlcnic_set_msglevel,
+	.get_msglevel		= qlcnic_get_msglevel,
+};
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hdr.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hdr.h
index 44197ca1456c..c0f0c0d0a790 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hdr.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hdr.h
@@ -669,7 +669,7 @@ enum {
 #define QLCNIC_CMDPEG_CHECK_RETRY_COUNT	60
 #define QLCNIC_CMDPEG_CHECK_DELAY	500
 #define QLCNIC_HEARTBEAT_PERIOD_MSECS	200
-#define QLCNIC_HEARTBEAT_CHECK_RETRY_COUNT	45
+#define QLCNIC_HEARTBEAT_CHECK_RETRY_COUNT	10
 
 #define QLCNIC_MAX_MC_COUNT		38
 #define QLCNIC_WATCHDOG_TIMEOUTVALUE	5
@@ -714,7 +714,9 @@ enum {
 	QLCNIC_MGMT_FUNC	= 0,
 	QLCNIC_PRIV_FUNC	= 1,
 	QLCNIC_NON_PRIV_FUNC	= 2,
-	QLCNIC_UNKNOWN_FUNC_MODE = 3
+	QLCNIC_SRIOV_PF_FUNC	= 3,
+	QLCNIC_SRIOV_VF_FUNC	= 4,
+	QLCNIC_UNKNOWN_FUNC_MODE = 5
 };
 
 enum {
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.c
index f89cc7a3fe6c..6a6512ba9f38 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.c
@@ -423,7 +423,7 @@ qlcnic_send_cmd_descs(struct qlcnic_adapter *adapter,
 }
 
 int qlcnic_82xx_sre_macaddr_change(struct qlcnic_adapter *adapter, u8 *addr,
-				   __le16 vlan_id, u8 op)
+				   u16 vlan_id, u8 op)
 {
 	struct qlcnic_nic_req req;
 	struct qlcnic_mac_req *mac_req;
@@ -441,7 +441,7 @@ int qlcnic_82xx_sre_macaddr_change(struct qlcnic_adapter *adapter, u8 *addr,
 	memcpy(mac_req->mac_addr, addr, 6);
 
 	vlan_req = (struct qlcnic_vlan_req *)&req.words[1];
-	vlan_req->vlan_id = vlan_id;
+	vlan_req->vlan_id = cpu_to_le16(vlan_id);
 
 	return qlcnic_send_cmd_descs(adapter, (struct cmd_desc_type0 *)&req, 1);
 }
@@ -468,7 +468,7 @@ int qlcnic_nic_del_mac(struct qlcnic_adapter *adapter, const u8 *addr)
 	return err;
 }
 
-int qlcnic_nic_add_mac(struct qlcnic_adapter *adapter, const u8 *addr)
+int qlcnic_nic_add_mac(struct qlcnic_adapter *adapter, const u8 *addr, u16 vlan)
 {
 	struct list_head *head;
 	struct qlcnic_mac_list_s *cur;
@@ -487,7 +487,7 @@ int qlcnic_nic_add_mac(struct qlcnic_adapter *adapter, const u8 *addr)
 	memcpy(cur->mac_addr, addr, ETH_ALEN);
 
 	if (qlcnic_sre_macaddr_change(adapter,
-				cur->mac_addr, 0, QLCNIC_MAC_ADD)) {
+				cur->mac_addr, vlan, QLCNIC_MAC_ADD)) {
 		kfree(cur);
 		return -EIO;
 	}
@@ -496,7 +496,7 @@ int qlcnic_nic_add_mac(struct qlcnic_adapter *adapter, const u8 *addr)
 	return 0;
 }
 
-void qlcnic_set_multi(struct net_device *netdev)
+void __qlcnic_set_multi(struct net_device *netdev, u16 vlan)
 {
 	struct qlcnic_adapter *adapter = netdev_priv(netdev);
 	struct netdev_hw_addr *ha;
@@ -508,8 +508,9 @@ void qlcnic_set_multi(struct net_device *netdev)
 	if (!test_bit(__QLCNIC_FW_ATTACHED, &adapter->state))
 		return;
 
-	qlcnic_nic_add_mac(adapter, adapter->mac_addr);
-	qlcnic_nic_add_mac(adapter, bcast_addr);
+	if (!qlcnic_sriov_vf_check(adapter))
+		qlcnic_nic_add_mac(adapter, adapter->mac_addr, vlan);
+	qlcnic_nic_add_mac(adapter, bcast_addr, vlan);
 
 	if (netdev->flags & IFF_PROMISC) {
 		if (!(adapter->flags & QLCNIC_PROMISC_DISABLED))
@@ -523,23 +524,55 @@ void qlcnic_set_multi(struct net_device *netdev)
 		goto send_fw_cmd;
 	}
 
-	if (!netdev_mc_empty(netdev)) {
+	if (!netdev_mc_empty(netdev) && !qlcnic_sriov_vf_check(adapter)) {
 		netdev_for_each_mc_addr(ha, netdev) {
-			qlcnic_nic_add_mac(adapter, ha->addr);
+			qlcnic_nic_add_mac(adapter, ha->addr, vlan);
 		}
 	}
 
+	if (qlcnic_sriov_vf_check(adapter))
+		qlcnic_vf_add_mc_list(netdev, vlan);
+
 send_fw_cmd:
-	if (mode == VPORT_MISS_MODE_ACCEPT_ALL && !adapter->fdb_mac_learn) {
-		qlcnic_alloc_lb_filters_mem(adapter);
-		adapter->drv_mac_learn = true;
-	} else {
-		adapter->drv_mac_learn = false;
+	if (!qlcnic_sriov_vf_check(adapter)) {
+		if (mode == VPORT_MISS_MODE_ACCEPT_ALL &&
+		    !adapter->fdb_mac_learn) {
+			qlcnic_alloc_lb_filters_mem(adapter);
+			adapter->drv_mac_learn = true;
+		} else {
+			adapter->drv_mac_learn = false;
+		}
 	}
 
 	qlcnic_nic_set_promisc(adapter, mode);
 }
 
+void qlcnic_set_multi(struct net_device *netdev)
+{
+	struct qlcnic_adapter *adapter = netdev_priv(netdev);
+	struct netdev_hw_addr *ha;
+	struct qlcnic_mac_list_s *cur;
+
+	if (!test_bit(__QLCNIC_FW_ATTACHED, &adapter->state))
+		return;
+	if (qlcnic_sriov_vf_check(adapter)) {
+		if (!netdev_mc_empty(netdev)) {
+			netdev_for_each_mc_addr(ha, netdev) {
+				cur = kzalloc(sizeof(struct qlcnic_mac_list_s),
+					      GFP_ATOMIC);
+				if (cur == NULL)
+					break;
+				memcpy(cur->mac_addr,
+				       ha->addr, ETH_ALEN);
+				list_add_tail(&cur->list, &adapter->vf_mc_list);
+			}
+		}
+		qlcnic_sriov_vf_schedule_multi(adapter->netdev);
+		return;
+	}
+	__qlcnic_set_multi(netdev, 0);
+}
+
 int qlcnic_82xx_nic_set_promisc(struct qlcnic_adapter *adapter, u32 mode)
 {
 	struct qlcnic_nic_req req;
@@ -559,7 +592,7 @@ int qlcnic_82xx_nic_set_promisc(struct qlcnic_adapter *adapter, u32 mode)
 				(struct cmd_desc_type0 *)&req, 1);
 }
 
-void qlcnic_free_mac_list(struct qlcnic_adapter *adapter)
+void qlcnic_82xx_free_mac_list(struct qlcnic_adapter *adapter)
 {
 	struct qlcnic_mac_list_s *cur;
 	struct list_head *head = &adapter->mac_list;
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.h
index 5b8749eda11f..95b1b5732838 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.h
@@ -83,6 +83,8 @@ enum qlcnic_regs {
 #define QLCNIC_CMD_CONFIG_PORT			0x2e
 #define QLCNIC_CMD_TEMP_SIZE			0x2f
 #define QLCNIC_CMD_GET_TEMP_HDR			0x30
+#define QLCNIC_CMD_BC_EVENT_SETUP		0x31
+#define	QLCNIC_CMD_CONFIG_VPORT			0x32
 #define QLCNIC_CMD_GET_MAC_STATS		0x37
 #define QLCNIC_CMD_SET_DRV_VER			0x38
 #define QLCNIC_CMD_CONFIGURE_RSS		0x41
@@ -114,6 +116,7 @@ enum qlcnic_regs {
 #define QLCNIC_SET_FAC_DEF_MAC			5
 
 #define QLCNIC_MBX_LINK_EVENT		0x8001
+#define QLCNIC_MBX_BC_EVENT		0x8002
 #define QLCNIC_MBX_COMP_EVENT		0x8100
 #define QLCNIC_MBX_REQUEST_EVENT	0x8101
 #define QLCNIC_MBX_TIME_EXTEND_EVENT	0x8102
@@ -156,7 +159,7 @@ int qlcnic_82xx_nic_set_promisc(struct qlcnic_adapter *adapter, u32);
 int qlcnic_82xx_napi_add(struct qlcnic_adapter *adapter,
 			 struct net_device *netdev);
 void qlcnic_82xx_change_filter(struct qlcnic_adapter *adapter,
-			       u64 *uaddr, __le16 vlan_id);
+			       u64 *uaddr, u16 vlan_id);
 void qlcnic_82xx_config_intr_coalesce(struct qlcnic_adapter *adapter);
 int qlcnic_82xx_config_rss(struct qlcnic_adapter *adapter, int);
 void qlcnic_82xx_config_ipaddr(struct qlcnic_adapter *adapter,
@@ -175,7 +178,10 @@ int qlcnic_82xx_issue_cmd(struct qlcnic_adapter *adapter,
 int qlcnic_82xx_fw_cmd_create_rx_ctx(struct qlcnic_adapter *);
 int qlcnic_82xx_fw_cmd_create_tx_ctx(struct qlcnic_adapter *,
 				     struct qlcnic_host_tx_ring *tx_ring, int);
-int qlcnic_82xx_sre_macaddr_change(struct qlcnic_adapter *, u8 *, __le16, u8);
+void qlcnic_82xx_fw_cmd_del_rx_ctx(struct qlcnic_adapter *);
+void qlcnic_82xx_fw_cmd_del_tx_ctx(struct qlcnic_adapter *,
+				   struct qlcnic_host_tx_ring *);
+int qlcnic_82xx_sre_macaddr_change(struct qlcnic_adapter *, u8 *, u16, u8);
 int qlcnic_82xx_get_mac_address(struct qlcnic_adapter *, u8*);
 int qlcnic_82xx_get_nic_info(struct qlcnic_adapter *, struct qlcnic_info *, u8);
 int qlcnic_82xx_set_nic_info(struct qlcnic_adapter *, struct qlcnic_info *);
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c
index 5fa847fe388a..d3f8797efcc3 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c
@@ -9,6 +9,7 @@
 #include <linux/if_vlan.h>
 #include <net/ip.h>
 #include <linux/ipv6.h>
+#include <net/checksum.h>
 
 #include "qlcnic.h"
 
@@ -146,7 +147,10 @@ static inline u8 qlcnic_mac_hash(u64 mac)
 static inline u32 qlcnic_get_ref_handle(struct qlcnic_adapter *adapter,
 					u16 handle, u8 ring_id)
 {
-	if (adapter->pdev->device == PCI_DEVICE_ID_QLOGIC_QLE834X)
+	unsigned short device = adapter->pdev->device;
+
+	if ((device == PCI_DEVICE_ID_QLOGIC_QLE834X) ||
+	    (device == PCI_DEVICE_ID_QLOGIC_VF_QLE834X))
 		return handle | (ring_id << 15);
 	else
 		return handle;
@@ -158,7 +162,7 @@ static inline int qlcnic_82xx_is_lb_pkt(u64 sts_data)
 }
 
 void qlcnic_add_lb_filter(struct qlcnic_adapter *adapter, struct sk_buff *skb,
-			  int loopback_pkt, __le16 vlan_id)
+			  int loopback_pkt, u16 vlan_id)
 {
 	struct ethhdr *phdr = (struct ethhdr *)(skb->data);
 	struct qlcnic_filter *fil, *tmp_fil;
@@ -236,7 +240,7 @@ void qlcnic_add_lb_filter(struct qlcnic_adapter *adapter, struct sk_buff *skb,
 }
 
 void qlcnic_82xx_change_filter(struct qlcnic_adapter *adapter, u64 *uaddr,
-			       __le16 vlan_id)
+			       u16 vlan_id)
 {
 	struct cmd_desc_type0 *hwdesc;
 	struct qlcnic_nic_req *req;
@@ -261,7 +265,7 @@ void qlcnic_82xx_change_filter(struct qlcnic_adapter *adapter, u64 *uaddr,
 	memcpy(mac_req->mac_addr, &uaddr, ETH_ALEN);
 
 	vlan_req = (struct qlcnic_vlan_req *)&req->words[1];
-	vlan_req->vlan_id = vlan_id;
+	vlan_req->vlan_id = cpu_to_le16(vlan_id);
 
 	tx_ring->producer = get_next_index(producer, tx_ring->num_desc);
 	smp_mb();
@@ -277,7 +281,7 @@ static void qlcnic_send_filter(struct qlcnic_adapter *adapter,
 	struct net_device *netdev = adapter->netdev;
 	struct ethhdr *phdr = (struct ethhdr *)(skb->data);
 	u64 src_addr = 0;
-	__le16 vlan_id = 0;
+	u16 vlan_id = 0;
 	u8 hindex;
 
 	if (ether_addr_equal(phdr->h_source, adapter->mac_addr))
@@ -340,14 +344,14 @@ static int qlcnic_tx_pkt(struct qlcnic_adapter *adapter,
 		flags = FLAGS_VLAN_OOB;
 		vlan_tci = vlan_tx_tag_get(skb);
 	}
-	if (unlikely(adapter->pvid)) {
+	if (unlikely(adapter->tx_pvid)) {
 		if (vlan_tci && !(adapter->flags & QLCNIC_TAGGING_ENABLED))
 			return -EIO;
 		if (vlan_tci && (adapter->flags & QLCNIC_TAGGING_ENABLED))
 			goto set_flags;
 
 		flags = FLAGS_VLAN_OOB;
-		vlan_tci = adapter->pvid;
+		vlan_tci = adapter->tx_pvid;
 	}
 set_flags:
 	qlcnic_set_tx_vlan_tci(first_desc, vlan_tci);
@@ -975,10 +979,10 @@ static inline int qlcnic_check_rx_tagging(struct qlcnic_adapter *adapter,
 		memmove(skb->data + VLAN_HLEN, eth_hdr, ETH_ALEN * 2);
 		skb_pull(skb, VLAN_HLEN);
 	}
-	if (!adapter->pvid)
+	if (!adapter->rx_pvid)
 		return 0;
 
-	if (*vlan_tag == adapter->pvid) {
+	if (*vlan_tag == adapter->rx_pvid) {
 		/* Outer vlan tag. Packet should follow non-vlan path */
 		*vlan_tag = 0xffff;
 		return 0;
@@ -1024,8 +1028,7 @@ qlcnic_process_rcv(struct qlcnic_adapter *adapter,
 	    (adapter->flags & QLCNIC_ESWITCH_ENABLED)) {
 		t_vid = 0;
 		is_lb_pkt = qlcnic_82xx_is_lb_pkt(sts_data0);
-		qlcnic_add_lb_filter(adapter, skb, is_lb_pkt,
-				     cpu_to_le16(t_vid));
+		qlcnic_add_lb_filter(adapter, skb, is_lb_pkt, t_vid);
 	}
 
 	if (length > rds_ring->skb_size)
@@ -1045,7 +1048,7 @@ qlcnic_process_rcv(struct qlcnic_adapter *adapter,
 	skb->protocol = eth_type_trans(skb, netdev);
 
 	if (vid != 0xffff)
-		__vlan_hwaccel_put_tag(skb, vid);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
 
 	napi_gro_receive(&sds_ring->napi, skb);
 
@@ -1102,8 +1105,7 @@ qlcnic_process_lro(struct qlcnic_adapter *adapter,
 	    (adapter->flags & QLCNIC_ESWITCH_ENABLED)) {
 		t_vid = 0;
 		is_lb_pkt = qlcnic_82xx_is_lb_pkt(sts_data0);
-		qlcnic_add_lb_filter(adapter, skb, is_lb_pkt,
-				     cpu_to_le16(t_vid));
+		qlcnic_add_lb_filter(adapter, skb, is_lb_pkt, t_vid);
 	}
 
 	if (timestamp)
@@ -1131,9 +1133,8 @@ qlcnic_process_lro(struct qlcnic_adapter *adapter,
 		iph = (struct iphdr *)skb->data;
 		th = (struct tcphdr *)(skb->data + (iph->ihl << 2));
 		length = (iph->ihl << 2) + (th->doff << 2) + lro_length;
+		csum_replace2(&iph->check, iph->tot_len, htons(length));
 		iph->tot_len = htons(length);
-		iph->check = 0;
-		iph->check = ip_fast_csum((unsigned char *)iph, iph->ihl);
 	}
 
 	th->psh = push;
@@ -1149,7 +1150,7 @@ qlcnic_process_lro(struct qlcnic_adapter *adapter,
 	}
 
 	if (vid != 0xffff)
-		__vlan_hwaccel_put_tag(skb, vid);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
 	netif_receive_skb(skb);
 
 	adapter->stats.lro_pkts++;
@@ -1496,8 +1497,7 @@ qlcnic_83xx_process_rcv(struct qlcnic_adapter *adapter,
 	    (adapter->flags & QLCNIC_ESWITCH_ENABLED)) {
 		t_vid = 0;
 		is_lb_pkt = qlcnic_83xx_is_lb_pkt(sts_data[1], 0);
-		qlcnic_add_lb_filter(adapter, skb, is_lb_pkt,
-				     cpu_to_le16(t_vid));
+		qlcnic_add_lb_filter(adapter, skb, is_lb_pkt, t_vid);
 	}
 
 	if (length > rds_ring->skb_size)
@@ -1514,7 +1514,7 @@ qlcnic_83xx_process_rcv(struct qlcnic_adapter *adapter,
 	skb->protocol = eth_type_trans(skb, netdev);
 
 	if (vid != 0xffff)
-		__vlan_hwaccel_put_tag(skb, vid);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
 
 	napi_gro_receive(&sds_ring->napi, skb);
 
@@ -1566,8 +1566,7 @@ qlcnic_83xx_process_lro(struct qlcnic_adapter *adapter,
 	    (adapter->flags & QLCNIC_ESWITCH_ENABLED)) {
 		t_vid = 0;
 		is_lb_pkt = qlcnic_83xx_is_lb_pkt(sts_data[1], 1);
-		qlcnic_add_lb_filter(adapter, skb, is_lb_pkt,
-				     cpu_to_le16(t_vid));
+		qlcnic_add_lb_filter(adapter, skb, is_lb_pkt, t_vid);
 	}
 	if (qlcnic_83xx_is_tstamp(sts_data[1]))
 		data_offset = l4_hdr_offset + QLCNIC_TCP_TS_HDR_SIZE;
@@ -1594,9 +1593,8 @@ qlcnic_83xx_process_lro(struct qlcnic_adapter *adapter,
 		iph = (struct iphdr *)skb->data;
 		th = (struct tcphdr *)(skb->data + (iph->ihl << 2));
 		length = (iph->ihl << 2) + (th->doff << 2) + lro_length;
+		csum_replace2(&iph->check, iph->tot_len, htons(length));
 		iph->tot_len = htons(length);
-		iph->check = 0;
-		iph->check = ip_fast_csum((unsigned char *)iph, iph->ihl);
 	}
 
 	th->psh = push;
@@ -1612,7 +1610,7 @@ qlcnic_83xx_process_lro(struct qlcnic_adapter *adapter,
 	}
 
 	if (vid != 0xffff)
-		__vlan_hwaccel_put_tag(skb, vid);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
 
 	netif_receive_skb(skb);
 
@@ -1691,6 +1689,29 @@ skip:
 	return count;
 }
 
+static int qlcnic_83xx_msix_sriov_vf_poll(struct napi_struct *napi, int budget)
+{
+	int tx_complete;
+	int work_done;
+	struct qlcnic_host_sds_ring *sds_ring;
+	struct qlcnic_adapter *adapter;
+	struct qlcnic_host_tx_ring *tx_ring;
+
+	sds_ring = container_of(napi, struct qlcnic_host_sds_ring, napi);
+	adapter = sds_ring->adapter;
+	/* tx ring count = 1 */
+	tx_ring = adapter->tx_ring;
+
+	tx_complete = qlcnic_process_cmd_ring(adapter, tx_ring, budget);
+	work_done = qlcnic_83xx_process_rcv_ring(sds_ring, budget);
+	if ((work_done < budget) && tx_complete) {
+		napi_complete(&sds_ring->napi);
+		qlcnic_83xx_enable_intr(adapter, sds_ring);
+	}
+
+	return work_done;
+}
+
 static int qlcnic_83xx_poll(struct napi_struct *napi, int budget)
 {
 	int tx_complete;
@@ -1768,7 +1789,8 @@ void qlcnic_83xx_napi_enable(struct qlcnic_adapter *adapter)
 			qlcnic_83xx_enable_intr(adapter, sds_ring);
 	}
 
-	if (adapter->flags & QLCNIC_MSIX_ENABLED) {
+	if ((adapter->flags & QLCNIC_MSIX_ENABLED) &&
+	    !(adapter->flags & QLCNIC_TX_INTR_SHARED)) {
 		for (ring = 0; ring < adapter->max_drv_tx_rings; ring++) {
 			tx_ring = &adapter->tx_ring[ring];
 			napi_enable(&tx_ring->napi);
@@ -1795,7 +1817,8 @@ void qlcnic_83xx_napi_disable(struct qlcnic_adapter *adapter)
 		napi_disable(&sds_ring->napi);
 	}
 
-	if (adapter->flags & QLCNIC_MSIX_ENABLED) {
+	if ((adapter->flags & QLCNIC_MSIX_ENABLED) &&
+	    !(adapter->flags & QLCNIC_TX_INTR_SHARED)) {
 		for (ring = 0; ring < adapter->max_drv_tx_rings; ring++) {
 			tx_ring = &adapter->tx_ring[ring];
 			qlcnic_83xx_disable_tx_intr(adapter, tx_ring);
@@ -1808,7 +1831,7 @@ void qlcnic_83xx_napi_disable(struct qlcnic_adapter *adapter)
 int qlcnic_83xx_napi_add(struct qlcnic_adapter *adapter,
 			 struct net_device *netdev)
 {
-	int ring, max_sds_rings;
+	int ring, max_sds_rings, temp;
 	struct qlcnic_host_sds_ring *sds_ring;
 	struct qlcnic_host_tx_ring *tx_ring;
 	struct qlcnic_recv_context *recv_ctx = adapter->recv_ctx;
@@ -1819,14 +1842,23 @@ int qlcnic_83xx_napi_add(struct qlcnic_adapter *adapter,
 	max_sds_rings = adapter->max_sds_rings;
 	for (ring = 0; ring < adapter->max_sds_rings; ring++) {
 		sds_ring = &recv_ctx->sds_rings[ring];
-		if (adapter->flags & QLCNIC_MSIX_ENABLED)
-			netif_napi_add(netdev, &sds_ring->napi,
-				       qlcnic_83xx_rx_poll,
-				       QLCNIC_NETDEV_WEIGHT * 2);
-		else
+		if (adapter->flags & QLCNIC_MSIX_ENABLED) {
+			if (!(adapter->flags & QLCNIC_TX_INTR_SHARED)) {
+				netif_napi_add(netdev, &sds_ring->napi,
+					       qlcnic_83xx_rx_poll,
+					       QLCNIC_NETDEV_WEIGHT * 2);
+			} else {
+				temp = QLCNIC_NETDEV_WEIGHT / max_sds_rings;
+				netif_napi_add(netdev, &sds_ring->napi,
+					       qlcnic_83xx_msix_sriov_vf_poll,
+					       temp);
+			}
+
+		} else {
 			netif_napi_add(netdev, &sds_ring->napi,
 				       qlcnic_83xx_poll,
 				       QLCNIC_NETDEV_WEIGHT / max_sds_rings);
+		}
 	}
 
 	if (qlcnic_alloc_tx_rings(adapter, netdev)) {
@@ -1834,7 +1866,8 @@ int qlcnic_83xx_napi_add(struct qlcnic_adapter *adapter,
 		return -ENOMEM;
 	}
 
-	if (adapter->flags & QLCNIC_MSIX_ENABLED) {
+	if ((adapter->flags & QLCNIC_MSIX_ENABLED) &&
+	    !(adapter->flags & QLCNIC_TX_INTR_SHARED)) {
 		for (ring = 0; ring < adapter->max_drv_tx_rings; ring++) {
 			tx_ring = &adapter->tx_ring[ring];
 			netif_napi_add(netdev, &tx_ring->napi,
@@ -1860,7 +1893,8 @@ void qlcnic_83xx_napi_del(struct qlcnic_adapter *adapter)
 
 	qlcnic_free_sds_rings(adapter->recv_ctx);
 
-	if ((adapter->flags & QLCNIC_MSIX_ENABLED)) {
+	if ((adapter->flags & QLCNIC_MSIX_ENABLED) &&
+	    !(adapter->flags & QLCNIC_TX_INTR_SHARED)) {
 		for (ring = 0; ring < adapter->max_drv_tx_rings; ring++) {
 			tx_ring = &adapter->tx_ring[ring];
 			netif_napi_del(&tx_ring->napi);
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
index 28a6d4838364..264d5a4f8153 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
@@ -9,6 +9,7 @@
 #include <linux/interrupt.h>
 
 #include "qlcnic.h"
+#include "qlcnic_sriov.h"
 #include "qlcnic_hw.h"
 
 #include <linux/swab.h>
@@ -85,8 +86,8 @@ static void qlcnic_dev_set_npar_ready(struct qlcnic_adapter *);
 static int qlcnicvf_start_firmware(struct qlcnic_adapter *);
 static void qlcnic_set_netdev_features(struct qlcnic_adapter *,
 				struct qlcnic_esw_func_cfg *);
-static int qlcnic_vlan_rx_add(struct net_device *, u16);
-static int qlcnic_vlan_rx_del(struct net_device *, u16);
+static int qlcnic_vlan_rx_add(struct net_device *, __be16, u16);
+static int qlcnic_vlan_rx_del(struct net_device *, __be16, u16);
 
 #define QLCNIC_IS_TSO_CAPABLE(adapter)	\
 	((adapter)->ahw->capabilities & QLCNIC_FW_CAPABILITY_TSO)
@@ -109,6 +110,7 @@ static u32 qlcnic_vlan_tx_check(struct qlcnic_adapter *adapter)
 static DEFINE_PCI_DEVICE_TABLE(qlcnic_pci_tbl) = {
 	ENTRY(PCI_DEVICE_ID_QLOGIC_QLE824X),
 	ENTRY(PCI_DEVICE_ID_QLOGIC_QLE834X),
+	ENTRY(PCI_DEVICE_ID_QLOGIC_VF_QLE834X),
 	{0,}
 };
 
@@ -154,25 +156,112 @@ static const u32 qlcnic_reg_tbl[] = {
 };
 
 static const struct qlcnic_board_info qlcnic_boards[] = {
-	{0x1077, 0x8020, 0x1077, 0x203,
-	 "8200 Series Single Port 10GbE Converged Network Adapter"
-	 "(TCP/IP Networking)"},
-	{0x1077, 0x8020, 0x1077, 0x207,
-	 "8200 Series Dual Port 10GbE Converged Network Adapter"
-	 "(TCP/IP Networking)"},
-	{0x1077, 0x8020, 0x1077, 0x20b,
-	 "3200 Series Dual Port 10Gb Intelligent Ethernet Adapter"},
-	{0x1077, 0x8020, 0x1077, 0x20c,
-	 "3200 Series Quad Port 1Gb Intelligent Ethernet Adapter"},
-	{0x1077, 0x8020, 0x1077, 0x20f,
-	 "3200 Series Single Port 10Gb Intelligent Ethernet Adapter"},
-	{0x1077, 0x8020, 0x103c, 0x3733,
-	 "NC523SFP 10Gb 2-port Server Adapter"},
-	{0x1077, 0x8020, 0x103c, 0x3346,
-	 "CN1000Q Dual Port Converged Network Adapter"},
-	{0x1077, 0x8020, 0x1077, 0x210,
-	 "QME8242-k 10GbE Dual Port Mezzanine Card"},
-	{0x1077, 0x8020, 0x0, 0x0, "cLOM8214 1/10GbE Controller"},
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE834X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x24e,
+	  "8300 Series Dual Port 10GbE Converged Network Adapter "
+	  "(TCP/IP Networking)" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE834X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x243,
+	  "8300 Series Single Port 10GbE Converged Network Adapter "
+	  "(TCP/IP Networking)" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE834X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x24a,
+	  "8300 Series Dual Port 10GbE Converged Network Adapter "
+	  "(TCP/IP Networking)" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE834X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x246,
+	  "8300 Series Dual Port 10GbE Converged Network Adapter "
+	  "(TCP/IP Networking)" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE834X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x252,
+	  "8300 Series Dual Port 10GbE Converged Network Adapter "
+	  "(TCP/IP Networking)" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE834X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x26e,
+	  "8300 Series Dual Port 10GbE Converged Network Adapter "
+	  "(TCP/IP Networking)" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE834X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x260,
+	  "8300 Series Dual Port 10GbE Converged Network Adapter "
+	  "(TCP/IP Networking)" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE834X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x266,
+	  "8300 Series Single Port 10GbE Converged Network Adapter "
+	  "(TCP/IP Networking)" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE834X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x269,
+	  "8300 Series Dual Port 10GbE Converged Network Adapter "
+	  "(TCP/IP Networking)" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE834X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x271,
+	  "8300 Series Dual Port 10GbE Converged Network Adapter "
+	  "(TCP/IP Networking)" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE834X,
+	  0x0, 0x0, "8300 Series 1/10GbE Controller" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE824X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x203,
+	  "8200 Series Single Port 10GbE Converged Network Adapter"
+	  "(TCP/IP Networking)" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE824X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x207,
+	  "8200 Series Dual Port 10GbE Converged Network Adapter"
+	  "(TCP/IP Networking)" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE824X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x20b,
+	  "3200 Series Dual Port 10Gb Intelligent Ethernet Adapter" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE824X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x20c,
+	  "3200 Series Quad Port 1Gb Intelligent Ethernet Adapter" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE824X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x20f,
+	  "3200 Series Single Port 10Gb Intelligent Ethernet Adapter" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE824X,
+	  0x103c, 0x3733,
+	  "NC523SFP 10Gb 2-port Server Adapter" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE824X,
+	  0x103c, 0x3346,
+	  "CN1000Q Dual Port Converged Network Adapter" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE824X,
+	  PCI_VENDOR_ID_QLOGIC,
+	  0x210,
+	  "QME8242-k 10GbE Dual Port Mezzanine Card" },
+	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE824X,
+	  0x0, 0x0, "cLOM8214 1/10GbE Controller" },
 };
 
 #define NUM_SUPPORTED_BOARDS ARRAY_SIZE(qlcnic_boards)
@@ -198,8 +287,7 @@ void qlcnic_free_sds_rings(struct qlcnic_recv_context *recv_ctx)
 	recv_ctx->sds_rings = NULL;
 }
 
-static int
-qlcnic_read_mac_addr(struct qlcnic_adapter *adapter)
+int qlcnic_read_mac_addr(struct qlcnic_adapter *adapter)
 {
 	u8 mac_addr[ETH_ALEN];
 	struct net_device *netdev = adapter->netdev;
@@ -225,6 +313,9 @@ static int qlcnic_set_mac(struct net_device *netdev, void *p)
 	struct qlcnic_adapter *adapter = netdev_priv(netdev);
 	struct sockaddr *addr = p;
 
+	if (qlcnic_sriov_vf_check(adapter))
+		return -EINVAL;
+
 	if ((adapter->flags & QLCNIC_MAC_OVERRIDE_DISABLED))
 		return -EOPNOTSUPP;
 
@@ -253,11 +344,8 @@ static int qlcnic_fdb_del(struct ndmsg *ndm, struct nlattr *tb[],
 	struct qlcnic_adapter *adapter = netdev_priv(netdev);
 	int err = -EOPNOTSUPP;
 
-	if (!adapter->fdb_mac_learn) {
-		pr_info("%s: Driver mac learn is enabled, FDB operation not allowed\n",
-			__func__);
-		return err;
-	}
+	if (!adapter->fdb_mac_learn)
+		return ndo_dflt_fdb_del(ndm, tb, netdev, addr);
 
 	if (adapter->flags & QLCNIC_ESWITCH_ENABLED) {
 		if (is_unicast_ether_addr(addr))
@@ -277,11 +365,8 @@ static int qlcnic_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 	struct qlcnic_adapter *adapter = netdev_priv(netdev);
 	int err = 0;
 
-	if (!adapter->fdb_mac_learn) {
-		pr_info("%s: Driver mac learn is enabled, FDB operation not allowed\n",
-			__func__);
-		return -EOPNOTSUPP;
-	}
+	if (!adapter->fdb_mac_learn)
+		return ndo_dflt_fdb_add(ndm, tb, netdev, addr, flags);
 
 	if (!(adapter->flags & QLCNIC_ESWITCH_ENABLED)) {
 		pr_info("%s: FDB e-switch is not enabled\n", __func__);
@@ -292,7 +377,7 @@ static int qlcnic_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 		return err;
 
 	if (is_unicast_ether_addr(addr))
-		err = qlcnic_nic_add_mac(adapter, addr);
+		err = qlcnic_nic_add_mac(adapter, addr, 0);
 	else if (is_multicast_ether_addr(addr))
 		err = dev_mc_add_excl(netdev, addr);
 	else
@@ -306,11 +391,8 @@ static int qlcnic_fdb_dump(struct sk_buff *skb, struct netlink_callback *ncb,
 {
 	struct qlcnic_adapter *adapter = netdev_priv(netdev);
 
-	if (!adapter->fdb_mac_learn) {
-		pr_info("%s: Driver mac learn is enabled, FDB operation not allowed\n",
-			__func__);
-		return -EOPNOTSUPP;
-	}
+	if (!adapter->fdb_mac_learn)
+		return ndo_dflt_fdb_dump(skb, ncb, netdev, idx);
 
 	if (adapter->flags & QLCNIC_ESWITCH_ENABLED)
 		idx = ndo_dflt_fdb_dump(skb, ncb, netdev, idx);
@@ -346,6 +428,12 @@ static const struct net_device_ops qlcnic_netdev_ops = {
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller = qlcnic_poll_controller,
 #endif
+#ifdef CONFIG_QLCNIC_SRIOV
+	.ndo_set_vf_mac		= qlcnic_sriov_set_vf_mac,
+	.ndo_set_vf_tx_rate	= qlcnic_sriov_set_vf_tx_rate,
+	.ndo_get_vf_config	= qlcnic_sriov_get_vf_config,
+	.ndo_set_vf_vlan	= qlcnic_sriov_set_vf_vlan,
+#endif
 };
 
 static const struct net_device_ops qlcnic_netdev_failed_ops = {
@@ -387,6 +475,8 @@ static struct qlcnic_hardware_ops qlcnic_hw_ops = {
 	.process_lb_rcv_ring_diag	= qlcnic_82xx_process_rcv_ring_diag,
 	.create_rx_ctx			= qlcnic_82xx_fw_cmd_create_rx_ctx,
 	.create_tx_ctx			= qlcnic_82xx_fw_cmd_create_tx_ctx,
+	.del_rx_ctx			= qlcnic_82xx_fw_cmd_del_rx_ctx,
+	.del_tx_ctx			= qlcnic_82xx_fw_cmd_del_tx_ctx,
 	.setup_link_event		= qlcnic_82xx_linkevent_request,
 	.get_nic_info			= qlcnic_82xx_get_nic_info,
 	.get_pci_info			= qlcnic_82xx_get_pci_info,
@@ -402,13 +492,22 @@ static struct qlcnic_hardware_ops qlcnic_hw_ops = {
 	.config_promisc_mode		= qlcnic_82xx_nic_set_promisc,
 	.change_l2_filter		= qlcnic_82xx_change_filter,
 	.get_board_info			= qlcnic_82xx_get_board_info,
+	.free_mac_list			= qlcnic_82xx_free_mac_list,
 };
 
 int qlcnic_enable_msix(struct qlcnic_adapter *adapter, u32 num_msix)
 {
 	struct pci_dev *pdev = adapter->pdev;
 	int err = -1, i;
-	int max_tx_rings;
+	int max_tx_rings, tx_vector;
+
+	if (adapter->flags & QLCNIC_TX_INTR_SHARED) {
+		max_tx_rings = 0;
+		tx_vector = 0;
+	} else {
+		max_tx_rings = adapter->max_drv_tx_rings;
+		tx_vector = 1;
+	}
 
 	if (!adapter->msix_entries) {
 		adapter->msix_entries = kcalloc(num_msix,
@@ -431,7 +530,6 @@ int qlcnic_enable_msix(struct qlcnic_adapter *adapter, u32 num_msix)
 			if (qlcnic_83xx_check(adapter)) {
 				adapter->ahw->num_msix = num_msix;
 				/* subtract mail box and tx ring vectors */
-				max_tx_rings = adapter->max_drv_tx_rings;
 				adapter->max_sds_rings = num_msix -
 							 max_tx_rings - 1;
 			} else {
@@ -444,11 +542,11 @@ int qlcnic_enable_msix(struct qlcnic_adapter *adapter, u32 num_msix)
 				 "Unable to allocate %d MSI-X interrupt vectors\n",
 				 num_msix);
 			if (qlcnic_83xx_check(adapter)) {
-				if (err < QLC_83XX_MINIMUM_VECTOR)
+				if (err < (QLC_83XX_MINIMUM_VECTOR - tx_vector))
 					return err;
-				err -= (adapter->max_drv_tx_rings + 1);
+				err -= (max_tx_rings + 1);
 				num_msix = rounddown_pow_of_two(err);
-				num_msix += (adapter->max_drv_tx_rings + 1);
+				num_msix += (max_tx_rings + 1);
 			} else {
 				num_msix = rounddown_pow_of_two(err);
 			}
@@ -542,11 +640,10 @@ void qlcnic_teardown_intr(struct qlcnic_adapter *adapter)
 	}
 }
 
-static void
-qlcnic_cleanup_pci_map(struct qlcnic_adapter *adapter)
+static void qlcnic_cleanup_pci_map(struct qlcnic_hardware_context *ahw)
 {
-	if (adapter->ahw->pci_base0 != NULL)
-		iounmap(adapter->ahw->pci_base0);
+	if (ahw->pci_base0 != NULL)
+		iounmap(ahw->pci_base0);
 }
 
 static int qlcnic_get_act_pci_func(struct qlcnic_adapter *adapter)
@@ -721,6 +818,7 @@ static void qlcnic_get_bar_length(u32 dev_id, ulong *bar)
 		*bar = QLCNIC_82XX_BAR0_LENGTH;
 		break;
 	case PCI_DEVICE_ID_QLOGIC_QLE834X:
+	case PCI_DEVICE_ID_QLOGIC_VF_QLE834X:
 		*bar = QLCNIC_83XX_BAR0_LENGTH;
 		break;
 	default:
@@ -751,7 +849,7 @@ static int qlcnic_setup_pci_map(struct pci_dev *pdev,
 		return -EIO;
 	}
 
-	dev_info(&pdev->dev, "%dMB memory map\n", (int)(mem_len>>20));
+	dev_info(&pdev->dev, "%dKB memory map\n", (int)(mem_len >> 10));
 
 	ahw->pci_base0 = mem_ptr0;
 	ahw->pci_len0 = pci_len0;
@@ -891,24 +989,50 @@ void qlcnic_set_vlan_config(struct qlcnic_adapter *adapter,
 	else
 		adapter->flags |= QLCNIC_TAGGING_ENABLED;
 
-	if (esw_cfg->vlan_id)
-		adapter->pvid = esw_cfg->vlan_id;
-	else
-		adapter->pvid = 0;
+	if (esw_cfg->vlan_id) {
+		adapter->rx_pvid = esw_cfg->vlan_id;
+		adapter->tx_pvid = esw_cfg->vlan_id;
+	} else {
+		adapter->rx_pvid = 0;
+		adapter->tx_pvid = 0;
+	}
 }
 
 static int
-qlcnic_vlan_rx_add(struct net_device *netdev, u16 vid)
+qlcnic_vlan_rx_add(struct net_device *netdev, __be16 proto, u16 vid)
 {
 	struct qlcnic_adapter *adapter = netdev_priv(netdev);
+	int err;
+
+	if (qlcnic_sriov_vf_check(adapter)) {
+		err = qlcnic_sriov_cfg_vf_guest_vlan(adapter, vid, 1);
+		if (err) {
+			netdev_err(netdev,
+				   "Cannot add VLAN filter for VLAN id %d, err=%d",
+				   vid, err);
+			return err;
+		}
+	}
+
 	set_bit(vid, adapter->vlans);
 	return 0;
 }
 
 static int
-qlcnic_vlan_rx_del(struct net_device *netdev, u16 vid)
+qlcnic_vlan_rx_del(struct net_device *netdev, __be16 proto, u16 vid)
 {
 	struct qlcnic_adapter *adapter = netdev_priv(netdev);
+	int err;
+
+	if (qlcnic_sriov_vf_check(adapter)) {
+		err = qlcnic_sriov_cfg_vf_guest_vlan(adapter, vid, 0);
+		if (err) {
+			netdev_err(netdev,
+				   "Cannot delete VLAN filter for VLAN id %d, err=%d",
+				   vid, err);
+			return err;
+		}
+	}
 
 	qlcnic_restore_indev_addr(netdev, NETDEV_DOWN);
 	clear_bit(vid, adapter->vlans);
@@ -1250,7 +1374,7 @@ qlcnic_request_irq(struct qlcnic_adapter *adapter)
 	irq_handler_t handler;
 	struct qlcnic_host_sds_ring *sds_ring;
 	struct qlcnic_host_tx_ring *tx_ring;
-	int err, ring;
+	int err, ring, num_sds_rings;
 
 	unsigned long flags = 0;
 	struct net_device *netdev = adapter->netdev;
@@ -1281,10 +1405,20 @@ qlcnic_request_irq(struct qlcnic_adapter *adapter)
 		if (qlcnic_82xx_check(adapter) ||
 		    (qlcnic_83xx_check(adapter) &&
 		     (adapter->flags & QLCNIC_MSIX_ENABLED))) {
-			for (ring = 0; ring < adapter->max_sds_rings; ring++) {
+			num_sds_rings = adapter->max_sds_rings;
+			for (ring = 0; ring < num_sds_rings; ring++) {
 				sds_ring = &recv_ctx->sds_rings[ring];
-				snprintf(sds_ring->name, sizeof(int) + IFNAMSIZ,
-					 "%s[%d]", netdev->name, ring);
+				if (qlcnic_82xx_check(adapter) &&
+				    (ring == (num_sds_rings - 1)))
+					snprintf(sds_ring->name,
+						 sizeof(sds_ring->name),
+						 "qlcnic-%s[Tx0+Rx%d]",
+						 netdev->name, ring);
+				else
+					snprintf(sds_ring->name,
+						 sizeof(sds_ring->name),
+						 "qlcnic-%s[Rx%d]",
+						 netdev->name, ring);
 				err = request_irq(sds_ring->irq, handler, flags,
 						  sds_ring->name, sds_ring);
 				if (err)
@@ -1292,14 +1426,14 @@ qlcnic_request_irq(struct qlcnic_adapter *adapter)
 			}
 		}
 		if (qlcnic_83xx_check(adapter) &&
-		    (adapter->flags & QLCNIC_MSIX_ENABLED)) {
+		    (adapter->flags & QLCNIC_MSIX_ENABLED) &&
+		    !(adapter->flags & QLCNIC_TX_INTR_SHARED)) {
 			handler = qlcnic_msix_tx_intr;
 			for (ring = 0; ring < adapter->max_drv_tx_rings;
 			     ring++) {
 				tx_ring = &adapter->tx_ring[ring];
-				snprintf(tx_ring->name, sizeof(int) + IFNAMSIZ,
-					 "%s[%d]", netdev->name,
-					 adapter->max_sds_rings + ring);
+				snprintf(tx_ring->name, sizeof(tx_ring->name),
+					 "qlcnic-%s[Tx%d]", netdev->name, ring);
 				err = request_irq(tx_ring->irq, handler, flags,
 						  tx_ring->name, tx_ring);
 				if (err)
@@ -1328,7 +1462,8 @@ qlcnic_free_irq(struct qlcnic_adapter *adapter)
 				free_irq(sds_ring->irq, sds_ring);
 			}
 		}
-		if (qlcnic_83xx_check(adapter)) {
+		if (qlcnic_83xx_check(adapter) &&
+		    !(adapter->flags & QLCNIC_TX_INTR_SHARED)) {
 			for (ring = 0; ring < adapter->max_drv_tx_rings;
 			     ring++) {
 				tx_ring = &adapter->tx_ring[ring];
@@ -1418,9 +1553,12 @@ void __qlcnic_down(struct qlcnic_adapter *adapter, struct net_device *netdev)
 	if (!test_and_clear_bit(__QLCNIC_DEV_UP, &adapter->state))
 		return;
 
+	if (qlcnic_sriov_vf_check(adapter))
+		qlcnic_sriov_cleanup_async_list(&adapter->ahw->sriov->bc);
 	smp_mb();
 	spin_lock(&adapter->tx_clean_lock);
 	netif_carrier_off(netdev);
+	adapter->ahw->linkup = 0;
 	netif_tx_disable(netdev);
 
 	qlcnic_free_mac_list(adapter);
@@ -1545,7 +1683,9 @@ out:
 
 static int qlcnic_alloc_adapter_resources(struct qlcnic_adapter *adapter)
 {
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
 	int err = 0;
+
 	adapter->recv_ctx = kzalloc(sizeof(struct qlcnic_recv_context),
 				GFP_KERNEL);
 	if (!adapter->recv_ctx) {
@@ -1553,9 +1693,14 @@ static int qlcnic_alloc_adapter_resources(struct qlcnic_adapter *adapter)
 		goto err_out;
 	}
 	/* Initialize interrupt coalesce parameters */
-	adapter->ahw->coal.flag = QLCNIC_INTR_DEFAULT;
-	adapter->ahw->coal.rx_time_us = QLCNIC_DEFAULT_INTR_COALESCE_RX_TIME_US;
-	adapter->ahw->coal.rx_packets = QLCNIC_DEFAULT_INTR_COALESCE_RX_PACKETS;
+	ahw->coal.flag = QLCNIC_INTR_DEFAULT;
+	ahw->coal.type = QLCNIC_INTR_COAL_TYPE_RX;
+	ahw->coal.rx_time_us = QLCNIC_DEF_INTR_COALESCE_RX_TIME_US;
+	ahw->coal.rx_packets = QLCNIC_DEF_INTR_COALESCE_RX_PACKETS;
+	if (qlcnic_83xx_check(adapter)) {
+		ahw->coal.tx_time_us = QLCNIC_DEF_INTR_COALESCE_TX_TIME_US;
+		ahw->coal.tx_packets = QLCNIC_DEF_INTR_COALESCE_TX_PACKETS;
+	}
 	/* clear stats */
 	memset(&adapter->stats, 0, sizeof(adapter->stats));
 err_out:
@@ -1685,7 +1830,7 @@ qlcnic_reset_context(struct qlcnic_adapter *adapter)
 	return err;
 }
 
-static int
+int
 qlcnic_setup_netdev(struct qlcnic_adapter *adapter, struct net_device *netdev,
 		    int pci_using_dac)
 {
@@ -1701,11 +1846,14 @@ qlcnic_setup_netdev(struct qlcnic_adapter *adapter, struct net_device *netdev,
 
 	qlcnic_change_mtu(netdev, netdev->mtu);
 
-	SET_ETHTOOL_OPS(netdev, &qlcnic_ethtool_ops);
+	if (qlcnic_sriov_vf_check(adapter))
+		SET_ETHTOOL_OPS(netdev, &qlcnic_sriov_vf_ethtool_ops);
+	else
+		SET_ETHTOOL_OPS(netdev, &qlcnic_ethtool_ops);
 
 	netdev->features |= (NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_RXCSUM |
 			     NETIF_F_IPV6_CSUM | NETIF_F_GRO |
-			     NETIF_F_HW_VLAN_RX);
+			     NETIF_F_HW_VLAN_CTAG_RX);
 	netdev->vlan_features |= (NETIF_F_SG | NETIF_F_IP_CSUM |
 				  NETIF_F_IPV6_CSUM);
 
@@ -1720,7 +1868,10 @@ qlcnic_setup_netdev(struct qlcnic_adapter *adapter, struct net_device *netdev,
 	}
 
 	if (qlcnic_vlan_tx_check(adapter))
-		netdev->features |= (NETIF_F_HW_VLAN_TX);
+		netdev->features |= (NETIF_F_HW_VLAN_CTAG_TX);
+
+	if (qlcnic_sriov_vf_check(adapter))
+		netdev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
 
 	if (adapter->ahw->capabilities & QLCNIC_FW_CAPABILITY_HW_LRO)
 		netdev->features |= NETIF_F_LRO;
@@ -1820,6 +1971,9 @@ qlcnic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	u32 capab2;
 	char board_name[QLCNIC_MAX_BOARD_NAME_LEN + 19]; /* MAC + ": " + name */
 
+	if (pdev->is_virtfn)
+		return -ENODEV;
+
 	err = pci_enable_device(pdev);
 	if (err)
 		return err;
@@ -1844,12 +1998,18 @@ qlcnic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (!ahw)
 		goto err_out_free_res;
 
-	if (ent->device == PCI_DEVICE_ID_QLOGIC_QLE824X) {
+	switch (ent->device) {
+	case PCI_DEVICE_ID_QLOGIC_QLE824X:
 		ahw->hw_ops = &qlcnic_hw_ops;
-		ahw->reg_tbl = (u32 *)qlcnic_reg_tbl;
-	} else if (ent->device == PCI_DEVICE_ID_QLOGIC_QLE834X) {
+		ahw->reg_tbl = (u32 *) qlcnic_reg_tbl;
+		break;
+	case PCI_DEVICE_ID_QLOGIC_QLE834X:
 		qlcnic_83xx_register_map(ahw);
-	} else {
+		break;
+	case PCI_DEVICE_ID_QLOGIC_VF_QLE834X:
+		qlcnic_sriov_vf_register_map(ahw);
+		break;
+	default:
 		goto err_out_free_hw_res;
 	}
 
@@ -1911,11 +2071,13 @@ qlcnic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	} else if (qlcnic_83xx_check(adapter)) {
 		qlcnic_83xx_check_vf(adapter, ent);
 		adapter->portnum = adapter->ahw->pci_func;
-		err = qlcnic_83xx_init(adapter);
+		err = qlcnic_83xx_init(adapter, pci_using_dac);
 		if (err) {
 			dev_err(&pdev->dev, "%s: failed\n", __func__);
 			goto err_out_free_hw;
 		}
+		if (qlcnic_sriov_vf_check(adapter))
+			return 0;
 	} else {
 		dev_err(&pdev->dev,
 			"%s: failed. Please Reboot\n", __func__);
@@ -1932,6 +2094,12 @@ qlcnic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 			module_name(THIS_MODULE),
 			board_name, adapter->ahw->revision_id);
 	}
+
+	if (qlcnic_83xx_check(adapter) && !qlcnic_use_msi_x &&
+	    !!qlcnic_use_msi)
+		dev_warn(&pdev->dev,
+			 "83xx adapter do not support MSI interrupts\n");
+
 	err = qlcnic_setup_intr(adapter, 0);
 	if (err) {
 		dev_err(&pdev->dev, "Failed to setup interrupt\n");
@@ -1999,7 +2167,7 @@ err_out_free_netdev:
 	free_netdev(netdev);
 
 err_out_iounmap:
-	qlcnic_cleanup_pci_map(adapter);
+	qlcnic_cleanup_pci_map(ahw);
 
 err_out_free_hw_res:
 	kfree(ahw);
@@ -2024,11 +2192,13 @@ static void qlcnic_remove(struct pci_dev *pdev)
 		return;
 
 	netdev = adapter->netdev;
+	qlcnic_sriov_pf_disable(adapter);
 
 	qlcnic_cancel_idc_work(adapter);
 	ahw = adapter->ahw;
 
 	unregister_netdev(netdev);
+	qlcnic_sriov_cleanup(adapter);
 
 	if (qlcnic_83xx_check(adapter)) {
 		qlcnic_83xx_free_mbx_intr(adapter);
@@ -2054,7 +2224,7 @@ static void qlcnic_remove(struct pci_dev *pdev)
 
 	qlcnic_remove_sysfs(adapter);
 
-	qlcnic_cleanup_pci_map(adapter);
+	qlcnic_cleanup_pci_map(adapter->ahw);
 
 	qlcnic_release_firmware(adapter);
 
@@ -2084,6 +2254,7 @@ static int __qlcnic_shutdown(struct pci_dev *pdev)
 	if (netif_running(netdev))
 		qlcnic_down(adapter, netdev);
 
+	qlcnic_sriov_cleanup(adapter);
 	if (qlcnic_82xx_check(adapter))
 		qlcnic_clr_all_drv_state(adapter, 0);
 
@@ -3205,20 +3376,40 @@ qlcnicvf_start_firmware(struct qlcnic_adapter *adapter)
 	return err;
 }
 
-int qlcnic_validate_max_rss(u8 max_hw, u8 val)
+int qlcnic_validate_max_rss(struct qlcnic_adapter *adapter,
+			    __u32 val)
 {
+	struct net_device *netdev = adapter->netdev;
+	u8 max_hw = adapter->ahw->max_rx_ques;
 	u32 max_allowed;
 
-	if (max_hw > QLC_MAX_SDS_RINGS) {
-		max_hw = QLC_MAX_SDS_RINGS;
-		pr_info("max rss reset to %d\n", QLC_MAX_SDS_RINGS);
+	if (val > QLC_MAX_SDS_RINGS) {
+		netdev_err(netdev, "RSS value should not be higher than %u\n",
+			   QLC_MAX_SDS_RINGS);
+		return -EINVAL;
 	}
 
 	max_allowed = rounddown_pow_of_two(min_t(int, max_hw,
 						 num_online_cpus()));
 	if ((val > max_allowed) || (val < 2) || !is_power_of_2(val)) {
-		pr_info("rss_ring valid range [2 - %x] in powers of 2\n",
-			max_allowed);
+		if (!is_power_of_2(val))
+			netdev_err(netdev, "RSS value should be a power of 2\n");
+
+		if (val < 2)
+			netdev_err(netdev, "RSS value should not be lower than 2\n");
+
+		if (val > max_hw)
+			netdev_err(netdev,
+				   "RSS value should not be higher than[%u], the max RSS rings supported by the adapter\n",
+				   max_hw);
+
+		if (val > num_online_cpus())
+			netdev_err(netdev,
+				   "RSS value should not be higher than[%u], number of online CPUs in the system\n",
+				   num_online_cpus());
+
+		netdev_err(netdev, "Unable to configure %u RSS rings\n", val);
+
 		return -EINVAL;
 	}
 	return 0;
@@ -3238,8 +3429,10 @@ int qlcnic_set_max_rss(struct qlcnic_adapter *adapter, u8 data, size_t len)
 
 	qlcnic_detach(adapter);
 
-	if (qlcnic_83xx_check(adapter))
+	if (qlcnic_83xx_check(adapter)) {
 		qlcnic_83xx_free_mbx_intr(adapter);
+		qlcnic_83xx_enable_mbx_poll(adapter);
+	}
 
 	qlcnic_teardown_intr(adapter);
 	err = qlcnic_setup_intr(adapter, data);
@@ -3253,6 +3446,7 @@ int qlcnic_set_max_rss(struct qlcnic_adapter *adapter, u8 data, size_t len)
 		/* register for NIC IDC AEN Events */
 		qlcnic_83xx_register_nic_idc_func(adapter, 1);
 		err = qlcnic_83xx_setup_mbx_intr(adapter);
+		qlcnic_83xx_disable_mbx_poll(adapter);
 		if (err) {
 			dev_err(&adapter->pdev->dev,
 				"failed to setup mbx interrupt\n");
@@ -3318,7 +3512,7 @@ void qlcnic_restore_indev_addr(struct net_device *netdev, unsigned long event)
 
 	rcu_read_lock();
 	for_each_set_bit(vid, adapter->vlans, VLAN_N_VID) {
-		dev = __vlan_find_dev_deep(netdev, vid);
+		dev = __vlan_find_dev_deep(netdev, htons(ETH_P_8021Q), vid);
 		if (!dev)
 			continue;
 		qlcnic_config_indev_addr(adapter, dev, event);
@@ -3432,7 +3626,10 @@ static struct pci_driver qlcnic_driver = {
 	.resume = qlcnic_resume,
 #endif
 	.shutdown = qlcnic_shutdown,
-	.err_handler = &qlcnic_err_handler
+	.err_handler = &qlcnic_err_handler,
+#ifdef CONFIG_QLCNIC_SRIOV
+	.sriov_configure = qlcnic_pci_sriov_configure,
+#endif
 
 };
 
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_minidump.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_minidump.c
index abbd22c814a6..4b9bab18ebd9 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_minidump.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_minidump.c
@@ -810,11 +810,8 @@ static int __qlcnic_fw_cmd_get_minidump_temp(struct qlcnic_adapter *adapter,
 
 	tmp_addr = dma_alloc_coherent(&adapter->pdev->dev, temp_size,
 				      &tmp_addr_t, GFP_KERNEL);
-	if (!tmp_addr) {
-		dev_err(&adapter->pdev->dev,
-			"Can't get memory for FW dump template\n");
+	if (!tmp_addr)
 		return -ENOMEM;
-	}
 
 	if (qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_GET_TEMP_HDR)) {
 		err = -ENOMEM;
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov.h
new file mode 100644
index 000000000000..d85fbb57c25b
--- /dev/null
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov.h
@@ -0,0 +1,263 @@
+/*
+ * QLogic qlcnic NIC Driver
+ * Copyright (c) 2009-2013 QLogic Corporation
+ *
+ * See LICENSE.qlcnic for copyright and licensing details.
+ */
+
+#ifndef _QLCNIC_83XX_SRIOV_H_
+#define _QLCNIC_83XX_SRIOV_H_
+
+#include "qlcnic.h"
+#include <linux/types.h>
+#include <linux/pci.h>
+
+extern const u32 qlcnic_83xx_reg_tbl[];
+extern const u32 qlcnic_83xx_ext_reg_tbl[];
+
+struct qlcnic_bc_payload {
+	u64 payload[126];
+};
+
+struct qlcnic_bc_hdr {
+#if defined(__LITTLE_ENDIAN)
+	u8	version;
+	u8	msg_type:4;
+	u8	rsvd1:3;
+	u8	op_type:1;
+	u8	num_cmds;
+	u8	num_frags;
+	u8	frag_num;
+	u8	cmd_op;
+	u16	seq_id;
+	u64	rsvd3;
+#elif defined(__BIG_ENDIAN)
+	u8	num_frags;
+	u8	num_cmds;
+	u8	op_type:1;
+	u8	rsvd1:3;
+	u8	msg_type:4;
+	u8	version;
+	u16	seq_id;
+	u8	cmd_op;
+	u8	frag_num;
+	u64	rsvd3;
+#endif
+};
+
+enum qlcnic_bc_commands {
+	QLCNIC_BC_CMD_CHANNEL_INIT = 0x0,
+	QLCNIC_BC_CMD_CHANNEL_TERM = 0x1,
+	QLCNIC_BC_CMD_GET_ACL = 0x2,
+	QLCNIC_BC_CMD_CFG_GUEST_VLAN = 0x3,
+};
+
+#define QLC_BC_CMD 1
+
+struct qlcnic_trans_list {
+	/* Lock for manipulating list */
+	spinlock_t		lock;
+	struct list_head	wait_list;
+	int			count;
+};
+
+enum qlcnic_trans_state {
+	QLC_INIT = 0,
+	QLC_WAIT_FOR_CHANNEL_FREE,
+	QLC_WAIT_FOR_RESP,
+	QLC_ABORT,
+	QLC_END,
+};
+
+struct qlcnic_bc_trans {
+	u8				func_id;
+	u8				active;
+	u8				curr_rsp_frag;
+	u8				curr_req_frag;
+	u16				cmd_id;
+	u16				req_pay_size;
+	u16				rsp_pay_size;
+	u32				trans_id;
+	enum qlcnic_trans_state		trans_state;
+	struct list_head		list;
+	struct qlcnic_bc_hdr		*req_hdr;
+	struct qlcnic_bc_hdr		*rsp_hdr;
+	struct qlcnic_bc_payload	*req_pay;
+	struct qlcnic_bc_payload	*rsp_pay;
+	struct completion		resp_cmpl;
+	struct qlcnic_vf_info		*vf;
+};
+
+enum qlcnic_vf_state {
+	QLC_BC_VF_SEND = 0,
+	QLC_BC_VF_RECV,
+	QLC_BC_VF_CHANNEL,
+	QLC_BC_VF_STATE,
+	QLC_BC_VF_FLR,
+	QLC_BC_VF_SOFT_FLR,
+};
+
+enum qlcnic_vlan_mode {
+	QLC_NO_VLAN_MODE = 0,
+	QLC_PVID_MODE,
+	QLC_GUEST_VLAN_MODE,
+};
+
+struct qlcnic_resources {
+	u16 num_tx_mac_filters;
+	u16 num_rx_ucast_mac_filters;
+	u16 num_rx_mcast_mac_filters;
+
+	u16 num_txvlan_keys;
+
+	u16 num_rx_queues;
+	u16 num_tx_queues;
+
+	u16 num_rx_buf_rings;
+	u16 num_rx_status_rings;
+
+	u16 num_destip;
+	u32 num_lro_flows_supported;
+	u16 max_local_ipv6_addrs;
+	u16 max_remote_ipv6_addrs;
+};
+
+struct qlcnic_vport {
+	u16			handle;
+	u16			max_tx_bw;
+	u16			min_tx_bw;
+	u8			vlan_mode;
+	u16			vlan;
+	u8			qos;
+	u8			mac[6];
+};
+
+struct qlcnic_vf_info {
+	u8				pci_func;
+	u16				rx_ctx_id;
+	u16				tx_ctx_id;
+	unsigned long			state;
+	struct completion		ch_free_cmpl;
+	struct work_struct		trans_work;
+	struct work_struct		flr_work;
+	/* It synchronizes commands sent from VF */
+	struct mutex			send_cmd_lock;
+	struct qlcnic_bc_trans		*send_cmd;
+	struct qlcnic_bc_trans		*flr_trans;
+	struct qlcnic_trans_list	rcv_act;
+	struct qlcnic_trans_list	rcv_pend;
+	struct qlcnic_adapter		*adapter;
+	struct qlcnic_vport		*vp;
+};
+
+struct qlcnic_async_work_list {
+	struct list_head	list;
+	struct work_struct	work;
+	void			*ptr;
+};
+
+struct qlcnic_back_channel {
+	u16			trans_counter;
+	struct workqueue_struct *bc_trans_wq;
+	struct workqueue_struct *bc_async_wq;
+	struct workqueue_struct *bc_flr_wq;
+	struct list_head	async_list;
+};
+
+struct qlcnic_sriov {
+	u16				vp_handle;
+	u8				num_vfs;
+	u8				any_vlan;
+	u8				vlan_mode;
+	u16				num_allowed_vlans;
+	u16				*allowed_vlans;
+	u16				vlan;
+	struct qlcnic_resources		ff_max;
+	struct qlcnic_back_channel	bc;
+	struct qlcnic_vf_info		*vf_info;
+};
+
+int qlcnic_sriov_init(struct qlcnic_adapter *, int);
+void qlcnic_sriov_cleanup(struct qlcnic_adapter *);
+void __qlcnic_sriov_cleanup(struct qlcnic_adapter *);
+void qlcnic_sriov_vf_register_map(struct qlcnic_hardware_context *);
+int qlcnic_sriov_vf_init(struct qlcnic_adapter *, int);
+void qlcnic_sriov_vf_set_ops(struct qlcnic_adapter *);
+int qlcnic_sriov_func_to_index(struct qlcnic_adapter *, u8);
+int qlcnic_sriov_channel_cfg_cmd(struct qlcnic_adapter *, u8);
+void qlcnic_sriov_handle_bc_event(struct qlcnic_adapter *, u32);
+int qlcnic_sriov_cfg_bc_intr(struct qlcnic_adapter *, u8);
+void qlcnic_sriov_cleanup_async_list(struct qlcnic_back_channel *);
+void qlcnic_sriov_cleanup_list(struct qlcnic_trans_list *);
+int __qlcnic_sriov_add_act_list(struct qlcnic_sriov *, struct qlcnic_vf_info *,
+				struct qlcnic_bc_trans *);
+int qlcnic_sriov_get_vf_vport_info(struct qlcnic_adapter *,
+				   struct qlcnic_info *, u16);
+int qlcnic_sriov_cfg_vf_guest_vlan(struct qlcnic_adapter *, u16, u8);
+
+static inline bool qlcnic_sriov_enable_check(struct qlcnic_adapter *adapter)
+{
+	return test_bit(__QLCNIC_SRIOV_ENABLE, &adapter->state) ? true : false;
+}
+
+#ifdef CONFIG_QLCNIC_SRIOV
+void qlcnic_sriov_pf_process_bc_cmd(struct qlcnic_adapter *,
+				    struct qlcnic_bc_trans *,
+				    struct qlcnic_cmd_args *);
+void qlcnic_sriov_pf_disable(struct qlcnic_adapter *);
+void qlcnic_sriov_pf_cleanup(struct qlcnic_adapter *);
+int qlcnic_pci_sriov_configure(struct pci_dev *, int);
+void qlcnic_pf_set_interface_id_create_rx_ctx(struct qlcnic_adapter *, u32 *);
+void qlcnic_pf_set_interface_id_create_tx_ctx(struct qlcnic_adapter *, u32 *);
+void qlcnic_pf_set_interface_id_del_rx_ctx(struct qlcnic_adapter *, u32 *);
+void qlcnic_pf_set_interface_id_del_tx_ctx(struct qlcnic_adapter *, u32 *);
+void qlcnic_pf_set_interface_id_promisc(struct qlcnic_adapter *, u32 *);
+void qlcnic_pf_set_interface_id_ipaddr(struct qlcnic_adapter *, u32 *);
+void qlcnic_pf_set_interface_id_macaddr(struct qlcnic_adapter *, u32 *);
+void qlcnic_sriov_pf_handle_flr(struct qlcnic_sriov *, struct qlcnic_vf_info *);
+bool qlcnic_sriov_soft_flr_check(struct qlcnic_adapter *,
+				 struct qlcnic_bc_trans *,
+				 struct qlcnic_vf_info *);
+void qlcnic_sriov_pf_reset(struct qlcnic_adapter *);
+int qlcnic_sriov_pf_reinit(struct qlcnic_adapter *);
+int qlcnic_sriov_set_vf_mac(struct net_device *, int, u8 *);
+int qlcnic_sriov_set_vf_tx_rate(struct net_device *, int, int);
+int qlcnic_sriov_get_vf_config(struct net_device *, int ,
+			       struct ifla_vf_info *);
+int qlcnic_sriov_set_vf_vlan(struct net_device *, int, u16, u8);
+#else
+static inline void qlcnic_sriov_pf_disable(struct qlcnic_adapter *adapter) {}
+static inline void qlcnic_sriov_pf_cleanup(struct qlcnic_adapter *adapter) {}
+static inline void
+qlcnic_pf_set_interface_id_create_rx_ctx(struct qlcnic_adapter *adapter,
+					 u32 *int_id) {}
+static inline void
+qlcnic_pf_set_interface_id_create_tx_ctx(struct qlcnic_adapter *adapter,
+					 u32 *int_id) {}
+static inline void
+qlcnic_pf_set_interface_id_del_rx_ctx(struct qlcnic_adapter *adapter,
+				      u32 *int_id) {}
+static inline void
+qlcnic_pf_set_interface_id_del_tx_ctx(struct qlcnic_adapter *adapter,
+				      u32 *int_id) {}
+static inline void
+qlcnic_pf_set_interface_id_ipaddr(struct qlcnic_adapter *adapter, u32 *int_id)
+{}
+static inline void
+qlcnic_pf_set_interface_id_macaddr(struct qlcnic_adapter *adapter, u32 *int_id)
+{}
+static inline void
+qlcnic_pf_set_interface_id_promisc(struct qlcnic_adapter *adapter, u32 *int_id)
+{}
+static inline void qlcnic_sriov_pf_handle_flr(struct qlcnic_sriov *sriov,
+					      struct qlcnic_vf_info *vf) {}
+static inline bool qlcnic_sriov_soft_flr_check(struct qlcnic_adapter *adapter,
+					       struct qlcnic_bc_trans *trans,
+					       struct qlcnic_vf_info *vf)
+{ return false; }
+static inline void qlcnic_sriov_pf_reset(struct qlcnic_adapter *adapter) {}
+static inline int qlcnic_sriov_pf_reinit(struct qlcnic_adapter *adapter)
+{ return 0; }
+#endif
+
+#endif
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov_common.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov_common.c
new file mode 100644
index 000000000000..44d547d78b84
--- /dev/null
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov_common.c
@@ -0,0 +1,1954 @@
+/*
+ * QLogic qlcnic NIC Driver
+ * Copyright (c) 2009-2013 QLogic Corporation
+ *
+ * See LICENSE.qlcnic for copyright and licensing details.
+ */
+
+#include "qlcnic_sriov.h"
+#include "qlcnic.h"
+#include "qlcnic_83xx_hw.h"
+#include <linux/types.h>
+
+#define QLC_BC_COMMAND	0
+#define QLC_BC_RESPONSE	1
+
+#define QLC_MBOX_RESP_TIMEOUT		(10 * HZ)
+#define QLC_MBOX_CH_FREE_TIMEOUT	(10 * HZ)
+
+#define QLC_BC_MSG		0
+#define QLC_BC_CFREE		1
+#define QLC_BC_FLR		2
+#define QLC_BC_HDR_SZ		16
+#define QLC_BC_PAYLOAD_SZ	(1024 - QLC_BC_HDR_SZ)
+
+#define QLC_DEFAULT_RCV_DESCRIPTORS_SRIOV_VF		2048
+#define QLC_DEFAULT_JUMBO_RCV_DESCRIPTORS_SRIOV_VF	512
+
+#define QLC_83XX_VF_RESET_FAIL_THRESH	8
+#define QLC_BC_CMD_MAX_RETRY_CNT	5
+
+static void qlcnic_sriov_vf_free_mac_list(struct qlcnic_adapter *);
+static int qlcnic_sriov_alloc_bc_mbx_args(struct qlcnic_cmd_args *, u32);
+static void qlcnic_sriov_vf_poll_dev_state(struct work_struct *);
+static void qlcnic_sriov_vf_cancel_fw_work(struct qlcnic_adapter *);
+static void qlcnic_sriov_cleanup_transaction(struct qlcnic_bc_trans *);
+static int qlcnic_sriov_vf_mbx_op(struct qlcnic_adapter *,
+				  struct qlcnic_cmd_args *);
+
+static struct qlcnic_hardware_ops qlcnic_sriov_vf_hw_ops = {
+	.read_crb			= qlcnic_83xx_read_crb,
+	.write_crb			= qlcnic_83xx_write_crb,
+	.read_reg			= qlcnic_83xx_rd_reg_indirect,
+	.write_reg			= qlcnic_83xx_wrt_reg_indirect,
+	.get_mac_address		= qlcnic_83xx_get_mac_address,
+	.setup_intr			= qlcnic_83xx_setup_intr,
+	.alloc_mbx_args			= qlcnic_83xx_alloc_mbx_args,
+	.mbx_cmd			= qlcnic_sriov_vf_mbx_op,
+	.get_func_no			= qlcnic_83xx_get_func_no,
+	.api_lock			= qlcnic_83xx_cam_lock,
+	.api_unlock			= qlcnic_83xx_cam_unlock,
+	.process_lb_rcv_ring_diag	= qlcnic_83xx_process_rcv_ring_diag,
+	.create_rx_ctx			= qlcnic_83xx_create_rx_ctx,
+	.create_tx_ctx			= qlcnic_83xx_create_tx_ctx,
+	.del_rx_ctx			= qlcnic_83xx_del_rx_ctx,
+	.del_tx_ctx			= qlcnic_83xx_del_tx_ctx,
+	.setup_link_event		= qlcnic_83xx_setup_link_event,
+	.get_nic_info			= qlcnic_83xx_get_nic_info,
+	.get_pci_info			= qlcnic_83xx_get_pci_info,
+	.set_nic_info			= qlcnic_83xx_set_nic_info,
+	.change_macvlan			= qlcnic_83xx_sre_macaddr_change,
+	.napi_enable			= qlcnic_83xx_napi_enable,
+	.napi_disable			= qlcnic_83xx_napi_disable,
+	.config_intr_coal		= qlcnic_83xx_config_intr_coal,
+	.config_rss			= qlcnic_83xx_config_rss,
+	.config_hw_lro			= qlcnic_83xx_config_hw_lro,
+	.config_promisc_mode		= qlcnic_83xx_nic_set_promisc,
+	.change_l2_filter		= qlcnic_83xx_change_l2_filter,
+	.get_board_info			= qlcnic_83xx_get_port_info,
+	.free_mac_list			= qlcnic_sriov_vf_free_mac_list,
+};
+
+static struct qlcnic_nic_template qlcnic_sriov_vf_ops = {
+	.config_bridged_mode	= qlcnic_config_bridged_mode,
+	.config_led		= qlcnic_config_led,
+	.cancel_idc_work        = qlcnic_sriov_vf_cancel_fw_work,
+	.napi_add		= qlcnic_83xx_napi_add,
+	.napi_del		= qlcnic_83xx_napi_del,
+	.config_ipaddr		= qlcnic_83xx_config_ipaddr,
+	.clear_legacy_intr	= qlcnic_83xx_clear_legacy_intr,
+};
+
+static const struct qlcnic_mailbox_metadata qlcnic_sriov_bc_mbx_tbl[] = {
+	{QLCNIC_BC_CMD_CHANNEL_INIT, 2, 2},
+	{QLCNIC_BC_CMD_CHANNEL_TERM, 2, 2},
+	{QLCNIC_BC_CMD_GET_ACL, 3, 14},
+	{QLCNIC_BC_CMD_CFG_GUEST_VLAN, 2, 2},
+};
+
+static inline bool qlcnic_sriov_bc_msg_check(u32 val)
+{
+	return (val & (1 << QLC_BC_MSG)) ? true : false;
+}
+
+static inline bool qlcnic_sriov_channel_free_check(u32 val)
+{
+	return (val & (1 << QLC_BC_CFREE)) ? true : false;
+}
+
+static inline bool qlcnic_sriov_flr_check(u32 val)
+{
+	return (val & (1 << QLC_BC_FLR)) ? true : false;
+}
+
+static inline u8 qlcnic_sriov_target_func_id(u32 val)
+{
+	return (val >> 4) & 0xff;
+}
+
+static int qlcnic_sriov_virtid_fn(struct qlcnic_adapter *adapter, int vf_id)
+{
+	struct pci_dev *dev = adapter->pdev;
+	int pos;
+	u16 stride, offset;
+
+	if (qlcnic_sriov_vf_check(adapter))
+		return 0;
+
+	pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_SRIOV);
+	pci_read_config_word(dev, pos + PCI_SRIOV_VF_OFFSET, &offset);
+	pci_read_config_word(dev, pos + PCI_SRIOV_VF_STRIDE, &stride);
+
+	return (dev->devfn + offset + stride * vf_id) & 0xff;
+}
+
+int qlcnic_sriov_init(struct qlcnic_adapter *adapter, int num_vfs)
+{
+	struct qlcnic_sriov *sriov;
+	struct qlcnic_back_channel *bc;
+	struct workqueue_struct *wq;
+	struct qlcnic_vport *vp;
+	struct qlcnic_vf_info *vf;
+	int err, i;
+
+	if (!qlcnic_sriov_enable_check(adapter))
+		return -EIO;
+
+	sriov  = kzalloc(sizeof(struct qlcnic_sriov), GFP_KERNEL);
+	if (!sriov)
+		return -ENOMEM;
+
+	adapter->ahw->sriov = sriov;
+	sriov->num_vfs = num_vfs;
+	bc = &sriov->bc;
+	sriov->vf_info = kzalloc(sizeof(struct qlcnic_vf_info) *
+				 num_vfs, GFP_KERNEL);
+	if (!sriov->vf_info) {
+		err = -ENOMEM;
+		goto qlcnic_free_sriov;
+	}
+
+	wq = create_singlethread_workqueue("bc-trans");
+	if (wq == NULL) {
+		err = -ENOMEM;
+		dev_err(&adapter->pdev->dev,
+			"Cannot create bc-trans workqueue\n");
+		goto qlcnic_free_vf_info;
+	}
+
+	bc->bc_trans_wq = wq;
+
+	wq = create_singlethread_workqueue("async");
+	if (wq == NULL) {
+		err = -ENOMEM;
+		dev_err(&adapter->pdev->dev, "Cannot create async workqueue\n");
+		goto qlcnic_destroy_trans_wq;
+	}
+
+	bc->bc_async_wq =  wq;
+	INIT_LIST_HEAD(&bc->async_list);
+
+	for (i = 0; i < num_vfs; i++) {
+		vf = &sriov->vf_info[i];
+		vf->adapter = adapter;
+		vf->pci_func = qlcnic_sriov_virtid_fn(adapter, i);
+		mutex_init(&vf->send_cmd_lock);
+		INIT_LIST_HEAD(&vf->rcv_act.wait_list);
+		INIT_LIST_HEAD(&vf->rcv_pend.wait_list);
+		spin_lock_init(&vf->rcv_act.lock);
+		spin_lock_init(&vf->rcv_pend.lock);
+		init_completion(&vf->ch_free_cmpl);
+
+		if (qlcnic_sriov_pf_check(adapter)) {
+			vp = kzalloc(sizeof(struct qlcnic_vport), GFP_KERNEL);
+			if (!vp) {
+				err = -ENOMEM;
+				goto qlcnic_destroy_async_wq;
+			}
+			sriov->vf_info[i].vp = vp;
+			vp->max_tx_bw = MAX_BW;
+			random_ether_addr(vp->mac);
+			dev_info(&adapter->pdev->dev,
+				 "MAC Address %pM is configured for VF %d\n",
+				 vp->mac, i);
+		}
+	}
+
+	return 0;
+
+qlcnic_destroy_async_wq:
+	destroy_workqueue(bc->bc_async_wq);
+
+qlcnic_destroy_trans_wq:
+	destroy_workqueue(bc->bc_trans_wq);
+
+qlcnic_free_vf_info:
+	kfree(sriov->vf_info);
+
+qlcnic_free_sriov:
+	kfree(adapter->ahw->sriov);
+	return err;
+}
+
+void qlcnic_sriov_cleanup_list(struct qlcnic_trans_list *t_list)
+{
+	struct qlcnic_bc_trans *trans;
+	struct qlcnic_cmd_args cmd;
+	unsigned long flags;
+
+	spin_lock_irqsave(&t_list->lock, flags);
+
+	while (!list_empty(&t_list->wait_list)) {
+		trans = list_first_entry(&t_list->wait_list,
+					 struct qlcnic_bc_trans, list);
+		list_del(&trans->list);
+		t_list->count--;
+		cmd.req.arg = (u32 *)trans->req_pay;
+		cmd.rsp.arg = (u32 *)trans->rsp_pay;
+		qlcnic_free_mbx_args(&cmd);
+		qlcnic_sriov_cleanup_transaction(trans);
+	}
+
+	spin_unlock_irqrestore(&t_list->lock, flags);
+}
+
+void __qlcnic_sriov_cleanup(struct qlcnic_adapter *adapter)
+{
+	struct qlcnic_sriov *sriov = adapter->ahw->sriov;
+	struct qlcnic_back_channel *bc = &sriov->bc;
+	struct qlcnic_vf_info *vf;
+	int i;
+
+	if (!qlcnic_sriov_enable_check(adapter))
+		return;
+
+	qlcnic_sriov_cleanup_async_list(bc);
+	destroy_workqueue(bc->bc_async_wq);
+
+	for (i = 0; i < sriov->num_vfs; i++) {
+		vf = &sriov->vf_info[i];
+		qlcnic_sriov_cleanup_list(&vf->rcv_pend);
+		cancel_work_sync(&vf->trans_work);
+		qlcnic_sriov_cleanup_list(&vf->rcv_act);
+	}
+
+	destroy_workqueue(bc->bc_trans_wq);
+
+	for (i = 0; i < sriov->num_vfs; i++)
+		kfree(sriov->vf_info[i].vp);
+
+	kfree(sriov->vf_info);
+	kfree(adapter->ahw->sriov);
+}
+
+static void qlcnic_sriov_vf_cleanup(struct qlcnic_adapter *adapter)
+{
+	qlcnic_sriov_channel_cfg_cmd(adapter, QLCNIC_BC_CMD_CHANNEL_TERM);
+	qlcnic_sriov_cfg_bc_intr(adapter, 0);
+	__qlcnic_sriov_cleanup(adapter);
+}
+
+void qlcnic_sriov_cleanup(struct qlcnic_adapter *adapter)
+{
+	if (qlcnic_sriov_pf_check(adapter))
+		qlcnic_sriov_pf_cleanup(adapter);
+
+	if (qlcnic_sriov_vf_check(adapter))
+		qlcnic_sriov_vf_cleanup(adapter);
+}
+
+static int qlcnic_sriov_post_bc_msg(struct qlcnic_adapter *adapter, u32 *hdr,
+				    u32 *pay, u8 pci_func, u8 size)
+{
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
+	unsigned long flags;
+	u32 rsp, mbx_val, fw_data, rsp_num, mbx_cmd, val;
+	u16 opcode;
+	u8 mbx_err_code;
+	int i, j;
+
+	opcode = ((struct qlcnic_bc_hdr *)hdr)->cmd_op;
+
+	if (!test_bit(QLC_83XX_MBX_READY, &adapter->ahw->idc.status)) {
+		dev_info(&adapter->pdev->dev,
+			 "Mailbox cmd attempted, 0x%x\n", opcode);
+		dev_info(&adapter->pdev->dev, "Mailbox detached\n");
+		return 0;
+	}
+
+	spin_lock_irqsave(&ahw->mbx_lock, flags);
+
+	mbx_val = QLCRDX(ahw, QLCNIC_HOST_MBX_CTRL);
+	if (mbx_val) {
+		QLCDB(adapter, DRV, "Mailbox cmd attempted, 0x%x\n", opcode);
+		spin_unlock_irqrestore(&ahw->mbx_lock, flags);
+		return QLCNIC_RCODE_TIMEOUT;
+	}
+	/* Fill in mailbox registers */
+	val = size + (sizeof(struct qlcnic_bc_hdr) / sizeof(u32));
+	mbx_cmd = 0x31 | (val << 16) | (adapter->ahw->fw_hal_version << 29);
+
+	writel(mbx_cmd, QLCNIC_MBX_HOST(ahw, 0));
+	mbx_cmd = 0x1 | (1 << 4);
+
+	if (qlcnic_sriov_pf_check(adapter))
+		mbx_cmd |= (pci_func << 5);
+
+	writel(mbx_cmd, QLCNIC_MBX_HOST(ahw, 1));
+	for (i = 2, j = 0; j < (sizeof(struct qlcnic_bc_hdr) / sizeof(u32));
+			i++, j++) {
+		writel(*(hdr++), QLCNIC_MBX_HOST(ahw, i));
+	}
+	for (j = 0; j < size; j++, i++)
+		writel(*(pay++), QLCNIC_MBX_HOST(ahw, i));
+
+	/* Signal FW about the impending command */
+	QLCWRX(ahw, QLCNIC_HOST_MBX_CTRL, QLCNIC_SET_OWNER);
+
+	/* Waiting for the mailbox cmd to complete and while waiting here
+	 * some AEN might arrive. If more than 5 seconds expire we can
+	 * assume something is wrong.
+	 */
+poll:
+	rsp = qlcnic_83xx_mbx_poll(adapter);
+	if (rsp != QLCNIC_RCODE_TIMEOUT) {
+		/* Get the FW response data */
+		fw_data = readl(QLCNIC_MBX_FW(ahw, 0));
+		if (fw_data &  QLCNIC_MBX_ASYNC_EVENT) {
+			__qlcnic_83xx_process_aen(adapter);
+			mbx_val = QLCRDX(ahw, QLCNIC_HOST_MBX_CTRL);
+			if (mbx_val)
+				goto poll;
+		}
+		mbx_err_code = QLCNIC_MBX_STATUS(fw_data);
+		rsp_num = QLCNIC_MBX_NUM_REGS(fw_data);
+		opcode = QLCNIC_MBX_RSP(fw_data);
+
+		switch (mbx_err_code) {
+		case QLCNIC_MBX_RSP_OK:
+		case QLCNIC_MBX_PORT_RSP_OK:
+			rsp = QLCNIC_RCODE_SUCCESS;
+			break;
+		default:
+			if (opcode == QLCNIC_CMD_CONFIG_MAC_VLAN) {
+				rsp = qlcnic_83xx_mac_rcode(adapter);
+				if (!rsp)
+					goto out;
+			}
+			dev_err(&adapter->pdev->dev,
+				"MBX command 0x%x failed with err:0x%x\n",
+				opcode, mbx_err_code);
+			rsp = mbx_err_code;
+			break;
+		}
+		goto out;
+	}
+
+	dev_err(&adapter->pdev->dev, "MBX command 0x%x timed out\n",
+		QLCNIC_MBX_RSP(mbx_cmd));
+	rsp = QLCNIC_RCODE_TIMEOUT;
+out:
+	/* clear fw mbx control register */
+	QLCWRX(ahw, QLCNIC_FW_MBX_CTRL, QLCNIC_CLR_OWNER);
+	spin_unlock_irqrestore(&adapter->ahw->mbx_lock, flags);
+	return rsp;
+}
+
+static void qlcnic_sriov_vf_cfg_buff_desc(struct qlcnic_adapter *adapter)
+{
+	adapter->num_rxd = QLC_DEFAULT_RCV_DESCRIPTORS_SRIOV_VF;
+	adapter->max_rxd = MAX_RCV_DESCRIPTORS_10G;
+	adapter->num_jumbo_rxd = QLC_DEFAULT_JUMBO_RCV_DESCRIPTORS_SRIOV_VF;
+	adapter->max_jumbo_rxd = MAX_JUMBO_RCV_DESCRIPTORS_10G;
+	adapter->num_txd = MAX_CMD_DESCRIPTORS;
+	adapter->max_rds_rings = MAX_RDS_RINGS;
+}
+
+int qlcnic_sriov_get_vf_vport_info(struct qlcnic_adapter *adapter,
+				   struct qlcnic_info *npar_info, u16 vport_id)
+{
+	struct device *dev = &adapter->pdev->dev;
+	struct qlcnic_cmd_args cmd;
+	int err;
+	u32 status;
+
+	err = qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_GET_NIC_INFO);
+	if (err)
+		return err;
+
+	cmd.req.arg[1] = vport_id << 16 | 0x1;
+	err = qlcnic_issue_cmd(adapter, &cmd);
+	if (err) {
+		dev_err(&adapter->pdev->dev,
+			"Failed to get vport info, err=%d\n", err);
+		qlcnic_free_mbx_args(&cmd);
+		return err;
+	}
+
+	status = cmd.rsp.arg[2] & 0xffff;
+	if (status & BIT_0)
+		npar_info->min_tx_bw = MSW(cmd.rsp.arg[2]);
+	if (status & BIT_1)
+		npar_info->max_tx_bw = LSW(cmd.rsp.arg[3]);
+	if (status & BIT_2)
+		npar_info->max_tx_ques = MSW(cmd.rsp.arg[3]);
+	if (status & BIT_3)
+		npar_info->max_tx_mac_filters = LSW(cmd.rsp.arg[4]);
+	if (status & BIT_4)
+		npar_info->max_rx_mcast_mac_filters = MSW(cmd.rsp.arg[4]);
+	if (status & BIT_5)
+		npar_info->max_rx_ucast_mac_filters = LSW(cmd.rsp.arg[5]);
+	if (status & BIT_6)
+		npar_info->max_rx_ip_addr = MSW(cmd.rsp.arg[5]);
+	if (status & BIT_7)
+		npar_info->max_rx_lro_flow = LSW(cmd.rsp.arg[6]);
+	if (status & BIT_8)
+		npar_info->max_rx_status_rings = MSW(cmd.rsp.arg[6]);
+	if (status & BIT_9)
+		npar_info->max_rx_buf_rings = LSW(cmd.rsp.arg[7]);
+
+	npar_info->max_rx_ques = MSW(cmd.rsp.arg[7]);
+	npar_info->max_tx_vlan_keys = LSW(cmd.rsp.arg[8]);
+	npar_info->max_local_ipv6_addrs = MSW(cmd.rsp.arg[8]);
+	npar_info->max_remote_ipv6_addrs = LSW(cmd.rsp.arg[9]);
+
+	dev_info(dev, "\n\tmin_tx_bw: %d, max_tx_bw: %d max_tx_ques: %d,\n"
+		 "\tmax_tx_mac_filters: %d max_rx_mcast_mac_filters: %d,\n"
+		 "\tmax_rx_ucast_mac_filters: 0x%x, max_rx_ip_addr: %d,\n"
+		 "\tmax_rx_lro_flow: %d max_rx_status_rings: %d,\n"
+		 "\tmax_rx_buf_rings: %d, max_rx_ques: %d, max_tx_vlan_keys %d\n"
+		 "\tlocal_ipv6_addr: %d, remote_ipv6_addr: %d\n",
+		 npar_info->min_tx_bw, npar_info->max_tx_bw,
+		 npar_info->max_tx_ques, npar_info->max_tx_mac_filters,
+		 npar_info->max_rx_mcast_mac_filters,
+		 npar_info->max_rx_ucast_mac_filters, npar_info->max_rx_ip_addr,
+		 npar_info->max_rx_lro_flow, npar_info->max_rx_status_rings,
+		 npar_info->max_rx_buf_rings, npar_info->max_rx_ques,
+		 npar_info->max_tx_vlan_keys, npar_info->max_local_ipv6_addrs,
+		 npar_info->max_remote_ipv6_addrs);
+
+	qlcnic_free_mbx_args(&cmd);
+	return err;
+}
+
+static int qlcnic_sriov_set_pvid_mode(struct qlcnic_adapter *adapter,
+				      struct qlcnic_cmd_args *cmd)
+{
+	adapter->rx_pvid = (cmd->rsp.arg[1] >> 16) & 0xffff;
+	adapter->flags &= ~QLCNIC_TAGGING_ENABLED;
+	return 0;
+}
+
+static int qlcnic_sriov_set_guest_vlan_mode(struct qlcnic_adapter *adapter,
+					    struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_sriov *sriov = adapter->ahw->sriov;
+	int i, num_vlans;
+	u16 *vlans;
+
+	if (sriov->allowed_vlans)
+		return 0;
+
+	sriov->any_vlan = cmd->rsp.arg[2] & 0xf;
+	if (!sriov->any_vlan)
+		return 0;
+
+	sriov->num_allowed_vlans = cmd->rsp.arg[2] >> 16;
+	num_vlans = sriov->num_allowed_vlans;
+	sriov->allowed_vlans = kzalloc(sizeof(u16) * num_vlans, GFP_KERNEL);
+	if (!sriov->allowed_vlans)
+		return -ENOMEM;
+
+	vlans = (u16 *)&cmd->rsp.arg[3];
+	for (i = 0; i < num_vlans; i++)
+		sriov->allowed_vlans[i] = vlans[i];
+
+	return 0;
+}
+
+static int qlcnic_sriov_get_vf_acl(struct qlcnic_adapter *adapter)
+{
+	struct qlcnic_sriov *sriov = adapter->ahw->sriov;
+	struct qlcnic_cmd_args cmd;
+	int ret;
+
+	ret = qlcnic_sriov_alloc_bc_mbx_args(&cmd, QLCNIC_BC_CMD_GET_ACL);
+	if (ret)
+		return ret;
+
+	ret = qlcnic_issue_cmd(adapter, &cmd);
+	if (ret) {
+		dev_err(&adapter->pdev->dev, "Failed to get ACL, err=%d\n",
+			ret);
+	} else {
+		sriov->vlan_mode = cmd.rsp.arg[1] & 0x3;
+		switch (sriov->vlan_mode) {
+		case QLC_GUEST_VLAN_MODE:
+			ret = qlcnic_sriov_set_guest_vlan_mode(adapter, &cmd);
+			break;
+		case QLC_PVID_MODE:
+			ret = qlcnic_sriov_set_pvid_mode(adapter, &cmd);
+			break;
+		}
+	}
+
+	qlcnic_free_mbx_args(&cmd);
+	return ret;
+}
+
+static int qlcnic_sriov_vf_init_driver(struct qlcnic_adapter *adapter)
+{
+	struct qlcnic_info nic_info;
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
+	int err;
+
+	err = qlcnic_sriov_get_vf_vport_info(adapter, &nic_info, 0);
+	if (err)
+		return err;
+
+	err = qlcnic_get_nic_info(adapter, &nic_info, ahw->pci_func);
+	if (err)
+		return -EIO;
+
+	err = qlcnic_sriov_get_vf_acl(adapter);
+	if (err)
+		return err;
+
+	if (qlcnic_83xx_get_port_info(adapter))
+		return -EIO;
+
+	qlcnic_sriov_vf_cfg_buff_desc(adapter);
+	adapter->flags |= QLCNIC_ADAPTER_INITIALIZED;
+	dev_info(&adapter->pdev->dev, "HAL Version: %d\n",
+		 adapter->ahw->fw_hal_version);
+
+	ahw->physical_port = (u8) nic_info.phys_port;
+	ahw->switch_mode = nic_info.switch_mode;
+	ahw->max_mtu = nic_info.max_mtu;
+	ahw->op_mode = nic_info.op_mode;
+	ahw->capabilities = nic_info.capabilities;
+	return 0;
+}
+
+static int qlcnic_sriov_setup_vf(struct qlcnic_adapter *adapter,
+				 int pci_using_dac)
+{
+	int err;
+
+	INIT_LIST_HEAD(&adapter->vf_mc_list);
+	if (!qlcnic_use_msi_x && !!qlcnic_use_msi)
+		dev_warn(&adapter->pdev->dev,
+			 "83xx adapter do not support MSI interrupts\n");
+
+	err = qlcnic_setup_intr(adapter, 1);
+	if (err) {
+		dev_err(&adapter->pdev->dev, "Failed to setup interrupt\n");
+		goto err_out_disable_msi;
+	}
+
+	err = qlcnic_83xx_setup_mbx_intr(adapter);
+	if (err)
+		goto err_out_disable_msi;
+
+	err = qlcnic_sriov_init(adapter, 1);
+	if (err)
+		goto err_out_disable_mbx_intr;
+
+	err = qlcnic_sriov_cfg_bc_intr(adapter, 1);
+	if (err)
+		goto err_out_cleanup_sriov;
+
+	err = qlcnic_sriov_channel_cfg_cmd(adapter, QLCNIC_BC_CMD_CHANNEL_INIT);
+	if (err)
+		goto err_out_disable_bc_intr;
+
+	err = qlcnic_sriov_vf_init_driver(adapter);
+	if (err)
+		goto err_out_send_channel_term;
+
+	err = qlcnic_setup_netdev(adapter, adapter->netdev, pci_using_dac);
+	if (err)
+		goto err_out_send_channel_term;
+
+	pci_set_drvdata(adapter->pdev, adapter);
+	dev_info(&adapter->pdev->dev, "%s: XGbE port initialized\n",
+		 adapter->netdev->name);
+	qlcnic_schedule_work(adapter, qlcnic_sriov_vf_poll_dev_state,
+			     adapter->ahw->idc.delay);
+	return 0;
+
+err_out_send_channel_term:
+	qlcnic_sriov_channel_cfg_cmd(adapter, QLCNIC_BC_CMD_CHANNEL_TERM);
+
+err_out_disable_bc_intr:
+	qlcnic_sriov_cfg_bc_intr(adapter, 0);
+
+err_out_cleanup_sriov:
+	__qlcnic_sriov_cleanup(adapter);
+
+err_out_disable_mbx_intr:
+	qlcnic_83xx_free_mbx_intr(adapter);
+
+err_out_disable_msi:
+	qlcnic_teardown_intr(adapter);
+	return err;
+}
+
+static int qlcnic_sriov_check_dev_ready(struct qlcnic_adapter *adapter)
+{
+	u32 state;
+
+	do {
+		msleep(20);
+		if (++adapter->fw_fail_cnt > QLC_BC_CMD_MAX_RETRY_CNT)
+			return -EIO;
+		state = QLCRDX(adapter->ahw, QLC_83XX_IDC_DEV_STATE);
+	} while (state != QLC_83XX_IDC_DEV_READY);
+
+	return 0;
+}
+
+int qlcnic_sriov_vf_init(struct qlcnic_adapter *adapter, int pci_using_dac)
+{
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
+	int err;
+
+	spin_lock_init(&ahw->mbx_lock);
+	set_bit(QLC_83XX_MBX_READY, &ahw->idc.status);
+	set_bit(QLC_83XX_MODULE_LOADED, &ahw->idc.status);
+	ahw->idc.delay = QLC_83XX_IDC_FW_POLL_DELAY;
+	ahw->reset_context = 0;
+	adapter->fw_fail_cnt = 0;
+	ahw->msix_supported = 1;
+	adapter->need_fw_reset = 0;
+	adapter->flags |= QLCNIC_TX_INTR_SHARED;
+
+	err = qlcnic_sriov_check_dev_ready(adapter);
+	if (err)
+		return err;
+
+	err = qlcnic_sriov_setup_vf(adapter, pci_using_dac);
+	if (err)
+		return err;
+
+	if (qlcnic_read_mac_addr(adapter))
+		dev_warn(&adapter->pdev->dev, "failed to read mac addr\n");
+
+	clear_bit(__QLCNIC_RESETTING, &adapter->state);
+	return 0;
+}
+
+void qlcnic_sriov_vf_set_ops(struct qlcnic_adapter *adapter)
+{
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
+
+	ahw->op_mode = QLCNIC_SRIOV_VF_FUNC;
+	dev_info(&adapter->pdev->dev,
+		 "HAL Version: %d Non Privileged SRIOV function\n",
+		 ahw->fw_hal_version);
+	adapter->nic_ops = &qlcnic_sriov_vf_ops;
+	set_bit(__QLCNIC_SRIOV_ENABLE, &adapter->state);
+	return;
+}
+
+void qlcnic_sriov_vf_register_map(struct qlcnic_hardware_context *ahw)
+{
+	ahw->hw_ops		= &qlcnic_sriov_vf_hw_ops;
+	ahw->reg_tbl		= (u32 *)qlcnic_83xx_reg_tbl;
+	ahw->ext_reg_tbl	= (u32 *)qlcnic_83xx_ext_reg_tbl;
+}
+
+static u32 qlcnic_sriov_get_bc_paysize(u32 real_pay_size, u8 curr_frag)
+{
+	u32 pay_size;
+
+	pay_size = real_pay_size / ((curr_frag + 1) * QLC_BC_PAYLOAD_SZ);
+
+	if (pay_size)
+		pay_size = QLC_BC_PAYLOAD_SZ;
+	else
+		pay_size = real_pay_size % QLC_BC_PAYLOAD_SZ;
+
+	return pay_size;
+}
+
+int qlcnic_sriov_func_to_index(struct qlcnic_adapter *adapter, u8 pci_func)
+{
+	struct qlcnic_vf_info *vf_info = adapter->ahw->sriov->vf_info;
+	u8 i;
+
+	if (qlcnic_sriov_vf_check(adapter))
+		return 0;
+
+	for (i = 0; i < adapter->ahw->sriov->num_vfs; i++) {
+		if (vf_info[i].pci_func == pci_func)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static inline int qlcnic_sriov_alloc_bc_trans(struct qlcnic_bc_trans **trans)
+{
+	*trans = kzalloc(sizeof(struct qlcnic_bc_trans), GFP_ATOMIC);
+	if (!*trans)
+		return -ENOMEM;
+
+	init_completion(&(*trans)->resp_cmpl);
+	return 0;
+}
+
+static inline int qlcnic_sriov_alloc_bc_msg(struct qlcnic_bc_hdr **hdr,
+					    u32 size)
+{
+	*hdr = kzalloc(sizeof(struct qlcnic_bc_hdr) * size, GFP_ATOMIC);
+	if (!*hdr)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static int qlcnic_sriov_alloc_bc_mbx_args(struct qlcnic_cmd_args *mbx, u32 type)
+{
+	const struct qlcnic_mailbox_metadata *mbx_tbl;
+	int i, size;
+
+	mbx_tbl = qlcnic_sriov_bc_mbx_tbl;
+	size = ARRAY_SIZE(qlcnic_sriov_bc_mbx_tbl);
+
+	for (i = 0; i < size; i++) {
+		if (type == mbx_tbl[i].cmd) {
+			mbx->op_type = QLC_BC_CMD;
+			mbx->req.num = mbx_tbl[i].in_args;
+			mbx->rsp.num = mbx_tbl[i].out_args;
+			mbx->req.arg = kcalloc(mbx->req.num, sizeof(u32),
+					       GFP_ATOMIC);
+			if (!mbx->req.arg)
+				return -ENOMEM;
+			mbx->rsp.arg = kcalloc(mbx->rsp.num, sizeof(u32),
+					       GFP_ATOMIC);
+			if (!mbx->rsp.arg) {
+				kfree(mbx->req.arg);
+				mbx->req.arg = NULL;
+				return -ENOMEM;
+			}
+			memset(mbx->req.arg, 0, sizeof(u32) * mbx->req.num);
+			memset(mbx->rsp.arg, 0, sizeof(u32) * mbx->rsp.num);
+			mbx->req.arg[0] = (type | (mbx->req.num << 16) |
+					   (3 << 29));
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+static int qlcnic_sriov_prepare_bc_hdr(struct qlcnic_bc_trans *trans,
+				       struct qlcnic_cmd_args *cmd,
+				       u16 seq, u8 msg_type)
+{
+	struct qlcnic_bc_hdr *hdr;
+	int i;
+	u32 num_regs, bc_pay_sz;
+	u16 remainder;
+	u8 cmd_op, num_frags, t_num_frags;
+
+	bc_pay_sz = QLC_BC_PAYLOAD_SZ;
+	if (msg_type == QLC_BC_COMMAND) {
+		trans->req_pay = (struct qlcnic_bc_payload *)cmd->req.arg;
+		trans->rsp_pay = (struct qlcnic_bc_payload *)cmd->rsp.arg;
+		num_regs = cmd->req.num;
+		trans->req_pay_size = (num_regs * 4);
+		num_regs = cmd->rsp.num;
+		trans->rsp_pay_size = (num_regs * 4);
+		cmd_op = cmd->req.arg[0] & 0xff;
+		remainder = (trans->req_pay_size) % (bc_pay_sz);
+		num_frags = (trans->req_pay_size) / (bc_pay_sz);
+		if (remainder)
+			num_frags++;
+		t_num_frags = num_frags;
+		if (qlcnic_sriov_alloc_bc_msg(&trans->req_hdr, num_frags))
+			return -ENOMEM;
+		remainder = (trans->rsp_pay_size) % (bc_pay_sz);
+		num_frags = (trans->rsp_pay_size) / (bc_pay_sz);
+		if (remainder)
+			num_frags++;
+		if (qlcnic_sriov_alloc_bc_msg(&trans->rsp_hdr, num_frags))
+			return -ENOMEM;
+		num_frags  = t_num_frags;
+		hdr = trans->req_hdr;
+	}  else {
+		cmd->req.arg = (u32 *)trans->req_pay;
+		cmd->rsp.arg = (u32 *)trans->rsp_pay;
+		cmd_op = cmd->req.arg[0] & 0xff;
+		remainder = (trans->rsp_pay_size) % (bc_pay_sz);
+		num_frags = (trans->rsp_pay_size) / (bc_pay_sz);
+		if (remainder)
+			num_frags++;
+		cmd->req.num = trans->req_pay_size / 4;
+		cmd->rsp.num = trans->rsp_pay_size / 4;
+		hdr = trans->rsp_hdr;
+	}
+
+	trans->trans_id = seq;
+	trans->cmd_id = cmd_op;
+	for (i = 0; i < num_frags; i++) {
+		hdr[i].version = 2;
+		hdr[i].msg_type = msg_type;
+		hdr[i].op_type = cmd->op_type;
+		hdr[i].num_cmds = 1;
+		hdr[i].num_frags = num_frags;
+		hdr[i].frag_num = i + 1;
+		hdr[i].cmd_op = cmd_op;
+		hdr[i].seq_id = seq;
+	}
+	return 0;
+}
+
+static void qlcnic_sriov_cleanup_transaction(struct qlcnic_bc_trans *trans)
+{
+	if (!trans)
+		return;
+	kfree(trans->req_hdr);
+	kfree(trans->rsp_hdr);
+	kfree(trans);
+}
+
+static int qlcnic_sriov_clear_trans(struct qlcnic_vf_info *vf,
+				    struct qlcnic_bc_trans *trans, u8 type)
+{
+	struct qlcnic_trans_list *t_list;
+	unsigned long flags;
+	int ret = 0;
+
+	if (type == QLC_BC_RESPONSE) {
+		t_list = &vf->rcv_act;
+		spin_lock_irqsave(&t_list->lock, flags);
+		t_list->count--;
+		list_del(&trans->list);
+		if (t_list->count > 0)
+			ret = 1;
+		spin_unlock_irqrestore(&t_list->lock, flags);
+	}
+	if (type == QLC_BC_COMMAND) {
+		while (test_and_set_bit(QLC_BC_VF_SEND, &vf->state))
+			msleep(100);
+		vf->send_cmd = NULL;
+		clear_bit(QLC_BC_VF_SEND, &vf->state);
+	}
+	return ret;
+}
+
+static void qlcnic_sriov_schedule_bc_cmd(struct qlcnic_sriov *sriov,
+					 struct qlcnic_vf_info *vf,
+					 work_func_t func)
+{
+	if (test_bit(QLC_BC_VF_FLR, &vf->state) ||
+	    vf->adapter->need_fw_reset)
+		return;
+
+	INIT_WORK(&vf->trans_work, func);
+	queue_work(sriov->bc.bc_trans_wq, &vf->trans_work);
+}
+
+static inline void qlcnic_sriov_wait_for_resp(struct qlcnic_bc_trans *trans)
+{
+	struct completion *cmpl = &trans->resp_cmpl;
+
+	if (wait_for_completion_timeout(cmpl, QLC_MBOX_RESP_TIMEOUT))
+		trans->trans_state = QLC_END;
+	else
+		trans->trans_state = QLC_ABORT;
+
+	return;
+}
+
+static void qlcnic_sriov_handle_multi_frags(struct qlcnic_bc_trans *trans,
+					    u8 type)
+{
+	if (type == QLC_BC_RESPONSE) {
+		trans->curr_rsp_frag++;
+		if (trans->curr_rsp_frag < trans->rsp_hdr->num_frags)
+			trans->trans_state = QLC_INIT;
+		else
+			trans->trans_state = QLC_END;
+	} else {
+		trans->curr_req_frag++;
+		if (trans->curr_req_frag < trans->req_hdr->num_frags)
+			trans->trans_state = QLC_INIT;
+		else
+			trans->trans_state = QLC_WAIT_FOR_RESP;
+	}
+}
+
+static void qlcnic_sriov_wait_for_channel_free(struct qlcnic_bc_trans *trans,
+					       u8 type)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	struct completion *cmpl = &vf->ch_free_cmpl;
+
+	if (!wait_for_completion_timeout(cmpl, QLC_MBOX_CH_FREE_TIMEOUT)) {
+		trans->trans_state = QLC_ABORT;
+		return;
+	}
+
+	clear_bit(QLC_BC_VF_CHANNEL, &vf->state);
+	qlcnic_sriov_handle_multi_frags(trans, type);
+}
+
+static void qlcnic_sriov_pull_bc_msg(struct qlcnic_adapter *adapter,
+				     u32 *hdr, u32 *pay, u32 size)
+{
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
+	u32 fw_mbx;
+	u8 i, max = 2, hdr_size, j;
+
+	hdr_size = (sizeof(struct qlcnic_bc_hdr) / sizeof(u32));
+	max = (size / sizeof(u32)) + hdr_size;
+
+	fw_mbx = readl(QLCNIC_MBX_FW(ahw, 0));
+	for (i = 2, j = 0; j < hdr_size; i++, j++)
+		*(hdr++) = readl(QLCNIC_MBX_FW(ahw, i));
+	for (; j < max; i++, j++)
+		*(pay++) = readl(QLCNIC_MBX_FW(ahw, i));
+}
+
+static int __qlcnic_sriov_issue_bc_post(struct qlcnic_vf_info *vf)
+{
+	int ret = -EBUSY;
+	u32 timeout = 10000;
+
+	do {
+		if (!test_and_set_bit(QLC_BC_VF_CHANNEL, &vf->state)) {
+			ret = 0;
+			break;
+		}
+		mdelay(1);
+	} while (--timeout);
+
+	return ret;
+}
+
+static int qlcnic_sriov_issue_bc_post(struct qlcnic_bc_trans *trans, u8 type)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	u32 pay_size, hdr_size;
+	u32 *hdr, *pay;
+	int ret;
+	u8 pci_func = trans->func_id;
+
+	if (__qlcnic_sriov_issue_bc_post(vf))
+		return -EBUSY;
+
+	if (type == QLC_BC_COMMAND) {
+		hdr = (u32 *)(trans->req_hdr + trans->curr_req_frag);
+		pay = (u32 *)(trans->req_pay + trans->curr_req_frag);
+		hdr_size = (sizeof(struct qlcnic_bc_hdr) / sizeof(u32));
+		pay_size = qlcnic_sriov_get_bc_paysize(trans->req_pay_size,
+						       trans->curr_req_frag);
+		pay_size = (pay_size / sizeof(u32));
+	} else {
+		hdr = (u32 *)(trans->rsp_hdr + trans->curr_rsp_frag);
+		pay = (u32 *)(trans->rsp_pay + trans->curr_rsp_frag);
+		hdr_size = (sizeof(struct qlcnic_bc_hdr) / sizeof(u32));
+		pay_size = qlcnic_sriov_get_bc_paysize(trans->rsp_pay_size,
+						       trans->curr_rsp_frag);
+		pay_size = (pay_size / sizeof(u32));
+	}
+
+	ret = qlcnic_sriov_post_bc_msg(vf->adapter, hdr, pay,
+				       pci_func, pay_size);
+	return ret;
+}
+
+static int __qlcnic_sriov_send_bc_msg(struct qlcnic_bc_trans *trans,
+				      struct qlcnic_vf_info *vf, u8 type)
+{
+	bool flag = true;
+	int err = -EIO;
+
+	while (flag) {
+		if (test_bit(QLC_BC_VF_FLR, &vf->state) ||
+		    vf->adapter->need_fw_reset)
+			trans->trans_state = QLC_ABORT;
+
+		switch (trans->trans_state) {
+		case QLC_INIT:
+			trans->trans_state = QLC_WAIT_FOR_CHANNEL_FREE;
+			if (qlcnic_sriov_issue_bc_post(trans, type))
+				trans->trans_state = QLC_ABORT;
+			break;
+		case QLC_WAIT_FOR_CHANNEL_FREE:
+			qlcnic_sriov_wait_for_channel_free(trans, type);
+			break;
+		case QLC_WAIT_FOR_RESP:
+			qlcnic_sriov_wait_for_resp(trans);
+			break;
+		case QLC_END:
+			err = 0;
+			flag = false;
+			break;
+		case QLC_ABORT:
+			err = -EIO;
+			flag = false;
+			clear_bit(QLC_BC_VF_CHANNEL, &vf->state);
+			break;
+		default:
+			err = -EIO;
+			flag = false;
+		}
+	}
+	return err;
+}
+
+static int qlcnic_sriov_send_bc_cmd(struct qlcnic_adapter *adapter,
+				    struct qlcnic_bc_trans *trans, int pci_func)
+{
+	struct qlcnic_vf_info *vf;
+	int err, index = qlcnic_sriov_func_to_index(adapter, pci_func);
+
+	if (index < 0)
+		return -EIO;
+
+	vf = &adapter->ahw->sriov->vf_info[index];
+	trans->vf = vf;
+	trans->func_id = pci_func;
+
+	if (!test_bit(QLC_BC_VF_STATE, &vf->state)) {
+		if (qlcnic_sriov_pf_check(adapter))
+			return -EIO;
+		if (qlcnic_sriov_vf_check(adapter) &&
+		    trans->cmd_id != QLCNIC_BC_CMD_CHANNEL_INIT)
+			return -EIO;
+	}
+
+	mutex_lock(&vf->send_cmd_lock);
+	vf->send_cmd = trans;
+	err = __qlcnic_sriov_send_bc_msg(trans, vf, QLC_BC_COMMAND);
+	qlcnic_sriov_clear_trans(vf, trans, QLC_BC_COMMAND);
+	mutex_unlock(&vf->send_cmd_lock);
+	return err;
+}
+
+static void __qlcnic_sriov_process_bc_cmd(struct qlcnic_adapter *adapter,
+					  struct qlcnic_bc_trans *trans,
+					  struct qlcnic_cmd_args *cmd)
+{
+#ifdef CONFIG_QLCNIC_SRIOV
+	if (qlcnic_sriov_pf_check(adapter)) {
+		qlcnic_sriov_pf_process_bc_cmd(adapter, trans, cmd);
+		return;
+	}
+#endif
+	cmd->rsp.arg[0] |= (0x9 << 25);
+	return;
+}
+
+static void qlcnic_sriov_process_bc_cmd(struct work_struct *work)
+{
+	struct qlcnic_vf_info *vf = container_of(work, struct qlcnic_vf_info,
+						 trans_work);
+	struct qlcnic_bc_trans *trans = NULL;
+	struct qlcnic_adapter *adapter  = vf->adapter;
+	struct qlcnic_cmd_args cmd;
+	u8 req;
+
+	if (adapter->need_fw_reset)
+		return;
+
+	if (test_bit(QLC_BC_VF_FLR, &vf->state))
+		return;
+
+	trans = list_first_entry(&vf->rcv_act.wait_list,
+				 struct qlcnic_bc_trans, list);
+	adapter = vf->adapter;
+
+	if (qlcnic_sriov_prepare_bc_hdr(trans, &cmd, trans->req_hdr->seq_id,
+					QLC_BC_RESPONSE))
+		goto cleanup_trans;
+
+	__qlcnic_sriov_process_bc_cmd(adapter, trans, &cmd);
+	trans->trans_state = QLC_INIT;
+	__qlcnic_sriov_send_bc_msg(trans, vf, QLC_BC_RESPONSE);
+
+cleanup_trans:
+	qlcnic_free_mbx_args(&cmd);
+	req = qlcnic_sriov_clear_trans(vf, trans, QLC_BC_RESPONSE);
+	qlcnic_sriov_cleanup_transaction(trans);
+	if (req)
+		qlcnic_sriov_schedule_bc_cmd(adapter->ahw->sriov, vf,
+					     qlcnic_sriov_process_bc_cmd);
+}
+
+static void qlcnic_sriov_handle_bc_resp(struct qlcnic_bc_hdr *hdr,
+					struct qlcnic_vf_info *vf)
+{
+	struct qlcnic_bc_trans *trans;
+	u32 pay_size;
+
+	if (test_and_set_bit(QLC_BC_VF_SEND, &vf->state))
+		return;
+
+	trans = vf->send_cmd;
+
+	if (trans == NULL)
+		goto clear_send;
+
+	if (trans->trans_id != hdr->seq_id)
+		goto clear_send;
+
+	pay_size = qlcnic_sriov_get_bc_paysize(trans->rsp_pay_size,
+					       trans->curr_rsp_frag);
+	qlcnic_sriov_pull_bc_msg(vf->adapter,
+				 (u32 *)(trans->rsp_hdr + trans->curr_rsp_frag),
+				 (u32 *)(trans->rsp_pay + trans->curr_rsp_frag),
+				 pay_size);
+	if (++trans->curr_rsp_frag < trans->rsp_hdr->num_frags)
+		goto clear_send;
+
+	complete(&trans->resp_cmpl);
+
+clear_send:
+	clear_bit(QLC_BC_VF_SEND, &vf->state);
+}
+
+int __qlcnic_sriov_add_act_list(struct qlcnic_sriov *sriov,
+				struct qlcnic_vf_info *vf,
+				struct qlcnic_bc_trans *trans)
+{
+	struct qlcnic_trans_list *t_list = &vf->rcv_act;
+
+	t_list->count++;
+	list_add_tail(&trans->list, &t_list->wait_list);
+	if (t_list->count == 1)
+		qlcnic_sriov_schedule_bc_cmd(sriov, vf,
+					     qlcnic_sriov_process_bc_cmd);
+	return 0;
+}
+
+static int qlcnic_sriov_add_act_list(struct qlcnic_sriov *sriov,
+				     struct qlcnic_vf_info *vf,
+				     struct qlcnic_bc_trans *trans)
+{
+	struct qlcnic_trans_list *t_list = &vf->rcv_act;
+
+	spin_lock(&t_list->lock);
+
+	__qlcnic_sriov_add_act_list(sriov, vf, trans);
+
+	spin_unlock(&t_list->lock);
+	return 0;
+}
+
+static void qlcnic_sriov_handle_pending_trans(struct qlcnic_sriov *sriov,
+					      struct qlcnic_vf_info *vf,
+					      struct qlcnic_bc_hdr *hdr)
+{
+	struct qlcnic_bc_trans *trans = NULL;
+	struct list_head *node;
+	u32 pay_size, curr_frag;
+	u8 found = 0, active = 0;
+
+	spin_lock(&vf->rcv_pend.lock);
+	if (vf->rcv_pend.count > 0) {
+		list_for_each(node, &vf->rcv_pend.wait_list) {
+			trans = list_entry(node, struct qlcnic_bc_trans, list);
+			if (trans->trans_id == hdr->seq_id) {
+				found = 1;
+				break;
+			}
+		}
+	}
+
+	if (found) {
+		curr_frag = trans->curr_req_frag;
+		pay_size = qlcnic_sriov_get_bc_paysize(trans->req_pay_size,
+						       curr_frag);
+		qlcnic_sriov_pull_bc_msg(vf->adapter,
+					 (u32 *)(trans->req_hdr + curr_frag),
+					 (u32 *)(trans->req_pay + curr_frag),
+					 pay_size);
+		trans->curr_req_frag++;
+		if (trans->curr_req_frag >= hdr->num_frags) {
+			vf->rcv_pend.count--;
+			list_del(&trans->list);
+			active = 1;
+		}
+	}
+	spin_unlock(&vf->rcv_pend.lock);
+
+	if (active)
+		if (qlcnic_sriov_add_act_list(sriov, vf, trans))
+			qlcnic_sriov_cleanup_transaction(trans);
+
+	return;
+}
+
+static void qlcnic_sriov_handle_bc_cmd(struct qlcnic_sriov *sriov,
+				       struct qlcnic_bc_hdr *hdr,
+				       struct qlcnic_vf_info *vf)
+{
+	struct qlcnic_bc_trans *trans;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	struct qlcnic_cmd_args cmd;
+	u32 pay_size;
+	int err;
+	u8 cmd_op;
+
+	if (adapter->need_fw_reset)
+		return;
+
+	if (!test_bit(QLC_BC_VF_STATE, &vf->state) &&
+	    hdr->op_type != QLC_BC_CMD &&
+	    hdr->cmd_op != QLCNIC_BC_CMD_CHANNEL_INIT)
+		return;
+
+	if (hdr->frag_num > 1) {
+		qlcnic_sriov_handle_pending_trans(sriov, vf, hdr);
+		return;
+	}
+
+	cmd_op = hdr->cmd_op;
+	if (qlcnic_sriov_alloc_bc_trans(&trans))
+		return;
+
+	if (hdr->op_type == QLC_BC_CMD)
+		err = qlcnic_sriov_alloc_bc_mbx_args(&cmd, cmd_op);
+	else
+		err = qlcnic_alloc_mbx_args(&cmd, adapter, cmd_op);
+
+	if (err) {
+		qlcnic_sriov_cleanup_transaction(trans);
+		return;
+	}
+
+	cmd.op_type = hdr->op_type;
+	if (qlcnic_sriov_prepare_bc_hdr(trans, &cmd, hdr->seq_id,
+					QLC_BC_COMMAND)) {
+		qlcnic_free_mbx_args(&cmd);
+		qlcnic_sriov_cleanup_transaction(trans);
+		return;
+	}
+
+	pay_size = qlcnic_sriov_get_bc_paysize(trans->req_pay_size,
+					 trans->curr_req_frag);
+	qlcnic_sriov_pull_bc_msg(vf->adapter,
+				 (u32 *)(trans->req_hdr + trans->curr_req_frag),
+				 (u32 *)(trans->req_pay + trans->curr_req_frag),
+				 pay_size);
+	trans->func_id = vf->pci_func;
+	trans->vf = vf;
+	trans->trans_id = hdr->seq_id;
+	trans->curr_req_frag++;
+
+	if (qlcnic_sriov_soft_flr_check(adapter, trans, vf))
+		return;
+
+	if (trans->curr_req_frag == trans->req_hdr->num_frags) {
+		if (qlcnic_sriov_add_act_list(sriov, vf, trans)) {
+			qlcnic_free_mbx_args(&cmd);
+			qlcnic_sriov_cleanup_transaction(trans);
+		}
+	} else {
+		spin_lock(&vf->rcv_pend.lock);
+		list_add_tail(&trans->list, &vf->rcv_pend.wait_list);
+		vf->rcv_pend.count++;
+		spin_unlock(&vf->rcv_pend.lock);
+	}
+}
+
+static void qlcnic_sriov_handle_msg_event(struct qlcnic_sriov *sriov,
+					  struct qlcnic_vf_info *vf)
+{
+	struct qlcnic_bc_hdr hdr;
+	u32 *ptr = (u32 *)&hdr;
+	u8 msg_type, i;
+
+	for (i = 2; i < 6; i++)
+		ptr[i - 2] = readl(QLCNIC_MBX_FW(vf->adapter->ahw, i));
+	msg_type = hdr.msg_type;
+
+	switch (msg_type) {
+	case QLC_BC_COMMAND:
+		qlcnic_sriov_handle_bc_cmd(sriov, &hdr, vf);
+		break;
+	case QLC_BC_RESPONSE:
+		qlcnic_sriov_handle_bc_resp(&hdr, vf);
+		break;
+	}
+}
+
+static void qlcnic_sriov_handle_flr_event(struct qlcnic_sriov *sriov,
+					  struct qlcnic_vf_info *vf)
+{
+	struct qlcnic_adapter *adapter = vf->adapter;
+
+	if (qlcnic_sriov_pf_check(adapter))
+		qlcnic_sriov_pf_handle_flr(sriov, vf);
+	else
+		dev_err(&adapter->pdev->dev,
+			"Invalid event to VF. VF should not get FLR event\n");
+}
+
+void qlcnic_sriov_handle_bc_event(struct qlcnic_adapter *adapter, u32 event)
+{
+	struct qlcnic_vf_info *vf;
+	struct qlcnic_sriov *sriov;
+	int index;
+	u8 pci_func;
+
+	sriov = adapter->ahw->sriov;
+	pci_func = qlcnic_sriov_target_func_id(event);
+	index = qlcnic_sriov_func_to_index(adapter, pci_func);
+
+	if (index < 0)
+		return;
+
+	vf = &sriov->vf_info[index];
+	vf->pci_func = pci_func;
+
+	if (qlcnic_sriov_channel_free_check(event))
+		complete(&vf->ch_free_cmpl);
+
+	if (qlcnic_sriov_flr_check(event)) {
+		qlcnic_sriov_handle_flr_event(sriov, vf);
+		return;
+	}
+
+	if (qlcnic_sriov_bc_msg_check(event))
+		qlcnic_sriov_handle_msg_event(sriov, vf);
+}
+
+int qlcnic_sriov_cfg_bc_intr(struct qlcnic_adapter *adapter, u8 enable)
+{
+	struct qlcnic_cmd_args cmd;
+	int err;
+
+	if (!test_bit(__QLCNIC_SRIOV_ENABLE, &adapter->state))
+		return 0;
+
+	if (qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_BC_EVENT_SETUP))
+		return -ENOMEM;
+
+	if (enable)
+		cmd.req.arg[1] = (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7);
+
+	err = qlcnic_83xx_mbx_op(adapter, &cmd);
+
+	if (err != QLCNIC_RCODE_SUCCESS) {
+		dev_err(&adapter->pdev->dev,
+			"Failed to %s bc events, err=%d\n",
+			(enable ? "enable" : "disable"), err);
+	}
+
+	qlcnic_free_mbx_args(&cmd);
+	return err;
+}
+
+static int qlcnic_sriov_retry_bc_cmd(struct qlcnic_adapter *adapter,
+				     struct qlcnic_bc_trans *trans)
+{
+	u8 max = QLC_BC_CMD_MAX_RETRY_CNT;
+	u32 state;
+
+	state = QLCRDX(adapter->ahw, QLC_83XX_IDC_DEV_STATE);
+	if (state == QLC_83XX_IDC_DEV_READY) {
+		msleep(20);
+		clear_bit(QLC_BC_VF_CHANNEL, &trans->vf->state);
+		trans->trans_state = QLC_INIT;
+		if (++adapter->fw_fail_cnt > max)
+			return -EIO;
+		else
+			return 0;
+	}
+
+	return -EIO;
+}
+
+static int qlcnic_sriov_vf_mbx_op(struct qlcnic_adapter *adapter,
+				  struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
+	struct device *dev = &adapter->pdev->dev;
+	struct qlcnic_bc_trans *trans;
+	int err;
+	u32 rsp_data, opcode, mbx_err_code, rsp;
+	u16 seq = ++adapter->ahw->sriov->bc.trans_counter;
+	u8 func = ahw->pci_func;
+
+	rsp = qlcnic_sriov_alloc_bc_trans(&trans);
+	if (rsp)
+		return rsp;
+
+	rsp = qlcnic_sriov_prepare_bc_hdr(trans, cmd, seq, QLC_BC_COMMAND);
+	if (rsp)
+		goto cleanup_transaction;
+
+retry:
+	if (!test_bit(QLC_83XX_MBX_READY, &adapter->ahw->idc.status)) {
+		rsp = -EIO;
+		QLCDB(adapter, DRV, "MBX not Ready!(cmd 0x%x) for VF 0x%x\n",
+		      QLCNIC_MBX_RSP(cmd->req.arg[0]), func);
+		goto err_out;
+	}
+
+	err = qlcnic_sriov_send_bc_cmd(adapter, trans, func);
+	if (err) {
+		dev_err(dev, "MBX command 0x%x timed out for VF %d\n",
+			(cmd->req.arg[0] & 0xffff), func);
+		rsp = QLCNIC_RCODE_TIMEOUT;
+
+		/* After adapter reset PF driver may take some time to
+		 * respond to VF's request. Retry request till maximum retries.
+		 */
+		if ((trans->req_hdr->cmd_op == QLCNIC_BC_CMD_CHANNEL_INIT) &&
+		    !qlcnic_sriov_retry_bc_cmd(adapter, trans))
+			goto retry;
+
+		goto err_out;
+	}
+
+	rsp_data = cmd->rsp.arg[0];
+	mbx_err_code = QLCNIC_MBX_STATUS(rsp_data);
+	opcode = QLCNIC_MBX_RSP(cmd->req.arg[0]);
+
+	if ((mbx_err_code == QLCNIC_MBX_RSP_OK) ||
+	    (mbx_err_code == QLCNIC_MBX_PORT_RSP_OK)) {
+		rsp = QLCNIC_RCODE_SUCCESS;
+	} else {
+		rsp = mbx_err_code;
+		if (!rsp)
+			rsp = 1;
+		dev_err(dev,
+			"MBX command 0x%x failed with err:0x%x for VF %d\n",
+			opcode, mbx_err_code, func);
+	}
+
+err_out:
+	if (rsp == QLCNIC_RCODE_TIMEOUT) {
+		ahw->reset_context = 1;
+		adapter->need_fw_reset = 1;
+		clear_bit(QLC_83XX_MBX_READY, &ahw->idc.status);
+	}
+
+cleanup_transaction:
+	qlcnic_sriov_cleanup_transaction(trans);
+	return rsp;
+}
+
+int qlcnic_sriov_channel_cfg_cmd(struct qlcnic_adapter *adapter, u8 cmd_op)
+{
+	struct qlcnic_cmd_args cmd;
+	struct qlcnic_vf_info *vf = &adapter->ahw->sriov->vf_info[0];
+	int ret;
+
+	if (qlcnic_sriov_alloc_bc_mbx_args(&cmd, cmd_op))
+		return -ENOMEM;
+
+	ret = qlcnic_issue_cmd(adapter, &cmd);
+	if (ret) {
+		dev_err(&adapter->pdev->dev,
+			"Failed bc channel %s %d\n", cmd_op ? "term" : "init",
+			ret);
+		goto out;
+	}
+
+	cmd_op = (cmd.rsp.arg[0] & 0xff);
+	if (cmd.rsp.arg[0] >> 25 == 2)
+		return 2;
+	if (cmd_op == QLCNIC_BC_CMD_CHANNEL_INIT)
+		set_bit(QLC_BC_VF_STATE, &vf->state);
+	else
+		clear_bit(QLC_BC_VF_STATE, &vf->state);
+
+out:
+	qlcnic_free_mbx_args(&cmd);
+	return ret;
+}
+
+void qlcnic_vf_add_mc_list(struct net_device *netdev, u16 vlan)
+{
+	struct qlcnic_adapter *adapter = netdev_priv(netdev);
+	struct qlcnic_mac_list_s *cur;
+	struct list_head *head, tmp_list;
+
+	INIT_LIST_HEAD(&tmp_list);
+	head = &adapter->vf_mc_list;
+	netif_addr_lock_bh(netdev);
+
+	while (!list_empty(head)) {
+		cur = list_entry(head->next, struct qlcnic_mac_list_s, list);
+		list_move(&cur->list, &tmp_list);
+	}
+
+	netif_addr_unlock_bh(netdev);
+
+	while (!list_empty(&tmp_list)) {
+		cur = list_entry((&tmp_list)->next,
+				 struct qlcnic_mac_list_s, list);
+		qlcnic_nic_add_mac(adapter, cur->mac_addr, vlan);
+		list_del(&cur->list);
+		kfree(cur);
+	}
+}
+
+void qlcnic_sriov_cleanup_async_list(struct qlcnic_back_channel *bc)
+{
+	struct list_head *head = &bc->async_list;
+	struct qlcnic_async_work_list *entry;
+
+	while (!list_empty(head)) {
+		entry = list_entry(head->next, struct qlcnic_async_work_list,
+				   list);
+		cancel_work_sync(&entry->work);
+		list_del(&entry->list);
+		kfree(entry);
+	}
+}
+
+static void qlcnic_sriov_vf_set_multi(struct net_device *netdev)
+{
+	struct qlcnic_adapter *adapter = netdev_priv(netdev);
+	u16 vlan;
+
+	if (!test_bit(__QLCNIC_FW_ATTACHED, &adapter->state))
+		return;
+
+	vlan = adapter->ahw->sriov->vlan;
+	__qlcnic_set_multi(netdev, vlan);
+}
+
+static void qlcnic_sriov_handle_async_multi(struct work_struct *work)
+{
+	struct qlcnic_async_work_list *entry;
+	struct net_device *netdev;
+
+	entry = container_of(work, struct qlcnic_async_work_list, work);
+	netdev = (struct net_device *)entry->ptr;
+
+	qlcnic_sriov_vf_set_multi(netdev);
+	return;
+}
+
+static struct qlcnic_async_work_list *
+qlcnic_sriov_get_free_node_async_work(struct qlcnic_back_channel *bc)
+{
+	struct list_head *node;
+	struct qlcnic_async_work_list *entry = NULL;
+	u8 empty = 0;
+
+	list_for_each(node, &bc->async_list) {
+		entry = list_entry(node, struct qlcnic_async_work_list, list);
+		if (!work_pending(&entry->work)) {
+			empty = 1;
+			break;
+		}
+	}
+
+	if (!empty) {
+		entry = kzalloc(sizeof(struct qlcnic_async_work_list),
+				GFP_ATOMIC);
+		if (entry == NULL)
+			return NULL;
+		list_add_tail(&entry->list, &bc->async_list);
+	}
+
+	return entry;
+}
+
+static void qlcnic_sriov_schedule_bc_async_work(struct qlcnic_back_channel *bc,
+						work_func_t func, void *data)
+{
+	struct qlcnic_async_work_list *entry = NULL;
+
+	entry = qlcnic_sriov_get_free_node_async_work(bc);
+	if (!entry)
+		return;
+
+	entry->ptr = data;
+	INIT_WORK(&entry->work, func);
+	queue_work(bc->bc_async_wq, &entry->work);
+}
+
+void qlcnic_sriov_vf_schedule_multi(struct net_device *netdev)
+{
+
+	struct qlcnic_adapter *adapter = netdev_priv(netdev);
+	struct qlcnic_back_channel *bc = &adapter->ahw->sriov->bc;
+
+	if (adapter->need_fw_reset)
+		return;
+
+	qlcnic_sriov_schedule_bc_async_work(bc, qlcnic_sriov_handle_async_multi,
+					    netdev);
+}
+
+static int qlcnic_sriov_vf_reinit_driver(struct qlcnic_adapter *adapter)
+{
+	int err;
+
+	set_bit(QLC_83XX_MBX_READY, &adapter->ahw->idc.status);
+	qlcnic_83xx_enable_mbx_intrpt(adapter);
+
+	err = qlcnic_sriov_cfg_bc_intr(adapter, 1);
+	if (err)
+		return err;
+
+	err = qlcnic_sriov_channel_cfg_cmd(adapter, QLCNIC_BC_CMD_CHANNEL_INIT);
+	if (err)
+		goto err_out_cleanup_bc_intr;
+
+	err = qlcnic_sriov_vf_init_driver(adapter);
+	if (err)
+		goto err_out_term_channel;
+
+	return 0;
+
+err_out_term_channel:
+	qlcnic_sriov_channel_cfg_cmd(adapter, QLCNIC_BC_CMD_CHANNEL_TERM);
+
+err_out_cleanup_bc_intr:
+	qlcnic_sriov_cfg_bc_intr(adapter, 0);
+	return err;
+}
+
+static void qlcnic_sriov_vf_attach(struct qlcnic_adapter *adapter)
+{
+	struct net_device *netdev = adapter->netdev;
+
+	if (netif_running(netdev)) {
+		if (!qlcnic_up(adapter, netdev))
+			qlcnic_restore_indev_addr(netdev, NETDEV_UP);
+	}
+
+	netif_device_attach(netdev);
+}
+
+static void qlcnic_sriov_vf_detach(struct qlcnic_adapter *adapter)
+{
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
+	struct qlcnic_intrpt_config *intr_tbl = ahw->intr_tbl;
+	struct net_device *netdev = adapter->netdev;
+	u8 i, max_ints = ahw->num_msix - 1;
+
+	qlcnic_83xx_disable_mbx_intr(adapter);
+	netif_device_detach(netdev);
+	if (netif_running(netdev))
+		qlcnic_down(adapter, netdev);
+
+	for (i = 0; i < max_ints; i++) {
+		intr_tbl[i].id = i;
+		intr_tbl[i].enabled = 0;
+		intr_tbl[i].src = 0;
+	}
+	ahw->reset_context = 0;
+}
+
+static int qlcnic_sriov_vf_handle_dev_ready(struct qlcnic_adapter *adapter)
+{
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
+	struct device *dev = &adapter->pdev->dev;
+	struct qlc_83xx_idc *idc = &ahw->idc;
+	u8 func = ahw->pci_func;
+	u32 state;
+
+	if ((idc->prev_state == QLC_83XX_IDC_DEV_NEED_RESET) ||
+	    (idc->prev_state == QLC_83XX_IDC_DEV_INIT)) {
+		if (!qlcnic_sriov_vf_reinit_driver(adapter)) {
+			qlcnic_sriov_vf_attach(adapter);
+			adapter->fw_fail_cnt = 0;
+			dev_info(dev,
+				 "%s: Reinitalization of VF 0x%x done after FW reset\n",
+				 __func__, func);
+		} else {
+			dev_err(dev,
+				"%s: Reinitialization of VF 0x%x failed after FW reset\n",
+				__func__, func);
+			state = QLCRDX(ahw, QLC_83XX_IDC_DEV_STATE);
+			dev_info(dev, "Current state 0x%x after FW reset\n",
+				 state);
+		}
+	}
+
+	return 0;
+}
+
+static int qlcnic_sriov_vf_handle_context_reset(struct qlcnic_adapter *adapter)
+{
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
+	struct device *dev = &adapter->pdev->dev;
+	struct qlc_83xx_idc *idc = &ahw->idc;
+	u8 func = ahw->pci_func;
+	u32 state;
+
+	adapter->reset_ctx_cnt++;
+
+	/* Skip the context reset and check if FW is hung */
+	if (adapter->reset_ctx_cnt < 3) {
+		adapter->need_fw_reset = 1;
+		clear_bit(QLC_83XX_MBX_READY, &idc->status);
+		dev_info(dev,
+			 "Resetting context, wait here to check if FW is in failed state\n");
+		return 0;
+	}
+
+	/* Check if number of resets exceed the threshold.
+	 * If it exceeds the threshold just fail the VF.
+	 */
+	if (adapter->reset_ctx_cnt > QLC_83XX_VF_RESET_FAIL_THRESH) {
+		clear_bit(QLC_83XX_MODULE_LOADED, &idc->status);
+		adapter->tx_timeo_cnt = 0;
+		adapter->fw_fail_cnt = 0;
+		adapter->reset_ctx_cnt = 0;
+		qlcnic_sriov_vf_detach(adapter);
+		dev_err(dev,
+			"Device context resets have exceeded the threshold, device interface will be shutdown\n");
+		return -EIO;
+	}
+
+	dev_info(dev, "Resetting context of VF 0x%x\n", func);
+	dev_info(dev, "%s: Context reset count %d for VF 0x%x\n",
+		 __func__, adapter->reset_ctx_cnt, func);
+	set_bit(__QLCNIC_RESETTING, &adapter->state);
+	adapter->need_fw_reset = 1;
+	clear_bit(QLC_83XX_MBX_READY, &idc->status);
+	qlcnic_sriov_vf_detach(adapter);
+	adapter->need_fw_reset = 0;
+
+	if (!qlcnic_sriov_vf_reinit_driver(adapter)) {
+		qlcnic_sriov_vf_attach(adapter);
+		adapter->netdev->trans_start = jiffies;
+		adapter->tx_timeo_cnt = 0;
+		adapter->reset_ctx_cnt = 0;
+		adapter->fw_fail_cnt = 0;
+		dev_info(dev, "Done resetting context for VF 0x%x\n", func);
+	} else {
+		dev_err(dev, "%s: Reinitialization of VF 0x%x failed\n",
+			__func__, func);
+		state = QLCRDX(ahw, QLC_83XX_IDC_DEV_STATE);
+		dev_info(dev, "%s: Current state 0x%x\n", __func__, state);
+	}
+
+	return 0;
+}
+
+static int qlcnic_sriov_vf_idc_ready_state(struct qlcnic_adapter *adapter)
+{
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
+	int ret = 0;
+
+	if (ahw->idc.prev_state != QLC_83XX_IDC_DEV_READY)
+		ret = qlcnic_sriov_vf_handle_dev_ready(adapter);
+	else if (ahw->reset_context)
+		ret = qlcnic_sriov_vf_handle_context_reset(adapter);
+
+	clear_bit(__QLCNIC_RESETTING, &adapter->state);
+	return ret;
+}
+
+static int qlcnic_sriov_vf_idc_failed_state(struct qlcnic_adapter *adapter)
+{
+	struct qlc_83xx_idc *idc = &adapter->ahw->idc;
+
+	dev_err(&adapter->pdev->dev, "Device is in failed state\n");
+	if (idc->prev_state == QLC_83XX_IDC_DEV_READY)
+		qlcnic_sriov_vf_detach(adapter);
+
+	clear_bit(QLC_83XX_MODULE_LOADED, &idc->status);
+	clear_bit(__QLCNIC_RESETTING, &adapter->state);
+	return -EIO;
+}
+
+static int
+qlcnic_sriov_vf_idc_need_quiescent_state(struct qlcnic_adapter *adapter)
+{
+	struct qlc_83xx_idc *idc = &adapter->ahw->idc;
+
+	dev_info(&adapter->pdev->dev, "Device is in quiescent state\n");
+	if (idc->prev_state == QLC_83XX_IDC_DEV_READY) {
+		set_bit(__QLCNIC_RESETTING, &adapter->state);
+		adapter->tx_timeo_cnt = 0;
+		adapter->reset_ctx_cnt = 0;
+		clear_bit(QLC_83XX_MBX_READY, &idc->status);
+		qlcnic_sriov_vf_detach(adapter);
+	}
+
+	return 0;
+}
+
+static int qlcnic_sriov_vf_idc_init_reset_state(struct qlcnic_adapter *adapter)
+{
+	struct qlc_83xx_idc *idc = &adapter->ahw->idc;
+	u8 func = adapter->ahw->pci_func;
+
+	if (idc->prev_state == QLC_83XX_IDC_DEV_READY) {
+		dev_err(&adapter->pdev->dev,
+			"Firmware hang detected by VF 0x%x\n", func);
+		set_bit(__QLCNIC_RESETTING, &adapter->state);
+		adapter->tx_timeo_cnt = 0;
+		adapter->reset_ctx_cnt = 0;
+		clear_bit(QLC_83XX_MBX_READY, &idc->status);
+		qlcnic_sriov_vf_detach(adapter);
+	}
+	return 0;
+}
+
+static int qlcnic_sriov_vf_idc_unknown_state(struct qlcnic_adapter *adapter)
+{
+	dev_err(&adapter->pdev->dev, "%s: Device in unknown state\n", __func__);
+	return 0;
+}
+
+static void qlcnic_sriov_vf_poll_dev_state(struct work_struct *work)
+{
+	struct qlcnic_adapter *adapter;
+	struct qlc_83xx_idc *idc;
+	int ret = 0;
+
+	adapter = container_of(work, struct qlcnic_adapter, fw_work.work);
+	idc = &adapter->ahw->idc;
+	idc->curr_state = QLCRDX(adapter->ahw, QLC_83XX_IDC_DEV_STATE);
+
+	switch (idc->curr_state) {
+	case QLC_83XX_IDC_DEV_READY:
+		ret = qlcnic_sriov_vf_idc_ready_state(adapter);
+		break;
+	case QLC_83XX_IDC_DEV_NEED_RESET:
+	case QLC_83XX_IDC_DEV_INIT:
+		ret = qlcnic_sriov_vf_idc_init_reset_state(adapter);
+		break;
+	case QLC_83XX_IDC_DEV_NEED_QUISCENT:
+		ret = qlcnic_sriov_vf_idc_need_quiescent_state(adapter);
+		break;
+	case QLC_83XX_IDC_DEV_FAILED:
+		ret = qlcnic_sriov_vf_idc_failed_state(adapter);
+		break;
+	case QLC_83XX_IDC_DEV_QUISCENT:
+		break;
+	default:
+		ret = qlcnic_sriov_vf_idc_unknown_state(adapter);
+	}
+
+	idc->prev_state = idc->curr_state;
+	if (!ret && test_bit(QLC_83XX_MODULE_LOADED, &idc->status))
+		qlcnic_schedule_work(adapter, qlcnic_sriov_vf_poll_dev_state,
+				     idc->delay);
+}
+
+static void qlcnic_sriov_vf_cancel_fw_work(struct qlcnic_adapter *adapter)
+{
+	while (test_and_set_bit(__QLCNIC_RESETTING, &adapter->state))
+		msleep(20);
+
+	clear_bit(QLC_83XX_MODULE_LOADED, &adapter->ahw->idc.status);
+	clear_bit(__QLCNIC_RESETTING, &adapter->state);
+	cancel_delayed_work_sync(&adapter->fw_work);
+}
+
+static int qlcnic_sriov_validate_vlan_cfg(struct qlcnic_sriov *sriov,
+					  u16 vid, u8 enable)
+{
+	u16 vlan = sriov->vlan;
+	u8 allowed = 0;
+	int i;
+
+	if (sriov->vlan_mode != QLC_GUEST_VLAN_MODE)
+		return -EINVAL;
+
+	if (enable) {
+		if (vlan)
+			return -EINVAL;
+
+		if (sriov->any_vlan) {
+			for (i = 0; i < sriov->num_allowed_vlans; i++) {
+				if (sriov->allowed_vlans[i] == vid)
+					allowed = 1;
+			}
+
+			if (!allowed)
+				return -EINVAL;
+		}
+	} else {
+		if (!vlan || vlan != vid)
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
+int qlcnic_sriov_cfg_vf_guest_vlan(struct qlcnic_adapter *adapter,
+				   u16 vid, u8 enable)
+{
+	struct qlcnic_sriov *sriov = adapter->ahw->sriov;
+	struct qlcnic_cmd_args cmd;
+	int ret;
+
+	if (vid == 0)
+		return 0;
+
+	ret = qlcnic_sriov_validate_vlan_cfg(sriov, vid, enable);
+	if (ret)
+		return ret;
+
+	ret = qlcnic_sriov_alloc_bc_mbx_args(&cmd,
+					     QLCNIC_BC_CMD_CFG_GUEST_VLAN);
+	if (ret)
+		return ret;
+
+	cmd.req.arg[1] = (enable & 1) | vid << 16;
+
+	qlcnic_sriov_cleanup_async_list(&sriov->bc);
+	ret = qlcnic_issue_cmd(adapter, &cmd);
+	if (ret) {
+		dev_err(&adapter->pdev->dev,
+			"Failed to configure guest VLAN, err=%d\n", ret);
+	} else {
+		qlcnic_free_mac_list(adapter);
+
+		if (enable)
+			sriov->vlan = vid;
+		else
+			sriov->vlan = 0;
+
+		qlcnic_sriov_vf_set_multi(adapter->netdev);
+	}
+
+	qlcnic_free_mbx_args(&cmd);
+	return ret;
+}
+
+static void qlcnic_sriov_vf_free_mac_list(struct qlcnic_adapter *adapter)
+{
+	struct list_head *head = &adapter->mac_list;
+	struct qlcnic_mac_list_s *cur;
+	u16 vlan;
+
+	vlan = adapter->ahw->sriov->vlan;
+
+	while (!list_empty(head)) {
+		cur = list_entry(head->next, struct qlcnic_mac_list_s, list);
+		qlcnic_sre_macaddr_change(adapter, cur->mac_addr,
+					  vlan, QLCNIC_MAC_DEL);
+		list_del(&cur->list);
+		kfree(cur);
+	}
+}
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov_pf.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov_pf.c
new file mode 100644
index 000000000000..c81be2da119b
--- /dev/null
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov_pf.c
@@ -0,0 +1,1780 @@
+/*
+ * QLogic qlcnic NIC Driver
+ * Copyright (c) 2009-2013 QLogic Corporation
+ *
+ * See LICENSE.qlcnic for copyright and licensing details.
+ */
+
+#include "qlcnic_sriov.h"
+#include "qlcnic.h"
+#include <linux/types.h>
+
+#define QLCNIC_SRIOV_VF_MAX_MAC 1
+#define QLC_VF_MIN_TX_RATE	100
+#define QLC_VF_MAX_TX_RATE	9999
+
+static int qlcnic_sriov_pf_get_vport_handle(struct qlcnic_adapter *, u8);
+
+struct qlcnic_sriov_cmd_handler {
+	int (*fn) (struct qlcnic_bc_trans *, struct qlcnic_cmd_args *);
+};
+
+struct qlcnic_sriov_fw_cmd_handler {
+	u32 cmd;
+	int (*fn) (struct qlcnic_bc_trans *, struct qlcnic_cmd_args *);
+};
+
+static int qlcnic_sriov_pf_set_vport_info(struct qlcnic_adapter *adapter,
+					  struct qlcnic_info *npar_info,
+					  u16 vport_id)
+{
+	struct qlcnic_cmd_args cmd;
+	int err;
+
+	if (qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_SET_NIC_INFO))
+		return -ENOMEM;
+
+	cmd.req.arg[1] = (vport_id << 16) | 0x1;
+	cmd.req.arg[2] = npar_info->bit_offsets;
+	cmd.req.arg[2] |= npar_info->min_tx_bw << 16;
+	cmd.req.arg[3] = npar_info->max_tx_bw | (npar_info->max_tx_ques << 16);
+	cmd.req.arg[4] = npar_info->max_tx_mac_filters;
+	cmd.req.arg[4] |= npar_info->max_rx_mcast_mac_filters << 16;
+	cmd.req.arg[5] = npar_info->max_rx_ucast_mac_filters |
+			 (npar_info->max_rx_ip_addr << 16);
+	cmd.req.arg[6] = npar_info->max_rx_lro_flow |
+			 (npar_info->max_rx_status_rings << 16);
+	cmd.req.arg[7] = npar_info->max_rx_buf_rings |
+			 (npar_info->max_rx_ques << 16);
+	cmd.req.arg[8] = npar_info->max_tx_vlan_keys;
+	cmd.req.arg[8] |= npar_info->max_local_ipv6_addrs << 16;
+	cmd.req.arg[9] = npar_info->max_remote_ipv6_addrs;
+
+	err = qlcnic_issue_cmd(adapter, &cmd);
+	if (err)
+		dev_err(&adapter->pdev->dev,
+			"Failed to set vport info, err=%d\n", err);
+
+	qlcnic_free_mbx_args(&cmd);
+	return err;
+}
+
+static int qlcnic_sriov_pf_cal_res_limit(struct qlcnic_adapter *adapter,
+					 struct qlcnic_info *info, u16 func)
+{
+	struct qlcnic_sriov *sriov = adapter->ahw->sriov;
+	struct qlcnic_resources *res = &sriov->ff_max;
+	u32 temp, num_vf_macs, num_vfs, max;
+	int ret = -EIO, vpid, id;
+	struct qlcnic_vport *vp;
+
+	vpid = qlcnic_sriov_pf_get_vport_handle(adapter, func);
+	if (vpid < 0)
+		return -EINVAL;
+
+	num_vfs = sriov->num_vfs;
+	max = num_vfs + 1;
+	info->bit_offsets = 0xffff;
+	info->max_tx_ques = res->num_tx_queues / max;
+	info->max_rx_mcast_mac_filters = res->num_rx_mcast_mac_filters;
+	num_vf_macs = QLCNIC_SRIOV_VF_MAX_MAC;
+
+	if (adapter->ahw->pci_func == func) {
+		temp = res->num_rx_mcast_mac_filters - (num_vfs * num_vf_macs);
+		info->max_rx_ucast_mac_filters = temp;
+		temp = res->num_tx_mac_filters - (num_vfs * num_vf_macs);
+		info->max_tx_mac_filters = temp;
+		info->min_tx_bw = 0;
+		info->max_tx_bw = MAX_BW;
+	} else {
+		id = qlcnic_sriov_func_to_index(adapter, func);
+		if (id < 0)
+			return id;
+		vp = sriov->vf_info[id].vp;
+		info->min_tx_bw = vp->min_tx_bw;
+		info->max_tx_bw = vp->max_tx_bw;
+		info->max_rx_ucast_mac_filters = num_vf_macs;
+		info->max_tx_mac_filters = num_vf_macs;
+	}
+
+	info->max_rx_ip_addr = res->num_destip / max;
+	info->max_rx_status_rings = res->num_rx_status_rings / max;
+	info->max_rx_buf_rings = res->num_rx_buf_rings / max;
+	info->max_rx_ques = res->num_rx_queues / max;
+	info->max_rx_lro_flow = res->num_lro_flows_supported / max;
+	info->max_tx_vlan_keys = res->num_txvlan_keys;
+	info->max_local_ipv6_addrs = res->max_local_ipv6_addrs;
+	info->max_remote_ipv6_addrs = res->max_remote_ipv6_addrs;
+
+	ret = qlcnic_sriov_pf_set_vport_info(adapter, info, vpid);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static void qlcnic_sriov_pf_set_ff_max_res(struct qlcnic_adapter *adapter,
+					   struct qlcnic_info *info)
+{
+	struct qlcnic_resources *ff_max = &adapter->ahw->sriov->ff_max;
+
+	ff_max->num_tx_mac_filters = info->max_tx_mac_filters;
+	ff_max->num_rx_ucast_mac_filters = info->max_rx_ucast_mac_filters;
+	ff_max->num_rx_mcast_mac_filters = info->max_rx_mcast_mac_filters;
+	ff_max->num_txvlan_keys = info->max_tx_vlan_keys;
+	ff_max->num_rx_queues = info->max_rx_ques;
+	ff_max->num_tx_queues = info->max_tx_ques;
+	ff_max->num_lro_flows_supported = info->max_rx_lro_flow;
+	ff_max->num_destip = info->max_rx_ip_addr;
+	ff_max->num_rx_buf_rings = info->max_rx_buf_rings;
+	ff_max->num_rx_status_rings = info->max_rx_status_rings;
+	ff_max->max_remote_ipv6_addrs = info->max_remote_ipv6_addrs;
+	ff_max->max_local_ipv6_addrs = info->max_local_ipv6_addrs;
+}
+
+static int qlcnic_sriov_get_pf_info(struct qlcnic_adapter *adapter,
+				    struct qlcnic_info *npar_info)
+{
+	int err;
+	struct qlcnic_cmd_args cmd;
+
+	if (qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_GET_NIC_INFO))
+		return -ENOMEM;
+
+	cmd.req.arg[1] = 0x2;
+	err = qlcnic_issue_cmd(adapter, &cmd);
+	if (err) {
+		dev_err(&adapter->pdev->dev,
+			"Failed to get PF info, err=%d\n", err);
+		goto out;
+	}
+
+	npar_info->total_pf = cmd.rsp.arg[2] & 0xff;
+	npar_info->total_rss_engines = (cmd.rsp.arg[2] >> 8) & 0xff;
+	npar_info->max_vports = MSW(cmd.rsp.arg[2]);
+	npar_info->max_tx_ques =  LSW(cmd.rsp.arg[3]);
+	npar_info->max_tx_mac_filters = MSW(cmd.rsp.arg[3]);
+	npar_info->max_rx_mcast_mac_filters = LSW(cmd.rsp.arg[4]);
+	npar_info->max_rx_ucast_mac_filters = MSW(cmd.rsp.arg[4]);
+	npar_info->max_rx_ip_addr = LSW(cmd.rsp.arg[5]);
+	npar_info->max_rx_lro_flow = MSW(cmd.rsp.arg[5]);
+	npar_info->max_rx_status_rings = LSW(cmd.rsp.arg[6]);
+	npar_info->max_rx_buf_rings = MSW(cmd.rsp.arg[6]);
+	npar_info->max_rx_ques = LSW(cmd.rsp.arg[7]);
+	npar_info->max_tx_vlan_keys = MSW(cmd.rsp.arg[7]);
+	npar_info->max_local_ipv6_addrs = LSW(cmd.rsp.arg[8]);
+	npar_info->max_remote_ipv6_addrs = MSW(cmd.rsp.arg[8]);
+
+	qlcnic_sriov_pf_set_ff_max_res(adapter, npar_info);
+	dev_info(&adapter->pdev->dev,
+		 "\n\ttotal_pf: %d,\n"
+		 "\n\ttotal_rss_engines: %d max_vports: %d max_tx_ques %d,\n"
+		 "\tmax_tx_mac_filters: %d max_rx_mcast_mac_filters: %d,\n"
+		 "\tmax_rx_ucast_mac_filters: 0x%x, max_rx_ip_addr: %d,\n"
+		 "\tmax_rx_lro_flow: %d max_rx_status_rings: %d,\n"
+		 "\tmax_rx_buf_rings: %d, max_rx_ques: %d, max_tx_vlan_keys %d\n"
+		 "\tmax_local_ipv6_addrs: %d, max_remote_ipv6_addrs: %d\n",
+		 npar_info->total_pf, npar_info->total_rss_engines,
+		 npar_info->max_vports, npar_info->max_tx_ques,
+		 npar_info->max_tx_mac_filters,
+		 npar_info->max_rx_mcast_mac_filters,
+		 npar_info->max_rx_ucast_mac_filters, npar_info->max_rx_ip_addr,
+		 npar_info->max_rx_lro_flow, npar_info->max_rx_status_rings,
+		 npar_info->max_rx_buf_rings, npar_info->max_rx_ques,
+		 npar_info->max_tx_vlan_keys, npar_info->max_local_ipv6_addrs,
+		 npar_info->max_remote_ipv6_addrs);
+
+out:
+	qlcnic_free_mbx_args(&cmd);
+	return err;
+}
+
+static void qlcnic_sriov_pf_reset_vport_handle(struct qlcnic_adapter *adapter,
+					       u8 func)
+{
+	struct qlcnic_sriov  *sriov = adapter->ahw->sriov;
+	struct qlcnic_vport *vp;
+	int index;
+
+	if (adapter->ahw->pci_func == func) {
+		sriov->vp_handle = 0;
+	} else {
+		index = qlcnic_sriov_func_to_index(adapter, func);
+		if (index < 0)
+			return;
+		vp = sriov->vf_info[index].vp;
+		vp->handle = 0;
+	}
+}
+
+static void qlcnic_sriov_pf_set_vport_handle(struct qlcnic_adapter *adapter,
+					     u16 vport_handle, u8 func)
+{
+	struct qlcnic_sriov  *sriov = adapter->ahw->sriov;
+	struct qlcnic_vport *vp;
+	int index;
+
+	if (adapter->ahw->pci_func == func) {
+		sriov->vp_handle = vport_handle;
+	} else {
+		index = qlcnic_sriov_func_to_index(adapter, func);
+		if (index < 0)
+			return;
+		vp = sriov->vf_info[index].vp;
+		vp->handle = vport_handle;
+	}
+}
+
+static int qlcnic_sriov_pf_get_vport_handle(struct qlcnic_adapter *adapter,
+					    u8 func)
+{
+	struct qlcnic_sriov  *sriov = adapter->ahw->sriov;
+	struct qlcnic_vf_info *vf_info;
+	int index;
+
+	if (adapter->ahw->pci_func == func) {
+		return sriov->vp_handle;
+	} else {
+		index = qlcnic_sriov_func_to_index(adapter, func);
+		if (index >= 0) {
+			vf_info = &sriov->vf_info[index];
+			return vf_info->vp->handle;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int qlcnic_sriov_pf_config_vport(struct qlcnic_adapter *adapter,
+					u8 flag, u16 func)
+{
+	struct qlcnic_cmd_args cmd;
+	int ret;
+	int vpid;
+
+	if (qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_CONFIG_VPORT))
+		return -ENOMEM;
+
+	if (flag) {
+		cmd.req.arg[3] = func << 8;
+	} else {
+		vpid = qlcnic_sriov_pf_get_vport_handle(adapter, func);
+		if (vpid < 0) {
+			ret = -EINVAL;
+			goto out;
+		}
+		cmd.req.arg[3] = ((vpid & 0xffff) << 8) | 1;
+	}
+
+	ret = qlcnic_issue_cmd(adapter, &cmd);
+	if (ret) {
+		dev_err(&adapter->pdev->dev,
+			"Failed %s vport, err %d for func 0x%x\n",
+			(flag ? "enable" : "disable"), ret, func);
+		goto out;
+	}
+
+	if (flag) {
+		vpid = cmd.rsp.arg[2] & 0xffff;
+		qlcnic_sriov_pf_set_vport_handle(adapter, vpid, func);
+	} else {
+		qlcnic_sriov_pf_reset_vport_handle(adapter, func);
+	}
+
+out:
+	qlcnic_free_mbx_args(&cmd);
+	return ret;
+}
+
+static int qlcnic_sriov_pf_cfg_vlan_filtering(struct qlcnic_adapter *adapter,
+					      u8 enable)
+{
+	struct qlcnic_cmd_args cmd;
+	int err;
+
+	err = qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_SET_NIC_INFO);
+	if (err)
+		return err;
+
+	cmd.req.arg[1] = 0x4;
+	if (enable)
+		cmd.req.arg[1] |= BIT_16;
+
+	err = qlcnic_issue_cmd(adapter, &cmd);
+	if (err)
+		dev_err(&adapter->pdev->dev,
+			"Failed to configure VLAN filtering, err=%d\n", err);
+
+	qlcnic_free_mbx_args(&cmd);
+	return err;
+}
+
+static int qlcnic_sriov_pf_cfg_eswitch(struct qlcnic_adapter *adapter,
+				       u8 func, u8 enable)
+{
+	struct qlcnic_cmd_args cmd;
+	int err = -EIO;
+
+	if (qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_TOGGLE_ESWITCH))
+		return -ENOMEM;
+
+	cmd.req.arg[0] |= (3 << 29);
+	cmd.req.arg[1] = ((func & 0xf) << 2) | BIT_6 | BIT_1;
+	if (enable)
+		cmd.req.arg[1] |= BIT_0;
+
+	err = qlcnic_issue_cmd(adapter, &cmd);
+
+	if (err != QLCNIC_RCODE_SUCCESS) {
+		dev_err(&adapter->pdev->dev,
+			"Failed to enable sriov eswitch%d\n", err);
+		err = -EIO;
+	}
+
+	qlcnic_free_mbx_args(&cmd);
+	return err;
+}
+
+static void qlcnic_sriov_pf_del_flr_queue(struct qlcnic_adapter *adapter)
+{
+	struct qlcnic_sriov *sriov = adapter->ahw->sriov;
+	struct qlcnic_back_channel *bc = &sriov->bc;
+	int i;
+
+	for (i = 0; i < sriov->num_vfs; i++)
+		cancel_work_sync(&sriov->vf_info[i].flr_work);
+
+	destroy_workqueue(bc->bc_flr_wq);
+}
+
+static int qlcnic_sriov_pf_create_flr_queue(struct qlcnic_adapter *adapter)
+{
+	struct qlcnic_back_channel *bc = &adapter->ahw->sriov->bc;
+	struct workqueue_struct *wq;
+
+	wq = create_singlethread_workqueue("qlcnic-flr");
+	if (wq == NULL) {
+		dev_err(&adapter->pdev->dev, "Cannot create FLR workqueue\n");
+		return -ENOMEM;
+	}
+
+	bc->bc_flr_wq =  wq;
+	return 0;
+}
+
+void qlcnic_sriov_pf_cleanup(struct qlcnic_adapter *adapter)
+{
+	u8 func = adapter->ahw->pci_func;
+
+	if (!qlcnic_sriov_enable_check(adapter))
+		return;
+
+	qlcnic_sriov_pf_del_flr_queue(adapter);
+	qlcnic_sriov_cfg_bc_intr(adapter, 0);
+	qlcnic_sriov_pf_config_vport(adapter, 0, func);
+	qlcnic_sriov_pf_cfg_eswitch(adapter, func, 0);
+	qlcnic_sriov_pf_cfg_vlan_filtering(adapter, 0);
+	__qlcnic_sriov_cleanup(adapter);
+	adapter->ahw->op_mode = QLCNIC_MGMT_FUNC;
+	clear_bit(__QLCNIC_SRIOV_ENABLE, &adapter->state);
+}
+
+void qlcnic_sriov_pf_disable(struct qlcnic_adapter *adapter)
+{
+	if (!qlcnic_sriov_pf_check(adapter))
+		return;
+
+	if (!qlcnic_sriov_enable_check(adapter))
+		return;
+
+	pci_disable_sriov(adapter->pdev);
+	netdev_info(adapter->netdev,
+		    "SR-IOV is disabled successfully on port %d\n",
+		    adapter->portnum);
+}
+
+static int qlcnic_pci_sriov_disable(struct qlcnic_adapter *adapter)
+{
+	struct net_device *netdev = adapter->netdev;
+
+	if (netif_running(netdev))
+		__qlcnic_down(adapter, netdev);
+
+	qlcnic_sriov_pf_disable(adapter);
+
+	qlcnic_sriov_pf_cleanup(adapter);
+
+	/* After disabling SRIOV re-init the driver in default mode
+	   configure opmode based on op_mode of function
+	 */
+	if (qlcnic_83xx_configure_opmode(adapter))
+		return -EIO;
+
+	if (netif_running(netdev))
+		__qlcnic_up(adapter, netdev);
+
+	return 0;
+}
+
+static int qlcnic_sriov_pf_init(struct qlcnic_adapter *adapter)
+{
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
+	struct qlcnic_info nic_info, pf_info, vp_info;
+	int err;
+	u8 func = ahw->pci_func;
+
+	if (!qlcnic_sriov_enable_check(adapter))
+		return 0;
+
+	err = qlcnic_sriov_pf_cfg_vlan_filtering(adapter, 1);
+	if (err)
+		return err;
+
+	err = qlcnic_sriov_pf_cfg_eswitch(adapter, func, 1);
+	if (err)
+		goto disable_vlan_filtering;
+
+	err = qlcnic_sriov_pf_config_vport(adapter, 1, func);
+	if (err)
+		goto disable_eswitch;
+
+	err = qlcnic_sriov_get_pf_info(adapter, &pf_info);
+	if (err)
+		goto delete_vport;
+
+	err = qlcnic_get_nic_info(adapter, &nic_info, func);
+	if (err)
+		goto delete_vport;
+
+	err = qlcnic_sriov_pf_cal_res_limit(adapter, &vp_info, func);
+	if (err)
+		goto delete_vport;
+
+	err = qlcnic_sriov_cfg_bc_intr(adapter, 1);
+	if (err)
+		goto delete_vport;
+
+	ahw->physical_port = (u8) nic_info.phys_port;
+	ahw->switch_mode = nic_info.switch_mode;
+	ahw->max_mtu = nic_info.max_mtu;
+	ahw->capabilities = nic_info.capabilities;
+	ahw->nic_mode = QLC_83XX_SRIOV_MODE;
+	return err;
+
+delete_vport:
+	qlcnic_sriov_pf_config_vport(adapter, 0, func);
+
+disable_eswitch:
+	qlcnic_sriov_pf_cfg_eswitch(adapter, func, 0);
+
+disable_vlan_filtering:
+	qlcnic_sriov_pf_cfg_vlan_filtering(adapter, 0);
+
+	return err;
+}
+
+static int qlcnic_sriov_pf_enable(struct qlcnic_adapter *adapter, int num_vfs)
+{
+	int err;
+
+	if (!qlcnic_sriov_enable_check(adapter))
+		return 0;
+
+	err = pci_enable_sriov(adapter->pdev, num_vfs);
+	if (err)
+		qlcnic_sriov_pf_cleanup(adapter);
+
+	return err;
+}
+
+static int __qlcnic_pci_sriov_enable(struct qlcnic_adapter *adapter,
+				     int num_vfs)
+{
+	int err = 0;
+
+	set_bit(__QLCNIC_SRIOV_ENABLE, &adapter->state);
+	adapter->ahw->op_mode = QLCNIC_SRIOV_PF_FUNC;
+
+	err = qlcnic_sriov_init(adapter, num_vfs);
+	if (err)
+		goto clear_op_mode;
+
+	err = qlcnic_sriov_pf_create_flr_queue(adapter);
+	if (err)
+		goto sriov_cleanup;
+
+	err = qlcnic_sriov_pf_init(adapter);
+	if (err)
+		goto del_flr_queue;
+
+	err = qlcnic_sriov_pf_enable(adapter, num_vfs);
+	return err;
+
+del_flr_queue:
+	qlcnic_sriov_pf_del_flr_queue(adapter);
+
+sriov_cleanup:
+	__qlcnic_sriov_cleanup(adapter);
+
+clear_op_mode:
+	clear_bit(__QLCNIC_SRIOV_ENABLE, &adapter->state);
+	adapter->ahw->op_mode = QLCNIC_MGMT_FUNC;
+	return err;
+}
+
+static int qlcnic_pci_sriov_enable(struct qlcnic_adapter *adapter, int num_vfs)
+{
+	struct net_device *netdev = adapter->netdev;
+	int err;
+
+	if (!(adapter->flags & QLCNIC_MSIX_ENABLED)) {
+		netdev_err(netdev,
+			   "SR-IOV cannot be enabled, when legacy interrupts are enabled\n");
+		return -EIO;
+	}
+
+	if (netif_running(netdev))
+		__qlcnic_down(adapter, netdev);
+
+	err = __qlcnic_pci_sriov_enable(adapter, num_vfs);
+	if (err) {
+		netdev_info(netdev, "Failed to enable SR-IOV on port %d\n",
+			    adapter->portnum);
+
+		err = -EIO;
+		if (qlcnic_83xx_configure_opmode(adapter))
+			goto error;
+	} else {
+		netdev_info(netdev,
+			    "SR-IOV is enabled successfully on port %d\n",
+			    adapter->portnum);
+		/* Return number of vfs enabled */
+		err = num_vfs;
+	}
+	if (netif_running(netdev))
+		__qlcnic_up(adapter, netdev);
+
+error:
+	return err;
+}
+
+int qlcnic_pci_sriov_configure(struct pci_dev *dev, int num_vfs)
+{
+	struct qlcnic_adapter *adapter = pci_get_drvdata(dev);
+	int err;
+
+	if (test_and_set_bit(__QLCNIC_RESETTING, &adapter->state))
+		return -EBUSY;
+
+	if (num_vfs == 0)
+		err = qlcnic_pci_sriov_disable(adapter);
+	else
+		err = qlcnic_pci_sriov_enable(adapter, num_vfs);
+
+	clear_bit(__QLCNIC_RESETTING, &adapter->state);
+	return err;
+}
+
+static int qlcnic_sriov_set_vf_acl(struct qlcnic_adapter *adapter, u8 func)
+{
+	struct qlcnic_cmd_args cmd;
+	struct qlcnic_vport *vp;
+	int err, id;
+
+	id = qlcnic_sriov_func_to_index(adapter, func);
+	if (id < 0)
+		return id;
+
+	vp = adapter->ahw->sriov->vf_info[id].vp;
+	err = qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_SET_NIC_INFO);
+	if (err)
+		return err;
+
+	cmd.req.arg[1] = 0x3 | func << 16;
+	if (vp->vlan_mode == QLC_PVID_MODE) {
+		cmd.req.arg[2] |= BIT_6;
+		cmd.req.arg[3] |= vp->vlan << 8;
+	}
+
+	err = qlcnic_issue_cmd(adapter, &cmd);
+	if (err)
+		dev_err(&adapter->pdev->dev, "Failed to set ACL, err=%d\n",
+			err);
+
+	qlcnic_free_mbx_args(&cmd);
+	return err;
+}
+
+static int qlcnic_sriov_set_vf_vport_info(struct qlcnic_adapter *adapter,
+					  u16 func)
+{
+	struct qlcnic_info defvp_info;
+	int err;
+
+	err = qlcnic_sriov_pf_cal_res_limit(adapter, &defvp_info, func);
+	if (err)
+		return -EIO;
+
+	err = qlcnic_sriov_set_vf_acl(adapter, func);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+static int qlcnic_sriov_pf_channel_cfg_cmd(struct qlcnic_bc_trans *trans,
+					   struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	int err;
+	u16 func = vf->pci_func;
+
+	cmd->rsp.arg[0] = trans->req_hdr->cmd_op;
+	cmd->rsp.arg[0] |= (1 << 16);
+
+	if (trans->req_hdr->cmd_op == QLCNIC_BC_CMD_CHANNEL_INIT) {
+		err = qlcnic_sriov_pf_config_vport(adapter, 1, func);
+		if (!err) {
+			err = qlcnic_sriov_set_vf_vport_info(adapter, func);
+			if (err)
+				qlcnic_sriov_pf_config_vport(adapter, 0, func);
+		}
+	} else {
+		err = qlcnic_sriov_pf_config_vport(adapter, 0, func);
+	}
+
+	if (err)
+		goto err_out;
+
+	cmd->rsp.arg[0] |= (1 << 25);
+
+	if (trans->req_hdr->cmd_op == QLCNIC_BC_CMD_CHANNEL_INIT)
+		set_bit(QLC_BC_VF_STATE, &vf->state);
+	else
+		clear_bit(QLC_BC_VF_STATE, &vf->state);
+
+	return err;
+
+err_out:
+	cmd->rsp.arg[0] |= (2 << 25);
+	return err;
+}
+
+static int qlcnic_sriov_cfg_vf_def_mac(struct qlcnic_adapter *adapter,
+				       struct qlcnic_vport *vp,
+				       u16 func, u16 vlan, u8 op)
+{
+	struct qlcnic_cmd_args cmd;
+	struct qlcnic_macvlan_mbx mv;
+	u8 *addr;
+	int err;
+	u32 *buf;
+	int vpid;
+
+	if (qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_CONFIG_MAC_VLAN))
+		return -ENOMEM;
+
+	vpid = qlcnic_sriov_pf_get_vport_handle(adapter, func);
+	if (vpid < 0) {
+		err = -EINVAL;
+		goto out;
+	}
+
+	if (vlan)
+		op = ((op == QLCNIC_MAC_ADD || op == QLCNIC_MAC_VLAN_ADD) ?
+		      QLCNIC_MAC_VLAN_ADD : QLCNIC_MAC_VLAN_DEL);
+
+	cmd.req.arg[1] = op | (1 << 8) | (3 << 6);
+	cmd.req.arg[1] |= ((vpid & 0xffff) << 16) | BIT_31;
+
+	addr = vp->mac;
+	mv.vlan = vlan;
+	mv.mac_addr0 = addr[0];
+	mv.mac_addr1 = addr[1];
+	mv.mac_addr2 = addr[2];
+	mv.mac_addr3 = addr[3];
+	mv.mac_addr4 = addr[4];
+	mv.mac_addr5 = addr[5];
+	buf = &cmd.req.arg[2];
+	memcpy(buf, &mv, sizeof(struct qlcnic_macvlan_mbx));
+
+	err = qlcnic_issue_cmd(adapter, &cmd);
+
+	if (err)
+		dev_err(&adapter->pdev->dev,
+			"MAC-VLAN %s to CAM failed, err=%d.\n",
+			((op == 1) ? "add " : "delete "), err);
+
+out:
+	qlcnic_free_mbx_args(&cmd);
+	return err;
+}
+
+static int qlcnic_sriov_validate_create_rx_ctx(struct qlcnic_cmd_args *cmd)
+{
+	if ((cmd->req.arg[0] >> 29) != 0x3)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int qlcnic_sriov_pf_create_rx_ctx_cmd(struct qlcnic_bc_trans *tran,
+					     struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = tran->vf;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	struct qlcnic_rcv_mbx_out *mbx_out;
+	int err;
+	u16 vlan;
+
+	err = qlcnic_sriov_validate_create_rx_ctx(cmd);
+	if (err) {
+		cmd->rsp.arg[0] |= (0x6 << 25);
+		return err;
+	}
+
+	cmd->req.arg[6] = vf->vp->handle;
+	err = qlcnic_issue_cmd(adapter, cmd);
+
+	vlan = vf->vp->vlan;
+	if (!err) {
+		mbx_out = (struct qlcnic_rcv_mbx_out *)&cmd->rsp.arg[1];
+		vf->rx_ctx_id = mbx_out->ctx_id;
+		qlcnic_sriov_cfg_vf_def_mac(adapter, vf->vp, vf->pci_func,
+					    vlan, QLCNIC_MAC_ADD);
+	} else {
+		vf->rx_ctx_id = 0;
+	}
+
+	return err;
+}
+
+static int qlcnic_sriov_pf_mac_address_cmd(struct qlcnic_bc_trans *trans,
+					   struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	u8 type, *mac;
+
+	type = cmd->req.arg[1];
+	switch (type) {
+	case QLCNIC_SET_STATION_MAC:
+	case QLCNIC_SET_FAC_DEF_MAC:
+		cmd->rsp.arg[0] = (2 << 25);
+		break;
+	case QLCNIC_GET_CURRENT_MAC:
+		cmd->rsp.arg[0] = (1 << 25);
+		mac = vf->vp->mac;
+		cmd->rsp.arg[2] = mac[1] | ((mac[0] << 8) & 0xff00);
+		cmd->rsp.arg[1] = mac[5] | ((mac[4] << 8) & 0xff00) |
+				  ((mac[3]) << 16 & 0xff0000) |
+				  ((mac[2]) << 24 & 0xff000000);
+	}
+
+	return 0;
+}
+
+static int qlcnic_sriov_validate_create_tx_ctx(struct qlcnic_cmd_args *cmd)
+{
+	if ((cmd->req.arg[0] >> 29) != 0x3)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int qlcnic_sriov_pf_create_tx_ctx_cmd(struct qlcnic_bc_trans *trans,
+					     struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	struct qlcnic_tx_mbx_out *mbx_out;
+	int err;
+
+	err = qlcnic_sriov_validate_create_tx_ctx(cmd);
+	if (err) {
+		cmd->rsp.arg[0] |= (0x6 << 25);
+		return err;
+	}
+
+	cmd->req.arg[5] |= vf->vp->handle << 16;
+	err = qlcnic_issue_cmd(adapter, cmd);
+	if (!err) {
+		mbx_out = (struct qlcnic_tx_mbx_out *)&cmd->rsp.arg[2];
+		vf->tx_ctx_id = mbx_out->ctx_id;
+	} else {
+		vf->tx_ctx_id = 0;
+	}
+
+	return err;
+}
+
+static int qlcnic_sriov_validate_del_rx_ctx(struct qlcnic_vf_info *vf,
+					    struct qlcnic_cmd_args *cmd)
+{
+	if ((cmd->req.arg[0] >> 29) != 0x3)
+		return -EINVAL;
+
+	if ((cmd->req.arg[1] & 0xffff) != vf->rx_ctx_id)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int qlcnic_sriov_pf_del_rx_ctx_cmd(struct qlcnic_bc_trans *trans,
+					  struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	int err;
+	u16 vlan;
+
+	err = qlcnic_sriov_validate_del_rx_ctx(vf, cmd);
+	if (err) {
+		cmd->rsp.arg[0] |= (0x6 << 25);
+		return err;
+	}
+
+	vlan = vf->vp->vlan;
+	qlcnic_sriov_cfg_vf_def_mac(adapter, vf->vp, vf->pci_func,
+				    vlan, QLCNIC_MAC_DEL);
+	cmd->req.arg[1] |= vf->vp->handle << 16;
+	err = qlcnic_issue_cmd(adapter, cmd);
+
+	if (!err)
+		vf->rx_ctx_id = 0;
+
+	return err;
+}
+
+static int qlcnic_sriov_validate_del_tx_ctx(struct qlcnic_vf_info *vf,
+					    struct qlcnic_cmd_args *cmd)
+{
+	if ((cmd->req.arg[0] >> 29) != 0x3)
+		return -EINVAL;
+
+	if ((cmd->req.arg[1] & 0xffff) != vf->tx_ctx_id)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int qlcnic_sriov_pf_del_tx_ctx_cmd(struct qlcnic_bc_trans *trans,
+					  struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	int err;
+
+	err = qlcnic_sriov_validate_del_tx_ctx(vf, cmd);
+	if (err) {
+		cmd->rsp.arg[0] |= (0x6 << 25);
+		return err;
+	}
+
+	cmd->req.arg[1] |= vf->vp->handle << 16;
+	err = qlcnic_issue_cmd(adapter, cmd);
+
+	if (!err)
+		vf->tx_ctx_id = 0;
+
+	return err;
+}
+
+static int qlcnic_sriov_validate_cfg_lro(struct qlcnic_vf_info *vf,
+					 struct qlcnic_cmd_args *cmd)
+{
+	if ((cmd->req.arg[1] >> 16) != vf->rx_ctx_id)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int qlcnic_sriov_pf_cfg_lro_cmd(struct qlcnic_bc_trans *trans,
+				       struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	int err;
+
+	err = qlcnic_sriov_validate_cfg_lro(vf, cmd);
+	if (err) {
+		cmd->rsp.arg[0] |= (0x6 << 25);
+		return err;
+	}
+
+	err = qlcnic_issue_cmd(adapter, cmd);
+	return err;
+}
+
+static int qlcnic_sriov_pf_cfg_ip_cmd(struct qlcnic_bc_trans *trans,
+				      struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	int err = -EIO;
+	u8 op;
+
+	op =  cmd->req.arg[1] & 0xff;
+
+	cmd->req.arg[1] |= vf->vp->handle << 16;
+	cmd->req.arg[1] |= BIT_31;
+
+	err = qlcnic_issue_cmd(adapter, cmd);
+	return err;
+}
+
+static int qlcnic_sriov_validate_cfg_intrpt(struct qlcnic_vf_info *vf,
+					    struct qlcnic_cmd_args *cmd)
+{
+	if (((cmd->req.arg[1] >> 8) & 0xff) != vf->pci_func)
+		return -EINVAL;
+
+	if (!(cmd->req.arg[1] & BIT_16))
+		return -EINVAL;
+
+	if ((cmd->req.arg[1] & 0xff) != 0x1)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int qlcnic_sriov_pf_cfg_intrpt_cmd(struct qlcnic_bc_trans *trans,
+					  struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	int err;
+
+	err = qlcnic_sriov_validate_cfg_intrpt(vf, cmd);
+	if (err)
+		cmd->rsp.arg[0] |= (0x6 << 25);
+	else
+		err = qlcnic_issue_cmd(adapter, cmd);
+
+	return err;
+}
+
+static int qlcnic_sriov_validate_mtu(struct qlcnic_adapter *adapter,
+				     struct qlcnic_vf_info *vf,
+				     struct qlcnic_cmd_args *cmd)
+{
+	if (cmd->req.arg[1] != vf->rx_ctx_id)
+		return -EINVAL;
+
+	if (cmd->req.arg[2] > adapter->ahw->max_mtu)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int qlcnic_sriov_pf_set_mtu_cmd(struct qlcnic_bc_trans *trans,
+				       struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	int err;
+
+	err = qlcnic_sriov_validate_mtu(adapter, vf, cmd);
+	if (err)
+		cmd->rsp.arg[0] |= (0x6 << 25);
+	else
+		err = qlcnic_issue_cmd(adapter, cmd);
+
+	return err;
+}
+
+static int qlcnic_sriov_validate_get_nic_info(struct qlcnic_vf_info *vf,
+					      struct qlcnic_cmd_args *cmd)
+{
+	if (cmd->req.arg[1] & BIT_31) {
+		if (((cmd->req.arg[1] >> 16) & 0x7fff) != vf->pci_func)
+			return -EINVAL;
+	} else {
+		cmd->req.arg[1] |= vf->vp->handle << 16;
+	}
+
+	return 0;
+}
+
+static int qlcnic_sriov_pf_get_nic_info_cmd(struct qlcnic_bc_trans *trans,
+					    struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	int err;
+
+	err = qlcnic_sriov_validate_get_nic_info(vf, cmd);
+	if (err) {
+		cmd->rsp.arg[0] |= (0x6 << 25);
+		return err;
+	}
+
+	err = qlcnic_issue_cmd(adapter, cmd);
+	return err;
+}
+
+static int qlcnic_sriov_validate_cfg_rss(struct qlcnic_vf_info *vf,
+					 struct qlcnic_cmd_args *cmd)
+{
+	if (cmd->req.arg[1] != vf->rx_ctx_id)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int qlcnic_sriov_pf_cfg_rss_cmd(struct qlcnic_bc_trans *trans,
+				       struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	int err;
+
+	err = qlcnic_sriov_validate_cfg_rss(vf, cmd);
+	if (err)
+		cmd->rsp.arg[0] |= (0x6 << 25);
+	else
+		err = qlcnic_issue_cmd(adapter, cmd);
+
+	return err;
+}
+
+static int qlcnic_sriov_validate_cfg_intrcoal(struct qlcnic_adapter *adapter,
+					      struct qlcnic_vf_info *vf,
+					      struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_nic_intr_coalesce *coal = &adapter->ahw->coal;
+	u16 ctx_id, pkts, time;
+
+	ctx_id = cmd->req.arg[1] >> 16;
+	pkts = cmd->req.arg[2] & 0xffff;
+	time = cmd->req.arg[2] >> 16;
+
+	if (ctx_id != vf->rx_ctx_id)
+		return -EINVAL;
+	if (pkts > coal->rx_packets)
+		return -EINVAL;
+	if (time < coal->rx_time_us)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int qlcnic_sriov_pf_cfg_intrcoal_cmd(struct qlcnic_bc_trans *tran,
+					    struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = tran->vf;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	int err;
+
+	err = qlcnic_sriov_validate_cfg_intrcoal(adapter, vf, cmd);
+	if (err) {
+		cmd->rsp.arg[0] |= (0x6 << 25);
+		return err;
+	}
+
+	err = qlcnic_issue_cmd(adapter, cmd);
+	return err;
+}
+
+static int qlcnic_sriov_validate_cfg_macvlan(struct qlcnic_adapter *adapter,
+					     struct qlcnic_vf_info *vf,
+					     struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_macvlan_mbx *macvlan;
+	struct qlcnic_vport *vp = vf->vp;
+	u8 op, new_op;
+
+	if (!(cmd->req.arg[1] & BIT_8))
+		return -EINVAL;
+
+	cmd->req.arg[1] |= (vf->vp->handle << 16);
+	cmd->req.arg[1] |= BIT_31;
+
+	macvlan = (struct qlcnic_macvlan_mbx *)&cmd->req.arg[2];
+	if (!(macvlan->mac_addr0 & BIT_0)) {
+		dev_err(&adapter->pdev->dev,
+			"MAC address change is not allowed from VF %d",
+			vf->pci_func);
+		return -EINVAL;
+	}
+
+	if (vp->vlan_mode == QLC_PVID_MODE) {
+		op = cmd->req.arg[1] & 0x7;
+		cmd->req.arg[1] &= ~0x7;
+		new_op = (op == QLCNIC_MAC_ADD || op == QLCNIC_MAC_VLAN_ADD) ?
+			 QLCNIC_MAC_VLAN_ADD : QLCNIC_MAC_VLAN_DEL;
+		cmd->req.arg[3] |= vp->vlan << 16;
+		cmd->req.arg[1] |= new_op;
+	}
+
+	return 0;
+}
+
+static int qlcnic_sriov_pf_cfg_macvlan_cmd(struct qlcnic_bc_trans *trans,
+					   struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	int err;
+
+	err = qlcnic_sriov_validate_cfg_macvlan(adapter, vf, cmd);
+	if (err) {
+		cmd->rsp.arg[0] |= (0x6 << 25);
+		return err;
+	}
+
+	err = qlcnic_issue_cmd(adapter, cmd);
+	return err;
+}
+
+static int qlcnic_sriov_validate_linkevent(struct qlcnic_vf_info *vf,
+					   struct qlcnic_cmd_args *cmd)
+{
+	if ((cmd->req.arg[1] >> 16) != vf->rx_ctx_id)
+		return -EINVAL;
+
+	if (!(cmd->req.arg[1] & BIT_8))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int qlcnic_sriov_pf_linkevent_cmd(struct qlcnic_bc_trans *trans,
+					 struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	int err;
+
+	err = qlcnic_sriov_validate_linkevent(vf, cmd);
+	if (err) {
+		cmd->rsp.arg[0] |= (0x6 << 25);
+		return err;
+	}
+
+	err = qlcnic_issue_cmd(adapter, cmd);
+	return err;
+}
+
+static int qlcnic_sriov_pf_cfg_promisc_cmd(struct qlcnic_bc_trans *trans,
+					   struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	struct qlcnic_adapter *adapter = vf->adapter;
+	int err;
+
+	cmd->req.arg[1] |= vf->vp->handle << 16;
+	cmd->req.arg[1] |= BIT_31;
+	err = qlcnic_issue_cmd(adapter, cmd);
+	return err;
+}
+
+static int qlcnic_sriov_pf_get_acl_cmd(struct qlcnic_bc_trans *trans,
+				       struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info *vf = trans->vf;
+	struct qlcnic_vport *vp = vf->vp;
+	u8 cmd_op, mode = vp->vlan_mode;
+
+	cmd_op = trans->req_hdr->cmd_op;
+	cmd->rsp.arg[0] = (cmd_op & 0xffff) | 14 << 16 | 1 << 25;
+
+	switch (mode) {
+	case QLC_GUEST_VLAN_MODE:
+		cmd->rsp.arg[1] = mode | 1 << 8;
+		cmd->rsp.arg[2] = 1 << 16;
+		break;
+	case QLC_PVID_MODE:
+		cmd->rsp.arg[1] = mode | 1 << 8 | vp->vlan << 16;
+		break;
+	}
+
+	return 0;
+}
+
+static int qlcnic_sriov_pf_del_guest_vlan(struct qlcnic_adapter *adapter,
+					  struct qlcnic_vf_info *vf)
+
+{
+	struct qlcnic_vport *vp = vf->vp;
+
+	if (!vp->vlan)
+		return -EINVAL;
+
+	if (!vf->rx_ctx_id) {
+		vp->vlan = 0;
+		return 0;
+	}
+
+	qlcnic_sriov_cfg_vf_def_mac(adapter, vp, vf->pci_func,
+				    vp->vlan, QLCNIC_MAC_DEL);
+	vp->vlan = 0;
+	qlcnic_sriov_cfg_vf_def_mac(adapter, vp, vf->pci_func,
+				    0, QLCNIC_MAC_ADD);
+	return 0;
+}
+
+static int qlcnic_sriov_pf_add_guest_vlan(struct qlcnic_adapter *adapter,
+					  struct qlcnic_vf_info *vf,
+					  struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vport *vp = vf->vp;
+	int err = -EIO;
+
+	if (vp->vlan)
+		return err;
+
+	if (!vf->rx_ctx_id) {
+		vp->vlan = cmd->req.arg[1] >> 16;
+		return 0;
+	}
+
+	err = qlcnic_sriov_cfg_vf_def_mac(adapter, vp, vf->pci_func,
+					  0, QLCNIC_MAC_DEL);
+	if (err)
+		return err;
+
+	vp->vlan = cmd->req.arg[1] >> 16;
+	err = qlcnic_sriov_cfg_vf_def_mac(adapter, vp, vf->pci_func,
+					  vp->vlan, QLCNIC_MAC_ADD);
+
+	if (err) {
+		qlcnic_sriov_cfg_vf_def_mac(adapter, vp, vf->pci_func,
+					    0, QLCNIC_MAC_ADD);
+		vp->vlan = 0;
+	}
+
+	return err;
+}
+
+static int qlcnic_sriov_pf_cfg_guest_vlan_cmd(struct qlcnic_bc_trans *tran,
+					      struct qlcnic_cmd_args *cmd)
+{
+	struct qlcnic_vf_info  *vf = tran->vf;
+	struct qlcnic_adapter *adapter =  vf->adapter;
+	struct qlcnic_vport *vp = vf->vp;
+	int err = -EIO;
+	u8 op;
+
+	if (vp->vlan_mode != QLC_GUEST_VLAN_MODE) {
+		cmd->rsp.arg[0] |= 2 << 25;
+		return err;
+	}
+
+	op = cmd->req.arg[1] & 0xf;
+
+	if (op)
+		err = qlcnic_sriov_pf_add_guest_vlan(adapter, vf, cmd);
+	else
+		err = qlcnic_sriov_pf_del_guest_vlan(adapter, vf);
+
+	cmd->rsp.arg[0] |= err ? 2 << 25 : 1 << 25;
+	return err;
+}
+
+static const int qlcnic_pf_passthru_supp_cmds[] = {
+	QLCNIC_CMD_GET_STATISTICS,
+	QLCNIC_CMD_GET_PORT_CONFIG,
+	QLCNIC_CMD_GET_LINK_STATUS,
+};
+
+static const struct qlcnic_sriov_cmd_handler qlcnic_pf_bc_cmd_hdlr[] = {
+	[QLCNIC_BC_CMD_CHANNEL_INIT] = {&qlcnic_sriov_pf_channel_cfg_cmd},
+	[QLCNIC_BC_CMD_CHANNEL_TERM] = {&qlcnic_sriov_pf_channel_cfg_cmd},
+	[QLCNIC_BC_CMD_GET_ACL]	= {&qlcnic_sriov_pf_get_acl_cmd},
+	[QLCNIC_BC_CMD_CFG_GUEST_VLAN]	= {&qlcnic_sriov_pf_cfg_guest_vlan_cmd},
+};
+
+static const struct qlcnic_sriov_fw_cmd_handler qlcnic_pf_fw_cmd_hdlr[] = {
+	{QLCNIC_CMD_CREATE_RX_CTX, qlcnic_sriov_pf_create_rx_ctx_cmd},
+	{QLCNIC_CMD_CREATE_TX_CTX, qlcnic_sriov_pf_create_tx_ctx_cmd},
+	{QLCNIC_CMD_MAC_ADDRESS, qlcnic_sriov_pf_mac_address_cmd},
+	{QLCNIC_CMD_DESTROY_RX_CTX, qlcnic_sriov_pf_del_rx_ctx_cmd},
+	{QLCNIC_CMD_DESTROY_TX_CTX, qlcnic_sriov_pf_del_tx_ctx_cmd},
+	{QLCNIC_CMD_CONFIGURE_HW_LRO, qlcnic_sriov_pf_cfg_lro_cmd},
+	{QLCNIC_CMD_CONFIGURE_IP_ADDR, qlcnic_sriov_pf_cfg_ip_cmd},
+	{QLCNIC_CMD_CONFIG_INTRPT, qlcnic_sriov_pf_cfg_intrpt_cmd},
+	{QLCNIC_CMD_SET_MTU, qlcnic_sriov_pf_set_mtu_cmd},
+	{QLCNIC_CMD_GET_NIC_INFO, qlcnic_sriov_pf_get_nic_info_cmd},
+	{QLCNIC_CMD_CONFIGURE_RSS, qlcnic_sriov_pf_cfg_rss_cmd},
+	{QLCNIC_CMD_CONFIG_INTR_COAL, qlcnic_sriov_pf_cfg_intrcoal_cmd},
+	{QLCNIC_CMD_CONFIG_MAC_VLAN, qlcnic_sriov_pf_cfg_macvlan_cmd},
+	{QLCNIC_CMD_GET_LINK_EVENT, qlcnic_sriov_pf_linkevent_cmd},
+	{QLCNIC_CMD_CONFIGURE_MAC_RX_MODE, qlcnic_sriov_pf_cfg_promisc_cmd},
+};
+
+void qlcnic_sriov_pf_process_bc_cmd(struct qlcnic_adapter *adapter,
+				    struct qlcnic_bc_trans *trans,
+				    struct qlcnic_cmd_args *cmd)
+{
+	u8 size, cmd_op;
+
+	cmd_op = trans->req_hdr->cmd_op;
+
+	if (trans->req_hdr->op_type == QLC_BC_CMD) {
+		size = ARRAY_SIZE(qlcnic_pf_bc_cmd_hdlr);
+		if (cmd_op < size) {
+			qlcnic_pf_bc_cmd_hdlr[cmd_op].fn(trans, cmd);
+			return;
+		}
+	} else {
+		int i;
+		size = ARRAY_SIZE(qlcnic_pf_fw_cmd_hdlr);
+		for (i = 0; i < size; i++) {
+			if (cmd_op == qlcnic_pf_fw_cmd_hdlr[i].cmd) {
+				qlcnic_pf_fw_cmd_hdlr[i].fn(trans, cmd);
+				return;
+			}
+		}
+
+		size = ARRAY_SIZE(qlcnic_pf_passthru_supp_cmds);
+		for (i = 0; i < size; i++) {
+			if (cmd_op == qlcnic_pf_passthru_supp_cmds[i]) {
+				qlcnic_issue_cmd(adapter, cmd);
+				return;
+			}
+		}
+	}
+
+	cmd->rsp.arg[0] |= (0x9 << 25);
+}
+
+void qlcnic_pf_set_interface_id_create_rx_ctx(struct qlcnic_adapter *adapter,
+					     u32 *int_id)
+{
+	u16 vpid;
+
+	vpid = qlcnic_sriov_pf_get_vport_handle(adapter,
+						adapter->ahw->pci_func);
+	*int_id |= vpid;
+}
+
+void qlcnic_pf_set_interface_id_del_rx_ctx(struct qlcnic_adapter *adapter,
+					   u32 *int_id)
+{
+	u16 vpid;
+
+	vpid = qlcnic_sriov_pf_get_vport_handle(adapter,
+						adapter->ahw->pci_func);
+	*int_id |= vpid << 16;
+}
+
+void qlcnic_pf_set_interface_id_create_tx_ctx(struct qlcnic_adapter *adapter,
+					      u32 *int_id)
+{
+	int vpid;
+
+	vpid = qlcnic_sriov_pf_get_vport_handle(adapter,
+						adapter->ahw->pci_func);
+	*int_id |= vpid << 16;
+}
+
+void qlcnic_pf_set_interface_id_del_tx_ctx(struct qlcnic_adapter *adapter,
+					   u32 *int_id)
+{
+	u16 vpid;
+
+	vpid = qlcnic_sriov_pf_get_vport_handle(adapter,
+						adapter->ahw->pci_func);
+	*int_id |= vpid << 16;
+}
+
+void qlcnic_pf_set_interface_id_promisc(struct qlcnic_adapter *adapter,
+					u32 *int_id)
+{
+	u16 vpid;
+
+	vpid = qlcnic_sriov_pf_get_vport_handle(adapter,
+						adapter->ahw->pci_func);
+	*int_id |= (vpid << 16) | BIT_31;
+}
+
+void qlcnic_pf_set_interface_id_ipaddr(struct qlcnic_adapter *adapter,
+				       u32 *int_id)
+{
+	u16 vpid;
+
+	vpid = qlcnic_sriov_pf_get_vport_handle(adapter,
+						adapter->ahw->pci_func);
+	*int_id |= (vpid << 16) | BIT_31;
+}
+
+void qlcnic_pf_set_interface_id_macaddr(struct qlcnic_adapter *adapter,
+					u32 *int_id)
+{
+	u16 vpid;
+
+	vpid = qlcnic_sriov_pf_get_vport_handle(adapter,
+						adapter->ahw->pci_func);
+	*int_id |= (vpid << 16) | BIT_31;
+}
+
+static void qlcnic_sriov_del_rx_ctx(struct qlcnic_adapter *adapter,
+				    struct qlcnic_vf_info *vf)
+{
+	struct qlcnic_cmd_args cmd;
+	int vpid;
+
+	if (!vf->rx_ctx_id)
+		return;
+
+	if (qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_DESTROY_RX_CTX))
+		return;
+
+	vpid = qlcnic_sriov_pf_get_vport_handle(adapter, vf->pci_func);
+	if (vpid >= 0) {
+		cmd.req.arg[1] = vf->rx_ctx_id | (vpid & 0xffff) << 16;
+		if (qlcnic_issue_cmd(adapter, &cmd))
+			dev_err(&adapter->pdev->dev,
+				"Failed to delete Tx ctx in firmware for func 0x%x\n",
+				vf->pci_func);
+		else
+			vf->rx_ctx_id = 0;
+	}
+
+	qlcnic_free_mbx_args(&cmd);
+}
+
+static void qlcnic_sriov_del_tx_ctx(struct qlcnic_adapter *adapter,
+				    struct qlcnic_vf_info *vf)
+{
+	struct qlcnic_cmd_args cmd;
+	int vpid;
+
+	if (!vf->tx_ctx_id)
+		return;
+
+	if (qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_DESTROY_TX_CTX))
+		return;
+
+	vpid = qlcnic_sriov_pf_get_vport_handle(adapter, vf->pci_func);
+	if (vpid >= 0) {
+		cmd.req.arg[1] |= vf->tx_ctx_id | (vpid & 0xffff) << 16;
+		if (qlcnic_issue_cmd(adapter, &cmd))
+			dev_err(&adapter->pdev->dev,
+				"Failed to delete Tx ctx in firmware for func 0x%x\n",
+				vf->pci_func);
+		else
+			vf->tx_ctx_id = 0;
+	}
+
+	qlcnic_free_mbx_args(&cmd);
+}
+
+static int qlcnic_sriov_add_act_list_irqsave(struct qlcnic_sriov *sriov,
+					     struct qlcnic_vf_info *vf,
+					     struct qlcnic_bc_trans *trans)
+{
+	struct qlcnic_trans_list *t_list = &vf->rcv_act;
+	unsigned long flag;
+
+	spin_lock_irqsave(&t_list->lock, flag);
+
+	__qlcnic_sriov_add_act_list(sriov, vf, trans);
+
+	spin_unlock_irqrestore(&t_list->lock, flag);
+	return 0;
+}
+
+static void __qlcnic_sriov_process_flr(struct qlcnic_vf_info *vf)
+{
+	struct qlcnic_adapter *adapter = vf->adapter;
+
+	qlcnic_sriov_cleanup_list(&vf->rcv_pend);
+	cancel_work_sync(&vf->trans_work);
+	qlcnic_sriov_cleanup_list(&vf->rcv_act);
+
+	if (test_bit(QLC_BC_VF_SOFT_FLR, &vf->state)) {
+		qlcnic_sriov_del_tx_ctx(adapter, vf);
+		qlcnic_sriov_del_rx_ctx(adapter, vf);
+	}
+
+	qlcnic_sriov_pf_config_vport(adapter, 0, vf->pci_func);
+
+	clear_bit(QLC_BC_VF_FLR, &vf->state);
+	if (test_bit(QLC_BC_VF_SOFT_FLR, &vf->state)) {
+		qlcnic_sriov_add_act_list_irqsave(adapter->ahw->sriov, vf,
+						  vf->flr_trans);
+		clear_bit(QLC_BC_VF_SOFT_FLR, &vf->state);
+		vf->flr_trans = NULL;
+	}
+}
+
+static void qlcnic_sriov_pf_process_flr(struct work_struct *work)
+{
+	struct qlcnic_vf_info *vf;
+
+	vf = container_of(work, struct qlcnic_vf_info, flr_work);
+	__qlcnic_sriov_process_flr(vf);
+	return;
+}
+
+static void qlcnic_sriov_schedule_flr(struct qlcnic_sriov *sriov,
+				      struct qlcnic_vf_info *vf,
+				      work_func_t func)
+{
+	if (test_bit(__QLCNIC_RESETTING, &vf->adapter->state))
+		return;
+
+	INIT_WORK(&vf->flr_work, func);
+	queue_work(sriov->bc.bc_flr_wq, &vf->flr_work);
+}
+
+static void qlcnic_sriov_handle_soft_flr(struct qlcnic_adapter *adapter,
+					 struct qlcnic_bc_trans *trans,
+					 struct qlcnic_vf_info *vf)
+{
+	struct qlcnic_sriov *sriov = adapter->ahw->sriov;
+
+	set_bit(QLC_BC_VF_FLR, &vf->state);
+	clear_bit(QLC_BC_VF_STATE, &vf->state);
+	set_bit(QLC_BC_VF_SOFT_FLR, &vf->state);
+	vf->flr_trans = trans;
+	qlcnic_sriov_schedule_flr(sriov, vf, qlcnic_sriov_pf_process_flr);
+	netdev_info(adapter->netdev, "Software FLR for PCI func %d\n",
+		    vf->pci_func);
+}
+
+bool qlcnic_sriov_soft_flr_check(struct qlcnic_adapter *adapter,
+				 struct qlcnic_bc_trans *trans,
+				 struct qlcnic_vf_info *vf)
+{
+	struct qlcnic_bc_hdr *hdr = trans->req_hdr;
+
+	if ((hdr->cmd_op == QLCNIC_BC_CMD_CHANNEL_INIT) &&
+	    (hdr->op_type == QLC_BC_CMD) &&
+	     test_bit(QLC_BC_VF_STATE, &vf->state)) {
+		qlcnic_sriov_handle_soft_flr(adapter, trans, vf);
+		return true;
+	}
+
+	return false;
+}
+
+void qlcnic_sriov_pf_handle_flr(struct qlcnic_sriov *sriov,
+				struct qlcnic_vf_info *vf)
+{
+	struct net_device *dev = vf->adapter->netdev;
+
+	if (!test_and_clear_bit(QLC_BC_VF_STATE, &vf->state)) {
+		clear_bit(QLC_BC_VF_FLR, &vf->state);
+		return;
+	}
+
+	if (test_and_set_bit(QLC_BC_VF_FLR, &vf->state)) {
+		netdev_info(dev, "FLR for PCI func %d in progress\n",
+			    vf->pci_func);
+		return;
+	}
+
+	qlcnic_sriov_schedule_flr(sriov, vf, qlcnic_sriov_pf_process_flr);
+	netdev_info(dev, "FLR received for PCI func %d\n", vf->pci_func);
+}
+
+void qlcnic_sriov_pf_reset(struct qlcnic_adapter *adapter)
+{
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
+	struct qlcnic_sriov *sriov = ahw->sriov;
+	struct qlcnic_vf_info *vf;
+	u16 num_vfs = sriov->num_vfs;
+	int i;
+
+	for (i = 0; i < num_vfs; i++) {
+		vf = &sriov->vf_info[i];
+		vf->rx_ctx_id = 0;
+		vf->tx_ctx_id = 0;
+		cancel_work_sync(&vf->flr_work);
+		__qlcnic_sriov_process_flr(vf);
+		clear_bit(QLC_BC_VF_STATE, &vf->state);
+	}
+
+	qlcnic_sriov_pf_reset_vport_handle(adapter, ahw->pci_func);
+	QLCWRX(ahw, QLCNIC_MBX_INTR_ENBL, (ahw->num_msix - 1) << 8);
+}
+
+int qlcnic_sriov_pf_reinit(struct qlcnic_adapter *adapter)
+{
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
+	int err;
+
+	if (!qlcnic_sriov_enable_check(adapter))
+		return 0;
+
+	ahw->op_mode = QLCNIC_SRIOV_PF_FUNC;
+
+	err = qlcnic_sriov_pf_init(adapter);
+	if (err)
+		return err;
+
+	dev_info(&adapter->pdev->dev, "%s: op_mode %d\n",
+		 __func__, ahw->op_mode);
+	return err;
+}
+
+int qlcnic_sriov_set_vf_mac(struct net_device *netdev, int vf, u8 *mac)
+{
+	struct qlcnic_adapter *adapter = netdev_priv(netdev);
+	struct qlcnic_sriov *sriov = adapter->ahw->sriov;
+	int i, num_vfs = sriov->num_vfs;
+	struct qlcnic_vf_info *vf_info;
+	u8 *curr_mac;
+
+	if (!qlcnic_sriov_pf_check(adapter))
+		return -EOPNOTSUPP;
+
+	if (!is_valid_ether_addr(mac) || vf >= num_vfs)
+		return -EINVAL;
+
+	if (!compare_ether_addr(adapter->mac_addr, mac)) {
+		netdev_err(netdev, "MAC address is already in use by the PF\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < num_vfs; i++) {
+		vf_info = &sriov->vf_info[i];
+		if (!compare_ether_addr(vf_info->vp->mac, mac)) {
+			netdev_err(netdev,
+				   "MAC address is already in use by VF %d\n",
+				   i);
+			return -EINVAL;
+		}
+	}
+
+	vf_info = &sriov->vf_info[vf];
+	curr_mac = vf_info->vp->mac;
+
+	if (test_bit(QLC_BC_VF_STATE, &vf_info->state)) {
+		netdev_err(netdev,
+			   "MAC address change failed for VF %d, as VF driver is loaded. Please unload VF driver and retry the operation\n",
+			   vf);
+		return -EOPNOTSUPP;
+	}
+
+	memcpy(curr_mac, mac, netdev->addr_len);
+	netdev_info(netdev, "MAC Address %pM  is configured for VF %d\n",
+		    mac, vf);
+	return 0;
+}
+
+int qlcnic_sriov_set_vf_tx_rate(struct net_device *netdev, int vf, int tx_rate)
+{
+	struct qlcnic_adapter *adapter = netdev_priv(netdev);
+	struct qlcnic_sriov *sriov = adapter->ahw->sriov;
+	struct qlcnic_vf_info *vf_info;
+	struct qlcnic_info nic_info;
+	struct qlcnic_vport *vp;
+	u16 vpid;
+
+	if (!qlcnic_sriov_pf_check(adapter))
+		return -EOPNOTSUPP;
+
+	if (vf >= sriov->num_vfs)
+		return -EINVAL;
+
+	if (tx_rate >= 10000 || tx_rate < 100) {
+		netdev_err(netdev,
+			   "Invalid Tx rate, allowed range is [%d - %d]",
+			   QLC_VF_MIN_TX_RATE, QLC_VF_MAX_TX_RATE);
+		return -EINVAL;
+	}
+
+	if (tx_rate == 0)
+		tx_rate = 10000;
+
+	vf_info = &sriov->vf_info[vf];
+	vp = vf_info->vp;
+	vpid = vp->handle;
+
+	if (test_bit(QLC_BC_VF_STATE, &vf_info->state)) {
+		if (qlcnic_sriov_get_vf_vport_info(adapter, &nic_info, vpid))
+			return -EIO;
+
+		nic_info.max_tx_bw = tx_rate / 100;
+		nic_info.bit_offsets = BIT_0;
+
+		if (qlcnic_sriov_pf_set_vport_info(adapter, &nic_info, vpid))
+			return -EIO;
+	}
+
+	vp->max_tx_bw = tx_rate / 100;
+	netdev_info(netdev,
+		    "Setting Tx rate %d (Mbps), %d %% of PF bandwidth, for VF %d\n",
+		    tx_rate, vp->max_tx_bw, vf);
+	return 0;
+}
+
+int qlcnic_sriov_set_vf_vlan(struct net_device *netdev, int vf,
+			     u16 vlan, u8 qos)
+{
+	struct qlcnic_adapter *adapter = netdev_priv(netdev);
+	struct qlcnic_sriov *sriov = adapter->ahw->sriov;
+	struct qlcnic_vf_info *vf_info;
+	struct qlcnic_vport *vp;
+
+	if (!qlcnic_sriov_pf_check(adapter))
+		return -EOPNOTSUPP;
+
+	if (vf >= sriov->num_vfs || qos > 7)
+		return -EINVAL;
+
+	if (vlan > MAX_VLAN_ID) {
+		netdev_err(netdev,
+			   "Invalid VLAN ID, allowed range is [0 - %d]\n",
+			   MAX_VLAN_ID);
+		return -EINVAL;
+	}
+
+	vf_info = &sriov->vf_info[vf];
+	vp = vf_info->vp;
+	if (test_bit(QLC_BC_VF_STATE, &vf_info->state)) {
+		netdev_err(netdev,
+			   "VLAN change failed for VF %d, as VF driver is loaded. Please unload VF driver and retry the operation\n",
+			   vf);
+		return -EOPNOTSUPP;
+	}
+
+	switch (vlan) {
+	case 4095:
+		vp->vlan_mode = QLC_GUEST_VLAN_MODE;
+		break;
+	case 0:
+		vp->vlan_mode = QLC_NO_VLAN_MODE;
+		vp->vlan = 0;
+		vp->qos = 0;
+		break;
+	default:
+		vp->vlan_mode = QLC_PVID_MODE;
+		vp->vlan = vlan;
+		vp->qos = qos;
+	}
+
+	netdev_info(netdev, "Setting VLAN %d, QoS %d, for VF %d\n",
+		    vlan, qos, vf);
+	return 0;
+}
+
+int qlcnic_sriov_get_vf_config(struct net_device *netdev,
+			       int vf, struct ifla_vf_info *ivi)
+{
+	struct qlcnic_adapter *adapter = netdev_priv(netdev);
+	struct qlcnic_sriov *sriov = adapter->ahw->sriov;
+	struct qlcnic_vport *vp;
+
+	if (!qlcnic_sriov_pf_check(adapter))
+		return -EOPNOTSUPP;
+
+	if (vf >= sriov->num_vfs)
+		return -EINVAL;
+
+	vp = sriov->vf_info[vf].vp;
+	memcpy(&ivi->mac, vp->mac, ETH_ALEN);
+	ivi->vlan = vp->vlan;
+	ivi->qos = vp->qos;
+	if (vp->max_tx_bw == MAX_BW)
+		ivi->tx_rate = 0;
+	else
+		ivi->tx_rate = vp->max_tx_bw * 100;
+
+	ivi->vf = vf;
+	return 0;
+}
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c
index 5ef328af61d0..4e22e794a186 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c
@@ -21,8 +21,6 @@
 #include <linux/aer.h>
 #include <linux/log2.h>
 
-#include <linux/sysfs.h>
-
 #define QLC_STATUS_UNSUPPORTED_CMD	-2
 
 int qlcnicvf_config_bridged_mode(struct qlcnic_adapter *adapter, u32 enable)
@@ -886,6 +884,244 @@ static ssize_t qlcnic_sysfs_read_pci_config(struct file *file,
 	return size;
 }
 
+static ssize_t qlcnic_83xx_sysfs_flash_read_handler(struct file *filp,
+						    struct kobject *kobj,
+						    struct bin_attribute *attr,
+						    char *buf, loff_t offset,
+						    size_t size)
+{
+	unsigned char *p_read_buf;
+	int  ret, count;
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct qlcnic_adapter *adapter = dev_get_drvdata(dev);
+
+	if (!size)
+		return QL_STATUS_INVALID_PARAM;
+	if (!buf)
+		return QL_STATUS_INVALID_PARAM;
+
+	count = size / sizeof(u32);
+
+	if (size % sizeof(u32))
+		count++;
+
+	p_read_buf = kcalloc(size, sizeof(unsigned char), GFP_KERNEL);
+	if (!p_read_buf)
+		return -ENOMEM;
+	if (qlcnic_83xx_lock_flash(adapter) != 0) {
+		kfree(p_read_buf);
+		return -EIO;
+	}
+
+	ret = qlcnic_83xx_lockless_flash_read32(adapter, offset, p_read_buf,
+						count);
+
+	if (ret) {
+		qlcnic_83xx_unlock_flash(adapter);
+		kfree(p_read_buf);
+		return ret;
+	}
+
+	qlcnic_83xx_unlock_flash(adapter);
+	memcpy(buf, p_read_buf, size);
+	kfree(p_read_buf);
+
+	return size;
+}
+
+static int qlcnic_83xx_sysfs_flash_bulk_write(struct qlcnic_adapter *adapter,
+					      char *buf, loff_t offset,
+					      size_t size)
+{
+	int  i, ret, count;
+	unsigned char *p_cache, *p_src;
+
+	p_cache = kcalloc(size, sizeof(unsigned char), GFP_KERNEL);
+	if (!p_cache)
+		return -ENOMEM;
+
+	memcpy(p_cache, buf, size);
+	p_src = p_cache;
+	count = size / sizeof(u32);
+
+	if (qlcnic_83xx_lock_flash(adapter) != 0) {
+		kfree(p_cache);
+		return -EIO;
+	}
+
+	if (adapter->ahw->fdt.mfg_id == adapter->flash_mfg_id) {
+		ret = qlcnic_83xx_enable_flash_write(adapter);
+		if (ret) {
+			kfree(p_cache);
+			qlcnic_83xx_unlock_flash(adapter);
+			return -EIO;
+		}
+	}
+
+	for (i = 0; i < count / QLC_83XX_FLASH_WRITE_MAX; i++) {
+		ret = qlcnic_83xx_flash_bulk_write(adapter, offset,
+						   (u32 *)p_src,
+						   QLC_83XX_FLASH_WRITE_MAX);
+
+		if (ret) {
+			if (adapter->ahw->fdt.mfg_id == adapter->flash_mfg_id) {
+				ret = qlcnic_83xx_disable_flash_write(adapter);
+				if (ret) {
+					kfree(p_cache);
+					qlcnic_83xx_unlock_flash(adapter);
+					return -EIO;
+				}
+			}
+
+			kfree(p_cache);
+			qlcnic_83xx_unlock_flash(adapter);
+			return -EIO;
+		}
+
+		p_src = p_src + sizeof(u32)*QLC_83XX_FLASH_WRITE_MAX;
+		offset = offset + sizeof(u32)*QLC_83XX_FLASH_WRITE_MAX;
+	}
+
+	if (adapter->ahw->fdt.mfg_id == adapter->flash_mfg_id) {
+		ret = qlcnic_83xx_disable_flash_write(adapter);
+		if (ret) {
+			kfree(p_cache);
+			qlcnic_83xx_unlock_flash(adapter);
+			return -EIO;
+		}
+	}
+
+	kfree(p_cache);
+	qlcnic_83xx_unlock_flash(adapter);
+
+	return 0;
+}
+
+static int qlcnic_83xx_sysfs_flash_write(struct qlcnic_adapter *adapter,
+					 char *buf, loff_t offset, size_t size)
+{
+	int  i, ret, count;
+	unsigned char *p_cache, *p_src;
+
+	p_cache = kcalloc(size, sizeof(unsigned char), GFP_KERNEL);
+	if (!p_cache)
+		return -ENOMEM;
+
+	memcpy(p_cache, buf, size);
+	p_src = p_cache;
+	count = size / sizeof(u32);
+
+	if (qlcnic_83xx_lock_flash(adapter) != 0) {
+		kfree(p_cache);
+		return -EIO;
+	}
+
+	if (adapter->ahw->fdt.mfg_id == adapter->flash_mfg_id) {
+		ret = qlcnic_83xx_enable_flash_write(adapter);
+		if (ret) {
+			kfree(p_cache);
+			qlcnic_83xx_unlock_flash(adapter);
+			return -EIO;
+		}
+	}
+
+	for (i = 0; i < count; i++) {
+		ret = qlcnic_83xx_flash_write32(adapter, offset, (u32 *)p_src);
+		if (ret) {
+			if (adapter->ahw->fdt.mfg_id == adapter->flash_mfg_id) {
+				ret = qlcnic_83xx_disable_flash_write(adapter);
+				if (ret) {
+					kfree(p_cache);
+					qlcnic_83xx_unlock_flash(adapter);
+					return -EIO;
+				}
+			}
+			kfree(p_cache);
+			qlcnic_83xx_unlock_flash(adapter);
+			return -EIO;
+		}
+
+		p_src = p_src + sizeof(u32);
+		offset = offset + sizeof(u32);
+	}
+
+	if (adapter->ahw->fdt.mfg_id == adapter->flash_mfg_id) {
+		ret = qlcnic_83xx_disable_flash_write(adapter);
+		if (ret) {
+			kfree(p_cache);
+			qlcnic_83xx_unlock_flash(adapter);
+			return -EIO;
+		}
+	}
+
+	kfree(p_cache);
+	qlcnic_83xx_unlock_flash(adapter);
+
+	return 0;
+}
+
+static ssize_t qlcnic_83xx_sysfs_flash_write_handler(struct file *filp,
+						     struct kobject *kobj,
+						     struct bin_attribute *attr,
+						     char *buf, loff_t offset,
+						     size_t size)
+{
+	int  ret;
+	static int flash_mode;
+	unsigned long data;
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct qlcnic_adapter *adapter = dev_get_drvdata(dev);
+
+	if (!buf)
+		return QL_STATUS_INVALID_PARAM;
+
+	ret = kstrtoul(buf, 16, &data);
+
+	switch (data) {
+	case QLC_83XX_FLASH_SECTOR_ERASE_CMD:
+		flash_mode = QLC_83XX_ERASE_MODE;
+		ret = qlcnic_83xx_erase_flash_sector(adapter, offset);
+		if (ret) {
+			dev_err(&adapter->pdev->dev,
+				"%s failed at %d\n", __func__, __LINE__);
+			return -EIO;
+		}
+		break;
+
+	case QLC_83XX_FLASH_BULK_WRITE_CMD:
+		flash_mode = QLC_83XX_BULK_WRITE_MODE;
+		break;
+
+	case QLC_83XX_FLASH_WRITE_CMD:
+		flash_mode = QLC_83XX_WRITE_MODE;
+		break;
+	default:
+		if (flash_mode == QLC_83XX_BULK_WRITE_MODE) {
+			ret = qlcnic_83xx_sysfs_flash_bulk_write(adapter, buf,
+								 offset, size);
+			if (ret) {
+				dev_err(&adapter->pdev->dev,
+					"%s failed at %d\n",
+					__func__, __LINE__);
+				return -EIO;
+			}
+		}
+
+		if (flash_mode == QLC_83XX_WRITE_MODE) {
+			ret = qlcnic_83xx_sysfs_flash_write(adapter, buf,
+							    offset, size);
+			if (ret) {
+				dev_err(&adapter->pdev->dev,
+					"%s failed at %d\n", __func__,
+					__LINE__);
+				return -EIO;
+			}
+		}
+	}
+
+	return size;
+}
+
 static struct device_attribute dev_attr_bridged_mode = {
        .attr = {.name = "bridged_mode", .mode = (S_IRUGO | S_IWUSR)},
        .show = qlcnic_show_bridged_mode,
@@ -960,6 +1196,13 @@ static struct bin_attribute bin_attr_pm_config = {
 	.write = qlcnic_sysfs_write_pm_config,
 };
 
+static struct bin_attribute bin_attr_flash = {
+	.attr = {.name = "flash", .mode = (S_IRUGO | S_IWUSR)},
+	.size = 0,
+	.read = qlcnic_83xx_sysfs_flash_read_handler,
+	.write = qlcnic_83xx_sysfs_flash_write_handler,
+};
+
 void qlcnic_create_sysfs_entries(struct qlcnic_adapter *adapter)
 {
 	struct device *dev = &adapter->pdev->dev;
@@ -1048,10 +1291,18 @@ void qlcnic_82xx_remove_sysfs(struct qlcnic_adapter *adapter)
 
 void qlcnic_83xx_add_sysfs(struct qlcnic_adapter *adapter)
 {
+	struct device *dev = &adapter->pdev->dev;
+
 	qlcnic_create_diag_entries(adapter);
+
+	if (sysfs_create_bin_file(&dev->kobj, &bin_attr_flash))
+		dev_info(dev, "failed to create flash sysfs entry\n");
 }
 
 void qlcnic_83xx_remove_sysfs(struct qlcnic_adapter *adapter)
 {
+	struct device *dev = &adapter->pdev->dev;
+
 	qlcnic_remove_diag_entries(adapter);
+	sysfs_remove_bin_file(&dev->kobj, &bin_attr_flash);
 }
diff --git a/drivers/net/ethernet/qlogic/qlge/qlge_main.c b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
index 8033555e53c2..87463bc701a6 100644
--- a/drivers/net/ethernet/qlogic/qlge/qlge_main.c
+++ b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
@@ -409,7 +409,7 @@ static int ql_set_mac_addr_reg(struct ql_adapter *qdev, u8 *addr, u32 type,
 				      (qdev->
 				       func << CAM_OUT_FUNC_SHIFT) |
 					(0 << CAM_OUT_CQ_ID_SHIFT));
-			if (qdev->ndev->features & NETIF_F_HW_VLAN_RX)
+			if (qdev->ndev->features & NETIF_F_HW_VLAN_CTAG_RX)
 				cam_output |= CAM_OUT_RV;
 			/* route to NIC core */
 			ql_write32(qdev, MAC_ADDR_DATA, cam_output);
@@ -1211,8 +1211,6 @@ static void ql_update_sbq(struct ql_adapter *qdev, struct rx_ring *rx_ring)
 				    netdev_alloc_skb(qdev->ndev,
 						     SMALL_BUFFER_SIZE);
 				if (sbq_desc->p.skb == NULL) {
-					netif_err(qdev, probe, qdev->ndev,
-						  "Couldn't get an skb.\n");
 					rx_ring->sbq_clean_idx = clean_idx;
 					return;
 				}
@@ -1508,7 +1506,7 @@ static void ql_process_mac_rx_gro_page(struct ql_adapter *qdev,
 	skb->ip_summed = CHECKSUM_UNNECESSARY;
 	skb_record_rx_queue(skb, rx_ring->cq_id);
 	if (vlan_id != 0xffff)
-		__vlan_hwaccel_put_tag(skb, vlan_id);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_id);
 	napi_gro_frags(napi);
 }
 
@@ -1527,8 +1525,6 @@ static void ql_process_mac_rx_page(struct ql_adapter *qdev,
 
 	skb = netdev_alloc_skb(ndev, length);
 	if (!skb) {
-		netif_err(qdev, drv, qdev->ndev,
-			  "Couldn't get an skb, need to unwind!.\n");
 		rx_ring->rx_dropped++;
 		put_page(lbq_desc->p.pg_chunk.page);
 		return;
@@ -1592,7 +1588,7 @@ static void ql_process_mac_rx_page(struct ql_adapter *qdev,
 
 	skb_record_rx_queue(skb, rx_ring->cq_id);
 	if (vlan_id != 0xffff)
-		__vlan_hwaccel_put_tag(skb, vlan_id);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_id);
 	if (skb->ip_summed == CHECKSUM_UNNECESSARY)
 		napi_gro_receive(napi, skb);
 	else
@@ -1619,8 +1615,6 @@ static void ql_process_mac_rx_skb(struct ql_adapter *qdev,
 	/* Allocate new_skb and copy */
 	new_skb = netdev_alloc_skb(qdev->ndev, length + NET_IP_ALIGN);
 	if (new_skb == NULL) {
-		netif_err(qdev, probe, qdev->ndev,
-			  "No skb available, drop the packet.\n");
 		rx_ring->rx_dropped++;
 		return;
 	}
@@ -1697,7 +1691,7 @@ static void ql_process_mac_rx_skb(struct ql_adapter *qdev,
 
 	skb_record_rx_queue(skb, rx_ring->cq_id);
 	if (vlan_id != 0xffff)
-		__vlan_hwaccel_put_tag(skb, vlan_id);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_id);
 	if (skb->ip_summed == CHECKSUM_UNNECESSARY)
 		napi_gro_receive(&rx_ring->napi, skb);
 	else
@@ -2009,7 +2003,7 @@ static void ql_process_mac_split_rx_intr(struct ql_adapter *qdev,
 	rx_ring->rx_bytes += skb->len;
 	skb_record_rx_queue(skb, rx_ring->cq_id);
 	if ((ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_V) && (vlan_id != 0))
-		__vlan_hwaccel_put_tag(skb, vlan_id);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_id);
 	if (skb->ip_summed == CHECKSUM_UNNECESSARY)
 		napi_gro_receive(&rx_ring->napi, skb);
 	else
@@ -2307,7 +2301,7 @@ static void qlge_vlan_mode(struct net_device *ndev, netdev_features_t features)
 {
 	struct ql_adapter *qdev = netdev_priv(ndev);
 
-	if (features & NETIF_F_HW_VLAN_RX) {
+	if (features & NETIF_F_HW_VLAN_CTAG_RX) {
 		ql_write32(qdev, NIC_RCV_CFG, NIC_RCV_CFG_VLAN_MASK |
 				 NIC_RCV_CFG_VLAN_MATCH_AND_NON);
 	} else {
@@ -2322,10 +2316,10 @@ static netdev_features_t qlge_fix_features(struct net_device *ndev,
 	 * Since there is no support for separate rx/tx vlan accel
 	 * enable/disable make sure tx flag is always in same state as rx.
 	 */
-	if (features & NETIF_F_HW_VLAN_RX)
-		features |= NETIF_F_HW_VLAN_TX;
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
+		features |= NETIF_F_HW_VLAN_CTAG_TX;
 	else
-		features &= ~NETIF_F_HW_VLAN_TX;
+		features &= ~NETIF_F_HW_VLAN_CTAG_TX;
 
 	return features;
 }
@@ -2335,7 +2329,7 @@ static int qlge_set_features(struct net_device *ndev,
 {
 	netdev_features_t changed = ndev->features ^ features;
 
-	if (changed & NETIF_F_HW_VLAN_RX)
+	if (changed & NETIF_F_HW_VLAN_CTAG_RX)
 		qlge_vlan_mode(ndev, features);
 
 	return 0;
@@ -2354,7 +2348,7 @@ static int __qlge_vlan_rx_add_vid(struct ql_adapter *qdev, u16 vid)
 	return err;
 }
 
-static int qlge_vlan_rx_add_vid(struct net_device *ndev, u16 vid)
+static int qlge_vlan_rx_add_vid(struct net_device *ndev, __be16 proto, u16 vid)
 {
 	struct ql_adapter *qdev = netdev_priv(ndev);
 	int status;
@@ -2385,7 +2379,7 @@ static int __qlge_vlan_rx_kill_vid(struct ql_adapter *qdev, u16 vid)
 	return err;
 }
 
-static int qlge_vlan_rx_kill_vid(struct net_device *ndev, u16 vid)
+static int qlge_vlan_rx_kill_vid(struct net_device *ndev, __be16 proto, u16 vid)
 {
 	struct ql_adapter *qdev = netdev_priv(ndev);
 	int status;
@@ -4693,9 +4687,9 @@ static int qlge_probe(struct pci_dev *pdev,
 	SET_NETDEV_DEV(ndev, &pdev->dev);
 	ndev->hw_features = NETIF_F_SG | NETIF_F_IP_CSUM |
 		NETIF_F_TSO | NETIF_F_TSO_ECN |
-		NETIF_F_HW_VLAN_TX | NETIF_F_RXCSUM;
+		NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_RXCSUM;
 	ndev->features = ndev->hw_features |
-		NETIF_F_HW_VLAN_RX | NETIF_F_HW_VLAN_FILTER;
+		NETIF_F_HW_VLAN_CTAG_RX | NETIF_F_HW_VLAN_CTAG_FILTER;
 	ndev->vlan_features = ndev->hw_features;
 
 	if (test_bit(QL_DMA64, &qdev->flags))
diff --git a/drivers/net/ethernet/rdc/r6040.c b/drivers/net/ethernet/rdc/r6040.c
index 5b4103db70f5..e9dc84943cfc 100644
--- a/drivers/net/ethernet/rdc/r6040.c
+++ b/drivers/net/ethernet/rdc/r6040.c
@@ -224,11 +224,14 @@ static int r6040_phy_read(void __iomem *ioaddr, int phy_addr, int reg)
 			break;
 	}
 
+	if (limit < 0)
+		return -ETIMEDOUT;
+
 	return ioread16(ioaddr + MMRD);
 }
 
 /* Write a word data from PHY Chip */
-static void r6040_phy_write(void __iomem *ioaddr,
+static int r6040_phy_write(void __iomem *ioaddr,
 					int phy_addr, int reg, u16 val)
 {
 	int limit = MAC_DEF_TIMEOUT;
@@ -243,6 +246,8 @@ static void r6040_phy_write(void __iomem *ioaddr,
 		if (!(cmd & MDIO_WRITE))
 			break;
 	}
+
+	return (limit < 0) ? -ETIMEDOUT : 0;
 }
 
 static int r6040_mdiobus_read(struct mii_bus *bus, int phy_addr, int reg)
@@ -261,9 +266,7 @@ static int r6040_mdiobus_write(struct mii_bus *bus, int phy_addr,
 	struct r6040_private *lp = netdev_priv(dev);
 	void __iomem *ioaddr = lp->base;
 
-	r6040_phy_write(ioaddr, phy_addr, reg, value);
-
-	return 0;
+	return r6040_phy_write(ioaddr, phy_addr, reg, value);
 }
 
 static int r6040_mdiobus_reset(struct mii_bus *bus)
@@ -347,7 +350,6 @@ static int r6040_alloc_rxbufs(struct net_device *dev)
 	do {
 		skb = netdev_alloc_skb(dev, MAX_BUF_SIZE);
 		if (!skb) {
-			netdev_err(dev, "failed to alloc skb for rx\n");
 			rc = -ENOMEM;
 			goto err_exit;
 		}
diff --git a/drivers/net/ethernet/realtek/8139cp.c b/drivers/net/ethernet/realtek/8139cp.c
index b62a32484f6a..7d1fb9ad1296 100644
--- a/drivers/net/ethernet/realtek/8139cp.c
+++ b/drivers/net/ethernet/realtek/8139cp.c
@@ -431,7 +431,7 @@ static inline void cp_rx_skb (struct cp_private *cp, struct sk_buff *skb,
 	cp->dev->stats.rx_bytes += skb->len;
 
 	if (opts2 & RxVlanTagged)
-		__vlan_hwaccel_put_tag(skb, swab16(opts2 & 0xffff));
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), swab16(opts2 & 0xffff));
 
 	napi_gro_receive(&cp->napi, skb);
 }
@@ -1438,7 +1438,7 @@ static int cp_set_features(struct net_device *dev, netdev_features_t features)
 	else
 		cp->cpcmd &= ~RxChkSum;
 
-	if (features & NETIF_F_HW_VLAN_RX)
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
 		cp->cpcmd |= RxVlanOn;
 	else
 		cp->cpcmd &= ~RxVlanOn;
@@ -1955,14 +1955,14 @@ static int cp_init_one (struct pci_dev *pdev, const struct pci_device_id *ent)
 	dev->ethtool_ops = &cp_ethtool_ops;
 	dev->watchdog_timeo = TX_TIMEOUT;
 
-	dev->features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
+	dev->features |= NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX;
 
 	if (pci_using_dac)
 		dev->features |= NETIF_F_HIGHDMA;
 
 	/* disabled by default until verified */
 	dev->hw_features |= NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_TSO |
-		NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
+		NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX;
 	dev->vlan_features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_TSO |
 		NETIF_F_HIGHDMA;
 
diff --git a/drivers/net/ethernet/realtek/8139too.c b/drivers/net/ethernet/realtek/8139too.c
index 1276ac71353a..3ccedeb8aba0 100644
--- a/drivers/net/ethernet/realtek/8139too.c
+++ b/drivers/net/ethernet/realtek/8139too.c
@@ -2041,8 +2041,6 @@ keep_pkt:
 
 			netif_receive_skb (skb);
 		} else {
-			if (net_ratelimit())
-				netdev_warn(dev, "Memory squeeze, dropping packet\n");
 			dev->stats.rx_dropped++;
 		}
 		received++;
diff --git a/drivers/net/ethernet/realtek/atp.c b/drivers/net/ethernet/realtek/atp.c
index 9f2d416de750..d77d60ea8202 100644
--- a/drivers/net/ethernet/realtek/atp.c
+++ b/drivers/net/ethernet/realtek/atp.c
@@ -782,8 +782,6 @@ static void net_rx(struct net_device *dev)
 
 		skb = netdev_alloc_skb(dev, pkt_len + 2);
 		if (skb == NULL) {
-			printk(KERN_ERR "%s: Memory squeeze, dropping packet.\n",
-				   dev->name);
 			dev->stats.rx_dropped++;
 			goto done;
 		}
diff --git a/drivers/net/ethernet/realtek/r8169.c b/drivers/net/ethernet/realtek/r8169.c
index 4ecbe64a758d..79c520b64fdd 100644
--- a/drivers/net/ethernet/realtek/r8169.c
+++ b/drivers/net/ethernet/realtek/r8169.c
@@ -47,7 +47,9 @@
 #define FIRMWARE_8402_1		"rtl_nic/rtl8402-1.fw"
 #define FIRMWARE_8411_1		"rtl_nic/rtl8411-1.fw"
 #define FIRMWARE_8106E_1	"rtl_nic/rtl8106e-1.fw"
-#define FIRMWARE_8168G_1	"rtl_nic/rtl8168g-1.fw"
+#define FIRMWARE_8106E_2	"rtl_nic/rtl8106e-2.fw"
+#define FIRMWARE_8168G_2	"rtl_nic/rtl8168g-2.fw"
+#define FIRMWARE_8168G_3	"rtl_nic/rtl8168g-3.fw"
 
 #ifdef RTL8169_DEBUG
 #define assert(expr) \
@@ -140,6 +142,8 @@ enum mac_version {
 	RTL_GIGA_MAC_VER_39,
 	RTL_GIGA_MAC_VER_40,
 	RTL_GIGA_MAC_VER_41,
+	RTL_GIGA_MAC_VER_42,
+	RTL_GIGA_MAC_VER_43,
 	RTL_GIGA_MAC_NONE   = 0xff,
 };
 
@@ -262,10 +266,16 @@ static const struct {
 		_R("RTL8106e",		RTL_TD_1, FIRMWARE_8106E_1,
 							JUMBO_1K, true),
 	[RTL_GIGA_MAC_VER_40] =
-		_R("RTL8168g/8111g",	RTL_TD_1, FIRMWARE_8168G_1,
+		_R("RTL8168g/8111g",	RTL_TD_1, FIRMWARE_8168G_2,
 							JUMBO_9K, false),
 	[RTL_GIGA_MAC_VER_41] =
 		_R("RTL8168g/8111g",	RTL_TD_1, NULL, JUMBO_9K, false),
+	[RTL_GIGA_MAC_VER_42] =
+		_R("RTL8168g/8111g",	RTL_TD_1, FIRMWARE_8168G_3,
+							JUMBO_9K, false),
+	[RTL_GIGA_MAC_VER_43] =
+		_R("RTL8106e",		RTL_TD_1, FIRMWARE_8106E_2,
+							JUMBO_1K, true),
 };
 #undef _R
 
@@ -329,6 +339,7 @@ enum rtl_registers {
 #define	RXCFG_FIFO_SHIFT		13
 					/* No threshold before first PCI xfer */
 #define	RX_FIFO_THRESH			(7 << RXCFG_FIFO_SHIFT)
+#define	RX_EARLY_OFF			(1 << 11)
 #define	RXCFG_DMA_SHIFT			8
 					/* Unlimited maximum PCI burst. */
 #define	RX_DMA_BURST			(7 << RXCFG_DMA_SHIFT)
@@ -513,6 +524,7 @@ enum rtl_register_content {
 	PMEnable	= (1 << 0),	/* Power Management Enable */
 
 	/* Config2 register p. 25 */
+	ClkReqEn	= (1 << 7),	/* Clock Request Enable */
 	MSIEnable	= (1 << 5),	/* 8169 only. Reserved in the 8168. */
 	PCI_Clock_66MHz = 0x01,
 	PCI_Clock_33MHz = 0x00,
@@ -533,6 +545,7 @@ enum rtl_register_content {
 	Spi_en		= (1 << 3),
 	LanWake		= (1 << 1),	/* LanWake enable/disable */
 	PMEStatus	= (1 << 0),	/* PME status can be reset by PCI RST# */
+	ASPM_en		= (1 << 0),	/* ASPM enable */
 
 	/* TBICSR p.28 */
 	TBIReset	= 0x80000000,
@@ -814,7 +827,9 @@ MODULE_FIRMWARE(FIRMWARE_8168F_2);
 MODULE_FIRMWARE(FIRMWARE_8402_1);
 MODULE_FIRMWARE(FIRMWARE_8411_1);
 MODULE_FIRMWARE(FIRMWARE_8106E_1);
-MODULE_FIRMWARE(FIRMWARE_8168G_1);
+MODULE_FIRMWARE(FIRMWARE_8106E_2);
+MODULE_FIRMWARE(FIRMWARE_8168G_2);
+MODULE_FIRMWARE(FIRMWARE_8168G_3);
 
 static void rtl_lock_work(struct rtl8169_private *tp)
 {
@@ -1024,14 +1039,6 @@ static u16 r8168_phy_ocp_read(struct rtl8169_private *tp, u32 reg)
 		(RTL_R32(GPHY_OCP) & 0xffff) : ~0;
 }
 
-static void rtl_w1w0_phy_ocp(struct rtl8169_private *tp, int reg, int p, int m)
-{
-	int val;
-
-	val = r8168_phy_ocp_read(tp, reg);
-	r8168_phy_ocp_write(tp, reg, (val | p) & ~m);
-}
-
 static void r8168_mac_ocp_write(struct rtl8169_private *tp, u32 reg, u32 data)
 {
 	void __iomem *ioaddr = tp->mmio_addr;
@@ -1077,6 +1084,21 @@ static int r8168g_mdio_read(struct rtl8169_private *tp, int reg)
 	return r8168_phy_ocp_read(tp, tp->ocp_base + reg * 2);
 }
 
+static void mac_mcu_write(struct rtl8169_private *tp, int reg, int value)
+{
+	if (reg == 0x1f) {
+		tp->ocp_base = value << 4;
+		return;
+	}
+
+	r8168_mac_ocp_write(tp, tp->ocp_base + reg, value);
+}
+
+static int mac_mcu_read(struct rtl8169_private *tp, int reg)
+{
+	return r8168_mac_ocp_read(tp, tp->ocp_base + reg);
+}
+
 DECLARE_RTL_COND(rtl_phyar_cond)
 {
 	void __iomem *ioaddr = tp->mmio_addr;
@@ -1771,16 +1793,17 @@ static void __rtl8169_set_features(struct net_device *dev,
 	netdev_features_t changed = features ^ dev->features;
 	void __iomem *ioaddr = tp->mmio_addr;
 
-	if (!(changed & (NETIF_F_RXALL | NETIF_F_RXCSUM | NETIF_F_HW_VLAN_RX)))
+	if (!(changed & (NETIF_F_RXALL | NETIF_F_RXCSUM |
+			 NETIF_F_HW_VLAN_CTAG_RX)))
 		return;
 
-	if (changed & (NETIF_F_RXCSUM | NETIF_F_HW_VLAN_RX)) {
+	if (changed & (NETIF_F_RXCSUM | NETIF_F_HW_VLAN_CTAG_RX)) {
 		if (features & NETIF_F_RXCSUM)
 			tp->cp_cmd |= RxChkSum;
 		else
 			tp->cp_cmd &= ~RxChkSum;
 
-		if (dev->features & NETIF_F_HW_VLAN_RX)
+		if (dev->features & NETIF_F_HW_VLAN_CTAG_RX)
 			tp->cp_cmd |= RxVlan;
 		else
 			tp->cp_cmd &= ~RxVlan;
@@ -1820,7 +1843,7 @@ static void rtl8169_rx_vlan_tag(struct RxDesc *desc, struct sk_buff *skb)
 	u32 opts2 = le32_to_cpu(desc->opts2);
 
 	if (opts2 & RxVlanTag)
-		__vlan_hwaccel_put_tag(skb, swab16(opts2 & 0xffff));
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), swab16(opts2 & 0xffff));
 }
 
 static int rtl8169_gset_tbi(struct net_device *dev, struct ethtool_cmd *cmd)
@@ -2028,6 +2051,7 @@ static void rtl8169_get_mac_version(struct rtl8169_private *tp,
 		int mac_version;
 	} mac_info[] = {
 		/* 8168G family. */
+		{ 0x7cf00000, 0x50900000,	RTL_GIGA_MAC_VER_42 },
 		{ 0x7cf00000, 0x4c100000,	RTL_GIGA_MAC_VER_41 },
 		{ 0x7cf00000, 0x4c000000,	RTL_GIGA_MAC_VER_40 },
 
@@ -2116,6 +2140,10 @@ static void rtl8169_get_mac_version(struct rtl8169_private *tp,
 		netif_notice(tp, probe, dev,
 			     "unknown MAC, using family default\n");
 		tp->mac_version = default_version;
+	} else if (tp->mac_version == RTL_GIGA_MAC_VER_42) {
+		tp->mac_version = tp->mii.supports_gmii ?
+				  RTL_GIGA_MAC_VER_42 :
+				  RTL_GIGA_MAC_VER_43;
 	}
 }
 
@@ -2142,9 +2170,7 @@ static void rtl_writephy_batch(struct rtl8169_private *tp,
 #define PHY_DATA_OR		0x10000000
 #define PHY_DATA_AND		0x20000000
 #define PHY_BJMPN		0x30000000
-#define PHY_READ_EFUSE		0x40000000
-#define PHY_READ_MAC_BYTE	0x50000000
-#define PHY_WRITE_MAC_BYTE	0x60000000
+#define PHY_MDIO_CHG		0x40000000
 #define PHY_CLEAR_READCOUNT	0x70000000
 #define PHY_WRITE		0x80000000
 #define PHY_READCOUNT_EQ_SKIP	0x90000000
@@ -2153,7 +2179,6 @@ static void rtl_writephy_batch(struct rtl8169_private *tp,
 #define PHY_WRITE_PREVIOUS	0xc0000000
 #define PHY_SKIPN		0xd0000000
 #define PHY_DELAY_MS		0xe0000000
-#define PHY_WRITE_ERI_WORD	0xf0000000
 
 struct fw_info {
 	u32	magic;
@@ -2230,7 +2255,7 @@ static bool rtl_fw_data_ok(struct rtl8169_private *tp, struct net_device *dev,
 		case PHY_READ:
 		case PHY_DATA_OR:
 		case PHY_DATA_AND:
-		case PHY_READ_EFUSE:
+		case PHY_MDIO_CHG:
 		case PHY_CLEAR_READCOUNT:
 		case PHY_WRITE:
 		case PHY_WRITE_PREVIOUS:
@@ -2261,9 +2286,6 @@ static bool rtl_fw_data_ok(struct rtl8169_private *tp, struct net_device *dev,
 			}
 			break;
 
-		case PHY_READ_MAC_BYTE:
-		case PHY_WRITE_MAC_BYTE:
-		case PHY_WRITE_ERI_WORD:
 		default:
 			netif_err(tp, ifup, tp->dev,
 				  "Invalid action 0x%08x\n", action);
@@ -2294,10 +2316,13 @@ out:
 static void rtl_phy_write_fw(struct rtl8169_private *tp, struct rtl_fw *rtl_fw)
 {
 	struct rtl_fw_phy_action *pa = &rtl_fw->phy_action;
+	struct mdio_ops org, *ops = &tp->mdio_ops;
 	u32 predata, count;
 	size_t index;
 
 	predata = count = 0;
+	org.write = ops->write;
+	org.read = ops->read;
 
 	for (index = 0; index < pa->size; ) {
 		u32 action = le32_to_cpu(pa->code[index]);
@@ -2324,8 +2349,15 @@ static void rtl_phy_write_fw(struct rtl8169_private *tp, struct rtl_fw *rtl_fw)
 		case PHY_BJMPN:
 			index -= regno;
 			break;
-		case PHY_READ_EFUSE:
-			predata = rtl8168d_efuse_read(tp, regno);
+		case PHY_MDIO_CHG:
+			if (data == 0) {
+				ops->write = org.write;
+				ops->read = org.read;
+			} else if (data == 1) {
+				ops->write = mac_mcu_write;
+				ops->read = mac_mcu_read;
+			}
+
 			index++;
 			break;
 		case PHY_CLEAR_READCOUNT:
@@ -2361,13 +2393,13 @@ static void rtl_phy_write_fw(struct rtl8169_private *tp, struct rtl_fw *rtl_fw)
 			index++;
 			break;
 
-		case PHY_READ_MAC_BYTE:
-		case PHY_WRITE_MAC_BYTE:
-		case PHY_WRITE_ERI_WORD:
 		default:
 			BUG();
 		}
 	}
+
+	ops->write = org.write;
+	ops->read = org.read;
 }
 
 static void rtl_release_firmware(struct rtl8169_private *tp)
@@ -3368,51 +3400,68 @@ static void rtl8411_hw_phy_config(struct rtl8169_private *tp)
 
 static void rtl8168g_1_hw_phy_config(struct rtl8169_private *tp)
 {
-	static const u16 mac_ocp_patch[] = {
-		0xe008, 0xe01b, 0xe01d, 0xe01f,
-		0xe021, 0xe023, 0xe025, 0xe027,
-		0x49d2, 0xf10d, 0x766c, 0x49e2,
-		0xf00a, 0x1ec0, 0x8ee1, 0xc60a,
-
-		0x77c0, 0x4870, 0x9fc0, 0x1ea0,
-		0xc707, 0x8ee1, 0x9d6c, 0xc603,
-		0xbe00, 0xb416, 0x0076, 0xe86c,
-		0xc602, 0xbe00, 0x0000, 0xc602,
-
-		0xbe00, 0x0000, 0xc602, 0xbe00,
-		0x0000, 0xc602, 0xbe00, 0x0000,
-		0xc602, 0xbe00, 0x0000, 0xc602,
-		0xbe00, 0x0000, 0xc602, 0xbe00,
-
-		0x0000, 0x0000, 0x0000, 0x0000
-	};
-	u32 i;
+	rtl_apply_firmware(tp);
 
-	/* Patch code for GPHY reset */
-	for (i = 0; i < ARRAY_SIZE(mac_ocp_patch); i++)
-		r8168_mac_ocp_write(tp, 0xf800 + 2*i, mac_ocp_patch[i]);
-	r8168_mac_ocp_write(tp, 0xfc26, 0x8000);
-	r8168_mac_ocp_write(tp, 0xfc28, 0x0075);
+	rtl_writephy(tp, 0x1f, 0x0a46);
+	if (rtl_readphy(tp, 0x10) & 0x0100) {
+		rtl_writephy(tp, 0x1f, 0x0bcc);
+		rtl_w1w0_phy(tp, 0x12, 0x0000, 0x8000);
+	} else {
+		rtl_writephy(tp, 0x1f, 0x0bcc);
+		rtl_w1w0_phy(tp, 0x12, 0x8000, 0x0000);
+	}
 
-	rtl_apply_firmware(tp);
+	rtl_writephy(tp, 0x1f, 0x0a46);
+	if (rtl_readphy(tp, 0x13) & 0x0100) {
+		rtl_writephy(tp, 0x1f, 0x0c41);
+		rtl_w1w0_phy(tp, 0x15, 0x0002, 0x0000);
+	} else {
+		rtl_writephy(tp, 0x1f, 0x0c41);
+		rtl_w1w0_phy(tp, 0x15, 0x0000, 0x0002);
+	}
 
-	if (r8168_phy_ocp_read(tp, 0xa460) & 0x0100)
-		rtl_w1w0_phy_ocp(tp, 0xbcc4, 0x0000, 0x8000);
-	else
-		rtl_w1w0_phy_ocp(tp, 0xbcc4, 0x8000, 0x0000);
+	/* Enable PHY auto speed down */
+	rtl_writephy(tp, 0x1f, 0x0a44);
+	rtl_w1w0_phy(tp, 0x11, 0x000c, 0x0000);
+
+	rtl_writephy(tp, 0x1f, 0x0bcc);
+	rtl_w1w0_phy(tp, 0x14, 0x0100, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0a44);
+	rtl_w1w0_phy(tp, 0x11, 0x00c0, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x8084);
+	rtl_w1w0_phy(tp, 0x14, 0x0000, 0x6000);
+	rtl_w1w0_phy(tp, 0x10, 0x1003, 0x0000);
+
+	/* EEE auto-fallback function */
+	rtl_writephy(tp, 0x1f, 0x0a4b);
+	rtl_w1w0_phy(tp, 0x11, 0x0004, 0x0000);
+
+	/* Enable UC LPF tune function */
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x8012);
+	rtl_w1w0_phy(tp, 0x14, 0x8000, 0x0000);
 
-	if (r8168_phy_ocp_read(tp, 0xa466) & 0x0100)
-		rtl_w1w0_phy_ocp(tp, 0xc41a, 0x0002, 0x0000);
-	else
-		rtl_w1w0_phy_ocp(tp, 0xbcc4, 0x0000, 0x0002);
+	rtl_writephy(tp, 0x1f, 0x0c42);
+	rtl_w1w0_phy(tp, 0x11, 0x4000, 0x2000);
 
-	rtl_w1w0_phy_ocp(tp, 0xa442, 0x000c, 0x0000);
-	rtl_w1w0_phy_ocp(tp, 0xa4b2, 0x0004, 0x0000);
+	/* Improve SWR Efficiency */
+	rtl_writephy(tp, 0x1f, 0x0bcd);
+	rtl_writephy(tp, 0x14, 0x5065);
+	rtl_writephy(tp, 0x14, 0xd065);
+	rtl_writephy(tp, 0x1f, 0x0bc8);
+	rtl_writephy(tp, 0x11, 0x5655);
+	rtl_writephy(tp, 0x1f, 0x0bcd);
+	rtl_writephy(tp, 0x14, 0x1065);
+	rtl_writephy(tp, 0x14, 0x9065);
+	rtl_writephy(tp, 0x14, 0x1065);
 
-	r8168_phy_ocp_write(tp, 0xa436, 0x8012);
-	rtl_w1w0_phy_ocp(tp, 0xa438, 0x8000, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+}
 
-	rtl_w1w0_phy_ocp(tp, 0xc422, 0x4000, 0x2000);
+static void rtl8168g_2_hw_phy_config(struct rtl8169_private *tp)
+{
+	rtl_apply_firmware(tp);
 }
 
 static void rtl8102e_hw_phy_config(struct rtl8169_private *tp)
@@ -3600,6 +3649,10 @@ static void rtl_hw_phy_config(struct net_device *dev)
 	case RTL_GIGA_MAC_VER_40:
 		rtl8168g_1_hw_phy_config(tp);
 		break;
+	case RTL_GIGA_MAC_VER_42:
+	case RTL_GIGA_MAC_VER_43:
+		rtl8168g_2_hw_phy_config(tp);
+		break;
 
 	case RTL_GIGA_MAC_VER_41:
 	default:
@@ -3808,6 +3861,8 @@ static void rtl_init_mdio_ops(struct rtl8169_private *tp)
 		break;
 	case RTL_GIGA_MAC_VER_40:
 	case RTL_GIGA_MAC_VER_41:
+	case RTL_GIGA_MAC_VER_42:
+	case RTL_GIGA_MAC_VER_43:
 		ops->write	= r8168g_mdio_write;
 		ops->read	= r8168g_mdio_read;
 		break;
@@ -3859,6 +3914,8 @@ static void rtl_wol_suspend_quirk(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_39:
 	case RTL_GIGA_MAC_VER_40:
 	case RTL_GIGA_MAC_VER_41:
+	case RTL_GIGA_MAC_VER_42:
+	case RTL_GIGA_MAC_VER_43:
 		RTL_W32(RxConfig, RTL_R32(RxConfig) |
 			AcceptBroadcast | AcceptMulticast | AcceptMyPhys);
 		break;
@@ -3966,6 +4023,8 @@ static void r8168_phy_power_down(struct rtl8169_private *tp)
 	switch (tp->mac_version) {
 	case RTL_GIGA_MAC_VER_32:
 	case RTL_GIGA_MAC_VER_33:
+	case RTL_GIGA_MAC_VER_40:
+	case RTL_GIGA_MAC_VER_41:
 		rtl_writephy(tp, MII_BMCR, BMCR_ANENABLE | BMCR_PDOWN);
 		break;
 
@@ -4027,6 +4086,11 @@ static void r8168_pll_power_down(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_33:
 		RTL_W8(PMCH, RTL_R8(PMCH) & ~0x80);
 		break;
+	case RTL_GIGA_MAC_VER_40:
+	case RTL_GIGA_MAC_VER_41:
+		rtl_w1w0_eri(tp, 0x1a8, ERIAR_MASK_1111, 0x00000000,
+			     0xfc000000, ERIAR_EXGMAC);
+		break;
 	}
 }
 
@@ -4044,6 +4108,11 @@ static void r8168_pll_power_up(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_33:
 		RTL_W8(PMCH, RTL_R8(PMCH) | 0x80);
 		break;
+	case RTL_GIGA_MAC_VER_40:
+	case RTL_GIGA_MAC_VER_41:
+		rtl_w1w0_eri(tp, 0x1a8, ERIAR_MASK_1111, 0xfc000000,
+			     0x00000000, ERIAR_EXGMAC);
+		break;
 	}
 
 	r8168_phy_power_up(tp);
@@ -4080,6 +4149,7 @@ static void rtl_init_pll_power_ops(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_30:
 	case RTL_GIGA_MAC_VER_37:
 	case RTL_GIGA_MAC_VER_39:
+	case RTL_GIGA_MAC_VER_43:
 		ops->down	= r810x_pll_power_down;
 		ops->up		= r810x_pll_power_up;
 		break;
@@ -4107,6 +4177,7 @@ static void rtl_init_pll_power_ops(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_38:
 	case RTL_GIGA_MAC_VER_40:
 	case RTL_GIGA_MAC_VER_41:
+	case RTL_GIGA_MAC_VER_42:
 		ops->down	= r8168_pll_power_down;
 		ops->up		= r8168_pll_power_up;
 		break;
@@ -4149,6 +4220,12 @@ static void rtl_init_rxcfg(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_34:
 		RTL_W32(RxConfig, RX128_INT_EN | RX_MULTI_EN | RX_DMA_BURST);
 		break;
+	case RTL_GIGA_MAC_VER_40:
+	case RTL_GIGA_MAC_VER_41:
+	case RTL_GIGA_MAC_VER_42:
+	case RTL_GIGA_MAC_VER_43:
+		RTL_W32(RxConfig, RX128_INT_EN | RX_DMA_BURST | RX_EARLY_OFF);
+		break;
 	default:
 		RTL_W32(RxConfig, RX128_INT_EN | RX_DMA_BURST);
 		break;
@@ -4305,6 +4382,8 @@ static void rtl_init_jumbo_ops(struct rtl8169_private *tp)
 	 */
 	case RTL_GIGA_MAC_VER_40:
 	case RTL_GIGA_MAC_VER_41:
+	case RTL_GIGA_MAC_VER_42:
+	case RTL_GIGA_MAC_VER_43:
 	default:
 		ops->disable	= NULL;
 		ops->enable	= NULL;
@@ -4412,6 +4491,8 @@ static void rtl8169_hw_reset(struct rtl8169_private *tp)
 	           tp->mac_version == RTL_GIGA_MAC_VER_37 ||
 	           tp->mac_version == RTL_GIGA_MAC_VER_40 ||
 	           tp->mac_version == RTL_GIGA_MAC_VER_41 ||
+	           tp->mac_version == RTL_GIGA_MAC_VER_42 ||
+	           tp->mac_version == RTL_GIGA_MAC_VER_43 ||
 	           tp->mac_version == RTL_GIGA_MAC_VER_38) {
 		RTL_W8(ChipCmd, RTL_R8(ChipCmd) | StopReq);
 		rtl_udelay_loop_wait_high(tp, &rtl_txcfg_empty_cond, 100, 666);
@@ -5127,6 +5208,8 @@ static void rtl_hw_start_8168g_1(struct rtl8169_private *tp)
 	void __iomem *ioaddr = tp->mmio_addr;
 	struct pci_dev *pdev = tp->pci_dev;
 
+	RTL_W32(TxConfig, RTL_R32(TxConfig) | TXCFG_AUTO_FIFO);
+
 	rtl_eri_write(tp, 0xc8, ERIAR_MASK_0101, 0x080002, ERIAR_EXGMAC);
 	rtl_eri_write(tp, 0xcc, ERIAR_MASK_0001, 0x38, ERIAR_EXGMAC);
 	rtl_eri_write(tp, 0xd0, ERIAR_MASK_0001, 0x48, ERIAR_EXGMAC);
@@ -5138,6 +5221,7 @@ static void rtl_hw_start_8168g_1(struct rtl8169_private *tp)
 
 	rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x00, 0x01, ERIAR_EXGMAC);
 	rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x01, 0x00, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0x2f8, ERIAR_MASK_0011, 0x1d8f, ERIAR_EXGMAC);
 
 	RTL_W8(ChipCmd, CmdTxEnb | CmdRxEnb);
 	RTL_W32(MISC, RTL_R32(MISC) & ~RXDV_GATED_EN);
@@ -5149,7 +5233,26 @@ static void rtl_hw_start_8168g_1(struct rtl8169_private *tp)
 	/* Adjust EEE LED frequency */
 	RTL_W8(EEE_LED, RTL_R8(EEE_LED) & ~0x07);
 
-	rtl_w1w0_eri(tp, 0x2fc, ERIAR_MASK_0001, 0x01, 0x02, ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0x2fc, ERIAR_MASK_0001, 0x01, 0x06, ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0x1b0, ERIAR_MASK_0011, 0x0000, 0x1000, ERIAR_EXGMAC);
+}
+
+static void rtl_hw_start_8168g_2(struct rtl8169_private *tp)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+	static const struct ephy_info e_info_8168g_2[] = {
+		{ 0x00, 0x0000,	0x0008 },
+		{ 0x0c, 0x3df0,	0x0200 },
+		{ 0x19, 0xffff,	0xfc00 },
+		{ 0x1e, 0xffff,	0x20eb }
+	};
+
+	rtl_hw_start_8168g_1(tp);
+
+	/* disable aspm and clock request before access ephy */
+	RTL_W8(Config2, RTL_R8(Config2) & ~ClkReqEn);
+	RTL_W8(Config5, RTL_R8(Config5) & ~ASPM_en);
+	rtl_ephy_init(tp, e_info_8168g_2, ARRAY_SIZE(e_info_8168g_2));
 }
 
 static void rtl_hw_start_8168(struct net_device *dev)
@@ -5177,10 +5280,7 @@ static void rtl_hw_start_8168(struct net_device *dev)
 
 	rtl_set_rx_tx_desc_registers(tp, ioaddr);
 
-	rtl_set_rx_mode(dev);
-
-	RTL_W32(TxConfig, (TX_DMA_BURST << TxDMAShift) |
-		(InterFrameGap << TxInterFrameGapShift));
+	rtl_set_rx_tx_config_registers(tp);
 
 	RTL_R8(IntrMask);
 
@@ -5257,6 +5357,9 @@ static void rtl_hw_start_8168(struct net_device *dev)
 	case RTL_GIGA_MAC_VER_41:
 		rtl_hw_start_8168g_1(tp);
 		break;
+	case RTL_GIGA_MAC_VER_42:
+		rtl_hw_start_8168g_2(tp);
+		break;
 
 	default:
 		printk(KERN_ERR PFX "%s: unknown chipset (mac_version = %d).\n",
@@ -5264,9 +5367,11 @@ static void rtl_hw_start_8168(struct net_device *dev)
 		break;
 	}
 
+	RTL_W8(Cfg9346, Cfg9346_Lock);
+
 	RTL_W8(ChipCmd, CmdTxEnb | CmdRxEnb);
 
-	RTL_W8(Cfg9346, Cfg9346_Lock);
+	rtl_set_rx_mode(dev);
 
 	RTL_W16(MultiIntr, RTL_R16(MultiIntr) & 0xF000);
 }
@@ -5424,6 +5529,17 @@ static void rtl_hw_start_8101(struct net_device *dev)
 
 	RTL_W8(Cfg9346, Cfg9346_Unlock);
 
+	RTL_W8(MaxTxPacketSize, TxPacketMax);
+
+	rtl_set_rx_max_size(ioaddr, rx_buf_sz);
+
+	tp->cp_cmd &= ~R810X_CPCMD_QUIRK_MASK;
+	RTL_W16(CPlusCmd, tp->cp_cmd);
+
+	rtl_set_rx_tx_desc_registers(tp, ioaddr);
+
+	rtl_set_rx_tx_config_registers(tp);
+
 	switch (tp->mac_version) {
 	case RTL_GIGA_MAC_VER_07:
 		rtl_hw_start_8102e_1(tp);
@@ -5451,28 +5567,21 @@ static void rtl_hw_start_8101(struct net_device *dev)
 	case RTL_GIGA_MAC_VER_39:
 		rtl_hw_start_8106(tp);
 		break;
+	case RTL_GIGA_MAC_VER_43:
+		rtl_hw_start_8168g_2(tp);
+		break;
 	}
 
 	RTL_W8(Cfg9346, Cfg9346_Lock);
 
-	RTL_W8(MaxTxPacketSize, TxPacketMax);
-
-	rtl_set_rx_max_size(ioaddr, rx_buf_sz);
-
-	tp->cp_cmd &= ~R810X_CPCMD_QUIRK_MASK;
-	RTL_W16(CPlusCmd, tp->cp_cmd);
-
 	RTL_W16(IntrMitigate, 0x0000);
 
-	rtl_set_rx_tx_desc_registers(tp, ioaddr);
-
 	RTL_W8(ChipCmd, CmdTxEnb | CmdRxEnb);
-	rtl_set_rx_tx_config_registers(tp);
-
-	RTL_R8(IntrMask);
 
 	rtl_set_rx_mode(dev);
 
+	RTL_R8(IntrMask);
+
 	RTL_W16(MultiIntr, RTL_R16(MultiIntr) & 0xf000);
 }
 
@@ -5787,6 +5896,14 @@ static netdev_tx_t rtl8169_start_xmit(struct sk_buff *skb,
 		goto err_stop_0;
 	}
 
+	/* 8168evl does not automatically pad to minimum length. */
+	if (unlikely(tp->mac_version == RTL_GIGA_MAC_VER_34 &&
+		     skb->len < ETH_ZLEN)) {
+		if (skb_padto(skb, ETH_ZLEN))
+			goto err_update_stats;
+		skb_put(skb, ETH_ZLEN - skb->len);
+	}
+
 	if (unlikely(le32_to_cpu(txd->opts1) & DescOwn))
 		goto err_stop_0;
 
@@ -5858,6 +5975,7 @@ err_dma_1:
 	rtl8169_unmap_tx_skb(d, tp->tx_skb + entry, txd);
 err_dma_0:
 	dev_kfree_skb(skb);
+err_update_stats:
 	dev->stats.tx_dropped++;
 	return NETDEV_TX_OK;
 
@@ -6744,6 +6862,8 @@ static void rtl_hw_initialize(struct rtl8169_private *tp)
 	switch (tp->mac_version) {
 	case RTL_GIGA_MAC_VER_40:
 	case RTL_GIGA_MAC_VER_41:
+	case RTL_GIGA_MAC_VER_42:
+	case RTL_GIGA_MAC_VER_43:
 		rtl_hw_init_8168g(tp);
 		break;
 
@@ -6926,16 +7046,17 @@ rtl_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	/* don't enable SG, IP_CSUM and TSO by default - it might not work
 	 * properly for all devices */
 	dev->features |= NETIF_F_RXCSUM |
-		NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
+		NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX;
 
 	dev->hw_features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_TSO |
-		NETIF_F_RXCSUM | NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
+		NETIF_F_RXCSUM | NETIF_F_HW_VLAN_CTAG_TX |
+		NETIF_F_HW_VLAN_CTAG_RX;
 	dev->vlan_features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_TSO |
 		NETIF_F_HIGHDMA;
 
 	if (tp->mac_version == RTL_GIGA_MAC_VER_05)
 		/* 8110SCd requires hardware Rx VLAN - disallow toggling */
-		dev->hw_features &= ~NETIF_F_HW_VLAN_RX;
+		dev->hw_features &= ~NETIF_F_HW_VLAN_CTAG_RX;
 
 	dev->hw_features |= NETIF_F_RXALL;
 	dev->hw_features |= NETIF_F_RXFCS;
diff --git a/drivers/net/ethernet/renesas/Kconfig b/drivers/net/ethernet/renesas/Kconfig
index 24c2305d7948..bed9841d728c 100644
--- a/drivers/net/ethernet/renesas/Kconfig
+++ b/drivers/net/ethernet/renesas/Kconfig
@@ -8,7 +8,8 @@ config SH_ETH
 		(CPU_SUBTYPE_SH7710 || CPU_SUBTYPE_SH7712 || \
 		 CPU_SUBTYPE_SH7763 || CPU_SUBTYPE_SH7619 || \
 		 CPU_SUBTYPE_SH7724 || CPU_SUBTYPE_SH7734 || \
-		 CPU_SUBTYPE_SH7757 || ARCH_R8A7740 || ARCH_R8A7779)
+		 CPU_SUBTYPE_SH7757 || ARCH_R8A7740 || \
+		 ARCH_R8A7778 || ARCH_R8A7779)
 	select CRC32
 	select NET_CORE
 	select MII
diff --git a/drivers/net/ethernet/renesas/sh_eth.c b/drivers/net/ethernet/renesas/sh_eth.c
index 6ed333fe5c04..33dc6f2418f2 100644
--- a/drivers/net/ethernet/renesas/sh_eth.c
+++ b/drivers/net/ethernet/renesas/sh_eth.c
@@ -2,7 +2,8 @@
  *  SuperH Ethernet device driver
  *
  *  Copyright (C) 2006-2012 Nobuhiro Iwamatsu
- *  Copyright (C) 2008-2012 Renesas Solutions Corp.
+ *  Copyright (C) 2008-2013 Renesas Solutions Corp.
+ *  Copyright (C) 2013 Cogent Embedded, Inc.
  *
  *  This program is free software; you can redistribute it and/or modify it
  *  under the terms and conditions of the GNU General Public License,
@@ -49,6 +50,269 @@
 		NETIF_MSG_RX_ERR| \
 		NETIF_MSG_TX_ERR)
 
+static const u16 sh_eth_offset_gigabit[SH_ETH_MAX_REGISTER_OFFSET] = {
+	[EDSR]		= 0x0000,
+	[EDMR]		= 0x0400,
+	[EDTRR]		= 0x0408,
+	[EDRRR]		= 0x0410,
+	[EESR]		= 0x0428,
+	[EESIPR]	= 0x0430,
+	[TDLAR]		= 0x0010,
+	[TDFAR]		= 0x0014,
+	[TDFXR]		= 0x0018,
+	[TDFFR]		= 0x001c,
+	[RDLAR]		= 0x0030,
+	[RDFAR]		= 0x0034,
+	[RDFXR]		= 0x0038,
+	[RDFFR]		= 0x003c,
+	[TRSCER]	= 0x0438,
+	[RMFCR]		= 0x0440,
+	[TFTR]		= 0x0448,
+	[FDR]		= 0x0450,
+	[RMCR]		= 0x0458,
+	[RPADIR]	= 0x0460,
+	[FCFTR]		= 0x0468,
+	[CSMR]		= 0x04E4,
+
+	[ECMR]		= 0x0500,
+	[ECSR]		= 0x0510,
+	[ECSIPR]	= 0x0518,
+	[PIR]		= 0x0520,
+	[PSR]		= 0x0528,
+	[PIPR]		= 0x052c,
+	[RFLR]		= 0x0508,
+	[APR]		= 0x0554,
+	[MPR]		= 0x0558,
+	[PFTCR]		= 0x055c,
+	[PFRCR]		= 0x0560,
+	[TPAUSER]	= 0x0564,
+	[GECMR]		= 0x05b0,
+	[BCULR]		= 0x05b4,
+	[MAHR]		= 0x05c0,
+	[MALR]		= 0x05c8,
+	[TROCR]		= 0x0700,
+	[CDCR]		= 0x0708,
+	[LCCR]		= 0x0710,
+	[CEFCR]		= 0x0740,
+	[FRECR]		= 0x0748,
+	[TSFRCR]	= 0x0750,
+	[TLFRCR]	= 0x0758,
+	[RFCR]		= 0x0760,
+	[CERCR]		= 0x0768,
+	[CEECR]		= 0x0770,
+	[MAFCR]		= 0x0778,
+	[RMII_MII]	= 0x0790,
+
+	[ARSTR]		= 0x0000,
+	[TSU_CTRST]	= 0x0004,
+	[TSU_FWEN0]	= 0x0010,
+	[TSU_FWEN1]	= 0x0014,
+	[TSU_FCM]	= 0x0018,
+	[TSU_BSYSL0]	= 0x0020,
+	[TSU_BSYSL1]	= 0x0024,
+	[TSU_PRISL0]	= 0x0028,
+	[TSU_PRISL1]	= 0x002c,
+	[TSU_FWSL0]	= 0x0030,
+	[TSU_FWSL1]	= 0x0034,
+	[TSU_FWSLC]	= 0x0038,
+	[TSU_QTAG0]	= 0x0040,
+	[TSU_QTAG1]	= 0x0044,
+	[TSU_FWSR]	= 0x0050,
+	[TSU_FWINMK]	= 0x0054,
+	[TSU_ADQT0]	= 0x0048,
+	[TSU_ADQT1]	= 0x004c,
+	[TSU_VTAG0]	= 0x0058,
+	[TSU_VTAG1]	= 0x005c,
+	[TSU_ADSBSY]	= 0x0060,
+	[TSU_TEN]	= 0x0064,
+	[TSU_POST1]	= 0x0070,
+	[TSU_POST2]	= 0x0074,
+	[TSU_POST3]	= 0x0078,
+	[TSU_POST4]	= 0x007c,
+	[TSU_ADRH0]	= 0x0100,
+	[TSU_ADRL0]	= 0x0104,
+	[TSU_ADRH31]	= 0x01f8,
+	[TSU_ADRL31]	= 0x01fc,
+
+	[TXNLCR0]	= 0x0080,
+	[TXALCR0]	= 0x0084,
+	[RXNLCR0]	= 0x0088,
+	[RXALCR0]	= 0x008c,
+	[FWNLCR0]	= 0x0090,
+	[FWALCR0]	= 0x0094,
+	[TXNLCR1]	= 0x00a0,
+	[TXALCR1]	= 0x00a0,
+	[RXNLCR1]	= 0x00a8,
+	[RXALCR1]	= 0x00ac,
+	[FWNLCR1]	= 0x00b0,
+	[FWALCR1]	= 0x00b4,
+};
+
+static const u16 sh_eth_offset_fast_rcar[SH_ETH_MAX_REGISTER_OFFSET] = {
+	[ECMR]		= 0x0300,
+	[RFLR]		= 0x0308,
+	[ECSR]		= 0x0310,
+	[ECSIPR]	= 0x0318,
+	[PIR]		= 0x0320,
+	[PSR]		= 0x0328,
+	[RDMLR]		= 0x0340,
+	[IPGR]		= 0x0350,
+	[APR]		= 0x0354,
+	[MPR]		= 0x0358,
+	[RFCF]		= 0x0360,
+	[TPAUSER]	= 0x0364,
+	[TPAUSECR]	= 0x0368,
+	[MAHR]		= 0x03c0,
+	[MALR]		= 0x03c8,
+	[TROCR]		= 0x03d0,
+	[CDCR]		= 0x03d4,
+	[LCCR]		= 0x03d8,
+	[CNDCR]		= 0x03dc,
+	[CEFCR]		= 0x03e4,
+	[FRECR]		= 0x03e8,
+	[TSFRCR]	= 0x03ec,
+	[TLFRCR]	= 0x03f0,
+	[RFCR]		= 0x03f4,
+	[MAFCR]		= 0x03f8,
+
+	[EDMR]		= 0x0200,
+	[EDTRR]		= 0x0208,
+	[EDRRR]		= 0x0210,
+	[TDLAR]		= 0x0218,
+	[RDLAR]		= 0x0220,
+	[EESR]		= 0x0228,
+	[EESIPR]	= 0x0230,
+	[TRSCER]	= 0x0238,
+	[RMFCR]		= 0x0240,
+	[TFTR]		= 0x0248,
+	[FDR]		= 0x0250,
+	[RMCR]		= 0x0258,
+	[TFUCR]		= 0x0264,
+	[RFOCR]		= 0x0268,
+	[FCFTR]		= 0x0270,
+	[TRIMD]		= 0x027c,
+};
+
+static const u16 sh_eth_offset_fast_sh4[SH_ETH_MAX_REGISTER_OFFSET] = {
+	[ECMR]		= 0x0100,
+	[RFLR]		= 0x0108,
+	[ECSR]		= 0x0110,
+	[ECSIPR]	= 0x0118,
+	[PIR]		= 0x0120,
+	[PSR]		= 0x0128,
+	[RDMLR]		= 0x0140,
+	[IPGR]		= 0x0150,
+	[APR]		= 0x0154,
+	[MPR]		= 0x0158,
+	[TPAUSER]	= 0x0164,
+	[RFCF]		= 0x0160,
+	[TPAUSECR]	= 0x0168,
+	[BCFRR]		= 0x016c,
+	[MAHR]		= 0x01c0,
+	[MALR]		= 0x01c8,
+	[TROCR]		= 0x01d0,
+	[CDCR]		= 0x01d4,
+	[LCCR]		= 0x01d8,
+	[CNDCR]		= 0x01dc,
+	[CEFCR]		= 0x01e4,
+	[FRECR]		= 0x01e8,
+	[TSFRCR]	= 0x01ec,
+	[TLFRCR]	= 0x01f0,
+	[RFCR]		= 0x01f4,
+	[MAFCR]		= 0x01f8,
+	[RTRATE]	= 0x01fc,
+
+	[EDMR]		= 0x0000,
+	[EDTRR]		= 0x0008,
+	[EDRRR]		= 0x0010,
+	[TDLAR]		= 0x0018,
+	[RDLAR]		= 0x0020,
+	[EESR]		= 0x0028,
+	[EESIPR]	= 0x0030,
+	[TRSCER]	= 0x0038,
+	[RMFCR]		= 0x0040,
+	[TFTR]		= 0x0048,
+	[FDR]		= 0x0050,
+	[RMCR]		= 0x0058,
+	[TFUCR]		= 0x0064,
+	[RFOCR]		= 0x0068,
+	[FCFTR]		= 0x0070,
+	[RPADIR]	= 0x0078,
+	[TRIMD]		= 0x007c,
+	[RBWAR]		= 0x00c8,
+	[RDFAR]		= 0x00cc,
+	[TBRAR]		= 0x00d4,
+	[TDFAR]		= 0x00d8,
+};
+
+static const u16 sh_eth_offset_fast_sh3_sh2[SH_ETH_MAX_REGISTER_OFFSET] = {
+	[ECMR]		= 0x0160,
+	[ECSR]		= 0x0164,
+	[ECSIPR]	= 0x0168,
+	[PIR]		= 0x016c,
+	[MAHR]		= 0x0170,
+	[MALR]		= 0x0174,
+	[RFLR]		= 0x0178,
+	[PSR]		= 0x017c,
+	[TROCR]		= 0x0180,
+	[CDCR]		= 0x0184,
+	[LCCR]		= 0x0188,
+	[CNDCR]		= 0x018c,
+	[CEFCR]		= 0x0194,
+	[FRECR]		= 0x0198,
+	[TSFRCR]	= 0x019c,
+	[TLFRCR]	= 0x01a0,
+	[RFCR]		= 0x01a4,
+	[MAFCR]		= 0x01a8,
+	[IPGR]		= 0x01b4,
+	[APR]		= 0x01b8,
+	[MPR]		= 0x01bc,
+	[TPAUSER]	= 0x01c4,
+	[BCFR]		= 0x01cc,
+
+	[ARSTR]		= 0x0000,
+	[TSU_CTRST]	= 0x0004,
+	[TSU_FWEN0]	= 0x0010,
+	[TSU_FWEN1]	= 0x0014,
+	[TSU_FCM]	= 0x0018,
+	[TSU_BSYSL0]	= 0x0020,
+	[TSU_BSYSL1]	= 0x0024,
+	[TSU_PRISL0]	= 0x0028,
+	[TSU_PRISL1]	= 0x002c,
+	[TSU_FWSL0]	= 0x0030,
+	[TSU_FWSL1]	= 0x0034,
+	[TSU_FWSLC]	= 0x0038,
+	[TSU_QTAGM0]	= 0x0040,
+	[TSU_QTAGM1]	= 0x0044,
+	[TSU_ADQT0]	= 0x0048,
+	[TSU_ADQT1]	= 0x004c,
+	[TSU_FWSR]	= 0x0050,
+	[TSU_FWINMK]	= 0x0054,
+	[TSU_ADSBSY]	= 0x0060,
+	[TSU_TEN]	= 0x0064,
+	[TSU_POST1]	= 0x0070,
+	[TSU_POST2]	= 0x0074,
+	[TSU_POST3]	= 0x0078,
+	[TSU_POST4]	= 0x007c,
+
+	[TXNLCR0]	= 0x0080,
+	[TXALCR0]	= 0x0084,
+	[RXNLCR0]	= 0x0088,
+	[RXALCR0]	= 0x008c,
+	[FWNLCR0]	= 0x0090,
+	[FWALCR0]	= 0x0094,
+	[TXNLCR1]	= 0x00a0,
+	[TXALCR1]	= 0x00a0,
+	[RXNLCR1]	= 0x00a8,
+	[RXALCR1]	= 0x00ac,
+	[FWNLCR1]	= 0x00b0,
+	[FWALCR1]	= 0x00b4,
+
+	[TSU_ADRH0]	= 0x0100,
+	[TSU_ADRL0]	= 0x0104,
+	[TSU_ADRL31]	= 0x01fc,
+};
+
 #if defined(CONFIG_CPU_SUBTYPE_SH7734) || \
 	defined(CONFIG_CPU_SUBTYPE_SH7763) || \
 	defined(CONFIG_ARCH_R8A7740)
@@ -78,7 +342,7 @@ static void sh_eth_select_mii(struct net_device *ndev)
 #endif
 
 /* There is CPU dependent code */
-#if defined(CONFIG_CPU_SUBTYPE_SH7724) || defined(CONFIG_ARCH_R8A7779)
+#if defined(CONFIG_ARCH_R8A7778) || defined(CONFIG_ARCH_R8A7779)
 #define SH_ETH_RESET_DEFAULT	1
 static void sh_eth_set_duplex(struct net_device *ndev)
 {
@@ -93,18 +357,60 @@ static void sh_eth_set_duplex(struct net_device *ndev)
 static void sh_eth_set_rate(struct net_device *ndev)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
-	unsigned int bits = ECMR_RTM;
 
-#if defined(CONFIG_ARCH_R8A7779)
-	bits |= ECMR_ELB;
-#endif
+	switch (mdp->speed) {
+	case 10: /* 10BASE */
+		sh_eth_write(ndev, sh_eth_read(ndev, ECMR) & ~ECMR_ELB, ECMR);
+		break;
+	case 100:/* 100BASE */
+		sh_eth_write(ndev, sh_eth_read(ndev, ECMR) | ECMR_ELB, ECMR);
+		break;
+	default:
+		break;
+	}
+}
+
+/* R8A7778/9 */
+static struct sh_eth_cpu_data sh_eth_my_cpu_data = {
+	.set_duplex	= sh_eth_set_duplex,
+	.set_rate	= sh_eth_set_rate,
+
+	.ecsr_value	= ECSR_PSRTO | ECSR_LCHNG | ECSR_ICD,
+	.ecsipr_value	= ECSIPR_PSRTOIP | ECSIPR_LCHNGIP | ECSIPR_ICDIP,
+	.eesipr_value	= 0x01ff009f,
+
+	.tx_check	= EESR_FTC | EESR_CND | EESR_DLC | EESR_CD | EESR_RTO,
+	.eesr_err_check	= EESR_TWB | EESR_TABT | EESR_RABT | EESR_RDE |
+			  EESR_RFRMER | EESR_TFE | EESR_TDE | EESR_ECI,
+	.tx_error_check	= EESR_TWB | EESR_TABT | EESR_TDE | EESR_TFE,
+
+	.apr		= 1,
+	.mpr		= 1,
+	.tpauser	= 1,
+	.hw_swap	= 1,
+};
+#elif defined(CONFIG_CPU_SUBTYPE_SH7724)
+#define SH_ETH_RESET_DEFAULT	1
+static void sh_eth_set_duplex(struct net_device *ndev)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+
+	if (mdp->duplex) /* Full */
+		sh_eth_write(ndev, sh_eth_read(ndev, ECMR) | ECMR_DM, ECMR);
+	else		/* Half */
+		sh_eth_write(ndev, sh_eth_read(ndev, ECMR) & ~ECMR_DM, ECMR);
+}
+
+static void sh_eth_set_rate(struct net_device *ndev)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
 
 	switch (mdp->speed) {
 	case 10: /* 10BASE */
-		sh_eth_write(ndev, sh_eth_read(ndev, ECMR) & ~bits, ECMR);
+		sh_eth_write(ndev, sh_eth_read(ndev, ECMR) & ~ECMR_RTM, ECMR);
 		break;
 	case 100:/* 100BASE */
-		sh_eth_write(ndev, sh_eth_read(ndev, ECMR) | bits, ECMR);
+		sh_eth_write(ndev, sh_eth_read(ndev, ECMR) | ECMR_RTM, ECMR);
 		break;
 	default:
 		break;
@@ -592,7 +898,7 @@ static int sh_eth_check_reset(struct net_device *ndev)
 		cnt--;
 	}
 	if (cnt < 0) {
-		printk(KERN_ERR "Device reset fail\n");
+		pr_err("Device reset fail\n");
 		ret = -ETIMEDOUT;
 	}
 	return ret;
@@ -908,11 +1214,8 @@ static int sh_eth_ring_init(struct net_device *ndev)
 	/* Allocate all Rx descriptors. */
 	rx_ringsize = sizeof(struct sh_eth_rxdesc) * mdp->num_rx_ring;
 	mdp->rx_ring = dma_alloc_coherent(NULL, rx_ringsize, &mdp->rx_desc_dma,
-			GFP_KERNEL);
-
+					  GFP_KERNEL);
 	if (!mdp->rx_ring) {
-		dev_err(&ndev->dev, "Cannot allocate Rx Ring (size %d bytes)\n",
-			rx_ringsize);
 		ret = -ENOMEM;
 		goto desc_ring_free;
 	}
@@ -922,10 +1225,8 @@ static int sh_eth_ring_init(struct net_device *ndev)
 	/* Allocate all Tx descriptors. */
 	tx_ringsize = sizeof(struct sh_eth_txdesc) * mdp->num_tx_ring;
 	mdp->tx_ring = dma_alloc_coherent(NULL, tx_ringsize, &mdp->tx_desc_dma,
-			GFP_KERNEL);
+					  GFP_KERNEL);
 	if (!mdp->tx_ring) {
-		dev_err(&ndev->dev, "Cannot allocate Tx Ring (size %d bytes)\n",
-			tx_ringsize);
 		ret = -ENOMEM;
 		goto desc_ring_free;
 	}
@@ -2147,7 +2448,8 @@ static int sh_eth_get_vtag_index(struct sh_eth_private *mdp)
 		return TSU_VTAG1;
 }
 
-static int sh_eth_vlan_rx_add_vid(struct net_device *ndev, u16 vid)
+static int sh_eth_vlan_rx_add_vid(struct net_device *ndev,
+				  __be16 proto, u16 vid)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
 	int vtag_reg_index = sh_eth_get_vtag_index(mdp);
@@ -2177,7 +2479,8 @@ static int sh_eth_vlan_rx_add_vid(struct net_device *ndev, u16 vid)
 	return 0;
 }
 
-static int sh_eth_vlan_rx_kill_vid(struct net_device *ndev, u16 vid)
+static int sh_eth_vlan_rx_kill_vid(struct net_device *ndev,
+				   __be16 proto, u16 vid)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
 	int vtag_reg_index = sh_eth_get_vtag_index(mdp);
@@ -2228,7 +2531,6 @@ static void sh_eth_tsu_init(struct sh_eth_private *mdp)
 /* MDIO bus release function */
 static int sh_mdio_release(struct net_device *ndev)
 {
-	struct sh_eth_private *mdp = netdev_priv(ndev);
 	struct mii_bus *bus = dev_get_drvdata(&ndev->dev);
 
 	/* unregister mdio bus */
@@ -2237,15 +2539,9 @@ static int sh_mdio_release(struct net_device *ndev)
 	/* remove mdio bus info from net_device */
 	dev_set_drvdata(&ndev->dev, NULL);
 
-	/* free interrupts memory */
-	kfree(bus->irq);
-
 	/* free bitbang info */
 	free_mdio_bitbang(bus);
 
-	/* free bitbang memory */
-	kfree(mdp->bitbang);
-
 	return 0;
 }
 
@@ -2258,7 +2554,8 @@ static int sh_mdio_init(struct net_device *ndev, int id,
 	struct sh_eth_private *mdp = netdev_priv(ndev);
 
 	/* create bit control struct for PHY */
-	bitbang = kzalloc(sizeof(struct bb_info), GFP_KERNEL);
+	bitbang = devm_kzalloc(&ndev->dev, sizeof(struct bb_info),
+			       GFP_KERNEL);
 	if (!bitbang) {
 		ret = -ENOMEM;
 		goto out;
@@ -2267,18 +2564,17 @@ static int sh_mdio_init(struct net_device *ndev, int id,
 	/* bitbang init */
 	bitbang->addr = mdp->addr + mdp->reg_offset[PIR];
 	bitbang->set_gate = pd->set_mdio_gate;
-	bitbang->mdi_msk = 0x08;
-	bitbang->mdo_msk = 0x04;
-	bitbang->mmd_msk = 0x02;/* MMD */
-	bitbang->mdc_msk = 0x01;
+	bitbang->mdi_msk = PIR_MDI;
+	bitbang->mdo_msk = PIR_MDO;
+	bitbang->mmd_msk = PIR_MMD;
+	bitbang->mdc_msk = PIR_MDC;
 	bitbang->ctrl.ops = &bb_ops;
 
 	/* MII controller setting */
-	mdp->bitbang = bitbang;
 	mdp->mii_bus = alloc_mdio_bitbang(&bitbang->ctrl);
 	if (!mdp->mii_bus) {
 		ret = -ENOMEM;
-		goto out_free_bitbang;
+		goto out;
 	}
 
 	/* Hook up MII support for ethtool */
@@ -2288,7 +2584,9 @@ static int sh_mdio_init(struct net_device *ndev, int id,
 		mdp->pdev->name, id);
 
 	/* PHY IRQ */
-	mdp->mii_bus->irq = kmalloc(sizeof(int)*PHY_MAX_ADDR, GFP_KERNEL);
+	mdp->mii_bus->irq = devm_kzalloc(&ndev->dev,
+					 sizeof(int) * PHY_MAX_ADDR,
+					 GFP_KERNEL);
 	if (!mdp->mii_bus->irq) {
 		ret = -ENOMEM;
 		goto out_free_bus;
@@ -2300,21 +2598,15 @@ static int sh_mdio_init(struct net_device *ndev, int id,
 	/* register mdio bus */
 	ret = mdiobus_register(mdp->mii_bus);
 	if (ret)
-		goto out_free_irq;
+		goto out_free_bus;
 
 	dev_set_drvdata(&ndev->dev, mdp->mii_bus);
 
 	return 0;
 
-out_free_irq:
-	kfree(mdp->mii_bus->irq);
-
 out_free_bus:
 	free_mdio_bitbang(mdp->mii_bus);
 
-out_free_bitbang:
-	kfree(bitbang);
-
 out:
 	return ret;
 }
@@ -2327,6 +2619,9 @@ static const u16 *sh_eth_get_register_offset(int register_type)
 	case SH_ETH_REG_GIGABIT:
 		reg_offset = sh_eth_offset_gigabit;
 		break;
+	case SH_ETH_REG_FAST_RCAR:
+		reg_offset = sh_eth_offset_fast_rcar;
+		break;
 	case SH_ETH_REG_FAST_SH4:
 		reg_offset = sh_eth_offset_fast_sh4;
 		break;
@@ -2334,7 +2629,7 @@ static const u16 *sh_eth_get_register_offset(int register_type)
 		reg_offset = sh_eth_offset_fast_sh3_sh2;
 		break;
 	default:
-		printk(KERN_ERR "Unknown register type (%d)\n", register_type);
+		pr_err("Unknown register type (%d)\n", register_type);
 		break;
 	}
 
@@ -2364,7 +2659,7 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
 	struct resource *res;
 	struct net_device *ndev = NULL;
 	struct sh_eth_private *mdp = NULL;
-	struct sh_eth_plat_data *pd;
+	struct sh_eth_plat_data *pd = pdev->dev.platform_data;
 
 	/* get base addr */
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -2402,10 +2697,9 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
 	mdp = netdev_priv(ndev);
 	mdp->num_tx_ring = TX_RING_SIZE;
 	mdp->num_rx_ring = RX_RING_SIZE;
-	mdp->addr = ioremap(res->start, resource_size(res));
-	if (mdp->addr == NULL) {
-		ret = -ENOMEM;
-		dev_err(&pdev->dev, "ioremap failed.\n");
+	mdp->addr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(mdp->addr)) {
+		ret = PTR_ERR(mdp->addr);
 		goto out_release;
 	}
 
@@ -2414,7 +2708,6 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
 	pm_runtime_enable(&pdev->dev);
 	pm_runtime_resume(&pdev->dev);
 
-	pd = (struct sh_eth_plat_data *)(pdev->dev.platform_data);
 	/* get PHY ID */
 	mdp->phy_id = pd->phy;
 	mdp->phy_interface = pd->phy_interface;
@@ -2442,6 +2735,11 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
 
 	/* read and set MAC address */
 	read_mac_address(ndev, pd->mac_addr);
+	if (!is_valid_ether_addr(ndev->dev_addr)) {
+		dev_warn(&pdev->dev,
+			 "no valid MAC address supplied, using a random one.\n");
+		eth_hw_addr_random(ndev);
+	}
 
 	/* ioremap the TSU registers */
 	if (mdp->cd->tsu) {
@@ -2452,15 +2750,13 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
 			ret = -ENODEV;
 			goto out_release;
 		}
-		mdp->tsu_addr = ioremap(rtsu->start,
-					resource_size(rtsu));
-		if (mdp->tsu_addr == NULL) {
-			ret = -ENOMEM;
-			dev_err(&pdev->dev, "TSU ioremap failed.\n");
+		mdp->tsu_addr = devm_ioremap_resource(&pdev->dev, rtsu);
+		if (IS_ERR(mdp->tsu_addr)) {
+			ret = PTR_ERR(mdp->tsu_addr);
 			goto out_release;
 		}
 		mdp->port = devno % 2;
-		ndev->features = NETIF_F_HW_VLAN_FILTER;
+		ndev->features = NETIF_F_HW_VLAN_CTAG_FILTER;
 	}
 
 	/* initialize first or needed device */
@@ -2497,10 +2793,6 @@ out_unregister:
 
 out_release:
 	/* net_dev free */
-	if (mdp && mdp->addr)
-		iounmap(mdp->addr);
-	if (mdp && mdp->tsu_addr)
-		iounmap(mdp->tsu_addr);
 	if (ndev)
 		free_netdev(ndev);
 
@@ -2511,14 +2803,10 @@ out:
 static int sh_eth_drv_remove(struct platform_device *pdev)
 {
 	struct net_device *ndev = platform_get_drvdata(pdev);
-	struct sh_eth_private *mdp = netdev_priv(ndev);
 
-	if (mdp->cd->tsu)
-		iounmap(mdp->tsu_addr);
 	sh_mdio_release(ndev);
 	unregister_netdev(ndev);
 	pm_runtime_disable(&pdev->dev);
-	iounmap(mdp->addr);
 	free_netdev(ndev);
 	platform_set_drvdata(pdev, NULL);
 
diff --git a/drivers/net/ethernet/renesas/sh_eth.h b/drivers/net/ethernet/renesas/sh_eth.h
index 828be4515008..1ddc9f235bcb 100644
--- a/drivers/net/ethernet/renesas/sh_eth.h
+++ b/drivers/net/ethernet/renesas/sh_eth.h
@@ -156,225 +156,6 @@ enum {
 	SH_ETH_MAX_REGISTER_OFFSET,
 };
 
-static const u16 sh_eth_offset_gigabit[SH_ETH_MAX_REGISTER_OFFSET] = {
-	[EDSR]	= 0x0000,
-	[EDMR]	= 0x0400,
-	[EDTRR]	= 0x0408,
-	[EDRRR]	= 0x0410,
-	[EESR]	= 0x0428,
-	[EESIPR]	= 0x0430,
-	[TDLAR]	= 0x0010,
-	[TDFAR]	= 0x0014,
-	[TDFXR]	= 0x0018,
-	[TDFFR]	= 0x001c,
-	[RDLAR]	= 0x0030,
-	[RDFAR]	= 0x0034,
-	[RDFXR]	= 0x0038,
-	[RDFFR]	= 0x003c,
-	[TRSCER]	= 0x0438,
-	[RMFCR]	= 0x0440,
-	[TFTR]	= 0x0448,
-	[FDR]	= 0x0450,
-	[RMCR]	= 0x0458,
-	[RPADIR]	= 0x0460,
-	[FCFTR]	= 0x0468,
-	[CSMR] = 0x04E4,
-
-	[ECMR]	= 0x0500,
-	[ECSR]	= 0x0510,
-	[ECSIPR]	= 0x0518,
-	[PIR]	= 0x0520,
-	[PSR]	= 0x0528,
-	[PIPR]	= 0x052c,
-	[RFLR]	= 0x0508,
-	[APR]	= 0x0554,
-	[MPR]	= 0x0558,
-	[PFTCR]	= 0x055c,
-	[PFRCR]	= 0x0560,
-	[TPAUSER]	= 0x0564,
-	[GECMR]	= 0x05b0,
-	[BCULR]	= 0x05b4,
-	[MAHR]	= 0x05c0,
-	[MALR]	= 0x05c8,
-	[TROCR]	= 0x0700,
-	[CDCR]	= 0x0708,
-	[LCCR]	= 0x0710,
-	[CEFCR]	= 0x0740,
-	[FRECR]	= 0x0748,
-	[TSFRCR]	= 0x0750,
-	[TLFRCR]	= 0x0758,
-	[RFCR]	= 0x0760,
-	[CERCR]	= 0x0768,
-	[CEECR]	= 0x0770,
-	[MAFCR]	= 0x0778,
-	[RMII_MII] =  0x0790,
-
-	[ARSTR]	= 0x0000,
-	[TSU_CTRST]	= 0x0004,
-	[TSU_FWEN0]	= 0x0010,
-	[TSU_FWEN1]	= 0x0014,
-	[TSU_FCM]	= 0x0018,
-	[TSU_BSYSL0]	= 0x0020,
-	[TSU_BSYSL1]	= 0x0024,
-	[TSU_PRISL0]	= 0x0028,
-	[TSU_PRISL1]	= 0x002c,
-	[TSU_FWSL0]	= 0x0030,
-	[TSU_FWSL1]	= 0x0034,
-	[TSU_FWSLC]	= 0x0038,
-	[TSU_QTAG0]	= 0x0040,
-	[TSU_QTAG1]	= 0x0044,
-	[TSU_FWSR]	= 0x0050,
-	[TSU_FWINMK]	= 0x0054,
-	[TSU_ADQT0]	= 0x0048,
-	[TSU_ADQT1]	= 0x004c,
-	[TSU_VTAG0]	= 0x0058,
-	[TSU_VTAG1]	= 0x005c,
-	[TSU_ADSBSY]	= 0x0060,
-	[TSU_TEN]	= 0x0064,
-	[TSU_POST1]	= 0x0070,
-	[TSU_POST2]	= 0x0074,
-	[TSU_POST3]	= 0x0078,
-	[TSU_POST4]	= 0x007c,
-	[TSU_ADRH0]	= 0x0100,
-	[TSU_ADRL0]	= 0x0104,
-	[TSU_ADRH31]	= 0x01f8,
-	[TSU_ADRL31]	= 0x01fc,
-
-	[TXNLCR0]	= 0x0080,
-	[TXALCR0]	= 0x0084,
-	[RXNLCR0]	= 0x0088,
-	[RXALCR0]	= 0x008c,
-	[FWNLCR0]	= 0x0090,
-	[FWALCR0]	= 0x0094,
-	[TXNLCR1]	= 0x00a0,
-	[TXALCR1]	= 0x00a0,
-	[RXNLCR1]	= 0x00a8,
-	[RXALCR1]	= 0x00ac,
-	[FWNLCR1]	= 0x00b0,
-	[FWALCR1]	= 0x00b4,
-};
-
-static const u16 sh_eth_offset_fast_sh4[SH_ETH_MAX_REGISTER_OFFSET] = {
-	[ECMR]	= 0x0100,
-	[RFLR]	= 0x0108,
-	[ECSR]	= 0x0110,
-	[ECSIPR]	= 0x0118,
-	[PIR]	= 0x0120,
-	[PSR]	= 0x0128,
-	[RDMLR]	= 0x0140,
-	[IPGR]	= 0x0150,
-	[APR]	= 0x0154,
-	[MPR]	= 0x0158,
-	[TPAUSER]	= 0x0164,
-	[RFCF]	= 0x0160,
-	[TPAUSECR]	= 0x0168,
-	[BCFRR]	= 0x016c,
-	[MAHR]	= 0x01c0,
-	[MALR]	= 0x01c8,
-	[TROCR]	= 0x01d0,
-	[CDCR]	= 0x01d4,
-	[LCCR]	= 0x01d8,
-	[CNDCR]	= 0x01dc,
-	[CEFCR]	= 0x01e4,
-	[FRECR]	= 0x01e8,
-	[TSFRCR]	= 0x01ec,
-	[TLFRCR]	= 0x01f0,
-	[RFCR]	= 0x01f4,
-	[MAFCR]	= 0x01f8,
-	[RTRATE]	= 0x01fc,
-
-	[EDMR]	= 0x0000,
-	[EDTRR]	= 0x0008,
-	[EDRRR]	= 0x0010,
-	[TDLAR]	= 0x0018,
-	[RDLAR]	= 0x0020,
-	[EESR]	= 0x0028,
-	[EESIPR]	= 0x0030,
-	[TRSCER]	= 0x0038,
-	[RMFCR]	= 0x0040,
-	[TFTR]	= 0x0048,
-	[FDR]	= 0x0050,
-	[RMCR]	= 0x0058,
-	[TFUCR]	= 0x0064,
-	[RFOCR]	= 0x0068,
-	[FCFTR]	= 0x0070,
-	[RPADIR]	= 0x0078,
-	[TRIMD]	= 0x007c,
-	[RBWAR]	= 0x00c8,
-	[RDFAR]	= 0x00cc,
-	[TBRAR]	= 0x00d4,
-	[TDFAR]	= 0x00d8,
-};
-
-static const u16 sh_eth_offset_fast_sh3_sh2[SH_ETH_MAX_REGISTER_OFFSET] = {
-	[ECMR]	= 0x0160,
-	[ECSR]	= 0x0164,
-	[ECSIPR]	= 0x0168,
-	[PIR]	= 0x016c,
-	[MAHR]	= 0x0170,
-	[MALR]	= 0x0174,
-	[RFLR]	= 0x0178,
-	[PSR]	= 0x017c,
-	[TROCR]	= 0x0180,
-	[CDCR]	= 0x0184,
-	[LCCR]	= 0x0188,
-	[CNDCR]	= 0x018c,
-	[CEFCR]	= 0x0194,
-	[FRECR]	= 0x0198,
-	[TSFRCR]	= 0x019c,
-	[TLFRCR]	= 0x01a0,
-	[RFCR]	= 0x01a4,
-	[MAFCR]	= 0x01a8,
-	[IPGR]	= 0x01b4,
-	[APR]	= 0x01b8,
-	[MPR]	= 0x01bc,
-	[TPAUSER]	= 0x01c4,
-	[BCFR]	= 0x01cc,
-
-	[ARSTR]	= 0x0000,
-	[TSU_CTRST]	= 0x0004,
-	[TSU_FWEN0]	= 0x0010,
-	[TSU_FWEN1]	= 0x0014,
-	[TSU_FCM]	= 0x0018,
-	[TSU_BSYSL0]	= 0x0020,
-	[TSU_BSYSL1]	= 0x0024,
-	[TSU_PRISL0]	= 0x0028,
-	[TSU_PRISL1]	= 0x002c,
-	[TSU_FWSL0]	= 0x0030,
-	[TSU_FWSL1]	= 0x0034,
-	[TSU_FWSLC]	= 0x0038,
-	[TSU_QTAGM0]	= 0x0040,
-	[TSU_QTAGM1]	= 0x0044,
-	[TSU_ADQT0]	= 0x0048,
-	[TSU_ADQT1]	= 0x004c,
-	[TSU_FWSR]	= 0x0050,
-	[TSU_FWINMK]	= 0x0054,
-	[TSU_ADSBSY]	= 0x0060,
-	[TSU_TEN]	= 0x0064,
-	[TSU_POST1]	= 0x0070,
-	[TSU_POST2]	= 0x0074,
-	[TSU_POST3]	= 0x0078,
-	[TSU_POST4]	= 0x007c,
-
-	[TXNLCR0]	= 0x0080,
-	[TXALCR0]	= 0x0084,
-	[RXNLCR0]	= 0x0088,
-	[RXALCR0]	= 0x008c,
-	[FWNLCR0]	= 0x0090,
-	[FWALCR0]	= 0x0094,
-	[TXNLCR1]	= 0x00a0,
-	[TXALCR1]	= 0x00a0,
-	[RXNLCR1]	= 0x00a8,
-	[RXALCR1]	= 0x00ac,
-	[FWNLCR1]	= 0x00b0,
-	[FWALCR1]	= 0x00b4,
-
-	[TSU_ADRH0]	= 0x0100,
-	[TSU_ADRL0]	= 0x0104,
-	[TSU_ADRL31]	= 0x01fc,
-
-};
-
 /* Driver's parameters */
 #if defined(CONFIG_CPU_SH4) || defined(CONFIG_ARCH_SHMOBILE)
 #define SH4_SKB_RX_ALIGN	32
@@ -705,7 +486,6 @@ struct sh_eth_private {
 	const u16 *reg_offset;
 	void __iomem *addr;
 	void __iomem *tsu_addr;
-	struct bb_info *bitbang;
 	u32 num_rx_ring;
 	u32 num_tx_ring;
 	dma_addr_t rx_desc_dma;
diff --git a/drivers/net/ethernet/s6gmac.c b/drivers/net/ethernet/s6gmac.c
index 21683e2b1ff4..b6739afeaca1 100644
--- a/drivers/net/ethernet/s6gmac.c
+++ b/drivers/net/ethernet/s6gmac.c
@@ -998,6 +998,7 @@ static int s6gmac_probe(struct platform_device *pdev)
 	mb = mdiobus_alloc();
 	if (!mb) {
 		printk(KERN_ERR DRV_PRMT "error allocating mii bus\n");
+		res = -ENOMEM;
 		goto errmii;
 	}
 	mb->name = "s6gmac_mii";
@@ -1053,20 +1054,7 @@ static struct platform_driver s6gmac_driver = {
 	},
 };
 
-static int __init s6gmac_init(void)
-{
-	printk(KERN_INFO DRV_PRMT "S6 GMAC ethernet driver\n");
-	return platform_driver_register(&s6gmac_driver);
-}
-
-
-static void __exit s6gmac_exit(void)
-{
-	platform_driver_unregister(&s6gmac_driver);
-}
-
-module_init(s6gmac_init);
-module_exit(s6gmac_exit);
+module_platform_driver(s6gmac_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("S6105 on chip Ethernet driver");
diff --git a/drivers/net/ethernet/seeq/ether3.c b/drivers/net/ethernet/seeq/ether3.c
index 3aca57853ed4..bdac936a68bc 100644
--- a/drivers/net/ethernet/seeq/ether3.c
+++ b/drivers/net/ethernet/seeq/ether3.c
@@ -651,8 +651,11 @@ if (next_ptr < RX_START || next_ptr >= RX_END) {
 				skb->protocol = eth_type_trans(skb, dev);
 				netif_rx(skb);
 				received ++;
-			} else
-				goto dropping;
+			} else {
+				ether3_outw(next_ptr >> 8, REG_RECVEND);
+				dev->stats.rx_dropped++;
+				goto done;
+			}
 		} else {
 			struct net_device_stats *stats = &dev->stats;
 			ether3_outw(next_ptr >> 8, REG_RECVEND);
@@ -679,21 +682,6 @@ done:
 	}
 
 	return maxcnt;
-
-dropping:{
-	static unsigned long last_warned;
-
-	ether3_outw(next_ptr >> 8, REG_RECVEND);
-	/*
-	 * Don't print this message too many times...
-	 */
-	if (time_after(jiffies, last_warned + 10 * HZ)) {
-		last_warned = jiffies;
-		printk("%s: memory squeeze, dropping packet.\n", dev->name);
-	}
-	dev->stats.rx_dropped++;
-	goto done;
-	}
 }
 
 /*
diff --git a/drivers/net/ethernet/seeq/sgiseeq.c b/drivers/net/ethernet/seeq/sgiseeq.c
index 0fde9ca28269..0ad5694b41f8 100644
--- a/drivers/net/ethernet/seeq/sgiseeq.c
+++ b/drivers/net/ethernet/seeq/sgiseeq.c
@@ -381,8 +381,6 @@ memory_squeeze:
 					dev->stats.rx_packets++;
 					dev->stats.rx_bytes += len;
 				} else {
-					printk(KERN_NOTICE "%s: Memory squeeze, deferring packet.\n",
-						dev->name);
 					dev->stats.rx_dropped++;
 				}
 			} else {
diff --git a/drivers/net/ethernet/sfc/efx.c b/drivers/net/ethernet/sfc/efx.c
index 0bc00991d310..01b99206139a 100644
--- a/drivers/net/ethernet/sfc/efx.c
+++ b/drivers/net/ethernet/sfc/efx.c
@@ -22,6 +22,7 @@
 #include <linux/topology.h>
 #include <linux/gfp.h>
 #include <linux/cpu_rmap.h>
+#include <linux/aer.h>
 #include "net_driver.h"
 #include "efx.h"
 #include "nic.h"
@@ -71,21 +72,21 @@ const char *const efx_loopback_mode_names[] = {
 
 const unsigned int efx_reset_type_max = RESET_TYPE_MAX;
 const char *const efx_reset_type_names[] = {
-	[RESET_TYPE_INVISIBLE]     = "INVISIBLE",
-	[RESET_TYPE_ALL]           = "ALL",
-	[RESET_TYPE_WORLD]         = "WORLD",
-	[RESET_TYPE_DISABLE]       = "DISABLE",
-	[RESET_TYPE_TX_WATCHDOG]   = "TX_WATCHDOG",
-	[RESET_TYPE_INT_ERROR]     = "INT_ERROR",
-	[RESET_TYPE_RX_RECOVERY]   = "RX_RECOVERY",
-	[RESET_TYPE_RX_DESC_FETCH] = "RX_DESC_FETCH",
-	[RESET_TYPE_TX_DESC_FETCH] = "TX_DESC_FETCH",
-	[RESET_TYPE_TX_SKIP]       = "TX_SKIP",
-	[RESET_TYPE_MC_FAILURE]    = "MC_FAILURE",
+	[RESET_TYPE_INVISIBLE]          = "INVISIBLE",
+	[RESET_TYPE_ALL]                = "ALL",
+	[RESET_TYPE_RECOVER_OR_ALL]     = "RECOVER_OR_ALL",
+	[RESET_TYPE_WORLD]              = "WORLD",
+	[RESET_TYPE_RECOVER_OR_DISABLE] = "RECOVER_OR_DISABLE",
+	[RESET_TYPE_DISABLE]            = "DISABLE",
+	[RESET_TYPE_TX_WATCHDOG]        = "TX_WATCHDOG",
+	[RESET_TYPE_INT_ERROR]          = "INT_ERROR",
+	[RESET_TYPE_RX_RECOVERY]        = "RX_RECOVERY",
+	[RESET_TYPE_RX_DESC_FETCH]      = "RX_DESC_FETCH",
+	[RESET_TYPE_TX_DESC_FETCH]      = "TX_DESC_FETCH",
+	[RESET_TYPE_TX_SKIP]            = "TX_SKIP",
+	[RESET_TYPE_MC_FAILURE]         = "MC_FAILURE",
 };
 
-#define EFX_MAX_MTU (9 * 1024)
-
 /* Reset workqueue. If any NIC has a hardware failure then a reset will be
  * queued onto this work queue. This is not a per-nic work queue, because
  * efx_reset_work() acquires the rtnl lock, so resets are naturally serialised.
@@ -117,9 +118,12 @@ MODULE_PARM_DESC(separate_tx_channels,
 static int napi_weight = 64;
 
 /* This is the time (in jiffies) between invocations of the hardware
- * monitor.  On Falcon-based NICs, this will:
+ * monitor.
+ * On Falcon-based NICs, this will:
  * - Check the on-board hardware monitor;
  * - Poll the link state and reconfigure the hardware as necessary.
+ * On Siena-based NICs for power systems with EEH support, this will give EEH a
+ * chance to start.
  */
 static unsigned int efx_monitor_interval = 1 * HZ;
 
@@ -203,13 +207,14 @@ static void efx_stop_all(struct efx_nic *efx);
 #define EFX_ASSERT_RESET_SERIALISED(efx)		\
 	do {						\
 		if ((efx->state == STATE_READY) ||	\
+		    (efx->state == STATE_RECOVERY) ||	\
 		    (efx->state == STATE_DISABLED))	\
 			ASSERT_RTNL();			\
 	} while (0)
 
 static int efx_check_disabled(struct efx_nic *efx)
 {
-	if (efx->state == STATE_DISABLED) {
+	if (efx->state == STATE_DISABLED || efx->state == STATE_RECOVERY) {
 		netif_err(efx, drv, efx->net_dev,
 			  "device is disabled due to earlier errors\n");
 		return -EIO;
@@ -242,15 +247,9 @@ static int efx_process_channel(struct efx_channel *channel, int budget)
 		struct efx_rx_queue *rx_queue =
 			efx_channel_get_rx_queue(channel);
 
-		/* Deliver last RX packet. */
-		if (channel->rx_pkt) {
-			__efx_rx_packet(channel, channel->rx_pkt);
-			channel->rx_pkt = NULL;
-		}
-		if (rx_queue->enabled) {
-			efx_rx_strategy(channel);
+		efx_rx_flush_packet(channel);
+		if (rx_queue->enabled)
 			efx_fast_push_rx_descriptors(rx_queue);
-		}
 	}
 
 	return spent;
@@ -625,20 +624,51 @@ fail:
  */
 static void efx_start_datapath(struct efx_nic *efx)
 {
+	bool old_rx_scatter = efx->rx_scatter;
 	struct efx_tx_queue *tx_queue;
 	struct efx_rx_queue *rx_queue;
 	struct efx_channel *channel;
+	size_t rx_buf_len;
 
 	/* Calculate the rx buffer allocation parameters required to
 	 * support the current MTU, including padding for header
 	 * alignment and overruns.
 	 */
-	efx->rx_buffer_len = (max(EFX_PAGE_IP_ALIGN, NET_IP_ALIGN) +
-			      EFX_MAX_FRAME_LEN(efx->net_dev->mtu) +
-			      efx->type->rx_buffer_hash_size +
-			      efx->type->rx_buffer_padding);
-	efx->rx_buffer_order = get_order(efx->rx_buffer_len +
-					 sizeof(struct efx_rx_page_state));
+	efx->rx_dma_len = (efx->type->rx_buffer_hash_size +
+			   EFX_MAX_FRAME_LEN(efx->net_dev->mtu) +
+			   efx->type->rx_buffer_padding);
+	rx_buf_len = (sizeof(struct efx_rx_page_state) +
+		      EFX_PAGE_IP_ALIGN + efx->rx_dma_len);
+	if (rx_buf_len <= PAGE_SIZE) {
+		efx->rx_scatter = false;
+		efx->rx_buffer_order = 0;
+	} else if (efx->type->can_rx_scatter) {
+		BUILD_BUG_ON(sizeof(struct efx_rx_page_state) +
+			     EFX_PAGE_IP_ALIGN + EFX_RX_USR_BUF_SIZE >
+			     PAGE_SIZE / 2);
+		efx->rx_scatter = true;
+		efx->rx_dma_len = EFX_RX_USR_BUF_SIZE;
+		efx->rx_buffer_order = 0;
+	} else {
+		efx->rx_scatter = false;
+		efx->rx_buffer_order = get_order(rx_buf_len);
+	}
+
+	efx_rx_config_page_split(efx);
+	if (efx->rx_buffer_order)
+		netif_dbg(efx, drv, efx->net_dev,
+			  "RX buf len=%u; page order=%u batch=%u\n",
+			  efx->rx_dma_len, efx->rx_buffer_order,
+			  efx->rx_pages_per_batch);
+	else
+		netif_dbg(efx, drv, efx->net_dev,
+			  "RX buf len=%u step=%u bpp=%u; page batch=%u\n",
+			  efx->rx_dma_len, efx->rx_page_buf_step,
+			  efx->rx_bufs_per_page, efx->rx_pages_per_batch);
+
+	/* RX filters also have scatter-enabled flags */
+	if (efx->rx_scatter != old_rx_scatter)
+		efx_filter_update_rx_scatter(efx);
 
 	/* We must keep at least one descriptor in a TX ring empty.
 	 * We could avoid this when the queue size does not exactly
@@ -655,16 +685,12 @@ static void efx_start_datapath(struct efx_nic *efx)
 		efx_for_each_channel_tx_queue(tx_queue, channel)
 			efx_init_tx_queue(tx_queue);
 
-		/* The rx buffer allocation strategy is MTU dependent */
-		efx_rx_strategy(channel);
-
 		efx_for_each_channel_rx_queue(rx_queue, channel) {
 			efx_init_rx_queue(rx_queue);
 			efx_nic_generate_fill_event(rx_queue);
 		}
 
-		WARN_ON(channel->rx_pkt != NULL);
-		efx_rx_strategy(channel);
+		WARN_ON(channel->rx_pkt_n_frags);
 	}
 
 	if (netif_device_present(efx->net_dev))
@@ -683,7 +709,7 @@ static void efx_stop_datapath(struct efx_nic *efx)
 	BUG_ON(efx->port_enabled);
 
 	/* Only perform flush if dma is enabled */
-	if (dev->is_busmaster) {
+	if (dev->is_busmaster && efx->state != STATE_RECOVERY) {
 		rc = efx_nic_flush_queues(efx);
 
 		if (rc && EFX_WORKAROUND_7803(efx)) {
@@ -1596,13 +1622,15 @@ static void efx_start_all(struct efx_nic *efx)
 	efx_start_port(efx);
 	efx_start_datapath(efx);
 
-	/* Start the hardware monitor if there is one. Otherwise (we're link
-	 * event driven), we have to poll the PHY because after an event queue
-	 * flush, we could have a missed a link state change */
-	if (efx->type->monitor != NULL) {
+	/* Start the hardware monitor if there is one */
+	if (efx->type->monitor != NULL)
 		queue_delayed_work(efx->workqueue, &efx->monitor_work,
 				   efx_monitor_interval);
-	} else {
+
+	/* If link state detection is normally event-driven, we have
+	 * to poll now because we could have missed a change
+	 */
+	if (efx_nic_rev(efx) >= EFX_REV_SIENA_A0) {
 		mutex_lock(&efx->mac_lock);
 		if (efx->phy_op->poll(efx))
 			efx_link_status_changed(efx);
@@ -2309,7 +2337,9 @@ int efx_reset(struct efx_nic *efx, enum reset_type method)
 
 out:
 	/* Leave device stopped if necessary */
-	disabled = rc || method == RESET_TYPE_DISABLE;
+	disabled = rc ||
+		method == RESET_TYPE_DISABLE ||
+		method == RESET_TYPE_RECOVER_OR_DISABLE;
 	rc2 = efx_reset_up(efx, method, !disabled);
 	if (rc2) {
 		disabled = true;
@@ -2328,13 +2358,48 @@ out:
 	return rc;
 }
 
+/* Try recovery mechanisms.
+ * For now only EEH is supported.
+ * Returns 0 if the recovery mechanisms are unsuccessful.
+ * Returns a non-zero value otherwise.
+ */
+static int efx_try_recovery(struct efx_nic *efx)
+{
+#ifdef CONFIG_EEH
+	/* A PCI error can occur and not be seen by EEH because nothing
+	 * happens on the PCI bus. In this case the driver may fail and
+	 * schedule a 'recover or reset', leading to this recovery handler.
+	 * Manually call the eeh failure check function.
+	 */
+	struct eeh_dev *eehdev =
+		of_node_to_eeh_dev(pci_device_to_OF_node(efx->pci_dev));
+
+	if (eeh_dev_check_failure(eehdev)) {
+		/* The EEH mechanisms will handle the error and reset the
+		 * device if necessary.
+		 */
+		return 1;
+	}
+#endif
+	return 0;
+}
+
 /* The worker thread exists so that code that cannot sleep can
  * schedule a reset for later.
  */
 static void efx_reset_work(struct work_struct *data)
 {
 	struct efx_nic *efx = container_of(data, struct efx_nic, reset_work);
-	unsigned long pending = ACCESS_ONCE(efx->reset_pending);
+	unsigned long pending;
+	enum reset_type method;
+
+	pending = ACCESS_ONCE(efx->reset_pending);
+	method = fls(pending) - 1;
+
+	if ((method == RESET_TYPE_RECOVER_OR_DISABLE ||
+	     method == RESET_TYPE_RECOVER_OR_ALL) &&
+	    efx_try_recovery(efx))
+		return;
 
 	if (!pending)
 		return;
@@ -2346,7 +2411,7 @@ static void efx_reset_work(struct work_struct *data)
 	 * it cannot change again.
 	 */
 	if (efx->state == STATE_READY)
-		(void)efx_reset(efx, fls(pending) - 1);
+		(void)efx_reset(efx, method);
 
 	rtnl_unlock();
 }
@@ -2355,11 +2420,20 @@ void efx_schedule_reset(struct efx_nic *efx, enum reset_type type)
 {
 	enum reset_type method;
 
+	if (efx->state == STATE_RECOVERY) {
+		netif_dbg(efx, drv, efx->net_dev,
+			  "recovering: skip scheduling %s reset\n",
+			  RESET_TYPE(type));
+		return;
+	}
+
 	switch (type) {
 	case RESET_TYPE_INVISIBLE:
 	case RESET_TYPE_ALL:
+	case RESET_TYPE_RECOVER_OR_ALL:
 	case RESET_TYPE_WORLD:
 	case RESET_TYPE_DISABLE:
+	case RESET_TYPE_RECOVER_OR_DISABLE:
 		method = type;
 		netif_dbg(efx, drv, efx->net_dev, "scheduling %s reset\n",
 			  RESET_TYPE(method));
@@ -2569,6 +2643,8 @@ static void efx_pci_remove(struct pci_dev *pci_dev)
 	efx_fini_struct(efx);
 	pci_set_drvdata(pci_dev, NULL);
 	free_netdev(efx->net_dev);
+
+	pci_disable_pcie_error_reporting(pci_dev);
 };
 
 /* NIC VPD information
@@ -2741,6 +2817,11 @@ static int efx_pci_probe(struct pci_dev *pci_dev,
 		netif_warn(efx, probe, efx->net_dev,
 			   "failed to create MTDs (%d)\n", rc);
 
+	rc = pci_enable_pcie_error_reporting(pci_dev);
+	if (rc && rc != -EINVAL)
+		netif_warn(efx, probe, efx->net_dev,
+			   "pci_enable_pcie_error_reporting failed (%d)\n", rc);
+
 	return 0;
 
  fail4:
@@ -2865,12 +2946,112 @@ static const struct dev_pm_ops efx_pm_ops = {
 	.restore	= efx_pm_resume,
 };
 
+/* A PCI error affecting this device was detected.
+ * At this point MMIO and DMA may be disabled.
+ * Stop the software path and request a slot reset.
+ */
+static pci_ers_result_t efx_io_error_detected(struct pci_dev *pdev,
+					      enum pci_channel_state state)
+{
+	pci_ers_result_t status = PCI_ERS_RESULT_RECOVERED;
+	struct efx_nic *efx = pci_get_drvdata(pdev);
+
+	if (state == pci_channel_io_perm_failure)
+		return PCI_ERS_RESULT_DISCONNECT;
+
+	rtnl_lock();
+
+	if (efx->state != STATE_DISABLED) {
+		efx->state = STATE_RECOVERY;
+		efx->reset_pending = 0;
+
+		efx_device_detach_sync(efx);
+
+		efx_stop_all(efx);
+		efx_stop_interrupts(efx, false);
+
+		status = PCI_ERS_RESULT_NEED_RESET;
+	} else {
+		/* If the interface is disabled we don't want to do anything
+		 * with it.
+		 */
+		status = PCI_ERS_RESULT_RECOVERED;
+	}
+
+	rtnl_unlock();
+
+	pci_disable_device(pdev);
+
+	return status;
+}
+
+/* Fake a successfull reset, which will be performed later in efx_io_resume. */
+static pci_ers_result_t efx_io_slot_reset(struct pci_dev *pdev)
+{
+	struct efx_nic *efx = pci_get_drvdata(pdev);
+	pci_ers_result_t status = PCI_ERS_RESULT_RECOVERED;
+	int rc;
+
+	if (pci_enable_device(pdev)) {
+		netif_err(efx, hw, efx->net_dev,
+			  "Cannot re-enable PCI device after reset.\n");
+		status =  PCI_ERS_RESULT_DISCONNECT;
+	}
+
+	rc = pci_cleanup_aer_uncorrect_error_status(pdev);
+	if (rc) {
+		netif_err(efx, hw, efx->net_dev,
+		"pci_cleanup_aer_uncorrect_error_status failed (%d)\n", rc);
+		/* Non-fatal error. Continue. */
+	}
+
+	return status;
+}
+
+/* Perform the actual reset and resume I/O operations. */
+static void efx_io_resume(struct pci_dev *pdev)
+{
+	struct efx_nic *efx = pci_get_drvdata(pdev);
+	int rc;
+
+	rtnl_lock();
+
+	if (efx->state == STATE_DISABLED)
+		goto out;
+
+	rc = efx_reset(efx, RESET_TYPE_ALL);
+	if (rc) {
+		netif_err(efx, hw, efx->net_dev,
+			  "efx_reset failed after PCI error (%d)\n", rc);
+	} else {
+		efx->state = STATE_READY;
+		netif_dbg(efx, hw, efx->net_dev,
+			  "Done resetting and resuming IO after PCI error.\n");
+	}
+
+out:
+	rtnl_unlock();
+}
+
+/* For simplicity and reliability, we always require a slot reset and try to
+ * reset the hardware when a pci error affecting the device is detected.
+ * We leave both the link_reset and mmio_enabled callback unimplemented:
+ * with our request for slot reset the mmio_enabled callback will never be
+ * called, and the link_reset callback is not used by AER or EEH mechanisms.
+ */
+static struct pci_error_handlers efx_err_handlers = {
+	.error_detected = efx_io_error_detected,
+	.slot_reset	= efx_io_slot_reset,
+	.resume		= efx_io_resume,
+};
+
 static struct pci_driver efx_pci_driver = {
 	.name		= KBUILD_MODNAME,
 	.id_table	= efx_pci_table,
 	.probe		= efx_pci_probe,
 	.remove		= efx_pci_remove,
 	.driver.pm	= &efx_pm_ops,
+	.err_handler	= &efx_err_handlers,
 };
 
 /**************************************************************************
diff --git a/drivers/net/ethernet/sfc/efx.h b/drivers/net/ethernet/sfc/efx.h
index d2f790df6dcb..8372da239b43 100644
--- a/drivers/net/ethernet/sfc/efx.h
+++ b/drivers/net/ethernet/sfc/efx.h
@@ -33,17 +33,22 @@ extern int efx_setup_tc(struct net_device *net_dev, u8 num_tc);
 extern unsigned int efx_tx_max_skb_descs(struct efx_nic *efx);
 
 /* RX */
+extern void efx_rx_config_page_split(struct efx_nic *efx);
 extern int efx_probe_rx_queue(struct efx_rx_queue *rx_queue);
 extern void efx_remove_rx_queue(struct efx_rx_queue *rx_queue);
 extern void efx_init_rx_queue(struct efx_rx_queue *rx_queue);
 extern void efx_fini_rx_queue(struct efx_rx_queue *rx_queue);
-extern void efx_rx_strategy(struct efx_channel *channel);
 extern void efx_fast_push_rx_descriptors(struct efx_rx_queue *rx_queue);
 extern void efx_rx_slow_fill(unsigned long context);
-extern void __efx_rx_packet(struct efx_channel *channel,
-			    struct efx_rx_buffer *rx_buf);
-extern void efx_rx_packet(struct efx_rx_queue *rx_queue, unsigned int index,
+extern void __efx_rx_packet(struct efx_channel *channel);
+extern void efx_rx_packet(struct efx_rx_queue *rx_queue,
+			  unsigned int index, unsigned int n_frags,
 			  unsigned int len, u16 flags);
+static inline void efx_rx_flush_packet(struct efx_channel *channel)
+{
+	if (channel->rx_pkt_n_frags)
+		__efx_rx_packet(channel);
+}
 extern void efx_schedule_slow_fill(struct efx_rx_queue *rx_queue);
 
 #define EFX_MAX_DMAQ_SIZE 4096UL
@@ -67,6 +72,7 @@ extern void efx_schedule_slow_fill(struct efx_rx_queue *rx_queue);
 extern int efx_probe_filters(struct efx_nic *efx);
 extern void efx_restore_filters(struct efx_nic *efx);
 extern void efx_remove_filters(struct efx_nic *efx);
+extern void efx_filter_update_rx_scatter(struct efx_nic *efx);
 extern s32 efx_filter_insert_filter(struct efx_nic *efx,
 				    struct efx_filter_spec *spec,
 				    bool replace);
diff --git a/drivers/net/ethernet/sfc/enum.h b/drivers/net/ethernet/sfc/enum.h
index 182dbe2cc6e4..ab8fb5889e55 100644
--- a/drivers/net/ethernet/sfc/enum.h
+++ b/drivers/net/ethernet/sfc/enum.h
@@ -137,8 +137,12 @@ enum efx_loopback_mode {
  * Reset methods are numbered in order of increasing scope.
  *
  * @RESET_TYPE_INVISIBLE: Reset datapath and MAC (Falcon only)
+ * @RESET_TYPE_RECOVER_OR_ALL: Try to recover. Apply RESET_TYPE_ALL
+ * if unsuccessful.
  * @RESET_TYPE_ALL: Reset datapath, MAC and PHY
  * @RESET_TYPE_WORLD: Reset as much as possible
+ * @RESET_TYPE_RECOVER_OR_DISABLE: Try to recover. Apply RESET_TYPE_DISABLE if
+ * unsuccessful.
  * @RESET_TYPE_DISABLE: Reset datapath, MAC and PHY; leave NIC disabled
  * @RESET_TYPE_TX_WATCHDOG: reset due to TX watchdog
  * @RESET_TYPE_INT_ERROR: reset due to internal error
@@ -150,9 +154,11 @@ enum efx_loopback_mode {
  */
 enum reset_type {
 	RESET_TYPE_INVISIBLE = 0,
-	RESET_TYPE_ALL = 1,
-	RESET_TYPE_WORLD = 2,
-	RESET_TYPE_DISABLE = 3,
+	RESET_TYPE_RECOVER_OR_ALL = 1,
+	RESET_TYPE_ALL = 2,
+	RESET_TYPE_WORLD = 3,
+	RESET_TYPE_RECOVER_OR_DISABLE = 4,
+	RESET_TYPE_DISABLE = 5,
 	RESET_TYPE_MAX_METHOD,
 	RESET_TYPE_TX_WATCHDOG,
 	RESET_TYPE_INT_ERROR,
diff --git a/drivers/net/ethernet/sfc/ethtool.c b/drivers/net/ethernet/sfc/ethtool.c
index 8e61cd06f66a..6e768175e7e0 100644
--- a/drivers/net/ethernet/sfc/ethtool.c
+++ b/drivers/net/ethernet/sfc/ethtool.c
@@ -154,6 +154,7 @@ static const struct efx_ethtool_stat efx_ethtool_stats[] = {
 	EFX_ETHTOOL_UINT_CHANNEL_STAT(rx_tcp_udp_chksum_err),
 	EFX_ETHTOOL_UINT_CHANNEL_STAT(rx_mcast_mismatch),
 	EFX_ETHTOOL_UINT_CHANNEL_STAT(rx_frm_trunc),
+	EFX_ETHTOOL_UINT_CHANNEL_STAT(rx_nodesc_trunc),
 };
 
 /* Number of ethtool statistics */
@@ -978,7 +979,8 @@ static int efx_ethtool_set_class_rule(struct efx_nic *efx,
 	     rule->m_ext.data[1]))
 		return -EINVAL;
 
-	efx_filter_init_rx(&spec, EFX_FILTER_PRI_MANUAL, 0,
+	efx_filter_init_rx(&spec, EFX_FILTER_PRI_MANUAL,
+			   efx->rx_scatter ? EFX_FILTER_FLAG_RX_SCATTER : 0,
 			   (rule->ring_cookie == RX_CLS_FLOW_DISC) ?
 			   0xfff : rule->ring_cookie);
 
diff --git a/drivers/net/ethernet/sfc/falcon.c b/drivers/net/ethernet/sfc/falcon.c
index 49bcd196e10d..71998e7995d9 100644
--- a/drivers/net/ethernet/sfc/falcon.c
+++ b/drivers/net/ethernet/sfc/falcon.c
@@ -1528,7 +1528,7 @@ static int falcon_probe_nic(struct efx_nic *efx)
 	return 0;
 
  fail6:
-	BUG_ON(i2c_del_adapter(&board->i2c_adap));
+	i2c_del_adapter(&board->i2c_adap);
 	memset(&board->i2c_adap, 0, sizeof(board->i2c_adap));
  fail5:
 	efx_nic_free_buffer(efx, &efx->irq_status);
@@ -1546,10 +1546,6 @@ static int falcon_probe_nic(struct efx_nic *efx)
 
 static void falcon_init_rx_cfg(struct efx_nic *efx)
 {
-	/* Prior to Siena the RX DMA engine will split each frame at
-	 * intervals of RX_USR_BUF_SIZE (32-byte units). We set it to
-	 * be so large that that never happens. */
-	const unsigned huge_buf_size = (3 * 4096) >> 5;
 	/* RX control FIFO thresholds (32 entries) */
 	const unsigned ctrl_xon_thr = 20;
 	const unsigned ctrl_xoff_thr = 25;
@@ -1557,10 +1553,15 @@ static void falcon_init_rx_cfg(struct efx_nic *efx)
 
 	efx_reado(efx, &reg, FR_AZ_RX_CFG);
 	if (efx_nic_rev(efx) <= EFX_REV_FALCON_A1) {
-		/* Data FIFO size is 5.5K */
+		/* Data FIFO size is 5.5K.  The RX DMA engine only
+		 * supports scattering for user-mode queues, but will
+		 * split DMA writes at intervals of RX_USR_BUF_SIZE
+		 * (32-byte units) even for kernel-mode queues.  We
+		 * set it to be so large that that never happens.
+		 */
 		EFX_SET_OWORD_FIELD(reg, FRF_AA_RX_DESC_PUSH_EN, 0);
 		EFX_SET_OWORD_FIELD(reg, FRF_AA_RX_USR_BUF_SIZE,
-				    huge_buf_size);
+				    (3 * 4096) >> 5);
 		EFX_SET_OWORD_FIELD(reg, FRF_AA_RX_XON_MAC_TH, 512 >> 8);
 		EFX_SET_OWORD_FIELD(reg, FRF_AA_RX_XOFF_MAC_TH, 2048 >> 8);
 		EFX_SET_OWORD_FIELD(reg, FRF_AA_RX_XON_TX_TH, ctrl_xon_thr);
@@ -1569,7 +1570,7 @@ static void falcon_init_rx_cfg(struct efx_nic *efx)
 		/* Data FIFO size is 80K; register fields moved */
 		EFX_SET_OWORD_FIELD(reg, FRF_BZ_RX_DESC_PUSH_EN, 0);
 		EFX_SET_OWORD_FIELD(reg, FRF_BZ_RX_USR_BUF_SIZE,
-				    huge_buf_size);
+				    EFX_RX_USR_BUF_SIZE >> 5);
 		/* Send XON and XOFF at ~3 * max MTU away from empty/full */
 		EFX_SET_OWORD_FIELD(reg, FRF_BZ_RX_XON_MAC_TH, 27648 >> 8);
 		EFX_SET_OWORD_FIELD(reg, FRF_BZ_RX_XOFF_MAC_TH, 54272 >> 8);
@@ -1665,13 +1666,11 @@ static void falcon_remove_nic(struct efx_nic *efx)
 {
 	struct falcon_nic_data *nic_data = efx->nic_data;
 	struct falcon_board *board = falcon_board(efx);
-	int rc;
 
 	board->type->fini(efx);
 
 	/* Remove I2C adapter and clear it in preparation for a retry */
-	rc = i2c_del_adapter(&board->i2c_adap);
-	BUG_ON(rc);
+	i2c_del_adapter(&board->i2c_adap);
 	memset(&board->i2c_adap, 0, sizeof(board->i2c_adap));
 
 	efx_nic_free_buffer(efx, &efx->irq_status);
@@ -1815,6 +1814,7 @@ const struct efx_nic_type falcon_a1_nic_type = {
 	.evq_rptr_tbl_base = FR_AA_EVQ_RPTR_KER,
 	.max_dma_mask = DMA_BIT_MASK(FSF_AZ_TX_KER_BUF_ADDR_WIDTH),
 	.rx_buffer_padding = 0x24,
+	.can_rx_scatter = false,
 	.max_interrupt_mode = EFX_INT_MODE_MSI,
 	.phys_addr_channels = 4,
 	.timer_period_max =  1 << FRF_AB_TC_TIMER_VAL_WIDTH,
@@ -1865,6 +1865,7 @@ const struct efx_nic_type falcon_b0_nic_type = {
 	.max_dma_mask = DMA_BIT_MASK(FSF_AZ_TX_KER_BUF_ADDR_WIDTH),
 	.rx_buffer_hash_size = 0x10,
 	.rx_buffer_padding = 0,
+	.can_rx_scatter = true,
 	.max_interrupt_mode = EFX_INT_MODE_MSIX,
 	.phys_addr_channels = 32, /* Hardware limit is 64, but the legacy
 				   * interrupt handler only supports 32
diff --git a/drivers/net/ethernet/sfc/filter.c b/drivers/net/ethernet/sfc/filter.c
index 8af42cd1feda..2397f0e8d3eb 100644
--- a/drivers/net/ethernet/sfc/filter.c
+++ b/drivers/net/ethernet/sfc/filter.c
@@ -66,6 +66,10 @@ struct efx_filter_state {
 #endif
 };
 
+static void efx_filter_table_clear_entry(struct efx_nic *efx,
+					 struct efx_filter_table *table,
+					 unsigned int filter_idx);
+
 /* The filter hash function is LFSR polynomial x^16 + x^3 + 1 of a 32-bit
  * key derived from the n-tuple.  The initial LFSR state is 0xffff. */
 static u16 efx_filter_hash(u32 key)
@@ -168,6 +172,25 @@ static void efx_filter_push_rx_config(struct efx_nic *efx)
 			filter_ctl, FRF_CZ_MULTICAST_NOMATCH_RSS_ENABLED,
 			!!(table->spec[EFX_FILTER_INDEX_MC_DEF].flags &
 			   EFX_FILTER_FLAG_RX_RSS));
+
+		/* There is a single bit to enable RX scatter for all
+		 * unmatched packets.  Only set it if scatter is
+		 * enabled in both filter specs.
+		 */
+		EFX_SET_OWORD_FIELD(
+			filter_ctl, FRF_BZ_SCATTER_ENBL_NO_MATCH_Q,
+			!!(table->spec[EFX_FILTER_INDEX_UC_DEF].flags &
+			   table->spec[EFX_FILTER_INDEX_MC_DEF].flags &
+			   EFX_FILTER_FLAG_RX_SCATTER));
+	} else if (efx_nic_rev(efx) >= EFX_REV_FALCON_B0) {
+		/* We don't expose 'default' filters because unmatched
+		 * packets always go to the queue number found in the
+		 * RSS table.  But we still need to set the RX scatter
+		 * bit here.
+		 */
+		EFX_SET_OWORD_FIELD(
+			filter_ctl, FRF_BZ_SCATTER_ENBL_NO_MATCH_Q,
+			efx->rx_scatter);
 	}
 
 	efx_writeo(efx, &filter_ctl, FR_BZ_RX_FILTER_CTL);
@@ -409,9 +432,18 @@ static void efx_filter_reset_rx_def(struct efx_nic *efx, unsigned filter_idx)
 	struct efx_filter_state *state = efx->filter_state;
 	struct efx_filter_table *table = &state->table[EFX_FILTER_TABLE_RX_DEF];
 	struct efx_filter_spec *spec = &table->spec[filter_idx];
+	enum efx_filter_flags flags = 0;
+
+	/* If there's only one channel then disable RSS for non VF
+	 * traffic, thereby allowing VFs to use RSS when the PF can't.
+	 */
+	if (efx->n_rx_channels > 1)
+		flags |= EFX_FILTER_FLAG_RX_RSS;
 
-	efx_filter_init_rx(spec, EFX_FILTER_PRI_MANUAL,
-			   EFX_FILTER_FLAG_RX_RSS, 0);
+	if (efx->rx_scatter)
+		flags |= EFX_FILTER_FLAG_RX_SCATTER;
+
+	efx_filter_init_rx(spec, EFX_FILTER_PRI_MANUAL, flags, 0);
 	spec->type = EFX_FILTER_UC_DEF + filter_idx;
 	table->used_bitmap[0] |= 1 << filter_idx;
 }
@@ -463,13 +495,6 @@ static u32 efx_filter_build(efx_oword_t *filter, struct efx_filter_spec *spec)
 		break;
 	}
 
-	case EFX_FILTER_TABLE_RX_DEF:
-		/* One filter spec per type */
-		BUILD_BUG_ON(EFX_FILTER_INDEX_UC_DEF != 0);
-		BUILD_BUG_ON(EFX_FILTER_INDEX_MC_DEF !=
-			     EFX_FILTER_MC_DEF - EFX_FILTER_UC_DEF);
-		return spec->type - EFX_FILTER_UC_DEF;
-
 	case EFX_FILTER_TABLE_RX_MAC: {
 		bool is_wild = spec->type == EFX_FILTER_MAC_WILD;
 		EFX_POPULATE_OWORD_7(
@@ -520,42 +545,6 @@ static bool efx_filter_equal(const struct efx_filter_spec *left,
 	return true;
 }
 
-static int efx_filter_search(struct efx_filter_table *table,
-			     struct efx_filter_spec *spec, u32 key,
-			     bool for_insert, unsigned int *depth_required)
-{
-	unsigned hash, incr, filter_idx, depth, depth_max;
-
-	hash = efx_filter_hash(key);
-	incr = efx_filter_increment(key);
-
-	filter_idx = hash & (table->size - 1);
-	depth = 1;
-	depth_max = (for_insert ?
-		     (spec->priority <= EFX_FILTER_PRI_HINT ?
-		      FILTER_CTL_SRCH_HINT_MAX : FILTER_CTL_SRCH_MAX) :
-		     table->search_depth[spec->type]);
-
-	for (;;) {
-		/* Return success if entry is used and matches this spec
-		 * or entry is unused and we are trying to insert.
-		 */
-		if (test_bit(filter_idx, table->used_bitmap) ?
-		    efx_filter_equal(spec, &table->spec[filter_idx]) :
-		    for_insert) {
-			*depth_required = depth;
-			return filter_idx;
-		}
-
-		/* Return failure if we reached the maximum search depth */
-		if (depth == depth_max)
-			return for_insert ? -EBUSY : -ENOENT;
-
-		filter_idx = (filter_idx + incr) & (table->size - 1);
-		++depth;
-	}
-}
-
 /*
  * Construct/deconstruct external filter IDs.  At least the RX filter
  * IDs must be ordered by matching priority, for RX NFC semantics.
@@ -650,44 +639,111 @@ u32 efx_filter_get_rx_id_limit(struct efx_nic *efx)
  * efx_filter_insert_filter - add or replace a filter
  * @efx: NIC in which to insert the filter
  * @spec: Specification for the filter
- * @replace: Flag for whether the specified filter may replace a filter
- *	with an identical match expression and equal or lower priority
+ * @replace_equal: Flag for whether the specified filter may replace an
+ *	existing filter with equal priority
  *
  * On success, return the filter ID.
  * On failure, return a negative error code.
+ *
+ * If an existing filter has equal match values to the new filter
+ * spec, then the new filter might replace it, depending on the
+ * relative priorities.  If the existing filter has lower priority, or
+ * if @replace_equal is set and it has equal priority, then it is
+ * replaced.  Otherwise the function fails, returning -%EPERM if
+ * the existing filter has higher priority or -%EEXIST if it has
+ * equal priority.
  */
 s32 efx_filter_insert_filter(struct efx_nic *efx, struct efx_filter_spec *spec,
-			     bool replace)
+			     bool replace_equal)
 {
 	struct efx_filter_state *state = efx->filter_state;
 	struct efx_filter_table *table = efx_filter_spec_table(state, spec);
-	struct efx_filter_spec *saved_spec;
 	efx_oword_t filter;
-	unsigned int filter_idx, depth = 0;
-	u32 key;
+	int rep_index, ins_index;
+	unsigned int depth = 0;
 	int rc;
 
 	if (!table || table->size == 0)
 		return -EINVAL;
 
-	key = efx_filter_build(&filter, spec);
-
 	netif_vdbg(efx, hw, efx->net_dev,
 		   "%s: type %d search_depth=%d", __func__, spec->type,
 		   table->search_depth[spec->type]);
 
-	spin_lock_bh(&state->lock);
+	if (table->id == EFX_FILTER_TABLE_RX_DEF) {
+		/* One filter spec per type */
+		BUILD_BUG_ON(EFX_FILTER_INDEX_UC_DEF != 0);
+		BUILD_BUG_ON(EFX_FILTER_INDEX_MC_DEF !=
+			     EFX_FILTER_MC_DEF - EFX_FILTER_UC_DEF);
+		rep_index = spec->type - EFX_FILTER_INDEX_UC_DEF;
+		ins_index = rep_index;
 
-	rc = efx_filter_search(table, spec, key, true, &depth);
-	if (rc < 0)
-		goto out;
-	filter_idx = rc;
-	BUG_ON(filter_idx >= table->size);
-	saved_spec = &table->spec[filter_idx];
-
-	if (test_bit(filter_idx, table->used_bitmap)) {
-		/* Should we replace the existing filter? */
-		if (!replace) {
+		spin_lock_bh(&state->lock);
+	} else {
+		/* Search concurrently for
+		 * (1) a filter to be replaced (rep_index): any filter
+		 *     with the same match values, up to the current
+		 *     search depth for this type, and
+		 * (2) the insertion point (ins_index): (1) or any
+		 *     free slot before it or up to the maximum search
+		 *     depth for this priority
+		 * We fail if we cannot find (2).
+		 *
+		 * We can stop once either
+		 * (a) we find (1), in which case we have definitely
+		 *     found (2) as well; or
+		 * (b) we have searched exhaustively for (1), and have
+		 *     either found (2) or searched exhaustively for it
+		 */
+		u32 key = efx_filter_build(&filter, spec);
+		unsigned int hash = efx_filter_hash(key);
+		unsigned int incr = efx_filter_increment(key);
+		unsigned int max_rep_depth = table->search_depth[spec->type];
+		unsigned int max_ins_depth =
+			spec->priority <= EFX_FILTER_PRI_HINT ?
+			FILTER_CTL_SRCH_HINT_MAX : FILTER_CTL_SRCH_MAX;
+		unsigned int i = hash & (table->size - 1);
+
+		ins_index = -1;
+		depth = 1;
+
+		spin_lock_bh(&state->lock);
+
+		for (;;) {
+			if (!test_bit(i, table->used_bitmap)) {
+				if (ins_index < 0)
+					ins_index = i;
+			} else if (efx_filter_equal(spec, &table->spec[i])) {
+				/* Case (a) */
+				if (ins_index < 0)
+					ins_index = i;
+				rep_index = i;
+				break;
+			}
+
+			if (depth >= max_rep_depth &&
+			    (ins_index >= 0 || depth >= max_ins_depth)) {
+				/* Case (b) */
+				if (ins_index < 0) {
+					rc = -EBUSY;
+					goto out;
+				}
+				rep_index = -1;
+				break;
+			}
+
+			i = (i + incr) & (table->size - 1);
+			++depth;
+		}
+	}
+
+	/* If we found a filter to be replaced, check whether we
+	 * should do so
+	 */
+	if (rep_index >= 0) {
+		struct efx_filter_spec *saved_spec = &table->spec[rep_index];
+
+		if (spec->priority == saved_spec->priority && !replace_equal) {
 			rc = -EEXIST;
 			goto out;
 		}
@@ -695,11 +751,14 @@ s32 efx_filter_insert_filter(struct efx_nic *efx, struct efx_filter_spec *spec,
 			rc = -EPERM;
 			goto out;
 		}
-	} else {
-		__set_bit(filter_idx, table->used_bitmap);
+	}
+
+	/* Insert the filter */
+	if (ins_index != rep_index) {
+		__set_bit(ins_index, table->used_bitmap);
 		++table->used;
 	}
-	*saved_spec = *spec;
+	table->spec[ins_index] = *spec;
 
 	if (table->id == EFX_FILTER_TABLE_RX_DEF) {
 		efx_filter_push_rx_config(efx);
@@ -713,13 +772,19 @@ s32 efx_filter_insert_filter(struct efx_nic *efx, struct efx_filter_spec *spec,
 		}
 
 		efx_writeo(efx, &filter,
-			   table->offset + table->step * filter_idx);
+			   table->offset + table->step * ins_index);
+
+		/* If we were able to replace a filter by inserting
+		 * at a lower depth, clear the replaced filter
+		 */
+		if (ins_index != rep_index && rep_index >= 0)
+			efx_filter_table_clear_entry(efx, table, rep_index);
 	}
 
 	netif_vdbg(efx, hw, efx->net_dev,
 		   "%s: filter type %d index %d rxq %u set",
-		   __func__, spec->type, filter_idx, spec->dmaq_id);
-	rc = efx_filter_make_id(spec, filter_idx);
+		   __func__, spec->type, ins_index, spec->dmaq_id);
+	rc = efx_filter_make_id(spec, ins_index);
 
 out:
 	spin_unlock_bh(&state->lock);
@@ -1060,6 +1125,50 @@ void efx_remove_filters(struct efx_nic *efx)
 	kfree(state);
 }
 
+/* Update scatter enable flags for filters pointing to our own RX queues */
+void efx_filter_update_rx_scatter(struct efx_nic *efx)
+{
+	struct efx_filter_state *state = efx->filter_state;
+	enum efx_filter_table_id table_id;
+	struct efx_filter_table *table;
+	efx_oword_t filter;
+	unsigned int filter_idx;
+
+	spin_lock_bh(&state->lock);
+
+	for (table_id = EFX_FILTER_TABLE_RX_IP;
+	     table_id <= EFX_FILTER_TABLE_RX_DEF;
+	     table_id++) {
+		table = &state->table[table_id];
+
+		for (filter_idx = 0; filter_idx < table->size; filter_idx++) {
+			if (!test_bit(filter_idx, table->used_bitmap) ||
+			    table->spec[filter_idx].dmaq_id >=
+			    efx->n_rx_channels)
+				continue;
+
+			if (efx->rx_scatter)
+				table->spec[filter_idx].flags |=
+					EFX_FILTER_FLAG_RX_SCATTER;
+			else
+				table->spec[filter_idx].flags &=
+					~EFX_FILTER_FLAG_RX_SCATTER;
+
+			if (table_id == EFX_FILTER_TABLE_RX_DEF)
+				/* Pushed by efx_filter_push_rx_config() */
+				continue;
+
+			efx_filter_build(&filter, &table->spec[filter_idx]);
+			efx_writeo(efx, &filter,
+				   table->offset + table->step * filter_idx);
+		}
+	}
+
+	efx_filter_push_rx_config(efx);
+
+	spin_unlock_bh(&state->lock);
+}
+
 #ifdef CONFIG_RFS_ACCEL
 
 int efx_filter_rfs(struct net_device *net_dev, const struct sk_buff *skb,
diff --git a/drivers/net/ethernet/sfc/mcdi.c b/drivers/net/ethernet/sfc/mcdi.c
index 0095ce95150b..97dd8f18c001 100644
--- a/drivers/net/ethernet/sfc/mcdi.c
+++ b/drivers/net/ethernet/sfc/mcdi.c
@@ -667,7 +667,7 @@ fail:
 int efx_mcdi_get_board_cfg(struct efx_nic *efx, u8 *mac_address,
 			   u16 *fw_subtype_list, u32 *capabilities)
 {
-	uint8_t outbuf[MC_CMD_GET_BOARD_CFG_OUT_LENMIN];
+	uint8_t outbuf[MC_CMD_GET_BOARD_CFG_OUT_LENMAX];
 	size_t outlen, offset, i;
 	int port_num = efx_port_num(efx);
 	int rc;
diff --git a/drivers/net/ethernet/sfc/mcdi_pcol.h b/drivers/net/ethernet/sfc/mcdi_pcol.h
index 9d426d0457bd..c5c9747861ba 100644
--- a/drivers/net/ethernet/sfc/mcdi_pcol.h
+++ b/drivers/net/ethernet/sfc/mcdi_pcol.h
@@ -553,6 +553,7 @@
 #define          MC_CMD_PTP_MODE_V1_VLAN 0x1 /* enum */
 #define          MC_CMD_PTP_MODE_V2 0x2 /* enum */
 #define          MC_CMD_PTP_MODE_V2_VLAN 0x3 /* enum */
+#define          MC_CMD_PTP_MODE_V2_ENHANCED 0x4 /* enum */
 
 /* MC_CMD_PTP_IN_DISABLE msgrequest */
 #define    MC_CMD_PTP_IN_DISABLE_LEN 8
diff --git a/drivers/net/ethernet/sfc/net_driver.h b/drivers/net/ethernet/sfc/net_driver.h
index 0a90abd2421b..9bd433a095c5 100644
--- a/drivers/net/ethernet/sfc/net_driver.h
+++ b/drivers/net/ethernet/sfc/net_driver.h
@@ -69,6 +69,12 @@
 #define EFX_TXQ_TYPES		4
 #define EFX_MAX_TX_QUEUES	(EFX_TXQ_TYPES * EFX_MAX_CHANNELS)
 
+/* Maximum possible MTU the driver supports */
+#define EFX_MAX_MTU (9 * 1024)
+
+/* Size of an RX scatter buffer.  Small enough to pack 2 into a 4K page. */
+#define EFX_RX_USR_BUF_SIZE 1824
+
 /* Forward declare Precision Time Protocol (PTP) support structure. */
 struct efx_ptp_data;
 
@@ -206,25 +212,23 @@ struct efx_tx_queue {
 /**
  * struct efx_rx_buffer - An Efx RX data buffer
  * @dma_addr: DMA base address of the buffer
- * @skb: The associated socket buffer. Valid iff !(@flags & %EFX_RX_BUF_PAGE).
- *	Will be %NULL if the buffer slot is currently free.
- * @page: The associated page buffer. Valif iff @flags & %EFX_RX_BUF_PAGE.
+ * @page: The associated page buffer.
  *	Will be %NULL if the buffer slot is currently free.
- * @page_offset: Offset within page. Valid iff @flags & %EFX_RX_BUF_PAGE.
- * @len: Buffer length, in bytes.
- * @flags: Flags for buffer and packet state.
+ * @page_offset: If pending: offset in @page of DMA base address.
+ *	If completed: offset in @page of Ethernet header.
+ * @len: If pending: length for DMA descriptor.
+ *	If completed: received length, excluding hash prefix.
+ * @flags: Flags for buffer and packet state.  These are only set on the
+ *	first buffer of a scattered packet.
  */
 struct efx_rx_buffer {
 	dma_addr_t dma_addr;
-	union {
-		struct sk_buff *skb;
-		struct page *page;
-	} u;
+	struct page *page;
 	u16 page_offset;
 	u16 len;
 	u16 flags;
 };
-#define EFX_RX_BUF_PAGE		0x0001
+#define EFX_RX_BUF_LAST_IN_PAGE	0x0001
 #define EFX_RX_PKT_CSUMMED	0x0002
 #define EFX_RX_PKT_DISCARD	0x0004
 
@@ -260,14 +264,23 @@ struct efx_rx_page_state {
  * @added_count: Number of buffers added to the receive queue.
  * @notified_count: Number of buffers given to NIC (<= @added_count).
  * @removed_count: Number of buffers removed from the receive queue.
+ * @scatter_n: Number of buffers used by current packet
+ * @page_ring: The ring to store DMA mapped pages for reuse.
+ * @page_add: Counter to calculate the write pointer for the recycle ring.
+ * @page_remove: Counter to calculate the read pointer for the recycle ring.
+ * @page_recycle_count: The number of pages that have been recycled.
+ * @page_recycle_failed: The number of pages that couldn't be recycled because
+ *      the kernel still held a reference to them.
+ * @page_recycle_full: The number of pages that were released because the
+ *      recycle ring was full.
+ * @page_ptr_mask: The number of pages in the RX recycle ring minus 1.
  * @max_fill: RX descriptor maximum fill level (<= ring size)
  * @fast_fill_trigger: RX descriptor fill level that will trigger a fast fill
  *	(<= @max_fill)
  * @min_fill: RX descriptor minimum non-zero fill level.
  *	This records the minimum fill level observed when a ring
  *	refill was triggered.
- * @alloc_page_count: RX allocation strategy counter.
- * @alloc_skb_count: RX allocation strategy counter.
+ * @recycle_count: RX buffer recycle counter.
  * @slow_fill: Timer used to defer efx_nic_generate_fill_event().
  */
 struct efx_rx_queue {
@@ -279,15 +292,22 @@ struct efx_rx_queue {
 	bool enabled;
 	bool flush_pending;
 
-	int added_count;
-	int notified_count;
-	int removed_count;
+	unsigned int added_count;
+	unsigned int notified_count;
+	unsigned int removed_count;
+	unsigned int scatter_n;
+	struct page **page_ring;
+	unsigned int page_add;
+	unsigned int page_remove;
+	unsigned int page_recycle_count;
+	unsigned int page_recycle_failed;
+	unsigned int page_recycle_full;
+	unsigned int page_ptr_mask;
 	unsigned int max_fill;
 	unsigned int fast_fill_trigger;
 	unsigned int min_fill;
 	unsigned int min_overfill;
-	unsigned int alloc_page_count;
-	unsigned int alloc_skb_count;
+	unsigned int recycle_count;
 	struct timer_list slow_fill;
 	unsigned int slow_fill_count;
 };
@@ -336,10 +356,6 @@ enum efx_rx_alloc_method {
  * @event_test_cpu: Last CPU to handle interrupt or test event for this channel
  * @irq_count: Number of IRQs since last adaptive moderation decision
  * @irq_mod_score: IRQ moderation score
- * @rx_alloc_level: Watermark based heuristic counter for pushing descriptors
- *	and diagnostic counters
- * @rx_alloc_push_pages: RX allocation method currently in use for pushing
- *	descriptors
  * @n_rx_tobe_disc: Count of RX_TOBE_DISC errors
  * @n_rx_ip_hdr_chksum_err: Count of RX IP header checksum errors
  * @n_rx_tcp_udp_chksum_err: Count of RX TCP and UDP checksum errors
@@ -347,6 +363,12 @@ enum efx_rx_alloc_method {
  * @n_rx_frm_trunc: Count of RX_FRM_TRUNC errors
  * @n_rx_overlength: Count of RX_OVERLENGTH errors
  * @n_skbuff_leaks: Count of skbuffs leaked due to RX overrun
+ * @n_rx_nodesc_trunc: Number of RX packets truncated and then dropped due to
+ *	lack of descriptors
+ * @rx_pkt_n_frags: Number of fragments in next packet to be delivered by
+ *	__efx_rx_packet(), or zero if there is none
+ * @rx_pkt_index: Ring index of first buffer for next packet to be delivered
+ *	by __efx_rx_packet(), if @rx_pkt_n_frags != 0
  * @rx_queue: RX queue for this channel
  * @tx_queue: TX queues for this channel
  */
@@ -371,9 +393,6 @@ struct efx_channel {
 	unsigned int rfs_filters_added;
 #endif
 
-	int rx_alloc_level;
-	int rx_alloc_push_pages;
-
 	unsigned n_rx_tobe_disc;
 	unsigned n_rx_ip_hdr_chksum_err;
 	unsigned n_rx_tcp_udp_chksum_err;
@@ -381,11 +400,10 @@ struct efx_channel {
 	unsigned n_rx_frm_trunc;
 	unsigned n_rx_overlength;
 	unsigned n_skbuff_leaks;
+	unsigned int n_rx_nodesc_trunc;
 
-	/* Used to pipeline received packets in order to optimise memory
-	 * access with prefetches.
-	 */
-	struct efx_rx_buffer *rx_pkt;
+	unsigned int rx_pkt_n_frags;
+	unsigned int rx_pkt_index;
 
 	struct efx_rx_queue rx_queue;
 	struct efx_tx_queue tx_queue[EFX_TXQ_TYPES];
@@ -410,7 +428,7 @@ struct efx_channel_type {
 	void (*post_remove)(struct efx_channel *);
 	void (*get_name)(struct efx_channel *, char *buf, size_t len);
 	struct efx_channel *(*copy)(const struct efx_channel *);
-	void (*receive_skb)(struct efx_channel *, struct sk_buff *);
+	bool (*receive_skb)(struct efx_channel *, struct sk_buff *);
 	bool keep_eventq;
 };
 
@@ -446,6 +464,7 @@ enum nic_state {
 	STATE_UNINIT = 0,	/* device being probed/removed or is frozen */
 	STATE_READY = 1,	/* hardware ready and netdev registered */
 	STATE_DISABLED = 2,	/* device disabled due to hardware errors */
+	STATE_RECOVERY = 3,	/* device recovering from PCI error */
 };
 
 /*
@@ -684,10 +703,13 @@ struct vfdi_status;
  * @n_channels: Number of channels in use
  * @n_rx_channels: Number of channels used for RX (= number of RX queues)
  * @n_tx_channels: Number of channels used for TX
- * @rx_buffer_len: RX buffer length
+ * @rx_dma_len: Current maximum RX DMA length
  * @rx_buffer_order: Order (log2) of number of pages for each RX buffer
+ * @rx_buffer_truesize: Amortised allocation size of an RX buffer,
+ *	for use in sk_buff::truesize
  * @rx_hash_key: Toeplitz hash key for RSS
  * @rx_indir_table: Indirection table for RSS
+ * @rx_scatter: Scatter mode enabled for receives
  * @int_error_count: Number of internal errors seen recently
  * @int_error_expire: Time at which error count will be expired
  * @irq_status: Interrupt status buffer
@@ -800,10 +822,15 @@ struct efx_nic {
 	unsigned rss_spread;
 	unsigned tx_channel_offset;
 	unsigned n_tx_channels;
-	unsigned int rx_buffer_len;
+	unsigned int rx_dma_len;
 	unsigned int rx_buffer_order;
+	unsigned int rx_buffer_truesize;
+	unsigned int rx_page_buf_step;
+	unsigned int rx_bufs_per_page;
+	unsigned int rx_pages_per_batch;
 	u8 rx_hash_key[40];
 	u32 rx_indir_table[128];
+	bool rx_scatter;
 
 	unsigned int_error_count;
 	unsigned long int_error_expire;
@@ -934,8 +961,9 @@ static inline unsigned int efx_port_num(struct efx_nic *efx)
  * @evq_ptr_tbl_base: Event queue pointer table base address
  * @evq_rptr_tbl_base: Event queue read-pointer table base address
  * @max_dma_mask: Maximum possible DMA mask
- * @rx_buffer_hash_size: Size of hash at start of RX buffer
- * @rx_buffer_padding: Size of padding at end of RX buffer
+ * @rx_buffer_hash_size: Size of hash at start of RX packet
+ * @rx_buffer_padding: Size of padding at end of RX packet
+ * @can_rx_scatter: NIC is able to scatter packet to multiple buffers
  * @max_interrupt_mode: Highest capability interrupt mode supported
  *	from &enum efx_init_mode.
  * @phys_addr_channels: Number of channels with physically addressed
@@ -983,6 +1011,7 @@ struct efx_nic_type {
 	u64 max_dma_mask;
 	unsigned int rx_buffer_hash_size;
 	unsigned int rx_buffer_padding;
+	bool can_rx_scatter;
 	unsigned int max_interrupt_mode;
 	unsigned int phys_addr_channels;
 	unsigned int timer_period_max;
diff --git a/drivers/net/ethernet/sfc/nic.c b/drivers/net/ethernet/sfc/nic.c
index eaa8e874a3cb..b0503cd8c2a0 100644
--- a/drivers/net/ethernet/sfc/nic.c
+++ b/drivers/net/ethernet/sfc/nic.c
@@ -305,11 +305,11 @@ int efx_nic_alloc_buffer(struct efx_nic *efx, struct efx_buffer *buffer,
 			 unsigned int len)
 {
 	buffer->addr = dma_alloc_coherent(&efx->pci_dev->dev, len,
-					  &buffer->dma_addr, GFP_ATOMIC);
+					  &buffer->dma_addr,
+					  GFP_ATOMIC | __GFP_ZERO);
 	if (!buffer->addr)
 		return -ENOMEM;
 	buffer->len = len;
-	memset(buffer->addr, 0, len);
 	return 0;
 }
 
@@ -592,12 +592,22 @@ void efx_nic_init_rx(struct efx_rx_queue *rx_queue)
 	struct efx_nic *efx = rx_queue->efx;
 	bool is_b0 = efx_nic_rev(efx) >= EFX_REV_FALCON_B0;
 	bool iscsi_digest_en = is_b0;
+	bool jumbo_en;
+
+	/* For kernel-mode queues in Falcon A1, the JUMBO flag enables
+	 * DMA to continue after a PCIe page boundary (and scattering
+	 * is not possible).  In Falcon B0 and Siena, it enables
+	 * scatter.
+	 */
+	jumbo_en = !is_b0 || efx->rx_scatter;
 
 	netif_dbg(efx, hw, efx->net_dev,
 		  "RX queue %d ring in special buffers %d-%d\n",
 		  efx_rx_queue_index(rx_queue), rx_queue->rxd.index,
 		  rx_queue->rxd.index + rx_queue->rxd.entries - 1);
 
+	rx_queue->scatter_n = 0;
+
 	/* Pin RX descriptor ring */
 	efx_init_special_buffer(efx, &rx_queue->rxd);
 
@@ -614,8 +624,7 @@ void efx_nic_init_rx(struct efx_rx_queue *rx_queue)
 			      FRF_AZ_RX_DESCQ_SIZE,
 			      __ffs(rx_queue->rxd.entries),
 			      FRF_AZ_RX_DESCQ_TYPE, 0 /* kernel queue */ ,
-			      /* For >=B0 this is scatter so disable */
-			      FRF_AZ_RX_DESCQ_JUMBO, !is_b0,
+			      FRF_AZ_RX_DESCQ_JUMBO, jumbo_en,
 			      FRF_AZ_RX_DESCQ_EN, 1);
 	efx_writeo_table(efx, &rx_desc_ptr, efx->type->rxd_ptr_tbl_base,
 			 efx_rx_queue_index(rx_queue));
@@ -969,13 +978,24 @@ static u16 efx_handle_rx_not_ok(struct efx_rx_queue *rx_queue,
 		EFX_RX_PKT_DISCARD : 0;
 }
 
-/* Handle receive events that are not in-order. */
-static void
+/* Handle receive events that are not in-order. Return true if this
+ * can be handled as a partial packet discard, false if it's more
+ * serious.
+ */
+static bool
 efx_handle_rx_bad_index(struct efx_rx_queue *rx_queue, unsigned index)
 {
+	struct efx_channel *channel = efx_rx_queue_channel(rx_queue);
 	struct efx_nic *efx = rx_queue->efx;
 	unsigned expected, dropped;
 
+	if (rx_queue->scatter_n &&
+	    index == ((rx_queue->removed_count + rx_queue->scatter_n - 1) &
+		      rx_queue->ptr_mask)) {
+		++channel->n_rx_nodesc_trunc;
+		return true;
+	}
+
 	expected = rx_queue->removed_count & rx_queue->ptr_mask;
 	dropped = (index - expected) & rx_queue->ptr_mask;
 	netif_info(efx, rx_err, efx->net_dev,
@@ -984,6 +1004,7 @@ efx_handle_rx_bad_index(struct efx_rx_queue *rx_queue, unsigned index)
 
 	efx_schedule_reset(efx, EFX_WORKAROUND_5676(efx) ?
 			   RESET_TYPE_RX_RECOVERY : RESET_TYPE_DISABLE);
+	return false;
 }
 
 /* Handle a packet received event
@@ -999,7 +1020,7 @@ efx_handle_rx_event(struct efx_channel *channel, const efx_qword_t *event)
 	unsigned int rx_ev_desc_ptr, rx_ev_byte_cnt;
 	unsigned int rx_ev_hdr_type, rx_ev_mcast_pkt;
 	unsigned expected_ptr;
-	bool rx_ev_pkt_ok;
+	bool rx_ev_pkt_ok, rx_ev_sop, rx_ev_cont;
 	u16 flags;
 	struct efx_rx_queue *rx_queue;
 	struct efx_nic *efx = channel->efx;
@@ -1007,21 +1028,56 @@ efx_handle_rx_event(struct efx_channel *channel, const efx_qword_t *event)
 	if (unlikely(ACCESS_ONCE(efx->reset_pending)))
 		return;
 
-	/* Basic packet information */
-	rx_ev_byte_cnt = EFX_QWORD_FIELD(*event, FSF_AZ_RX_EV_BYTE_CNT);
-	rx_ev_pkt_ok = EFX_QWORD_FIELD(*event, FSF_AZ_RX_EV_PKT_OK);
-	rx_ev_hdr_type = EFX_QWORD_FIELD(*event, FSF_AZ_RX_EV_HDR_TYPE);
-	WARN_ON(EFX_QWORD_FIELD(*event, FSF_AZ_RX_EV_JUMBO_CONT));
-	WARN_ON(EFX_QWORD_FIELD(*event, FSF_AZ_RX_EV_SOP) != 1);
+	rx_ev_cont = EFX_QWORD_FIELD(*event, FSF_AZ_RX_EV_JUMBO_CONT);
+	rx_ev_sop = EFX_QWORD_FIELD(*event, FSF_AZ_RX_EV_SOP);
 	WARN_ON(EFX_QWORD_FIELD(*event, FSF_AZ_RX_EV_Q_LABEL) !=
 		channel->channel);
 
 	rx_queue = efx_channel_get_rx_queue(channel);
 
 	rx_ev_desc_ptr = EFX_QWORD_FIELD(*event, FSF_AZ_RX_EV_DESC_PTR);
-	expected_ptr = rx_queue->removed_count & rx_queue->ptr_mask;
-	if (unlikely(rx_ev_desc_ptr != expected_ptr))
-		efx_handle_rx_bad_index(rx_queue, rx_ev_desc_ptr);
+	expected_ptr = ((rx_queue->removed_count + rx_queue->scatter_n) &
+			rx_queue->ptr_mask);
+
+	/* Check for partial drops and other errors */
+	if (unlikely(rx_ev_desc_ptr != expected_ptr) ||
+	    unlikely(rx_ev_sop != (rx_queue->scatter_n == 0))) {
+		if (rx_ev_desc_ptr != expected_ptr &&
+		    !efx_handle_rx_bad_index(rx_queue, rx_ev_desc_ptr))
+			return;
+
+		/* Discard all pending fragments */
+		if (rx_queue->scatter_n) {
+			efx_rx_packet(
+				rx_queue,
+				rx_queue->removed_count & rx_queue->ptr_mask,
+				rx_queue->scatter_n, 0, EFX_RX_PKT_DISCARD);
+			rx_queue->removed_count += rx_queue->scatter_n;
+			rx_queue->scatter_n = 0;
+		}
+
+		/* Return if there is no new fragment */
+		if (rx_ev_desc_ptr != expected_ptr)
+			return;
+
+		/* Discard new fragment if not SOP */
+		if (!rx_ev_sop) {
+			efx_rx_packet(
+				rx_queue,
+				rx_queue->removed_count & rx_queue->ptr_mask,
+				1, 0, EFX_RX_PKT_DISCARD);
+			++rx_queue->removed_count;
+			return;
+		}
+	}
+
+	++rx_queue->scatter_n;
+	if (rx_ev_cont)
+		return;
+
+	rx_ev_byte_cnt = EFX_QWORD_FIELD(*event, FSF_AZ_RX_EV_BYTE_CNT);
+	rx_ev_pkt_ok = EFX_QWORD_FIELD(*event, FSF_AZ_RX_EV_PKT_OK);
+	rx_ev_hdr_type = EFX_QWORD_FIELD(*event, FSF_AZ_RX_EV_HDR_TYPE);
 
 	if (likely(rx_ev_pkt_ok)) {
 		/* If packet is marked as OK and packet type is TCP/IP or
@@ -1049,7 +1105,11 @@ efx_handle_rx_event(struct efx_channel *channel, const efx_qword_t *event)
 	channel->irq_mod_score += 2;
 
 	/* Handle received packet */
-	efx_rx_packet(rx_queue, rx_ev_desc_ptr, rx_ev_byte_cnt, flags);
+	efx_rx_packet(rx_queue,
+		      rx_queue->removed_count & rx_queue->ptr_mask,
+		      rx_queue->scatter_n, rx_ev_byte_cnt, flags);
+	rx_queue->removed_count += rx_queue->scatter_n;
+	rx_queue->scatter_n = 0;
 }
 
 /* If this flush done event corresponds to a &struct efx_tx_queue, then
diff --git a/drivers/net/ethernet/sfc/ptp.c b/drivers/net/ethernet/sfc/ptp.c
index 3f93624fc273..9a95abf2dedf 100644
--- a/drivers/net/ethernet/sfc/ptp.c
+++ b/drivers/net/ethernet/sfc/ptp.c
@@ -99,6 +99,9 @@
 #define PTP_V2_VERSION_LENGTH	1
 #define PTP_V2_VERSION_OFFSET	29
 
+#define PTP_V2_UUID_LENGTH	8
+#define PTP_V2_UUID_OFFSET	48
+
 /* Although PTP V2 UUIDs are comprised a ClockIdentity (8) and PortNumber (2),
  * the MC only captures the last six bytes of the clock identity. These values
  * reflect those, not the ones used in the standard.  The standard permits
@@ -429,13 +432,10 @@ static int efx_ptp_process_times(struct efx_nic *efx, u8 *synch_buf,
 	unsigned number_readings = (response_length /
 			       MC_CMD_PTP_OUT_SYNCHRONIZE_TIMESET_LEN);
 	unsigned i;
-	unsigned min;
-	unsigned min_set = 0;
 	unsigned total;
 	unsigned ngood = 0;
 	unsigned last_good = 0;
 	struct efx_ptp_data *ptp = efx->ptp_data;
-	bool min_valid = false;
 	u32 last_sec;
 	u32 start_sec;
 	struct timespec delta;
@@ -443,35 +443,17 @@ static int efx_ptp_process_times(struct efx_nic *efx, u8 *synch_buf,
 	if (number_readings == 0)
 		return -EAGAIN;
 
-	/* Find minimum value in this set of results, discarding clearly
-	 * erroneous results.
+	/* Read the set of results and increment stats for any results that
+	 * appera to be erroneous.
 	 */
 	for (i = 0; i < number_readings; i++) {
 		efx_ptp_read_timeset(synch_buf, &ptp->timeset[i]);
 		synch_buf += MC_CMD_PTP_OUT_SYNCHRONIZE_TIMESET_LEN;
-		if (ptp->timeset[i].window > SYNCHRONISATION_GRANULARITY_NS) {
-			if (min_valid) {
-				if (ptp->timeset[i].window < min_set)
-					min_set = ptp->timeset[i].window;
-			} else {
-				min_valid = true;
-				min_set = ptp->timeset[i].window;
-			}
-		}
-	}
-
-	if (min_valid) {
-		if (ptp->base_sync_valid && (min_set > ptp->base_sync_ns))
-			min = ptp->base_sync_ns;
-		else
-			min = min_set;
-	} else {
-		min = SYNCHRONISATION_GRANULARITY_NS;
 	}
 
-	/* Discard excessively long synchronise durations.  The MC times
-	 * when it finishes reading the host time so the corrected window
-	 * time should be fairly constant for a given platform.
+	/* Find the last good host-MC synchronization result. The MC times
+	 * when it finishes reading the host time so the corrected window time
+	 * should be fairly constant for a given platform.
 	 */
 	total = 0;
 	for (i = 0; i < number_readings; i++)
@@ -489,8 +471,8 @@ static int efx_ptp_process_times(struct efx_nic *efx, u8 *synch_buf,
 
 	if (ngood == 0) {
 		netif_warn(efx, drv, efx->net_dev,
-			   "PTP no suitable synchronisations %dns %dns\n",
-			   ptp->base_sync_ns, min_set);
+			   "PTP no suitable synchronisations %dns\n",
+			   ptp->base_sync_ns);
 		return -EAGAIN;
 	}
 
@@ -930,8 +912,10 @@ static int efx_ptp_probe_channel(struct efx_channel *channel)
 
 	ptp->phc_clock = ptp_clock_register(&ptp->phc_clock_info,
 					    &efx->pci_dev->dev);
-	if (!ptp->phc_clock)
+	if (IS_ERR(ptp->phc_clock)) {
+		rc = PTR_ERR(ptp->phc_clock);
 		goto fail3;
+	}
 
 	INIT_WORK(&ptp->pps_work, efx_ptp_pps_worker);
 	ptp->pps_workwq = create_singlethread_workqueue("sfc_pps");
@@ -1006,43 +990,53 @@ bool efx_ptp_is_ptp_tx(struct efx_nic *efx, struct sk_buff *skb)
  * the receive timestamp from the MC - this will probably occur after the
  * packet arrival because of the processing in the MC.
  */
-static void efx_ptp_rx(struct efx_channel *channel, struct sk_buff *skb)
+static bool efx_ptp_rx(struct efx_channel *channel, struct sk_buff *skb)
 {
 	struct efx_nic *efx = channel->efx;
 	struct efx_ptp_data *ptp = efx->ptp_data;
 	struct efx_ptp_match *match = (struct efx_ptp_match *)skb->cb;
-	u8 *data;
+	u8 *match_data_012, *match_data_345;
 	unsigned int version;
 
 	match->expiry = jiffies + msecs_to_jiffies(PKT_EVENT_LIFETIME_MS);
 
 	/* Correct version? */
 	if (ptp->mode == MC_CMD_PTP_MODE_V1) {
-		if (skb->len < PTP_V1_MIN_LENGTH) {
-			netif_receive_skb(skb);
-			return;
+		if (!pskb_may_pull(skb, PTP_V1_MIN_LENGTH)) {
+			return false;
 		}
 		version = ntohs(*(__be16 *)&skb->data[PTP_V1_VERSION_OFFSET]);
 		if (version != PTP_VERSION_V1) {
-			netif_receive_skb(skb);
-			return;
+			return false;
 		}
+
+		/* PTP V1 uses all six bytes of the UUID to match the packet
+		 * to the timestamp
+		 */
+		match_data_012 = skb->data + PTP_V1_UUID_OFFSET;
+		match_data_345 = skb->data + PTP_V1_UUID_OFFSET + 3;
 	} else {
-		if (skb->len < PTP_V2_MIN_LENGTH) {
-			netif_receive_skb(skb);
-			return;
+		if (!pskb_may_pull(skb, PTP_V2_MIN_LENGTH)) {
+			return false;
 		}
 		version = skb->data[PTP_V2_VERSION_OFFSET];
-
-		BUG_ON(ptp->mode != MC_CMD_PTP_MODE_V2);
-		BUILD_BUG_ON(PTP_V1_UUID_OFFSET != PTP_V2_MC_UUID_OFFSET);
-		BUILD_BUG_ON(PTP_V1_UUID_LENGTH != PTP_V2_MC_UUID_LENGTH);
-		BUILD_BUG_ON(PTP_V1_SEQUENCE_OFFSET != PTP_V2_SEQUENCE_OFFSET);
-		BUILD_BUG_ON(PTP_V1_SEQUENCE_LENGTH != PTP_V2_SEQUENCE_LENGTH);
-
 		if ((version & PTP_VERSION_V2_MASK) != PTP_VERSION_V2) {
-			netif_receive_skb(skb);
-			return;
+			return false;
+		}
+
+		/* The original V2 implementation uses bytes 2-7 of
+		 * the UUID to match the packet to the timestamp. This
+		 * discards two of the bytes of the MAC address used
+		 * to create the UUID (SF bug 33070).  The PTP V2
+		 * enhanced mode fixes this issue and uses bytes 0-2
+		 * and byte 5-7 of the UUID.
+		 */
+		match_data_345 = skb->data + PTP_V2_UUID_OFFSET + 5;
+		if (ptp->mode == MC_CMD_PTP_MODE_V2) {
+			match_data_012 = skb->data + PTP_V2_UUID_OFFSET + 2;
+		} else {
+			match_data_012 = skb->data + PTP_V2_UUID_OFFSET + 0;
+			BUG_ON(ptp->mode != MC_CMD_PTP_MODE_V2_ENHANCED);
 		}
 	}
 
@@ -1056,14 +1050,19 @@ static void efx_ptp_rx(struct efx_channel *channel, struct sk_buff *skb)
 		timestamps = skb_hwtstamps(skb);
 		memset(timestamps, 0, sizeof(*timestamps));
 
+		/* We expect the sequence number to be in the same position in
+		 * the packet for PTP V1 and V2
+		 */
+		BUILD_BUG_ON(PTP_V1_SEQUENCE_OFFSET != PTP_V2_SEQUENCE_OFFSET);
+		BUILD_BUG_ON(PTP_V1_SEQUENCE_LENGTH != PTP_V2_SEQUENCE_LENGTH);
+
 		/* Extract UUID/Sequence information */
-		data = skb->data + PTP_V1_UUID_OFFSET;
-		match->words[0] = (data[0]         |
-				   (data[1] << 8)  |
-				   (data[2] << 16) |
-				   (data[3] << 24));
-		match->words[1] = (data[4]         |
-				   (data[5] << 8)  |
+		match->words[0] = (match_data_012[0]         |
+				   (match_data_012[1] << 8)  |
+				   (match_data_012[2] << 16) |
+				   (match_data_345[0] << 24));
+		match->words[1] = (match_data_345[1]         |
+				   (match_data_345[2] << 8)  |
 				   (skb->data[PTP_V1_SEQUENCE_OFFSET +
 					      PTP_V1_SEQUENCE_LENGTH - 1] <<
 				    16));
@@ -1073,6 +1072,8 @@ static void efx_ptp_rx(struct efx_channel *channel, struct sk_buff *skb)
 
 	skb_queue_tail(&ptp->rxq, skb);
 	queue_work(ptp->workwq, &ptp->work);
+
+	return true;
 }
 
 /* Transmit a PTP packet.  This has to be transmitted by the MC
@@ -1167,7 +1168,7 @@ static int efx_ptp_ts_init(struct efx_nic *efx, struct hwtstamp_config *init)
 	 * timestamped
 	 */
 		init->rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
-		new_mode = MC_CMD_PTP_MODE_V2;
+		new_mode = MC_CMD_PTP_MODE_V2_ENHANCED;
 		enable_wanted = true;
 		break;
 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
@@ -1186,7 +1187,14 @@ static int efx_ptp_ts_init(struct efx_nic *efx, struct hwtstamp_config *init)
 	if (init->tx_type != HWTSTAMP_TX_OFF)
 		enable_wanted = true;
 
+	/* Old versions of the firmware do not support the improved
+	 * UUID filtering option (SF bug 33070).  If the firmware does
+	 * not accept the enhanced mode, fall back to the standard PTP
+	 * v2 UUID filtering.
+	 */
 	rc = efx_ptp_change_mode(efx, enable_wanted, new_mode);
+	if ((rc != 0) && (new_mode == MC_CMD_PTP_MODE_V2_ENHANCED))
+		rc = efx_ptp_change_mode(efx, enable_wanted, MC_CMD_PTP_MODE_V2);
 	if (rc != 0)
 		return rc;
 
diff --git a/drivers/net/ethernet/sfc/rx.c b/drivers/net/ethernet/sfc/rx.c
index bb579a6128c8..e73e30bac10e 100644
--- a/drivers/net/ethernet/sfc/rx.c
+++ b/drivers/net/ethernet/sfc/rx.c
@@ -16,6 +16,7 @@
 #include <linux/udp.h>
 #include <linux/prefetch.h>
 #include <linux/moduleparam.h>
+#include <linux/iommu.h>
 #include <net/ip.h>
 #include <net/checksum.h>
 #include "net_driver.h"
@@ -24,85 +25,39 @@
 #include "selftest.h"
 #include "workarounds.h"
 
-/* Number of RX descriptors pushed at once. */
-#define EFX_RX_BATCH  8
+/* Preferred number of descriptors to fill at once */
+#define EFX_RX_PREFERRED_BATCH 8U
 
-/* Maximum size of a buffer sharing a page */
-#define EFX_RX_HALF_PAGE ((PAGE_SIZE >> 1) - sizeof(struct efx_rx_page_state))
+/* Number of RX buffers to recycle pages for.  When creating the RX page recycle
+ * ring, this number is divided by the number of buffers per page to calculate
+ * the number of pages to store in the RX page recycle ring.
+ */
+#define EFX_RECYCLE_RING_SIZE_IOMMU 4096
+#define EFX_RECYCLE_RING_SIZE_NOIOMMU (2 * EFX_RX_PREFERRED_BATCH)
 
 /* Size of buffer allocated for skb header area. */
 #define EFX_SKB_HEADERS  64u
 
-/*
- * rx_alloc_method - RX buffer allocation method
- *
- * This driver supports two methods for allocating and using RX buffers:
- * each RX buffer may be backed by an skb or by an order-n page.
- *
- * When GRO is in use then the second method has a lower overhead,
- * since we don't have to allocate then free skbs on reassembled frames.
- *
- * Values:
- *   - RX_ALLOC_METHOD_AUTO = 0
- *   - RX_ALLOC_METHOD_SKB  = 1
- *   - RX_ALLOC_METHOD_PAGE = 2
- *
- * The heuristic for %RX_ALLOC_METHOD_AUTO is a simple hysteresis count
- * controlled by the parameters below.
- *
- *   - Since pushing and popping descriptors are separated by the rx_queue
- *     size, so the watermarks should be ~rxd_size.
- *   - The performance win by using page-based allocation for GRO is less
- *     than the performance hit of using page-based allocation of non-GRO,
- *     so the watermarks should reflect this.
- *
- * Per channel we maintain a single variable, updated by each channel:
- *
- *   rx_alloc_level += (gro_performed ? RX_ALLOC_FACTOR_GRO :
- *                      RX_ALLOC_FACTOR_SKB)
- * Per NAPI poll interval, we constrain rx_alloc_level to 0..MAX (which
- * limits the hysteresis), and update the allocation strategy:
- *
- *   rx_alloc_method = (rx_alloc_level > RX_ALLOC_LEVEL_GRO ?
- *                      RX_ALLOC_METHOD_PAGE : RX_ALLOC_METHOD_SKB)
- */
-static int rx_alloc_method = RX_ALLOC_METHOD_AUTO;
-
-#define RX_ALLOC_LEVEL_GRO 0x2000
-#define RX_ALLOC_LEVEL_MAX 0x3000
-#define RX_ALLOC_FACTOR_GRO 1
-#define RX_ALLOC_FACTOR_SKB (-2)
-
 /* This is the percentage fill level below which new RX descriptors
  * will be added to the RX descriptor ring.
  */
 static unsigned int rx_refill_threshold;
 
+/* Each packet can consume up to ceil(max_frame_len / buffer_size) buffers */
+#define EFX_RX_MAX_FRAGS DIV_ROUND_UP(EFX_MAX_FRAME_LEN(EFX_MAX_MTU), \
+				      EFX_RX_USR_BUF_SIZE)
+
 /*
  * RX maximum head room required.
  *
- * This must be at least 1 to prevent overflow and at least 2 to allow
- * pipelined receives.
+ * This must be at least 1 to prevent overflow, plus one packet-worth
+ * to allow pipelined receives.
  */
-#define EFX_RXD_HEAD_ROOM 2
+#define EFX_RXD_HEAD_ROOM (1 + EFX_RX_MAX_FRAGS)
 
-/* Offset of ethernet header within page */
-static inline unsigned int efx_rx_buf_offset(struct efx_nic *efx,
-					     struct efx_rx_buffer *buf)
+static inline u8 *efx_rx_buf_va(struct efx_rx_buffer *buf)
 {
-	return buf->page_offset + efx->type->rx_buffer_hash_size;
-}
-static inline unsigned int efx_rx_buf_size(struct efx_nic *efx)
-{
-	return PAGE_SIZE << efx->rx_buffer_order;
-}
-
-static u8 *efx_rx_buf_eh(struct efx_nic *efx, struct efx_rx_buffer *buf)
-{
-	if (buf->flags & EFX_RX_BUF_PAGE)
-		return page_address(buf->u.page) + efx_rx_buf_offset(efx, buf);
-	else
-		return (u8 *)buf->u.skb->data + efx->type->rx_buffer_hash_size;
+	return page_address(buf->page) + buf->page_offset;
 }
 
 static inline u32 efx_rx_buf_hash(const u8 *eh)
@@ -119,66 +74,81 @@ static inline u32 efx_rx_buf_hash(const u8 *eh)
 #endif
 }
 
-/**
- * efx_init_rx_buffers_skb - create EFX_RX_BATCH skb-based RX buffers
- *
- * @rx_queue:		Efx RX queue
- *
- * This allocates EFX_RX_BATCH skbs, maps them for DMA, and populates a
- * struct efx_rx_buffer for each one. Return a negative error code or 0
- * on success. May fail having only inserted fewer than EFX_RX_BATCH
- * buffers.
- */
-static int efx_init_rx_buffers_skb(struct efx_rx_queue *rx_queue)
+static inline struct efx_rx_buffer *
+efx_rx_buf_next(struct efx_rx_queue *rx_queue, struct efx_rx_buffer *rx_buf)
+{
+	if (unlikely(rx_buf == efx_rx_buffer(rx_queue, rx_queue->ptr_mask)))
+		return efx_rx_buffer(rx_queue, 0);
+	else
+		return rx_buf + 1;
+}
+
+static inline void efx_sync_rx_buffer(struct efx_nic *efx,
+				      struct efx_rx_buffer *rx_buf,
+				      unsigned int len)
+{
+	dma_sync_single_for_cpu(&efx->pci_dev->dev, rx_buf->dma_addr, len,
+				DMA_FROM_DEVICE);
+}
+
+void efx_rx_config_page_split(struct efx_nic *efx)
+{
+	efx->rx_page_buf_step = ALIGN(efx->rx_dma_len + EFX_PAGE_IP_ALIGN,
+				      L1_CACHE_BYTES);
+	efx->rx_bufs_per_page = efx->rx_buffer_order ? 1 :
+		((PAGE_SIZE - sizeof(struct efx_rx_page_state)) /
+		 efx->rx_page_buf_step);
+	efx->rx_buffer_truesize = (PAGE_SIZE << efx->rx_buffer_order) /
+		efx->rx_bufs_per_page;
+	efx->rx_pages_per_batch = DIV_ROUND_UP(EFX_RX_PREFERRED_BATCH,
+					       efx->rx_bufs_per_page);
+}
+
+/* Check the RX page recycle ring for a page that can be reused. */
+static struct page *efx_reuse_page(struct efx_rx_queue *rx_queue)
 {
 	struct efx_nic *efx = rx_queue->efx;
-	struct net_device *net_dev = efx->net_dev;
-	struct efx_rx_buffer *rx_buf;
-	struct sk_buff *skb;
-	int skb_len = efx->rx_buffer_len;
-	unsigned index, count;
+	struct page *page;
+	struct efx_rx_page_state *state;
+	unsigned index;
 
-	for (count = 0; count < EFX_RX_BATCH; ++count) {
-		index = rx_queue->added_count & rx_queue->ptr_mask;
-		rx_buf = efx_rx_buffer(rx_queue, index);
-
-		rx_buf->u.skb = skb = netdev_alloc_skb(net_dev, skb_len);
-		if (unlikely(!skb))
-			return -ENOMEM;
-
-		/* Adjust the SKB for padding */
-		skb_reserve(skb, NET_IP_ALIGN);
-		rx_buf->len = skb_len - NET_IP_ALIGN;
-		rx_buf->flags = 0;
-
-		rx_buf->dma_addr = dma_map_single(&efx->pci_dev->dev,
-						  skb->data, rx_buf->len,
-						  DMA_FROM_DEVICE);
-		if (unlikely(dma_mapping_error(&efx->pci_dev->dev,
-					       rx_buf->dma_addr))) {
-			dev_kfree_skb_any(skb);
-			rx_buf->u.skb = NULL;
-			return -EIO;
-		}
+	index = rx_queue->page_remove & rx_queue->page_ptr_mask;
+	page = rx_queue->page_ring[index];
+	if (page == NULL)
+		return NULL;
+
+	rx_queue->page_ring[index] = NULL;
+	/* page_remove cannot exceed page_add. */
+	if (rx_queue->page_remove != rx_queue->page_add)
+		++rx_queue->page_remove;
 
-		++rx_queue->added_count;
-		++rx_queue->alloc_skb_count;
+	/* If page_count is 1 then we hold the only reference to this page. */
+	if (page_count(page) == 1) {
+		++rx_queue->page_recycle_count;
+		return page;
+	} else {
+		state = page_address(page);
+		dma_unmap_page(&efx->pci_dev->dev, state->dma_addr,
+			       PAGE_SIZE << efx->rx_buffer_order,
+			       DMA_FROM_DEVICE);
+		put_page(page);
+		++rx_queue->page_recycle_failed;
 	}
 
-	return 0;
+	return NULL;
 }
 
 /**
- * efx_init_rx_buffers_page - create EFX_RX_BATCH page-based RX buffers
+ * efx_init_rx_buffers - create EFX_RX_BATCH page-based RX buffers
  *
  * @rx_queue:		Efx RX queue
  *
- * This allocates memory for EFX_RX_BATCH receive buffers, maps them for DMA,
- * and populates struct efx_rx_buffers for each one. Return a negative error
- * code or 0 on success. If a single page can be split between two buffers,
- * then the page will either be inserted fully, or not at at all.
+ * This allocates a batch of pages, maps them for DMA, and populates
+ * struct efx_rx_buffers for each one. Return a negative error code or
+ * 0 on success. If a single page can be used for multiple buffers,
+ * then the page will either be inserted fully, or not at all.
  */
-static int efx_init_rx_buffers_page(struct efx_rx_queue *rx_queue)
+static int efx_init_rx_buffers(struct efx_rx_queue *rx_queue)
 {
 	struct efx_nic *efx = rx_queue->efx;
 	struct efx_rx_buffer *rx_buf;
@@ -188,150 +158,140 @@ static int efx_init_rx_buffers_page(struct efx_rx_queue *rx_queue)
 	dma_addr_t dma_addr;
 	unsigned index, count;
 
-	/* We can split a page between two buffers */
-	BUILD_BUG_ON(EFX_RX_BATCH & 1);
-
-	for (count = 0; count < EFX_RX_BATCH; ++count) {
-		page = alloc_pages(__GFP_COLD | __GFP_COMP | GFP_ATOMIC,
-				   efx->rx_buffer_order);
-		if (unlikely(page == NULL))
-			return -ENOMEM;
-		dma_addr = dma_map_page(&efx->pci_dev->dev, page, 0,
-					efx_rx_buf_size(efx),
-					DMA_FROM_DEVICE);
-		if (unlikely(dma_mapping_error(&efx->pci_dev->dev, dma_addr))) {
-			__free_pages(page, efx->rx_buffer_order);
-			return -EIO;
+	count = 0;
+	do {
+		page = efx_reuse_page(rx_queue);
+		if (page == NULL) {
+			page = alloc_pages(__GFP_COLD | __GFP_COMP | GFP_ATOMIC,
+					   efx->rx_buffer_order);
+			if (unlikely(page == NULL))
+				return -ENOMEM;
+			dma_addr =
+				dma_map_page(&efx->pci_dev->dev, page, 0,
+					     PAGE_SIZE << efx->rx_buffer_order,
+					     DMA_FROM_DEVICE);
+			if (unlikely(dma_mapping_error(&efx->pci_dev->dev,
+						       dma_addr))) {
+				__free_pages(page, efx->rx_buffer_order);
+				return -EIO;
+			}
+			state = page_address(page);
+			state->dma_addr = dma_addr;
+		} else {
+			state = page_address(page);
+			dma_addr = state->dma_addr;
 		}
-		state = page_address(page);
-		state->refcnt = 0;
-		state->dma_addr = dma_addr;
 
 		dma_addr += sizeof(struct efx_rx_page_state);
 		page_offset = sizeof(struct efx_rx_page_state);
 
-	split:
-		index = rx_queue->added_count & rx_queue->ptr_mask;
-		rx_buf = efx_rx_buffer(rx_queue, index);
-		rx_buf->dma_addr = dma_addr + EFX_PAGE_IP_ALIGN;
-		rx_buf->u.page = page;
-		rx_buf->page_offset = page_offset + EFX_PAGE_IP_ALIGN;
-		rx_buf->len = efx->rx_buffer_len - EFX_PAGE_IP_ALIGN;
-		rx_buf->flags = EFX_RX_BUF_PAGE;
-		++rx_queue->added_count;
-		++rx_queue->alloc_page_count;
-		++state->refcnt;
-
-		if ((~count & 1) && (efx->rx_buffer_len <= EFX_RX_HALF_PAGE)) {
-			/* Use the second half of the page */
+		do {
+			index = rx_queue->added_count & rx_queue->ptr_mask;
+			rx_buf = efx_rx_buffer(rx_queue, index);
+			rx_buf->dma_addr = dma_addr + EFX_PAGE_IP_ALIGN;
+			rx_buf->page = page;
+			rx_buf->page_offset = page_offset + EFX_PAGE_IP_ALIGN;
+			rx_buf->len = efx->rx_dma_len;
+			rx_buf->flags = 0;
+			++rx_queue->added_count;
 			get_page(page);
-			dma_addr += (PAGE_SIZE >> 1);
-			page_offset += (PAGE_SIZE >> 1);
-			++count;
-			goto split;
-		}
-	}
+			dma_addr += efx->rx_page_buf_step;
+			page_offset += efx->rx_page_buf_step;
+		} while (page_offset + efx->rx_page_buf_step <= PAGE_SIZE);
+
+		rx_buf->flags = EFX_RX_BUF_LAST_IN_PAGE;
+	} while (++count < efx->rx_pages_per_batch);
 
 	return 0;
 }
 
+/* Unmap a DMA-mapped page.  This function is only called for the final RX
+ * buffer in a page.
+ */
 static void efx_unmap_rx_buffer(struct efx_nic *efx,
-				struct efx_rx_buffer *rx_buf,
-				unsigned int used_len)
+				struct efx_rx_buffer *rx_buf)
 {
-	if ((rx_buf->flags & EFX_RX_BUF_PAGE) && rx_buf->u.page) {
-		struct efx_rx_page_state *state;
-
-		state = page_address(rx_buf->u.page);
-		if (--state->refcnt == 0) {
-			dma_unmap_page(&efx->pci_dev->dev,
-				       state->dma_addr,
-				       efx_rx_buf_size(efx),
-				       DMA_FROM_DEVICE);
-		} else if (used_len) {
-			dma_sync_single_for_cpu(&efx->pci_dev->dev,
-						rx_buf->dma_addr, used_len,
-						DMA_FROM_DEVICE);
-		}
-	} else if (!(rx_buf->flags & EFX_RX_BUF_PAGE) && rx_buf->u.skb) {
-		dma_unmap_single(&efx->pci_dev->dev, rx_buf->dma_addr,
-				 rx_buf->len, DMA_FROM_DEVICE);
+	struct page *page = rx_buf->page;
+
+	if (page) {
+		struct efx_rx_page_state *state = page_address(page);
+		dma_unmap_page(&efx->pci_dev->dev,
+			       state->dma_addr,
+			       PAGE_SIZE << efx->rx_buffer_order,
+			       DMA_FROM_DEVICE);
 	}
 }
 
-static void efx_free_rx_buffer(struct efx_nic *efx,
-			       struct efx_rx_buffer *rx_buf)
+static void efx_free_rx_buffer(struct efx_rx_buffer *rx_buf)
 {
-	if ((rx_buf->flags & EFX_RX_BUF_PAGE) && rx_buf->u.page) {
-		__free_pages(rx_buf->u.page, efx->rx_buffer_order);
-		rx_buf->u.page = NULL;
-	} else if (!(rx_buf->flags & EFX_RX_BUF_PAGE) && rx_buf->u.skb) {
-		dev_kfree_skb_any(rx_buf->u.skb);
-		rx_buf->u.skb = NULL;
+	if (rx_buf->page) {
+		put_page(rx_buf->page);
+		rx_buf->page = NULL;
 	}
 }
 
-static void efx_fini_rx_buffer(struct efx_rx_queue *rx_queue,
-			       struct efx_rx_buffer *rx_buf)
+/* Attempt to recycle the page if there is an RX recycle ring; the page can
+ * only be added if this is the final RX buffer, to prevent pages being used in
+ * the descriptor ring and appearing in the recycle ring simultaneously.
+ */
+static void efx_recycle_rx_page(struct efx_channel *channel,
+				struct efx_rx_buffer *rx_buf)
 {
-	efx_unmap_rx_buffer(rx_queue->efx, rx_buf, 0);
-	efx_free_rx_buffer(rx_queue->efx, rx_buf);
-}
+	struct page *page = rx_buf->page;
+	struct efx_rx_queue *rx_queue = efx_channel_get_rx_queue(channel);
+	struct efx_nic *efx = rx_queue->efx;
+	unsigned index;
 
-/* Attempt to resurrect the other receive buffer that used to share this page,
- * which had previously been passed up to the kernel and freed. */
-static void efx_resurrect_rx_buffer(struct efx_rx_queue *rx_queue,
-				    struct efx_rx_buffer *rx_buf)
-{
-	struct efx_rx_page_state *state = page_address(rx_buf->u.page);
-	struct efx_rx_buffer *new_buf;
-	unsigned fill_level, index;
-
-	/* +1 because efx_rx_packet() incremented removed_count. +1 because
-	 * we'd like to insert an additional descriptor whilst leaving
-	 * EFX_RXD_HEAD_ROOM for the non-recycle path */
-	fill_level = (rx_queue->added_count - rx_queue->removed_count + 2);
-	if (unlikely(fill_level > rx_queue->max_fill)) {
-		/* We could place "state" on a list, and drain the list in
-		 * efx_fast_push_rx_descriptors(). For now, this will do. */
+	/* Only recycle the page after processing the final buffer. */
+	if (!(rx_buf->flags & EFX_RX_BUF_LAST_IN_PAGE))
 		return;
-	}
 
-	++state->refcnt;
-	get_page(rx_buf->u.page);
+	index = rx_queue->page_add & rx_queue->page_ptr_mask;
+	if (rx_queue->page_ring[index] == NULL) {
+		unsigned read_index = rx_queue->page_remove &
+			rx_queue->page_ptr_mask;
 
-	index = rx_queue->added_count & rx_queue->ptr_mask;
-	new_buf = efx_rx_buffer(rx_queue, index);
-	new_buf->dma_addr = rx_buf->dma_addr ^ (PAGE_SIZE >> 1);
-	new_buf->u.page = rx_buf->u.page;
-	new_buf->len = rx_buf->len;
-	new_buf->flags = EFX_RX_BUF_PAGE;
-	++rx_queue->added_count;
+		/* The next slot in the recycle ring is available, but
+		 * increment page_remove if the read pointer currently
+		 * points here.
+		 */
+		if (read_index == index)
+			++rx_queue->page_remove;
+		rx_queue->page_ring[index] = page;
+		++rx_queue->page_add;
+		return;
+	}
+	++rx_queue->page_recycle_full;
+	efx_unmap_rx_buffer(efx, rx_buf);
+	put_page(rx_buf->page);
 }
 
-/* Recycle the given rx buffer directly back into the rx_queue. There is
- * always room to add this buffer, because we've just popped a buffer. */
-static void efx_recycle_rx_buffer(struct efx_channel *channel,
-				  struct efx_rx_buffer *rx_buf)
+static void efx_fini_rx_buffer(struct efx_rx_queue *rx_queue,
+			       struct efx_rx_buffer *rx_buf)
 {
-	struct efx_nic *efx = channel->efx;
-	struct efx_rx_queue *rx_queue = efx_channel_get_rx_queue(channel);
-	struct efx_rx_buffer *new_buf;
-	unsigned index;
-
-	rx_buf->flags &= EFX_RX_BUF_PAGE;
-
-	if ((rx_buf->flags & EFX_RX_BUF_PAGE) &&
-	    efx->rx_buffer_len <= EFX_RX_HALF_PAGE &&
-	    page_count(rx_buf->u.page) == 1)
-		efx_resurrect_rx_buffer(rx_queue, rx_buf);
+	/* Release the page reference we hold for the buffer. */
+	if (rx_buf->page)
+		put_page(rx_buf->page);
+
+	/* If this is the last buffer in a page, unmap and free it. */
+	if (rx_buf->flags & EFX_RX_BUF_LAST_IN_PAGE) {
+		efx_unmap_rx_buffer(rx_queue->efx, rx_buf);
+		efx_free_rx_buffer(rx_buf);
+	}
+	rx_buf->page = NULL;
+}
 
-	index = rx_queue->added_count & rx_queue->ptr_mask;
-	new_buf = efx_rx_buffer(rx_queue, index);
+/* Recycle the pages that are used by buffers that have just been received. */
+static void efx_recycle_rx_buffers(struct efx_channel *channel,
+				   struct efx_rx_buffer *rx_buf,
+				   unsigned int n_frags)
+{
+	struct efx_rx_queue *rx_queue = efx_channel_get_rx_queue(channel);
 
-	memcpy(new_buf, rx_buf, sizeof(*new_buf));
-	rx_buf->u.page = NULL;
-	++rx_queue->added_count;
+	do {
+		efx_recycle_rx_page(channel, rx_buf);
+		rx_buf = efx_rx_buf_next(rx_queue, rx_buf);
+	} while (--n_frags);
 }
 
 /**
@@ -348,8 +308,8 @@ static void efx_recycle_rx_buffer(struct efx_channel *channel,
  */
 void efx_fast_push_rx_descriptors(struct efx_rx_queue *rx_queue)
 {
-	struct efx_channel *channel = efx_rx_queue_channel(rx_queue);
-	unsigned fill_level;
+	struct efx_nic *efx = rx_queue->efx;
+	unsigned int fill_level, batch_size;
 	int space, rc = 0;
 
 	/* Calculate current fill level, and exit if we don't need to fill */
@@ -364,28 +324,26 @@ void efx_fast_push_rx_descriptors(struct efx_rx_queue *rx_queue)
 			rx_queue->min_fill = fill_level;
 	}
 
+	batch_size = efx->rx_pages_per_batch * efx->rx_bufs_per_page;
 	space = rx_queue->max_fill - fill_level;
-	EFX_BUG_ON_PARANOID(space < EFX_RX_BATCH);
+	EFX_BUG_ON_PARANOID(space < batch_size);
 
 	netif_vdbg(rx_queue->efx, rx_status, rx_queue->efx->net_dev,
 		   "RX queue %d fast-filling descriptor ring from"
-		   " level %d to level %d using %s allocation\n",
+		   " level %d to level %d\n",
 		   efx_rx_queue_index(rx_queue), fill_level,
-		   rx_queue->max_fill,
-		   channel->rx_alloc_push_pages ? "page" : "skb");
+		   rx_queue->max_fill);
+
 
 	do {
-		if (channel->rx_alloc_push_pages)
-			rc = efx_init_rx_buffers_page(rx_queue);
-		else
-			rc = efx_init_rx_buffers_skb(rx_queue);
+		rc = efx_init_rx_buffers(rx_queue);
 		if (unlikely(rc)) {
 			/* Ensure that we don't leave the rx queue empty */
 			if (rx_queue->added_count == rx_queue->removed_count)
 				efx_schedule_slow_fill(rx_queue);
 			goto out;
 		}
-	} while ((space -= EFX_RX_BATCH) >= EFX_RX_BATCH);
+	} while ((space -= batch_size) >= batch_size);
 
 	netif_vdbg(rx_queue->efx, rx_status, rx_queue->efx->net_dev,
 		   "RX queue %d fast-filled descriptor ring "
@@ -408,7 +366,7 @@ void efx_rx_slow_fill(unsigned long context)
 
 static void efx_rx_packet__check_len(struct efx_rx_queue *rx_queue,
 				     struct efx_rx_buffer *rx_buf,
-				     int len, bool *leak_packet)
+				     int len)
 {
 	struct efx_nic *efx = rx_queue->efx;
 	unsigned max_len = rx_buf->len - efx->type->rx_buffer_padding;
@@ -428,11 +386,6 @@ static void efx_rx_packet__check_len(struct efx_rx_queue *rx_queue,
 				  "RX event (0x%x > 0x%x+0x%x). Leaking\n",
 				  efx_rx_queue_index(rx_queue), len, max_len,
 				  efx->type->rx_buffer_padding);
-		/* If this buffer was skb-allocated, then the meta
-		 * data at the end of the skb will be trashed. So
-		 * we have no choice but to leak the fragment.
-		 */
-		*leak_packet = !(rx_buf->flags & EFX_RX_BUF_PAGE);
 		efx_schedule_reset(efx, RESET_TYPE_RX_RECOVERY);
 	} else {
 		if (net_ratelimit())
@@ -448,212 +401,238 @@ static void efx_rx_packet__check_len(struct efx_rx_queue *rx_queue,
 /* Pass a received packet up through GRO.  GRO can handle pages
  * regardless of checksum state and skbs with a good checksum.
  */
-static void efx_rx_packet_gro(struct efx_channel *channel,
-			      struct efx_rx_buffer *rx_buf,
-			      const u8 *eh)
+static void
+efx_rx_packet_gro(struct efx_channel *channel, struct efx_rx_buffer *rx_buf,
+		  unsigned int n_frags, u8 *eh)
 {
 	struct napi_struct *napi = &channel->napi_str;
 	gro_result_t gro_result;
+	struct efx_nic *efx = channel->efx;
+	struct sk_buff *skb;
 
-	if (rx_buf->flags & EFX_RX_BUF_PAGE) {
-		struct efx_nic *efx = channel->efx;
-		struct page *page = rx_buf->u.page;
-		struct sk_buff *skb;
+	skb = napi_get_frags(napi);
+	if (unlikely(!skb)) {
+		while (n_frags--) {
+			put_page(rx_buf->page);
+			rx_buf->page = NULL;
+			rx_buf = efx_rx_buf_next(&channel->rx_queue, rx_buf);
+		}
+		return;
+	}
 
-		rx_buf->u.page = NULL;
+	if (efx->net_dev->features & NETIF_F_RXHASH)
+		skb->rxhash = efx_rx_buf_hash(eh);
+	skb->ip_summed = ((rx_buf->flags & EFX_RX_PKT_CSUMMED) ?
+			  CHECKSUM_UNNECESSARY : CHECKSUM_NONE);
+
+	for (;;) {
+		skb_fill_page_desc(skb, skb_shinfo(skb)->nr_frags,
+				   rx_buf->page, rx_buf->page_offset,
+				   rx_buf->len);
+		rx_buf->page = NULL;
+		skb->len += rx_buf->len;
+		if (skb_shinfo(skb)->nr_frags == n_frags)
+			break;
+
+		rx_buf = efx_rx_buf_next(&channel->rx_queue, rx_buf);
+	}
 
-		skb = napi_get_frags(napi);
-		if (!skb) {
-			put_page(page);
-			return;
-		}
+	skb->data_len = skb->len;
+	skb->truesize += n_frags * efx->rx_buffer_truesize;
+
+	skb_record_rx_queue(skb, channel->rx_queue.core_index);
+
+	gro_result = napi_gro_frags(napi);
+	if (gro_result != GRO_DROP)
+		channel->irq_mod_score += 2;
+}
 
-		if (efx->net_dev->features & NETIF_F_RXHASH)
-			skb->rxhash = efx_rx_buf_hash(eh);
+/* Allocate and construct an SKB around page fragments */
+static struct sk_buff *efx_rx_mk_skb(struct efx_channel *channel,
+				     struct efx_rx_buffer *rx_buf,
+				     unsigned int n_frags,
+				     u8 *eh, int hdr_len)
+{
+	struct efx_nic *efx = channel->efx;
+	struct sk_buff *skb;
 
-		skb_fill_page_desc(skb, 0, page,
-				   efx_rx_buf_offset(efx, rx_buf), rx_buf->len);
+	/* Allocate an SKB to store the headers */
+	skb = netdev_alloc_skb(efx->net_dev, hdr_len + EFX_PAGE_SKB_ALIGN);
+	if (unlikely(skb == NULL))
+		return NULL;
 
-		skb->len = rx_buf->len;
-		skb->data_len = rx_buf->len;
-		skb->truesize += rx_buf->len;
-		skb->ip_summed = ((rx_buf->flags & EFX_RX_PKT_CSUMMED) ?
-				  CHECKSUM_UNNECESSARY : CHECKSUM_NONE);
+	EFX_BUG_ON_PARANOID(rx_buf->len < hdr_len);
 
-		skb_record_rx_queue(skb, channel->rx_queue.core_index);
+	skb_reserve(skb, EFX_PAGE_SKB_ALIGN);
+	memcpy(__skb_put(skb, hdr_len), eh, hdr_len);
 
-		gro_result = napi_gro_frags(napi);
-	} else {
-		struct sk_buff *skb = rx_buf->u.skb;
+	/* Append the remaining page(s) onto the frag list */
+	if (rx_buf->len > hdr_len) {
+		rx_buf->page_offset += hdr_len;
+		rx_buf->len -= hdr_len;
 
-		EFX_BUG_ON_PARANOID(!(rx_buf->flags & EFX_RX_PKT_CSUMMED));
-		rx_buf->u.skb = NULL;
-		skb->ip_summed = CHECKSUM_UNNECESSARY;
+		for (;;) {
+			skb_fill_page_desc(skb, skb_shinfo(skb)->nr_frags,
+					   rx_buf->page, rx_buf->page_offset,
+					   rx_buf->len);
+			rx_buf->page = NULL;
+			skb->len += rx_buf->len;
+			skb->data_len += rx_buf->len;
+			if (skb_shinfo(skb)->nr_frags == n_frags)
+				break;
 
-		gro_result = napi_gro_receive(napi, skb);
+			rx_buf = efx_rx_buf_next(&channel->rx_queue, rx_buf);
+		}
+	} else {
+		__free_pages(rx_buf->page, efx->rx_buffer_order);
+		rx_buf->page = NULL;
+		n_frags = 0;
 	}
 
-	if (gro_result == GRO_NORMAL) {
-		channel->rx_alloc_level += RX_ALLOC_FACTOR_SKB;
-	} else if (gro_result != GRO_DROP) {
-		channel->rx_alloc_level += RX_ALLOC_FACTOR_GRO;
-		channel->irq_mod_score += 2;
-	}
+	skb->truesize += n_frags * efx->rx_buffer_truesize;
+
+	/* Move past the ethernet header */
+	skb->protocol = eth_type_trans(skb, efx->net_dev);
+
+	return skb;
 }
 
 void efx_rx_packet(struct efx_rx_queue *rx_queue, unsigned int index,
-		   unsigned int len, u16 flags)
+		   unsigned int n_frags, unsigned int len, u16 flags)
 {
 	struct efx_nic *efx = rx_queue->efx;
 	struct efx_channel *channel = efx_rx_queue_channel(rx_queue);
 	struct efx_rx_buffer *rx_buf;
-	bool leak_packet = false;
 
 	rx_buf = efx_rx_buffer(rx_queue, index);
 	rx_buf->flags |= flags;
 
-	/* This allows the refill path to post another buffer.
-	 * EFX_RXD_HEAD_ROOM ensures that the slot we are using
-	 * isn't overwritten yet.
-	 */
-	rx_queue->removed_count++;
-
-	/* Validate the length encoded in the event vs the descriptor pushed */
-	efx_rx_packet__check_len(rx_queue, rx_buf, len, &leak_packet);
+	/* Validate the number of fragments and completed length */
+	if (n_frags == 1) {
+		efx_rx_packet__check_len(rx_queue, rx_buf, len);
+	} else if (unlikely(n_frags > EFX_RX_MAX_FRAGS) ||
+		   unlikely(len <= (n_frags - 1) * EFX_RX_USR_BUF_SIZE) ||
+		   unlikely(len > n_frags * EFX_RX_USR_BUF_SIZE) ||
+		   unlikely(!efx->rx_scatter)) {
+		/* If this isn't an explicit discard request, either
+		 * the hardware or the driver is broken.
+		 */
+		WARN_ON(!(len == 0 && rx_buf->flags & EFX_RX_PKT_DISCARD));
+		rx_buf->flags |= EFX_RX_PKT_DISCARD;
+	}
 
 	netif_vdbg(efx, rx_status, efx->net_dev,
-		   "RX queue %d received id %x at %llx+%x %s%s\n",
+		   "RX queue %d received ids %x-%x len %d %s%s\n",
 		   efx_rx_queue_index(rx_queue), index,
-		   (unsigned long long)rx_buf->dma_addr, len,
+		   (index + n_frags - 1) & rx_queue->ptr_mask, len,
 		   (rx_buf->flags & EFX_RX_PKT_CSUMMED) ? " [SUMMED]" : "",
 		   (rx_buf->flags & EFX_RX_PKT_DISCARD) ? " [DISCARD]" : "");
 
-	/* Discard packet, if instructed to do so */
+	/* Discard packet, if instructed to do so.  Process the
+	 * previous receive first.
+	 */
 	if (unlikely(rx_buf->flags & EFX_RX_PKT_DISCARD)) {
-		if (unlikely(leak_packet))
-			channel->n_skbuff_leaks++;
-		else
-			efx_recycle_rx_buffer(channel, rx_buf);
-
-		/* Don't hold off the previous receive */
-		rx_buf = NULL;
-		goto out;
+		efx_rx_flush_packet(channel);
+		put_page(rx_buf->page);
+		efx_recycle_rx_buffers(channel, rx_buf, n_frags);
+		return;
 	}
 
-	/* Release and/or sync DMA mapping - assumes all RX buffers
-	 * consumed in-order per RX queue
+	if (n_frags == 1)
+		rx_buf->len = len;
+
+	/* Release and/or sync the DMA mapping - assumes all RX buffers
+	 * consumed in-order per RX queue.
 	 */
-	efx_unmap_rx_buffer(efx, rx_buf, len);
+	efx_sync_rx_buffer(efx, rx_buf, rx_buf->len);
 
 	/* Prefetch nice and early so data will (hopefully) be in cache by
 	 * the time we look at it.
 	 */
-	prefetch(efx_rx_buf_eh(efx, rx_buf));
+	prefetch(efx_rx_buf_va(rx_buf));
+
+	rx_buf->page_offset += efx->type->rx_buffer_hash_size;
+	rx_buf->len -= efx->type->rx_buffer_hash_size;
+
+	if (n_frags > 1) {
+		/* Release/sync DMA mapping for additional fragments.
+		 * Fix length for last fragment.
+		 */
+		unsigned int tail_frags = n_frags - 1;
+
+		for (;;) {
+			rx_buf = efx_rx_buf_next(rx_queue, rx_buf);
+			if (--tail_frags == 0)
+				break;
+			efx_sync_rx_buffer(efx, rx_buf, EFX_RX_USR_BUF_SIZE);
+		}
+		rx_buf->len = len - (n_frags - 1) * EFX_RX_USR_BUF_SIZE;
+		efx_sync_rx_buffer(efx, rx_buf, rx_buf->len);
+	}
+
+	/* All fragments have been DMA-synced, so recycle buffers and pages. */
+	rx_buf = efx_rx_buffer(rx_queue, index);
+	efx_recycle_rx_buffers(channel, rx_buf, n_frags);
 
 	/* Pipeline receives so that we give time for packet headers to be
 	 * prefetched into cache.
 	 */
-	rx_buf->len = len - efx->type->rx_buffer_hash_size;
-out:
-	if (channel->rx_pkt)
-		__efx_rx_packet(channel, channel->rx_pkt);
-	channel->rx_pkt = rx_buf;
+	efx_rx_flush_packet(channel);
+	channel->rx_pkt_n_frags = n_frags;
+	channel->rx_pkt_index = index;
 }
 
-static void efx_rx_deliver(struct efx_channel *channel,
-			   struct efx_rx_buffer *rx_buf)
+static void efx_rx_deliver(struct efx_channel *channel, u8 *eh,
+			   struct efx_rx_buffer *rx_buf,
+			   unsigned int n_frags)
 {
 	struct sk_buff *skb;
+	u16 hdr_len = min_t(u16, rx_buf->len, EFX_SKB_HEADERS);
 
-	/* We now own the SKB */
-	skb = rx_buf->u.skb;
-	rx_buf->u.skb = NULL;
+	skb = efx_rx_mk_skb(channel, rx_buf, n_frags, eh, hdr_len);
+	if (unlikely(skb == NULL)) {
+		efx_free_rx_buffer(rx_buf);
+		return;
+	}
+	skb_record_rx_queue(skb, channel->rx_queue.core_index);
 
 	/* Set the SKB flags */
 	skb_checksum_none_assert(skb);
 
-	/* Record the rx_queue */
-	skb_record_rx_queue(skb, channel->rx_queue.core_index);
-
-	/* Pass the packet up */
 	if (channel->type->receive_skb)
-		channel->type->receive_skb(channel, skb);
-	else
-		netif_receive_skb(skb);
+		if (channel->type->receive_skb(channel, skb))
+			return;
 
-	/* Update allocation strategy method */
-	channel->rx_alloc_level += RX_ALLOC_FACTOR_SKB;
+	/* Pass the packet up */
+	netif_receive_skb(skb);
 }
 
 /* Handle a received packet.  Second half: Touches packet payload. */
-void __efx_rx_packet(struct efx_channel *channel, struct efx_rx_buffer *rx_buf)
+void __efx_rx_packet(struct efx_channel *channel)
 {
 	struct efx_nic *efx = channel->efx;
-	u8 *eh = efx_rx_buf_eh(efx, rx_buf);
+	struct efx_rx_buffer *rx_buf =
+		efx_rx_buffer(&channel->rx_queue, channel->rx_pkt_index);
+	u8 *eh = efx_rx_buf_va(rx_buf);
 
 	/* If we're in loopback test, then pass the packet directly to the
 	 * loopback layer, and free the rx_buf here
 	 */
 	if (unlikely(efx->loopback_selftest)) {
 		efx_loopback_rx_packet(efx, eh, rx_buf->len);
-		efx_free_rx_buffer(efx, rx_buf);
-		return;
-	}
-
-	if (!(rx_buf->flags & EFX_RX_BUF_PAGE)) {
-		struct sk_buff *skb = rx_buf->u.skb;
-
-		prefetch(skb_shinfo(skb));
-
-		skb_reserve(skb, efx->type->rx_buffer_hash_size);
-		skb_put(skb, rx_buf->len);
-
-		if (efx->net_dev->features & NETIF_F_RXHASH)
-			skb->rxhash = efx_rx_buf_hash(eh);
-
-		/* Move past the ethernet header. rx_buf->data still points
-		 * at the ethernet header */
-		skb->protocol = eth_type_trans(skb, efx->net_dev);
-
-		skb_record_rx_queue(skb, channel->rx_queue.core_index);
+		efx_free_rx_buffer(rx_buf);
+		goto out;
 	}
 
 	if (unlikely(!(efx->net_dev->features & NETIF_F_RXCSUM)))
 		rx_buf->flags &= ~EFX_RX_PKT_CSUMMED;
 
-	if (likely(rx_buf->flags & (EFX_RX_BUF_PAGE | EFX_RX_PKT_CSUMMED)) &&
-	    !channel->type->receive_skb)
-		efx_rx_packet_gro(channel, rx_buf, eh);
+	if (!channel->type->receive_skb)
+		efx_rx_packet_gro(channel, rx_buf, channel->rx_pkt_n_frags, eh);
 	else
-		efx_rx_deliver(channel, rx_buf);
-}
-
-void efx_rx_strategy(struct efx_channel *channel)
-{
-	enum efx_rx_alloc_method method = rx_alloc_method;
-
-	if (channel->type->receive_skb) {
-		channel->rx_alloc_push_pages = false;
-		return;
-	}
-
-	/* Only makes sense to use page based allocation if GRO is enabled */
-	if (!(channel->efx->net_dev->features & NETIF_F_GRO)) {
-		method = RX_ALLOC_METHOD_SKB;
-	} else if (method == RX_ALLOC_METHOD_AUTO) {
-		/* Constrain the rx_alloc_level */
-		if (channel->rx_alloc_level < 0)
-			channel->rx_alloc_level = 0;
-		else if (channel->rx_alloc_level > RX_ALLOC_LEVEL_MAX)
-			channel->rx_alloc_level = RX_ALLOC_LEVEL_MAX;
-
-		/* Decide on the allocation method */
-		method = ((channel->rx_alloc_level > RX_ALLOC_LEVEL_GRO) ?
-			  RX_ALLOC_METHOD_PAGE : RX_ALLOC_METHOD_SKB);
-	}
-
-	/* Push the option */
-	channel->rx_alloc_push_pages = (method == RX_ALLOC_METHOD_PAGE);
+		efx_rx_deliver(channel, eh, rx_buf, channel->rx_pkt_n_frags);
+out:
+	channel->rx_pkt_n_frags = 0;
 }
 
 int efx_probe_rx_queue(struct efx_rx_queue *rx_queue)
@@ -683,9 +662,32 @@ int efx_probe_rx_queue(struct efx_rx_queue *rx_queue)
 		kfree(rx_queue->buffer);
 		rx_queue->buffer = NULL;
 	}
+
 	return rc;
 }
 
+static void efx_init_rx_recycle_ring(struct efx_nic *efx,
+				     struct efx_rx_queue *rx_queue)
+{
+	unsigned int bufs_in_recycle_ring, page_ring_size;
+
+	/* Set the RX recycle ring size */
+#ifdef CONFIG_PPC64
+	bufs_in_recycle_ring = EFX_RECYCLE_RING_SIZE_IOMMU;
+#else
+	if (efx->pci_dev->dev.iommu_group)
+		bufs_in_recycle_ring = EFX_RECYCLE_RING_SIZE_IOMMU;
+	else
+		bufs_in_recycle_ring = EFX_RECYCLE_RING_SIZE_NOIOMMU;
+#endif /* CONFIG_PPC64 */
+
+	page_ring_size = roundup_pow_of_two(bufs_in_recycle_ring /
+					    efx->rx_bufs_per_page);
+	rx_queue->page_ring = kcalloc(page_ring_size,
+				      sizeof(*rx_queue->page_ring), GFP_KERNEL);
+	rx_queue->page_ptr_mask = page_ring_size - 1;
+}
+
 void efx_init_rx_queue(struct efx_rx_queue *rx_queue)
 {
 	struct efx_nic *efx = rx_queue->efx;
@@ -699,10 +701,18 @@ void efx_init_rx_queue(struct efx_rx_queue *rx_queue)
 	rx_queue->notified_count = 0;
 	rx_queue->removed_count = 0;
 	rx_queue->min_fill = -1U;
+	efx_init_rx_recycle_ring(efx, rx_queue);
+
+	rx_queue->page_remove = 0;
+	rx_queue->page_add = rx_queue->page_ptr_mask + 1;
+	rx_queue->page_recycle_count = 0;
+	rx_queue->page_recycle_failed = 0;
+	rx_queue->page_recycle_full = 0;
 
 	/* Initialise limit fields */
 	max_fill = efx->rxq_entries - EFX_RXD_HEAD_ROOM;
-	max_trigger = max_fill - EFX_RX_BATCH;
+	max_trigger =
+		max_fill - efx->rx_pages_per_batch * efx->rx_bufs_per_page;
 	if (rx_refill_threshold != 0) {
 		trigger = max_fill * min(rx_refill_threshold, 100U) / 100U;
 		if (trigger > max_trigger)
@@ -722,6 +732,7 @@ void efx_init_rx_queue(struct efx_rx_queue *rx_queue)
 void efx_fini_rx_queue(struct efx_rx_queue *rx_queue)
 {
 	int i;
+	struct efx_nic *efx = rx_queue->efx;
 	struct efx_rx_buffer *rx_buf;
 
 	netif_dbg(rx_queue->efx, drv, rx_queue->efx->net_dev,
@@ -733,13 +744,32 @@ void efx_fini_rx_queue(struct efx_rx_queue *rx_queue)
 	del_timer_sync(&rx_queue->slow_fill);
 	efx_nic_fini_rx(rx_queue);
 
-	/* Release RX buffers NB start at index 0 not current HW ptr */
+	/* Release RX buffers from the current read ptr to the write ptr */
 	if (rx_queue->buffer) {
-		for (i = 0; i <= rx_queue->ptr_mask; i++) {
-			rx_buf = efx_rx_buffer(rx_queue, i);
+		for (i = rx_queue->removed_count; i < rx_queue->added_count;
+		     i++) {
+			unsigned index = i & rx_queue->ptr_mask;
+			rx_buf = efx_rx_buffer(rx_queue, index);
 			efx_fini_rx_buffer(rx_queue, rx_buf);
 		}
 	}
+
+	/* Unmap and release the pages in the recycle ring. Remove the ring. */
+	for (i = 0; i <= rx_queue->page_ptr_mask; i++) {
+		struct page *page = rx_queue->page_ring[i];
+		struct efx_rx_page_state *state;
+
+		if (page == NULL)
+			continue;
+
+		state = page_address(page);
+		dma_unmap_page(&efx->pci_dev->dev, state->dma_addr,
+			       PAGE_SIZE << efx->rx_buffer_order,
+			       DMA_FROM_DEVICE);
+		put_page(page);
+	}
+	kfree(rx_queue->page_ring);
+	rx_queue->page_ring = NULL;
 }
 
 void efx_remove_rx_queue(struct efx_rx_queue *rx_queue)
@@ -754,9 +784,6 @@ void efx_remove_rx_queue(struct efx_rx_queue *rx_queue)
 }
 
 
-module_param(rx_alloc_method, int, 0644);
-MODULE_PARM_DESC(rx_alloc_method, "Allocation method used for RX buffers");
-
 module_param(rx_refill_threshold, uint, 0444);
 MODULE_PARM_DESC(rx_refill_threshold,
 		 "RX descriptor ring refill threshold (%)");
diff --git a/drivers/net/ethernet/sfc/siena.c b/drivers/net/ethernet/sfc/siena.c
index ba40f67e4f05..51669244d154 100644
--- a/drivers/net/ethernet/sfc/siena.c
+++ b/drivers/net/ethernet/sfc/siena.c
@@ -202,7 +202,7 @@ out:
 
 static enum reset_type siena_map_reset_reason(enum reset_type reason)
 {
-	return RESET_TYPE_ALL;
+	return RESET_TYPE_RECOVER_OR_ALL;
 }
 
 static int siena_map_reset_flags(u32 *flags)
@@ -245,6 +245,22 @@ static int siena_reset_hw(struct efx_nic *efx, enum reset_type method)
 		return efx_mcdi_reset_port(efx);
 }
 
+#ifdef CONFIG_EEH
+/* When a PCI device is isolated from the bus, a subsequent MMIO read is
+ * required for the kernel EEH mechanisms to notice. As the Solarflare driver
+ * was written to minimise MMIO read (for latency) then a periodic call to check
+ * the EEH status of the device is required so that device recovery can happen
+ * in a timely fashion.
+ */
+static void siena_monitor(struct efx_nic *efx)
+{
+	struct eeh_dev *eehdev =
+		of_node_to_eeh_dev(pci_device_to_OF_node(efx->pci_dev));
+
+	eeh_dev_check_failure(eehdev);
+}
+#endif
+
 static int siena_probe_nvconfig(struct efx_nic *efx)
 {
 	u32 caps = 0;
@@ -398,6 +414,8 @@ static int siena_init_nic(struct efx_nic *efx)
 	EFX_SET_OWORD_FIELD(temp, FRF_BZ_RX_HASH_INSRT_HDR, 1);
 	EFX_SET_OWORD_FIELD(temp, FRF_BZ_RX_HASH_ALG, 1);
 	EFX_SET_OWORD_FIELD(temp, FRF_BZ_RX_IP_HASH, 1);
+	EFX_SET_OWORD_FIELD(temp, FRF_BZ_RX_USR_BUF_SIZE,
+			    EFX_RX_USR_BUF_SIZE >> 5);
 	efx_writeo(efx, &temp, FR_AZ_RX_CFG);
 
 	/* Set hash key for IPv4 */
@@ -665,7 +683,11 @@ const struct efx_nic_type siena_a0_nic_type = {
 	.init = siena_init_nic,
 	.dimension_resources = siena_dimension_resources,
 	.fini = efx_port_dummy_op_void,
+#ifdef CONFIG_EEH
+	.monitor = siena_monitor,
+#else
 	.monitor = NULL,
+#endif
 	.map_reset_reason = siena_map_reset_reason,
 	.map_reset_flags = siena_map_reset_flags,
 	.reset = siena_reset_hw,
@@ -698,6 +720,7 @@ const struct efx_nic_type siena_a0_nic_type = {
 	.max_dma_mask = DMA_BIT_MASK(FSF_AZ_TX_KER_BUF_ADDR_WIDTH),
 	.rx_buffer_hash_size = 0x10,
 	.rx_buffer_padding = 0,
+	.can_rx_scatter = true,
 	.max_interrupt_mode = EFX_INT_MODE_MSIX,
 	.phys_addr_channels = 32, /* Hardware limit is 64, but the legacy
 				   * interrupt handler only supports 32
diff --git a/drivers/net/ethernet/sgi/meth.c b/drivers/net/ethernet/sgi/meth.c
index 79ad9c94a21b..4bdbaad9932d 100644
--- a/drivers/net/ethernet/sgi/meth.c
+++ b/drivers/net/ethernet/sgi/meth.c
@@ -213,10 +213,11 @@ static int meth_init_tx_ring(struct meth_private *priv)
 {
 	/* Init TX ring */
 	priv->tx_ring = dma_alloc_coherent(NULL, TX_RING_BUFFER_SIZE,
-	                                   &priv->tx_ring_dma, GFP_ATOMIC);
+	                                   &priv->tx_ring_dma,
+					   GFP_ATOMIC | __GFP_ZERO);
 	if (!priv->tx_ring)
 		return -ENOMEM;
-	memset(priv->tx_ring, 0, TX_RING_BUFFER_SIZE);
+
 	priv->tx_count = priv->tx_read = priv->tx_write = 0;
 	mace->eth.tx_ring_base = priv->tx_ring_dma;
 	/* Now init skb save area */
diff --git a/drivers/net/ethernet/sis/sis900.c b/drivers/net/ethernet/sis/sis900.c
index efca14eaefa9..eb4aea3fe793 100644
--- a/drivers/net/ethernet/sis/sis900.c
+++ b/drivers/net/ethernet/sis/sis900.c
@@ -1187,8 +1187,14 @@ sis900_init_rx_ring(struct net_device *net_dev)
 		}
 		sis_priv->rx_skbuff[i] = skb;
 		sis_priv->rx_ring[i].cmdsts = RX_BUF_SIZE;
-                sis_priv->rx_ring[i].bufptr = pci_map_single(sis_priv->pci_dev,
-                        skb->data, RX_BUF_SIZE, PCI_DMA_FROMDEVICE);
+		sis_priv->rx_ring[i].bufptr = pci_map_single(sis_priv->pci_dev,
+				skb->data, RX_BUF_SIZE, PCI_DMA_FROMDEVICE);
+		if (unlikely(pci_dma_mapping_error(sis_priv->pci_dev,
+				sis_priv->rx_ring[i].bufptr))) {
+			dev_kfree_skb(skb);
+			sis_priv->rx_skbuff[i] = NULL;
+			break;
+		}
 	}
 	sis_priv->dirty_rx = (unsigned int) (i - NUM_RX_DESC);
 
@@ -1621,6 +1627,14 @@ sis900_start_xmit(struct sk_buff *skb, struct net_device *net_dev)
 	/* set the transmit buffer descriptor and enable Transmit State Machine */
 	sis_priv->tx_ring[entry].bufptr = pci_map_single(sis_priv->pci_dev,
 		skb->data, skb->len, PCI_DMA_TODEVICE);
+	if (unlikely(pci_dma_mapping_error(sis_priv->pci_dev,
+		sis_priv->tx_ring[entry].bufptr))) {
+			dev_kfree_skb(skb);
+			sis_priv->tx_skbuff[entry] = NULL;
+			net_dev->stats.tx_dropped++;
+			spin_unlock_irqrestore(&sis_priv->lock, flags);
+			return NETDEV_TX_OK;
+	}
 	sis_priv->tx_ring[entry].cmdsts = (OWN | skb->len);
 	sw32(cr, TxENA | sr32(cr));
 
@@ -1824,9 +1838,15 @@ static int sis900_rx(struct net_device *net_dev)
 refill_rx_ring:
 			sis_priv->rx_skbuff[entry] = skb;
 			sis_priv->rx_ring[entry].cmdsts = RX_BUF_SIZE;
-                	sis_priv->rx_ring[entry].bufptr =
+			sis_priv->rx_ring[entry].bufptr =
 				pci_map_single(sis_priv->pci_dev, skb->data,
 					RX_BUF_SIZE, PCI_DMA_FROMDEVICE);
+			if (unlikely(pci_dma_mapping_error(sis_priv->pci_dev,
+				sis_priv->rx_ring[entry].bufptr))) {
+				dev_kfree_skb_irq(skb);
+				sis_priv->rx_skbuff[entry] = NULL;
+				break;
+			}
 		}
 		sis_priv->cur_rx++;
 		entry = sis_priv->cur_rx % NUM_RX_DESC;
@@ -1841,23 +1861,26 @@ refill_rx_ring:
 		entry = sis_priv->dirty_rx % NUM_RX_DESC;
 
 		if (sis_priv->rx_skbuff[entry] == NULL) {
-			if ((skb = netdev_alloc_skb(net_dev, RX_BUF_SIZE)) == NULL) {
+			skb = netdev_alloc_skb(net_dev, RX_BUF_SIZE);
+			if (skb == NULL) {
 				/* not enough memory for skbuff, this makes a
 				 * "hole" on the buffer ring, it is not clear
 				 * how the hardware will react to this kind
 				 * of degenerated buffer */
-				if (netif_msg_rx_err(sis_priv))
-					printk(KERN_INFO "%s: Memory squeeze, "
-						"deferring packet.\n",
-						net_dev->name);
 				net_dev->stats.rx_dropped++;
 				break;
 			}
 			sis_priv->rx_skbuff[entry] = skb;
 			sis_priv->rx_ring[entry].cmdsts = RX_BUF_SIZE;
-                	sis_priv->rx_ring[entry].bufptr =
+			sis_priv->rx_ring[entry].bufptr =
 				pci_map_single(sis_priv->pci_dev, skb->data,
 					RX_BUF_SIZE, PCI_DMA_FROMDEVICE);
+			if (unlikely(pci_dma_mapping_error(sis_priv->pci_dev,
+					sis_priv->rx_ring[entry].bufptr))) {
+				dev_kfree_skb_irq(skb);
+				sis_priv->rx_skbuff[entry] = NULL;
+				break;
+			}
 		}
 	}
 	/* re-enable the potentially idle receive state matchine */
diff --git a/drivers/net/ethernet/smsc/Kconfig b/drivers/net/ethernet/smsc/Kconfig
index 5a689af516e9..bb4c1674ff99 100644
--- a/drivers/net/ethernet/smsc/Kconfig
+++ b/drivers/net/ethernet/smsc/Kconfig
@@ -5,7 +5,7 @@
 config NET_VENDOR_SMSC
 	bool "SMC (SMSC)/Western Digital devices"
 	default y
-	depends on ARM || ISA || MAC || ARM || MIPS || M32R || SUPERH || \
+	depends on ARM || ISA || MAC || ARM64 || MIPS || M32R || SUPERH || \
 		BLACKFIN || MN10300 || COLDFIRE || PCI || PCMCIA
 	---help---
 	  If you have a network (Ethernet) card belonging to this class, say Y
@@ -40,7 +40,7 @@ config SMC91X
 	select NET_CORE
 	select MII
 	depends on (ARM || M32R || SUPERH || MIPS || BLACKFIN || \
-		    MN10300 || COLDFIRE)
+		    MN10300 || COLDFIRE || ARM64)
 	---help---
 	  This is a driver for SMC's 91x series of Ethernet chipsets,
 	  including the SMC91C94 and the SMC91C111. Say Y if you want it
diff --git a/drivers/net/ethernet/smsc/smc9194.c b/drivers/net/ethernet/smsc/smc9194.c
index 50823da9dc1e..e85c2e7e8246 100644
--- a/drivers/net/ethernet/smsc/smc9194.c
+++ b/drivers/net/ethernet/smsc/smc9194.c
@@ -1223,9 +1223,7 @@ static void smc_rcv(struct net_device *dev)
 			dev->stats.multicast++;
 
 		skb = netdev_alloc_skb(dev, packet_length + 5);
-
 		if ( skb == NULL ) {
-			printk(KERN_NOTICE CARDNAME ": Low memory, packet dropped.\n");
 			dev->stats.rx_dropped++;
 			goto done;
 		}
diff --git a/drivers/net/ethernet/smsc/smc91x.c b/drivers/net/ethernet/smsc/smc91x.c
index 591650a8de38..dfbf978315df 100644
--- a/drivers/net/ethernet/smsc/smc91x.c
+++ b/drivers/net/ethernet/smsc/smc91x.c
@@ -465,8 +465,6 @@ static inline void  smc_rcv(struct net_device *dev)
 		 */
 		skb = netdev_alloc_skb(dev, packet_len);
 		if (unlikely(skb == NULL)) {
-			printk(KERN_NOTICE "%s: Low memory, packet dropped.\n",
-				dev->name);
 			SMC_WAIT_MMU_BUSY(lp);
 			SMC_SET_MMU_CMD(lp, MC_RELEASE);
 			dev->stats.rx_dropped++;
diff --git a/drivers/net/ethernet/smsc/smsc911x.c b/drivers/net/ethernet/smsc/smsc911x.c
index da5cc9a3b34c..3663b9e04a31 100644
--- a/drivers/net/ethernet/smsc/smsc911x.c
+++ b/drivers/net/ethernet/smsc/smsc911x.c
@@ -33,6 +33,7 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/crc32.h>
+#include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/errno.h>
 #include <linux/etherdevice.h>
@@ -144,6 +145,9 @@ struct smsc911x_data {
 
 	/* regulators */
 	struct regulator_bulk_data supplies[SMSC911X_NUM_SUPPLIES];
+
+	/* clock */
+	struct clk *clk;
 };
 
 /* Easy access to information */
@@ -369,7 +373,7 @@ out:
 }
 
 /*
- * enable resources, currently just regulators.
+ * enable regulator and clock resources.
  */
 static int smsc911x_enable_resources(struct platform_device *pdev)
 {
@@ -382,6 +386,13 @@ static int smsc911x_enable_resources(struct platform_device *pdev)
 	if (ret)
 		netdev_err(ndev, "failed to enable regulators %d\n",
 				ret);
+
+	if (!IS_ERR(pdata->clk)) {
+		ret = clk_prepare_enable(pdata->clk);
+		if (ret < 0)
+			netdev_err(ndev, "failed to enable clock %d\n", ret);
+	}
+
 	return ret;
 }
 
@@ -396,6 +407,10 @@ static int smsc911x_disable_resources(struct platform_device *pdev)
 
 	ret = regulator_bulk_disable(ARRAY_SIZE(pdata->supplies),
 			pdata->supplies);
+
+	if (!IS_ERR(pdata->clk))
+		clk_disable_unprepare(pdata->clk);
+
 	return ret;
 }
 
@@ -421,6 +436,12 @@ static int smsc911x_request_resources(struct platform_device *pdev)
 	if (ret)
 		netdev_err(ndev, "couldn't get regulators %d\n",
 				ret);
+
+	/* Request clock */
+	pdata->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(pdata->clk))
+		netdev_warn(ndev, "couldn't get clock %li\n", PTR_ERR(pdata->clk));
+
 	return ret;
 }
 
@@ -436,6 +457,12 @@ static void smsc911x_free_resources(struct platform_device *pdev)
 	/* Free regulators */
 	regulator_bulk_free(ARRAY_SIZE(pdata->supplies),
 			pdata->supplies);
+
+	/* Free clock */
+	if (!IS_ERR(pdata->clk)) {
+		clk_put(pdata->clk);
+		pdata->clk = NULL;
+	}
 }
 
 /* waits for MAC not busy, with timeout.  Only called by smsc911x_mac_read
@@ -2115,7 +2142,7 @@ static int smsc911x_init(struct net_device *dev)
 	spin_lock_init(&pdata->dev_lock);
 	spin_lock_init(&pdata->mac_lock);
 
-	if (pdata->ioaddr == 0) {
+	if (pdata->ioaddr == NULL) {
 		SMSC_WARN(pdata, probe, "pdata->ioaddr: 0x00000000");
 		return -ENODEV;
 	}
diff --git a/drivers/net/ethernet/smsc/smsc9420.c b/drivers/net/ethernet/smsc/smsc9420.c
index d457fa2d7509..ffa5c4ad1210 100644
--- a/drivers/net/ethernet/smsc/smsc9420.c
+++ b/drivers/net/ethernet/smsc/smsc9420.c
@@ -848,10 +848,8 @@ static int smsc9420_alloc_rx_buffer(struct smsc9420_pdata *pd, int index)
 	BUG_ON(pd->rx_buffers[index].skb);
 	BUG_ON(pd->rx_buffers[index].mapping);
 
-	if (unlikely(!skb)) {
-		smsc_warn(RX_ERR, "Failed to allocate new skb!");
+	if (unlikely(!skb))
 		return -ENOMEM;
-	}
 
 	mapping = pci_map_single(pd->pdev, skb_tail_pointer(skb),
 				 PKT_BUF_SZ, PCI_DMA_FROMDEVICE);
diff --git a/drivers/net/ethernet/stmicro/stmmac/Kconfig b/drivers/net/ethernet/stmicro/stmmac/Kconfig
index c0ea838c78d1..f695a50bac47 100644
--- a/drivers/net/ethernet/stmicro/stmmac/Kconfig
+++ b/drivers/net/ethernet/stmicro/stmmac/Kconfig
@@ -5,6 +5,7 @@ config STMMAC_ETH
 	select MII
 	select PHYLIB
 	select CRC32
+	select PTP_1588_CLOCK
 	---help---
 	  This is the driver for the Ethernet IPs are built around a
 	  Synopsys IP Core and only tested on the STMicroelectronics
@@ -54,22 +55,4 @@ config STMMAC_DA
 	  By default, the DMA arbitration scheme is based on Round-robin
 	  (rx:tx priority is 1:1).
 
-choice
-	prompt "Select the DMA TX/RX descriptor operating modes"
-	depends on STMMAC_ETH
-	---help---
-	  This driver supports DMA descriptor to operate both in dual buffer
-	  (RING) and linked-list(CHAINED) mode. In RING mode each descriptor
-	  points to two data buffer pointers whereas in CHAINED mode they
-	  points to only one data buffer pointer.
-
-config STMMAC_RING
-	bool "Enable Descriptor Ring Mode"
-
-config STMMAC_CHAINED
-	bool "Enable Descriptor Chained Mode"
-
-endchoice
-
-
 endif
diff --git a/drivers/net/ethernet/stmicro/stmmac/Makefile b/drivers/net/ethernet/stmicro/stmmac/Makefile
index c8e8ea60ac19..356a9dd32be7 100644
--- a/drivers/net/ethernet/stmicro/stmmac/Makefile
+++ b/drivers/net/ethernet/stmicro/stmmac/Makefile
@@ -1,9 +1,7 @@
 obj-$(CONFIG_STMMAC_ETH) += stmmac.o
-stmmac-$(CONFIG_STMMAC_RING) += ring_mode.o
-stmmac-$(CONFIG_STMMAC_CHAINED) += chain_mode.o
 stmmac-$(CONFIG_STMMAC_PLATFORM) += stmmac_platform.o
 stmmac-$(CONFIG_STMMAC_PCI) += stmmac_pci.o
-stmmac-objs:= stmmac_main.o stmmac_ethtool.o stmmac_mdio.o	\
-	      dwmac_lib.o dwmac1000_core.o  dwmac1000_dma.o	\
+stmmac-objs:= stmmac_main.o stmmac_ethtool.o stmmac_mdio.o ring_mode.o	\
+	      chain_mode.o dwmac_lib.o dwmac1000_core.o  dwmac1000_dma.o \
 	      dwmac100_core.o dwmac100_dma.o enh_desc.o  norm_desc.o \
-	      mmc_core.o $(stmmac-y)
+	      mmc_core.o stmmac_hwtstamp.o stmmac_ptp.o $(stmmac-y)
diff --git a/drivers/net/ethernet/stmicro/stmmac/chain_mode.c b/drivers/net/ethernet/stmicro/stmmac/chain_mode.c
index 0668659803ed..d234ab540b29 100644
--- a/drivers/net/ethernet/stmicro/stmmac/chain_mode.c
+++ b/drivers/net/ethernet/stmicro/stmmac/chain_mode.c
@@ -28,9 +28,9 @@
 
 #include "stmmac.h"
 
-unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
+static unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 {
-	struct stmmac_priv *priv = (struct stmmac_priv *) p;
+	struct stmmac_priv *priv = (struct stmmac_priv *)p;
 	unsigned int txsize = priv->dma_tx_size;
 	unsigned int entry = priv->cur_tx % txsize;
 	struct dma_desc *desc = priv->dma_tx + entry;
@@ -47,7 +47,8 @@ unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 
 	desc->des2 = dma_map_single(priv->device, skb->data,
 				    bmax, DMA_TO_DEVICE);
-	priv->hw->desc->prepare_tx_desc(desc, 1, bmax, csum);
+	priv->tx_skbuff_dma[entry] = desc->des2;
+	priv->hw->desc->prepare_tx_desc(desc, 1, bmax, csum, STMMAC_CHAIN_MODE);
 
 	while (len != 0) {
 		entry = (++priv->cur_tx) % txsize;
@@ -57,8 +58,9 @@ unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 			desc->des2 = dma_map_single(priv->device,
 						    (skb->data + bmax * i),
 						    bmax, DMA_TO_DEVICE);
-			priv->hw->desc->prepare_tx_desc(desc, 0, bmax,
-							csum);
+			priv->tx_skbuff_dma[entry] = desc->des2;
+			priv->hw->desc->prepare_tx_desc(desc, 0, bmax, csum,
+							STMMAC_CHAIN_MODE);
 			priv->hw->desc->set_tx_owner(desc);
 			priv->tx_skbuff[entry] = NULL;
 			len -= bmax;
@@ -67,8 +69,9 @@ unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 			desc->des2 = dma_map_single(priv->device,
 						    (skb->data + bmax * i), len,
 						    DMA_TO_DEVICE);
-			priv->hw->desc->prepare_tx_desc(desc, 0, len,
-							csum);
+			priv->tx_skbuff_dma[entry] = desc->des2;
+			priv->hw->desc->prepare_tx_desc(desc, 0, len, csum,
+							STMMAC_CHAIN_MODE);
 			priv->hw->desc->set_tx_owner(desc);
 			priv->tx_skbuff[entry] = NULL;
 			len = 0;
@@ -89,49 +92,70 @@ static unsigned int stmmac_is_jumbo_frm(int len, int enh_desc)
 	return ret;
 }
 
-static void stmmac_refill_desc3(int bfsize, struct dma_desc *p)
-{
-}
-
-static void stmmac_init_desc3(int des3_as_data_buf, struct dma_desc *p)
-{
-}
-
-static void stmmac_clean_desc3(struct dma_desc *p)
-{
-}
-
-static void stmmac_init_dma_chain(struct dma_desc *des, dma_addr_t phy_addr,
-				  unsigned int size)
+static void stmmac_init_dma_chain(void *des, dma_addr_t phy_addr,
+				  unsigned int size, unsigned int extend_desc)
 {
 	/*
 	 * In chained mode the des3 points to the next element in the ring.
 	 * The latest element has to point to the head.
 	 */
 	int i;
-	struct dma_desc *p = des;
 	dma_addr_t dma_phy = phy_addr;
 
-	for (i = 0; i < (size - 1); i++) {
-		dma_phy += sizeof(struct dma_desc);
-		p->des3 = (unsigned int)dma_phy;
-		p++;
+	if (extend_desc) {
+		struct dma_extended_desc *p = (struct dma_extended_desc *)des;
+		for (i = 0; i < (size - 1); i++) {
+			dma_phy += sizeof(struct dma_extended_desc);
+			p->basic.des3 = (unsigned int)dma_phy;
+			p++;
+		}
+		p->basic.des3 = (unsigned int)phy_addr;
+
+	} else {
+		struct dma_desc *p = (struct dma_desc *)des;
+		for (i = 0; i < (size - 1); i++) {
+			dma_phy += sizeof(struct dma_desc);
+			p->des3 = (unsigned int)dma_phy;
+			p++;
+		}
+		p->des3 = (unsigned int)phy_addr;
 	}
-	p->des3 = (unsigned int)phy_addr;
 }
 
-static int stmmac_set_16kib_bfsize(int mtu)
+static void stmmac_refill_desc3(void *priv_ptr, struct dma_desc *p)
+{
+	struct stmmac_priv *priv = (struct stmmac_priv *)priv_ptr;
+
+	if (priv->hwts_rx_en && !priv->extend_desc)
+		/* NOTE: Device will overwrite des3 with timestamp value if
+		 * 1588-2002 time stamping is enabled, hence reinitialize it
+		 * to keep explicit chaining in the descriptor.
+		 */
+		p->des3 = (unsigned int)(priv->dma_rx_phy +
+					 (((priv->dirty_rx) + 1) %
+					  priv->dma_rx_size) *
+					 sizeof(struct dma_desc));
+}
+
+static void stmmac_clean_desc3(void *priv_ptr, struct dma_desc *p)
 {
-	/* Not supported */
-	return 0;
+	struct stmmac_priv *priv = (struct stmmac_priv *)priv_ptr;
+
+	if (priv->hw->desc->get_tx_ls(p) && !priv->extend_desc)
+		/* NOTE: Device will overwrite des3 with timestamp value if
+		 * 1588-2002 time stamping is enabled, hence reinitialize it
+		 * to keep explicit chaining in the descriptor.
+		 */
+		p->des3 = (unsigned int)(priv->dma_tx_phy +
+					 (((priv->dirty_tx + 1) %
+					   priv->dma_tx_size) *
+					  sizeof(struct dma_desc)));
 }
 
-const struct stmmac_ring_mode_ops ring_mode_ops = {
+const struct stmmac_chain_mode_ops chain_mode_ops = {
+	.init = stmmac_init_dma_chain,
 	.is_jumbo_frm = stmmac_is_jumbo_frm,
 	.jumbo_frm = stmmac_jumbo_frm,
 	.refill_desc3 = stmmac_refill_desc3,
-	.init_desc3 = stmmac_init_desc3,
-	.init_dma_chain = stmmac_init_dma_chain,
 	.clean_desc3 = stmmac_clean_desc3,
-	.set_16kib_bfsize = stmmac_set_16kib_bfsize,
 };
diff --git a/drivers/net/ethernet/stmicro/stmmac/common.h b/drivers/net/ethernet/stmicro/stmmac/common.h
index 186d14806122..7788fbe44f0a 100644
--- a/drivers/net/ethernet/stmicro/stmmac/common.h
+++ b/drivers/net/ethernet/stmicro/stmmac/common.h
@@ -117,6 +117,36 @@ struct stmmac_extra_stats {
 	unsigned long irq_rx_path_in_lpi_mode_n;
 	unsigned long irq_rx_path_exit_lpi_mode_n;
 	unsigned long phy_eee_wakeup_error_n;
+	/* Extended RDES status */
+	unsigned long ip_hdr_err;
+	unsigned long ip_payload_err;
+	unsigned long ip_csum_bypassed;
+	unsigned long ipv4_pkt_rcvd;
+	unsigned long ipv6_pkt_rcvd;
+	unsigned long rx_msg_type_ext_no_ptp;
+	unsigned long rx_msg_type_sync;
+	unsigned long rx_msg_type_follow_up;
+	unsigned long rx_msg_type_delay_req;
+	unsigned long rx_msg_type_delay_resp;
+	unsigned long rx_msg_type_pdelay_req;
+	unsigned long rx_msg_type_pdelay_resp;
+	unsigned long rx_msg_type_pdelay_follow_up;
+	unsigned long ptp_frame_type;
+	unsigned long ptp_ver;
+	unsigned long timestamp_dropped;
+	unsigned long av_pkt_rcvd;
+	unsigned long av_tagged_pkt_rcvd;
+	unsigned long vlan_tag_priority_val;
+	unsigned long l3_filter_match;
+	unsigned long l4_filter_match;
+	unsigned long l3_l4_filter_no_match;
+	/* PCS */
+	unsigned long irq_pcs_ane_n;
+	unsigned long irq_pcs_link_n;
+	unsigned long irq_rgmii_n;
+	unsigned long pcs_link;
+	unsigned long pcs_duplex;
+	unsigned long pcs_speed;
 };
 
 /* CSR Frequency Access Defines*/
@@ -138,37 +168,43 @@ struct stmmac_extra_stats {
 #define FLOW_TX		2
 #define FLOW_AUTO	(FLOW_TX | FLOW_RX)
 
-#define SF_DMA_MODE 1 /* DMA STORE-AND-FORWARD Operation Mode */
+/* PCS defines */
+#define STMMAC_PCS_RGMII	(1 << 0)
+#define STMMAC_PCS_SGMII	(1 << 1)
+#define STMMAC_PCS_TBI		(1 << 2)
+#define STMMAC_PCS_RTBI		(1 << 3)
+
+#define SF_DMA_MODE 1		/* DMA STORE-AND-FORWARD Operation Mode */
 
 /* DAM HW feature register fields */
-#define DMA_HW_FEAT_MIISEL	0x00000001 /* 10/100 Mbps Support */
-#define DMA_HW_FEAT_GMIISEL	0x00000002 /* 1000 Mbps Support */
-#define DMA_HW_FEAT_HDSEL	0x00000004 /* Half-Duplex Support */
-#define DMA_HW_FEAT_EXTHASHEN	0x00000008 /* Expanded DA Hash Filter */
-#define DMA_HW_FEAT_HASHSEL	0x00000010 /* HASH Filter */
-#define DMA_HW_FEAT_ADDMACADRSEL	0x00000020 /* Multiple MAC Addr Reg */
-#define DMA_HW_FEAT_PCSSEL	0x00000040 /* PCS registers */
-#define DMA_HW_FEAT_L3L4FLTREN	0x00000080 /* Layer 3 & Layer 4 Feature */
-#define DMA_HW_FEAT_SMASEL	0x00000100 /* SMA(MDIO) Interface */
-#define DMA_HW_FEAT_RWKSEL	0x00000200 /* PMT Remote Wakeup */
-#define DMA_HW_FEAT_MGKSEL	0x00000400 /* PMT Magic Packet */
-#define DMA_HW_FEAT_MMCSEL	0x00000800 /* RMON Module */
-#define DMA_HW_FEAT_TSVER1SEL	0x00001000 /* Only IEEE 1588-2002 Timestamp */
-#define DMA_HW_FEAT_TSVER2SEL	0x00002000 /* IEEE 1588-2008 Adv Timestamp */
-#define DMA_HW_FEAT_EEESEL	0x00004000 /* Energy Efficient Ethernet */
-#define DMA_HW_FEAT_AVSEL	0x00008000 /* AV Feature */
-#define DMA_HW_FEAT_TXCOESEL	0x00010000 /* Checksum Offload in Tx */
-#define DMA_HW_FEAT_RXTYP1COE	0x00020000 /* IP csum Offload(Type 1) in Rx */
-#define DMA_HW_FEAT_RXTYP2COE	0x00040000 /* IP csum Offload(Type 2) in Rx */
-#define DMA_HW_FEAT_RXFIFOSIZE	0x00080000 /* Rx FIFO > 2048 Bytes */
-#define DMA_HW_FEAT_RXCHCNT	0x00300000 /* No. of additional Rx Channels */
-#define DMA_HW_FEAT_TXCHCNT	0x00c00000 /* No. of additional Tx Channels */
-#define DMA_HW_FEAT_ENHDESSEL	0x01000000 /* Alternate (Enhanced Descriptor) */
-#define DMA_HW_FEAT_INTTSEN	0x02000000 /* Timestamping with Internal
-					      System Time */
-#define DMA_HW_FEAT_FLEXIPPSEN	0x04000000 /* Flexible PPS Output */
-#define DMA_HW_FEAT_SAVLANINS	0x08000000 /* Source Addr or VLAN Insertion */
-#define DMA_HW_FEAT_ACTPHYIF	0x70000000 /* Active/selected PHY interface */
+#define DMA_HW_FEAT_MIISEL	0x00000001	/* 10/100 Mbps Support */
+#define DMA_HW_FEAT_GMIISEL	0x00000002	/* 1000 Mbps Support */
+#define DMA_HW_FEAT_HDSEL	0x00000004	/* Half-Duplex Support */
+#define DMA_HW_FEAT_EXTHASHEN	0x00000008	/* Expanded DA Hash Filter */
+#define DMA_HW_FEAT_HASHSEL	0x00000010	/* HASH Filter */
+#define DMA_HW_FEAT_ADDMAC	0x00000020	/* Multiple MAC Addr Reg */
+#define DMA_HW_FEAT_PCSSEL	0x00000040	/* PCS registers */
+#define DMA_HW_FEAT_L3L4FLTREN	0x00000080	/* Layer 3 & Layer 4 Feature */
+#define DMA_HW_FEAT_SMASEL	0x00000100	/* SMA(MDIO) Interface */
+#define DMA_HW_FEAT_RWKSEL	0x00000200	/* PMT Remote Wakeup */
+#define DMA_HW_FEAT_MGKSEL	0x00000400	/* PMT Magic Packet */
+#define DMA_HW_FEAT_MMCSEL	0x00000800	/* RMON Module */
+#define DMA_HW_FEAT_TSVER1SEL	0x00001000	/* Only IEEE 1588-2002 */
+#define DMA_HW_FEAT_TSVER2SEL	0x00002000	/* IEEE 1588-2008 PTPv2 */
+#define DMA_HW_FEAT_EEESEL	0x00004000	/* Energy Efficient Ethernet */
+#define DMA_HW_FEAT_AVSEL	0x00008000	/* AV Feature */
+#define DMA_HW_FEAT_TXCOESEL	0x00010000	/* Checksum Offload in Tx */
+#define DMA_HW_FEAT_RXTYP1COE	0x00020000	/* IP COE (Type 1) in Rx */
+#define DMA_HW_FEAT_RXTYP2COE	0x00040000	/* IP COE (Type 2) in Rx */
+#define DMA_HW_FEAT_RXFIFOSIZE	0x00080000	/* Rx FIFO > 2048 Bytes */
+#define DMA_HW_FEAT_RXCHCNT	0x00300000	/* No. additional Rx Channels */
+#define DMA_HW_FEAT_TXCHCNT	0x00c00000	/* No. additional Tx Channels */
+#define DMA_HW_FEAT_ENHDESSEL	0x01000000	/* Alternate Descriptor */
+/* Timestamping with Internal System Time */
+#define DMA_HW_FEAT_INTTSEN	0x02000000
+#define DMA_HW_FEAT_FLEXIPPSEN	0x04000000	/* Flexible PPS Output */
+#define DMA_HW_FEAT_SAVLANINS	0x08000000	/* Source Addr or VLAN */
+#define DMA_HW_FEAT_ACTPHYIF	0x70000000	/* Active/selected PHY iface */
 #define DEFAULT_DMA_PBL		8
 
 /* Max/Min RI Watchdog Timer count value */
@@ -180,7 +216,8 @@ struct stmmac_extra_stats {
 #define STMMAC_TX_MAX_FRAMES	256
 #define STMMAC_TX_FRAMES	64
 
-enum rx_frame_status { /* IPC status */
+/* Rx IPC status */
+enum rx_frame_status {
 	good_frame = 0,
 	discard_frame = 1,
 	csum_none = 2,
@@ -194,17 +231,25 @@ enum dma_irq_status {
 	handle_tx = 0x8,
 };
 
-enum core_specific_irq_mask {
-	core_mmc_tx_irq = 1,
-	core_mmc_rx_irq = 2,
-	core_mmc_rx_csum_offload_irq = 4,
-	core_irq_receive_pmt_irq = 8,
-	core_irq_tx_path_in_lpi_mode = 16,
-	core_irq_tx_path_exit_lpi_mode = 32,
-	core_irq_rx_path_in_lpi_mode = 64,
-	core_irq_rx_path_exit_lpi_mode = 128,
+#define	CORE_IRQ_TX_PATH_IN_LPI_MODE	(1 << 1)
+#define	CORE_IRQ_TX_PATH_EXIT_LPI_MODE	(1 << 2)
+#define	CORE_IRQ_RX_PATH_IN_LPI_MODE	(1 << 3)
+#define	CORE_IRQ_RX_PATH_EXIT_LPI_MODE	(1 << 4)
+
+#define	CORE_PCS_ANE_COMPLETE		(1 << 5)
+#define	CORE_PCS_LINK_STATUS		(1 << 6)
+#define	CORE_RGMII_IRQ			(1 << 7)
+
+struct rgmii_adv {
+	unsigned int pause;
+	unsigned int duplex;
+	unsigned int lp_pause;
+	unsigned int lp_duplex;
 };
 
+#define STMMAC_PCS_PAUSE	1
+#define STMMAC_PCS_ASYM_PAUSE	2
+
 /* DMA HW capabilities */
 struct dma_features {
 	unsigned int mbps_10_100;
@@ -217,9 +262,9 @@ struct dma_features {
 	unsigned int pmt_remote_wake_up;
 	unsigned int pmt_magic_frame;
 	unsigned int rmon;
-	/* IEEE 1588-2002*/
+	/* IEEE 1588-2002 */
 	unsigned int time_stamp;
-	/* IEEE 1588-2008*/
+	/* IEEE 1588-2008 */
 	unsigned int atime_stamp;
 	/* 802.3az - Energy-Efficient Ethernet (EEE) */
 	unsigned int eee;
@@ -232,7 +277,7 @@ struct dma_features {
 	/* TX and RX number of channels */
 	unsigned int number_rx_channel;
 	unsigned int number_tx_channel;
-	/* Alternate (enhanced) DESC mode*/
+	/* Alternate (enhanced) DESC mode */
 	unsigned int enh_desc;
 };
 
@@ -255,23 +300,26 @@ struct dma_features {
 #define STMMAC_DEFAULT_LIT_LS_TIMER	0x3E8
 #define STMMAC_DEFAULT_TWT_LS_TIMER	0x0
 
+#define STMMAC_CHAIN_MODE	0x1
+#define STMMAC_RING_MODE	0x2
+
 struct stmmac_desc_ops {
 	/* DMA RX descriptor ring initialization */
-	void (*init_rx_desc) (struct dma_desc *p, unsigned int ring_size,
-			      int disable_rx_ic);
+	void (*init_rx_desc) (struct dma_desc *p, int disable_rx_ic, int mode,
+			      int end);
 	/* DMA TX descriptor ring initialization */
-	void (*init_tx_desc) (struct dma_desc *p, unsigned int ring_size);
+	void (*init_tx_desc) (struct dma_desc *p, int mode, int end);
 
 	/* Invoked by the xmit function to prepare the tx descriptor */
 	void (*prepare_tx_desc) (struct dma_desc *p, int is_fs, int len,
-				 int csum_flag);
+				 int csum_flag, int mode);
 	/* Set/get the owner of the descriptor */
 	void (*set_tx_owner) (struct dma_desc *p);
 	int (*get_tx_owner) (struct dma_desc *p);
 	/* Invoked by the xmit function to close the tx descriptor */
 	void (*close_tx_desc) (struct dma_desc *p);
 	/* Clean the tx descriptor as soon as the tx irq is received */
-	void (*release_tx_desc) (struct dma_desc *p);
+	void (*release_tx_desc) (struct dma_desc *p, int mode);
 	/* Clear interrupt on tx frame completion. When this bit is
 	 * set an interrupt happens as soon as the frame is transmitted */
 	void (*clear_tx_ic) (struct dma_desc *p);
@@ -290,12 +338,22 @@ struct stmmac_desc_ops {
 	/* Return the reception status looking at the RDES1 */
 	int (*rx_status) (void *data, struct stmmac_extra_stats *x,
 			  struct dma_desc *p);
+	void (*rx_extended_status) (void *data, struct stmmac_extra_stats *x,
+				    struct dma_extended_desc *p);
+	/* Set tx timestamp enable bit */
+	void (*enable_tx_timestamp) (struct dma_desc *p);
+	/* get tx timestamp status */
+	int (*get_tx_timestamp_status) (struct dma_desc *p);
+	/* get timestamp value */
+	 u64(*get_timestamp) (void *desc, u32 ats);
+	/* get rx timestamp status */
+	int (*get_rx_timestamp_status) (void *desc, u32 ats);
 };
 
 struct stmmac_dma_ops {
 	/* DMA core initialization */
 	int (*init) (void __iomem *ioaddr, int pbl, int fb, int mb,
-		     int burst_len, u32 dma_tx, u32 dma_rx);
+		     int burst_len, u32 dma_tx, u32 dma_rx, int atds);
 	/* Dump DMA registers */
 	void (*dump_regs) (void __iomem *ioaddr);
 	/* Set tx/rx threshold in the csr6 register
@@ -321,13 +379,14 @@ struct stmmac_dma_ops {
 
 struct stmmac_ops {
 	/* MAC core initialization */
-	void (*core_init) (void __iomem *ioaddr) ____cacheline_aligned;
+	void (*core_init) (void __iomem *ioaddr);
 	/* Enable and verify that the IPC module is supported */
 	int (*rx_ipc) (void __iomem *ioaddr);
 	/* Dump MAC registers */
 	void (*dump_regs) (void __iomem *ioaddr);
 	/* Handle extra events on specific interrupts hw dependent */
-	int (*host_irq_status) (void __iomem *ioaddr);
+	int (*host_irq_status) (void __iomem *ioaddr,
+				struct stmmac_extra_stats *x);
 	/* Multicast filter setting */
 	void (*set_filter) (struct net_device *dev, int id);
 	/* Flow control setting */
@@ -344,6 +403,18 @@ struct stmmac_ops {
 	void (*reset_eee_mode) (void __iomem *ioaddr);
 	void (*set_eee_timer) (void __iomem *ioaddr, int ls, int tw);
 	void (*set_eee_pls) (void __iomem *ioaddr, int link);
+	void (*ctrl_ane) (void __iomem *ioaddr, bool restart);
+	void (*get_adv) (void __iomem *ioaddr, struct rgmii_adv *adv);
+};
+
+struct stmmac_hwtimestamp {
+	void (*config_hw_tstamping) (void __iomem *ioaddr, u32 data);
+	void (*config_sub_second_increment) (void __iomem *ioaddr);
+	int (*init_systime) (void __iomem *ioaddr, u32 sec, u32 nsec);
+	int (*config_addend) (void __iomem *ioaddr, u32 addend);
+	int (*adjust_systime) (void __iomem *ioaddr, u32 sec, u32 nsec,
+			       int add_sub);
+	 u64(*get_systime) (void __iomem *ioaddr);
 };
 
 struct mac_link {
@@ -360,19 +431,28 @@ struct mii_regs {
 struct stmmac_ring_mode_ops {
 	unsigned int (*is_jumbo_frm) (int len, int ehn_desc);
 	unsigned int (*jumbo_frm) (void *priv, struct sk_buff *skb, int csum);
-	void (*refill_desc3) (int bfsize, struct dma_desc *p);
-	void (*init_desc3) (int des3_as_data_buf, struct dma_desc *p);
-	void (*init_dma_chain) (struct dma_desc *des, dma_addr_t phy_addr,
-				unsigned int size);
-	void (*clean_desc3) (struct dma_desc *p);
+	void (*refill_desc3) (void *priv, struct dma_desc *p);
+	void (*init_desc3) (struct dma_desc *p);
+	void (*clean_desc3) (void *priv, struct dma_desc *p);
 	int (*set_16kib_bfsize) (int mtu);
 };
 
+struct stmmac_chain_mode_ops {
+	void (*init) (void *des, dma_addr_t phy_addr, unsigned int size,
+		      unsigned int extend_desc);
+	unsigned int (*is_jumbo_frm) (int len, int ehn_desc);
+	unsigned int (*jumbo_frm) (void *priv, struct sk_buff *skb, int csum);
+	void (*refill_desc3) (void *priv, struct dma_desc *p);
+	void (*clean_desc3) (void *priv, struct dma_desc *p);
+};
+
 struct mac_device_info {
-	const struct stmmac_ops		*mac;
-	const struct stmmac_desc_ops	*desc;
-	const struct stmmac_dma_ops	*dma;
-	const struct stmmac_ring_mode_ops	*ring;
+	const struct stmmac_ops *mac;
+	const struct stmmac_desc_ops *desc;
+	const struct stmmac_dma_ops *dma;
+	const struct stmmac_ring_mode_ops *ring;
+	const struct stmmac_chain_mode_ops *chain;
+	const struct stmmac_hwtimestamp *ptp;
 	struct mii_regs mii;	/* MII register Addresses */
 	struct mac_link link;
 	unsigned int synopsys_uid;
@@ -390,5 +470,6 @@ extern void stmmac_set_mac(void __iomem *ioaddr, bool enable);
 
 extern void dwmac_dma_flush_tx_fifo(void __iomem *ioaddr);
 extern const struct stmmac_ring_mode_ops ring_mode_ops;
+extern const struct stmmac_chain_mode_ops chain_mode_ops;
 
 #endif /* __COMMON_H__ */
diff --git a/drivers/net/ethernet/stmicro/stmmac/descs.h b/drivers/net/ethernet/stmicro/stmmac/descs.h
index 223adf95fd03..ad3996038018 100644
--- a/drivers/net/ethernet/stmicro/stmmac/descs.h
+++ b/drivers/net/ethernet/stmicro/stmmac/descs.h
@@ -24,6 +24,7 @@
 #ifndef __DESCS_H__
 #define __DESCS_H__
 
+/* Basic descriptor structure for normal and alternate descriptors */
 struct dma_desc {
 	/* Receive descriptor */
 	union {
@@ -60,7 +61,7 @@ struct dma_desc {
 		} rx;
 		struct {
 			/* RDES0 */
-			u32 payload_csum_error:1;
+			u32 rx_mac_addr:1;
 			u32 crc_error:1;
 			u32 dribbling:1;
 			u32 error_gmii:1;
@@ -162,13 +163,57 @@ struct dma_desc {
 	unsigned int des3;
 };
 
+/* Extended descriptor structure (supported by new SYNP GMAC generations) */
+struct dma_extended_desc {
+	struct dma_desc basic;
+	union {
+		struct {
+			u32 ip_payload_type:3;
+			u32 ip_hdr_err:1;
+			u32 ip_payload_err:1;
+			u32 ip_csum_bypassed:1;
+			u32 ipv4_pkt_rcvd:1;
+			u32 ipv6_pkt_rcvd:1;
+			u32 msg_type:4;
+			u32 ptp_frame_type:1;
+			u32 ptp_ver:1;
+			u32 timestamp_dropped:1;
+			u32 reserved:1;
+			u32 av_pkt_rcvd:1;
+			u32 av_tagged_pkt_rcvd:1;
+			u32 vlan_tag_priority_val:3;
+			u32 reserved3:3;
+			u32 l3_filter_match:1;
+			u32 l4_filter_match:1;
+			u32 l3_l4_filter_no_match:2;
+			u32 reserved4:4;
+		} erx;
+		struct {
+			u32 reserved;
+		} etx;
+	} des4;
+	unsigned int des5;	/* Reserved */
+	unsigned int des6;	/* Tx/Rx Timestamp Low */
+	unsigned int des7;	/* Tx/Rx Timestamp High */
+};
+
 /* Transmit checksum insertion control */
 enum tdes_csum_insertion {
 	cic_disabled = 0,	/* Checksum Insertion Control */
 	cic_only_ip = 1,	/* Only IP header */
-	cic_no_pseudoheader = 2,	/* IP header but pseudoheader
-					 * is not calculated */
+	/* IP header but pseudoheader is not calculated */
+	cic_no_pseudoheader = 2,
 	cic_full = 3,		/* IP header and pseudoheader */
 };
 
+/* Extended RDES4 definitions */
+#define RDES_EXT_NO_PTP			0
+#define RDES_EXT_SYNC			0x1
+#define RDES_EXT_FOLLOW_UP		0x2
+#define RDES_EXT_DELAY_REQ		0x3
+#define RDES_EXT_DELAY_RESP		0x4
+#define RDES_EXT_PDELAY_REQ		0x5
+#define RDES_EXT_PDELAY_RESP		0x6
+#define RDES_EXT_PDELAY_FOLLOW_UP	0x7
+
 #endif /* __DESCS_H__ */
diff --git a/drivers/net/ethernet/stmicro/stmmac/descs_com.h b/drivers/net/ethernet/stmicro/stmmac/descs_com.h
index 7ee9499a6e38..6f2cc78c5cf5 100644
--- a/drivers/net/ethernet/stmicro/stmmac/descs_com.h
+++ b/drivers/net/ethernet/stmicro/stmmac/descs_com.h
@@ -30,26 +30,28 @@
 #ifndef __DESC_COM_H__
 #define __DESC_COM_H__
 
-#if defined(CONFIG_STMMAC_RING)
-static inline void ehn_desc_rx_set_on_ring_chain(struct dma_desc *p, int end)
+/* Specific functions used for Ring mode */
+
+/* Enhanced descriptors */
+static inline void ehn_desc_rx_set_on_ring(struct dma_desc *p, int end)
 {
 	p->des01.erx.buffer2_size = BUF_SIZE_8KiB - 1;
 	if (end)
 		p->des01.erx.end_ring = 1;
 }
 
-static inline void ehn_desc_tx_set_on_ring_chain(struct dma_desc *p, int end)
+static inline void ehn_desc_tx_set_on_ring(struct dma_desc *p, int end)
 {
 	if (end)
 		p->des01.etx.end_ring = 1;
 }
 
-static inline void enh_desc_end_tx_desc(struct dma_desc *p, int ter)
+static inline void enh_desc_end_tx_desc_on_ring(struct dma_desc *p, int ter)
 {
 	p->des01.etx.end_ring = ter;
 }
 
-static inline void enh_set_tx_desc_len(struct dma_desc *p, int len)
+static inline void enh_set_tx_desc_len_on_ring(struct dma_desc *p, int len)
 {
 	if (unlikely(len > BUF_SIZE_4KiB)) {
 		p->des01.etx.buffer1_size = BUF_SIZE_4KiB;
@@ -58,25 +60,26 @@ static inline void enh_set_tx_desc_len(struct dma_desc *p, int len)
 		p->des01.etx.buffer1_size = len;
 }
 
-static inline void ndesc_rx_set_on_ring_chain(struct dma_desc *p, int end)
+/* Normal descriptors */
+static inline void ndesc_rx_set_on_ring(struct dma_desc *p, int end)
 {
 	p->des01.rx.buffer2_size = BUF_SIZE_2KiB - 1;
 	if (end)
 		p->des01.rx.end_ring = 1;
 }
 
-static inline void ndesc_tx_set_on_ring_chain(struct dma_desc *p, int end)
+static inline void ndesc_tx_set_on_ring(struct dma_desc *p, int end)
 {
 	if (end)
 		p->des01.tx.end_ring = 1;
 }
 
-static inline void ndesc_end_tx_desc(struct dma_desc *p, int ter)
+static inline void ndesc_end_tx_desc_on_ring(struct dma_desc *p, int ter)
 {
 	p->des01.tx.end_ring = ter;
 }
 
-static inline void norm_set_tx_desc_len(struct dma_desc *p, int len)
+static inline void norm_set_tx_desc_len_on_ring(struct dma_desc *p, int len)
 {
 	if (unlikely(len > BUF_SIZE_2KiB)) {
 		p->des01.etx.buffer1_size = BUF_SIZE_2KiB - 1;
@@ -85,47 +88,47 @@ static inline void norm_set_tx_desc_len(struct dma_desc *p, int len)
 		p->des01.tx.buffer1_size = len;
 }
 
-#else
+/* Specific functions used for Chain mode */
 
-static inline void ehn_desc_rx_set_on_ring_chain(struct dma_desc *p, int end)
+/* Enhanced descriptors */
+static inline void ehn_desc_rx_set_on_chain(struct dma_desc *p, int end)
 {
 	p->des01.erx.second_address_chained = 1;
 }
 
-static inline void ehn_desc_tx_set_on_ring_chain(struct dma_desc *p, int end)
+static inline void ehn_desc_tx_set_on_chain(struct dma_desc *p, int end)
 {
 	p->des01.etx.second_address_chained = 1;
 }
 
-static inline void enh_desc_end_tx_desc(struct dma_desc *p, int ter)
+static inline void enh_desc_end_tx_desc_on_chain(struct dma_desc *p, int ter)
 {
 	p->des01.etx.second_address_chained = 1;
 }
 
-static inline void enh_set_tx_desc_len(struct dma_desc *p, int len)
+static inline void enh_set_tx_desc_len_on_chain(struct dma_desc *p, int len)
 {
 	p->des01.etx.buffer1_size = len;
 }
 
-static inline void ndesc_rx_set_on_ring_chain(struct dma_desc *p, int end)
+/* Normal descriptors */
+static inline void ndesc_rx_set_on_chain(struct dma_desc *p, int end)
 {
 	p->des01.rx.second_address_chained = 1;
 }
 
-static inline void ndesc_tx_set_on_ring_chain(struct dma_desc *p, int ring_size)
+static inline void ndesc_tx_set_on_chain(struct dma_desc *p, int ring_size)
 {
 	p->des01.tx.second_address_chained = 1;
 }
 
-static inline void ndesc_end_tx_desc(struct dma_desc *p, int ter)
+static inline void ndesc_end_tx_desc_on_chain(struct dma_desc *p, int ter)
 {
 	p->des01.tx.second_address_chained = 1;
 }
 
-static inline void norm_set_tx_desc_len(struct dma_desc *p, int len)
+static inline void norm_set_tx_desc_len_on_chain(struct dma_desc *p, int len)
 {
 	p->des01.tx.buffer1_size = len;
 }
-#endif
-
 #endif /* __DESC_COM_H__ */
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
index 7ad56afd6324..c12aabb8cf93 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
@@ -89,13 +89,46 @@ enum power_event {
 				(reg * 8))
 #define GMAC_MAX_PERFECT_ADDRESSES	32
 
+/* PCS registers (AN/TBI/SGMII/RGMII) offset */
 #define GMAC_AN_CTRL	0x000000c0	/* AN control */
 #define GMAC_AN_STATUS	0x000000c4	/* AN status */
 #define GMAC_ANE_ADV	0x000000c8	/* Auto-Neg. Advertisement */
-#define GMAC_ANE_LINK	0x000000cc	/* Auto-Neg. link partener ability */
+#define GMAC_ANE_LPA	0x000000cc	/* Auto-Neg. link partener ability */
 #define GMAC_ANE_EXP	0x000000d0	/* ANE expansion */
 #define GMAC_TBI	0x000000d4	/* TBI extend status */
-#define GMAC_GMII_STATUS 0x000000d8	/* S/R-GMII status */
+#define GMAC_S_R_GMII	0x000000d8	/* SGMII RGMII status */
+
+/* AN Configuration defines */
+#define GMAC_AN_CTRL_RAN	0x00000200	/* Restart Auto-Negotiation */
+#define GMAC_AN_CTRL_ANE	0x00001000	/* Auto-Negotiation Enable */
+#define GMAC_AN_CTRL_ELE	0x00004000	/* External Loopback Enable */
+#define GMAC_AN_CTRL_ECD	0x00010000	/* Enable Comma Detect */
+#define GMAC_AN_CTRL_LR		0x00020000	/* Lock to Reference */
+#define GMAC_AN_CTRL_SGMRAL	0x00040000	/* SGMII RAL Control */
+
+/* AN Status defines */
+#define GMAC_AN_STATUS_LS	0x00000004	/* Link Status 0:down 1:up */
+#define GMAC_AN_STATUS_ANA	0x00000008	/* Auto-Negotiation Ability */
+#define GMAC_AN_STATUS_ANC	0x00000020	/* Auto-Negotiation Complete */
+#define GMAC_AN_STATUS_ES	0x00000100	/* Extended Status */
+
+/* Register 54 (SGMII/RGMII status register) */
+#define GMAC_S_R_GMII_LINK		0x8
+#define GMAC_S_R_GMII_SPEED		0x5
+#define GMAC_S_R_GMII_SPEED_SHIFT	0x1
+#define GMAC_S_R_GMII_MODE		0x1
+#define GMAC_S_R_GMII_SPEED_125		2
+#define GMAC_S_R_GMII_SPEED_25		1
+
+/* Common ADV and LPA defines */
+#define GMAC_ANE_FD		(1 << 5)
+#define GMAC_ANE_HD		(1 << 6)
+#define GMAC_ANE_PSE		(3 << 7)
+#define GMAC_ANE_PSE_SHIFT	7
+
+ /* GMAC Configuration defines */
+#define GMAC_CONTROL_TC	0x01000000	/* Transmit Conf. in RGMII/SGMII */
+#define GMAC_CONTROL_WD	0x00800000	/* Disable Watchdog on receive */
 
 /* GMAC Configuration defines */
 #define GMAC_CONTROL_TC	0x01000000	/* Transmit Conf. in RGMII/SGMII */
@@ -108,19 +141,19 @@ enum inter_frame_gap {
 	GMAC_CONTROL_IFG_80 = 0x00020000,
 	GMAC_CONTROL_IFG_40 = 0x000e0000,
 };
-#define GMAC_CONTROL_DCRS	0x00010000 /* Disable carrier sense during tx */
-#define GMAC_CONTROL_PS		0x00008000 /* Port Select 0:GMI 1:MII */
-#define GMAC_CONTROL_FES	0x00004000 /* Speed 0:10 1:100 */
-#define GMAC_CONTROL_DO		0x00002000 /* Disable Rx Own */
-#define GMAC_CONTROL_LM		0x00001000 /* Loop-back mode */
-#define GMAC_CONTROL_DM		0x00000800 /* Duplex Mode */
-#define GMAC_CONTROL_IPC	0x00000400 /* Checksum Offload */
-#define GMAC_CONTROL_DR		0x00000200 /* Disable Retry */
-#define GMAC_CONTROL_LUD	0x00000100 /* Link up/down */
-#define GMAC_CONTROL_ACS	0x00000080 /* Automatic Pad/FCS Stripping */
-#define GMAC_CONTROL_DC		0x00000010 /* Deferral Check */
-#define GMAC_CONTROL_TE		0x00000008 /* Transmitter Enable */
-#define GMAC_CONTROL_RE		0x00000004 /* Receiver Enable */
+#define GMAC_CONTROL_DCRS	0x00010000	/* Disable carrier sense */
+#define GMAC_CONTROL_PS		0x00008000	/* Port Select 0:GMI 1:MII */
+#define GMAC_CONTROL_FES	0x00004000	/* Speed 0:10 1:100 */
+#define GMAC_CONTROL_DO		0x00002000	/* Disable Rx Own */
+#define GMAC_CONTROL_LM		0x00001000	/* Loop-back mode */
+#define GMAC_CONTROL_DM		0x00000800	/* Duplex Mode */
+#define GMAC_CONTROL_IPC	0x00000400	/* Checksum Offload */
+#define GMAC_CONTROL_DR		0x00000200	/* Disable Retry */
+#define GMAC_CONTROL_LUD	0x00000100	/* Link up/down */
+#define GMAC_CONTROL_ACS	0x00000080	/* Auto Pad/FCS Stripping */
+#define GMAC_CONTROL_DC		0x00000010	/* Deferral Check */
+#define GMAC_CONTROL_TE		0x00000008	/* Transmitter Enable */
+#define GMAC_CONTROL_RE		0x00000004	/* Receiver Enable */
 
 #define GMAC_CORE_INIT (GMAC_CONTROL_JD | GMAC_CONTROL_PS | GMAC_CONTROL_ACS | \
 			GMAC_CONTROL_JE | GMAC_CONTROL_BE)
@@ -151,15 +184,16 @@ enum inter_frame_gap {
 #define DMA_BUS_MODE_SFT_RESET	0x00000001	/* Software Reset */
 #define DMA_BUS_MODE_DA		0x00000002	/* Arbitration scheme */
 #define DMA_BUS_MODE_DSL_MASK	0x0000007c	/* Descriptor Skip Length */
-#define DMA_BUS_MODE_DSL_SHIFT	2	/*   (in DWORDS)      */
+#define DMA_BUS_MODE_DSL_SHIFT	2		/*   (in DWORDS)      */
 /* Programmable burst length (passed thorugh platform)*/
 #define DMA_BUS_MODE_PBL_MASK	0x00003f00	/* Programmable Burst Len */
 #define DMA_BUS_MODE_PBL_SHIFT	8
+#define DMA_BUS_MODE_ATDS	0x00000080	/* Alternate Descriptor Size */
 
 enum rx_tx_priority_ratio {
-	double_ratio = 0x00004000,	/*2:1 */
-	triple_ratio = 0x00008000,	/*3:1 */
-	quadruple_ratio = 0x0000c000,	/*4:1 */
+	double_ratio = 0x00004000,	/* 2:1 */
+	triple_ratio = 0x00008000,	/* 3:1 */
+	quadruple_ratio = 0x0000c000,	/* 4:1 */
 };
 
 #define DMA_BUS_MODE_FB		0x00010000	/* Fixed burst */
@@ -179,9 +213,10 @@ enum rx_tx_priority_ratio {
 #define DMA_BUS_FB	  	  0x00010000	/* Fixed Burst */
 
 /* DMA operation mode defines (start/stop tx/rx are placed in common header)*/
-#define DMA_CONTROL_DT		0x04000000 /* Disable Drop TCP/IP csum error */
-#define DMA_CONTROL_RSF		0x02000000 /* Receive Store and Forward */
-#define DMA_CONTROL_DFF		0x01000000 /* Disaable flushing */
+/* Disable Drop TCP/IP csum error */
+#define DMA_CONTROL_DT		0x04000000
+#define DMA_CONTROL_RSF		0x02000000	/* Receive Store and Forward */
+#define DMA_CONTROL_DFF		0x01000000	/* Disaable flushing */
 /* Threshold for Activating the FC */
 enum rfa {
 	act_full_minus_1 = 0x00800000,
@@ -196,7 +231,7 @@ enum rfd {
 	deac_full_minus_3 = 0x00401000,
 	deac_full_minus_4 = 0x00401800,
 };
-#define DMA_CONTROL_TSF		0x00200000 /* Transmit  Store and Forward */
+#define DMA_CONTROL_TSF	0x00200000	/* Transmit  Store and Forward */
 
 enum ttc_control {
 	DMA_CONTROL_TTC_64 = 0x00000000,
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
index bfe022605498..7e05e8d0f1c2 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
@@ -28,6 +28,7 @@
 
 #include <linux/crc32.h>
 #include <linux/slab.h>
+#include <linux/ethtool.h>
 #include <asm/io.h>
 #include "dwmac1000.h"
 
@@ -71,22 +72,22 @@ static void dwmac1000_dump_regs(void __iomem *ioaddr)
 }
 
 static void dwmac1000_set_umac_addr(void __iomem *ioaddr, unsigned char *addr,
-				unsigned int reg_n)
+				    unsigned int reg_n)
 {
 	stmmac_set_mac_addr(ioaddr, addr, GMAC_ADDR_HIGH(reg_n),
-				GMAC_ADDR_LOW(reg_n));
+			    GMAC_ADDR_LOW(reg_n));
 }
 
 static void dwmac1000_get_umac_addr(void __iomem *ioaddr, unsigned char *addr,
-				unsigned int reg_n)
+				    unsigned int reg_n)
 {
 	stmmac_get_mac_addr(ioaddr, addr, GMAC_ADDR_HIGH(reg_n),
-				GMAC_ADDR_LOW(reg_n));
+			    GMAC_ADDR_LOW(reg_n));
 }
 
 static void dwmac1000_set_filter(struct net_device *dev, int id)
 {
-	void __iomem *ioaddr = (void __iomem *) dev->base_addr;
+	void __iomem *ioaddr = (void __iomem *)dev->base_addr;
 	unsigned int value = 0;
 	unsigned int perfect_addr_number;
 
@@ -96,7 +97,7 @@ static void dwmac1000_set_filter(struct net_device *dev, int id)
 	if (dev->flags & IFF_PROMISC)
 		value = GMAC_FRAME_FILTER_PR;
 	else if ((netdev_mc_count(dev) > HASH_TABLE_SIZE)
-		   || (dev->flags & IFF_ALLMULTI)) {
+		 || (dev->flags & IFF_ALLMULTI)) {
 		value = GMAC_FRAME_FILTER_PM;	/* pass all multi */
 		writel(0xffffffff, ioaddr + GMAC_HASH_HIGH);
 		writel(0xffffffff, ioaddr + GMAC_HASH_LOW);
@@ -110,12 +111,13 @@ static void dwmac1000_set_filter(struct net_device *dev, int id)
 		memset(mc_filter, 0, sizeof(mc_filter));
 		netdev_for_each_mc_addr(ha, dev) {
 			/* The upper 6 bits of the calculated CRC are used to
-			   index the contens of the hash table */
-			int bit_nr =
-			    bitrev32(~crc32_le(~0, ha->addr, 6)) >> 26;
+			 * index the contens of the hash table
+			 */
+			int bit_nr = bitrev32(~crc32_le(~0, ha->addr, 6)) >> 26;
 			/* The most significant bit determines the register to
 			 * use (H/L) while the other 5 bits determine the bit
-			 * within the register. */
+			 * within the register.
+			 */
 			mc_filter[bit_nr >> 5] |= 1 << (bit_nr & 31);
 		}
 		writel(mc_filter[0], ioaddr + GMAC_HASH_LOW);
@@ -128,10 +130,11 @@ static void dwmac1000_set_filter(struct net_device *dev, int id)
 	else
 		perfect_addr_number = GMAC_MAX_PERFECT_ADDRESSES / 2;
 
-	/* Handle multiple unicast addresses (perfect filtering)*/
+	/* Handle multiple unicast addresses (perfect filtering) */
 	if (netdev_uc_count(dev) > perfect_addr_number)
-		/* Switch to promiscuous mode is more than 16 addrs
-		   are required */
+		/* Switch to promiscuous mode if more than 16 addrs
+		 * are required
+		 */
 		value |= GMAC_FRAME_FILTER_PR;
 	else {
 		int reg = 1;
@@ -149,13 +152,13 @@ static void dwmac1000_set_filter(struct net_device *dev, int id)
 #endif
 	writel(value, ioaddr + GMAC_FRAME_FILTER);
 
-	CHIP_DBG(KERN_INFO "\tFrame Filter reg: 0x%08x\n\tHash regs: "
-	    "HI 0x%08x, LO 0x%08x\n", readl(ioaddr + GMAC_FRAME_FILTER),
-	    readl(ioaddr + GMAC_HASH_HIGH), readl(ioaddr + GMAC_HASH_LOW));
+	CHIP_DBG(KERN_INFO "\tFilter: 0x%08x\n\tHash: HI 0x%08x, LO 0x%08x\n",
+		 readl(ioaddr + GMAC_FRAME_FILTER),
+		 readl(ioaddr + GMAC_HASH_HIGH), readl(ioaddr + GMAC_HASH_LOW));
 }
 
 static void dwmac1000_flow_ctrl(void __iomem *ioaddr, unsigned int duplex,
-			   unsigned int fc, unsigned int pause_time)
+				unsigned int fc, unsigned int pause_time)
 {
 	unsigned int flow = 0;
 
@@ -193,74 +196,106 @@ static void dwmac1000_pmt(void __iomem *ioaddr, unsigned long mode)
 	writel(pmt, ioaddr + GMAC_PMT);
 }
 
-
-static int dwmac1000_irq_status(void __iomem *ioaddr)
+static int dwmac1000_irq_status(void __iomem *ioaddr,
+				struct stmmac_extra_stats *x)
 {
 	u32 intr_status = readl(ioaddr + GMAC_INT_STATUS);
-	int status = 0;
+	int ret = 0;
 
 	/* Not used events (e.g. MMC interrupts) are not handled. */
 	if ((intr_status & mmc_tx_irq)) {
 		CHIP_DBG(KERN_INFO "GMAC: MMC tx interrupt: 0x%08x\n",
-		    readl(ioaddr + GMAC_MMC_TX_INTR));
-		status |= core_mmc_tx_irq;
+			 readl(ioaddr + GMAC_MMC_TX_INTR));
+		x->mmc_tx_irq_n++;
 	}
 	if (unlikely(intr_status & mmc_rx_irq)) {
 		CHIP_DBG(KERN_INFO "GMAC: MMC rx interrupt: 0x%08x\n",
-		    readl(ioaddr + GMAC_MMC_RX_INTR));
-		status |= core_mmc_rx_irq;
+			 readl(ioaddr + GMAC_MMC_RX_INTR));
+		x->mmc_rx_irq_n++;
 	}
 	if (unlikely(intr_status & mmc_rx_csum_offload_irq)) {
 		CHIP_DBG(KERN_INFO "GMAC: MMC rx csum offload: 0x%08x\n",
-		    readl(ioaddr + GMAC_MMC_RX_CSUM_OFFLOAD));
-		status |= core_mmc_rx_csum_offload_irq;
+			 readl(ioaddr + GMAC_MMC_RX_CSUM_OFFLOAD));
+		x->mmc_rx_csum_offload_irq_n++;
 	}
 	if (unlikely(intr_status & pmt_irq)) {
 		CHIP_DBG(KERN_INFO "GMAC: received Magic frame\n");
-		/* clear the PMT bits 5 and 6 by reading the PMT
-		 * status register. */
+		/* clear the PMT bits 5 and 6 by reading the PMT status reg */
 		readl(ioaddr + GMAC_PMT);
-		status |= core_irq_receive_pmt_irq;
+		x->irq_receive_pmt_irq_n++;
 	}
 	/* MAC trx/rx EEE LPI entry/exit interrupts */
 	if (intr_status & lpiis_irq) {
 		/* Clean LPI interrupt by reading the Reg 12 */
-		u32 lpi_status = readl(ioaddr + LPI_CTRL_STATUS);
+		ret = readl(ioaddr + LPI_CTRL_STATUS);
 
-		if (lpi_status & LPI_CTRL_STATUS_TLPIEN) {
+		if (ret & LPI_CTRL_STATUS_TLPIEN) {
 			CHIP_DBG(KERN_INFO "GMAC TX entered in LPI\n");
-			status |= core_irq_tx_path_in_lpi_mode;
+			x->irq_tx_path_in_lpi_mode_n++;
 		}
-		if (lpi_status & LPI_CTRL_STATUS_TLPIEX) {
+		if (ret & LPI_CTRL_STATUS_TLPIEX) {
 			CHIP_DBG(KERN_INFO "GMAC TX exit from LPI\n");
-			status |= core_irq_tx_path_exit_lpi_mode;
+			x->irq_tx_path_exit_lpi_mode_n++;
 		}
-		if (lpi_status & LPI_CTRL_STATUS_RLPIEN) {
+		if (ret & LPI_CTRL_STATUS_RLPIEN) {
 			CHIP_DBG(KERN_INFO "GMAC RX entered in LPI\n");
-			status |= core_irq_rx_path_in_lpi_mode;
+			x->irq_rx_path_in_lpi_mode_n++;
 		}
-		if (lpi_status & LPI_CTRL_STATUS_RLPIEX) {
+		if (ret & LPI_CTRL_STATUS_RLPIEX) {
 			CHIP_DBG(KERN_INFO "GMAC RX exit from LPI\n");
-			status |= core_irq_rx_path_exit_lpi_mode;
+			x->irq_rx_path_exit_lpi_mode_n++;
+		}
+	}
+
+	if ((intr_status & pcs_ane_irq) || (intr_status & pcs_link_irq)) {
+		CHIP_DBG(KERN_INFO "GMAC PCS ANE IRQ\n");
+		readl(ioaddr + GMAC_AN_STATUS);
+		x->irq_pcs_ane_n++;
+	}
+	if (intr_status & rgmii_irq) {
+		u32 status = readl(ioaddr + GMAC_S_R_GMII);
+		CHIP_DBG(KERN_INFO "GMAC RGMII/SGMII interrupt\n");
+		x->irq_rgmii_n++;
+
+		/* Save and dump the link status. */
+		if (status & GMAC_S_R_GMII_LINK) {
+			int speed_value = (status & GMAC_S_R_GMII_SPEED) >>
+			    GMAC_S_R_GMII_SPEED_SHIFT;
+			x->pcs_duplex = (status & GMAC_S_R_GMII_MODE);
+
+			if (speed_value == GMAC_S_R_GMII_SPEED_125)
+				x->pcs_speed = SPEED_1000;
+			else if (speed_value == GMAC_S_R_GMII_SPEED_25)
+				x->pcs_speed = SPEED_100;
+			else
+				x->pcs_speed = SPEED_10;
+
+			x->pcs_link = 1;
+			pr_debug("Link is Up - %d/%s\n", (int)x->pcs_speed,
+				 x->pcs_duplex ? "Full" : "Half");
+		} else {
+			x->pcs_link = 0;
+			pr_debug("Link is Down\n");
 		}
 	}
 
-	return status;
+	return ret;
 }
 
-static void  dwmac1000_set_eee_mode(void __iomem *ioaddr)
+static void dwmac1000_set_eee_mode(void __iomem *ioaddr)
 {
 	u32 value;
 
 	/* Enable the link status receive on RGMII, SGMII ore SMII
 	 * receive path and instruct the transmit to enter in LPI
-	 * state. */
+	 * state.
+	 */
 	value = readl(ioaddr + LPI_CTRL_STATUS);
 	value |= LPI_CTRL_STATUS_LPIEN | LPI_CTRL_STATUS_LPITXA;
 	writel(value, ioaddr + LPI_CTRL_STATUS);
 }
 
-static void  dwmac1000_reset_eee_mode(void __iomem *ioaddr)
+static void dwmac1000_reset_eee_mode(void __iomem *ioaddr)
 {
 	u32 value;
 
@@ -269,7 +304,7 @@ static void  dwmac1000_reset_eee_mode(void __iomem *ioaddr)
 	writel(value, ioaddr + LPI_CTRL_STATUS);
 }
 
-static void  dwmac1000_set_eee_pls(void __iomem *ioaddr, int link)
+static void dwmac1000_set_eee_pls(void __iomem *ioaddr, int link)
 {
 	u32 value;
 
@@ -283,7 +318,7 @@ static void  dwmac1000_set_eee_pls(void __iomem *ioaddr, int link)
 	writel(value, ioaddr + LPI_CTRL_STATUS);
 }
 
-static void  dwmac1000_set_eee_timer(void __iomem *ioaddr, int ls, int tw)
+static void dwmac1000_set_eee_timer(void __iomem *ioaddr, int ls, int tw)
 {
 	int value = ((tw & 0xffff)) | ((ls & 0x7ff) << 16);
 
@@ -297,6 +332,41 @@ static void  dwmac1000_set_eee_timer(void __iomem *ioaddr, int ls, int tw)
 	writel(value, ioaddr + LPI_TIMER_CTRL);
 }
 
+static void dwmac1000_ctrl_ane(void __iomem *ioaddr, bool restart)
+{
+	u32 value;
+
+	value = readl(ioaddr + GMAC_AN_CTRL);
+	/* auto negotiation enable and External Loopback enable */
+	value = GMAC_AN_CTRL_ANE | GMAC_AN_CTRL_ELE;
+
+	if (restart)
+		value |= GMAC_AN_CTRL_RAN;
+
+	writel(value, ioaddr + GMAC_AN_CTRL);
+}
+
+static void dwmac1000_get_adv(void __iomem *ioaddr, struct rgmii_adv *adv)
+{
+	u32 value = readl(ioaddr + GMAC_ANE_ADV);
+
+	if (value & GMAC_ANE_FD)
+		adv->duplex = DUPLEX_FULL;
+	if (value & GMAC_ANE_HD)
+		adv->duplex |= DUPLEX_HALF;
+
+	adv->pause = (value & GMAC_ANE_PSE) >> GMAC_ANE_PSE_SHIFT;
+
+	value = readl(ioaddr + GMAC_ANE_LPA);
+
+	if (value & GMAC_ANE_FD)
+		adv->lp_duplex = DUPLEX_FULL;
+	if (value & GMAC_ANE_HD)
+		adv->lp_duplex = DUPLEX_HALF;
+
+	adv->lp_pause = (value & GMAC_ANE_PSE) >> GMAC_ANE_PSE_SHIFT;
+}
+
 static const struct stmmac_ops dwmac1000_ops = {
 	.core_init = dwmac1000_core_init,
 	.rx_ipc = dwmac1000_rx_ipc_enable,
@@ -307,10 +377,12 @@ static const struct stmmac_ops dwmac1000_ops = {
 	.pmt = dwmac1000_pmt,
 	.set_umac_addr = dwmac1000_set_umac_addr,
 	.get_umac_addr = dwmac1000_get_umac_addr,
-	.set_eee_mode =  dwmac1000_set_eee_mode,
-	.reset_eee_mode =  dwmac1000_reset_eee_mode,
-	.set_eee_timer =  dwmac1000_set_eee_timer,
-	.set_eee_pls =  dwmac1000_set_eee_pls,
+	.set_eee_mode = dwmac1000_set_eee_mode,
+	.reset_eee_mode = dwmac1000_reset_eee_mode,
+	.set_eee_timer = dwmac1000_set_eee_timer,
+	.set_eee_pls = dwmac1000_set_eee_pls,
+	.ctrl_ane = dwmac1000_ctrl_ane,
+	.get_adv = dwmac1000_get_adv,
 };
 
 struct mac_device_info *dwmac1000_setup(void __iomem *ioaddr)
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c
index bf83c03bfd06..2c431b616058 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c
@@ -30,8 +30,8 @@
 #include "dwmac1000.h"
 #include "dwmac_dma.h"
 
-static int dwmac1000_dma_init(void __iomem *ioaddr, int pbl, int fb,
-			      int mb, int burst_len, u32 dma_tx, u32 dma_rx)
+static int dwmac1000_dma_init(void __iomem *ioaddr, int pbl, int fb, int mb,
+			      int burst_len, u32 dma_tx, u32 dma_rx, int atds)
 {
 	u32 value = readl(ioaddr + DMA_BUS_MODE);
 	int limit;
@@ -60,7 +60,7 @@ static int dwmac1000_dma_init(void __iomem *ioaddr, int pbl, int fb,
 	 * depending on pbl value.
 	 */
 	value = DMA_BUS_MODE_PBL | ((pbl << DMA_BUS_MODE_PBL_SHIFT) |
-		(pbl << DMA_BUS_MODE_RPBL_SHIFT));
+				    (pbl << DMA_BUS_MODE_RPBL_SHIFT));
 
 	/* Set the Fixed burst mode */
 	if (fb)
@@ -73,6 +73,10 @@ static int dwmac1000_dma_init(void __iomem *ioaddr, int pbl, int fb,
 #ifdef CONFIG_STMMAC_DA
 	value |= DMA_BUS_MODE_DA;	/* Rx has priority over tx */
 #endif
+
+	if (atds)
+		value |= DMA_BUS_MODE_ATDS;
+
 	writel(value, ioaddr + DMA_BUS_MODE);
 
 	/* In case of GMAC AXI configuration, program the DMA_AXI_BUS_MODE
@@ -90,14 +94,16 @@ static int dwmac1000_dma_init(void __iomem *ioaddr, int pbl, int fb,
 	 *
 	 *  For Non Fixed Burst Mode: provide the maximum value of the
 	 *  burst length. Any burst equal or below the provided burst
-	 *  length would be allowed to perform. */
+	 *  length would be allowed to perform.
+	 */
 	writel(burst_len, ioaddr + DMA_AXI_BUS_MODE);
 
 	/* Mask interrupts by writing to CSR7 */
 	writel(DMA_INTR_DEFAULT_MASK, ioaddr + DMA_INTR_ENA);
 
-	/* The base address of the RX/TX descriptor lists must be written into
-	 * DMA CSR3 and CSR4, respectively. */
+	/* RX/TX descriptor base address lists must be written into
+	 * DMA CSR3 and CSR4, respectively
+	 */
 	writel(dma_tx, ioaddr + DMA_TX_BASE_ADDR);
 	writel(dma_rx, ioaddr + DMA_RCV_BASE_ADDR);
 
@@ -105,7 +111,7 @@ static int dwmac1000_dma_init(void __iomem *ioaddr, int pbl, int fb,
 }
 
 static void dwmac1000_dma_operation_mode(void __iomem *ioaddr, int txmode,
-				    int rxmode)
+					 int rxmode)
 {
 	u32 csr6 = readl(ioaddr + DMA_CONTROL);
 
@@ -114,11 +120,12 @@ static void dwmac1000_dma_operation_mode(void __iomem *ioaddr, int txmode,
 		/* Transmit COE type 2 cannot be done in cut-through mode. */
 		csr6 |= DMA_CONTROL_TSF;
 		/* Operating on second frame increase the performance
-		 * especially when transmit store-and-forward is used.*/
+		 * especially when transmit store-and-forward is used.
+		 */
 		csr6 |= DMA_CONTROL_OSF;
 	} else {
-		CHIP_DBG(KERN_DEBUG "GMAC: disabling TX store and forward mode"
-			      " (threshold = %d)\n", txmode);
+		CHIP_DBG(KERN_DEBUG "GMAC: disabling TX SF (threshold %d)\n",
+			 txmode);
 		csr6 &= ~DMA_CONTROL_TSF;
 		csr6 &= DMA_CONTROL_TC_TX_MASK;
 		/* Set the transmit threshold */
@@ -138,8 +145,8 @@ static void dwmac1000_dma_operation_mode(void __iomem *ioaddr, int txmode,
 		CHIP_DBG(KERN_DEBUG "GMAC: enable RX store and forward mode\n");
 		csr6 |= DMA_CONTROL_RSF;
 	} else {
-		CHIP_DBG(KERN_DEBUG "GMAC: disabling RX store and forward mode"
-			      " (threshold = %d)\n", rxmode);
+		CHIP_DBG(KERN_DEBUG "GMAC: disable RX SF mode (threshold %d)\n",
+			 rxmode);
 		csr6 &= ~DMA_CONTROL_RSF;
 		csr6 &= DMA_CONTROL_TC_RX_MASK;
 		if (rxmode <= 32)
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c
index f83210e7c221..007bb2be3f10 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c
@@ -47,8 +47,7 @@ static void dwmac100_dump_mac_regs(void __iomem *ioaddr)
 {
 	pr_info("\t----------------------------------------------\n"
 		"\t  DWMAC 100 CSR (base addr = 0x%p)\n"
-		"\t----------------------------------------------\n",
-		ioaddr);
+		"\t----------------------------------------------\n", ioaddr);
 	pr_info("\tcontrol reg (offset 0x%x): 0x%08x\n", MAC_CONTROL,
 		readl(ioaddr + MAC_CONTROL));
 	pr_info("\taddr HI (offset 0x%x): 0x%08x\n ", MAC_ADDR_HIGH,
@@ -72,7 +71,8 @@ static int dwmac100_rx_ipc_enable(void __iomem *ioaddr)
 	return 0;
 }
 
-static int dwmac100_irq_status(void __iomem *ioaddr)
+static int dwmac100_irq_status(void __iomem *ioaddr,
+			       struct stmmac_extra_stats *x)
 {
 	return 0;
 }
@@ -91,7 +91,7 @@ static void dwmac100_get_umac_addr(void __iomem *ioaddr, unsigned char *addr,
 
 static void dwmac100_set_filter(struct net_device *dev, int id)
 {
-	void __iomem *ioaddr = (void __iomem *) dev->base_addr;
+	void __iomem *ioaddr = (void __iomem *)dev->base_addr;
 	u32 value = readl(ioaddr + MAC_CONTROL);
 
 	if (dev->flags & IFF_PROMISC) {
@@ -112,7 +112,8 @@ static void dwmac100_set_filter(struct net_device *dev, int id)
 		struct netdev_hw_addr *ha;
 
 		/* Perfect filter mode for physical address and Hash
-		   filter for multicast */
+		 * filter for multicast
+		 */
 		value |= MAC_CONTROL_HP;
 		value &= ~(MAC_CONTROL_PM | MAC_CONTROL_PR |
 			   MAC_CONTROL_IF | MAC_CONTROL_HO);
@@ -120,12 +121,13 @@ static void dwmac100_set_filter(struct net_device *dev, int id)
 		memset(mc_filter, 0, sizeof(mc_filter));
 		netdev_for_each_mc_addr(ha, dev) {
 			/* The upper 6 bits of the calculated CRC are used to
-			 * index the contens of the hash table */
-			int bit_nr =
-			    ether_crc(ETH_ALEN, ha->addr) >> 26;
+			 * index the contens of the hash table
+			 */
+			int bit_nr = ether_crc(ETH_ALEN, ha->addr) >> 26;
 			/* The most significant bit determines the register to
 			 * use (H/L) while the other 5 bits determine the bit
-			 * within the register. */
+			 * within the register.
+			 */
 			mc_filter[bit_nr >> 5] |= 1 << (bit_nr & 31);
 		}
 		writel(mc_filter[0], ioaddr + MAC_HASH_LOW);
@@ -134,10 +136,9 @@ static void dwmac100_set_filter(struct net_device *dev, int id)
 
 	writel(value, ioaddr + MAC_CONTROL);
 
-	CHIP_DBG(KERN_INFO "%s: CTRL reg: 0x%08x Hash regs: "
-	    "HI 0x%08x, LO 0x%08x\n",
-	    __func__, readl(ioaddr + MAC_CONTROL),
-	    readl(ioaddr + MAC_HASH_HIGH), readl(ioaddr + MAC_HASH_LOW));
+	CHIP_DBG(KERN_INFO "%s: Filter: 0x%08x Hash: HI 0x%08x, LO 0x%08x\n",
+		 __func__, readl(ioaddr + MAC_CONTROL),
+		 readl(ioaddr + MAC_HASH_HIGH), readl(ioaddr + MAC_HASH_LOW));
 }
 
 static void dwmac100_flow_ctrl(void __iomem *ioaddr, unsigned int duplex,
@@ -150,9 +151,7 @@ static void dwmac100_flow_ctrl(void __iomem *ioaddr, unsigned int duplex,
 	writel(flow, ioaddr + MAC_FLOW_CTRL);
 }
 
-/* No PMT module supported for this Ethernet Controller.
- * Tested on ST platforms only.
- */
+/* No PMT module supported on ST boards with this Eth chip. */
 static void dwmac100_pmt(void __iomem *ioaddr, unsigned long mode)
 {
 	return;
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac100_dma.c b/drivers/net/ethernet/stmicro/stmmac/dwmac100_dma.c
index c2b4d55a79b6..67551c154138 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac100_dma.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac100_dma.c
@@ -32,8 +32,8 @@
 #include "dwmac100.h"
 #include "dwmac_dma.h"
 
-static int dwmac100_dma_init(void __iomem *ioaddr, int pbl, int fb,
-			     int mb, int burst_len, u32 dma_tx, u32 dma_rx)
+static int dwmac100_dma_init(void __iomem *ioaddr, int pbl, int fb, int mb,
+			     int burst_len, u32 dma_tx, u32 dma_rx, int atds)
 {
 	u32 value = readl(ioaddr + DMA_BUS_MODE);
 	int limit;
@@ -52,22 +52,25 @@ static int dwmac100_dma_init(void __iomem *ioaddr, int pbl, int fb,
 
 	/* Enable Application Access by writing to DMA CSR0 */
 	writel(DMA_BUS_MODE_DEFAULT | (pbl << DMA_BUS_MODE_PBL_SHIFT),
-			ioaddr + DMA_BUS_MODE);
+	       ioaddr + DMA_BUS_MODE);
 
 	/* Mask interrupts by writing to CSR7 */
 	writel(DMA_INTR_DEFAULT_MASK, ioaddr + DMA_INTR_ENA);
 
-	/* The base address of the RX/TX descriptor lists must be written into
-	 * DMA CSR3 and CSR4, respectively. */
+	/* RX/TX descriptor base addr lists must be written into
+	 * DMA CSR3 and CSR4, respectively
+	 */
 	writel(dma_tx, ioaddr + DMA_TX_BASE_ADDR);
 	writel(dma_rx, ioaddr + DMA_RCV_BASE_ADDR);
 
 	return 0;
 }
 
-/* Store and Forward capability is not used at all..
- * The transmit threshold can be programmed by
- * setting the TTC bits in the DMA control register.*/
+/* Store and Forward capability is not used at all.
+ *
+ * The transmit threshold can be programmed by setting the TTC bits in the DMA
+ * control register.
+ */
 static void dwmac100_dma_operation_mode(void __iomem *ioaddr, int txmode,
 					int rxmode)
 {
@@ -90,16 +93,15 @@ static void dwmac100_dump_dma_regs(void __iomem *ioaddr)
 	CHIP_DBG(KERN_DEBUG "DWMAC 100 DMA CSR\n");
 	for (i = 0; i < 9; i++)
 		pr_debug("\t CSR%d (offset 0x%x): 0x%08x\n", i,
-		       (DMA_BUS_MODE + i * 4),
-		       readl(ioaddr + DMA_BUS_MODE + i * 4));
+			 (DMA_BUS_MODE + i * 4),
+			 readl(ioaddr + DMA_BUS_MODE + i * 4));
 	CHIP_DBG(KERN_DEBUG "\t CSR20 (offset 0x%x): 0x%08x\n",
-	    DMA_CUR_TX_BUF_ADDR, readl(ioaddr + DMA_CUR_TX_BUF_ADDR));
+		 DMA_CUR_TX_BUF_ADDR, readl(ioaddr + DMA_CUR_TX_BUF_ADDR));
 	CHIP_DBG(KERN_DEBUG "\t CSR21 (offset 0x%x): 0x%08x\n",
-	    DMA_CUR_RX_BUF_ADDR, readl(ioaddr + DMA_CUR_RX_BUF_ADDR));
+		 DMA_CUR_RX_BUF_ADDR, readl(ioaddr + DMA_CUR_RX_BUF_ADDR));
 }
 
-/* DMA controller has two counters to track the number of
- * the receive missed frames. */
+/* DMA controller has two counters to track the number of the missed frames. */
 static void dwmac100_dma_diagnostic_fr(void *data, struct stmmac_extra_stats *x,
 				       void __iomem *ioaddr)
 {
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h b/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h
index ab4896ecac1c..8e5662ce488b 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h
@@ -102,7 +102,7 @@
 #define DMA_STATUS_TU	0x00000004	/* Transmit Buffer Unavailable */
 #define DMA_STATUS_TPS	0x00000002	/* Transmit Process Stopped */
 #define DMA_STATUS_TI	0x00000001	/* Transmit Interrupt */
-#define DMA_CONTROL_FTF		0x00100000 /* Flush transmit FIFO */
+#define DMA_CONTROL_FTF		0x00100000	/* Flush transmit FIFO */
 
 extern void dwmac_enable_dma_transmission(void __iomem *ioaddr);
 extern void dwmac_enable_dma_irq(void __iomem *ioaddr);
@@ -112,6 +112,6 @@ extern void dwmac_dma_stop_tx(void __iomem *ioaddr);
 extern void dwmac_dma_start_rx(void __iomem *ioaddr);
 extern void dwmac_dma_stop_rx(void __iomem *ioaddr);
 extern int dwmac_dma_interrupt(void __iomem *ioaddr,
-				struct stmmac_extra_stats *x);
+			       struct stmmac_extra_stats *x);
 
 #endif /* __DWMAC_DMA_H__ */
diff --git a/drivers/net/ethernet/stmicro/stmmac/enh_desc.c b/drivers/net/ethernet/stmicro/stmmac/enh_desc.c
index 2fc8ef95f97a..0fbc8fafa706 100644
--- a/drivers/net/ethernet/stmicro/stmmac/enh_desc.c
+++ b/drivers/net/ethernet/stmicro/stmmac/enh_desc.c
@@ -150,6 +150,57 @@ static int enh_desc_coe_rdes0(int ipc_err, int type, int payload_err)
 	return ret;
 }
 
+static void enh_desc_get_ext_status(void *data, struct stmmac_extra_stats *x,
+				    struct dma_extended_desc *p)
+{
+	if (unlikely(p->basic.des01.erx.rx_mac_addr)) {
+		if (p->des4.erx.ip_hdr_err)
+			x->ip_hdr_err++;
+		if (p->des4.erx.ip_payload_err)
+			x->ip_payload_err++;
+		if (p->des4.erx.ip_csum_bypassed)
+			x->ip_csum_bypassed++;
+		if (p->des4.erx.ipv4_pkt_rcvd)
+			x->ipv4_pkt_rcvd++;
+		if (p->des4.erx.ipv6_pkt_rcvd)
+			x->ipv6_pkt_rcvd++;
+		if (p->des4.erx.msg_type == RDES_EXT_SYNC)
+			x->rx_msg_type_sync++;
+		else if (p->des4.erx.msg_type == RDES_EXT_FOLLOW_UP)
+			x->rx_msg_type_follow_up++;
+		else if (p->des4.erx.msg_type == RDES_EXT_DELAY_REQ)
+			x->rx_msg_type_delay_req++;
+		else if (p->des4.erx.msg_type == RDES_EXT_DELAY_RESP)
+			x->rx_msg_type_delay_resp++;
+		else if (p->des4.erx.msg_type == RDES_EXT_DELAY_REQ)
+			x->rx_msg_type_pdelay_req++;
+		else if (p->des4.erx.msg_type == RDES_EXT_PDELAY_RESP)
+			x->rx_msg_type_pdelay_resp++;
+		else if (p->des4.erx.msg_type == RDES_EXT_PDELAY_FOLLOW_UP)
+			x->rx_msg_type_pdelay_follow_up++;
+		else
+			x->rx_msg_type_ext_no_ptp++;
+		if (p->des4.erx.ptp_frame_type)
+			x->ptp_frame_type++;
+		if (p->des4.erx.ptp_ver)
+			x->ptp_ver++;
+		if (p->des4.erx.timestamp_dropped)
+			x->timestamp_dropped++;
+		if (p->des4.erx.av_pkt_rcvd)
+			x->av_pkt_rcvd++;
+		if (p->des4.erx.av_tagged_pkt_rcvd)
+			x->av_tagged_pkt_rcvd++;
+		if (p->des4.erx.vlan_tag_priority_val)
+			x->vlan_tag_priority_val++;
+		if (p->des4.erx.l3_filter_match)
+			x->l3_filter_match++;
+		if (p->des4.erx.l4_filter_match)
+			x->l4_filter_match++;
+		if (p->des4.erx.l3_l4_filter_no_match)
+			x->l3_l4_filter_no_match++;
+	}
+}
+
 static int enh_desc_get_rx_status(void *data, struct stmmac_extra_stats *x,
 				  struct dma_desc *p)
 {
@@ -198,7 +249,7 @@ static int enh_desc_get_rx_status(void *data, struct stmmac_extra_stats *x,
 	 * At any rate, we need to understand if the CSUM hw computation is ok
 	 * and report this info to the upper layers. */
 	ret = enh_desc_coe_rdes0(p->des01.erx.ipc_csum_error,
-		p->des01.erx.frame_type, p->des01.erx.payload_csum_error);
+		p->des01.erx.frame_type, p->des01.erx.rx_mac_addr);
 
 	if (unlikely(p->des01.erx.dribbling)) {
 		CHIP_DBG(KERN_ERR "GMAC RX: dribbling error\n");
@@ -225,34 +276,32 @@ static int enh_desc_get_rx_status(void *data, struct stmmac_extra_stats *x,
 		x->rx_vlan++;
 	}
 #endif
+
 	return ret;
 }
 
-static void enh_desc_init_rx_desc(struct dma_desc *p, unsigned int ring_size,
-				  int disable_rx_ic)
+static void enh_desc_init_rx_desc(struct dma_desc *p, int disable_rx_ic,
+				  int mode, int end)
 {
-	int i;
-	for (i = 0; i < ring_size; i++) {
-		p->des01.erx.own = 1;
-		p->des01.erx.buffer1_size = BUF_SIZE_8KiB - 1;
+	p->des01.erx.own = 1;
+	p->des01.erx.buffer1_size = BUF_SIZE_8KiB - 1;
 
-		ehn_desc_rx_set_on_ring_chain(p, (i == ring_size - 1));
+	if (mode == STMMAC_CHAIN_MODE)
+		ehn_desc_rx_set_on_chain(p, end);
+	else
+		ehn_desc_rx_set_on_ring(p, end);
 
-		if (disable_rx_ic)
-			p->des01.erx.disable_ic = 1;
-		p++;
-	}
+	if (disable_rx_ic)
+		p->des01.erx.disable_ic = 1;
 }
 
-static void enh_desc_init_tx_desc(struct dma_desc *p, unsigned int ring_size)
+static void enh_desc_init_tx_desc(struct dma_desc *p, int mode, int end)
 {
-	int i;
-
-	for (i = 0; i < ring_size; i++) {
-		p->des01.etx.own = 0;
-		ehn_desc_tx_set_on_ring_chain(p, (i == ring_size - 1));
-		p++;
-	}
+	p->des01.etx.own = 0;
+	if (mode == STMMAC_CHAIN_MODE)
+		ehn_desc_tx_set_on_chain(p, end);
+	else
+		ehn_desc_tx_set_on_ring(p, end);
 }
 
 static int enh_desc_get_tx_owner(struct dma_desc *p)
@@ -280,20 +329,26 @@ static int enh_desc_get_tx_ls(struct dma_desc *p)
 	return p->des01.etx.last_segment;
 }
 
-static void enh_desc_release_tx_desc(struct dma_desc *p)
+static void enh_desc_release_tx_desc(struct dma_desc *p, int mode)
 {
 	int ter = p->des01.etx.end_ring;
 
 	memset(p, 0, offsetof(struct dma_desc, des2));
-	enh_desc_end_tx_desc(p, ter);
+	if (mode == STMMAC_CHAIN_MODE)
+		enh_desc_end_tx_desc_on_chain(p, ter);
+	else
+		enh_desc_end_tx_desc_on_ring(p, ter);
 }
 
 static void enh_desc_prepare_tx_desc(struct dma_desc *p, int is_fs, int len,
-				     int csum_flag)
+				     int csum_flag, int mode)
 {
 	p->des01.etx.first_segment = is_fs;
 
-	enh_set_tx_desc_len(p, len);
+	if (mode == STMMAC_CHAIN_MODE)
+		enh_set_tx_desc_len_on_chain(p, len);
+	else
+		enh_set_tx_desc_len_on_ring(p, len);
 
 	if (likely(csum_flag))
 		p->des01.etx.checksum_insertion = cic_full;
@@ -323,6 +378,49 @@ static int enh_desc_get_rx_frame_len(struct dma_desc *p, int rx_coe_type)
 		return p->des01.erx.frame_length;
 }
 
+static void enh_desc_enable_tx_timestamp(struct dma_desc *p)
+{
+	p->des01.etx.time_stamp_enable = 1;
+}
+
+static int enh_desc_get_tx_timestamp_status(struct dma_desc *p)
+{
+	return p->des01.etx.time_stamp_status;
+}
+
+static u64 enh_desc_get_timestamp(void *desc, u32 ats)
+{
+	u64 ns;
+
+	if (ats) {
+		struct dma_extended_desc *p = (struct dma_extended_desc *)desc;
+		ns = p->des6;
+		/* convert high/sec time stamp value to nanosecond */
+		ns += p->des7 * 1000000000ULL;
+	} else {
+		struct dma_desc *p = (struct dma_desc *)desc;
+		ns = p->des2;
+		ns += p->des3 * 1000000000ULL;
+	}
+
+	return ns;
+}
+
+static int enh_desc_get_rx_timestamp_status(void *desc, u32 ats)
+{
+	if (ats) {
+		struct dma_extended_desc *p = (struct dma_extended_desc *)desc;
+		return p->basic.des01.erx.ipc_csum_error;
+	} else {
+		struct dma_desc *p = (struct dma_desc *)desc;
+		if ((p->des2 == 0xffffffff) && (p->des3 == 0xffffffff))
+			/* timestamp is corrupted, hence don't store it */
+			return 0;
+		else
+			return 1;
+	}
+}
+
 const struct stmmac_desc_ops enh_desc_ops = {
 	.tx_status = enh_desc_get_tx_status,
 	.rx_status = enh_desc_get_rx_status,
@@ -339,4 +437,9 @@ const struct stmmac_desc_ops enh_desc_ops = {
 	.set_tx_owner = enh_desc_set_tx_owner,
 	.set_rx_owner = enh_desc_set_rx_owner,
 	.get_rx_frame_len = enh_desc_get_rx_frame_len,
+	.rx_extended_status = enh_desc_get_ext_status,
+	.enable_tx_timestamp = enh_desc_enable_tx_timestamp,
+	.get_tx_timestamp_status = enh_desc_get_tx_timestamp_status,
+	.get_timestamp = enh_desc_get_timestamp,
+	.get_rx_timestamp_status = enh_desc_get_rx_timestamp_status,
 };
diff --git a/drivers/net/ethernet/stmicro/stmmac/mmc.h b/drivers/net/ethernet/stmicro/stmmac/mmc.h
index 67995ef25251..48ec001566b5 100644
--- a/drivers/net/ethernet/stmicro/stmmac/mmc.h
+++ b/drivers/net/ethernet/stmicro/stmmac/mmc.h
@@ -28,8 +28,7 @@
 /* MMC control register */
 /* When set, all counter are reset */
 #define MMC_CNTRL_COUNTER_RESET		0x1
-/* When set, do not roll over zero
- * after reaching the max value*/
+/* When set, do not roll over zero after reaching the max value*/
 #define MMC_CNTRL_COUNTER_STOP_ROLLOVER	0x2
 #define MMC_CNTRL_RESET_ON_READ		0x4	/* Reset after reading */
 #define MMC_CNTRL_COUNTER_FREEZER	0x8	/* Freeze counter values to the
diff --git a/drivers/net/ethernet/stmicro/stmmac/norm_desc.c b/drivers/net/ethernet/stmicro/stmmac/norm_desc.c
index 68962c549a2d..11775b99afc5 100644
--- a/drivers/net/ethernet/stmicro/stmmac/norm_desc.c
+++ b/drivers/net/ethernet/stmicro/stmmac/norm_desc.c
@@ -79,8 +79,8 @@ static int ndesc_get_rx_status(void *data, struct stmmac_extra_stats *x,
 	struct net_device_stats *stats = (struct net_device_stats *)data;
 
 	if (unlikely(p->des01.rx.last_descriptor == 0)) {
-		pr_warning("ndesc Error: Oversized Ethernet "
-			   "frame spanned multiple buffers\n");
+		pr_warn("%s: Oversized frame spanned multiple buffers\n",
+			__func__);
 		stats->rx_length_errors++;
 		return discard_frame;
 	}
@@ -122,30 +122,28 @@ static int ndesc_get_rx_status(void *data, struct stmmac_extra_stats *x,
 	return ret;
 }
 
-static void ndesc_init_rx_desc(struct dma_desc *p, unsigned int ring_size,
-			       int disable_rx_ic)
+static void ndesc_init_rx_desc(struct dma_desc *p, int disable_rx_ic, int mode,
+			       int end)
 {
-	int i;
-	for (i = 0; i < ring_size; i++) {
-		p->des01.rx.own = 1;
-		p->des01.rx.buffer1_size = BUF_SIZE_2KiB - 1;
+	p->des01.rx.own = 1;
+	p->des01.rx.buffer1_size = BUF_SIZE_2KiB - 1;
 
-		ndesc_rx_set_on_ring_chain(p, (i == ring_size - 1));
+	if (mode == STMMAC_CHAIN_MODE)
+		ndesc_rx_set_on_chain(p, end);
+	else
+		ndesc_rx_set_on_ring(p, end);
 
-		if (disable_rx_ic)
-			p->des01.rx.disable_ic = 1;
-		p++;
-	}
+	if (disable_rx_ic)
+		p->des01.rx.disable_ic = 1;
 }
 
-static void ndesc_init_tx_desc(struct dma_desc *p, unsigned int ring_size)
+static void ndesc_init_tx_desc(struct dma_desc *p, int mode, int end)
 {
-	int i;
-	for (i = 0; i < ring_size; i++) {
-		p->des01.tx.own = 0;
-		ndesc_tx_set_on_ring_chain(p, (i == (ring_size - 1)));
-		p++;
-	}
+	p->des01.tx.own = 0;
+	if (mode == STMMAC_CHAIN_MODE)
+		ndesc_tx_set_on_chain(p, end);
+	else
+		ndesc_tx_set_on_ring(p, end);
 }
 
 static int ndesc_get_tx_owner(struct dma_desc *p)
@@ -173,19 +171,25 @@ static int ndesc_get_tx_ls(struct dma_desc *p)
 	return p->des01.tx.last_segment;
 }
 
-static void ndesc_release_tx_desc(struct dma_desc *p)
+static void ndesc_release_tx_desc(struct dma_desc *p, int mode)
 {
 	int ter = p->des01.tx.end_ring;
 
 	memset(p, 0, offsetof(struct dma_desc, des2));
-	ndesc_end_tx_desc(p, ter);
+	if (mode == STMMAC_CHAIN_MODE)
+		ndesc_end_tx_desc_on_chain(p, ter);
+	else
+		ndesc_end_tx_desc_on_ring(p, ter);
 }
 
 static void ndesc_prepare_tx_desc(struct dma_desc *p, int is_fs, int len,
-				  int csum_flag)
+				  int csum_flag, int mode)
 {
 	p->des01.tx.first_segment = is_fs;
-	norm_set_tx_desc_len(p, len);
+	if (mode == STMMAC_CHAIN_MODE)
+		norm_set_tx_desc_len_on_chain(p, len);
+	else
+		norm_set_tx_desc_len_on_ring(p, len);
 
 	if (likely(csum_flag))
 		p->des01.tx.checksum_insertion = cic_full;
@@ -215,6 +219,39 @@ static int ndesc_get_rx_frame_len(struct dma_desc *p, int rx_coe_type)
 		return p->des01.rx.frame_length;
 }
 
+static void ndesc_enable_tx_timestamp(struct dma_desc *p)
+{
+	p->des01.tx.time_stamp_enable = 1;
+}
+
+static int ndesc_get_tx_timestamp_status(struct dma_desc *p)
+{
+	return p->des01.tx.time_stamp_status;
+}
+
+static u64 ndesc_get_timestamp(void *desc, u32 ats)
+{
+	struct dma_desc *p = (struct dma_desc *)desc;
+	u64 ns;
+
+	ns = p->des2;
+	/* convert high/sec time stamp value to nanosecond */
+	ns += p->des3 * 1000000000ULL;
+
+	return ns;
+}
+
+static int ndesc_get_rx_timestamp_status(void *desc, u32 ats)
+{
+	struct dma_desc *p = (struct dma_desc *)desc;
+
+	if ((p->des2 == 0xffffffff) && (p->des3 == 0xffffffff))
+		/* timestamp is corrupted, hence don't store it */
+		return 0;
+	else
+		return 1;
+}
+
 const struct stmmac_desc_ops ndesc_ops = {
 	.tx_status = ndesc_get_tx_status,
 	.rx_status = ndesc_get_rx_status,
@@ -231,4 +268,8 @@ const struct stmmac_desc_ops ndesc_ops = {
 	.set_tx_owner = ndesc_set_tx_owner,
 	.set_rx_owner = ndesc_set_rx_owner,
 	.get_rx_frame_len = ndesc_get_rx_frame_len,
+	.enable_tx_timestamp = ndesc_enable_tx_timestamp,
+	.get_tx_timestamp_status = ndesc_get_tx_timestamp_status,
+	.get_timestamp = ndesc_get_timestamp,
+	.get_rx_timestamp_status = ndesc_get_rx_timestamp_status,
 };
diff --git a/drivers/net/ethernet/stmicro/stmmac/ring_mode.c b/drivers/net/ethernet/stmicro/stmmac/ring_mode.c
index 4b785e10f2ed..c9d942a5c335 100644
--- a/drivers/net/ethernet/stmicro/stmmac/ring_mode.c
+++ b/drivers/net/ethernet/stmicro/stmmac/ring_mode.c
@@ -30,7 +30,7 @@
 
 static unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 {
-	struct stmmac_priv *priv = (struct stmmac_priv *) p;
+	struct stmmac_priv *priv = (struct stmmac_priv *)p;
 	unsigned int txsize = priv->dma_tx_size;
 	unsigned int entry = priv->cur_tx % txsize;
 	struct dma_desc *desc = priv->dma_tx + entry;
@@ -48,25 +48,30 @@ static unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 
 		desc->des2 = dma_map_single(priv->device, skb->data,
 					    bmax, DMA_TO_DEVICE);
+		priv->tx_skbuff_dma[entry] = desc->des2;
 		desc->des3 = desc->des2 + BUF_SIZE_4KiB;
-		priv->hw->desc->prepare_tx_desc(desc, 1, bmax,
-						csum);
+		priv->hw->desc->prepare_tx_desc(desc, 1, bmax, csum,
+						STMMAC_RING_MODE);
 		wmb();
 		entry = (++priv->cur_tx) % txsize;
 		desc = priv->dma_tx + entry;
 
 		desc->des2 = dma_map_single(priv->device, skb->data + bmax,
 					    len, DMA_TO_DEVICE);
+		priv->tx_skbuff_dma[entry] = desc->des2;
 		desc->des3 = desc->des2 + BUF_SIZE_4KiB;
-		priv->hw->desc->prepare_tx_desc(desc, 0, len, csum);
+		priv->hw->desc->prepare_tx_desc(desc, 0, len, csum,
+						STMMAC_RING_MODE);
 		wmb();
 		priv->hw->desc->set_tx_owner(desc);
 		priv->tx_skbuff[entry] = NULL;
 	} else {
 		desc->des2 = dma_map_single(priv->device, skb->data,
 					    nopaged_len, DMA_TO_DEVICE);
+		priv->tx_skbuff_dma[entry] = desc->des2;
 		desc->des3 = desc->des2 + BUF_SIZE_4KiB;
-		priv->hw->desc->prepare_tx_desc(desc, 1, nopaged_len, csum);
+		priv->hw->desc->prepare_tx_desc(desc, 1, nopaged_len, csum,
+						STMMAC_RING_MODE);
 	}
 
 	return entry;
@@ -82,27 +87,23 @@ static unsigned int stmmac_is_jumbo_frm(int len, int enh_desc)
 	return ret;
 }
 
-static void stmmac_refill_desc3(int bfsize, struct dma_desc *p)
+static void stmmac_refill_desc3(void *priv_ptr, struct dma_desc *p)
 {
-	/* Fill DES3 in case of RING mode */
-	if (bfsize >= BUF_SIZE_8KiB)
-		p->des3 = p->des2 + BUF_SIZE_8KiB;
-}
+	struct stmmac_priv *priv = (struct stmmac_priv *)priv_ptr;
 
-/* In ring mode we need to fill the desc3 because it is used
- * as buffer */
-static void stmmac_init_desc3(int des3_as_data_buf, struct dma_desc *p)
-{
-	if (unlikely(des3_as_data_buf))
-		p->des3 = p->des2 + BUF_SIZE_8KiB;
+	if (unlikely(priv->plat->has_gmac))
+		/* Fill DES3 in case of RING mode */
+		if (priv->dma_buf_sz >= BUF_SIZE_8KiB)
+			p->des3 = p->des2 + BUF_SIZE_8KiB;
 }
 
-static void stmmac_init_dma_chain(struct dma_desc *des, dma_addr_t phy_addr,
-				  unsigned int size)
+/* In ring mode we need to fill the desc3 because it is used as buffer */
+static void stmmac_init_desc3(struct dma_desc *p)
 {
+	p->des3 = p->des2 + BUF_SIZE_8KiB;
 }
 
-static void stmmac_clean_desc3(struct dma_desc *p)
+static void stmmac_clean_desc3(void *priv_ptr, struct dma_desc *p)
 {
 	if (unlikely(p->des3))
 		p->des3 = 0;
@@ -121,7 +122,6 @@ const struct stmmac_ring_mode_ops ring_mode_ops = {
 	.jumbo_frm = stmmac_jumbo_frm,
 	.refill_desc3 = stmmac_refill_desc3,
 	.init_desc3 = stmmac_init_desc3,
-	.init_dma_chain = stmmac_init_dma_chain,
 	.clean_desc3 = stmmac_clean_desc3,
 	.set_16kib_bfsize = stmmac_set_16kib_bfsize,
 };
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index b05df8983be5..c922fde929a1 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -24,43 +24,56 @@
 #define __STMMAC_H__
 
 #define STMMAC_RESOURCE_NAME   "stmmaceth"
-#define DRV_MODULE_VERSION	"Nov_2012"
+#define DRV_MODULE_VERSION	"March_2013"
 
 #include <linux/clk.h>
 #include <linux/stmmac.h>
 #include <linux/phy.h>
 #include <linux/pci.h>
 #include "common.h"
+#include <linux/ptp_clock_kernel.h>
 
 struct stmmac_priv {
 	/* Frequently used values are kept adjacent for cache effect */
-	struct dma_desc *dma_tx ____cacheline_aligned;
-	dma_addr_t dma_tx_phy;
+	struct dma_extended_desc *dma_etx ____cacheline_aligned_in_smp;
+	struct dma_desc *dma_tx;
 	struct sk_buff **tx_skbuff;
 	unsigned int cur_tx;
 	unsigned int dirty_tx;
 	unsigned int dma_tx_size;
+	u32 tx_count_frames;
+	u32 tx_coal_frames;
+	u32 tx_coal_timer;
+	dma_addr_t *tx_skbuff_dma;
+	dma_addr_t dma_tx_phy;
 	int tx_coalesce;
+	int hwts_tx_en;
+	spinlock_t tx_lock;
+	bool tx_path_in_lpi_mode;
+	struct timer_list txtimer;
 
-	struct dma_desc *dma_rx ;
+	struct dma_desc *dma_rx	____cacheline_aligned_in_smp;
+	struct dma_extended_desc *dma_erx;
+	struct sk_buff **rx_skbuff;
 	unsigned int cur_rx;
 	unsigned int dirty_rx;
-	struct sk_buff **rx_skbuff;
+	unsigned int dma_rx_size;
+	unsigned int dma_buf_sz;
+	u32 rx_riwt;
+	int hwts_rx_en;
 	dma_addr_t *rx_skbuff_dma;
+	dma_addr_t dma_rx_phy;
 
+	struct napi_struct napi ____cacheline_aligned_in_smp;
+
+	void __iomem *ioaddr;
 	struct net_device *dev;
-	dma_addr_t dma_rx_phy;
-	unsigned int dma_rx_size;
-	unsigned int dma_buf_sz;
 	struct device *device;
 	struct mac_device_info *hw;
-	void __iomem *ioaddr;
-
-	struct stmmac_extra_stats xstats;
-	struct napi_struct napi;
 	int no_csum_insertion;
+	spinlock_t lock;
 
-	struct phy_device *phydev;
+	struct phy_device *phydev ____cacheline_aligned_in_smp;
 	int oldlink;
 	int speed;
 	int oldduplex;
@@ -69,30 +82,31 @@ struct stmmac_priv {
 	struct mii_bus *mii;
 	int mii_irq[PHY_MAX_ADDR];
 
-	u32 msg_enable;
-	spinlock_t lock;
-	spinlock_t tx_lock;
-	int wolopts;
-	int wol_irq;
+	struct stmmac_extra_stats xstats ____cacheline_aligned_in_smp;
 	struct plat_stmmacenet_data *plat;
-	struct stmmac_counters mmc;
 	struct dma_features dma_cap;
+	struct stmmac_counters mmc;
 	int hw_cap_support;
+	int synopsys_id;
+	u32 msg_enable;
+	int wolopts;
+	int wol_irq;
 	struct clk *stmmac_clk;
 	int clk_csr;
-	int synopsys_id;
 	struct timer_list eee_ctrl_timer;
-	bool tx_path_in_lpi_mode;
 	int lpi_irq;
 	int eee_enabled;
 	int eee_active;
 	int tx_lpi_timer;
-	struct timer_list txtimer;
-	u32 tx_count_frames;
-	u32 tx_coal_frames;
-	u32 tx_coal_timer;
+	int pcs;
+	unsigned int mode;
+	int extend_desc;
+	struct ptp_clock *ptp_clock;
+	struct ptp_clock_info ptp_clock_ops;
+	unsigned int default_addend;
+	u32 adv_ts;
 	int use_riwt;
-	u32 rx_riwt;
+	spinlock_t ptp_lock;
 };
 
 extern int phyaddr;
@@ -102,6 +116,9 @@ extern int stmmac_mdio_register(struct net_device *ndev);
 extern void stmmac_set_ethtool_ops(struct net_device *netdev);
 extern const struct stmmac_desc_ops enh_desc_ops;
 extern const struct stmmac_desc_ops ndesc_ops;
+extern const struct stmmac_hwtimestamp stmmac_ptp;
+extern int stmmac_ptp_register(struct stmmac_priv *priv);
+extern void stmmac_ptp_unregister(struct stmmac_priv *priv);
 int stmmac_freeze(struct net_device *ndev);
 int stmmac_restore(struct net_device *ndev);
 int stmmac_resume(struct net_device *ndev);
@@ -125,6 +142,7 @@ static inline int stmmac_register_platform(void)
 
 	return err;
 }
+
 static inline void stmmac_unregister_platform(void)
 {
 	platform_driver_unregister(&stmmac_pltfr_driver);
@@ -136,6 +154,7 @@ static inline int stmmac_register_platform(void)
 
 	return 0;
 }
+
 static inline void stmmac_unregister_platform(void)
 {
 }
@@ -153,6 +172,7 @@ static inline int stmmac_register_pci(void)
 
 	return err;
 }
+
 static inline void stmmac_unregister_pci(void)
 {
 	pci_unregister_driver(&stmmac_pci_driver);
@@ -164,6 +184,7 @@ static inline int stmmac_register_pci(void)
 
 	return 0;
 }
+
 static inline void stmmac_unregister_pci(void)
 {
 }
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
index d1ac39c1b05d..c5f9cb85c8ef 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
@@ -27,6 +27,7 @@
 #include <linux/interrupt.h>
 #include <linux/mii.h>
 #include <linux/phy.h>
+#include <linux/net_tstamp.h>
 #include <asm/io.h>
 
 #include "stmmac.h"
@@ -108,6 +109,33 @@ static const struct stmmac_stats stmmac_gstrings_stats[] = {
 	STMMAC_STAT(irq_rx_path_in_lpi_mode_n),
 	STMMAC_STAT(irq_rx_path_exit_lpi_mode_n),
 	STMMAC_STAT(phy_eee_wakeup_error_n),
+	/* Extended RDES status */
+	STMMAC_STAT(ip_hdr_err),
+	STMMAC_STAT(ip_payload_err),
+	STMMAC_STAT(ip_csum_bypassed),
+	STMMAC_STAT(ipv4_pkt_rcvd),
+	STMMAC_STAT(ipv6_pkt_rcvd),
+	STMMAC_STAT(rx_msg_type_ext_no_ptp),
+	STMMAC_STAT(rx_msg_type_sync),
+	STMMAC_STAT(rx_msg_type_follow_up),
+	STMMAC_STAT(rx_msg_type_delay_req),
+	STMMAC_STAT(rx_msg_type_delay_resp),
+	STMMAC_STAT(rx_msg_type_pdelay_req),
+	STMMAC_STAT(rx_msg_type_pdelay_resp),
+	STMMAC_STAT(rx_msg_type_pdelay_follow_up),
+	STMMAC_STAT(ptp_frame_type),
+	STMMAC_STAT(ptp_ver),
+	STMMAC_STAT(timestamp_dropped),
+	STMMAC_STAT(av_pkt_rcvd),
+	STMMAC_STAT(av_tagged_pkt_rcvd),
+	STMMAC_STAT(vlan_tag_priority_val),
+	STMMAC_STAT(l3_filter_match),
+	STMMAC_STAT(l4_filter_match),
+	STMMAC_STAT(l3_l4_filter_no_match),
+	/* PCS */
+	STMMAC_STAT(irq_pcs_ane_n),
+	STMMAC_STAT(irq_pcs_link_n),
+	STMMAC_STAT(irq_rgmii_n),
 };
 #define STMMAC_STATS_LEN ARRAY_SIZE(stmmac_gstrings_stats)
 
@@ -219,6 +247,70 @@ static int stmmac_ethtool_getsettings(struct net_device *dev,
 	struct stmmac_priv *priv = netdev_priv(dev);
 	struct phy_device *phy = priv->phydev;
 	int rc;
+
+	if ((priv->pcs & STMMAC_PCS_RGMII) || (priv->pcs & STMMAC_PCS_SGMII)) {
+		struct rgmii_adv adv;
+
+		if (!priv->xstats.pcs_link) {
+			ethtool_cmd_speed_set(cmd, SPEED_UNKNOWN);
+			cmd->duplex = DUPLEX_UNKNOWN;
+			return 0;
+		}
+		cmd->duplex = priv->xstats.pcs_duplex;
+
+		ethtool_cmd_speed_set(cmd, priv->xstats.pcs_speed);
+
+		/* Get and convert ADV/LP_ADV from the HW AN registers */
+		if (priv->hw->mac->get_adv)
+			priv->hw->mac->get_adv(priv->ioaddr, &adv);
+		else
+			return -EOPNOTSUPP;	/* should never happen indeed */
+
+		/* Encoding of PSE bits is defined in 802.3z, 37.2.1.4 */
+
+		if (adv.pause & STMMAC_PCS_PAUSE)
+			cmd->advertising |= ADVERTISED_Pause;
+		if (adv.pause & STMMAC_PCS_ASYM_PAUSE)
+			cmd->advertising |= ADVERTISED_Asym_Pause;
+		if (adv.lp_pause & STMMAC_PCS_PAUSE)
+			cmd->lp_advertising |= ADVERTISED_Pause;
+		if (adv.lp_pause & STMMAC_PCS_ASYM_PAUSE)
+			cmd->lp_advertising |= ADVERTISED_Asym_Pause;
+
+		/* Reg49[3] always set because ANE is always supported */
+		cmd->autoneg = ADVERTISED_Autoneg;
+		cmd->supported |= SUPPORTED_Autoneg;
+		cmd->advertising |= ADVERTISED_Autoneg;
+		cmd->lp_advertising |= ADVERTISED_Autoneg;
+
+		if (adv.duplex) {
+			cmd->supported |= (SUPPORTED_1000baseT_Full |
+					   SUPPORTED_100baseT_Full |
+					   SUPPORTED_10baseT_Full);
+			cmd->advertising |= (ADVERTISED_1000baseT_Full |
+					     ADVERTISED_100baseT_Full |
+					     ADVERTISED_10baseT_Full);
+		} else {
+			cmd->supported |= (SUPPORTED_1000baseT_Half |
+					   SUPPORTED_100baseT_Half |
+					   SUPPORTED_10baseT_Half);
+			cmd->advertising |= (ADVERTISED_1000baseT_Half |
+					     ADVERTISED_100baseT_Half |
+					     ADVERTISED_10baseT_Half);
+		}
+		if (adv.lp_duplex)
+			cmd->lp_advertising |= (ADVERTISED_1000baseT_Full |
+						ADVERTISED_100baseT_Full |
+						ADVERTISED_10baseT_Full);
+		else
+			cmd->lp_advertising |= (ADVERTISED_1000baseT_Half |
+						ADVERTISED_100baseT_Half |
+						ADVERTISED_10baseT_Half);
+		cmd->port = PORT_OTHER;
+
+		return 0;
+	}
+
 	if (phy == NULL) {
 		pr_err("%s: %s: PHY is not registered\n",
 		       __func__, dev->name);
@@ -243,6 +335,30 @@ static int stmmac_ethtool_setsettings(struct net_device *dev,
 	struct phy_device *phy = priv->phydev;
 	int rc;
 
+	if ((priv->pcs & STMMAC_PCS_RGMII) || (priv->pcs & STMMAC_PCS_SGMII)) {
+		u32 mask = ADVERTISED_Autoneg | ADVERTISED_Pause;
+
+		/* Only support ANE */
+		if (cmd->autoneg != AUTONEG_ENABLE)
+			return -EINVAL;
+
+		if (cmd->autoneg == AUTONEG_ENABLE) {
+			mask &= (ADVERTISED_1000baseT_Half |
+			ADVERTISED_1000baseT_Full |
+			ADVERTISED_100baseT_Half |
+			ADVERTISED_100baseT_Full |
+			ADVERTISED_10baseT_Half |
+			ADVERTISED_10baseT_Full);
+
+			spin_lock(&priv->lock);
+			if (priv->hw->mac->ctrl_ane)
+				priv->hw->mac->ctrl_ane(priv->ioaddr, 1);
+			spin_unlock(&priv->lock);
+		}
+
+		return 0;
+	}
+
 	spin_lock(&priv->lock);
 	rc = phy_ethtool_sset(phy, cmd);
 	spin_unlock(&priv->lock);
@@ -312,6 +428,9 @@ stmmac_get_pauseparam(struct net_device *netdev,
 {
 	struct stmmac_priv *priv = netdev_priv(netdev);
 
+	if (priv->pcs)	/* FIXME */
+		return;
+
 	spin_lock(&priv->lock);
 
 	pause->rx_pause = 0;
@@ -335,6 +454,9 @@ stmmac_set_pauseparam(struct net_device *netdev,
 	int new_pause = FLOW_OFF;
 	int ret = 0;
 
+	if (priv->pcs)	/* FIXME */
+		return -EOPNOTSUPP;
+
 	spin_lock(&priv->lock);
 
 	if (pause->rx_pause)
@@ -604,6 +726,38 @@ static int stmmac_set_coalesce(struct net_device *dev,
 	return 0;
 }
 
+static int stmmac_get_ts_info(struct net_device *dev,
+			      struct ethtool_ts_info *info)
+{
+	struct stmmac_priv *priv = netdev_priv(dev);
+
+	if ((priv->hwts_tx_en) && (priv->hwts_rx_en)) {
+
+		info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
+					SOF_TIMESTAMPING_RX_HARDWARE |
+					SOF_TIMESTAMPING_RAW_HARDWARE;
+
+		if (priv->ptp_clock)
+			info->phc_index = ptp_clock_index(priv->ptp_clock);
+
+		info->tx_types = (1 << HWTSTAMP_TX_OFF) | (1 << HWTSTAMP_TX_ON);
+
+		info->rx_filters = ((1 << HWTSTAMP_FILTER_NONE) |
+				    (1 << HWTSTAMP_FILTER_PTP_V1_L4_EVENT) |
+				    (1 << HWTSTAMP_FILTER_PTP_V1_L4_SYNC) |
+				    (1 << HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ) |
+				    (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) |
+				    (1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC) |
+				    (1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ) |
+				    (1 << HWTSTAMP_FILTER_PTP_V2_EVENT) |
+				    (1 << HWTSTAMP_FILTER_PTP_V2_SYNC) |
+				    (1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ) |
+				    (1 << HWTSTAMP_FILTER_ALL));
+		return 0;
+	} else
+		return ethtool_op_get_ts_info(dev, info);
+}
+
 static const struct ethtool_ops stmmac_ethtool_ops = {
 	.begin = stmmac_check_if_running,
 	.get_drvinfo = stmmac_ethtool_getdrvinfo,
@@ -623,7 +777,7 @@ static const struct ethtool_ops stmmac_ethtool_ops = {
 	.get_eee = stmmac_ethtool_op_get_eee,
 	.set_eee = stmmac_ethtool_op_set_eee,
 	.get_sset_count	= stmmac_get_sset_count,
-	.get_ts_info = ethtool_op_get_ts_info,
+	.get_ts_info = stmmac_get_ts_info,
 	.get_coalesce = stmmac_get_coalesce,
 	.set_coalesce = stmmac_set_coalesce,
 };
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c
new file mode 100644
index 000000000000..def7e75e1d57
--- /dev/null
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c
@@ -0,0 +1,148 @@
+/*******************************************************************************
+  Copyright (C) 2013  Vayavya Labs Pvt Ltd
+
+  This implements all the API for managing HW timestamp & PTP.
+
+  This program is free software; you can redistribute it and/or modify it
+  under the terms and conditions of the GNU General Public License,
+  version 2, as published by the Free Software Foundation.
+
+  This program is distributed in the hope it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Author: Rayagond Kokatanur <rayagond@vayavyalabs.com>
+  Author: Giuseppe Cavallaro <peppe.cavallaro@st.com>
+*******************************************************************************/
+
+#include <linux/io.h>
+#include <linux/delay.h>
+#include "common.h"
+#include "stmmac_ptp.h"
+
+static void stmmac_config_hw_tstamping(void __iomem *ioaddr, u32 data)
+{
+	writel(data, ioaddr + PTP_TCR);
+}
+
+static void stmmac_config_sub_second_increment(void __iomem *ioaddr)
+{
+	u32 value = readl(ioaddr + PTP_TCR);
+	unsigned long data;
+
+	/* Convert the ptp_clock to nano second
+	 * formula = (1/ptp_clock) * 1000000000
+	 * where, ptp_clock = 50MHz.
+	 */
+	data = (1000000000ULL / 50000000);
+
+	/* 0.465ns accuracy */
+	if (value & PTP_TCR_TSCTRLSSR)
+		data = (data * 100) / 465;
+
+	writel(data, ioaddr + PTP_SSIR);
+}
+
+static int stmmac_init_systime(void __iomem *ioaddr, u32 sec, u32 nsec)
+{
+	int limit;
+	u32 value;
+
+	writel(sec, ioaddr + PTP_STSUR);
+	writel(nsec, ioaddr + PTP_STNSUR);
+	/* issue command to initialize the system time value */
+	value = readl(ioaddr + PTP_TCR);
+	value |= PTP_TCR_TSINIT;
+	writel(value, ioaddr + PTP_TCR);
+
+	/* wait for present system time initialize to complete */
+	limit = 10;
+	while (limit--) {
+		if (!(readl(ioaddr + PTP_TCR) & PTP_TCR_TSINIT))
+			break;
+		mdelay(10);
+	}
+	if (limit < 0)
+		return -EBUSY;
+
+	return 0;
+}
+
+static int stmmac_config_addend(void __iomem *ioaddr, u32 addend)
+{
+	u32 value;
+	int limit;
+
+	writel(addend, ioaddr + PTP_TAR);
+	/* issue command to update the addend value */
+	value = readl(ioaddr + PTP_TCR);
+	value |= PTP_TCR_TSADDREG;
+	writel(value, ioaddr + PTP_TCR);
+
+	/* wait for present addend update to complete */
+	limit = 10;
+	while (limit--) {
+		if (!(readl(ioaddr + PTP_TCR) & PTP_TCR_TSADDREG))
+			break;
+		mdelay(10);
+	}
+	if (limit < 0)
+		return -EBUSY;
+
+	return 0;
+}
+
+static int stmmac_adjust_systime(void __iomem *ioaddr, u32 sec, u32 nsec,
+				 int add_sub)
+{
+	u32 value;
+	int limit;
+
+	writel(sec, ioaddr + PTP_STSUR);
+	writel(((add_sub << PTP_STNSUR_ADDSUB_SHIFT) | nsec),
+		ioaddr + PTP_STNSUR);
+	/* issue command to initialize the system time value */
+	value = readl(ioaddr + PTP_TCR);
+	value |= PTP_TCR_TSUPDT;
+	writel(value, ioaddr + PTP_TCR);
+
+	/* wait for present system time adjust/update to complete */
+	limit = 10;
+	while (limit--) {
+		if (!(readl(ioaddr + PTP_TCR) & PTP_TCR_TSUPDT))
+			break;
+		mdelay(10);
+	}
+	if (limit < 0)
+		return -EBUSY;
+
+	return 0;
+}
+
+static u64 stmmac_get_systime(void __iomem *ioaddr)
+{
+	u64 ns;
+
+	ns = readl(ioaddr + PTP_STNSR);
+	/* convert sec time value to nanosecond */
+	ns += readl(ioaddr + PTP_STSR) * 1000000000ULL;
+
+	return ns;
+}
+
+const struct stmmac_hwtimestamp stmmac_ptp = {
+	.config_hw_tstamping = stmmac_config_hw_tstamping,
+	.init_systime = stmmac_init_systime,
+	.config_sub_second_increment = stmmac_config_sub_second_increment,
+	.config_addend = stmmac_config_addend,
+	.adjust_systime = stmmac_adjust_systime,
+	.get_systime = stmmac_get_systime,
+};
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 39c6c5524633..618446ae1ec1 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -46,7 +46,9 @@
 #ifdef CONFIG_STMMAC_DEBUG_FS
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
-#endif
+#endif /* CONFIG_STMMAC_DEBUG_FS */
+#include <linux/net_tstamp.h>
+#include "stmmac_ptp.h"
 #include "stmmac.h"
 
 #undef STMMAC_DEBUG
@@ -79,14 +81,14 @@
 #define JUMBO_LEN	9000
 
 /* Module parameters */
-#define TX_TIMEO 5000 /* default 5 seconds */
+#define TX_TIMEO	5000
 static int watchdog = TX_TIMEO;
 module_param(watchdog, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(watchdog, "Transmit timeout in milliseconds");
+MODULE_PARM_DESC(watchdog, "Transmit timeout in milliseconds (default 5s)");
 
-static int debug = -1;		/* -1: default, 0: no output, 16:  all */
+static int debug = -1;
 module_param(debug, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(debug, "Message Level (0: no output, 16: all)");
+MODULE_PARM_DESC(debug, "Message Level (-1: default, 0: no output, 16: all)");
 
 int phyaddr = -1;
 module_param(phyaddr, int, S_IRUGO);
@@ -130,6 +132,13 @@ module_param(eee_timer, int, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(eee_timer, "LPI tx expiration time in msec");
 #define STMMAC_LPI_TIMER(x) (jiffies + msecs_to_jiffies(x))
 
+/* By default the driver will use the ring mode to manage tx and rx descriptors
+ * but passing this value so user can force to use the chain instead of the ring
+ */
+static unsigned int chain_mode;
+module_param(chain_mode, int, S_IRUGO);
+MODULE_PARM_DESC(chain_mode, "To use chain instead of ring mode");
+
 static irqreturn_t stmmac_interrupt(int irq, void *dev_id);
 
 #ifdef CONFIG_STMMAC_DEBUG_FS
@@ -164,6 +173,18 @@ static void stmmac_verify_args(void)
 		eee_timer = STMMAC_DEFAULT_LPI_TIMER;
 }
 
+/**
+ * stmmac_clk_csr_set - dynamically set the MDC clock
+ * @priv: driver private structure
+ * Description: this is to dynamically set the MDC clock according to the csr
+ * clock input.
+ * Note:
+ *	If a specific clk_csr value is passed from the platform
+ *	this means that the CSR Clock Range selection cannot be
+ *	changed at run-time and it is fixed (as reported in the driver
+ *	documentation). Viceversa the driver will try to set the MDC
+ *	clock dynamically according to the actual clock input.
+ */
 static void stmmac_clk_csr_set(struct stmmac_priv *priv)
 {
 	u32 clk_rate;
@@ -171,7 +192,12 @@ static void stmmac_clk_csr_set(struct stmmac_priv *priv)
 	clk_rate = clk_get_rate(priv->stmmac_clk);
 
 	/* Platform provided default clk_csr would be assumed valid
-	 * for all other cases except for the below mentioned ones. */
+	 * for all other cases except for the below mentioned ones.
+	 * For values higher than the IEEE 802.3 specified frequency
+	 * we can not estimate the proper divider as it is not known
+	 * the frequency of clk_csr_i. So we do not change the default
+	 * divider.
+	 */
 	if (!(priv->clk_csr & MAC_CSR_H_FRQ_MASK)) {
 		if (clk_rate < CSR_F_35M)
 			priv->clk_csr = STMMAC_CSR_20_35M;
@@ -185,10 +211,7 @@ static void stmmac_clk_csr_set(struct stmmac_priv *priv)
 			priv->clk_csr = STMMAC_CSR_150_250M;
 		else if ((clk_rate >= CSR_F_250M) && (clk_rate < CSR_F_300M))
 			priv->clk_csr = STMMAC_CSR_250_300M;
-	} /* For values higher than the IEEE 802.3 specified frequency
-	   * we can not estimate the proper divider as it is not known
-	   * the frequency of clk_csr_i. So we do not change the default
-	   * divider. */
+	}
 }
 
 #if defined(STMMAC_XMIT_DEBUG) || defined(STMMAC_RX_DEBUG)
@@ -213,18 +236,25 @@ static inline u32 stmmac_tx_avail(struct stmmac_priv *priv)
 	return priv->dirty_tx + priv->dma_tx_size - priv->cur_tx - 1;
 }
 
-/* On some ST platforms, some HW system configuraton registers have to be
- * set according to the link speed negotiated.
+/**
+ * stmmac_hw_fix_mac_speed: callback for speed selection
+ * @priv: driver private structure
+ * Description: on some platforms (e.g. ST), some HW system configuraton
+ * registers have to be set according to the link speed negotiated.
  */
 static inline void stmmac_hw_fix_mac_speed(struct stmmac_priv *priv)
 {
 	struct phy_device *phydev = priv->phydev;
 
 	if (likely(priv->plat->fix_mac_speed))
-		priv->plat->fix_mac_speed(priv->plat->bsp_priv,
-					  phydev->speed);
+		priv->plat->fix_mac_speed(priv->plat->bsp_priv, phydev->speed);
 }
 
+/**
+ * stmmac_enable_eee_mode: Check and enter in LPI mode
+ * @priv: driver private structure
+ * Description: this function is to verify and enter in LPI mode for EEE.
+ */
 static void stmmac_enable_eee_mode(struct stmmac_priv *priv)
 {
 	/* Check and enter in LPI mode */
@@ -233,19 +263,24 @@ static void stmmac_enable_eee_mode(struct stmmac_priv *priv)
 		priv->hw->mac->set_eee_mode(priv->ioaddr);
 }
 
+/**
+ * stmmac_disable_eee_mode: disable/exit from EEE
+ * @priv: driver private structure
+ * Description: this function is to exit and disable EEE in case of
+ * LPI state is true. This is called by the xmit.
+ */
 void stmmac_disable_eee_mode(struct stmmac_priv *priv)
 {
-	/* Exit and disable EEE in case of we are are in LPI state. */
 	priv->hw->mac->reset_eee_mode(priv->ioaddr);
 	del_timer_sync(&priv->eee_ctrl_timer);
 	priv->tx_path_in_lpi_mode = false;
 }
 
 /**
- * stmmac_eee_ctrl_timer
+ * stmmac_eee_ctrl_timer: EEE TX SW timer.
  * @arg : data hook
  * Description:
- *  If there is no data transfer and if we are not in LPI state,
+ *  if there is no data transfer and if we are not in LPI state,
  *  then MAC Transmitter can be moved to LPI state.
  */
 static void stmmac_eee_ctrl_timer(unsigned long arg)
@@ -257,8 +292,8 @@ static void stmmac_eee_ctrl_timer(unsigned long arg)
 }
 
 /**
- * stmmac_eee_init
- * @priv: private device pointer
+ * stmmac_eee_init: init EEE
+ * @priv: driver private structure
  * Description:
  *  If the EEE support has been enabled while configuring the driver,
  *  if the GMAC actually supports the EEE (from the HW cap reg) and the
@@ -294,16 +329,359 @@ out:
 	return ret;
 }
 
+/**
+ * stmmac_eee_adjust: adjust HW EEE according to the speed
+ * @priv: driver private structure
+ * Description:
+ *	When the EEE has been already initialised we have to
+ *	modify the PLS bit in the LPI ctrl & status reg according
+ *	to the PHY link status. For this reason.
+ */
 static void stmmac_eee_adjust(struct stmmac_priv *priv)
 {
-	/* When the EEE has been already initialised we have to
-	 * modify the PLS bit in the LPI ctrl & status reg according
-	 * to the PHY link status. For this reason.
-	 */
 	if (priv->eee_enabled)
 		priv->hw->mac->set_eee_pls(priv->ioaddr, priv->phydev->link);
 }
 
+/* stmmac_get_tx_hwtstamp: get HW TX timestamps
+ * @priv: driver private structure
+ * @entry : descriptor index to be used.
+ * @skb : the socket buffer
+ * Description :
+ * This function will read timestamp from the descriptor & pass it to stack.
+ * and also perform some sanity checks.
+ */
+static void stmmac_get_tx_hwtstamp(struct stmmac_priv *priv,
+				   unsigned int entry, struct sk_buff *skb)
+{
+	struct skb_shared_hwtstamps shhwtstamp;
+	u64 ns;
+	void *desc = NULL;
+
+	if (!priv->hwts_tx_en)
+		return;
+
+	/* exit if skb doesn't support hw tstamp */
+	if (likely(!(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS)))
+		return;
+
+	if (priv->adv_ts)
+		desc = (priv->dma_etx + entry);
+	else
+		desc = (priv->dma_tx + entry);
+
+	/* check tx tstamp status */
+	if (!priv->hw->desc->get_tx_timestamp_status((struct dma_desc *)desc))
+		return;
+
+	/* get the valid tstamp */
+	ns = priv->hw->desc->get_timestamp(desc, priv->adv_ts);
+
+	memset(&shhwtstamp, 0, sizeof(struct skb_shared_hwtstamps));
+	shhwtstamp.hwtstamp = ns_to_ktime(ns);
+	/* pass tstamp to stack */
+	skb_tstamp_tx(skb, &shhwtstamp);
+
+	return;
+}
+
+/* stmmac_get_rx_hwtstamp: get HW RX timestamps
+ * @priv: driver private structure
+ * @entry : descriptor index to be used.
+ * @skb : the socket buffer
+ * Description :
+ * This function will read received packet's timestamp from the descriptor
+ * and pass it to stack. It also perform some sanity checks.
+ */
+static void stmmac_get_rx_hwtstamp(struct stmmac_priv *priv,
+				   unsigned int entry, struct sk_buff *skb)
+{
+	struct skb_shared_hwtstamps *shhwtstamp = NULL;
+	u64 ns;
+	void *desc = NULL;
+
+	if (!priv->hwts_rx_en)
+		return;
+
+	if (priv->adv_ts)
+		desc = (priv->dma_erx + entry);
+	else
+		desc = (priv->dma_rx + entry);
+
+	/* exit if rx tstamp is not valid */
+	if (!priv->hw->desc->get_rx_timestamp_status(desc, priv->adv_ts))
+		return;
+
+	/* get valid tstamp */
+	ns = priv->hw->desc->get_timestamp(desc, priv->adv_ts);
+	shhwtstamp = skb_hwtstamps(skb);
+	memset(shhwtstamp, 0, sizeof(struct skb_shared_hwtstamps));
+	shhwtstamp->hwtstamp = ns_to_ktime(ns);
+}
+
+/**
+ *  stmmac_hwtstamp_ioctl - control hardware timestamping.
+ *  @dev: device pointer.
+ *  @ifr: An IOCTL specefic structure, that can contain a pointer to
+ *  a proprietary structure used to pass information to the driver.
+ *  Description:
+ *  This function configures the MAC to enable/disable both outgoing(TX)
+ *  and incoming(RX) packets time stamping based on user input.
+ *  Return Value:
+ *  0 on success and an appropriate -ve integer on failure.
+ */
+static int stmmac_hwtstamp_ioctl(struct net_device *dev, struct ifreq *ifr)
+{
+	struct stmmac_priv *priv = netdev_priv(dev);
+	struct hwtstamp_config config;
+	struct timespec now;
+	u64 temp = 0;
+	u32 ptp_v2 = 0;
+	u32 tstamp_all = 0;
+	u32 ptp_over_ipv4_udp = 0;
+	u32 ptp_over_ipv6_udp = 0;
+	u32 ptp_over_ethernet = 0;
+	u32 snap_type_sel = 0;
+	u32 ts_master_en = 0;
+	u32 ts_event_en = 0;
+	u32 value = 0;
+
+	if (!(priv->dma_cap.time_stamp || priv->adv_ts)) {
+		netdev_alert(priv->dev, "No support for HW time stamping\n");
+		priv->hwts_tx_en = 0;
+		priv->hwts_rx_en = 0;
+
+		return -EOPNOTSUPP;
+	}
+
+	if (copy_from_user(&config, ifr->ifr_data,
+			   sizeof(struct hwtstamp_config)))
+		return -EFAULT;
+
+	pr_debug("%s config flags:0x%x, tx_type:0x%x, rx_filter:0x%x\n",
+		 __func__, config.flags, config.tx_type, config.rx_filter);
+
+	/* reserved for future extensions */
+	if (config.flags)
+		return -EINVAL;
+
+	switch (config.tx_type) {
+	case HWTSTAMP_TX_OFF:
+		priv->hwts_tx_en = 0;
+		break;
+	case HWTSTAMP_TX_ON:
+		priv->hwts_tx_en = 1;
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	if (priv->adv_ts) {
+		switch (config.rx_filter) {
+		case HWTSTAMP_FILTER_NONE:
+			/* time stamp no incoming packet at all */
+			config.rx_filter = HWTSTAMP_FILTER_NONE;
+			break;
+
+		case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+			/* PTP v1, UDP, any kind of event packet */
+			config.rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_EVENT;
+			/* take time stamp for all event messages */
+			snap_type_sel = PTP_TCR_SNAPTYPSEL_1;
+
+			ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
+			ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
+			break;
+
+		case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+			/* PTP v1, UDP, Sync packet */
+			config.rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_SYNC;
+			/* take time stamp for SYNC messages only */
+			ts_event_en = PTP_TCR_TSEVNTENA;
+
+			ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
+			ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
+			break;
+
+		case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+			/* PTP v1, UDP, Delay_req packet */
+			config.rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ;
+			/* take time stamp for Delay_Req messages only */
+			ts_master_en = PTP_TCR_TSMSTRENA;
+			ts_event_en = PTP_TCR_TSEVNTENA;
+
+			ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
+			ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
+			break;
+
+		case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+			/* PTP v2, UDP, any kind of event packet */
+			config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
+			ptp_v2 = PTP_TCR_TSVER2ENA;
+			/* take time stamp for all event messages */
+			snap_type_sel = PTP_TCR_SNAPTYPSEL_1;
+
+			ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
+			ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
+			break;
+
+		case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+			/* PTP v2, UDP, Sync packet */
+			config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_SYNC;
+			ptp_v2 = PTP_TCR_TSVER2ENA;
+			/* take time stamp for SYNC messages only */
+			ts_event_en = PTP_TCR_TSEVNTENA;
+
+			ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
+			ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
+			break;
+
+		case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+			/* PTP v2, UDP, Delay_req packet */
+			config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ;
+			ptp_v2 = PTP_TCR_TSVER2ENA;
+			/* take time stamp for Delay_Req messages only */
+			ts_master_en = PTP_TCR_TSMSTRENA;
+			ts_event_en = PTP_TCR_TSEVNTENA;
+
+			ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
+			ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
+			break;
+
+		case HWTSTAMP_FILTER_PTP_V2_EVENT:
+			/* PTP v2/802.AS1 any layer, any kind of event packet */
+			config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
+			ptp_v2 = PTP_TCR_TSVER2ENA;
+			/* take time stamp for all event messages */
+			snap_type_sel = PTP_TCR_SNAPTYPSEL_1;
+
+			ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
+			ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
+			ptp_over_ethernet = PTP_TCR_TSIPENA;
+			break;
+
+		case HWTSTAMP_FILTER_PTP_V2_SYNC:
+			/* PTP v2/802.AS1, any layer, Sync packet */
+			config.rx_filter = HWTSTAMP_FILTER_PTP_V2_SYNC;
+			ptp_v2 = PTP_TCR_TSVER2ENA;
+			/* take time stamp for SYNC messages only */
+			ts_event_en = PTP_TCR_TSEVNTENA;
+
+			ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
+			ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
+			ptp_over_ethernet = PTP_TCR_TSIPENA;
+			break;
+
+		case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+			/* PTP v2/802.AS1, any layer, Delay_req packet */
+			config.rx_filter = HWTSTAMP_FILTER_PTP_V2_DELAY_REQ;
+			ptp_v2 = PTP_TCR_TSVER2ENA;
+			/* take time stamp for Delay_Req messages only */
+			ts_master_en = PTP_TCR_TSMSTRENA;
+			ts_event_en = PTP_TCR_TSEVNTENA;
+
+			ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
+			ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
+			ptp_over_ethernet = PTP_TCR_TSIPENA;
+			break;
+
+		case HWTSTAMP_FILTER_ALL:
+			/* time stamp any incoming packet */
+			config.rx_filter = HWTSTAMP_FILTER_ALL;
+			tstamp_all = PTP_TCR_TSENALL;
+			break;
+
+		default:
+			return -ERANGE;
+		}
+	} else {
+		switch (config.rx_filter) {
+		case HWTSTAMP_FILTER_NONE:
+			config.rx_filter = HWTSTAMP_FILTER_NONE;
+			break;
+		default:
+			/* PTP v1, UDP, any kind of event packet */
+			config.rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_EVENT;
+			break;
+		}
+	}
+	priv->hwts_rx_en = ((config.rx_filter == HWTSTAMP_FILTER_NONE) ? 0 : 1);
+
+	if (!priv->hwts_tx_en && !priv->hwts_rx_en)
+		priv->hw->ptp->config_hw_tstamping(priv->ioaddr, 0);
+	else {
+		value = (PTP_TCR_TSENA | PTP_TCR_TSCFUPDT | PTP_TCR_TSCTRLSSR |
+			 tstamp_all | ptp_v2 | ptp_over_ethernet |
+			 ptp_over_ipv6_udp | ptp_over_ipv4_udp | ts_event_en |
+			 ts_master_en | snap_type_sel);
+
+		priv->hw->ptp->config_hw_tstamping(priv->ioaddr, value);
+
+		/* program Sub Second Increment reg */
+		priv->hw->ptp->config_sub_second_increment(priv->ioaddr);
+
+		/* calculate default added value:
+		 * formula is :
+		 * addend = (2^32)/freq_div_ratio;
+		 * where, freq_div_ratio = STMMAC_SYSCLOCK/50MHz
+		 * hence, addend = ((2^32) * 50MHz)/STMMAC_SYSCLOCK;
+		 * NOTE: STMMAC_SYSCLOCK should be >= 50MHz to
+		 *       achive 20ns accuracy.
+		 *
+		 * 2^x * y == (y << x), hence
+		 * 2^32 * 50000000 ==> (50000000 << 32)
+		 */
+		temp = (u64) (50000000ULL << 32);
+		priv->default_addend = div_u64(temp, STMMAC_SYSCLOCK);
+		priv->hw->ptp->config_addend(priv->ioaddr,
+					     priv->default_addend);
+
+		/* initialize system time */
+		getnstimeofday(&now);
+		priv->hw->ptp->init_systime(priv->ioaddr, now.tv_sec,
+					    now.tv_nsec);
+	}
+
+	return copy_to_user(ifr->ifr_data, &config,
+			    sizeof(struct hwtstamp_config)) ? -EFAULT : 0;
+}
+
+/**
+ * stmmac_init_ptp: init PTP
+ * @priv: driver private structure
+ * Description: this is to verify if the HW supports the PTPv1 or v2.
+ * This is done by looking at the HW cap. register.
+ * Also it registers the ptp driver.
+ */
+static int stmmac_init_ptp(struct stmmac_priv *priv)
+{
+	if (!(priv->dma_cap.time_stamp || priv->dma_cap.atime_stamp))
+		return -EOPNOTSUPP;
+
+	if (netif_msg_hw(priv)) {
+		if (priv->dma_cap.time_stamp) {
+			pr_debug("IEEE 1588-2002 Time Stamp supported\n");
+			priv->adv_ts = 0;
+		}
+		if (priv->dma_cap.atime_stamp && priv->extend_desc) {
+			pr_debug
+			    ("IEEE 1588-2008 Advanced Time Stamp supported\n");
+			priv->adv_ts = 1;
+		}
+	}
+
+	priv->hw->ptp = &stmmac_ptp;
+	priv->hwts_tx_en = 0;
+	priv->hwts_rx_en = 0;
+
+	return stmmac_ptp_register(priv);
+}
+
+static void stmmac_release_ptp(struct stmmac_priv *priv)
+{
+	stmmac_ptp_unregister(priv);
+}
+
 /**
  * stmmac_adjust_link
  * @dev: net device structure
@@ -349,7 +727,7 @@ static void stmmac_adjust_link(struct net_device *dev)
 			case 1000:
 				if (likely(priv->plat->has_gmac))
 					ctrl &= ~priv->hw->link.port;
-					stmmac_hw_fix_mac_speed(priv);
+				stmmac_hw_fix_mac_speed(priv);
 				break;
 			case 100:
 			case 10:
@@ -367,8 +745,8 @@ static void stmmac_adjust_link(struct net_device *dev)
 				break;
 			default:
 				if (netif_msg_link(priv))
-					pr_warning("%s: Speed (%d) is not 10"
-				       " or 100!\n", dev->name, phydev->speed);
+					pr_warn("%s: Speed (%d) not 10/100\n",
+						dev->name, phydev->speed);
 				break;
 			}
 
@@ -399,6 +777,31 @@ static void stmmac_adjust_link(struct net_device *dev)
 }
 
 /**
+ * stmmac_check_pcs_mode: verify if RGMII/SGMII is supported
+ * @priv: driver private structure
+ * Description: this is to verify if the HW supports the PCS.
+ * Physical Coding Sublayer (PCS) interface that can be used when the MAC is
+ * configured for the TBI, RTBI, or SGMII PHY interface.
+ */
+static void stmmac_check_pcs_mode(struct stmmac_priv *priv)
+{
+	int interface = priv->plat->interface;
+
+	if (priv->dma_cap.pcs) {
+		if ((interface & PHY_INTERFACE_MODE_RGMII) ||
+		    (interface & PHY_INTERFACE_MODE_RGMII_ID) ||
+		    (interface & PHY_INTERFACE_MODE_RGMII_RXID) ||
+		    (interface & PHY_INTERFACE_MODE_RGMII_TXID)) {
+			pr_debug("STMMAC: PCS RGMII support enable\n");
+			priv->pcs = STMMAC_PCS_RGMII;
+		} else if (interface & PHY_INTERFACE_MODE_SGMII) {
+			pr_debug("STMMAC: PCS SGMII support enable\n");
+			priv->pcs = STMMAC_PCS_SGMII;
+		}
+	}
+}
+
+/**
  * stmmac_init_phy - PHY initialization
  * @dev: net device structure
  * Description: it initializes the driver's PHY state, and attaches the PHY
@@ -419,10 +822,10 @@ static int stmmac_init_phy(struct net_device *dev)
 
 	if (priv->plat->phy_bus_name)
 		snprintf(bus_id, MII_BUS_ID_SIZE, "%s-%x",
-				priv->plat->phy_bus_name, priv->plat->bus_id);
+			 priv->plat->phy_bus_name, priv->plat->bus_id);
 	else
 		snprintf(bus_id, MII_BUS_ID_SIZE, "stmmac-%x",
-				priv->plat->bus_id);
+			 priv->plat->bus_id);
 
 	snprintf(phy_id_fmt, MII_BUS_ID_SIZE + 3, PHY_ID_FMT, bus_id,
 		 priv->plat->phy_addr);
@@ -461,29 +864,57 @@ static int stmmac_init_phy(struct net_device *dev)
 }
 
 /**
- * display_ring
- * @p: pointer to the ring.
+ * stmmac_display_ring: display ring
+ * @head: pointer to the head of the ring passed.
  * @size: size of the ring.
- * Description: display all the descriptors within the ring.
+ * @extend_desc: to verify if extended descriptors are used.
+ * Description: display the control/status and buffer descriptors.
  */
-static void display_ring(struct dma_desc *p, int size)
+static void stmmac_display_ring(void *head, int size, int extend_desc)
 {
-	struct tmp_s {
-		u64 a;
-		unsigned int b;
-		unsigned int c;
-	};
 	int i;
+	struct dma_extended_desc *ep = (struct dma_extended_desc *)head;
+	struct dma_desc *p = (struct dma_desc *)head;
+
 	for (i = 0; i < size; i++) {
-		struct tmp_s *x = (struct tmp_s *)(p + i);
-		pr_info("\t%d [0x%x]: DES0=0x%x DES1=0x%x BUF1=0x%x BUF2=0x%x",
-		       i, (unsigned int)virt_to_phys(&p[i]),
-		       (unsigned int)(x->a), (unsigned int)((x->a) >> 32),
-		       x->b, x->c);
+		u64 x;
+		if (extend_desc) {
+			x = *(u64 *) ep;
+			pr_info("%d [0x%x]: 0x%x 0x%x 0x%x 0x%x\n",
+				i, (unsigned int)virt_to_phys(ep),
+				(unsigned int)x, (unsigned int)(x >> 32),
+				ep->basic.des2, ep->basic.des3);
+			ep++;
+		} else {
+			x = *(u64 *) p;
+			pr_info("%d [0x%x]: 0x%x 0x%x 0x%x 0x%x",
+				i, (unsigned int)virt_to_phys(p),
+				(unsigned int)x, (unsigned int)(x >> 32),
+				p->des2, p->des3);
+			p++;
+		}
 		pr_info("\n");
 	}
 }
 
+static void stmmac_display_rings(struct stmmac_priv *priv)
+{
+	unsigned int txsize = priv->dma_tx_size;
+	unsigned int rxsize = priv->dma_rx_size;
+
+	if (priv->extend_desc) {
+		pr_info("Extended RX descriptor ring:\n");
+		stmmac_display_ring((void *)priv->dma_erx, rxsize, 1);
+		pr_info("Extended TX descriptor ring:\n");
+		stmmac_display_ring((void *)priv->dma_etx, txsize, 1);
+	} else {
+		pr_info("RX descriptor ring:\n");
+		stmmac_display_ring((void *)priv->dma_rx, rxsize, 0);
+		pr_info("TX descriptor ring:\n");
+		stmmac_display_ring((void *)priv->dma_tx, txsize, 0);
+	}
+}
+
 static int stmmac_set_bfsize(int mtu, int bufsize)
 {
 	int ret = bufsize;
@@ -501,6 +932,65 @@ static int stmmac_set_bfsize(int mtu, int bufsize)
 }
 
 /**
+ * stmmac_clear_descriptors: clear descriptors
+ * @priv: driver private structure
+ * Description: this function is called to clear the tx and rx descriptors
+ * in case of both basic and extended descriptors are used.
+ */
+static void stmmac_clear_descriptors(struct stmmac_priv *priv)
+{
+	int i;
+	unsigned int txsize = priv->dma_tx_size;
+	unsigned int rxsize = priv->dma_rx_size;
+
+	/* Clear the Rx/Tx descriptors */
+	for (i = 0; i < rxsize; i++)
+		if (priv->extend_desc)
+			priv->hw->desc->init_rx_desc(&priv->dma_erx[i].basic,
+						     priv->use_riwt, priv->mode,
+						     (i == rxsize - 1));
+		else
+			priv->hw->desc->init_rx_desc(&priv->dma_rx[i],
+						     priv->use_riwt, priv->mode,
+						     (i == rxsize - 1));
+	for (i = 0; i < txsize; i++)
+		if (priv->extend_desc)
+			priv->hw->desc->init_tx_desc(&priv->dma_etx[i].basic,
+						     priv->mode,
+						     (i == txsize - 1));
+		else
+			priv->hw->desc->init_tx_desc(&priv->dma_tx[i],
+						     priv->mode,
+						     (i == txsize - 1));
+}
+
+static int stmmac_init_rx_buffers(struct stmmac_priv *priv, struct dma_desc *p,
+				  int i)
+{
+	struct sk_buff *skb;
+
+	skb = __netdev_alloc_skb(priv->dev, priv->dma_buf_sz + NET_IP_ALIGN,
+				 GFP_KERNEL);
+	if (unlikely(skb == NULL)) {
+		pr_err("%s: Rx init fails; skb is NULL\n", __func__);
+		return 1;
+	}
+	skb_reserve(skb, NET_IP_ALIGN);
+	priv->rx_skbuff[i] = skb;
+	priv->rx_skbuff_dma[i] = dma_map_single(priv->device, skb->data,
+						priv->dma_buf_sz,
+						DMA_FROM_DEVICE);
+
+	p->des2 = priv->rx_skbuff_dma[i];
+
+	if ((priv->mode == STMMAC_RING_MODE) &&
+	    (priv->dma_buf_sz == BUF_SIZE_16KiB))
+		priv->hw->ring->init_desc3(p);
+
+	return 0;
+}
+
+/**
  * init_dma_desc_rings - init the RX/TX descriptor rings
  * @dev: net device structure
  * Description:  this function initializes the DMA RX/TX descriptors
@@ -511,110 +1001,114 @@ static void init_dma_desc_rings(struct net_device *dev)
 {
 	int i;
 	struct stmmac_priv *priv = netdev_priv(dev);
-	struct sk_buff *skb;
 	unsigned int txsize = priv->dma_tx_size;
 	unsigned int rxsize = priv->dma_rx_size;
-	unsigned int bfsize;
-	int dis_ic = 0;
-	int des3_as_data_buf = 0;
+	unsigned int bfsize = 0;
 
 	/* Set the max buffer size according to the DESC mode
-	 * and the MTU. Note that RING mode allows 16KiB bsize. */
-	bfsize = priv->hw->ring->set_16kib_bfsize(dev->mtu);
+	 * and the MTU. Note that RING mode allows 16KiB bsize.
+	 */
+	if (priv->mode == STMMAC_RING_MODE)
+		bfsize = priv->hw->ring->set_16kib_bfsize(dev->mtu);
 
-	if (bfsize == BUF_SIZE_16KiB)
-		des3_as_data_buf = 1;
-	else
+	if (bfsize < BUF_SIZE_16KiB)
 		bfsize = stmmac_set_bfsize(dev->mtu, priv->dma_buf_sz);
 
 	DBG(probe, INFO, "stmmac: txsize %d, rxsize %d, bfsize %d\n",
 	    txsize, rxsize, bfsize);
 
-	priv->rx_skbuff_dma = kmalloc_array(rxsize, sizeof(dma_addr_t),
-					    GFP_KERNEL);
-	priv->rx_skbuff = kmalloc_array(rxsize, sizeof(struct sk_buff *),
-					GFP_KERNEL);
-	priv->dma_rx =
-	    (struct dma_desc *)dma_alloc_coherent(priv->device,
-						  rxsize *
+	if (priv->extend_desc) {
+		priv->dma_erx = dma_alloc_coherent(priv->device, rxsize *
+						   sizeof(struct
+							  dma_extended_desc),
+						   &priv->dma_rx_phy,
+						   GFP_KERNEL);
+		priv->dma_etx = dma_alloc_coherent(priv->device, txsize *
+						   sizeof(struct
+							  dma_extended_desc),
+						   &priv->dma_tx_phy,
+						   GFP_KERNEL);
+		if ((!priv->dma_erx) || (!priv->dma_etx))
+			return;
+	} else {
+		priv->dma_rx = dma_alloc_coherent(priv->device, rxsize *
 						  sizeof(struct dma_desc),
 						  &priv->dma_rx_phy,
 						  GFP_KERNEL);
-	priv->tx_skbuff = kmalloc_array(txsize, sizeof(struct sk_buff *),
-					GFP_KERNEL);
-	priv->dma_tx =
-	    (struct dma_desc *)dma_alloc_coherent(priv->device,
-						  txsize *
+		priv->dma_tx = dma_alloc_coherent(priv->device, txsize *
 						  sizeof(struct dma_desc),
 						  &priv->dma_tx_phy,
 						  GFP_KERNEL);
-
-	if ((priv->dma_rx == NULL) || (priv->dma_tx == NULL)) {
-		pr_err("%s:ERROR allocating the DMA Tx/Rx desc\n", __func__);
-		return;
+		if ((!priv->dma_rx) || (!priv->dma_tx))
+			return;
 	}
 
-	DBG(probe, INFO, "stmmac (%s) DMA desc: virt addr (Rx %p, "
-	    "Tx %p)\n\tDMA phy addr (Rx 0x%08x, Tx 0x%08x)\n",
-	    dev->name, priv->dma_rx, priv->dma_tx,
-	    (unsigned int)priv->dma_rx_phy, (unsigned int)priv->dma_tx_phy);
+	priv->rx_skbuff_dma = kmalloc_array(rxsize, sizeof(dma_addr_t),
+					    GFP_KERNEL);
+	priv->rx_skbuff = kmalloc_array(rxsize, sizeof(struct sk_buff *),
+					GFP_KERNEL);
+	priv->tx_skbuff_dma = kmalloc_array(txsize, sizeof(dma_addr_t),
+					    GFP_KERNEL);
+	priv->tx_skbuff = kmalloc_array(txsize, sizeof(struct sk_buff *),
+					GFP_KERNEL);
+	if (netif_msg_drv(priv))
+		pr_debug("(%s) dma_rx_phy=0x%08x dma_tx_phy=0x%08x\n", __func__,
+			 (u32) priv->dma_rx_phy, (u32) priv->dma_tx_phy);
 
 	/* RX INITIALIZATION */
-	DBG(probe, INFO, "stmmac: SKB addresses:\n"
-			 "skb\t\tskb data\tdma data\n");
-
+	DBG(probe, INFO, "stmmac: SKB addresses:\nskb\t\tskb data\tdma data\n");
 	for (i = 0; i < rxsize; i++) {
-		struct dma_desc *p = priv->dma_rx + i;
+		struct dma_desc *p;
+		if (priv->extend_desc)
+			p = &((priv->dma_erx + i)->basic);
+		else
+			p = priv->dma_rx + i;
 
-		skb = __netdev_alloc_skb(dev, bfsize + NET_IP_ALIGN,
-					 GFP_KERNEL);
-		if (unlikely(skb == NULL)) {
-			pr_err("%s: Rx init fails; skb is NULL\n", __func__);
+		if (stmmac_init_rx_buffers(priv, p, i))
 			break;
-		}
-		skb_reserve(skb, NET_IP_ALIGN);
-		priv->rx_skbuff[i] = skb;
-		priv->rx_skbuff_dma[i] = dma_map_single(priv->device, skb->data,
-						bfsize, DMA_FROM_DEVICE);
-
-		p->des2 = priv->rx_skbuff_dma[i];
-
-		priv->hw->ring->init_desc3(des3_as_data_buf, p);
 
 		DBG(probe, INFO, "[%p]\t[%p]\t[%x]\n", priv->rx_skbuff[i],
-			priv->rx_skbuff[i]->data, priv->rx_skbuff_dma[i]);
+		    priv->rx_skbuff[i]->data, priv->rx_skbuff_dma[i]);
 	}
 	priv->cur_rx = 0;
 	priv->dirty_rx = (unsigned int)(i - rxsize);
 	priv->dma_buf_sz = bfsize;
 	buf_sz = bfsize;
 
+	/* Setup the chained descriptor addresses */
+	if (priv->mode == STMMAC_CHAIN_MODE) {
+		if (priv->extend_desc) {
+			priv->hw->chain->init(priv->dma_erx, priv->dma_rx_phy,
+					      rxsize, 1);
+			priv->hw->chain->init(priv->dma_etx, priv->dma_tx_phy,
+					      txsize, 1);
+		} else {
+			priv->hw->chain->init(priv->dma_rx, priv->dma_rx_phy,
+					      rxsize, 0);
+			priv->hw->chain->init(priv->dma_tx, priv->dma_tx_phy,
+					      txsize, 0);
+		}
+	}
+
 	/* TX INITIALIZATION */
 	for (i = 0; i < txsize; i++) {
+		struct dma_desc *p;
+		if (priv->extend_desc)
+			p = &((priv->dma_etx + i)->basic);
+		else
+			p = priv->dma_tx + i;
+		p->des2 = 0;
+		priv->tx_skbuff_dma[i] = 0;
 		priv->tx_skbuff[i] = NULL;
-		priv->dma_tx[i].des2 = 0;
 	}
 
-	/* In case of Chained mode this sets the des3 to the next
-	 * element in the chain */
-	priv->hw->ring->init_dma_chain(priv->dma_rx, priv->dma_rx_phy, rxsize);
-	priv->hw->ring->init_dma_chain(priv->dma_tx, priv->dma_tx_phy, txsize);
-
 	priv->dirty_tx = 0;
 	priv->cur_tx = 0;
 
-	if (priv->use_riwt)
-		dis_ic = 1;
-	/* Clear the Rx/Tx descriptors */
-	priv->hw->desc->init_rx_desc(priv->dma_rx, rxsize, dis_ic);
-	priv->hw->desc->init_tx_desc(priv->dma_tx, txsize);
+	stmmac_clear_descriptors(priv);
 
-	if (netif_msg_hw(priv)) {
-		pr_info("RX descriptor ring:\n");
-		display_ring(priv->dma_rx, rxsize);
-		pr_info("TX descriptor ring:\n");
-		display_ring(priv->dma_tx, txsize);
-	}
+	if (netif_msg_hw(priv))
+		stmmac_display_rings(priv);
 }
 
 static void dma_free_rx_skbufs(struct stmmac_priv *priv)
@@ -637,13 +1131,20 @@ static void dma_free_tx_skbufs(struct stmmac_priv *priv)
 
 	for (i = 0; i < priv->dma_tx_size; i++) {
 		if (priv->tx_skbuff[i] != NULL) {
-			struct dma_desc *p = priv->dma_tx + i;
-			if (p->des2)
-				dma_unmap_single(priv->device, p->des2,
+			struct dma_desc *p;
+			if (priv->extend_desc)
+				p = &((priv->dma_etx + i)->basic);
+			else
+				p = priv->dma_tx + i;
+
+			if (priv->tx_skbuff_dma[i])
+				dma_unmap_single(priv->device,
+						 priv->tx_skbuff_dma[i],
 						 priv->hw->desc->get_tx_len(p),
 						 DMA_TO_DEVICE);
 			dev_kfree_skb_any(priv->tx_skbuff[i]);
 			priv->tx_skbuff[i] = NULL;
+			priv->tx_skbuff_dma[i] = 0;
 		}
 	}
 }
@@ -654,29 +1155,38 @@ static void free_dma_desc_resources(struct stmmac_priv *priv)
 	dma_free_rx_skbufs(priv);
 	dma_free_tx_skbufs(priv);
 
-	/* Free the region of consistent memory previously allocated for
-	 * the DMA */
-	dma_free_coherent(priv->device,
-			  priv->dma_tx_size * sizeof(struct dma_desc),
-			  priv->dma_tx, priv->dma_tx_phy);
-	dma_free_coherent(priv->device,
-			  priv->dma_rx_size * sizeof(struct dma_desc),
-			  priv->dma_rx, priv->dma_rx_phy);
+	/* Free DMA regions of consistent memory previously allocated */
+	if (!priv->extend_desc) {
+		dma_free_coherent(priv->device,
+				  priv->dma_tx_size * sizeof(struct dma_desc),
+				  priv->dma_tx, priv->dma_tx_phy);
+		dma_free_coherent(priv->device,
+				  priv->dma_rx_size * sizeof(struct dma_desc),
+				  priv->dma_rx, priv->dma_rx_phy);
+	} else {
+		dma_free_coherent(priv->device, priv->dma_tx_size *
+				  sizeof(struct dma_extended_desc),
+				  priv->dma_etx, priv->dma_tx_phy);
+		dma_free_coherent(priv->device, priv->dma_rx_size *
+				  sizeof(struct dma_extended_desc),
+				  priv->dma_erx, priv->dma_rx_phy);
+	}
 	kfree(priv->rx_skbuff_dma);
 	kfree(priv->rx_skbuff);
+	kfree(priv->tx_skbuff_dma);
 	kfree(priv->tx_skbuff);
 }
 
 /**
  *  stmmac_dma_operation_mode - HW DMA operation mode
- *  @priv : pointer to the private device structure.
+ *  @priv: driver private structure
  *  Description: it sets the DMA operation mode: tx/rx DMA thresholds
  *  or Store-And-Forward capability.
  */
 static void stmmac_dma_operation_mode(struct stmmac_priv *priv)
 {
 	if (likely(priv->plat->force_sf_dma_mode ||
-		((priv->plat->tx_coe) && (!priv->no_csum_insertion)))) {
+		   ((priv->plat->tx_coe) && (!priv->no_csum_insertion)))) {
 		/*
 		 * In case of GMAC, SF mode can be enabled
 		 * to perform the TX COE in HW. This depends on:
@@ -684,8 +1194,7 @@ static void stmmac_dma_operation_mode(struct stmmac_priv *priv)
 		 * 2) There is no bugged Jumbo frame support
 		 *    that needs to not insert csum in the TDES.
 		 */
-		priv->hw->dma->dma_mode(priv->ioaddr,
-					SF_DMA_MODE, SF_DMA_MODE);
+		priv->hw->dma->dma_mode(priv->ioaddr, SF_DMA_MODE, SF_DMA_MODE);
 		tc = SF_DMA_MODE;
 	} else
 		priv->hw->dma->dma_mode(priv->ioaddr, tc, SF_DMA_MODE);
@@ -693,7 +1202,7 @@ static void stmmac_dma_operation_mode(struct stmmac_priv *priv)
 
 /**
  * stmmac_tx_clean:
- * @priv: private data pointer
+ * @priv: driver private structure
  * Description: it reclaims resources after transmission completes.
  */
 static void stmmac_tx_clean(struct stmmac_priv *priv)
@@ -708,40 +1217,50 @@ static void stmmac_tx_clean(struct stmmac_priv *priv)
 		int last;
 		unsigned int entry = priv->dirty_tx % txsize;
 		struct sk_buff *skb = priv->tx_skbuff[entry];
-		struct dma_desc *p = priv->dma_tx + entry;
+		struct dma_desc *p;
+
+		if (priv->extend_desc)
+			p = (struct dma_desc *)(priv->dma_etx + entry);
+		else
+			p = priv->dma_tx + entry;
 
 		/* Check if the descriptor is owned by the DMA. */
 		if (priv->hw->desc->get_tx_owner(p))
 			break;
 
-		/* Verify tx error by looking at the last segment */
+		/* Verify tx error by looking at the last segment. */
 		last = priv->hw->desc->get_tx_ls(p);
 		if (likely(last)) {
 			int tx_error =
-				priv->hw->desc->tx_status(&priv->dev->stats,
-							  &priv->xstats, p,
-							  priv->ioaddr);
+			    priv->hw->desc->tx_status(&priv->dev->stats,
+						      &priv->xstats, p,
+						      priv->ioaddr);
 			if (likely(tx_error == 0)) {
 				priv->dev->stats.tx_packets++;
 				priv->xstats.tx_pkt_n++;
 			} else
 				priv->dev->stats.tx_errors++;
+
+			stmmac_get_tx_hwtstamp(priv, entry, skb);
 		}
 		TX_DBG("%s: curr %d, dirty %d\n", __func__,
-			priv->cur_tx, priv->dirty_tx);
+		       priv->cur_tx, priv->dirty_tx);
 
-		if (likely(p->des2))
-			dma_unmap_single(priv->device, p->des2,
+		if (likely(priv->tx_skbuff_dma[entry])) {
+			dma_unmap_single(priv->device,
+					 priv->tx_skbuff_dma[entry],
 					 priv->hw->desc->get_tx_len(p),
 					 DMA_TO_DEVICE);
-		priv->hw->ring->clean_desc3(p);
+			priv->tx_skbuff_dma[entry] = 0;
+		}
+		priv->hw->ring->clean_desc3(priv, p);
 
 		if (likely(skb != NULL)) {
 			dev_kfree_skb(skb);
 			priv->tx_skbuff[entry] = NULL;
 		}
 
-		priv->hw->desc->release_tx_desc(p);
+		priv->hw->desc->release_tx_desc(p, priv->mode);
 
 		priv->dirty_tx++;
 	}
@@ -749,7 +1268,7 @@ static void stmmac_tx_clean(struct stmmac_priv *priv)
 		     stmmac_tx_avail(priv) > STMMAC_TX_THRESH(priv))) {
 		netif_tx_lock(priv->dev);
 		if (netif_queue_stopped(priv->dev) &&
-		     stmmac_tx_avail(priv) > STMMAC_TX_THRESH(priv)) {
+		    stmmac_tx_avail(priv) > STMMAC_TX_THRESH(priv)) {
 			TX_DBG("%s: restart transmit\n", __func__);
 			netif_wake_queue(priv->dev);
 		}
@@ -773,20 +1292,29 @@ static inline void stmmac_disable_dma_irq(struct stmmac_priv *priv)
 	priv->hw->dma->disable_dma_irq(priv->ioaddr);
 }
 
-
 /**
- * stmmac_tx_err:
- * @priv: pointer to the private device structure
+ * stmmac_tx_err: irq tx error mng function
+ * @priv: driver private structure
  * Description: it cleans the descriptors and restarts the transmission
  * in case of errors.
  */
 static void stmmac_tx_err(struct stmmac_priv *priv)
 {
+	int i;
+	int txsize = priv->dma_tx_size;
 	netif_stop_queue(priv->dev);
 
 	priv->hw->dma->stop_tx(priv->ioaddr);
 	dma_free_tx_skbufs(priv);
-	priv->hw->desc->init_tx_desc(priv->dma_tx, priv->dma_tx_size);
+	for (i = 0; i < txsize; i++)
+		if (priv->extend_desc)
+			priv->hw->desc->init_tx_desc(&priv->dma_etx[i].basic,
+						     priv->mode,
+						     (i == txsize - 1));
+		else
+			priv->hw->desc->init_tx_desc(&priv->dma_tx[i],
+						     priv->mode,
+						     (i == txsize - 1));
 	priv->dirty_tx = 0;
 	priv->cur_tx = 0;
 	priv->hw->dma->start_tx(priv->ioaddr);
@@ -795,6 +1323,14 @@ static void stmmac_tx_err(struct stmmac_priv *priv)
 	netif_wake_queue(priv->dev);
 }
 
+/**
+ * stmmac_dma_interrupt: DMA ISR
+ * @priv: driver private structure
+ * Description: this is the DMA ISR. It is called by the main ISR.
+ * It calls the dwmac dma routine to understand which type of interrupt
+ * happened. In case of there is a Normal interrupt and either TX or RX
+ * interrupt happened so the NAPI is scheduled.
+ */
 static void stmmac_dma_interrupt(struct stmmac_priv *priv)
 {
 	int status;
@@ -817,13 +1353,16 @@ static void stmmac_dma_interrupt(struct stmmac_priv *priv)
 		stmmac_tx_err(priv);
 }
 
+/**
+ * stmmac_mmc_setup: setup the Mac Management Counters (MMC)
+ * @priv: driver private structure
+ * Description: this masks the MMC irq, in fact, the counters are managed in SW.
+ */
 static void stmmac_mmc_setup(struct stmmac_priv *priv)
 {
 	unsigned int mode = MMC_CNTRL_RESET_ON_READ | MMC_CNTRL_COUNTER_RESET |
-			    MMC_CNTRL_PRESET | MMC_CNTRL_FULL_HALF_PRESET;
+	    MMC_CNTRL_PRESET | MMC_CNTRL_FULL_HALF_PRESET;
 
-	/* Mask MMC irq, counters are managed in SW and registers
-	 * are cleared on each READ eventually. */
 	dwmac_mmc_intr_all_mask(priv->ioaddr);
 
 	if (priv->dma_cap.rmon) {
@@ -837,8 +1376,7 @@ static u32 stmmac_get_synopsys_id(struct stmmac_priv *priv)
 {
 	u32 hwid = priv->hw->synopsys_uid;
 
-	/* Only check valid Synopsys Id because old MAC chips
-	 * have no HW registers where get the ID */
+	/* Check Synopsys Id (not available on old chips) */
 	if (likely(hwid)) {
 		u32 uid = ((hwid & 0x0000ff00) >> 8);
 		u32 synid = (hwid & 0x000000ff);
@@ -852,14 +1390,24 @@ static u32 stmmac_get_synopsys_id(struct stmmac_priv *priv)
 }
 
 /**
- * stmmac_selec_desc_mode
- * @priv : private structure
- * Description: select the Enhanced/Alternate or Normal descriptors
+ * stmmac_selec_desc_mode: to select among: normal/alternate/extend descriptors
+ * @priv: driver private structure
+ * Description: select the Enhanced/Alternate or Normal descriptors.
+ * In case of Enhanced/Alternate, it looks at the extended descriptors are
+ * supported by the HW cap. register.
  */
 static void stmmac_selec_desc_mode(struct stmmac_priv *priv)
 {
 	if (priv->plat->enh_desc) {
 		pr_info(" Enhanced/Alternate descriptors\n");
+
+		/* GMAC older than 3.50 has no extended descriptors */
+		if (priv->synopsys_id >= DWMAC_CORE_3_50) {
+			pr_info("\tEnabled extended descriptors\n");
+			priv->extend_desc = 1;
+		} else
+			pr_warn("Extended descriptors not supported\n");
+
 		priv->hw->desc = &enh_desc_ops;
 	} else {
 		pr_info(" Normal descriptors\n");
@@ -868,8 +1416,8 @@ static void stmmac_selec_desc_mode(struct stmmac_priv *priv)
 }
 
 /**
- * stmmac_get_hw_features
- * @priv : private device pointer
+ * stmmac_get_hw_features: get MAC capabilities from the HW cap. register.
+ * @priv: driver private structure
  * Description:
  *  new GMAC chip generations have a new register to indicate the
  *  presence of the optional feature/functions.
@@ -887,69 +1435,78 @@ static int stmmac_get_hw_features(struct stmmac_priv *priv)
 		priv->dma_cap.mbps_1000 = (hw_cap & DMA_HW_FEAT_GMIISEL) >> 1;
 		priv->dma_cap.half_duplex = (hw_cap & DMA_HW_FEAT_HDSEL) >> 2;
 		priv->dma_cap.hash_filter = (hw_cap & DMA_HW_FEAT_HASHSEL) >> 4;
-		priv->dma_cap.multi_addr =
-			(hw_cap & DMA_HW_FEAT_ADDMACADRSEL) >> 5;
+		priv->dma_cap.multi_addr = (hw_cap & DMA_HW_FEAT_ADDMAC) >> 5;
 		priv->dma_cap.pcs = (hw_cap & DMA_HW_FEAT_PCSSEL) >> 6;
 		priv->dma_cap.sma_mdio = (hw_cap & DMA_HW_FEAT_SMASEL) >> 8;
 		priv->dma_cap.pmt_remote_wake_up =
-			(hw_cap & DMA_HW_FEAT_RWKSEL) >> 9;
+		    (hw_cap & DMA_HW_FEAT_RWKSEL) >> 9;
 		priv->dma_cap.pmt_magic_frame =
-			(hw_cap & DMA_HW_FEAT_MGKSEL) >> 10;
+		    (hw_cap & DMA_HW_FEAT_MGKSEL) >> 10;
 		/* MMC */
 		priv->dma_cap.rmon = (hw_cap & DMA_HW_FEAT_MMCSEL) >> 11;
-		/* IEEE 1588-2002*/
+		/* IEEE 1588-2002 */
 		priv->dma_cap.time_stamp =
-			(hw_cap & DMA_HW_FEAT_TSVER1SEL) >> 12;
-		/* IEEE 1588-2008*/
+		    (hw_cap & DMA_HW_FEAT_TSVER1SEL) >> 12;
+		/* IEEE 1588-2008 */
 		priv->dma_cap.atime_stamp =
-			(hw_cap & DMA_HW_FEAT_TSVER2SEL) >> 13;
+		    (hw_cap & DMA_HW_FEAT_TSVER2SEL) >> 13;
 		/* 802.3az - Energy-Efficient Ethernet (EEE) */
 		priv->dma_cap.eee = (hw_cap & DMA_HW_FEAT_EEESEL) >> 14;
 		priv->dma_cap.av = (hw_cap & DMA_HW_FEAT_AVSEL) >> 15;
 		/* TX and RX csum */
 		priv->dma_cap.tx_coe = (hw_cap & DMA_HW_FEAT_TXCOESEL) >> 16;
 		priv->dma_cap.rx_coe_type1 =
-			(hw_cap & DMA_HW_FEAT_RXTYP1COE) >> 17;
+		    (hw_cap & DMA_HW_FEAT_RXTYP1COE) >> 17;
 		priv->dma_cap.rx_coe_type2 =
-			(hw_cap & DMA_HW_FEAT_RXTYP2COE) >> 18;
+		    (hw_cap & DMA_HW_FEAT_RXTYP2COE) >> 18;
 		priv->dma_cap.rxfifo_over_2048 =
-			(hw_cap & DMA_HW_FEAT_RXFIFOSIZE) >> 19;
+		    (hw_cap & DMA_HW_FEAT_RXFIFOSIZE) >> 19;
 		/* TX and RX number of channels */
 		priv->dma_cap.number_rx_channel =
-			(hw_cap & DMA_HW_FEAT_RXCHCNT) >> 20;
+		    (hw_cap & DMA_HW_FEAT_RXCHCNT) >> 20;
 		priv->dma_cap.number_tx_channel =
-			(hw_cap & DMA_HW_FEAT_TXCHCNT) >> 22;
-		/* Alternate (enhanced) DESC mode*/
-		priv->dma_cap.enh_desc =
-			(hw_cap & DMA_HW_FEAT_ENHDESSEL) >> 24;
+		    (hw_cap & DMA_HW_FEAT_TXCHCNT) >> 22;
+		/* Alternate (enhanced) DESC mode */
+		priv->dma_cap.enh_desc = (hw_cap & DMA_HW_FEAT_ENHDESSEL) >> 24;
 	}
 
 	return hw_cap;
 }
 
+/**
+ * stmmac_check_ether_addr: check if the MAC addr is valid
+ * @priv: driver private structure
+ * Description:
+ * it is to verify if the MAC address is valid, in case of failures it
+ * generates a random MAC address
+ */
 static void stmmac_check_ether_addr(struct stmmac_priv *priv)
 {
-	/* verify if the MAC address is valid, in case of failures it
-	 * generates a random MAC address */
 	if (!is_valid_ether_addr(priv->dev->dev_addr)) {
 		priv->hw->mac->get_umac_addr((void __iomem *)
 					     priv->dev->base_addr,
 					     priv->dev->dev_addr, 0);
-		if  (!is_valid_ether_addr(priv->dev->dev_addr))
+		if (!is_valid_ether_addr(priv->dev->dev_addr))
 			eth_hw_addr_random(priv->dev);
 	}
-	pr_warning("%s: device MAC address %pM\n", priv->dev->name,
-						   priv->dev->dev_addr);
+	pr_warn("%s: device MAC address %pM\n", priv->dev->name,
+		priv->dev->dev_addr);
 }
 
+/**
+ * stmmac_init_dma_engine: DMA init.
+ * @priv: driver private structure
+ * Description:
+ * It inits the DMA invoking the specific MAC/GMAC callback.
+ * Some DMA parameters can be passed from the platform;
+ * in case of these are not passed a default is kept for the MAC or GMAC.
+ */
 static int stmmac_init_dma_engine(struct stmmac_priv *priv)
 {
 	int pbl = DEFAULT_DMA_PBL, fixed_burst = 0, burst_len = 0;
 	int mixed_burst = 0;
+	int atds = 0;
 
-	/* Some DMA parameters can be passed from the platform;
-	 * in case of these are not passed we keep a default
-	 * (good for all the chips) and init the DMA! */
 	if (priv->plat->dma_cfg) {
 		pbl = priv->plat->dma_cfg->pbl;
 		fixed_burst = priv->plat->dma_cfg->fixed_burst;
@@ -957,13 +1514,16 @@ static int stmmac_init_dma_engine(struct stmmac_priv *priv)
 		burst_len = priv->plat->dma_cfg->burst_len;
 	}
 
+	if (priv->extend_desc && (priv->mode == STMMAC_RING_MODE))
+		atds = 1;
+
 	return priv->hw->dma->init(priv->ioaddr, pbl, fixed_burst, mixed_burst,
 				   burst_len, priv->dma_tx_phy,
-				   priv->dma_rx_phy);
+				   priv->dma_rx_phy, atds);
 }
 
 /**
- * stmmac_tx_timer:
+ * stmmac_tx_timer: mitigation sw timer for tx.
  * @data: data pointer
  * Description:
  * This is the timer handler to directly invoke the stmmac_tx_clean.
@@ -976,8 +1536,8 @@ static void stmmac_tx_timer(unsigned long data)
 }
 
 /**
- * stmmac_tx_timer:
- * @priv: private data structure
+ * stmmac_init_tx_coalesce: init tx mitigation options.
+ * @priv: driver private structure
  * Description:
  * This inits the transmit coalesce parameters: i.e. timer rate,
  * timer handler and default threshold used for enabling the
@@ -1012,10 +1572,14 @@ static int stmmac_open(struct net_device *dev)
 
 	stmmac_check_ether_addr(priv);
 
-	ret = stmmac_init_phy(dev);
-	if (unlikely(ret)) {
-		pr_err("%s: Cannot attach to PHY (error: %d)\n", __func__, ret);
-		goto open_error;
+	if (priv->pcs != STMMAC_PCS_RGMII && priv->pcs != STMMAC_PCS_TBI &&
+	    priv->pcs != STMMAC_PCS_RTBI) {
+		ret = stmmac_init_phy(dev);
+		if (ret) {
+			pr_err("%s: Cannot attach to PHY (error: %d)\n",
+			       __func__, ret);
+			goto open_error;
+		}
 	}
 
 	/* Create and initialize the TX/RX descriptors chains. */
@@ -1043,7 +1607,7 @@ static int stmmac_open(struct net_device *dev)
 
 	/* Request the IRQ lines */
 	ret = request_irq(dev->irq, stmmac_interrupt,
-			 IRQF_SHARED, dev->name, dev);
+			  IRQF_SHARED, dev->name, dev);
 	if (unlikely(ret < 0)) {
 		pr_err("%s: ERROR: allocating the IRQ %d (error: %d)\n",
 		       __func__, dev->irq, ret);
@@ -1055,8 +1619,8 @@ static int stmmac_open(struct net_device *dev)
 		ret = request_irq(priv->wol_irq, stmmac_interrupt,
 				  IRQF_SHARED, dev->name, dev);
 		if (unlikely(ret < 0)) {
-			pr_err("%s: ERROR: allocating the ext WoL IRQ %d "
-			       "(error: %d)\n",	__func__, priv->wol_irq, ret);
+			pr_err("%s: ERROR: allocating the WoL IRQ %d (%d)\n",
+			       __func__, priv->wol_irq, ret);
 			goto open_error_wolirq;
 		}
 	}
@@ -1084,10 +1648,14 @@ static int stmmac_open(struct net_device *dev)
 
 	stmmac_mmc_setup(priv);
 
+	ret = stmmac_init_ptp(priv);
+	if (ret)
+		pr_warn("%s: failed PTP initialisation\n", __func__);
+
 #ifdef CONFIG_STMMAC_DEBUG_FS
 	ret = stmmac_init_fs(dev);
 	if (ret < 0)
-		pr_warning("%s: failed debugFS registration\n", __func__);
+		pr_warn("%s: failed debugFS registration\n", __func__);
 #endif
 	/* Start the ball rolling... */
 	DBG(probe, DEBUG, "%s: DMA RX/TX processes started...\n", dev->name);
@@ -1104,7 +1672,13 @@ static int stmmac_open(struct net_device *dev)
 		phy_start(priv->phydev);
 
 	priv->tx_lpi_timer = STMMAC_DEFAULT_TWT_LS_TIMER;
-	priv->eee_enabled = stmmac_eee_init(priv);
+
+	/* Using PCS we cannot dial with the phy registers at this stage
+	 * so we do not support extra feature like EEE.
+	 */
+	if (priv->pcs != STMMAC_PCS_RGMII && priv->pcs != STMMAC_PCS_TBI &&
+	    priv->pcs != STMMAC_PCS_RTBI)
+		priv->eee_enabled = stmmac_eee_init(priv);
 
 	stmmac_init_tx_coalesce(priv);
 
@@ -1113,6 +1687,9 @@ static int stmmac_open(struct net_device *dev)
 		priv->hw->dma->rx_watchdog(priv->ioaddr, MAX_DMA_RIWT);
 	}
 
+	if (priv->pcs && priv->hw->mac->ctrl_ane)
+		priv->hw->mac->ctrl_ane(priv->ioaddr, 0);
+
 	napi_enable(&priv->napi);
 	netif_start_queue(dev);
 
@@ -1184,21 +1761,25 @@ static int stmmac_release(struct net_device *dev)
 #endif
 	clk_disable_unprepare(priv->stmmac_clk);
 
+	stmmac_release_ptp(priv);
+
 	return 0;
 }
 
 /**
- *  stmmac_xmit:
+ *  stmmac_xmit: Tx entry point of the driver
  *  @skb : the socket buffer
  *  @dev : device pointer
- *  Description : Tx entry point of the driver.
+ *  Description : this is the tx entry point of the driver.
+ *  It programs the chain or the ring and supports oversized frames
+ *  and SG feature.
  */
 static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct stmmac_priv *priv = netdev_priv(dev);
 	unsigned int txsize = priv->dma_tx_size;
 	unsigned int entry;
-	int i, csum_insertion = 0;
+	int i, csum_insertion = 0, is_jumbo = 0;
 	int nfrags = skb_shinfo(skb)->nr_frags;
 	struct dma_desc *desc, *first;
 	unsigned int nopaged_len = skb_headlen(skb);
@@ -1207,8 +1788,7 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 		if (!netif_queue_stopped(dev)) {
 			netif_stop_queue(dev);
 			/* This is a hard error, log it. */
-			pr_err("%s: BUG! Tx Ring full when queue awake\n",
-				__func__);
+			pr_err("%s: Tx Ring full when queue awake\n", __func__);
 		}
 		return NETDEV_TX_BUSY;
 	}
@@ -1222,10 +1802,9 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 
 #ifdef STMMAC_XMIT_DEBUG
 	if ((skb->len > ETH_FRAME_LEN) || nfrags)
-		pr_debug("stmmac xmit: [entry %d]\n"
-			 "\tskb addr %p - len: %d - nopaged_len: %d\n"
+		pr_debug("%s: [entry %d]: skb addr %p len: %d nopagedlen: %d\n"
 			 "\tn_frags: %d - ip_summed: %d - %s gso\n"
-			 "\ttx_count_frames %d\n", entry,
+			 "\ttx_count_frames %d\n", __func__, entry,
 			 skb, skb->len, nopaged_len, nfrags, skb->ip_summed,
 			 !skb_is_gso(skb) ? "isn't" : "is",
 			 priv->tx_count_frames);
@@ -1233,7 +1812,11 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	csum_insertion = (skb->ip_summed == CHECKSUM_PARTIAL);
 
-	desc = priv->dma_tx + entry;
+	if (priv->extend_desc)
+		desc = (struct dma_desc *)(priv->dma_etx + entry);
+	else
+		desc = priv->dma_tx + entry;
+
 	first = desc;
 
 #ifdef STMMAC_XMIT_DEBUG
@@ -1244,28 +1827,46 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 #endif
 	priv->tx_skbuff[entry] = skb;
 
-	if (priv->hw->ring->is_jumbo_frm(skb->len, priv->plat->enh_desc)) {
-		entry = priv->hw->ring->jumbo_frm(priv, skb, csum_insertion);
-		desc = priv->dma_tx + entry;
+	/* To program the descriptors according to the size of the frame */
+	if (priv->mode == STMMAC_RING_MODE) {
+		is_jumbo = priv->hw->ring->is_jumbo_frm(skb->len,
+							priv->plat->enh_desc);
+		if (unlikely(is_jumbo))
+			entry = priv->hw->ring->jumbo_frm(priv, skb,
+							  csum_insertion);
 	} else {
+		is_jumbo = priv->hw->chain->is_jumbo_frm(skb->len,
+							 priv->plat->enh_desc);
+		if (unlikely(is_jumbo))
+			entry = priv->hw->chain->jumbo_frm(priv, skb,
+							   csum_insertion);
+	}
+	if (likely(!is_jumbo)) {
 		desc->des2 = dma_map_single(priv->device, skb->data,
-					nopaged_len, DMA_TO_DEVICE);
+					    nopaged_len, DMA_TO_DEVICE);
+		priv->tx_skbuff_dma[entry] = desc->des2;
 		priv->hw->desc->prepare_tx_desc(desc, 1, nopaged_len,
-						csum_insertion);
-	}
+						csum_insertion, priv->mode);
+	} else
+		desc = first;
 
 	for (i = 0; i < nfrags; i++) {
 		const skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
 		int len = skb_frag_size(frag);
 
 		entry = (++priv->cur_tx) % txsize;
-		desc = priv->dma_tx + entry;
+		if (priv->extend_desc)
+			desc = (struct dma_desc *)(priv->dma_etx + entry);
+		else
+			desc = priv->dma_tx + entry;
 
 		TX_DBG("\t[entry %d] segment len: %d\n", entry, len);
 		desc->des2 = skb_frag_dma_map(priv->device, frag, 0, len,
 					      DMA_TO_DEVICE);
+		priv->tx_skbuff_dma[entry] = desc->des2;
 		priv->tx_skbuff[entry] = NULL;
-		priv->hw->desc->prepare_tx_desc(desc, 0, len, csum_insertion);
+		priv->hw->desc->prepare_tx_desc(desc, 0, len, csum_insertion,
+						priv->mode);
 		wmb();
 		priv->hw->desc->set_tx_owner(desc);
 		wmb();
@@ -1298,11 +1899,14 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 
 #ifdef STMMAC_XMIT_DEBUG
 	if (netif_msg_pktdata(priv)) {
-		pr_info("stmmac xmit: current=%d, dirty=%d, entry=%d, "
-		       "first=%p, nfrags=%d\n",
-		       (priv->cur_tx % txsize), (priv->dirty_tx % txsize),
-		       entry, first, nfrags);
-		display_ring(priv->dma_tx, txsize);
+		pr_info("%s: curr %d dirty=%d entry=%d, first=%p, nfrags=%d"
+			__func__, (priv->cur_tx % txsize),
+			(priv->dirty_tx % txsize), entry, first, nfrags);
+		if (priv->extend_desc)
+			stmmac_display_ring((void *)priv->dma_etx, txsize, 1);
+		else
+			stmmac_display_ring((void *)priv->dma_tx, txsize, 0);
+
 		pr_info(">>> frame to be transmitted: ");
 		print_pkt(skb->data, skb->len);
 	}
@@ -1314,7 +1918,15 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	dev->stats.tx_bytes += skb->len;
 
-	skb_tx_timestamp(skb);
+	if (unlikely((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
+		     priv->hwts_tx_en)) {
+		/* declare that device is doing timestamping */
+		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+		priv->hw->desc->enable_tx_timestamp(first);
+	}
+
+	if (!priv->hwts_tx_en)
+		skb_tx_timestamp(skb);
 
 	priv->hw->dma->enable_dma_transmission(priv->ioaddr);
 
@@ -1323,14 +1935,26 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 	return NETDEV_TX_OK;
 }
 
+/**
+ * stmmac_rx_refill: refill used skb preallocated buffers
+ * @priv: driver private structure
+ * Description : this is to reallocate the skb for the reception process
+ * that is based on zero-copy.
+ */
 static inline void stmmac_rx_refill(struct stmmac_priv *priv)
 {
 	unsigned int rxsize = priv->dma_rx_size;
 	int bfsize = priv->dma_buf_sz;
-	struct dma_desc *p = priv->dma_rx;
 
 	for (; priv->cur_rx - priv->dirty_rx > 0; priv->dirty_rx++) {
 		unsigned int entry = priv->dirty_rx % rxsize;
+		struct dma_desc *p;
+
+		if (priv->extend_desc)
+			p = (struct dma_desc *)(priv->dma_erx + entry);
+		else
+			p = priv->dma_rx + entry;
+
 		if (likely(priv->rx_skbuff[entry] == NULL)) {
 			struct sk_buff *skb;
 
@@ -1344,80 +1968,116 @@ static inline void stmmac_rx_refill(struct stmmac_priv *priv)
 			    dma_map_single(priv->device, skb->data, bfsize,
 					   DMA_FROM_DEVICE);
 
-			(p + entry)->des2 = priv->rx_skbuff_dma[entry];
+			p->des2 = priv->rx_skbuff_dma[entry];
 
-			if (unlikely(priv->plat->has_gmac))
-				priv->hw->ring->refill_desc3(bfsize, p + entry);
+			priv->hw->ring->refill_desc3(priv, p);
 
 			RX_DBG(KERN_INFO "\trefill entry #%d\n", entry);
 		}
 		wmb();
-		priv->hw->desc->set_rx_owner(p + entry);
+		priv->hw->desc->set_rx_owner(p);
 		wmb();
 	}
 }
 
+/**
+ * stmmac_rx_refill: refill used skb preallocated buffers
+ * @priv: driver private structure
+ * @limit: napi bugget.
+ * Description :  this the function called by the napi poll method.
+ * It gets all the frames inside the ring.
+ */
 static int stmmac_rx(struct stmmac_priv *priv, int limit)
 {
 	unsigned int rxsize = priv->dma_rx_size;
 	unsigned int entry = priv->cur_rx % rxsize;
 	unsigned int next_entry;
 	unsigned int count = 0;
-	struct dma_desc *p = priv->dma_rx + entry;
-	struct dma_desc *p_next;
+	int coe = priv->plat->rx_coe;
 
 #ifdef STMMAC_RX_DEBUG
 	if (netif_msg_hw(priv)) {
 		pr_debug(">>> stmmac_rx: descriptor ring:\n");
-		display_ring(priv->dma_rx, rxsize);
+		if (priv->extend_desc)
+			stmmac_display_ring((void *)priv->dma_erx, rxsize, 1);
+		else
+			stmmac_display_ring((void *)priv->dma_rx, rxsize, 0);
 	}
 #endif
-	while (!priv->hw->desc->get_rx_owner(p)) {
+	while (count < limit) {
 		int status;
+		struct dma_desc *p;
 
-		if (count >= limit)
+		if (priv->extend_desc)
+			p = (struct dma_desc *)(priv->dma_erx + entry);
+		else
+			p = priv->dma_rx + entry;
+
+		if (priv->hw->desc->get_rx_owner(p))
 			break;
 
 		count++;
 
 		next_entry = (++priv->cur_rx) % rxsize;
-		p_next = priv->dma_rx + next_entry;
-		prefetch(p_next);
+		if (priv->extend_desc)
+			prefetch(priv->dma_erx + next_entry);
+		else
+			prefetch(priv->dma_rx + next_entry);
 
 		/* read the status of the incoming frame */
-		status = (priv->hw->desc->rx_status(&priv->dev->stats,
-						    &priv->xstats, p));
-		if (unlikely(status == discard_frame))
+		status = priv->hw->desc->rx_status(&priv->dev->stats,
+						   &priv->xstats, p);
+		if ((priv->extend_desc) && (priv->hw->desc->rx_extended_status))
+			priv->hw->desc->rx_extended_status(&priv->dev->stats,
+							   &priv->xstats,
+							   priv->dma_erx +
+							   entry);
+		if (unlikely(status == discard_frame)) {
 			priv->dev->stats.rx_errors++;
-		else {
+			if (priv->hwts_rx_en && !priv->extend_desc) {
+				/* DESC2 & DESC3 will be overwitten by device
+				 * with timestamp value, hence reinitialize
+				 * them in stmmac_rx_refill() function so that
+				 * device can reuse it.
+				 */
+				priv->rx_skbuff[entry] = NULL;
+				dma_unmap_single(priv->device,
+						 priv->rx_skbuff_dma[entry],
+						 priv->dma_buf_sz,
+						 DMA_FROM_DEVICE);
+			}
+		} else {
 			struct sk_buff *skb;
 			int frame_len;
 
-			frame_len = priv->hw->desc->get_rx_frame_len(p,
-					priv->plat->rx_coe);
+			frame_len = priv->hw->desc->get_rx_frame_len(p, coe);
+
 			/* ACS is set; GMAC core strips PAD/FCS for IEEE 802.3
-			 * Type frames (LLC/LLC-SNAP) */
+			 * Type frames (LLC/LLC-SNAP)
+			 */
 			if (unlikely(status != llc_snap))
 				frame_len -= ETH_FCS_LEN;
 #ifdef STMMAC_RX_DEBUG
 			if (frame_len > ETH_FRAME_LEN)
 				pr_debug("\tRX frame size %d, COE status: %d\n",
-					frame_len, status);
+					 frame_len, status);
 
 			if (netif_msg_hw(priv))
 				pr_debug("\tdesc: %p [entry %d] buff=0x%x\n",
-					p, entry, p->des2);
+					 p, entry, p->des2);
 #endif
 			skb = priv->rx_skbuff[entry];
 			if (unlikely(!skb)) {
 				pr_err("%s: Inconsistent Rx descriptor chain\n",
-					priv->dev->name);
+				       priv->dev->name);
 				priv->dev->stats.rx_dropped++;
 				break;
 			}
 			prefetch(skb->data - NET_IP_ALIGN);
 			priv->rx_skbuff[entry] = NULL;
 
+			stmmac_get_rx_hwtstamp(priv, entry, skb);
+
 			skb_put(skb, frame_len);
 			dma_unmap_single(priv->device,
 					 priv->rx_skbuff_dma[entry],
@@ -1430,7 +2090,7 @@ static int stmmac_rx(struct stmmac_priv *priv, int limit)
 #endif
 			skb->protocol = eth_type_trans(skb, priv->dev);
 
-			if (unlikely(!priv->plat->rx_coe))
+			if (unlikely(!coe))
 				skb_checksum_none_assert(skb);
 			else
 				skb->ip_summed = CHECKSUM_UNNECESSARY;
@@ -1441,7 +2101,6 @@ static int stmmac_rx(struct stmmac_priv *priv, int limit)
 			priv->dev->stats.rx_bytes += frame_len;
 		}
 		entry = next_entry;
-		p = p_next;	/* use prefetched values */
 	}
 
 	stmmac_rx_refill(priv);
@@ -1499,18 +2158,16 @@ static int stmmac_config(struct net_device *dev, struct ifmap *map)
 
 	/* Don't allow changing the I/O address */
 	if (map->base_addr != dev->base_addr) {
-		pr_warning("%s: can't change I/O address\n", dev->name);
+		pr_warn("%s: can't change I/O address\n", dev->name);
 		return -EOPNOTSUPP;
 	}
 
 	/* Don't allow changing the IRQ */
 	if (map->irq != dev->irq) {
-		pr_warning("%s: can't change IRQ number %d\n",
-		       dev->name, dev->irq);
+		pr_warn("%s: not change IRQ number %d\n", dev->name, dev->irq);
 		return -EOPNOTSUPP;
 	}
 
-	/* ignore other fields */
 	return 0;
 }
 
@@ -1570,7 +2227,7 @@ static int stmmac_change_mtu(struct net_device *dev, int new_mtu)
 }
 
 static netdev_features_t stmmac_fix_features(struct net_device *dev,
-	netdev_features_t features)
+					     netdev_features_t features)
 {
 	struct stmmac_priv *priv = netdev_priv(dev);
 
@@ -1584,13 +2241,22 @@ static netdev_features_t stmmac_fix_features(struct net_device *dev,
 	/* Some GMAC devices have a bugged Jumbo frame support that
 	 * needs to have the Tx COE disabled for oversized frames
 	 * (due to limited buffer sizes). In this case we disable
-	 * the TX csum insertionin the TDES and not use SF. */
+	 * the TX csum insertionin the TDES and not use SF.
+	 */
 	if (priv->plat->bugged_jumbo && (dev->mtu > ETH_DATA_LEN))
 		features &= ~NETIF_F_ALL_CSUM;
 
 	return features;
 }
 
+/**
+ *  stmmac_interrupt - main ISR
+ *  @irq: interrupt number.
+ *  @dev_id: to pass the net device pointer.
+ *  Description: this is the main driver interrupt service routine.
+ *  It calls the DMA ISR and also the core ISR to manage PMT, MMC, LPI
+ *  interrupts.
+ */
 static irqreturn_t stmmac_interrupt(int irq, void *dev_id)
 {
 	struct net_device *dev = (struct net_device *)dev_id;
@@ -1604,30 +2270,14 @@ static irqreturn_t stmmac_interrupt(int irq, void *dev_id)
 	/* To handle GMAC own interrupts */
 	if (priv->plat->has_gmac) {
 		int status = priv->hw->mac->host_irq_status((void __iomem *)
-							    dev->base_addr);
+							    dev->base_addr,
+							    &priv->xstats);
 		if (unlikely(status)) {
-			if (status & core_mmc_tx_irq)
-				priv->xstats.mmc_tx_irq_n++;
-			if (status & core_mmc_rx_irq)
-				priv->xstats.mmc_rx_irq_n++;
-			if (status & core_mmc_rx_csum_offload_irq)
-				priv->xstats.mmc_rx_csum_offload_irq_n++;
-			if (status & core_irq_receive_pmt_irq)
-				priv->xstats.irq_receive_pmt_irq_n++;
-
 			/* For LPI we need to save the tx status */
-			if (status & core_irq_tx_path_in_lpi_mode) {
-				priv->xstats.irq_tx_path_in_lpi_mode_n++;
+			if (status & CORE_IRQ_TX_PATH_IN_LPI_MODE)
 				priv->tx_path_in_lpi_mode = true;
-			}
-			if (status & core_irq_tx_path_exit_lpi_mode) {
-				priv->xstats.irq_tx_path_exit_lpi_mode_n++;
+			if (status & CORE_IRQ_TX_PATH_EXIT_LPI_MODE)
 				priv->tx_path_in_lpi_mode = false;
-			}
-			if (status & core_irq_rx_path_in_lpi_mode)
-				priv->xstats.irq_rx_path_in_lpi_mode_n++;
-			if (status & core_irq_rx_path_exit_lpi_mode)
-				priv->xstats.irq_rx_path_exit_lpi_mode_n++;
 		}
 	}
 
@@ -1639,7 +2289,8 @@ static irqreturn_t stmmac_interrupt(int irq, void *dev_id)
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
 /* Polling receive - used by NETCONSOLE and other diagnostic tools
- * to allow network I/O with interrupts disabled. */
+ * to allow network I/O with interrupts disabled.
+ */
 static void stmmac_poll_controller(struct net_device *dev)
 {
 	disable_irq(dev->irq);
@@ -1655,21 +2306,30 @@ static void stmmac_poll_controller(struct net_device *dev)
  *  a proprietary structure used to pass information to the driver.
  *  @cmd: IOCTL command
  *  Description:
- *  Currently there are no special functionality supported in IOCTL, just the
- *  phy_mii_ioctl(...) can be invoked.
+ *  Currently it supports the phy_mii_ioctl(...) and HW time stamping.
  */
 static int stmmac_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 {
 	struct stmmac_priv *priv = netdev_priv(dev);
-	int ret;
+	int ret = -EOPNOTSUPP;
 
 	if (!netif_running(dev))
 		return -EINVAL;
 
-	if (!priv->phydev)
-		return -EINVAL;
-
-	ret = phy_mii_ioctl(priv->phydev, rq, cmd);
+	switch (cmd) {
+	case SIOCGMIIPHY:
+	case SIOCGMIIREG:
+	case SIOCSMIIREG:
+		if (!priv->phydev)
+			return -EINVAL;
+		ret = phy_mii_ioctl(priv->phydev, rq, cmd);
+		break;
+	case SIOCSHWTSTAMP:
+		ret = stmmac_hwtstamp_ioctl(dev, rq);
+		break;
+	default:
+		break;
+	}
 
 	return ret;
 }
@@ -1679,40 +2339,51 @@ static struct dentry *stmmac_fs_dir;
 static struct dentry *stmmac_rings_status;
 static struct dentry *stmmac_dma_cap;
 
-static int stmmac_sysfs_ring_read(struct seq_file *seq, void *v)
+static void sysfs_display_ring(void *head, int size, int extend_desc,
+			       struct seq_file *seq)
 {
-	struct tmp_s {
-		u64 a;
-		unsigned int b;
-		unsigned int c;
-	};
 	int i;
-	struct net_device *dev = seq->private;
-	struct stmmac_priv *priv = netdev_priv(dev);
+	struct dma_extended_desc *ep = (struct dma_extended_desc *)head;
+	struct dma_desc *p = (struct dma_desc *)head;
 
-	seq_printf(seq, "=======================\n");
-	seq_printf(seq, " RX descriptor ring\n");
-	seq_printf(seq, "=======================\n");
-
-	for (i = 0; i < priv->dma_rx_size; i++) {
-		struct tmp_s *x = (struct tmp_s *)(priv->dma_rx + i);
-		seq_printf(seq, "[%d] DES0=0x%x DES1=0x%x BUF1=0x%x BUF2=0x%x",
-			   i, (unsigned int)(x->a),
-			   (unsigned int)((x->a) >> 32), x->b, x->c);
+	for (i = 0; i < size; i++) {
+		u64 x;
+		if (extend_desc) {
+			x = *(u64 *) ep;
+			seq_printf(seq, "%d [0x%x]: 0x%x 0x%x 0x%x 0x%x\n",
+				   i, (unsigned int)virt_to_phys(ep),
+				   (unsigned int)x, (unsigned int)(x >> 32),
+				   ep->basic.des2, ep->basic.des3);
+			ep++;
+		} else {
+			x = *(u64 *) p;
+			seq_printf(seq, "%d [0x%x]: 0x%x 0x%x 0x%x 0x%x\n",
+				   i, (unsigned int)virt_to_phys(ep),
+				   (unsigned int)x, (unsigned int)(x >> 32),
+				   p->des2, p->des3);
+			p++;
+		}
 		seq_printf(seq, "\n");
 	}
+}
 
-	seq_printf(seq, "\n");
-	seq_printf(seq, "=======================\n");
-	seq_printf(seq, "  TX descriptor ring\n");
-	seq_printf(seq, "=======================\n");
+static int stmmac_sysfs_ring_read(struct seq_file *seq, void *v)
+{
+	struct net_device *dev = seq->private;
+	struct stmmac_priv *priv = netdev_priv(dev);
+	unsigned int txsize = priv->dma_tx_size;
+	unsigned int rxsize = priv->dma_rx_size;
 
-	for (i = 0; i < priv->dma_tx_size; i++) {
-		struct tmp_s *x = (struct tmp_s *)(priv->dma_tx + i);
-		seq_printf(seq, "[%d] DES0=0x%x DES1=0x%x BUF1=0x%x BUF2=0x%x",
-			   i, (unsigned int)(x->a),
-			   (unsigned int)((x->a) >> 32), x->b, x->c);
-		seq_printf(seq, "\n");
+	if (priv->extend_desc) {
+		seq_printf(seq, "Extended RX descriptor ring:\n");
+		sysfs_display_ring((void *)priv->dma_erx, rxsize, 1, seq);
+		seq_printf(seq, "Extended TX descriptor ring:\n");
+		sysfs_display_ring((void *)priv->dma_etx, txsize, 1, seq);
+	} else {
+		seq_printf(seq, "RX descriptor ring:\n");
+		sysfs_display_ring((void *)priv->dma_rx, rxsize, 0, seq);
+		seq_printf(seq, "TX descriptor ring:\n");
+		sysfs_display_ring((void *)priv->dma_tx, txsize, 0, seq);
 	}
 
 	return 0;
@@ -1817,8 +2488,8 @@ static int stmmac_init_fs(struct net_device *dev)
 
 	/* Entry to report DMA RX/TX rings */
 	stmmac_rings_status = debugfs_create_file("descriptors_status",
-					   S_IRUGO, stmmac_fs_dir, dev,
-					   &stmmac_rings_status_fops);
+						  S_IRUGO, stmmac_fs_dir, dev,
+						  &stmmac_rings_status_fops);
 
 	if (!stmmac_rings_status || IS_ERR(stmmac_rings_status)) {
 		pr_info("ERROR creating stmmac ring debugfs file\n");
@@ -1868,7 +2539,7 @@ static const struct net_device_ops stmmac_netdev_ops = {
 
 /**
  *  stmmac_hw_init - Init the MAC device
- *  @priv : pointer to the private device structure.
+ *  @priv: driver private structure
  *  Description: this function detects which MAC device
  *  (GMAC/MAC10-100) has to attached, checks the HW capability
  *  (if supported) and sets the driver's features (for example
@@ -1877,7 +2548,7 @@ static const struct net_device_ops stmmac_netdev_ops = {
  */
 static int stmmac_hw_init(struct stmmac_priv *priv)
 {
-	int ret = 0;
+	int ret;
 	struct mac_device_info *mac;
 
 	/* Identify the MAC HW device */
@@ -1892,12 +2563,23 @@ static int stmmac_hw_init(struct stmmac_priv *priv)
 
 	priv->hw = mac;
 
-	/* To use the chained or ring mode */
-	priv->hw->ring = &ring_mode_ops;
-
 	/* Get and dump the chip ID */
 	priv->synopsys_id = stmmac_get_synopsys_id(priv);
 
+	/* To use alternate (extended) or normal descriptor structures */
+	stmmac_selec_desc_mode(priv);
+
+	/* To use the chained or ring mode */
+	if (chain_mode) {
+		priv->hw->chain = &chain_mode_ops;
+		pr_info(" Chain mode enabled\n");
+		priv->mode = STMMAC_CHAIN_MODE;
+	} else {
+		priv->hw->ring = &ring_mode_ops;
+		pr_info(" Ring mode enabled\n");
+		priv->mode = STMMAC_RING_MODE;
+	}
+
 	/* Get the HW capability (new GMAC newer than 3.50a) */
 	priv->hw_cap_support = stmmac_get_hw_features(priv);
 	if (priv->hw_cap_support) {
@@ -1921,14 +2603,9 @@ static int stmmac_hw_init(struct stmmac_priv *priv)
 	} else
 		pr_info(" No HW DMA feature register supported");
 
-	/* Select the enhnaced/normal descriptor structures */
-	stmmac_selec_desc_mode(priv);
-
-	/* Enable the IPC (Checksum Offload) and check if the feature has been
-	 * enabled during the core configuration. */
 	ret = priv->hw->mac->rx_ipc(priv->ioaddr);
 	if (!ret) {
-		pr_warning(" RX IPC Checksum Offload not configured.\n");
+		pr_warn(" RX IPC Checksum Offload not configured.\n");
 		priv->plat->rx_coe = STMMAC_RX_COE_NONE;
 	}
 
@@ -1943,7 +2620,7 @@ static int stmmac_hw_init(struct stmmac_priv *priv)
 		device_set_wakeup_capable(priv->device, 1);
 	}
 
-	return ret;
+	return 0;
 }
 
 /**
@@ -1984,12 +2661,15 @@ struct stmmac_priv *stmmac_dvr_probe(struct device *device,
 	stmmac_verify_args();
 
 	/* Override with kernel parameters if supplied XXX CRS XXX
-	 * this needs to have multiple instances */
+	 * this needs to have multiple instances
+	 */
 	if ((phyaddr >= 0) && (phyaddr <= 31))
 		priv->plat->phy_addr = phyaddr;
 
 	/* Init MAC and get the capabilities */
-	stmmac_hw_init(priv);
+	ret = stmmac_hw_init(priv);
+	if (ret)
+		goto error_free_netdev;
 
 	ndev->netdev_ops = &stmmac_netdev_ops;
 
@@ -1999,7 +2679,7 @@ struct stmmac_priv *stmmac_dvr_probe(struct device *device,
 	ndev->watchdog_timeo = msecs_to_jiffies(watchdog);
 #ifdef STMMAC_VLAN_TAG_USED
 	/* Both mac100 and gmac support receive VLAN tag detection */
-	ndev->features |= NETIF_F_HW_VLAN_RX;
+	ndev->features |= NETIF_F_HW_VLAN_CTAG_RX;
 #endif
 	priv->msg_enable = netif_msg_init(debug, default_msg_level);
 
@@ -2029,7 +2709,7 @@ struct stmmac_priv *stmmac_dvr_probe(struct device *device,
 
 	priv->stmmac_clk = clk_get(priv->device, STMMAC_RESOURCE_NAME);
 	if (IS_ERR(priv->stmmac_clk)) {
-		pr_warning("%s: warning: cannot get CSR clock\n", __func__);
+		pr_warn("%s: warning: cannot get CSR clock\n", __func__);
 		goto error_clk_get;
 	}
 
@@ -2044,12 +2724,17 @@ struct stmmac_priv *stmmac_dvr_probe(struct device *device,
 	else
 		priv->clk_csr = priv->plat->clk_csr;
 
-	/* MDIO bus Registration */
-	ret = stmmac_mdio_register(ndev);
-	if (ret < 0) {
-		pr_debug("%s: MDIO bus (id: %d) registration failed",
-			 __func__, priv->plat->bus_id);
-		goto error_mdio_register;
+	stmmac_check_pcs_mode(priv);
+
+	if (priv->pcs != STMMAC_PCS_RGMII && priv->pcs != STMMAC_PCS_TBI &&
+	    priv->pcs != STMMAC_PCS_RTBI) {
+		/* MDIO bus Registration */
+		ret = stmmac_mdio_register(ndev);
+		if (ret < 0) {
+			pr_debug("%s: MDIO bus (id: %d) registration failed",
+				 __func__, priv->plat->bus_id);
+			goto error_mdio_register;
+		}
 	}
 
 	return priv;
@@ -2060,6 +2745,7 @@ error_clk_get:
 	unregister_netdev(ndev);
 error_netdev_register:
 	netif_napi_del(&priv->napi);
+error_free_netdev:
 	free_netdev(ndev);
 
 	return NULL;
@@ -2081,7 +2767,9 @@ int stmmac_dvr_remove(struct net_device *ndev)
 	priv->hw->dma->stop_tx(priv->ioaddr);
 
 	stmmac_set_mac(priv->ioaddr, false);
-	stmmac_mdio_unregister(ndev);
+	if (priv->pcs != STMMAC_PCS_RGMII && priv->pcs != STMMAC_PCS_TBI &&
+	    priv->pcs != STMMAC_PCS_RTBI)
+		stmmac_mdio_unregister(ndev);
 	netif_carrier_off(ndev);
 	unregister_netdev(ndev);
 	free_netdev(ndev);
@@ -2093,7 +2781,6 @@ int stmmac_dvr_remove(struct net_device *ndev)
 int stmmac_suspend(struct net_device *ndev)
 {
 	struct stmmac_priv *priv = netdev_priv(ndev);
-	int dis_ic = 0;
 	unsigned long flags;
 
 	if (!ndev || !netif_running(ndev))
@@ -2107,18 +2794,13 @@ int stmmac_suspend(struct net_device *ndev)
 	netif_device_detach(ndev);
 	netif_stop_queue(ndev);
 
-	if (priv->use_riwt)
-		dis_ic = 1;
-
 	napi_disable(&priv->napi);
 
 	/* Stop TX/RX DMA */
 	priv->hw->dma->stop_tx(priv->ioaddr);
 	priv->hw->dma->stop_rx(priv->ioaddr);
-	/* Clear the Rx/Tx descriptors */
-	priv->hw->desc->init_rx_desc(priv->dma_rx, priv->dma_rx_size,
-				     dis_ic);
-	priv->hw->desc->init_tx_desc(priv->dma_tx, priv->dma_tx_size);
+
+	stmmac_clear_descriptors(priv);
 
 	/* Enable Power down mode by programming the PMT regs */
 	if (device_may_wakeup(priv->device))
@@ -2146,7 +2828,8 @@ int stmmac_resume(struct net_device *ndev)
 	 * automatically as soon as a magic packet or a Wake-up frame
 	 * is received. Anyway, it's better to manually clear
 	 * this bit because it can generate problems while resuming
-	 * from another devices (e.g. serial console). */
+	 * from another devices (e.g. serial console).
+	 */
 	if (device_may_wakeup(priv->device))
 		priv->hw->mac->pmt(priv->ioaddr, 0);
 	else
@@ -2257,6 +2940,9 @@ static int __init stmmac_cmdline_opt(char *str)
 		} else if (!strncmp(opt, "eee_timer:", 10)) {
 			if (kstrtoint(opt + 10, 0, &eee_timer))
 				goto err;
+		} else if (!strncmp(opt, "chain_mode:", 11)) {
+			if (kstrtoint(opt + 11, 0, &chain_mode))
+				goto err;
 		}
 	}
 	return 0;
@@ -2267,7 +2953,7 @@ err:
 }
 
 __setup("stmmaceth=", stmmac_cmdline_opt);
-#endif
+#endif /* MODULE */
 
 MODULE_DESCRIPTION("STMMAC 10/100/1000 Ethernet device driver");
 MODULE_AUTHOR("Giuseppe Cavallaro <peppe.cavallaro@st.com>");
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c
index 0b9829fe3eea..cc15039eaa47 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c
@@ -177,7 +177,7 @@ int stmmac_mdio_register(struct net_device *ndev)
 	new_bus->write = &stmmac_mdio_write;
 	new_bus->reset = &stmmac_mdio_reset;
 	snprintf(new_bus->id, MII_BUS_ID_SIZE, "%s-%x",
-		new_bus->name, priv->plat->bus_id);
+		 new_bus->name, priv->plat->bus_id);
 	new_bus->priv = ndev;
 	new_bus->irq = irqlist;
 	new_bus->phy_mask = mdio_bus_data->phy_mask;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c
index 19b3a2567a46..023b7c29cb2f 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c
@@ -88,7 +88,7 @@ static int stmmac_pci_probe(struct pci_dev *pdev,
 			continue;
 		addr = pci_iomap(pdev, i, 0);
 		if (addr == NULL) {
-			pr_err("%s: ERROR: cannot map register memory, aborting",
+			pr_err("%s: ERROR: cannot map register memory aborting",
 			       __func__);
 			ret = -EIO;
 			goto err_out_map_failed;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
index b43d68b40e50..1d3780f55ba2 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
@@ -88,11 +88,9 @@ static int stmmac_pltfr_probe(struct platform_device *pdev)
 	if (!res)
 		return -ENODEV;
 
-	addr = devm_request_and_ioremap(dev, res);
-	if (!addr) {
-		pr_err("%s: ERROR: memory mapping failed", __func__);
-		return -ENOMEM;
-	}
+	addr = devm_ioremap_resource(dev, res);
+	if (IS_ERR(addr))
+		return PTR_ERR(addr);
 
 	if (pdev->dev.of_node) {
 		plat_dat = devm_kzalloc(&pdev->dev,
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c
new file mode 100644
index 000000000000..b8b0eeed0f92
--- /dev/null
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c
@@ -0,0 +1,211 @@
+/*******************************************************************************
+  PTP 1588 clock using the STMMAC.
+
+  Copyright (C) 2013  Vayavya Labs Pvt Ltd
+
+  This program is free software; you can redistribute it and/or modify it
+  under the terms and conditions of the GNU General Public License,
+  version 2, as published by the Free Software Foundation.
+
+  This program is distributed in the hope it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Author: Rayagond Kokatanur <rayagond@vayavyalabs.com>
+*******************************************************************************/
+#include "stmmac.h"
+#include "stmmac_ptp.h"
+
+/**
+ * stmmac_adjust_freq
+ *
+ * @ptp: pointer to ptp_clock_info structure
+ * @ppb: desired period change in parts ber billion
+ *
+ * Description: this function will adjust the frequency of hardware clock.
+ */
+static int stmmac_adjust_freq(struct ptp_clock_info *ptp, s32 ppb)
+{
+	struct stmmac_priv *priv =
+	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
+	unsigned long flags;
+	u32 diff, addend;
+	int neg_adj = 0;
+	u64 adj;
+
+	if (ppb < 0) {
+		neg_adj = 1;
+		ppb = -ppb;
+	}
+
+	addend = priv->default_addend;
+	adj = addend;
+	adj *= ppb;
+	diff = div_u64(adj, 1000000000ULL);
+	addend = neg_adj ? (addend - diff) : (addend + diff);
+
+	spin_lock_irqsave(&priv->ptp_lock, flags);
+
+	priv->hw->ptp->config_addend(priv->ioaddr, addend);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+/**
+ * stmmac_adjust_time
+ *
+ * @ptp: pointer to ptp_clock_info structure
+ * @delta: desired change in nanoseconds
+ *
+ * Description: this function will shift/adjust the hardware clock time.
+ */
+static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct stmmac_priv *priv =
+	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
+	unsigned long flags;
+	u32 sec, nsec;
+	u32 quotient, reminder;
+	int neg_adj = 0;
+
+	if (delta < 0) {
+		neg_adj = 1;
+		delta = -delta;
+	}
+
+	quotient = div_u64_rem(delta, 1000000000ULL, &reminder);
+	sec = quotient;
+	nsec = reminder;
+
+	spin_lock_irqsave(&priv->ptp_lock, flags);
+
+	priv->hw->ptp->adjust_systime(priv->ioaddr, sec, nsec, neg_adj);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+/**
+ * stmmac_get_time
+ *
+ * @ptp: pointer to ptp_clock_info structure
+ * @ts: pointer to hold time/result
+ *
+ * Description: this function will read the current time from the
+ * hardware clock and store it in @ts.
+ */
+static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec *ts)
+{
+	struct stmmac_priv *priv =
+	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
+	unsigned long flags;
+	u64 ns;
+	u32 reminder;
+
+	spin_lock_irqsave(&priv->ptp_lock, flags);
+
+	ns = priv->hw->ptp->get_systime(priv->ioaddr);
+
+	spin_unlock_irqrestore(&priv->ptp_lock, flags);
+
+	ts->tv_sec = div_u64_rem(ns, 1000000000ULL, &reminder);
+	ts->tv_nsec = reminder;
+
+	return 0;
+}
+
+/**
+ * stmmac_set_time
+ *
+ * @ptp: pointer to ptp_clock_info structure
+ * @ts: time value to set
+ *
+ * Description: this function will set the current time on the
+ * hardware clock.
+ */
+static int stmmac_set_time(struct ptp_clock_info *ptp,
+			   const struct timespec *ts)
+{
+	struct stmmac_priv *priv =
+	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->ptp_lock, flags);
+
+	priv->hw->ptp->init_systime(priv->ioaddr, ts->tv_sec, ts->tv_nsec);
+
+	spin_unlock_irqrestore(&priv->ptp_lock, flags);
+
+	return 0;
+}
+
+static int stmmac_enable(struct ptp_clock_info *ptp,
+			 struct ptp_clock_request *rq, int on)
+{
+	return -EOPNOTSUPP;
+}
+
+/* structure describing a PTP hardware clock */
+static struct ptp_clock_info stmmac_ptp_clock_ops = {
+	.owner = THIS_MODULE,
+	.name = "stmmac_ptp_clock",
+	.max_adj = 62500000,
+	.n_alarm = 0,
+	.n_ext_ts = 0,
+	.n_per_out = 0,
+	.pps = 0,
+	.adjfreq = stmmac_adjust_freq,
+	.adjtime = stmmac_adjust_time,
+	.gettime = stmmac_get_time,
+	.settime = stmmac_set_time,
+	.enable = stmmac_enable,
+};
+
+/**
+ * stmmac_ptp_register
+ * @priv: driver private structure
+ * Description: this function will register the ptp clock driver
+ * to kernel. It also does some house keeping work.
+ */
+int stmmac_ptp_register(struct stmmac_priv *priv)
+{
+	spin_lock_init(&priv->ptp_lock);
+	priv->ptp_clock_ops = stmmac_ptp_clock_ops;
+
+	priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
+					     priv->device);
+	if (IS_ERR(priv->ptp_clock)) {
+		priv->ptp_clock = NULL;
+		pr_err("ptp_clock_register() failed on %s\n", priv->dev->name);
+	} else
+		pr_debug("Added PTP HW clock successfully on %s\n",
+			 priv->dev->name);
+
+	return 0;
+}
+
+/**
+ * stmmac_ptp_unregister
+ * @priv: driver private structure
+ * Description: this function will remove/unregister the ptp clock driver
+ * from the kernel.
+ */
+void stmmac_ptp_unregister(struct stmmac_priv *priv)
+{
+	if (priv->ptp_clock) {
+		ptp_clock_unregister(priv->ptp_clock);
+		pr_debug("Removed PTP HW clock successfully on %s\n",
+			 priv->dev->name);
+	}
+}
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h
new file mode 100644
index 000000000000..3dbc047622fa
--- /dev/null
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h
@@ -0,0 +1,74 @@
+/******************************************************************************
+  PTP Header file
+
+  Copyright (C) 2013  Vayavya Labs Pvt Ltd
+
+  This program is free software; you can redistribute it and/or modify it
+  under the terms and conditions of the GNU General Public License,
+  version 2, as published by the Free Software Foundation.
+
+  This program is distributed in the hope it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Author: Rayagond Kokatanur <rayagond@vayavyalabs.com>
+******************************************************************************/
+
+#ifndef __STMMAC_PTP_H__
+#define __STMMAC_PTP_H__
+
+#define STMMAC_SYSCLOCK 62500000
+
+/* IEEE 1588 PTP register offsets */
+#define PTP_TCR		0x0700	/* Timestamp Control Reg */
+#define PTP_SSIR	0x0704	/* Sub-Second Increment Reg */
+#define PTP_STSR	0x0708	/* System Time – Seconds Regr */
+#define PTP_STNSR	0x070C	/* System Time – Nanoseconds Reg */
+#define PTP_STSUR	0x0710	/* System Time – Seconds Update Reg */
+#define PTP_STNSUR	0x0714	/* System Time – Nanoseconds Update Reg */
+#define PTP_TAR		0x0718	/* Timestamp Addend Reg */
+#define PTP_TTSR	0x071C	/* Target Time Seconds Reg */
+#define PTP_TTNSR	0x0720	/* Target Time Nanoseconds Reg */
+#define	PTP_STHWSR	0x0724	/* System Time - Higher Word Seconds Reg */
+#define PTP_TSR		0x0728	/* Timestamp Status */
+
+#define PTP_STNSUR_ADDSUB_SHIFT 31
+
+/* PTP TCR defines */
+#define PTP_TCR_TSENA		0x00000001 /* Timestamp Enable */
+#define PTP_TCR_TSCFUPDT	0x00000002 /* Timestamp Fine/Coarse Update */
+#define PTP_TCR_TSINIT		0x00000004 /* Timestamp Initialize */
+#define PTP_TCR_TSUPDT		0x00000008 /* Timestamp Update */
+/* Timestamp Interrupt Trigger Enable */
+#define PTP_TCR_TSTRIG		0x00000010
+#define PTP_TCR_TSADDREG	0x00000020 /* Addend Reg Update */
+#define PTP_TCR_TSENALL		0x00000100 /* Enable Timestamp for All Frames */
+/* Timestamp Digital or Binary Rollover Control */
+#define PTP_TCR_TSCTRLSSR	0x00000200
+
+/* Enable PTP packet Processing for Version 2 Format */
+#define PTP_TCR_TSVER2ENA	0x00000400
+/* Enable Processing of PTP over Ethernet Frames */
+#define PTP_TCR_TSIPENA		0x00000800
+/* Enable Processing of PTP Frames Sent over IPv6-UDP */
+#define PTP_TCR_TSIPV6ENA	0x00001000
+/* Enable Processing of PTP Frames Sent over IPv4-UDP */
+#define PTP_TCR_TSIPV4ENA	0x00002000
+/* Enable Timestamp Snapshot for Event Messages */
+#define PTP_TCR_TSEVNTENA	0x00004000
+/* Enable Snapshot for Messages Relevant to Master */
+#define PTP_TCR_TSMSTRENA	0x00008000
+/* Select PTP packets for Taking Snapshots */
+#define PTP_TCR_SNAPTYPSEL_1	0x00010000
+/* Enable MAC address for PTP Frame Filtering */
+#define PTP_TCR_TSENMACADDR	0x00040000
+
+#endif /* __STMMAC_PTP_H__ */
diff --git a/drivers/net/ethernet/sun/niu.c b/drivers/net/ethernet/sun/niu.c
index e4c1c88e4c2a..95cff98d8a34 100644
--- a/drivers/net/ethernet/sun/niu.c
+++ b/drivers/net/ethernet/sun/niu.c
@@ -6618,7 +6618,7 @@ static u64 niu_compute_tx_flags(struct sk_buff *skb, struct ethhdr *ehdr,
 	       (len << TXHDR_LEN_SHIFT) |
 	       ((l3off / 2) << TXHDR_L3START_SHIFT) |
 	       (ihl << TXHDR_IHL_SHIFT) |
-	       ((eth_proto_inner < 1536) ? TXHDR_LLC : 0) |
+	       ((eth_proto_inner < ETH_P_802_3_MIN) ? TXHDR_LLC : 0) |
 	       ((eth_proto == ETH_P_8021Q) ? TXHDR_VLAN : 0) |
 	       (ipv6 ? TXHDR_IP_VER : 0) |
 	       csum_bits);
diff --git a/drivers/net/ethernet/sun/sunbmac.c b/drivers/net/ethernet/sun/sunbmac.c
index 5fafca065305..054975939a18 100644
--- a/drivers/net/ethernet/sun/sunbmac.c
+++ b/drivers/net/ethernet/sun/sunbmac.c
@@ -1169,10 +1169,8 @@ static int bigmac_ether_init(struct platform_device *op,
 	bp->bmac_block = dma_alloc_coherent(&bp->bigmac_op->dev,
 					    PAGE_SIZE,
 					    &bp->bblock_dvma, GFP_ATOMIC);
-	if (bp->bmac_block == NULL || bp->bblock_dvma == 0) {
-		printk(KERN_ERR "BIGMAC: Cannot allocate consistent DMA.\n");
+	if (bp->bmac_block == NULL || bp->bblock_dvma == 0)
 		goto fail_and_cleanup;
-	}
 
 	/* Get the board revision of this BigMAC. */
 	bp->board_rev = of_getintprop_default(bp->bigmac_op->dev.of_node,
diff --git a/drivers/net/ethernet/sun/sunhme.c b/drivers/net/ethernet/sun/sunhme.c
index a1bff49a8155..436fa9d5a071 100644
--- a/drivers/net/ethernet/sun/sunhme.c
+++ b/drivers/net/ethernet/sun/sunhme.c
@@ -2752,10 +2752,8 @@ static int happy_meal_sbus_probe_one(struct platform_device *op, int is_qfe)
 					     &hp->hblock_dvma,
 					     GFP_ATOMIC);
 	err = -ENOMEM;
-	if (!hp->happy_block) {
-		printk(KERN_ERR "happymeal: Cannot allocate descriptors.\n");
+	if (!hp->happy_block)
 		goto err_out_iounmap;
-	}
 
 	/* Force check of the link first time we are brought up. */
 	hp->linkcheck = 0;
@@ -3068,14 +3066,11 @@ static int happy_meal_pci_probe(struct pci_dev *pdev,
 	hp->happy_bursts = DMA_BURSTBITS;
 #endif
 
-	hp->happy_block = (struct hmeal_init_block *)
-		dma_alloc_coherent(&pdev->dev, PAGE_SIZE, &hp->hblock_dvma, GFP_KERNEL);
-
+	hp->happy_block = dma_alloc_coherent(&pdev->dev, PAGE_SIZE,
+					     &hp->hblock_dvma, GFP_KERNEL);
 	err = -ENODEV;
-	if (!hp->happy_block) {
-		printk(KERN_ERR "happymeal(PCI): Cannot get hme init block.\n");
+	if (!hp->happy_block)
 		goto err_out_iounmap;
-	}
 
 	hp->linkcheck = 0;
 	hp->timer_state = asleep;
diff --git a/drivers/net/ethernet/sun/sunqe.c b/drivers/net/ethernet/sun/sunqe.c
index 49bf3e2eb652..8182591bc187 100644
--- a/drivers/net/ethernet/sun/sunqe.c
+++ b/drivers/net/ethernet/sun/sunqe.c
@@ -414,7 +414,7 @@ static void qe_rx(struct sunqe *qep)
 	struct qe_rxd *this;
 	struct sunqe_buffers *qbufs = qep->buffers;
 	__u32 qbufs_dvma = qep->buffers_dvma;
-	int elem = qep->rx_new, drops = 0;
+	int elem = qep->rx_new;
 	u32 flags;
 
 	this = &rxbase[elem];
@@ -436,7 +436,6 @@ static void qe_rx(struct sunqe *qep)
 		} else {
 			skb = netdev_alloc_skb(dev, len + 2);
 			if (skb == NULL) {
-				drops++;
 				dev->stats.rx_dropped++;
 			} else {
 				skb_reserve(skb, 2);
@@ -456,8 +455,6 @@ static void qe_rx(struct sunqe *qep)
 		this = &rxbase[elem];
 	}
 	qep->rx_new = elem;
-	if (drops)
-		printk(KERN_NOTICE "%s: Memory squeeze, deferring packet.\n", qep->dev->name);
 }
 
 static void qe_tx_reclaim(struct sunqe *qep);
diff --git a/drivers/net/ethernet/tehuti/tehuti.c b/drivers/net/ethernet/tehuti/tehuti.c
index e15cc71b826d..571452e786d5 100644
--- a/drivers/net/ethernet/tehuti/tehuti.c
+++ b/drivers/net/ethernet/tehuti/tehuti.c
@@ -733,7 +733,7 @@ static void __bdx_vlan_rx_vid(struct net_device *ndev, uint16_t vid, int enable)
  * @ndev: network device
  * @vid:  VLAN vid to add
  */
-static int bdx_vlan_rx_add_vid(struct net_device *ndev, uint16_t vid)
+static int bdx_vlan_rx_add_vid(struct net_device *ndev, __be16 proto, u16 vid)
 {
 	__bdx_vlan_rx_vid(ndev, vid, 1);
 	return 0;
@@ -744,7 +744,7 @@ static int bdx_vlan_rx_add_vid(struct net_device *ndev, uint16_t vid)
  * @ndev: network device
  * @vid:  VLAN vid to kill
  */
-static int bdx_vlan_rx_kill_vid(struct net_device *ndev, unsigned short vid)
+static int bdx_vlan_rx_kill_vid(struct net_device *ndev, __be16 proto, u16 vid)
 {
 	__bdx_vlan_rx_vid(ndev, vid, 0);
 	return 0;
@@ -1102,10 +1102,9 @@ static void bdx_rx_alloc_skbs(struct bdx_priv *priv, struct rxf_fifo *f)
 	dno = bdx_rxdb_available(db) - 1;
 	while (dno > 0) {
 		skb = netdev_alloc_skb(priv->ndev, f->m.pktsz + NET_IP_ALIGN);
-		if (!skb) {
-			pr_err("NO MEM: netdev_alloc_skb failed\n");
+		if (!skb)
 			break;
-		}
+
 		skb_reserve(skb, NET_IP_ALIGN);
 
 		idx = bdx_rxdb_alloc_elem(db);
@@ -1149,7 +1148,7 @@ NETIF_RX_MUX(struct bdx_priv *priv, u32 rxd_val1, u16 rxd_vlan,
 		    priv->ndev->name,
 		    GET_RXD_VLAN_ID(rxd_vlan),
 		    GET_RXD_VTAG(rxd_val1));
-		__vlan_hwaccel_put_tag(skb, GET_RXD_VLAN_TCI(rxd_vlan));
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), GET_RXD_VLAN_TCI(rxd_vlan));
 	}
 	netif_receive_skb(skb);
 }
@@ -2018,12 +2017,12 @@ bdx_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		 * so we can have them same for all ports of the board */
 		ndev->if_port = port;
 		ndev->features = NETIF_F_IP_CSUM | NETIF_F_SG | NETIF_F_TSO
-		    | NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX |
-		    NETIF_F_HW_VLAN_FILTER | NETIF_F_RXCSUM
+		    | NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX |
+		    NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_RXCSUM
 		    /*| NETIF_F_FRAGLIST */
 		    ;
 		ndev->hw_features = NETIF_F_IP_CSUM | NETIF_F_SG |
-			NETIF_F_TSO | NETIF_F_HW_VLAN_TX;
+			NETIF_F_TSO | NETIF_F_HW_VLAN_CTAG_TX;
 
 		if (pci_using_dac)
 			ndev->features |= NETIF_F_HIGHDMA;
diff --git a/drivers/net/ethernet/ti/cpsw.c b/drivers/net/ethernet/ti/cpsw.c
index 4781d3d8e182..21a5b291b4b3 100644
--- a/drivers/net/ethernet/ti/cpsw.c
+++ b/drivers/net/ethernet/ti/cpsw.c
@@ -126,6 +126,13 @@ do {								\
 #define CPSW_FIFO_DUAL_MAC_MODE		(1 << 15)
 #define CPSW_FIFO_RATE_LIMIT_MODE	(2 << 15)
 
+#define CPSW_INTPACEEN		(0x3f << 16)
+#define CPSW_INTPRESCALE_MASK	(0x7FF << 0)
+#define CPSW_CMINTMAX_CNT	63
+#define CPSW_CMINTMIN_CNT	2
+#define CPSW_CMINTMAX_INTVL	(1000 / CPSW_CMINTMIN_CNT)
+#define CPSW_CMINTMIN_INTVL	((1000 / CPSW_CMINTMAX_CNT) + 1)
+
 #define cpsw_enable_irq(priv)	\
 	do {			\
 		u32 i;		\
@@ -139,6 +146,10 @@ do {								\
 			disable_irq_nosync(priv->irqs_table[i]); \
 	} while (0);
 
+#define cpsw_slave_index(priv)				\
+		((priv->data.dual_emac) ? priv->emac_port :	\
+		priv->data.active_slave)
+
 static int debug_level;
 module_param(debug_level, int, 0);
 MODULE_PARM_DESC(debug_level, "cpsw debug level (NETIF_MSG bits)");
@@ -160,6 +171,15 @@ struct cpsw_wr_regs {
 	u32	rx_en;
 	u32	tx_en;
 	u32	misc_en;
+	u32	mem_allign1[8];
+	u32	rx_thresh_stat;
+	u32	rx_stat;
+	u32	tx_stat;
+	u32	misc_stat;
+	u32	mem_allign2[8];
+	u32	rx_imax;
+	u32	tx_imax;
+
 };
 
 struct cpsw_ss_regs {
@@ -314,6 +334,8 @@ struct cpsw_priv {
 	struct cpsw_host_regs __iomem	*host_port_regs;
 	u32				msg_enable;
 	u32				version;
+	u32				coal_intvl;
+	u32				bus_freq_mhz;
 	struct net_device_stats		stats;
 	int				rx_packet_max;
 	int				host_port;
@@ -326,6 +348,7 @@ struct cpsw_priv {
 	/* snapshot of IRQ numbers */
 	u32 irqs_table[4];
 	u32 num_irqs;
+	bool irq_enabled;
 	struct cpts *cpts;
 	u32 emac_port;
 };
@@ -333,12 +356,15 @@ struct cpsw_priv {
 #define napi_to_priv(napi)	container_of(napi, struct cpsw_priv, napi)
 #define for_each_slave(priv, func, arg...)				\
 	do {								\
-		int idx;						\
+		struct cpsw_slave *slave;				\
+		int n;							\
 		if (priv->data.dual_emac)				\
 			(func)((priv)->slaves + priv->emac_port, ##arg);\
 		else							\
-			for (idx = 0; idx < (priv)->data.slaves; idx++)	\
-				(func)((priv)->slaves + idx, ##arg);	\
+			for (n = (priv)->data.slaves,			\
+					slave = (priv)->slaves;		\
+					n; n--)				\
+				(func)(slave++, ##arg);			\
 	} while (0)
 #define cpsw_get_slave_ndev(priv, __slave_no__)				\
 	(priv->slaves[__slave_no__].ndev)
@@ -446,62 +472,69 @@ void cpsw_tx_handler(void *token, int len, int status)
 void cpsw_rx_handler(void *token, int len, int status)
 {
 	struct sk_buff		*skb = token;
+	struct sk_buff		*new_skb;
 	struct net_device	*ndev = skb->dev;
 	struct cpsw_priv	*priv = netdev_priv(ndev);
 	int			ret = 0;
 
 	cpsw_dual_emac_src_port_detect(status, priv, ndev, skb);
 
-	/* free and bail if we are shutting down */
-	if (unlikely(!netif_running(ndev)) ||
-			unlikely(!netif_carrier_ok(ndev))) {
+	if (unlikely(status < 0)) {
+		/* the interface is going down, skbs are purged */
 		dev_kfree_skb_any(skb);
 		return;
 	}
-	if (likely(status >= 0)) {
+
+	new_skb = netdev_alloc_skb_ip_align(ndev, priv->rx_packet_max);
+	if (new_skb) {
 		skb_put(skb, len);
 		cpts_rx_timestamp(priv->cpts, skb);
 		skb->protocol = eth_type_trans(skb, ndev);
 		netif_receive_skb(skb);
 		priv->stats.rx_bytes += len;
 		priv->stats.rx_packets++;
-		skb = NULL;
-	}
-
-	if (unlikely(!netif_running(ndev))) {
-		if (skb)
-			dev_kfree_skb_any(skb);
-		return;
+	} else {
+		priv->stats.rx_dropped++;
+		new_skb = skb;
 	}
 
-	if (likely(!skb)) {
-		skb = netdev_alloc_skb_ip_align(ndev, priv->rx_packet_max);
-		if (WARN_ON(!skb))
-			return;
-
-		ret = cpdma_chan_submit(priv->rxch, skb, skb->data,
-					skb_tailroom(skb), 0, GFP_KERNEL);
-	}
-	WARN_ON(ret < 0);
+	ret = cpdma_chan_submit(priv->rxch, new_skb, new_skb->data,
+			skb_tailroom(new_skb), 0);
+	if (WARN_ON(ret < 0))
+		dev_kfree_skb_any(new_skb);
 }
 
 static irqreturn_t cpsw_interrupt(int irq, void *dev_id)
 {
 	struct cpsw_priv *priv = dev_id;
+	u32 rx, tx, rx_thresh;
 
-	if (likely(netif_running(priv->ndev))) {
-		cpsw_intr_disable(priv);
+	rx_thresh = __raw_readl(&priv->wr_regs->rx_thresh_stat);
+	rx = __raw_readl(&priv->wr_regs->rx_stat);
+	tx = __raw_readl(&priv->wr_regs->tx_stat);
+	if (!rx_thresh && !rx && !tx)
+		return IRQ_NONE;
+
+	cpsw_intr_disable(priv);
+	if (priv->irq_enabled == true) {
 		cpsw_disable_irq(priv);
+		priv->irq_enabled = false;
+	}
+
+	if (netif_running(priv->ndev)) {
 		napi_schedule(&priv->napi);
-	} else {
-		priv = cpsw_get_slave_priv(priv, 1);
-		if (likely(priv) && likely(netif_running(priv->ndev))) {
-			cpsw_intr_disable(priv);
-			cpsw_disable_irq(priv);
-			napi_schedule(&priv->napi);
-		}
+		return IRQ_HANDLED;
+	}
+
+	priv = cpsw_get_slave_priv(priv, 1);
+	if (!priv)
+		return IRQ_NONE;
+
+	if (netif_running(priv->ndev)) {
+		napi_schedule(&priv->napi);
+		return IRQ_HANDLED;
 	}
-	return IRQ_HANDLED;
+	return IRQ_NONE;
 }
 
 static int cpsw_poll(struct napi_struct *napi, int budget)
@@ -515,10 +548,16 @@ static int cpsw_poll(struct napi_struct *napi, int budget)
 
 	num_rx = cpdma_chan_process(priv->rxch, budget);
 	if (num_rx < budget) {
+		struct cpsw_priv *prim_cpsw;
+
 		napi_complete(napi);
 		cpsw_intr_enable(priv);
 		cpdma_ctlr_eoi(priv->dma, CPDMA_EOI_RX);
-		cpsw_enable_irq(priv);
+		prim_cpsw = cpsw_get_slave_priv(priv, 0);
+		if (prim_cpsw->irq_enabled == false) {
+			prim_cpsw->irq_enabled = true;
+			cpsw_enable_irq(priv);
+		}
 	}
 
 	if (num_rx || num_tx)
@@ -612,6 +651,77 @@ static void cpsw_adjust_link(struct net_device *ndev)
 	}
 }
 
+static int cpsw_get_coalesce(struct net_device *ndev,
+				struct ethtool_coalesce *coal)
+{
+	struct cpsw_priv *priv = netdev_priv(ndev);
+
+	coal->rx_coalesce_usecs = priv->coal_intvl;
+	return 0;
+}
+
+static int cpsw_set_coalesce(struct net_device *ndev,
+				struct ethtool_coalesce *coal)
+{
+	struct cpsw_priv *priv = netdev_priv(ndev);
+	u32 int_ctrl;
+	u32 num_interrupts = 0;
+	u32 prescale = 0;
+	u32 addnl_dvdr = 1;
+	u32 coal_intvl = 0;
+
+	if (!coal->rx_coalesce_usecs)
+		return -EINVAL;
+
+	coal_intvl = coal->rx_coalesce_usecs;
+
+	int_ctrl =  readl(&priv->wr_regs->int_control);
+	prescale = priv->bus_freq_mhz * 4;
+
+	if (coal_intvl < CPSW_CMINTMIN_INTVL)
+		coal_intvl = CPSW_CMINTMIN_INTVL;
+
+	if (coal_intvl > CPSW_CMINTMAX_INTVL) {
+		/* Interrupt pacer works with 4us Pulse, we can
+		 * throttle further by dilating the 4us pulse.
+		 */
+		addnl_dvdr = CPSW_INTPRESCALE_MASK / prescale;
+
+		if (addnl_dvdr > 1) {
+			prescale *= addnl_dvdr;
+			if (coal_intvl > (CPSW_CMINTMAX_INTVL * addnl_dvdr))
+				coal_intvl = (CPSW_CMINTMAX_INTVL
+						* addnl_dvdr);
+		} else {
+			addnl_dvdr = 1;
+			coal_intvl = CPSW_CMINTMAX_INTVL;
+		}
+	}
+
+	num_interrupts = (1000 * addnl_dvdr) / coal_intvl;
+	writel(num_interrupts, &priv->wr_regs->rx_imax);
+	writel(num_interrupts, &priv->wr_regs->tx_imax);
+
+	int_ctrl |= CPSW_INTPACEEN;
+	int_ctrl &= (~CPSW_INTPRESCALE_MASK);
+	int_ctrl |= (prescale & CPSW_INTPRESCALE_MASK);
+	writel(int_ctrl, &priv->wr_regs->int_control);
+
+	cpsw_notice(priv, timer, "Set coalesce to %d usecs.\n", coal_intvl);
+	if (priv->data.dual_emac) {
+		int i;
+
+		for (i = 0; i < priv->data.slaves; i++) {
+			priv = netdev_priv(priv->slaves[i].ndev);
+			priv->coal_intvl = coal_intvl;
+		}
+	} else {
+		priv->coal_intvl = coal_intvl;
+	}
+
+	return 0;
+}
+
 static inline int __show_stat(char *buf, int maxlen, const char *name, u32 val)
 {
 	static char *leader = "........................................";
@@ -643,14 +753,14 @@ static inline int cpsw_tx_packet_submit(struct net_device *ndev,
 {
 	if (!priv->data.dual_emac)
 		return cpdma_chan_submit(priv->txch, skb, skb->data,
-				  skb->len, 0, GFP_KERNEL);
+				  skb->len, 0);
 
 	if (ndev == cpsw_get_slave_ndev(priv, 0))
 		return cpdma_chan_submit(priv->txch, skb, skb->data,
-				  skb->len, 1, GFP_KERNEL);
+				  skb->len, 1);
 	else
 		return cpdma_chan_submit(priv->txch, skb, skb->data,
-				  skb->len, 2, GFP_KERNEL);
+				  skb->len, 2);
 }
 
 static inline void cpsw_add_dual_emac_def_ale_entries(
@@ -774,9 +884,19 @@ static void cpsw_init_host_port(struct cpsw_priv *priv)
 	}
 }
 
+static void cpsw_slave_stop(struct cpsw_slave *slave, struct cpsw_priv *priv)
+{
+	if (!slave->phy)
+		return;
+	phy_stop(slave->phy);
+	phy_disconnect(slave->phy);
+	slave->phy = NULL;
+}
+
 static int cpsw_ndo_open(struct net_device *ndev)
 {
 	struct cpsw_priv *priv = netdev_priv(ndev);
+	struct cpsw_priv *prim_cpsw;
 	int i, ret;
 	u32 reg;
 
@@ -819,14 +939,16 @@ static int cpsw_ndo_open(struct net_device *ndev)
 			struct sk_buff *skb;
 
 			ret = -ENOMEM;
-			skb = netdev_alloc_skb_ip_align(priv->ndev,
-							priv->rx_packet_max);
+			skb = __netdev_alloc_skb_ip_align(priv->ndev,
+					priv->rx_packet_max, GFP_KERNEL);
 			if (!skb)
-				break;
+				goto err_cleanup;
 			ret = cpdma_chan_submit(priv->rxch, skb, skb->data,
-					skb_tailroom(skb), 0, GFP_KERNEL);
-			if (WARN_ON(ret < 0))
-				break;
+					skb_tailroom(skb), 0);
+			if (ret < 0) {
+				kfree_skb(skb);
+				goto err_cleanup;
+			}
 		}
 		/* continue even if we didn't manage to submit all
 		 * receive descs
@@ -834,6 +956,22 @@ static int cpsw_ndo_open(struct net_device *ndev)
 		cpsw_info(priv, ifup, "submitted %d rx descriptors\n", i);
 	}
 
+	/* Enable Interrupt pacing if configured */
+	if (priv->coal_intvl != 0) {
+		struct ethtool_coalesce coal;
+
+		coal.rx_coalesce_usecs = (priv->coal_intvl << 4);
+		cpsw_set_coalesce(ndev, &coal);
+	}
+
+	prim_cpsw = cpsw_get_slave_priv(priv, 0);
+	if (prim_cpsw->irq_enabled == false) {
+		if ((priv == prim_cpsw) || !netif_running(prim_cpsw->ndev)) {
+			prim_cpsw->irq_enabled = true;
+			cpsw_enable_irq(prim_cpsw);
+		}
+	}
+
 	cpdma_ctlr_start(priv->dma);
 	cpsw_intr_enable(priv);
 	napi_enable(&priv->napi);
@@ -843,15 +981,13 @@ static int cpsw_ndo_open(struct net_device *ndev)
 	if (priv->data.dual_emac)
 		priv->slaves[priv->emac_port].open_stat = true;
 	return 0;
-}
 
-static void cpsw_slave_stop(struct cpsw_slave *slave, struct cpsw_priv *priv)
-{
-	if (!slave->phy)
-		return;
-	phy_stop(slave->phy);
-	phy_disconnect(slave->phy);
-	slave->phy = NULL;
+err_cleanup:
+	cpdma_ctlr_stop(priv->dma);
+	for_each_slave(priv, cpsw_slave_stop, priv);
+	pm_runtime_put_sync(&priv->pdev->dev);
+	netif_carrier_off(priv->ndev);
+	return ret;
 }
 
 static int cpsw_ndo_stop(struct net_device *ndev)
@@ -942,7 +1078,7 @@ static void cpsw_ndo_change_rx_flags(struct net_device *ndev, int flags)
 
 static void cpsw_hwtstamp_v1(struct cpsw_priv *priv)
 {
-	struct cpsw_slave *slave = &priv->slaves[priv->data.cpts_active_slave];
+	struct cpsw_slave *slave = &priv->slaves[priv->data.active_slave];
 	u32 ts_en, seq_id;
 
 	if (!priv->cpts->tx_enable && !priv->cpts->rx_enable) {
@@ -971,7 +1107,7 @@ static void cpsw_hwtstamp_v2(struct cpsw_priv *priv)
 	if (priv->data.dual_emac)
 		slave = &priv->slaves[priv->emac_port];
 	else
-		slave = &priv->slaves[priv->data.cpts_active_slave];
+		slave = &priv->slaves[priv->data.active_slave];
 
 	ctrl = slave_read(slave, CPSW2_CONTROL);
 	ctrl &= ~CTRL_ALL_TS_MASK;
@@ -1056,14 +1192,26 @@ static int cpsw_hwtstamp_ioctl(struct net_device *dev, struct ifreq *ifr)
 
 static int cpsw_ndo_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
 {
+	struct cpsw_priv *priv = netdev_priv(dev);
+	struct mii_ioctl_data *data = if_mii(req);
+	int slave_no = cpsw_slave_index(priv);
+
 	if (!netif_running(dev))
 		return -EINVAL;
 
+	switch (cmd) {
 #ifdef CONFIG_TI_CPTS
-	if (cmd == SIOCSHWTSTAMP)
+	case SIOCSHWTSTAMP:
 		return cpsw_hwtstamp_ioctl(dev, req);
 #endif
-	return -ENOTSUPP;
+	case SIOCGMIIPHY:
+		data->phy_id = priv->slaves[slave_no].phy->addr;
+		break;
+	default:
+		return -ENOTSUPP;
+	}
+
+	return 0;
 }
 
 static void cpsw_ndo_tx_timeout(struct net_device *ndev)
@@ -1138,7 +1286,7 @@ clean_vid:
 }
 
 static int cpsw_ndo_vlan_rx_add_vid(struct net_device *ndev,
-		unsigned short vid)
+				    __be16 proto, u16 vid)
 {
 	struct cpsw_priv *priv = netdev_priv(ndev);
 
@@ -1150,7 +1298,7 @@ static int cpsw_ndo_vlan_rx_add_vid(struct net_device *ndev,
 }
 
 static int cpsw_ndo_vlan_rx_kill_vid(struct net_device *ndev,
-		unsigned short vid)
+				     __be16 proto, u16 vid)
 {
 	struct cpsw_priv *priv = netdev_priv(ndev);
 	int ret;
@@ -1244,12 +1392,39 @@ static int cpsw_get_ts_info(struct net_device *ndev,
 	return 0;
 }
 
+static int cpsw_get_settings(struct net_device *ndev,
+			     struct ethtool_cmd *ecmd)
+{
+	struct cpsw_priv *priv = netdev_priv(ndev);
+	int slave_no = cpsw_slave_index(priv);
+
+	if (priv->slaves[slave_no].phy)
+		return phy_ethtool_gset(priv->slaves[slave_no].phy, ecmd);
+	else
+		return -EOPNOTSUPP;
+}
+
+static int cpsw_set_settings(struct net_device *ndev, struct ethtool_cmd *ecmd)
+{
+	struct cpsw_priv *priv = netdev_priv(ndev);
+	int slave_no = cpsw_slave_index(priv);
+
+	if (priv->slaves[slave_no].phy)
+		return phy_ethtool_sset(priv->slaves[slave_no].phy, ecmd);
+	else
+		return -EOPNOTSUPP;
+}
+
 static const struct ethtool_ops cpsw_ethtool_ops = {
 	.get_drvinfo	= cpsw_get_drvinfo,
 	.get_msglevel	= cpsw_get_msglevel,
 	.set_msglevel	= cpsw_set_msglevel,
 	.get_link	= ethtool_op_get_link,
 	.get_ts_info	= cpsw_get_ts_info,
+	.get_settings	= cpsw_get_settings,
+	.set_settings	= cpsw_set_settings,
+	.get_coalesce	= cpsw_get_coalesce,
+	.set_coalesce	= cpsw_set_coalesce,
 };
 
 static void cpsw_slave_init(struct cpsw_slave *slave, struct cpsw_priv *priv,
@@ -1282,12 +1457,12 @@ static int cpsw_probe_dt(struct cpsw_platform_data *data,
 	}
 	data->slaves = prop;
 
-	if (of_property_read_u32(node, "cpts_active_slave", &prop)) {
-		pr_err("Missing cpts_active_slave property in the DT.\n");
+	if (of_property_read_u32(node, "active_slave", &prop)) {
+		pr_err("Missing active_slave property in the DT.\n");
 		ret = -EINVAL;
 		goto error_ret;
 	}
-	data->cpts_active_slave = prop;
+	data->active_slave = prop;
 
 	if (of_property_read_u32(node, "cpts_clock_mult", &prop)) {
 		pr_err("Missing cpts_clock_mult property in the DT.\n");
@@ -1437,6 +1612,9 @@ static int cpsw_probe_dual_emac(struct platform_device *pdev,
 	priv_sl2->slaves = priv->slaves;
 	priv_sl2->clk = priv->clk;
 
+	priv_sl2->coal_intvl = 0;
+	priv_sl2->bus_freq_mhz = priv->bus_freq_mhz;
+
 	priv_sl2->cpsw_res = priv->cpsw_res;
 	priv_sl2->regs = priv->regs;
 	priv_sl2->host_port = priv->host_port;
@@ -1455,8 +1633,7 @@ static int cpsw_probe_dual_emac(struct platform_device *pdev,
 		priv_sl2->irqs_table[i] = priv->irqs_table[i];
 		priv_sl2->num_irqs = priv->num_irqs;
 	}
-
-	ndev->features |= NETIF_F_HW_VLAN_FILTER;
+	ndev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
 
 	ndev->netdev_ops = &cpsw_netdev_ops;
 	SET_ETHTOOL_OPS(ndev, &cpsw_ethtool_ops);
@@ -1476,7 +1653,7 @@ static int cpsw_probe_dual_emac(struct platform_device *pdev,
 
 static int cpsw_probe(struct platform_device *pdev)
 {
-	struct cpsw_platform_data	*data = pdev->dev.platform_data;
+	struct cpsw_platform_data	*data;
 	struct net_device		*ndev;
 	struct cpsw_priv		*priv;
 	struct cpdma_params		dma_params;
@@ -1501,6 +1678,7 @@ static int cpsw_probe(struct platform_device *pdev)
 	priv->msg_enable = netif_msg_init(debug_level, CPSW_DEBUG);
 	priv->rx_packet_max = max(rx_packet_max, 128);
 	priv->cpts = devm_kzalloc(&pdev->dev, sizeof(struct cpts), GFP_KERNEL);
+	priv->irq_enabled = true;
 	if (!ndev) {
 		pr_err("error allocating cpts\n");
 		goto clean_ndev_ret;
@@ -1546,6 +1724,8 @@ static int cpsw_probe(struct platform_device *pdev)
 		ret = -ENODEV;
 		goto clean_slave_ret;
 	}
+	priv->coal_intvl = 0;
+	priv->bus_freq_mhz = clk_get_rate(priv->clk) / 1000000;
 
 	priv->cpsw_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!priv->cpsw_res) {
@@ -1687,12 +1867,12 @@ static int cpsw_probe(struct platform_device *pdev)
 				goto clean_ale_ret;
 			}
 			priv->irqs_table[k] = i;
-			priv->num_irqs = k;
+			priv->num_irqs = k + 1;
 		}
 		k++;
 	}
 
-	ndev->features |= NETIF_F_HW_VLAN_FILTER;
+	ndev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
 
 	ndev->netdev_ops = &cpsw_netdev_ops;
 	SET_ETHTOOL_OPS(ndev, &cpsw_ethtool_ops);
@@ -1725,7 +1905,8 @@ static int cpsw_probe(struct platform_device *pdev)
 	return 0;
 
 clean_irq_ret:
-	free_irq(ndev->irq, priv);
+	for (i = 0; i < priv->num_irqs; i++)
+		free_irq(priv->irqs_table[i], priv);
 clean_ale_ret:
 	cpsw_ale_destroy(priv->ale);
 clean_dma_ret:
@@ -1748,7 +1929,8 @@ clean_slave_ret:
 	pm_runtime_disable(&pdev->dev);
 	kfree(priv->slaves);
 clean_ndev_ret:
-	free_netdev(ndev);
+	kfree(priv->data.slave_data);
+	free_netdev(priv->ndev);
 	return ret;
 }
 
@@ -1756,12 +1938,17 @@ static int cpsw_remove(struct platform_device *pdev)
 {
 	struct net_device *ndev = platform_get_drvdata(pdev);
 	struct cpsw_priv *priv = netdev_priv(ndev);
+	int i;
 
-	pr_info("removing device");
 	platform_set_drvdata(pdev, NULL);
+	if (priv->data.dual_emac)
+		unregister_netdev(cpsw_get_slave_ndev(priv, 1));
+	unregister_netdev(ndev);
 
 	cpts_unregister(priv->cpts);
-	free_irq(ndev->irq, priv);
+	for (i = 0; i < priv->num_irqs; i++)
+		free_irq(priv->irqs_table[i], priv);
+
 	cpsw_ale_destroy(priv->ale);
 	cpdma_chan_destroy(priv->txch);
 	cpdma_chan_destroy(priv->rxch);
@@ -1775,8 +1962,10 @@ static int cpsw_remove(struct platform_device *pdev)
 	pm_runtime_disable(&pdev->dev);
 	clk_put(priv->clk);
 	kfree(priv->slaves);
+	kfree(priv->data.slave_data);
+	if (priv->data.dual_emac)
+		free_netdev(cpsw_get_slave_ndev(priv, 1));
 	free_netdev(ndev);
-
 	return 0;
 }
 
@@ -1812,6 +2001,7 @@ static const struct of_device_id cpsw_of_mtable[] = {
 	{ .compatible = "ti,cpsw", },
 	{ /* sentinel */ },
 };
+MODULE_DEVICE_TABLE(of, cpsw_of_mtable);
 
 static struct platform_driver cpsw_driver = {
 	.driver = {
diff --git a/drivers/net/ethernet/ti/davinci_cpdma.c b/drivers/net/ethernet/ti/davinci_cpdma.c
index ee13dc78430c..49dfd592ac1e 100644
--- a/drivers/net/ethernet/ti/davinci_cpdma.c
+++ b/drivers/net/ethernet/ti/davinci_cpdma.c
@@ -20,6 +20,7 @@
 #include <linux/err.h>
 #include <linux/dma-mapping.h>
 #include <linux/io.h>
+#include <linux/delay.h>
 
 #include "davinci_cpdma.h"
 
@@ -312,14 +313,16 @@ int cpdma_ctlr_start(struct cpdma_ctlr *ctlr)
 	}
 
 	if (ctlr->params.has_soft_reset) {
-		unsigned long timeout = jiffies + HZ/10;
+		unsigned timeout = 10 * 100;
 
 		dma_reg_write(ctlr, CPDMA_SOFTRESET, 1);
-		while (time_before(jiffies, timeout)) {
+		while (timeout) {
 			if (dma_reg_read(ctlr, CPDMA_SOFTRESET) == 0)
 				break;
+			udelay(10);
+			timeout--;
 		}
-		WARN_ON(!time_before(jiffies, timeout));
+		WARN_ON(!timeout);
 	}
 
 	for (i = 0; i < ctlr->num_chan; i++) {
@@ -673,7 +676,7 @@ static void __cpdma_chan_submit(struct cpdma_chan *chan,
 }
 
 int cpdma_chan_submit(struct cpdma_chan *chan, void *token, void *data,
-		      int len, int directed, gfp_t gfp_mask)
+		      int len, int directed)
 {
 	struct cpdma_ctlr		*ctlr = chan->ctlr;
 	struct cpdma_desc __iomem	*desc;
@@ -773,6 +776,7 @@ static int __cpdma_chan_process(struct cpdma_chan *chan)
 	struct cpdma_ctlr		*ctlr = chan->ctlr;
 	struct cpdma_desc __iomem	*desc;
 	int				status, outlen;
+	int				cb_status = 0;
 	struct cpdma_desc_pool		*pool = ctlr->pool;
 	dma_addr_t			desc_dma;
 	unsigned long			flags;
@@ -808,8 +812,12 @@ static int __cpdma_chan_process(struct cpdma_chan *chan)
 	}
 
 	spin_unlock_irqrestore(&chan->lock, flags);
+	if (unlikely(status & CPDMA_DESC_TD_COMPLETE))
+		cb_status = -ENOSYS;
+	else
+		cb_status = status;
 
-	__cpdma_chan_free(chan, desc, outlen, status);
+	__cpdma_chan_free(chan, desc, outlen, cb_status);
 	return status;
 
 unlock_ret:
@@ -868,7 +876,7 @@ int cpdma_chan_stop(struct cpdma_chan *chan)
 	struct cpdma_desc_pool	*pool = ctlr->pool;
 	unsigned long		flags;
 	int			ret;
-	unsigned long		timeout;
+	unsigned		timeout;
 
 	spin_lock_irqsave(&chan->lock, flags);
 	if (chan->state != CPDMA_STATE_ACTIVE) {
@@ -883,14 +891,15 @@ int cpdma_chan_stop(struct cpdma_chan *chan)
 	dma_reg_write(ctlr, chan->td, chan_linear(chan));
 
 	/* wait for teardown complete */
-	timeout = jiffies + HZ/10;	/* 100 msec */
-	while (time_before(jiffies, timeout)) {
+	timeout = 100 * 100; /* 100 ms */
+	while (timeout) {
 		u32 cp = chan_read(chan, cp);
 		if ((cp & CPDMA_TEARDOWN_VALUE) == CPDMA_TEARDOWN_VALUE)
 			break;
-		cpu_relax();
+		udelay(10);
+		timeout--;
 	}
-	WARN_ON(!time_before(jiffies, timeout));
+	WARN_ON(!timeout);
 	chan_write(chan, cp, CPDMA_TEARDOWN_VALUE);
 
 	/* handle completed packets */
@@ -1031,3 +1040,5 @@ unlock_ret:
 	return ret;
 }
 EXPORT_SYMBOL_GPL(cpdma_control_set);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/ethernet/ti/davinci_cpdma.h b/drivers/net/ethernet/ti/davinci_cpdma.h
index d9bcc6032fdc..86dee487f2f0 100644
--- a/drivers/net/ethernet/ti/davinci_cpdma.h
+++ b/drivers/net/ethernet/ti/davinci_cpdma.h
@@ -89,7 +89,7 @@ int cpdma_chan_dump(struct cpdma_chan *chan);
 int cpdma_chan_get_stats(struct cpdma_chan *chan,
 			 struct cpdma_chan_stats *stats);
 int cpdma_chan_submit(struct cpdma_chan *chan, void *token, void *data,
-		      int len, int directed, gfp_t gfp_mask);
+		      int len, int directed);
 int cpdma_chan_process(struct cpdma_chan *chan, int quota);
 
 int cpdma_ctlr_int_ctrl(struct cpdma_ctlr *ctlr, bool enable);
diff --git a/drivers/net/ethernet/ti/davinci_emac.c b/drivers/net/ethernet/ti/davinci_emac.c
index 72300bc9e378..860e15ddfbcb 100644
--- a/drivers/net/ethernet/ti/davinci_emac.c
+++ b/drivers/net/ethernet/ti/davinci_emac.c
@@ -1037,7 +1037,7 @@ static void emac_rx_handler(void *token, int len, int status)
 
 recycle:
 	ret = cpdma_chan_submit(priv->rxchan, skb, skb->data,
-			skb_tailroom(skb), 0, GFP_KERNEL);
+			skb_tailroom(skb), 0);
 
 	WARN_ON(ret == -ENOMEM);
 	if (unlikely(ret < 0))
@@ -1092,7 +1092,7 @@ static int emac_dev_xmit(struct sk_buff *skb, struct net_device *ndev)
 	skb_tx_timestamp(skb);
 
 	ret_code = cpdma_chan_submit(priv->txchan, skb, skb->data, skb->len,
-				     0, GFP_KERNEL);
+				     0);
 	if (unlikely(ret_code != 0)) {
 		if (netif_msg_tx_err(priv) && net_ratelimit())
 			dev_err(emac_dev, "DaVinci EMAC: desc submit failed");
@@ -1438,7 +1438,7 @@ static int emac_poll(struct napi_struct *napi, int budget)
  * Polled functionality used by netconsole and others in non interrupt mode
  *
  */
-void emac_poll_controller(struct net_device *ndev)
+static void emac_poll_controller(struct net_device *ndev)
 {
 	struct emac_priv *priv = netdev_priv(ndev);
 
@@ -1558,7 +1558,7 @@ static int emac_dev_open(struct net_device *ndev)
 			break;
 
 		ret = cpdma_chan_submit(priv->rxchan, skb, skb->data,
-					skb_tailroom(skb), 0, GFP_KERNEL);
+					skb_tailroom(skb), 0);
 		if (WARN_ON(ret < 0))
 			break;
 	}
@@ -1865,21 +1865,18 @@ static int davinci_emac_probe(struct platform_device *pdev)
 
 
 	/* obtain emac clock from kernel */
-	emac_clk = clk_get(&pdev->dev, NULL);
+	emac_clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(emac_clk)) {
 		dev_err(&pdev->dev, "failed to get EMAC clock\n");
 		return -EBUSY;
 	}
 	emac_bus_frequency = clk_get_rate(emac_clk);
-	clk_put(emac_clk);
 
 	/* TODO: Probe PHY here if possible */
 
 	ndev = alloc_etherdev(sizeof(struct emac_priv));
-	if (!ndev) {
-		rc = -ENOMEM;
-		goto no_ndev;
-	}
+	if (!ndev)
+		return -ENOMEM;
 
 	platform_set_drvdata(pdev, ndev);
 	priv = netdev_priv(ndev);
@@ -1893,7 +1890,7 @@ static int davinci_emac_probe(struct platform_device *pdev)
 	if (!pdata) {
 		dev_err(&pdev->dev, "no platform data\n");
 		rc = -ENODEV;
-		goto probe_quit;
+		goto no_pdata;
 	}
 
 	/* MAC addr and PHY mask , RMII enable info from platform_data */
@@ -1913,23 +1910,23 @@ static int davinci_emac_probe(struct platform_device *pdev)
 	if (!res) {
 		dev_err(&pdev->dev,"error getting res\n");
 		rc = -ENOENT;
-		goto probe_quit;
+		goto no_pdata;
 	}
 
 	priv->emac_base_phys = res->start + pdata->ctrl_reg_offset;
 	size = resource_size(res);
-	if (!request_mem_region(res->start, size, ndev->name)) {
+	if (!devm_request_mem_region(&pdev->dev, res->start,
+				     size, ndev->name)) {
 		dev_err(&pdev->dev, "failed request_mem_region() for regs\n");
 		rc = -ENXIO;
-		goto probe_quit;
+		goto no_pdata;
 	}
 
-	priv->remap_addr = ioremap(res->start, size);
+	priv->remap_addr = devm_ioremap(&pdev->dev, res->start, size);
 	if (!priv->remap_addr) {
 		dev_err(&pdev->dev, "unable to map IO\n");
 		rc = -ENOMEM;
-		release_mem_region(res->start, size);
-		goto probe_quit;
+		goto no_pdata;
 	}
 	priv->emac_base = priv->remap_addr + pdata->ctrl_reg_offset;
 	ndev->base_addr = (unsigned long)priv->remap_addr;
@@ -1962,7 +1959,7 @@ static int davinci_emac_probe(struct platform_device *pdev)
 	if (!priv->dma) {
 		dev_err(&pdev->dev, "error initializing DMA\n");
 		rc = -ENOMEM;
-		goto no_dma;
+		goto no_pdata;
 	}
 
 	priv->txchan = cpdma_chan_create(priv->dma, tx_chan_num(EMAC_DEF_TX_CH),
@@ -1971,14 +1968,14 @@ static int davinci_emac_probe(struct platform_device *pdev)
 				       emac_rx_handler);
 	if (WARN_ON(!priv->txchan || !priv->rxchan)) {
 		rc = -ENOMEM;
-		goto no_irq_res;
+		goto no_cpdma_chan;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (!res) {
 		dev_err(&pdev->dev, "error getting irq res\n");
 		rc = -ENOENT;
-		goto no_irq_res;
+		goto no_cpdma_chan;
 	}
 	ndev->irq = res->start;
 
@@ -2000,7 +1997,7 @@ static int davinci_emac_probe(struct platform_device *pdev)
 	if (rc) {
 		dev_err(&pdev->dev, "error in register_netdev\n");
 		rc = -ENODEV;
-		goto no_irq_res;
+		goto no_cpdma_chan;
 	}
 
 
@@ -2015,20 +2012,14 @@ static int davinci_emac_probe(struct platform_device *pdev)
 
 	return 0;
 
-no_irq_res:
+no_cpdma_chan:
 	if (priv->txchan)
 		cpdma_chan_destroy(priv->txchan);
 	if (priv->rxchan)
 		cpdma_chan_destroy(priv->rxchan);
 	cpdma_ctlr_destroy(priv->dma);
-no_dma:
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	release_mem_region(res->start, resource_size(res));
-	iounmap(priv->remap_addr);
-
-probe_quit:
+no_pdata:
 	free_netdev(ndev);
-no_ndev:
 	return rc;
 }
 
@@ -2041,14 +2032,12 @@ no_ndev:
  */
 static int davinci_emac_remove(struct platform_device *pdev)
 {
-	struct resource *res;
 	struct net_device *ndev = platform_get_drvdata(pdev);
 	struct emac_priv *priv = netdev_priv(ndev);
 
 	dev_notice(&ndev->dev, "DaVinci EMAC: davinci_emac_remove()\n");
 
 	platform_set_drvdata(pdev, NULL);
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
 	if (priv->txchan)
 		cpdma_chan_destroy(priv->txchan);
@@ -2056,10 +2045,7 @@ static int davinci_emac_remove(struct platform_device *pdev)
 		cpdma_chan_destroy(priv->rxchan);
 	cpdma_ctlr_destroy(priv->dma);
 
-	release_mem_region(res->start, resource_size(res));
-
 	unregister_netdev(ndev);
-	iounmap(priv->remap_addr);
 	free_netdev(ndev);
 
 	return 0;
diff --git a/drivers/net/ethernet/ti/davinci_mdio.c b/drivers/net/ethernet/ti/davinci_mdio.c
index d04a622b08d4..12aec173564c 100644
--- a/drivers/net/ethernet/ti/davinci_mdio.c
+++ b/drivers/net/ethernet/ti/davinci_mdio.c
@@ -485,6 +485,7 @@ static const struct of_device_id davinci_mdio_of_mtable[] = {
 	{ .compatible = "ti,davinci_mdio", },
 	{ /* sentinel */ },
 };
+MODULE_DEVICE_TABLE(of, davinci_mdio_of_mtable);
 
 static struct platform_driver davinci_mdio_driver = {
 	.driver = {
diff --git a/drivers/net/ethernet/ti/tlan.c b/drivers/net/ethernet/ti/tlan.c
index 22725386c5de..60c400f6d01f 100644
--- a/drivers/net/ethernet/ti/tlan.c
+++ b/drivers/net/ethernet/ti/tlan.c
@@ -320,6 +320,7 @@ static void tlan_remove_one(struct pci_dev *pdev)
 	free_netdev(dev);
 
 	pci_set_drvdata(pdev, NULL);
+	cancel_work_sync(&priv->tlan_tqueue);
 }
 
 static void tlan_start(struct net_device *dev)
@@ -1911,10 +1912,8 @@ static void tlan_reset_lists(struct net_device *dev)
 		list->frame_size = TLAN_MAX_FRAME_SIZE;
 		list->buffer[0].count = TLAN_MAX_FRAME_SIZE | TLAN_LAST_BUFFER;
 		skb = netdev_alloc_skb_ip_align(dev, TLAN_MAX_FRAME_SIZE + 5);
-		if (!skb) {
-			netdev_err(dev, "Out of memory for received data\n");
+		if (!skb)
 			break;
-		}
 
 		list->buffer[0].address = pci_map_single(priv->pci_dev,
 							 skb->data,
diff --git a/drivers/net/ethernet/tile/tilegx.c b/drivers/net/ethernet/tile/tilegx.c
index 66e025ad5df1..f3c2d034b32c 100644
--- a/drivers/net/ethernet/tile/tilegx.c
+++ b/drivers/net/ethernet/tile/tilegx.c
@@ -930,7 +930,7 @@ static int tile_net_setup_interrupts(struct net_device *dev)
 		if (info->has_iqueue) {
 			gxio_mpipe_request_notif_ring_interrupt(
 				&context, cpu_x(cpu), cpu_y(cpu),
-				1, ingress_irq, info->iqueue.ring);
+				KERNEL_PL, ingress_irq, info->iqueue.ring);
 		}
 	}
 
diff --git a/drivers/net/ethernet/toshiba/ps3_gelic_net.c b/drivers/net/ethernet/toshiba/ps3_gelic_net.c
index 445c0595c997..ad32af67e618 100644
--- a/drivers/net/ethernet/toshiba/ps3_gelic_net.c
+++ b/drivers/net/ethernet/toshiba/ps3_gelic_net.c
@@ -58,13 +58,6 @@ MODULE_DESCRIPTION("Gelic Network driver");
 MODULE_LICENSE("GPL");
 
 
-static inline void gelic_card_enable_rxdmac(struct gelic_card *card);
-static inline void gelic_card_disable_rxdmac(struct gelic_card *card);
-static inline void gelic_card_disable_txdmac(struct gelic_card *card);
-static inline void gelic_card_reset_chain(struct gelic_card *card,
-					  struct gelic_descr_chain *chain,
-					  struct gelic_descr *start_descr);
-
 /* set irq_mask */
 int gelic_card_set_irq_mask(struct gelic_card *card, u64 mask)
 {
@@ -78,12 +71,12 @@ int gelic_card_set_irq_mask(struct gelic_card *card, u64 mask)
 	return status;
 }
 
-static inline void gelic_card_rx_irq_on(struct gelic_card *card)
+static void gelic_card_rx_irq_on(struct gelic_card *card)
 {
 	card->irq_mask |= GELIC_CARD_RXINT;
 	gelic_card_set_irq_mask(card, card->irq_mask);
 }
-static inline void gelic_card_rx_irq_off(struct gelic_card *card)
+static void gelic_card_rx_irq_off(struct gelic_card *card)
 {
 	card->irq_mask &= ~GELIC_CARD_RXINT;
 	gelic_card_set_irq_mask(card, card->irq_mask);
@@ -127,6 +120,120 @@ static int gelic_card_set_link_mode(struct gelic_card *card, int mode)
 	return 0;
 }
 
+/**
+ * gelic_card_disable_txdmac - disables the transmit DMA controller
+ * @card: card structure
+ *
+ * gelic_card_disable_txdmac terminates processing on the DMA controller by
+ * turing off DMA and issuing a force end
+ */
+static void gelic_card_disable_txdmac(struct gelic_card *card)
+{
+	int status;
+
+	/* this hvc blocks until the DMA in progress really stopped */
+	status = lv1_net_stop_tx_dma(bus_id(card), dev_id(card));
+	if (status)
+		dev_err(ctodev(card),
+			"lv1_net_stop_tx_dma failed, status=%d\n", status);
+}
+
+/**
+ * gelic_card_enable_rxdmac - enables the receive DMA controller
+ * @card: card structure
+ *
+ * gelic_card_enable_rxdmac enables the DMA controller by setting RX_DMA_EN
+ * in the GDADMACCNTR register
+ */
+static void gelic_card_enable_rxdmac(struct gelic_card *card)
+{
+	int status;
+
+#ifdef DEBUG
+	if (gelic_descr_get_status(card->rx_chain.head) !=
+	    GELIC_DESCR_DMA_CARDOWNED) {
+		printk(KERN_ERR "%s: status=%x\n", __func__,
+		       be32_to_cpu(card->rx_chain.head->dmac_cmd_status));
+		printk(KERN_ERR "%s: nextphy=%x\n", __func__,
+		       be32_to_cpu(card->rx_chain.head->next_descr_addr));
+		printk(KERN_ERR "%s: head=%p\n", __func__,
+		       card->rx_chain.head);
+	}
+#endif
+	status = lv1_net_start_rx_dma(bus_id(card), dev_id(card),
+				card->rx_chain.head->bus_addr, 0);
+	if (status)
+		dev_info(ctodev(card),
+			 "lv1_net_start_rx_dma failed, status=%d\n", status);
+}
+
+/**
+ * gelic_card_disable_rxdmac - disables the receive DMA controller
+ * @card: card structure
+ *
+ * gelic_card_disable_rxdmac terminates processing on the DMA controller by
+ * turing off DMA and issuing a force end
+ */
+static void gelic_card_disable_rxdmac(struct gelic_card *card)
+{
+	int status;
+
+	/* this hvc blocks until the DMA in progress really stopped */
+	status = lv1_net_stop_rx_dma(bus_id(card), dev_id(card));
+	if (status)
+		dev_err(ctodev(card),
+			"lv1_net_stop_rx_dma failed, %d\n", status);
+}
+
+/**
+ * gelic_descr_set_status -- sets the status of a descriptor
+ * @descr: descriptor to change
+ * @status: status to set in the descriptor
+ *
+ * changes the status to the specified value. Doesn't change other bits
+ * in the status
+ */
+static void gelic_descr_set_status(struct gelic_descr *descr,
+				   enum gelic_descr_dma_status status)
+{
+	descr->dmac_cmd_status = cpu_to_be32(status |
+			(be32_to_cpu(descr->dmac_cmd_status) &
+			 ~GELIC_DESCR_DMA_STAT_MASK));
+	/*
+	 * dma_cmd_status field is used to indicate whether the descriptor
+	 * is valid or not.
+	 * Usually caller of this function wants to inform that to the
+	 * hardware, so we assure here the hardware sees the change.
+	 */
+	wmb();
+}
+
+/**
+ * gelic_card_reset_chain - reset status of a descriptor chain
+ * @card: card structure
+ * @chain: address of chain
+ * @start_descr: address of descriptor array
+ *
+ * Reset the status of dma descriptors to ready state
+ * and re-initialize the hardware chain for later use
+ */
+static void gelic_card_reset_chain(struct gelic_card *card,
+				   struct gelic_descr_chain *chain,
+				   struct gelic_descr *start_descr)
+{
+	struct gelic_descr *descr;
+
+	for (descr = start_descr; start_descr != descr->next; descr++) {
+		gelic_descr_set_status(descr, GELIC_DESCR_DMA_CARDOWNED);
+		descr->next_descr_addr = cpu_to_be32(descr->next->bus_addr);
+	}
+
+	chain->head = start_descr;
+	chain->tail = (descr - 1);
+
+	(descr - 1)->next_descr_addr = 0;
+}
+
 void gelic_card_up(struct gelic_card *card)
 {
 	pr_debug("%s: called\n", __func__);
@@ -183,29 +290,6 @@ gelic_descr_get_status(struct gelic_descr *descr)
 }
 
 /**
- * gelic_descr_set_status -- sets the status of a descriptor
- * @descr: descriptor to change
- * @status: status to set in the descriptor
- *
- * changes the status to the specified value. Doesn't change other bits
- * in the status
- */
-static void gelic_descr_set_status(struct gelic_descr *descr,
-				   enum gelic_descr_dma_status status)
-{
-	descr->dmac_cmd_status = cpu_to_be32(status |
-			(be32_to_cpu(descr->dmac_cmd_status) &
-			 ~GELIC_DESCR_DMA_STAT_MASK));
-	/*
-	 * dma_cmd_status field is used to indicate whether the descriptor
-	 * is valid or not.
-	 * Usually caller of this function wants to inform that to the
-	 * hardware, so we assure here the hardware sees the change.
-	 */
-	wmb();
-}
-
-/**
  * gelic_card_free_chain - free descriptor chain
  * @card: card structure
  * @descr_in: address of desc
@@ -286,31 +370,6 @@ iommu_error:
 }
 
 /**
- * gelic_card_reset_chain - reset status of a descriptor chain
- * @card: card structure
- * @chain: address of chain
- * @start_descr: address of descriptor array
- *
- * Reset the status of dma descriptors to ready state
- * and re-initialize the hardware chain for later use
- */
-static void gelic_card_reset_chain(struct gelic_card *card,
-				   struct gelic_descr_chain *chain,
-				   struct gelic_descr *start_descr)
-{
-	struct gelic_descr *descr;
-
-	for (descr = start_descr; start_descr != descr->next; descr++) {
-		gelic_descr_set_status(descr, GELIC_DESCR_DMA_CARDOWNED);
-		descr->next_descr_addr = cpu_to_be32(descr->next->bus_addr);
-	}
-
-	chain->head = start_descr;
-	chain->tail = (descr - 1);
-
-	(descr - 1)->next_descr_addr = 0;
-}
-/**
  * gelic_descr_prepare_rx - reinitializes a rx descriptor
  * @card: card structure
  * @descr: descriptor to re-init
@@ -599,71 +658,6 @@ void gelic_net_set_multi(struct net_device *netdev)
 }
 
 /**
- * gelic_card_enable_rxdmac - enables the receive DMA controller
- * @card: card structure
- *
- * gelic_card_enable_rxdmac enables the DMA controller by setting RX_DMA_EN
- * in the GDADMACCNTR register
- */
-static inline void gelic_card_enable_rxdmac(struct gelic_card *card)
-{
-	int status;
-
-#ifdef DEBUG
-	if (gelic_descr_get_status(card->rx_chain.head) !=
-	    GELIC_DESCR_DMA_CARDOWNED) {
-		printk(KERN_ERR "%s: status=%x\n", __func__,
-		       be32_to_cpu(card->rx_chain.head->dmac_cmd_status));
-		printk(KERN_ERR "%s: nextphy=%x\n", __func__,
-		       be32_to_cpu(card->rx_chain.head->next_descr_addr));
-		printk(KERN_ERR "%s: head=%p\n", __func__,
-		       card->rx_chain.head);
-	}
-#endif
-	status = lv1_net_start_rx_dma(bus_id(card), dev_id(card),
-				card->rx_chain.head->bus_addr, 0);
-	if (status)
-		dev_info(ctodev(card),
-			 "lv1_net_start_rx_dma failed, status=%d\n", status);
-}
-
-/**
- * gelic_card_disable_rxdmac - disables the receive DMA controller
- * @card: card structure
- *
- * gelic_card_disable_rxdmac terminates processing on the DMA controller by
- * turing off DMA and issuing a force end
- */
-static inline void gelic_card_disable_rxdmac(struct gelic_card *card)
-{
-	int status;
-
-	/* this hvc blocks until the DMA in progress really stopped */
-	status = lv1_net_stop_rx_dma(bus_id(card), dev_id(card));
-	if (status)
-		dev_err(ctodev(card),
-			"lv1_net_stop_rx_dma failed, %d\n", status);
-}
-
-/**
- * gelic_card_disable_txdmac - disables the transmit DMA controller
- * @card: card structure
- *
- * gelic_card_disable_txdmac terminates processing on the DMA controller by
- * turing off DMA and issuing a force end
- */
-static inline void gelic_card_disable_txdmac(struct gelic_card *card)
-{
-	int status;
-
-	/* this hvc blocks until the DMA in progress really stopped */
-	status = lv1_net_stop_tx_dma(bus_id(card), dev_id(card));
-	if (status)
-		dev_err(ctodev(card),
-			"lv1_net_stop_tx_dma failed, status=%d\n", status);
-}
-
-/**
  * gelic_net_stop - called upon ifconfig down
  * @netdev: interface device structure
  *
@@ -746,7 +740,7 @@ static void gelic_descr_set_tx_cmdstat(struct gelic_descr *descr,
 	}
 }
 
-static inline struct sk_buff *gelic_put_vlan_tag(struct sk_buff *skb,
+static struct sk_buff *gelic_put_vlan_tag(struct sk_buff *skb,
 						 unsigned short tag)
 {
 	struct vlan_ethhdr *veth;
diff --git a/drivers/net/ethernet/toshiba/spider_net.c b/drivers/net/ethernet/toshiba/spider_net.c
index f1b91fd7e41c..5734480c1ecf 100644
--- a/drivers/net/ethernet/toshiba/spider_net.c
+++ b/drivers/net/ethernet/toshiba/spider_net.c
@@ -352,8 +352,7 @@ spider_net_init_chain(struct spider_net_card *card,
 	alloc_size = chain->num_desc * sizeof(struct spider_net_hw_descr);
 
 	chain->hwring = dma_alloc_coherent(&card->pdev->dev, alloc_size,
-		&chain->dma_addr, GFP_KERNEL);
-
+					   &chain->dma_addr, GFP_KERNEL);
 	if (!chain->hwring)
 		return -ENOMEM;
 
@@ -1991,7 +1990,8 @@ spider_net_open(struct net_device *netdev)
 		goto alloc_rx_failed;
 
 	/* Allocate rx skbs */
-	if (spider_net_alloc_rx_skbs(card))
+	result = spider_net_alloc_rx_skbs(card);
+	if (result)
 		goto alloc_skbs_failed;
 
 	spider_net_set_multi(netdev);
@@ -2330,8 +2330,8 @@ spider_net_setup_netdev(struct spider_net_card *card)
 	if (SPIDER_NET_RX_CSUM_DEFAULT)
 		netdev->features |= NETIF_F_RXCSUM;
 	netdev->features |= NETIF_F_IP_CSUM | NETIF_F_LLTX;
-	/* some time: NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX |
-	 *		NETIF_F_HW_VLAN_FILTER */
+	/* some time: NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX |
+	 *		NETIF_F_HW_VLAN_CTAG_FILTER */
 
 	netdev->irq = card->pdev->irq;
 	card->num_rx_ints = 0;
diff --git a/drivers/net/ethernet/tundra/tsi108_eth.c b/drivers/net/ethernet/tundra/tsi108_eth.c
index 8fa947a2d929..3c69a0460832 100644
--- a/drivers/net/ethernet/tundra/tsi108_eth.c
+++ b/drivers/net/ethernet/tundra/tsi108_eth.c
@@ -1308,27 +1308,16 @@ static int tsi108_open(struct net_device *dev)
 		       data->id, dev->irq, dev->name);
 	}
 
-	data->rxring = dma_alloc_coherent(NULL, rxring_size,
-			&data->rxdma, GFP_KERNEL);
-
-	if (!data->rxring) {
-		printk(KERN_DEBUG
-		       "TSI108_ETH: failed to allocate memory for rxring!\n");
+	data->rxring = dma_alloc_coherent(NULL, rxring_size, &data->rxdma,
+					  GFP_KERNEL | __GFP_ZERO);
+	if (!data->rxring)
 		return -ENOMEM;
-	} else {
-		memset(data->rxring, 0, rxring_size);
-	}
-
-	data->txring = dma_alloc_coherent(NULL, txring_size,
-			&data->txdma, GFP_KERNEL);
 
+	data->txring = dma_alloc_coherent(NULL, txring_size, &data->txdma,
+					  GFP_KERNEL | __GFP_ZERO);
 	if (!data->txring) {
-		printk(KERN_DEBUG
-		       "TSI108_ETH: failed to allocate memory for txring!\n");
 		pci_free_consistent(0, rxring_size, data->rxring, data->rxdma);
 		return -ENOMEM;
-	} else {
-		memset(data->txring, 0, txring_size);
 	}
 
 	for (i = 0; i < TSI108_RXRING_LEN; i++) {
diff --git a/drivers/net/ethernet/via/via-rhine.c b/drivers/net/ethernet/via/via-rhine.c
index 185c721c52d7..ca98acabf1b4 100644
--- a/drivers/net/ethernet/via/via-rhine.c
+++ b/drivers/net/ethernet/via/via-rhine.c
@@ -508,8 +508,10 @@ static struct rtnl_link_stats64 *rhine_get_stats64(struct net_device *dev,
 static int netdev_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
 static const struct ethtool_ops netdev_ethtool_ops;
 static int  rhine_close(struct net_device *dev);
-static int rhine_vlan_rx_add_vid(struct net_device *dev, unsigned short vid);
-static int rhine_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid);
+static int rhine_vlan_rx_add_vid(struct net_device *dev,
+				 __be16 proto, u16 vid);
+static int rhine_vlan_rx_kill_vid(struct net_device *dev,
+				  __be16 proto, u16 vid);
 static void rhine_restart_tx(struct net_device *dev);
 
 static void rhine_wait_bit(struct rhine_private *rp, u8 reg, u8 mask, bool low)
@@ -1026,8 +1028,9 @@ static int rhine_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		dev->features |= NETIF_F_SG|NETIF_F_HW_CSUM;
 
 	if (pdev->revision >= VT6105M)
-		dev->features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX |
-		NETIF_F_HW_VLAN_FILTER;
+		dev->features |= NETIF_F_HW_VLAN_CTAG_TX |
+				 NETIF_F_HW_VLAN_CTAG_RX |
+				 NETIF_F_HW_VLAN_CTAG_FILTER;
 
 	/* dev->name not defined before register_netdev()! */
 	rc = register_netdev(dev);
@@ -1414,7 +1417,7 @@ static void rhine_update_vcam(struct net_device *dev)
 	rhine_set_vlan_cam_mask(ioaddr, vCAMmask);
 }
 
-static int rhine_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
+static int rhine_vlan_rx_add_vid(struct net_device *dev, __be16 proto, u16 vid)
 {
 	struct rhine_private *rp = netdev_priv(dev);
 
@@ -1425,7 +1428,7 @@ static int rhine_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
 	return 0;
 }
 
-static int rhine_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
+static int rhine_vlan_rx_kill_vid(struct net_device *dev, __be16 proto, u16 vid)
 {
 	struct rhine_private *rp = netdev_priv(dev);
 
@@ -1933,7 +1936,7 @@ static int rhine_rx(struct net_device *dev, int limit)
 			skb->protocol = eth_type_trans(skb, dev);
 
 			if (unlikely(desc_length & DescTag))
-				__vlan_hwaccel_put_tag(skb, vlan_tci);
+				__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_tci);
 			netif_receive_skb(skb);
 
 			u64_stats_update_begin(&rp->rx_stats.syncp);
diff --git a/drivers/net/ethernet/via/via-velocity.c b/drivers/net/ethernet/via/via-velocity.c
index 1bc7f9fd2583..fb6248956ee2 100644
--- a/drivers/net/ethernet/via/via-velocity.c
+++ b/drivers/net/ethernet/via/via-velocity.c
@@ -525,7 +525,8 @@ static void velocity_init_cam_filter(struct velocity_info *vptr)
 	mac_set_vlan_cam_mask(regs, vptr->vCAMmask);
 }
 
-static int velocity_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
+static int velocity_vlan_rx_add_vid(struct net_device *dev,
+				    __be16 proto, u16 vid)
 {
 	struct velocity_info *vptr = netdev_priv(dev);
 
@@ -536,7 +537,8 @@ static int velocity_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
 	return 0;
 }
 
-static int velocity_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
+static int velocity_vlan_rx_kill_vid(struct net_device *dev,
+				     __be16 proto, u16 vid)
 {
 	struct velocity_info *vptr = netdev_priv(dev);
 
@@ -2078,7 +2080,7 @@ static int velocity_receive_frame(struct velocity_info *vptr, int idx)
 	if (rd->rdesc0.RSR & RSR_DETAG) {
 		u16 vid = swab16(le16_to_cpu(rd->rdesc1.PQTAG));
 
-		__vlan_hwaccel_put_tag(skb, vid);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
 	}
 	netif_rx(skb);
 
@@ -2810,9 +2812,10 @@ static int velocity_found1(struct pci_dev *pdev,
 	dev->ethtool_ops = &velocity_ethtool_ops;
 	netif_napi_add(dev, &vptr->napi, velocity_poll, VELOCITY_NAPI_WEIGHT);
 
-	dev->hw_features = NETIF_F_IP_CSUM | NETIF_F_SG | NETIF_F_HW_VLAN_TX;
-	dev->features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_FILTER |
-		NETIF_F_HW_VLAN_RX | NETIF_F_IP_CSUM;
+	dev->hw_features = NETIF_F_IP_CSUM | NETIF_F_SG |
+			   NETIF_F_HW_VLAN_CTAG_TX;
+	dev->features |= NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_FILTER |
+			 NETIF_F_HW_VLAN_CTAG_RX | NETIF_F_IP_CSUM;
 
 	ret = register_netdev(dev);
 	if (ret < 0)
diff --git a/drivers/net/ethernet/wiznet/w5100.c b/drivers/net/ethernet/wiznet/w5100.c
index 545043cc4c0b..a518dcab396e 100644
--- a/drivers/net/ethernet/wiznet/w5100.c
+++ b/drivers/net/ethernet/wiznet/w5100.c
@@ -754,7 +754,7 @@ static int w5100_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int w5100_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
@@ -787,7 +787,7 @@ static int w5100_resume(struct device *dev)
 	}
 	return 0;
 }
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 static SIMPLE_DEV_PM_OPS(w5100_pm_ops, w5100_suspend, w5100_resume);
 
diff --git a/drivers/net/ethernet/wiznet/w5300.c b/drivers/net/ethernet/wiznet/w5300.c
index 7cbd0e6fc6f3..6e00e3f94ce4 100644
--- a/drivers/net/ethernet/wiznet/w5300.c
+++ b/drivers/net/ethernet/wiznet/w5300.c
@@ -666,7 +666,7 @@ static int w5300_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int w5300_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
@@ -699,7 +699,7 @@ static int w5300_resume(struct device *dev)
 	}
 	return 0;
 }
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 static SIMPLE_DEV_PM_OPS(w5300_pm_ops, w5300_suspend, w5300_resume);
 
diff --git a/drivers/net/ethernet/xilinx/ll_temac_main.c b/drivers/net/ethernet/xilinx/ll_temac_main.c
index 9fc2ada4c3c2..57c2e5ef2804 100644
--- a/drivers/net/ethernet/xilinx/ll_temac_main.c
+++ b/drivers/net/ethernet/xilinx/ll_temac_main.c
@@ -245,39 +245,30 @@ static int temac_dma_bd_init(struct net_device *ndev)
 	/* returns a virtual address and a physical address. */
 	lp->tx_bd_v = dma_alloc_coherent(ndev->dev.parent,
 					 sizeof(*lp->tx_bd_v) * TX_BD_NUM,
-					 &lp->tx_bd_p, GFP_KERNEL);
-	if (!lp->tx_bd_v) {
-		dev_err(&ndev->dev,
-				"unable to allocate DMA TX buffer descriptors");
+					 &lp->tx_bd_p, GFP_KERNEL | __GFP_ZERO);
+	if (!lp->tx_bd_v)
 		goto out;
-	}
+
 	lp->rx_bd_v = dma_alloc_coherent(ndev->dev.parent,
 					 sizeof(*lp->rx_bd_v) * RX_BD_NUM,
-					 &lp->rx_bd_p, GFP_KERNEL);
-	if (!lp->rx_bd_v) {
-		dev_err(&ndev->dev,
-				"unable to allocate DMA RX buffer descriptors");
+					 &lp->rx_bd_p, GFP_KERNEL | __GFP_ZERO);
+	if (!lp->rx_bd_v)
 		goto out;
-	}
 
-	memset(lp->tx_bd_v, 0, sizeof(*lp->tx_bd_v) * TX_BD_NUM);
 	for (i = 0; i < TX_BD_NUM; i++) {
 		lp->tx_bd_v[i].next = lp->tx_bd_p +
 				sizeof(*lp->tx_bd_v) * ((i + 1) % TX_BD_NUM);
 	}
 
-	memset(lp->rx_bd_v, 0, sizeof(*lp->rx_bd_v) * RX_BD_NUM);
 	for (i = 0; i < RX_BD_NUM; i++) {
 		lp->rx_bd_v[i].next = lp->rx_bd_p +
 				sizeof(*lp->rx_bd_v) * ((i + 1) % RX_BD_NUM);
 
 		skb = netdev_alloc_skb_ip_align(ndev,
 						XTE_MAX_JUMBO_FRAME_SIZE);
-
-		if (skb == 0) {
-			dev_err(&ndev->dev, "alloc_skb error %d\n", i);
+		if (!skb)
 			goto out;
-		}
+
 		lp->rx_skb[i] = skb;
 		/* returns physical address of skb->data */
 		lp->rx_bd_v[i].phys = dma_map_single(ndev->dev.parent,
@@ -789,9 +780,7 @@ static void ll_temac_recv(struct net_device *ndev)
 
 		new_skb = netdev_alloc_skb_ip_align(ndev,
 						XTE_MAX_JUMBO_FRAME_SIZE);
-
-		if (new_skb == 0) {
-			dev_err(&ndev->dev, "no memory for new sk_buff\n");
+		if (!new_skb) {
 			spin_unlock_irqrestore(&lp->rx_lock, flags);
 			return;
 		}
@@ -1029,9 +1018,9 @@ static int temac_of_probe(struct platform_device *op)
 	ndev->features |= NETIF_F_HW_CSUM; /* Can checksum all the packets. */
 	ndev->features |= NETIF_F_IPV6_CSUM; /* Can checksum IPV6 TCP/UDP */
 	ndev->features |= NETIF_F_HIGHDMA; /* Can DMA to high memory. */
-	ndev->features |= NETIF_F_HW_VLAN_TX; /* Transmit VLAN hw accel */
-	ndev->features |= NETIF_F_HW_VLAN_RX; /* Receive VLAN hw acceleration */
-	ndev->features |= NETIF_F_HW_VLAN_FILTER; /* Receive VLAN filtering */
+	ndev->features |= NETIF_F_HW_VLAN_CTAG_TX; /* Transmit VLAN hw accel */
+	ndev->features |= NETIF_F_HW_VLAN_CTAG_RX; /* Receive VLAN hw acceleration */
+	ndev->features |= NETIF_F_HW_VLAN_CTAG_FILTER; /* Receive VLAN filtering */
 	ndev->features |= NETIF_F_VLAN_CHALLENGED; /* cannot handle VLAN pkts */
 	ndev->features |= NETIF_F_GSO; /* Enable software GSO. */
 	ndev->features |= NETIF_F_MULTI_QUEUE; /* Has multiple TX/RX queues */
diff --git a/drivers/net/ethernet/xilinx/xilinx_axienet_main.c b/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
index 278c9db3b5b8..24748e8367a1 100644
--- a/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
+++ b/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
@@ -204,41 +204,31 @@ static int axienet_dma_bd_init(struct net_device *ndev)
 	lp->tx_bd_v = dma_alloc_coherent(ndev->dev.parent,
 					 sizeof(*lp->tx_bd_v) * TX_BD_NUM,
 					 &lp->tx_bd_p,
-					 GFP_KERNEL);
-	if (!lp->tx_bd_v) {
-		dev_err(&ndev->dev, "unable to allocate DMA Tx buffer "
-			"descriptors");
+					 GFP_KERNEL | __GFP_ZERO);
+	if (!lp->tx_bd_v)
 		goto out;
-	}
 
 	lp->rx_bd_v = dma_alloc_coherent(ndev->dev.parent,
 					 sizeof(*lp->rx_bd_v) * RX_BD_NUM,
 					 &lp->rx_bd_p,
-					 GFP_KERNEL);
-	if (!lp->rx_bd_v) {
-		dev_err(&ndev->dev, "unable to allocate DMA Rx buffer "
-			"descriptors");
+					 GFP_KERNEL | __GFP_ZERO);
+	if (!lp->rx_bd_v)
 		goto out;
-	}
 
-	memset(lp->tx_bd_v, 0, sizeof(*lp->tx_bd_v) * TX_BD_NUM);
 	for (i = 0; i < TX_BD_NUM; i++) {
 		lp->tx_bd_v[i].next = lp->tx_bd_p +
 				      sizeof(*lp->tx_bd_v) *
 				      ((i + 1) % TX_BD_NUM);
 	}
 
-	memset(lp->rx_bd_v, 0, sizeof(*lp->rx_bd_v) * RX_BD_NUM);
 	for (i = 0; i < RX_BD_NUM; i++) {
 		lp->rx_bd_v[i].next = lp->rx_bd_p +
 				      sizeof(*lp->rx_bd_v) *
 				      ((i + 1) % RX_BD_NUM);
 
 		skb = netdev_alloc_skb_ip_align(ndev, lp->max_frm_size);
-		if (!skb) {
-			dev_err(&ndev->dev, "alloc_skb error %d\n", i);
+		if (!skb)
 			goto out;
-		}
 
 		lp->rx_bd_v[i].sw_id_offset = (u32) skb;
 		lp->rx_bd_v[i].phys = dma_map_single(ndev->dev.parent,
@@ -777,10 +767,9 @@ static void axienet_recv(struct net_device *ndev)
 		packets++;
 
 		new_skb = netdev_alloc_skb_ip_align(ndev, lp->max_frm_size);
-		if (!new_skb) {
-			dev_err(&ndev->dev, "no memory for new sk_buff\n");
+		if (!new_skb)
 			return;
-		}
+
 		cur_p->phys = dma_map_single(ndev->dev.parent, new_skb->data,
 					     lp->max_frm_size,
 					     DMA_FROM_DEVICE);
diff --git a/drivers/net/ethernet/xircom/xirc2ps_cs.c b/drivers/net/ethernet/xircom/xirc2ps_cs.c
index 1025b4e937d2..bdd20b888cf6 100644
--- a/drivers/net/ethernet/xircom/xirc2ps_cs.c
+++ b/drivers/net/ethernet/xircom/xirc2ps_cs.c
@@ -1041,7 +1041,6 @@ xirc2ps_interrupt(int irq, void *dev_id)
 	    /* 1 extra so we can use insw */
 	    skb = netdev_alloc_skb(dev, pktlen + 3);
 	    if (!skb) {
-		pr_notice("low memory, packet dropped (size=%u)\n", pktlen);
 		dev->stats.rx_dropped++;
 	    } else { /* okay get the packet */
 		skb_reserve(skb, 2);
diff --git a/drivers/net/fddi/defxx.c b/drivers/net/fddi/defxx.c
index 502c8ff1d985..4c8ddc944d51 100644
--- a/drivers/net/fddi/defxx.c
+++ b/drivers/net/fddi/defxx.c
@@ -1070,13 +1070,10 @@ static int dfx_driver_init(struct net_device *dev, const char *print_name,
 					(PI_ALIGN_K_DESC_BLK - 1);
 	bp->kmalloced = top_v = dma_alloc_coherent(bp->bus_dev, alloc_size,
 						   &bp->kmalloced_dma,
-						   GFP_ATOMIC);
-	if (top_v == NULL) {
-		printk("%s: Could not allocate memory for host buffers "
-		       "and structures!\n", print_name);
+						   GFP_ATOMIC | __GFP_ZERO);
+	if (top_v == NULL)
 		return DFX_K_FAILURE;
-	}
-	memset(top_v, 0, alloc_size);	/* zero out memory before continuing */
+
 	top_p = bp->kmalloced_dma;	/* get physical address of buffer */
 
 	/*
diff --git a/drivers/net/hamradio/baycom_epp.c b/drivers/net/hamradio/baycom_epp.c
index 49b8b58fc5c6..484f77ec2ce1 100644
--- a/drivers/net/hamradio/baycom_epp.c
+++ b/drivers/net/hamradio/baycom_epp.c
@@ -449,7 +449,7 @@ static int transmit(struct baycom_state *bc, int cnt, unsigned char stat)
 			if ((--bc->hdlctx.slotcnt) > 0)
 				return 0;
 			bc->hdlctx.slotcnt = bc->ch_params.slottime;
-			if ((random32() % 256) > bc->ch_params.ppersist)
+			if ((prandom_u32() % 256) > bc->ch_params.ppersist)
 				return 0;
 		}
 	}
diff --git a/drivers/net/hamradio/hdlcdrv.c b/drivers/net/hamradio/hdlcdrv.c
index a4a3516b6bbf..3169252613fa 100644
--- a/drivers/net/hamradio/hdlcdrv.c
+++ b/drivers/net/hamradio/hdlcdrv.c
@@ -389,7 +389,7 @@ void hdlcdrv_arbitrate(struct net_device *dev, struct hdlcdrv_state *s)
 	if ((--s->hdlctx.slotcnt) > 0)
 		return;
 	s->hdlctx.slotcnt = s->ch_params.slottime;
-	if ((random32() % 256) > s->ch_params.ppersist)
+	if ((prandom_u32() % 256) > s->ch_params.ppersist)
 		return;
 	start_tx(dev, s);
 }
diff --git a/drivers/net/hamradio/yam.c b/drivers/net/hamradio/yam.c
index 4cf8f1017aad..0721e72f9299 100644
--- a/drivers/net/hamradio/yam.c
+++ b/drivers/net/hamradio/yam.c
@@ -638,7 +638,7 @@ static void yam_arbitrate(struct net_device *dev)
 	yp->slotcnt = yp->slot / 10;
 
 	/* is random > persist ? */
-	if ((random32() % 256) > yp->pers)
+	if ((prandom_u32() % 256) > yp->pers)
 		return;
 
 	yam_start_tx(dev, yp);
@@ -866,7 +866,7 @@ static int yam_open(struct net_device *dev)
 
 	printk(KERN_INFO "Trying %s at iobase 0x%lx irq %u\n", dev->name, dev->base_addr, dev->irq);
 
-	if (!dev || !yp->bitrate)
+	if (!yp->bitrate)
 		return -ENXIO;
 	if (!dev->base_addr || dev->base_addr > 0x1000 - YAM_EXTENT ||
 		dev->irq < 2 || dev->irq > 15) {
diff --git a/drivers/net/hyperv/netvsc.c b/drivers/net/hyperv/netvsc.c
index f5f0f09e4cc5..2b0480416b31 100644
--- a/drivers/net/hyperv/netvsc.c
+++ b/drivers/net/hyperv/netvsc.c
@@ -522,7 +522,7 @@ int netvsc_send(struct hv_device *device,
 	sendMessage.msg.v1_msg.send_rndis_pkt.send_buf_section_size = 0;
 
 	if (packet->completion.send.send_completion)
-		req_id = (u64)packet;
+		req_id = (ulong)packet;
 	else
 		req_id = 0;
 
diff --git a/drivers/net/hyperv/netvsc_drv.c b/drivers/net/hyperv/netvsc_drv.c
index 8341b62e5521..088c55496191 100644
--- a/drivers/net/hyperv/netvsc_drv.c
+++ b/drivers/net/hyperv/netvsc_drv.c
@@ -429,7 +429,7 @@ static int netvsc_probe(struct hv_device *dev,
 
 	/* TODO: Add GSO and Checksum offload */
 	net->hw_features = NETIF_F_SG;
-	net->features = NETIF_F_SG | NETIF_F_HW_VLAN_TX;
+	net->features = NETIF_F_SG | NETIF_F_HW_VLAN_CTAG_TX;
 
 	SET_ETHTOOL_OPS(net, &ethtool_ops);
 	SET_NETDEV_DEV(net, &dev->device);
diff --git a/drivers/net/ieee802154/at86rf230.c b/drivers/net/ieee802154/at86rf230.c
index fc1687ea4a42..6f10b4964726 100644
--- a/drivers/net/ieee802154/at86rf230.c
+++ b/drivers/net/ieee802154/at86rf230.c
@@ -51,7 +51,7 @@ struct at86rf230_local {
 	struct ieee802154_dev *dev;
 
 	spinlock_t lock;
-	bool irq_disabled;
+	bool irq_busy;
 	bool is_tx;
 };
 
@@ -219,6 +219,9 @@ struct at86rf230_local {
 #define IRQ_PLL_UNL	(1 << 1)
 #define IRQ_PLL_LOCK	(1 << 0)
 
+#define IRQ_ACTIVE_HIGH	0
+#define IRQ_ACTIVE_LOW	1
+
 #define STATE_P_ON		0x00	/* BUSY */
 #define STATE_BUSY_RX		0x01
 #define STATE_BUSY_TX		0x02
@@ -233,8 +236,8 @@ struct at86rf230_local {
 #define STATE_SLEEP		0x0F
 #define STATE_BUSY_RX_AACK	0x11
 #define STATE_BUSY_TX_ARET	0x12
-#define STATE_BUSY_RX_AACK_ON	0x16
-#define STATE_BUSY_TX_ARET_ON	0x19
+#define STATE_RX_AACK_ON	0x16
+#define STATE_TX_ARET_ON	0x19
 #define STATE_RX_ON_NOCLK	0x1C
 #define STATE_RX_AACK_ON_NOCLK	0x1D
 #define STATE_BUSY_RX_AACK_NOCLK 0x1E
@@ -544,7 +547,7 @@ at86rf230_xmit(struct ieee802154_dev *dev, struct sk_buff *skb)
 	unsigned long flags;
 
 	spin_lock(&lp->lock);
-	if  (lp->irq_disabled) {
+	if  (lp->irq_busy) {
 		spin_unlock(&lp->lock);
 		return -EBUSY;
 	}
@@ -619,6 +622,52 @@ err:
 	return -EINVAL;
 }
 
+static int
+at86rf230_set_hw_addr_filt(struct ieee802154_dev *dev,
+			   struct ieee802154_hw_addr_filt *filt,
+			   unsigned long changed)
+{
+	struct at86rf230_local *lp = dev->priv;
+
+	if (changed & IEEE802515_AFILT_SADDR_CHANGED) {
+		dev_vdbg(&lp->spi->dev,
+			"at86rf230_set_hw_addr_filt called for saddr\n");
+		__at86rf230_write(lp, RG_SHORT_ADDR_0, filt->short_addr);
+		__at86rf230_write(lp, RG_SHORT_ADDR_1, filt->short_addr >> 8);
+	}
+
+	if (changed & IEEE802515_AFILT_PANID_CHANGED) {
+		dev_vdbg(&lp->spi->dev,
+			"at86rf230_set_hw_addr_filt called for pan id\n");
+		__at86rf230_write(lp, RG_PAN_ID_0, filt->pan_id);
+		__at86rf230_write(lp, RG_PAN_ID_1, filt->pan_id >> 8);
+	}
+
+	if (changed & IEEE802515_AFILT_IEEEADDR_CHANGED) {
+		dev_vdbg(&lp->spi->dev,
+			"at86rf230_set_hw_addr_filt called for IEEE addr\n");
+		at86rf230_write_subreg(lp, SR_IEEE_ADDR_0, filt->ieee_addr[7]);
+		at86rf230_write_subreg(lp, SR_IEEE_ADDR_1, filt->ieee_addr[6]);
+		at86rf230_write_subreg(lp, SR_IEEE_ADDR_2, filt->ieee_addr[5]);
+		at86rf230_write_subreg(lp, SR_IEEE_ADDR_3, filt->ieee_addr[4]);
+		at86rf230_write_subreg(lp, SR_IEEE_ADDR_4, filt->ieee_addr[3]);
+		at86rf230_write_subreg(lp, SR_IEEE_ADDR_5, filt->ieee_addr[2]);
+		at86rf230_write_subreg(lp, SR_IEEE_ADDR_6, filt->ieee_addr[1]);
+		at86rf230_write_subreg(lp, SR_IEEE_ADDR_7, filt->ieee_addr[0]);
+	}
+
+	if (changed & IEEE802515_AFILT_PANC_CHANGED) {
+		dev_vdbg(&lp->spi->dev,
+			"at86rf230_set_hw_addr_filt called for panc change\n");
+		if (filt->pan_coord)
+			at86rf230_write_subreg(lp, SR_AACK_I_AM_COORD, 1);
+		else
+			at86rf230_write_subreg(lp, SR_AACK_I_AM_COORD, 0);
+	}
+
+	return 0;
+}
+
 static struct ieee802154_ops at86rf230_ops = {
 	.owner = THIS_MODULE,
 	.xmit = at86rf230_xmit,
@@ -626,6 +675,7 @@ static struct ieee802154_ops at86rf230_ops = {
 	.set_channel = at86rf230_channel,
 	.start = at86rf230_start,
 	.stop = at86rf230_stop,
+	.set_hw_addr_filt = at86rf230_set_hw_addr_filt,
 };
 
 static void at86rf230_irqwork(struct work_struct *work)
@@ -658,8 +708,16 @@ static void at86rf230_irqwork(struct work_struct *work)
 	}
 
 	spin_lock_irqsave(&lp->lock, flags);
-	lp->irq_disabled = 0;
+	lp->irq_busy = 0;
 	spin_unlock_irqrestore(&lp->lock, flags);
+}
+
+static void at86rf230_irqwork_level(struct work_struct *work)
+{
+	struct at86rf230_local *lp =
+		container_of(work, struct at86rf230_local, irqwork);
+
+	at86rf230_irqwork(work);
 
 	enable_irq(lp->spi->irq);
 }
@@ -668,10 +726,8 @@ static irqreturn_t at86rf230_isr(int irq, void *data)
 {
 	struct at86rf230_local *lp = data;
 
-	disable_irq_nosync(irq);
-
 	spin_lock(&lp->lock);
-	lp->irq_disabled = 1;
+	lp->irq_busy = 1;
 	spin_unlock(&lp->lock);
 
 	schedule_work(&lp->irqwork);
@@ -679,11 +735,23 @@ static irqreturn_t at86rf230_isr(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
+static irqreturn_t at86rf230_isr_level(int irq, void *data)
+{
+	disable_irq_nosync(irq);
+
+	return at86rf230_isr(irq, data);
+}
+
+static int at86rf230_irq_polarity(struct at86rf230_local *lp, int pol)
+{
+	return at86rf230_write_subreg(lp, SR_IRQ_POLARITY, pol);
+}
 
 static int at86rf230_hw_init(struct at86rf230_local *lp)
 {
+	struct at86rf230_platform_data *pdata = lp->spi->dev.platform_data;
+	int rc, irq_pol;
 	u8 status;
-	int rc;
 
 	rc = at86rf230_read_subreg(lp, SR_TRX_STATUS, &status);
 	if (rc)
@@ -701,12 +769,17 @@ static int at86rf230_hw_init(struct at86rf230_local *lp)
 		dev_info(&lp->spi->dev, "Status: %02x\n", status);
 	}
 
-	rc = at86rf230_write_subreg(lp, SR_IRQ_MASK, 0xff); /* IRQ_TRX_UR |
-							     * IRQ_CCA_ED |
-							     * IRQ_TRX_END |
-							     * IRQ_PLL_UNL |
-							     * IRQ_PLL_LOCK
-							     */
+	/* configure irq polarity, defaults to high active */
+	if (pdata->irq_type & (IRQF_TRIGGER_FALLING | IRQF_TRIGGER_LOW))
+		irq_pol = IRQ_ACTIVE_LOW;
+	else
+		irq_pol = IRQ_ACTIVE_HIGH;
+
+	rc = at86rf230_irq_polarity(lp, irq_pol);
+	if (rc)
+		return rc;
+
+	rc = at86rf230_write_subreg(lp, SR_IRQ_MASK, IRQ_TRX_END);
 	if (rc)
 		return rc;
 
@@ -751,37 +824,38 @@ static int at86rf230_hw_init(struct at86rf230_local *lp)
 	return 0;
 }
 
-static int at86rf230_fill_data(struct spi_device *spi)
+static void at86rf230_fill_data(struct spi_device *spi)
 {
 	struct at86rf230_local *lp = spi_get_drvdata(spi);
 	struct at86rf230_platform_data *pdata = spi->dev.platform_data;
 
-	if (!pdata) {
-		dev_err(&spi->dev, "no platform_data\n");
-		return -EINVAL;
-	}
-
 	lp->rstn = pdata->rstn;
 	lp->slp_tr = pdata->slp_tr;
 	lp->dig2 = pdata->dig2;
-
-	return 0;
 }
 
 static int at86rf230_probe(struct spi_device *spi)
 {
+	struct at86rf230_platform_data *pdata;
 	struct ieee802154_dev *dev;
 	struct at86rf230_local *lp;
-	u8 man_id_0, man_id_1;
-	int rc;
+	u8 man_id_0, man_id_1, status;
+	irq_handler_t irq_handler;
+	work_func_t irq_worker;
+	int rc, supported = 0;
 	const char *chip;
-	int supported = 0;
 
 	if (!spi->irq) {
 		dev_err(&spi->dev, "no IRQ specified\n");
 		return -EINVAL;
 	}
 
+	pdata = spi->dev.platform_data;
+	if (!pdata) {
+		dev_err(&spi->dev, "no platform_data\n");
+		return -EINVAL;
+	}
+
 	dev = ieee802154_alloc_device(sizeof(*lp), &at86rf230_ops);
 	if (!dev)
 		return -ENOMEM;
@@ -791,23 +865,28 @@ static int at86rf230_probe(struct spi_device *spi)
 
 	lp->spi = spi;
 
-	dev->priv = lp;
 	dev->parent = &spi->dev;
 	dev->extra_tx_headroom = 0;
 	/* We do support only 2.4 Ghz */
 	dev->phy->channels_supported[0] = 0x7FFF800;
 	dev->flags = IEEE802154_HW_OMIT_CKSUM;
 
+	if (pdata->irq_type & (IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING)) {
+		irq_worker = at86rf230_irqwork;
+		irq_handler = at86rf230_isr;
+	} else {
+		irq_worker = at86rf230_irqwork_level;
+		irq_handler = at86rf230_isr_level;
+	}
+
 	mutex_init(&lp->bmux);
-	INIT_WORK(&lp->irqwork, at86rf230_irqwork);
+	INIT_WORK(&lp->irqwork, irq_worker);
 	spin_lock_init(&lp->lock);
 	init_completion(&lp->tx_complete);
 
 	spi_set_drvdata(spi, lp);
 
-	rc = at86rf230_fill_data(spi);
-	if (rc)
-		goto err_fill;
+	at86rf230_fill_data(spi);
 
 	rc = gpio_request(lp->rstn, "rstn");
 	if (rc)
@@ -882,18 +961,23 @@ static int at86rf230_probe(struct spi_device *spi)
 	if (rc)
 		goto err_gpio_dir;
 
-	rc = request_irq(spi->irq, at86rf230_isr, IRQF_SHARED,
+	rc = request_irq(spi->irq, irq_handler,
+			 IRQF_SHARED | pdata->irq_type,
 			 dev_name(&spi->dev), lp);
 	if (rc)
 		goto err_gpio_dir;
 
+	/* Read irq status register to reset irq line */
+	rc = at86rf230_read_subreg(lp, RG_IRQ_STATUS, 0xff, 0, &status);
+	if (rc)
+		goto err_irq;
+
 	rc = ieee802154_register_device(lp->dev);
 	if (rc)
 		goto err_irq;
 
 	return rc;
 
-	ieee802154_unregister_device(lp->dev);
 err_irq:
 	free_irq(spi->irq, lp);
 	flush_work(&lp->irqwork);
@@ -903,7 +987,6 @@ err_gpio_dir:
 err_slp_tr:
 	gpio_free(lp->rstn);
 err_rstn:
-err_fill:
 	spi_set_drvdata(spi, NULL);
 	mutex_destroy(&lp->bmux);
 	ieee802154_free_device(lp->dev);
diff --git a/drivers/net/ieee802154/fakehard.c b/drivers/net/ieee802154/fakehard.c
index 8f1c25676d44..bf0d55e2dd63 100644
--- a/drivers/net/ieee802154/fakehard.c
+++ b/drivers/net/ieee802154/fakehard.c
@@ -106,26 +106,6 @@ static u8 fake_get_dsn(const struct net_device *dev)
 }
 
 /**
- * fake_get_bsn - Retrieve the BSN of the device.
- * @dev: The network device to retrieve the BSN for.
- *
- * Returns the IEEE 802.15.4 BSN for the network device.
- * The BSN is the sequence number which will be added to each
- * beacon frame sent by the MAC.
- *
- * BSN means 'Beacon Sequence Number'.
- *
- * Note: This is in section 7.2.1.2 of the IEEE 802.15.4-2006
- *       document.
- */
-static u8 fake_get_bsn(const struct net_device *dev)
-{
-	BUG_ON(dev->type != ARPHRD_IEEE802154);
-
-	return 0x00; /* BSN are implemented in HW, so return just 0 */
-}
-
-/**
  * fake_assoc_req - Make an association request to the HW.
  * @dev: The network device which we are associating to a network.
  * @addr: The coordinator with which we wish to associate.
@@ -264,7 +244,6 @@ static struct ieee802154_mlme_ops fake_mlme = {
 	.get_pan_id = fake_get_pan_id,
 	.get_short_addr = fake_get_short_addr,
 	.get_dsn = fake_get_dsn,
-	.get_bsn = fake_get_bsn,
 };
 
 static int ieee802154_fake_open(struct net_device *dev)
diff --git a/drivers/net/ieee802154/mrf24j40.c b/drivers/net/ieee802154/mrf24j40.c
index 3f2c7aaf28c4..ede3ce4912f9 100644
--- a/drivers/net/ieee802154/mrf24j40.c
+++ b/drivers/net/ieee802154/mrf24j40.c
@@ -22,8 +22,10 @@
 #include <linux/spi/spi.h>
 #include <linux/interrupt.h>
 #include <linux/module.h>
+#include <linux/pinctrl/consumer.h>
 #include <net/wpan-phy.h>
 #include <net/mac802154.h>
+#include <net/ieee802154.h>
 
 /* MRF24J40 Short Address Registers */
 #define REG_RXMCR    0x00  /* Receive MAC control */
@@ -91,9 +93,8 @@ struct mrf24j40 {
 #define MRF24J40_READLONG(reg) (1 << 15 | (reg) << 5)
 #define MRF24J40_WRITELONG(reg) (1 << 15 | (reg) << 5 | 1 << 4)
 
-/* Maximum speed to run the device at. TODO: Get the real max value from
- * someone at Microchip since it isn't in the datasheet. */
-#define MAX_SPI_SPEED_HZ 1000000
+/* The datasheet indicates the theoretical maximum for SCK to be 10MHz */
+#define MAX_SPI_SPEED_HZ 10000000
 
 #define printdev(X) (&X->spi->dev)
 
@@ -349,7 +350,9 @@ static int mrf24j40_tx(struct ieee802154_dev *dev, struct sk_buff *skb)
 	if (ret)
 		goto err;
 	val |= 0x1;
-	val &= ~0x4;
+	/* Set TXNACKREQ if the ACK bit is set in the packet. */
+	if (skb->data[0] & IEEE802154_FC_ACK_REQ)
+		val |= 0x4;
 	write_short_reg(devrec, REG_TXNCON, val);
 
 	INIT_COMPLETION(devrec->tx_complete);
@@ -361,6 +364,7 @@ static int mrf24j40_tx(struct ieee802154_dev *dev, struct sk_buff *skb)
 	if (ret == -ERESTARTSYS)
 		goto err;
 	if (ret == 0) {
+		dev_warn(printdev(devrec), "Timeout waiting for TX interrupt\n");
 		ret = -ETIMEDOUT;
 		goto err;
 	}
@@ -370,7 +374,7 @@ static int mrf24j40_tx(struct ieee802154_dev *dev, struct sk_buff *skb)
 	if (ret)
 		goto err;
 	if (val & 0x1) {
-		dev_err(printdev(devrec), "Error Sending. Retry count exceeded\n");
+		dev_dbg(printdev(devrec), "Error Sending. Retry count exceeded\n");
 		ret = -ECOMM; /* TODO: Better error code ? */
 	} else
 		dev_dbg(printdev(devrec), "Packet Sent\n");
@@ -477,7 +481,7 @@ static int mrf24j40_filter(struct ieee802154_dev *dev,
 		int i;
 		for (i = 0; i < 8; i++)
 			write_short_reg(devrec, REG_EADR0+i,
-					filt->ieee_addr[i]);
+					filt->ieee_addr[7-i]);
 
 #ifdef DEBUG
 		printk(KERN_DEBUG "Set long addr to: ");
@@ -623,6 +627,7 @@ static int mrf24j40_probe(struct spi_device *spi)
 	int ret = -ENOMEM;
 	u8 val;
 	struct mrf24j40 *devrec;
+	struct pinctrl *pinctrl;
 
 	printk(KERN_INFO "mrf24j40: probe(). IRQ: %d\n", spi->irq);
 
@@ -633,6 +638,11 @@ static int mrf24j40_probe(struct spi_device *spi)
 	if (!devrec->buf)
 		goto err_buf;
 
+	pinctrl = devm_pinctrl_get_select_default(&spi->dev);
+	if (IS_ERR(pinctrl))
+		dev_warn(&spi->dev,
+			"pinctrl pins are not configured from the driver");
+
 	spi->mode = SPI_MODE_0; /* TODO: Is this appropriate for right here? */
 	if (spi->max_speed_hz > MAX_SPI_SPEED_HZ)
 		spi->max_speed_hz = MAX_SPI_SPEED_HZ;
@@ -641,7 +651,7 @@ static int mrf24j40_probe(struct spi_device *spi)
 	init_completion(&devrec->tx_complete);
 	INIT_WORK(&devrec->irqwork, mrf24j40_isrwork);
 	devrec->spi = spi;
-	dev_set_drvdata(&spi->dev, devrec);
+	spi_set_drvdata(spi, devrec);
 
 	/* Register with the 802154 subsystem */
 
@@ -713,7 +723,7 @@ err_devrec:
 
 static int mrf24j40_remove(struct spi_device *spi)
 {
-	struct mrf24j40 *devrec = dev_get_drvdata(&spi->dev);
+	struct mrf24j40 *devrec = spi_get_drvdata(spi);
 
 	dev_dbg(printdev(devrec), "remove\n");
 
@@ -725,7 +735,7 @@ static int mrf24j40_remove(struct spi_device *spi)
 	 * complete? */
 
 	/* Clean up the SPI stuff. */
-	dev_set_drvdata(&spi->dev, NULL);
+	spi_set_drvdata(spi, NULL);
 	kfree(devrec->buf);
 	kfree(devrec);
 	return 0;
@@ -749,18 +759,7 @@ static struct spi_driver mrf24j40_driver = {
 	.remove = mrf24j40_remove,
 };
 
-static int __init mrf24j40_init(void)
-{
-	return spi_register_driver(&mrf24j40_driver);
-}
-
-static void __exit mrf24j40_exit(void)
-{
-	spi_unregister_driver(&mrf24j40_driver);
-}
-
-module_init(mrf24j40_init);
-module_exit(mrf24j40_exit);
+module_spi_driver(mrf24j40_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Alan Ott");
diff --git a/drivers/net/ifb.c b/drivers/net/ifb.c
index 82164381f778..dc9f6a45515d 100644
--- a/drivers/net/ifb.c
+++ b/drivers/net/ifb.c
@@ -166,7 +166,8 @@ static const struct net_device_ops ifb_netdev_ops = {
 
 #define IFB_FEATURES (NETIF_F_HW_CSUM | NETIF_F_SG  | NETIF_F_FRAGLIST	| \
 		      NETIF_F_TSO_ECN | NETIF_F_TSO | NETIF_F_TSO6	| \
-		      NETIF_F_HIGHDMA | NETIF_F_HW_VLAN_TX)
+		      NETIF_F_HIGHDMA | NETIF_F_HW_VLAN_CTAG_TX		| \
+		      NETIF_F_HW_VLAN_STAG_TX)
 
 static void ifb_setup(struct net_device *dev)
 {
diff --git a/drivers/net/irda/ali-ircc.c b/drivers/net/irda/ali-ircc.c
index 9cea451a6081..3adb43ce138f 100644
--- a/drivers/net/irda/ali-ircc.c
+++ b/drivers/net/irda/ali-ircc.c
@@ -352,21 +352,19 @@ static int ali_ircc_open(int i, chipio_t *info)
 	/* Allocate memory if needed */
 	self->rx_buff.head =
 		dma_alloc_coherent(NULL, self->rx_buff.truesize,
-				   &self->rx_buff_dma, GFP_KERNEL);
+				   &self->rx_buff_dma, GFP_KERNEL | __GFP_ZERO);
 	if (self->rx_buff.head == NULL) {
 		err = -ENOMEM;
 		goto err_out2;
 	}
-	memset(self->rx_buff.head, 0, self->rx_buff.truesize);
 	
 	self->tx_buff.head =
 		dma_alloc_coherent(NULL, self->tx_buff.truesize,
-				   &self->tx_buff_dma, GFP_KERNEL);
+				   &self->tx_buff_dma, GFP_KERNEL | __GFP_ZERO);
 	if (self->tx_buff.head == NULL) {
 		err = -ENOMEM;
 		goto err_out3;
 	}
-	memset(self->tx_buff.head, 0, self->tx_buff.truesize);
 
 	self->rx_buff.in_frame = FALSE;
 	self->rx_buff.state = OUTSIDE_FRAME;
diff --git a/drivers/net/irda/au1k_ir.c b/drivers/net/irda/au1k_ir.c
index b5151e4ced61..7a1f684edcb5 100644
--- a/drivers/net/irda/au1k_ir.c
+++ b/drivers/net/irda/au1k_ir.c
@@ -27,6 +27,7 @@
 #include <linux/slab.h>
 #include <linux/time.h>
 #include <linux/types.h>
+#include <linux/ioport.h>
 
 #include <net/irda/irda.h>
 #include <net/irda/irmod.h>
@@ -882,12 +883,12 @@ static int au1k_irda_probe(struct platform_device *pdev)
 		goto out;
 
 	err = -EBUSY;
-	aup->ioarea = request_mem_region(r->start, r->end - r->start + 1,
+	aup->ioarea = request_mem_region(r->start, resource_size(r),
 					 pdev->name);
 	if (!aup->ioarea)
 		goto out;
 
-	aup->iobase = ioremap_nocache(r->start, r->end - r->start + 1);
+	aup->iobase = ioremap_nocache(r->start, resource_size(r));
 	if (!aup->iobase)
 		goto out2;
 
@@ -952,18 +953,7 @@ static struct platform_driver au1k_irda_driver = {
 	.remove		= au1k_irda_remove,
 };
 
-static int __init au1k_irda_load(void)
-{
-	return platform_driver_register(&au1k_irda_driver);
-}
-
-static void __exit au1k_irda_unload(void)
-{
-	return platform_driver_unregister(&au1k_irda_driver);
-}
+module_platform_driver(au1k_irda_driver);
 
 MODULE_AUTHOR("Pete Popov <ppopov@mvista.com>");
 MODULE_DESCRIPTION("Au1000 IrDA Device Driver");
-
-module_init(au1k_irda_load);
-module_exit(au1k_irda_unload);
diff --git a/drivers/net/irda/bfin_sir.c b/drivers/net/irda/bfin_sir.c
index fed4a05d55c7..22b4527321b1 100644
--- a/drivers/net/irda/bfin_sir.c
+++ b/drivers/net/irda/bfin_sir.c
@@ -389,7 +389,8 @@ static int bfin_sir_startup(struct bfin_sir_port *port, struct net_device *dev)
 	set_dma_callback(port->rx_dma_channel, bfin_sir_dma_rx_int, dev);
 	set_dma_callback(port->tx_dma_channel, bfin_sir_dma_tx_int, dev);
 
-	port->rx_dma_buf.buf = (unsigned char *)dma_alloc_coherent(NULL, PAGE_SIZE, &dma_handle, GFP_DMA);
+	port->rx_dma_buf.buf = dma_alloc_coherent(NULL, PAGE_SIZE,
+						  &dma_handle, GFP_DMA);
 	port->rx_dma_buf.head = 0;
 	port->rx_dma_buf.tail = 0;
 	port->rx_dma_nrows = 0;
@@ -608,7 +609,7 @@ static int bfin_sir_open(struct net_device *dev)
 {
 	struct bfin_sir_self *self = netdev_priv(dev);
 	struct bfin_sir_port *port = self->sir_port;
-	int err = -ENOMEM;
+	int err;
 
 	self->newspeed = 0;
 	self->speed = 9600;
@@ -622,8 +623,10 @@ static int bfin_sir_open(struct net_device *dev)
 	bfin_sir_set_speed(port, 9600);
 
 	self->irlap = irlap_open(dev, &self->qos, DRIVER_NAME);
-	if (!self->irlap)
+	if (!self->irlap) {
+		err = -ENOMEM;
 		goto err_irlap;
+	}
 
 	INIT_WORK(&self->work, bfin_sir_send_work);
 
diff --git a/drivers/net/irda/nsc-ircc.c b/drivers/net/irda/nsc-ircc.c
index 2a4f2f153244..9cf836b57c49 100644
--- a/drivers/net/irda/nsc-ircc.c
+++ b/drivers/net/irda/nsc-ircc.c
@@ -431,22 +431,20 @@ static int __init nsc_ircc_open(chipio_t *info)
 	/* Allocate memory if needed */
 	self->rx_buff.head =
 		dma_alloc_coherent(NULL, self->rx_buff.truesize,
-				   &self->rx_buff_dma, GFP_KERNEL);
+				   &self->rx_buff_dma, GFP_KERNEL | __GFP_ZERO);
 	if (self->rx_buff.head == NULL) {
 		err = -ENOMEM;
 		goto out2;
 
 	}
-	memset(self->rx_buff.head, 0, self->rx_buff.truesize);
 	
 	self->tx_buff.head =
 		dma_alloc_coherent(NULL, self->tx_buff.truesize,
-				   &self->tx_buff_dma, GFP_KERNEL);
+				   &self->tx_buff_dma, GFP_KERNEL | __GFP_ZERO);
 	if (self->tx_buff.head == NULL) {
 		err = -ENOMEM;
 		goto out3;
 	}
-	memset(self->tx_buff.head, 0, self->tx_buff.truesize);
 
 	self->rx_buff.in_frame = FALSE;
 	self->rx_buff.state = OUTSIDE_FRAME;
diff --git a/drivers/net/irda/pxaficp_ir.c b/drivers/net/irda/pxaficp_ir.c
index 858de05bdb7d..964b116a0ab7 100644
--- a/drivers/net/irda/pxaficp_ir.c
+++ b/drivers/net/irda/pxaficp_ir.c
@@ -700,12 +700,12 @@ static int pxa_irda_start(struct net_device *dev)
 
 	err = -ENOMEM;
 	si->dma_rx_buff = dma_alloc_coherent(si->dev, IRDA_FRAME_SIZE_LIMIT,
-					     &si->dma_rx_buff_phy, GFP_KERNEL );
+					     &si->dma_rx_buff_phy, GFP_KERNEL);
 	if (!si->dma_rx_buff)
 		goto err_dma_rx_buff;
 
 	si->dma_tx_buff = dma_alloc_coherent(si->dev, IRDA_FRAME_SIZE_LIMIT,
-					     &si->dma_tx_buff_phy, GFP_KERNEL );
+					     &si->dma_tx_buff_phy, GFP_KERNEL);
 	if (!si->dma_tx_buff)
 		goto err_dma_tx_buff;
 
diff --git a/drivers/net/irda/smsc-ircc2.c b/drivers/net/irda/smsc-ircc2.c
index 5290952b60c2..aa05dad75335 100644
--- a/drivers/net/irda/smsc-ircc2.c
+++ b/drivers/net/irda/smsc-ircc2.c
@@ -563,24 +563,15 @@ static int smsc_ircc_open(unsigned int fir_base, unsigned int sir_base, u8 dma,
 
 	self->rx_buff.head =
 		dma_alloc_coherent(NULL, self->rx_buff.truesize,
-				   &self->rx_buff_dma, GFP_KERNEL);
-	if (self->rx_buff.head == NULL) {
-		IRDA_ERROR("%s, Can't allocate memory for receive buffer!\n",
-			   driver_name);
+				   &self->rx_buff_dma, GFP_KERNEL | __GFP_ZERO);
+	if (self->rx_buff.head == NULL)
 		goto err_out2;
-	}
 
 	self->tx_buff.head =
 		dma_alloc_coherent(NULL, self->tx_buff.truesize,
-				   &self->tx_buff_dma, GFP_KERNEL);
-	if (self->tx_buff.head == NULL) {
-		IRDA_ERROR("%s, Can't allocate memory for transmit buffer!\n",
-			   driver_name);
+				   &self->tx_buff_dma, GFP_KERNEL | __GFP_ZERO);
+	if (self->tx_buff.head == NULL)
 		goto err_out3;
-	}
-
-	memset(self->rx_buff.head, 0, self->rx_buff.truesize);
-	memset(self->tx_buff.head, 0, self->tx_buff.truesize);
 
 	self->rx_buff.in_frame = FALSE;
 	self->rx_buff.state = OUTSIDE_FRAME;
diff --git a/drivers/net/irda/via-ircc.c b/drivers/net/irda/via-ircc.c
index f9033c6a888c..51f2bc376101 100644
--- a/drivers/net/irda/via-ircc.c
+++ b/drivers/net/irda/via-ircc.c
@@ -364,21 +364,19 @@ static int via_ircc_open(struct pci_dev *pdev, chipio_t *info, unsigned int id)
 	/* Allocate memory if needed */
 	self->rx_buff.head =
 		dma_alloc_coherent(&pdev->dev, self->rx_buff.truesize,
-				   &self->rx_buff_dma, GFP_KERNEL);
+				   &self->rx_buff_dma, GFP_KERNEL | __GFP_ZERO);
 	if (self->rx_buff.head == NULL) {
 		err = -ENOMEM;
 		goto err_out2;
 	}
-	memset(self->rx_buff.head, 0, self->rx_buff.truesize);
 
 	self->tx_buff.head =
 		dma_alloc_coherent(&pdev->dev, self->tx_buff.truesize,
-				   &self->tx_buff_dma, GFP_KERNEL);
+				   &self->tx_buff_dma, GFP_KERNEL | __GFP_ZERO);
 	if (self->tx_buff.head == NULL) {
 		err = -ENOMEM;
 		goto err_out3;
 	}
-	memset(self->tx_buff.head, 0, self->tx_buff.truesize);
 
 	self->rx_buff.in_frame = FALSE;
 	self->rx_buff.state = OUTSIDE_FRAME;
diff --git a/drivers/net/irda/vlsi_ir.c b/drivers/net/irda/vlsi_ir.c
index 2f99f8881dfc..5f4758492e4c 100644
--- a/drivers/net/irda/vlsi_ir.c
+++ b/drivers/net/irda/vlsi_ir.c
@@ -383,7 +383,7 @@ static int vlsi_seq_show(struct seq_file *seq, void *v)
 
 static int vlsi_seq_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, vlsi_seq_show, PDE(inode)->data);
+	return single_open(file, vlsi_seq_show, PDE_DATA(inode));
 }
 
 static const struct file_operations vlsi_proc_fops = {
@@ -1678,7 +1678,7 @@ vlsi_irda_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 			IRDA_WARNING("%s: failed to create proc entry\n",
 				     __func__);
 		} else {
-			ent->size = 0;
+			proc_set_size(ent, 0);
 		}
 		idev->proc_entry = ent;
 	}
diff --git a/drivers/net/irda/w83977af_ir.c b/drivers/net/irda/w83977af_ir.c
index f5bb92f15880..bb8857a158a6 100644
--- a/drivers/net/irda/w83977af_ir.c
+++ b/drivers/net/irda/w83977af_ir.c
@@ -216,22 +216,19 @@ static int w83977af_open(int i, unsigned int iobase, unsigned int irq,
 	/* Allocate memory if needed */
 	self->rx_buff.head =
 		dma_alloc_coherent(NULL, self->rx_buff.truesize,
-				   &self->rx_buff_dma, GFP_KERNEL);
+				   &self->rx_buff_dma, GFP_KERNEL | __GFP_ZERO);
 	if (self->rx_buff.head == NULL) {
 		err = -ENOMEM;
 		goto err_out1;
 	}
 
-	memset(self->rx_buff.head, 0, self->rx_buff.truesize);
-	
 	self->tx_buff.head =
 		dma_alloc_coherent(NULL, self->tx_buff.truesize,
-				   &self->tx_buff_dma, GFP_KERNEL);
+				   &self->tx_buff_dma, GFP_KERNEL | __GFP_ZERO);
 	if (self->tx_buff.head == NULL) {
 		err = -ENOMEM;
 		goto err_out2;
 	}
-	memset(self->tx_buff.head, 0, self->tx_buff.truesize);
 
 	self->rx_buff.in_frame = FALSE;
 	self->rx_buff.state = OUTSIDE_FRAME;
diff --git a/drivers/net/macvlan.c b/drivers/net/macvlan.c
index 73abbc1655d5..d5a141c7c4e7 100644
--- a/drivers/net/macvlan.c
+++ b/drivers/net/macvlan.c
@@ -46,9 +46,16 @@ struct macvlan_port {
 
 static void macvlan_port_destroy(struct net_device *dev);
 
-#define macvlan_port_get_rcu(dev) \
-	((struct macvlan_port *) rcu_dereference(dev->rx_handler_data))
-#define macvlan_port_get(dev) ((struct macvlan_port *) dev->rx_handler_data)
+static struct macvlan_port *macvlan_port_get_rcu(const struct net_device *dev)
+{
+	return rcu_dereference(dev->rx_handler_data);
+}
+
+static struct macvlan_port *macvlan_port_get_rtnl(const struct net_device *dev)
+{
+	return rtnl_dereference(dev->rx_handler_data);
+}
+
 #define macvlan_port_exists(dev) (dev->priv_flags & IFF_MACVLAN_PORT)
 
 static struct macvlan_dev *macvlan_hash_lookup(const struct macvlan_port *port,
@@ -464,7 +471,7 @@ static struct lock_class_key macvlan_netdev_addr_lock_key;
 	(NETIF_F_SG | NETIF_F_ALL_CSUM | NETIF_F_HIGHDMA | NETIF_F_FRAGLIST | \
 	 NETIF_F_GSO | NETIF_F_TSO | NETIF_F_UFO | NETIF_F_GSO_ROBUST | \
 	 NETIF_F_TSO_ECN | NETIF_F_TSO6 | NETIF_F_GRO | NETIF_F_RXCSUM | \
-	 NETIF_F_HW_VLAN_FILTER)
+	 NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_HW_VLAN_STAG_FILTER)
 
 #define MACVLAN_STATE_MASK \
 	((1<<__LINK_STATE_NOCARRIER) | (1<<__LINK_STATE_DORMANT))
@@ -560,21 +567,21 @@ static struct rtnl_link_stats64 *macvlan_dev_get_stats64(struct net_device *dev,
 }
 
 static int macvlan_vlan_rx_add_vid(struct net_device *dev,
-				    unsigned short vid)
+				   __be16 proto, u16 vid)
 {
 	struct macvlan_dev *vlan = netdev_priv(dev);
 	struct net_device *lowerdev = vlan->lowerdev;
 
-	return vlan_vid_add(lowerdev, vid);
+	return vlan_vid_add(lowerdev, proto, vid);
 }
 
 static int macvlan_vlan_rx_kill_vid(struct net_device *dev,
-				     unsigned short vid)
+				    __be16 proto, u16 vid)
 {
 	struct macvlan_dev *vlan = netdev_priv(dev);
 	struct net_device *lowerdev = vlan->lowerdev;
 
-	vlan_vid_del(lowerdev, vid);
+	vlan_vid_del(lowerdev, proto, vid);
 	return 0;
 }
 
@@ -703,7 +710,7 @@ static int macvlan_port_create(struct net_device *dev)
 
 static void macvlan_port_destroy(struct net_device *dev)
 {
-	struct macvlan_port *port = macvlan_port_get(dev);
+	struct macvlan_port *port = macvlan_port_get_rtnl(dev);
 
 	dev->priv_flags &= ~IFF_MACVLAN_PORT;
 	netdev_rx_handler_unregister(dev);
@@ -772,7 +779,7 @@ int macvlan_common_newlink(struct net *src_net, struct net_device *dev,
 		if (err < 0)
 			return err;
 	}
-	port = macvlan_port_get(lowerdev);
+	port = macvlan_port_get_rtnl(lowerdev);
 
 	/* Only 1 macvlan device can be created in passthru mode */
 	if (port->passthru)
@@ -921,7 +928,7 @@ static int macvlan_device_event(struct notifier_block *unused,
 	if (!macvlan_port_exists(dev))
 		return NOTIFY_DONE;
 
-	port = macvlan_port_get(dev);
+	port = macvlan_port_get_rtnl(dev);
 
 	switch (event) {
 	case NETDEV_CHANGE:
diff --git a/drivers/net/macvtap.c b/drivers/net/macvtap.c
index a449439bd653..59e9605de316 100644
--- a/drivers/net/macvtap.c
+++ b/drivers/net/macvtap.c
@@ -725,6 +725,8 @@ static ssize_t macvtap_get_user(struct macvtap_queue *q, struct msghdr *m,
 			goto err_kfree;
 	}
 
+	skb_probe_transport_header(skb, ETH_HLEN);
+
 	rcu_read_lock_bh();
 	vlan = rcu_dereference_bh(q->vlan);
 	/* copy skb_ubuf_info for callback when skb has no error */
diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
index 450345261bd3..1e11f2bfd9ce 100644
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -126,7 +126,7 @@ config MDIO_BITBANG
 
 config MDIO_GPIO
 	tristate "Support for GPIO lib-based bitbanged MDIO buses"
-	depends on MDIO_BITBANG && GENERIC_GPIO
+	depends on MDIO_BITBANG && GPIOLIB
 	---help---
 	  Supports GPIO lib-based MDIO busses.
 
diff --git a/drivers/net/phy/lxt.c b/drivers/net/phy/lxt.c
index ec40ba882f61..ff2e45e9cb54 100644
--- a/drivers/net/phy/lxt.c
+++ b/drivers/net/phy/lxt.c
@@ -159,7 +159,7 @@ static int lxt973a2_update_link(struct phy_device *phydev)
 	return 0;
 }
 
-int lxt973a2_read_status(struct phy_device *phydev)
+static int lxt973a2_read_status(struct phy_device *phydev)
 {
 	int adv;
 	int err;
diff --git a/drivers/net/phy/marvell.c b/drivers/net/phy/marvell.c
index 22dec9c7ef05..202fe1ff1987 100644
--- a/drivers/net/phy/marvell.c
+++ b/drivers/net/phy/marvell.c
@@ -7,6 +7,8 @@
  *
  * Copyright (c) 2004 Freescale Semiconductor, Inc.
  *
+ * Copyright (c) 2013 Michael Stapelberg <michael@stapelberg.de>
+ *
  * 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
@@ -80,6 +82,28 @@
 #define MII_88E1318S_PHY_MSCR1_REG	16
 #define MII_88E1318S_PHY_MSCR1_PAD_ODD	BIT(6)
 
+/* Copper Specific Interrupt Enable Register */
+#define MII_88E1318S_PHY_CSIER                              0x12
+/* WOL Event Interrupt Enable */
+#define MII_88E1318S_PHY_CSIER_WOL_EIE                      BIT(7)
+
+/* LED Timer Control Register */
+#define MII_88E1318S_PHY_LED_PAGE                           0x03
+#define MII_88E1318S_PHY_LED_TCR                            0x12
+#define MII_88E1318S_PHY_LED_TCR_FORCE_INT                  BIT(15)
+#define MII_88E1318S_PHY_LED_TCR_INTn_ENABLE                BIT(7)
+#define MII_88E1318S_PHY_LED_TCR_INT_ACTIVE_LOW             BIT(11)
+
+/* Magic Packet MAC address registers */
+#define MII_88E1318S_PHY_MAGIC_PACKET_WORD2                 0x17
+#define MII_88E1318S_PHY_MAGIC_PACKET_WORD1                 0x18
+#define MII_88E1318S_PHY_MAGIC_PACKET_WORD0                 0x19
+
+#define MII_88E1318S_PHY_WOL_PAGE                           0x11
+#define MII_88E1318S_PHY_WOL_CTRL                           0x10
+#define MII_88E1318S_PHY_WOL_CTRL_CLEAR_WOL_STATUS          BIT(12)
+#define MII_88E1318S_PHY_WOL_CTRL_MAGIC_PACKET_MATCH_ENABLE BIT(14)
+
 #define MII_88E1121_PHY_LED_CTRL	16
 #define MII_88E1121_PHY_LED_PAGE	3
 #define MII_88E1121_PHY_LED_DEF		0x0030
@@ -696,6 +720,107 @@ static int m88e1121_did_interrupt(struct phy_device *phydev)
 	return 0;
 }
 
+static void m88e1318_get_wol(struct phy_device *phydev, struct ethtool_wolinfo *wol)
+{
+	wol->supported = WAKE_MAGIC;
+	wol->wolopts = 0;
+
+	if (phy_write(phydev, MII_MARVELL_PHY_PAGE,
+		      MII_88E1318S_PHY_WOL_PAGE) < 0)
+		return;
+
+	if (phy_read(phydev, MII_88E1318S_PHY_WOL_CTRL) &
+	    MII_88E1318S_PHY_WOL_CTRL_MAGIC_PACKET_MATCH_ENABLE)
+		wol->wolopts |= WAKE_MAGIC;
+
+	if (phy_write(phydev, MII_MARVELL_PHY_PAGE, 0x00) < 0)
+		return;
+}
+
+static int m88e1318_set_wol(struct phy_device *phydev, struct ethtool_wolinfo *wol)
+{
+	int err, oldpage, temp;
+
+	oldpage = phy_read(phydev, MII_MARVELL_PHY_PAGE);
+
+	if (wol->wolopts & WAKE_MAGIC) {
+		/* Explicitly switch to page 0x00, just to be sure */
+		err = phy_write(phydev, MII_MARVELL_PHY_PAGE, 0x00);
+		if (err < 0)
+			return err;
+
+		/* Enable the WOL interrupt */
+		temp = phy_read(phydev, MII_88E1318S_PHY_CSIER);
+		temp |= MII_88E1318S_PHY_CSIER_WOL_EIE;
+		err = phy_write(phydev, MII_88E1318S_PHY_CSIER, temp);
+		if (err < 0)
+			return err;
+
+		err = phy_write(phydev, MII_MARVELL_PHY_PAGE,
+				MII_88E1318S_PHY_LED_PAGE);
+		if (err < 0)
+			return err;
+
+		/* Setup LED[2] as interrupt pin (active low) */
+		temp = phy_read(phydev, MII_88E1318S_PHY_LED_TCR);
+		temp &= ~MII_88E1318S_PHY_LED_TCR_FORCE_INT;
+		temp |= MII_88E1318S_PHY_LED_TCR_INTn_ENABLE;
+		temp |= MII_88E1318S_PHY_LED_TCR_INT_ACTIVE_LOW;
+		err = phy_write(phydev, MII_88E1318S_PHY_LED_TCR, temp);
+		if (err < 0)
+			return err;
+
+		err = phy_write(phydev, MII_MARVELL_PHY_PAGE,
+				MII_88E1318S_PHY_WOL_PAGE);
+		if (err < 0)
+			return err;
+
+		/* Store the device address for the magic packet */
+		err = phy_write(phydev, MII_88E1318S_PHY_MAGIC_PACKET_WORD2,
+				((phydev->attached_dev->dev_addr[5] << 8) |
+				 phydev->attached_dev->dev_addr[4]));
+		if (err < 0)
+			return err;
+		err = phy_write(phydev, MII_88E1318S_PHY_MAGIC_PACKET_WORD1,
+				((phydev->attached_dev->dev_addr[3] << 8) |
+				 phydev->attached_dev->dev_addr[2]));
+		if (err < 0)
+			return err;
+		err = phy_write(phydev, MII_88E1318S_PHY_MAGIC_PACKET_WORD0,
+				((phydev->attached_dev->dev_addr[1] << 8) |
+				 phydev->attached_dev->dev_addr[0]));
+		if (err < 0)
+			return err;
+
+		/* Clear WOL status and enable magic packet matching */
+		temp = phy_read(phydev, MII_88E1318S_PHY_WOL_CTRL);
+		temp |= MII_88E1318S_PHY_WOL_CTRL_CLEAR_WOL_STATUS;
+		temp |= MII_88E1318S_PHY_WOL_CTRL_MAGIC_PACKET_MATCH_ENABLE;
+		err = phy_write(phydev, MII_88E1318S_PHY_WOL_CTRL, temp);
+		if (err < 0)
+			return err;
+	} else {
+		err = phy_write(phydev, MII_MARVELL_PHY_PAGE,
+				MII_88E1318S_PHY_WOL_PAGE);
+		if (err < 0)
+			return err;
+
+		/* Clear WOL status and disable magic packet matching */
+		temp = phy_read(phydev, MII_88E1318S_PHY_WOL_CTRL);
+		temp |= MII_88E1318S_PHY_WOL_CTRL_CLEAR_WOL_STATUS;
+		temp &= ~MII_88E1318S_PHY_WOL_CTRL_MAGIC_PACKET_MATCH_ENABLE;
+		err = phy_write(phydev, MII_88E1318S_PHY_WOL_CTRL, temp);
+		if (err < 0)
+			return err;
+	}
+
+	err = phy_write(phydev, MII_MARVELL_PHY_PAGE, oldpage);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+
 static struct phy_driver marvell_drivers[] = {
 	{
 		.phy_id = MARVELL_PHY_ID_88E1101,
@@ -772,6 +897,8 @@ static struct phy_driver marvell_drivers[] = {
 		.ack_interrupt = &marvell_ack_interrupt,
 		.config_intr = &marvell_config_intr,
 		.did_interrupt = &m88e1121_did_interrupt,
+		.get_wol = &m88e1318_get_wol,
+		.set_wol = &m88e1318_set_wol,
 		.driver = { .owner = THIS_MODULE },
 	},
 	{
diff --git a/drivers/net/phy/mdio-gpio.c b/drivers/net/phy/mdio-gpio.c
index 27274986ab56..a47f9236d966 100644
--- a/drivers/net/phy/mdio-gpio.c
+++ b/drivers/net/phy/mdio-gpio.c
@@ -235,17 +235,7 @@ static struct platform_driver mdio_gpio_driver = {
 	},
 };
 
-static int __init mdio_gpio_init(void)
-{
-	return platform_driver_register(&mdio_gpio_driver);
-}
-module_init(mdio_gpio_init);
-
-static void __exit mdio_gpio_exit(void)
-{
-	platform_driver_unregister(&mdio_gpio_driver);
-}
-module_exit(mdio_gpio_exit);
+module_platform_driver(mdio_gpio_driver);
 
 MODULE_ALIAS("platform:mdio-gpio");
 MODULE_AUTHOR("Laurent Pinchart, Paulius Zaleckas");
diff --git a/drivers/net/phy/mdio-octeon.c b/drivers/net/phy/mdio-octeon.c
index 09297fe05ae5..b51fa1f469b0 100644
--- a/drivers/net/phy/mdio-octeon.c
+++ b/drivers/net/phy/mdio-octeon.c
@@ -3,7 +3,7 @@
  * License.  See the file "COPYING" in the main directory of this archive
  * for more details.
  *
- * Copyright (C) 2009,2011 Cavium, Inc.
+ * Copyright (C) 2009-2012 Cavium, Inc.
  */
 
 #include <linux/platform_device.h>
@@ -27,30 +27,98 @@
 #define SMI_CLK		0x18
 #define SMI_EN		0x20
 
+enum octeon_mdiobus_mode {
+	UNINIT = 0,
+	C22,
+	C45
+};
+
 struct octeon_mdiobus {
 	struct mii_bus *mii_bus;
 	u64 register_base;
 	resource_size_t mdio_phys;
 	resource_size_t regsize;
+	enum octeon_mdiobus_mode mode;
 	int phy_irq[PHY_MAX_ADDR];
 };
 
+static void octeon_mdiobus_set_mode(struct octeon_mdiobus *p,
+				    enum octeon_mdiobus_mode m)
+{
+	union cvmx_smix_clk smi_clk;
+
+	if (m == p->mode)
+		return;
+
+	smi_clk.u64 = cvmx_read_csr(p->register_base + SMI_CLK);
+	smi_clk.s.mode = (m == C45) ? 1 : 0;
+	smi_clk.s.preamble = 1;
+	cvmx_write_csr(p->register_base + SMI_CLK, smi_clk.u64);
+	p->mode = m;
+}
+
+static int octeon_mdiobus_c45_addr(struct octeon_mdiobus *p,
+				   int phy_id, int regnum)
+{
+	union cvmx_smix_cmd smi_cmd;
+	union cvmx_smix_wr_dat smi_wr;
+	int timeout = 1000;
+
+	octeon_mdiobus_set_mode(p, C45);
+
+	smi_wr.u64 = 0;
+	smi_wr.s.dat = regnum & 0xffff;
+	cvmx_write_csr(p->register_base + SMI_WR_DAT, smi_wr.u64);
+
+	regnum = (regnum >> 16) & 0x1f;
+
+	smi_cmd.u64 = 0;
+	smi_cmd.s.phy_op = 0; /* MDIO_CLAUSE_45_ADDRESS */
+	smi_cmd.s.phy_adr = phy_id;
+	smi_cmd.s.reg_adr = regnum;
+	cvmx_write_csr(p->register_base + SMI_CMD, smi_cmd.u64);
+
+	do {
+		/* Wait 1000 clocks so we don't saturate the RSL bus
+		 * doing reads.
+		 */
+		__delay(1000);
+		smi_wr.u64 = cvmx_read_csr(p->register_base + SMI_WR_DAT);
+	} while (smi_wr.s.pending && --timeout);
+
+	if (timeout <= 0)
+		return -EIO;
+	return 0;
+}
+
 static int octeon_mdiobus_read(struct mii_bus *bus, int phy_id, int regnum)
 {
 	struct octeon_mdiobus *p = bus->priv;
 	union cvmx_smix_cmd smi_cmd;
 	union cvmx_smix_rd_dat smi_rd;
+	unsigned int op = 1; /* MDIO_CLAUSE_22_READ */
 	int timeout = 1000;
 
+	if (regnum & MII_ADDR_C45) {
+		int r = octeon_mdiobus_c45_addr(p, phy_id, regnum);
+		if (r < 0)
+			return r;
+
+		regnum = (regnum >> 16) & 0x1f;
+		op = 3; /* MDIO_CLAUSE_45_READ */
+	} else {
+		octeon_mdiobus_set_mode(p, C22);
+	}
+
+
 	smi_cmd.u64 = 0;
-	smi_cmd.s.phy_op = 1; /* MDIO_CLAUSE_22_READ */
+	smi_cmd.s.phy_op = op;
 	smi_cmd.s.phy_adr = phy_id;
 	smi_cmd.s.reg_adr = regnum;
 	cvmx_write_csr(p->register_base + SMI_CMD, smi_cmd.u64);
 
 	do {
-		/*
-		 * Wait 1000 clocks so we don't saturate the RSL bus
+		/* Wait 1000 clocks so we don't saturate the RSL bus
 		 * doing reads.
 		 */
 		__delay(1000);
@@ -69,21 +137,33 @@ static int octeon_mdiobus_write(struct mii_bus *bus, int phy_id,
 	struct octeon_mdiobus *p = bus->priv;
 	union cvmx_smix_cmd smi_cmd;
 	union cvmx_smix_wr_dat smi_wr;
+	unsigned int op = 0; /* MDIO_CLAUSE_22_WRITE */
 	int timeout = 1000;
 
+
+	if (regnum & MII_ADDR_C45) {
+		int r = octeon_mdiobus_c45_addr(p, phy_id, regnum);
+		if (r < 0)
+			return r;
+
+		regnum = (regnum >> 16) & 0x1f;
+		op = 1; /* MDIO_CLAUSE_45_WRITE */
+	} else {
+		octeon_mdiobus_set_mode(p, C22);
+	}
+
 	smi_wr.u64 = 0;
 	smi_wr.s.dat = val;
 	cvmx_write_csr(p->register_base + SMI_WR_DAT, smi_wr.u64);
 
 	smi_cmd.u64 = 0;
-	smi_cmd.s.phy_op = 0; /* MDIO_CLAUSE_22_WRITE */
+	smi_cmd.s.phy_op = op;
 	smi_cmd.s.phy_adr = phy_id;
 	smi_cmd.s.reg_adr = regnum;
 	cvmx_write_csr(p->register_base + SMI_CMD, smi_cmd.u64);
 
 	do {
-		/*
-		 * Wait 1000 clocks so we don't saturate the RSL bus
+		/* Wait 1000 clocks so we don't saturate the RSL bus
 		 * doing reads.
 		 */
 		__delay(1000);
@@ -197,18 +277,7 @@ void octeon_mdiobus_force_mod_depencency(void)
 }
 EXPORT_SYMBOL(octeon_mdiobus_force_mod_depencency);
 
-static int __init octeon_mdiobus_mod_init(void)
-{
-	return platform_driver_register(&octeon_mdiobus_driver);
-}
-
-static void __exit octeon_mdiobus_mod_exit(void)
-{
-	platform_driver_unregister(&octeon_mdiobus_driver);
-}
-
-module_init(octeon_mdiobus_mod_init);
-module_exit(octeon_mdiobus_mod_exit);
+module_platform_driver(octeon_mdiobus_driver);
 
 MODULE_DESCRIPTION(DRV_DESCRIPTION);
 MODULE_VERSION(DRV_VERSION);
diff --git a/drivers/net/phy/micrel.c b/drivers/net/phy/micrel.c
index abf7b6153d00..2510435f34ed 100644
--- a/drivers/net/phy/micrel.c
+++ b/drivers/net/phy/micrel.c
@@ -53,6 +53,18 @@
 #define KS8737_CTRL_INT_ACTIVE_HIGH		(1 << 14)
 #define KSZ8051_RMII_50MHZ_CLK			(1 << 7)
 
+static int ksz_config_flags(struct phy_device *phydev)
+{
+	int regval;
+
+	if (phydev->dev_flags & MICREL_PHY_50MHZ_CLK) {
+		regval = phy_read(phydev, MII_KSZPHY_CTRL);
+		regval |= KSZ8051_RMII_50MHZ_CLK;
+		return phy_write(phydev, MII_KSZPHY_CTRL, regval);
+	}
+	return 0;
+}
+
 static int kszphy_ack_interrupt(struct phy_device *phydev)
 {
 	/* bit[7..0] int status, which is a read and clear register. */
@@ -114,22 +126,19 @@ static int kszphy_config_init(struct phy_device *phydev)
 
 static int ksz8021_config_init(struct phy_device *phydev)
 {
+	int rc;
 	const u16 val = KSZPHY_OMSO_B_CAST_OFF | KSZPHY_OMSO_RMII_OVERRIDE;
 	phy_write(phydev, MII_KSZPHY_OMSO, val);
-	return 0;
+	rc = ksz_config_flags(phydev);
+	return rc < 0 ? rc : 0;
 }
 
 static int ks8051_config_init(struct phy_device *phydev)
 {
-	int regval;
-
-	if (phydev->dev_flags & MICREL_PHY_50MHZ_CLK) {
-		regval = phy_read(phydev, MII_KSZPHY_CTRL);
-		regval |= KSZ8051_RMII_50MHZ_CLK;
-		phy_write(phydev, MII_KSZPHY_CTRL, regval);
-	}
+	int rc;
 
-	return 0;
+	rc = ksz_config_flags(phydev);
+	return rc < 0 ? rc : 0;
 }
 
 #define KSZ8873MLL_GLOBAL_CONTROL_4	0x06
@@ -192,6 +201,19 @@ static struct phy_driver ksphy_driver[] = {
 	.config_intr	= kszphy_config_intr,
 	.driver		= { .owner = THIS_MODULE,},
 }, {
+	.phy_id		= PHY_ID_KSZ8031,
+	.phy_id_mask	= 0x00ffffff,
+	.name		= "Micrel KSZ8031",
+	.features	= (PHY_BASIC_FEATURES | SUPPORTED_Pause |
+			   SUPPORTED_Asym_Pause),
+	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.config_init	= ksz8021_config_init,
+	.config_aneg	= genphy_config_aneg,
+	.read_status	= genphy_read_status,
+	.ack_interrupt	= kszphy_ack_interrupt,
+	.config_intr	= kszphy_config_intr,
+	.driver		= { .owner = THIS_MODULE,},
+}, {
 	.phy_id		= PHY_ID_KSZ8041,
 	.phy_id_mask	= 0x00fffff0,
 	.name		= "Micrel KSZ8041",
@@ -325,6 +347,7 @@ static struct mdio_device_id __maybe_unused micrel_tbl[] = {
 	{ PHY_ID_KSZ8001, 0x00ffffff },
 	{ PHY_ID_KS8737, 0x00fffff0 },
 	{ PHY_ID_KSZ8021, 0x00ffffff },
+	{ PHY_ID_KSZ8031, 0x00ffffff },
 	{ PHY_ID_KSZ8041, 0x00fffff0 },
 	{ PHY_ID_KSZ8051, 0x00fffff0 },
 	{ PHY_ID_KSZ8061, 0x00fffff0 },
diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index ef9ea9248223..c14f14741b3f 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -463,33 +463,6 @@ void phy_stop_machine(struct phy_device *phydev)
 }
 
 /**
- * phy_force_reduction - reduce PHY speed/duplex settings by one step
- * @phydev: target phy_device struct
- *
- * Description: Reduces the speed/duplex settings by one notch,
- *   in this order--
- *   1000/FULL, 1000/HALF, 100/FULL, 100/HALF, 10/FULL, 10/HALF.
- *   The function bottoms out at 10/HALF.
- */
-static void phy_force_reduction(struct phy_device *phydev)
-{
-	int idx;
-
-	idx = phy_find_setting(phydev->speed, phydev->duplex);
-	
-	idx++;
-
-	idx = phy_find_valid(idx, phydev->supported);
-
-	phydev->speed = settings[idx].speed;
-	phydev->duplex = settings[idx].duplex;
-
-	pr_info("Trying %d/%s\n",
-		phydev->speed, DUPLEX_FULL == phydev->duplex ? "FULL" : "HALF");
-}
-
-
-/**
  * phy_error - enter HALTED state for this PHY device
  * @phydev: target phy_device struct
  *
@@ -818,30 +791,11 @@ void phy_state_machine(struct work_struct *work)
 				phydev->adjust_link(phydev->attached_dev);
 
 			} else if (0 == phydev->link_timeout--) {
-				int idx;
-
 				needs_aneg = 1;
 				/* If we have the magic_aneg bit,
 				 * we try again */
 				if (phydev->drv->flags & PHY_HAS_MAGICANEG)
 					break;
-
-				/* The timer expired, and we still
-				 * don't have a setting, so we try
-				 * forcing it until we find one that
-				 * works, starting from the fastest speed,
-				 * and working our way down */
-				idx = phy_find_valid(0, phydev->supported);
-
-				phydev->speed = settings[idx].speed;
-				phydev->duplex = settings[idx].duplex;
-
-				phydev->autoneg = AUTONEG_DISABLE;
-
-				pr_info("Trying %d/%s\n",
-					phydev->speed,
-					DUPLEX_FULL == phydev->duplex ?
-					"FULL" : "HALF");
 			}
 			break;
 		case PHY_NOLINK:
@@ -866,10 +820,8 @@ void phy_state_machine(struct work_struct *work)
 				phydev->state = PHY_RUNNING;
 				netif_carrier_on(phydev->attached_dev);
 			} else {
-				if (0 == phydev->link_timeout--) {
-					phy_force_reduction(phydev);
+				if (0 == phydev->link_timeout--)
 					needs_aneg = 1;
-				}
 			}
 
 			phydev->adjust_link(phydev->attached_dev);
@@ -1188,3 +1140,19 @@ int phy_ethtool_set_eee(struct phy_device *phydev, struct ethtool_eee *data)
 	return 0;
 }
 EXPORT_SYMBOL(phy_ethtool_set_eee);
+
+int phy_ethtool_set_wol(struct phy_device *phydev, struct ethtool_wolinfo *wol)
+{
+	if (phydev->drv->set_wol)
+		return phydev->drv->set_wol(phydev, wol);
+
+	return -EOPNOTSUPP;
+}
+EXPORT_SYMBOL(phy_ethtool_set_wol);
+
+void phy_ethtool_get_wol(struct phy_device *phydev, struct ethtool_wolinfo *wol)
+{
+	if (phydev->drv->get_wol)
+		phydev->drv->get_wol(phydev, wol);
+}
+EXPORT_SYMBOL(phy_ethtool_get_wol);
diff --git a/drivers/net/phy/spi_ks8995.c b/drivers/net/phy/spi_ks8995.c
index 5c87eef40bf9..d11c93e69e03 100644
--- a/drivers/net/phy/spi_ks8995.c
+++ b/drivers/net/phy/spi_ks8995.c
@@ -281,7 +281,7 @@ static int ks8995_probe(struct spi_device *spi)
 	mutex_init(&ks->lock);
 	ks->pdata = pdata;
 	ks->spi = spi_dev_get(spi);
-	dev_set_drvdata(&spi->dev, ks);
+	spi_set_drvdata(spi, ks);
 
 	spi->mode = SPI_MODE_0;
 	spi->bits_per_word = 8;
@@ -325,7 +325,7 @@ static int ks8995_probe(struct spi_device *spi)
 	return 0;
 
 err_drvdata:
-	dev_set_drvdata(&spi->dev, NULL);
+	spi_set_drvdata(spi, NULL);
 	kfree(ks);
 	return err;
 }
@@ -334,10 +334,10 @@ static int ks8995_remove(struct spi_device *spi)
 {
 	struct ks8995_data      *ks8995;
 
-	ks8995 = dev_get_drvdata(&spi->dev);
+	ks8995 = spi_get_drvdata(spi);
 	sysfs_remove_bin_file(&spi->dev.kobj, &ks8995_registers_attr);
 
-	dev_set_drvdata(&spi->dev, NULL);
+	spi_set_drvdata(spi, NULL);
 	kfree(ks8995);
 
 	return 0;
diff --git a/drivers/net/phy/vitesse.c b/drivers/net/phy/vitesse.c
index 2585c383e623..3492b5391273 100644
--- a/drivers/net/phy/vitesse.c
+++ b/drivers/net/phy/vitesse.c
@@ -61,7 +61,7 @@ MODULE_DESCRIPTION("Vitesse PHY driver");
 MODULE_AUTHOR("Kriston Carson");
 MODULE_LICENSE("GPL");
 
-int vsc824x_add_skew(struct phy_device *phydev)
+static int vsc824x_add_skew(struct phy_device *phydev)
 {
 	int err;
 	int extcon;
@@ -81,7 +81,6 @@ int vsc824x_add_skew(struct phy_device *phydev)
 
 	return err;
 }
-EXPORT_SYMBOL(vsc824x_add_skew);
 
 static int vsc824x_config_init(struct phy_device *phydev)
 {
diff --git a/drivers/net/plip/plip.c b/drivers/net/plip/plip.c
index bed62d9c53c8..1f7bef90b467 100644
--- a/drivers/net/plip/plip.c
+++ b/drivers/net/plip/plip.c
@@ -560,7 +560,7 @@ static __be16 plip_type_trans(struct sk_buff *skb, struct net_device *dev)
 	 *	so don't forget to remove it.
 	 */
 
-	if (ntohs(eth->h_proto) >= 1536)
+	if (ntohs(eth->h_proto) >= ETH_P_802_3_MIN)
 		return eth->h_proto;
 
 	rawp = skb->data;
diff --git a/drivers/net/ppp/ppp_synctty.c b/drivers/net/ppp/ppp_synctty.c
index bdf3b13a71a8..925d3e295bac 100644
--- a/drivers/net/ppp/ppp_synctty.c
+++ b/drivers/net/ppp/ppp_synctty.c
@@ -105,64 +105,15 @@ static const struct ppp_channel_ops sync_ops = {
 };
 
 /*
- * Utility procedures to print a buffer in hex/ascii
+ * Utility procedure to print a buffer in hex/ascii
  */
 static void
-ppp_print_hex (register __u8 * out, const __u8 * in, int count)
-{
-	register __u8 next_ch;
-	static const char hex[] = "0123456789ABCDEF";
-
-	while (count-- > 0) {
-		next_ch = *in++;
-		*out++ = hex[(next_ch >> 4) & 0x0F];
-		*out++ = hex[next_ch & 0x0F];
-		++out;
-	}
-}
-
-static void
-ppp_print_char (register __u8 * out, const __u8 * in, int count)
-{
-	register __u8 next_ch;
-
-	while (count-- > 0) {
-		next_ch = *in++;
-
-		if (next_ch < 0x20 || next_ch > 0x7e)
-			*out++ = '.';
-		else {
-			*out++ = next_ch;
-			if (next_ch == '%')   /* printk/syslogd has a bug !! */
-				*out++ = '%';
-		}
-	}
-	*out = '\0';
-}
-
-static void
 ppp_print_buffer (const char *name, const __u8 *buf, int count)
 {
-	__u8 line[44];
-
 	if (name != NULL)
 		printk(KERN_DEBUG "ppp_synctty: %s, count = %d\n", name, count);
 
-	while (count > 8) {
-		memset (line, 32, 44);
-		ppp_print_hex (line, buf, 8);
-		ppp_print_char (&line[8 * 3], buf, 8);
-		printk(KERN_DEBUG "%s\n", line);
-		count -= 8;
-		buf += 8;
-	}
-
-	if (count > 0) {
-		memset (line, 32, 44);
-		ppp_print_hex (line, buf, count);
-		ppp_print_char (&line[8 * 3], buf, count);
-		printk(KERN_DEBUG "%s\n", line);
-	}
+	print_hex_dump_bytes("", DUMP_PREFIX_NONE, buf, count);
 }
 
 
diff --git a/drivers/net/team/Kconfig b/drivers/net/team/Kconfig
index c3011af68e91..c853d84fd99f 100644
--- a/drivers/net/team/Kconfig
+++ b/drivers/net/team/Kconfig
@@ -37,6 +37,18 @@ config NET_TEAM_MODE_ROUNDROBIN
 	  To compile this team mode as a module, choose M here: the module
 	  will be called team_mode_roundrobin.
 
+config NET_TEAM_MODE_RANDOM
+	tristate "Random mode support"
+	depends on NET_TEAM
+	---help---
+	  Basic mode where port used for transmitting packets is selected
+	  randomly.
+
+	  All added ports are setup to have team's device address.
+
+	  To compile this team mode as a module, choose M here: the module
+	  will be called team_mode_random.
+
 config NET_TEAM_MODE_ACTIVEBACKUP
 	tristate "Active-backup mode support"
 	depends on NET_TEAM
diff --git a/drivers/net/team/Makefile b/drivers/net/team/Makefile
index 975763014e5a..c57e85889751 100644
--- a/drivers/net/team/Makefile
+++ b/drivers/net/team/Makefile
@@ -5,5 +5,6 @@
 obj-$(CONFIG_NET_TEAM) += team.o
 obj-$(CONFIG_NET_TEAM_MODE_BROADCAST) += team_mode_broadcast.o
 obj-$(CONFIG_NET_TEAM_MODE_ROUNDROBIN) += team_mode_roundrobin.o
+obj-$(CONFIG_NET_TEAM_MODE_RANDOM) += team_mode_random.o
 obj-$(CONFIG_NET_TEAM_MODE_ACTIVEBACKUP) += team_mode_activebackup.o
 obj-$(CONFIG_NET_TEAM_MODE_LOADBALANCE) += team_mode_loadbalance.o
diff --git a/drivers/net/team/team.c b/drivers/net/team/team.c
index bf3419297875..7c43261975bd 100644
--- a/drivers/net/team/team.c
+++ b/drivers/net/team/team.c
@@ -73,11 +73,24 @@ static int team_port_set_orig_dev_addr(struct team_port *port)
 	return __set_port_dev_addr(port->dev, port->orig.dev_addr);
 }
 
-int team_port_set_team_dev_addr(struct team_port *port)
+static int team_port_set_team_dev_addr(struct team *team,
+				       struct team_port *port)
+{
+	return __set_port_dev_addr(port->dev, team->dev->dev_addr);
+}
+
+int team_modeop_port_enter(struct team *team, struct team_port *port)
+{
+	return team_port_set_team_dev_addr(team, port);
+}
+EXPORT_SYMBOL(team_modeop_port_enter);
+
+void team_modeop_port_change_dev_addr(struct team *team,
+				      struct team_port *port)
 {
-	return __set_port_dev_addr(port->dev, port->team->dev->dev_addr);
+	team_port_set_team_dev_addr(team, port);
 }
-EXPORT_SYMBOL(team_port_set_team_dev_addr);
+EXPORT_SYMBOL(team_modeop_port_change_dev_addr);
 
 static void team_refresh_port_linkup(struct team_port *port)
 {
@@ -490,9 +503,9 @@ static bool team_dummy_transmit(struct team *team, struct sk_buff *skb)
 	return false;
 }
 
-rx_handler_result_t team_dummy_receive(struct team *team,
-				       struct team_port *port,
-				       struct sk_buff *skb)
+static rx_handler_result_t team_dummy_receive(struct team *team,
+					      struct team_port *port,
+					      struct sk_buff *skb)
 {
 	return RX_HANDLER_ANOTHER;
 }
@@ -1491,8 +1504,8 @@ static void team_set_rx_mode(struct net_device *dev)
 
 	rcu_read_lock();
 	list_for_each_entry_rcu(port, &team->port_list, list) {
-		dev_uc_sync(port->dev, dev);
-		dev_mc_sync(port->dev, dev);
+		dev_uc_sync_multiple(port->dev, dev);
+		dev_mc_sync_multiple(port->dev, dev);
 	}
 	rcu_read_unlock();
 }
@@ -1585,7 +1598,7 @@ team_get_stats64(struct net_device *dev, struct rtnl_link_stats64 *stats)
 	return stats;
 }
 
-static int team_vlan_rx_add_vid(struct net_device *dev, uint16_t vid)
+static int team_vlan_rx_add_vid(struct net_device *dev, __be16 proto, u16 vid)
 {
 	struct team *team = netdev_priv(dev);
 	struct team_port *port;
@@ -1597,7 +1610,7 @@ static int team_vlan_rx_add_vid(struct net_device *dev, uint16_t vid)
 	 */
 	mutex_lock(&team->lock);
 	list_for_each_entry(port, &team->port_list, list) {
-		err = vlan_vid_add(port->dev, vid);
+		err = vlan_vid_add(port->dev, proto, vid);
 		if (err)
 			goto unwind;
 	}
@@ -1607,20 +1620,20 @@ static int team_vlan_rx_add_vid(struct net_device *dev, uint16_t vid)
 
 unwind:
 	list_for_each_entry_continue_reverse(port, &team->port_list, list)
-		vlan_vid_del(port->dev, vid);
+		vlan_vid_del(port->dev, proto, vid);
 	mutex_unlock(&team->lock);
 
 	return err;
 }
 
-static int team_vlan_rx_kill_vid(struct net_device *dev, uint16_t vid)
+static int team_vlan_rx_kill_vid(struct net_device *dev, __be16 proto, u16 vid)
 {
 	struct team *team = netdev_priv(dev);
 	struct team_port *port;
 
 	rcu_read_lock();
 	list_for_each_entry_rcu(port, &team->port_list, list)
-		vlan_vid_del(port->dev, vid);
+		vlan_vid_del(port->dev, proto, vid);
 	rcu_read_unlock();
 
 	return 0;
@@ -1828,9 +1841,9 @@ static void team_setup(struct net_device *dev)
 	dev->features |= NETIF_F_LLTX;
 	dev->features |= NETIF_F_GRO;
 	dev->hw_features = TEAM_VLAN_FEATURES |
-			   NETIF_F_HW_VLAN_TX |
-			   NETIF_F_HW_VLAN_RX |
-			   NETIF_F_HW_VLAN_FILTER;
+			   NETIF_F_HW_VLAN_CTAG_TX |
+			   NETIF_F_HW_VLAN_CTAG_RX |
+			   NETIF_F_HW_VLAN_CTAG_FILTER;
 
 	dev->hw_features &= ~(NETIF_F_ALL_CSUM & ~NETIF_F_HW_CSUM);
 	dev->features |= dev->hw_features;
diff --git a/drivers/net/team/team_mode_broadcast.c b/drivers/net/team/team_mode_broadcast.c
index c5db428e73fa..c366cd299c06 100644
--- a/drivers/net/team/team_mode_broadcast.c
+++ b/drivers/net/team/team_mode_broadcast.c
@@ -46,20 +46,10 @@ static bool bc_transmit(struct team *team, struct sk_buff *skb)
 	return sum_ret;
 }
 
-static int bc_port_enter(struct team *team, struct team_port *port)
-{
-	return team_port_set_team_dev_addr(port);
-}
-
-static void bc_port_change_dev_addr(struct team *team, struct team_port *port)
-{
-	team_port_set_team_dev_addr(port);
-}
-
 static const struct team_mode_ops bc_mode_ops = {
 	.transmit		= bc_transmit,
-	.port_enter		= bc_port_enter,
-	.port_change_dev_addr	= bc_port_change_dev_addr,
+	.port_enter		= team_modeop_port_enter,
+	.port_change_dev_addr	= team_modeop_port_change_dev_addr,
 };
 
 static const struct team_mode bc_mode = {
diff --git a/drivers/net/team/team_mode_random.c b/drivers/net/team/team_mode_random.c
new file mode 100644
index 000000000000..5ca14d463ba7
--- /dev/null
+++ b/drivers/net/team/team_mode_random.c
@@ -0,0 +1,71 @@
+/*
+ * drivers/net/team/team_mode_random.c - Random mode for team
+ * Copyright (c) 2013 Jiri Pirko <jiri@resnulli.us>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/skbuff.h>
+#include <linux/reciprocal_div.h>
+#include <linux/if_team.h>
+
+static u32 random_N(unsigned int N)
+{
+	return reciprocal_divide(prandom_u32(), N);
+}
+
+static bool rnd_transmit(struct team *team, struct sk_buff *skb)
+{
+	struct team_port *port;
+	int port_index;
+
+	port_index = random_N(team->en_port_count);
+	port = team_get_port_by_index_rcu(team, port_index);
+	port = team_get_first_port_txable_rcu(team, port);
+	if (unlikely(!port))
+		goto drop;
+	if (team_dev_queue_xmit(team, port, skb))
+		return false;
+	return true;
+
+drop:
+	dev_kfree_skb_any(skb);
+	return false;
+}
+
+static const struct team_mode_ops rnd_mode_ops = {
+	.transmit		= rnd_transmit,
+	.port_enter		= team_modeop_port_enter,
+	.port_change_dev_addr	= team_modeop_port_change_dev_addr,
+};
+
+static const struct team_mode rnd_mode = {
+	.kind		= "random",
+	.owner		= THIS_MODULE,
+	.ops		= &rnd_mode_ops,
+};
+
+static int __init rnd_init_module(void)
+{
+	return team_mode_register(&rnd_mode);
+}
+
+static void __exit rnd_cleanup_module(void)
+{
+	team_mode_unregister(&rnd_mode);
+}
+
+module_init(rnd_init_module);
+module_exit(rnd_cleanup_module);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Jiri Pirko <jiri@resnulli.us>");
+MODULE_DESCRIPTION("Random mode for team");
+MODULE_ALIAS("team-mode-random");
diff --git a/drivers/net/team/team_mode_roundrobin.c b/drivers/net/team/team_mode_roundrobin.c
index 105135aa8f05..d268e4de781b 100644
--- a/drivers/net/team/team_mode_roundrobin.c
+++ b/drivers/net/team/team_mode_roundrobin.c
@@ -25,26 +25,6 @@ static struct rr_priv *rr_priv(struct team *team)
 	return (struct rr_priv *) &team->mode_priv;
 }
 
-static struct team_port *__get_first_port_up(struct team *team,
-					     struct team_port *port)
-{
-	struct team_port *cur;
-
-	if (team_port_txable(port))
-		return port;
-	cur = port;
-	list_for_each_entry_continue_rcu(cur, &team->port_list, list)
-		if (team_port_txable(port))
-			return cur;
-	list_for_each_entry_rcu(cur, &team->port_list, list) {
-		if (cur == port)
-			break;
-		if (team_port_txable(port))
-			return cur;
-	}
-	return NULL;
-}
-
 static bool rr_transmit(struct team *team, struct sk_buff *skb)
 {
 	struct team_port *port;
@@ -52,7 +32,7 @@ static bool rr_transmit(struct team *team, struct sk_buff *skb)
 
 	port_index = rr_priv(team)->sent_packets++ % team->en_port_count;
 	port = team_get_port_by_index_rcu(team, port_index);
-	port = __get_first_port_up(team, port);
+	port = team_get_first_port_txable_rcu(team, port);
 	if (unlikely(!port))
 		goto drop;
 	if (team_dev_queue_xmit(team, port, skb))
@@ -64,20 +44,10 @@ drop:
 	return false;
 }
 
-static int rr_port_enter(struct team *team, struct team_port *port)
-{
-	return team_port_set_team_dev_addr(port);
-}
-
-static void rr_port_change_dev_addr(struct team *team, struct team_port *port)
-{
-	team_port_set_team_dev_addr(port);
-}
-
 static const struct team_mode_ops rr_mode_ops = {
 	.transmit		= rr_transmit,
-	.port_enter		= rr_port_enter,
-	.port_change_dev_addr	= rr_port_change_dev_addr,
+	.port_enter		= team_modeop_port_enter,
+	.port_change_dev_addr	= team_modeop_port_change_dev_addr,
 };
 
 static const struct team_mode rr_mode = {
diff --git a/drivers/net/tun.c b/drivers/net/tun.c
index 729ed533bb33..f042b0373e5d 100644
--- a/drivers/net/tun.c
+++ b/drivers/net/tun.c
@@ -409,14 +409,12 @@ static void __tun_detach(struct tun_file *tfile, bool clean)
 {
 	struct tun_file *ntfile;
 	struct tun_struct *tun;
-	struct net_device *dev;
 
 	tun = rtnl_dereference(tfile->tun);
 
 	if (tun && !tfile->detached) {
 		u16 index = tfile->queue_index;
 		BUG_ON(index >= tun->numqueues);
-		dev = tun->dev;
 
 		rcu_assign_pointer(tun->tfiles[index],
 				   tun->tfiles[tun->numqueues - 1]);
@@ -1205,6 +1203,8 @@ static ssize_t tun_get_user(struct tun_struct *tun, struct tun_file *tfile,
 	}
 
 	skb_reset_network_header(skb);
+	skb_probe_transport_header(skb, 0);
+
 	rxhash = skb_get_rxhash(skb);
 	netif_rx_ni(skb);
 
@@ -1471,14 +1471,17 @@ static int tun_recvmsg(struct kiocb *iocb, struct socket *sock,
 	if (!tun)
 		return -EBADFD;
 
-	if (flags & ~(MSG_DONTWAIT|MSG_TRUNC))
-		return -EINVAL;
+	if (flags & ~(MSG_DONTWAIT|MSG_TRUNC)) {
+		ret = -EINVAL;
+		goto out;
+	}
 	ret = tun_do_read(tun, tfile, iocb, m->msg_iov, total_len,
 			  flags & MSG_DONTWAIT);
 	if (ret > total_len) {
 		m->msg_flags |= MSG_TRUNC;
 		ret = flags & MSG_TRUNC ? ret : total_len;
 	}
+out:
 	tun_put(tun);
 	return ret;
 }
@@ -1593,8 +1596,12 @@ static int tun_set_iff(struct net *net, struct file *file, struct ifreq *ifr)
 			return err;
 
 		if (tun->flags & TUN_TAP_MQ &&
-		    (tun->numqueues + tun->numdisabled > 1))
-			return -EBUSY;
+		    (tun->numqueues + tun->numdisabled > 1)) {
+			/* One or more queue has already been attached, no need
+			 * to initialize the device again.
+			 */
+			return 0;
+		}
 	}
 	else {
 		char *name;
@@ -1656,6 +1663,7 @@ static int tun_set_iff(struct net *net, struct file *file, struct ifreq *ifr)
 		dev->hw_features = NETIF_F_SG | NETIF_F_FRAGLIST |
 			TUN_USER_FEATURES;
 		dev->features = dev->hw_features;
+		dev->vlan_features = dev->features;
 
 		INIT_LIST_HEAD(&tun->disabled);
 		err = tun_attach(tun, file);
diff --git a/drivers/net/usb/Kconfig b/drivers/net/usb/Kconfig
index 7c769d8e25ad..287cc624b90b 100644
--- a/drivers/net/usb/Kconfig
+++ b/drivers/net/usb/Kconfig
@@ -93,6 +93,17 @@ config USB_RTL8150
 	  To compile this driver as a module, choose M here: the
 	  module will be called rtl8150.
 
+config USB_RTL8152
+	tristate "Realtek RTL8152 Based USB 2.0 Ethernet Adapters"
+	select NET_CORE
+	select MII
+	help
+	  This option adds support for Realtek RTL8152 based USB 2.0
+	  10/100 Ethernet adapters.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called r8152.
+
 config USB_USBNET
 	tristate "Multi-purpose USB Networking Framework"
 	select NET_CORE
diff --git a/drivers/net/usb/Makefile b/drivers/net/usb/Makefile
index 119b06c9aa16..9ab5c9d4b45a 100644
--- a/drivers/net/usb/Makefile
+++ b/drivers/net/usb/Makefile
@@ -6,6 +6,7 @@ obj-$(CONFIG_USB_CATC)		+= catc.o
 obj-$(CONFIG_USB_KAWETH)	+= kaweth.o
 obj-$(CONFIG_USB_PEGASUS)	+= pegasus.o
 obj-$(CONFIG_USB_RTL8150)	+= rtl8150.o
+obj-$(CONFIG_USB_RTL8152)	+= r8152.o
 obj-$(CONFIG_USB_HSO)		+= hso.o
 obj-$(CONFIG_USB_NET_AX8817X)	+= asix.o
 asix-y := asix_devices.o asix_common.o ax88172a.o
diff --git a/drivers/net/usb/asix_common.c b/drivers/net/usb/asix_common.c
index f7f623a5390e..577c72d5f369 100644
--- a/drivers/net/usb/asix_common.c
+++ b/drivers/net/usb/asix_common.c
@@ -100,6 +100,9 @@ int asix_rx_fixup_internal(struct usbnet *dev, struct sk_buff *skb,
 			netdev_err(dev->net, "asix_rx_fixup() Bad RX Length %d\n",
 				   rx->size);
 			kfree_skb(rx->ax_skb);
+			rx->ax_skb = NULL;
+			rx->size = 0U;
+
 			return 0;
 		}
 
diff --git a/drivers/net/usb/asix_devices.c b/drivers/net/usb/asix_devices.c
index 709753469099..ad5d1e4384db 100644
--- a/drivers/net/usb/asix_devices.c
+++ b/drivers/net/usb/asix_devices.c
@@ -55,11 +55,7 @@ static void asix_status(struct usbnet *dev, struct urb *urb)
 	event = urb->transfer_buffer;
 	link = event->link & 0x01;
 	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);
+		usbnet_link_change(dev, link, 1);
 		netdev_dbg(dev->net, "Link Status is: %d\n", link);
 	}
 }
diff --git a/drivers/net/usb/ax88179_178a.c b/drivers/net/usb/ax88179_178a.c
index 71c27d8d214f..bd8758fa38c1 100644
--- a/drivers/net/usb/ax88179_178a.c
+++ b/drivers/net/usb/ax88179_178a.c
@@ -352,11 +352,7 @@ static void ax88179_status(struct usbnet *dev, struct urb *urb)
 	link = (((__force u32)event->intdata1) & AX_INT_PPLS_LINK) >> 16;
 
 	if (netif_carrier_ok(dev->net) != link) {
-		if (link)
-			usbnet_defer_kevent(dev, EVENT_LINK_RESET);
-		else
-			netif_carrier_off(dev->net);
-
+		usbnet_link_change(dev, link, 1);
 		netdev_info(dev->net, "ax88179 - Link status is: %d\n", link);
 	}
 }
@@ -455,7 +451,7 @@ static int ax88179_resume(struct usb_interface *intf)
 	u16 tmp16;
 	u8 tmp8;
 
-	netif_carrier_off(dev->net);
+	usbnet_link_change(dev, 0, 0);
 
 	/* Power up ethernet PHY */
 	tmp16 = 0;
@@ -1068,7 +1064,7 @@ static int ax88179_bind(struct usbnet *dev, struct usb_interface *intf)
 	/* Restart autoneg */
 	mii_nway_restart(&dev->mii);
 
-	netif_carrier_off(dev->net);
+	usbnet_link_change(dev, 0, 0);
 
 	return 0;
 }
@@ -1356,7 +1352,7 @@ static int ax88179_reset(struct usbnet *dev)
 	/* Restart autoneg */
 	mii_nway_restart(&dev->mii);
 
-	netif_carrier_off(dev->net);
+	usbnet_link_change(dev, 0, 0);
 
 	return 0;
 }
diff --git a/drivers/net/usb/cdc_ether.c b/drivers/net/usb/cdc_ether.c
index 57136dc1b887..078795fe6e31 100644
--- a/drivers/net/usb/cdc_ether.c
+++ b/drivers/net/usb/cdc_ether.c
@@ -406,10 +406,7 @@ void usbnet_cdc_status(struct usbnet *dev, struct urb *urb)
 	case USB_CDC_NOTIFY_NETWORK_CONNECTION:
 		netif_dbg(dev, timer, dev->net, "CDC: carrier %s\n",
 			  event->wValue ? "on" : "off");
-		if (event->wValue)
-			netif_carrier_on(dev->net);
-		else
-			netif_carrier_off(dev->net);
+		usbnet_link_change(dev, !!event->wValue, 0);
 		break;
 	case USB_CDC_NOTIFY_SPEED_CHANGE:	/* tx/rx rates */
 		netif_dbg(dev, timer, dev->net, "CDC: speed change (len %d)\n",
@@ -482,6 +479,7 @@ static const struct driver_info wwan_info = {
 #define NOVATEL_VENDOR_ID	0x1410
 #define ZTE_VENDOR_ID		0x19D2
 #define DELL_VENDOR_ID		0x413C
+#define REALTEK_VENDOR_ID	0x0bda
 
 static const struct usb_device_id	products [] = {
 /*
@@ -615,6 +613,13 @@ static const struct usb_device_id	products [] = {
 	.driver_info = 0,
 },
 
+/* Dell Wireless 5804 (Novatel E371) - handled by qmi_wwan */
+{
+	USB_DEVICE_AND_INTERFACE_INFO(DELL_VENDOR_ID, 0x819b, USB_CLASS_COMM,
+			USB_CDC_SUBCLASS_ETHERNET, USB_CDC_PROTO_NONE),
+	.driver_info = 0,
+},
+
 /* AnyDATA ADU960S - handled by qmi_wwan */
 {
 	USB_DEVICE_AND_INTERFACE_INFO(0x16d5, 0x650a, USB_CLASS_COMM,
@@ -622,6 +627,15 @@ static const struct usb_device_id	products [] = {
 	.driver_info = 0,
 },
 
+/* Realtek RTL8152 Based USB 2.0 Ethernet Adapters */
+#if defined(CONFIG_USB_RTL8152) || defined(CONFIG_USB_RTL8152_MODULE)
+{
+	USB_DEVICE_AND_INTERFACE_INFO(REALTEK_VENDOR_ID, 0x8152, USB_CLASS_COMM,
+			USB_CDC_SUBCLASS_ETHERNET, USB_CDC_PROTO_NONE),
+	.driver_info = 0,
+},
+#endif
+
 /*
  * WHITELIST!!!
  *
diff --git a/drivers/net/usb/cdc_mbim.c b/drivers/net/usb/cdc_mbim.c
index 32a76059e7da..872819851aef 100644
--- a/drivers/net/usb/cdc_mbim.c
+++ b/drivers/net/usb/cdc_mbim.c
@@ -101,7 +101,7 @@ static int cdc_mbim_bind(struct usbnet *dev, struct usb_interface *intf)
 	dev->net->flags |= IFF_NOARP;
 
 	/* no need to put the VLAN tci in the packet headers */
-	dev->net->features |= NETIF_F_HW_VLAN_TX;
+	dev->net->features |= NETIF_F_HW_VLAN_CTAG_TX;
 err:
 	return ret;
 }
@@ -221,7 +221,7 @@ static struct sk_buff *cdc_mbim_process_dgram(struct usbnet *dev, u8 *buf, size_
 
 	/* map MBIM session to VLAN */
 	if (tci)
-		vlan_put_tag(skb, tci);
+		vlan_put_tag(skb, htons(ETH_P_8021Q), tci);
 err:
 	return skb;
 }
diff --git a/drivers/net/usb/cdc_ncm.c b/drivers/net/usb/cdc_ncm.c
index 4709fa3497cf..43afde8f48d2 100644
--- a/drivers/net/usb/cdc_ncm.c
+++ b/drivers/net/usb/cdc_ncm.c
@@ -362,8 +362,8 @@ int cdc_ncm_bind_common(struct usbnet *dev, struct usb_interface *intf, u8 data_
 	u8 iface_no;
 
 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
-	if (ctx == NULL)
-		return -ENODEV;
+	if (!ctx)
+		return -ENOMEM;
 
 	hrtimer_init(&ctx->tx_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
 	ctx->tx_timer.function = &cdc_ncm_tx_timer_cb;
@@ -610,7 +610,7 @@ static int cdc_ncm_bind(struct usbnet *dev, struct usb_interface *intf)
 	 * (carrier is OFF) during attach, so the IP network stack does not
 	 * start IPv6 negotiation and more.
 	 */
-	netif_carrier_off(dev->net);
+	usbnet_link_change(dev, 0, 0);
 	return ret;
 }
 
@@ -1106,12 +1106,9 @@ static void cdc_ncm_status(struct usbnet *dev, struct urb *urb)
 			" %sconnected\n",
 			ctx->netdev->name, ctx->connected ? "" : "dis");
 
-		if (ctx->connected)
-			netif_carrier_on(dev->net);
-		else {
-			netif_carrier_off(dev->net);
+		usbnet_link_change(dev, ctx->connected, 0);
+		if (!ctx->connected)
 			ctx->tx_speed = ctx->rx_speed = 0;
-		}
 		break;
 
 	case USB_CDC_NOTIFY_SPEED_CHANGE:
@@ -1124,8 +1121,9 @@ static void cdc_ncm_status(struct usbnet *dev, struct urb *urb)
 		break;
 
 	default:
-		dev_err(&dev->udev->dev, "NCM: unexpected "
-			"notification 0x%02x!\n", event->bNotificationType);
+		dev_dbg(&dev->udev->dev,
+			"NCM: unexpected notification 0x%02x!\n",
+			event->bNotificationType);
 		break;
 	}
 }
diff --git a/drivers/net/usb/dm9601.c b/drivers/net/usb/dm9601.c
index 174e5ecea4cc..2dbb9460349d 100644
--- a/drivers/net/usb/dm9601.c
+++ b/drivers/net/usb/dm9601.c
@@ -524,12 +524,7 @@ static void dm9601_status(struct usbnet *dev, struct urb *urb)
 
 	link = !!(buf[0] & 0x40);
 	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);
+		usbnet_link_change(dev, link, 1);
 		netdev_dbg(dev->net, "Link Status is: %d\n", link);
 	}
 }
diff --git a/drivers/net/usb/mcs7830.c b/drivers/net/usb/mcs7830.c
index 3f3f566afa0b..03832d3780aa 100644
--- a/drivers/net/usb/mcs7830.c
+++ b/drivers/net/usb/mcs7830.c
@@ -576,11 +576,7 @@ static void mcs7830_status(struct usbnet *dev, struct urb *urb)
 		 */
 		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);
+			usbnet_link_change(dev, link, 0);
 			netdev_dbg(dev->net, "Link Status is: %d\n", link);
 		}
 	} else
diff --git a/drivers/net/usb/pegasus.c b/drivers/net/usb/pegasus.c
index 73051d10ead2..03e8a15d7deb 100644
--- a/drivers/net/usb/pegasus.c
+++ b/drivers/net/usb/pegasus.c
@@ -1,5 +1,5 @@
 /*
- *  Copyright (c) 1999-2005 Petko Manolov (petkan@users.sourceforge.net)
+ *  Copyright (c) 1999-2013 Petko Manolov (petkan@nucleusys.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
@@ -26,6 +26,9 @@
  *		v0.5.1	ethtool support added
  *		v0.5.5	rx socket buffers are in a pool and the their allocation
  *			is out of the interrupt routine.
+ *		...
+ *		v0.9.3	simplified [get|set]_register(s), async update registers
+ *			logic revisited, receive skb_pool removed.
  */
 
 #include <linux/sched.h>
@@ -45,8 +48,8 @@
 /*
  * Version Information
  */
-#define DRIVER_VERSION "v0.6.14 (2006/09/27)"
-#define DRIVER_AUTHOR "Petko Manolov <petkan@users.sourceforge.net>"
+#define DRIVER_VERSION "v0.9.3 (2013/04/25)"
+#define DRIVER_AUTHOR "Petko Manolov <petkan@nucleusys.com>"
 #define DRIVER_DESC "Pegasus/Pegasus II USB Ethernet driver"
 
 static const char driver_name[] = "pegasus";
@@ -108,251 +111,137 @@ MODULE_PARM_DESC(msg_level, "Override default message level");
 MODULE_DEVICE_TABLE(usb, pegasus_ids);
 static const struct net_device_ops pegasus_netdev_ops;
 
-static int update_eth_regs_async(pegasus_t *);
-/* Aargh!!! I _really_ hate such tweaks */
-static void ctrl_callback(struct urb *urb)
+/*****/
+
+static void async_ctrl_callback(struct urb *urb)
 {
-	pegasus_t *pegasus = urb->context;
+	struct usb_ctrlrequest *req = (struct usb_ctrlrequest *)urb->context;
 	int status = urb->status;
 
-	if (!pegasus)
-		return;
-
-	switch (status) {
-	case 0:
-		if (pegasus->flags & ETH_REGS_CHANGE) {
-			pegasus->flags &= ~ETH_REGS_CHANGE;
-			pegasus->flags |= ETH_REGS_CHANGED;
-			update_eth_regs_async(pegasus);
-			return;
-		}
-		break;
-	case -EINPROGRESS:
-		return;
-	case -ENOENT:
-		break;
-	default:
-		if (net_ratelimit())
-			netif_dbg(pegasus, drv, pegasus->net,
-				  "%s, status %d\n", __func__, status);
-		break;
-	}
-	pegasus->flags &= ~ETH_REGS_CHANGED;
-	wake_up(&pegasus->ctrl_wait);
+	if (status < 0)
+		dev_dbg(&urb->dev->dev, "%s failed with %d", __func__, status);
+	kfree(req);
+	usb_free_urb(urb);
 }
 
-static int get_registers(pegasus_t *pegasus, __u16 indx, __u16 size,
-			 void *data)
+static int get_registers(pegasus_t *pegasus, __u16 indx, __u16 size, void *data)
 {
 	int ret;
-	char *buffer;
-	DECLARE_WAITQUEUE(wait, current);
-
-	buffer = kmalloc(size, GFP_KERNEL);
-	if (!buffer)
-		return -ENOMEM;
-
-	add_wait_queue(&pegasus->ctrl_wait, &wait);
-	set_current_state(TASK_UNINTERRUPTIBLE);
-	while (pegasus->flags & ETH_REGS_CHANGED)
-		schedule();
-	remove_wait_queue(&pegasus->ctrl_wait, &wait);
-	set_current_state(TASK_RUNNING);
-
-	pegasus->dr.bRequestType = PEGASUS_REQT_READ;
-	pegasus->dr.bRequest = PEGASUS_REQ_GET_REGS;
-	pegasus->dr.wValue = cpu_to_le16(0);
-	pegasus->dr.wIndex = cpu_to_le16(indx);
-	pegasus->dr.wLength = cpu_to_le16(size);
-	pegasus->ctrl_urb->transfer_buffer_length = size;
-
-	usb_fill_control_urb(pegasus->ctrl_urb, pegasus->usb,
-			     usb_rcvctrlpipe(pegasus->usb, 0),
-			     (char *) &pegasus->dr,
-			     buffer, size, ctrl_callback, pegasus);
-
-	add_wait_queue(&pegasus->ctrl_wait, &wait);
-	set_current_state(TASK_UNINTERRUPTIBLE);
-
-	/* using ATOMIC, we'd never wake up if we slept */
-	if ((ret = usb_submit_urb(pegasus->ctrl_urb, GFP_ATOMIC))) {
-		set_current_state(TASK_RUNNING);
-		if (ret == -ENODEV)
-			netif_device_detach(pegasus->net);
-		if (net_ratelimit())
-			netif_err(pegasus, drv, pegasus->net,
-				  "%s, status %d\n", __func__, ret);
-		goto out;
-	}
-
-	schedule();
-out:
-	remove_wait_queue(&pegasus->ctrl_wait, &wait);
-	memcpy(data, buffer, size);
-	kfree(buffer);
 
+	ret = usb_control_msg(pegasus->usb, usb_rcvctrlpipe(pegasus->usb, 0),
+			      PEGASUS_REQ_GET_REGS, PEGASUS_REQT_READ, 0,
+			      indx, data, size, 1000);
+	if (ret < 0)
+		netif_dbg(pegasus, drv, pegasus->net,
+			  "%s returned %d\n", __func__, ret);
 	return ret;
 }
 
-static int set_registers(pegasus_t *pegasus, __u16 indx, __u16 size,
-			 void *data)
+static int set_registers(pegasus_t *pegasus, __u16 indx, __u16 size, void *data)
 {
 	int ret;
-	char *buffer;
-	DECLARE_WAITQUEUE(wait, current);
-
-	buffer = kmemdup(data, size, GFP_KERNEL);
-	if (!buffer) {
-		netif_warn(pegasus, drv, pegasus->net,
-			   "out of memory in %s\n", __func__);
-		return -ENOMEM;
-	}
-
-	add_wait_queue(&pegasus->ctrl_wait, &wait);
-	set_current_state(TASK_UNINTERRUPTIBLE);
-	while (pegasus->flags & ETH_REGS_CHANGED)
-		schedule();
-	remove_wait_queue(&pegasus->ctrl_wait, &wait);
-	set_current_state(TASK_RUNNING);
-
-	pegasus->dr.bRequestType = PEGASUS_REQT_WRITE;
-	pegasus->dr.bRequest = PEGASUS_REQ_SET_REGS;
-	pegasus->dr.wValue = cpu_to_le16(0);
-	pegasus->dr.wIndex = cpu_to_le16(indx);
-	pegasus->dr.wLength = cpu_to_le16(size);
-	pegasus->ctrl_urb->transfer_buffer_length = size;
-
-	usb_fill_control_urb(pegasus->ctrl_urb, pegasus->usb,
-			     usb_sndctrlpipe(pegasus->usb, 0),
-			     (char *) &pegasus->dr,
-			     buffer, size, ctrl_callback, pegasus);
-
-	add_wait_queue(&pegasus->ctrl_wait, &wait);
-	set_current_state(TASK_UNINTERRUPTIBLE);
-
-	if ((ret = usb_submit_urb(pegasus->ctrl_urb, GFP_ATOMIC))) {
-		if (ret == -ENODEV)
-			netif_device_detach(pegasus->net);
-		netif_err(pegasus, drv, pegasus->net,
-			  "%s, status %d\n", __func__, ret);
-		goto out;
-	}
-
-	schedule();
-out:
-	remove_wait_queue(&pegasus->ctrl_wait, &wait);
-	kfree(buffer);
 
+	ret = usb_control_msg(pegasus->usb, usb_sndctrlpipe(pegasus->usb, 0),
+			      PEGASUS_REQ_SET_REGS, PEGASUS_REQT_WRITE, 0,
+			      indx, data, size, 100);
+	if (ret < 0)
+		netif_dbg(pegasus, drv, pegasus->net,
+			  "%s returned %d\n", __func__, ret);
 	return ret;
 }
 
 static int set_register(pegasus_t *pegasus, __u16 indx, __u8 data)
 {
 	int ret;
-	char *tmp;
-	DECLARE_WAITQUEUE(wait, current);
-
-	tmp = kmemdup(&data, 1, GFP_KERNEL);
-	if (!tmp) {
-		netif_warn(pegasus, drv, pegasus->net,
-			   "out of memory in %s\n", __func__);
-		return -ENOMEM;
-	}
-	add_wait_queue(&pegasus->ctrl_wait, &wait);
-	set_current_state(TASK_UNINTERRUPTIBLE);
-	while (pegasus->flags & ETH_REGS_CHANGED)
-		schedule();
-	remove_wait_queue(&pegasus->ctrl_wait, &wait);
-	set_current_state(TASK_RUNNING);
-
-	pegasus->dr.bRequestType = PEGASUS_REQT_WRITE;
-	pegasus->dr.bRequest = PEGASUS_REQ_SET_REG;
-	pegasus->dr.wValue = cpu_to_le16(data);
-	pegasus->dr.wIndex = cpu_to_le16(indx);
-	pegasus->dr.wLength = cpu_to_le16(1);
-	pegasus->ctrl_urb->transfer_buffer_length = 1;
-
-	usb_fill_control_urb(pegasus->ctrl_urb, pegasus->usb,
-			     usb_sndctrlpipe(pegasus->usb, 0),
-			     (char *) &pegasus->dr,
-			     tmp, 1, ctrl_callback, pegasus);
-
-	add_wait_queue(&pegasus->ctrl_wait, &wait);
-	set_current_state(TASK_UNINTERRUPTIBLE);
-
-	if ((ret = usb_submit_urb(pegasus->ctrl_urb, GFP_ATOMIC))) {
-		if (ret == -ENODEV)
-			netif_device_detach(pegasus->net);
-		if (net_ratelimit())
-			netif_err(pegasus, drv, pegasus->net,
-				  "%s, status %d\n", __func__, ret);
-		goto out;
-	}
-
-	schedule();
-out:
-	remove_wait_queue(&pegasus->ctrl_wait, &wait);
-	kfree(tmp);
 
+	ret = usb_control_msg(pegasus->usb, usb_sndctrlpipe(pegasus->usb, 0),
+			      PEGASUS_REQ_SET_REG, PEGASUS_REQT_WRITE, data,
+			      indx, &data, 1, 1000);
+	if (ret < 0)
+		netif_dbg(pegasus, drv, pegasus->net,
+			  "%s returned %d\n", __func__, ret);
 	return ret;
 }
 
 static int update_eth_regs_async(pegasus_t *pegasus)
 {
-	int ret;
-
-	pegasus->dr.bRequestType = PEGASUS_REQT_WRITE;
-	pegasus->dr.bRequest = PEGASUS_REQ_SET_REGS;
-	pegasus->dr.wValue = cpu_to_le16(0);
-	pegasus->dr.wIndex = cpu_to_le16(EthCtrl0);
-	pegasus->dr.wLength = cpu_to_le16(3);
-	pegasus->ctrl_urb->transfer_buffer_length = 3;
-
-	usb_fill_control_urb(pegasus->ctrl_urb, pegasus->usb,
-			     usb_sndctrlpipe(pegasus->usb, 0),
-			     (char *) &pegasus->dr,
-			     pegasus->eth_regs, 3, ctrl_callback, pegasus);
-
-	if ((ret = usb_submit_urb(pegasus->ctrl_urb, GFP_ATOMIC))) {
+	int ret = -ENOMEM;
+	struct urb *async_urb;
+	struct usb_ctrlrequest *req;
+
+	req = kmalloc(sizeof(struct usb_ctrlrequest), GFP_ATOMIC);
+	if (req == NULL)
+		return ret;
+
+	async_urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (async_urb == NULL) {
+		kfree(req);
+		return ret;
+	}
+	req->bRequestType = PEGASUS_REQT_WRITE;
+	req->bRequest = PEGASUS_REQ_SET_REGS;
+	req->wValue = cpu_to_le16(0);
+	req->wIndex = cpu_to_le16(EthCtrl0);
+	req->wLength = cpu_to_le16(3);
+
+	usb_fill_control_urb(async_urb, pegasus->usb,
+			     usb_sndctrlpipe(pegasus->usb, 0), (void *)req,
+			     pegasus->eth_regs, 3, async_ctrl_callback, req);
+
+	ret = usb_submit_urb(async_urb, GFP_ATOMIC);
+	if (ret) {
 		if (ret == -ENODEV)
 			netif_device_detach(pegasus->net);
 		netif_err(pegasus, drv, pegasus->net,
-			  "%s, status %d\n", __func__, ret);
+			  "%s returned %d\n", __func__, ret);
 	}
-
 	return ret;
 }
 
-/* Returns 0 on success, error on failure */
-static int read_mii_word(pegasus_t *pegasus, __u8 phy, __u8 indx, __u16 *regd)
+static int __mii_op(pegasus_t *p, __u8 phy, __u8 indx, __u16 *regd, __u8 cmd)
 {
 	int i;
 	__u8 data[4] = { phy, 0, 0, indx };
 	__le16 regdi;
-	int ret;
+	int ret = -ETIMEDOUT;
 
-	set_register(pegasus, PhyCtrl, 0);
-	set_registers(pegasus, PhyAddr, sizeof(data), data);
-	set_register(pegasus, PhyCtrl, (indx | PHY_READ));
+	if (cmd & PHY_WRITE) {
+		__le16 *t = (__le16 *) & data[1];
+		*t = cpu_to_le16(*regd);
+	}
+	set_register(p, PhyCtrl, 0);
+	set_registers(p, PhyAddr, sizeof(data), data);
+	set_register(p, PhyCtrl, (indx | cmd));
 	for (i = 0; i < REG_TIMEOUT; i++) {
-		ret = get_registers(pegasus, PhyCtrl, 1, data);
-		if (ret == -ESHUTDOWN)
+		ret = get_registers(p, PhyCtrl, 1, data);
+		if (ret < 0)
 			goto fail;
 		if (data[0] & PHY_DONE)
 			break;
 	}
-
 	if (i >= REG_TIMEOUT)
 		goto fail;
-
-	ret = get_registers(pegasus, PhyData, 2, &regdi);
-	*regd = le16_to_cpu(regdi);
+	if (cmd & PHY_READ) {
+		ret = get_registers(p, PhyData, 2, &regdi);
+		*regd = le16_to_cpu(regdi);
+		return ret;
+	}
+	return 0;
+fail:
+	netif_dbg(p, drv, p->net, "%s failed\n", __func__);
 	return ret;
+}
 
-fail:
-	netif_warn(pegasus, drv, pegasus->net, "%s failed\n", __func__);
+/* Returns non-negative int on success, error on failure */
+static int read_mii_word(pegasus_t *pegasus, __u8 phy, __u8 indx, __u16 *regd)
+{
+	return __mii_op(pegasus, phy, indx, regd, PHY_READ);
+}
 
-	return ret;
+/* Returns zero on success, error on failure */
+static int write_mii_word(pegasus_t *pegasus, __u8 phy, __u8 indx, __u16 *regd)
+{
+	return __mii_op(pegasus, phy, indx, regd, PHY_WRITE);
 }
 
 static int mdio_read(struct net_device *dev, int phy_id, int loc)
@@ -364,40 +253,12 @@ static int mdio_read(struct net_device *dev, int phy_id, int loc)
 	return (int)res;
 }
 
-static int write_mii_word(pegasus_t *pegasus, __u8 phy, __u8 indx, __u16 regd)
-{
-	int i;
-	__u8 data[4] = { phy, 0, 0, indx };
-	int ret;
-
-	data[1] = (u8) regd;
-	data[2] = (u8) (regd >> 8);
-	set_register(pegasus, PhyCtrl, 0);
-	set_registers(pegasus, PhyAddr, sizeof(data), data);
-	set_register(pegasus, PhyCtrl, (indx | PHY_WRITE));
-	for (i = 0; i < REG_TIMEOUT; i++) {
-		ret = get_registers(pegasus, PhyCtrl, 1, data);
-		if (ret == -ESHUTDOWN)
-			goto fail;
-		if (data[0] & PHY_DONE)
-			break;
-	}
-
-	if (i >= REG_TIMEOUT)
-		goto fail;
-
-	return ret;
-
-fail:
-	netif_warn(pegasus, drv, pegasus->net, "%s failed\n", __func__);
-	return -ETIMEDOUT;
-}
-
 static void mdio_write(struct net_device *dev, int phy_id, int loc, int val)
 {
 	pegasus_t *pegasus = netdev_priv(dev);
+	u16 data = val;
 
-	write_mii_word(pegasus, phy_id, loc, val);
+	write_mii_word(pegasus, phy_id, loc, &data);
 }
 
 static int read_eprom_word(pegasus_t *pegasus, __u8 index, __u16 *retdata)
@@ -434,7 +295,6 @@ fail:
 static inline void enable_eprom_write(pegasus_t *pegasus)
 {
 	__u8 tmp;
-	int ret;
 
 	get_registers(pegasus, EthCtrl2, 1, &tmp);
 	set_register(pegasus, EthCtrl2, tmp | EPROM_WR_ENABLE);
@@ -443,7 +303,6 @@ static inline void enable_eprom_write(pegasus_t *pegasus)
 static inline void disable_eprom_write(pegasus_t *pegasus)
 {
 	__u8 tmp;
-	int ret;
 
 	get_registers(pegasus, EthCtrl2, 1, &tmp);
 	set_register(pegasus, EpromCtrl, 0);
@@ -537,7 +396,8 @@ static inline int reset_mac(pegasus_t *pegasus)
 	if (usb_dev_id[pegasus->dev_index].vendor == VENDOR_ELCON) {
 		__u16 auxmode;
 		read_mii_word(pegasus, 3, 0x1b, &auxmode);
-		write_mii_word(pegasus, 3, 0x1b, auxmode | 4);
+		auxmode |= 4;
+		write_mii_word(pegasus, 3, 0x1b, &auxmode);
 	}
 
 	return 0;
@@ -569,57 +429,13 @@ static int enable_net_traffic(struct net_device *dev, struct usb_device *usb)
 	    usb_dev_id[pegasus->dev_index].vendor == VENDOR_DLINK) {
 		u16 auxmode;
 		read_mii_word(pegasus, 0, 0x1b, &auxmode);
-		write_mii_word(pegasus, 0, 0x1b, auxmode | 4);
+		auxmode |= 4;
+		write_mii_word(pegasus, 0, 0x1b, &auxmode);
 	}
 
 	return ret;
 }
 
-static void fill_skb_pool(pegasus_t *pegasus)
-{
-	int i;
-
-	for (i = 0; i < RX_SKBS; i++) {
-		if (pegasus->rx_pool[i])
-			continue;
-		pegasus->rx_pool[i] = dev_alloc_skb(PEGASUS_MTU + 2);
-		/*
-		 ** we give up if the allocation fail. the tasklet will be
-		 ** rescheduled again anyway...
-		 */
-		if (pegasus->rx_pool[i] == NULL)
-			return;
-		skb_reserve(pegasus->rx_pool[i], 2);
-	}
-}
-
-static void free_skb_pool(pegasus_t *pegasus)
-{
-	int i;
-
-	for (i = 0; i < RX_SKBS; i++) {
-		if (pegasus->rx_pool[i]) {
-			dev_kfree_skb(pegasus->rx_pool[i]);
-			pegasus->rx_pool[i] = NULL;
-		}
-	}
-}
-
-static inline struct sk_buff *pull_skb(pegasus_t * pegasus)
-{
-	int i;
-	struct sk_buff *skb;
-
-	for (i = 0; i < RX_SKBS; i++) {
-		if (likely(pegasus->rx_pool[i] != NULL)) {
-			skb = pegasus->rx_pool[i];
-			pegasus->rx_pool[i] = NULL;
-			return skb;
-		}
-	}
-	return NULL;
-}
-
 static void read_bulk_callback(struct urb *urb)
 {
 	pegasus_t *pegasus = urb->context;
@@ -704,9 +520,8 @@ static void read_bulk_callback(struct urb *urb)
 	if (pegasus->flags & PEGASUS_UNPLUG)
 		return;
 
-	spin_lock(&pegasus->rx_pool_lock);
-	pegasus->rx_skb = pull_skb(pegasus);
-	spin_unlock(&pegasus->rx_pool_lock);
+	pegasus->rx_skb = __netdev_alloc_skb_ip_align(pegasus->net, PEGASUS_MTU,
+						      GFP_ATOMIC);
 
 	if (pegasus->rx_skb == NULL)
 		goto tl_sched;
@@ -734,24 +549,23 @@ tl_sched:
 static void rx_fixup(unsigned long data)
 {
 	pegasus_t *pegasus;
-	unsigned long flags;
 	int status;
 
 	pegasus = (pegasus_t *) data;
 	if (pegasus->flags & PEGASUS_UNPLUG)
 		return;
 
-	spin_lock_irqsave(&pegasus->rx_pool_lock, flags);
-	fill_skb_pool(pegasus);
 	if (pegasus->flags & PEGASUS_RX_URB_FAIL)
 		if (pegasus->rx_skb)
 			goto try_again;
 	if (pegasus->rx_skb == NULL)
-		pegasus->rx_skb = pull_skb(pegasus);
+		pegasus->rx_skb = __netdev_alloc_skb_ip_align(pegasus->net,
+							      PEGASUS_MTU,
+							      GFP_ATOMIC);
 	if (pegasus->rx_skb == NULL) {
 		netif_warn(pegasus, rx_err, pegasus->net, "low on memory\n");
 		tasklet_schedule(&pegasus->rx_tl);
-		goto done;
+		return;
 	}
 	usb_fill_bulk_urb(pegasus->rx_urb, pegasus->usb,
 			  usb_rcvbulkpipe(pegasus->usb, 1),
@@ -767,8 +581,6 @@ try_again:
 	} else {
 		pegasus->flags &= ~PEGASUS_RX_URB_FAIL;
 	}
-done:
-	spin_unlock_irqrestore(&pegasus->rx_pool_lock, flags);
 }
 
 static void write_bulk_callback(struct urb *urb)
@@ -963,7 +775,6 @@ static void free_all_urbs(pegasus_t *pegasus)
 	usb_free_urb(pegasus->intr_urb);
 	usb_free_urb(pegasus->tx_urb);
 	usb_free_urb(pegasus->rx_urb);
-	usb_free_urb(pegasus->ctrl_urb);
 }
 
 static void unlink_all_urbs(pegasus_t *pegasus)
@@ -971,48 +782,42 @@ static void unlink_all_urbs(pegasus_t *pegasus)
 	usb_kill_urb(pegasus->intr_urb);
 	usb_kill_urb(pegasus->tx_urb);
 	usb_kill_urb(pegasus->rx_urb);
-	usb_kill_urb(pegasus->ctrl_urb);
 }
 
 static int alloc_urbs(pegasus_t *pegasus)
 {
-	pegasus->ctrl_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!pegasus->ctrl_urb)
-		return 0;
+	int res = -ENOMEM;
+
 	pegasus->rx_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!pegasus->rx_urb) {
-		usb_free_urb(pegasus->ctrl_urb);
-		return 0;
+		return res;
 	}
 	pegasus->tx_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!pegasus->tx_urb) {
 		usb_free_urb(pegasus->rx_urb);
-		usb_free_urb(pegasus->ctrl_urb);
-		return 0;
+		return res;
 	}
 	pegasus->intr_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!pegasus->intr_urb) {
 		usb_free_urb(pegasus->tx_urb);
 		usb_free_urb(pegasus->rx_urb);
-		usb_free_urb(pegasus->ctrl_urb);
-		return 0;
+		return res;
 	}
 
-	return 1;
+	return 0;
 }
 
 static int pegasus_open(struct net_device *net)
 {
 	pegasus_t *pegasus = netdev_priv(net);
-	int res;
+	int res=-ENOMEM;
 
 	if (pegasus->rx_skb == NULL)
-		pegasus->rx_skb = pull_skb(pegasus);
-	/*
-	 ** Note: no point to free the pool.  it is empty :-)
-	 */
+		pegasus->rx_skb = __netdev_alloc_skb_ip_align(pegasus->net,
+							      PEGASUS_MTU,
+							      GFP_KERNEL);
 	if (!pegasus->rx_skb)
-		return -ENOMEM;
+		goto exit;
 
 	res = set_registers(pegasus, EthID, 6, net->dev_addr);
 
@@ -1038,13 +843,13 @@ static int pegasus_open(struct net_device *net)
 		usb_kill_urb(pegasus->rx_urb);
 		goto exit;
 	}
-	if ((res = enable_net_traffic(net, pegasus->usb))) {
+	res = enable_net_traffic(net, pegasus->usb);
+	if (res < 0) {
 		netif_dbg(pegasus, ifup, net,
 			  "can't enable_net_traffic() - %d\n", res);
 		res = -EIO;
 		usb_kill_urb(pegasus->rx_urb);
 		usb_kill_urb(pegasus->intr_urb);
-		free_skb_pool(pegasus);
 		goto exit;
 	}
 	set_carrier(net);
@@ -1195,7 +1000,7 @@ static int pegasus_ioctl(struct net_device *net, struct ifreq *rq, int cmd)
 	case SIOCDEVPRIVATE + 2:
 		if (!capable(CAP_NET_ADMIN))
 			return -EPERM;
-		write_mii_word(pegasus, pegasus->phy, data[1] & 0x1f, data[2]);
+		write_mii_word(pegasus, pegasus->phy, data[1] & 0x1f, &data[2]);
 		res = 0;
 		break;
 	default:
@@ -1219,11 +1024,7 @@ static void pegasus_set_multicast(struct net_device *net)
 		pegasus->eth_regs[EthCtrl0] &= ~RX_MULTICAST;
 		pegasus->eth_regs[EthCtrl2] &= ~RX_PROMISCUOUS;
 	}
-
-	pegasus->ctrl_urb->status = 0;
-
-	pegasus->flags |= ETH_REGS_CHANGE;
-	ctrl_callback(pegasus->ctrl_urb);
+	update_eth_regs_async(pegasus);
 }
 
 static __u8 mii_phy_probe(pegasus_t *pegasus)
@@ -1340,9 +1141,9 @@ static int pegasus_probe(struct usb_interface *intf,
 
 	pegasus = netdev_priv(net);
 	pegasus->dev_index = dev_index;
-	init_waitqueue_head(&pegasus->ctrl_wait);
 
-	if (!alloc_urbs(pegasus)) {
+	res = alloc_urbs(pegasus);
+	if (res < 0) {
 		dev_err(&intf->dev, "can't allocate %s\n", "urbs");
 		goto out1;
 	}
@@ -1364,7 +1165,6 @@ static int pegasus_probe(struct usb_interface *intf,
 	pegasus->mii.mdio_write = mdio_write;
 	pegasus->mii.phy_id_mask = 0x1f;
 	pegasus->mii.reg_num_mask = 0x1f;
-	spin_lock_init(&pegasus->rx_pool_lock);
 	pegasus->msg_enable = netif_msg_init(msg_level, NETIF_MSG_DRV
 				| NETIF_MSG_PROBE | NETIF_MSG_LINK);
 
@@ -1376,7 +1176,6 @@ static int pegasus_probe(struct usb_interface *intf,
 		goto out2;
 	}
 	set_ethernet_addr(pegasus);
-	fill_skb_pool(pegasus);
 	if (pegasus->features & PEGASUS_II) {
 		dev_info(&intf->dev, "setup Pegasus II specific registers\n");
 		setup_pegasus_II(pegasus);
@@ -1394,17 +1193,13 @@ static int pegasus_probe(struct usb_interface *intf,
 	if (res)
 		goto out3;
 	queue_delayed_work(pegasus_workqueue, &pegasus->carrier_check,
-				CARRIER_CHECK_DELAY);
-
-	dev_info(&intf->dev, "%s, %s, %pM\n",
-		 net->name,
-		 usb_dev_id[dev_index].name,
-		 net->dev_addr);
+			   CARRIER_CHECK_DELAY);
+	dev_info(&intf->dev, "%s, %s, %pM\n", net->name,
+		 usb_dev_id[dev_index].name, net->dev_addr);
 	return 0;
 
 out3:
 	usb_set_intfdata(intf, NULL);
-	free_skb_pool(pegasus);
 out2:
 	free_all_urbs(pegasus);
 out1:
@@ -1429,7 +1224,6 @@ static void pegasus_disconnect(struct usb_interface *intf)
 	unregister_netdev(pegasus->net);
 	unlink_all_urbs(pegasus);
 	free_all_urbs(pegasus);
-	free_skb_pool(pegasus);
 	if (pegasus->rx_skb != NULL) {
 		dev_kfree_skb(pegasus->rx_skb);
 		pegasus->rx_skb = NULL;
diff --git a/drivers/net/usb/pegasus.h b/drivers/net/usb/pegasus.h
index 65b78b35b73c..d15646244fdf 100644
--- a/drivers/net/usb/pegasus.h
+++ b/drivers/net/usb/pegasus.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 1999-2003 Petko Manolov - Petkan (petkan@users.sourceforge.net)
+ * Copyright (c) 1999-2013 Petko Manolov (petkan@nucleusys.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
@@ -13,7 +13,6 @@
 #define	HAS_HOME_PNA		0x40000000
 
 #define	PEGASUS_MTU		1536
-#define	RX_SKBS			4
 
 #define	EPROM_WRITE		0x01
 #define	EPROM_READ		0x02
@@ -34,8 +33,6 @@
 #define	CTRL_URB_SLEEP		0x00000020
 #define	PEGASUS_UNPLUG		0x00000040
 #define	PEGASUS_RX_URB_FAIL	0x00000080
-#define	ETH_REGS_CHANGE		0x40000000
-#define	ETH_REGS_CHANGED	0x80000000
 
 #define	RX_MULTICAST		2
 #define	RX_PROMISCUOUS		4
@@ -96,12 +93,8 @@ typedef struct pegasus {
 	int			intr_interval;
 	struct tasklet_struct	rx_tl;
 	struct delayed_work	carrier_check;
-	struct urb		*ctrl_urb, *rx_urb, *tx_urb, *intr_urb;
-	struct sk_buff		*rx_pool[RX_SKBS];
+	struct urb		*rx_urb, *tx_urb, *intr_urb;
 	struct sk_buff		*rx_skb;
-	struct usb_ctrlrequest	dr;
-	wait_queue_head_t	ctrl_wait;
-	spinlock_t		rx_pool_lock;
 	int			chip;
 	unsigned char		intr_buff[8];
 	__u8			tx_buff[PEGASUS_MTU];
diff --git a/drivers/net/usb/qmi_wwan.c b/drivers/net/usb/qmi_wwan.c
index 5a88e72090ce..cf887c2384e9 100644
--- a/drivers/net/usb/qmi_wwan.c
+++ b/drivers/net/usb/qmi_wwan.c
@@ -501,6 +501,13 @@ static const struct usb_device_id products[] = {
 					      USB_CDC_PROTO_NONE),
 		.driver_info        = (unsigned long)&qmi_wwan_info,
 	},
+	{	/* Dell Wireless 5804 (Novatel E371) */
+		USB_DEVICE_AND_INTERFACE_INFO(0x413C, 0x819b,
+					      USB_CLASS_COMM,
+					      USB_CDC_SUBCLASS_ETHERNET,
+					      USB_CDC_PROTO_NONE),
+		.driver_info        = (unsigned long)&qmi_wwan_info,
+	},
 	{	/* ADU960S */
 		USB_DEVICE_AND_INTERFACE_INFO(0x16d5, 0x650a,
 					      USB_CLASS_COMM,
@@ -548,6 +555,7 @@ static const struct usb_device_id products[] = {
 	{QMI_FIXED_INTF(0x19d2, 0x0265, 4)},	/* ONDA MT8205 4G LTE */
 	{QMI_FIXED_INTF(0x19d2, 0x0284, 4)},	/* ZTE MF880 */
 	{QMI_FIXED_INTF(0x19d2, 0x0326, 4)},	/* ZTE MF821D */
+	{QMI_FIXED_INTF(0x19d2, 0x0412, 4)},	/* Telewell TW-LTE 4G */
 	{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)},
diff --git a/drivers/net/usb/r8152.c b/drivers/net/usb/r8152.c
new file mode 100644
index 000000000000..14e519888631
--- /dev/null
+++ b/drivers/net/usb/r8152.c
@@ -0,0 +1,1767 @@
+/*
+ *  Copyright (c) 2013 Realtek Semiconductor Corp. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/signal.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/mii.h>
+#include <linux/ethtool.h>
+#include <linux/usb.h>
+#include <linux/crc32.h>
+#include <linux/if_vlan.h>
+#include <linux/uaccess.h>
+
+/* Version Information */
+#define DRIVER_VERSION "v1.0.0 (2013/05/03)"
+#define DRIVER_AUTHOR "Realtek linux nic maintainers <nic_swsd@realtek.com>"
+#define DRIVER_DESC "Realtek RTL8152 Based USB 2.0 Ethernet Adapters"
+#define MODULENAME "r8152"
+
+#define R8152_PHY_ID		32
+
+#define PLA_IDR			0xc000
+#define PLA_RCR			0xc010
+#define PLA_RMS			0xc016
+#define PLA_RXFIFO_CTRL0	0xc0a0
+#define PLA_RXFIFO_CTRL1	0xc0a4
+#define PLA_RXFIFO_CTRL2	0xc0a8
+#define PLA_FMC			0xc0b4
+#define PLA_CFG_WOL		0xc0b6
+#define PLA_MAR			0xcd00
+#define PAL_BDC_CR		0xd1a0
+#define PLA_LEDSEL		0xdd90
+#define PLA_LED_FEATURE		0xdd92
+#define PLA_PHYAR		0xde00
+#define PLA_GPHY_INTR_IMR	0xe022
+#define PLA_EEE_CR		0xe040
+#define PLA_EEEP_CR		0xe080
+#define PLA_MAC_PWR_CTRL	0xe0c0
+#define PLA_TCR0		0xe610
+#define PLA_TCR1		0xe612
+#define PLA_TXFIFO_CTRL		0xe618
+#define PLA_RSTTELLY		0xe800
+#define PLA_CR			0xe813
+#define PLA_CRWECR		0xe81c
+#define PLA_CONFIG5		0xe822
+#define PLA_PHY_PWR		0xe84c
+#define PLA_OOB_CTRL		0xe84f
+#define PLA_CPCR		0xe854
+#define PLA_MISC_0		0xe858
+#define PLA_MISC_1		0xe85a
+#define PLA_OCP_GPHY_BASE	0xe86c
+#define PLA_TELLYCNT		0xe890
+#define PLA_SFF_STS_7		0xe8de
+#define PLA_PHYSTATUS		0xe908
+#define PLA_BP_BA		0xfc26
+#define PLA_BP_0		0xfc28
+#define PLA_BP_1		0xfc2a
+#define PLA_BP_2		0xfc2c
+#define PLA_BP_3		0xfc2e
+#define PLA_BP_4		0xfc30
+#define PLA_BP_5		0xfc32
+#define PLA_BP_6		0xfc34
+#define PLA_BP_7		0xfc36
+
+#define USB_DEV_STAT		0xb808
+#define USB_USB_CTRL		0xd406
+#define USB_PHY_CTRL		0xd408
+#define USB_TX_AGG		0xd40a
+#define USB_RX_BUF_TH		0xd40c
+#define USB_USB_TIMER		0xd428
+#define USB_PM_CTRL_STATUS	0xd432
+#define USB_TX_DMA		0xd434
+#define USB_UPS_CTRL		0xd800
+#define USB_BP_BA		0xfc26
+#define USB_BP_0		0xfc28
+#define USB_BP_1		0xfc2a
+#define USB_BP_2		0xfc2c
+#define USB_BP_3		0xfc2e
+#define USB_BP_4		0xfc30
+#define USB_BP_5		0xfc32
+#define USB_BP_6		0xfc34
+#define USB_BP_7		0xfc36
+
+/* OCP Registers */
+#define OCP_ALDPS_CONFIG	0x2010
+#define OCP_EEE_CONFIG1		0x2080
+#define OCP_EEE_CONFIG2		0x2092
+#define OCP_EEE_CONFIG3		0x2094
+#define OCP_EEE_AR		0xa41a
+#define OCP_EEE_DATA		0xa41c
+
+/* PLA_RCR */
+#define RCR_AAP			0x00000001
+#define RCR_APM			0x00000002
+#define RCR_AM			0x00000004
+#define RCR_AB			0x00000008
+#define RCR_ACPT_ALL		(RCR_AAP | RCR_APM | RCR_AM | RCR_AB)
+
+/* PLA_RXFIFO_CTRL0 */
+#define RXFIFO_THR1_NORMAL	0x00080002
+#define RXFIFO_THR1_OOB		0x01800003
+
+/* PLA_RXFIFO_CTRL1 */
+#define RXFIFO_THR2_FULL	0x00000060
+#define RXFIFO_THR2_HIGH	0x00000038
+#define RXFIFO_THR2_OOB		0x0000004a
+
+/* PLA_RXFIFO_CTRL2 */
+#define RXFIFO_THR3_FULL	0x00000078
+#define RXFIFO_THR3_HIGH	0x00000048
+#define RXFIFO_THR3_OOB		0x0000005a
+
+/* PLA_TXFIFO_CTRL */
+#define TXFIFO_THR_NORMAL	0x00400008
+
+/* PLA_FMC */
+#define FMC_FCR_MCU_EN		0x0001
+
+/* PLA_EEEP_CR */
+#define EEEP_CR_EEEP_TX		0x0002
+
+/* PLA_TCR0 */
+#define TCR0_TX_EMPTY		0x0800
+#define TCR0_AUTO_FIFO		0x0080
+
+/* PLA_TCR1 */
+#define VERSION_MASK		0x7cf0
+
+/* PLA_CR */
+#define CR_RST			0x10
+#define CR_RE			0x08
+#define CR_TE			0x04
+
+/* PLA_CRWECR */
+#define CRWECR_NORAML		0x00
+#define CRWECR_CONFIG		0xc0
+
+/* PLA_OOB_CTRL */
+#define NOW_IS_OOB		0x80
+#define TXFIFO_EMPTY		0x20
+#define RXFIFO_EMPTY		0x10
+#define LINK_LIST_READY		0x02
+#define DIS_MCU_CLROOB		0x01
+#define FIFO_EMPTY		(TXFIFO_EMPTY | RXFIFO_EMPTY)
+
+/* PLA_MISC_1 */
+#define RXDY_GATED_EN		0x0008
+
+/* PLA_SFF_STS_7 */
+#define RE_INIT_LL		0x8000
+#define MCU_BORW_EN		0x4000
+
+/* PLA_CPCR */
+#define CPCR_RX_VLAN		0x0040
+
+/* PLA_CFG_WOL */
+#define MAGIC_EN		0x0001
+
+/* PAL_BDC_CR */
+#define ALDPS_PROXY_MODE	0x0001
+
+/* PLA_CONFIG5 */
+#define LAN_WAKE_EN		0x0002
+
+/* PLA_LED_FEATURE */
+#define LED_MODE_MASK		0x0700
+
+/* PLA_PHY_PWR */
+#define TX_10M_IDLE_EN		0x0080
+#define PFM_PWM_SWITCH		0x0040
+
+/* PLA_MAC_PWR_CTRL */
+#define D3_CLK_GATED_EN		0x00004000
+#define MCU_CLK_RATIO		0x07010f07
+#define MCU_CLK_RATIO_MASK	0x0f0f0f0f
+
+/* PLA_GPHY_INTR_IMR */
+#define GPHY_STS_MSK		0x0001
+#define SPEED_DOWN_MSK		0x0002
+#define SPDWN_RXDV_MSK		0x0004
+#define SPDWN_LINKCHG_MSK	0x0008
+
+/* PLA_PHYAR */
+#define PHYAR_FLAG		0x80000000
+
+/* PLA_EEE_CR */
+#define EEE_RX_EN		0x0001
+#define EEE_TX_EN		0x0002
+
+/* USB_DEV_STAT */
+#define STAT_SPEED_MASK		0x0006
+#define STAT_SPEED_HIGH		0x0000
+#define STAT_SPEED_FULL		0x0001
+
+/* USB_TX_AGG */
+#define TX_AGG_MAX_THRESHOLD	0x03
+
+/* USB_RX_BUF_TH */
+#define RX_BUF_THR		0x7a120180
+
+/* USB_TX_DMA */
+#define TEST_MODE_DISABLE	0x00000001
+#define TX_SIZE_ADJUST1		0x00000100
+
+/* USB_UPS_CTRL */
+#define POWER_CUT		0x0100
+
+/* USB_PM_CTRL_STATUS */
+#define RWSUME_INDICATE		0x0001
+
+/* USB_USB_CTRL */
+#define RX_AGG_DISABLE		0x0010
+
+/* OCP_ALDPS_CONFIG */
+#define ENPWRSAVE		0x8000
+#define ENPDNPS			0x0200
+#define LINKENA			0x0100
+#define DIS_SDSAVE		0x0010
+
+/* OCP_EEE_CONFIG1 */
+#define RG_TXLPI_MSK_HFDUP	0x8000
+#define RG_MATCLR_EN		0x4000
+#define EEE_10_CAP		0x2000
+#define EEE_NWAY_EN		0x1000
+#define TX_QUIET_EN		0x0200
+#define RX_QUIET_EN		0x0100
+#define SDRISETIME		0x0010	/* bit 4 ~ 6 */
+#define RG_RXLPI_MSK_HFDUP	0x0008
+#define SDFALLTIME		0x0007	/* bit 0 ~ 2 */
+
+/* OCP_EEE_CONFIG2 */
+#define RG_LPIHYS_NUM		0x7000	/* bit 12 ~ 15 */
+#define RG_DACQUIET_EN		0x0400
+#define RG_LDVQUIET_EN		0x0200
+#define RG_CKRSEL		0x0020
+#define RG_EEEPRG_EN		0x0010
+
+/* OCP_EEE_CONFIG3 */
+#define FST_SNR_EYE_R		0x1500	/* bit 7 ~ 15 */
+#define RG_LFS_SEL		0x0060	/* bit 6 ~ 5 */
+#define MSK_PH			0x0006	/* bit 0 ~ 3 */
+
+/* OCP_EEE_AR */
+/* bit[15:14] function */
+#define FUN_ADDR		0x0000
+#define FUN_DATA		0x4000
+/* bit[4:0] device addr */
+#define DEVICE_ADDR		0x0007
+
+/* OCP_EEE_DATA */
+#define EEE_ADDR		0x003C
+#define EEE_DATA		0x0002
+
+enum rtl_register_content {
+	_100bps		= 0x08,
+	_10bps		= 0x04,
+	LINK_STATUS	= 0x02,
+	FULL_DUP	= 0x01,
+};
+
+#define RTL8152_REQT_READ	0xc0
+#define RTL8152_REQT_WRITE	0x40
+#define RTL8152_REQ_GET_REGS	0x05
+#define RTL8152_REQ_SET_REGS	0x05
+
+#define BYTE_EN_DWORD		0xff
+#define BYTE_EN_WORD		0x33
+#define BYTE_EN_BYTE		0x11
+#define BYTE_EN_SIX_BYTES	0x3f
+#define BYTE_EN_START_MASK	0x0f
+#define BYTE_EN_END_MASK	0xf0
+
+#define RTL8152_RMS		(VLAN_ETH_FRAME_LEN + VLAN_HLEN)
+#define RTL8152_TX_TIMEOUT	(HZ)
+
+/* rtl8152 flags */
+enum rtl8152_flags {
+	RTL8152_UNPLUG = 0,
+	RX_URB_FAIL,
+	RTL8152_SET_RX_MODE,
+	WORK_ENABLE
+};
+
+/* Define these values to match your device */
+#define VENDOR_ID_REALTEK		0x0bda
+#define PRODUCT_ID_RTL8152		0x8152
+
+#define MCU_TYPE_PLA			0x0100
+#define MCU_TYPE_USB			0x0000
+
+struct rx_desc {
+	u32 opts1;
+#define RX_LEN_MASK			0x7fff
+	u32 opts2;
+	u32 opts3;
+	u32 opts4;
+	u32 opts5;
+	u32 opts6;
+};
+
+struct tx_desc {
+	u32 opts1;
+#define TX_FS			(1 << 31) /* First segment of a packet */
+#define TX_LS			(1 << 30) /* Final segment of a packet */
+#define TX_LEN_MASK		0xffff
+	u32 opts2;
+};
+
+struct r8152 {
+	unsigned long flags;
+	struct usb_device *udev;
+	struct tasklet_struct tl;
+	struct net_device *netdev;
+	struct urb *rx_urb, *tx_urb;
+	struct sk_buff *tx_skb, *rx_skb;
+	struct delayed_work schedule;
+	struct mii_if_info mii;
+	u32 msg_enable;
+	u16 ocp_base;
+	u8 version;
+	u8 speed;
+};
+
+enum rtl_version {
+	RTL_VER_UNKNOWN = 0,
+	RTL_VER_01,
+	RTL_VER_02
+};
+
+/* Maximum number of multicast addresses to filter (vs. Rx-all-multicast).
+ * The RTL chips use a 64 element hash table based on the Ethernet CRC.
+ */
+static const int multicast_filter_limit = 32;
+
+static
+int get_registers(struct r8152 *tp, u16 value, u16 index, u16 size, void *data)
+{
+	return usb_control_msg(tp->udev, usb_rcvctrlpipe(tp->udev, 0),
+			       RTL8152_REQ_GET_REGS, RTL8152_REQT_READ,
+			       value, index, data, size, 500);
+}
+
+static
+int set_registers(struct r8152 *tp, u16 value, u16 index, u16 size, void *data)
+{
+	return usb_control_msg(tp->udev, usb_sndctrlpipe(tp->udev, 0),
+			       RTL8152_REQ_SET_REGS, RTL8152_REQT_WRITE,
+			       value, index, data, size, 500);
+}
+
+static int generic_ocp_read(struct r8152 *tp, u16 index, u16 size,
+				void *data, u16 type)
+{
+	u16	limit = 64;
+	int	ret = 0;
+
+	if (test_bit(RTL8152_UNPLUG, &tp->flags))
+		return -ENODEV;
+
+	/* both size and indix must be 4 bytes align */
+	if ((size & 3) || !size || (index & 3) || !data)
+		return -EPERM;
+
+	if ((u32)index + (u32)size > 0xffff)
+		return -EPERM;
+
+	while (size) {
+		if (size > limit) {
+			ret = get_registers(tp, index, type, limit, data);
+			if (ret < 0)
+				break;
+
+			index += limit;
+			data += limit;
+			size -= limit;
+		} else {
+			ret = get_registers(tp, index, type, size, data);
+			if (ret < 0)
+				break;
+
+			index += size;
+			data += size;
+			size = 0;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static int generic_ocp_write(struct r8152 *tp, u16 index, u16 byteen,
+				u16 size, void *data, u16 type)
+{
+	int	ret;
+	u16	byteen_start, byteen_end, byen;
+	u16	limit = 512;
+
+	if (test_bit(RTL8152_UNPLUG, &tp->flags))
+		return -ENODEV;
+
+	/* both size and indix must be 4 bytes align */
+	if ((size & 3) || !size || (index & 3) || !data)
+		return -EPERM;
+
+	if ((u32)index + (u32)size > 0xffff)
+		return -EPERM;
+
+	byteen_start = byteen & BYTE_EN_START_MASK;
+	byteen_end = byteen & BYTE_EN_END_MASK;
+
+	byen = byteen_start | (byteen_start << 4);
+	ret = set_registers(tp, index, type | byen, 4, data);
+	if (ret < 0)
+		goto error1;
+
+	index += 4;
+	data += 4;
+	size -= 4;
+
+	if (size) {
+		size -= 4;
+
+		while (size) {
+			if (size > limit) {
+				ret = set_registers(tp, index,
+					type | BYTE_EN_DWORD,
+					limit, data);
+				if (ret < 0)
+					goto error1;
+
+				index += limit;
+				data += limit;
+				size -= limit;
+			} else {
+				ret = set_registers(tp, index,
+					type | BYTE_EN_DWORD,
+					size, data);
+				if (ret < 0)
+					goto error1;
+
+				index += size;
+				data += size;
+				size = 0;
+				break;
+			}
+		}
+
+		byen = byteen_end | (byteen_end >> 4);
+		ret = set_registers(tp, index, type | byen, 4, data);
+		if (ret < 0)
+			goto error1;
+	}
+
+error1:
+	return ret;
+}
+
+static inline
+int pla_ocp_read(struct r8152 *tp, u16 index, u16 size, void *data)
+{
+	return generic_ocp_read(tp, index, size, data, MCU_TYPE_PLA);
+}
+
+static inline
+int pla_ocp_write(struct r8152 *tp, u16 index, u16 byteen, u16 size, void *data)
+{
+	return generic_ocp_write(tp, index, byteen, size, data, MCU_TYPE_PLA);
+}
+
+static inline
+int usb_ocp_read(struct r8152 *tp, u16 index, u16 size, void *data)
+{
+	return generic_ocp_read(tp, index, size, data, MCU_TYPE_USB);
+}
+
+static inline
+int usb_ocp_write(struct r8152 *tp, u16 index, u16 byteen, u16 size, void *data)
+{
+	return generic_ocp_write(tp, index, byteen, size, data, MCU_TYPE_USB);
+}
+
+static u32 ocp_read_dword(struct r8152 *tp, u16 type, u16 index)
+{
+	u32 data;
+
+	if (type == MCU_TYPE_PLA)
+		pla_ocp_read(tp, index, sizeof(data), &data);
+	else
+		usb_ocp_read(tp, index, sizeof(data), &data);
+
+	return __le32_to_cpu(data);
+}
+
+static void ocp_write_dword(struct r8152 *tp, u16 type, u16 index, u32 data)
+{
+	if (type == MCU_TYPE_PLA)
+		pla_ocp_write(tp, index, BYTE_EN_DWORD, sizeof(data), &data);
+	else
+		usb_ocp_write(tp, index, BYTE_EN_DWORD, sizeof(data), &data);
+}
+
+static u16 ocp_read_word(struct r8152 *tp, u16 type, u16 index)
+{
+	u32 data;
+	u8 shift = index & 2;
+
+	index &= ~3;
+
+	if (type == MCU_TYPE_PLA)
+		pla_ocp_read(tp, index, sizeof(data), &data);
+	else
+		usb_ocp_read(tp, index, sizeof(data), &data);
+
+	data = __le32_to_cpu(data);
+	data >>= (shift * 8);
+	data &= 0xffff;
+
+	return (u16)data;
+}
+
+static void ocp_write_word(struct r8152 *tp, u16 type, u16 index, u32 data)
+{
+	u32 tmp, mask = 0xffff;
+	u16 byen = BYTE_EN_WORD;
+	u8 shift = index & 2;
+
+	data &= mask;
+
+	if (index & 2) {
+		byen <<= shift;
+		mask <<= (shift * 8);
+		data <<= (shift * 8);
+		index &= ~3;
+	}
+
+	if (type == MCU_TYPE_PLA)
+		pla_ocp_read(tp, index, sizeof(tmp), &tmp);
+	else
+		usb_ocp_read(tp, index, sizeof(tmp), &tmp);
+
+	tmp = __le32_to_cpu(tmp) & ~mask;
+	tmp |= data;
+	tmp = __cpu_to_le32(tmp);
+
+	if (type == MCU_TYPE_PLA)
+		pla_ocp_write(tp, index, byen, sizeof(tmp), &tmp);
+	else
+		usb_ocp_write(tp, index, byen, sizeof(tmp), &tmp);
+}
+
+static u8 ocp_read_byte(struct r8152 *tp, u16 type, u16 index)
+{
+	u32 data;
+	u8 shift = index & 3;
+
+	index &= ~3;
+
+	if (type == MCU_TYPE_PLA)
+		pla_ocp_read(tp, index, sizeof(data), &data);
+	else
+		usb_ocp_read(tp, index, sizeof(data), &data);
+
+	data = __le32_to_cpu(data);
+	data >>= (shift * 8);
+	data &= 0xff;
+
+	return (u8)data;
+}
+
+static void ocp_write_byte(struct r8152 *tp, u16 type, u16 index, u32 data)
+{
+	u32 tmp, mask = 0xff;
+	u16 byen = BYTE_EN_BYTE;
+	u8 shift = index & 3;
+
+	data &= mask;
+
+	if (index & 3) {
+		byen <<= shift;
+		mask <<= (shift * 8);
+		data <<= (shift * 8);
+		index &= ~3;
+	}
+
+	if (type == MCU_TYPE_PLA)
+		pla_ocp_read(tp, index, sizeof(tmp), &tmp);
+	else
+		usb_ocp_read(tp, index, sizeof(tmp), &tmp);
+
+	tmp = __le32_to_cpu(tmp) & ~mask;
+	tmp |= data;
+	tmp = __cpu_to_le32(tmp);
+
+	if (type == MCU_TYPE_PLA)
+		pla_ocp_write(tp, index, byen, sizeof(tmp), &tmp);
+	else
+		usb_ocp_write(tp, index, byen, sizeof(tmp), &tmp);
+}
+
+static void r8152_mdio_write(struct r8152 *tp, u32 reg_addr, u32 value)
+{
+	u32	ocp_data;
+	int	i;
+
+	ocp_data = PHYAR_FLAG | ((reg_addr & 0x1f) << 16) |
+		   (value & 0xffff);
+
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_PHYAR, ocp_data);
+
+	for (i = 20; i > 0; i--) {
+		udelay(25);
+		ocp_data = ocp_read_dword(tp, MCU_TYPE_PLA, PLA_PHYAR);
+		if (!(ocp_data & PHYAR_FLAG))
+			break;
+	}
+	udelay(20);
+}
+
+static int r8152_mdio_read(struct r8152 *tp, u32 reg_addr)
+{
+	u32	ocp_data;
+	int	i;
+
+	ocp_data = (reg_addr & 0x1f) << 16;
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_PHYAR, ocp_data);
+
+	for (i = 20; i > 0; i--) {
+		udelay(25);
+		ocp_data = ocp_read_dword(tp, MCU_TYPE_PLA, PLA_PHYAR);
+		if (ocp_data & PHYAR_FLAG)
+			break;
+	}
+	udelay(20);
+
+	if (!(ocp_data & PHYAR_FLAG))
+		return -EAGAIN;
+
+	return (u16)(ocp_data & 0xffff);
+}
+
+static int read_mii_word(struct net_device *netdev, int phy_id, int reg)
+{
+	struct r8152 *tp = netdev_priv(netdev);
+
+	if (phy_id != R8152_PHY_ID)
+		return -EINVAL;
+
+	return r8152_mdio_read(tp, reg);
+}
+
+static
+void write_mii_word(struct net_device *netdev, int phy_id, int reg, int val)
+{
+	struct r8152 *tp = netdev_priv(netdev);
+
+	if (phy_id != R8152_PHY_ID)
+		return;
+
+	r8152_mdio_write(tp, reg, val);
+}
+
+static void ocp_reg_write(struct r8152 *tp, u16 addr, u16 data)
+{
+	u16 ocp_base, ocp_index;
+
+	ocp_base = addr & 0xf000;
+	if (ocp_base != tp->ocp_base) {
+		ocp_write_word(tp, MCU_TYPE_PLA, PLA_OCP_GPHY_BASE, ocp_base);
+		tp->ocp_base = ocp_base;
+	}
+
+	ocp_index = (addr & 0x0fff) | 0xb000;
+	ocp_write_word(tp, MCU_TYPE_PLA, ocp_index, data);
+}
+
+static inline void set_ethernet_addr(struct r8152 *tp)
+{
+	struct net_device *dev = tp->netdev;
+	u8 *node_id;
+
+	node_id = kmalloc(sizeof(u8) * 8, GFP_KERNEL);
+	if (!node_id) {
+		netif_err(tp, probe, dev, "out of memory");
+		return;
+	}
+
+	if (pla_ocp_read(tp, PLA_IDR, sizeof(u8) * 8, node_id) < 0)
+		netif_notice(tp, probe, dev, "inet addr fail\n");
+	else {
+		memcpy(dev->dev_addr, node_id, dev->addr_len);
+		memcpy(dev->perm_addr, dev->dev_addr, dev->addr_len);
+	}
+	kfree(node_id);
+}
+
+static int rtl8152_set_mac_address(struct net_device *netdev, void *p)
+{
+	struct r8152 *tp = netdev_priv(netdev);
+	struct sockaddr *addr = p;
+
+	if (!is_valid_ether_addr(addr->sa_data))
+		return -EADDRNOTAVAIL;
+
+	memcpy(netdev->dev_addr, addr->sa_data, netdev->addr_len);
+
+	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_CRWECR, CRWECR_CONFIG);
+	pla_ocp_write(tp, PLA_IDR, BYTE_EN_SIX_BYTES, 8, addr->sa_data);
+	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_CRWECR, CRWECR_NORAML);
+
+	return 0;
+}
+
+static int alloc_all_urbs(struct r8152 *tp)
+{
+	tp->rx_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!tp->rx_urb)
+		return 0;
+	tp->tx_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!tp->tx_urb) {
+		usb_free_urb(tp->rx_urb);
+		return 0;
+	}
+
+	return 1;
+}
+
+static void free_all_urbs(struct r8152 *tp)
+{
+	usb_free_urb(tp->rx_urb);
+	usb_free_urb(tp->tx_urb);
+}
+
+static struct net_device_stats *rtl8152_get_stats(struct net_device *dev)
+{
+	return &dev->stats;
+}
+
+static void read_bulk_callback(struct urb *urb)
+{
+	struct r8152 *tp;
+	unsigned pkt_len;
+	struct sk_buff *skb;
+	struct net_device *netdev;
+	struct net_device_stats *stats;
+	int status = urb->status;
+	int result;
+	struct rx_desc *rx_desc;
+
+	tp = urb->context;
+	if (!tp)
+		return;
+	if (test_bit(RTL8152_UNPLUG, &tp->flags))
+		return;
+	netdev = tp->netdev;
+	if (!netif_device_present(netdev))
+		return;
+
+	stats = rtl8152_get_stats(netdev);
+	switch (status) {
+	case 0:
+		break;
+	case -ESHUTDOWN:
+		set_bit(RTL8152_UNPLUG, &tp->flags);
+		netif_device_detach(tp->netdev);
+	case -ENOENT:
+		return;	/* the urb is in unlink state */
+	case -ETIME:
+		pr_warn_ratelimited("may be reset is needed?..\n");
+		goto goon;
+	default:
+		pr_warn_ratelimited("Rx status %d\n", status);
+		goto goon;
+	}
+
+	/* protect against short packets (tell me why we got some?!?) */
+	if (urb->actual_length < sizeof(*rx_desc))
+		goto goon;
+
+
+	rx_desc = (struct rx_desc *)urb->transfer_buffer;
+	pkt_len = le32_to_cpu(rx_desc->opts1) & RX_LEN_MASK;
+	if (urb->actual_length < sizeof(struct rx_desc) + pkt_len)
+		goto goon;
+
+	skb = netdev_alloc_skb_ip_align(netdev, pkt_len);
+	if (!skb)
+		goto goon;
+
+	memcpy(skb->data, tp->rx_skb->data + sizeof(struct rx_desc), pkt_len);
+	skb_put(skb, pkt_len);
+	skb->protocol = eth_type_trans(skb, netdev);
+	netif_rx(skb);
+	stats->rx_packets++;
+	stats->rx_bytes += pkt_len;
+goon:
+	usb_fill_bulk_urb(tp->rx_urb, tp->udev, usb_rcvbulkpipe(tp->udev, 1),
+		      tp->rx_skb->data, RTL8152_RMS + sizeof(struct rx_desc),
+		      (usb_complete_t)read_bulk_callback, tp);
+	result = usb_submit_urb(tp->rx_urb, GFP_ATOMIC);
+	if (result == -ENODEV) {
+		netif_device_detach(tp->netdev);
+	} else if (result) {
+		set_bit(RX_URB_FAIL, &tp->flags);
+		goto resched;
+	} else {
+		clear_bit(RX_URB_FAIL, &tp->flags);
+	}
+
+	return;
+resched:
+	tasklet_schedule(&tp->tl);
+}
+
+static void rx_fixup(unsigned long data)
+{
+	struct r8152 *tp;
+	int status;
+
+	tp = (struct r8152 *)data;
+	if (!test_bit(WORK_ENABLE, &tp->flags))
+		return;
+
+	status = usb_submit_urb(tp->rx_urb, GFP_ATOMIC);
+	if (status == -ENODEV) {
+		netif_device_detach(tp->netdev);
+	} else if (status) {
+		set_bit(RX_URB_FAIL, &tp->flags);
+		goto tlsched;
+	} else {
+		clear_bit(RX_URB_FAIL, &tp->flags);
+	}
+
+	return;
+tlsched:
+	tasklet_schedule(&tp->tl);
+}
+
+static void write_bulk_callback(struct urb *urb)
+{
+	struct r8152 *tp;
+	int status = urb->status;
+
+	tp = urb->context;
+	if (!tp)
+		return;
+	dev_kfree_skb_irq(tp->tx_skb);
+	if (!netif_device_present(tp->netdev))
+		return;
+	if (status)
+		dev_info(&urb->dev->dev, "%s: Tx status %d\n",
+			 tp->netdev->name, status);
+	tp->netdev->trans_start = jiffies;
+	netif_wake_queue(tp->netdev);
+}
+
+static void rtl8152_tx_timeout(struct net_device *netdev)
+{
+	struct r8152 *tp = netdev_priv(netdev);
+	struct net_device_stats *stats = rtl8152_get_stats(netdev);
+	netif_warn(tp, tx_err, netdev, "Tx timeout.\n");
+	usb_unlink_urb(tp->tx_urb);
+	stats->tx_errors++;
+}
+
+static void rtl8152_set_rx_mode(struct net_device *netdev)
+{
+	struct r8152 *tp = netdev_priv(netdev);
+
+	if (tp->speed & LINK_STATUS)
+		set_bit(RTL8152_SET_RX_MODE, &tp->flags);
+}
+
+static void _rtl8152_set_rx_mode(struct net_device *netdev)
+{
+	struct r8152 *tp = netdev_priv(netdev);
+	u32 tmp, *mc_filter;	/* Multicast hash filter */
+	u32 ocp_data;
+
+	mc_filter = kmalloc(sizeof(u32) * 2, GFP_KERNEL);
+	if (!mc_filter) {
+		netif_err(tp, link, netdev, "out of memory");
+		return;
+	}
+
+	clear_bit(RTL8152_SET_RX_MODE, &tp->flags);
+	netif_stop_queue(netdev);
+	ocp_data = ocp_read_dword(tp, MCU_TYPE_PLA, PLA_RCR);
+	ocp_data &= ~RCR_ACPT_ALL;
+	ocp_data |= RCR_AB | RCR_APM;
+
+	if (netdev->flags & IFF_PROMISC) {
+		/* Unconditionally log net taps. */
+		netif_notice(tp, link, netdev, "Promiscuous mode enabled\n");
+		ocp_data |= RCR_AM | RCR_AAP;
+		mc_filter[1] = mc_filter[0] = 0xffffffff;
+	} else if ((netdev_mc_count(netdev) > multicast_filter_limit) ||
+		   (netdev->flags & IFF_ALLMULTI)) {
+		/* Too many to filter perfectly -- accept all multicasts. */
+		ocp_data |= RCR_AM;
+		mc_filter[1] = mc_filter[0] = 0xffffffff;
+	} else {
+		struct netdev_hw_addr *ha;
+
+		mc_filter[1] = mc_filter[0] = 0;
+		netdev_for_each_mc_addr(ha, netdev) {
+			int bit_nr = ether_crc(ETH_ALEN, ha->addr) >> 26;
+			mc_filter[bit_nr >> 5] |= 1 << (bit_nr & 31);
+			ocp_data |= RCR_AM;
+		}
+	}
+
+	tmp = mc_filter[0];
+	mc_filter[0] = __cpu_to_le32(swab32(mc_filter[1]));
+	mc_filter[1] = __cpu_to_le32(swab32(tmp));
+
+	pla_ocp_write(tp, PLA_MAR, BYTE_EN_DWORD, sizeof(u32) * 2, mc_filter);
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RCR, ocp_data);
+	netif_wake_queue(netdev);
+	kfree(mc_filter);
+}
+
+static netdev_tx_t rtl8152_start_xmit(struct sk_buff *skb,
+					    struct net_device *netdev)
+{
+	struct r8152 *tp = netdev_priv(netdev);
+	struct net_device_stats *stats = rtl8152_get_stats(netdev);
+	struct tx_desc *tx_desc;
+	int len, res;
+
+	netif_stop_queue(netdev);
+	len = skb->len;
+	if (skb_header_cloned(skb) || skb_headroom(skb) < sizeof(*tx_desc)) {
+		struct sk_buff *tx_skb;
+
+		tx_skb = skb_copy_expand(skb, sizeof(*tx_desc), 0, GFP_ATOMIC);
+		dev_kfree_skb_any(skb);
+		if (!tx_skb) {
+			stats->tx_dropped++;
+			netif_wake_queue(netdev);
+			return NETDEV_TX_OK;
+		}
+		skb = tx_skb;
+	}
+	tx_desc = (struct tx_desc *)skb_push(skb, sizeof(*tx_desc));
+	memset(tx_desc, 0, sizeof(*tx_desc));
+	tx_desc->opts1 = cpu_to_le32((len & TX_LEN_MASK) | TX_FS | TX_LS);
+	tp->tx_skb = skb;
+	skb_tx_timestamp(skb);
+	usb_fill_bulk_urb(tp->tx_urb, tp->udev, usb_sndbulkpipe(tp->udev, 2),
+			  skb->data, skb->len,
+			  (usb_complete_t)write_bulk_callback, tp);
+	res = usb_submit_urb(tp->tx_urb, GFP_ATOMIC);
+	if (res) {
+		/* Can we get/handle EPIPE here? */
+		if (res == -ENODEV) {
+			netif_device_detach(tp->netdev);
+		} else {
+			netif_warn(tp, tx_err, netdev,
+				   "failed tx_urb %d\n", res);
+			stats->tx_errors++;
+			netif_start_queue(netdev);
+		}
+	} else {
+		stats->tx_packets++;
+		stats->tx_bytes += skb->len;
+	}
+
+	return NETDEV_TX_OK;
+}
+
+static void r8152b_reset_packet_filter(struct r8152 *tp)
+{
+	u32	ocp_data;
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_FMC);
+	ocp_data &= ~FMC_FCR_MCU_EN;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_FMC, ocp_data);
+	ocp_data |= FMC_FCR_MCU_EN;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_FMC, ocp_data);
+}
+
+static void rtl8152_nic_reset(struct r8152 *tp)
+{
+	int	i;
+
+	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_CR, CR_RST);
+
+	for (i = 0; i < 1000; i++) {
+		if (!(ocp_read_byte(tp, MCU_TYPE_PLA, PLA_CR) & CR_RST))
+			break;
+		udelay(100);
+	}
+}
+
+static inline u8 rtl8152_get_speed(struct r8152 *tp)
+{
+	return ocp_read_byte(tp, MCU_TYPE_PLA, PLA_PHYSTATUS);
+}
+
+static int rtl8152_enable(struct r8152 *tp)
+{
+	u32	ocp_data;
+	u8 speed;
+
+	speed = rtl8152_get_speed(tp);
+	if (speed & _100bps) {
+		ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_EEEP_CR);
+		ocp_data &= ~EEEP_CR_EEEP_TX;
+		ocp_write_word(tp, MCU_TYPE_PLA, PLA_EEEP_CR, ocp_data);
+	} else {
+		ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_EEEP_CR);
+		ocp_data |= EEEP_CR_EEEP_TX;
+		ocp_write_word(tp, MCU_TYPE_PLA, PLA_EEEP_CR, ocp_data);
+	}
+
+	r8152b_reset_packet_filter(tp);
+
+	ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_CR);
+	ocp_data |= CR_RE | CR_TE;
+	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_CR, ocp_data);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_MISC_1);
+	ocp_data &= ~RXDY_GATED_EN;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_MISC_1, ocp_data);
+
+	usb_fill_bulk_urb(tp->rx_urb, tp->udev, usb_rcvbulkpipe(tp->udev, 1),
+		      tp->rx_skb->data, RTL8152_RMS + sizeof(struct rx_desc),
+		      (usb_complete_t)read_bulk_callback, tp);
+
+	return usb_submit_urb(tp->rx_urb, GFP_KERNEL);
+}
+
+static void rtl8152_disable(struct r8152 *tp)
+{
+	u32	ocp_data;
+	int	i;
+
+	ocp_data = ocp_read_dword(tp, MCU_TYPE_PLA, PLA_RCR);
+	ocp_data &= ~RCR_ACPT_ALL;
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RCR, ocp_data);
+
+	usb_kill_urb(tp->tx_urb);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_MISC_1);
+	ocp_data |= RXDY_GATED_EN;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_MISC_1, ocp_data);
+
+	for (i = 0; i < 1000; i++) {
+		ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
+		if ((ocp_data & FIFO_EMPTY) == FIFO_EMPTY)
+			break;
+		mdelay(1);
+	}
+
+	for (i = 0; i < 1000; i++) {
+		if (ocp_read_word(tp, MCU_TYPE_PLA, PLA_TCR0) & TCR0_TX_EMPTY)
+			break;
+		mdelay(1);
+	}
+
+	usb_kill_urb(tp->rx_urb);
+
+	rtl8152_nic_reset(tp);
+}
+
+static void r8152b_exit_oob(struct r8152 *tp)
+{
+	u32	ocp_data;
+	int	i;
+
+	ocp_data = ocp_read_dword(tp, MCU_TYPE_PLA, PLA_RCR);
+	ocp_data &= ~RCR_ACPT_ALL;
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RCR, ocp_data);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_MISC_1);
+	ocp_data |= RXDY_GATED_EN;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_MISC_1, ocp_data);
+
+	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_CRWECR, CRWECR_NORAML);
+	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_CR, 0x00);
+
+	ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
+	ocp_data &= ~NOW_IS_OOB;
+	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL, ocp_data);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_SFF_STS_7);
+	ocp_data &= ~MCU_BORW_EN;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_SFF_STS_7, ocp_data);
+
+	for (i = 0; i < 1000; i++) {
+		ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
+		if (ocp_data & LINK_LIST_READY)
+			break;
+		mdelay(1);
+	}
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_SFF_STS_7);
+	ocp_data |= RE_INIT_LL;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_SFF_STS_7, ocp_data);
+
+	for (i = 0; i < 1000; i++) {
+		ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
+		if (ocp_data & LINK_LIST_READY)
+			break;
+		mdelay(1);
+	}
+
+	rtl8152_nic_reset(tp);
+
+	/* rx share fifo credit full threshold */
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RXFIFO_CTRL0, RXFIFO_THR1_NORMAL);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_DEV_STAT);
+	ocp_data &= STAT_SPEED_MASK;
+	if (ocp_data == STAT_SPEED_FULL) {
+		/* rx share fifo credit near full threshold */
+		ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RXFIFO_CTRL1,
+				RXFIFO_THR2_FULL);
+		ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RXFIFO_CTRL2,
+				RXFIFO_THR3_FULL);
+	} else {
+		/* rx share fifo credit near full threshold */
+		ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RXFIFO_CTRL1,
+				RXFIFO_THR2_HIGH);
+		ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RXFIFO_CTRL2,
+				RXFIFO_THR3_HIGH);
+	}
+
+	/* TX share fifo free credit full threshold */
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_TXFIFO_CTRL, TXFIFO_THR_NORMAL);
+
+	ocp_write_byte(tp, MCU_TYPE_USB, USB_TX_AGG, TX_AGG_MAX_THRESHOLD);
+	ocp_write_dword(tp, MCU_TYPE_USB, USB_RX_BUF_TH, RX_BUF_THR);
+	ocp_write_dword(tp, MCU_TYPE_USB, USB_TX_DMA,
+			TEST_MODE_DISABLE | TX_SIZE_ADJUST1);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_CPCR);
+	ocp_data &= ~CPCR_RX_VLAN;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_CPCR, ocp_data);
+
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_RMS, RTL8152_RMS);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_TCR0);
+	ocp_data |= TCR0_AUTO_FIFO;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_TCR0, ocp_data);
+}
+
+static void r8152b_enter_oob(struct r8152 *tp)
+{
+	u32	ocp_data;
+	int	i;
+
+	ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
+	ocp_data &= ~NOW_IS_OOB;
+	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL, ocp_data);
+
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RXFIFO_CTRL0, RXFIFO_THR1_OOB);
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RXFIFO_CTRL1, RXFIFO_THR2_OOB);
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RXFIFO_CTRL2, RXFIFO_THR3_OOB);
+
+	rtl8152_disable(tp);
+
+	for (i = 0; i < 1000; i++) {
+		ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
+		if (ocp_data & LINK_LIST_READY)
+			break;
+		mdelay(1);
+	}
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_SFF_STS_7);
+	ocp_data |= RE_INIT_LL;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_SFF_STS_7, ocp_data);
+
+	for (i = 0; i < 1000; i++) {
+		ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
+		if (ocp_data & LINK_LIST_READY)
+			break;
+		mdelay(1);
+	}
+
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_RMS, RTL8152_RMS);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_CFG_WOL);
+	ocp_data |= MAGIC_EN;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_CFG_WOL, ocp_data);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_CPCR);
+	ocp_data |= CPCR_RX_VLAN;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_CPCR, ocp_data);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PAL_BDC_CR);
+	ocp_data |= ALDPS_PROXY_MODE;
+	ocp_write_word(tp, MCU_TYPE_PLA, PAL_BDC_CR, ocp_data);
+
+	ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
+	ocp_data |= NOW_IS_OOB | DIS_MCU_CLROOB;
+	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL, ocp_data);
+
+	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_CONFIG5, LAN_WAKE_EN);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_MISC_1);
+	ocp_data &= ~RXDY_GATED_EN;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_MISC_1, ocp_data);
+
+	ocp_data = ocp_read_dword(tp, MCU_TYPE_PLA, PLA_RCR);
+	ocp_data |= RCR_APM | RCR_AM | RCR_AB;
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RCR, ocp_data);
+}
+
+static void r8152b_disable_aldps(struct r8152 *tp)
+{
+	ocp_reg_write(tp, OCP_ALDPS_CONFIG, ENPDNPS | LINKENA | DIS_SDSAVE);
+	msleep(20);
+}
+
+static inline void r8152b_enable_aldps(struct r8152 *tp)
+{
+	ocp_reg_write(tp, OCP_ALDPS_CONFIG, ENPWRSAVE | ENPDNPS |
+					    LINKENA | DIS_SDSAVE);
+}
+
+static int rtl8152_set_speed(struct r8152 *tp, u8 autoneg, u16 speed, u8 duplex)
+{
+	u16 bmcr, anar;
+	int ret = 0;
+
+	cancel_delayed_work_sync(&tp->schedule);
+	anar = r8152_mdio_read(tp, MII_ADVERTISE);
+	anar &= ~(ADVERTISE_10HALF | ADVERTISE_10FULL |
+		  ADVERTISE_100HALF | ADVERTISE_100FULL);
+
+	if (autoneg == AUTONEG_DISABLE) {
+		if (speed == SPEED_10) {
+			bmcr = 0;
+			anar |= ADVERTISE_10HALF | ADVERTISE_10FULL;
+		} else if (speed == SPEED_100) {
+			bmcr = BMCR_SPEED100;
+			anar |= ADVERTISE_100HALF | ADVERTISE_100FULL;
+		} else {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		if (duplex == DUPLEX_FULL)
+			bmcr |= BMCR_FULLDPLX;
+	} else {
+		if (speed == SPEED_10) {
+			if (duplex == DUPLEX_FULL)
+				anar |= ADVERTISE_10HALF | ADVERTISE_10FULL;
+			else
+				anar |= ADVERTISE_10HALF;
+		} else if (speed == SPEED_100) {
+			if (duplex == DUPLEX_FULL) {
+				anar |= ADVERTISE_10HALF | ADVERTISE_10FULL;
+				anar |= ADVERTISE_100HALF | ADVERTISE_100FULL;
+			} else {
+				anar |= ADVERTISE_10HALF;
+				anar |= ADVERTISE_100HALF;
+			}
+		} else {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		bmcr = BMCR_ANENABLE | BMCR_ANRESTART;
+	}
+
+	r8152_mdio_write(tp, MII_ADVERTISE, anar);
+	r8152_mdio_write(tp, MII_BMCR, bmcr);
+
+out:
+	schedule_delayed_work(&tp->schedule, 5 * HZ);
+
+	return ret;
+}
+
+static void rtl8152_down(struct r8152 *tp)
+{
+	u32	ocp_data;
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_UPS_CTRL);
+	ocp_data &= ~POWER_CUT;
+	ocp_write_word(tp, MCU_TYPE_USB, USB_UPS_CTRL, ocp_data);
+
+	r8152b_disable_aldps(tp);
+	r8152b_enter_oob(tp);
+	r8152b_enable_aldps(tp);
+}
+
+static void set_carrier(struct r8152 *tp)
+{
+	struct net_device *netdev = tp->netdev;
+	u8 speed;
+
+	speed = rtl8152_get_speed(tp);
+
+	if (speed & LINK_STATUS) {
+		if (!(tp->speed & LINK_STATUS)) {
+			rtl8152_enable(tp);
+			set_bit(RTL8152_SET_RX_MODE, &tp->flags);
+			netif_carrier_on(netdev);
+		}
+	} else {
+		if (tp->speed & LINK_STATUS) {
+			netif_carrier_off(netdev);
+			rtl8152_disable(tp);
+		}
+	}
+	tp->speed = speed;
+}
+
+static void rtl_work_func_t(struct work_struct *work)
+{
+	struct r8152 *tp = container_of(work, struct r8152, schedule.work);
+
+	if (!test_bit(WORK_ENABLE, &tp->flags))
+		goto out1;
+
+	if (test_bit(RTL8152_UNPLUG, &tp->flags))
+		goto out1;
+
+	set_carrier(tp);
+
+	if (test_bit(RTL8152_SET_RX_MODE, &tp->flags))
+		_rtl8152_set_rx_mode(tp->netdev);
+
+	schedule_delayed_work(&tp->schedule, HZ);
+
+out1:
+	return;
+}
+
+static int rtl8152_open(struct net_device *netdev)
+{
+	struct r8152 *tp = netdev_priv(netdev);
+	int res = 0;
+
+	tp->speed = rtl8152_get_speed(tp);
+	if (tp->speed & LINK_STATUS) {
+		res = rtl8152_enable(tp);
+		if (res) {
+			if (res == -ENODEV)
+				netif_device_detach(tp->netdev);
+
+			netif_err(tp, ifup, netdev,
+				  "rtl8152_open failed: %d\n", res);
+			return res;
+		}
+
+		netif_carrier_on(netdev);
+	} else {
+		netif_stop_queue(netdev);
+		netif_carrier_off(netdev);
+	}
+
+	rtl8152_set_speed(tp, AUTONEG_ENABLE, SPEED_100, DUPLEX_FULL);
+	netif_start_queue(netdev);
+	set_bit(WORK_ENABLE, &tp->flags);
+	schedule_delayed_work(&tp->schedule, 0);
+
+	return res;
+}
+
+static int rtl8152_close(struct net_device *netdev)
+{
+	struct r8152 *tp = netdev_priv(netdev);
+	int res = 0;
+
+	clear_bit(WORK_ENABLE, &tp->flags);
+	cancel_delayed_work_sync(&tp->schedule);
+	netif_stop_queue(netdev);
+	rtl8152_disable(tp);
+
+	return res;
+}
+
+static void rtl_clear_bp(struct r8152 *tp)
+{
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_BP_0, 0);
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_BP_2, 0);
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_BP_4, 0);
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_BP_6, 0);
+	ocp_write_dword(tp, MCU_TYPE_USB, USB_BP_0, 0);
+	ocp_write_dword(tp, MCU_TYPE_USB, USB_BP_2, 0);
+	ocp_write_dword(tp, MCU_TYPE_USB, USB_BP_4, 0);
+	ocp_write_dword(tp, MCU_TYPE_USB, USB_BP_6, 0);
+	mdelay(3);
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_BP_BA, 0);
+	ocp_write_word(tp, MCU_TYPE_USB, USB_BP_BA, 0);
+}
+
+static void r8152b_enable_eee(struct r8152 *tp)
+{
+	u32	ocp_data;
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_EEE_CR);
+	ocp_data |= EEE_RX_EN | EEE_TX_EN;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_EEE_CR, ocp_data);
+	ocp_reg_write(tp, OCP_EEE_CONFIG1, RG_TXLPI_MSK_HFDUP | RG_MATCLR_EN |
+					   EEE_10_CAP | EEE_NWAY_EN |
+					   TX_QUIET_EN | RX_QUIET_EN |
+					   SDRISETIME | RG_RXLPI_MSK_HFDUP |
+					   SDFALLTIME);
+	ocp_reg_write(tp, OCP_EEE_CONFIG2, RG_LPIHYS_NUM | RG_DACQUIET_EN |
+					   RG_LDVQUIET_EN | RG_CKRSEL |
+					   RG_EEEPRG_EN);
+	ocp_reg_write(tp, OCP_EEE_CONFIG3, FST_SNR_EYE_R | RG_LFS_SEL | MSK_PH);
+	ocp_reg_write(tp, OCP_EEE_AR, FUN_ADDR | DEVICE_ADDR);
+	ocp_reg_write(tp, OCP_EEE_DATA, EEE_ADDR);
+	ocp_reg_write(tp, OCP_EEE_AR, FUN_DATA | DEVICE_ADDR);
+	ocp_reg_write(tp, OCP_EEE_DATA, EEE_DATA);
+	ocp_reg_write(tp, OCP_EEE_AR, 0x0000);
+}
+
+static void r8152b_enable_fc(struct r8152 *tp)
+{
+	u16 anar;
+
+	anar = r8152_mdio_read(tp, MII_ADVERTISE);
+	anar |= ADVERTISE_PAUSE_CAP | ADVERTISE_PAUSE_ASYM;
+	r8152_mdio_write(tp, MII_ADVERTISE, anar);
+}
+
+static void r8152b_hw_phy_cfg(struct r8152 *tp)
+{
+	r8152_mdio_write(tp, MII_BMCR, BMCR_ANENABLE);
+	r8152b_disable_aldps(tp);
+}
+
+static void r8152b_init(struct r8152 *tp)
+{
+	u32	ocp_data;
+	int	i;
+
+	rtl_clear_bp(tp);
+
+	if (tp->version == RTL_VER_01) {
+		ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_LED_FEATURE);
+		ocp_data &= ~LED_MODE_MASK;
+		ocp_write_word(tp, MCU_TYPE_PLA, PLA_LED_FEATURE, ocp_data);
+	}
+
+	r8152b_hw_phy_cfg(tp);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_UPS_CTRL);
+	ocp_data &= ~POWER_CUT;
+	ocp_write_word(tp, MCU_TYPE_USB, USB_UPS_CTRL, ocp_data);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_PM_CTRL_STATUS);
+	ocp_data &= ~RWSUME_INDICATE;
+	ocp_write_word(tp, MCU_TYPE_USB, USB_PM_CTRL_STATUS, ocp_data);
+
+	r8152b_exit_oob(tp);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_PHY_PWR);
+	ocp_data |= TX_10M_IDLE_EN | PFM_PWM_SWITCH;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_PHY_PWR, ocp_data);
+	ocp_data = ocp_read_dword(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL);
+	ocp_data &= ~MCU_CLK_RATIO_MASK;
+	ocp_data |= MCU_CLK_RATIO | D3_CLK_GATED_EN;
+	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL, ocp_data);
+	ocp_data = GPHY_STS_MSK | SPEED_DOWN_MSK |
+		   SPDWN_RXDV_MSK | SPDWN_LINKCHG_MSK;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_GPHY_INTR_IMR, ocp_data);
+
+	r8152b_enable_eee(tp);
+	r8152b_enable_aldps(tp);
+	r8152b_enable_fc(tp);
+
+	r8152_mdio_write(tp, MII_BMCR, BMCR_RESET | BMCR_ANENABLE |
+				       BMCR_ANRESTART);
+	for (i = 0; i < 100; i++) {
+		udelay(100);
+		if (!(r8152_mdio_read(tp, MII_BMCR) & BMCR_RESET))
+			break;
+	}
+
+	/* disable rx aggregation */
+	ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_USB_CTRL);
+	ocp_data |= RX_AGG_DISABLE;
+	ocp_write_word(tp, MCU_TYPE_USB, USB_USB_CTRL, ocp_data);
+}
+
+static int rtl8152_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	struct r8152 *tp = usb_get_intfdata(intf);
+
+	netif_device_detach(tp->netdev);
+
+	if (netif_running(tp->netdev)) {
+		clear_bit(WORK_ENABLE, &tp->flags);
+		cancel_delayed_work_sync(&tp->schedule);
+	}
+
+	rtl8152_down(tp);
+
+	return 0;
+}
+
+static int rtl8152_resume(struct usb_interface *intf)
+{
+	struct r8152 *tp = usb_get_intfdata(intf);
+
+	r8152b_init(tp);
+	netif_device_attach(tp->netdev);
+	if (netif_running(tp->netdev)) {
+		rtl8152_enable(tp);
+		set_bit(WORK_ENABLE, &tp->flags);
+		set_bit(RTL8152_SET_RX_MODE, &tp->flags);
+		schedule_delayed_work(&tp->schedule, 0);
+	}
+
+	return 0;
+}
+
+static void rtl8152_get_drvinfo(struct net_device *netdev,
+				struct ethtool_drvinfo *info)
+{
+	struct r8152 *tp = netdev_priv(netdev);
+
+	strncpy(info->driver, MODULENAME, ETHTOOL_BUSINFO_LEN);
+	strncpy(info->version, DRIVER_VERSION, ETHTOOL_BUSINFO_LEN);
+	usb_make_path(tp->udev, info->bus_info, sizeof(info->bus_info));
+}
+
+static
+int rtl8152_get_settings(struct net_device *netdev, struct ethtool_cmd *cmd)
+{
+	struct r8152 *tp = netdev_priv(netdev);
+
+	if (!tp->mii.mdio_read)
+		return -EOPNOTSUPP;
+
+	return mii_ethtool_gset(&tp->mii, cmd);
+}
+
+static int rtl8152_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
+{
+	struct r8152 *tp = netdev_priv(dev);
+
+	return rtl8152_set_speed(tp, cmd->autoneg, cmd->speed, cmd->duplex);
+}
+
+static struct ethtool_ops ops = {
+	.get_drvinfo = rtl8152_get_drvinfo,
+	.get_settings = rtl8152_get_settings,
+	.set_settings = rtl8152_set_settings,
+	.get_link = ethtool_op_get_link,
+};
+
+static int rtl8152_ioctl(struct net_device *netdev, struct ifreq *rq, int cmd)
+{
+	struct r8152 *tp = netdev_priv(netdev);
+	struct mii_ioctl_data *data = if_mii(rq);
+	int res = 0;
+
+	switch (cmd) {
+	case SIOCGMIIPHY:
+		data->phy_id = R8152_PHY_ID; /* Internal PHY */
+		break;
+
+	case SIOCGMIIREG:
+		data->val_out = r8152_mdio_read(tp, data->reg_num);
+		break;
+
+	case SIOCSMIIREG:
+		if (!capable(CAP_NET_ADMIN)) {
+			res = -EPERM;
+			break;
+		}
+		r8152_mdio_write(tp, data->reg_num, data->val_in);
+		break;
+
+	default:
+		res = -EOPNOTSUPP;
+	}
+
+	return res;
+}
+
+static const struct net_device_ops rtl8152_netdev_ops = {
+	.ndo_open		= rtl8152_open,
+	.ndo_stop		= rtl8152_close,
+	.ndo_do_ioctl		= rtl8152_ioctl,
+	.ndo_start_xmit		= rtl8152_start_xmit,
+	.ndo_tx_timeout		= rtl8152_tx_timeout,
+	.ndo_set_rx_mode	= rtl8152_set_rx_mode,
+	.ndo_set_mac_address	= rtl8152_set_mac_address,
+
+	.ndo_change_mtu		= eth_change_mtu,
+	.ndo_validate_addr	= eth_validate_addr,
+};
+
+static void r8152b_get_version(struct r8152 *tp)
+{
+	u32	ocp_data;
+	u16	version;
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_TCR1);
+	version = (u16)(ocp_data & VERSION_MASK);
+
+	switch (version) {
+	case 0x4c00:
+		tp->version = RTL_VER_01;
+		break;
+	case 0x4c10:
+		tp->version = RTL_VER_02;
+		break;
+	default:
+		netif_info(tp, probe, tp->netdev,
+			   "Unknown version 0x%04x\n", version);
+		break;
+	}
+}
+
+static int rtl8152_probe(struct usb_interface *intf,
+			 const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct r8152 *tp;
+	struct net_device *netdev;
+
+	if (udev->actconfig->desc.bConfigurationValue != 1) {
+		usb_driver_set_configuration(udev, 1);
+		return -ENODEV;
+	}
+
+	netdev = alloc_etherdev(sizeof(struct r8152));
+	if (!netdev) {
+		dev_err(&intf->dev, "Out of memory");
+		return -ENOMEM;
+	}
+
+	tp = netdev_priv(netdev);
+	tp->msg_enable = 0x7FFF;
+
+	tasklet_init(&tp->tl, rx_fixup, (unsigned long)tp);
+	INIT_DELAYED_WORK(&tp->schedule, rtl_work_func_t);
+
+	tp->udev = udev;
+	tp->netdev = netdev;
+	netdev->netdev_ops = &rtl8152_netdev_ops;
+	netdev->watchdog_timeo = RTL8152_TX_TIMEOUT;
+	netdev->features &= ~NETIF_F_IP_CSUM;
+	SET_ETHTOOL_OPS(netdev, &ops);
+	tp->speed = 0;
+
+	tp->mii.dev = netdev;
+	tp->mii.mdio_read = read_mii_word;
+	tp->mii.mdio_write = write_mii_word;
+	tp->mii.phy_id_mask = 0x3f;
+	tp->mii.reg_num_mask = 0x1f;
+	tp->mii.phy_id = R8152_PHY_ID;
+	tp->mii.supports_gmii = 0;
+
+	r8152b_get_version(tp);
+	r8152b_init(tp);
+	set_ethernet_addr(tp);
+
+	if (!alloc_all_urbs(tp)) {
+		netif_err(tp, probe, netdev, "out of memory");
+		goto out;
+	}
+
+	tp->rx_skb = netdev_alloc_skb(netdev,
+			RTL8152_RMS + sizeof(struct rx_desc));
+	if (!tp->rx_skb)
+		goto out1;
+
+	usb_set_intfdata(intf, tp);
+	SET_NETDEV_DEV(netdev, &intf->dev);
+
+
+	if (register_netdev(netdev) != 0) {
+		netif_err(tp, probe, netdev, "couldn't register the device");
+		goto out2;
+	}
+
+	netif_info(tp, probe, netdev, "%s", DRIVER_VERSION);
+
+	return 0;
+
+out2:
+	usb_set_intfdata(intf, NULL);
+	dev_kfree_skb(tp->rx_skb);
+out1:
+	free_all_urbs(tp);
+out:
+	free_netdev(netdev);
+	return -EIO;
+}
+
+static void rtl8152_unload(struct r8152 *tp)
+{
+	u32	ocp_data;
+
+	if (tp->version != RTL_VER_01) {
+		ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_UPS_CTRL);
+		ocp_data |= POWER_CUT;
+		ocp_write_word(tp, MCU_TYPE_USB, USB_UPS_CTRL, ocp_data);
+	}
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_PM_CTRL_STATUS);
+	ocp_data &= ~RWSUME_INDICATE;
+	ocp_write_word(tp, MCU_TYPE_USB, USB_PM_CTRL_STATUS, ocp_data);
+}
+
+static void rtl8152_disconnect(struct usb_interface *intf)
+{
+	struct r8152 *tp = usb_get_intfdata(intf);
+
+	usb_set_intfdata(intf, NULL);
+	if (tp) {
+		set_bit(RTL8152_UNPLUG, &tp->flags);
+		tasklet_kill(&tp->tl);
+		unregister_netdev(tp->netdev);
+		rtl8152_unload(tp);
+		free_all_urbs(tp);
+		if (tp->rx_skb)
+			dev_kfree_skb(tp->rx_skb);
+		free_netdev(tp->netdev);
+	}
+}
+
+/* table of devices that work with this driver */
+static struct usb_device_id rtl8152_table[] = {
+	{USB_DEVICE(VENDOR_ID_REALTEK, PRODUCT_ID_RTL8152)},
+	{}
+};
+
+MODULE_DEVICE_TABLE(usb, rtl8152_table);
+
+static struct usb_driver rtl8152_driver = {
+	.name =		MODULENAME,
+	.probe =	rtl8152_probe,
+	.disconnect =	rtl8152_disconnect,
+	.id_table =	rtl8152_table,
+	.suspend =	rtl8152_suspend,
+	.resume =	rtl8152_resume
+};
+
+static int __init usb_rtl8152_init(void)
+{
+	return usb_register(&rtl8152_driver);
+}
+
+static void __exit usb_rtl8152_exit(void)
+{
+	usb_deregister(&rtl8152_driver);
+}
+
+module_init(usb_rtl8152_init);
+module_exit(usb_rtl8152_exit);
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/usb/sierra_net.c b/drivers/net/usb/sierra_net.c
index 79ab2435d9d3..a79e9d334928 100644
--- a/drivers/net/usb/sierra_net.c
+++ b/drivers/net/usb/sierra_net.c
@@ -413,11 +413,10 @@ static void sierra_net_handle_lsi(struct usbnet *dev, char *data,
 	if (link_up) {
 		sierra_net_set_ctx_index(priv, hh->msgspecific.byte);
 		priv->link_up = 1;
-		netif_carrier_on(dev->net);
 	} else {
 		priv->link_up = 0;
-		netif_carrier_off(dev->net);
 	}
+	usbnet_link_change(dev, link_up, 0);
 }
 
 static void sierra_net_dosync(struct usbnet *dev)
@@ -427,6 +426,13 @@ static void sierra_net_dosync(struct usbnet *dev)
 
 	dev_dbg(&dev->udev->dev, "%s", __func__);
 
+	/* The SIERRA_NET_HIP_MSYNC_ID command appears to request that the
+	 * firmware restart itself.  After restarting, the modem will respond
+	 * with the SIERRA_NET_HIP_RESTART_ID indication.  The driver continues
+	 * sending MSYNC commands every few seconds until it receives the
+	 * RESTART event from the firmware
+	 */
+
 	/* tell modem we are ready */
 	status = sierra_net_send_sync(dev);
 	if (status < 0)
@@ -705,6 +711,9 @@ static int sierra_net_bind(struct usbnet *dev, struct usb_interface *intf)
 	/* set context index initially to 0 - prepares tx hdr template */
 	sierra_net_set_ctx_index(priv, 0);
 
+	/* prepare sync message template */
+	memcpy(priv->sync_msg, sync_tmplate, sizeof(priv->sync_msg));
+
 	/* decrease the rx_urb_size and max_tx_size to 4k on USB 1.1 */
 	dev->rx_urb_size  = SIERRA_NET_RX_URB_SIZE;
 	if (dev->udev->speed != USB_SPEED_HIGH)
@@ -740,11 +749,6 @@ static int sierra_net_bind(struct usbnet *dev, struct usb_interface *intf)
 		kfree(priv);
 		return -ENODEV;
 	}
-	/* prepare sync message from template */
-	memcpy(priv->sync_msg, sync_tmplate, sizeof(priv->sync_msg));
-
-	/* initiate the sync sequence */
-	sierra_net_dosync(dev);
 
 	return 0;
 }
@@ -767,8 +771,9 @@ static void sierra_net_unbind(struct usbnet *dev, struct usb_interface *intf)
 		netdev_err(dev->net,
 			"usb_control_msg failed, status %d\n", status);
 
-	sierra_net_set_private(dev, NULL);
+	usbnet_status_stop(dev);
 
+	sierra_net_set_private(dev, NULL);
 	kfree(priv);
 }
 
@@ -909,6 +914,24 @@ static const struct driver_info sierra_net_info_direct_ip = {
 	.tx_fixup = sierra_net_tx_fixup,
 };
 
+static int
+sierra_net_probe(struct usb_interface *udev, const struct usb_device_id *prod)
+{
+	int ret;
+
+	ret = usbnet_probe(udev, prod);
+	if (ret == 0) {
+		struct usbnet *dev = usb_get_intfdata(udev);
+
+		ret = usbnet_status_start(dev, GFP_KERNEL);
+		if (ret == 0) {
+			/* Interrupt URB now set up; initiate sync sequence */
+			sierra_net_dosync(dev);
+		}
+	}
+	return ret;
+}
+
 #define DIRECT_IP_DEVICE(vend, prod) \
 	{USB_DEVICE_INTERFACE_NUMBER(vend, prod, 7), \
 	.driver_info = (unsigned long)&sierra_net_info_direct_ip}, \
@@ -931,7 +954,7 @@ MODULE_DEVICE_TABLE(usb, products);
 static struct usb_driver sierra_net_driver = {
 	.name = "sierra_net",
 	.id_table = products,
-	.probe = usbnet_probe,
+	.probe = sierra_net_probe,
 	.disconnect = usbnet_disconnect,
 	.suspend = usbnet_suspend,
 	.resume = usbnet_resume,
diff --git a/drivers/net/usb/usbnet.c b/drivers/net/usb/usbnet.c
index 51f3192f3931..f95cb032394b 100644
--- a/drivers/net/usb/usbnet.c
+++ b/drivers/net/usb/usbnet.c
@@ -252,6 +252,70 @@ static int init_status (struct usbnet *dev, struct usb_interface *intf)
 	return 0;
 }
 
+/* Submit the interrupt URB if not previously submitted, increasing refcount */
+int usbnet_status_start(struct usbnet *dev, gfp_t mem_flags)
+{
+	int ret = 0;
+
+	WARN_ON_ONCE(dev->interrupt == NULL);
+	if (dev->interrupt) {
+		mutex_lock(&dev->interrupt_mutex);
+
+		if (++dev->interrupt_count == 1)
+			ret = usb_submit_urb(dev->interrupt, mem_flags);
+
+		dev_dbg(&dev->udev->dev, "incremented interrupt URB count to %d\n",
+			dev->interrupt_count);
+		mutex_unlock(&dev->interrupt_mutex);
+	}
+	return ret;
+}
+EXPORT_SYMBOL_GPL(usbnet_status_start);
+
+/* For resume; submit interrupt URB if previously submitted */
+static int __usbnet_status_start_force(struct usbnet *dev, gfp_t mem_flags)
+{
+	int ret = 0;
+
+	mutex_lock(&dev->interrupt_mutex);
+	if (dev->interrupt_count) {
+		ret = usb_submit_urb(dev->interrupt, mem_flags);
+		dev_dbg(&dev->udev->dev,
+			"submitted interrupt URB for resume\n");
+	}
+	mutex_unlock(&dev->interrupt_mutex);
+	return ret;
+}
+
+/* Kill the interrupt URB if all submitters want it killed */
+void usbnet_status_stop(struct usbnet *dev)
+{
+	if (dev->interrupt) {
+		mutex_lock(&dev->interrupt_mutex);
+		WARN_ON(dev->interrupt_count == 0);
+
+		if (dev->interrupt_count && --dev->interrupt_count == 0)
+			usb_kill_urb(dev->interrupt);
+
+		dev_dbg(&dev->udev->dev,
+			"decremented interrupt URB count to %d\n",
+			dev->interrupt_count);
+		mutex_unlock(&dev->interrupt_mutex);
+	}
+}
+EXPORT_SYMBOL_GPL(usbnet_status_stop);
+
+/* For suspend; always kill interrupt URB */
+static void __usbnet_status_stop_force(struct usbnet *dev)
+{
+	if (dev->interrupt) {
+		mutex_lock(&dev->interrupt_mutex);
+		usb_kill_urb(dev->interrupt);
+		dev_dbg(&dev->udev->dev, "killed interrupt URB for suspend\n");
+		mutex_unlock(&dev->interrupt_mutex);
+	}
+}
+
 /* Passes this packet up the stack, updating its accounting.
  * Some link protocols batch packets, so their rx_fixup paths
  * can return clones as well as just modify the original skb.
@@ -725,7 +789,7 @@ int usbnet_stop (struct net_device *net)
 	if (!(info->flags & FLAG_AVOID_UNLINK_URBS))
 		usbnet_terminate_urbs(dev);
 
-	usb_kill_urb(dev->interrupt);
+	usbnet_status_stop(dev);
 
 	usbnet_purge_paused_rxq(dev);
 
@@ -787,7 +851,7 @@ int usbnet_open (struct net_device *net)
 
 	/* start any status interrupt transfer */
 	if (dev->interrupt) {
-		retval = usb_submit_urb (dev->interrupt, GFP_KERNEL);
+		retval = usbnet_status_start(dev, GFP_KERNEL);
 		if (retval < 0) {
 			netif_err(dev, ifup, dev->net,
 				  "intr submit %d\n", retval);
@@ -938,6 +1002,27 @@ static const struct ethtool_ops usbnet_ethtool_ops = {
 
 /*-------------------------------------------------------------------------*/
 
+static void __handle_link_change(struct usbnet *dev)
+{
+	if (!test_bit(EVENT_DEV_OPEN, &dev->flags))
+		return;
+
+	if (!netif_carrier_ok(dev->net)) {
+		/* kill URBs for reading packets to save bus bandwidth */
+		unlink_urbs(dev, &dev->rxq);
+
+		/*
+		 * tx_timeout will unlink URBs for sending packets and
+		 * tx queue is stopped by netcore after link becomes off
+		 */
+	} else {
+		/* submitting URBs for reading packets */
+		tasklet_schedule(&dev->bh);
+	}
+
+	clear_bit(EVENT_LINK_CHANGE, &dev->flags);
+}
+
 /* work that cannot be done in interrupt context uses keventd.
  *
  * NOTE:  with 2.5 we could do more of this using completion callbacks,
@@ -1035,8 +1120,14 @@ skip_reset:
 		} else {
 			usb_autopm_put_interface(dev->intf);
 		}
+
+		/* handle link change from link resetting */
+		__handle_link_change(dev);
 	}
 
+	if (test_bit (EVENT_LINK_CHANGE, &dev->flags))
+		__handle_link_change(dev);
+
 	if (dev->flags)
 		netdev_dbg(dev->net, "kevent done, flags = 0x%lx\n", dev->flags);
 }
@@ -1286,6 +1377,7 @@ static void usbnet_bh (unsigned long param)
 	// or are we maybe short a few urbs?
 	} else if (netif_running (dev->net) &&
 		   netif_device_present (dev->net) &&
+		   netif_carrier_ok(dev->net) &&
 		   !timer_pending (&dev->delay) &&
 		   !test_bit (EVENT_RX_HALT, &dev->flags)) {
 		int	temp = dev->rxq.qlen;
@@ -1430,6 +1522,8 @@ usbnet_probe (struct usb_interface *udev, const struct usb_device_id *prod)
 	dev->delay.data = (unsigned long) dev;
 	init_timer (&dev->delay);
 	mutex_init (&dev->phy_mutex);
+	mutex_init(&dev->interrupt_mutex);
+	dev->interrupt_count = 0;
 
 	dev->net = net;
 	strcpy (net->name, "usb%d");
@@ -1521,7 +1615,7 @@ usbnet_probe (struct usb_interface *udev, const struct usb_device_id *prod)
 	netif_device_attach (net);
 
 	if (dev->driver_info->flags & FLAG_LINK_INTR)
-		netif_carrier_off(net);
+		usbnet_link_change(dev, 0, 0);
 
 	return 0;
 
@@ -1565,7 +1659,7 @@ int usbnet_suspend (struct usb_interface *intf, pm_message_t message)
 		 */
 		netif_device_detach (dev->net);
 		usbnet_terminate_urbs(dev);
-		usb_kill_urb(dev->interrupt);
+		__usbnet_status_stop_force(dev);
 
 		/*
 		 * reattach so runtime management can use and
@@ -1585,9 +1679,8 @@ int usbnet_resume (struct usb_interface *intf)
 	int                     retval;
 
 	if (!--dev->suspend_count) {
-		/* resume interrupt URBs */
-		if (dev->interrupt && test_bit(EVENT_DEV_OPEN, &dev->flags))
-			usb_submit_urb(dev->interrupt, GFP_NOIO);
+		/* resume interrupt URB if it was previously submitted */
+		__usbnet_status_start_force(dev, GFP_NOIO);
 
 		spin_lock_irq(&dev->txq.lock);
 		while ((res = usb_get_from_anchor(&dev->deferred))) {
@@ -1653,6 +1746,21 @@ int usbnet_manage_power(struct usbnet *dev, int on)
 }
 EXPORT_SYMBOL(usbnet_manage_power);
 
+void usbnet_link_change(struct usbnet *dev, bool link, bool need_reset)
+{
+	/* update link after link is reseted */
+	if (link && !need_reset)
+		netif_carrier_on(dev->net);
+	else
+		netif_carrier_off(dev->net);
+
+	if (need_reset && link)
+		usbnet_defer_kevent(dev, EVENT_LINK_RESET);
+	else
+		usbnet_defer_kevent(dev, EVENT_LINK_CHANGE);
+}
+EXPORT_SYMBOL(usbnet_link_change);
+
 /*-------------------------------------------------------------------------*/
 static int __usbnet_read_cmd(struct usbnet *dev, u8 cmd, u8 reqtype,
 			     u16 value, u16 index, void *data, u16 size)
diff --git a/drivers/net/veth.c b/drivers/net/veth.c
index 07a4af0aa3dc..177f911f5946 100644
--- a/drivers/net/veth.c
+++ b/drivers/net/veth.c
@@ -255,7 +255,8 @@ static const struct net_device_ops veth_netdev_ops = {
 
 #define VETH_FEATURES (NETIF_F_SG | NETIF_F_FRAGLIST | NETIF_F_ALL_TSO |    \
 		       NETIF_F_HW_CSUM | NETIF_F_RXCSUM | NETIF_F_HIGHDMA | \
-		       NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX)
+		       NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX | \
+		       NETIF_F_HW_VLAN_STAG_TX | NETIF_F_HW_VLAN_STAG_RX )
 
 static void veth_setup(struct net_device *dev)
 {
diff --git a/drivers/net/virtio_net.c b/drivers/net/virtio_net.c
index 57ac4b0294bc..3c23fdc27bf0 100644
--- a/drivers/net/virtio_net.c
+++ b/drivers/net/virtio_net.c
@@ -39,7 +39,6 @@ module_param(gso, bool, 0444);
 #define MAX_PACKET_LEN (ETH_HLEN + VLAN_HLEN + ETH_DATA_LEN)
 #define GOOD_COPY_LEN	128
 
-#define VIRTNET_SEND_COMMAND_SG_MAX    2
 #define VIRTNET_DRIVER_VERSION "1.0.0"
 
 struct virtnet_stats {
@@ -154,7 +153,7 @@ struct padded_vnet_hdr {
  */
 static int vq2txq(struct virtqueue *vq)
 {
-	return (virtqueue_get_queue_index(vq) - 1) / 2;
+	return (vq->index - 1) / 2;
 }
 
 static int txq2vq(int txq)
@@ -164,7 +163,7 @@ static int txq2vq(int txq)
 
 static int vq2rxq(struct virtqueue *vq)
 {
-	return virtqueue_get_queue_index(vq) / 2;
+	return vq->index / 2;
 }
 
 static int rxq2vq(int rxq)
@@ -444,7 +443,7 @@ static int add_recvbuf_small(struct receive_queue *rq, gfp_t gfp)
 
 	skb_to_sgvec(skb, rq->sg + 1, 0, skb->len);
 
-	err = virtqueue_add_buf(rq->vq, rq->sg, 0, 2, skb, gfp);
+	err = virtqueue_add_inbuf(rq->vq, rq->sg, 2, skb, gfp);
 	if (err < 0)
 		dev_kfree_skb(skb);
 
@@ -489,8 +488,8 @@ static int add_recvbuf_big(struct receive_queue *rq, gfp_t gfp)
 
 	/* chain first in list head */
 	first->private = (unsigned long)list;
-	err = virtqueue_add_buf(rq->vq, rq->sg, 0, MAX_SKB_FRAGS + 2,
-				first, gfp);
+	err = virtqueue_add_inbuf(rq->vq, rq->sg, MAX_SKB_FRAGS + 2,
+				  first, gfp);
 	if (err < 0)
 		give_pages(rq, first);
 
@@ -508,7 +507,7 @@ static int add_recvbuf_mergeable(struct receive_queue *rq, gfp_t gfp)
 
 	sg_init_one(rq->sg, page_address(page), PAGE_SIZE);
 
-	err = virtqueue_add_buf(rq->vq, rq->sg, 0, 1, page, gfp);
+	err = virtqueue_add_inbuf(rq->vq, rq->sg, 1, page, gfp);
 	if (err < 0)
 		give_pages(rq, page);
 
@@ -582,7 +581,7 @@ static void refill_work(struct work_struct *work)
 	bool still_empty;
 	int i;
 
-	for (i = 0; i < vi->max_queue_pairs; i++) {
+	for (i = 0; i < vi->curr_queue_pairs; i++) {
 		struct receive_queue *rq = &vi->rq[i];
 
 		napi_disable(&rq->napi);
@@ -637,7 +636,7 @@ static int virtnet_open(struct net_device *dev)
 	struct virtnet_info *vi = netdev_priv(dev);
 	int i;
 
-	for (i = 0; i < vi->max_queue_pairs; i++) {
+	for (i = 0; i < vi->curr_queue_pairs; i++) {
 		/* Make sure we have some buffers: if oom use wq. */
 		if (!try_fill_recv(&vi->rq[i], GFP_KERNEL))
 			schedule_delayed_work(&vi->refill, 0);
@@ -711,8 +710,7 @@ static int xmit_skb(struct send_queue *sq, struct sk_buff *skb)
 		sg_set_buf(sq->sg, &hdr->hdr, sizeof hdr->hdr);
 
 	num_sg = skb_to_sgvec(skb, sq->sg + 1, 0, skb->len) + 1;
-	return virtqueue_add_buf(sq->vq, sq->sg, num_sg,
-				 0, skb, GFP_ATOMIC);
+	return virtqueue_add_outbuf(sq->vq, sq->sg, num_sg, skb, GFP_ATOMIC);
 }
 
 static netdev_tx_t start_xmit(struct sk_buff *skb, struct net_device *dev)
@@ -767,32 +765,35 @@ static netdev_tx_t start_xmit(struct sk_buff *skb, struct net_device *dev)
  * never fail unless improperly formated.
  */
 static bool virtnet_send_command(struct virtnet_info *vi, u8 class, u8 cmd,
-				 struct scatterlist *data, int out, int in)
+				 struct scatterlist *out,
+				 struct scatterlist *in)
 {
-	struct scatterlist *s, sg[VIRTNET_SEND_COMMAND_SG_MAX + 2];
+	struct scatterlist *sgs[4], hdr, stat;
 	struct virtio_net_ctrl_hdr ctrl;
 	virtio_net_ctrl_ack status = ~0;
-	unsigned int tmp;
-	int i;
+	unsigned out_num = 0, in_num = 0, tmp;
 
 	/* Caller should know better */
-	BUG_ON(!virtio_has_feature(vi->vdev, VIRTIO_NET_F_CTRL_VQ) ||
-		(out + in > VIRTNET_SEND_COMMAND_SG_MAX));
-
-	out++; /* Add header */
-	in++; /* Add return status */
+	BUG_ON(!virtio_has_feature(vi->vdev, VIRTIO_NET_F_CTRL_VQ));
 
 	ctrl.class = class;
 	ctrl.cmd = cmd;
+	/* Add header */
+	sg_init_one(&hdr, &ctrl, sizeof(ctrl));
+	sgs[out_num++] = &hdr;
 
-	sg_init_table(sg, out + in);
+	if (out)
+		sgs[out_num++] = out;
+	if (in)
+		sgs[out_num + in_num++] = in;
 
-	sg_set_buf(&sg[0], &ctrl, sizeof(ctrl));
-	for_each_sg(data, s, out + in - 2, i)
-		sg_set_buf(&sg[i + 1], sg_virt(s), s->length);
-	sg_set_buf(&sg[out + in - 1], &status, sizeof(status));
+	/* Add return status. */
+	sg_init_one(&stat, &status, sizeof(status));
+	sgs[out_num + in_num++] = &stat;
 
-	BUG_ON(virtqueue_add_buf(vi->cvq, sg, out, in, vi, GFP_ATOMIC) < 0);
+	BUG_ON(out_num + in_num > ARRAY_SIZE(sgs));
+	BUG_ON(virtqueue_add_sgs(vi->cvq, sgs, out_num, in_num, vi, GFP_ATOMIC)
+	       < 0);
 
 	virtqueue_kick(vi->cvq);
 
@@ -821,7 +822,7 @@ static int virtnet_set_mac_address(struct net_device *dev, void *p)
 		sg_init_one(&sg, addr->sa_data, dev->addr_len);
 		if (!virtnet_send_command(vi, VIRTIO_NET_CTRL_MAC,
 					  VIRTIO_NET_CTRL_MAC_ADDR_SET,
-					  &sg, 1, 0)) {
+					  &sg, NULL)) {
 			dev_warn(&vdev->dev,
 				 "Failed to set mac address by vq command.\n");
 			return -EINVAL;
@@ -889,8 +890,7 @@ static void virtnet_ack_link_announce(struct virtnet_info *vi)
 {
 	rtnl_lock();
 	if (!virtnet_send_command(vi, VIRTIO_NET_CTRL_ANNOUNCE,
-				  VIRTIO_NET_CTRL_ANNOUNCE_ACK, NULL,
-				  0, 0))
+				  VIRTIO_NET_CTRL_ANNOUNCE_ACK, NULL, NULL))
 		dev_warn(&vi->dev->dev, "Failed to ack link announce.\n");
 	rtnl_unlock();
 }
@@ -900,6 +900,7 @@ static int virtnet_set_queues(struct virtnet_info *vi, u16 queue_pairs)
 	struct scatterlist sg;
 	struct virtio_net_ctrl_mq s;
 	struct net_device *dev = vi->dev;
+	int i;
 
 	if (!vi->has_cvq || !virtio_has_feature(vi->vdev, VIRTIO_NET_F_MQ))
 		return 0;
@@ -908,12 +909,16 @@ static int virtnet_set_queues(struct virtnet_info *vi, u16 queue_pairs)
 	sg_init_one(&sg, &s, sizeof(s));
 
 	if (!virtnet_send_command(vi, VIRTIO_NET_CTRL_MQ,
-				  VIRTIO_NET_CTRL_MQ_VQ_PAIRS_SET, &sg, 1, 0)){
+				  VIRTIO_NET_CTRL_MQ_VQ_PAIRS_SET, &sg, NULL)) {
 		dev_warn(&dev->dev, "Fail to set num of queue pairs to %d\n",
 			 queue_pairs);
 		return -EINVAL;
-	} else
+	} else {
+		for (i = vi->curr_queue_pairs; i < queue_pairs; i++)
+			if (!try_fill_recv(&vi->rq[i], GFP_KERNEL))
+				schedule_delayed_work(&vi->refill, 0);
 		vi->curr_queue_pairs = queue_pairs;
+	}
 
 	return 0;
 }
@@ -955,7 +960,7 @@ static void virtnet_set_rx_mode(struct net_device *dev)
 
 	if (!virtnet_send_command(vi, VIRTIO_NET_CTRL_RX,
 				  VIRTIO_NET_CTRL_RX_PROMISC,
-				  sg, 1, 0))
+				  sg, NULL))
 		dev_warn(&dev->dev, "Failed to %sable promisc mode.\n",
 			 promisc ? "en" : "dis");
 
@@ -963,7 +968,7 @@ static void virtnet_set_rx_mode(struct net_device *dev)
 
 	if (!virtnet_send_command(vi, VIRTIO_NET_CTRL_RX,
 				  VIRTIO_NET_CTRL_RX_ALLMULTI,
-				  sg, 1, 0))
+				  sg, NULL))
 		dev_warn(&dev->dev, "Failed to %sable allmulti mode.\n",
 			 allmulti ? "en" : "dis");
 
@@ -1000,13 +1005,14 @@ static void virtnet_set_rx_mode(struct net_device *dev)
 
 	if (!virtnet_send_command(vi, VIRTIO_NET_CTRL_MAC,
 				  VIRTIO_NET_CTRL_MAC_TABLE_SET,
-				  sg, 2, 0))
+				  sg, NULL))
 		dev_warn(&dev->dev, "Failed to set MAC fitler table.\n");
 
 	kfree(buf);
 }
 
-static int virtnet_vlan_rx_add_vid(struct net_device *dev, u16 vid)
+static int virtnet_vlan_rx_add_vid(struct net_device *dev,
+				   __be16 proto, u16 vid)
 {
 	struct virtnet_info *vi = netdev_priv(dev);
 	struct scatterlist sg;
@@ -1014,12 +1020,13 @@ static int virtnet_vlan_rx_add_vid(struct net_device *dev, u16 vid)
 	sg_init_one(&sg, &vid, sizeof(vid));
 
 	if (!virtnet_send_command(vi, VIRTIO_NET_CTRL_VLAN,
-				  VIRTIO_NET_CTRL_VLAN_ADD, &sg, 1, 0))
+				  VIRTIO_NET_CTRL_VLAN_ADD, &sg, NULL))
 		dev_warn(&dev->dev, "Failed to add VLAN ID %d.\n", vid);
 	return 0;
 }
 
-static int virtnet_vlan_rx_kill_vid(struct net_device *dev, u16 vid)
+static int virtnet_vlan_rx_kill_vid(struct net_device *dev,
+				    __be16 proto, u16 vid)
 {
 	struct virtnet_info *vi = netdev_priv(dev);
 	struct scatterlist sg;
@@ -1027,7 +1034,7 @@ static int virtnet_vlan_rx_kill_vid(struct net_device *dev, u16 vid)
 	sg_init_one(&sg, &vid, sizeof(vid));
 
 	if (!virtnet_send_command(vi, VIRTIO_NET_CTRL_VLAN,
-				  VIRTIO_NET_CTRL_VLAN_DEL, &sg, 1, 0))
+				  VIRTIO_NET_CTRL_VLAN_DEL, &sg, NULL))
 		dev_warn(&dev->dev, "Failed to kill VLAN ID %d.\n", vid);
 	return 0;
 }
@@ -1376,7 +1383,7 @@ static int virtnet_find_vqs(struct virtnet_info *vi)
 	if (vi->has_cvq) {
 		vi->cvq = vqs[total_vqs - 1];
 		if (virtio_has_feature(vi->vdev, VIRTIO_NET_F_CTRL_VLAN))
-			vi->dev->features |= NETIF_F_HW_VLAN_FILTER;
+			vi->dev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
 	}
 
 	for (i = 0; i < vi->max_queue_pairs; i++) {
@@ -1511,6 +1518,8 @@ static int virtnet_probe(struct virtio_device *vdev)
 		/* (!csum && gso) case will be fixed by register_netdev() */
 	}
 
+	dev->vlan_features = dev->features;
+
 	/* Configuration may specify what MAC to use.  Otherwise random. */
 	if (virtio_config_val_len(vdev, VIRTIO_NET_F_MAC,
 				  offsetof(struct virtio_net_config, mac),
@@ -1566,7 +1575,7 @@ static int virtnet_probe(struct virtio_device *vdev)
 	}
 
 	/* Last of all, set up some receive buffers. */
-	for (i = 0; i < vi->max_queue_pairs; i++) {
+	for (i = 0; i < vi->curr_queue_pairs; i++) {
 		try_fill_recv(&vi->rq[i], GFP_KERNEL);
 
 		/* If we didn't even get one input buffer, we're useless. */
@@ -1690,7 +1699,7 @@ static int virtnet_restore(struct virtio_device *vdev)
 
 	netif_device_attach(vi->dev);
 
-	for (i = 0; i < vi->max_queue_pairs; i++)
+	for (i = 0; i < vi->curr_queue_pairs; i++)
 		if (!try_fill_recv(&vi->rq[i], GFP_KERNEL))
 			schedule_delayed_work(&vi->refill, 0);
 
diff --git a/drivers/net/vmxnet3/vmxnet3_drv.c b/drivers/net/vmxnet3/vmxnet3_drv.c
index eae7a03d4f9b..55a62cae2cb4 100644
--- a/drivers/net/vmxnet3/vmxnet3_drv.c
+++ b/drivers/net/vmxnet3/vmxnet3_drv.c
@@ -1293,7 +1293,7 @@ vmxnet3_rq_rx_complete(struct vmxnet3_rx_queue *rq,
 			skb->protocol = eth_type_trans(skb, adapter->netdev);
 
 			if (unlikely(rcd->ts))
-				__vlan_hwaccel_put_tag(skb, rcd->tci);
+				__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), rcd->tci);
 
 			if (adapter->netdev->features & NETIF_F_LRO)
 				netif_receive_skb(skb);
@@ -1931,7 +1931,7 @@ vmxnet3_restore_vlan(struct vmxnet3_adapter *adapter)
 
 
 static int
-vmxnet3_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
+vmxnet3_vlan_rx_add_vid(struct net_device *netdev, __be16 proto, u16 vid)
 {
 	struct vmxnet3_adapter *adapter = netdev_priv(netdev);
 
@@ -1953,7 +1953,7 @@ vmxnet3_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
 
 
 static int
-vmxnet3_vlan_rx_kill_vid(struct net_device *netdev, u16 vid)
+vmxnet3_vlan_rx_kill_vid(struct net_device *netdev, __be16 proto, u16 vid)
 {
 	struct vmxnet3_adapter *adapter = netdev_priv(netdev);
 
@@ -2107,7 +2107,7 @@ vmxnet3_setup_driver_shared(struct vmxnet3_adapter *adapter)
 		devRead->misc.uptFeatures |= UPT1_F_LRO;
 		devRead->misc.maxNumRxSG = cpu_to_le16(1 + MAX_SKB_FRAGS);
 	}
-	if (adapter->netdev->features & NETIF_F_HW_VLAN_RX)
+	if (adapter->netdev->features & NETIF_F_HW_VLAN_CTAG_RX)
 		devRead->misc.uptFeatures |= UPT1_F_RXVLAN;
 
 	devRead->misc.mtu = cpu_to_le32(adapter->netdev->mtu);
@@ -2669,14 +2669,15 @@ vmxnet3_declare_features(struct vmxnet3_adapter *adapter, bool dma64)
 	struct net_device *netdev = adapter->netdev;
 
 	netdev->hw_features = NETIF_F_SG | NETIF_F_RXCSUM |
-		NETIF_F_HW_CSUM | NETIF_F_HW_VLAN_TX |
-		NETIF_F_HW_VLAN_RX | NETIF_F_TSO | NETIF_F_TSO6 |
+		NETIF_F_HW_CSUM | NETIF_F_HW_VLAN_CTAG_TX |
+		NETIF_F_HW_VLAN_CTAG_RX | NETIF_F_TSO | NETIF_F_TSO6 |
 		NETIF_F_LRO;
 	if (dma64)
 		netdev->hw_features |= NETIF_F_HIGHDMA;
 	netdev->vlan_features = netdev->hw_features &
-				~(NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX);
-	netdev->features = netdev->hw_features | NETIF_F_HW_VLAN_FILTER;
+				~(NETIF_F_HW_VLAN_CTAG_TX |
+				  NETIF_F_HW_VLAN_CTAG_RX);
+	netdev->features = netdev->hw_features | NETIF_F_HW_VLAN_CTAG_FILTER;
 }
 
 
diff --git a/drivers/net/vmxnet3/vmxnet3_ethtool.c b/drivers/net/vmxnet3/vmxnet3_ethtool.c
index 63a124340cbe..600ab56c0008 100644
--- a/drivers/net/vmxnet3/vmxnet3_ethtool.c
+++ b/drivers/net/vmxnet3/vmxnet3_ethtool.c
@@ -263,7 +263,8 @@ int vmxnet3_set_features(struct net_device *netdev, netdev_features_t features)
 	unsigned long flags;
 	netdev_features_t changed = features ^ netdev->features;
 
-	if (changed & (NETIF_F_RXCSUM | NETIF_F_LRO | NETIF_F_HW_VLAN_RX)) {
+	if (changed & (NETIF_F_RXCSUM | NETIF_F_LRO |
+		       NETIF_F_HW_VLAN_CTAG_RX)) {
 		if (features & NETIF_F_RXCSUM)
 			adapter->shared->devRead.misc.uptFeatures |=
 			UPT1_F_RXCSUM;
@@ -279,7 +280,7 @@ int vmxnet3_set_features(struct net_device *netdev, netdev_features_t features)
 			adapter->shared->devRead.misc.uptFeatures &=
 							~UPT1_F_LRO;
 
-		if (features & NETIF_F_HW_VLAN_RX)
+		if (features & NETIF_F_HW_VLAN_CTAG_RX)
 			adapter->shared->devRead.misc.uptFeatures |=
 			UPT1_F_RXVLAN;
 		else
diff --git a/drivers/net/vxlan.c b/drivers/net/vxlan.c
index 7cee7a3068ec..ba81f3c39a83 100644
--- a/drivers/net/vxlan.c
+++ b/drivers/net/vxlan.c
@@ -1,14 +1,13 @@
 /*
  * VXLAN: Virtual eXtensible Local Area Network
  *
- * Copyright (c) 2012 Vyatta Inc.
+ * Copyright (c) 2012-2013 Vyatta Inc.
  *
  * 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.
  *
  * TODO
- *  - use IANA UDP port number (when defined)
  *  - IPv6 (not in RFC)
  */
 
@@ -33,7 +32,7 @@
 #include <net/arp.h>
 #include <net/ndisc.h>
 #include <net/ip.h>
-#include <net/ipip.h>
+#include <net/ip_tunnels.h>
 #include <net/icmp.h>
 #include <net/udp.h>
 #include <net/rtnetlink.h>
@@ -65,7 +64,10 @@ struct vxlanhdr {
 	__be32 vx_vni;
 };
 
-/* UDP port for VXLAN traffic. */
+/* UDP port for VXLAN traffic.
+ * The IANA assigned port is 4789, but the Linux default is 8472
+ * for compatability with early adopters.
+ */
 static unsigned int vxlan_port __read_mostly = 8472;
 module_param_named(udp_port, vxlan_port, uint, 0444);
 MODULE_PARM_DESC(udp_port, "Destination UDP port");
@@ -81,35 +83,34 @@ struct vxlan_net {
 	struct hlist_head vni_list[VNI_HASH_SIZE];
 };
 
+struct vxlan_rdst {
+	struct rcu_head		 rcu;
+	__be32			 remote_ip;
+	__be16			 remote_port;
+	u32			 remote_vni;
+	u32			 remote_ifindex;
+	struct vxlan_rdst	*remote_next;
+};
+
 /* Forwarding table entry */
 struct vxlan_fdb {
 	struct hlist_node hlist;	/* linked list of entries */
 	struct rcu_head	  rcu;
 	unsigned long	  updated;	/* jiffies */
 	unsigned long	  used;
-	__be32		  remote_ip;
+	struct vxlan_rdst remote;
 	u16		  state;	/* see ndm_state */
+	u8		  flags;	/* see ndm_flags */
 	u8		  eth_addr[ETH_ALEN];
 };
 
-/* Per-cpu network traffic stats */
-struct vxlan_stats {
-	u64			rx_packets;
-	u64			rx_bytes;
-	u64			tx_packets;
-	u64			tx_bytes;
-	struct u64_stats_sync	syncp;
-};
-
 /* Pseudo network device */
 struct vxlan_dev {
 	struct hlist_node hlist;
 	struct net_device *dev;
-	struct vxlan_stats __percpu *stats;
-	__u32		  vni;		/* virtual network id */
-	__be32	          gaddr;	/* multicast group */
+	struct vxlan_rdst default_dst;	/* default destination */
 	__be32		  saddr;	/* source address */
-	unsigned int      link;		/* link to multicast over */
+	__be16		  dst_port;
 	__u16		  port_min;	/* source port range */
 	__u16		  port_max;
 	__u8		  tos;		/* TOS override */
@@ -147,7 +148,7 @@ static struct vxlan_dev *vxlan_find_vni(struct net *net, u32 id)
 	struct vxlan_dev *vxlan;
 
 	hlist_for_each_entry_rcu(vxlan, vni_head(net, id), hlist) {
-		if (vxlan->vni == id)
+		if (vxlan->default_dst.remote_vni == id)
 			return vxlan;
 	}
 
@@ -157,7 +158,8 @@ static struct vxlan_dev *vxlan_find_vni(struct net *net, u32 id)
 /* Fill in neighbour message in skbuff. */
 static int vxlan_fdb_info(struct sk_buff *skb, struct vxlan_dev *vxlan,
 			   const struct vxlan_fdb *fdb,
-			   u32 portid, u32 seq, int type, unsigned int flags)
+			   u32 portid, u32 seq, int type, unsigned int flags,
+			   const struct vxlan_rdst *rdst)
 {
 	unsigned long now = jiffies;
 	struct nda_cacheinfo ci;
@@ -176,19 +178,29 @@ static int vxlan_fdb_info(struct sk_buff *skb, struct vxlan_dev *vxlan,
 
 	if (type == RTM_GETNEIGH) {
 		ndm->ndm_family	= AF_INET;
-		send_ip = fdb->remote_ip != 0;
+		send_ip = rdst->remote_ip != htonl(INADDR_ANY);
 		send_eth = !is_zero_ether_addr(fdb->eth_addr);
 	} else
 		ndm->ndm_family	= AF_BRIDGE;
 	ndm->ndm_state = fdb->state;
 	ndm->ndm_ifindex = vxlan->dev->ifindex;
-	ndm->ndm_flags = NTF_SELF;
+	ndm->ndm_flags = fdb->flags;
 	ndm->ndm_type = NDA_DST;
 
 	if (send_eth && nla_put(skb, NDA_LLADDR, ETH_ALEN, &fdb->eth_addr))
 		goto nla_put_failure;
 
-	if (send_ip && nla_put_be32(skb, NDA_DST, fdb->remote_ip))
+	if (send_ip && nla_put_be32(skb, NDA_DST, rdst->remote_ip))
+		goto nla_put_failure;
+
+	if (rdst->remote_port && rdst->remote_port != vxlan->dst_port &&
+	    nla_put_be16(skb, NDA_PORT, rdst->remote_port))
+		goto nla_put_failure;
+	if (rdst->remote_vni != vxlan->default_dst.remote_vni &&
+	    nla_put_be32(skb, NDA_VNI, rdst->remote_vni))
+		goto nla_put_failure;
+	if (rdst->remote_ifindex &&
+	    nla_put_u32(skb, NDA_IFINDEX, rdst->remote_ifindex))
 		goto nla_put_failure;
 
 	ci.ndm_used	 = jiffies_to_clock_t(now - fdb->used);
@@ -211,6 +223,9 @@ static inline size_t vxlan_nlmsg_size(void)
 	return NLMSG_ALIGN(sizeof(struct ndmsg))
 		+ nla_total_size(ETH_ALEN) /* NDA_LLADDR */
 		+ nla_total_size(sizeof(__be32)) /* NDA_DST */
+		+ nla_total_size(sizeof(__be16)) /* NDA_PORT */
+		+ nla_total_size(sizeof(__be32)) /* NDA_VNI */
+		+ nla_total_size(sizeof(__u32)) /* NDA_IFINDEX */
 		+ nla_total_size(sizeof(struct nda_cacheinfo));
 }
 
@@ -225,7 +240,7 @@ static void vxlan_fdb_notify(struct vxlan_dev *vxlan,
 	if (skb == NULL)
 		goto errout;
 
-	err = vxlan_fdb_info(skb, vxlan, fdb, 0, 0, type, 0);
+	err = vxlan_fdb_info(skb, vxlan, fdb, 0, 0, type, 0, &fdb->remote);
 	if (err < 0) {
 		/* -EMSGSIZE implies BUG in vxlan_nlmsg_size() */
 		WARN_ON(err == -EMSGSIZE);
@@ -247,7 +262,8 @@ static void vxlan_ip_miss(struct net_device *dev, __be32 ipa)
 
 	memset(&f, 0, sizeof f);
 	f.state = NUD_STALE;
-	f.remote_ip = ipa; /* goes to NDA_DST */
+	f.remote.remote_ip = ipa; /* goes to NDA_DST */
+	f.remote.remote_vni = VXLAN_N_VID;
 
 	vxlan_fdb_notify(vxlan, &f, RTM_GETNEIGH);
 }
@@ -300,10 +316,39 @@ static struct vxlan_fdb *vxlan_find_mac(struct vxlan_dev *vxlan,
 	return NULL;
 }
 
+/* Add/update destinations for multicast */
+static int vxlan_fdb_append(struct vxlan_fdb *f,
+			    __be32 ip, __be16 port, __u32 vni, __u32 ifindex)
+{
+	struct vxlan_rdst *rd_prev, *rd;
+
+	rd_prev = NULL;
+	for (rd = &f->remote; rd; rd = rd->remote_next) {
+		if (rd->remote_ip == ip &&
+		    rd->remote_port == port &&
+		    rd->remote_vni == vni &&
+		    rd->remote_ifindex == ifindex)
+			return 0;
+		rd_prev = rd;
+	}
+	rd = kmalloc(sizeof(*rd), GFP_ATOMIC);
+	if (rd == NULL)
+		return -ENOBUFS;
+	rd->remote_ip = ip;
+	rd->remote_port = port;
+	rd->remote_vni = vni;
+	rd->remote_ifindex = ifindex;
+	rd->remote_next = NULL;
+	rd_prev->remote_next = rd;
+	return 1;
+}
+
 /* Add new entry to forwarding table -- assumes lock held */
 static int vxlan_fdb_create(struct vxlan_dev *vxlan,
 			    const u8 *mac, __be32 ip,
-			    __u16 state, __u16 flags)
+			    __u16 state, __u16 flags,
+			    __be16 port, __u32 vni, __u32 ifindex,
+			    __u8 ndm_flags)
 {
 	struct vxlan_fdb *f;
 	int notify = 0;
@@ -320,6 +365,19 @@ static int vxlan_fdb_create(struct vxlan_dev *vxlan,
 			f->updated = jiffies;
 			notify = 1;
 		}
+		if (f->flags != ndm_flags) {
+			f->flags = ndm_flags;
+			f->updated = jiffies;
+			notify = 1;
+		}
+		if ((flags & NLM_F_APPEND) &&
+		    is_multicast_ether_addr(f->eth_addr)) {
+			int rc = vxlan_fdb_append(f, ip, port, vni, ifindex);
+
+			if (rc < 0)
+				return rc;
+			notify |= rc;
+		}
 	} else {
 		if (!(flags & NLM_F_CREATE))
 			return -ENOENT;
@@ -333,8 +391,13 @@ static int vxlan_fdb_create(struct vxlan_dev *vxlan,
 			return -ENOMEM;
 
 		notify = 1;
-		f->remote_ip = ip;
+		f->remote.remote_ip = ip;
+		f->remote.remote_port = port;
+		f->remote.remote_vni = vni;
+		f->remote.remote_ifindex = ifindex;
+		f->remote.remote_next = NULL;
 		f->state = state;
+		f->flags = ndm_flags;
 		f->updated = f->used = jiffies;
 		memcpy(f->eth_addr, mac, ETH_ALEN);
 
@@ -349,6 +412,19 @@ static int vxlan_fdb_create(struct vxlan_dev *vxlan,
 	return 0;
 }
 
+static void vxlan_fdb_free(struct rcu_head *head)
+{
+	struct vxlan_fdb *f = container_of(head, struct vxlan_fdb, rcu);
+
+	while (f->remote.remote_next) {
+		struct vxlan_rdst *rd = f->remote.remote_next;
+
+		f->remote.remote_next = rd->remote_next;
+		kfree(rd);
+	}
+	kfree(f);
+}
+
 static void vxlan_fdb_destroy(struct vxlan_dev *vxlan, struct vxlan_fdb *f)
 {
 	netdev_dbg(vxlan->dev,
@@ -358,7 +434,7 @@ static void vxlan_fdb_destroy(struct vxlan_dev *vxlan, struct vxlan_fdb *f)
 	vxlan_fdb_notify(vxlan, f, RTM_DELNEIGH);
 
 	hlist_del_rcu(&f->hlist);
-	kfree_rcu(f, rcu);
+	call_rcu(&f->rcu, vxlan_fdb_free);
 }
 
 /* Add static entry (via netlink) */
@@ -367,7 +443,10 @@ static int vxlan_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 			 const unsigned char *addr, u16 flags)
 {
 	struct vxlan_dev *vxlan = netdev_priv(dev);
+	struct net *net = dev_net(vxlan->dev);
 	__be32 ip;
+	__be16 port;
+	u32 vni, ifindex;
 	int err;
 
 	if (!(ndm->ndm_state & (NUD_PERMANENT|NUD_REACHABLE))) {
@@ -384,8 +463,36 @@ static int vxlan_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 
 	ip = nla_get_be32(tb[NDA_DST]);
 
+	if (tb[NDA_PORT]) {
+		if (nla_len(tb[NDA_PORT]) != sizeof(__be16))
+			return -EINVAL;
+		port = nla_get_be16(tb[NDA_PORT]);
+	} else
+		port = vxlan->dst_port;
+
+	if (tb[NDA_VNI]) {
+		if (nla_len(tb[NDA_VNI]) != sizeof(u32))
+			return -EINVAL;
+		vni = nla_get_u32(tb[NDA_VNI]);
+	} else
+		vni = vxlan->default_dst.remote_vni;
+
+	if (tb[NDA_IFINDEX]) {
+		struct net_device *tdev;
+
+		if (nla_len(tb[NDA_IFINDEX]) != sizeof(u32))
+			return -EINVAL;
+		ifindex = nla_get_u32(tb[NDA_IFINDEX]);
+		tdev = dev_get_by_index(net, ifindex);
+		if (!tdev)
+			return -EADDRNOTAVAIL;
+		dev_put(tdev);
+	} else
+		ifindex = 0;
+
 	spin_lock_bh(&vxlan->hash_lock);
-	err = vxlan_fdb_create(vxlan, addr, ip, ndm->ndm_state, flags);
+	err = vxlan_fdb_create(vxlan, addr, ip, ndm->ndm_state, flags,
+			       port, vni, ifindex, ndm->ndm_flags);
 	spin_unlock_bh(&vxlan->hash_lock);
 
 	return err;
@@ -423,18 +530,21 @@ static int vxlan_fdb_dump(struct sk_buff *skb, struct netlink_callback *cb,
 		int err;
 
 		hlist_for_each_entry_rcu(f, &vxlan->fdb_head[h], hlist) {
-			if (idx < cb->args[0])
-				goto skip;
-
-			err = vxlan_fdb_info(skb, vxlan, f,
-					     NETLINK_CB(cb->skb).portid,
-					     cb->nlh->nlmsg_seq,
-					     RTM_NEWNEIGH,
-					     NLM_F_MULTI);
-			if (err < 0)
-				break;
+			struct vxlan_rdst *rd;
+			for (rd = &f->remote; rd; rd = rd->remote_next) {
+				if (idx < cb->args[0])
+					goto skip;
+
+				err = vxlan_fdb_info(skb, vxlan, f,
+						     NETLINK_CB(cb->skb).portid,
+						     cb->nlh->nlmsg_seq,
+						     RTM_NEWNEIGH,
+						     NLM_F_MULTI, rd);
+				if (err < 0)
+					break;
 skip:
-			++idx;
+				++idx;
+			}
 		}
 	}
 
@@ -454,22 +564,25 @@ static void vxlan_snoop(struct net_device *dev,
 	f = vxlan_find_mac(vxlan, src_mac);
 	if (likely(f)) {
 		f->used = jiffies;
-		if (likely(f->remote_ip == src_ip))
+		if (likely(f->remote.remote_ip == src_ip))
 			return;
 
 		if (net_ratelimit())
 			netdev_info(dev,
 				    "%pM migrated from %pI4 to %pI4\n",
-				    src_mac, &f->remote_ip, &src_ip);
+				    src_mac, &f->remote.remote_ip, &src_ip);
 
-		f->remote_ip = src_ip;
+		f->remote.remote_ip = src_ip;
 		f->updated = jiffies;
 	} else {
 		/* learned new entry */
 		spin_lock(&vxlan->hash_lock);
 		err = vxlan_fdb_create(vxlan, src_mac, src_ip,
 				       NUD_REACHABLE,
-				       NLM_F_EXCL|NLM_F_CREATE);
+				       NLM_F_EXCL|NLM_F_CREATE,
+				       vxlan->dst_port,
+				       vxlan->default_dst.remote_vni,
+				       0, NTF_SELF);
 		spin_unlock(&vxlan->hash_lock);
 	}
 }
@@ -490,7 +603,7 @@ static bool vxlan_group_used(struct vxlan_net *vn,
 			if (!netif_running(vxlan->dev))
 				continue;
 
-			if (vxlan->gaddr == this->gaddr)
+			if (vxlan->default_dst.remote_ip == this->default_dst.remote_ip)
 				return true;
 		}
 
@@ -504,8 +617,8 @@ static int vxlan_join_group(struct net_device *dev)
 	struct vxlan_net *vn = net_generic(dev_net(dev), vxlan_net_id);
 	struct sock *sk = vn->sock->sk;
 	struct ip_mreqn mreq = {
-		.imr_multiaddr.s_addr	= vxlan->gaddr,
-		.imr_ifindex		= vxlan->link,
+		.imr_multiaddr.s_addr	= vxlan->default_dst.remote_ip,
+		.imr_ifindex		= vxlan->default_dst.remote_ifindex,
 	};
 	int err;
 
@@ -532,8 +645,8 @@ static int vxlan_leave_group(struct net_device *dev)
 	int err = 0;
 	struct sock *sk = vn->sock->sk;
 	struct ip_mreqn mreq = {
-		.imr_multiaddr.s_addr	= vxlan->gaddr,
-		.imr_ifindex		= vxlan->link,
+		.imr_multiaddr.s_addr	= vxlan->default_dst.remote_ip,
+		.imr_ifindex		= vxlan->default_dst.remote_ifindex,
 	};
 
 	/* Only leave group when last vxlan is done. */
@@ -556,7 +669,7 @@ static int vxlan_udp_encap_recv(struct sock *sk, struct sk_buff *skb)
 	struct iphdr *oip;
 	struct vxlanhdr *vxh;
 	struct vxlan_dev *vxlan;
-	struct vxlan_stats *stats;
+	struct pcpu_tstats *stats;
 	__u32 vni;
 	int err;
 
@@ -632,7 +745,7 @@ static int vxlan_udp_encap_recv(struct sock *sk, struct sk_buff *skb)
 		}
 	}
 
-	stats = this_cpu_ptr(vxlan->stats);
+	stats = this_cpu_ptr(vxlan->dev->tstats);
 	u64_stats_update_begin(&stats->syncp);
 	stats->rx_packets++;
 	stats->rx_bytes += skb->len;
@@ -691,7 +804,6 @@ static int arp_reduce(struct net_device *dev, struct sk_buff *skb)
 	n = neigh_lookup(&arp_tbl, &tip, dev);
 
 	if (n) {
-		struct vxlan_dev *vxlan = netdev_priv(dev);
 		struct vxlan_fdb *f;
 		struct sk_buff	*reply;
 
@@ -701,7 +813,7 @@ static int arp_reduce(struct net_device *dev, struct sk_buff *skb)
 		}
 
 		f = vxlan_find_mac(vxlan, n->ha);
-		if (f && f->remote_ip == 0) {
+		if (f && f->remote.remote_ip == htonl(INADDR_ANY)) {
 			/* bridge-local neighbor */
 			neigh_release(n);
 			goto out;
@@ -763,28 +875,6 @@ static bool route_shortcircuit(struct net_device *dev, struct sk_buff *skb)
 	return false;
 }
 
-/* Extract dsfield from inner protocol */
-static inline u8 vxlan_get_dsfield(const struct iphdr *iph,
-				   const struct sk_buff *skb)
-{
-	if (skb->protocol == htons(ETH_P_IP))
-		return iph->tos;
-	else if (skb->protocol == htons(ETH_P_IPV6))
-		return ipv6_get_dsfield((const struct ipv6hdr *)iph);
-	else
-		return 0;
-}
-
-/* Propogate ECN bits out */
-static inline u8 vxlan_ecn_encap(u8 tos,
-				 const struct iphdr *iph,
-				 const struct sk_buff *skb)
-{
-	u8 inner = vxlan_get_dsfield(iph, skb);
-
-	return INET_ECN_encapsulate(tos, inner);
-}
-
 static void vxlan_sock_free(struct sk_buff *skb)
 {
 	sock_put(skb->sk);
@@ -807,7 +897,7 @@ static void vxlan_set_owner(struct net_device *dev, struct sk_buff *skb)
  *     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)
+static __be16 vxlan_src_port(const struct vxlan_dev *vxlan, struct sk_buff *skb)
 {
 	unsigned int range = (vxlan->port_max - vxlan->port_min) + 1;
 	u32 hash;
@@ -817,71 +907,78 @@ static u16 vxlan_src_port(const struct vxlan_dev *vxlan, struct sk_buff *skb)
 		hash = jhash(skb->data, 2 * ETH_ALEN,
 			     (__force u32) skb->protocol);
 
-	return (((u64) hash * range) >> 32) + vxlan->port_min;
+	return htons((((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
- */
-static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
+static int handle_offloads(struct sk_buff *skb)
+{
+	if (skb_is_gso(skb)) {
+		int err = skb_unclone(skb, GFP_ATOMIC);
+		if (unlikely(err))
+			return err;
+
+		skb_shinfo(skb)->gso_type |= SKB_GSO_UDP_TUNNEL;
+	} else if (skb->ip_summed != CHECKSUM_PARTIAL)
+		skb->ip_summed = CHECKSUM_NONE;
+
+	return 0;
+}
+
+/* Bypass encapsulation if the destination is local */
+static void vxlan_encap_bypass(struct sk_buff *skb, struct vxlan_dev *src_vxlan,
+			       struct vxlan_dev *dst_vxlan)
+{
+	struct pcpu_tstats *tx_stats = this_cpu_ptr(src_vxlan->dev->tstats);
+	struct pcpu_tstats *rx_stats = this_cpu_ptr(dst_vxlan->dev->tstats);
+
+	skb->pkt_type = PACKET_HOST;
+	skb->encapsulation = 0;
+	skb->dev = dst_vxlan->dev;
+	__skb_pull(skb, skb_network_offset(skb));
+
+	if (dst_vxlan->flags & VXLAN_F_LEARN)
+		vxlan_snoop(skb->dev, htonl(INADDR_LOOPBACK),
+			    eth_hdr(skb)->h_source);
+
+	u64_stats_update_begin(&tx_stats->syncp);
+	tx_stats->tx_packets++;
+	tx_stats->tx_bytes += skb->len;
+	u64_stats_update_end(&tx_stats->syncp);
+
+	if (netif_rx(skb) == NET_RX_SUCCESS) {
+		u64_stats_update_begin(&rx_stats->syncp);
+		rx_stats->rx_packets++;
+		rx_stats->rx_bytes += skb->len;
+		u64_stats_update_end(&rx_stats->syncp);
+	} else {
+		skb->dev->stats.rx_dropped++;
+	}
+}
+
+static netdev_tx_t vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
+				  struct vxlan_rdst *rdst, bool did_rsc)
 {
 	struct vxlan_dev *vxlan = netdev_priv(dev);
 	struct rtable *rt;
 	const struct iphdr *old_iph;
-	struct ethhdr *eth;
 	struct iphdr *iph;
 	struct vxlanhdr *vxh;
 	struct udphdr *uh;
 	struct flowi4 fl4;
-	unsigned int pkt_len = skb->len;
 	__be32 dst;
-	__u16 src_port;
+	__be16 src_port, dst_port;
+        u32 vni;
 	__be16 df = 0;
 	__u8 tos, ttl;
-	int err;
-	bool did_rsc = false;
-	const struct vxlan_fdb *f;
-
-	skb_reset_mac_header(skb);
-	eth = eth_hdr(skb);
 
-	if ((vxlan->flags & VXLAN_F_PROXY) && ntohs(eth->h_proto) == ETH_P_ARP)
-		return arp_reduce(dev, skb);
-	else if ((vxlan->flags&VXLAN_F_RSC) && ntohs(eth->h_proto) == ETH_P_IP)
-		did_rsc = route_shortcircuit(dev, skb);
-
-	f = vxlan_find_mac(vxlan, eth->h_dest);
-	if (f == NULL) {
-		did_rsc = false;
-		dst = vxlan->gaddr;
-		if (!dst && (vxlan->flags & VXLAN_F_L2MISS) &&
-		    !is_multicast_ether_addr(eth->h_dest))
-			vxlan_fdb_miss(vxlan, eth->h_dest);
-	} else
-		dst = f->remote_ip;
+	dst_port = rdst->remote_port ? rdst->remote_port : vxlan->dst_port;
+	vni = rdst->remote_vni;
+	dst = rdst->remote_ip;
 
 	if (!dst) {
 		if (did_rsc) {
-			__skb_pull(skb, skb_network_offset(skb));
-			skb->ip_summed = CHECKSUM_NONE;
-			skb->pkt_type = PACKET_HOST;
-
 			/* short-circuited back to local bridge */
-			if (netif_rx(skb) == NET_RX_SUCCESS) {
-				struct vxlan_stats *stats =
-						this_cpu_ptr(vxlan->stats);
-
-				u64_stats_update_begin(&stats->syncp);
-				stats->tx_packets++;
-				stats->tx_bytes += pkt_len;
-				u64_stats_update_end(&stats->syncp);
-			} else {
-				dev->stats.tx_errors++;
-				dev->stats.tx_aborted_errors++;
-			}
+			vxlan_encap_bypass(skb, vxlan, vxlan);
 			return NETDEV_TX_OK;
 		}
 		goto drop;
@@ -904,12 +1001,12 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	tos = vxlan->tos;
 	if (tos == 1)
-		tos = vxlan_get_dsfield(old_iph, skb);
+		tos = ip_tunnel_get_dsfield(old_iph, skb);
 
 	src_port = vxlan_src_port(vxlan, skb);
 
 	memset(&fl4, 0, sizeof(fl4));
-	fl4.flowi4_oif = vxlan->link;
+	fl4.flowi4_oif = rdst->remote_ifindex;
 	fl4.flowi4_tos = RT_TOS(tos);
 	fl4.daddr = dst;
 	fl4.saddr = vxlan->saddr;
@@ -928,6 +1025,19 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 		goto tx_error;
 	}
 
+	/* Bypass encapsulation if the destination is local */
+	if (rt->rt_flags & RTCF_LOCAL &&
+	    !(rt->rt_flags & (RTCF_BROADCAST | RTCF_MULTICAST))) {
+		struct vxlan_dev *dst_vxlan;
+
+		ip_rt_put(rt);
+		dst_vxlan = vxlan_find_vni(dev_net(dev), vni);
+		if (!dst_vxlan)
+			goto tx_error;
+		vxlan_encap_bypass(skb, vxlan, dst_vxlan);
+		return NETDEV_TX_OK;
+	}
+
 	memset(&(IPCB(skb)->opt), 0, sizeof(IPCB(skb)->opt));
 	IPCB(skb)->flags &= ~(IPSKB_XFRM_TUNNEL_SIZE | IPSKB_XFRM_TRANSFORMED |
 			      IPSKB_REROUTED);
@@ -936,14 +1046,14 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	vxh = (struct vxlanhdr *) __skb_push(skb, sizeof(*vxh));
 	vxh->vx_flags = htonl(VXLAN_FLAGS);
-	vxh->vx_vni = htonl(vxlan->vni << 8);
+	vxh->vx_vni = htonl(vni << 8);
 
 	__skb_push(skb, sizeof(*uh));
 	skb_reset_transport_header(skb);
 	uh = udp_hdr(skb);
 
-	uh->dest = htons(vxlan_port);
-	uh->source = htons(src_port);
+	uh->dest = dst_port;
+	uh->source = src_port;
 
 	uh->len = htons(skb->len);
 	uh->check = 0;
@@ -955,7 +1065,7 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 	iph->ihl	= sizeof(struct iphdr) >> 2;
 	iph->frag_off	= df;
 	iph->protocol	= IPPROTO_UDP;
-	iph->tos	= vxlan_ecn_encap(tos, old_iph, skb);
+	iph->tos	= ip_tunnel_ecn_encap(tos, old_iph, skb);
 	iph->daddr	= dst;
 	iph->saddr	= fl4.saddr;
 	iph->ttl	= ttl ? : ip4_dst_hoplimit(&rt->dst);
@@ -965,22 +1075,10 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	vxlan_set_owner(dev, skb);
 
-	/* See iptunnel_xmit() */
-	if (skb->ip_summed != CHECKSUM_PARTIAL)
-		skb->ip_summed = CHECKSUM_NONE;
-
-	err = ip_local_out(skb);
-	if (likely(net_xmit_eval(err) == 0)) {
-		struct vxlan_stats *stats = this_cpu_ptr(vxlan->stats);
+	if (handle_offloads(skb))
+		goto drop;
 
-		u64_stats_update_begin(&stats->syncp);
-		stats->tx_packets++;
-		stats->tx_bytes += pkt_len;
-		u64_stats_update_end(&stats->syncp);
-	} else {
-		dev->stats.tx_errors++;
-		dev->stats.tx_aborted_errors++;
-	}
+	iptunnel_xmit(skb, dev);
 	return NETDEV_TX_OK;
 
 drop:
@@ -994,6 +1092,65 @@ tx_free:
 	return NETDEV_TX_OK;
 }
 
+/* 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
+ */
+static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct vxlan_dev *vxlan = netdev_priv(dev);
+	struct ethhdr *eth;
+	bool did_rsc = false;
+	struct vxlan_rdst *rdst0, *rdst;
+	struct vxlan_fdb *f;
+	int rc1, rc;
+
+	skb_reset_mac_header(skb);
+	eth = eth_hdr(skb);
+
+	if ((vxlan->flags & VXLAN_F_PROXY) && ntohs(eth->h_proto) == ETH_P_ARP)
+		return arp_reduce(dev, skb);
+
+	f = vxlan_find_mac(vxlan, eth->h_dest);
+	did_rsc = false;
+
+	if (f && (f->flags & NTF_ROUTER) && (vxlan->flags & VXLAN_F_RSC) &&
+	    ntohs(eth->h_proto) == ETH_P_IP) {
+		did_rsc = route_shortcircuit(dev, skb);
+		if (did_rsc)
+			f = vxlan_find_mac(vxlan, eth->h_dest);
+	}
+
+	if (f == NULL) {
+		rdst0 = &vxlan->default_dst;
+
+		if (rdst0->remote_ip == htonl(INADDR_ANY) &&
+		    (vxlan->flags & VXLAN_F_L2MISS) &&
+		    !is_multicast_ether_addr(eth->h_dest))
+			vxlan_fdb_miss(vxlan, eth->h_dest);
+	} else
+		rdst0 = &f->remote;
+
+	rc = NETDEV_TX_OK;
+
+	/* if there are multiple destinations, send copies */
+	for (rdst = rdst0->remote_next; rdst; rdst = rdst->remote_next) {
+		struct sk_buff *skb1;
+
+		skb1 = skb_clone(skb, GFP_ATOMIC);
+		rc1 = vxlan_xmit_one(skb1, dev, rdst, did_rsc);
+		if (rc == NETDEV_TX_OK)
+			rc = rc1;
+	}
+
+	rc1 = vxlan_xmit_one(skb, dev, rdst0, did_rsc);
+	if (rc == NETDEV_TX_OK)
+		rc = rc1;
+	return rc;
+}
+
 /* Walk the forwarding table and purge stale entries */
 static void vxlan_cleanup(unsigned long arg)
 {
@@ -1034,10 +1191,8 @@ static void vxlan_cleanup(unsigned long arg)
 /* Setup stats when device is created */
 static int vxlan_init(struct net_device *dev)
 {
-	struct vxlan_dev *vxlan = netdev_priv(dev);
-
-	vxlan->stats = alloc_percpu(struct vxlan_stats);
-	if (!vxlan->stats)
+	dev->tstats = alloc_percpu(struct pcpu_tstats);
+	if (!dev->tstats)
 		return -ENOMEM;
 
 	return 0;
@@ -1049,7 +1204,7 @@ static int vxlan_open(struct net_device *dev)
 	struct vxlan_dev *vxlan = netdev_priv(dev);
 	int err;
 
-	if (vxlan->gaddr) {
+	if (IN_MULTICAST(ntohl(vxlan->default_dst.remote_ip))) {
 		err = vxlan_join_group(dev);
 		if (err)
 			return err;
@@ -1083,7 +1238,7 @@ static int vxlan_stop(struct net_device *dev)
 {
 	struct vxlan_dev *vxlan = netdev_priv(dev);
 
-	if (vxlan->gaddr)
+	if (IN_MULTICAST(ntohl(vxlan->default_dst.remote_ip)))
 		vxlan_leave_group(dev);
 
 	del_timer_sync(&vxlan->age_timer);
@@ -1093,49 +1248,6 @@ static int vxlan_stop(struct net_device *dev)
 	return 0;
 }
 
-/* Merge per-cpu statistics */
-static struct rtnl_link_stats64 *vxlan_stats64(struct net_device *dev,
-					       struct rtnl_link_stats64 *stats)
-{
-	struct vxlan_dev *vxlan = netdev_priv(dev);
-	struct vxlan_stats tmp, sum = { 0 };
-	unsigned int cpu;
-
-	for_each_possible_cpu(cpu) {
-		unsigned int start;
-		const struct vxlan_stats *stats
-			= per_cpu_ptr(vxlan->stats, cpu);
-
-		do {
-			start = u64_stats_fetch_begin_bh(&stats->syncp);
-			memcpy(&tmp, stats, sizeof(tmp));
-		} while (u64_stats_fetch_retry_bh(&stats->syncp, start));
-
-		sum.tx_bytes   += tmp.tx_bytes;
-		sum.tx_packets += tmp.tx_packets;
-		sum.rx_bytes   += tmp.rx_bytes;
-		sum.rx_packets += tmp.rx_packets;
-	}
-
-	stats->tx_bytes   = sum.tx_bytes;
-	stats->tx_packets = sum.tx_packets;
-	stats->rx_bytes   = sum.rx_bytes;
-	stats->rx_packets = sum.rx_packets;
-
-	stats->multicast = dev->stats.multicast;
-	stats->rx_length_errors = dev->stats.rx_length_errors;
-	stats->rx_frame_errors = dev->stats.rx_frame_errors;
-	stats->rx_errors = dev->stats.rx_errors;
-
-	stats->tx_dropped = dev->stats.tx_dropped;
-	stats->tx_carrier_errors  = dev->stats.tx_carrier_errors;
-	stats->tx_aborted_errors  = dev->stats.tx_aborted_errors;
-	stats->collisions  = dev->stats.collisions;
-	stats->tx_errors = dev->stats.tx_errors;
-
-	return stats;
-}
-
 /* Stub, nothing needs to be done. */
 static void vxlan_set_multicast_list(struct net_device *dev)
 {
@@ -1146,7 +1258,7 @@ static const struct net_device_ops vxlan_netdev_ops = {
 	.ndo_open		= vxlan_open,
 	.ndo_stop		= vxlan_stop,
 	.ndo_start_xmit		= vxlan_xmit,
-	.ndo_get_stats64	= vxlan_stats64,
+	.ndo_get_stats64	= ip_tunnel_get_stats64,
 	.ndo_set_rx_mode	= vxlan_set_multicast_list,
 	.ndo_change_mtu		= eth_change_mtu,
 	.ndo_validate_addr	= eth_validate_addr,
@@ -1163,9 +1275,7 @@ static struct device_type vxlan_type = {
 
 static void vxlan_free(struct net_device *dev)
 {
-	struct vxlan_dev *vxlan = netdev_priv(dev);
-
-	free_percpu(vxlan->stats);
+	free_percpu(dev->tstats);
 	free_netdev(dev);
 }
 
@@ -1189,8 +1299,10 @@ static void vxlan_setup(struct net_device *dev)
 	dev->features	|= NETIF_F_NETNS_LOCAL;
 	dev->features	|= NETIF_F_SG | NETIF_F_HW_CSUM;
 	dev->features   |= NETIF_F_RXCSUM;
+	dev->features   |= NETIF_F_GSO_SOFTWARE;
 
 	dev->hw_features |= NETIF_F_SG | NETIF_F_HW_CSUM | NETIF_F_RXCSUM;
+	dev->hw_features |= NETIF_F_GSO_SOFTWARE;
 	dev->priv_flags	&= ~IFF_XMIT_DST_RELEASE;
 	dev->priv_flags |= IFF_LIVE_ADDR_CHANGE;
 
@@ -1203,6 +1315,7 @@ static void vxlan_setup(struct net_device *dev)
 	inet_get_local_port_range(&low, &high);
 	vxlan->port_min = low;
 	vxlan->port_max = high;
+	vxlan->dst_port = htons(vxlan_port);
 
 	vxlan->dev = dev;
 
@@ -1225,6 +1338,7 @@ static const struct nla_policy vxlan_policy[IFLA_VXLAN_MAX + 1] = {
 	[IFLA_VXLAN_RSC]	= { .type = NLA_U8 },
 	[IFLA_VXLAN_L2MISS]	= { .type = NLA_U8 },
 	[IFLA_VXLAN_L3MISS]	= { .type = NLA_U8 },
+	[IFLA_VXLAN_PORT]	= { .type = NLA_U16 },
 };
 
 static int vxlan_validate(struct nlattr *tb[], struct nlattr *data[])
@@ -1250,14 +1364,6 @@ static int vxlan_validate(struct nlattr *tb[], struct nlattr *data[])
 			return -ERANGE;
 	}
 
-	if (data[IFLA_VXLAN_GROUP]) {
-		__be32 gaddr = nla_get_be32(data[IFLA_VXLAN_GROUP]);
-		if (!IN_MULTICAST(ntohl(gaddr))) {
-			pr_debug("group address is not IPv4 multicast\n");
-			return -EADDRNOTAVAIL;
-		}
-	}
-
 	if (data[IFLA_VXLAN_PORT_RANGE]) {
 		const struct ifla_vxlan_port_range *p
 			= nla_data(data[IFLA_VXLAN_PORT_RANGE]);
@@ -1288,6 +1394,7 @@ static int vxlan_newlink(struct net *net, struct net_device *dev,
 			 struct nlattr *tb[], struct nlattr *data[])
 {
 	struct vxlan_dev *vxlan = netdev_priv(dev);
+	struct vxlan_rdst *dst = &vxlan->default_dst;
 	__u32 vni;
 	int err;
 
@@ -1299,21 +1406,21 @@ static int vxlan_newlink(struct net *net, struct net_device *dev,
 		pr_info("duplicate VNI %u\n", vni);
 		return -EEXIST;
 	}
-	vxlan->vni = vni;
+	dst->remote_vni = vni;
 
 	if (data[IFLA_VXLAN_GROUP])
-		vxlan->gaddr = nla_get_be32(data[IFLA_VXLAN_GROUP]);
+		dst->remote_ip = nla_get_be32(data[IFLA_VXLAN_GROUP]);
 
 	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]))) {
+	    (dst->remote_ifindex = nla_get_u32(data[IFLA_VXLAN_LINK]))) {
 		struct net_device *lowerdev
-			 = __dev_get_by_index(net, vxlan->link);
+			 = __dev_get_by_index(net, dst->remote_ifindex);
 
 		if (!lowerdev) {
-			pr_info("ifindex %d does not exist\n", vxlan->link);
+			pr_info("ifindex %d does not exist\n", dst->remote_ifindex);
 			return -ENODEV;
 		}
 
@@ -1361,11 +1468,14 @@ static int vxlan_newlink(struct net *net, struct net_device *dev,
 		vxlan->port_max = ntohs(p->high);
 	}
 
+	if (data[IFLA_VXLAN_PORT])
+		vxlan->dst_port = nla_get_be16(data[IFLA_VXLAN_PORT]);
+
 	SET_ETHTOOL_OPS(dev, &vxlan_ethtool_ops);
 
 	err = register_netdevice(dev);
 	if (!err)
-		hlist_add_head_rcu(&vxlan->hlist, vni_head(net, vxlan->vni));
+		hlist_add_head_rcu(&vxlan->hlist, vni_head(net, dst->remote_vni));
 
 	return err;
 }
@@ -1396,24 +1506,26 @@ static size_t vxlan_get_size(const struct net_device *dev)
 		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)) +
+		nla_total_size(sizeof(__be16))+ /* IFLA_VXLAN_PORT */
 		0;
 }
 
 static int vxlan_fill_info(struct sk_buff *skb, const struct net_device *dev)
 {
 	const struct vxlan_dev *vxlan = netdev_priv(dev);
+	const struct vxlan_rdst *dst = &vxlan->default_dst;
 	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))
+	if (nla_put_u32(skb, IFLA_VXLAN_ID, dst->remote_vni))
 		goto nla_put_failure;
 
-	if (vxlan->gaddr && nla_put_be32(skb, IFLA_VXLAN_GROUP, vxlan->gaddr))
+	if (dst->remote_ip && nla_put_be32(skb, IFLA_VXLAN_GROUP, dst->remote_ip))
 		goto nla_put_failure;
 
-	if (vxlan->link && nla_put_u32(skb, IFLA_VXLAN_LINK, vxlan->link))
+	if (dst->remote_ifindex && nla_put_u32(skb, IFLA_VXLAN_LINK, dst->remote_ifindex))
 		goto nla_put_failure;
 
 	if (vxlan->saddr && nla_put_be32(skb, IFLA_VXLAN_LOCAL, vxlan->saddr))
@@ -1431,7 +1543,8 @@ static int vxlan_fill_info(struct sk_buff *skb, const struct net_device *dev)
 	    nla_put_u8(skb, IFLA_VXLAN_L3MISS,
 			!!(vxlan->flags & VXLAN_F_L3MISS)) ||
 	    nla_put_u32(skb, IFLA_VXLAN_AGEING, vxlan->age_interval) ||
-	    nla_put_u32(skb, IFLA_VXLAN_LIMIT, vxlan->addrmax))
+	    nla_put_u32(skb, IFLA_VXLAN_LIMIT, vxlan->addrmax) ||
+	    nla_put_be16(skb, IFLA_VXLAN_PORT, vxlan->dst_port))
 		goto nla_put_failure;
 
 	if (nla_put(skb, IFLA_VXLAN_PORT_RANGE, sizeof(ports), &ports))
@@ -1555,10 +1668,11 @@ static void __exit vxlan_cleanup_module(void)
 {
 	rtnl_link_unregister(&vxlan_link_ops);
 	unregister_pernet_device(&vxlan_net_ops);
+	rcu_barrier();
 }
 module_exit(vxlan_cleanup_module);
 
 MODULE_LICENSE("GPL");
 MODULE_VERSION(VXLAN_VERSION);
-MODULE_AUTHOR("Stephen Hemminger <shemminger@vyatta.com>");
+MODULE_AUTHOR("Stephen Hemminger <stephen@networkplumber.org>");
 MODULE_ALIAS_RTNL_LINK("vxlan");
diff --git a/drivers/net/wireless/adm8211.c b/drivers/net/wireless/adm8211.c
index 3d339e04efb7..f9a24e599dee 100644
--- a/drivers/net/wireless/adm8211.c
+++ b/drivers/net/wireless/adm8211.c
@@ -1293,7 +1293,8 @@ static int adm8211_config(struct ieee80211_hw *dev, u32 changed)
 {
 	struct adm8211_priv *priv = dev->priv;
 	struct ieee80211_conf *conf = &dev->conf;
-	int channel = ieee80211_frequency_to_channel(conf->channel->center_freq);
+	int channel =
+		ieee80211_frequency_to_channel(conf->chandef.chan->center_freq);
 
 	if (channel != priv->channel) {
 		priv->channel = channel;
diff --git a/drivers/net/wireless/airo.c b/drivers/net/wireless/airo.c
index 53295418f576..6125adb520a3 100644
--- a/drivers/net/wireless/airo.c
+++ b/drivers/net/wireless/airo.c
@@ -4506,108 +4506,75 @@ static int setup_proc_entry( struct net_device *dev,
 	apriv->proc_entry = proc_mkdir_mode(apriv->proc_name, airo_perm,
 					    airo_entry);
 	if (!apriv->proc_entry)
-		goto fail;
-	apriv->proc_entry->uid = proc_kuid;
-	apriv->proc_entry->gid = proc_kgid;
+		return -ENOMEM;
+	proc_set_user(apriv->proc_entry, proc_kuid, proc_kgid);
 
 	/* Setup the StatsDelta */
 	entry = proc_create_data("StatsDelta", S_IRUGO & proc_perm,
 				 apriv->proc_entry, &proc_statsdelta_ops, dev);
 	if (!entry)
-		goto fail_stats_delta;
-	entry->uid = proc_kuid;
-	entry->gid = proc_kgid;
+		goto fail;
+	proc_set_user(entry, proc_kuid, proc_kgid);
 
 	/* Setup the Stats */
 	entry = proc_create_data("Stats", S_IRUGO & proc_perm,
 				 apriv->proc_entry, &proc_stats_ops, dev);
 	if (!entry)
-		goto fail_stats;
-	entry->uid = proc_kuid;
-	entry->gid = proc_kgid;
+		goto fail;
+	proc_set_user(entry, proc_kuid, proc_kgid);
 
 	/* Setup the Status */
 	entry = proc_create_data("Status", S_IRUGO & proc_perm,
 				 apriv->proc_entry, &proc_status_ops, dev);
 	if (!entry)
-		goto fail_status;
-	entry->uid = proc_kuid;
-	entry->gid = proc_kgid;
+		goto fail;
+	proc_set_user(entry, proc_kuid, proc_kgid);
 
 	/* Setup the Config */
 	entry = proc_create_data("Config", proc_perm,
 				 apriv->proc_entry, &proc_config_ops, dev);
 	if (!entry)
-		goto fail_config;
-	entry->uid = proc_kuid;
-	entry->gid = proc_kgid;
+		goto fail;
+	proc_set_user(entry, proc_kuid, proc_kgid);
 
 	/* Setup the SSID */
 	entry = proc_create_data("SSID", proc_perm,
 				 apriv->proc_entry, &proc_SSID_ops, dev);
 	if (!entry)
-		goto fail_ssid;
-	entry->uid = proc_kuid;
-	entry->gid = proc_kgid;
+		goto fail;
+	proc_set_user(entry, proc_kuid, proc_kgid);
 
 	/* Setup the APList */
 	entry = proc_create_data("APList", proc_perm,
 				 apriv->proc_entry, &proc_APList_ops, dev);
 	if (!entry)
-		goto fail_aplist;
-	entry->uid = proc_kuid;
-	entry->gid = proc_kgid;
+		goto fail;
+	proc_set_user(entry, proc_kuid, proc_kgid);
 
 	/* Setup the BSSList */
 	entry = proc_create_data("BSSList", proc_perm,
 				 apriv->proc_entry, &proc_BSSList_ops, dev);
 	if (!entry)
-		goto fail_bsslist;
-	entry->uid = proc_kuid;
-	entry->gid = proc_kgid;
+		goto fail;
+	proc_set_user(entry, proc_kuid, proc_kgid);
 
 	/* Setup the WepKey */
 	entry = proc_create_data("WepKey", proc_perm,
 				 apriv->proc_entry, &proc_wepkey_ops, dev);
 	if (!entry)
-		goto fail_wepkey;
-	entry->uid = proc_kuid;
-	entry->gid = proc_kgid;
-
+		goto fail;
+	proc_set_user(entry, proc_kuid, proc_kgid);
 	return 0;
 
-fail_wepkey:
-	remove_proc_entry("BSSList", apriv->proc_entry);
-fail_bsslist:
-	remove_proc_entry("APList", apriv->proc_entry);
-fail_aplist:
-	remove_proc_entry("SSID", apriv->proc_entry);
-fail_ssid:
-	remove_proc_entry("Config", apriv->proc_entry);
-fail_config:
-	remove_proc_entry("Status", apriv->proc_entry);
-fail_status:
-	remove_proc_entry("Stats", apriv->proc_entry);
-fail_stats:
-	remove_proc_entry("StatsDelta", apriv->proc_entry);
-fail_stats_delta:
-	remove_proc_entry(apriv->proc_name, airo_entry);
 fail:
+	remove_proc_subtree(apriv->proc_name, airo_entry);
 	return -ENOMEM;
 }
 
 static int takedown_proc_entry( struct net_device *dev,
-				struct airo_info *apriv ) {
-	if ( !apriv->proc_entry->namelen ) return 0;
-	remove_proc_entry("Stats",apriv->proc_entry);
-	remove_proc_entry("StatsDelta",apriv->proc_entry);
-	remove_proc_entry("Status",apriv->proc_entry);
-	remove_proc_entry("Config",apriv->proc_entry);
-	remove_proc_entry("SSID",apriv->proc_entry);
-	remove_proc_entry("APList",apriv->proc_entry);
-	remove_proc_entry("BSSList",apriv->proc_entry);
-	remove_proc_entry("WepKey",apriv->proc_entry);
-	remove_proc_entry(apriv->proc_name,airo_entry);
+				struct airo_info *apriv )
+{
+	remove_proc_subtree(apriv->proc_name, airo_entry);
 	return 0;
 }
 
@@ -4663,8 +4630,7 @@ static ssize_t proc_write( struct file *file,
 static int proc_status_open(struct inode *inode, struct file *file)
 {
 	struct proc_data *data;
-	struct proc_dir_entry *dp = PDE(inode);
-	struct net_device *dev = dp->data;
+	struct net_device *dev = PDE_DATA(inode);
 	struct airo_info *apriv = dev->ml_priv;
 	CapabilityRid cap_rid;
 	StatusRid status_rid;
@@ -4746,8 +4712,7 @@ static int proc_stats_rid_open( struct inode *inode,
 				u16 rid )
 {
 	struct proc_data *data;
-	struct proc_dir_entry *dp = PDE(inode);
-	struct net_device *dev = dp->data;
+	struct net_device *dev = PDE_DATA(inode);
 	struct airo_info *apriv = dev->ml_priv;
 	StatsRid stats;
 	int i, j;
@@ -4809,8 +4774,7 @@ static inline int sniffing_mode(struct airo_info *ai)
 static void proc_config_on_close(struct inode *inode, struct file *file)
 {
 	struct proc_data *data = file->private_data;
-	struct proc_dir_entry *dp = PDE(inode);
-	struct net_device *dev = dp->data;
+	struct net_device *dev = PDE_DATA(inode);
 	struct airo_info *ai = dev->ml_priv;
 	char *line;
 
@@ -5021,8 +4985,7 @@ static const char *get_rmode(__le16 mode)
 static int proc_config_open(struct inode *inode, struct file *file)
 {
 	struct proc_data *data;
-	struct proc_dir_entry *dp = PDE(inode);
-	struct net_device *dev = dp->data;
+	struct net_device *dev = PDE_DATA(inode);
 	struct airo_info *ai = dev->ml_priv;
 	int i;
 	__le16 mode;
@@ -5112,8 +5075,7 @@ static int proc_config_open(struct inode *inode, struct file *file)
 static void proc_SSID_on_close(struct inode *inode, struct file *file)
 {
 	struct proc_data *data = file->private_data;
-	struct proc_dir_entry *dp = PDE(inode);
-	struct net_device *dev = dp->data;
+	struct net_device *dev = PDE_DATA(inode);
 	struct airo_info *ai = dev->ml_priv;
 	SsidRid SSID_rid;
 	int i;
@@ -5148,8 +5110,7 @@ static void proc_SSID_on_close(struct inode *inode, struct file *file)
 
 static void proc_APList_on_close( struct inode *inode, struct file *file ) {
 	struct proc_data *data = file->private_data;
-	struct proc_dir_entry *dp = PDE(inode);
-	struct net_device *dev = dp->data;
+	struct net_device *dev = PDE_DATA(inode);
 	struct airo_info *ai = dev->ml_priv;
 	APListRid APList_rid;
 	int i;
@@ -5283,8 +5244,7 @@ static int set_wep_tx_idx(struct airo_info *ai, u16 index, int perm, int lock)
 
 static void proc_wepkey_on_close( struct inode *inode, struct file *file ) {
 	struct proc_data *data;
-	struct proc_dir_entry *dp = PDE(inode);
-	struct net_device *dev = dp->data;
+	struct net_device *dev = PDE_DATA(inode);
 	struct airo_info *ai = dev->ml_priv;
 	int i, rc;
 	char key[16];
@@ -5335,8 +5295,7 @@ static void proc_wepkey_on_close( struct inode *inode, struct file *file ) {
 static int proc_wepkey_open( struct inode *inode, struct file *file )
 {
 	struct proc_data *data;
-	struct proc_dir_entry *dp = PDE(inode);
-	struct net_device *dev = dp->data;
+	struct net_device *dev = PDE_DATA(inode);
 	struct airo_info *ai = dev->ml_priv;
 	char *ptr;
 	WepKeyRid wkr;
@@ -5384,8 +5343,7 @@ static int proc_wepkey_open( struct inode *inode, struct file *file )
 static int proc_SSID_open(struct inode *inode, struct file *file)
 {
 	struct proc_data *data;
-	struct proc_dir_entry *dp = PDE(inode);
-	struct net_device *dev = dp->data;
+	struct net_device *dev = PDE_DATA(inode);
 	struct airo_info *ai = dev->ml_priv;
 	int i;
 	char *ptr;
@@ -5428,8 +5386,7 @@ static int proc_SSID_open(struct inode *inode, struct file *file)
 
 static int proc_APList_open( struct inode *inode, struct file *file ) {
 	struct proc_data *data;
-	struct proc_dir_entry *dp = PDE(inode);
-	struct net_device *dev = dp->data;
+	struct net_device *dev = PDE_DATA(inode);
 	struct airo_info *ai = dev->ml_priv;
 	int i;
 	char *ptr;
@@ -5468,8 +5425,7 @@ static int proc_APList_open( struct inode *inode, struct file *file ) {
 
 static int proc_BSSList_open( struct inode *inode, struct file *file ) {
 	struct proc_data *data;
-	struct proc_dir_entry *dp = PDE(inode);
-	struct net_device *dev = dp->data;
+	struct net_device *dev = PDE_DATA(inode);
 	struct airo_info *ai = dev->ml_priv;
 	char *ptr;
 	BSSListRid BSSList_rid;
@@ -5706,10 +5662,8 @@ static int __init airo_init_module( void )
 
 	airo_entry = proc_mkdir_mode("driver/aironet", airo_perm, NULL);
 
-	if (airo_entry) {
-		airo_entry->uid = proc_kuid;
-		airo_entry->gid = proc_kgid;
-	}
+	if (airo_entry)
+		proc_set_user(airo_entry, proc_kuid, proc_kgid);
 
 	for (i = 0; i < 4 && io[i] && irq[i]; i++) {
 		airo_print_info("", "Trying to configure ISA adapter at irq=%d "
diff --git a/drivers/net/wireless/at76c50x-usb.c b/drivers/net/wireless/at76c50x-usb.c
index 5ac5f7ae2721..34c8a33cac06 100644
--- a/drivers/net/wireless/at76c50x-usb.c
+++ b/drivers/net/wireless/at76c50x-usb.c
@@ -1943,12 +1943,12 @@ static int at76_config(struct ieee80211_hw *hw, u32 changed)
 	struct at76_priv *priv = hw->priv;
 
 	at76_dbg(DBG_MAC80211, "%s(): channel %d",
-		 __func__, hw->conf.channel->hw_value);
+		 __func__, hw->conf.chandef.chan->hw_value);
 	at76_dbg_dump(DBG_MAC80211, priv->bssid, ETH_ALEN, "bssid:");
 
 	mutex_lock(&priv->mtx);
 
-	priv->channel = hw->conf.channel->hw_value;
+	priv->channel = hw->conf.chandef.chan->hw_value;
 
 	if (is_valid_ether_addr(priv->bssid))
 		at76_join(priv);
diff --git a/drivers/net/wireless/ath/ar5523/ar5523.c b/drivers/net/wireless/ath/ar5523/ar5523.c
index 7157f7d311c5..17d7fece35d2 100644
--- a/drivers/net/wireless/ath/ar5523/ar5523.c
+++ b/drivers/net/wireless/ath/ar5523/ar5523.c
@@ -457,14 +457,14 @@ static int ar5523_set_chan(struct ar5523 *ar)
 	memset(&reset, 0, sizeof(reset));
 	reset.flags |= cpu_to_be32(UATH_CHAN_2GHZ);
 	reset.flags |= cpu_to_be32(UATH_CHAN_OFDM);
-	reset.freq = cpu_to_be32(conf->channel->center_freq);
+	reset.freq = cpu_to_be32(conf->chandef.chan->center_freq);
 	reset.maxrdpower = cpu_to_be32(50);	/* XXX */
 	reset.channelchange = cpu_to_be32(1);
 	reset.keeprccontent = cpu_to_be32(0);
 
 	ar5523_dbg(ar, "set chan flags 0x%x freq %d\n",
 		   be32_to_cpu(reset.flags),
-		   conf->channel->center_freq);
+		   conf->chandef.chan->center_freq);
 	return ar5523_cmd_write(ar, WDCMSG_RESET, &reset, sizeof(reset), 0);
 }
 
@@ -594,7 +594,7 @@ static void ar5523_data_rx_cb(struct urb *urb)
 	rx_status = IEEE80211_SKB_RXCB(data->skb);
 	memset(rx_status, 0, sizeof(*rx_status));
 	rx_status->freq = be32_to_cpu(desc->channel);
-	rx_status->band = hw->conf.channel->band;
+	rx_status->band = hw->conf.chandef.chan->band;
 	rx_status->signal = -95 + be32_to_cpu(desc->rssi);
 
 	ieee80211_rx_irqsafe(hw, data->skb);
@@ -1091,7 +1091,7 @@ static int ar5523_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
 	return ret;
 }
 
-static void ar5523_flush(struct ieee80211_hw *hw, bool drop)
+static void ar5523_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 {
 	struct ar5523 *ar = hw->priv;
 
@@ -1153,13 +1153,13 @@ static int ar5523_get_wlan_mode(struct ar5523 *ar,
 	struct ieee80211_sta *sta;
 	u32 sta_rate_set;
 
-	band = ar->hw->wiphy->bands[ar->hw->conf.channel->band];
+	band = ar->hw->wiphy->bands[ar->hw->conf.chandef.chan->band];
 	sta = ieee80211_find_sta(ar->vif, bss_conf->bssid);
 	if (!sta) {
 		ar5523_info(ar, "STA not found!\n");
 		return WLAN_MODE_11b;
 	}
-	sta_rate_set = sta->supp_rates[ar->hw->conf.channel->band];
+	sta_rate_set = sta->supp_rates[ar->hw->conf.chandef.chan->band];
 
 	for (bit = 0; bit < band->n_bitrates; bit++) {
 		if (sta_rate_set & 1) {
@@ -1197,11 +1197,11 @@ static void ar5523_create_rateset(struct ar5523 *ar,
 		ar5523_info(ar, "STA not found. Cannot set rates\n");
 		sta_rate_set = bss_conf->basic_rates;
 	} else
-		sta_rate_set = sta->supp_rates[ar->hw->conf.channel->band];
+		sta_rate_set = sta->supp_rates[ar->hw->conf.chandef.chan->band];
 
 	ar5523_dbg(ar, "sta rate_set = %08x\n", sta_rate_set);
 
-	band = ar->hw->wiphy->bands[ar->hw->conf.channel->band];
+	band = ar->hw->wiphy->bands[ar->hw->conf.chandef.chan->band];
 	for (bit = 0; bit < band->n_bitrates; bit++) {
 		BUG_ON(i >= AR5523_MAX_NRATES);
 		ar5523_dbg(ar, "Considering rate %d : %d\n",
diff --git a/drivers/net/wireless/ath/ath5k/Makefile b/drivers/net/wireless/ath/ath5k/Makefile
index f60b3899afc4..1b3a34f7f224 100644
--- a/drivers/net/wireless/ath/ath5k/Makefile
+++ b/drivers/net/wireless/ath/ath5k/Makefile
@@ -10,6 +10,7 @@ ath5k-y				+= phy.o
 ath5k-y				+= reset.o
 ath5k-y				+= attach.o
 ath5k-y				+= base.o
+CFLAGS_base.o			+= -I$(src)
 ath5k-y				+= led.o
 ath5k-y				+= rfkill.o
 ath5k-y				+= ani.o
diff --git a/drivers/net/wireless/ath/ath5k/ath5k.h b/drivers/net/wireless/ath/ath5k/ath5k.h
index 3150def17193..2d691b8b95b9 100644
--- a/drivers/net/wireless/ath/ath5k/ath5k.h
+++ b/drivers/net/wireless/ath/ath5k/ath5k.h
@@ -1523,7 +1523,8 @@ int ath5k_hw_dma_stop(struct ath5k_hw *ah);
 /* EEPROM access functions */
 int ath5k_eeprom_init(struct ath5k_hw *ah);
 void ath5k_eeprom_detach(struct ath5k_hw *ah);
-
+int ath5k_eeprom_mode_from_channel(struct ath5k_hw *ah,
+		struct ieee80211_channel *channel);
 
 /* Protocol Control Unit Functions */
 /* Helpers */
diff --git a/drivers/net/wireless/ath/ath5k/base.c b/drivers/net/wireless/ath/ath5k/base.c
index 1d264c0f5a9b..9b20d9ee2719 100644
--- a/drivers/net/wireless/ath/ath5k/base.c
+++ b/drivers/net/wireless/ath/ath5k/base.c
@@ -2639,7 +2639,7 @@ int ath5k_start(struct ieee80211_hw *hw)
 	 * be followed by initialization of the appropriate bits
 	 * and then setup of the interrupt mask.
 	 */
-	ah->curchan = ah->hw->conf.channel;
+	ah->curchan = ah->hw->conf.chandef.chan;
 	ah->imask = AR5K_INT_RXOK
 		| AR5K_INT_RXERR
 		| AR5K_INT_RXEOL
diff --git a/drivers/net/wireless/ath/ath5k/eeprom.c b/drivers/net/wireless/ath/ath5k/eeprom.c
index b7e0258887e7..94d34ee02265 100644
--- a/drivers/net/wireless/ath/ath5k/eeprom.c
+++ b/drivers/net/wireless/ath/ath5k/eeprom.c
@@ -1779,7 +1779,8 @@ ath5k_eeprom_detach(struct ath5k_hw *ah)
 }
 
 int
-ath5k_eeprom_mode_from_channel(struct ieee80211_channel *channel)
+ath5k_eeprom_mode_from_channel(struct ath5k_hw *ah,
+		struct ieee80211_channel *channel)
 {
 	switch (channel->hw_value) {
 	case AR5K_MODE_11A:
@@ -1789,6 +1790,7 @@ ath5k_eeprom_mode_from_channel(struct ieee80211_channel *channel)
 	case AR5K_MODE_11B:
 		return AR5K_EEPROM_MODE_11B;
 	default:
-		return -1;
+		ATH5K_WARN(ah, "channel is not A/B/G!");
+		return AR5K_EEPROM_MODE_11A;
 	}
 }
diff --git a/drivers/net/wireless/ath/ath5k/eeprom.h b/drivers/net/wireless/ath/ath5k/eeprom.h
index 94a9bbea6874..693296ee9693 100644
--- a/drivers/net/wireless/ath/ath5k/eeprom.h
+++ b/drivers/net/wireless/ath/ath5k/eeprom.h
@@ -493,6 +493,3 @@ struct ath5k_eeprom_info {
 	/* Antenna raw switch tables */
 	u32	ee_antenna[AR5K_EEPROM_N_MODES][AR5K_ANT_MAX];
 };
-
-int
-ath5k_eeprom_mode_from_channel(struct ieee80211_channel *channel);
diff --git a/drivers/net/wireless/ath/ath5k/mac80211-ops.c b/drivers/net/wireless/ath/ath5k/mac80211-ops.c
index 4264341533ea..06f86f435711 100644
--- a/drivers/net/wireless/ath/ath5k/mac80211-ops.c
+++ b/drivers/net/wireless/ath/ath5k/mac80211-ops.c
@@ -202,7 +202,7 @@ ath5k_config(struct ieee80211_hw *hw, u32 changed)
 	mutex_lock(&ah->lock);
 
 	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
-		ret = ath5k_chan_set(ah, conf->channel);
+		ret = ath5k_chan_set(ah, conf->chandef.chan);
 		if (ret < 0)
 			goto unlock;
 	}
@@ -678,7 +678,7 @@ ath5k_get_survey(struct ieee80211_hw *hw, int idx, struct survey_info *survey)
 
 	memcpy(survey, &ah->survey, sizeof(*survey));
 
-	survey->channel = conf->channel;
+	survey->channel = conf->chandef.chan;
 	survey->noise = ah->ah_noise_floor;
 	survey->filled = SURVEY_INFO_NOISE_DBM |
 			SURVEY_INFO_CHANNEL_TIME |
diff --git a/drivers/net/wireless/ath/ath5k/phy.c b/drivers/net/wireless/ath/ath5k/phy.c
index a78afa98c650..d6bc7cb61bfb 100644
--- a/drivers/net/wireless/ath/ath5k/phy.c
+++ b/drivers/net/wireless/ath/ath5k/phy.c
@@ -1612,11 +1612,7 @@ ath5k_hw_update_noise_floor(struct ath5k_hw *ah)
 
 	ah->ah_cal_mask |= AR5K_CALIBRATION_NF;
 
-	ee_mode = ath5k_eeprom_mode_from_channel(ah->ah_current_channel);
-	if (WARN_ON(ee_mode < 0)) {
-		ah->ah_cal_mask &= ~AR5K_CALIBRATION_NF;
-		return;
-	}
+	ee_mode = ath5k_eeprom_mode_from_channel(ah, ah->ah_current_channel);
 
 	/* completed NF calibration, test threshold */
 	nf = ath5k_hw_read_measured_noise_floor(ah);
@@ -2317,12 +2313,7 @@ ath5k_hw_set_antenna_mode(struct ath5k_hw *ah, u8 ant_mode)
 
 	def_ant = ah->ah_def_ant;
 
-	ee_mode = ath5k_eeprom_mode_from_channel(channel);
-	if (ee_mode < 0) {
-		ATH5K_ERR(ah,
-			"invalid channel: %d\n", channel->center_freq);
-		return;
-	}
+	ee_mode = ath5k_eeprom_mode_from_channel(ah, channel);
 
 	switch (ant_mode) {
 	case AR5K_ANTMODE_DEFAULT:
@@ -3622,12 +3613,7 @@ ath5k_hw_txpower(struct ath5k_hw *ah, struct ieee80211_channel *channel,
 		return -EINVAL;
 	}
 
-	ee_mode = ath5k_eeprom_mode_from_channel(channel);
-	if (ee_mode < 0) {
-		ATH5K_ERR(ah,
-			"invalid channel: %d\n", channel->center_freq);
-		return -EINVAL;
-	}
+	ee_mode = ath5k_eeprom_mode_from_channel(ah, channel);
 
 	/* Initialize TX power table */
 	switch (ah->ah_radio) {
diff --git a/drivers/net/wireless/ath/ath5k/reset.c b/drivers/net/wireless/ath/ath5k/reset.c
index e2d8b2cf19eb..a3399c4f13a9 100644
--- a/drivers/net/wireless/ath/ath5k/reset.c
+++ b/drivers/net/wireless/ath/ath5k/reset.c
@@ -984,9 +984,7 @@ ath5k_hw_commit_eeprom_settings(struct ath5k_hw *ah,
 	if (ah->ah_version == AR5K_AR5210)
 		return;
 
-	ee_mode = ath5k_eeprom_mode_from_channel(channel);
-	if (WARN_ON(ee_mode < 0))
-		return;
+	ee_mode = ath5k_eeprom_mode_from_channel(ah, channel);
 
 	/* Adjust power delta for channel 14 */
 	if (channel->center_freq == 2484)
diff --git a/drivers/net/wireless/ath/ath5k/trace.h b/drivers/net/wireless/ath/ath5k/trace.h
index 00f015819344..c6eef519bb61 100644
--- a/drivers/net/wireless/ath/ath5k/trace.h
+++ b/drivers/net/wireless/ath/ath5k/trace.h
@@ -97,7 +97,7 @@ TRACE_EVENT(ath5k_tx_complete,
 #if defined(CONFIG_ATH5K_TRACER) && !defined(__CHECKER__)
 
 #undef TRACE_INCLUDE_PATH
-#define TRACE_INCLUDE_PATH ../../drivers/net/wireless/ath/ath5k
+#define TRACE_INCLUDE_PATH .
 #undef TRACE_INCLUDE_FILE
 #define TRACE_INCLUDE_FILE trace
 
diff --git a/drivers/net/wireless/ath/ath6kl/Kconfig b/drivers/net/wireless/ath/ath6kl/Kconfig
index 630c83db056e..e39e5860a2e9 100644
--- a/drivers/net/wireless/ath/ath6kl/Kconfig
+++ b/drivers/net/wireless/ath/ath6kl/Kconfig
@@ -30,6 +30,15 @@ config ATH6KL_DEBUG
 	---help---
 	  Enables debug support
 
+config ATH6KL_TRACING
+	bool "Atheros ath6kl tracing support"
+	depends on ATH6KL
+	depends on EVENT_TRACING
+	---help---
+	  Select this to ath6kl use tracing infrastructure.
+
+	  If unsure, say Y to make it easier to debug problems.
+
 config ATH6KL_REGDOMAIN
 	bool "Atheros ath6kl regdomain support"
 	depends on ATH6KL
diff --git a/drivers/net/wireless/ath/ath6kl/Makefile b/drivers/net/wireless/ath/ath6kl/Makefile
index cab0ec0d5380..dc2b3b46781e 100644
--- a/drivers/net/wireless/ath/ath6kl/Makefile
+++ b/drivers/net/wireless/ath/ath6kl/Makefile
@@ -35,10 +35,15 @@ ath6kl_core-y += txrx.o
 ath6kl_core-y += wmi.o
 ath6kl_core-y += core.o
 ath6kl_core-y += recovery.o
+
 ath6kl_core-$(CONFIG_NL80211_TESTMODE) += testmode.o
+ath6kl_core-$(CONFIG_ATH6KL_TRACING) += trace.o
 
 obj-$(CONFIG_ATH6KL_SDIO) += ath6kl_sdio.o
 ath6kl_sdio-y += sdio.o
 
 obj-$(CONFIG_ATH6KL_USB) += ath6kl_usb.o
 ath6kl_usb-y += usb.o
+
+# for tracing framework to find trace.h
+CFLAGS_trace.o := -I$(src)
diff --git a/drivers/net/wireless/ath/ath6kl/cfg80211.c b/drivers/net/wireless/ath/ath6kl/cfg80211.c
index 752ffc4f4166..5c9736a94e54 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.c
+++ b/drivers/net/wireless/ath/ath6kl/cfg80211.c
@@ -402,7 +402,7 @@ static bool ath6kl_is_valid_iftype(struct ath6kl *ar, enum nl80211_iftype type,
 	if (type == NL80211_IFTYPE_STATION ||
 	    type == NL80211_IFTYPE_AP || type == NL80211_IFTYPE_ADHOC) {
 		for (i = 0; i < ar->vif_max; i++) {
-			if ((ar->avail_idx_map >> i) & BIT(0)) {
+			if ((ar->avail_idx_map) & BIT(i)) {
 				*if_idx = i;
 				return true;
 			}
@@ -412,7 +412,7 @@ static bool ath6kl_is_valid_iftype(struct ath6kl *ar, enum nl80211_iftype type,
 	if (type == NL80211_IFTYPE_P2P_CLIENT ||
 	    type == NL80211_IFTYPE_P2P_GO) {
 		for (i = ar->max_norm_iface; i < ar->vif_max; i++) {
-			if ((ar->avail_idx_map >> i) & BIT(0)) {
+			if ((ar->avail_idx_map) & BIT(i)) {
 				*if_idx = i;
 				return true;
 			}
@@ -1535,7 +1535,9 @@ static int ath6kl_cfg80211_del_iface(struct wiphy *wiphy,
 
 	ath6kl_cfg80211_vif_stop(vif, test_bit(WMI_READY, &ar->flag));
 
+	rtnl_lock();
 	ath6kl_cfg80211_vif_cleanup(vif);
+	rtnl_unlock();
 
 	return 0;
 }
@@ -2990,13 +2992,15 @@ static int ath6kl_change_station(struct wiphy *wiphy, struct net_device *dev,
 {
 	struct ath6kl *ar = ath6kl_priv(dev);
 	struct ath6kl_vif *vif = netdev_priv(dev);
+	int err;
 
 	if (vif->nw_type != AP_NETWORK)
 		return -EOPNOTSUPP;
 
-	/* Use this only for authorizing/unauthorizing a station */
-	if (!(params->sta_flags_mask & BIT(NL80211_STA_FLAG_AUTHORIZED)))
-		return -EOPNOTSUPP;
+	err = cfg80211_check_station_change(wiphy, params,
+					    CFG80211_STA_AP_MLME_CLIENT);
+	if (err)
+		return err;
 
 	if (params->sta_flags_set & BIT(NL80211_STA_FLAG_AUTHORIZED))
 		return ath6kl_wmi_ap_set_mlme(ar->wmi, vif->fw_vif_idx,
@@ -3659,7 +3663,6 @@ struct wireless_dev *ath6kl_interface_add(struct ath6kl *ar, const char *name,
 	vif->sme_state = SME_DISCONNECTED;
 	set_bit(WLAN_ENABLED, &vif->flags);
 	ar->wlan_pwr_state = WLAN_POWER_STATE_ON;
-	set_bit(NETDEV_REGISTERED, &vif->flags);
 
 	if (type == NL80211_IFTYPE_ADHOC)
 		ar->ibss_if_active = true;
diff --git a/drivers/net/wireless/ath/ath6kl/core.h b/drivers/net/wireless/ath/ath6kl/core.h
index 61b2f98b4e77..26b0f92424e1 100644
--- a/drivers/net/wireless/ath/ath6kl/core.h
+++ b/drivers/net/wireless/ath/ath6kl/core.h
@@ -560,7 +560,6 @@ enum ath6kl_vif_state {
 	WMM_ENABLED,
 	NETQ_STOPPED,
 	DTIM_EXPIRED,
-	NETDEV_REGISTERED,
 	CLEAR_BSSFILTER_ON_BEACON,
 	DTIM_PERIOD_AVAIL,
 	WLAN_ENABLED,
@@ -936,8 +935,6 @@ void aggr_recv_addba_req_evt(struct ath6kl_vif *vif, u8 tid, u16 seq_no,
 			     u8 win_sz);
 void ath6kl_wakeup_event(void *dev);
 
-void ath6kl_reset_device(struct ath6kl *ar, u32 target_type,
-			 bool wait_fot_compltn, bool cold_reset);
 void ath6kl_init_control_info(struct ath6kl_vif *vif);
 struct ath6kl_vif *ath6kl_vif_first(struct ath6kl *ar);
 void ath6kl_cfg80211_vif_stop(struct ath6kl_vif *vif, bool wmi_ready);
diff --git a/drivers/net/wireless/ath/ath6kl/debug.c b/drivers/net/wireless/ath/ath6kl/debug.c
index 15cfe30e54fd..fe38b836cb26 100644
--- a/drivers/net/wireless/ath/ath6kl/debug.c
+++ b/drivers/net/wireless/ath/ath6kl/debug.c
@@ -56,6 +56,60 @@ int ath6kl_printk(const char *level, const char *fmt, ...)
 }
 EXPORT_SYMBOL(ath6kl_printk);
 
+int ath6kl_info(const char *fmt, ...)
+{
+	struct va_format vaf = {
+		.fmt = fmt,
+	};
+	va_list args;
+	int ret;
+
+	va_start(args, fmt);
+	vaf.va = &args;
+	ret = ath6kl_printk(KERN_INFO, "%pV", &vaf);
+	trace_ath6kl_log_info(&vaf);
+	va_end(args);
+
+	return ret;
+}
+EXPORT_SYMBOL(ath6kl_info);
+
+int ath6kl_err(const char *fmt, ...)
+{
+	struct va_format vaf = {
+		.fmt = fmt,
+	};
+	va_list args;
+	int ret;
+
+	va_start(args, fmt);
+	vaf.va = &args;
+	ret = ath6kl_printk(KERN_ERR, "%pV", &vaf);
+	trace_ath6kl_log_err(&vaf);
+	va_end(args);
+
+	return ret;
+}
+EXPORT_SYMBOL(ath6kl_err);
+
+int ath6kl_warn(const char *fmt, ...)
+{
+	struct va_format vaf = {
+		.fmt = fmt,
+	};
+	va_list args;
+	int ret;
+
+	va_start(args, fmt);
+	vaf.va = &args;
+	ret = ath6kl_printk(KERN_WARNING, "%pV", &vaf);
+	trace_ath6kl_log_warn(&vaf);
+	va_end(args);
+
+	return ret;
+}
+EXPORT_SYMBOL(ath6kl_warn);
+
 #ifdef CONFIG_ATH6KL_DEBUG
 
 void ath6kl_dbg(enum ATH6K_DEBUG_MASK mask, const char *fmt, ...)
@@ -63,15 +117,15 @@ void ath6kl_dbg(enum ATH6K_DEBUG_MASK mask, const char *fmt, ...)
 	struct va_format vaf;
 	va_list args;
 
-	if (!(debug_mask & mask))
-		return;
-
 	va_start(args, fmt);
 
 	vaf.fmt = fmt;
 	vaf.va = &args;
 
-	ath6kl_printk(KERN_DEBUG, "%pV", &vaf);
+	if (debug_mask & mask)
+		ath6kl_printk(KERN_DEBUG, "%pV", &vaf);
+
+	trace_ath6kl_log_dbg(mask, &vaf);
 
 	va_end(args);
 }
@@ -87,6 +141,10 @@ void ath6kl_dbg_dump(enum ATH6K_DEBUG_MASK mask,
 
 		print_hex_dump_bytes(prefix, DUMP_PREFIX_OFFSET, buf, len);
 	}
+
+	/* tracing code doesn't like null strings :/ */
+	trace_ath6kl_log_dbg_dump(msg ? msg : "", prefix ? prefix : "",
+				  buf, len);
 }
 EXPORT_SYMBOL(ath6kl_dbg_dump);
 
@@ -1752,8 +1810,10 @@ int ath6kl_debug_init_fs(struct ath6kl *ar)
 	debugfs_create_file("tgt_stats", S_IRUSR, ar->debugfs_phy, ar,
 			    &fops_tgt_stats);
 
-	debugfs_create_file("credit_dist_stats", S_IRUSR, ar->debugfs_phy, ar,
-			    &fops_credit_dist_stats);
+	if (ar->hif_type == ATH6KL_HIF_TYPE_SDIO)
+		debugfs_create_file("credit_dist_stats", S_IRUSR,
+				    ar->debugfs_phy, ar,
+				    &fops_credit_dist_stats);
 
 	debugfs_create_file("endpoint_stats", S_IRUSR | S_IWUSR,
 			    ar->debugfs_phy, ar, &fops_endpoint_stats);
diff --git a/drivers/net/wireless/ath/ath6kl/debug.h b/drivers/net/wireless/ath/ath6kl/debug.h
index f97cd4ead543..74369de00fb5 100644
--- a/drivers/net/wireless/ath/ath6kl/debug.h
+++ b/drivers/net/wireless/ath/ath6kl/debug.h
@@ -19,6 +19,7 @@
 #define DEBUG_H
 
 #include "hif.h"
+#include "trace.h"
 
 enum ATH6K_DEBUG_MASK {
 	ATH6KL_DBG_CREDIT	= BIT(0),
@@ -51,13 +52,9 @@ enum ATH6K_DEBUG_MASK {
 extern unsigned int debug_mask;
 extern __printf(2, 3)
 int ath6kl_printk(const char *level, const char *fmt, ...);
-
-#define ath6kl_info(fmt, ...)				\
-	ath6kl_printk(KERN_INFO, fmt, ##__VA_ARGS__)
-#define ath6kl_err(fmt, ...)					\
-	ath6kl_printk(KERN_ERR, fmt, ##__VA_ARGS__)
-#define ath6kl_warn(fmt, ...)					\
-	ath6kl_printk(KERN_WARNING, fmt, ##__VA_ARGS__)
+extern __printf(1, 2) int ath6kl_info(const char *fmt, ...);
+extern __printf(1, 2) int ath6kl_err(const char *fmt, ...);
+extern __printf(1, 2) int ath6kl_warn(const char *fmt, ...);
 
 enum ath6kl_war {
 	ATH6KL_WAR_INVALID_RATE,
diff --git a/drivers/net/wireless/ath/ath6kl/hif.c b/drivers/net/wireless/ath/ath6kl/hif.c
index a6b614421fa4..fea7709b5dda 100644
--- a/drivers/net/wireless/ath/ath6kl/hif.c
+++ b/drivers/net/wireless/ath/ath6kl/hif.c
@@ -22,6 +22,7 @@
 #include "target.h"
 #include "hif-ops.h"
 #include "debug.h"
+#include "trace.h"
 
 #define MAILBOX_FOR_BLOCK_SIZE          1
 
@@ -436,6 +437,8 @@ static int proc_pending_irqs(struct ath6kl_device *dev, bool *done)
 
 		ath6kl_dump_registers(dev, &dev->irq_proc_reg,
 				      &dev->irq_en_reg);
+		trace_ath6kl_sdio_irq(&dev->irq_en_reg,
+				      sizeof(dev->irq_en_reg));
 
 		/* Update only those registers that are enabled */
 		host_int_status = dev->irq_proc_reg.host_int_status &
diff --git a/drivers/net/wireless/ath/ath6kl/htc_mbox.c b/drivers/net/wireless/ath/ath6kl/htc_mbox.c
index fbb78dfe078f..65e5b719093d 100644
--- a/drivers/net/wireless/ath/ath6kl/htc_mbox.c
+++ b/drivers/net/wireless/ath/ath6kl/htc_mbox.c
@@ -19,6 +19,8 @@
 #include "hif.h"
 #include "debug.h"
 #include "hif-ops.h"
+#include "trace.h"
+
 #include <asm/unaligned.h>
 
 #define CALC_TXRX_PADDED_LEN(dev, len)  (__ALIGN_MASK((len), (dev)->block_mask))
@@ -537,6 +539,8 @@ static int ath6kl_htc_tx_issue(struct htc_target *target,
 				packet->buf, padded_len,
 				HIF_WR_ASYNC_BLOCK_INC, packet);
 
+	trace_ath6kl_htc_tx(status, packet->endpoint, packet->buf, send_len);
+
 	return status;
 }
 
@@ -757,7 +761,8 @@ static void ath6kl_htc_tx_bundle(struct htc_endpoint *endpoint,
 {
 	struct htc_target *target = endpoint->target;
 	struct hif_scatter_req *scat_req = NULL;
-	int n_scat, n_sent_bundle = 0, tot_pkts_bundle = 0;
+	int n_scat, n_sent_bundle = 0, tot_pkts_bundle = 0, i;
+	struct htc_packet *packet;
 	int status;
 	u32 txb_mask;
 	u8 ac = WMM_NUM_AC;
@@ -832,6 +837,13 @@ static void ath6kl_htc_tx_bundle(struct htc_endpoint *endpoint,
 		ath6kl_dbg(ATH6KL_DBG_HTC,
 			   "htc tx scatter bytes %d entries %d\n",
 			   scat_req->len, scat_req->scat_entries);
+
+		for (i = 0; i < scat_req->scat_entries; i++) {
+			packet = scat_req->scat_list[i].packet;
+			trace_ath6kl_htc_tx(packet->status, packet->endpoint,
+					    packet->buf, packet->act_len);
+		}
+
 		ath6kl_hif_submit_scat_req(target->dev, scat_req, false);
 
 		if (status)
@@ -1903,6 +1915,7 @@ static void ath6kl_htc_rx_complete(struct htc_endpoint *endpoint,
 		ath6kl_dbg(ATH6KL_DBG_HTC,
 			   "htc rx complete ep %d packet 0x%p\n",
 			   endpoint->eid, packet);
+
 		endpoint->ep_cb.rx(endpoint->target, packet);
 }
 
@@ -2011,6 +2024,9 @@ static int ath6kl_htc_rx_process_packets(struct htc_target *target,
 	list_for_each_entry_safe(packet, tmp_pkt, comp_pktq, list) {
 		ep = &target->endpoint[packet->endpoint];
 
+		trace_ath6kl_htc_rx(packet->status, packet->endpoint,
+				    packet->buf, packet->act_len);
+
 		/* process header for each of the recv packet */
 		status = ath6kl_htc_rx_process_hdr(target, packet, lk_ahds,
 						   n_lk_ahd);
@@ -2291,6 +2307,9 @@ static struct htc_packet *htc_wait_for_ctrl_msg(struct htc_target *target)
 	if (ath6kl_htc_rx_packet(target, packet, packet->act_len))
 		goto fail_ctrl_rx;
 
+	trace_ath6kl_htc_rx(packet->status, packet->endpoint,
+			    packet->buf, packet->act_len);
+
 	/* process receive header */
 	packet->status = ath6kl_htc_rx_process_hdr(target, packet, NULL, NULL);
 
diff --git a/drivers/net/wireless/ath/ath6kl/htc_pipe.c b/drivers/net/wireless/ath/ath6kl/htc_pipe.c
index 281390178e3d..67aa924ed8b3 100644
--- a/drivers/net/wireless/ath/ath6kl/htc_pipe.c
+++ b/drivers/net/wireless/ath/ath6kl/htc_pipe.c
@@ -988,8 +988,6 @@ static int ath6kl_htc_pipe_rx_complete(struct ath6kl *ar, struct sk_buff *skb,
 
 	htc_hdr = (struct htc_frame_hdr *) netdata;
 
-	ep = &target->endpoint[htc_hdr->eid];
-
 	if (htc_hdr->eid >= ENDPOINT_MAX) {
 		ath6kl_dbg(ATH6KL_DBG_HTC,
 			   "HTC Rx: invalid EndpointID=%d\n",
@@ -997,6 +995,7 @@ static int ath6kl_htc_pipe_rx_complete(struct ath6kl *ar, struct sk_buff *skb,
 		status = -EINVAL;
 		goto free_skb;
 	}
+	ep = &target->endpoint[htc_hdr->eid];
 
 	payload_len = le16_to_cpu(get_unaligned(&htc_hdr->payld_len));
 
@@ -1168,8 +1167,8 @@ static int htc_wait_recv_ctrl_message(struct htc_target *target)
 	}
 
 	if (count <= 0) {
-		ath6kl_dbg(ATH6KL_DBG_HTC, "%s: Timeout!\n", __func__);
-		return -ECOMM;
+		ath6kl_warn("htc pipe control receive timeout!\n");
+		return -ETIMEDOUT;
 	}
 
 	return 0;
@@ -1582,16 +1581,16 @@ static int ath6kl_htc_pipe_wait_target(struct htc_target *target)
 		return status;
 
 	if (target->pipe.ctrl_response_len < sizeof(*ready_msg)) {
-		ath6kl_dbg(ATH6KL_DBG_HTC, "invalid htc ready msg len:%d!\n",
-			   target->pipe.ctrl_response_len);
+		ath6kl_warn("invalid htc pipe ready msg len: %d\n",
+			    target->pipe.ctrl_response_len);
 		return -ECOMM;
 	}
 
 	ready_msg = (struct htc_ready_ext_msg *) target->pipe.ctrl_response_buf;
 
 	if (ready_msg->ver2_0_info.msg_id != cpu_to_le16(HTC_MSG_READY_ID)) {
-		ath6kl_dbg(ATH6KL_DBG_HTC, "invalid htc ready msg : 0x%X !\n",
-			   ready_msg->ver2_0_info.msg_id);
+		ath6kl_warn("invalid htc pipe ready msg: 0x%x\n",
+			    ready_msg->ver2_0_info.msg_id);
 		return -ECOMM;
 	}
 
diff --git a/drivers/net/wireless/ath/ath6kl/init.c b/drivers/net/wireless/ath/ath6kl/init.c
index 5d434cf88f35..40ffee6184fd 100644
--- a/drivers/net/wireless/ath/ath6kl/init.c
+++ b/drivers/net/wireless/ath/ath6kl/init.c
@@ -201,8 +201,8 @@ struct sk_buff *ath6kl_buf_alloc(int size)
 	u16 reserved;
 
 	/* Add chacheline space at front and back of buffer */
-	reserved = (2 * L1_CACHE_BYTES) + ATH6KL_DATA_OFFSET +
-		   sizeof(struct htc_packet) + ATH6KL_HTC_ALIGN_BYTES;
+	reserved = roundup((2 * L1_CACHE_BYTES) + ATH6KL_DATA_OFFSET +
+		   sizeof(struct htc_packet) + ATH6KL_HTC_ALIGN_BYTES, 4);
 	skb = dev_alloc_skb(size + reserved);
 
 	if (skb)
@@ -1549,10 +1549,89 @@ static const char *ath6kl_init_get_hif_name(enum ath6kl_hif_type type)
 	return NULL;
 }
 
+
+static const struct fw_capa_str_map {
+	int id;
+	const char *name;
+} fw_capa_map[] = {
+	{ ATH6KL_FW_CAPABILITY_HOST_P2P, "host-p2p" },
+	{ ATH6KL_FW_CAPABILITY_SCHED_SCAN, "sched-scan" },
+	{ ATH6KL_FW_CAPABILITY_STA_P2PDEV_DUPLEX, "sta-p2pdev-duplex" },
+	{ ATH6KL_FW_CAPABILITY_INACTIVITY_TIMEOUT, "inactivity-timeout" },
+	{ ATH6KL_FW_CAPABILITY_RSN_CAP_OVERRIDE, "rsn-cap-override" },
+	{ ATH6KL_FW_CAPABILITY_WOW_MULTICAST_FILTER, "wow-mc-filter" },
+	{ ATH6KL_FW_CAPABILITY_BMISS_ENHANCE, "bmiss-enhance" },
+	{ ATH6KL_FW_CAPABILITY_SCHED_SCAN_MATCH_LIST, "sscan-match-list" },
+	{ ATH6KL_FW_CAPABILITY_RSSI_SCAN_THOLD, "rssi-scan-thold" },
+	{ ATH6KL_FW_CAPABILITY_CUSTOM_MAC_ADDR, "custom-mac-addr" },
+	{ ATH6KL_FW_CAPABILITY_TX_ERR_NOTIFY, "tx-err-notify" },
+	{ ATH6KL_FW_CAPABILITY_REGDOMAIN, "regdomain" },
+	{ ATH6KL_FW_CAPABILITY_SCHED_SCAN_V2, "sched-scan-v2" },
+	{ ATH6KL_FW_CAPABILITY_HEART_BEAT_POLL, "hb-poll" },
+};
+
+static const char *ath6kl_init_get_fw_capa_name(unsigned int id)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(fw_capa_map); i++) {
+		if (fw_capa_map[i].id == id)
+			return fw_capa_map[i].name;
+	}
+
+	return "<unknown>";
+}
+
+static void ath6kl_init_get_fwcaps(struct ath6kl *ar, char *buf, size_t buf_len)
+{
+	u8 *data = (u8 *) ar->fw_capabilities;
+	size_t trunc_len, len = 0;
+	int i, index, bit;
+	char *trunc = "...";
+
+	for (i = 0; i < ATH6KL_FW_CAPABILITY_MAX; i++) {
+		index = i / 8;
+		bit = i % 8;
+
+		if (index >= sizeof(ar->fw_capabilities) * 4)
+			break;
+
+		if (buf_len - len < 4) {
+			ath6kl_warn("firmware capability buffer too small!\n");
+
+			/* add "..." to the end of string */
+			trunc_len = strlen(trunc) + 1;
+			strncpy(buf + buf_len - trunc_len, trunc, trunc_len);
+
+			return;
+		}
+
+		if (data[index] & (1 << bit)) {
+			len += scnprintf(buf + len, buf_len - len, "%s,",
+					    ath6kl_init_get_fw_capa_name(i));
+		}
+	}
+
+	/* overwrite the last comma */
+	if (len > 0)
+		len--;
+
+	buf[len] = '\0';
+}
+
+static int ath6kl_init_hw_reset(struct ath6kl *ar)
+{
+	ath6kl_dbg(ATH6KL_DBG_BOOT, "cold resetting the device");
+
+	return ath6kl_diag_write32(ar, RESET_CONTROL_ADDRESS,
+				   cpu_to_le32(RESET_CONTROL_COLD_RST));
+}
+
 static int __ath6kl_init_hw_start(struct ath6kl *ar)
 {
 	long timeleft;
 	int ret, i;
+	char buf[200];
 
 	ath6kl_dbg(ATH6KL_DBG_BOOT, "hw start\n");
 
@@ -1569,24 +1648,35 @@ static int __ath6kl_init_hw_start(struct ath6kl *ar)
 		goto err_power_off;
 
 	/* Do we need to finish the BMI phase */
-	/* FIXME: return error from ath6kl_bmi_done() */
-	if (ath6kl_bmi_done(ar)) {
-		ret = -EIO;
+	ret = ath6kl_bmi_done(ar);
+	if (ret)
 		goto err_power_off;
-	}
 
 	/*
 	 * The reason we have to wait for the target here is that the
 	 * driver layer has to init BMI in order to set the host block
 	 * size.
 	 */
-	if (ath6kl_htc_wait_target(ar->htc_target)) {
-		ret = -EIO;
+	ret = ath6kl_htc_wait_target(ar->htc_target);
+
+	if (ret == -ETIMEDOUT) {
+		/*
+		 * Most likely USB target is in odd state after reboot and
+		 * needs a reset. A cold reset makes the whole device
+		 * disappear from USB bus and initialisation starts from
+		 * beginning.
+		 */
+		ath6kl_warn("htc wait target timed out, resetting device\n");
+		ath6kl_init_hw_reset(ar);
+		goto err_power_off;
+	} else if (ret) {
+		ath6kl_err("htc wait target failed: %d\n", ret);
 		goto err_power_off;
 	}
 
-	if (ath6kl_init_service_ep(ar)) {
-		ret = -EIO;
+	ret = ath6kl_init_service_ep(ar);
+	if (ret) {
+		ath6kl_err("Endpoint service initilisation failed: %d\n", ret);
 		goto err_cleanup_scatter;
 	}
 
@@ -1617,6 +1707,8 @@ static int __ath6kl_init_hw_start(struct ath6kl *ar)
 			    ar->wiphy->fw_version,
 			    ar->fw_api,
 			    test_bit(TESTMODE, &ar->flag) ? " testmode" : "");
+		ath6kl_init_get_fwcaps(ar, buf, sizeof(buf));
+		ath6kl_info("firmware supports: %s\n", buf);
 	}
 
 	if (ar->version.abi_ver != ATH6KL_ABI_VERSION) {
@@ -1765,9 +1857,7 @@ void ath6kl_stop_txrx(struct ath6kl *ar)
 	 * Try to reset the device if we can. The driver may have been
 	 * configure NOT to reset the target during a debug session.
 	 */
-	ath6kl_dbg(ATH6KL_DBG_TRC,
-		   "attempting to reset target on instance destroy\n");
-	ath6kl_reset_device(ar, ar->target_type, true, true);
+	ath6kl_init_hw_reset(ar);
 
 	up(&ar->sem);
 }
diff --git a/drivers/net/wireless/ath/ath6kl/main.c b/drivers/net/wireless/ath/ath6kl/main.c
index bd50b6b7b492..d4fcfcad57d0 100644
--- a/drivers/net/wireless/ath/ath6kl/main.c
+++ b/drivers/net/wireless/ath/ath6kl/main.c
@@ -345,39 +345,6 @@ out:
 	return ret;
 }
 
-/* FIXME: move to a better place, target.h? */
-#define AR6003_RESET_CONTROL_ADDRESS 0x00004000
-#define AR6004_RESET_CONTROL_ADDRESS 0x00004000
-
-void ath6kl_reset_device(struct ath6kl *ar, u32 target_type,
-			 bool wait_fot_compltn, bool cold_reset)
-{
-	int status = 0;
-	u32 address;
-	__le32 data;
-
-	if (target_type != TARGET_TYPE_AR6003 &&
-	    target_type != TARGET_TYPE_AR6004)
-		return;
-
-	data = cold_reset ? cpu_to_le32(RESET_CONTROL_COLD_RST) :
-			    cpu_to_le32(RESET_CONTROL_MBOX_RST);
-
-	switch (target_type) {
-	case TARGET_TYPE_AR6003:
-		address = AR6003_RESET_CONTROL_ADDRESS;
-		break;
-	case TARGET_TYPE_AR6004:
-		address = AR6004_RESET_CONTROL_ADDRESS;
-		break;
-	}
-
-	status = ath6kl_diag_write32(ar, address, data);
-
-	if (status)
-		ath6kl_err("failed to reset target\n");
-}
-
 static void ath6kl_install_static_wep_keys(struct ath6kl_vif *vif)
 {
 	u8 index;
@@ -1327,9 +1294,11 @@ void init_netdev(struct net_device *dev)
 	dev->watchdog_timeo = ATH6KL_TX_TIMEOUT;
 
 	dev->needed_headroom = ETH_HLEN;
-	dev->needed_headroom += sizeof(struct ath6kl_llc_snap_hdr) +
-				sizeof(struct wmi_data_hdr) + HTC_HDR_LENGTH
-				+ WMI_MAX_TX_META_SZ + ATH6KL_HTC_ALIGN_BYTES;
+	dev->needed_headroom += roundup(sizeof(struct ath6kl_llc_snap_hdr) +
+					sizeof(struct wmi_data_hdr) +
+					HTC_HDR_LENGTH +
+					WMI_MAX_TX_META_SZ +
+					ATH6KL_HTC_ALIGN_BYTES, 4);
 
 	dev->hw_features |= NETIF_F_IP_CSUM | NETIF_F_RXCSUM;
 
diff --git a/drivers/net/wireless/ath/ath6kl/sdio.c b/drivers/net/wireless/ath/ath6kl/sdio.c
index d111980d44c0..fb141454c6d2 100644
--- a/drivers/net/wireless/ath/ath6kl/sdio.c
+++ b/drivers/net/wireless/ath/ath6kl/sdio.c
@@ -28,6 +28,7 @@
 #include "target.h"
 #include "debug.h"
 #include "cfg80211.h"
+#include "trace.h"
 
 struct ath6kl_sdio {
 	struct sdio_func *func;
@@ -179,6 +180,8 @@ static int ath6kl_sdio_io(struct sdio_func *func, u32 request, u32 addr,
 		   request & HIF_FIXED_ADDRESS ? " (fixed)" : "", buf, len);
 	ath6kl_dbg_dump(ATH6KL_DBG_SDIO_DUMP, NULL, "sdio ", buf, len);
 
+	trace_ath6kl_sdio(addr, request, buf, len);
+
 	return ret;
 }
 
@@ -309,6 +312,13 @@ static int ath6kl_sdio_scat_rw(struct ath6kl_sdio *ar_sdio,
 	sdio_claim_host(ar_sdio->func);
 
 	mmc_set_data_timeout(&data, ar_sdio->func->card);
+
+	trace_ath6kl_sdio_scat(scat_req->addr,
+			       scat_req->req,
+			       scat_req->len,
+			       scat_req->scat_entries,
+			       scat_req->scat_list);
+
 	/* synchronous call to process request */
 	mmc_wait_for_req(ar_sdio->func->card->host, &mmc_req);
 
@@ -1123,10 +1133,12 @@ static int ath6kl_sdio_bmi_write(struct ath6kl *ar, u8 *buf, u32 len)
 
 	ret = ath6kl_sdio_read_write_sync(ar, addr, buf, len,
 					  HIF_WR_SYNC_BYTE_INC);
-	if (ret)
+	if (ret) {
 		ath6kl_err("unable to send the bmi data to the device\n");
+		return ret;
+	}
 
-	return ret;
+	return 0;
 }
 
 static int ath6kl_sdio_bmi_read(struct ath6kl *ar, u8 *buf, u32 len)
diff --git a/drivers/net/wireless/ath/ath6kl/target.h b/drivers/net/wireless/ath/ath6kl/target.h
index a98c12ba70c1..a580a629a0da 100644
--- a/drivers/net/wireless/ath/ath6kl/target.h
+++ b/drivers/net/wireless/ath/ath6kl/target.h
@@ -25,7 +25,7 @@
 #define AR6004_BOARD_DATA_SZ     6144
 #define AR6004_BOARD_EXT_DATA_SZ 0
 
-#define RESET_CONTROL_ADDRESS		0x00000000
+#define RESET_CONTROL_ADDRESS		0x00004000
 #define RESET_CONTROL_COLD_RST		0x00000100
 #define RESET_CONTROL_MBOX_RST		0x00000004
 
diff --git a/drivers/net/wireless/ath/ath6kl/trace.c b/drivers/net/wireless/ath/ath6kl/trace.c
new file mode 100644
index 000000000000..e7d64b1285cb
--- /dev/null
+++ b/drivers/net/wireless/ath/ath6kl/trace.c
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2012 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <linux/module.h>
+
+#define CREATE_TRACE_POINTS
+#include "trace.h"
+
+EXPORT_TRACEPOINT_SYMBOL(ath6kl_sdio);
+EXPORT_TRACEPOINT_SYMBOL(ath6kl_sdio_scat);
diff --git a/drivers/net/wireless/ath/ath6kl/trace.h b/drivers/net/wireless/ath/ath6kl/trace.h
new file mode 100644
index 000000000000..1a1ea7881b4d
--- /dev/null
+++ b/drivers/net/wireless/ath/ath6kl/trace.h
@@ -0,0 +1,332 @@
+#if !defined(_ATH6KL_TRACE_H) || defined(TRACE_HEADER_MULTI_READ)
+
+#include <net/cfg80211.h>
+#include <linux/skbuff.h>
+#include <linux/tracepoint.h>
+#include "wmi.h"
+#include "hif.h"
+
+#if !defined(_ATH6KL_TRACE_H)
+static inline unsigned int ath6kl_get_wmi_id(void *buf, size_t buf_len)
+{
+	struct wmi_cmd_hdr *hdr = buf;
+
+	if (buf_len < sizeof(*hdr))
+		return 0;
+
+	return le16_to_cpu(hdr->cmd_id);
+}
+#endif /* __ATH6KL_TRACE_H */
+
+#define _ATH6KL_TRACE_H
+
+/* create empty functions when tracing is disabled */
+#if !defined(CONFIG_ATH6KL_TRACING)
+#undef TRACE_EVENT
+#define TRACE_EVENT(name, proto, ...) \
+static inline void trace_ ## name(proto) {}
+#undef DECLARE_EVENT_CLASS
+#define DECLARE_EVENT_CLASS(...)
+#undef DEFINE_EVENT
+#define DEFINE_EVENT(evt_class, name, proto, ...) \
+static inline void trace_ ## name(proto) {}
+#endif /* !CONFIG_ATH6KL_TRACING || __CHECKER__ */
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM ath6kl
+
+TRACE_EVENT(ath6kl_wmi_cmd,
+	TP_PROTO(void *buf, size_t buf_len),
+
+	TP_ARGS(buf, buf_len),
+
+	TP_STRUCT__entry(
+		__field(unsigned int, id)
+		__field(size_t, buf_len)
+		__dynamic_array(u8, buf, buf_len)
+	),
+
+	TP_fast_assign(
+		__entry->id = ath6kl_get_wmi_id(buf, buf_len);
+		__entry->buf_len = buf_len;
+		memcpy(__get_dynamic_array(buf), buf, buf_len);
+	),
+
+	TP_printk(
+		"id %d len %zd",
+		__entry->id, __entry->buf_len
+	)
+);
+
+TRACE_EVENT(ath6kl_wmi_event,
+	TP_PROTO(void *buf, size_t buf_len),
+
+	TP_ARGS(buf, buf_len),
+
+	TP_STRUCT__entry(
+		__field(unsigned int, id)
+		__field(size_t, buf_len)
+		__dynamic_array(u8, buf, buf_len)
+	),
+
+	TP_fast_assign(
+		__entry->id = ath6kl_get_wmi_id(buf, buf_len);
+		__entry->buf_len = buf_len;
+		memcpy(__get_dynamic_array(buf), buf, buf_len);
+	),
+
+	TP_printk(
+		"id %d len %zd",
+		__entry->id, __entry->buf_len
+	)
+);
+
+TRACE_EVENT(ath6kl_sdio,
+	TP_PROTO(unsigned int addr, int flags,
+		 void *buf, size_t buf_len),
+
+	TP_ARGS(addr, flags, buf, buf_len),
+
+	TP_STRUCT__entry(
+		__field(unsigned int, tx)
+		__field(unsigned int, addr)
+		__field(int, flags)
+		__field(size_t, buf_len)
+		__dynamic_array(u8, buf, buf_len)
+	),
+
+	TP_fast_assign(
+		__entry->addr = addr;
+		__entry->flags = flags;
+		__entry->buf_len = buf_len;
+		memcpy(__get_dynamic_array(buf), buf, buf_len);
+
+		if (flags & HIF_WRITE)
+			__entry->tx = 1;
+		else
+			__entry->tx = 0;
+	),
+
+	TP_printk(
+		"%s addr 0x%x flags 0x%x len %zd\n",
+		__entry->tx ? "tx" : "rx",
+		__entry->addr,
+		__entry->flags,
+		__entry->buf_len
+	)
+);
+
+TRACE_EVENT(ath6kl_sdio_scat,
+	TP_PROTO(unsigned int addr, int flags, unsigned int total_len,
+		 unsigned int entries, struct hif_scatter_item *list),
+
+	TP_ARGS(addr, flags, total_len, entries, list),
+
+	TP_STRUCT__entry(
+		__field(unsigned int, tx)
+		__field(unsigned int, addr)
+		__field(int, flags)
+		__field(unsigned int, entries)
+		__field(size_t, total_len)
+		__dynamic_array(unsigned int, len_array, entries)
+		__dynamic_array(u8, data, total_len)
+	),
+
+	TP_fast_assign(
+		unsigned int *len_array;
+		int i, offset = 0;
+		size_t len;
+
+		__entry->addr = addr;
+		__entry->flags = flags;
+		__entry->entries = entries;
+		__entry->total_len = total_len;
+
+		if (flags & HIF_WRITE)
+			__entry->tx = 1;
+		else
+			__entry->tx = 0;
+
+		len_array = __get_dynamic_array(len_array);
+
+		for (i = 0; i < entries; i++) {
+			len = list[i].len;
+
+			memcpy((u8 *) __get_dynamic_array(data) + offset,
+			       list[i].buf, len);
+
+			len_array[i] = len;
+			offset += len;
+		}
+	),
+
+	TP_printk(
+		"%s addr 0x%x flags 0x%x entries %d total_len %zd\n",
+		__entry->tx ? "tx" : "rx",
+		__entry->addr,
+		__entry->flags,
+		__entry->entries,
+		__entry->total_len
+	)
+);
+
+TRACE_EVENT(ath6kl_sdio_irq,
+	TP_PROTO(void *buf, size_t buf_len),
+
+	TP_ARGS(buf, buf_len),
+
+	TP_STRUCT__entry(
+		__field(size_t, buf_len)
+		__dynamic_array(u8, buf, buf_len)
+	),
+
+	TP_fast_assign(
+		__entry->buf_len = buf_len;
+		memcpy(__get_dynamic_array(buf), buf, buf_len);
+	),
+
+	TP_printk(
+		"irq len %zd\n", __entry->buf_len
+	)
+);
+
+TRACE_EVENT(ath6kl_htc_rx,
+	TP_PROTO(int status, int endpoint, void *buf,
+		 size_t buf_len),
+
+	TP_ARGS(status, endpoint, buf, buf_len),
+
+	TP_STRUCT__entry(
+		__field(int, status)
+		__field(int, endpoint)
+		__field(size_t, buf_len)
+		__dynamic_array(u8, buf, buf_len)
+	),
+
+	TP_fast_assign(
+		__entry->status = status;
+		__entry->endpoint = endpoint;
+		__entry->buf_len = buf_len;
+		memcpy(__get_dynamic_array(buf), buf, buf_len);
+	),
+
+	TP_printk(
+		"status %d endpoint %d len %zd\n",
+		__entry->status,
+		__entry->endpoint,
+		__entry->buf_len
+	)
+);
+
+TRACE_EVENT(ath6kl_htc_tx,
+	TP_PROTO(int status, int endpoint, void *buf,
+		 size_t buf_len),
+
+	TP_ARGS(status, endpoint, buf, buf_len),
+
+	TP_STRUCT__entry(
+		__field(int, status)
+		__field(int, endpoint)
+		__field(size_t, buf_len)
+		__dynamic_array(u8, buf, buf_len)
+	),
+
+	TP_fast_assign(
+		__entry->status = status;
+		__entry->endpoint = endpoint;
+		__entry->buf_len = buf_len;
+		memcpy(__get_dynamic_array(buf), buf, buf_len);
+	),
+
+	TP_printk(
+		"status %d endpoint %d len %zd\n",
+		__entry->status,
+		__entry->endpoint,
+		__entry->buf_len
+	)
+);
+
+#define ATH6KL_MSG_MAX 200
+
+DECLARE_EVENT_CLASS(ath6kl_log_event,
+	TP_PROTO(struct va_format *vaf),
+	TP_ARGS(vaf),
+	TP_STRUCT__entry(
+		__dynamic_array(char, msg, ATH6KL_MSG_MAX)
+	),
+	TP_fast_assign(
+		WARN_ON_ONCE(vsnprintf(__get_dynamic_array(msg),
+				       ATH6KL_MSG_MAX,
+				       vaf->fmt,
+				       *vaf->va) >= ATH6KL_MSG_MAX);
+	),
+	TP_printk("%s", __get_str(msg))
+);
+
+DEFINE_EVENT(ath6kl_log_event, ath6kl_log_err,
+	     TP_PROTO(struct va_format *vaf),
+	     TP_ARGS(vaf)
+);
+
+DEFINE_EVENT(ath6kl_log_event, ath6kl_log_warn,
+	     TP_PROTO(struct va_format *vaf),
+	     TP_ARGS(vaf)
+);
+
+DEFINE_EVENT(ath6kl_log_event, ath6kl_log_info,
+	     TP_PROTO(struct va_format *vaf),
+	     TP_ARGS(vaf)
+);
+
+TRACE_EVENT(ath6kl_log_dbg,
+	TP_PROTO(unsigned int level, struct va_format *vaf),
+	TP_ARGS(level, vaf),
+	TP_STRUCT__entry(
+		__field(unsigned int, level)
+		__dynamic_array(char, msg, ATH6KL_MSG_MAX)
+	),
+	TP_fast_assign(
+		__entry->level = level;
+		WARN_ON_ONCE(vsnprintf(__get_dynamic_array(msg),
+				       ATH6KL_MSG_MAX,
+				       vaf->fmt,
+				       *vaf->va) >= ATH6KL_MSG_MAX);
+	),
+	TP_printk("%s", __get_str(msg))
+);
+
+TRACE_EVENT(ath6kl_log_dbg_dump,
+	TP_PROTO(const char *msg, const char *prefix,
+		 const void *buf, size_t buf_len),
+
+	TP_ARGS(msg, prefix, buf, buf_len),
+
+	TP_STRUCT__entry(
+		__string(msg, msg)
+		__string(prefix, prefix)
+		__field(size_t, buf_len)
+		__dynamic_array(u8, buf, buf_len)
+	),
+
+	TP_fast_assign(
+		__assign_str(msg, msg);
+		__assign_str(prefix, prefix);
+		__entry->buf_len = buf_len;
+		memcpy(__get_dynamic_array(buf), buf, buf_len);
+	),
+
+	TP_printk(
+		"%s/%s\n", __get_str(prefix), __get_str(msg)
+	)
+);
+
+#endif /* _ ATH6KL_TRACE_H || TRACE_HEADER_MULTI_READ*/
+
+/* we don't want to use include/trace/events */
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE trace
+
+/* This part must be outside protection */
+#include <trace/define_trace.h>
diff --git a/drivers/net/wireless/ath/ath6kl/txrx.c b/drivers/net/wireless/ath/ath6kl/txrx.c
index 78b369286579..ebb24045a8ae 100644
--- a/drivers/net/wireless/ath/ath6kl/txrx.c
+++ b/drivers/net/wireless/ath/ath6kl/txrx.c
@@ -20,6 +20,7 @@
 #include "core.h"
 #include "debug.h"
 #include "htc-ops.h"
+#include "trace.h"
 
 /*
  * tid - tid_mux0..tid_mux3
@@ -288,6 +289,8 @@ int ath6kl_control_tx(void *devt, struct sk_buff *skb,
 	int status = 0;
 	struct ath6kl_cookie *cookie = NULL;
 
+	trace_ath6kl_wmi_cmd(skb->data, skb->len);
+
 	if (WARN_ON_ONCE(ar->state == ATH6KL_STATE_WOW)) {
 		dev_kfree_skb(skb);
 		return -EACCES;
@@ -1324,7 +1327,7 @@ void ath6kl_rx(struct htc_target *target, struct htc_packet *packet)
 		   __func__, ar, ept, skb, packet->buf,
 		   packet->act_len, status);
 
-	if (status || !(skb->data + HTC_HDR_LENGTH)) {
+	if (status || packet->act_len < HTC_HDR_LENGTH) {
 		dev_kfree_skb(skb);
 		return;
 	}
diff --git a/drivers/net/wireless/ath/ath6kl/usb.c b/drivers/net/wireless/ath/ath6kl/usb.c
index 5fcd342762de..bed0d337712d 100644
--- a/drivers/net/wireless/ath/ath6kl/usb.c
+++ b/drivers/net/wireless/ath/ath6kl/usb.c
@@ -856,11 +856,9 @@ static int ath6kl_usb_submit_ctrl_out(struct ath6kl_usb *ar_usb,
 	int ret;
 
 	if (size > 0) {
-		buf = kmalloc(size, GFP_KERNEL);
+		buf = kmemdup(data, size, GFP_KERNEL);
 		if (buf == NULL)
 			return -ENOMEM;
-
-		memcpy(buf, data, size);
 	}
 
 	/* note: if successful returns number of bytes transfered */
@@ -872,8 +870,9 @@ static int ath6kl_usb_submit_ctrl_out(struct ath6kl_usb *ar_usb,
 			      size, 1000);
 
 	if (ret < 0) {
-		ath6kl_dbg(ATH6KL_DBG_USB, "%s failed,result = %d\n",
-			   __func__, ret);
+		ath6kl_warn("Failed to submit usb control message: %d\n", ret);
+		kfree(buf);
+		return ret;
 	}
 
 	kfree(buf);
@@ -903,8 +902,9 @@ static int ath6kl_usb_submit_ctrl_in(struct ath6kl_usb *ar_usb,
 				 size, 2 * HZ);
 
 	if (ret < 0) {
-		ath6kl_dbg(ATH6KL_DBG_USB, "%s failed,result = %d\n",
-			   __func__, ret);
+		ath6kl_warn("Failed to read usb control message: %d\n", ret);
+		kfree(buf);
+		return ret;
 	}
 
 	memcpy((u8 *) data, buf, size);
@@ -961,8 +961,10 @@ static int ath6kl_usb_diag_read32(struct ath6kl *ar, u32 address, u32 *data)
 				ATH6KL_USB_CONTROL_REQ_DIAG_RESP,
 				ar_usb->diag_resp_buffer, &resp_len);
 
-	if (ret)
+	if (ret) {
+		ath6kl_warn("diag read32 failed: %d\n", ret);
 		return ret;
+	}
 
 	resp = (struct ath6kl_usb_ctrl_diag_resp_read *)
 		ar_usb->diag_resp_buffer;
@@ -976,6 +978,7 @@ static int ath6kl_usb_diag_write32(struct ath6kl *ar, u32 address, __le32 data)
 {
 	struct ath6kl_usb *ar_usb = ar->hif_priv;
 	struct ath6kl_usb_ctrl_diag_cmd_write *cmd;
+	int ret;
 
 	cmd = (struct ath6kl_usb_ctrl_diag_cmd_write *) ar_usb->diag_cmd_buffer;
 
@@ -984,12 +987,17 @@ static int ath6kl_usb_diag_write32(struct ath6kl *ar, u32 address, __le32 data)
 	cmd->address = cpu_to_le32(address);
 	cmd->value = data;
 
-	return ath6kl_usb_ctrl_msg_exchange(ar_usb,
-					    ATH6KL_USB_CONTROL_REQ_DIAG_CMD,
-					    (u8 *) cmd,
-					    sizeof(*cmd),
-					    0, NULL, NULL);
+	ret = ath6kl_usb_ctrl_msg_exchange(ar_usb,
+					   ATH6KL_USB_CONTROL_REQ_DIAG_CMD,
+					   (u8 *) cmd,
+					   sizeof(*cmd),
+					   0, NULL, NULL);
+	if (ret) {
+		ath6kl_warn("diag_write32 failed: %d\n", ret);
+		return ret;
+	}
 
+	return 0;
 }
 
 static int ath6kl_usb_bmi_read(struct ath6kl *ar, u8 *buf, u32 len)
@@ -1001,7 +1009,7 @@ static int ath6kl_usb_bmi_read(struct ath6kl *ar, u8 *buf, u32 len)
 	ret = ath6kl_usb_submit_ctrl_in(ar_usb,
 					ATH6KL_USB_CONTROL_REQ_RECV_BMI_RESP,
 					0, 0, buf, len);
-	if (ret != 0) {
+	if (ret) {
 		ath6kl_err("Unable to read the bmi data from the device: %d\n",
 			   ret);
 		return ret;
@@ -1019,7 +1027,7 @@ static int ath6kl_usb_bmi_write(struct ath6kl *ar, u8 *buf, u32 len)
 	ret = ath6kl_usb_submit_ctrl_out(ar_usb,
 					 ATH6KL_USB_CONTROL_REQ_SEND_BMI_CMD,
 					 0, 0, buf, len);
-	if (ret != 0) {
+	if (ret) {
 		ath6kl_err("unable to send the bmi data to the device: %d\n",
 			   ret);
 		return ret;
diff --git a/drivers/net/wireless/ath/ath6kl/wmi.c b/drivers/net/wireless/ath/ath6kl/wmi.c
index d76b5bd81a0d..87aefb4c4c23 100644
--- a/drivers/net/wireless/ath/ath6kl/wmi.c
+++ b/drivers/net/wireless/ath/ath6kl/wmi.c
@@ -20,6 +20,7 @@
 #include "core.h"
 #include "debug.h"
 #include "testmode.h"
+#include "trace.h"
 #include "../regd.h"
 #include "../regd_common.h"
 
@@ -2028,6 +2029,9 @@ int ath6kl_wmi_beginscan_cmd(struct wmi *wmi, u8 if_idx,
 		if (!sband)
 			continue;
 
+		if (WARN_ON(band >= ATH6KL_NUM_BANDS))
+			break;
+
 		ratemask = rates[band];
 		supp_rates = sc->supp_rates[band].rates;
 		num_rates = 0;
@@ -4086,6 +4090,8 @@ int ath6kl_wmi_control_rx(struct wmi *wmi, struct sk_buff *skb)
 		return -EINVAL;
 	}
 
+	trace_ath6kl_wmi_event(skb->data, skb->len);
+
 	return ath6kl_wmi_proc_events(wmi, skb);
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/ar5008_phy.c b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
index fd69376ecc83..391da5ad6a99 100644
--- a/drivers/net/wireless/ath/ath9k/ar5008_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
@@ -18,6 +18,7 @@
 #include "hw-ops.h"
 #include "../regd.h"
 #include "ar9002_phy.h"
+#include "ar5008_initvals.h"
 
 /* All code below is for AR5008, AR9001, AR9002 */
 
@@ -43,23 +44,16 @@ static const int m2ThreshLowExt_off = 127;
 static const int m1ThreshExt_off = 127;
 static const int m2ThreshExt_off = 127;
 
+static const struct ar5416IniArray bank0 = STATIC_INI_ARRAY(ar5416Bank0);
+static const struct ar5416IniArray bank1 = STATIC_INI_ARRAY(ar5416Bank1);
+static const struct ar5416IniArray bank2 = STATIC_INI_ARRAY(ar5416Bank2);
+static const struct ar5416IniArray bank3 = STATIC_INI_ARRAY(ar5416Bank3);
+static const struct ar5416IniArray bank7 = STATIC_INI_ARRAY(ar5416Bank7);
 
-static void ar5008_rf_bank_setup(u32 *bank, struct ar5416IniArray *array,
-				 int col)
-{
-	int i;
-
-	for (i = 0; i < array->ia_rows; i++)
-		bank[i] = INI_RA(array, i, col);
-}
-
-
-#define REG_WRITE_RF_ARRAY(iniarray, regData, regWr) \
-	ar5008_write_rf_array(ah, iniarray, regData, &(regWr))
-
-static void ar5008_write_rf_array(struct ath_hw *ah, struct ar5416IniArray *array,
-				  u32 *data, unsigned int *writecnt)
+static void ar5008_write_bank6(struct ath_hw *ah, unsigned int *writecnt)
 {
+	struct ar5416IniArray *array = &ah->iniBank6;
+	u32 *data = ah->analogBank6Data;
 	int r;
 
 	ENABLE_REGWRITE_BUFFER(ah);
@@ -165,7 +159,7 @@ static void ar5008_hw_force_bias(struct ath_hw *ah, u16 synth_freq)
 	ar5008_hw_phy_modify_rx_buffer(ah->analogBank6Data, tmp_reg, 3, 181, 3);
 
 	/* write Bank 6 with new params */
-	REG_WRITE_RF_ARRAY(&ah->iniBank6, ah->analogBank6Data, reg_writes);
+	ar5008_write_bank6(ah, &reg_writes);
 }
 
 /**
@@ -469,31 +463,16 @@ static void ar5008_hw_spur_mitigate(struct ath_hw *ah,
  */
 static int ar5008_hw_rf_alloc_ext_banks(struct ath_hw *ah)
 {
-#define ATH_ALLOC_BANK(bank, size) do { \
-		bank = devm_kzalloc(ah->dev, sizeof(u32) * size, GFP_KERNEL); \
-		if (!bank) \
-			goto error; \
-	} while (0);
-
-	struct ath_common *common = ath9k_hw_common(ah);
+	int size = ah->iniBank6.ia_rows * sizeof(u32);
 
 	if (AR_SREV_9280_20_OR_LATER(ah))
 	    return 0;
 
-	ATH_ALLOC_BANK(ah->analogBank0Data, ah->iniBank0.ia_rows);
-	ATH_ALLOC_BANK(ah->analogBank1Data, ah->iniBank1.ia_rows);
-	ATH_ALLOC_BANK(ah->analogBank2Data, ah->iniBank2.ia_rows);
-	ATH_ALLOC_BANK(ah->analogBank3Data, ah->iniBank3.ia_rows);
-	ATH_ALLOC_BANK(ah->analogBank6Data, ah->iniBank6.ia_rows);
-	ATH_ALLOC_BANK(ah->analogBank6TPCData, ah->iniBank6TPC.ia_rows);
-	ATH_ALLOC_BANK(ah->analogBank7Data, ah->iniBank7.ia_rows);
-	ATH_ALLOC_BANK(ah->bank6Temp, ah->iniBank6.ia_rows);
+	ah->analogBank6Data = devm_kzalloc(ah->dev, size, GFP_KERNEL);
+	if (!ah->analogBank6Data)
+		return -ENOMEM;
 
 	return 0;
-#undef ATH_ALLOC_BANK
-error:
-	ath_err(common, "Cannot allocate RF banks\n");
-	return -ENOMEM;
 }
 
 
@@ -517,6 +496,7 @@ static bool ar5008_hw_set_rf_regs(struct ath_hw *ah,
 	u32 ob5GHz = 0, db5GHz = 0;
 	u32 ob2GHz = 0, db2GHz = 0;
 	int regWrites = 0;
+	int i;
 
 	/*
 	 * Software does not need to program bank data
@@ -529,25 +509,8 @@ static bool ar5008_hw_set_rf_regs(struct ath_hw *ah,
 	/* Setup rf parameters */
 	eepMinorRev = ah->eep_ops->get_eeprom(ah, EEP_MINOR_REV);
 
-	/* Setup Bank 0 Write */
-	ar5008_rf_bank_setup(ah->analogBank0Data, &ah->iniBank0, 1);
-
-	/* Setup Bank 1 Write */
-	ar5008_rf_bank_setup(ah->analogBank1Data, &ah->iniBank1, 1);
-
-	/* Setup Bank 2 Write */
-	ar5008_rf_bank_setup(ah->analogBank2Data, &ah->iniBank2, 1);
-
-	/* Setup Bank 6 Write */
-	ar5008_rf_bank_setup(ah->analogBank3Data, &ah->iniBank3,
-		      modesIndex);
-	{
-		int i;
-		for (i = 0; i < ah->iniBank6TPC.ia_rows; i++) {
-			ah->analogBank6Data[i] =
-			    INI_RA(&ah->iniBank6TPC, i, modesIndex);
-		}
-	}
+	for (i = 0; i < ah->iniBank6.ia_rows; i++)
+		ah->analogBank6Data[i] = INI_RA(&ah->iniBank6, i, modesIndex);
 
 	/* Only the 5 or 2 GHz OB/DB need to be set for a mode */
 	if (eepMinorRev >= 2) {
@@ -568,22 +531,13 @@ static bool ar5008_hw_set_rf_regs(struct ath_hw *ah,
 		}
 	}
 
-	/* Setup Bank 7 Setup */
-	ar5008_rf_bank_setup(ah->analogBank7Data, &ah->iniBank7, 1);
-
 	/* Write Analog registers */
-	REG_WRITE_RF_ARRAY(&ah->iniBank0, ah->analogBank0Data,
-			   regWrites);
-	REG_WRITE_RF_ARRAY(&ah->iniBank1, ah->analogBank1Data,
-			   regWrites);
-	REG_WRITE_RF_ARRAY(&ah->iniBank2, ah->analogBank2Data,
-			   regWrites);
-	REG_WRITE_RF_ARRAY(&ah->iniBank3, ah->analogBank3Data,
-			   regWrites);
-	REG_WRITE_RF_ARRAY(&ah->iniBank6TPC, ah->analogBank6Data,
-			   regWrites);
-	REG_WRITE_RF_ARRAY(&ah->iniBank7, ah->analogBank7Data,
-			   regWrites);
+	REG_WRITE_ARRAY(&bank0, 1, regWrites);
+	REG_WRITE_ARRAY(&bank1, 1, regWrites);
+	REG_WRITE_ARRAY(&bank2, 1, regWrites);
+	REG_WRITE_ARRAY(&bank3, modesIndex, regWrites);
+	ar5008_write_bank6(ah, &regWrites);
+	REG_WRITE_ARRAY(&bank7, 1, regWrites);
 
 	return true;
 }
diff --git a/drivers/net/wireless/ath/ath9k/ar9002_calib.c b/drivers/net/wireless/ath/ath9k/ar9002_calib.c
index c55e5bbafc46..9f589744a9f9 100644
--- a/drivers/net/wireless/ath/ath9k/ar9002_calib.c
+++ b/drivers/net/wireless/ath/ath9k/ar9002_calib.c
@@ -731,7 +731,8 @@ static bool ar9285_hw_cl_cal(struct ath_hw *ah, struct ath9k_channel *chan)
 		if (!ath9k_hw_wait(ah, AR_PHY_AGC_CONTROL,
 				  AR_PHY_AGC_CONTROL_CAL, 0, AH_WAIT_TIMEOUT)) {
 			ath_dbg(common, CALIBRATE,
-				"offset calibration failed to complete in 1ms; noisy environment?\n");
+				"offset calibration failed to complete in %d ms; noisy environment?\n",
+				AH_WAIT_TIMEOUT / 1000);
 			return false;
 		}
 		REG_CLR_BIT(ah, AR_PHY_TURBO, AR_PHY_FC_DYN2040_EN);
@@ -745,7 +746,8 @@ static bool ar9285_hw_cl_cal(struct ath_hw *ah, struct ath9k_channel *chan)
 	if (!ath9k_hw_wait(ah, AR_PHY_AGC_CONTROL, AR_PHY_AGC_CONTROL_CAL,
 			  0, AH_WAIT_TIMEOUT)) {
 		ath_dbg(common, CALIBRATE,
-			"offset calibration failed to complete in 1ms; noisy environment?\n");
+			"offset calibration failed to complete in %d ms; noisy environment?\n",
+			AH_WAIT_TIMEOUT / 1000);
 		return false;
 	}
 
@@ -841,7 +843,8 @@ static bool ar9002_hw_init_cal(struct ath_hw *ah, struct ath9k_channel *chan)
 				   AR_PHY_AGC_CONTROL_CAL,
 				   0, AH_WAIT_TIMEOUT)) {
 			ath_dbg(common, CALIBRATE,
-				"offset calibration failed to complete in 1ms; noisy environment?\n");
+				"offset calibration failed to complete in %d ms; noisy environment?\n",
+				AH_WAIT_TIMEOUT / 1000);
 			return false;
 		}
 
diff --git a/drivers/net/wireless/ath/ath9k/ar9002_hw.c b/drivers/net/wireless/ath/ath9k/ar9002_hw.c
index f053d978540e..830daa12feb6 100644
--- a/drivers/net/wireless/ath/ath9k/ar9002_hw.c
+++ b/drivers/net/wireless/ath/ath9k/ar9002_hw.c
@@ -67,12 +67,10 @@ static int ar9002_hw_init_mode_regs(struct ath_hw *ah)
 	} else if (AR_SREV_9100_OR_LATER(ah)) {
 		INIT_INI_ARRAY(&ah->iniModes, ar5416Modes_9100);
 		INIT_INI_ARRAY(&ah->iniCommon, ar5416Common_9100);
-		INIT_INI_ARRAY(&ah->iniBank6, ar5416Bank6_9100);
 		INIT_INI_ARRAY(&ah->iniAddac, ar5416Addac_9100);
 	} else {
 		INIT_INI_ARRAY(&ah->iniModes, ar5416Modes);
 		INIT_INI_ARRAY(&ah->iniCommon, ar5416Common);
-		INIT_INI_ARRAY(&ah->iniBank6TPC, ar5416Bank6TPC);
 		INIT_INI_ARRAY(&ah->iniAddac, ar5416Addac);
 	}
 
@@ -80,20 +78,11 @@ static int ar9002_hw_init_mode_regs(struct ath_hw *ah)
 		/* Common for AR5416, AR913x, AR9160 */
 		INIT_INI_ARRAY(&ah->iniBB_RfGain, ar5416BB_RfGain);
 
-		INIT_INI_ARRAY(&ah->iniBank0, ar5416Bank0);
-		INIT_INI_ARRAY(&ah->iniBank1, ar5416Bank1);
-		INIT_INI_ARRAY(&ah->iniBank2, ar5416Bank2);
-		INIT_INI_ARRAY(&ah->iniBank3, ar5416Bank3);
-		INIT_INI_ARRAY(&ah->iniBank7, ar5416Bank7);
-
-		/* Common for AR5416, AR9160 */
-		if (!AR_SREV_9100(ah))
-			INIT_INI_ARRAY(&ah->iniBank6, ar5416Bank6);
-
 		/* Common for AR913x, AR9160 */
 		if (!AR_SREV_5416(ah))
-			INIT_INI_ARRAY(&ah->iniBank6TPC,
-				      ar5416Bank6TPC_9100);
+			INIT_INI_ARRAY(&ah->iniBank6, ar5416Bank6TPC_9100);
+		else
+			INIT_INI_ARRAY(&ah->iniBank6, ar5416Bank6TPC);
 	}
 
 	/* iniAddac needs to be modified for these chips */
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_calib.c b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
index f76c3ca07a45..639ba7d18ea4 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_calib.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
@@ -1126,7 +1126,8 @@ skip_tx_iqcal:
 			ar9003_hw_rtt_disable(ah);
 
 		ath_dbg(common, CALIBRATE,
-			"offset calibration failed to complete in 1ms; noisy environment?\n");
+			"offset calibration failed to complete in %d ms; noisy environment?\n",
+			AH_WAIT_TIMEOUT / 1000);
 		return false;
 	}
 
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
index 881e989ea470..e6b92ff265fd 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
@@ -3606,6 +3606,12 @@ static void ar9003_hw_ant_ctrl_apply(struct ath_hw *ah, bool is2ghz)
 	value = ar9003_hw_ant_ctrl_common_2_get(ah, is2ghz);
 	REG_RMW_FIELD(ah, AR_PHY_SWITCH_COM_2, AR_SWITCH_TABLE_COM2_ALL, value);
 
+	if ((AR_SREV_9462(ah)) && (ah->rxchainmask == 0x2)) {
+		value = ar9003_hw_ant_ctrl_chain_get(ah, 1, is2ghz);
+		REG_RMW_FIELD(ah, switch_chain_reg[0],
+			      AR_SWITCH_TABLE_ALL, value);
+	}
+
 	for (chain = 0; chain < AR9300_MAX_CHAINS; chain++) {
 		if ((ah->rxchainmask & BIT(chain)) ||
 		    (ah->txchainmask & BIT(chain))) {
@@ -3772,6 +3778,17 @@ static void ar9003_hw_atten_apply(struct ath_hw *ah, struct ath9k_channel *chan)
 					  AR_PHY_EXT_ATTEN_CTL_2,
 					 };
 
+	if ((AR_SREV_9462(ah)) && (ah->rxchainmask == 0x2)) {
+		value = ar9003_hw_atten_chain_get(ah, 1, chan);
+		REG_RMW_FIELD(ah, ext_atten_reg[0],
+			      AR_PHY_EXT_ATTEN_CTL_XATTEN1_DB, value);
+
+		value = ar9003_hw_atten_chain_get_margin(ah, 1, chan);
+		REG_RMW_FIELD(ah, ext_atten_reg[0],
+			      AR_PHY_EXT_ATTEN_CTL_XATTEN1_MARGIN,
+			      value);
+	}
+
 	/* Test value. if 0 then attenuation is unused. Don't load anything. */
 	for (i = 0; i < 3; i++) {
 		if (ah->txchainmask & BIT(i)) {
diff --git a/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h b/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
index ccc42a71b436..999ab08c34e6 100644
--- a/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
@@ -37,28 +37,28 @@ static const u32 ar9462_pciephy_clkreq_enable_L1_2p0[][2] = {
 	/* Addr      allmodes  */
 	{0x00018c00, 0x18253ede},
 	{0x00018c04, 0x000801d8},
-	{0x00018c08, 0x0003580c},
+	{0x00018c08, 0x0003780c},
 };
 
 static const u32 ar9462_2p0_baseband_postamble[][5] = {
 	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
 	{0x00009810, 0xd00a8005, 0xd00a8005, 0xd00a8011, 0xd00a800d},
 	{0x00009820, 0x206a022e, 0x206a022e, 0x206a012e, 0x206a01ae},
-	{0x00009824, 0x5ac640de, 0x5ac640d0, 0x5ac640d0, 0x63c640da},
+	{0x00009824, 0x63c640de, 0x5ac640d0, 0x5ac640d0, 0x63c640da},
 	{0x00009828, 0x0796be89, 0x0696b081, 0x0696b881, 0x09143e81},
 	{0x0000982c, 0x05eea6d4, 0x05eea6d4, 0x05eea6d4, 0x05eea6d4},
 	{0x00009830, 0x0000059c, 0x0000059c, 0x0000119c, 0x0000119c},
 	{0x00009c00, 0x000000c4, 0x000000c4, 0x000000c4, 0x000000c4},
-	{0x00009e00, 0x0372111a, 0x0372111a, 0x037216a0, 0x037216a0},
+	{0x00009e00, 0x0372111a, 0x0372111a, 0x037216a0, 0x037216a2},
 	{0x00009e04, 0x001c2020, 0x001c2020, 0x001c2020, 0x001c2020},
 	{0x00009e0c, 0x6c4000e2, 0x6d4000e2, 0x6d4000e2, 0x6c4000d8},
 	{0x00009e10, 0x92c88d2e, 0x7ec88d2e, 0x7ec84d2e, 0x7ec86d2e},
-	{0x00009e14, 0x37b95d5e, 0x37b9605e, 0x3376605e, 0x32395d5e},
+	{0x00009e14, 0x37b95d5e, 0x37b9605e, 0x3236605e, 0x32365a5e},
 	{0x00009e18, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
 	{0x00009e1c, 0x0001cf9c, 0x0001cf9c, 0x00021f9c, 0x00021f9c},
 	{0x00009e20, 0x000003b5, 0x000003b5, 0x000003ce, 0x000003ce},
 	{0x00009e2c, 0x0000001c, 0x0000001c, 0x00000021, 0x00000021},
-	{0x00009e3c, 0xcf946222, 0xcf946222, 0xcfd5c782, 0xcfd5c282},
+	{0x00009e3c, 0xcf946220, 0xcf946220, 0xcfd5c782, 0xcfd5c282},
 	{0x00009e44, 0x62321e27, 0x62321e27, 0xfe291e27, 0xfe291e27},
 	{0x00009e48, 0x5030201a, 0x5030201a, 0x50302012, 0x50302012},
 	{0x00009fc8, 0x0003f000, 0x0003f000, 0x0001a000, 0x0001a000},
@@ -82,9 +82,9 @@ static const u32 ar9462_2p0_baseband_postamble[][5] = {
 	{0x0000a2d0, 0x00041981, 0x00041981, 0x00041981, 0x00041982},
 	{0x0000a2d8, 0x7999a83b, 0x7999a83b, 0x7999a83b, 0x7999a83b},
 	{0x0000a358, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a3a4, 0x00000010, 0x00000010, 0x00000000, 0x00000000},
+	{0x0000a3a4, 0x00000050, 0x00000050, 0x00000000, 0x00000000},
 	{0x0000a3a8, 0xaaaaaaaa, 0xaaaaaaaa, 0xaaaaaaaa, 0xaaaaaaaa},
-	{0x0000a3ac, 0xaaaaaa00, 0xaaaaaa30, 0xaaaaaa00, 0xaaaaaa00},
+	{0x0000a3ac, 0xaaaaaa00, 0xaa30aa30, 0xaaaaaa00, 0xaaaaaa00},
 	{0x0000a41c, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce},
 	{0x0000a420, 0x000001ce, 0x000001ce, 0x000001ce, 0x000001ce},
 	{0x0000a424, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce},
@@ -363,14 +363,14 @@ static const u32 ar9462_pciephy_clkreq_disable_L1_2p0[][2] = {
 	/* Addr      allmodes  */
 	{0x00018c00, 0x18213ede},
 	{0x00018c04, 0x000801d8},
-	{0x00018c08, 0x0003580c},
+	{0x00018c08, 0x0003780c},
 };
 
 static const u32 ar9462_pciephy_pll_on_clkreq_disable_L1_2p0[][2] = {
 	/* Addr      allmodes  */
 	{0x00018c00, 0x18212ede},
 	{0x00018c04, 0x000801d8},
-	{0x00018c08, 0x0003580c},
+	{0x00018c08, 0x0003780c},
 };
 
 static const u32 ar9462_2p0_radio_postamble_sys2ant[][5] = {
@@ -775,7 +775,7 @@ static const u32 ar9462_2p0_baseband_core[][2] = {
 	{0x00009fc0, 0x803e4788},
 	{0x00009fc4, 0x0001efb5},
 	{0x00009fcc, 0x40000014},
-	{0x00009fd0, 0x01193b93},
+	{0x00009fd0, 0x0a193b93},
 	{0x0000a20c, 0x00000000},
 	{0x0000a220, 0x00000000},
 	{0x0000a224, 0x00000000},
@@ -850,7 +850,7 @@ static const u32 ar9462_2p0_baseband_core[][2] = {
 	{0x0000a7cc, 0x00000000},
 	{0x0000a7d0, 0x00000000},
 	{0x0000a7d4, 0x00000004},
-	{0x0000a7dc, 0x00000001},
+	{0x0000a7dc, 0x00000000},
 	{0x0000a7f0, 0x80000000},
 	{0x0000a8d0, 0x004b6a8e},
 	{0x0000a8d4, 0x00000820},
@@ -886,7 +886,7 @@ static const u32 ar9462_modes_high_ob_db_tx_gain_table_2p0[][5] = {
 	{0x0000a2e0, 0x0000f000, 0x0000f000, 0x03ccc584, 0x03ccc584},
 	{0x0000a2e4, 0x01ff0000, 0x01ff0000, 0x03f0f800, 0x03f0f800},
 	{0x0000a2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
-	{0x0000a410, 0x000050d9, 0x000050d9, 0x000050d9, 0x000050d9},
+	{0x0000a410, 0x000050da, 0x000050da, 0x000050de, 0x000050de},
 	{0x0000a458, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
 	{0x0000a500, 0x00002220, 0x00002220, 0x00000000, 0x00000000},
 	{0x0000a504, 0x06002223, 0x06002223, 0x04000002, 0x04000002},
@@ -906,20 +906,20 @@ static const u32 ar9462_modes_high_ob_db_tx_gain_table_2p0[][5] = {
 	{0x0000a53c, 0x41025e4a, 0x41025e4a, 0x34001640, 0x34001640},
 	{0x0000a540, 0x48025e6c, 0x48025e6c, 0x38001660, 0x38001660},
 	{0x0000a544, 0x4e025e8e, 0x4e025e8e, 0x3b001861, 0x3b001861},
-	{0x0000a548, 0x53025eb2, 0x53025eb2, 0x3e001a81, 0x3e001a81},
-	{0x0000a54c, 0x59025eb6, 0x59025eb6, 0x42001a83, 0x42001a83},
-	{0x0000a550, 0x5d025ef6, 0x5d025ef6, 0x44001c84, 0x44001c84},
+	{0x0000a548, 0x55025eb3, 0x55025eb3, 0x3e001a81, 0x3e001a81},
+	{0x0000a54c, 0x58025ef3, 0x58025ef3, 0x42001a83, 0x42001a83},
+	{0x0000a550, 0x5d025ef6, 0x5d025ef6, 0x44001a84, 0x44001a84},
 	{0x0000a554, 0x62025f56, 0x62025f56, 0x48001ce3, 0x48001ce3},
 	{0x0000a558, 0x66027f56, 0x66027f56, 0x4c001ce5, 0x4c001ce5},
 	{0x0000a55c, 0x6a029f56, 0x6a029f56, 0x50001ce9, 0x50001ce9},
 	{0x0000a560, 0x70049f56, 0x70049f56, 0x54001ceb, 0x54001ceb},
-	{0x0000a564, 0x7504ff56, 0x7504ff56, 0x56001eec, 0x56001eec},
-	{0x0000a568, 0x7504ff56, 0x7504ff56, 0x56001eec, 0x56001eec},
-	{0x0000a56c, 0x7504ff56, 0x7504ff56, 0x56001eec, 0x56001eec},
-	{0x0000a570, 0x7504ff56, 0x7504ff56, 0x56001eec, 0x56001eec},
-	{0x0000a574, 0x7504ff56, 0x7504ff56, 0x56001eec, 0x56001eec},
-	{0x0000a578, 0x7504ff56, 0x7504ff56, 0x56001eec, 0x56001eec},
-	{0x0000a57c, 0x7504ff56, 0x7504ff56, 0x56001eec, 0x56001eec},
+	{0x0000a564, 0x751ffff6, 0x751ffff6, 0x56001eec, 0x56001eec},
+	{0x0000a568, 0x751ffff6, 0x751ffff6, 0x58001ef0, 0x58001ef0},
+	{0x0000a56c, 0x751ffff6, 0x751ffff6, 0x5a001ef4, 0x5a001ef4},
+	{0x0000a570, 0x751ffff6, 0x751ffff6, 0x5c001ff6, 0x5c001ff6},
+	{0x0000a574, 0x751ffff6, 0x751ffff6, 0x5c001ff6, 0x5c001ff6},
+	{0x0000a578, 0x751ffff6, 0x751ffff6, 0x5c001ff6, 0x5c001ff6},
+	{0x0000a57c, 0x751ffff6, 0x751ffff6, 0x5c001ff6, 0x5c001ff6},
 	{0x0000a600, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
 	{0x0000a604, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
 	{0x0000a608, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
@@ -1053,7 +1053,6 @@ static const u32 ar9462_2p0_mac_core[][2] = {
 	{0x00008044, 0x00000000},
 	{0x00008048, 0x00000000},
 	{0x0000804c, 0xffffffff},
-	{0x00008050, 0xffffffff},
 	{0x00008054, 0x00000000},
 	{0x00008058, 0x00000000},
 	{0x0000805c, 0x000fc78f},
@@ -1117,9 +1116,9 @@ static const u32 ar9462_2p0_mac_core[][2] = {
 	{0x000081f8, 0x00000000},
 	{0x000081fc, 0x00000000},
 	{0x00008240, 0x00100000},
-	{0x00008244, 0x0010f424},
+	{0x00008244, 0x0010f400},
 	{0x00008248, 0x00000800},
-	{0x0000824c, 0x0001e848},
+	{0x0000824c, 0x0001e800},
 	{0x00008250, 0x00000000},
 	{0x00008254, 0x00000000},
 	{0x00008258, 0x00000000},
diff --git a/drivers/net/wireless/ath/ath9k/ath9k.h b/drivers/net/wireless/ath/ath9k/ath9k.h
index a56b2416e2f9..8a1888d02070 100644
--- a/drivers/net/wireless/ath/ath9k/ath9k.h
+++ b/drivers/net/wireless/ath/ath9k/ath9k.h
@@ -234,6 +234,7 @@ struct ath_buf {
 	dma_addr_t bf_daddr;		/* physical addr of desc */
 	dma_addr_t bf_buf_addr;	/* physical addr of data buffer, for DMA */
 	bool bf_stale;
+	struct ieee80211_tx_rate rates[4];
 	struct ath_buf_state bf_state;
 };
 
@@ -311,6 +312,7 @@ struct ath_rx_edma {
 struct ath_rx {
 	u8 defant;
 	u8 rxotherant;
+	bool discard_next;
 	u32 *rxlink;
 	u32 num_pkts;
 	unsigned int rxfilter;
@@ -657,11 +659,10 @@ enum sc_op_flags {
 struct ath_rate_table;
 
 struct ath9k_vif_iter_data {
-	const u8 *hw_macaddr; /* phy's hardware address, set
-			       * before starting iteration for
-			       * valid bssid mask.
-			       */
+	u8 hw_macaddr[ETH_ALEN]; /* address of the first vif */
 	u8 mask[ETH_ALEN]; /* bssid mask */
+	bool has_hw_macaddr;
+
 	int naps;      /* number of AP vifs */
 	int nmeshes;   /* number of mesh vifs */
 	int nstations; /* number of station vifs */
diff --git a/drivers/net/wireless/ath/ath9k/beacon.c b/drivers/net/wireless/ath/ath9k/beacon.c
index 5f05c26d1ec4..2ff570f7f8ff 100644
--- a/drivers/net/wireless/ath/ath9k/beacon.c
+++ b/drivers/net/wireless/ath/ath9k/beacon.c
@@ -79,7 +79,7 @@ static void ath9k_beacon_setup(struct ath_softc *sc, struct ieee80211_vif *vif,
 	u8 chainmask = ah->txchainmask;
 	u8 rate = 0;
 
-	sband = &sc->sbands[common->hw->conf.channel->band];
+	sband = &sc->sbands[common->hw->conf.chandef.chan->band];
 	rate = sband->bitrates[rateidx].hw_value;
 	if (vif->bss_conf.use_short_preamble)
 		rate |= sband->bitrates[rateidx].hw_value_short;
diff --git a/drivers/net/wireless/ath/ath9k/calib.c b/drivers/net/wireless/ath/ath9k/calib.c
index 1e8508530e98..7304e7585009 100644
--- a/drivers/net/wireless/ath/ath9k/calib.c
+++ b/drivers/net/wireless/ath/ath9k/calib.c
@@ -208,7 +208,7 @@ bool ath9k_hw_reset_calvalid(struct ath_hw *ah)
 		return true;
 
 	ath_dbg(common, CALIBRATE, "Resetting Cal %d state for channel %u\n",
-		currCal->calData->calType, conf->channel->center_freq);
+		currCal->calData->calType, conf->chandef.chan->center_freq);
 
 	ah->caldata->CalValid &= ~currCal->calData->calType;
 	currCal->calState = CAL_WAITING;
@@ -369,7 +369,6 @@ bool ath9k_hw_getnf(struct ath_hw *ah, struct ath9k_channel *chan)
 	struct ieee80211_channel *c = chan->chan;
 	struct ath9k_hw_cal_data *caldata = ah->caldata;
 
-	chan->channelFlags &= (~CHANNEL_CW_INT);
 	if (REG_READ(ah, AR_PHY_AGC_CONTROL) & AR_PHY_AGC_CONTROL_NF) {
 		ath_dbg(common, CALIBRATE,
 			"NF did not complete in calibration window\n");
@@ -384,7 +383,6 @@ bool ath9k_hw_getnf(struct ath_hw *ah, struct ath9k_channel *chan)
 		ath_dbg(common, CALIBRATE,
 			"noise floor failed detected; detected %d, threshold %d\n",
 			nf, nfThresh);
-		chan->channelFlags |= CHANNEL_CW_INT;
 	}
 
 	if (!caldata) {
@@ -410,7 +408,7 @@ void ath9k_init_nfcal_hist_buffer(struct ath_hw *ah,
 	int i, j;
 
 	ah->caldata->channel = chan->channel;
-	ah->caldata->channelFlags = chan->channelFlags & ~CHANNEL_CW_INT;
+	ah->caldata->channelFlags = chan->channelFlags;
 	ah->caldata->chanmode = chan->chanmode;
 	h = ah->caldata->nfCalHist;
 	default_nf = ath9k_hw_get_default_nf(ah, chan);
diff --git a/drivers/net/wireless/ath/ath9k/calib.h b/drivers/net/wireless/ath/ath9k/calib.h
index 60dcb6c22db9..3d70b8c2bcdd 100644
--- a/drivers/net/wireless/ath/ath9k/calib.h
+++ b/drivers/net/wireless/ath/ath9k/calib.h
@@ -33,6 +33,12 @@ struct ar5416IniArray {
 	u32 ia_columns;
 };
 
+#define STATIC_INI_ARRAY(array) {			\
+		.ia_array = (u32 *)(array),		\
+		.ia_rows = ARRAY_SIZE(array),		\
+		.ia_columns = ARRAY_SIZE(array[0]),	\
+	}
+
 #define INIT_INI_ARRAY(iniarray, array) do {	\
 		(iniarray)->ia_array = (u32 *)(array);		\
 		(iniarray)->ia_rows = ARRAY_SIZE(array);	\
diff --git a/drivers/net/wireless/ath/ath9k/common.c b/drivers/net/wireless/ath/ath9k/common.c
index 905f1b313961..344fdde1d7a3 100644
--- a/drivers/net/wireless/ath/ath9k/common.c
+++ b/drivers/net/wireless/ath/ath9k/common.c
@@ -27,20 +27,6 @@ MODULE_AUTHOR("Atheros Communications");
 MODULE_DESCRIPTION("Shared library for Atheros wireless 802.11n LAN cards.");
 MODULE_LICENSE("Dual BSD/GPL");
 
-int ath9k_cmn_padpos(__le16 frame_control)
-{
-	int padpos = 24;
-	if (ieee80211_has_a4(frame_control)) {
-		padpos += ETH_ALEN;
-	}
-	if (ieee80211_is_data_qos(frame_control)) {
-		padpos += IEEE80211_QOS_CTL_LEN;
-	}
-
-	return padpos;
-}
-EXPORT_SYMBOL(ath9k_cmn_padpos);
-
 int ath9k_cmn_get_hw_crypto_keytype(struct sk_buff *skb)
 {
 	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
@@ -133,13 +119,14 @@ EXPORT_SYMBOL(ath9k_cmn_update_ichannel);
 struct ath9k_channel *ath9k_cmn_get_curchannel(struct ieee80211_hw *hw,
 					       struct ath_hw *ah)
 {
-	struct ieee80211_channel *curchan = hw->conf.channel;
+	struct ieee80211_channel *curchan = hw->conf.chandef.chan;
 	struct ath9k_channel *channel;
 	u8 chan_idx;
 
 	chan_idx = curchan->hw_value;
 	channel = &ah->channels[chan_idx];
-	ath9k_cmn_update_ichannel(channel, curchan, hw->conf.channel_type);
+	ath9k_cmn_update_ichannel(channel, curchan,
+				  cfg80211_get_chandef_type(&hw->conf.chandef));
 
 	return channel;
 }
diff --git a/drivers/net/wireless/ath/ath9k/common.h b/drivers/net/wireless/ath/ath9k/common.h
index 050ca4a4850d..207d06995b15 100644
--- a/drivers/net/wireless/ath/ath9k/common.h
+++ b/drivers/net/wireless/ath/ath9k/common.h
@@ -40,9 +40,8 @@
 	x = ATH_LPF_RSSI((x), ATH_RSSI_IN((y)), ATH_RSSI_LPF_LEN);  	\
 } while (0)
 #define ATH_EP_RND(x, mul) 						\
-	((((x)%(mul)) >= ((mul)/2)) ? ((x) + ((mul) - 1)) / (mul) : (x)/(mul))
+	(((x) + ((mul)/2)) / (mul))
 
-int ath9k_cmn_padpos(__le16 frame_control);
 int ath9k_cmn_get_hw_crypto_keytype(struct sk_buff *skb);
 void ath9k_cmn_update_ichannel(struct ath9k_channel *ichan,
 			       struct ieee80211_channel *chan,
diff --git a/drivers/net/wireless/ath/ath9k/debug.c b/drivers/net/wireless/ath/ath9k/debug.c
index 3714b971d18e..e6307b86363a 100644
--- a/drivers/net/wireless/ath/ath9k/debug.c
+++ b/drivers/net/wireless/ath/ath9k/debug.c
@@ -537,6 +537,7 @@ static ssize_t read_file_xmit(struct file *file, char __user *user_buf,
 	PR("AMPDUs Completed:", a_completed);
 	PR("AMPDUs Retried:  ", a_retries);
 	PR("AMPDUs XRetried: ", a_xretries);
+	PR("TXERR Filtered:  ", txerr_filtered);
 	PR("FIFO Underrun:   ", fifo_underrun);
 	PR("TXOP Exceeded:   ", xtxop);
 	PR("TXTIMER Expiry:  ", timer_exp);
@@ -756,6 +757,8 @@ void ath_debug_stat_tx(struct ath_softc *sc, struct ath_buf *bf,
 			TX_STAT_INC(qnum, completed);
 	}
 
+	if (ts->ts_status & ATH9K_TXERR_FILT)
+		TX_STAT_INC(qnum, txerr_filtered);
 	if (ts->ts_status & ATH9K_TXERR_FIFO)
 		TX_STAT_INC(qnum, fifo_underrun);
 	if (ts->ts_status & ATH9K_TXERR_XTXOP)
@@ -1909,6 +1912,7 @@ static const char ath9k_gstrings_stats[][ETH_GSTRING_LEN] = {
 	AMKSTR(d_tx_desc_cfg_err),
 	AMKSTR(d_tx_data_underrun),
 	AMKSTR(d_tx_delim_underrun),
+	"d_rx_crc_err",
 	"d_rx_decrypt_crc_err",
 	"d_rx_phy_err",
 	"d_rx_mic_err",
@@ -1989,6 +1993,7 @@ void ath9k_get_et_stats(struct ieee80211_hw *hw,
 	AWDATA(data_underrun);
 	AWDATA(delim_underrun);
 
+	AWDATA_RX(crc_err);
 	AWDATA_RX(decrypt_crc_err);
 	AWDATA_RX(phy_err);
 	AWDATA_RX(mic_err);
@@ -2067,7 +2072,7 @@ int ath9k_init_debug(struct ath_hw *ah)
 			    &fops_modal_eeprom);
 	sc->rfs_chan_spec_scan = relay_open("spectral_scan",
 					    sc->debug.debugfs_phy,
-					    262144, 4, &rfs_spec_scan_cb,
+					    1024, 256, &rfs_spec_scan_cb,
 					    NULL);
 	debugfs_create_file("spectral_scan_ctl", S_IRUSR | S_IWUSR,
 			    sc->debug.debugfs_phy, sc,
diff --git a/drivers/net/wireless/ath/ath9k/debug.h b/drivers/net/wireless/ath/ath9k/debug.h
index 410d6d8f1aa7..794a7ec83a24 100644
--- a/drivers/net/wireless/ath/ath9k/debug.h
+++ b/drivers/net/wireless/ath/ath9k/debug.h
@@ -142,6 +142,7 @@ struct ath_interrupt_stats {
  * @a_completed: Total AMPDUs completed
  * @a_retries: No. of AMPDUs retried (SW)
  * @a_xretries: No. of AMPDUs dropped due to xretries
+ * @txerr_filtered: No. of frames with TXERR_FILT flag set.
  * @fifo_underrun: FIFO underrun occurrences
 	Valid only for:
 		- non-aggregate condition.
@@ -168,6 +169,7 @@ struct ath_tx_stats {
 	u32 a_completed;
 	u32 a_retries;
 	u32 a_xretries;
+	u32 txerr_filtered;
 	u32 fifo_underrun;
 	u32 xtxop;
 	u32 timer_exp;
diff --git a/drivers/net/wireless/ath/ath9k/dfs.c b/drivers/net/wireless/ath/ath9k/dfs.c
index ecc81792f2dc..7187d3671512 100644
--- a/drivers/net/wireless/ath/ath9k/dfs.c
+++ b/drivers/net/wireless/ath/ath9k/dfs.c
@@ -55,12 +55,6 @@ ath9k_postprocess_radar_event(struct ath_softc *sc,
 	u8 rssi;
 	u16 dur;
 
-	ath_dbg(ath9k_hw_common(sc->sc_ah), DFS,
-		"pulse_bw_info=0x%x, pri,ext len/rssi=(%u/%u, %u/%u)\n",
-		ard->pulse_bw_info,
-		ard->pulse_length_pri, ard->rssi,
-		ard->pulse_length_ext, ard->ext_rssi);
-
 	/*
 	 * Only the last 2 bits of the BW info are relevant, they indicate
 	 * which channel the radar was detected in.
@@ -193,9 +187,7 @@ void ath9k_dfs_process_phyerr(struct ath_softc *sc, void *data,
 		DFS_STAT_INC(sc, pulses_processed);
 		if (pd != NULL && pd->add_pulse(pd, &pe)) {
 			DFS_STAT_INC(sc, radar_detected);
-			/*
-			 * TODO: forward radar event to DFS management layer
-			 */
+			ieee80211_radar_detected(sc->hw);
 		}
 	}
 }
diff --git a/drivers/net/wireless/ath/ath9k/dfs_debug.c b/drivers/net/wireless/ath/ath9k/dfs_debug.c
index 55d28072adeb..b7611b7bbe43 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_debug.c
+++ b/drivers/net/wireless/ath/ath9k/dfs_debug.c
@@ -105,6 +105,24 @@ static ssize_t write_file_dfs(struct file *file, const char __user *user_buf,
 	return count;
 }
 
+static ssize_t write_file_simulate_radar(struct file *file,
+					 const char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct ath_softc *sc = file->private_data;
+
+	ieee80211_radar_detected(sc->hw);
+
+	return count;
+}
+
+static const struct file_operations fops_simulate_radar = {
+	.write = write_file_simulate_radar,
+	.open = simple_open,
+	.owner = THIS_MODULE,
+	.llseek = default_llseek,
+};
+
 static const struct file_operations fops_dfs_stats = {
 	.read = read_file_dfs,
 	.write = write_file_dfs,
@@ -117,4 +135,6 @@ void ath9k_dfs_init_debug(struct ath_softc *sc)
 {
 	debugfs_create_file("dfs_stats", S_IRUSR,
 			    sc->debug.debugfs_phy, sc, &fops_dfs_stats);
+	debugfs_create_file("dfs_simulate_radar", S_IWUSR,
+			    sc->debug.debugfs_phy, sc, &fops_simulate_radar);
 }
diff --git a/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.c b/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.c
index 73fe8d6db566..491305c81fce 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.c
+++ b/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.c
@@ -19,6 +19,7 @@
 
 #include "dfs_pattern_detector.h"
 #include "dfs_pri_detector.h"
+#include "ath9k.h"
 
 /*
  * tolerated deviation of radar time stamp in usecs on both sides
@@ -142,6 +143,7 @@ channel_detector_create(struct dfs_pattern_detector *dpd, u16 freq)
 {
 	u32 sz, i;
 	struct channel_detector *cd;
+	struct ath_common *common = ath9k_hw_common(dpd->ah);
 
 	cd = kmalloc(sizeof(*cd), GFP_ATOMIC);
 	if (cd == NULL)
@@ -165,7 +167,8 @@ channel_detector_create(struct dfs_pattern_detector *dpd, u16 freq)
 	return cd;
 
 fail:
-	pr_err("failed to allocate channel_detector for freq=%d\n", freq);
+	ath_dbg(common, DFS,
+		"failed to allocate channel_detector for freq=%d\n", freq);
 	channel_detector_exit(dpd, cd);
 	return NULL;
 }
@@ -216,34 +219,34 @@ static bool
 dpd_add_pulse(struct dfs_pattern_detector *dpd, struct pulse_event *event)
 {
 	u32 i;
-	bool ts_wraparound;
 	struct channel_detector *cd;
 
-	if (dpd->region == NL80211_DFS_UNSET) {
-		/*
-		 * pulses received for a non-supported or un-initialized
-		 * domain are treated as detected radars
-		 */
+	/*
+	 * pulses received for a non-supported or un-initialized
+	 * domain are treated as detected radars for fail-safety
+	 */
+	if (dpd->region == NL80211_DFS_UNSET)
 		return true;
-	}
 
 	cd = channel_detector_get(dpd, event->freq);
 	if (cd == NULL)
 		return false;
 
-	ts_wraparound = (event->ts < dpd->last_pulse_ts);
 	dpd->last_pulse_ts = event->ts;
-	if (ts_wraparound) {
-		/*
-		 * reset detector on time stamp wraparound
-		 * with monotonic time stamps, this should never happen
-		 */
-		pr_warn("DFS: time stamp wraparound detected, resetting\n");
+	/* reset detector on time stamp wraparound, caused by TSF reset */
+	if (event->ts < dpd->last_pulse_ts)
 		dpd_reset(dpd);
-	}
+
 	/* do type individual pattern matching */
 	for (i = 0; i < dpd->num_radar_types; i++) {
-		if (cd->detectors[i]->add_pulse(cd->detectors[i], event) != 0) {
+		struct pri_detector *pd = cd->detectors[i];
+		struct pri_sequence *ps = pd->add_pulse(pd, event);
+		if (ps != NULL) {
+			ath_dbg(ath9k_hw_common(dpd->ah), DFS,
+				"DFS: radar found on freq=%d: id=%d, pri=%d, "
+				"count=%d, count_false=%d\n",
+				event->freq, pd->rs->type_id,
+				ps->pri, ps->count, ps->count_falses);
 			channel_detector_reset(dpd, cd);
 			return true;
 		}
@@ -285,9 +288,10 @@ static struct dfs_pattern_detector default_dpd = {
 };
 
 struct dfs_pattern_detector *
-dfs_pattern_detector_init(enum nl80211_dfs_regions region)
+dfs_pattern_detector_init(struct ath_hw *ah, enum nl80211_dfs_regions region)
 {
 	struct dfs_pattern_detector *dpd;
+	struct ath_common *common = ath9k_hw_common(ah);
 
 	dpd = kmalloc(sizeof(*dpd), GFP_KERNEL);
 	if (dpd == NULL)
@@ -296,10 +300,11 @@ dfs_pattern_detector_init(enum nl80211_dfs_regions region)
 	*dpd = default_dpd;
 	INIT_LIST_HEAD(&dpd->channel_detectors);
 
+	dpd->ah = ah;
 	if (dpd->set_dfs_domain(dpd, region))
 		return dpd;
 
-	pr_err("Could not set DFS domain to %d. ", region);
+	ath_dbg(common, DFS,"Could not set DFS domain to %d", region);
 	kfree(dpd);
 	return NULL;
 }
diff --git a/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.h b/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.h
index cda52f39f28a..90a5abcc4265 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.h
+++ b/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.h
@@ -80,6 +80,8 @@ struct dfs_pattern_detector {
 	enum nl80211_dfs_regions region;
 	u8 num_radar_types;
 	u64 last_pulse_ts;
+	/* needed for ath_dbg() */
+	struct ath_hw *ah;
 
 	const struct radar_detector_specs *radar_spec;
 	struct list_head channel_detectors;
@@ -92,10 +94,10 @@ struct dfs_pattern_detector {
  */
 #if defined(CONFIG_ATH9K_DFS_CERTIFIED)
 extern struct dfs_pattern_detector *
-dfs_pattern_detector_init(enum nl80211_dfs_regions region);
+dfs_pattern_detector_init(struct ath_hw *ah, enum nl80211_dfs_regions region);
 #else
 static inline struct dfs_pattern_detector *
-dfs_pattern_detector_init(enum nl80211_dfs_regions region)
+dfs_pattern_detector_init(struct ath_hw *ah, enum nl80211_dfs_regions region)
 {
 	return NULL;
 }
diff --git a/drivers/net/wireless/ath/ath9k/dfs_pri_detector.c b/drivers/net/wireless/ath/ath9k/dfs_pri_detector.c
index 5e48c5515b8c..5ba4b6fe37c0 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_pri_detector.c
+++ b/drivers/net/wireless/ath/ath9k/dfs_pri_detector.c
@@ -23,28 +23,6 @@
 #include "dfs_debug.h"
 
 /**
- * struct pri_sequence - sequence of pulses matching one PRI
- * @head: list_head
- * @pri: pulse repetition interval (PRI) in usecs
- * @dur: duration of sequence in usecs
- * @count: number of pulses in this sequence
- * @count_falses: number of not matching pulses in this sequence
- * @first_ts: time stamp of first pulse in usecs
- * @last_ts: time stamp of last pulse in usecs
- * @deadline_ts: deadline when this sequence becomes invalid (first_ts + dur)
- */
-struct pri_sequence {
-	struct list_head head;
-	u32 pri;
-	u32 dur;
-	u32 count;
-	u32 count_falses;
-	u64 first_ts;
-	u64 last_ts;
-	u64 deadline_ts;
-};
-
-/**
  * struct pulse_elem - elements in pulse queue
  * @ts: time stamp in usecs
  */
@@ -393,8 +371,8 @@ static void pri_detector_exit(struct pri_detector *de)
 	kfree(de);
 }
 
-static bool pri_detector_add_pulse(struct pri_detector *de,
-				   struct pulse_event *event)
+static struct pri_sequence *pri_detector_add_pulse(struct pri_detector *de,
+						   struct pulse_event *event)
 {
 	u32 max_updated_seq;
 	struct pri_sequence *ps;
@@ -403,38 +381,33 @@ static bool pri_detector_add_pulse(struct pri_detector *de,
 
 	/* ignore pulses not within width range */
 	if ((rs->width_min > event->width) || (rs->width_max < event->width))
-		return false;
+		return NULL;
 
 	if ((ts - de->last_ts) < rs->max_pri_tolerance)
 		/* if delta to last pulse is too short, don't use this pulse */
-		return false;
+		return NULL;
 	de->last_ts = ts;
 
 	max_updated_seq = pseq_handler_add_to_existing_seqs(de, ts);
 
 	if (!pseq_handler_create_sequences(de, ts, max_updated_seq)) {
-		pr_err("failed to create pulse sequences\n");
 		pri_detector_reset(de, ts);
 		return false;
 	}
 
 	ps = pseq_handler_check_detection(de);
 
-	if (ps != NULL) {
-		pr_info("DFS: radar found: pri=%d, count=%d, count_false=%d\n",
-			 ps->pri, ps->count, ps->count_falses);
-		pri_detector_reset(de, ts);
-		return true;
-	}
-	pulse_queue_enqueue(de, ts);
-	return false;
+	if (ps == NULL)
+		pulse_queue_enqueue(de, ts);
+
+	return ps;
 }
 
-struct pri_detector *
-pri_detector_init(const struct radar_detector_specs *rs)
+struct pri_detector *pri_detector_init(const struct radar_detector_specs *rs)
 {
 	struct pri_detector *de;
-	de = kzalloc(sizeof(*de), GFP_KERNEL);
+
+	de = kzalloc(sizeof(*de), GFP_ATOMIC);
 	if (de == NULL)
 		return NULL;
 	de->exit = pri_detector_exit;
diff --git a/drivers/net/wireless/ath/ath9k/dfs_pri_detector.h b/drivers/net/wireless/ath/ath9k/dfs_pri_detector.h
index 81cde9f28e44..723962d1abc6 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_pri_detector.h
+++ b/drivers/net/wireless/ath/ath9k/dfs_pri_detector.h
@@ -20,9 +20,31 @@
 #include <linux/list.h>
 
 /**
+ * struct pri_sequence - sequence of pulses matching one PRI
+ * @head: list_head
+ * @pri: pulse repetition interval (PRI) in usecs
+ * @dur: duration of sequence in usecs
+ * @count: number of pulses in this sequence
+ * @count_falses: number of not matching pulses in this sequence
+ * @first_ts: time stamp of first pulse in usecs
+ * @last_ts: time stamp of last pulse in usecs
+ * @deadline_ts: deadline when this sequence becomes invalid (first_ts + dur)
+ */
+struct pri_sequence {
+	struct list_head head;
+	u32 pri;
+	u32 dur;
+	u32 count;
+	u32 count_falses;
+	u64 first_ts;
+	u64 last_ts;
+	u64 deadline_ts;
+};
+
+/**
  * struct pri_detector - PRI detector element for a dedicated radar type
  * @exit(): destructor
- * @add_pulse(): add pulse event, returns true if pattern was detected
+ * @add_pulse(): add pulse event, returns pri_sequence if pattern was detected
  * @reset(): clear states and reset to given time stamp
  * @rs: detector specs for this detector element
  * @last_ts: last pulse time stamp considered for this element in usecs
@@ -34,7 +56,8 @@
  */
 struct pri_detector {
 	void (*exit)     (struct pri_detector *de);
-	bool (*add_pulse)(struct pri_detector *de, struct pulse_event *e);
+	struct pri_sequence *
+	     (*add_pulse)(struct pri_detector *de, struct pulse_event *e);
 	void (*reset)    (struct pri_detector *de, u64 ts);
 
 /* private: internal use only */
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c b/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
index d0ce1f5bba10..f13f458dd656 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
@@ -308,7 +308,7 @@ static void ath9k_htc_send_buffered(struct ath9k_htc_priv *priv,
 	while(skb) {
 		hdr = (struct ieee80211_hdr *) skb->data;
 
-		padpos = ath9k_cmn_padpos(hdr->frame_control);
+		padpos = ieee80211_hdrlen(hdr->frame_control);
 		padsize = padpos & 3;
 		if (padsize && skb->len > padpos) {
 			if (skb_headroom(skb) < padsize) {
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_main.c b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
index a8016d70088a..0743a47cef8f 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_main.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
@@ -190,7 +190,7 @@ void ath9k_htc_reset(struct ath9k_htc_priv *priv)
 {
 	struct ath_hw *ah = priv->ah;
 	struct ath_common *common = ath9k_hw_common(ah);
-	struct ieee80211_channel *channel = priv->hw->conf.channel;
+	struct ieee80211_channel *channel = priv->hw->conf.chandef.chan;
 	struct ath9k_hw_cal_data *caldata = NULL;
 	enum htc_phymode mode;
 	__be16 htc_mode;
@@ -250,7 +250,7 @@ static int ath9k_htc_set_channel(struct ath9k_htc_priv *priv,
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ieee80211_conf *conf = &common->hw->conf;
 	bool fastcc;
-	struct ieee80211_channel *channel = hw->conf.channel;
+	struct ieee80211_channel *channel = hw->conf.chandef.chan;
 	struct ath9k_hw_cal_data *caldata = NULL;
 	enum htc_phymode mode;
 	__be16 htc_mode;
@@ -602,7 +602,7 @@ static void ath9k_htc_setup_rate(struct ath9k_htc_priv *priv,
 	u32 caps = 0;
 	int i, j;
 
-	sband = priv->hw->wiphy->bands[priv->hw->conf.channel->band];
+	sband = priv->hw->wiphy->bands[priv->hw->conf.chandef.chan->band];
 
 	for (i = 0, j = 0; i < sband->n_bitrates; i++) {
 		if (sta->supp_rates[sband->band] & BIT(i)) {
@@ -866,7 +866,7 @@ static void ath9k_htc_tx(struct ieee80211_hw *hw,
 	hdr = (struct ieee80211_hdr *) skb->data;
 
 	/* Add the padding after the header if this is not already done */
-	padpos = ath9k_cmn_padpos(hdr->frame_control);
+	padpos = ieee80211_hdrlen(hdr->frame_control);
 	padsize = padpos & 3;
 	if (padsize && skb->len > padpos) {
 		if (skb_headroom(skb) < padsize) {
@@ -904,7 +904,7 @@ static int ath9k_htc_start(struct ieee80211_hw *hw)
 	struct ath9k_htc_priv *priv = hw->priv;
 	struct ath_hw *ah = priv->ah;
 	struct ath_common *common = ath9k_hw_common(ah);
-	struct ieee80211_channel *curchan = hw->conf.channel;
+	struct ieee80211_channel *curchan = hw->conf.chandef.chan;
 	struct ath9k_channel *init_channel;
 	int ret = 0;
 	enum htc_phymode mode;
@@ -1193,15 +1193,17 @@ static int ath9k_htc_config(struct ieee80211_hw *hw, u32 changed)
 	}
 
 	if ((changed & IEEE80211_CONF_CHANGE_CHANNEL) || chip_reset) {
-		struct ieee80211_channel *curchan = hw->conf.channel;
+		struct ieee80211_channel *curchan = hw->conf.chandef.chan;
+		enum nl80211_channel_type channel_type =
+			cfg80211_get_chandef_type(&hw->conf.chandef);
 		int pos = curchan->hw_value;
 
 		ath_dbg(common, CONFIG, "Set channel: %d MHz\n",
 			curchan->center_freq);
 
 		ath9k_cmn_update_ichannel(&priv->ah->channels[pos],
-					  hw->conf.channel,
-					  hw->conf.channel_type);
+					  hw->conf.chandef.chan,
+					  channel_type);
 
 		if (ath9k_htc_set_channel(priv, hw, &priv->ah->channels[pos]) < 0) {
 			ath_err(common, "Unable to set channel\n");
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c b/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
index bd8251c1c749..6bd0e92ea2aa 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
@@ -490,7 +490,7 @@ static void ath9k_htc_tx_process(struct ath9k_htc_priv *priv,
 		if (txs->ts_flags & ATH9K_HTC_TXSTAT_SGI)
 			rate->flags |= IEEE80211_TX_RC_SHORT_GI;
 	} else {
-		if (cur_conf->channel->band == IEEE80211_BAND_5GHZ)
+		if (cur_conf->chandef.chan->band == IEEE80211_BAND_5GHZ)
 			rate->idx += 4; /* No CCK rates */
 	}
 
@@ -939,7 +939,7 @@ static void ath9k_process_rate(struct ieee80211_hw *hw,
 		return;
 	}
 
-	band = hw->conf.channel->band;
+	band = hw->conf.chandef.chan->band;
 	sband = hw->wiphy->bands[band];
 
 	for (i = 0; i < sband->n_bitrates; i++) {
@@ -966,7 +966,7 @@ static bool ath9k_rx_prepare(struct ath9k_htc_priv *priv,
 	struct sk_buff *skb = rxbuf->skb;
 	struct ath_common *common = ath9k_hw_common(priv->ah);
 	struct ath_htc_rx_status *rxstatus;
-	int hdrlen, padpos, padsize;
+	int hdrlen, padsize;
 	int last_rssi = ATH_RSSI_DUMMY_MARKER;
 	__le16 fc;
 
@@ -996,11 +996,9 @@ static bool ath9k_rx_prepare(struct ath9k_htc_priv *priv,
 	fc = hdr->frame_control;
 	hdrlen = ieee80211_get_hdrlen_from_skb(skb);
 
-	padpos = ath9k_cmn_padpos(fc);
-
-	padsize = padpos & 3;
-	if (padsize && skb->len >= padpos+padsize+FCS_LEN) {
-		memmove(skb->data + padsize, skb->data, padpos);
+	padsize = hdrlen & 3;
+	if (padsize && skb->len >= hdrlen+padsize+FCS_LEN) {
+		memmove(skb->data + padsize, skb->data, hdrlen);
 		skb_pull(skb, padsize);
 	}
 
@@ -1082,8 +1080,8 @@ static bool ath9k_rx_prepare(struct ath9k_htc_priv *priv,
 	}
 
 	rx_status->mactime = be64_to_cpu(rxbuf->rxstatus.rs_tstamp);
-	rx_status->band = hw->conf.channel->band;
-	rx_status->freq = hw->conf.channel->center_freq;
+	rx_status->band = hw->conf.chandef.chan->band;
+	rx_status->freq = hw->conf.chandef.chan->center_freq;
 	rx_status->signal =  rxbuf->rxstatus.rs_rssi + ATH_DEFAULT_NOISE_FLOOR;
 	rx_status->antenna = rxbuf->rxstatus.rs_antenna;
 	rx_status->flag |= RX_FLAG_MACTIME_END;
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index 07e25260c31d..7f25da8444fe 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -139,7 +139,7 @@ static void ath9k_hw_set_clockrate(struct ath_hw *ah)
 		clockrate = 117;
 	else if (!ah->curchan) /* should really check for CCK instead */
 		clockrate = ATH9K_CLOCK_RATE_CCK;
-	else if (conf->channel->band == IEEE80211_BAND_2GHZ)
+	else if (conf->chandef.chan->band == IEEE80211_BAND_2GHZ)
 		clockrate = ATH9K_CLOCK_RATE_2GHZ_OFDM;
 	else if (ah->caps.hw_caps & ATH9K_HW_CAP_FASTCLOCK)
 		clockrate = ATH9K_CLOCK_FAST_RATE_5GHZ_OFDM;
@@ -1100,7 +1100,8 @@ void ath9k_hw_init_global_settings(struct ath_hw *ah)
 	}
 
 	/* As defined by IEEE 802.11-2007 17.3.8.6 */
-	acktimeout = slottime + sifstime + 3 * ah->coverage_class + ack_offset;
+	slottime += 3 * ah->coverage_class;
+	acktimeout = slottime + sifstime + ack_offset;
 	ctstimeout = acktimeout;
 
 	/*
@@ -1110,7 +1111,8 @@ void ath9k_hw_init_global_settings(struct ath_hw *ah)
 	 * BA frames in some implementations, but it has been found to fix ACK
 	 * timeout issues in other cases as well.
 	 */
-	if (conf->channel && conf->channel->band == IEEE80211_BAND_2GHZ &&
+	if (conf->chandef.chan &&
+	    conf->chandef.chan->band == IEEE80211_BAND_2GHZ &&
 	    !IS_CHAN_HALF_RATE(chan) && !IS_CHAN_QUARTER_RATE(chan)) {
 		acktimeout += 64 - sifstime - ah->slottime;
 		ctstimeout += 48 - sifstime - ah->slottime;
@@ -1669,6 +1671,103 @@ bool ath9k_hw_check_alive(struct ath_hw *ah)
 }
 EXPORT_SYMBOL(ath9k_hw_check_alive);
 
+static void ath9k_hw_init_mfp(struct ath_hw *ah)
+{
+	/* Setup MFP options for CCMP */
+	if (AR_SREV_9280_20_OR_LATER(ah)) {
+		/* Mask Retry(b11), PwrMgt(b12), MoreData(b13) to 0 in mgmt
+		 * frames when constructing CCMP AAD. */
+		REG_RMW_FIELD(ah, AR_AES_MUTE_MASK1, AR_AES_MUTE_MASK1_FC_MGMT,
+			      0xc7ff);
+		ah->sw_mgmt_crypto = false;
+	} else if (AR_SREV_9160_10_OR_LATER(ah)) {
+		/* Disable hardware crypto for management frames */
+		REG_CLR_BIT(ah, AR_PCU_MISC_MODE2,
+			    AR_PCU_MISC_MODE2_MGMT_CRYPTO_ENABLE);
+		REG_SET_BIT(ah, AR_PCU_MISC_MODE2,
+			    AR_PCU_MISC_MODE2_NO_CRYPTO_FOR_NON_DATA_PKT);
+		ah->sw_mgmt_crypto = true;
+	} else {
+		ah->sw_mgmt_crypto = true;
+	}
+}
+
+static void ath9k_hw_reset_opmode(struct ath_hw *ah,
+				  u32 macStaId1, u32 saveDefAntenna)
+{
+	struct ath_common *common = ath9k_hw_common(ah);
+
+	ENABLE_REGWRITE_BUFFER(ah);
+
+	REG_RMW(ah, AR_STA_ID1, macStaId1
+		  | AR_STA_ID1_RTS_USE_DEF
+		  | (ah->config.ack_6mb ? AR_STA_ID1_ACKCTS_6MB : 0)
+		  | ah->sta_id1_defaults,
+		  ~AR_STA_ID1_SADH_MASK);
+	ath_hw_setbssidmask(common);
+	REG_WRITE(ah, AR_DEF_ANTENNA, saveDefAntenna);
+	ath9k_hw_write_associd(ah);
+	REG_WRITE(ah, AR_ISR, ~0);
+	REG_WRITE(ah, AR_RSSI_THR, INIT_RSSI_THR);
+
+	REGWRITE_BUFFER_FLUSH(ah);
+
+	ath9k_hw_set_operating_mode(ah, ah->opmode);
+}
+
+static void ath9k_hw_init_queues(struct ath_hw *ah)
+{
+	int i;
+
+	ENABLE_REGWRITE_BUFFER(ah);
+
+	for (i = 0; i < AR_NUM_DCU; i++)
+		REG_WRITE(ah, AR_DQCUMASK(i), 1 << i);
+
+	REGWRITE_BUFFER_FLUSH(ah);
+
+	ah->intr_txqs = 0;
+	for (i = 0; i < ATH9K_NUM_TX_QUEUES; i++)
+		ath9k_hw_resettxqueue(ah, i);
+}
+
+/*
+ * For big endian systems turn on swapping for descriptors
+ */
+static void ath9k_hw_init_desc(struct ath_hw *ah)
+{
+	struct ath_common *common = ath9k_hw_common(ah);
+
+	if (AR_SREV_9100(ah)) {
+		u32 mask;
+		mask = REG_READ(ah, AR_CFG);
+		if (mask & (AR_CFG_SWRB | AR_CFG_SWTB | AR_CFG_SWRG)) {
+			ath_dbg(common, RESET, "CFG Byte Swap Set 0x%x\n",
+				mask);
+		} else {
+			mask = INIT_CONFIG_STATUS | AR_CFG_SWRB | AR_CFG_SWTB;
+			REG_WRITE(ah, AR_CFG, mask);
+			ath_dbg(common, RESET, "Setting CFG 0x%x\n",
+				REG_READ(ah, AR_CFG));
+		}
+	} else {
+		if (common->bus_ops->ath_bus_type == ATH_USB) {
+			/* Configure AR9271 target WLAN */
+			if (AR_SREV_9271(ah))
+				REG_WRITE(ah, AR_CFG, AR_CFG_SWRB | AR_CFG_SWTB);
+			else
+				REG_WRITE(ah, AR_CFG, AR_CFG_SWTD | AR_CFG_SWRD);
+		}
+#ifdef __BIG_ENDIAN
+		else if (AR_SREV_9330(ah) || AR_SREV_9340(ah) ||
+			 AR_SREV_9550(ah))
+			REG_RMW(ah, AR_CFG, AR_CFG_SWRB | AR_CFG_SWTB, 0);
+		else
+			REG_WRITE(ah, AR_CFG, AR_CFG_SWTD | AR_CFG_SWRD);
+#endif
+	}
+}
+
 /*
  * Fast channel change:
  * (Change synthesizer based on channel freq without resetting chip)
@@ -1746,7 +1845,7 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 	u32 saveDefAntenna;
 	u32 macStaId1;
 	u64 tsf = 0;
-	int i, r;
+	int r;
 	bool start_mci_reset = false;
 	bool save_fullsleep = ah->chip_fullsleep;
 
@@ -1763,10 +1862,8 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 		ath9k_hw_getnf(ah, ah->curchan);
 
 	ah->caldata = caldata;
-	if (caldata &&
-	    (chan->channel != caldata->channel ||
-	     (chan->channelFlags & ~CHANNEL_CW_INT) !=
-	     (caldata->channelFlags & ~CHANNEL_CW_INT))) {
+	if (caldata && (chan->channel != caldata->channel ||
+			chan->channelFlags != caldata->channelFlags)) {
 		/* Operating channel changed, reset channel calibration data */
 		memset(caldata, 0, sizeof(*caldata));
 		ath9k_init_nfcal_hist_buffer(ah, chan);
@@ -1853,22 +1950,7 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 		ath9k_hw_settsf64(ah, tsf);
 	}
 
-	/* Setup MFP options for CCMP */
-	if (AR_SREV_9280_20_OR_LATER(ah)) {
-		/* Mask Retry(b11), PwrMgt(b12), MoreData(b13) to 0 in mgmt
-		 * frames when constructing CCMP AAD. */
-		REG_RMW_FIELD(ah, AR_AES_MUTE_MASK1, AR_AES_MUTE_MASK1_FC_MGMT,
-			      0xc7ff);
-		ah->sw_mgmt_crypto = false;
-	} else if (AR_SREV_9160_10_OR_LATER(ah)) {
-		/* Disable hardware crypto for management frames */
-		REG_CLR_BIT(ah, AR_PCU_MISC_MODE2,
-			    AR_PCU_MISC_MODE2_MGMT_CRYPTO_ENABLE);
-		REG_SET_BIT(ah, AR_PCU_MISC_MODE2,
-			    AR_PCU_MISC_MODE2_NO_CRYPTO_FOR_NON_DATA_PKT);
-		ah->sw_mgmt_crypto = true;
-	} else
-		ah->sw_mgmt_crypto = true;
+	ath9k_hw_init_mfp(ah);
 
 	if (IS_CHAN_OFDM(chan) || IS_CHAN_HT(chan))
 		ath9k_hw_set_delta_slope(ah, chan);
@@ -1876,24 +1958,7 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 	ath9k_hw_spur_mitigate_freq(ah, chan);
 	ah->eep_ops->set_board_values(ah, chan);
 
-	ENABLE_REGWRITE_BUFFER(ah);
-
-	REG_WRITE(ah, AR_STA_ID0, get_unaligned_le32(common->macaddr));
-	REG_WRITE(ah, AR_STA_ID1, get_unaligned_le16(common->macaddr + 4)
-		  | macStaId1
-		  | AR_STA_ID1_RTS_USE_DEF
-		  | (ah->config.
-		     ack_6mb ? AR_STA_ID1_ACKCTS_6MB : 0)
-		  | ah->sta_id1_defaults);
-	ath_hw_setbssidmask(common);
-	REG_WRITE(ah, AR_DEF_ANTENNA, saveDefAntenna);
-	ath9k_hw_write_associd(ah);
-	REG_WRITE(ah, AR_ISR, ~0);
-	REG_WRITE(ah, AR_RSSI_THR, INIT_RSSI_THR);
-
-	REGWRITE_BUFFER_FLUSH(ah);
-
-	ath9k_hw_set_operating_mode(ah, ah->opmode);
+	ath9k_hw_reset_opmode(ah, macStaId1, saveDefAntenna);
 
 	r = ath9k_hw_rf_set_freq(ah, chan);
 	if (r)
@@ -1901,17 +1966,7 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 
 	ath9k_hw_set_clockrate(ah);
 
-	ENABLE_REGWRITE_BUFFER(ah);
-
-	for (i = 0; i < AR_NUM_DCU; i++)
-		REG_WRITE(ah, AR_DQCUMASK(i), 1 << i);
-
-	REGWRITE_BUFFER_FLUSH(ah);
-
-	ah->intr_txqs = 0;
-	for (i = 0; i < ATH9K_NUM_TX_QUEUES; i++)
-		ath9k_hw_resettxqueue(ah, i);
-
+	ath9k_hw_init_queues(ah);
 	ath9k_hw_init_interrupt_masks(ah, ah->opmode);
 	ath9k_hw_ani_cache_ini_regs(ah);
 	ath9k_hw_init_qos(ah);
@@ -1966,38 +2021,7 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 
 	REGWRITE_BUFFER_FLUSH(ah);
 
-	/*
-	 * For big endian systems turn on swapping for descriptors
-	 */
-	if (AR_SREV_9100(ah)) {
-		u32 mask;
-		mask = REG_READ(ah, AR_CFG);
-		if (mask & (AR_CFG_SWRB | AR_CFG_SWTB | AR_CFG_SWRG)) {
-			ath_dbg(common, RESET, "CFG Byte Swap Set 0x%x\n",
-				mask);
-		} else {
-			mask =
-				INIT_CONFIG_STATUS | AR_CFG_SWRB | AR_CFG_SWTB;
-			REG_WRITE(ah, AR_CFG, mask);
-			ath_dbg(common, RESET, "Setting CFG 0x%x\n",
-				REG_READ(ah, AR_CFG));
-		}
-	} else {
-		if (common->bus_ops->ath_bus_type == ATH_USB) {
-			/* Configure AR9271 target WLAN */
-			if (AR_SREV_9271(ah))
-				REG_WRITE(ah, AR_CFG, AR_CFG_SWRB | AR_CFG_SWTB);
-			else
-				REG_WRITE(ah, AR_CFG, AR_CFG_SWTD | AR_CFG_SWRD);
-		}
-#ifdef __BIG_ENDIAN
-		else if (AR_SREV_9330(ah) || AR_SREV_9340(ah) ||
-			 AR_SREV_9550(ah))
-			REG_RMW(ah, AR_CFG, AR_CFG_SWRB | AR_CFG_SWTB, 0);
-		else
-			REG_WRITE(ah, AR_CFG, AR_CFG_SWTD | AR_CFG_SWRD);
-#endif
-	}
+	ath9k_hw_init_desc(ah);
 
 	if (ath9k_hw_btcoex_is_enabled(ah))
 		ath9k_hw_btcoex_enable(ah);
@@ -2010,7 +2034,6 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 
 	if (AR_SREV_9300_20_OR_LATER(ah)) {
 		ar9003_hw_bb_watchdog_config(ah);
-
 		ar9003_hw_disable_phy_restart(ah);
 	}
 
@@ -2358,8 +2381,11 @@ static bool ath9k_hw_dfs_tested(struct ath_hw *ah)
 {
 
 	switch (ah->hw_version.macVersion) {
+	/* for temporary testing DFS with 9280 */
+	case AR_SREV_VERSION_9280:
 	/* AR9580 will likely be our first target to get testing on */
 	case AR_SREV_VERSION_9580:
+		return true;
 	default:
 		return false;
 	}
diff --git a/drivers/net/wireless/ath/ath9k/hw.h b/drivers/net/wireless/ath/ath9k/hw.h
index 784e81ccb903..ae3034374bc4 100644
--- a/drivers/net/wireless/ath/ath9k/hw.h
+++ b/drivers/net/wireless/ath/ath9k/hw.h
@@ -363,7 +363,6 @@ enum ath9k_int {
 	ATH9K_INT_NOCARD = 0xffffffff
 };
 
-#define CHANNEL_CW_INT    0x00002
 #define CHANNEL_CCK       0x00020
 #define CHANNEL_OFDM      0x00040
 #define CHANNEL_2GHZ      0x00080
@@ -848,14 +847,7 @@ struct ath_hw {
 	struct ath_hw_ops ops;
 
 	/* Used to program the radio on non single-chip devices */
-	u32 *analogBank0Data;
-	u32 *analogBank1Data;
-	u32 *analogBank2Data;
-	u32 *analogBank3Data;
 	u32 *analogBank6Data;
-	u32 *analogBank6TPCData;
-	u32 *analogBank7Data;
-	u32 *bank6Temp;
 
 	int coverage_class;
 	u32 slottime;
@@ -886,14 +878,8 @@ struct ath_hw {
 
 	struct ar5416IniArray iniModes;
 	struct ar5416IniArray iniCommon;
-	struct ar5416IniArray iniBank0;
 	struct ar5416IniArray iniBB_RfGain;
-	struct ar5416IniArray iniBank1;
-	struct ar5416IniArray iniBank2;
-	struct ar5416IniArray iniBank3;
 	struct ar5416IniArray iniBank6;
-	struct ar5416IniArray iniBank6TPC;
-	struct ar5416IniArray iniBank7;
 	struct ar5416IniArray iniAddac;
 	struct ar5416IniArray iniPcieSerdes;
 #ifdef CONFIG_PM_SLEEP
diff --git a/drivers/net/wireless/ath/ath9k/init.c b/drivers/net/wireless/ath/ath9k/init.c
index af932c9444de..0237b2868961 100644
--- a/drivers/net/wireless/ath/ath9k/init.c
+++ b/drivers/net/wireless/ath/ath9k/init.c
@@ -319,6 +319,10 @@ static void ath9k_reg_notifier(struct wiphy *wiphy,
 		ath9k_ps_wakeup(sc);
 		ath9k_hw_set_txpowerlimit(ah, sc->config.txpowlimit, false);
 		sc->curtxpow = ath9k_hw_regulatory(ah)->power_limit;
+		/* synchronize DFS detector if regulatory domain changed */
+		if (sc->dfs_detector != NULL)
+			sc->dfs_detector->set_dfs_domain(sc->dfs_detector,
+							 request->dfs_region);
 		ath9k_ps_restore(sc);
 	}
 }
@@ -573,7 +577,7 @@ static int ath9k_init_softc(u16 devid, struct ath_softc *sc,
 	atomic_set(&ah->intr_ref_cnt, -1);
 	sc->sc_ah = ah;
 
-	sc->dfs_detector = dfs_pattern_detector_init(NL80211_DFS_UNSET);
+	sc->dfs_detector = dfs_pattern_detector_init(ah, NL80211_DFS_UNSET);
 
 	if (!pdata) {
 		ah->ah_flags |= AH_USE_EEPROM;
@@ -727,12 +731,28 @@ static const struct ieee80211_iface_limit if_limits[] = {
 				 BIT(NL80211_IFTYPE_P2P_GO) },
 };
 
-static const struct ieee80211_iface_combination if_comb = {
-	.limits = if_limits,
-	.n_limits = ARRAY_SIZE(if_limits),
-	.max_interfaces = 2048,
-	.num_different_channels = 1,
-	.beacon_int_infra_match = true,
+
+static const struct ieee80211_iface_limit if_dfs_limits[] = {
+	{ .max = 1,	.types = BIT(NL80211_IFTYPE_AP) },
+};
+
+static const struct ieee80211_iface_combination if_comb[] = {
+	{
+		.limits = if_limits,
+		.n_limits = ARRAY_SIZE(if_limits),
+		.max_interfaces = 2048,
+		.num_different_channels = 1,
+		.beacon_int_infra_match = true,
+	},
+	{
+		.limits = if_dfs_limits,
+		.n_limits = ARRAY_SIZE(if_dfs_limits),
+		.max_interfaces = 1,
+		.num_different_channels = 1,
+		.beacon_int_infra_match = true,
+		.radar_detect_widths =	BIT(NL80211_CHAN_NO_HT) |
+					BIT(NL80211_CHAN_HT20),
+	}
 };
 
 void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
@@ -746,7 +766,8 @@ void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
 		IEEE80211_HW_SUPPORTS_PS |
 		IEEE80211_HW_PS_NULLFUNC_STACK |
 		IEEE80211_HW_SPECTRUM_MGMT |
-		IEEE80211_HW_REPORTS_TX_ACK_STATUS;
+		IEEE80211_HW_REPORTS_TX_ACK_STATUS |
+		IEEE80211_HW_SUPPORTS_RC_TABLE;
 
 	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_HT)
 		 hw->flags |= IEEE80211_HW_AMPDU_AGGREGATION;
@@ -763,8 +784,8 @@ void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
 		BIT(NL80211_IFTYPE_ADHOC) |
 		BIT(NL80211_IFTYPE_MESH_POINT);
 
-	hw->wiphy->iface_combinations = &if_comb;
-	hw->wiphy->n_iface_combinations = 1;
+	hw->wiphy->iface_combinations = if_comb;
+	hw->wiphy->n_iface_combinations = ARRAY_SIZE(if_comb);
 
 	if (AR_SREV_5416(sc->sc_ah))
 		hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
diff --git a/drivers/net/wireless/ath/ath9k/link.c b/drivers/net/wireless/ath/ath9k/link.c
index 7fdac6c7b3ea..849259b07370 100644
--- a/drivers/net/wireless/ath/ath9k/link.c
+++ b/drivers/net/wireless/ath/ath9k/link.c
@@ -214,7 +214,7 @@ static bool ath_paprd_send_frame(struct ath_softc *sc, struct sk_buff *skb, int
 	txctl.txq = sc->tx.txq_map[IEEE80211_AC_BE];
 
 	memset(tx_info, 0, sizeof(*tx_info));
-	tx_info->band = hw->conf.channel->band;
+	tx_info->band = hw->conf.chandef.chan->band;
 	tx_info->flags |= IEEE80211_TX_CTL_NO_ACK;
 	tx_info->control.rates[0].idx = 0;
 	tx_info->control.rates[0].count = 1;
diff --git a/drivers/net/wireless/ath/ath9k/mac.c b/drivers/net/wireless/ath/ath9k/mac.c
index 811007ec07a7..498fee04afa0 100644
--- a/drivers/net/wireless/ath/ath9k/mac.c
+++ b/drivers/net/wireless/ath/ath9k/mac.c
@@ -615,6 +615,14 @@ int ath9k_hw_rxprocdesc(struct ath_hw *ah, struct ath_desc *ds,
 			rs->rs_status |= ATH9K_RXERR_DECRYPT;
 		else if (ads.ds_rxstatus8 & AR_MichaelErr)
 			rs->rs_status |= ATH9K_RXERR_MIC;
+	} else {
+		if (ads.ds_rxstatus8 &
+		    (AR_CRCErr | AR_PHYErr | AR_DecryptCRCErr | AR_MichaelErr))
+			rs->rs_status |= ATH9K_RXERR_CORRUPT_DESC;
+
+		/* Only up to MCS16 supported, everything above is invalid */
+		if (rs->rs_rate >= 0x90)
+			rs->rs_status |= ATH9K_RXERR_CORRUPT_DESC;
 	}
 
 	if (ads.ds_rxstatus8 & AR_KeyMiss)
diff --git a/drivers/net/wireless/ath/ath9k/mac.h b/drivers/net/wireless/ath/ath9k/mac.h
index 1ff817061ebc..5865f92998e1 100644
--- a/drivers/net/wireless/ath/ath9k/mac.h
+++ b/drivers/net/wireless/ath/ath9k/mac.h
@@ -183,6 +183,7 @@ struct ath_htc_rx_status {
 #define ATH9K_RXERR_DECRYPT       0x08
 #define ATH9K_RXERR_MIC           0x10
 #define ATH9K_RXERR_KEYMISS       0x20
+#define ATH9K_RXERR_CORRUPT_DESC  0x40
 
 #define ATH9K_RX_MORE             0x01
 #define ATH9K_RX_MORE_AGGR        0x02
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index 988372d218a4..6963862a1872 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -589,7 +589,7 @@ static int ath9k_start(struct ieee80211_hw *hw)
 	struct ath_softc *sc = hw->priv;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
-	struct ieee80211_channel *curchan = hw->conf.channel;
+	struct ieee80211_channel *curchan = hw->conf.chandef.chan;
 	struct ath9k_channel *init_channel;
 	int r;
 
@@ -839,10 +839,14 @@ static void ath9k_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
 	struct ath9k_vif_iter_data *iter_data = data;
 	int i;
 
-	if (iter_data->hw_macaddr)
+	if (iter_data->has_hw_macaddr) {
 		for (i = 0; i < ETH_ALEN; i++)
 			iter_data->mask[i] &=
 				~(iter_data->hw_macaddr[i] ^ mac[i]);
+	} else {
+		memcpy(iter_data->hw_macaddr, mac, ETH_ALEN);
+		iter_data->has_hw_macaddr = true;
+	}
 
 	switch (vif->type) {
 	case NL80211_IFTYPE_AP:
@@ -891,7 +895,6 @@ void ath9k_calculate_iter_data(struct ieee80211_hw *hw,
 	 * together with the BSSID mask when matching addresses.
 	 */
 	memset(iter_data, 0, sizeof(*iter_data));
-	iter_data->hw_macaddr = common->macaddr;
 	memset(&iter_data->mask, 0xff, ETH_ALEN);
 
 	if (vif)
@@ -901,6 +904,8 @@ void ath9k_calculate_iter_data(struct ieee80211_hw *hw,
 	ieee80211_iterate_active_interfaces_atomic(
 		sc->hw, IEEE80211_IFACE_ITER_RESUME_ALL,
 		ath9k_vif_iter, iter_data);
+
+	memcpy(common->macaddr, iter_data->hw_macaddr, ETH_ALEN);
 }
 
 /* Called with sc->mutex held. */
@@ -1188,7 +1193,9 @@ static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
 	}
 
 	if ((changed & IEEE80211_CONF_CHANGE_CHANNEL) || reset_channel) {
-		struct ieee80211_channel *curchan = hw->conf.channel;
+		struct ieee80211_channel *curchan = hw->conf.chandef.chan;
+		enum nl80211_channel_type channel_type =
+			cfg80211_get_chandef_type(&conf->chandef);
 		int pos = curchan->hw_value;
 		int old_pos = -1;
 		unsigned long flags;
@@ -1197,7 +1204,7 @@ static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
 			old_pos = ah->curchan - &ah->channels[0];
 
 		ath_dbg(common, CONFIG, "Set channel: %d MHz type: %d\n",
-			curchan->center_freq, conf->channel_type);
+			curchan->center_freq, channel_type);
 
 		/* update survey stats for the old channel before switching */
 		spin_lock_irqsave(&common->cc_lock, flags);
@@ -1212,7 +1219,7 @@ static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
 			ath9k_hw_getnf(ah, ah->curchan);
 
 		ath9k_cmn_update_ichannel(&sc->sc_ah->channels[pos],
-					  curchan, conf->channel_type);
+					  curchan, channel_type);
 
 		/*
 		 * If the operating channel changes, change the survey in-use flags
@@ -1249,10 +1256,27 @@ static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
 		if (old_pos >= 0)
 			ath_update_survey_nf(sc, old_pos);
 
-		/* perform spectral scan if requested. */
-		if (sc->scanning && sc->spectral_mode == SPECTRAL_CHANSCAN)
-			ath9k_spectral_scan_trigger(hw);
-
+		/*
+		 * Enable radar pulse detection if on a DFS channel. Spectral
+		 * scanning and radar detection can not be used concurrently.
+		 */
+		if (hw->conf.radar_enabled) {
+			u32 rxfilter;
+
+			/* set HW specific DFS configuration */
+			ath9k_hw_set_radar_params(ah);
+			rxfilter = ath9k_hw_getrxfilter(ah);
+			rxfilter |= ATH9K_RX_FILTER_PHYRADAR |
+				    ATH9K_RX_FILTER_PHYERR;
+			ath9k_hw_setrxfilter(ah, rxfilter);
+			ath_dbg(common, DFS, "DFS enabled at freq %d\n",
+				curchan->center_freq);
+		} else {
+			/* perform spectral scan if requested. */
+			if (sc->scanning &&
+			    sc->spectral_mode == SPECTRAL_CHANSCAN)
+				ath9k_spectral_scan_trigger(hw);
+		}
 	}
 
 	if (changed & IEEE80211_CONF_CHANGE_POWER) {
@@ -1749,7 +1773,7 @@ static void ath9k_set_coverage_class(struct ieee80211_hw *hw, u8 coverage_class)
 	mutex_unlock(&sc->mutex);
 }
 
-static void ath9k_flush(struct ieee80211_hw *hw, bool drop)
+static void ath9k_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 {
 	struct ath_softc *sc = hw->priv;
 	struct ath_hw *ah = sc->sc_ah;
diff --git a/drivers/net/wireless/ath/ath9k/rc.c b/drivers/net/wireless/ath/ath9k/rc.c
index 96ac433ba7f6..aa4d368d8d3d 100644
--- a/drivers/net/wireless/ath/ath9k/rc.c
+++ b/drivers/net/wireless/ath/ath9k/rc.c
@@ -814,7 +814,7 @@ static void ath_get_rate(void *priv, struct ieee80211_sta *sta, void *priv_sta,
 	 * So, set fourth rate in series to be same as third one for
 	 * above conditions.
 	 */
-	if ((sc->hw->conf.channel->band == IEEE80211_BAND_2GHZ) &&
+	if ((sc->hw->conf.chandef.chan->band == IEEE80211_BAND_2GHZ) &&
 	    (conf_is_ht(&sc->hw->conf))) {
 		u8 dot11rate = rate_table->info[rix].dot11rate;
 		u8 phy = rate_table->info[rix].phy;
@@ -1328,7 +1328,7 @@ static void ath_rate_update(void *priv, struct ieee80211_supported_band *sband,
 
 		ath_dbg(ath9k_hw_common(sc->sc_ah), CONFIG,
 			"Operating HT Bandwidth changed to: %d\n",
-			sc->hw->conf.channel_type);
+			cfg80211_get_chandef_type(&sc->hw->conf.chandef));
 	}
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/recv.c b/drivers/net/wireless/ath/ath9k/recv.c
index ee156e543147..8be2b5d8c155 100644
--- a/drivers/net/wireless/ath/ath9k/recv.c
+++ b/drivers/net/wireless/ath/ath9k/recv.c
@@ -124,13 +124,13 @@ static bool ath_rx_edma_buf_link(struct ath_softc *sc,
 
 	SKB_CB_ATHBUF(skb) = bf;
 	ath9k_hw_addrxbuf_edma(ah, bf->bf_buf_addr, qtype);
-	skb_queue_tail(&rx_edma->rx_fifo, skb);
+	__skb_queue_tail(&rx_edma->rx_fifo, skb);
 
 	return true;
 }
 
 static void ath_rx_addbuffer_edma(struct ath_softc *sc,
-				  enum ath9k_rx_qtype qtype, int size)
+				  enum ath9k_rx_qtype qtype)
 {
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_buf *bf, *tbf;
@@ -155,7 +155,7 @@ static void ath_rx_remove_buffer(struct ath_softc *sc,
 
 	rx_edma = &sc->rx.rx_edma[qtype];
 
-	while ((skb = skb_dequeue(&rx_edma->rx_fifo)) != NULL) {
+	while ((skb = __skb_dequeue(&rx_edma->rx_fifo)) != NULL) {
 		bf = SKB_CB_ATHBUF(skb);
 		BUG_ON(!bf);
 		list_add_tail(&bf->list, &sc->rx.rxbuf);
@@ -250,15 +250,9 @@ rx_init_fail:
 static void ath_edma_start_recv(struct ath_softc *sc)
 {
 	ath9k_hw_rxena(sc->sc_ah);
-
-	ath_rx_addbuffer_edma(sc, ATH9K_RX_QUEUE_HP,
-			      sc->rx.rx_edma[ATH9K_RX_QUEUE_HP].rx_fifo_hwsize);
-
-	ath_rx_addbuffer_edma(sc, ATH9K_RX_QUEUE_LP,
-			      sc->rx.rx_edma[ATH9K_RX_QUEUE_LP].rx_fifo_hwsize);
-
+	ath_rx_addbuffer_edma(sc, ATH9K_RX_QUEUE_HP);
+	ath_rx_addbuffer_edma(sc, ATH9K_RX_QUEUE_LP);
 	ath_opmode_init(sc);
-
 	ath9k_hw_startpcureceive(sc->sc_ah, !!(sc->hw->conf.flags & IEEE80211_CONF_OFFCHANNEL));
 }
 
@@ -280,49 +274,47 @@ int ath_rx_init(struct ath_softc *sc, int nbufs)
 	common->rx_bufsize = IEEE80211_MAX_MPDU_LEN / 2 +
 			     sc->sc_ah->caps.rx_status_len;
 
-	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_EDMA) {
+	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_EDMA)
 		return ath_rx_edma_init(sc, nbufs);
-	} else {
-		ath_dbg(common, CONFIG, "cachelsz %u rxbufsize %u\n",
-			common->cachelsz, common->rx_bufsize);
 
-		/* Initialize rx descriptors */
+	ath_dbg(common, CONFIG, "cachelsz %u rxbufsize %u\n",
+		common->cachelsz, common->rx_bufsize);
 
-		error = ath_descdma_setup(sc, &sc->rx.rxdma, &sc->rx.rxbuf,
-				"rx", nbufs, 1, 0);
-		if (error != 0) {
-			ath_err(common,
-				"failed to allocate rx descriptors: %d\n",
-				error);
+	/* Initialize rx descriptors */
+
+	error = ath_descdma_setup(sc, &sc->rx.rxdma, &sc->rx.rxbuf,
+				  "rx", nbufs, 1, 0);
+	if (error != 0) {
+		ath_err(common,
+			"failed to allocate rx descriptors: %d\n",
+			error);
+		goto err;
+	}
+
+	list_for_each_entry(bf, &sc->rx.rxbuf, list) {
+		skb = ath_rxbuf_alloc(common, common->rx_bufsize,
+				      GFP_KERNEL);
+		if (skb == NULL) {
+			error = -ENOMEM;
 			goto err;
 		}
 
-		list_for_each_entry(bf, &sc->rx.rxbuf, list) {
-			skb = ath_rxbuf_alloc(common, common->rx_bufsize,
-					      GFP_KERNEL);
-			if (skb == NULL) {
-				error = -ENOMEM;
-				goto err;
-			}
-
-			bf->bf_mpdu = skb;
-			bf->bf_buf_addr = dma_map_single(sc->dev, skb->data,
-					common->rx_bufsize,
-					DMA_FROM_DEVICE);
-			if (unlikely(dma_mapping_error(sc->dev,
-							bf->bf_buf_addr))) {
-				dev_kfree_skb_any(skb);
-				bf->bf_mpdu = NULL;
-				bf->bf_buf_addr = 0;
-				ath_err(common,
-					"dma_mapping_error() on RX init\n");
-				error = -ENOMEM;
-				goto err;
-			}
+		bf->bf_mpdu = skb;
+		bf->bf_buf_addr = dma_map_single(sc->dev, skb->data,
+						 common->rx_bufsize,
+						 DMA_FROM_DEVICE);
+		if (unlikely(dma_mapping_error(sc->dev,
+					       bf->bf_buf_addr))) {
+			dev_kfree_skb_any(skb);
+			bf->bf_mpdu = NULL;
+			bf->bf_buf_addr = 0;
+			ath_err(common,
+				"dma_mapping_error() on RX init\n");
+			error = -ENOMEM;
+			goto err;
 		}
-		sc->rx.rxlink = NULL;
 	}
-
+	sc->rx.rxlink = NULL;
 err:
 	if (error)
 		ath_rx_cleanup(sc);
@@ -340,17 +332,17 @@ void ath_rx_cleanup(struct ath_softc *sc)
 	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_EDMA) {
 		ath_rx_edma_cleanup(sc);
 		return;
-	} else {
-		list_for_each_entry(bf, &sc->rx.rxbuf, list) {
-			skb = bf->bf_mpdu;
-			if (skb) {
-				dma_unmap_single(sc->dev, bf->bf_buf_addr,
-						common->rx_bufsize,
-						DMA_FROM_DEVICE);
-				dev_kfree_skb(skb);
-				bf->bf_buf_addr = 0;
-				bf->bf_mpdu = NULL;
-			}
+	}
+
+	list_for_each_entry(bf, &sc->rx.rxbuf, list) {
+		skb = bf->bf_mpdu;
+		if (skb) {
+			dma_unmap_single(sc->dev, bf->bf_buf_addr,
+					 common->rx_bufsize,
+					 DMA_FROM_DEVICE);
+			dev_kfree_skb(skb);
+			bf->bf_buf_addr = 0;
+			bf->bf_mpdu = NULL;
 		}
 	}
 }
@@ -381,6 +373,10 @@ u32 ath_calcrxfilter(struct ath_softc *sc)
 	rfilt = ATH9K_RX_FILTER_UCAST | ATH9K_RX_FILTER_BCAST
 		| ATH9K_RX_FILTER_MCAST;
 
+	/* if operating on a DFS channel, enable radar pulse detection */
+	if (sc->hw->conf.radar_enabled)
+		rfilt |= ATH9K_RX_FILTER_PHYRADAR | ATH9K_RX_FILTER_PHYERR;
+
 	if (sc->rx.rxfilter & FIF_PROBE_REQ)
 		rfilt |= ATH9K_RX_FILTER_PROBEREQ;
 
@@ -723,6 +719,13 @@ static struct ath_buf *ath_get_next_rx_buf(struct ath_softc *sc,
 		ret = ath9k_hw_rxprocdesc(ah, tds, &trs);
 		if (ret == -EINPROGRESS)
 			return NULL;
+
+		/*
+		 * mark descriptor as zero-length and set the 'more'
+		 * flag to ensure that both buffers get discarded
+		 */
+		rs->rs_datalen = 0;
+		rs->rs_more = true;
 	}
 
 	list_del(&bf->list);
@@ -859,7 +862,7 @@ static int ath9k_process_rate(struct ath_common *common,
 	unsigned int i = 0;
 	struct ath_softc __maybe_unused *sc = common->priv;
 
-	band = hw->conf.channel->band;
+	band = hw->conf.chandef.chan->band;
 	sband = hw->wiphy->bands[band];
 
 	if (rx_stats->rs_rate & 0x80) {
@@ -929,14 +932,20 @@ static void ath9k_process_rssi(struct ath_common *common,
  * up the frame up to let mac80211 handle the actual error case, be it no
  * decryption key or real decryption error. This let us keep statistics there.
  */
-static int ath9k_rx_skb_preprocess(struct ath_common *common,
-				   struct ieee80211_hw *hw,
+static int ath9k_rx_skb_preprocess(struct ath_softc *sc,
 				   struct ieee80211_hdr *hdr,
 				   struct ath_rx_status *rx_stats,
 				   struct ieee80211_rx_status *rx_status,
 				   bool *decrypt_error)
 {
-	struct ath_hw *ah = common->ah;
+	struct ieee80211_hw *hw = sc->hw;
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
+	bool discard_current = sc->rx.discard_next;
+
+	sc->rx.discard_next = rx_stats->rs_more;
+	if (discard_current)
+		return -EINVAL;
 
 	/*
 	 * everything but the rate is checked here, the rate check is done
@@ -954,14 +963,15 @@ static int ath9k_rx_skb_preprocess(struct ath_common *common,
 	if (ath9k_process_rate(common, hw, rx_stats, rx_status))
 		return -EINVAL;
 
-	rx_status->band = hw->conf.channel->band;
-	rx_status->freq = hw->conf.channel->center_freq;
+	rx_status->band = hw->conf.chandef.chan->band;
+	rx_status->freq = hw->conf.chandef.chan->center_freq;
 	rx_status->signal = ah->noise + rx_stats->rs_rssi;
 	rx_status->antenna = rx_stats->rs_antenna;
 	rx_status->flag |= RX_FLAG_MACTIME_END;
 	if (rx_stats->rs_moreaggr)
 		rx_status->flag |= RX_FLAG_NO_SIGNAL_VAL;
 
+	sc->rx.discard_next = false;
 	return 0;
 }
 
@@ -981,7 +991,7 @@ static void ath9k_rx_skb_postprocess(struct ath_common *common,
 	hdr = (struct ieee80211_hdr *) skb->data;
 	hdrlen = ieee80211_get_hdrlen_from_skb(skb);
 	fc = hdr->frame_control;
-	padpos = ath9k_cmn_padpos(hdr->frame_control);
+	padpos = ieee80211_hdrlen(fc);
 
 	/* The MAC header is padded to have 32-bit boundary if the
 	 * packet payload is non-zero. The general calculation for
@@ -1162,6 +1172,7 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 	u64 tsf = 0;
 	u32 tsf_lower = 0;
 	unsigned long flags;
+	dma_addr_t new_buf_addr;
 
 	if (edma)
 		dma_type = DMA_BIDIRECTIONAL;
@@ -1228,6 +1239,9 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 		    unlikely(tsf_lower - rs.rs_tstamp > 0x10000000))
 			rxs->mactime += 0x100000000ULL;
 
+		if (rs.rs_phyerr == ATH9K_PHYERR_RADAR)
+			ath9k_dfs_process_phyerr(sc, hdr, &rs, rxs->mactime);
+
 		if (rs.rs_status & ATH9K_RXERR_PHY) {
 			if (ath_process_fft(sc, hdr, &rs, rxs->mactime)) {
 				RX_STAT_INC(rx_spectral);
@@ -1235,8 +1249,8 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 			}
 		}
 
-		retval = ath9k_rx_skb_preprocess(common, hw, hdr, &rs,
-						 rxs, &decrypt_error);
+		retval = ath9k_rx_skb_preprocess(sc, hdr, &rs, rxs,
+						 &decrypt_error);
 		if (retval)
 			goto requeue_drop_frag;
 
@@ -1257,10 +1271,20 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 			goto requeue_drop_frag;
 		}
 
+		/* We will now give hardware our shiny new allocated skb */
+		new_buf_addr = dma_map_single(sc->dev, requeue_skb->data,
+					      common->rx_bufsize, dma_type);
+		if (unlikely(dma_mapping_error(sc->dev, new_buf_addr))) {
+			dev_kfree_skb_any(requeue_skb);
+			goto requeue_drop_frag;
+		}
+
 		/* Unmap the frame */
 		dma_unmap_single(sc->dev, bf->bf_buf_addr,
-				 common->rx_bufsize,
-				 dma_type);
+				 common->rx_bufsize, dma_type);
+
+		bf->bf_mpdu = requeue_skb;
+		bf->bf_buf_addr = new_buf_addr;
 
 		skb_put(skb, rs.rs_datalen + ah->caps.rx_status_len);
 		if (ah->caps.rx_status_len)
@@ -1270,21 +1294,6 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 			ath9k_rx_skb_postprocess(common, hdr_skb, &rs,
 						 rxs, decrypt_error);
 
-		/* We will now give hardware our shiny new allocated skb */
-		bf->bf_mpdu = requeue_skb;
-		bf->bf_buf_addr = dma_map_single(sc->dev, requeue_skb->data,
-						 common->rx_bufsize,
-						 dma_type);
-		if (unlikely(dma_mapping_error(sc->dev,
-			  bf->bf_buf_addr))) {
-			dev_kfree_skb_any(requeue_skb);
-			bf->bf_mpdu = NULL;
-			bf->bf_buf_addr = 0;
-			ath_err(common, "dma_mapping_error() on RX\n");
-			ieee80211_rx(hw, skb);
-			break;
-		}
-
 		if (rs.rs_more) {
 			RX_STAT_INC(rx_frags);
 			/*
@@ -1302,6 +1311,8 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 			sc->rx.frag = skb;
 			goto requeue;
 		}
+		if (rs.rs_status & ATH9K_RXERR_CORRUPT_DESC)
+			goto requeue_drop_frag;
 
 		if (sc->rx.frag) {
 			int space = skb->len - skb_tailroom(hdr_skb);
diff --git a/drivers/net/wireless/ath/ath9k/reg.h b/drivers/net/wireless/ath/ath9k/reg.h
index 5929850649f0..5c4ab5026dca 100644
--- a/drivers/net/wireless/ath/ath9k/reg.h
+++ b/drivers/net/wireless/ath/ath9k/reg.h
@@ -1493,9 +1493,6 @@ enum {
 #define AR9271_RADIO_RF_RST			0x20
 #define AR9271_GATE_MAC_CTL			0x4000
 
-#define AR_STA_ID0                 0x8000
-#define AR_STA_ID1                 0x8004
-#define AR_STA_ID1_SADH_MASK       0x0000FFFF
 #define AR_STA_ID1_STA_AP          0x00010000
 #define AR_STA_ID1_ADHOC           0x00020000
 #define AR_STA_ID1_PWR_SAV         0x00040000
diff --git a/drivers/net/wireless/ath/ath9k/xmit.c b/drivers/net/wireless/ath/ath9k/xmit.c
index 89a64411b82e..eab0fcb7ded6 100644
--- a/drivers/net/wireless/ath/ath9k/xmit.c
+++ b/drivers/net/wireless/ath/ath9k/xmit.c
@@ -157,6 +157,13 @@ static void ath_send_bar(struct ath_atx_tid *tid, u16 seqno)
 			   seqno << IEEE80211_SEQ_SEQ_SHIFT);
 }
 
+static void ath_set_rates(struct ieee80211_vif *vif, struct ieee80211_sta *sta,
+			  struct ath_buf *bf)
+{
+	ieee80211_get_tx_rates(vif, sta, bf->bf_mpdu, bf->rates,
+			       ARRAY_SIZE(bf->rates));
+}
+
 static void ath_tx_flush_tid(struct ath_softc *sc, struct ath_atx_tid *tid)
 {
 	struct ath_txq *txq = tid->ac->txq;
@@ -189,6 +196,7 @@ static void ath_tx_flush_tid(struct ath_softc *sc, struct ath_atx_tid *tid)
 			ath_tx_complete_buf(sc, bf, txq, &bf_head, &ts, 0);
 			sendbar = true;
 		} else {
+			ath_set_rates(tid->an->vif, tid->an->sta, bf);
 			ath_tx_send_normal(sc, txq, NULL, skb);
 		}
 	}
@@ -407,7 +415,7 @@ static void ath_tx_complete_aggr(struct ath_softc *sc, struct ath_txq *txq,
 
 	tx_info = IEEE80211_SKB_CB(skb);
 
-	memcpy(rates, tx_info->control.rates, sizeof(rates));
+	memcpy(rates, bf->rates, sizeof(rates));
 
 	retries = ts->ts_longretry + 1;
 	for (i = 0; i < ts->ts_rateindex; i++)
@@ -516,8 +524,7 @@ static void ath_tx_complete_aggr(struct ath_softc *sc, struct ath_txq *txq,
 		 * not a holding desc.
 		 */
 		INIT_LIST_HEAD(&bf_head);
-		if ((sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_EDMA) ||
-		    bf_next != NULL || !bf_last->bf_stale)
+		if (bf_next != NULL || !bf_last->bf_stale)
 			list_move_tail(&bf->list, &bf_head);
 
 		if (!txpending || (tid->state & AGGR_CLEANUP)) {
@@ -537,8 +544,7 @@ static void ath_tx_complete_aggr(struct ath_softc *sc, struct ath_txq *txq,
 				!txfail);
 		} else {
 			/* retry the un-acked ones */
-			if (!(sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_EDMA) &&
-			    bf->bf_next == NULL && bf_last->bf_stale) {
+			if (bf->bf_next == NULL && bf_last->bf_stale) {
 				struct ath_buf *tbf;
 
 				tbf = ath_clone_txbuf(sc, bf_last);
@@ -738,8 +744,6 @@ static int ath_compute_num_delims(struct ath_softc *sc, struct ath_atx_tid *tid,
 				  bool first_subfrm)
 {
 #define FIRST_DESC_NDELIMS 60
-	struct sk_buff *skb = bf->bf_mpdu;
-	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
 	u32 nsymbits, nsymbols;
 	u16 minlen;
 	u8 flags, rix;
@@ -780,8 +784,8 @@ static int ath_compute_num_delims(struct ath_softc *sc, struct ath_atx_tid *tid,
 	if (tid->an->mpdudensity == 0)
 		return ndelim;
 
-	rix = tx_info->control.rates[0].idx;
-	flags = tx_info->control.rates[0].flags;
+	rix = bf->rates[0].idx;
+	flags = bf->rates[0].flags;
 	width = (flags & IEEE80211_TX_RC_40_MHZ_WIDTH) ? 1 : 0;
 	half_gi = (flags & IEEE80211_TX_RC_SHORT_GI) ? 1 : 0;
 
@@ -860,6 +864,7 @@ static enum ATH_AGGR_STATUS ath_tx_form_aggr(struct ath_softc *sc,
 			bf_first = bf;
 
 		if (!rl) {
+			ath_set_rates(tid->an->vif, tid->an->sta, bf);
 			aggr_limit = ath_lookup_rate(sc, bf, tid);
 			rl = 1;
 		}
@@ -1000,14 +1005,14 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf,
 
 	skb = bf->bf_mpdu;
 	tx_info = IEEE80211_SKB_CB(skb);
-	rates = tx_info->control.rates;
+	rates = bf->rates;
 	hdr = (struct ieee80211_hdr *)skb->data;
 
 	/* set dur_update_en for l-sig computation except for PS-Poll frames */
 	info->dur_update = !ieee80211_is_pspoll(hdr->frame_control);
 	info->rtscts_rate = fi->rtscts_rate;
 
-	for (i = 0; i < 4; i++) {
+	for (i = 0; i < ARRAY_SIZE(bf->rates); i++) {
 		bool is_40, is_sgi, is_sp;
 		int phy;
 
@@ -1745,6 +1750,7 @@ static void ath_tx_send_ampdu(struct ath_softc *sc, struct ath_atx_tid *tid,
 		return;
 	}
 
+	ath_set_rates(tid->an->vif, tid->an->sta, bf);
 	bf->bf_state.bf_type = BUF_AMPDU;
 	INIT_LIST_HEAD(&bf_head);
 	list_add(&bf->list, &bf_head);
@@ -1894,49 +1900,6 @@ static struct ath_buf *ath_tx_setup_buffer(struct ath_softc *sc,
 	return bf;
 }
 
-/* FIXME: tx power */
-static void ath_tx_start_dma(struct ath_softc *sc, struct sk_buff *skb,
-			     struct ath_tx_control *txctl)
-{
-	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
-	struct ath_atx_tid *tid = NULL;
-	struct ath_buf *bf;
-	u8 tidno;
-
-	if (txctl->an && ieee80211_is_data_qos(hdr->frame_control)) {
-		tidno = ieee80211_get_qos_ctl(hdr)[0] &
-			IEEE80211_QOS_CTL_TID_MASK;
-		tid = ATH_AN_2_TID(txctl->an, tidno);
-
-		WARN_ON(tid->ac->txq != txctl->txq);
-	}
-
-	if ((tx_info->flags & IEEE80211_TX_CTL_AMPDU) && tid) {
-		/*
-		 * Try aggregation if it's a unicast data frame
-		 * and the destination is HT capable.
-		 */
-		ath_tx_send_ampdu(sc, tid, skb, txctl);
-	} else {
-		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;
-
-		if (txctl->paprd)
-			bf->bf_state.bfs_paprd_timestamp = jiffies;
-
-		ath_tx_send_normal(sc, txctl->txq, tid, skb);
-	}
-}
-
 /* Upon failure caller should free skb */
 int ath_tx_start(struct ieee80211_hw *hw, struct sk_buff *skb,
 		 struct ath_tx_control *txctl)
@@ -1947,8 +1910,11 @@ int ath_tx_start(struct ieee80211_hw *hw, struct sk_buff *skb,
 	struct ieee80211_vif *vif = info->control.vif;
 	struct ath_softc *sc = hw->priv;
 	struct ath_txq *txq = txctl->txq;
+	struct ath_atx_tid *tid = NULL;
+	struct ath_buf *bf;
 	int padpos, padsize;
 	int frmlen = skb->len + FCS_LEN;
+	u8 tidno;
 	int q;
 
 	/* NOTE:  sta can be NULL according to net/mac80211.h */
@@ -1971,7 +1937,7 @@ int ath_tx_start(struct ieee80211_hw *hw, struct sk_buff *skb,
 	}
 
 	/* Add the padding after the header if this is not already done */
-	padpos = ath9k_cmn_padpos(hdr->frame_control);
+	padpos = ieee80211_hdrlen(hdr->frame_control);
 	padsize = padpos & 3;
 	if (padsize && skb->len > padpos) {
 		if (skb_headroom(skb) < padsize)
@@ -2004,8 +1970,41 @@ int ath_tx_start(struct ieee80211_hw *hw, struct sk_buff *skb,
 		txq->stopped = true;
 	}
 
-	ath_tx_start_dma(sc, skb, txctl);
+	if (txctl->an && ieee80211_is_data_qos(hdr->frame_control)) {
+		tidno = ieee80211_get_qos_ctl(hdr)[0] &
+			IEEE80211_QOS_CTL_TID_MASK;
+		tid = ATH_AN_2_TID(txctl->an, tidno);
+
+		WARN_ON(tid->ac->txq != txctl->txq);
+	}
+
+	if ((info->flags & IEEE80211_TX_CTL_AMPDU) && tid) {
+		/*
+		 * Try aggregation if it's a unicast data frame
+		 * and the destination is HT capable.
+		 */
+		ath_tx_send_ampdu(sc, tid, skb, txctl);
+		goto out;
+	}
 
+	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);
+		goto out;
+	}
+
+	bf->bf_state.bfs_paprd = txctl->paprd;
+
+	if (txctl->paprd)
+		bf->bf_state.bfs_paprd_timestamp = jiffies;
+
+	ath_set_rates(vif, sta, bf);
+	ath_tx_send_normal(sc, txctl->txq, tid, skb);
+
+out:
 	ath_txq_unlock(sc, txq);
 
 	return 0;
@@ -2033,7 +2032,7 @@ static void ath_tx_complete(struct ath_softc *sc, struct sk_buff *skb,
 		/* Frame was ACKed */
 		tx_info->flags |= IEEE80211_TX_STAT_ACK;
 
-	padpos = ath9k_cmn_padpos(hdr->frame_control);
+	padpos = ieee80211_hdrlen(hdr->frame_control);
 	padsize = padpos & 3;
 	if (padsize && skb->len>padpos+padsize) {
 		/*
@@ -2264,6 +2263,7 @@ void ath_tx_edma_tasklet(struct ath_softc *sc)
 	struct ath_txq *txq;
 	struct ath_buf *bf, *lastbf;
 	struct list_head bf_head;
+	struct list_head *fifo_list;
 	int status;
 
 	for (;;) {
@@ -2291,20 +2291,24 @@ void ath_tx_edma_tasklet(struct ath_softc *sc)
 
 		TX_STAT_INC(txq->axq_qnum, txprocdesc);
 
-		if (list_empty(&txq->txq_fifo[txq->txq_tailidx])) {
+		fifo_list = &txq->txq_fifo[txq->txq_tailidx];
+		if (list_empty(fifo_list)) {
 			ath_txq_unlock(sc, txq);
 			return;
 		}
 
-		bf = list_first_entry(&txq->txq_fifo[txq->txq_tailidx],
-				      struct ath_buf, list);
+		bf = list_first_entry(fifo_list, struct ath_buf, list);
+		if (bf->bf_stale) {
+			list_del(&bf->list);
+			ath_tx_return_buffer(sc, bf);
+			bf = list_first_entry(fifo_list, struct ath_buf, list);
+		}
+
 		lastbf = bf->bf_lastbf;
 
 		INIT_LIST_HEAD(&bf_head);
-		list_cut_position(&bf_head, &txq->txq_fifo[txq->txq_tailidx],
-				  &lastbf->list);
-
-		if (list_empty(&txq->txq_fifo[txq->txq_tailidx])) {
+		if (list_is_last(&lastbf->list, fifo_list)) {
+			list_splice_tail_init(fifo_list, &bf_head);
 			INCR(txq->txq_tailidx, ATH_TXFIFO_DEPTH);
 
 			if (!list_empty(&txq->axq_q)) {
@@ -2315,6 +2319,11 @@ void ath_tx_edma_tasklet(struct ath_softc *sc)
 				list_splice_tail_init(&txq->axq_q, &bf_q);
 				ath_tx_txqaddbuf(sc, txq, &bf_q, true);
 			}
+		} else {
+			lastbf->bf_stale = true;
+			if (bf != lastbf)
+				list_cut_position(&bf_head, fifo_list,
+						  lastbf->list.prev);
 		}
 
 		ath_tx_process_buffer(sc, txq, &ts, bf, &bf_head);
diff --git a/drivers/net/wireless/ath/carl9170/carl9170.h b/drivers/net/wireless/ath/carl9170/carl9170.h
index 25599741cd8a..9dce106cd6d4 100644
--- a/drivers/net/wireless/ath/carl9170/carl9170.h
+++ b/drivers/net/wireless/ath/carl9170/carl9170.h
@@ -70,12 +70,6 @@
 
 static const u8 ar9170_qmap[__AR9170_NUM_TXQ] = { 3, 2, 1, 0 };
 
-enum carl9170_rf_init_mode {
-	CARL9170_RFI_NONE,
-	CARL9170_RFI_WARM,
-	CARL9170_RFI_COLD,
-};
-
 #define CARL9170_MAX_RX_BUFFER_SIZE		8192
 
 enum carl9170_device_state {
@@ -599,7 +593,7 @@ int carl9170_led_set_state(struct ar9170 *ar, const u32 led_state);
 
 /* PHY / RF */
 int carl9170_set_channel(struct ar9170 *ar, struct ieee80211_channel *channel,
-	enum nl80211_channel_type bw, enum carl9170_rf_init_mode rfi);
+			 enum nl80211_channel_type bw);
 int carl9170_get_noisefloor(struct ar9170 *ar);
 
 /* FW */
diff --git a/drivers/net/wireless/ath/carl9170/debug.c b/drivers/net/wireless/ath/carl9170/debug.c
index 93fe6003a493..3d70cd277fd7 100644
--- a/drivers/net/wireless/ath/carl9170/debug.c
+++ b/drivers/net/wireless/ath/carl9170/debug.c
@@ -654,8 +654,8 @@ static ssize_t carl9170_debugfs_bug_write(struct ar9170 *ar, const char *buf,
 		goto out;
 
 	case 'P':
-		err = carl9170_set_channel(ar, ar->hw->conf.channel,
-			ar->hw->conf.channel_type, CARL9170_RFI_COLD);
+		err = carl9170_set_channel(ar, ar->hw->conf.chandef.chan,
+			cfg80211_get_chandef_type(&ar->hw->conf.chandef));
 		if (err < 0)
 			count = err;
 
diff --git a/drivers/net/wireless/ath/carl9170/mac.c b/drivers/net/wireless/ath/carl9170/mac.c
index 24d75ab94f0d..a2f005703c04 100644
--- a/drivers/net/wireless/ath/carl9170/mac.c
+++ b/drivers/net/wireless/ath/carl9170/mac.c
@@ -48,7 +48,7 @@ int carl9170_set_dyn_sifs_ack(struct ar9170 *ar)
 	if (conf_is_ht40(&ar->hw->conf))
 		val = 0x010a;
 	else {
-		if (ar->hw->conf.channel->band == IEEE80211_BAND_2GHZ)
+		if (ar->hw->conf.chandef.chan->band == IEEE80211_BAND_2GHZ)
 			val = 0x105;
 		else
 			val = 0x104;
@@ -66,7 +66,7 @@ int carl9170_set_rts_cts_rate(struct ar9170 *ar)
 		rts_rate = 0x1da;
 		cts_rate = 0x10a;
 	} else {
-		if (ar->hw->conf.channel->band == IEEE80211_BAND_2GHZ) {
+		if (ar->hw->conf.chandef.chan->band == IEEE80211_BAND_2GHZ) {
 			/* 11 mbit CCK */
 			rts_rate = 033;
 			cts_rate = 003;
@@ -93,7 +93,7 @@ int carl9170_set_slot_time(struct ar9170 *ar)
 		return 0;
 	}
 
-	if ((ar->hw->conf.channel->band == IEEE80211_BAND_5GHZ) ||
+	if ((ar->hw->conf.chandef.chan->band == IEEE80211_BAND_5GHZ) ||
 	    vif->bss_conf.use_short_slot)
 		slottime = 9;
 
@@ -120,7 +120,7 @@ int carl9170_set_mac_rates(struct ar9170 *ar)
 	basic |= (vif->bss_conf.basic_rates & 0xff0) << 4;
 	rcu_read_unlock();
 
-	if (ar->hw->conf.channel->band == IEEE80211_BAND_5GHZ)
+	if (ar->hw->conf.chandef.chan->band == IEEE80211_BAND_5GHZ)
 		mandatory = 0xff00; /* OFDM 6/9/12/18/24/36/48/54 */
 	else
 		mandatory = 0xff0f; /* OFDM (6/9../54) + CCK (1/2/5.5/11) */
diff --git a/drivers/net/wireless/ath/carl9170/main.c b/drivers/net/wireless/ath/carl9170/main.c
index f293b3ff4756..e9010a481dfd 100644
--- a/drivers/net/wireless/ath/carl9170/main.c
+++ b/drivers/net/wireless/ath/carl9170/main.c
@@ -929,6 +929,9 @@ static int carl9170_op_config(struct ieee80211_hw *hw, u32 changed)
 	}
 
 	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
+		enum nl80211_channel_type channel_type =
+			cfg80211_get_chandef_type(&hw->conf.chandef);
+
 		/* adjust slot time for 5 GHz */
 		err = carl9170_set_slot_time(ar);
 		if (err)
@@ -938,8 +941,8 @@ static int carl9170_op_config(struct ieee80211_hw *hw, u32 changed)
 		if (err)
 			goto out;
 
-		err = carl9170_set_channel(ar, hw->conf.channel,
-			hw->conf.channel_type, CARL9170_RFI_NONE);
+		err = carl9170_set_channel(ar, hw->conf.chandef.chan,
+					   channel_type);
 		if (err)
 			goto out;
 
@@ -957,7 +960,7 @@ static int carl9170_op_config(struct ieee80211_hw *hw, u32 changed)
 	}
 
 	if (changed & IEEE80211_CONF_CHANGE_POWER) {
-		err = carl9170_set_mac_tpc(ar, ar->hw->conf.channel);
+		err = carl9170_set_mac_tpc(ar, ar->hw->conf.chandef.chan);
 		if (err)
 			goto out;
 	}
@@ -1703,7 +1706,7 @@ found:
 	return 0;
 }
 
-static void carl9170_op_flush(struct ieee80211_hw *hw, bool drop)
+static void carl9170_op_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 {
 	struct ar9170 *ar = hw->priv;
 	unsigned int vid;
diff --git a/drivers/net/wireless/ath/carl9170/phy.c b/drivers/net/wireless/ath/carl9170/phy.c
index b72c09cf43a4..ab4ee7d39ad3 100644
--- a/drivers/net/wireless/ath/carl9170/phy.c
+++ b/drivers/net/wireless/ath/carl9170/phy.c
@@ -1331,7 +1331,7 @@ static void carl9170_calc_ctl(struct ar9170 *ar, u32 freq, enum carl9170_bw bw)
 	 * CTL_ETSI for 2GHz and CTL_FCC for 5GHz.
 	 */
 	ctl_grp = ath_regd_get_band_ctl(&ar->common.regulatory,
-					ar->hw->conf.channel->band);
+					ar->hw->conf.chandef.chan->band);
 
 	/* ctl group not found - either invalid band (NO_CTL) or ww roaming */
 	if (ctl_grp == NO_CTL || ctl_grp == SD_NO_CTL)
@@ -1341,7 +1341,7 @@ static void carl9170_calc_ctl(struct ar9170 *ar, u32 freq, enum carl9170_bw bw)
 		/* skip CTL and heavy clip for CTL_MKK and CTL_ETSI */
 		return;
 
-	if (ar->hw->conf.channel->band == IEEE80211_BAND_2GHZ) {
+	if (ar->hw->conf.chandef.chan->band == IEEE80211_BAND_2GHZ) {
 		modes = mode_list_2ghz;
 		nr_modes = ARRAY_SIZE(mode_list_2ghz);
 	} else {
@@ -1569,16 +1569,14 @@ static enum carl9170_bw nl80211_to_carl(enum nl80211_channel_type type)
 }
 
 int carl9170_set_channel(struct ar9170 *ar, struct ieee80211_channel *channel,
-			 enum nl80211_channel_type _bw,
-			 enum carl9170_rf_init_mode rfi)
+			 enum nl80211_channel_type _bw)
 {
 	const struct carl9170_phy_freq_params *freqpar;
 	struct carl9170_rf_init_result rf_res;
 	struct carl9170_rf_init rf;
-	u32 cmd, tmp, offs = 0, new_ht = 0;
+	u32 tmp, offs = 0, new_ht = 0;
 	int err;
 	enum carl9170_bw bw;
-	bool warm_reset;
 	struct ieee80211_channel *old_channel = NULL;
 
 	bw = nl80211_to_carl(_bw);
@@ -1592,51 +1590,27 @@ int carl9170_set_channel(struct ar9170 *ar, struct ieee80211_channel *channel,
 	/* may be NULL at first setup */
 	if (ar->channel) {
 		old_channel = ar->channel;
-		warm_reset = (old_channel->band != channel->band) ||
-			     (old_channel->center_freq ==
-			      channel->center_freq) ||
-			     (ar->ht_settings != new_ht);
-
 		ar->channel = NULL;
-	} else {
-		warm_reset = true;
 	}
 
-	/* HW workaround */
-	if (!ar->hw->wiphy->bands[IEEE80211_BAND_5GHZ] &&
-	    channel->center_freq <= 2417)
-		warm_reset = true;
-
-	if (rfi != CARL9170_RFI_NONE || warm_reset) {
-		u32 val;
-
-		if (rfi == CARL9170_RFI_COLD)
-			val = AR9170_PWR_RESET_BB_COLD_RESET;
-		else
-			val = AR9170_PWR_RESET_BB_WARM_RESET;
-
-		/* warm/cold reset BB/ADDA */
-		err = carl9170_write_reg(ar, AR9170_PWR_REG_RESET, val);
-		if (err)
-			return err;
-
-		err = carl9170_write_reg(ar, AR9170_PWR_REG_RESET, 0x0);
-		if (err)
-			return err;
+	/* cold reset BB/ADDA */
+	err = carl9170_write_reg(ar, AR9170_PWR_REG_RESET,
+				 AR9170_PWR_RESET_BB_COLD_RESET);
+	if (err)
+		return err;
 
-		err = carl9170_init_phy(ar, channel->band);
-		if (err)
-			return err;
+	err = carl9170_write_reg(ar, AR9170_PWR_REG_RESET, 0x0);
+	if (err)
+		return err;
 
-		err = carl9170_init_rf_banks_0_7(ar,
-			channel->band == IEEE80211_BAND_5GHZ);
-		if (err)
-			return err;
+	err = carl9170_init_phy(ar, channel->band);
+	if (err)
+		return err;
 
-		cmd = CARL9170_CMD_RF_INIT;
-	} else {
-		cmd = CARL9170_CMD_FREQUENCY;
-	}
+	err = carl9170_init_rf_banks_0_7(ar,
+					 channel->band == IEEE80211_BAND_5GHZ);
+	if (err)
+		return err;
 
 	err = carl9170_exec_cmd(ar, CARL9170_CMD_FREQ_START, 0, NULL, 0, NULL);
 	if (err)
@@ -1648,8 +1622,8 @@ int carl9170_set_channel(struct ar9170 *ar, struct ieee80211_channel *channel,
 		return err;
 
 	err = carl9170_init_rf_bank4_pwr(ar,
-		channel->band == IEEE80211_BAND_5GHZ,
-		channel->center_freq, bw);
+					 channel->band == IEEE80211_BAND_5GHZ,
+					 channel->center_freq, bw);
 	if (err)
 		return err;
 
@@ -1703,13 +1677,8 @@ int carl9170_set_channel(struct ar9170 *ar, struct ieee80211_channel *channel,
 	rf.delta_slope_coeff_man = cpu_to_le32(freqpar->coeff_man);
 	rf.delta_slope_coeff_exp_shgi = cpu_to_le32(freqpar->coeff_exp_shgi);
 	rf.delta_slope_coeff_man_shgi = cpu_to_le32(freqpar->coeff_man_shgi);
-
-	if (rfi != CARL9170_RFI_NONE)
-		rf.finiteLoopCount = cpu_to_le32(2000);
-	else
-		rf.finiteLoopCount = cpu_to_le32(1000);
-
-	err = carl9170_exec_cmd(ar, cmd, sizeof(rf), &rf,
+	rf.finiteLoopCount = cpu_to_le32(2000);
+	err = carl9170_exec_cmd(ar, CARL9170_CMD_RF_INIT, sizeof(rf), &rf,
 				sizeof(rf_res), &rf_res);
 	if (err)
 		return err;
@@ -1724,9 +1693,8 @@ int carl9170_set_channel(struct ar9170 *ar, struct ieee80211_channel *channel,
 			  old_channel->center_freq : -1, channel->center_freq,
 			  err);
 
-		if ((rfi == CARL9170_RFI_COLD) || (ar->chan_fail > 3)) {
-			/*
-			 * We have tried very hard to change to _another_
+		if (ar->chan_fail > 3) {
+			/* We have tried very hard to change to _another_
 			 * channel and we've failed to do so!
 			 * Chances are that the PHY/RF is no longer
 			 * operable (due to corruptions/fatal events/bugs?)
@@ -1736,8 +1704,7 @@ int carl9170_set_channel(struct ar9170 *ar, struct ieee80211_channel *channel,
 			return 0;
 		}
 
-		err = carl9170_set_channel(ar, channel, _bw,
-					   CARL9170_RFI_COLD);
+		err = carl9170_set_channel(ar, channel, _bw);
 		if (err)
 			return err;
 	} else {
diff --git a/drivers/net/wireless/ath/carl9170/tx.c b/drivers/net/wireless/ath/carl9170/tx.c
index 9c0b150d5b8e..c61cafa2665b 100644
--- a/drivers/net/wireless/ath/carl9170/tx.c
+++ b/drivers/net/wireless/ath/carl9170/tx.c
@@ -387,8 +387,7 @@ static void carl9170_tx_status_process_ampdu(struct ar9170 *ar,
 	u8 tid;
 
 	if (!(txinfo->flags & IEEE80211_TX_CTL_AMPDU) ||
-	    txinfo->flags & IEEE80211_TX_CTL_INJECTED ||
-	   (!(super->f.mac_control & cpu_to_le16(AR9170_TX_MAC_AGGR))))
+	    txinfo->flags & IEEE80211_TX_CTL_INJECTED)
 		return;
 
 	rcu_read_lock();
@@ -981,30 +980,6 @@ static int carl9170_tx_prepare(struct ar9170 *ar,
 
 		SET_VAL(CARL9170_TX_SUPER_AMPDU_FACTOR,
 			txc->s.ampdu_settings, factor);
-
-		for (i = 0; i < CARL9170_TX_MAX_RATES; i++) {
-			txrate = &info->control.rates[i];
-			if (txrate->idx >= 0) {
-				txc->s.ri[i] =
-					CARL9170_TX_SUPER_RI_AMPDU;
-
-				if (WARN_ON(!(txrate->flags &
-					      IEEE80211_TX_RC_MCS))) {
-					/*
-					 * Not sure if it's even possible
-					 * to aggregate non-ht rates with
-					 * this HW.
-					 */
-					goto err_out;
-				}
-				continue;
-			}
-
-			txrate->idx = 0;
-			txrate->count = ar->hw->max_rate_tries;
-		}
-
-		mac_tmp |= cpu_to_le16(AR9170_TX_MAC_AGGR);
 	}
 
 	/*
@@ -1012,11 +987,31 @@ static int carl9170_tx_prepare(struct ar9170 *ar,
 	 * taken from mac_control. For all fallback rate, the firmware
 	 * updates the mac_control flags from the rate info field.
 	 */
-	for (i = 1; i < CARL9170_TX_MAX_RATES; i++) {
+	for (i = 0; i < CARL9170_TX_MAX_RATES; i++) {
+		__le32 phy_set;
 		txrate = &info->control.rates[i];
 		if (txrate->idx < 0)
 			break;
 
+		phy_set = carl9170_tx_physet(ar, info, txrate);
+		if (i == 0) {
+			/* first rate - part of the hw's frame header */
+			txc->f.phy_control = phy_set;
+
+			if (ampdu && txrate->flags & IEEE80211_TX_RC_MCS)
+				mac_tmp |= cpu_to_le16(AR9170_TX_MAC_AGGR);
+			if (carl9170_tx_rts_check(ar, txrate, ampdu, no_ack))
+				mac_tmp |= cpu_to_le16(AR9170_TX_MAC_PROT_RTS);
+			else if (carl9170_tx_cts_check(ar, txrate))
+				mac_tmp |= cpu_to_le16(AR9170_TX_MAC_PROT_CTS);
+
+		} else {
+			/* fallback rates are stored in the firmware's
+			 * retry rate set array.
+			 */
+			txc->s.rr[i - 1] = phy_set;
+		}
+
 		SET_VAL(CARL9170_TX_SUPER_RI_TRIES, txc->s.ri[i],
 			txrate->count);
 
@@ -1027,21 +1022,13 @@ static int carl9170_tx_prepare(struct ar9170 *ar,
 			txc->s.ri[i] |= (AR9170_TX_MAC_PROT_CTS <<
 				CARL9170_TX_SUPER_RI_ERP_PROT_S);
 
-		txc->s.rr[i - 1] = carl9170_tx_physet(ar, info, txrate);
+		if (ampdu && (txrate->flags & IEEE80211_TX_RC_MCS))
+			txc->s.ri[i] |= CARL9170_TX_SUPER_RI_AMPDU;
 	}
 
-	txrate = &info->control.rates[0];
-	SET_VAL(CARL9170_TX_SUPER_RI_TRIES, txc->s.ri[0], txrate->count);
-
-	if (carl9170_tx_rts_check(ar, txrate, ampdu, no_ack))
-		mac_tmp |= cpu_to_le16(AR9170_TX_MAC_PROT_RTS);
-	else if (carl9170_tx_cts_check(ar, txrate))
-		mac_tmp |= cpu_to_le16(AR9170_TX_MAC_PROT_CTS);
-
 	txc->s.len = cpu_to_le16(skb->len);
 	txc->f.length = cpu_to_le16(len + FCS_LEN);
 	txc->f.mac_control = mac_tmp;
-	txc->f.phy_control = carl9170_tx_physet(ar, info, txrate);
 
 	arinfo = (void *)info->rate_driver_data;
 	arinfo->timeout = jiffies;
@@ -1381,9 +1368,9 @@ static void carl9170_tx(struct ar9170 *ar)
 }
 
 static bool carl9170_tx_ampdu_queue(struct ar9170 *ar,
-	struct ieee80211_sta *sta, struct sk_buff *skb)
+	struct ieee80211_sta *sta, struct sk_buff *skb,
+	struct ieee80211_tx_info *txinfo)
 {
-	struct _carl9170_tx_superframe *super = (void *) skb->data;
 	struct carl9170_sta_info *sta_info;
 	struct carl9170_sta_tid *agg;
 	struct sk_buff *iter;
@@ -1450,7 +1437,7 @@ err_unlock:
 
 err_unlock_rcu:
 	rcu_read_unlock();
-	super->f.mac_control &= ~cpu_to_le16(AR9170_TX_MAC_AGGR);
+	txinfo->flags &= ~IEEE80211_TX_CTL_AMPDU;
 	carl9170_tx_status(ar, skb, false);
 	ar->tx_dropped++;
 	return false;
@@ -1492,7 +1479,7 @@ void carl9170_op_tx(struct ieee80211_hw *hw,
 		 * sta == NULL checks are redundant in this
 		 * special case.
 		 */
-		run = carl9170_tx_ampdu_queue(ar, sta, skb);
+		run = carl9170_tx_ampdu_queue(ar, sta, skb, info);
 		if (run)
 			carl9170_tx_ampdu(ar);
 
diff --git a/drivers/net/wireless/ath/hw.c b/drivers/net/wireless/ath/hw.c
index 39e8a590d7fc..eae9abf540a7 100644
--- a/drivers/net/wireless/ath/hw.c
+++ b/drivers/net/wireless/ath/hw.c
@@ -118,6 +118,12 @@
 void ath_hw_setbssidmask(struct ath_common *common)
 {
 	void *ah = common->ah;
+	u32 id1;
+
+	REG_WRITE(ah, AR_STA_ID0, get_unaligned_le32(common->macaddr));
+	id1 = REG_READ(ah, AR_STA_ID1) & ~AR_STA_ID1_SADH_MASK;
+	id1 |= get_unaligned_le16(common->macaddr + 4);
+	REG_WRITE(ah, AR_STA_ID1, id1);
 
 	REG_WRITE(ah, AR_BSSMSKL, get_unaligned_le32(common->bssidmask));
 	REG_WRITE(ah, AR_BSSMSKU, get_unaligned_le16(common->bssidmask + 4));
diff --git a/drivers/net/wireless/ath/key.c b/drivers/net/wireless/ath/key.c
index 5c54aa43ca2d..1816b4e7dc26 100644
--- a/drivers/net/wireless/ath/key.c
+++ b/drivers/net/wireless/ath/key.c
@@ -45,7 +45,8 @@ bool ath_hw_keyreset(struct ath_common *common, u16 entry)
 	void *ah = common->ah;
 
 	if (entry >= common->keymax) {
-		ath_err(common, "keycache entry %u out of range\n", entry);
+		ath_err(common, "keyreset: keycache entry %u out of range\n",
+			entry);
 		return false;
 	}
 
@@ -91,7 +92,8 @@ static bool ath_hw_keysetmac(struct ath_common *common,
 	void *ah = common->ah;
 
 	if (entry >= common->keymax) {
-		ath_err(common, "keycache entry %u out of range\n", entry);
+		ath_err(common, "keysetmac: keycache entry %u out of range\n",
+			entry);
 		return false;
 	}
 
@@ -133,7 +135,8 @@ static bool ath_hw_set_keycache_entry(struct ath_common *common, u16 entry,
 	u32 keyType;
 
 	if (entry >= common->keymax) {
-		ath_err(common, "keycache entry %u out of range\n", entry);
+		ath_err(common, "set-entry: keycache entry %u out of range\n",
+			entry);
 		return false;
 	}
 
diff --git a/drivers/net/wireless/ath/reg.h b/drivers/net/wireless/ath/reg.h
index 298e53f3fa48..3ad4c774bd22 100644
--- a/drivers/net/wireless/ath/reg.h
+++ b/drivers/net/wireless/ath/reg.h
@@ -23,6 +23,10 @@
 #define AR_MIBC_CMC		0x00000004
 #define AR_MIBC_MCS		0x00000008
 
+#define AR_STA_ID0		0x8000
+#define AR_STA_ID1		0x8004
+#define AR_STA_ID1_SADH_MASK	0x0000ffff
+
 /*
  * BSSID mask registers. See ath_hw_set_bssid_mask()
  * for detailed documentation about these registers.
diff --git a/drivers/net/wireless/ath/wil6210/Makefile b/drivers/net/wireless/ath/wil6210/Makefile
index 9396dc9fe3c5..d288eea0a26a 100644
--- a/drivers/net/wireless/ath/wil6210/Makefile
+++ b/drivers/net/wireless/ath/wil6210/Makefile
@@ -9,5 +9,7 @@ wil6210-objs += wmi.o
 wil6210-objs += interrupt.o
 wil6210-objs += txrx.o
 
-subdir-ccflags-y += -Werror
+ifeq (, $(findstring -W,$(EXTRA_CFLAGS)))
+	subdir-ccflags-y += -Werror
+endif
 subdir-ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/ath/wil6210/cfg80211.c b/drivers/net/wireless/ath/wil6210/cfg80211.c
index 9ecc1968262c..c5d4a87abaaf 100644
--- a/drivers/net/wireless/ath/wil6210/cfg80211.c
+++ b/drivers/net/wireless/ath/wil6210/cfg80211.c
@@ -14,16 +14,6 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
-#include <linux/kernel.h>
-#include <linux/netdevice.h>
-#include <linux/sched.h>
-#include <linux/etherdevice.h>
-#include <linux/wireless.h>
-#include <linux/ieee80211.h>
-#include <linux/slab.h>
-#include <linux/version.h>
-#include <net/cfg80211.h>
-
 #include "wil6210.h"
 #include "wmi.h"
 
@@ -292,7 +282,7 @@ static int wil_cfg80211_connect(struct wiphy *wiphy,
 
 	/* WMI_CONNECT_CMD */
 	memset(&conn, 0, sizeof(conn));
-	switch (bss->capability & 0x03) {
+	switch (bss->capability & WLAN_CAPABILITY_DMG_TYPE_MASK) {
 	case WLAN_CAPABILITY_DMG_TYPE_AP:
 		conn.network_type = WMI_NETTYPE_INFRA;
 		break;
@@ -437,17 +427,18 @@ static int wil_cfg80211_start_ap(struct wiphy *wiphy,
 	if (rc)
 		return rc;
 
-	rc = wmi_set_channel(wil, channel->hw_value);
-	if (rc)
-		return rc;
-
 	/* MAC address - pre-requisite for other commands */
 	wmi_set_mac_address(wil, ndev->dev_addr);
 
 	/* IE's */
 	/* bcon 'head IE's are not relevant for 60g band */
-	wmi_set_ie(wil, WMI_FRAME_BEACON, bcon->beacon_ies_len,
-		   bcon->beacon_ies);
+	/*
+	 * FW do not form regular beacon, so bcon IE's are not set
+	 * For the DMG bcon, when it will be supported, bcon IE's will
+	 * be reused; add something like:
+	 * wmi_set_ie(wil, WMI_FRAME_BEACON, bcon->beacon_ies_len,
+	 * bcon->beacon_ies);
+	 */
 	wmi_set_ie(wil, WMI_FRAME_PROBE_RESP, bcon->proberesp_ies_len,
 		   bcon->proberesp_ies);
 	wmi_set_ie(wil, WMI_FRAME_ASSOC_RESP, bcon->assocresp_ies_len,
@@ -455,7 +446,8 @@ static int wil_cfg80211_start_ap(struct wiphy *wiphy,
 
 	wil->secure_pcp = info->privacy;
 
-	rc = wmi_set_bcon(wil, info->beacon_interval, wmi_nettype);
+	rc = wmi_pcp_start(wil, info->beacon_interval, wmi_nettype,
+			   channel->hw_value);
 	if (rc)
 		return rc;
 
@@ -472,11 +464,8 @@ static int wil_cfg80211_stop_ap(struct wiphy *wiphy,
 {
 	int rc = 0;
 	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
-	struct wireless_dev *wdev = ndev->ieee80211_ptr;
-	u8 wmi_nettype = wil_iftype_nl2wmi(wdev->iftype);
 
-	/* To stop beaconing, set BI to 0 */
-	rc = wmi_set_bcon(wil, 0, wmi_nettype);
+	rc = wmi_pcp_stop(wil);
 
 	return rc;
 }
diff --git a/drivers/net/wireless/ath/wil6210/dbg_hexdump.h b/drivers/net/wireless/ath/wil6210/dbg_hexdump.h
deleted file mode 100644
index e5712f026c47..000000000000
--- a/drivers/net/wireless/ath/wil6210/dbg_hexdump.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef WIL_DBG_HEXDUMP_H_
-#define WIL_DBG_HEXDUMP_H_
-
-#include <linux/printk.h>
-#include <linux/dynamic_debug.h>
-
-#if defined(CONFIG_DYNAMIC_DEBUG)
-#define wil_print_hex_dump_debug(prefix_str, prefix_type, rowsize,	\
-				 groupsize, buf, len, ascii)		\
-	dynamic_hex_dump(prefix_str, prefix_type, rowsize,		\
-			     groupsize, buf, len, ascii)
-
-#else /* defined(CONFIG_DYNAMIC_DEBUG) */
-#define wil_print_hex_dump_debug(prefix_str, prefix_type, rowsize,	\
-				 groupsize, buf, len, ascii)		\
-	print_hex_dump(KERN_DEBUG, prefix_str, prefix_type, rowsize,	\
-		       groupsize, buf, len, ascii)
-#endif /* defined(CONFIG_DYNAMIC_DEBUG) */
-
-#endif /* WIL_DBG_HEXDUMP_H_ */
diff --git a/drivers/net/wireless/ath/wil6210/debugfs.c b/drivers/net/wireless/ath/wil6210/debugfs.c
index 65fc9683bfd8..727b1f53e6ad 100644
--- a/drivers/net/wireless/ath/wil6210/debugfs.c
+++ b/drivers/net/wireless/ath/wil6210/debugfs.c
@@ -216,7 +216,7 @@ DEFINE_SIMPLE_ATTRIBUTE(fops_iomem_x32, wil_debugfs_iomem_x32_get,
 			wil_debugfs_iomem_x32_set, "0x%08llx\n");
 
 static struct dentry *wil_debugfs_create_iomem_x32(const char *name,
-						   mode_t mode,
+						   umode_t mode,
 						   struct dentry *parent,
 						   void __iomem *value)
 {
@@ -312,14 +312,6 @@ static const struct file_operations fops_memread = {
 	.llseek		= seq_lseek,
 };
 
-static int wil_default_open(struct inode *inode, struct file *file)
-{
-	if (inode->i_private)
-		file->private_data = inode->i_private;
-
-	return 0;
-}
-
 static ssize_t wil_read_file_ioblob(struct file *file, char __user *user_buf,
 				size_t count, loff_t *ppos)
 {
@@ -361,13 +353,13 @@ static ssize_t wil_read_file_ioblob(struct file *file, char __user *user_buf,
 
 static const struct file_operations fops_ioblob = {
 	.read =		wil_read_file_ioblob,
-	.open =		wil_default_open,
+	.open =		simple_open,
 	.llseek =	default_llseek,
 };
 
 static
 struct dentry *wil_debugfs_create_ioblob(const char *name,
-					 mode_t mode,
+					 umode_t mode,
 					 struct dentry *parent,
 					 struct debugfs_blob_wrapper *blob)
 {
@@ -396,7 +388,7 @@ static ssize_t wil_write_file_reset(struct file *file, const char __user *buf,
 
 static const struct file_operations fops_reset = {
 	.write = wil_write_file_reset,
-	.open  = wil_default_open,
+	.open  = simple_open,
 };
 /*---------Tx descriptor------------*/
 
@@ -526,7 +518,50 @@ static ssize_t wil_write_file_ssid(struct file *file, const char __user *buf,
 static const struct file_operations fops_ssid = {
 	.read = wil_read_file_ssid,
 	.write = wil_write_file_ssid,
-	.open  = wil_default_open,
+	.open  = simple_open,
+};
+
+/*---------temp------------*/
+static void print_temp(struct seq_file *s, const char *prefix, u32 t)
+{
+	switch (t) {
+	case 0:
+	case ~(u32)0:
+		seq_printf(s, "%s N/A\n", prefix);
+	break;
+	default:
+		seq_printf(s, "%s %d.%03d\n", prefix, t / 1000, t % 1000);
+		break;
+	}
+}
+
+static int wil_temp_debugfs_show(struct seq_file *s, void *data)
+{
+	struct wil6210_priv *wil = s->private;
+	u32 t_m, t_r;
+
+	int rc = wmi_get_temperature(wil, &t_m, &t_r);
+	if (rc) {
+		seq_printf(s, "Failed\n");
+		return 0;
+	}
+
+	print_temp(s, "MAC temperature   :", t_m);
+	print_temp(s, "Radio temperature :", t_r);
+
+	return 0;
+}
+
+static int wil_temp_seq_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, wil_temp_debugfs_show, inode->i_private);
+}
+
+static const struct file_operations fops_temp = {
+	.open		= wil_temp_seq_open,
+	.release	= single_release,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
 };
 
 /*----------------*/
@@ -563,6 +598,7 @@ int wil6210_debugfs_init(struct wil6210_priv *wil)
 	debugfs_create_file("mem_val", S_IRUGO, dbg, wil, &fops_memread);
 
 	debugfs_create_file("reset", S_IWUSR, dbg, wil, &fops_reset);
+	debugfs_create_file("temp", S_IRUGO, dbg, wil, &fops_temp);
 
 	wil->rgf_blob.data = (void * __force)wil->csr + 0;
 	wil->rgf_blob.size = 0xa000;
diff --git a/drivers/net/wireless/ath/wil6210/interrupt.c b/drivers/net/wireless/ath/wil6210/interrupt.c
index dc97e7b2609c..e3c1e7684f9c 100644
--- a/drivers/net/wireless/ath/wil6210/interrupt.c
+++ b/drivers/net/wireless/ath/wil6210/interrupt.c
@@ -240,6 +240,15 @@ static void wil_notify_fw_error(struct wil6210_priv *wil)
 	kobject_uevent_env(&dev->kobj, KOBJ_CHANGE, envp);
 }
 
+static void wil_cache_mbox_regs(struct wil6210_priv *wil)
+{
+	/* make shadow copy of registers that should not change on run time */
+	wil_memcpy_fromio_32(&wil->mbox_ctl, wil->csr + HOST_MBOX,
+			     sizeof(struct wil6210_mbox_ctl));
+	wil_mbox_ring_le2cpus(&wil->mbox_ctl.rx);
+	wil_mbox_ring_le2cpus(&wil->mbox_ctl.tx);
+}
+
 static irqreturn_t wil6210_irq_misc(int irq, void *cookie)
 {
 	struct wil6210_priv *wil = cookie;
@@ -257,14 +266,19 @@ static irqreturn_t wil6210_irq_misc(int irq, void *cookie)
 	wil6210_mask_irq_misc(wil);
 
 	if (isr & ISR_MISC_FW_ERROR) {
-		wil_dbg_irq(wil, "IRQ: Firmware error\n");
+		wil_err(wil, "Firmware error detected\n");
 		clear_bit(wil_status_fwready, &wil->status);
-		wil_notify_fw_error(wil);
-		isr &= ~ISR_MISC_FW_ERROR;
+		/*
+		 * do not clear @isr here - we do 2-nd part in thread
+		 * there, user space get notified, and it should be done
+		 * in non-atomic context
+		 */
 	}
 
 	if (isr & ISR_MISC_FW_READY) {
 		wil_dbg_irq(wil, "IRQ: FW ready\n");
+		wil_cache_mbox_regs(wil);
+		set_bit(wil_status_reset_done, &wil->status);
 		/**
 		 * Actual FW ready indicated by the
 		 * WMI_FW_READY_EVENTID
@@ -289,6 +303,11 @@ static irqreturn_t wil6210_irq_misc_thread(int irq, void *cookie)
 
 	wil_dbg_irq(wil, "Thread ISR MISC 0x%08x\n", isr);
 
+	if (isr & ISR_MISC_FW_ERROR) {
+		wil_notify_fw_error(wil);
+		isr &= ~ISR_MISC_FW_ERROR;
+	}
+
 	if (isr & ISR_MISC_MBOX_EVT) {
 		wil_dbg_irq(wil, "MBOX event\n");
 		wmi_recv_cmd(wil);
diff --git a/drivers/net/wireless/ath/wil6210/main.c b/drivers/net/wireless/ath/wil6210/main.c
index 761c389586d4..a0478e2f6868 100644
--- a/drivers/net/wireless/ath/wil6210/main.c
+++ b/drivers/net/wireless/ath/wil6210/main.c
@@ -14,12 +14,6 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
-#include <linux/kernel.h>
-#include <linux/netdevice.h>
-#include <linux/sched.h>
-#include <linux/ieee80211.h>
-#include <linux/wireless.h>
-#include <linux/slab.h>
 #include <linux/moduleparam.h>
 #include <linux/if_arp.h>
 
@@ -109,13 +103,24 @@ static void wil_connect_timer_fn(ulong x)
 	schedule_work(&wil->disconnect_worker);
 }
 
-static void wil_cache_mbox_regs(struct wil6210_priv *wil)
+static void wil_connect_worker(struct work_struct *work)
 {
-	/* make shadow copy of registers that should not change on run time */
-	wil_memcpy_fromio_32(&wil->mbox_ctl, wil->csr + HOST_MBOX,
-			     sizeof(struct wil6210_mbox_ctl));
-	wil_mbox_ring_le2cpus(&wil->mbox_ctl.rx);
-	wil_mbox_ring_le2cpus(&wil->mbox_ctl.tx);
+	int rc;
+	struct wil6210_priv *wil = container_of(work, struct wil6210_priv,
+						connect_worker);
+	int cid = wil->pending_connect_cid;
+
+	if (cid < 0) {
+		wil_err(wil, "No connection pending\n");
+		return;
+	}
+
+	wil_dbg_wmi(wil, "Configure for connection CID %d\n", cid);
+
+	rc = wil_vring_init_tx(wil, 0, WIL6210_TX_RING_SIZE, cid, 0);
+	wil->pending_connect_cid = -1;
+	if (rc == 0)
+		wil_link_on(wil);
 }
 
 int wil_priv_init(struct wil6210_priv *wil)
@@ -130,7 +135,7 @@ int wil_priv_init(struct wil6210_priv *wil)
 	wil->pending_connect_cid = -1;
 	setup_timer(&wil->connect_timer, wil_connect_timer_fn, (ulong)wil);
 
-	INIT_WORK(&wil->wmi_connect_worker, wmi_connect_worker);
+	INIT_WORK(&wil->connect_worker, wil_connect_worker);
 	INIT_WORK(&wil->disconnect_worker, wil_disconnect_worker);
 	INIT_WORK(&wil->wmi_event_worker, wmi_event_worker);
 
@@ -147,8 +152,6 @@ int wil_priv_init(struct wil6210_priv *wil)
 		return -EAGAIN;
 	}
 
-	wil_cache_mbox_regs(wil);
-
 	return 0;
 }
 
@@ -185,15 +188,11 @@ static void wil_target_reset(struct wil6210_priv *wil)
 	W(RGF_USER_MAC_CPU_0,  BIT(1)); /* mac_cpu_man_rst */
 	W(RGF_USER_USER_CPU_0, BIT(1)); /* user_cpu_man_rst */
 
-	msleep(100);
-
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0xFE000000);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_1, 0x0000003F);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_3, 0x00000170);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_0, 0xFFE7FC00);
 
-	msleep(100);
-
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_3, 0);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_1, 0);
@@ -203,12 +202,6 @@ static void wil_target_reset(struct wil6210_priv *wil)
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0x00000080);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_0, 0);
 
-	msleep(2000);
-
-	W(RGF_USER_USER_CPU_0, BIT(0)); /* user_cpu_man_de_rst */
-
-	msleep(2000);
-
 	wil_dbg_misc(wil, "Reset completed\n");
 
 #undef W
@@ -265,8 +258,6 @@ int wil_reset(struct wil6210_priv *wil)
 	wil->pending_connect_cid = -1;
 	INIT_COMPLETION(wil->wmi_ready);
 
-	wil_cache_mbox_regs(wil);
-
 	/* TODO: release MAC reset */
 	wil6210_enable_irq(wil);
 
@@ -352,9 +343,9 @@ static int __wil_up(struct wil6210_priv *wil)
 			wil_err(wil, "SSID not set\n");
 			return -EINVAL;
 		}
-		wmi_set_ssid(wil, wdev->ssid_len, wdev->ssid);
-		if (channel)
-			wmi_set_channel(wil, channel->hw_value);
+		rc = wmi_set_ssid(wil, wdev->ssid_len, wdev->ssid);
+		if (rc)
+			return rc;
 		break;
 	default:
 		break;
@@ -364,9 +355,12 @@ static int __wil_up(struct wil6210_priv *wil)
 	wmi_set_mac_address(wil, ndev->dev_addr);
 
 	/* Set up beaconing if required. */
-	rc = wmi_set_bcon(wil, bi, wmi_nettype);
-	if (rc)
-		return rc;
+	if (bi > 0) {
+		rc = wmi_pcp_start(wil, bi, wmi_nettype,
+				   (channel ? channel->hw_value : 0));
+		if (rc)
+			return rc;
+	}
 
 	/* Rx VRING. After MAC and beacon */
 	wil_rx_init(wil);
diff --git a/drivers/net/wireless/ath/wil6210/netdev.c b/drivers/net/wireless/ath/wil6210/netdev.c
index 8ce2e33dce20..098a8ec6b841 100644
--- a/drivers/net/wireless/ath/wil6210/netdev.c
+++ b/drivers/net/wireless/ath/wil6210/netdev.c
@@ -14,10 +14,7 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
-#include <linux/module.h>
-#include <linux/netdevice.h>
 #include <linux/etherdevice.h>
-#include <linux/slab.h>
 
 #include "wil6210.h"
 
diff --git a/drivers/net/wireless/ath/wil6210/pcie_bus.c b/drivers/net/wireless/ath/wil6210/pcie_bus.c
index 81c35c6e3832..eb1dc7ad80fb 100644
--- a/drivers/net/wireless/ath/wil6210/pcie_bus.c
+++ b/drivers/net/wireless/ath/wil6210/pcie_bus.c
@@ -14,10 +14,7 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
-#include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/netdevice.h>
 #include <linux/debugfs.h>
 #include <linux/pci.h>
 #include <linux/moduleparam.h>
diff --git a/drivers/net/wireless/ath/wil6210/txrx.c b/drivers/net/wireless/ath/wil6210/txrx.c
index d1315b442375..797024507c71 100644
--- a/drivers/net/wireless/ath/wil6210/txrx.c
+++ b/drivers/net/wireless/ath/wil6210/txrx.c
@@ -14,10 +14,7 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
-#include <linux/kernel.h>
-#include <linux/netdevice.h>
 #include <linux/etherdevice.h>
-#include <linux/hardirq.h>
 #include <net/ieee80211_radiotap.h>
 #include <linux/if_arp.h>
 #include <linux/moduleparam.h>
@@ -83,8 +80,6 @@ static int wil_vring_alloc(struct wil6210_priv *wil, struct vring *vring)
 	 */
 	vring->va = dma_alloc_coherent(dev, sz, &vring->pa, GFP_KERNEL);
 	if (!vring->va) {
-		wil_err(wil, "vring_alloc [%d] failed to alloc DMA mem\n",
-			vring->size);
 		kfree(vring->ctx);
 		vring->ctx = NULL;
 		return -ENOMEM;
@@ -196,8 +191,7 @@ static int wil_vring_alloc_skb(struct wil6210_priv *wil, struct vring *vring,
  *  - Phy info
  */
 static void wil_rx_add_radiotap_header(struct wil6210_priv *wil,
-				       struct sk_buff *skb,
-				       volatile struct vring_rx_desc *d)
+				       struct sk_buff *skb)
 {
 	struct wireless_dev *wdev = wil->wdev;
 	struct wil6210_rtap {
@@ -221,6 +215,7 @@ static void wil_rx_add_radiotap_header(struct wil6210_priv *wil,
 		__le16 vendor_skip;
 		u8 vendor_data[0];
 	} __packed;
+	struct vring_rx_desc *d = wil_skb_rxdesc(skb);
 	struct wil6210_rtap_vendor *rtap_vendor;
 	int rtap_len = sizeof(struct wil6210_rtap);
 	int phy_length = 0; /* phy info header size, bytes */
@@ -317,6 +312,8 @@ static void wil_swap_ethaddr(void *data)
 /**
  * reap 1 frame from @swhead
  *
+ * Rx descriptor copied to skb->cb
+ *
  * Safe to call from IRQ
  */
 static struct sk_buff *wil_vring_reap_rx(struct wil6210_priv *wil,
@@ -325,12 +322,15 @@ static struct sk_buff *wil_vring_reap_rx(struct wil6210_priv *wil,
 	struct device *dev = wil_to_dev(wil);
 	struct net_device *ndev = wil_to_ndev(wil);
 	volatile struct vring_rx_desc *d;
+	struct vring_rx_desc *d1;
 	struct sk_buff *skb;
 	dma_addr_t pa;
 	unsigned int sz = RX_BUF_LEN;
 	u8 ftype;
 	u8 ds_bits;
 
+	BUILD_BUG_ON(sizeof(struct vring_rx_desc) > sizeof(skb->cb));
+
 	if (wil_vring_is_empty(vring))
 		return NULL;
 
@@ -345,11 +345,14 @@ static struct sk_buff *wil_vring_reap_rx(struct wil6210_priv *wil,
 	dma_unmap_single(dev, pa, sz, DMA_FROM_DEVICE);
 	skb_trim(skb, d->dma.length);
 
-	wil->stats.last_mcs_rx = wil_rxdesc_mcs(d);
+	d1 = wil_skb_rxdesc(skb);
+	*d1 = *d;
+
+	wil->stats.last_mcs_rx = wil_rxdesc_mcs(d1);
 
 	/* use radiotap header only if required */
 	if (ndev->type == ARPHRD_IEEE80211_RADIOTAP)
-		wil_rx_add_radiotap_header(wil, skb, d);
+		wil_rx_add_radiotap_header(wil, skb);
 
 	wil_dbg_txrx(wil, "Rx[%3d] : %d bytes\n", vring->swhead, d->dma.length);
 	wil_hex_dump_txrx("Rx ", DUMP_PREFIX_NONE, 32, 4,
@@ -365,7 +368,7 @@ static struct sk_buff *wil_vring_reap_rx(struct wil6210_priv *wil,
 	 * Driver should recognize it by frame type, that is found
 	 * in Rx descriptor. If type is not data, it is 802.11 frame as is
 	 */
-	ftype = wil_rxdesc_ftype(d) << 2;
+	ftype = wil_rxdesc_ftype(d1) << 2;
 	if (ftype != IEEE80211_FTYPE_DATA) {
 		wil_dbg_txrx(wil, "Non-data frame ftype 0x%08x\n", ftype);
 		/* TODO: process it */
@@ -380,7 +383,7 @@ static struct sk_buff *wil_vring_reap_rx(struct wil6210_priv *wil,
 		return NULL;
 	}
 
-	ds_bits = wil_rxdesc_ds_bits(d);
+	ds_bits = wil_rxdesc_ds_bits(d1);
 	if (ds_bits == 1) {
 		/*
 		 * HW bug - in ToDS mode, i.e. Rx on AP side,
@@ -522,6 +525,7 @@ int wil_vring_init_tx(struct wil6210_priv *wil, int id, int size,
 		.vring_cfg = {
 			.tx_sw_ring = {
 				.max_mpdu_size = cpu_to_le16(TX_BUF_LEN),
+				.ring_size = cpu_to_le16(size),
 			},
 			.ringid = id,
 			.cidxtid = (cid & 0xf) | ((tid & 0xf) << 4),
@@ -553,14 +557,13 @@ int wil_vring_init_tx(struct wil6210_priv *wil, int id, int size,
 		goto out;
 
 	cmd.vring_cfg.tx_sw_ring.ring_mem_base = cpu_to_le64(vring->pa);
-	cmd.vring_cfg.tx_sw_ring.ring_size = cpu_to_le16(vring->size);
 
 	rc = wmi_call(wil, WMI_VRING_CFG_CMDID, &cmd, sizeof(cmd),
 		      WMI_VRING_CFG_DONE_EVENTID, &reply, sizeof(reply), 100);
 	if (rc)
 		goto out_free;
 
-	if (reply.cmd.status != WMI_VRING_CFG_SUCCESS) {
+	if (reply.cmd.status != WMI_FW_STATUS_SUCCESS) {
 		wil_err(wil, "Tx config failed, status 0x%02x\n",
 			reply.cmd.status);
 		rc = -EINVAL;
@@ -784,9 +787,14 @@ void wil_tx_complete(struct wil6210_priv *wil, int ringid)
 	wil_dbg_txrx(wil, "%s(%d)\n", __func__, ringid);
 
 	while (!wil_vring_is_empty(vring)) {
-		volatile struct vring_tx_desc *d = &vring->va[vring->swtail].tx;
+		volatile struct vring_tx_desc *d1 =
+					      &vring->va[vring->swtail].tx;
+		struct vring_tx_desc dd, *d = &dd;
 		dma_addr_t pa;
 		struct sk_buff *skb;
+
+		dd = *d1;
+
 		if (!(d->dma.status & TX_DMA_STATUS_DU))
 			break;
 
diff --git a/drivers/net/wireless/ath/wil6210/txrx.h b/drivers/net/wireless/ath/wil6210/txrx.h
index 45a61f597c5c..adef12fb2aee 100644
--- a/drivers/net/wireless/ath/wil6210/txrx.h
+++ b/drivers/net/wireless/ath/wil6210/txrx.h
@@ -339,24 +339,59 @@ union vring_desc {
 	struct vring_rx_desc rx;
 } __packed;
 
-static inline int wil_rxdesc_phy_length(volatile struct vring_rx_desc *d)
+static inline int wil_rxdesc_tid(struct vring_rx_desc *d)
 {
-	return WIL_GET_BITS(d->dma.d0, 16, 29);
+	return WIL_GET_BITS(d->mac.d0, 0, 3);
 }
 
-static inline int wil_rxdesc_mcs(volatile struct vring_rx_desc *d)
+static inline int wil_rxdesc_cid(struct vring_rx_desc *d)
 {
-	return WIL_GET_BITS(d->mac.d1, 21, 24);
+	return WIL_GET_BITS(d->mac.d0, 4, 6);
 }
 
-static inline int wil_rxdesc_ds_bits(volatile struct vring_rx_desc *d)
+static inline int wil_rxdesc_mid(struct vring_rx_desc *d)
 {
-	return WIL_GET_BITS(d->mac.d1, 8, 9);
+	return WIL_GET_BITS(d->mac.d0, 8, 9);
 }
 
-static inline int wil_rxdesc_ftype(volatile struct vring_rx_desc *d)
+static inline int wil_rxdesc_ftype(struct vring_rx_desc *d)
 {
 	return WIL_GET_BITS(d->mac.d0, 10, 11);
 }
 
+static inline int wil_rxdesc_subtype(struct vring_rx_desc *d)
+{
+	return WIL_GET_BITS(d->mac.d0, 12, 15);
+}
+
+static inline int wil_rxdesc_seq(struct vring_rx_desc *d)
+{
+	return WIL_GET_BITS(d->mac.d0, 16, 27);
+}
+
+static inline int wil_rxdesc_ext_subtype(struct vring_rx_desc *d)
+{
+	return WIL_GET_BITS(d->mac.d0, 28, 31);
+}
+
+static inline int wil_rxdesc_ds_bits(struct vring_rx_desc *d)
+{
+	return WIL_GET_BITS(d->mac.d1, 8, 9);
+}
+
+static inline int wil_rxdesc_mcs(struct vring_rx_desc *d)
+{
+	return WIL_GET_BITS(d->mac.d1, 21, 24);
+}
+
+static inline int wil_rxdesc_phy_length(struct vring_rx_desc *d)
+{
+	return WIL_GET_BITS(d->dma.d0, 16, 29);
+}
+
+static inline struct vring_rx_desc *wil_skb_rxdesc(struct sk_buff *skb)
+{
+	return (void *)skb->cb;
+}
+
 #endif /* WIL6210_TXRX_H */
diff --git a/drivers/net/wireless/ath/wil6210/wil6210.h b/drivers/net/wireless/ath/wil6210/wil6210.h
index aea961ff8f08..8f76ecd8a7e5 100644
--- a/drivers/net/wireless/ath/wil6210/wil6210.h
+++ b/drivers/net/wireless/ath/wil6210/wil6210.h
@@ -21,8 +21,6 @@
 #include <linux/wireless.h>
 #include <net/cfg80211.h>
 
-#include "dbg_hexdump.h"
-
 #define WIL_NAME "wil6210"
 
 /**
@@ -188,6 +186,7 @@ enum { /* for wil6210_priv.status */
 	wil_status_fwready = 0,
 	wil_status_fwconnected,
 	wil_status_dontscan,
+	wil_status_reset_done,
 	wil_status_irqen, /* FIXME: interrupts enabled - for debug */
 };
 
@@ -210,6 +209,8 @@ struct wil6210_priv {
 	struct wireless_dev *wdev;
 	void __iomem *csr;
 	ulong status;
+	u32 fw_version;
+	u8 n_mids; /* number of additional MIDs as reported by FW */
 	/* profile */
 	u32 monitor_flags;
 	u32 secure_pcp; /* create secure PCP? */
@@ -227,7 +228,7 @@ struct wil6210_priv {
 	struct workqueue_struct *wmi_wq; /* for deferred calls */
 	struct work_struct wmi_event_worker;
 	struct workqueue_struct *wmi_wq_conn; /* for connect worker */
-	struct work_struct wmi_connect_worker;
+	struct work_struct connect_worker;
 	struct work_struct disconnect_worker;
 	struct timer_list connect_timer;
 	int pending_connect_cid;
@@ -277,13 +278,13 @@ struct wil6210_priv {
 
 #define wil_hex_dump_txrx(prefix_str, prefix_type, rowsize,	\
 			  groupsize, buf, len, ascii)		\
-			  wil_print_hex_dump_debug("DBG[TXRX]" prefix_str,\
+			  print_hex_dump_debug("DBG[TXRX]" prefix_str,\
 					 prefix_type, rowsize,	\
 					 groupsize, buf, len, ascii)
 
 #define wil_hex_dump_wmi(prefix_str, prefix_type, rowsize,	\
 			 groupsize, buf, len, ascii)		\
-			 wil_print_hex_dump_debug("DBG[ WMI]" prefix_str,\
+			 print_hex_dump_debug("DBG[ WMI]" prefix_str,\
 					prefix_type, rowsize,	\
 					groupsize, buf, len, ascii)
 
@@ -313,7 +314,6 @@ int wmi_send(struct wil6210_priv *wil, u16 cmdid, void *buf, u16 len);
 void wmi_recv_cmd(struct wil6210_priv *wil);
 int wmi_call(struct wil6210_priv *wil, u16 cmdid, void *buf, u16 len,
 	     u16 reply_id, void *reply, u8 reply_size, int to_msec);
-void wmi_connect_worker(struct work_struct *work);
 void wmi_event_worker(struct work_struct *work);
 void wmi_event_flush(struct wil6210_priv *wil);
 int wmi_set_ssid(struct wil6210_priv *wil, u8 ssid_len, const void *ssid);
@@ -328,6 +328,8 @@ int wmi_add_cipher_key(struct wil6210_priv *wil, u8 key_index,
 int wmi_echo(struct wil6210_priv *wil);
 int wmi_set_ie(struct wil6210_priv *wil, u8 type, u16 ie_len, const void *ie);
 int wmi_rx_chain_add(struct wil6210_priv *wil, struct vring *vring);
+int wmi_p2p_cfg(struct wil6210_priv *wil, int channel);
+int wmi_get_temperature(struct wil6210_priv *wil, u32 *t_m, u32 *t_r);
 
 int wil6210_init_irq(struct wil6210_priv *wil, int irq);
 void wil6210_fini_irq(struct wil6210_priv *wil, int irq);
@@ -341,7 +343,8 @@ struct wireless_dev *wil_cfg80211_init(struct device *dev);
 void wil_wdev_free(struct wil6210_priv *wil);
 
 int wmi_set_mac_address(struct wil6210_priv *wil, void *addr);
-int wmi_set_bcon(struct wil6210_priv *wil, int bi, u8 wmi_nettype);
+int wmi_pcp_start(struct wil6210_priv *wil, int bi, u8 wmi_nettype, u8 chan);
+int wmi_pcp_stop(struct wil6210_priv *wil);
 void wil6210_disconnect(struct wil6210_priv *wil, void *bssid);
 
 int wil_rx_init(struct wil6210_priv *wil);
diff --git a/drivers/net/wireless/ath/wil6210/wmi.c b/drivers/net/wireless/ath/wil6210/wmi.c
index 0bb3b76b4b58..45b04e383f9a 100644
--- a/drivers/net/wireless/ath/wil6210/wmi.c
+++ b/drivers/net/wireless/ath/wil6210/wmi.c
@@ -14,9 +14,6 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
-#include <linux/pci.h>
-#include <linux/io.h>
-#include <linux/list.h>
 #include <linux/etherdevice.h>
 #include <linux/if_arp.h>
 
@@ -272,16 +269,18 @@ static void wmi_evt_ready(struct wil6210_priv *wil, int id, void *d, int len)
 	struct net_device *ndev = wil_to_ndev(wil);
 	struct wireless_dev *wdev = wil->wdev;
 	struct wmi_ready_event *evt = d;
-	u32 ver = le32_to_cpu(evt->sw_version);
+	wil->fw_version = le32_to_cpu(evt->sw_version);
+	wil->n_mids = evt->numof_additional_mids;
 
-	wil_dbg_wmi(wil, "FW ver. %d; MAC %pM\n", ver, evt->mac);
+	wil_dbg_wmi(wil, "FW ver. %d; MAC %pM; %d MID's\n", wil->fw_version,
+		    evt->mac, wil->n_mids);
 
 	if (!is_valid_ether_addr(ndev->dev_addr)) {
 		memcpy(ndev->dev_addr, evt->mac, ETH_ALEN);
 		memcpy(ndev->perm_addr, evt->mac, ETH_ALEN);
 	}
 	snprintf(wdev->wiphy->fw_version, sizeof(wdev->wiphy->fw_version),
-		 "%d", ver);
+		 "%d", wil->fw_version);
 }
 
 static void wmi_evt_fw_ready(struct wil6210_priv *wil, int id, void *d,
@@ -324,17 +323,9 @@ static void wmi_evt_rx_mgmt(struct wil6210_priv *wil, int id, void *d, int len)
 
 	if (ieee80211_is_beacon(fc) || ieee80211_is_probe_resp(fc)) {
 		struct cfg80211_bss *bss;
-		u64 tsf = le64_to_cpu(rx_mgmt_frame->u.beacon.timestamp);
-		u16 cap = le16_to_cpu(rx_mgmt_frame->u.beacon.capab_info);
-		u16 bi = le16_to_cpu(rx_mgmt_frame->u.beacon.beacon_int);
-		const u8 *ie_buf = rx_mgmt_frame->u.beacon.variable;
-		size_t ie_len = d_len - offsetof(struct ieee80211_mgmt,
-						 u.beacon.variable);
-		wil_dbg_wmi(wil, "Capability info : 0x%04x\n", cap);
-
-		bss = cfg80211_inform_bss(wiphy, channel, rx_mgmt_frame->bssid,
-					  tsf, cap, bi, ie_buf, ie_len,
-					  signal, GFP_KERNEL);
+
+		bss = cfg80211_inform_bss_frame(wiphy, channel, rx_mgmt_frame,
+						d_len, signal, GFP_KERNEL);
 		if (bss) {
 			wil_dbg_wmi(wil, "Added BSS %pM\n",
 				    rx_mgmt_frame->bssid);
@@ -342,6 +333,9 @@ static void wmi_evt_rx_mgmt(struct wil6210_priv *wil, int id, void *d, int len)
 		} else {
 			wil_err(wil, "cfg80211_inform_bss() failed\n");
 		}
+	} else {
+		cfg80211_rx_mgmt(wil->wdev, freq, signal,
+				 (void *)rx_mgmt_frame, d_len, GFP_KERNEL);
 	}
 }
 
@@ -443,7 +437,7 @@ static void wmi_evt_connect(struct wil6210_priv *wil, int id, void *d, int len)
 	memcpy(wil->dst_addr[0], evt->bssid, ETH_ALEN);
 
 	wil->pending_connect_cid = evt->cid;
-	queue_work(wil->wmi_wq_conn, &wil->wmi_connect_worker);
+	queue_work(wil->wmi_wq_conn, &wil->connect_worker);
 }
 
 static void wmi_evt_disconnect(struct wil6210_priv *wil, int id,
@@ -528,6 +522,37 @@ static void wmi_evt_eapol_rx(struct wil6210_priv *wil, int id,
 	}
 }
 
+static void wmi_evt_linkup(struct wil6210_priv *wil, int id, void *d, int len)
+{
+	struct net_device *ndev = wil_to_ndev(wil);
+	struct wmi_data_port_open_event *evt = d;
+
+	wil_dbg_wmi(wil, "Link UP for CID %d\n", evt->cid);
+
+	netif_carrier_on(ndev);
+}
+
+static void wmi_evt_linkdown(struct wil6210_priv *wil, int id, void *d, int len)
+{
+	struct net_device *ndev = wil_to_ndev(wil);
+	struct wmi_wbe_link_down_event *evt = d;
+
+	wil_dbg_wmi(wil, "Link DOWN for CID %d, reason %d\n",
+		    evt->cid, le32_to_cpu(evt->reason));
+
+	netif_carrier_off(ndev);
+}
+
+static void wmi_evt_ba_status(struct wil6210_priv *wil, int id, void *d,
+			      int len)
+{
+	struct wmi_vring_ba_status_event *evt = d;
+
+	wil_dbg_wmi(wil, "BACK[%d] %s {%d} timeout %d\n",
+		    evt->ringid, evt->status ? "N/A" : "OK", evt->agg_wsize,
+		    __le16_to_cpu(evt->ba_timeout));
+}
+
 static const struct {
 	int eventid;
 	void (*handler)(struct wil6210_priv *wil, int eventid,
@@ -541,6 +566,9 @@ static const struct {
 	{WMI_DISCONNECT_EVENTID,	wmi_evt_disconnect},
 	{WMI_NOTIFY_REQ_DONE_EVENTID,	wmi_evt_notify},
 	{WMI_EAPOL_RX_EVENTID,		wmi_evt_eapol_rx},
+	{WMI_DATA_PORT_OPEN_EVENTID,	wmi_evt_linkup},
+	{WMI_WBE_LINKDOWN_EVENTID,	wmi_evt_linkdown},
+	{WMI_BA_STATUS_EVENTID,		wmi_evt_ba_status},
 };
 
 /*
@@ -559,6 +587,11 @@ void wmi_recv_cmd(struct wil6210_priv *wil)
 	void __iomem *src;
 	ulong flags;
 
+	if (!test_bit(wil_status_reset_done, &wil->status)) {
+		wil_err(wil, "Reset not completed\n");
+		return;
+	}
+
 	for (;;) {
 		u16 len;
 
@@ -683,18 +716,39 @@ int wmi_set_mac_address(struct wil6210_priv *wil, void *addr)
 	return wmi_send(wil, WMI_SET_MAC_ADDRESS_CMDID, &cmd, sizeof(cmd));
 }
 
-int wmi_set_bcon(struct wil6210_priv *wil, int bi, u8 wmi_nettype)
+int wmi_pcp_start(struct wil6210_priv *wil, int bi, u8 wmi_nettype, u8 chan)
 {
-	struct wmi_bcon_ctrl_cmd cmd = {
+	int rc;
+
+	struct wmi_pcp_start_cmd cmd = {
 		.bcon_interval = cpu_to_le16(bi),
 		.network_type = wmi_nettype,
 		.disable_sec_offload = 1,
+		.channel = chan,
 	};
+	struct {
+		struct wil6210_mbox_hdr_wmi wmi;
+		struct wmi_pcp_started_event evt;
+	} __packed reply;
 
 	if (!wil->secure_pcp)
 		cmd.disable_sec = 1;
 
-	return wmi_send(wil, WMI_BCON_CTRL_CMDID, &cmd, sizeof(cmd));
+	rc = wmi_call(wil, WMI_PCP_START_CMDID, &cmd, sizeof(cmd),
+		      WMI_PCP_STARTED_EVENTID, &reply, sizeof(reply), 100);
+	if (rc)
+		return rc;
+
+	if (reply.evt.status != WMI_FW_STATUS_SUCCESS)
+		rc = -EINVAL;
+
+	return rc;
+}
+
+int wmi_pcp_stop(struct wil6210_priv *wil)
+{
+	return wmi_call(wil, WMI_PCP_STOP_CMDID, NULL, 0,
+			WMI_PCP_STOPPED_EVENTID, NULL, 0, 20);
 }
 
 int wmi_set_ssid(struct wil6210_priv *wil, u8 ssid_len, const void *ssid)
@@ -765,6 +819,16 @@ int wmi_get_channel(struct wil6210_priv *wil, int *channel)
 	return 0;
 }
 
+int wmi_p2p_cfg(struct wil6210_priv *wil, int channel)
+{
+	struct wmi_p2p_cfg_cmd cmd = {
+		.discovery_mode = WMI_DISCOVERY_MODE_NON_OFFLOAD,
+		.channel = channel - 1,
+	};
+
+	return wmi_send(wil, WMI_P2P_CFG_CMDID, &cmd, sizeof(cmd));
+}
+
 int wmi_tx_eapol(struct wil6210_priv *wil, struct sk_buff *skb)
 {
 	struct wmi_eapol_tx_cmd *cmd;
@@ -843,7 +907,7 @@ int wmi_set_ie(struct wil6210_priv *wil, u8 type, u16 ie_len, const void *ie)
 	/* BUG: FW API define ieLen as u8. Will fix FW */
 	cmd->ie_len = cpu_to_le16(ie_len);
 	memcpy(cmd->ie_info, ie, ie_len);
-	rc = wmi_send(wil, WMI_SET_APPIE_CMDID, &cmd, len);
+	rc = wmi_send(wil, WMI_SET_APPIE_CMDID, cmd, len);
 	kfree(cmd);
 
 	return rc;
@@ -898,6 +962,31 @@ int wmi_rx_chain_add(struct wil6210_priv *wil, struct vring *vring)
 	return rc;
 }
 
+int wmi_get_temperature(struct wil6210_priv *wil, u32 *t_m, u32 *t_r)
+{
+	int rc;
+	struct wmi_temp_sense_cmd cmd = {
+		.measure_marlon_m_en = cpu_to_le32(!!t_m),
+		.measure_marlon_r_en = cpu_to_le32(!!t_r),
+	};
+	struct {
+		struct wil6210_mbox_hdr_wmi wmi;
+		struct wmi_temp_sense_done_event evt;
+	} __packed reply;
+
+	rc = wmi_call(wil, WMI_TEMP_SENSE_CMDID, &cmd, sizeof(cmd),
+		      WMI_TEMP_SENSE_DONE_EVENTID, &reply, sizeof(reply), 100);
+	if (rc)
+		return rc;
+
+	if (t_m)
+		*t_m = le32_to_cpu(reply.evt.marlon_m_t1000);
+	if (t_r)
+		*t_r = le32_to_cpu(reply.evt.marlon_r_t1000);
+
+	return 0;
+}
+
 void wmi_event_flush(struct wil6210_priv *wil)
 {
 	struct pending_wmi_event *evt, *t;
@@ -997,24 +1086,3 @@ void wmi_event_worker(struct work_struct *work)
 		kfree(evt);
 	}
 }
-
-void wmi_connect_worker(struct work_struct *work)
-{
-	int rc;
-	struct wil6210_priv *wil = container_of(work, struct wil6210_priv,
-						wmi_connect_worker);
-
-	if (wil->pending_connect_cid < 0) {
-		wil_err(wil, "No connection pending\n");
-		return;
-	}
-
-	wil_dbg_wmi(wil, "Configure for connection CID %d\n",
-		    wil->pending_connect_cid);
-
-	rc = wil_vring_init_tx(wil, 0, WIL6210_TX_RING_SIZE,
-			       wil->pending_connect_cid, 0);
-	wil->pending_connect_cid = -1;
-	if (rc == 0)
-		wil_link_on(wil);
-}
diff --git a/drivers/net/wireless/ath/wil6210/wmi.h b/drivers/net/wireless/ath/wil6210/wmi.h
index 3bbf87572b07..50b8528394f4 100644
--- a/drivers/net/wireless/ath/wil6210/wmi.h
+++ b/drivers/net/wireless/ath/wil6210/wmi.h
@@ -36,6 +36,7 @@
 enum wmi_command_id {
 	WMI_CONNECT_CMDID		= 0x0001,
 	WMI_DISCONNECT_CMDID		= 0x0003,
+	WMI_DISCONNECT_STA_CMDID	= 0x0004,
 	WMI_START_SCAN_CMDID		= 0x0007,
 	WMI_SET_BSS_FILTER_CMDID	= 0x0009,
 	WMI_SET_PROBED_SSID_CMDID	= 0x000a,
@@ -44,7 +45,6 @@ enum wmi_command_id {
 	WMI_ADD_CIPHER_KEY_CMDID	= 0x0016,
 	WMI_DELETE_CIPHER_KEY_CMDID	= 0x0017,
 	WMI_SET_APPIE_CMDID		= 0x003f,
-	WMI_GET_APPIE_CMDID		= 0x0040,
 	WMI_SET_WSC_STATUS_CMDID	= 0x0041,
 	WMI_PXMT_RANGE_CFG_CMDID	= 0x0042,
 	WMI_PXMT_SNR2_RANGE_CFG_CMDID	= 0x0043,
@@ -55,11 +55,11 @@ enum wmi_command_id {
 	WMI_DEEP_ECHO_CMDID		= 0x0804,
 	WMI_CONFIG_MAC_CMDID		= 0x0805,
 	WMI_CONFIG_PHY_DEBUG_CMDID	= 0x0806,
-	WMI_ADD_STATION_CMDID		= 0x0807,
 	WMI_ADD_DEBUG_TX_PCKT_CMDID	= 0x0808,
 	WMI_PHY_GET_STATISTICS_CMDID	= 0x0809,
 	WMI_FS_TUNE_CMDID		= 0x080a,
 	WMI_CORR_MEASURE_CMDID		= 0x080b,
+	WMI_READ_RSSI_CMDID		= 0x080c,
 	WMI_TEMP_SENSE_CMDID		= 0x080e,
 	WMI_DC_CALIB_CMDID		= 0x080f,
 	WMI_SEND_TONE_CMDID		= 0x0810,
@@ -75,9 +75,9 @@ enum wmi_command_id {
 	MAC_IO_STATIC_PARAMS_CMDID	= 0x081b,
 	MAC_IO_DYNAMIC_PARAMS_CMDID	= 0x081c,
 	WMI_SILENT_RSSI_CALIB_CMDID	= 0x081d,
+	WMI_RF_RX_TEST_CMDID		= 0x081e,
 	WMI_CFG_RX_CHAIN_CMDID		= 0x0820,
 	WMI_VRING_CFG_CMDID		= 0x0821,
-	WMI_RX_ON_CMDID			= 0x0822,
 	WMI_VRING_BA_EN_CMDID		= 0x0823,
 	WMI_VRING_BA_DIS_CMDID		= 0x0824,
 	WMI_RCP_ADDBA_RESP_CMDID	= 0x0825,
@@ -87,7 +87,6 @@ enum wmi_command_id {
 	WMI_SET_PCP_CHANNEL_CMDID	= 0x0829,
 	WMI_GET_PCP_CHANNEL_CMDID	= 0x082a,
 	WMI_SW_TX_REQ_CMDID		= 0x082b,
-	WMI_RX_OFF_CMDID		= 0x082c,
 	WMI_READ_MAC_RXQ_CMDID		= 0x0830,
 	WMI_READ_MAC_TXQ_CMDID		= 0x0831,
 	WMI_WRITE_MAC_RXQ_CMDID		= 0x0832,
@@ -112,6 +111,18 @@ enum wmi_command_id {
 	WMI_FLASH_READ_CMDID		= 0x0902,
 	WMI_FLASH_WRITE_CMDID		= 0x0903,
 	WMI_SECURITY_UNIT_TEST_CMDID	= 0x0904,
+	/*P2P*/
+	WMI_P2P_CFG_CMDID		= 0x0910,
+	WMI_PORT_ALLOCATE_CMDID		= 0x0911,
+	WMI_PORT_DELETE_CMDID		= 0x0912,
+	WMI_POWER_MGMT_CFG_CMDID	= 0x0913,
+	WMI_START_LISTEN_CMDID		= 0x0914,
+	WMI_START_SEARCH_CMDID		= 0x0915,
+	WMI_DISCOVERY_START_CMDID	= 0x0916,
+	WMI_DISCOVERY_STOP_CMDID	= 0x0917,
+	WMI_PCP_START_CMDID		= 0x0918,
+	WMI_PCP_STOP_CMDID		= 0x0919,
+	WMI_GET_PCP_FACTOR_CMDID	= 0x091b,
 
 	WMI_SET_MAC_ADDRESS_CMDID	= 0xf003,
 	WMI_ABORT_SCAN_CMDID		= 0xf007,
@@ -132,18 +143,6 @@ enum wmi_command_id {
  */
 
 /*
- * Frame Types
- */
-enum wmi_mgmt_frame_type {
-	WMI_FRAME_BEACON	= 0,
-	WMI_FRAME_PROBE_REQ	= 1,
-	WMI_FRAME_PROBE_RESP	= 2,
-	WMI_FRAME_ASSOC_REQ	= 3,
-	WMI_FRAME_ASSOC_RESP	= 4,
-	WMI_NUM_MGMT_FRAME,
-};
-
-/*
  * WMI_CONNECT_CMDID
  */
 enum wmi_network_type {
@@ -184,7 +183,7 @@ enum wmi_crypto_type {
 enum wmi_connect_ctrl_flag_bits {
 	WMI_CONNECT_ASSOC_POLICY_USER		= 0x0001,
 	WMI_CONNECT_SEND_REASSOC		= 0x0002,
-	WMI_CONNECT_IGNORE_WPAx_GROUP_CIPHER	= 0x0004,
+	WMI_CONNECT_IGNORE_WPA_GROUP_CIPHER	= 0x0004,
 	WMI_CONNECT_PROFILE_MATCH_DONE		= 0x0008,
 	WMI_CONNECT_IGNORE_AAC_BEACON		= 0x0010,
 	WMI_CONNECT_CSA_FOLLOW_BSS		= 0x0020,
@@ -212,6 +211,13 @@ struct wmi_connect_cmd {
 	u8 reserved1[2];
 } __packed;
 
+/*
+ * WMI_DISCONNECT_STA_CMDID
+ */
+struct wmi_disconnect_sta_cmd {
+	u8 dst_mac[WMI_MAC_LEN];
+	__le16 disconnect_reason;
+} __packed;
 
 /*
  * WMI_RECONNECT_CMDID
@@ -289,10 +295,12 @@ struct wmi_delete_cipher_key_cmd {
 enum wmi_scan_type {
 	WMI_LONG_SCAN		= 0,
 	WMI_SHORT_SCAN		= 1,
+	WMI_PBC_SCAN		= 2,
 };
 
 struct wmi_start_scan_cmd {
 	u8 reserved[8];
+
 	__le32 home_dwell_time;	/* Max duration in the home channel(ms) */
 	__le32 force_scan_interval;	/* Time interval between scans (ms)*/
 	u8 scan_type;		/* wmi_scan_type */
@@ -309,7 +317,7 @@ struct wmi_start_scan_cmd {
 /*
  * WMI_SET_PROBED_SSID_CMDID
  */
-#define MAX_PROBED_SSID_INDEX   (15)
+#define MAX_PROBED_SSID_INDEX	(3)
 
 enum wmi_ssid_flag {
 	WMI_SSID_FLAG_DISABLE	= 0,	/* disables entry */
@@ -328,6 +336,20 @@ struct wmi_probed_ssid_cmd {
  * WMI_SET_APPIE_CMDID
  * Add Application specified IE to a management frame
  */
+#define WMI_MAX_IE_LEN		(1024)
+
+/*
+ * Frame Types
+ */
+enum wmi_mgmt_frame_type {
+	WMI_FRAME_BEACON	= 0,
+	WMI_FRAME_PROBE_REQ	= 1,
+	WMI_FRAME_PROBE_RESP	= 2,
+	WMI_FRAME_ASSOC_REQ	= 3,
+	WMI_FRAME_ASSOC_RESP	= 4,
+	WMI_NUM_MGMT_FRAME,
+};
+
 struct wmi_set_appie_cmd {
 	u8 mgmt_frm_type;	/* enum wmi_mgmt_frame_type */
 	u8 reserved;
@@ -335,13 +357,18 @@ struct wmi_set_appie_cmd {
 	u8 ie_info[0];
 } __packed;
 
-#define WMI_MAX_IE_LEN (1024)
 
+/*
+ * WMI_PXMT_RANGE_CFG_CMDID
+ */
 struct wmi_pxmt_range_cfg_cmd {
 	u8 dst_mac[WMI_MAC_LEN];
 	__le16 range;
 } __packed;
 
+/*
+ * WMI_PXMT_SNR2_RANGE_CFG_CMDID
+ */
 struct wmi_pxmt_snr2_range_cfg_cmd {
 	s8 snr2range_arr[WMI_PROX_RANGE_NUM-1];
 } __packed;
@@ -359,6 +386,23 @@ struct wmi_rf_mgmt_cmd {
 	__le32 rf_mgmt_type;
 } __packed;
 
+
+/*
+ * WMI_RF_RX_TEST_CMDID
+ */
+struct wmi_rf_rx_test_cmd {
+	__le32 sector;
+} __packed;
+
+/*
+ * WMI_CORR_MEASURE_CMDID
+ */
+struct wmi_corr_measure_cmd {
+	s32 freq_mhz;
+	__le32 length_samples;
+	__le32 iterations;
+} __packed;
+
 /*
  * WMI_SET_SSID_CMDID
  */
@@ -388,6 +432,74 @@ struct wmi_bcon_ctrl_cmd {
 	u8 disable_sec;
 } __packed;
 
+
+/******* P2P ***********/
+
+/*
+ * WMI_PORT_ALLOCATE_CMDID
+ */
+enum wmi_port_role {
+	WMI_PORT_STA		= 0,
+	WMI_PORT_PCP		= 1,
+	WMI_PORT_AP		= 2,
+	WMI_PORT_P2P_DEV	= 3,
+	WMI_PORT_P2P_CLIENT	= 4,
+	WMI_PORT_P2P_GO		= 5,
+};
+
+struct wmi_port_allocate_cmd {
+	u8 mac[WMI_MAC_LEN];
+	u8 port_role;
+	u8 midid;
+} __packed;
+
+/*
+ * WMI_PORT_DELETE_CMDID
+ */
+struct wmi_delete_port_cmd {
+	u8 mid;
+	u8 reserved[3];
+} __packed;
+
+/*
+ * WMI_P2P_CFG_CMDID
+ */
+enum wmi_discovery_mode {
+	WMI_DISCOVERY_MODE_NON_OFFLOAD	= 0,
+	WMI_DISCOVERY_MODE_OFFLOAD	= 1,
+};
+
+struct wmi_p2p_cfg_cmd {
+	u8 discovery_mode;	/* wmi_discovery_mode */
+	u8 channel;
+	__le16 bcon_interval; /* base to listen/search duration calculation */
+} __packed;
+
+/*
+ * WMI_POWER_MGMT_CFG_CMDID
+ */
+enum wmi_power_source_type {
+	WMI_POWER_SOURCE_BATTERY	= 0,
+	WMI_POWER_SOURCE_OTHER		= 1,
+};
+
+struct wmi_power_mgmt_cfg_cmd {
+	u8 power_source;	/* wmi_power_source_type */
+	u8 reserved[3];
+} __packed;
+
+/*
+ * WMI_PCP_START_CMDID
+ */
+struct wmi_pcp_start_cmd {
+	__le16 bcon_interval;
+	u8 reserved0[10];
+	u8 network_type;
+	u8 channel;
+	u8 disable_sec_offload;
+	u8 disable_sec;
+} __packed;
+
 /*
  * WMI_SW_TX_REQ_CMDID
  */
@@ -435,16 +547,17 @@ enum wmi_vring_cfg_schd_params_priority {
 	WMI_SCH_PRIO_HIGH			= 1,
 };
 
+#define CIDXTID_CID_POS (0)
+#define CIDXTID_CID_LEN (4)
+#define CIDXTID_CID_MSK (0xF)
+#define CIDXTID_TID_POS (4)
+#define CIDXTID_TID_LEN (4)
+#define CIDXTID_TID_MSK (0xF0)
+
 struct wmi_vring_cfg {
 	struct wmi_sw_ring_cfg tx_sw_ring;
 	u8 ringid;				/* 0-23 vrings */
 
-	#define CIDXTID_CID_POS (0)
-	#define CIDXTID_CID_LEN (4)
-	#define CIDXTID_CID_MSK (0xF)
-	#define CIDXTID_TID_POS (4)
-	#define CIDXTID_TID_LEN (4)
-	#define CIDXTID_TID_MSK (0xF0)
 	u8 cidxtid;
 
 	u8 encap_trans_type;
@@ -501,8 +614,14 @@ struct wmi_vring_ba_dis_cmd {
  */
 struct wmi_notify_req_cmd {
 	u8 cid;
-	u8 reserved[3];
+	u8 year;
+	u8 month;
+	u8 day;
 	__le32 interval_usec;
+	u8 hour;
+	u8 minute;
+	u8 second;
+	u8 miliseconds;
 } __packed;
 
 /*
@@ -548,6 +667,11 @@ enum wmi_cfg_rx_chain_cmd_nwifi_ds_trans_type {
 	WMI_NWIFI_RX_TRANS_MODE_PBSS2STA	= 2,
 };
 
+enum wmi_cfg_rx_chain_cmd_reorder_type {
+	WMI_RX_HW_REORDER = 0,
+	WMI_RX_SW_REORDER = 1,
+};
+
 struct wmi_cfg_rx_chain_cmd {
 	__le32 action;
 	struct wmi_sw_ring_cfg rx_sw_ring;
@@ -596,7 +720,8 @@ struct wmi_cfg_rx_chain_cmd {
 	__le16 wb_thrsh;
 	__le32 itr_value;
 	__le16 host_thrsh;
-	u8 reserved[2];
+	u8 reorder_type;
+	u8 reserved;
 	struct wmi_sniffer_cfg sniffer_cfg;
 } __packed;
 
@@ -604,15 +729,7 @@ struct wmi_cfg_rx_chain_cmd {
  * WMI_RCP_ADDBA_RESP_CMDID
  */
 struct wmi_rcp_addba_resp_cmd {
-
-	#define CIDXTID_CID_POS (0)
-	#define CIDXTID_CID_LEN (4)
-	#define CIDXTID_CID_MSK (0xF)
-	#define CIDXTID_TID_POS (4)
-	#define CIDXTID_TID_LEN (4)
-	#define CIDXTID_TID_MSK (0xF0)
 	u8 cidxtid;
-
 	u8 dialog_token;
 	__le16 status_code;
 	__le16 ba_param_set;	/* ieee80211_ba_parameterset field to send */
@@ -623,15 +740,7 @@ struct wmi_rcp_addba_resp_cmd {
  * WMI_RCP_DELBA_CMDID
  */
 struct wmi_rcp_delba_cmd {
-
-	#define CIDXTID_CID_POS (0)
-	#define CIDXTID_CID_LEN (4)
-	#define CIDXTID_CID_MSK (0xF)
-	#define CIDXTID_TID_POS (4)
-	#define CIDXTID_TID_LEN (4)
-	#define CIDXTID_TID_MSK (0xF0)
 	u8 cidxtid;
-
 	u8 reserved;
 	__le16 reason;
 } __packed;
@@ -640,15 +749,7 @@ struct wmi_rcp_delba_cmd {
  * WMI_RCP_ADDBA_REQ_CMDID
  */
 struct wmi_rcp_addba_req_cmd {
-
-	#define CIDXTID_CID_POS (0)
-	#define CIDXTID_CID_LEN (4)
-	#define CIDXTID_CID_MSK (0xF)
-	#define CIDXTID_TID_POS (4)
-	#define CIDXTID_TID_LEN (4)
-	#define CIDXTID_TID_MSK (0xF0)
 	u8 cidxtid;
-
 	u8 dialog_token;
 	/* ieee80211_ba_parameterset field as it received */
 	__le16 ba_param_set;
@@ -665,7 +766,6 @@ struct wmi_set_mac_address_cmd {
 	u8 reserved[2];
 } __packed;
 
-
 /*
 * WMI_EAPOL_TX_CMDID
 */
@@ -692,6 +792,17 @@ struct wmi_echo_cmd {
 } __packed;
 
 /*
+ * WMI_TEMP_SENSE_CMDID
+ *
+ * Measure MAC and radio temperatures
+ */
+struct wmi_temp_sense_cmd {
+	__le32 measure_marlon_m_en;
+	__le32 measure_marlon_r_en;
+} __packed;
+
+
+/*
  * WMI Events
  */
 
@@ -699,7 +810,6 @@ struct wmi_echo_cmd {
  * List of Events (target to host)
  */
 enum wmi_event_id {
-	WMI_IMM_RSP_EVENTID			= 0x0000,
 	WMI_READY_EVENTID			= 0x1001,
 	WMI_CONNECT_EVENTID			= 0x1002,
 	WMI_DISCONNECT_EVENTID			= 0x1003,
@@ -709,13 +819,9 @@ enum wmi_event_id {
 	WMI_FW_READY_EVENTID			= 0x1801,
 	WMI_EXIT_FAST_MEM_ACC_MODE_EVENTID	= 0x0200,
 	WMI_ECHO_RSP_EVENTID			= 0x1803,
-	WMI_CONFIG_MAC_DONE_EVENTID		= 0x1805,
-	WMI_CONFIG_PHY_DEBUG_DONE_EVENTID	= 0x1806,
-	WMI_ADD_STATION_DONE_EVENTID		= 0x1807,
-	WMI_ADD_DEBUG_TX_PCKT_DONE_EVENTID	= 0x1808,
-	WMI_PHY_GET_STATISTICS_EVENTID		= 0x1809,
 	WMI_FS_TUNE_DONE_EVENTID		= 0x180a,
-	WMI_CORR_MEASURE_DONE_EVENTID		= 0x180b,
+	WMI_CORR_MEASURE_EVENTID		= 0x180b,
+	WMI_READ_RSSI_EVENTID			= 0x180c,
 	WMI_TEMP_SENSE_DONE_EVENTID		= 0x180e,
 	WMI_DC_CALIB_DONE_EVENTID		= 0x180f,
 	WMI_IQ_TX_CALIB_DONE_EVENTID		= 0x1811,
@@ -727,10 +833,9 @@ enum wmi_event_id {
 	WMI_MARLON_R_WRITE_DONE_EVENTID		= 0x1819,
 	WMI_MARLON_R_TXRX_SEL_DONE_EVENTID	= 0x181a,
 	WMI_SILENT_RSSI_CALIB_DONE_EVENTID	= 0x181d,
-
+	WMI_RF_RX_TEST_DONE_EVENTID		= 0x181e,
 	WMI_CFG_RX_CHAIN_DONE_EVENTID		= 0x1820,
 	WMI_VRING_CFG_DONE_EVENTID		= 0x1821,
-	WMI_RX_ON_DONE_EVENTID			= 0x1822,
 	WMI_BA_STATUS_EVENTID			= 0x1823,
 	WMI_RCP_ADDBA_REQ_EVENTID		= 0x1824,
 	WMI_ADDBA_RESP_SENT_EVENTID		= 0x1825,
@@ -738,7 +843,6 @@ enum wmi_event_id {
 	WMI_GET_SSID_EVENTID			= 0x1828,
 	WMI_GET_PCP_CHANNEL_EVENTID		= 0x182a,
 	WMI_SW_TX_COMPLETE_EVENTID		= 0x182b,
-	WMI_RX_OFF_DONE_EVENTID			= 0x182c,
 
 	WMI_READ_MAC_RXQ_EVENTID		= 0x1830,
 	WMI_READ_MAC_TXQ_EVENTID		= 0x1831,
@@ -765,7 +869,16 @@ enum wmi_event_id {
 	WMI_UNIT_TEST_EVENTID			= 0x1900,
 	WMI_FLASH_READ_DONE_EVENTID		= 0x1902,
 	WMI_FLASH_WRITE_DONE_EVENTID		= 0x1903,
-
+	/*P2P*/
+	WMI_PORT_ALLOCATED_EVENTID		= 0x1911,
+	WMI_PORT_DELETED_EVENTID		= 0x1912,
+	WMI_LISTEN_STARTED_EVENTID		= 0x1914,
+	WMI_SEARCH_STARTED_EVENTID		= 0x1915,
+	WMI_DISCOVERY_STARTED_EVENTID		= 0x1916,
+	WMI_DISCOVERY_STOPPED_EVENTID		= 0x1917,
+	WMI_PCP_STARTED_EVENTID			= 0x1918,
+	WMI_PCP_STOPPED_EVENTID			= 0x1919,
+	WMI_PCP_FACTOR_EVENTID			= 0x191a,
 	WMI_SET_CHANNEL_EVENTID			= 0x9000,
 	WMI_ASSOC_REQ_EVENTID			= 0x9001,
 	WMI_EAPOL_RX_EVENTID			= 0x9002,
@@ -777,6 +890,12 @@ enum wmi_event_id {
  * Events data structures
  */
 
+
+enum wmi_fw_status {
+	WMI_FW_STATUS_SUCCESS,
+	WMI_FW_STATUS_FAILURE,
+};
+
 /*
  * WMI_RF_MGMT_STATUS_EVENTID
  */
@@ -857,7 +976,7 @@ struct wmi_ready_event {
 	__le32 abi_version;
 	u8 mac[WMI_MAC_LEN];
 	u8 phy_capability;		/* enum wmi_phy_capability */
-	u8 reserved;
+	u8 numof_additional_mids;
 } __packed;
 
 /*
@@ -876,6 +995,8 @@ struct wmi_notify_req_done_event {
 	__le16 other_rx_sector;
 	__le16 other_tx_sector;
 	__le16 range;
+	u8 sqi;
+	u8 reserved[3];
 } __packed;
 
 /*
@@ -951,27 +1072,15 @@ struct wmi_vring_ba_status_event {
  * WMI_DELBA_EVENTID
  */
 struct wmi_delba_event {
-
-	#define CIDXTID_CID_POS (0)
-	#define CIDXTID_CID_LEN (4)
-	#define CIDXTID_CID_MSK (0xF)
-	#define CIDXTID_TID_POS (4)
-	#define CIDXTID_TID_LEN (4)
-	#define CIDXTID_TID_MSK (0xF0)
 	u8 cidxtid;
-
 	u8 from_initiator;
 	__le16 reason;
 } __packed;
 
+
 /*
  * WMI_VRING_CFG_DONE_EVENTID
  */
-enum wmi_vring_cfg_done_event_status {
-	WMI_VRING_CFG_SUCCESS		= 0,
-	WMI_VRING_CFG_FAILURE		= 1,
-};
-
 struct wmi_vring_cfg_done_event {
 	u8 ringid;
 	u8 status;
@@ -982,21 +1091,8 @@ struct wmi_vring_cfg_done_event {
 /*
  * WMI_ADDBA_RESP_SENT_EVENTID
  */
-enum wmi_rcp_addba_resp_sent_event_status {
-	WMI_ADDBA_SUCCESS		= 0,
-	WMI_ADDBA_FAIL			= 1,
-};
-
 struct wmi_rcp_addba_resp_sent_event {
-
-	#define CIDXTID_CID_POS (0)
-	#define CIDXTID_CID_LEN (4)
-	#define CIDXTID_CID_MSK (0xF)
-	#define CIDXTID_TID_POS (4)
-	#define CIDXTID_TID_LEN (4)
-	#define CIDXTID_TID_MSK (0xF0)
 	u8 cidxtid;
-
 	u8 reserved;
 	__le16 status;
 } __packed;
@@ -1005,15 +1101,7 @@ struct wmi_rcp_addba_resp_sent_event {
  * WMI_RCP_ADDBA_REQ_EVENTID
  */
 struct wmi_rcp_addba_req_event {
-
-	#define CIDXTID_CID_POS (0)
-	#define CIDXTID_CID_LEN (4)
-	#define CIDXTID_CID_MSK (0xF)
-	#define CIDXTID_TID_POS (4)
-	#define CIDXTID_TID_LEN (4)
-	#define CIDXTID_TID_MSK (0xF0)
 	u8 cidxtid;
-
 	u8 dialog_token;
 	__le16 ba_param_set;	/* ieee80211_ba_parameterset as it received */
 	__le16 ba_timeout;
@@ -1055,6 +1143,7 @@ struct wmi_data_port_open_event {
 	u8 reserved[3];
 } __packed;
 
+
 /*
  * WMI_GET_PCP_CHANNEL_EVENTID
  */
@@ -1063,6 +1152,54 @@ struct wmi_get_pcp_channel_event {
 	u8 reserved[3];
 } __packed;
 
+
+/*
+* WMI_PORT_ALLOCATED_EVENTID
+*/
+struct wmi_port_allocated_event {
+	u8 status;	/* wmi_fw_status */
+	u8 reserved[3];
+} __packed;
+
+/*
+* WMI_PORT_DELETED_EVENTID
+*/
+struct wmi_port_deleted_event {
+	u8 status;	/* wmi_fw_status */
+	u8 reserved[3];
+} __packed;
+
+/*
+ * WMI_LISTEN_STARTED_EVENTID
+ */
+struct wmi_listen_started_event {
+	u8 status;	/* wmi_fw_status */
+	u8 reserved[3];
+} __packed;
+
+/*
+ * WMI_SEARCH_STARTED_EVENTID
+ */
+struct wmi_search_started_event {
+	u8 status;	/* wmi_fw_status */
+	u8 reserved[3];
+} __packed;
+
+/*
+ * WMI_PCP_STARTED_EVENTID
+ */
+struct wmi_pcp_started_event {
+	u8 status;	/* wmi_fw_status */
+	u8 reserved[3];
+} __packed;
+
+/*
+ * WMI_PCP_FACTOR_EVENTID
+ */
+struct wmi_pcp_factor_event {
+	__le32 pcp_factor;
+} __packed;
+
 /*
  * WMI_SW_TX_COMPLETE_EVENTID
  */
@@ -1078,6 +1215,23 @@ struct wmi_sw_tx_complete_event {
 } __packed;
 
 /*
+ * WMI_CORR_MEASURE_EVENTID
+ */
+struct wmi_corr_measure_event {
+	s32 i;
+	s32 q;
+	s32 image_i;
+	s32 image_q;
+} __packed;
+
+/*
+ * WMI_READ_RSSI_EVENTID
+ */
+struct wmi_read_rssi_event {
+	__le32 ina_rssi_adc_dbm;
+} __packed;
+
+/*
  * WMI_GET_SSID_EVENTID
  */
 struct wmi_get_ssid_event {
@@ -1091,7 +1245,8 @@ struct wmi_get_ssid_event {
 struct wmi_rx_mgmt_info {
 	u8 mcs;
 	s8 snr;
-	__le16 range;
+	u8 range;
+	u8 sqi;
 	__le16 stype;
 	__le16 status;
 	__le32 len;
@@ -1113,4 +1268,14 @@ struct wmi_echo_event {
 	__le32 echoed_value;
 } __packed;
 
+/*
+ * WMI_TEMP_SENSE_DONE_EVENTID
+ *
+ * Measure MAC and radio temperatures
+ */
+struct wmi_temp_sense_done_event {
+	__le32 marlon_m_t1000;
+	__le32 marlon_r_t1000;
+} __packed;
+
 #endif /* __WILOCITY_WMI_H__ */
diff --git a/drivers/net/wireless/atmel.c b/drivers/net/wireless/atmel.c
index 4374079dfc2a..830bb1d1f957 100644
--- a/drivers/net/wireless/atmel.c
+++ b/drivers/net/wireless/atmel.c
@@ -63,6 +63,7 @@
 #include <net/iw_handler.h>
 #include <linux/crc32.h>
 #include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 #include <linux/device.h>
 #include <linux/moduleparam.h>
 #include <linux/firmware.h>
@@ -1409,30 +1410,28 @@ static int atmel_validate_channel(struct atmel_private *priv, int channel)
 	return 0;
 }
 
-static int atmel_proc_output (char *buf, struct atmel_private *priv)
+static int atmel_proc_show(struct seq_file *m, void *v)
 {
+	struct atmel_private *priv = m->private;
 	int i;
-	char *p = buf;
 	char *s, *r, *c;
 
-	p += sprintf(p, "Driver version:\t\t%d.%d\n",
-		     DRIVER_MAJOR, DRIVER_MINOR);
+	seq_printf(m, "Driver version:\t\t%d.%d\n", DRIVER_MAJOR, DRIVER_MINOR);
 
 	if (priv->station_state != STATION_STATE_DOWN) {
-		p += sprintf(p, "Firmware version:\t%d.%d build %d\n"
-				"Firmware location:\t",
-			     priv->host_info.major_version,
-			     priv->host_info.minor_version,
-			     priv->host_info.build_version);
+		seq_printf(m,
+			   "Firmware version:\t%d.%d build %d\n"
+			   "Firmware location:\t",
+			   priv->host_info.major_version,
+			   priv->host_info.minor_version,
+			   priv->host_info.build_version);
 
 		if (priv->card_type != CARD_TYPE_EEPROM)
-			p += sprintf(p, "on card\n");
+			seq_puts(m, "on card\n");
 		else if (priv->firmware)
-			p += sprintf(p, "%s loaded by host\n",
-				     priv->firmware_id);
+			seq_printf(m, "%s loaded by host\n", priv->firmware_id);
 		else
-			p += sprintf(p, "%s loaded by hotplug\n",
-				     priv->firmware_id);
+			seq_printf(m, "%s loaded by hotplug\n", priv->firmware_id);
 
 		switch (priv->card_type) {
 		case CARD_TYPE_PARALLEL_FLASH:
@@ -1453,12 +1452,12 @@ static int atmel_proc_output (char *buf, struct atmel_private *priv)
 			if (priv->reg_domain == channel_table[i].reg_domain)
 				r = channel_table[i].name;
 
-		p += sprintf(p, "MAC memory type:\t%s\n", c);
-		p += sprintf(p, "Regulatory domain:\t%s\n", r);
-		p += sprintf(p, "Host CRC checking:\t%s\n",
-			     priv->do_rx_crc ? "On" : "Off");
-		p += sprintf(p, "WPA-capable firmware:\t%s\n",
-			     priv->use_wpa ? "Yes" : "No");
+		seq_printf(m, "MAC memory type:\t%s\n", c);
+		seq_printf(m, "Regulatory domain:\t%s\n", r);
+		seq_printf(m, "Host CRC checking:\t%s\n",
+			 priv->do_rx_crc ? "On" : "Off");
+		seq_printf(m, "WPA-capable firmware:\t%s\n",
+			 priv->use_wpa ? "Yes" : "No");
 	}
 
 	switch (priv->station_state) {
@@ -1490,26 +1489,22 @@ static int atmel_proc_output (char *buf, struct atmel_private *priv)
 		s = "<unknown>";
 	}
 
-	p += sprintf(p, "Current state:\t\t%s\n", s);
-	return p - buf;
+	seq_printf(m, "Current state:\t\t%s\n", s);
+	return 0;
 }
 
-static int atmel_read_proc(char *page, char **start, off_t off,
-			   int count, int *eof, void *data)
+static int atmel_proc_open(struct inode *inode, struct file *file)
 {
-	struct atmel_private *priv = data;
-	int len = atmel_proc_output (page, priv);
-	if (len <= off+count)
-		*eof = 1;
-	*start = page + off;
-	len -= off;
-	if (len > count)
-		len = count;
-	if (len < 0)
-		len = 0;
-	return len;
+	return single_open(file, atmel_proc_show, PDE_DATA(inode));
 }
 
+static const struct file_operations atmel_proc_fops = {
+	.open		= atmel_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
 static const struct net_device_ops atmel_netdev_ops = {
 	.ndo_open 		= atmel_open,
 	.ndo_stop		= atmel_close,
@@ -1525,7 +1520,6 @@ struct net_device *init_atmel_card(unsigned short irq, unsigned long port,
 				   struct device *sys_dev,
 				   int (*card_present)(void *), void *card)
 {
-	struct proc_dir_entry *ent;
 	struct net_device *dev;
 	struct atmel_private *priv;
 	int rc;
@@ -1630,8 +1624,7 @@ struct net_device *init_atmel_card(unsigned short irq, unsigned long port,
 
 	netif_carrier_off(dev);
 
-	ent = create_proc_read_entry ("driver/atmel", 0, NULL, atmel_read_proc, priv);
-	if (!ent)
+	if (!proc_create_data("driver/atmel", 0, NULL, &atmel_proc_fops, priv));
 		printk(KERN_WARNING "atmel: unable to create /proc entry.\n");
 
 	printk(KERN_INFO "%s: Atmel at76c50x. Version %d.%d. MAC %pM\n",
diff --git a/drivers/net/wireless/b43/Kconfig b/drivers/net/wireless/b43/Kconfig
index 287c6b670a36..078e6f3477a9 100644
--- a/drivers/net/wireless/b43/Kconfig
+++ b/drivers/net/wireless/b43/Kconfig
@@ -131,7 +131,7 @@ config B43_PHY_LP
 
 config B43_PHY_HT
 	bool "Support for HT-PHY (high throughput) devices"
-	depends on B43
+	depends on B43 && B43_BCMA
 	---help---
 	  Support for the HT-PHY.
 
@@ -166,8 +166,8 @@ config B43_DEBUG
 	  Broadcom 43xx debugging.
 
 	  This adds additional runtime sanity checks and statistics to the driver.
-	  These checks and statistics might me expensive and hurt runtime performance
-	  of your system.
+	  These checks and statistics might be expensive and hurt the runtime
+	  performance of your system.
 	  This also adds the b43 debugfs interface.
 
 	  Do not enable this, unless you are debugging the driver.
diff --git a/drivers/net/wireless/b43/b43.h b/drivers/net/wireless/b43/b43.h
index 10e288d470e7..7f3d461f7e8d 100644
--- a/drivers/net/wireless/b43/b43.h
+++ b/drivers/net/wireless/b43/b43.h
@@ -285,7 +285,9 @@ enum {
 #define B43_SHM_SH_DTIMPER		0x0012	/* DTIM period */
 #define B43_SHM_SH_NOSLPZNATDTIM	0x004C	/* NOSLPZNAT DTIM */
 /* SHM_SHARED beacon/AP variables */
+#define B43_SHM_SH_BT_BASE0		0x0068	/* Beacon template base 0 */
 #define B43_SHM_SH_BTL0			0x0018	/* Beacon template length 0 */
+#define B43_SHM_SH_BT_BASE1		0x0468	/* Beacon template base 1 */
 #define B43_SHM_SH_BTL1			0x001A	/* Beacon template length 1 */
 #define B43_SHM_SH_BTSFOFF		0x001C	/* Beacon TSF offset */
 #define B43_SHM_SH_TIMBPOS		0x001E	/* TIM B position in beacon */
@@ -473,6 +475,12 @@ enum {
 #define B43_MACCMD_CCA			0x00000008	/* Clear channel assessment */
 #define B43_MACCMD_BGNOISE		0x00000010	/* Background noise */
 
+/* See BCMA_CLKCTLST_EXTRESREQ and BCMA_CLKCTLST_EXTRESST */
+#define B43_BCMA_CLKCTLST_80211_PLL_REQ	0x00000100
+#define B43_BCMA_CLKCTLST_PHY_PLL_REQ	0x00000200
+#define B43_BCMA_CLKCTLST_80211_PLL_ST	0x01000000
+#define B43_BCMA_CLKCTLST_PHY_PLL_ST	0x02000000
+
 /* BCMA 802.11 core specific IO Control (BCMA_IOCTL) flags */
 #define B43_BCMA_IOCTL_PHY_CLKEN	0x00000004	/* PHY Clock Enable */
 #define B43_BCMA_IOCTL_PHY_RESET	0x00000008	/* PHY Reset */
@@ -972,7 +980,7 @@ static inline int b43_is_mode(struct b43_wl *wl, int type)
  */
 static inline enum ieee80211_band b43_current_band(struct b43_wl *wl)
 {
-	return wl->hw->conf.channel->band;
+	return wl->hw->conf.chandef.chan->band;
 }
 
 static inline int b43_bus_may_powerdown(struct b43_wldev *wldev)
diff --git a/drivers/net/wireless/b43/dma.c b/drivers/net/wireless/b43/dma.c
index 122146943bf2..523355b87659 100644
--- a/drivers/net/wireless/b43/dma.c
+++ b/drivers/net/wireless/b43/dma.c
@@ -419,8 +419,6 @@ static inline
 
 static int alloc_ringmemory(struct b43_dmaring *ring)
 {
-	gfp_t flags = GFP_KERNEL;
-
 	/* The specs call for 4K buffers for 30- and 32-bit DMA with 4K
 	 * alignment and 8K buffers for 64-bit DMA with 8K alignment.
 	 * In practice we could use smaller buffers for the latter, but the
@@ -435,12 +433,9 @@ static int alloc_ringmemory(struct b43_dmaring *ring)
 
 	ring->descbase = dma_alloc_coherent(ring->dev->dev->dma_dev,
 					    ring_mem_size, &(ring->dmabase),
-					    flags);
-	if (!ring->descbase) {
-		b43err(ring->dev->wl, "DMA ringmemory allocation failed\n");
+					    GFP_KERNEL | __GFP_ZERO);
+	if (!ring->descbase)
 		return -ENOMEM;
-	}
-	memset(ring->descbase, 0, ring_mem_size);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index 05682736e466..d377f77d30b5 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -1189,10 +1189,15 @@ static void b43_bcma_phy_reset(struct b43_wldev *dev)
 
 static void b43_bcma_wireless_core_reset(struct b43_wldev *dev, bool gmode)
 {
+	u32 req = B43_BCMA_CLKCTLST_80211_PLL_REQ |
+		  B43_BCMA_CLKCTLST_PHY_PLL_REQ;
+	u32 status = B43_BCMA_CLKCTLST_80211_PLL_ST |
+		     B43_BCMA_CLKCTLST_PHY_PLL_ST;
+
 	b43_device_enable(dev, B43_BCMA_IOCTL_PHY_CLKEN);
 	bcma_core_set_clockmode(dev->dev->bdev, BCMA_CLKMODE_FAST);
 	b43_bcma_phy_reset(dev);
-	bcma_core_pll_ctl(dev->dev->bdev, 0x300, 0x3000000, true);
+	bcma_core_pll_ctl(dev->dev->bdev, req, status, true);
 }
 #endif
 
@@ -1305,17 +1310,19 @@ static u32 b43_jssi_read(struct b43_wldev *dev)
 {
 	u32 val = 0;
 
-	val = b43_shm_read16(dev, B43_SHM_SHARED, 0x08A);
+	val = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_JSSI1);
 	val <<= 16;
-	val |= b43_shm_read16(dev, B43_SHM_SHARED, 0x088);
+	val |= b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_JSSI0);
 
 	return val;
 }
 
 static void b43_jssi_write(struct b43_wldev *dev, u32 jssi)
 {
-	b43_shm_write16(dev, B43_SHM_SHARED, 0x088, (jssi & 0x0000FFFF));
-	b43_shm_write16(dev, B43_SHM_SHARED, 0x08A, (jssi & 0xFFFF0000) >> 16);
+	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_JSSI0,
+			(jssi & 0x0000FFFF));
+	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_JSSI1,
+			(jssi & 0xFFFF0000) >> 16);
 }
 
 static void b43_generate_noise_sample(struct b43_wldev *dev)
@@ -1618,7 +1625,7 @@ static void b43_upload_beacon0(struct b43_wldev *dev)
 
 	if (wl->beacon0_uploaded)
 		return;
-	b43_write_beacon_template(dev, 0x68, 0x18);
+	b43_write_beacon_template(dev, B43_SHM_SH_BT_BASE0, B43_SHM_SH_BTL0);
 	wl->beacon0_uploaded = true;
 }
 
@@ -1628,7 +1635,7 @@ static void b43_upload_beacon1(struct b43_wldev *dev)
 
 	if (wl->beacon1_uploaded)
 		return;
-	b43_write_beacon_template(dev, 0x468, 0x1A);
+	b43_write_beacon_template(dev, B43_SHM_SH_BT_BASE1, B43_SHM_SH_BTL1);
 	wl->beacon1_uploaded = true;
 }
 
@@ -2775,9 +2782,7 @@ static int b43_gpio_init(struct b43_wldev *dev)
 	switch (dev->dev->bus_type) {
 #ifdef CONFIG_B43_BCMA
 	case B43_BUS_BCMA:
-		bcma_cc_write32(&dev->dev->bdev->bus->drv_cc, BCMA_CC_GPIOCTL,
-				(bcma_cc_read32(&dev->dev->bdev->bus->drv_cc,
-					BCMA_CC_GPIOCTL) & ~mask) | set);
+		bcma_chipco_gpio_control(&dev->dev->bdev->bus->drv_cc, mask, set);
 		break;
 #endif
 #ifdef CONFIG_B43_SSB
@@ -2802,8 +2807,7 @@ static void b43_gpio_cleanup(struct b43_wldev *dev)
 	switch (dev->dev->bus_type) {
 #ifdef CONFIG_B43_BCMA
 	case B43_BUS_BCMA:
-		bcma_cc_write32(&dev->dev->bdev->bus->drv_cc, BCMA_CC_GPIOCTL,
-				0);
+		bcma_chipco_gpio_control(&dev->dev->bdev->bus->drv_cc, ~0, 0);
 		break;
 #endif
 #ifdef CONFIG_B43_SSB
@@ -3111,7 +3115,7 @@ static int b43_chip_init(struct b43_wldev *dev)
 
 	/* Probe Response Timeout value */
 	/* FIXME: Default to 0, has to be set by ioctl probably... :-/ */
-	b43_shm_write16(dev, B43_SHM_SHARED, 0x0074, 0x0000);
+	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRMAXTIME, 0);
 
 	/* Initially set the wireless operation mode. */
 	b43_adjust_opmode(dev);
@@ -3848,7 +3852,7 @@ static int b43_op_config(struct ieee80211_hw *hw, u32 changed)
 	dev = wl->current_dev;
 
 	/* Switch the band (if necessary). This might change the active core. */
-	err = b43_switch_band(wl, conf->channel);
+	err = b43_switch_band(wl, conf->chandef.chan);
 	if (err)
 		goto out_unlock_mutex;
 
@@ -3878,8 +3882,8 @@ static int b43_op_config(struct ieee80211_hw *hw, u32 changed)
 
 	/* Switch to the requested channel.
 	 * The firmware takes care of races with the TX handler. */
-	if (conf->channel->hw_value != phy->channel)
-		b43_switch_channel(dev, conf->channel->hw_value);
+	if (conf->chandef.chan->hw_value != phy->channel)
+		b43_switch_channel(dev, conf->chandef.chan->hw_value);
 
 	dev->wl->radiotap_enabled = !!(conf->flags & IEEE80211_CONF_MONITOR);
 
@@ -5002,7 +5006,7 @@ static int b43_op_get_survey(struct ieee80211_hw *hw, int idx,
 	if (idx != 0)
 		return -ENOENT;
 
-	survey->channel = conf->channel;
+	survey->channel = conf->chandef.chan;
 	survey->filled = SURVEY_INFO_NOISE_DBM;
 	survey->noise = dev->stats.link_noise;
 
diff --git a/drivers/net/wireless/b43/phy_ht.c b/drivers/net/wireless/b43/phy_ht.c
index 7416c5e9154d..5d6833f18498 100644
--- a/drivers/net/wireless/b43/phy_ht.c
+++ b/drivers/net/wireless/b43/phy_ht.c
@@ -30,6 +30,17 @@
 #include "radio_2059.h"
 #include "main.h"
 
+/* Force values to keep compatibility with wl */
+enum ht_rssi_type {
+	HT_RSSI_W1 = 0,
+	HT_RSSI_W2 = 1,
+	HT_RSSI_NB = 2,
+	HT_RSSI_IQ = 3,
+	HT_RSSI_TSSI_2G = 4,
+	HT_RSSI_TSSI_5G = 5,
+	HT_RSSI_TBD = 6,
+};
+
 /**************************************************
  * Radio 2059.
  **************************************************/
@@ -37,8 +48,9 @@
 static void b43_radio_2059_channel_setup(struct b43_wldev *dev,
 			const struct b43_phy_ht_channeltab_e_radio2059 *e)
 {
-	u8 i;
-	u16 routing;
+	static const u16 routing[] = { R2059_C1, R2059_C2, R2059_C3, };
+	u16 r;
+	int core;
 
 	b43_radio_write(dev, 0x16, e->radio_syn16);
 	b43_radio_write(dev, 0x17, e->radio_syn17);
@@ -53,25 +65,17 @@ static void b43_radio_2059_channel_setup(struct b43_wldev *dev,
 	b43_radio_write(dev, 0x41, e->radio_syn41);
 	b43_radio_write(dev, 0x43, e->radio_syn43);
 	b43_radio_write(dev, 0x47, e->radio_syn47);
-	b43_radio_write(dev, 0x4a, e->radio_syn4a);
-	b43_radio_write(dev, 0x58, e->radio_syn58);
-	b43_radio_write(dev, 0x5a, e->radio_syn5a);
-	b43_radio_write(dev, 0x6a, e->radio_syn6a);
-	b43_radio_write(dev, 0x6d, e->radio_syn6d);
-	b43_radio_write(dev, 0x6e, e->radio_syn6e);
-	b43_radio_write(dev, 0x92, e->radio_syn92);
-	b43_radio_write(dev, 0x98, e->radio_syn98);
-
-	for (i = 0; i < 2; i++) {
-		routing = i ? R2059_RXRX1 : R2059_TXRX0;
-		b43_radio_write(dev, routing | 0x4a, e->radio_rxtx4a);
-		b43_radio_write(dev, routing | 0x58, e->radio_rxtx58);
-		b43_radio_write(dev, routing | 0x5a, e->radio_rxtx5a);
-		b43_radio_write(dev, routing | 0x6a, e->radio_rxtx6a);
-		b43_radio_write(dev, routing | 0x6d, e->radio_rxtx6d);
-		b43_radio_write(dev, routing | 0x6e, e->radio_rxtx6e);
-		b43_radio_write(dev, routing | 0x92, e->radio_rxtx92);
-		b43_radio_write(dev, routing | 0x98, e->radio_rxtx98);
+
+	for (core = 0; core < 3; core++) {
+		r = routing[core];
+		b43_radio_write(dev, r | 0x4a, e->radio_rxtx4a);
+		b43_radio_write(dev, r | 0x58, e->radio_rxtx58);
+		b43_radio_write(dev, r | 0x5a, e->radio_rxtx5a);
+		b43_radio_write(dev, r | 0x6a, e->radio_rxtx6a);
+		b43_radio_write(dev, r | 0x6d, e->radio_rxtx6d);
+		b43_radio_write(dev, r | 0x6e, e->radio_rxtx6e);
+		b43_radio_write(dev, r | 0x92, e->radio_rxtx92);
+		b43_radio_write(dev, r | 0x98, e->radio_rxtx98);
 	}
 
 	udelay(50);
@@ -87,7 +91,7 @@ static void b43_radio_2059_channel_setup(struct b43_wldev *dev,
 
 static void b43_radio_2059_init(struct b43_wldev *dev)
 {
-	const u16 routing[] = { R2059_SYN, R2059_TXRX0, R2059_RXRX1 };
+	const u16 routing[] = { R2059_C1, R2059_C2, R2059_C3 };
 	const u16 radio_values[3][2] = {
 		{ 0x61, 0xE9 }, { 0x69, 0xD5 }, { 0x73, 0x99 },
 	};
@@ -106,17 +110,17 @@ static void b43_radio_2059_init(struct b43_wldev *dev)
 	b43_radio_mask(dev, 0xc0, ~0x0080);
 
 	if (1) { /* FIXME */
-		b43_radio_set(dev, R2059_RXRX1 | 0x4, 0x1);
+		b43_radio_set(dev, R2059_C3 | 0x4, 0x1);
 		udelay(10);
-		b43_radio_set(dev, R2059_RXRX1 | 0x0BF, 0x1);
-		b43_radio_maskset(dev, R2059_RXRX1 | 0x19B, 0x3, 0x2);
+		b43_radio_set(dev, R2059_C3 | 0x0BF, 0x1);
+		b43_radio_maskset(dev, R2059_C3 | 0x19B, 0x3, 0x2);
 
-		b43_radio_set(dev, R2059_RXRX1 | 0x4, 0x2);
+		b43_radio_set(dev, R2059_C3 | 0x4, 0x2);
 		udelay(100);
-		b43_radio_mask(dev, R2059_RXRX1 | 0x4, ~0x2);
+		b43_radio_mask(dev, R2059_C3 | 0x4, ~0x2);
 
 		for (i = 0; i < 10000; i++) {
-			if (b43_radio_read(dev, R2059_RXRX1 | 0x145) & 1) {
+			if (b43_radio_read(dev, R2059_C3 | 0x145) & 1) {
 				i = 0;
 				break;
 			}
@@ -125,7 +129,7 @@ static void b43_radio_2059_init(struct b43_wldev *dev)
 		if (i)
 			b43err(dev->wl, "radio 0x945 timeout\n");
 
-		b43_radio_mask(dev, R2059_RXRX1 | 0x4, ~0x1);
+		b43_radio_mask(dev, R2059_C3 | 0x4, ~0x1);
 		b43_radio_set(dev, 0xa, 0x60);
 
 		for (i = 0; i < 3; i++) {
@@ -154,9 +158,84 @@ static void b43_radio_2059_init(struct b43_wldev *dev)
 }
 
 /**************************************************
+ * RF
+ **************************************************/
+
+static void b43_phy_ht_force_rf_sequence(struct b43_wldev *dev, u16 rf_seq)
+{
+	u8 i;
+
+	u16 save_seq_mode = b43_phy_read(dev, B43_PHY_HT_RF_SEQ_MODE);
+	b43_phy_set(dev, B43_PHY_HT_RF_SEQ_MODE, 0x3);
+
+	b43_phy_set(dev, B43_PHY_HT_RF_SEQ_TRIG, rf_seq);
+	for (i = 0; i < 200; i++) {
+		if (!(b43_phy_read(dev, B43_PHY_HT_RF_SEQ_STATUS) & rf_seq)) {
+			i = 0;
+			break;
+		}
+		msleep(1);
+	}
+	if (i)
+		b43err(dev->wl, "Forcing RF sequence timeout\n");
+
+	b43_phy_write(dev, B43_PHY_HT_RF_SEQ_MODE, save_seq_mode);
+}
+
+static void b43_phy_ht_pa_override(struct b43_wldev *dev, bool enable)
+{
+	struct b43_phy_ht *htphy = dev->phy.ht;
+	static const u16 regs[3] = { B43_PHY_HT_RF_CTL_INT_C1,
+				     B43_PHY_HT_RF_CTL_INT_C2,
+				     B43_PHY_HT_RF_CTL_INT_C3 };
+	int i;
+
+	if (enable) {
+		for (i = 0; i < 3; i++)
+			b43_phy_write(dev, regs[i], htphy->rf_ctl_int_save[i]);
+	} else {
+		for (i = 0; i < 3; i++)
+			htphy->rf_ctl_int_save[i] = b43_phy_read(dev, regs[i]);
+		/* TODO: Does 5GHz band use different value (not 0x0400)? */
+		for (i = 0; i < 3; i++)
+			b43_phy_write(dev, regs[i], 0x0400);
+	}
+}
+
+/**************************************************
  * Various PHY ops
  **************************************************/
 
+static u16 b43_phy_ht_classifier(struct b43_wldev *dev, u16 mask, u16 val)
+{
+	u16 tmp;
+	u16 allowed = B43_PHY_HT_CLASS_CTL_CCK_EN |
+		      B43_PHY_HT_CLASS_CTL_OFDM_EN |
+		      B43_PHY_HT_CLASS_CTL_WAITED_EN;
+
+	tmp = b43_phy_read(dev, B43_PHY_HT_CLASS_CTL);
+	tmp &= allowed;
+	tmp &= ~mask;
+	tmp |= (val & mask);
+	b43_phy_maskset(dev, B43_PHY_HT_CLASS_CTL, ~allowed, tmp);
+
+	return tmp;
+}
+
+static void b43_phy_ht_reset_cca(struct b43_wldev *dev)
+{
+	u16 bbcfg;
+
+	b43_phy_force_clock(dev, true);
+	bbcfg = b43_phy_read(dev, B43_PHY_HT_BBCFG);
+	b43_phy_write(dev, B43_PHY_HT_BBCFG, bbcfg | B43_PHY_HT_BBCFG_RSTCCA);
+	udelay(1);
+	b43_phy_write(dev, B43_PHY_HT_BBCFG, bbcfg & ~B43_PHY_HT_BBCFG_RSTCCA);
+	b43_phy_force_clock(dev, false);
+
+	b43_phy_ht_force_rf_sequence(dev, B43_PHY_HT_RF_SEQ_TRIG_RST2RX);
+}
+
 static void b43_phy_ht_zero_extg(struct b43_wldev *dev)
 {
 	u8 i, j;
@@ -176,10 +255,10 @@ static void b43_phy_ht_afe_unk1(struct b43_wldev *dev)
 {
 	u8 i;
 
-	const u16 ctl_regs[3][2] = {
-		{ B43_PHY_HT_AFE_CTL1, B43_PHY_HT_AFE_CTL2 },
-		{ B43_PHY_HT_AFE_CTL3, B43_PHY_HT_AFE_CTL4 },
-		{ B43_PHY_HT_AFE_CTL5, B43_PHY_HT_AFE_CTL6},
+	static const u16 ctl_regs[3][2] = {
+		{ B43_PHY_HT_AFE_C1_OVER, B43_PHY_HT_AFE_C1 },
+		{ B43_PHY_HT_AFE_C2_OVER, B43_PHY_HT_AFE_C2 },
+		{ B43_PHY_HT_AFE_C3_OVER, B43_PHY_HT_AFE_C3},
 	};
 
 	for (i = 0; i < 3; i++) {
@@ -193,27 +272,6 @@ static void b43_phy_ht_afe_unk1(struct b43_wldev *dev)
 	}
 }
 
-static void b43_phy_ht_force_rf_sequence(struct b43_wldev *dev, u16 rf_seq)
-{
-	u8 i;
-
-	u16 save_seq_mode = b43_phy_read(dev, B43_PHY_HT_RF_SEQ_MODE);
-	b43_phy_set(dev, B43_PHY_HT_RF_SEQ_MODE, 0x3);
-
-	b43_phy_set(dev, B43_PHY_HT_RF_SEQ_TRIG, rf_seq);
-	for (i = 0; i < 200; i++) {
-		if (!(b43_phy_read(dev, B43_PHY_HT_RF_SEQ_STATUS) & rf_seq)) {
-			i = 0;
-			break;
-		}
-		msleep(1);
-	}
-	if (i)
-		b43err(dev->wl, "Forcing RF sequence timeout\n");
-
-	b43_phy_write(dev, B43_PHY_HT_RF_SEQ_MODE, save_seq_mode);
-}
-
 static void b43_phy_ht_read_clip_detection(struct b43_wldev *dev, u16 *clip_st)
 {
 	clip_st[0] = b43_phy_read(dev, B43_PHY_HT_C1_CLIP1THRES);
@@ -240,15 +298,456 @@ static void b43_phy_ht_bphy_init(struct b43_wldev *dev)
 }
 
 /**************************************************
+ * Samples
+ **************************************************/
+
+static void b43_phy_ht_stop_playback(struct b43_wldev *dev)
+{
+	struct b43_phy_ht *phy_ht = dev->phy.ht;
+	u16 tmp;
+	int i;
+
+	tmp = b43_phy_read(dev, B43_PHY_HT_SAMP_STAT);
+	if (tmp & 0x1)
+		b43_phy_set(dev, B43_PHY_HT_SAMP_CMD, B43_PHY_HT_SAMP_CMD_STOP);
+	else if (tmp & 0x2)
+		b43_phy_mask(dev, B43_PHY_HT_IQLOCAL_CMDGCTL, 0x7FFF);
+
+	b43_phy_mask(dev, B43_PHY_HT_SAMP_CMD, ~0x0004);
+
+	for (i = 0; i < 3; i++) {
+		if (phy_ht->bb_mult_save[i] >= 0) {
+			b43_httab_write(dev, B43_HTTAB16(13, 0x63 + i * 4),
+					phy_ht->bb_mult_save[i]);
+			b43_httab_write(dev, B43_HTTAB16(13, 0x67 + i * 4),
+					phy_ht->bb_mult_save[i]);
+		}
+	}
+}
+
+static u16 b43_phy_ht_load_samples(struct b43_wldev *dev)
+{
+	int i;
+	u16 len = 20 << 3;
+
+	b43_phy_write(dev, B43_PHY_HT_TABLE_ADDR, 0x4400);
+
+	for (i = 0; i < len; i++) {
+		b43_phy_write(dev, B43_PHY_HT_TABLE_DATAHI, 0);
+		b43_phy_write(dev, B43_PHY_HT_TABLE_DATALO, 0);
+	}
+
+	return len;
+}
+
+static void b43_phy_ht_run_samples(struct b43_wldev *dev, u16 samps, u16 loops,
+				   u16 wait)
+{
+	struct b43_phy_ht *phy_ht = dev->phy.ht;
+	u16 save_seq_mode;
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		if (phy_ht->bb_mult_save[i] < 0)
+			phy_ht->bb_mult_save[i] = b43_httab_read(dev, B43_HTTAB16(13, 0x63 + i * 4));
+	}
+
+	b43_phy_write(dev, B43_PHY_HT_SAMP_DEP_CNT, samps - 1);
+	if (loops != 0xFFFF)
+		loops--;
+	b43_phy_write(dev, B43_PHY_HT_SAMP_LOOP_CNT, loops);
+	b43_phy_write(dev, B43_PHY_HT_SAMP_WAIT_CNT, wait);
+
+	save_seq_mode = b43_phy_read(dev, B43_PHY_HT_RF_SEQ_MODE);
+	b43_phy_set(dev, B43_PHY_HT_RF_SEQ_MODE,
+		    B43_PHY_HT_RF_SEQ_MODE_CA_OVER);
+
+	/* TODO: find out mask bits! Do we need more function arguments? */
+	b43_phy_mask(dev, B43_PHY_HT_SAMP_CMD, ~0);
+	b43_phy_mask(dev, B43_PHY_HT_SAMP_CMD, ~0);
+	b43_phy_mask(dev, B43_PHY_HT_IQLOCAL_CMDGCTL, ~0);
+	b43_phy_set(dev, B43_PHY_HT_SAMP_CMD, 0x1);
+
+	for (i = 0; i < 100; i++) {
+		if (!(b43_phy_read(dev, B43_PHY_HT_RF_SEQ_STATUS) & 1)) {
+			i = 0;
+			break;
+		}
+		udelay(10);
+	}
+	if (i)
+		b43err(dev->wl, "run samples timeout\n");
+
+	b43_phy_write(dev, B43_PHY_HT_RF_SEQ_MODE, save_seq_mode);
+}
+
+static void b43_phy_ht_tx_tone(struct b43_wldev *dev)
+{
+	u16 samp;
+
+	samp = b43_phy_ht_load_samples(dev);
+	b43_phy_ht_run_samples(dev, samp, 0xFFFF, 0);
+}
+
+/**************************************************
+ * RSSI
+ **************************************************/
+
+static void b43_phy_ht_rssi_select(struct b43_wldev *dev, u8 core_sel,
+				   enum ht_rssi_type rssi_type)
+{
+	static const u16 ctl_regs[3][2] = {
+		{ B43_PHY_HT_AFE_C1, B43_PHY_HT_AFE_C1_OVER, },
+		{ B43_PHY_HT_AFE_C2, B43_PHY_HT_AFE_C2_OVER, },
+		{ B43_PHY_HT_AFE_C3, B43_PHY_HT_AFE_C3_OVER, },
+	};
+	static const u16 radio_r[] = { R2059_C1, R2059_C2, R2059_C3, };
+	int core;
+
+	if (core_sel == 0) {
+		b43err(dev->wl, "RSSI selection for core off not implemented yet\n");
+	} else {
+		for (core = 0; core < 3; core++) {
+			/* Check if caller requested a one specific core */
+			if ((core_sel == 1 && core != 0) ||
+			    (core_sel == 2 && core != 1) ||
+			    (core_sel == 3 && core != 2))
+				continue;
+
+			switch (rssi_type) {
+			case HT_RSSI_TSSI_2G:
+				b43_phy_set(dev, ctl_regs[core][0], 0x3 << 8);
+				b43_phy_set(dev, ctl_regs[core][0], 0x3 << 10);
+				b43_phy_set(dev, ctl_regs[core][1], 0x1 << 9);
+				b43_phy_set(dev, ctl_regs[core][1], 0x1 << 10);
+
+				b43_radio_set(dev, R2059_C3 | 0xbf, 0x1);
+				b43_radio_write(dev, radio_r[core] | 0x159,
+						0x11);
+				break;
+			default:
+				b43err(dev->wl, "RSSI selection for type %d not implemented yet\n",
+				       rssi_type);
+			}
+		}
+	}
+}
+
+static void b43_phy_ht_poll_rssi(struct b43_wldev *dev, enum ht_rssi_type type,
+				 s32 *buf, u8 nsamp)
+{
+	u16 phy_regs_values[12];
+	static const u16 phy_regs_to_save[] = {
+		B43_PHY_HT_AFE_C1, B43_PHY_HT_AFE_C1_OVER,
+		0x848, 0x841,
+		B43_PHY_HT_AFE_C2, B43_PHY_HT_AFE_C2_OVER,
+		0x868, 0x861,
+		B43_PHY_HT_AFE_C3, B43_PHY_HT_AFE_C3_OVER,
+		0x888, 0x881,
+	};
+	u16 tmp[3];
+	int i;
+
+	for (i = 0; i < 12; i++)
+		phy_regs_values[i] = b43_phy_read(dev, phy_regs_to_save[i]);
+
+	b43_phy_ht_rssi_select(dev, 5, type);
+
+	for (i = 0; i < 6; i++)
+		buf[i] = 0;
+
+	for (i = 0; i < nsamp; i++) {
+		tmp[0] = b43_phy_read(dev, B43_PHY_HT_RSSI_C1);
+		tmp[1] = b43_phy_read(dev, B43_PHY_HT_RSSI_C2);
+		tmp[2] = b43_phy_read(dev, B43_PHY_HT_RSSI_C3);
+
+		buf[0] += ((s8)((tmp[0] & 0x3F) << 2)) >> 2;
+		buf[1] += ((s8)(((tmp[0] >> 8) & 0x3F) << 2)) >> 2;
+		buf[2] += ((s8)((tmp[1] & 0x3F) << 2)) >> 2;
+		buf[3] += ((s8)(((tmp[1] >> 8) & 0x3F) << 2)) >> 2;
+		buf[4] += ((s8)((tmp[2] & 0x3F) << 2)) >> 2;
+		buf[5] += ((s8)(((tmp[2] >> 8) & 0x3F) << 2)) >> 2;
+	}
+
+	for (i = 0; i < 12; i++)
+		b43_phy_write(dev, phy_regs_to_save[i], phy_regs_values[i]);
+}
+
+/**************************************************
+ * Tx/Rx
+ **************************************************/
+
+static void b43_phy_ht_tx_power_fix(struct b43_wldev *dev)
+{
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		u16 mask;
+		u32 tmp = b43_httab_read(dev, B43_HTTAB32(26, 0xE8));
+
+		if (0) /* FIXME */
+			mask = 0x2 << (i * 4);
+		else
+			mask = 0;
+		b43_phy_mask(dev, B43_PHY_EXTG(0x108), mask);
+
+		b43_httab_write(dev, B43_HTTAB16(7, 0x110 + i), tmp >> 16);
+		b43_httab_write(dev, B43_HTTAB8(13, 0x63 + (i * 4)),
+				tmp & 0xFF);
+		b43_httab_write(dev, B43_HTTAB8(13, 0x73 + (i * 4)),
+				tmp & 0xFF);
+	}
+}
+
+static void b43_phy_ht_tx_power_ctl(struct b43_wldev *dev, bool enable)
+{
+	struct b43_phy_ht *phy_ht = dev->phy.ht;
+	u16 en_bits = B43_PHY_HT_TXPCTL_CMD_C1_COEFF |
+		      B43_PHY_HT_TXPCTL_CMD_C1_HWPCTLEN |
+		      B43_PHY_HT_TXPCTL_CMD_C1_PCTLEN;
+	static const u16 cmd_regs[3] = { B43_PHY_HT_TXPCTL_CMD_C1,
+					 B43_PHY_HT_TXPCTL_CMD_C2,
+					 B43_PHY_HT_TXPCTL_CMD_C3 };
+	static const u16 status_regs[3] = { B43_PHY_HT_TX_PCTL_STATUS_C1,
+					    B43_PHY_HT_TX_PCTL_STATUS_C2,
+					    B43_PHY_HT_TX_PCTL_STATUS_C3 };
+	int i;
+
+	if (!enable) {
+		if (b43_phy_read(dev, B43_PHY_HT_TXPCTL_CMD_C1) & en_bits) {
+			/* We disable enabled TX pwr ctl, save it's state */
+			for (i = 0; i < 3; i++)
+				phy_ht->tx_pwr_idx[i] =
+					b43_phy_read(dev, status_regs[i]);
+		}
+		b43_phy_mask(dev, B43_PHY_HT_TXPCTL_CMD_C1, ~en_bits);
+	} else {
+		b43_phy_set(dev, B43_PHY_HT_TXPCTL_CMD_C1, en_bits);
+
+		if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
+			for (i = 0; i < 3; i++)
+				b43_phy_write(dev, cmd_regs[i], 0x32);
+		}
+
+		for (i = 0; i < 3; i++)
+			if (phy_ht->tx_pwr_idx[i] <=
+			    B43_PHY_HT_TXPCTL_CMD_C1_INIT)
+				b43_phy_write(dev, cmd_regs[i],
+					      phy_ht->tx_pwr_idx[i]);
+	}
+
+	phy_ht->tx_pwr_ctl = enable;
+}
+
+static void b43_phy_ht_tx_power_ctl_idle_tssi(struct b43_wldev *dev)
+{
+	struct b43_phy_ht *phy_ht = dev->phy.ht;
+	static const u16 base[] = { 0x840, 0x860, 0x880 };
+	u16 save_regs[3][3];
+	s32 rssi_buf[6];
+	int core;
+
+	for (core = 0; core < 3; core++) {
+		save_regs[core][1] = b43_phy_read(dev, base[core] + 6);
+		save_regs[core][2] = b43_phy_read(dev, base[core] + 7);
+		save_regs[core][0] = b43_phy_read(dev, base[core] + 0);
+
+		b43_phy_write(dev, base[core] + 6, 0);
+		b43_phy_mask(dev, base[core] + 7, ~0xF); /* 0xF? Or just 0x6? */
+		b43_phy_set(dev, base[core] + 0, 0x0400);
+		b43_phy_set(dev, base[core] + 0, 0x1000);
+	}
+
+	b43_phy_ht_tx_tone(dev);
+	udelay(20);
+	b43_phy_ht_poll_rssi(dev, HT_RSSI_TSSI_2G, rssi_buf, 1);
+	b43_phy_ht_stop_playback(dev);
+	b43_phy_ht_reset_cca(dev);
+
+	phy_ht->idle_tssi[0] = rssi_buf[0] & 0xff;
+	phy_ht->idle_tssi[1] = rssi_buf[2] & 0xff;
+	phy_ht->idle_tssi[2] = rssi_buf[4] & 0xff;
+
+	for (core = 0; core < 3; core++) {
+		b43_phy_write(dev, base[core] + 0, save_regs[core][0]);
+		b43_phy_write(dev, base[core] + 6, save_regs[core][1]);
+		b43_phy_write(dev, base[core] + 7, save_regs[core][2]);
+	}
+}
+
+static void b43_phy_ht_tssi_setup(struct b43_wldev *dev)
+{
+	static const u16 routing[] = { R2059_C1, R2059_C2, R2059_C3, };
+	int core;
+
+	/* 0x159 is probably TX_SSI_MUX or TSSIG (by comparing to N-PHY) */
+	for (core = 0; core < 3; core++) {
+		b43_radio_set(dev, 0x8bf, 0x1);
+		b43_radio_write(dev, routing[core] | 0x0159, 0x0011);
+	}
+}
+
+static void b43_phy_ht_tx_power_ctl_setup(struct b43_wldev *dev)
+{
+	struct b43_phy_ht *phy_ht = dev->phy.ht;
+	struct ssb_sprom *sprom = dev->dev->bus_sprom;
+
+	u8 *idle = phy_ht->idle_tssi;
+	u8 target[3];
+	s16 a1[3], b0[3], b1[3];
+
+	u16 freq = dev->phy.channel_freq;
+	int i, c;
+
+	if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
+		for (c = 0; c < 3; c++) {
+			target[c] = sprom->core_pwr_info[c].maxpwr_2g;
+			a1[c] = sprom->core_pwr_info[c].pa_2g[0];
+			b0[c] = sprom->core_pwr_info[c].pa_2g[1];
+			b1[c] = sprom->core_pwr_info[c].pa_2g[2];
+		}
+	} else if (freq >= 4900 && freq < 5100) {
+		for (c = 0; c < 3; c++) {
+			target[c] = sprom->core_pwr_info[c].maxpwr_5gl;
+			a1[c] = sprom->core_pwr_info[c].pa_5gl[0];
+			b0[c] = sprom->core_pwr_info[c].pa_5gl[1];
+			b1[c] = sprom->core_pwr_info[c].pa_5gl[2];
+		}
+	} else if (freq >= 5100 && freq < 5500) {
+		for (c = 0; c < 3; c++) {
+			target[c] = sprom->core_pwr_info[c].maxpwr_5g;
+			a1[c] = sprom->core_pwr_info[c].pa_5g[0];
+			b0[c] = sprom->core_pwr_info[c].pa_5g[1];
+			b1[c] = sprom->core_pwr_info[c].pa_5g[2];
+		}
+	} else if (freq >= 5500) {
+		for (c = 0; c < 3; c++) {
+			target[c] = sprom->core_pwr_info[c].maxpwr_5gh;
+			a1[c] = sprom->core_pwr_info[c].pa_5gh[0];
+			b0[c] = sprom->core_pwr_info[c].pa_5gh[1];
+			b1[c] = sprom->core_pwr_info[c].pa_5gh[2];
+		}
+	} else {
+		target[0] = target[1] = target[2] = 52;
+		a1[0] = a1[1] = a1[2] = -424;
+		b0[0] = b0[1] = b0[2] = 5612;
+		b1[0] = b1[1] = b1[2] = -1393;
+	}
+
+	b43_phy_set(dev, B43_PHY_HT_TSSIMODE, B43_PHY_HT_TSSIMODE_EN);
+	b43_phy_mask(dev, B43_PHY_HT_TXPCTL_CMD_C1,
+		     ~B43_PHY_HT_TXPCTL_CMD_C1_PCTLEN & 0xFFFF);
+
+	/* TODO: Does it depend on sprom->fem.ghz2.tssipos? */
+	b43_phy_set(dev, B43_PHY_HT_TXPCTL_IDLE_TSSI, 0x4000);
+
+	b43_phy_maskset(dev, B43_PHY_HT_TXPCTL_CMD_C1,
+			~B43_PHY_HT_TXPCTL_CMD_C1_INIT, 0x19);
+	b43_phy_maskset(dev, B43_PHY_HT_TXPCTL_CMD_C2,
+			~B43_PHY_HT_TXPCTL_CMD_C2_INIT, 0x19);
+	b43_phy_maskset(dev, B43_PHY_HT_TXPCTL_CMD_C3,
+			~B43_PHY_HT_TXPCTL_CMD_C3_INIT, 0x19);
+
+	b43_phy_set(dev, B43_PHY_HT_TXPCTL_IDLE_TSSI,
+		    B43_PHY_HT_TXPCTL_IDLE_TSSI_BINF);
+
+	b43_phy_maskset(dev, B43_PHY_HT_TXPCTL_IDLE_TSSI,
+			~B43_PHY_HT_TXPCTL_IDLE_TSSI_C1,
+			idle[0] << B43_PHY_HT_TXPCTL_IDLE_TSSI_C1_SHIFT);
+	b43_phy_maskset(dev, B43_PHY_HT_TXPCTL_IDLE_TSSI,
+			~B43_PHY_HT_TXPCTL_IDLE_TSSI_C2,
+			idle[1] << B43_PHY_HT_TXPCTL_IDLE_TSSI_C2_SHIFT);
+	b43_phy_maskset(dev, B43_PHY_HT_TXPCTL_IDLE_TSSI2,
+			~B43_PHY_HT_TXPCTL_IDLE_TSSI2_C3,
+			idle[2] << B43_PHY_HT_TXPCTL_IDLE_TSSI2_C3_SHIFT);
+
+	b43_phy_maskset(dev, B43_PHY_HT_TXPCTL_N, ~B43_PHY_HT_TXPCTL_N_TSSID,
+			0xf0);
+	b43_phy_maskset(dev, B43_PHY_HT_TXPCTL_N, ~B43_PHY_HT_TXPCTL_N_NPTIL2,
+			0x3 << B43_PHY_HT_TXPCTL_N_NPTIL2_SHIFT);
+#if 0
+	/* TODO: what to mask/set? */
+	b43_phy_maskset(dev, B43_PHY_HT_TXPCTL_CMD_C1, 0x800, 0)
+	b43_phy_maskset(dev, B43_PHY_HT_TXPCTL_CMD_C1, 0x400, 0)
+#endif
+
+	b43_phy_maskset(dev, B43_PHY_HT_TXPCTL_TARG_PWR,
+			~B43_PHY_HT_TXPCTL_TARG_PWR_C1,
+			target[0] << B43_PHY_HT_TXPCTL_TARG_PWR_C1_SHIFT);
+	b43_phy_maskset(dev, B43_PHY_HT_TXPCTL_TARG_PWR,
+			~B43_PHY_HT_TXPCTL_TARG_PWR_C2 & 0xFFFF,
+			target[1] << B43_PHY_HT_TXPCTL_TARG_PWR_C2_SHIFT);
+	b43_phy_maskset(dev, B43_PHY_HT_TXPCTL_TARG_PWR2,
+			~B43_PHY_HT_TXPCTL_TARG_PWR2_C3,
+			target[2] << B43_PHY_HT_TXPCTL_TARG_PWR2_C3_SHIFT);
+
+	for (c = 0; c < 3; c++) {
+		s32 num, den, pwr;
+		u32 regval[64];
+
+		for (i = 0; i < 64; i++) {
+			num = 8 * (16 * b0[c] + b1[c] * i);
+			den = 32768 + a1[c] * i;
+			pwr = max((4 * num + den / 2) / den, -8);
+			regval[i] = pwr;
+		}
+		b43_httab_write_bulk(dev, B43_HTTAB16(26 + c, 0), 64, regval);
+	}
+}
+
+/**************************************************
  * Channel switching ops.
  **************************************************/
 
+static void b43_phy_ht_spur_avoid(struct b43_wldev *dev,
+				  struct ieee80211_channel *new_channel)
+{
+	struct bcma_device *core = dev->dev->bdev;
+	int spuravoid = 0;
+	u16 tmp;
+
+	/* Check for 13 and 14 is just a guess, we don't have enough logs. */
+	if (new_channel->hw_value == 13 || new_channel->hw_value == 14)
+		spuravoid = 1;
+	bcma_core_pll_ctl(core, B43_BCMA_CLKCTLST_PHY_PLL_REQ, 0, false);
+	bcma_pmu_spuravoid_pllupdate(&core->bus->drv_cc, spuravoid);
+	bcma_core_pll_ctl(core,
+			  B43_BCMA_CLKCTLST_80211_PLL_REQ |
+			  B43_BCMA_CLKCTLST_PHY_PLL_REQ,
+			  B43_BCMA_CLKCTLST_80211_PLL_ST |
+			  B43_BCMA_CLKCTLST_PHY_PLL_ST, false);
+
+	/* Values has been taken from wlc_bmac_switch_macfreq comments */
+	switch (spuravoid) {
+	case 2: /* 126MHz */
+		tmp = 0x2082;
+		break;
+	case 1: /* 123MHz */
+		tmp = 0x5341;
+		break;
+	default: /* 120MHz */
+		tmp = 0x8889;
+	}
+
+	b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_LOW, tmp);
+	b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_HIGH, 0x8);
+
+	/* TODO: reset PLL */
+
+	if (spuravoid)
+		b43_phy_set(dev, B43_PHY_HT_BBCFG, B43_PHY_HT_BBCFG_RSTRX);
+	else
+		b43_phy_mask(dev, B43_PHY_HT_BBCFG,
+				~B43_PHY_HT_BBCFG_RSTRX & 0xFFFF);
+
+	b43_phy_ht_reset_cca(dev);
+}
+
 static void b43_phy_ht_channel_setup(struct b43_wldev *dev,
 				const struct b43_phy_ht_channeltab_e_phy *e,
 				struct ieee80211_channel *new_channel)
 {
 	bool old_band_5ghz;
-	u8 i;
 
 	old_band_5ghz = b43_phy_read(dev, B43_PHY_HT_BANDCTL) & 0; /* FIXME */
 	if (new_channel->band == IEEE80211_BAND_5GHZ && !old_band_5ghz) {
@@ -264,25 +763,20 @@ static void b43_phy_ht_channel_setup(struct b43_wldev *dev,
 	b43_phy_write(dev, B43_PHY_HT_BW5, e->bw5);
 	b43_phy_write(dev, B43_PHY_HT_BW6, e->bw6);
 
-	/* TODO: some ops on PHY regs 0x0B0 and 0xC0A */
-
-	/* TODO: separated function? */
-	for (i = 0; i < 3; i++) {
-		u16 mask;
-		u32 tmp = b43_httab_read(dev, B43_HTTAB32(26, 0xE8));
+	if (new_channel->hw_value == 14) {
+		b43_phy_ht_classifier(dev, B43_PHY_HT_CLASS_CTL_OFDM_EN, 0);
+		b43_phy_set(dev, B43_PHY_HT_TEST, 0x0800);
+	} else {
+		b43_phy_ht_classifier(dev, B43_PHY_HT_CLASS_CTL_OFDM_EN,
+				      B43_PHY_HT_CLASS_CTL_OFDM_EN);
+		if (new_channel->band == IEEE80211_BAND_2GHZ)
+			b43_phy_mask(dev, B43_PHY_HT_TEST, ~0x840);
+	}
 
-		if (0) /* FIXME */
-			mask = 0x2 << (i * 4);
-		else
-			mask = 0;
-		b43_phy_mask(dev, B43_PHY_EXTG(0x108), mask);
+	if (1) /* TODO: On N it's for early devices only, what about HT? */
+		b43_phy_ht_tx_power_fix(dev);
 
-		b43_httab_write(dev, B43_HTTAB16(7, 0x110 + i), tmp >> 16);
-		b43_httab_write(dev, B43_HTTAB8(13, 0x63 + (i * 4)),
-				tmp & 0xFF);
-		b43_httab_write(dev, B43_HTTAB8(13, 0x73 + (i * 4)),
-				tmp & 0xFF);
-	}
+	b43_phy_ht_spur_avoid(dev, new_channel);
 
 	b43_phy_write(dev, 0x017e, 0x3830);
 }
@@ -337,14 +831,29 @@ static void b43_phy_ht_op_prepare_structs(struct b43_wldev *dev)
 {
 	struct b43_phy *phy = &dev->phy;
 	struct b43_phy_ht *phy_ht = phy->ht;
+	int i;
 
 	memset(phy_ht, 0, sizeof(*phy_ht));
+
+	phy_ht->tx_pwr_ctl = true;
+	for (i = 0; i < 3; i++)
+		phy_ht->tx_pwr_idx[i] = B43_PHY_HT_TXPCTL_CMD_C1_INIT + 1;
+
+	for (i = 0; i < 3; i++)
+		phy_ht->bb_mult_save[i] = -1;
 }
 
 static int b43_phy_ht_op_init(struct b43_wldev *dev)
 {
+	struct b43_phy_ht *phy_ht = dev->phy.ht;
 	u16 tmp;
 	u16 clip_state[3];
+	bool saved_tx_pwr_ctl;
+
+	if (dev->dev->bus_type != B43_BUS_BCMA) {
+		b43err(dev->wl, "HT-PHY is supported only on BCMA bus!\n");
+		return -EOPNOTSUPP;
+	}
 
 	b43_phy_ht_tables_init(dev);
 
@@ -357,9 +866,9 @@ static int b43_phy_ht_op_init(struct b43_wldev *dev)
 
 	b43_phy_mask(dev, B43_PHY_EXTG(0), ~0x3);
 
-	b43_phy_write(dev, B43_PHY_HT_AFE_CTL1, 0);
-	b43_phy_write(dev, B43_PHY_HT_AFE_CTL3, 0);
-	b43_phy_write(dev, B43_PHY_HT_AFE_CTL5, 0);
+	b43_phy_write(dev, B43_PHY_HT_AFE_C1_OVER, 0);
+	b43_phy_write(dev, B43_PHY_HT_AFE_C2_OVER, 0);
+	b43_phy_write(dev, B43_PHY_HT_AFE_C3_OVER, 0);
 
 	b43_phy_write(dev, B43_PHY_EXTG(0x103), 0x20);
 	b43_phy_write(dev, B43_PHY_EXTG(0x101), 0x20);
@@ -371,8 +880,11 @@ static int b43_phy_ht_op_init(struct b43_wldev *dev)
 	if (0) /* TODO: condition */
 		; /* TODO: PHY op on reg 0x217 */
 
-	b43_phy_read(dev, 0xb0); /* TODO: what for? */
-	b43_phy_set(dev, 0xb0, 0x1);
+	if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ)
+		b43_phy_ht_classifier(dev, B43_PHY_HT_CLASS_CTL_CCK_EN, 0);
+	else
+		b43_phy_ht_classifier(dev, B43_PHY_HT_CLASS_CTL_CCK_EN,
+				      B43_PHY_HT_CLASS_CTL_CCK_EN);
 
 	b43_phy_set(dev, 0xb1, 0x91);
 	b43_phy_write(dev, 0x32f, 0x0003);
@@ -448,12 +960,13 @@ static int b43_phy_ht_op_init(struct b43_wldev *dev)
 
 	b43_mac_phy_clock_set(dev, true);
 
+	b43_phy_ht_pa_override(dev, false);
 	b43_phy_ht_force_rf_sequence(dev, B43_PHY_HT_RF_SEQ_TRIG_RX2TX);
 	b43_phy_ht_force_rf_sequence(dev, B43_PHY_HT_RF_SEQ_TRIG_RST2RX);
-
-	/* TODO: PHY op on reg 0xb0 */
+	b43_phy_ht_pa_override(dev, true);
 
 	/* TODO: Should we restore it? Or store it in global PHY info? */
+	b43_phy_ht_classifier(dev, 0, 0);
 	b43_phy_ht_read_clip_detection(dev, clip_state);
 
 	if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
@@ -462,6 +975,14 @@ static int b43_phy_ht_op_init(struct b43_wldev *dev)
 	b43_httab_write_bulk(dev, B43_HTTAB32(0x1a, 0xc0),
 			B43_HTTAB_1A_C0_LATE_SIZE, b43_httab_0x1a_0xc0_late);
 
+	saved_tx_pwr_ctl = phy_ht->tx_pwr_ctl;
+	b43_phy_ht_tx_power_fix(dev);
+	b43_phy_ht_tx_power_ctl(dev, false);
+	b43_phy_ht_tx_power_ctl_idle_tssi(dev);
+	b43_phy_ht_tx_power_ctl_setup(dev);
+	b43_phy_ht_tssi_setup(dev);
+	b43_phy_ht_tx_power_ctl(dev, saved_tx_pwr_ctl);
+
 	return 0;
 }
 
@@ -506,27 +1027,28 @@ static void b43_phy_ht_op_software_rfkill(struct b43_wldev *dev,
 static void b43_phy_ht_op_switch_analog(struct b43_wldev *dev, bool on)
 {
 	if (on) {
-		b43_phy_write(dev, B43_PHY_HT_AFE_CTL2, 0x00cd);
-		b43_phy_write(dev, B43_PHY_HT_AFE_CTL1, 0x0000);
-		b43_phy_write(dev, B43_PHY_HT_AFE_CTL4, 0x00cd);
-		b43_phy_write(dev, B43_PHY_HT_AFE_CTL3, 0x0000);
-		b43_phy_write(dev, B43_PHY_HT_AFE_CTL6, 0x00cd);
-		b43_phy_write(dev, B43_PHY_HT_AFE_CTL5, 0x0000);
+		b43_phy_write(dev, B43_PHY_HT_AFE_C1, 0x00cd);
+		b43_phy_write(dev, B43_PHY_HT_AFE_C1_OVER, 0x0000);
+		b43_phy_write(dev, B43_PHY_HT_AFE_C2, 0x00cd);
+		b43_phy_write(dev, B43_PHY_HT_AFE_C2_OVER, 0x0000);
+		b43_phy_write(dev, B43_PHY_HT_AFE_C3, 0x00cd);
+		b43_phy_write(dev, B43_PHY_HT_AFE_C3_OVER, 0x0000);
 	} else {
-		b43_phy_write(dev, B43_PHY_HT_AFE_CTL1, 0x07ff);
-		b43_phy_write(dev, B43_PHY_HT_AFE_CTL2, 0x00fd);
-		b43_phy_write(dev, B43_PHY_HT_AFE_CTL3, 0x07ff);
-		b43_phy_write(dev, B43_PHY_HT_AFE_CTL4, 0x00fd);
-		b43_phy_write(dev, B43_PHY_HT_AFE_CTL5, 0x07ff);
-		b43_phy_write(dev, B43_PHY_HT_AFE_CTL6, 0x00fd);
+		b43_phy_write(dev, B43_PHY_HT_AFE_C1_OVER, 0x07ff);
+		b43_phy_write(dev, B43_PHY_HT_AFE_C1, 0x00fd);
+		b43_phy_write(dev, B43_PHY_HT_AFE_C2_OVER, 0x07ff);
+		b43_phy_write(dev, B43_PHY_HT_AFE_C2, 0x00fd);
+		b43_phy_write(dev, B43_PHY_HT_AFE_C3_OVER, 0x07ff);
+		b43_phy_write(dev, B43_PHY_HT_AFE_C3, 0x00fd);
 	}
 }
 
 static int b43_phy_ht_op_switch_channel(struct b43_wldev *dev,
 					unsigned int new_channel)
 {
-	struct ieee80211_channel *channel = dev->wl->hw->conf.channel;
-	enum nl80211_channel_type channel_type = dev->wl->hw->conf.channel_type;
+	struct ieee80211_channel *channel = dev->wl->hw->conf.chandef.chan;
+	enum nl80211_channel_type channel_type =
+		cfg80211_get_chandef_type(&dev->wl->hw->conf.chandef);
 
 	if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
 		if ((new_channel < 1) || (new_channel > 14))
diff --git a/drivers/net/wireless/b43/phy_ht.h b/drivers/net/wireless/b43/phy_ht.h
index 6544c4293b34..6cae370d1018 100644
--- a/drivers/net/wireless/b43/phy_ht.h
+++ b/drivers/net/wireless/b43/phy_ht.h
@@ -12,18 +12,65 @@
 #define B43_PHY_HT_TABLE_ADDR			0x072 /* Table address */
 #define B43_PHY_HT_TABLE_DATALO			0x073 /* Table data low */
 #define B43_PHY_HT_TABLE_DATAHI			0x074 /* Table data high */
+#define B43_PHY_HT_CLASS_CTL			0x0B0 /* Classifier control */
+#define  B43_PHY_HT_CLASS_CTL_CCK_EN		0x0001 /* CCK enable */
+#define  B43_PHY_HT_CLASS_CTL_OFDM_EN		0x0002 /* OFDM enable */
+#define  B43_PHY_HT_CLASS_CTL_WAITED_EN		0x0004 /* Waited enable */
+#define B43_PHY_HT_IQLOCAL_CMDGCTL		0x0C2	/* I/Q LO cal command G control */
+#define B43_PHY_HT_SAMP_CMD			0x0C3	/* Sample command */
+#define  B43_PHY_HT_SAMP_CMD_STOP		0x0002	/* Stop */
+#define B43_PHY_HT_SAMP_LOOP_CNT		0x0C4	/* Sample loop count */
+#define B43_PHY_HT_SAMP_WAIT_CNT		0x0C5	/* Sample wait count */
+#define B43_PHY_HT_SAMP_DEP_CNT			0x0C6	/* Sample depth count */
+#define B43_PHY_HT_SAMP_STAT			0x0C7	/* Sample status */
+#define B43_PHY_HT_EST_PWR_C1			0x118
+#define B43_PHY_HT_EST_PWR_C2			0x119
+#define B43_PHY_HT_EST_PWR_C3			0x11A
+#define B43_PHY_HT_TSSIMODE			0x122	/* TSSI mode */
+#define  B43_PHY_HT_TSSIMODE_EN			0x0001	/* TSSI enable */
+#define  B43_PHY_HT_TSSIMODE_PDEN		0x0002	/* Power det enable */
 #define B43_PHY_HT_BW1				0x1CE
 #define B43_PHY_HT_BW2				0x1CF
 #define B43_PHY_HT_BW3				0x1D0
 #define B43_PHY_HT_BW4				0x1D1
 #define B43_PHY_HT_BW5				0x1D2
 #define B43_PHY_HT_BW6				0x1D3
+#define B43_PHY_HT_TXPCTL_CMD_C1		0x1E7	/* TX power control command */
+#define  B43_PHY_HT_TXPCTL_CMD_C1_INIT		0x007F	/* Init */
+#define  B43_PHY_HT_TXPCTL_CMD_C1_COEFF		0x2000	/* Power control coefficients */
+#define  B43_PHY_HT_TXPCTL_CMD_C1_HWPCTLEN	0x4000	/* Hardware TX power control enable */
+#define  B43_PHY_HT_TXPCTL_CMD_C1_PCTLEN	0x8000	/* TX power control enable */
+#define B43_PHY_HT_TXPCTL_N			0x1E8	/* TX power control N num */
+#define  B43_PHY_HT_TXPCTL_N_TSSID		0x00FF	/* N TSSI delay */
+#define  B43_PHY_HT_TXPCTL_N_TSSID_SHIFT	0
+#define  B43_PHY_HT_TXPCTL_N_NPTIL2		0x0700	/* N PT integer log2 */
+#define  B43_PHY_HT_TXPCTL_N_NPTIL2_SHIFT	8
+#define B43_PHY_HT_TXPCTL_IDLE_TSSI		0x1E9	/* TX power control idle TSSI */
+#define  B43_PHY_HT_TXPCTL_IDLE_TSSI_C1		0x003F
+#define  B43_PHY_HT_TXPCTL_IDLE_TSSI_C1_SHIFT	0
+#define  B43_PHY_HT_TXPCTL_IDLE_TSSI_C2		0x3F00
+#define  B43_PHY_HT_TXPCTL_IDLE_TSSI_C2_SHIFT	8
+#define  B43_PHY_HT_TXPCTL_IDLE_TSSI_BINF	0x8000	/* Raw TSSI offset bin format */
+#define B43_PHY_HT_TXPCTL_TARG_PWR		0x1EA	/* TX power control target power */
+#define  B43_PHY_HT_TXPCTL_TARG_PWR_C1		0x00FF	/* Power 0 */
+#define  B43_PHY_HT_TXPCTL_TARG_PWR_C1_SHIFT	0
+#define  B43_PHY_HT_TXPCTL_TARG_PWR_C2		0xFF00	/* Power 1 */
+#define  B43_PHY_HT_TXPCTL_TARG_PWR_C2_SHIFT	8
+#define B43_PHY_HT_TX_PCTL_STATUS_C1		0x1ED
+#define B43_PHY_HT_TX_PCTL_STATUS_C2		0x1EE
+#define B43_PHY_HT_TXPCTL_CMD_C2		0x222
+#define  B43_PHY_HT_TXPCTL_CMD_C2_INIT		0x007F
+#define B43_PHY_HT_RSSI_C1			0x219
+#define B43_PHY_HT_RSSI_C2			0x21A
+#define B43_PHY_HT_RSSI_C3			0x21B
 
 #define B43_PHY_HT_C1_CLIP1THRES		B43_PHY_OFDM(0x00E)
 #define B43_PHY_HT_C2_CLIP1THRES		B43_PHY_OFDM(0x04E)
 #define B43_PHY_HT_C3_CLIP1THRES		B43_PHY_OFDM(0x08E)
 
 #define B43_PHY_HT_RF_SEQ_MODE			B43_PHY_EXTG(0x000)
+#define  B43_PHY_HT_RF_SEQ_MODE_CA_OVER		0x0001	/* Core active override */
+#define  B43_PHY_HT_RF_SEQ_MODE_TR_OVER		0x0002	/* Trigger override */
 #define B43_PHY_HT_RF_SEQ_TRIG			B43_PHY_EXTG(0x003)
 #define  B43_PHY_HT_RF_SEQ_TRIG_RX2TX		0x0001 /* RX2TX */
 #define  B43_PHY_HT_RF_SEQ_TRIG_TX2RX		0x0002 /* TX2RX */
@@ -36,12 +83,28 @@
 
 #define B43_PHY_HT_RF_CTL1			B43_PHY_EXTG(0x010)
 
-#define B43_PHY_HT_AFE_CTL1			B43_PHY_EXTG(0x110)
-#define B43_PHY_HT_AFE_CTL2			B43_PHY_EXTG(0x111)
-#define B43_PHY_HT_AFE_CTL3			B43_PHY_EXTG(0x114)
-#define B43_PHY_HT_AFE_CTL4			B43_PHY_EXTG(0x115)
-#define B43_PHY_HT_AFE_CTL5			B43_PHY_EXTG(0x118)
-#define B43_PHY_HT_AFE_CTL6			B43_PHY_EXTG(0x119)
+#define B43_PHY_HT_RF_CTL_INT_C1		B43_PHY_EXTG(0x04c)
+#define B43_PHY_HT_RF_CTL_INT_C2		B43_PHY_EXTG(0x06c)
+#define B43_PHY_HT_RF_CTL_INT_C3		B43_PHY_EXTG(0x08c)
+
+#define B43_PHY_HT_AFE_C1_OVER			B43_PHY_EXTG(0x110)
+#define B43_PHY_HT_AFE_C1			B43_PHY_EXTG(0x111)
+#define B43_PHY_HT_AFE_C2_OVER			B43_PHY_EXTG(0x114)
+#define B43_PHY_HT_AFE_C2			B43_PHY_EXTG(0x115)
+#define B43_PHY_HT_AFE_C3_OVER			B43_PHY_EXTG(0x118)
+#define B43_PHY_HT_AFE_C3			B43_PHY_EXTG(0x119)
+
+#define B43_PHY_HT_TXPCTL_CMD_C3		B43_PHY_EXTG(0x164)
+#define  B43_PHY_HT_TXPCTL_CMD_C3_INIT		0x007F
+#define B43_PHY_HT_TXPCTL_IDLE_TSSI2		B43_PHY_EXTG(0x165)	/* TX power control idle TSSI */
+#define  B43_PHY_HT_TXPCTL_IDLE_TSSI2_C3	0x003F
+#define  B43_PHY_HT_TXPCTL_IDLE_TSSI2_C3_SHIFT	0
+#define B43_PHY_HT_TXPCTL_TARG_PWR2		B43_PHY_EXTG(0x166)	/* TX power control target power */
+#define  B43_PHY_HT_TXPCTL_TARG_PWR2_C3		0x00FF
+#define  B43_PHY_HT_TXPCTL_TARG_PWR2_C3_SHIFT	0
+#define B43_PHY_HT_TX_PCTL_STATUS_C3		B43_PHY_EXTG(0x169)
+
+#define B43_PHY_HT_TEST				B43_PHY_N_BMODE(0x00A)
 
 
 /* Values for PHY registers used on channel switching */
@@ -56,6 +119,14 @@ struct b43_phy_ht_channeltab_e_phy {
 
 
 struct b43_phy_ht {
+	u16 rf_ctl_int_save[3];
+
+	bool tx_pwr_ctl;
+	u8 tx_pwr_idx[3];
+
+	s32 bb_mult_save[3];
+
+	u8 idle_tssi[3];
 };
 
 
diff --git a/drivers/net/wireless/b43/phy_lcn.c b/drivers/net/wireless/b43/phy_lcn.c
index a13e28ef6246..0bafa3b17035 100644
--- a/drivers/net/wireless/b43/phy_lcn.c
+++ b/drivers/net/wireless/b43/phy_lcn.c
@@ -808,8 +808,9 @@ static void b43_phy_lcn_op_switch_analog(struct b43_wldev *dev, bool on)
 static int b43_phy_lcn_op_switch_channel(struct b43_wldev *dev,
 					unsigned int new_channel)
 {
-	struct ieee80211_channel *channel = dev->wl->hw->conf.channel;
-	enum nl80211_channel_type channel_type = dev->wl->hw->conf.channel_type;
+	struct ieee80211_channel *channel = dev->wl->hw->conf.chandef.chan;
+	enum nl80211_channel_type channel_type =
+		cfg80211_get_chandef_type(&dev->wl->hw->conf.chandef);
 
 	if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
 		if ((new_channel < 1) || (new_channel > 14))
diff --git a/drivers/net/wireless/b43/phy_lp.c b/drivers/net/wireless/b43/phy_lp.c
index 3ae28561f7a4..92190dacf689 100644
--- a/drivers/net/wireless/b43/phy_lp.c
+++ b/drivers/net/wireless/b43/phy_lp.c
@@ -104,14 +104,8 @@ static void lpphy_read_band_sprom(struct b43_wldev *dev)
 		maxpwr = sprom->maxpwr_bg;
 		lpphy->max_tx_pwr_med_band = maxpwr;
 		cckpo = sprom->cck2gpo;
-		/*
-		 * We don't read SPROM's opo as specs say. On rev8 SPROMs
-		 * opo == ofdm2gpo and we don't know any SSB with LP-PHY
-		 * and SPROM rev below 8.
-		 */
-		B43_WARN_ON(sprom->revision < 8);
-		ofdmpo = sprom->ofdm2gpo;
 		if (cckpo) {
+			ofdmpo = sprom->ofdm2gpo;
 			for (i = 0; i < 4; i++) {
 				lpphy->tx_max_rate[i] =
 					maxpwr - (ofdmpo & 0xF) * 2;
@@ -124,11 +118,11 @@ static void lpphy_read_band_sprom(struct b43_wldev *dev)
 				ofdmpo >>= 4;
 			}
 		} else {
-			ofdmpo &= 0xFF;
+			u8 opo = sprom->opo;
 			for (i = 0; i < 4; i++)
 				lpphy->tx_max_rate[i] = maxpwr;
 			for (i = 4; i < 15; i++)
-				lpphy->tx_max_rate[i] = maxpwr - ofdmpo;
+				lpphy->tx_max_rate[i] = maxpwr - opo;
 		}
 	} else { /* 5GHz */
 		lpphy->tx_isolation_low_band = sprom->tri5gl;
@@ -287,8 +281,8 @@ static void lpphy_baseband_rev0_1_init(struct b43_wldev *dev)
 		b43_phy_maskset(dev, B43_LPPHY_TR_LOOKUP_8, 0xFFC0, 0x000A);
 		b43_phy_maskset(dev, B43_LPPHY_TR_LOOKUP_8, 0xC0FF, 0x0B00);
 	} else if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ ||
-		  (dev->dev->board_type == 0x048A) || ((dev->phy.rev == 0) &&
-		  (sprom->boardflags_lo & B43_BFL_FEM))) {
+		   (dev->dev->board_type == SSB_BOARD_BU4312) ||
+		   (dev->phy.rev == 0 && (sprom->boardflags_lo & B43_BFL_FEM))) {
 		b43_phy_maskset(dev, B43_LPPHY_TR_LOOKUP_1, 0xFFC0, 0x0001);
 		b43_phy_maskset(dev, B43_LPPHY_TR_LOOKUP_1, 0xC0FF, 0x0400);
 		b43_phy_maskset(dev, B43_LPPHY_TR_LOOKUP_2, 0xFFC0, 0x0001);
diff --git a/drivers/net/wireless/b43/phy_n.c b/drivers/net/wireless/b43/phy_n.c
index b70f220bc4b3..7c970d3ae358 100644
--- a/drivers/net/wireless/b43/phy_n.c
+++ b/drivers/net/wireless/b43/phy_n.c
@@ -69,14 +69,27 @@ enum b43_nphy_rf_sequence {
 	B43_RFSEQ_UPDATE_GAINU,
 };
 
-enum b43_nphy_rssi_type {
-	B43_NPHY_RSSI_X = 0,
-	B43_NPHY_RSSI_Y,
-	B43_NPHY_RSSI_Z,
-	B43_NPHY_RSSI_PWRDET,
-	B43_NPHY_RSSI_TSSI_I,
-	B43_NPHY_RSSI_TSSI_Q,
-	B43_NPHY_RSSI_TBD,
+enum n_intc_override {
+	N_INTC_OVERRIDE_OFF = 0,
+	N_INTC_OVERRIDE_TRSW = 1,
+	N_INTC_OVERRIDE_PA = 2,
+	N_INTC_OVERRIDE_EXT_LNA_PU = 3,
+	N_INTC_OVERRIDE_EXT_LNA_GAIN = 4,
+};
+
+enum n_rssi_type {
+	N_RSSI_W1 = 0,
+	N_RSSI_W2,
+	N_RSSI_NB,
+	N_RSSI_IQ,
+	N_RSSI_TSSI_2G,
+	N_RSSI_TSSI_5G,
+	N_RSSI_TBD,
+};
+
+enum n_rail_type {
+	N_RAIL_I = 0,
+	N_RAIL_Q = 1,
 };
 
 static inline bool b43_nphy_ipa(struct b43_wldev *dev)
@@ -94,7 +107,7 @@ static u8 b43_nphy_get_rx_core_state(struct b43_wldev *dev)
 }
 
 /**************************************************
- * RF (just without b43_nphy_rf_control_intc_override)
+ * RF (just without b43_nphy_rf_ctl_intc_override)
  **************************************************/
 
 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/ForceRFSeq */
@@ -128,9 +141,9 @@ ok:
 }
 
 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RFCtrlOverrideRev7 */
-static void b43_nphy_rf_control_override_rev7(struct b43_wldev *dev, u16 field,
-					      u16 value, u8 core, bool off,
-					      u8 override)
+static void b43_nphy_rf_ctl_override_rev7(struct b43_wldev *dev, u16 field,
+					  u16 value, u8 core, bool off,
+					  u8 override)
 {
 	const struct nphy_rf_control_override_rev7 *e;
 	u16 en_addrs[3][2] = {
@@ -168,8 +181,8 @@ static void b43_nphy_rf_control_override_rev7(struct b43_wldev *dev, u16 field,
 }
 
 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RFCtrlOverride */
-static void b43_nphy_rf_control_override(struct b43_wldev *dev, u16 field,
-						u16 value, u8 core, bool off)
+static void b43_nphy_rf_ctl_override(struct b43_wldev *dev, u16 field,
+				     u16 value, u8 core, bool off)
 {
 	int i;
 	u8 index = fls(field);
@@ -244,14 +257,14 @@ static void b43_nphy_rf_control_override(struct b43_wldev *dev, u16 field,
 }
 
 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RFCtrlIntcOverride */
-static void b43_nphy_rf_control_intc_override(struct b43_wldev *dev, u8 field,
-						u16 value, u8 core)
+static void b43_nphy_rf_ctl_intc_override(struct b43_wldev *dev,
+					  enum n_intc_override intc_override,
+					  u16 value, u8 core)
 {
 	u8 i, j;
 	u16 reg, tmp, val;
 
 	B43_WARN_ON(dev->phy.rev < 3);
-	B43_WARN_ON(field > 4);
 
 	for (i = 0; i < 2; i++) {
 		if ((core == 1 && i == 1) || (core == 2 && !i))
@@ -261,12 +274,12 @@ static void b43_nphy_rf_control_intc_override(struct b43_wldev *dev, u8 field,
 			B43_NPHY_RFCTL_INTC1 : B43_NPHY_RFCTL_INTC2;
 		b43_phy_set(dev, reg, 0x400);
 
-		switch (field) {
-		case 0:
+		switch (intc_override) {
+		case N_INTC_OVERRIDE_OFF:
 			b43_phy_write(dev, reg, 0);
 			b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RESET2RX);
 			break;
-		case 1:
+		case N_INTC_OVERRIDE_TRSW:
 			if (!i) {
 				b43_phy_maskset(dev, B43_NPHY_RFCTL_INTC1,
 						0xFC3F, (value << 6));
@@ -307,7 +320,7 @@ static void b43_nphy_rf_control_intc_override(struct b43_wldev *dev, u8 field,
 						0xFFFE);
 			}
 			break;
-		case 2:
+		case N_INTC_OVERRIDE_PA:
 			if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
 				tmp = 0x0020;
 				val = value << 5;
@@ -317,7 +330,7 @@ static void b43_nphy_rf_control_intc_override(struct b43_wldev *dev, u8 field,
 			}
 			b43_phy_maskset(dev, reg, ~tmp, val);
 			break;
-		case 3:
+		case N_INTC_OVERRIDE_EXT_LNA_PU:
 			if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
 				tmp = 0x0001;
 				val = value;
@@ -327,7 +340,7 @@ static void b43_nphy_rf_control_intc_override(struct b43_wldev *dev, u8 field,
 			}
 			b43_phy_maskset(dev, reg, ~tmp, val);
 			break;
-		case 4:
+		case N_INTC_OVERRIDE_EXT_LNA_GAIN:
 			if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
 				tmp = 0x0002;
 				val = value << 1;
@@ -1011,7 +1024,7 @@ static void b43_radio_init2055_post(struct b43_wldev *dev)
 
 	if (sprom->revision < 4)
 		workaround = (dev->dev->board_vendor != PCI_VENDOR_ID_BROADCOM
-			      && dev->dev->board_type == 0x46D
+			      && dev->dev->board_type == SSB_BOARD_CB2_4321
 			      && dev->dev->board_rev >= 0x41);
 	else
 		workaround =
@@ -1207,8 +1220,9 @@ static void b43_nphy_run_samples(struct b43_wldev *dev, u16 samps, u16 loops,
 
 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/ScaleOffsetRssi */
 static void b43_nphy_scale_offset_rssi(struct b43_wldev *dev, u16 scale,
-					s8 offset, u8 core, u8 rail,
-					enum b43_nphy_rssi_type type)
+					s8 offset, u8 core,
+					enum n_rail_type rail,
+					enum n_rssi_type rssi_type)
 {
 	u16 tmp;
 	bool core1or5 = (core == 1) || (core == 5);
@@ -1217,63 +1231,74 @@ static void b43_nphy_scale_offset_rssi(struct b43_wldev *dev, u16 scale,
 	offset = clamp_val(offset, -32, 31);
 	tmp = ((scale & 0x3F) << 8) | (offset & 0x3F);
 
-	if (core1or5 && (rail == 0) && (type == B43_NPHY_RSSI_Z))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_Z, tmp);
-	if (core1or5 && (rail == 1) && (type == B43_NPHY_RSSI_Z))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_Z, tmp);
-	if (core2or5 && (rail == 0) && (type == B43_NPHY_RSSI_Z))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_Z, tmp);
-	if (core2or5 && (rail == 1) && (type == B43_NPHY_RSSI_Z))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_Z, tmp);
-
-	if (core1or5 && (rail == 0) && (type == B43_NPHY_RSSI_X))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_X, tmp);
-	if (core1or5 && (rail == 1) && (type == B43_NPHY_RSSI_X))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_X, tmp);
-	if (core2or5 && (rail == 0) && (type == B43_NPHY_RSSI_X))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_X, tmp);
-	if (core2or5 && (rail == 1) && (type == B43_NPHY_RSSI_X))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_X, tmp);
-
-	if (core1or5 && (rail == 0) && (type == B43_NPHY_RSSI_Y))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_Y, tmp);
-	if (core1or5 && (rail == 1) && (type == B43_NPHY_RSSI_Y))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_Y, tmp);
-	if (core2or5 && (rail == 0) && (type == B43_NPHY_RSSI_Y))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_Y, tmp);
-	if (core2or5 && (rail == 1) && (type == B43_NPHY_RSSI_Y))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_Y, tmp);
-
-	if (core1or5 && (rail == 0) && (type == B43_NPHY_RSSI_TBD))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_0I_TBD, tmp);
-	if (core1or5 && (rail == 1) && (type == B43_NPHY_RSSI_TBD))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_TBD, tmp);
-	if (core2or5 && (rail == 0) && (type == B43_NPHY_RSSI_TBD))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_1I_TBD, tmp);
-	if (core2or5 && (rail == 1) && (type == B43_NPHY_RSSI_TBD))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_TBD, tmp);
-
-	if (core1or5 && (rail == 0) && (type == B43_NPHY_RSSI_PWRDET))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_0I_PWRDET, tmp);
-	if (core1or5 && (rail == 1) && (type == B43_NPHY_RSSI_PWRDET))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_PWRDET, tmp);
-	if (core2or5 && (rail == 0) && (type == B43_NPHY_RSSI_PWRDET))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_1I_PWRDET, tmp);
-	if (core2or5 && (rail == 1) && (type == B43_NPHY_RSSI_PWRDET))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_PWRDET, tmp);
-
-	if (core1or5 && (type == B43_NPHY_RSSI_TSSI_I))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_0I_TSSI, tmp);
-	if (core2or5 && (type == B43_NPHY_RSSI_TSSI_I))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_1I_TSSI, tmp);
-
-	if (core1or5 && (type == B43_NPHY_RSSI_TSSI_Q))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_TSSI, tmp);
-	if (core2or5 && (type == B43_NPHY_RSSI_TSSI_Q))
-		b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_TSSI, tmp);
+	switch (rssi_type) {
+	case N_RSSI_NB:
+		if (core1or5 && rail == N_RAIL_I)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_Z, tmp);
+		if (core1or5 && rail == N_RAIL_Q)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_Z, tmp);
+		if (core2or5 && rail == N_RAIL_I)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_Z, tmp);
+		if (core2or5 && rail == N_RAIL_Q)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_Z, tmp);
+		break;
+	case N_RSSI_W1:
+		if (core1or5 && rail == N_RAIL_I)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_X, tmp);
+		if (core1or5 && rail == N_RAIL_Q)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_X, tmp);
+		if (core2or5 && rail == N_RAIL_I)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_X, tmp);
+		if (core2or5 && rail == N_RAIL_Q)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_X, tmp);
+		break;
+	case N_RSSI_W2:
+		if (core1or5 && rail == N_RAIL_I)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_Y, tmp);
+		if (core1or5 && rail == N_RAIL_Q)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_Y, tmp);
+		if (core2or5 && rail == N_RAIL_I)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_1I_RSSI_Y, tmp);
+		if (core2or5 && rail == N_RAIL_Q)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_RSSI_Y, tmp);
+		break;
+	case N_RSSI_TBD:
+		if (core1or5 && rail == N_RAIL_I)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_0I_TBD, tmp);
+		if (core1or5 && rail == N_RAIL_Q)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_TBD, tmp);
+		if (core2or5 && rail == N_RAIL_I)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_1I_TBD, tmp);
+		if (core2or5 && rail == N_RAIL_Q)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_TBD, tmp);
+		break;
+	case N_RSSI_IQ:
+		if (core1or5 && rail == N_RAIL_I)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_0I_PWRDET, tmp);
+		if (core1or5 && rail == N_RAIL_Q)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_PWRDET, tmp);
+		if (core2or5 && rail == N_RAIL_I)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_1I_PWRDET, tmp);
+		if (core2or5 && rail == N_RAIL_Q)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_PWRDET, tmp);
+		break;
+	case N_RSSI_TSSI_2G:
+		if (core1or5)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_0I_TSSI, tmp);
+		if (core2or5)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_1I_TSSI, tmp);
+		break;
+	case N_RSSI_TSSI_5G:
+		if (core1or5)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_TSSI, tmp);
+		if (core2or5)
+			b43_phy_write(dev, B43_NPHY_RSSIMC_1Q_TSSI, tmp);
+		break;
+	}
 }
 
-static void b43_nphy_rev3_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
+static void b43_nphy_rev3_rssi_select(struct b43_wldev *dev, u8 code,
+				      enum n_rssi_type rssi_type)
 {
 	u8 i;
 	u16 reg, val;
@@ -1296,7 +1321,9 @@ static void b43_nphy_rev3_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
 				B43_NPHY_AFECTL_OVER1 : B43_NPHY_AFECTL_OVER;
 			b43_phy_maskset(dev, reg, 0xFDFF, 0x0200);
 
-			if (type < 3) {
+			if (rssi_type == N_RSSI_W1 ||
+			    rssi_type == N_RSSI_W2 ||
+			    rssi_type == N_RSSI_NB) {
 				reg = (i == 0) ?
 					B43_NPHY_AFECTL_C1 :
 					B43_NPHY_AFECTL_C2;
@@ -1307,9 +1334,9 @@ static void b43_nphy_rev3_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
 					B43_NPHY_RFCTL_LUT_TRSW_UP2;
 				b43_phy_maskset(dev, reg, 0xFFC3, 0);
 
-				if (type == 0)
+				if (rssi_type == N_RSSI_W1)
 					val = (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) ? 4 : 8;
-				else if (type == 1)
+				else if (rssi_type == N_RSSI_W2)
 					val = 16;
 				else
 					val = 32;
@@ -1320,9 +1347,9 @@ static void b43_nphy_rev3_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
 					B43_NPHY_TXF_40CO_B32S1;
 				b43_phy_set(dev, reg, 0x0020);
 			} else {
-				if (type == 6)
+				if (rssi_type == N_RSSI_TBD)
 					val = 0x0100;
-				else if (type == 3)
+				else if (rssi_type == N_RSSI_IQ)
 					val = 0x0200;
 				else
 					val = 0x0300;
@@ -1334,7 +1361,8 @@ static void b43_nphy_rev3_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
 				b43_phy_maskset(dev, reg, 0xFCFF, val);
 				b43_phy_maskset(dev, reg, 0xF3FF, val << 2);
 
-				if (type != 3 && type != 6) {
+				if (rssi_type != N_RSSI_IQ &&
+				    rssi_type != N_RSSI_TBD) {
 					enum ieee80211_band band =
 						b43_current_band(dev->wl);
 
@@ -1344,7 +1372,7 @@ static void b43_nphy_rev3_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
 						val = 0x11;
 					reg = (i == 0) ? 0x2000 : 0x3000;
 					reg |= B2055_PADDRV;
-					b43_radio_write16(dev, reg, val);
+					b43_radio_write(dev, reg, val);
 
 					reg = (i == 0) ?
 						B43_NPHY_AFECTL_OVER1 :
@@ -1356,33 +1384,43 @@ static void b43_nphy_rev3_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
 	}
 }
 
-static void b43_nphy_rev2_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
+static void b43_nphy_rev2_rssi_select(struct b43_wldev *dev, u8 code,
+				      enum n_rssi_type rssi_type)
 {
 	u16 val;
+	bool rssi_w1_w2_nb = false;
 
-	if (type < 3)
+	switch (rssi_type) {
+	case N_RSSI_W1:
+	case N_RSSI_W2:
+	case N_RSSI_NB:
 		val = 0;
-	else if (type == 6)
+		rssi_w1_w2_nb = true;
+		break;
+	case N_RSSI_TBD:
 		val = 1;
-	else if (type == 3)
+		break;
+	case N_RSSI_IQ:
 		val = 2;
-	else
+		break;
+	default:
 		val = 3;
+	}
 
 	val = (val << 12) | (val << 14);
 	b43_phy_maskset(dev, B43_NPHY_AFECTL_C1, 0x0FFF, val);
 	b43_phy_maskset(dev, B43_NPHY_AFECTL_C2, 0x0FFF, val);
 
-	if (type < 3) {
+	if (rssi_w1_w2_nb) {
 		b43_phy_maskset(dev, B43_NPHY_RFCTL_RSSIO1, 0xFFCF,
-				(type + 1) << 4);
+				(rssi_type + 1) << 4);
 		b43_phy_maskset(dev, B43_NPHY_RFCTL_RSSIO2, 0xFFCF,
-				(type + 1) << 4);
+				(rssi_type + 1) << 4);
 	}
 
 	if (code == 0) {
 		b43_phy_mask(dev, B43_NPHY_AFECTL_OVER, ~0x3000);
-		if (type < 3) {
+		if (rssi_w1_w2_nb) {
 			b43_phy_mask(dev, B43_NPHY_RFCTL_CMD,
 				~(B43_NPHY_RFCTL_CMD_RXEN |
 				  B43_NPHY_RFCTL_CMD_CORESEL));
@@ -1398,7 +1436,7 @@ static void b43_nphy_rev2_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
 		}
 	} else {
 		b43_phy_set(dev, B43_NPHY_AFECTL_OVER, 0x3000);
-		if (type < 3) {
+		if (rssi_w1_w2_nb) {
 			b43_phy_maskset(dev, B43_NPHY_RFCTL_CMD,
 				~(B43_NPHY_RFCTL_CMD_RXEN |
 				  B43_NPHY_RFCTL_CMD_CORESEL),
@@ -1418,7 +1456,8 @@ static void b43_nphy_rev2_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
 }
 
 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RSSISel */
-static void b43_nphy_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
+static void b43_nphy_rssi_select(struct b43_wldev *dev, u8 code,
+				 enum n_rssi_type type)
 {
 	if (dev->phy.rev >= 3)
 		b43_nphy_rev3_rssi_select(dev, code, type);
@@ -1427,11 +1466,12 @@ static void b43_nphy_rssi_select(struct b43_wldev *dev, u8 code, u8 type)
 }
 
 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/SetRssi2055Vcm */
-static void b43_nphy_set_rssi_2055_vcm(struct b43_wldev *dev, u8 type, u8 *buf)
+static void b43_nphy_set_rssi_2055_vcm(struct b43_wldev *dev,
+				       enum n_rssi_type rssi_type, u8 *buf)
 {
 	int i;
 	for (i = 0; i < 2; i++) {
-		if (type == 2) {
+		if (rssi_type == N_RSSI_NB) {
 			if (i == 0) {
 				b43_radio_maskset(dev, B2055_C1_B0NB_RSSIVCM,
 						  0xFC, buf[0]);
@@ -1455,8 +1495,8 @@ static void b43_nphy_set_rssi_2055_vcm(struct b43_wldev *dev, u8 type, u8 *buf)
 }
 
 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/PollRssi */
-static int b43_nphy_poll_rssi(struct b43_wldev *dev, u8 type, s32 *buf,
-				u8 nsamp)
+static int b43_nphy_poll_rssi(struct b43_wldev *dev, enum n_rssi_type rssi_type,
+			      s32 *buf, u8 nsamp)
 {
 	int i;
 	int out;
@@ -1487,7 +1527,7 @@ static int b43_nphy_poll_rssi(struct b43_wldev *dev, u8 type, s32 *buf,
 		save_regs_phy[8] = 0;
 	}
 
-	b43_nphy_rssi_select(dev, 5, type);
+	b43_nphy_rssi_select(dev, 5, rssi_type);
 
 	if (dev->phy.rev < 2) {
 		save_regs_phy[8] = b43_phy_read(dev, B43_NPHY_GPIO_SEL);
@@ -1574,7 +1614,7 @@ static void b43_nphy_rev3_rssi_cal(struct b43_wldev *dev)
 
 	u16 r; /* routing */
 	u8 rx_core_state;
-	u8 core, i, j;
+	int core, i, j, vcm;
 
 	class = b43_nphy_classifier(dev, 0, 0);
 	b43_nphy_classifier(dev, 7, 4);
@@ -1586,19 +1626,19 @@ static void b43_nphy_rev3_rssi_cal(struct b43_wldev *dev)
 	for (i = 0; i < ARRAY_SIZE(regs_to_store); i++)
 		saved_regs_phy[i] = b43_phy_read(dev, regs_to_store[i]);
 
-	b43_nphy_rf_control_intc_override(dev, 0, 0, 7);
-	b43_nphy_rf_control_intc_override(dev, 1, 1, 7);
-	b43_nphy_rf_control_override(dev, 0x1, 0, 0, false);
-	b43_nphy_rf_control_override(dev, 0x2, 1, 0, false);
-	b43_nphy_rf_control_override(dev, 0x80, 1, 0, false);
-	b43_nphy_rf_control_override(dev, 0x40, 1, 0, false);
+	b43_nphy_rf_ctl_intc_override(dev, N_INTC_OVERRIDE_OFF, 0, 7);
+	b43_nphy_rf_ctl_intc_override(dev, N_INTC_OVERRIDE_TRSW, 1, 7);
+	b43_nphy_rf_ctl_override(dev, 0x1, 0, 0, false);
+	b43_nphy_rf_ctl_override(dev, 0x2, 1, 0, false);
+	b43_nphy_rf_ctl_override(dev, 0x80, 1, 0, false);
+	b43_nphy_rf_ctl_override(dev, 0x40, 1, 0, false);
 
 	if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
-		b43_nphy_rf_control_override(dev, 0x20, 0, 0, false);
-		b43_nphy_rf_control_override(dev, 0x10, 1, 0, false);
+		b43_nphy_rf_ctl_override(dev, 0x20, 0, 0, false);
+		b43_nphy_rf_ctl_override(dev, 0x10, 1, 0, false);
 	} else {
-		b43_nphy_rf_control_override(dev, 0x10, 0, 0, false);
-		b43_nphy_rf_control_override(dev, 0x20, 1, 0, false);
+		b43_nphy_rf_ctl_override(dev, 0x10, 0, 0, false);
+		b43_nphy_rf_ctl_override(dev, 0x20, 1, 0, false);
 	}
 
 	rx_core_state = b43_nphy_get_rx_core_state(dev);
@@ -1606,35 +1646,44 @@ static void b43_nphy_rev3_rssi_cal(struct b43_wldev *dev)
 		if (!(rx_core_state & (1 << core)))
 			continue;
 		r = core ? B2056_RX1 : B2056_RX0;
-		b43_nphy_scale_offset_rssi(dev, 0, 0, core + 1, 0, 2);
-		b43_nphy_scale_offset_rssi(dev, 0, 0, core + 1, 1, 2);
-		for (i = 0; i < 8; i++) {
+		b43_nphy_scale_offset_rssi(dev, 0, 0, core + 1, N_RAIL_I,
+					   N_RSSI_NB);
+		b43_nphy_scale_offset_rssi(dev, 0, 0, core + 1, N_RAIL_Q,
+					   N_RSSI_NB);
+
+		/* Grab RSSI results for every possible VCM */
+		for (vcm = 0; vcm < 8; vcm++) {
 			b43_radio_maskset(dev, r | B2056_RX_RSSI_MISC, 0xE3,
-					i << 2);
-			b43_nphy_poll_rssi(dev, 2, results[i], 8);
+					vcm << 2);
+			b43_nphy_poll_rssi(dev, N_RSSI_NB, results[vcm], 8);
 		}
+
+		/* Find out which VCM got the best results */
 		for (i = 0; i < 4; i += 2) {
-			s32 curr;
+			s32 currd;
 			s32 mind = 0x100000;
 			s32 minpoll = 249;
 			u8 minvcm = 0;
 			if (2 * core != i)
 				continue;
-			for (j = 0; j < 8; j++) {
-				curr = results[j][i] * results[j][i] +
-					results[j][i + 1] * results[j][i];
-				if (curr < mind) {
-					mind = curr;
-					minvcm = j;
+			for (vcm = 0; vcm < 8; vcm++) {
+				currd = results[vcm][i] * results[vcm][i] +
+					results[vcm][i + 1] * results[vcm][i];
+				if (currd < mind) {
+					mind = currd;
+					minvcm = vcm;
 				}
-				if (results[j][i] < minpoll)
-					minpoll = results[j][i];
+				if (results[vcm][i] < minpoll)
+					minpoll = results[vcm][i];
 			}
 			vcm_final = minvcm;
 			results_min[i] = minpoll;
 		}
+
+		/* Select the best VCM */
 		b43_radio_maskset(dev, r | B2056_RX_RSSI_MISC, 0xE3,
 				  vcm_final << 2);
+
 		for (i = 0; i < 4; i++) {
 			if (core != i / 2)
 				continue;
@@ -1647,16 +1696,19 @@ static void b43_nphy_rev3_rssi_cal(struct b43_wldev *dev)
 				offset[i] = -32;
 			b43_nphy_scale_offset_rssi(dev, 0, offset[i],
 						   (i / 2 == 0) ? 1 : 2,
-						   (i % 2 == 0) ? 0 : 1,
-						   2);
+						   (i % 2 == 0) ? N_RAIL_I : N_RAIL_Q,
+						   N_RSSI_NB);
 		}
 	}
+
 	for (core = 0; core < 2; core++) {
 		if (!(rx_core_state & (1 << core)))
 			continue;
 		for (i = 0; i < 2; i++) {
-			b43_nphy_scale_offset_rssi(dev, 0, 0, core + 1, 0, i);
-			b43_nphy_scale_offset_rssi(dev, 0, 0, core + 1, 1, i);
+			b43_nphy_scale_offset_rssi(dev, 0, 0, core + 1,
+						   N_RAIL_I, i);
+			b43_nphy_scale_offset_rssi(dev, 0, 0, core + 1,
+						   N_RAIL_Q, i);
 			b43_nphy_poll_rssi(dev, i, poll_results, 8);
 			for (j = 0; j < 4; j++) {
 				if (j / 2 == core) {
@@ -1696,8 +1748,13 @@ static void b43_nphy_rev3_rssi_cal(struct b43_wldev *dev)
 		rssical_radio_regs = nphy->rssical_cache.rssical_radio_regs_5G;
 		rssical_phy_regs = nphy->rssical_cache.rssical_phy_regs_5G;
 	}
-	rssical_radio_regs[0] = b43_radio_read(dev, 0x602B);
-	rssical_radio_regs[0] = b43_radio_read(dev, 0x702B);
+	if (dev->phy.rev >= 7) {
+	} else {
+		rssical_radio_regs[0] = b43_radio_read(dev, B2056_RX0 |
+						       B2056_RX_RSSI_MISC);
+		rssical_radio_regs[1] = b43_radio_read(dev, B2056_RX1 |
+						       B2056_RX_RSSI_MISC);
+	}
 	rssical_phy_regs[0] = b43_phy_read(dev, B43_NPHY_RSSIMC_0I_RSSI_Z);
 	rssical_phy_regs[1] = b43_phy_read(dev, B43_NPHY_RSSIMC_0Q_RSSI_Z);
 	rssical_phy_regs[2] = b43_phy_read(dev, B43_NPHY_RSSIMC_1I_RSSI_Z);
@@ -1723,9 +1780,9 @@ static void b43_nphy_rev3_rssi_cal(struct b43_wldev *dev)
 }
 
 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/RSSICal */
-static void b43_nphy_rev2_rssi_cal(struct b43_wldev *dev, u8 type)
+static void b43_nphy_rev2_rssi_cal(struct b43_wldev *dev, enum n_rssi_type type)
 {
-	int i, j;
+	int i, j, vcm;
 	u8 state[4];
 	u8 code, val;
 	u16 class, override;
@@ -1743,10 +1800,10 @@ static void b43_nphy_rev2_rssi_cal(struct b43_wldev *dev, u8 type)
 	s32 results[4][4] = { };
 	s32 miniq[4][2] = { };
 
-	if (type == 2) {
+	if (type == N_RSSI_NB) {
 		code = 0;
 		val = 6;
-	} else if (type < 2) {
+	} else if (type == N_RSSI_W1 || type == N_RSSI_W2) {
 		code = 25;
 		val = 4;
 	} else {
@@ -1765,63 +1822,63 @@ static void b43_nphy_rev2_rssi_cal(struct b43_wldev *dev, u8 type)
 		override = 0x110;
 
 	regs_save_phy[0] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC1);
-	regs_save_radio[0] = b43_radio_read16(dev, B2055_C1_PD_RXTX);
+	regs_save_radio[0] = b43_radio_read(dev, B2055_C1_PD_RXTX);
 	b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, override);
-	b43_radio_write16(dev, B2055_C1_PD_RXTX, val);
+	b43_radio_write(dev, B2055_C1_PD_RXTX, val);
 
 	regs_save_phy[1] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC2);
-	regs_save_radio[1] = b43_radio_read16(dev, B2055_C2_PD_RXTX);
+	regs_save_radio[1] = b43_radio_read(dev, B2055_C2_PD_RXTX);
 	b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, override);
-	b43_radio_write16(dev, B2055_C2_PD_RXTX, val);
+	b43_radio_write(dev, B2055_C2_PD_RXTX, val);
 
-	state[0] = b43_radio_read16(dev, B2055_C1_PD_RSSIMISC) & 0x07;
-	state[1] = b43_radio_read16(dev, B2055_C2_PD_RSSIMISC) & 0x07;
+	state[0] = b43_radio_read(dev, B2055_C1_PD_RSSIMISC) & 0x07;
+	state[1] = b43_radio_read(dev, B2055_C2_PD_RSSIMISC) & 0x07;
 	b43_radio_mask(dev, B2055_C1_PD_RSSIMISC, 0xF8);
 	b43_radio_mask(dev, B2055_C2_PD_RSSIMISC, 0xF8);
-	state[2] = b43_radio_read16(dev, B2055_C1_SP_RSSI) & 0x07;
-	state[3] = b43_radio_read16(dev, B2055_C2_SP_RSSI) & 0x07;
+	state[2] = b43_radio_read(dev, B2055_C1_SP_RSSI) & 0x07;
+	state[3] = b43_radio_read(dev, B2055_C2_SP_RSSI) & 0x07;
 
 	b43_nphy_rssi_select(dev, 5, type);
-	b43_nphy_scale_offset_rssi(dev, 0, 0, 5, 0, type);
-	b43_nphy_scale_offset_rssi(dev, 0, 0, 5, 1, type);
+	b43_nphy_scale_offset_rssi(dev, 0, 0, 5, N_RAIL_I, type);
+	b43_nphy_scale_offset_rssi(dev, 0, 0, 5, N_RAIL_Q, type);
 
-	for (i = 0; i < 4; i++) {
+	for (vcm = 0; vcm < 4; vcm++) {
 		u8 tmp[4];
 		for (j = 0; j < 4; j++)
-			tmp[j] = i;
-		if (type != 1)
+			tmp[j] = vcm;
+		if (type != N_RSSI_W2)
 			b43_nphy_set_rssi_2055_vcm(dev, type, tmp);
-		b43_nphy_poll_rssi(dev, type, results[i], 8);
-		if (type < 2)
+		b43_nphy_poll_rssi(dev, type, results[vcm], 8);
+		if (type == N_RSSI_W1 || type == N_RSSI_W2)
 			for (j = 0; j < 2; j++)
-				miniq[i][j] = min(results[i][2 * j],
-						results[i][2 * j + 1]);
+				miniq[vcm][j] = min(results[vcm][2 * j],
+						    results[vcm][2 * j + 1]);
 	}
 
 	for (i = 0; i < 4; i++) {
 		s32 mind = 0x100000;
 		u8 minvcm = 0;
 		s32 minpoll = 249;
-		s32 curr;
-		for (j = 0; j < 4; j++) {
-			if (type == 2)
-				curr = abs(results[j][i]);
+		s32 currd;
+		for (vcm = 0; vcm < 4; vcm++) {
+			if (type == N_RSSI_NB)
+				currd = abs(results[vcm][i] - code * 8);
 			else
-				curr = abs(miniq[j][i / 2] - code * 8);
+				currd = abs(miniq[vcm][i / 2] - code * 8);
 
-			if (curr < mind) {
-				mind = curr;
-				minvcm = j;
+			if (currd < mind) {
+				mind = currd;
+				minvcm = vcm;
 			}
 
-			if (results[j][i] < minpoll)
-				minpoll = results[j][i];
+			if (results[vcm][i] < minpoll)
+				minpoll = results[vcm][i];
 		}
 		results_min[i] = minpoll;
 		vcm_final[i] = minvcm;
 	}
 
-	if (type != 1)
+	if (type != N_RSSI_W2)
 		b43_nphy_set_rssi_2055_vcm(dev, type, vcm_final);
 
 	for (i = 0; i < 4; i++) {
@@ -1836,7 +1893,7 @@ static void b43_nphy_rev2_rssi_cal(struct b43_wldev *dev, u8 type)
 			offset[i] = code - 32;
 
 		core = (i / 2) ? 2 : 1;
-		rail = (i % 2) ? 1 : 0;
+		rail = (i % 2) ? N_RAIL_Q : N_RAIL_I;
 
 		b43_nphy_scale_offset_rssi(dev, 0, offset[i], core, rail,
 						type);
@@ -1847,37 +1904,37 @@ static void b43_nphy_rev2_rssi_cal(struct b43_wldev *dev, u8 type)
 
 	switch (state[2]) {
 	case 1:
-		b43_nphy_rssi_select(dev, 1, 2);
+		b43_nphy_rssi_select(dev, 1, N_RSSI_NB);
 		break;
 	case 4:
-		b43_nphy_rssi_select(dev, 1, 0);
+		b43_nphy_rssi_select(dev, 1, N_RSSI_W1);
 		break;
 	case 2:
-		b43_nphy_rssi_select(dev, 1, 1);
+		b43_nphy_rssi_select(dev, 1, N_RSSI_W2);
 		break;
 	default:
-		b43_nphy_rssi_select(dev, 1, 1);
+		b43_nphy_rssi_select(dev, 1, N_RSSI_W2);
 		break;
 	}
 
 	switch (state[3]) {
 	case 1:
-		b43_nphy_rssi_select(dev, 2, 2);
+		b43_nphy_rssi_select(dev, 2, N_RSSI_NB);
 		break;
 	case 4:
-		b43_nphy_rssi_select(dev, 2, 0);
+		b43_nphy_rssi_select(dev, 2, N_RSSI_W1);
 		break;
 	default:
-		b43_nphy_rssi_select(dev, 2, 1);
+		b43_nphy_rssi_select(dev, 2, N_RSSI_W2);
 		break;
 	}
 
 	b43_nphy_rssi_select(dev, 0, type);
 
 	b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, regs_save_phy[0]);
-	b43_radio_write16(dev, B2055_C1_PD_RXTX, regs_save_radio[0]);
+	b43_radio_write(dev, B2055_C1_PD_RXTX, regs_save_radio[0]);
 	b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, regs_save_phy[1]);
-	b43_radio_write16(dev, B2055_C2_PD_RXTX, regs_save_radio[1]);
+	b43_radio_write(dev, B2055_C2_PD_RXTX, regs_save_radio[1]);
 
 	b43_nphy_classifier(dev, 7, class);
 	b43_nphy_write_clip_detection(dev, clip_state);
@@ -1895,9 +1952,9 @@ static void b43_nphy_rssi_cal(struct b43_wldev *dev)
 	if (dev->phy.rev >= 3) {
 		b43_nphy_rev3_rssi_cal(dev);
 	} else {
-		b43_nphy_rev2_rssi_cal(dev, B43_NPHY_RSSI_Z);
-		b43_nphy_rev2_rssi_cal(dev, B43_NPHY_RSSI_X);
-		b43_nphy_rev2_rssi_cal(dev, B43_NPHY_RSSI_Y);
+		b43_nphy_rev2_rssi_cal(dev, N_RSSI_NB);
+		b43_nphy_rev2_rssi_cal(dev, N_RSSI_W1);
+		b43_nphy_rev2_rssi_cal(dev, N_RSSI_W2);
 	}
 }
 
@@ -1930,10 +1987,8 @@ static void b43_nphy_gain_ctl_workarounds_rev3plus(struct b43_wldev *dev)
 	b43_phy_set(dev, B43_NPHY_RXCTL, 0x0040);
 
 	/* Set Clip 2 detect */
-	b43_phy_set(dev, B43_NPHY_C1_CGAINI,
-			B43_NPHY_C1_CGAINI_CL2DETECT);
-	b43_phy_set(dev, B43_NPHY_C2_CGAINI,
-			B43_NPHY_C2_CGAINI_CL2DETECT);
+	b43_phy_set(dev, B43_NPHY_C1_CGAINI, B43_NPHY_C1_CGAINI_CL2DETECT);
+	b43_phy_set(dev, B43_NPHY_C2_CGAINI, B43_NPHY_C2_CGAINI_CL2DETECT);
 
 	b43_radio_write(dev, B2056_RX0 | B2056_RX_BIASPOLE_LNAG1_IDAC,
 			0x17);
@@ -1967,22 +2022,22 @@ static void b43_nphy_gain_ctl_workarounds_rev3plus(struct b43_wldev *dev)
 	b43_ntab_write_bulk(dev, B43_NTAB8(2, 0x40), 6, lpf_bits);
 	b43_ntab_write_bulk(dev, B43_NTAB8(3, 0x40), 6, lpf_bits);
 
-	b43_phy_write(dev, B43_NPHY_C1_INITGAIN, e->init_gain);
-	b43_phy_write(dev, 0x2A7, e->init_gain);
+	b43_phy_write(dev, B43_NPHY_REV3_C1_INITGAIN_A, e->init_gain);
+	b43_phy_write(dev, B43_NPHY_REV3_C2_INITGAIN_A, e->init_gain);
+
 	b43_ntab_write_bulk(dev, B43_NTAB16(7, 0x106), 2,
 				e->rfseq_init);
 
-	/* TODO: check defines. Do not match variables names */
-	b43_phy_write(dev, B43_NPHY_C1_CLIP1_MEDGAIN, e->cliphi_gain);
-	b43_phy_write(dev, 0x2A9, e->cliphi_gain);
-	b43_phy_write(dev, B43_NPHY_C1_CLIP2_GAIN, e->clipmd_gain);
-	b43_phy_write(dev, 0x2AB, e->clipmd_gain);
-	b43_phy_write(dev, B43_NPHY_C2_CLIP1_HIGAIN, e->cliplo_gain);
-	b43_phy_write(dev, 0x2AD, e->cliplo_gain);
-
-	b43_phy_maskset(dev, 0x27D, 0xFF00, e->crsmin);
-	b43_phy_maskset(dev, 0x280, 0xFF00, e->crsminl);
-	b43_phy_maskset(dev, 0x283, 0xFF00, e->crsminu);
+	b43_phy_write(dev, B43_NPHY_REV3_C1_CLIP_HIGAIN_A, e->cliphi_gain);
+	b43_phy_write(dev, B43_NPHY_REV3_C2_CLIP_HIGAIN_A, e->cliphi_gain);
+	b43_phy_write(dev, B43_NPHY_REV3_C1_CLIP_MEDGAIN_A, e->clipmd_gain);
+	b43_phy_write(dev, B43_NPHY_REV3_C2_CLIP_MEDGAIN_A, e->clipmd_gain);
+	b43_phy_write(dev, B43_NPHY_REV3_C1_CLIP_LOGAIN_A, e->cliplo_gain);
+	b43_phy_write(dev, B43_NPHY_REV3_C2_CLIP_LOGAIN_A, e->cliplo_gain);
+
+	b43_phy_maskset(dev, B43_NPHY_CRSMINPOWER0, 0xFF00, e->crsmin);
+	b43_phy_maskset(dev, B43_NPHY_CRSMINPOWERL0, 0xFF00, e->crsminl);
+	b43_phy_maskset(dev, B43_NPHY_CRSMINPOWERU0, 0xFF00, e->crsminu);
 	b43_phy_write(dev, B43_NPHY_C1_NBCLIPTHRES, e->nbclip);
 	b43_phy_write(dev, B43_NPHY_C2_NBCLIPTHRES, e->nbclip);
 	b43_phy_maskset(dev, B43_NPHY_C1_CLIPWBTHRES,
@@ -2164,8 +2219,8 @@ static void b43_nphy_workarounds_rev7plus(struct b43_wldev *dev)
 		b43_phy_maskset(dev, B43_NPHY_FREQGAIN7, 0x80FF, 0x4000);
 	}
 	if (phy->rev <= 8) {
-		b43_phy_write(dev, 0x23F, 0x1B0);
-		b43_phy_write(dev, 0x240, 0x1B0);
+		b43_phy_write(dev, B43_NPHY_FORCEFRONT0, 0x1B0);
+		b43_phy_write(dev, B43_NPHY_FORCEFRONT1, 0x1B0);
 	}
 	if (phy->rev >= 8)
 		b43_phy_maskset(dev, B43_NPHY_TXTAILCNT, ~0xFF, 0x72);
@@ -2182,8 +2237,8 @@ static void b43_nphy_workarounds_rev7plus(struct b43_wldev *dev)
 		b43_nphy_set_rf_sequence(dev, 0, rx2tx_events_ipa,
 				rx2tx_delays_ipa, ARRAY_SIZE(rx2tx_events_ipa));
 
-	b43_phy_maskset(dev, 0x299, 0x3FFF, 0x4000);
-	b43_phy_maskset(dev, 0x29D, 0x3FFF, 0x4000);
+	b43_phy_maskset(dev, B43_NPHY_EPS_OVERRIDEI_0, 0x3FFF, 0x4000);
+	b43_phy_maskset(dev, B43_NPHY_EPS_OVERRIDEI_1, 0x3FFF, 0x4000);
 
 	lpf_20 = b43_nphy_read_lpf_ctl(dev, 0x154);
 	lpf_40 = b43_nphy_read_lpf_ctl(dev, 0x159);
@@ -2260,11 +2315,11 @@ static void b43_nphy_workarounds_rev7plus(struct b43_wldev *dev)
 			b43_ntab_write(dev, B43_NTAB16(7, 0x159 + core * 16),
 				       rx2tx_lut_40_11n);
 		}
-		b43_nphy_rf_control_override_rev7(dev, 16, 1, 3, false, 2);
+		b43_nphy_rf_ctl_override_rev7(dev, 16, 1, 3, false, 2);
 	}
 	b43_phy_write(dev, 0x32F, 0x3);
 	if (phy->radio_rev == 4 || phy->radio_rev == 6)
-		b43_nphy_rf_control_override_rev7(dev, 4, 1, 3, false, 0);
+		b43_nphy_rf_ctl_override_rev7(dev, 4, 1, 3, false, 0);
 
 	if (phy->radio_rev == 3 || phy->radio_rev == 4 || phy->radio_rev == 6) {
 		if (sprom->revision &&
@@ -2450,8 +2505,8 @@ static void b43_nphy_workarounds_rev3plus(struct b43_wldev *dev)
 	u16 tmp16;
 	u32 tmp32;
 
-	b43_phy_write(dev, 0x23f, 0x1f8);
-	b43_phy_write(dev, 0x240, 0x1f8);
+	b43_phy_write(dev, B43_NPHY_FORCEFRONT0, 0x1f8);
+	b43_phy_write(dev, B43_NPHY_FORCEFRONT1, 0x1f8);
 
 	tmp32 = b43_ntab_read(dev, B43_NTAB32(30, 0));
 	tmp32 &= 0xffffff;
@@ -2464,8 +2519,8 @@ static void b43_nphy_workarounds_rev3plus(struct b43_wldev *dev)
 	b43_phy_write(dev, B43_NPHY_PHASETR_B1, 0x00CD);
 	b43_phy_write(dev, B43_NPHY_PHASETR_B2, 0x0020);
 
-	b43_phy_write(dev, B43_NPHY_C2_CLIP1_MEDGAIN, 0x000C);
-	b43_phy_write(dev, 0x2AE, 0x000C);
+	b43_phy_write(dev, B43_NPHY_REV3_C1_CLIP_LOGAIN_B, 0x000C);
+	b43_phy_write(dev, B43_NPHY_REV3_C2_CLIP_LOGAIN_B, 0x000C);
 
 	/* TX to RX */
 	b43_nphy_set_rf_sequence(dev, 1, tx2rx_events, tx2rx_delays,
@@ -2490,7 +2545,7 @@ static void b43_nphy_workarounds_rev3plus(struct b43_wldev *dev)
 		0x2 : 0x9C40;
 	b43_phy_write(dev, B43_NPHY_ENDROP_TLEN, tmp16);
 
-	b43_phy_maskset(dev, 0x294, 0xF0FF, 0x0700);
+	b43_phy_maskset(dev, B43_NPHY_SGILTRNOFFSET, 0xF0FF, 0x0700);
 
 	if (!dev->phy.is_40mhz) {
 		b43_ntab_write(dev, B43_NTAB32(16, 3), 0x18D);
@@ -2542,18 +2597,18 @@ static void b43_nphy_workarounds_rev3plus(struct b43_wldev *dev)
 	}
 
 	/* Dropped probably-always-true condition */
-	b43_phy_write(dev, 0x224, 0x03eb);
-	b43_phy_write(dev, 0x225, 0x03eb);
-	b43_phy_write(dev, 0x226, 0x0341);
-	b43_phy_write(dev, 0x227, 0x0341);
-	b43_phy_write(dev, 0x228, 0x042b);
-	b43_phy_write(dev, 0x229, 0x042b);
-	b43_phy_write(dev, 0x22a, 0x0381);
-	b43_phy_write(dev, 0x22b, 0x0381);
-	b43_phy_write(dev, 0x22c, 0x042b);
-	b43_phy_write(dev, 0x22d, 0x042b);
-	b43_phy_write(dev, 0x22e, 0x0381);
-	b43_phy_write(dev, 0x22f, 0x0381);
+	b43_phy_write(dev, B43_NPHY_ED_CRS40ASSERTTHRESH0, 0x03eb);
+	b43_phy_write(dev, B43_NPHY_ED_CRS40ASSERTTHRESH1, 0x03eb);
+	b43_phy_write(dev, B43_NPHY_ED_CRS40DEASSERTTHRESH1, 0x0341);
+	b43_phy_write(dev, B43_NPHY_ED_CRS40DEASSERTTHRESH1, 0x0341);
+	b43_phy_write(dev, B43_NPHY_ED_CRS20LASSERTTHRESH0, 0x042b);
+	b43_phy_write(dev, B43_NPHY_ED_CRS20LASSERTTHRESH1, 0x042b);
+	b43_phy_write(dev, B43_NPHY_ED_CRS20LDEASSERTTHRESH0, 0x0381);
+	b43_phy_write(dev, B43_NPHY_ED_CRS20LDEASSERTTHRESH1, 0x0381);
+	b43_phy_write(dev, B43_NPHY_ED_CRS20UASSERTTHRESH0, 0x042b);
+	b43_phy_write(dev, B43_NPHY_ED_CRS20UASSERTTHRESH1, 0x042b);
+	b43_phy_write(dev, B43_NPHY_ED_CRS20UDEASSERTTHRESH0, 0x0381);
+	b43_phy_write(dev, B43_NPHY_ED_CRS20UDEASSERTTHRESH1, 0x0381);
 
 	if (dev->phy.rev >= 6 && sprom->boardflags2_lo & B43_BFL2_SINGLEANT_CCK)
 		; /* TODO: 0x0080000000000000 HF */
@@ -2572,7 +2627,7 @@ static void b43_nphy_workarounds_rev1_2(struct b43_wldev *dev)
 	u8 delays2[7] = { 0x8, 0x6, 0x2, 0x4, 0x4, 0x6, 0x1 };
 
 	if (sprom->boardflags2_lo & B43_BFL2_SKWRKFEM_BRD ||
-	    dev->dev->board_type == 0x8B) {
+	    dev->dev->board_type == BCMA_BOARD_TYPE_BCM943224M93) {
 		delays1[0] = 0x1;
 		delays1[5] = 0x14;
 	}
@@ -2789,10 +2844,6 @@ static void b43_nphy_iq_cal_gain_params(struct b43_wldev *dev, u16 core,
  * Tx and Rx
  **************************************************/
 
-void b43_nphy_set_rxantenna(struct b43_wldev *dev, int antenna)
-{//TODO
-}
-
 static void b43_nphy_op_adjust_txpower(struct b43_wldev *dev)
 {//TODO
 }
@@ -3124,21 +3175,21 @@ static void b43_nphy_tx_power_ctl_idle_tssi(struct b43_wldev *dev)
 		b43_nphy_ipa_internal_tssi_setup(dev);
 
 	if (phy->rev >= 7)
-		b43_nphy_rf_control_override_rev7(dev, 0x2000, 0, 3, false, 0);
+		b43_nphy_rf_ctl_override_rev7(dev, 0x2000, 0, 3, false, 0);
 	else if (phy->rev >= 3)
-		b43_nphy_rf_control_override(dev, 0x2000, 0, 3, false);
+		b43_nphy_rf_ctl_override(dev, 0x2000, 0, 3, false);
 
 	b43_nphy_stop_playback(dev);
 	b43_nphy_tx_tone(dev, 0xFA0, 0, false, false);
 	udelay(20);
-	tmp = b43_nphy_poll_rssi(dev, 4, rssi, 1);
+	tmp = b43_nphy_poll_rssi(dev, N_RSSI_TSSI_2G, rssi, 1);
 	b43_nphy_stop_playback(dev);
-	b43_nphy_rssi_select(dev, 0, 0);
+	b43_nphy_rssi_select(dev, 0, N_RSSI_W1);
 
 	if (phy->rev >= 7)
-		b43_nphy_rf_control_override_rev7(dev, 0x2000, 0, 3, true, 0);
+		b43_nphy_rf_ctl_override_rev7(dev, 0x2000, 0, 3, true, 0);
 	else if (phy->rev >= 3)
-		b43_nphy_rf_control_override(dev, 0x2000, 0, 3, true);
+		b43_nphy_rf_ctl_override(dev, 0x2000, 0, 3, true);
 
 	if (phy->rev >= 3) {
 		nphy->pwr_ctl_info[0].idle_tssi_5g = (tmp >> 24) & 0xFF;
@@ -3577,8 +3628,8 @@ static void b43_nphy_rx_cal_phy_setup(struct b43_wldev *dev, u8 core)
 		b43_phy_set(dev, B43_NPHY_AFECTL_OVER, 0x0007);
 	}
 
-	b43_nphy_rf_control_intc_override(dev, 2, 0, 3);
-	b43_nphy_rf_control_override(dev, 8, 0, 3, false);
+	b43_nphy_rf_ctl_intc_override(dev, N_INTC_OVERRIDE_PA, 0, 3);
+	b43_nphy_rf_ctl_override(dev, 8, 0, 3, false);
 	b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RX2TX);
 
 	if (core == 0) {
@@ -3588,8 +3639,10 @@ static void b43_nphy_rx_cal_phy_setup(struct b43_wldev *dev, u8 core)
 		rxval = 4;
 		txval = 2;
 	}
-	b43_nphy_rf_control_intc_override(dev, 1, rxval, (core + 1));
-	b43_nphy_rf_control_intc_override(dev, 1, txval, (2 - core));
+	b43_nphy_rf_ctl_intc_override(dev, N_INTC_OVERRIDE_TRSW, rxval,
+				      core + 1);
+	b43_nphy_rf_ctl_intc_override(dev, N_INTC_OVERRIDE_TRSW, txval,
+				      2 - core);
 }
 #endif
 
@@ -3851,9 +3904,13 @@ static void b43_nphy_restore_rssi_cal(struct b43_wldev *dev)
 		rssical_phy_regs = nphy->rssical_cache.rssical_phy_regs_5G;
 	}
 
-	/* TODO use some definitions */
-	b43_radio_maskset(dev, 0x602B, 0xE3, rssical_radio_regs[0]);
-	b43_radio_maskset(dev, 0x702B, 0xE3, rssical_radio_regs[1]);
+	if (dev->phy.rev >= 7) {
+	} else {
+		b43_radio_maskset(dev, B2056_RX0 | B2056_RX_RSSI_MISC, 0xE3,
+				  rssical_radio_regs[0]);
+		b43_radio_maskset(dev, B2056_RX1 | B2056_RX_RSSI_MISC, 0xE3,
+				  rssical_radio_regs[1]);
+	}
 
 	b43_phy_write(dev, B43_NPHY_RSSIMC_0I_RSSI_Z, rssical_phy_regs[0]);
 	b43_phy_write(dev, B43_NPHY_RSSIMC_0Q_RSSI_Z, rssical_phy_regs[1]);
@@ -3884,75 +3941,75 @@ static void b43_nphy_tx_cal_radio_setup(struct b43_wldev *dev)
 		tmp = (i == 0) ? 0x2000 : 0x3000;
 		offset = i * 11;
 
-		save[offset + 0] = b43_radio_read16(dev, B2055_CAL_RVARCTL);
-		save[offset + 1] = b43_radio_read16(dev, B2055_CAL_LPOCTL);
-		save[offset + 2] = b43_radio_read16(dev, B2055_CAL_TS);
-		save[offset + 3] = b43_radio_read16(dev, B2055_CAL_RCCALRTS);
-		save[offset + 4] = b43_radio_read16(dev, B2055_CAL_RCALRTS);
-		save[offset + 5] = b43_radio_read16(dev, B2055_PADDRV);
-		save[offset + 6] = b43_radio_read16(dev, B2055_XOCTL1);
-		save[offset + 7] = b43_radio_read16(dev, B2055_XOCTL2);
-		save[offset + 8] = b43_radio_read16(dev, B2055_XOREGUL);
-		save[offset + 9] = b43_radio_read16(dev, B2055_XOMISC);
-		save[offset + 10] = b43_radio_read16(dev, B2055_PLL_LFC1);
+		save[offset + 0] = b43_radio_read(dev, B2055_CAL_RVARCTL);
+		save[offset + 1] = b43_radio_read(dev, B2055_CAL_LPOCTL);
+		save[offset + 2] = b43_radio_read(dev, B2055_CAL_TS);
+		save[offset + 3] = b43_radio_read(dev, B2055_CAL_RCCALRTS);
+		save[offset + 4] = b43_radio_read(dev, B2055_CAL_RCALRTS);
+		save[offset + 5] = b43_radio_read(dev, B2055_PADDRV);
+		save[offset + 6] = b43_radio_read(dev, B2055_XOCTL1);
+		save[offset + 7] = b43_radio_read(dev, B2055_XOCTL2);
+		save[offset + 8] = b43_radio_read(dev, B2055_XOREGUL);
+		save[offset + 9] = b43_radio_read(dev, B2055_XOMISC);
+		save[offset + 10] = b43_radio_read(dev, B2055_PLL_LFC1);
 
 		if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
-			b43_radio_write16(dev, tmp | B2055_CAL_RVARCTL, 0x0A);
-			b43_radio_write16(dev, tmp | B2055_CAL_LPOCTL, 0x40);
-			b43_radio_write16(dev, tmp | B2055_CAL_TS, 0x55);
-			b43_radio_write16(dev, tmp | B2055_CAL_RCCALRTS, 0);
-			b43_radio_write16(dev, tmp | B2055_CAL_RCALRTS, 0);
+			b43_radio_write(dev, tmp | B2055_CAL_RVARCTL, 0x0A);
+			b43_radio_write(dev, tmp | B2055_CAL_LPOCTL, 0x40);
+			b43_radio_write(dev, tmp | B2055_CAL_TS, 0x55);
+			b43_radio_write(dev, tmp | B2055_CAL_RCCALRTS, 0);
+			b43_radio_write(dev, tmp | B2055_CAL_RCALRTS, 0);
 			if (nphy->ipa5g_on) {
-				b43_radio_write16(dev, tmp | B2055_PADDRV, 4);
-				b43_radio_write16(dev, tmp | B2055_XOCTL1, 1);
+				b43_radio_write(dev, tmp | B2055_PADDRV, 4);
+				b43_radio_write(dev, tmp | B2055_XOCTL1, 1);
 			} else {
-				b43_radio_write16(dev, tmp | B2055_PADDRV, 0);
-				b43_radio_write16(dev, tmp | B2055_XOCTL1, 0x2F);
+				b43_radio_write(dev, tmp | B2055_PADDRV, 0);
+				b43_radio_write(dev, tmp | B2055_XOCTL1, 0x2F);
 			}
-			b43_radio_write16(dev, tmp | B2055_XOCTL2, 0);
+			b43_radio_write(dev, tmp | B2055_XOCTL2, 0);
 		} else {
-			b43_radio_write16(dev, tmp | B2055_CAL_RVARCTL, 0x06);
-			b43_radio_write16(dev, tmp | B2055_CAL_LPOCTL, 0x40);
-			b43_radio_write16(dev, tmp | B2055_CAL_TS, 0x55);
-			b43_radio_write16(dev, tmp | B2055_CAL_RCCALRTS, 0);
-			b43_radio_write16(dev, tmp | B2055_CAL_RCALRTS, 0);
-			b43_radio_write16(dev, tmp | B2055_XOCTL1, 0);
+			b43_radio_write(dev, tmp | B2055_CAL_RVARCTL, 0x06);
+			b43_radio_write(dev, tmp | B2055_CAL_LPOCTL, 0x40);
+			b43_radio_write(dev, tmp | B2055_CAL_TS, 0x55);
+			b43_radio_write(dev, tmp | B2055_CAL_RCCALRTS, 0);
+			b43_radio_write(dev, tmp | B2055_CAL_RCALRTS, 0);
+			b43_radio_write(dev, tmp | B2055_XOCTL1, 0);
 			if (nphy->ipa2g_on) {
-				b43_radio_write16(dev, tmp | B2055_PADDRV, 6);
-				b43_radio_write16(dev, tmp | B2055_XOCTL2,
+				b43_radio_write(dev, tmp | B2055_PADDRV, 6);
+				b43_radio_write(dev, tmp | B2055_XOCTL2,
 					(dev->phy.rev < 5) ? 0x11 : 0x01);
 			} else {
-				b43_radio_write16(dev, tmp | B2055_PADDRV, 0);
-				b43_radio_write16(dev, tmp | B2055_XOCTL2, 0);
+				b43_radio_write(dev, tmp | B2055_PADDRV, 0);
+				b43_radio_write(dev, tmp | B2055_XOCTL2, 0);
 			}
 		}
-		b43_radio_write16(dev, tmp | B2055_XOREGUL, 0);
-		b43_radio_write16(dev, tmp | B2055_XOMISC, 0);
-		b43_radio_write16(dev, tmp | B2055_PLL_LFC1, 0);
+		b43_radio_write(dev, tmp | B2055_XOREGUL, 0);
+		b43_radio_write(dev, tmp | B2055_XOMISC, 0);
+		b43_radio_write(dev, tmp | B2055_PLL_LFC1, 0);
 	    }
 	} else {
-		save[0] = b43_radio_read16(dev, B2055_C1_TX_RF_IQCAL1);
-		b43_radio_write16(dev, B2055_C1_TX_RF_IQCAL1, 0x29);
+		save[0] = b43_radio_read(dev, B2055_C1_TX_RF_IQCAL1);
+		b43_radio_write(dev, B2055_C1_TX_RF_IQCAL1, 0x29);
 
-		save[1] = b43_radio_read16(dev, B2055_C1_TX_RF_IQCAL2);
-		b43_radio_write16(dev, B2055_C1_TX_RF_IQCAL2, 0x54);
+		save[1] = b43_radio_read(dev, B2055_C1_TX_RF_IQCAL2);
+		b43_radio_write(dev, B2055_C1_TX_RF_IQCAL2, 0x54);
 
-		save[2] = b43_radio_read16(dev, B2055_C2_TX_RF_IQCAL1);
-		b43_radio_write16(dev, B2055_C2_TX_RF_IQCAL1, 0x29);
+		save[2] = b43_radio_read(dev, B2055_C2_TX_RF_IQCAL1);
+		b43_radio_write(dev, B2055_C2_TX_RF_IQCAL1, 0x29);
 
-		save[3] = b43_radio_read16(dev, B2055_C2_TX_RF_IQCAL2);
-		b43_radio_write16(dev, B2055_C2_TX_RF_IQCAL2, 0x54);
+		save[3] = b43_radio_read(dev, B2055_C2_TX_RF_IQCAL2);
+		b43_radio_write(dev, B2055_C2_TX_RF_IQCAL2, 0x54);
 
-		save[3] = b43_radio_read16(dev, B2055_C1_PWRDET_RXTX);
-		save[4] = b43_radio_read16(dev, B2055_C2_PWRDET_RXTX);
+		save[3] = b43_radio_read(dev, B2055_C1_PWRDET_RXTX);
+		save[4] = b43_radio_read(dev, B2055_C2_PWRDET_RXTX);
 
 		if (!(b43_phy_read(dev, B43_NPHY_BANDCTL) &
 		    B43_NPHY_BANDCTL_5GHZ)) {
-			b43_radio_write16(dev, B2055_C1_PWRDET_RXTX, 0x04);
-			b43_radio_write16(dev, B2055_C2_PWRDET_RXTX, 0x04);
+			b43_radio_write(dev, B2055_C1_PWRDET_RXTX, 0x04);
+			b43_radio_write(dev, B2055_C2_PWRDET_RXTX, 0x04);
 		} else {
-			b43_radio_write16(dev, B2055_C1_PWRDET_RXTX, 0x20);
-			b43_radio_write16(dev, B2055_C2_PWRDET_RXTX, 0x20);
+			b43_radio_write(dev, B2055_C1_PWRDET_RXTX, 0x20);
+			b43_radio_write(dev, B2055_C2_PWRDET_RXTX, 0x20);
 		}
 
 		if (dev->phy.rev < 2) {
@@ -4148,9 +4205,9 @@ static void b43_nphy_tx_cal_phy_setup(struct b43_wldev *dev)
 		regs[7] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC1);
 		regs[8] = b43_phy_read(dev, B43_NPHY_RFCTL_INTC2);
 
-		b43_nphy_rf_control_intc_override(dev, 2, 1, 3);
-		b43_nphy_rf_control_intc_override(dev, 1, 2, 1);
-		b43_nphy_rf_control_intc_override(dev, 1, 8, 2);
+		b43_nphy_rf_ctl_intc_override(dev, N_INTC_OVERRIDE_PA, 1, 3);
+		b43_nphy_rf_ctl_intc_override(dev, N_INTC_OVERRIDE_TRSW, 2, 1);
+		b43_nphy_rf_ctl_intc_override(dev, N_INTC_OVERRIDE_TRSW, 8, 2);
 
 		regs[9] = b43_phy_read(dev, B43_NPHY_PAPD_EN0);
 		regs[10] = b43_phy_read(dev, B43_NPHY_PAPD_EN1);
@@ -4683,7 +4740,7 @@ static int b43_nphy_rev2_cal_rx_iq(struct b43_wldev *dev,
 
 			tmp[0] = ((cur_hpf2 << 8) | (cur_hpf1 << 4) |
 					(cur_lna << 2));
-			b43_nphy_rf_control_override(dev, 0x400, tmp[0], 3,
+			b43_nphy_rf_ctl_override(dev, 0x400, tmp[0], 3,
 									false);
 			b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RESET2RX);
 			b43_nphy_stop_playback(dev);
@@ -4732,7 +4789,7 @@ static int b43_nphy_rev2_cal_rx_iq(struct b43_wldev *dev,
 			break;
 	}
 
-	b43_nphy_rf_control_override(dev, 0x400, 0, 3, true);
+	b43_nphy_rf_ctl_override(dev, 0x400, 0, 3, true);
 	b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RESET2RX);
 	b43_ntab_write_bulk(dev, B43_NTAB16(7, 0x110), 2, gain_save);
 
@@ -4801,18 +4858,6 @@ static void b43_nphy_set_rx_core_state(struct b43_wldev *dev, u8 mask)
  * N-PHY init
  **************************************************/
 
-/*
- * Upload the N-PHY tables.
- * http://bcm-v4.sipsolutions.net/802.11/PHY/N/InitTables
- */
-static void b43_nphy_tables_init(struct b43_wldev *dev)
-{
-	if (dev->phy.rev < 3)
-		b43_nphy_rev0_1_2_tables_init(dev);
-	else
-		b43_nphy_rev3plus_tables_init(dev);
-}
-
 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/MIMOConfig */
 static void b43_nphy_update_mimo_config(struct b43_wldev *dev, s32 preamble)
 {
@@ -4892,7 +4937,7 @@ static void b43_nphy_superswitch_init(struct b43_wldev *dev, bool init)
 }
 
 /* http://bcm-v4.sipsolutions.net/802.11/PHY/Init/N */
-int b43_phy_initn(struct b43_wldev *dev)
+static int b43_phy_initn(struct b43_wldev *dev)
 {
 	struct ssb_sprom *sprom = dev->dev->bus_sprom;
 	struct b43_phy *phy = &dev->phy;
@@ -4962,7 +5007,7 @@ int b43_phy_initn(struct b43_wldev *dev)
 
 	if (sprom->boardflags2_lo & B43_BFL2_SKWRKFEM_BRD ||
 	    (dev->dev->board_vendor == PCI_VENDOR_ID_APPLE &&
-	     dev->dev->board_type == 0x8B))
+	     dev->dev->board_type == BCMA_BOARD_TYPE_BCM943224M93))
 		b43_phy_write(dev, B43_NPHY_TXREALFD, 0xA0);
 	else
 		b43_phy_write(dev, B43_NPHY_TXREALFD, 0xB8);
@@ -5104,63 +5149,11 @@ static void b43_chantab_phy_upload(struct b43_wldev *dev,
 /* http://bcm-v4.sipsolutions.net/802.11/PmuSpurAvoid */
 static void b43_nphy_pmu_spur_avoid(struct b43_wldev *dev, bool avoid)
 {
-	struct bcma_drv_cc __maybe_unused *cc;
-	u32 __maybe_unused pmu_ctl;
-
 	switch (dev->dev->bus_type) {
 #ifdef CONFIG_B43_BCMA
 	case B43_BUS_BCMA:
-		cc = &dev->dev->bdev->bus->drv_cc;
-		if (dev->dev->chip_id == 43224 || dev->dev->chip_id == 43225) {
-			if (avoid) {
-				bcma_chipco_pll_write(cc, 0x0, 0x11500010);
-				bcma_chipco_pll_write(cc, 0x1, 0x000C0C06);
-				bcma_chipco_pll_write(cc, 0x2, 0x0F600a08);
-				bcma_chipco_pll_write(cc, 0x3, 0x00000000);
-				bcma_chipco_pll_write(cc, 0x4, 0x2001E920);
-				bcma_chipco_pll_write(cc, 0x5, 0x88888815);
-			} else {
-				bcma_chipco_pll_write(cc, 0x0, 0x11100010);
-				bcma_chipco_pll_write(cc, 0x1, 0x000c0c06);
-				bcma_chipco_pll_write(cc, 0x2, 0x03000a08);
-				bcma_chipco_pll_write(cc, 0x3, 0x00000000);
-				bcma_chipco_pll_write(cc, 0x4, 0x200005c0);
-				bcma_chipco_pll_write(cc, 0x5, 0x88888815);
-			}
-			pmu_ctl = BCMA_CC_PMU_CTL_PLL_UPD;
-		} else if (dev->dev->chip_id == 0x4716) {
-			if (avoid) {
-				bcma_chipco_pll_write(cc, 0x0, 0x11500060);
-				bcma_chipco_pll_write(cc, 0x1, 0x080C0C06);
-				bcma_chipco_pll_write(cc, 0x2, 0x0F600000);
-				bcma_chipco_pll_write(cc, 0x3, 0x00000000);
-				bcma_chipco_pll_write(cc, 0x4, 0x2001E924);
-				bcma_chipco_pll_write(cc, 0x5, 0x88888815);
-			} else {
-				bcma_chipco_pll_write(cc, 0x0, 0x11100060);
-				bcma_chipco_pll_write(cc, 0x1, 0x080c0c06);
-				bcma_chipco_pll_write(cc, 0x2, 0x03000000);
-				bcma_chipco_pll_write(cc, 0x3, 0x00000000);
-				bcma_chipco_pll_write(cc, 0x4, 0x200005c0);
-				bcma_chipco_pll_write(cc, 0x5, 0x88888815);
-			}
-			pmu_ctl = BCMA_CC_PMU_CTL_PLL_UPD |
-				  BCMA_CC_PMU_CTL_NOILPONW;
-		} else if (dev->dev->chip_id == 0x4322 ||
-			   dev->dev->chip_id == 0x4340 ||
-			   dev->dev->chip_id == 0x4341) {
-			bcma_chipco_pll_write(cc, 0x0, 0x11100070);
-			bcma_chipco_pll_write(cc, 0x1, 0x1014140a);
-			bcma_chipco_pll_write(cc, 0x5, 0x88888854);
-			if (avoid)
-				bcma_chipco_pll_write(cc, 0x2, 0x05201828);
-			else
-				bcma_chipco_pll_write(cc, 0x2, 0x05001828);
-			pmu_ctl = BCMA_CC_PMU_CTL_PLL_UPD;
-		} else {
-			return;
-		}
-		bcma_cc_set32(cc, BCMA_CC_PMU_CTL, pmu_ctl);
+		bcma_pmu_spuravoid_pllupdate(&dev->dev->bdev->bus->drv_cc,
+					     avoid);
 		break;
 #endif
 #ifdef CONFIG_B43_SSB
@@ -5531,8 +5524,9 @@ static void b43_nphy_op_switch_analog(struct b43_wldev *dev, bool on)
 static int b43_nphy_op_switch_channel(struct b43_wldev *dev,
 				      unsigned int new_channel)
 {
-	struct ieee80211_channel *channel = dev->wl->hw->conf.channel;
-	enum nl80211_channel_type channel_type = dev->wl->hw->conf.channel_type;
+	struct ieee80211_channel *channel = dev->wl->hw->conf.chandef.chan;
+	enum nl80211_channel_type channel_type =
+		cfg80211_get_chandef_type(&dev->wl->hw->conf.chandef);
 
 	if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
 		if ((new_channel < 1) || (new_channel > 14))
diff --git a/drivers/net/wireless/b43/phy_n.h b/drivers/net/wireless/b43/phy_n.h
index 092c0140c249..9a5b6bc27d24 100644
--- a/drivers/net/wireless/b43/phy_n.h
+++ b/drivers/net/wireless/b43/phy_n.h
@@ -54,10 +54,15 @@
 #define  B43_NPHY_C1_INITGAIN_HPVGA2_SHIFT	7
 #define  B43_NPHY_C1_INITGAIN_TRRX		0x1000 /* TR RX index */
 #define  B43_NPHY_C1_INITGAIN_TRTX		0x2000 /* TR TX index */
+#define B43_NPHY_REV3_C1_INITGAIN_A		B43_PHY_N(0x020)
 #define B43_NPHY_C1_CLIP1_HIGAIN		B43_PHY_N(0x021) /* Core 1 clip1 high gain code */
+#define B43_NPHY_REV3_C1_INITGAIN_B		B43_PHY_N(0x021)
 #define B43_NPHY_C1_CLIP1_MEDGAIN		B43_PHY_N(0x022) /* Core 1 clip1 medium gain code */
+#define B43_NPHY_REV3_C1_CLIP_HIGAIN_A		B43_PHY_N(0x022)
 #define B43_NPHY_C1_CLIP1_LOGAIN		B43_PHY_N(0x023) /* Core 1 clip1 low gain code */
+#define B43_NPHY_REV3_C1_CLIP_HIGAIN_B		B43_PHY_N(0x023)
 #define B43_NPHY_C1_CLIP2_GAIN			B43_PHY_N(0x024) /* Core 1 clip2 gain code */
+#define B43_NPHY_REV3_C1_CLIP_MEDGAIN_A		B43_PHY_N(0x024)
 #define B43_NPHY_C1_FILTERGAIN			B43_PHY_N(0x025) /* Core 1 filter gain */
 #define B43_NPHY_C1_LPF_QHPF_BW			B43_PHY_N(0x026) /* Core 1 LPF Q HP F bandwidth */
 #define B43_NPHY_C1_CLIPWBTHRES			B43_PHY_N(0x027) /* Core 1 clip wideband threshold */
@@ -107,10 +112,15 @@
 #define  B43_NPHY_C2_INITGAIN_HPVGA2_SHIFT	7
 #define  B43_NPHY_C2_INITGAIN_TRRX		0x1000 /* TR RX index */
 #define  B43_NPHY_C2_INITGAIN_TRTX		0x2000 /* TR TX index */
+#define B43_NPHY_REV3_C1_CLIP_MEDGAIN_B		B43_PHY_N(0x036)
 #define B43_NPHY_C2_CLIP1_HIGAIN		B43_PHY_N(0x037) /* Core 2 clip1 high gain code */
+#define B43_NPHY_REV3_C1_CLIP_LOGAIN_A		B43_PHY_N(0x037)
 #define B43_NPHY_C2_CLIP1_MEDGAIN		B43_PHY_N(0x038) /* Core 2 clip1 medium gain code */
+#define B43_NPHY_REV3_C1_CLIP_LOGAIN_B		B43_PHY_N(0x038)
 #define B43_NPHY_C2_CLIP1_LOGAIN		B43_PHY_N(0x039) /* Core 2 clip1 low gain code */
+#define B43_NPHY_REV3_C1_CLIP2_GAIN_A		B43_PHY_N(0x039)
 #define B43_NPHY_C2_CLIP2_GAIN			B43_PHY_N(0x03A) /* Core 2 clip2 gain code */
+#define B43_NPHY_REV3_C1_CLIP2_GAIN_B		B43_PHY_N(0x03A)
 #define B43_NPHY_C2_FILTERGAIN			B43_PHY_N(0x03B) /* Core 2 filter gain */
 #define B43_NPHY_C2_LPF_QHPF_BW			B43_PHY_N(0x03C) /* Core 2 LPF Q HP F bandwidth */
 #define B43_NPHY_C2_CLIPWBTHRES			B43_PHY_N(0x03D) /* Core 2 clip wideband threshold */
@@ -706,10 +716,146 @@
 #define B43_NPHY_TXPCTL_INIT			B43_PHY_N(0x222) /* TX power control init */
 #define  B43_NPHY_TXPCTL_INIT_PIDXI1		0x00FF /* Power index init 1 */
 #define  B43_NPHY_TXPCTL_INIT_PIDXI1_SHIFT	0
+#define B43_NPHY_ED_CRSEN			B43_PHY_N(0x223)
+#define B43_NPHY_ED_CRS40ASSERTTHRESH0		B43_PHY_N(0x224)
+#define B43_NPHY_ED_CRS40ASSERTTHRESH1		B43_PHY_N(0x225)
+#define B43_NPHY_ED_CRS40DEASSERTTHRESH0	B43_PHY_N(0x226)
+#define B43_NPHY_ED_CRS40DEASSERTTHRESH1	B43_PHY_N(0x227)
+#define B43_NPHY_ED_CRS20LASSERTTHRESH0		B43_PHY_N(0x228)
+#define B43_NPHY_ED_CRS20LASSERTTHRESH1		B43_PHY_N(0x229)
+#define B43_NPHY_ED_CRS20LDEASSERTTHRESH0	B43_PHY_N(0x22A)
+#define B43_NPHY_ED_CRS20LDEASSERTTHRESH1	B43_PHY_N(0x22B)
+#define B43_NPHY_ED_CRS20UASSERTTHRESH0		B43_PHY_N(0x22C)
+#define B43_NPHY_ED_CRS20UASSERTTHRESH1		B43_PHY_N(0x22D)
+#define B43_NPHY_ED_CRS20UDEASSERTTHRESH0	B43_PHY_N(0x22E)
+#define B43_NPHY_ED_CRS20UDEASSERTTHRESH1	B43_PHY_N(0x22F)
+#define B43_NPHY_ED_CRS				B43_PHY_N(0x230)
+#define B43_NPHY_TIMEOUTEN			B43_PHY_N(0x231)
+#define B43_NPHY_OFDMPAYDECODETIMEOUTLEN	B43_PHY_N(0x232)
+#define B43_NPHY_CCKPAYDECODETIMEOUTLEN		B43_PHY_N(0x233)
+#define B43_NPHY_NONPAYDECODETIMEOUTLEN		B43_PHY_N(0x234)
+#define B43_NPHY_TIMEOUTSTATUS			B43_PHY_N(0x235)
+#define B43_NPHY_RFCTRLCORE0GPIO0		B43_PHY_N(0x236)
+#define B43_NPHY_RFCTRLCORE0GPIO1		B43_PHY_N(0x237)
+#define B43_NPHY_RFCTRLCORE0GPIO2		B43_PHY_N(0x238)
+#define B43_NPHY_RFCTRLCORE0GPIO3		B43_PHY_N(0x239)
+#define B43_NPHY_RFCTRLCORE1GPIO0		B43_PHY_N(0x23A)
+#define B43_NPHY_RFCTRLCORE1GPIO1		B43_PHY_N(0x23B)
+#define B43_NPHY_RFCTRLCORE1GPIO2		B43_PHY_N(0x23C)
+#define B43_NPHY_RFCTRLCORE1GPIO3		B43_PHY_N(0x23D)
+#define B43_NPHY_BPHYTESTCONTROL		B43_PHY_N(0x23E)
+/* REV3+ */
+#define B43_NPHY_FORCEFRONT0			B43_PHY_N(0x23F)
+#define B43_NPHY_FORCEFRONT1			B43_PHY_N(0x240)
+#define B43_NPHY_NORMVARHYSTTH			B43_PHY_N(0x241)
+#define B43_NPHY_TXCCKERROR			B43_PHY_N(0x242)
+#define B43_NPHY_AFESEQINITDACGAIN		B43_PHY_N(0x243)
+#define B43_NPHY_TXANTSWLUT			B43_PHY_N(0x244)
+#define B43_NPHY_CORECONFIG			B43_PHY_N(0x245)
+#define B43_NPHY_ANTENNADIVDWELLTIME		B43_PHY_N(0x246)
+#define B43_NPHY_ANTENNACCKDIVDWELLTIME		B43_PHY_N(0x247)
+#define B43_NPHY_ANTENNADIVBACKOFFGAIN		B43_PHY_N(0x248)
+#define B43_NPHY_ANTENNADIVMINGAIN		B43_PHY_N(0x249)
+#define B43_NPHY_BRDSEL_NORMVARHYSTTH		B43_PHY_N(0x24A)
+#define B43_NPHY_RXANTSWITCHCTRL		B43_PHY_N(0x24B)
+#define B43_NPHY_ENERGYDROPTIMEOUTLEN2		B43_PHY_N(0x24C)
+#define B43_NPHY_ML_LOG_TXEVM0			B43_PHY_N(0x250)
+#define B43_NPHY_ML_LOG_TXEVM1			B43_PHY_N(0x251)
+#define B43_NPHY_ML_LOG_TXEVM2			B43_PHY_N(0x252)
+#define B43_NPHY_ML_LOG_TXEVM3			B43_PHY_N(0x253)
+#define B43_NPHY_ML_LOG_TXEVM4			B43_PHY_N(0x254)
+#define B43_NPHY_ML_LOG_TXEVM5			B43_PHY_N(0x255)
+#define B43_NPHY_ML_LOG_TXEVM6			B43_PHY_N(0x256)
+#define B43_NPHY_ML_LOG_TXEVM7			B43_PHY_N(0x257)
+#define B43_NPHY_ML_SCALE_TWEAK			B43_PHY_N(0x258)
+#define B43_NPHY_MLUA				B43_PHY_N(0x259)
+#define B43_NPHY_ZFUA				B43_PHY_N(0x25A)
+#define B43_NPHY_CHANUPSYM01			B43_PHY_N(0x25B)
+#define B43_NPHY_CHANUPSYM2			B43_PHY_N(0x25C)
+#define B43_NPHY_RXSTRNFILT20NUM00		B43_PHY_N(0x25D)
+#define B43_NPHY_RXSTRNFILT20NUM01		B43_PHY_N(0x25E)
+#define B43_NPHY_RXSTRNFILT20NUM02		B43_PHY_N(0x25F)
+#define B43_NPHY_RXSTRNFILT20DEN00		B43_PHY_N(0x260)
+#define B43_NPHY_RXSTRNFILT20DEN01		B43_PHY_N(0x261)
+#define B43_NPHY_RXSTRNFILT20NUM10		B43_PHY_N(0x262)
+#define B43_NPHY_RXSTRNFILT20NUM11		B43_PHY_N(0x263)
+#define B43_NPHY_RXSTRNFILT20NUM12		B43_PHY_N(0x264)
+#define B43_NPHY_RXSTRNFILT20DEN10		B43_PHY_N(0x265)
+#define B43_NPHY_RXSTRNFILT20DEN11		B43_PHY_N(0x266)
+#define B43_NPHY_RXSTRNFILT40NUM00		B43_PHY_N(0x267)
+#define B43_NPHY_RXSTRNFILT40NUM01		B43_PHY_N(0x268)
+#define B43_NPHY_RXSTRNFILT40NUM02		B43_PHY_N(0x269)
+#define B43_NPHY_RXSTRNFILT40DEN00		B43_PHY_N(0x26A)
+#define B43_NPHY_RXSTRNFILT40DEN01		B43_PHY_N(0x26B)
+#define B43_NPHY_RXSTRNFILT40NUM10		B43_PHY_N(0x26C)
+#define B43_NPHY_RXSTRNFILT40NUM11		B43_PHY_N(0x26D)
+#define B43_NPHY_RXSTRNFILT40NUM12		B43_PHY_N(0x26E)
+#define B43_NPHY_RXSTRNFILT40DEN10		B43_PHY_N(0x26F)
+#define B43_NPHY_RXSTRNFILT40DEN11		B43_PHY_N(0x270)
+#define B43_NPHY_CRSHIGHPOWTHRESHOLD1		B43_PHY_N(0x271)
+#define B43_NPHY_CRSHIGHPOWTHRESHOLD2		B43_PHY_N(0x272)
+#define B43_NPHY_CRSHIGHLOWPOWTHRESHOLD		B43_PHY_N(0x273)
+#define B43_NPHY_CRSHIGHPOWTHRESHOLD1L		B43_PHY_N(0x274)
+#define B43_NPHY_CRSHIGHPOWTHRESHOLD2L		B43_PHY_N(0x275)
+#define B43_NPHY_CRSHIGHLOWPOWTHRESHOLDL	B43_PHY_N(0x276)
+#define B43_NPHY_CRSHIGHPOWTHRESHOLD1U		B43_PHY_N(0x277)
+#define B43_NPHY_CRSHIGHPOWTHRESHOLD2U		B43_PHY_N(0x278)
+#define B43_NPHY_CRSHIGHLOWPOWTHRESHOLDU	B43_PHY_N(0x279)
+#define B43_NPHY_CRSACIDETECTTHRESH		B43_PHY_N(0x27A)
+#define B43_NPHY_CRSACIDETECTTHRESHL		B43_PHY_N(0x27B)
+#define B43_NPHY_CRSACIDETECTTHRESHU		B43_PHY_N(0x27C)
+#define B43_NPHY_CRSMINPOWER0			B43_PHY_N(0x27D)
+#define B43_NPHY_CRSMINPOWER1			B43_PHY_N(0x27E)
+#define B43_NPHY_CRSMINPOWER2			B43_PHY_N(0x27F)
+#define B43_NPHY_CRSMINPOWERL0			B43_PHY_N(0x280)
+#define B43_NPHY_CRSMINPOWERL1			B43_PHY_N(0x281)
+#define B43_NPHY_CRSMINPOWERL2			B43_PHY_N(0x282)
+#define B43_NPHY_CRSMINPOWERU0			B43_PHY_N(0x283)
+#define B43_NPHY_CRSMINPOWERU1			B43_PHY_N(0x284)
+#define B43_NPHY_CRSMINPOWERU2			B43_PHY_N(0x285)
+#define B43_NPHY_STRPARAM			B43_PHY_N(0x286)
+#define B43_NPHY_STRPARAML			B43_PHY_N(0x287)
+#define B43_NPHY_STRPARAMU			B43_PHY_N(0x288)
+#define B43_NPHY_BPHYCRSMINPOWER0		B43_PHY_N(0x289)
+#define B43_NPHY_BPHYCRSMINPOWER1		B43_PHY_N(0x28A)
+#define B43_NPHY_BPHYCRSMINPOWER2		B43_PHY_N(0x28B)
+#define B43_NPHY_BPHYFILTDEN0COEF		B43_PHY_N(0x28C)
+#define B43_NPHY_BPHYFILTDEN1COEF		B43_PHY_N(0x28D)
+#define B43_NPHY_BPHYFILTDEN2COEF		B43_PHY_N(0x28E)
+#define B43_NPHY_BPHYFILTNUM0COEF		B43_PHY_N(0x28F)
+#define B43_NPHY_BPHYFILTNUM1COEF		B43_PHY_N(0x290)
+#define B43_NPHY_BPHYFILTNUM2COEF		B43_PHY_N(0x291)
+#define B43_NPHY_BPHYFILTNUM01COEF2		B43_PHY_N(0x292)
+#define B43_NPHY_BPHYFILTBYPASS			B43_PHY_N(0x293)
+#define B43_NPHY_SGILTRNOFFSET			B43_PHY_N(0x294)
+#define B43_NPHY_RADAR_T2_MIN			B43_PHY_N(0x295)
+#define B43_NPHY_TXPWRCTRLDAMPING		B43_PHY_N(0x296)
 #define B43_NPHY_PAPD_EN0			B43_PHY_N(0x297) /* PAPD Enable0 TBD */
 #define B43_NPHY_EPS_TABLE_ADJ0			B43_PHY_N(0x298) /* EPS Table Adj0 TBD */
+#define B43_NPHY_EPS_OVERRIDEI_0		B43_PHY_N(0x299)
+#define B43_NPHY_EPS_OVERRIDEQ_0		B43_PHY_N(0x29A)
 #define B43_NPHY_PAPD_EN1			B43_PHY_N(0x29B) /* PAPD Enable1 TBD */
 #define B43_NPHY_EPS_TABLE_ADJ1			B43_PHY_N(0x29C) /* EPS Table Adj1 TBD */
+#define B43_NPHY_EPS_OVERRIDEI_1		B43_PHY_N(0x29D)
+#define B43_NPHY_EPS_OVERRIDEQ_1		B43_PHY_N(0x29E)
+#define B43_NPHY_PAPD_CAL_ADDRESS		B43_PHY_N(0x29F)
+#define B43_NPHY_PAPD_CAL_YREFEPSILON		B43_PHY_N(0x2A0)
+#define B43_NPHY_PAPD_CAL_SETTLE		B43_PHY_N(0x2A1)
+#define B43_NPHY_PAPD_CAL_CORRELATE		B43_PHY_N(0x2A2)
+#define B43_NPHY_PAPD_CAL_SHIFTS0		B43_PHY_N(0x2A3)
+#define B43_NPHY_PAPD_CAL_SHIFTS1		B43_PHY_N(0x2A4)
+#define B43_NPHY_SAMPLE_START_ADDR		B43_PHY_N(0x2A5)
+#define B43_NPHY_RADAR_ADC_TO_DBM		B43_PHY_N(0x2A6)
+#define B43_NPHY_REV3_C2_INITGAIN_A		B43_PHY_N(0x2A7)
+#define B43_NPHY_REV3_C2_INITGAIN_B		B43_PHY_N(0x2A8)
+#define B43_NPHY_REV3_C2_CLIP_HIGAIN_A		B43_PHY_N(0x2A9)
+#define B43_NPHY_REV3_C2_CLIP_HIGAIN_B		B43_PHY_N(0x2AA)
+#define B43_NPHY_REV3_C2_CLIP_MEDGAIN_A		B43_PHY_N(0x2AB)
+#define B43_NPHY_REV3_C2_CLIP_MEDGAIN_B		B43_PHY_N(0x2AC)
+#define B43_NPHY_REV3_C2_CLIP_LOGAIN_A		B43_PHY_N(0x2AD)
+#define B43_NPHY_REV3_C2_CLIP_LOGAIN_B		B43_PHY_N(0x2AE)
+#define B43_NPHY_REV3_C2_CLIP2_GAIN_A		B43_PHY_N(0x2AF)
+#define B43_NPHY_REV3_C2_CLIP2_GAIN_B		B43_PHY_N(0x2B0)
 
 #define B43_PHY_B_BBCFG				B43_PHY_N_BMODE(0x001) /* BB config */
 #define B43_PHY_B_TEST				B43_PHY_N_BMODE(0x00A)
diff --git a/drivers/net/wireless/b43/radio_2056.c b/drivers/net/wireless/b43/radio_2056.c
index ce037fb6789a..b4fd9345d673 100644
--- a/drivers/net/wireless/b43/radio_2056.c
+++ b/drivers/net/wireless/b43/radio_2056.c
@@ -2980,7 +2980,7 @@ static const struct b2056_inittab_entry b2056_inittab_rev8_rx[] = {
 	.rx		= prefix##_rx,			\
 	.rx_length	= ARRAY_SIZE(prefix##_rx)
 
-struct b2056_inittabs_pts b2056_inittabs[] = {
+static const struct b2056_inittabs_pts b2056_inittabs[] = {
 	[3] = { INITTABSPTS(b2056_inittab_rev3) },
 	[4] = { INITTABSPTS(b2056_inittab_rev4) },
 	[5] = { INITTABSPTS(b2056_inittab_rev5) },
@@ -9035,7 +9035,7 @@ static void b2056_upload_inittab(struct b43_wldev *dev, bool ghz5,
 void b2056_upload_inittabs(struct b43_wldev *dev,
 			   bool ghz5, bool ignore_uploadflag)
 {
-	struct b2056_inittabs_pts *pts;
+	const struct b2056_inittabs_pts *pts;
 
 	if (dev->phy.rev >= ARRAY_SIZE(b2056_inittabs)) {
 		B43_WARN_ON(1);
@@ -9057,7 +9057,7 @@ void b2056_upload_inittabs(struct b43_wldev *dev,
 
 void b2056_upload_syn_pll_cp2(struct b43_wldev *dev, bool ghz5)
 {
-	struct b2056_inittabs_pts *pts;
+	const struct b2056_inittabs_pts *pts;
 	const struct b2056_inittab_entry *e;
 
 	if (dev->phy.rev >= ARRAY_SIZE(b2056_inittabs)) {
diff --git a/drivers/net/wireless/b43/radio_2059.c b/drivers/net/wireless/b43/radio_2059.c
index d4ce8a12ff9a..38e31d857e3e 100644
--- a/drivers/net/wireless/b43/radio_2059.c
+++ b/drivers/net/wireless/b43/radio_2059.c
@@ -27,7 +27,7 @@
 
 #define RADIOREGS(r00, r01, r02, r03, r04, r05, r06, r07, r08, r09, \
 		  r10, r11, r12, r13, r14, r15, r16, r17, r18, r19, \
-		  r20, r21, r22, r23, r24, r25, r26, r27, r28) \
+		  r20) \
 	.radio_syn16			= r00,	\
 	.radio_syn17			= r01,	\
 	.radio_syn22			= r02,	\
@@ -41,22 +41,14 @@
 	.radio_syn41			= r10,	\
 	.radio_syn43			= r11,	\
 	.radio_syn47			= r12,	\
-	.radio_syn4a			= r13,	\
-	.radio_syn58			= r14,	\
-	.radio_syn5a			= r15,	\
-	.radio_syn6a			= r16,	\
-	.radio_syn6d			= r17,	\
-	.radio_syn6e			= r18,	\
-	.radio_syn92			= r19,	\
-	.radio_syn98			= r20,	\
-	.radio_rxtx4a			= r21,	\
-	.radio_rxtx58			= r22,	\
-	.radio_rxtx5a			= r23,	\
-	.radio_rxtx6a			= r24,	\
-	.radio_rxtx6d			= r25,	\
-	.radio_rxtx6e			= r26,	\
-	.radio_rxtx92			= r27,	\
-	.radio_rxtx98			= r28
+	.radio_rxtx4a			= r13,	\
+	.radio_rxtx58			= r14,	\
+	.radio_rxtx5a			= r15,	\
+	.radio_rxtx6a			= r16,	\
+	.radio_rxtx6d			= r17,	\
+	.radio_rxtx6e			= r18,	\
+	.radio_rxtx92			= r19,	\
+	.radio_rxtx98			= r20
 
 #define PHYREGS(r0, r1, r2, r3, r4, r5)	\
 	.phy_regs.bw1	= r0,	\
@@ -70,91 +62,78 @@ static const struct b43_phy_ht_channeltab_e_radio2059 b43_phy_ht_channeltab_radi
   {	.freq			= 2412,
 	RADIOREGS(0x48, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x6c,
 		  0x09, 0x0f, 0x0a, 0x00, 0x0a, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x61, 0x03,
 		  0x00, 0x00, 0x00, 0xf0, 0x00),
 	PHYREGS(0x03c9, 0x03c5, 0x03c1, 0x043a, 0x043f, 0x0443),
   },
   {	.freq			= 2417,
 	RADIOREGS(0x4b, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x71,
 		  0x09, 0x0f, 0x0a, 0x00, 0x0a, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x61, 0x03,
 		  0x00, 0x00, 0x00, 0xf0, 0x00),
 	PHYREGS(0x03cb, 0x03c7, 0x03c3, 0x0438, 0x043d, 0x0441),
   },
   {	.freq			= 2422,
 	RADIOREGS(0x4e, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x76,
 		  0x09, 0x0f, 0x09, 0x00, 0x09, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x61, 0x03,
 		  0x00, 0x00, 0x00, 0xf0, 0x00),
 	PHYREGS(0x03cd, 0x03c9, 0x03c5, 0x0436, 0x043a, 0x043f),
   },
   {	.freq			= 2427,
 	RADIOREGS(0x52, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x7b,
 		  0x09, 0x0f, 0x09, 0x00, 0x09, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x61, 0x03,
 		  0x00, 0x00, 0x00, 0xf0, 0x00),
 	PHYREGS(0x03cf, 0x03cb, 0x03c7, 0x0434, 0x0438, 0x043d),
   },
   {	.freq			= 2432,
 	RADIOREGS(0x55, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x80,
 		  0x09, 0x0f, 0x08, 0x00, 0x08, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x61, 0x03,
 		  0x00, 0x00, 0x00, 0xf0, 0x00),
 	PHYREGS(0x03d1, 0x03cd, 0x03c9, 0x0431, 0x0436, 0x043a),
   },
   {	.freq			= 2437,
 	RADIOREGS(0x58, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x85,
 		  0x09, 0x0f, 0x08, 0x00, 0x08, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x61, 0x03,
 		  0x00, 0x00, 0x00, 0xf0, 0x00),
 	PHYREGS(0x03d3, 0x03cf, 0x03cb, 0x042f, 0x0434, 0x0438),
   },
   {	.freq			= 2442,
 	RADIOREGS(0x5c, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x8a,
 		  0x09, 0x0f, 0x07, 0x00, 0x07, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x61, 0x03,
 		  0x00, 0x00, 0x00, 0xf0, 0x00),
 	PHYREGS(0x03d5, 0x03d1, 0x03cd, 0x042d, 0x0431, 0x0436),
   },
   {	.freq			= 2447,
 	RADIOREGS(0x5f, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x8f,
 		  0x09, 0x0f, 0x07, 0x00, 0x07, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x61, 0x03,
 		  0x00, 0x00, 0x00, 0xf0, 0x00),
 	PHYREGS(0x03d7, 0x03d3, 0x03cf, 0x042b, 0x042f, 0x0434),
   },
   {	.freq			= 2452,
 	RADIOREGS(0x62, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x94,
 		  0x09, 0x0f, 0x07, 0x00, 0x07, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x61, 0x03,
 		  0x00, 0x00, 0x00, 0xf0, 0x00),
 	PHYREGS(0x03d9, 0x03d5, 0x03d1, 0x0429, 0x042d, 0x0431),
   },
   {	.freq			= 2457,
 	RADIOREGS(0x66, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x99,
 		  0x09, 0x0f, 0x06, 0x00, 0x06, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x61, 0x03,
 		  0x00, 0x00, 0x00, 0xf0, 0x00),
 	PHYREGS(0x03db, 0x03d7, 0x03d3, 0x0427, 0x042b, 0x042f),
   },
   {	.freq			= 2462,
 	RADIOREGS(0x69, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x9e,
 		  0x09, 0x0f, 0x06, 0x00, 0x06, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x61, 0x03,
 		  0x00, 0x00, 0x00, 0xf0, 0x00),
 	PHYREGS(0x03dd, 0x03d9, 0x03d5, 0x0424, 0x0429, 0x042d),
   },
   {	.freq			= 2467,
 	RADIOREGS(0x6c, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0xa3,
 		  0x09, 0x0f, 0x05, 0x00, 0x05, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x61, 0x03,
 		  0x00, 0x00, 0x00, 0xf0, 0x00),
 	PHYREGS(0x03df, 0x03db, 0x03d7, 0x0422, 0x0427, 0x042b),
   },
   {	.freq			= 2472,
 	RADIOREGS(0x70, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0xa8,
 		  0x09, 0x0f, 0x05, 0x00, 0x05, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x61, 0x03,
 		  0x00, 0x00, 0x00, 0xf0, 0x00),
 	PHYREGS(0x03e1, 0x03dd, 0x03d9, 0x0420, 0x0424, 0x0429),
   },
diff --git a/drivers/net/wireless/b43/radio_2059.h b/drivers/net/wireless/b43/radio_2059.h
index e4d69e55e9fe..40a82d7f510c 100644
--- a/drivers/net/wireless/b43/radio_2059.h
+++ b/drivers/net/wireless/b43/radio_2059.h
@@ -5,9 +5,9 @@
 
 #include "phy_ht.h"
 
-#define R2059_SYN			0x000
-#define R2059_TXRX0			0x400
-#define R2059_RXRX1			0x800
+#define R2059_C1			0x000
+#define R2059_C2			0x400
+#define R2059_C3			0x800
 #define R2059_ALL			0xC00
 
 /* Values for various registers uploaded on channel switching */
@@ -28,14 +28,6 @@ struct b43_phy_ht_channeltab_e_radio2059 {
 	u8 radio_syn41;
 	u8 radio_syn43;
 	u8 radio_syn47;
-	u8 radio_syn4a;
-	u8 radio_syn58;
-	u8 radio_syn5a;
-	u8 radio_syn6a;
-	u8 radio_syn6d;
-	u8 radio_syn6e;
-	u8 radio_syn92;
-	u8 radio_syn98;
 	u8 radio_rxtx4a;
 	u8 radio_rxtx58;
 	u8 radio_rxtx5a;
diff --git a/drivers/net/wireless/b43/sdio.h b/drivers/net/wireless/b43/sdio.h
index fb633094403a..1e93926f388f 100644
--- a/drivers/net/wireless/b43/sdio.h
+++ b/drivers/net/wireless/b43/sdio.h
@@ -25,12 +25,12 @@ void b43_sdio_exit(void);
 #else /* CONFIG_B43_SDIO */
 
 
-int b43_sdio_request_irq(struct b43_wldev *dev,
+static inline int b43_sdio_request_irq(struct b43_wldev *dev,
 			 void (*handler)(struct b43_wldev *dev))
 {
 	return -ENODEV;
 }
-void b43_sdio_free_irq(struct b43_wldev *dev)
+static inline void b43_sdio_free_irq(struct b43_wldev *dev)
 {
 }
 static inline int b43_sdio_init(void)
diff --git a/drivers/net/wireless/b43/tables_nphy.c b/drivers/net/wireless/b43/tables_nphy.c
index aaca60c6f575..94c755fdda14 100644
--- a/drivers/net/wireless/b43/tables_nphy.c
+++ b/drivers/net/wireless/b43/tables_nphy.c
@@ -2174,7 +2174,7 @@ static const u16 b43_ntab_loftlt1_r3[] = {
 /* volatile  tables, PHY revision >= 3 */
 
 /* indexed by antswctl2g */
-static const u16 b43_ntab_antswctl2g_r3[4][32] = {
+static const u16 b43_ntab_antswctl_r3[4][32] = {
 	{
 		0x0082, 0x0082, 0x0211, 0x0222, 0x0328,
 		0x0000, 0x0000, 0x0000, 0x0144, 0x0000,
@@ -2800,7 +2800,7 @@ static const struct nphy_rf_control_override_rev7
 	{ 0x0010, 0x344, 0x345, 0x0010, 4 },
 };
 
-struct nphy_gain_ctl_workaround_entry nphy_gain_ctl_wa_phy6_radio11_ghz2 = {
+static struct nphy_gain_ctl_workaround_entry nphy_gain_ctl_wa_phy6_radio11_ghz2 = {
 	{ 10, 14, 19, 27 },
 	{ -5, 6, 10, 15 },
 	{ 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA },
@@ -2811,7 +2811,7 @@ struct nphy_gain_ctl_workaround_entry nphy_gain_ctl_wa_phy6_radio11_ghz2 = {
 	0x18, 0x18, 0x18,
 	0x01D0, 0x5,
 };
-struct nphy_gain_ctl_workaround_entry nphy_gain_ctl_workaround[2][4] = {
+static struct nphy_gain_ctl_workaround_entry nphy_gain_ctl_workaround[2][4] = {
 	{ /* 2GHz */
 		{ /* PHY rev 3 */
 			{ 7, 11, 16, 23 },
@@ -3095,9 +3095,55 @@ void b43_ntab_write_bulk(struct b43_wldev *dev, u32 offset,
 }
 
 #define ntab_upload(dev, offset, data) do { \
-		b43_ntab_write_bulk(dev, offset, offset##_SIZE, data);	\
+		b43_ntab_write_bulk(dev, offset, ARRAY_SIZE(data), data); \
 	} while (0)
-void b43_nphy_rev0_1_2_tables_init(struct b43_wldev *dev)
+
+static void b43_nphy_tables_init_rev3(struct b43_wldev *dev)
+{
+	struct ssb_sprom *sprom = dev->dev->bus_sprom;
+	u8 antswlut;
+
+	if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ)
+		antswlut = sprom->fem.ghz5.antswlut;
+	else
+		antswlut = sprom->fem.ghz2.antswlut;
+
+	/* Static tables */
+	ntab_upload(dev, B43_NTAB_FRAMESTRUCT_R3, b43_ntab_framestruct_r3);
+	ntab_upload(dev, B43_NTAB_PILOT_R3, b43_ntab_pilot_r3);
+	ntab_upload(dev, B43_NTAB_TMAP_R3, b43_ntab_tmap_r3);
+	ntab_upload(dev, B43_NTAB_INTLEVEL_R3, b43_ntab_intlevel_r3);
+	ntab_upload(dev, B43_NTAB_TDTRN_R3, b43_ntab_tdtrn_r3);
+	ntab_upload(dev, B43_NTAB_NOISEVAR0_R3, b43_ntab_noisevar0_r3);
+	ntab_upload(dev, B43_NTAB_NOISEVAR1_R3, b43_ntab_noisevar1_r3);
+	ntab_upload(dev, B43_NTAB_MCS_R3, b43_ntab_mcs_r3);
+	ntab_upload(dev, B43_NTAB_TDI20A0_R3, b43_ntab_tdi20a0_r3);
+	ntab_upload(dev, B43_NTAB_TDI20A1_R3, b43_ntab_tdi20a1_r3);
+	ntab_upload(dev, B43_NTAB_TDI40A0_R3, b43_ntab_tdi40a0_r3);
+	ntab_upload(dev, B43_NTAB_TDI40A1_R3, b43_ntab_tdi40a1_r3);
+	ntab_upload(dev, B43_NTAB_PILOTLT_R3, b43_ntab_pilotlt_r3);
+	ntab_upload(dev, B43_NTAB_CHANEST_R3, b43_ntab_channelest_r3);
+	ntab_upload(dev, B43_NTAB_FRAMELT_R3, b43_ntab_framelookup_r3);
+	ntab_upload(dev, B43_NTAB_C0_ESTPLT_R3, b43_ntab_estimatepowerlt0_r3);
+	ntab_upload(dev, B43_NTAB_C1_ESTPLT_R3, b43_ntab_estimatepowerlt1_r3);
+	ntab_upload(dev, B43_NTAB_C0_ADJPLT_R3, b43_ntab_adjustpower0_r3);
+	ntab_upload(dev, B43_NTAB_C1_ADJPLT_R3, b43_ntab_adjustpower1_r3);
+	ntab_upload(dev, B43_NTAB_C0_GAINCTL_R3, b43_ntab_gainctl0_r3);
+	ntab_upload(dev, B43_NTAB_C1_GAINCTL_R3, b43_ntab_gainctl1_r3);
+	ntab_upload(dev, B43_NTAB_C0_IQLT_R3, b43_ntab_iqlt0_r3);
+	ntab_upload(dev, B43_NTAB_C1_IQLT_R3, b43_ntab_iqlt1_r3);
+	ntab_upload(dev, B43_NTAB_C0_LOFEEDTH_R3, b43_ntab_loftlt0_r3);
+	ntab_upload(dev, B43_NTAB_C1_LOFEEDTH_R3, b43_ntab_loftlt1_r3);
+
+	/* Volatile tables */
+	if (antswlut < ARRAY_SIZE(b43_ntab_antswctl_r3))
+		ntab_upload(dev, B43_NTAB_ANT_SW_CTL_R3,
+			    b43_ntab_antswctl_r3[antswlut]);
+	else
+		B43_WARN_ON(1);
+}
+
+static void b43_nphy_tables_init_rev0(struct b43_wldev *dev)
 {
 	/* Static tables */
 	ntab_upload(dev, B43_NTAB_FRAMESTRUCT, b43_ntab_framestruct);
@@ -3130,48 +3176,13 @@ void b43_nphy_rev0_1_2_tables_init(struct b43_wldev *dev)
 	ntab_upload(dev, B43_NTAB_C1_LOFEEDTH, b43_ntab_loftlt1);
 }
 
-#define ntab_upload_r3(dev, offset, data) do { \
-		b43_ntab_write_bulk(dev, offset, ARRAY_SIZE(data), data); \
-	} while (0)
-void b43_nphy_rev3plus_tables_init(struct b43_wldev *dev)
+/* http://bcm-v4.sipsolutions.net/802.11/PHY/N/InitTables */
+void b43_nphy_tables_init(struct b43_wldev *dev)
 {
-	struct ssb_sprom *sprom = dev->dev->bus_sprom;
-
-	/* Static tables */
-	ntab_upload_r3(dev, B43_NTAB_FRAMESTRUCT_R3, b43_ntab_framestruct_r3);
-	ntab_upload_r3(dev, B43_NTAB_PILOT_R3, b43_ntab_pilot_r3);
-	ntab_upload_r3(dev, B43_NTAB_TMAP_R3, b43_ntab_tmap_r3);
-	ntab_upload_r3(dev, B43_NTAB_INTLEVEL_R3, b43_ntab_intlevel_r3);
-	ntab_upload_r3(dev, B43_NTAB_TDTRN_R3, b43_ntab_tdtrn_r3);
-	ntab_upload_r3(dev, B43_NTAB_NOISEVAR0_R3, b43_ntab_noisevar0_r3);
-	ntab_upload_r3(dev, B43_NTAB_NOISEVAR1_R3, b43_ntab_noisevar1_r3);
-	ntab_upload_r3(dev, B43_NTAB_MCS_R3, b43_ntab_mcs_r3);
-	ntab_upload_r3(dev, B43_NTAB_TDI20A0_R3, b43_ntab_tdi20a0_r3);
-	ntab_upload_r3(dev, B43_NTAB_TDI20A1_R3, b43_ntab_tdi20a1_r3);
-	ntab_upload_r3(dev, B43_NTAB_TDI40A0_R3, b43_ntab_tdi40a0_r3);
-	ntab_upload_r3(dev, B43_NTAB_TDI40A1_R3, b43_ntab_tdi40a1_r3);
-	ntab_upload_r3(dev, B43_NTAB_PILOTLT_R3, b43_ntab_pilotlt_r3);
-	ntab_upload_r3(dev, B43_NTAB_CHANEST_R3, b43_ntab_channelest_r3);
-	ntab_upload_r3(dev, B43_NTAB_FRAMELT_R3, b43_ntab_framelookup_r3);
-	ntab_upload_r3(dev, B43_NTAB_C0_ESTPLT_R3,
-		       b43_ntab_estimatepowerlt0_r3);
-	ntab_upload_r3(dev, B43_NTAB_C1_ESTPLT_R3,
-		       b43_ntab_estimatepowerlt1_r3);
-	ntab_upload_r3(dev, B43_NTAB_C0_ADJPLT_R3, b43_ntab_adjustpower0_r3);
-	ntab_upload_r3(dev, B43_NTAB_C1_ADJPLT_R3, b43_ntab_adjustpower1_r3);
-	ntab_upload_r3(dev, B43_NTAB_C0_GAINCTL_R3, b43_ntab_gainctl0_r3);
-	ntab_upload_r3(dev, B43_NTAB_C1_GAINCTL_R3, b43_ntab_gainctl1_r3);
-	ntab_upload_r3(dev, B43_NTAB_C0_IQLT_R3, b43_ntab_iqlt0_r3);
-	ntab_upload_r3(dev, B43_NTAB_C1_IQLT_R3, b43_ntab_iqlt1_r3);
-	ntab_upload_r3(dev, B43_NTAB_C0_LOFEEDTH_R3, b43_ntab_loftlt0_r3);
-	ntab_upload_r3(dev, B43_NTAB_C1_LOFEEDTH_R3, b43_ntab_loftlt1_r3);
-
-	/* Volatile tables */
-	if (sprom->fem.ghz2.antswlut < ARRAY_SIZE(b43_ntab_antswctl2g_r3))
-		ntab_upload_r3(dev, B43_NTAB_ANT_SW_CTL_R3,
-			       b43_ntab_antswctl2g_r3[sprom->fem.ghz2.antswlut]);
+	if (dev->phy.rev >= 3)
+		b43_nphy_tables_init_rev3(dev);
 	else
-		B43_WARN_ON(1);
+		b43_nphy_tables_init_rev0(dev);
 }
 
 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/GetIpaGainTbl */
diff --git a/drivers/net/wireless/b43/tables_nphy.h b/drivers/net/wireless/b43/tables_nphy.h
index c600700ceedc..9ff33adcff89 100644
--- a/drivers/net/wireless/b43/tables_nphy.h
+++ b/drivers/net/wireless/b43/tables_nphy.h
@@ -115,22 +115,22 @@ struct nphy_gain_ctl_workaround_entry *b43_nphy_get_gain_ctl_workaround_ent(
 #define B43_NTAB_NOISEVAR11_SIZE	256
 #define B43_NTAB_C0_ESTPLT		B43_NTAB8 (0x1A, 0x000) /* Estimate Power Lookup Table Core 0 */
 #define B43_NTAB_C0_ESTPLT_SIZE		64
-#define B43_NTAB_C1_ESTPLT		B43_NTAB8 (0x1B, 0x000) /* Estimate Power Lookup Table Core 1 */
-#define B43_NTAB_C1_ESTPLT_SIZE		64
 #define B43_NTAB_C0_ADJPLT		B43_NTAB8 (0x1A, 0x040) /* Adjust Power Lookup Table Core 0 */
 #define B43_NTAB_C0_ADJPLT_SIZE		128
-#define B43_NTAB_C1_ADJPLT		B43_NTAB8 (0x1B, 0x040) /* Adjust Power Lookup Table Core 1 */
-#define B43_NTAB_C1_ADJPLT_SIZE		128
 #define B43_NTAB_C0_GAINCTL		B43_NTAB32(0x1A, 0x0C0) /* Gain Control Lookup Table Core 0 */
 #define B43_NTAB_C0_GAINCTL_SIZE	128
-#define B43_NTAB_C1_GAINCTL		B43_NTAB32(0x1B, 0x0C0) /* Gain Control Lookup Table Core 1 */
-#define B43_NTAB_C1_GAINCTL_SIZE	128
 #define B43_NTAB_C0_IQLT		B43_NTAB32(0x1A, 0x140) /* IQ Lookup Table Core 0 */
 #define B43_NTAB_C0_IQLT_SIZE		128
-#define B43_NTAB_C1_IQLT		B43_NTAB32(0x1B, 0x140) /* IQ Lookup Table Core 1 */
-#define B43_NTAB_C1_IQLT_SIZE		128
 #define B43_NTAB_C0_LOFEEDTH		B43_NTAB16(0x1A, 0x1C0) /* Local Oscillator Feed Through Lookup Table Core 0 */
 #define B43_NTAB_C0_LOFEEDTH_SIZE	128
+#define B43_NTAB_C1_ESTPLT		B43_NTAB8 (0x1B, 0x000) /* Estimate Power Lookup Table Core 1 */
+#define B43_NTAB_C1_ESTPLT_SIZE		64
+#define B43_NTAB_C1_ADJPLT		B43_NTAB8 (0x1B, 0x040) /* Adjust Power Lookup Table Core 1 */
+#define B43_NTAB_C1_ADJPLT_SIZE		128
+#define B43_NTAB_C1_GAINCTL		B43_NTAB32(0x1B, 0x0C0) /* Gain Control Lookup Table Core 1 */
+#define B43_NTAB_C1_GAINCTL_SIZE	128
+#define B43_NTAB_C1_IQLT		B43_NTAB32(0x1B, 0x140) /* IQ Lookup Table Core 1 */
+#define B43_NTAB_C1_IQLT_SIZE		128
 #define B43_NTAB_C1_LOFEEDTH		B43_NTAB16(0x1B, 0x1C0) /* Local Oscillator Feed Through Lookup Table Core 1 */
 #define B43_NTAB_C1_LOFEEDTH_SIZE	128
 
@@ -154,15 +154,17 @@ struct nphy_gain_ctl_workaround_entry *b43_nphy_get_gain_ctl_workaround_ent(
 #define B43_NTAB_CHANEST_R3		B43_NTAB32(22,   0) /* channel estimate  */
 #define B43_NTAB_FRAMELT_R3		 B43_NTAB8(24,   0) /* frame lookup  */
 #define B43_NTAB_C0_ESTPLT_R3		 B43_NTAB8(26,   0) /* estimated power lookup 0  */
-#define B43_NTAB_C1_ESTPLT_R3		 B43_NTAB8(27,   0) /* estimated power lookup 1  */
 #define B43_NTAB_C0_ADJPLT_R3		 B43_NTAB8(26,  64) /* adjusted power lookup 0  */
-#define B43_NTAB_C1_ADJPLT_R3		 B43_NTAB8(27,  64) /* adjusted power lookup 1  */
 #define B43_NTAB_C0_GAINCTL_R3		B43_NTAB32(26, 192) /* gain control lookup 0  */
-#define B43_NTAB_C1_GAINCTL_R3		B43_NTAB32(27, 192) /* gain control lookup 1  */
 #define B43_NTAB_C0_IQLT_R3		B43_NTAB32(26, 320) /* I/Q lookup 0  */
-#define B43_NTAB_C1_IQLT_R3		B43_NTAB32(27, 320) /* I/Q lookup 1  */
 #define B43_NTAB_C0_LOFEEDTH_R3		B43_NTAB16(26, 448) /* Local Oscillator Feed Through lookup 0  */
+#define B43_NTAB_C0_PAPD_COMP_R3	B43_NTAB16(26, 576)
+#define B43_NTAB_C1_ESTPLT_R3		 B43_NTAB8(27,   0) /* estimated power lookup 1  */
+#define B43_NTAB_C1_ADJPLT_R3		 B43_NTAB8(27,  64) /* adjusted power lookup 1  */
+#define B43_NTAB_C1_GAINCTL_R3		B43_NTAB32(27, 192) /* gain control lookup 1  */
+#define B43_NTAB_C1_IQLT_R3		B43_NTAB32(27, 320) /* I/Q lookup 1  */
 #define B43_NTAB_C1_LOFEEDTH_R3		B43_NTAB16(27, 448) /* Local Oscillator Feed Through lookup 1 */
+#define B43_NTAB_C1_PAPD_COMP_R3	B43_NTAB16(27, 576)
 
 #define B43_NTAB_TX_IQLO_CAL_LOFT_LADDER_40_SIZE	18
 #define B43_NTAB_TX_IQLO_CAL_LOFT_LADDER_20_SIZE	18
@@ -182,8 +184,7 @@ void b43_ntab_write(struct b43_wldev *dev, u32 offset, u32 value);
 void b43_ntab_write_bulk(struct b43_wldev *dev, u32 offset,
 			  unsigned int nr_elements, const void *_data);
 
-void b43_nphy_rev0_1_2_tables_init(struct b43_wldev *dev);
-void b43_nphy_rev3plus_tables_init(struct b43_wldev *dev);
+void b43_nphy_tables_init(struct b43_wldev *dev);
 
 const u32 *b43_nphy_get_tx_gain_table(struct b43_wldev *dev);
 
diff --git a/drivers/net/wireless/b43/tables_phy_lcn.c b/drivers/net/wireless/b43/tables_phy_lcn.c
index 5176363cadf2..e347b8d80ea4 100644
--- a/drivers/net/wireless/b43/tables_phy_lcn.c
+++ b/drivers/net/wireless/b43/tables_phy_lcn.c
@@ -313,7 +313,7 @@ static const u32 b43_lcntab_0x18[] = {
  * TX gain.
  **************************************************/
 
-const struct b43_lcntab_tx_gain_tbl_entry
+static const struct b43_lcntab_tx_gain_tbl_entry
 	b43_lcntab_tx_gain_tbl_2ghz_ext_pa_rev0[B43_LCNTAB_TX_GAIN_SIZE] = {
 	{ 0x03, 0x00, 0x1f, 0x0, 0x48 },
 	{ 0x03, 0x00, 0x1f, 0x0, 0x46 },
@@ -449,7 +449,7 @@ const struct b43_lcntab_tx_gain_tbl_entry
  * SW control.
  **************************************************/
 
-const u16 b43_lcntab_sw_ctl_4313_epa_rev0[] = {
+static const u16 b43_lcntab_sw_ctl_4313_epa_rev0[] = {
 	0x0002, 0x0008, 0x0004, 0x0001, 0x0002, 0x0008,
 	0x0004, 0x0001, 0x0002, 0x0008, 0x0004, 0x0001,
 	0x0002, 0x0008, 0x0004, 0x0001, 0x0002, 0x0008,
@@ -631,7 +631,7 @@ static void b43_phy_lcn_upload_static_tables(struct b43_wldev *dev)
 	lcntab_upload(dev, B43_LCNTAB32(0x18, 0), b43_lcntab_0x18);
 }
 
-void b43_phy_lcn_load_tx_gain_tab(struct b43_wldev *dev,
+static void b43_phy_lcn_load_tx_gain_tab(struct b43_wldev *dev,
 			const struct b43_lcntab_tx_gain_tbl_entry *gain_table)
 {
 	u32 i;
diff --git a/drivers/net/wireless/b43legacy/dma.c b/drivers/net/wireless/b43legacy/dma.c
index 2d3c6644f82d..faeafe219c57 100644
--- a/drivers/net/wireless/b43legacy/dma.c
+++ b/drivers/net/wireless/b43legacy/dma.c
@@ -334,13 +334,9 @@ static int alloc_ringmemory(struct b43legacy_dmaring *ring)
 	ring->descbase = dma_alloc_coherent(ring->dev->dev->dma_dev,
 					    B43legacy_DMA_RINGMEMSIZE,
 					    &(ring->dmabase),
-					    GFP_KERNEL);
-	if (!ring->descbase) {
-		b43legacyerr(ring->dev->wl, "DMA ringmemory allocation"
-			     " failed\n");
+					    GFP_KERNEL | __GFP_ZERO);
+	if (!ring->descbase)
 		return -ENOMEM;
-	}
-	memset(ring->descbase, 0, B43legacy_DMA_RINGMEMSIZE);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/b43legacy/main.c b/drivers/net/wireless/b43legacy/main.c
index 8c3f70e1a013..572668821862 100644
--- a/drivers/net/wireless/b43legacy/main.c
+++ b/drivers/net/wireless/b43legacy/main.c
@@ -2720,7 +2720,7 @@ static int b43legacy_op_dev_config(struct ieee80211_hw *hw,
 		goto out_unlock_mutex;
 
 	/* Switch the PHY mode (if necessary). */
-	switch (conf->channel->band) {
+	switch (conf->chandef.chan->band) {
 	case IEEE80211_BAND_2GHZ:
 		if (phy->type == B43legacy_PHYTYPE_B)
 			new_phymode = B43legacy_PHYMODE_B;
@@ -2748,8 +2748,9 @@ static int b43legacy_op_dev_config(struct ieee80211_hw *hw,
 
 	/* Switch to the requested channel.
 	 * The firmware takes care of races with the TX handler. */
-	if (conf->channel->hw_value != phy->channel)
-		b43legacy_radio_selectchannel(dev, conf->channel->hw_value, 0);
+	if (conf->chandef.chan->hw_value != phy->channel)
+		b43legacy_radio_selectchannel(dev, conf->chandef.chan->hw_value,
+					      0);
 
 	dev->wl->radiotap_enabled = !!(conf->flags & IEEE80211_CONF_MONITOR);
 
@@ -3558,7 +3559,7 @@ static int b43legacy_op_get_survey(struct ieee80211_hw *hw, int idx,
 	if (idx != 0)
 		return -ENOENT;
 
-	survey->channel = conf->channel;
+	survey->channel = conf->chandef.chan;
 	survey->filled = SURVEY_INFO_NOISE_DBM;
 	survey->noise = dev->stats.link_noise;
 
diff --git a/drivers/net/wireless/brcm80211/Kconfig b/drivers/net/wireless/brcm80211/Kconfig
index 1d92d874ebb6..fc8a0fa6d3b2 100644
--- a/drivers/net/wireless/brcm80211/Kconfig
+++ b/drivers/net/wireless/brcm80211/Kconfig
@@ -12,8 +12,9 @@ config BRCMSMAC
 	select CORDIC
 	---help---
 	  This module adds support for PCIe wireless adapters based on Broadcom
-	  IEEE802.11n SoftMAC chipsets.  If you choose to build a module, it'll
-	  be called brcmsmac.ko.
+	  IEEE802.11n SoftMAC chipsets. It also has WLAN led support, which will
+	  be available if you select BCMA_DRIVER_GPIO. If you choose to build a
+	  module, the driver will be called brcmsmac.ko.
 
 config BRCMFMAC
 	tristate "Broadcom IEEE802.11n embedded FullMAC WLAN driver"
@@ -36,15 +37,6 @@ config BRCMFMAC_SDIO
 	  IEEE802.11n embedded FullMAC WLAN driver. Say Y if you want to
 	  use the driver for a SDIO wireless card.
 
-config BRCMFMAC_SDIO_OOB
-	bool "Out of band interrupt support for SDIO interface chipset"
-	depends on BRCMFMAC_SDIO
-	---help---
-	  This option enables out-of-band interrupt support for Broadcom
-	  SDIO Wifi chipset using fullmac in order to gain better
-	  performance and deep sleep wake up capability on certain
-	  platforms. Say N if you are unsure.
-
 config BRCMFMAC_USB
 	bool "USB bus interface support for FullMAC driver"
 	depends on USB
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/Makefile b/drivers/net/wireless/brcm80211/brcmfmac/Makefile
index 756e19fc2795..8e9b1221b32c 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/Makefile
+++ b/drivers/net/wireless/brcm80211/brcmfmac/Makefile
@@ -26,10 +26,12 @@ brcmfmac-objs += \
 		wl_cfg80211.o \
 		fwil.o \
 		fweh.o \
+		fwsignal.o \
 		p2p.o \
 		dhd_cdc.o \
 		dhd_common.o \
-		dhd_linux.o
+		dhd_linux.o \
+		btcoex.o
 brcmfmac-$(CONFIG_BRCMFMAC_SDIO) += \
 		dhd_sdio.o \
 		bcmsdh.o \
@@ -39,3 +41,5 @@ brcmfmac-$(CONFIG_BRCMFMAC_USB) += \
 		usb.o
 brcmfmac-$(CONFIG_BRCMDBG) += \
 		dhd_dbg.o
+brcmfmac-$(CONFIG_BRCM_TRACING) += \
+		tracepoint.o
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
index 11fd1c735589..4891e3df2058 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
@@ -25,6 +25,7 @@
 #include <linux/mmc/sdio.h>
 #include <linux/mmc/sdio_func.h>
 #include <linux/mmc/card.h>
+#include <linux/platform_data/brcmfmac-sdio.h>
 
 #include <defs.h>
 #include <brcm_hw_ids.h>
@@ -37,16 +38,15 @@
 
 #define SDIOH_API_ACCESS_RETRY_LIMIT	2
 
-#ifdef CONFIG_BRCMFMAC_SDIO_OOB
-static irqreturn_t brcmf_sdio_irqhandler(int irq, void *dev_id)
+
+static irqreturn_t brcmf_sdio_oob_irqhandler(int irq, void *dev_id)
 {
 	struct brcmf_bus *bus_if = dev_get_drvdata(dev_id);
 	struct brcmf_sdio_dev *sdiodev = bus_if->bus_priv.sdio;
 
-	brcmf_dbg(INTR, "oob intr triggered\n");
+	brcmf_dbg(INTR, "OOB intr triggered\n");
 
-	/*
-	 * out-of-band interrupt is level-triggered which won't
+	/* out-of-band interrupt is level-triggered which won't
 	 * be cleared until dpc
 	 */
 	if (sdiodev->irq_en) {
@@ -59,72 +59,12 @@ static irqreturn_t brcmf_sdio_irqhandler(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-int brcmf_sdio_intr_register(struct brcmf_sdio_dev *sdiodev)
-{
-	int ret = 0;
-	u8 data;
-	unsigned long flags;
-
-	brcmf_dbg(TRACE, "Entering: irq %d\n", sdiodev->irq);
-
-	ret = request_irq(sdiodev->irq, brcmf_sdio_irqhandler,
-			  sdiodev->irq_flags, "brcmf_oob_intr",
-			  &sdiodev->func[1]->dev);
-	if (ret != 0)
-		return ret;
-	spin_lock_init(&sdiodev->irq_en_lock);
-	spin_lock_irqsave(&sdiodev->irq_en_lock, flags);
-	sdiodev->irq_en = true;
-	spin_unlock_irqrestore(&sdiodev->irq_en_lock, flags);
-
-	ret = enable_irq_wake(sdiodev->irq);
-	if (ret != 0)
-		return ret;
-	sdiodev->irq_wake = true;
-
-	sdio_claim_host(sdiodev->func[1]);
-
-	/* must configure SDIO_CCCR_IENx to enable irq */
-	data = brcmf_sdio_regrb(sdiodev, SDIO_CCCR_IENx, &ret);
-	data |= 1 << SDIO_FUNC_1 | 1 << SDIO_FUNC_2 | 1;
-	brcmf_sdio_regwb(sdiodev, SDIO_CCCR_IENx, data, &ret);
-
-	/* redirect, configure and enable io for interrupt signal */
-	data = SDIO_SEPINT_MASK | SDIO_SEPINT_OE;
-	if (sdiodev->irq_flags & IRQF_TRIGGER_HIGH)
-		data |= SDIO_SEPINT_ACT_HI;
-	brcmf_sdio_regwb(sdiodev, SDIO_CCCR_BRCM_SEPINT, data, &ret);
-
-	sdio_release_host(sdiodev->func[1]);
-
-	return 0;
-}
-
-int brcmf_sdio_intr_unregister(struct brcmf_sdio_dev *sdiodev)
-{
-	brcmf_dbg(TRACE, "Entering\n");
-
-	sdio_claim_host(sdiodev->func[1]);
-	brcmf_sdio_regwb(sdiodev, SDIO_CCCR_BRCM_SEPINT, 0, NULL);
-	brcmf_sdio_regwb(sdiodev, SDIO_CCCR_IENx, 0, NULL);
-	sdio_release_host(sdiodev->func[1]);
-
-	if (sdiodev->irq_wake) {
-		disable_irq_wake(sdiodev->irq);
-		sdiodev->irq_wake = false;
-	}
-	free_irq(sdiodev->irq, &sdiodev->func[1]->dev);
-	sdiodev->irq_en = false;
-
-	return 0;
-}
-#else		/* CONFIG_BRCMFMAC_SDIO_OOB */
-static void brcmf_sdio_irqhandler(struct sdio_func *func)
+static void brcmf_sdio_ib_irqhandler(struct sdio_func *func)
 {
 	struct brcmf_bus *bus_if = dev_get_drvdata(&func->dev);
 	struct brcmf_sdio_dev *sdiodev = bus_if->bus_priv.sdio;
 
-	brcmf_dbg(INTR, "ib intr triggered\n");
+	brcmf_dbg(INTR, "IB intr triggered\n");
 
 	brcmf_sdbrcm_isr(sdiodev->bus);
 }
@@ -136,28 +76,89 @@ static void brcmf_sdio_dummy_irqhandler(struct sdio_func *func)
 
 int brcmf_sdio_intr_register(struct brcmf_sdio_dev *sdiodev)
 {
-	brcmf_dbg(TRACE, "Entering\n");
+	int ret = 0;
+	u8 data;
+	unsigned long flags;
 
-	sdio_claim_host(sdiodev->func[1]);
-	sdio_claim_irq(sdiodev->func[1], brcmf_sdio_irqhandler);
-	sdio_claim_irq(sdiodev->func[2], brcmf_sdio_dummy_irqhandler);
-	sdio_release_host(sdiodev->func[1]);
+	if ((sdiodev->pdata) && (sdiodev->pdata->oob_irq_supported)) {
+		brcmf_dbg(SDIO, "Enter, register OOB IRQ %d\n",
+			  sdiodev->pdata->oob_irq_nr);
+		ret = request_irq(sdiodev->pdata->oob_irq_nr,
+				  brcmf_sdio_oob_irqhandler,
+				  sdiodev->pdata->oob_irq_flags,
+				  "brcmf_oob_intr",
+				  &sdiodev->func[1]->dev);
+		if (ret != 0) {
+			brcmf_err("request_irq failed %d\n", ret);
+			return ret;
+		}
+		sdiodev->oob_irq_requested = true;
+		spin_lock_init(&sdiodev->irq_en_lock);
+		spin_lock_irqsave(&sdiodev->irq_en_lock, flags);
+		sdiodev->irq_en = true;
+		spin_unlock_irqrestore(&sdiodev->irq_en_lock, flags);
+
+		ret = enable_irq_wake(sdiodev->pdata->oob_irq_nr);
+		if (ret != 0) {
+			brcmf_err("enable_irq_wake failed %d\n", ret);
+			return ret;
+		}
+		sdiodev->irq_wake = true;
+
+		sdio_claim_host(sdiodev->func[1]);
+
+		/* must configure SDIO_CCCR_IENx to enable irq */
+		data = brcmf_sdio_regrb(sdiodev, SDIO_CCCR_IENx, &ret);
+		data |= 1 << SDIO_FUNC_1 | 1 << SDIO_FUNC_2 | 1;
+		brcmf_sdio_regwb(sdiodev, SDIO_CCCR_IENx, data, &ret);
+
+		/* redirect, configure and enable io for interrupt signal */
+		data = SDIO_SEPINT_MASK | SDIO_SEPINT_OE;
+		if (sdiodev->pdata->oob_irq_flags & IRQF_TRIGGER_HIGH)
+			data |= SDIO_SEPINT_ACT_HI;
+		brcmf_sdio_regwb(sdiodev, SDIO_CCCR_BRCM_SEPINT, data, &ret);
+
+		sdio_release_host(sdiodev->func[1]);
+	} else {
+		brcmf_dbg(SDIO, "Entering\n");
+		sdio_claim_host(sdiodev->func[1]);
+		sdio_claim_irq(sdiodev->func[1], brcmf_sdio_ib_irqhandler);
+		sdio_claim_irq(sdiodev->func[2], brcmf_sdio_dummy_irqhandler);
+		sdio_release_host(sdiodev->func[1]);
+	}
 
 	return 0;
 }
 
 int brcmf_sdio_intr_unregister(struct brcmf_sdio_dev *sdiodev)
 {
-	brcmf_dbg(TRACE, "Entering\n");
-
-	sdio_claim_host(sdiodev->func[1]);
-	sdio_release_irq(sdiodev->func[2]);
-	sdio_release_irq(sdiodev->func[1]);
-	sdio_release_host(sdiodev->func[1]);
+	brcmf_dbg(SDIO, "Entering\n");
+
+	if ((sdiodev->pdata) && (sdiodev->pdata->oob_irq_supported)) {
+		sdio_claim_host(sdiodev->func[1]);
+		brcmf_sdio_regwb(sdiodev, SDIO_CCCR_BRCM_SEPINT, 0, NULL);
+		brcmf_sdio_regwb(sdiodev, SDIO_CCCR_IENx, 0, NULL);
+		sdio_release_host(sdiodev->func[1]);
+
+		if (sdiodev->oob_irq_requested) {
+			sdiodev->oob_irq_requested = false;
+			if (sdiodev->irq_wake) {
+				disable_irq_wake(sdiodev->pdata->oob_irq_nr);
+				sdiodev->irq_wake = false;
+			}
+			free_irq(sdiodev->pdata->oob_irq_nr,
+				 &sdiodev->func[1]->dev);
+			sdiodev->irq_en = false;
+		}
+	} else {
+		sdio_claim_host(sdiodev->func[1]);
+		sdio_release_irq(sdiodev->func[2]);
+		sdio_release_irq(sdiodev->func[1]);
+		sdio_release_host(sdiodev->func[1]);
+	}
 
 	return 0;
 }
-#endif		/* CONFIG_BRCMFMAC_SDIO_OOB */
 
 int
 brcmf_sdcard_set_sbaddr_window(struct brcmf_sdio_dev *sdiodev, u32 address)
@@ -253,9 +254,9 @@ u8 brcmf_sdio_regrb(struct brcmf_sdio_dev *sdiodev, u32 addr, int *ret)
 	u8 data;
 	int retval;
 
-	brcmf_dbg(INFO, "addr:0x%08x\n", addr);
+	brcmf_dbg(SDIO, "addr:0x%08x\n", addr);
 	retval = brcmf_sdio_regrw_helper(sdiodev, addr, &data, false);
-	brcmf_dbg(INFO, "data:0x%02x\n", data);
+	brcmf_dbg(SDIO, "data:0x%02x\n", data);
 
 	if (ret)
 		*ret = retval;
@@ -268,9 +269,9 @@ u32 brcmf_sdio_regrl(struct brcmf_sdio_dev *sdiodev, u32 addr, int *ret)
 	u32 data;
 	int retval;
 
-	brcmf_dbg(INFO, "addr:0x%08x\n", addr);
+	brcmf_dbg(SDIO, "addr:0x%08x\n", addr);
 	retval = brcmf_sdio_regrw_helper(sdiodev, addr, &data, false);
-	brcmf_dbg(INFO, "data:0x%08x\n", data);
+	brcmf_dbg(SDIO, "data:0x%08x\n", data);
 
 	if (ret)
 		*ret = retval;
@@ -283,7 +284,7 @@ void brcmf_sdio_regwb(struct brcmf_sdio_dev *sdiodev, u32 addr,
 {
 	int retval;
 
-	brcmf_dbg(INFO, "addr:0x%08x, data:0x%02x\n", addr, data);
+	brcmf_dbg(SDIO, "addr:0x%08x, data:0x%02x\n", addr, data);
 	retval = brcmf_sdio_regrw_helper(sdiodev, addr, &data, true);
 
 	if (ret)
@@ -295,7 +296,7 @@ void brcmf_sdio_regwl(struct brcmf_sdio_dev *sdiodev, u32 addr,
 {
 	int retval;
 
-	brcmf_dbg(INFO, "addr:0x%08x, data:0x%08x\n", addr, data);
+	brcmf_dbg(SDIO, "addr:0x%08x, data:0x%08x\n", addr, data);
 	retval = brcmf_sdio_regrw_helper(sdiodev, addr, &data, true);
 
 	if (ret)
@@ -358,7 +359,7 @@ brcmf_sdcard_recv_pkt(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
 	uint width;
 	int err = 0;
 
-	brcmf_dbg(INFO, "fun = %d, addr = 0x%x, size = %d\n",
+	brcmf_dbg(SDIO, "fun = %d, addr = 0x%x, size = %d\n",
 		  fn, addr, pkt->len);
 
 	width = (flags & SDIO_REQ_4BYTE) ? 4 : 2;
@@ -381,7 +382,7 @@ int brcmf_sdcard_recv_chain(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
 	uint width;
 	int err = 0;
 
-	brcmf_dbg(INFO, "fun = %d, addr = 0x%x, size = %d\n",
+	brcmf_dbg(SDIO, "fun = %d, addr = 0x%x, size = %d\n",
 		  fn, addr, pktq->qlen);
 
 	width = (flags & SDIO_REQ_4BYTE) ? 4 : 2;
@@ -428,7 +429,7 @@ brcmf_sdcard_send_pkt(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
 	uint bar0 = addr & ~SBSDIO_SB_OFT_ADDR_MASK;
 	int err = 0;
 
-	brcmf_dbg(INFO, "fun = %d, addr = 0x%x, size = %d\n",
+	brcmf_dbg(SDIO, "fun = %d, addr = 0x%x, size = %d\n",
 		  fn, addr, pkt->len);
 
 	/* Async not implemented yet */
@@ -457,48 +458,92 @@ done:
 	return err;
 }
 
-int brcmf_sdcard_rwdata(struct brcmf_sdio_dev *sdiodev, uint rw, u32 addr,
-			u8 *buf, uint nbytes)
+int
+brcmf_sdio_ramrw(struct brcmf_sdio_dev *sdiodev, bool write, u32 address,
+		 u8 *data, uint size)
 {
-	struct sk_buff *mypkt;
-	bool write = rw ? SDIOH_WRITE : SDIOH_READ;
-	int err;
+	int bcmerror = 0;
+	struct sk_buff *pkt;
+	u32 sdaddr;
+	uint dsize;
+
+	dsize = min_t(uint, SBSDIO_SB_OFT_ADDR_LIMIT, size);
+	pkt = dev_alloc_skb(dsize);
+	if (!pkt) {
+		brcmf_err("dev_alloc_skb failed: len %d\n", dsize);
+		return -EIO;
+	}
+	pkt->priority = 0;
 
-	addr &= SBSDIO_SB_OFT_ADDR_MASK;
-	addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
+	/* Determine initial transfer parameters */
+	sdaddr = address & SBSDIO_SB_OFT_ADDR_MASK;
+	if ((sdaddr + size) & SBSDIO_SBWINDOW_MASK)
+		dsize = (SBSDIO_SB_OFT_ADDR_LIMIT - sdaddr);
+	else
+		dsize = size;
 
-	mypkt = brcmu_pkt_buf_get_skb(nbytes);
-	if (!mypkt) {
-		brcmf_err("brcmu_pkt_buf_get_skb failed: len %d\n",
-			  nbytes);
-		return -EIO;
+	sdio_claim_host(sdiodev->func[1]);
+
+	/* Do the transfer(s) */
+	while (size) {
+		/* Set the backplane window to include the start address */
+		bcmerror = brcmf_sdcard_set_sbaddr_window(sdiodev, address);
+		if (bcmerror)
+			break;
+
+		brcmf_dbg(SDIO, "%s %d bytes at offset 0x%08x in window 0x%08x\n",
+			  write ? "write" : "read", dsize,
+			  sdaddr, address & SBSDIO_SBWINDOW_MASK);
+
+		sdaddr &= SBSDIO_SB_OFT_ADDR_MASK;
+		sdaddr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
+
+		skb_put(pkt, dsize);
+		if (write)
+			memcpy(pkt->data, data, dsize);
+		bcmerror = brcmf_sdioh_request_buffer(sdiodev, SDIOH_DATA_INC,
+						      write, SDIO_FUNC_1,
+						      sdaddr, pkt);
+		if (bcmerror) {
+			brcmf_err("membytes transfer failed\n");
+			break;
+		}
+		if (!write)
+			memcpy(data, pkt->data, dsize);
+		skb_trim(pkt, dsize);
+
+		/* Adjust for next transfer (if any) */
+		size -= dsize;
+		if (size) {
+			data += dsize;
+			address += dsize;
+			sdaddr = 0;
+			dsize = min_t(uint, SBSDIO_SB_OFT_ADDR_LIMIT, size);
+		}
 	}
 
-	/* For a write, copy the buffer data into the packet. */
-	if (write)
-		memcpy(mypkt->data, buf, nbytes);
+	dev_kfree_skb(pkt);
 
-	err = brcmf_sdioh_request_buffer(sdiodev, SDIOH_DATA_INC, write,
-					 SDIO_FUNC_1, addr, mypkt);
+	/* Return the window to backplane enumeration space for core access */
+	if (brcmf_sdcard_set_sbaddr_window(sdiodev, sdiodev->sbwad))
+		brcmf_err("FAILED to set window back to 0x%x\n",
+			  sdiodev->sbwad);
 
-	/* For a read, copy the packet data back to the buffer. */
-	if (!err && !write)
-		memcpy(buf, mypkt->data, nbytes);
+	sdio_release_host(sdiodev->func[1]);
 
-	brcmu_pkt_buf_free_skb(mypkt);
-	return err;
+	return bcmerror;
 }
 
 int brcmf_sdcard_abort(struct brcmf_sdio_dev *sdiodev, uint fn)
 {
 	char t_func = (char)fn;
-	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_dbg(SDIO, "Enter\n");
 
 	/* issue abort cmd52 command through F0 */
 	brcmf_sdioh_request_byte(sdiodev, SDIOH_WRITE, SDIO_FUNC_0,
 				 SDIO_CCCR_ABORT, &t_func);
 
-	brcmf_dbg(TRACE, "Exit\n");
+	brcmf_dbg(SDIO, "Exit\n");
 	return 0;
 }
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
index d92d373733d7..44fa0cdbf97b 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
@@ -26,6 +26,7 @@
 #include <linux/sched.h>	/* request_irq() */
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/platform_data/brcmfmac-sdio.h>
 #include <net/cfg80211.h>
 
 #include <defs.h>
@@ -40,32 +41,30 @@
 
 #define DMA_ALIGN_MASK	0x03
 
+#define SDIO_DEVICE_ID_BROADCOM_43143	43143
 #define SDIO_DEVICE_ID_BROADCOM_43241	0x4324
 #define SDIO_DEVICE_ID_BROADCOM_4329	0x4329
 #define SDIO_DEVICE_ID_BROADCOM_4330	0x4330
 #define SDIO_DEVICE_ID_BROADCOM_4334	0x4334
+#define SDIO_DEVICE_ID_BROADCOM_4335	0x4335
 
 #define SDIO_FUNC1_BLOCKSIZE		64
 #define SDIO_FUNC2_BLOCKSIZE		512
 
 /* devices we support, null terminated */
 static const struct sdio_device_id brcmf_sdmmc_ids[] = {
+	{SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_43143)},
 	{SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_43241)},
 	{SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4329)},
 	{SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4330)},
 	{SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4334)},
+	{SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4335)},
 	{ /* end: all zeroes */ },
 };
 MODULE_DEVICE_TABLE(sdio, brcmf_sdmmc_ids);
 
-#ifdef CONFIG_BRCMFMAC_SDIO_OOB
-static struct list_head oobirq_lh;
-struct brcmf_sdio_oobirq {
-	unsigned int irq;
-	unsigned long flags;
-	struct list_head list;
-};
-#endif		/* CONFIG_BRCMFMAC_SDIO_OOB */
+static struct brcmfmac_sdio_platform_data *brcmfmac_sdio_pdata;
+
 
 static bool
 brcmf_pm_resume_error(struct brcmf_sdio_dev *sdiodev)
@@ -139,7 +138,7 @@ int brcmf_sdioh_request_byte(struct brcmf_sdio_dev *sdiodev, uint rw, uint func,
 {
 	int err_ret;
 
-	brcmf_dbg(INFO, "rw=%d, func=%d, addr=0x%05x\n", rw, func, regaddr);
+	brcmf_dbg(SDIO, "rw=%d, func=%d, addr=0x%05x\n", rw, func, regaddr);
 
 	brcmf_pm_resume_wait(sdiodev, &sdiodev->request_byte_wait);
 	if (brcmf_pm_resume_error(sdiodev))
@@ -179,7 +178,7 @@ int brcmf_sdioh_request_word(struct brcmf_sdio_dev *sdiodev,
 		return -EINVAL;
 	}
 
-	brcmf_dbg(INFO, "rw=%d, func=%d, addr=0x%05x, nbytes=%d\n",
+	brcmf_dbg(SDIO, "rw=%d, func=%d, addr=0x%05x, nbytes=%d\n",
 		  rw, func, addr, nbytes);
 
 	brcmf_pm_resume_wait(sdiodev, &sdiodev->request_word_wait);
@@ -252,7 +251,7 @@ brcmf_sdioh_request_chain(struct brcmf_sdio_dev *sdiodev, uint fix_inc,
 
 	struct sk_buff *pkt;
 
-	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_dbg(SDIO, "Enter\n");
 
 	brcmf_pm_resume_wait(sdiodev, &sdiodev->request_chain_wait);
 	if (brcmf_pm_resume_error(sdiodev))
@@ -270,7 +269,7 @@ brcmf_sdioh_request_chain(struct brcmf_sdio_dev *sdiodev, uint fix_inc,
 				  write ? "TX" : "RX", pkt, SGCount, addr,
 				  pkt_len, err_ret);
 		} else {
-			brcmf_dbg(TRACE, "%s xfr'd %p[%d], addr=0x%05x, len=%d\n",
+			brcmf_dbg(SDIO, "%s xfr'd %p[%d], addr=0x%05x, len=%d\n",
 				  write ? "TX" : "RX", pkt, SGCount, addr,
 				  pkt_len);
 		}
@@ -280,7 +279,7 @@ brcmf_sdioh_request_chain(struct brcmf_sdio_dev *sdiodev, uint fix_inc,
 		SGCount++;
 	}
 
-	brcmf_dbg(TRACE, "Exit\n");
+	brcmf_dbg(SDIO, "Exit\n");
 	return err_ret;
 }
 
@@ -295,7 +294,7 @@ int brcmf_sdioh_request_buffer(struct brcmf_sdio_dev *sdiodev,
 	uint pkt_len;
 	bool fifo = (fix_inc == SDIOH_DATA_FIX);
 
-	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_dbg(SDIO, "Enter\n");
 
 	if (pkt == NULL)
 		return -EINVAL;
@@ -314,7 +313,7 @@ int brcmf_sdioh_request_buffer(struct brcmf_sdio_dev *sdiodev,
 		brcmf_err("%s FAILED %p, addr=0x%05x, pkt_len=%d, ERR=0x%08x\n",
 			  write ? "TX" : "RX", pkt, addr, pkt_len, status);
 	} else {
-		brcmf_dbg(TRACE, "%s xfr'd %p, addr=0x%05x, len=%d\n",
+		brcmf_dbg(SDIO, "%s xfr'd %p, addr=0x%05x, len=%d\n",
 			  write ? "TX" : "RX", pkt, addr, pkt_len);
 	}
 
@@ -350,12 +349,12 @@ static int brcmf_sdioh_enablefuncs(struct brcmf_sdio_dev *sdiodev)
 	u32 fbraddr;
 	u8 func;
 
-	brcmf_dbg(TRACE, "\n");
+	brcmf_dbg(SDIO, "\n");
 
 	/* Get the Card's common CIS address */
 	sdiodev->func_cis_ptr[0] = brcmf_sdioh_get_cisaddr(sdiodev,
 							   SDIO_CCCR_CIS);
-	brcmf_dbg(INFO, "Card's Common CIS Ptr = 0x%x\n",
+	brcmf_dbg(SDIO, "Card's Common CIS Ptr = 0x%x\n",
 		  sdiodev->func_cis_ptr[0]);
 
 	/* Get the Card's function CIS (for each function) */
@@ -363,7 +362,7 @@ static int brcmf_sdioh_enablefuncs(struct brcmf_sdio_dev *sdiodev)
 	     func <= sdiodev->num_funcs; func++, fbraddr += SDIOD_FBR_SIZE) {
 		sdiodev->func_cis_ptr[func] =
 		    brcmf_sdioh_get_cisaddr(sdiodev, SDIO_FBR_CIS + fbraddr);
-		brcmf_dbg(INFO, "Function %d CIS Ptr = 0x%x\n",
+		brcmf_dbg(SDIO, "Function %d CIS Ptr = 0x%x\n",
 			  func, sdiodev->func_cis_ptr[func]);
 	}
 
@@ -382,7 +381,7 @@ int brcmf_sdioh_attach(struct brcmf_sdio_dev *sdiodev)
 {
 	int err_ret = 0;
 
-	brcmf_dbg(TRACE, "\n");
+	brcmf_dbg(SDIO, "\n");
 
 	sdiodev->num_funcs = 2;
 
@@ -404,13 +403,13 @@ int brcmf_sdioh_attach(struct brcmf_sdio_dev *sdiodev)
 
 out:
 	sdio_release_host(sdiodev->func[1]);
-	brcmf_dbg(TRACE, "Done\n");
+	brcmf_dbg(SDIO, "Done\n");
 	return err_ret;
 }
 
 void brcmf_sdioh_detach(struct brcmf_sdio_dev *sdiodev)
 {
-	brcmf_dbg(TRACE, "\n");
+	brcmf_dbg(SDIO, "\n");
 
 	/* Disable Function 2 */
 	sdio_claim_host(sdiodev->func[2]);
@@ -424,33 +423,6 @@ void brcmf_sdioh_detach(struct brcmf_sdio_dev *sdiodev)
 
 }
 
-#ifdef CONFIG_BRCMFMAC_SDIO_OOB
-static int brcmf_sdio_getintrcfg(struct brcmf_sdio_dev *sdiodev)
-{
-	struct brcmf_sdio_oobirq *oobirq_entry;
-
-	if (list_empty(&oobirq_lh)) {
-		brcmf_err("no valid oob irq resource\n");
-		return -ENXIO;
-	}
-
-	oobirq_entry = list_first_entry(&oobirq_lh, struct brcmf_sdio_oobirq,
-					list);
-
-	sdiodev->irq = oobirq_entry->irq;
-	sdiodev->irq_flags = oobirq_entry->flags;
-	list_del(&oobirq_entry->list);
-	kfree(oobirq_entry);
-
-	return 0;
-}
-#else
-static inline int brcmf_sdio_getintrcfg(struct brcmf_sdio_dev *sdiodev)
-{
-	return 0;
-}
-#endif		/* CONFIG_BRCMFMAC_SDIO_OOB */
-
 static int brcmf_ops_sdio_probe(struct sdio_func *func,
 				const struct sdio_device_id *id)
 {
@@ -458,11 +430,11 @@ static int brcmf_ops_sdio_probe(struct sdio_func *func,
 	struct brcmf_sdio_dev *sdiodev;
 	struct brcmf_bus *bus_if;
 
-	brcmf_dbg(TRACE, "Enter\n");
-	brcmf_dbg(TRACE, "Class=%x\n", func->class);
-	brcmf_dbg(TRACE, "sdio vendor ID: 0x%04x\n", func->vendor);
-	brcmf_dbg(TRACE, "sdio device ID: 0x%04x\n", func->device);
-	brcmf_dbg(TRACE, "Function#: %d\n", func->num);
+	brcmf_dbg(SDIO, "Enter\n");
+	brcmf_dbg(SDIO, "Class=%x\n", func->class);
+	brcmf_dbg(SDIO, "sdio vendor ID: 0x%04x\n", func->vendor);
+	brcmf_dbg(SDIO, "sdio device ID: 0x%04x\n", func->device);
+	brcmf_dbg(SDIO, "Function#: %d\n", func->num);
 
 	/* Consume func num 1 but dont do anything with it. */
 	if (func->num == 1)
@@ -491,23 +463,21 @@ static int brcmf_ops_sdio_probe(struct sdio_func *func,
 	dev_set_drvdata(&func->dev, bus_if);
 	dev_set_drvdata(&sdiodev->func[1]->dev, bus_if);
 	sdiodev->dev = &sdiodev->func[1]->dev;
+	sdiodev->pdata = brcmfmac_sdio_pdata;
 
 	atomic_set(&sdiodev->suspend, false);
 	init_waitqueue_head(&sdiodev->request_byte_wait);
 	init_waitqueue_head(&sdiodev->request_word_wait);
 	init_waitqueue_head(&sdiodev->request_chain_wait);
 	init_waitqueue_head(&sdiodev->request_buffer_wait);
-	err = brcmf_sdio_getintrcfg(sdiodev);
-	if (err)
-		goto fail;
 
-	brcmf_dbg(TRACE, "F2 found, calling brcmf_sdio_probe...\n");
+	brcmf_dbg(SDIO, "F2 found, calling brcmf_sdio_probe...\n");
 	err = brcmf_sdio_probe(sdiodev);
 	if (err) {
 		brcmf_err("F2 error, probe failed %d...\n", err);
 		goto fail;
 	}
-	brcmf_dbg(TRACE, "F2 init completed...\n");
+	brcmf_dbg(SDIO, "F2 init completed...\n");
 	return 0;
 
 fail:
@@ -523,10 +493,10 @@ static void brcmf_ops_sdio_remove(struct sdio_func *func)
 	struct brcmf_bus *bus_if;
 	struct brcmf_sdio_dev *sdiodev;
 
-	brcmf_dbg(TRACE, "Enter\n");
-	brcmf_dbg(TRACE, "sdio vendor ID: 0x%04x\n", func->vendor);
-	brcmf_dbg(TRACE, "sdio device ID: 0x%04x\n", func->device);
-	brcmf_dbg(TRACE, "Function: %d\n", func->num);
+	brcmf_dbg(SDIO, "Enter\n");
+	brcmf_dbg(SDIO, "sdio vendor ID: 0x%04x\n", func->vendor);
+	brcmf_dbg(SDIO, "sdio device ID: 0x%04x\n", func->device);
+	brcmf_dbg(SDIO, "Function: %d\n", func->num);
 
 	if (func->num != 1 && func->num != 2)
 		return;
@@ -543,7 +513,7 @@ static void brcmf_ops_sdio_remove(struct sdio_func *func)
 		kfree(sdiodev);
 	}
 
-	brcmf_dbg(TRACE, "Exit\n");
+	brcmf_dbg(SDIO, "Exit\n");
 }
 
 #ifdef CONFIG_PM_SLEEP
@@ -554,7 +524,7 @@ static int brcmf_sdio_suspend(struct device *dev)
 	struct brcmf_sdio_dev *sdiodev = bus_if->bus_priv.sdio;
 	int ret = 0;
 
-	brcmf_dbg(TRACE, "\n");
+	brcmf_dbg(SDIO, "\n");
 
 	atomic_set(&sdiodev->suspend, true);
 
@@ -594,7 +564,7 @@ static const struct dev_pm_ops brcmf_sdio_pm_ops = {
 static struct sdio_driver brcmf_sdmmc_driver = {
 	.probe = brcmf_ops_sdio_probe,
 	.remove = brcmf_ops_sdio_remove,
-	.name = "brcmfmac",
+	.name = BRCMFMAC_SDIO_PDATA_NAME,
 	.id_table = brcmf_sdmmc_ids,
 #ifdef CONFIG_PM_SLEEP
 	.drv = {
@@ -603,83 +573,65 @@ static struct sdio_driver brcmf_sdmmc_driver = {
 #endif	/* CONFIG_PM_SLEEP */
 };
 
-#ifdef CONFIG_BRCMFMAC_SDIO_OOB
 static int brcmf_sdio_pd_probe(struct platform_device *pdev)
 {
-	struct resource *res;
-	struct brcmf_sdio_oobirq *oobirq_entry;
-	int i, ret;
+	int ret;
 
-	INIT_LIST_HEAD(&oobirq_lh);
+	brcmf_dbg(SDIO, "Enter\n");
 
-	for (i = 0; ; i++) {
-		res = platform_get_resource(pdev, IORESOURCE_IRQ, i);
-		if (!res)
-			break;
+	brcmfmac_sdio_pdata = pdev->dev.platform_data;
 
-		oobirq_entry = kzalloc(sizeof(struct brcmf_sdio_oobirq),
-				       GFP_KERNEL);
-		if (!oobirq_entry)
-			return -ENOMEM;
-		oobirq_entry->irq = res->start;
-		oobirq_entry->flags = res->flags & IRQF_TRIGGER_MASK;
-		list_add_tail(&oobirq_entry->list, &oobirq_lh);
-	}
-	if (i == 0)
-		return -ENXIO;
+	if (brcmfmac_sdio_pdata->power_on)
+		brcmfmac_sdio_pdata->power_on();
 
 	ret = sdio_register_driver(&brcmf_sdmmc_driver);
-
 	if (ret)
 		brcmf_err("sdio_register_driver failed: %d\n", ret);
 
 	return ret;
 }
 
-static struct platform_driver brcmf_sdio_pd = {
-	.probe		= brcmf_sdio_pd_probe,
-	.driver		= {
-		.name	= "brcmf_sdio_pd"
-	}
-};
-
-void brcmf_sdio_exit(void)
+static int brcmf_sdio_pd_remove(struct platform_device *pdev)
 {
-	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_dbg(SDIO, "Enter\n");
+
+	if (brcmfmac_sdio_pdata->power_off)
+		brcmfmac_sdio_pdata->power_off();
 
 	sdio_unregister_driver(&brcmf_sdmmc_driver);
 
-	platform_driver_unregister(&brcmf_sdio_pd);
+	return 0;
 }
 
-void brcmf_sdio_init(void)
-{
-	int ret;
-
-	brcmf_dbg(TRACE, "Enter\n");
-
-	ret = platform_driver_register(&brcmf_sdio_pd);
+static struct platform_driver brcmf_sdio_pd = {
+	.remove		= brcmf_sdio_pd_remove,
+	.driver		= {
+		.name	= BRCMFMAC_SDIO_PDATA_NAME
+	}
+};
 
-	if (ret)
-		brcmf_err("platform_driver_register failed: %d\n", ret);
-}
-#else
 void brcmf_sdio_exit(void)
 {
-	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_dbg(SDIO, "Enter\n");
 
-	sdio_unregister_driver(&brcmf_sdmmc_driver);
+	if (brcmfmac_sdio_pdata)
+		platform_driver_unregister(&brcmf_sdio_pd);
+	else
+		sdio_unregister_driver(&brcmf_sdmmc_driver);
 }
 
 void brcmf_sdio_init(void)
 {
 	int ret;
 
-	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_dbg(SDIO, "Enter\n");
 
-	ret = sdio_register_driver(&brcmf_sdmmc_driver);
+	ret = platform_driver_probe(&brcmf_sdio_pd, brcmf_sdio_pd_probe);
+	if (ret == -ENODEV) {
+		brcmf_dbg(SDIO, "No platform data available, registering without.\n");
+		ret = sdio_register_driver(&brcmf_sdmmc_driver);
+	}
 
 	if (ret)
-		brcmf_err("sdio_register_driver failed: %d\n", ret);
+		brcmf_err("driver registration failed: %d\n", ret);
 }
-#endif		/* CONFIG_BRCMFMAC_SDIO_OOB */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/btcoex.c b/drivers/net/wireless/brcm80211/brcmfmac/btcoex.c
new file mode 100644
index 000000000000..0cb591b050b3
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmfmac/btcoex.c
@@ -0,0 +1,497 @@
+/*
+ * Copyright (c) 2013 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include <linux/slab.h>
+#include <linux/netdevice.h>
+#include <net/cfg80211.h>
+
+#include <brcmu_wifi.h>
+#include <brcmu_utils.h>
+#include <defs.h>
+#include <dhd.h>
+#include <dhd_dbg.h>
+#include "fwil.h"
+#include "fwil_types.h"
+#include "btcoex.h"
+#include "p2p.h"
+#include "wl_cfg80211.h"
+
+/* T1 start SCO/eSCO priority suppression */
+#define BRCMF_BTCOEX_OPPR_WIN_TIME   2000
+
+/* BT registers values during DHCP */
+#define BRCMF_BT_DHCP_REG50 0x8022
+#define BRCMF_BT_DHCP_REG51 0
+#define BRCMF_BT_DHCP_REG64 0
+#define BRCMF_BT_DHCP_REG65 0
+#define BRCMF_BT_DHCP_REG71 0
+#define BRCMF_BT_DHCP_REG66 0x2710
+#define BRCMF_BT_DHCP_REG41 0x33
+#define BRCMF_BT_DHCP_REG68 0x190
+
+/* number of samples for SCO detection */
+#define BRCMF_BT_SCO_SAMPLES 12
+
+/**
+* enum brcmf_btcoex_state - BT coex DHCP state machine states
+* @BRCMF_BT_DHCP_IDLE: DCHP is idle
+* @BRCMF_BT_DHCP_START: DHCP started, wait before
+*	boosting wifi priority
+* @BRCMF_BT_DHCP_OPPR_WIN: graceful DHCP opportunity ended,
+*	boost wifi priority
+* @BRCMF_BT_DHCP_FLAG_FORCE_TIMEOUT: wifi priority boost end,
+*	restore defaults
+*/
+enum brcmf_btcoex_state {
+	BRCMF_BT_DHCP_IDLE,
+	BRCMF_BT_DHCP_START,
+	BRCMF_BT_DHCP_OPPR_WIN,
+	BRCMF_BT_DHCP_FLAG_FORCE_TIMEOUT
+};
+
+/**
+ * struct brcmf_btcoex_info - BT coex related information
+ * @vif: interface for which request was done.
+ * @timer: timer for DHCP state machine
+ * @timeout: configured timeout.
+ * @timer_on:  DHCP timer active
+ * @dhcp_done: DHCP finished before T1/T2 timer expiration
+ * @bt_state: DHCP state machine state
+ * @work: DHCP state machine work
+ * @cfg: driver private data for cfg80211 interface
+ * @reg66: saved value of btc_params 66
+ * @reg41: saved value of btc_params 41
+ * @reg68: saved value of btc_params 68
+ * @saved_regs_part1: flag indicating regs 66,41,68
+ *	have been saved
+ * @reg51: saved value of btc_params 51
+ * @reg64: saved value of btc_params 64
+ * @reg65: saved value of btc_params 65
+ * @reg71: saved value of btc_params 71
+ * @saved_regs_part1: flag indicating regs 50,51,64,65,71
+ *	have been saved
+ */
+struct brcmf_btcoex_info {
+	struct brcmf_cfg80211_vif *vif;
+	struct timer_list timer;
+	u16 timeout;
+	bool timer_on;
+	bool dhcp_done;
+	enum brcmf_btcoex_state bt_state;
+	struct work_struct work;
+	struct brcmf_cfg80211_info *cfg;
+	u32 reg66;
+	u32 reg41;
+	u32 reg68;
+	bool saved_regs_part1;
+	u32 reg50;
+	u32 reg51;
+	u32 reg64;
+	u32 reg65;
+	u32 reg71;
+	bool saved_regs_part2;
+};
+
+/**
+ * brcmf_btcoex_params_write() - write btc_params firmware variable
+ * @ifp: interface
+ * @addr: btc_params register number
+ * @data: data to write
+ */
+static s32 brcmf_btcoex_params_write(struct brcmf_if *ifp, u32 addr, u32 data)
+{
+	struct {
+		__le32 addr;
+		__le32 data;
+	} reg_write;
+
+	reg_write.addr = cpu_to_le32(addr);
+	reg_write.data = cpu_to_le32(data);
+	return brcmf_fil_iovar_data_set(ifp, "btc_params",
+					&reg_write, sizeof(reg_write));
+}
+
+/**
+ * brcmf_btcoex_params_read() - read btc_params firmware variable
+ * @ifp: interface
+ * @addr: btc_params register number
+ * @data: read data
+ */
+static s32 brcmf_btcoex_params_read(struct brcmf_if *ifp, u32 addr, u32 *data)
+{
+	*data = addr;
+
+	return brcmf_fil_iovar_int_get(ifp, "btc_params", data);
+}
+
+/**
+ * brcmf_btcoex_boost_wifi() - control BT SCO/eSCO parameters
+ * @btci: BT coex info
+ * @trump_sco:
+ *	true - set SCO/eSCO parameters for compatibility
+ *		during DHCP window
+ *	false - restore saved parameter values
+ *
+ * Enhanced BT COEX settings for eSCO compatibility during DHCP window
+ */
+static void brcmf_btcoex_boost_wifi(struct brcmf_btcoex_info *btci,
+				    bool trump_sco)
+{
+	struct brcmf_if *ifp = btci->cfg->pub->iflist[0];
+
+	if (trump_sco && !btci->saved_regs_part2) {
+		/* this should reduce eSCO agressive
+		 * retransmit w/o breaking it
+		 */
+
+		/* save current */
+		brcmf_dbg(TRACE, "new SCO/eSCO coex algo {save & override}\n");
+		brcmf_btcoex_params_read(ifp, 50, &btci->reg50);
+		brcmf_btcoex_params_read(ifp, 51, &btci->reg51);
+		brcmf_btcoex_params_read(ifp, 64, &btci->reg64);
+		brcmf_btcoex_params_read(ifp, 65, &btci->reg65);
+		brcmf_btcoex_params_read(ifp, 71, &btci->reg71);
+
+		btci->saved_regs_part2 = true;
+		brcmf_dbg(TRACE,
+			  "saved bt_params[50,51,64,65,71]: 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+			  btci->reg50, btci->reg51, btci->reg64,
+			  btci->reg65, btci->reg71);
+
+		/* pacify the eSco   */
+		brcmf_btcoex_params_write(ifp, 50, BRCMF_BT_DHCP_REG50);
+		brcmf_btcoex_params_write(ifp, 51, BRCMF_BT_DHCP_REG51);
+		brcmf_btcoex_params_write(ifp, 64, BRCMF_BT_DHCP_REG64);
+		brcmf_btcoex_params_write(ifp, 65, BRCMF_BT_DHCP_REG65);
+		brcmf_btcoex_params_write(ifp, 71, BRCMF_BT_DHCP_REG71);
+
+	} else if (btci->saved_regs_part2) {
+		/* restore previously saved bt params */
+		brcmf_dbg(TRACE, "Do new SCO/eSCO coex algo {restore}\n");
+		brcmf_btcoex_params_write(ifp, 50, btci->reg50);
+		brcmf_btcoex_params_write(ifp, 51, btci->reg51);
+		brcmf_btcoex_params_write(ifp, 64, btci->reg64);
+		brcmf_btcoex_params_write(ifp, 65, btci->reg65);
+		brcmf_btcoex_params_write(ifp, 71, btci->reg71);
+
+		brcmf_dbg(TRACE,
+			  "restored bt_params[50,51,64,65,71]: 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+			  btci->reg50, btci->reg51, btci->reg64,
+			  btci->reg65, btci->reg71);
+
+		btci->saved_regs_part2 = false;
+	} else {
+		brcmf_err("attempted to restore not saved BTCOEX params\n");
+	}
+}
+
+/**
+ * brcmf_btcoex_is_sco_active() - check if SCO/eSCO is active
+ * @ifp: interface
+ *
+ * return: true if SCO/eSCO session is active
+ */
+static bool brcmf_btcoex_is_sco_active(struct brcmf_if *ifp)
+{
+	int ioc_res = 0;
+	bool res = false;
+	int sco_id_cnt = 0;
+	u32 param27;
+	int i;
+
+	for (i = 0; i < BRCMF_BT_SCO_SAMPLES; i++) {
+		ioc_res = brcmf_btcoex_params_read(ifp, 27, &param27);
+
+		if (ioc_res < 0) {
+			brcmf_err("ioc read btc params error\n");
+			break;
+		}
+
+		brcmf_dbg(TRACE, "sample[%d], btc_params 27:%x\n", i, param27);
+
+		if ((param27 & 0x6) == 2) { /* count both sco & esco  */
+			sco_id_cnt++;
+		}
+
+		if (sco_id_cnt > 2) {
+			brcmf_dbg(TRACE,
+				  "sco/esco detected, pkt id_cnt:%d samples:%d\n",
+				  sco_id_cnt, i);
+			res = true;
+			break;
+		}
+	}
+	brcmf_dbg(TRACE, "exit: result=%d\n", res);
+	return res;
+}
+
+/**
+ * btcmf_btcoex_save_part1() - save first step parameters.
+ */
+static void btcmf_btcoex_save_part1(struct brcmf_btcoex_info *btci)
+{
+	struct brcmf_if *ifp = btci->vif->ifp;
+
+	if (!btci->saved_regs_part1) {
+		/* Retrieve and save original reg value */
+		brcmf_btcoex_params_read(ifp, 66, &btci->reg66);
+		brcmf_btcoex_params_read(ifp, 41, &btci->reg41);
+		brcmf_btcoex_params_read(ifp, 68, &btci->reg68);
+		btci->saved_regs_part1 = true;
+		brcmf_dbg(TRACE,
+			  "saved btc_params regs (66,41,68) 0x%x 0x%x 0x%x\n",
+			  btci->reg66, btci->reg41,
+			  btci->reg68);
+	}
+}
+
+/**
+ * brcmf_btcoex_restore_part1() - restore first step parameters.
+ */
+static void brcmf_btcoex_restore_part1(struct brcmf_btcoex_info *btci)
+{
+	struct brcmf_if *ifp;
+
+	if (btci->saved_regs_part1) {
+		btci->saved_regs_part1 = false;
+		ifp = btci->vif->ifp;
+		brcmf_btcoex_params_write(ifp, 66, btci->reg66);
+		brcmf_btcoex_params_write(ifp, 41, btci->reg41);
+		brcmf_btcoex_params_write(ifp, 68, btci->reg68);
+		brcmf_dbg(TRACE,
+			  "restored btc_params regs {66,41,68} 0x%x 0x%x 0x%x\n",
+			  btci->reg66, btci->reg41,
+			  btci->reg68);
+	}
+}
+
+/**
+ * brcmf_btcoex_timerfunc() - BT coex timer callback
+ */
+static void brcmf_btcoex_timerfunc(ulong data)
+{
+	struct brcmf_btcoex_info *bt_local = (struct brcmf_btcoex_info *)data;
+	brcmf_dbg(TRACE, "enter\n");
+
+	bt_local->timer_on = false;
+	schedule_work(&bt_local->work);
+}
+
+/**
+ * brcmf_btcoex_handler() - BT coex state machine work handler
+ * @work: work
+ */
+static void brcmf_btcoex_handler(struct work_struct *work)
+{
+	struct brcmf_btcoex_info *btci;
+	btci = container_of(work, struct brcmf_btcoex_info, work);
+	if (btci->timer_on) {
+		btci->timer_on = false;
+		del_timer_sync(&btci->timer);
+	}
+
+	switch (btci->bt_state) {
+	case BRCMF_BT_DHCP_START:
+		/* DHCP started provide OPPORTUNITY window
+		   to get DHCP address
+		*/
+		brcmf_dbg(TRACE, "DHCP started\n");
+		btci->bt_state = BRCMF_BT_DHCP_OPPR_WIN;
+		if (btci->timeout < BRCMF_BTCOEX_OPPR_WIN_TIME) {
+			mod_timer(&btci->timer, btci->timer.expires);
+		} else {
+			btci->timeout -= BRCMF_BTCOEX_OPPR_WIN_TIME;
+			mod_timer(&btci->timer,
+				  jiffies +
+				  msecs_to_jiffies(BRCMF_BTCOEX_OPPR_WIN_TIME));
+		}
+		btci->timer_on = true;
+		break;
+
+	case BRCMF_BT_DHCP_OPPR_WIN:
+		if (btci->dhcp_done) {
+			brcmf_dbg(TRACE, "DHCP done before T1 expiration\n");
+			goto idle;
+		}
+
+		/* DHCP is not over yet, start lowering BT priority */
+		brcmf_dbg(TRACE, "DHCP T1:%d expired\n",
+			  BRCMF_BTCOEX_OPPR_WIN_TIME);
+		brcmf_btcoex_boost_wifi(btci, true);
+
+		btci->bt_state = BRCMF_BT_DHCP_FLAG_FORCE_TIMEOUT;
+		mod_timer(&btci->timer,
+			  jiffies + msecs_to_jiffies(btci->timeout));
+		btci->timer_on = true;
+		break;
+
+	case BRCMF_BT_DHCP_FLAG_FORCE_TIMEOUT:
+		if (btci->dhcp_done)
+			brcmf_dbg(TRACE, "DHCP done before T2 expiration\n");
+		else
+			brcmf_dbg(TRACE, "DHCP T2:%d expired\n",
+				  BRCMF_BT_DHCP_FLAG_FORCE_TIMEOUT);
+
+		goto idle;
+
+	default:
+		brcmf_err("invalid state=%d !!!\n", btci->bt_state);
+		goto idle;
+	}
+
+	return;
+
+idle:
+	btci->bt_state = BRCMF_BT_DHCP_IDLE;
+	btci->timer_on = false;
+	brcmf_btcoex_boost_wifi(btci, false);
+	cfg80211_crit_proto_stopped(&btci->vif->wdev, GFP_KERNEL);
+	brcmf_btcoex_restore_part1(btci);
+	btci->vif = NULL;
+}
+
+/**
+ * brcmf_btcoex_attach() - initialize BT coex data
+ * @cfg: driver private cfg80211 data
+ *
+ * return: 0 on success
+ */
+int brcmf_btcoex_attach(struct brcmf_cfg80211_info *cfg)
+{
+	struct brcmf_btcoex_info *btci = NULL;
+	brcmf_dbg(TRACE, "enter\n");
+
+	btci = kmalloc(sizeof(struct brcmf_btcoex_info), GFP_KERNEL);
+	if (!btci)
+		return -ENOMEM;
+
+	btci->bt_state = BRCMF_BT_DHCP_IDLE;
+
+	/* Set up timer for BT  */
+	btci->timer_on = false;
+	btci->timeout = BRCMF_BTCOEX_OPPR_WIN_TIME;
+	init_timer(&btci->timer);
+	btci->timer.data = (ulong)btci;
+	btci->timer.function = brcmf_btcoex_timerfunc;
+	btci->cfg = cfg;
+	btci->saved_regs_part1 = false;
+	btci->saved_regs_part2 = false;
+
+	INIT_WORK(&btci->work, brcmf_btcoex_handler);
+
+	cfg->btcoex = btci;
+	return 0;
+}
+
+/**
+ * brcmf_btcoex_detach - clean BT coex data
+ * @cfg: driver private cfg80211 data
+ */
+void brcmf_btcoex_detach(struct brcmf_cfg80211_info *cfg)
+{
+	brcmf_dbg(TRACE, "enter\n");
+
+	if (!cfg->btcoex)
+		return;
+
+	if (cfg->btcoex->timer_on) {
+		cfg->btcoex->timer_on = false;
+		del_timer_sync(&cfg->btcoex->timer);
+	}
+
+	cancel_work_sync(&cfg->btcoex->work);
+
+	brcmf_btcoex_boost_wifi(cfg->btcoex, false);
+	brcmf_btcoex_restore_part1(cfg->btcoex);
+
+	kfree(cfg->btcoex);
+	cfg->btcoex = NULL;
+}
+
+static void brcmf_btcoex_dhcp_start(struct brcmf_btcoex_info *btci)
+{
+	struct brcmf_if *ifp = btci->vif->ifp;
+
+	btcmf_btcoex_save_part1(btci);
+	/* set new regs values */
+	brcmf_btcoex_params_write(ifp, 66, BRCMF_BT_DHCP_REG66);
+	brcmf_btcoex_params_write(ifp, 41, BRCMF_BT_DHCP_REG41);
+	brcmf_btcoex_params_write(ifp, 68, BRCMF_BT_DHCP_REG68);
+	btci->dhcp_done = false;
+	btci->bt_state = BRCMF_BT_DHCP_START;
+	schedule_work(&btci->work);
+	brcmf_dbg(TRACE, "enable BT DHCP Timer\n");
+}
+
+static void brcmf_btcoex_dhcp_end(struct brcmf_btcoex_info *btci)
+{
+	/* Stop any bt timer because DHCP session is done */
+	btci->dhcp_done = true;
+	if (btci->timer_on) {
+		brcmf_dbg(TRACE, "disable BT DHCP Timer\n");
+		btci->timer_on = false;
+		del_timer_sync(&btci->timer);
+
+		/* schedule worker if transition to IDLE is needed */
+		if (btci->bt_state != BRCMF_BT_DHCP_IDLE) {
+			brcmf_dbg(TRACE, "bt_state:%d\n",
+				  btci->bt_state);
+			schedule_work(&btci->work);
+		}
+	} else {
+		/* Restore original values */
+		brcmf_btcoex_restore_part1(btci);
+	}
+}
+
+/**
+ * brcmf_btcoex_set_mode - set BT coex mode
+ * @cfg: driver private cfg80211 data
+ * @mode: Wifi-Bluetooth coexistence mode
+ *
+ * return: 0 on success
+ */
+int brcmf_btcoex_set_mode(struct brcmf_cfg80211_vif *vif,
+			  enum brcmf_btcoex_mode mode, u16 duration)
+{
+	struct brcmf_cfg80211_info *cfg = wiphy_priv(vif->wdev.wiphy);
+	struct brcmf_btcoex_info *btci = cfg->btcoex;
+	struct brcmf_if *ifp = cfg->pub->iflist[0];
+
+	switch (mode) {
+	case BRCMF_BTCOEX_DISABLED:
+		brcmf_dbg(TRACE, "DHCP session starts\n");
+		if (btci->bt_state != BRCMF_BT_DHCP_IDLE)
+			return -EBUSY;
+		/* Start BT timer only for SCO connection */
+		if (brcmf_btcoex_is_sco_active(ifp)) {
+			btci->timeout = duration;
+			btci->vif = vif;
+			brcmf_btcoex_dhcp_start(btci);
+		}
+		break;
+
+	case BRCMF_BTCOEX_ENABLED:
+		brcmf_dbg(TRACE, "DHCP session ends\n");
+		if (btci->bt_state != BRCMF_BT_DHCP_IDLE &&
+		    vif == btci->vif) {
+			brcmf_btcoex_dhcp_end(btci);
+		}
+		break;
+	default:
+		brcmf_dbg(TRACE, "Unknown mode, ignored\n");
+	}
+	return 0;
+}
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/btcoex.h b/drivers/net/wireless/brcm80211/brcmfmac/btcoex.h
new file mode 100644
index 000000000000..19647c68aa9e
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmfmac/btcoex.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (c) 2013 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#ifndef WL_BTCOEX_H_
+#define WL_BTCOEX_H_
+
+enum brcmf_btcoex_mode {
+	BRCMF_BTCOEX_DISABLED,
+	BRCMF_BTCOEX_ENABLED
+};
+
+int brcmf_btcoex_attach(struct brcmf_cfg80211_info *cfg);
+void brcmf_btcoex_detach(struct brcmf_cfg80211_info *cfg);
+int brcmf_btcoex_set_mode(struct brcmf_cfg80211_vif *vif,
+			  enum brcmf_btcoex_mode mode, u16 duration);
+
+#endif /* WL_BTCOEX_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
index ef6f23be6d32..28db9cf39672 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
@@ -28,6 +28,7 @@
 /*******************************************************************************
  * IO codes that are interpreted by dongle firmware
  ******************************************************************************/
+#define BRCMF_C_GET_VERSION			1
 #define BRCMF_C_UP				2
 #define BRCMF_C_DOWN				3
 #define BRCMF_C_SET_PROMISC			10
@@ -72,6 +73,7 @@
 #define BRCMF_C_SET_WSEC			134
 #define BRCMF_C_GET_PHY_NOISE			135
 #define BRCMF_C_GET_BSS_INFO			136
+#define BRCMF_C_GET_BANDLIST			140
 #define BRCMF_C_SET_SCB_TIMEOUT			158
 #define BRCMF_C_GET_PHYLIST			180
 #define BRCMF_C_SET_SCAN_CHANNEL_TIME		185
@@ -475,6 +477,11 @@ struct brcmf_sta_info_le {
 	__le32	rx_decrypt_failures;	/* # of packet decrypted failed */
 };
 
+struct brcmf_chanspec_list {
+	__le32	count;		/* # of entries */
+	__le32	element[1];	/* variable length uint32 list */
+};
+
 /*
  * WLC_E_PROBRESP_MSG
  * WLC_E_P2P_PROBREQ_MSG
@@ -501,6 +508,7 @@ struct brcmf_dcmd {
 /* Forward decls for struct brcmf_pub (see below) */
 struct brcmf_proto;	/* device communication protocol info */
 struct brcmf_cfg80211_dev; /* cfg80211 device info */
+struct brcmf_fws_info; /* firmware signalling info */
 
 /* Common structure for module and instance linkage */
 struct brcmf_pub {
@@ -527,6 +535,10 @@ struct brcmf_pub {
 	unsigned char proto_buf[BRCMF_DCMD_MAXLEN];
 
 	struct brcmf_fweh_info fweh;
+
+	bool fw_signals;
+	struct brcmf_fws_info *fws;
+	spinlock_t fws_spinlock;
 #ifdef DEBUG
 	struct dentry *dbgfs_dir;
 #endif
@@ -537,10 +549,25 @@ struct brcmf_if_event {
 	u8 action;
 	u8 flags;
 	u8 bssidx;
+	u8 role;
 };
 
-/* forward declaration */
+/* forward declarations */
 struct brcmf_cfg80211_vif;
+struct brcmf_fws_mac_descriptor;
+
+/**
+ * enum brcmf_netif_stop_reason - reason for stopping netif queue.
+ *
+ * @BRCMF_NETIF_STOP_REASON_FWS_FC:
+ *	netif stopped due to firmware signalling flow control.
+ * @BRCMF_NETIF_STOP_REASON_BLOCK_BUS:
+ *	netif stopped due to bus blocking.
+ */
+enum brcmf_netif_stop_reason {
+	BRCMF_NETIF_STOP_REASON_FWS_FC = 1,
+	BRCMF_NETIF_STOP_REASON_BLOCK_BUS = 2
+};
 
 /**
  * struct brcmf_if - interface control information.
@@ -549,9 +576,13 @@ struct brcmf_cfg80211_vif;
  * @vif: points to cfg80211 specific interface information.
  * @ndev: associated network device.
  * @stats: interface specific network statistics.
+ * @setmacaddr_work: worker object for setting mac address.
+ * @multicast_work: worker object for multicast provisioning.
+ * @fws_desc: interface specific firmware-signalling descriptor.
  * @ifidx: interface index in device firmware.
  * @bssidx: index of bss associated with this interface.
  * @mac_addr: assigned mac address.
+ * @netif_stop: bitmap indicates reason why netif queues are stopped.
  * @pend_8021x_cnt: tracks outstanding number of 802.1x frames.
  * @pend_8021x_wait: used for signalling change in count.
  */
@@ -562,9 +593,11 @@ struct brcmf_if {
 	struct net_device_stats stats;
 	struct work_struct setmacaddr_work;
 	struct work_struct multicast_work;
+	struct brcmf_fws_mac_descriptor *fws_desc;
 	int ifidx;
 	s32 bssidx;
 	u8 mac_addr[ETH_ALEN];
+	u8 netif_stop;
 	atomic_t pend_8021x_cnt;
 	wait_queue_head_t pend_8021x_wait;
 };
@@ -582,13 +615,17 @@ extern int brcmf_proto_cdc_set_dcmd(struct brcmf_pub *drvr, int ifidx, uint cmd,
 				    void *buf, uint len);
 
 /* Remove any protocol-specific data header. */
-extern int brcmf_proto_hdrpull(struct brcmf_pub *drvr, u8 *ifidx,
+extern int brcmf_proto_hdrpull(struct brcmf_pub *drvr, bool do_fws, u8 *ifidx,
 			       struct sk_buff *rxp);
 
 extern int brcmf_net_attach(struct brcmf_if *ifp, bool rtnl_locked);
 extern struct brcmf_if *brcmf_add_if(struct brcmf_pub *drvr, s32 bssidx,
 				     s32 ifidx, char *name, u8 *mac_addr);
 extern void brcmf_del_if(struct brcmf_pub *drvr, s32 bssidx);
+void brcmf_txflowblock_if(struct brcmf_if *ifp,
+			  enum brcmf_netif_stop_reason reason, bool state);
 extern u32 brcmf_get_chip_info(struct brcmf_if *ifp);
+extern void brcmf_txfinalize(struct brcmf_pub *drvr, struct sk_buff *txp,
+			     bool success);
 
 #endif				/* _BRCMF_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h
index ad25c3408b59..080395f49fa5 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h
@@ -39,10 +39,12 @@ struct brcmf_bus_dcmd {
  * @txdata: send a data frame to the dongle (callee disposes skb).
  * @txctl: transmit a control request message to dongle.
  * @rxctl: receive a control response message from dongle.
+ * @gettxq: obtain a reference of bus transmit queue (optional).
  *
  * This structure provides an abstract interface towards the
  * bus specific driver. For control messages to common driver
- * will assure there is only one active transaction.
+ * will assure there is only one active transaction. Unless
+ * indicated otherwise these callbacks are mandatory.
  */
 struct brcmf_bus_ops {
 	int (*init)(struct device *dev);
@@ -50,6 +52,7 @@ struct brcmf_bus_ops {
 	int (*txdata)(struct device *dev, struct sk_buff *skb);
 	int (*txctl)(struct device *dev, unsigned char *msg, uint len);
 	int (*rxctl)(struct device *dev, unsigned char *msg, uint len);
+	struct pktq * (*gettxq)(struct device *dev);
 };
 
 /**
@@ -115,6 +118,14 @@ int brcmf_bus_rxctl(struct brcmf_bus *bus, unsigned char *msg, uint len)
 	return bus->ops->rxctl(bus->dev, msg, len);
 }
 
+static inline
+struct pktq *brcmf_bus_gettxq(struct brcmf_bus *bus)
+{
+	if (!bus->ops->gettxq)
+		return ERR_PTR(-ENOENT);
+
+	return bus->ops->gettxq(bus->dev);
+}
 /*
  * interface functions from common layer
  */
@@ -134,7 +145,7 @@ extern void brcmf_dev_reset(struct device *dev);
 /* Indication from bus module to change flow-control state */
 extern void brcmf_txflowblock(struct device *dev, bool state);
 
-/* Notify tx completion */
+/* Notify the bus has transferred the tx packet to firmware */
 extern void brcmf_txcomplete(struct device *dev, struct sk_buff *txp,
 			     bool success);
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_cdc.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_cdc.c
index a2354d951dd7..59c77aa3b959 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_cdc.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_cdc.c
@@ -28,6 +28,7 @@
 #include "dhd.h"
 #include "dhd_proto.h"
 #include "dhd_bus.h"
+#include "fwsignal.h"
 #include "dhd_dbg.h"
 
 struct brcmf_proto_cdc_dcmd {
@@ -71,13 +72,26 @@ struct brcmf_proto_cdc_dcmd {
 	((hdr)->flags2 = (((hdr)->flags2 & ~BDC_FLAG2_IF_MASK) | \
 	((idx) << BDC_FLAG2_IF_SHIFT)))
 
+/**
+ * struct brcmf_proto_bdc_header - BDC header format
+ *
+ * @flags: flags contain protocol and checksum info.
+ * @priority: 802.1d priority and USB flow control info (bit 4:7).
+ * @flags2: additional flags containing dongle interface index.
+ * @data_offset: start of packet data. header is following by firmware signals.
+ */
 struct brcmf_proto_bdc_header {
 	u8 flags;
-	u8 priority;	/* 802.1d Priority, 4:7 flow control info for usb */
+	u8 priority;
 	u8 flags2;
 	u8 data_offset;
 };
 
+/*
+ * maximum length of firmware signal data between
+ * the BDC header and packet data in the tx path.
+ */
+#define BRCMF_PROT_FW_SIGNAL_MAX_TXBYTES	12
 
 #define RETRIES 2 /* # of retries to retrieve matching dcmd response */
 #define BUS_HEADER_LEN	(16+64)		/* Must be atleast SDPCM_RESERVE
@@ -258,7 +272,7 @@ static void pkt_set_sum_good(struct sk_buff *skb, bool x)
 	skb->ip_summed = (x ? CHECKSUM_UNNECESSARY : CHECKSUM_NONE);
 }
 
-void brcmf_proto_hdrpush(struct brcmf_pub *drvr, int ifidx,
+void brcmf_proto_hdrpush(struct brcmf_pub *drvr, int ifidx, u8 offset,
 			 struct sk_buff *pktbuf)
 {
 	struct brcmf_proto_bdc_header *h;
@@ -266,7 +280,6 @@ void brcmf_proto_hdrpush(struct brcmf_pub *drvr, int ifidx,
 	brcmf_dbg(CDC, "Enter\n");
 
 	/* Push BDC header used to convey priority for buses that don't */
-
 	skb_push(pktbuf, BDC_HEADER_LEN);
 
 	h = (struct brcmf_proto_bdc_header *)(pktbuf->data);
@@ -277,11 +290,11 @@ void brcmf_proto_hdrpush(struct brcmf_pub *drvr, int ifidx,
 
 	h->priority = (pktbuf->priority & BDC_PRIORITY_MASK);
 	h->flags2 = 0;
-	h->data_offset = 0;
+	h->data_offset = offset;
 	BDC_SET_IF_IDX(h, ifidx);
 }
 
-int brcmf_proto_hdrpull(struct brcmf_pub *drvr, u8 *ifidx,
+int brcmf_proto_hdrpull(struct brcmf_pub *drvr, bool do_fws, u8 *ifidx,
 			struct sk_buff *pktbuf)
 {
 	struct brcmf_proto_bdc_header *h;
@@ -290,8 +303,8 @@ int brcmf_proto_hdrpull(struct brcmf_pub *drvr, u8 *ifidx,
 
 	/* Pop BDC header used to convey priority for buses that don't */
 
-	if (pktbuf->len < BDC_HEADER_LEN) {
-		brcmf_err("rx data too short (%d < %d)\n",
+	if (pktbuf->len <= BDC_HEADER_LEN) {
+		brcmf_dbg(INFO, "rx data too short (%d <= %d)\n",
 			  pktbuf->len, BDC_HEADER_LEN);
 		return -EBADE;
 	}
@@ -328,7 +341,10 @@ int brcmf_proto_hdrpull(struct brcmf_pub *drvr, u8 *ifidx,
 	pktbuf->priority = h->priority & BDC_PRIORITY_MASK;
 
 	skb_pull(pktbuf, BDC_HEADER_LEN);
-	skb_pull(pktbuf, h->data_offset << 2);
+	if (do_fws)
+		brcmf_fws_hdrpull(drvr, *ifidx, h->data_offset << 2, pktbuf);
+	else
+		skb_pull(pktbuf, h->data_offset << 2);
 
 	if (pktbuf->len == 0)
 		return -ENODATA;
@@ -350,7 +366,7 @@ int brcmf_proto_attach(struct brcmf_pub *drvr)
 	}
 
 	drvr->prot = cdc;
-	drvr->hdrlen += BDC_HEADER_LEN;
+	drvr->hdrlen += BDC_HEADER_LEN + BRCMF_PROT_FW_SIGNAL_MAX_TXBYTES;
 	drvr->bus_if->maxctl = BRCMF_DCMD_MAXLEN +
 			sizeof(struct brcmf_proto_cdc_dcmd) + ROUND_UP_MARGIN;
 	return 0;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
index 4544342a0428..be0787cab24f 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
@@ -24,6 +24,7 @@
 #include "dhd_proto.h"
 #include "dhd_dbg.h"
 #include "fwil.h"
+#include "tracepoint.h"
 
 #define PKTFILTER_BUF_SIZE		128
 #define BRCMF_ARPOL_MODE		0xb	/* agent|snoop|peer_autoreply */
@@ -373,3 +374,35 @@ int brcmf_c_preinit_dcmds(struct brcmf_if *ifp)
 done:
 	return err;
 }
+
+#ifdef CONFIG_BRCM_TRACING
+void __brcmf_err(const char *func, const char *fmt, ...)
+{
+	struct va_format vaf = {
+		.fmt = fmt,
+	};
+	va_list args;
+
+	va_start(args, fmt);
+	vaf.va = &args;
+	pr_err("%s: %pV", func, &vaf);
+	trace_brcmf_err(func, &vaf);
+	va_end(args);
+}
+#endif
+#if defined(CONFIG_BRCM_TRACING) || defined(CONFIG_BRCMDBG)
+void __brcmf_dbg(u32 level, const char *func, const char *fmt, ...)
+{
+	struct va_format vaf = {
+		.fmt = fmt,
+	};
+	va_list args;
+
+	va_start(args, fmt);
+	vaf.va = &args;
+	if (brcmf_msg_level & level)
+		pr_debug("%s %pV", func, &vaf);
+	trace_brcmf_dbg(level, func, &vaf);
+	va_end(args);
+}
+#endif
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.c
index 57671eddf79d..202869cd0932 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.c
@@ -22,6 +22,7 @@
 #include "dhd.h"
 #include "dhd_bus.h"
 #include "dhd_dbg.h"
+#include "tracepoint.h"
 
 static struct dentry *root_folder;
 
@@ -123,3 +124,82 @@ void brcmf_debugfs_create_sdio_count(struct brcmf_pub *drvr,
 		debugfs_create_file("counters", S_IRUGO, dentry,
 				    sdcnt, &brcmf_debugfs_sdio_counter_ops);
 }
+
+static
+ssize_t brcmf_debugfs_fws_stats_read(struct file *f, char __user *data,
+				     size_t count, loff_t *ppos)
+{
+	struct brcmf_fws_stats *fwstats = f->private_data;
+	char buf[650];
+	int res;
+
+	/* only allow read from start */
+	if (*ppos > 0)
+		return 0;
+
+	res = scnprintf(buf, sizeof(buf),
+			"header_pulls:      %u\n"
+			"header_only_pkt:   %u\n"
+			"tlv_parse_failed:  %u\n"
+			"tlv_invalid_type:  %u\n"
+			"mac_update_fails:  %u\n"
+			"ps_update_fails:   %u\n"
+			"if_update_fails:   %u\n"
+			"pkt2bus:           %u\n"
+			"generic_error:     %u\n"
+			"rollback_success:  %u\n"
+			"rollback_failed:   %u\n"
+			"delayq_full:       %u\n"
+			"supprq_full:       %u\n"
+			"txs_indicate:      %u\n"
+			"txs_discard:       %u\n"
+			"txs_suppr_core:    %u\n"
+			"txs_suppr_ps:      %u\n"
+			"txs_tossed:        %u\n"
+			"send_pkts:         BK:%u BE:%u VO:%u VI:%u BCMC:%u\n"
+			"fifo_credits_sent: BK:%u BE:%u VO:%u VI:%u BCMC:%u\n",
+			fwstats->header_pulls,
+			fwstats->header_only_pkt,
+			fwstats->tlv_parse_failed,
+			fwstats->tlv_invalid_type,
+			fwstats->mac_update_failed,
+			fwstats->mac_ps_update_failed,
+			fwstats->if_update_failed,
+			fwstats->pkt2bus,
+			fwstats->generic_error,
+			fwstats->rollback_success,
+			fwstats->rollback_failed,
+			fwstats->delayq_full_error,
+			fwstats->supprq_full_error,
+			fwstats->txs_indicate,
+			fwstats->txs_discard,
+			fwstats->txs_supp_core,
+			fwstats->txs_supp_ps,
+			fwstats->txs_tossed,
+			fwstats->send_pkts[0], fwstats->send_pkts[1],
+			fwstats->send_pkts[2], fwstats->send_pkts[3],
+			fwstats->send_pkts[4],
+			fwstats->fifo_credits_sent[0],
+			fwstats->fifo_credits_sent[1],
+			fwstats->fifo_credits_sent[2],
+			fwstats->fifo_credits_sent[3],
+			fwstats->fifo_credits_sent[4]);
+
+	return simple_read_from_buffer(data, count, ppos, buf, res);
+}
+
+static const struct file_operations brcmf_debugfs_fws_stats_ops = {
+	.owner = THIS_MODULE,
+	.open = simple_open,
+	.read = brcmf_debugfs_fws_stats_read
+};
+
+void brcmf_debugfs_create_fws_stats(struct brcmf_pub *drvr,
+				    struct brcmf_fws_stats *stats)
+{
+	struct dentry *dentry =  drvr->dbgfs_dir;
+
+	if (!IS_ERR_OR_NULL(dentry))
+		debugfs_create_file("fws_stats", S_IRUGO, dentry,
+				    stats, &brcmf_debugfs_fws_stats_ops);
+}
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h
index bc013cbe06f6..009c87bfd9ae 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h
@@ -34,6 +34,7 @@
 #define BRCMF_SCAN_VAL	0x00004000
 #define BRCMF_CONN_VAL	0x00008000
 #define BRCMF_CDC_VAL	0x00010000
+#define BRCMF_SDIO_VAL	0x00020000
 
 /* set default print format */
 #undef pr_fmt
@@ -43,6 +44,7 @@
  * debugging is not selected. When debugging the driver error
  * messages are as important as other tracing or even more so.
  */
+#ifndef CONFIG_BRCM_TRACING
 #ifdef CONFIG_BRCMDBG
 #define brcmf_err(fmt, ...)	pr_err("%s: " fmt, __func__, ##__VA_ARGS__)
 #else
@@ -52,15 +54,21 @@
 			pr_err("%s: " fmt, __func__, ##__VA_ARGS__);	\
 	} while (0)
 #endif
+#else
+__printf(2, 3)
+void __brcmf_err(const char *func, const char *fmt, ...);
+#define brcmf_err(fmt, ...) \
+	__brcmf_err(__func__, fmt, ##__VA_ARGS__)
+#endif
 
-#if defined(DEBUG)
-
+#if defined(DEBUG) || defined(CONFIG_BRCM_TRACING)
+__printf(3, 4)
+void __brcmf_dbg(u32 level, const char *func, const char *fmt, ...);
 #define brcmf_dbg(level, fmt, ...)				\
 do {								\
-	if (brcmf_msg_level & BRCMF_##level##_VAL)		\
-		pr_debug("%s: " fmt, __func__, ##__VA_ARGS__);	\
+	__brcmf_dbg(BRCMF_##level##_VAL, __func__,		\
+		    fmt, ##__VA_ARGS__);			\
 } while (0)
-
 #define BRCMF_DATA_ON()		(brcmf_msg_level & BRCMF_DATA_VAL)
 #define BRCMF_CTL_ON()		(brcmf_msg_level & BRCMF_CTL_VAL)
 #define BRCMF_HDRS_ON()		(brcmf_msg_level & BRCMF_HDRS_VAL)
@@ -69,7 +77,7 @@ do {								\
 #define BRCMF_EVENT_ON()	(brcmf_msg_level & BRCMF_EVENT_VAL)
 #define BRCMF_FIL_ON()		(brcmf_msg_level & BRCMF_FIL_VAL)
 
-#else	/* (defined DEBUG) || (defined DEBUG) */
+#else /* defined(DEBUG) || defined(CONFIG_BRCM_TRACING) */
 
 #define brcmf_dbg(level, fmt, ...) no_printk(fmt, ##__VA_ARGS__)
 
@@ -81,10 +89,11 @@ do {								\
 #define BRCMF_EVENT_ON()	0
 #define BRCMF_FIL_ON()		0
 
-#endif				/* defined(DEBUG) */
+#endif /* defined(DEBUG) || defined(CONFIG_BRCM_TRACING) */
 
 #define brcmf_dbg_hex_dump(test, data, len, fmt, ...)			\
 do {									\
+	trace_brcmf_hexdump((void *)data, len);				\
 	if (test)							\
 		brcmu_dbg_hex_dump(data, len, fmt, ##__VA_ARGS__);	\
 } while (0)
@@ -125,6 +134,32 @@ struct brcmf_sdio_count {
 	ulong rx_readahead_cnt;	/* packets where header read-ahead was used */
 };
 
+struct brcmf_fws_stats {
+	u32 tlv_parse_failed;
+	u32 tlv_invalid_type;
+	u32 header_only_pkt;
+	u32 header_pulls;
+	u32 pkt2bus;
+	u32 send_pkts[5];
+	u32 fifo_credits_sent[5];
+	u32 fifo_credits_back[6];
+	u32 generic_error;
+	u32 mac_update_failed;
+	u32 mac_ps_update_failed;
+	u32 if_update_failed;
+	u32 packet_request_failed;
+	u32 credit_request_failed;
+	u32 rollback_success;
+	u32 rollback_failed;
+	u32 delayq_full_error;
+	u32 supprq_full_error;
+	u32 txs_indicate;
+	u32 txs_discard;
+	u32 txs_supp_core;
+	u32 txs_supp_ps;
+	u32 txs_tossed;
+};
+
 struct brcmf_pub;
 #ifdef DEBUG
 void brcmf_debugfs_init(void);
@@ -134,6 +169,8 @@ void brcmf_debugfs_detach(struct brcmf_pub *drvr);
 struct dentry *brcmf_debugfs_get_devdir(struct brcmf_pub *drvr);
 void brcmf_debugfs_create_sdio_count(struct brcmf_pub *drvr,
 				     struct brcmf_sdio_count *sdcnt);
+void brcmf_debugfs_create_fws_stats(struct brcmf_pub *drvr,
+				    struct brcmf_fws_stats *stats);
 #else
 static inline void brcmf_debugfs_init(void)
 {
@@ -148,6 +185,10 @@ static inline int brcmf_debugfs_attach(struct brcmf_pub *drvr)
 static inline void brcmf_debugfs_detach(struct brcmf_pub *drvr)
 {
 }
+static inline void brcmf_debugfs_create_fws_stats(struct brcmf_pub *drvr,
+						  struct brcmf_fws_stats *stats)
+{
+}
 #endif
 
 #endif				/* _BRCMF_DBG_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
index c06cea88df0d..59c25463e428 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
@@ -30,17 +30,18 @@
 #include "p2p.h"
 #include "wl_cfg80211.h"
 #include "fwil.h"
+#include "fwsignal.h"
 
 MODULE_AUTHOR("Broadcom Corporation");
 MODULE_DESCRIPTION("Broadcom 802.11 wireless LAN fullmac driver.");
-MODULE_SUPPORTED_DEVICE("Broadcom 802.11 WLAN fullmac cards");
 MODULE_LICENSE("Dual BSD/GPL");
 
 #define MAX_WAIT_FOR_8021X_TX		50	/* msecs */
 
 /* Error bits */
 int brcmf_msg_level;
-module_param(brcmf_msg_level, int, 0);
+module_param_named(debug, brcmf_msg_level, int, S_IRUSR | S_IWUSR);
+MODULE_PARM_DESC(debug, "level of debug output");
 
 /* P2P0 enable */
 static int brcmf_p2p_enable;
@@ -222,18 +223,7 @@ static netdev_tx_t brcmf_netdev_start_xmit(struct sk_buff *skb,
 		goto done;
 	}
 
-	/* handle ethernet header */
-	eh = (struct ethhdr *)(skb->data);
-	if (is_multicast_ether_addr(eh->h_dest))
-		drvr->tx_multicast++;
-	if (ntohs(eh->h_proto) == ETH_P_PAE)
-		atomic_inc(&ifp->pend_8021x_cnt);
-
-	/* If the protocol uses a data header, apply it */
-	brcmf_proto_hdrpush(drvr, ifp->ifidx, skb);
-
-	/* Use bus module to send data frame */
-	ret =  brcmf_bus_txdata(drvr->bus_if, skb);
+	ret = brcmf_fws_process_skb(ifp, skb);
 
 done:
 	if (ret) {
@@ -247,9 +237,27 @@ done:
 	return NETDEV_TX_OK;
 }
 
+void brcmf_txflowblock_if(struct brcmf_if *ifp,
+			  enum brcmf_netif_stop_reason reason, bool state)
+{
+	if (!ifp)
+		return;
+
+	brcmf_dbg(TRACE, "enter: idx=%d stop=0x%X reason=%d state=%d\n",
+		  ifp->bssidx, ifp->netif_stop, reason, state);
+	if (state) {
+		if (!ifp->netif_stop)
+			netif_stop_queue(ifp->ndev);
+		ifp->netif_stop |= reason;
+	} else {
+		ifp->netif_stop &= ~reason;
+		if (!ifp->netif_stop)
+			netif_wake_queue(ifp->ndev);
+	}
+}
+
 void brcmf_txflowblock(struct device *dev, bool state)
 {
-	struct net_device *ndev;
 	struct brcmf_bus *bus_if = dev_get_drvdata(dev);
 	struct brcmf_pub *drvr = bus_if->drvr;
 	int i;
@@ -257,13 +265,8 @@ void brcmf_txflowblock(struct device *dev, bool state)
 	brcmf_dbg(TRACE, "Enter\n");
 
 	for (i = 0; i < BRCMF_MAX_IFS; i++)
-		if (drvr->iflist[i]) {
-			ndev = drvr->iflist[i]->ndev;
-			if (state)
-				netif_stop_queue(ndev);
-			else
-				netif_wake_queue(ndev);
-		}
+		brcmf_txflowblock_if(drvr->iflist[i],
+				     BRCMF_NETIF_STOP_REASON_BLOCK_BUS, state);
 }
 
 void brcmf_rx_frames(struct device *dev, struct sk_buff_head *skb_list)
@@ -283,7 +286,7 @@ void brcmf_rx_frames(struct device *dev, struct sk_buff_head *skb_list)
 		skb_unlink(skb, skb_list);
 
 		/* process and remove protocol-specific header */
-		ret = brcmf_proto_hdrpull(drvr, &ifidx, skb);
+		ret = brcmf_proto_hdrpull(drvr, drvr->fw_signals, &ifidx, skb);
 		ifp = drvr->iflist[ifidx];
 
 		if (ret || !ifp || !ifp->ndev) {
@@ -320,13 +323,8 @@ void brcmf_rx_frames(struct device *dev, struct sk_buff_head *skb_list)
 		/* Strip header, count, deliver upward */
 		skb_pull(skb, ETH_HLEN);
 
-		/* Process special event packets and then discard them */
-		brcmf_fweh_process_skb(drvr, skb, &ifidx);
-
-		if (drvr->iflist[ifidx]) {
-			ifp = drvr->iflist[ifidx];
-			ifp->ndev->last_rx = jiffies;
-		}
+		/* Process special event packets */
+		brcmf_fweh_process_skb(drvr, skb);
 
 		if (!(ifp->ndev->flags & IFF_UP)) {
 			brcmu_pkt_buf_free_skb(skb);
@@ -349,31 +347,49 @@ void brcmf_rx_frames(struct device *dev, struct sk_buff_head *skb_list)
 	}
 }
 
-void brcmf_txcomplete(struct device *dev, struct sk_buff *txp, bool success)
+void brcmf_txfinalize(struct brcmf_pub *drvr, struct sk_buff *txp,
+		      bool success)
 {
-	u8 ifidx;
+	struct brcmf_if *ifp;
 	struct ethhdr *eh;
+	u8 ifidx;
 	u16 type;
-	struct brcmf_bus *bus_if = dev_get_drvdata(dev);
-	struct brcmf_pub *drvr = bus_if->drvr;
-	struct brcmf_if *ifp;
+	int res;
 
-	brcmf_proto_hdrpull(drvr, &ifidx, txp);
+	res = brcmf_proto_hdrpull(drvr, false, &ifidx, txp);
 
 	ifp = drvr->iflist[ifidx];
 	if (!ifp)
-		return;
+		goto done;
 
-	eh = (struct ethhdr *)(txp->data);
-	type = ntohs(eh->h_proto);
+	if (res == 0) {
+		eh = (struct ethhdr *)(txp->data);
+		type = ntohs(eh->h_proto);
 
-	if (type == ETH_P_PAE) {
-		atomic_dec(&ifp->pend_8021x_cnt);
-		if (waitqueue_active(&ifp->pend_8021x_wait))
-			wake_up(&ifp->pend_8021x_wait);
+		if (type == ETH_P_PAE) {
+			atomic_dec(&ifp->pend_8021x_cnt);
+			if (waitqueue_active(&ifp->pend_8021x_wait))
+				wake_up(&ifp->pend_8021x_wait);
+		}
 	}
 	if (!success)
 		ifp->stats.tx_errors++;
+done:
+	brcmu_pkt_buf_free_skb(txp);
+}
+
+void brcmf_txcomplete(struct device *dev, struct sk_buff *txp, bool success)
+{
+	struct brcmf_bus *bus_if = dev_get_drvdata(dev);
+	struct brcmf_pub *drvr = bus_if->drvr;
+
+	/* await txstatus signal for firmware if active */
+	if (brcmf_fws_fc_active(drvr->fws)) {
+		if (!success)
+			brcmf_fws_bustxfail(drvr->fws, txp);
+	} else {
+		brcmf_txfinalize(drvr, txp, success);
+	}
 }
 
 static struct net_device_stats *brcmf_netdev_get_stats(struct net_device *ndev)
@@ -734,28 +750,35 @@ struct brcmf_if *brcmf_add_if(struct brcmf_pub *drvr, s32 bssidx, s32 ifidx,
 		}
 	}
 
-	/* Allocate netdev, including space for private structure */
-	ndev = alloc_netdev(sizeof(struct brcmf_if), name, ether_setup);
-	if (!ndev) {
-		brcmf_err("OOM - alloc_netdev\n");
-		return ERR_PTR(-ENOMEM);
+	if (!brcmf_p2p_enable && bssidx == 1) {
+		/* this is P2P_DEVICE interface */
+		brcmf_dbg(INFO, "allocate non-netdev interface\n");
+		ifp = kzalloc(sizeof(*ifp), GFP_KERNEL);
+		if (!ifp)
+			return ERR_PTR(-ENOMEM);
+	} else {
+		brcmf_dbg(INFO, "allocate netdev interface\n");
+		/* Allocate netdev, including space for private structure */
+		ndev = alloc_netdev(sizeof(*ifp), name, ether_setup);
+		if (!ndev)
+			return ERR_PTR(-ENOMEM);
+
+		ifp = netdev_priv(ndev);
+		ifp->ndev = ndev;
 	}
 
-	ifp = netdev_priv(ndev);
-	ifp->ndev = ndev;
 	ifp->drvr = drvr;
 	drvr->iflist[bssidx] = ifp;
 	ifp->ifidx = ifidx;
 	ifp->bssidx = bssidx;
 
-
 	init_waitqueue_head(&ifp->pend_8021x_wait);
 
 	if (mac_addr != NULL)
 		memcpy(ifp->mac_addr, mac_addr, ETH_ALEN);
 
 	brcmf_dbg(TRACE, " ==== pid:%x, if:%s (%pM) created ===\n",
-		  current->pid, ifp->ndev->name, ifp->mac_addr);
+		  current->pid, name, ifp->mac_addr);
 
 	return ifp;
 }
@@ -787,11 +810,13 @@ void brcmf_del_if(struct brcmf_pub *drvr, s32 bssidx)
 		}
 
 		unregister_netdev(ifp->ndev);
-		drvr->iflist[bssidx] = NULL;
 		if (bssidx == 0)
 			brcmf_cfg80211_detach(drvr->config);
 		free_netdev(ifp->ndev);
+	} else {
+		kfree(ifp);
 	}
+	drvr->iflist[bssidx] = NULL;
 }
 
 int brcmf_attach(uint bus_hdrlen, struct device *dev)
@@ -873,6 +898,13 @@ int brcmf_bus_start(struct device *dev)
 	if (ret < 0)
 		goto fail;
 
+	drvr->fw_signals = true;
+	ret = brcmf_fws_init(drvr);
+	if (ret < 0)
+		goto fail;
+
+	brcmf_fws_add_interface(ifp);
+
 	drvr->config = brcmf_cfg80211_attach(drvr, bus_if->dev);
 	if (drvr->config == NULL) {
 		ret = -ENOMEM;
@@ -889,6 +921,10 @@ fail:
 		brcmf_err("failed: %d\n", ret);
 		if (drvr->config)
 			brcmf_cfg80211_detach(drvr->config);
+		if (drvr->fws) {
+			brcmf_fws_del_interface(ifp);
+			brcmf_fws_deinit(drvr);
+		}
 		free_netdev(ifp->ndev);
 		drvr->iflist[0] = NULL;
 		if (p2p_ifp) {
@@ -944,14 +980,18 @@ void brcmf_detach(struct device *dev)
 
 	/* make sure primary interface removed last */
 	for (i = BRCMF_MAX_IFS-1; i > -1; i--)
-		if (drvr->iflist[i])
+		if (drvr->iflist[i]) {
+			brcmf_fws_del_interface(drvr->iflist[i]);
 			brcmf_del_if(drvr, i);
+		}
 
 	brcmf_bus_detach(drvr);
 
 	if (drvr->prot)
 		brcmf_proto_detach(drvr);
 
+	brcmf_fws_deinit(drvr);
+
 	brcmf_debugfs_detach(drvr);
 	bus_if->drvr = NULL;
 	kfree(drvr);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_proto.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd_proto.h
index 48fa70302192..ef9179883748 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_proto.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_proto.h
@@ -33,7 +33,7 @@ extern void brcmf_proto_stop(struct brcmf_pub *drvr);
 /* Add any protocol-specific data header.
  * Caller must reserve prot_hdrlen prepend space.
  */
-extern void brcmf_proto_hdrpush(struct brcmf_pub *, int ifidx,
+extern void brcmf_proto_hdrpush(struct brcmf_pub *, int ifidx, u8 offset,
 				struct sk_buff *txp);
 
 /* Sets dongle media info (drv_version, mac address). */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
index 35fc68be158d..d2487518bd2a 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
@@ -31,6 +31,7 @@
 #include <linux/bcma/bcma.h>
 #include <linux/debugfs.h>
 #include <linux/vmalloc.h>
+#include <linux/platform_data/brcmfmac-sdio.h>
 #include <asm/unaligned.h>
 #include <defs.h>
 #include <brcmu_wifi.h>
@@ -94,6 +95,7 @@ struct rte_console {
 
 #include "dhd_bus.h"
 #include "dhd_dbg.h"
+#include "tracepoint.h"
 
 #define TXQLEN		2048	/* bulk tx queue length */
 #define TXHI		(TXQLEN - 256)	/* turn on flow control above TXHI */
@@ -323,6 +325,9 @@ MODULE_FIRMWARE(BRCMF_SDIO_NV_NAME);
 					 */
 #define BRCMF_IDLE_INTERVAL	1
 
+#define KSO_WAIT_US 50
+#define MAX_KSO_ATTEMPTS (PMU_MAX_TRANSITION_DLY/KSO_WAIT_US)
+
 /*
  * Conversion of 802.1D priority to precedence level
  */
@@ -332,95 +337,6 @@ static uint prio2prec(u32 prio)
 	       (prio^2) : prio;
 }
 
-/* core registers */
-struct sdpcmd_regs {
-	u32 corecontrol;		/* 0x00, rev8 */
-	u32 corestatus;			/* rev8 */
-	u32 PAD[1];
-	u32 biststatus;			/* rev8 */
-
-	/* PCMCIA access */
-	u16 pcmciamesportaladdr;	/* 0x010, rev8 */
-	u16 PAD[1];
-	u16 pcmciamesportalmask;	/* rev8 */
-	u16 PAD[1];
-	u16 pcmciawrframebc;		/* rev8 */
-	u16 PAD[1];
-	u16 pcmciaunderflowtimer;	/* rev8 */
-	u16 PAD[1];
-
-	/* interrupt */
-	u32 intstatus;			/* 0x020, rev8 */
-	u32 hostintmask;		/* rev8 */
-	u32 intmask;			/* rev8 */
-	u32 sbintstatus;		/* rev8 */
-	u32 sbintmask;			/* rev8 */
-	u32 funcintmask;		/* rev4 */
-	u32 PAD[2];
-	u32 tosbmailbox;		/* 0x040, rev8 */
-	u32 tohostmailbox;		/* rev8 */
-	u32 tosbmailboxdata;		/* rev8 */
-	u32 tohostmailboxdata;		/* rev8 */
-
-	/* synchronized access to registers in SDIO clock domain */
-	u32 sdioaccess;			/* 0x050, rev8 */
-	u32 PAD[3];
-
-	/* PCMCIA frame control */
-	u8 pcmciaframectrl;		/* 0x060, rev8 */
-	u8 PAD[3];
-	u8 pcmciawatermark;		/* rev8 */
-	u8 PAD[155];
-
-	/* interrupt batching control */
-	u32 intrcvlazy;			/* 0x100, rev8 */
-	u32 PAD[3];
-
-	/* counters */
-	u32 cmd52rd;			/* 0x110, rev8 */
-	u32 cmd52wr;			/* rev8 */
-	u32 cmd53rd;			/* rev8 */
-	u32 cmd53wr;			/* rev8 */
-	u32 abort;			/* rev8 */
-	u32 datacrcerror;		/* rev8 */
-	u32 rdoutofsync;		/* rev8 */
-	u32 wroutofsync;		/* rev8 */
-	u32 writebusy;			/* rev8 */
-	u32 readwait;			/* rev8 */
-	u32 readterm;			/* rev8 */
-	u32 writeterm;			/* rev8 */
-	u32 PAD[40];
-	u32 clockctlstatus;		/* rev8 */
-	u32 PAD[7];
-
-	u32 PAD[128];			/* DMA engines */
-
-	/* SDIO/PCMCIA CIS region */
-	char cis[512];			/* 0x400-0x5ff, rev6 */
-
-	/* PCMCIA function control registers */
-	char pcmciafcr[256];		/* 0x600-6ff, rev6 */
-	u16 PAD[55];
-
-	/* PCMCIA backplane access */
-	u16 backplanecsr;		/* 0x76E, rev6 */
-	u16 backplaneaddr0;		/* rev6 */
-	u16 backplaneaddr1;		/* rev6 */
-	u16 backplaneaddr2;		/* rev6 */
-	u16 backplaneaddr3;		/* rev6 */
-	u16 backplanedata0;		/* rev6 */
-	u16 backplanedata1;		/* rev6 */
-	u16 backplanedata2;		/* rev6 */
-	u16 backplanedata3;		/* rev6 */
-	u16 PAD[31];
-
-	/* sprom "size" & "blank" info */
-	u16 spromstatus;		/* 0x7BE, rev2 */
-	u32 PAD[464];
-
-	u16 PAD[0x80];
-};
-
 #ifdef DEBUG
 /* Device console log buffer state */
 struct brcmf_console {
@@ -587,12 +503,14 @@ struct brcmf_sdio {
 
 	bool txoff;		/* Transmit flow-controlled */
 	struct brcmf_sdio_count sdcnt;
+	bool sr_enabled; /* SaveRestore enabled */
+	bool sleeping; /* SDIO bus sleeping */
 };
 
 /* clkstate */
 #define CLK_NONE	0
 #define CLK_SDONLY	1
-#define CLK_PENDING	2	/* Not used yet */
+#define CLK_PENDING	2
 #define CLK_AVAIL	3
 
 #ifdef DEBUG
@@ -600,7 +518,7 @@ static int qcount[NUMPRIO];
 static int tx_packets[NUMPRIO];
 #endif				/* DEBUG */
 
-#define SDIO_DRIVE_STRENGTH	6	/* in milliamps */
+#define DEFAULT_SDIO_DRIVE_STRENGTH	6	/* in milliamps */
 
 #define RETRYCHAN(chan) ((chan) == SDPCM_EVENT_CHANNEL)
 
@@ -664,6 +582,62 @@ w_sdreg32(struct brcmf_sdio *bus, u32 regval, u32 reg_offset)
 	return ret;
 }
 
+static int
+brcmf_sdbrcm_kso_control(struct brcmf_sdio *bus, bool on)
+{
+	u8 wr_val = 0, rd_val, cmp_val, bmask;
+	int err = 0;
+	int try_cnt = 0;
+
+	brcmf_dbg(TRACE, "Enter\n");
+
+	wr_val = (on << SBSDIO_FUNC1_SLEEPCSR_KSO_SHIFT);
+	/* 1st KSO write goes to AOS wake up core if device is asleep  */
+	brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_SLEEPCSR,
+			 wr_val, &err);
+	if (err) {
+		brcmf_err("SDIO_AOS KSO write error: %d\n", err);
+		return err;
+	}
+
+	if (on) {
+		/* device WAKEUP through KSO:
+		 * write bit 0 & read back until
+		 * both bits 0 (kso bit) & 1 (dev on status) are set
+		 */
+		cmp_val = SBSDIO_FUNC1_SLEEPCSR_KSO_MASK |
+			  SBSDIO_FUNC1_SLEEPCSR_DEVON_MASK;
+		bmask = cmp_val;
+		usleep_range(2000, 3000);
+	} else {
+		/* Put device to sleep, turn off KSO */
+		cmp_val = 0;
+		/* only check for bit0, bit1(dev on status) may not
+		 * get cleared right away
+		 */
+		bmask = SBSDIO_FUNC1_SLEEPCSR_KSO_MASK;
+	}
+
+	do {
+		/* reliable KSO bit set/clr:
+		 * the sdiod sleep write access is synced to PMU 32khz clk
+		 * just one write attempt may fail,
+		 * read it back until it matches written value
+		 */
+		rd_val = brcmf_sdio_regrb(bus->sdiodev, SBSDIO_FUNC1_SLEEPCSR,
+					  &err);
+		if (((rd_val & bmask) == cmp_val) && !err)
+			break;
+		brcmf_dbg(SDIO, "KSO wr/rd retry:%d (max: %d) ERR:%x\n",
+			  try_cnt, MAX_KSO_ATTEMPTS, err);
+		udelay(KSO_WAIT_US);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_SLEEPCSR,
+				 wr_val, &err);
+	} while (try_cnt++ < MAX_KSO_ATTEMPTS);
+
+	return err;
+}
+
 #define PKT_AVAILABLE()		(intstatus & I_HMB_FRAME_IND)
 
 #define HOSTINTMASK		(I_HMB_SW_MASK | I_CHIPACTIVE)
@@ -675,10 +649,15 @@ static int brcmf_sdbrcm_htclk(struct brcmf_sdio *bus, bool on, bool pendok)
 	u8 clkctl, clkreq, devctl;
 	unsigned long timeout;
 
-	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_dbg(SDIO, "Enter\n");
 
 	clkctl = 0;
 
+	if (bus->sr_enabled) {
+		bus->clkstate = (on ? CLK_AVAIL : CLK_SDONLY);
+		return 0;
+	}
+
 	if (on) {
 		/* Request HT Avail */
 		clkreq =
@@ -713,7 +692,7 @@ static int brcmf_sdbrcm_htclk(struct brcmf_sdio *bus, bool on, bool pendok)
 			devctl |= SBSDIO_DEVCTL_CA_INT_ONLY;
 			brcmf_sdio_regwb(bus->sdiodev, SBSDIO_DEVICE_CTL,
 					 devctl, &err);
-			brcmf_dbg(INFO, "CLKCTL: set PENDING\n");
+			brcmf_dbg(SDIO, "CLKCTL: set PENDING\n");
 			bus->clkstate = CLK_PENDING;
 
 			return 0;
@@ -750,7 +729,7 @@ static int brcmf_sdbrcm_htclk(struct brcmf_sdio *bus, bool on, bool pendok)
 
 		/* Mark clock available */
 		bus->clkstate = CLK_AVAIL;
-		brcmf_dbg(INFO, "CLKCTL: turned ON\n");
+		brcmf_dbg(SDIO, "CLKCTL: turned ON\n");
 
 #if defined(DEBUG)
 		if (!bus->alp_only) {
@@ -775,7 +754,7 @@ static int brcmf_sdbrcm_htclk(struct brcmf_sdio *bus, bool on, bool pendok)
 		bus->clkstate = CLK_SDONLY;
 		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
 				 clkreq, &err);
-		brcmf_dbg(INFO, "CLKCTL: turned OFF\n");
+		brcmf_dbg(SDIO, "CLKCTL: turned OFF\n");
 		if (err) {
 			brcmf_err("Failed access turning clock off: %d\n",
 				  err);
@@ -788,7 +767,7 @@ static int brcmf_sdbrcm_htclk(struct brcmf_sdio *bus, bool on, bool pendok)
 /* Change idle/active SD state */
 static int brcmf_sdbrcm_sdclk(struct brcmf_sdio *bus, bool on)
 {
-	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_dbg(SDIO, "Enter\n");
 
 	if (on)
 		bus->clkstate = CLK_SDONLY;
@@ -805,7 +784,7 @@ static int brcmf_sdbrcm_clkctl(struct brcmf_sdio *bus, uint target, bool pendok)
 	uint oldstate = bus->clkstate;
 #endif				/* DEBUG */
 
-	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_dbg(SDIO, "Enter\n");
 
 	/* Early exit if we're already there */
 	if (bus->clkstate == target) {
@@ -849,12 +828,69 @@ static int brcmf_sdbrcm_clkctl(struct brcmf_sdio *bus, uint target, bool pendok)
 		break;
 	}
 #ifdef DEBUG
-	brcmf_dbg(INFO, "%d -> %d\n", oldstate, bus->clkstate);
+	brcmf_dbg(SDIO, "%d -> %d\n", oldstate, bus->clkstate);
 #endif				/* DEBUG */
 
 	return 0;
 }
 
+static int
+brcmf_sdbrcm_bus_sleep(struct brcmf_sdio *bus, bool sleep, bool pendok)
+{
+	int err = 0;
+	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_dbg(SDIO, "request %s currently %s\n",
+		  (sleep ? "SLEEP" : "WAKE"),
+		  (bus->sleeping ? "SLEEP" : "WAKE"));
+
+	/* If SR is enabled control bus state with KSO */
+	if (bus->sr_enabled) {
+		/* Done if we're already in the requested state */
+		if (sleep == bus->sleeping)
+			goto end;
+
+		/* Going to sleep */
+		if (sleep) {
+			/* Don't sleep if something is pending */
+			if (atomic_read(&bus->intstatus) ||
+			    atomic_read(&bus->ipend) > 0 ||
+			    (!atomic_read(&bus->fcstate) &&
+			    brcmu_pktq_mlen(&bus->txq, ~bus->flowcontrol) &&
+			    data_ok(bus)))
+				 return -EBUSY;
+			err = brcmf_sdbrcm_kso_control(bus, false);
+			/* disable watchdog */
+			if (!err)
+				brcmf_sdbrcm_wd_timer(bus, 0);
+		} else {
+			bus->idlecount = 0;
+			err = brcmf_sdbrcm_kso_control(bus, true);
+		}
+		if (!err) {
+			/* Change state */
+			bus->sleeping = sleep;
+			brcmf_dbg(SDIO, "new state %s\n",
+				  (sleep ? "SLEEP" : "WAKE"));
+		} else {
+			brcmf_err("error while changing bus sleep state %d\n",
+				  err);
+			return err;
+		}
+	}
+
+end:
+	/* control clocks */
+	if (sleep) {
+		if (!bus->sr_enabled)
+			brcmf_sdbrcm_clkctl(bus, CLK_NONE, pendok);
+	} else {
+		brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, pendok);
+	}
+
+	return err;
+
+}
+
 static u32 brcmf_sdbrcm_hostmail(struct brcmf_sdio *bus)
 {
 	u32 intstatus = 0;
@@ -862,7 +898,7 @@ static u32 brcmf_sdbrcm_hostmail(struct brcmf_sdio *bus)
 	u8 fcbits;
 	int ret;
 
-	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_dbg(SDIO, "Enter\n");
 
 	/* Read mailbox data and ack that we did so */
 	ret = r_sdreg32(bus, &hmb_data,
@@ -875,7 +911,7 @@ static u32 brcmf_sdbrcm_hostmail(struct brcmf_sdio *bus)
 
 	/* Dongle recomposed rx frames, accept them again */
 	if (hmb_data & HMB_DATA_NAKHANDLED) {
-		brcmf_dbg(INFO, "Dongle reports NAK handled, expect rtx of %d\n",
+		brcmf_dbg(SDIO, "Dongle reports NAK handled, expect rtx of %d\n",
 			  bus->rx_seq);
 		if (!bus->rxskip)
 			brcmf_err("unexpected NAKHANDLED!\n");
@@ -896,7 +932,7 @@ static u32 brcmf_sdbrcm_hostmail(struct brcmf_sdio *bus)
 				  "expecting %d\n",
 				  bus->sdpcm_ver, SDPCM_PROT_VERSION);
 		else
-			brcmf_dbg(INFO, "Dongle ready, protocol version %d\n",
+			brcmf_dbg(SDIO, "Dongle ready, protocol version %d\n",
 				  bus->sdpcm_ver);
 	}
 
@@ -970,7 +1006,7 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_sdio *bus, bool abort, bool rtx)
 	if (!retries)
 		brcmf_err("count never zeroed: last 0x%04x\n", lastrbc);
 	else
-		brcmf_dbg(INFO, "flush took %d iterations\n", 0xffff - retries);
+		brcmf_dbg(SDIO, "flush took %d iterations\n", 0xffff - retries);
 
 	if (rtx) {
 		bus->sdcnt.rxrtx++;
@@ -1173,7 +1209,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
 	/* If packets, issue read(s) and send up packet chain */
 	/* Return sequence numbers consumed? */
 
-	brcmf_dbg(TRACE, "start: glomd %p glom %p\n",
+	brcmf_dbg(SDIO, "start: glomd %p glom %p\n",
 		  bus->glomd, skb_peek(&bus->glom));
 
 	/* If there's a descriptor, generate the packet chain */
@@ -1546,7 +1582,7 @@ static uint brcmf_sdio_readframes(struct brcmf_sdio *bus, uint maxframes)
 	struct sk_buff_head pktlist;	/* needed for bus interface */
 	u16 pad;		/* Number of pad bytes to read */
 	uint rxleft = 0;	/* Remaining number of frames allowed */
-	int sdret;		/* Return code from calls */
+	int ret;		/* Return code from calls */
 	uint rxcount = 0;	/* Total frames read */
 	struct brcmf_sdio_read *rd = &bus->cur_read, rd_new;
 	u8 head_read = 0;
@@ -1577,15 +1613,15 @@ static uint brcmf_sdio_readframes(struct brcmf_sdio *bus, uint maxframes)
 		/* read header first for unknow frame length */
 		sdio_claim_host(bus->sdiodev->func[1]);
 		if (!rd->len) {
-			sdret = brcmf_sdcard_recv_buf(bus->sdiodev,
+			ret = brcmf_sdcard_recv_buf(bus->sdiodev,
 						      bus->sdiodev->sbwad,
 						      SDIO_FUNC_2, F2SYNC,
 						      bus->rxhdr,
 						      BRCMF_FIRSTREAD);
 			bus->sdcnt.f2rxhdrs++;
-			if (sdret < 0) {
+			if (ret < 0) {
 				brcmf_err("RXHEADER FAILED: %d\n",
-					  sdret);
+					  ret);
 				bus->sdcnt.rx_hdrfail++;
 				brcmf_sdbrcm_rxfail(bus, true, true);
 				sdio_release_host(bus->sdiodev->func[1]);
@@ -1637,14 +1673,14 @@ static uint brcmf_sdio_readframes(struct brcmf_sdio *bus, uint maxframes)
 		skb_pull(pkt, head_read);
 		pkt_align(pkt, rd->len_left, BRCMF_SDALIGN);
 
-		sdret = brcmf_sdcard_recv_pkt(bus->sdiodev, bus->sdiodev->sbwad,
+		ret = brcmf_sdcard_recv_pkt(bus->sdiodev, bus->sdiodev->sbwad,
 					      SDIO_FUNC_2, F2SYNC, pkt);
 		bus->sdcnt.f2rxdata++;
 		sdio_release_host(bus->sdiodev->func[1]);
 
-		if (sdret < 0) {
+		if (ret < 0) {
 			brcmf_err("read %d bytes from channel %d failed: %d\n",
-				  rd->len, rd->channel, sdret);
+				  rd->len, rd->channel, ret);
 			brcmu_pkt_buf_free_skb(pkt);
 			sdio_claim_host(bus->sdiodev->func[1]);
 			brcmf_sdbrcm_rxfail(bus, true,
@@ -1775,13 +1811,12 @@ brcmf_sdbrcm_wait_event_wakeup(struct brcmf_sdio *bus)
 /* Writes a HW/SW header into the packet and sends it. */
 /* Assumes: (a) header space already there, (b) caller holds lock */
 static int brcmf_sdbrcm_txpkt(struct brcmf_sdio *bus, struct sk_buff *pkt,
-			      uint chan, bool free_pkt)
+			      uint chan)
 {
 	int ret;
 	u8 *frame;
 	u16 len, pad = 0;
 	u32 swheader;
-	struct sk_buff *new;
 	int i;
 
 	brcmf_dbg(TRACE, "Enter\n");
@@ -1795,30 +1830,14 @@ static int brcmf_sdbrcm_txpkt(struct brcmf_sdio *bus, struct sk_buff *pkt,
 			brcmf_dbg(INFO, "insufficient headroom %d for %d pad\n",
 				  skb_headroom(pkt), pad);
 			bus->sdiodev->bus_if->tx_realloc++;
-			new = brcmu_pkt_buf_get_skb(pkt->len + BRCMF_SDALIGN);
-			if (!new) {
-				brcmf_err("couldn't allocate new %d-byte packet\n",
-					  pkt->len + BRCMF_SDALIGN);
-				ret = -ENOMEM;
+			ret = skb_cow(pkt, BRCMF_SDALIGN);
+			if (ret)
 				goto done;
-			}
-
-			pkt_align(new, pkt->len, BRCMF_SDALIGN);
-			memcpy(new->data, pkt->data, pkt->len);
-			if (free_pkt)
-				brcmu_pkt_buf_free_skb(pkt);
-			/* free the pkt if canned one is not used */
-			free_pkt = true;
-			pkt = new;
-			frame = (u8 *) (pkt->data);
-			/* precondition: (frame % BRCMF_SDALIGN) == 0) */
-			pad = 0;
-		} else {
-			skb_push(pkt, pad);
-			frame = (u8 *) (pkt->data);
-			/* precondition: pad + SDPCM_HDRLEN <= pkt->len */
-			memset(frame, 0, pad + SDPCM_HDRLEN);
+			pad = ((unsigned long)frame % BRCMF_SDALIGN);
 		}
+		skb_push(pkt, pad);
+		frame = (u8 *) (pkt->data);
+		memset(frame, 0, pad + SDPCM_HDRLEN);
 	}
 	/* precondition: pad < BRCMF_SDALIGN */
 
@@ -1833,8 +1852,8 @@ static int brcmf_sdbrcm_txpkt(struct brcmf_sdio *bus, struct sk_buff *pkt,
 	    (((pad +
 	       SDPCM_HDRLEN) << SDPCM_DOFFSET_SHIFT) & SDPCM_DOFFSET_MASK);
 
-	put_unaligned_le32(swheader, frame + SDPCM_FRAMETAG_LEN);
-	put_unaligned_le32(0, frame + SDPCM_FRAMETAG_LEN + sizeof(swheader));
+	*(((__le32 *) frame) + 1) = cpu_to_le32(swheader);
+	*(((__le32 *) frame) + 2) = 0;
 
 #ifdef DEBUG
 	tx_packets[pkt->priority]++;
@@ -1900,11 +1919,7 @@ static int brcmf_sdbrcm_txpkt(struct brcmf_sdio *bus, struct sk_buff *pkt,
 done:
 	/* restore pkt buffer pointer before calling tx complete routine */
 	skb_pull(pkt, SDPCM_HDRLEN + pad);
-	brcmf_txcomplete(bus->sdiodev->dev, pkt, ret != 0);
-
-	if (free_pkt)
-		brcmu_pkt_buf_free_skb(pkt);
-
+	brcmf_txcomplete(bus->sdiodev->dev, pkt, ret == 0);
 	return ret;
 }
 
@@ -1932,7 +1947,7 @@ static uint brcmf_sdbrcm_sendfromq(struct brcmf_sdio *bus, uint maxframes)
 		spin_unlock_bh(&bus->txqlock);
 		datalen = pkt->len - SDPCM_HDRLEN;
 
-		ret = brcmf_sdbrcm_txpkt(bus, pkt, SDPCM_DATA_CHANNEL, true);
+		ret = brcmf_sdbrcm_txpkt(bus, pkt, SDPCM_DATA_CHANNEL);
 
 		/* In poll mode, need to check for other events */
 		if (!bus->intr && cnt) {
@@ -1980,7 +1995,7 @@ static void brcmf_sdbrcm_bus_stop(struct device *dev)
 	sdio_claim_host(bus->sdiodev->func[1]);
 
 	/* Enable clock for device interrupts */
-	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
+	brcmf_sdbrcm_bus_sleep(bus, false, false);
 
 	/* Disable and clear interrupts at the chip level also */
 	w_sdreg32(bus, 0, offsetof(struct sdpcmd_regs, hostintmask));
@@ -2032,23 +2047,19 @@ static void brcmf_sdbrcm_bus_stop(struct device *dev)
 	bus->tx_seq = bus->rx_seq = 0;
 }
 
-#ifdef CONFIG_BRCMFMAC_SDIO_OOB
 static inline void brcmf_sdbrcm_clrintr(struct brcmf_sdio *bus)
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&bus->sdiodev->irq_en_lock, flags);
-	if (!bus->sdiodev->irq_en && !atomic_read(&bus->ipend)) {
-		enable_irq(bus->sdiodev->irq);
-		bus->sdiodev->irq_en = true;
+	if (bus->sdiodev->oob_irq_requested) {
+		spin_lock_irqsave(&bus->sdiodev->irq_en_lock, flags);
+		if (!bus->sdiodev->irq_en && !atomic_read(&bus->ipend)) {
+			enable_irq(bus->sdiodev->pdata->oob_irq_nr);
+			bus->sdiodev->irq_en = true;
+		}
+		spin_unlock_irqrestore(&bus->sdiodev->irq_en_lock, flags);
 	}
-	spin_unlock_irqrestore(&bus->sdiodev->irq_en_lock, flags);
 }
-#else
-static inline void brcmf_sdbrcm_clrintr(struct brcmf_sdio *bus)
-{
-}
-#endif		/* CONFIG_BRCMFMAC_SDIO_OOB */
 
 static inline void brcmf_sdbrcm_adddpctsk(struct brcmf_sdio *bus)
 {
@@ -2116,7 +2127,7 @@ static void brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 	sdio_claim_host(bus->sdiodev->func[1]);
 
 	/* If waiting for HTAVAIL, check status */
-	if (bus->clkstate == CLK_PENDING) {
+	if (!bus->sr_enabled && bus->clkstate == CLK_PENDING) {
 		u8 clkctl, devctl = 0;
 
 #ifdef DEBUG
@@ -2138,7 +2149,7 @@ static void brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 			bus->sdiodev->bus_if->state = BRCMF_BUS_DOWN;
 		}
 
-		brcmf_dbg(INFO, "DPC: PENDING, devctl 0x%02x clkctl 0x%02x\n",
+		brcmf_dbg(SDIO, "DPC: PENDING, devctl 0x%02x clkctl 0x%02x\n",
 			  devctl, clkctl);
 
 		if (SBSDIO_HTAV(clkctl)) {
@@ -2162,7 +2173,7 @@ static void brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 	}
 
 	/* Make sure backplane clock is on */
-	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, true);
+	brcmf_sdbrcm_bus_sleep(bus, false, true);
 
 	/* Pending interrupt indicates new device status */
 	if (atomic_read(&bus->ipend) > 0) {
@@ -2308,12 +2319,22 @@ static void brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 	if ((bus->clkstate != CLK_PENDING)
 	    && bus->idletime == BRCMF_IDLE_IMMEDIATE) {
 		bus->activity = false;
+		brcmf_dbg(SDIO, "idle state\n");
 		sdio_claim_host(bus->sdiodev->func[1]);
-		brcmf_sdbrcm_clkctl(bus, CLK_NONE, false);
+		brcmf_sdbrcm_bus_sleep(bus, true, false);
 		sdio_release_host(bus->sdiodev->func[1]);
 	}
 }
 
+static struct pktq *brcmf_sdbrcm_bus_gettxq(struct device *dev)
+{
+	struct brcmf_bus *bus_if = dev_get_drvdata(dev);
+	struct brcmf_sdio_dev *sdiodev = bus_if->bus_priv.sdio;
+	struct brcmf_sdio *bus = sdiodev->bus;
+
+	return &bus->txq;
+}
+
 static int brcmf_sdbrcm_bus_txdata(struct device *dev, struct sk_buff *pkt)
 {
 	int ret = -EBADE;
@@ -2343,7 +2364,6 @@ static int brcmf_sdbrcm_bus_txdata(struct device *dev, struct sk_buff *pkt)
 	if (!brcmf_c_prec_enq(bus->sdiodev->dev, &bus->txq, pkt, prec)) {
 		skb_pull(pkt, SDPCM_HDRLEN);
 		brcmf_txcomplete(bus->sdiodev->dev, pkt, false);
-		brcmu_pkt_buf_free_skb(pkt);
 		brcmf_err("out of bus->txq !!!\n");
 		ret = -ENOSR;
 	} else {
@@ -2374,69 +2394,6 @@ static int brcmf_sdbrcm_bus_txdata(struct device *dev, struct sk_buff *pkt)
 	return ret;
 }
 
-static int
-brcmf_sdbrcm_membytes(struct brcmf_sdio *bus, bool write, u32 address, u8 *data,
-		 uint size)
-{
-	int bcmerror = 0;
-	u32 sdaddr;
-	uint dsize;
-
-	/* Determine initial transfer parameters */
-	sdaddr = address & SBSDIO_SB_OFT_ADDR_MASK;
-	if ((sdaddr + size) & SBSDIO_SBWINDOW_MASK)
-		dsize = (SBSDIO_SB_OFT_ADDR_LIMIT - sdaddr);
-	else
-		dsize = size;
-
-	sdio_claim_host(bus->sdiodev->func[1]);
-
-	/* Set the backplane window to include the start address */
-	bcmerror = brcmf_sdcard_set_sbaddr_window(bus->sdiodev, address);
-	if (bcmerror) {
-		brcmf_err("window change failed\n");
-		goto xfer_done;
-	}
-
-	/* Do the transfer(s) */
-	while (size) {
-		brcmf_dbg(INFO, "%s %d bytes at offset 0x%08x in window 0x%08x\n",
-			  write ? "write" : "read", dsize,
-			  sdaddr, address & SBSDIO_SBWINDOW_MASK);
-		bcmerror = brcmf_sdcard_rwdata(bus->sdiodev, write,
-					       sdaddr, data, dsize);
-		if (bcmerror) {
-			brcmf_err("membytes transfer failed\n");
-			break;
-		}
-
-		/* Adjust for next transfer (if any) */
-		size -= dsize;
-		if (size) {
-			data += dsize;
-			address += dsize;
-			bcmerror = brcmf_sdcard_set_sbaddr_window(bus->sdiodev,
-								  address);
-			if (bcmerror) {
-				brcmf_err("window change failed\n");
-				break;
-			}
-			sdaddr = 0;
-			dsize = min_t(uint, SBSDIO_SB_OFT_ADDR_LIMIT, size);
-		}
-	}
-
-xfer_done:
-	/* Return the window to backplane enumeration space for core access */
-	if (brcmf_sdcard_set_sbaddr_window(bus->sdiodev, bus->sdiodev->sbwad))
-		brcmf_err("FAILED to set window back to 0x%x\n",
-			  bus->sdiodev->sbwad);
-
-	sdio_release_host(bus->sdiodev->func[1]);
-
-	return bcmerror;
-}
-
 #ifdef DEBUG
 #define CONSOLE_LINE_MAX	192
 
@@ -2453,8 +2410,8 @@ static int brcmf_sdbrcm_readconsole(struct brcmf_sdio *bus)
 
 	/* Read console log struct */
 	addr = bus->console_addr + offsetof(struct rte_console, log_le);
-	rv = brcmf_sdbrcm_membytes(bus, false, addr, (u8 *)&c->log_le,
-				   sizeof(c->log_le));
+	rv = brcmf_sdio_ramrw(bus->sdiodev, false, addr, (u8 *)&c->log_le,
+			      sizeof(c->log_le));
 	if (rv < 0)
 		return rv;
 
@@ -2479,7 +2436,7 @@ static int brcmf_sdbrcm_readconsole(struct brcmf_sdio *bus)
 
 	/* Read the console buffer */
 	addr = le32_to_cpu(c->log_le.buf);
-	rv = brcmf_sdbrcm_membytes(bus, false, addr, c->buf, c->bufsize);
+	rv = brcmf_sdio_ramrw(bus->sdiodev, false, addr, c->buf, c->bufsize);
 	if (rv < 0)
 		return rv;
 
@@ -2604,7 +2561,7 @@ brcmf_sdbrcm_bus_txctl(struct device *dev, unsigned char *msg, uint msglen)
 
 	/* Make sure backplane clock is on */
 	sdio_claim_host(bus->sdiodev->func[1]);
-	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
+	brcmf_sdbrcm_bus_sleep(bus, false, false);
 	sdio_release_host(bus->sdiodev->func[1]);
 
 	/* Hardware tag: 2 byte len followed by 2 byte ~len check (all LE) */
@@ -2633,10 +2590,10 @@ brcmf_sdbrcm_bus_txctl(struct device *dev, unsigned char *msg, uint msglen)
 						 msecs_to_jiffies(2000));
 
 		if (!bus->ctrl_frame_stat) {
-			brcmf_dbg(INFO, "ctrl_frame_stat == false\n");
+			brcmf_dbg(SDIO, "ctrl_frame_stat == false\n");
 			ret = 0;
 		} else {
-			brcmf_dbg(INFO, "ctrl_frame_stat == true\n");
+			brcmf_dbg(SDIO, "ctrl_frame_stat == true\n");
 			ret = -1;
 		}
 	}
@@ -2662,6 +2619,7 @@ brcmf_sdbrcm_bus_txctl(struct device *dev, unsigned char *msg, uint msglen)
 
 		bus->activity = false;
 		sdio_claim_host(bus->sdiodev->func[1]);
+		brcmf_dbg(INFO, "idle\n");
 		brcmf_sdbrcm_clkctl(bus, CLK_NONE, true);
 		sdio_release_host(bus->sdiodev->func[1]);
 	} else {
@@ -2691,23 +2649,22 @@ static int brcmf_sdio_readshared(struct brcmf_sdio *bus,
 	struct sdpcm_shared_le sh_le;
 	__le32 addr_le;
 
-	shaddr = bus->ramsize - 4;
+	shaddr = bus->ci->rambase + bus->ramsize - 4;
 
 	/*
 	 * Read last word in socram to determine
 	 * address of sdpcm_shared structure
 	 */
 	sdio_claim_host(bus->sdiodev->func[1]);
-	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
-	rv = brcmf_sdbrcm_membytes(bus, false, shaddr,
-				   (u8 *)&addr_le, 4);
+	brcmf_sdbrcm_bus_sleep(bus, false, false);
+	rv = brcmf_sdio_ramrw(bus->sdiodev, false, shaddr, (u8 *)&addr_le, 4);
 	sdio_release_host(bus->sdiodev->func[1]);
 	if (rv < 0)
 		return rv;
 
 	addr = le32_to_cpu(addr_le);
 
-	brcmf_dbg(INFO, "sdpcm_shared address 0x%08X\n", addr);
+	brcmf_dbg(SDIO, "sdpcm_shared address 0x%08X\n", addr);
 
 	/*
 	 * Check if addr is valid.
@@ -2720,8 +2677,8 @@ static int brcmf_sdio_readshared(struct brcmf_sdio *bus,
 	}
 
 	/* Read hndrte_shared structure */
-	rv = brcmf_sdbrcm_membytes(bus, false, addr, (u8 *)&sh_le,
-				   sizeof(struct sdpcm_shared_le));
+	rv = brcmf_sdio_ramrw(bus->sdiodev, false, addr, (u8 *)&sh_le,
+			      sizeof(struct sdpcm_shared_le));
 	if (rv < 0)
 		return rv;
 
@@ -2734,8 +2691,8 @@ static int brcmf_sdio_readshared(struct brcmf_sdio *bus,
 	sh->console_addr = le32_to_cpu(sh_le.console_addr);
 	sh->msgtrace_addr = le32_to_cpu(sh_le.msgtrace_addr);
 
-	if ((sh->flags & SDPCM_SHARED_VERSION_MASK) != SDPCM_SHARED_VERSION) {
-		brcmf_err("sdpcm_shared version mismatch: dhd %d dongle %d\n",
+	if ((sh->flags & SDPCM_SHARED_VERSION_MASK) > SDPCM_SHARED_VERSION) {
+		brcmf_err("sdpcm shared version unsupported: dhd %d dongle %d\n",
 			  SDPCM_SHARED_VERSION,
 			  sh->flags & SDPCM_SHARED_VERSION_MASK);
 		return -EPROTO;
@@ -2757,22 +2714,22 @@ static int brcmf_sdio_dump_console(struct brcmf_sdio *bus,
 
 	/* obtain console information from device memory */
 	addr = sh->console_addr + offsetof(struct rte_console, log_le);
-	rv = brcmf_sdbrcm_membytes(bus, false, addr,
-			(u8 *)&sh_val, sizeof(u32));
+	rv = brcmf_sdio_ramrw(bus->sdiodev, false, addr,
+			      (u8 *)&sh_val, sizeof(u32));
 	if (rv < 0)
 		return rv;
 	console_ptr = le32_to_cpu(sh_val);
 
 	addr = sh->console_addr + offsetof(struct rte_console, log_le.buf_size);
-	rv = brcmf_sdbrcm_membytes(bus, false, addr,
-			(u8 *)&sh_val, sizeof(u32));
+	rv = brcmf_sdio_ramrw(bus->sdiodev, false, addr,
+			      (u8 *)&sh_val, sizeof(u32));
 	if (rv < 0)
 		return rv;
 	console_size = le32_to_cpu(sh_val);
 
 	addr = sh->console_addr + offsetof(struct rte_console, log_le.idx);
-	rv = brcmf_sdbrcm_membytes(bus, false, addr,
-			(u8 *)&sh_val, sizeof(u32));
+	rv = brcmf_sdio_ramrw(bus->sdiodev, false, addr,
+			      (u8 *)&sh_val, sizeof(u32));
 	if (rv < 0)
 		return rv;
 	console_index = le32_to_cpu(sh_val);
@@ -2786,8 +2743,8 @@ static int brcmf_sdio_dump_console(struct brcmf_sdio *bus,
 
 	/* obtain the console data from device */
 	conbuf[console_size] = '\0';
-	rv = brcmf_sdbrcm_membytes(bus, false, console_ptr, (u8 *)conbuf,
-				   console_size);
+	rv = brcmf_sdio_ramrw(bus->sdiodev, false, console_ptr, (u8 *)conbuf,
+			      console_size);
 	if (rv < 0)
 		goto done;
 
@@ -2817,21 +2774,18 @@ static int brcmf_sdio_trap_info(struct brcmf_sdio *bus, struct sdpcm_shared *sh,
 	int error, res;
 	char buf[350];
 	struct brcmf_trap_info tr;
-	int nbytes;
 	loff_t pos = 0;
 
-	if ((sh->flags & SDPCM_SHARED_TRAP) == 0)
+	if ((sh->flags & SDPCM_SHARED_TRAP) == 0) {
+		brcmf_dbg(INFO, "no trap in firmware\n");
 		return 0;
+	}
 
-	error = brcmf_sdbrcm_membytes(bus, false, sh->trap_addr, (u8 *)&tr,
-				      sizeof(struct brcmf_trap_info));
+	error = brcmf_sdio_ramrw(bus->sdiodev, false, sh->trap_addr, (u8 *)&tr,
+				 sizeof(struct brcmf_trap_info));
 	if (error < 0)
 		return error;
 
-	nbytes = brcmf_sdio_dump_console(bus, sh, data, count);
-	if (nbytes < 0)
-		return nbytes;
-
 	res = scnprintf(buf, sizeof(buf),
 			"dongle trap info: type 0x%x @ epc 0x%08x\n"
 			"  cpsr 0x%08x spsr 0x%08x sp 0x%08x\n"
@@ -2847,12 +2801,7 @@ static int brcmf_sdio_trap_info(struct brcmf_sdio *bus, struct sdpcm_shared *sh,
 			le32_to_cpu(tr.r4), le32_to_cpu(tr.r5),
 			le32_to_cpu(tr.r6), le32_to_cpu(tr.r7));
 
-	error = simple_read_from_buffer(data+nbytes, count, &pos, buf, res);
-	if (error < 0)
-		return error;
-
-	nbytes += error;
-	return nbytes;
+	return simple_read_from_buffer(data, count, &pos, buf, res);
 }
 
 static int brcmf_sdio_assert_info(struct brcmf_sdio *bus,
@@ -2876,14 +2825,14 @@ static int brcmf_sdio_assert_info(struct brcmf_sdio *bus,
 
 	sdio_claim_host(bus->sdiodev->func[1]);
 	if (sh->assert_file_addr != 0) {
-		error = brcmf_sdbrcm_membytes(bus, false, sh->assert_file_addr,
-					      (u8 *)file, 80);
+		error = brcmf_sdio_ramrw(bus->sdiodev, false,
+					 sh->assert_file_addr, (u8 *)file, 80);
 		if (error < 0)
 			return error;
 	}
 	if (sh->assert_exp_addr != 0) {
-		error = brcmf_sdbrcm_membytes(bus, false, sh->assert_exp_addr,
-					      (u8 *)expr, 80);
+		error = brcmf_sdio_ramrw(bus->sdiodev, false,
+					 sh->assert_exp_addr, (u8 *)expr, 80);
 		if (error < 0)
 			return error;
 	}
@@ -2934,14 +2883,20 @@ static int brcmf_sdbrcm_died_dump(struct brcmf_sdio *bus, char __user *data,
 	error = brcmf_sdio_assert_info(bus, &sh, data, count);
 	if (error < 0)
 		goto done;
-
 	nbytes = error;
-	error = brcmf_sdio_trap_info(bus, &sh, data, count);
+
+	error = brcmf_sdio_trap_info(bus, &sh, data+nbytes, count);
+	if (error < 0)
+		goto done;
+	nbytes += error;
+
+	error = brcmf_sdio_dump_console(bus, &sh, data+nbytes, count);
 	if (error < 0)
 		goto done;
+	nbytes += error;
 
-	error += nbytes;
-	*ppos += error;
+	error = nbytes;
+	*ppos += nbytes;
 done:
 	return error;
 }
@@ -3035,84 +2990,8 @@ brcmf_sdbrcm_bus_rxctl(struct device *dev, unsigned char *msg, uint msglen)
 	return rxlen ? (int)rxlen : -ETIMEDOUT;
 }
 
-static int brcmf_sdbrcm_write_vars(struct brcmf_sdio *bus)
+static bool brcmf_sdbrcm_download_state(struct brcmf_sdio *bus, bool enter)
 {
-	int bcmerror = 0;
-	u32 varaddr;
-	u32 varsizew;
-	__le32 varsizew_le;
-#ifdef DEBUG
-	char *nvram_ularray;
-#endif				/* DEBUG */
-
-	/* Even if there are no vars are to be written, we still
-		 need to set the ramsize. */
-	varaddr = (bus->ramsize - 4) - bus->varsz;
-
-	if (bus->vars) {
-		/* Write the vars list */
-		bcmerror = brcmf_sdbrcm_membytes(bus, true, varaddr,
-						 bus->vars, bus->varsz);
-#ifdef DEBUG
-		/* Verify NVRAM bytes */
-		brcmf_dbg(INFO, "Compare NVRAM dl & ul; varsize=%d\n",
-			  bus->varsz);
-		nvram_ularray = kmalloc(bus->varsz, GFP_ATOMIC);
-		if (!nvram_ularray)
-			return -ENOMEM;
-
-		/* Upload image to verify downloaded contents. */
-		memset(nvram_ularray, 0xaa, bus->varsz);
-
-		/* Read the vars list to temp buffer for comparison */
-		bcmerror = brcmf_sdbrcm_membytes(bus, false, varaddr,
-						 nvram_ularray, bus->varsz);
-		if (bcmerror) {
-			brcmf_err("error %d on reading %d nvram bytes at 0x%08x\n",
-				  bcmerror, bus->varsz, varaddr);
-		}
-		/* Compare the org NVRAM with the one read from RAM */
-		if (memcmp(bus->vars, nvram_ularray, bus->varsz))
-			brcmf_err("Downloaded NVRAM image is corrupted\n");
-		else
-			brcmf_err("Download/Upload/Compare of NVRAM ok\n");
-
-		kfree(nvram_ularray);
-#endif				/* DEBUG */
-	}
-
-	/* adjust to the user specified RAM */
-	brcmf_dbg(INFO, "Physical memory size: %d\n", bus->ramsize);
-	brcmf_dbg(INFO, "Vars are at %d, orig varsize is %d\n",
-		  varaddr, bus->varsz);
-
-	/*
-	 * Determine the length token:
-	 * Varsize, converted to words, in lower 16-bits, checksum
-	 * in upper 16-bits.
-	 */
-	if (bcmerror) {
-		varsizew = 0;
-		varsizew_le = cpu_to_le32(0);
-	} else {
-		varsizew = bus->varsz / 4;
-		varsizew = (~varsizew << 16) | (varsizew & 0x0000FFFF);
-		varsizew_le = cpu_to_le32(varsizew);
-	}
-
-	brcmf_dbg(INFO, "New varsize is %d, length token=0x%08x\n",
-		  bus->varsz, varsizew);
-
-	/* Write the length token to the last word */
-	bcmerror = brcmf_sdbrcm_membytes(bus, true, (bus->ramsize - 4),
-					 (u8 *)&varsizew_le, 4);
-
-	return bcmerror;
-}
-
-static int brcmf_sdbrcm_download_state(struct brcmf_sdio *bus, bool enter)
-{
-	int bcmerror = 0;
 	struct chip_info *ci = bus->ci;
 
 	/* To enter download state, disable ARM and reset SOCRAM.
@@ -3121,41 +3000,19 @@ static int brcmf_sdbrcm_download_state(struct brcmf_sdio *bus, bool enter)
 	if (enter) {
 		bus->alp_only = true;
 
-		ci->coredisable(bus->sdiodev, ci, BCMA_CORE_ARM_CM3);
-
-		ci->resetcore(bus->sdiodev, ci, BCMA_CORE_INTERNAL_MEM);
-
-		/* Clear the top bit of memory */
-		if (bus->ramsize) {
-			u32 zeros = 0;
-			brcmf_sdbrcm_membytes(bus, true, bus->ramsize - 4,
-					 (u8 *)&zeros, 4);
-		}
+		brcmf_sdio_chip_enter_download(bus->sdiodev, ci);
 	} else {
-		if (!ci->iscoreup(bus->sdiodev, ci, BCMA_CORE_INTERNAL_MEM)) {
-			brcmf_err("SOCRAM core is down after reset?\n");
-			bcmerror = -EBADE;
-			goto fail;
-		}
-
-		bcmerror = brcmf_sdbrcm_write_vars(bus);
-		if (bcmerror) {
-			brcmf_err("no vars written to RAM\n");
-			bcmerror = 0;
-		}
-
-		w_sdreg32(bus, 0xFFFFFFFF,
-			  offsetof(struct sdpcmd_regs, intstatus));
-
-		ci->resetcore(bus->sdiodev, ci, BCMA_CORE_ARM_CM3);
+		if (!brcmf_sdio_chip_exit_download(bus->sdiodev, ci, bus->vars,
+						   bus->varsz))
+			return false;
 
 		/* Allow HT Clock now that the ARM is running. */
 		bus->alp_only = false;
 
 		bus->sdiodev->bus_if->state = BRCMF_BUS_LOAD;
 	}
-fail:
-	return bcmerror;
+
+	return true;
 }
 
 static int brcmf_sdbrcm_get_image(char *buf, int len, struct brcmf_sdio *bus)
@@ -3170,10 +3027,11 @@ static int brcmf_sdbrcm_get_image(char *buf, int len, struct brcmf_sdio *bus)
 
 static int brcmf_sdbrcm_download_code_file(struct brcmf_sdio *bus)
 {
-	int offset = 0;
+	int offset;
 	uint len;
 	u8 *memblock = NULL, *memptr;
 	int ret;
+	u8 idx;
 
 	brcmf_dbg(INFO, "Enter\n");
 
@@ -3194,10 +3052,15 @@ static int brcmf_sdbrcm_download_code_file(struct brcmf_sdio *bus)
 		memptr += (BRCMF_SDALIGN -
 			   ((u32)(unsigned long)memblock % BRCMF_SDALIGN));
 
+	offset = bus->ci->rambase;
+
 	/* Download image */
-	while ((len =
-		brcmf_sdbrcm_get_image((char *)memptr, MEMBLOCK, bus))) {
-		ret = brcmf_sdbrcm_membytes(bus, true, offset, memptr, len);
+	len = brcmf_sdbrcm_get_image((char *)memptr, MEMBLOCK, bus);
+	idx = brcmf_sdio_chip_getinfidx(bus->ci, BCMA_CORE_ARM_CR4);
+	if (BRCMF_MAX_CORENUM != idx)
+		memcpy(&bus->ci->rst_vec, memptr, sizeof(bus->ci->rst_vec));
+	while (len) {
+		ret = brcmf_sdio_ramrw(bus->sdiodev, true, offset, memptr, len);
 		if (ret) {
 			brcmf_err("error %d on writing %d membytes at 0x%08x\n",
 				  ret, MEMBLOCK, offset);
@@ -3205,6 +3068,7 @@ static int brcmf_sdbrcm_download_code_file(struct brcmf_sdio *bus)
 		}
 
 		offset += MEMBLOCK;
+		len = brcmf_sdbrcm_get_image((char *)memptr, MEMBLOCK, bus);
 	}
 
 err:
@@ -3312,7 +3176,7 @@ static int _brcmf_sdbrcm_download_firmware(struct brcmf_sdio *bus)
 	int bcmerror = -1;
 
 	/* Keep arm in reset */
-	if (brcmf_sdbrcm_download_state(bus, true)) {
+	if (!brcmf_sdbrcm_download_state(bus, true)) {
 		brcmf_err("error placing ARM core in reset\n");
 		goto err;
 	}
@@ -3328,7 +3192,7 @@ static int _brcmf_sdbrcm_download_firmware(struct brcmf_sdio *bus)
 	}
 
 	/* Take arm out of reset */
-	if (brcmf_sdbrcm_download_state(bus, false)) {
+	if (!brcmf_sdbrcm_download_state(bus, false)) {
 		brcmf_err("error getting out of ARM core reset\n");
 		goto err;
 	}
@@ -3339,6 +3203,103 @@ err:
 	return bcmerror;
 }
 
+static bool brcmf_sdbrcm_sr_capable(struct brcmf_sdio *bus)
+{
+	u32 addr, reg;
+
+	brcmf_dbg(TRACE, "Enter\n");
+
+	/* old chips with PMU version less than 17 don't support save restore */
+	if (bus->ci->pmurev < 17)
+		return false;
+
+	/* read PMU chipcontrol register 3*/
+	addr = CORE_CC_REG(bus->ci->c_inf[0].base, chipcontrol_addr);
+	brcmf_sdio_regwl(bus->sdiodev, addr, 3, NULL);
+	addr = CORE_CC_REG(bus->ci->c_inf[0].base, chipcontrol_data);
+	reg = brcmf_sdio_regrl(bus->sdiodev, addr, NULL);
+
+	return (bool)reg;
+}
+
+static void brcmf_sdbrcm_sr_init(struct brcmf_sdio *bus)
+{
+	int err = 0;
+	u8 val;
+
+	brcmf_dbg(TRACE, "Enter\n");
+
+	val = brcmf_sdio_regrb(bus->sdiodev, SBSDIO_FUNC1_WAKEUPCTRL,
+			       &err);
+	if (err) {
+		brcmf_err("error reading SBSDIO_FUNC1_WAKEUPCTRL\n");
+		return;
+	}
+
+	val |= 1 << SBSDIO_FUNC1_WCTRL_HTWAIT_SHIFT;
+	brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_WAKEUPCTRL,
+			 val, &err);
+	if (err) {
+		brcmf_err("error writing SBSDIO_FUNC1_WAKEUPCTRL\n");
+		return;
+	}
+
+	/* Add CMD14 Support */
+	brcmf_sdio_regwb(bus->sdiodev, SDIO_CCCR_BRCM_CARDCAP,
+			 (SDIO_CCCR_BRCM_CARDCAP_CMD14_SUPPORT |
+			  SDIO_CCCR_BRCM_CARDCAP_CMD14_EXT),
+			 &err);
+	if (err) {
+		brcmf_err("error writing SDIO_CCCR_BRCM_CARDCAP\n");
+		return;
+	}
+
+	brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+			 SBSDIO_FORCE_HT, &err);
+	if (err) {
+		brcmf_err("error writing SBSDIO_FUNC1_CHIPCLKCSR\n");
+		return;
+	}
+
+	/* set flag */
+	bus->sr_enabled = true;
+	brcmf_dbg(INFO, "SR enabled\n");
+}
+
+/* enable KSO bit */
+static int brcmf_sdbrcm_kso_init(struct brcmf_sdio *bus)
+{
+	u8 val;
+	int err = 0;
+
+	brcmf_dbg(TRACE, "Enter\n");
+
+	/* KSO bit added in SDIO core rev 12 */
+	if (bus->ci->c_inf[1].rev < 12)
+		return 0;
+
+	val = brcmf_sdio_regrb(bus->sdiodev, SBSDIO_FUNC1_SLEEPCSR,
+			       &err);
+	if (err) {
+		brcmf_err("error reading SBSDIO_FUNC1_SLEEPCSR\n");
+		return err;
+	}
+
+	if (!(val & SBSDIO_FUNC1_SLEEPCSR_KSO_MASK)) {
+		val |= (SBSDIO_FUNC1_SLEEPCSR_KSO_EN <<
+			SBSDIO_FUNC1_SLEEPCSR_KSO_SHIFT);
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_SLEEPCSR,
+				 val, &err);
+		if (err) {
+			brcmf_err("error writing SBSDIO_FUNC1_SLEEPCSR\n");
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+
 static bool
 brcmf_sdbrcm_download_firmware(struct brcmf_sdio *bus)
 {
@@ -3437,8 +3398,13 @@ static int brcmf_sdbrcm_bus_init(struct device *dev)
 		ret = -ENODEV;
 	}
 
-	/* Restore previous clock setting */
-	brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR, saveclk, &err);
+	if (brcmf_sdbrcm_sr_capable(bus)) {
+		brcmf_sdbrcm_sr_init(bus);
+	} else {
+		/* Restore previous clock setting */
+		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_CHIPCLKCSR,
+				 saveclk, &err);
+	}
 
 	if (ret == 0) {
 		ret = brcmf_sdio_intr_register(bus->sdiodev);
@@ -3499,7 +3465,8 @@ static bool brcmf_sdbrcm_bus_watchdog(struct brcmf_sdio *bus)
 	brcmf_dbg(TIMER, "Enter\n");
 
 	/* Poll period: check device if appropriate. */
-	if (bus->poll && (++bus->polltick >= bus->pollrate)) {
+	if (!bus->sr_enabled &&
+	    bus->poll && (++bus->polltick >= bus->pollrate)) {
 		u32 intstatus = 0;
 
 		/* Reset poll tick */
@@ -3550,7 +3517,7 @@ static bool brcmf_sdbrcm_bus_watchdog(struct brcmf_sdio *bus)
 			bus->console.count -= bus->console_interval;
 			sdio_claim_host(bus->sdiodev->func[1]);
 			/* Make sure backplane clock is on */
-			brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
+			brcmf_sdbrcm_bus_sleep(bus, false, false);
 			if (brcmf_sdbrcm_readconsole(bus) < 0)
 				/* stop on error */
 				bus->console_interval = 0;
@@ -3567,8 +3534,9 @@ static bool brcmf_sdbrcm_bus_watchdog(struct brcmf_sdio *bus)
 				bus->activity = false;
 				brcmf_sdbrcm_wd_timer(bus, BRCMF_WD_POLL_MS);
 			} else {
+				brcmf_dbg(SDIO, "idle\n");
 				sdio_claim_host(bus->sdiodev->func[1]);
-				brcmf_sdbrcm_clkctl(bus, CLK_NONE, false);
+				brcmf_sdbrcm_bus_sleep(bus, true, false);
 				sdio_release_host(bus->sdiodev->func[1]);
 			}
 		}
@@ -3579,6 +3547,8 @@ static bool brcmf_sdbrcm_bus_watchdog(struct brcmf_sdio *bus)
 
 static bool brcmf_sdbrcm_chipmatch(u16 chipid)
 {
+	if (chipid == BCM43143_CHIP_ID)
+		return true;
 	if (chipid == BCM43241_CHIP_ID)
 		return true;
 	if (chipid == BCM4329_CHIP_ID)
@@ -3587,6 +3557,8 @@ static bool brcmf_sdbrcm_chipmatch(u16 chipid)
 		return true;
 	if (chipid == BCM4334_CHIP_ID)
 		return true;
+	if (chipid == BCM4335_CHIP_ID)
+		return true;
 	return false;
 }
 
@@ -3664,7 +3636,7 @@ brcmf_sdbrcm_probe_attach(struct brcmf_sdio *bus, u32 regsva)
 	int err = 0;
 	int reg_addr;
 	u32 reg_val;
-	u8 idx;
+	u32 drivestrength;
 
 	bus->alp_only = true;
 
@@ -3700,8 +3672,16 @@ brcmf_sdbrcm_probe_attach(struct brcmf_sdio *bus, u32 regsva)
 		goto fail;
 	}
 
-	brcmf_sdio_chip_drivestrengthinit(bus->sdiodev, bus->ci,
-					  SDIO_DRIVE_STRENGTH);
+	if (brcmf_sdbrcm_kso_init(bus)) {
+		brcmf_err("error enabling KSO\n");
+		goto fail;
+	}
+
+	if ((bus->sdiodev->pdata) && (bus->sdiodev->pdata->drive_strength))
+		drivestrength = bus->sdiodev->pdata->drive_strength;
+	else
+		drivestrength = DEFAULT_SDIO_DRIVE_STRENGTH;
+	brcmf_sdio_chip_drivestrengthinit(bus->sdiodev, bus->ci, drivestrength);
 
 	/* Get info on the SOCRAM cores... */
 	bus->ramsize = bus->ci->ramsize;
@@ -3710,12 +3690,37 @@ brcmf_sdbrcm_probe_attach(struct brcmf_sdio *bus, u32 regsva)
 		goto fail;
 	}
 
-	/* Set core control so an SDIO reset does a backplane reset */
-	idx = brcmf_sdio_chip_getinfidx(bus->ci, BCMA_CORE_SDIO_DEV);
-	reg_addr = bus->ci->c_inf[idx].base +
-		   offsetof(struct sdpcmd_regs, corecontrol);
-	reg_val = brcmf_sdio_regrl(bus->sdiodev, reg_addr, NULL);
-	brcmf_sdio_regwl(bus->sdiodev, reg_addr, reg_val | CC_BPRESEN, NULL);
+	/* Set card control so an SDIO card reset does a WLAN backplane reset */
+	reg_val = brcmf_sdio_regrb(bus->sdiodev,
+				   SDIO_CCCR_BRCM_CARDCTRL, &err);
+	if (err)
+		goto fail;
+
+	reg_val |= SDIO_CCCR_BRCM_CARDCTRL_WLANRESET;
+
+	brcmf_sdio_regwb(bus->sdiodev,
+			 SDIO_CCCR_BRCM_CARDCTRL, reg_val, &err);
+	if (err)
+		goto fail;
+
+	/* set PMUControl so a backplane reset does PMU state reload */
+	reg_addr = CORE_CC_REG(bus->ci->c_inf[0].base,
+			       pmucontrol);
+	reg_val = brcmf_sdio_regrl(bus->sdiodev,
+				   reg_addr,
+				   &err);
+	if (err)
+		goto fail;
+
+	reg_val |= (BCMA_CC_PMU_CTL_RES_RELOAD << BCMA_CC_PMU_CTL_RES_SHIFT);
+
+	brcmf_sdio_regwl(bus->sdiodev,
+			 reg_addr,
+			 reg_val,
+			 &err);
+	if (err)
+		goto fail;
+
 
 	sdio_release_host(bus->sdiodev->func[1]);
 
@@ -3769,6 +3774,10 @@ static bool brcmf_sdbrcm_probe_init(struct brcmf_sdio *bus)
 	bus->use_rxchain = false;
 	bus->sd_rxchain = false;
 
+	/* SR state */
+	bus->sleeping = false;
+	bus->sr_enabled = false;
+
 	return true;
 }
 
@@ -3856,6 +3865,7 @@ static struct brcmf_bus_ops brcmf_sdio_bus_ops = {
 	.txdata = brcmf_sdbrcm_bus_txdata,
 	.txctl = brcmf_sdbrcm_bus_txctl,
 	.rxctl = brcmf_sdbrcm_bus_rxctl,
+	.gettxq = brcmf_sdbrcm_bus_gettxq,
 };
 
 void *brcmf_sdbrcm_probe(u32 regsva, struct brcmf_sdio_dev *sdiodev)
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fweh.c b/drivers/net/wireless/brcm80211/brcmfmac/fweh.c
index e9d6f91a1f2b..5a64280e6485 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fweh.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fweh.c
@@ -20,6 +20,8 @@
 
 #include "dhd.h"
 #include "dhd_dbg.h"
+#include "tracepoint.h"
+#include "fwsignal.h"
 #include "fweh.h"
 #include "fwil.h"
 
@@ -154,7 +156,7 @@ static int brcmf_fweh_call_event_handler(struct brcmf_if *ifp,
 		fweh = &ifp->drvr->fweh;
 
 		/* handle the event if valid interface and handler */
-		if (ifp->ndev && fweh->evt_handler[code])
+		if (fweh->evt_handler[code])
 			err = fweh->evt_handler[code](ifp, emsg, data);
 		else
 			brcmf_err("unhandled event %d ignored\n", code);
@@ -179,9 +181,9 @@ static void brcmf_fweh_handle_if_event(struct brcmf_pub *drvr,
 	struct brcmf_if *ifp;
 	int err = 0;
 
-	brcmf_dbg(EVENT, "action: %u idx: %u bsscfg: %u flags: %u\n",
-		  ifevent->action, ifevent->ifidx,
-		  ifevent->bssidx, ifevent->flags);
+	brcmf_dbg(EVENT, "action: %u idx: %u bsscfg: %u flags: %u role: %u\n",
+		  ifevent->action, ifevent->ifidx, ifevent->bssidx,
+		  ifevent->flags, ifevent->role);
 
 	if (ifevent->ifidx >= BRCMF_MAX_IFS) {
 		brcmf_err("invalid interface index: %u\n",
@@ -198,15 +200,20 @@ static void brcmf_fweh_handle_if_event(struct brcmf_pub *drvr,
 				   emsg->ifname, emsg->addr);
 		if (IS_ERR(ifp))
 			return;
-
+		brcmf_fws_add_interface(ifp);
 		if (!drvr->fweh.evt_handler[BRCMF_E_IF])
 			err = brcmf_net_attach(ifp, false);
 	}
 
+	if (ifevent->action == BRCMF_E_IF_CHANGE)
+		brcmf_fws_reset_interface(ifp);
+
 	err = brcmf_fweh_call_event_handler(ifp, emsg->event_code, emsg, data);
 
-	if (ifevent->action == BRCMF_E_IF_DEL)
+	if (ifevent->action == BRCMF_E_IF_DEL) {
+		brcmf_fws_del_interface(ifp);
 		brcmf_del_if(drvr, ifevent->bssidx);
+	}
 }
 
 /**
@@ -400,13 +407,12 @@ int brcmf_fweh_activate_events(struct brcmf_if *ifp)
  *
  * @drvr: driver information object.
  * @event_packet: event packet to process.
- * @ifidx: index of the firmware interface (may change).
  *
  * If the packet buffer contains a firmware event message it will
  * dispatch the event to a registered handler (using worker).
  */
 void brcmf_fweh_process_event(struct brcmf_pub *drvr,
-			      struct brcmf_event *event_packet, u8 *ifidx)
+			      struct brcmf_event *event_packet)
 {
 	enum brcmf_fweh_event_code code;
 	struct brcmf_fweh_info *fweh = &drvr->fweh;
@@ -418,7 +424,6 @@ void brcmf_fweh_process_event(struct brcmf_pub *drvr,
 	/* get event info */
 	code = get_unaligned_be32(&event_packet->msg.event_type);
 	datalen = get_unaligned_be32(&event_packet->msg.datalen);
-	*ifidx = event_packet->msg.ifidx;
 	data = &event_packet[1];
 
 	if (code >= BRCMF_E_LAST)
@@ -435,7 +440,7 @@ void brcmf_fweh_process_event(struct brcmf_pub *drvr,
 		return;
 
 	event->code = code;
-	event->ifidx = *ifidx;
+	event->ifidx = event_packet->msg.ifidx;
 
 	/* use memcpy to get aligned event message */
 	memcpy(&event->emsg, &event_packet->msg, sizeof(event->emsg));
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fweh.h b/drivers/net/wireless/brcm80211/brcmfmac/fweh.h
index 8c39b51dcccf..6ec5db9c60a5 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fweh.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fweh.h
@@ -187,10 +187,10 @@ void brcmf_fweh_unregister(struct brcmf_pub *drvr,
 			   enum brcmf_fweh_event_code code);
 int brcmf_fweh_activate_events(struct brcmf_if *ifp);
 void brcmf_fweh_process_event(struct brcmf_pub *drvr,
-			      struct brcmf_event *event_packet, u8 *ifidx);
+			      struct brcmf_event *event_packet);
 
 static inline void brcmf_fweh_process_skb(struct brcmf_pub *drvr,
-					  struct sk_buff *skb, u8 *ifidx)
+					  struct sk_buff *skb)
 {
 	struct brcmf_event *event_packet;
 	u8 *data;
@@ -213,7 +213,7 @@ static inline void brcmf_fweh_process_skb(struct brcmf_pub *drvr,
 	if (usr_stype != BCMILCP_BCM_SUBTYPE_EVENT)
 		return;
 
-	brcmf_fweh_process_event(drvr, event_packet, ifidx);
+	brcmf_fweh_process_event(drvr, event_packet);
 }
 
 #endif /* FWEH_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwil.c b/drivers/net/wireless/brcm80211/brcmfmac/fwil.c
index 8d1def935b8d..04f395930d86 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fwil.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fwil.c
@@ -25,6 +25,7 @@
 #include "dhd.h"
 #include "dhd_bus.h"
 #include "dhd_dbg.h"
+#include "tracepoint.h"
 #include "fwil.h"
 
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c b/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c
new file mode 100644
index 000000000000..5352dc1fdf3c
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c
@@ -0,0 +1,2067 @@
+/*
+ * Copyright (c) 2010 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/if_ether.h>
+#include <linux/spinlock.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/err.h>
+#include <linux/jiffies.h>
+#include <uapi/linux/nl80211.h>
+#include <net/cfg80211.h>
+
+#include <brcmu_utils.h>
+#include <brcmu_wifi.h>
+#include "dhd.h"
+#include "dhd_proto.h"
+#include "dhd_dbg.h"
+#include "dhd_bus.h"
+#include "fwil.h"
+#include "fwil_types.h"
+#include "fweh.h"
+#include "fwsignal.h"
+#include "p2p.h"
+#include "wl_cfg80211.h"
+
+/**
+ * DOC: Firmware Signalling
+ *
+ * Firmware can send signals to host and vice versa, which are passed in the
+ * data packets using TLV based header. This signalling layer is on top of the
+ * BDC bus protocol layer.
+ */
+
+/*
+ * single definition for firmware-driver flow control tlv's.
+ *
+ * each tlv is specified by BRCMF_FWS_TLV_DEF(name, ID, length).
+ * A length value 0 indicates variable length tlv.
+ */
+#define BRCMF_FWS_TLV_DEFLIST \
+	BRCMF_FWS_TLV_DEF(MAC_OPEN, 1, 1) \
+	BRCMF_FWS_TLV_DEF(MAC_CLOSE, 2, 1) \
+	BRCMF_FWS_TLV_DEF(MAC_REQUEST_CREDIT, 3, 2) \
+	BRCMF_FWS_TLV_DEF(TXSTATUS, 4, 4) \
+	BRCMF_FWS_TLV_DEF(PKTTAG, 5, 4) \
+	BRCMF_FWS_TLV_DEF(MACDESC_ADD,	6, 8) \
+	BRCMF_FWS_TLV_DEF(MACDESC_DEL, 7, 8) \
+	BRCMF_FWS_TLV_DEF(RSSI, 8, 1) \
+	BRCMF_FWS_TLV_DEF(INTERFACE_OPEN, 9, 1) \
+	BRCMF_FWS_TLV_DEF(INTERFACE_CLOSE, 10, 1) \
+	BRCMF_FWS_TLV_DEF(FIFO_CREDITBACK, 11, 6) \
+	BRCMF_FWS_TLV_DEF(PENDING_TRAFFIC_BMP, 12, 2) \
+	BRCMF_FWS_TLV_DEF(MAC_REQUEST_PACKET, 13, 3) \
+	BRCMF_FWS_TLV_DEF(HOST_REORDER_RXPKTS, 14, 10) \
+	BRCMF_FWS_TLV_DEF(TRANS_ID, 18, 6) \
+	BRCMF_FWS_TLV_DEF(COMP_TXSTATUS, 19, 1) \
+	BRCMF_FWS_TLV_DEF(FILLER, 255, 0)
+
+/*
+ * enum brcmf_fws_tlv_type - definition of tlv identifiers.
+ */
+#define BRCMF_FWS_TLV_DEF(name, id, len) \
+	BRCMF_FWS_TYPE_ ## name =  id,
+enum brcmf_fws_tlv_type {
+	BRCMF_FWS_TLV_DEFLIST
+	BRCMF_FWS_TYPE_INVALID
+};
+#undef BRCMF_FWS_TLV_DEF
+
+/*
+ * enum brcmf_fws_tlv_len - definition of tlv lengths.
+ */
+#define BRCMF_FWS_TLV_DEF(name, id, len) \
+	BRCMF_FWS_TYPE_ ## name ## _LEN = (len),
+enum brcmf_fws_tlv_len {
+	BRCMF_FWS_TLV_DEFLIST
+};
+#undef BRCMF_FWS_TLV_DEF
+
+#ifdef DEBUG
+/*
+ * brcmf_fws_tlv_names - array of tlv names.
+ */
+#define BRCMF_FWS_TLV_DEF(name, id, len) \
+	{ id, #name },
+static struct {
+	enum brcmf_fws_tlv_type id;
+	const char *name;
+} brcmf_fws_tlv_names[] = {
+	BRCMF_FWS_TLV_DEFLIST
+};
+#undef BRCMF_FWS_TLV_DEF
+
+static const char *brcmf_fws_get_tlv_name(enum brcmf_fws_tlv_type id)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(brcmf_fws_tlv_names); i++)
+		if (brcmf_fws_tlv_names[i].id == id)
+			return brcmf_fws_tlv_names[i].name;
+
+	return "INVALID";
+}
+#else
+static const char *brcmf_fws_get_tlv_name(enum brcmf_fws_tlv_type id)
+{
+	return "NODEBUG";
+}
+#endif /* DEBUG */
+
+/*
+ * flags used to enable tlv signalling from firmware.
+ */
+#define BRCMF_FWS_FLAGS_RSSI_SIGNALS			0x0001
+#define BRCMF_FWS_FLAGS_XONXOFF_SIGNALS			0x0002
+#define BRCMF_FWS_FLAGS_CREDIT_STATUS_SIGNALS		0x0004
+#define BRCMF_FWS_FLAGS_HOST_PROPTXSTATUS_ACTIVE	0x0008
+#define BRCMF_FWS_FLAGS_PSQ_GENERATIONFSM_ENABLE	0x0010
+#define BRCMF_FWS_FLAGS_PSQ_ZERO_BUFFER_ENABLE		0x0020
+#define BRCMF_FWS_FLAGS_HOST_RXREORDER_ACTIVE		0x0040
+
+#define BRCMF_FWS_MAC_DESC_TABLE_SIZE			32
+#define BRCMF_FWS_MAC_DESC_ID_INVALID			0xff
+
+#define BRCMF_FWS_HOSTIF_FLOWSTATE_OFF			0
+#define BRCMF_FWS_HOSTIF_FLOWSTATE_ON			1
+#define BRCMF_FWS_FLOWCONTROL_HIWATER			128
+#define BRCMF_FWS_FLOWCONTROL_LOWATER			64
+
+#define BRCMF_FWS_PSQ_PREC_COUNT		((NL80211_NUM_ACS + 1) * 2)
+#define BRCMF_FWS_PSQ_LEN				256
+
+#define BRCMF_FWS_HTOD_FLAG_PKTFROMHOST			0x01
+#define BRCMF_FWS_HTOD_FLAG_PKT_REQUESTED		0x02
+
+#define BRCMF_FWS_RET_OK_NOSCHEDULE	0
+#define BRCMF_FWS_RET_OK_SCHEDULE	1
+
+/**
+ * enum brcmf_fws_skb_state - indicates processing state of skb.
+ *
+ * @BRCMF_FWS_SKBSTATE_NEW: sk_buff is newly arrived in the driver.
+ * @BRCMF_FWS_SKBSTATE_DELAYED: sk_buff had to wait on queue.
+ * @BRCMF_FWS_SKBSTATE_SUPPRESSED: sk_buff has been suppressed by firmware.
+ */
+enum brcmf_fws_skb_state {
+	BRCMF_FWS_SKBSTATE_NEW,
+	BRCMF_FWS_SKBSTATE_DELAYED,
+	BRCMF_FWS_SKBSTATE_SUPPRESSED
+};
+
+/**
+ * struct brcmf_skbuff_cb - control buffer associated with skbuff.
+ *
+ * @if_flags: holds interface index and packet related flags.
+ * @htod: host to device packet identifier (used in PKTTAG tlv).
+ * @state: transmit state of the packet.
+ * @mac: descriptor related to destination for this packet.
+ *
+ * This information is stored in control buffer struct sk_buff::cb, which
+ * provides 48 bytes of storage so this structure should not exceed that.
+ */
+struct brcmf_skbuff_cb {
+	u16 if_flags;
+	u32 htod;
+	enum brcmf_fws_skb_state state;
+	struct brcmf_fws_mac_descriptor *mac;
+};
+
+/*
+ * macro casting skbuff control buffer to struct brcmf_skbuff_cb.
+ */
+#define brcmf_skbcb(skb)	((struct brcmf_skbuff_cb *)((skb)->cb))
+
+/*
+ * sk_buff control if flags
+ *
+ *	b[11]  - packet sent upon firmware request.
+ *	b[10]  - packet only contains signalling data.
+ *	b[9]   - packet is a tx packet.
+ *	b[8]   - packet uses FIFO credit (non-pspoll).
+ *	b[7]   - interface in AP mode.
+ *	b[6:4] - AC FIFO number.
+ *	b[3:0] - interface index.
+ */
+#define BRCMF_SKB_IF_FLAGS_REQUESTED_MASK	0x0800
+#define BRCMF_SKB_IF_FLAGS_REQUESTED_SHIFT	11
+#define BRCMF_SKB_IF_FLAGS_SIGNAL_ONLY_MASK	0x0400
+#define BRCMF_SKB_IF_FLAGS_SIGNAL_ONLY_SHIFT	10
+#define BRCMF_SKB_IF_FLAGS_TRANSMIT_MASK        0x0200
+#define BRCMF_SKB_IF_FLAGS_TRANSMIT_SHIFT	9
+#define BRCMF_SKB_IF_FLAGS_CREDITCHECK_MASK	0x0100
+#define BRCMF_SKB_IF_FLAGS_CREDITCHECK_SHIFT	8
+#define BRCMF_SKB_IF_FLAGS_IF_AP_MASK		0x0080
+#define BRCMF_SKB_IF_FLAGS_IF_AP_SHIFT		7
+#define BRCMF_SKB_IF_FLAGS_FIFO_MASK		0x0070
+#define BRCMF_SKB_IF_FLAGS_FIFO_SHIFT		4
+#define BRCMF_SKB_IF_FLAGS_INDEX_MASK		0x000f
+#define BRCMF_SKB_IF_FLAGS_INDEX_SHIFT		0
+
+#define brcmf_skb_if_flags_set_field(skb, field, value) \
+	brcmu_maskset16(&(brcmf_skbcb(skb)->if_flags), \
+			BRCMF_SKB_IF_FLAGS_ ## field ## _MASK, \
+			BRCMF_SKB_IF_FLAGS_ ## field ## _SHIFT, (value))
+#define brcmf_skb_if_flags_get_field(skb, field) \
+	brcmu_maskget16(brcmf_skbcb(skb)->if_flags, \
+			BRCMF_SKB_IF_FLAGS_ ## field ## _MASK, \
+			BRCMF_SKB_IF_FLAGS_ ## field ## _SHIFT)
+
+/*
+ * sk_buff control packet identifier
+ *
+ * 32-bit packet identifier used in PKTTAG tlv from host to dongle.
+ *
+ * - Generated at the host (e.g. dhd)
+ * - Seen as a generic sequence number by firmware except for the flags field.
+ *
+ * Generation	: b[31]	=> generation number for this packet [host->fw]
+ *			   OR, current generation number [fw->host]
+ * Flags	: b[30:27] => command, status flags
+ * FIFO-AC	: b[26:24] => AC-FIFO id
+ * h-slot	: b[23:8] => hanger-slot
+ * freerun	: b[7:0] => A free running counter
+ */
+#define BRCMF_SKB_HTOD_TAG_GENERATION_MASK		0x80000000
+#define BRCMF_SKB_HTOD_TAG_GENERATION_SHIFT		31
+#define BRCMF_SKB_HTOD_TAG_FLAGS_MASK			0x78000000
+#define BRCMF_SKB_HTOD_TAG_FLAGS_SHIFT			27
+#define BRCMF_SKB_HTOD_TAG_FIFO_MASK			0x07000000
+#define BRCMF_SKB_HTOD_TAG_FIFO_SHIFT			24
+#define BRCMF_SKB_HTOD_TAG_HSLOT_MASK			0x00ffff00
+#define BRCMF_SKB_HTOD_TAG_HSLOT_SHIFT			8
+#define BRCMF_SKB_HTOD_TAG_FREERUN_MASK			0x000000ff
+#define BRCMF_SKB_HTOD_TAG_FREERUN_SHIFT			0
+
+#define brcmf_skb_htod_tag_set_field(skb, field, value) \
+	brcmu_maskset32(&(brcmf_skbcb(skb)->htod), \
+			BRCMF_SKB_HTOD_TAG_ ## field ## _MASK, \
+			BRCMF_SKB_HTOD_TAG_ ## field ## _SHIFT, (value))
+#define brcmf_skb_htod_tag_get_field(skb, field) \
+	brcmu_maskget32(brcmf_skbcb(skb)->htod, \
+			BRCMF_SKB_HTOD_TAG_ ## field ## _MASK, \
+			BRCMF_SKB_HTOD_TAG_ ## field ## _SHIFT)
+
+#define BRCMF_FWS_TXSTAT_GENERATION_MASK	0x80000000
+#define BRCMF_FWS_TXSTAT_GENERATION_SHIFT	31
+#define BRCMF_FWS_TXSTAT_FLAGS_MASK		0x78000000
+#define BRCMF_FWS_TXSTAT_FLAGS_SHIFT		27
+#define BRCMF_FWS_TXSTAT_FIFO_MASK		0x07000000
+#define BRCMF_FWS_TXSTAT_FIFO_SHIFT		24
+#define BRCMF_FWS_TXSTAT_HSLOT_MASK		0x00FFFF00
+#define BRCMF_FWS_TXSTAT_HSLOT_SHIFT		8
+#define BRCMF_FWS_TXSTAT_PKTID_MASK		0x00FFFFFF
+#define BRCMF_FWS_TXSTAT_PKTID_SHIFT		0
+
+#define brcmf_txstatus_get_field(txs, field) \
+	brcmu_maskget32(txs, BRCMF_FWS_TXSTAT_ ## field ## _MASK, \
+			BRCMF_FWS_TXSTAT_ ## field ## _SHIFT)
+
+/* How long to defer borrowing in jiffies */
+#define BRCMF_FWS_BORROW_DEFER_PERIOD		(HZ / 10)
+
+/**
+ * enum brcmf_fws_fifo - fifo indices used by dongle firmware.
+ *
+ * @BRCMF_FWS_FIFO_AC_BK: fifo for background traffic.
+ * @BRCMF_FWS_FIFO_AC_BE: fifo for best-effort traffic.
+ * @BRCMF_FWS_FIFO_AC_VI: fifo for video traffic.
+ * @BRCMF_FWS_FIFO_AC_VO: fifo for voice traffic.
+ * @BRCMF_FWS_FIFO_BCMC: fifo for broadcast/multicast (AP only).
+ * @BRCMF_FWS_FIFO_ATIM: fifo for ATIM (AP only).
+ * @BRCMF_FWS_FIFO_COUNT: number of fifos.
+ */
+enum brcmf_fws_fifo {
+	BRCMF_FWS_FIFO_AC_BK,
+	BRCMF_FWS_FIFO_AC_BE,
+	BRCMF_FWS_FIFO_AC_VI,
+	BRCMF_FWS_FIFO_AC_VO,
+	BRCMF_FWS_FIFO_BCMC,
+	BRCMF_FWS_FIFO_ATIM,
+	BRCMF_FWS_FIFO_COUNT
+};
+
+/**
+ * enum brcmf_fws_txstatus - txstatus flag values.
+ *
+ * @BRCMF_FWS_TXSTATUS_DISCARD:
+ *	host is free to discard the packet.
+ * @BRCMF_FWS_TXSTATUS_CORE_SUPPRESS:
+ *	802.11 core suppressed the packet.
+ * @BRCMF_FWS_TXSTATUS_FW_PS_SUPPRESS:
+ *	firmware suppress the packet as device is already in PS mode.
+ * @BRCMF_FWS_TXSTATUS_FW_TOSSED:
+ *	firmware tossed the packet.
+ */
+enum brcmf_fws_txstatus {
+	BRCMF_FWS_TXSTATUS_DISCARD,
+	BRCMF_FWS_TXSTATUS_CORE_SUPPRESS,
+	BRCMF_FWS_TXSTATUS_FW_PS_SUPPRESS,
+	BRCMF_FWS_TXSTATUS_FW_TOSSED
+};
+
+enum brcmf_fws_fcmode {
+	BRCMF_FWS_FCMODE_NONE,
+	BRCMF_FWS_FCMODE_IMPLIED_CREDIT,
+	BRCMF_FWS_FCMODE_EXPLICIT_CREDIT
+};
+
+enum brcmf_fws_mac_desc_state {
+	BRCMF_FWS_STATE_OPEN = 1,
+	BRCMF_FWS_STATE_CLOSE
+};
+
+/**
+ * struct brcmf_fws_mac_descriptor - firmware signalling data per node/interface
+ *
+ * @occupied: slot is in use.
+ * @mac_handle: handle for mac entry determined by firmware.
+ * @interface_id: interface index.
+ * @state: current state.
+ * @suppressed: mac entry is suppressed.
+ * @generation: generation bit.
+ * @ac_bitmap: ac queue bitmap.
+ * @requested_credit: credits requested by firmware.
+ * @ea: ethernet address.
+ * @seq: per-node free-running sequence.
+ * @psq: power-save queue.
+ * @transit_count: packet in transit to firmware.
+ */
+struct brcmf_fws_mac_descriptor {
+	u8 occupied;
+	u8 mac_handle;
+	u8 interface_id;
+	u8 state;
+	bool suppressed;
+	u8 generation;
+	u8 ac_bitmap;
+	u8 requested_credit;
+	u8 requested_packet;
+	u8 ea[ETH_ALEN];
+	u8 seq[BRCMF_FWS_FIFO_COUNT];
+	struct pktq psq;
+	int transit_count;
+	int suppress_count;
+	int suppr_transit_count;
+	bool send_tim_signal;
+	u8 traffic_pending_bmp;
+	u8 traffic_lastreported_bmp;
+};
+
+#define BRCMF_FWS_HANGER_MAXITEMS	1024
+
+/**
+ * enum brcmf_fws_hanger_item_state - state of hanger item.
+ *
+ * @BRCMF_FWS_HANGER_ITEM_STATE_FREE: item is free for use.
+ * @BRCMF_FWS_HANGER_ITEM_STATE_INUSE: item is in use.
+ * @BRCMF_FWS_HANGER_ITEM_STATE_INUSE_SUPPRESSED: item was suppressed.
+ */
+enum brcmf_fws_hanger_item_state {
+	BRCMF_FWS_HANGER_ITEM_STATE_FREE = 1,
+	BRCMF_FWS_HANGER_ITEM_STATE_INUSE,
+	BRCMF_FWS_HANGER_ITEM_STATE_INUSE_SUPPRESSED
+};
+
+
+/**
+ * struct brcmf_fws_hanger_item - single entry for tx pending packet.
+ *
+ * @state: entry is either free or occupied.
+ * @gen: generation.
+ * @pkt: packet itself.
+ */
+struct brcmf_fws_hanger_item {
+	enum brcmf_fws_hanger_item_state state;
+	u8 gen;
+	struct sk_buff *pkt;
+};
+
+/**
+ * struct brcmf_fws_hanger - holds packets awaiting firmware txstatus.
+ *
+ * @pushed: packets pushed to await txstatus.
+ * @popped: packets popped upon handling txstatus.
+ * @failed_to_push: packets that could not be pushed.
+ * @failed_to_pop: packets that could not be popped.
+ * @failed_slotfind: packets for which failed to find an entry.
+ * @slot_pos: last returned item index for a free entry.
+ * @items: array of hanger items.
+ */
+struct brcmf_fws_hanger {
+	u32 pushed;
+	u32 popped;
+	u32 failed_to_push;
+	u32 failed_to_pop;
+	u32 failed_slotfind;
+	u32 slot_pos;
+	struct brcmf_fws_hanger_item items[BRCMF_FWS_HANGER_MAXITEMS];
+};
+
+struct brcmf_fws_macdesc_table {
+	struct brcmf_fws_mac_descriptor nodes[BRCMF_FWS_MAC_DESC_TABLE_SIZE];
+	struct brcmf_fws_mac_descriptor iface[BRCMF_MAX_IFS];
+	struct brcmf_fws_mac_descriptor other;
+};
+
+struct brcmf_fws_info {
+	struct brcmf_pub *drvr;
+	struct brcmf_fws_stats stats;
+	struct brcmf_fws_hanger hanger;
+	enum brcmf_fws_fcmode fcmode;
+	struct brcmf_fws_macdesc_table desc;
+	struct workqueue_struct *fws_wq;
+	struct work_struct fws_dequeue_work;
+	u32 fifo_enqpkt[BRCMF_FWS_FIFO_COUNT];
+	int fifo_credit[BRCMF_FWS_FIFO_COUNT];
+	int credits_borrowed[BRCMF_FWS_FIFO_AC_VO + 1];
+	int deq_node_pos[BRCMF_FWS_FIFO_COUNT];
+	u32 fifo_credit_map;
+	u32 fifo_delay_map;
+	unsigned long borrow_defer_timestamp;
+};
+
+/*
+ * brcmf_fws_prio2fifo - mapping from 802.1d priority to firmware fifo index.
+ */
+static const int brcmf_fws_prio2fifo[] = {
+	BRCMF_FWS_FIFO_AC_BE,
+	BRCMF_FWS_FIFO_AC_BK,
+	BRCMF_FWS_FIFO_AC_BK,
+	BRCMF_FWS_FIFO_AC_BE,
+	BRCMF_FWS_FIFO_AC_VI,
+	BRCMF_FWS_FIFO_AC_VI,
+	BRCMF_FWS_FIFO_AC_VO,
+	BRCMF_FWS_FIFO_AC_VO
+};
+
+static int fcmode;
+module_param(fcmode, int, S_IRUSR);
+MODULE_PARM_DESC(fcmode, "mode of firmware signalled flow control");
+
+#define BRCMF_FWS_TLV_DEF(name, id, len) \
+	case BRCMF_FWS_TYPE_ ## name: \
+		return len;
+
+/**
+ * brcmf_fws_get_tlv_len() - returns defined length for given tlv id.
+ *
+ * @fws: firmware-signalling information.
+ * @id: identifier of the TLV.
+ *
+ * Return: the specified length for the given TLV; Otherwise -EINVAL.
+ */
+static int brcmf_fws_get_tlv_len(struct brcmf_fws_info *fws,
+				 enum brcmf_fws_tlv_type id)
+{
+	switch (id) {
+	BRCMF_FWS_TLV_DEFLIST
+	default:
+		fws->stats.tlv_invalid_type++;
+		break;
+	}
+	return -EINVAL;
+}
+#undef BRCMF_FWS_TLV_DEF
+
+static bool brcmf_fws_ifidx_match(struct sk_buff *skb, void *arg)
+{
+	u32 ifidx = brcmf_skb_if_flags_get_field(skb, INDEX);
+	return ifidx == *(int *)arg;
+}
+
+static void brcmf_fws_psq_flush(struct brcmf_fws_info *fws, struct pktq *q,
+				int ifidx)
+{
+	bool (*matchfn)(struct sk_buff *, void *) = NULL;
+	struct sk_buff *skb;
+	int prec;
+
+	if (ifidx != -1)
+		matchfn = brcmf_fws_ifidx_match;
+	for (prec = 0; prec < q->num_prec; prec++) {
+		skb = brcmu_pktq_pdeq_match(q, prec, matchfn, &ifidx);
+		while (skb) {
+			brcmu_pkt_buf_free_skb(skb);
+			skb = brcmu_pktq_pdeq_match(q, prec, matchfn, &ifidx);
+		}
+	}
+}
+
+static void brcmf_fws_hanger_init(struct brcmf_fws_hanger *hanger)
+{
+	int i;
+
+	brcmf_dbg(TRACE, "enter\n");
+	memset(hanger, 0, sizeof(*hanger));
+	for (i = 0; i < ARRAY_SIZE(hanger->items); i++)
+		hanger->items[i].state = BRCMF_FWS_HANGER_ITEM_STATE_FREE;
+}
+
+static u32 brcmf_fws_hanger_get_free_slot(struct brcmf_fws_hanger *h)
+{
+	u32 i;
+
+	brcmf_dbg(TRACE, "enter\n");
+	i = (h->slot_pos + 1) % BRCMF_FWS_HANGER_MAXITEMS;
+
+	while (i != h->slot_pos) {
+		if (h->items[i].state == BRCMF_FWS_HANGER_ITEM_STATE_FREE) {
+			h->slot_pos = i;
+			goto done;
+		}
+		i++;
+		if (i == BRCMF_FWS_HANGER_MAXITEMS)
+			i = 0;
+	}
+	brcmf_err("all slots occupied\n");
+	h->failed_slotfind++;
+	i = BRCMF_FWS_HANGER_MAXITEMS;
+done:
+	brcmf_dbg(TRACE, "exit: %d\n", i);
+	return i;
+}
+
+static int brcmf_fws_hanger_pushpkt(struct brcmf_fws_hanger *h,
+					   struct sk_buff *pkt, u32 slot_id)
+{
+	brcmf_dbg(TRACE, "enter\n");
+	if (slot_id >= BRCMF_FWS_HANGER_MAXITEMS)
+		return -ENOENT;
+
+	if (h->items[slot_id].state != BRCMF_FWS_HANGER_ITEM_STATE_FREE) {
+		brcmf_err("slot is not free\n");
+		h->failed_to_push++;
+		return -EINVAL;
+	}
+
+	h->items[slot_id].state = BRCMF_FWS_HANGER_ITEM_STATE_INUSE;
+	h->items[slot_id].pkt = pkt;
+	h->pushed++;
+	return 0;
+}
+
+static int brcmf_fws_hanger_poppkt(struct brcmf_fws_hanger *h,
+					  u32 slot_id, struct sk_buff **pktout,
+					  bool remove_item)
+{
+	brcmf_dbg(TRACE, "enter\n");
+	if (slot_id >= BRCMF_FWS_HANGER_MAXITEMS)
+		return -ENOENT;
+
+	if (h->items[slot_id].state == BRCMF_FWS_HANGER_ITEM_STATE_FREE) {
+		brcmf_err("entry not in use\n");
+		h->failed_to_pop++;
+		return -EINVAL;
+	}
+
+	*pktout = h->items[slot_id].pkt;
+	if (remove_item) {
+		h->items[slot_id].state = BRCMF_FWS_HANGER_ITEM_STATE_FREE;
+		h->items[slot_id].pkt = NULL;
+		h->items[slot_id].gen = 0xff;
+		h->popped++;
+	}
+	return 0;
+}
+
+static int brcmf_fws_hanger_mark_suppressed(struct brcmf_fws_hanger *h,
+						   u32 slot_id, u8 gen)
+{
+	brcmf_dbg(TRACE, "enter\n");
+
+	if (slot_id >= BRCMF_FWS_HANGER_MAXITEMS)
+		return -ENOENT;
+
+	h->items[slot_id].gen = gen;
+
+	if (h->items[slot_id].state != BRCMF_FWS_HANGER_ITEM_STATE_INUSE) {
+		brcmf_err("entry not in use\n");
+		return -EINVAL;
+	}
+
+	h->items[slot_id].state = BRCMF_FWS_HANGER_ITEM_STATE_INUSE_SUPPRESSED;
+	return 0;
+}
+
+static int brcmf_fws_hanger_get_genbit(struct brcmf_fws_hanger *hanger,
+					      struct sk_buff *pkt, u32 slot_id,
+					      int *gen)
+{
+	brcmf_dbg(TRACE, "enter\n");
+	*gen = 0xff;
+
+	if (slot_id >= BRCMF_FWS_HANGER_MAXITEMS)
+		return -ENOENT;
+
+	if (hanger->items[slot_id].state == BRCMF_FWS_HANGER_ITEM_STATE_FREE) {
+		brcmf_err("slot not in use\n");
+		return -EINVAL;
+	}
+
+	*gen = hanger->items[slot_id].gen;
+	return 0;
+}
+
+static void brcmf_fws_hanger_cleanup(struct brcmf_fws_info *fws,
+				     bool (*fn)(struct sk_buff *, void *),
+				     int ifidx)
+{
+	struct brcmf_fws_hanger *h = &fws->hanger;
+	struct sk_buff *skb;
+	int i;
+	enum brcmf_fws_hanger_item_state s;
+
+	brcmf_dbg(TRACE, "enter: ifidx=%d\n", ifidx);
+	for (i = 0; i < ARRAY_SIZE(h->items); i++) {
+		s = h->items[i].state;
+		if (s == BRCMF_FWS_HANGER_ITEM_STATE_INUSE ||
+		    s == BRCMF_FWS_HANGER_ITEM_STATE_INUSE_SUPPRESSED) {
+			skb = h->items[i].pkt;
+			if (fn == NULL || fn(skb, &ifidx)) {
+				/* suppress packets freed from psq */
+				if (s == BRCMF_FWS_HANGER_ITEM_STATE_INUSE)
+					brcmu_pkt_buf_free_skb(skb);
+				h->items[i].state =
+					BRCMF_FWS_HANGER_ITEM_STATE_FREE;
+			}
+		}
+	}
+}
+
+static void brcmf_fws_init_mac_descriptor(struct brcmf_fws_mac_descriptor *desc,
+					  u8 *addr, u8 ifidx)
+{
+	brcmf_dbg(TRACE,
+		  "enter: desc %p ea=%pM, ifidx=%u\n", desc, addr, ifidx);
+	desc->occupied = 1;
+	desc->state = BRCMF_FWS_STATE_OPEN;
+	desc->requested_credit = 0;
+	/* depending on use may need ifp->bssidx instead */
+	desc->interface_id = ifidx;
+	desc->ac_bitmap = 0xff; /* update this when handling APSD */
+	if (addr)
+		memcpy(&desc->ea[0], addr, ETH_ALEN);
+}
+
+static
+void brcmf_fws_clear_mac_descriptor(struct brcmf_fws_mac_descriptor *desc)
+{
+	brcmf_dbg(TRACE,
+		  "enter: ea=%pM, ifidx=%u\n", desc->ea, desc->interface_id);
+	desc->occupied = 0;
+	desc->state = BRCMF_FWS_STATE_CLOSE;
+	desc->requested_credit = 0;
+}
+
+static struct brcmf_fws_mac_descriptor *
+brcmf_fws_mac_descriptor_lookup(struct brcmf_fws_info *fws, u8 *ea)
+{
+	struct brcmf_fws_mac_descriptor *entry;
+	int i;
+
+	brcmf_dbg(TRACE, "enter: ea=%pM\n", ea);
+	if (ea == NULL)
+		return ERR_PTR(-EINVAL);
+
+	entry = &fws->desc.nodes[0];
+	for (i = 0; i < ARRAY_SIZE(fws->desc.nodes); i++) {
+		if (entry->occupied && !memcmp(entry->ea, ea, ETH_ALEN))
+			return entry;
+		entry++;
+	}
+
+	return ERR_PTR(-ENOENT);
+}
+
+static struct brcmf_fws_mac_descriptor*
+brcmf_fws_find_mac_desc(struct brcmf_fws_info *fws, struct brcmf_if *ifp,
+			u8 *da)
+{
+	struct brcmf_fws_mac_descriptor *entry = &fws->desc.other;
+	bool multicast;
+	enum nl80211_iftype iftype;
+
+	brcmf_dbg(TRACE, "enter: idx=%d\n", ifp->bssidx);
+
+	multicast = is_multicast_ether_addr(da);
+	iftype = brcmf_cfg80211_get_iftype(ifp);
+
+	/* Multicast destination and P2P clients get the interface entry.
+	 * STA gets the interface entry if there is no exact match. For
+	 * example, TDLS destinations have their own entry.
+	 */
+	entry = NULL;
+	if ((multicast || iftype == NL80211_IFTYPE_STATION ||
+	     iftype == NL80211_IFTYPE_P2P_CLIENT) && ifp->fws_desc)
+		entry = ifp->fws_desc;
+
+	if (entry != NULL && iftype != NL80211_IFTYPE_STATION)
+		goto done;
+
+	entry = brcmf_fws_mac_descriptor_lookup(fws, da);
+	if (IS_ERR(entry))
+		entry = &fws->desc.other;
+
+done:
+	brcmf_dbg(TRACE, "exit: entry=%p\n", entry);
+	return entry;
+}
+
+static bool brcmf_fws_mac_desc_closed(struct brcmf_fws_info *fws,
+				      struct brcmf_fws_mac_descriptor *entry,
+				      int fifo)
+{
+	struct brcmf_fws_mac_descriptor *if_entry;
+	bool closed;
+
+	/* for unique destination entries the related interface
+	 * may be closed.
+	 */
+	if (entry->mac_handle) {
+		if_entry = &fws->desc.iface[entry->interface_id];
+		if (if_entry->state == BRCMF_FWS_STATE_CLOSE)
+			return true;
+	}
+	/* an entry is closed when the state is closed and
+	 * the firmware did not request anything.
+	 */
+	closed = entry->state == BRCMF_FWS_STATE_CLOSE &&
+		 !entry->requested_credit && !entry->requested_packet;
+
+	/* Or firmware does not allow traffic for given fifo */
+	return closed || !(entry->ac_bitmap & BIT(fifo));
+}
+
+static void brcmf_fws_mac_desc_cleanup(struct brcmf_fws_info *fws,
+				       struct brcmf_fws_mac_descriptor *entry,
+				       int ifidx)
+{
+	brcmf_dbg(TRACE, "enter: entry=(ea=%pM, ifid=%d), ifidx=%d\n",
+		  entry->ea, entry->interface_id, ifidx);
+	if (entry->occupied && (ifidx == -1 || ifidx == entry->interface_id)) {
+		brcmf_dbg(TRACE, "flush psq: ifidx=%d, qlen=%d\n",
+			  ifidx, entry->psq.len);
+		brcmf_fws_psq_flush(fws, &entry->psq, ifidx);
+		entry->occupied = !!(entry->psq.len);
+	}
+}
+
+static void brcmf_fws_bus_txq_cleanup(struct brcmf_fws_info *fws,
+				      bool (*fn)(struct sk_buff *, void *),
+				      int ifidx)
+{
+	struct brcmf_fws_hanger_item *hi;
+	struct pktq *txq;
+	struct sk_buff *skb;
+	int prec;
+	u32 hslot;
+
+	brcmf_dbg(TRACE, "enter: ifidx=%d\n", ifidx);
+	txq = brcmf_bus_gettxq(fws->drvr->bus_if);
+	if (IS_ERR(txq)) {
+		brcmf_dbg(TRACE, "no txq to clean up\n");
+		return;
+	}
+
+	for (prec = 0; prec < txq->num_prec; prec++) {
+		skb = brcmu_pktq_pdeq_match(txq, prec, fn, &ifidx);
+		while (skb) {
+			hslot = brcmf_skb_htod_tag_get_field(skb, HSLOT);
+			hi = &fws->hanger.items[hslot];
+			WARN_ON(skb != hi->pkt);
+			hi->state = BRCMF_FWS_HANGER_ITEM_STATE_FREE;
+			brcmu_pkt_buf_free_skb(skb);
+			skb = brcmu_pktq_pdeq_match(txq, prec, fn, &ifidx);
+		}
+	}
+}
+
+static void brcmf_fws_cleanup(struct brcmf_fws_info *fws, int ifidx)
+{
+	int i;
+	struct brcmf_fws_mac_descriptor *table;
+	bool (*matchfn)(struct sk_buff *, void *) = NULL;
+
+	brcmf_dbg(TRACE, "enter: ifidx=%d\n", ifidx);
+	if (fws == NULL)
+		return;
+
+	if (ifidx != -1)
+		matchfn = brcmf_fws_ifidx_match;
+
+	/* cleanup individual nodes */
+	table = &fws->desc.nodes[0];
+	for (i = 0; i < ARRAY_SIZE(fws->desc.nodes); i++)
+		brcmf_fws_mac_desc_cleanup(fws, &table[i], ifidx);
+
+	brcmf_fws_mac_desc_cleanup(fws, &fws->desc.other, ifidx);
+	brcmf_fws_bus_txq_cleanup(fws, matchfn, ifidx);
+	brcmf_fws_hanger_cleanup(fws, matchfn, ifidx);
+}
+
+static void brcmf_fws_tim_update(struct brcmf_fws_info *ctx,
+				 struct brcmf_fws_mac_descriptor *entry,
+				 int prec)
+{
+	brcmf_dbg(TRACE, "enter: ea=%pM\n", entry->ea);
+	if (entry->state == BRCMF_FWS_STATE_CLOSE) {
+		/* check delayedQ and suppressQ in one call using bitmap */
+		if (brcmu_pktq_mlen(&entry->psq, 3 << (prec * 2)) == 0)
+			entry->traffic_pending_bmp =
+				entry->traffic_pending_bmp & ~NBITVAL(prec);
+		else
+			entry->traffic_pending_bmp =
+				entry->traffic_pending_bmp | NBITVAL(prec);
+	}
+	/* request a TIM update to firmware at the next piggyback opportunity */
+	if (entry->traffic_lastreported_bmp != entry->traffic_pending_bmp)
+		entry->send_tim_signal = true;
+}
+
+static void
+brcmf_fws_flow_control_check(struct brcmf_fws_info *fws, struct pktq *pq,
+			     u8 if_id)
+{
+	struct brcmf_if *ifp = fws->drvr->iflist[if_id];
+
+	if (WARN_ON(!ifp))
+		return;
+
+	brcmf_dbg(TRACE,
+		  "enter: bssidx=%d, ifidx=%d\n", ifp->bssidx, ifp->ifidx);
+
+	if ((ifp->netif_stop & BRCMF_NETIF_STOP_REASON_FWS_FC) &&
+	    pq->len <= BRCMF_FWS_FLOWCONTROL_LOWATER)
+		brcmf_txflowblock_if(ifp,
+				     BRCMF_NETIF_STOP_REASON_FWS_FC, false);
+	if (!(ifp->netif_stop & BRCMF_NETIF_STOP_REASON_FWS_FC) &&
+	    pq->len >= BRCMF_FWS_FLOWCONTROL_HIWATER)
+		brcmf_txflowblock_if(ifp, BRCMF_NETIF_STOP_REASON_FWS_FC, true);
+	return;
+}
+
+static int brcmf_fws_rssi_indicate(struct brcmf_fws_info *fws, s8 rssi)
+{
+	brcmf_dbg(CTL, "rssi %d\n", rssi);
+	return 0;
+}
+
+static
+int brcmf_fws_macdesc_indicate(struct brcmf_fws_info *fws, u8 type, u8 *data)
+{
+	struct brcmf_fws_mac_descriptor *entry, *existing;
+	u8 mac_handle;
+	u8 ifidx;
+	u8 *addr;
+
+	mac_handle = *data++;
+	ifidx = *data++;
+	addr = data;
+
+	entry = &fws->desc.nodes[mac_handle & 0x1F];
+	if (type == BRCMF_FWS_TYPE_MACDESC_DEL) {
+		brcmf_dbg(TRACE, "deleting mac %pM idx %d\n", addr, ifidx);
+		if (entry->occupied) {
+			brcmf_fws_mac_desc_cleanup(fws, entry, -1);
+			brcmf_fws_clear_mac_descriptor(entry);
+		} else
+			fws->stats.mac_update_failed++;
+		return 0;
+	}
+
+	brcmf_dbg(TRACE,
+		  "add mac %pM handle %u idx %d\n", addr, mac_handle, ifidx);
+	existing = brcmf_fws_mac_descriptor_lookup(fws, addr);
+	if (IS_ERR(existing)) {
+		if (!entry->occupied) {
+			entry->mac_handle = mac_handle;
+			brcmf_fws_init_mac_descriptor(entry, addr, ifidx);
+			brcmu_pktq_init(&entry->psq, BRCMF_FWS_PSQ_PREC_COUNT,
+					BRCMF_FWS_PSQ_LEN);
+		} else {
+			fws->stats.mac_update_failed++;
+		}
+	} else {
+		if (entry != existing) {
+			brcmf_dbg(TRACE, "relocate mac\n");
+			memcpy(entry, existing,
+			       offsetof(struct brcmf_fws_mac_descriptor, psq));
+			entry->mac_handle = mac_handle;
+			brcmf_fws_clear_mac_descriptor(existing);
+		} else {
+			brcmf_dbg(TRACE, "use existing\n");
+			WARN_ON(entry->mac_handle != mac_handle);
+			/* TODO: what should we do here: continue, reinit, .. */
+		}
+	}
+	return 0;
+}
+
+static int brcmf_fws_macdesc_state_indicate(struct brcmf_fws_info *fws,
+					    u8 type, u8 *data)
+{
+	struct brcmf_fws_mac_descriptor *entry;
+	u8 mac_handle;
+	int i;
+
+	mac_handle = data[0];
+	entry = &fws->desc.nodes[mac_handle & 0x1F];
+	if (!entry->occupied) {
+		fws->stats.mac_ps_update_failed++;
+		return -ESRCH;
+	}
+
+	/* a state update should wipe old credits? */
+	entry->requested_credit = 0;
+	if (type == BRCMF_FWS_TYPE_MAC_OPEN) {
+		entry->state = BRCMF_FWS_STATE_OPEN;
+		return BRCMF_FWS_RET_OK_SCHEDULE;
+	} else {
+		entry->state = BRCMF_FWS_STATE_CLOSE;
+		for (i = BRCMF_FWS_FIFO_AC_BE; i < NL80211_NUM_ACS; i++)
+			brcmf_fws_tim_update(fws, entry, i);
+	}
+	return BRCMF_FWS_RET_OK_NOSCHEDULE;
+}
+
+static int brcmf_fws_interface_state_indicate(struct brcmf_fws_info *fws,
+					      u8 type, u8 *data)
+{
+	struct brcmf_fws_mac_descriptor *entry;
+	u8 ifidx;
+	int ret;
+
+	ifidx = data[0];
+
+	brcmf_dbg(TRACE, "enter: ifidx=%d\n", ifidx);
+	if (ifidx >= BRCMF_MAX_IFS) {
+		ret = -ERANGE;
+		goto fail;
+	}
+
+	entry = &fws->desc.iface[ifidx];
+	if (!entry->occupied) {
+		ret = -ESRCH;
+		goto fail;
+	}
+
+	switch (type) {
+	case BRCMF_FWS_TYPE_INTERFACE_OPEN:
+		entry->state = BRCMF_FWS_STATE_OPEN;
+		return BRCMF_FWS_RET_OK_SCHEDULE;
+	case BRCMF_FWS_TYPE_INTERFACE_CLOSE:
+		entry->state = BRCMF_FWS_STATE_CLOSE;
+		return BRCMF_FWS_RET_OK_NOSCHEDULE;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+fail:
+	fws->stats.if_update_failed++;
+	return ret;
+}
+
+static int brcmf_fws_request_indicate(struct brcmf_fws_info *fws, u8 type,
+				      u8 *data)
+{
+	struct brcmf_fws_mac_descriptor *entry;
+
+	entry = &fws->desc.nodes[data[1] & 0x1F];
+	if (!entry->occupied) {
+		if (type == BRCMF_FWS_TYPE_MAC_REQUEST_CREDIT)
+			fws->stats.credit_request_failed++;
+		else
+			fws->stats.packet_request_failed++;
+		return -ESRCH;
+	}
+
+	if (type == BRCMF_FWS_TYPE_MAC_REQUEST_CREDIT)
+		entry->requested_credit = data[0];
+	else
+		entry->requested_packet = data[0];
+
+	entry->ac_bitmap = data[2];
+	return BRCMF_FWS_RET_OK_SCHEDULE;
+}
+
+static void brcmf_fws_return_credits(struct brcmf_fws_info *fws,
+				     u8 fifo, u8 credits)
+{
+	int lender_ac;
+	int *borrowed;
+	int *fifo_credit;
+
+	if (!credits)
+		return;
+
+	if ((fifo == BRCMF_FWS_FIFO_AC_BE) &&
+	    (fws->credits_borrowed[0])) {
+		for (lender_ac = BRCMF_FWS_FIFO_AC_VO; lender_ac >= 0;
+		     lender_ac--) {
+			borrowed = &fws->credits_borrowed[lender_ac];
+			if (*borrowed) {
+				fws->fifo_credit_map |= (1 << lender_ac);
+				fifo_credit = &fws->fifo_credit[lender_ac];
+				if (*borrowed >= credits) {
+					*borrowed -= credits;
+					*fifo_credit += credits;
+					return;
+				} else {
+					credits -= *borrowed;
+					*fifo_credit += *borrowed;
+					*borrowed = 0;
+				}
+			}
+		}
+	}
+
+	fws->fifo_credit_map |= 1 << fifo;
+	fws->fifo_credit[fifo] += credits;
+}
+
+static void brcmf_fws_schedule_deq(struct brcmf_fws_info *fws)
+{
+	/* only schedule dequeue when there are credits for delayed traffic */
+	if (fws->fifo_credit_map & fws->fifo_delay_map)
+		queue_work(fws->fws_wq, &fws->fws_dequeue_work);
+}
+
+static void brcmf_skb_pick_up_credit(struct brcmf_fws_info *fws, int fifo,
+				     struct sk_buff *p)
+{
+	struct brcmf_fws_mac_descriptor *entry = brcmf_skbcb(p)->mac;
+
+	if (brcmf_skbcb(p)->if_flags & BRCMF_SKB_IF_FLAGS_CREDITCHECK_MASK) {
+		if (fws->fcmode != BRCMF_FWS_FCMODE_IMPLIED_CREDIT)
+			return;
+		brcmf_fws_return_credits(fws, fifo, 1);
+	} else {
+		/*
+		 * if this packet did not count against FIFO credit, it
+		 * must have taken a requested_credit from the destination
+		 * entry (for pspoll etc.)
+		 */
+		if (!brcmf_skb_if_flags_get_field(p, REQUESTED))
+			entry->requested_credit++;
+	}
+	brcmf_fws_schedule_deq(fws);
+}
+
+static int brcmf_fws_enq(struct brcmf_fws_info *fws,
+			 enum brcmf_fws_skb_state state, int fifo,
+			 struct sk_buff *p)
+{
+	int prec = 2 * fifo;
+	u32 *qfull_stat = &fws->stats.delayq_full_error;
+
+	struct brcmf_fws_mac_descriptor *entry;
+
+	entry = brcmf_skbcb(p)->mac;
+	if (entry == NULL) {
+		brcmf_err("no mac descriptor found for skb %p\n", p);
+		return -ENOENT;
+	}
+
+	brcmf_dbg(TRACE, "enter: ea=%pM, qlen=%d\n", entry->ea, entry->psq.len);
+	if (state == BRCMF_FWS_SKBSTATE_SUPPRESSED) {
+		prec += 1;
+		qfull_stat = &fws->stats.supprq_full_error;
+	}
+
+	if (brcmu_pktq_penq(&entry->psq, prec, p) == NULL) {
+		*qfull_stat += 1;
+		return -ENFILE;
+	}
+
+	/* increment total enqueued packet count */
+	fws->fifo_delay_map |= 1 << fifo;
+	fws->fifo_enqpkt[fifo]++;
+
+	/* update the sk_buff state */
+	brcmf_skbcb(p)->state = state;
+	if (state == BRCMF_FWS_SKBSTATE_SUPPRESSED)
+		entry->suppress_count++;
+
+	/*
+	 * A packet has been pushed so update traffic
+	 * availability bitmap, if applicable
+	 */
+	brcmf_fws_tim_update(fws, entry, fifo);
+	brcmf_fws_flow_control_check(fws, &entry->psq,
+				     brcmf_skb_if_flags_get_field(p, INDEX));
+	return 0;
+}
+
+static struct sk_buff *brcmf_fws_deq(struct brcmf_fws_info *fws, int fifo)
+{
+	struct brcmf_fws_mac_descriptor *table;
+	struct brcmf_fws_mac_descriptor *entry;
+	struct sk_buff *p;
+	int use_credit = 1;
+	int num_nodes;
+	int node_pos;
+	int prec_out;
+	int pmsk;
+	int i;
+
+	table = (struct brcmf_fws_mac_descriptor *)&fws->desc;
+	num_nodes = sizeof(fws->desc) / sizeof(struct brcmf_fws_mac_descriptor);
+	node_pos = fws->deq_node_pos[fifo];
+
+	for (i = 0; i < num_nodes; i++) {
+		entry = &table[(node_pos + i) % num_nodes];
+		if (!entry->occupied ||
+		    brcmf_fws_mac_desc_closed(fws, entry, fifo))
+			continue;
+
+		if (entry->suppressed)
+			pmsk = 2;
+		else
+			pmsk = 3;
+		p = brcmu_pktq_mdeq(&entry->psq, pmsk << (fifo * 2), &prec_out);
+		if (p == NULL) {
+			if (entry->suppressed) {
+				if (entry->suppr_transit_count >
+				    entry->suppress_count)
+					return NULL;
+				entry->suppressed = false;
+				p = brcmu_pktq_mdeq(&entry->psq,
+						    1 << (fifo * 2), &prec_out);
+			}
+		}
+		if  (p == NULL)
+			continue;
+
+		/* did the packet come from suppress sub-queue? */
+		if (entry->requested_credit > 0) {
+			entry->requested_credit--;
+			/*
+			 * if the packet was pulled out while destination is in
+			 * closed state but had a non-zero packets requested,
+			 * then this should not count against the FIFO credit.
+			 * That is due to the fact that the firmware will
+			 * most likely hold onto this packet until a suitable
+			 * time later to push it to the appropriate AC FIFO.
+			 */
+			if (entry->state == BRCMF_FWS_STATE_CLOSE)
+				use_credit = 0;
+		} else if (entry->requested_packet > 0) {
+			entry->requested_packet--;
+			brcmf_skb_if_flags_set_field(p, REQUESTED, 1);
+			if (entry->state == BRCMF_FWS_STATE_CLOSE)
+				use_credit = 0;
+		}
+		brcmf_skb_if_flags_set_field(p, CREDITCHECK, use_credit);
+
+		/* move dequeue position to ensure fair round-robin */
+		fws->deq_node_pos[fifo] = (node_pos + i + 1) % num_nodes;
+		brcmf_fws_flow_control_check(fws, &entry->psq,
+					     brcmf_skb_if_flags_get_field(p,
+									  INDEX)
+					     );
+		/*
+		 * A packet has been picked up, update traffic
+		 * availability bitmap, if applicable
+		 */
+		brcmf_fws_tim_update(fws, entry, fifo);
+
+		/*
+		 * decrement total enqueued fifo packets and
+		 * clear delay bitmap if done.
+		 */
+		fws->fifo_enqpkt[fifo]--;
+		if (fws->fifo_enqpkt[fifo] == 0)
+			fws->fifo_delay_map &= ~(1 << fifo);
+		goto done;
+	}
+	p = NULL;
+done:
+	brcmf_dbg(TRACE, "exit: fifo %d skb %p\n", fifo, p);
+	return p;
+}
+
+static int brcmf_fws_txstatus_suppressed(struct brcmf_fws_info *fws, int fifo,
+					 struct sk_buff *skb, u32 genbit)
+{
+	struct brcmf_fws_mac_descriptor *entry = brcmf_skbcb(skb)->mac;
+	u32 hslot;
+	int ret;
+
+	hslot = brcmf_skb_htod_tag_get_field(skb, HSLOT);
+
+	/* this packet was suppressed */
+	if (!entry->suppressed || entry->generation != genbit) {
+		entry->suppressed = true;
+		entry->suppress_count = brcmu_pktq_mlen(&entry->psq,
+							1 << (fifo * 2 + 1));
+		entry->suppr_transit_count = entry->transit_count;
+	}
+
+	entry->generation = genbit;
+
+	ret = brcmf_fws_enq(fws, BRCMF_FWS_SKBSTATE_SUPPRESSED, fifo, skb);
+	if (ret != 0) {
+		/* suppress q is full, drop this packet */
+		brcmf_fws_hanger_poppkt(&fws->hanger, hslot, &skb,
+					true);
+	} else {
+		/*
+		 * Mark suppressed to avoid a double free during
+		 * wlfc cleanup
+		 */
+		brcmf_fws_hanger_mark_suppressed(&fws->hanger, hslot,
+						 genbit);
+		entry->suppress_count++;
+	}
+
+	return ret;
+}
+
+static int
+brcmf_fws_txstatus_process(struct brcmf_fws_info *fws, u8 flags, u32 hslot,
+			   u32 genbit)
+{
+	u32 fifo;
+	int ret;
+	bool remove_from_hanger = true;
+	struct sk_buff *skb;
+	struct brcmf_fws_mac_descriptor *entry = NULL;
+
+	brcmf_dbg(TRACE, "status: flags=0x%X, hslot=%d\n",
+		  flags, hslot);
+
+	if (flags == BRCMF_FWS_TXSTATUS_DISCARD)
+		fws->stats.txs_discard++;
+	else if (flags == BRCMF_FWS_TXSTATUS_CORE_SUPPRESS) {
+		fws->stats.txs_supp_core++;
+		remove_from_hanger = false;
+	} else if (flags == BRCMF_FWS_TXSTATUS_FW_PS_SUPPRESS) {
+		fws->stats.txs_supp_ps++;
+		remove_from_hanger = false;
+	} else if (flags == BRCMF_FWS_TXSTATUS_FW_TOSSED)
+		fws->stats.txs_tossed++;
+	else
+		brcmf_err("unexpected txstatus\n");
+
+	ret = brcmf_fws_hanger_poppkt(&fws->hanger, hslot, &skb,
+				      remove_from_hanger);
+	if (ret != 0) {
+		brcmf_err("no packet in hanger slot: hslot=%d\n", hslot);
+		return ret;
+	}
+
+	entry = brcmf_skbcb(skb)->mac;
+	if (WARN_ON(!entry)) {
+		brcmu_pkt_buf_free_skb(skb);
+		return -EINVAL;
+	}
+
+	/* pick up the implicit credit from this packet */
+	fifo = brcmf_skb_htod_tag_get_field(skb, FIFO);
+	brcmf_skb_pick_up_credit(fws, fifo, skb);
+
+	if (!remove_from_hanger)
+		ret = brcmf_fws_txstatus_suppressed(fws, fifo, skb, genbit);
+
+	if (remove_from_hanger || ret) {
+		entry->transit_count--;
+		if (entry->suppressed)
+			entry->suppr_transit_count--;
+
+		brcmf_txfinalize(fws->drvr, skb, true);
+	}
+	return 0;
+}
+
+static int brcmf_fws_fifocreditback_indicate(struct brcmf_fws_info *fws,
+					     u8 *data)
+{
+	int i;
+
+	if (fws->fcmode != BRCMF_FWS_FCMODE_EXPLICIT_CREDIT) {
+		brcmf_dbg(INFO, "ignored\n");
+		return BRCMF_FWS_RET_OK_NOSCHEDULE;
+	}
+
+	brcmf_dbg(TRACE, "enter: data %pM\n", data);
+	for (i = 0; i < BRCMF_FWS_FIFO_COUNT; i++)
+		brcmf_fws_return_credits(fws, i, data[i]);
+
+	brcmf_dbg(INFO, "map: credit %x delay %x\n", fws->fifo_credit_map,
+		  fws->fifo_delay_map);
+	return BRCMF_FWS_RET_OK_SCHEDULE;
+}
+
+static int brcmf_fws_txstatus_indicate(struct brcmf_fws_info *fws, u8 *data)
+{
+	__le32 status_le;
+	u32 status;
+	u32 hslot;
+	u32 genbit;
+	u8 flags;
+
+	fws->stats.txs_indicate++;
+	memcpy(&status_le, data, sizeof(status_le));
+	status = le32_to_cpu(status_le);
+	flags = brcmf_txstatus_get_field(status, FLAGS);
+	hslot = brcmf_txstatus_get_field(status, HSLOT);
+	genbit = brcmf_txstatus_get_field(status, GENERATION);
+
+	return brcmf_fws_txstatus_process(fws, flags, hslot, genbit);
+}
+
+static int brcmf_fws_dbg_seqnum_check(struct brcmf_fws_info *fws, u8 *data)
+{
+	__le32 timestamp;
+
+	memcpy(&timestamp, &data[2], sizeof(timestamp));
+	brcmf_dbg(INFO, "received: seq %d, timestamp %d\n", data[1],
+		  le32_to_cpu(timestamp));
+	return 0;
+}
+
+/* using macro so sparse checking does not complain
+ * about locking imbalance.
+ */
+#define brcmf_fws_lock(drvr, flags)				\
+do {								\
+	flags = 0;						\
+	spin_lock_irqsave(&((drvr)->fws_spinlock), (flags));	\
+} while (0)
+
+/* using macro so sparse checking does not complain
+ * about locking imbalance.
+ */
+#define brcmf_fws_unlock(drvr, flags) \
+	spin_unlock_irqrestore(&((drvr)->fws_spinlock), (flags))
+
+static int brcmf_fws_notify_credit_map(struct brcmf_if *ifp,
+				       const struct brcmf_event_msg *e,
+				       void *data)
+{
+	struct brcmf_fws_info *fws = ifp->drvr->fws;
+	int i;
+	ulong flags;
+	u8 *credits = data;
+
+	if (e->datalen < BRCMF_FWS_FIFO_COUNT) {
+		brcmf_err("event payload too small (%d)\n", e->datalen);
+		return -EINVAL;
+	}
+
+	brcmf_dbg(TRACE, "enter: credits %pM\n", credits);
+	brcmf_fws_lock(ifp->drvr, flags);
+	for (i = 0; i < ARRAY_SIZE(fws->fifo_credit); i++) {
+		if (*credits)
+			fws->fifo_credit_map |= 1 << i;
+		else
+			fws->fifo_credit_map &= ~(1 << i);
+		fws->fifo_credit[i] = *credits++;
+	}
+	brcmf_fws_schedule_deq(fws);
+	brcmf_fws_unlock(ifp->drvr, flags);
+	return 0;
+}
+
+int brcmf_fws_hdrpull(struct brcmf_pub *drvr, int ifidx, s16 signal_len,
+		      struct sk_buff *skb)
+{
+	struct brcmf_fws_info *fws = drvr->fws;
+	ulong flags;
+	u8 *signal_data;
+	s16 data_len;
+	u8 type;
+	u8 len;
+	u8 *data;
+	s32 status;
+	s32 err;
+
+	brcmf_dbg(TRACE, "enter: ifidx %d, skblen %u, sig %d\n",
+		  ifidx, skb->len, signal_len);
+
+	WARN_ON(signal_len > skb->len);
+
+	/* if flow control disabled, skip to packet data and leave */
+	if (!signal_len || !drvr->fw_signals) {
+		skb_pull(skb, signal_len);
+		return 0;
+	}
+
+	/* lock during tlv parsing */
+	brcmf_fws_lock(drvr, flags);
+
+	fws->stats.header_pulls++;
+	data_len = signal_len;
+	signal_data = skb->data;
+
+	status = BRCMF_FWS_RET_OK_NOSCHEDULE;
+	while (data_len > 0) {
+		/* extract tlv info */
+		type = signal_data[0];
+
+		/* FILLER type is actually not a TLV, but
+		 * a single byte that can be skipped.
+		 */
+		if (type == BRCMF_FWS_TYPE_FILLER) {
+			signal_data += 1;
+			data_len -= 1;
+			continue;
+		}
+		len = signal_data[1];
+		data = signal_data + 2;
+
+		brcmf_dbg(INFO, "tlv type=%d (%s), len=%d, data[0]=%d\n", type,
+			  brcmf_fws_get_tlv_name(type), len, *data);
+
+		/* abort parsing when length invalid */
+		if (data_len < len + 2)
+			break;
+
+		if (len != brcmf_fws_get_tlv_len(fws, type))
+			break;
+
+		err = BRCMF_FWS_RET_OK_NOSCHEDULE;
+		switch (type) {
+		case BRCMF_FWS_TYPE_HOST_REORDER_RXPKTS:
+		case BRCMF_FWS_TYPE_COMP_TXSTATUS:
+			break;
+		case BRCMF_FWS_TYPE_MACDESC_ADD:
+		case BRCMF_FWS_TYPE_MACDESC_DEL:
+			brcmf_fws_macdesc_indicate(fws, type, data);
+			break;
+		case BRCMF_FWS_TYPE_MAC_OPEN:
+		case BRCMF_FWS_TYPE_MAC_CLOSE:
+			err = brcmf_fws_macdesc_state_indicate(fws, type, data);
+			break;
+		case BRCMF_FWS_TYPE_INTERFACE_OPEN:
+		case BRCMF_FWS_TYPE_INTERFACE_CLOSE:
+			err = brcmf_fws_interface_state_indicate(fws, type,
+								 data);
+			break;
+		case BRCMF_FWS_TYPE_MAC_REQUEST_CREDIT:
+		case BRCMF_FWS_TYPE_MAC_REQUEST_PACKET:
+			err = brcmf_fws_request_indicate(fws, type, data);
+			break;
+		case BRCMF_FWS_TYPE_TXSTATUS:
+			brcmf_fws_txstatus_indicate(fws, data);
+			break;
+		case BRCMF_FWS_TYPE_FIFO_CREDITBACK:
+			err = brcmf_fws_fifocreditback_indicate(fws, data);
+			break;
+		case BRCMF_FWS_TYPE_RSSI:
+			brcmf_fws_rssi_indicate(fws, *data);
+			break;
+		case BRCMF_FWS_TYPE_TRANS_ID:
+			brcmf_fws_dbg_seqnum_check(fws, data);
+			break;
+		case BRCMF_FWS_TYPE_PKTTAG:
+		case BRCMF_FWS_TYPE_PENDING_TRAFFIC_BMP:
+		default:
+			fws->stats.tlv_invalid_type++;
+			break;
+		}
+		if (err == BRCMF_FWS_RET_OK_SCHEDULE)
+			status = BRCMF_FWS_RET_OK_SCHEDULE;
+		signal_data += len + 2;
+		data_len -= len + 2;
+	}
+
+	if (data_len != 0)
+		fws->stats.tlv_parse_failed++;
+
+	if (status == BRCMF_FWS_RET_OK_SCHEDULE)
+		brcmf_fws_schedule_deq(fws);
+
+	/* signalling processing result does
+	 * not affect the actual ethernet packet.
+	 */
+	skb_pull(skb, signal_len);
+
+	/* this may be a signal-only packet
+	 */
+	if (skb->len == 0)
+		fws->stats.header_only_pkt++;
+
+	brcmf_fws_unlock(drvr, flags);
+	return 0;
+}
+
+static int brcmf_fws_hdrpush(struct brcmf_fws_info *fws, struct sk_buff *skb)
+{
+	struct brcmf_fws_mac_descriptor *entry = brcmf_skbcb(skb)->mac;
+	u8 *wlh;
+	u16 data_offset = 0;
+	u8 fillers;
+	__le32 pkttag = cpu_to_le32(brcmf_skbcb(skb)->htod);
+
+	brcmf_dbg(TRACE, "enter: ea=%pM, ifidx=%u, pkttag=0x%08X\n",
+		  entry->ea, entry->interface_id, le32_to_cpu(pkttag));
+	if (entry->send_tim_signal)
+		data_offset += 2 + BRCMF_FWS_TYPE_PENDING_TRAFFIC_BMP_LEN;
+
+	/* +2 is for Type[1] and Len[1] in TLV, plus TIM signal */
+	data_offset += 2 + BRCMF_FWS_TYPE_PKTTAG_LEN;
+	fillers = round_up(data_offset, 4) - data_offset;
+	data_offset += fillers;
+
+	skb_push(skb, data_offset);
+	wlh = skb->data;
+
+	wlh[0] = BRCMF_FWS_TYPE_PKTTAG;
+	wlh[1] = BRCMF_FWS_TYPE_PKTTAG_LEN;
+	memcpy(&wlh[2], &pkttag, sizeof(pkttag));
+	wlh += BRCMF_FWS_TYPE_PKTTAG_LEN + 2;
+
+	if (entry->send_tim_signal) {
+		entry->send_tim_signal = 0;
+		wlh[0] = BRCMF_FWS_TYPE_PENDING_TRAFFIC_BMP;
+		wlh[1] = BRCMF_FWS_TYPE_PENDING_TRAFFIC_BMP_LEN;
+		wlh[2] = entry->mac_handle;
+		wlh[3] = entry->traffic_pending_bmp;
+		wlh += BRCMF_FWS_TYPE_PENDING_TRAFFIC_BMP_LEN + 2;
+		entry->traffic_lastreported_bmp = entry->traffic_pending_bmp;
+	}
+	if (fillers)
+		memset(wlh, BRCMF_FWS_TYPE_FILLER, fillers);
+
+	brcmf_proto_hdrpush(fws->drvr, brcmf_skb_if_flags_get_field(skb, INDEX),
+			    data_offset >> 2, skb);
+	return 0;
+}
+
+static int brcmf_fws_precommit_skb(struct brcmf_fws_info *fws, int fifo,
+				   struct sk_buff *p)
+{
+	struct brcmf_skbuff_cb *skcb = brcmf_skbcb(p);
+	struct brcmf_fws_mac_descriptor *entry = skcb->mac;
+	int rc = 0;
+	bool header_needed;
+	int hslot = BRCMF_FWS_HANGER_MAXITEMS;
+	u8 free_ctr;
+	u8 ifidx;
+	u8 flags;
+
+	header_needed = skcb->state != BRCMF_FWS_SKBSTATE_SUPPRESSED;
+
+	if (header_needed) {
+		/* obtaining free slot may fail, but that will be caught
+		 * by the hanger push. This assures the packet has a BDC
+		 * header upon return.
+		 */
+		hslot = brcmf_fws_hanger_get_free_slot(&fws->hanger);
+		free_ctr = entry->seq[fifo];
+		brcmf_skb_htod_tag_set_field(p, HSLOT, hslot);
+		brcmf_skb_htod_tag_set_field(p, FREERUN, free_ctr);
+		brcmf_skb_htod_tag_set_field(p, GENERATION, 1);
+		entry->transit_count++;
+	}
+	brcmf_skb_if_flags_set_field(p, TRANSMIT, 1);
+	brcmf_skb_htod_tag_set_field(p, FIFO, fifo);
+
+	flags = BRCMF_FWS_HTOD_FLAG_PKTFROMHOST;
+	if (!(skcb->if_flags & BRCMF_SKB_IF_FLAGS_CREDITCHECK_MASK)) {
+		/*
+		Indicate that this packet is being sent in response to an
+		explicit request from the firmware side.
+		*/
+		flags |= BRCMF_FWS_HTOD_FLAG_PKT_REQUESTED;
+	}
+	brcmf_skb_htod_tag_set_field(p, FLAGS, flags);
+	if (header_needed) {
+		brcmf_fws_hdrpush(fws, p);
+		rc = brcmf_fws_hanger_pushpkt(&fws->hanger, p, hslot);
+		if (rc)
+			brcmf_err("hanger push failed: rc=%d\n", rc);
+	} else {
+		int gen;
+
+		/* remove old header */
+		rc = brcmf_proto_hdrpull(fws->drvr, false, &ifidx, p);
+		if (rc == 0) {
+			hslot = brcmf_skb_htod_tag_get_field(p, HSLOT);
+			brcmf_fws_hanger_get_genbit(&fws->hanger, p,
+						    hslot, &gen);
+			brcmf_skb_htod_tag_set_field(p, GENERATION, gen);
+
+			/* push new header */
+			brcmf_fws_hdrpush(fws, p);
+		}
+	}
+
+	return rc;
+}
+
+static void
+brcmf_fws_rollback_toq(struct brcmf_fws_info *fws, struct sk_buff *skb)
+{
+	/*
+	put the packet back to the head of queue
+
+	- suppressed packet goes back to suppress sub-queue
+	- pull out the header, if new or delayed packet
+
+	Note: hslot is used only when header removal is done.
+	*/
+	struct brcmf_fws_mac_descriptor *entry;
+	enum brcmf_fws_skb_state state;
+	struct sk_buff *pktout;
+	int rc = 0;
+	int fifo;
+	int hslot;
+	u8 ifidx;
+
+	fifo = brcmf_skb_if_flags_get_field(skb, FIFO);
+	state = brcmf_skbcb(skb)->state;
+	entry = brcmf_skbcb(skb)->mac;
+
+	if (entry != NULL) {
+		if (state == BRCMF_FWS_SKBSTATE_SUPPRESSED) {
+			/* wl-header is saved for suppressed packets */
+			pktout = brcmu_pktq_penq_head(&entry->psq, 2 * fifo + 1,
+						      skb);
+			if (pktout == NULL) {
+				brcmf_err("suppress queue full\n");
+				rc = -ENOSPC;
+			}
+		} else {
+			hslot = brcmf_skb_htod_tag_get_field(skb, HSLOT);
+
+			/* remove header first */
+			rc = brcmf_proto_hdrpull(fws->drvr, false, &ifidx, skb);
+			if (rc) {
+				brcmf_err("header removal failed\n");
+				/* free the hanger slot */
+				brcmf_fws_hanger_poppkt(&fws->hanger, hslot,
+							&pktout, true);
+				rc = -EINVAL;
+				goto fail;
+			}
+
+			/* delay-q packets are going to delay-q */
+			pktout = brcmu_pktq_penq_head(&entry->psq,
+						      2 * fifo, skb);
+			if (pktout == NULL) {
+				brcmf_err("delay queue full\n");
+				rc = -ENOSPC;
+			}
+
+			/* free the hanger slot */
+			brcmf_fws_hanger_poppkt(&fws->hanger, hslot, &pktout,
+						true);
+
+			/* decrement sequence count */
+			entry->seq[fifo]--;
+		}
+		/*
+		if this packet did not count against FIFO credit, it must have
+		taken a requested_credit from the firmware (for pspoll etc.)
+		*/
+		if (!(brcmf_skbcb(skb)->if_flags &
+		      BRCMF_SKB_IF_FLAGS_CREDITCHECK_MASK))
+			entry->requested_credit++;
+	} else {
+		brcmf_err("no mac entry linked\n");
+		rc = -ENOENT;
+	}
+
+
+fail:
+	if (rc) {
+		brcmf_txfinalize(fws->drvr, skb, false);
+		fws->stats.rollback_failed++;
+	} else
+		fws->stats.rollback_success++;
+}
+
+static int brcmf_fws_borrow_credit(struct brcmf_fws_info *fws)
+{
+	int lender_ac;
+
+	if (time_after(fws->borrow_defer_timestamp, jiffies))
+		return -ENAVAIL;
+
+	for (lender_ac = 0; lender_ac <= BRCMF_FWS_FIFO_AC_VO; lender_ac++) {
+		if (fws->fifo_credit[lender_ac]) {
+			fws->credits_borrowed[lender_ac]++;
+			fws->fifo_credit[lender_ac]--;
+			if (fws->fifo_credit[lender_ac] == 0)
+				fws->fifo_credit_map &= ~(1 << lender_ac);
+			brcmf_dbg(TRACE, "borrow credit from: %d\n", lender_ac);
+			return 0;
+		}
+	}
+	return -ENAVAIL;
+}
+
+static int brcmf_fws_consume_credit(struct brcmf_fws_info *fws, int fifo,
+				    struct sk_buff *skb)
+{
+	struct brcmf_fws_mac_descriptor *entry = brcmf_skbcb(skb)->mac;
+	int *credit = &fws->fifo_credit[fifo];
+	int use_credit = 1;
+
+	brcmf_dbg(TRACE, "enter: ac=%d, credits=%d\n", fifo, *credit);
+
+	if (entry->requested_credit > 0) {
+		/*
+		 * if the packet was pulled out while destination is in
+		 * closed state but had a non-zero packets requested,
+		 * then this should not count against the FIFO credit.
+		 * That is due to the fact that the firmware will
+		 * most likely hold onto this packet until a suitable
+		 * time later to push it to the appropriate AC FIFO.
+		 */
+		entry->requested_credit--;
+		if (entry->state == BRCMF_FWS_STATE_CLOSE)
+			use_credit = 0;
+	} else if (entry->requested_packet > 0) {
+		entry->requested_packet--;
+		brcmf_skb_if_flags_set_field(skb, REQUESTED, 1);
+		if (entry->state == BRCMF_FWS_STATE_CLOSE)
+			use_credit = 0;
+	}
+	brcmf_skb_if_flags_set_field(skb, CREDITCHECK, use_credit);
+	if (!use_credit) {
+		brcmf_dbg(TRACE, "exit: no creditcheck set\n");
+		return 0;
+	}
+
+	if (fifo != BRCMF_FWS_FIFO_AC_BE)
+		fws->borrow_defer_timestamp = jiffies +
+					      BRCMF_FWS_BORROW_DEFER_PERIOD;
+
+	if (!(*credit)) {
+		/* Try to borrow a credit from other queue */
+		if (fifo == BRCMF_FWS_FIFO_AC_BE &&
+		    brcmf_fws_borrow_credit(fws) == 0)
+			return 0;
+
+		brcmf_dbg(TRACE, "exit: ac=%d, credits depleted\n", fifo);
+		return -ENAVAIL;
+	}
+	(*credit)--;
+	if (!(*credit))
+		fws->fifo_credit_map &= ~(1 << fifo);
+	brcmf_dbg(TRACE, "exit: ac=%d, credits=%d\n", fifo, *credit);
+	return 0;
+}
+
+static int brcmf_fws_commit_skb(struct brcmf_fws_info *fws, int fifo,
+				struct sk_buff *skb)
+{
+	struct brcmf_skbuff_cb *skcb = brcmf_skbcb(skb);
+	struct brcmf_fws_mac_descriptor *entry;
+	struct brcmf_bus *bus = fws->drvr->bus_if;
+	int rc;
+
+	entry = skcb->mac;
+	if (IS_ERR(entry))
+		return PTR_ERR(entry);
+
+	rc = brcmf_fws_precommit_skb(fws, fifo, skb);
+	if (rc < 0) {
+		fws->stats.generic_error++;
+		goto rollback;
+	}
+
+	rc = brcmf_bus_txdata(bus, skb);
+	if (rc < 0)
+		goto rollback;
+
+	entry->seq[fifo]++;
+	fws->stats.pkt2bus++;
+	if (brcmf_skbcb(skb)->if_flags & BRCMF_SKB_IF_FLAGS_CREDITCHECK_MASK) {
+		fws->stats.send_pkts[fifo]++;
+		fws->stats.fifo_credits_sent[fifo]++;
+	}
+
+	return rc;
+
+rollback:
+	brcmf_fws_rollback_toq(fws, skb);
+	return rc;
+}
+
+int brcmf_fws_process_skb(struct brcmf_if *ifp, struct sk_buff *skb)
+{
+	struct brcmf_pub *drvr = ifp->drvr;
+	struct brcmf_fws_info *fws = drvr->fws;
+	struct brcmf_skbuff_cb *skcb = brcmf_skbcb(skb);
+	struct ethhdr *eh = (struct ethhdr *)(skb->data);
+	ulong flags;
+	int fifo = BRCMF_FWS_FIFO_BCMC;
+	bool multicast = is_multicast_ether_addr(eh->h_dest);
+
+	/* determine the priority */
+	if (!skb->priority)
+		skb->priority = cfg80211_classify8021d(skb);
+
+	drvr->tx_multicast += !!multicast;
+	if (ntohs(eh->h_proto) == ETH_P_PAE)
+		atomic_inc(&ifp->pend_8021x_cnt);
+
+	if (!brcmf_fws_fc_active(fws)) {
+		/* If the protocol uses a data header, apply it */
+		brcmf_proto_hdrpush(drvr, ifp->ifidx, 0, skb);
+
+		/* Use bus module to send data frame */
+		return brcmf_bus_txdata(drvr->bus_if, skb);
+	}
+
+	/* set control buffer information */
+	skcb->if_flags = 0;
+	skcb->mac = brcmf_fws_find_mac_desc(fws, ifp, eh->h_dest);
+	skcb->state = BRCMF_FWS_SKBSTATE_NEW;
+	brcmf_skb_if_flags_set_field(skb, INDEX, ifp->ifidx);
+	if (!multicast)
+		fifo = brcmf_fws_prio2fifo[skb->priority];
+	brcmf_skb_if_flags_set_field(skb, FIFO, fifo);
+
+	brcmf_dbg(TRACE, "ea=%pM, multi=%d, fifo=%d\n", eh->h_dest,
+		  multicast, fifo);
+
+	brcmf_fws_lock(drvr, flags);
+	if (skcb->mac->suppressed ||
+	    brcmf_fws_mac_desc_closed(fws, skcb->mac, fifo) ||
+	    brcmu_pktq_mlen(&skcb->mac->psq, 3 << (fifo * 2)) ||
+	    (!multicast &&
+	     brcmf_fws_consume_credit(fws, fifo, skb) < 0)) {
+		/* enqueue the packet in delayQ */
+		drvr->fws->fifo_delay_map |= 1 << fifo;
+		brcmf_fws_enq(fws, BRCMF_FWS_SKBSTATE_DELAYED, fifo, skb);
+	} else {
+		if (brcmf_fws_commit_skb(fws, fifo, skb))
+			if (!multicast)
+				brcmf_skb_pick_up_credit(fws, fifo, skb);
+	}
+	brcmf_fws_unlock(drvr, flags);
+	return 0;
+}
+
+void brcmf_fws_reset_interface(struct brcmf_if *ifp)
+{
+	struct brcmf_fws_mac_descriptor *entry = ifp->fws_desc;
+
+	brcmf_dbg(TRACE, "enter: idx=%d\n", ifp->bssidx);
+	if (!entry)
+		return;
+
+	brcmf_fws_init_mac_descriptor(entry, ifp->mac_addr, ifp->ifidx);
+}
+
+void brcmf_fws_add_interface(struct brcmf_if *ifp)
+{
+	struct brcmf_fws_info *fws = ifp->drvr->fws;
+	struct brcmf_fws_mac_descriptor *entry;
+
+	brcmf_dbg(TRACE, "enter: idx=%d, mac=%pM\n",
+		  ifp->bssidx, ifp->mac_addr);
+	if (!ifp->ndev || !ifp->drvr->fw_signals)
+		return;
+
+	entry = &fws->desc.iface[ifp->ifidx];
+	ifp->fws_desc = entry;
+	brcmf_fws_init_mac_descriptor(entry, ifp->mac_addr, ifp->ifidx);
+	brcmu_pktq_init(&entry->psq, BRCMF_FWS_PSQ_PREC_COUNT,
+			BRCMF_FWS_PSQ_LEN);
+}
+
+void brcmf_fws_del_interface(struct brcmf_if *ifp)
+{
+	struct brcmf_fws_mac_descriptor *entry = ifp->fws_desc;
+	ulong flags;
+
+	brcmf_dbg(TRACE, "enter: idx=%d\n", ifp->bssidx);
+	if (!entry)
+		return;
+
+	brcmf_fws_lock(ifp->drvr, flags);
+	ifp->fws_desc = NULL;
+	brcmf_fws_clear_mac_descriptor(entry);
+	brcmf_fws_cleanup(ifp->drvr->fws, ifp->ifidx);
+	brcmf_fws_unlock(ifp->drvr, flags);
+}
+
+static void brcmf_fws_dequeue_worker(struct work_struct *worker)
+{
+	struct brcmf_fws_info *fws;
+	struct sk_buff *skb;
+	ulong flags;
+	int fifo;
+	int credit;
+
+	fws = container_of(worker, struct brcmf_fws_info, fws_dequeue_work);
+
+	brcmf_dbg(TRACE, "enter: fws=%p\n", fws);
+	brcmf_fws_lock(fws->drvr, flags);
+	for (fifo = NL80211_NUM_ACS; fifo >= 0; fifo--) {
+		brcmf_dbg(TRACE, "fifo %d credit %d\n", fifo,
+			  fws->fifo_credit[fifo]);
+		for (credit = 0; credit < fws->fifo_credit[fifo]; /* nop */) {
+			skb = brcmf_fws_deq(fws, fifo);
+			if (!skb || brcmf_fws_commit_skb(fws, fifo, skb))
+				break;
+			if (brcmf_skbcb(skb)->if_flags &
+			    BRCMF_SKB_IF_FLAGS_CREDITCHECK_MASK)
+				credit++;
+		}
+		if ((fifo == BRCMF_FWS_FIFO_AC_BE) &&
+		    (credit == fws->fifo_credit[fifo])) {
+			fws->fifo_credit[fifo] -= credit;
+			while (brcmf_fws_borrow_credit(fws) == 0) {
+				skb = brcmf_fws_deq(fws, fifo);
+				if (!skb) {
+					brcmf_fws_return_credits(fws, fifo, 1);
+					break;
+				}
+				if (brcmf_fws_commit_skb(fws, fifo, skb)) {
+					brcmf_fws_return_credits(fws, fifo, 1);
+					break;
+				}
+			}
+		} else {
+			fws->fifo_credit[fifo] -= credit;
+		}
+	}
+	brcmf_fws_unlock(fws->drvr, flags);
+}
+
+int brcmf_fws_init(struct brcmf_pub *drvr)
+{
+	u32 tlv = BRCMF_FWS_FLAGS_RSSI_SIGNALS;
+	int rc;
+
+	if (!drvr->fw_signals)
+		return 0;
+
+	spin_lock_init(&drvr->fws_spinlock);
+
+	drvr->fws = kzalloc(sizeof(*(drvr->fws)), GFP_KERNEL);
+	if (!drvr->fws) {
+		rc = -ENOMEM;
+		goto fail;
+	}
+
+	/* set linkage back */
+	drvr->fws->drvr = drvr;
+	drvr->fws->fcmode = fcmode;
+
+	drvr->fws->fws_wq = create_singlethread_workqueue("brcmf_fws_wq");
+	if (drvr->fws->fws_wq == NULL) {
+		brcmf_err("workqueue creation failed\n");
+		rc = -EBADF;
+		goto fail;
+	}
+	INIT_WORK(&drvr->fws->fws_dequeue_work, brcmf_fws_dequeue_worker);
+
+	/* enable firmware signalling if fcmode active */
+	if (drvr->fws->fcmode != BRCMF_FWS_FCMODE_NONE)
+		tlv |= BRCMF_FWS_FLAGS_XONXOFF_SIGNALS |
+		       BRCMF_FWS_FLAGS_CREDIT_STATUS_SIGNALS |
+		       BRCMF_FWS_FLAGS_HOST_PROPTXSTATUS_ACTIVE;
+
+	rc = brcmf_fweh_register(drvr, BRCMF_E_FIFO_CREDIT_MAP,
+				 brcmf_fws_notify_credit_map);
+	if (rc < 0) {
+		brcmf_err("register credit map handler failed\n");
+		goto fail;
+	}
+
+	/* setting the iovar may fail if feature is unsupported
+	 * so leave the rc as is so driver initialization can
+	 * continue.
+	 */
+	if (brcmf_fil_iovar_int_set(drvr->iflist[0], "tlv", tlv)) {
+		brcmf_err("failed to set bdcv2 tlv signaling\n");
+		goto fail_event;
+	}
+
+	brcmf_fws_hanger_init(&drvr->fws->hanger);
+	brcmf_fws_init_mac_descriptor(&drvr->fws->desc.other, NULL, 0);
+	brcmu_pktq_init(&drvr->fws->desc.other.psq, BRCMF_FWS_PSQ_PREC_COUNT,
+			BRCMF_FWS_PSQ_LEN);
+
+	/* create debugfs file for statistics */
+	brcmf_debugfs_create_fws_stats(drvr, &drvr->fws->stats);
+
+	/* TODO: remove upon feature delivery */
+	brcmf_err("%s bdcv2 tlv signaling [%x]\n",
+		  drvr->fw_signals ? "enabled" : "disabled", tlv);
+	return 0;
+
+fail_event:
+	brcmf_fweh_unregister(drvr, BRCMF_E_FIFO_CREDIT_MAP);
+fail:
+	brcmf_fws_deinit(drvr);
+	return rc;
+}
+
+void brcmf_fws_deinit(struct brcmf_pub *drvr)
+{
+	struct brcmf_fws_info *fws = drvr->fws;
+	ulong flags;
+
+	if (!fws)
+		return;
+
+	/* disable firmware signalling entirely
+	 * to avoid using the workqueue.
+	 */
+	drvr->fw_signals = false;
+
+	if (drvr->fws->fws_wq)
+		destroy_workqueue(drvr->fws->fws_wq);
+
+	/* cleanup */
+	brcmf_fws_lock(drvr, flags);
+	brcmf_fws_cleanup(fws, -1);
+	drvr->fws = NULL;
+	brcmf_fws_unlock(drvr, flags);
+
+	/* free top structure */
+	kfree(fws);
+}
+
+bool brcmf_fws_fc_active(struct brcmf_fws_info *fws)
+{
+	if (!fws)
+		return false;
+
+	brcmf_dbg(TRACE, "enter: mode=%d\n", fws->fcmode);
+	return fws->fcmode != BRCMF_FWS_FCMODE_NONE;
+}
+
+void brcmf_fws_bustxfail(struct brcmf_fws_info *fws, struct sk_buff *skb)
+{
+	ulong flags;
+
+	brcmf_fws_lock(fws->drvr, flags);
+	brcmf_fws_txstatus_process(fws, BRCMF_FWS_TXSTATUS_FW_TOSSED,
+				   brcmf_skb_htod_tag_get_field(skb, HSLOT), 0);
+	/* the packet never reached firmware so reclaim credit */
+	if (fws->fcmode == BRCMF_FWS_FCMODE_EXPLICIT_CREDIT &&
+	    brcmf_skbcb(skb)->if_flags & BRCMF_SKB_IF_FLAGS_CREDITCHECK_MASK) {
+		brcmf_fws_return_credits(fws,
+					 brcmf_skb_htod_tag_get_field(skb,
+								      FIFO),
+					 1);
+		brcmf_fws_schedule_deq(fws);
+	}
+	brcmf_fws_unlock(fws->drvr, flags);
+}
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.h b/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.h
new file mode 100644
index 000000000000..fbe483d23752
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (c) 2012 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+
+#ifndef FWSIGNAL_H_
+#define FWSIGNAL_H_
+
+int brcmf_fws_init(struct brcmf_pub *drvr);
+void brcmf_fws_deinit(struct brcmf_pub *drvr);
+bool brcmf_fws_fc_active(struct brcmf_fws_info *fws);
+int brcmf_fws_hdrpull(struct brcmf_pub *drvr, int ifidx, s16 signal_len,
+		      struct sk_buff *skb);
+int brcmf_fws_process_skb(struct brcmf_if *ifp, struct sk_buff *skb);
+
+void brcmf_fws_reset_interface(struct brcmf_if *ifp);
+void brcmf_fws_add_interface(struct brcmf_if *ifp);
+void brcmf_fws_del_interface(struct brcmf_if *ifp);
+void brcmf_fws_bustxfail(struct brcmf_fws_info *fws, struct sk_buff *skb);
+
+#endif /* FWSIGNAL_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/p2p.c b/drivers/net/wireless/brcm80211/brcmfmac/p2p.c
index 4166e642068b..e7a1a4770996 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/p2p.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/p2p.c
@@ -15,6 +15,7 @@
  */
 #include <linux/slab.h>
 #include <linux/netdevice.h>
+#include <linux/etherdevice.h>
 #include <net/cfg80211.h>
 
 #include <brcmu_wifi.h>
@@ -423,29 +424,6 @@ static void brcmf_p2p_print_actframe(bool tx, void *frame, u32 frame_len)
 
 
 /**
- * brcmf_p2p_chnr_to_chspec() - convert channel number to chanspec.
- *
- * @channel: channel number
- */
-static u16 brcmf_p2p_chnr_to_chspec(u16 channel)
-{
-	u16 chanspec;
-
-	chanspec = channel & WL_CHANSPEC_CHAN_MASK;
-
-	if (channel <= CH_MAX_2G_CHANNEL)
-		chanspec |= WL_CHANSPEC_BAND_2G;
-	else
-		chanspec |= WL_CHANSPEC_BAND_5G;
-
-	chanspec |= WL_CHANSPEC_BW_20;
-	chanspec |= WL_CHANSPEC_CTL_SB_NONE;
-
-	return chanspec;
-}
-
-
-/**
  * brcmf_p2p_set_firmware() - prepare firmware for peer-to-peer operation.
  *
  * @ifp: ifp to use for iovars (primary).
@@ -455,7 +433,9 @@ static int brcmf_p2p_set_firmware(struct brcmf_if *ifp, u8 *p2p_mac)
 {
 	s32 ret = 0;
 
+	brcmf_fil_cmd_int_set(ifp, BRCMF_C_DOWN, 1);
 	brcmf_fil_iovar_int_set(ifp, "apsta", 1);
+	brcmf_fil_cmd_int_set(ifp, BRCMF_C_UP, 1);
 
 	/* In case of COB type, firmware has default mac address
 	 * After Initializing firmware, we have to set current mac address to
@@ -473,28 +453,35 @@ static int brcmf_p2p_set_firmware(struct brcmf_if *ifp, u8 *p2p_mac)
  * brcmf_p2p_generate_bss_mac() - derive mac addresses for P2P.
  *
  * @p2p: P2P specific data.
+ * @dev_addr: optional device address.
  *
- * P2P needs mac addresses for P2P device and interface. These are
- * derived from the primary net device, ie. the permanent ethernet
- * address of the device.
+ * P2P needs mac addresses for P2P device and interface. If no device
+ * address it specified, these are derived from the primary net device, ie.
+ * the permanent ethernet address of the device.
  */
-static void brcmf_p2p_generate_bss_mac(struct brcmf_p2p_info *p2p)
+static void brcmf_p2p_generate_bss_mac(struct brcmf_p2p_info *p2p, u8 *dev_addr)
 {
 	struct brcmf_if *pri_ifp = p2p->bss_idx[P2PAPI_BSSCFG_PRIMARY].vif->ifp;
-	struct brcmf_if *p2p_ifp = p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif->ifp;
+	bool local_admin = false;
+
+	if (!dev_addr || is_zero_ether_addr(dev_addr)) {
+		dev_addr = pri_ifp->mac_addr;
+		local_admin = true;
+	}
 
 	/* Generate the P2P Device Address.  This consists of the device's
 	 * primary MAC address with the locally administered bit set.
 	 */
-	memcpy(p2p->dev_addr, pri_ifp->mac_addr, ETH_ALEN);
-	p2p->dev_addr[0] |= 0x02;
-	memcpy(p2p_ifp->mac_addr, p2p->dev_addr, ETH_ALEN);
+	memcpy(p2p->dev_addr, dev_addr, ETH_ALEN);
+	if (local_admin)
+		p2p->dev_addr[0] |= 0x02;
 
 	/* Generate the P2P Interface Address.  If the discovery and connection
 	 * BSSCFGs need to simultaneously co-exist, then this address must be
 	 * different from the P2P Device Address, but also locally administered.
 	 */
 	memcpy(p2p->int_addr, p2p->dev_addr, ETH_ALEN);
+	p2p->int_addr[0] |= 0x02;
 	p2p->int_addr[4] ^= 0x80;
 }
 
@@ -773,7 +760,7 @@ exit:
  * validates the channels in the request.
  */
 static s32 brcmf_p2p_run_escan(struct brcmf_cfg80211_info *cfg,
-			       struct net_device *ndev,
+			       struct brcmf_if *ifp,
 			       struct cfg80211_scan_request *request,
 			       u16 action)
 {
@@ -827,7 +814,8 @@ static s32 brcmf_p2p_run_escan(struct brcmf_cfg80211_info *cfg,
 					   IEEE80211_CHAN_PASSIVE_SCAN))
 				continue;
 
-			chanspecs[i] = channel_to_chanspec(chan);
+			chanspecs[i] = channel_to_chanspec(&p2p->cfg->d11inf,
+							   chan);
 			brcmf_dbg(INFO, "%d: chan=%d, channel spec=%x\n",
 				  num_nodfs, chan->hw_value, chanspecs[i]);
 			num_nodfs++;
@@ -935,8 +923,8 @@ static s32
 brcmf_p2p_discover_listen(struct brcmf_p2p_info *p2p, u16 channel, u32 duration)
 {
 	struct brcmf_cfg80211_vif *vif;
+	struct brcmu_chan ch;
 	s32 err = 0;
-	u16 chanspec;
 
 	vif = p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif;
 	if (!vif) {
@@ -951,9 +939,11 @@ brcmf_p2p_discover_listen(struct brcmf_p2p_info *p2p, u16 channel, u32 duration)
 		goto exit;
 	}
 
-	chanspec = brcmf_p2p_chnr_to_chspec(channel);
+	ch.chnum = channel;
+	ch.bw = BRCMU_CHAN_BW_20;
+	p2p->cfg->d11inf.encchspec(&ch);
 	err = brcmf_p2p_set_discover_state(vif->ifp, WL_P2P_DISC_ST_LISTEN,
-					   chanspec, (u16)duration);
+					   ch.chspec, (u16)duration);
 	if (!err) {
 		set_bit(BRCMF_P2P_STATUS_DISCOVER_LISTEN, &p2p->status);
 		p2p->remain_on_channel_cookie++;
@@ -1065,6 +1055,7 @@ static s32 brcmf_p2p_act_frm_search(struct brcmf_p2p_info *p2p, u16 channel)
 	u32 channel_cnt;
 	u16 *default_chan_list;
 	u32 i;
+	struct brcmu_chan ch;
 
 	brcmf_dbg(TRACE, "Enter\n");
 
@@ -1079,15 +1070,23 @@ static s32 brcmf_p2p_act_frm_search(struct brcmf_p2p_info *p2p, u16 channel)
 		err = -ENOMEM;
 		goto exit;
 	}
+	ch.bw = BRCMU_CHAN_BW_20;
 	if (channel) {
+		ch.chnum = channel;
+		p2p->cfg->d11inf.encchspec(&ch);
 		/* insert same channel to the chan_list */
 		for (i = 0; i < channel_cnt; i++)
-			default_chan_list[i] =
-					brcmf_p2p_chnr_to_chspec(channel);
+			default_chan_list[i] = ch.chspec;
 	} else {
-		default_chan_list[0] = brcmf_p2p_chnr_to_chspec(SOCIAL_CHAN_1);
-		default_chan_list[1] = brcmf_p2p_chnr_to_chspec(SOCIAL_CHAN_2);
-		default_chan_list[2] = brcmf_p2p_chnr_to_chspec(SOCIAL_CHAN_3);
+		ch.chnum = SOCIAL_CHAN_1;
+		p2p->cfg->d11inf.encchspec(&ch);
+		default_chan_list[0] = ch.chspec;
+		ch.chnum = SOCIAL_CHAN_2;
+		p2p->cfg->d11inf.encchspec(&ch);
+		default_chan_list[1] = ch.chspec;
+		ch.chnum = SOCIAL_CHAN_3;
+		p2p->cfg->d11inf.encchspec(&ch);
+		default_chan_list[2] = ch.chspec;
 	}
 	err = brcmf_p2p_escan(p2p, channel_cnt, default_chan_list,
 			      WL_P2P_DISC_ST_SEARCH, WL_ESCAN_ACTION_START,
@@ -1118,7 +1117,7 @@ static void brcmf_p2p_afx_handler(struct work_struct *work)
 	if (afx_hdl->is_listen && afx_hdl->my_listen_chan)
 		/* 100ms ~ 300ms */
 		err = brcmf_p2p_discover_listen(p2p, afx_hdl->my_listen_chan,
-						100 * (1 + (random32() % 3)));
+						100 * (1 + prandom_u32() % 3));
 	else
 		err = brcmf_p2p_act_frm_search(p2p, afx_hdl->peer_listen_chan);
 
@@ -1217,6 +1216,7 @@ bool brcmf_p2p_scan_finding_common_channel(struct brcmf_cfg80211_info *cfg,
 {
 	struct brcmf_p2p_info *p2p = &cfg->p2p;
 	struct afx_hdl *afx_hdl = &p2p->afx_hdl;
+	struct brcmu_chan ch;
 	u8 *ie;
 	s32 err;
 	u8 p2p_dev_addr[ETH_ALEN];
@@ -1242,8 +1242,12 @@ bool brcmf_p2p_scan_finding_common_channel(struct brcmf_cfg80211_info *cfg,
 					    p2p_dev_addr, sizeof(p2p_dev_addr));
 	if ((err >= 0) &&
 	    (!memcmp(p2p_dev_addr, afx_hdl->tx_dst_addr, ETH_ALEN))) {
-		afx_hdl->peer_chan = bi->ctl_ch ? bi->ctl_ch :
-				      CHSPEC_CHANNEL(le16_to_cpu(bi->chanspec));
+		if (!bi->ctl_ch) {
+			ch.chspec = le16_to_cpu(bi->chanspec);
+			cfg->d11inf.decchspec(&ch);
+			bi->ctl_ch = ch.chnum;
+		}
+		afx_hdl->peer_chan = bi->ctl_ch;
 		brcmf_dbg(TRACE, "ACTION FRAME SCAN : Peer %pM found, channel : %d\n",
 			  afx_hdl->tx_dst_addr, afx_hdl->peer_chan);
 		complete(&afx_hdl->act_frm_scan);
@@ -1261,7 +1265,7 @@ static void
 brcmf_p2p_stop_wait_next_action_frame(struct brcmf_cfg80211_info *cfg)
 {
 	struct brcmf_p2p_info *p2p = &cfg->p2p;
-	struct net_device *ndev = cfg->escan_info.ndev;
+	struct brcmf_if *ifp = cfg->escan_info.ifp;
 
 	if (test_bit(BRCMF_P2P_STATUS_SENDING_ACT_FRAME, &p2p->status) &&
 	    (test_bit(BRCMF_P2P_STATUS_ACTION_TX_COMPLETED, &p2p->status) ||
@@ -1271,12 +1275,12 @@ brcmf_p2p_stop_wait_next_action_frame(struct brcmf_cfg80211_info *cfg)
 		 * So abort scan for off channel completion.
 		 */
 		if (p2p->af_sent_channel)
-			brcmf_notify_escan_complete(cfg, ndev, true, true);
+			brcmf_notify_escan_complete(cfg, ifp, true, true);
 	} else if (test_bit(BRCMF_P2P_STATUS_WAITING_NEXT_AF_LISTEN,
 			    &p2p->status)) {
 		brcmf_dbg(TRACE, "*** Wake UP ** abort listen for next af frame\n");
 		/* So abort scan to cancel listen */
-		brcmf_notify_escan_complete(cfg, ndev, true, true);
+		brcmf_notify_escan_complete(cfg, ifp, true, true);
 	}
 }
 
@@ -1350,12 +1354,14 @@ int brcmf_p2p_notify_action_frame_rx(struct brcmf_if *ifp,
 	u8 *frame = (u8 *)(rxframe + 1);
 	struct brcmf_p2p_pub_act_frame *act_frm;
 	struct brcmf_p2psd_gas_pub_act_frame *sd_act_frm;
-	u16 chanspec = be16_to_cpu(rxframe->chanspec);
+	struct brcmu_chan ch;
 	struct ieee80211_mgmt *mgmt_frame;
 	s32 freq;
 	u16 mgmt_type;
 	u8 action;
 
+	ch.chspec = be16_to_cpu(rxframe->chanspec);
+	cfg->d11inf.decchspec(&ch);
 	/* Check if wpa_supplicant has registered for this frame */
 	brcmf_dbg(INFO, "ifp->vif->mgmt_rx_reg %04x\n", ifp->vif->mgmt_rx_reg);
 	mgmt_type = (IEEE80211_STYPE_ACTION & IEEE80211_FCTL_STYPE) >> 4;
@@ -1374,7 +1380,7 @@ int brcmf_p2p_notify_action_frame_rx(struct brcmf_if *ifp,
 				     &p2p->status) &&
 			    (memcmp(afx_hdl->tx_dst_addr, e->addr,
 				    ETH_ALEN) == 0)) {
-				afx_hdl->peer_chan = CHSPEC_CHANNEL(chanspec);
+				afx_hdl->peer_chan = ch.chnum;
 				brcmf_dbg(INFO, "GON request: Peer found, channel=%d\n",
 					  afx_hdl->peer_chan);
 				complete(&afx_hdl->act_frm_scan);
@@ -1384,7 +1390,7 @@ int brcmf_p2p_notify_action_frame_rx(struct brcmf_if *ifp,
 		/* After complete GO Negotiation, roll back to mpc mode */
 		if ((action == P2P_PAF_GON_CONF) ||
 		    (action == P2P_PAF_PROVDIS_RSP))
-			brcmf_set_mpc(ifp->ndev, 1);
+			brcmf_set_mpc(ifp, 1);
 		if (action == P2P_PAF_GON_CONF) {
 			brcmf_dbg(TRACE, "P2P: GO_NEG_PHASE status cleared\n");
 			clear_bit(BRCMF_P2P_STATUS_GO_NEG_PHASE, &p2p->status);
@@ -1417,11 +1423,12 @@ int brcmf_p2p_notify_action_frame_rx(struct brcmf_if *ifp,
 	memcpy(&mgmt_frame->u, frame, mgmt_frame_len);
 	mgmt_frame_len += offsetof(struct ieee80211_mgmt, u);
 
-	freq = ieee80211_channel_to_frequency(CHSPEC_CHANNEL(chanspec),
-					      CHSPEC_IS2G(chanspec) ?
+	freq = ieee80211_channel_to_frequency(ch.chnum,
+					      ch.band == BRCMU_CHAN_BAND_2G ?
 					      IEEE80211_BAND_2GHZ :
 					      IEEE80211_BAND_5GHZ);
-	wdev = ifp->ndev->ieee80211_ptr;
+
+	wdev = &ifp->vif->wdev;
 	cfg80211_rx_mgmt(wdev, freq, 0, (u8 *)mgmt_frame, mgmt_frame_len,
 			 GFP_ATOMIC);
 
@@ -1637,6 +1644,7 @@ bool brcmf_p2p_send_action_frame(struct brcmf_cfg80211_info *cfg,
 				 struct brcmf_fil_af_params_le *af_params)
 {
 	struct brcmf_p2p_info *p2p = &cfg->p2p;
+	struct brcmf_if *ifp = netdev_priv(ndev);
 	struct brcmf_fil_action_frame_le *action_frame;
 	struct brcmf_config_af_params config_af_params;
 	struct afx_hdl *afx_hdl = &p2p->afx_hdl;
@@ -1725,7 +1733,7 @@ bool brcmf_p2p_send_action_frame(struct brcmf_cfg80211_info *cfg,
 
 	/* To make sure to send successfully action frame, turn off mpc */
 	if (config_af_params.mpc_onoff == 0)
-		brcmf_set_mpc(ndev, 0);
+		brcmf_set_mpc(ifp, 0);
 
 	/* set status and destination address before sending af */
 	if (p2p->next_af_subtype != P2P_PAF_SUBTYPE_INVALID) {
@@ -1753,7 +1761,7 @@ bool brcmf_p2p_send_action_frame(struct brcmf_cfg80211_info *cfg,
 		 * care of current piggback algo, lets abort the scan here
 		 * itself.
 		 */
-		brcmf_notify_escan_complete(cfg, ndev, true, true);
+		brcmf_notify_escan_complete(cfg, ifp, true, true);
 
 		/* update channel */
 		af_params->channel = cpu_to_le32(afx_hdl->peer_chan);
@@ -1820,7 +1828,7 @@ exit:
 	clear_bit(BRCMF_P2P_STATUS_WAITING_NEXT_ACT_FRAME, &p2p->status);
 	/* if all done, turn mpc on again */
 	if (config_af_params.mpc_onoff == 1)
-		brcmf_set_mpc(ndev, 1);
+		brcmf_set_mpc(ifp, 1);
 
 	return ack;
 }
@@ -1839,10 +1847,10 @@ s32 brcmf_p2p_notify_rx_mgmt_p2p_probereq(struct brcmf_if *ifp,
 	struct brcmf_cfg80211_info *cfg = ifp->drvr->config;
 	struct brcmf_p2p_info *p2p = &cfg->p2p;
 	struct afx_hdl *afx_hdl = &p2p->afx_hdl;
-	struct wireless_dev *wdev;
 	struct brcmf_cfg80211_vif *vif = ifp->vif;
 	struct brcmf_rx_mgmt_data *rxframe = (struct brcmf_rx_mgmt_data *)data;
 	u16 chanspec = be16_to_cpu(rxframe->chanspec);
+	struct brcmu_chan ch;
 	u8 *mgmt_frame;
 	u32 mgmt_frame_len;
 	s32 freq;
@@ -1851,9 +1859,12 @@ s32 brcmf_p2p_notify_rx_mgmt_p2p_probereq(struct brcmf_if *ifp,
 	brcmf_dbg(INFO, "Enter: event %d reason %d\n", e->event_code,
 		  e->reason);
 
+	ch.chspec = be16_to_cpu(rxframe->chanspec);
+	cfg->d11inf.decchspec(&ch);
+
 	if (test_bit(BRCMF_P2P_STATUS_FINDING_COMMON_CHANNEL, &p2p->status) &&
 	    (memcmp(afx_hdl->tx_dst_addr, e->addr, ETH_ALEN) == 0)) {
-		afx_hdl->peer_chan = CHSPEC_CHANNEL(chanspec);
+		afx_hdl->peer_chan = ch.chnum;
 		brcmf_dbg(INFO, "PROBE REQUEST: Peer found, channel=%d\n",
 			  afx_hdl->peer_chan);
 		complete(&afx_hdl->act_frm_scan);
@@ -1878,12 +1889,13 @@ s32 brcmf_p2p_notify_rx_mgmt_p2p_probereq(struct brcmf_if *ifp,
 
 	mgmt_frame = (u8 *)(rxframe + 1);
 	mgmt_frame_len = e->datalen - sizeof(*rxframe);
-	freq = ieee80211_channel_to_frequency(CHSPEC_CHANNEL(chanspec),
-					      CHSPEC_IS2G(chanspec) ?
+	freq = ieee80211_channel_to_frequency(ch.chnum,
+					      ch.band == BRCMU_CHAN_BAND_2G ?
 					      IEEE80211_BAND_2GHZ :
 					      IEEE80211_BAND_5GHZ);
-	wdev = ifp->ndev->ieee80211_ptr;
-	cfg80211_rx_mgmt(wdev, freq, 0, mgmt_frame, mgmt_frame_len, GFP_ATOMIC);
+
+	cfg80211_rx_mgmt(&vif->wdev, freq, 0, mgmt_frame, mgmt_frame_len,
+			 GFP_ATOMIC);
 
 	brcmf_dbg(INFO, "mgmt_frame_len (%d) , e->datalen (%d), chanspec (%04x), freq (%d)\n",
 		  mgmt_frame_len, e->datalen, chanspec, freq);
@@ -1934,7 +1946,8 @@ s32 brcmf_p2p_attach(struct brcmf_cfg80211_info *cfg)
 
 		p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif = p2p_vif;
 
-		brcmf_p2p_generate_bss_mac(p2p);
+		brcmf_p2p_generate_bss_mac(p2p, NULL);
+		memcpy(p2p_ifp->mac_addr, p2p->dev_addr, ETH_ALEN);
 		brcmf_p2p_set_firmware(pri_ifp, p2p->dev_addr);
 
 		/* Initialize P2P Discovery in the firmware */
@@ -2001,21 +2014,19 @@ static void brcmf_p2p_get_current_chanspec(struct brcmf_p2p_info *p2p,
 {
 	struct brcmf_if *ifp;
 	struct brcmf_fil_chan_info_le ci;
+	struct brcmu_chan ch;
 	s32 err;
 
 	ifp = p2p->bss_idx[P2PAPI_BSSCFG_PRIMARY].vif->ifp;
 
-	*chanspec = 11 & WL_CHANSPEC_CHAN_MASK;
+	ch.chnum = 11;
 
 	err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_CHANNEL, &ci, sizeof(ci));
-	if (!err) {
-		*chanspec = le32_to_cpu(ci.hw_channel) & WL_CHANSPEC_CHAN_MASK;
-		if (*chanspec < CH_MAX_2G_CHANNEL)
-			*chanspec |= WL_CHANSPEC_BAND_2G;
-		else
-			*chanspec |= WL_CHANSPEC_BAND_5G;
-	}
-	*chanspec |= WL_CHANSPEC_BW_20 | WL_CHANSPEC_CTL_SB_NONE;
+	if (!err)
+		ch.chnum = le32_to_cpu(ci.hw_channel);
+	ch.bw = BRCMU_CHAN_BW_20;
+	p2p->cfg->d11inf.encchspec(&ch);
+	*chanspec = ch.chspec;
 }
 
 /**
@@ -2040,13 +2051,13 @@ int brcmf_p2p_ifchange(struct brcmf_cfg80211_info *cfg,
 		brcmf_err("vif for P2PAPI_BSSCFG_PRIMARY does not exist\n");
 		return -EPERM;
 	}
-	brcmf_notify_escan_complete(cfg, vif->ifp->ndev, true, true);
+	brcmf_notify_escan_complete(cfg, vif->ifp, true, true);
 	vif = p2p->bss_idx[P2PAPI_BSSCFG_CONNECTION].vif;
 	if (!vif) {
 		brcmf_err("vif for P2PAPI_BSSCFG_CONNECTION does not exist\n");
 		return -EPERM;
 	}
-	brcmf_set_mpc(vif->ifp->ndev, 0);
+	brcmf_set_mpc(vif->ifp, 0);
 
 	/* In concurrency case, STA may be already associated in a particular */
 	/* channel. so retrieve the current channel of primary interface and  */
@@ -2124,13 +2135,105 @@ static int brcmf_p2p_release_p2p_if(struct brcmf_cfg80211_vif *vif)
 }
 
 /**
+ * brcmf_p2p_create_p2pdev() - create a P2P_DEVICE virtual interface.
+ *
+ * @p2p: P2P specific data.
+ * @wiphy: wiphy device of new interface.
+ * @addr: mac address for this new interface.
+ */
+static struct wireless_dev *brcmf_p2p_create_p2pdev(struct brcmf_p2p_info *p2p,
+						    struct wiphy *wiphy,
+						    u8 *addr)
+{
+	struct brcmf_cfg80211_vif *p2p_vif;
+	struct brcmf_if *p2p_ifp;
+	struct brcmf_if *pri_ifp;
+	int err;
+	u32 bssidx;
+
+	if (p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif)
+		return ERR_PTR(-ENOSPC);
+
+	p2p_vif = brcmf_alloc_vif(p2p->cfg, NL80211_IFTYPE_P2P_DEVICE,
+				  false);
+	if (IS_ERR(p2p_vif)) {
+		brcmf_err("could not create discovery vif\n");
+		return (struct wireless_dev *)p2p_vif;
+	}
+
+	pri_ifp = p2p->bss_idx[P2PAPI_BSSCFG_PRIMARY].vif->ifp;
+	brcmf_p2p_generate_bss_mac(p2p, addr);
+	brcmf_p2p_set_firmware(pri_ifp, p2p->dev_addr);
+
+	brcmf_cfg80211_arm_vif_event(p2p->cfg, p2p_vif);
+
+	/* Initialize P2P Discovery in the firmware */
+	err = brcmf_fil_iovar_int_set(pri_ifp, "p2p_disc", 1);
+	if (err < 0) {
+		brcmf_err("set p2p_disc error\n");
+		brcmf_cfg80211_arm_vif_event(p2p->cfg, NULL);
+		goto fail;
+	}
+
+	/* wait for firmware event */
+	err = brcmf_cfg80211_wait_vif_event_timeout(p2p->cfg, BRCMF_E_IF_ADD,
+						    msecs_to_jiffies(1500));
+	brcmf_cfg80211_arm_vif_event(p2p->cfg, NULL);
+	if (!err) {
+		brcmf_err("timeout occurred\n");
+		err = -EIO;
+		goto fail;
+	}
+
+	/* discovery interface created */
+	p2p_ifp = p2p_vif->ifp;
+	p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif = p2p_vif;
+	memcpy(p2p_ifp->mac_addr, p2p->dev_addr, ETH_ALEN);
+	memcpy(&p2p_vif->wdev.address, p2p->dev_addr, sizeof(p2p->dev_addr));
+
+	/* verify bsscfg index for P2P discovery */
+	err = brcmf_fil_iovar_int_get(pri_ifp, "p2p_dev", &bssidx);
+	if (err < 0) {
+		brcmf_err("retrieving discover bsscfg index failed\n");
+		goto fail;
+	}
+
+	WARN_ON(p2p_ifp->bssidx != bssidx);
+
+	init_completion(&p2p->send_af_done);
+	INIT_WORK(&p2p->afx_hdl.afx_work, brcmf_p2p_afx_handler);
+	init_completion(&p2p->afx_hdl.act_frm_scan);
+	init_completion(&p2p->wait_next_af);
+
+	return &p2p_vif->wdev;
+
+fail:
+	brcmf_free_vif(p2p_vif);
+	return ERR_PTR(err);
+}
+
+/**
+ * brcmf_p2p_delete_p2pdev() - delete P2P_DEVICE virtual interface.
+ *
+ * @vif: virtual interface object to delete.
+ */
+static void brcmf_p2p_delete_p2pdev(struct brcmf_cfg80211_vif *vif)
+{
+	struct brcmf_p2p_info *p2p = &vif->ifp->drvr->config->p2p;
+
+	cfg80211_unregister_wdev(&vif->wdev);
+	p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif = NULL;
+	brcmf_free_vif(vif);
+}
+
+/**
  * brcmf_p2p_add_vif() - create a new P2P virtual interface.
  *
  * @wiphy: wiphy device of new interface.
  * @name: name of the new interface.
  * @type: nl80211 interface type.
- * @flags: TBD
- * @params: TBD
+ * @flags: not used.
+ * @params: contains mac address for P2P device.
  */
 struct wireless_dev *brcmf_p2p_add_vif(struct wiphy *wiphy, const char *name,
 				       enum nl80211_iftype type, u32 *flags,
@@ -2157,6 +2260,9 @@ struct wireless_dev *brcmf_p2p_add_vif(struct wiphy *wiphy, const char *name,
 		iftype = BRCMF_FIL_P2P_IF_GO;
 		mode = WL_MODE_AP;
 		break;
+	case NL80211_IFTYPE_P2P_DEVICE:
+		return brcmf_p2p_create_p2pdev(&cfg->p2p, wiphy,
+					       params->macaddr);
 	default:
 		return ERR_PTR(-EOPNOTSUPP);
 	}
@@ -2244,6 +2350,8 @@ int brcmf_p2p_del_vif(struct wiphy *wiphy, struct wireless_dev *wdev)
 		break;
 
 	case NL80211_IFTYPE_P2P_DEVICE:
+		brcmf_p2p_delete_p2pdev(vif);
+		return 0;
 	default:
 		return -ENOTSUPP;
 		break;
@@ -2275,3 +2383,33 @@ int brcmf_p2p_del_vif(struct wiphy *wiphy, struct wireless_dev *wdev)
 
 	return err;
 }
+
+int brcmf_p2p_start_device(struct wiphy *wiphy, struct wireless_dev *wdev)
+{
+	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+	struct brcmf_p2p_info *p2p = &cfg->p2p;
+	struct brcmf_cfg80211_vif *vif;
+	int err;
+
+	vif = container_of(wdev, struct brcmf_cfg80211_vif, wdev);
+	mutex_lock(&cfg->usr_sync);
+	err = brcmf_p2p_enable_discovery(p2p);
+	if (!err)
+		set_bit(BRCMF_VIF_STATUS_READY, &vif->sme_state);
+	mutex_unlock(&cfg->usr_sync);
+	return err;
+}
+
+void brcmf_p2p_stop_device(struct wiphy *wiphy, struct wireless_dev *wdev)
+{
+	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+	struct brcmf_p2p_info *p2p = &cfg->p2p;
+	struct brcmf_cfg80211_vif *vif;
+
+	vif = container_of(wdev, struct brcmf_cfg80211_vif, wdev);
+	mutex_lock(&cfg->usr_sync);
+	(void)brcmf_p2p_deinit_discovery(p2p);
+	brcmf_abort_scanning(cfg);
+	clear_bit(BRCMF_VIF_STATUS_READY, &vif->sme_state);
+	mutex_unlock(&cfg->usr_sync);
+}
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c b/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
index 14be2d5530ce..ca72177388b9 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
@@ -40,6 +40,15 @@
 #define BCM4329_CORE_ARM_BASE		0x18002000
 #define BCM4329_RAMSIZE			0x48000
 
+/* bcm43143 */
+/* SDIO device core */
+#define BCM43143_CORE_BUS_BASE		0x18002000
+/* internal memory core */
+#define BCM43143_CORE_SOCRAM_BASE	0x18004000
+/* ARM Cortex M3 core, ID 0x82a */
+#define BCM43143_CORE_ARM_BASE		0x18003000
+#define BCM43143_RAMSIZE		0x70000
+
 #define	SBCOREREV(sbidh) \
 	((((sbidh) & SSB_IDHIGH_RCHI) >> SSB_IDHIGH_RCHI_SHIFT) | \
 	  ((sbidh) & SSB_IDHIGH_RCLO))
@@ -52,6 +61,9 @@
 #define CIB_REV_MASK		0xff000000
 #define CIB_REV_SHIFT		24
 
+/* ARM CR4 core specific control flag bits */
+#define ARMCR4_BCMA_IOCTL_CPUHALT	0x0020
+
 #define SDIOD_DRVSTR_KEY(chip, pmu)     (((chip) << 16) | (pmu))
 /* SDIO Pad drive strength to select value mappings */
 struct sdiod_drive_str {
@@ -70,6 +82,14 @@ static const struct sdiod_drive_str sdiod_drvstr_tab1_1v8[] = {
 	{0, 0x1}
 };
 
+/* SDIO Drive Strength to sel value table for 43143 PMU Rev 17 (3.3V) */
+static const struct sdiod_drive_str sdiod_drvstr_tab2_3v3[] = {
+	{16, 0x7},
+	{12, 0x5},
+	{8,  0x3},
+	{4,  0x1}
+};
+
 u8
 brcmf_sdio_chip_getinfidx(struct chip_info *ci, u16 coreid)
 {
@@ -149,7 +169,7 @@ brcmf_sdio_ai_iscoreup(struct brcmf_sdio_dev *sdiodev,
 
 static void
 brcmf_sdio_sb_coredisable(struct brcmf_sdio_dev *sdiodev,
-			  struct chip_info *ci, u16 coreid)
+			  struct chip_info *ci, u16 coreid, u32 core_bits)
 {
 	u32 regdata, base;
 	u8 idx;
@@ -235,7 +255,7 @@ brcmf_sdio_sb_coredisable(struct brcmf_sdio_dev *sdiodev,
 
 static void
 brcmf_sdio_ai_coredisable(struct brcmf_sdio_dev *sdiodev,
-			  struct chip_info *ci, u16 coreid)
+			  struct chip_info *ci, u16 coreid, u32 core_bits)
 {
 	u8 idx;
 	u32 regdata;
@@ -249,19 +269,36 @@ brcmf_sdio_ai_coredisable(struct brcmf_sdio_dev *sdiodev,
 	if ((regdata & BCMA_RESET_CTL_RESET) != 0)
 		return;
 
-	brcmf_sdio_regwl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL, 0, NULL);
-	regdata = brcmf_sdio_regrl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
+	/* ensure no pending backplane operation
+	 * 300uc should be sufficient for backplane ops to be finish
+	 * extra 10ms is taken into account for firmware load stage
+	 * after 10300us carry on disabling the core anyway
+	 */
+	SPINWAIT(brcmf_sdio_regrl(sdiodev,
+				  ci->c_inf[idx].wrapbase+BCMA_RESET_ST,
+				  NULL), 10300);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   ci->c_inf[idx].wrapbase+BCMA_RESET_ST,
 				   NULL);
-	udelay(10);
+	if (regdata)
+		brcmf_err("disabling core 0x%x with reset status %x\n",
+			  coreid, regdata);
 
 	brcmf_sdio_regwl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
 			 BCMA_RESET_CTL_RESET, NULL);
 	udelay(1);
+
+	brcmf_sdio_regwl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
+			 core_bits, NULL);
+	regdata = brcmf_sdio_regrl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
+				   NULL);
+	usleep_range(10, 20);
+
 }
 
 static void
 brcmf_sdio_sb_resetcore(struct brcmf_sdio_dev *sdiodev,
-			struct chip_info *ci, u16 coreid)
+			struct chip_info *ci, u16 coreid, u32 core_bits)
 {
 	u32 regdata;
 	u8 idx;
@@ -272,7 +309,7 @@ brcmf_sdio_sb_resetcore(struct brcmf_sdio_dev *sdiodev,
 	 * Must do the disable sequence first to work for
 	 * arbitrary current core state.
 	 */
-	brcmf_sdio_sb_coredisable(sdiodev, ci, coreid);
+	brcmf_sdio_sb_coredisable(sdiodev, ci, coreid, 0);
 
 	/*
 	 * Now do the initialization sequence.
@@ -325,7 +362,7 @@ brcmf_sdio_sb_resetcore(struct brcmf_sdio_dev *sdiodev,
 
 static void
 brcmf_sdio_ai_resetcore(struct brcmf_sdio_dev *sdiodev,
-			struct chip_info *ci, u16 coreid)
+			struct chip_info *ci, u16 coreid, u32 core_bits)
 {
 	u8 idx;
 	u32 regdata;
@@ -333,31 +370,69 @@ brcmf_sdio_ai_resetcore(struct brcmf_sdio_dev *sdiodev,
 	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
 
 	/* must disable first to work for arbitrary current core state */
-	brcmf_sdio_ai_coredisable(sdiodev, ci, coreid);
+	brcmf_sdio_ai_coredisable(sdiodev, ci, coreid, core_bits);
 
 	/* now do initialization sequence */
 	brcmf_sdio_regwl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
-			 BCMA_IOCTL_FGC | BCMA_IOCTL_CLK, NULL);
+			 core_bits | BCMA_IOCTL_FGC | BCMA_IOCTL_CLK, NULL);
 	regdata = brcmf_sdio_regrl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
 				   NULL);
 	brcmf_sdio_regwl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
 			 0, NULL);
+	regdata = brcmf_sdio_regrl(sdiodev,
+				   ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
+				   NULL);
 	udelay(1);
 
 	brcmf_sdio_regwl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
-			 BCMA_IOCTL_CLK, NULL);
+			 core_bits | BCMA_IOCTL_CLK, NULL);
 	regdata = brcmf_sdio_regrl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
 				   NULL);
 	udelay(1);
 }
 
+#ifdef DEBUG
+/* safety check for chipinfo */
+static int brcmf_sdio_chip_cichk(struct chip_info *ci)
+{
+	u8 core_idx;
+
+	/* check RAM core presence for ARM CM3 core */
+	core_idx = brcmf_sdio_chip_getinfidx(ci, BCMA_CORE_ARM_CM3);
+	if (BRCMF_MAX_CORENUM != core_idx) {
+		core_idx = brcmf_sdio_chip_getinfidx(ci,
+						     BCMA_CORE_INTERNAL_MEM);
+		if (BRCMF_MAX_CORENUM == core_idx) {
+			brcmf_err("RAM core not provided with ARM CM3 core\n");
+			return -ENODEV;
+		}
+	}
+
+	/* check RAM base for ARM CR4 core */
+	core_idx = brcmf_sdio_chip_getinfidx(ci, BCMA_CORE_ARM_CR4);
+	if (BRCMF_MAX_CORENUM != core_idx) {
+		if (ci->rambase == 0) {
+			brcmf_err("RAM base not provided with ARM CR4 core\n");
+			return -ENOMEM;
+		}
+	}
+
+	return 0;
+}
+#else	/* DEBUG */
+static inline int brcmf_sdio_chip_cichk(struct chip_info *ci)
+{
+	return 0;
+}
+#endif
+
 static int brcmf_sdio_chip_recognition(struct brcmf_sdio_dev *sdiodev,
 				       struct chip_info *ci, u32 regs)
 {
 	u32 regdata;
+	int ret;
 
-	/*
-	 * Get CC core rev
+	/* Get CC core rev
 	 * Chipid is assume to be at offset 0 from regs arg
 	 * For different chiptypes or old sdio hosts w/o chipcommon,
 	 * other ways of recognition should be added here.
@@ -375,6 +450,23 @@ static int brcmf_sdio_chip_recognition(struct brcmf_sdio_dev *sdiodev,
 
 	/* Address of cores for new chips should be added here */
 	switch (ci->chip) {
+	case BCM43143_CHIP_ID:
+		ci->c_inf[0].wrapbase = ci->c_inf[0].base + 0x00100000;
+		ci->c_inf[0].cib = 0x2b000000;
+		ci->c_inf[1].id = BCMA_CORE_SDIO_DEV;
+		ci->c_inf[1].base = BCM43143_CORE_BUS_BASE;
+		ci->c_inf[1].wrapbase = ci->c_inf[1].base + 0x00100000;
+		ci->c_inf[1].cib = 0x18000000;
+		ci->c_inf[2].id = BCMA_CORE_INTERNAL_MEM;
+		ci->c_inf[2].base = BCM43143_CORE_SOCRAM_BASE;
+		ci->c_inf[2].wrapbase = ci->c_inf[2].base + 0x00100000;
+		ci->c_inf[2].cib = 0x14000000;
+		ci->c_inf[3].id = BCMA_CORE_ARM_CM3;
+		ci->c_inf[3].base = BCM43143_CORE_ARM_BASE;
+		ci->c_inf[3].wrapbase = ci->c_inf[3].base + 0x00100000;
+		ci->c_inf[3].cib = 0x07000000;
+		ci->ramsize = BCM43143_RAMSIZE;
+		break;
 	case BCM43241_CHIP_ID:
 		ci->c_inf[0].wrapbase = 0x18100000;
 		ci->c_inf[0].cib = 0x2a084411;
@@ -435,11 +527,29 @@ static int brcmf_sdio_chip_recognition(struct brcmf_sdio_dev *sdiodev,
 		ci->c_inf[3].cib = 0x07004211;
 		ci->ramsize = 0x80000;
 		break;
+	case BCM4335_CHIP_ID:
+		ci->c_inf[0].wrapbase = 0x18100000;
+		ci->c_inf[0].cib = 0x2b084411;
+		ci->c_inf[1].id = BCMA_CORE_SDIO_DEV;
+		ci->c_inf[1].base = 0x18005000;
+		ci->c_inf[1].wrapbase = 0x18105000;
+		ci->c_inf[1].cib = 0x0f004211;
+		ci->c_inf[2].id = BCMA_CORE_ARM_CR4;
+		ci->c_inf[2].base = 0x18002000;
+		ci->c_inf[2].wrapbase = 0x18102000;
+		ci->c_inf[2].cib = 0x01084411;
+		ci->ramsize = 0xc0000;
+		ci->rambase = 0x180000;
+		break;
 	default:
 		brcmf_err("chipid 0x%x is not supported\n", ci->chip);
 		return -ENODEV;
 	}
 
+	ret = brcmf_sdio_chip_cichk(ci);
+	if (ret)
+		return ret;
+
 	switch (ci->socitype) {
 	case SOCI_SB:
 		ci->iscoreup = brcmf_sdio_sb_iscoreup;
@@ -539,7 +649,7 @@ brcmf_sdio_chip_buscoresetup(struct brcmf_sdio_dev *sdiodev,
 	 * Make sure any on-chip ARM is off (in case strapping is wrong),
 	 * or downloaded code was already running.
 	 */
-	ci->coredisable(sdiodev, ci, BCMA_CORE_ARM_CM3);
+	ci->coredisable(sdiodev, ci, BCMA_CORE_ARM_CM3, 0);
 }
 
 int brcmf_sdio_chip_attach(struct brcmf_sdio_dev *sdiodev,
@@ -600,21 +710,37 @@ void
 brcmf_sdio_chip_drivestrengthinit(struct brcmf_sdio_dev *sdiodev,
 				  struct chip_info *ci, u32 drivestrength)
 {
-	struct sdiod_drive_str *str_tab = NULL;
-	u32 str_mask = 0;
-	u32 str_shift = 0;
+	const struct sdiod_drive_str *str_tab = NULL;
+	u32 str_mask;
+	u32 str_shift;
 	char chn[8];
 	u32 base = ci->c_inf[0].base;
+	u32 i;
+	u32 drivestrength_sel = 0;
+	u32 cc_data_temp;
+	u32 addr;
 
 	if (!(ci->c_inf[0].caps & CC_CAP_PMU))
 		return;
 
 	switch (SDIOD_DRVSTR_KEY(ci->chip, ci->pmurev)) {
 	case SDIOD_DRVSTR_KEY(BCM4330_CHIP_ID, 12):
-		str_tab = (struct sdiod_drive_str *)&sdiod_drvstr_tab1_1v8;
+		str_tab = sdiod_drvstr_tab1_1v8;
 		str_mask = 0x00003800;
 		str_shift = 11;
 		break;
+	case SDIOD_DRVSTR_KEY(BCM43143_CHIP_ID, 17):
+		/* note: 43143 does not support tristate */
+		i = ARRAY_SIZE(sdiod_drvstr_tab2_3v3) - 1;
+		if (drivestrength >= sdiod_drvstr_tab2_3v3[i].strength) {
+			str_tab = sdiod_drvstr_tab2_3v3;
+			str_mask = 0x00000007;
+			str_shift = 0;
+		} else
+			brcmf_err("Invalid SDIO Drive strength for chip %s, strength=%d\n",
+				  brcmf_sdio_chip_name(ci->chip, chn, 8),
+				  drivestrength);
+		break;
 	default:
 		brcmf_err("No SDIO Drive strength init done for chip %s rev %d pmurev %d\n",
 			  brcmf_sdio_chip_name(ci->chip, chn, 8),
@@ -623,30 +749,207 @@ brcmf_sdio_chip_drivestrengthinit(struct brcmf_sdio_dev *sdiodev,
 	}
 
 	if (str_tab != NULL) {
-		u32 drivestrength_sel = 0;
-		u32 cc_data_temp;
-		int i;
-
 		for (i = 0; str_tab[i].strength != 0; i++) {
 			if (drivestrength >= str_tab[i].strength) {
 				drivestrength_sel = str_tab[i].sel;
 				break;
 			}
 		}
-
-		brcmf_sdio_regwl(sdiodev, CORE_CC_REG(base, chipcontrol_addr),
-				 1, NULL);
-		cc_data_temp =
-			brcmf_sdio_regrl(sdiodev,
-					 CORE_CC_REG(base, chipcontrol_addr),
-					 NULL);
+		addr = CORE_CC_REG(base, chipcontrol_addr);
+		brcmf_sdio_regwl(sdiodev, addr, 1, NULL);
+		cc_data_temp = brcmf_sdio_regrl(sdiodev, addr, NULL);
 		cc_data_temp &= ~str_mask;
 		drivestrength_sel <<= str_shift;
 		cc_data_temp |= drivestrength_sel;
-		brcmf_sdio_regwl(sdiodev, CORE_CC_REG(base, chipcontrol_addr),
-				 cc_data_temp, NULL);
+		brcmf_sdio_regwl(sdiodev, addr, cc_data_temp, NULL);
 
-		brcmf_dbg(INFO, "SDIO: %dmA drive strength selected, set to 0x%08x\n",
-			  drivestrength, cc_data_temp);
+		brcmf_dbg(INFO, "SDIO: %d mA (req=%d mA) drive strength selected, set to 0x%08x\n",
+			  str_tab[i].strength, drivestrength, cc_data_temp);
 	}
 }
+
+#ifdef DEBUG
+static bool
+brcmf_sdio_chip_verifynvram(struct brcmf_sdio_dev *sdiodev, u32 nvram_addr,
+			    char *nvram_dat, uint nvram_sz)
+{
+	char *nvram_ularray;
+	int err;
+	bool ret = true;
+
+	/* read back and verify */
+	brcmf_dbg(INFO, "Compare NVRAM dl & ul; size=%d\n", nvram_sz);
+	nvram_ularray = kmalloc(nvram_sz, GFP_KERNEL);
+	/* do not proceed while no memory but  */
+	if (!nvram_ularray)
+		return true;
+
+	/* Upload image to verify downloaded contents. */
+	memset(nvram_ularray, 0xaa, nvram_sz);
+
+	/* Read the vars list to temp buffer for comparison */
+	err = brcmf_sdio_ramrw(sdiodev, false, nvram_addr, nvram_ularray,
+			       nvram_sz);
+	if (err) {
+		brcmf_err("error %d on reading %d nvram bytes at 0x%08x\n",
+			  err, nvram_sz, nvram_addr);
+	} else if (memcmp(nvram_dat, nvram_ularray, nvram_sz)) {
+		brcmf_err("Downloaded NVRAM image is corrupted\n");
+		ret = false;
+	}
+	kfree(nvram_ularray);
+
+	return ret;
+}
+#else	/* DEBUG */
+static inline bool
+brcmf_sdio_chip_verifynvram(struct brcmf_sdio_dev *sdiodev, u32 nvram_addr,
+			    char *nvram_dat, uint nvram_sz)
+{
+	return true;
+}
+#endif	/* DEBUG */
+
+static bool brcmf_sdio_chip_writenvram(struct brcmf_sdio_dev *sdiodev,
+				       struct chip_info *ci,
+				       char *nvram_dat, uint nvram_sz)
+{
+	int err;
+	u32 nvram_addr;
+	u32 token;
+	__le32 token_le;
+
+	nvram_addr = (ci->ramsize - 4) - nvram_sz + ci->rambase;
+
+	/* Write the vars list */
+	err = brcmf_sdio_ramrw(sdiodev, true, nvram_addr, nvram_dat, nvram_sz);
+	if (err) {
+		brcmf_err("error %d on writing %d nvram bytes at 0x%08x\n",
+			  err, nvram_sz, nvram_addr);
+		return false;
+	}
+
+	if (!brcmf_sdio_chip_verifynvram(sdiodev, nvram_addr,
+					 nvram_dat, nvram_sz))
+		return false;
+
+	/* generate token:
+	 * nvram size, converted to words, in lower 16-bits, checksum
+	 * in upper 16-bits.
+	 */
+	token = nvram_sz / 4;
+	token = (~token << 16) | (token & 0x0000FFFF);
+	token_le = cpu_to_le32(token);
+
+	brcmf_dbg(INFO, "RAM size: %d\n", ci->ramsize);
+	brcmf_dbg(INFO, "nvram is placed at %d, size %d, token=0x%08x\n",
+		  nvram_addr, nvram_sz, token);
+
+	/* Write the length token to the last word */
+	if (brcmf_sdio_ramrw(sdiodev, true, (ci->ramsize - 4 + ci->rambase),
+			     (u8 *)&token_le, 4))
+		return false;
+
+	return true;
+}
+
+static void
+brcmf_sdio_chip_cm3_enterdl(struct brcmf_sdio_dev *sdiodev,
+			    struct chip_info *ci)
+{
+	u32 zeros = 0;
+
+	ci->coredisable(sdiodev, ci, BCMA_CORE_ARM_CM3, 0);
+	ci->resetcore(sdiodev, ci, BCMA_CORE_INTERNAL_MEM, 0);
+
+	/* clear length token */
+	brcmf_sdio_ramrw(sdiodev, true, ci->ramsize - 4, (u8 *)&zeros, 4);
+}
+
+static bool
+brcmf_sdio_chip_cm3_exitdl(struct brcmf_sdio_dev *sdiodev, struct chip_info *ci,
+			   char *nvram_dat, uint nvram_sz)
+{
+	u8 core_idx;
+	u32 reg_addr;
+
+	if (!ci->iscoreup(sdiodev, ci, BCMA_CORE_INTERNAL_MEM)) {
+		brcmf_err("SOCRAM core is down after reset?\n");
+		return false;
+	}
+
+	if (!brcmf_sdio_chip_writenvram(sdiodev, ci, nvram_dat, nvram_sz))
+		return false;
+
+	/* clear all interrupts */
+	core_idx = brcmf_sdio_chip_getinfidx(ci, BCMA_CORE_SDIO_DEV);
+	reg_addr = ci->c_inf[core_idx].base;
+	reg_addr += offsetof(struct sdpcmd_regs, intstatus);
+	brcmf_sdio_regwl(sdiodev, reg_addr, 0xFFFFFFFF, NULL);
+
+	ci->resetcore(sdiodev, ci, BCMA_CORE_ARM_CM3, 0);
+
+	return true;
+}
+
+static inline void
+brcmf_sdio_chip_cr4_enterdl(struct brcmf_sdio_dev *sdiodev,
+			    struct chip_info *ci)
+{
+	ci->resetcore(sdiodev, ci, BCMA_CORE_ARM_CR4,
+		      ARMCR4_BCMA_IOCTL_CPUHALT);
+}
+
+static bool
+brcmf_sdio_chip_cr4_exitdl(struct brcmf_sdio_dev *sdiodev, struct chip_info *ci,
+			   char *nvram_dat, uint nvram_sz)
+{
+	u8 core_idx;
+	u32 reg_addr;
+
+	if (!brcmf_sdio_chip_writenvram(sdiodev, ci, nvram_dat, nvram_sz))
+		return false;
+
+	/* clear all interrupts */
+	core_idx = brcmf_sdio_chip_getinfidx(ci, BCMA_CORE_SDIO_DEV);
+	reg_addr = ci->c_inf[core_idx].base;
+	reg_addr += offsetof(struct sdpcmd_regs, intstatus);
+	brcmf_sdio_regwl(sdiodev, reg_addr, 0xFFFFFFFF, NULL);
+
+	/* Write reset vector to address 0 */
+	brcmf_sdio_ramrw(sdiodev, true, 0, (void *)&ci->rst_vec,
+			 sizeof(ci->rst_vec));
+
+	/* restore ARM */
+	ci->resetcore(sdiodev, ci, BCMA_CORE_ARM_CR4, 0);
+
+	return true;
+}
+
+void brcmf_sdio_chip_enter_download(struct brcmf_sdio_dev *sdiodev,
+				    struct chip_info *ci)
+{
+	u8 arm_core_idx;
+
+	arm_core_idx = brcmf_sdio_chip_getinfidx(ci, BCMA_CORE_ARM_CM3);
+	if (BRCMF_MAX_CORENUM != arm_core_idx) {
+		brcmf_sdio_chip_cm3_enterdl(sdiodev, ci);
+		return;
+	}
+
+	brcmf_sdio_chip_cr4_enterdl(sdiodev, ci);
+}
+
+bool brcmf_sdio_chip_exit_download(struct brcmf_sdio_dev *sdiodev,
+				   struct chip_info *ci, char *nvram_dat,
+				   uint nvram_sz)
+{
+	u8 arm_core_idx;
+
+	arm_core_idx = brcmf_sdio_chip_getinfidx(ci, BCMA_CORE_ARM_CM3);
+	if (BRCMF_MAX_CORENUM != arm_core_idx)
+		return brcmf_sdio_chip_cm3_exitdl(sdiodev, ci, nvram_dat,
+						  nvram_sz);
+
+	return brcmf_sdio_chip_cr4_exitdl(sdiodev, ci, nvram_dat, nvram_sz);
+}
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.h b/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.h
index ce974d76bd92..83c041f1bf4a 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.h
@@ -73,15 +73,17 @@ struct chip_info {
 	u32 pmurev;
 	u32 pmucaps;
 	u32 ramsize;
+	u32 rambase;
+	u32 rst_vec;	/* reset vertor for ARM CR4 core */
 
 	bool (*iscoreup)(struct brcmf_sdio_dev *sdiodev, struct chip_info *ci,
 			 u16 coreid);
 	u32 (*corerev)(struct brcmf_sdio_dev *sdiodev, struct chip_info *ci,
 			 u16 coreid);
 	void (*coredisable)(struct brcmf_sdio_dev *sdiodev,
-			struct chip_info *ci, u16 coreid);
+			struct chip_info *ci, u16 coreid, u32 core_bits);
 	void (*resetcore)(struct brcmf_sdio_dev *sdiodev,
-			struct chip_info *ci, u16 coreid);
+			struct chip_info *ci, u16 coreid, u32 core_bits);
 };
 
 struct sbconfig {
@@ -124,6 +126,95 @@ struct sbconfig {
 	u32 sbidhigh;	/* identification */
 };
 
+/* sdio core registers */
+struct sdpcmd_regs {
+	u32 corecontrol;		/* 0x00, rev8 */
+	u32 corestatus;			/* rev8 */
+	u32 PAD[1];
+	u32 biststatus;			/* rev8 */
+
+	/* PCMCIA access */
+	u16 pcmciamesportaladdr;	/* 0x010, rev8 */
+	u16 PAD[1];
+	u16 pcmciamesportalmask;	/* rev8 */
+	u16 PAD[1];
+	u16 pcmciawrframebc;		/* rev8 */
+	u16 PAD[1];
+	u16 pcmciaunderflowtimer;	/* rev8 */
+	u16 PAD[1];
+
+	/* interrupt */
+	u32 intstatus;			/* 0x020, rev8 */
+	u32 hostintmask;		/* rev8 */
+	u32 intmask;			/* rev8 */
+	u32 sbintstatus;		/* rev8 */
+	u32 sbintmask;			/* rev8 */
+	u32 funcintmask;		/* rev4 */
+	u32 PAD[2];
+	u32 tosbmailbox;		/* 0x040, rev8 */
+	u32 tohostmailbox;		/* rev8 */
+	u32 tosbmailboxdata;		/* rev8 */
+	u32 tohostmailboxdata;		/* rev8 */
+
+	/* synchronized access to registers in SDIO clock domain */
+	u32 sdioaccess;			/* 0x050, rev8 */
+	u32 PAD[3];
+
+	/* PCMCIA frame control */
+	u8 pcmciaframectrl;		/* 0x060, rev8 */
+	u8 PAD[3];
+	u8 pcmciawatermark;		/* rev8 */
+	u8 PAD[155];
+
+	/* interrupt batching control */
+	u32 intrcvlazy;			/* 0x100, rev8 */
+	u32 PAD[3];
+
+	/* counters */
+	u32 cmd52rd;			/* 0x110, rev8 */
+	u32 cmd52wr;			/* rev8 */
+	u32 cmd53rd;			/* rev8 */
+	u32 cmd53wr;			/* rev8 */
+	u32 abort;			/* rev8 */
+	u32 datacrcerror;		/* rev8 */
+	u32 rdoutofsync;		/* rev8 */
+	u32 wroutofsync;		/* rev8 */
+	u32 writebusy;			/* rev8 */
+	u32 readwait;			/* rev8 */
+	u32 readterm;			/* rev8 */
+	u32 writeterm;			/* rev8 */
+	u32 PAD[40];
+	u32 clockctlstatus;		/* rev8 */
+	u32 PAD[7];
+
+	u32 PAD[128];			/* DMA engines */
+
+	/* SDIO/PCMCIA CIS region */
+	char cis[512];			/* 0x400-0x5ff, rev6 */
+
+	/* PCMCIA function control registers */
+	char pcmciafcr[256];		/* 0x600-6ff, rev6 */
+	u16 PAD[55];
+
+	/* PCMCIA backplane access */
+	u16 backplanecsr;		/* 0x76E, rev6 */
+	u16 backplaneaddr0;		/* rev6 */
+	u16 backplaneaddr1;		/* rev6 */
+	u16 backplaneaddr2;		/* rev6 */
+	u16 backplaneaddr3;		/* rev6 */
+	u16 backplanedata0;		/* rev6 */
+	u16 backplanedata1;		/* rev6 */
+	u16 backplanedata2;		/* rev6 */
+	u16 backplanedata3;		/* rev6 */
+	u16 PAD[31];
+
+	/* sprom "size" & "blank" info */
+	u16 spromstatus;		/* 0x7BE, rev2 */
+	u32 PAD[464];
+
+	u16 PAD[0x80];
+};
+
 extern int brcmf_sdio_chip_attach(struct brcmf_sdio_dev *sdiodev,
 				  struct chip_info **ci_ptr, u32 regs);
 extern void brcmf_sdio_chip_detach(struct chip_info **ci_ptr);
@@ -131,6 +222,10 @@ extern void brcmf_sdio_chip_drivestrengthinit(struct brcmf_sdio_dev *sdiodev,
 					      struct chip_info *ci,
 					      u32 drivestrength);
 extern u8 brcmf_sdio_chip_getinfidx(struct chip_info *ci, u16 coreid);
-
+extern void brcmf_sdio_chip_enter_download(struct brcmf_sdio_dev *sdiodev,
+					   struct chip_info *ci);
+extern bool brcmf_sdio_chip_exit_download(struct brcmf_sdio_dev *sdiodev,
+					  struct chip_info *ci, char *nvram_dat,
+					  uint nvram_sz);
 
 #endif		/* _BRCMFMAC_SDIO_CHIP_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h b/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h
index 0d30afd8c672..7c1b6332747e 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h
@@ -48,7 +48,13 @@
 #define SBSDIO_NUM_FUNCTION		3
 
 /* function 0 vendor specific CCCR registers */
-#define SDIO_CCCR_BRCM_SEPINT		0xf2
+#define SDIO_CCCR_BRCM_CARDCAP			0xf0
+#define SDIO_CCCR_BRCM_CARDCAP_CMD14_SUPPORT	0x02
+#define SDIO_CCCR_BRCM_CARDCAP_CMD14_EXT	0x04
+#define SDIO_CCCR_BRCM_CARDCAP_CMD_NODEC	0x08
+#define SDIO_CCCR_BRCM_CARDCTRL		0xf1
+#define SDIO_CCCR_BRCM_CARDCTRL_WLANRESET	0x02
+#define SDIO_CCCR_BRCM_SEPINT			0xf2
 
 #define  SDIO_SEPINT_MASK		0x01
 #define  SDIO_SEPINT_OE			0x02
@@ -97,9 +103,23 @@
 #define SBSDIO_FUNC1_RFRAMEBCLO		0x1001B
 /* Read Frame Byte Count High */
 #define SBSDIO_FUNC1_RFRAMEBCHI		0x1001C
+/* MesBusyCtl (rev 11) */
+#define SBSDIO_FUNC1_MESBUSYCTRL	0x1001D
+/* Sdio Core Rev 12 */
+#define SBSDIO_FUNC1_WAKEUPCTRL		0x1001E
+#define SBSDIO_FUNC1_WCTRL_ALPWAIT_MASK		0x1
+#define SBSDIO_FUNC1_WCTRL_ALPWAIT_SHIFT	0
+#define SBSDIO_FUNC1_WCTRL_HTWAIT_MASK		0x2
+#define SBSDIO_FUNC1_WCTRL_HTWAIT_SHIFT		1
+#define SBSDIO_FUNC1_SLEEPCSR		0x1001F
+#define SBSDIO_FUNC1_SLEEPCSR_KSO_MASK		0x1
+#define SBSDIO_FUNC1_SLEEPCSR_KSO_SHIFT		0
+#define SBSDIO_FUNC1_SLEEPCSR_KSO_EN		1
+#define SBSDIO_FUNC1_SLEEPCSR_DEVON_MASK	0x2
+#define SBSDIO_FUNC1_SLEEPCSR_DEVON_SHIFT	1
 
 #define SBSDIO_FUNC1_MISC_REG_START	0x10000	/* f1 misc register start */
-#define SBSDIO_FUNC1_MISC_REG_LIMIT	0x1001C	/* f1 misc register end */
+#define SBSDIO_FUNC1_MISC_REG_LIMIT	0x1001F	/* f1 misc register end */
 
 /* function 1 OCP space */
 
@@ -154,13 +174,11 @@ struct brcmf_sdio_dev {
 	wait_queue_head_t request_buffer_wait;
 	struct device *dev;
 	struct brcmf_bus *bus_if;
-#ifdef CONFIG_BRCMFMAC_SDIO_OOB
-	unsigned int irq;		/* oob interrupt number */
-	unsigned long irq_flags;	/* board specific oob flags */
+	struct brcmfmac_sdio_platform_data *pdata;
+	bool oob_irq_requested;
 	bool irq_en;			/* irq enable flags */
 	spinlock_t irq_en_lock;
 	bool irq_wake;			/* irq wake enable flags */
-#endif		/* CONFIG_BRCMFMAC_SDIO_OOB */
 };
 
 /* Register/deregister interrupt handler. */
@@ -224,6 +242,8 @@ brcmf_sdcard_recv_chain(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
  */
 extern int brcmf_sdcard_rwdata(struct brcmf_sdio_dev *sdiodev, uint rw,
 			       u32 addr, u8 *buf, uint nbytes);
+extern int brcmf_sdio_ramrw(struct brcmf_sdio_dev *sdiodev, bool write,
+			    u32 address, u8 *data, uint size);
 
 /* Issue an abort to the specified function */
 extern int brcmf_sdcard_abort(struct brcmf_sdio_dev *sdiodev, uint fn);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/tracepoint.c b/drivers/net/wireless/brcm80211/brcmfmac/tracepoint.c
new file mode 100644
index 000000000000..b505db48c60d
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmfmac/tracepoint.c
@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 2012 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <linux/module.h> /* bug in tracepoint.h, it should include this */
+
+#ifndef __CHECKER__
+#define CREATE_TRACE_POINTS
+#include "tracepoint.h"
+#endif
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/tracepoint.h b/drivers/net/wireless/brcm80211/brcmfmac/tracepoint.h
new file mode 100644
index 000000000000..9df1f7a681e0
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmfmac/tracepoint.h
@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2013 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#if !defined(BRCMF_TRACEPOINT_H_) || defined(TRACE_HEADER_MULTI_READ)
+#define BRCMF_TRACEPOINT_H_
+
+#include <linux/types.h>
+#include <linux/tracepoint.h>
+
+#ifndef CONFIG_BRCM_TRACING
+
+#undef TRACE_EVENT
+#define TRACE_EVENT(name, proto, ...) \
+static inline void trace_ ## name(proto) {}
+
+#undef DECLARE_EVENT_CLASS
+#define DECLARE_EVENT_CLASS(...)
+
+#undef DEFINE_EVENT
+#define DEFINE_EVENT(evt_class, name, proto, ...) \
+static inline void trace_ ## name(proto) {}
+
+#endif /* CONFIG_BRCM_TRACING */
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM	brcmfmac
+
+#define MAX_MSG_LEN		100
+
+TRACE_EVENT(brcmf_err,
+	TP_PROTO(const char *func, struct va_format *vaf),
+	TP_ARGS(func, vaf),
+	TP_STRUCT__entry(
+		__string(func, func)
+		__dynamic_array(char, msg, MAX_MSG_LEN)
+	),
+	TP_fast_assign(
+		__assign_str(func, func);
+		WARN_ON_ONCE(vsnprintf(__get_dynamic_array(msg),
+				       MAX_MSG_LEN, vaf->fmt,
+				       *vaf->va) >= MAX_MSG_LEN);
+	),
+	TP_printk("%s: %s", __get_str(func), __get_str(msg))
+);
+
+TRACE_EVENT(brcmf_dbg,
+	TP_PROTO(u32 level, const char *func, struct va_format *vaf),
+	TP_ARGS(level, func, vaf),
+	TP_STRUCT__entry(
+		__field(u32, level)
+		__string(func, func)
+		__dynamic_array(char, msg, MAX_MSG_LEN)
+	),
+	TP_fast_assign(
+		__entry->level = level;
+		__assign_str(func, func);
+		WARN_ON_ONCE(vsnprintf(__get_dynamic_array(msg),
+				       MAX_MSG_LEN, vaf->fmt,
+				       *vaf->va) >= MAX_MSG_LEN);
+	),
+	TP_printk("%s: %s", __get_str(func), __get_str(msg))
+);
+
+TRACE_EVENT(brcmf_hexdump,
+	TP_PROTO(void *data, size_t len),
+	TP_ARGS(data, len),
+	TP_STRUCT__entry(
+		__field(unsigned long, len)
+		__dynamic_array(u8, hdata, len)
+	),
+	TP_fast_assign(
+		__entry->len = len;
+		memcpy(__get_dynamic_array(hdata), data, len);
+	),
+	TP_printk("hexdump [length=%lu]", __entry->len)
+);
+
+#ifdef CONFIG_BRCM_TRACING
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE tracepoint
+
+#include <trace/define_trace.h>
+
+#endif /* CONFIG_BRCM_TRACING */
+
+#endif /* BRCMF_TRACEPOINT_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/usb.c b/drivers/net/wireless/brcm80211/brcmfmac/usb.c
index 42289e9ea886..01aed7ad6bec 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/usb.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/usb.c
@@ -112,11 +112,6 @@ struct brcmf_usbdev_info {
 static void brcmf_usb_rx_refill(struct brcmf_usbdev_info *devinfo,
 				struct brcmf_usbreq  *req);
 
-MODULE_AUTHOR("Broadcom Corporation");
-MODULE_DESCRIPTION("Broadcom 802.11n wireless LAN fullmac usb driver.");
-MODULE_SUPPORTED_DEVICE("Broadcom 802.11n WLAN fullmac usb cards");
-MODULE_LICENSE("Dual BSD/GPL");
-
 static struct brcmf_usbdev *brcmf_usb_get_buspub(struct device *dev)
 {
 	struct brcmf_bus *bus_if = dev_get_drvdata(dev);
@@ -422,8 +417,6 @@ static void brcmf_usb_tx_complete(struct urb *urb)
 	brcmf_usb_del_fromq(devinfo, req);
 
 	brcmf_txcomplete(devinfo->dev, req->skb, urb->status == 0);
-
-	brcmu_pkt_buf_free_skb(req->skb);
 	req->skb = NULL;
 	brcmf_usb_enq(devinfo, &devinfo->tx_freeq, req, &devinfo->tx_freecount);
 	if (devinfo->tx_freecount > devinfo->tx_high_watermark &&
@@ -577,15 +570,17 @@ static int brcmf_usb_tx(struct device *dev, struct sk_buff *skb)
 	int ret;
 
 	brcmf_dbg(USB, "Enter, skb=%p\n", skb);
-	if (devinfo->bus_pub.state != BRCMFMAC_USB_STATE_UP)
-		return -EIO;
+	if (devinfo->bus_pub.state != BRCMFMAC_USB_STATE_UP) {
+		ret = -EIO;
+		goto fail;
+	}
 
 	req = brcmf_usb_deq(devinfo, &devinfo->tx_freeq,
 					&devinfo->tx_freecount);
 	if (!req) {
-		brcmu_pkt_buf_free_skb(skb);
 		brcmf_err("no req to send\n");
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto fail;
 	}
 
 	req->skb = skb;
@@ -598,18 +593,21 @@ static int brcmf_usb_tx(struct device *dev, struct sk_buff *skb)
 	if (ret) {
 		brcmf_err("brcmf_usb_tx usb_submit_urb FAILED\n");
 		brcmf_usb_del_fromq(devinfo, req);
-		brcmu_pkt_buf_free_skb(req->skb);
 		req->skb = NULL;
 		brcmf_usb_enq(devinfo, &devinfo->tx_freeq, req,
-						&devinfo->tx_freecount);
-	} else {
-		if (devinfo->tx_freecount < devinfo->tx_low_watermark &&
-			!devinfo->tx_flowblock) {
-			brcmf_txflowblock(dev, true);
-			devinfo->tx_flowblock = true;
-		}
+			      &devinfo->tx_freecount);
+		goto fail;
 	}
 
+	if (devinfo->tx_freecount < devinfo->tx_low_watermark &&
+	    !devinfo->tx_flowblock) {
+		brcmf_txflowblock(dev, true);
+		devinfo->tx_flowblock = true;
+	}
+	return 0;
+
+fail:
+	brcmf_txcomplete(dev, skb, false);
 	return ret;
 }
 
@@ -1485,6 +1483,7 @@ static struct usb_device_id brcmf_usb_devid_table[] = {
 	{ USB_DEVICE(BRCMF_USB_VENDOR_ID_BROADCOM, BRCMF_USB_DEVICE_ID_BCMFW) },
 	{ }
 };
+
 MODULE_DEVICE_TABLE(usb, brcmf_usb_devid_table);
 MODULE_FIRMWARE(BRCMF_USB_43143_FW_NAME);
 MODULE_FIRMWARE(BRCMF_USB_43236_FW_NAME);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
index 78da3eff75e8..6d758f285352 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
@@ -26,8 +26,10 @@
 #include <brcmu_wifi.h>
 #include "dhd.h"
 #include "dhd_dbg.h"
+#include "tracepoint.h"
 #include "fwil_types.h"
 #include "p2p.h"
+#include "btcoex.h"
 #include "wl_cfg80211.h"
 #include "fwil.h"
 
@@ -182,64 +184,6 @@ static struct ieee80211_channel __wl_5ghz_a_channels[] = {
 	CHAN5G(216, 0),
 };
 
-static struct ieee80211_channel __wl_5ghz_n_channels[] = {
-	CHAN5G(32, 0), CHAN5G(34, 0),
-	CHAN5G(36, 0), CHAN5G(38, 0),
-	CHAN5G(40, 0), CHAN5G(42, 0),
-	CHAN5G(44, 0), CHAN5G(46, 0),
-	CHAN5G(48, 0), CHAN5G(50, 0),
-	CHAN5G(52, 0), CHAN5G(54, 0),
-	CHAN5G(56, 0), CHAN5G(58, 0),
-	CHAN5G(60, 0), CHAN5G(62, 0),
-	CHAN5G(64, 0), CHAN5G(66, 0),
-	CHAN5G(68, 0), CHAN5G(70, 0),
-	CHAN5G(72, 0), CHAN5G(74, 0),
-	CHAN5G(76, 0), CHAN5G(78, 0),
-	CHAN5G(80, 0), CHAN5G(82, 0),
-	CHAN5G(84, 0), CHAN5G(86, 0),
-	CHAN5G(88, 0), CHAN5G(90, 0),
-	CHAN5G(92, 0), CHAN5G(94, 0),
-	CHAN5G(96, 0), CHAN5G(98, 0),
-	CHAN5G(100, 0), CHAN5G(102, 0),
-	CHAN5G(104, 0), CHAN5G(106, 0),
-	CHAN5G(108, 0), CHAN5G(110, 0),
-	CHAN5G(112, 0), CHAN5G(114, 0),
-	CHAN5G(116, 0), CHAN5G(118, 0),
-	CHAN5G(120, 0), CHAN5G(122, 0),
-	CHAN5G(124, 0), CHAN5G(126, 0),
-	CHAN5G(128, 0), CHAN5G(130, 0),
-	CHAN5G(132, 0), CHAN5G(134, 0),
-	CHAN5G(136, 0), CHAN5G(138, 0),
-	CHAN5G(140, 0), CHAN5G(142, 0),
-	CHAN5G(144, 0), CHAN5G(145, 0),
-	CHAN5G(146, 0), CHAN5G(147, 0),
-	CHAN5G(148, 0), CHAN5G(149, 0),
-	CHAN5G(150, 0), CHAN5G(151, 0),
-	CHAN5G(152, 0), CHAN5G(153, 0),
-	CHAN5G(154, 0), CHAN5G(155, 0),
-	CHAN5G(156, 0), CHAN5G(157, 0),
-	CHAN5G(158, 0), CHAN5G(159, 0),
-	CHAN5G(160, 0), CHAN5G(161, 0),
-	CHAN5G(162, 0), CHAN5G(163, 0),
-	CHAN5G(164, 0), CHAN5G(165, 0),
-	CHAN5G(166, 0), CHAN5G(168, 0),
-	CHAN5G(170, 0), CHAN5G(172, 0),
-	CHAN5G(174, 0), CHAN5G(176, 0),
-	CHAN5G(178, 0), CHAN5G(180, 0),
-	CHAN5G(182, 0), CHAN5G(184, 0),
-	CHAN5G(186, 0), CHAN5G(188, 0),
-	CHAN5G(190, 0), CHAN5G(192, 0),
-	CHAN5G(194, 0), CHAN5G(196, 0),
-	CHAN5G(198, 0), CHAN5G(200, 0),
-	CHAN5G(202, 0), CHAN5G(204, 0),
-	CHAN5G(206, 0), CHAN5G(208, 0),
-	CHAN5G(210, 0), CHAN5G(212, 0),
-	CHAN5G(214, 0), CHAN5G(216, 0),
-	CHAN5G(218, 0), CHAN5G(220, 0),
-	CHAN5G(222, 0), CHAN5G(224, 0),
-	CHAN5G(226, 0), CHAN5G(228, 0),
-};
-
 static struct ieee80211_supported_band __wl_band_2ghz = {
 	.band = IEEE80211_BAND_2GHZ,
 	.channels = __wl_2ghz_channels,
@@ -256,12 +200,28 @@ static struct ieee80211_supported_band __wl_band_5ghz_a = {
 	.n_bitrates = wl_a_rates_size,
 };
 
-static struct ieee80211_supported_band __wl_band_5ghz_n = {
-	.band = IEEE80211_BAND_5GHZ,
-	.channels = __wl_5ghz_n_channels,
-	.n_channels = ARRAY_SIZE(__wl_5ghz_n_channels),
-	.bitrates = wl_a_rates,
-	.n_bitrates = wl_a_rates_size,
+/* This is to override regulatory domains defined in cfg80211 module (reg.c)
+ * By default world regulatory domain defined in reg.c puts the flags
+ * NL80211_RRF_PASSIVE_SCAN and NL80211_RRF_NO_IBSS for 5GHz channels (for
+ * 36..48 and 149..165). With respect to these flags, wpa_supplicant doesn't
+ * start p2p operations on 5GHz channels. All the changes in world regulatory
+ * domain are to be done here.
+ */
+static const struct ieee80211_regdomain brcmf_regdom = {
+	.n_reg_rules = 4,
+	.alpha2 =  "99",
+	.reg_rules = {
+		/* IEEE 802.11b/g, channels 1..11 */
+		REG_RULE(2412-10, 2472+10, 40, 6, 20, 0),
+		/* If any */
+		/* IEEE 802.11 channel 14 - Only JP enables
+		 * this and for 802.11b only
+		 */
+		REG_RULE(2484-10, 2484+10, 20, 6, 20, 0),
+		/* IEEE 802.11a, channel 36..64 */
+		REG_RULE(5150-10, 5350+10, 40, 6, 20, 0),
+		/* IEEE 802.11a, channel 100..165 */
+		REG_RULE(5470-10, 5850+10, 40, 6, 20, 0), }
 };
 
 static const u32 __wl_cipher_suites[] = {
@@ -375,22 +335,16 @@ static u8 brcmf_mw_to_qdbm(u16 mw)
 	return qdbm;
 }
 
-u16 channel_to_chanspec(struct ieee80211_channel *ch)
+u16 channel_to_chanspec(struct brcmu_d11inf *d11inf,
+			struct ieee80211_channel *ch)
 {
-	u16 chanspec;
+	struct brcmu_chan ch_inf;
 
-	chanspec = ieee80211_frequency_to_channel(ch->center_freq);
-	chanspec &= WL_CHANSPEC_CHAN_MASK;
+	ch_inf.chnum = ieee80211_frequency_to_channel(ch->center_freq);
+	ch_inf.bw = BRCMU_CHAN_BW_20;
+	d11inf->encchspec(&ch_inf);
 
-	if (ch->band == IEEE80211_BAND_2GHZ)
-		chanspec |= WL_CHANSPEC_BAND_2G;
-	else
-		chanspec |= WL_CHANSPEC_BAND_5G;
-
-	chanspec |= WL_CHANSPEC_BW_20;
-	chanspec |= WL_CHANSPEC_CTL_SB_NONE;
-
-	return chanspec;
+	return ch_inf.chspec;
 }
 
 /* Traverse a string of 1-byte tag/1-byte length/variable-length value
@@ -523,17 +477,16 @@ static struct wireless_dev *brcmf_cfg80211_add_iface(struct wiphy *wiphy,
 		return ERR_PTR(-EOPNOTSUPP);
 	case NL80211_IFTYPE_P2P_CLIENT:
 	case NL80211_IFTYPE_P2P_GO:
+	case NL80211_IFTYPE_P2P_DEVICE:
 		return brcmf_p2p_add_vif(wiphy, name, type, flags, params);
 	case NL80211_IFTYPE_UNSPECIFIED:
-	case NL80211_IFTYPE_P2P_DEVICE:
 	default:
 		return ERR_PTR(-EINVAL);
 	}
 }
 
-void brcmf_set_mpc(struct net_device *ndev, int mpc)
+void brcmf_set_mpc(struct brcmf_if *ifp, int mpc)
 {
-	struct brcmf_if *ifp = netdev_priv(ndev);
 	s32 err = 0;
 
 	if (check_vif_up(ifp->vif)) {
@@ -546,10 +499,9 @@ void brcmf_set_mpc(struct net_device *ndev, int mpc)
 	}
 }
 
-s32
-brcmf_notify_escan_complete(struct brcmf_cfg80211_info *cfg,
-			    struct net_device *ndev,
-			    bool aborted, bool fw_abort)
+s32 brcmf_notify_escan_complete(struct brcmf_cfg80211_info *cfg,
+				struct brcmf_if *ifp, bool aborted,
+				bool fw_abort)
 {
 	struct brcmf_scan_params_le params_le;
 	struct cfg80211_scan_request *scan_request;
@@ -580,7 +532,7 @@ brcmf_notify_escan_complete(struct brcmf_cfg80211_info *cfg,
 		/* Scan is aborted by setting channel_list[0] to -1 */
 		params_le.channel_list[0] = cpu_to_le16(-1);
 		/* E-Scan (or anyother type) can be aborted by SCAN */
-		err = brcmf_fil_cmd_data_set(netdev_priv(ndev), BRCMF_C_SCAN,
+		err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SCAN,
 					     &params_le, sizeof(params_le));
 		if (err)
 			brcmf_err("Scan abort  failed\n");
@@ -594,12 +546,12 @@ brcmf_notify_escan_complete(struct brcmf_cfg80211_info *cfg,
 		cfg->sched_escan = false;
 		if (!aborted)
 			cfg80211_sched_scan_results(cfg_to_wiphy(cfg));
-		brcmf_set_mpc(ndev, 1);
+		brcmf_set_mpc(ifp, 1);
 	} else if (scan_request) {
 		brcmf_dbg(SCAN, "ESCAN Completed scan: %s\n",
 			  aborted ? "Aborted" : "Done");
 		cfg80211_scan_done(scan_request, aborted);
-		brcmf_set_mpc(ndev, 1);
+		brcmf_set_mpc(ifp, 1);
 	}
 	if (!test_and_clear_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status))
 		brcmf_dbg(SCAN, "Scan complete, probably P2P scan\n");
@@ -619,9 +571,9 @@ int brcmf_cfg80211_del_iface(struct wiphy *wiphy, struct wireless_dev *wdev)
 
 	if (ndev) {
 		if (test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status) &&
-		    cfg->escan_info.ndev == ndev)
-			brcmf_notify_escan_complete(cfg, ndev, true,
-						    true);
+		    cfg->escan_info.ifp == netdev_priv(ndev))
+			brcmf_notify_escan_complete(cfg, netdev_priv(ndev),
+						    true, true);
 
 		brcmf_fil_iovar_int_set(netdev_priv(ndev), "mpc", 1);
 	}
@@ -637,9 +589,9 @@ int brcmf_cfg80211_del_iface(struct wiphy *wiphy, struct wireless_dev *wdev)
 		return -EOPNOTSUPP;
 	case NL80211_IFTYPE_P2P_CLIENT:
 	case NL80211_IFTYPE_P2P_GO:
+	case NL80211_IFTYPE_P2P_DEVICE:
 		return brcmf_p2p_del_vif(wiphy, wdev);
 	case NL80211_IFTYPE_UNSPECIFIED:
-	case NL80211_IFTYPE_P2P_DEVICE:
 	default:
 		return -EINVAL;
 	}
@@ -723,7 +675,8 @@ done:
 	return err;
 }
 
-static void brcmf_escan_prep(struct brcmf_scan_params_le *params_le,
+static void brcmf_escan_prep(struct brcmf_cfg80211_info *cfg,
+			     struct brcmf_scan_params_le *params_le,
 			     struct cfg80211_scan_request *request)
 {
 	u32 n_ssids;
@@ -755,7 +708,8 @@ static void brcmf_escan_prep(struct brcmf_scan_params_le *params_le,
 		  n_channels);
 	if (n_channels > 0) {
 		for (i = 0; i < n_channels; i++) {
-			chanspec = channel_to_chanspec(request->channels[i]);
+			chanspec = channel_to_chanspec(&cfg->d11inf,
+						       request->channels[i]);
 			brcmf_dbg(SCAN, "Chan : %d, Channel spec: %x\n",
 				  request->channels[i]->hw_value, chanspec);
 			params_le->channel_list[i] = cpu_to_le16(chanspec);
@@ -803,7 +757,7 @@ static void brcmf_escan_prep(struct brcmf_scan_params_le *params_le,
 }
 
 static s32
-brcmf_run_escan(struct brcmf_cfg80211_info *cfg, struct net_device *ndev,
+brcmf_run_escan(struct brcmf_cfg80211_info *cfg, struct brcmf_if *ifp,
 		struct cfg80211_scan_request *request, u16 action)
 {
 	s32 params_size = BRCMF_SCAN_PARAMS_FIXED_SIZE +
@@ -827,13 +781,12 @@ brcmf_run_escan(struct brcmf_cfg80211_info *cfg, struct net_device *ndev,
 		goto exit;
 	}
 	BUG_ON(params_size + sizeof("escan") >= BRCMF_DCMD_MEDLEN);
-	brcmf_escan_prep(&params->params_le, request);
+	brcmf_escan_prep(cfg, &params->params_le, request);
 	params->version = cpu_to_le32(BRCMF_ESCAN_REQ_VERSION);
 	params->action = cpu_to_le16(action);
 	params->sync_id = cpu_to_le16(0x1234);
 
-	err = brcmf_fil_iovar_data_set(netdev_priv(ndev), "escan",
-				       params, params_size);
+	err = brcmf_fil_iovar_data_set(ifp, "escan", params, params_size);
 	if (err) {
 		if (err == -EBUSY)
 			brcmf_dbg(INFO, "system busy : escan canceled\n");
@@ -848,7 +801,7 @@ exit:
 
 static s32
 brcmf_do_escan(struct brcmf_cfg80211_info *cfg, struct wiphy *wiphy,
-	       struct net_device *ndev, struct cfg80211_scan_request *request)
+	       struct brcmf_if *ifp, struct cfg80211_scan_request *request)
 {
 	s32 err;
 	u32 passive_scan;
@@ -856,35 +809,35 @@ brcmf_do_escan(struct brcmf_cfg80211_info *cfg, struct wiphy *wiphy,
 	struct escan_info *escan = &cfg->escan_info;
 
 	brcmf_dbg(SCAN, "Enter\n");
-	escan->ndev = ndev;
+	escan->ifp = ifp;
 	escan->wiphy = wiphy;
 	escan->escan_state = WL_ESCAN_STATE_SCANNING;
 	passive_scan = cfg->active_scan ? 0 : 1;
-	err = brcmf_fil_cmd_int_set(netdev_priv(ndev), BRCMF_C_SET_PASSIVE_SCAN,
+	err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_PASSIVE_SCAN,
 				    passive_scan);
 	if (err) {
 		brcmf_err("error (%d)\n", err);
 		return err;
 	}
-	brcmf_set_mpc(ndev, 0);
+	brcmf_set_mpc(ifp, 0);
 	results = (struct brcmf_scan_results *)cfg->escan_info.escan_buf;
 	results->version = 0;
 	results->count = 0;
 	results->buflen = WL_ESCAN_RESULTS_FIXED_SIZE;
 
-	err = escan->run(cfg, ndev, request, WL_ESCAN_ACTION_START);
+	err = escan->run(cfg, ifp, request, WL_ESCAN_ACTION_START);
 	if (err)
-		brcmf_set_mpc(ndev, 1);
+		brcmf_set_mpc(ifp, 1);
 	return err;
 }
 
 static s32
-brcmf_cfg80211_escan(struct wiphy *wiphy, struct net_device *ndev,
+brcmf_cfg80211_escan(struct wiphy *wiphy, struct brcmf_cfg80211_vif *vif,
 		     struct cfg80211_scan_request *request,
 		     struct cfg80211_ssid *this_ssid)
 {
-	struct brcmf_if *ifp = netdev_priv(ndev);
-	struct brcmf_cfg80211_info *cfg = ndev_to_cfg(ndev);
+	struct brcmf_if *ifp = vif->ifp;
+	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
 	struct cfg80211_ssid *ssids;
 	struct brcmf_cfg80211_scan_req *sr = &cfg->scan_req_int;
 	u32 passive_scan;
@@ -904,16 +857,19 @@ brcmf_cfg80211_escan(struct wiphy *wiphy, struct net_device *ndev,
 			  cfg->scan_status);
 		return -EAGAIN;
 	}
+	if (test_bit(BRCMF_SCAN_STATUS_SUPPRESS, &cfg->scan_status)) {
+		brcmf_err("Scanning suppressed: status (%lu)\n",
+			  cfg->scan_status);
+		return -EAGAIN;
+	}
 	if (test_bit(BRCMF_VIF_STATUS_CONNECTING, &ifp->vif->sme_state)) {
 		brcmf_err("Connecting: status (%lu)\n", ifp->vif->sme_state);
 		return -EAGAIN;
 	}
 
 	/* If scan req comes for p2p0, send it over primary I/F */
-	if (ifp->vif == cfg->p2p.bss_idx[P2PAPI_BSSCFG_DEVICE].vif) {
-		ifp = cfg->p2p.bss_idx[P2PAPI_BSSCFG_PRIMARY].vif->ifp;
-		ndev = ifp->ndev;
-	}
+	if (vif == cfg->p2p.bss_idx[P2PAPI_BSSCFG_DEVICE].vif)
+		vif = cfg->p2p.bss_idx[P2PAPI_BSSCFG_PRIMARY].vif;
 
 	/* Arm scan timeout timer */
 	mod_timer(&cfg->escan_timeout, jiffies +
@@ -934,11 +890,11 @@ brcmf_cfg80211_escan(struct wiphy *wiphy, struct net_device *ndev,
 	set_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status);
 	if (escan_req) {
 		cfg->escan_info.run = brcmf_run_escan;
-		err = brcmf_p2p_scan_prep(wiphy, request, ifp->vif);
+		err = brcmf_p2p_scan_prep(wiphy, request, vif);
 		if (err)
 			goto scan_out;
 
-		err = brcmf_do_escan(cfg, wiphy, ndev, request);
+		err = brcmf_do_escan(cfg, wiphy, vif->ifp, request);
 		if (err)
 			goto scan_out;
 	} else {
@@ -962,7 +918,7 @@ brcmf_cfg80211_escan(struct wiphy *wiphy, struct net_device *ndev,
 			brcmf_err("WLC_SET_PASSIVE_SCAN error (%d)\n", err);
 			goto scan_out;
 		}
-		brcmf_set_mpc(ndev, 0);
+		brcmf_set_mpc(ifp, 0);
 		err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SCAN,
 					     &sr->ssid_le, sizeof(sr->ssid_le));
 		if (err) {
@@ -972,7 +928,7 @@ brcmf_cfg80211_escan(struct wiphy *wiphy, struct net_device *ndev,
 			else
 				brcmf_err("WLC_SCAN error (%d)\n", err);
 
-			brcmf_set_mpc(ndev, 1);
+			brcmf_set_mpc(ifp, 1);
 			goto scan_out;
 		}
 	}
@@ -990,16 +946,15 @@ scan_out:
 static s32
 brcmf_cfg80211_scan(struct wiphy *wiphy, struct cfg80211_scan_request *request)
 {
-	struct net_device *ndev = request->wdev->netdev;
+	struct brcmf_cfg80211_vif *vif;
 	s32 err = 0;
 
 	brcmf_dbg(TRACE, "Enter\n");
-
-	if (!check_vif_up(container_of(request->wdev,
-				       struct brcmf_cfg80211_vif, wdev)))
+	vif = container_of(request->wdev, struct brcmf_cfg80211_vif, wdev);
+	if (!check_vif_up(vif))
 		return -EIO;
 
-	err = brcmf_cfg80211_escan(wiphy, ndev, request, NULL);
+	err = brcmf_cfg80211_escan(wiphy, vif, request, NULL);
 
 	if (err)
 		brcmf_err("scan error (%d)\n", err);
@@ -1097,6 +1052,7 @@ static void brcmf_init_prof(struct brcmf_cfg80211_profile *prof)
 
 static void brcmf_link_down(struct brcmf_cfg80211_vif *vif)
 {
+	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(vif->wdev.wiphy);
 	s32 err = 0;
 
 	brcmf_dbg(TRACE, "Enter\n");
@@ -1110,6 +1066,8 @@ static void brcmf_link_down(struct brcmf_cfg80211_vif *vif)
 		clear_bit(BRCMF_VIF_STATUS_CONNECTED, &vif->sme_state);
 	}
 	clear_bit(BRCMF_VIF_STATUS_CONNECTING, &vif->sme_state);
+	clear_bit(BRCMF_SCAN_STATUS_SUPPRESS, &cfg->scan_status);
+	brcmf_btcoex_set_mode(vif, BRCMF_BTCOEX_ENABLED, 0);
 	brcmf_dbg(TRACE, "Exit\n");
 }
 
@@ -1229,7 +1187,8 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
 				params->chandef.chan->center_freq);
 		if (params->channel_fixed) {
 			/* adding chanspec */
-			chanspec = channel_to_chanspec(params->chandef.chan);
+			chanspec = channel_to_chanspec(&cfg->d11inf,
+						       params->chandef.chan);
 			join_params.params_le.chanspec_list[0] =
 				cpu_to_le16(chanspec);
 			join_params.params_le.chanspec_num = cpu_to_le32(1);
@@ -1619,7 +1578,7 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
 	if (chan) {
 		cfg->channel =
 			ieee80211_frequency_to_channel(chan->center_freq);
-		chanspec = channel_to_chanspec(chan);
+		chanspec = channel_to_chanspec(&cfg->d11inf, chan);
 		brcmf_dbg(CONN, "channel=%d, center_req=%d, chanspec=0x%04x\n",
 			  cfg->channel, chan->center_freq, chanspec);
 	} else {
@@ -2278,6 +2237,7 @@ static s32 brcmf_inform_single_bss(struct brcmf_cfg80211_info *cfg,
 	struct ieee80211_channel *notify_channel;
 	struct cfg80211_bss *bss;
 	struct ieee80211_supported_band *band;
+	struct brcmu_chan ch;
 	s32 err = 0;
 	u16 channel;
 	u32 freq;
@@ -2292,8 +2252,12 @@ static s32 brcmf_inform_single_bss(struct brcmf_cfg80211_info *cfg,
 		return 0;
 	}
 
-	channel = bi->ctl_ch ? bi->ctl_ch :
-				CHSPEC_CHANNEL(le16_to_cpu(bi->chanspec));
+	if (!bi->ctl_ch) {
+		ch.chspec = le16_to_cpu(bi->chanspec);
+		cfg->d11inf.decchspec(&ch);
+		bi->ctl_ch = ch.chnum;
+	}
+	channel = bi->ctl_ch;
 
 	if (channel <= CH_MAX_2G_CHANNEL)
 		band = wiphy->bands[IEEE80211_BAND_2GHZ];
@@ -2368,9 +2332,9 @@ static s32 wl_inform_ibss(struct brcmf_cfg80211_info *cfg,
 	struct brcmf_bss_info_le *bi = NULL;
 	struct ieee80211_supported_band *band;
 	struct cfg80211_bss *bss;
+	struct brcmu_chan ch;
 	u8 *buf = NULL;
 	s32 err = 0;
-	u16 channel;
 	u32 freq;
 	u16 notify_capability;
 	u16 notify_interval;
@@ -2397,15 +2361,15 @@ static s32 wl_inform_ibss(struct brcmf_cfg80211_info *cfg,
 
 	bi = (struct brcmf_bss_info_le *)(buf + 4);
 
-	channel = bi->ctl_ch ? bi->ctl_ch :
-				CHSPEC_CHANNEL(le16_to_cpu(bi->chanspec));
+	ch.chspec = le16_to_cpu(bi->chanspec);
+	cfg->d11inf.decchspec(&ch);
 
-	if (channel <= CH_MAX_2G_CHANNEL)
+	if (ch.band == BRCMU_CHAN_BAND_2G)
 		band = wiphy->bands[IEEE80211_BAND_2GHZ];
 	else
 		band = wiphy->bands[IEEE80211_BAND_5GHZ];
 
-	freq = ieee80211_channel_to_frequency(channel, band->band);
+	freq = ieee80211_channel_to_frequency(ch.chnum, band->band);
 	notify_channel = ieee80211_get_channel(wiphy, freq);
 
 	notify_capability = le16_to_cpu(bi->capability);
@@ -2414,7 +2378,7 @@ static s32 wl_inform_ibss(struct brcmf_cfg80211_info *cfg,
 	notify_ielen = le32_to_cpu(bi->ie_length);
 	notify_signal = (s16)le16_to_cpu(bi->RSSI) * 100;
 
-	brcmf_dbg(CONN, "channel: %d(%d)\n", channel, freq);
+	brcmf_dbg(CONN, "channel: %d(%d)\n", ch.chnum, freq);
 	brcmf_dbg(CONN, "capability: %X\n", notify_capability);
 	brcmf_dbg(CONN, "beacon interval: %d\n", notify_interval);
 	brcmf_dbg(CONN, "signal: %d\n", notify_signal);
@@ -2510,7 +2474,7 @@ void brcmf_abort_scanning(struct brcmf_cfg80211_info *cfg)
 	set_bit(BRCMF_SCAN_STATUS_ABORT, &cfg->scan_status);
 	if (cfg->scan_request) {
 		escan->escan_state = WL_ESCAN_STATE_IDLE;
-		brcmf_notify_escan_complete(cfg, escan->ndev, true, true);
+		brcmf_notify_escan_complete(cfg, escan->ifp, true, true);
 	}
 	clear_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status);
 	clear_bit(BRCMF_SCAN_STATUS_ABORT, &cfg->scan_status);
@@ -2522,7 +2486,7 @@ static void brcmf_cfg80211_escan_timeout_worker(struct work_struct *work)
 			container_of(work, struct brcmf_cfg80211_info,
 				     escan_timeout_work);
 
-	brcmf_notify_escan_complete(cfg, cfg->escan_info.ndev, true, true);
+	brcmf_notify_escan_complete(cfg, cfg->escan_info.ifp, true, true);
 }
 
 static void brcmf_escan_timeout(unsigned long data)
@@ -2537,12 +2501,19 @@ static void brcmf_escan_timeout(unsigned long data)
 }
 
 static s32
-brcmf_compare_update_same_bss(struct brcmf_bss_info_le *bss,
+brcmf_compare_update_same_bss(struct brcmf_cfg80211_info *cfg,
+			      struct brcmf_bss_info_le *bss,
 			      struct brcmf_bss_info_le *bss_info_le)
 {
+	struct brcmu_chan ch_bss, ch_bss_info_le;
+
+	ch_bss.chspec = le16_to_cpu(bss->chanspec);
+	cfg->d11inf.decchspec(&ch_bss);
+	ch_bss_info_le.chspec = le16_to_cpu(bss_info_le->chanspec);
+	cfg->d11inf.decchspec(&ch_bss_info_le);
+
 	if (!memcmp(&bss_info_le->BSSID, &bss->BSSID, ETH_ALEN) &&
-		(CHSPEC_BAND(le16_to_cpu(bss_info_le->chanspec)) ==
-		CHSPEC_BAND(le16_to_cpu(bss->chanspec))) &&
+		ch_bss.band == ch_bss_info_le.band &&
 		bss_info_le->SSID_len == bss->SSID_len &&
 		!memcmp(bss_info_le->SSID, bss->SSID, bss_info_le->SSID_len)) {
 		if ((bss->flags & WLC_BSS_RSSI_ON_CHANNEL) ==
@@ -2573,7 +2544,6 @@ brcmf_cfg80211_escan_handler(struct brcmf_if *ifp,
 			     const struct brcmf_event_msg *e, void *data)
 {
 	struct brcmf_cfg80211_info *cfg = ifp->drvr->config;
-	struct net_device *ndev = ifp->ndev;
 	s32 status;
 	s32 err = 0;
 	struct brcmf_escan_result_le *escan_result_le;
@@ -2586,9 +2556,8 @@ brcmf_cfg80211_escan_handler(struct brcmf_if *ifp,
 
 	status = e->status;
 
-	if (!ndev || !test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status)) {
-		brcmf_err("scan not ready ndev %p drv_status %x\n", ndev,
-			  !test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status));
+	if (!test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status)) {
+		brcmf_err("scan not ready, bssidx=%d\n", ifp->bssidx);
 		return -EPERM;
 	}
 
@@ -2642,7 +2611,8 @@ brcmf_cfg80211_escan_handler(struct brcmf_if *ifp,
 			bss = bss ? (struct brcmf_bss_info_le *)
 				((unsigned char *)bss +
 				le32_to_cpu(bss->length)) : list->bss_info_le;
-			if (brcmf_compare_update_same_bss(bss, bss_info_le))
+			if (brcmf_compare_update_same_bss(cfg, bss,
+							  bss_info_le))
 				goto exit;
 		}
 		memcpy(&(cfg->escan_info.escan_buf[list->buflen]),
@@ -2659,7 +2629,7 @@ brcmf_cfg80211_escan_handler(struct brcmf_if *ifp,
 				cfg->escan_info.escan_buf;
 			brcmf_inform_bss(cfg);
 			aborted = status != BRCMF_E_STATUS_SUCCESS;
-			brcmf_notify_escan_complete(cfg, ndev, aborted,
+			brcmf_notify_escan_complete(cfg, ifp, aborted,
 						    false);
 		} else
 			brcmf_dbg(SCAN, "Ignored scan complete result 0x%x\n",
@@ -2737,7 +2707,7 @@ static s32 brcmf_cfg80211_suspend(struct wiphy *wiphy,
 		brcmf_abort_scanning(cfg);
 
 	/* Turn off watchdog timer */
-	brcmf_set_mpc(ndev, 1);
+	brcmf_set_mpc(netdev_priv(ndev), 1);
 
 exit:
 	brcmf_dbg(TRACE, "Exit\n");
@@ -2895,7 +2865,6 @@ brcmf_notify_sched_scan_results(struct brcmf_if *ifp,
 				const struct brcmf_event_msg *e, void *data)
 {
 	struct brcmf_cfg80211_info *cfg = ifp->drvr->config;
-	struct net_device *ndev = ifp->ndev;
 	struct brcmf_pno_net_info_le *netinfo, *netinfo_start;
 	struct cfg80211_scan_request *request = NULL;
 	struct cfg80211_ssid *ssid = NULL;
@@ -2979,7 +2948,7 @@ brcmf_notify_sched_scan_results(struct brcmf_if *ifp,
 		}
 
 		set_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status);
-		err = brcmf_do_escan(cfg, wiphy, ndev, request);
+		err = brcmf_do_escan(cfg, wiphy, ifp, request);
 		if (err) {
 			clear_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status);
 			goto out_err;
@@ -3051,16 +3020,21 @@ brcmf_cfg80211_sched_scan_start(struct wiphy *wiphy,
 	int i;
 	int ret = 0;
 
-	brcmf_dbg(SCAN, "Enter n_match_sets:%d   n_ssids:%d\n",
+	brcmf_dbg(SCAN, "Enter n_match_sets:%d n_ssids:%d\n",
 		  request->n_match_sets, request->n_ssids);
 	if (test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status)) {
 		brcmf_err("Scanning already: status (%lu)\n", cfg->scan_status);
 		return -EAGAIN;
 	}
+	if (test_bit(BRCMF_SCAN_STATUS_SUPPRESS, &cfg->scan_status)) {
+		brcmf_err("Scanning suppressed: status (%lu)\n",
+			  cfg->scan_status);
+		return -EAGAIN;
+	}
 
-	if (!request || !request->n_ssids || !request->n_match_sets) {
+	if (!request->n_ssids || !request->n_match_sets) {
 		brcmf_err("Invalid sched scan req!! n_ssids:%d\n",
-			  request ? request->n_ssids : 0);
+			  request->n_ssids);
 		return -EINVAL;
 	}
 
@@ -3136,7 +3110,7 @@ static int brcmf_cfg80211_sched_scan_stop(struct wiphy *wiphy,
 	brcmf_dbg(SCAN, "enter\n");
 	brcmf_dev_pno_clean(ndev);
 	if (cfg->sched_escan)
-		brcmf_notify_escan_complete(cfg, ndev, true, true);
+		brcmf_notify_escan_complete(cfg, netdev_priv(ndev), true, true);
 	return 0;
 }
 
@@ -3708,7 +3682,7 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
 		ssid_le.SSID_len = cpu_to_le32((u32)settings->ssid_len);
 	}
 
-	brcmf_set_mpc(ndev, 0);
+	brcmf_set_mpc(ifp, 0);
 
 	/* find the RSN_IE */
 	rsn_ie = brcmf_parse_tlvs((u8 *)settings->beacon.tail,
@@ -3816,7 +3790,7 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
 
 exit:
 	if (err)
-		brcmf_set_mpc(ndev, 1);
+		brcmf_set_mpc(ifp, 1);
 	return err;
 }
 
@@ -3856,7 +3830,7 @@ static int brcmf_cfg80211_stop_ap(struct wiphy *wiphy, struct net_device *ndev)
 		if (err < 0)
 			brcmf_err("bss_enable config failed %d\n", err);
 	}
-	brcmf_set_mpc(ndev, 1);
+	brcmf_set_mpc(ifp, 1);
 	set_bit(BRCMF_VIF_STATUS_AP_CREATING, &ifp->vif->sme_state);
 	clear_bit(BRCMF_VIF_STATUS_AP_CREATED, &ifp->vif->sme_state);
 
@@ -3913,13 +3887,13 @@ brcmf_cfg80211_mgmt_frame_register(struct wiphy *wiphy,
 				   struct wireless_dev *wdev,
 				   u16 frame_type, bool reg)
 {
-	struct brcmf_if *ifp = netdev_priv(wdev->netdev);
-	struct brcmf_cfg80211_vif *vif = ifp->vif;
+	struct brcmf_cfg80211_vif *vif;
 	u16 mgmt_type;
 
 	brcmf_dbg(TRACE, "Enter, frame_type %04x, reg=%d\n", frame_type, reg);
 
 	mgmt_type = (frame_type & IEEE80211_FCTL_STYPE) >> 4;
+	vif = container_of(wdev, struct brcmf_cfg80211_vif, wdev);
 	if (reg)
 		vif->mgmt_rx_reg |= BIT(mgmt_type);
 	else
@@ -3935,7 +3909,6 @@ brcmf_cfg80211_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
 {
 	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
 	const struct ieee80211_mgmt *mgmt;
-	struct brcmf_if *ifp;
 	struct brcmf_cfg80211_vif *vif;
 	s32 err = 0;
 	s32 ie_offset;
@@ -3971,8 +3944,7 @@ brcmf_cfg80211_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
 		ie_offset =  DOT11_MGMT_HDR_LEN +
 			     DOT11_BCN_PRB_FIXED_LEN;
 		ie_len = len - ie_offset;
-		ifp = netdev_priv(wdev->netdev);
-		vif = ifp->vif;
+		vif = container_of(wdev, struct brcmf_cfg80211_vif, wdev);
 		if (vif == cfg->p2p.bss_idx[P2PAPI_BSSCFG_PRIMARY].vif)
 			vif = cfg->p2p.bss_idx[P2PAPI_BSSCFG_DEVICE].vif;
 		err = brcmf_vif_set_mgmt_ie(vif,
@@ -4007,7 +3979,7 @@ brcmf_cfg80211_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
 			  *cookie, le16_to_cpu(action_frame->len),
 			  chan->center_freq);
 
-		ack = brcmf_p2p_send_action_frame(cfg, wdev->netdev,
+		ack = brcmf_p2p_send_action_frame(cfg, cfg_to_ndev(cfg),
 						  af_params);
 
 		cfg80211_mgmt_tx_status(wdev, *cookie, buf, len, ack,
@@ -4045,6 +4017,39 @@ exit:
 	return err;
 }
 
+static int brcmf_cfg80211_crit_proto_start(struct wiphy *wiphy,
+					   struct wireless_dev *wdev,
+					   enum nl80211_crit_proto_id proto,
+					   u16 duration)
+{
+	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+	struct brcmf_cfg80211_vif *vif;
+
+	vif = container_of(wdev, struct brcmf_cfg80211_vif, wdev);
+
+	/* only DHCP support for now */
+	if (proto != NL80211_CRIT_PROTO_DHCP)
+		return -EINVAL;
+
+	/* suppress and abort scanning */
+	set_bit(BRCMF_SCAN_STATUS_SUPPRESS, &cfg->scan_status);
+	brcmf_abort_scanning(cfg);
+
+	return brcmf_btcoex_set_mode(vif, BRCMF_BTCOEX_DISABLED, duration);
+}
+
+static void brcmf_cfg80211_crit_proto_stop(struct wiphy *wiphy,
+					   struct wireless_dev *wdev)
+{
+	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+	struct brcmf_cfg80211_vif *vif;
+
+	vif = container_of(wdev, struct brcmf_cfg80211_vif, wdev);
+
+	brcmf_btcoex_set_mode(vif, BRCMF_BTCOEX_ENABLED, 0);
+	clear_bit(BRCMF_SCAN_STATUS_SUPPRESS, &cfg->scan_status);
+}
+
 static struct cfg80211_ops wl_cfg80211_ops = {
 	.add_virtual_intf = brcmf_cfg80211_add_iface,
 	.del_virtual_intf = brcmf_cfg80211_del_iface,
@@ -4079,6 +4084,10 @@ static struct cfg80211_ops wl_cfg80211_ops = {
 	.mgmt_tx = brcmf_cfg80211_mgmt_tx,
 	.remain_on_channel = brcmf_p2p_remain_on_channel,
 	.cancel_remain_on_channel = brcmf_cfg80211_cancel_remain_on_channel,
+	.start_p2p_device = brcmf_p2p_start_device,
+	.stop_p2p_device = brcmf_p2p_stop_device,
+	.crit_proto_start = brcmf_cfg80211_crit_proto_start,
+	.crit_proto_stop = brcmf_cfg80211_crit_proto_stop,
 #ifdef CONFIG_NL80211_TESTMODE
 	.testmode_cmd = brcmf_cfg80211_testmode
 #endif
@@ -4162,6 +4171,11 @@ brcmf_txrx_stypes[NUM_NL80211_IFTYPES] = {
 		      BIT(IEEE80211_STYPE_AUTH >> 4) |
 		      BIT(IEEE80211_STYPE_DEAUTH >> 4) |
 		      BIT(IEEE80211_STYPE_ACTION >> 4)
+	},
+	[NL80211_IFTYPE_P2P_DEVICE] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+		      BIT(IEEE80211_STYPE_PROBE_REQ >> 4)
 	}
 };
 
@@ -4187,13 +4201,6 @@ static struct wiphy *brcmf_setup_wiphy(struct device *phydev)
 	wiphy->iface_combinations = brcmf_iface_combos;
 	wiphy->n_iface_combinations = ARRAY_SIZE(brcmf_iface_combos);
 	wiphy->bands[IEEE80211_BAND_2GHZ] = &__wl_band_2ghz;
-	wiphy->bands[IEEE80211_BAND_5GHZ] = &__wl_band_5ghz_a;	/* Set
-						* it as 11a by default.
-						* This will be updated with
-						* 11n phy tables in
-						* "ifconfig up"
-						* if phy has 11n capability
-						*/
 	wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
 	wiphy->cipher_suites = __wl_cipher_suites;
 	wiphy->n_cipher_suites = ARRAY_SIZE(__wl_cipher_suites);
@@ -4203,6 +4210,9 @@ static struct wiphy *brcmf_setup_wiphy(struct device *phydev)
 	wiphy->mgmt_stypes = brcmf_txrx_stypes;
 	wiphy->max_remain_on_channel_duration = 5000;
 	brcmf_wiphy_pno_params(wiphy);
+	brcmf_dbg(INFO, "Registering custom regulatory\n");
+	wiphy->flags |= WIPHY_FLAG_CUSTOM_REGULATORY;
+	wiphy_apply_custom_regulatory(wiphy, &brcmf_regdom);
 	err = wiphy_register(wiphy);
 	if (err < 0) {
 		brcmf_err("Could not register wiphy device (%d)\n", err);
@@ -4386,9 +4396,9 @@ brcmf_bss_roaming_done(struct brcmf_cfg80211_info *cfg,
 	struct ieee80211_channel *notify_channel = NULL;
 	struct ieee80211_supported_band *band;
 	struct brcmf_bss_info_le *bi;
+	struct brcmu_chan ch;
 	u32 freq;
 	s32 err = 0;
-	u32 target_channel;
 	u8 *buf;
 
 	brcmf_dbg(TRACE, "Enter\n");
@@ -4412,15 +4422,15 @@ brcmf_bss_roaming_done(struct brcmf_cfg80211_info *cfg,
 		goto done;
 
 	bi = (struct brcmf_bss_info_le *)(buf + 4);
-	target_channel = bi->ctl_ch ? bi->ctl_ch :
-				      CHSPEC_CHANNEL(le16_to_cpu(bi->chanspec));
+	ch.chspec = le16_to_cpu(bi->chanspec);
+	cfg->d11inf.decchspec(&ch);
 
-	if (target_channel <= CH_MAX_2G_CHANNEL)
+	if (ch.band == BRCMU_CHAN_BAND_2G)
 		band = wiphy->bands[IEEE80211_BAND_2GHZ];
 	else
 		band = wiphy->bands[IEEE80211_BAND_5GHZ];
 
-	freq = ieee80211_channel_to_frequency(target_channel, band->band);
+	freq = ieee80211_channel_to_frequency(ch.chnum, band->band);
 	notify_channel = ieee80211_get_channel(wiphy, freq);
 
 done:
@@ -4621,9 +4631,11 @@ static s32 brcmf_notify_vif_event(struct brcmf_if *ifp,
 
 		ifp->vif = vif;
 		vif->ifp = ifp;
-		vif->wdev.netdev = ifp->ndev;
-		ifp->ndev->ieee80211_ptr = &vif->wdev;
-		SET_NETDEV_DEV(ifp->ndev, wiphy_dev(cfg->wiphy));
+		if (ifp->ndev) {
+			vif->wdev.netdev = ifp->ndev;
+			ifp->ndev->ieee80211_ptr = &vif->wdev;
+			SET_NETDEV_DEV(ifp->ndev, wiphy_dev(cfg->wiphy));
+		}
 		mutex_unlock(&event->vif_event_lock);
 		wake_up(&event->vif_wq);
 		return 0;
@@ -4772,6 +4784,7 @@ struct brcmf_cfg80211_info *brcmf_cfg80211_attach(struct brcmf_pub *drvr,
 	struct brcmf_cfg80211_vif *vif;
 	struct brcmf_if *ifp;
 	s32 err = 0;
+	s32 io_type;
 
 	if (!ndev) {
 		brcmf_err("ndev is invalid\n");
@@ -4812,6 +4825,21 @@ struct brcmf_cfg80211_info *brcmf_cfg80211_attach(struct brcmf_pub *drvr,
 		brcmf_err("P2P initilisation failed (%d)\n", err);
 		goto cfg80211_p2p_attach_out;
 	}
+	err = brcmf_btcoex_attach(cfg);
+	if (err) {
+		brcmf_err("BT-coex initialisation failed (%d)\n", err);
+		brcmf_p2p_detach(&cfg->p2p);
+		goto cfg80211_p2p_attach_out;
+	}
+
+	err = brcmf_fil_cmd_int_get(ifp, BRCMF_C_GET_VERSION,
+				    &io_type);
+	if (err) {
+		brcmf_err("Failed to get D11 version (%d)\n", err);
+		goto cfg80211_p2p_attach_out;
+	}
+	cfg->d11inf.io_type = (u8)io_type;
+	brcmu_d11_attach(&cfg->d11inf);
 
 	return cfg;
 
@@ -4830,6 +4858,7 @@ void brcmf_cfg80211_detach(struct brcmf_cfg80211_info *cfg)
 	struct brcmf_cfg80211_vif *tmp;
 
 	wl_deinit_priv(cfg);
+	brcmf_btcoex_detach(cfg);
 	list_for_each_entry_safe(vif, tmp, &cfg->vif_list, list) {
 		brcmf_free_vif(vif);
 	}
@@ -4926,34 +4955,234 @@ dongle_scantime_out:
 	return err;
 }
 
-static s32 wl_update_wiphybands(struct brcmf_cfg80211_info *cfg)
+
+static s32 brcmf_construct_reginfo(struct brcmf_cfg80211_info *cfg, u32 bw_cap)
+{
+	struct brcmf_if *ifp = netdev_priv(cfg_to_ndev(cfg));
+	struct ieee80211_channel *band_chan_arr;
+	struct brcmf_chanspec_list *list;
+	struct brcmu_chan ch;
+	s32 err;
+	u8 *pbuf;
+	u32 i, j;
+	u32 total;
+	enum ieee80211_band band;
+	u32 channel;
+	u32 *n_cnt;
+	bool ht40_allowed;
+	u32 index;
+	u32 ht40_flag;
+	bool update;
+	u32 array_size;
+
+	pbuf = kzalloc(BRCMF_DCMD_MEDLEN, GFP_KERNEL);
+
+	if (pbuf == NULL)
+		return -ENOMEM;
+
+	list = (struct brcmf_chanspec_list *)pbuf;
+
+	err = brcmf_fil_iovar_data_get(ifp, "chanspecs", pbuf,
+				       BRCMF_DCMD_MEDLEN);
+	if (err) {
+		brcmf_err("get chanspecs error (%d)\n", err);
+		goto exit;
+	}
+
+	__wl_band_2ghz.n_channels = 0;
+	__wl_band_5ghz_a.n_channels = 0;
+
+	total = le32_to_cpu(list->count);
+	for (i = 0; i < total; i++) {
+		ch.chspec = (u16)le32_to_cpu(list->element[i]);
+		cfg->d11inf.decchspec(&ch);
+
+		if (ch.band == BRCMU_CHAN_BAND_2G) {
+			band_chan_arr = __wl_2ghz_channels;
+			array_size = ARRAY_SIZE(__wl_2ghz_channels);
+			n_cnt = &__wl_band_2ghz.n_channels;
+			band = IEEE80211_BAND_2GHZ;
+			ht40_allowed = (bw_cap == WLC_N_BW_40ALL);
+		} else if (ch.band == BRCMU_CHAN_BAND_5G) {
+			band_chan_arr = __wl_5ghz_a_channels;
+			array_size = ARRAY_SIZE(__wl_5ghz_a_channels);
+			n_cnt = &__wl_band_5ghz_a.n_channels;
+			band = IEEE80211_BAND_5GHZ;
+			ht40_allowed = !(bw_cap == WLC_N_BW_20ALL);
+		} else {
+			brcmf_err("Invalid channel Sepc. 0x%x.\n", ch.chspec);
+			continue;
+		}
+		if (!ht40_allowed && ch.bw == BRCMU_CHAN_BW_40)
+			continue;
+		update = false;
+		for (j = 0; (j < *n_cnt && (*n_cnt < array_size)); j++) {
+			if (band_chan_arr[j].hw_value == ch.chnum) {
+				update = true;
+				break;
+			}
+		}
+		if (update)
+			index = j;
+		else
+			index = *n_cnt;
+		if (index <  array_size) {
+			band_chan_arr[index].center_freq =
+				ieee80211_channel_to_frequency(ch.chnum, band);
+			band_chan_arr[index].hw_value = ch.chnum;
+
+			if (ch.bw == BRCMU_CHAN_BW_40 && ht40_allowed) {
+				/* assuming the order is HT20, HT40 Upper,
+				 * HT40 lower from chanspecs
+				 */
+				ht40_flag = band_chan_arr[index].flags &
+					    IEEE80211_CHAN_NO_HT40;
+				if (ch.sb == BRCMU_CHAN_SB_U) {
+					if (ht40_flag == IEEE80211_CHAN_NO_HT40)
+						band_chan_arr[index].flags &=
+							~IEEE80211_CHAN_NO_HT40;
+					band_chan_arr[index].flags |=
+						IEEE80211_CHAN_NO_HT40PLUS;
+				} else {
+					/* It should be one of
+					 * IEEE80211_CHAN_NO_HT40 or
+					 * IEEE80211_CHAN_NO_HT40PLUS
+					 */
+					band_chan_arr[index].flags &=
+							~IEEE80211_CHAN_NO_HT40;
+					if (ht40_flag == IEEE80211_CHAN_NO_HT40)
+						band_chan_arr[index].flags |=
+						    IEEE80211_CHAN_NO_HT40MINUS;
+				}
+			} else {
+				band_chan_arr[index].flags =
+							IEEE80211_CHAN_NO_HT40;
+				ch.bw = BRCMU_CHAN_BW_20;
+				cfg->d11inf.encchspec(&ch);
+				channel = ch.chspec;
+				err = brcmf_fil_bsscfg_int_get(ifp,
+							       "per_chan_info",
+							       &channel);
+				if (!err) {
+					if (channel & WL_CHAN_RADAR)
+						band_chan_arr[index].flags |=
+							(IEEE80211_CHAN_RADAR |
+							IEEE80211_CHAN_NO_IBSS);
+					if (channel & WL_CHAN_PASSIVE)
+						band_chan_arr[index].flags |=
+						    IEEE80211_CHAN_PASSIVE_SCAN;
+				}
+			}
+			if (!update)
+				(*n_cnt)++;
+		}
+	}
+exit:
+	kfree(pbuf);
+	return err;
+}
+
+
+static s32 brcmf_update_wiphybands(struct brcmf_cfg80211_info *cfg)
 {
 	struct brcmf_if *ifp = netdev_priv(cfg_to_ndev(cfg));
 	struct wiphy *wiphy;
 	s32 phy_list;
+	u32 band_list[3];
+	u32 nmode;
+	u32 bw_cap = 0;
 	s8 phy;
-	s32 err = 0;
+	s32 err;
+	u32 nband;
+	s32 i;
+	struct ieee80211_supported_band *bands[IEEE80211_NUM_BANDS];
+	s32 index;
 
 	err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_PHYLIST,
 				     &phy_list, sizeof(phy_list));
 	if (err) {
-		brcmf_err("error (%d)\n", err);
+		brcmf_err("BRCMF_C_GET_PHYLIST error (%d)\n", err);
 		return err;
 	}
 
 	phy = ((char *)&phy_list)[0];
-	brcmf_dbg(INFO, "%c phy\n", phy);
-	if (phy == 'n' || phy == 'a') {
-		wiphy = cfg_to_wiphy(cfg);
-		wiphy->bands[IEEE80211_BAND_5GHZ] = &__wl_band_5ghz_n;
+	brcmf_dbg(INFO, "BRCMF_C_GET_PHYLIST reported: %c phy\n", phy);
+
+
+	err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_BANDLIST,
+				     &band_list, sizeof(band_list));
+	if (err) {
+		brcmf_err("BRCMF_C_GET_BANDLIST error (%d)\n", err);
+		return err;
+	}
+	brcmf_dbg(INFO, "BRCMF_C_GET_BANDLIST reported: 0x%08x 0x%08x 0x%08x phy\n",
+		  band_list[0], band_list[1], band_list[2]);
+
+	err = brcmf_fil_iovar_int_get(ifp, "nmode", &nmode);
+	if (err) {
+		brcmf_err("nmode error (%d)\n", err);
+	} else {
+		err = brcmf_fil_iovar_int_get(ifp, "mimo_bw_cap", &bw_cap);
+		if (err)
+			brcmf_err("mimo_bw_cap error (%d)\n", err);
 	}
+	brcmf_dbg(INFO, "nmode=%d, mimo_bw_cap=%d\n", nmode, bw_cap);
+
+	err = brcmf_construct_reginfo(cfg, bw_cap);
+	if (err) {
+		brcmf_err("brcmf_construct_reginfo failed (%d)\n", err);
+		return err;
+	}
+
+	nband = band_list[0];
+	memset(bands, 0, sizeof(bands));
+
+	for (i = 1; i <= nband && i < ARRAY_SIZE(band_list); i++) {
+		index = -1;
+		if ((band_list[i] == WLC_BAND_5G) &&
+		    (__wl_band_5ghz_a.n_channels > 0)) {
+			index = IEEE80211_BAND_5GHZ;
+			bands[index] = &__wl_band_5ghz_a;
+			if ((bw_cap == WLC_N_BW_40ALL) ||
+			    (bw_cap == WLC_N_BW_20IN2G_40IN5G))
+				bands[index]->ht_cap.cap |=
+							IEEE80211_HT_CAP_SGI_40;
+		} else if ((band_list[i] == WLC_BAND_2G) &&
+			   (__wl_band_2ghz.n_channels > 0)) {
+			index = IEEE80211_BAND_2GHZ;
+			bands[index] = &__wl_band_2ghz;
+			if (bw_cap == WLC_N_BW_40ALL)
+				bands[index]->ht_cap.cap |=
+							IEEE80211_HT_CAP_SGI_40;
+		}
+
+		if ((index >= 0) && nmode) {
+			bands[index]->ht_cap.cap |= IEEE80211_HT_CAP_SGI_20;
+			bands[index]->ht_cap.cap |= IEEE80211_HT_CAP_DSSSCCK40;
+			bands[index]->ht_cap.ht_supported = true;
+			bands[index]->ht_cap.ampdu_factor =
+						IEEE80211_HT_MAX_AMPDU_64K;
+			bands[index]->ht_cap.ampdu_density =
+						IEEE80211_HT_MPDU_DENSITY_16;
+			/* An HT shall support all EQM rates for one spatial
+			 * stream
+			 */
+			bands[index]->ht_cap.mcs.rx_mask[0] = 0xff;
+		}
+	}
+
+	wiphy = cfg_to_wiphy(cfg);
+	wiphy->bands[IEEE80211_BAND_2GHZ] = bands[IEEE80211_BAND_2GHZ];
+	wiphy->bands[IEEE80211_BAND_5GHZ] = bands[IEEE80211_BAND_5GHZ];
+	wiphy_apply_custom_regulatory(wiphy, &brcmf_regdom);
 
 	return err;
 }
 
+
 static s32 brcmf_dongle_probecap(struct brcmf_cfg80211_info *cfg)
 {
-	return wl_update_wiphybands(cfg);
+	return brcmf_update_wiphybands(cfg);
 }
 
 static s32 brcmf_config_dongle(struct brcmf_cfg80211_info *cfg)
@@ -5059,6 +5288,13 @@ s32 brcmf_cfg80211_down(struct net_device *ndev)
 	return err;
 }
 
+enum nl80211_iftype brcmf_cfg80211_get_iftype(struct brcmf_if *ifp)
+{
+	struct wireless_dev *wdev = &ifp->vif->wdev;
+
+	return wdev->iftype;
+}
+
 u32 wl_get_vif_state_all(struct brcmf_cfg80211_info *cfg, unsigned long state)
 {
 	struct brcmf_cfg80211_vif *vif;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h
index 8b5d4989906c..a71cff84cdcf 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h
@@ -17,6 +17,9 @@
 #ifndef _wl_cfg80211_h_
 #define _wl_cfg80211_h_
 
+/* for brcmu_d11inf */
+#include <brcmu_d11.h>
+
 #define WL_NUM_SCAN_MAX			10
 #define WL_NUM_PMKIDS_MAX		MAXPMKID
 #define WL_TLV_INFO_MAX			1024
@@ -74,14 +77,16 @@
 
 
 /**
- * enum brcmf_scan_status - dongle scan status
+ * enum brcmf_scan_status - scan engine status
  *
  * @BRCMF_SCAN_STATUS_BUSY: scanning in progress on dongle.
  * @BRCMF_SCAN_STATUS_ABORT: scan being aborted on dongle.
+ * @BRCMF_SCAN_STATUS_SUPPRESS: scanning is suppressed in driver.
  */
 enum brcmf_scan_status {
 	BRCMF_SCAN_STATUS_BUSY,
 	BRCMF_SCAN_STATUS_ABORT,
+	BRCMF_SCAN_STATUS_SUPPRESS,
 };
 
 /**
@@ -238,9 +243,8 @@ struct escan_info {
 	u32 escan_state;
 	u8 escan_buf[WL_ESCAN_BUF_SIZE];
 	struct wiphy *wiphy;
-	struct net_device *ndev;
-	s32 (*run)(struct brcmf_cfg80211_info *cfg,
-		   struct net_device *ndev,
+	struct brcmf_if *ifp;
+	s32 (*run)(struct brcmf_cfg80211_info *cfg, struct brcmf_if *ifp,
 		   struct cfg80211_scan_request *request, u16 action);
 };
 
@@ -347,6 +351,7 @@ struct brcmf_cfg80211_vif_event {
  * @wiphy: wiphy object for cfg80211 interface.
  * @conf: dongle configuration.
  * @p2p: peer-to-peer specific information.
+ * @btcoex: Bluetooth coexistence information.
  * @scan_request: cfg80211 scan request object.
  * @usr_sync: mainly for dongle up/down synchronization.
  * @bss_list: bss_list holding scanned ap information.
@@ -380,6 +385,7 @@ struct brcmf_cfg80211_info {
 	struct wiphy *wiphy;
 	struct brcmf_cfg80211_conf *conf;
 	struct brcmf_p2p_info p2p;
+	struct brcmf_btcoex_info *btcoex;
 	struct cfg80211_scan_request *scan_request;
 	struct mutex usr_sync;
 	struct brcmf_scan_results *bss_list;
@@ -409,6 +415,7 @@ struct brcmf_cfg80211_info {
 	u8 vif_cnt;
 	struct brcmf_cfg80211_vif_event vif_event;
 	struct completion vif_disabled;
+	struct brcmu_d11inf d11inf;
 };
 
 /**
@@ -475,6 +482,7 @@ struct brcmf_cfg80211_info *brcmf_cfg80211_attach(struct brcmf_pub *drvr,
 void brcmf_cfg80211_detach(struct brcmf_cfg80211_info *cfg);
 s32 brcmf_cfg80211_up(struct net_device *ndev);
 s32 brcmf_cfg80211_down(struct net_device *ndev);
+enum nl80211_iftype brcmf_cfg80211_get_iftype(struct brcmf_if *ifp);
 
 struct brcmf_cfg80211_vif *brcmf_alloc_vif(struct brcmf_cfg80211_info *cfg,
 					   enum nl80211_iftype type,
@@ -485,7 +493,8 @@ s32 brcmf_vif_set_mgmt_ie(struct brcmf_cfg80211_vif *vif, s32 pktflag,
 			  const u8 *vndr_ie_buf, u32 vndr_ie_len);
 s32 brcmf_vif_clear_mgmt_ies(struct brcmf_cfg80211_vif *vif);
 struct brcmf_tlv *brcmf_parse_tlvs(void *buf, int buflen, uint key);
-u16 channel_to_chanspec(struct ieee80211_channel *ch);
+u16 channel_to_chanspec(struct brcmu_d11inf *d11inf,
+			struct ieee80211_channel *ch);
 u32 wl_get_vif_state_all(struct brcmf_cfg80211_info *cfg, unsigned long state);
 void brcmf_cfg80211_arm_vif_event(struct brcmf_cfg80211_info *cfg,
 				  struct brcmf_cfg80211_vif *vif);
@@ -493,9 +502,9 @@ bool brcmf_cfg80211_vif_event_armed(struct brcmf_cfg80211_info *cfg);
 int brcmf_cfg80211_wait_vif_event_timeout(struct brcmf_cfg80211_info *cfg,
 					  u8 action, ulong timeout);
 s32 brcmf_notify_escan_complete(struct brcmf_cfg80211_info *cfg,
-				struct net_device *ndev,
-				bool aborted, bool fw_abort);
-void brcmf_set_mpc(struct net_device *ndev, int mpc);
+				struct brcmf_if *ifp, bool aborted,
+				bool fw_abort);
+void brcmf_set_mpc(struct brcmf_if *ndev, int mpc);
 void brcmf_abort_scanning(struct brcmf_cfg80211_info *cfg);
 
 #endif				/* _wl_cfg80211_h_ */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/Makefile b/drivers/net/wireless/brcm80211/brcmsmac/Makefile
index d3d4151c3eda..32464acccd90 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/Makefile
+++ b/drivers/net/wireless/brcm80211/brcmsmac/Makefile
@@ -21,7 +21,7 @@ ccflags-y := \
 	-Idrivers/net/wireless/brcm80211/brcmsmac/phy \
 	-Idrivers/net/wireless/brcm80211/include
 
-BRCMSMAC_OFILES := \
+brcmsmac-y := \
 	mac80211_if.o \
 	ucode_loader.o \
 	ampdu.o \
@@ -43,7 +43,6 @@ BRCMSMAC_OFILES := \
 	brcms_trace_events.o \
 	debug.o
 
-MODULEPFX := brcmsmac
+brcmsmac-$(CONFIG_BCMA_DRIVER_GPIO) += led.o
 
-obj-$(CONFIG_BRCMSMAC)	+= $(MODULEPFX).o
-$(MODULEPFX)-objs	= $(BRCMSMAC_OFILES)
+obj-$(CONFIG_BRCMSMAC)	+= brcmsmac.o
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
index f0888a9ee32e..e4fd1ee3d690 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
@@ -318,12 +318,6 @@
 #define	IS_SIM(chippkg)	\
 	((chippkg == HDLSIM_PKG_ID) || (chippkg == HWSIM_PKG_ID))
 
-#ifdef DEBUG
-#define	SI_MSG(fmt, ...)	pr_debug(fmt, ##__VA_ARGS__)
-#else
-#define	SI_MSG(fmt, ...)	no_printk(fmt, ##__VA_ARGS__)
-#endif				/* DEBUG */
-
 #define	GOODCOREADDR(x, b) \
 	(((x) >= (b)) && ((x) < ((b) + SI_MAXCORES * SI_CORE_SIZE)) && \
 		IS_ALIGNED((x), SI_CORE_SIZE))
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/channel.c b/drivers/net/wireless/brcm80211/brcmsmac/channel.c
index 10ee314c4229..cc87926f5055 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/channel.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/channel.c
@@ -379,7 +379,7 @@ brcms_c_channel_set_chanspec(struct brcms_cm_info *wlc_cm, u16 chanspec,
 			 u8 local_constraint_qdbm)
 {
 	struct brcms_c_info *wlc = wlc_cm->wlc;
-	struct ieee80211_channel *ch = wlc->pub->ieee_hw->conf.channel;
+	struct ieee80211_channel *ch = wlc->pub->ieee_hw->conf.chandef.chan;
 	struct txpwr_limits txpwr;
 
 	brcms_c_channel_reg_limits(wlc_cm, chanspec, &txpwr);
@@ -404,7 +404,7 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, u16 chanspec,
 		       struct txpwr_limits *txpwr)
 {
 	struct brcms_c_info *wlc = wlc_cm->wlc;
-	struct ieee80211_channel *ch = wlc->pub->ieee_hw->conf.channel;
+	struct ieee80211_channel *ch = wlc->pub->ieee_hw->conf.chandef.chan;
 	uint i;
 	uint chan;
 	int maxpwr;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/d11.h b/drivers/net/wireless/brcm80211/brcmsmac/d11.h
index 3f659e09f1cc..9035cc4d6ff3 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/d11.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/d11.h
@@ -457,6 +457,7 @@ struct d11regs {
 /*== maccontrol register ==*/
 #define	MCTL_GMODE		(1U << 31)
 #define	MCTL_DISCARD_PMQ	(1 << 30)
+#define	MCTL_TBTTHOLD		(1 << 28)
 #define	MCTL_WAKE		(1 << 26)
 #define	MCTL_HPS		(1 << 25)
 #define	MCTL_PROMISC		(1 << 24)
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/led.c b/drivers/net/wireless/brcm80211/brcmsmac/led.c
new file mode 100644
index 000000000000..74b17cecb189
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmsmac/led.c
@@ -0,0 +1,126 @@
+#include <net/mac80211.h>
+#include <linux/bcma/bcma_driver_chipcommon.h>
+#include <linux/gpio.h>
+
+#include "mac80211_if.h"
+#include "pub.h"
+#include "main.h"
+#include "led.h"
+
+	/* number of leds */
+#define  BRCMS_LED_NO		4
+	/* behavior mask */
+#define  BRCMS_LED_BEH_MASK	0x7f
+	/* activelow (polarity) bit */
+#define  BRCMS_LED_AL_MASK	0x80
+	/* radio enabled */
+#define  BRCMS_LED_RADIO	3
+
+static void brcms_radio_led_ctrl(struct brcms_info *wl, bool state)
+{
+	if (wl->radio_led.gpio == -1)
+		return;
+
+	if (wl->radio_led.active_low)
+		state = !state;
+
+	if (state)
+		gpio_set_value(wl->radio_led.gpio, 1);
+	else
+		gpio_set_value(wl->radio_led.gpio, 0);
+}
+
+
+/* Callback from the LED subsystem. */
+static void brcms_led_brightness_set(struct led_classdev *led_dev,
+				   enum led_brightness brightness)
+{
+	struct brcms_info *wl = container_of(led_dev,
+		struct brcms_info, led_dev);
+	brcms_radio_led_ctrl(wl, brightness);
+}
+
+void brcms_led_unregister(struct brcms_info *wl)
+{
+	if (wl->led_dev.dev)
+		led_classdev_unregister(&wl->led_dev);
+	if (wl->radio_led.gpio != -1)
+		gpio_free(wl->radio_led.gpio);
+}
+
+int brcms_led_register(struct brcms_info *wl)
+{
+	int i, err;
+	struct brcms_led *radio_led = &wl->radio_led;
+	/* get CC core */
+	struct bcma_drv_cc *cc_drv  = &wl->wlc->hw->d11core->bus->drv_cc;
+	struct gpio_chip *bcma_gpio = &cc_drv->gpio;
+	struct ssb_sprom *sprom = &wl->wlc->hw->d11core->bus->sprom;
+	u8 *leds[] = { &sprom->gpio0,
+		&sprom->gpio1,
+		&sprom->gpio2,
+		&sprom->gpio3 };
+	unsigned gpio = -1;
+	bool active_low = false;
+
+	/* none by default */
+	radio_led->gpio = -1;
+	radio_led->active_low = false;
+
+	if (!bcma_gpio || !gpio_is_valid(bcma_gpio->base))
+		return -ENODEV;
+
+	/* find radio enabled LED */
+	for (i = 0; i < BRCMS_LED_NO; i++) {
+		u8 led = *leds[i];
+		if ((led & BRCMS_LED_BEH_MASK) == BRCMS_LED_RADIO) {
+			gpio = bcma_gpio->base + i;
+			if (led & BRCMS_LED_AL_MASK)
+				active_low = true;
+			break;
+		}
+	}
+
+	if (gpio == -1 || !gpio_is_valid(gpio))
+		return -ENODEV;
+
+	/* request and configure LED gpio */
+	err = gpio_request_one(gpio,
+				active_low ? GPIOF_OUT_INIT_HIGH
+					: GPIOF_OUT_INIT_LOW,
+				"radio on");
+	if (err) {
+		wiphy_err(wl->wiphy, "requesting led gpio %d failed (err: %d)\n",
+			  gpio, err);
+		return err;
+	}
+	err = gpio_direction_output(gpio, 1);
+	if (err) {
+		wiphy_err(wl->wiphy, "cannot set led gpio %d to output (err: %d)\n",
+			  gpio, err);
+		return err;
+	}
+
+	snprintf(wl->radio_led.name, sizeof(wl->radio_led.name),
+		 "brcmsmac-%s:radio", wiphy_name(wl->wiphy));
+
+	wl->led_dev.name = wl->radio_led.name;
+	wl->led_dev.default_trigger =
+		ieee80211_get_radio_led_name(wl->pub->ieee_hw);
+	wl->led_dev.brightness_set = brcms_led_brightness_set;
+	err = led_classdev_register(wiphy_dev(wl->wiphy), &wl->led_dev);
+
+	if (err) {
+		wiphy_err(wl->wiphy, "cannot register led device: %s (err: %d)\n",
+			  wl->radio_led.name, err);
+		return err;
+	}
+
+	wiphy_info(wl->wiphy, "registered radio enabled led device: %s gpio: %d\n",
+		   wl->radio_led.name,
+		   gpio);
+	radio_led->gpio = gpio;
+	radio_led->active_low = active_low;
+
+	return 0;
+}
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/led.h b/drivers/net/wireless/brcm80211/brcmsmac/led.h
new file mode 100644
index 000000000000..17a0b1f5dbcf
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmsmac/led.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2012 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _BRCM_LED_H_
+#define _BRCM_LED_H_
+struct brcms_led {
+	char name[32];
+	unsigned gpio;
+	bool active_low;
+};
+
+#ifdef CONFIG_BCMA_DRIVER_GPIO
+void brcms_led_unregister(struct brcms_info *wl);
+int brcms_led_register(struct brcms_info *wl);
+#else
+static inline void brcms_led_unregister(struct brcms_info *wl) {};
+static inline int brcms_led_register(struct brcms_info *wl)
+{
+	return -ENOTSUPP;
+};
+#endif
+
+#endif /* _BRCM_LED_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
index e2340b231aa1..3a6544710c8a 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2010 Broadcom Corporation
+ * Copyright (c) 2013 Hauke Mehrtens <hauke@hauke-m.de>
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -34,6 +35,7 @@
 #include "mac80211_if.h"
 #include "main.h"
 #include "debug.h"
+#include "led.h"
 
 #define N_TX_QUEUES	4 /* #tx queues on mac80211<->driver interface */
 #define BRCMS_FLUSH_TIMEOUT	500 /* msec */
@@ -334,6 +336,7 @@ static void brcms_remove(struct bcma_device *pdev)
 	struct brcms_info *wl = hw->priv;
 
 	if (wl->wlc) {
+		brcms_led_unregister(wl);
 		wiphy_rfkill_set_hw_state(wl->pub->ieee_hw->wiphy, false);
 		wiphy_rfkill_stop_polling(wl->pub->ieee_hw->wiphy);
 		ieee80211_unregister_hw(hw);
@@ -487,18 +490,26 @@ brcms_ops_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 {
 	struct brcms_info *wl = hw->priv;
 
-	/* Just STA for now */
-	if (vif->type != NL80211_IFTYPE_STATION) {
+	/* Just STA, AP and ADHOC for now */
+	if (vif->type != NL80211_IFTYPE_STATION &&
+	    vif->type != NL80211_IFTYPE_AP &&
+	    vif->type != NL80211_IFTYPE_ADHOC) {
 		brcms_err(wl->wlc->hw->d11core,
-			  "%s: Attempt to add type %d, only STA for now\n",
+			  "%s: Attempt to add type %d, only STA, AP and AdHoc for now\n",
 			  __func__, vif->type);
 		return -EOPNOTSUPP;
 	}
 
 	spin_lock_bh(&wl->lock);
-	memcpy(wl->pub->cur_etheraddr, vif->addr, sizeof(vif->addr));
 	wl->mute_tx = false;
 	brcms_c_mute(wl->wlc, false);
+	if (vif->type == NL80211_IFTYPE_STATION)
+		brcms_c_start_station(wl->wlc, vif->addr);
+	else if (vif->type == NL80211_IFTYPE_AP)
+		brcms_c_start_ap(wl->wlc, vif->addr, vif->bss_conf.bssid,
+				 vif->bss_conf.ssid, vif->bss_conf.ssid_len);
+	else if (vif->type == NL80211_IFTYPE_ADHOC)
+		brcms_c_start_adhoc(wl->wlc, vif->addr);
 	spin_unlock_bh(&wl->lock);
 
 	return 0;
@@ -546,10 +557,10 @@ static int brcms_ops_config(struct ieee80211_hw *hw, u32 changed)
 				  new_int);
 	}
 	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
-		if (conf->channel_type == NL80211_CHAN_HT20 ||
-		    conf->channel_type == NL80211_CHAN_NO_HT)
+		if (conf->chandef.width == NL80211_CHAN_WIDTH_20 ||
+		    conf->chandef.width == NL80211_CHAN_WIDTH_20_NOHT)
 			err = brcms_c_set_channel(wl->wlc,
-						  conf->channel->hw_value);
+						  conf->chandef.chan->hw_value);
 		else
 			err = -ENOTSUPP;
 	}
@@ -650,14 +661,43 @@ brcms_ops_bss_info_changed(struct ieee80211_hw *hw,
 		brcms_c_set_addrmatch(wl->wlc, RCM_BSSID_OFFSET, info->bssid);
 		spin_unlock_bh(&wl->lock);
 	}
-	if (changed & BSS_CHANGED_BEACON)
+	if (changed & BSS_CHANGED_SSID) {
+		/* BSSID changed, for whatever reason (IBSS and managed mode) */
+		spin_lock_bh(&wl->lock);
+		brcms_c_set_ssid(wl->wlc, info->ssid, info->ssid_len);
+		spin_unlock_bh(&wl->lock);
+	}
+	if (changed & BSS_CHANGED_BEACON) {
 		/* Beacon data changed, retrieve new beacon (beaconing modes) */
-		brcms_err(core, "%s: beacon changed\n", __func__);
+		struct sk_buff *beacon;
+		u16 tim_offset = 0;
+
+		spin_lock_bh(&wl->lock);
+		beacon = ieee80211_beacon_get_tim(hw, vif, &tim_offset, NULL);
+		brcms_c_set_new_beacon(wl->wlc, beacon, tim_offset,
+				       info->dtim_period);
+		spin_unlock_bh(&wl->lock);
+	}
+
+	if (changed & BSS_CHANGED_AP_PROBE_RESP) {
+		struct sk_buff *probe_resp;
+
+		spin_lock_bh(&wl->lock);
+		probe_resp = ieee80211_proberesp_get(hw, vif);
+		brcms_c_set_new_probe_resp(wl->wlc, probe_resp);
+		spin_unlock_bh(&wl->lock);
+	}
 
 	if (changed & BSS_CHANGED_BEACON_ENABLED) {
 		/* Beaconing should be enabled/disabled (beaconing modes) */
 		brcms_err(core, "%s: Beacon enabled: %s\n", __func__,
 			  info->enable_beacon ? "true" : "false");
+		if (info->enable_beacon &&
+		    hw->wiphy->flags & WIPHY_FLAG_AP_PROBE_RESP_OFFLOAD) {
+			brcms_c_enable_probe_resp(wl->wlc, true);
+		} else {
+			brcms_c_enable_probe_resp(wl->wlc, false);
+		}
 	}
 
 	if (changed & BSS_CHANGED_CQM) {
@@ -855,7 +895,7 @@ static bool brcms_tx_flush_completed(struct brcms_info *wl)
 	return result;
 }
 
-static void brcms_ops_flush(struct ieee80211_hw *hw, bool drop)
+static void brcms_ops_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 {
 	struct brcms_info *wl = hw->priv;
 	int ret;
@@ -870,6 +910,28 @@ static void brcms_ops_flush(struct ieee80211_hw *hw, bool drop)
 			   "ret=%d\n", jiffies_to_msecs(ret));
 }
 
+static u64 brcms_ops_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct brcms_info *wl = hw->priv;
+	u64 tsf;
+
+	spin_lock_bh(&wl->lock);
+	tsf = brcms_c_tsf_get(wl->wlc);
+	spin_unlock_bh(&wl->lock);
+
+	return tsf;
+}
+
+static void brcms_ops_set_tsf(struct ieee80211_hw *hw,
+			   struct ieee80211_vif *vif, u64 tsf)
+{
+	struct brcms_info *wl = hw->priv;
+
+	spin_lock_bh(&wl->lock);
+	brcms_c_tsf_set(wl->wlc, tsf);
+	spin_unlock_bh(&wl->lock);
+}
+
 static const struct ieee80211_ops brcms_ops = {
 	.tx = brcms_ops_tx,
 	.start = brcms_ops_start,
@@ -886,6 +948,8 @@ static const struct ieee80211_ops brcms_ops = {
 	.ampdu_action = brcms_ops_ampdu_action,
 	.rfkill_poll = brcms_ops_rfkill_poll,
 	.flush = brcms_ops_flush,
+	.get_tsf = brcms_ops_get_tsf,
+	.set_tsf = brcms_ops_set_tsf,
 };
 
 void brcms_dpc(unsigned long data)
@@ -1004,7 +1068,16 @@ static int ieee_hw_init(struct ieee80211_hw *hw)
 
 	/* channel change time is dependent on chip and band  */
 	hw->channel_change_time = 7 * 1000;
-	hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION);
+	hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
+				     BIT(NL80211_IFTYPE_AP) |
+				     BIT(NL80211_IFTYPE_ADHOC);
+
+	/*
+	 * deactivate sending probe responses by ucude, because this will
+	 * cause problems when WPS is used.
+	 *
+	 * hw->wiphy->flags |= WIPHY_FLAG_AP_PROBE_RESP_OFFLOAD;
+	 */
 
 	hw->rate_control_algorithm = "minstrel_ht";
 
@@ -1151,6 +1224,8 @@ static int brcms_bcma_probe(struct bcma_device *pdev)
 		pr_err("%s: brcms_attach failed!\n", __func__);
 		return -ENODEV;
 	}
+	brcms_led_register(wl);
+
 	return 0;
 }
 
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.h b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.h
index 947ccacf43e6..4090032e81a2 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.h
@@ -20,8 +20,10 @@
 #include <linux/timer.h>
 #include <linux/interrupt.h>
 #include <linux/workqueue.h>
+#include <linux/leds.h>
 
 #include "ucode_loader.h"
+#include "led.h"
 /*
  * Starting index for 5G rates in the
  * legacy rate table.
@@ -81,6 +83,8 @@ struct brcms_info {
 	struct wiphy *wiphy;
 	struct brcms_ucode ucode;
 	bool mute_tx;
+	struct brcms_led radio_led;
+	struct led_classdev led_dev;
 };
 
 /* misc callbacks */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/main.c b/drivers/net/wireless/brcm80211/brcmsmac/main.c
index 8ef02dca8f8c..28e7aeedd184 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/main.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/main.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2010 Broadcom Corporation
+ * Copyright (c) 2013 Hauke Mehrtens <hauke@hauke-m.de>
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -448,6 +449,10 @@ static void brcms_c_detach_mfree(struct brcms_c_info *wlc)
 	kfree(wlc->corestate);
 	kfree(wlc->hw->bandstate[0]);
 	kfree(wlc->hw);
+	if (wlc->beacon)
+		dev_kfree_skb_any(wlc->beacon);
+	if (wlc->probe_resp)
+		dev_kfree_skb_any(wlc->probe_resp);
 
 	/* free the wlc */
 	kfree(wlc);
@@ -1069,7 +1074,7 @@ brcms_b_txstatus(struct brcms_hardware *wlc_hw, bool bound, bool *fatal)
 
 static void brcms_c_tbtt(struct brcms_c_info *wlc)
 {
-	if (!wlc->bsscfg->BSS)
+	if (wlc->bsscfg->type == BRCMS_TYPE_ADHOC)
 		/*
 		 * DirFrmQ is now valid...defer setting until end
 		 * of ATIM window
@@ -2163,6 +2168,32 @@ void brcms_b_switch_macfreq(struct brcms_hardware *wlc_hw, u8 spurmode)
 	}
 }
 
+void brcms_c_start_station(struct brcms_c_info *wlc, u8 *addr)
+{
+	memcpy(wlc->pub->cur_etheraddr, addr, sizeof(wlc->pub->cur_etheraddr));
+	wlc->bsscfg->type = BRCMS_TYPE_STATION;
+}
+
+void brcms_c_start_ap(struct brcms_c_info *wlc, u8 *addr, const u8 *bssid,
+		      u8 *ssid, size_t ssid_len)
+{
+	brcms_c_set_ssid(wlc, ssid, ssid_len);
+
+	memcpy(wlc->pub->cur_etheraddr, addr, sizeof(wlc->pub->cur_etheraddr));
+	memcpy(wlc->bsscfg->BSSID, bssid, sizeof(wlc->bsscfg->BSSID));
+	wlc->bsscfg->type = BRCMS_TYPE_AP;
+
+	brcms_b_mctrl(wlc->hw, MCTL_AP | MCTL_INFRA, MCTL_AP | MCTL_INFRA);
+}
+
+void brcms_c_start_adhoc(struct brcms_c_info *wlc, u8 *addr)
+{
+	memcpy(wlc->pub->cur_etheraddr, addr, sizeof(wlc->pub->cur_etheraddr));
+	wlc->bsscfg->type = BRCMS_TYPE_ADHOC;
+
+	brcms_b_mctrl(wlc->hw, MCTL_AP | MCTL_INFRA, 0);
+}
+
 /* Initialize GPIOs that are controlled by D11 core */
 static void brcms_c_gpio_init(struct brcms_c_info *wlc)
 {
@@ -3043,8 +3074,6 @@ static void brcms_b_antsel_set(struct brcms_hardware *wlc_hw, u32 antsel_avail)
  */
 static bool brcms_c_ps_allowed(struct brcms_c_info *wlc)
 {
-	struct brcms_bss_cfg *cfg = wlc->bsscfg;
-
 	/* disallow PS when one of the following global conditions meets */
 	if (!wlc->pub->associated)
 		return false;
@@ -3053,16 +3082,11 @@ static bool brcms_c_ps_allowed(struct brcms_c_info *wlc)
 	if (wlc->filter_flags & FIF_PROMISC_IN_BSS)
 		return false;
 
-	if (cfg->associated) {
-		/*
-		 * disallow PS when one of the following
-		 * bsscfg specific conditions meets
-		 */
-		if (!cfg->BSS)
-			return false;
+	if (wlc->bsscfg->type == BRCMS_TYPE_AP)
+		return false;
 
+	if (wlc->bsscfg->type == BRCMS_TYPE_ADHOC)
 		return false;
-	}
 
 	return true;
 }
@@ -3771,7 +3795,7 @@ static int brcms_c_set_mac(struct brcms_bss_cfg *bsscfg)
 	struct brcms_c_info *wlc = bsscfg->wlc;
 
 	/* enter the MAC addr into the RXE match registers */
-	brcms_c_set_addrmatch(wlc, RCM_MAC_OFFSET, bsscfg->cur_etheraddr);
+	brcms_c_set_addrmatch(wlc, RCM_MAC_OFFSET, wlc->pub->cur_etheraddr);
 
 	brcms_c_ampdu_macaddr_upd(wlc);
 
@@ -3787,6 +3811,15 @@ static void brcms_c_set_bssid(struct brcms_bss_cfg *bsscfg)
 	brcms_c_set_addrmatch(bsscfg->wlc, RCM_BSSID_OFFSET, bsscfg->BSSID);
 }
 
+void brcms_c_set_ssid(struct brcms_c_info *wlc, u8 *ssid, size_t ssid_len)
+{
+	u8 len = min_t(u8, sizeof(wlc->bsscfg->SSID), ssid_len);
+	memset(wlc->bsscfg->SSID, 0, sizeof(wlc->bsscfg->SSID));
+
+	memcpy(wlc->bsscfg->SSID, ssid, len);
+	wlc->bsscfg->SSID_len = len;
+}
+
 static void brcms_b_set_shortslot(struct brcms_hardware *wlc_hw, bool shortslot)
 {
 	wlc_hw->shortslot = shortslot;
@@ -3821,7 +3854,7 @@ static void brcms_c_set_home_chanspec(struct brcms_c_info *wlc, u16 chanspec)
 	if (wlc->home_chanspec != chanspec) {
 		wlc->home_chanspec = chanspec;
 
-		if (wlc->bsscfg->associated)
+		if (wlc->pub->associated)
 			wlc->bsscfg->current_bss->chanspec = chanspec;
 	}
 }
@@ -4091,10 +4124,14 @@ void brcms_c_wme_setparams(struct brcms_c_info *wlc, u16 aci,
 					  *shm_entry++);
 	}
 
-	if (suspend) {
+	if (suspend)
 		brcms_c_suspend_mac_and_wait(wlc);
+
+	brcms_c_update_beacon(wlc);
+	brcms_c_update_probe_resp(wlc, false);
+
+	if (suspend)
 		brcms_c_enable_mac(wlc);
-	}
 }
 
 static void brcms_c_edcf_setparams(struct brcms_c_info *wlc, bool suspend)
@@ -4332,7 +4369,6 @@ static void brcms_c_info_init(struct brcms_c_info *wlc, int unit)
 
 	/* WME QoS mode is Auto by default */
 	wlc->pub->_ampdu = AMPDU_AGG_HOST;
-	wlc->pub->bcmerror = 0;
 }
 
 static uint brcms_c_attach_module(struct brcms_c_info *wlc)
@@ -5072,8 +5108,8 @@ int brcms_c_up(struct brcms_c_info *wlc)
 				struct brcms_bss_cfg *bsscfg = wlc->bsscfg;
 				mboolset(wlc->pub->radio_disabled,
 					 WL_RADIO_HW_DISABLE);
-
-				if (bsscfg->enable && bsscfg->BSS)
+				if (bsscfg->type == BRCMS_TYPE_STATION ||
+				    bsscfg->type == BRCMS_TYPE_ADHOC)
 					brcms_err(wlc->hw->d11core,
 						  "wl%d: up: rfdisable -> "
 						  "bsscfg_disable()\n",
@@ -5099,7 +5135,7 @@ int brcms_c_up(struct brcms_c_info *wlc)
 	wlc->pub->up = true;
 
 	if (wlc->bandinit_pending) {
-		ch = wlc->pub->ieee_hw->conf.channel;
+		ch = wlc->pub->ieee_hw->conf.chandef.chan;
 		brcms_c_suspend_mac_and_wait(wlc);
 		brcms_c_set_chanspec(wlc, ch20mhz_chspec(ch->hw_value));
 		wlc->bandinit_pending = false;
@@ -5434,7 +5470,7 @@ static void brcms_c_ofdm_rateset_war(struct brcms_c_info *wlc)
 	u8 r;
 	bool war = false;
 
-	if (wlc->bsscfg->associated)
+	if (wlc->pub->associated)
 		r = wlc->bsscfg->current_bss->rateset.rates[0];
 	else
 		r = wlc->default_bss->rateset.rates[0];
@@ -5528,7 +5564,7 @@ int brcms_c_set_rateset(struct brcms_c_info *wlc, struct brcm_rateset *rs)
 	/* merge rateset coming in with the current mcsset */
 	if (wlc->pub->_n_enab & SUPPORT_11N) {
 		struct brcms_bss_info *mcsset_bss;
-		if (wlc->bsscfg->associated)
+		if (wlc->pub->associated)
 			mcsset_bss = wlc->bsscfg->current_bss;
 		else
 			mcsset_bss = wlc->default_bss;
@@ -5543,12 +5579,36 @@ int brcms_c_set_rateset(struct brcms_c_info *wlc, struct brcm_rateset *rs)
 	return bcmerror;
 }
 
+static void brcms_c_time_lock(struct brcms_c_info *wlc)
+{
+	bcma_set32(wlc->hw->d11core, D11REGOFFS(maccontrol), MCTL_TBTTHOLD);
+	/* Commit the write */
+	bcma_read32(wlc->hw->d11core, D11REGOFFS(maccontrol));
+}
+
+static void brcms_c_time_unlock(struct brcms_c_info *wlc)
+{
+	bcma_mask32(wlc->hw->d11core, D11REGOFFS(maccontrol), ~MCTL_TBTTHOLD);
+	/* Commit the write */
+	bcma_read32(wlc->hw->d11core, D11REGOFFS(maccontrol));
+}
+
 int brcms_c_set_beacon_period(struct brcms_c_info *wlc, u16 period)
 {
+	u32 bcnint_us;
+
 	if (period == 0)
 		return -EINVAL;
 
 	wlc->default_bss->beacon_period = period;
+
+	bcnint_us = period << 10;
+	brcms_c_time_lock(wlc);
+	bcma_write32(wlc->hw->d11core, D11REGOFFS(tsf_cfprep),
+		     (bcnint_us << CFPREP_CBI_SHIFT));
+	bcma_write32(wlc->hw->d11core, D11REGOFFS(tsf_cfpstart), bcnint_us);
+	brcms_c_time_unlock(wlc);
+
 	return 0;
 }
 
@@ -7291,72 +7351,110 @@ brcms_c_mod_prb_rsp_rate_table(struct brcms_c_info *wlc, uint frame_len)
 	}
 }
 
-/*	Max buffering needed for beacon template/prb resp template is 142 bytes.
- *
- *	PLCP header is 6 bytes.
- *	802.11 A3 header is 24 bytes.
- *	Max beacon frame body template length is 112 bytes.
- *	Max probe resp frame body template length is 110 bytes.
- *
- *      *len on input contains the max length of the packet available.
- *
- *	The *len value is set to the number of bytes in buf used, and starts
- *	with the PLCP and included up to, but not including, the 4 byte FCS.
- */
-static void
-brcms_c_bcn_prb_template(struct brcms_c_info *wlc, u16 type,
-			 u32 bcn_rspec,
-			 struct brcms_bss_cfg *cfg, u16 *buf, int *len)
+int brcms_c_get_header_len(void)
 {
-	static const u8 ether_bcast[ETH_ALEN] = {255, 255, 255, 255, 255, 255};
-	struct cck_phy_hdr *plcp;
-	struct ieee80211_mgmt *h;
-	int hdr_len, body_len;
-
-	hdr_len = D11_PHY_HDR_LEN + DOT11_MAC_HDR_LEN;
+	return TXOFF;
+}
 
-	/* calc buffer size provided for frame body */
-	body_len = *len - hdr_len;
-	/* return actual size */
-	*len = hdr_len + body_len;
+static void brcms_c_beacon_write(struct brcms_c_info *wlc,
+				 struct sk_buff *beacon, u16 tim_offset,
+				 u16 dtim_period, bool bcn0, bool bcn1)
+{
+	size_t len;
+	struct ieee80211_tx_info *tx_info;
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	struct ieee80211_hw *ieee_hw = brcms_c_pub(wlc)->ieee_hw;
 
-	/* format PHY and MAC headers */
-	memset(buf, 0, hdr_len);
+	/* Get tx_info */
+	tx_info = IEEE80211_SKB_CB(beacon);
 
-	plcp = (struct cck_phy_hdr *) buf;
+	len = min_t(size_t, beacon->len, BCN_TMPL_LEN);
+	wlc->bcn_rspec = ieee80211_get_tx_rate(ieee_hw, tx_info)->hw_value;
 
-	/*
-	 * PLCP for Probe Response frames are filled in from
-	 * core's rate table
-	 */
-	if (type == IEEE80211_STYPE_BEACON)
-		/* fill in PLCP */
-		brcms_c_compute_plcp(wlc, bcn_rspec,
-				 (DOT11_MAC_HDR_LEN + body_len + FCS_LEN),
-				 (u8 *) plcp);
+	brcms_c_compute_plcp(wlc, wlc->bcn_rspec,
+			     len + FCS_LEN - D11_PHY_HDR_LEN, beacon->data);
 
 	/* "Regular" and 16 MBSS but not for 4 MBSS */
 	/* Update the phytxctl for the beacon based on the rspec */
-	brcms_c_beacon_phytxctl_txant_upd(wlc, bcn_rspec);
+	brcms_c_beacon_phytxctl_txant_upd(wlc, wlc->bcn_rspec);
 
-	h = (struct ieee80211_mgmt *)&plcp[1];
+	if (bcn0) {
+		/* write the probe response into the template region */
+		brcms_b_write_template_ram(wlc_hw, T_BCN0_TPL_BASE,
+					    (len + 3) & ~3, beacon->data);
 
-	/* fill in 802.11 header */
-	h->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT | type);
+		/* write beacon length to SCR */
+		brcms_b_write_shm(wlc_hw, M_BCN0_FRM_BYTESZ, (u16) len);
+	}
+	if (bcn1) {
+		/* write the probe response into the template region */
+		brcms_b_write_template_ram(wlc_hw, T_BCN1_TPL_BASE,
+					    (len + 3) & ~3, beacon->data);
 
-	/* DUR is 0 for multicast bcn, or filled in by MAC for prb resp */
-	/* A1 filled in by MAC for prb resp, broadcast for bcn */
-	if (type == IEEE80211_STYPE_BEACON)
-		memcpy(&h->da, &ether_bcast, ETH_ALEN);
-	memcpy(&h->sa, &cfg->cur_etheraddr, ETH_ALEN);
-	memcpy(&h->bssid, &cfg->BSSID, ETH_ALEN);
+		/* write beacon length to SCR */
+		brcms_b_write_shm(wlc_hw, M_BCN1_FRM_BYTESZ, (u16) len);
+	}
 
-	/* SEQ filled in by MAC */
+	if (tim_offset != 0) {
+		brcms_b_write_shm(wlc_hw, M_TIMBPOS_INBEACON,
+				  tim_offset + D11B_PHY_HDR_LEN);
+		brcms_b_write_shm(wlc_hw, M_DOT11_DTIMPERIOD, dtim_period);
+	} else {
+		brcms_b_write_shm(wlc_hw, M_TIMBPOS_INBEACON,
+				  len + D11B_PHY_HDR_LEN);
+		brcms_b_write_shm(wlc_hw, M_DOT11_DTIMPERIOD, 0);
+	}
 }
 
-int brcms_c_get_header_len(void)
+static void brcms_c_update_beacon_hw(struct brcms_c_info *wlc,
+				     struct sk_buff *beacon, u16 tim_offset,
+				     u16 dtim_period)
 {
-	return TXOFF;
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	struct bcma_device *core = wlc_hw->d11core;
+
+	/* Hardware beaconing for this config */
+	u32 both_valid = MCMD_BCN0VLD | MCMD_BCN1VLD;
+
+	/* Check if both templates are in use, if so sched. an interrupt
+	 *      that will call back into this routine
+	 */
+	if ((bcma_read32(core, D11REGOFFS(maccommand)) & both_valid) == both_valid)
+		/* clear any previous status */
+		bcma_write32(core, D11REGOFFS(macintstatus), MI_BCNTPL);
+
+	if (wlc->beacon_template_virgin) {
+		wlc->beacon_template_virgin = false;
+		brcms_c_beacon_write(wlc, beacon, tim_offset, dtim_period, true,
+				     true);
+		/* mark beacon0 valid */
+		bcma_set32(core, D11REGOFFS(maccommand), MCMD_BCN0VLD);
+		return;
+	}
+
+	/* Check that after scheduling the interrupt both of the
+	 *      templates are still busy. if not clear the int. & remask
+	 */
+	if ((bcma_read32(core, D11REGOFFS(maccommand)) & both_valid) == both_valid) {
+		wlc->defmacintmask |= MI_BCNTPL;
+		return;
+	}
+
+	if (!(bcma_read32(core, D11REGOFFS(maccommand)) & MCMD_BCN0VLD)) {
+		brcms_c_beacon_write(wlc, beacon, tim_offset, dtim_period, true,
+				     false);
+		/* mark beacon0 valid */
+		bcma_set32(core, D11REGOFFS(maccommand), MCMD_BCN0VLD);
+		return;
+	}
+	if (!(bcma_read32(core, D11REGOFFS(maccommand)) & MCMD_BCN1VLD)) {
+		brcms_c_beacon_write(wlc, beacon, tim_offset, dtim_period,
+				     false, true);
+		/* mark beacon0 valid */
+		bcma_set32(core, D11REGOFFS(maccommand), MCMD_BCN1VLD);
+		return;
+	}
+	return;
 }
 
 /*
@@ -7366,9 +7464,57 @@ void brcms_c_update_beacon(struct brcms_c_info *wlc)
 {
 	struct brcms_bss_cfg *bsscfg = wlc->bsscfg;
 
-	if (bsscfg->up && !bsscfg->BSS)
+	if (wlc->pub->up && (bsscfg->type == BRCMS_TYPE_AP ||
+			     bsscfg->type == BRCMS_TYPE_ADHOC)) {
 		/* Clear the soft intmask */
 		wlc->defmacintmask &= ~MI_BCNTPL;
+		if (!wlc->beacon)
+			return;
+		brcms_c_update_beacon_hw(wlc, wlc->beacon,
+					 wlc->beacon_tim_offset,
+					 wlc->beacon_dtim_period);
+	}
+}
+
+void brcms_c_set_new_beacon(struct brcms_c_info *wlc, struct sk_buff *beacon,
+			    u16 tim_offset, u16 dtim_period)
+{
+	if (!beacon)
+		return;
+	if (wlc->beacon)
+		dev_kfree_skb_any(wlc->beacon);
+	wlc->beacon = beacon;
+
+	/* add PLCP */
+	skb_push(wlc->beacon, D11_PHY_HDR_LEN);
+	wlc->beacon_tim_offset = tim_offset;
+	wlc->beacon_dtim_period = dtim_period;
+	brcms_c_update_beacon(wlc);
+}
+
+void brcms_c_set_new_probe_resp(struct brcms_c_info *wlc,
+				struct sk_buff *probe_resp)
+{
+	if (!probe_resp)
+		return;
+	if (wlc->probe_resp)
+		dev_kfree_skb_any(wlc->probe_resp);
+	wlc->probe_resp = probe_resp;
+
+	/* add PLCP */
+	skb_push(wlc->probe_resp, D11_PHY_HDR_LEN);
+	brcms_c_update_probe_resp(wlc, false);
+}
+
+void brcms_c_enable_probe_resp(struct brcms_c_info *wlc, bool enable)
+{
+	/*
+	 * prevent ucode from sending probe responses by setting the timeout
+	 * to 1, it can not send it in that time frame.
+	 */
+	wlc->prb_resp_timeout = enable ? BRCMS_PRB_RESP_TIMEOUT : 1;
+	brcms_b_write_shm(wlc->hw, M_PRS_MAXTIME, wlc->prb_resp_timeout);
+	/* TODO: if (enable) => also deactivate receiving of probe request */
 }
 
 /* Write ssid into shared memory */
@@ -7390,30 +7536,19 @@ brcms_c_shm_ssid_upd(struct brcms_c_info *wlc, struct brcms_bss_cfg *cfg)
 static void
 brcms_c_bss_update_probe_resp(struct brcms_c_info *wlc,
 			      struct brcms_bss_cfg *cfg,
+			      struct sk_buff *probe_resp,
 			      bool suspend)
 {
-	u16 *prb_resp;
-	int len = BCN_TMPL_LEN;
-
-	prb_resp = kmalloc(BCN_TMPL_LEN, GFP_ATOMIC);
-	if (!prb_resp)
-		return;
-
-	/*
-	 * write the probe response to hardware, or save in
-	 * the config structure
-	 */
+	int len;
 
-	/* create the probe response template */
-	brcms_c_bcn_prb_template(wlc, IEEE80211_STYPE_PROBE_RESP, 0,
-				 cfg, prb_resp, &len);
+	len = min_t(size_t, probe_resp->len, BCN_TMPL_LEN);
 
 	if (suspend)
 		brcms_c_suspend_mac_and_wait(wlc);
 
 	/* write the probe response into the template region */
 	brcms_b_write_template_ram(wlc->hw, T_PRS_TPL_BASE,
-				    (len + 3) & ~3, prb_resp);
+				    (len + 3) & ~3, probe_resp->data);
 
 	/* write the length of the probe response frame (+PLCP/-FCS) */
 	brcms_b_write_shm(wlc->hw, M_PRB_RESP_FRM_LEN, (u16) len);
@@ -7427,13 +7562,11 @@ brcms_c_bss_update_probe_resp(struct brcms_c_info *wlc,
 	 * PLCP header for the call to brcms_c_mod_prb_rsp_rate_table()
 	 * by subtracting the PLCP len and adding the FCS.
 	 */
-	len += (-D11_PHY_HDR_LEN + FCS_LEN);
-	brcms_c_mod_prb_rsp_rate_table(wlc, (u16) len);
+	brcms_c_mod_prb_rsp_rate_table(wlc,
+				      (u16)len + FCS_LEN - D11_PHY_HDR_LEN);
 
 	if (suspend)
 		brcms_c_enable_mac(wlc);
-
-	kfree(prb_resp);
 }
 
 void brcms_c_update_probe_resp(struct brcms_c_info *wlc, bool suspend)
@@ -7441,8 +7574,13 @@ void brcms_c_update_probe_resp(struct brcms_c_info *wlc, bool suspend)
 	struct brcms_bss_cfg *bsscfg = wlc->bsscfg;
 
 	/* update AP or IBSS probe responses */
-	if (bsscfg->up && !bsscfg->BSS)
-		brcms_c_bss_update_probe_resp(wlc, bsscfg, suspend);
+	if (wlc->pub->up && (bsscfg->type == BRCMS_TYPE_AP ||
+			     bsscfg->type == BRCMS_TYPE_ADHOC)) {
+		if (!wlc->probe_resp)
+			return;
+		brcms_c_bss_update_probe_resp(wlc, bsscfg, wlc->probe_resp,
+					      suspend);
+	}
 }
 
 int brcms_b_xmtfifo_sz_get(struct brcms_hardware *wlc_hw, uint fifo,
@@ -7481,7 +7619,6 @@ void brcms_c_scan_stop(struct brcms_c_info *wlc)
 void brcms_c_associate_upd(struct brcms_c_info *wlc, bool state)
 {
 	wlc->pub->associated = state;
-	wlc->bsscfg->associated = state;
 }
 
 /*
@@ -7526,6 +7663,36 @@ void brcms_c_set_beacon_listen_interval(struct brcms_c_info *wlc, u8 interval)
 		brcms_c_bcn_li_upd(wlc);
 }
 
+u64 brcms_c_tsf_get(struct brcms_c_info *wlc)
+{
+	u32 tsf_h, tsf_l;
+	u64 tsf;
+
+	brcms_b_read_tsf(wlc->hw, &tsf_l, &tsf_h);
+
+	tsf = tsf_h;
+	tsf <<= 32;
+	tsf |= tsf_l;
+
+	return tsf;
+}
+
+void brcms_c_tsf_set(struct brcms_c_info *wlc, u64 tsf)
+{
+	u32 tsf_h, tsf_l;
+
+	brcms_c_time_lock(wlc);
+
+	tsf_l = tsf;
+	tsf_h = (tsf >> 32);
+
+	/* read the tsf timer low, then high to get an atomic read */
+	bcma_write32(wlc->hw->d11core, D11REGOFFS(tsf_timerlow), tsf_l);
+	bcma_write32(wlc->hw->d11core, D11REGOFFS(tsf_timerhigh), tsf_h);
+
+	brcms_c_time_unlock(wlc);
+}
+
 int brcms_c_set_tx_power(struct brcms_c_info *wlc, int txpwr)
 {
 	uint qdbm;
@@ -7737,6 +7904,10 @@ bool brcms_c_dpc(struct brcms_c_info *wlc, bool bounded)
 		brcms_rfkill_set_hw_state(wlc->wl);
 	}
 
+	/* BCN template is available */
+	if (macintstatus & MI_BCNTPL)
+		brcms_c_update_beacon(wlc);
+
 	/* it isn't done and needs to be resched if macintstatus is non-zero */
 	return wlc->macintstatus != 0;
 
@@ -7748,7 +7919,7 @@ bool brcms_c_dpc(struct brcms_c_info *wlc, bool bounded)
 void brcms_c_init(struct brcms_c_info *wlc, bool mute_tx)
 {
 	struct bcma_device *core = wlc->hw->d11core;
-	struct ieee80211_channel *ch = wlc->pub->ieee_hw->conf.channel;
+	struct ieee80211_channel *ch = wlc->pub->ieee_hw->conf.chandef.chan;
 	u16 chanspec;
 
 	brcms_dbg_info(core, "wl%d\n", wlc->pub->unit);
@@ -7765,7 +7936,7 @@ void brcms_c_init(struct brcms_c_info *wlc, bool mute_tx)
 	brcms_c_set_bssid(wlc->bsscfg);
 
 	/* Update tsf_cfprep if associated and up */
-	if (wlc->pub->associated && wlc->bsscfg->up) {
+	if (wlc->pub->associated && wlc->pub->up) {
 		u32 bi;
 
 		/* get beacon period and convert to uS */
@@ -7810,9 +7981,14 @@ void brcms_c_init(struct brcms_c_info *wlc, bool mute_tx)
 
 	/* read the ucode version if we have not yet done so */
 	if (wlc->ucode_rev == 0) {
-		wlc->ucode_rev =
-		    brcms_b_read_shm(wlc->hw, M_BOM_REV_MAJOR) << NBITS(u16);
-		wlc->ucode_rev |= brcms_b_read_shm(wlc->hw, M_BOM_REV_MINOR);
+		u16 rev;
+		u16 patch;
+
+		rev = brcms_b_read_shm(wlc->hw, M_BOM_REV_MAJOR);
+		patch = brcms_b_read_shm(wlc->hw, M_BOM_REV_MINOR);
+		wlc->ucode_rev = (rev << NBITS(u16)) | patch;
+		snprintf(wlc->wiphy->fw_version,
+			 sizeof(wlc->wiphy->fw_version), "%u.%u", rev, patch);
 	}
 
 	/* ..now really unleash hell (allow the MAC out of suspend) */
@@ -7868,6 +8044,7 @@ brcms_c_attach(struct brcms_info *wl, struct bcma_device *core, uint unit,
 	pub->unit = unit;
 	pub->_piomode = piomode;
 	wlc->bandinit_pending = false;
+	wlc->beacon_template_virgin = true;
 
 	/* populate struct brcms_c_info with default values  */
 	brcms_c_info_init(wlc, unit);
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/main.h b/drivers/net/wireless/brcm80211/brcmsmac/main.h
index fb447747c2c6..b5d7a38b53fe 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/main.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/main.h
@@ -492,6 +492,8 @@ struct brcms_c_info {
 	bool radio_monitor;
 	bool going_down;
 
+	bool beacon_template_virgin;
+
 	struct brcms_timer *wdtimer;
 	struct brcms_timer *radio_timer;
 
@@ -561,6 +563,11 @@ struct brcms_c_info {
 
 	struct wiphy *wiphy;
 	struct scb pri_scb;
+
+	struct sk_buff *beacon;
+	u16 beacon_tim_offset;
+	u16 beacon_dtim_period;
+	struct sk_buff *probe_resp;
 };
 
 /* antsel module specific state */
@@ -576,14 +583,17 @@ struct antsel_info {
 	struct brcms_antselcfg antcfg_cur; /* current antenna config (auto) */
 };
 
+enum brcms_bss_type {
+	BRCMS_TYPE_STATION,
+	BRCMS_TYPE_AP,
+	BRCMS_TYPE_ADHOC,
+};
+
 /*
  * BSS configuration state
  *
  * wlc: wlc to which this bsscfg belongs to.
- * up: is this configuration up operational
- * enable: is this configuration enabled
- * associated: is BSS in ASSOCIATED state
- * BSS: infraustructure or adhoc
+ * type: interface type
  * SSID_len: the length of SSID
  * SSID: SSID string
  *
@@ -599,14 +609,10 @@ struct antsel_info {
  */
 struct brcms_bss_cfg {
 	struct brcms_c_info *wlc;
-	bool up;
-	bool enable;
-	bool associated;
-	bool BSS;
+	enum brcms_bss_type type;
 	u8 SSID_len;
 	u8 SSID[IEEE80211_MAX_SSID_LEN];
 	u8 BSSID[ETH_ALEN];
-	u8 cur_etheraddr[ETH_ALEN];
 	struct brcms_bss_info *current_bss;
 };
 
@@ -631,7 +637,6 @@ extern u16 brcms_c_compute_rtscts_dur(struct brcms_c_info *wlc, bool cts_only,
 extern void brcms_c_inval_dma_pkts(struct brcms_hardware *hw,
 			       struct ieee80211_sta *sta,
 			       void (*dma_callback_fn));
-extern void brcms_c_update_beacon(struct brcms_c_info *wlc);
 extern void brcms_c_update_probe_resp(struct brcms_c_info *wlc, bool suspend);
 extern int brcms_c_set_nmode(struct brcms_c_info *wlc);
 extern void brcms_c_beacon_phytxctl_txant_upd(struct brcms_c_info *wlc,
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c
index 91937c5025ce..b0fd807f2b2b 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c
@@ -198,8 +198,6 @@ u16 read_radio_reg(struct brcms_phy *pi, u16 addr)
 
 void write_radio_reg(struct brcms_phy *pi, u16 addr, u16 val)
 {
-	struct si_info *sii = container_of(pi->sh->sih, struct si_info, pub);
-
 	if ((D11REV_GE(pi->sh->corerev, 24)) ||
 	    (D11REV_IS(pi->sh->corerev, 22)
 	     && (pi->pubpi.phy_type != PHY_TYPE_SSN))) {
@@ -211,7 +209,7 @@ void write_radio_reg(struct brcms_phy *pi, u16 addr, u16 val)
 		bcma_write16(pi->d11core, D11REGOFFS(phy4wdatalo), val);
 	}
 
-	if ((sii->icbus->hosttype == BCMA_HOSTTYPE_PCI) &&
+	if ((pi->d11core->bus->hosttype == BCMA_HOSTTYPE_PCI) &&
 	    (++pi->phy_wreg >= pi->phy_wreg_limit)) {
 		(void)bcma_read32(pi->d11core, D11REGOFFS(maccontrol));
 		pi->phy_wreg = 0;
@@ -297,10 +295,8 @@ void write_phy_reg(struct brcms_phy *pi, u16 addr, u16 val)
 	if (addr == 0x72)
 		(void)bcma_read16(pi->d11core, D11REGOFFS(phyregdata));
 #else
-	struct si_info *sii = container_of(pi->sh->sih, struct si_info, pub);
-
 	bcma_write32(pi->d11core, D11REGOFFS(phyregaddr), addr | (val << 16));
-	if ((sii->icbus->hosttype == BCMA_HOSTTYPE_PCI) &&
+	if ((pi->d11core->bus->hosttype == BCMA_HOSTTYPE_PCI) &&
 	    (++pi->phy_wreg >= pi->phy_wreg_limit)) {
 		pi->phy_wreg = 0;
 		(void)bcma_read16(pi->d11core, D11REGOFFS(phyversion));
@@ -374,7 +370,6 @@ struct shared_phy *wlc_phy_shared_attach(struct shared_phy_params *shp)
 	if (sh == NULL)
 		return NULL;
 
-	sh->sih = shp->sih;
 	sh->physhim = shp->physhim;
 	sh->unit = shp->unit;
 	sh->corerev = shp->corerev;
@@ -2911,29 +2906,24 @@ void wlc_lcnphy_epa_switch(struct brcms_phy *pi, bool mode)
 				mod_phy_reg(pi, 0x44c, (0x1 << 2), (1) << 2);
 
 			}
-			ai_cc_reg(pi->sh->sih,
-				  offsetof(struct chipcregs, gpiocontrol),
-				  ~0x0, 0x0);
-			ai_cc_reg(pi->sh->sih,
-				  offsetof(struct chipcregs, gpioout),
-				  0x40, 0x40);
-			ai_cc_reg(pi->sh->sih,
-				  offsetof(struct chipcregs, gpioouten),
-				  0x40, 0x40);
+
+			bcma_chipco_gpio_control(&pi->d11core->bus->drv_cc,
+						 0x0, 0x0);
+			bcma_chipco_gpio_out(&pi->d11core->bus->drv_cc,
+					     ~0x40, 0x40);
+			bcma_chipco_gpio_outen(&pi->d11core->bus->drv_cc,
+					       ~0x40, 0x40);
 		} else {
 			mod_phy_reg(pi, 0x44c, (0x1 << 2), (0) << 2);
 
 			mod_phy_reg(pi, 0x44d, (0x1 << 2), (0) << 2);
 
-			ai_cc_reg(pi->sh->sih,
-				  offsetof(struct chipcregs, gpioout),
-				  0x40, 0x00);
-			ai_cc_reg(pi->sh->sih,
-				  offsetof(struct chipcregs, gpioouten),
-				  0x40, 0x0);
-			ai_cc_reg(pi->sh->sih,
-				  offsetof(struct chipcregs, gpiocontrol),
-				  ~0x0, 0x40);
+			bcma_chipco_gpio_out(&pi->d11core->bus->drv_cc,
+					     ~0x40, 0x00);
+			bcma_chipco_gpio_outen(&pi->d11core->bus->drv_cc,
+					       ~0x40, 0x00);
+			bcma_chipco_gpio_control(&pi->d11core->bus->drv_cc,
+						 0x0, 0x40);
 		}
 	}
 }
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_int.h b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_int.h
index af00e2c2b266..1dc767c31653 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_int.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_int.h
@@ -488,7 +488,6 @@ struct lcnphy_cal_results {
 struct shared_phy {
 	struct brcms_phy *phy_head;
 	uint unit;
-	struct si_pub *sih;
 	struct phy_shim_info *physhim;
 	uint corerev;
 	u32 machwcap;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
index 18d37645e2cd..3d6b16ce4687 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
@@ -1595,11 +1595,15 @@ wlc_lcnphy_set_chanspec_tweaks(struct brcms_phy *pi, u16 chanspec)
 	if (channel == 1 || channel == 2 || channel == 3 ||
 	    channel == 4 || channel == 9 ||
 	    channel == 10 || channel == 11 || channel == 12) {
-		si_pmu_pllcontrol(pi->sh->sih, 0x2, 0xffffffff, 0x03000c04);
-		si_pmu_pllcontrol(pi->sh->sih, 0x3, 0xffffff, 0x0);
-		si_pmu_pllcontrol(pi->sh->sih, 0x4, 0xffffffff, 0x200005c0);
-
-		si_pmu_pllupd(pi->sh->sih);
+		bcma_chipco_pll_write(&pi->d11core->bus->drv_cc, 0x2,
+				      0x03000c04);
+		bcma_chipco_pll_maskset(&pi->d11core->bus->drv_cc, 0x3,
+					~0x00ffffff, 0x0);
+		bcma_chipco_pll_write(&pi->d11core->bus->drv_cc, 0x4,
+				      0x200005c0);
+
+		bcma_cc_set32(&pi->d11core->bus->drv_cc, BCMA_CC_PMU_CTL,
+			      BCMA_CC_PMU_CTL_PLL_UPD);
 		write_phy_reg(pi, 0x942, 0);
 		wlc_lcnphy_txrx_spur_avoidance_mode(pi, false);
 		pi_lcn->lcnphy_spurmod = false;
@@ -1607,11 +1611,15 @@ wlc_lcnphy_set_chanspec_tweaks(struct brcms_phy *pi, u16 chanspec)
 
 		write_phy_reg(pi, 0x425, 0x5907);
 	} else {
-		si_pmu_pllcontrol(pi->sh->sih, 0x2, 0xffffffff, 0x03140c04);
-		si_pmu_pllcontrol(pi->sh->sih, 0x3, 0xffffff, 0x333333);
-		si_pmu_pllcontrol(pi->sh->sih, 0x4, 0xffffffff, 0x202c2820);
-
-		si_pmu_pllupd(pi->sh->sih);
+		bcma_chipco_pll_write(&pi->d11core->bus->drv_cc, 0x2,
+				      0x03140c04);
+		bcma_chipco_pll_maskset(&pi->d11core->bus->drv_cc, 0x3,
+					~0x00ffffff, 0x333333);
+		bcma_chipco_pll_write(&pi->d11core->bus->drv_cc, 0x4,
+				      0x202c2820);
+
+		bcma_cc_set32(&pi->d11core->bus->drv_cc, BCMA_CC_PMU_CTL,
+			      BCMA_CC_PMU_CTL_PLL_UPD);
 		write_phy_reg(pi, 0x942, 0);
 		wlc_lcnphy_txrx_spur_avoidance_mode(pi, true);
 
@@ -4755,9 +4763,10 @@ void wlc_phy_init_lcnphy(struct brcms_phy *pi)
 
 	wlc_phy_chanspec_set((struct brcms_phy_pub *) pi, pi->radio_chanspec);
 
-	si_pmu_regcontrol(pi->sh->sih, 0, 0xf, 0x9);
+	bcma_chipco_regctl_maskset(&pi->d11core->bus->drv_cc, 0, ~0xf, 0x9);
 
-	si_pmu_chipcontrol(pi->sh->sih, 0, 0xffffffff, 0x03CDDDDD);
+	bcma_chipco_chipctl_maskset(&pi->d11core->bus->drv_cc, 0, 0x0,
+				    0x03CDDDDD);
 
 	if ((pi->sh->boardflags & BFL_FEM)
 	    && wlc_lcnphy_tempsense_based_pwr_ctrl_enabled(pi))
@@ -4968,7 +4977,7 @@ bool wlc_phy_attach_lcnphy(struct brcms_phy *pi)
 		pi->hwpwrctrl_capable = true;
 	}
 
-	pi->xtalfreq = si_pmu_alp_clock(pi->sh->sih);
+	pi->xtalfreq = bcma_chipco_get_alp_clock(&pi->d11core->bus->drv_cc);
 	pi_lcn->lcnphy_papd_rxGnCtrl_init = 0;
 
 	pi->pi_fptr.init = wlc_phy_init_lcnphy;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
index 65db9b7458dc..3e9f5b25be63 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
@@ -19321,14 +19321,13 @@ void wlc_phy_init_nphy(struct brcms_phy *pi)
 	     (pi->sh->chippkg == BCMA_PKG_ID_BCM4718))) {
 		if ((pi->sh->boardflags & BFL_EXTLNA) &&
 		    (CHSPEC_IS2G(pi->radio_chanspec)))
-			ai_cc_reg(pi->sh->sih,
-				  offsetof(struct chipcregs, chipcontrol),
-				  0x40, 0x40);
+			bcma_cc_set32(&pi->d11core->bus->drv_cc,
+				      BCMA_CC_CHIPCTL, 0x40);
 	}
 
 	if ((!PHY_IPA(pi)) && (pi->sh->chip == BCMA_CHIP_ID_BCM5357))
-		si_pmu_chipcontrol(pi->sh->sih, 1, CCTRL5357_EXTPA,
-				   CCTRL5357_EXTPA);
+		bcma_chipco_chipctl_maskset(&pi->d11core->bus->drv_cc, 1,
+					    ~CCTRL5357_EXTPA, CCTRL5357_EXTPA);
 
 	if ((pi->nphy_gband_spurwar2_en) && CHSPEC_IS2G(pi->radio_chanspec) &&
 	    CHSPEC_IS40(pi->radio_chanspec)) {
@@ -21133,7 +21132,6 @@ wlc_phy_chanspec_nphy_setup(struct brcms_phy *pi, u16 chanspec,
 			    const struct nphy_sfo_cfg *ci)
 {
 	u16 val;
-	struct si_info *sii = container_of(pi->sh->sih, struct si_info, pub);
 
 	val = read_phy_reg(pi, 0x09) & NPHY_BandControl_currentBand;
 	if (CHSPEC_IS5G(chanspec) && !val) {
@@ -21221,11 +21219,11 @@ wlc_phy_chanspec_nphy_setup(struct brcms_phy *pi, u16 chanspec,
 
 		if ((pi->sh->chip == BCMA_CHIP_ID_BCM4716) ||
 		    (pi->sh->chip == BCMA_CHIP_ID_BCM43225)) {
-			bcma_pmu_spuravoid_pllupdate(&sii->icbus->drv_cc,
+			bcma_pmu_spuravoid_pllupdate(&pi->d11core->bus->drv_cc,
 						     spuravoid);
 		} else {
 			wlapi_bmac_core_phypll_ctl(pi->sh->physhim, false);
-			bcma_pmu_spuravoid_pllupdate(&sii->icbus->drv_cc,
+			bcma_pmu_spuravoid_pllupdate(&pi->d11core->bus->drv_cc,
 						     spuravoid);
 			wlapi_bmac_core_phypll_ctl(pi->sh->physhim, true);
 		}
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/pmu.c b/drivers/net/wireless/brcm80211/brcmsmac/pmu.c
index 7e9df566c733..71b80381f3ad 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/pmu.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/pmu.c
@@ -115,60 +115,6 @@ u16 si_pmu_fast_pwrup_delay(struct si_pub *sih)
 	return (u16) delay;
 }
 
-/* Read/write a chipcontrol reg */
-u32 si_pmu_chipcontrol(struct si_pub *sih, uint reg, u32 mask, u32 val)
-{
-	ai_cc_reg(sih, offsetof(struct chipcregs, chipcontrol_addr), ~0, reg);
-	return ai_cc_reg(sih, offsetof(struct chipcregs, chipcontrol_data),
-			 mask, val);
-}
-
-/* Read/write a regcontrol reg */
-u32 si_pmu_regcontrol(struct si_pub *sih, uint reg, u32 mask, u32 val)
-{
-	ai_cc_reg(sih, offsetof(struct chipcregs, regcontrol_addr), ~0, reg);
-	return ai_cc_reg(sih, offsetof(struct chipcregs, regcontrol_data),
-			 mask, val);
-}
-
-/* Read/write a pllcontrol reg */
-u32 si_pmu_pllcontrol(struct si_pub *sih, uint reg, u32 mask, u32 val)
-{
-	ai_cc_reg(sih, offsetof(struct chipcregs, pllcontrol_addr), ~0, reg);
-	return ai_cc_reg(sih, offsetof(struct chipcregs, pllcontrol_data),
-			 mask, val);
-}
-
-/* PMU PLL update */
-void si_pmu_pllupd(struct si_pub *sih)
-{
-	ai_cc_reg(sih, offsetof(struct chipcregs, pmucontrol),
-		  PCTL_PLL_PLLCTL_UPD, PCTL_PLL_PLLCTL_UPD);
-}
-
-/* query alp/xtal clock frequency */
-u32 si_pmu_alp_clock(struct si_pub *sih)
-{
-	u32 clock = ALP_CLOCK;
-
-	/* bail out with default */
-	if (!(ai_get_cccaps(sih) & CC_CAP_PMU))
-		return clock;
-
-	switch (ai_get_chip_id(sih)) {
-	case BCMA_CHIP_ID_BCM43224:
-	case BCMA_CHIP_ID_BCM43225:
-	case BCMA_CHIP_ID_BCM4313:
-		/* always 20Mhz */
-		clock = 20000 * 1000;
-		break;
-	default:
-		break;
-	}
-
-	return clock;
-}
-
 u32 si_pmu_measure_alpclk(struct si_pub *sih)
 {
 	struct si_info *sii = container_of(sih, struct si_info, pub);
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/pmu.h b/drivers/net/wireless/brcm80211/brcmsmac/pmu.h
index f7cff873578b..20e2012d5a3a 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/pmu.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/pmu.h
@@ -21,12 +21,6 @@
 #include "types.h"
 
 extern u16 si_pmu_fast_pwrup_delay(struct si_pub *sih);
-extern void si_pmu_sprom_enable(struct si_pub *sih, bool enable);
-extern u32 si_pmu_chipcontrol(struct si_pub *sih, uint reg, u32 mask, u32 val);
-extern u32 si_pmu_regcontrol(struct si_pub *sih, uint reg, u32 mask, u32 val);
-extern u32 si_pmu_alp_clock(struct si_pub *sih);
-extern void si_pmu_pllupd(struct si_pub *sih);
-extern u32 si_pmu_pllcontrol(struct si_pub *sih, uint reg, u32 mask, u32 val);
 extern u32 si_pmu_measure_alpclk(struct si_pub *sih);
 
 #endif /* _BRCM_PMU_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/pub.h b/drivers/net/wireless/brcm80211/brcmsmac/pub.h
index b0f14b7b8616..d36ea5e1cc49 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/pub.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/pub.h
@@ -164,8 +164,6 @@ struct brcms_pub {
 
 	u8 cur_etheraddr[ETH_ALEN];	/* our local ethernet address */
 
-	int bcmerror;		/* last bcm error */
-
 	u32 radio_disabled;	/* bit vector for radio disabled reasons */
 
 	u16 boardrev;	/* version # of particular board */
@@ -326,10 +324,25 @@ extern void brcms_c_set_shortslot_override(struct brcms_c_info *wlc,
 				    s8 sslot_override);
 extern void brcms_c_set_beacon_listen_interval(struct brcms_c_info *wlc,
 					u8 interval);
+extern u64 brcms_c_tsf_get(struct brcms_c_info *wlc);
+extern void brcms_c_tsf_set(struct brcms_c_info *wlc, u64 tsf);
 extern int brcms_c_set_tx_power(struct brcms_c_info *wlc, int txpwr);
 extern int brcms_c_get_tx_power(struct brcms_c_info *wlc);
 extern bool brcms_c_check_radio_disabled(struct brcms_c_info *wlc);
 extern void brcms_c_mute(struct brcms_c_info *wlc, bool on);
 extern bool brcms_c_tx_flush_completed(struct brcms_c_info *wlc);
+extern void brcms_c_start_station(struct brcms_c_info *wlc, u8 *addr);
+extern void brcms_c_start_ap(struct brcms_c_info *wlc, u8 *addr,
+			     const u8 *bssid, u8 *ssid, size_t ssid_len);
+extern void brcms_c_start_adhoc(struct brcms_c_info *wlc, u8 *addr);
+extern void brcms_c_update_beacon(struct brcms_c_info *wlc);
+extern void brcms_c_set_new_beacon(struct brcms_c_info *wlc,
+				   struct sk_buff *beacon, u16 tim_offset,
+				   u16 dtim_period);
+extern void brcms_c_set_new_probe_resp(struct brcms_c_info *wlc,
+				       struct sk_buff *probe_resp);
+extern void brcms_c_enable_probe_resp(struct brcms_c_info *wlc, bool enable);
+extern void brcms_c_set_ssid(struct brcms_c_info *wlc, u8 *ssid,
+			     size_t ssid_len);
 
 #endif				/* _BRCM_PUB_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmutil/Makefile b/drivers/net/wireless/brcm80211/brcmutil/Makefile
index 6281c416289e..8a928184016a 100644
--- a/drivers/net/wireless/brcm80211/brcmutil/Makefile
+++ b/drivers/net/wireless/brcm80211/brcmutil/Makefile
@@ -19,10 +19,5 @@ ccflags-y :=				\
 	-Idrivers/net/wireless/brcm80211/brcmutil \
 	-Idrivers/net/wireless/brcm80211/include
 
-BRCMUTIL_OFILES := \
-	utils.o
-
-MODULEPFX := brcmutil
-
-obj-$(CONFIG_BRCMUTIL)	+= $(MODULEPFX).o
-$(MODULEPFX)-objs	= $(BRCMUTIL_OFILES)
+obj-$(CONFIG_BRCMUTIL)	+= brcmutil.o
+brcmutil-objs	= utils.o d11.o
diff --git a/drivers/net/wireless/brcm80211/brcmutil/d11.c b/drivers/net/wireless/brcm80211/brcmutil/d11.c
new file mode 100644
index 000000000000..30e54e2c6c9b
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmutil/d11.c
@@ -0,0 +1,162 @@
+/*
+ * Copyright (c) 2013 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*********************channel spec common functions*********************/
+
+#include <linux/module.h>
+
+#include <brcmu_utils.h>
+#include <brcmu_wifi.h>
+#include <brcmu_d11.h>
+
+static void brcmu_d11n_encchspec(struct brcmu_chan *ch)
+{
+	ch->chspec = ch->chnum & BRCMU_CHSPEC_CH_MASK;
+
+	switch (ch->bw) {
+	case BRCMU_CHAN_BW_20:
+		ch->chspec |= BRCMU_CHSPEC_D11N_BW_20 | BRCMU_CHSPEC_D11N_SB_N;
+		break;
+	case BRCMU_CHAN_BW_40:
+	default:
+		WARN_ON_ONCE(1);
+		break;
+	}
+
+	if (ch->chnum <= CH_MAX_2G_CHANNEL)
+		ch->chspec |= BRCMU_CHSPEC_D11N_BND_2G;
+	else
+		ch->chspec |= BRCMU_CHSPEC_D11N_BND_5G;
+}
+
+static void brcmu_d11ac_encchspec(struct brcmu_chan *ch)
+{
+	ch->chspec = ch->chnum & BRCMU_CHSPEC_CH_MASK;
+
+	switch (ch->bw) {
+	case BRCMU_CHAN_BW_20:
+		ch->chspec |= BRCMU_CHSPEC_D11AC_BW_20;
+		break;
+	case BRCMU_CHAN_BW_40:
+	case BRCMU_CHAN_BW_80:
+	case BRCMU_CHAN_BW_80P80:
+	case BRCMU_CHAN_BW_160:
+	default:
+		WARN_ON_ONCE(1);
+		break;
+	}
+
+	if (ch->chnum <= CH_MAX_2G_CHANNEL)
+		ch->chspec |= BRCMU_CHSPEC_D11AC_BND_2G;
+	else
+		ch->chspec |= BRCMU_CHSPEC_D11AC_BND_5G;
+}
+
+static void brcmu_d11n_decchspec(struct brcmu_chan *ch)
+{
+	u16 val;
+
+	ch->chnum = (u8)(ch->chspec & BRCMU_CHSPEC_CH_MASK);
+
+	switch (ch->chspec & BRCMU_CHSPEC_D11N_BW_MASK) {
+	case BRCMU_CHSPEC_D11N_BW_20:
+		ch->bw = BRCMU_CHAN_BW_20;
+		break;
+	case BRCMU_CHSPEC_D11N_BW_40:
+		ch->bw = BRCMU_CHAN_BW_40;
+		val = ch->chspec & BRCMU_CHSPEC_D11N_SB_MASK;
+		if (val == BRCMU_CHSPEC_D11N_SB_L) {
+			ch->sb = BRCMU_CHAN_SB_L;
+			ch->chnum -= CH_10MHZ_APART;
+		} else {
+			ch->sb = BRCMU_CHAN_SB_U;
+			ch->chnum += CH_10MHZ_APART;
+		}
+		break;
+	default:
+		WARN_ON_ONCE(1);
+		break;
+	}
+
+	switch (ch->chspec & BRCMU_CHSPEC_D11N_BND_MASK) {
+	case BRCMU_CHSPEC_D11N_BND_5G:
+		ch->band = BRCMU_CHAN_BAND_5G;
+		break;
+	case BRCMU_CHSPEC_D11N_BND_2G:
+		ch->band = BRCMU_CHAN_BAND_2G;
+		break;
+	default:
+		WARN_ON_ONCE(1);
+		break;
+	}
+}
+
+static void brcmu_d11ac_decchspec(struct brcmu_chan *ch)
+{
+	u16 val;
+
+	ch->chnum = (u8)(ch->chspec & BRCMU_CHSPEC_CH_MASK);
+
+	switch (ch->chspec & BRCMU_CHSPEC_D11AC_BW_MASK) {
+	case BRCMU_CHSPEC_D11AC_BW_20:
+		ch->bw = BRCMU_CHAN_BW_20;
+		break;
+	case BRCMU_CHSPEC_D11AC_BW_40:
+		ch->bw = BRCMU_CHAN_BW_40;
+		val = ch->chspec & BRCMU_CHSPEC_D11AC_SB_MASK;
+		if (val == BRCMU_CHSPEC_D11AC_SB_L) {
+			ch->sb = BRCMU_CHAN_SB_L;
+			ch->chnum -= CH_10MHZ_APART;
+		} else if (val == BRCMU_CHSPEC_D11AC_SB_U) {
+			ch->sb = BRCMU_CHAN_SB_U;
+			ch->chnum += CH_10MHZ_APART;
+		} else {
+			WARN_ON_ONCE(1);
+		}
+		break;
+	case BRCMU_CHSPEC_D11AC_BW_80:
+		ch->bw = BRCMU_CHAN_BW_80;
+		break;
+	case BRCMU_CHSPEC_D11AC_BW_8080:
+	case BRCMU_CHSPEC_D11AC_BW_160:
+	default:
+		WARN_ON_ONCE(1);
+		break;
+	}
+
+	switch (ch->chspec & BRCMU_CHSPEC_D11AC_BND_MASK) {
+	case BRCMU_CHSPEC_D11AC_BND_5G:
+		ch->band = BRCMU_CHAN_BAND_5G;
+		break;
+	case BRCMU_CHSPEC_D11AC_BND_2G:
+		ch->band = BRCMU_CHAN_BAND_2G;
+		break;
+	default:
+		WARN_ON_ONCE(1);
+		break;
+	}
+}
+
+void brcmu_d11_attach(struct brcmu_d11inf *d11inf)
+{
+	if (d11inf->io_type == BRCMU_D11N_IOTYPE) {
+		d11inf->encchspec = brcmu_d11n_encchspec;
+		d11inf->decchspec = brcmu_d11n_decchspec;
+	} else {
+		d11inf->encchspec = brcmu_d11ac_encchspec;
+		d11inf->decchspec = brcmu_d11ac_decchspec;
+	}
+}
+EXPORT_SYMBOL(brcmu_d11_attach);
diff --git a/drivers/net/wireless/brcm80211/brcmutil/utils.c b/drivers/net/wireless/brcm80211/brcmutil/utils.c
index 3e6405e06ac0..0f7e1c7b6f58 100644
--- a/drivers/net/wireless/brcm80211/brcmutil/utils.c
+++ b/drivers/net/wireless/brcm80211/brcmutil/utils.c
@@ -45,17 +45,9 @@ void brcmu_pkt_buf_free_skb(struct sk_buff *skb)
 {
 	if (!skb)
 		return;
+
 	WARN_ON(skb->next);
-	if (skb->destructor)
-		/* cannot kfree_skb() on hard IRQ (net/core/skbuff.c) if
-		 * destructor exists
-		 */
-		dev_kfree_skb_any(skb);
-	else
-		/* can free immediately (even in_irq()) if destructor
-		 * does not exist
-		 */
-		dev_kfree_skb(skb);
+	dev_kfree_skb_any(skb);
 }
 EXPORT_SYMBOL(brcmu_pkt_buf_free_skb);
 
@@ -116,6 +108,31 @@ struct sk_buff *brcmu_pktq_pdeq(struct pktq *pq, int prec)
 }
 EXPORT_SYMBOL(brcmu_pktq_pdeq);
 
+/*
+ * precedence based dequeue with match function. Passing a NULL pointer
+ * for the match function parameter is considered to be a wildcard so
+ * any packet on the queue is returned. In that case it is no different
+ * from brcmu_pktq_pdeq() above.
+ */
+struct sk_buff *brcmu_pktq_pdeq_match(struct pktq *pq, int prec,
+				      bool (*match_fn)(struct sk_buff *skb,
+						       void *arg), void *arg)
+{
+	struct sk_buff_head *q;
+	struct sk_buff *p, *next;
+
+	q = &pq->q[prec].skblist;
+	skb_queue_walk_safe(q, p, next) {
+		if (match_fn == NULL || match_fn(p, arg)) {
+			skb_unlink(p, q);
+			pq->len--;
+			return p;
+		}
+	}
+	return NULL;
+}
+EXPORT_SYMBOL(brcmu_pktq_pdeq_match);
+
 struct sk_buff *brcmu_pktq_pdeq_tail(struct pktq *pq, int prec)
 {
 	struct sk_buff_head *q;
diff --git a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h b/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
index e8682855b73a..c1fe245bb07e 100644
--- a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
+++ b/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
@@ -29,6 +29,7 @@
 
 /* Chipcommon Core Chip IDs */
 #define BCM4313_CHIP_ID		0x4313
+#define BCM43143_CHIP_ID	43143
 #define BCM43224_CHIP_ID	43224
 #define BCM43225_CHIP_ID	43225
 #define BCM43235_CHIP_ID	43235
@@ -39,5 +40,6 @@
 #define BCM4330_CHIP_ID		0x4330
 #define BCM4331_CHIP_ID		0x4331
 #define BCM4334_CHIP_ID		0x4334
+#define BCM4335_CHIP_ID		0x4335
 
 #endif				/* _BRCM_HW_IDS_H_ */
diff --git a/drivers/net/wireless/brcm80211/include/brcmu_d11.h b/drivers/net/wireless/brcm80211/include/brcmu_d11.h
new file mode 100644
index 000000000000..92623f02b1c0
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/include/brcmu_d11.h
@@ -0,0 +1,145 @@
+/*
+ * Copyright (c) 2010 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef	_BRCMU_D11_H_
+#define	_BRCMU_D11_H_
+
+/* d11 io type */
+#define BRCMU_D11N_IOTYPE		1
+#define BRCMU_D11AC_IOTYPE		2
+
+/* A chanspec (channel specification) holds the channel number, band,
+ * bandwidth and control sideband
+ */
+
+/* chanspec binary format */
+
+#define BRCMU_CHSPEC_INVALID		255
+/* bit 0~7 channel number
+ * for 80+80 channels: bit 0~3 low channel id, bit 4~7 high channel id
+ */
+#define BRCMU_CHSPEC_CH_MASK		0x00ff
+#define BRCMU_CHSPEC_CH_SHIFT		0
+#define BRCMU_CHSPEC_CHL_MASK		0x000f
+#define BRCMU_CHSPEC_CHL_SHIFT		0
+#define BRCMU_CHSPEC_CHH_MASK		0x00f0
+#define BRCMU_CHSPEC_CHH_SHIFT		4
+
+/* bit 8~16 for dot 11n IO types
+ * bit 8~9 sideband
+ * bit 10~11 bandwidth
+ * bit 12~13 spectral band
+ * bit 14~15 not used
+ */
+#define BRCMU_CHSPEC_D11N_SB_MASK	0x0300
+#define BRCMU_CHSPEC_D11N_SB_SHIFT	8
+#define  BRCMU_CHSPEC_D11N_SB_L		0x0100	/* control lower */
+#define  BRCMU_CHSPEC_D11N_SB_U		0x0200	/* control upper */
+#define  BRCMU_CHSPEC_D11N_SB_N		0x0300	/* none */
+#define BRCMU_CHSPEC_D11N_BW_MASK	0x0c00
+#define BRCMU_CHSPEC_D11N_BW_SHIFT	10
+#define  BRCMU_CHSPEC_D11N_BW_10	0x0400
+#define  BRCMU_CHSPEC_D11N_BW_20	0x0800
+#define  BRCMU_CHSPEC_D11N_BW_40	0x0c00
+#define BRCMU_CHSPEC_D11N_BND_MASK	0x3000
+#define BRCMU_CHSPEC_D11N_BND_SHIFT	12
+#define  BRCMU_CHSPEC_D11N_BND_5G	0x1000
+#define  BRCMU_CHSPEC_D11N_BND_2G	0x2000
+
+/* bit 8~16 for dot 11ac IO types
+ * bit 8~10 sideband
+ * bit 11~13 bandwidth
+ * bit 14~15 spectral band
+ */
+#define BRCMU_CHSPEC_D11AC_SB_MASK	0x0700
+#define BRCMU_CHSPEC_D11AC_SB_SHIFT	8
+#define  BRCMU_CHSPEC_D11AC_SB_LLL	0x0000
+#define  BRCMU_CHSPEC_D11AC_SB_LLU	0x0100
+#define  BRCMU_CHSPEC_D11AC_SB_LUL	0x0200
+#define  BRCMU_CHSPEC_D11AC_SB_LUU	0x0300
+#define  BRCMU_CHSPEC_D11AC_SB_ULL	0x0400
+#define  BRCMU_CHSPEC_D11AC_SB_ULU	0x0500
+#define  BRCMU_CHSPEC_D11AC_SB_UUL	0x0600
+#define  BRCMU_CHSPEC_D11AC_SB_UUU	0x0700
+#define  BRCMU_CHSPEC_D11AC_SB_LL	BRCMU_CHSPEC_D11AC_SB_LLL
+#define  BRCMU_CHSPEC_D11AC_SB_LU	BRCMU_CHSPEC_D11AC_SB_LLU
+#define  BRCMU_CHSPEC_D11AC_SB_UL	BRCMU_CHSPEC_D11AC_SB_LUL
+#define  BRCMU_CHSPEC_D11AC_SB_UU	BRCMU_CHSPEC_D11AC_SB_LUU
+#define  BRCMU_CHSPEC_D11AC_SB_L	BRCMU_CHSPEC_D11AC_SB_LLL
+#define  BRCMU_CHSPEC_D11AC_SB_U	BRCMU_CHSPEC_D11AC_SB_LLU
+#define BRCMU_CHSPEC_D11AC_BW_MASK	0x3800
+#define BRCMU_CHSPEC_D11AC_BW_SHIFT	11
+#define  BRCMU_CHSPEC_D11AC_BW_5	0x0000
+#define  BRCMU_CHSPEC_D11AC_BW_10	0x0800
+#define  BRCMU_CHSPEC_D11AC_BW_20	0x1000
+#define  BRCMU_CHSPEC_D11AC_BW_40	0x1800
+#define  BRCMU_CHSPEC_D11AC_BW_80	0x2000
+#define  BRCMU_CHSPEC_D11AC_BW_160	0x2800
+#define  BRCMU_CHSPEC_D11AC_BW_8080	0x3000
+#define BRCMU_CHSPEC_D11AC_BND_MASK	0xc000
+#define BRCMU_CHSPEC_D11AC_BND_SHIFT	14
+#define  BRCMU_CHSPEC_D11AC_BND_2G	0x0000
+#define  BRCMU_CHSPEC_D11AC_BND_3G	0x4000
+#define  BRCMU_CHSPEC_D11AC_BND_4G	0x8000
+#define  BRCMU_CHSPEC_D11AC_BND_5G	0xc000
+
+#define BRCMU_CHAN_BAND_2G		0
+#define BRCMU_CHAN_BAND_5G		1
+
+enum brcmu_chan_bw {
+	BRCMU_CHAN_BW_20,
+	BRCMU_CHAN_BW_40,
+	BRCMU_CHAN_BW_80,
+	BRCMU_CHAN_BW_80P80,
+	BRCMU_CHAN_BW_160,
+};
+
+enum brcmu_chan_sb {
+	BRCMU_CHAN_SB_NONE = 0,
+	BRCMU_CHAN_SB_L,
+	BRCMU_CHAN_SB_U,
+	BRCMU_CHAN_SB_LL,
+	BRCMU_CHAN_SB_LU,
+	BRCMU_CHAN_SB_UL,
+	BRCMU_CHAN_SB_UU,
+	BRCMU_CHAN_SB_LLL,
+	BRCMU_CHAN_SB_LLU,
+	BRCMU_CHAN_SB_LUL,
+	BRCMU_CHAN_SB_LUU,
+	BRCMU_CHAN_SB_ULL,
+	BRCMU_CHAN_SB_ULU,
+	BRCMU_CHAN_SB_UUL,
+	BRCMU_CHAN_SB_UUU,
+};
+
+struct brcmu_chan {
+	u16 chspec;
+	u8 chnum;
+	u8 band;
+	enum brcmu_chan_bw bw;
+	enum brcmu_chan_sb sb;
+};
+
+struct brcmu_d11inf {
+	u8 io_type;
+
+	void (*encchspec)(struct brcmu_chan *ch);
+	void (*decchspec)(struct brcmu_chan *ch);
+};
+
+extern void brcmu_d11_attach(struct brcmu_d11inf *d11inf);
+
+#endif	/* _BRCMU_CHANNELS_H_ */
diff --git a/drivers/net/wireless/brcm80211/include/brcmu_utils.h b/drivers/net/wireless/brcm80211/include/brcmu_utils.h
index 477b92ad3d62..898cacb8d01d 100644
--- a/drivers/net/wireless/brcm80211/include/brcmu_utils.h
+++ b/drivers/net/wireless/brcm80211/include/brcmu_utils.h
@@ -120,6 +120,10 @@ extern struct sk_buff *brcmu_pktq_penq_head(struct pktq *pq, int prec,
 				      struct sk_buff *p);
 extern struct sk_buff *brcmu_pktq_pdeq(struct pktq *pq, int prec);
 extern struct sk_buff *brcmu_pktq_pdeq_tail(struct pktq *pq, int prec);
+extern struct sk_buff *brcmu_pktq_pdeq_match(struct pktq *pq, int prec,
+					     bool (*match_fn)(struct sk_buff *p,
+							      void *arg),
+					     void *arg);
 
 /* packet primitives */
 extern struct sk_buff *brcmu_pkt_buf_get_skb(uint len);
@@ -173,6 +177,29 @@ extern void brcmu_pktq_flush(struct pktq *pq, bool dir,
 /* ip address */
 struct ipv4_addr;
 
+/*
+ * bitfield macros using masking and shift
+ *
+ * remark: the mask parameter should be a shifted mask.
+ */
+static inline void brcmu_maskset32(u32 *var, u32 mask, u8 shift, u32 value)
+{
+	value = (value << shift) & mask;
+	*var = (*var & ~mask) | value;
+}
+static inline u32 brcmu_maskget32(u32 var, u32 mask, u8 shift)
+{
+	return (var & mask) >> shift;
+}
+static inline void brcmu_maskset16(u16 *var, u16 mask, u8 shift, u16 value)
+{
+	value = (value << shift) & mask;
+	*var = (*var & ~mask) | value;
+}
+static inline u16 brcmu_maskget16(u16 var, u16 mask, u8 shift)
+{
+	return (var & mask) >> shift;
+}
 
 /* externs */
 /* format/print */
diff --git a/drivers/net/wireless/brcm80211/include/brcmu_wifi.h b/drivers/net/wireless/brcm80211/include/brcmu_wifi.h
index c11a290a1edf..0505cc065e0d 100644
--- a/drivers/net/wireless/brcm80211/include/brcmu_wifi.h
+++ b/drivers/net/wireless/brcm80211/include/brcmu_wifi.h
@@ -32,8 +32,9 @@
 #define CH_20MHZ_APART			4
 #define CH_10MHZ_APART			2
 #define CH_5MHZ_APART			1 /* 2G band channels are 5 Mhz apart */
+#define CH_MIN_2G_CHANNEL		1
 #define CH_MAX_2G_CHANNEL		14	/* Max channel in 2G band */
-#define BRCM_MAX_2G_CHANNEL	CH_MAX_2G_CHANNEL	/* legacy define */
+#define CH_MIN_5G_CHANNEL		34
 
 /* bandstate array indices */
 #define BAND_2G_INDEX		0	/* wlc->bandstate[x] index */
@@ -60,6 +61,7 @@
 #define WL_CHANSPEC_BW_10		0x0400
 #define WL_CHANSPEC_BW_20		0x0800
 #define WL_CHANSPEC_BW_40		0x0C00
+#define WL_CHANSPEC_BW_80		0x2000
 
 #define WL_CHANSPEC_BAND_MASK		0xf000
 #define WL_CHANSPEC_BAND_SHIFT		12
@@ -67,6 +69,25 @@
 #define WL_CHANSPEC_BAND_2G		0x2000
 #define INVCHANSPEC			255
 
+#define WL_CHAN_VALID_HW		(1 << 0) /* valid with current HW */
+#define WL_CHAN_VALID_SW		(1 << 1) /* valid with country sett. */
+#define WL_CHAN_BAND_5G			(1 << 2) /* 5GHz-band channel */
+#define WL_CHAN_RADAR			(1 << 3) /* radar sensitive  channel */
+#define WL_CHAN_INACTIVE		(1 << 4) /* inactive due to radar */
+#define WL_CHAN_PASSIVE			(1 << 5) /* channel in passive mode */
+#define WL_CHAN_RESTRICTED		(1 << 6) /* restricted use channel */
+
+/* values for band specific 40MHz capabilities  */
+#define WLC_N_BW_20ALL			0
+#define WLC_N_BW_40ALL			1
+#define WLC_N_BW_20IN2G_40IN5G		2
+
+/* band types */
+#define	WLC_BAND_AUTO			0	/* auto-select */
+#define	WLC_BAND_5G			1	/* 5 Ghz */
+#define	WLC_BAND_2G			2	/* 2.4 Ghz */
+#define	WLC_BAND_ALL			3	/* all bands */
+
 #define CHSPEC_CHANNEL(chspec)	((u8)((chspec) & WL_CHANSPEC_CHAN_MASK))
 #define CHSPEC_BAND(chspec)	((chspec) & WL_CHANSPEC_BAND_MASK)
 
@@ -79,10 +100,11 @@
 #define CHSPEC_IS20(chspec) \
 	(((chspec) & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_20)
 
-#ifndef CHSPEC_IS40
 #define CHSPEC_IS40(chspec) \
 	(((chspec) & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_40)
-#endif
+
+#define CHSPEC_IS80(chspec) \
+	(((chspec) & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_80)
 
 #define CHSPEC_IS5G(chspec) \
 	(((chspec) & WL_CHANSPEC_BAND_MASK) == WL_CHANSPEC_BAND_5G)
diff --git a/drivers/net/wireless/brcm80211/include/chipcommon.h b/drivers/net/wireless/brcm80211/include/chipcommon.h
index f96834a7c055..d242333b7559 100644
--- a/drivers/net/wireless/brcm80211/include/chipcommon.h
+++ b/drivers/net/wireless/brcm80211/include/chipcommon.h
@@ -205,7 +205,7 @@ struct chipcregs {
 	u32 res_req_timer_sel;
 	u32 res_req_timer;
 	u32 res_req_mask;
-	u32 PAD;
+	u32 pmucapabilities_ext; /* 0x64c, pmurev >=15 */
 	u32 chipcontrol_addr;	/* 0x650 */
 	u32 chipcontrol_data;	/* 0x654 */
 	u32 regcontrol_addr;
@@ -214,7 +214,11 @@ struct chipcregs {
 	u32 pllcontrol_data;
 	u32 pmustrapopt;	/* 0x668, corerev >= 28 */
 	u32 pmu_xtalfreq;	/* 0x66C, pmurev >= 10 */
-	u32 PAD[100];
+	u32 retention_ctl;          /* 0x670, pmurev >= 15 */
+	u32 PAD[3];
+	u32 retention_grpidx;       /* 0x680 */
+	u32 retention_grpctl;       /* 0x684 */
+	u32 PAD[94];
 	u16 sromotp[768];
 };
 
@@ -276,6 +280,12 @@ struct chipcregs {
 #define PCAP5_VC_SHIFT	22
 #define PCAP5_CC_MASK	0xf8000000
 #define PCAP5_CC_SHIFT	27
+/* pmucapabilites_ext PMU rev >= 15 */
+#define PCAPEXT_SR_SUPPORTED_MASK	(1 << 1)
+/* retention_ctl PMU rev >= 15 */
+#define PMU_RCTL_MACPHY_DISABLE_MASK        (1 << 26)
+#define PMU_RCTL_LOGIC_DISABLE_MASK         (1 << 27)
+
 
 /*
 * Maximum delay for the PMU state transition in us.
diff --git a/drivers/net/wireless/hostap/hostap_ap.c b/drivers/net/wireless/hostap/hostap_ap.c
index dd9a18f8dbca..d6033a8e5dea 100644
--- a/drivers/net/wireless/hostap/hostap_ap.c
+++ b/drivers/net/wireless/hostap/hostap_ap.c
@@ -17,6 +17,7 @@
  */
 
 #include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 #include <linux/delay.h>
 #include <linux/random.h>
 #include <linux/if_arp.h>
@@ -64,28 +65,32 @@ static void prism2_send_mgmt(struct net_device *dev,
 
 
 #ifndef PRISM2_NO_PROCFS_DEBUG
-static int ap_debug_proc_read(char *page, char **start, off_t off,
-			      int count, int *eof, void *data)
-{
-	char *p = page;
-	struct ap_data *ap = (struct ap_data *) data;
-
-	if (off != 0) {
-		*eof = 1;
-		return 0;
-	}
-
-	p += sprintf(p, "BridgedUnicastFrames=%u\n", ap->bridged_unicast);
-	p += sprintf(p, "BridgedMulticastFrames=%u\n", ap->bridged_multicast);
-	p += sprintf(p, "max_inactivity=%u\n", ap->max_inactivity / HZ);
-	p += sprintf(p, "bridge_packets=%u\n", ap->bridge_packets);
-	p += sprintf(p, "nullfunc_ack=%u\n", ap->nullfunc_ack);
-	p += sprintf(p, "autom_ap_wds=%u\n", ap->autom_ap_wds);
-	p += sprintf(p, "auth_algs=%u\n", ap->local->auth_algs);
-	p += sprintf(p, "tx_drop_nonassoc=%u\n", ap->tx_drop_nonassoc);
+static int ap_debug_proc_show(struct seq_file *m, void *v)
+{
+	struct ap_data *ap = m->private;
+
+	seq_printf(m, "BridgedUnicastFrames=%u\n", ap->bridged_unicast);
+	seq_printf(m, "BridgedMulticastFrames=%u\n", ap->bridged_multicast);
+	seq_printf(m, "max_inactivity=%u\n", ap->max_inactivity / HZ);
+	seq_printf(m, "bridge_packets=%u\n", ap->bridge_packets);
+	seq_printf(m, "nullfunc_ack=%u\n", ap->nullfunc_ack);
+	seq_printf(m, "autom_ap_wds=%u\n", ap->autom_ap_wds);
+	seq_printf(m, "auth_algs=%u\n", ap->local->auth_algs);
+	seq_printf(m, "tx_drop_nonassoc=%u\n", ap->tx_drop_nonassoc);
+	return 0;
+}
 
-	return (p - page);
+static int ap_debug_proc_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ap_debug_proc_show, PDE_DATA(inode));
 }
+
+static const struct file_operations ap_debug_proc_fops = {
+	.open		= ap_debug_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 #endif /* PRISM2_NO_PROCFS_DEBUG */
 
 
@@ -325,50 +330,81 @@ void hostap_deauth_all_stas(struct net_device *dev, struct ap_data *ap,
 }
 
 
-static int ap_control_proc_read(char *page, char **start, off_t off,
-				int count, int *eof, void *data)
+static int ap_control_proc_show(struct seq_file *m, void *v)
 {
-	char *p = page;
-	struct ap_data *ap = (struct ap_data *) data;
+	struct ap_data *ap = m->private;
 	char *policy_txt;
 	struct mac_entry *entry;
 
-	if (off != 0) {
-		*eof = 1;
+	if (v == SEQ_START_TOKEN) {
+		switch (ap->mac_restrictions.policy) {
+		case MAC_POLICY_OPEN:
+			policy_txt = "open";
+			break;
+		case MAC_POLICY_ALLOW:
+			policy_txt = "allow";
+			break;
+		case MAC_POLICY_DENY:
+			policy_txt = "deny";
+			break;
+		default:
+			policy_txt = "unknown";
+			break;
+		}
+		seq_printf(m, "MAC policy: %s\n", policy_txt);
+		seq_printf(m, "MAC entries: %u\n", ap->mac_restrictions.entries);
+		seq_puts(m, "MAC list:\n");
 		return 0;
 	}
 
-	switch (ap->mac_restrictions.policy) {
-	case MAC_POLICY_OPEN:
-		policy_txt = "open";
-		break;
-	case MAC_POLICY_ALLOW:
-		policy_txt = "allow";
-		break;
-	case MAC_POLICY_DENY:
-		policy_txt = "deny";
-		break;
-	default:
-		policy_txt = "unknown";
-		break;
-	}
-	p += sprintf(p, "MAC policy: %s\n", policy_txt);
-	p += sprintf(p, "MAC entries: %u\n", ap->mac_restrictions.entries);
-	p += sprintf(p, "MAC list:\n");
+	entry = v;
+	seq_printf(m, "%pM\n", entry->addr);
+	return 0;
+}
+
+static void *ap_control_proc_start(struct seq_file *m, loff_t *_pos)
+{
+	struct ap_data *ap = m->private;
 	spin_lock_bh(&ap->mac_restrictions.lock);
-	list_for_each_entry(entry, &ap->mac_restrictions.mac_list, list) {
-		if (p - page > PAGE_SIZE - 80) {
-			p += sprintf(p, "All entries did not fit one page.\n");
-			break;
-		}
+	return seq_list_start_head(&ap->mac_restrictions.mac_list, *_pos);
+}
 
-		p += sprintf(p, "%pM\n", entry->addr);
-	}
+static void *ap_control_proc_next(struct seq_file *m, void *v, loff_t *_pos)
+{
+	struct ap_data *ap = m->private;
+	return seq_list_next(v, &ap->mac_restrictions.mac_list, _pos);
+}
+
+static void ap_control_proc_stop(struct seq_file *m, void *v)
+{
+	struct ap_data *ap = m->private;
 	spin_unlock_bh(&ap->mac_restrictions.lock);
+}
 
-	return (p - page);
+static const struct seq_operations ap_control_proc_seqops = {
+	.start	= ap_control_proc_start,
+	.next	= ap_control_proc_next,
+	.stop	= ap_control_proc_stop,
+	.show	= ap_control_proc_show,
+};
+
+static int ap_control_proc_open(struct inode *inode, struct file *file)
+{
+	int ret = seq_open(file, &ap_control_proc_seqops);
+	if (ret == 0) {
+		struct seq_file *m = file->private_data;
+		m->private = PDE_DATA(inode);
+	}
+	return ret;
 }
 
+static const struct file_operations ap_control_proc_fops = {
+	.open		= ap_control_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= seq_release,
+};
+
 
 int ap_control_add_mac(struct mac_restrictions *mac_restrictions, u8 *mac)
 {
@@ -510,61 +546,84 @@ void ap_control_kickall(struct ap_data *ap)
 
 #ifndef PRISM2_NO_KERNEL_IEEE80211_MGMT
 
-#define PROC_LIMIT (PAGE_SIZE - 80)
-
-static int prism2_ap_proc_read(char *page, char **start, off_t off,
-			       int count, int *eof, void *data)
+static int prism2_ap_proc_show(struct seq_file *m, void *v)
 {
-	char *p = page;
-	struct ap_data *ap = (struct ap_data *) data;
-	struct sta_info *sta;
+	struct sta_info *sta = v;
 	int i;
 
-	if (off > PROC_LIMIT) {
-		*eof = 1;
+	if (v == SEQ_START_TOKEN) {
+		seq_printf(m, "# BSSID CHAN SIGNAL NOISE RATE SSID FLAGS\n");
 		return 0;
 	}
 
-	p += sprintf(p, "# BSSID CHAN SIGNAL NOISE RATE SSID FLAGS\n");
-	spin_lock_bh(&ap->sta_table_lock);
-	list_for_each_entry(sta, &ap->sta_list, list) {
-		if (!sta->ap)
-			continue;
+	if (!sta->ap)
+		return 0;
 
-		p += sprintf(p, "%pM %d %d %d %d '",
-			     sta->addr,
-			     sta->u.ap.channel, sta->last_rx_signal,
-			     sta->last_rx_silence, sta->last_rx_rate);
-		for (i = 0; i < sta->u.ap.ssid_len; i++)
-			p += sprintf(p, ((sta->u.ap.ssid[i] >= 32 &&
-					  sta->u.ap.ssid[i] < 127) ?
-					 "%c" : "<%02x>"),
-				     sta->u.ap.ssid[i]);
-		p += sprintf(p, "'");
-		if (sta->capability & WLAN_CAPABILITY_ESS)
-			p += sprintf(p, " [ESS]");
-		if (sta->capability & WLAN_CAPABILITY_IBSS)
-			p += sprintf(p, " [IBSS]");
-		if (sta->capability & WLAN_CAPABILITY_PRIVACY)
-			p += sprintf(p, " [WEP]");
-		p += sprintf(p, "\n");
-
-		if ((p - page) > PROC_LIMIT) {
-			printk(KERN_DEBUG "hostap: ap proc did not fit\n");
-			break;
-		}
-	}
-	spin_unlock_bh(&ap->sta_table_lock);
+	seq_printf(m, "%pM %d %d %d %d '",
+		   sta->addr,
+		   sta->u.ap.channel, sta->last_rx_signal,
+		   sta->last_rx_silence, sta->last_rx_rate);
 
-	if ((p - page) <= off) {
-		*eof = 1;
-		return 0;
+	for (i = 0; i < sta->u.ap.ssid_len; i++) {
+		if (sta->u.ap.ssid[i] >= 32 && sta->u.ap.ssid[i] < 127)
+			seq_putc(m, sta->u.ap.ssid[i]);
+		else
+			seq_printf(m, "<%02x>", sta->u.ap.ssid[i]);
 	}
 
-	*start = page + off;
+	seq_putc(m, '\'');
+	if (sta->capability & WLAN_CAPABILITY_ESS)
+		seq_puts(m, " [ESS]");
+	if (sta->capability & WLAN_CAPABILITY_IBSS)
+		seq_puts(m, " [IBSS]");
+	if (sta->capability & WLAN_CAPABILITY_PRIVACY)
+		seq_puts(m, " [WEP]");
+	seq_putc(m, '\n');
+	return 0;
+}
+
+static void *prism2_ap_proc_start(struct seq_file *m, loff_t *_pos)
+{
+	struct ap_data *ap = m->private;
+	spin_lock_bh(&ap->sta_table_lock);
+	return seq_list_start_head(&ap->sta_list, *_pos);
+}
 
-	return (p - page - off);
+static void *prism2_ap_proc_next(struct seq_file *m, void *v, loff_t *_pos)
+{
+	struct ap_data *ap = m->private;
+	return seq_list_next(v, &ap->sta_list, _pos);
 }
+
+static void prism2_ap_proc_stop(struct seq_file *m, void *v)
+{
+	struct ap_data *ap = m->private;
+	spin_unlock_bh(&ap->sta_table_lock);
+}
+
+static const struct seq_operations prism2_ap_proc_seqops = {
+	.start	= prism2_ap_proc_start,
+	.next	= prism2_ap_proc_next,
+	.stop	= prism2_ap_proc_stop,
+	.show	= prism2_ap_proc_show,
+};
+
+static int prism2_ap_proc_open(struct inode *inode, struct file *file)
+{
+	int ret = seq_open(file, &prism2_ap_proc_seqops);
+	if (ret == 0) {
+		struct seq_file *m = file->private_data;
+		m->private = PDE_DATA(inode);
+	}
+	return ret;
+}
+
+static const struct file_operations prism2_ap_proc_fops = {
+	.open		= prism2_ap_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= seq_release,
+};
 #endif /* PRISM2_NO_KERNEL_IEEE80211_MGMT */
 
 
@@ -836,15 +895,12 @@ void hostap_init_ap_proc(local_info_t *local)
 		return;
 
 #ifndef PRISM2_NO_PROCFS_DEBUG
-	create_proc_read_entry("ap_debug", 0, ap->proc,
-			       ap_debug_proc_read, ap);
+	proc_create_data("ap_debug", 0, ap->proc, &ap_debug_proc_fops, ap);
 #endif /* PRISM2_NO_PROCFS_DEBUG */
 
 #ifndef PRISM2_NO_KERNEL_IEEE80211_MGMT
-	create_proc_read_entry("ap_control", 0, ap->proc,
-			       ap_control_proc_read, ap);
-	create_proc_read_entry("ap", 0, ap->proc,
-			       prism2_ap_proc_read, ap);
+	proc_create_data("ap_control", 0, ap->proc, &ap_control_proc_fops, ap);
+	proc_create_data("ap", 0, ap->proc, &prism2_ap_proc_fops, ap);
 #endif /* PRISM2_NO_KERNEL_IEEE80211_MGMT */
 
 }
@@ -982,79 +1038,86 @@ static void prism2_send_mgmt(struct net_device *dev,
 #endif /* PRISM2_NO_KERNEL_IEEE80211_MGMT */
 
 
-static int prism2_sta_proc_read(char *page, char **start, off_t off,
-				int count, int *eof, void *data)
+static int prism2_sta_proc_show(struct seq_file *m, void *v)
 {
-	char *p = page;
-	struct sta_info *sta = (struct sta_info *) data;
+	struct sta_info *sta = m->private;
 	int i;
 
 	/* FIX: possible race condition.. the STA data could have just expired,
 	 * but proc entry was still here so that the read could have started;
 	 * some locking should be done here.. */
 
-	if (off != 0) {
-		*eof = 1;
-		return 0;
-	}
-
-	p += sprintf(p, "%s=%pM\nusers=%d\naid=%d\n"
-		     "flags=0x%04x%s%s%s%s%s%s%s\n"
-		     "capability=0x%02x\nlisten_interval=%d\nsupported_rates=",
-		     sta->ap ? "AP" : "STA",
-		     sta->addr, atomic_read(&sta->users), sta->aid,
-		     sta->flags,
-		     sta->flags & WLAN_STA_AUTH ? " AUTH" : "",
-		     sta->flags & WLAN_STA_ASSOC ? " ASSOC" : "",
-		     sta->flags & WLAN_STA_PS ? " PS" : "",
-		     sta->flags & WLAN_STA_TIM ? " TIM" : "",
-		     sta->flags & WLAN_STA_PERM ? " PERM" : "",
-		     sta->flags & WLAN_STA_AUTHORIZED ? " AUTHORIZED" : "",
-		     sta->flags & WLAN_STA_PENDING_POLL ? " POLL" : "",
-		     sta->capability, sta->listen_interval);
+	seq_printf(m,
+		   "%s=%pM\nusers=%d\naid=%d\n"
+		   "flags=0x%04x%s%s%s%s%s%s%s\n"
+		   "capability=0x%02x\nlisten_interval=%d\nsupported_rates=",
+		   sta->ap ? "AP" : "STA",
+		   sta->addr, atomic_read(&sta->users), sta->aid,
+		   sta->flags,
+		   sta->flags & WLAN_STA_AUTH ? " AUTH" : "",
+		   sta->flags & WLAN_STA_ASSOC ? " ASSOC" : "",
+		   sta->flags & WLAN_STA_PS ? " PS" : "",
+		   sta->flags & WLAN_STA_TIM ? " TIM" : "",
+		   sta->flags & WLAN_STA_PERM ? " PERM" : "",
+		   sta->flags & WLAN_STA_AUTHORIZED ? " AUTHORIZED" : "",
+		   sta->flags & WLAN_STA_PENDING_POLL ? " POLL" : "",
+		   sta->capability, sta->listen_interval);
 	/* supported_rates: 500 kbit/s units with msb ignored */
 	for (i = 0; i < sizeof(sta->supported_rates); i++)
 		if (sta->supported_rates[i] != 0)
-			p += sprintf(p, "%d%sMbps ",
-				     (sta->supported_rates[i] & 0x7f) / 2,
-				     sta->supported_rates[i] & 1 ? ".5" : "");
-	p += sprintf(p, "\njiffies=%lu\nlast_auth=%lu\nlast_assoc=%lu\n"
-		     "last_rx=%lu\nlast_tx=%lu\nrx_packets=%lu\n"
-		     "tx_packets=%lu\n"
-		     "rx_bytes=%lu\ntx_bytes=%lu\nbuffer_count=%d\n"
-		     "last_rx: silence=%d dBm signal=%d dBm rate=%d%s Mbps\n"
-		     "tx_rate=%d\ntx[1M]=%d\ntx[2M]=%d\ntx[5.5M]=%d\n"
-		     "tx[11M]=%d\n"
-		     "rx[1M]=%d\nrx[2M]=%d\nrx[5.5M]=%d\nrx[11M]=%d\n",
-		     jiffies, sta->last_auth, sta->last_assoc, sta->last_rx,
-		     sta->last_tx,
-		     sta->rx_packets, sta->tx_packets, sta->rx_bytes,
-		     sta->tx_bytes, skb_queue_len(&sta->tx_buf),
-		     sta->last_rx_silence,
-		     sta->last_rx_signal, sta->last_rx_rate / 10,
-		     sta->last_rx_rate % 10 ? ".5" : "",
-		     sta->tx_rate, sta->tx_count[0], sta->tx_count[1],
-		     sta->tx_count[2], sta->tx_count[3],  sta->rx_count[0],
-		     sta->rx_count[1], sta->rx_count[2], sta->rx_count[3]);
+			seq_printf(m, "%d%sMbps ",
+				   (sta->supported_rates[i] & 0x7f) / 2,
+				   sta->supported_rates[i] & 1 ? ".5" : "");
+	seq_printf(m,
+		   "\njiffies=%lu\nlast_auth=%lu\nlast_assoc=%lu\n"
+		   "last_rx=%lu\nlast_tx=%lu\nrx_packets=%lu\n"
+		   "tx_packets=%lu\n"
+		   "rx_bytes=%lu\ntx_bytes=%lu\nbuffer_count=%d\n"
+		   "last_rx: silence=%d dBm signal=%d dBm rate=%d%s Mbps\n"
+		   "tx_rate=%d\ntx[1M]=%d\ntx[2M]=%d\ntx[5.5M]=%d\n"
+		   "tx[11M]=%d\n"
+		   "rx[1M]=%d\nrx[2M]=%d\nrx[5.5M]=%d\nrx[11M]=%d\n",
+		   jiffies, sta->last_auth, sta->last_assoc, sta->last_rx,
+		   sta->last_tx,
+		   sta->rx_packets, sta->tx_packets, sta->rx_bytes,
+		   sta->tx_bytes, skb_queue_len(&sta->tx_buf),
+		   sta->last_rx_silence,
+		   sta->last_rx_signal, sta->last_rx_rate / 10,
+		   sta->last_rx_rate % 10 ? ".5" : "",
+		   sta->tx_rate, sta->tx_count[0], sta->tx_count[1],
+		   sta->tx_count[2], sta->tx_count[3],  sta->rx_count[0],
+		   sta->rx_count[1], sta->rx_count[2], sta->rx_count[3]);
 	if (sta->crypt && sta->crypt->ops && sta->crypt->ops->print_stats)
-		p = sta->crypt->ops->print_stats(p, sta->crypt->priv);
+		sta->crypt->ops->print_stats(m, sta->crypt->priv);
 #ifndef PRISM2_NO_KERNEL_IEEE80211_MGMT
 	if (sta->ap) {
 		if (sta->u.ap.channel >= 0)
-			p += sprintf(p, "channel=%d\n", sta->u.ap.channel);
-		p += sprintf(p, "ssid=");
-		for (i = 0; i < sta->u.ap.ssid_len; i++)
-			p += sprintf(p, ((sta->u.ap.ssid[i] >= 32 &&
-					  sta->u.ap.ssid[i] < 127) ?
-					 "%c" : "<%02x>"),
-				     sta->u.ap.ssid[i]);
-		p += sprintf(p, "\n");
+			seq_printf(m, "channel=%d\n", sta->u.ap.channel);
+		seq_puts(m, "ssid=");
+		for (i = 0; i < sta->u.ap.ssid_len; i++) {
+			if (sta->u.ap.ssid[i] >= 32 && sta->u.ap.ssid[i] < 127)
+				seq_putc(m, sta->u.ap.ssid[i]);
+			else
+				seq_printf(m, "<%02x>", sta->u.ap.ssid[i]);
+		}
+		seq_putc(m, '\n');
 	}
 #endif /* PRISM2_NO_KERNEL_IEEE80211_MGMT */
 
-	return (p - page);
+	return 0;
+}
+
+static int prism2_sta_proc_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, prism2_sta_proc_show, PDE_DATA(inode));
 }
 
+static const struct file_operations prism2_sta_proc_fops = {
+	.open		= prism2_sta_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 
 static void handle_add_proc_queue(struct work_struct *work)
 {
@@ -1076,9 +1139,9 @@ static void handle_add_proc_queue(struct work_struct *work)
 
 		if (sta) {
 			sprintf(name, "%pM", sta->addr);
-			sta->proc = create_proc_read_entry(
+			sta->proc = proc_create_data(
 				name, 0, ap->proc,
-				prism2_sta_proc_read, sta);
+				&prism2_sta_proc_fops, sta);
 
 			atomic_dec(&sta->users);
 		}
diff --git a/drivers/net/wireless/hostap/hostap_download.c b/drivers/net/wireless/hostap/hostap_download.c
index e73bf739fd9b..705fe668b969 100644
--- a/drivers/net/wireless/hostap/hostap_download.c
+++ b/drivers/net/wireless/hostap/hostap_download.c
@@ -174,20 +174,70 @@ static int prism2_pda_ok(u8 *buf)
 }
 
 
-static int prism2_download_aux_dump(struct net_device *dev,
-				     unsigned int addr, int len, u8 *buf)
-{
-	int res;
+#define prism2_download_aux_dump_npages 65536
 
-	prism2_enable_aux_port(dev, 1);
-	res = hfa384x_from_aux(dev, addr, len, buf);
-	prism2_enable_aux_port(dev, 0);
-	if (res)
-		return -1;
+struct prism2_download_aux_dump {
+	local_info_t *local;
+	u16 page[0x80];
+};
+
+static int prism2_download_aux_dump_proc_show(struct seq_file *m, void *v)
+{
+	struct prism2_download_aux_dump *ctx = m->private;
 
+	hfa384x_from_aux(ctx->local->dev, (unsigned long)v - 1, 0x80, ctx->page);
+	seq_write(m, ctx->page, 0x80);
 	return 0;
 }
 
+static void *prism2_download_aux_dump_proc_start(struct seq_file *m, loff_t *_pos)
+{
+	struct prism2_download_aux_dump *ctx = m->private;
+	prism2_enable_aux_port(ctx->local->dev, 1);
+	if (*_pos >= prism2_download_aux_dump_npages)
+		return NULL;
+	return (void *)((unsigned long)*_pos + 1);
+}
+
+static void *prism2_download_aux_dump_proc_next(struct seq_file *m, void *v, loff_t *_pos)
+{
+	++*_pos;
+	if (*_pos >= prism2_download_aux_dump_npages)
+		return NULL;
+	return (void *)((unsigned long)*_pos + 1);
+}
+
+static void prism2_download_aux_dump_proc_stop(struct seq_file *m, void *v)
+{
+	struct prism2_download_aux_dump *ctx = m->private;
+	prism2_enable_aux_port(ctx->local->dev, 0);
+}
+
+static const struct seq_operations prism2_download_aux_dump_proc_seqops = {
+	.start	= prism2_download_aux_dump_proc_start,
+	.next	= prism2_download_aux_dump_proc_next,
+	.stop	= prism2_download_aux_dump_proc_stop,
+	.show	= prism2_download_aux_dump_proc_show,
+};
+
+static int prism2_download_aux_dump_proc_open(struct inode *inode, struct file *file)
+{
+	int ret = seq_open_private(file, &prism2_download_aux_dump_proc_seqops,
+				   sizeof(struct prism2_download_aux_dump));
+	if (ret == 0) {
+		struct seq_file *m = file->private_data;
+		m->private = PDE_DATA(inode);
+	}
+	return ret;
+}
+
+static const struct file_operations prism2_download_aux_dump_proc_fops = {
+	.open		= prism2_download_aux_dump_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= seq_release_private,
+};
+
 
 static u8 * prism2_read_pda(struct net_device *dev)
 {
diff --git a/drivers/net/wireless/hostap/hostap_hw.c b/drivers/net/wireless/hostap/hostap_hw.c
index 8e7000fd4414..6307a4e36c85 100644
--- a/drivers/net/wireless/hostap/hostap_hw.c
+++ b/drivers/net/wireless/hostap/hostap_hw.c
@@ -38,6 +38,7 @@
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 #include <linux/if_arp.h>
 #include <linux/delay.h>
 #include <linux/random.h>
@@ -129,8 +130,7 @@ static void prism2_check_sta_fw_version(local_info_t *local);
 
 #ifdef PRISM2_DOWNLOAD_SUPPORT
 /* hostap_download.c */
-static int prism2_download_aux_dump(struct net_device *dev,
-				    unsigned int addr, int len, u8 *buf);
+static const struct file_operations prism2_download_aux_dump_proc_fops;
 static u8 * prism2_read_pda(struct net_device *dev);
 static int prism2_download(local_info_t *local,
 			   struct prism2_download_param *param);
@@ -2894,19 +2894,12 @@ static void hostap_tick_timer(unsigned long data)
 
 
 #ifndef PRISM2_NO_PROCFS_DEBUG
-static int prism2_registers_proc_read(char *page, char **start, off_t off,
-				      int count, int *eof, void *data)
+static int prism2_registers_proc_show(struct seq_file *m, void *v)
 {
-	char *p = page;
-	local_info_t *local = (local_info_t *) data;
-
-	if (off != 0) {
-		*eof = 1;
-		return 0;
-	}
+	local_info_t *local = m->private;
 
 #define SHOW_REG(n) \
-p += sprintf(p, #n "=%04x\n", hfa384x_read_reg(local->dev, HFA384X_##n##_OFF))
+  seq_printf(m, #n "=%04x\n", hfa384x_read_reg(local->dev, HFA384X_##n##_OFF))
 
 	SHOW_REG(CMD);
 	SHOW_REG(PARAM0);
@@ -2952,8 +2945,21 @@ p += sprintf(p, #n "=%04x\n", hfa384x_read_reg(local->dev, HFA384X_##n##_OFF))
 	SHOW_REG(PCI_M1_CTL);
 #endif /* PRISM2_PCI */
 
-	return (p - page);
+	return 0;
 }
+
+static int prism2_registers_proc_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, prism2_registers_proc_show, PDE_DATA(inode));
+}
+
+static const struct file_operations prism2_registers_proc_fops = {
+	.open		= prism2_registers_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
 #endif /* PRISM2_NO_PROCFS_DEBUG */
 
 
@@ -3128,7 +3134,7 @@ prism2_init_local_data(struct prism2_helper_functions *funcs, int card_idx,
 	local->func->reset_port = prism2_reset_port;
 	local->func->schedule_reset = prism2_schedule_reset;
 #ifdef PRISM2_DOWNLOAD_SUPPORT
-	local->func->read_aux = prism2_download_aux_dump;
+	local->func->read_aux_fops = &prism2_download_aux_dump_proc_fops;
 	local->func->download = prism2_download;
 #endif /* PRISM2_DOWNLOAD_SUPPORT */
 	local->func->tx = prism2_tx_80211;
@@ -3274,8 +3280,8 @@ static int hostap_hw_ready(struct net_device *dev)
 		}
 		hostap_init_proc(local);
 #ifndef PRISM2_NO_PROCFS_DEBUG
-		create_proc_read_entry("registers", 0, local->proc,
-				       prism2_registers_proc_read, local);
+		proc_create_data("registers", 0, local->proc,
+				 &prism2_registers_proc_fops, local);
 #endif /* PRISM2_NO_PROCFS_DEBUG */
 		hostap_init_ap_proc(local);
 		return 0;
diff --git a/drivers/net/wireless/hostap/hostap_proc.c b/drivers/net/wireless/hostap/hostap_proc.c
index dc447c1b5abe..aa7ad3a7a69b 100644
--- a/drivers/net/wireless/hostap/hostap_proc.c
+++ b/drivers/net/wireless/hostap/hostap_proc.c
@@ -12,259 +12,297 @@
 
 
 #ifndef PRISM2_NO_PROCFS_DEBUG
-static int prism2_debug_proc_read(char *page, char **start, off_t off,
-				  int count, int *eof, void *data)
+static int prism2_debug_proc_show(struct seq_file *m, void *v)
 {
-	char *p = page;
-	local_info_t *local = (local_info_t *) data;
+	local_info_t *local = m->private;
 	int i;
 
-	if (off != 0) {
-		*eof = 1;
-		return 0;
-	}
-
-	p += sprintf(p, "next_txfid=%d next_alloc=%d\n",
-		     local->next_txfid, local->next_alloc);
+	seq_printf(m, "next_txfid=%d next_alloc=%d\n",
+		   local->next_txfid, local->next_alloc);
 	for (i = 0; i < PRISM2_TXFID_COUNT; i++)
-		p += sprintf(p, "FID: tx=%04X intransmit=%04X\n",
-			     local->txfid[i], local->intransmitfid[i]);
-	p += sprintf(p, "FW TX rate control: %d\n", local->fw_tx_rate_control);
-	p += sprintf(p, "beacon_int=%d\n", local->beacon_int);
-	p += sprintf(p, "dtim_period=%d\n", local->dtim_period);
-	p += sprintf(p, "wds_max_connections=%d\n",
-		     local->wds_max_connections);
-	p += sprintf(p, "dev_enabled=%d\n", local->dev_enabled);
-	p += sprintf(p, "sw_tick_stuck=%d\n", local->sw_tick_stuck);
+		seq_printf(m, "FID: tx=%04X intransmit=%04X\n",
+			   local->txfid[i], local->intransmitfid[i]);
+	seq_printf(m, "FW TX rate control: %d\n", local->fw_tx_rate_control);
+	seq_printf(m, "beacon_int=%d\n", local->beacon_int);
+	seq_printf(m, "dtim_period=%d\n", local->dtim_period);
+	seq_printf(m, "wds_max_connections=%d\n", local->wds_max_connections);
+	seq_printf(m, "dev_enabled=%d\n", local->dev_enabled);
+	seq_printf(m, "sw_tick_stuck=%d\n", local->sw_tick_stuck);
 	for (i = 0; i < WEP_KEYS; i++) {
 		if (local->crypt_info.crypt[i] &&
 		    local->crypt_info.crypt[i]->ops) {
-			p += sprintf(p, "crypt[%d]=%s\n", i,
-				     local->crypt_info.crypt[i]->ops->name);
+			seq_printf(m, "crypt[%d]=%s\n", i,
+				   local->crypt_info.crypt[i]->ops->name);
 		}
 	}
-	p += sprintf(p, "pri_only=%d\n", local->pri_only);
-	p += sprintf(p, "pci=%d\n", local->func->hw_type == HOSTAP_HW_PCI);
-	p += sprintf(p, "sram_type=%d\n", local->sram_type);
-	p += sprintf(p, "no_pri=%d\n", local->no_pri);
+	seq_printf(m, "pri_only=%d\n", local->pri_only);
+	seq_printf(m, "pci=%d\n", local->func->hw_type == HOSTAP_HW_PCI);
+	seq_printf(m, "sram_type=%d\n", local->sram_type);
+	seq_printf(m, "no_pri=%d\n", local->no_pri);
 
-	return (p - page);
+	return 0;
 }
+
+static int prism2_debug_proc_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, prism2_debug_proc_show, PDE_DATA(inode));
+}
+
+static const struct file_operations prism2_debug_proc_fops = {
+	.open		= prism2_debug_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 #endif /* PRISM2_NO_PROCFS_DEBUG */
 
 
-static int prism2_stats_proc_read(char *page, char **start, off_t off,
-				  int count, int *eof, void *data)
+static int prism2_stats_proc_show(struct seq_file *m, void *v)
 {
-	char *p = page;
-	local_info_t *local = (local_info_t *) data;
+	local_info_t *local = m->private;
 	struct comm_tallies_sums *sums = &local->comm_tallies;
 
-	if (off != 0) {
-		*eof = 1;
-		return 0;
-	}
-
-	p += sprintf(p, "TxUnicastFrames=%u\n", sums->tx_unicast_frames);
-	p += sprintf(p, "TxMulticastframes=%u\n", sums->tx_multicast_frames);
-	p += sprintf(p, "TxFragments=%u\n", sums->tx_fragments);
-	p += sprintf(p, "TxUnicastOctets=%u\n", sums->tx_unicast_octets);
-	p += sprintf(p, "TxMulticastOctets=%u\n", sums->tx_multicast_octets);
-	p += sprintf(p, "TxDeferredTransmissions=%u\n",
-		     sums->tx_deferred_transmissions);
-	p += sprintf(p, "TxSingleRetryFrames=%u\n",
-		     sums->tx_single_retry_frames);
-	p += sprintf(p, "TxMultipleRetryFrames=%u\n",
-		     sums->tx_multiple_retry_frames);
-	p += sprintf(p, "TxRetryLimitExceeded=%u\n",
-		     sums->tx_retry_limit_exceeded);
-	p += sprintf(p, "TxDiscards=%u\n", sums->tx_discards);
-	p += sprintf(p, "RxUnicastFrames=%u\n", sums->rx_unicast_frames);
-	p += sprintf(p, "RxMulticastFrames=%u\n", sums->rx_multicast_frames);
-	p += sprintf(p, "RxFragments=%u\n", sums->rx_fragments);
-	p += sprintf(p, "RxUnicastOctets=%u\n", sums->rx_unicast_octets);
-	p += sprintf(p, "RxMulticastOctets=%u\n", sums->rx_multicast_octets);
-	p += sprintf(p, "RxFCSErrors=%u\n", sums->rx_fcs_errors);
-	p += sprintf(p, "RxDiscardsNoBuffer=%u\n",
-		     sums->rx_discards_no_buffer);
-	p += sprintf(p, "TxDiscardsWrongSA=%u\n", sums->tx_discards_wrong_sa);
-	p += sprintf(p, "RxDiscardsWEPUndecryptable=%u\n",
-		     sums->rx_discards_wep_undecryptable);
-	p += sprintf(p, "RxMessageInMsgFragments=%u\n",
-		     sums->rx_message_in_msg_fragments);
-	p += sprintf(p, "RxMessageInBadMsgFragments=%u\n",
-		     sums->rx_message_in_bad_msg_fragments);
+	seq_printf(m, "TxUnicastFrames=%u\n", sums->tx_unicast_frames);
+	seq_printf(m, "TxMulticastframes=%u\n", sums->tx_multicast_frames);
+	seq_printf(m, "TxFragments=%u\n", sums->tx_fragments);
+	seq_printf(m, "TxUnicastOctets=%u\n", sums->tx_unicast_octets);
+	seq_printf(m, "TxMulticastOctets=%u\n", sums->tx_multicast_octets);
+	seq_printf(m, "TxDeferredTransmissions=%u\n",
+		   sums->tx_deferred_transmissions);
+	seq_printf(m, "TxSingleRetryFrames=%u\n", sums->tx_single_retry_frames);
+	seq_printf(m, "TxMultipleRetryFrames=%u\n",
+		   sums->tx_multiple_retry_frames);
+	seq_printf(m, "TxRetryLimitExceeded=%u\n",
+		   sums->tx_retry_limit_exceeded);
+	seq_printf(m, "TxDiscards=%u\n", sums->tx_discards);
+	seq_printf(m, "RxUnicastFrames=%u\n", sums->rx_unicast_frames);
+	seq_printf(m, "RxMulticastFrames=%u\n", sums->rx_multicast_frames);
+	seq_printf(m, "RxFragments=%u\n", sums->rx_fragments);
+	seq_printf(m, "RxUnicastOctets=%u\n", sums->rx_unicast_octets);
+	seq_printf(m, "RxMulticastOctets=%u\n", sums->rx_multicast_octets);
+	seq_printf(m, "RxFCSErrors=%u\n", sums->rx_fcs_errors);
+	seq_printf(m, "RxDiscardsNoBuffer=%u\n", sums->rx_discards_no_buffer);
+	seq_printf(m, "TxDiscardsWrongSA=%u\n", sums->tx_discards_wrong_sa);
+	seq_printf(m, "RxDiscardsWEPUndecryptable=%u\n",
+		   sums->rx_discards_wep_undecryptable);
+	seq_printf(m, "RxMessageInMsgFragments=%u\n",
+		   sums->rx_message_in_msg_fragments);
+	seq_printf(m, "RxMessageInBadMsgFragments=%u\n",
+		   sums->rx_message_in_bad_msg_fragments);
 	/* FIX: this may grow too long for one page(?) */
 
-	return (p - page);
+	return 0;
+}
+
+static int prism2_stats_proc_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, prism2_stats_proc_show, PDE_DATA(inode));
 }
 
+static const struct file_operations prism2_stats_proc_fops = {
+	.open		= prism2_stats_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 
-static int prism2_wds_proc_read(char *page, char **start, off_t off,
-				int count, int *eof, void *data)
+
+static int prism2_wds_proc_show(struct seq_file *m, void *v)
 {
-	char *p = page;
-	local_info_t *local = (local_info_t *) data;
-	struct list_head *ptr;
+	struct list_head *ptr = v;
 	struct hostap_interface *iface;
 
-	if (off > PROC_LIMIT) {
-		*eof = 1;
-		return 0;
-	}
+	iface = list_entry(ptr, struct hostap_interface, list);
+	if (iface->type == HOSTAP_INTERFACE_WDS)
+		seq_printf(m, "%s\t%pM\n",
+			   iface->dev->name, iface->u.wds.remote_addr);
+	return 0;
+}
 
+static void *prism2_wds_proc_start(struct seq_file *m, loff_t *_pos)
+{
+	local_info_t *local = m->private;
 	read_lock_bh(&local->iface_lock);
-	list_for_each(ptr, &local->hostap_interfaces) {
-		iface = list_entry(ptr, struct hostap_interface, list);
-		if (iface->type != HOSTAP_INTERFACE_WDS)
-			continue;
-		p += sprintf(p, "%s\t%pM\n",
-			     iface->dev->name,
-			     iface->u.wds.remote_addr);
-		if ((p - page) > PROC_LIMIT) {
-			printk(KERN_DEBUG "%s: wds proc did not fit\n",
-			       local->dev->name);
-			break;
-		}
-	}
-	read_unlock_bh(&local->iface_lock);
+	return seq_list_start(&local->hostap_interfaces, *_pos);
+}
 
-	if ((p - page) <= off) {
-		*eof = 1;
-		return 0;
-	}
+static void *prism2_wds_proc_next(struct seq_file *m, void *v, loff_t *_pos)
+{
+	local_info_t *local = m->private;
+	return seq_list_next(v, &local->hostap_interfaces, _pos);
+}
 
-	*start = page + off;
+static void prism2_wds_proc_stop(struct seq_file *m, void *v)
+{
+	local_info_t *local = m->private;
+	read_unlock_bh(&local->iface_lock);
+}
 
-	return (p - page - off);
+static const struct seq_operations prism2_wds_proc_seqops = {
+	.start	= prism2_wds_proc_start,
+	.next	= prism2_wds_proc_next,
+	.stop	= prism2_wds_proc_stop,
+	.show	= prism2_wds_proc_show,
+};
+
+static int prism2_wds_proc_open(struct inode *inode, struct file *file)
+{
+	int ret = seq_open(file, &prism2_wds_proc_seqops);
+	if (ret == 0) {
+		struct seq_file *m = file->private_data;
+		m->private = PDE_DATA(inode);
+	}
+	return ret;
 }
 
+static const struct file_operations prism2_wds_proc_fops = {
+	.open		= prism2_wds_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= seq_release,
+};
 
-static int prism2_bss_list_proc_read(char *page, char **start, off_t off,
-				     int count, int *eof, void *data)
+
+static int prism2_bss_list_proc_show(struct seq_file *m, void *v)
 {
-	char *p = page;
-	local_info_t *local = (local_info_t *) data;
-	struct list_head *ptr;
+	local_info_t *local = m->private;
+	struct list_head *ptr = v;
 	struct hostap_bss_info *bss;
 	int i;
 
-	if (off > PROC_LIMIT) {
-		*eof = 1;
+	if (ptr == &local->bss_list) {
+		seq_printf(m, "#BSSID\tlast_update\tcount\tcapab_info\tSSID(txt)\t"
+			   "SSID(hex)\tWPA IE\n");
 		return 0;
 	}
 
-	p += sprintf(p, "#BSSID\tlast_update\tcount\tcapab_info\tSSID(txt)\t"
-		     "SSID(hex)\tWPA IE\n");
+	bss = list_entry(ptr, struct hostap_bss_info, list);
+	seq_printf(m, "%pM\t%lu\t%u\t0x%x\t",
+		   bss->bssid, bss->last_update,
+		   bss->count, bss->capab_info);
+
+	for (i = 0; i < bss->ssid_len; i++)
+		seq_putc(m,bss->ssid[i] >= 32 && bss->ssid[i] < 127 ?
+			   bss->ssid[i] : '_');
+
+	seq_putc(m, '\t');
+	for (i = 0; i < bss->ssid_len; i++)
+		seq_printf(m, "%02x", bss->ssid[i]);
+	seq_putc(m, '\t');
+	for (i = 0; i < bss->wpa_ie_len; i++)
+		seq_printf(m, "%02x", bss->wpa_ie[i]);
+	seq_putc(m, '\n');
+	return 0;
+}
+
+static void *prism2_bss_list_proc_start(struct seq_file *m, loff_t *_pos)
+{
+	local_info_t *local = m->private;
 	spin_lock_bh(&local->lock);
-	list_for_each(ptr, &local->bss_list) {
-		bss = list_entry(ptr, struct hostap_bss_info, list);
-		p += sprintf(p, "%pM\t%lu\t%u\t0x%x\t",
-			     bss->bssid, bss->last_update,
-			     bss->count, bss->capab_info);
-		for (i = 0; i < bss->ssid_len; i++) {
-			p += sprintf(p, "%c",
-				     bss->ssid[i] >= 32 && bss->ssid[i] < 127 ?
-				     bss->ssid[i] : '_');
-		}
-		p += sprintf(p, "\t");
-		for (i = 0; i < bss->ssid_len; i++) {
-			p += sprintf(p, "%02x", bss->ssid[i]);
-		}
-		p += sprintf(p, "\t");
-		for (i = 0; i < bss->wpa_ie_len; i++) {
-			p += sprintf(p, "%02x", bss->wpa_ie[i]);
-		}
-		p += sprintf(p, "\n");
-		if ((p - page) > PROC_LIMIT) {
-			printk(KERN_DEBUG "%s: BSS proc did not fit\n",
-			       local->dev->name);
-			break;
-		}
-	}
-	spin_unlock_bh(&local->lock);
+	return seq_list_start_head(&local->bss_list, *_pos);
+}
 
-	if ((p - page) <= off) {
-		*eof = 1;
-		return 0;
-	}
+static void *prism2_bss_list_proc_next(struct seq_file *m, void *v, loff_t *_pos)
+{
+	local_info_t *local = m->private;
+	return seq_list_next(v, &local->bss_list, _pos);
+}
 
-	*start = page + off;
+static void prism2_bss_list_proc_stop(struct seq_file *m, void *v)
+{
+	local_info_t *local = m->private;
+	spin_unlock_bh(&local->lock);
+}
+
+static const struct seq_operations prism2_bss_list_proc_seqops = {
+	.start	= prism2_bss_list_proc_start,
+	.next	= prism2_bss_list_proc_next,
+	.stop	= prism2_bss_list_proc_stop,
+	.show	= prism2_bss_list_proc_show,
+};
 
-	return (p - page - off);
+static int prism2_bss_list_proc_open(struct inode *inode, struct file *file)
+{
+	int ret = seq_open(file, &prism2_bss_list_proc_seqops);
+	if (ret == 0) {
+		struct seq_file *m = file->private_data;
+		m->private = PDE_DATA(inode);
+	}
+	return ret;
 }
 
+static const struct file_operations prism2_bss_list_proc_fops = {
+	.open		= prism2_bss_list_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= seq_release,
+};
 
-static int prism2_crypt_proc_read(char *page, char **start, off_t off,
-				  int count, int *eof, void *data)
+
+static int prism2_crypt_proc_show(struct seq_file *m, void *v)
 {
-	char *p = page;
-	local_info_t *local = (local_info_t *) data;
+	local_info_t *local = m->private;
 	int i;
 
-	if (off > PROC_LIMIT) {
-		*eof = 1;
-		return 0;
-	}
-
-	p += sprintf(p, "tx_keyidx=%d\n", local->crypt_info.tx_keyidx);
+	seq_printf(m, "tx_keyidx=%d\n", local->crypt_info.tx_keyidx);
 	for (i = 0; i < WEP_KEYS; i++) {
 		if (local->crypt_info.crypt[i] &&
 		    local->crypt_info.crypt[i]->ops &&
 		    local->crypt_info.crypt[i]->ops->print_stats) {
-			p = local->crypt_info.crypt[i]->ops->print_stats(
-				p, local->crypt_info.crypt[i]->priv);
+			local->crypt_info.crypt[i]->ops->print_stats(
+				m, local->crypt_info.crypt[i]->priv);
 		}
 	}
+	return 0;
+}
 
-	if ((p - page) <= off) {
-		*eof = 1;
-		return 0;
-	}
-
-	*start = page + off;
-
-	return (p - page - off);
+static int prism2_crypt_proc_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, prism2_crypt_proc_show, PDE_DATA(inode));
 }
 
+static const struct file_operations prism2_crypt_proc_fops = {
+	.open		= prism2_crypt_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 
-static int prism2_pda_proc_read(char *page, char **start, off_t off,
-				int count, int *eof, void *data)
+
+static ssize_t prism2_pda_proc_read(struct file *file, char __user *buf,
+				    size_t count, loff_t *_pos)
 {
-	local_info_t *local = (local_info_t *) data;
+	local_info_t *local = PDE_DATA(file_inode(file));
+	size_t off;
 
-	if (local->pda == NULL || off >= PRISM2_PDA_SIZE) {
-		*eof = 1;
+	if (local->pda == NULL || *_pos >= PRISM2_PDA_SIZE)
 		return 0;
-	}
 
-	if (off + count > PRISM2_PDA_SIZE)
+	off = *_pos;
+	if (count > PRISM2_PDA_SIZE - off)
 		count = PRISM2_PDA_SIZE - off;
-
-	memcpy(page, local->pda + off, count);
+	if (copy_to_user(buf, local->pda + off, count) != 0)
+		return -EFAULT;
+	*_pos += count;
 	return count;
 }
 
+static const struct file_operations prism2_pda_proc_fops = {
+	.read		= prism2_pda_proc_read,
+	.llseek		= generic_file_llseek,
+};
 
-static int prism2_aux_dump_proc_read(char *page, char **start, off_t off,
-				     int count, int *eof, void *data)
-{
-	local_info_t *local = (local_info_t *) data;
-
-	if (local->func->read_aux == NULL) {
-		*eof = 1;
-		return 0;
-	}
 
-	if (local->func->read_aux(local->dev, off, count, page)) {
-		*eof = 1;
-		return 0;
-	}
-	*start = page;
-
-	return count;
+static ssize_t prism2_aux_dump_proc_no_read(struct file *file, char __user *buf,
+					    size_t bufsize, loff_t *_pos)
+{
+	return 0;
 }
 
+static const struct file_operations prism2_aux_dump_proc_fops = {
+	.read		= prism2_aux_dump_proc_no_read,
+};
+
 
 #ifdef PRISM2_IO_DEBUG
 static int prism2_io_debug_proc_read(char *page, char **start, off_t off,
@@ -306,82 +344,108 @@ static int prism2_io_debug_proc_read(char *page, char **start, off_t off,
 
 
 #ifndef PRISM2_NO_STATION_MODES
-static int prism2_scan_results_proc_read(char *page, char **start, off_t off,
-					 int count, int *eof, void *data)
+static int prism2_scan_results_proc_show(struct seq_file *m, void *v)
 {
-	char *p = page;
-	local_info_t *local = (local_info_t *) data;
-	int entry, i, len, total = 0;
+	local_info_t *local = m->private;
+	unsigned long entry;
+	int i, len;
 	struct hfa384x_hostscan_result *scanres;
-	u8 *pos;
+	u8 *p;
 
-	p += sprintf(p, "CHID ANL SL BcnInt Capab Rate BSSID ATIM SupRates "
-		     "SSID\n");
+	if (v == SEQ_START_TOKEN) {
+		seq_printf(m,
+			   "CHID ANL SL BcnInt Capab Rate BSSID ATIM SupRates SSID\n");
+		return 0;
+	}
 
+	entry = (unsigned long)v - 2;
+	scanres = &local->last_scan_results[entry];
+
+	seq_printf(m, "%d %d %d %d 0x%02x %d %pM %d ",
+		   le16_to_cpu(scanres->chid),
+		   (s16) le16_to_cpu(scanres->anl),
+		   (s16) le16_to_cpu(scanres->sl),
+		   le16_to_cpu(scanres->beacon_interval),
+		   le16_to_cpu(scanres->capability),
+		   le16_to_cpu(scanres->rate),
+		   scanres->bssid,
+		   le16_to_cpu(scanres->atim));
+
+	p = scanres->sup_rates;
+	for (i = 0; i < sizeof(scanres->sup_rates); i++) {
+		if (p[i] == 0)
+			break;
+		seq_printf(m, "<%02x>", p[i]);
+	}
+	seq_putc(m, ' ');
+
+	p = scanres->ssid;
+	len = le16_to_cpu(scanres->ssid_len);
+	if (len > 32)
+		len = 32;
+	for (i = 0; i < len; i++) {
+		unsigned char c = p[i];
+		if (c >= 32 && c < 127)
+			seq_putc(m, c);
+		else
+			seq_printf(m, "<%02x>", c);
+	}
+	seq_putc(m, '\n');
+	return 0;
+}
+
+static void *prism2_scan_results_proc_start(struct seq_file *m, loff_t *_pos)
+{
+	local_info_t *local = m->private;
 	spin_lock_bh(&local->lock);
-	for (entry = 0; entry < local->last_scan_results_count; entry++) {
-		scanres = &local->last_scan_results[entry];
 
-		if (total + (p - page) <= off) {
-			total += p - page;
-			p = page;
-		}
-		if (total + (p - page) > off + count)
-			break;
-		if ((p - page) > (PAGE_SIZE - 200))
-			break;
+	/* We have a header (pos 0) + N results to show (pos 1...N) */
+	if (*_pos > local->last_scan_results_count)
+		return NULL;
+	return (void *)(unsigned long)(*_pos + 1); /* 0 would be EOF */
+}
 
-		p += sprintf(p, "%d %d %d %d 0x%02x %d %pM %d ",
-			     le16_to_cpu(scanres->chid),
-			     (s16) le16_to_cpu(scanres->anl),
-			     (s16) le16_to_cpu(scanres->sl),
-			     le16_to_cpu(scanres->beacon_interval),
-			     le16_to_cpu(scanres->capability),
-			     le16_to_cpu(scanres->rate),
-			     scanres->bssid,
-			     le16_to_cpu(scanres->atim));
-
-		pos = scanres->sup_rates;
-		for (i = 0; i < sizeof(scanres->sup_rates); i++) {
-			if (pos[i] == 0)
-				break;
-			p += sprintf(p, "<%02x>", pos[i]);
-		}
-		p += sprintf(p, " ");
-
-		pos = scanres->ssid;
-		len = le16_to_cpu(scanres->ssid_len);
-		if (len > 32)
-			len = 32;
-		for (i = 0; i < len; i++) {
-			unsigned char c = pos[i];
-			if (c >= 32 && c < 127)
-				p += sprintf(p, "%c", c);
-			else
-				p += sprintf(p, "<%02x>", c);
-		}
-		p += sprintf(p, "\n");
-	}
+static void *prism2_scan_results_proc_next(struct seq_file *m, void *v, loff_t *_pos)
+{
+	local_info_t *local = m->private;
+
+	++*_pos;
+	if (*_pos > local->last_scan_results_count)
+		return NULL;
+	return (void *)(unsigned long)(*_pos + 1); /* 0 would be EOF */
+}
+
+static void prism2_scan_results_proc_stop(struct seq_file *m, void *v)
+{
+	local_info_t *local = m->private;
 	spin_unlock_bh(&local->lock);
+}
 
-	total += (p - page);
-	if (total >= off + count)
-		*eof = 1;
+static const struct seq_operations prism2_scan_results_proc_seqops = {
+	.start	= prism2_scan_results_proc_start,
+	.next	= prism2_scan_results_proc_next,
+	.stop	= prism2_scan_results_proc_stop,
+	.show	= prism2_scan_results_proc_show,
+};
 
-	if (total < off) {
-		*eof = 1;
-		return 0;
+static int prism2_scan_results_proc_open(struct inode *inode, struct file *file)
+{
+	int ret = seq_open(file, &prism2_scan_results_proc_seqops);
+	if (ret == 0) {
+		struct seq_file *m = file->private_data;
+		m->private = PDE_DATA(inode);
 	}
+	return ret;
+}
+
+static const struct file_operations prism2_scan_results_proc_fops = {
+	.open		= prism2_scan_results_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= seq_release,
+};
 
-	len = total - off;
-	if (len > (p - page))
-		len = p - page;
-	*start = p - len;
-	if (len > count)
-		len = count;
 
-	return len;
-}
 #endif /* PRISM2_NO_STATION_MODES */
 
 
@@ -403,53 +467,36 @@ void hostap_init_proc(local_info_t *local)
 	}
 
 #ifndef PRISM2_NO_PROCFS_DEBUG
-	create_proc_read_entry("debug", 0, local->proc,
-			       prism2_debug_proc_read, local);
+	proc_create_data("debug", 0, local->proc,
+			 &prism2_debug_proc_fops, local);
 #endif /* PRISM2_NO_PROCFS_DEBUG */
-	create_proc_read_entry("stats", 0, local->proc,
-			       prism2_stats_proc_read, local);
-	create_proc_read_entry("wds", 0, local->proc,
-			       prism2_wds_proc_read, local);
-	create_proc_read_entry("pda", 0, local->proc,
-			       prism2_pda_proc_read, local);
-	create_proc_read_entry("aux_dump", 0, local->proc,
-			       prism2_aux_dump_proc_read, local);
-	create_proc_read_entry("bss_list", 0, local->proc,
-			       prism2_bss_list_proc_read, local);
-	create_proc_read_entry("crypt", 0, local->proc,
-			       prism2_crypt_proc_read, local);
+	proc_create_data("stats", 0, local->proc,
+			 &prism2_stats_proc_fops, local);
+	proc_create_data("wds", 0, local->proc,
+			 &prism2_wds_proc_fops, local);
+	proc_create_data("pda", 0, local->proc,
+			 &prism2_pda_proc_fops, local);
+	proc_create_data("aux_dump", 0, local->proc,
+			 local->func->read_aux_fops ?: &prism2_aux_dump_proc_fops,
+			 local);
+	proc_create_data("bss_list", 0, local->proc,
+			 &prism2_bss_list_proc_fops, local);
+	proc_create_data("crypt", 0, local->proc,
+			 &prism2_crypt_proc_fops, local);
 #ifdef PRISM2_IO_DEBUG
-	create_proc_read_entry("io_debug", 0, local->proc,
-			       prism2_io_debug_proc_read, local);
+	proc_create_data("io_debug", 0, local->proc,
+			 &prism2_io_debug_proc_fops, local);
 #endif /* PRISM2_IO_DEBUG */
 #ifndef PRISM2_NO_STATION_MODES
-	create_proc_read_entry("scan_results", 0, local->proc,
-			       prism2_scan_results_proc_read, local);
+	proc_create_data("scan_results", 0, local->proc,
+			 &prism2_scan_results_proc_fops, local);
 #endif /* PRISM2_NO_STATION_MODES */
 }
 
 
 void hostap_remove_proc(local_info_t *local)
 {
-	if (local->proc != NULL) {
-#ifndef PRISM2_NO_STATION_MODES
-		remove_proc_entry("scan_results", local->proc);
-#endif /* PRISM2_NO_STATION_MODES */
-#ifdef PRISM2_IO_DEBUG
-		remove_proc_entry("io_debug", local->proc);
-#endif /* PRISM2_IO_DEBUG */
-		remove_proc_entry("pda", local->proc);
-		remove_proc_entry("aux_dump", local->proc);
-		remove_proc_entry("wds", local->proc);
-		remove_proc_entry("stats", local->proc);
-		remove_proc_entry("bss_list", local->proc);
-		remove_proc_entry("crypt", local->proc);
-#ifndef PRISM2_NO_PROCFS_DEBUG
-		remove_proc_entry("debug", local->proc);
-#endif /* PRISM2_NO_PROCFS_DEBUG */
-		if (hostap_proc != NULL)
-			remove_proc_entry(local->proc->name, hostap_proc);
-	}
+	remove_proc_subtree(local->ddev->name, hostap_proc);
 }
 
 
diff --git a/drivers/net/wireless/hostap/hostap_wlan.h b/drivers/net/wireless/hostap/hostap_wlan.h
index 7bb0b4b3f2cb..57904015380f 100644
--- a/drivers/net/wireless/hostap/hostap_wlan.h
+++ b/drivers/net/wireless/hostap/hostap_wlan.h
@@ -596,8 +596,7 @@ struct prism2_helper_functions {
 			struct prism2_download_param *param);
 	int (*tx)(struct sk_buff *skb, struct net_device *dev);
 	int (*set_tim)(struct net_device *dev, int aid, int set);
-	int (*read_aux)(struct net_device *dev, unsigned addr, int len,
-			u8 *buf);
+	const struct file_operations *read_aux_fops;
 
 	int need_tx_headroom; /* number of bytes of headroom needed before
 			       * IEEE 802.11 header */
diff --git a/drivers/net/wireless/ipw2x00/ipw2100.c b/drivers/net/wireless/ipw2x00/ipw2100.c
index cb066f62879d..15920aaa5dd6 100644
--- a/drivers/net/wireless/ipw2x00/ipw2100.c
+++ b/drivers/net/wireless/ipw2x00/ipw2100.c
@@ -4167,17 +4167,11 @@ static ssize_t show_debug_level(struct device_driver *d, char *buf)
 static ssize_t store_debug_level(struct device_driver *d,
 				 const char *buf, size_t count)
 {
-	char *p = (char *)buf;
 	u32 val;
+	int ret;
 
-	if (p[1] == 'x' || p[1] == 'X' || p[0] == 'x' || p[0] == 'X') {
-		p++;
-		if (p[0] == 'x' || p[0] == 'X')
-			p++;
-		val = simple_strtoul(p, &p, 16);
-	} else
-		val = simple_strtoul(p, &p, 10);
-	if (p == buf)
+	ret = kstrtou32(buf, 0, &val);
+	if (ret)
 		IPW_DEBUG_INFO(": %s is not in hex or decimal form.\n", buf);
 	else
 		ipw2100_debug_level = val;
@@ -4238,27 +4232,15 @@ static ssize_t store_scan_age(struct device *d, struct device_attribute *attr,
 {
 	struct ipw2100_priv *priv = dev_get_drvdata(d);
 	struct net_device *dev = priv->net_dev;
-	char buffer[] = "00000000";
-	unsigned long len =
-	    (sizeof(buffer) - 1) > count ? count : sizeof(buffer) - 1;
 	unsigned long val;
-	char *p = buffer;
+	int ret;
 
 	(void)dev;		/* kill unused-var warning for debug-only code */
 
 	IPW_DEBUG_INFO("enter\n");
 
-	strncpy(buffer, buf, len);
-	buffer[len] = 0;
-
-	if (p[1] == 'x' || p[1] == 'X' || p[0] == 'x' || p[0] == 'X') {
-		p++;
-		if (p[0] == 'x' || p[0] == 'X')
-			p++;
-		val = simple_strtoul(p, &p, 16);
-	} else
-		val = simple_strtoul(p, &p, 10);
-	if (p == buffer) {
+	ret = kstrtoul(buf, 0, &val);
+	if (ret) {
 		IPW_DEBUG_INFO("%s: user supplied invalid value.\n", dev->name);
 	} else {
 		priv->ieee->scan_age = val;
@@ -4266,7 +4248,7 @@ static ssize_t store_scan_age(struct device *d, struct device_attribute *attr,
 	}
 
 	IPW_DEBUG_INFO("exit\n");
-	return len;
+	return strnlen(buf, count);
 }
 
 static DEVICE_ATTR(scan_age, S_IWUSR | S_IRUGO, show_scan_age, store_scan_age);
diff --git a/drivers/net/wireless/iwlegacy/3945-mac.c b/drivers/net/wireless/iwlegacy/3945-mac.c
index c353b5f19c8c..b37a582ccbe7 100644
--- a/drivers/net/wireless/iwlegacy/3945-mac.c
+++ b/drivers/net/wireless/iwlegacy/3945-mac.c
@@ -3477,7 +3477,7 @@ static struct attribute_group il3945_attribute_group = {
 	.attrs = il3945_sysfs_entries,
 };
 
-struct ieee80211_ops il3945_mac_ops = {
+static struct ieee80211_ops il3945_mac_ops __read_mostly = {
 	.tx = il3945_mac_tx,
 	.start = il3945_mac_start,
 	.stop = il3945_mac_stop,
diff --git a/drivers/net/wireless/iwlegacy/3945-rs.c b/drivers/net/wireless/iwlegacy/3945-rs.c
index d4fd29ad90dc..c9f197d9ca1e 100644
--- a/drivers/net/wireless/iwlegacy/3945-rs.c
+++ b/drivers/net/wireless/iwlegacy/3945-rs.c
@@ -347,7 +347,7 @@ il3945_rs_rate_init(struct il_priv *il, struct ieee80211_sta *sta, u8 sta_id)
 
 	psta = (struct il3945_sta_priv *)sta->drv_priv;
 	rs_sta = &psta->rs_sta;
-	sband = hw->wiphy->bands[conf->channel->band];
+	sband = hw->wiphy->bands[conf->chandef.chan->band];
 
 	rs_sta->il = il;
 
diff --git a/drivers/net/wireless/iwlegacy/3945.c b/drivers/net/wireless/iwlegacy/3945.c
index e0b9d7fa5de0..dc1e6da9976a 100644
--- a/drivers/net/wireless/iwlegacy/3945.c
+++ b/drivers/net/wireless/iwlegacy/3945.c
@@ -2379,10 +2379,8 @@ il3945_hw_set_hw_params(struct il_priv *il)
 	il->_3945.shared_virt =
 	    dma_alloc_coherent(&il->pci_dev->dev, sizeof(struct il3945_shared),
 			       &il->_3945.shared_phys, GFP_KERNEL);
-	if (!il->_3945.shared_virt) {
-		IL_ERR("failed to allocate pci memory\n");
+	if (!il->_3945.shared_virt)
 		return -ENOMEM;
-	}
 
 	il->hw_params.bcast_id = IL3945_BROADCAST_ID;
 
diff --git a/drivers/net/wireless/iwlegacy/3945.h b/drivers/net/wireless/iwlegacy/3945.h
index 1d45075e0d5b..9a8703def0ba 100644
--- a/drivers/net/wireless/iwlegacy/3945.h
+++ b/drivers/net/wireless/iwlegacy/3945.h
@@ -150,10 +150,6 @@ struct il3945_frame {
 	struct list_head list;
 };
 
-#define SEQ_TO_SN(seq) (((seq) & IEEE80211_SCTL_SEQ) >> 4)
-#define SN_TO_SEQ(ssn) (((ssn) << 4) & IEEE80211_SCTL_SEQ)
-#define MAX_SN ((IEEE80211_SCTL_SEQ) >> 4)
-
 #define SUP_RATE_11A_MAX_NUM_CHANNELS  8
 #define SUP_RATE_11B_MAX_NUM_CHANNELS  4
 #define SUP_RATE_11G_MAX_NUM_CHANNELS  12
diff --git a/drivers/net/wireless/iwlegacy/4965-mac.c b/drivers/net/wireless/iwlegacy/4965-mac.c
index 7941eb3a0166..b8f82e688c72 100644
--- a/drivers/net/wireless/iwlegacy/4965-mac.c
+++ b/drivers/net/wireless/iwlegacy/4965-mac.c
@@ -612,7 +612,7 @@ il4965_pass_packet_to_mac80211(struct il_priv *il, struct ieee80211_hdr *hdr,
 
 /* Called for N_RX (legacy ABG frames), or
  * N_RX_MPDU (HT high-throughput N frames). */
-void
+static void
 il4965_hdl_rx(struct il_priv *il, struct il_rx_buf *rxb)
 {
 	struct ieee80211_hdr *header;
@@ -744,7 +744,7 @@ il4965_hdl_rx(struct il_priv *il, struct il_rx_buf *rxb)
 
 /* Cache phy data (Rx signal strength, etc) for HT frame (N_RX_PHY).
  * This will be used later in il_hdl_rx() for N_RX_MPDU. */
-void
+static void
 il4965_hdl_rx_phy(struct il_priv *il, struct il_rx_buf *rxb)
 {
 	struct il_rx_pkt *pkt = rxb_addr(rxb);
@@ -1250,7 +1250,7 @@ il4965_dump_fh(struct il_priv *il, char **buf, bool display)
 	return 0;
 }
 
-void
+static void
 il4965_hdl_missed_beacon(struct il_priv *il, struct il_rx_buf *rxb)
 {
 	struct il_rx_pkt *pkt = rxb_addr(rxb);
@@ -1357,7 +1357,7 @@ il4965_accumulative_stats(struct il_priv *il, __le32 * stats)
 }
 #endif
 
-void
+static void
 il4965_hdl_stats(struct il_priv *il, struct il_rx_buf *rxb)
 {
 	const int recalib_seconds = 60;
@@ -1399,7 +1399,7 @@ il4965_hdl_stats(struct il_priv *il, struct il_rx_buf *rxb)
 		il4965_temperature_calib(il);
 }
 
-void
+static void
 il4965_hdl_c_stats(struct il_priv *il, struct il_rx_buf *rxb)
 {
 	struct il_rx_pkt *pkt = rxb_addr(rxb);
@@ -1921,8 +1921,8 @@ drop_unlock:
 static inline int
 il4965_alloc_dma_ptr(struct il_priv *il, struct il_dma_ptr *ptr, size_t size)
 {
-	ptr->addr =
-	    dma_alloc_coherent(&il->pci_dev->dev, size, &ptr->dma, GFP_KERNEL);
+	ptr->addr = dma_alloc_coherent(&il->pci_dev->dev, size, &ptr->dma,
+				       GFP_KERNEL);
 	if (!ptr->addr)
 		return -ENOMEM;
 	ptr->size = size;
@@ -2050,7 +2050,7 @@ il4965_txq_ctx_reset(struct il_priv *il)
 		il_tx_queue_reset(il, txq_id);
 }
 
-void
+static void
 il4965_txq_ctx_unmap(struct il_priv *il)
 {
 	int txq_id;
@@ -2258,7 +2258,7 @@ il4965_tx_agg_start(struct il_priv *il, struct ieee80211_vif *vif,
 
 	spin_lock_irqsave(&il->sta_lock, flags);
 	tid_data = &il->stations[sta_id].tid[tid];
-	*ssn = SEQ_TO_SN(tid_data->seq_number);
+	*ssn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
 	tid_data->agg.txq_id = txq_id;
 	il_set_swq_id(&il->txq[txq_id], il4965_get_ac_from_tid(tid), txq_id);
 	spin_unlock_irqrestore(&il->sta_lock, flags);
@@ -2408,7 +2408,7 @@ il4965_txq_check_empty(struct il_priv *il, int sta_id, u8 tid, int txq_id)
 		/* aggregated HW queue */
 		if (txq_id == tid_data->agg.txq_id &&
 		    q->read_ptr == q->write_ptr) {
-			u16 ssn = SEQ_TO_SN(tid_data->seq_number);
+			u16 ssn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
 			int tx_fifo = il4965_get_fifo_from_tid(tid);
 			D_HT("HW queue empty: continue DELBA flow\n");
 			il4965_txq_agg_disable(il, txq_id, ssn, tx_fifo);
@@ -2627,7 +2627,8 @@ il4965_get_ra_sta_id(struct il_priv *il, struct ieee80211_hdr *hdr)
 static inline u32
 il4965_get_scd_ssn(struct il4965_tx_resp *tx_resp)
 {
-	return le32_to_cpup(&tx_resp->u.status + tx_resp->frame_count) & MAX_SN;
+	return le32_to_cpup(&tx_resp->u.status +
+			    tx_resp->frame_count) & IEEE80211_MAX_SN;
 }
 
 static inline u32
@@ -2717,15 +2718,15 @@ il4965_tx_status_reply_tx(struct il_priv *il, struct il_ht_agg *agg,
 			hdr = (struct ieee80211_hdr *) skb->data;
 
 			sc = le16_to_cpu(hdr->seq_ctrl);
-			if (idx != (SEQ_TO_SN(sc) & 0xff)) {
+			if (idx != (IEEE80211_SEQ_TO_SN(sc) & 0xff)) {
 				IL_ERR("BUG_ON idx doesn't match seq control"
 				       " idx=%d, seq_idx=%d, seq=%d\n", idx,
-				       SEQ_TO_SN(sc), hdr->seq_ctrl);
+				       IEEE80211_SEQ_TO_SN(sc), hdr->seq_ctrl);
 				return -1;
 			}
 
 			D_TX_REPLY("AGG Frame i=%d idx %d seq=%d\n", i, idx,
-				   SEQ_TO_SN(sc));
+				   IEEE80211_SEQ_TO_SN(sc));
 
 			sh = idx - start;
 			if (sh > 64) {
@@ -2895,7 +2896,7 @@ il4965_hwrate_to_tx_control(struct il_priv *il, u32 rate_n_flags,
  * Handles block-acknowledge notification from device, which reports success
  * of frames sent via aggregation.
  */
-void
+static void
 il4965_hdl_compressed_ba(struct il_priv *il, struct il_rx_buf *rxb)
 {
 	struct il_rx_pkt *pkt = rxb_addr(rxb);
@@ -6056,7 +6057,7 @@ il4965_mac_channel_switch(struct ieee80211_hw *hw,
 	struct il_priv *il = hw->priv;
 	const struct il_channel_info *ch_info;
 	struct ieee80211_conf *conf = &hw->conf;
-	struct ieee80211_channel *channel = ch_switch->channel;
+	struct ieee80211_channel *channel = ch_switch->chandef.chan;
 	struct il_ht_config *ht_conf = &il->current_ht_config;
 	u16 ch;
 
@@ -6093,23 +6094,21 @@ il4965_mac_channel_switch(struct ieee80211_hw *hw,
 	il->current_ht_config.smps = conf->smps_mode;
 
 	/* Configure HT40 channels */
-	il->ht.enabled = conf_is_ht(conf);
-	if (il->ht.enabled) {
-		if (conf_is_ht40_minus(conf)) {
-			il->ht.extension_chan_offset =
-			    IEEE80211_HT_PARAM_CHA_SEC_BELOW;
-			il->ht.is_40mhz = true;
-		} else if (conf_is_ht40_plus(conf)) {
-			il->ht.extension_chan_offset =
-			    IEEE80211_HT_PARAM_CHA_SEC_ABOVE;
-			il->ht.is_40mhz = true;
-		} else {
-			il->ht.extension_chan_offset =
-			    IEEE80211_HT_PARAM_CHA_SEC_NONE;
-			il->ht.is_40mhz = false;
-		}
-	} else
+	switch (cfg80211_get_chandef_type(&ch_switch->chandef)) {
+	case NL80211_CHAN_NO_HT:
+	case NL80211_CHAN_HT20:
 		il->ht.is_40mhz = false;
+		il->ht.extension_chan_offset = IEEE80211_HT_PARAM_CHA_SEC_NONE;
+		break;
+	case NL80211_CHAN_HT40MINUS:
+		il->ht.extension_chan_offset = IEEE80211_HT_PARAM_CHA_SEC_BELOW;
+		il->ht.is_40mhz = true;
+		break;
+	case NL80211_CHAN_HT40PLUS:
+		il->ht.extension_chan_offset = IEEE80211_HT_PARAM_CHA_SEC_ABOVE;
+		il->ht.is_40mhz = true;
+		break;
+	}
 
 	if ((le16_to_cpu(il->staging.channel) != ch))
 		il->staging.flags = 0;
@@ -6316,7 +6315,7 @@ il4965_tx_queue_set_status(struct il_priv *il, struct il_tx_queue *txq,
 	       scd_retry ? "BA" : "AC", txq_id, tx_fifo_id);
 }
 
-const struct ieee80211_ops il4965_mac_ops = {
+static const struct ieee80211_ops il4965_mac_ops = {
 	.tx = il4965_mac_tx,
 	.start = il4965_mac_start,
 	.stop = il4965_mac_stop,
diff --git a/drivers/net/wireless/iwlegacy/4965-rs.c b/drivers/net/wireless/iwlegacy/4965-rs.c
index 6c7493c2d698..1fc0b227e120 100644
--- a/drivers/net/wireless/iwlegacy/4965-rs.c
+++ b/drivers/net/wireless/iwlegacy/4965-rs.c
@@ -2300,7 +2300,7 @@ il4965_rs_rate_init(struct il_priv *il, struct ieee80211_sta *sta, u8 sta_id)
 
 	sta_priv = (struct il_station_priv *)sta->drv_priv;
 	lq_sta = &sta_priv->lq_sta;
-	sband = hw->wiphy->bands[conf->channel->band];
+	sband = hw->wiphy->bands[conf->chandef.chan->band];
 
 	lq_sta->lq.sta_id = sta_id;
 
diff --git a/drivers/net/wireless/iwlegacy/4965.c b/drivers/net/wireless/iwlegacy/4965.c
index 91eb2d07fdb8..777a578294bd 100644
--- a/drivers/net/wireless/iwlegacy/4965.c
+++ b/drivers/net/wireless/iwlegacy/4965.c
@@ -1493,7 +1493,7 @@ il4965_hw_channel_switch(struct il_priv *il,
 
 	cmd.band = band;
 	cmd.expect_beacon = 0;
-	ch = ch_switch->channel->hw_value;
+	ch = ch_switch->chandef.chan->hw_value;
 	cmd.channel = cpu_to_le16(ch);
 	cmd.rxon_flags = il->staging.flags;
 	cmd.rxon_filter_flags = il->staging.filter_flags;
diff --git a/drivers/net/wireless/iwlegacy/common.c b/drivers/net/wireless/iwlegacy/common.c
index e006ea831320..592d0aa634a8 100644
--- a/drivers/net/wireless/iwlegacy/common.c
+++ b/drivers/net/wireless/iwlegacy/common.c
@@ -1122,7 +1122,7 @@ il_set_power(struct il_priv *il, struct il_powertable_cmd *cmd)
 			       sizeof(struct il_powertable_cmd), cmd);
 }
 
-int
+static int
 il_power_set_mode(struct il_priv *il, struct il_powertable_cmd *cmd, bool force)
 {
 	int ret;
@@ -2566,15 +2566,13 @@ il_rx_queue_alloc(struct il_priv *il)
 	INIT_LIST_HEAD(&rxq->rx_used);
 
 	/* Alloc the circular buffer of Read Buffer Descriptors (RBDs) */
-	rxq->bd =
-	    dma_alloc_coherent(dev, 4 * RX_QUEUE_SIZE, &rxq->bd_dma,
-			       GFP_KERNEL);
+	rxq->bd = dma_alloc_coherent(dev, 4 * RX_QUEUE_SIZE, &rxq->bd_dma,
+				     GFP_KERNEL);
 	if (!rxq->bd)
 		goto err_bd;
 
-	rxq->rb_stts =
-	    dma_alloc_coherent(dev, sizeof(struct il_rb_status),
-			       &rxq->rb_stts_dma, GFP_KERNEL);
+	rxq->rb_stts = dma_alloc_coherent(dev, sizeof(struct il_rb_status),
+					  &rxq->rb_stts_dma, GFP_KERNEL);
 	if (!rxq->rb_stts)
 		goto err_rb;
 
@@ -2941,10 +2939,9 @@ il_tx_queue_alloc(struct il_priv *il, struct il_tx_queue *txq, u32 id)
 	 * shared with device */
 	txq->tfds =
 	    dma_alloc_coherent(dev, tfd_sz, &txq->q.dma_addr, GFP_KERNEL);
-	if (!txq->tfds) {
-		IL_ERR("Fail to alloc TFDs\n");
+	if (!txq->tfds)
 		goto error;
-	}
+
 	txq->q.id = id;
 
 	return 0;
@@ -4704,8 +4701,7 @@ out:
 }
 EXPORT_SYMBOL(il_mac_change_interface);
 
-void
-il_mac_flush(struct ieee80211_hw *hw, bool drop)
+void il_mac_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 {
 	struct il_priv *il = hw->priv;
 	unsigned long timeout = jiffies + msecs_to_jiffies(500);
@@ -4891,7 +4887,7 @@ il_add_beacon_time(struct il_priv *il, u32 base, u32 addon,
 }
 EXPORT_SYMBOL(il_add_beacon_time);
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 
 static int
 il_pci_suspend(struct device *device)
@@ -4942,7 +4938,7 @@ il_pci_resume(struct device *device)
 SIMPLE_DEV_PM_OPS(il_pm_ops, il_pci_suspend, il_pci_resume);
 EXPORT_SYMBOL(il_pm_ops);
 
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 static void
 il_update_qos(struct il_priv *il)
@@ -4975,7 +4971,7 @@ il_mac_config(struct ieee80211_hw *hw, u32 changed)
 	struct il_priv *il = hw->priv;
 	const struct il_channel_info *ch_info;
 	struct ieee80211_conf *conf = &hw->conf;
-	struct ieee80211_channel *channel = conf->channel;
+	struct ieee80211_channel *channel = conf->chandef.chan;
 	struct il_ht_config *ht_conf = &il->current_ht_config;
 	unsigned long flags = 0;
 	int ret = 0;
diff --git a/drivers/net/wireless/iwlegacy/common.h b/drivers/net/wireless/iwlegacy/common.h
index 96f2025d936e..f8246f2d88f9 100644
--- a/drivers/net/wireless/iwlegacy/common.h
+++ b/drivers/net/wireless/iwlegacy/common.h
@@ -541,10 +541,6 @@ struct il_frame {
 	struct list_head list;
 };
 
-#define SEQ_TO_SN(seq) (((seq) & IEEE80211_SCTL_SEQ) >> 4)
-#define SN_TO_SEQ(ssn) (((ssn) << 4) & IEEE80211_SCTL_SEQ)
-#define MAX_SN ((IEEE80211_SCTL_SEQ) >> 4)
-
 enum {
 	CMD_SYNC = 0,
 	CMD_SIZE_NORMAL = 0,
@@ -1724,7 +1720,7 @@ void il_mac_remove_interface(struct ieee80211_hw *hw,
 			     struct ieee80211_vif *vif);
 int il_mac_change_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			    enum nl80211_iftype newtype, bool newp2p);
-void il_mac_flush(struct ieee80211_hw *hw, bool drop);
+void il_mac_flush(struct ieee80211_hw *hw, u32 queues, bool drop);
 int il_alloc_txq_mem(struct il_priv *il);
 void il_free_txq_mem(struct il_priv *il);
 
@@ -2235,9 +2231,8 @@ il_alloc_fw_desc(struct pci_dev *pci_dev, struct fw_desc *desc)
 		return -EINVAL;
 	}
 
-	desc->v_addr =
-	    dma_alloc_coherent(&pci_dev->dev, desc->len, &desc->p_addr,
-			       GFP_KERNEL);
+	desc->v_addr = dma_alloc_coherent(&pci_dev->dev, desc->len,
+					  &desc->p_addr, GFP_KERNEL);
 	return (desc->v_addr != NULL) ? 0 : -ENOMEM;
 }
 
diff --git a/drivers/net/wireless/iwlwifi/Kconfig b/drivers/net/wireless/iwlwifi/Kconfig
index ba319cba3f1e..56c2040a955b 100644
--- a/drivers/net/wireless/iwlwifi/Kconfig
+++ b/drivers/net/wireless/iwlwifi/Kconfig
@@ -6,7 +6,6 @@ config IWLWIFI
 	select LEDS_CLASS
 	select LEDS_TRIGGERS
 	select MAC80211_LEDS
-	select IWLDVM
 	---help---
 	  Select to build the driver supporting the:
 
@@ -45,6 +44,7 @@ config IWLWIFI
 config IWLDVM
 	tristate "Intel Wireless WiFi DVM Firmware support"
 	depends on IWLWIFI
+	default IWLWIFI
 	help
 	  This is the driver supporting the DVM firmware which is
 	  currently the only firmware available for existing devices.
@@ -58,6 +58,15 @@ config IWLMVM
 
 	  Say yes if you have such a device.
 
+# don't call it _MODULE -- will confuse Kconfig/fixdep/...
+config IWLWIFI_OPMODE_MODULAR
+	bool
+	default y if IWLDVM=m
+	default y if IWLMVM=m
+
+comment "WARNING: iwlwifi is useless without IWLDVM or IWLMVM"
+	depends on IWLWIFI && IWLDVM=n && IWLMVM=n
+
 menu "Debugging Options"
 	depends on IWLWIFI
 
diff --git a/drivers/net/wireless/iwlwifi/Makefile b/drivers/net/wireless/iwlwifi/Makefile
index 6c7800044a04..3b5613ea458b 100644
--- a/drivers/net/wireless/iwlwifi/Makefile
+++ b/drivers/net/wireless/iwlwifi/Makefile
@@ -7,8 +7,7 @@ iwlwifi-objs		+= iwl-notif-wait.o
 iwlwifi-objs		+= iwl-eeprom-read.o iwl-eeprom-parse.o
 iwlwifi-objs		+= iwl-phy-db.o iwl-nvm-parse.o
 iwlwifi-objs		+= pcie/drv.o pcie/rx.o pcie/tx.o pcie/trans.o
-iwlwifi-objs		+= pcie/1000.o pcie/2000.o pcie/5000.o pcie/6000.o
-iwlwifi-objs		+= pcie/7000.o
+iwlwifi-objs		+= iwl-1000.o iwl-2000.o iwl-5000.o iwl-6000.o iwl-7000.o
 
 iwlwifi-$(CONFIG_IWLWIFI_DEVICE_TRACING) += iwl-devtrace.o
 iwlwifi-$(CONFIG_IWLWIFI_DEVICE_TESTMODE) += iwl-test.o
diff --git a/drivers/net/wireless/iwlwifi/dvm/agn.h b/drivers/net/wireless/iwlwifi/dvm/agn.h
index 41ec27cb6efe..48545ab00311 100644
--- a/drivers/net/wireless/iwlwifi/dvm/agn.h
+++ b/drivers/net/wireless/iwlwifi/dvm/agn.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -73,6 +73,8 @@
 /* AUX (TX during scan dwell) queue */
 #define IWL_AUX_QUEUE		10
 
+#define IWL_INVALID_STATION	255
+
 /* device operations */
 extern struct iwl_lib_ops iwl1000_lib;
 extern struct iwl_lib_ops iwl2000_lib;
@@ -170,13 +172,13 @@ int iwl_calib_set(struct iwl_priv *priv,
 		  const struct iwl_calib_hdr *cmd, int len);
 void iwl_calib_free_results(struct iwl_priv *priv);
 int iwl_dump_nic_event_log(struct iwl_priv *priv, bool full_log,
-			    char **buf, bool display);
+			    char **buf);
 int iwlagn_hw_valid_rtc_data_addr(u32 addr);
 
 /* lib */
 int iwlagn_send_tx_power(struct iwl_priv *priv);
 void iwlagn_temperature(struct iwl_priv *priv);
-int iwlagn_txfifo_flush(struct iwl_priv *priv);
+int iwlagn_txfifo_flush(struct iwl_priv *priv, u32 scd_q_msk);
 void iwlagn_dev_txfifo_flush(struct iwl_priv *priv);
 int iwlagn_send_beacon_cmd(struct iwl_priv *priv);
 int iwl_send_statistics_request(struct iwl_priv *priv,
@@ -210,6 +212,8 @@ int iwlagn_tx_agg_oper(struct iwl_priv *priv, struct ieee80211_vif *vif,
 			struct ieee80211_sta *sta, u16 tid, u8 buf_size);
 int iwlagn_tx_agg_stop(struct iwl_priv *priv, struct ieee80211_vif *vif,
 		       struct ieee80211_sta *sta, u16 tid);
+int iwlagn_tx_agg_flush(struct iwl_priv *priv, struct ieee80211_vif *vif,
+			struct ieee80211_sta *sta, u16 tid);
 int iwlagn_rx_reply_compressed_ba(struct iwl_priv *priv,
 				   struct iwl_rx_cmd_buffer *rxb,
 				   struct iwl_device_cmd *cmd);
diff --git a/drivers/net/wireless/iwlwifi/dvm/calib.c b/drivers/net/wireless/iwlwifi/dvm/calib.c
index 6468de8634b0..d6c4cf2ad7c5 100644
--- a/drivers/net/wireless/iwlwifi/dvm/calib.c
+++ b/drivers/net/wireless/iwlwifi/dvm/calib.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/dvm/calib.h b/drivers/net/wireless/iwlwifi/dvm/calib.h
index 65e920cab2b7..cfddde194940 100644
--- a/drivers/net/wireless/iwlwifi/dvm/calib.h
+++ b/drivers/net/wireless/iwlwifi/dvm/calib.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/dvm/commands.h b/drivers/net/wireless/iwlwifi/dvm/commands.h
index 84e2c0fcfef6..95ca026ecc9d 100644
--- a/drivers/net/wireless/iwlwifi/dvm/commands.h
+++ b/drivers/net/wireless/iwlwifi/dvm/commands.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -1526,6 +1526,7 @@ struct iwl_compressed_ba_resp {
 	__le16 scd_ssn;
 	u8 txed;	/* number of frames sent */
 	u8 txed_2_done; /* number of frames acked */
+	__le16 reserved1;
 } __packed;
 
 /*
diff --git a/drivers/net/wireless/iwlwifi/dvm/debugfs.c b/drivers/net/wireless/iwlwifi/dvm/debugfs.c
index 20806cae11b7..d5329489245a 100644
--- a/drivers/net/wireless/iwlwifi/dvm/debugfs.c
+++ b/drivers/net/wireless/iwlwifi/dvm/debugfs.c
@@ -19,7 +19,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -2237,15 +2237,13 @@ static ssize_t iwl_dbgfs_log_event_read(struct file *file,
 					 size_t count, loff_t *ppos)
 {
 	struct iwl_priv *priv = file->private_data;
-	char *buf;
-	int pos = 0;
-	ssize_t ret = -ENOMEM;
+	char *buf = NULL;
+	ssize_t ret;
 
-	ret = pos = iwl_dump_nic_event_log(priv, true, &buf, true);
-	if (buf) {
-		ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
-		kfree(buf);
-	}
+	ret = iwl_dump_nic_event_log(priv, true, &buf);
+	if (ret > 0)
+		ret = simple_read_from_buffer(user_buf, count, ppos, buf, ret);
+	kfree(buf);
 	return ret;
 }
 
@@ -2269,7 +2267,7 @@ static ssize_t iwl_dbgfs_log_event_write(struct file *file,
 	if (sscanf(buf, "%d", &event_log_flag) != 1)
 		return -EFAULT;
 	if (event_log_flag == 1)
-		iwl_dump_nic_event_log(priv, true, NULL, false);
+		iwl_dump_nic_event_log(priv, true, NULL);
 
 	return count;
 }
@@ -2324,6 +2322,28 @@ static ssize_t iwl_dbgfs_calib_disabled_write(struct file *file,
 	return count;
 }
 
+static ssize_t iwl_dbgfs_fw_restart_write(struct file *file,
+					  const char __user *user_buf,
+					  size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	bool restart_fw = iwlwifi_mod_params.restart_fw;
+	int ret;
+
+	iwlwifi_mod_params.restart_fw = true;
+
+	mutex_lock(&priv->mutex);
+
+	/* take the return value to make compiler happy - it will fail anyway */
+	ret = iwl_dvm_send_cmd_pdu(priv, REPLY_ERROR, CMD_SYNC, 0, NULL);
+
+	mutex_unlock(&priv->mutex);
+
+	iwlwifi_mod_params.restart_fw = restart_fw;
+
+	return count;
+}
+
 DEBUGFS_READ_FILE_OPS(ucode_rx_stats);
 DEBUGFS_READ_FILE_OPS(ucode_tx_stats);
 DEBUGFS_READ_FILE_OPS(ucode_general_stats);
@@ -2343,6 +2363,7 @@ DEBUGFS_READ_FILE_OPS(bt_traffic);
 DEBUGFS_READ_WRITE_FILE_OPS(protection_mode);
 DEBUGFS_READ_FILE_OPS(reply_tx_error);
 DEBUGFS_WRITE_FILE_OPS(echo_test);
+DEBUGFS_WRITE_FILE_OPS(fw_restart);
 #ifdef CONFIG_IWLWIFI_DEBUG
 DEBUGFS_READ_WRITE_FILE_OPS(log_event);
 #endif
@@ -2400,6 +2421,7 @@ int iwl_dbgfs_register(struct iwl_priv *priv, struct dentry *dbgfs_dir)
 	DEBUGFS_ADD_FILE(rxon_flags, dir_debug, S_IWUSR);
 	DEBUGFS_ADD_FILE(rxon_filter_flags, dir_debug, S_IWUSR);
 	DEBUGFS_ADD_FILE(echo_test, dir_debug, S_IWUSR);
+	DEBUGFS_ADD_FILE(fw_restart, dir_debug, S_IWUSR);
 #ifdef CONFIG_IWLWIFI_DEBUG
 	DEBUGFS_ADD_FILE(log_event, dir_debug, S_IWUSR | S_IRUSR);
 #endif
diff --git a/drivers/net/wireless/iwlwifi/dvm/devices.c b/drivers/net/wireless/iwlwifi/dvm/devices.c
index 15cca2ef9294..c48907c8ab43 100644
--- a/drivers/net/wireless/iwlwifi/dvm/devices.c
+++ b/drivers/net/wireless/iwlwifi/dvm/devices.c
@@ -379,7 +379,7 @@ static int iwl5000_hw_channel_switch(struct iwl_priv *priv,
 	};
 
 	cmd.band = priv->band == IEEE80211_BAND_2GHZ;
-	ch = ch_switch->channel->hw_value;
+	ch = ch_switch->chandef.chan->hw_value;
 	IWL_DEBUG_11H(priv, "channel switch from %d to %d\n",
 		      ctx->active.channel, ch);
 	cmd.channel = cpu_to_le16(ch);
@@ -414,7 +414,8 @@ static int iwl5000_hw_channel_switch(struct iwl_priv *priv,
 	}
 	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.expect_beacon =
+		ch_switch->chandef.chan->flags & IEEE80211_CHAN_RADAR;
 
 	return iwl_dvm_send_cmd(priv, &hcmd);
 }
@@ -540,7 +541,7 @@ static int iwl6000_hw_channel_switch(struct iwl_priv *priv,
 	hcmd.data[0] = cmd;
 
 	cmd->band = priv->band == IEEE80211_BAND_2GHZ;
-	ch = ch_switch->channel->hw_value;
+	ch = ch_switch->chandef.chan->hw_value;
 	IWL_DEBUG_11H(priv, "channel switch from %u to %u\n",
 		      ctx->active.channel, ch);
 	cmd->channel = cpu_to_le16(ch);
@@ -575,7 +576,8 @@ static int iwl6000_hw_channel_switch(struct iwl_priv *priv,
 	}
 	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->expect_beacon =
+		ch_switch->chandef.chan->flags & IEEE80211_CHAN_RADAR;
 
 	err = iwl_dvm_send_cmd(priv, &hcmd);
 	kfree(cmd);
diff --git a/drivers/net/wireless/iwlwifi/dvm/lib.c b/drivers/net/wireless/iwlwifi/dvm/lib.c
index 44ca0e57f9f7..54f553380aa8 100644
--- a/drivers/net/wireless/iwlwifi/dvm/lib.c
+++ b/drivers/net/wireless/iwlwifi/dvm/lib.c
@@ -19,7 +19,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -136,7 +136,7 @@ int iwlagn_manage_ibss_station(struct iwl_priv *priv,
  *  1. acquire mutex before calling
  *  2. make sure rf is on and not in exit state
  */
-int iwlagn_txfifo_flush(struct iwl_priv *priv)
+int iwlagn_txfifo_flush(struct iwl_priv *priv, u32 scd_q_msk)
 {
 	struct iwl_txfifo_flush_cmd flush_cmd;
 	struct iwl_host_cmd cmd = {
@@ -162,6 +162,9 @@ int iwlagn_txfifo_flush(struct iwl_priv *priv)
 	if (priv->nvm_data->sku_cap_11n_enable)
 		flush_cmd.queue_control |= IWL_AGG_TX_QUEUE_MSK;
 
+	if (scd_q_msk)
+		flush_cmd.queue_control = cpu_to_le32(scd_q_msk);
+
 	IWL_DEBUG_INFO(priv, "queue control: 0x%x\n",
 		       flush_cmd.queue_control);
 	flush_cmd.flush_control = cpu_to_le16(IWL_DROP_ALL);
@@ -173,7 +176,7 @@ void iwlagn_dev_txfifo_flush(struct iwl_priv *priv)
 {
 	mutex_lock(&priv->mutex);
 	ieee80211_stop_queues(priv->hw);
-	if (iwlagn_txfifo_flush(priv)) {
+	if (iwlagn_txfifo_flush(priv, 0)) {
 		IWL_ERR(priv, "flush request fail\n");
 		goto done;
 	}
@@ -1084,7 +1087,14 @@ int iwlagn_suspend(struct iwl_priv *priv, struct cfg80211_wowlan *wowlan)
 	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
 	struct iwlagn_wowlan_kek_kck_material_cmd kek_kck_cmd;
 	struct iwlagn_wowlan_tkip_params_cmd tkip_cmd = {};
-	struct iwlagn_d3_config_cmd d3_cfg_cmd = {};
+	struct iwlagn_d3_config_cmd d3_cfg_cmd = {
+		/*
+		 * Program the minimum sleep time to 10 seconds, as many
+		 * platforms have issues processing a wakeup signal while
+		 * still being in the process of suspending.
+		 */
+		.min_sleep_time = cpu_to_le32(10 * 1000 * 1000),
+	};
 	struct wowlan_key_data key_data = {
 		.ctx = ctx,
 		.bssid = ctx->active.bssid_addr,
diff --git a/drivers/net/wireless/iwlwifi/dvm/mac80211.c b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
index 323e4a33fcac..cab23af0be9e 100644
--- a/drivers/net/wireless/iwlwifi/dvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
@@ -777,9 +777,12 @@ static int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
 		IWL_DEBUG_HT(priv, "start Tx\n");
 		ret = iwlagn_tx_agg_start(priv, vif, sta, tid, ssn);
 		break;
-	case IEEE80211_AMPDU_TX_STOP_CONT:
 	case IEEE80211_AMPDU_TX_STOP_FLUSH:
 	case IEEE80211_AMPDU_TX_STOP_FLUSH_CONT:
+		IWL_DEBUG_HT(priv, "Flush Tx\n");
+		ret = iwlagn_tx_agg_flush(priv, vif, sta, tid);
+		break;
+	case IEEE80211_AMPDU_TX_STOP_CONT:
 		IWL_DEBUG_HT(priv, "stop Tx\n");
 		ret = iwlagn_tx_agg_stop(priv, vif, sta, tid);
 		if ((ret == 0) && (priv->agg_tids_count > 0)) {
@@ -967,7 +970,7 @@ static void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	struct ieee80211_conf *conf = &hw->conf;
-	struct ieee80211_channel *channel = ch_switch->channel;
+	struct ieee80211_channel *channel = ch_switch->chandef.chan;
 	struct iwl_ht_config *ht_conf = &priv->current_ht_config;
 	/*
 	 * MULTI-FIXME
@@ -1005,11 +1008,21 @@ static void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
 	priv->current_ht_config.smps = conf->smps_mode;
 
 	/* Configure HT40 channels */
-	ctx->ht.enabled = conf_is_ht(conf);
-	if (ctx->ht.enabled)
-		iwlagn_config_ht40(conf, ctx);
-	else
+	switch (cfg80211_get_chandef_type(&ch_switch->chandef)) {
+	case NL80211_CHAN_NO_HT:
+	case NL80211_CHAN_HT20:
 		ctx->ht.is_40mhz = false;
+		ctx->ht.extension_chan_offset = IEEE80211_HT_PARAM_CHA_SEC_NONE;
+		break;
+	case NL80211_CHAN_HT40MINUS:
+		ctx->ht.extension_chan_offset = IEEE80211_HT_PARAM_CHA_SEC_BELOW;
+		ctx->ht.is_40mhz = true;
+		break;
+	case NL80211_CHAN_HT40PLUS:
+		ctx->ht.extension_chan_offset = IEEE80211_HT_PARAM_CHA_SEC_ABOVE;
+		ctx->ht.is_40mhz = true;
+		break;
+	}
 
 	if ((le16_to_cpu(ctx->staging.channel) != ch))
 		ctx->staging.flags = 0;
@@ -1100,7 +1113,7 @@ static void iwlagn_configure_filter(struct ieee80211_hw *hw,
 			FIF_BCN_PRBRESP_PROMISC | FIF_CONTROL;
 }
 
-static void iwlagn_mac_flush(struct ieee80211_hw *hw, bool drop)
+static void iwlagn_mac_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -1122,7 +1135,7 @@ static void iwlagn_mac_flush(struct ieee80211_hw *hw, bool drop)
 	 */
 	if (drop) {
 		IWL_DEBUG_MAC80211(priv, "send flush command\n");
-		if (iwlagn_txfifo_flush(priv)) {
+		if (iwlagn_txfifo_flush(priv, 0)) {
 			IWL_ERR(priv, "flush request fail\n");
 			goto done;
 		}
@@ -1137,7 +1150,8 @@ done:
 static int iwlagn_mac_remain_on_channel(struct ieee80211_hw *hw,
 				     struct ieee80211_vif *vif,
 				     struct ieee80211_channel *channel,
-				     int duration)
+				     int duration,
+				     enum ieee80211_roc_type type)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_PAN];
diff --git a/drivers/net/wireless/iwlwifi/dvm/main.c b/drivers/net/wireless/iwlwifi/dvm/main.c
index b9e3517652d6..74d7572e7091 100644
--- a/drivers/net/wireless/iwlwifi/dvm/main.c
+++ b/drivers/net/wireless/iwlwifi/dvm/main.c
@@ -1795,7 +1795,7 @@ static int iwl_print_last_event_logs(struct iwl_priv *priv, u32 capacity,
 #define DEFAULT_DUMP_EVENT_LOG_ENTRIES (20)
 
 int iwl_dump_nic_event_log(struct iwl_priv *priv, bool full_log,
-			    char **buf, bool display)
+			    char **buf)
 {
 	u32 base;       /* SRAM byte address of event log header */
 	u32 capacity;   /* event log capacity in # entries */
@@ -1866,7 +1866,7 @@ int iwl_dump_nic_event_log(struct iwl_priv *priv, bool full_log,
 		size);
 
 #ifdef CONFIG_IWLWIFI_DEBUG
-	if (display) {
+	if (buf) {
 		if (full_log)
 			bufsz = capacity * 48;
 		else
@@ -1962,7 +1962,7 @@ static void iwl_nic_error(struct iwl_op_mode *op_mode)
 		priv->fw->fw_version);
 
 	iwl_dump_nic_error_log(priv);
-	iwl_dump_nic_event_log(priv, false, NULL, false);
+	iwl_dump_nic_event_log(priv, false, NULL);
 
 	iwlagn_fw_error(priv, false);
 }
diff --git a/drivers/net/wireless/iwlwifi/dvm/rs.c b/drivers/net/wireless/iwlwifi/dvm/rs.c
index abe304267261..907bd6e50aad 100644
--- a/drivers/net/wireless/iwlwifi/dvm/rs.c
+++ b/drivers/net/wireless/iwlwifi/dvm/rs.c
@@ -2831,7 +2831,7 @@ void iwl_rs_rate_init(struct iwl_priv *priv, struct ieee80211_sta *sta, u8 sta_i
 
 	sta_priv = (struct iwl_station_priv *) sta->drv_priv;
 	lq_sta = &sta_priv->lq_sta;
-	sband = hw->wiphy->bands[conf->channel->band];
+	sband = hw->wiphy->bands[conf->chandef.chan->band];
 
 
 	lq_sta->lq.sta_id = sta_id;
diff --git a/drivers/net/wireless/iwlwifi/dvm/rxon.c b/drivers/net/wireless/iwlwifi/dvm/rxon.c
index a82b6b39d4ff..707446fa00bd 100644
--- a/drivers/net/wireless/iwlwifi/dvm/rxon.c
+++ b/drivers/net/wireless/iwlwifi/dvm/rxon.c
@@ -78,8 +78,9 @@ void iwl_connection_init_rx_config(struct iwl_priv *priv,
 		ctx->staging.flags |= RXON_FLG_SHORT_PREAMBLE_MSK;
 #endif
 
-	ctx->staging.channel = cpu_to_le16(priv->hw->conf.channel->hw_value);
-	priv->band = priv->hw->conf.channel->band;
+	ctx->staging.channel =
+		cpu_to_le16(priv->hw->conf.chandef.chan->hw_value);
+	priv->band = priv->hw->conf.chandef.chan->band;
 
 	iwl_set_flags_for_band(priv, ctx, priv->band, ctx->vif);
 
@@ -951,7 +952,7 @@ static void iwl_calc_basic_rates(struct iwl_priv *priv,
 		unsigned long basic = ctx->vif->bss_conf.basic_rates;
 		int i;
 
-		sband = priv->hw->wiphy->bands[priv->hw->conf.channel->band];
+		sband = priv->hw->wiphy->bands[priv->hw->conf.chandef.chan->band];
 
 		for_each_set_bit(i, &basic, BITS_PER_LONG) {
 			int hw = sband->bitrates[i].hw_value;
@@ -1159,7 +1160,7 @@ int iwlagn_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 }
 
 void iwlagn_config_ht40(struct ieee80211_conf *conf,
-	struct iwl_rxon_context *ctx)
+			struct iwl_rxon_context *ctx)
 {
 	if (conf_is_ht40_minus(conf)) {
 		ctx->ht.extension_chan_offset =
@@ -1181,7 +1182,7 @@ int iwlagn_mac_config(struct ieee80211_hw *hw, u32 changed)
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	struct iwl_rxon_context *ctx;
 	struct ieee80211_conf *conf = &hw->conf;
-	struct ieee80211_channel *channel = conf->channel;
+	struct ieee80211_channel *channel = conf->chandef.chan;
 	int ret = 0;
 
 	IWL_DEBUG_MAC80211(priv, "enter: changed %#x\n", changed);
diff --git a/drivers/net/wireless/iwlwifi/dvm/scan.c b/drivers/net/wireless/iwlwifi/dvm/scan.c
index 3a4aa5239c45..d69b55866714 100644
--- a/drivers/net/wireless/iwlwifi/dvm/scan.c
+++ b/drivers/net/wireless/iwlwifi/dvm/scan.c
@@ -19,7 +19,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/dvm/sta.c b/drivers/net/wireless/iwlwifi/dvm/sta.c
index b775769f8322..db183b44e038 100644
--- a/drivers/net/wireless/iwlwifi/dvm/sta.c
+++ b/drivers/net/wireless/iwlwifi/dvm/sta.c
@@ -695,6 +695,7 @@ void iwl_clear_ucode_stations(struct iwl_priv *priv,
 void iwl_restore_stations(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 {
 	struct iwl_addsta_cmd sta_cmd;
+	static const struct iwl_link_quality_cmd zero_lq = {};
 	struct iwl_link_quality_cmd lq;
 	int i;
 	bool found = false;
@@ -733,7 +734,9 @@ void iwl_restore_stations(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 				else
 					memcpy(&lq, priv->stations[i].lq,
 					       sizeof(struct iwl_link_quality_cmd));
-				send_lq = true;
+
+				if (!memcmp(&lq, &zero_lq, sizeof(lq)))
+					send_lq = true;
 			}
 			spin_unlock_bh(&priv->sta_lock);
 			ret = iwl_send_add_sta(priv, &sta_cmd, CMD_SYNC);
diff --git a/drivers/net/wireless/iwlwifi/dvm/testmode.c b/drivers/net/wireless/iwlwifi/dvm/testmode.c
index dc6f965a123a..b89b9d9b9969 100644
--- a/drivers/net/wireless/iwlwifi/dvm/testmode.c
+++ b/drivers/net/wireless/iwlwifi/dvm/testmode.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/dvm/tx.c b/drivers/net/wireless/iwlwifi/dvm/tx.c
index d1a670d7b10c..a900aaf47790 100644
--- a/drivers/net/wireless/iwlwifi/dvm/tx.c
+++ b/drivers/net/wireless/iwlwifi/dvm/tx.c
@@ -19,7 +19,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -418,7 +418,8 @@ int iwlagn_tx_skb(struct iwl_priv *priv,
 				" Tx flags = 0x%08x, agg.state = %d",
 				info->flags, tid_data->agg.state);
 			IWL_ERR(priv, "sta_id = %d, tid = %d seq_num = %d",
-				sta_id, tid, SEQ_TO_SN(tid_data->seq_number));
+				sta_id, tid,
+				IEEE80211_SEQ_TO_SN(tid_data->seq_number));
 			goto drop_unlock_sta;
 		}
 
@@ -569,7 +570,7 @@ int iwlagn_tx_agg_stop(struct iwl_priv *priv, struct ieee80211_vif *vif,
 		return 0;
 	}
 
-	tid_data->agg.ssn = SEQ_TO_SN(tid_data->seq_number);
+	tid_data->agg.ssn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
 
 	/* There are still packets for this RA / TID in the HW */
 	if (!test_bit(txq_id, priv->agg_q_alloc)) {
@@ -651,7 +652,7 @@ int iwlagn_tx_agg_start(struct iwl_priv *priv, struct ieee80211_vif *vif,
 
 	spin_lock_bh(&priv->sta_lock);
 	tid_data = &priv->tid_data[sta_id][tid];
-	tid_data->agg.ssn = SEQ_TO_SN(tid_data->seq_number);
+	tid_data->agg.ssn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
 	tid_data->agg.txq_id = txq_id;
 
 	*ssn = tid_data->agg.ssn;
@@ -673,6 +674,51 @@ int iwlagn_tx_agg_start(struct iwl_priv *priv, struct ieee80211_vif *vif,
 	return ret;
 }
 
+int iwlagn_tx_agg_flush(struct iwl_priv *priv, struct ieee80211_vif *vif,
+			struct ieee80211_sta *sta, u16 tid)
+{
+	struct iwl_tid_data *tid_data;
+	enum iwl_agg_state agg_state;
+	int sta_id, txq_id;
+	sta_id = iwl_sta_id(sta);
+
+	/*
+	 * First set the agg state to OFF to avoid calling
+	 * ieee80211_stop_tx_ba_cb in iwlagn_check_ratid_empty.
+	 */
+	spin_lock_bh(&priv->sta_lock);
+
+	tid_data = &priv->tid_data[sta_id][tid];
+	txq_id = tid_data->agg.txq_id;
+	agg_state = tid_data->agg.state;
+	IWL_DEBUG_TX_QUEUES(priv, "Flush AGG: sta %d tid %d q %d state %d\n",
+			    sta_id, tid, txq_id, tid_data->agg.state);
+
+	tid_data->agg.state = IWL_AGG_OFF;
+
+	spin_unlock_bh(&priv->sta_lock);
+
+	if (iwlagn_txfifo_flush(priv, BIT(txq_id)))
+		IWL_ERR(priv, "Couldn't flush the AGG queue\n");
+
+	if (test_bit(txq_id, priv->agg_q_alloc)) {
+		/*
+		 * If the transport didn't know that we wanted to start
+		 * agreggation, don't tell it that we want to stop them.
+		 * This can happen when we don't get the addBA response on
+		 * time, or we hadn't time to drain the AC queues.
+		 */
+		if (agg_state == IWL_AGG_ON)
+			iwl_trans_txq_disable(priv->trans, txq_id);
+		else
+			IWL_DEBUG_TX_QUEUES(priv, "Don't disable tx agg: %d\n",
+					    agg_state);
+		iwlagn_dealloc_agg_txq(priv, txq_id);
+	}
+
+	return 0;
+}
+
 int iwlagn_tx_agg_oper(struct iwl_priv *priv, struct ieee80211_vif *vif,
 			struct ieee80211_sta *sta, u16 tid, u8 buf_size)
 {
@@ -911,7 +957,7 @@ static void iwlagn_count_agg_tx_err_status(struct iwl_priv *priv, u16 status)
 static inline u32 iwlagn_get_scd_ssn(struct iwlagn_tx_resp *tx_resp)
 {
 	return le32_to_cpup((__le32 *)&tx_resp->status +
-			    tx_resp->frame_count) & MAX_SN;
+			    tx_resp->frame_count) & IEEE80211_MAX_SN;
 }
 
 static void iwl_rx_reply_tx_agg(struct iwl_priv *priv,
@@ -1148,7 +1194,7 @@ int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
 
 	if (tx_resp->frame_count == 1) {
 		u16 next_reclaimed = le16_to_cpu(tx_resp->seq_ctl);
-		next_reclaimed = SEQ_TO_SN(next_reclaimed + 0x10);
+		next_reclaimed = IEEE80211_SEQ_TO_SN(next_reclaimed + 0x10);
 
 		if (is_agg) {
 			/* If this is an aggregation queue, we can rely on the
diff --git a/drivers/net/wireless/iwlwifi/dvm/ucode.c b/drivers/net/wireless/iwlwifi/dvm/ucode.c
index 1a4ac9236a44..0a1cdc5e856b 100644
--- a/drivers/net/wireless/iwlwifi/dvm/ucode.c
+++ b/drivers/net/wireless/iwlwifi/dvm/ucode.c
@@ -19,7 +19,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/pcie/1000.c b/drivers/net/wireless/iwlwifi/iwl-1000.c
index ff3389757281..c080ae3070b2 100644
--- a/drivers/net/wireless/iwlwifi/pcie/1000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-1000.c
@@ -29,7 +29,6 @@
 #include "iwl-config.h"
 #include "iwl-csr.h"
 #include "iwl-agn-hw.h"
-#include "cfg.h"
 
 /* Highest firmware API version supported */
 #define IWL1000_UCODE_API_MAX 5
diff --git a/drivers/net/wireless/iwlwifi/pcie/2000.c b/drivers/net/wireless/iwlwifi/iwl-2000.c
index e7de33128b16..a6ddd2f9fba0 100644
--- a/drivers/net/wireless/iwlwifi/pcie/2000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-2000.c
@@ -28,7 +28,6 @@
 #include <linux/stringify.h>
 #include "iwl-config.h"
 #include "iwl-agn-hw.h"
-#include "cfg.h"
 #include "dvm/commands.h" /* needed for BT for now */
 
 /* Highest firmware API version supported */
diff --git a/drivers/net/wireless/iwlwifi/pcie/5000.c b/drivers/net/wireless/iwlwifi/iwl-5000.c
index 5096f7c96ab6..403f3f224bf6 100644
--- a/drivers/net/wireless/iwlwifi/pcie/5000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-5000.c
@@ -29,7 +29,6 @@
 #include "iwl-config.h"
 #include "iwl-agn-hw.h"
 #include "iwl-csr.h"
-#include "cfg.h"
 
 /* Highest firmware API version supported */
 #define IWL5000_UCODE_API_MAX 5
diff --git a/drivers/net/wireless/iwlwifi/pcie/6000.c b/drivers/net/wireless/iwlwifi/iwl-6000.c
index 801ff49796dd..b5ab8d1bcac0 100644
--- a/drivers/net/wireless/iwlwifi/pcie/6000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-6000.c
@@ -28,7 +28,6 @@
 #include <linux/stringify.h>
 #include "iwl-config.h"
 #include "iwl-agn-hw.h"
-#include "cfg.h"
 #include "dvm/commands.h" /* needed for BT for now */
 
 /* Highest firmware API version supported */
diff --git a/drivers/net/wireless/iwlwifi/pcie/7000.c b/drivers/net/wireless/iwlwifi/iwl-7000.c
index 6e35b2b72332..50263e87fe15 100644
--- a/drivers/net/wireless/iwlwifi/pcie/7000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-7000.c
@@ -1,34 +1,70 @@
 /******************************************************************************
  *
- * Copyright(c) 2008 - 2013 Intel Corporation. All rights reserved.
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
  *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2012 - 2013 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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.
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
  *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
+ * The full GNU General Public License is included in this distribution
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2012 - 2013 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
  *****************************************************************************/
 
 #include <linux/module.h>
 #include <linux/stringify.h>
 #include "iwl-config.h"
 #include "iwl-agn-hw.h"
-#include "cfg.h"
 
 /* Highest firmware API version supported */
 #define IWL7260_UCODE_API_MAX	6
@@ -70,7 +106,6 @@ static const struct iwl_base_params iwl7000_base_params = {
 };
 
 static const struct iwl_ht_params iwl7000_ht_params = {
-	.ht_greenfield_support = true,
 	.use_rts_for_aggregation = true, /* use rts/cts protection */
 	.ht40_bands = BIT(IEEE80211_BAND_2GHZ) | BIT(IEEE80211_BAND_5GHZ),
 };
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-hw.h b/drivers/net/wireless/iwlwifi/iwl-agn-hw.h
index e9975c54c276..6d73f943cefa 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-hw.h
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-hw.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/iwl-config.h b/drivers/net/wireless/iwlwifi/iwl-config.h
index 743b48343358..c38aa8f77554 100644
--- a/drivers/net/wireless/iwlwifi/iwl-config.h
+++ b/drivers/net/wireless/iwlwifi/iwl-config.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -275,4 +275,51 @@ struct iwl_cfg {
 	const bool temp_offset_v2;
 };
 
+/*
+ * This list declares the config structures for all devices.
+ */
+extern const struct iwl_cfg iwl5300_agn_cfg;
+extern const struct iwl_cfg iwl5100_agn_cfg;
+extern const struct iwl_cfg iwl5350_agn_cfg;
+extern const struct iwl_cfg iwl5100_bgn_cfg;
+extern const struct iwl_cfg iwl5100_abg_cfg;
+extern const struct iwl_cfg iwl5150_agn_cfg;
+extern const struct iwl_cfg iwl5150_abg_cfg;
+extern const struct iwl_cfg iwl6005_2agn_cfg;
+extern const struct iwl_cfg iwl6005_2abg_cfg;
+extern const struct iwl_cfg iwl6005_2bg_cfg;
+extern const struct iwl_cfg iwl6005_2agn_sff_cfg;
+extern const struct iwl_cfg iwl6005_2agn_d_cfg;
+extern const struct iwl_cfg iwl6005_2agn_mow1_cfg;
+extern const struct iwl_cfg iwl6005_2agn_mow2_cfg;
+extern const struct iwl_cfg iwl1030_bgn_cfg;
+extern const struct iwl_cfg iwl1030_bg_cfg;
+extern const struct iwl_cfg iwl6030_2agn_cfg;
+extern const struct iwl_cfg iwl6030_2abg_cfg;
+extern const struct iwl_cfg iwl6030_2bgn_cfg;
+extern const struct iwl_cfg iwl6030_2bg_cfg;
+extern const struct iwl_cfg iwl6000i_2agn_cfg;
+extern const struct iwl_cfg iwl6000i_2abg_cfg;
+extern const struct iwl_cfg iwl6000i_2bg_cfg;
+extern const struct iwl_cfg iwl6000_3agn_cfg;
+extern const struct iwl_cfg iwl6050_2agn_cfg;
+extern const struct iwl_cfg iwl6050_2abg_cfg;
+extern const struct iwl_cfg iwl6150_bgn_cfg;
+extern const struct iwl_cfg iwl6150_bg_cfg;
+extern const struct iwl_cfg iwl1000_bgn_cfg;
+extern const struct iwl_cfg iwl1000_bg_cfg;
+extern const struct iwl_cfg iwl100_bgn_cfg;
+extern const struct iwl_cfg iwl100_bg_cfg;
+extern const struct iwl_cfg iwl130_bgn_cfg;
+extern const struct iwl_cfg iwl130_bg_cfg;
+extern const struct iwl_cfg iwl2000_2bgn_cfg;
+extern const struct iwl_cfg iwl2000_2bgn_d_cfg;
+extern const struct iwl_cfg iwl2030_2bgn_cfg;
+extern const struct iwl_cfg iwl6035_2agn_cfg;
+extern const struct iwl_cfg iwl105_bgn_cfg;
+extern const struct iwl_cfg iwl105_bgn_d_cfg;
+extern const struct iwl_cfg iwl135_bgn_cfg;
+extern const struct iwl_cfg iwl7260_2ac_cfg;
+extern const struct iwl_cfg iwl3160_ac_cfg;
+
 #endif /* __IWL_CONFIG_H__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-csr.h b/drivers/net/wireless/iwlwifi/iwl-csr.h
index df3463a38704..20e845d4da04 100644
--- a/drivers/net/wireless/iwlwifi/iwl-csr.h
+++ b/drivers/net/wireless/iwlwifi/iwl-csr.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/iwl-debug.c b/drivers/net/wireless/iwlwifi/iwl-debug.c
index 87535a67de76..8a44f594528d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debug.c
+++ b/drivers/net/wireless/iwlwifi/iwl-debug.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -66,6 +66,7 @@
 #include <linux/device.h>
 #include <linux/interrupt.h>
 #include <linux/export.h>
+#include "iwl-drv.h"
 #include "iwl-debug.h"
 #include "iwl-devtrace.h"
 
@@ -85,11 +86,11 @@ void __iwl_ ##fn(struct device *dev, const char *fmt, ...)	\
 }
 
 __iwl_fn(warn)
-EXPORT_SYMBOL_GPL(__iwl_warn);
+IWL_EXPORT_SYMBOL(__iwl_warn);
 __iwl_fn(info)
-EXPORT_SYMBOL_GPL(__iwl_info);
+IWL_EXPORT_SYMBOL(__iwl_info);
 __iwl_fn(crit)
-EXPORT_SYMBOL_GPL(__iwl_crit);
+IWL_EXPORT_SYMBOL(__iwl_crit);
 
 void __iwl_err(struct device *dev, bool rfkill_prefix, bool trace_only,
 		const char *fmt, ...)
@@ -110,7 +111,7 @@ void __iwl_err(struct device *dev, bool rfkill_prefix, bool trace_only,
 	trace_iwlwifi_err(&vaf);
 	va_end(args);
 }
-EXPORT_SYMBOL_GPL(__iwl_err);
+IWL_EXPORT_SYMBOL(__iwl_err);
 
 #if defined(CONFIG_IWLWIFI_DEBUG) || defined(CONFIG_IWLWIFI_DEVICE_TRACING)
 void __iwl_dbg(struct device *dev,
@@ -133,5 +134,5 @@ void __iwl_dbg(struct device *dev,
 	trace_iwlwifi_dbg(level, in_interrupt(), function, &vaf);
 	va_end(args);
 }
-EXPORT_SYMBOL_GPL(__iwl_dbg);
+IWL_EXPORT_SYMBOL(__iwl_dbg);
 #endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace.h b/drivers/net/wireless/iwlwifi/iwl-devtrace.h
index 81aa91fab5aa..4491c1c72cc7 100644
--- a/drivers/net/wireless/iwlwifi/iwl-devtrace.h
+++ b/drivers/net/wireless/iwlwifi/iwl-devtrace.h
@@ -298,7 +298,7 @@ TRACE_EVENT(iwlwifi_dbg,
 				       MAX_MSG_LEN, vaf->fmt,
 				       *vaf->va) >= MAX_MSG_LEN);
 	),
-	TP_printk("%s", (char *)__get_dynamic_array(msg))
+	TP_printk("%s", __get_str(msg))
 );
 
 #undef TRACE_SYSTEM
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.c b/drivers/net/wireless/iwlwifi/iwl-drv.c
index fbfd2d137117..39aad9893e0b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.c
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -912,8 +912,6 @@ static void iwl_req_fw_callback(const struct firmware *ucode_raw, void *context)
 		}
 	}
 
-	IWL_INFO(drv, "loaded firmware version %s", drv->fw.fw_version);
-
 	/*
 	 * In mvm uCode there is no difference between data and instructions
 	 * sections.
@@ -970,6 +968,9 @@ static void iwl_req_fw_callback(const struct firmware *ucode_raw, void *context)
 	else
 		op = &iwlwifi_opmode_table[DVM_OP_MODE];
 
+	IWL_INFO(drv, "loaded firmware version %s op_mode %s\n",
+		 drv->fw.fw_version, op->name);
+
 	/* add this device to the list of devices using this op_mode */
 	list_add_tail(&drv->list, &op->drv);
 
@@ -997,8 +998,13 @@ static void iwl_req_fw_callback(const struct firmware *ucode_raw, void *context)
 	 * else from proceeding if the module fails to load
 	 * or hangs loading.
 	 */
-	if (load_module)
-		request_module("%s", op->name);
+	if (load_module) {
+		err = request_module("%s", op->name);
+		if (err)
+			IWL_ERR(drv,
+				"failed to load module %s (error %d), is dynamic loading enabled?\n",
+				op->name, err);
+	}
 	return;
 
  try_again:
@@ -1102,7 +1108,7 @@ void iwl_drv_stop(struct iwl_drv *drv)
 
 /* shared module parameters */
 struct iwl_mod_params iwlwifi_mod_params = {
-	.restart_fw = 1,
+	.restart_fw = true,
 	.plcp_check = true,
 	.bt_coex_active = true,
 	.power_level = IWL_POWER_INDEX_1,
@@ -1111,7 +1117,7 @@ struct iwl_mod_params iwlwifi_mod_params = {
 	.wd_disable = true,
 	/* the rest are 0 by default */
 };
-EXPORT_SYMBOL_GPL(iwlwifi_mod_params);
+IWL_EXPORT_SYMBOL(iwlwifi_mod_params);
 
 int iwl_opmode_register(const char *name, const struct iwl_op_mode_ops *ops)
 {
@@ -1135,7 +1141,7 @@ int iwl_opmode_register(const char *name, const struct iwl_op_mode_ops *ops)
 	mutex_unlock(&iwlwifi_opmode_table_mtx);
 	return -EIO;
 }
-EXPORT_SYMBOL_GPL(iwl_opmode_register);
+IWL_EXPORT_SYMBOL(iwl_opmode_register);
 
 void iwl_opmode_deregister(const char *name)
 {
@@ -1157,7 +1163,7 @@ void iwl_opmode_deregister(const char *name)
 	}
 	mutex_unlock(&iwlwifi_opmode_table_mtx);
 }
-EXPORT_SYMBOL_GPL(iwl_opmode_deregister);
+IWL_EXPORT_SYMBOL(iwl_opmode_deregister);
 
 static int __init iwl_drv_init(void)
 {
@@ -1207,8 +1213,8 @@ MODULE_PARM_DESC(11n_disable,
 module_param_named(amsdu_size_8K, iwlwifi_mod_params.amsdu_size_8K,
 		   int, S_IRUGO);
 MODULE_PARM_DESC(amsdu_size_8K, "enable 8K amsdu size (default 0)");
-module_param_named(fw_restart, iwlwifi_mod_params.restart_fw, int, S_IRUGO);
-MODULE_PARM_DESC(fw_restart, "restart firmware in case of error");
+module_param_named(fw_restart, iwlwifi_mod_params.restart_fw, bool, S_IRUGO);
+MODULE_PARM_DESC(fw_restart, "restart firmware in case of error (default true)");
 
 module_param_named(antenna_coupling, iwlwifi_mod_params.ant_coupling,
 		   int, S_IRUGO);
@@ -1266,7 +1272,3 @@ module_param_named(auto_agg, iwlwifi_mod_params.auto_agg,
 		bool, S_IRUGO);
 MODULE_PARM_DESC(auto_agg,
 		 "enable agg w/o check traffic load (default: enable)");
-
-module_param_named(5ghz_disable, iwlwifi_mod_params.disable_5ghz,
-		bool, S_IRUGO);
-MODULE_PARM_DESC(5ghz_disable, "disable 5GHz band (default: 0 [enabled])");
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.h b/drivers/net/wireless/iwlwifi/iwl-drv.h
index 594a5c71b272..7d1450916308 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.h
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -63,6 +63,8 @@
 #ifndef __iwl_drv_h__
 #define __iwl_drv_h__
 
+#include <linux/module.h>
+
 /* for all modules */
 #define DRV_NAME        "iwlwifi"
 #define IWLWIFI_VERSION "in-tree:"
@@ -123,4 +125,17 @@ struct iwl_drv *iwl_drv_start(struct iwl_trans *trans,
  */
 void iwl_drv_stop(struct iwl_drv *drv);
 
+/*
+ * exported symbol management
+ *
+ * The driver can be split into multiple modules, in which case some symbols
+ * must be exported for the sub-modules. However, if it's not split and
+ * everything is built-in, then we can avoid that.
+ */
+#ifdef CONFIG_IWLWIFI_OPMODE_MODULAR
+#define IWL_EXPORT_SYMBOL(sym)	EXPORT_SYMBOL_GPL(sym)
+#else
+#define IWL_EXPORT_SYMBOL(sym)
+#endif
+
 #endif /* __iwl_drv_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.c b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.c
index 034f2ff4f43d..600c9fdd7f71 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.c
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -62,6 +62,7 @@
 #include <linux/types.h>
 #include <linux/slab.h>
 #include <linux/export.h>
+#include "iwl-drv.h"
 #include "iwl-modparams.h"
 #include "iwl-eeprom-parse.h"
 
@@ -749,7 +750,7 @@ void iwl_init_ht_hw_capab(const struct iwl_cfg *cfg,
 	}
 
 	ht_info->ht_supported = true;
-	ht_info->cap = 0;
+	ht_info->cap = IEEE80211_HT_CAP_DSSSCCK40;
 
 	if (iwlwifi_mod_params.amsdu_size_8K)
 		ht_info->cap |= IEEE80211_HT_CAP_MAX_AMSDU;
@@ -909,7 +910,7 @@ iwl_parse_eeprom_data(struct device *dev, const struct iwl_cfg *cfg,
 	kfree(data);
 	return NULL;
 }
-EXPORT_SYMBOL_GPL(iwl_parse_eeprom_data);
+IWL_EXPORT_SYMBOL(iwl_parse_eeprom_data);
 
 /* helper functions */
 int iwl_nvm_check_version(struct iwl_nvm_data *data,
@@ -928,4 +929,4 @@ int iwl_nvm_check_version(struct iwl_nvm_data *data,
 		data->calib_version,  trans->cfg->nvm_calib_ver);
 	return -EINVAL;
 }
-EXPORT_SYMBOL_GPL(iwl_nvm_check_version);
+IWL_EXPORT_SYMBOL(iwl_nvm_check_version);
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h
index 683fe6a8c58f..37f115390b19 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c b/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c
index ef4806f27cf8..e5f2e362ab0b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -63,6 +63,7 @@
 #include <linux/slab.h>
 #include <linux/export.h>
 
+#include "iwl-drv.h"
 #include "iwl-debug.h"
 #include "iwl-eeprom-read.h"
 #include "iwl-io.h"
@@ -460,4 +461,4 @@ int iwl_read_eeprom(struct iwl_trans *trans, u8 **eeprom, size_t *eeprom_size)
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(iwl_read_eeprom);
+IWL_EXPORT_SYMBOL(iwl_read_eeprom);
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-read.h b/drivers/net/wireless/iwlwifi/iwl-eeprom-read.h
index b2588c5cbf93..8e941f8bd7d6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom-read.h
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-read.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/iwl-fh.h b/drivers/net/wireless/iwlwifi/iwl-fh.h
index f5592fb3b1ed..484d318245fb 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fh.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fh.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw-file.h b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
index 90873eca35f7..8b6c6fd95ed0 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw-file.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw.h b/drivers/net/wireless/iwlwifi/iwl-fw.h
index b545178e46e3..c4c446d41eb0 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -73,12 +73,14 @@
  *	treats good CRC threshold as a boolean
  * @IWL_UCODE_TLV_FLAGS_MFP: This uCode image supports MFP (802.11w).
  * @IWL_UCODE_TLV_FLAGS_P2P: This uCode image supports P2P.
+ * @IWL_UCODE_TLV_FLAGS_DW_BC_TABLE: The SCD byte count table is in DWORDS
  */
 enum iwl_ucode_tlv_flag {
 	IWL_UCODE_TLV_FLAGS_PAN		= BIT(0),
 	IWL_UCODE_TLV_FLAGS_NEWSCAN	= BIT(1),
 	IWL_UCODE_TLV_FLAGS_MFP		= BIT(2),
 	IWL_UCODE_TLV_FLAGS_P2P		= BIT(3),
+	IWL_UCODE_TLV_FLAGS_DW_BC_TABLE	= BIT(4),
 };
 
 /* The default calibrate table size if not specified by firmware file */
@@ -152,6 +154,19 @@ struct iwl_tlv_calib_ctrl {
 	__le32 event_trigger;
 } __packed;
 
+enum iwl_fw_phy_cfg {
+	FW_PHY_CFG_RADIO_TYPE_POS = 0,
+	FW_PHY_CFG_RADIO_TYPE = 0x3 << FW_PHY_CFG_RADIO_TYPE_POS,
+	FW_PHY_CFG_RADIO_STEP_POS = 2,
+	FW_PHY_CFG_RADIO_STEP = 0x3 << FW_PHY_CFG_RADIO_STEP_POS,
+	FW_PHY_CFG_RADIO_DASH_POS = 4,
+	FW_PHY_CFG_RADIO_DASH = 0x3 << FW_PHY_CFG_RADIO_DASH_POS,
+	FW_PHY_CFG_TX_CHAIN_POS = 16,
+	FW_PHY_CFG_TX_CHAIN = 0xf << FW_PHY_CFG_TX_CHAIN_POS,
+	FW_PHY_CFG_RX_CHAIN_POS = 20,
+	FW_PHY_CFG_RX_CHAIN = 0xf << FW_PHY_CFG_RX_CHAIN_POS,
+};
+
 /**
  * struct iwl_fw - variables associated with the firmware
  *
@@ -188,4 +203,16 @@ struct iwl_fw {
 	bool mvm_fw;
 };
 
+static inline u8 iwl_fw_valid_tx_ant(const struct iwl_fw *fw)
+{
+	return (fw->phy_config & FW_PHY_CFG_TX_CHAIN) >>
+		FW_PHY_CFG_TX_CHAIN_POS;
+}
+
+static inline u8 iwl_fw_valid_rx_ant(const struct iwl_fw *fw)
+{
+	return (fw->phy_config & FW_PHY_CFG_RX_CHAIN) >>
+		FW_PHY_CFG_RX_CHAIN_POS;
+}
+
 #endif  /* __iwl_fw_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-io.c b/drivers/net/wireless/iwlwifi/iwl-io.c
index 276410d82de4..305c81f2c2b4 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.c
+++ b/drivers/net/wireless/iwlwifi/iwl-io.c
@@ -29,6 +29,7 @@
 #include <linux/device.h>
 #include <linux/export.h>
 
+#include "iwl-drv.h"
 #include "iwl-io.h"
 #include "iwl-csr.h"
 #include "iwl-debug.h"
@@ -49,7 +50,7 @@ int iwl_poll_bit(struct iwl_trans *trans, u32 addr,
 
 	return -ETIMEDOUT;
 }
-EXPORT_SYMBOL_GPL(iwl_poll_bit);
+IWL_EXPORT_SYMBOL(iwl_poll_bit);
 
 u32 iwl_read_direct32(struct iwl_trans *trans, u32 reg)
 {
@@ -62,7 +63,7 @@ u32 iwl_read_direct32(struct iwl_trans *trans, u32 reg)
 
 	return value;
 }
-EXPORT_SYMBOL_GPL(iwl_read_direct32);
+IWL_EXPORT_SYMBOL(iwl_read_direct32);
 
 void iwl_write_direct32(struct iwl_trans *trans, u32 reg, u32 value)
 {
@@ -73,7 +74,7 @@ void iwl_write_direct32(struct iwl_trans *trans, u32 reg, u32 value)
 		iwl_trans_release_nic_access(trans, &flags);
 	}
 }
-EXPORT_SYMBOL_GPL(iwl_write_direct32);
+IWL_EXPORT_SYMBOL(iwl_write_direct32);
 
 int iwl_poll_direct_bit(struct iwl_trans *trans, u32 addr, u32 mask,
 			int timeout)
@@ -89,7 +90,7 @@ int iwl_poll_direct_bit(struct iwl_trans *trans, u32 addr, u32 mask,
 
 	return -ETIMEDOUT;
 }
-EXPORT_SYMBOL_GPL(iwl_poll_direct_bit);
+IWL_EXPORT_SYMBOL(iwl_poll_direct_bit);
 
 static inline u32 __iwl_read_prph(struct iwl_trans *trans, u32 ofs)
 {
@@ -115,7 +116,7 @@ u32 iwl_read_prph(struct iwl_trans *trans, u32 ofs)
 	}
 	return val;
 }
-EXPORT_SYMBOL_GPL(iwl_read_prph);
+IWL_EXPORT_SYMBOL(iwl_read_prph);
 
 void iwl_write_prph(struct iwl_trans *trans, u32 ofs, u32 val)
 {
@@ -126,7 +127,7 @@ void iwl_write_prph(struct iwl_trans *trans, u32 ofs, u32 val)
 		iwl_trans_release_nic_access(trans, &flags);
 	}
 }
-EXPORT_SYMBOL_GPL(iwl_write_prph);
+IWL_EXPORT_SYMBOL(iwl_write_prph);
 
 void iwl_set_bits_prph(struct iwl_trans *trans, u32 ofs, u32 mask)
 {
@@ -138,7 +139,7 @@ void iwl_set_bits_prph(struct iwl_trans *trans, u32 ofs, u32 mask)
 		iwl_trans_release_nic_access(trans, &flags);
 	}
 }
-EXPORT_SYMBOL_GPL(iwl_set_bits_prph);
+IWL_EXPORT_SYMBOL(iwl_set_bits_prph);
 
 void iwl_set_bits_mask_prph(struct iwl_trans *trans, u32 ofs,
 			    u32 bits, u32 mask)
@@ -151,7 +152,7 @@ void iwl_set_bits_mask_prph(struct iwl_trans *trans, u32 ofs,
 		iwl_trans_release_nic_access(trans, &flags);
 	}
 }
-EXPORT_SYMBOL_GPL(iwl_set_bits_mask_prph);
+IWL_EXPORT_SYMBOL(iwl_set_bits_mask_prph);
 
 void iwl_clear_bits_prph(struct iwl_trans *trans, u32 ofs, u32 mask)
 {
@@ -164,4 +165,4 @@ void iwl_clear_bits_prph(struct iwl_trans *trans, u32 ofs, u32 mask)
 		iwl_trans_release_nic_access(trans, &flags);
 	}
 }
-EXPORT_SYMBOL_GPL(iwl_clear_bits_prph);
+IWL_EXPORT_SYMBOL(iwl_clear_bits_prph);
diff --git a/drivers/net/wireless/iwlwifi/iwl-modparams.h b/drivers/net/wireless/iwlwifi/iwl-modparams.h
index 2c2a729092f5..d6f6c37c09fd 100644
--- a/drivers/net/wireless/iwlwifi/iwl-modparams.h
+++ b/drivers/net/wireless/iwlwifi/iwl-modparams.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -103,13 +103,12 @@ enum iwl_power_level {
  * @ant_coupling: antenna coupling in dB, default = 0
  * @bt_ch_announce: BT channel inhibition, default = enable
  * @auto_agg: enable agg. without check, default = true
- * @disable_5ghz: disable 5GHz capability, default = false
  */
 struct iwl_mod_params {
 	int sw_crypto;
 	unsigned int disable_11n;
 	int amsdu_size_8K;
-	int restart_fw;
+	bool restart_fw;
 	bool plcp_check;
 	int  wd_disable;
 	bool bt_coex_active;
@@ -120,7 +119,6 @@ struct iwl_mod_params {
 	int ant_coupling;
 	bool bt_ch_announce;
 	bool auto_agg;
-	bool disable_5ghz;
 };
 
 #endif /* #__iwl_modparams_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-notif-wait.c b/drivers/net/wireless/iwlwifi/iwl-notif-wait.c
index c3affbc62cdf..940b8a9d5285 100644
--- a/drivers/net/wireless/iwlwifi/iwl-notif-wait.c
+++ b/drivers/net/wireless/iwlwifi/iwl-notif-wait.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -63,6 +63,7 @@
 #include <linux/sched.h>
 #include <linux/export.h>
 
+#include "iwl-drv.h"
 #include "iwl-notif-wait.h"
 
 
@@ -72,7 +73,7 @@ void iwl_notification_wait_init(struct iwl_notif_wait_data *notif_wait)
 	INIT_LIST_HEAD(&notif_wait->notif_waits);
 	init_waitqueue_head(&notif_wait->notif_waitq);
 }
-EXPORT_SYMBOL_GPL(iwl_notification_wait_init);
+IWL_EXPORT_SYMBOL(iwl_notification_wait_init);
 
 void iwl_notification_wait_notify(struct iwl_notif_wait_data *notif_wait,
 				  struct iwl_rx_packet *pkt)
@@ -117,7 +118,7 @@ void iwl_notification_wait_notify(struct iwl_notif_wait_data *notif_wait,
 	if (triggered)
 		wake_up_all(&notif_wait->notif_waitq);
 }
-EXPORT_SYMBOL_GPL(iwl_notification_wait_notify);
+IWL_EXPORT_SYMBOL(iwl_notification_wait_notify);
 
 void iwl_abort_notification_waits(struct iwl_notif_wait_data *notif_wait)
 {
@@ -130,7 +131,7 @@ void iwl_abort_notification_waits(struct iwl_notif_wait_data *notif_wait)
 
 	wake_up_all(&notif_wait->notif_waitq);
 }
-EXPORT_SYMBOL_GPL(iwl_abort_notification_waits);
+IWL_EXPORT_SYMBOL(iwl_abort_notification_waits);
 
 void
 iwl_init_notification_wait(struct iwl_notif_wait_data *notif_wait,
@@ -154,7 +155,7 @@ iwl_init_notification_wait(struct iwl_notif_wait_data *notif_wait,
 	list_add(&wait_entry->list, &notif_wait->notif_waits);
 	spin_unlock_bh(&notif_wait->notif_wait_lock);
 }
-EXPORT_SYMBOL_GPL(iwl_init_notification_wait);
+IWL_EXPORT_SYMBOL(iwl_init_notification_wait);
 
 int iwl_wait_notification(struct iwl_notif_wait_data *notif_wait,
 			  struct iwl_notification_wait *wait_entry,
@@ -178,7 +179,7 @@ int iwl_wait_notification(struct iwl_notif_wait_data *notif_wait,
 		return -ETIMEDOUT;
 	return 0;
 }
-EXPORT_SYMBOL_GPL(iwl_wait_notification);
+IWL_EXPORT_SYMBOL(iwl_wait_notification);
 
 void iwl_remove_notification(struct iwl_notif_wait_data *notif_wait,
 			     struct iwl_notification_wait *wait_entry)
@@ -187,4 +188,4 @@ void iwl_remove_notification(struct iwl_notif_wait_data *notif_wait,
 	list_del(&wait_entry->list);
 	spin_unlock_bh(&notif_wait->notif_wait_lock);
 }
-EXPORT_SYMBOL_GPL(iwl_remove_notification);
+IWL_EXPORT_SYMBOL(iwl_remove_notification);
diff --git a/drivers/net/wireless/iwlwifi/iwl-notif-wait.h b/drivers/net/wireless/iwlwifi/iwl-notif-wait.h
index c2ce764463a3..2e2f1c8c99f9 100644
--- a/drivers/net/wireless/iwlwifi/iwl-notif-wait.h
+++ b/drivers/net/wireless/iwlwifi/iwl-notif-wait.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
index a70213bdb83c..6199a0a597a6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
+++ b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -62,6 +62,7 @@
 #include <linux/types.h>
 #include <linux/slab.h>
 #include <linux/export.h>
+#include "iwl-drv.h"
 #include "iwl-modparams.h"
 #include "iwl-nvm-parse.h"
 
@@ -149,6 +150,8 @@ static struct ieee80211_rate iwl_cfg80211_rates[] = {
  * @NVM_CHANNEL_DFS: dynamic freq selection candidate
  * @NVM_CHANNEL_WIDE: 20 MHz channel okay (?)
  * @NVM_CHANNEL_40MHZ: 40 MHz channel okay (?)
+ * @NVM_CHANNEL_80MHZ: 80 MHz channel okay (?)
+ * @NVM_CHANNEL_160MHZ: 160 MHz channel okay (?)
  */
 enum iwl_nvm_channel_flags {
 	NVM_CHANNEL_VALID = BIT(0),
@@ -158,6 +161,8 @@ enum iwl_nvm_channel_flags {
 	NVM_CHANNEL_DFS = BIT(7),
 	NVM_CHANNEL_WIDE = BIT(8),
 	NVM_CHANNEL_40MHZ = BIT(9),
+	NVM_CHANNEL_80MHZ = BIT(10),
+	NVM_CHANNEL_160MHZ = BIT(11),
 };
 
 #define CHECK_AND_PRINT_I(x)	\
@@ -210,6 +215,10 @@ static int iwl_init_channel_map(struct device *dev, const struct iwl_cfg *cfg,
 			else
 				channel->flags &= ~IEEE80211_CHAN_NO_HT40MINUS;
 		}
+		if (!(ch_flags & NVM_CHANNEL_80MHZ))
+			channel->flags |= IEEE80211_CHAN_NO_80MHZ;
+		if (!(ch_flags & NVM_CHANNEL_160MHZ))
+			channel->flags |= IEEE80211_CHAN_NO_160MHZ;
 
 		if (!(ch_flags & NVM_CHANNEL_IBSS))
 			channel->flags |= IEEE80211_CHAN_NO_IBSS;
@@ -245,6 +254,43 @@ static int iwl_init_channel_map(struct device *dev, const struct iwl_cfg *cfg,
 	return n_channels;
 }
 
+static void iwl_init_vht_hw_capab(const struct iwl_cfg *cfg,
+				  struct iwl_nvm_data *data,
+				  struct ieee80211_sta_vht_cap *vht_cap)
+{
+	/* For now, assume new devices with NVM are VHT capable */
+
+	vht_cap->vht_supported = true;
+
+	vht_cap->cap = IEEE80211_VHT_CAP_SHORT_GI_80 |
+		       IEEE80211_VHT_CAP_RXSTBC_1 |
+		       IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE |
+		       7 << IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_SHIFT;
+
+	if (iwlwifi_mod_params.amsdu_size_8K)
+		vht_cap->cap |= IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_7991;
+
+	vht_cap->vht_mcs.rx_mcs_map =
+		cpu_to_le16(IEEE80211_VHT_MCS_SUPPORT_0_9 << 0 |
+			    IEEE80211_VHT_MCS_SUPPORT_0_9 << 2 |
+			    IEEE80211_VHT_MCS_NOT_SUPPORTED << 4 |
+			    IEEE80211_VHT_MCS_NOT_SUPPORTED << 6 |
+			    IEEE80211_VHT_MCS_NOT_SUPPORTED << 8 |
+			    IEEE80211_VHT_MCS_NOT_SUPPORTED << 10 |
+			    IEEE80211_VHT_MCS_NOT_SUPPORTED << 12 |
+			    IEEE80211_VHT_MCS_NOT_SUPPORTED << 14);
+
+	if (data->valid_rx_ant == 1 || cfg->rx_with_siso_diversity) {
+		vht_cap->cap |= IEEE80211_VHT_CAP_RX_ANTENNA_PATTERN |
+				IEEE80211_VHT_CAP_TX_ANTENNA_PATTERN;
+		/* this works because NOT_SUPPORTED == 3 */
+		vht_cap->vht_mcs.rx_mcs_map |=
+			cpu_to_le16(IEEE80211_VHT_MCS_NOT_SUPPORTED << 2);
+	}
+
+	vht_cap->vht_mcs.tx_mcs_map = vht_cap->vht_mcs.rx_mcs_map;
+}
+
 static void iwl_init_sbands(struct device *dev, const struct iwl_cfg *cfg,
 			    struct iwl_nvm_data *data, const __le16 *nvm_sw)
 {
@@ -268,6 +314,7 @@ static void iwl_init_sbands(struct device *dev, const struct iwl_cfg *cfg,
 	n_used += iwl_init_sband_channels(data, sband, n_channels,
 					  IEEE80211_BAND_5GHZ);
 	iwl_init_ht_hw_capab(cfg, data, &sband->ht_cap, IEEE80211_BAND_5GHZ);
+	iwl_init_vht_hw_capab(cfg, data, &sband->vht_cap);
 
 	if (n_channels != n_used)
 		IWL_ERR_DEV(dev, "NVM: used only %d of %d channels\n",
@@ -343,4 +390,4 @@ iwl_parse_nvm_data(struct device *dev, const struct iwl_cfg *cfg,
 
 	return data;
 }
-EXPORT_SYMBOL_GPL(iwl_parse_nvm_data);
+IWL_EXPORT_SYMBOL(iwl_parse_nvm_data);
diff --git a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.h b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.h
index b2692bd287fa..e57fb989661e 100644
--- a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.h
+++ b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/iwl-op-mode.h b/drivers/net/wireless/iwlwifi/iwl-op-mode.h
index 4a680019e117..98c7aa7346da 100644
--- a/drivers/net/wireless/iwlwifi/iwl-op-mode.h
+++ b/drivers/net/wireless/iwlwifi/iwl-op-mode.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/iwl-phy-db.c b/drivers/net/wireless/iwlwifi/iwl-phy-db.c
index 3392011a8768..25745daa0d5d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-phy-db.c
+++ b/drivers/net/wireless/iwlwifi/iwl-phy-db.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -65,6 +65,7 @@
 #include <linux/string.h>
 #include <linux/export.h>
 
+#include "iwl-drv.h"
 #include "iwl-phy-db.h"
 #include "iwl-debug.h"
 #include "iwl-op-mode.h"
@@ -149,7 +150,7 @@ struct iwl_phy_db *iwl_phy_db_init(struct iwl_trans *trans)
 	/* TODO: add default values of the phy db. */
 	return phy_db;
 }
-EXPORT_SYMBOL(iwl_phy_db_init);
+IWL_EXPORT_SYMBOL(iwl_phy_db_init);
 
 /*
  * get phy db section: returns a pointer to a phy db section specified by
@@ -215,7 +216,7 @@ void iwl_phy_db_free(struct iwl_phy_db *phy_db)
 
 	kfree(phy_db);
 }
-EXPORT_SYMBOL(iwl_phy_db_free);
+IWL_EXPORT_SYMBOL(iwl_phy_db_free);
 
 int iwl_phy_db_set_section(struct iwl_phy_db *phy_db, struct iwl_rx_packet *pkt,
 			   gfp_t alloc_ctx)
@@ -260,7 +261,7 @@ int iwl_phy_db_set_section(struct iwl_phy_db *phy_db, struct iwl_rx_packet *pkt,
 
 	return 0;
 }
-EXPORT_SYMBOL(iwl_phy_db_set_section);
+IWL_EXPORT_SYMBOL(iwl_phy_db_set_section);
 
 static int is_valid_channel(u16 ch_id)
 {
@@ -495,4 +496,4 @@ int iwl_send_phy_db_data(struct iwl_phy_db *phy_db)
 		       "Finished sending phy db non channel data\n");
 	return 0;
 }
-EXPORT_SYMBOL(iwl_send_phy_db_data);
+IWL_EXPORT_SYMBOL(iwl_send_phy_db_data);
diff --git a/drivers/net/wireless/iwlwifi/iwl-phy-db.h b/drivers/net/wireless/iwlwifi/iwl-phy-db.h
index d0e43d96ab38..ce983af79644 100644
--- a/drivers/net/wireless/iwlwifi/iwl-phy-db.h
+++ b/drivers/net/wireless/iwlwifi/iwl-phy-db.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/iwl-prph.h b/drivers/net/wireless/iwlwifi/iwl-prph.h
index f76e9cad7757..386f2a7c87cb 100644
--- a/drivers/net/wireless/iwlwifi/iwl-prph.h
+++ b/drivers/net/wireless/iwlwifi/iwl-prph.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/iwl-test.c b/drivers/net/wireless/iwlwifi/iwl-test.c
index ce0c67b425ee..5cfd55b86ed3 100644
--- a/drivers/net/wireless/iwlwifi/iwl-test.c
+++ b/drivers/net/wireless/iwlwifi/iwl-test.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -64,6 +64,7 @@
 #include <linux/export.h>
 #include <net/netlink.h>
 
+#include "iwl-drv.h"
 #include "iwl-io.h"
 #include "iwl-fh.h"
 #include "iwl-prph.h"
@@ -271,7 +272,7 @@ static int iwl_test_fw_cmd(struct iwl_test *tst, struct nlattr **tb)
 
 	reply_len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
 	skb = iwl_test_alloc_reply(tst, reply_len + 20);
-	reply_buf = kmalloc(reply_len, GFP_KERNEL);
+	reply_buf = kmemdup(&pkt->hdr, reply_len, GFP_KERNEL);
 	if (!skb || !reply_buf) {
 		kfree_skb(skb);
 		kfree(reply_buf);
@@ -279,7 +280,6 @@ static int iwl_test_fw_cmd(struct iwl_test *tst, struct nlattr **tb)
 	}
 
 	/* The reply is in a page, that we cannot send to user space. */
-	memcpy(reply_buf, &(pkt->hdr), reply_len);
 	iwl_free_resp(&cmd);
 
 	if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND,
@@ -653,7 +653,7 @@ int iwl_test_parse(struct iwl_test *tst, struct nlattr **tb,
 	}
 	return 0;
 }
-EXPORT_SYMBOL_GPL(iwl_test_parse);
+IWL_EXPORT_SYMBOL(iwl_test_parse);
 
 /*
  * Handle test commands.
@@ -715,7 +715,7 @@ int iwl_test_handle_cmd(struct iwl_test *tst, struct nlattr **tb)
 	}
 	return result;
 }
-EXPORT_SYMBOL_GPL(iwl_test_handle_cmd);
+IWL_EXPORT_SYMBOL(iwl_test_handle_cmd);
 
 static int iwl_test_trace_dump(struct iwl_test *tst, struct sk_buff *skb,
 			       struct netlink_callback *cb)
@@ -803,7 +803,7 @@ int iwl_test_dump(struct iwl_test *tst, u32 cmd, struct sk_buff *skb,
 	}
 	return result;
 }
-EXPORT_SYMBOL_GPL(iwl_test_dump);
+IWL_EXPORT_SYMBOL(iwl_test_dump);
 
 /*
  * Multicast a spontaneous messages from the device to the user space.
@@ -849,4 +849,4 @@ void iwl_test_rx(struct iwl_test *tst, struct iwl_rx_cmd_buffer *rxb)
 	if (tst->notify)
 		iwl_test_send_rx(tst, rxb);
 }
-EXPORT_SYMBOL_GPL(iwl_test_rx);
+IWL_EXPORT_SYMBOL(iwl_test_rx);
diff --git a/drivers/net/wireless/iwlwifi/iwl-test.h b/drivers/net/wireless/iwlwifi/iwl-test.h
index 7fbf4d717caa..8fbd21704840 100644
--- a/drivers/net/wireless/iwlwifi/iwl-test.h
+++ b/drivers/net/wireless/iwlwifi/iwl-test.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/iwl-testmode.h b/drivers/net/wireless/iwlwifi/iwl-testmode.h
index a963f45c6849..98f48a9afc98 100644
--- a/drivers/net/wireless/iwlwifi/iwl-testmode.h
+++ b/drivers/net/wireless/iwlwifi/iwl-testmode.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans.h b/drivers/net/wireless/iwlwifi/iwl-trans.h
index 0cac2b7af78b..7a13790b5bfe 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans.h
+++ b/drivers/net/wireless/iwlwifi/iwl-trans.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -114,9 +114,6 @@
  * completely agnostic to these differences.
  * The transport does provide helper functionnality (i.e. SYNC / ASYNC mode),
  */
-#define SEQ_TO_SN(seq) (((seq) & IEEE80211_SCTL_SEQ) >> 4)
-#define SN_TO_SEQ(ssn) (((ssn) << 4) & IEEE80211_SCTL_SEQ)
-#define MAX_SN ((IEEE80211_SCTL_SEQ) >> 4)
 #define SEQ_TO_QUEUE(s)	(((s) >> 8) & 0x1f)
 #define QUEUE_TO_SEQ(q)	(((q) & 0x1f) << 8)
 #define SEQ_TO_INDEX(s)	((s) & 0xff)
@@ -308,7 +305,6 @@ static inline void iwl_free_rxb(struct iwl_rx_cmd_buffer *r)
  * currently supports
  */
 #define IWL_MAX_HW_QUEUES		32
-#define IWL_INVALID_STATION	255
 #define IWL_MAX_TID_COUNT	8
 #define IWL_FRAME_LIMIT	64
 
@@ -685,7 +681,7 @@ static inline void iwl_trans_txq_enable(struct iwl_trans *trans, int queue,
 static inline void iwl_trans_ac_txq_enable(struct iwl_trans *trans, int queue,
 					   int fifo)
 {
-	iwl_trans_txq_enable(trans, queue, fifo, IWL_INVALID_STATION,
+	iwl_trans_txq_enable(trans, queue, fifo, -1,
 			     IWL_MAX_TID_COUNT, IWL_FRAME_LIMIT, 0);
 }
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/Makefile b/drivers/net/wireless/iwlwifi/mvm/Makefile
index 807b250ec396..2acc44b40986 100644
--- a/drivers/net/wireless/iwlwifi/mvm/Makefile
+++ b/drivers/net/wireless/iwlwifi/mvm/Makefile
@@ -2,7 +2,7 @@ obj-$(CONFIG_IWLMVM)   += iwlmvm.o
 iwlmvm-y += fw.o mac80211.o nvm.o ops.o phy-ctxt.o mac-ctxt.o
 iwlmvm-y += utils.o rx.o tx.o binding.o quota.o sta.o
 iwlmvm-y += scan.o time-event.o rs.o
-iwlmvm-y += power.o
+iwlmvm-y += power.o bt-coex.o
 iwlmvm-y += led.o
 iwlmvm-$(CONFIG_IWLWIFI_DEBUGFS) += debugfs.o
 iwlmvm-$(CONFIG_PM_SLEEP) += d3.o
diff --git a/drivers/net/wireless/iwlwifi/mvm/binding.c b/drivers/net/wireless/iwlwifi/mvm/binding.c
index 73d24aacb90a..93fd1457954b 100644
--- a/drivers/net/wireless/iwlwifi/mvm/binding.c
+++ b/drivers/net/wireless/iwlwifi/mvm/binding.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/mvm/bt-coex.c b/drivers/net/wireless/iwlwifi/mvm/bt-coex.c
new file mode 100644
index 000000000000..810bfa5f6de0
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/mvm/bt-coex.c
@@ -0,0 +1,589 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2013 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called COPYING.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2013 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#include <net/mac80211.h>
+
+#include "fw-api-bt-coex.h"
+#include "iwl-modparams.h"
+#include "mvm.h"
+#include "iwl-debug.h"
+
+#define EVENT_PRIO_ANT(_evt, _prio, _shrd_ant)			\
+	[(_evt)] = (((_prio) << BT_COEX_PRIO_TBL_PRIO_POS) |	\
+		   ((_shrd_ant) << BT_COEX_PRIO_TBL_SHRD_ANT_POS))
+
+static const u8 iwl_bt_prio_tbl[BT_COEX_PRIO_TBL_EVT_MAX] = {
+	EVENT_PRIO_ANT(BT_COEX_PRIO_TBL_EVT_INIT_CALIB1,
+		       BT_COEX_PRIO_TBL_PRIO_BYPASS, 0),
+	EVENT_PRIO_ANT(BT_COEX_PRIO_TBL_EVT_INIT_CALIB2,
+		       BT_COEX_PRIO_TBL_PRIO_BYPASS, 1),
+	EVENT_PRIO_ANT(BT_COEX_PRIO_TBL_EVT_PERIODIC_CALIB_LOW1,
+		       BT_COEX_PRIO_TBL_PRIO_LOW, 0),
+	EVENT_PRIO_ANT(BT_COEX_PRIO_TBL_EVT_PERIODIC_CALIB_LOW2,
+		       BT_COEX_PRIO_TBL_PRIO_LOW, 1),
+	EVENT_PRIO_ANT(BT_COEX_PRIO_TBL_EVT_PERIODIC_CALIB_HIGH1,
+		       BT_COEX_PRIO_TBL_PRIO_HIGH, 0),
+	EVENT_PRIO_ANT(BT_COEX_PRIO_TBL_EVT_PERIODIC_CALIB_HIGH2,
+		       BT_COEX_PRIO_TBL_PRIO_HIGH, 1),
+	EVENT_PRIO_ANT(BT_COEX_PRIO_TBL_EVT_DTIM,
+		       BT_COEX_PRIO_TBL_DISABLED, 0),
+	EVENT_PRIO_ANT(BT_COEX_PRIO_TBL_EVT_SCAN52,
+		       BT_COEX_PRIO_TBL_PRIO_COEX_OFF, 0),
+	EVENT_PRIO_ANT(BT_COEX_PRIO_TBL_EVT_SCAN24,
+		       BT_COEX_PRIO_TBL_PRIO_COEX_ON, 0),
+	EVENT_PRIO_ANT(BT_COEX_PRIO_TBL_EVT_IDLE,
+		       BT_COEX_PRIO_TBL_PRIO_COEX_IDLE, 0),
+	0, 0, 0, 0, 0, 0,
+};
+
+#undef EVENT_PRIO_ANT
+
+/* BT Antenna Coupling Threshold (dB) */
+#define IWL_BT_ANTENNA_COUPLING_THRESHOLD	(35)
+#define IWL_BT_LOAD_FORCE_SISO_THRESHOLD	(3)
+
+#define BT_ENABLE_REDUCED_TXPOWER_THRESHOLD	(-62)
+#define BT_DISABLE_REDUCED_TXPOWER_THRESHOLD	(-65)
+#define BT_REDUCED_TX_POWER_BIT			BIT(7)
+
+static inline bool is_loose_coex(void)
+{
+	return iwlwifi_mod_params.ant_coupling >
+		IWL_BT_ANTENNA_COUPLING_THRESHOLD;
+}
+
+int iwl_send_bt_prio_tbl(struct iwl_mvm *mvm)
+{
+	return iwl_mvm_send_cmd_pdu(mvm, BT_COEX_PRIO_TABLE, CMD_SYNC,
+				    sizeof(struct iwl_bt_coex_prio_tbl_cmd),
+				    &iwl_bt_prio_tbl);
+}
+
+static int iwl_send_bt_env(struct iwl_mvm *mvm, u8 action, u8 type)
+{
+	struct iwl_bt_coex_prot_env_cmd env_cmd;
+	int ret;
+
+	env_cmd.action = action;
+	env_cmd.type = type;
+	ret = iwl_mvm_send_cmd_pdu(mvm, BT_COEX_PROT_ENV, CMD_SYNC,
+				   sizeof(env_cmd), &env_cmd);
+	if (ret)
+		IWL_ERR(mvm, "failed to send BT env command\n");
+	return ret;
+}
+
+enum iwl_bt_kill_msk {
+	BT_KILL_MSK_DEFAULT,
+	BT_KILL_MSK_SCO_HID_A2DP,
+	BT_KILL_MSK_REDUCED_TXPOW,
+	BT_KILL_MSK_MAX,
+};
+
+static const u32 iwl_bt_ack_kill_msk[BT_KILL_MSK_MAX] = {
+	[BT_KILL_MSK_DEFAULT] = 0xffff0000,
+	[BT_KILL_MSK_SCO_HID_A2DP] = 0xffffffff,
+	[BT_KILL_MSK_REDUCED_TXPOW] = 0,
+};
+
+static const u32 iwl_bt_cts_kill_msk[BT_KILL_MSK_MAX] = {
+	[BT_KILL_MSK_DEFAULT] = 0xffff0000,
+	[BT_KILL_MSK_SCO_HID_A2DP] = 0xffffffff,
+	[BT_KILL_MSK_REDUCED_TXPOW] = 0,
+};
+
+#define IWL_BT_DEFAULT_BOOST (0xf0f0f0f0)
+
+/* Tight Coex */
+static const __le32 iwl_tight_lookup[BT_COEX_LUT_SIZE] = {
+	cpu_to_le32(0xaaaaaaaa),
+	cpu_to_le32(0xaaaaaaaa),
+	cpu_to_le32(0xaeaaaaaa),
+	cpu_to_le32(0xaaaaaaaa),
+	cpu_to_le32(0xcc00ff28),
+	cpu_to_le32(0x0000aaaa),
+	cpu_to_le32(0xcc00aaaa),
+	cpu_to_le32(0x0000aaaa),
+	cpu_to_le32(0xc0004000),
+	cpu_to_le32(0x00000000),
+	cpu_to_le32(0xf0005000),
+	cpu_to_le32(0xf0005000),
+};
+
+/* Loose Coex */
+static const __le32 iwl_loose_lookup[BT_COEX_LUT_SIZE] = {
+	cpu_to_le32(0xaaaaaaaa),
+	cpu_to_le32(0xaaaaaaaa),
+	cpu_to_le32(0xaeaaaaaa),
+	cpu_to_le32(0xaaaaaaaa),
+	cpu_to_le32(0xcc00ff28),
+	cpu_to_le32(0x0000aaaa),
+	cpu_to_le32(0xcc00aaaa),
+	cpu_to_le32(0x0000aaaa),
+	cpu_to_le32(0x00000000),
+	cpu_to_le32(0x00000000),
+	cpu_to_le32(0xf0005000),
+	cpu_to_le32(0xf0005000),
+};
+
+/* Full concurrency */
+static const __le32 iwl_concurrent_lookup[BT_COEX_LUT_SIZE] = {
+	cpu_to_le32(0xaaaaaaaa),
+	cpu_to_le32(0xaaaaaaaa),
+	cpu_to_le32(0xaaaaaaaa),
+	cpu_to_le32(0xaaaaaaaa),
+	cpu_to_le32(0xaaaaaaaa),
+	cpu_to_le32(0xaaaaaaaa),
+	cpu_to_le32(0xaaaaaaaa),
+	cpu_to_le32(0xaaaaaaaa),
+	cpu_to_le32(0x00000000),
+	cpu_to_le32(0x00000000),
+	cpu_to_le32(0x00000000),
+	cpu_to_le32(0x00000000),
+};
+
+int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
+{
+	struct iwl_bt_coex_cmd cmd = {
+		.max_kill = 5,
+		.bt3_time_t7_value = 1,
+		.bt3_prio_sample_time = 2,
+		.bt3_timer_t2_value = 0xc,
+	};
+	int ret;
+
+	cmd.flags = iwlwifi_mod_params.bt_coex_active ?
+			BT_COEX_NW : BT_COEX_DISABLE;
+	cmd.flags |= BT_CH_PRIMARY_EN | BT_SYNC_2_BT_DISABLE;
+
+	cmd.valid_bit_msk = cpu_to_le16(BT_VALID_ENABLE |
+					BT_VALID_BT_PRIO_BOOST |
+					BT_VALID_MAX_KILL |
+					BT_VALID_3W_TMRS |
+					BT_VALID_KILL_ACK |
+					BT_VALID_KILL_CTS |
+					BT_VALID_REDUCED_TX_POWER |
+					BT_VALID_LUT);
+
+	if (is_loose_coex())
+		memcpy(&cmd.decision_lut, iwl_loose_lookup,
+		       sizeof(iwl_tight_lookup));
+	else
+		memcpy(&cmd.decision_lut, iwl_tight_lookup,
+		       sizeof(iwl_tight_lookup));
+
+	cmd.bt_prio_boost = cpu_to_le32(IWL_BT_DEFAULT_BOOST);
+	cmd.kill_ack_msk =
+		cpu_to_le32(iwl_bt_ack_kill_msk[BT_KILL_MSK_DEFAULT]);
+	cmd.kill_cts_msk =
+		cpu_to_le32(iwl_bt_cts_kill_msk[BT_KILL_MSK_DEFAULT]);
+
+	memset(&mvm->last_bt_notif, 0, sizeof(mvm->last_bt_notif));
+
+	/* go to CALIB state in internal BT-Coex state machine */
+	ret = iwl_send_bt_env(mvm, BT_COEX_ENV_OPEN,
+			      BT_COEX_PRIO_TBL_EVT_INIT_CALIB2);
+	if (ret)
+		return ret;
+
+	ret  = iwl_send_bt_env(mvm, BT_COEX_ENV_CLOSE,
+			       BT_COEX_PRIO_TBL_EVT_INIT_CALIB2);
+	if (ret)
+		return ret;
+
+	return iwl_mvm_send_cmd_pdu(mvm, BT_CONFIG, CMD_SYNC,
+				    sizeof(cmd), &cmd);
+}
+
+static int iwl_mvm_bt_udpate_ctrl_kill_msk(struct iwl_mvm *mvm,
+					   bool reduced_tx_power)
+{
+	enum iwl_bt_kill_msk bt_kill_msk;
+	struct iwl_bt_coex_cmd cmd = {};
+	struct iwl_bt_coex_profile_notif *notif = &mvm->last_bt_notif;
+
+	lockdep_assert_held(&mvm->mutex);
+
+	if (reduced_tx_power) {
+		/* Reduced Tx power has precedence on the type of the profile */
+		bt_kill_msk = BT_KILL_MSK_REDUCED_TXPOW;
+	} else {
+		/* Low latency BT profile is active: give higher prio to BT */
+		if (BT_MBOX_MSG(notif, 3, SCO_STATE)  ||
+		    BT_MBOX_MSG(notif, 3, A2DP_STATE) ||
+		    BT_MBOX_MSG(notif, 3, SNIFF_STATE))
+			bt_kill_msk = BT_KILL_MSK_SCO_HID_A2DP;
+		else
+			bt_kill_msk = BT_KILL_MSK_DEFAULT;
+	}
+
+	IWL_DEBUG_COEX(mvm,
+		       "Update kill_msk: %d - SCO %sactive A2DP %sactive SNIFF %sactive\n",
+		       bt_kill_msk,
+		       BT_MBOX_MSG(notif, 3, SCO_STATE) ? "" : "in",
+		       BT_MBOX_MSG(notif, 3, A2DP_STATE) ? "" : "in",
+		       BT_MBOX_MSG(notif, 3, SNIFF_STATE) ? "" : "in");
+
+	/* Don't send HCMD if there is no update */
+	if (bt_kill_msk == mvm->bt_kill_msk)
+		return 0;
+
+	mvm->bt_kill_msk = bt_kill_msk;
+	cmd.kill_ack_msk = cpu_to_le32(iwl_bt_ack_kill_msk[bt_kill_msk]);
+	cmd.kill_cts_msk = cpu_to_le32(iwl_bt_cts_kill_msk[bt_kill_msk]);
+	cmd.valid_bit_msk = cpu_to_le16(BT_VALID_KILL_ACK | BT_VALID_KILL_CTS);
+
+	IWL_DEBUG_COEX(mvm, "bt_kill_msk = %d\n", bt_kill_msk);
+	return iwl_mvm_send_cmd_pdu(mvm, BT_CONFIG, CMD_SYNC,
+				    sizeof(cmd), &cmd);
+}
+
+static int iwl_mvm_bt_coex_reduced_txp(struct iwl_mvm *mvm, u8 sta_id,
+				       bool enable)
+{
+	struct iwl_bt_coex_cmd cmd = {
+		.valid_bit_msk = cpu_to_le16(BT_VALID_REDUCED_TX_POWER),
+		.bt_reduced_tx_power = sta_id,
+	};
+	struct ieee80211_sta *sta;
+	struct iwl_mvm_sta *mvmsta;
+
+	/* This can happen if the station has been removed right now */
+	if (sta_id == IWL_MVM_STATION_COUNT)
+		return 0;
+
+	sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
+					lockdep_is_held(&mvm->mutex));
+	mvmsta = (void *)sta->drv_priv;
+
+	/* nothing to do */
+	if (mvmsta->bt_reduced_txpower == enable)
+		return 0;
+
+	if (enable)
+		cmd.bt_reduced_tx_power |= BT_REDUCED_TX_POWER_BIT;
+
+	IWL_DEBUG_COEX(mvm, "%sable reduced Tx Power for sta %d\n",
+		       enable ? "en" : "dis", sta_id);
+
+	mvmsta->bt_reduced_txpower = enable;
+
+	/* Send ASYNC since this can be sent from an atomic context */
+	return iwl_mvm_send_cmd_pdu(mvm, BT_CONFIG, CMD_ASYNC,
+				    sizeof(cmd), &cmd);
+}
+
+struct iwl_bt_iterator_data {
+	struct iwl_bt_coex_profile_notif *notif;
+	struct iwl_mvm *mvm;
+	u32 num_bss_ifaces;
+	bool reduced_tx_power;
+};
+
+static void iwl_mvm_bt_notif_iterator(void *_data, u8 *mac,
+				      struct ieee80211_vif *vif)
+{
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	struct iwl_bt_iterator_data *data = _data;
+	struct iwl_mvm *mvm = data->mvm;
+	struct ieee80211_chanctx_conf *chanctx_conf;
+	enum ieee80211_smps_mode smps_mode;
+	enum ieee80211_band band;
+	int ave_rssi;
+
+	if (vif->type != NL80211_IFTYPE_STATION)
+		return;
+
+	rcu_read_lock();
+	chanctx_conf = rcu_dereference(vif->chanctx_conf);
+	if (chanctx_conf && chanctx_conf->def.chan)
+		band = chanctx_conf->def.chan->band;
+	else
+		band = -1;
+	rcu_read_unlock();
+
+	smps_mode = IEEE80211_SMPS_AUTOMATIC;
+
+	if (band != IEEE80211_BAND_2GHZ) {
+		ieee80211_request_smps(vif, smps_mode);
+		return;
+	}
+
+	if (data->notif->bt_status)
+		smps_mode = IEEE80211_SMPS_DYNAMIC;
+
+	if (data->notif->bt_traffic_load >= IWL_BT_LOAD_FORCE_SISO_THRESHOLD)
+		smps_mode = IEEE80211_SMPS_STATIC;
+
+	IWL_DEBUG_COEX(data->mvm,
+		       "mac %d: bt_status %d traffic_load %d smps_req %d\n",
+		       mvmvif->id,  data->notif->bt_status,
+		       data->notif->bt_traffic_load, smps_mode);
+
+	ieee80211_request_smps(vif, smps_mode);
+
+	/* don't reduce the Tx power if in loose scheme */
+	if (is_loose_coex())
+		return;
+
+	data->num_bss_ifaces++;
+
+	/* reduced Txpower only if there are open BT connections, so ...*/
+	if (!BT_MBOX_MSG(data->notif, 3, OPEN_CON_2)) {
+		/* ... cancel reduced Tx power ... */
+		if (iwl_mvm_bt_coex_reduced_txp(mvm, mvmvif->ap_sta_id, false))
+			IWL_ERR(mvm, "Couldn't send BT_CONFIG cmd\n");
+		data->reduced_tx_power = false;
+
+		/* ... and there is no need to get reports on RSSI any more. */
+		ieee80211_disable_rssi_reports(vif);
+		return;
+	}
+
+	ave_rssi = ieee80211_ave_rssi(vif);
+
+	/* if the RSSI isn't valid, fake it is very low */
+	if (!ave_rssi)
+		ave_rssi = -100;
+	if (ave_rssi > BT_ENABLE_REDUCED_TXPOWER_THRESHOLD) {
+		if (iwl_mvm_bt_coex_reduced_txp(mvm, mvmvif->ap_sta_id, true))
+			IWL_ERR(mvm, "Couldn't send BT_CONFIG cmd\n");
+
+		/*
+		 * bt_kill_msk can be BT_KILL_MSK_REDUCED_TXPOW only if all the
+		 * BSS / P2P clients have rssi above threshold.
+		 * We set the bt_kill_msk to BT_KILL_MSK_REDUCED_TXPOW before
+		 * the iteration, if one interface's rssi isn't good enough,
+		 * bt_kill_msk will be set to default values.
+		 */
+	} else if (ave_rssi < BT_DISABLE_REDUCED_TXPOWER_THRESHOLD) {
+		if (iwl_mvm_bt_coex_reduced_txp(mvm, mvmvif->ap_sta_id, false))
+			IWL_ERR(mvm, "Couldn't send BT_CONFIG cmd\n");
+
+		/*
+		 * One interface hasn't rssi above threshold, bt_kill_msk must
+		 * be set to default values.
+		 */
+		data->reduced_tx_power = false;
+	}
+
+	/* Begin to monitor the RSSI: it may influence the reduced Tx power */
+	ieee80211_enable_rssi_reports(vif, BT_DISABLE_REDUCED_TXPOWER_THRESHOLD,
+				      BT_ENABLE_REDUCED_TXPOWER_THRESHOLD);
+}
+
+static void iwl_mvm_bt_coex_notif_handle(struct iwl_mvm *mvm)
+{
+	struct iwl_bt_iterator_data data = {
+		.mvm = mvm,
+		.notif = &mvm->last_bt_notif,
+		.reduced_tx_power = true,
+	};
+
+	ieee80211_iterate_active_interfaces_atomic(
+					mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
+					iwl_mvm_bt_notif_iterator, &data);
+
+	/*
+	 * If there are no BSS / P2P client interfaces, reduced Tx Power is
+	 * irrelevant since it is based on the RSSI coming from the beacon.
+	 * Use BT_KILL_MSK_DEFAULT in that case.
+	 */
+	data.reduced_tx_power = data.reduced_tx_power && data.num_bss_ifaces;
+
+	if (iwl_mvm_bt_udpate_ctrl_kill_msk(mvm, data.reduced_tx_power))
+		IWL_ERR(mvm, "Failed to update the ctrl_kill_msk\n");
+}
+
+/* upon association, the fw will send in BT Coex notification */
+int iwl_mvm_rx_bt_coex_notif(struct iwl_mvm *mvm,
+			     struct iwl_rx_cmd_buffer *rxb,
+			     struct iwl_device_cmd *dev_cmd)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_bt_coex_profile_notif *notif = (void *)pkt->data;
+
+
+	IWL_DEBUG_COEX(mvm, "BT Coex Notification received\n");
+	IWL_DEBUG_COEX(mvm, "\tBT %salive\n", notif->bt_status ? "" : "not ");
+	IWL_DEBUG_COEX(mvm, "\tBT open conn %d\n", notif->bt_open_conn);
+	IWL_DEBUG_COEX(mvm, "\tBT traffic load %d\n", notif->bt_traffic_load);
+	IWL_DEBUG_COEX(mvm, "\tBT agg traffic load %d\n",
+		       notif->bt_agg_traffic_load);
+	IWL_DEBUG_COEX(mvm, "\tBT ci compliance %d\n", notif->bt_ci_compliance);
+
+	/* remember this notification for future use: rssi fluctuations */
+	memcpy(&mvm->last_bt_notif, notif, sizeof(mvm->last_bt_notif));
+
+	iwl_mvm_bt_coex_notif_handle(mvm);
+
+	/*
+	 * This is an async handler for a notification, returning anything other
+	 * than 0 doesn't make sense even if HCMD failed.
+	 */
+	return 0;
+}
+
+static void iwl_mvm_bt_rssi_iterator(void *_data, u8 *mac,
+				   struct ieee80211_vif *vif)
+{
+	struct iwl_mvm_vif *mvmvif = (void *)vif->drv_priv;
+	struct iwl_bt_iterator_data *data = _data;
+	struct iwl_mvm *mvm = data->mvm;
+
+	struct ieee80211_sta *sta;
+	struct iwl_mvm_sta *mvmsta;
+
+	if (vif->type != NL80211_IFTYPE_STATION ||
+	    mvmvif->ap_sta_id == IWL_MVM_STATION_COUNT)
+		return;
+
+	sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[mvmvif->ap_sta_id],
+					lockdep_is_held(&mvm->mutex));
+	mvmsta = (void *)sta->drv_priv;
+
+	/*
+	 * This interface doesn't support reduced Tx power (because of low
+	 * RSSI probably), then set bt_kill_msk to default values.
+	 */
+	if (!mvmsta->bt_reduced_txpower)
+		data->reduced_tx_power = false;
+	/* else - possibly leave it to BT_KILL_MSK_REDUCED_TXPOW */
+}
+
+void iwl_mvm_bt_rssi_event(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			   enum ieee80211_rssi_event rssi_event)
+{
+	struct iwl_mvm_vif *mvmvif = (void *)vif->drv_priv;
+	struct iwl_bt_iterator_data data = {
+		.mvm = mvm,
+		.reduced_tx_power = true,
+	};
+	int ret;
+
+	mutex_lock(&mvm->mutex);
+
+	/* Rssi update while not associated ?! */
+	if (WARN_ON_ONCE(mvmvif->ap_sta_id == IWL_MVM_STATION_COUNT))
+		goto out_unlock;
+
+	/* No open connection - reports should be disabled */
+	if (!BT_MBOX_MSG(&mvm->last_bt_notif, 3, OPEN_CON_2))
+		goto out_unlock;
+
+	IWL_DEBUG_COEX(mvm, "RSSI for %pM is now %s\n", vif->bss_conf.bssid,
+		       rssi_event == RSSI_EVENT_HIGH ? "HIGH" : "LOW");
+
+	/*
+	 * Check if rssi is good enough for reduced Tx power, but not in loose
+	 * scheme.
+	 */
+	if (rssi_event == RSSI_EVENT_LOW || is_loose_coex())
+		ret = iwl_mvm_bt_coex_reduced_txp(mvm, mvmvif->ap_sta_id,
+						  false);
+	else
+		ret = iwl_mvm_bt_coex_reduced_txp(mvm, mvmvif->ap_sta_id, true);
+
+	if (ret)
+		IWL_ERR(mvm, "couldn't send BT_CONFIG HCMD upon RSSI event\n");
+
+	ieee80211_iterate_active_interfaces_atomic(
+		mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
+		iwl_mvm_bt_rssi_iterator, &data);
+
+	/*
+	 * If there are no BSS / P2P client interfaces, reduced Tx Power is
+	 * irrelevant since it is based on the RSSI coming from the beacon.
+	 * Use BT_KILL_MSK_DEFAULT in that case.
+	 */
+	data.reduced_tx_power = data.reduced_tx_power && data.num_bss_ifaces;
+
+	if (iwl_mvm_bt_udpate_ctrl_kill_msk(mvm, data.reduced_tx_power))
+		IWL_ERR(mvm, "Failed to update the ctrl_kill_msk\n");
+
+ out_unlock:
+	mutex_unlock(&mvm->mutex);
+}
+
+void iwl_mvm_bt_coex_vif_assoc(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
+{
+	struct ieee80211_chanctx_conf *chanctx_conf;
+	enum ieee80211_band band;
+
+	rcu_read_lock();
+	chanctx_conf = rcu_dereference(vif->chanctx_conf);
+	if (chanctx_conf && chanctx_conf->def.chan)
+		band = chanctx_conf->def.chan->band;
+	else
+		band = -1;
+	rcu_read_unlock();
+
+	/* if we are in 2GHz we will get a notification from the fw */
+	if (band == IEEE80211_BAND_2GHZ)
+		return;
+
+	/* else, we can remove all the constraints */
+	memset(&mvm->last_bt_notif, 0, sizeof(mvm->last_bt_notif));
+
+	iwl_mvm_bt_coex_notif_handle(mvm);
+}
diff --git a/drivers/net/wireless/iwlwifi/mvm/d3.c b/drivers/net/wireless/iwlwifi/mvm/d3.c
index 994c8c263dc0..16bbdcc8627a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/d3.c
+++ b/drivers/net/wireless/iwlwifi/mvm/d3.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -62,8 +62,10 @@
  *****************************************************************************/
 
 #include <linux/etherdevice.h>
+#include <linux/ip.h>
 #include <net/cfg80211.h>
 #include <net/ipv6.h>
+#include <net/tcp.h>
 #include "iwl-modparams.h"
 #include "fw-api.h"
 #include "mvm.h"
@@ -402,6 +404,233 @@ static int iwl_mvm_send_proto_offload(struct iwl_mvm *mvm,
 				    sizeof(cmd), &cmd);
 }
 
+enum iwl_mvm_tcp_packet_type {
+	MVM_TCP_TX_SYN,
+	MVM_TCP_RX_SYNACK,
+	MVM_TCP_TX_DATA,
+	MVM_TCP_RX_ACK,
+	MVM_TCP_RX_WAKE,
+	MVM_TCP_TX_FIN,
+};
+
+static __le16 pseudo_hdr_check(int len, __be32 saddr, __be32 daddr)
+{
+	__sum16 check = tcp_v4_check(len, saddr, daddr, 0);
+	return cpu_to_le16(be16_to_cpu((__force __be16)check));
+}
+
+static void iwl_mvm_build_tcp_packet(struct iwl_mvm *mvm,
+				     struct ieee80211_vif *vif,
+				     struct cfg80211_wowlan_tcp *tcp,
+				     void *_pkt, u8 *mask,
+				     __le16 *pseudo_hdr_csum,
+				     enum iwl_mvm_tcp_packet_type ptype)
+{
+	struct {
+		struct ethhdr eth;
+		struct iphdr ip;
+		struct tcphdr tcp;
+		u8 data[];
+	} __packed *pkt = _pkt;
+	u16 ip_tot_len = sizeof(struct iphdr) + sizeof(struct tcphdr);
+	int i;
+
+	pkt->eth.h_proto = cpu_to_be16(ETH_P_IP),
+	pkt->ip.version = 4;
+	pkt->ip.ihl = 5;
+	pkt->ip.protocol = IPPROTO_TCP;
+
+	switch (ptype) {
+	case MVM_TCP_TX_SYN:
+	case MVM_TCP_TX_DATA:
+	case MVM_TCP_TX_FIN:
+		memcpy(pkt->eth.h_dest, tcp->dst_mac, ETH_ALEN);
+		memcpy(pkt->eth.h_source, vif->addr, ETH_ALEN);
+		pkt->ip.ttl = 128;
+		pkt->ip.saddr = tcp->src;
+		pkt->ip.daddr = tcp->dst;
+		pkt->tcp.source = cpu_to_be16(tcp->src_port);
+		pkt->tcp.dest = cpu_to_be16(tcp->dst_port);
+		/* overwritten for TX SYN later */
+		pkt->tcp.doff = sizeof(struct tcphdr) / 4;
+		pkt->tcp.window = cpu_to_be16(65000);
+		break;
+	case MVM_TCP_RX_SYNACK:
+	case MVM_TCP_RX_ACK:
+	case MVM_TCP_RX_WAKE:
+		memcpy(pkt->eth.h_dest, vif->addr, ETH_ALEN);
+		memcpy(pkt->eth.h_source, tcp->dst_mac, ETH_ALEN);
+		pkt->ip.saddr = tcp->dst;
+		pkt->ip.daddr = tcp->src;
+		pkt->tcp.source = cpu_to_be16(tcp->dst_port);
+		pkt->tcp.dest = cpu_to_be16(tcp->src_port);
+		break;
+	default:
+		WARN_ON(1);
+		return;
+	}
+
+	switch (ptype) {
+	case MVM_TCP_TX_SYN:
+		/* firmware assumes 8 option bytes - 8 NOPs for now */
+		memset(pkt->data, 0x01, 8);
+		ip_tot_len += 8;
+		pkt->tcp.doff = (sizeof(struct tcphdr) + 8) / 4;
+		pkt->tcp.syn = 1;
+		break;
+	case MVM_TCP_TX_DATA:
+		ip_tot_len += tcp->payload_len;
+		memcpy(pkt->data, tcp->payload, tcp->payload_len);
+		pkt->tcp.psh = 1;
+		pkt->tcp.ack = 1;
+		break;
+	case MVM_TCP_TX_FIN:
+		pkt->tcp.fin = 1;
+		pkt->tcp.ack = 1;
+		break;
+	case MVM_TCP_RX_SYNACK:
+		pkt->tcp.syn = 1;
+		pkt->tcp.ack = 1;
+		break;
+	case MVM_TCP_RX_ACK:
+		pkt->tcp.ack = 1;
+		break;
+	case MVM_TCP_RX_WAKE:
+		ip_tot_len += tcp->wake_len;
+		pkt->tcp.psh = 1;
+		pkt->tcp.ack = 1;
+		memcpy(pkt->data, tcp->wake_data, tcp->wake_len);
+		break;
+	}
+
+	switch (ptype) {
+	case MVM_TCP_TX_SYN:
+	case MVM_TCP_TX_DATA:
+	case MVM_TCP_TX_FIN:
+		pkt->ip.tot_len = cpu_to_be16(ip_tot_len);
+		pkt->ip.check = ip_fast_csum(&pkt->ip, pkt->ip.ihl);
+		break;
+	case MVM_TCP_RX_WAKE:
+		for (i = 0; i < DIV_ROUND_UP(tcp->wake_len, 8); i++) {
+			u8 tmp = tcp->wake_mask[i];
+			mask[i + 6] |= tmp << 6;
+			if (i + 1 < DIV_ROUND_UP(tcp->wake_len, 8))
+				mask[i + 7] = tmp >> 2;
+		}
+		/* fall through for ethernet/IP/TCP headers mask */
+	case MVM_TCP_RX_SYNACK:
+	case MVM_TCP_RX_ACK:
+		mask[0] = 0xff; /* match ethernet */
+		/*
+		 * match ethernet, ip.version, ip.ihl
+		 * the ip.ihl half byte is really masked out by firmware
+		 */
+		mask[1] = 0x7f;
+		mask[2] = 0x80; /* match ip.protocol */
+		mask[3] = 0xfc; /* match ip.saddr, ip.daddr */
+		mask[4] = 0x3f; /* match ip.daddr, tcp.source, tcp.dest */
+		mask[5] = 0x80; /* match tcp flags */
+		/* leave rest (0 or set for MVM_TCP_RX_WAKE) */
+		break;
+	};
+
+	*pseudo_hdr_csum = pseudo_hdr_check(ip_tot_len - sizeof(struct iphdr),
+					    pkt->ip.saddr, pkt->ip.daddr);
+}
+
+static int iwl_mvm_send_remote_wake_cfg(struct iwl_mvm *mvm,
+					struct ieee80211_vif *vif,
+					struct cfg80211_wowlan_tcp *tcp)
+{
+	struct iwl_wowlan_remote_wake_config *cfg;
+	struct iwl_host_cmd cmd = {
+		.id = REMOTE_WAKE_CONFIG_CMD,
+		.len = { sizeof(*cfg), },
+		.dataflags = { IWL_HCMD_DFL_NOCOPY, },
+		.flags = CMD_SYNC,
+	};
+	int ret;
+
+	if (!tcp)
+		return 0;
+
+	cfg = kzalloc(sizeof(*cfg), GFP_KERNEL);
+	if (!cfg)
+		return -ENOMEM;
+	cmd.data[0] = cfg;
+
+	cfg->max_syn_retries = 10;
+	cfg->max_data_retries = 10;
+	cfg->tcp_syn_ack_timeout = 1; /* seconds */
+	cfg->tcp_ack_timeout = 1; /* seconds */
+
+	/* SYN (TX) */
+	iwl_mvm_build_tcp_packet(
+		mvm, vif, tcp, cfg->syn_tx.data, NULL,
+		&cfg->syn_tx.info.tcp_pseudo_header_checksum,
+		MVM_TCP_TX_SYN);
+	cfg->syn_tx.info.tcp_payload_length = 0;
+
+	/* SYN/ACK (RX) */
+	iwl_mvm_build_tcp_packet(
+		mvm, vif, tcp, cfg->synack_rx.data, cfg->synack_rx.rx_mask,
+		&cfg->synack_rx.info.tcp_pseudo_header_checksum,
+		MVM_TCP_RX_SYNACK);
+	cfg->synack_rx.info.tcp_payload_length = 0;
+
+	/* KEEPALIVE/ACK (TX) */
+	iwl_mvm_build_tcp_packet(
+		mvm, vif, tcp, cfg->keepalive_tx.data, NULL,
+		&cfg->keepalive_tx.info.tcp_pseudo_header_checksum,
+		MVM_TCP_TX_DATA);
+	cfg->keepalive_tx.info.tcp_payload_length =
+		cpu_to_le16(tcp->payload_len);
+	cfg->sequence_number_offset = tcp->payload_seq.offset;
+	/* length must be 0..4, the field is little endian */
+	cfg->sequence_number_length = tcp->payload_seq.len;
+	cfg->initial_sequence_number = cpu_to_le32(tcp->payload_seq.start);
+	cfg->keepalive_interval = cpu_to_le16(tcp->data_interval);
+	if (tcp->payload_tok.len) {
+		cfg->token_offset = tcp->payload_tok.offset;
+		cfg->token_length = tcp->payload_tok.len;
+		cfg->num_tokens =
+			cpu_to_le16(tcp->tokens_size % tcp->payload_tok.len);
+		memcpy(cfg->tokens, tcp->payload_tok.token_stream,
+		       tcp->tokens_size);
+	} else {
+		/* set tokens to max value to almost never run out */
+		cfg->num_tokens = cpu_to_le16(65535);
+	}
+
+	/* ACK (RX) */
+	iwl_mvm_build_tcp_packet(
+		mvm, vif, tcp, cfg->keepalive_ack_rx.data,
+		cfg->keepalive_ack_rx.rx_mask,
+		&cfg->keepalive_ack_rx.info.tcp_pseudo_header_checksum,
+		MVM_TCP_RX_ACK);
+	cfg->keepalive_ack_rx.info.tcp_payload_length = 0;
+
+	/* WAKEUP (RX) */
+	iwl_mvm_build_tcp_packet(
+		mvm, vif, tcp, cfg->wake_rx.data, cfg->wake_rx.rx_mask,
+		&cfg->wake_rx.info.tcp_pseudo_header_checksum,
+		MVM_TCP_RX_WAKE);
+	cfg->wake_rx.info.tcp_payload_length =
+		cpu_to_le16(tcp->wake_len);
+
+	/* FIN */
+	iwl_mvm_build_tcp_packet(
+		mvm, vif, tcp, cfg->fin_tx.data, NULL,
+		&cfg->fin_tx.info.tcp_pseudo_header_checksum,
+		MVM_TCP_TX_FIN);
+	cfg->fin_tx.info.tcp_payload_length = 0;
+
+	ret = iwl_mvm_send_cmd(mvm, &cmd);
+	kfree(cfg);
+
+	return ret;
+}
+
 struct iwl_d3_iter_data {
 	struct iwl_mvm *mvm;
 	struct ieee80211_vif *vif;
@@ -540,7 +769,14 @@ int iwl_mvm_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
 	struct iwl_wowlan_config_cmd wowlan_config_cmd = {};
 	struct iwl_wowlan_kek_kck_material_cmd kek_kck_cmd = {};
 	struct iwl_wowlan_tkip_params_cmd tkip_cmd = {};
-	struct iwl_d3_manager_config d3_cfg_cmd = {};
+	struct iwl_d3_manager_config d3_cfg_cmd = {
+		/*
+		 * Program the minimum sleep time to 10 seconds, as many
+		 * platforms have issues processing a wakeup signal while
+		 * still being in the process of suspending.
+		 */
+		.min_sleep_time = cpu_to_le32(10 * 1000 * 1000),
+	};
 	struct wowlan_key_data key_data = {
 		.use_rsc_tsc = false,
 		.tkip = &tkip_cmd,
@@ -637,9 +873,21 @@ int iwl_mvm_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
 			cpu_to_le32(IWL_WOWLAN_WAKEUP_PATTERN_MATCH);
 
 	if (wowlan->rfkill_release)
-		d3_cfg_cmd.wakeup_flags |=
+		wowlan_config_cmd.wakeup_filter |=
 			cpu_to_le32(IWL_WOWLAN_WAKEUP_RF_KILL_DEASSERT);
 
+	if (wowlan->tcp) {
+		/*
+		 * Set the "link change" (really "link lost") flag as well
+		 * since that implies losing the TCP connection.
+		 */
+		wowlan_config_cmd.wakeup_filter |=
+			cpu_to_le32(IWL_WOWLAN_WAKEUP_REMOTE_LINK_LOSS |
+				    IWL_WOWLAN_WAKEUP_REMOTE_SIGNATURE_TABLE |
+				    IWL_WOWLAN_WAKEUP_REMOTE_WAKEUP_PACKET |
+				    IWL_WOWLAN_WAKEUP_LINK_CHANGE);
+	}
+
 	iwl_mvm_cancel_scan(mvm);
 
 	iwl_trans_stop_device(mvm->trans);
@@ -755,6 +1003,10 @@ int iwl_mvm_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
 	if (ret)
 		goto out;
 
+	ret = iwl_mvm_send_remote_wake_cfg(mvm, vif, wowlan->tcp);
+	if (ret)
+		goto out;
+
 	/* must be last -- this switches firmware state */
 	ret = iwl_mvm_send_cmd_pdu(mvm, D3_CONFIG_CMD, CMD_SYNC,
 				   sizeof(d3_cfg_cmd), &d3_cfg_cmd);
@@ -874,6 +1126,15 @@ static void iwl_mvm_query_wakeup_reasons(struct iwl_mvm *mvm,
 	if (reasons & IWL_WOWLAN_WAKEUP_BY_FOUR_WAY_HANDSHAKE)
 		wakeup.four_way_handshake = true;
 
+	if (reasons & IWL_WOWLAN_WAKEUP_BY_REM_WAKE_LINK_LOSS)
+		wakeup.tcp_connlost = true;
+
+	if (reasons & IWL_WOWLAN_WAKEUP_BY_REM_WAKE_SIGNATURE_TABLE)
+		wakeup.tcp_nomoretokens = true;
+
+	if (reasons & IWL_WOWLAN_WAKEUP_BY_REM_WAKE_WAKEUP_PACKET)
+		wakeup.tcp_match = true;
+
 	if (status->wake_packet_bufsize) {
 		int pktsize = le32_to_cpu(status->wake_packet_bufsize);
 		int pktlen = le32_to_cpu(status->wake_packet_length);
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs.c b/drivers/net/wireless/iwlwifi/mvm/debugfs.c
index c1bdb5582126..2053dccefcd6 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/debugfs.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -69,12 +69,6 @@ struct iwl_dbgfs_mvm_ctx {
 	struct ieee80211_vif *vif;
 };
 
-static int iwl_dbgfs_open_file_generic(struct inode *inode, struct file *file)
-{
-	file->private_data = inode->i_private;
-	return 0;
-}
-
 static ssize_t iwl_dbgfs_tx_flush_write(struct file *file,
 					const char __user *user_buf,
 					size_t count, loff_t *ppos)
@@ -306,10 +300,191 @@ static ssize_t iwl_dbgfs_power_down_d3_allow_write(struct file *file,
 	return count;
 }
 
+static ssize_t iwl_dbgfs_mac_params_read(struct file *file,
+					 char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct ieee80211_vif *vif = file->private_data;
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	struct iwl_mvm *mvm = mvmvif->dbgfs_data;
+	u8 ap_sta_id;
+	struct ieee80211_chanctx_conf *chanctx_conf;
+	char buf[512];
+	int bufsz = sizeof(buf);
+	int pos = 0;
+	int i;
+
+	mutex_lock(&mvm->mutex);
+
+	ap_sta_id = mvmvif->ap_sta_id;
+
+	pos += scnprintf(buf+pos, bufsz-pos, "mac id/color: %d / %d\n",
+			 mvmvif->id, mvmvif->color);
+	pos += scnprintf(buf+pos, bufsz-pos, "bssid: %pM\n",
+			 vif->bss_conf.bssid);
+	pos += scnprintf(buf+pos, bufsz-pos, "QoS:\n");
+	for (i = 0; i < ARRAY_SIZE(mvmvif->queue_params); i++) {
+		pos += scnprintf(buf+pos, bufsz-pos,
+				 "\t%d: txop:%d - cw_min:%d - cw_max = %d - aifs = %d upasd = %d\n",
+				 i, mvmvif->queue_params[i].txop,
+				 mvmvif->queue_params[i].cw_min,
+				 mvmvif->queue_params[i].cw_max,
+				 mvmvif->queue_params[i].aifs,
+				 mvmvif->queue_params[i].uapsd);
+	}
+
+	if (vif->type == NL80211_IFTYPE_STATION &&
+	    ap_sta_id != IWL_MVM_STATION_COUNT) {
+		struct ieee80211_sta *sta;
+		struct iwl_mvm_sta *mvm_sta;
+
+		sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[ap_sta_id],
+						lockdep_is_held(&mvm->mutex));
+		mvm_sta = (void *)sta->drv_priv;
+		pos += scnprintf(buf+pos, bufsz-pos,
+				 "ap_sta_id %d - reduced Tx power %d\n",
+				 ap_sta_id, mvm_sta->bt_reduced_txpower);
+	}
+
+	rcu_read_lock();
+	chanctx_conf = rcu_dereference(vif->chanctx_conf);
+	if (chanctx_conf) {
+		pos += scnprintf(buf+pos, bufsz-pos,
+				 "idle rx chains %d, active rx chains: %d\n",
+				 chanctx_conf->rx_chains_static,
+				 chanctx_conf->rx_chains_dynamic);
+	}
+	rcu_read_unlock();
+
+	mutex_unlock(&mvm->mutex);
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+#define BT_MBOX_MSG(_notif, _num, _field)				     \
+	((le32_to_cpu((_notif)->mbox_msg[(_num)]) & BT_MBOX##_num##_##_field)\
+	>> BT_MBOX##_num##_##_field##_POS)
+
+
+#define BT_MBOX_PRINT(_num, _field, _end)				    \
+			pos += scnprintf(buf + pos, bufsz - pos,	    \
+					 "\t%s: %d%s",			    \
+					 #_field,			    \
+					 BT_MBOX_MSG(notif, _num, _field),  \
+					 true ? "\n" : ", ");
+
+static ssize_t iwl_dbgfs_bt_notif_read(struct file *file, char __user *user_buf,
+				       size_t count, loff_t *ppos)
+{
+	struct iwl_mvm *mvm = file->private_data;
+	struct iwl_bt_coex_profile_notif *notif = &mvm->last_bt_notif;
+	char *buf;
+	int ret, pos = 0, bufsz = sizeof(char) * 1024;
+
+	buf = kmalloc(bufsz, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	mutex_lock(&mvm->mutex);
+
+	pos += scnprintf(buf+pos, bufsz-pos, "MBOX dw0:\n");
+
+	BT_MBOX_PRINT(0, LE_SLAVE_LAT, false);
+	BT_MBOX_PRINT(0, LE_PROF1, false);
+	BT_MBOX_PRINT(0, LE_PROF2, false);
+	BT_MBOX_PRINT(0, LE_PROF_OTHER, false);
+	BT_MBOX_PRINT(0, CHL_SEQ_N, false);
+	BT_MBOX_PRINT(0, INBAND_S, false);
+	BT_MBOX_PRINT(0, LE_MIN_RSSI, false);
+	BT_MBOX_PRINT(0, LE_SCAN, false);
+	BT_MBOX_PRINT(0, LE_ADV, false);
+	BT_MBOX_PRINT(0, LE_MAX_TX_POWER, false);
+	BT_MBOX_PRINT(0, OPEN_CON_1, true);
+
+	pos += scnprintf(buf+pos, bufsz-pos, "MBOX dw1:\n");
+
+	BT_MBOX_PRINT(1, BR_MAX_TX_POWER, false);
+	BT_MBOX_PRINT(1, IP_SR, false);
+	BT_MBOX_PRINT(1, LE_MSTR, false);
+	BT_MBOX_PRINT(1, AGGR_TRFC_LD, false);
+	BT_MBOX_PRINT(1, MSG_TYPE, false);
+	BT_MBOX_PRINT(1, SSN, true);
+
+	pos += scnprintf(buf+pos, bufsz-pos, "MBOX dw2:\n");
+
+	BT_MBOX_PRINT(2, SNIFF_ACT, false);
+	BT_MBOX_PRINT(2, PAG, false);
+	BT_MBOX_PRINT(2, INQUIRY, false);
+	BT_MBOX_PRINT(2, CONN, false);
+	BT_MBOX_PRINT(2, SNIFF_INTERVAL, false);
+	BT_MBOX_PRINT(2, DISC, false);
+	BT_MBOX_PRINT(2, SCO_TX_ACT, false);
+	BT_MBOX_PRINT(2, SCO_RX_ACT, false);
+	BT_MBOX_PRINT(2, ESCO_RE_TX, false);
+	BT_MBOX_PRINT(2, SCO_DURATION, true);
+
+	pos += scnprintf(buf+pos, bufsz-pos, "MBOX dw3:\n");
+
+	BT_MBOX_PRINT(3, SCO_STATE, false);
+	BT_MBOX_PRINT(3, SNIFF_STATE, false);
+	BT_MBOX_PRINT(3, A2DP_STATE, false);
+	BT_MBOX_PRINT(3, ACL_STATE, false);
+	BT_MBOX_PRINT(3, MSTR_STATE, false);
+	BT_MBOX_PRINT(3, OBX_STATE, false);
+	BT_MBOX_PRINT(3, OPEN_CON_2, false);
+	BT_MBOX_PRINT(3, TRAFFIC_LOAD, false);
+	BT_MBOX_PRINT(3, CHL_SEQN_LSB, false);
+	BT_MBOX_PRINT(3, INBAND_P, false);
+	BT_MBOX_PRINT(3, MSG_TYPE_2, false);
+	BT_MBOX_PRINT(3, SSN_2, false);
+	BT_MBOX_PRINT(3, UPDATE_REQUEST, true);
+
+	pos += scnprintf(buf+pos, bufsz-pos, "bt_status = %d\n",
+					 notif->bt_status);
+	pos += scnprintf(buf+pos, bufsz-pos, "bt_open_conn = %d\n",
+					 notif->bt_open_conn);
+	pos += scnprintf(buf+pos, bufsz-pos, "bt_traffic_load = %d\n",
+					 notif->bt_traffic_load);
+	pos += scnprintf(buf+pos, bufsz-pos, "bt_agg_traffic_load = %d\n",
+					 notif->bt_agg_traffic_load);
+	pos += scnprintf(buf+pos, bufsz-pos, "bt_ci_compliance = %d\n",
+					 notif->bt_ci_compliance);
+
+	mutex_unlock(&mvm->mutex);
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+
+	return ret;
+}
+#undef BT_MBOX_PRINT
+
+static ssize_t iwl_dbgfs_fw_restart_write(struct file *file,
+					  const char __user *user_buf,
+					  size_t count, loff_t *ppos)
+{
+	struct iwl_mvm *mvm = file->private_data;
+	bool restart_fw = iwlwifi_mod_params.restart_fw;
+	int ret;
+
+	iwlwifi_mod_params.restart_fw = true;
+
+	mutex_lock(&mvm->mutex);
+
+	/* take the return value to make compiler happy - it will fail anyway */
+	ret = iwl_mvm_send_cmd_pdu(mvm, REPLY_ERROR, CMD_SYNC, 0, NULL);
+
+	mutex_unlock(&mvm->mutex);
+
+	iwlwifi_mod_params.restart_fw = restart_fw;
+
+	return count;
+}
+
 #define MVM_DEBUGFS_READ_FILE_OPS(name)					\
 static const struct file_operations iwl_dbgfs_##name##_ops = {	\
 	.read = iwl_dbgfs_##name##_read,				\
-	.open = iwl_dbgfs_open_file_generic,				\
+	.open = simple_open,						\
 	.llseek = generic_file_llseek,					\
 }
 
@@ -317,14 +492,14 @@ static const struct file_operations iwl_dbgfs_##name##_ops = {	\
 static const struct file_operations iwl_dbgfs_##name##_ops = {	\
 	.write = iwl_dbgfs_##name##_write,				\
 	.read = iwl_dbgfs_##name##_read,				\
-	.open = iwl_dbgfs_open_file_generic,				\
+	.open = simple_open,						\
 	.llseek = generic_file_llseek,					\
 };
 
 #define MVM_DEBUGFS_WRITE_FILE_OPS(name)				\
 static const struct file_operations iwl_dbgfs_##name##_ops = {	\
 	.write = iwl_dbgfs_##name##_write,				\
-	.open = iwl_dbgfs_open_file_generic,				\
+	.open = simple_open,						\
 	.llseek = generic_file_llseek,					\
 };
 
@@ -345,8 +520,13 @@ MVM_DEBUGFS_WRITE_FILE_OPS(tx_flush);
 MVM_DEBUGFS_WRITE_FILE_OPS(sta_drain);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(sram);
 MVM_DEBUGFS_READ_FILE_OPS(stations);
+MVM_DEBUGFS_READ_FILE_OPS(bt_notif);
 MVM_DEBUGFS_WRITE_FILE_OPS(power_down_allow);
 MVM_DEBUGFS_WRITE_FILE_OPS(power_down_d3_allow);
+MVM_DEBUGFS_WRITE_FILE_OPS(fw_restart);
+
+/* Interface specific debugfs entries */
+MVM_DEBUGFS_READ_FILE_OPS(mac_params);
 
 int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir)
 {
@@ -358,8 +538,10 @@ int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir)
 	MVM_DEBUGFS_ADD_FILE(sta_drain, mvm->debugfs_dir, S_IWUSR);
 	MVM_DEBUGFS_ADD_FILE(sram, mvm->debugfs_dir, S_IWUSR | S_IRUSR);
 	MVM_DEBUGFS_ADD_FILE(stations, dbgfs_dir, S_IRUSR);
+	MVM_DEBUGFS_ADD_FILE(bt_notif, dbgfs_dir, S_IRUSR);
 	MVM_DEBUGFS_ADD_FILE(power_down_allow, mvm->debugfs_dir, S_IWUSR);
 	MVM_DEBUGFS_ADD_FILE(power_down_d3_allow, mvm->debugfs_dir, S_IWUSR);
+	MVM_DEBUGFS_ADD_FILE(fw_restart, mvm->debugfs_dir, S_IWUSR);
 
 	/*
 	 * Create a symlink with mac80211. It will be removed when mac80211
@@ -376,3 +558,58 @@ err:
 	IWL_ERR(mvm, "Can't create the mvm debugfs directory\n");
 	return -ENOMEM;
 }
+
+void iwl_mvm_vif_dbgfs_register(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
+{
+	struct dentry *dbgfs_dir = vif->debugfs_dir;
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	char buf[100];
+
+	if (!dbgfs_dir)
+		return;
+
+	mvmvif->dbgfs_dir = debugfs_create_dir("iwlmvm", dbgfs_dir);
+	mvmvif->dbgfs_data = mvm;
+
+	if (!mvmvif->dbgfs_dir) {
+		IWL_ERR(mvm, "Failed to create debugfs directory under %s\n",
+			dbgfs_dir->d_name.name);
+		return;
+	}
+
+	MVM_DEBUGFS_ADD_FILE_VIF(mac_params, mvmvif->dbgfs_dir,
+				 S_IRUSR);
+
+	/*
+	 * Create symlink for convenience pointing to interface specific
+	 * debugfs entries for the driver. For example, under
+	 * /sys/kernel/debug/iwlwifi/0000\:02\:00.0/iwlmvm/
+	 * find
+	 * netdev:wlan0 -> ../../../ieee80211/phy0/netdev:wlan0/iwlmvm/
+	 */
+	snprintf(buf, 100, "../../../%s/%s/%s/%s",
+		 dbgfs_dir->d_parent->d_parent->d_name.name,
+		 dbgfs_dir->d_parent->d_name.name,
+		 dbgfs_dir->d_name.name,
+		 mvmvif->dbgfs_dir->d_name.name);
+
+	mvmvif->dbgfs_slink = debugfs_create_symlink(dbgfs_dir->d_name.name,
+						     mvm->debugfs_dir, buf);
+	if (!mvmvif->dbgfs_slink)
+		IWL_ERR(mvm, "Can't create debugfs symbolic link under %s\n",
+			dbgfs_dir->d_name.name);
+	return;
+err:
+	IWL_ERR(mvm, "Can't create debugfs entity\n");
+}
+
+void iwl_mvm_vif_dbgfs_clean(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
+{
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+
+	debugfs_remove(mvmvif->dbgfs_slink);
+	mvmvif->dbgfs_slink = NULL;
+
+	debugfs_remove_recursive(mvmvif->dbgfs_dir);
+	mvmvif->dbgfs_dir = NULL;
+}
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-bt-coex.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-bt-coex.h
new file mode 100644
index 000000000000..05c61d6f384e
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-bt-coex.h
@@ -0,0 +1,319 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2013 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called COPYING.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2013 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *****************************************************************************/
+
+#ifndef __fw_api_bt_coex_h__
+#define __fw_api_bt_coex_h__
+
+#include <linux/types.h>
+#include <linux/bitops.h>
+
+#define BITS(nb) (BIT(nb) - 1)
+
+/**
+ * enum iwl_bt_coex_flags - flags for BT_COEX command
+ * @BT_CH_PRIMARY_EN:
+ * @BT_CH_SECONDARY_EN:
+ * @BT_NOTIF_COEX_OFF:
+ * @BT_COEX_MODE_POS:
+ * @BT_COEX_MODE_MSK:
+ * @BT_COEX_DISABLE:
+ * @BT_COEX_2W:
+ * @BT_COEX_3W:
+ * @BT_COEX_NW:
+ * @BT_USE_DEFAULTS:
+ * @BT_SYNC_2_BT_DISABLE:
+ * @BT_COEX_CORUNNING_TBL_EN:
+ */
+enum iwl_bt_coex_flags {
+	BT_CH_PRIMARY_EN		= BIT(0),
+	BT_CH_SECONDARY_EN		= BIT(1),
+	BT_NOTIF_COEX_OFF		= BIT(2),
+	BT_COEX_MODE_POS		= 3,
+	BT_COEX_MODE_MSK		= BITS(3) << BT_COEX_MODE_POS,
+	BT_COEX_DISABLE			= 0x0 << BT_COEX_MODE_POS,
+	BT_COEX_2W			= 0x1 << BT_COEX_MODE_POS,
+	BT_COEX_3W			= 0x2 << BT_COEX_MODE_POS,
+	BT_COEX_NW			= 0x3 << BT_COEX_MODE_POS,
+	BT_USE_DEFAULTS			= BIT(6),
+	BT_SYNC_2_BT_DISABLE		= BIT(7),
+	/*
+	 * For future use - when the flags will be enlarged
+	 * BT_COEX_CORUNNING_TBL_EN	= BIT(8),
+	 */
+};
+
+/*
+ * indicates what has changed in the BT_COEX command.
+ */
+enum iwl_bt_coex_valid_bit_msk {
+	BT_VALID_ENABLE			= BIT(0),
+	BT_VALID_BT_PRIO_BOOST		= BIT(1),
+	BT_VALID_MAX_KILL		= BIT(2),
+	BT_VALID_3W_TMRS		= BIT(3),
+	BT_VALID_KILL_ACK		= BIT(4),
+	BT_VALID_KILL_CTS		= BIT(5),
+	BT_VALID_REDUCED_TX_POWER	= BIT(6),
+	BT_VALID_LUT			= BIT(7),
+	BT_VALID_WIFI_RX_SW_PRIO_BOOST	= BIT(8),
+	BT_VALID_WIFI_TX_SW_PRIO_BOOST	= BIT(9),
+	BT_VALID_MULTI_PRIO_LUT		= BIT(10),
+	BT_VALID_TRM_KICK_FILTER	= BIT(11),
+	BT_VALID_CORUN_LUT_20		= BIT(12),
+	BT_VALID_CORUN_LUT_40		= BIT(13),
+	BT_VALID_ANT_ISOLATION		= BIT(14),
+	BT_VALID_ANT_ISOLATION_THRS	= BIT(15),
+	/*
+	 * For future use - when the valid flags will be enlarged
+	 * BT_VALID_TXTX_DELTA_FREQ_THRS	= BIT(16),
+	 * BT_VALID_TXRX_MAX_FREQ_0	= BIT(17),
+	 */
+};
+
+/**
+ * enum iwl_bt_reduced_tx_power - allows to reduce txpower for WiFi frames.
+ * @BT_REDUCED_TX_POWER_CTL: reduce Tx power for control frames
+ * @BT_REDUCED_TX_POWER_DATA: reduce Tx power for data frames
+ *
+ * This mechanism allows to have BT and WiFi run concurrently. Since WiFi
+ * reduces its Tx power, it can work along with BT, hence reducing the amount
+ * of WiFi frames being killed by BT.
+ */
+enum iwl_bt_reduced_tx_power {
+	BT_REDUCED_TX_POWER_CTL		= BIT(0),
+	BT_REDUCED_TX_POWER_DATA	= BIT(1),
+};
+
+#define BT_COEX_LUT_SIZE (12)
+
+/**
+ * struct iwl_bt_coex_cmd - bt coex configuration command
+ * @flags:&enum iwl_bt_coex_flags
+ * @lead_time:
+ * @max_kill:
+ * @bt3_time_t7_value:
+ * @kill_ack_msk:
+ * @kill_cts_msk:
+ * @bt3_prio_sample_time:
+ * @bt3_timer_t2_value:
+ * @bt4_reaction_time:
+ * @decision_lut[12]:
+ * @bt_reduced_tx_power: enum %iwl_bt_reduced_tx_power
+ * @valid_bit_msk: enum %iwl_bt_coex_valid_bit_msk
+ * @bt_prio_boost: values for PTA boost register
+ * @wifi_tx_prio_boost: SW boost of wifi tx priority
+ * @wifi_rx_prio_boost: SW boost of wifi rx priority
+ *
+ * The structure is used for the BT_COEX command.
+ */
+struct iwl_bt_coex_cmd {
+	u8 flags;
+	u8 lead_time;
+	u8 max_kill;
+	u8 bt3_time_t7_value;
+	__le32 kill_ack_msk;
+	__le32 kill_cts_msk;
+	u8 bt3_prio_sample_time;
+	u8 bt3_timer_t2_value;
+	__le16 bt4_reaction_time;
+	__le32 decision_lut[BT_COEX_LUT_SIZE];
+	u8 bt_reduced_tx_power;
+	u8 reserved;
+	__le16 valid_bit_msk;
+	__le32 bt_prio_boost;
+	u8 reserved2;
+	u8 wifi_tx_prio_boost;
+	__le16 wifi_rx_prio_boost;
+} __packed; /* BT_COEX_CMD_API_S_VER_3 */
+
+#define BT_MBOX(n_dw, _msg, _pos, _nbits)	\
+	BT_MBOX##n_dw##_##_msg##_POS = (_pos),	\
+	BT_MBOX##n_dw##_##_msg = BITS(_nbits) << BT_MBOX##n_dw##_##_msg##_POS
+
+enum iwl_bt_mxbox_dw0 {
+	BT_MBOX(0, LE_SLAVE_LAT, 0, 3),
+	BT_MBOX(0, LE_PROF1, 3, 1),
+	BT_MBOX(0, LE_PROF2, 4, 1),
+	BT_MBOX(0, LE_PROF_OTHER, 5, 1),
+	BT_MBOX(0, CHL_SEQ_N, 8, 4),
+	BT_MBOX(0, INBAND_S, 13, 1),
+	BT_MBOX(0, LE_MIN_RSSI, 16, 4),
+	BT_MBOX(0, LE_SCAN, 20, 1),
+	BT_MBOX(0, LE_ADV, 21, 1),
+	BT_MBOX(0, LE_MAX_TX_POWER, 24, 4),
+	BT_MBOX(0, OPEN_CON_1, 28, 2),
+};
+
+enum iwl_bt_mxbox_dw1 {
+	BT_MBOX(1, BR_MAX_TX_POWER, 0, 4),
+	BT_MBOX(1, IP_SR, 4, 1),
+	BT_MBOX(1, LE_MSTR, 5, 1),
+	BT_MBOX(1, AGGR_TRFC_LD, 8, 6),
+	BT_MBOX(1, MSG_TYPE, 16, 3),
+	BT_MBOX(1, SSN, 19, 2),
+};
+
+enum iwl_bt_mxbox_dw2 {
+	BT_MBOX(2, SNIFF_ACT, 0, 3),
+	BT_MBOX(2, PAG, 3, 1),
+	BT_MBOX(2, INQUIRY, 4, 1),
+	BT_MBOX(2, CONN, 5, 1),
+	BT_MBOX(2, SNIFF_INTERVAL, 8, 5),
+	BT_MBOX(2, DISC, 13, 1),
+	BT_MBOX(2, SCO_TX_ACT, 16, 2),
+	BT_MBOX(2, SCO_RX_ACT, 18, 2),
+	BT_MBOX(2, ESCO_RE_TX, 20, 2),
+	BT_MBOX(2, SCO_DURATION, 24, 6),
+};
+
+enum iwl_bt_mxbox_dw3 {
+	BT_MBOX(3, SCO_STATE, 0, 1),
+	BT_MBOX(3, SNIFF_STATE, 1, 1),
+	BT_MBOX(3, A2DP_STATE, 2, 1),
+	BT_MBOX(3, ACL_STATE, 3, 1),
+	BT_MBOX(3, MSTR_STATE, 4, 1),
+	BT_MBOX(3, OBX_STATE, 5, 1),
+	BT_MBOX(3, OPEN_CON_2, 8, 2),
+	BT_MBOX(3, TRAFFIC_LOAD, 10, 2),
+	BT_MBOX(3, CHL_SEQN_LSB, 12, 1),
+	BT_MBOX(3, INBAND_P, 13, 1),
+	BT_MBOX(3, MSG_TYPE_2, 16, 3),
+	BT_MBOX(3, SSN_2, 19, 2),
+	BT_MBOX(3, UPDATE_REQUEST, 21, 1),
+};
+
+#define BT_MBOX_MSG(_notif, _num, _field)				     \
+	((le32_to_cpu((_notif)->mbox_msg[(_num)]) & BT_MBOX##_num##_##_field)\
+	>> BT_MBOX##_num##_##_field##_POS)
+
+/**
+ * struct iwl_bt_coex_profile_notif - notification about BT coex
+ * @mbox_msg: message from BT to WiFi
+ * @:bt_status: 0 - off, 1 - on
+ * @:bt_open_conn: number of BT connections open
+ * @:bt_traffic_load: load of BT traffic
+ * @:bt_agg_traffic_load: aggregated load of BT traffic
+ * @:bt_ci_compliance: 0 - no CI compliance, 1 - CI compliant
+ */
+struct iwl_bt_coex_profile_notif {
+	__le32 mbox_msg[4];
+	u8 bt_status;
+	u8 bt_open_conn;
+	u8 bt_traffic_load;
+	u8 bt_agg_traffic_load;
+	u8 bt_ci_compliance;
+	u8 reserved[3];
+} __packed; /* BT_COEX_PROFILE_NTFY_API_S_VER_2 */
+
+enum iwl_bt_coex_prio_table_event {
+	BT_COEX_PRIO_TBL_EVT_INIT_CALIB1		= 0,
+	BT_COEX_PRIO_TBL_EVT_INIT_CALIB2		= 1,
+	BT_COEX_PRIO_TBL_EVT_PERIODIC_CALIB_LOW1	= 2,
+	BT_COEX_PRIO_TBL_EVT_PERIODIC_CALIB_LOW2	= 3,
+	BT_COEX_PRIO_TBL_EVT_PERIODIC_CALIB_HIGH1	= 4,
+	BT_COEX_PRIO_TBL_EVT_PERIODIC_CALIB_HIGH2	= 5,
+	BT_COEX_PRIO_TBL_EVT_DTIM			= 6,
+	BT_COEX_PRIO_TBL_EVT_SCAN52			= 7,
+	BT_COEX_PRIO_TBL_EVT_SCAN24			= 8,
+	BT_COEX_PRIO_TBL_EVT_IDLE			= 9,
+	BT_COEX_PRIO_TBL_EVT_MAX			= 16,
+}; /* BT_COEX_PRIO_TABLE_EVENTS_API_E_VER_1 */
+
+enum iwl_bt_coex_prio_table_prio {
+	BT_COEX_PRIO_TBL_DISABLED	= 0,
+	BT_COEX_PRIO_TBL_PRIO_LOW	= 1,
+	BT_COEX_PRIO_TBL_PRIO_HIGH	= 2,
+	BT_COEX_PRIO_TBL_PRIO_BYPASS	= 3,
+	BT_COEX_PRIO_TBL_PRIO_COEX_OFF	= 4,
+	BT_COEX_PRIO_TBL_PRIO_COEX_ON	= 5,
+	BT_COEX_PRIO_TBL_PRIO_COEX_IDLE = 6,
+	BT_COEX_PRIO_TBL_MAX		= 8,
+}; /* BT_COEX_PRIO_TABLE_PRIORITIES_API_E_VER_1 */
+
+#define BT_COEX_PRIO_TBL_SHRD_ANT_POS     (0)
+#define BT_COEX_PRIO_TBL_PRIO_POS         (1)
+#define BT_COEX_PRIO_TBL_RESERVED_POS     (4)
+
+/**
+ * struct iwl_bt_coex_prio_tbl_cmd - priority table for BT coex
+ * @prio_tbl:
+ */
+struct iwl_bt_coex_prio_tbl_cmd {
+	u8 prio_tbl[BT_COEX_PRIO_TBL_EVT_MAX];
+} __packed;
+
+enum iwl_bt_coex_env_action {
+	BT_COEX_ENV_CLOSE	 = 0,
+	BT_COEX_ENV_OPEN	 = 1,
+}; /* BT_COEX_PROT_ENV_ACTION_API_E_VER_1 */
+
+/**
+ * struct iwl_bt_coex_prot_env_cmd - BT Protection Envelope
+ * @action: enum %iwl_bt_coex_env_action
+ * @type: enum %iwl_bt_coex_prio_table_event
+ */
+struct iwl_bt_coex_prot_env_cmd {
+	u8 action; /* 0 = closed, 1 = open */
+	u8 type; /* 0 .. 15 */
+	u8 reserved[2];
+} __packed;
+
+#endif /* __fw_api_bt_coex_h__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
index cf6f9a02fb74..51e015d1dfb2 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -258,7 +258,7 @@ enum iwl_wowlan_wakeup_reason {
 	IWL_WOWLAN_WAKEUP_BY_FOUR_WAY_HANDSHAKE			= BIT(8),
 	IWL_WOWLAN_WAKEUP_BY_REM_WAKE_LINK_LOSS			= BIT(9),
 	IWL_WOWLAN_WAKEUP_BY_REM_WAKE_SIGNATURE_TABLE		= BIT(10),
-	IWL_WOWLAN_WAKEUP_BY_REM_WAKE_TCP_EXTERNAL		= BIT(11),
+	/* BIT(11) reserved */
 	IWL_WOWLAN_WAKEUP_BY_REM_WAKE_WAKEUP_PACKET		= BIT(12),
 }; /* WOWLAN_WAKE_UP_REASON_API_E_VER_2 */
 
@@ -277,6 +277,55 @@ struct iwl_wowlan_status {
 	u8 wake_packet[]; /* can be truncated from _length to _bufsize */
 } __packed; /* WOWLAN_STATUSES_API_S_VER_4 */
 
+#define IWL_WOWLAN_TCP_MAX_PACKET_LEN		64
+#define IWL_WOWLAN_REMOTE_WAKE_MAX_PACKET_LEN	128
+#define IWL_WOWLAN_REMOTE_WAKE_MAX_TOKENS	2048
+
+struct iwl_tcp_packet_info {
+	__le16 tcp_pseudo_header_checksum;
+	__le16 tcp_payload_length;
+} __packed; /* TCP_PACKET_INFO_API_S_VER_2 */
+
+struct iwl_tcp_packet {
+	struct iwl_tcp_packet_info info;
+	u8 rx_mask[IWL_WOWLAN_MAX_PATTERN_LEN / 8];
+	u8 data[IWL_WOWLAN_TCP_MAX_PACKET_LEN];
+} __packed; /* TCP_PROTOCOL_PACKET_API_S_VER_1 */
+
+struct iwl_remote_wake_packet {
+	struct iwl_tcp_packet_info info;
+	u8 rx_mask[IWL_WOWLAN_MAX_PATTERN_LEN / 8];
+	u8 data[IWL_WOWLAN_REMOTE_WAKE_MAX_PACKET_LEN];
+} __packed; /* TCP_PROTOCOL_PACKET_API_S_VER_1 */
+
+struct iwl_wowlan_remote_wake_config {
+	__le32 connection_max_time; /* unused */
+	/* TCP_PROTOCOL_CONFIG_API_S_VER_1 */
+	u8 max_syn_retries;
+	u8 max_data_retries;
+	u8 tcp_syn_ack_timeout;
+	u8 tcp_ack_timeout;
+
+	struct iwl_tcp_packet syn_tx;
+	struct iwl_tcp_packet synack_rx;
+	struct iwl_tcp_packet keepalive_ack_rx;
+	struct iwl_tcp_packet fin_tx;
+
+	struct iwl_remote_wake_packet keepalive_tx;
+	struct iwl_remote_wake_packet wake_rx;
+
+	/* REMOTE_WAKE_OFFSET_INFO_API_S_VER_1 */
+	u8 sequence_number_offset;
+	u8 sequence_number_length;
+	u8 token_offset;
+	u8 token_length;
+	/* REMOTE_WAKE_PROTOCOL_PARAMS_API_S_VER_1 */
+	__le32 initial_sequence_number;
+	__le16 keepalive_interval;
+	__le16 num_tokens;
+	u8 tokens[IWL_WOWLAN_REMOTE_WAKE_MAX_TOKENS];
+} __packed; /* REMOTE_WAKE_CONFIG_API_S_VER_2 */
+
 /* TODO: NetDetect API */
 
 #endif /* __fw_api_d3_h__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-mac.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-mac.h
index ae39b7dfda7b..d68640ea41d4 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-mac.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-mac.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h
index be36b7604b7f..81fe45f46be7 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -68,73 +68,53 @@
 
 /**
  * enum iwl_scan_flags - masks for power table command flags
+ * @POWER_FLAGS_POWER_SAVE_ENA_MSK: '1' Allow to save power by turning off
+ *		receiver and transmitter. '0' - does not allow.
  * @POWER_FLAGS_POWER_MANAGEMENT_ENA_MSK: '0' Driver disables power management,
  *		'1' Driver enables PM (use rest of parameters)
- * @POWER_FLAGS_SLEEP_OVER_DTIM_MSK: '0' PM have to walk up every DTIM,
+ * @POWER_FLAGS_SKIP_OVER_DTIM_MSK: '0' PM have to walk up every DTIM,
  *		'1' PM could sleep over DTIM till listen Interval.
- * @POWER_FLAGS_LPRX_ENA_MSK: Low Power RX enable.
- * @POWER_FLAGS_SNOOZE_ENA_MSK: Enable snoozing only if uAPSD is enabled and all
- *		access categories are both delivery and trigger enabled.
- * @POWER_FLAGS_BT_SCO_ENA: Enable BT SCO coex only if uAPSD and
- *		PBW Snoozing enabled
  * @POWER_FLAGS_ADVANCE_PM_ENA_MSK: Advanced PM (uAPSD) enable mask
+ * @POWER_FLAGS_LPRX_ENA_MSK: Low Power RX enable.
 */
 enum iwl_power_flags {
-	POWER_FLAGS_POWER_MANAGEMENT_ENA_MSK	= BIT(0),
-	POWER_FLAGS_SLEEP_OVER_DTIM_MSK		= BIT(1),
-	POWER_FLAGS_LPRX_ENA_MSK		= BIT(2),
-	POWER_FLAGS_SNOOZE_ENA_MSK		= BIT(3),
-	POWER_FLAGS_BT_SCO_ENA			= BIT(4),
-	POWER_FLAGS_ADVANCE_PM_ENA_MSK		= BIT(5)
+	POWER_FLAGS_POWER_SAVE_ENA_MSK		= BIT(0),
+	POWER_FLAGS_POWER_MANAGEMENT_ENA_MSK	= BIT(1),
+	POWER_FLAGS_SKIP_OVER_DTIM_MSK		= BIT(2),
+	POWER_FLAGS_ADVANCE_PM_ENA_MSK		= BIT(9),
+	POWER_FLAGS_LPRX_ENA_MSK		= BIT(11),
 };
 
+#define IWL_POWER_VEC_SIZE 5
+
 /**
  * struct iwl_powertable_cmd - Power Table Command
  * POWER_TABLE_CMD = 0x77 (command, has simple generic response)
  *
- * @id_and_color:	MAC contex identifier
- * @action:		Action on context - no action, add new,
- *			modify existent, remove
  * @flags:		Power table command flags from POWER_FLAGS_*
  * @keep_alive_seconds: Keep alive period in seconds. Default - 25 sec.
- *			Minimum allowed:- 3 * DTIM
+ *			Minimum allowed:- 3 * DTIM. Keep alive period must be
+ *			set regardless of power scheme or current power state.
+ *			FW use this value also when PM is disabled.
  * @rx_data_timeout:    Minimum time (usec) from last Rx packet for AM to
  *			PSM transition - legacy PM
  * @tx_data_timeout:    Minimum time (usec) from last Tx packet for AM to
  *			PSM transition - legacy PM
- * @rx_data_timeout_uapsd: Minimum time (usec) from last Rx packet for AM to
- *			PSM transition - uAPSD
- * @tx_data_timeout_uapsd: Minimum time (usec) from last Tx packet for AM to
- *			PSM transition - uAPSD
+ * @sleep_interval:	not in use
+ * @keep_alive_beacons:	not in use
  * @lprx_rssi_threshold: Signal strength up to which LP RX can be enabled.
  *			Default: 80dbm
- * @num_skip_dtim:      Number of DTIMs to skip if Skip over DTIM flag is set
- * @snooze_interval:    TBD
- * @snooze_window:      TBD
- * @snooze_step:        TBD
- * @qndp_tid:           TBD
- * @uapsd_ac_flags:     TBD
- * @uapsd_max_sp:       TBD
  */
 struct iwl_powertable_cmd {
-	/* COMMON_INDEX_HDR_API_S_VER_1 */
-	__le32 id_and_color;
-	__le32 action;
+	/* PM_POWER_TABLE_CMD_API_S_VER_5 */
 	__le16 flags;
-	u8 reserved;
-	__le16 keep_alive_seconds;
+	u8 keep_alive_seconds;
+	u8 debug_flags;
 	__le32 rx_data_timeout;
 	__le32 tx_data_timeout;
-	__le32 rx_data_timeout_uapsd;
-	__le32 tx_data_timeout_uapsd;
-	u8 lprx_rssi_threshold;
-	u8 num_skip_dtim;
-	__le16 snooze_interval;
-	__le16 snooze_window;
-	u8 snooze_step;
-	u8 qndp_tid;
-	u8 uapsd_ac_flags;
-	u8 uapsd_max_sp;
+	__le32 sleep_interval[IWL_POWER_VEC_SIZE];
+	__le32 keep_alive_beacons;
+	__le32 lprx_rssi_threshold;
 } __packed;
 
 #endif
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-rs.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-rs.h
index aa3474d08231..fdd33bc0a594 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-rs.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-rs.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
index 670ac8f95e26..b60d14151721 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h
index 0acb53dda22d..a30691a8a85b 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-tx.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-tx.h
index 2677914bf0a6..007a93b25bd7 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-tx.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-tx.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -537,6 +537,12 @@ struct iwl_mac_beacon_cmd {
 	struct ieee80211_hdr frame[0];
 } __packed;
 
+struct iwl_beacon_notif {
+	struct iwl_mvm_tx_resp beacon_notify_hdr;
+	__le64 tsf;
+	__le32 ibss_mgr_status;
+} __packed;
+
 /**
  * enum iwl_dump_control - dump (flush) control flags
  * @DUMP_TX_FIFO_FLUSH: Dump MSDUs until the the FIFO is empty
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api.h b/drivers/net/wireless/iwlwifi/mvm/fw-api.h
index 2adb61f103f4..191dcae8ba47 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -70,6 +70,7 @@
 #include "fw-api-mac.h"
 #include "fw-api-power.h"
 #include "fw-api-d3.h"
+#include "fw-api-bt-coex.h"
 
 /* queue and FIFO numbers by usage */
 enum {
@@ -150,8 +151,10 @@ enum {
 
 	SET_CALIB_DEFAULT_CMD = 0x8e,
 
+	BEACON_NOTIFICATION = 0x90,
 	BEACON_TEMPLATE_CMD = 0x91,
 	TX_ANT_CONFIGURATION_CMD = 0x98,
+	BT_CONFIG = 0x9b,
 	STATISTICS_NOTIFICATION = 0x9d,
 
 	/* RF-KILL commands and notifications */
@@ -162,6 +165,11 @@ enum {
 	REPLY_RX_MPDU_CMD = 0xc1,
 	BA_NOTIF = 0xc5,
 
+	/* BT Coex */
+	BT_COEX_PRIO_TABLE = 0xcc,
+	BT_COEX_PROT_ENV = 0xcd,
+	BT_PROFILE_NOTIFICATION = 0xce,
+
 	REPLY_DEBUG_CMD = 0xf0,
 	DEBUG_LOG_MSG = 0xf7,
 
@@ -271,38 +279,7 @@ enum {
 	NVM_ACCESS_TARGET_EEPROM = 2,
 };
 
-/**
- * struct iwl_nvm_access_cmd_ver1 - Request the device to send the NVM.
- * @op_code: 0 - read, 1 - write.
- * @target: NVM_ACCESS_TARGET_*. should be 0 for read.
- * @cache_refresh: 0 - None, 1- NVM.
- * @offset: offset in the nvm data.
- * @length: of the chunk.
- * @data: empty on read, the NVM chunk on write
- */
-struct iwl_nvm_access_cmd_ver1 {
-	u8 op_code;
-	u8 target;
-	u8 cache_refresh;
-	u8 reserved;
-	__le16 offset;
-	__le16 length;
-	u8 data[];
-} __packed; /* NVM_ACCESS_CMD_API_S_VER_1 */
-
-/**
- * struct iwl_nvm_access_resp_ver1 - response to NVM_ACCESS_CMD
- * @offset: the offset in the nvm data
- * @length: of the chunk
- * @data: the nvm chunk on when NVM_ACCESS_CMD was read, nothing on write
- */
-struct iwl_nvm_access_resp_ver1 {
-	__le16 offset;
-	__le16 length;
-	u8 data[];
-} __packed; /* NVM_ACCESS_CMD_RESP_API_S_VER_1 */
-
-/* Section types for NVM_ACCESS_CMD version 2 */
+/* Section types for NVM_ACCESS_CMD */
 enum {
 	NVM_SECTION_TYPE_HW = 0,
 	NVM_SECTION_TYPE_SW,
@@ -323,7 +300,7 @@ enum {
  * @length: in bytes, to read/write
  * @data: if write operation, the data to write. On read its empty
  */
-struct iwl_nvm_access_cmd_ver2 {
+struct iwl_nvm_access_cmd {
 	u8 op_code;
 	u8 target;
 	__le16 type;
@@ -340,7 +317,7 @@ struct iwl_nvm_access_cmd_ver2 {
  * @status: 0 for success, fail otherwise
  * @data: if read operation, the data returned. Empty on write.
  */
-struct iwl_nvm_access_resp_ver2 {
+struct iwl_nvm_access_resp {
 	__le16 offset;
 	__le16 length;
 	__le16 type;
@@ -503,15 +480,34 @@ enum {
 	TE_DEP_TSF		= 2,
 	TE_EVENT_SOCIOPATHIC	= 4,
 }; /* MAC_EVENT_DEPENDENCY_POLICY_API_E_VER_2 */
-
-/* When to send Time Event notifications and to whom (internal = FW) */
+/*
+ * Supported Time event notifications configuration.
+ * A notification (both event and fragment) includes a status indicating weather
+ * the FW was able to schedule the event or not. For fragment start/end
+ * notification the status is always success. There is no start/end fragment
+ * notification for monolithic events.
+ *
+ * @TE_NOTIF_NONE: no notifications
+ * @TE_NOTIF_HOST_EVENT_START: request/receive notification on event start
+ * @TE_NOTIF_HOST_EVENT_END:request/receive notification on event end
+ * @TE_NOTIF_INTERNAL_EVENT_START: internal FW use
+ * @TE_NOTIF_INTERNAL_EVENT_END: internal FW use.
+ * @TE_NOTIF_HOST_FRAG_START: request/receive notification on frag start
+ * @TE_NOTIF_HOST_FRAG_END:request/receive notification on frag end
+ * @TE_NOTIF_INTERNAL_FRAG_START: internal FW use.
+ * @TE_NOTIF_INTERNAL_FRAG_END: internal FW use.
+ */
 enum {
 	TE_NOTIF_NONE = 0,
-	TE_NOTIF_HOST_START = 0x1,
-	TE_NOTIF_HOST_END = 0x2,
-	TE_NOTIF_INTERNAL_START = 0x4,
-	TE_NOTIF_INTERNAL_END = 0x8
-}; /* MAC_EVENT_ACTION_API_E_VER_1 */
+	TE_NOTIF_HOST_EVENT_START = 0x1,
+	TE_NOTIF_HOST_EVENT_END = 0x2,
+	TE_NOTIF_INTERNAL_EVENT_START = 0x4,
+	TE_NOTIF_INTERNAL_EVENT_END = 0x8,
+	TE_NOTIF_HOST_FRAG_START = 0x10,
+	TE_NOTIF_HOST_FRAG_END = 0x20,
+	TE_NOTIF_INTERNAL_FRAG_START = 0x40,
+	TE_NOTIF_INTERNAL_FRAG_END = 0x80
+}; /* MAC_EVENT_ACTION_API_E_VER_2 */
 
 /*
  * @TE_FRAG_NONE: fragmentation of the time event is NOT allowed.
@@ -794,6 +790,7 @@ struct iwl_phy_context_cmd {
  * @byte_count: frame's byte-count
  * @frame_time: frame's time on the air, based on byte count and frame rate
  *	calculation
+ * @mac_active_msk: what MACs were active when the frame was received
  *
  * Before each Rx, the device sends this data. It contains PHY information
  * about the reception of the packet.
@@ -811,7 +808,7 @@ struct iwl_rx_phy_info {
 	__le32 non_cfg_phy[IWL_RX_INFO_PHY_CNT];
 	__le32 rate_n_flags;
 	__le32 byte_count;
-	__le16 reserved2;
+	__le16 mac_active_msk;
 	__le16 frame_time;
 } __packed;
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw.c b/drivers/net/wireless/iwlwifi/mvm/fw.c
index 500f818dba04..e18c92dd60ec 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw.c
+++ b/drivers/net/wireless/iwlwifi/mvm/fw.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -114,7 +114,7 @@ static int iwl_send_tx_ant_cfg(struct iwl_mvm *mvm, u8 valid_tx_ant)
 		.valid = cpu_to_le32(valid_tx_ant),
 	};
 
-	IWL_DEBUG_HC(mvm, "select valid tx ant: %u\n", valid_tx_ant);
+	IWL_DEBUG_FW(mvm, "select valid tx ant: %u\n", valid_tx_ant);
 	return iwl_mvm_send_cmd_pdu(mvm, TX_ANT_CONFIGURATION_CMD, CMD_SYNC,
 				    sizeof(tx_ant_cmd), &tx_ant_cmd);
 }
@@ -134,9 +134,10 @@ static bool iwl_alive_fn(struct iwl_notif_wait_data *notif_wait,
 	alive_data->scd_base_addr = le32_to_cpu(palive->scd_base_ptr);
 
 	alive_data->valid = le16_to_cpu(palive->status) == IWL_ALIVE_STATUS_OK;
-	IWL_DEBUG_FW(mvm, "Alive ucode status 0x%04x revision 0x%01X 0x%01X\n",
+	IWL_DEBUG_FW(mvm,
+		     "Alive ucode status 0x%04x revision 0x%01X 0x%01X flags 0x%01X\n",
 		     le16_to_cpu(palive->status), palive->ver_type,
-		     palive->ver_subtype);
+		     palive->ver_subtype, palive->flags);
 
 	return true;
 }
@@ -309,6 +310,10 @@ int iwl_run_init_mvm_ucode(struct iwl_mvm *mvm, bool read_nvm)
 		goto error;
 	}
 
+	ret = iwl_send_bt_prio_tbl(mvm);
+	if (ret)
+		goto error;
+
 	if (read_nvm) {
 		/* Read nvm */
 		ret = iwl_nvm_init(mvm);
@@ -322,16 +327,14 @@ int iwl_run_init_mvm_ucode(struct iwl_mvm *mvm, bool read_nvm)
 	WARN_ON(ret);
 
 	/* Send TX valid antennas before triggering calibrations */
-	ret = iwl_send_tx_ant_cfg(mvm, mvm->nvm_data->valid_tx_ant);
+	ret = iwl_send_tx_ant_cfg(mvm, iwl_fw_valid_tx_ant(mvm->fw));
 	if (ret)
 		goto error;
 
-	/* WkP doesn't have all calibrations, need to set default values */
-	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_7000) {
-		ret = iwl_set_default_calibrations(mvm);
-		if (ret)
-			goto error;
-	}
+	/* need to set default values */
+	ret = iwl_set_default_calibrations(mvm);
+	if (ret)
+		goto error;
 
 	/*
 	 * Send phy configurations command to init uCode
@@ -410,7 +413,15 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
 		goto error;
 	}
 
-	ret = iwl_send_tx_ant_cfg(mvm, mvm->nvm_data->valid_tx_ant);
+	ret = iwl_send_tx_ant_cfg(mvm, iwl_fw_valid_tx_ant(mvm->fw));
+	if (ret)
+		goto error;
+
+	ret = iwl_send_bt_prio_tbl(mvm);
+	if (ret)
+		goto error;
+
+	ret = iwl_send_bt_init_conf(mvm);
 	if (ret)
 		goto error;
 
@@ -456,7 +467,7 @@ int iwl_mvm_load_d3_fw(struct iwl_mvm *mvm)
 		goto error;
 	}
 
-	ret = iwl_send_tx_ant_cfg(mvm, mvm->nvm_data->valid_tx_ant);
+	ret = iwl_send_tx_ant_cfg(mvm, iwl_fw_valid_tx_ant(mvm->fw));
 	if (ret)
 		goto error;
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/led.c b/drivers/net/wireless/iwlwifi/mvm/led.c
index 011906e73a05..2269a9e5cc67 100644
--- a/drivers/net/wireless/iwlwifi/mvm/led.c
+++ b/drivers/net/wireless/iwlwifi/mvm/led.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
index 341dbc0237ea..e6eca4d66f6c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -196,7 +196,7 @@ u32 iwl_mvm_mac_get_queues_mask(struct iwl_mvm *mvm,
 	u32 qmask, ac;
 
 	if (vif->type == NL80211_IFTYPE_P2P_DEVICE)
-		return BIT(IWL_OFFCHANNEL_QUEUE);
+		return BIT(IWL_MVM_OFFCHANNEL_QUEUE);
 
 	qmask = (vif->cab_queue != IEEE80211_INVAL_HW_QUEUE) ?
 		BIT(vif->cab_queue) : 0;
@@ -553,9 +553,9 @@ static void iwl_mvm_mac_ctxt_cmd_common(struct iwl_mvm *mvm,
 	if (vif->bss_conf.qos)
 		cmd->qos_flags |= cpu_to_le32(MAC_QOS_FLG_UPDATE_EDCA);
 
+	/* Don't use cts to self as the fw doesn't support it currently. */
 	if (vif->bss_conf.use_cts_prot)
-		cmd->protection_flags |= cpu_to_le32(MAC_PROT_FLG_TGG_PROTECT |
-						     MAC_PROT_FLG_SELF_CTS_EN);
+		cmd->protection_flags |= cpu_to_le32(MAC_PROT_FLG_TGG_PROTECT);
 
 	/*
 	 * I think that we should enable these 2 flags regardless the HT PROT
@@ -651,6 +651,13 @@ static int iwl_mvm_mac_ctxt_cmd_station(struct iwl_mvm *mvm,
 	/* Fill the common data for all mac context types */
 	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, action);
 
+	/* Allow beacons to pass through as long as we are not associated,or we
+	 * do not have dtim period information */
+	if (!vif->bss_conf.assoc || !vif->bss_conf.dtim_period)
+		cmd.filter_flags |= cpu_to_le32(MAC_FILTER_IN_BEACON);
+	else
+		cmd.filter_flags &= ~cpu_to_le32(MAC_FILTER_IN_BEACON);
+
 	/* Fill the data specific for station mode */
 	iwl_mvm_mac_ctxt_cmd_fill_sta(mvm, vif, &cmd.sta);
 
@@ -662,6 +669,7 @@ static int iwl_mvm_mac_ctxt_cmd_p2p_client(struct iwl_mvm *mvm,
 					   u32 action)
 {
 	struct iwl_mac_ctx_cmd cmd = {};
+	struct ieee80211_p2p_noa_attr *noa = &vif->bss_conf.p2p_noa_attr;
 
 	WARN_ON(vif->type != NL80211_IFTYPE_STATION || !vif->p2p);
 
@@ -671,7 +679,8 @@ static int iwl_mvm_mac_ctxt_cmd_p2p_client(struct iwl_mvm *mvm,
 	/* Fill the data specific for station mode */
 	iwl_mvm_mac_ctxt_cmd_fill_sta(mvm, vif, &cmd.p2p_sta.sta);
 
-	cmd.p2p_sta.ctwin = cpu_to_le32(vif->bss_conf.p2p_ctwindow);
+	cmd.p2p_sta.ctwin = cpu_to_le32(noa->oppps_ctwindow &
+					IEEE80211_P2P_OPPPS_CTWINDOW_MASK);
 
 	return iwl_mvm_mac_ctxt_send_cmd(mvm, &cmd);
 }
@@ -685,7 +694,12 @@ static int iwl_mvm_mac_ctxt_cmd_listener(struct iwl_mvm *mvm,
 	WARN_ON(vif->type != NL80211_IFTYPE_MONITOR);
 
 	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, action);
-	/* No other data to be filled */
+
+	cmd.filter_flags = cpu_to_le32(MAC_FILTER_IN_PROMISC |
+				       MAC_FILTER_IN_CONTROL_AND_MGMT |
+				       MAC_FILTER_IN_BEACON |
+				       MAC_FILTER_IN_PROBE_REQUEST);
+
 	return iwl_mvm_mac_ctxt_send_cmd(mvm, &cmd);
 }
 
@@ -714,7 +728,9 @@ static int iwl_mvm_mac_ctxt_cmd_p2p_device(struct iwl_mvm *mvm,
 	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, action);
 
 	cmd.protection_flags |= cpu_to_le32(MAC_PROT_FLG_TGG_PROTECT);
-	cmd.filter_flags |= cpu_to_le32(MAC_FILTER_IN_PROMISC);
+
+	/* Override the filter flags to accept only probe requests */
+	cmd.filter_flags = cpu_to_le32(MAC_FILTER_IN_PROBE_REQUEST);
 
 	/*
 	 * This flag should be set to true when the P2P Device is
@@ -789,7 +805,7 @@ static int iwl_mvm_mac_ctxt_send_beacon(struct iwl_mvm *mvm,
 					     TX_CMD_FLG_TSF);
 
 	mvm->mgmt_last_antenna_idx =
-		iwl_mvm_next_antenna(mvm, mvm->nvm_data->valid_tx_ant,
+		iwl_mvm_next_antenna(mvm, iwl_fw_valid_tx_ant(mvm->fw),
 				     mvm->mgmt_last_antenna_idx);
 
 	beacon_cmd.tx.rate_n_flags =
@@ -846,10 +862,10 @@ int iwl_mvm_mac_ctxt_beacon_changed(struct iwl_mvm *mvm,
  */
 static void iwl_mvm_mac_ctxt_cmd_fill_ap(struct iwl_mvm *mvm,
 					 struct ieee80211_vif *vif,
-					 struct iwl_mac_data_ap *ctxt_ap)
+					 struct iwl_mac_data_ap *ctxt_ap,
+					 bool add)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
-	u32 curr_dev_time;
 
 	ctxt_ap->bi = cpu_to_le32(vif->bss_conf.beacon_int);
 	ctxt_ap->bi_reciprocal =
@@ -861,10 +877,19 @@ static void iwl_mvm_mac_ctxt_cmd_fill_ap(struct iwl_mvm *mvm,
 					       vif->bss_conf.dtim_period));
 
 	ctxt_ap->mcast_qid = cpu_to_le32(vif->cab_queue);
-	curr_dev_time = iwl_read_prph(mvm->trans, DEVICE_SYSTEM_TIME_REG);
-	ctxt_ap->beacon_time = cpu_to_le32(curr_dev_time);
 
-	ctxt_ap->beacon_tsf = cpu_to_le64(curr_dev_time);
+	/*
+	 * Only read the system time when the MAC is being added, when we
+	 * just modify the MAC then we should keep the time -- the firmware
+	 * can otherwise have a "jumping" TBTT.
+	 */
+	if (add)
+		mvmvif->ap_beacon_time =
+			iwl_read_prph(mvm->trans, DEVICE_SYSTEM_TIME_REG);
+
+	ctxt_ap->beacon_time = cpu_to_le32(mvmvif->ap_beacon_time);
+
+	ctxt_ap->beacon_tsf = 0; /* unused */
 
 	/* TODO: Assume that the beacon id == mac context id */
 	ctxt_ap->beacon_template = cpu_to_le32(mvmvif->id);
@@ -881,8 +906,12 @@ static int iwl_mvm_mac_ctxt_cmd_ap(struct iwl_mvm *mvm,
 	/* Fill the common data for all mac context types */
 	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, action);
 
+	/* Also enable probe requests to pass */
+	cmd.filter_flags |= cpu_to_le32(MAC_FILTER_IN_PROBE_REQUEST);
+
 	/* Fill the data specific for ap mode */
-	iwl_mvm_mac_ctxt_cmd_fill_ap(mvm, vif, &cmd.ap);
+	iwl_mvm_mac_ctxt_cmd_fill_ap(mvm, vif, &cmd.ap,
+				     action == FW_CTXT_ACTION_ADD);
 
 	return iwl_mvm_mac_ctxt_send_cmd(mvm, &cmd);
 }
@@ -892,6 +921,7 @@ static int iwl_mvm_mac_ctxt_cmd_go(struct iwl_mvm *mvm,
 				   u32 action)
 {
 	struct iwl_mac_ctx_cmd cmd = {};
+	struct ieee80211_p2p_noa_attr *noa = &vif->bss_conf.p2p_noa_attr;
 
 	WARN_ON(vif->type != NL80211_IFTYPE_AP || !vif->p2p);
 
@@ -899,10 +929,14 @@ static int iwl_mvm_mac_ctxt_cmd_go(struct iwl_mvm *mvm,
 	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, action);
 
 	/* Fill the data specific for GO mode */
-	iwl_mvm_mac_ctxt_cmd_fill_ap(mvm, vif, &cmd.go.ap);
+	iwl_mvm_mac_ctxt_cmd_fill_ap(mvm, vif, &cmd.go.ap,
+				     action == FW_CTXT_ACTION_ADD);
 
-	cmd.go.ctwin = cpu_to_le32(vif->bss_conf.p2p_ctwindow);
-	cmd.go.opp_ps_enabled = cpu_to_le32(!!vif->bss_conf.p2p_oppps);
+	cmd.go.ctwin = cpu_to_le32(noa->oppps_ctwindow &
+					IEEE80211_P2P_OPPPS_CTWINDOW_MASK);
+	cmd.go.opp_ps_enabled =
+			cpu_to_le32(!!(noa->oppps_ctwindow &
+					IEEE80211_P2P_OPPPS_ENABLE_BIT));
 
 	return iwl_mvm_mac_ctxt_send_cmd(mvm, &cmd);
 }
@@ -990,3 +1024,22 @@ int iwl_mvm_mac_ctxt_remove(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 	mvmvif->uploaded = false;
 	return 0;
 }
+
+int iwl_mvm_rx_beacon_notif(struct iwl_mvm *mvm,
+			    struct iwl_rx_cmd_buffer *rxb,
+			    struct iwl_device_cmd *cmd)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_beacon_notif *beacon = (void *)pkt->data;
+	u16 status __maybe_unused =
+		le16_to_cpu(beacon->beacon_notify_hdr.status.status);
+	u32 rate __maybe_unused =
+		le32_to_cpu(beacon->beacon_notify_hdr.initial_rate);
+
+	IWL_DEBUG_RX(mvm, "beacon status %#x retries:%d tsf:0x%16llX rate:%d\n",
+		     status & TX_STATUS_MSK,
+		     beacon->beacon_notify_hdr.failure_frame,
+		     le64_to_cpu(beacon->tsf),
+		     rate);
+	return 0;
+}
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac80211.c b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
index 7e169b085afe..dd158ec571fb 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -65,7 +65,9 @@
 #include <linux/skbuff.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
+#include <linux/ip.h>
 #include <net/mac80211.h>
+#include <net/tcp.h>
 
 #include "iwl-op-mode.h"
 #include "iwl-io.h"
@@ -102,10 +104,33 @@ static const struct ieee80211_iface_combination iwl_mvm_iface_combinations[] = {
 	},
 };
 
+#ifdef CONFIG_PM_SLEEP
+static const struct nl80211_wowlan_tcp_data_token_feature
+iwl_mvm_wowlan_tcp_token_feature = {
+	.min_len = 0,
+	.max_len = 255,
+	.bufsize = IWL_WOWLAN_REMOTE_WAKE_MAX_TOKENS,
+};
+
+static const struct wiphy_wowlan_tcp_support iwl_mvm_wowlan_tcp_support = {
+	.tok = &iwl_mvm_wowlan_tcp_token_feature,
+	.data_payload_max = IWL_WOWLAN_TCP_MAX_PACKET_LEN -
+			    sizeof(struct ethhdr) -
+			    sizeof(struct iphdr) -
+			    sizeof(struct tcphdr),
+	.data_interval_max = 65535, /* __le16 in API */
+	.wake_payload_max = IWL_WOWLAN_REMOTE_WAKE_MAX_PACKET_LEN -
+			    sizeof(struct ethhdr) -
+			    sizeof(struct iphdr) -
+			    sizeof(struct tcphdr),
+	.seq = true,
+};
+#endif
+
 int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 {
 	struct ieee80211_hw *hw = mvm->hw;
-	int num_mac, ret;
+	int num_mac, ret, i;
 
 	/* Tell mac80211 our characteristics */
 	hw->flags = IEEE80211_HW_SIGNAL_DBM |
@@ -118,8 +143,8 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 		    IEEE80211_HW_AMPDU_AGGREGATION |
 		    IEEE80211_HW_TIMING_BEACON_ONLY;
 
-	hw->queues = IWL_FIRST_AMPDU_QUEUE;
-	hw->offchannel_tx_hw_queue = IWL_OFFCHANNEL_QUEUE;
+	hw->queues = IWL_MVM_FIRST_AGG_QUEUE;
+	hw->offchannel_tx_hw_queue = IWL_MVM_OFFCHANNEL_QUEUE;
 	hw->rate_control_algorithm = "iwl-mvm-rs";
 
 	/*
@@ -149,18 +174,22 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 	hw->wiphy->n_iface_combinations =
 		ARRAY_SIZE(iwl_mvm_iface_combinations);
 
-	hw->wiphy->max_remain_on_channel_duration = 500;
+	hw->wiphy->max_remain_on_channel_duration = 10000;
 	hw->max_listen_interval = IWL_CONN_MAX_LISTEN_INTERVAL;
 
 	/* Extract MAC address */
 	memcpy(mvm->addresses[0].addr, mvm->nvm_data->hw_addr, ETH_ALEN);
 	hw->wiphy->addresses = mvm->addresses;
 	hw->wiphy->n_addresses = 1;
-	num_mac = mvm->nvm_data->n_hw_addrs;
-	if (num_mac > 1) {
-		memcpy(mvm->addresses[1].addr, mvm->addresses[0].addr,
+
+	/* Extract additional MAC addresses if available */
+	num_mac = (mvm->nvm_data->n_hw_addrs > 1) ?
+		min(IWL_MVM_MAX_ADDRESSES, mvm->nvm_data->n_hw_addrs) : 1;
+
+	for (i = 1; i < num_mac; i++) {
+		memcpy(mvm->addresses[i].addr, mvm->addresses[i-1].addr,
 		       ETH_ALEN);
-		mvm->addresses[1].addr[5]++;
+		mvm->addresses[i].addr[5]++;
 		hw->wiphy->n_addresses++;
 	}
 
@@ -178,7 +207,7 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 
 	hw->wiphy->hw_version = mvm->trans->hw_id;
 
-	if (iwlwifi_mod_params.power_save)
+	if (iwlmvm_mod_params.power_scheme != IWL_POWER_SCHEME_CAM)
 		hw->wiphy->flags |= WIPHY_FLAG_PS_ON_BY_DEFAULT;
 	else
 		hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
@@ -206,6 +235,7 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 		hw->wiphy->wowlan.n_patterns = IWL_WOWLAN_MAX_PATTERNS;
 		hw->wiphy->wowlan.pattern_min_len = IWL_WOWLAN_MIN_PATTERN_LEN;
 		hw->wiphy->wowlan.pattern_max_len = IWL_WOWLAN_MAX_PATTERN_LEN;
+		hw->wiphy->wowlan.tcp = &iwl_mvm_wowlan_tcp_support;
 	}
 #endif
 
@@ -227,7 +257,7 @@ static void iwl_mvm_mac_tx(struct ieee80211_hw *hw,
 		goto drop;
 	}
 
-	if (IEEE80211_SKB_CB(skb)->hw_queue == IWL_OFFCHANNEL_QUEUE &&
+	if (IEEE80211_SKB_CB(skb)->hw_queue == IWL_MVM_OFFCHANNEL_QUEUE &&
 	    !test_bit(IWL_MVM_STATUS_ROC_RUNNING, &mvm->status))
 		goto drop;
 
@@ -273,12 +303,18 @@ static int iwl_mvm_mac_ampdu_action(struct ieee80211_hw *hw,
 		ret = iwl_mvm_sta_rx_agg(mvm, sta, tid, 0, false);
 		break;
 	case IEEE80211_AMPDU_TX_START:
+		if (iwlwifi_mod_params.disable_11n & IWL_DISABLE_HT_TXAGG) {
+			ret = -EINVAL;
+			break;
+		}
 		ret = iwl_mvm_sta_tx_agg_start(mvm, vif, sta, tid, ssn);
 		break;
 	case IEEE80211_AMPDU_TX_STOP_CONT:
+		ret = iwl_mvm_sta_tx_agg_stop(mvm, vif, sta, tid);
+		break;
 	case IEEE80211_AMPDU_TX_STOP_FLUSH:
 	case IEEE80211_AMPDU_TX_STOP_FLUSH_CONT:
-		ret = iwl_mvm_sta_tx_agg_stop(mvm, vif, sta, tid);
+		ret = iwl_mvm_sta_tx_agg_flush(mvm, vif, sta, tid);
 		break;
 	case IEEE80211_AMPDU_TX_OPERATIONAL:
 		ret = iwl_mvm_sta_tx_agg_oper(mvm, vif, sta, tid, buf_size);
@@ -466,11 +502,15 @@ static int iwl_mvm_mac_add_interface(struct ieee80211_hw *hw,
 	/*
 	 * TODO: remove this temporary code.
 	 * Currently MVM FW supports power management only on single MAC.
-	 * Iterate and disable PM on all active interfaces.
+	 * If new interface added, disable PM on existing interface.
+	 * P2P device is a special case, since it is handled by FW similary to
+	 * scan. If P2P deviced is added, PM remains enabled on existing
+	 * interface.
 	 * Note: the method below does not count the new interface being added
 	 * at this moment.
 	 */
-	mvm->vif_count++;
+	if (vif->type != NL80211_IFTYPE_P2P_DEVICE)
+		mvm->vif_count++;
 	if (mvm->vif_count > 1) {
 		IWL_DEBUG_MAC80211(mvm,
 				   "Disable power on existing interfaces\n");
@@ -526,6 +566,7 @@ static int iwl_mvm_mac_add_interface(struct ieee80211_hw *hw,
 		mvm->p2p_device_vif = vif;
 	}
 
+	iwl_mvm_vif_dbgfs_register(mvm, vif);
 	goto out_unlock;
 
  out_unbind:
@@ -539,10 +580,11 @@ static int iwl_mvm_mac_add_interface(struct ieee80211_hw *hw,
 	/*
 	 * TODO: remove this temporary code.
 	 * Currently MVM FW supports power management only on single MAC.
-	 * Check if only one additional interface remains after rereasing
+	 * Check if only one additional interface remains after releasing
 	 * current one. Update power mode on the remaining interface.
 	 */
-	mvm->vif_count--;
+	if (vif->type != NL80211_IFTYPE_P2P_DEVICE)
+		mvm->vif_count--;
 	IWL_DEBUG_MAC80211(mvm, "Currently %d interfaces active\n",
 			   mvm->vif_count);
 	if (mvm->vif_count == 1) {
@@ -604,6 +646,8 @@ static void iwl_mvm_mac_remove_interface(struct ieee80211_hw *hw,
 
 	mutex_lock(&mvm->mutex);
 
+	iwl_mvm_vif_dbgfs_clean(mvm, vif);
+
 	/*
 	 * For AP/GO interface, the tear down of the resources allocated to the
 	 * interface is be handled as part of the stop_ap flow.
@@ -627,7 +671,7 @@ static void iwl_mvm_mac_remove_interface(struct ieee80211_hw *hw,
 	 * Check if only one additional interface remains after removing
 	 * current one. Update power mode on the remaining interface.
 	 */
-	if (mvm->vif_count)
+	if (mvm->vif_count && vif->type != NL80211_IFTYPE_P2P_DEVICE)
 		mvm->vif_count--;
 	IWL_DEBUG_MAC80211(mvm, "Currently %d interfaces active\n",
 			   mvm->vif_count);
@@ -677,6 +721,7 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 				IWL_ERR(mvm, "failed to update quotas\n");
 				return;
 			}
+			iwl_mvm_bt_coex_vif_assoc(mvm, vif);
 		} else if (mvmvif->ap_sta_id != IWL_MVM_STATION_COUNT) {
 			/* remove AP station now that the MAC is unassoc */
 			ret = iwl_mvm_rm_sta_id(mvm, vif, mvmvif->ap_sta_id);
@@ -895,7 +940,7 @@ static void iwl_mvm_mac_sta_notify(struct ieee80211_hw *hw,
 		 */
 		break;
 	case STA_NOTIFY_AWAKE:
-		if (WARN_ON(mvmsta->sta_id == IWL_INVALID_STATION))
+		if (WARN_ON(mvmsta->sta_id == IWL_MVM_STATION_COUNT))
 			break;
 		iwl_mvm_sta_modify_ps_wake(mvm, sta);
 		break;
@@ -1051,6 +1096,13 @@ static int iwl_mvm_mac_set_key(struct ieee80211_hw *hw,
 
 	switch (cmd) {
 	case SET_KEY:
+		if (vif->type == NL80211_IFTYPE_AP && !sta) {
+			/* GTK on AP interface is a TX-only key, return 0 */
+			ret = 0;
+			key->hw_key_idx = STA_KEY_IDX_INVALID;
+			break;
+		}
+
 		IWL_DEBUG_MAC80211(mvm, "set hwcrypto key\n");
 		ret = iwl_mvm_set_sta_key(mvm, vif, sta, key, false);
 		if (ret) {
@@ -1059,11 +1111,17 @@ static int iwl_mvm_mac_set_key(struct ieee80211_hw *hw,
 			 * can't add key for RX, but we don't need it
 			 * in the device for TX so still return 0
 			 */
+			key->hw_key_idx = STA_KEY_IDX_INVALID;
 			ret = 0;
 		}
 
 		break;
 	case DISABLE_KEY:
+		if (key->hw_key_idx == STA_KEY_IDX_INVALID) {
+			ret = 0;
+			break;
+		}
+
 		IWL_DEBUG_MAC80211(mvm, "disable hwcrypto key\n");
 		ret = iwl_mvm_remove_sta_key(mvm, vif, sta, key);
 		break;
@@ -1090,7 +1148,8 @@ static void iwl_mvm_mac_update_tkip_key(struct ieee80211_hw *hw,
 static int iwl_mvm_roc(struct ieee80211_hw *hw,
 		       struct ieee80211_vif *vif,
 		       struct ieee80211_channel *channel,
-		       int duration)
+		       int duration,
+		       enum ieee80211_roc_type type)
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 	struct cfg80211_chan_def chandef;
@@ -1101,8 +1160,8 @@ static int iwl_mvm_roc(struct ieee80211_hw *hw,
 		return -EINVAL;
 	}
 
-	IWL_DEBUG_MAC80211(mvm, "enter (%d, %d)\n", channel->hw_value,
-			   duration);
+	IWL_DEBUG_MAC80211(mvm, "enter (%d, %d, %d)\n", channel->hw_value,
+			   duration, type);
 
 	mutex_lock(&mvm->mutex);
 
@@ -1111,7 +1170,7 @@ static int iwl_mvm_roc(struct ieee80211_hw *hw,
 				       &chandef, 1, 1);
 
 	/* Schedule the time events */
-	ret = iwl_mvm_start_p2p_roc(mvm, vif, duration);
+	ret = iwl_mvm_start_p2p_roc(mvm, vif, duration, type);
 
 	mutex_unlock(&mvm->mutex);
 	IWL_DEBUG_MAC80211(mvm, "leave\n");
@@ -1215,6 +1274,7 @@ static int iwl_mvm_assign_vif_chanctx(struct ieee80211_hw *hw,
 	 * will handle quota settings.
 	 */
 	if (vif->type == NL80211_IFTYPE_MONITOR) {
+		mvmvif->monitor_active = true;
 		ret = iwl_mvm_update_quotas(mvm, vif);
 		if (ret)
 			goto out_remove_binding;
@@ -1245,15 +1305,16 @@ static void iwl_mvm_unassign_vif_chanctx(struct ieee80211_hw *hw,
 	if (vif->type == NL80211_IFTYPE_AP)
 		goto out_unlock;
 
-	iwl_mvm_binding_remove_vif(mvm, vif);
 	switch (vif->type) {
 	case NL80211_IFTYPE_MONITOR:
-		iwl_mvm_update_quotas(mvm, vif);
+		mvmvif->monitor_active = false;
+		iwl_mvm_update_quotas(mvm, NULL);
 		break;
 	default:
 		break;
 	}
 
+	iwl_mvm_binding_remove_vif(mvm, vif);
 out_unlock:
 	mvmvif->phy_ctxt = NULL;
 	mutex_unlock(&mvm->mutex);
@@ -1274,6 +1335,15 @@ static int iwl_mvm_set_tim(struct ieee80211_hw *hw,
 	return iwl_mvm_mac_ctxt_beacon_changed(mvm, mvm_sta->vif);
 }
 
+static void iwl_mvm_mac_rssi_callback(struct ieee80211_hw *hw,
+				      struct ieee80211_vif *vif,
+				      enum ieee80211_rssi_event rssi_event)
+{
+	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+
+	iwl_mvm_bt_rssi_event(mvm, vif, rssi_event);
+}
+
 struct ieee80211_ops iwl_mvm_hw_ops = {
 	.tx = iwl_mvm_mac_tx,
 	.ampdu_action = iwl_mvm_mac_ampdu_action,
@@ -1297,6 +1367,7 @@ struct ieee80211_ops iwl_mvm_hw_ops = {
 	.update_tkip_key = iwl_mvm_mac_update_tkip_key,
 	.remain_on_channel = iwl_mvm_roc,
 	.cancel_remain_on_channel = iwl_mvm_cancel_roc,
+	.rssi_callback = iwl_mvm_mac_rssi_callback,
 
 	.add_chanctx = iwl_mvm_add_chanctx,
 	.remove_chanctx = iwl_mvm_remove_chanctx,
diff --git a/drivers/net/wireless/iwlwifi/mvm/mvm.h b/drivers/net/wireless/iwlwifi/mvm/mvm.h
index bdae700c769e..8269bc562951 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mvm.h
+++ b/drivers/net/wireless/iwlwifi/mvm/mvm.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -79,7 +79,7 @@
 #include "fw-api.h"
 
 #define IWL_INVALID_MAC80211_QUEUE	0xff
-#define IWL_MVM_MAX_ADDRESSES		2
+#define IWL_MVM_MAX_ADDRESSES		5
 /* RSSI offset for WkP */
 #define IWL_RSSI_OFFSET 50
 
@@ -90,10 +90,6 @@ enum iwl_mvm_tx_fifo {
 	IWL_MVM_TX_FIFO_VO,
 };
 
-/* Placeholder */
-#define IWL_OFFCHANNEL_QUEUE 8
-#define IWL_FIRST_AMPDU_QUEUE 11
-
 extern struct ieee80211_ops iwl_mvm_hw_ops;
 /**
  * struct iwl_mvm_mod_params - module parameters for iwlmvm
@@ -161,6 +157,8 @@ enum iwl_power_scheme {
  * @uploaded: indicates the MAC context has been added to the device
  * @ap_active: indicates that ap context is configured, and that the interface
  *  should get quota etc.
+ * @monitor_active: indicates that monitor context is configured, and that the
+ * interface should get quota etc.
  * @queue_params: QoS params for this MAC
  * @bcast_sta: station used for broadcast packets. Used by the following
  *  vifs: P2P_DEVICE, GO and AP.
@@ -173,6 +171,9 @@ struct iwl_mvm_vif {
 
 	bool uploaded;
 	bool ap_active;
+	bool monitor_active;
+
+	u32 ap_beacon_time;
 
 	enum iwl_tsf_id tsf_id;
 
@@ -211,6 +212,7 @@ struct iwl_mvm_vif {
 
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 	struct dentry *dbgfs_dir;
+	struct dentry *dbgfs_slink;
 	void *dbgfs_data;
 #endif
 };
@@ -279,10 +281,7 @@ struct iwl_mvm {
 	atomic_t queue_stop_count[IWL_MAX_HW_QUEUES];
 
 	struct iwl_nvm_data *nvm_data;
-	/* eeprom blob for debugfs/testmode */
-	u8 *eeprom_blob;
-	size_t eeprom_blob_size;
-	/* NVM sections for 7000 family */
+	/* NVM sections */
 	struct iwl_nvm_section nvm_sections[NVM_NUM_OF_SECTIONS];
 
 	/* EEPROM MAC addresses */
@@ -323,6 +322,13 @@ struct iwl_mvm {
 	 * can hold 16 keys at most. Reflect this fact.
 	 */
 	unsigned long fw_key_table[BITS_TO_LONGS(STA_KEY_MAX_NUM)];
+
+	/*
+	 * This counter of created interfaces is referenced only in conjunction
+	 * with FW limitation related to power management. Currently PM is
+	 * supported only on a single interface.
+	 * IMPORTANT: this variable counts all interfaces except P2P device.
+	 */
 	u8 vif_count;
 
 	struct led_classdev led;
@@ -332,6 +338,10 @@ struct iwl_mvm {
 #ifdef CONFIG_PM_SLEEP
 	int gtk_ivlen, gtk_icvlen, ptk_ivlen, ptk_icvlen;
 #endif
+
+	/* BT-Coex */
+	u8 bt_kill_msk;
+	struct iwl_bt_coex_profile_notif last_bt_notif;
 };
 
 /* Extract MVM priv from op_mode and _hw */
@@ -445,6 +455,9 @@ u32 iwl_mvm_mac_get_queues_mask(struct iwl_mvm *mvm,
 				struct ieee80211_vif *vif);
 int iwl_mvm_mac_ctxt_beacon_changed(struct iwl_mvm *mvm,
 				    struct ieee80211_vif *vif);
+int iwl_mvm_rx_beacon_notif(struct iwl_mvm *mvm,
+			    struct iwl_rx_cmd_buffer *rxb,
+			    struct iwl_device_cmd *cmd);
 
 /* Bindings */
 int iwl_mvm_binding_add_vif(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
@@ -466,16 +479,22 @@ void iwl_mvm_cancel_scan(struct iwl_mvm *mvm);
 /* MVM debugfs */
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir);
-int iwl_mvm_vif_dbgfs_register(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			       struct dentry *dbgfs_dir);
-void iwl_power_get_params(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			  struct iwl_powertable_cmd *cmd);
+void iwl_mvm_vif_dbgfs_register(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+void iwl_mvm_vif_dbgfs_clean(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
 #else
 static inline int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm,
 					 struct dentry *dbgfs_dir)
 {
 	return 0;
 }
+static inline void
+iwl_mvm_vif_dbgfs_register(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
+{
+}
+static inline void
+iwl_mvm_vif_dbgfs_clean(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
+{
+}
 #endif /* CONFIG_IWLWIFI_DEBUGFS */
 
 /* rate scaling */
@@ -485,6 +504,8 @@ int iwl_mvm_send_lq_cmd(struct iwl_mvm *mvm, struct iwl_lq_cmd *lq,
 /* power managment */
 int iwl_mvm_power_update_mode(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
 int iwl_mvm_power_disable(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+void iwl_mvm_power_build_cmd(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			     struct iwl_powertable_cmd *cmd);
 
 int iwl_mvm_leds_init(struct iwl_mvm *mvm);
 void iwl_mvm_leds_exit(struct iwl_mvm *mvm);
@@ -502,4 +523,14 @@ void iwl_mvm_ipv6_addr_change(struct ieee80211_hw *hw,
 void iwl_mvm_set_default_unicast_key(struct ieee80211_hw *hw,
 				     struct ieee80211_vif *vif, int idx);
 
+/* BT Coex */
+int iwl_send_bt_prio_tbl(struct iwl_mvm *mvm);
+int iwl_send_bt_init_conf(struct iwl_mvm *mvm);
+int iwl_mvm_rx_bt_coex_notif(struct iwl_mvm *mvm,
+			     struct iwl_rx_cmd_buffer *rxb,
+			     struct iwl_device_cmd *cmd);
+void iwl_mvm_bt_rssi_event(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			   enum ieee80211_rssi_event rssi_event);
+void iwl_mvm_bt_coex_vif_assoc(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+
 #endif /* __IWL_MVM_H__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/nvm.c b/drivers/net/wireless/iwlwifi/mvm/nvm.c
index 20016bcbdeab..b8ec02f89acc 100644
--- a/drivers/net/wireless/iwlwifi/mvm/nvm.c
+++ b/drivers/net/wireless/iwlwifi/mvm/nvm.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -74,26 +74,11 @@ static const int nvm_to_read[] = {
 	NVM_SECTION_TYPE_PRODUCTION,
 };
 
-/* used to simplify the shared operations on NCM_ACCESS_CMD versions */
-union iwl_nvm_access_cmd {
-	struct iwl_nvm_access_cmd_ver1 ver1;
-	struct iwl_nvm_access_cmd_ver2 ver2;
-};
-union iwl_nvm_access_resp {
-	struct iwl_nvm_access_resp_ver1 ver1;
-	struct iwl_nvm_access_resp_ver2 ver2;
-};
-
-static inline void iwl_nvm_fill_read_ver1(struct iwl_nvm_access_cmd_ver1 *cmd,
-					  u16 offset, u16 length)
-{
-	cmd->offset = cpu_to_le16(offset);
-	cmd->length = cpu_to_le16(length);
-	cmd->cache_refresh = 1;
-}
+/* Default NVM size to read */
+#define IWL_NVM_DEFAULT_CHUNK_SIZE (2*1024);
 
-static inline void iwl_nvm_fill_read_ver2(struct iwl_nvm_access_cmd_ver2 *cmd,
-					  u16 offset, u16 length, u16 section)
+static inline void iwl_nvm_fill_read(struct iwl_nvm_access_cmd *cmd,
+				     u16 offset, u16 length, u16 section)
 {
 	cmd->offset = cpu_to_le16(offset);
 	cmd->length = cpu_to_le16(length);
@@ -103,8 +88,8 @@ static inline void iwl_nvm_fill_read_ver2(struct iwl_nvm_access_cmd_ver2 *cmd,
 static int iwl_nvm_read_chunk(struct iwl_mvm *mvm, u16 section,
 			      u16 offset, u16 length, u8 *data)
 {
-	union iwl_nvm_access_cmd nvm_access_cmd;
-	union iwl_nvm_access_resp *nvm_resp;
+	struct iwl_nvm_access_cmd nvm_access_cmd = {};
+	struct iwl_nvm_access_resp *nvm_resp;
 	struct iwl_rx_packet *pkt;
 	struct iwl_host_cmd cmd = {
 		.id = NVM_ACCESS_CMD,
@@ -114,18 +99,8 @@ static int iwl_nvm_read_chunk(struct iwl_mvm *mvm, u16 section,
 	int ret, bytes_read, offset_read;
 	u8 *resp_data;
 
-	memset(&nvm_access_cmd, 0, sizeof(nvm_access_cmd));
-
-	/* TODO: not sure family should be the decider, maybe FW version? */
-	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_7000) {
-		iwl_nvm_fill_read_ver2(&(nvm_access_cmd.ver2),
-				       offset, length, section);
-		cmd.len[0] = sizeof(struct iwl_nvm_access_cmd_ver2);
-	} else {
-		iwl_nvm_fill_read_ver1(&(nvm_access_cmd.ver1),
-				       offset, length);
-		cmd.len[0] = sizeof(struct iwl_nvm_access_cmd_ver1);
-	}
+	iwl_nvm_fill_read(&nvm_access_cmd, offset, length, section);
+	cmd.len[0] = sizeof(struct iwl_nvm_access_cmd);
 
 	ret = iwl_mvm_send_cmd(mvm, &cmd);
 	if (ret)
@@ -141,17 +116,10 @@ static int iwl_nvm_read_chunk(struct iwl_mvm *mvm, u16 section,
 
 	/* Extract NVM response */
 	nvm_resp = (void *)pkt->data;
-	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_7000) {
-		ret = le16_to_cpu(nvm_resp->ver2.status);
-		bytes_read = le16_to_cpu(nvm_resp->ver2.length);
-		offset_read = le16_to_cpu(nvm_resp->ver2.offset);
-		resp_data = nvm_resp->ver2.data;
-	} else {
-		ret = le16_to_cpu(nvm_resp->ver1.length) <= 0;
-		bytes_read = le16_to_cpu(nvm_resp->ver1.length);
-		offset_read = le16_to_cpu(nvm_resp->ver1.offset);
-		resp_data = nvm_resp->ver1.data;
-	}
+	ret = le16_to_cpu(nvm_resp->status);
+	bytes_read = le16_to_cpu(nvm_resp->length);
+	offset_read = le16_to_cpu(nvm_resp->offset);
+	resp_data = nvm_resp->data;
 	if (ret) {
 		IWL_ERR(mvm,
 			"NVM access command failed with status %d (device: %s)\n",
@@ -191,17 +159,10 @@ static int iwl_nvm_read_section(struct iwl_mvm *mvm, u16 section,
 {
 	u16 length, offset = 0;
 	int ret;
-	bool old_eeprom = mvm->cfg->device_family != IWL_DEVICE_FAMILY_7000;
 
-	length = (iwlwifi_mod_params.amsdu_size_8K ? (8 * 1024) : (4 * 1024))
-		- sizeof(union iwl_nvm_access_cmd)
-		- sizeof(struct iwl_rx_packet);
-	/*
-	 * if length is greater than EEPROM size, truncate it because uCode
-	 * doesn't check it by itself, and exit the loop when reached.
-	 */
-	if (old_eeprom && length > mvm->cfg->base_params->eeprom_size)
-		length = mvm->cfg->base_params->eeprom_size;
+	/* Set nvm section read length */
+	length = IWL_NVM_DEFAULT_CHUNK_SIZE;
+
 	ret = length;
 
 	/* Read the NVM until exhausted (reading less than requested) */
@@ -214,8 +175,6 @@ static int iwl_nvm_read_section(struct iwl_mvm *mvm, u16 section,
 			return ret;
 		}
 		offset += ret;
-		if (old_eeprom && offset == mvm->cfg->base_params->eeprom_size)
-			break;
 	}
 
 	IWL_INFO(mvm, "NVM section %d read completed\n", section);
@@ -249,63 +208,31 @@ int iwl_nvm_init(struct iwl_mvm *mvm)
 	int ret, i, section;
 	u8 *nvm_buffer, *temp;
 
-	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_7000) {
-		/* TODO: find correct NVM max size for a section */
-		nvm_buffer = kmalloc(mvm->cfg->base_params->eeprom_size,
-				     GFP_KERNEL);
-		if (!nvm_buffer)
-			return -ENOMEM;
-		for (i = 0; i < ARRAY_SIZE(nvm_to_read); i++) {
-			section = nvm_to_read[i];
-			/* we override the constness for initial read */
-			ret = iwl_nvm_read_section(mvm, section, nvm_buffer);
-			if (ret < 0)
-				break;
-			temp = kmemdup(nvm_buffer, ret, GFP_KERNEL);
-			if (!temp) {
-				ret = -ENOMEM;
-				break;
-			}
-			mvm->nvm_sections[section].data = temp;
-			mvm->nvm_sections[section].length = ret;
-		}
-		kfree(nvm_buffer);
+	/* TODO: find correct NVM max size for a section */
+	nvm_buffer = kmalloc(mvm->cfg->base_params->eeprom_size,
+			     GFP_KERNEL);
+	if (!nvm_buffer)
+		return -ENOMEM;
+	for (i = 0; i < ARRAY_SIZE(nvm_to_read); i++) {
+		section = nvm_to_read[i];
+		/* we override the constness for initial read */
+		ret = iwl_nvm_read_section(mvm, section, nvm_buffer);
 		if (ret < 0)
-			return ret;
-	} else {
-		/* allocate eeprom */
-		mvm->eeprom_blob_size = mvm->cfg->base_params->eeprom_size;
-		IWL_DEBUG_EEPROM(mvm->trans->dev, "NVM size = %zd\n",
-				 mvm->eeprom_blob_size);
-		mvm->eeprom_blob = kzalloc(mvm->eeprom_blob_size, GFP_KERNEL);
-		if (!mvm->eeprom_blob)
-			return -ENOMEM;
-
-		ret = iwl_nvm_read_section(mvm, 0, mvm->eeprom_blob);
-		if (ret != mvm->eeprom_blob_size) {
-			IWL_ERR(mvm, "Read partial NVM %d/%zd\n",
-				ret, mvm->eeprom_blob_size);
-			kfree(mvm->eeprom_blob);
-			mvm->eeprom_blob = NULL;
-			return -EINVAL;
+			break;
+		temp = kmemdup(nvm_buffer, ret, GFP_KERNEL);
+		if (!temp) {
+			ret = -ENOMEM;
+			break;
 		}
+		mvm->nvm_sections[section].data = temp;
+		mvm->nvm_sections[section].length = ret;
 	}
+	kfree(nvm_buffer);
+	if (ret < 0)
+		return ret;
 
 	ret = 0;
-	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_7000)
-		mvm->nvm_data = iwl_parse_nvm_sections(mvm);
-	else
-		mvm->nvm_data =
-			iwl_parse_eeprom_data(mvm->trans->dev,
-					      mvm->cfg,
-					      mvm->eeprom_blob,
-					      mvm->eeprom_blob_size);
-
-	if (!mvm->nvm_data) {
-		kfree(mvm->eeprom_blob);
-		mvm->eeprom_blob = NULL;
-		ret = -ENOMEM;
-	}
+	mvm->nvm_data = iwl_parse_nvm_sections(mvm);
 
 	return ret;
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/ops.c b/drivers/net/wireless/iwlwifi/mvm/ops.c
index d0f9c1e0475e..fe031d304d1e 100644
--- a/drivers/net/wireless/iwlwifi/mvm/ops.c
+++ b/drivers/net/wireless/iwlwifi/mvm/ops.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -143,21 +143,12 @@ static void iwl_mvm_nic_config(struct iwl_op_mode *op_mode)
 	u8 radio_cfg_type, radio_cfg_step, radio_cfg_dash;
 	u32 reg_val = 0;
 
-	/*
-	 * We can't upload the correct value to the INIT image
-	 * as we don't have nvm_data by that time.
-	 *
-	 * TODO: Figure out what we should do here
-	 */
-	if (mvm->nvm_data) {
-		radio_cfg_type = mvm->nvm_data->radio_cfg_type;
-		radio_cfg_step = mvm->nvm_data->radio_cfg_step;
-		radio_cfg_dash = mvm->nvm_data->radio_cfg_dash;
-	} else {
-		radio_cfg_type = 0;
-		radio_cfg_step = 0;
-		radio_cfg_dash = 0;
-	}
+	radio_cfg_type = (mvm->fw->phy_config & FW_PHY_CFG_RADIO_TYPE) >>
+			  FW_PHY_CFG_RADIO_TYPE_POS;
+	radio_cfg_step = (mvm->fw->phy_config & FW_PHY_CFG_RADIO_STEP) >>
+			  FW_PHY_CFG_RADIO_STEP_POS;
+	radio_cfg_dash = (mvm->fw->phy_config & FW_PHY_CFG_RADIO_DASH) >>
+			  FW_PHY_CFG_RADIO_DASH_POS;
 
 	/* SKU control */
 	reg_val |= CSR_HW_REV_STEP(mvm->trans->hw_rev) <<
@@ -175,7 +166,6 @@ static void iwl_mvm_nic_config(struct iwl_op_mode *op_mode)
 
 	/* silicon bits */
 	reg_val |= CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI;
-	reg_val |= CSR_HW_IF_CONFIG_REG_BIT_MAC_SI;
 
 	iwl_trans_set_bits_mask(mvm->trans, CSR_HW_IF_CONFIG_REG,
 				CSR_HW_IF_CONFIG_REG_MSK_MAC_DASH |
@@ -230,6 +220,9 @@ static const struct iwl_rx_handlers iwl_mvm_rx_handlers[] = {
 	RX_HANDLER(SCAN_REQUEST_CMD, iwl_mvm_rx_scan_response, false),
 	RX_HANDLER(SCAN_COMPLETE_NOTIFICATION, iwl_mvm_rx_scan_complete, false),
 
+	RX_HANDLER(BT_PROFILE_NOTIFICATION, iwl_mvm_rx_bt_coex_notif, true),
+	RX_HANDLER(BEACON_NOTIFICATION, iwl_mvm_rx_beacon_notif, false),
+
 	RX_HANDLER(RADIO_VERSION_NOTIFICATION, iwl_mvm_rx_radio_ver, false),
 	RX_HANDLER(CARD_STATE_NOTIFICATION, iwl_mvm_rx_card_state_notif, false),
 
@@ -274,6 +267,7 @@ static const char *iwl_mvm_cmd_strings[REPLY_MAX] = {
 	CMD(WEP_KEY),
 	CMD(REPLY_RX_PHY_CMD),
 	CMD(REPLY_RX_MPDU_CMD),
+	CMD(BEACON_NOTIFICATION),
 	CMD(BEACON_TEMPLATE_CMD),
 	CMD(STATISTICS_NOTIFICATION),
 	CMD(TX_ANT_CONFIGURATION_CMD),
@@ -293,6 +287,11 @@ static const char *iwl_mvm_cmd_strings[REPLY_MAX] = {
 	CMD(NET_DETECT_PROFILES_CMD),
 	CMD(NET_DETECT_HOTSPOTS_CMD),
 	CMD(NET_DETECT_HOTSPOTS_QUERY_CMD),
+	CMD(CARD_STATE_NOTIFICATION),
+	CMD(BT_COEX_PRIO_TABLE),
+	CMD(BT_COEX_PROT_ENV),
+	CMD(BT_PROFILE_NOTIFICATION),
+	CMD(BT_CONFIG),
 };
 #undef CMD
 
@@ -312,16 +311,6 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	};
 	int err, scan_size;
 
-	switch (cfg->device_family) {
-	case IWL_DEVICE_FAMILY_6030:
-	case IWL_DEVICE_FAMILY_6005:
-	case IWL_DEVICE_FAMILY_7000:
-		break;
-	default:
-		IWL_ERR(trans, "Trying to load mvm on an unsupported device\n");
-		return NULL;
-	}
-
 	/********************************
 	 * 1. Allocating and configuring HW data
 	 ********************************/
@@ -363,8 +352,7 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	trans_cfg.n_no_reclaim_cmds = ARRAY_SIZE(no_reclaim_cmds);
 	trans_cfg.rx_buf_size_8k = iwlwifi_mod_params.amsdu_size_8K;
 
-	/* TODO: this should really be a TLV */
-	if (cfg->device_family == IWL_DEVICE_FAMILY_7000)
+	if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_DW_BC_TABLE)
 		trans_cfg.bc_table_dword = true;
 
 	if (!iwlwifi_mod_params.wd_disable)
@@ -438,7 +426,6 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
  out_free:
 	iwl_phy_db_free(mvm->phy_db);
 	kfree(mvm->scan_cmd);
-	kfree(mvm->eeprom_blob);
 	iwl_trans_stop_hw(trans, true);
 	ieee80211_free_hw(mvm->hw);
 	return NULL;
@@ -460,7 +447,6 @@ static void iwl_op_mode_mvm_stop(struct iwl_op_mode *op_mode)
 	iwl_phy_db_free(mvm->phy_db);
 	mvm->phy_db = NULL;
 
-	kfree(mvm->eeprom_blob);
 	iwl_free_nvm_data(mvm->nvm_data);
 	for (i = 0; i < NVM_NUM_OF_SECTIONS; i++)
 		kfree(mvm->nvm_sections[i].data);
diff --git a/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c b/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
index b428448f8ddf..a28a1d1f23eb 100644
--- a/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -142,7 +142,7 @@ static void iwl_mvm_phy_ctxt_cmd_data(struct iwl_mvm *mvm,
 				      struct cfg80211_chan_def *chandef,
 				      u8 chains_static, u8 chains_dynamic)
 {
-	u8 valid_rx_chains, active_cnt, idle_cnt;
+	u8 active_cnt, idle_cnt;
 
 	/* Set the channel info data */
 	cmd->ci.band = (chandef->chan->band == IEEE80211_BAND_2GHZ ?
@@ -153,22 +153,16 @@ static void iwl_mvm_phy_ctxt_cmd_data(struct iwl_mvm *mvm,
 	cmd->ci.ctrl_pos = iwl_mvm_get_ctrl_pos(chandef);
 
 	/* Set rx the chains */
-
-	/* TODO:
-	 * Need to add on chain noise calibration limitations, and
-	 * BT coex considerations.
-	 */
-	valid_rx_chains = mvm->nvm_data->valid_rx_ant;
 	idle_cnt = chains_static;
 	active_cnt = chains_dynamic;
 
-	cmd->rxchain_info = cpu_to_le32(valid_rx_chains <<
+	cmd->rxchain_info = cpu_to_le32(iwl_fw_valid_rx_ant(mvm->fw) <<
 					PHY_RX_CHAIN_VALID_POS);
 	cmd->rxchain_info |= cpu_to_le32(idle_cnt << PHY_RX_CHAIN_CNT_POS);
 	cmd->rxchain_info |= cpu_to_le32(active_cnt <<
 					 PHY_RX_CHAIN_MIMO_CNT_POS);
 
-	cmd->txchain_info = cpu_to_le32(mvm->nvm_data->valid_tx_ant);
+	cmd->txchain_info = cpu_to_le32(iwl_fw_valid_tx_ant(mvm->fw));
 }
 
 /*
diff --git a/drivers/net/wireless/iwlwifi/mvm/power.c b/drivers/net/wireless/iwlwifi/mvm/power.c
index 5a92a4978795..ed77e437aac4 100644
--- a/drivers/net/wireless/iwlwifi/mvm/power.c
+++ b/drivers/net/wireless/iwlwifi/mvm/power.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -75,23 +75,48 @@
 
 #define POWER_KEEP_ALIVE_PERIOD_SEC    25
 
-static void iwl_power_build_cmd(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-				struct iwl_powertable_cmd *cmd)
+static void iwl_mvm_power_log(struct iwl_mvm *mvm,
+			      struct iwl_powertable_cmd *cmd)
+{
+	IWL_DEBUG_POWER(mvm,
+			"Sending power table command for power level %d, flags = 0x%X\n",
+			iwlmvm_mod_params.power_scheme,
+			le16_to_cpu(cmd->flags));
+	IWL_DEBUG_POWER(mvm, "Keep alive = %u sec\n", cmd->keep_alive_seconds);
+
+	if (cmd->flags & cpu_to_le16(POWER_FLAGS_POWER_MANAGEMENT_ENA_MSK)) {
+		IWL_DEBUG_POWER(mvm, "Rx timeout = %u usec\n",
+				le32_to_cpu(cmd->rx_data_timeout));
+		IWL_DEBUG_POWER(mvm, "Tx timeout = %u usec\n",
+				le32_to_cpu(cmd->tx_data_timeout));
+		IWL_DEBUG_POWER(mvm, "LP RX RSSI threshold = %u\n",
+				cmd->lprx_rssi_threshold);
+	}
+}
+
+void iwl_mvm_power_build_cmd(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			     struct iwl_powertable_cmd *cmd)
 {
 	struct ieee80211_hw *hw = mvm->hw;
-	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct ieee80211_chanctx_conf *chanctx_conf;
 	struct ieee80211_channel *chan;
 	int dtimper, dtimper_msec;
 	int keep_alive;
 	bool radar_detect = false;
 
-	cmd->id_and_color = cpu_to_le32(FW_CMD_ID_AND_COLOR(mvmvif->id,
-							    mvmvif->color));
-	cmd->action = cpu_to_le32(FW_CTXT_ACTION_MODIFY);
+	/*
+	 * Regardless of power management state the driver must set
+	 * keep alive period. FW will use it for sending keep alive NDPs
+	 * immediately after association.
+	 */
+	cmd->keep_alive_seconds = POWER_KEEP_ALIVE_PERIOD_SEC;
 
-	if ((!vif->bss_conf.ps) ||
-	    (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_CAM))
+	if (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_CAM)
+		return;
+
+	cmd->flags |= cpu_to_le16(POWER_FLAGS_POWER_SAVE_ENA_MSK);
+
+	if (!vif->bss_conf.ps)
 		return;
 
 	cmd->flags |= cpu_to_le16(POWER_FLAGS_POWER_MANAGEMENT_ENA_MSK);
@@ -110,63 +135,29 @@ static void iwl_power_build_cmd(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
 	/* Check skip over DTIM conditions */
 	if (!radar_detect && (dtimper <= 10) &&
-	    (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_LP)) {
-		cmd->flags |= cpu_to_le16(POWER_FLAGS_SLEEP_OVER_DTIM_MSK);
-		cmd->num_skip_dtim = 2;
-	}
+	    (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_LP))
+		cmd->flags |= cpu_to_le16(POWER_FLAGS_SKIP_OVER_DTIM_MSK);
 
 	/* Check that keep alive period is at least 3 * DTIM */
 	dtimper_msec = dtimper * vif->bss_conf.beacon_int;
 	keep_alive = max_t(int, 3 * dtimper_msec,
-			   MSEC_PER_SEC * POWER_KEEP_ALIVE_PERIOD_SEC);
+			   MSEC_PER_SEC * cmd->keep_alive_seconds);
 	keep_alive = DIV_ROUND_UP(keep_alive, MSEC_PER_SEC);
+	cmd->keep_alive_seconds = keep_alive;
 
-	cmd->keep_alive_seconds = cpu_to_le16(keep_alive);
-
-	if (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_LP) {
-		/* TODO: Also for D3 (device sleep / WoWLAN) */
-		cmd->rx_data_timeout = cpu_to_le32(10);
-		cmd->tx_data_timeout = cpu_to_le32(10);
-	} else {
-		cmd->rx_data_timeout = cpu_to_le32(50);
-		cmd->tx_data_timeout = cpu_to_le32(50);
-	}
+	cmd->rx_data_timeout = cpu_to_le32(100 * USEC_PER_MSEC);
+	cmd->tx_data_timeout = cpu_to_le32(100 * USEC_PER_MSEC);
 }
 
 int iwl_mvm_power_update_mode(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 {
 	struct iwl_powertable_cmd cmd = {};
 
-	if (!iwlwifi_mod_params.power_save) {
-		IWL_DEBUG_POWER(mvm, "Power management is not allowed\n");
-		return 0;
-	}
-
 	if (vif->type != NL80211_IFTYPE_STATION || vif->p2p)
 		return 0;
 
-	iwl_power_build_cmd(mvm, vif, &cmd);
-
-	IWL_DEBUG_POWER(mvm,
-			"Sending power table command on mac id 0x%X for power level %d, flags = 0x%X\n",
-			cmd.id_and_color, iwlmvm_mod_params.power_scheme,
-			le16_to_cpu(cmd.flags));
-
-	if (cmd.flags & cpu_to_le16(POWER_FLAGS_POWER_MANAGEMENT_ENA_MSK)) {
-		IWL_DEBUG_POWER(mvm, "Keep alive = %u sec\n",
-				le16_to_cpu(cmd.keep_alive_seconds));
-		IWL_DEBUG_POWER(mvm, "Rx timeout = %u usec\n",
-				le32_to_cpu(cmd.rx_data_timeout));
-		IWL_DEBUG_POWER(mvm, "Tx timeout = %u usec\n",
-				le32_to_cpu(cmd.tx_data_timeout));
-		IWL_DEBUG_POWER(mvm, "Rx timeout (uAPSD) = %u usec\n",
-				le32_to_cpu(cmd.rx_data_timeout_uapsd));
-		IWL_DEBUG_POWER(mvm, "Tx timeout = %u usec\n",
-				le32_to_cpu(cmd.tx_data_timeout_uapsd));
-		IWL_DEBUG_POWER(mvm, "LP RX RSSI threshold = %u\n",
-				cmd.lprx_rssi_threshold);
-		IWL_DEBUG_POWER(mvm, "DTIMs to skip = %u\n", cmd.num_skip_dtim);
-	}
+	iwl_mvm_power_build_cmd(mvm, vif, &cmd);
+	iwl_mvm_power_log(mvm, &cmd);
 
 	return iwl_mvm_send_cmd_pdu(mvm, POWER_TABLE_CMD, CMD_SYNC,
 				    sizeof(cmd), &cmd);
@@ -175,33 +166,15 @@ int iwl_mvm_power_update_mode(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 int iwl_mvm_power_disable(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 {
 	struct iwl_powertable_cmd cmd = {};
-	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
-
-	if (!iwlwifi_mod_params.power_save) {
-		IWL_DEBUG_POWER(mvm, "Power management is not allowed\n");
-		return 0;
-	}
 
 	if (vif->type != NL80211_IFTYPE_STATION || vif->p2p)
 		return 0;
 
-	cmd.id_and_color = cpu_to_le32(FW_CMD_ID_AND_COLOR(mvmvif->id,
-							    mvmvif->color));
-	cmd.action = cpu_to_le32(FW_CTXT_ACTION_MODIFY);
+	if (iwlmvm_mod_params.power_scheme != IWL_POWER_SCHEME_CAM)
+		cmd.flags |= cpu_to_le16(POWER_FLAGS_POWER_SAVE_ENA_MSK);
 
-	IWL_DEBUG_POWER(mvm,
-			"Sending power table command on mac id 0x%X for power level %d, flags = 0x%X\n",
-			cmd.id_and_color, iwlmvm_mod_params.power_scheme,
-			le16_to_cpu(cmd.flags));
+	iwl_mvm_power_log(mvm, &cmd);
 
 	return iwl_mvm_send_cmd_pdu(mvm, POWER_TABLE_CMD, CMD_ASYNC,
 				    sizeof(cmd), &cmd);
 }
-
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-void iwl_power_get_params(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			  struct iwl_powertable_cmd *cmd)
-{
-	iwl_power_build_cmd(mvm, vif, cmd);
-}
-#endif /* CONFIG_IWLWIFI_DEBUGFS */
diff --git a/drivers/net/wireless/iwlwifi/mvm/quota.c b/drivers/net/wireless/iwlwifi/mvm/quota.c
index 925628468146..a1e3e923ea3e 100644
--- a/drivers/net/wireless/iwlwifi/mvm/quota.c
+++ b/drivers/net/wireless/iwlwifi/mvm/quota.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -114,7 +114,8 @@ static void iwl_mvm_quota_iterator(void *_data, u8 *mac,
 			data->n_interfaces[id]++;
 		break;
 	case NL80211_IFTYPE_MONITOR:
-		data->n_interfaces[id]++;
+		if (mvmvif->monitor_active)
+			data->n_interfaces[id]++;
 		break;
 	case NL80211_IFTYPE_P2P_DEVICE:
 		break;
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.c b/drivers/net/wireless/iwlwifi/mvm/rs.c
index 56b636d9ab30..55334d542e26 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.c
@@ -680,12 +680,14 @@ static int rs_toggle_antenna(u32 valid_ant, u32 *rate_n_flags,
  */
 static bool rs_use_green(struct ieee80211_sta *sta)
 {
-	struct iwl_mvm_sta *sta_priv = (void *)sta->drv_priv;
-
-	bool use_green = !(sta_priv->vif->bss_conf.ht_operation_mode &
-				IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT);
-
-	return (sta->ht_cap.cap & IEEE80211_HT_CAP_GRN_FLD) && use_green;
+	/*
+	 * There's a bug somewhere in this code that causes the
+	 * scaling to get stuck because GF+SGI can't be combined
+	 * in SISO rates. Until we find that bug, disable GF, it
+	 * has only limited benefit and we still interoperate with
+	 * GF APs since we can always receive GF transmissions.
+	 */
+	return false;
 }
 
 /**
@@ -791,7 +793,7 @@ static u32 rs_get_lower_rate(struct iwl_lq_sta *lq_sta,
 
 		if (num_of_ant(tbl->ant_type) > 1)
 			tbl->ant_type =
-			    first_antenna(mvm->nvm_data->valid_tx_ant);
+			    first_antenna(iwl_fw_valid_tx_ant(mvm->fw));
 
 		tbl->is_ht40 = 0;
 		tbl->is_SGI = 0;
@@ -1233,7 +1235,7 @@ static int rs_switch_to_mimo2(struct iwl_mvm *mvm,
 		return -1;
 
 	/* Need both Tx chains/antennas to support MIMO */
-	if (num_of_ant(mvm->nvm_data->valid_tx_ant) < 2)
+	if (num_of_ant(iwl_fw_valid_tx_ant(mvm->fw)) < 2)
 		return -1;
 
 	IWL_DEBUG_RATE(mvm, "LQ: try to switch to MIMO2\n");
@@ -1285,7 +1287,7 @@ static int rs_switch_to_mimo3(struct iwl_mvm *mvm,
 		return -1;
 
 	/* Need both Tx chains/antennas to support MIMO */
-	if (num_of_ant(mvm->nvm_data->valid_tx_ant) < 3)
+	if (num_of_ant(iwl_fw_valid_tx_ant(mvm->fw)) < 3)
 		return -1;
 
 	IWL_DEBUG_RATE(mvm, "LQ: try to switch to MIMO3\n");
@@ -1379,7 +1381,7 @@ static int rs_move_legacy_other(struct iwl_mvm *mvm,
 	u32 sz = (sizeof(struct iwl_scale_tbl_info) -
 		  (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
 	u8 start_action;
-	u8 valid_tx_ant = mvm->nvm_data->valid_tx_ant;
+	u8 valid_tx_ant = iwl_fw_valid_tx_ant(mvm->fw);
 	u8 tx_chains_num = num_of_ant(valid_tx_ant);
 	int ret;
 	u8 update_search_tbl_counter = 0;
@@ -1512,7 +1514,7 @@ static int rs_move_siso_to_other(struct iwl_mvm *mvm,
 	u32 sz = (sizeof(struct iwl_scale_tbl_info) -
 		  (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
 	u8 start_action;
-	u8 valid_tx_ant = mvm->nvm_data->valid_tx_ant;
+	u8 valid_tx_ant = iwl_fw_valid_tx_ant(mvm->fw);
 	u8 tx_chains_num = num_of_ant(valid_tx_ant);
 	u8 update_search_tbl_counter = 0;
 	int ret;
@@ -1647,7 +1649,7 @@ static int rs_move_mimo2_to_other(struct iwl_mvm *mvm,
 	u32 sz = (sizeof(struct iwl_scale_tbl_info) -
 		  (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
 	u8 start_action;
-	u8 valid_tx_ant = mvm->nvm_data->valid_tx_ant;
+	u8 valid_tx_ant = iwl_fw_valid_tx_ant(mvm->fw);
 	u8 tx_chains_num = num_of_ant(valid_tx_ant);
 	u8 update_search_tbl_counter = 0;
 	int ret;
@@ -1784,7 +1786,7 @@ static int rs_move_mimo3_to_other(struct iwl_mvm *mvm,
 	u32 sz = (sizeof(struct iwl_scale_tbl_info) -
 		  (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
 	u8 start_action;
-	u8 valid_tx_ant = mvm->nvm_data->valid_tx_ant;
+	u8 valid_tx_ant = iwl_fw_valid_tx_ant(mvm->fw);
 	u8 tx_chains_num = num_of_ant(valid_tx_ant);
 	int ret;
 	u8 update_search_tbl_counter = 0;
@@ -2447,7 +2449,7 @@ static void rs_initialize_lq(struct iwl_mvm *mvm,
 
 	i = lq_sta->last_txrate_idx;
 
-	valid_tx_ant = mvm->nvm_data->valid_tx_ant;
+	valid_tx_ant = iwl_fw_valid_tx_ant(mvm->fw);
 
 	if (!lq_sta->search_better_tbl)
 		active_tbl = lq_sta->active_tbl;
@@ -2637,15 +2639,15 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 
 	/* These values will be overridden later */
 	lq_sta->lq.single_stream_ant_msk =
-		first_antenna(mvm->nvm_data->valid_tx_ant);
+		first_antenna(iwl_fw_valid_tx_ant(mvm->fw));
 	lq_sta->lq.dual_stream_ant_msk =
-		mvm->nvm_data->valid_tx_ant &
-		~first_antenna(mvm->nvm_data->valid_tx_ant);
+		iwl_fw_valid_tx_ant(mvm->fw) &
+		~first_antenna(iwl_fw_valid_tx_ant(mvm->fw));
 	if (!lq_sta->lq.dual_stream_ant_msk) {
 		lq_sta->lq.dual_stream_ant_msk = ANT_AB;
-	} else if (num_of_ant(mvm->nvm_data->valid_tx_ant) == 2) {
+	} else if (num_of_ant(iwl_fw_valid_tx_ant(mvm->fw)) == 2) {
 		lq_sta->lq.dual_stream_ant_msk =
-			mvm->nvm_data->valid_tx_ant;
+			iwl_fw_valid_tx_ant(mvm->fw);
 	}
 
 	/* as default allow aggregation for all tids */
@@ -2706,7 +2708,7 @@ static void rs_fill_link_cmd(struct iwl_mvm *mvm,
 	index++;
 	repeat_rate--;
 	if (mvm)
-		valid_tx_ant = mvm->nvm_data->valid_tx_ant;
+		valid_tx_ant = iwl_fw_valid_tx_ant(mvm->fw);
 
 	/* Fill rest of rate table */
 	while (index < LINK_QUAL_MAX_RETRY_NUM) {
@@ -2811,7 +2813,7 @@ static void rs_dbgfs_set_mcs(struct iwl_lq_sta *lq_sta,
 	u8 ant_sel_tx;
 
 	mvm = lq_sta->drv;
-	valid_tx_ant = mvm->nvm_data->valid_tx_ant;
+	valid_tx_ant = iwl_fw_valid_tx_ant(mvm->fw);
 	if (lq_sta->dbg_fixed_rate) {
 		ant_sel_tx =
 		  ((lq_sta->dbg_fixed_rate & RATE_MCS_ANT_ABC_MSK)
@@ -2882,9 +2884,9 @@ static ssize_t rs_sta_dbgfs_scale_table_read(struct file *file,
 	desc += sprintf(buff+desc, "fixed rate 0x%X\n",
 			lq_sta->dbg_fixed_rate);
 	desc += sprintf(buff+desc, "valid_tx_ant %s%s%s\n",
-	    (mvm->nvm_data->valid_tx_ant & ANT_A) ? "ANT_A," : "",
-	    (mvm->nvm_data->valid_tx_ant & ANT_B) ? "ANT_B," : "",
-	    (mvm->nvm_data->valid_tx_ant & ANT_C) ? "ANT_C" : "");
+	    (iwl_fw_valid_tx_ant(mvm->fw) & ANT_A) ? "ANT_A," : "",
+	    (iwl_fw_valid_tx_ant(mvm->fw) & ANT_B) ? "ANT_B," : "",
+	    (iwl_fw_valid_tx_ant(mvm->fw) & ANT_C) ? "ANT_C" : "");
 	desc += sprintf(buff+desc, "lq type %s\n",
 	   (is_legacy(tbl->lq_type)) ? "legacy" : "HT");
 	if (is_Ht(tbl->lq_type)) {
diff --git a/drivers/net/wireless/iwlwifi/mvm/rx.c b/drivers/net/wireless/iwlwifi/mvm/rx.c
index b0b190d0ec23..4dfc21a3e83e 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rx.c
+++ b/drivers/net/wireless/iwlwifi/mvm/rx.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
diff --git a/drivers/net/wireless/iwlwifi/mvm/scan.c b/drivers/net/wireless/iwlwifi/mvm/scan.c
index 9b21b92aa8d1..2157b0f8ced5 100644
--- a/drivers/net/wireless/iwlwifi/mvm/scan.c
+++ b/drivers/net/wireless/iwlwifi/mvm/scan.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -74,7 +74,7 @@
 static inline __le16 iwl_mvm_scan_rx_chain(struct iwl_mvm *mvm)
 {
 	u16 rx_chain;
-	u8 rx_ant = mvm->nvm_data->valid_rx_ant;
+	u8 rx_ant = iwl_fw_valid_rx_ant(mvm->fw);
 
 	rx_chain = rx_ant << PHY_RX_CHAIN_VALID_POS;
 	rx_chain |= rx_ant << PHY_RX_CHAIN_FORCE_MIMO_SEL_POS;
@@ -115,7 +115,7 @@ iwl_mvm_scan_rate_n_flags(struct iwl_mvm *mvm, enum ieee80211_band band,
 	u32 tx_ant;
 
 	mvm->scan_last_antenna_idx =
-		iwl_mvm_next_antenna(mvm, mvm->nvm_data->valid_tx_ant,
+		iwl_mvm_next_antenna(mvm, iwl_fw_valid_tx_ant(mvm->fw),
 				     mvm->scan_last_antenna_idx);
 	tx_ant = BIT(mvm->scan_last_antenna_idx) << RATE_MCS_ANT_POS;
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/sta.c b/drivers/net/wireless/iwlwifi/mvm/sta.c
index 274f44e2ef60..0fd96e4da461 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sta.c
+++ b/drivers/net/wireless/iwlwifi/mvm/sta.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -101,8 +101,55 @@ int iwl_mvm_sta_send_to_fw(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 	}
 	add_sta_cmd.add_modify = update ? 1 : 0;
 
-	/* STA_FLG_FAT_EN_MSK ? */
-	/* STA_FLG_MIMO_EN_MSK ? */
+	add_sta_cmd.station_flags_msk |= cpu_to_le32(STA_FLG_FAT_EN_MSK |
+						     STA_FLG_MIMO_EN_MSK);
+
+	switch (sta->bandwidth) {
+	case IEEE80211_STA_RX_BW_160:
+		add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_FAT_EN_160MHZ);
+		/* fall through */
+	case IEEE80211_STA_RX_BW_80:
+		add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_FAT_EN_80MHZ);
+		/* fall through */
+	case IEEE80211_STA_RX_BW_40:
+		add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_FAT_EN_40MHZ);
+		/* fall through */
+	case IEEE80211_STA_RX_BW_20:
+		if (sta->ht_cap.ht_supported)
+			add_sta_cmd.station_flags |=
+				cpu_to_le32(STA_FLG_FAT_EN_20MHZ);
+		break;
+	}
+
+	switch (sta->rx_nss) {
+	case 1:
+		add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_SISO);
+		break;
+	case 2:
+		add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_MIMO2);
+		break;
+	case 3 ... 8:
+		add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_MIMO3);
+		break;
+	}
+
+	switch (sta->smps_mode) {
+	case IEEE80211_SMPS_AUTOMATIC:
+	case IEEE80211_SMPS_NUM_MODES:
+		WARN_ON(1);
+		break;
+	case IEEE80211_SMPS_STATIC:
+		/* override NSS */
+		add_sta_cmd.station_flags &= ~cpu_to_le32(STA_FLG_MIMO_EN_MSK);
+		add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_MIMO_EN_SISO);
+		break;
+	case IEEE80211_SMPS_DYNAMIC:
+		add_sta_cmd.station_flags |= cpu_to_le32(STA_FLG_RTS_MIMO_PROT);
+		break;
+	case IEEE80211_SMPS_OFF:
+		/* nothing */
+		break;
+	}
 
 	if (sta->ht_cap.ht_supported) {
 		add_sta_cmd.station_flags_msk |=
@@ -340,6 +387,9 @@ int iwl_mvm_rm_sta(struct iwl_mvm *mvm,
 
 	if (vif->type == NL80211_IFTYPE_STATION &&
 	    mvmvif->ap_sta_id == mvm_sta->sta_id) {
+		/* flush its queues here since we are freeing mvm_sta */
+		ret = iwl_mvm_flush_tx_path(mvm, mvm_sta->tfd_queue_msk, true);
+
 		/*
 		 * Put a non-NULL since the fw station isn't removed.
 		 * It will be removed after the MAC will be set as
@@ -348,9 +398,6 @@ int iwl_mvm_rm_sta(struct iwl_mvm *mvm,
 		rcu_assign_pointer(mvm->fw_id_to_mac_id[mvm_sta->sta_id],
 				   ERR_PTR(-EINVAL));
 
-		/* flush its queues here since we are freeing mvm_sta */
-		ret = iwl_mvm_flush_tx_path(mvm, mvm_sta->tfd_queue_msk, true);
-
 		/* if we are associated - we can't remove the AP STA now */
 		if (vif->bss_conf.assoc)
 			return ret;
@@ -686,7 +733,7 @@ int iwl_mvm_sta_tx_agg_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
 	spin_lock_bh(&mvmsta->lock);
 	tid_data = &mvmsta->tid_data[tid];
-	tid_data->ssn = SEQ_TO_SN(tid_data->seq_number);
+	tid_data->ssn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
 	tid_data->txq_id = txq_id;
 	*ssn = tid_data->ssn;
 
@@ -789,7 +836,7 @@ int iwl_mvm_sta_tx_agg_stop(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
 	switch (tid_data->state) {
 	case IWL_AGG_ON:
-		tid_data->ssn = SEQ_TO_SN(tid_data->seq_number);
+		tid_data->ssn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
 
 		IWL_DEBUG_TX_QUEUES(mvm,
 				    "ssn = %d, next_recl = %d\n",
@@ -834,6 +881,34 @@ int iwl_mvm_sta_tx_agg_stop(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	return err;
 }
 
+int iwl_mvm_sta_tx_agg_flush(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			    struct ieee80211_sta *sta, u16 tid)
+{
+	struct iwl_mvm_sta *mvmsta = (void *)sta->drv_priv;
+	struct iwl_mvm_tid_data *tid_data = &mvmsta->tid_data[tid];
+	u16 txq_id;
+
+	/*
+	 * First set the agg state to OFF to avoid calling
+	 * ieee80211_stop_tx_ba_cb in iwl_mvm_check_ratid_empty.
+	 */
+	spin_lock_bh(&mvmsta->lock);
+	txq_id = tid_data->txq_id;
+	IWL_DEBUG_TX_QUEUES(mvm, "Flush AGG: sta %d tid %d q %d state %d\n",
+			    mvmsta->sta_id, tid, txq_id, tid_data->state);
+	tid_data->state = IWL_AGG_OFF;
+	spin_unlock_bh(&mvmsta->lock);
+
+	if (iwl_mvm_flush_tx_path(mvm, BIT(txq_id), true))
+		IWL_ERR(mvm, "Couldn't flush the AGG queue\n");
+
+	iwl_trans_txq_disable(mvm->trans, tid_data->txq_id);
+	mvm->queue_to_mac80211[tid_data->txq_id] =
+				IWL_INVALID_MAC80211_QUEUE;
+
+	return 0;
+}
+
 static int iwl_mvm_set_fw_key_idx(struct iwl_mvm *mvm)
 {
 	int i;
@@ -870,7 +945,7 @@ static u8 iwl_mvm_get_key_sta_id(struct ieee80211_vif *vif,
 	    mvmvif->ap_sta_id != IWL_MVM_STATION_COUNT)
 		return mvmvif->ap_sta_id;
 
-	return IWL_INVALID_STATION;
+	return IWL_MVM_STATION_COUNT;
 }
 
 static int iwl_mvm_send_sta_key(struct iwl_mvm *mvm,
@@ -1018,7 +1093,7 @@ int iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
 
 	/* Get the station id from the mvm local station table */
 	sta_id = iwl_mvm_get_key_sta_id(vif, sta);
-	if (sta_id == IWL_INVALID_STATION) {
+	if (sta_id == IWL_MVM_STATION_COUNT) {
 		IWL_ERR(mvm, "Failed to find station id\n");
 		return -EINVAL;
 	}
@@ -1113,7 +1188,7 @@ int iwl_mvm_remove_sta_key(struct iwl_mvm *mvm,
 		return -ENOENT;
 	}
 
-	if (sta_id == IWL_INVALID_STATION) {
+	if (sta_id == IWL_MVM_STATION_COUNT) {
 		IWL_DEBUG_WEP(mvm, "station non-existent, early return.\n");
 		return 0;
 	}
@@ -1179,7 +1254,7 @@ void iwl_mvm_update_tkip_key(struct iwl_mvm *mvm,
 	struct iwl_mvm_sta *mvm_sta;
 	u8 sta_id = iwl_mvm_get_key_sta_id(vif, sta);
 
-	if (WARN_ON_ONCE(sta_id == IWL_INVALID_STATION))
+	if (WARN_ON_ONCE(sta_id == IWL_MVM_STATION_COUNT))
 		return;
 
 	rcu_read_lock();
diff --git a/drivers/net/wireless/iwlwifi/mvm/sta.h b/drivers/net/wireless/iwlwifi/mvm/sta.h
index 896f88ac8145..12abd2d71835 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sta.h
+++ b/drivers/net/wireless/iwlwifi/mvm/sta.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -271,6 +271,7 @@ struct iwl_mvm_tid_data {
  * @tid_disable_agg: bitmap: if bit(tid) is set, the fw won't send ampdus for
  *	tid.
  * @max_agg_bufsize: the maximal size of the AGG buffer for this station
+ * @bt_reduced_txpower: is reduced tx power enabled for this station
  * @lock: lock to protect the whole struct. Since %tid_data is access from Tx
  * and from Tx response flow, it needs a spinlock.
  * @pending_frames: number of frames for this STA on the shared Tx queues.
@@ -287,6 +288,7 @@ struct iwl_mvm_sta {
 	u32 mac_id_n_color;
 	u16 tid_disable_agg;
 	u8 max_agg_bufsize;
+	bool bt_reduced_txpower;
 	spinlock_t lock;
 	atomic_t pending_frames;
 	struct iwl_mvm_tid_data tid_data[IWL_MAX_TID_COUNT];
@@ -348,6 +350,8 @@ int iwl_mvm_sta_tx_agg_oper(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 			struct ieee80211_sta *sta, u16 tid, u8 buf_size);
 int iwl_mvm_sta_tx_agg_stop(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 			    struct ieee80211_sta *sta, u16 tid);
+int iwl_mvm_sta_tx_agg_flush(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			    struct ieee80211_sta *sta, u16 tid);
 
 int iwl_mvm_add_aux_sta(struct iwl_mvm *mvm);
 int iwl_mvm_allocate_int_sta(struct iwl_mvm *mvm, struct iwl_mvm_int_sta *sta,
diff --git a/drivers/net/wireless/iwlwifi/mvm/time-event.c b/drivers/net/wireless/iwlwifi/mvm/time-event.c
index e437e02c7149..ad9bbca99213 100644
--- a/drivers/net/wireless/iwlwifi/mvm/time-event.c
+++ b/drivers/net/wireless/iwlwifi/mvm/time-event.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -76,14 +76,12 @@
 #define TU_TO_JIFFIES(_tu)	(usecs_to_jiffies((_tu) * 1024))
 #define MSEC_TO_TU(_msec)	(_msec*1000/1024)
 
-/* For ROC use a TE type which has priority high enough to be scheduled when
- * there is a concurrent BSS or GO/AP. Currently, use a TE type that has
- * priority similar to the TE priority used for action scans by the FW.
- * TODO: This needs to be changed, based on the reason for the ROC, i.e., use
- * TE_P2P_DEVICE_DISCOVERABLE for remain on channel without mgmt skb, and use
- * TE_P2P_DEVICE_ACTION_SCAN
+/*
+ * For the high priority TE use a time event type that has similar priority to
+ * the FW's action scan priority.
  */
-#define IWL_MVM_ROC_TE_TYPE TE_P2P_DEVICE_ACTION_SCAN
+#define IWL_MVM_ROC_TE_TYPE_NORMAL TE_P2P_DEVICE_DISCOVERABLE
+#define IWL_MVM_ROC_TE_TYPE_MGMT_TX TE_P2P_CLIENT_ASSOC
 
 void iwl_mvm_te_clear_data(struct iwl_mvm *mvm,
 			   struct iwl_mvm_time_event_data *te_data)
@@ -116,7 +114,7 @@ void iwl_mvm_roc_done_wk(struct work_struct *wk)
 	 * issue as it will have to complete before the next command is
 	 * executed, and a new time event means a new command.
 	 */
-	iwl_mvm_flush_tx_path(mvm, BIT(IWL_OFFCHANNEL_QUEUE), false);
+	iwl_mvm_flush_tx_path(mvm, BIT(IWL_MVM_OFFCHANNEL_QUEUE), false);
 }
 
 static void iwl_mvm_roc_finished(struct iwl_mvm *mvm)
@@ -168,7 +166,7 @@ static void iwl_mvm_te_handle_notif(struct iwl_mvm *mvm,
 	WARN_ONCE(!le32_to_cpu(notif->status),
 		  "Failed to schedule time event\n");
 
-	if (le32_to_cpu(notif->action) == TE_NOTIF_HOST_END) {
+	if (le32_to_cpu(notif->action) & TE_NOTIF_HOST_EVENT_END) {
 		IWL_DEBUG_TE(mvm,
 			     "TE ended - current time %lu, estimated end %lu\n",
 			     jiffies, te_data->end_jiffies);
@@ -191,7 +189,7 @@ static void iwl_mvm_te_handle_notif(struct iwl_mvm *mvm,
 		}
 
 		iwl_mvm_te_clear_data(mvm, te_data);
-	} else if (le32_to_cpu(notif->action) == TE_NOTIF_HOST_START) {
+	} else if (le32_to_cpu(notif->action) & TE_NOTIF_HOST_EVENT_START) {
 		te_data->running = true;
 		te_data->end_jiffies = jiffies +
 			TU_TO_JIFFIES(te_data->duration);
@@ -370,7 +368,8 @@ void iwl_mvm_protect_session(struct iwl_mvm *mvm,
 	time_cmd.interval_reciprocal = cpu_to_le32(iwl_mvm_reciprocal(1));
 	time_cmd.duration = cpu_to_le32(duration);
 	time_cmd.repeat = cpu_to_le32(1);
-	time_cmd.notify = cpu_to_le32(TE_NOTIF_HOST_START | TE_NOTIF_HOST_END);
+	time_cmd.notify = cpu_to_le32(TE_NOTIF_HOST_EVENT_START |
+				      TE_NOTIF_HOST_EVENT_END);
 
 	iwl_mvm_time_event_send_add(mvm, vif, te_data, &time_cmd);
 }
@@ -438,7 +437,7 @@ void iwl_mvm_stop_session_protection(struct iwl_mvm *mvm,
 }
 
 int iwl_mvm_start_p2p_roc(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			  int duration)
+			  int duration, enum ieee80211_roc_type type)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct iwl_mvm_time_event_data *te_data = &mvmvif->time_event_data;
@@ -459,27 +458,36 @@ int iwl_mvm_start_p2p_roc(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	time_cmd.action = cpu_to_le32(FW_CTXT_ACTION_ADD);
 	time_cmd.id_and_color =
 		cpu_to_le32(FW_CMD_ID_AND_COLOR(mvmvif->id, mvmvif->color));
-	time_cmd.id = cpu_to_le32(IWL_MVM_ROC_TE_TYPE);
+
+	switch (type) {
+	case IEEE80211_ROC_TYPE_NORMAL:
+		time_cmd.id = cpu_to_le32(IWL_MVM_ROC_TE_TYPE_NORMAL);
+		break;
+	case IEEE80211_ROC_TYPE_MGMT_TX:
+		time_cmd.id = cpu_to_le32(IWL_MVM_ROC_TE_TYPE_MGMT_TX);
+		break;
+	default:
+		WARN_ONCE(1, "Got an invalid ROC type\n");
+		return -EINVAL;
+	}
 
 	time_cmd.apply_time = cpu_to_le32(0);
 	time_cmd.dep_policy = cpu_to_le32(TE_INDEPENDENT);
 	time_cmd.is_present = cpu_to_le32(1);
-
 	time_cmd.interval = cpu_to_le32(1);
 
 	/*
-	 * IWL_MVM_ROC_TE_TYPE can have lower priority than other events
+	 * The P2P Device TEs can have lower priority than other events
 	 * that are being scheduled by the driver/fw, and thus it might not be
-	 * scheduled. To improve the chances of it being scheduled, allow it to
-	 * be fragmented.
-	 * In addition, for the same reasons, allow to delay the scheduling of
-	 * the time event.
+	 * scheduled. To improve the chances of it being scheduled, allow them
+	 * to be fragmented, and in addition allow them to be delayed.
 	 */
 	time_cmd.max_frags = cpu_to_le32(MSEC_TO_TU(duration)/20);
 	time_cmd.max_delay = cpu_to_le32(MSEC_TO_TU(duration/2));
 	time_cmd.duration = cpu_to_le32(MSEC_TO_TU(duration));
 	time_cmd.repeat = cpu_to_le32(1);
-	time_cmd.notify = cpu_to_le32(TE_NOTIF_HOST_START | TE_NOTIF_HOST_END);
+	time_cmd.notify = cpu_to_le32(TE_NOTIF_HOST_EVENT_START |
+				      TE_NOTIF_HOST_EVENT_END);
 
 	return iwl_mvm_time_event_send_add(mvm, vif, te_data, &time_cmd);
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/time-event.h b/drivers/net/wireless/iwlwifi/mvm/time-event.h
index 64fb57a5ab43..f86c51065ed3 100644
--- a/drivers/net/wireless/iwlwifi/mvm/time-event.h
+++ b/drivers/net/wireless/iwlwifi/mvm/time-event.h
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -162,6 +162,7 @@ int iwl_mvm_rx_time_event_notif(struct iwl_mvm *mvm,
  * that the vif type is NL80211_IFTYPE_P2P_DEVICE
  * @duration: the requested duration in millisecond for the fw to be on the
  * channel that is bound to the vif.
+ * @type: the remain on channel request type
  *
  * This function can be used to issue a remain on channel session,
  * which means that the fw will stay in the channel for the request %duration
@@ -172,7 +173,7 @@ int iwl_mvm_rx_time_event_notif(struct iwl_mvm *mvm,
  * another notification to the driver.
  */
 int iwl_mvm_start_p2p_roc(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			  int duration);
+			  int duration, enum ieee80211_roc_type type);
 
 /**
  * iwl_mvm_stop_p2p_roc - stop remain on channel for p2p device functionlity
diff --git a/drivers/net/wireless/iwlwifi/mvm/tx.c b/drivers/net/wireless/iwlwifi/mvm/tx.c
index 6645efe5c03e..479074303bd7 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tx.c
+++ b/drivers/net/wireless/iwlwifi/mvm/tx.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -205,7 +205,7 @@ static void iwl_mvm_set_tx_cmd_rate(struct iwl_mvm *mvm,
 	rate_plcp = iwl_mvm_mac80211_idx_to_hwrate(rate_idx);
 
 	mvm->mgmt_last_antenna_idx =
-		iwl_mvm_next_antenna(mvm, mvm->nvm_data->valid_tx_ant,
+		iwl_mvm_next_antenna(mvm, iwl_fw_valid_tx_ant(mvm->fw),
 				     mvm->mgmt_last_antenna_idx);
 	rate_flags = BIT(mvm->mgmt_last_antenna_idx) << RATE_MCS_ANT_POS;
 
@@ -365,7 +365,7 @@ int iwl_mvm_tx_skb(struct iwl_mvm *mvm, struct sk_buff *skb,
 	if (WARN_ON_ONCE(!mvmsta))
 		return -1;
 
-	if (WARN_ON_ONCE(mvmsta->sta_id == IWL_INVALID_STATION))
+	if (WARN_ON_ONCE(mvmsta->sta_id == IWL_MVM_STATION_COUNT))
 		return -1;
 
 	dev_cmd = iwl_mvm_set_tx_params(mvm, skb, sta, mvmsta->sta_id);
@@ -417,7 +417,7 @@ int iwl_mvm_tx_skb(struct iwl_mvm *mvm, struct sk_buff *skb,
 	spin_unlock(&mvmsta->lock);
 
 	if (mvmsta->vif->type == NL80211_IFTYPE_AP &&
-	    txq_id < IWL_FIRST_AMPDU_QUEUE)
+	    txq_id < IWL_MVM_FIRST_AGG_QUEUE)
 		atomic_inc(&mvmsta->pending_frames);
 
 	return 0;
@@ -606,7 +606,7 @@ static void iwl_mvm_rx_tx_cmd_single(struct iwl_mvm *mvm,
 					     info);
 
 		/* Single frame failure in an AMPDU queue => send BAR */
-		if (txq_id >= IWL_FIRST_AMPDU_QUEUE &&
+		if (txq_id >= IWL_MVM_FIRST_AGG_QUEUE &&
 		    !(info->flags & IEEE80211_TX_STAT_ACK))
 			info->flags |= IEEE80211_TX_STAT_AMPDU_NO_BACK;
 
@@ -619,7 +619,7 @@ static void iwl_mvm_rx_tx_cmd_single(struct iwl_mvm *mvm,
 		ieee80211_tx_status_ni(mvm->hw, skb);
 	}
 
-	if (txq_id >= IWL_FIRST_AMPDU_QUEUE) {
+	if (txq_id >= IWL_MVM_FIRST_AGG_QUEUE) {
 		/* If this is an aggregation queue, we use the ssn since:
 		 * ssn = wifi seq_num % 256.
 		 * The seq_ctl is the sequence control of the packet to which
@@ -637,14 +637,16 @@ static void iwl_mvm_rx_tx_cmd_single(struct iwl_mvm *mvm,
 		next_reclaimed = ssn;
 	} else {
 		/* The next packet to be reclaimed is the one after this one */
-		next_reclaimed = SEQ_TO_SN(seq_ctl + 0x10);
+		next_reclaimed = IEEE80211_SEQ_TO_SN(seq_ctl + 0x10);
 	}
 
 	IWL_DEBUG_TX_REPLY(mvm,
-			   "TXQ %d status %s (0x%08x)\n\t\t\t\tinitial_rate 0x%x "
-			    "retries %d, idx=%d ssn=%d next_reclaimed=0x%x seq_ctl=0x%x\n",
-			   txq_id, iwl_mvm_get_tx_fail_reason(status),
-			   status, le32_to_cpu(tx_resp->initial_rate),
+			   "TXQ %d status %s (0x%08x)\n",
+			   txq_id, iwl_mvm_get_tx_fail_reason(status), status);
+
+	IWL_DEBUG_TX_REPLY(mvm,
+			   "\t\t\t\tinitial_rate 0x%x retries %d, idx=%d ssn=%d next_reclaimed=0x%x seq_ctl=0x%x\n",
+			   le32_to_cpu(tx_resp->initial_rate),
 			   tx_resp->failure_frame, SEQ_TO_INDEX(sequence),
 			   ssn, next_reclaimed, seq_ctl);
 
@@ -681,7 +683,7 @@ static void iwl_mvm_rx_tx_cmd_single(struct iwl_mvm *mvm,
 	 * If there are no pending frames for this STA, notify mac80211 that
 	 * this station can go to sleep in its STA table.
 	 */
-	if (txq_id < IWL_FIRST_AMPDU_QUEUE && mvmsta &&
+	if (txq_id < IWL_MVM_FIRST_AGG_QUEUE && mvmsta &&
 	    !WARN_ON(skb_freed > 1) &&
 	    mvmsta->vif->type == NL80211_IFTYPE_AP &&
 	    atomic_sub_and_test(skb_freed, &mvmsta->pending_frames)) {
@@ -750,7 +752,7 @@ static void iwl_mvm_rx_tx_cmd_agg(struct iwl_mvm *mvm,
 	u16 sequence = le16_to_cpu(pkt->hdr.sequence);
 	struct ieee80211_sta *sta;
 
-	if (WARN_ON_ONCE(SEQ_TO_QUEUE(sequence) < IWL_FIRST_AMPDU_QUEUE))
+	if (WARN_ON_ONCE(SEQ_TO_QUEUE(sequence) < IWL_MVM_FIRST_AGG_QUEUE))
 		return;
 
 	if (WARN_ON_ONCE(tid == IWL_TID_NON_QOS))
diff --git a/drivers/net/wireless/iwlwifi/mvm/utils.c b/drivers/net/wireless/iwlwifi/mvm/utils.c
index 000e842c2edd..687b34e387ac 100644
--- a/drivers/net/wireless/iwlwifi/mvm/utils.c
+++ b/drivers/net/wireless/iwlwifi/mvm/utils.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -253,8 +253,9 @@ int iwl_mvm_rx_fw_error(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 u8 first_antenna(u8 mask)
 {
 	BUILD_BUG_ON(ANT_A != BIT(0)); /* using ffs is wrong if not */
-	WARN_ON_ONCE(!mask); /* ffs will return 0 if mask is zeroed */
-	return (u8)(BIT(ffs(mask)));
+	if (WARN_ON_ONCE(!mask)) /* ffs will return 0 if mask is zeroed */
+		return BIT(0);
+	return BIT(ffs(mask) - 1);
 }
 
 /*
@@ -462,7 +463,7 @@ int iwl_mvm_send_lq_cmd(struct iwl_mvm *mvm, struct iwl_lq_cmd *lq,
 		.data = { lq, },
 	};
 
-	if (WARN_ON(lq->sta_id == IWL_INVALID_STATION))
+	if (WARN_ON(lq->sta_id == IWL_MVM_STATION_COUNT))
 		return -EINVAL;
 
 	if (WARN_ON(init && (cmd.flags & CMD_ASYNC)))
diff --git a/drivers/net/wireless/iwlwifi/pcie/cfg.h b/drivers/net/wireless/iwlwifi/pcie/cfg.h
deleted file mode 100644
index c6f8e83c3551..000000000000
--- a/drivers/net/wireless/iwlwifi/pcie/cfg.h
+++ /dev/null
@@ -1,115 +0,0 @@
-/******************************************************************************
- *
- * This file is provided under a dual BSD/GPLv2 license.  When using or
- * redistributing this file, you may do so under either license.
- *
- * GPL LICENSE SUMMARY
- *
- * Copyright(c) 2007 - 2013 Intel Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
- * USA
- *
- * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
- *
- * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
- * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
- *
- * BSD LICENSE
- *
- * Copyright(c) 2005 - 2013 Intel Corporation. All rights reserved.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *  * Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- *  * Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- *  * Neither the name Intel Corporation nor the names of its
- *    contributors may be used to endorse or promote products derived
- *    from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *****************************************************************************/
-#ifndef __iwl_pci_h__
-#define __iwl_pci_h__
-
-
-/*
- * This file declares the config structures for all devices.
- */
-
-extern const struct iwl_cfg iwl5300_agn_cfg;
-extern const struct iwl_cfg iwl5100_agn_cfg;
-extern const struct iwl_cfg iwl5350_agn_cfg;
-extern const struct iwl_cfg iwl5100_bgn_cfg;
-extern const struct iwl_cfg iwl5100_abg_cfg;
-extern const struct iwl_cfg iwl5150_agn_cfg;
-extern const struct iwl_cfg iwl5150_abg_cfg;
-extern const struct iwl_cfg iwl6005_2agn_cfg;
-extern const struct iwl_cfg iwl6005_2abg_cfg;
-extern const struct iwl_cfg iwl6005_2bg_cfg;
-extern const struct iwl_cfg iwl6005_2agn_sff_cfg;
-extern const struct iwl_cfg iwl6005_2agn_d_cfg;
-extern const struct iwl_cfg iwl6005_2agn_mow1_cfg;
-extern const struct iwl_cfg iwl6005_2agn_mow2_cfg;
-extern const struct iwl_cfg iwl1030_bgn_cfg;
-extern const struct iwl_cfg iwl1030_bg_cfg;
-extern const struct iwl_cfg iwl6030_2agn_cfg;
-extern const struct iwl_cfg iwl6030_2abg_cfg;
-extern const struct iwl_cfg iwl6030_2bgn_cfg;
-extern const struct iwl_cfg iwl6030_2bg_cfg;
-extern const struct iwl_cfg iwl6000i_2agn_cfg;
-extern const struct iwl_cfg iwl6000i_2abg_cfg;
-extern const struct iwl_cfg iwl6000i_2bg_cfg;
-extern const struct iwl_cfg iwl6000_3agn_cfg;
-extern const struct iwl_cfg iwl6050_2agn_cfg;
-extern const struct iwl_cfg iwl6050_2abg_cfg;
-extern const struct iwl_cfg iwl6150_bgn_cfg;
-extern const struct iwl_cfg iwl6150_bg_cfg;
-extern const struct iwl_cfg iwl1000_bgn_cfg;
-extern const struct iwl_cfg iwl1000_bg_cfg;
-extern const struct iwl_cfg iwl100_bgn_cfg;
-extern const struct iwl_cfg iwl100_bg_cfg;
-extern const struct iwl_cfg iwl130_bgn_cfg;
-extern const struct iwl_cfg iwl130_bg_cfg;
-extern const struct iwl_cfg iwl2000_2bgn_cfg;
-extern const struct iwl_cfg iwl2000_2bgn_d_cfg;
-extern const struct iwl_cfg iwl2030_2bgn_cfg;
-extern const struct iwl_cfg iwl6035_2agn_cfg;
-extern const struct iwl_cfg iwl105_bgn_cfg;
-extern const struct iwl_cfg iwl105_bgn_d_cfg;
-extern const struct iwl_cfg iwl135_bgn_cfg;
-extern const struct iwl_cfg iwl7260_2ac_cfg;
-extern const struct iwl_cfg iwl3160_ac_cfg;
-
-#endif /* __iwl_pci_h__ */
diff --git a/drivers/net/wireless/iwlwifi/pcie/drv.c b/drivers/net/wireless/iwlwifi/pcie/drv.c
index 7bc0fb9128dd..8cb53ec2b77b 100644
--- a/drivers/net/wireless/iwlwifi/pcie/drv.c
+++ b/drivers/net/wireless/iwlwifi/pcie/drv.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -69,8 +69,6 @@
 
 #include "iwl-trans.h"
 #include "iwl-drv.h"
-
-#include "cfg.h"
 #include "internal.h"
 
 #define IWL_PCI_DEVICE(dev, subdev, cfg) \
@@ -243,6 +241,7 @@ static DEFINE_PCI_DEVICE_TABLE(iwl_hw_card_ids) = {
 	{IWL_PCI_DEVICE(0x088F, 0x4260, iwl6035_2agn_cfg)},
 	{IWL_PCI_DEVICE(0x088E, 0x4460, iwl6035_2agn_cfg)},
 	{IWL_PCI_DEVICE(0x088E, 0x4860, iwl6035_2agn_cfg)},
+	{IWL_PCI_DEVICE(0x088F, 0x5260, iwl6035_2agn_cfg)},
 
 /* 105 Series */
 	{IWL_PCI_DEVICE(0x0894, 0x0022, iwl105_bgn_cfg)},
@@ -257,6 +256,7 @@ static DEFINE_PCI_DEVICE_TABLE(iwl_hw_card_ids) = {
 
 /* 7000 Series */
 	{IWL_PCI_DEVICE(0x08B1, 0x4070, iwl7260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x08B1, 0x4062, iwl7260_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x08B1, 0xC070, iwl7260_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x08B3, 0x0070, iwl3160_ac_cfg)},
 	{IWL_PCI_DEVICE(0x08B3, 0x8070, iwl3160_ac_cfg)},
diff --git a/drivers/net/wireless/iwlwifi/pcie/trans.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index 12c4f31ca8fb..50ba0a468f94 100644
--- a/drivers/net/wireless/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -22,7 +22,7 @@
  * USA
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
+ * in the file called COPYING.
  *
  * Contact Information:
  *  Intel Linux Wireless <ilw@linux.intel.com>
@@ -728,7 +728,8 @@ static u32 iwl_trans_pcie_read32(struct iwl_trans *trans, u32 ofs)
 
 static u32 iwl_trans_pcie_read_prph(struct iwl_trans *trans, u32 reg)
 {
-	iwl_trans_pcie_write32(trans, HBUS_TARG_PRPH_RADDR, reg | (3 << 24));
+	iwl_trans_pcie_write32(trans, HBUS_TARG_PRPH_RADDR,
+			       ((reg & 0x000FFFFF) | (3 << 24)));
 	return iwl_trans_pcie_read32(trans, HBUS_TARG_PRPH_RDAT);
 }
 
@@ -736,7 +737,7 @@ static void iwl_trans_pcie_write_prph(struct iwl_trans *trans, u32 addr,
 				      u32 val)
 {
 	iwl_trans_pcie_write32(trans, HBUS_TARG_PRPH_WADDR,
-			       ((addr & 0x0000FFFF) | (3 << 24)));
+			       ((addr & 0x000FFFFF) | (3 << 24)));
 	iwl_trans_pcie_write32(trans, HBUS_TARG_PRPH_WDAT, val);
 }
 
@@ -1383,28 +1384,11 @@ static ssize_t iwl_dbgfs_fh_reg_read(struct file *file,
 	return ret;
 }
 
-static ssize_t iwl_dbgfs_fw_restart_write(struct file *file,
-					  const char __user *user_buf,
-					  size_t count, loff_t *ppos)
-{
-	struct iwl_trans *trans = file->private_data;
-
-	if (!trans->op_mode)
-		return -EAGAIN;
-
-	local_bh_disable();
-	iwl_op_mode_nic_error(trans->op_mode);
-	local_bh_enable();
-
-	return count;
-}
-
 DEBUGFS_READ_WRITE_FILE_OPS(interrupt);
 DEBUGFS_READ_FILE_OPS(fh_reg);
 DEBUGFS_READ_FILE_OPS(rx_queue);
 DEBUGFS_READ_FILE_OPS(tx_queue);
 DEBUGFS_WRITE_FILE_OPS(csr);
-DEBUGFS_WRITE_FILE_OPS(fw_restart);
 
 /*
  * Create the debugfs files and directories
@@ -1418,7 +1402,6 @@ static int iwl_trans_pcie_dbgfs_register(struct iwl_trans *trans,
 	DEBUGFS_ADD_FILE(interrupt, dir, S_IWUSR | S_IRUSR);
 	DEBUGFS_ADD_FILE(csr, dir, S_IWUSR);
 	DEBUGFS_ADD_FILE(fh_reg, dir, S_IRUSR);
-	DEBUGFS_ADD_FILE(fw_restart, dir, S_IWUSR);
 	return 0;
 
 err:
diff --git a/drivers/net/wireless/iwlwifi/pcie/tx.c b/drivers/net/wireless/iwlwifi/pcie/tx.c
index cb5c6792e3a8..c5e30294c5ac 100644
--- a/drivers/net/wireless/iwlwifi/pcie/tx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/tx.c
@@ -501,10 +501,8 @@ static int iwl_pcie_txq_alloc(struct iwl_trans *trans,
 	 * shared with device */
 	txq->tfds = dma_alloc_coherent(trans->dev, tfd_sz,
 				       &txq->q.dma_addr, GFP_KERNEL);
-	if (!txq->tfds) {
-		IWL_ERR(trans, "dma_alloc_coherent(%zd) failed\n", tfd_sz);
+	if (!txq->tfds)
 		goto error;
-	}
 
 	BUILD_BUG_ON(IWL_HCMD_SCRATCHBUF_SIZE != sizeof(*txq->scratchbufs));
 	BUILD_BUG_ON(offsetof(struct iwl_pcie_txq_scratch_buf, scratch) !=
@@ -1063,7 +1061,7 @@ void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, int fifo,
 		iwl_set_bits_prph(trans, SCD_QUEUECHAIN_SEL, BIT(txq_id));
 
 	/* If this queue is mapped to a certain station: it is an AGG queue */
-	if (sta_id != IWL_INVALID_STATION) {
+	if (sta_id >= 0) {
 		u16 ra_tid = BUILD_RAxTID(sta_id, tid);
 
 		/* Map receiver-address / traffic-ID to this queue */
@@ -1566,8 +1564,11 @@ int iwl_trans_pcie_send_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 	if (test_bit(STATUS_FW_ERROR, &trans_pcie->status))
 		return -EIO;
 
-	if (test_bit(STATUS_RFKILL, &trans_pcie->status))
+	if (test_bit(STATUS_RFKILL, &trans_pcie->status)) {
+		IWL_DEBUG_RF_KILL(trans, "Dropping CMD 0x%x: RF KILL\n",
+				  cmd->id);
 		return -ERFKILL;
+	}
 
 	if (cmd->flags & CMD_ASYNC)
 		return iwl_pcie_send_hcmd_async(trans, cmd);
@@ -1609,7 +1610,7 @@ int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
 	 * Check here that the packets are in the right place on the ring.
 	 */
 #ifdef CONFIG_IWLWIFI_DEBUG
-	wifi_seq = SEQ_TO_SN(le16_to_cpu(hdr->seq_ctrl));
+	wifi_seq = IEEE80211_SEQ_TO_SN(le16_to_cpu(hdr->seq_ctrl));
 	WARN_ONCE((iwl_read_prph(trans, SCD_AGGR_SEL) & BIT(txq_id)) &&
 		  ((wifi_seq & 0xff) != q->write_ptr),
 		  "Q: %d WiFi Seq %d tfdNum %d",
diff --git a/drivers/net/wireless/libertas_tf/main.c b/drivers/net/wireless/libertas_tf/main.c
index 7001856241e6..088de9d25c39 100644
--- a/drivers/net/wireless/libertas_tf/main.c
+++ b/drivers/net/wireless/libertas_tf/main.c
@@ -412,9 +412,9 @@ static int lbtf_op_config(struct ieee80211_hw *hw, u32 changed)
 	struct ieee80211_conf *conf = &hw->conf;
 	lbtf_deb_enter(LBTF_DEB_MACOPS);
 
-	if (conf->channel->center_freq != priv->cur_freq) {
-		priv->cur_freq = conf->channel->center_freq;
-		lbtf_set_channel(priv, conf->channel->hw_value);
+	if (conf->chandef.chan->center_freq != priv->cur_freq) {
+		priv->cur_freq = conf->chandef.chan->center_freq;
+		lbtf_set_channel(priv, conf->chandef.chan->hw_value);
 	}
 	lbtf_deb_leave(LBTF_DEB_MACOPS);
 	return 0;
@@ -537,7 +537,7 @@ static int lbtf_op_get_survey(struct ieee80211_hw *hw, int idx,
 	if (idx != 0)
 		return -ENOENT;
 
-	survey->channel = conf->channel;
+	survey->channel = conf->chandef.chan;
 	survey->filled = SURVEY_INFO_NOISE_DBM;
 	survey->noise = priv->noise;
 
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index cffdf4fbf161..b878a32e7a98 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -25,6 +25,7 @@
 #include <linux/if_arp.h>
 #include <linux/rtnetlink.h>
 #include <linux/etherdevice.h>
+#include <linux/platform_device.h>
 #include <linux/debugfs.h>
 #include <linux/module.h>
 #include <linux/ktime.h>
@@ -52,6 +53,10 @@ static bool paged_rx = false;
 module_param(paged_rx, bool, 0644);
 MODULE_PARM_DESC(paged_rx, "Use paged SKBs for RX instead of linear ones");
 
+static bool rctbl = false;
+module_param(rctbl, bool, 0444);
+MODULE_PARM_DESC(rctbl, "Handle rate control table");
+
 /**
  * enum hwsim_regtest - the type of regulatory tests we offer
  *
@@ -717,9 +722,17 @@ static bool mac80211_hwsim_tx_frame_no_nl(struct ieee80211_hw *hw,
 	rx_status.flag |= RX_FLAG_MACTIME_START;
 	rx_status.freq = chan->center_freq;
 	rx_status.band = chan->band;
-	rx_status.rate_idx = info->control.rates[0].idx;
-	if (info->control.rates[0].flags & IEEE80211_TX_RC_MCS)
-		rx_status.flag |= RX_FLAG_HT;
+	if (info->control.rates[0].flags & IEEE80211_TX_RC_VHT_MCS) {
+		rx_status.rate_idx =
+			ieee80211_rate_get_vht_mcs(&info->control.rates[0]);
+		rx_status.vht_nss =
+			ieee80211_rate_get_vht_nss(&info->control.rates[0]);
+		rx_status.flag |= RX_FLAG_VHT;
+	} else {
+		rx_status.rate_idx = info->control.rates[0].idx;
+		if (info->control.rates[0].flags & IEEE80211_TX_RC_MCS)
+			rx_status.flag |= RX_FLAG_HT;
+	}
 	if (info->control.rates[0].flags & IEEE80211_TX_RC_40_MHZ_WIDTH)
 		rx_status.flag |= RX_FLAG_40MHZ;
 	if (info->control.rates[0].flags & IEEE80211_TX_RC_SHORT_GI)
@@ -886,8 +899,12 @@ static void mac80211_hwsim_tx(struct ieee80211_hw *hw,
 	if (control->sta)
 		hwsim_check_sta_magic(control->sta);
 
-	txi->rate_driver_data[0] = channel;
+	if (rctbl)
+		ieee80211_get_tx_rates(txi->control.vif, control->sta, skb,
+				       txi->control.rates,
+				       ARRAY_SIZE(txi->control.rates));
 
+	txi->rate_driver_data[0] = channel;
 	mac80211_hwsim_monitor_rx(hw, skb, channel);
 
 	/* wmediumd mode check */
@@ -964,6 +981,12 @@ static int mac80211_hwsim_change_interface(struct ieee80211_hw *hw,
 		    newtype, vif->addr);
 	hwsim_check_magic(vif);
 
+	/*
+	 * interface may change from non-AP to AP in
+	 * which case this needs to be set up again
+	 */
+	vif->cab_queue = 0;
+
 	return 0;
 }
 
@@ -983,6 +1006,13 @@ static void mac80211_hwsim_tx_frame(struct ieee80211_hw *hw,
 {
 	u32 _pid = ACCESS_ONCE(wmediumd_portid);
 
+	if (rctbl) {
+		struct ieee80211_tx_info *txi = IEEE80211_SKB_CB(skb);
+		ieee80211_get_tx_rates(txi->control.vif, NULL, skb,
+				       txi->control.rates,
+				       ARRAY_SIZE(txi->control.rates));
+	}
+
 	mac80211_hwsim_monitor_rx(hw, skb, chan);
 
 	if (_pid)
@@ -1013,6 +1043,11 @@ static void mac80211_hwsim_beacon_tx(void *arg, u8 *mac,
 	if (skb == NULL)
 		return;
 	info = IEEE80211_SKB_CB(skb);
+	if (rctbl)
+		ieee80211_get_tx_rates(vif, NULL, skb,
+				       info->control.rates,
+				       ARRAY_SIZE(info->control.rates));
+
 	txrate = ieee80211_get_tx_rate(hw, info);
 
 	mgmt = (struct ieee80211_mgmt *) skb->data;
@@ -1056,11 +1091,13 @@ out:
 	return HRTIMER_NORESTART;
 }
 
-static const char *hwsim_chantypes[] = {
-	[NL80211_CHAN_NO_HT] = "noht",
-	[NL80211_CHAN_HT20] = "ht20",
-	[NL80211_CHAN_HT40MINUS] = "ht40-",
-	[NL80211_CHAN_HT40PLUS] = "ht40+",
+static const char * const hwsim_chanwidths[] = {
+	[NL80211_CHAN_WIDTH_20_NOHT] = "noht",
+	[NL80211_CHAN_WIDTH_20] = "ht20",
+	[NL80211_CHAN_WIDTH_40] = "ht40",
+	[NL80211_CHAN_WIDTH_80] = "vht80",
+	[NL80211_CHAN_WIDTH_80P80] = "vht80p80",
+	[NL80211_CHAN_WIDTH_160] = "vht160",
 };
 
 static int mac80211_hwsim_config(struct ieee80211_hw *hw, u32 changed)
@@ -1074,18 +1111,28 @@ static int mac80211_hwsim_config(struct ieee80211_hw *hw, u32 changed)
 		[IEEE80211_SMPS_DYNAMIC] = "dynamic",
 	};
 
-	wiphy_debug(hw->wiphy,
-		    "%s (freq=%d/%s idle=%d ps=%d smps=%s)\n",
-		    __func__,
-		    conf->channel ? conf->channel->center_freq : 0,
-		    hwsim_chantypes[conf->channel_type],
-		    !!(conf->flags & IEEE80211_CONF_IDLE),
-		    !!(conf->flags & IEEE80211_CONF_PS),
-		    smps_modes[conf->smps_mode]);
+	if (conf->chandef.chan)
+		wiphy_debug(hw->wiphy,
+			    "%s (freq=%d(%d - %d)/%s idle=%d ps=%d smps=%s)\n",
+			    __func__,
+			    conf->chandef.chan->center_freq,
+			    conf->chandef.center_freq1,
+			    conf->chandef.center_freq2,
+			    hwsim_chanwidths[conf->chandef.width],
+			    !!(conf->flags & IEEE80211_CONF_IDLE),
+			    !!(conf->flags & IEEE80211_CONF_PS),
+			    smps_modes[conf->smps_mode]);
+	else
+		wiphy_debug(hw->wiphy,
+			    "%s (freq=0 idle=%d ps=%d smps=%s)\n",
+			    __func__,
+			    !!(conf->flags & IEEE80211_CONF_IDLE),
+			    !!(conf->flags & IEEE80211_CONF_PS),
+			    smps_modes[conf->smps_mode]);
 
 	data->idle = !!(conf->flags & IEEE80211_CONF_IDLE);
 
-	data->channel = conf->channel;
+	data->channel = conf->chandef.chan;
 
 	WARN_ON(data->channel && channels > 1);
 
@@ -1271,7 +1318,7 @@ static int mac80211_hwsim_get_survey(
 		return -ENOENT;
 
 	/* Current channel */
-	survey->channel = conf->channel;
+	survey->channel = conf->chandef.chan;
 
 	/*
 	 * Magically conjured noise level --- this is only ok for simulated hardware.
@@ -1389,7 +1436,7 @@ static int mac80211_hwsim_ampdu_action(struct ieee80211_hw *hw,
 	return 0;
 }
 
-static void mac80211_hwsim_flush(struct ieee80211_hw *hw, bool drop)
+static void mac80211_hwsim_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 {
 	/* Not implemented, queues only on kernel side */
 }
@@ -1535,7 +1582,8 @@ static void hw_roc_done(struct work_struct *work)
 static int mac80211_hwsim_roc(struct ieee80211_hw *hw,
 			      struct ieee80211_vif *vif,
 			      struct ieee80211_channel *chan,
-			      int duration)
+			      int duration,
+			      enum ieee80211_roc_type type)
 {
 	struct mac80211_hwsim_data *hwsim = hw->priv;
 
@@ -1668,6 +1716,7 @@ static void mac80211_hwsim_free(void)
 		debugfs_remove(data->debugfs_ps);
 		debugfs_remove(data->debugfs);
 		ieee80211_unregister_hw(data->hw);
+		device_release_driver(data->dev);
 		device_unregister(data->dev);
 		ieee80211_free_hw(data->hw);
 	}
@@ -1676,7 +1725,9 @@ static void mac80211_hwsim_free(void)
 
 
 static struct device_driver mac80211_hwsim_driver = {
-	.name = "mac80211_hwsim"
+	.name = "mac80211_hwsim",
+	.bus = &platform_bus_type,
+	.owner = THIS_MODULE,
 };
 
 static const struct net_device_ops hwsim_netdev_ops = {
@@ -2168,9 +2219,15 @@ static int __init init_mac80211_hwsim(void)
 	spin_lock_init(&hwsim_radio_lock);
 	INIT_LIST_HEAD(&hwsim_radios);
 
+	err = driver_register(&mac80211_hwsim_driver);
+	if (err)
+		return err;
+
 	hwsim_class = class_create(THIS_MODULE, "mac80211_hwsim");
-	if (IS_ERR(hwsim_class))
-		return PTR_ERR(hwsim_class);
+	if (IS_ERR(hwsim_class)) {
+		err = PTR_ERR(hwsim_class);
+		goto failed_unregister_driver;
+	}
 
 	memset(addr, 0, ETH_ALEN);
 	addr[0] = 0x02;
@@ -2192,12 +2249,20 @@ static int __init init_mac80211_hwsim(void)
 					  "hwsim%d", i);
 		if (IS_ERR(data->dev)) {
 			printk(KERN_DEBUG
-			       "mac80211_hwsim: device_create "
-			       "failed (%ld)\n", PTR_ERR(data->dev));
+			       "mac80211_hwsim: device_create failed (%ld)\n",
+			       PTR_ERR(data->dev));
 			err = -ENOMEM;
 			goto failed_drvdata;
 		}
 		data->dev->driver = &mac80211_hwsim_driver;
+		err = device_bind_driver(data->dev);
+		if (err != 0) {
+			printk(KERN_DEBUG
+			       "mac80211_hwsim: device_bind_driver failed (%d)\n",
+			       err);
+			goto failed_hw;
+		}
+
 		skb_queue_head_init(&data->pending);
 
 		SET_IEEE80211_DEV(hw, data->dev);
@@ -2240,6 +2305,8 @@ static int __init init_mac80211_hwsim(void)
 			    IEEE80211_HW_AMPDU_AGGREGATION |
 			    IEEE80211_HW_WANT_MONITOR_VIF |
 			    IEEE80211_HW_QUEUE_CONTROL;
+		if (rctbl)
+			hw->flags |= IEEE80211_HW_SUPPORTS_RC_TABLE;
 
 		hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS |
 				    WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
@@ -2291,9 +2358,6 @@ static int __init init_mac80211_hwsim(void)
 
 			hw->wiphy->bands[band] = sband;
 
-			if (channels == 1)
-				continue;
-
 			sband->vht_cap.vht_supported = true;
 			sband->vht_cap.cap =
 				IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_11454 |
@@ -2499,6 +2563,8 @@ failed_drvdata:
 	ieee80211_free_hw(hw);
 failed:
 	mac80211_hwsim_free();
+failed_unregister_driver:
+	driver_unregister(&mac80211_hwsim_driver);
 	return err;
 }
 module_init(init_mac80211_hwsim);
@@ -2511,5 +2577,6 @@ static void __exit exit_mac80211_hwsim(void)
 
 	mac80211_hwsim_free();
 	unregister_netdev(hwsim_mon);
+	driver_unregister(&mac80211_hwsim_driver);
 }
 module_exit(exit_mac80211_hwsim);
diff --git a/drivers/net/wireless/mwifiex/11ac.c b/drivers/net/wireless/mwifiex/11ac.c
index cf43b3c29250..5e0eec4d71c7 100644
--- a/drivers/net/wireless/mwifiex/11ac.c
+++ b/drivers/net/wireless/mwifiex/11ac.c
@@ -200,7 +200,7 @@ int mwifiex_cmd_append_11ac_tlv(struct mwifiex_private *priv,
 
 	/* VHT Operation IE */
 	if (bss_desc->bcn_vht_oper) {
-		if (priv->bss_mode == HostCmd_BSS_MODE_IBSS) {
+		if (priv->bss_mode == NL80211_IFTYPE_STATION) {
 			vht_op = (struct mwifiex_ie_types_vht_oper *)*buffer;
 			memset(vht_op, 0, sizeof(*vht_op));
 			vht_op->header.type =
@@ -259,3 +259,44 @@ int mwifiex_cmd_append_11ac_tlv(struct mwifiex_private *priv,
 
 	return ret_len;
 }
+
+int mwifiex_cmd_11ac_cfg(struct mwifiex_private *priv,
+			 struct host_cmd_ds_command *cmd, u16 cmd_action,
+			 struct mwifiex_11ac_vht_cfg *cfg)
+{
+	struct host_cmd_11ac_vht_cfg *vhtcfg = &cmd->params.vht_cfg;
+
+	cmd->command = cpu_to_le16(HostCmd_CMD_11AC_CFG);
+	cmd->size = cpu_to_le16(sizeof(struct host_cmd_11ac_vht_cfg) +
+				S_DS_GEN);
+	vhtcfg->action = cpu_to_le16(cmd_action);
+	vhtcfg->band_config = cfg->band_config;
+	vhtcfg->misc_config = cfg->misc_config;
+	vhtcfg->cap_info = cpu_to_le32(cfg->cap_info);
+	vhtcfg->mcs_tx_set = cpu_to_le32(cfg->mcs_tx_set);
+	vhtcfg->mcs_rx_set = cpu_to_le32(cfg->mcs_rx_set);
+
+	return 0;
+}
+
+/* This function initializes the BlockACK setup information for given
+ * mwifiex_private structure for 11ac enabled networks.
+ */
+void mwifiex_set_11ac_ba_params(struct mwifiex_private *priv)
+{
+	priv->add_ba_param.timeout = MWIFIEX_DEFAULT_BLOCK_ACK_TIMEOUT;
+
+	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP) {
+		priv->add_ba_param.tx_win_size =
+					   MWIFIEX_11AC_UAP_AMPDU_DEF_TXWINSIZE;
+		priv->add_ba_param.rx_win_size =
+					   MWIFIEX_11AC_UAP_AMPDU_DEF_RXWINSIZE;
+	} else {
+		priv->add_ba_param.tx_win_size =
+					   MWIFIEX_11AC_STA_AMPDU_DEF_TXWINSIZE;
+		priv->add_ba_param.rx_win_size =
+					   MWIFIEX_11AC_STA_AMPDU_DEF_RXWINSIZE;
+	}
+
+	return;
+}
diff --git a/drivers/net/wireless/mwifiex/11ac.h b/drivers/net/wireless/mwifiex/11ac.h
index 80fd1ba46200..7c2c69b5b3eb 100644
--- a/drivers/net/wireless/mwifiex/11ac.h
+++ b/drivers/net/wireless/mwifiex/11ac.h
@@ -20,7 +20,24 @@
 #ifndef _MWIFIEX_11AC_H_
 #define _MWIFIEX_11AC_H_
 
+#define VHT_CFG_2GHZ BIT(0)
+#define VHT_CFG_5GHZ BIT(1)
+
+enum vht_cfg_misc_config {
+	VHT_CAP_TX_OPERATION = 1,
+	VHT_CAP_ASSOCIATION,
+	VHT_CAP_UAP_ONLY
+};
+
+#define DEFAULT_VHT_MCS_SET 0xfffa
+#define DISABLE_VHT_MCS_SET 0xffff
+
+#define VHT_BW_80_160_80P80 BIT(2)
+
 int mwifiex_cmd_append_11ac_tlv(struct mwifiex_private *priv,
 				struct mwifiex_bssdescriptor *bss_desc,
 				u8 **buffer);
+int mwifiex_cmd_11ac_cfg(struct mwifiex_private *priv,
+			 struct host_cmd_ds_command *cmd, u16 cmd_action,
+			 struct mwifiex_11ac_vht_cfg *cfg);
 #endif /* _MWIFIEX_11AC_H_ */
diff --git a/drivers/net/wireless/mwifiex/11n.c b/drivers/net/wireless/mwifiex/11n.c
index 45f19716687e..41e9d25a2d8e 100644
--- a/drivers/net/wireless/mwifiex/11n.c
+++ b/drivers/net/wireless/mwifiex/11n.c
@@ -679,3 +679,25 @@ void mwifiex_del_tx_ba_stream_tbl_by_ra(struct mwifiex_private *priv, u8 *ra)
 
 	return;
 }
+
+/* This function initializes the BlockACK setup information for given
+ * mwifiex_private structure.
+ */
+void mwifiex_set_ba_params(struct mwifiex_private *priv)
+{
+	priv->add_ba_param.timeout = MWIFIEX_DEFAULT_BLOCK_ACK_TIMEOUT;
+
+	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP) {
+		priv->add_ba_param.tx_win_size =
+						MWIFIEX_UAP_AMPDU_DEF_TXWINSIZE;
+		priv->add_ba_param.rx_win_size =
+						MWIFIEX_UAP_AMPDU_DEF_RXWINSIZE;
+	} else {
+		priv->add_ba_param.tx_win_size =
+						MWIFIEX_STA_AMPDU_DEF_TXWINSIZE;
+		priv->add_ba_param.rx_win_size =
+						MWIFIEX_STA_AMPDU_DEF_RXWINSIZE;
+	}
+
+	return;
+}
diff --git a/drivers/net/wireless/mwifiex/11n_aggr.c b/drivers/net/wireless/mwifiex/11n_aggr.c
index af8fe6352eed..a78e0651409c 100644
--- a/drivers/net/wireless/mwifiex/11n_aggr.c
+++ b/drivers/net/wireless/mwifiex/11n_aggr.c
@@ -296,19 +296,7 @@ mwifiex_11n_aggregate_pkt(struct mwifiex_private *priv,
 		break;
 	}
 	if (ret != -EBUSY) {
-		spin_lock_irqsave(&priv->wmm.ra_list_spinlock, ra_list_flags);
-		if (mwifiex_is_ralist_valid(priv, pra_list, ptrindex)) {
-			priv->wmm.packets_out[ptrindex]++;
-			priv->wmm.tid_tbl_ptr[ptrindex].ra_list_curr = pra_list;
-		}
-		/* Now bss_prio_cur pointer points to next node */
-		adapter->bss_prio_tbl[priv->bss_priority].bss_prio_cur =
-			list_first_entry(
-				&adapter->bss_prio_tbl[priv->bss_priority]
-				.bss_prio_cur->list,
-				struct mwifiex_bss_prio_node, list);
-		spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
-				       ra_list_flags);
+		mwifiex_rotate_priolists(priv, pra_list, ptrindex);
 	}
 
 	return 0;
diff --git a/drivers/net/wireless/mwifiex/11n_rxreorder.c b/drivers/net/wireless/mwifiex/11n_rxreorder.c
index 5e796f847088..ada809f576fe 100644
--- a/drivers/net/wireless/mwifiex/11n_rxreorder.c
+++ b/drivers/net/wireless/mwifiex/11n_rxreorder.c
@@ -447,7 +447,7 @@ int mwifiex_11n_rx_reorder_pkt(struct mwifiex_private *priv,
 	end_win = ((start_win + win_size) - 1) & (MAX_TID_VALUE - 1);
 	del_timer(&tbl->timer_context.timer);
 	mod_timer(&tbl->timer_context.timer,
-		  jiffies + (MIN_FLUSH_TIMER_MS * win_size * HZ) / 1000);
+		  jiffies + msecs_to_jiffies(MIN_FLUSH_TIMER_MS * win_size));
 
 	/*
 	 * If seq_num is less then starting win then ignore and drop the
diff --git a/drivers/net/wireless/mwifiex/Makefile b/drivers/net/wireless/mwifiex/Makefile
index 97b245cbafd8..ecf28464367f 100644
--- a/drivers/net/wireless/mwifiex/Makefile
+++ b/drivers/net/wireless/mwifiex/Makefile
@@ -39,6 +39,7 @@ mwifiex-y += sta_tx.o
 mwifiex-y += sta_rx.o
 mwifiex-y += uap_txrx.o
 mwifiex-y += cfg80211.o
+mwifiex-y += ethtool.o
 mwifiex-$(CONFIG_DEBUG_FS) += debugfs.o
 obj-$(CONFIG_MWIFIEX) += mwifiex.o
 
diff --git a/drivers/net/wireless/mwifiex/cfg80211.c b/drivers/net/wireless/mwifiex/cfg80211.c
index 8aaf56ade4d9..d3c8ece980d8 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/mwifiex/cfg80211.c
@@ -216,7 +216,7 @@ mwifiex_cfg80211_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
 	mwifiex_form_mgmt_frame(skb, buf, len);
 	mwifiex_queue_tx_pkt(priv, skb);
 
-	*cookie = random32() | 1;
+	*cookie = prandom_u32() | 1;
 	cfg80211_mgmt_tx_status(wdev, *cookie, buf, len, true, GFP_ATOMIC);
 
 	wiphy_dbg(wiphy, "info: management frame transmitted\n");
@@ -271,7 +271,7 @@ mwifiex_cfg80211_remain_on_channel(struct wiphy *wiphy,
 					 duration);
 
 	if (!ret) {
-		*cookie = random32() | 1;
+		*cookie = prandom_u32() | 1;
 		priv->roc_cfg.cookie = *cookie;
 		priv->roc_cfg.chan = *chan;
 
@@ -1374,6 +1374,18 @@ static int mwifiex_cfg80211_start_ap(struct wiphy *wiphy,
 	}
 
 	mwifiex_set_ht_params(priv, bss_cfg, params);
+
+	if (priv->adapter->is_hw_11ac_capable) {
+		mwifiex_set_vht_params(priv, bss_cfg, params);
+		mwifiex_set_vht_width(priv, params->chandef.width,
+				      priv->ap_11ac_enabled);
+	}
+
+	if (priv->ap_11ac_enabled)
+		mwifiex_set_11ac_ba_params(priv);
+	else
+		mwifiex_set_ba_params(priv);
+
 	mwifiex_set_wmm_params(priv, bss_cfg, params);
 
 	if (params->inactivity_timeout > 0) {
@@ -1654,17 +1666,13 @@ mwifiex_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 			 struct cfg80211_connect_params *sme)
 {
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
-	int ret = 0;
-
-	if (priv->bss_mode == NL80211_IFTYPE_ADHOC) {
-		wiphy_err(wiphy, "received infra assoc request "
-				"when station is in ibss mode\n");
-		goto done;
-	}
+	int ret;
 
-	if (priv->bss_mode == NL80211_IFTYPE_AP) {
-		wiphy_err(wiphy, "skip association request for AP interface\n");
-		goto done;
+	if (priv->bss_mode != NL80211_IFTYPE_STATION) {
+		wiphy_err(wiphy,
+			  "%s: reject infra assoc request in non-STA mode\n",
+			  dev->name);
+		return -EINVAL;
 	}
 
 	wiphy_dbg(wiphy, "info: Trying to associate to %s and bssid %pM\n",
@@ -1672,7 +1680,6 @@ mwifiex_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 
 	ret = mwifiex_cfg80211_assoc(priv, sme->ssid_len, sme->ssid, sme->bssid,
 				     priv->bss_mode, sme->channel, sme, 0);
-done:
 	if (!ret) {
 		cfg80211_connect_result(priv->netdev, priv->cfg_bssid, NULL, 0,
 					NULL, 0, WLAN_STATUS_SUCCESS,
@@ -1933,66 +1940,10 @@ static void mwifiex_setup_vht_caps(struct ieee80211_sta_vht_cap *vht_info,
 				   struct mwifiex_private *priv)
 {
 	struct mwifiex_adapter *adapter = priv->adapter;
-	u32 vht_cap = 0, cap = adapter->hw_dot_11ac_dev_cap;
 
 	vht_info->vht_supported = true;
 
-	switch (GET_VHTCAP_MAXMPDULEN(cap)) {
-	case 0x00:
-		vht_cap |= IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_3895;
-		break;
-	case 0x01:
-		vht_cap |= IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_7991;
-		break;
-	case 0x10:
-		vht_cap |= IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_11454;
-	    break;
-	default:
-	    dev_err(adapter->dev, "unsupported MAX MPDU len\n");
-	    break;
-	}
-
-	if (ISSUPP_11ACVHTHTCVHT(cap))
-		vht_cap |= IEEE80211_VHT_CAP_HTC_VHT;
-
-	if (ISSUPP_11ACVHTTXOPPS(cap))
-		vht_cap |= IEEE80211_VHT_CAP_VHT_TXOP_PS;
-
-	if (ISSUPP_11ACMURXBEAMFORMEE(cap))
-		vht_cap |= IEEE80211_VHT_CAP_MU_BEAMFORMER_CAPABLE;
-
-	if (ISSUPP_11ACMUTXBEAMFORMEE(cap))
-		vht_cap |= IEEE80211_VHT_CAP_MU_BEAMFORMEE_CAPABLE;
-
-	if (ISSUPP_11ACSUBEAMFORMER(cap))
-		vht_cap |= IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE;
-
-	if (ISSUPP_11ACSUBEAMFORMEE(cap))
-		vht_cap |= IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE;
-
-	if (ISSUPP_11ACRXSTBC(cap))
-		vht_cap |= IEEE80211_VHT_CAP_RXSTBC_1;
-
-	if (ISSUPP_11ACTXSTBC(cap))
-		vht_cap |= IEEE80211_VHT_CAP_TXSTBC;
-
-	if (ISSUPP_11ACSGI160(cap))
-		vht_cap |= IEEE80211_VHT_CAP_SHORT_GI_160;
-
-	if (ISSUPP_11ACSGI80(cap))
-		vht_cap |= IEEE80211_VHT_CAP_SHORT_GI_80;
-
-	if (ISSUPP_11ACLDPC(cap))
-		vht_cap |= IEEE80211_VHT_CAP_RXLDPC;
-
-	if (ISSUPP_11ACBW8080(cap))
-		vht_cap |= IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160_80PLUS80MHZ;
-
-	if (ISSUPP_11ACBW160(cap))
-		vht_cap |= IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160MHZ;
-
-	vht_info->cap = vht_cap;
-
+	vht_info->cap = adapter->hw_dot_11ac_dev_cap;
 	/* Update MCS support for VHT */
 	vht_info->vht_mcs.rx_mcs_map = cpu_to_le16(
 				adapter->hw_dot_11ac_mcs_support & 0xFFFF);
@@ -2180,10 +2131,9 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 
 		/* At start-up, wpa_supplicant tries to change the interface
 		 * to NL80211_IFTYPE_STATION if it is not managed mode.
-		 * So, we initialize it to STA mode.
 		 */
-		wdev->iftype = NL80211_IFTYPE_STATION;
-		priv->bss_mode = NL80211_IFTYPE_STATION;
+		wdev->iftype = NL80211_IFTYPE_P2P_CLIENT;
+		priv->bss_mode = NL80211_IFTYPE_P2P_CLIENT;
 
 		/* Setting bss_type to P2P tells firmware that this interface
 		 * is receiving P2P peers found during find phase and doing
@@ -2197,6 +2147,9 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 		priv->bss_started = 0;
 		priv->bss_num = 0;
 
+		if (mwifiex_cfg80211_init_p2p_client(priv))
+			return ERR_PTR(-EFAULT);
+
 		break;
 	default:
 		wiphy_err(wiphy, "type not supported\n");
@@ -2236,6 +2189,7 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 	dev->flags |= IFF_BROADCAST | IFF_MULTICAST;
 	dev->watchdog_timeo = MWIFIEX_DEFAULT_WATCHDOG_TIMEOUT;
 	dev->hard_header_len += MWIFIEX_MIN_DATA_HEADER_LEN;
+	dev->ethtool_ops = &mwifiex_ethtool_ops;
 
 	mdev_priv = netdev_priv(dev);
 	*((unsigned long *) mdev_priv) = (unsigned long) priv;
@@ -2294,6 +2248,152 @@ int mwifiex_del_virtual_intf(struct wiphy *wiphy, struct wireless_dev *wdev)
 }
 EXPORT_SYMBOL_GPL(mwifiex_del_virtual_intf);
 
+#ifdef CONFIG_PM
+static bool
+mwifiex_is_pattern_supported(struct cfg80211_wowlan_trig_pkt_pattern *pat,
+			     s8 *byte_seq)
+{
+	int j, k, valid_byte_cnt = 0;
+	bool dont_care_byte = false;
+
+	for (j = 0; j < DIV_ROUND_UP(pat->pattern_len, 8); j++) {
+		for (k = 0; k < 8; k++) {
+			if (pat->mask[j] & 1 << k) {
+				memcpy(byte_seq + valid_byte_cnt,
+				       &pat->pattern[j * 8 + k], 1);
+				valid_byte_cnt++;
+				if (dont_care_byte)
+					return false;
+			} else {
+				if (valid_byte_cnt)
+					dont_care_byte = true;
+			}
+
+			if (valid_byte_cnt > MAX_BYTESEQ)
+				return false;
+		}
+	}
+
+	byte_seq[MAX_BYTESEQ] = valid_byte_cnt;
+
+	return true;
+}
+
+static int mwifiex_cfg80211_suspend(struct wiphy *wiphy,
+				    struct cfg80211_wowlan *wowlan)
+{
+	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
+	struct mwifiex_ds_mef_cfg mef_cfg;
+	struct mwifiex_mef_entry *mef_entry;
+	int i, filt_num = 0, ret;
+	bool first_pat = true;
+	u8 byte_seq[MAX_BYTESEQ + 1];
+	const u8 ipv4_mc_mac[] = {0x33, 0x33};
+	const u8 ipv6_mc_mac[] = {0x01, 0x00, 0x5e};
+	struct mwifiex_private *priv =
+			mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_STA);
+
+	if (!wowlan) {
+		dev_warn(adapter->dev, "None of the WOWLAN triggers enabled\n");
+		return 0;
+	}
+
+	if (!priv->media_connected) {
+		dev_warn(adapter->dev,
+			 "Can not configure WOWLAN in disconnected state\n");
+		return 0;
+	}
+
+	mef_entry = kzalloc(sizeof(*mef_entry), GFP_KERNEL);
+	if (!mef_entry)
+		return -ENOMEM;
+
+	memset(&mef_cfg, 0, sizeof(mef_cfg));
+	mef_cfg.num_entries = 1;
+	mef_cfg.mef_entry = mef_entry;
+	mef_entry->mode = MEF_MODE_HOST_SLEEP;
+	mef_entry->action = MEF_ACTION_ALLOW_AND_WAKEUP_HOST;
+
+	for (i = 0; i < wowlan->n_patterns; i++) {
+		memset(byte_seq, 0, sizeof(byte_seq));
+		if (!mwifiex_is_pattern_supported(&wowlan->patterns[i],
+						  byte_seq)) {
+			wiphy_err(wiphy, "Pattern not supported\n");
+			kfree(mef_entry);
+			return -EOPNOTSUPP;
+		}
+
+		if (!wowlan->patterns[i].pkt_offset) {
+			if (!(byte_seq[0] & 0x01) &&
+			    (byte_seq[MAX_BYTESEQ] == 1)) {
+				mef_cfg.criteria |= MWIFIEX_CRITERIA_UNICAST;
+				continue;
+			} else if (is_broadcast_ether_addr(byte_seq)) {
+				mef_cfg.criteria |= MWIFIEX_CRITERIA_BROADCAST;
+				continue;
+			} else if ((!memcmp(byte_seq, ipv4_mc_mac, 2) &&
+				    (byte_seq[MAX_BYTESEQ] == 2)) ||
+				   (!memcmp(byte_seq, ipv6_mc_mac, 3) &&
+				    (byte_seq[MAX_BYTESEQ] == 3))) {
+				mef_cfg.criteria |= MWIFIEX_CRITERIA_MULTICAST;
+				continue;
+			}
+		}
+
+		mef_entry->filter[filt_num].repeat = 1;
+		mef_entry->filter[filt_num].offset =
+						wowlan->patterns[i].pkt_offset;
+		memcpy(mef_entry->filter[filt_num].byte_seq, byte_seq,
+		       sizeof(byte_seq));
+		mef_entry->filter[filt_num].filt_type = TYPE_EQ;
+
+		if (first_pat)
+			first_pat = false;
+		else
+			mef_entry->filter[filt_num].filt_action = TYPE_AND;
+
+		filt_num++;
+	}
+
+	if (wowlan->magic_pkt) {
+		mef_cfg.criteria |= MWIFIEX_CRITERIA_UNICAST;
+		mef_entry->filter[filt_num].repeat = 16;
+		memcpy(mef_entry->filter[filt_num].byte_seq, priv->curr_addr,
+		       ETH_ALEN);
+		mef_entry->filter[filt_num].byte_seq[MAX_BYTESEQ] = ETH_ALEN;
+		mef_entry->filter[filt_num].offset = 14;
+		mef_entry->filter[filt_num].filt_type = TYPE_EQ;
+		if (filt_num)
+			mef_entry->filter[filt_num].filt_action = TYPE_OR;
+	}
+
+	if (!mef_cfg.criteria)
+		mef_cfg.criteria = MWIFIEX_CRITERIA_BROADCAST |
+				   MWIFIEX_CRITERIA_UNICAST |
+				   MWIFIEX_CRITERIA_MULTICAST;
+
+	ret =  mwifiex_send_cmd_sync(priv, HostCmd_CMD_MEF_CFG,
+				     HostCmd_ACT_GEN_SET, 0,
+				     &mef_cfg);
+
+	kfree(mef_entry);
+	return ret;
+}
+
+static int mwifiex_cfg80211_resume(struct wiphy *wiphy)
+{
+	return 0;
+}
+
+static void mwifiex_cfg80211_set_wakeup(struct wiphy *wiphy,
+				       bool enabled)
+{
+	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
+
+	device_set_wakeup_enable(adapter->dev, enabled);
+}
+#endif
+
 /* station cfg80211 operations */
 static struct cfg80211_ops mwifiex_cfg80211_ops = {
 	.add_virtual_intf = mwifiex_add_virtual_intf,
@@ -2322,6 +2422,11 @@ static struct cfg80211_ops mwifiex_cfg80211_ops = {
 	.change_beacon = mwifiex_cfg80211_change_beacon,
 	.set_cqm_rssi_config = mwifiex_cfg80211_set_cqm_rssi_config,
 	.set_antenna = mwifiex_cfg80211_set_antenna,
+#ifdef CONFIG_PM
+	.suspend = mwifiex_cfg80211_suspend,
+	.resume = mwifiex_cfg80211_resume,
+	.set_wakeup = mwifiex_cfg80211_set_wakeup,
+#endif
 };
 
 /*
@@ -2380,6 +2485,14 @@ int mwifiex_register_cfg80211(struct mwifiex_adapter *adapter)
 
 	wiphy_apply_custom_regulatory(wiphy, &mwifiex_world_regdom_custom);
 
+#ifdef CONFIG_PM
+	wiphy->wowlan.flags = WIPHY_WOWLAN_MAGIC_PKT;
+	wiphy->wowlan.n_patterns = MWIFIEX_MAX_FILTERS;
+	wiphy->wowlan.pattern_min_len = 1;
+	wiphy->wowlan.pattern_max_len = MWIFIEX_MAX_PATTERN_LEN;
+	wiphy->wowlan.max_pkt_offset = MWIFIEX_MAX_OFFSET_LEN;
+#endif
+
 	wiphy->probe_resp_offload = NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS |
 				    NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS2 |
 				    NL80211_PROBE_RESP_OFFLOAD_SUPPORT_P2P;
diff --git a/drivers/net/wireless/mwifiex/cmdevt.c b/drivers/net/wireless/mwifiex/cmdevt.c
index b5c8b962ce12..74db0d24a579 100644
--- a/drivers/net/wireless/mwifiex/cmdevt.c
+++ b/drivers/net/wireless/mwifiex/cmdevt.c
@@ -153,7 +153,7 @@ static int mwifiex_dnld_cmd_to_fw(struct mwifiex_private *priv,
 			" or cmd size is 0, not sending\n");
 		if (cmd_node->wait_q_enabled)
 			adapter->cmd_wait_q.status = -1;
-		mwifiex_insert_cmd_to_free_q(adapter, cmd_node);
+		mwifiex_recycle_cmd_node(adapter, cmd_node);
 		return -1;
 	}
 
@@ -167,7 +167,7 @@ static int mwifiex_dnld_cmd_to_fw(struct mwifiex_private *priv,
 			"DNLD_CMD: FW in reset state, ignore cmd %#x\n",
 			cmd_code);
 		mwifiex_complete_cmd(adapter, cmd_node);
-		mwifiex_insert_cmd_to_free_q(adapter, cmd_node);
+		mwifiex_recycle_cmd_node(adapter, cmd_node);
 		return -1;
 	}
 
@@ -228,7 +228,7 @@ static int mwifiex_dnld_cmd_to_fw(struct mwifiex_private *priv,
 			adapter->cmd_sent = false;
 		if (cmd_node->wait_q_enabled)
 			adapter->cmd_wait_q.status = -1;
-		mwifiex_insert_cmd_to_free_q(adapter, adapter->curr_cmd);
+		mwifiex_recycle_cmd_node(adapter, adapter->curr_cmd);
 
 		spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
 		adapter->curr_cmd = NULL;
@@ -250,7 +250,7 @@ static int mwifiex_dnld_cmd_to_fw(struct mwifiex_private *priv,
 
 	/* Setup the timer after transmit command */
 	mod_timer(&adapter->cmd_timer,
-		  jiffies + (MWIFIEX_TIMER_10S * HZ) / 1000);
+		  jiffies + msecs_to_jiffies(MWIFIEX_TIMER_10S));
 
 	return 0;
 }
@@ -632,6 +632,20 @@ mwifiex_insert_cmd_to_free_q(struct mwifiex_adapter *adapter,
 	spin_unlock_irqrestore(&adapter->cmd_free_q_lock, flags);
 }
 
+/* This function reuses a command node. */
+void mwifiex_recycle_cmd_node(struct mwifiex_adapter *adapter,
+			      struct cmd_ctrl_node *cmd_node)
+{
+	struct host_cmd_ds_command *host_cmd = (void *)cmd_node->cmd_skb->data;
+
+	mwifiex_insert_cmd_to_free_q(adapter, cmd_node);
+
+	atomic_dec(&adapter->cmd_pending);
+	dev_dbg(adapter->dev, "cmd: FREE_CMD: cmd=%#x, cmd_pending=%d\n",
+		le16_to_cpu(host_cmd->command),
+		atomic_read(&adapter->cmd_pending));
+}
+
 /*
  * This function queues a command to the command pending queue.
  *
@@ -673,7 +687,9 @@ mwifiex_insert_cmd_to_pending_q(struct mwifiex_adapter *adapter,
 		list_add(&cmd_node->list, &adapter->cmd_pending_q);
 	spin_unlock_irqrestore(&adapter->cmd_pending_q_lock, flags);
 
-	dev_dbg(adapter->dev, "cmd: QUEUE_CMD: cmd=%#x is queued\n", command);
+	atomic_inc(&adapter->cmd_pending);
+	dev_dbg(adapter->dev, "cmd: QUEUE_CMD: cmd=%#x, cmd_pending=%d\n",
+		command, atomic_read(&adapter->cmd_pending));
 }
 
 /*
@@ -783,7 +799,7 @@ int mwifiex_process_cmdresp(struct mwifiex_adapter *adapter)
 	if (adapter->curr_cmd->cmd_flag & CMD_F_CANCELED) {
 		dev_err(adapter->dev, "CMD_RESP: %#x been canceled\n",
 			le16_to_cpu(resp->command));
-		mwifiex_insert_cmd_to_free_q(adapter, adapter->curr_cmd);
+		mwifiex_recycle_cmd_node(adapter, adapter->curr_cmd);
 		spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
 		adapter->curr_cmd = NULL;
 		spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
@@ -833,7 +849,7 @@ int mwifiex_process_cmdresp(struct mwifiex_adapter *adapter)
 		if (adapter->curr_cmd->wait_q_enabled)
 			adapter->cmd_wait_q.status = -1;
 
-		mwifiex_insert_cmd_to_free_q(adapter, adapter->curr_cmd);
+		mwifiex_recycle_cmd_node(adapter, adapter->curr_cmd);
 		spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
 		adapter->curr_cmd = NULL;
 		spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
@@ -865,8 +881,7 @@ int mwifiex_process_cmdresp(struct mwifiex_adapter *adapter)
 		if (adapter->curr_cmd->wait_q_enabled)
 			adapter->cmd_wait_q.status = ret;
 
-		/* Clean up and put current command back to cmd_free_q */
-		mwifiex_insert_cmd_to_free_q(adapter, adapter->curr_cmd);
+		mwifiex_recycle_cmd_node(adapter, adapter->curr_cmd);
 
 		spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
 		adapter->curr_cmd = NULL;
@@ -993,7 +1008,7 @@ mwifiex_cancel_all_pending_cmd(struct mwifiex_adapter *adapter)
 			mwifiex_complete_cmd(adapter, cmd_node);
 			cmd_node->wait_q_enabled = false;
 		}
-		mwifiex_insert_cmd_to_free_q(adapter, cmd_node);
+		mwifiex_recycle_cmd_node(adapter, cmd_node);
 		spin_lock_irqsave(&adapter->cmd_pending_q_lock, flags);
 	}
 	spin_unlock_irqrestore(&adapter->cmd_pending_q_lock, flags);
@@ -1040,7 +1055,7 @@ mwifiex_cancel_pending_ioctl(struct mwifiex_adapter *adapter)
 		cmd_node = adapter->curr_cmd;
 		cmd_node->wait_q_enabled = false;
 		cmd_node->cmd_flag |= CMD_F_CANCELED;
-		mwifiex_insert_cmd_to_free_q(adapter, cmd_node);
+		mwifiex_recycle_cmd_node(adapter, cmd_node);
 		mwifiex_complete_cmd(adapter, adapter->curr_cmd);
 		adapter->curr_cmd = NULL;
 		spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, cmd_flags);
@@ -1149,7 +1164,7 @@ int mwifiex_ret_802_11_hs_cfg(struct mwifiex_private *priv,
 			phs_cfg->params.hs_config.gpio,
 			phs_cfg->params.hs_config.gap);
 	}
-	if (conditions != HOST_SLEEP_CFG_CANCEL) {
+	if (conditions != HS_CFG_CANCEL) {
 		adapter->is_hs_configured = true;
 		if (adapter->iface_type == MWIFIEX_USB ||
 		    adapter->iface_type == MWIFIEX_PCIE)
diff --git a/drivers/net/wireless/mwifiex/decl.h b/drivers/net/wireless/mwifiex/decl.h
index e8a569aaa2e8..94cc09d48444 100644
--- a/drivers/net/wireless/mwifiex/decl.h
+++ b/drivers/net/wireless/mwifiex/decl.h
@@ -41,8 +41,15 @@
 #define MWIFIEX_MAX_TX_BASTREAM_SUPPORTED	2
 #define MWIFIEX_MAX_RX_BASTREAM_SUPPORTED	16
 
-#define MWIFIEX_AMPDU_DEF_TXWINSIZE        32
-#define MWIFIEX_AMPDU_DEF_RXWINSIZE        16
+#define MWIFIEX_STA_AMPDU_DEF_TXWINSIZE        16
+#define MWIFIEX_STA_AMPDU_DEF_RXWINSIZE        32
+#define MWIFIEX_UAP_AMPDU_DEF_TXWINSIZE        32
+#define MWIFIEX_UAP_AMPDU_DEF_RXWINSIZE        16
+#define MWIFIEX_11AC_STA_AMPDU_DEF_TXWINSIZE   32
+#define MWIFIEX_11AC_STA_AMPDU_DEF_RXWINSIZE   48
+#define MWIFIEX_11AC_UAP_AMPDU_DEF_TXWINSIZE   48
+#define MWIFIEX_11AC_UAP_AMPDU_DEF_RXWINSIZE   32
+
 #define MWIFIEX_DEFAULT_BLOCK_ACK_TIMEOUT  0xffff
 
 #define MWIFIEX_RATE_BITMAP_MCS0   32
diff --git a/drivers/net/wireless/mwifiex/ethtool.c b/drivers/net/wireless/mwifiex/ethtool.c
new file mode 100644
index 000000000000..bfb39908b2c6
--- /dev/null
+++ b/drivers/net/wireless/mwifiex/ethtool.c
@@ -0,0 +1,70 @@
+/*
+ * Marvell Wireless LAN device driver: ethtool
+ *
+ * Copyright (C) 2013, Marvell International Ltd.
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available by writing to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ */
+
+#include "main.h"
+
+static void mwifiex_ethtool_get_wol(struct net_device *dev,
+				    struct ethtool_wolinfo *wol)
+{
+	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+	u32 conditions = le32_to_cpu(priv->adapter->hs_cfg.conditions);
+
+	wol->supported = WAKE_UCAST|WAKE_MCAST|WAKE_BCAST|WAKE_PHY;
+
+	if (conditions == HS_CFG_COND_DEF)
+		return;
+
+	if (conditions & HS_CFG_COND_UNICAST_DATA)
+		wol->wolopts |= WAKE_UCAST;
+	if (conditions & HS_CFG_COND_MULTICAST_DATA)
+		wol->wolopts |= WAKE_MCAST;
+	if (conditions & HS_CFG_COND_BROADCAST_DATA)
+		wol->wolopts |= WAKE_BCAST;
+	if (conditions & HS_CFG_COND_MAC_EVENT)
+		wol->wolopts |= WAKE_PHY;
+}
+
+static int mwifiex_ethtool_set_wol(struct net_device *dev,
+				   struct ethtool_wolinfo *wol)
+{
+	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+	u32 conditions = 0;
+
+	if (wol->wolopts & ~(WAKE_UCAST|WAKE_MCAST|WAKE_BCAST|WAKE_PHY))
+		return -EOPNOTSUPP;
+
+	if (wol->wolopts & WAKE_UCAST)
+		conditions |= HS_CFG_COND_UNICAST_DATA;
+	if (wol->wolopts & WAKE_MCAST)
+		conditions |= HS_CFG_COND_MULTICAST_DATA;
+	if (wol->wolopts & WAKE_BCAST)
+		conditions |= HS_CFG_COND_BROADCAST_DATA;
+	if (wol->wolopts & WAKE_PHY)
+		conditions |= HS_CFG_COND_MAC_EVENT;
+	if (wol->wolopts == 0)
+		conditions |= HS_CFG_COND_DEF;
+	priv->adapter->hs_cfg.conditions = cpu_to_le32(conditions);
+
+	return 0;
+}
+
+const struct ethtool_ops mwifiex_ethtool_ops = {
+	.get_wol = mwifiex_ethtool_get_wol,
+	.set_wol = mwifiex_ethtool_set_wol,
+};
diff --git a/drivers/net/wireless/mwifiex/fw.h b/drivers/net/wireless/mwifiex/fw.h
index 25acb0682c56..1f7578d553ec 100644
--- a/drivers/net/wireless/mwifiex/fw.h
+++ b/drivers/net/wireless/mwifiex/fw.h
@@ -230,40 +230,12 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 
 #define ISSUPP_11ACENABLED(fw_cap_info) (fw_cap_info & (BIT(13)|BIT(14)))
 
-#define GET_VHTCAP_MAXMPDULEN(vht_cap_info) (vht_cap_info & 0x3)
 #define GET_VHTCAP_CHWDSET(vht_cap_info)    ((vht_cap_info >> 2) & 0x3)
 #define GET_VHTNSSMCS(mcs_mapset, nss) ((mcs_mapset >> (2 * (nss - 1))) & 0x3)
 #define SET_VHTNSSMCS(mcs_mapset, nss, value) (mcs_mapset |= (value & 0x3) << \
 					      (2 * (nss - 1)))
 #define NO_NSS_SUPPORT		0x3
 
-/* HW_SPEC: HTC-VHT supported */
-#define ISSUPP_11ACVHTHTCVHT(Dot11acDevCap) (Dot11acDevCap & BIT(22))
-/* HW_SPEC: VHT TXOP PS support */
-#define ISSUPP_11ACVHTTXOPPS(Dot11acDevCap) (Dot11acDevCap & BIT(21))
-/* HW_SPEC: MU RX beamformee support */
-#define ISSUPP_11ACMURXBEAMFORMEE(Dot11acDevCap) (Dot11acDevCap & BIT(20))
-/* HW_SPEC: MU TX beamformee support */
-#define ISSUPP_11ACMUTXBEAMFORMEE(Dot11acDevCap) (Dot11acDevCap & BIT(19))
-/* HW_SPEC: SU Beamformee support */
-#define ISSUPP_11ACSUBEAMFORMEE(Dot11acDevCap) (Dot11acDevCap & BIT(10))
-/* HW_SPEC: SU Beamformer support */
-#define ISSUPP_11ACSUBEAMFORMER(Dot11acDevCap) (Dot11acDevCap & BIT(9))
-/* HW_SPEC: Rx STBC support */
-#define ISSUPP_11ACRXSTBC(Dot11acDevCap) (Dot11acDevCap & BIT(8))
-/* HW_SPEC: Tx STBC support */
-#define ISSUPP_11ACTXSTBC(Dot11acDevCap) (Dot11acDevCap & BIT(7))
-/* HW_SPEC: Short GI support for 160MHz BW */
-#define ISSUPP_11ACSGI160(Dot11acDevCap) (Dot11acDevCap & BIT(6))
-/* HW_SPEC: Short GI support for 80MHz BW */
-#define ISSUPP_11ACSGI80(Dot11acDevCap) (Dot11acDevCap & BIT(5))
-/* HW_SPEC: LDPC coding support */
-#define ISSUPP_11ACLDPC(Dot11acDevCap) (Dot11acDevCap & BIT(4))
-/* HW_SPEC: Channel BW 20/40/80/160/80+80 MHz support */
-#define ISSUPP_11ACBW8080(Dot11acDevCap) (Dot11acDevCap & BIT(3))
-/* HW_SPEC: Channel BW 20/40/80/160 MHz support */
-#define ISSUPP_11ACBW160(Dot11acDevCap) (Dot11acDevCap & BIT(2))
-
 #define GET_DEVTXMCSMAP(dev_mcs_map)      (dev_mcs_map >> 16)
 #define GET_DEVRXMCSMAP(dev_mcs_map)      (dev_mcs_map & 0xFFFF)
 
@@ -300,6 +272,7 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define HostCmd_CMD_802_11_TX_RATE_QUERY              0x007f
 #define HostCmd_CMD_802_11_IBSS_COALESCING_STATUS     0x0083
 #define HostCmd_CMD_VERSION_EXT                       0x0097
+#define HostCmd_CMD_MEF_CFG                           0x009a
 #define HostCmd_CMD_RSSI_INFO                         0x00a4
 #define HostCmd_CMD_FUNC_INIT                         0x00a9
 #define HostCmd_CMD_FUNC_SHUTDOWN                     0x00aa
@@ -322,6 +295,7 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define HostCmd_CMD_PCIE_DESC_DETAILS                 0x00fa
 #define HostCmd_CMD_MGMT_FRAME_REG                    0x010c
 #define HostCmd_CMD_REMAIN_ON_CHAN                    0x010d
+#define HostCmd_CMD_11AC_CFG			      0x0112
 
 #define PROTOCOL_NO_SECURITY        0x01
 #define PROTOCOL_STATIC_WEP         0x02
@@ -376,10 +350,14 @@ enum P2P_MODES {
 #define HostCmd_SCAN_RADIO_TYPE_BG          0
 #define HostCmd_SCAN_RADIO_TYPE_A           1
 
-#define HOST_SLEEP_CFG_CANCEL		0xffffffff
-#define HOST_SLEEP_CFG_COND_DEF		0x00000000
-#define HOST_SLEEP_CFG_GPIO_DEF		0xff
-#define HOST_SLEEP_CFG_GAP_DEF		0
+#define HS_CFG_CANCEL			0xffffffff
+#define HS_CFG_COND_DEF			0x00000000
+#define HS_CFG_GPIO_DEF			0xff
+#define HS_CFG_GAP_DEF			0
+#define HS_CFG_COND_BROADCAST_DATA	0x00000001
+#define HS_CFG_COND_UNICAST_DATA	0x00000002
+#define HS_CFG_COND_MAC_EVENT		0x00000004
+#define HS_CFG_COND_MULTICAST_DATA	0x00000008
 
 #define MWIFIEX_TIMEOUT_FOR_AP_RESP		0xfffc
 #define MWIFIEX_STATUS_CODE_AUTH_TIMEOUT	2
@@ -469,6 +447,23 @@ enum P2P_MODES {
 #define EVENT_GET_BSS_TYPE(event_cause)         \
 	(((event_cause) >> 24) & 0x00ff)
 
+#define MWIFIEX_MAX_PATTERN_LEN		20
+#define MWIFIEX_MAX_OFFSET_LEN		50
+#define STACK_NBYTES			100
+#define TYPE_DNUM			1
+#define TYPE_BYTESEQ			2
+#define MAX_OPERAND			0x40
+#define TYPE_EQ				(MAX_OPERAND+1)
+#define TYPE_EQ_DNUM			(MAX_OPERAND+2)
+#define TYPE_EQ_BIT			(MAX_OPERAND+3)
+#define TYPE_AND			(MAX_OPERAND+4)
+#define TYPE_OR				(MAX_OPERAND+5)
+#define MEF_MODE_HOST_SLEEP			1
+#define MEF_ACTION_ALLOW_AND_WAKEUP_HOST	3
+#define MWIFIEX_CRITERIA_BROADCAST	BIT(0)
+#define MWIFIEX_CRITERIA_UNICAST	BIT(1)
+#define MWIFIEX_CRITERIA_MULTICAST	BIT(3)
+
 struct mwifiex_ie_types_header {
 	__le16 type;
 	__le16 len;
@@ -1369,6 +1364,15 @@ struct host_cmd_ds_sys_config {
 	u8 tlv[0];
 };
 
+struct host_cmd_11ac_vht_cfg {
+	__le16 action;
+	u8 band_config;
+	u8 misc_config;
+	__le32 cap_info;
+	__le32 mcs_tx_set;
+	__le32 mcs_rx_set;
+} __packed;
+
 struct host_cmd_tlv_akmp {
 	struct host_cmd_tlv tlv;
 	__le16 key_mgmt;
@@ -1499,6 +1503,19 @@ struct host_cmd_ds_802_11_ibss_status {
 	__le16 use_g_rate_protect;
 } __packed;
 
+struct mwifiex_fw_mef_entry {
+	u8 mode;
+	u8 action;
+	__le16 exprsize;
+	u8 expr[0];
+} __packed;
+
+struct host_cmd_ds_mef_cfg {
+	__le32 criteria;
+	__le16 num_entries;
+	struct mwifiex_fw_mef_entry mef_entry[0];
+} __packed;
+
 #define CONNECTION_TYPE_INFRA   0
 #define CONNECTION_TYPE_ADHOC   1
 #define CONNECTION_TYPE_AP      2
@@ -1603,6 +1620,7 @@ struct host_cmd_ds_command {
 		struct host_cmd_ds_remain_on_chan roc_cfg;
 		struct host_cmd_ds_p2p_mode_cfg mode_cfg;
 		struct host_cmd_ds_802_11_ibss_status ibss_coalescing;
+		struct host_cmd_ds_mef_cfg mef_cfg;
 		struct host_cmd_ds_mac_reg_access mac_reg;
 		struct host_cmd_ds_bbp_reg_access bbp_reg;
 		struct host_cmd_ds_rf_reg_access rf_reg;
@@ -1612,6 +1630,7 @@ struct host_cmd_ds_command {
 		struct host_cmd_ds_802_11_eeprom_access eeprom;
 		struct host_cmd_ds_802_11_subsc_evt subsc_evt;
 		struct host_cmd_ds_sys_config uap_sys_config;
+		struct host_cmd_11ac_vht_cfg vht_cfg;
 	} params;
 } __packed;
 
diff --git a/drivers/net/wireless/mwifiex/init.c b/drivers/net/wireless/mwifiex/init.c
index 0ff4c37ab42a..9f44fda19db9 100644
--- a/drivers/net/wireless/mwifiex/init.c
+++ b/drivers/net/wireless/mwifiex/init.c
@@ -44,8 +44,6 @@ static int mwifiex_add_bss_prio_tbl(struct mwifiex_private *priv)
 
 	bss_prio->priv = priv;
 	INIT_LIST_HEAD(&bss_prio->list);
-	if (!tbl[priv->bss_priority].bss_prio_cur)
-		tbl[priv->bss_priority].bss_prio_cur = bss_prio;
 
 	spin_lock_irqsave(&tbl[priv->bss_priority].bss_prio_lock, flags);
 	list_add_tail(&bss_prio->list, &tbl[priv->bss_priority].bss_prio_head);
@@ -318,9 +316,9 @@ static void mwifiex_init_adapter(struct mwifiex_adapter *adapter)
 	adapter->curr_tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_2K;
 
 	adapter->is_hs_configured = false;
-	adapter->hs_cfg.conditions = cpu_to_le32(HOST_SLEEP_CFG_COND_DEF);
-	adapter->hs_cfg.gpio = HOST_SLEEP_CFG_GPIO_DEF;
-	adapter->hs_cfg.gap = HOST_SLEEP_CFG_GAP_DEF;
+	adapter->hs_cfg.conditions = cpu_to_le32(HS_CFG_COND_DEF);
+	adapter->hs_cfg.gpio = HS_CFG_GPIO_DEF;
+	adapter->hs_cfg.gap = HS_CFG_GAP_DEF;
 	adapter->hs_activated = false;
 
 	memset(adapter->event_body, 0, sizeof(adapter->event_body));
@@ -525,7 +523,6 @@ int mwifiex_init_lock_list(struct mwifiex_adapter *adapter)
 
 	for (i = 0; i < adapter->priv_num; ++i) {
 		INIT_LIST_HEAD(&adapter->bss_prio_tbl[i].bss_prio_head);
-		adapter->bss_prio_tbl[i].bss_prio_cur = NULL;
 		spin_lock_init(&adapter->bss_prio_tbl[i].bss_prio_lock);
 	}
 
@@ -533,10 +530,8 @@ int mwifiex_init_lock_list(struct mwifiex_adapter *adapter)
 		if (!adapter->priv[i])
 			continue;
 		priv = adapter->priv[i];
-		for (j = 0; j < MAX_NUM_TID; ++j) {
+		for (j = 0; j < MAX_NUM_TID; ++j)
 			INIT_LIST_HEAD(&priv->wmm.tid_tbl_ptr[j].ra_list);
-			spin_lock_init(&priv->wmm.tid_tbl_ptr[j].tid_tbl_lock);
-		}
 		INIT_LIST_HEAD(&priv->tx_ba_stream_tbl_ptr);
 		INIT_LIST_HEAD(&priv->rx_reorder_tbl_ptr);
 		INIT_LIST_HEAD(&priv->sta_list);
@@ -627,42 +622,36 @@ static void mwifiex_delete_bss_prio_tbl(struct mwifiex_private *priv)
 {
 	int i;
 	struct mwifiex_adapter *adapter = priv->adapter;
-	struct mwifiex_bss_prio_node *bssprio_node, *tmp_node, **cur;
+	struct mwifiex_bss_prio_node *bssprio_node, *tmp_node;
 	struct list_head *head;
 	spinlock_t *lock; /* bss priority lock */
 	unsigned long flags;
 
 	for (i = 0; i < adapter->priv_num; ++i) {
 		head = &adapter->bss_prio_tbl[i].bss_prio_head;
-		cur = &adapter->bss_prio_tbl[i].bss_prio_cur;
 		lock = &adapter->bss_prio_tbl[i].bss_prio_lock;
 		dev_dbg(adapter->dev, "info: delete BSS priority table,"
 				" bss_type = %d, bss_num = %d, i = %d,"
-				" head = %p, cur = %p\n",
-			      priv->bss_type, priv->bss_num, i, head, *cur);
-		if (*cur) {
+				" head = %p\n",
+			      priv->bss_type, priv->bss_num, i, head);
+
+		{
 			spin_lock_irqsave(lock, flags);
 			if (list_empty(head)) {
 				spin_unlock_irqrestore(lock, flags);
 				continue;
 			}
-			bssprio_node = list_first_entry(head,
-					struct mwifiex_bss_prio_node, list);
-			spin_unlock_irqrestore(lock, flags);
-
 			list_for_each_entry_safe(bssprio_node, tmp_node, head,
 						 list) {
 				if (bssprio_node->priv == priv) {
 					dev_dbg(adapter->dev, "info: Delete "
 						"node %p, next = %p\n",
 						bssprio_node, tmp_node);
-					spin_lock_irqsave(lock, flags);
 					list_del(&bssprio_node->list);
-					spin_unlock_irqrestore(lock, flags);
 					kfree(bssprio_node);
 				}
 			}
-			*cur = (struct mwifiex_bss_prio_node *)head;
+			spin_unlock_irqrestore(lock, flags);
 		}
 	}
 }
@@ -713,7 +702,7 @@ mwifiex_shutdown_drv(struct mwifiex_adapter *adapter)
 	if (adapter->curr_cmd) {
 		dev_warn(adapter->dev, "curr_cmd is still in processing\n");
 		del_timer(&adapter->cmd_timer);
-		mwifiex_insert_cmd_to_free_q(adapter, adapter->curr_cmd);
+		mwifiex_recycle_cmd_node(adapter, adapter->curr_cmd);
 		adapter->curr_cmd = NULL;
 	}
 
diff --git a/drivers/net/wireless/mwifiex/ioctl.h b/drivers/net/wireless/mwifiex/ioctl.h
index d85e6eb1f58a..7f27e45680b5 100644
--- a/drivers/net/wireless/mwifiex/ioctl.h
+++ b/drivers/net/wireless/mwifiex/ioctl.h
@@ -272,6 +272,14 @@ struct mwifiex_ds_pm_cfg {
 	} param;
 };
 
+struct mwifiex_11ac_vht_cfg {
+	u8 band_config;
+	u8 misc_config;
+	u32 cap_info;
+	u32 mcs_tx_set;
+	u32 mcs_rx_set;
+};
+
 struct mwifiex_ds_11n_tx_cfg {
 	u16 tx_htcap;
 	u16 tx_htinfo;
@@ -354,6 +362,29 @@ struct mwifiex_ds_misc_subsc_evt {
 	struct subsc_evt_cfg bcn_h_rssi_cfg;
 };
 
+#define MAX_BYTESEQ		6	/* non-adjustable */
+#define MWIFIEX_MAX_FILTERS	10
+
+struct mwifiex_mef_filter {
+	u16 repeat;
+	u16 offset;
+	s8 byte_seq[MAX_BYTESEQ + 1];
+	u8 filt_type;
+	u8 filt_action;
+};
+
+struct mwifiex_mef_entry {
+	u8 mode;
+	u8 action;
+	struct mwifiex_mef_filter filter[MWIFIEX_MAX_FILTERS];
+};
+
+struct mwifiex_ds_mef_cfg {
+	u32 criteria;
+	u16 num_entries;
+	struct mwifiex_mef_entry *mef_entry;
+};
+
 #define MWIFIEX_MAX_VSIE_LEN       (256)
 #define MWIFIEX_MAX_VSIE_NUM       (8)
 #define MWIFIEX_VSIE_MASK_CLEAR    0x00
diff --git a/drivers/net/wireless/mwifiex/join.c b/drivers/net/wireless/mwifiex/join.c
index 2fe0ceba4400..6bcb66e6e97c 100644
--- a/drivers/net/wireless/mwifiex/join.c
+++ b/drivers/net/wireless/mwifiex/join.c
@@ -1295,6 +1295,14 @@ int mwifiex_associate(struct mwifiex_private *priv,
 	    (bss_desc->bss_mode != NL80211_IFTYPE_STATION))
 		return -1;
 
+	if (ISSUPP_11ACENABLED(priv->adapter->fw_cap_info) &&
+	    !bss_desc->disable_11n && !bss_desc->disable_11ac &&
+	    (priv->adapter->config_bands & BAND_GAC ||
+	     priv->adapter->config_bands & BAND_AAC))
+		mwifiex_set_11ac_ba_params(priv);
+	else
+		mwifiex_set_ba_params(priv);
+
 	memcpy(&current_bssid,
 	       &priv->curr_bss_params.bss_descriptor.mac_address,
 	       sizeof(current_bssid));
@@ -1323,6 +1331,13 @@ mwifiex_adhoc_start(struct mwifiex_private *priv,
 	dev_dbg(priv->adapter->dev, "info: curr_bss_params.band = %d\n",
 		priv->curr_bss_params.band);
 
+	if (ISSUPP_11ACENABLED(priv->adapter->fw_cap_info) &&
+	    (priv->adapter->config_bands & BAND_GAC ||
+	     priv->adapter->config_bands & BAND_AAC))
+		mwifiex_set_11ac_ba_params(priv);
+	else
+		mwifiex_set_ba_params(priv);
+
 	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_AD_HOC_START,
 				    HostCmd_ACT_GEN_SET, 0, adhoc_ssid);
 }
@@ -1356,6 +1371,14 @@ int mwifiex_adhoc_join(struct mwifiex_private *priv,
 		return -1;
 	}
 
+	if (ISSUPP_11ACENABLED(priv->adapter->fw_cap_info) &&
+	    !bss_desc->disable_11n && !bss_desc->disable_11ac &&
+	    (priv->adapter->config_bands & BAND_GAC ||
+	     priv->adapter->config_bands & BAND_AAC))
+		mwifiex_set_11ac_ba_params(priv);
+	else
+		mwifiex_set_ba_params(priv);
+
 	dev_dbg(priv->adapter->dev, "info: curr_bss_params.channel = %d\n",
 		priv->curr_bss_params.bss_descriptor.channel);
 	dev_dbg(priv->adapter->dev, "info: curr_bss_params.band = %c\n",
diff --git a/drivers/net/wireless/mwifiex/main.c b/drivers/net/wireless/mwifiex/main.c
index 9c802ede9c3b..121443a0f2a1 100644
--- a/drivers/net/wireless/mwifiex/main.c
+++ b/drivers/net/wireless/mwifiex/main.c
@@ -588,10 +588,19 @@ mwifiex_tx_timeout(struct net_device *dev)
 {
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
 
-	dev_err(priv->adapter->dev, "%lu : Tx timeout, bss_type-num = %d-%d\n",
-		jiffies, priv->bss_type, priv->bss_num);
-	mwifiex_set_trans_start(dev);
 	priv->num_tx_timeout++;
+	priv->tx_timeout_cnt++;
+	dev_err(priv->adapter->dev,
+		"%lu : Tx timeout(#%d), bss_type-num = %d-%d\n",
+		jiffies, priv->tx_timeout_cnt, priv->bss_type, priv->bss_num);
+	mwifiex_set_trans_start(dev);
+
+	if (priv->tx_timeout_cnt > TX_TIMEOUT_THRESHOLD &&
+	    priv->adapter->if_ops.card_reset) {
+		dev_err(priv->adapter->dev,
+			"tx_timeout_cnt exceeds threshold. Triggering card reset!\n");
+		priv->adapter->if_ops.card_reset(priv->adapter);
+	}
 }
 
 /*
diff --git a/drivers/net/wireless/mwifiex/main.h b/drivers/net/wireless/mwifiex/main.h
index 7035ade9af74..4ef67fca06d3 100644
--- a/drivers/net/wireless/mwifiex/main.h
+++ b/drivers/net/wireless/mwifiex/main.h
@@ -130,6 +130,9 @@ enum {
 #define MWIFIEX_USB_TYPE_DATA			0xBEADC0DE
 #define MWIFIEX_USB_TYPE_EVENT			0xBEEFFACE
 
+/* Threshold for tx_timeout_cnt before we trigger a card reset */
+#define TX_TIMEOUT_THRESHOLD	6
+
 struct mwifiex_dbg {
 	u32 num_cmd_host_to_card_failure;
 	u32 num_cmd_sleep_cfm_host_to_card_failure;
@@ -210,15 +213,11 @@ struct mwifiex_ra_list_tbl {
 
 struct mwifiex_tid_tbl {
 	struct list_head ra_list;
-	/* spin lock for tid table */
-	spinlock_t tid_tbl_lock;
-	struct mwifiex_ra_list_tbl *ra_list_curr;
 };
 
 #define WMM_HIGHEST_PRIORITY		7
 #define HIGH_PRIO_TID				7
 #define LOW_PRIO_TID				0
-#define NO_PKT_PRIO_TID				(-1)
 
 struct mwifiex_wmm_desc {
 	struct mwifiex_tid_tbl tid_tbl_ptr[MAX_NUM_TID];
@@ -394,6 +393,8 @@ struct mwifiex_private {
 	u8 curr_addr[ETH_ALEN];
 	u8 media_connected;
 	u32 num_tx_timeout;
+	/* track consecutive timeout */
+	u8 tx_timeout_cnt;
 	struct net_device *netdev;
 	struct net_device_stats stats;
 	u16 curr_pkt_filter;
@@ -793,6 +794,8 @@ void mwifiex_cancel_pending_ioctl(struct mwifiex_adapter *adapter);
 
 void mwifiex_insert_cmd_to_free_q(struct mwifiex_adapter *adapter,
 				  struct cmd_ctrl_node *cmd_node);
+void mwifiex_recycle_cmd_node(struct mwifiex_adapter *adapter,
+			      struct cmd_ctrl_node *cmd_node);
 
 void mwifiex_insert_cmd_to_pending_q(struct mwifiex_adapter *adapter,
 				     struct cmd_ctrl_node *cmd_node,
@@ -907,12 +910,20 @@ int mwifiex_set_secure_params(struct mwifiex_private *priv,
 void mwifiex_set_ht_params(struct mwifiex_private *priv,
 			   struct mwifiex_uap_bss_param *bss_cfg,
 			   struct cfg80211_ap_settings *params);
+void mwifiex_set_vht_params(struct mwifiex_private *priv,
+			    struct mwifiex_uap_bss_param *bss_cfg,
+			    struct cfg80211_ap_settings *params);
 void mwifiex_set_uap_rates(struct mwifiex_uap_bss_param *bss_cfg,
 			   struct cfg80211_ap_settings *params);
+void mwifiex_set_vht_width(struct mwifiex_private *priv,
+			   enum nl80211_chan_width width,
+			   bool ap_11ac_disable);
 void
 mwifiex_set_wmm_params(struct mwifiex_private *priv,
 		       struct mwifiex_uap_bss_param *bss_cfg,
 		       struct cfg80211_ap_settings *params);
+void mwifiex_set_ba_params(struct mwifiex_private *priv);
+void mwifiex_set_11ac_ba_params(struct mwifiex_private *priv);
 
 /*
  * This function checks if the queuing is RA based or not.
@@ -1098,11 +1109,15 @@ int mwifiex_del_virtual_intf(struct wiphy *wiphy, struct wireless_dev *wdev);
 
 void mwifiex_set_sys_config_invalid_data(struct mwifiex_uap_bss_param *config);
 
+int mwifiex_add_wowlan_magic_pkt_filter(struct mwifiex_adapter *adapter);
+
 int mwifiex_set_mgmt_ies(struct mwifiex_private *priv,
 			 struct cfg80211_beacon_data *data);
 int mwifiex_del_mgmt_ies(struct mwifiex_private *priv);
 u8 *mwifiex_11d_code_2_region(u8 code);
 
+extern const struct ethtool_ops mwifiex_ethtool_ops;
+
 #ifdef CONFIG_DEBUG_FS
 void mwifiex_debugfs_init(void);
 void mwifiex_debugfs_remove(void);
diff --git a/drivers/net/wireless/mwifiex/pcie.c b/drivers/net/wireless/mwifiex/pcie.c
index feb204613397..20c9c4c7b0b2 100644
--- a/drivers/net/wireless/mwifiex/pcie.c
+++ b/drivers/net/wireless/mwifiex/pcie.c
@@ -36,8 +36,6 @@ static u8 user_rmmod;
 static struct mwifiex_if_ops pcie_ops;
 
 static struct semaphore add_remove_card_sem;
-static int mwifiex_pcie_enable_host_int(struct mwifiex_adapter *adapter);
-static int mwifiex_pcie_resume(struct pci_dev *pdev);
 
 static int
 mwifiex_map_pci_memory(struct mwifiex_adapter *adapter, struct sk_buff *skb,
@@ -78,6 +76,82 @@ static bool mwifiex_pcie_ok_to_access_hw(struct mwifiex_adapter *adapter)
 	return false;
 }
 
+#ifdef CONFIG_PM
+/*
+ * Kernel needs to suspend all functions separately. Therefore all
+ * registered functions must have drivers with suspend and resume
+ * methods. Failing that the kernel simply removes the whole card.
+ *
+ * If already not suspended, this function allocates and sends a host
+ * sleep activate request to the firmware and turns off the traffic.
+ */
+static int mwifiex_pcie_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	struct mwifiex_adapter *adapter;
+	struct pcie_service_card *card;
+	int hs_actived;
+
+	if (pdev) {
+		card = (struct pcie_service_card *) pci_get_drvdata(pdev);
+		if (!card || !card->adapter) {
+			pr_err("Card or adapter structure is not valid\n");
+			return 0;
+		}
+	} else {
+		pr_err("PCIE device is not specified\n");
+		return 0;
+	}
+
+	adapter = card->adapter;
+
+	hs_actived = mwifiex_enable_hs(adapter);
+
+	/* Indicate device suspended */
+	adapter->is_suspended = true;
+
+	return 0;
+}
+
+/*
+ * Kernel needs to suspend all functions separately. Therefore all
+ * registered functions must have drivers with suspend and resume
+ * methods. Failing that the kernel simply removes the whole card.
+ *
+ * If already not resumed, this function turns on the traffic and
+ * sends a host sleep cancel request to the firmware.
+ */
+static int mwifiex_pcie_resume(struct pci_dev *pdev)
+{
+	struct mwifiex_adapter *adapter;
+	struct pcie_service_card *card;
+
+	if (pdev) {
+		card = (struct pcie_service_card *) pci_get_drvdata(pdev);
+		if (!card || !card->adapter) {
+			pr_err("Card or adapter structure is not valid\n");
+			return 0;
+		}
+	} else {
+		pr_err("PCIE device is not specified\n");
+		return 0;
+	}
+
+	adapter = card->adapter;
+
+	if (!adapter->is_suspended) {
+		dev_warn(adapter->dev, "Device already resumed\n");
+		return 0;
+	}
+
+	adapter->is_suspended = false;
+
+	mwifiex_cancel_hs(mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_STA),
+			  MWIFIEX_ASYNC_CMD);
+
+	return 0;
+}
+#endif
+
 /*
  * This function probes an mwifiex device and registers it. It allocates
  * the card structure, enables PCIE function number and initiates the
@@ -159,80 +233,6 @@ static void mwifiex_pcie_remove(struct pci_dev *pdev)
 	kfree(card);
 }
 
-/*
- * Kernel needs to suspend all functions separately. Therefore all
- * registered functions must have drivers with suspend and resume
- * methods. Failing that the kernel simply removes the whole card.
- *
- * If already not suspended, this function allocates and sends a host
- * sleep activate request to the firmware and turns off the traffic.
- */
-static int mwifiex_pcie_suspend(struct pci_dev *pdev, pm_message_t state)
-{
-	struct mwifiex_adapter *adapter;
-	struct pcie_service_card *card;
-	int hs_actived;
-
-	if (pdev) {
-		card = (struct pcie_service_card *) pci_get_drvdata(pdev);
-		if (!card || !card->adapter) {
-			pr_err("Card or adapter structure is not valid\n");
-			return 0;
-		}
-	} else {
-		pr_err("PCIE device is not specified\n");
-		return 0;
-	}
-
-	adapter = card->adapter;
-
-	hs_actived = mwifiex_enable_hs(adapter);
-
-	/* Indicate device suspended */
-	adapter->is_suspended = true;
-
-	return 0;
-}
-
-/*
- * Kernel needs to suspend all functions separately. Therefore all
- * registered functions must have drivers with suspend and resume
- * methods. Failing that the kernel simply removes the whole card.
- *
- * If already not resumed, this function turns on the traffic and
- * sends a host sleep cancel request to the firmware.
- */
-static int mwifiex_pcie_resume(struct pci_dev *pdev)
-{
-	struct mwifiex_adapter *adapter;
-	struct pcie_service_card *card;
-
-	if (pdev) {
-		card = (struct pcie_service_card *) pci_get_drvdata(pdev);
-		if (!card || !card->adapter) {
-			pr_err("Card or adapter structure is not valid\n");
-			return 0;
-		}
-	} else {
-		pr_err("PCIE device is not specified\n");
-		return 0;
-	}
-
-	adapter = card->adapter;
-
-	if (!adapter->is_suspended) {
-		dev_warn(adapter->dev, "Device already resumed\n");
-		return 0;
-	}
-
-	adapter->is_suspended = false;
-
-	mwifiex_cancel_hs(mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_STA),
-			  MWIFIEX_ASYNC_CMD);
-
-	return 0;
-}
-
 static DEFINE_PCI_DEVICE_TABLE(mwifiex_ids) = {
 	{
 		PCIE_VENDOR_ID_MARVELL, PCIE_DEVICE_ID_MARVELL_88W8766P,
@@ -287,18 +287,13 @@ static int mwifiex_read_reg(struct mwifiex_adapter *adapter, int reg, u32 *data)
 }
 
 /*
- * This function wakes up the card.
- *
- * A host power up command is written to the card configuration
- * register to wake up the card.
+ * This function adds delay loop to ensure FW is awake before proceeding.
  */
-static int mwifiex_pm_wakeup_card(struct mwifiex_adapter *adapter)
+static void mwifiex_pcie_dev_wakeup_delay(struct mwifiex_adapter *adapter)
 {
 	int i = 0;
-	struct pcie_service_card *card = adapter->card;
-	const struct mwifiex_pcie_card_reg *reg = card->pcie.reg;
 
-	while (reg->sleep_cookie && mwifiex_pcie_ok_to_access_hw(adapter)) {
+	while (mwifiex_pcie_ok_to_access_hw(adapter)) {
 		i++;
 		usleep_range(10, 20);
 		/* 50ms max wait */
@@ -306,16 +301,32 @@ static int mwifiex_pm_wakeup_card(struct mwifiex_adapter *adapter)
 			break;
 	}
 
+	return;
+}
+
+/* This function wakes up the card by reading fw_status register. */
+static int mwifiex_pm_wakeup_card(struct mwifiex_adapter *adapter)
+{
+	u32 fw_status;
+	struct pcie_service_card *card = adapter->card;
+	const struct mwifiex_pcie_card_reg *reg = card->pcie.reg;
+
 	dev_dbg(adapter->dev, "event: Wakeup device...\n");
 
-	/* Enable interrupts or any chip access will wakeup device */
-	if (mwifiex_write_reg(adapter, PCIE_HOST_INT_MASK, HOST_INTR_MASK)) {
-		dev_warn(adapter->dev, "Enable host interrupt failed\n");
+	if (reg->sleep_cookie)
+		mwifiex_pcie_dev_wakeup_delay(adapter);
+
+	/* Reading fw_status register will wakeup device */
+	if (mwifiex_read_reg(adapter, reg->fw_status, &fw_status)) {
+		dev_warn(adapter->dev, "Reading fw_status register failed\n");
 		return -1;
 	}
 
-	dev_dbg(adapter->dev, "PCIE wakeup: Setting PS_STATE_AWAKE\n");
-	adapter->ps_state = PS_STATE_AWAKE;
+	if (reg->sleep_cookie) {
+		mwifiex_pcie_dev_wakeup_delay(adapter);
+		dev_dbg(adapter->dev, "PCIE wakeup: Setting PS_STATE_AWAKE\n");
+		adapter->ps_state = PS_STATE_AWAKE;
+	}
 
 	return 0;
 }
@@ -561,7 +572,7 @@ static void mwifiex_cleanup_rxq_ring(struct mwifiex_adapter *adapter)
 			if (card->rx_buf_list[i]) {
 				skb = card->rx_buf_list[i];
 				pci_unmap_single(card->dev, desc2->paddr,
-						 skb->len, PCI_DMA_TODEVICE);
+						 skb->len, PCI_DMA_FROMDEVICE);
 				dev_kfree_skb_any(skb);
 			}
 			memset(desc2, 0, sizeof(*desc2));
@@ -570,7 +581,7 @@ static void mwifiex_cleanup_rxq_ring(struct mwifiex_adapter *adapter)
 			if (card->rx_buf_list[i]) {
 				skb = card->rx_buf_list[i];
 				pci_unmap_single(card->dev, desc->paddr,
-						 skb->len, PCI_DMA_TODEVICE);
+						 skb->len, PCI_DMA_FROMDEVICE);
 				dev_kfree_skb_any(skb);
 			}
 			memset(desc, 0, sizeof(*desc));
@@ -850,9 +861,8 @@ static int mwifiex_pcie_delete_cmdrsp_buf(struct mwifiex_adapter *adapter)
 
 	if (card && card->cmd_buf) {
 		MWIFIEX_SKB_PACB(card->cmd_buf, &buf_pa);
-		pci_unmap_single(card->dev, buf_pa, MWIFIEX_SIZE_OF_CMD_BUFFER,
+		pci_unmap_single(card->dev, buf_pa, card->cmd_buf->len,
 				 PCI_DMA_TODEVICE);
-		dev_kfree_skb_any(card->cmd_buf);
 	}
 	return 0;
 }
@@ -1030,8 +1040,8 @@ mwifiex_pcie_send_data(struct mwifiex_adapter *adapter, struct sk_buff *skb,
 	u32 wrindx, num_tx_buffs, rx_val;
 	int ret;
 	dma_addr_t buf_pa;
-	struct mwifiex_pcie_buf_desc *desc;
-	struct mwifiex_pfu_buf_desc *desc2;
+	struct mwifiex_pcie_buf_desc *desc = NULL;
+	struct mwifiex_pfu_buf_desc *desc2 = NULL;
 	__le16 *tmp;
 
 	if (!(skb->data && skb->len)) {
@@ -1562,7 +1572,7 @@ static int mwifiex_pcie_cmdrsp_complete(struct mwifiex_adapter *adapter,
 	skb_tmp = card->cmd_buf;
 	if (skb_tmp) {
 		MWIFIEX_SKB_PACB(skb_tmp, &buf_pa);
-		pci_unmap_single(card->dev, buf_pa, MWIFIEX_UPLD_SIZE,
+		pci_unmap_single(card->dev, buf_pa, skb_tmp->len,
 				 PCI_DMA_FROMDEVICE);
 		card->cmd_buf = NULL;
 	}
@@ -1984,12 +1994,13 @@ static void mwifiex_interrupt_status(struct mwifiex_adapter *adapter)
 				}
 			}
 		} else if (!adapter->pps_uapsd_mode &&
-			   adapter->ps_state == PS_STATE_SLEEP) {
+			   adapter->ps_state == PS_STATE_SLEEP &&
+			   mwifiex_pcie_ok_to_access_hw(adapter)) {
 				/* Potentially for PCIe we could get other
 				 * interrupts like shared. Don't change power
 				 * state until cookie is set */
-				if (mwifiex_pcie_ok_to_access_hw(adapter))
-					adapter->ps_state = PS_STATE_AWAKE;
+				adapter->ps_state = PS_STATE_AWAKE;
+				adapter->pm_wakeup_fw_try = false;
 		}
 	}
 }
@@ -2112,7 +2123,8 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 	}
 	dev_dbg(adapter->dev, "info: cmd_sent=%d data_sent=%d\n",
 		adapter->cmd_sent, adapter->data_sent);
-	mwifiex_pcie_enable_host_int(adapter);
+	if (adapter->ps_state != PS_STATE_SLEEP)
+		mwifiex_pcie_enable_host_int(adapter);
 
 	return 0;
 }
@@ -2281,9 +2293,9 @@ static void mwifiex_pcie_cleanup(struct mwifiex_adapter *adapter)
 	if (pdev) {
 		pci_iounmap(pdev, card->pci_mmap);
 		pci_iounmap(pdev, card->pci_mmap1);
-
-		pci_release_regions(pdev);
 		pci_disable_device(pdev);
+		pci_release_region(pdev, 2);
+		pci_release_region(pdev, 0);
 		pci_set_drvdata(pdev, NULL);
 	}
 }
diff --git a/drivers/net/wireless/mwifiex/scan.c b/drivers/net/wireless/mwifiex/scan.c
index e7f6deaf715e..9cf5d8f07df8 100644
--- a/drivers/net/wireless/mwifiex/scan.c
+++ b/drivers/net/wireless/mwifiex/scan.c
@@ -1500,43 +1500,22 @@ static int mwifiex_update_curr_bss_params(struct mwifiex_private *priv,
 	if (ret)
 		goto done;
 
-	/* Update current bss descriptor parameters */
 	spin_lock_irqsave(&priv->curr_bcn_buf_lock, flags);
-	priv->curr_bss_params.bss_descriptor.bcn_wpa_ie = NULL;
-	priv->curr_bss_params.bss_descriptor.wpa_offset = 0;
-	priv->curr_bss_params.bss_descriptor.bcn_rsn_ie = NULL;
-	priv->curr_bss_params.bss_descriptor.rsn_offset = 0;
-	priv->curr_bss_params.bss_descriptor.bcn_wapi_ie = NULL;
-	priv->curr_bss_params.bss_descriptor.wapi_offset = 0;
-	priv->curr_bss_params.bss_descriptor.bcn_ht_cap = NULL;
-	priv->curr_bss_params.bss_descriptor.ht_cap_offset = 0;
-	priv->curr_bss_params.bss_descriptor.bcn_ht_oper = NULL;
-	priv->curr_bss_params.bss_descriptor.ht_info_offset = 0;
-	priv->curr_bss_params.bss_descriptor.bcn_bss_co_2040 = NULL;
-	priv->curr_bss_params.bss_descriptor.bss_co_2040_offset = 0;
-	priv->curr_bss_params.bss_descriptor.bcn_ext_cap = NULL;
-	priv->curr_bss_params.bss_descriptor.ext_cap_offset = 0;
-	priv->curr_bss_params.bss_descriptor.beacon_buf = NULL;
-	priv->curr_bss_params.bss_descriptor.beacon_buf_size = 0;
-	priv->curr_bss_params.bss_descriptor.bcn_vht_cap = NULL;
-	priv->curr_bss_params.bss_descriptor.vht_cap_offset = 0;
-	priv->curr_bss_params.bss_descriptor.bcn_vht_oper = NULL;
-	priv->curr_bss_params.bss_descriptor.vht_info_offset = 0;
-	priv->curr_bss_params.bss_descriptor.oper_mode = NULL;
-	priv->curr_bss_params.bss_descriptor.oper_mode_offset = 0;
-
-	/* Disable 11ac by default. Enable it only where there
-	 * exist VHT_CAP IE in AP beacon
-	 */
-	priv->curr_bss_params.bss_descriptor.disable_11ac = true;
-
 	/* Make a copy of current BSSID descriptor */
 	memcpy(&priv->curr_bss_params.bss_descriptor, bss_desc,
 	       sizeof(priv->curr_bss_params.bss_descriptor));
+
+	/* The contents of beacon_ie will be copied to its own buffer
+	 * in mwifiex_save_curr_bcn()
+	 */
 	mwifiex_save_curr_bcn(priv);
 	spin_unlock_irqrestore(&priv->curr_bcn_buf_lock, flags);
 
 done:
+	/* beacon_ie buffer was allocated in function
+	 * mwifiex_fill_new_bss_desc(). Free it now.
+	 */
+	kfree(bss_desc->beacon_buf);
 	kfree(bss_desc);
 	return 0;
 }
diff --git a/drivers/net/wireless/mwifiex/sta_cmd.c b/drivers/net/wireless/mwifiex/sta_cmd.c
index c55c5bb93134..b193e25977d2 100644
--- a/drivers/net/wireless/mwifiex/sta_cmd.c
+++ b/drivers/net/wireless/mwifiex/sta_cmd.c
@@ -24,6 +24,7 @@
 #include "main.h"
 #include "wmm.h"
 #include "11n.h"
+#include "11ac.h"
 
 /*
  * This function prepares command to set/get RSSI information.
@@ -334,7 +335,7 @@ mwifiex_cmd_802_11_hs_cfg(struct mwifiex_private *priv,
 	cmd->command = cpu_to_le16(HostCmd_CMD_802_11_HS_CFG_ENH);
 
 	if (!hs_activate &&
-	    (hscfg_param->conditions != cpu_to_le32(HOST_SLEEP_CFG_CANCEL)) &&
+	    (hscfg_param->conditions != cpu_to_le32(HS_CFG_CANCEL)) &&
 	    ((adapter->arp_filter_size > 0) &&
 	     (adapter->arp_filter_size <= ARP_FILTER_MAX_BUF_SIZE))) {
 		dev_dbg(adapter->dev,
@@ -1059,6 +1060,80 @@ mwifiex_cmd_802_11_subsc_evt(struct mwifiex_private *priv,
 	return 0;
 }
 
+static int
+mwifiex_cmd_append_rpn_expression(struct mwifiex_private *priv,
+				  struct mwifiex_mef_entry *mef_entry,
+				  u8 **buffer)
+{
+	struct mwifiex_mef_filter *filter = mef_entry->filter;
+	int i, byte_len;
+	u8 *stack_ptr = *buffer;
+
+	for (i = 0; i < MWIFIEX_MAX_FILTERS; i++) {
+		filter = &mef_entry->filter[i];
+		if (!filter->filt_type)
+			break;
+		*(__le32 *)stack_ptr = cpu_to_le32((u32)filter->repeat);
+		stack_ptr += 4;
+		*stack_ptr = TYPE_DNUM;
+		stack_ptr += 1;
+
+		byte_len = filter->byte_seq[MAX_BYTESEQ];
+		memcpy(stack_ptr, filter->byte_seq, byte_len);
+		stack_ptr += byte_len;
+		*stack_ptr = byte_len;
+		stack_ptr += 1;
+		*stack_ptr = TYPE_BYTESEQ;
+		stack_ptr += 1;
+
+		*(__le32 *)stack_ptr = cpu_to_le32((u32)filter->offset);
+		stack_ptr += 4;
+		*stack_ptr = TYPE_DNUM;
+		stack_ptr += 1;
+
+		*stack_ptr = filter->filt_type;
+		stack_ptr += 1;
+
+		if (filter->filt_action) {
+			*stack_ptr = filter->filt_action;
+			stack_ptr += 1;
+		}
+
+		if (stack_ptr - *buffer > STACK_NBYTES)
+			return -1;
+	}
+
+	*buffer = stack_ptr;
+	return 0;
+}
+
+static int
+mwifiex_cmd_mef_cfg(struct mwifiex_private *priv,
+		    struct host_cmd_ds_command *cmd,
+		    struct mwifiex_ds_mef_cfg *mef)
+{
+	struct host_cmd_ds_mef_cfg *mef_cfg = &cmd->params.mef_cfg;
+	u8 *pos = (u8 *)mef_cfg;
+
+	cmd->command = cpu_to_le16(HostCmd_CMD_MEF_CFG);
+
+	mef_cfg->criteria = cpu_to_le32(mef->criteria);
+	mef_cfg->num_entries = cpu_to_le16(mef->num_entries);
+	pos += sizeof(*mef_cfg);
+	mef_cfg->mef_entry->mode = mef->mef_entry->mode;
+	mef_cfg->mef_entry->action = mef->mef_entry->action;
+	pos += sizeof(*(mef_cfg->mef_entry));
+
+	if (mwifiex_cmd_append_rpn_expression(priv, mef->mef_entry, &pos))
+		return -1;
+
+	mef_cfg->mef_entry->exprsize =
+			cpu_to_le16(pos - mef_cfg->mef_entry->expr);
+	cmd->size = cpu_to_le16((u16) (pos - (u8 *)mef_cfg) + S_DS_GEN);
+
+	return 0;
+}
+
 /*
  * This function prepares the commands before sending them to the firmware.
  *
@@ -1184,6 +1259,9 @@ int mwifiex_sta_prepare_cmd(struct mwifiex_private *priv, uint16_t cmd_no,
 		      cpu_to_le16(sizeof(struct host_cmd_ds_remain_on_chan) +
 				  S_DS_GEN);
 		break;
+	case HostCmd_CMD_11AC_CFG:
+		ret = mwifiex_cmd_11ac_cfg(priv, cmd_ptr, cmd_action, data_buf);
+		break;
 	case HostCmd_CMD_P2P_MODE_CFG:
 		cmd_ptr->command = cpu_to_le16(cmd_no);
 		cmd_ptr->params.mode_cfg.action = cpu_to_le16(cmd_action);
@@ -1273,6 +1351,9 @@ int mwifiex_sta_prepare_cmd(struct mwifiex_private *priv, uint16_t cmd_no,
 	case HostCmd_CMD_802_11_SUBSCRIBE_EVENT:
 		ret = mwifiex_cmd_802_11_subsc_evt(priv, cmd_ptr, data_buf);
 		break;
+	case HostCmd_CMD_MEF_CFG:
+		ret = mwifiex_cmd_mef_cfg(priv, cmd_ptr, data_buf);
+		break;
 	default:
 		dev_err(priv->adapter->dev,
 			"PREP_CMD: unknown cmd- %#x\n", cmd_no);
diff --git a/drivers/net/wireless/mwifiex/sta_cmdresp.c b/drivers/net/wireless/mwifiex/sta_cmdresp.c
index 4669f8d9389f..9f990e14966e 100644
--- a/drivers/net/wireless/mwifiex/sta_cmdresp.c
+++ b/drivers/net/wireless/mwifiex/sta_cmdresp.c
@@ -95,7 +95,7 @@ mwifiex_process_cmdresp_error(struct mwifiex_private *priv,
 		break;
 	}
 	/* Handling errors here */
-	mwifiex_insert_cmd_to_free_q(adapter, adapter->curr_cmd);
+	mwifiex_recycle_cmd_node(adapter, adapter->curr_cmd);
 
 	spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
 	adapter->curr_cmd = NULL;
@@ -907,6 +907,8 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 	case HostCmd_CMD_REMAIN_ON_CHAN:
 		ret = mwifiex_ret_remain_on_chan(priv, resp, data_buf);
 		break;
+	case HostCmd_CMD_11AC_CFG:
+		break;
 	case HostCmd_CMD_P2P_MODE_CFG:
 		ret = mwifiex_ret_p2p_mode_cfg(priv, resp, data_buf);
 		break;
@@ -976,6 +978,8 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 	case HostCmd_CMD_UAP_BSS_STOP:
 		priv->bss_started = 0;
 		break;
+	case HostCmd_CMD_MEF_CFG:
+		break;
 	default:
 		dev_err(adapter->dev, "CMD_RESP: unknown cmd response %#x\n",
 			resp->command);
diff --git a/drivers/net/wireless/mwifiex/sta_ioctl.c b/drivers/net/wireless/mwifiex/sta_ioctl.c
index 13100f8de3db..311d0b26b81c 100644
--- a/drivers/net/wireless/mwifiex/sta_ioctl.c
+++ b/drivers/net/wireless/mwifiex/sta_ioctl.c
@@ -59,9 +59,6 @@ int mwifiex_wait_queue_complete(struct mwifiex_adapter *adapter,
 {
 	int status;
 
-	dev_dbg(adapter->dev, "cmd pending\n");
-	atomic_inc(&adapter->cmd_pending);
-
 	/* Wait for completion */
 	status = wait_event_interruptible(adapter->cmd_wait_q.wait,
 					  *(cmd_queued->condition));
@@ -143,12 +140,13 @@ int mwifiex_request_set_multicast_list(struct mwifiex_private *priv,
 /*
  * This function fills bss descriptor structure using provided
  * information.
+ * beacon_ie buffer is allocated in this function. It is caller's
+ * responsibility to free the memory.
  */
 int mwifiex_fill_new_bss_desc(struct mwifiex_private *priv,
 			      struct cfg80211_bss *bss,
 			      struct mwifiex_bssdescriptor *bss_desc)
 {
-	int ret;
 	u8 *beacon_ie;
 	size_t beacon_ie_len;
 	struct mwifiex_bss_priv *bss_priv = (void *)bss->priv;
@@ -168,6 +166,7 @@ int mwifiex_fill_new_bss_desc(struct mwifiex_private *priv,
 
 	memcpy(bss_desc->mac_address, bss->bssid, ETH_ALEN);
 	bss_desc->rssi = bss->signal;
+	/* The caller of this function will free beacon_ie */
 	bss_desc->beacon_buf = beacon_ie;
 	bss_desc->beacon_buf_size = beacon_ie_len;
 	bss_desc->beacon_period = bss->beacon_interval;
@@ -185,10 +184,12 @@ int mwifiex_fill_new_bss_desc(struct mwifiex_private *priv,
 	else
 		bss_desc->bss_mode = NL80211_IFTYPE_STATION;
 
-	ret = mwifiex_update_bss_desc_with_ie(priv->adapter, bss_desc);
+	/* Disable 11ac by default. Enable it only where there
+	 * exist VHT_CAP IE in AP beacon
+	 */
+	bss_desc->disable_11ac = true;
 
-	kfree(beacon_ie);
-	return ret;
+	return mwifiex_update_bss_desc_with_ie(priv->adapter, bss_desc);
 }
 
 static int mwifiex_process_country_ie(struct mwifiex_private *priv,
@@ -352,6 +353,11 @@ int mwifiex_bss_start(struct mwifiex_private *priv, struct cfg80211_bss *bss,
 	}
 
 done:
+	/* beacon_ie buffer was allocated in function
+	 * mwifiex_fill_new_bss_desc(). Free it now.
+	 */
+	if (bss_desc)
+		kfree(bss_desc->beacon_buf);
 	kfree(bss_desc);
 	return ret;
 }
@@ -382,7 +388,7 @@ static int mwifiex_set_hs_params(struct mwifiex_private *priv, u16 action,
 			break;
 		}
 		if (hs_cfg->is_invoke_hostcmd) {
-			if (hs_cfg->conditions == HOST_SLEEP_CFG_CANCEL) {
+			if (hs_cfg->conditions == HS_CFG_CANCEL) {
 				if (!adapter->is_hs_configured)
 					/* Already cancelled */
 					break;
@@ -397,8 +403,8 @@ static int mwifiex_set_hs_params(struct mwifiex_private *priv, u16 action,
 				adapter->hs_cfg.gpio = (u8)hs_cfg->gpio;
 				if (hs_cfg->gap)
 					adapter->hs_cfg.gap = (u8)hs_cfg->gap;
-			} else if (adapter->hs_cfg.conditions
-				   == cpu_to_le32(HOST_SLEEP_CFG_CANCEL)) {
+			} else if (adapter->hs_cfg.conditions ==
+				   cpu_to_le32(HS_CFG_CANCEL)) {
 				/* Return failure if no parameters for HS
 				   enable */
 				status = -1;
@@ -414,7 +420,7 @@ static int mwifiex_set_hs_params(struct mwifiex_private *priv, u16 action,
 						HostCmd_CMD_802_11_HS_CFG_ENH,
 						HostCmd_ACT_GEN_SET, 0,
 						&adapter->hs_cfg);
-			if (hs_cfg->conditions == HOST_SLEEP_CFG_CANCEL)
+			if (hs_cfg->conditions == HS_CFG_CANCEL)
 				/* Restore previous condition */
 				adapter->hs_cfg.conditions =
 						cpu_to_le32(prev_cond);
@@ -448,7 +454,7 @@ int mwifiex_cancel_hs(struct mwifiex_private *priv, int cmd_type)
 {
 	struct mwifiex_ds_hs_cfg hscfg;
 
-	hscfg.conditions = HOST_SLEEP_CFG_CANCEL;
+	hscfg.conditions = HS_CFG_CANCEL;
 	hscfg.is_invoke_hostcmd = true;
 
 	return mwifiex_set_hs_params(priv, HostCmd_ACT_GEN_SET,
diff --git a/drivers/net/wireless/mwifiex/txrx.c b/drivers/net/wireless/mwifiex/txrx.c
index 296faec14365..8f923d0d2ba6 100644
--- a/drivers/net/wireless/mwifiex/txrx.c
+++ b/drivers/net/wireless/mwifiex/txrx.c
@@ -169,6 +169,8 @@ int mwifiex_write_data_complete(struct mwifiex_adapter *adapter,
 	if (!status) {
 		priv->stats.tx_packets++;
 		priv->stats.tx_bytes += skb->len;
+		if (priv->tx_timeout_cnt)
+			priv->tx_timeout_cnt = 0;
 	} else {
 		priv->stats.tx_errors++;
 	}
diff --git a/drivers/net/wireless/mwifiex/uap_cmd.c b/drivers/net/wireless/mwifiex/uap_cmd.c
index 6e76a15a8950..b04b1db29100 100644
--- a/drivers/net/wireless/mwifiex/uap_cmd.c
+++ b/drivers/net/wireless/mwifiex/uap_cmd.c
@@ -18,6 +18,7 @@
  */
 
 #include "main.h"
+#include "11ac.h"
 
 /* This function parses security related parameters from cfg80211_ap_settings
  * and sets into FW understandable bss_config structure.
@@ -177,6 +178,60 @@ mwifiex_set_ht_params(struct mwifiex_private *priv,
 	return;
 }
 
+/* This function updates 11ac related parameters from IE
+ * and sets them into bss_config structure.
+ */
+void mwifiex_set_vht_params(struct mwifiex_private *priv,
+			    struct mwifiex_uap_bss_param *bss_cfg,
+			    struct cfg80211_ap_settings *params)
+{
+	const u8 *vht_ie;
+
+	vht_ie = cfg80211_find_ie(WLAN_EID_VHT_CAPABILITY, params->beacon.tail,
+				  params->beacon.tail_len);
+	if (vht_ie) {
+		memcpy(&bss_cfg->vht_cap, vht_ie + 2,
+		       sizeof(struct ieee80211_vht_cap));
+		priv->ap_11ac_enabled = 1;
+	} else {
+		priv->ap_11ac_enabled = 0;
+	}
+
+	return;
+}
+
+/* Enable VHT only when cfg80211_ap_settings has VHT IE.
+ * Otherwise disable VHT.
+ */
+void mwifiex_set_vht_width(struct mwifiex_private *priv,
+			   enum nl80211_chan_width width,
+			   bool ap_11ac_enable)
+{
+	struct mwifiex_adapter *adapter = priv->adapter;
+	struct mwifiex_11ac_vht_cfg vht_cfg;
+
+	vht_cfg.band_config = VHT_CFG_5GHZ;
+	vht_cfg.cap_info = adapter->hw_dot_11ac_dev_cap;
+
+	if (!ap_11ac_enable) {
+		vht_cfg.mcs_tx_set = DISABLE_VHT_MCS_SET;
+		vht_cfg.mcs_rx_set = DISABLE_VHT_MCS_SET;
+	} else {
+		vht_cfg.mcs_tx_set = DEFAULT_VHT_MCS_SET;
+		vht_cfg.mcs_rx_set = DEFAULT_VHT_MCS_SET;
+	}
+
+	vht_cfg.misc_config  = VHT_CAP_UAP_ONLY;
+
+	if (ap_11ac_enable && width >= NL80211_CHAN_WIDTH_80)
+		vht_cfg.misc_config |= VHT_BW_80_160_80P80;
+
+	mwifiex_send_cmd_sync(priv, HostCmd_CMD_11AC_CFG,
+			      HostCmd_ACT_GEN_SET, 0, &vht_cfg);
+
+	return;
+}
+
 /* This function finds supported rates IE from beacon parameter and sets
  * these rates into bss_config structure.
  */
diff --git a/drivers/net/wireless/mwifiex/util.c b/drivers/net/wireless/mwifiex/util.c
index 21553976b550..e57ac0dd3ab5 100644
--- a/drivers/net/wireless/mwifiex/util.c
+++ b/drivers/net/wireless/mwifiex/util.c
@@ -195,7 +195,7 @@ int mwifiex_recv_packet(struct mwifiex_private *priv, struct sk_buff *skb)
 	skb->protocol = eth_type_trans(skb, priv->netdev);
 	skb->ip_summed = CHECKSUM_NONE;
 
-	/* This is required only in case of 11n and USB as we alloc
+	/* This is required only in case of 11n and USB/PCIE as we alloc
 	 * a buffer of 4K only if its 11N (to be able to receive 4K
 	 * AMSDU packets). In case of SD we allocate buffers based
 	 * on the size of packet and hence this is not needed.
@@ -212,7 +212,8 @@ int mwifiex_recv_packet(struct mwifiex_private *priv, struct sk_buff *skb)
 	 * fragments. Currently we fail the Filesndl-ht.scr script
 	 * for UDP, hence this fix
 	 */
-	if ((priv->adapter->iface_type == MWIFIEX_USB) &&
+	if ((priv->adapter->iface_type == MWIFIEX_USB ||
+	     priv->adapter->iface_type == MWIFIEX_PCIE) &&
 	    (skb->truesize > MWIFIEX_RX_DATA_BUF_SIZE))
 		skb->truesize += (skb->len - MWIFIEX_RX_DATA_BUF_SIZE);
 
@@ -238,7 +239,6 @@ int mwifiex_recv_packet(struct mwifiex_private *priv, struct sk_buff *skb)
 int mwifiex_complete_cmd(struct mwifiex_adapter *adapter,
 			 struct cmd_ctrl_node *cmd_node)
 {
-	atomic_dec(&adapter->cmd_pending);
 	dev_dbg(adapter->dev, "cmd completed: status=%d\n",
 		adapter->cmd_wait_q.status);
 
diff --git a/drivers/net/wireless/mwifiex/wmm.c b/drivers/net/wireless/mwifiex/wmm.c
index 32adc878041d..4be3d33ceae8 100644
--- a/drivers/net/wireless/mwifiex/wmm.c
+++ b/drivers/net/wireless/mwifiex/wmm.c
@@ -191,9 +191,6 @@ mwifiex_ralist_add(struct mwifiex_private *priv, u8 *ra)
 		}
 		list_add_tail(&ra_list->list,
 			      &priv->wmm.tid_tbl_ptr[i].ra_list);
-
-		if (!priv->wmm.tid_tbl_ptr[i].ra_list_curr)
-			priv->wmm.tid_tbl_ptr[i].ra_list_curr = ra_list;
 	}
 }
 
@@ -424,7 +421,6 @@ mwifiex_wmm_init(struct mwifiex_adapter *adapter)
 			priv->aggr_prio_tbl[i].amsdu = tos_to_tid_inv[i];
 			priv->aggr_prio_tbl[i].ampdu_ap = tos_to_tid_inv[i];
 			priv->aggr_prio_tbl[i].ampdu_user = tos_to_tid_inv[i];
-			priv->wmm.tid_tbl_ptr[i].ra_list_curr = NULL;
 		}
 
 		priv->aggr_prio_tbl[6].amsdu
@@ -436,10 +432,7 @@ mwifiex_wmm_init(struct mwifiex_adapter *adapter)
 					= priv->aggr_prio_tbl[7].ampdu_user
 					= BA_STREAM_NOT_ALLOWED;
 
-		priv->add_ba_param.timeout = MWIFIEX_DEFAULT_BLOCK_ACK_TIMEOUT;
-		priv->add_ba_param.tx_win_size = MWIFIEX_AMPDU_DEF_TXWINSIZE;
-		priv->add_ba_param.rx_win_size = MWIFIEX_AMPDU_DEF_RXWINSIZE;
-
+		mwifiex_set_ba_params(priv);
 		mwifiex_reset_11n_rx_seq_num(priv);
 
 		atomic_set(&priv->wmm.tx_pkts_queued, 0);
@@ -533,8 +526,6 @@ static void mwifiex_wmm_delete_all_ralist(struct mwifiex_private *priv)
 		}
 
 		INIT_LIST_HEAD(&priv->wmm.tid_tbl_ptr[i].ra_list);
-
-		priv->wmm.tid_tbl_ptr[i].ra_list_curr = NULL;
 	}
 }
 
@@ -688,13 +679,13 @@ mwifiex_wmm_add_buf_txqueue(struct mwifiex_private *priv,
 	ra_list->total_pkts_size += skb->len;
 	ra_list->pkt_count++;
 
-	atomic_inc(&priv->wmm.tx_pkts_queued);
-
 	if (atomic_read(&priv->wmm.highest_queued_prio) <
 						tos_to_tid_inv[tid_down])
 		atomic_set(&priv->wmm.highest_queued_prio,
 			   tos_to_tid_inv[tid_down]);
 
+	atomic_inc(&priv->wmm.tx_pkts_queued);
+
 	spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock, flags);
 }
 
@@ -886,128 +877,65 @@ mwifiex_wmm_get_highest_priolist_ptr(struct mwifiex_adapter *adapter,
 				     struct mwifiex_private **priv, int *tid)
 {
 	struct mwifiex_private *priv_tmp;
-	struct mwifiex_ra_list_tbl *ptr, *head;
-	struct mwifiex_bss_prio_node *bssprio_node, *bssprio_head;
+	struct mwifiex_ra_list_tbl *ptr;
 	struct mwifiex_tid_tbl *tid_ptr;
 	atomic_t *hqp;
-	int is_list_empty;
-	unsigned long flags;
+	unsigned long flags_bss, flags_ra;
 	int i, j;
 
+	/* check the BSS with highest priority first */
 	for (j = adapter->priv_num - 1; j >= 0; --j) {
 		spin_lock_irqsave(&adapter->bss_prio_tbl[j].bss_prio_lock,
-				  flags);
-		is_list_empty = list_empty(&adapter->bss_prio_tbl[j]
-					   .bss_prio_head);
-		spin_unlock_irqrestore(&adapter->bss_prio_tbl[j].bss_prio_lock,
-				       flags);
-		if (is_list_empty)
-			continue;
+				  flags_bss);
 
-		if (adapter->bss_prio_tbl[j].bss_prio_cur ==
-		    (struct mwifiex_bss_prio_node *)
-		    &adapter->bss_prio_tbl[j].bss_prio_head) {
-			adapter->bss_prio_tbl[j].bss_prio_cur =
-				list_first_entry(&adapter->bss_prio_tbl[j]
-						 .bss_prio_head,
-						 struct mwifiex_bss_prio_node,
-						 list);
-		}
+		/* iterate over BSS with the equal priority */
+		list_for_each_entry(adapter->bss_prio_tbl[j].bss_prio_cur,
+				    &adapter->bss_prio_tbl[j].bss_prio_head,
+				    list) {
 
-		bssprio_node = adapter->bss_prio_tbl[j].bss_prio_cur;
-		bssprio_head = bssprio_node;
+			priv_tmp = adapter->bss_prio_tbl[j].bss_prio_cur->priv;
 
-		do {
-			priv_tmp = bssprio_node->priv;
-			hqp = &priv_tmp->wmm.highest_queued_prio;
+			if (atomic_read(&priv_tmp->wmm.tx_pkts_queued) == 0)
+				continue;
 
+			/* iterate over the WMM queues of the BSS */
+			hqp = &priv_tmp->wmm.highest_queued_prio;
 			for (i = atomic_read(hqp); i >= LOW_PRIO_TID; --i) {
 
+				spin_lock_irqsave(&priv_tmp->wmm.
+						  ra_list_spinlock, flags_ra);
+
 				tid_ptr = &(priv_tmp)->wmm.
 					tid_tbl_ptr[tos_to_tid[i]];
 
-				/* For non-STA ra_list_curr may be NULL */
-				if (!tid_ptr->ra_list_curr)
-					continue;
-
-				spin_lock_irqsave(&tid_ptr->tid_tbl_lock,
-						  flags);
-				is_list_empty =
-					list_empty(&adapter->bss_prio_tbl[j]
-						   .bss_prio_head);
-				spin_unlock_irqrestore(&tid_ptr->tid_tbl_lock,
-						       flags);
-				if (is_list_empty)
-					continue;
-
-				/*
-				 * Always choose the next ra we transmitted
-				 * last time, this way we pick the ra's in
-				 * round robin fashion.
-				 */
-				ptr = list_first_entry(
-						&tid_ptr->ra_list_curr->list,
-						struct mwifiex_ra_list_tbl,
-						list);
-
-				head = ptr;
-				if (ptr == (struct mwifiex_ra_list_tbl *)
-						&tid_ptr->ra_list) {
-					/* Get next ra */
-					ptr = list_first_entry(&ptr->list,
-					    struct mwifiex_ra_list_tbl, list);
-					head = ptr;
-				}
-
-				do {
-					is_list_empty =
-						skb_queue_empty(&ptr->skb_head);
+				/* iterate over receiver addresses */
+				list_for_each_entry(ptr, &tid_ptr->ra_list,
+						    list) {
 
-					if (!is_list_empty)
+					if (!skb_queue_empty(&ptr->skb_head))
+						/* holds both locks */
 						goto found;
+				}
 
-					/* Get next ra */
-					ptr = list_first_entry(&ptr->list,
-						 struct mwifiex_ra_list_tbl,
-						 list);
-					if (ptr ==
-					    (struct mwifiex_ra_list_tbl *)
-					    &tid_ptr->ra_list)
-						ptr = list_first_entry(
-						    &ptr->list,
-						    struct mwifiex_ra_list_tbl,
-						    list);
-				} while (ptr != head);
+				spin_unlock_irqrestore(&priv_tmp->wmm.
+						       ra_list_spinlock,
+						       flags_ra);
 			}
+		}
 
-			/* No packet at any TID for this priv. Mark as such
-			 * to skip checking TIDs for this priv (until pkt is
-			 * added).
-			 */
-			atomic_set(hqp, NO_PKT_PRIO_TID);
-
-			/* Get next bss priority node */
-			bssprio_node = list_first_entry(&bssprio_node->list,
-						struct mwifiex_bss_prio_node,
-						list);
-
-			if (bssprio_node ==
-			    (struct mwifiex_bss_prio_node *)
-			    &adapter->bss_prio_tbl[j].bss_prio_head)
-				/* Get next bss priority node */
-				bssprio_node = list_first_entry(
-						&bssprio_node->list,
-						struct mwifiex_bss_prio_node,
-						list);
-		} while (bssprio_node != bssprio_head);
+		spin_unlock_irqrestore(&adapter->bss_prio_tbl[j].bss_prio_lock,
+				       flags_bss);
 	}
+
 	return NULL;
 
 found:
-	spin_lock_irqsave(&priv_tmp->wmm.ra_list_spinlock, flags);
+	/* holds bss_prio_lock / ra_list_spinlock */
 	if (atomic_read(hqp) > i)
 		atomic_set(hqp, i);
-	spin_unlock_irqrestore(&priv_tmp->wmm.ra_list_spinlock, flags);
+	spin_unlock_irqrestore(&priv_tmp->wmm.ra_list_spinlock, flags_ra);
+	spin_unlock_irqrestore(&adapter->bss_prio_tbl[j].bss_prio_lock,
+			       flags_bss);
 
 	*priv = priv_tmp;
 	*tid = tos_to_tid[i];
@@ -1015,6 +943,42 @@ found:
 	return ptr;
 }
 
+/* This functions rotates ra and bss lists so packets are picked round robin.
+ *
+ * After a packet is successfully transmitted, rotate the ra list, so the ra
+ * next to the one transmitted, will come first in the list. This way we pick
+ * the ra' in a round robin fashion. Same applies to bss nodes of equal
+ * priority.
+ *
+ * Function also increments wmm.packets_out counter.
+ */
+void mwifiex_rotate_priolists(struct mwifiex_private *priv,
+				 struct mwifiex_ra_list_tbl *ra,
+				 int tid)
+{
+	struct mwifiex_adapter *adapter = priv->adapter;
+	struct mwifiex_bss_prio_tbl *tbl = adapter->bss_prio_tbl;
+	struct mwifiex_tid_tbl *tid_ptr = &priv->wmm.tid_tbl_ptr[tid];
+	unsigned long flags;
+
+	spin_lock_irqsave(&tbl[priv->bss_priority].bss_prio_lock, flags);
+	/*
+	 * dirty trick: we remove 'head' temporarily and reinsert it after
+	 * curr bss node. imagine list to stay fixed while head is moved
+	 */
+	list_move(&tbl[priv->bss_priority].bss_prio_head,
+		  &tbl[priv->bss_priority].bss_prio_cur->list);
+	spin_unlock_irqrestore(&tbl[priv->bss_priority].bss_prio_lock, flags);
+
+	spin_lock_irqsave(&priv->wmm.ra_list_spinlock, flags);
+	if (mwifiex_is_ralist_valid(priv, ra, tid)) {
+		priv->wmm.packets_out[tid]++;
+		/* same as above */
+		list_move(&tid_ptr->ra_list, &ra->list);
+	}
+	spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock, flags);
+}
+
 /*
  * This function checks if 11n aggregation is possible.
  */
@@ -1101,20 +1065,8 @@ mwifiex_send_single_packet(struct mwifiex_private *priv,
 		spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
 				       ra_list_flags);
 	} else {
-		spin_lock_irqsave(&priv->wmm.ra_list_spinlock, ra_list_flags);
-		if (mwifiex_is_ralist_valid(priv, ptr, ptr_index)) {
-			priv->wmm.packets_out[ptr_index]++;
-			priv->wmm.tid_tbl_ptr[ptr_index].ra_list_curr = ptr;
-		}
-		adapter->bss_prio_tbl[priv->bss_priority].bss_prio_cur =
-			list_first_entry(
-				&adapter->bss_prio_tbl[priv->bss_priority]
-				.bss_prio_cur->list,
-				struct mwifiex_bss_prio_node,
-				list);
+		mwifiex_rotate_priolists(priv, ptr, ptr_index);
 		atomic_dec(&priv->wmm.tx_pkts_queued);
-		spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
-				       ra_list_flags);
 	}
 }
 
@@ -1218,20 +1170,8 @@ mwifiex_send_processed_packet(struct mwifiex_private *priv,
 		break;
 	}
 	if (ret != -EBUSY) {
-		spin_lock_irqsave(&priv->wmm.ra_list_spinlock, ra_list_flags);
-		if (mwifiex_is_ralist_valid(priv, ptr, ptr_index)) {
-			priv->wmm.packets_out[ptr_index]++;
-			priv->wmm.tid_tbl_ptr[ptr_index].ra_list_curr = ptr;
-		}
-		adapter->bss_prio_tbl[priv->bss_priority].bss_prio_cur =
-			list_first_entry(
-				&adapter->bss_prio_tbl[priv->bss_priority]
-				.bss_prio_cur->list,
-				struct mwifiex_bss_prio_node,
-				list);
+		mwifiex_rotate_priolists(priv, ptr, ptr_index);
 		atomic_dec(&priv->wmm.tx_pkts_queued);
-		spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
-				       ra_list_flags);
 	}
 }
 
diff --git a/drivers/net/wireless/mwifiex/wmm.h b/drivers/net/wireless/mwifiex/wmm.h
index b92f39d8963b..644d6e0c51cc 100644
--- a/drivers/net/wireless/mwifiex/wmm.h
+++ b/drivers/net/wireless/mwifiex/wmm.h
@@ -85,6 +85,9 @@ mwifiex_wmm_is_ra_list_empty(struct list_head *ra_list_hhead)
 void mwifiex_wmm_add_buf_txqueue(struct mwifiex_private *priv,
 					struct sk_buff *skb);
 void mwifiex_ralist_add(struct mwifiex_private *priv, u8 *ra);
+void mwifiex_rotate_priolists(struct mwifiex_private *priv,
+			      struct mwifiex_ra_list_tbl *ra,
+			      int tid);
 
 int mwifiex_wmm_lists_empty(struct mwifiex_adapter *adapter);
 void mwifiex_wmm_process_tx(struct mwifiex_adapter *adapter);
diff --git a/drivers/net/wireless/mwl8k.c b/drivers/net/wireless/mwl8k.c
index 091d9a64080a..6820fce4016b 100644
--- a/drivers/net/wireless/mwl8k.c
+++ b/drivers/net/wireless/mwl8k.c
@@ -193,10 +193,10 @@ struct mwl8k_priv {
 	struct rxd_ops *rxd_ops;
 	struct ieee80211_supported_band band_24;
 	struct ieee80211_channel channels_24[14];
-	struct ieee80211_rate rates_24[14];
+	struct ieee80211_rate rates_24[13];
 	struct ieee80211_supported_band band_50;
 	struct ieee80211_channel channels_50[4];
-	struct ieee80211_rate rates_50[9];
+	struct ieee80211_rate rates_50[8];
 	u32 ap_macids_supported;
 	u32 sta_macids_supported;
 
@@ -232,6 +232,7 @@ struct mwl8k_priv {
 	u16 num_mcaddrs;
 	u8 hw_rev;
 	u32 fw_rev;
+	u32 caps;
 
 	/*
 	 * Running count of TX packets in flight, to avoid
@@ -284,6 +285,7 @@ struct mwl8k_priv {
 	unsigned fw_state;
 	char *fw_pref;
 	char *fw_alt;
+	bool is_8764;
 	struct completion firmware_loading_complete;
 
 	/* bitmap of running BSSes */
@@ -364,7 +366,6 @@ static const struct ieee80211_rate mwl8k_rates_24[] = {
 	{ .bitrate = 360, .hw_value = 72, },
 	{ .bitrate = 480, .hw_value = 96, },
 	{ .bitrate = 540, .hw_value = 108, },
-	{ .bitrate = 720, .hw_value = 144, },
 };
 
 static const struct ieee80211_channel mwl8k_channels_50[] = {
@@ -383,7 +384,6 @@ static const struct ieee80211_rate mwl8k_rates_50[] = {
 	{ .bitrate = 360, .hw_value = 72, },
 	{ .bitrate = 480, .hw_value = 96, },
 	{ .bitrate = 540, .hw_value = 108, },
-	{ .bitrate = 720, .hw_value = 144, },
 };
 
 /* Set or get info from Firmware */
@@ -600,13 +600,18 @@ mwl8k_send_fw_load_cmd(struct mwl8k_priv *priv, void *data, int length)
 	loops = 1000;
 	do {
 		u32 int_code;
-
-		int_code = ioread32(regs + MWL8K_HIU_INT_CODE);
-		if (int_code == MWL8K_INT_CODE_CMD_FINISHED) {
-			iowrite32(0, regs + MWL8K_HIU_INT_CODE);
-			break;
+		if (priv->is_8764) {
+			int_code = ioread32(regs +
+					    MWL8K_HIU_H2A_INTERRUPT_STATUS);
+			if (int_code == 0)
+				break;
+		} else {
+			int_code = ioread32(regs + MWL8K_HIU_INT_CODE);
+			if (int_code == MWL8K_INT_CODE_CMD_FINISHED) {
+				iowrite32(0, regs + MWL8K_HIU_INT_CODE);
+				break;
+			}
 		}
-
 		cond_resched();
 		udelay(1);
 	} while (--loops);
@@ -724,7 +729,7 @@ static int mwl8k_load_firmware(struct ieee80211_hw *hw)
 	int rc;
 	int loops;
 
-	if (!memcmp(fw->data, "\x01\x00\x00\x00", 4)) {
+	if (!memcmp(fw->data, "\x01\x00\x00\x00", 4) && !priv->is_8764) {
 		const struct firmware *helper = priv->fw_helper;
 
 		if (helper == NULL) {
@@ -743,7 +748,10 @@ static int mwl8k_load_firmware(struct ieee80211_hw *hw)
 
 		rc = mwl8k_feed_fw_image(priv, fw->data, fw->size);
 	} else {
-		rc = mwl8k_load_fw_image(priv, fw->data, fw->size);
+		if (priv->is_8764)
+			rc = mwl8k_feed_fw_image(priv, fw->data, fw->size);
+		else
+			rc = mwl8k_load_fw_image(priv, fw->data, fw->size);
 	}
 
 	if (rc) {
@@ -908,9 +916,9 @@ static void mwl8k_encapsulate_tx_frame(struct mwl8k_priv *priv,
 }
 
 /*
- * Packet reception for 88w8366 AP firmware.
+ * Packet reception for 88w8366/88w8764 AP firmware.
  */
-struct mwl8k_rxd_8366_ap {
+struct mwl8k_rxd_ap {
 	__le16 pkt_len;
 	__u8 sq2;
 	__u8 rate;
@@ -928,30 +936,30 @@ struct mwl8k_rxd_8366_ap {
 	__u8 rx_ctrl;
 } __packed;
 
-#define MWL8K_8366_AP_RATE_INFO_MCS_FORMAT	0x80
-#define MWL8K_8366_AP_RATE_INFO_40MHZ		0x40
-#define MWL8K_8366_AP_RATE_INFO_RATEID(x)	((x) & 0x3f)
+#define MWL8K_AP_RATE_INFO_MCS_FORMAT		0x80
+#define MWL8K_AP_RATE_INFO_40MHZ		0x40
+#define MWL8K_AP_RATE_INFO_RATEID(x)		((x) & 0x3f)
 
-#define MWL8K_8366_AP_RX_CTRL_OWNED_BY_HOST	0x80
+#define MWL8K_AP_RX_CTRL_OWNED_BY_HOST		0x80
 
-/* 8366 AP rx_status bits */
-#define MWL8K_8366_AP_RXSTAT_DECRYPT_ERR_MASK		0x80
-#define MWL8K_8366_AP_RXSTAT_GENERAL_DECRYPT_ERR	0xFF
-#define MWL8K_8366_AP_RXSTAT_TKIP_DECRYPT_MIC_ERR	0x02
-#define MWL8K_8366_AP_RXSTAT_WEP_DECRYPT_ICV_ERR	0x04
-#define MWL8K_8366_AP_RXSTAT_TKIP_DECRYPT_ICV_ERR	0x08
+/* 8366/8764 AP rx_status bits */
+#define MWL8K_AP_RXSTAT_DECRYPT_ERR_MASK		0x80
+#define MWL8K_AP_RXSTAT_GENERAL_DECRYPT_ERR		0xFF
+#define MWL8K_AP_RXSTAT_TKIP_DECRYPT_MIC_ERR		0x02
+#define MWL8K_AP_RXSTAT_WEP_DECRYPT_ICV_ERR		0x04
+#define MWL8K_AP_RXSTAT_TKIP_DECRYPT_ICV_ERR		0x08
 
-static void mwl8k_rxd_8366_ap_init(void *_rxd, dma_addr_t next_dma_addr)
+static void mwl8k_rxd_ap_init(void *_rxd, dma_addr_t next_dma_addr)
 {
-	struct mwl8k_rxd_8366_ap *rxd = _rxd;
+	struct mwl8k_rxd_ap *rxd = _rxd;
 
 	rxd->next_rxd_phys_addr = cpu_to_le32(next_dma_addr);
-	rxd->rx_ctrl = MWL8K_8366_AP_RX_CTRL_OWNED_BY_HOST;
+	rxd->rx_ctrl = MWL8K_AP_RX_CTRL_OWNED_BY_HOST;
 }
 
-static void mwl8k_rxd_8366_ap_refill(void *_rxd, dma_addr_t addr, int len)
+static void mwl8k_rxd_ap_refill(void *_rxd, dma_addr_t addr, int len)
 {
-	struct mwl8k_rxd_8366_ap *rxd = _rxd;
+	struct mwl8k_rxd_ap *rxd = _rxd;
 
 	rxd->pkt_len = cpu_to_le16(len);
 	rxd->pkt_phys_addr = cpu_to_le32(addr);
@@ -960,12 +968,12 @@ static void mwl8k_rxd_8366_ap_refill(void *_rxd, dma_addr_t addr, int len)
 }
 
 static int
-mwl8k_rxd_8366_ap_process(void *_rxd, struct ieee80211_rx_status *status,
-			  __le16 *qos, s8 *noise)
+mwl8k_rxd_ap_process(void *_rxd, struct ieee80211_rx_status *status,
+		     __le16 *qos, s8 *noise)
 {
-	struct mwl8k_rxd_8366_ap *rxd = _rxd;
+	struct mwl8k_rxd_ap *rxd = _rxd;
 
-	if (!(rxd->rx_ctrl & MWL8K_8366_AP_RX_CTRL_OWNED_BY_HOST))
+	if (!(rxd->rx_ctrl & MWL8K_AP_RX_CTRL_OWNED_BY_HOST))
 		return -1;
 	rmb();
 
@@ -974,11 +982,11 @@ mwl8k_rxd_8366_ap_process(void *_rxd, struct ieee80211_rx_status *status,
 	status->signal = -rxd->rssi;
 	*noise = -rxd->noise_floor;
 
-	if (rxd->rate & MWL8K_8366_AP_RATE_INFO_MCS_FORMAT) {
+	if (rxd->rate & MWL8K_AP_RATE_INFO_MCS_FORMAT) {
 		status->flag |= RX_FLAG_HT;
-		if (rxd->rate & MWL8K_8366_AP_RATE_INFO_40MHZ)
+		if (rxd->rate & MWL8K_AP_RATE_INFO_40MHZ)
 			status->flag |= RX_FLAG_40MHZ;
-		status->rate_idx = MWL8K_8366_AP_RATE_INFO_RATEID(rxd->rate);
+		status->rate_idx = MWL8K_AP_RATE_INFO_RATEID(rxd->rate);
 	} else {
 		int i;
 
@@ -1002,19 +1010,19 @@ mwl8k_rxd_8366_ap_process(void *_rxd, struct ieee80211_rx_status *status,
 
 	*qos = rxd->qos_control;
 
-	if ((rxd->rx_status != MWL8K_8366_AP_RXSTAT_GENERAL_DECRYPT_ERR) &&
-	    (rxd->rx_status & MWL8K_8366_AP_RXSTAT_DECRYPT_ERR_MASK) &&
-	    (rxd->rx_status & MWL8K_8366_AP_RXSTAT_TKIP_DECRYPT_MIC_ERR))
+	if ((rxd->rx_status != MWL8K_AP_RXSTAT_GENERAL_DECRYPT_ERR) &&
+	    (rxd->rx_status & MWL8K_AP_RXSTAT_DECRYPT_ERR_MASK) &&
+	    (rxd->rx_status & MWL8K_AP_RXSTAT_TKIP_DECRYPT_MIC_ERR))
 		status->flag |= RX_FLAG_MMIC_ERROR;
 
 	return le16_to_cpu(rxd->pkt_len);
 }
 
-static struct rxd_ops rxd_8366_ap_ops = {
-	.rxd_size	= sizeof(struct mwl8k_rxd_8366_ap),
-	.rxd_init	= mwl8k_rxd_8366_ap_init,
-	.rxd_refill	= mwl8k_rxd_8366_ap_refill,
-	.rxd_process	= mwl8k_rxd_8366_ap_process,
+static struct rxd_ops rxd_ap_ops = {
+	.rxd_size	= sizeof(struct mwl8k_rxd_ap),
+	.rxd_init	= mwl8k_rxd_ap_init,
+	.rxd_refill	= mwl8k_rxd_ap_refill,
+	.rxd_process	= mwl8k_rxd_ap_process,
 };
 
 /*
@@ -2401,6 +2409,9 @@ mwl8k_set_caps(struct ieee80211_hw *hw, u32 caps)
 {
 	struct mwl8k_priv *priv = hw->priv;
 
+	if (priv->caps)
+		return;
+
 	if ((caps & MWL8K_CAP_2GHZ4) || !(caps & MWL8K_CAP_BAND_MASK)) {
 		mwl8k_setup_2ghz_band(hw);
 		if (caps & MWL8K_CAP_MIMO)
@@ -2412,6 +2423,8 @@ mwl8k_set_caps(struct ieee80211_hw *hw, u32 caps)
 		if (caps & MWL8K_CAP_MIMO)
 			mwl8k_set_ht_caps(hw, &priv->band_50, caps);
 	}
+
+	priv->caps = caps;
 }
 
 static int mwl8k_cmd_get_hw_spec_sta(struct ieee80211_hw *hw)
@@ -2837,7 +2850,9 @@ static int mwl8k_cmd_tx_power(struct ieee80211_hw *hw,
 				     struct ieee80211_conf *conf,
 				     unsigned short pwr)
 {
-	struct ieee80211_channel *channel = conf->channel;
+	struct ieee80211_channel *channel = conf->chandef.chan;
+	enum nl80211_channel_type channel_type =
+		cfg80211_get_chandef_type(&conf->chandef);
 	struct mwl8k_cmd_tx_power *cmd;
 	int rc;
 	int i;
@@ -2857,14 +2872,14 @@ static int mwl8k_cmd_tx_power(struct ieee80211_hw *hw,
 
 	cmd->channel = cpu_to_le16(channel->hw_value);
 
-	if (conf->channel_type == NL80211_CHAN_NO_HT ||
-	    conf->channel_type == NL80211_CHAN_HT20) {
+	if (channel_type == NL80211_CHAN_NO_HT ||
+	    channel_type == NL80211_CHAN_HT20) {
 		cmd->bw = cpu_to_le16(0x2);
 	} else {
 		cmd->bw = cpu_to_le16(0x4);
-		if (conf->channel_type == NL80211_CHAN_HT40MINUS)
+		if (channel_type == NL80211_CHAN_HT40MINUS)
 			cmd->sub_ch = cpu_to_le16(0x3);
-		else if (conf->channel_type == NL80211_CHAN_HT40PLUS)
+		else if (channel_type == NL80211_CHAN_HT40PLUS)
 			cmd->sub_ch = cpu_to_le16(0x1);
 	}
 
@@ -3008,7 +3023,9 @@ struct mwl8k_cmd_set_rf_channel {
 static int mwl8k_cmd_set_rf_channel(struct ieee80211_hw *hw,
 				    struct ieee80211_conf *conf)
 {
-	struct ieee80211_channel *channel = conf->channel;
+	struct ieee80211_channel *channel = conf->chandef.chan;
+	enum nl80211_channel_type channel_type =
+		cfg80211_get_chandef_type(&conf->chandef);
 	struct mwl8k_cmd_set_rf_channel *cmd;
 	int rc;
 
@@ -3026,12 +3043,12 @@ static int mwl8k_cmd_set_rf_channel(struct ieee80211_hw *hw,
 	else if (channel->band == IEEE80211_BAND_5GHZ)
 		cmd->channel_flags |= cpu_to_le32(0x00000004);
 
-	if (conf->channel_type == NL80211_CHAN_NO_HT ||
-	    conf->channel_type == NL80211_CHAN_HT20)
+	if (channel_type == NL80211_CHAN_NO_HT ||
+	    channel_type == NL80211_CHAN_HT20)
 		cmd->channel_flags |= cpu_to_le32(0x00000080);
-	else if (conf->channel_type == NL80211_CHAN_HT40MINUS)
+	else if (channel_type == NL80211_CHAN_HT40MINUS)
 		cmd->channel_flags |= cpu_to_le32(0x000001900);
-	else if (conf->channel_type == NL80211_CHAN_HT40PLUS)
+	else if (channel_type == NL80211_CHAN_HT40PLUS)
 		cmd->channel_flags |= cpu_to_le32(0x000000900);
 
 	rc = mwl8k_post_cmd(hw, &cmd->header);
@@ -3064,11 +3081,11 @@ static void legacy_rate_mask_to_array(u8 *rates, u32 mask)
 	int j;
 
 	/*
-	 * Clear nonstandard rates 4 and 13.
+	 * Clear nonstandard rate 4.
 	 */
 	mask &= 0x1fef;
 
-	for (i = 0, j = 0; i < 14; i++) {
+	for (i = 0, j = 0; i < 13; i++) {
 		if (mask & (1 << i))
 			rates[j++] = mwl8k_rates_24[i].hw_value;
 	}
@@ -3950,7 +3967,7 @@ static int mwl8k_cmd_set_new_stn_add(struct ieee80211_hw *hw,
 	memcpy(cmd->mac_addr, sta->addr, ETH_ALEN);
 	cmd->stn_id = cpu_to_le16(sta->aid);
 	cmd->action = cpu_to_le16(MWL8K_STA_ACTION_ADD);
-	if (hw->conf.channel->band == IEEE80211_BAND_2GHZ)
+	if (hw->conf.chandef.chan->band == IEEE80211_BAND_2GHZ)
 		rates = sta->supp_rates[IEEE80211_BAND_2GHZ];
 	else
 		rates = sta->supp_rates[IEEE80211_BAND_5GHZ] << 5;
@@ -4385,7 +4402,7 @@ static int mwl8k_cmd_update_stadb_add(struct ieee80211_hw *hw,
 	p->ht_caps = cpu_to_le16(sta->ht_cap.cap);
 	p->extended_ht_caps = (sta->ht_cap.ampdu_factor & 3) |
 		((sta->ht_cap.ampdu_density & 7) << 2);
-	if (hw->conf.channel->band == IEEE80211_BAND_2GHZ)
+	if (hw->conf.chandef.chan->band == IEEE80211_BAND_2GHZ)
 		rates = sta->supp_rates[IEEE80211_BAND_2GHZ];
 	else
 		rates = sta->supp_rates[IEEE80211_BAND_5GHZ] << 5;
@@ -4792,16 +4809,14 @@ static int mwl8k_config(struct ieee80211_hw *hw, u32 changed)
 	struct mwl8k_priv *priv = hw->priv;
 	int rc;
 
-	if (conf->flags & IEEE80211_CONF_IDLE) {
-		mwl8k_cmd_radio_disable(hw);
-		return 0;
-	}
-
 	rc = mwl8k_fw_lock(hw);
 	if (rc)
 		return rc;
 
-	rc = mwl8k_cmd_radio_enable(hw);
+	if (conf->flags & IEEE80211_CONF_IDLE)
+		rc = mwl8k_cmd_radio_disable(hw);
+	else
+		rc = mwl8k_cmd_radio_enable(hw);
 	if (rc)
 		goto out;
 
@@ -4868,7 +4883,7 @@ mwl8k_bss_info_changed_sta(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			goto out;
 		}
 
-		if (hw->conf.channel->band == IEEE80211_BAND_2GHZ) {
+		if (hw->conf.chandef.chan->band == IEEE80211_BAND_2GHZ) {
 			ap_legacy_rates = ap->supp_rates[IEEE80211_BAND_2GHZ];
 		} else {
 			ap_legacy_rates =
@@ -4900,7 +4915,7 @@ mwl8k_bss_info_changed_sta(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			if (idx)
 				idx--;
 
-			if (hw->conf.channel->band == IEEE80211_BAND_2GHZ)
+			if (hw->conf.chandef.chan->band == IEEE80211_BAND_2GHZ)
 				rate = mwl8k_rates_24[idx].hw_value;
 			else
 				rate = mwl8k_rates_50[idx].hw_value;
@@ -4973,7 +4988,7 @@ mwl8k_bss_info_changed_ap(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		if (idx)
 			idx--;
 
-		if (hw->conf.channel->band == IEEE80211_BAND_2GHZ)
+		if (hw->conf.chandef.chan->band == IEEE80211_BAND_2GHZ)
 			rate = mwl8k_rates_24[idx].hw_value;
 		else
 			rate = mwl8k_rates_50[idx].hw_value;
@@ -5246,7 +5261,7 @@ static int mwl8k_get_survey(struct ieee80211_hw *hw, int idx,
 	if (idx != 0)
 		return -ENOENT;
 
-	survey->channel = conf->channel;
+	survey->channel = conf->chandef.chan;
 	survey->filled = SURVEY_INFO_NOISE_DBM;
 	survey->noise = priv->noise;
 
@@ -5429,12 +5444,17 @@ enum {
 	MWL8363 = 0,
 	MWL8687,
 	MWL8366,
+	MWL8764,
 };
 
 #define MWL8K_8366_AP_FW_API 3
 #define _MWL8K_8366_AP_FW(api) "mwl8k/fmimage_8366_ap-" #api ".fw"
 #define MWL8K_8366_AP_FW(api) _MWL8K_8366_AP_FW(api)
 
+#define MWL8K_8764_AP_FW_API 1
+#define _MWL8K_8764_AP_FW(api) "mwl8k/fmimage_8764_ap-" #api ".fw"
+#define MWL8K_8764_AP_FW(api) _MWL8K_8764_AP_FW(api)
+
 static struct mwl8k_device_info mwl8k_info_tbl[] = {
 	[MWL8363] = {
 		.part_name	= "88w8363",
@@ -5452,7 +5472,13 @@ static struct mwl8k_device_info mwl8k_info_tbl[] = {
 		.fw_image_sta	= "mwl8k/fmimage_8366.fw",
 		.fw_image_ap	= MWL8K_8366_AP_FW(MWL8K_8366_AP_FW_API),
 		.fw_api_ap	= MWL8K_8366_AP_FW_API,
-		.ap_rxd_ops	= &rxd_8366_ap_ops,
+		.ap_rxd_ops	= &rxd_ap_ops,
+	},
+	[MWL8764] = {
+		.part_name	= "88w8764",
+		.fw_image_ap	= MWL8K_8764_AP_FW(MWL8K_8764_AP_FW_API),
+		.fw_api_ap	= MWL8K_8764_AP_FW_API,
+		.ap_rxd_ops	= &rxd_ap_ops,
 	},
 };
 
@@ -5474,6 +5500,7 @@ static DEFINE_PCI_DEVICE_TABLE(mwl8k_pci_id_table) = {
 	{ PCI_VDEVICE(MARVELL, 0x2a41), .driver_data = MWL8366, },
 	{ PCI_VDEVICE(MARVELL, 0x2a42), .driver_data = MWL8366, },
 	{ PCI_VDEVICE(MARVELL, 0x2a43), .driver_data = MWL8366, },
+	{ PCI_VDEVICE(MARVELL, 0x2b36), .driver_data = MWL8764, },
 	{ },
 };
 MODULE_DEVICE_TABLE(pci, mwl8k_pci_id_table);
@@ -5995,6 +6022,8 @@ static int mwl8k_probe(struct pci_dev *pdev,
 	priv->pdev = pdev;
 	priv->device_info = &mwl8k_info_tbl[id->driver_data];
 
+	if (id->driver_data == MWL8764)
+		priv->is_8764 = true;
 
 	priv->sram = pci_iomap(pdev, 0, 0x10000);
 	if (priv->sram == NULL) {
diff --git a/drivers/net/wireless/orinoco/orinoco_usb.c b/drivers/net/wireless/orinoco/orinoco_usb.c
index 7744f42de1ea..1f9cb55c3360 100644
--- a/drivers/net/wireless/orinoco/orinoco_usb.c
+++ b/drivers/net/wireless/orinoco/orinoco_usb.c
@@ -1584,7 +1584,7 @@ static int ezusb_probe(struct usb_interface *interface,
 	struct ezusb_priv *upriv = NULL;
 	struct usb_interface_descriptor *iface_desc;
 	struct usb_endpoint_descriptor *ep;
-	const struct firmware *fw_entry;
+	const struct firmware *fw_entry = NULL;
 	int retval = 0;
 	int i;
 
diff --git a/drivers/net/wireless/p54/fwio.c b/drivers/net/wireless/p54/fwio.c
index 9ba85106eec0..b3879fbf5368 100644
--- a/drivers/net/wireless/p54/fwio.c
+++ b/drivers/net/wireless/p54/fwio.c
@@ -402,7 +402,7 @@ int p54_scan(struct p54_common *priv, u16 mode, u16 dwell)
 	struct p54_rssi_db_entry *rssi_data;
 	unsigned int i;
 	void *entry;
-	__le16 freq = cpu_to_le16(priv->hw->conf.channel->center_freq);
+	__le16 freq = cpu_to_le16(priv->hw->conf.chandef.chan->center_freq);
 
 	skb = p54_alloc_skb(priv, P54_HDR_FLAG_CONTROL_OPSET, sizeof(*head) +
 			    2 + sizeof(*iq_autocal) + sizeof(*body) +
@@ -532,7 +532,7 @@ int p54_scan(struct p54_common *priv, u16 mode, u16 dwell)
 err:
 	wiphy_err(priv->hw->wiphy, "frequency change to channel %d failed.\n",
 		  ieee80211_frequency_to_channel(
-			  priv->hw->conf.channel->center_freq));
+			  priv->hw->conf.chandef.chan->center_freq));
 
 	dev_kfree_skb_any(skb);
 	return -EINVAL;
diff --git a/drivers/net/wireless/p54/main.c b/drivers/net/wireless/p54/main.c
index aadda99989c0..067e6f2fd050 100644
--- a/drivers/net/wireless/p54/main.c
+++ b/drivers/net/wireless/p54/main.c
@@ -340,7 +340,7 @@ static int p54_config(struct ieee80211_hw *dev, u32 changed)
 		 * TODO: Use the LM_SCAN_TRAP to determine the current
 		 * operating channel.
 		 */
-		priv->curchan = priv->hw->conf.channel;
+		priv->curchan = priv->hw->conf.chandef.chan;
 		p54_reset_stats(priv);
 		WARN_ON(p54_fetch_statistics(priv));
 	}
@@ -480,7 +480,7 @@ static void p54_bss_info_changed(struct ieee80211_hw *dev,
 		p54_set_edcf(priv);
 	}
 	if (changed & BSS_CHANGED_BASIC_RATES) {
-		if (dev->conf.channel->band == IEEE80211_BAND_5GHZ)
+		if (dev->conf.chandef.chan->band == IEEE80211_BAND_5GHZ)
 			priv->basic_rate_mask = (info->basic_rates << 4);
 		else
 			priv->basic_rate_mask = info->basic_rates;
@@ -670,7 +670,7 @@ static unsigned int p54_flush_count(struct p54_common *priv)
 	return total;
 }
 
-static void p54_flush(struct ieee80211_hw *dev, bool drop)
+static void p54_flush(struct ieee80211_hw *dev, u32 queues, bool drop)
 {
 	struct p54_common *priv = dev->priv;
 	unsigned int total, i;
diff --git a/drivers/net/wireless/p54/p54spi.c b/drivers/net/wireless/p54/p54spi.c
index 4fd49a007b51..978e7eb26567 100644
--- a/drivers/net/wireless/p54/p54spi.c
+++ b/drivers/net/wireless/p54/p54spi.c
@@ -396,7 +396,7 @@ static int p54spi_rx(struct p54s_priv *priv)
 static irqreturn_t p54spi_interrupt(int irq, void *config)
 {
 	struct spi_device *spi = config;
-	struct p54s_priv *priv = dev_get_drvdata(&spi->dev);
+	struct p54s_priv *priv = spi_get_drvdata(spi);
 
 	ieee80211_queue_work(priv->hw, &priv->work);
 
@@ -609,7 +609,7 @@ static int p54spi_probe(struct spi_device *spi)
 
 	priv = hw->priv;
 	priv->hw = hw;
-	dev_set_drvdata(&spi->dev, priv);
+	spi_set_drvdata(spi, priv);
 	priv->spi = spi;
 
 	spi->bits_per_word = 16;
@@ -685,7 +685,7 @@ err_free:
 
 static int p54spi_remove(struct spi_device *spi)
 {
-	struct p54s_priv *priv = dev_get_drvdata(&spi->dev);
+	struct p54s_priv *priv = spi_get_drvdata(spi);
 
 	p54_unregister_common(priv->hw);
 
diff --git a/drivers/net/wireless/p54/txrx.c b/drivers/net/wireless/p54/txrx.c
index 12f0a34477f2..f95de0d16216 100644
--- a/drivers/net/wireless/p54/txrx.c
+++ b/drivers/net/wireless/p54/txrx.c
@@ -354,13 +354,13 @@ static int p54_rx_data(struct p54_common *priv, struct sk_buff *skb)
 	rx_status->signal = p54_rssi_to_dbm(priv, hdr->rssi);
 	if (hdr->rate & 0x10)
 		rx_status->flag |= RX_FLAG_SHORTPRE;
-	if (priv->hw->conf.channel->band == IEEE80211_BAND_5GHZ)
+	if (priv->hw->conf.chandef.chan->band == IEEE80211_BAND_5GHZ)
 		rx_status->rate_idx = (rate < 4) ? 0 : rate - 4;
 	else
 		rx_status->rate_idx = rate;
 
 	rx_status->freq = freq;
-	rx_status->band =  priv->hw->conf.channel->band;
+	rx_status->band =  priv->hw->conf.chandef.chan->band;
 	rx_status->antenna = hdr->antenna;
 
 	tsf32 = le32_to_cpu(hdr->tsf32);
diff --git a/drivers/net/wireless/ray_cs.c b/drivers/net/wireless/ray_cs.c
index 3109c0db66e1..9b557a1bb7f8 100644
--- a/drivers/net/wireless/ray_cs.c
+++ b/drivers/net/wireless/ray_cs.c
@@ -144,7 +144,7 @@ static int psm;
 static char *essid;
 
 /* Default to encapsulation unless translation requested */
-static int translate = 1;
+static bool translate = 1;
 
 static int country = USA;
 
@@ -178,7 +178,7 @@ module_param(hop_dwell, int, 0);
 module_param(beacon_period, int, 0);
 module_param(psm, int, 0);
 module_param(essid, charp, 0);
-module_param(translate, int, 0);
+module_param(translate, bool, 0);
 module_param(country, int, 0);
 module_param(sniffer, int, 0);
 module_param(bc, int, 0);
@@ -953,7 +953,7 @@ static int translate_frame(ray_dev_t *local, struct tx_msg __iomem *ptx,
 			   unsigned char *data, int len)
 {
 	__be16 proto = ((struct ethhdr *)data)->h_proto;
-	if (ntohs(proto) >= 1536) { /* DIX II ethernet frame */
+	if (ntohs(proto) >= ETH_P_802_3_MIN) { /* DIX II ethernet frame */
 		pr_debug("ray_cs translate_frame DIX II\n");
 		/* Copy LLC header to card buffer */
 		memcpy_toio(&ptx->var, eth2_llc, sizeof(eth2_llc));
@@ -1353,7 +1353,7 @@ static int ray_get_range(struct net_device *dev, struct iw_request_info *info,
 static int ray_set_framing(struct net_device *dev, struct iw_request_info *info,
 			   union iwreq_data *wrqu, char *extra)
 {
-	translate = *(extra);	/* Set framing mode */
+	translate = !!*(extra);	/* Set framing mode */
 
 	return 0;
 }
@@ -2778,7 +2778,7 @@ static ssize_t int_proc_write(struct file *file, const char __user *buffer,
 		nr = nr * 10 + c;
 		p++;
 	} while (--len);
-	*(int *)PDE(file_inode(file))->data = nr;
+	*(int *)PDE_DATA(file_inode(file)) = nr;
 	return count;
 }
 
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index 525fd7521dff..8169a85c4498 100644
--- a/drivers/net/wireless/rndis_wlan.c
+++ b/drivers/net/wireless/rndis_wlan.c
@@ -2,7 +2,7 @@
  * Driver for RNDIS based wireless USB devices.
  *
  * Copyright (C) 2007 by Bjorge Dijkstra <bjd@jooz.net>
- * Copyright (C) 2008-2009 by Jussi Kivilinna <jussi.kivilinna@mbnet.fi>
+ * Copyright (C) 2008-2009 by Jussi Kivilinna <jussi.kivilinna@iki.fi>
  *
  * 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
@@ -2839,8 +2839,7 @@ static void rndis_wlan_do_link_up_work(struct usbnet *usbdev)
 	} else if (priv->infra_mode == NDIS_80211_INFRA_ADHOC)
 		cfg80211_ibss_joined(usbdev->net, bssid, GFP_KERNEL);
 
-	if (info != NULL)
-		kfree(info);
+	kfree(info);
 
 	priv->connected = true;
 	memcpy(priv->bssid, bssid, ETH_ALEN);
diff --git a/drivers/net/wireless/rt2x00/Kconfig b/drivers/net/wireless/rt2x00/Kconfig
index 76cd47eb901e..9b915d3a44be 100644
--- a/drivers/net/wireless/rt2x00/Kconfig
+++ b/drivers/net/wireless/rt2x00/Kconfig
@@ -173,6 +173,13 @@ config RT2800USB_RT53XX
          rt2800usb driver.
          Supported chips: RT5370
 
+config RT2800USB_RT55XX
+       bool "rt2800usb - Include support for rt55xx devices (EXPERIMENTAL)"
+       ---help---
+         This adds support for rt55xx wireless chipset family to the
+         rt2800usb driver.
+         Supported chips: RT5572
+
 config RT2800USB_UNKNOWN
 	bool "rt2800usb - Include support for unknown (USB) devices"
 	default n
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
index dcfb54e0c516..f7143733d7e9 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/rt2x00/rt2400pci.c
@@ -41,7 +41,7 @@
 /*
  * Register access.
  * All access to the CSR registers will go through the methods
- * rt2x00pci_register_read and rt2x00pci_register_write.
+ * rt2x00mmio_register_read and rt2x00mmio_register_write.
  * BBP and RF register require indirect register access,
  * and use the CSR registers BBPCSR and RFCSR to achieve this.
  * These indirect registers work with busy bits,
@@ -52,9 +52,9 @@
  * and we will print an error.
  */
 #define WAIT_FOR_BBP(__dev, __reg) \
-	rt2x00pci_regbusy_read((__dev), BBPCSR, BBPCSR_BUSY, (__reg))
+	rt2x00mmio_regbusy_read((__dev), BBPCSR, BBPCSR_BUSY, (__reg))
 #define WAIT_FOR_RF(__dev, __reg) \
-	rt2x00pci_regbusy_read((__dev), RFCSR, RFCSR_BUSY, (__reg))
+	rt2x00mmio_regbusy_read((__dev), RFCSR, RFCSR_BUSY, (__reg))
 
 static void rt2400pci_bbp_write(struct rt2x00_dev *rt2x00dev,
 				const unsigned int word, const u8 value)
@@ -74,7 +74,7 @@ static void rt2400pci_bbp_write(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
 		rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 1);
 
-		rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, BBPCSR, reg);
 	}
 
 	mutex_unlock(&rt2x00dev->csr_mutex);
@@ -101,7 +101,7 @@ static void rt2400pci_bbp_read(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
 		rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 0);
 
-		rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, BBPCSR, reg);
 
 		WAIT_FOR_BBP(rt2x00dev, &reg);
 	}
@@ -129,7 +129,7 @@ static void rt2400pci_rf_write(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, RFCSR_IF_SELECT, 0);
 		rt2x00_set_field32(&reg, RFCSR_BUSY, 1);
 
-		rt2x00pci_register_write(rt2x00dev, RFCSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, RFCSR, reg);
 		rt2x00_rf_write(rt2x00dev, word, value);
 	}
 
@@ -141,7 +141,7 @@ static void rt2400pci_eepromregister_read(struct eeprom_93cx6 *eeprom)
 	struct rt2x00_dev *rt2x00dev = eeprom->data;
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, CSR21, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR21, &reg);
 
 	eeprom->reg_data_in = !!rt2x00_get_field32(reg, CSR21_EEPROM_DATA_IN);
 	eeprom->reg_data_out = !!rt2x00_get_field32(reg, CSR21_EEPROM_DATA_OUT);
@@ -163,15 +163,15 @@ static void rt2400pci_eepromregister_write(struct eeprom_93cx6 *eeprom)
 	rt2x00_set_field32(&reg, CSR21_EEPROM_CHIP_SELECT,
 			   !!eeprom->reg_chip_select);
 
-	rt2x00pci_register_write(rt2x00dev, CSR21, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR21, reg);
 }
 
 #ifdef CONFIG_RT2X00_LIB_DEBUGFS
 static const struct rt2x00debug rt2400pci_rt2x00debug = {
 	.owner	= THIS_MODULE,
 	.csr	= {
-		.read		= rt2x00pci_register_read,
-		.write		= rt2x00pci_register_write,
+		.read		= rt2x00mmio_register_read,
+		.write		= rt2x00mmio_register_write,
 		.flags		= RT2X00DEBUGFS_OFFSET,
 		.word_base	= CSR_REG_BASE,
 		.word_size	= sizeof(u32),
@@ -205,7 +205,7 @@ static int rt2400pci_rfkill_poll(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, GPIOCSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, GPIOCSR, &reg);
 	return rt2x00_get_field32(reg, GPIOCSR_VAL0);
 }
 
@@ -218,14 +218,14 @@ static void rt2400pci_brightness_set(struct led_classdev *led_cdev,
 	unsigned int enabled = brightness != LED_OFF;
 	u32 reg;
 
-	rt2x00pci_register_read(led->rt2x00dev, LEDCSR, &reg);
+	rt2x00mmio_register_read(led->rt2x00dev, LEDCSR, &reg);
 
 	if (led->type == LED_TYPE_RADIO || led->type == LED_TYPE_ASSOC)
 		rt2x00_set_field32(&reg, LEDCSR_LINK, enabled);
 	else if (led->type == LED_TYPE_ACTIVITY)
 		rt2x00_set_field32(&reg, LEDCSR_ACTIVITY, enabled);
 
-	rt2x00pci_register_write(led->rt2x00dev, LEDCSR, reg);
+	rt2x00mmio_register_write(led->rt2x00dev, LEDCSR, reg);
 }
 
 static int rt2400pci_blink_set(struct led_classdev *led_cdev,
@@ -236,10 +236,10 @@ static int rt2400pci_blink_set(struct led_classdev *led_cdev,
 	    container_of(led_cdev, struct rt2x00_led, led_dev);
 	u32 reg;
 
-	rt2x00pci_register_read(led->rt2x00dev, LEDCSR, &reg);
+	rt2x00mmio_register_read(led->rt2x00dev, LEDCSR, &reg);
 	rt2x00_set_field32(&reg, LEDCSR_ON_PERIOD, *delay_on);
 	rt2x00_set_field32(&reg, LEDCSR_OFF_PERIOD, *delay_off);
-	rt2x00pci_register_write(led->rt2x00dev, LEDCSR, reg);
+	rt2x00mmio_register_write(led->rt2x00dev, LEDCSR, reg);
 
 	return 0;
 }
@@ -269,7 +269,7 @@ static void rt2400pci_config_filter(struct rt2x00_dev *rt2x00dev,
 	 * Note that the version error will always be dropped
 	 * since there is no filter for it at this time.
 	 */
-	rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, RXCSR0, &reg);
 	rt2x00_set_field32(&reg, RXCSR0_DROP_CRC,
 			   !(filter_flags & FIF_FCSFAIL));
 	rt2x00_set_field32(&reg, RXCSR0_DROP_PHYSICAL,
@@ -282,7 +282,7 @@ static void rt2400pci_config_filter(struct rt2x00_dev *rt2x00dev,
 			   !(filter_flags & FIF_PROMISC_IN_BSS) &&
 			   !rt2x00dev->intf_ap_count);
 	rt2x00_set_field32(&reg, RXCSR0_DROP_VERSION_ERROR, 1);
-	rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
+	rt2x00mmio_register_write(rt2x00dev, RXCSR0, reg);
 }
 
 static void rt2400pci_config_intf(struct rt2x00_dev *rt2x00dev,
@@ -298,25 +298,26 @@ static void rt2400pci_config_intf(struct rt2x00_dev *rt2x00dev,
 		 * Enable beacon config
 		 */
 		bcn_preload = PREAMBLE + GET_DURATION(IEEE80211_HEADER, 20);
-		rt2x00pci_register_read(rt2x00dev, BCNCSR1, &reg);
+		rt2x00mmio_register_read(rt2x00dev, BCNCSR1, &reg);
 		rt2x00_set_field32(&reg, BCNCSR1_PRELOAD, bcn_preload);
-		rt2x00pci_register_write(rt2x00dev, BCNCSR1, reg);
+		rt2x00mmio_register_write(rt2x00dev, BCNCSR1, reg);
 
 		/*
 		 * Enable synchronisation.
 		 */
-		rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
 		rt2x00_set_field32(&reg, CSR14_TSF_SYNC, conf->sync);
-		rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 	}
 
 	if (flags & CONFIG_UPDATE_MAC)
-		rt2x00pci_register_multiwrite(rt2x00dev, CSR3,
-					      conf->mac, sizeof(conf->mac));
+		rt2x00mmio_register_multiwrite(rt2x00dev, CSR3,
+					       conf->mac, sizeof(conf->mac));
 
 	if (flags & CONFIG_UPDATE_BSSID)
-		rt2x00pci_register_multiwrite(rt2x00dev, CSR5,
-					      conf->bssid, sizeof(conf->bssid));
+		rt2x00mmio_register_multiwrite(rt2x00dev, CSR5,
+					       conf->bssid,
+					       sizeof(conf->bssid));
 }
 
 static void rt2400pci_config_erp(struct rt2x00_dev *rt2x00dev,
@@ -332,68 +333,68 @@ static void rt2400pci_config_erp(struct rt2x00_dev *rt2x00dev,
 	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
 		preamble_mask = erp->short_preamble << 3;
 
-		rt2x00pci_register_read(rt2x00dev, TXCSR1, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXCSR1, &reg);
 		rt2x00_set_field32(&reg, TXCSR1_ACK_TIMEOUT, 0x1ff);
 		rt2x00_set_field32(&reg, TXCSR1_ACK_CONSUME_TIME, 0x13a);
 		rt2x00_set_field32(&reg, TXCSR1_TSF_OFFSET, IEEE80211_HEADER);
 		rt2x00_set_field32(&reg, TXCSR1_AUTORESPONDER, 1);
-		rt2x00pci_register_write(rt2x00dev, TXCSR1, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXCSR1, reg);
 
-		rt2x00pci_register_read(rt2x00dev, ARCSR2, &reg);
+		rt2x00mmio_register_read(rt2x00dev, ARCSR2, &reg);
 		rt2x00_set_field32(&reg, ARCSR2_SIGNAL, 0x00);
 		rt2x00_set_field32(&reg, ARCSR2_SERVICE, 0x04);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
 				   GET_DURATION(ACK_SIZE, 10));
-		rt2x00pci_register_write(rt2x00dev, ARCSR2, reg);
+		rt2x00mmio_register_write(rt2x00dev, ARCSR2, reg);
 
-		rt2x00pci_register_read(rt2x00dev, ARCSR3, &reg);
+		rt2x00mmio_register_read(rt2x00dev, ARCSR3, &reg);
 		rt2x00_set_field32(&reg, ARCSR3_SIGNAL, 0x01 | preamble_mask);
 		rt2x00_set_field32(&reg, ARCSR3_SERVICE, 0x04);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
 				   GET_DURATION(ACK_SIZE, 20));
-		rt2x00pci_register_write(rt2x00dev, ARCSR3, reg);
+		rt2x00mmio_register_write(rt2x00dev, ARCSR3, reg);
 
-		rt2x00pci_register_read(rt2x00dev, ARCSR4, &reg);
+		rt2x00mmio_register_read(rt2x00dev, ARCSR4, &reg);
 		rt2x00_set_field32(&reg, ARCSR4_SIGNAL, 0x02 | preamble_mask);
 		rt2x00_set_field32(&reg, ARCSR4_SERVICE, 0x04);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
 				   GET_DURATION(ACK_SIZE, 55));
-		rt2x00pci_register_write(rt2x00dev, ARCSR4, reg);
+		rt2x00mmio_register_write(rt2x00dev, ARCSR4, reg);
 
-		rt2x00pci_register_read(rt2x00dev, ARCSR5, &reg);
+		rt2x00mmio_register_read(rt2x00dev, ARCSR5, &reg);
 		rt2x00_set_field32(&reg, ARCSR5_SIGNAL, 0x03 | preamble_mask);
 		rt2x00_set_field32(&reg, ARCSR5_SERVICE, 0x84);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
 				   GET_DURATION(ACK_SIZE, 110));
-		rt2x00pci_register_write(rt2x00dev, ARCSR5, reg);
+		rt2x00mmio_register_write(rt2x00dev, ARCSR5, reg);
 	}
 
 	if (changed & BSS_CHANGED_BASIC_RATES)
-		rt2x00pci_register_write(rt2x00dev, ARCSR1, erp->basic_rates);
+		rt2x00mmio_register_write(rt2x00dev, ARCSR1, erp->basic_rates);
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
-		rt2x00pci_register_read(rt2x00dev, CSR11, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR11, &reg);
 		rt2x00_set_field32(&reg, CSR11_SLOT_TIME, erp->slot_time);
-		rt2x00pci_register_write(rt2x00dev, CSR11, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR11, reg);
 
-		rt2x00pci_register_read(rt2x00dev, CSR18, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR18, &reg);
 		rt2x00_set_field32(&reg, CSR18_SIFS, erp->sifs);
 		rt2x00_set_field32(&reg, CSR18_PIFS, erp->pifs);
-		rt2x00pci_register_write(rt2x00dev, CSR18, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR18, reg);
 
-		rt2x00pci_register_read(rt2x00dev, CSR19, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR19, &reg);
 		rt2x00_set_field32(&reg, CSR19_DIFS, erp->difs);
 		rt2x00_set_field32(&reg, CSR19_EIFS, erp->eifs);
-		rt2x00pci_register_write(rt2x00dev, CSR19, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR19, reg);
 	}
 
 	if (changed & BSS_CHANGED_BEACON_INT) {
-		rt2x00pci_register_read(rt2x00dev, CSR12, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR12, &reg);
 		rt2x00_set_field32(&reg, CSR12_BEACON_INTERVAL,
 				   erp->beacon_int * 16);
 		rt2x00_set_field32(&reg, CSR12_CFP_MAX_DURATION,
 				   erp->beacon_int * 16);
-		rt2x00pci_register_write(rt2x00dev, CSR12, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR12, reg);
 	}
 }
 
@@ -497,7 +498,7 @@ static void rt2400pci_config_channel(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Clear false CRC during channel switch.
 	 */
-	rt2x00pci_register_read(rt2x00dev, CNT0, &rf->rf1);
+	rt2x00mmio_register_read(rt2x00dev, CNT0, &rf->rf1);
 }
 
 static void rt2400pci_config_txpower(struct rt2x00_dev *rt2x00dev, int txpower)
@@ -510,12 +511,12 @@ static void rt2400pci_config_retry_limit(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, CSR11, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR11, &reg);
 	rt2x00_set_field32(&reg, CSR11_LONG_RETRY,
 			   libconf->conf->long_frame_max_tx_count);
 	rt2x00_set_field32(&reg, CSR11_SHORT_RETRY,
 			   libconf->conf->short_frame_max_tx_count);
-	rt2x00pci_register_write(rt2x00dev, CSR11, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR11, reg);
 }
 
 static void rt2400pci_config_ps(struct rt2x00_dev *rt2x00dev,
@@ -527,7 +528,7 @@ static void rt2400pci_config_ps(struct rt2x00_dev *rt2x00dev,
 	u32 reg;
 
 	if (state == STATE_SLEEP) {
-		rt2x00pci_register_read(rt2x00dev, CSR20, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR20, &reg);
 		rt2x00_set_field32(&reg, CSR20_DELAY_AFTER_TBCN,
 				   (rt2x00dev->beacon_int - 20) * 16);
 		rt2x00_set_field32(&reg, CSR20_TBCN_BEFORE_WAKEUP,
@@ -535,14 +536,14 @@ static void rt2400pci_config_ps(struct rt2x00_dev *rt2x00dev,
 
 		/* We must first disable autowake before it can be enabled */
 		rt2x00_set_field32(&reg, CSR20_AUTOWAKE, 0);
-		rt2x00pci_register_write(rt2x00dev, CSR20, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR20, reg);
 
 		rt2x00_set_field32(&reg, CSR20_AUTOWAKE, 1);
-		rt2x00pci_register_write(rt2x00dev, CSR20, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR20, reg);
 	} else {
-		rt2x00pci_register_read(rt2x00dev, CSR20, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR20, &reg);
 		rt2x00_set_field32(&reg, CSR20_AUTOWAKE, 0);
-		rt2x00pci_register_write(rt2x00dev, CSR20, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR20, reg);
 	}
 
 	rt2x00dev->ops->lib->set_device_state(rt2x00dev, state);
@@ -568,10 +569,10 @@ static void rt2400pci_config_cw(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, CSR11, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR11, &reg);
 	rt2x00_set_field32(&reg, CSR11_CWMIN, cw_min);
 	rt2x00_set_field32(&reg, CSR11_CWMAX, cw_max);
-	rt2x00pci_register_write(rt2x00dev, CSR11, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR11, reg);
 }
 
 /*
@@ -586,7 +587,7 @@ static void rt2400pci_link_stats(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Update FCS error count from register.
 	 */
-	rt2x00pci_register_read(rt2x00dev, CNT0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CNT0, &reg);
 	qual->rx_failed = rt2x00_get_field32(reg, CNT0_FCS_ERROR);
 
 	/*
@@ -641,16 +642,16 @@ static void rt2400pci_start_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, RXCSR0, &reg);
 		rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX, 0);
-		rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
+		rt2x00mmio_register_write(rt2x00dev, RXCSR0, reg);
 		break;
 	case QID_BEACON:
-		rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
 		rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 1);
 		rt2x00_set_field32(&reg, CSR14_TBCN, 1);
 		rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 1);
-		rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 		break;
 	default:
 		break;
@@ -664,19 +665,19 @@ static void rt2400pci_kick_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_AC_VO:
-		rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
 		rt2x00_set_field32(&reg, TXCSR0_KICK_PRIO, 1);
-		rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
 	case QID_AC_VI:
-		rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
 		rt2x00_set_field32(&reg, TXCSR0_KICK_TX, 1);
-		rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
 	case QID_ATIM:
-		rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
 		rt2x00_set_field32(&reg, TXCSR0_KICK_ATIM, 1);
-		rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
 	default:
 		break;
@@ -692,21 +693,21 @@ static void rt2400pci_stop_queue(struct data_queue *queue)
 	case QID_AC_VO:
 	case QID_AC_VI:
 	case QID_ATIM:
-		rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
 		rt2x00_set_field32(&reg, TXCSR0_ABORT, 1);
-		rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
 	case QID_RX:
-		rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, RXCSR0, &reg);
 		rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX, 1);
-		rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
+		rt2x00mmio_register_write(rt2x00dev, RXCSR0, reg);
 		break;
 	case QID_BEACON:
-		rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
 		rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 0);
 		rt2x00_set_field32(&reg, CSR14_TBCN, 0);
 		rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
-		rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 
 		/*
 		 * Wait for possibly running tbtt tasklets.
@@ -723,7 +724,7 @@ static void rt2400pci_stop_queue(struct data_queue *queue)
  */
 static bool rt2400pci_get_entry_state(struct queue_entry *entry)
 {
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	u32 word;
 
 	if (entry->queue->qid == QID_RX) {
@@ -740,7 +741,7 @@ static bool rt2400pci_get_entry_state(struct queue_entry *entry)
 
 static void rt2400pci_clear_entry(struct queue_entry *entry)
 {
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
 	u32 word;
 
@@ -766,53 +767,53 @@ static void rt2400pci_clear_entry(struct queue_entry *entry)
 
 static int rt2400pci_init_queues(struct rt2x00_dev *rt2x00dev)
 {
-	struct queue_entry_priv_pci *entry_priv;
+	struct queue_entry_priv_mmio *entry_priv;
 	u32 reg;
 
 	/*
 	 * Initialize registers.
 	 */
-	rt2x00pci_register_read(rt2x00dev, TXCSR2, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXCSR2, &reg);
 	rt2x00_set_field32(&reg, TXCSR2_TXD_SIZE, rt2x00dev->tx[0].desc_size);
 	rt2x00_set_field32(&reg, TXCSR2_NUM_TXD, rt2x00dev->tx[1].limit);
 	rt2x00_set_field32(&reg, TXCSR2_NUM_ATIM, rt2x00dev->atim->limit);
 	rt2x00_set_field32(&reg, TXCSR2_NUM_PRIO, rt2x00dev->tx[0].limit);
-	rt2x00pci_register_write(rt2x00dev, TXCSR2, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXCSR2, reg);
 
 	entry_priv = rt2x00dev->tx[1].entries[0].priv_data;
-	rt2x00pci_register_read(rt2x00dev, TXCSR3, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXCSR3, &reg);
 	rt2x00_set_field32(&reg, TXCSR3_TX_RING_REGISTER,
 			   entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, TXCSR3, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXCSR3, reg);
 
 	entry_priv = rt2x00dev->tx[0].entries[0].priv_data;
-	rt2x00pci_register_read(rt2x00dev, TXCSR5, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXCSR5, &reg);
 	rt2x00_set_field32(&reg, TXCSR5_PRIO_RING_REGISTER,
 			   entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, TXCSR5, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXCSR5, reg);
 
 	entry_priv = rt2x00dev->atim->entries[0].priv_data;
-	rt2x00pci_register_read(rt2x00dev, TXCSR4, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXCSR4, &reg);
 	rt2x00_set_field32(&reg, TXCSR4_ATIM_RING_REGISTER,
 			   entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, TXCSR4, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXCSR4, reg);
 
 	entry_priv = rt2x00dev->bcn->entries[0].priv_data;
-	rt2x00pci_register_read(rt2x00dev, TXCSR6, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXCSR6, &reg);
 	rt2x00_set_field32(&reg, TXCSR6_BEACON_RING_REGISTER,
 			   entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, TXCSR6, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXCSR6, reg);
 
-	rt2x00pci_register_read(rt2x00dev, RXCSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, RXCSR1, &reg);
 	rt2x00_set_field32(&reg, RXCSR1_RXD_SIZE, rt2x00dev->rx->desc_size);
 	rt2x00_set_field32(&reg, RXCSR1_NUM_RXD, rt2x00dev->rx->limit);
-	rt2x00pci_register_write(rt2x00dev, RXCSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, RXCSR1, reg);
 
 	entry_priv = rt2x00dev->rx->entries[0].priv_data;
-	rt2x00pci_register_read(rt2x00dev, RXCSR2, &reg);
+	rt2x00mmio_register_read(rt2x00dev, RXCSR2, &reg);
 	rt2x00_set_field32(&reg, RXCSR2_RX_RING_REGISTER,
 			   entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, RXCSR2, reg);
+	rt2x00mmio_register_write(rt2x00dev, RXCSR2, reg);
 
 	return 0;
 }
@@ -821,23 +822,23 @@ static int rt2400pci_init_registers(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2x00pci_register_write(rt2x00dev, PSCSR0, 0x00020002);
-	rt2x00pci_register_write(rt2x00dev, PSCSR1, 0x00000002);
-	rt2x00pci_register_write(rt2x00dev, PSCSR2, 0x00023f20);
-	rt2x00pci_register_write(rt2x00dev, PSCSR3, 0x00000002);
+	rt2x00mmio_register_write(rt2x00dev, PSCSR0, 0x00020002);
+	rt2x00mmio_register_write(rt2x00dev, PSCSR1, 0x00000002);
+	rt2x00mmio_register_write(rt2x00dev, PSCSR2, 0x00023f20);
+	rt2x00mmio_register_write(rt2x00dev, PSCSR3, 0x00000002);
 
-	rt2x00pci_register_read(rt2x00dev, TIMECSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TIMECSR, &reg);
 	rt2x00_set_field32(&reg, TIMECSR_US_COUNT, 33);
 	rt2x00_set_field32(&reg, TIMECSR_US_64_COUNT, 63);
 	rt2x00_set_field32(&reg, TIMECSR_BEACON_EXPECT, 0);
-	rt2x00pci_register_write(rt2x00dev, TIMECSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, TIMECSR, reg);
 
-	rt2x00pci_register_read(rt2x00dev, CSR9, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR9, &reg);
 	rt2x00_set_field32(&reg, CSR9_MAX_FRAME_UNIT,
 			   (rt2x00dev->rx->data_size / 128));
-	rt2x00pci_register_write(rt2x00dev, CSR9, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR9, reg);
 
-	rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
 	rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 0);
 	rt2x00_set_field32(&reg, CSR14_TSF_SYNC, 0);
 	rt2x00_set_field32(&reg, CSR14_TBCN, 0);
@@ -846,63 +847,63 @@ static int rt2400pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
 	rt2x00_set_field32(&reg, CSR14_CFP_COUNT_PRELOAD, 0);
 	rt2x00_set_field32(&reg, CSR14_TBCM_PRELOAD, 0);
-	rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 
-	rt2x00pci_register_write(rt2x00dev, CNT3, 0x3f080000);
+	rt2x00mmio_register_write(rt2x00dev, CNT3, 0x3f080000);
 
-	rt2x00pci_register_read(rt2x00dev, ARCSR0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, ARCSR0, &reg);
 	rt2x00_set_field32(&reg, ARCSR0_AR_BBP_DATA0, 133);
 	rt2x00_set_field32(&reg, ARCSR0_AR_BBP_ID0, 134);
 	rt2x00_set_field32(&reg, ARCSR0_AR_BBP_DATA1, 136);
 	rt2x00_set_field32(&reg, ARCSR0_AR_BBP_ID1, 135);
-	rt2x00pci_register_write(rt2x00dev, ARCSR0, reg);
+	rt2x00mmio_register_write(rt2x00dev, ARCSR0, reg);
 
-	rt2x00pci_register_read(rt2x00dev, RXCSR3, &reg);
+	rt2x00mmio_register_read(rt2x00dev, RXCSR3, &reg);
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID0, 3); /* Tx power.*/
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID0_VALID, 1);
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID1, 32); /* Signal */
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID1_VALID, 1);
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID2, 36); /* Rssi */
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID2_VALID, 1);
-	rt2x00pci_register_write(rt2x00dev, RXCSR3, reg);
+	rt2x00mmio_register_write(rt2x00dev, RXCSR3, reg);
 
-	rt2x00pci_register_write(rt2x00dev, PWRCSR0, 0x3f3b3100);
+	rt2x00mmio_register_write(rt2x00dev, PWRCSR0, 0x3f3b3100);
 
 	if (rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_AWAKE))
 		return -EBUSY;
 
-	rt2x00pci_register_write(rt2x00dev, MACCSR0, 0x00217223);
-	rt2x00pci_register_write(rt2x00dev, MACCSR1, 0x00235518);
+	rt2x00mmio_register_write(rt2x00dev, MACCSR0, 0x00217223);
+	rt2x00mmio_register_write(rt2x00dev, MACCSR1, 0x00235518);
 
-	rt2x00pci_register_read(rt2x00dev, MACCSR2, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MACCSR2, &reg);
 	rt2x00_set_field32(&reg, MACCSR2_DELAY, 64);
-	rt2x00pci_register_write(rt2x00dev, MACCSR2, reg);
+	rt2x00mmio_register_write(rt2x00dev, MACCSR2, reg);
 
-	rt2x00pci_register_read(rt2x00dev, RALINKCSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, RALINKCSR, &reg);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_DATA0, 17);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_ID0, 154);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_DATA1, 0);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_ID1, 154);
-	rt2x00pci_register_write(rt2x00dev, RALINKCSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, RALINKCSR, reg);
 
-	rt2x00pci_register_read(rt2x00dev, CSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR1, &reg);
 	rt2x00_set_field32(&reg, CSR1_SOFT_RESET, 1);
 	rt2x00_set_field32(&reg, CSR1_BBP_RESET, 0);
 	rt2x00_set_field32(&reg, CSR1_HOST_READY, 0);
-	rt2x00pci_register_write(rt2x00dev, CSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR1, reg);
 
-	rt2x00pci_register_read(rt2x00dev, CSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR1, &reg);
 	rt2x00_set_field32(&reg, CSR1_SOFT_RESET, 0);
 	rt2x00_set_field32(&reg, CSR1_HOST_READY, 1);
-	rt2x00pci_register_write(rt2x00dev, CSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR1, reg);
 
 	/*
 	 * We must clear the FCS and FIFO error count.
 	 * These registers are cleared on read,
 	 * so we may pass a useless variable to store the value.
 	 */
-	rt2x00pci_register_read(rt2x00dev, CNT0, &reg);
-	rt2x00pci_register_read(rt2x00dev, CNT4, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CNT0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CNT4, &reg);
 
 	return 0;
 }
@@ -919,7 +920,7 @@ static int rt2400pci_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 		udelay(REGISTER_BUSY_DELAY);
 	}
 
-	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
+	rt2x00_err(rt2x00dev, "BBP register access failed, aborting\n");
 	return -EACCES;
 }
 
@@ -976,8 +977,8 @@ static void rt2400pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	 * should clear the register to assure a clean state.
 	 */
 	if (state == STATE_RADIO_IRQ_ON) {
-		rt2x00pci_register_read(rt2x00dev, CSR7, &reg);
-		rt2x00pci_register_write(rt2x00dev, CSR7, reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR7, &reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR7, reg);
 	}
 
 	/*
@@ -986,13 +987,13 @@ static void rt2400pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	 */
 	spin_lock_irqsave(&rt2x00dev->irqmask_lock, flags);
 
-	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
 	rt2x00_set_field32(&reg, CSR8_TBCN_EXPIRE, mask);
 	rt2x00_set_field32(&reg, CSR8_TXDONE_TXRING, mask);
 	rt2x00_set_field32(&reg, CSR8_TXDONE_ATIMRING, mask);
 	rt2x00_set_field32(&reg, CSR8_TXDONE_PRIORING, mask);
 	rt2x00_set_field32(&reg, CSR8_RXDONE, mask);
-	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR8, reg);
 
 	spin_unlock_irqrestore(&rt2x00dev->irqmask_lock, flags);
 
@@ -1025,7 +1026,7 @@ static void rt2400pci_disable_radio(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Disable power
 	 */
-	rt2x00pci_register_write(rt2x00dev, PWRCSR0, 0);
+	rt2x00mmio_register_write(rt2x00dev, PWRCSR0, 0);
 }
 
 static int rt2400pci_set_state(struct rt2x00_dev *rt2x00dev,
@@ -1039,12 +1040,12 @@ static int rt2400pci_set_state(struct rt2x00_dev *rt2x00dev,
 
 	put_to_sleep = (state != STATE_AWAKE);
 
-	rt2x00pci_register_read(rt2x00dev, PWRCSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, PWRCSR1, &reg);
 	rt2x00_set_field32(&reg, PWRCSR1_SET_STATE, 1);
 	rt2x00_set_field32(&reg, PWRCSR1_BBP_DESIRE_STATE, state);
 	rt2x00_set_field32(&reg, PWRCSR1_RF_DESIRE_STATE, state);
 	rt2x00_set_field32(&reg, PWRCSR1_PUT_TO_SLEEP, put_to_sleep);
-	rt2x00pci_register_write(rt2x00dev, PWRCSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, PWRCSR1, reg);
 
 	/*
 	 * Device is not guaranteed to be in the requested state yet.
@@ -1052,12 +1053,12 @@ static int rt2400pci_set_state(struct rt2x00_dev *rt2x00dev,
 	 * device has entered the correct state.
 	 */
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2x00pci_register_read(rt2x00dev, PWRCSR1, &reg2);
+		rt2x00mmio_register_read(rt2x00dev, PWRCSR1, &reg2);
 		bbp_state = rt2x00_get_field32(reg2, PWRCSR1_BBP_CURR_STATE);
 		rf_state = rt2x00_get_field32(reg2, PWRCSR1_RF_CURR_STATE);
 		if (bbp_state == state && rf_state == state)
 			return 0;
-		rt2x00pci_register_write(rt2x00dev, PWRCSR1, reg);
+		rt2x00mmio_register_write(rt2x00dev, PWRCSR1, reg);
 		msleep(10);
 	}
 
@@ -1092,8 +1093,8 @@ static int rt2400pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 	}
 
 	if (unlikely(retval))
-		ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
-		      state, retval);
+		rt2x00_err(rt2x00dev, "Device failed to enter state %d (%d)\n",
+			   state, retval);
 
 	return retval;
 }
@@ -1105,7 +1106,7 @@ static void rt2400pci_write_tx_desc(struct queue_entry *entry,
 				    struct txentry_desc *txdesc)
 {
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	__le32 *txd = entry_priv->desc;
 	u32 word;
 
@@ -1182,12 +1183,12 @@ static void rt2400pci_write_beacon(struct queue_entry *entry,
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
-	rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
 	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
-	rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 
 	if (rt2x00queue_map_txskb(entry)) {
-		ERROR(rt2x00dev, "Fail to map beacon, aborting\n");
+		rt2x00_err(rt2x00dev, "Fail to map beacon, aborting\n");
 		goto out;
 	}
 	/*
@@ -1208,7 +1209,7 @@ out:
 	 * Enable beaconing again.
 	 */
 	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 1);
-	rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 }
 
 /*
@@ -1218,7 +1219,7 @@ static void rt2400pci_fill_rxdone(struct queue_entry *entry,
 				  struct rxdone_entry_desc *rxdesc)
 {
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	u32 word0;
 	u32 word2;
 	u32 word3;
@@ -1276,7 +1277,7 @@ static void rt2400pci_txdone(struct rt2x00_dev *rt2x00dev,
 			     const enum data_queue_qid queue_idx)
 {
 	struct data_queue *queue = rt2x00queue_get_tx_queue(rt2x00dev, queue_idx);
-	struct queue_entry_priv_pci *entry_priv;
+	struct queue_entry_priv_mmio *entry_priv;
 	struct queue_entry *entry;
 	struct txdone_entry_desc txdesc;
 	u32 word;
@@ -1322,9 +1323,9 @@ static inline void rt2400pci_enable_interrupt(struct rt2x00_dev *rt2x00dev,
 	 */
 	spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
 	rt2x00_set_field32(&reg, irq_field, 0);
-	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR8, reg);
 
 	spin_unlock_irq(&rt2x00dev->irqmask_lock);
 }
@@ -1347,11 +1348,11 @@ static void rt2400pci_txstatus_tasklet(unsigned long data)
 	if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags)) {
 		spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-		rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
 		rt2x00_set_field32(&reg, CSR8_TXDONE_TXRING, 0);
 		rt2x00_set_field32(&reg, CSR8_TXDONE_ATIMRING, 0);
 		rt2x00_set_field32(&reg, CSR8_TXDONE_PRIORING, 0);
-		rt2x00pci_register_write(rt2x00dev, CSR8, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR8, reg);
 
 		spin_unlock_irq(&rt2x00dev->irqmask_lock);
 	}
@@ -1368,7 +1369,7 @@ static void rt2400pci_tbtt_tasklet(unsigned long data)
 static void rt2400pci_rxdone_tasklet(unsigned long data)
 {
 	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
-	if (rt2x00pci_rxdone(rt2x00dev))
+	if (rt2x00mmio_rxdone(rt2x00dev))
 		tasklet_schedule(&rt2x00dev->rxdone_tasklet);
 	else if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
 		rt2400pci_enable_interrupt(rt2x00dev, CSR8_RXDONE);
@@ -1383,8 +1384,8 @@ static irqreturn_t rt2400pci_interrupt(int irq, void *dev_instance)
 	 * Get the interrupt sources & saved to local variable.
 	 * Write register value back to clear pending interrupts.
 	 */
-	rt2x00pci_register_read(rt2x00dev, CSR7, &reg);
-	rt2x00pci_register_write(rt2x00dev, CSR7, reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR7, &reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR7, reg);
 
 	if (!reg)
 		return IRQ_NONE;
@@ -1421,9 +1422,9 @@ static irqreturn_t rt2400pci_interrupt(int irq, void *dev_instance)
 	 */
 	spin_lock(&rt2x00dev->irqmask_lock);
 
-	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
 	reg |= mask;
-	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR8, reg);
 
 	spin_unlock(&rt2x00dev->irqmask_lock);
 
@@ -1442,7 +1443,7 @@ static int rt2400pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	u16 word;
 	u8 *mac;
 
-	rt2x00pci_register_read(rt2x00dev, CSR21, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR21, &reg);
 
 	eeprom.data = rt2x00dev;
 	eeprom.register_read = rt2400pci_eepromregister_read;
@@ -1463,12 +1464,12 @@ static int rt2400pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	if (!is_valid_ether_addr(mac)) {
 		eth_random_addr(mac);
-		EEPROM(rt2x00dev, "MAC: %pM\n", mac);
+		rt2x00_eeprom_dbg(rt2x00dev, "MAC: %pM\n", mac);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &word);
 	if (word == 0xffff) {
-		ERROR(rt2x00dev, "Invalid EEPROM data detected.\n");
+		rt2x00_err(rt2x00dev, "Invalid EEPROM data detected\n");
 		return -EINVAL;
 	}
 
@@ -1490,12 +1491,12 @@ static int rt2400pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	 * Identify RF chipset.
 	 */
 	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RF_TYPE);
-	rt2x00pci_register_read(rt2x00dev, CSR0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR0, &reg);
 	rt2x00_set_chip(rt2x00dev, RT2460, value,
 			rt2x00_get_field32(reg, CSR0_REVISION));
 
 	if (!rt2x00_rf(rt2x00dev, RF2420) && !rt2x00_rf(rt2x00dev, RF2421)) {
-		ERROR(rt2x00dev, "Invalid RF chipset detected.\n");
+		rt2x00_err(rt2x00dev, "Invalid RF chipset detected\n");
 		return -ENODEV;
 	}
 
@@ -1635,9 +1636,9 @@ static int rt2400pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 	 * Enable rfkill polling by setting GPIO direction of the
 	 * rfkill switch GPIO pin correctly.
 	 */
-	rt2x00pci_register_read(rt2x00dev, GPIOCSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, GPIOCSR, &reg);
 	rt2x00_set_field32(&reg, GPIOCSR_DIR0, 1);
-	rt2x00pci_register_write(rt2x00dev, GPIOCSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, GPIOCSR, reg);
 
 	/*
 	 * Initialize hw specifications.
@@ -1697,9 +1698,9 @@ static u64 rt2400pci_get_tsf(struct ieee80211_hw *hw,
 	u64 tsf;
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, CSR17, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR17, &reg);
 	tsf = (u64) rt2x00_get_field32(reg, CSR17_HIGH_TSFTIMER) << 32;
-	rt2x00pci_register_read(rt2x00dev, CSR16, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR16, &reg);
 	tsf |= rt2x00_get_field32(reg, CSR16_LOW_TSFTIMER);
 
 	return tsf;
@@ -1710,7 +1711,7 @@ static int rt2400pci_tx_last_beacon(struct ieee80211_hw *hw)
 	struct rt2x00_dev *rt2x00dev = hw->priv;
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, CSR15, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR15, &reg);
 	return rt2x00_get_field32(reg, CSR15_BEACON_SENT);
 }
 
@@ -1743,8 +1744,8 @@ static const struct rt2x00lib_ops rt2400pci_rt2x00_ops = {
 	.tbtt_tasklet		= rt2400pci_tbtt_tasklet,
 	.rxdone_tasklet		= rt2400pci_rxdone_tasklet,
 	.probe_hw		= rt2400pci_probe_hw,
-	.initialize		= rt2x00pci_initialize,
-	.uninitialize		= rt2x00pci_uninitialize,
+	.initialize		= rt2x00mmio_initialize,
+	.uninitialize		= rt2x00mmio_uninitialize,
 	.get_entry_state	= rt2400pci_get_entry_state,
 	.clear_entry		= rt2400pci_clear_entry,
 	.set_device_state	= rt2400pci_set_device_state,
@@ -1755,7 +1756,7 @@ static const struct rt2x00lib_ops rt2400pci_rt2x00_ops = {
 	.start_queue		= rt2400pci_start_queue,
 	.kick_queue		= rt2400pci_kick_queue,
 	.stop_queue		= rt2400pci_stop_queue,
-	.flush_queue		= rt2x00pci_flush_queue,
+	.flush_queue		= rt2x00mmio_flush_queue,
 	.write_tx_desc		= rt2400pci_write_tx_desc,
 	.write_beacon		= rt2400pci_write_beacon,
 	.fill_rxdone		= rt2400pci_fill_rxdone,
@@ -1770,28 +1771,28 @@ static const struct data_queue_desc rt2400pci_queue_rx = {
 	.entry_num		= 24,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= RXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci),
+	.priv_size		= sizeof(struct queue_entry_priv_mmio),
 };
 
 static const struct data_queue_desc rt2400pci_queue_tx = {
 	.entry_num		= 24,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci),
+	.priv_size		= sizeof(struct queue_entry_priv_mmio),
 };
 
 static const struct data_queue_desc rt2400pci_queue_bcn = {
 	.entry_num		= 1,
 	.data_size		= MGMT_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci),
+	.priv_size		= sizeof(struct queue_entry_priv_mmio),
 };
 
 static const struct data_queue_desc rt2400pci_queue_atim = {
 	.entry_num		= 8,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci),
+	.priv_size		= sizeof(struct queue_entry_priv_mmio),
 };
 
 static const struct rt2x00_ops rt2400pci_ops = {
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.c b/drivers/net/wireless/rt2x00/rt2500pci.c
index e1d2dc9ed28a..77e45b223d15 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.c
+++ b/drivers/net/wireless/rt2x00/rt2500pci.c
@@ -41,7 +41,7 @@
 /*
  * Register access.
  * All access to the CSR registers will go through the methods
- * rt2x00pci_register_read and rt2x00pci_register_write.
+ * rt2x00mmio_register_read and rt2x00mmio_register_write.
  * BBP and RF register require indirect register access,
  * and use the CSR registers BBPCSR and RFCSR to achieve this.
  * These indirect registers work with busy bits,
@@ -52,9 +52,9 @@
  * and we will print an error.
  */
 #define WAIT_FOR_BBP(__dev, __reg) \
-	rt2x00pci_regbusy_read((__dev), BBPCSR, BBPCSR_BUSY, (__reg))
+	rt2x00mmio_regbusy_read((__dev), BBPCSR, BBPCSR_BUSY, (__reg))
 #define WAIT_FOR_RF(__dev, __reg) \
-	rt2x00pci_regbusy_read((__dev), RFCSR, RFCSR_BUSY, (__reg))
+	rt2x00mmio_regbusy_read((__dev), RFCSR, RFCSR_BUSY, (__reg))
 
 static void rt2500pci_bbp_write(struct rt2x00_dev *rt2x00dev,
 				const unsigned int word, const u8 value)
@@ -74,7 +74,7 @@ static void rt2500pci_bbp_write(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
 		rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 1);
 
-		rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, BBPCSR, reg);
 	}
 
 	mutex_unlock(&rt2x00dev->csr_mutex);
@@ -101,7 +101,7 @@ static void rt2500pci_bbp_read(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
 		rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 0);
 
-		rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, BBPCSR, reg);
 
 		WAIT_FOR_BBP(rt2x00dev, &reg);
 	}
@@ -129,7 +129,7 @@ static void rt2500pci_rf_write(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, RFCSR_IF_SELECT, 0);
 		rt2x00_set_field32(&reg, RFCSR_BUSY, 1);
 
-		rt2x00pci_register_write(rt2x00dev, RFCSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, RFCSR, reg);
 		rt2x00_rf_write(rt2x00dev, word, value);
 	}
 
@@ -141,7 +141,7 @@ static void rt2500pci_eepromregister_read(struct eeprom_93cx6 *eeprom)
 	struct rt2x00_dev *rt2x00dev = eeprom->data;
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, CSR21, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR21, &reg);
 
 	eeprom->reg_data_in = !!rt2x00_get_field32(reg, CSR21_EEPROM_DATA_IN);
 	eeprom->reg_data_out = !!rt2x00_get_field32(reg, CSR21_EEPROM_DATA_OUT);
@@ -163,15 +163,15 @@ static void rt2500pci_eepromregister_write(struct eeprom_93cx6 *eeprom)
 	rt2x00_set_field32(&reg, CSR21_EEPROM_CHIP_SELECT,
 			   !!eeprom->reg_chip_select);
 
-	rt2x00pci_register_write(rt2x00dev, CSR21, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR21, reg);
 }
 
 #ifdef CONFIG_RT2X00_LIB_DEBUGFS
 static const struct rt2x00debug rt2500pci_rt2x00debug = {
 	.owner	= THIS_MODULE,
 	.csr	= {
-		.read		= rt2x00pci_register_read,
-		.write		= rt2x00pci_register_write,
+		.read		= rt2x00mmio_register_read,
+		.write		= rt2x00mmio_register_write,
 		.flags		= RT2X00DEBUGFS_OFFSET,
 		.word_base	= CSR_REG_BASE,
 		.word_size	= sizeof(u32),
@@ -205,7 +205,7 @@ static int rt2500pci_rfkill_poll(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, GPIOCSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, GPIOCSR, &reg);
 	return rt2x00_get_field32(reg, GPIOCSR_VAL0);
 }
 
@@ -218,14 +218,14 @@ static void rt2500pci_brightness_set(struct led_classdev *led_cdev,
 	unsigned int enabled = brightness != LED_OFF;
 	u32 reg;
 
-	rt2x00pci_register_read(led->rt2x00dev, LEDCSR, &reg);
+	rt2x00mmio_register_read(led->rt2x00dev, LEDCSR, &reg);
 
 	if (led->type == LED_TYPE_RADIO || led->type == LED_TYPE_ASSOC)
 		rt2x00_set_field32(&reg, LEDCSR_LINK, enabled);
 	else if (led->type == LED_TYPE_ACTIVITY)
 		rt2x00_set_field32(&reg, LEDCSR_ACTIVITY, enabled);
 
-	rt2x00pci_register_write(led->rt2x00dev, LEDCSR, reg);
+	rt2x00mmio_register_write(led->rt2x00dev, LEDCSR, reg);
 }
 
 static int rt2500pci_blink_set(struct led_classdev *led_cdev,
@@ -236,10 +236,10 @@ static int rt2500pci_blink_set(struct led_classdev *led_cdev,
 	    container_of(led_cdev, struct rt2x00_led, led_dev);
 	u32 reg;
 
-	rt2x00pci_register_read(led->rt2x00dev, LEDCSR, &reg);
+	rt2x00mmio_register_read(led->rt2x00dev, LEDCSR, &reg);
 	rt2x00_set_field32(&reg, LEDCSR_ON_PERIOD, *delay_on);
 	rt2x00_set_field32(&reg, LEDCSR_OFF_PERIOD, *delay_off);
-	rt2x00pci_register_write(led->rt2x00dev, LEDCSR, reg);
+	rt2x00mmio_register_write(led->rt2x00dev, LEDCSR, reg);
 
 	return 0;
 }
@@ -270,7 +270,7 @@ static void rt2500pci_config_filter(struct rt2x00_dev *rt2x00dev,
 	 * and broadcast frames will always be accepted since
 	 * there is no filter for it at this time.
 	 */
-	rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, RXCSR0, &reg);
 	rt2x00_set_field32(&reg, RXCSR0_DROP_CRC,
 			   !(filter_flags & FIF_FCSFAIL));
 	rt2x00_set_field32(&reg, RXCSR0_DROP_PHYSICAL,
@@ -286,7 +286,7 @@ static void rt2500pci_config_filter(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field32(&reg, RXCSR0_DROP_MCAST,
 			   !(filter_flags & FIF_ALLMULTI));
 	rt2x00_set_field32(&reg, RXCSR0_DROP_BCAST, 0);
-	rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
+	rt2x00mmio_register_write(rt2x00dev, RXCSR0, reg);
 }
 
 static void rt2500pci_config_intf(struct rt2x00_dev *rt2x00dev,
@@ -303,25 +303,25 @@ static void rt2500pci_config_intf(struct rt2x00_dev *rt2x00dev,
 		 * Enable beacon config
 		 */
 		bcn_preload = PREAMBLE + GET_DURATION(IEEE80211_HEADER, 20);
-		rt2x00pci_register_read(rt2x00dev, BCNCSR1, &reg);
+		rt2x00mmio_register_read(rt2x00dev, BCNCSR1, &reg);
 		rt2x00_set_field32(&reg, BCNCSR1_PRELOAD, bcn_preload);
 		rt2x00_set_field32(&reg, BCNCSR1_BEACON_CWMIN, queue->cw_min);
-		rt2x00pci_register_write(rt2x00dev, BCNCSR1, reg);
+		rt2x00mmio_register_write(rt2x00dev, BCNCSR1, reg);
 
 		/*
 		 * Enable synchronisation.
 		 */
-		rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
 		rt2x00_set_field32(&reg, CSR14_TSF_SYNC, conf->sync);
-		rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 	}
 
 	if (flags & CONFIG_UPDATE_MAC)
-		rt2x00pci_register_multiwrite(rt2x00dev, CSR3,
+		rt2x00mmio_register_multiwrite(rt2x00dev, CSR3,
 					      conf->mac, sizeof(conf->mac));
 
 	if (flags & CONFIG_UPDATE_BSSID)
-		rt2x00pci_register_multiwrite(rt2x00dev, CSR5,
+		rt2x00mmio_register_multiwrite(rt2x00dev, CSR5,
 					      conf->bssid, sizeof(conf->bssid));
 }
 
@@ -338,68 +338,68 @@ static void rt2500pci_config_erp(struct rt2x00_dev *rt2x00dev,
 	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
 		preamble_mask = erp->short_preamble << 3;
 
-		rt2x00pci_register_read(rt2x00dev, TXCSR1, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXCSR1, &reg);
 		rt2x00_set_field32(&reg, TXCSR1_ACK_TIMEOUT, 0x162);
 		rt2x00_set_field32(&reg, TXCSR1_ACK_CONSUME_TIME, 0xa2);
 		rt2x00_set_field32(&reg, TXCSR1_TSF_OFFSET, IEEE80211_HEADER);
 		rt2x00_set_field32(&reg, TXCSR1_AUTORESPONDER, 1);
-		rt2x00pci_register_write(rt2x00dev, TXCSR1, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXCSR1, reg);
 
-		rt2x00pci_register_read(rt2x00dev, ARCSR2, &reg);
+		rt2x00mmio_register_read(rt2x00dev, ARCSR2, &reg);
 		rt2x00_set_field32(&reg, ARCSR2_SIGNAL, 0x00);
 		rt2x00_set_field32(&reg, ARCSR2_SERVICE, 0x04);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
 				   GET_DURATION(ACK_SIZE, 10));
-		rt2x00pci_register_write(rt2x00dev, ARCSR2, reg);
+		rt2x00mmio_register_write(rt2x00dev, ARCSR2, reg);
 
-		rt2x00pci_register_read(rt2x00dev, ARCSR3, &reg);
+		rt2x00mmio_register_read(rt2x00dev, ARCSR3, &reg);
 		rt2x00_set_field32(&reg, ARCSR3_SIGNAL, 0x01 | preamble_mask);
 		rt2x00_set_field32(&reg, ARCSR3_SERVICE, 0x04);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
 				   GET_DURATION(ACK_SIZE, 20));
-		rt2x00pci_register_write(rt2x00dev, ARCSR3, reg);
+		rt2x00mmio_register_write(rt2x00dev, ARCSR3, reg);
 
-		rt2x00pci_register_read(rt2x00dev, ARCSR4, &reg);
+		rt2x00mmio_register_read(rt2x00dev, ARCSR4, &reg);
 		rt2x00_set_field32(&reg, ARCSR4_SIGNAL, 0x02 | preamble_mask);
 		rt2x00_set_field32(&reg, ARCSR4_SERVICE, 0x04);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
 				   GET_DURATION(ACK_SIZE, 55));
-		rt2x00pci_register_write(rt2x00dev, ARCSR4, reg);
+		rt2x00mmio_register_write(rt2x00dev, ARCSR4, reg);
 
-		rt2x00pci_register_read(rt2x00dev, ARCSR5, &reg);
+		rt2x00mmio_register_read(rt2x00dev, ARCSR5, &reg);
 		rt2x00_set_field32(&reg, ARCSR5_SIGNAL, 0x03 | preamble_mask);
 		rt2x00_set_field32(&reg, ARCSR5_SERVICE, 0x84);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
 				   GET_DURATION(ACK_SIZE, 110));
-		rt2x00pci_register_write(rt2x00dev, ARCSR5, reg);
+		rt2x00mmio_register_write(rt2x00dev, ARCSR5, reg);
 	}
 
 	if (changed & BSS_CHANGED_BASIC_RATES)
-		rt2x00pci_register_write(rt2x00dev, ARCSR1, erp->basic_rates);
+		rt2x00mmio_register_write(rt2x00dev, ARCSR1, erp->basic_rates);
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
-		rt2x00pci_register_read(rt2x00dev, CSR11, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR11, &reg);
 		rt2x00_set_field32(&reg, CSR11_SLOT_TIME, erp->slot_time);
-		rt2x00pci_register_write(rt2x00dev, CSR11, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR11, reg);
 
-		rt2x00pci_register_read(rt2x00dev, CSR18, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR18, &reg);
 		rt2x00_set_field32(&reg, CSR18_SIFS, erp->sifs);
 		rt2x00_set_field32(&reg, CSR18_PIFS, erp->pifs);
-		rt2x00pci_register_write(rt2x00dev, CSR18, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR18, reg);
 
-		rt2x00pci_register_read(rt2x00dev, CSR19, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR19, &reg);
 		rt2x00_set_field32(&reg, CSR19_DIFS, erp->difs);
 		rt2x00_set_field32(&reg, CSR19_EIFS, erp->eifs);
-		rt2x00pci_register_write(rt2x00dev, CSR19, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR19, reg);
 	}
 
 	if (changed & BSS_CHANGED_BEACON_INT) {
-		rt2x00pci_register_read(rt2x00dev, CSR12, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR12, &reg);
 		rt2x00_set_field32(&reg, CSR12_BEACON_INTERVAL,
 				   erp->beacon_int * 16);
 		rt2x00_set_field32(&reg, CSR12_CFP_MAX_DURATION,
 				   erp->beacon_int * 16);
-		rt2x00pci_register_write(rt2x00dev, CSR12, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR12, reg);
 	}
 
 }
@@ -418,7 +418,7 @@ static void rt2500pci_config_ant(struct rt2x00_dev *rt2x00dev,
 	BUG_ON(ant->rx == ANTENNA_SW_DIVERSITY ||
 	       ant->tx == ANTENNA_SW_DIVERSITY);
 
-	rt2x00pci_register_read(rt2x00dev, BBPCSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, BBPCSR1, &reg);
 	rt2500pci_bbp_read(rt2x00dev, 14, &r14);
 	rt2500pci_bbp_read(rt2x00dev, 2, &r2);
 
@@ -470,7 +470,7 @@ static void rt2500pci_config_ant(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, BBPCSR1_OFDM_FLIP, 0);
 	}
 
-	rt2x00pci_register_write(rt2x00dev, BBPCSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, BBPCSR1, reg);
 	rt2500pci_bbp_write(rt2x00dev, 14, r14);
 	rt2500pci_bbp_write(rt2x00dev, 2, r2);
 }
@@ -541,7 +541,7 @@ static void rt2500pci_config_channel(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Clear false CRC during channel switch.
 	 */
-	rt2x00pci_register_read(rt2x00dev, CNT0, &rf->rf1);
+	rt2x00mmio_register_read(rt2x00dev, CNT0, &rf->rf1);
 }
 
 static void rt2500pci_config_txpower(struct rt2x00_dev *rt2x00dev,
@@ -559,12 +559,12 @@ static void rt2500pci_config_retry_limit(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, CSR11, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR11, &reg);
 	rt2x00_set_field32(&reg, CSR11_LONG_RETRY,
 			   libconf->conf->long_frame_max_tx_count);
 	rt2x00_set_field32(&reg, CSR11_SHORT_RETRY,
 			   libconf->conf->short_frame_max_tx_count);
-	rt2x00pci_register_write(rt2x00dev, CSR11, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR11, reg);
 }
 
 static void rt2500pci_config_ps(struct rt2x00_dev *rt2x00dev,
@@ -576,7 +576,7 @@ static void rt2500pci_config_ps(struct rt2x00_dev *rt2x00dev,
 	u32 reg;
 
 	if (state == STATE_SLEEP) {
-		rt2x00pci_register_read(rt2x00dev, CSR20, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR20, &reg);
 		rt2x00_set_field32(&reg, CSR20_DELAY_AFTER_TBCN,
 				   (rt2x00dev->beacon_int - 20) * 16);
 		rt2x00_set_field32(&reg, CSR20_TBCN_BEFORE_WAKEUP,
@@ -584,14 +584,14 @@ static void rt2500pci_config_ps(struct rt2x00_dev *rt2x00dev,
 
 		/* We must first disable autowake before it can be enabled */
 		rt2x00_set_field32(&reg, CSR20_AUTOWAKE, 0);
-		rt2x00pci_register_write(rt2x00dev, CSR20, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR20, reg);
 
 		rt2x00_set_field32(&reg, CSR20_AUTOWAKE, 1);
-		rt2x00pci_register_write(rt2x00dev, CSR20, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR20, reg);
 	} else {
-		rt2x00pci_register_read(rt2x00dev, CSR20, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR20, &reg);
 		rt2x00_set_field32(&reg, CSR20_AUTOWAKE, 0);
-		rt2x00pci_register_write(rt2x00dev, CSR20, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR20, reg);
 	}
 
 	rt2x00dev->ops->lib->set_device_state(rt2x00dev, state);
@@ -625,13 +625,13 @@ static void rt2500pci_link_stats(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Update FCS error count from register.
 	 */
-	rt2x00pci_register_read(rt2x00dev, CNT0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CNT0, &reg);
 	qual->rx_failed = rt2x00_get_field32(reg, CNT0_FCS_ERROR);
 
 	/*
 	 * Update False CCA count from register.
 	 */
-	rt2x00pci_register_read(rt2x00dev, CNT3, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CNT3, &reg);
 	qual->false_cca = rt2x00_get_field32(reg, CNT3_FALSE_CCA);
 }
 
@@ -731,16 +731,16 @@ static void rt2500pci_start_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, RXCSR0, &reg);
 		rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX, 0);
-		rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
+		rt2x00mmio_register_write(rt2x00dev, RXCSR0, reg);
 		break;
 	case QID_BEACON:
-		rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
 		rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 1);
 		rt2x00_set_field32(&reg, CSR14_TBCN, 1);
 		rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 1);
-		rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 		break;
 	default:
 		break;
@@ -754,19 +754,19 @@ static void rt2500pci_kick_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_AC_VO:
-		rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
 		rt2x00_set_field32(&reg, TXCSR0_KICK_PRIO, 1);
-		rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
 	case QID_AC_VI:
-		rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
 		rt2x00_set_field32(&reg, TXCSR0_KICK_TX, 1);
-		rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
 	case QID_ATIM:
-		rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
 		rt2x00_set_field32(&reg, TXCSR0_KICK_ATIM, 1);
-		rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
 	default:
 		break;
@@ -782,21 +782,21 @@ static void rt2500pci_stop_queue(struct data_queue *queue)
 	case QID_AC_VO:
 	case QID_AC_VI:
 	case QID_ATIM:
-		rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
 		rt2x00_set_field32(&reg, TXCSR0_ABORT, 1);
-		rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
 	case QID_RX:
-		rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, RXCSR0, &reg);
 		rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX, 1);
-		rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
+		rt2x00mmio_register_write(rt2x00dev, RXCSR0, reg);
 		break;
 	case QID_BEACON:
-		rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
 		rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 0);
 		rt2x00_set_field32(&reg, CSR14_TBCN, 0);
 		rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
-		rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 
 		/*
 		 * Wait for possibly running tbtt tasklets.
@@ -813,7 +813,7 @@ static void rt2500pci_stop_queue(struct data_queue *queue)
  */
 static bool rt2500pci_get_entry_state(struct queue_entry *entry)
 {
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	u32 word;
 
 	if (entry->queue->qid == QID_RX) {
@@ -830,7 +830,7 @@ static bool rt2500pci_get_entry_state(struct queue_entry *entry)
 
 static void rt2500pci_clear_entry(struct queue_entry *entry)
 {
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
 	u32 word;
 
@@ -852,53 +852,53 @@ static void rt2500pci_clear_entry(struct queue_entry *entry)
 
 static int rt2500pci_init_queues(struct rt2x00_dev *rt2x00dev)
 {
-	struct queue_entry_priv_pci *entry_priv;
+	struct queue_entry_priv_mmio *entry_priv;
 	u32 reg;
 
 	/*
 	 * Initialize registers.
 	 */
-	rt2x00pci_register_read(rt2x00dev, TXCSR2, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXCSR2, &reg);
 	rt2x00_set_field32(&reg, TXCSR2_TXD_SIZE, rt2x00dev->tx[0].desc_size);
 	rt2x00_set_field32(&reg, TXCSR2_NUM_TXD, rt2x00dev->tx[1].limit);
 	rt2x00_set_field32(&reg, TXCSR2_NUM_ATIM, rt2x00dev->atim->limit);
 	rt2x00_set_field32(&reg, TXCSR2_NUM_PRIO, rt2x00dev->tx[0].limit);
-	rt2x00pci_register_write(rt2x00dev, TXCSR2, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXCSR2, reg);
 
 	entry_priv = rt2x00dev->tx[1].entries[0].priv_data;
-	rt2x00pci_register_read(rt2x00dev, TXCSR3, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXCSR3, &reg);
 	rt2x00_set_field32(&reg, TXCSR3_TX_RING_REGISTER,
 			   entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, TXCSR3, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXCSR3, reg);
 
 	entry_priv = rt2x00dev->tx[0].entries[0].priv_data;
-	rt2x00pci_register_read(rt2x00dev, TXCSR5, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXCSR5, &reg);
 	rt2x00_set_field32(&reg, TXCSR5_PRIO_RING_REGISTER,
 			   entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, TXCSR5, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXCSR5, reg);
 
 	entry_priv = rt2x00dev->atim->entries[0].priv_data;
-	rt2x00pci_register_read(rt2x00dev, TXCSR4, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXCSR4, &reg);
 	rt2x00_set_field32(&reg, TXCSR4_ATIM_RING_REGISTER,
 			   entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, TXCSR4, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXCSR4, reg);
 
 	entry_priv = rt2x00dev->bcn->entries[0].priv_data;
-	rt2x00pci_register_read(rt2x00dev, TXCSR6, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXCSR6, &reg);
 	rt2x00_set_field32(&reg, TXCSR6_BEACON_RING_REGISTER,
 			   entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, TXCSR6, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXCSR6, reg);
 
-	rt2x00pci_register_read(rt2x00dev, RXCSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, RXCSR1, &reg);
 	rt2x00_set_field32(&reg, RXCSR1_RXD_SIZE, rt2x00dev->rx->desc_size);
 	rt2x00_set_field32(&reg, RXCSR1_NUM_RXD, rt2x00dev->rx->limit);
-	rt2x00pci_register_write(rt2x00dev, RXCSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, RXCSR1, reg);
 
 	entry_priv = rt2x00dev->rx->entries[0].priv_data;
-	rt2x00pci_register_read(rt2x00dev, RXCSR2, &reg);
+	rt2x00mmio_register_read(rt2x00dev, RXCSR2, &reg);
 	rt2x00_set_field32(&reg, RXCSR2_RX_RING_REGISTER,
 			   entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, RXCSR2, reg);
+	rt2x00mmio_register_write(rt2x00dev, RXCSR2, reg);
 
 	return 0;
 }
@@ -907,30 +907,30 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2x00pci_register_write(rt2x00dev, PSCSR0, 0x00020002);
-	rt2x00pci_register_write(rt2x00dev, PSCSR1, 0x00000002);
-	rt2x00pci_register_write(rt2x00dev, PSCSR2, 0x00020002);
-	rt2x00pci_register_write(rt2x00dev, PSCSR3, 0x00000002);
+	rt2x00mmio_register_write(rt2x00dev, PSCSR0, 0x00020002);
+	rt2x00mmio_register_write(rt2x00dev, PSCSR1, 0x00000002);
+	rt2x00mmio_register_write(rt2x00dev, PSCSR2, 0x00020002);
+	rt2x00mmio_register_write(rt2x00dev, PSCSR3, 0x00000002);
 
-	rt2x00pci_register_read(rt2x00dev, TIMECSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TIMECSR, &reg);
 	rt2x00_set_field32(&reg, TIMECSR_US_COUNT, 33);
 	rt2x00_set_field32(&reg, TIMECSR_US_64_COUNT, 63);
 	rt2x00_set_field32(&reg, TIMECSR_BEACON_EXPECT, 0);
-	rt2x00pci_register_write(rt2x00dev, TIMECSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, TIMECSR, reg);
 
-	rt2x00pci_register_read(rt2x00dev, CSR9, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR9, &reg);
 	rt2x00_set_field32(&reg, CSR9_MAX_FRAME_UNIT,
 			   rt2x00dev->rx->data_size / 128);
-	rt2x00pci_register_write(rt2x00dev, CSR9, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR9, reg);
 
 	/*
 	 * Always use CWmin and CWmax set in descriptor.
 	 */
-	rt2x00pci_register_read(rt2x00dev, CSR11, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR11, &reg);
 	rt2x00_set_field32(&reg, CSR11_CW_SELECT, 0);
-	rt2x00pci_register_write(rt2x00dev, CSR11, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR11, reg);
 
-	rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
 	rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 0);
 	rt2x00_set_field32(&reg, CSR14_TSF_SYNC, 0);
 	rt2x00_set_field32(&reg, CSR14_TBCN, 0);
@@ -939,11 +939,11 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
 	rt2x00_set_field32(&reg, CSR14_CFP_COUNT_PRELOAD, 0);
 	rt2x00_set_field32(&reg, CSR14_TBCM_PRELOAD, 0);
-	rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 
-	rt2x00pci_register_write(rt2x00dev, CNT3, 0);
+	rt2x00mmio_register_write(rt2x00dev, CNT3, 0);
 
-	rt2x00pci_register_read(rt2x00dev, TXCSR8, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXCSR8, &reg);
 	rt2x00_set_field32(&reg, TXCSR8_BBP_ID0, 10);
 	rt2x00_set_field32(&reg, TXCSR8_BBP_ID0_VALID, 1);
 	rt2x00_set_field32(&reg, TXCSR8_BBP_ID1, 11);
@@ -952,30 +952,30 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, TXCSR8_BBP_ID2_VALID, 1);
 	rt2x00_set_field32(&reg, TXCSR8_BBP_ID3, 12);
 	rt2x00_set_field32(&reg, TXCSR8_BBP_ID3_VALID, 1);
-	rt2x00pci_register_write(rt2x00dev, TXCSR8, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXCSR8, reg);
 
-	rt2x00pci_register_read(rt2x00dev, ARTCSR0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, ARTCSR0, &reg);
 	rt2x00_set_field32(&reg, ARTCSR0_ACK_CTS_1MBS, 112);
 	rt2x00_set_field32(&reg, ARTCSR0_ACK_CTS_2MBS, 56);
 	rt2x00_set_field32(&reg, ARTCSR0_ACK_CTS_5_5MBS, 20);
 	rt2x00_set_field32(&reg, ARTCSR0_ACK_CTS_11MBS, 10);
-	rt2x00pci_register_write(rt2x00dev, ARTCSR0, reg);
+	rt2x00mmio_register_write(rt2x00dev, ARTCSR0, reg);
 
-	rt2x00pci_register_read(rt2x00dev, ARTCSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, ARTCSR1, &reg);
 	rt2x00_set_field32(&reg, ARTCSR1_ACK_CTS_6MBS, 45);
 	rt2x00_set_field32(&reg, ARTCSR1_ACK_CTS_9MBS, 37);
 	rt2x00_set_field32(&reg, ARTCSR1_ACK_CTS_12MBS, 33);
 	rt2x00_set_field32(&reg, ARTCSR1_ACK_CTS_18MBS, 29);
-	rt2x00pci_register_write(rt2x00dev, ARTCSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, ARTCSR1, reg);
 
-	rt2x00pci_register_read(rt2x00dev, ARTCSR2, &reg);
+	rt2x00mmio_register_read(rt2x00dev, ARTCSR2, &reg);
 	rt2x00_set_field32(&reg, ARTCSR2_ACK_CTS_24MBS, 29);
 	rt2x00_set_field32(&reg, ARTCSR2_ACK_CTS_36MBS, 25);
 	rt2x00_set_field32(&reg, ARTCSR2_ACK_CTS_48MBS, 25);
 	rt2x00_set_field32(&reg, ARTCSR2_ACK_CTS_54MBS, 25);
-	rt2x00pci_register_write(rt2x00dev, ARTCSR2, reg);
+	rt2x00mmio_register_write(rt2x00dev, ARTCSR2, reg);
 
-	rt2x00pci_register_read(rt2x00dev, RXCSR3, &reg);
+	rt2x00mmio_register_read(rt2x00dev, RXCSR3, &reg);
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID0, 47); /* CCK Signal */
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID0_VALID, 1);
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID1, 51); /* Rssi */
@@ -984,9 +984,9 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID2_VALID, 1);
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID3, 51); /* RSSI */
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID3_VALID, 1);
-	rt2x00pci_register_write(rt2x00dev, RXCSR3, reg);
+	rt2x00mmio_register_write(rt2x00dev, RXCSR3, reg);
 
-	rt2x00pci_register_read(rt2x00dev, PCICSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, PCICSR, &reg);
 	rt2x00_set_field32(&reg, PCICSR_BIG_ENDIAN, 0);
 	rt2x00_set_field32(&reg, PCICSR_RX_TRESHOLD, 0);
 	rt2x00_set_field32(&reg, PCICSR_TX_TRESHOLD, 3);
@@ -994,54 +994,54 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, PCICSR_ENABLE_CLK, 1);
 	rt2x00_set_field32(&reg, PCICSR_READ_MULTIPLE, 1);
 	rt2x00_set_field32(&reg, PCICSR_WRITE_INVALID, 1);
-	rt2x00pci_register_write(rt2x00dev, PCICSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, PCICSR, reg);
 
-	rt2x00pci_register_write(rt2x00dev, PWRCSR0, 0x3f3b3100);
+	rt2x00mmio_register_write(rt2x00dev, PWRCSR0, 0x3f3b3100);
 
-	rt2x00pci_register_write(rt2x00dev, GPIOCSR, 0x0000ff00);
-	rt2x00pci_register_write(rt2x00dev, TESTCSR, 0x000000f0);
+	rt2x00mmio_register_write(rt2x00dev, GPIOCSR, 0x0000ff00);
+	rt2x00mmio_register_write(rt2x00dev, TESTCSR, 0x000000f0);
 
 	if (rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_AWAKE))
 		return -EBUSY;
 
-	rt2x00pci_register_write(rt2x00dev, MACCSR0, 0x00213223);
-	rt2x00pci_register_write(rt2x00dev, MACCSR1, 0x00235518);
+	rt2x00mmio_register_write(rt2x00dev, MACCSR0, 0x00213223);
+	rt2x00mmio_register_write(rt2x00dev, MACCSR1, 0x00235518);
 
-	rt2x00pci_register_read(rt2x00dev, MACCSR2, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MACCSR2, &reg);
 	rt2x00_set_field32(&reg, MACCSR2_DELAY, 64);
-	rt2x00pci_register_write(rt2x00dev, MACCSR2, reg);
+	rt2x00mmio_register_write(rt2x00dev, MACCSR2, reg);
 
-	rt2x00pci_register_read(rt2x00dev, RALINKCSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, RALINKCSR, &reg);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_DATA0, 17);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_ID0, 26);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_VALID0, 1);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_DATA1, 0);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_ID1, 26);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_VALID1, 1);
-	rt2x00pci_register_write(rt2x00dev, RALINKCSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, RALINKCSR, reg);
 
-	rt2x00pci_register_write(rt2x00dev, BBPCSR1, 0x82188200);
+	rt2x00mmio_register_write(rt2x00dev, BBPCSR1, 0x82188200);
 
-	rt2x00pci_register_write(rt2x00dev, TXACKCSR0, 0x00000020);
+	rt2x00mmio_register_write(rt2x00dev, TXACKCSR0, 0x00000020);
 
-	rt2x00pci_register_read(rt2x00dev, CSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR1, &reg);
 	rt2x00_set_field32(&reg, CSR1_SOFT_RESET, 1);
 	rt2x00_set_field32(&reg, CSR1_BBP_RESET, 0);
 	rt2x00_set_field32(&reg, CSR1_HOST_READY, 0);
-	rt2x00pci_register_write(rt2x00dev, CSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR1, reg);
 
-	rt2x00pci_register_read(rt2x00dev, CSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR1, &reg);
 	rt2x00_set_field32(&reg, CSR1_SOFT_RESET, 0);
 	rt2x00_set_field32(&reg, CSR1_HOST_READY, 1);
-	rt2x00pci_register_write(rt2x00dev, CSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR1, reg);
 
 	/*
 	 * We must clear the FCS and FIFO error count.
 	 * These registers are cleared on read,
 	 * so we may pass a useless variable to store the value.
 	 */
-	rt2x00pci_register_read(rt2x00dev, CNT0, &reg);
-	rt2x00pci_register_read(rt2x00dev, CNT4, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CNT0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CNT4, &reg);
 
 	return 0;
 }
@@ -1058,7 +1058,7 @@ static int rt2500pci_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 		udelay(REGISTER_BUSY_DELAY);
 	}
 
-	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
+	rt2x00_err(rt2x00dev, "BBP register access failed, aborting\n");
 	return -EACCES;
 }
 
@@ -1131,8 +1131,8 @@ static void rt2500pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	 * should clear the register to assure a clean state.
 	 */
 	if (state == STATE_RADIO_IRQ_ON) {
-		rt2x00pci_register_read(rt2x00dev, CSR7, &reg);
-		rt2x00pci_register_write(rt2x00dev, CSR7, reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR7, &reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR7, reg);
 	}
 
 	/*
@@ -1141,13 +1141,13 @@ static void rt2500pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	 */
 	spin_lock_irqsave(&rt2x00dev->irqmask_lock, flags);
 
-	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
 	rt2x00_set_field32(&reg, CSR8_TBCN_EXPIRE, mask);
 	rt2x00_set_field32(&reg, CSR8_TXDONE_TXRING, mask);
 	rt2x00_set_field32(&reg, CSR8_TXDONE_ATIMRING, mask);
 	rt2x00_set_field32(&reg, CSR8_TXDONE_PRIORING, mask);
 	rt2x00_set_field32(&reg, CSR8_RXDONE, mask);
-	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR8, reg);
 
 	spin_unlock_irqrestore(&rt2x00dev->irqmask_lock, flags);
 
@@ -1179,7 +1179,7 @@ static void rt2500pci_disable_radio(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Disable power
 	 */
-	rt2x00pci_register_write(rt2x00dev, PWRCSR0, 0);
+	rt2x00mmio_register_write(rt2x00dev, PWRCSR0, 0);
 }
 
 static int rt2500pci_set_state(struct rt2x00_dev *rt2x00dev,
@@ -1193,12 +1193,12 @@ static int rt2500pci_set_state(struct rt2x00_dev *rt2x00dev,
 
 	put_to_sleep = (state != STATE_AWAKE);
 
-	rt2x00pci_register_read(rt2x00dev, PWRCSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, PWRCSR1, &reg);
 	rt2x00_set_field32(&reg, PWRCSR1_SET_STATE, 1);
 	rt2x00_set_field32(&reg, PWRCSR1_BBP_DESIRE_STATE, state);
 	rt2x00_set_field32(&reg, PWRCSR1_RF_DESIRE_STATE, state);
 	rt2x00_set_field32(&reg, PWRCSR1_PUT_TO_SLEEP, put_to_sleep);
-	rt2x00pci_register_write(rt2x00dev, PWRCSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, PWRCSR1, reg);
 
 	/*
 	 * Device is not guaranteed to be in the requested state yet.
@@ -1206,12 +1206,12 @@ static int rt2500pci_set_state(struct rt2x00_dev *rt2x00dev,
 	 * device has entered the correct state.
 	 */
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2x00pci_register_read(rt2x00dev, PWRCSR1, &reg2);
+		rt2x00mmio_register_read(rt2x00dev, PWRCSR1, &reg2);
 		bbp_state = rt2x00_get_field32(reg2, PWRCSR1_BBP_CURR_STATE);
 		rf_state = rt2x00_get_field32(reg2, PWRCSR1_RF_CURR_STATE);
 		if (bbp_state == state && rf_state == state)
 			return 0;
-		rt2x00pci_register_write(rt2x00dev, PWRCSR1, reg);
+		rt2x00mmio_register_write(rt2x00dev, PWRCSR1, reg);
 		msleep(10);
 	}
 
@@ -1246,8 +1246,8 @@ static int rt2500pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 	}
 
 	if (unlikely(retval))
-		ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
-		      state, retval);
+		rt2x00_err(rt2x00dev, "Device failed to enter state %d (%d)\n",
+			   state, retval);
 
 	return retval;
 }
@@ -1259,7 +1259,7 @@ static void rt2500pci_write_tx_desc(struct queue_entry *entry,
 				    struct txentry_desc *txdesc)
 {
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	__le32 *txd = entry_priv->desc;
 	u32 word;
 
@@ -1335,12 +1335,12 @@ static void rt2500pci_write_beacon(struct queue_entry *entry,
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
-	rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
 	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
-	rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 
 	if (rt2x00queue_map_txskb(entry)) {
-		ERROR(rt2x00dev, "Fail to map beacon, aborting\n");
+		rt2x00_err(rt2x00dev, "Fail to map beacon, aborting\n");
 		goto out;
 	}
 
@@ -1358,7 +1358,7 @@ out:
 	 * Enable beaconing again.
 	 */
 	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 1);
-	rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 }
 
 /*
@@ -1367,7 +1367,7 @@ out:
 static void rt2500pci_fill_rxdone(struct queue_entry *entry,
 				  struct rxdone_entry_desc *rxdesc)
 {
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	u32 word0;
 	u32 word2;
 
@@ -1405,7 +1405,7 @@ static void rt2500pci_txdone(struct rt2x00_dev *rt2x00dev,
 			     const enum data_queue_qid queue_idx)
 {
 	struct data_queue *queue = rt2x00queue_get_tx_queue(rt2x00dev, queue_idx);
-	struct queue_entry_priv_pci *entry_priv;
+	struct queue_entry_priv_mmio *entry_priv;
 	struct queue_entry *entry;
 	struct txdone_entry_desc txdesc;
 	u32 word;
@@ -1451,9 +1451,9 @@ static inline void rt2500pci_enable_interrupt(struct rt2x00_dev *rt2x00dev,
 	 */
 	spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
 	rt2x00_set_field32(&reg, irq_field, 0);
-	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR8, reg);
 
 	spin_unlock_irq(&rt2x00dev->irqmask_lock);
 }
@@ -1476,11 +1476,11 @@ static void rt2500pci_txstatus_tasklet(unsigned long data)
 	if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags)) {
 		spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-		rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+		rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
 		rt2x00_set_field32(&reg, CSR8_TXDONE_TXRING, 0);
 		rt2x00_set_field32(&reg, CSR8_TXDONE_ATIMRING, 0);
 		rt2x00_set_field32(&reg, CSR8_TXDONE_PRIORING, 0);
-		rt2x00pci_register_write(rt2x00dev, CSR8, reg);
+		rt2x00mmio_register_write(rt2x00dev, CSR8, reg);
 
 		spin_unlock_irq(&rt2x00dev->irqmask_lock);
 	}
@@ -1497,7 +1497,7 @@ static void rt2500pci_tbtt_tasklet(unsigned long data)
 static void rt2500pci_rxdone_tasklet(unsigned long data)
 {
 	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
-	if (rt2x00pci_rxdone(rt2x00dev))
+	if (rt2x00mmio_rxdone(rt2x00dev))
 		tasklet_schedule(&rt2x00dev->rxdone_tasklet);
 	else if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
 		rt2500pci_enable_interrupt(rt2x00dev, CSR8_RXDONE);
@@ -1512,8 +1512,8 @@ static irqreturn_t rt2500pci_interrupt(int irq, void *dev_instance)
 	 * Get the interrupt sources & saved to local variable.
 	 * Write register value back to clear pending interrupts.
 	 */
-	rt2x00pci_register_read(rt2x00dev, CSR7, &reg);
-	rt2x00pci_register_write(rt2x00dev, CSR7, reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR7, &reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR7, reg);
 
 	if (!reg)
 		return IRQ_NONE;
@@ -1550,9 +1550,9 @@ static irqreturn_t rt2500pci_interrupt(int irq, void *dev_instance)
 	 */
 	spin_lock(&rt2x00dev->irqmask_lock);
 
-	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
 	reg |= mask;
-	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
+	rt2x00mmio_register_write(rt2x00dev, CSR8, reg);
 
 	spin_unlock(&rt2x00dev->irqmask_lock);
 
@@ -1569,7 +1569,7 @@ static int rt2500pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	u16 word;
 	u8 *mac;
 
-	rt2x00pci_register_read(rt2x00dev, CSR21, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR21, &reg);
 
 	eeprom.data = rt2x00dev;
 	eeprom.register_read = rt2500pci_eepromregister_read;
@@ -1590,7 +1590,7 @@ static int rt2500pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	if (!is_valid_ether_addr(mac)) {
 		eth_random_addr(mac);
-		EEPROM(rt2x00dev, "MAC: %pM\n", mac);
+		rt2x00_eeprom_dbg(rt2x00dev, "MAC: %pM\n", mac);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &word);
@@ -1606,7 +1606,7 @@ static int rt2500pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_HARDWARE_RADIO, 0);
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_RF_TYPE, RF2522);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_ANTENNA, word);
-		EEPROM(rt2x00dev, "Antenna: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "Antenna: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &word);
@@ -1615,7 +1615,7 @@ static int rt2500pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_NIC_DYN_BBP_TUNE, 0);
 		rt2x00_set_field16(&word, EEPROM_NIC_CCK_TX_POWER, 0);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_NIC, word);
-		EEPROM(rt2x00dev, "NIC: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "NIC: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_CALIBRATE_OFFSET, &word);
@@ -1623,7 +1623,8 @@ static int rt2500pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_CALIBRATE_OFFSET_RSSI,
 				   DEFAULT_RSSI_OFFSET);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_CALIBRATE_OFFSET, word);
-		EEPROM(rt2x00dev, "Calibrate offset: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "Calibrate offset: 0x%04x\n",
+				  word);
 	}
 
 	return 0;
@@ -1644,7 +1645,7 @@ static int rt2500pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	 * Identify RF chipset.
 	 */
 	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RF_TYPE);
-	rt2x00pci_register_read(rt2x00dev, CSR0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR0, &reg);
 	rt2x00_set_chip(rt2x00dev, RT2560, value,
 			rt2x00_get_field32(reg, CSR0_REVISION));
 
@@ -1654,7 +1655,7 @@ static int rt2500pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	    !rt2x00_rf(rt2x00dev, RF2525) &&
 	    !rt2x00_rf(rt2x00dev, RF2525E) &&
 	    !rt2x00_rf(rt2x00dev, RF5222)) {
-		ERROR(rt2x00dev, "Invalid RF chipset detected.\n");
+		rt2x00_err(rt2x00dev, "Invalid RF chipset detected\n");
 		return -ENODEV;
 	}
 
@@ -1950,9 +1951,9 @@ static int rt2500pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 	 * Enable rfkill polling by setting GPIO direction of the
 	 * rfkill switch GPIO pin correctly.
 	 */
-	rt2x00pci_register_read(rt2x00dev, GPIOCSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, GPIOCSR, &reg);
 	rt2x00_set_field32(&reg, GPIOCSR_DIR0, 1);
-	rt2x00pci_register_write(rt2x00dev, GPIOCSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, GPIOCSR, reg);
 
 	/*
 	 * Initialize hw specifications.
@@ -1986,9 +1987,9 @@ static u64 rt2500pci_get_tsf(struct ieee80211_hw *hw,
 	u64 tsf;
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, CSR17, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR17, &reg);
 	tsf = (u64) rt2x00_get_field32(reg, CSR17_HIGH_TSFTIMER) << 32;
-	rt2x00pci_register_read(rt2x00dev, CSR16, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR16, &reg);
 	tsf |= rt2x00_get_field32(reg, CSR16_LOW_TSFTIMER);
 
 	return tsf;
@@ -1999,7 +2000,7 @@ static int rt2500pci_tx_last_beacon(struct ieee80211_hw *hw)
 	struct rt2x00_dev *rt2x00dev = hw->priv;
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, CSR15, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CSR15, &reg);
 	return rt2x00_get_field32(reg, CSR15_BEACON_SENT);
 }
 
@@ -2032,8 +2033,8 @@ static const struct rt2x00lib_ops rt2500pci_rt2x00_ops = {
 	.tbtt_tasklet		= rt2500pci_tbtt_tasklet,
 	.rxdone_tasklet		= rt2500pci_rxdone_tasklet,
 	.probe_hw		= rt2500pci_probe_hw,
-	.initialize		= rt2x00pci_initialize,
-	.uninitialize		= rt2x00pci_uninitialize,
+	.initialize		= rt2x00mmio_initialize,
+	.uninitialize		= rt2x00mmio_uninitialize,
 	.get_entry_state	= rt2500pci_get_entry_state,
 	.clear_entry		= rt2500pci_clear_entry,
 	.set_device_state	= rt2500pci_set_device_state,
@@ -2044,7 +2045,7 @@ static const struct rt2x00lib_ops rt2500pci_rt2x00_ops = {
 	.start_queue		= rt2500pci_start_queue,
 	.kick_queue		= rt2500pci_kick_queue,
 	.stop_queue		= rt2500pci_stop_queue,
-	.flush_queue		= rt2x00pci_flush_queue,
+	.flush_queue		= rt2x00mmio_flush_queue,
 	.write_tx_desc		= rt2500pci_write_tx_desc,
 	.write_beacon		= rt2500pci_write_beacon,
 	.fill_rxdone		= rt2500pci_fill_rxdone,
@@ -2059,28 +2060,28 @@ static const struct data_queue_desc rt2500pci_queue_rx = {
 	.entry_num		= 32,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= RXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci),
+	.priv_size		= sizeof(struct queue_entry_priv_mmio),
 };
 
 static const struct data_queue_desc rt2500pci_queue_tx = {
 	.entry_num		= 32,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci),
+	.priv_size		= sizeof(struct queue_entry_priv_mmio),
 };
 
 static const struct data_queue_desc rt2500pci_queue_bcn = {
 	.entry_num		= 1,
 	.data_size		= MGMT_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci),
+	.priv_size		= sizeof(struct queue_entry_priv_mmio),
 };
 
 static const struct data_queue_desc rt2500pci_queue_atim = {
 	.entry_num		= 8,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci),
+	.priv_size		= sizeof(struct queue_entry_priv_mmio),
 };
 
 static const struct rt2x00_ops rt2500pci_ops = {
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index 6b2e1e431dd2..a7f7b365eff4 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -134,8 +134,8 @@ static int rt2500usb_regbusy_read(struct rt2x00_dev *rt2x00dev,
 		udelay(REGISTER_BUSY_DELAY);
 	}
 
-	ERROR(rt2x00dev, "Indirect register access failed: "
-	      "offset=0x%.08x, value=0x%.08x\n", offset, *reg);
+	rt2x00_err(rt2x00dev, "Indirect register access failed: offset=0x%.08x, value=0x%.08x\n",
+		   offset, *reg);
 	*reg = ~0;
 
 	return 0;
@@ -916,7 +916,7 @@ static int rt2500usb_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 		udelay(REGISTER_BUSY_DELAY);
 	}
 
-	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
+	rt2x00_err(rt2x00dev, "BBP register access failed, aborting\n");
 	return -EACCES;
 }
 
@@ -1069,8 +1069,8 @@ static int rt2500usb_set_device_state(struct rt2x00_dev *rt2x00dev,
 	}
 
 	if (unlikely(retval))
-		ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
-		      state, retval);
+		rt2x00_err(rt2x00dev, "Device failed to enter state %d (%d)\n",
+			   state, retval);
 
 	return retval;
 }
@@ -1353,7 +1353,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	if (!is_valid_ether_addr(mac)) {
 		eth_random_addr(mac);
-		EEPROM(rt2x00dev, "MAC: %pM\n", mac);
+		rt2x00_eeprom_dbg(rt2x00dev, "MAC: %pM\n", mac);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &word);
@@ -1369,7 +1369,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_HARDWARE_RADIO, 0);
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_RF_TYPE, RF2522);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_ANTENNA, word);
-		EEPROM(rt2x00dev, "Antenna: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "Antenna: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &word);
@@ -1378,7 +1378,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_NIC_DYN_BBP_TUNE, 0);
 		rt2x00_set_field16(&word, EEPROM_NIC_CCK_TX_POWER, 0);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_NIC, word);
-		EEPROM(rt2x00dev, "NIC: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "NIC: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_CALIBRATE_OFFSET, &word);
@@ -1386,14 +1386,15 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_CALIBRATE_OFFSET_RSSI,
 				   DEFAULT_RSSI_OFFSET);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_CALIBRATE_OFFSET, word);
-		EEPROM(rt2x00dev, "Calibrate offset: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "Calibrate offset: 0x%04x\n",
+				  word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE, &word);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_THRESHOLD, 45);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_BBPTUNE, word);
-		EEPROM(rt2x00dev, "BBPtune: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "BBPtune: 0x%04x\n", word);
 	}
 
 	/*
@@ -1408,7 +1409,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_VGCUPPER, 0x40);
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_VGCLOWER, bbp);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_BBPTUNE_VGC, word);
-		EEPROM(rt2x00dev, "BBPtune vgc: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "BBPtune vgc: 0x%04x\n", word);
 	} else {
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_VGCLOWER, bbp);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_BBPTUNE_VGC, word);
@@ -1419,7 +1420,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R17_LOW, 0x48);
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R17_HIGH, 0x41);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_BBPTUNE_R17, word);
-		EEPROM(rt2x00dev, "BBPtune r17: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "BBPtune r17: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R24, &word);
@@ -1427,7 +1428,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R24_LOW, 0x40);
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R24_HIGH, 0x80);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_BBPTUNE_R24, word);
-		EEPROM(rt2x00dev, "BBPtune r24: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "BBPtune r24: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R25, &word);
@@ -1435,7 +1436,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R25_LOW, 0x40);
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R25_HIGH, 0x50);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_BBPTUNE_R25, word);
-		EEPROM(rt2x00dev, "BBPtune r25: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "BBPtune r25: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R61, &word);
@@ -1443,7 +1444,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R61_LOW, 0x60);
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R61_HIGH, 0x6d);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_BBPTUNE_R61, word);
-		EEPROM(rt2x00dev, "BBPtune r61: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "BBPtune r61: 0x%04x\n", word);
 	}
 
 	return 0;
@@ -1468,7 +1469,7 @@ static int rt2500usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_chip(rt2x00dev, RT2570, value, reg);
 
 	if (((reg & 0xfff0) != 0) || ((reg & 0x0000000f) == 0)) {
-		ERROR(rt2x00dev, "Invalid RT chipset detected.\n");
+		rt2x00_err(rt2x00dev, "Invalid RT chipset detected\n");
 		return -ENODEV;
 	}
 
@@ -1478,7 +1479,7 @@ static int rt2500usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	    !rt2x00_rf(rt2x00dev, RF2525) &&
 	    !rt2x00_rf(rt2x00dev, RF2525E) &&
 	    !rt2x00_rf(rt2x00dev, RF5222)) {
-		ERROR(rt2x00dev, "Invalid RF chipset detected.\n");
+		rt2x00_err(rt2x00dev, "Invalid RF chipset detected\n");
 		return -ENODEV;
 	}
 
diff --git a/drivers/net/wireless/rt2x00/rt2800.h b/drivers/net/wireless/rt2x00/rt2800.h
index 4db1088a847f..a7630d5ec892 100644
--- a/drivers/net/wireless/rt2x00/rt2800.h
+++ b/drivers/net/wireless/rt2x00/rt2800.h
@@ -51,6 +51,7 @@
  * RF3320 2.4G 1T1R(RT3350/RT3370/RT3390)
  * RF3322 2.4G 2T2R(RT3352/RT3371/RT3372/RT3391/RT3392)
  * RF3053 2.4G/5G 3T3R(RT3883/RT3563/RT3573/RT3593/RT3662)
+ * RF5592 2.4G/5G 2T2R
  * RF5360 2.4G 1T1R
  * RF5370 2.4G 1T1R
  * RF5390 2.4G 1T1R
@@ -68,6 +69,7 @@
 #define RF3320				0x000b
 #define RF3322				0x000c
 #define RF3053				0x000d
+#define RF5592				0x000f
 #define RF3290				0x3290
 #define RF5360				0x5360
 #define RF5370				0x5370
@@ -88,11 +90,8 @@
 #define REV_RT3390E			0x0211
 #define REV_RT5390F			0x0502
 #define REV_RT5390R			0x1502
+#define REV_RT5592C			0x0221
 
-/*
- * Signal information.
- * Default offset is required for RSSI <-> dBm conversion.
- */
 #define DEFAULT_RSSI_OFFSET		120
 
 /*
@@ -690,6 +689,12 @@
 #define GPIO_SWITCH_7			FIELD32(0x00000080)
 
 /*
+ * FIXME: where the DEBUG_INDEX name come from?
+ */
+#define MAC_DEBUG_INDEX			0x05e8
+#define MAC_DEBUG_INDEX_XTAL		FIELD32(0x80000000)
+
+/*
  * MAC Control/Status Registers(CSR).
  * Some values are set in TU, whereas 1 TU == 1024 us.
  */
@@ -1934,6 +1939,9 @@ struct mac_iveiv_entry {
 #define BBP4_BANDWIDTH			FIELD8(0x18)
 #define BBP4_MAC_IF_CTRL		FIELD8(0x40)
 
+/* BBP27 */
+#define BBP27_RX_CHAIN_SEL		FIELD8(0x60)
+
 /*
  * BBP 47: Bandwidth
  */
@@ -1948,6 +1956,20 @@ struct mac_iveiv_entry {
 #define BBP49_UPDATE_FLAG		FIELD8(0x01)
 
 /*
+ * BBP 105:
+ * - bit0: detect SIG on primary channel only (on 40MHz bandwidth)
+ * - bit1: FEQ (Feed Forward Compensation) for independend streams
+ * - bit2: MLD (Maximum Likehood Detection) for 2 streams (reserved on single
+ *	   stream)
+ * - bit4: channel estimation updates based on remodulation of
+ *	   L-SIG and HT-SIG symbols
+ */
+#define BBP105_DETECT_SIG_ON_PRIMARY	FIELD8(0x01)
+#define BBP105_FEQ			FIELD8(0x02)
+#define BBP105_MLD			FIELD8(0x04)
+#define BBP105_SIG_REMODULATION		FIELD8(0x08)
+
+/*
  * BBP 109
  */
 #define BBP109_TX0_POWER		FIELD8(0x0f)
@@ -1967,6 +1989,11 @@ struct mac_iveiv_entry {
 #define BBP152_RX_DEFAULT_ANT		FIELD8(0x80)
 
 /*
+ * BBP 254: unknown
+ */
+#define BBP254_BIT7			FIELD8(0x80)
+
+/*
  * RFCSR registers
  * The wordsize of the RFCSR is 8 bits.
  */
@@ -2022,9 +2049,18 @@ struct mac_iveiv_entry {
 #define RFCSR7_BITS67			FIELD8(0xc0)
 
 /*
+ * RFCSR 9:
+ */
+#define RFCSR9_K			FIELD8(0x0f)
+#define RFCSR9_N			FIELD8(0x10)
+#define RFCSR9_UNKNOWN			FIELD8(0x60)
+#define RFCSR9_MOD			FIELD8(0x80)
+
+/*
  * RFCSR 11:
  */
 #define RFCSR11_R			FIELD8(0x03)
+#define RFCSR11_MOD			FIELD8(0xc0)
 
 /*
  * RFCSR 12:
@@ -2130,11 +2166,13 @@ struct mac_iveiv_entry {
  * RFCSR 49:
  */
 #define RFCSR49_TX			FIELD8(0x3f)
+#define RFCSR49_EP			FIELD8(0xc0)
 
 /*
  * RFCSR 50:
  */
 #define RFCSR50_TX			FIELD8(0x3f)
+#define RFCSR50_EP			FIELD8(0xc0)
 
 /*
  * RF registers
@@ -2497,6 +2535,61 @@ struct mac_iveiv_entry {
 #define EEPROM_BBP_REG_ID		FIELD16(0xff00)
 
 /*
+ * EEPROM IQ Calibration, unlike other entries those are byte addresses.
+ */
+
+#define EEPROM_IQ_GAIN_CAL_TX0_2G			0x130
+#define EEPROM_IQ_PHASE_CAL_TX0_2G			0x131
+#define EEPROM_IQ_GROUPDELAY_CAL_TX0_2G			0x132
+#define EEPROM_IQ_GAIN_CAL_TX1_2G			0x133
+#define EEPROM_IQ_PHASE_CAL_TX1_2G			0x134
+#define EEPROM_IQ_GROUPDELAY_CAL_TX1_2G			0x135
+#define EEPROM_IQ_GAIN_CAL_RX0_2G			0x136
+#define EEPROM_IQ_PHASE_CAL_RX0_2G			0x137
+#define EEPROM_IQ_GROUPDELAY_CAL_RX0_2G			0x138
+#define EEPROM_IQ_GAIN_CAL_RX1_2G			0x139
+#define EEPROM_IQ_PHASE_CAL_RX1_2G			0x13A
+#define EEPROM_IQ_GROUPDELAY_CAL_RX1_2G			0x13B
+#define EEPROM_RF_IQ_COMPENSATION_CONTROL		0x13C
+#define EEPROM_RF_IQ_IMBALANCE_COMPENSATION_CONTROL	0x13D
+#define EEPROM_IQ_GAIN_CAL_TX0_CH36_TO_CH64_5G		0x144
+#define EEPROM_IQ_PHASE_CAL_TX0_CH36_TO_CH64_5G		0x145
+#define EEPROM_IQ_GAIN_CAL_TX0_CH100_TO_CH138_5G	0X146
+#define EEPROM_IQ_PHASE_CAL_TX0_CH100_TO_CH138_5G	0x147
+#define EEPROM_IQ_GAIN_CAL_TX0_CH140_TO_CH165_5G	0x148
+#define EEPROM_IQ_PHASE_CAL_TX0_CH140_TO_CH165_5G	0x149
+#define EEPROM_IQ_GAIN_CAL_TX1_CH36_TO_CH64_5G		0x14A
+#define EEPROM_IQ_PHASE_CAL_TX1_CH36_TO_CH64_5G		0x14B
+#define EEPROM_IQ_GAIN_CAL_TX1_CH100_TO_CH138_5G	0X14C
+#define EEPROM_IQ_PHASE_CAL_TX1_CH100_TO_CH138_5G	0x14D
+#define EEPROM_IQ_GAIN_CAL_TX1_CH140_TO_CH165_5G	0x14E
+#define EEPROM_IQ_PHASE_CAL_TX1_CH140_TO_CH165_5G	0x14F
+#define EEPROM_IQ_GROUPDELAY_CAL_TX0_CH36_TO_CH64_5G	0x150
+#define EEPROM_IQ_GROUPDELAY_CAL_TX1_CH36_TO_CH64_5G	0x151
+#define EEPROM_IQ_GROUPDELAY_CAL_TX0_CH100_TO_CH138_5G	0x152
+#define EEPROM_IQ_GROUPDELAY_CAL_TX1_CH100_TO_CH138_5G	0x153
+#define EEPROM_IQ_GROUPDELAY_CAL_TX0_CH140_TO_CH165_5G	0x154
+#define EEPROM_IQ_GROUPDELAY_CAL_TX1_CH140_TO_CH165_5G	0x155
+#define EEPROM_IQ_GAIN_CAL_RX0_CH36_TO_CH64_5G		0x156
+#define EEPROM_IQ_PHASE_CAL_RX0_CH36_TO_CH64_5G		0x157
+#define EEPROM_IQ_GAIN_CAL_RX0_CH100_TO_CH138_5G	0X158
+#define EEPROM_IQ_PHASE_CAL_RX0_CH100_TO_CH138_5G	0x159
+#define EEPROM_IQ_GAIN_CAL_RX0_CH140_TO_CH165_5G	0x15A
+#define EEPROM_IQ_PHASE_CAL_RX0_CH140_TO_CH165_5G	0x15B
+#define EEPROM_IQ_GAIN_CAL_RX1_CH36_TO_CH64_5G		0x15C
+#define EEPROM_IQ_PHASE_CAL_RX1_CH36_TO_CH64_5G		0x15D
+#define EEPROM_IQ_GAIN_CAL_RX1_CH100_TO_CH138_5G	0X15E
+#define EEPROM_IQ_PHASE_CAL_RX1_CH100_TO_CH138_5G	0x15F
+#define EEPROM_IQ_GAIN_CAL_RX1_CH140_TO_CH165_5G	0x160
+#define EEPROM_IQ_PHASE_CAL_RX1_CH140_TO_CH165_5G	0x161
+#define EEPROM_IQ_GROUPDELAY_CAL_RX0_CH36_TO_CH64_5G	0x162
+#define EEPROM_IQ_GROUPDELAY_CAL_RX1_CH36_TO_CH64_5G	0x163
+#define EEPROM_IQ_GROUPDELAY_CAL_RX0_CH100_TO_CH138_5G	0x164
+#define EEPROM_IQ_GROUPDELAY_CAL_RX1_CH100_TO_CH138_5G	0x165
+#define EEPROM_IQ_GROUPDELAY_CAL_RX0_CH140_TO_CH165_5G	0x166
+#define EEPROM_IQ_GROUPDELAY_CAL_RX1_CH140_TO_CH165_5G	0x167
+
+/*
  * MCU mailbox commands.
  * MCU_SLEEP - go to power-save mode.
  *             arg1: 1: save as much power as possible, 0: save less power.
@@ -2535,6 +2628,8 @@ struct mac_iveiv_entry {
 #define TXWI_DESC_SIZE			(4 * sizeof(__le32))
 #define RXWI_DESC_SIZE			(4 * sizeof(__le32))
 
+#define TXWI_DESC_SIZE_5592		(5 * sizeof(__le32))
+#define RXWI_DESC_SIZE_5592		(6 * sizeof(__le32))
 /*
  * TX WI structure
  */
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index a658b4bc7da2..b52d70c75e1a 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -80,7 +80,7 @@ static inline bool rt2800_is_305x_soc(struct rt2x00_dev *rt2x00dev)
 	    rt2x00_rf(rt2x00dev, RF3022))
 		return true;
 
-	WARNING(rt2x00dev, "Unknown RF chipset on rt305x\n");
+	rt2x00_warn(rt2x00dev, "Unknown RF chipset on rt305x\n");
 	return false;
 }
 
@@ -328,7 +328,7 @@ int rt2800_wait_csr_ready(struct rt2x00_dev *rt2x00dev)
 		msleep(1);
 	}
 
-	ERROR(rt2x00dev, "Unstable hardware.\n");
+	rt2x00_err(rt2x00dev, "Unstable hardware\n");
 	return -EBUSY;
 }
 EXPORT_SYMBOL_GPL(rt2800_wait_csr_ready);
@@ -351,7 +351,7 @@ int rt2800_wait_wpdma_ready(struct rt2x00_dev *rt2x00dev)
 		msleep(10);
 	}
 
-	ERROR(rt2x00dev, "WPDMA TX/RX busy [0x%08x].\n", reg);
+	rt2x00_err(rt2x00dev, "WPDMA TX/RX busy [0x%08x]\n", reg);
 	return -EACCES;
 }
 EXPORT_SYMBOL_GPL(rt2800_wait_wpdma_ready);
@@ -512,7 +512,7 @@ int rt2800_load_firmware(struct rt2x00_dev *rt2x00dev,
 	}
 
 	if (i == REGISTER_BUSY_COUNT) {
-		ERROR(rt2x00dev, "PBF system register not ready.\n");
+		rt2x00_err(rt2x00dev, "PBF system register not ready\n");
 		return -EBUSY;
 	}
 
@@ -527,8 +527,10 @@ int rt2800_load_firmware(struct rt2x00_dev *rt2x00dev,
 	 */
 	rt2800_register_write(rt2x00dev, H2M_BBP_AGENT, 0);
 	rt2800_register_write(rt2x00dev, H2M_MAILBOX_CSR, 0);
-	if (rt2x00_is_usb(rt2x00dev))
+	if (rt2x00_is_usb(rt2x00dev)) {
 		rt2800_register_write(rt2x00dev, H2M_INT_SRC, 0);
+		rt2800_mcu_request(rt2x00dev, MCU_BOOT_SIGNAL, 0, 0, 0);
+	}
 	msleep(1);
 
 	return 0;
@@ -540,6 +542,7 @@ void rt2800_write_tx_data(struct queue_entry *entry,
 {
 	__le32 *txwi = rt2800_drv_get_txwi(entry);
 	u32 word;
+	int i;
 
 	/*
 	 * Initialize TX Info descriptor
@@ -582,14 +585,16 @@ void rt2800_write_tx_data(struct queue_entry *entry,
 	rt2x00_desc_write(txwi, 1, word);
 
 	/*
-	 * Always write 0 to IV/EIV fields, hardware will insert the IV
-	 * from the IVEIV register when TXD_W3_WIV is set to 0.
+	 * Always write 0 to IV/EIV fields (word 2 and 3), hardware will insert
+	 * the IV from the IVEIV register when TXD_W3_WIV is set to 0.
 	 * When TXD_W3_WIV is set to 1 it will use the IV data
 	 * from the descriptor. The TXWI_W1_WIRELESS_CLI_ID indicates which
 	 * crypto entry in the registers should be used to encrypt the frame.
+	 *
+	 * Nulify all remaining words as well, we don't know how to program them.
 	 */
-	_rt2x00_desc_write(txwi, 2, 0 /* skbdesc->iv[0] */);
-	_rt2x00_desc_write(txwi, 3, 0 /* skbdesc->iv[1] */);
+	for (i = 2; i < entry->queue->winfo_size / sizeof(__le32); i++)
+		_rt2x00_desc_write(txwi, i, 0);
 }
 EXPORT_SYMBOL_GPL(rt2800_write_tx_data);
 
@@ -674,11 +679,10 @@ void rt2800_process_rxwi(struct queue_entry *entry,
 	 * Convert descriptor AGC value to RSSI value.
 	 */
 	rxdesc->rssi = rt2800_agc_to_rssi(entry->queue->rt2x00dev, word);
-
 	/*
-	 * Remove RXWI descriptor from start of buffer.
+	 * Remove RXWI descriptor from start of the buffer.
 	 */
-	skb_pull(entry->skb, RXWI_DESC_SIZE);
+	skb_pull(entry->skb, entry->queue->winfo_size);
 }
 EXPORT_SYMBOL_GPL(rt2800_process_rxwi);
 
@@ -769,6 +773,7 @@ void rt2800_write_beacon(struct queue_entry *entry, struct txentry_desc *txdesc)
 	unsigned int beacon_base;
 	unsigned int padding_len;
 	u32 orig_reg, reg;
+	const int txwi_desc_size = entry->queue->winfo_size;
 
 	/*
 	 * Disable beaconing while we are reloading the beacon data,
@@ -782,14 +787,14 @@ void rt2800_write_beacon(struct queue_entry *entry, struct txentry_desc *txdesc)
 	/*
 	 * Add space for the TXWI in front of the skb.
 	 */
-	memset(skb_push(entry->skb, TXWI_DESC_SIZE), 0, TXWI_DESC_SIZE);
+	memset(skb_push(entry->skb, txwi_desc_size), 0, txwi_desc_size);
 
 	/*
 	 * Register descriptor details in skb frame descriptor.
 	 */
 	skbdesc->flags |= SKBDESC_DESC_IN_SKB;
 	skbdesc->desc = entry->skb->data;
-	skbdesc->desc_len = TXWI_DESC_SIZE;
+	skbdesc->desc_len = txwi_desc_size;
 
 	/*
 	 * Add the TXWI for the beacon to the skb.
@@ -806,7 +811,7 @@ void rt2800_write_beacon(struct queue_entry *entry, struct txentry_desc *txdesc)
 	 */
 	padding_len = roundup(entry->skb->len, 4) - entry->skb->len;
 	if (padding_len && skb_pad(entry->skb, padding_len)) {
-		ERROR(rt2x00dev, "Failure padding beacon, aborting\n");
+		rt2x00_err(rt2x00dev, "Failure padding beacon, aborting\n");
 		/* skb freed by skb_pad() on failure */
 		entry->skb = NULL;
 		rt2800_register_write(rt2x00dev, BCN_TIME_CFG, orig_reg);
@@ -835,13 +840,14 @@ static inline void rt2800_clear_beacon_register(struct rt2x00_dev *rt2x00dev,
 						unsigned int beacon_base)
 {
 	int i;
+	const int txwi_desc_size = rt2x00dev->ops->bcn->winfo_size;
 
 	/*
 	 * For the Beacon base registers we only need to clear
 	 * the whole TXWI which (when set to 0) will invalidate
 	 * the entire beacon.
 	 */
-	for (i = 0; i < TXWI_DESC_SIZE; i += sizeof(__le32))
+	for (i = 0; i < txwi_desc_size; i += sizeof(__le32))
 		rt2800_register_write(rt2x00dev, beacon_base + i, 0);
 }
 
@@ -1988,8 +1994,21 @@ static void rt2800_config_channel_rf3052(struct rt2x00_dev *rt2x00dev,
 }
 
 #define POWER_BOUND		0x27
+#define POWER_BOUND_5G		0x2b
 #define FREQ_OFFSET_BOUND	0x5f
 
+static void rt2800_adjust_freq_offset(struct rt2x00_dev *rt2x00dev)
+{
+	u8 rfcsr;
+
+	rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
+	if (rt2x00dev->freq_offset > FREQ_OFFSET_BOUND)
+		rt2x00_set_field8(&rfcsr, RFCSR17_CODE, FREQ_OFFSET_BOUND);
+	else
+		rt2x00_set_field8(&rfcsr, RFCSR17_CODE, rt2x00dev->freq_offset);
+	rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
+}
+
 static void rt2800_config_channel_rf3290(struct rt2x00_dev *rt2x00dev,
 					 struct ieee80211_conf *conf,
 					 struct rf_channel *rf,
@@ -2010,12 +2029,7 @@ static void rt2800_config_channel_rf3290(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field8(&rfcsr, RFCSR49_TX, info->default_power1);
 	rt2800_rfcsr_write(rt2x00dev, 49, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
-	if (rt2x00dev->freq_offset > FREQ_OFFSET_BOUND)
-		rt2x00_set_field8(&rfcsr, RFCSR17_CODE, FREQ_OFFSET_BOUND);
-	else
-		rt2x00_set_field8(&rfcsr, RFCSR17_CODE, rt2x00dev->freq_offset);
-	rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
+	rt2800_adjust_freq_offset(rt2x00dev);
 
 	if (rf->channel <= 14) {
 		if (rf->channel == 6)
@@ -2056,13 +2070,7 @@ static void rt2800_config_channel_rf3322(struct rt2x00_dev *rt2x00dev,
 	else
 		rt2800_rfcsr_write(rt2x00dev, 48, info->default_power2);
 
-	rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
-	if (rt2x00dev->freq_offset > FREQ_OFFSET_BOUND)
-		rt2x00_set_field8(&rfcsr, RFCSR17_CODE, FREQ_OFFSET_BOUND);
-	else
-		rt2x00_set_field8(&rfcsr, RFCSR17_CODE, rt2x00dev->freq_offset);
-
-	rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
+	rt2800_adjust_freq_offset(rt2x00dev);
 
 	rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
 	rt2x00_set_field8(&rfcsr, RFCSR1_RX0_PD, 1);
@@ -2127,12 +2135,7 @@ static void rt2800_config_channel_rf53xx(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field8(&rfcsr, RFCSR1_TX0_PD, 1);
 	rt2800_rfcsr_write(rt2x00dev, 1, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
-	if (rt2x00dev->freq_offset > FREQ_OFFSET_BOUND)
-		rt2x00_set_field8(&rfcsr, RFCSR17_CODE, FREQ_OFFSET_BOUND);
-	else
-		rt2x00_set_field8(&rfcsr, RFCSR17_CODE, rt2x00dev->freq_offset);
-	rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
+	rt2800_adjust_freq_offset(rt2x00dev);
 
 	if (rf->channel <= 14) {
 		int idx = rf->channel-1;
@@ -2184,6 +2187,382 @@ static void rt2800_config_channel_rf53xx(struct rt2x00_dev *rt2x00dev,
 	}
 }
 
+static void rt2800_config_channel_rf55xx(struct rt2x00_dev *rt2x00dev,
+					 struct ieee80211_conf *conf,
+					 struct rf_channel *rf,
+					 struct channel_info *info)
+{
+	u8 rfcsr, ep_reg;
+	u32 reg;
+	int power_bound;
+
+	/* TODO */
+	const bool is_11b = false;
+	const bool is_type_ep = false;
+
+	rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
+	rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL,
+			   (rf->channel > 14 || conf_is_ht40(conf)) ? 5 : 0);
+	rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
+
+	/* Order of values on rf_channel entry: N, K, mod, R */
+	rt2800_rfcsr_write(rt2x00dev, 8, rf->rf1 & 0xff);
+
+	rt2800_rfcsr_read(rt2x00dev,  9, &rfcsr);
+	rt2x00_set_field8(&rfcsr, RFCSR9_K, rf->rf2 & 0xf);
+	rt2x00_set_field8(&rfcsr, RFCSR9_N, (rf->rf1 & 0x100) >> 8);
+	rt2x00_set_field8(&rfcsr, RFCSR9_MOD, ((rf->rf3 - 8) & 0x4) >> 2);
+	rt2800_rfcsr_write(rt2x00dev, 9, rfcsr);
+
+	rt2800_rfcsr_read(rt2x00dev, 11, &rfcsr);
+	rt2x00_set_field8(&rfcsr, RFCSR11_R, rf->rf4 - 1);
+	rt2x00_set_field8(&rfcsr, RFCSR11_MOD, (rf->rf3 - 8) & 0x3);
+	rt2800_rfcsr_write(rt2x00dev, 11, rfcsr);
+
+	if (rf->channel <= 14) {
+		rt2800_rfcsr_write(rt2x00dev, 10, 0x90);
+		/* FIXME: RF11 owerwrite ? */
+		rt2800_rfcsr_write(rt2x00dev, 11, 0x4A);
+		rt2800_rfcsr_write(rt2x00dev, 12, 0x52);
+		rt2800_rfcsr_write(rt2x00dev, 13, 0x42);
+		rt2800_rfcsr_write(rt2x00dev, 22, 0x40);
+		rt2800_rfcsr_write(rt2x00dev, 24, 0x4A);
+		rt2800_rfcsr_write(rt2x00dev, 25, 0x80);
+		rt2800_rfcsr_write(rt2x00dev, 27, 0x42);
+		rt2800_rfcsr_write(rt2x00dev, 36, 0x80);
+		rt2800_rfcsr_write(rt2x00dev, 37, 0x08);
+		rt2800_rfcsr_write(rt2x00dev, 38, 0x89);
+		rt2800_rfcsr_write(rt2x00dev, 39, 0x1B);
+		rt2800_rfcsr_write(rt2x00dev, 40, 0x0D);
+		rt2800_rfcsr_write(rt2x00dev, 41, 0x9B);
+		rt2800_rfcsr_write(rt2x00dev, 42, 0xD5);
+		rt2800_rfcsr_write(rt2x00dev, 43, 0x72);
+		rt2800_rfcsr_write(rt2x00dev, 44, 0x0E);
+		rt2800_rfcsr_write(rt2x00dev, 45, 0xA2);
+		rt2800_rfcsr_write(rt2x00dev, 46, 0x6B);
+		rt2800_rfcsr_write(rt2x00dev, 48, 0x10);
+		rt2800_rfcsr_write(rt2x00dev, 51, 0x3E);
+		rt2800_rfcsr_write(rt2x00dev, 52, 0x48);
+		rt2800_rfcsr_write(rt2x00dev, 54, 0x38);
+		rt2800_rfcsr_write(rt2x00dev, 56, 0xA1);
+		rt2800_rfcsr_write(rt2x00dev, 57, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 58, 0x39);
+		rt2800_rfcsr_write(rt2x00dev, 60, 0x45);
+		rt2800_rfcsr_write(rt2x00dev, 61, 0x91);
+		rt2800_rfcsr_write(rt2x00dev, 62, 0x39);
+
+		/* TODO RF27 <- tssi */
+
+		rfcsr = rf->channel <= 10 ? 0x07 : 0x06;
+		rt2800_rfcsr_write(rt2x00dev, 23, rfcsr);
+		rt2800_rfcsr_write(rt2x00dev, 59, rfcsr);
+
+		if (is_11b) {
+			/* CCK */
+			rt2800_rfcsr_write(rt2x00dev, 31, 0xF8);
+			rt2800_rfcsr_write(rt2x00dev, 32, 0xC0);
+			if (is_type_ep)
+				rt2800_rfcsr_write(rt2x00dev, 55, 0x06);
+			else
+				rt2800_rfcsr_write(rt2x00dev, 55, 0x47);
+		} else {
+			/* OFDM */
+			if (is_type_ep)
+				rt2800_rfcsr_write(rt2x00dev, 55, 0x03);
+			else
+				rt2800_rfcsr_write(rt2x00dev, 55, 0x43);
+		}
+
+		power_bound = POWER_BOUND;
+		ep_reg = 0x2;
+	} else {
+		rt2800_rfcsr_write(rt2x00dev, 10, 0x97);
+		/* FIMXE: RF11 overwrite */
+		rt2800_rfcsr_write(rt2x00dev, 11, 0x40);
+		rt2800_rfcsr_write(rt2x00dev, 25, 0xBF);
+		rt2800_rfcsr_write(rt2x00dev, 27, 0x42);
+		rt2800_rfcsr_write(rt2x00dev, 36, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 37, 0x04);
+		rt2800_rfcsr_write(rt2x00dev, 38, 0x85);
+		rt2800_rfcsr_write(rt2x00dev, 40, 0x42);
+		rt2800_rfcsr_write(rt2x00dev, 41, 0xBB);
+		rt2800_rfcsr_write(rt2x00dev, 42, 0xD7);
+		rt2800_rfcsr_write(rt2x00dev, 45, 0x41);
+		rt2800_rfcsr_write(rt2x00dev, 48, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 57, 0x77);
+		rt2800_rfcsr_write(rt2x00dev, 60, 0x05);
+		rt2800_rfcsr_write(rt2x00dev, 61, 0x01);
+
+		/* TODO RF27 <- tssi */
+
+		if (rf->channel >= 36 && rf->channel <= 64) {
+
+			rt2800_rfcsr_write(rt2x00dev, 12, 0x2E);
+			rt2800_rfcsr_write(rt2x00dev, 13, 0x22);
+			rt2800_rfcsr_write(rt2x00dev, 22, 0x60);
+			rt2800_rfcsr_write(rt2x00dev, 23, 0x7F);
+			if (rf->channel <= 50)
+				rt2800_rfcsr_write(rt2x00dev, 24, 0x09);
+			else if (rf->channel >= 52)
+				rt2800_rfcsr_write(rt2x00dev, 24, 0x07);
+			rt2800_rfcsr_write(rt2x00dev, 39, 0x1C);
+			rt2800_rfcsr_write(rt2x00dev, 43, 0x5B);
+			rt2800_rfcsr_write(rt2x00dev, 44, 0X40);
+			rt2800_rfcsr_write(rt2x00dev, 46, 0X00);
+			rt2800_rfcsr_write(rt2x00dev, 51, 0xFE);
+			rt2800_rfcsr_write(rt2x00dev, 52, 0x0C);
+			rt2800_rfcsr_write(rt2x00dev, 54, 0xF8);
+			if (rf->channel <= 50) {
+				rt2800_rfcsr_write(rt2x00dev, 55, 0x06),
+				rt2800_rfcsr_write(rt2x00dev, 56, 0xD3);
+			} else if (rf->channel >= 52) {
+				rt2800_rfcsr_write(rt2x00dev, 55, 0x04);
+				rt2800_rfcsr_write(rt2x00dev, 56, 0xBB);
+			}
+
+			rt2800_rfcsr_write(rt2x00dev, 58, 0x15);
+			rt2800_rfcsr_write(rt2x00dev, 59, 0x7F);
+			rt2800_rfcsr_write(rt2x00dev, 62, 0x15);
+
+		} else if (rf->channel >= 100 && rf->channel <= 165) {
+
+			rt2800_rfcsr_write(rt2x00dev, 12, 0x0E);
+			rt2800_rfcsr_write(rt2x00dev, 13, 0x42);
+			rt2800_rfcsr_write(rt2x00dev, 22, 0x40);
+			if (rf->channel <= 153) {
+				rt2800_rfcsr_write(rt2x00dev, 23, 0x3C);
+				rt2800_rfcsr_write(rt2x00dev, 24, 0x06);
+			} else if (rf->channel >= 155) {
+				rt2800_rfcsr_write(rt2x00dev, 23, 0x38);
+				rt2800_rfcsr_write(rt2x00dev, 24, 0x05);
+			}
+			if (rf->channel <= 138) {
+				rt2800_rfcsr_write(rt2x00dev, 39, 0x1A);
+				rt2800_rfcsr_write(rt2x00dev, 43, 0x3B);
+				rt2800_rfcsr_write(rt2x00dev, 44, 0x20);
+				rt2800_rfcsr_write(rt2x00dev, 46, 0x18);
+			} else if (rf->channel >= 140) {
+				rt2800_rfcsr_write(rt2x00dev, 39, 0x18);
+				rt2800_rfcsr_write(rt2x00dev, 43, 0x1B);
+				rt2800_rfcsr_write(rt2x00dev, 44, 0x10);
+				rt2800_rfcsr_write(rt2x00dev, 46, 0X08);
+			}
+			if (rf->channel <= 124)
+				rt2800_rfcsr_write(rt2x00dev, 51, 0xFC);
+			else if (rf->channel >= 126)
+				rt2800_rfcsr_write(rt2x00dev, 51, 0xEC);
+			if (rf->channel <= 138)
+				rt2800_rfcsr_write(rt2x00dev, 52, 0x06);
+			else if (rf->channel >= 140)
+				rt2800_rfcsr_write(rt2x00dev, 52, 0x06);
+			rt2800_rfcsr_write(rt2x00dev, 54, 0xEB);
+			if (rf->channel <= 138)
+				rt2800_rfcsr_write(rt2x00dev, 55, 0x01);
+			else if (rf->channel >= 140)
+				rt2800_rfcsr_write(rt2x00dev, 55, 0x00);
+			if (rf->channel <= 128)
+				rt2800_rfcsr_write(rt2x00dev, 56, 0xBB);
+			else if (rf->channel >= 130)
+				rt2800_rfcsr_write(rt2x00dev, 56, 0xAB);
+			if (rf->channel <= 116)
+				rt2800_rfcsr_write(rt2x00dev, 58, 0x1D);
+			else if (rf->channel >= 118)
+				rt2800_rfcsr_write(rt2x00dev, 58, 0x15);
+			if (rf->channel <= 138)
+				rt2800_rfcsr_write(rt2x00dev, 59, 0x3F);
+			else if (rf->channel >= 140)
+				rt2800_rfcsr_write(rt2x00dev, 59, 0x7C);
+			if (rf->channel <= 116)
+				rt2800_rfcsr_write(rt2x00dev, 62, 0x1D);
+			else if (rf->channel >= 118)
+				rt2800_rfcsr_write(rt2x00dev, 62, 0x15);
+		}
+
+		power_bound = POWER_BOUND_5G;
+		ep_reg = 0x3;
+	}
+
+	rt2800_rfcsr_read(rt2x00dev, 49, &rfcsr);
+	if (info->default_power1 > power_bound)
+		rt2x00_set_field8(&rfcsr, RFCSR49_TX, power_bound);
+	else
+		rt2x00_set_field8(&rfcsr, RFCSR49_TX, info->default_power1);
+	if (is_type_ep)
+		rt2x00_set_field8(&rfcsr, RFCSR49_EP, ep_reg);
+	rt2800_rfcsr_write(rt2x00dev, 49, rfcsr);
+
+	rt2800_rfcsr_read(rt2x00dev, 50, &rfcsr);
+	if (info->default_power1 > power_bound)
+		rt2x00_set_field8(&rfcsr, RFCSR50_TX, power_bound);
+	else
+		rt2x00_set_field8(&rfcsr, RFCSR50_TX, info->default_power2);
+	if (is_type_ep)
+		rt2x00_set_field8(&rfcsr, RFCSR50_EP, ep_reg);
+	rt2800_rfcsr_write(rt2x00dev, 50, rfcsr);
+
+	rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
+	rt2x00_set_field8(&rfcsr, RFCSR1_RF_BLOCK_EN, 1);
+	rt2x00_set_field8(&rfcsr, RFCSR1_PLL_PD, 1);
+
+	rt2x00_set_field8(&rfcsr, RFCSR1_TX0_PD,
+			  rt2x00dev->default_ant.tx_chain_num >= 1);
+	rt2x00_set_field8(&rfcsr, RFCSR1_TX1_PD,
+			  rt2x00dev->default_ant.tx_chain_num == 2);
+	rt2x00_set_field8(&rfcsr, RFCSR1_TX2_PD, 0);
+
+	rt2x00_set_field8(&rfcsr, RFCSR1_RX0_PD,
+			  rt2x00dev->default_ant.rx_chain_num >= 1);
+	rt2x00_set_field8(&rfcsr, RFCSR1_RX1_PD,
+			  rt2x00dev->default_ant.rx_chain_num == 2);
+	rt2x00_set_field8(&rfcsr, RFCSR1_RX2_PD, 0);
+
+	rt2800_rfcsr_write(rt2x00dev, 1, rfcsr);
+	rt2800_rfcsr_write(rt2x00dev, 6, 0xe4);
+
+	if (conf_is_ht40(conf))
+		rt2800_rfcsr_write(rt2x00dev, 30, 0x16);
+	else
+		rt2800_rfcsr_write(rt2x00dev, 30, 0x10);
+
+	if (!is_11b) {
+		rt2800_rfcsr_write(rt2x00dev, 31, 0x80);
+		rt2800_rfcsr_write(rt2x00dev, 32, 0x80);
+	}
+
+	/* TODO proper frequency adjustment */
+	rt2800_adjust_freq_offset(rt2x00dev);
+
+	/* TODO merge with others */
+	rt2800_rfcsr_read(rt2x00dev, 3, &rfcsr);
+	rt2x00_set_field8(&rfcsr, RFCSR3_VCOCAL_EN, 1);
+	rt2800_rfcsr_write(rt2x00dev, 3, rfcsr);
+
+	/* BBP settings */
+	rt2800_bbp_write(rt2x00dev, 62, 0x37 - rt2x00dev->lna_gain);
+	rt2800_bbp_write(rt2x00dev, 63, 0x37 - rt2x00dev->lna_gain);
+	rt2800_bbp_write(rt2x00dev, 64, 0x37 - rt2x00dev->lna_gain);
+
+	rt2800_bbp_write(rt2x00dev, 79, (rf->channel <= 14) ? 0x1C : 0x18);
+	rt2800_bbp_write(rt2x00dev, 80, (rf->channel <= 14) ? 0x0E : 0x08);
+	rt2800_bbp_write(rt2x00dev, 81, (rf->channel <= 14) ? 0x3A : 0x38);
+	rt2800_bbp_write(rt2x00dev, 82, (rf->channel <= 14) ? 0x62 : 0x92);
+
+	/* GLRT band configuration */
+	rt2800_bbp_write(rt2x00dev, 195, 128);
+	rt2800_bbp_write(rt2x00dev, 196, (rf->channel <= 14) ? 0xE0 : 0xF0);
+	rt2800_bbp_write(rt2x00dev, 195, 129);
+	rt2800_bbp_write(rt2x00dev, 196, (rf->channel <= 14) ? 0x1F : 0x1E);
+	rt2800_bbp_write(rt2x00dev, 195, 130);
+	rt2800_bbp_write(rt2x00dev, 196, (rf->channel <= 14) ? 0x38 : 0x28);
+	rt2800_bbp_write(rt2x00dev, 195, 131);
+	rt2800_bbp_write(rt2x00dev, 196, (rf->channel <= 14) ? 0x32 : 0x20);
+	rt2800_bbp_write(rt2x00dev, 195, 133);
+	rt2800_bbp_write(rt2x00dev, 196, (rf->channel <= 14) ? 0x28 : 0x7F);
+	rt2800_bbp_write(rt2x00dev, 195, 124);
+	rt2800_bbp_write(rt2x00dev, 196, (rf->channel <= 14) ? 0x19 : 0x7F);
+}
+
+static void rt2800_bbp_write_with_rx_chain(struct rt2x00_dev *rt2x00dev,
+					   const unsigned int word,
+					   const u8 value)
+{
+	u8 chain, reg;
+
+	for (chain = 0; chain < rt2x00dev->default_ant.rx_chain_num; chain++) {
+		rt2800_bbp_read(rt2x00dev, 27, &reg);
+		rt2x00_set_field8(&reg,  BBP27_RX_CHAIN_SEL, chain);
+		rt2800_bbp_write(rt2x00dev, 27, reg);
+
+		rt2800_bbp_write(rt2x00dev, word, value);
+	}
+}
+
+static void rt2800_iq_calibrate(struct rt2x00_dev *rt2x00dev, int channel)
+{
+	u8 cal;
+
+	/* TX0 IQ Gain */
+	rt2800_bbp_write(rt2x00dev, 158, 0x2c);
+	if (channel <= 14)
+		cal = rt2x00_eeprom_byte(rt2x00dev, EEPROM_IQ_GAIN_CAL_TX0_2G);
+	else if (channel >= 36 && channel <= 64)
+		cal = rt2x00_eeprom_byte(rt2x00dev,
+					 EEPROM_IQ_GAIN_CAL_TX0_CH36_TO_CH64_5G);
+	else if (channel >= 100 && channel <= 138)
+		cal = rt2x00_eeprom_byte(rt2x00dev,
+					 EEPROM_IQ_GAIN_CAL_TX0_CH100_TO_CH138_5G);
+	else if (channel >= 140 && channel <= 165)
+		cal = rt2x00_eeprom_byte(rt2x00dev,
+					 EEPROM_IQ_GAIN_CAL_TX0_CH140_TO_CH165_5G);
+	else
+		cal = 0;
+	rt2800_bbp_write(rt2x00dev, 159, cal);
+
+	/* TX0 IQ Phase */
+	rt2800_bbp_write(rt2x00dev, 158, 0x2d);
+	if (channel <= 14)
+		cal = rt2x00_eeprom_byte(rt2x00dev, EEPROM_IQ_PHASE_CAL_TX0_2G);
+	else if (channel >= 36 && channel <= 64)
+		cal = rt2x00_eeprom_byte(rt2x00dev,
+					 EEPROM_IQ_PHASE_CAL_TX0_CH36_TO_CH64_5G);
+	else if (channel >= 100 && channel <= 138)
+		cal = rt2x00_eeprom_byte(rt2x00dev,
+					 EEPROM_IQ_PHASE_CAL_TX0_CH100_TO_CH138_5G);
+	else if (channel >= 140 && channel <= 165)
+		cal = rt2x00_eeprom_byte(rt2x00dev,
+					 EEPROM_IQ_PHASE_CAL_TX0_CH140_TO_CH165_5G);
+	else
+		cal = 0;
+	rt2800_bbp_write(rt2x00dev, 159, cal);
+
+	/* TX1 IQ Gain */
+	rt2800_bbp_write(rt2x00dev, 158, 0x4a);
+	if (channel <= 14)
+		cal = rt2x00_eeprom_byte(rt2x00dev, EEPROM_IQ_GAIN_CAL_TX1_2G);
+	else if (channel >= 36 && channel <= 64)
+		cal = rt2x00_eeprom_byte(rt2x00dev,
+					 EEPROM_IQ_GAIN_CAL_TX1_CH36_TO_CH64_5G);
+	else if (channel >= 100 && channel <= 138)
+		cal = rt2x00_eeprom_byte(rt2x00dev,
+					 EEPROM_IQ_GAIN_CAL_TX1_CH100_TO_CH138_5G);
+	else if (channel >= 140 && channel <= 165)
+		cal = rt2x00_eeprom_byte(rt2x00dev,
+					 EEPROM_IQ_GAIN_CAL_TX1_CH140_TO_CH165_5G);
+	else
+		cal = 0;
+	rt2800_bbp_write(rt2x00dev, 159, cal);
+
+	/* TX1 IQ Phase */
+	rt2800_bbp_write(rt2x00dev, 158, 0x4b);
+	if (channel <= 14)
+		cal = rt2x00_eeprom_byte(rt2x00dev, EEPROM_IQ_PHASE_CAL_TX1_2G);
+	else if (channel >= 36 && channel <= 64)
+		cal = rt2x00_eeprom_byte(rt2x00dev,
+					 EEPROM_IQ_PHASE_CAL_TX1_CH36_TO_CH64_5G);
+	else if (channel >= 100 && channel <= 138)
+		cal = rt2x00_eeprom_byte(rt2x00dev,
+					 EEPROM_IQ_PHASE_CAL_TX1_CH100_TO_CH138_5G);
+	else if (channel >= 140 && channel <= 165)
+		cal = rt2x00_eeprom_byte(rt2x00dev,
+					 EEPROM_IQ_PHASE_CAL_TX1_CH140_TO_CH165_5G);
+	else
+		cal = 0;
+	rt2800_bbp_write(rt2x00dev, 159, cal);
+
+	/* FIXME: possible RX0, RX1 callibration ? */
+
+	/* RF IQ compensation control */
+	rt2800_bbp_write(rt2x00dev, 158, 0x04);
+	cal = rt2x00_eeprom_byte(rt2x00dev, EEPROM_RF_IQ_COMPENSATION_CONTROL);
+	rt2800_bbp_write(rt2x00dev, 159, cal != 0xff ? cal : 0);
+
+	/* RF IQ imbalance compensation control */
+	rt2800_bbp_write(rt2x00dev, 158, 0x03);
+	cal = rt2x00_eeprom_byte(rt2x00dev,
+				 EEPROM_RF_IQ_IMBALANCE_COMPENSATION_CONTROL);
+	rt2800_bbp_write(rt2x00dev, 159, cal != 0xff ? cal : 0);
+}
+
 static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 				  struct ieee80211_conf *conf,
 				  struct rf_channel *rf,
@@ -2225,6 +2604,9 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 	case RF5392:
 		rt2800_config_channel_rf53xx(rt2x00dev, conf, rf, info);
 		break;
+	case RF5592:
+		rt2800_config_channel_rf55xx(rt2x00dev, conf, rf, info);
+		break;
 	default:
 		rt2800_config_channel_rf2xxx(rt2x00dev, conf, rf, info);
 	}
@@ -2326,6 +2708,17 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 	if (rt2x00_rt(rt2x00dev, RT3572))
 		rt2800_rfcsr_write(rt2x00dev, 8, 0x80);
 
+	if (rt2x00_rt(rt2x00dev, RT5592)) {
+		rt2800_bbp_write(rt2x00dev, 195, 141);
+		rt2800_bbp_write(rt2x00dev, 196, conf_is_ht40(conf) ? 0x10 : 0x1a);
+
+		/* AGC init */
+		reg = (rf->channel <= 14 ? 0x1c : 0x24) + 2 * rt2x00dev->lna_gain;
+		rt2800_bbp_write_with_rx_chain(rt2x00dev, 66, reg);
+
+		rt2800_iq_calibrate(rt2x00dev, rf->channel);
+	}
+
 	rt2800_bbp_read(rt2x00dev, 4, &bbp);
 	rt2x00_set_field8(&bbp, BBP4_BANDWIDTH, 2 * conf_is_ht40(conf));
 	rt2800_bbp_write(rt2x00dev, 4, bbp);
@@ -2763,7 +3156,7 @@ static void rt2800_config_txpower(struct rt2x00_dev *rt2x00dev,
 
 void rt2800_gain_calibration(struct rt2x00_dev *rt2x00dev)
 {
-	rt2800_config_txpower(rt2x00dev, rt2x00dev->hw->conf.channel,
+	rt2800_config_txpower(rt2x00dev, rt2x00dev->hw->conf.chandef.chan,
 			      rt2x00dev->tx_power);
 }
 EXPORT_SYMBOL_GPL(rt2800_gain_calibration);
@@ -2898,11 +3291,11 @@ void rt2800_config(struct rt2x00_dev *rt2x00dev,
 	if (flags & IEEE80211_CONF_CHANGE_CHANNEL) {
 		rt2800_config_channel(rt2x00dev, libconf->conf,
 				      &libconf->rf, &libconf->channel);
-		rt2800_config_txpower(rt2x00dev, libconf->conf->channel,
+		rt2800_config_txpower(rt2x00dev, libconf->conf->chandef.chan,
 				      libconf->conf->power_level);
 	}
 	if (flags & IEEE80211_CONF_CHANGE_POWER)
-		rt2800_config_txpower(rt2x00dev, libconf->conf->channel,
+		rt2800_config_txpower(rt2x00dev, libconf->conf->chandef.chan,
 				      libconf->conf->power_level);
 	if (flags & IEEE80211_CONF_CHANGE_RETRY_LIMITS)
 		rt2800_config_retry_limit(rt2x00dev, libconf);
@@ -2938,13 +3331,16 @@ static u8 rt2800_get_default_vgc(struct rt2x00_dev *rt2x00dev)
 		    rt2x00_rt(rt2x00dev, RT3390) ||
 		    rt2x00_rt(rt2x00dev, RT3572) ||
 		    rt2x00_rt(rt2x00dev, RT5390) ||
-		    rt2x00_rt(rt2x00dev, RT5392))
+		    rt2x00_rt(rt2x00dev, RT5392) ||
+		    rt2x00_rt(rt2x00dev, RT5592))
 			vgc = 0x1c + (2 * rt2x00dev->lna_gain);
 		else
 			vgc = 0x2e + rt2x00dev->lna_gain;
 	} else { /* 5GHZ band */
 		if (rt2x00_rt(rt2x00dev, RT3572))
 			vgc = 0x22 + (rt2x00dev->lna_gain * 5) / 3;
+		else if (rt2x00_rt(rt2x00dev, RT5592))
+			vgc = 0x24 + (2 * rt2x00dev->lna_gain);
 		else {
 			if (!test_bit(CONFIG_CHANNEL_HT40, &rt2x00dev->flags))
 				vgc = 0x32 + (rt2x00dev->lna_gain * 5) / 3;
@@ -2960,7 +3356,11 @@ static inline void rt2800_set_vgc(struct rt2x00_dev *rt2x00dev,
 				  struct link_qual *qual, u8 vgc_level)
 {
 	if (qual->vgc_level != vgc_level) {
-		rt2800_bbp_write(rt2x00dev, 66, vgc_level);
+		if (rt2x00_rt(rt2x00dev, RT5592)) {
+			rt2800_bbp_write(rt2x00dev, 83, qual->rssi > -65 ? 0x4a : 0x7a);
+			rt2800_bbp_write_with_rx_chain(rt2x00dev, 66, vgc_level);
+		} else
+			rt2800_bbp_write(rt2x00dev, 66, vgc_level);
 		qual->vgc_level = vgc_level;
 		qual->vgc_level_reg = vgc_level;
 	}
@@ -2975,15 +3375,23 @@ EXPORT_SYMBOL_GPL(rt2800_reset_tuner);
 void rt2800_link_tuner(struct rt2x00_dev *rt2x00dev, struct link_qual *qual,
 		       const u32 count)
 {
+	u8 vgc;
+
 	if (rt2x00_rt_rev(rt2x00dev, RT2860, REV_RT2860C))
 		return;
-
 	/*
-	 * When RSSI is better then -80 increase VGC level with 0x10
+	 * When RSSI is better then -80 increase VGC level with 0x10, except
+	 * for rt5592 chip.
 	 */
-	rt2800_set_vgc(rt2x00dev, qual,
-		       rt2800_get_default_vgc(rt2x00dev) +
-		       ((qual->rssi > -80) * 0x10));
+
+	vgc = rt2800_get_default_vgc(rt2x00dev);
+
+	if (rt2x00_rt(rt2x00dev, RT5592) && qual->rssi > -65)
+		vgc += 0x20;
+	else if (qual->rssi > -80)
+		vgc += 0x10;
+
+	rt2800_set_vgc(rt2x00dev, qual, vgc);
 }
 EXPORT_SYMBOL_GPL(rt2800_link_tuner);
 
@@ -3122,7 +3530,8 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 		rt2800_register_write(rt2x00dev, TX_SW_CFG0, 0x00000400);
 		rt2800_register_write(rt2x00dev, TX_SW_CFG1, 0x00080606);
 	} else if (rt2x00_rt(rt2x00dev, RT5390) ||
-		   rt2x00_rt(rt2x00dev, RT5392)) {
+		   rt2x00_rt(rt2x00dev, RT5392) ||
+		   rt2x00_rt(rt2x00dev, RT5592)) {
 		rt2800_register_write(rt2x00dev, TX_SW_CFG0, 0x00000404);
 		rt2800_register_write(rt2x00dev, TX_SW_CFG1, 0x00080606);
 		rt2800_register_write(rt2x00dev, TX_SW_CFG2, 0x00000000);
@@ -3302,7 +3711,8 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, TXOP_CTRL_CFG_EXT_CWMIN, 0);
 	rt2800_register_write(rt2x00dev, TXOP_CTRL_CFG, reg);
 
-	rt2800_register_write(rt2x00dev, TXOP_HLDR_ET, 0x00000002);
+	reg = rt2x00_rt(rt2x00dev, RT5592) ? 0x00000082 : 0x00000002;
+	rt2800_register_write(rt2x00dev, TXOP_HLDR_ET, reg);
 
 	rt2800_register_read(rt2x00dev, TX_RTS_CFG, &reg);
 	rt2x00_set_field32(&reg, TX_RTS_CFG_AUTO_RTS_RETRY_LIMIT, 32);
@@ -3459,7 +3869,7 @@ static int rt2800_wait_bbp_rf_ready(struct rt2x00_dev *rt2x00dev)
 		udelay(REGISTER_BUSY_DELAY);
 	}
 
-	ERROR(rt2x00dev, "BBP/RF register access failed, aborting.\n");
+	rt2x00_err(rt2x00dev, "BBP/RF register access failed, aborting\n");
 	return -EACCES;
 }
 
@@ -3483,10 +3893,140 @@ static int rt2800_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 		udelay(REGISTER_BUSY_DELAY);
 	}
 
-	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
+	rt2x00_err(rt2x00dev, "BBP register access failed, aborting\n");
 	return -EACCES;
 }
 
+static void rt2800_bbp4_mac_if_ctrl(struct rt2x00_dev *rt2x00dev)
+{
+	u8 value;
+
+	rt2800_bbp_read(rt2x00dev, 4, &value);
+	rt2x00_set_field8(&value, BBP4_MAC_IF_CTRL, 1);
+	rt2800_bbp_write(rt2x00dev, 4, value);
+}
+
+static void rt2800_init_freq_calibration(struct rt2x00_dev *rt2x00dev)
+{
+	rt2800_bbp_write(rt2x00dev, 142, 1);
+	rt2800_bbp_write(rt2x00dev, 143, 57);
+}
+
+static void rt2800_init_bbp_5592_glrt(struct rt2x00_dev *rt2x00dev)
+{
+	const u8 glrt_table[] = {
+		0xE0, 0x1F, 0X38, 0x32, 0x08, 0x28, 0x19, 0x0A, 0xFF, 0x00, /* 128 ~ 137 */
+		0x16, 0x10, 0x10, 0x0B, 0x36, 0x2C, 0x26, 0x24, 0x42, 0x36, /* 138 ~ 147 */
+		0x30, 0x2D, 0x4C, 0x46, 0x3D, 0x40, 0x3E, 0x42, 0x3D, 0x40, /* 148 ~ 157 */
+		0X3C, 0x34, 0x2C, 0x2F, 0x3C, 0x35, 0x2E, 0x2A, 0x49, 0x41, /* 158 ~ 167 */
+		0x36, 0x31, 0x30, 0x30, 0x0E, 0x0D, 0x28, 0x21, 0x1C, 0x16, /* 168 ~ 177 */
+		0x50, 0x4A, 0x43, 0x40, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, /* 178 ~ 187 */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 188 ~ 197 */
+		0x00, 0x00, 0x7D, 0x14, 0x32, 0x2C, 0x36, 0x4C, 0x43, 0x2C, /* 198 ~ 207 */
+		0x2E, 0x36, 0x30, 0x6E,					    /* 208 ~ 211 */
+	};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(glrt_table); i++) {
+		rt2800_bbp_write(rt2x00dev, 195, 128 + i);
+		rt2800_bbp_write(rt2x00dev, 196, glrt_table[i]);
+	}
+};
+
+static void rt2800_init_bbp_early(struct rt2x00_dev *rt2x00dev)
+{
+	rt2800_bbp_write(rt2x00dev, 65, 0x2C);
+	rt2800_bbp_write(rt2x00dev, 66, 0x38);
+	rt2800_bbp_write(rt2x00dev, 68, 0x0B);
+	rt2800_bbp_write(rt2x00dev, 69, 0x12);
+	rt2800_bbp_write(rt2x00dev, 70, 0x0a);
+	rt2800_bbp_write(rt2x00dev, 73, 0x10);
+	rt2800_bbp_write(rt2x00dev, 81, 0x37);
+	rt2800_bbp_write(rt2x00dev, 82, 0x62);
+	rt2800_bbp_write(rt2x00dev, 83, 0x6A);
+	rt2800_bbp_write(rt2x00dev, 84, 0x99);
+	rt2800_bbp_write(rt2x00dev, 86, 0x00);
+	rt2800_bbp_write(rt2x00dev, 91, 0x04);
+	rt2800_bbp_write(rt2x00dev, 92, 0x00);
+	rt2800_bbp_write(rt2x00dev, 103, 0x00);
+	rt2800_bbp_write(rt2x00dev, 105, 0x05);
+	rt2800_bbp_write(rt2x00dev, 106, 0x35);
+}
+
+static void rt2800_init_bbp_5592(struct rt2x00_dev *rt2x00dev)
+{
+	int ant, div_mode;
+	u16 eeprom;
+	u8 value;
+
+	rt2800_init_bbp_early(rt2x00dev);
+
+	rt2800_bbp_read(rt2x00dev, 105, &value);
+	rt2x00_set_field8(&value, BBP105_MLD,
+			  rt2x00dev->default_ant.rx_chain_num == 2);
+	rt2800_bbp_write(rt2x00dev, 105, value);
+
+	rt2800_bbp4_mac_if_ctrl(rt2x00dev);
+
+	rt2800_bbp_write(rt2x00dev, 20, 0x06);
+	rt2800_bbp_write(rt2x00dev, 31, 0x08);
+	rt2800_bbp_write(rt2x00dev, 65, 0x2C);
+	rt2800_bbp_write(rt2x00dev, 68, 0xDD);
+	rt2800_bbp_write(rt2x00dev, 69, 0x1A);
+	rt2800_bbp_write(rt2x00dev, 70, 0x05);
+	rt2800_bbp_write(rt2x00dev, 73, 0x13);
+	rt2800_bbp_write(rt2x00dev, 74, 0x0F);
+	rt2800_bbp_write(rt2x00dev, 75, 0x4F);
+	rt2800_bbp_write(rt2x00dev, 76, 0x28);
+	rt2800_bbp_write(rt2x00dev, 77, 0x59);
+	rt2800_bbp_write(rt2x00dev, 84, 0x9A);
+	rt2800_bbp_write(rt2x00dev, 86, 0x38);
+	rt2800_bbp_write(rt2x00dev, 88, 0x90);
+	rt2800_bbp_write(rt2x00dev, 91, 0x04);
+	rt2800_bbp_write(rt2x00dev, 92, 0x02);
+	rt2800_bbp_write(rt2x00dev, 95, 0x9a);
+	rt2800_bbp_write(rt2x00dev, 98, 0x12);
+	rt2800_bbp_write(rt2x00dev, 103, 0xC0);
+	rt2800_bbp_write(rt2x00dev, 104, 0x92);
+	/* FIXME BBP105 owerwrite */
+	rt2800_bbp_write(rt2x00dev, 105, 0x3C);
+	rt2800_bbp_write(rt2x00dev, 106, 0x35);
+	rt2800_bbp_write(rt2x00dev, 128, 0x12);
+	rt2800_bbp_write(rt2x00dev, 134, 0xD0);
+	rt2800_bbp_write(rt2x00dev, 135, 0xF6);
+	rt2800_bbp_write(rt2x00dev, 137, 0x0F);
+
+	/* Initialize GLRT (Generalized Likehood Radio Test) */
+	rt2800_init_bbp_5592_glrt(rt2x00dev);
+
+	rt2800_bbp4_mac_if_ctrl(rt2x00dev);
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1, &eeprom);
+	div_mode = rt2x00_get_field16(eeprom, EEPROM_NIC_CONF1_ANT_DIVERSITY);
+	ant = (div_mode == 3) ? 1 : 0;
+	rt2800_bbp_read(rt2x00dev, 152, &value);
+	if (ant == 0) {
+		/* Main antenna */
+		rt2x00_set_field8(&value, BBP152_RX_DEFAULT_ANT, 1);
+	} else {
+		/* Auxiliary antenna */
+		rt2x00_set_field8(&value, BBP152_RX_DEFAULT_ANT, 0);
+	}
+	rt2800_bbp_write(rt2x00dev, 152, value);
+
+	if (rt2x00_rt_rev_gte(rt2x00dev, RT5592, REV_RT5592C)) {
+		rt2800_bbp_read(rt2x00dev, 254, &value);
+		rt2x00_set_field8(&value, BBP254_BIT7, 1);
+		rt2800_bbp_write(rt2x00dev, 254, value);
+	}
+
+	rt2800_init_freq_calibration(rt2x00dev);
+
+	rt2800_bbp_write(rt2x00dev, 84, 0x19);
+	if (rt2x00_rt_rev_gte(rt2x00dev, RT5592, REV_RT5592C))
+		rt2800_bbp_write(rt2x00dev, 103, 0xc0);
+}
+
 static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 {
 	unsigned int i;
@@ -3498,6 +4038,11 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 		     rt2800_wait_bbp_ready(rt2x00dev)))
 		return -EACCES;
 
+	if (rt2x00_rt(rt2x00dev, RT5592)) {
+		rt2800_init_bbp_5592(rt2x00dev);
+		return 0;
+	}
+
 	if (rt2x00_rt(rt2x00dev, RT3352)) {
 		rt2800_bbp_write(rt2x00dev, 3, 0x00);
 		rt2800_bbp_write(rt2x00dev, 4, 0x50);
@@ -3505,11 +4050,8 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 
 	if (rt2x00_rt(rt2x00dev, RT3290) ||
 	    rt2x00_rt(rt2x00dev, RT5390) ||
-	    rt2x00_rt(rt2x00dev, RT5392)) {
-		rt2800_bbp_read(rt2x00dev, 4, &value);
-		rt2x00_set_field8(&value, BBP4_MAC_IF_CTRL, 1);
-		rt2800_bbp_write(rt2x00dev, 4, value);
-	}
+	    rt2x00_rt(rt2x00dev, RT5392))
+		rt2800_bbp4_mac_if_ctrl(rt2x00dev);
 
 	if (rt2800_is_305x_soc(rt2x00dev) ||
 	    rt2x00_rt(rt2x00dev, RT3290) ||
@@ -3783,9 +4325,7 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 			rt2x00_set_field8(&value, BBP152_RX_DEFAULT_ANT, 0);
 		rt2800_bbp_write(rt2x00dev, 152, value);
 
-		/* Init frequency calibration */
-		rt2800_bbp_write(rt2x00dev, 142, 1);
-		rt2800_bbp_write(rt2x00dev, 143, 57);
+		rt2800_init_freq_calibration(rt2x00dev);
 	}
 
 	for (i = 0; i < EEPROM_BBP_SIZE; i++) {
@@ -3801,8 +4341,17 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 	return 0;
 }
 
-static u8 rt2800_init_rx_filter(struct rt2x00_dev *rt2x00dev,
-				bool bw40, u8 rfcsr24, u8 filter_target)
+static void rt2800_led_open_drain_enable(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	rt2800_register_read(rt2x00dev, OPT_14_CSR, &reg);
+	rt2x00_set_field32(&reg, OPT_14_CSR_BIT0, 1);
+	rt2800_register_write(rt2x00dev, OPT_14_CSR, reg);
+}
+
+static u8 rt2800_init_rx_filter(struct rt2x00_dev *rt2x00dev, bool bw40,
+				u8 filter_target)
 {
 	unsigned int i;
 	u8 bbp;
@@ -3810,6 +4359,7 @@ static u8 rt2800_init_rx_filter(struct rt2x00_dev *rt2x00dev,
 	u8 passband;
 	u8 stopband;
 	u8 overtuned = 0;
+	u8 rfcsr24 = (bw40) ? 0x27 : 0x07;
 
 	rt2800_rfcsr_write(rt2x00dev, 24, rfcsr24);
 
@@ -3865,8 +4415,169 @@ static u8 rt2800_init_rx_filter(struct rt2x00_dev *rt2x00dev,
 	return rfcsr24;
 }
 
+static void rt2800_rf_init_calibration(struct rt2x00_dev *rt2x00dev,
+				       const unsigned int rf_reg)
+{
+	u8 rfcsr;
+
+	rt2800_rfcsr_read(rt2x00dev, rf_reg, &rfcsr);
+	rt2x00_set_field8(&rfcsr, FIELD8(0x80), 1);
+	rt2800_rfcsr_write(rt2x00dev, rf_reg, rfcsr);
+	msleep(1);
+	rt2x00_set_field8(&rfcsr, FIELD8(0x80), 0);
+	rt2800_rfcsr_write(rt2x00dev, rf_reg, rfcsr);
+}
+
+static void rt2800_rx_filter_calibration(struct rt2x00_dev *rt2x00dev)
+{
+	struct rt2800_drv_data *drv_data = rt2x00dev->drv_data;
+	u8 filter_tgt_bw20;
+	u8 filter_tgt_bw40;
+	u8 rfcsr, bbp;
+
+	/*
+	 * TODO: sync filter_tgt values with vendor driver
+	 */
+	if (rt2x00_rt(rt2x00dev, RT3070)) {
+		filter_tgt_bw20 = 0x16;
+		filter_tgt_bw40 = 0x19;
+	} else {
+		filter_tgt_bw20 = 0x13;
+		filter_tgt_bw40 = 0x15;
+	}
+
+	drv_data->calibration_bw20 =
+		rt2800_init_rx_filter(rt2x00dev, false, filter_tgt_bw20);
+	drv_data->calibration_bw40 =
+		rt2800_init_rx_filter(rt2x00dev, true, filter_tgt_bw40);
+
+	/*
+	 * Save BBP 25 & 26 values for later use in channel switching (for 3052)
+	 */
+	rt2800_bbp_read(rt2x00dev, 25, &drv_data->bbp25);
+	rt2800_bbp_read(rt2x00dev, 26, &drv_data->bbp26);
+
+	/*
+	 * Set back to initial state
+	 */
+	rt2800_bbp_write(rt2x00dev, 24, 0);
+
+	rt2800_rfcsr_read(rt2x00dev, 22, &rfcsr);
+	rt2x00_set_field8(&rfcsr, RFCSR22_BASEBAND_LOOPBACK, 0);
+	rt2800_rfcsr_write(rt2x00dev, 22, rfcsr);
+
+	/*
+	 * Set BBP back to BW20
+	 */
+	rt2800_bbp_read(rt2x00dev, 4, &bbp);
+	rt2x00_set_field8(&bbp, BBP4_BANDWIDTH, 0);
+	rt2800_bbp_write(rt2x00dev, 4, bbp);
+}
+
+static void rt2800_normal_mode_setup_3xxx(struct rt2x00_dev *rt2x00dev)
+{
+	struct rt2800_drv_data *drv_data = rt2x00dev->drv_data;
+	u8 min_gain, rfcsr, bbp;
+	u16 eeprom;
+
+	rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
+
+	rt2x00_set_field8(&rfcsr, RFCSR17_TX_LO1_EN, 0);
+	if (rt2x00_rt(rt2x00dev, RT3070) ||
+	    rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
+	    rt2x00_rt_rev_lt(rt2x00dev, RT3090, REV_RT3090E) ||
+	    rt2x00_rt_rev_lt(rt2x00dev, RT3390, REV_RT3390E)) {
+		if (!test_bit(CAPABILITY_EXTERNAL_LNA_BG, &rt2x00dev->cap_flags))
+			rt2x00_set_field8(&rfcsr, RFCSR17_R, 1);
+	}
+
+	min_gain = rt2x00_rt(rt2x00dev, RT3070) ? 1 : 2;
+	if (drv_data->txmixer_gain_24g >= min_gain) {
+		rt2x00_set_field8(&rfcsr, RFCSR17_TXMIXER_GAIN,
+				  drv_data->txmixer_gain_24g);
+	}
+
+	rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
+
+	if (rt2x00_rt(rt2x00dev, RT3090)) {
+		/*  Turn off unused DAC1 and ADC1 to reduce power consumption */
+		rt2800_bbp_read(rt2x00dev, 138, &bbp);
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0, &eeprom);
+		if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RXPATH) == 1)
+			rt2x00_set_field8(&bbp, BBP138_RX_ADC1, 0);
+		if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_TXPATH) == 1)
+			rt2x00_set_field8(&bbp, BBP138_TX_DAC1, 1);
+		rt2800_bbp_write(rt2x00dev, 138, bbp);
+	}
+
+	if (rt2x00_rt(rt2x00dev, RT3070)) {
+		rt2800_rfcsr_read(rt2x00dev, 27, &rfcsr);
+		if (rt2x00_rt_rev_lt(rt2x00dev, RT3070, REV_RT3070F))
+			rt2x00_set_field8(&rfcsr, RFCSR27_R1, 3);
+		else
+			rt2x00_set_field8(&rfcsr, RFCSR27_R1, 0);
+		rt2x00_set_field8(&rfcsr, RFCSR27_R2, 0);
+		rt2x00_set_field8(&rfcsr, RFCSR27_R3, 0);
+		rt2x00_set_field8(&rfcsr, RFCSR27_R4, 0);
+		rt2800_rfcsr_write(rt2x00dev, 27, rfcsr);
+	} else if (rt2x00_rt(rt2x00dev, RT3071) ||
+		   rt2x00_rt(rt2x00dev, RT3090) ||
+		   rt2x00_rt(rt2x00dev, RT3390)) {
+		rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR1_RF_BLOCK_EN, 1);
+		rt2x00_set_field8(&rfcsr, RFCSR1_RX0_PD, 0);
+		rt2x00_set_field8(&rfcsr, RFCSR1_TX0_PD, 0);
+		rt2x00_set_field8(&rfcsr, RFCSR1_RX1_PD, 1);
+		rt2x00_set_field8(&rfcsr, RFCSR1_TX1_PD, 1);
+		rt2800_rfcsr_write(rt2x00dev, 1, rfcsr);
+
+		rt2800_rfcsr_read(rt2x00dev, 15, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR15_TX_LO2_EN, 0);
+		rt2800_rfcsr_write(rt2x00dev, 15, rfcsr);
+
+		rt2800_rfcsr_read(rt2x00dev, 20, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR20_RX_LO1_EN, 0);
+		rt2800_rfcsr_write(rt2x00dev, 20, rfcsr);
+
+		rt2800_rfcsr_read(rt2x00dev, 21, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR21_RX_LO2_EN, 0);
+		rt2800_rfcsr_write(rt2x00dev, 21, rfcsr);
+	}
+}
+
+static void rt2800_normal_mode_setup_5xxx(struct rt2x00_dev *rt2x00dev)
+{
+	u8 reg;
+	u16 eeprom;
+
+	/*  Turn off unused DAC1 and ADC1 to reduce power consumption */
+	rt2800_bbp_read(rt2x00dev, 138, &reg);
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0, &eeprom);
+	if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RXPATH) == 1)
+		rt2x00_set_field8(&reg, BBP138_RX_ADC1, 0);
+	if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_TXPATH) == 1)
+		rt2x00_set_field8(&reg, BBP138_TX_DAC1, 1);
+	rt2800_bbp_write(rt2x00dev, 138, reg);
+
+	rt2800_rfcsr_read(rt2x00dev, 38, &reg);
+	rt2x00_set_field8(&reg, RFCSR38_RX_LO1_EN, 0);
+	rt2800_rfcsr_write(rt2x00dev, 38, reg);
+
+	rt2800_rfcsr_read(rt2x00dev, 39, &reg);
+	rt2x00_set_field8(&reg, RFCSR39_RX_LO2_EN, 0);
+	rt2800_rfcsr_write(rt2x00dev, 39, reg);
+
+	rt2800_bbp4_mac_if_ctrl(rt2x00dev);
+
+	rt2800_rfcsr_read(rt2x00dev, 30, &reg);
+	rt2x00_set_field8(&reg, RFCSR30_RX_VCM, 2);
+	rt2800_rfcsr_write(rt2x00dev, 30, reg);
+}
+
 static void rt2800_init_rfcsr_305x_soc(struct rt2x00_dev *rt2x00dev)
 {
+	rt2800_rf_init_calibration(rt2x00dev, 30);
+
 	rt2800_rfcsr_write(rt2x00dev, 0, 0x50);
 	rt2800_rfcsr_write(rt2x00dev, 1, 0x01);
 	rt2800_rfcsr_write(rt2x00dev, 2, 0xf7);
@@ -3903,6 +4614,13 @@ static void rt2800_init_rfcsr_305x_soc(struct rt2x00_dev *rt2x00dev)
 
 static void rt2800_init_rfcsr_30xx(struct rt2x00_dev *rt2x00dev)
 {
+	u8 rfcsr;
+	u16 eeprom;
+	u32 reg;
+
+	/* XXX vendor driver do this only for 3070 */
+	rt2800_rf_init_calibration(rt2x00dev, 30);
+
 	rt2800_rfcsr_write(rt2x00dev, 4, 0x40);
 	rt2800_rfcsr_write(rt2x00dev, 5, 0x03);
 	rt2800_rfcsr_write(rt2x00dev, 6, 0x02);
@@ -3922,10 +4640,54 @@ static void rt2800_init_rfcsr_30xx(struct rt2x00_dev *rt2x00dev)
 	rt2800_rfcsr_write(rt2x00dev, 24, 0x16);
 	rt2800_rfcsr_write(rt2x00dev, 25, 0x01);
 	rt2800_rfcsr_write(rt2x00dev, 29, 0x1f);
+
+	if (rt2x00_rt_rev_lt(rt2x00dev, RT3070, REV_RT3070F)) {
+		rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
+		rt2x00_set_field32(&reg, LDO_CFG0_BGSEL, 1);
+		rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 3);
+		rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
+	} else if (rt2x00_rt(rt2x00dev, RT3071) ||
+		   rt2x00_rt(rt2x00dev, RT3090)) {
+		rt2800_rfcsr_write(rt2x00dev, 31, 0x14);
+
+		rt2800_rfcsr_read(rt2x00dev, 6, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR6_R2, 1);
+		rt2800_rfcsr_write(rt2x00dev, 6, rfcsr);
+
+		rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
+		rt2x00_set_field32(&reg, LDO_CFG0_BGSEL, 1);
+		if (rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
+		    rt2x00_rt_rev_lt(rt2x00dev, RT3090, REV_RT3090E)) {
+			rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1, &eeprom);
+			if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF1_DAC_TEST))
+				rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 3);
+			else
+				rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 0);
+		}
+		rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
+
+		rt2800_register_read(rt2x00dev, GPIO_SWITCH, &reg);
+		rt2x00_set_field32(&reg, GPIO_SWITCH_5, 0);
+		rt2800_register_write(rt2x00dev, GPIO_SWITCH, reg);
+	}
+
+	rt2800_rx_filter_calibration(rt2x00dev);
+
+	if (rt2x00_rt_rev_lt(rt2x00dev, RT3070, REV_RT3070F) ||
+	    rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
+	    rt2x00_rt_rev_lt(rt2x00dev, RT3090, REV_RT3090E))
+		rt2800_rfcsr_write(rt2x00dev, 27, 0x03);
+
+	rt2800_led_open_drain_enable(rt2x00dev);
+	rt2800_normal_mode_setup_3xxx(rt2x00dev);
 }
 
 static void rt2800_init_rfcsr_3290(struct rt2x00_dev *rt2x00dev)
 {
+	u8 rfcsr;
+
+	rt2800_rf_init_calibration(rt2x00dev, 2);
+
 	rt2800_rfcsr_write(rt2x00dev, 1, 0x0f);
 	rt2800_rfcsr_write(rt2x00dev, 2, 0x80);
 	rt2800_rfcsr_write(rt2x00dev, 3, 0x08);
@@ -3972,10 +4734,19 @@ static void rt2800_init_rfcsr_3290(struct rt2x00_dev *rt2x00dev)
 	rt2800_rfcsr_write(rt2x00dev, 59, 0x09);
 	rt2800_rfcsr_write(rt2x00dev, 60, 0x45);
 	rt2800_rfcsr_write(rt2x00dev, 61, 0xc1);
+
+	rt2800_rfcsr_read(rt2x00dev, 29, &rfcsr);
+	rt2x00_set_field8(&rfcsr, RFCSR29_RSSI_GAIN, 3);
+	rt2800_rfcsr_write(rt2x00dev, 29, rfcsr);
+
+	rt2800_led_open_drain_enable(rt2x00dev);
+	rt2800_normal_mode_setup_3xxx(rt2x00dev);
 }
 
 static void rt2800_init_rfcsr_3352(struct rt2x00_dev *rt2x00dev)
 {
+	rt2800_rf_init_calibration(rt2x00dev, 30);
+
 	rt2800_rfcsr_write(rt2x00dev, 0, 0xf0);
 	rt2800_rfcsr_write(rt2x00dev, 1, 0x23);
 	rt2800_rfcsr_write(rt2x00dev, 2, 0x50);
@@ -4039,10 +4810,18 @@ static void rt2800_init_rfcsr_3352(struct rt2x00_dev *rt2x00dev)
 	rt2800_rfcsr_write(rt2x00dev, 61, 0x00);
 	rt2800_rfcsr_write(rt2x00dev, 62, 0x00);
 	rt2800_rfcsr_write(rt2x00dev, 63, 0x00);
+
+	rt2800_rx_filter_calibration(rt2x00dev);
+	rt2800_led_open_drain_enable(rt2x00dev);
+	rt2800_normal_mode_setup_3xxx(rt2x00dev);
 }
 
 static void rt2800_init_rfcsr_3390(struct rt2x00_dev *rt2x00dev)
 {
+	u32 reg;
+
+	rt2800_rf_init_calibration(rt2x00dev, 30);
+
 	rt2800_rfcsr_write(rt2x00dev, 0, 0xa0);
 	rt2800_rfcsr_write(rt2x00dev, 1, 0xe1);
 	rt2800_rfcsr_write(rt2x00dev, 2, 0xf1);
@@ -4075,10 +4854,27 @@ static void rt2800_init_rfcsr_3390(struct rt2x00_dev *rt2x00dev)
 	rt2800_rfcsr_write(rt2x00dev, 29, 0x8f);
 	rt2800_rfcsr_write(rt2x00dev, 30, 0x20);
 	rt2800_rfcsr_write(rt2x00dev, 31, 0x0f);
+
+	rt2800_register_read(rt2x00dev, GPIO_SWITCH, &reg);
+	rt2x00_set_field32(&reg, GPIO_SWITCH_5, 0);
+	rt2800_register_write(rt2x00dev, GPIO_SWITCH, reg);
+
+	rt2800_rx_filter_calibration(rt2x00dev);
+
+	if (rt2x00_rt_rev_lt(rt2x00dev, RT3390, REV_RT3390E))
+		rt2800_rfcsr_write(rt2x00dev, 27, 0x03);
+
+	rt2800_led_open_drain_enable(rt2x00dev);
+	rt2800_normal_mode_setup_3xxx(rt2x00dev);
 }
 
 static void rt2800_init_rfcsr_3572(struct rt2x00_dev *rt2x00dev)
 {
+	u8 rfcsr;
+	u32 reg;
+
+	rt2800_rf_init_calibration(rt2x00dev, 30);
+
 	rt2800_rfcsr_write(rt2x00dev, 0, 0x70);
 	rt2800_rfcsr_write(rt2x00dev, 1, 0x81);
 	rt2800_rfcsr_write(rt2x00dev, 2, 0xf1);
@@ -4110,10 +4906,30 @@ static void rt2800_init_rfcsr_3572(struct rt2x00_dev *rt2x00dev)
 	rt2800_rfcsr_write(rt2x00dev, 29, 0x9b);
 	rt2800_rfcsr_write(rt2x00dev, 30, 0x09);
 	rt2800_rfcsr_write(rt2x00dev, 31, 0x10);
+
+	rt2800_rfcsr_read(rt2x00dev, 6, &rfcsr);
+	rt2x00_set_field8(&rfcsr, RFCSR6_R2, 1);
+	rt2800_rfcsr_write(rt2x00dev, 6, rfcsr);
+
+	rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
+	rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 3);
+	rt2x00_set_field32(&reg, LDO_CFG0_BGSEL, 1);
+	rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
+	msleep(1);
+	rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
+	rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 0);
+	rt2x00_set_field32(&reg, LDO_CFG0_BGSEL, 1);
+	rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
+
+	rt2800_rx_filter_calibration(rt2x00dev);
+	rt2800_led_open_drain_enable(rt2x00dev);
+	rt2800_normal_mode_setup_3xxx(rt2x00dev);
 }
 
 static void rt2800_init_rfcsr_5390(struct rt2x00_dev *rt2x00dev)
 {
+	rt2800_rf_init_calibration(rt2x00dev, 2);
+
 	rt2800_rfcsr_write(rt2x00dev, 1, 0x0f);
 	rt2800_rfcsr_write(rt2x00dev, 2, 0x80);
 	rt2800_rfcsr_write(rt2x00dev, 3, 0x88);
@@ -4194,10 +5010,16 @@ static void rt2800_init_rfcsr_5390(struct rt2x00_dev *rt2x00dev)
 		rt2800_rfcsr_write(rt2x00dev, 61, 0xdd);
 	rt2800_rfcsr_write(rt2x00dev, 62, 0x00);
 	rt2800_rfcsr_write(rt2x00dev, 63, 0x00);
+
+	rt2800_normal_mode_setup_5xxx(rt2x00dev);
+
+	rt2800_led_open_drain_enable(rt2x00dev);
 }
 
 static void rt2800_init_rfcsr_5392(struct rt2x00_dev *rt2x00dev)
 {
+	rt2800_rf_init_calibration(rt2x00dev, 2);
+
 	rt2800_rfcsr_write(rt2x00dev, 1, 0x17);
 	rt2800_rfcsr_write(rt2x00dev, 2, 0x80);
 	rt2800_rfcsr_write(rt2x00dev, 3, 0x88);
@@ -4257,53 +5079,61 @@ static void rt2800_init_rfcsr_5392(struct rt2x00_dev *rt2x00dev)
 	rt2800_rfcsr_write(rt2x00dev, 61, 0x91);
 	rt2800_rfcsr_write(rt2x00dev, 62, 0x39);
 	rt2800_rfcsr_write(rt2x00dev, 63, 0x07);
+
+	rt2800_normal_mode_setup_5xxx(rt2x00dev);
+
+	rt2800_led_open_drain_enable(rt2x00dev);
 }
 
-static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
+static void rt2800_init_rfcsr_5592(struct rt2x00_dev *rt2x00dev)
 {
-	struct rt2800_drv_data *drv_data = rt2x00dev->drv_data;
-	u8 rfcsr;
-	u8 bbp;
-	u32 reg;
-	u16 eeprom;
+	rt2800_rf_init_calibration(rt2x00dev, 30);
 
-	if (!rt2x00_rt(rt2x00dev, RT3070) &&
-	    !rt2x00_rt(rt2x00dev, RT3071) &&
-	    !rt2x00_rt(rt2x00dev, RT3090) &&
-	    !rt2x00_rt(rt2x00dev, RT3290) &&
-	    !rt2x00_rt(rt2x00dev, RT3352) &&
-	    !rt2x00_rt(rt2x00dev, RT3390) &&
-	    !rt2x00_rt(rt2x00dev, RT3572) &&
-	    !rt2x00_rt(rt2x00dev, RT5390) &&
-	    !rt2x00_rt(rt2x00dev, RT5392) &&
-	    !rt2800_is_305x_soc(rt2x00dev))
-		return 0;
+	rt2800_rfcsr_write(rt2x00dev, 1, 0x3F);
+	rt2800_rfcsr_write(rt2x00dev, 3, 0x08);
+	rt2800_rfcsr_write(rt2x00dev, 3, 0x08);
+	rt2800_rfcsr_write(rt2x00dev, 5, 0x10);
+	rt2800_rfcsr_write(rt2x00dev, 6, 0xE4);
+	rt2800_rfcsr_write(rt2x00dev, 7, 0x00);
+	rt2800_rfcsr_write(rt2x00dev, 14, 0x00);
+	rt2800_rfcsr_write(rt2x00dev, 15, 0x00);
+	rt2800_rfcsr_write(rt2x00dev, 16, 0x00);
+	rt2800_rfcsr_write(rt2x00dev, 18, 0x03);
+	rt2800_rfcsr_write(rt2x00dev, 19, 0x4D);
+	rt2800_rfcsr_write(rt2x00dev, 20, 0x10);
+	rt2800_rfcsr_write(rt2x00dev, 21, 0x8D);
+	rt2800_rfcsr_write(rt2x00dev, 26, 0x82);
+	rt2800_rfcsr_write(rt2x00dev, 28, 0x00);
+	rt2800_rfcsr_write(rt2x00dev, 29, 0x10);
+	rt2800_rfcsr_write(rt2x00dev, 33, 0xC0);
+	rt2800_rfcsr_write(rt2x00dev, 34, 0x07);
+	rt2800_rfcsr_write(rt2x00dev, 35, 0x12);
+	rt2800_rfcsr_write(rt2x00dev, 47, 0x0C);
+	rt2800_rfcsr_write(rt2x00dev, 53, 0x22);
+	rt2800_rfcsr_write(rt2x00dev, 63, 0x07);
 
-	/*
-	 * Init RF calibration.
-	 */
+	rt2800_rfcsr_write(rt2x00dev, 2, 0x80);
+	msleep(1);
 
-	if (rt2x00_rt(rt2x00dev, RT3290) ||
-	    rt2x00_rt(rt2x00dev, RT5390) ||
-	    rt2x00_rt(rt2x00dev, RT5392)) {
-		rt2800_rfcsr_read(rt2x00dev, 2, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR2_RESCAL_EN, 1);
-		rt2800_rfcsr_write(rt2x00dev, 2, rfcsr);
-		msleep(1);
-		rt2x00_set_field8(&rfcsr, RFCSR2_RESCAL_EN, 0);
-		rt2800_rfcsr_write(rt2x00dev, 2, rfcsr);
-	} else {
-		rt2800_rfcsr_read(rt2x00dev, 30, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR30_RF_CALIBRATION, 1);
-		rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
-		msleep(1);
-		rt2x00_set_field8(&rfcsr, RFCSR30_RF_CALIBRATION, 0);
-		rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
-	}
+	rt2800_adjust_freq_offset(rt2x00dev);
+
+	/* Enable DC filter */
+	if (rt2x00_rt_rev_gte(rt2x00dev, RT5592, REV_RT5592C))
+		rt2800_bbp_write(rt2x00dev, 103, 0xc0);
+
+	rt2800_normal_mode_setup_5xxx(rt2x00dev);
+
+	if (rt2x00_rt_rev_lt(rt2x00dev, RT5592, REV_RT5592C))
+		rt2800_rfcsr_write(rt2x00dev, 27, 0x03);
 
+	rt2800_led_open_drain_enable(rt2x00dev);
+}
+
+static void rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
+{
 	if (rt2800_is_305x_soc(rt2x00dev)) {
 		rt2800_init_rfcsr_305x_soc(rt2x00dev);
-		return 0;
+		return;
 	}
 
 	switch (rt2x00dev->chip.rt) {
@@ -4330,198 +5160,10 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 	case RT5392:
 		rt2800_init_rfcsr_5392(rt2x00dev);
 		break;
+	case RT5592:
+		rt2800_init_rfcsr_5592(rt2x00dev);
+		break;
 	}
-
-	if (rt2x00_rt_rev_lt(rt2x00dev, RT3070, REV_RT3070F)) {
-		rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
-		rt2x00_set_field32(&reg, LDO_CFG0_BGSEL, 1);
-		rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 3);
-		rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
-	} else if (rt2x00_rt(rt2x00dev, RT3071) ||
-		   rt2x00_rt(rt2x00dev, RT3090)) {
-		rt2800_rfcsr_write(rt2x00dev, 31, 0x14);
-
-		rt2800_rfcsr_read(rt2x00dev, 6, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR6_R2, 1);
-		rt2800_rfcsr_write(rt2x00dev, 6, rfcsr);
-
-		rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
-		rt2x00_set_field32(&reg, LDO_CFG0_BGSEL, 1);
-		if (rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
-		    rt2x00_rt_rev_lt(rt2x00dev, RT3090, REV_RT3090E)) {
-			rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1, &eeprom);
-			if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF1_DAC_TEST))
-				rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 3);
-			else
-				rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 0);
-		}
-		rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
-
-		rt2800_register_read(rt2x00dev, GPIO_SWITCH, &reg);
-		rt2x00_set_field32(&reg, GPIO_SWITCH_5, 0);
-		rt2800_register_write(rt2x00dev, GPIO_SWITCH, reg);
-	} else if (rt2x00_rt(rt2x00dev, RT3390)) {
-		rt2800_register_read(rt2x00dev, GPIO_SWITCH, &reg);
-		rt2x00_set_field32(&reg, GPIO_SWITCH_5, 0);
-		rt2800_register_write(rt2x00dev, GPIO_SWITCH, reg);
-	} else if (rt2x00_rt(rt2x00dev, RT3572)) {
-		rt2800_rfcsr_read(rt2x00dev, 6, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR6_R2, 1);
-		rt2800_rfcsr_write(rt2x00dev, 6, rfcsr);
-
-		rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
-		rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 3);
-		rt2x00_set_field32(&reg, LDO_CFG0_BGSEL, 1);
-		rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
-		msleep(1);
-		rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
-		rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 0);
-		rt2x00_set_field32(&reg, LDO_CFG0_BGSEL, 1);
-		rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
-	}
-
-	/*
-	 * Set RX Filter calibration for 20MHz and 40MHz
-	 */
-	if (rt2x00_rt(rt2x00dev, RT3070)) {
-		drv_data->calibration_bw20 =
-			rt2800_init_rx_filter(rt2x00dev, false, 0x07, 0x16);
-		drv_data->calibration_bw40 =
-			rt2800_init_rx_filter(rt2x00dev, true, 0x27, 0x19);
-	} else if (rt2x00_rt(rt2x00dev, RT3071) ||
-		   rt2x00_rt(rt2x00dev, RT3090) ||
-		   rt2x00_rt(rt2x00dev, RT3352) ||
-		   rt2x00_rt(rt2x00dev, RT3390) ||
-		   rt2x00_rt(rt2x00dev, RT3572)) {
-		drv_data->calibration_bw20 =
-			rt2800_init_rx_filter(rt2x00dev, false, 0x07, 0x13);
-		drv_data->calibration_bw40 =
-			rt2800_init_rx_filter(rt2x00dev, true, 0x27, 0x15);
-	}
-
-	/*
-	 * Save BBP 25 & 26 values for later use in channel switching
-	 */
-	rt2800_bbp_read(rt2x00dev, 25, &drv_data->bbp25);
-	rt2800_bbp_read(rt2x00dev, 26, &drv_data->bbp26);
-
-	if (!rt2x00_rt(rt2x00dev, RT5390) &&
-	    !rt2x00_rt(rt2x00dev, RT5392)) {
-		/*
-		 * Set back to initial state
-		 */
-		rt2800_bbp_write(rt2x00dev, 24, 0);
-
-		rt2800_rfcsr_read(rt2x00dev, 22, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR22_BASEBAND_LOOPBACK, 0);
-		rt2800_rfcsr_write(rt2x00dev, 22, rfcsr);
-
-		/*
-		 * Set BBP back to BW20
-		 */
-		rt2800_bbp_read(rt2x00dev, 4, &bbp);
-		rt2x00_set_field8(&bbp, BBP4_BANDWIDTH, 0);
-		rt2800_bbp_write(rt2x00dev, 4, bbp);
-	}
-
-	if (rt2x00_rt_rev_lt(rt2x00dev, RT3070, REV_RT3070F) ||
-	    rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
-	    rt2x00_rt_rev_lt(rt2x00dev, RT3090, REV_RT3090E) ||
-	    rt2x00_rt_rev_lt(rt2x00dev, RT3390, REV_RT3390E))
-		rt2800_rfcsr_write(rt2x00dev, 27, 0x03);
-
-	rt2800_register_read(rt2x00dev, OPT_14_CSR, &reg);
-	rt2x00_set_field32(&reg, OPT_14_CSR_BIT0, 1);
-	rt2800_register_write(rt2x00dev, OPT_14_CSR, reg);
-
-	if (!rt2x00_rt(rt2x00dev, RT5390) &&
-	    !rt2x00_rt(rt2x00dev, RT5392)) {
-		rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR17_TX_LO1_EN, 0);
-		if (rt2x00_rt(rt2x00dev, RT3070) ||
-		    rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
-		    rt2x00_rt_rev_lt(rt2x00dev, RT3090, REV_RT3090E) ||
-		    rt2x00_rt_rev_lt(rt2x00dev, RT3390, REV_RT3390E)) {
-			if (!test_bit(CAPABILITY_EXTERNAL_LNA_BG,
-				      &rt2x00dev->cap_flags))
-				rt2x00_set_field8(&rfcsr, RFCSR17_R, 1);
-		}
-		rt2x00_set_field8(&rfcsr, RFCSR17_TXMIXER_GAIN,
-				  drv_data->txmixer_gain_24g);
-		rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
-	}
-
-	if (rt2x00_rt(rt2x00dev, RT3090)) {
-		rt2800_bbp_read(rt2x00dev, 138, &bbp);
-
-		/*  Turn off unused DAC1 and ADC1 to reduce power consumption */
-		rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0, &eeprom);
-		if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RXPATH) == 1)
-			rt2x00_set_field8(&bbp, BBP138_RX_ADC1, 0);
-		if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_TXPATH) == 1)
-			rt2x00_set_field8(&bbp, BBP138_TX_DAC1, 1);
-
-		rt2800_bbp_write(rt2x00dev, 138, bbp);
-	}
-
-	if (rt2x00_rt(rt2x00dev, RT3071) ||
-	    rt2x00_rt(rt2x00dev, RT3090) ||
-	    rt2x00_rt(rt2x00dev, RT3390)) {
-		rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR1_RF_BLOCK_EN, 1);
-		rt2x00_set_field8(&rfcsr, RFCSR1_RX0_PD, 0);
-		rt2x00_set_field8(&rfcsr, RFCSR1_TX0_PD, 0);
-		rt2x00_set_field8(&rfcsr, RFCSR1_RX1_PD, 1);
-		rt2x00_set_field8(&rfcsr, RFCSR1_TX1_PD, 1);
-		rt2800_rfcsr_write(rt2x00dev, 1, rfcsr);
-
-		rt2800_rfcsr_read(rt2x00dev, 15, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR15_TX_LO2_EN, 0);
-		rt2800_rfcsr_write(rt2x00dev, 15, rfcsr);
-
-		rt2800_rfcsr_read(rt2x00dev, 20, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR20_RX_LO1_EN, 0);
-		rt2800_rfcsr_write(rt2x00dev, 20, rfcsr);
-
-		rt2800_rfcsr_read(rt2x00dev, 21, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR21_RX_LO2_EN, 0);
-		rt2800_rfcsr_write(rt2x00dev, 21, rfcsr);
-	}
-
-	if (rt2x00_rt(rt2x00dev, RT3070)) {
-		rt2800_rfcsr_read(rt2x00dev, 27, &rfcsr);
-		if (rt2x00_rt_rev_lt(rt2x00dev, RT3070, REV_RT3070F))
-			rt2x00_set_field8(&rfcsr, RFCSR27_R1, 3);
-		else
-			rt2x00_set_field8(&rfcsr, RFCSR27_R1, 0);
-		rt2x00_set_field8(&rfcsr, RFCSR27_R2, 0);
-		rt2x00_set_field8(&rfcsr, RFCSR27_R3, 0);
-		rt2x00_set_field8(&rfcsr, RFCSR27_R4, 0);
-		rt2800_rfcsr_write(rt2x00dev, 27, rfcsr);
-	}
-
-	if (rt2x00_rt(rt2x00dev, RT3290)) {
-		rt2800_rfcsr_read(rt2x00dev, 29, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR29_RSSI_GAIN, 3);
-		rt2800_rfcsr_write(rt2x00dev, 29, rfcsr);
-	}
-
-	if (rt2x00_rt(rt2x00dev, RT5390) ||
-	    rt2x00_rt(rt2x00dev, RT5392)) {
-		rt2800_rfcsr_read(rt2x00dev, 38, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR38_RX_LO1_EN, 0);
-		rt2800_rfcsr_write(rt2x00dev, 38, rfcsr);
-
-		rt2800_rfcsr_read(rt2x00dev, 39, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR39_RX_LO2_EN, 0);
-		rt2800_rfcsr_write(rt2x00dev, 39, rfcsr);
-
-		rt2800_rfcsr_read(rt2x00dev, 30, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR30_RX_VCM, 2);
-		rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
-	}
-
-	return 0;
 }
 
 int rt2800_enable_radio(struct rt2x00_dev *rt2x00dev)
@@ -4533,15 +5175,24 @@ int rt2800_enable_radio(struct rt2x00_dev *rt2x00dev)
 	 * Initialize all registers.
 	 */
 	if (unlikely(rt2800_wait_wpdma_ready(rt2x00dev) ||
-		     rt2800_init_registers(rt2x00dev) ||
-		     rt2800_init_bbp(rt2x00dev) ||
-		     rt2800_init_rfcsr(rt2x00dev)))
+		     rt2800_init_registers(rt2x00dev)))
 		return -EIO;
 
 	/*
 	 * Send signal to firmware during boot time.
 	 */
-	rt2800_mcu_request(rt2x00dev, MCU_BOOT_SIGNAL, 0, 0, 0);
+	rt2800_register_write(rt2x00dev, H2M_BBP_AGENT, 0);
+	rt2800_register_write(rt2x00dev, H2M_MAILBOX_CSR, 0);
+	if (rt2x00_is_usb(rt2x00dev)) {
+		rt2800_register_write(rt2x00dev, H2M_INT_SRC, 0);
+		rt2800_mcu_request(rt2x00dev, MCU_BOOT_SIGNAL, 0, 0, 0);
+	}
+	msleep(1);
+
+	if (unlikely(rt2800_init_bbp(rt2x00dev)))
+		return -EIO;
+
+	rt2800_init_rfcsr(rt2x00dev);
 
 	if (rt2x00_is_usb(rt2x00dev) &&
 	    (rt2x00_rt(rt2x00dev, RT3070) ||
@@ -4702,7 +5353,7 @@ static int rt2800_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	if (!is_valid_ether_addr(mac)) {
 		eth_random_addr(mac);
-		EEPROM(rt2x00dev, "MAC: %pM\n", mac);
+		rt2x00_eeprom_dbg(rt2x00dev, "MAC: %pM\n", mac);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0, &word);
@@ -4711,7 +5362,7 @@ static int rt2800_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_NIC_CONF0_TXPATH, 1);
 		rt2x00_set_field16(&word, EEPROM_NIC_CONF0_RF_TYPE, RF2820);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_NIC_CONF0, word);
-		EEPROM(rt2x00dev, "Antenna: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "Antenna: 0x%04x\n", word);
 	} else if (rt2x00_rt(rt2x00dev, RT2860) ||
 		   rt2x00_rt(rt2x00dev, RT2872)) {
 		/*
@@ -4740,14 +5391,14 @@ static int rt2800_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_NIC_CONF1_BT_COEXIST, 0);
 		rt2x00_set_field16(&word, EEPROM_NIC_CONF1_DAC_TEST, 0);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_NIC_CONF1, word);
-		EEPROM(rt2x00dev, "NIC: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "NIC: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ, &word);
 	if ((word & 0x00ff) == 0x00ff) {
 		rt2x00_set_field16(&word, EEPROM_FREQ_OFFSET, 0);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_FREQ, word);
-		EEPROM(rt2x00dev, "Freq: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "Freq: 0x%04x\n", word);
 	}
 	if ((word & 0xff00) == 0xff00) {
 		rt2x00_set_field16(&word, EEPROM_FREQ_LED_MODE,
@@ -4757,7 +5408,7 @@ static int rt2800_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_LED_AG_CONF, 0x5555);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_LED_ACT_CONF, 0x2221);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_LED_POLARITY, 0xa9f8);
-		EEPROM(rt2x00dev, "Led Mode: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "Led Mode: 0x%04x\n", word);
 	}
 
 	/*
@@ -4821,9 +5472,9 @@ static int rt2800_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 
 static int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 {
-	u32 reg;
 	u16 value;
 	u16 eeprom;
+	u16 rf;
 
 	/*
 	 * Read EEPROM word for configuration.
@@ -4835,41 +5486,14 @@ static int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	 * RT28xx/RT30xx: defined in "EEPROM_NIC_CONF0_RF_TYPE" field
 	 * RT53xx: defined in "EEPROM_CHIP_ID" field
 	 */
-	if (rt2x00_rt(rt2x00dev, RT3290))
-		rt2800_register_read(rt2x00dev, MAC_CSR0_3290, &reg);
-	else
-		rt2800_register_read(rt2x00dev, MAC_CSR0, &reg);
-
-	if (rt2x00_get_field32(reg, MAC_CSR0_CHIPSET) == RT3290 ||
-	    rt2x00_get_field32(reg, MAC_CSR0_CHIPSET) == RT5390 ||
-	    rt2x00_get_field32(reg, MAC_CSR0_CHIPSET) == RT5392)
-		rt2x00_eeprom_read(rt2x00dev, EEPROM_CHIP_ID, &value);
+	if (rt2x00_rt(rt2x00dev, RT3290) ||
+	    rt2x00_rt(rt2x00dev, RT5390) ||
+	    rt2x00_rt(rt2x00dev, RT5392))
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_CHIP_ID, &rf);
 	else
-		value = rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RF_TYPE);
+		rf = rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RF_TYPE);
 
-	rt2x00_set_chip(rt2x00dev, rt2x00_get_field32(reg, MAC_CSR0_CHIPSET),
-			value, rt2x00_get_field32(reg, MAC_CSR0_REVISION));
-
-	switch (rt2x00dev->chip.rt) {
-	case RT2860:
-	case RT2872:
-	case RT2883:
-	case RT3070:
-	case RT3071:
-	case RT3090:
-	case RT3290:
-	case RT3352:
-	case RT3390:
-	case RT3572:
-	case RT5390:
-	case RT5392:
-		break;
-	default:
-		ERROR(rt2x00dev, "Invalid RT chipset 0x%04x detected.\n", rt2x00dev->chip.rt);
-		return -ENODEV;
-	}
-
-	switch (rt2x00dev->chip.rf) {
+	switch (rf) {
 	case RF2820:
 	case RF2850:
 	case RF2720:
@@ -4887,13 +5511,16 @@ static int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	case RF5372:
 	case RF5390:
 	case RF5392:
+	case RF5592:
 		break;
 	default:
-		ERROR(rt2x00dev, "Invalid RF chipset 0x%04x detected.\n",
-		      rt2x00dev->chip.rf);
+		rt2x00_err(rt2x00dev, "Invalid RF chipset 0x%04x detected\n",
+			   rf);
 		return -ENODEV;
 	}
 
+	rt2x00_set_rf(rt2x00dev, rf);
+
 	/*
 	 * Identify default antenna configuration.
 	 */
@@ -5122,6 +5749,138 @@ static const struct rf_channel rf_vals_3x[] = {
 	{173, 0x61, 0, 9},
 };
 
+static const struct rf_channel rf_vals_5592_xtal20[] = {
+	/* Channel, N, K, mod, R */
+	{1, 482, 4, 10, 3},
+	{2, 483, 4, 10, 3},
+	{3, 484, 4, 10, 3},
+	{4, 485, 4, 10, 3},
+	{5, 486, 4, 10, 3},
+	{6, 487, 4, 10, 3},
+	{7, 488, 4, 10, 3},
+	{8, 489, 4, 10, 3},
+	{9, 490, 4, 10, 3},
+	{10, 491, 4, 10, 3},
+	{11, 492, 4, 10, 3},
+	{12, 493, 4, 10, 3},
+	{13, 494, 4, 10, 3},
+	{14, 496, 8, 10, 3},
+	{36, 172, 8, 12, 1},
+	{38, 173, 0, 12, 1},
+	{40, 173, 4, 12, 1},
+	{42, 173, 8, 12, 1},
+	{44, 174, 0, 12, 1},
+	{46, 174, 4, 12, 1},
+	{48, 174, 8, 12, 1},
+	{50, 175, 0, 12, 1},
+	{52, 175, 4, 12, 1},
+	{54, 175, 8, 12, 1},
+	{56, 176, 0, 12, 1},
+	{58, 176, 4, 12, 1},
+	{60, 176, 8, 12, 1},
+	{62, 177, 0, 12, 1},
+	{64, 177, 4, 12, 1},
+	{100, 183, 4, 12, 1},
+	{102, 183, 8, 12, 1},
+	{104, 184, 0, 12, 1},
+	{106, 184, 4, 12, 1},
+	{108, 184, 8, 12, 1},
+	{110, 185, 0, 12, 1},
+	{112, 185, 4, 12, 1},
+	{114, 185, 8, 12, 1},
+	{116, 186, 0, 12, 1},
+	{118, 186, 4, 12, 1},
+	{120, 186, 8, 12, 1},
+	{122, 187, 0, 12, 1},
+	{124, 187, 4, 12, 1},
+	{126, 187, 8, 12, 1},
+	{128, 188, 0, 12, 1},
+	{130, 188, 4, 12, 1},
+	{132, 188, 8, 12, 1},
+	{134, 189, 0, 12, 1},
+	{136, 189, 4, 12, 1},
+	{138, 189, 8, 12, 1},
+	{140, 190, 0, 12, 1},
+	{149, 191, 6, 12, 1},
+	{151, 191, 10, 12, 1},
+	{153, 192, 2, 12, 1},
+	{155, 192, 6, 12, 1},
+	{157, 192, 10, 12, 1},
+	{159, 193, 2, 12, 1},
+	{161, 193, 6, 12, 1},
+	{165, 194, 2, 12, 1},
+	{184, 164, 0, 12, 1},
+	{188, 164, 4, 12, 1},
+	{192, 165, 8, 12, 1},
+	{196, 166, 0, 12, 1},
+};
+
+static const struct rf_channel rf_vals_5592_xtal40[] = {
+	/* Channel, N, K, mod, R */
+	{1, 241, 2, 10, 3},
+	{2, 241, 7, 10, 3},
+	{3, 242, 2, 10, 3},
+	{4, 242, 7, 10, 3},
+	{5, 243, 2, 10, 3},
+	{6, 243, 7, 10, 3},
+	{7, 244, 2, 10, 3},
+	{8, 244, 7, 10, 3},
+	{9, 245, 2, 10, 3},
+	{10, 245, 7, 10, 3},
+	{11, 246, 2, 10, 3},
+	{12, 246, 7, 10, 3},
+	{13, 247, 2, 10, 3},
+	{14, 248, 4, 10, 3},
+	{36, 86, 4, 12, 1},
+	{38, 86, 6, 12, 1},
+	{40, 86, 8, 12, 1},
+	{42, 86, 10, 12, 1},
+	{44, 87, 0, 12, 1},
+	{46, 87, 2, 12, 1},
+	{48, 87, 4, 12, 1},
+	{50, 87, 6, 12, 1},
+	{52, 87, 8, 12, 1},
+	{54, 87, 10, 12, 1},
+	{56, 88, 0, 12, 1},
+	{58, 88, 2, 12, 1},
+	{60, 88, 4, 12, 1},
+	{62, 88, 6, 12, 1},
+	{64, 88, 8, 12, 1},
+	{100, 91, 8, 12, 1},
+	{102, 91, 10, 12, 1},
+	{104, 92, 0, 12, 1},
+	{106, 92, 2, 12, 1},
+	{108, 92, 4, 12, 1},
+	{110, 92, 6, 12, 1},
+	{112, 92, 8, 12, 1},
+	{114, 92, 10, 12, 1},
+	{116, 93, 0, 12, 1},
+	{118, 93, 2, 12, 1},
+	{120, 93, 4, 12, 1},
+	{122, 93, 6, 12, 1},
+	{124, 93, 8, 12, 1},
+	{126, 93, 10, 12, 1},
+	{128, 94, 0, 12, 1},
+	{130, 94, 2, 12, 1},
+	{132, 94, 4, 12, 1},
+	{134, 94, 6, 12, 1},
+	{136, 94, 8, 12, 1},
+	{138, 94, 10, 12, 1},
+	{140, 95, 0, 12, 1},
+	{149, 95, 9, 12, 1},
+	{151, 95, 11, 12, 1},
+	{153, 96, 1, 12, 1},
+	{155, 96, 3, 12, 1},
+	{157, 96, 5, 12, 1},
+	{159, 96, 7, 12, 1},
+	{161, 96, 9, 12, 1},
+	{165, 97, 1, 12, 1},
+	{184, 82, 0, 12, 1},
+	{188, 82, 4, 12, 1},
+	{192, 82, 8, 12, 1},
+	{196, 83, 0, 12, 1},
+};
+
 static int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 {
 	struct hw_mode_spec *spec = &rt2x00dev->spec;
@@ -5130,6 +5889,7 @@ static int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	char *default_power2;
 	unsigned int i;
 	u16 eeprom;
+	u32 reg;
 
 	/*
 	 * Disable powersaving as default on PCI devices.
@@ -5211,8 +5971,22 @@ static int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 		spec->supported_bands |= SUPPORT_BAND_5GHZ;
 		spec->num_channels = ARRAY_SIZE(rf_vals_3x);
 		spec->channels = rf_vals_3x;
+	} else if (rt2x00_rf(rt2x00dev, RF5592)) {
+		spec->supported_bands |= SUPPORT_BAND_5GHZ;
+
+		rt2800_register_read(rt2x00dev, MAC_DEBUG_INDEX, &reg);
+		if (rt2x00_get_field32(reg, MAC_DEBUG_INDEX_XTAL)) {
+			spec->num_channels = ARRAY_SIZE(rf_vals_5592_xtal40);
+			spec->channels = rf_vals_5592_xtal40;
+		} else {
+			spec->num_channels = ARRAY_SIZE(rf_vals_5592_xtal20);
+			spec->channels = rf_vals_5592_xtal20;
+		}
 	}
 
+	if (WARN_ON_ONCE(!spec->channels))
+		return -ENODEV;
+
 	/*
 	 * Initialize HT information.
 	 */
@@ -5300,11 +6074,55 @@ static int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	return 0;
 }
 
+static int rt2800_probe_rt(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+	u32 rt;
+	u32 rev;
+
+	if (rt2x00_rt(rt2x00dev, RT3290))
+		rt2800_register_read(rt2x00dev, MAC_CSR0_3290, &reg);
+	else
+		rt2800_register_read(rt2x00dev, MAC_CSR0, &reg);
+
+	rt = rt2x00_get_field32(reg, MAC_CSR0_CHIPSET);
+	rev = rt2x00_get_field32(reg, MAC_CSR0_REVISION);
+
+	switch (rt) {
+	case RT2860:
+	case RT2872:
+	case RT2883:
+	case RT3070:
+	case RT3071:
+	case RT3090:
+	case RT3290:
+	case RT3352:
+	case RT3390:
+	case RT3572:
+	case RT5390:
+	case RT5392:
+	case RT5592:
+		break;
+	default:
+		rt2x00_err(rt2x00dev, "Invalid RT chipset 0x%04x, rev %04x detected\n",
+			   rt, rev);
+		return -ENODEV;
+	}
+
+	rt2x00_set_rt(rt2x00dev, rt, rev);
+
+	return 0;
+}
+
 int rt2800_probe_hw(struct rt2x00_dev *rt2x00dev)
 {
 	int retval;
 	u32 reg;
 
+	retval = rt2800_probe_rt(rt2x00dev);
+	if (retval)
+		return retval;
+
 	/*
 	 * Allocate eeprom data.
 	 */
@@ -5546,7 +6364,8 @@ int rt2800_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	case IEEE80211_AMPDU_TX_OPERATIONAL:
 		break;
 	default:
-		WARNING((struct rt2x00_dev *)hw->priv, "Unknown AMPDU action\n");
+		rt2x00_warn((struct rt2x00_dev *)hw->priv,
+			    "Unknown AMPDU action\n");
 	}
 
 	return ret;
@@ -5563,7 +6382,7 @@ int rt2800_get_survey(struct ieee80211_hw *hw, int idx,
 	if (idx != 0)
 		return -ENOENT;
 
-	survey->channel = conf->channel;
+	survey->channel = conf->chandef.chan;
 
 	rt2800_register_read(rt2x00dev, CH_IDLE_STA, &idle);
 	rt2800_register_read(rt2x00dev, CH_BUSY_STA, &busy);
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.c b/drivers/net/wireless/rt2x00/rt2800pci.c
index ba5a05625aaa..6f4a861af336 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.c
+++ b/drivers/net/wireless/rt2x00/rt2800pci.c
@@ -72,7 +72,7 @@ static void rt2800pci_mcu_status(struct rt2x00_dev *rt2x00dev, const u8 token)
 		return;
 
 	for (i = 0; i < 200; i++) {
-		rt2x00pci_register_read(rt2x00dev, H2M_MAILBOX_CID, &reg);
+		rt2x00mmio_register_read(rt2x00dev, H2M_MAILBOX_CID, &reg);
 
 		if ((rt2x00_get_field32(reg, H2M_MAILBOX_CID_CMD0) == token) ||
 		    (rt2x00_get_field32(reg, H2M_MAILBOX_CID_CMD1) == token) ||
@@ -84,10 +84,10 @@ static void rt2800pci_mcu_status(struct rt2x00_dev *rt2x00dev, const u8 token)
 	}
 
 	if (i == 200)
-		ERROR(rt2x00dev, "MCU request failed, no response from hardware\n");
+		rt2x00_err(rt2x00dev, "MCU request failed, no response from hardware\n");
 
-	rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_STATUS, ~0);
-	rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_CID, ~0);
+	rt2x00mmio_register_write(rt2x00dev, H2M_MAILBOX_STATUS, ~0);
+	rt2x00mmio_register_write(rt2x00dev, H2M_MAILBOX_CID, ~0);
 }
 
 #if defined(CONFIG_SOC_RT288X) || defined(CONFIG_SOC_RT305X)
@@ -116,7 +116,7 @@ static void rt2800pci_eepromregister_read(struct eeprom_93cx6 *eeprom)
 	struct rt2x00_dev *rt2x00dev = eeprom->data;
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, E2PROM_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, E2PROM_CSR, &reg);
 
 	eeprom->reg_data_in = !!rt2x00_get_field32(reg, E2PROM_CSR_DATA_IN);
 	eeprom->reg_data_out = !!rt2x00_get_field32(reg, E2PROM_CSR_DATA_OUT);
@@ -138,7 +138,7 @@ static void rt2800pci_eepromregister_write(struct eeprom_93cx6 *eeprom)
 	rt2x00_set_field32(&reg, E2PROM_CSR_CHIP_SELECT,
 			   !!eeprom->reg_chip_select);
 
-	rt2x00pci_register_write(rt2x00dev, E2PROM_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, E2PROM_CSR, reg);
 }
 
 static int rt2800pci_read_eeprom_pci(struct rt2x00_dev *rt2x00dev)
@@ -146,7 +146,7 @@ static int rt2800pci_read_eeprom_pci(struct rt2x00_dev *rt2x00dev)
 	struct eeprom_93cx6 eeprom;
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, E2PROM_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, E2PROM_CSR, &reg);
 
 	eeprom.data = rt2x00dev;
 	eeprom.register_read = rt2800pci_eepromregister_read;
@@ -210,20 +210,20 @@ static void rt2800pci_start_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2x00pci_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
+		rt2x00mmio_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
 		rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_RX, 1);
-		rt2x00pci_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
+		rt2x00mmio_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
 		break;
 	case QID_BEACON:
-		rt2x00pci_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+		rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_TICKING, 1);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TBTT_ENABLE, 1);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 1);
-		rt2x00pci_register_write(rt2x00dev, BCN_TIME_CFG, reg);
+		rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
 
-		rt2x00pci_register_read(rt2x00dev, INT_TIMER_EN, &reg);
+		rt2x00mmio_register_read(rt2x00dev, INT_TIMER_EN, &reg);
 		rt2x00_set_field32(&reg, INT_TIMER_EN_PRE_TBTT_TIMER, 1);
-		rt2x00pci_register_write(rt2x00dev, INT_TIMER_EN, reg);
+		rt2x00mmio_register_write(rt2x00dev, INT_TIMER_EN, reg);
 		break;
 	default:
 		break;
@@ -241,13 +241,13 @@ static void rt2800pci_kick_queue(struct data_queue *queue)
 	case QID_AC_BE:
 	case QID_AC_BK:
 		entry = rt2x00queue_get_entry(queue, Q_INDEX);
-		rt2x00pci_register_write(rt2x00dev, TX_CTX_IDX(queue->qid),
-					 entry->entry_idx);
+		rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX(queue->qid),
+					  entry->entry_idx);
 		break;
 	case QID_MGMT:
 		entry = rt2x00queue_get_entry(queue, Q_INDEX);
-		rt2x00pci_register_write(rt2x00dev, TX_CTX_IDX(5),
-					 entry->entry_idx);
+		rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX(5),
+					  entry->entry_idx);
 		break;
 	default:
 		break;
@@ -261,20 +261,20 @@ static void rt2800pci_stop_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2x00pci_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
+		rt2x00mmio_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
 		rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_RX, 0);
-		rt2x00pci_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
+		rt2x00mmio_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
 		break;
 	case QID_BEACON:
-		rt2x00pci_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+		rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_TICKING, 0);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TBTT_ENABLE, 0);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 0);
-		rt2x00pci_register_write(rt2x00dev, BCN_TIME_CFG, reg);
+		rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
 
-		rt2x00pci_register_read(rt2x00dev, INT_TIMER_EN, &reg);
+		rt2x00mmio_register_read(rt2x00dev, INT_TIMER_EN, &reg);
 		rt2x00_set_field32(&reg, INT_TIMER_EN_PRE_TBTT_TIMER, 0);
-		rt2x00pci_register_write(rt2x00dev, INT_TIMER_EN, reg);
+		rt2x00mmio_register_write(rt2x00dev, INT_TIMER_EN, reg);
 
 		/*
 		 * Wait for current invocation to finish. The tasklet
@@ -314,19 +314,19 @@ static int rt2800pci_write_firmware(struct rt2x00_dev *rt2x00dev,
 	 */
 	reg = 0;
 	rt2x00_set_field32(&reg, PBF_SYS_CTRL_HOST_RAM_WRITE, 1);
-	rt2x00pci_register_write(rt2x00dev, PBF_SYS_CTRL, reg);
+	rt2x00mmio_register_write(rt2x00dev, PBF_SYS_CTRL, reg);
 
 	/*
 	 * Write firmware to device.
 	 */
-	rt2x00pci_register_multiwrite(rt2x00dev, FIRMWARE_IMAGE_BASE,
-				      data, len);
+	rt2x00mmio_register_multiwrite(rt2x00dev, FIRMWARE_IMAGE_BASE,
+				       data, len);
 
-	rt2x00pci_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00000);
-	rt2x00pci_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00001);
+	rt2x00mmio_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00000);
+	rt2x00mmio_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00001);
 
-	rt2x00pci_register_write(rt2x00dev, H2M_BBP_AGENT, 0);
-	rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_CSR, 0);
+	rt2x00mmio_register_write(rt2x00dev, H2M_BBP_AGENT, 0);
+	rt2x00mmio_register_write(rt2x00dev, H2M_MAILBOX_CSR, 0);
 
 	return 0;
 }
@@ -336,7 +336,7 @@ static int rt2800pci_write_firmware(struct rt2x00_dev *rt2x00dev,
  */
 static bool rt2800pci_get_entry_state(struct queue_entry *entry)
 {
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	u32 word;
 
 	if (entry->queue->qid == QID_RX) {
@@ -352,7 +352,7 @@ static bool rt2800pci_get_entry_state(struct queue_entry *entry)
 
 static void rt2800pci_clear_entry(struct queue_entry *entry)
 {
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
 	u32 word;
@@ -370,8 +370,8 @@ static void rt2800pci_clear_entry(struct queue_entry *entry)
 		 * Set RX IDX in register to inform hardware that we have
 		 * handled this entry and it is available for reuse again.
 		 */
-		rt2x00pci_register_write(rt2x00dev, RX_CRX_IDX,
-				      entry->entry_idx);
+		rt2x00mmio_register_write(rt2x00dev, RX_CRX_IDX,
+					  entry->entry_idx);
 	} else {
 		rt2x00_desc_read(entry_priv->desc, 1, &word);
 		rt2x00_set_field32(&word, TXD_W1_DMA_DONE, 1);
@@ -381,60 +381,65 @@ static void rt2800pci_clear_entry(struct queue_entry *entry)
 
 static int rt2800pci_init_queues(struct rt2x00_dev *rt2x00dev)
 {
-	struct queue_entry_priv_pci *entry_priv;
+	struct queue_entry_priv_mmio *entry_priv;
 
 	/*
 	 * Initialize registers.
 	 */
 	entry_priv = rt2x00dev->tx[0].entries[0].priv_data;
-	rt2x00pci_register_write(rt2x00dev, TX_BASE_PTR0, entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, TX_MAX_CNT0,
-				 rt2x00dev->tx[0].limit);
-	rt2x00pci_register_write(rt2x00dev, TX_CTX_IDX0, 0);
-	rt2x00pci_register_write(rt2x00dev, TX_DTX_IDX0, 0);
+	rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR0,
+				  entry_priv->desc_dma);
+	rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT0,
+				  rt2x00dev->tx[0].limit);
+	rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX0, 0);
+	rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX0, 0);
 
 	entry_priv = rt2x00dev->tx[1].entries[0].priv_data;
-	rt2x00pci_register_write(rt2x00dev, TX_BASE_PTR1, entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, TX_MAX_CNT1,
-				 rt2x00dev->tx[1].limit);
-	rt2x00pci_register_write(rt2x00dev, TX_CTX_IDX1, 0);
-	rt2x00pci_register_write(rt2x00dev, TX_DTX_IDX1, 0);
+	rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR1,
+				  entry_priv->desc_dma);
+	rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT1,
+				  rt2x00dev->tx[1].limit);
+	rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX1, 0);
+	rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX1, 0);
 
 	entry_priv = rt2x00dev->tx[2].entries[0].priv_data;
-	rt2x00pci_register_write(rt2x00dev, TX_BASE_PTR2, entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, TX_MAX_CNT2,
-				 rt2x00dev->tx[2].limit);
-	rt2x00pci_register_write(rt2x00dev, TX_CTX_IDX2, 0);
-	rt2x00pci_register_write(rt2x00dev, TX_DTX_IDX2, 0);
+	rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR2,
+				  entry_priv->desc_dma);
+	rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT2,
+				  rt2x00dev->tx[2].limit);
+	rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX2, 0);
+	rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX2, 0);
 
 	entry_priv = rt2x00dev->tx[3].entries[0].priv_data;
-	rt2x00pci_register_write(rt2x00dev, TX_BASE_PTR3, entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, TX_MAX_CNT3,
-				 rt2x00dev->tx[3].limit);
-	rt2x00pci_register_write(rt2x00dev, TX_CTX_IDX3, 0);
-	rt2x00pci_register_write(rt2x00dev, TX_DTX_IDX3, 0);
-
-	rt2x00pci_register_write(rt2x00dev, TX_BASE_PTR4, 0);
-	rt2x00pci_register_write(rt2x00dev, TX_MAX_CNT4, 0);
-	rt2x00pci_register_write(rt2x00dev, TX_CTX_IDX4, 0);
-	rt2x00pci_register_write(rt2x00dev, TX_DTX_IDX4, 0);
-
-	rt2x00pci_register_write(rt2x00dev, TX_BASE_PTR5, 0);
-	rt2x00pci_register_write(rt2x00dev, TX_MAX_CNT5, 0);
-	rt2x00pci_register_write(rt2x00dev, TX_CTX_IDX5, 0);
-	rt2x00pci_register_write(rt2x00dev, TX_DTX_IDX5, 0);
+	rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR3,
+				  entry_priv->desc_dma);
+	rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT3,
+				  rt2x00dev->tx[3].limit);
+	rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX3, 0);
+	rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX3, 0);
+
+	rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR4, 0);
+	rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT4, 0);
+	rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX4, 0);
+	rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX4, 0);
+
+	rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR5, 0);
+	rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT5, 0);
+	rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX5, 0);
+	rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX5, 0);
 
 	entry_priv = rt2x00dev->rx->entries[0].priv_data;
-	rt2x00pci_register_write(rt2x00dev, RX_BASE_PTR, entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, RX_MAX_CNT,
-				 rt2x00dev->rx[0].limit);
-	rt2x00pci_register_write(rt2x00dev, RX_CRX_IDX,
-				 rt2x00dev->rx[0].limit - 1);
-	rt2x00pci_register_write(rt2x00dev, RX_DRX_IDX, 0);
+	rt2x00mmio_register_write(rt2x00dev, RX_BASE_PTR,
+				  entry_priv->desc_dma);
+	rt2x00mmio_register_write(rt2x00dev, RX_MAX_CNT,
+				  rt2x00dev->rx[0].limit);
+	rt2x00mmio_register_write(rt2x00dev, RX_CRX_IDX,
+				  rt2x00dev->rx[0].limit - 1);
+	rt2x00mmio_register_write(rt2x00dev, RX_DRX_IDX, 0);
 
 	rt2800_disable_wpdma(rt2x00dev);
 
-	rt2x00pci_register_write(rt2x00dev, DELAY_INT_CFG, 0);
+	rt2x00mmio_register_write(rt2x00dev, DELAY_INT_CFG, 0);
 
 	return 0;
 }
@@ -453,8 +458,8 @@ static void rt2800pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	 * should clear the register to assure a clean state.
 	 */
 	if (state == STATE_RADIO_IRQ_ON) {
-		rt2x00pci_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
-		rt2x00pci_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
+		rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
+		rt2x00mmio_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
 	}
 
 	spin_lock_irqsave(&rt2x00dev->irqmask_lock, flags);
@@ -466,7 +471,7 @@ static void rt2800pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, INT_MASK_CSR_TX_FIFO_STATUS, 1);
 		rt2x00_set_field32(&reg, INT_MASK_CSR_AUTO_WAKEUP, 1);
 	}
-	rt2x00pci_register_write(rt2x00dev, INT_MASK_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
 	spin_unlock_irqrestore(&rt2x00dev->irqmask_lock, flags);
 
 	if (state == STATE_RADIO_IRQ_OFF) {
@@ -488,7 +493,7 @@ static int rt2800pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Reset DMA indexes
 	 */
-	rt2x00pci_register_read(rt2x00dev, WPDMA_RST_IDX, &reg);
+	rt2x00mmio_register_read(rt2x00dev, WPDMA_RST_IDX, &reg);
 	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX0, 1);
 	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX1, 1);
 	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX2, 1);
@@ -496,29 +501,29 @@ static int rt2800pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX4, 1);
 	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX5, 1);
 	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DRX_IDX0, 1);
-	rt2x00pci_register_write(rt2x00dev, WPDMA_RST_IDX, reg);
+	rt2x00mmio_register_write(rt2x00dev, WPDMA_RST_IDX, reg);
 
-	rt2x00pci_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00000e1f);
-	rt2x00pci_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00000e00);
+	rt2x00mmio_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00000e1f);
+	rt2x00mmio_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00000e00);
 
 	if (rt2x00_is_pcie(rt2x00dev) &&
 	    (rt2x00_rt(rt2x00dev, RT3572) ||
 	     rt2x00_rt(rt2x00dev, RT5390) ||
 	     rt2x00_rt(rt2x00dev, RT5392))) {
-		rt2x00pci_register_read(rt2x00dev, AUX_CTRL, &reg);
+		rt2x00mmio_register_read(rt2x00dev, AUX_CTRL, &reg);
 		rt2x00_set_field32(&reg, AUX_CTRL_FORCE_PCIE_CLK, 1);
 		rt2x00_set_field32(&reg, AUX_CTRL_WAKE_PCIE_EN, 1);
-		rt2x00pci_register_write(rt2x00dev, AUX_CTRL, reg);
+		rt2x00mmio_register_write(rt2x00dev, AUX_CTRL, reg);
 	}
 
-	rt2x00pci_register_write(rt2x00dev, PWR_PIN_CFG, 0x00000003);
+	rt2x00mmio_register_write(rt2x00dev, PWR_PIN_CFG, 0x00000003);
 
 	reg = 0;
 	rt2x00_set_field32(&reg, MAC_SYS_CTRL_RESET_CSR, 1);
 	rt2x00_set_field32(&reg, MAC_SYS_CTRL_RESET_BBP, 1);
-	rt2x00pci_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
+	rt2x00mmio_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
 
-	rt2x00pci_register_write(rt2x00dev, MAC_SYS_CTRL, 0x00000000);
+	rt2x00mmio_register_write(rt2x00dev, MAC_SYS_CTRL, 0x00000000);
 
 	return 0;
 }
@@ -538,8 +543,8 @@ static int rt2800pci_enable_radio(struct rt2x00_dev *rt2x00dev)
 		return retval;
 
 	/* After resume MCU_BOOT_SIGNAL will trash these. */
-	rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_STATUS, ~0);
-	rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_CID, ~0);
+	rt2x00mmio_register_write(rt2x00dev, H2M_MAILBOX_STATUS, ~0);
+	rt2x00mmio_register_write(rt2x00dev, H2M_MAILBOX_CID, ~0);
 
 	rt2800_mcu_request(rt2x00dev, MCU_SLEEP, TOKEN_RADIO_OFF, 0xff, 0x02);
 	rt2800pci_mcu_status(rt2x00dev, TOKEN_RADIO_OFF);
@@ -554,8 +559,8 @@ static void rt2800pci_disable_radio(struct rt2x00_dev *rt2x00dev)
 {
 	if (rt2x00_is_soc(rt2x00dev)) {
 		rt2800_disable_radio(rt2x00dev);
-		rt2x00pci_register_write(rt2x00dev, PWR_PIN_CFG, 0);
-		rt2x00pci_register_write(rt2x00dev, TX_PIN_CFG, 0);
+		rt2x00mmio_register_write(rt2x00dev, PWR_PIN_CFG, 0);
+		rt2x00mmio_register_write(rt2x00dev, TX_PIN_CFG, 0);
 	}
 }
 
@@ -567,10 +572,10 @@ static int rt2800pci_set_state(struct rt2x00_dev *rt2x00dev,
 				   0, 0x02);
 		rt2800pci_mcu_status(rt2x00dev, TOKEN_WAKEUP);
 	} else if (state == STATE_SLEEP) {
-		rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_STATUS,
-					 0xffffffff);
-		rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_CID,
-					 0xffffffff);
+		rt2x00mmio_register_write(rt2x00dev, H2M_MAILBOX_STATUS,
+					  0xffffffff);
+		rt2x00mmio_register_write(rt2x00dev, H2M_MAILBOX_CID,
+					  0xffffffff);
 		rt2800_mcu_request(rt2x00dev, MCU_SLEEP, TOKEN_SLEEP,
 				   0xff, 0x01);
 	}
@@ -611,8 +616,8 @@ static int rt2800pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 	}
 
 	if (unlikely(retval))
-		ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
-		      state, retval);
+		rt2x00_err(rt2x00dev, "Device failed to enter state %d (%d)\n",
+			   state, retval);
 
 	return retval;
 }
@@ -629,7 +634,7 @@ static void rt2800pci_write_tx_desc(struct queue_entry *entry,
 				    struct txentry_desc *txdesc)
 {
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	__le32 *txd = entry_priv->desc;
 	u32 word;
 
@@ -683,7 +688,7 @@ static void rt2800pci_write_tx_desc(struct queue_entry *entry,
 static void rt2800pci_fill_rxdone(struct queue_entry *entry,
 				  struct rxdone_entry_desc *rxdesc)
 {
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	__le32 *rxd = entry_priv->desc;
 	u32 word;
 
@@ -743,10 +748,90 @@ static void rt2800pci_wakeup(struct rt2x00_dev *rt2x00dev)
 	rt2800_config(rt2x00dev, &libconf, IEEE80211_CONF_CHANGE_PS);
 }
 
+static bool rt2800pci_txdone_entry_check(struct queue_entry *entry, u32 status)
+{
+	__le32 *txwi;
+	u32 word;
+	int wcid, tx_wcid;
+
+	wcid = rt2x00_get_field32(status, TX_STA_FIFO_WCID);
+
+	txwi = rt2800_drv_get_txwi(entry);
+	rt2x00_desc_read(txwi, 1, &word);
+	tx_wcid = rt2x00_get_field32(word, TXWI_W1_WIRELESS_CLI_ID);
+
+	return (tx_wcid == wcid);
+}
+
+static bool rt2800pci_txdone_find_entry(struct queue_entry *entry, void *data)
+{
+	u32 status = *(u32 *)data;
+
+	/*
+	 * rt2800pci hardware might reorder frames when exchanging traffic
+	 * with multiple BA enabled STAs.
+	 *
+	 * For example, a tx queue
+	 *    [ STA1 | STA2 | STA1 | STA2 ]
+	 * can result in tx status reports
+	 *    [ STA1 | STA1 | STA2 | STA2 ]
+	 * when the hw decides to aggregate the frames for STA1 into one AMPDU.
+	 *
+	 * To mitigate this effect, associate the tx status to the first frame
+	 * in the tx queue with a matching wcid.
+	 */
+	if (rt2800pci_txdone_entry_check(entry, status) &&
+	    !test_bit(ENTRY_DATA_STATUS_SET, &entry->flags)) {
+		/*
+		 * Got a matching frame, associate the tx status with
+		 * the frame
+		 */
+		entry->status = status;
+		set_bit(ENTRY_DATA_STATUS_SET, &entry->flags);
+		return true;
+	}
+
+	/* Check the next frame */
+	return false;
+}
+
+static bool rt2800pci_txdone_match_first(struct queue_entry *entry, void *data)
+{
+	u32 status = *(u32 *)data;
+
+	/*
+	 * Find the first frame without tx status and assign this status to it
+	 * regardless if it matches or not.
+	 */
+	if (!test_bit(ENTRY_DATA_STATUS_SET, &entry->flags)) {
+		/*
+		 * Got a matching frame, associate the tx status with
+		 * the frame
+		 */
+		entry->status = status;
+		set_bit(ENTRY_DATA_STATUS_SET, &entry->flags);
+		return true;
+	}
+
+	/* Check the next frame */
+	return false;
+}
+static bool rt2800pci_txdone_release_entries(struct queue_entry *entry,
+					     void *data)
+{
+	if (test_bit(ENTRY_DATA_STATUS_SET, &entry->flags)) {
+		rt2800_txdone_entry(entry, entry->status,
+				    rt2800pci_get_txwi(entry));
+		return false;
+	}
+
+	/* No more frames to release */
+	return true;
+}
+
 static bool rt2800pci_txdone(struct rt2x00_dev *rt2x00dev)
 {
 	struct data_queue *queue;
-	struct queue_entry *entry;
 	u32 status;
 	u8 qid;
 	int max_tx_done = 16;
@@ -758,8 +843,8 @@ static bool rt2800pci_txdone(struct rt2x00_dev *rt2x00dev)
 			 * Unknown queue, this shouldn't happen. Just drop
 			 * this tx status.
 			 */
-			WARNING(rt2x00dev, "Got TX status report with "
-					   "unexpected pid %u, dropping\n", qid);
+			rt2x00_warn(rt2x00dev, "Got TX status report with unexpected pid %u, dropping\n",
+				    qid);
 			break;
 		}
 
@@ -769,8 +854,8 @@ static bool rt2800pci_txdone(struct rt2x00_dev *rt2x00dev)
 			 * The queue is NULL, this shouldn't happen. Stop
 			 * processing here and drop the tx status
 			 */
-			WARNING(rt2x00dev, "Got TX status for an unavailable "
-					   "queue %u, dropping\n", qid);
+			rt2x00_warn(rt2x00dev, "Got TX status for an unavailable queue %u, dropping\n",
+				    qid);
 			break;
 		}
 
@@ -779,13 +864,37 @@ static bool rt2800pci_txdone(struct rt2x00_dev *rt2x00dev)
 			 * The queue is empty. Stop processing here
 			 * and drop the tx status.
 			 */
-			WARNING(rt2x00dev, "Got TX status for an empty "
-					   "queue %u, dropping\n", qid);
+			rt2x00_warn(rt2x00dev, "Got TX status for an empty queue %u, dropping\n",
+				    qid);
 			break;
 		}
 
-		entry = rt2x00queue_get_entry(queue, Q_INDEX_DONE);
-		rt2800_txdone_entry(entry, status, rt2800pci_get_txwi(entry));
+		/*
+		 * Let's associate this tx status with the first
+		 * matching frame.
+		 */
+		if (!rt2x00queue_for_each_entry(queue, Q_INDEX_DONE,
+						Q_INDEX, &status,
+						rt2800pci_txdone_find_entry)) {
+			/*
+			 * We cannot match the tx status to any frame, so just
+			 * use the first one.
+			 */
+			if (!rt2x00queue_for_each_entry(queue, Q_INDEX_DONE,
+							Q_INDEX, &status,
+							rt2800pci_txdone_match_first)) {
+				rt2x00_warn(rt2x00dev, "No frame found for TX status on queue %u, dropping\n",
+					    qid);
+				break;
+			}
+		}
+
+		/*
+		 * Release all frames with a valid tx status.
+		 */
+		rt2x00queue_for_each_entry(queue, Q_INDEX_DONE,
+					   Q_INDEX, NULL,
+					   rt2800pci_txdone_release_entries);
 
 		if (--max_tx_done == 0)
 			break;
@@ -804,9 +913,9 @@ static inline void rt2800pci_enable_interrupt(struct rt2x00_dev *rt2x00dev,
 	 * access needs locking.
 	 */
 	spin_lock_irq(&rt2x00dev->irqmask_lock);
-	rt2x00pci_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR, &reg);
 	rt2x00_set_field32(&reg, irq_field, 1);
-	rt2x00pci_register_write(rt2x00dev, INT_MASK_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
 	spin_unlock_irq(&rt2x00dev->irqmask_lock);
 }
 
@@ -847,15 +956,15 @@ static void rt2800pci_tbtt_tasklet(unsigned long data)
 		 * interval every 64 beacons by 64us to mitigate this effect.
 		 */
 		if (drv_data->tbtt_tick == (BCN_TBTT_OFFSET - 2)) {
-			rt2x00pci_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+			rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
 			rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_INTERVAL,
 					   (rt2x00dev->beacon_int * 16) - 1);
-			rt2x00pci_register_write(rt2x00dev, BCN_TIME_CFG, reg);
+			rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
 		} else if (drv_data->tbtt_tick == (BCN_TBTT_OFFSET - 1)) {
-			rt2x00pci_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+			rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
 			rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_INTERVAL,
 					   (rt2x00dev->beacon_int * 16));
-			rt2x00pci_register_write(rt2x00dev, BCN_TIME_CFG, reg);
+			rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
 		}
 		drv_data->tbtt_tick++;
 		drv_data->tbtt_tick %= BCN_TBTT_OFFSET;
@@ -868,7 +977,7 @@ static void rt2800pci_tbtt_tasklet(unsigned long data)
 static void rt2800pci_rxdone_tasklet(unsigned long data)
 {
 	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
-	if (rt2x00pci_rxdone(rt2x00dev))
+	if (rt2x00mmio_rxdone(rt2x00dev))
 		tasklet_schedule(&rt2x00dev->rxdone_tasklet);
 	else if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
 		rt2800pci_enable_interrupt(rt2x00dev, INT_MASK_CSR_RX_DONE);
@@ -906,14 +1015,13 @@ static void rt2800pci_txstatus_interrupt(struct rt2x00_dev *rt2x00dev)
 	 * need to lock the kfifo.
 	 */
 	for (i = 0; i < rt2x00dev->ops->tx->entry_num; i++) {
-		rt2x00pci_register_read(rt2x00dev, TX_STA_FIFO, &status);
+		rt2x00mmio_register_read(rt2x00dev, TX_STA_FIFO, &status);
 
 		if (!rt2x00_get_field32(status, TX_STA_FIFO_VALID))
 			break;
 
 		if (!kfifo_put(&rt2x00dev->txstatus_fifo, &status)) {
-			WARNING(rt2x00dev, "TX status FIFO overrun,"
-				"drop tx status report.\n");
+			rt2x00_warn(rt2x00dev, "TX status FIFO overrun, drop tx status report\n");
 			break;
 		}
 	}
@@ -928,8 +1036,8 @@ static irqreturn_t rt2800pci_interrupt(int irq, void *dev_instance)
 	u32 reg, mask;
 
 	/* Read status and ACK all interrupts */
-	rt2x00pci_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
-	rt2x00pci_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
+	rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
+	rt2x00mmio_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
 
 	if (!reg)
 		return IRQ_NONE;
@@ -969,9 +1077,9 @@ static irqreturn_t rt2800pci_interrupt(int irq, void *dev_instance)
 	 * the tasklet will reenable the appropriate interrupts.
 	 */
 	spin_lock(&rt2x00dev->irqmask_lock);
-	rt2x00pci_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR, &reg);
 	reg &= mask;
-	rt2x00pci_register_write(rt2x00dev, INT_MASK_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
 	spin_unlock(&rt2x00dev->irqmask_lock);
 
 	return IRQ_HANDLED;
@@ -1022,13 +1130,13 @@ static const struct ieee80211_ops rt2800pci_mac80211_ops = {
 };
 
 static const struct rt2800_ops rt2800pci_rt2800_ops = {
-	.register_read		= rt2x00pci_register_read,
-	.register_read_lock	= rt2x00pci_register_read, /* same for PCI */
-	.register_write		= rt2x00pci_register_write,
-	.register_write_lock	= rt2x00pci_register_write, /* same for PCI */
-	.register_multiread	= rt2x00pci_register_multiread,
-	.register_multiwrite	= rt2x00pci_register_multiwrite,
-	.regbusy_read		= rt2x00pci_regbusy_read,
+	.register_read		= rt2x00mmio_register_read,
+	.register_read_lock	= rt2x00mmio_register_read, /* same for PCI */
+	.register_write		= rt2x00mmio_register_write,
+	.register_write_lock	= rt2x00mmio_register_write, /* same for PCI */
+	.register_multiread	= rt2x00mmio_register_multiread,
+	.register_multiwrite	= rt2x00mmio_register_multiwrite,
+	.regbusy_read		= rt2x00mmio_regbusy_read,
 	.read_eeprom		= rt2800pci_read_eeprom,
 	.hwcrypt_disabled	= rt2800pci_hwcrypt_disabled,
 	.drv_write_firmware	= rt2800pci_write_firmware,
@@ -1047,8 +1155,8 @@ static const struct rt2x00lib_ops rt2800pci_rt2x00_ops = {
 	.get_firmware_name	= rt2800pci_get_firmware_name,
 	.check_firmware		= rt2800_check_firmware,
 	.load_firmware		= rt2800_load_firmware,
-	.initialize		= rt2x00pci_initialize,
-	.uninitialize		= rt2x00pci_uninitialize,
+	.initialize		= rt2x00mmio_initialize,
+	.uninitialize		= rt2x00mmio_uninitialize,
 	.get_entry_state	= rt2800pci_get_entry_state,
 	.clear_entry		= rt2800pci_clear_entry,
 	.set_device_state	= rt2800pci_set_device_state,
@@ -1061,7 +1169,7 @@ static const struct rt2x00lib_ops rt2800pci_rt2x00_ops = {
 	.start_queue		= rt2800pci_start_queue,
 	.kick_queue		= rt2800pci_kick_queue,
 	.stop_queue		= rt2800pci_stop_queue,
-	.flush_queue		= rt2x00pci_flush_queue,
+	.flush_queue		= rt2x00mmio_flush_queue,
 	.write_tx_desc		= rt2800pci_write_tx_desc,
 	.write_tx_data		= rt2800_write_tx_data,
 	.write_beacon		= rt2800_write_beacon,
@@ -1082,21 +1190,24 @@ static const struct data_queue_desc rt2800pci_queue_rx = {
 	.entry_num		= 128,
 	.data_size		= AGGREGATION_SIZE,
 	.desc_size		= RXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci),
+	.winfo_size		= RXWI_DESC_SIZE,
+	.priv_size		= sizeof(struct queue_entry_priv_mmio),
 };
 
 static const struct data_queue_desc rt2800pci_queue_tx = {
 	.entry_num		= 64,
 	.data_size		= AGGREGATION_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci),
+	.winfo_size		= TXWI_DESC_SIZE,
+	.priv_size		= sizeof(struct queue_entry_priv_mmio),
 };
 
 static const struct data_queue_desc rt2800pci_queue_bcn = {
 	.entry_num		= 8,
 	.data_size		= 0, /* No DMA required for beacons */
-	.desc_size		= TXWI_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci),
+	.desc_size		= TXD_DESC_SIZE,
+	.winfo_size		= TXWI_DESC_SIZE,
+	.priv_size		= sizeof(struct queue_entry_priv_mmio),
 };
 
 static const struct rt2x00_ops rt2800pci_ops = {
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index 098613ed93fb..ac854d75bd6c 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -128,9 +128,9 @@ static inline bool rt2800usb_entry_txstatus_timeout(struct queue_entry *entry)
 
 	tout = time_after(jiffies, entry->last_action + msecs_to_jiffies(100));
 	if (unlikely(tout))
-		WARNING(entry->queue->rt2x00dev,
-			"TX status timeout for entry %d in queue %d\n",
-			entry->entry_idx, entry->queue->qid);
+		rt2x00_warn(entry->queue->rt2x00dev,
+			    "TX status timeout for entry %d in queue %d\n",
+			    entry->entry_idx, entry->queue->qid);
 	return tout;
 
 }
@@ -154,7 +154,8 @@ static bool rt2800usb_tx_sta_fifo_read_completed(struct rt2x00_dev *rt2x00dev,
 	bool valid;
 
 	if (urb_status) {
-		WARNING(rt2x00dev, "TX status read failed %d\n", urb_status);
+		rt2x00_warn(rt2x00dev, "TX status read failed %d\n",
+			    urb_status);
 
 		goto stop_reading;
 	}
@@ -162,7 +163,7 @@ static bool rt2800usb_tx_sta_fifo_read_completed(struct rt2x00_dev *rt2x00dev,
 	valid = rt2x00_get_field32(tx_status, TX_STA_FIFO_VALID);
 	if (valid) {
 		if (!kfifo_put(&rt2x00dev->txstatus_fifo, &tx_status))
-			WARNING(rt2x00dev, "TX status FIFO overrun\n");
+			rt2x00_warn(rt2x00dev, "TX status FIFO overrun\n");
 
 		queue_work(rt2x00dev->workqueue, &rt2x00dev->txdone_work);
 
@@ -269,7 +270,7 @@ static int rt2800usb_write_firmware(struct rt2x00_dev *rt2x00dev,
 					     0, USB_MODE_FIRMWARE,
 					     REGISTER_TIMEOUT_FIRMWARE);
 	if (status < 0) {
-		ERROR(rt2x00dev, "Failed to write Firmware to device.\n");
+		rt2x00_err(rt2x00dev, "Failed to write Firmware to device\n");
 		return status;
 	}
 
@@ -392,8 +393,8 @@ static int rt2800usb_set_device_state(struct rt2x00_dev *rt2x00dev,
 	}
 
 	if (unlikely(retval))
-		ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
-		      state, retval);
+		rt2x00_err(rt2x00dev, "Device failed to enter state %d (%d)\n",
+			   state, retval);
 
 	return retval;
 }
@@ -408,8 +409,7 @@ static void rt2800usb_watchdog(struct rt2x00_dev *rt2x00dev)
 
 	rt2x00usb_register_read(rt2x00dev, TXRXQ_PCNT, &reg);
 	if (rt2x00_get_field32(reg, TXRXQ_PCNT_TX0Q)) {
-		WARNING(rt2x00dev, "TX HW queue 0 timed out,"
-			" invoke forced kick\n");
+		rt2x00_warn(rt2x00dev, "TX HW queue 0 timed out, invoke forced kick\n");
 
 		rt2x00usb_register_write(rt2x00dev, PBF_CFG, 0xf40012);
 
@@ -424,8 +424,7 @@ static void rt2800usb_watchdog(struct rt2x00_dev *rt2x00dev)
 
 	rt2x00usb_register_read(rt2x00dev, TXRXQ_PCNT, &reg);
 	if (rt2x00_get_field32(reg, TXRXQ_PCNT_TX1Q)) {
-		WARNING(rt2x00dev, "TX HW queue 1 timed out,"
-			" invoke forced kick\n");
+		rt2x00_warn(rt2x00dev, "TX HW queue 1 timed out, invoke forced kick\n");
 
 		rt2x00usb_register_write(rt2x00dev, PBF_CFG, 0xf4000a);
 
@@ -485,7 +484,7 @@ static void rt2800usb_write_tx_desc(struct queue_entry *entry,
 	 */
 	skbdesc->flags |= SKBDESC_DESC_IN_SKB;
 	skbdesc->desc = txi;
-	skbdesc->desc_len = TXINFO_DESC_SIZE + TXWI_DESC_SIZE;
+	skbdesc->desc_len = TXINFO_DESC_SIZE + entry->queue->winfo_size;
 }
 
 /*
@@ -540,9 +539,9 @@ rt2800usb_txdone_entry_check(struct queue_entry *entry, u32 reg)
 	tx_pid  = rt2x00_get_field32(word, TXWI_W1_PACKETID);
 
 	if (wcid != tx_wcid || ack != tx_ack || (!is_agg && pid != tx_pid)) {
-		DEBUG(entry->queue->rt2x00dev,
-		      "TX status report missed for queue %d entry %d\n",
-		      entry->queue->qid, entry->entry_idx);
+		rt2x00_dbg(entry->queue->rt2x00dev,
+			   "TX status report missed for queue %d entry %d\n",
+			   entry->queue->qid, entry->entry_idx);
 		return TXDONE_UNKNOWN;
 	}
 
@@ -566,8 +565,8 @@ static void rt2800usb_txdone(struct rt2x00_dev *rt2x00dev)
 		queue = rt2x00queue_get_tx_queue(rt2x00dev, qid);
 
 		if (unlikely(rt2x00queue_empty(queue))) {
-			WARNING(rt2x00dev, "Got TX status for an empty "
-					   "queue %u, dropping\n", qid);
+			rt2x00_warn(rt2x00dev, "Got TX status for an empty queue %u, dropping\n",
+				    qid);
 			break;
 		}
 
@@ -575,8 +574,8 @@ static void rt2800usb_txdone(struct rt2x00_dev *rt2x00dev)
 
 		if (unlikely(test_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags) ||
 			     !test_bit(ENTRY_DATA_STATUS_PENDING, &entry->flags))) {
-			WARNING(rt2x00dev, "Data pending for entry %u "
-					   "in queue %u\n", entry->entry_idx, qid);
+			rt2x00_warn(rt2x00dev, "Data pending for entry %u in queue %u\n",
+				    entry->entry_idx, qid);
 			break;
 		}
 
@@ -677,8 +676,8 @@ static void rt2800usb_fill_rxdone(struct queue_entry *entry,
 	 */
 	if (unlikely(rx_pkt_len == 0 ||
 			rx_pkt_len > entry->queue->data_size)) {
-		ERROR(entry->queue->rt2x00dev,
-			"Bad frame size %d, forcing to 0\n", rx_pkt_len);
+		rt2x00_err(entry->queue->rt2x00dev,
+			   "Bad frame size %d, forcing to 0\n", rx_pkt_len);
 		return;
 	}
 
@@ -853,21 +852,24 @@ static const struct rt2x00lib_ops rt2800usb_rt2x00_ops = {
 static const struct data_queue_desc rt2800usb_queue_rx = {
 	.entry_num		= 128,
 	.data_size		= AGGREGATION_SIZE,
-	.desc_size		= RXINFO_DESC_SIZE + RXWI_DESC_SIZE,
+	.desc_size		= RXINFO_DESC_SIZE,
+	.winfo_size		= RXWI_DESC_SIZE,
 	.priv_size		= sizeof(struct queue_entry_priv_usb),
 };
 
 static const struct data_queue_desc rt2800usb_queue_tx = {
 	.entry_num		= 16,
 	.data_size		= AGGREGATION_SIZE,
-	.desc_size		= TXINFO_DESC_SIZE + TXWI_DESC_SIZE,
+	.desc_size		= TXINFO_DESC_SIZE,
+	.winfo_size		= TXWI_DESC_SIZE,
 	.priv_size		= sizeof(struct queue_entry_priv_usb),
 };
 
 static const struct data_queue_desc rt2800usb_queue_bcn = {
 	.entry_num		= 8,
 	.data_size		= MGMT_FRAME_SIZE,
-	.desc_size		= TXINFO_DESC_SIZE + TXWI_DESC_SIZE,
+	.desc_size		= TXINFO_DESC_SIZE,
+	.winfo_size		= TXWI_DESC_SIZE,
 	.priv_size		= sizeof(struct queue_entry_priv_usb),
 };
 
@@ -890,6 +892,50 @@ static const struct rt2x00_ops rt2800usb_ops = {
 #endif /* CONFIG_RT2X00_LIB_DEBUGFS */
 };
 
+static const struct data_queue_desc rt2800usb_queue_rx_5592 = {
+	.entry_num		= 128,
+	.data_size		= AGGREGATION_SIZE,
+	.desc_size		= RXINFO_DESC_SIZE,
+	.winfo_size		= RXWI_DESC_SIZE_5592,
+	.priv_size		= sizeof(struct queue_entry_priv_usb),
+};
+
+static const struct data_queue_desc rt2800usb_queue_tx_5592 = {
+	.entry_num		= 16,
+	.data_size		= AGGREGATION_SIZE,
+	.desc_size		= TXINFO_DESC_SIZE,
+	.winfo_size		= TXWI_DESC_SIZE_5592,
+	.priv_size		= sizeof(struct queue_entry_priv_usb),
+};
+
+static const struct data_queue_desc rt2800usb_queue_bcn_5592 = {
+	.entry_num		= 8,
+	.data_size		= MGMT_FRAME_SIZE,
+	.desc_size		= TXINFO_DESC_SIZE,
+	.winfo_size		= TXWI_DESC_SIZE_5592,
+	.priv_size		= sizeof(struct queue_entry_priv_usb),
+};
+
+
+static const struct rt2x00_ops rt2800usb_ops_5592 = {
+	.name			= KBUILD_MODNAME,
+	.drv_data_size		= sizeof(struct rt2800_drv_data),
+	.max_ap_intf		= 8,
+	.eeprom_size		= EEPROM_SIZE,
+	.rf_size		= RF_SIZE,
+	.tx_queues		= NUM_TX_QUEUES,
+	.extra_tx_headroom	= TXINFO_DESC_SIZE + TXWI_DESC_SIZE_5592,
+	.rx			= &rt2800usb_queue_rx_5592,
+	.tx			= &rt2800usb_queue_tx_5592,
+	.bcn			= &rt2800usb_queue_bcn_5592,
+	.lib			= &rt2800usb_rt2x00_ops,
+	.drv			= &rt2800usb_rt2800_ops,
+	.hw			= &rt2800usb_mac80211_ops,
+#ifdef CONFIG_RT2X00_LIB_DEBUGFS
+	.debugfs		= &rt2800_rt2x00debug,
+#endif /* CONFIG_RT2X00_LIB_DEBUGFS */
+};
+
 /*
  * rt2800usb module information.
  */
@@ -1200,6 +1246,18 @@ static struct usb_device_id rt2800usb_device_table[] = {
 	{ USB_DEVICE(0x148f, 0x5370) },
 	{ USB_DEVICE(0x148f, 0x5372) },
 #endif
+#ifdef CONFIG_RT2800USB_RT55XX
+	/* Arcadyan */
+	{ USB_DEVICE(0x043e, 0x7a32), .driver_info = 5592 },
+	/* AVM GmbH */
+	{ USB_DEVICE(0x057c, 0x8501), .driver_info = 5592 },
+	/* D-Link DWA-160-B2 */
+	{ USB_DEVICE(0x2001, 0x3c1a), .driver_info = 5592 },
+	/* Proware */
+	{ USB_DEVICE(0x043e, 0x7a13), .driver_info = 5592 },
+	/* Ralink */
+	{ USB_DEVICE(0x148f, 0x5572), .driver_info = 5592 },
+#endif
 #ifdef CONFIG_RT2800USB_UNKNOWN
 	/*
 	 * Unclear what kind of devices these are (they aren't supported by the
@@ -1303,6 +1361,9 @@ MODULE_LICENSE("GPL");
 static int rt2800usb_probe(struct usb_interface *usb_intf,
 			   const struct usb_device_id *id)
 {
+	if (id->driver_info == 5592)
+		return rt2x00usb_probe(usb_intf, &rt2800usb_ops_5592);
+
 	return rt2x00usb_probe(usb_intf, &rt2800usb_ops);
 }
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index 086abb403a4f..7510723a8c37 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -54,47 +54,36 @@
 #define DRV_VERSION	"2.3.0"
 #define DRV_PROJECT	"http://rt2x00.serialmonkey.com"
 
-/*
- * Debug definitions.
+/* Debug definitions.
  * Debug output has to be enabled during compile time.
  */
-#define DEBUG_PRINTK_MSG(__dev, __kernlvl, __lvl, __msg, __args...)	\
-	printk(__kernlvl "%s -> %s: %s - " __msg,			\
-	       wiphy_name((__dev)->hw->wiphy), __func__, __lvl, ##__args)
-
-#define DEBUG_PRINTK_PROBE(__kernlvl, __lvl, __msg, __args...)	\
-	printk(__kernlvl "%s -> %s: %s - " __msg,		\
-	       KBUILD_MODNAME, __func__, __lvl, ##__args)
-
 #ifdef CONFIG_RT2X00_DEBUG
-#define DEBUG_PRINTK(__dev, __kernlvl, __lvl, __msg, __args...)	\
-	DEBUG_PRINTK_MSG(__dev, __kernlvl, __lvl, __msg, ##__args)
-#else
-#define DEBUG_PRINTK(__dev, __kernlvl, __lvl, __msg, __args...)	\
-	do { } while (0)
+#define DEBUG
 #endif /* CONFIG_RT2X00_DEBUG */
 
-/*
- * Various debug levels.
- * The debug levels PANIC and ERROR both indicate serious problems,
- * for this reason they should never be ignored.
- * The special ERROR_PROBE message is for messages that are generated
- * when the rt2x00_dev is not yet initialized.
+/* Utility printing macros
+ * rt2x00_probe_err is for messages when rt2x00_dev is uninitialized
  */
-#define PANIC(__dev, __msg, __args...) \
-	DEBUG_PRINTK_MSG(__dev, KERN_CRIT, "Panic", __msg, ##__args)
-#define ERROR(__dev, __msg, __args...)	\
-	DEBUG_PRINTK_MSG(__dev, KERN_ERR, "Error", __msg, ##__args)
-#define ERROR_PROBE(__msg, __args...) \
-	DEBUG_PRINTK_PROBE(KERN_ERR, "Error", __msg, ##__args)
-#define WARNING(__dev, __msg, __args...) \
-	DEBUG_PRINTK_MSG(__dev, KERN_WARNING, "Warning", __msg, ##__args)
-#define INFO(__dev, __msg, __args...) \
-	DEBUG_PRINTK_MSG(__dev, KERN_INFO, "Info", __msg, ##__args)
-#define DEBUG(__dev, __msg, __args...) \
-	DEBUG_PRINTK(__dev, KERN_DEBUG, "Debug", __msg, ##__args)
-#define EEPROM(__dev, __msg, __args...) \
-	DEBUG_PRINTK(__dev, KERN_DEBUG, "EEPROM recovery", __msg, ##__args)
+#define rt2x00_probe_err(fmt, ...)					\
+	printk(KERN_ERR KBUILD_MODNAME ": %s: Error - " fmt,		\
+	       __func__, ##__VA_ARGS__)
+#define rt2x00_err(dev, fmt, ...)					\
+	wiphy_err((dev)->hw->wiphy, "%s: Error - " fmt,			\
+		  __func__, ##__VA_ARGS__)
+#define rt2x00_warn(dev, fmt, ...)					\
+	wiphy_warn((dev)->hw->wiphy, "%s: Warning - " fmt,		\
+		   __func__, ##__VA_ARGS__)
+#define rt2x00_info(dev, fmt, ...)					\
+	wiphy_info((dev)->hw->wiphy, "%s: Info - " fmt,			\
+		   __func__, ##__VA_ARGS__)
+
+/* Various debug levels */
+#define rt2x00_dbg(dev, fmt, ...)					\
+	wiphy_dbg((dev)->hw->wiphy, "%s: Debug - " fmt,			\
+		  __func__, ##__VA_ARGS__)
+#define rt2x00_eeprom_dbg(dev, fmt, ...)				\
+	wiphy_dbg((dev)->hw->wiphy, "%s: EEPROM recovery - " fmt,	\
+		  __func__, ##__VA_ARGS__)
 
 /*
  * Duration calculations
@@ -193,6 +182,7 @@ struct rt2x00_chip {
 #define RT3883		0x3883	/* WSOC */
 #define RT5390		0x5390  /* 2.4GHz */
 #define RT5392		0x5392  /* 2.4GHz */
+#define RT5592		0x5592
 
 	u16 rf;
 	u16 rev;
@@ -1064,8 +1054,7 @@ static inline void rt2x00_rf_write(struct rt2x00_dev *rt2x00dev,
 }
 
 /*
- *  Generic EEPROM access.
- * The EEPROM is being accessed by word index.
+ * Generic EEPROM access. The EEPROM is being accessed by word or byte index.
  */
 static inline void *rt2x00_eeprom_addr(struct rt2x00_dev *rt2x00dev,
 				       const unsigned int word)
@@ -1085,6 +1074,12 @@ static inline void rt2x00_eeprom_write(struct rt2x00_dev *rt2x00dev,
 	rt2x00dev->eeprom[word] = cpu_to_le16(data);
 }
 
+static inline u8 rt2x00_eeprom_byte(struct rt2x00_dev *rt2x00dev,
+				    const unsigned int byte)
+{
+	return *(((u8 *)rt2x00dev->eeprom) + byte);
+}
+
 /*
  * Chipset handlers
  */
@@ -1095,9 +1090,27 @@ static inline void rt2x00_set_chip(struct rt2x00_dev *rt2x00dev,
 	rt2x00dev->chip.rf = rf;
 	rt2x00dev->chip.rev = rev;
 
-	INFO(rt2x00dev,
-	     "Chipset detected - rt: %04x, rf: %04x, rev: %04x.\n",
-	     rt2x00dev->chip.rt, rt2x00dev->chip.rf, rt2x00dev->chip.rev);
+	rt2x00_info(rt2x00dev, "Chipset detected - rt: %04x, rf: %04x, rev: %04x\n",
+		    rt2x00dev->chip.rt, rt2x00dev->chip.rf,
+		    rt2x00dev->chip.rev);
+}
+
+static inline void rt2x00_set_rt(struct rt2x00_dev *rt2x00dev,
+				 const u16 rt, const u16 rev)
+{
+	rt2x00dev->chip.rt = rt;
+	rt2x00dev->chip.rev = rev;
+
+	rt2x00_info(rt2x00dev, "RT chipset %04x, rev %04x detected\n",
+		    rt2x00dev->chip.rt, rt2x00dev->chip.rev);
+}
+
+static inline void rt2x00_set_rf(struct rt2x00_dev *rt2x00dev, const u16 rf)
+{
+	rt2x00dev->chip.rf = rf;
+
+	rt2x00_info(rt2x00dev, "RF chipset %04x detected\n",
+		    rt2x00dev->chip.rf);
 }
 
 static inline bool rt2x00_rt(struct rt2x00_dev *rt2x00dev, const u16 rt)
@@ -1360,7 +1373,7 @@ int rt2x00mac_conf_tx(struct ieee80211_hw *hw,
 		      struct ieee80211_vif *vif, u16 queue,
 		      const struct ieee80211_tx_queue_params *params);
 void rt2x00mac_rfkill_poll(struct ieee80211_hw *hw);
-void rt2x00mac_flush(struct ieee80211_hw *hw, bool drop);
+void rt2x00mac_flush(struct ieee80211_hw *hw, u32 queues, bool drop);
 int rt2x00mac_set_antenna(struct ieee80211_hw *hw, u32 tx_ant, u32 rx_ant);
 int rt2x00mac_get_antenna(struct ieee80211_hw *hw, u32 *tx_ant, u32 *rx_ant);
 void rt2x00mac_get_ringparam(struct ieee80211_hw *hw,
diff --git a/drivers/net/wireless/rt2x00/rt2x00config.c b/drivers/net/wireless/rt2x00/rt2x00config.c
index 49a63e973934..8cb43f8f3efc 100644
--- a/drivers/net/wireless/rt2x00/rt2x00config.c
+++ b/drivers/net/wireless/rt2x00/rt2x00config.c
@@ -184,7 +184,7 @@ static u16 rt2x00ht_center_channel(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Initialize center channel to current channel.
 	 */
-	center_channel = spec->channels[conf->channel->hw_value].channel;
+	center_channel = spec->channels[conf->chandef.chan->hw_value].channel;
 
 	/*
 	 * Adjust center channel to HT40+ and HT40- operation.
@@ -199,7 +199,7 @@ static u16 rt2x00ht_center_channel(struct rt2x00_dev *rt2x00dev,
 			return i;
 
 	WARN_ON(1);
-	return conf->channel->hw_value;
+	return conf->chandef.chan->hw_value;
 }
 
 void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
@@ -227,7 +227,7 @@ void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
 			hw_value = rt2x00ht_center_channel(rt2x00dev, conf);
 		} else {
 			clear_bit(CONFIG_CHANNEL_HT40, &rt2x00dev->flags);
-			hw_value = conf->channel->hw_value;
+			hw_value = conf->chandef.chan->hw_value;
 		}
 
 		memcpy(&libconf.rf,
@@ -279,8 +279,8 @@ void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
 	else
 		clear_bit(CONFIG_POWERSAVING, &rt2x00dev->flags);
 
-	rt2x00dev->curr_band = conf->channel->band;
-	rt2x00dev->curr_freq = conf->channel->center_freq;
+	rt2x00dev->curr_band = conf->chandef.chan->band;
+	rt2x00dev->curr_freq = conf->chandef.chan->center_freq;
 	rt2x00dev->tx_power = conf->power_level;
 	rt2x00dev->short_retry = conf->short_frame_max_tx_count;
 	rt2x00dev->long_retry = conf->long_frame_max_tx_count;
diff --git a/drivers/net/wireless/rt2x00/rt2x00debug.c b/drivers/net/wireless/rt2x00/rt2x00debug.c
index 3bb8cafbac59..fe7a7f63a9ed 100644
--- a/drivers/net/wireless/rt2x00/rt2x00debug.c
+++ b/drivers/net/wireless/rt2x00/rt2x00debug.c
@@ -174,7 +174,7 @@ void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
 	do_gettimeofday(&timestamp);
 
 	if (skb_queue_len(&intf->frame_dump_skbqueue) > 20) {
-		DEBUG(rt2x00dev, "txrx dump queue length exceeded.\n");
+		rt2x00_dbg(rt2x00dev, "txrx dump queue length exceeded\n");
 		return;
 	}
 
@@ -185,7 +185,7 @@ void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
 	skbcopy = alloc_skb(sizeof(*dump_hdr) + skbdesc->desc_len + data_len,
 			    GFP_ATOMIC);
 	if (!skbcopy) {
-		DEBUG(rt2x00dev, "Failed to copy skb for dump.\n");
+		rt2x00_dbg(rt2x00dev, "Failed to copy skb for dump\n");
 		return;
 	}
 
@@ -657,7 +657,7 @@ void rt2x00debug_register(struct rt2x00_dev *rt2x00dev)
 
 	intf = kzalloc(sizeof(struct rt2x00debug_intf), GFP_KERNEL);
 	if (!intf) {
-		ERROR(rt2x00dev, "Failed to allocate debug handler.\n");
+		rt2x00_err(rt2x00dev, "Failed to allocate debug handler\n");
 		return;
 	}
 
@@ -760,7 +760,7 @@ void rt2x00debug_register(struct rt2x00_dev *rt2x00dev)
 
 exit:
 	rt2x00debug_deregister(rt2x00dev);
-	ERROR(rt2x00dev, "Failed to register debug handler.\n");
+	rt2x00_err(rt2x00dev, "Failed to register debug handler\n");
 }
 
 void rt2x00debug_deregister(struct rt2x00_dev *rt2x00dev)
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index 189744db65e0..90dc14336980 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -171,7 +171,7 @@ static void rt2x00lib_autowakeup(struct work_struct *work)
 		return;
 
 	if (rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_AWAKE))
-		ERROR(rt2x00dev, "Device failed to wakeup.\n");
+		rt2x00_err(rt2x00dev, "Device failed to wakeup\n");
 	clear_bit(CONFIG_POWERSAVING, &rt2x00dev->flags);
 }
 
@@ -673,9 +673,8 @@ static int rt2x00lib_rxdone_read_signal(struct rt2x00_dev *rt2x00dev,
 		break;
 	}
 
-	WARNING(rt2x00dev, "Frame received with unrecognized signal, "
-		"mode=0x%.4x, signal=0x%.4x, type=%d.\n",
-		rxdesc->rate_mode, signal, type);
+	rt2x00_warn(rt2x00dev, "Frame received with unrecognized signal, mode=0x%.4x, signal=0x%.4x, type=%d\n",
+		    rxdesc->rate_mode, signal, type);
 	return 0;
 }
 
@@ -720,8 +719,8 @@ void rt2x00lib_rxdone(struct queue_entry *entry, gfp_t gfp)
 	 */
 	if (unlikely(rxdesc.size == 0 ||
 		     rxdesc.size > entry->queue->data_size)) {
-		ERROR(rt2x00dev, "Wrong frame size %d max %d.\n",
-			rxdesc.size, entry->queue->data_size);
+		rt2x00_err(rt2x00dev, "Wrong frame size %d max %d\n",
+			   rxdesc.size, entry->queue->data_size);
 		dev_kfree_skb(entry->skb);
 		goto renew_skb;
 	}
@@ -1006,7 +1005,7 @@ static int rt2x00lib_probe_hw_modes(struct rt2x00_dev *rt2x00dev,
 
  exit_free_channels:
 	kfree(channels);
-	ERROR(rt2x00dev, "Allocation ieee80211 modes failed.\n");
+	rt2x00_err(rt2x00dev, "Allocation ieee80211 modes failed\n");
 	return -ENOMEM;
 }
 
@@ -1337,7 +1336,7 @@ int rt2x00lib_probe_dev(struct rt2x00_dev *rt2x00dev)
 	 */
 	retval = rt2x00dev->ops->lib->probe_hw(rt2x00dev);
 	if (retval) {
-		ERROR(rt2x00dev, "Failed to allocate device.\n");
+		rt2x00_err(rt2x00dev, "Failed to allocate device\n");
 		goto exit;
 	}
 
@@ -1353,7 +1352,7 @@ int rt2x00lib_probe_dev(struct rt2x00_dev *rt2x00dev)
 	 */
 	retval = rt2x00lib_probe_hw(rt2x00dev);
 	if (retval) {
-		ERROR(rt2x00dev, "Failed to initialize hw.\n");
+		rt2x00_err(rt2x00dev, "Failed to initialize hw\n");
 		goto exit;
 	}
 
@@ -1451,7 +1450,7 @@ EXPORT_SYMBOL_GPL(rt2x00lib_remove_dev);
 #ifdef CONFIG_PM
 int rt2x00lib_suspend(struct rt2x00_dev *rt2x00dev, pm_message_t state)
 {
-	DEBUG(rt2x00dev, "Going to sleep.\n");
+	rt2x00_dbg(rt2x00dev, "Going to sleep\n");
 
 	/*
 	 * Prevent mac80211 from accessing driver while suspended.
@@ -1482,8 +1481,7 @@ int rt2x00lib_suspend(struct rt2x00_dev *rt2x00dev, pm_message_t state)
 	 * device is as good as disabled.
 	 */
 	if (rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_SLEEP))
-		WARNING(rt2x00dev, "Device failed to enter sleep state, "
-			"continue suspending.\n");
+		rt2x00_warn(rt2x00dev, "Device failed to enter sleep state, continue suspending\n");
 
 	return 0;
 }
@@ -1491,7 +1489,7 @@ EXPORT_SYMBOL_GPL(rt2x00lib_suspend);
 
 int rt2x00lib_resume(struct rt2x00_dev *rt2x00dev)
 {
-	DEBUG(rt2x00dev, "Waking up.\n");
+	rt2x00_dbg(rt2x00dev, "Waking up\n");
 
 	/*
 	 * Restore/enable extra components.
diff --git a/drivers/net/wireless/rt2x00/rt2x00firmware.c b/drivers/net/wireless/rt2x00/rt2x00firmware.c
index f316aad30612..1b4254b4272d 100644
--- a/drivers/net/wireless/rt2x00/rt2x00firmware.c
+++ b/drivers/net/wireless/rt2x00/rt2x00firmware.c
@@ -42,28 +42,28 @@ static int rt2x00lib_request_firmware(struct rt2x00_dev *rt2x00dev)
 	 */
 	fw_name = rt2x00dev->ops->lib->get_firmware_name(rt2x00dev);
 	if (!fw_name) {
-		ERROR(rt2x00dev,
-		      "Invalid firmware filename.\n"
-		      "Please file bug report to %s.\n", DRV_PROJECT);
+		rt2x00_err(rt2x00dev,
+			   "Invalid firmware filename\n"
+			   "Please file bug report to %s\n", DRV_PROJECT);
 		return -EINVAL;
 	}
 
-	INFO(rt2x00dev, "Loading firmware file '%s'.\n", fw_name);
+	rt2x00_info(rt2x00dev, "Loading firmware file '%s'\n", fw_name);
 
 	retval = request_firmware(&fw, fw_name, device);
 	if (retval) {
-		ERROR(rt2x00dev, "Failed to request Firmware.\n");
+		rt2x00_err(rt2x00dev, "Failed to request Firmware\n");
 		return retval;
 	}
 
 	if (!fw || !fw->size || !fw->data) {
-		ERROR(rt2x00dev, "Failed to read Firmware.\n");
+		rt2x00_err(rt2x00dev, "Failed to read Firmware\n");
 		release_firmware(fw);
 		return -ENOENT;
 	}
 
-	INFO(rt2x00dev, "Firmware detected - version: %d.%d.\n",
-	     fw->data[fw->size - 4], fw->data[fw->size - 3]);
+	rt2x00_info(rt2x00dev, "Firmware detected - version: %d.%d\n",
+		    fw->data[fw->size - 4], fw->data[fw->size - 3]);
 	snprintf(rt2x00dev->hw->wiphy->fw_version,
 			sizeof(rt2x00dev->hw->wiphy->fw_version), "%d.%d",
 			fw->data[fw->size - 4], fw->data[fw->size - 3]);
@@ -73,15 +73,14 @@ static int rt2x00lib_request_firmware(struct rt2x00_dev *rt2x00dev)
 	case FW_OK:
 		break;
 	case FW_BAD_CRC:
-		ERROR(rt2x00dev, "Firmware checksum error.\n");
+		rt2x00_err(rt2x00dev, "Firmware checksum error\n");
 		goto exit;
 	case FW_BAD_LENGTH:
-		ERROR(rt2x00dev,
-		      "Invalid firmware file length (len=%zu)\n", fw->size);
+		rt2x00_err(rt2x00dev, "Invalid firmware file length (len=%zu)\n",
+			   fw->size);
 		goto exit;
 	case FW_BAD_VERSION:
-		ERROR(rt2x00dev,
-		      "Current firmware does not support detected chipset.\n");
+		rt2x00_err(rt2x00dev, "Current firmware does not support detected chipset\n");
 		goto exit;
 	}
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00leds.c b/drivers/net/wireless/rt2x00/rt2x00leds.c
index 8679d781a264..997a6c89e66e 100644
--- a/drivers/net/wireless/rt2x00/rt2x00leds.c
+++ b/drivers/net/wireless/rt2x00/rt2x00leds.c
@@ -113,7 +113,7 @@ static int rt2x00leds_register_led(struct rt2x00_dev *rt2x00dev,
 
 	retval = led_classdev_register(device, &led->led_dev);
 	if (retval) {
-		ERROR(rt2x00dev, "Failed to register led handler.\n");
+		rt2x00_err(rt2x00dev, "Failed to register led handler\n");
 		return retval;
 	}
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/rt2x00/rt2x00mac.c
index 20c6eccce5aa..f883802f3505 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mac.c
+++ b/drivers/net/wireless/rt2x00/rt2x00mac.c
@@ -46,7 +46,7 @@ static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
 
 	skb = dev_alloc_skb(data_length + rt2x00dev->hw->extra_tx_headroom);
 	if (unlikely(!skb)) {
-		WARNING(rt2x00dev, "Failed to create RTS/CTS frame.\n");
+		rt2x00_warn(rt2x00dev, "Failed to create RTS/CTS frame\n");
 		return -ENOMEM;
 	}
 
@@ -93,7 +93,7 @@ static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
 	retval = rt2x00queue_write_tx_frame(queue, skb, true);
 	if (retval) {
 		dev_kfree_skb_any(skb);
-		WARNING(rt2x00dev, "Failed to send RTS/CTS frame.\n");
+		rt2x00_warn(rt2x00dev, "Failed to send RTS/CTS frame\n");
 	}
 
 	return retval;
@@ -126,9 +126,9 @@ void rt2x00mac_tx(struct ieee80211_hw *hw,
 
 	queue = rt2x00queue_get_tx_queue(rt2x00dev, qid);
 	if (unlikely(!queue)) {
-		ERROR(rt2x00dev,
-		      "Attempt to send packet over invalid queue %d.\n"
-		      "Please file bug report to %s.\n", qid, DRV_PROJECT);
+		rt2x00_err(rt2x00dev,
+			   "Attempt to send packet over invalid queue %d\n"
+			   "Please file bug report to %s\n", qid, DRV_PROJECT);
 		goto exit_free_skb;
 	}
 
@@ -731,9 +731,10 @@ int rt2x00mac_conf_tx(struct ieee80211_hw *hw,
 	queue->aifs = params->aifs;
 	queue->txop = params->txop;
 
-	DEBUG(rt2x00dev,
-	      "Configured TX queue %d - CWmin: %d, CWmax: %d, Aifs: %d, TXop: %d.\n",
-	      queue_idx, queue->cw_min, queue->cw_max, queue->aifs, queue->txop);
+	rt2x00_dbg(rt2x00dev,
+		   "Configured TX queue %d - CWmin: %d, CWmax: %d, Aifs: %d, TXop: %d\n",
+		   queue_idx, queue->cw_min, queue->cw_max, queue->aifs,
+		   queue->txop);
 
 	return 0;
 }
@@ -748,7 +749,7 @@ void rt2x00mac_rfkill_poll(struct ieee80211_hw *hw)
 }
 EXPORT_SYMBOL_GPL(rt2x00mac_rfkill_poll);
 
-void rt2x00mac_flush(struct ieee80211_hw *hw, bool drop)
+void rt2x00mac_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 {
 	struct rt2x00_dev *rt2x00dev = hw->priv;
 	struct data_queue *queue;
diff --git a/drivers/net/wireless/rt2x00/rt2x00mmio.c b/drivers/net/wireless/rt2x00/rt2x00mmio.c
index d84a680ba0c9..64b06c6abe58 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mmio.c
+++ b/drivers/net/wireless/rt2x00/rt2x00mmio.c
@@ -34,10 +34,10 @@
 /*
  * Register access.
  */
-int rt2x00pci_regbusy_read(struct rt2x00_dev *rt2x00dev,
-			   const unsigned int offset,
-			   const struct rt2x00_field32 field,
-			   u32 *reg)
+int rt2x00mmio_regbusy_read(struct rt2x00_dev *rt2x00dev,
+			    const unsigned int offset,
+			    const struct rt2x00_field32 field,
+			    u32 *reg)
 {
 	unsigned int i;
 
@@ -45,7 +45,7 @@ int rt2x00pci_regbusy_read(struct rt2x00_dev *rt2x00dev,
 		return 0;
 
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2x00pci_register_read(rt2x00dev, offset, reg);
+		rt2x00mmio_register_read(rt2x00dev, offset, reg);
 		if (!rt2x00_get_field32(*reg, field))
 			return 1;
 		udelay(REGISTER_BUSY_DELAY);
@@ -57,13 +57,13 @@ int rt2x00pci_regbusy_read(struct rt2x00_dev *rt2x00dev,
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(rt2x00pci_regbusy_read);
+EXPORT_SYMBOL_GPL(rt2x00mmio_regbusy_read);
 
-bool rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
+bool rt2x00mmio_rxdone(struct rt2x00_dev *rt2x00dev)
 {
 	struct data_queue *queue = rt2x00dev->rx;
 	struct queue_entry *entry;
-	struct queue_entry_priv_pci *entry_priv;
+	struct queue_entry_priv_mmio *entry_priv;
 	struct skb_frame_desc *skbdesc;
 	int max_rx = 16;
 
@@ -96,24 +96,24 @@ bool rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
 
 	return !max_rx;
 }
-EXPORT_SYMBOL_GPL(rt2x00pci_rxdone);
+EXPORT_SYMBOL_GPL(rt2x00mmio_rxdone);
 
-void rt2x00pci_flush_queue(struct data_queue *queue, bool drop)
+void rt2x00mmio_flush_queue(struct data_queue *queue, bool drop)
 {
 	unsigned int i;
 
 	for (i = 0; !rt2x00queue_empty(queue) && i < 10; i++)
 		msleep(10);
 }
-EXPORT_SYMBOL_GPL(rt2x00pci_flush_queue);
+EXPORT_SYMBOL_GPL(rt2x00mmio_flush_queue);
 
 /*
  * Device initialization handlers.
  */
-static int rt2x00pci_alloc_queue_dma(struct rt2x00_dev *rt2x00dev,
-				     struct data_queue *queue)
+static int rt2x00mmio_alloc_queue_dma(struct rt2x00_dev *rt2x00dev,
+				      struct data_queue *queue)
 {
-	struct queue_entry_priv_pci *entry_priv;
+	struct queue_entry_priv_mmio *entry_priv;
 	void *addr;
 	dma_addr_t dma;
 	unsigned int i;
@@ -141,10 +141,10 @@ static int rt2x00pci_alloc_queue_dma(struct rt2x00_dev *rt2x00dev,
 	return 0;
 }
 
-static void rt2x00pci_free_queue_dma(struct rt2x00_dev *rt2x00dev,
-				     struct data_queue *queue)
+static void rt2x00mmio_free_queue_dma(struct rt2x00_dev *rt2x00dev,
+				      struct data_queue *queue)
 {
-	struct queue_entry_priv_pci *entry_priv =
+	struct queue_entry_priv_mmio *entry_priv =
 	    queue->entries[0].priv_data;
 
 	if (entry_priv->desc)
@@ -154,7 +154,7 @@ static void rt2x00pci_free_queue_dma(struct rt2x00_dev *rt2x00dev,
 	entry_priv->desc = NULL;
 }
 
-int rt2x00pci_initialize(struct rt2x00_dev *rt2x00dev)
+int rt2x00mmio_initialize(struct rt2x00_dev *rt2x00dev)
 {
 	struct data_queue *queue;
 	int status;
@@ -163,7 +163,7 @@ int rt2x00pci_initialize(struct rt2x00_dev *rt2x00dev)
 	 * Allocate DMA
 	 */
 	queue_for_each(rt2x00dev, queue) {
-		status = rt2x00pci_alloc_queue_dma(rt2x00dev, queue);
+		status = rt2x00mmio_alloc_queue_dma(rt2x00dev, queue);
 		if (status)
 			goto exit;
 	}
@@ -175,8 +175,8 @@ int rt2x00pci_initialize(struct rt2x00_dev *rt2x00dev)
 			     rt2x00dev->ops->lib->irq_handler,
 			     IRQF_SHARED, rt2x00dev->name, rt2x00dev);
 	if (status) {
-		ERROR(rt2x00dev, "IRQ %d allocation failed (error %d).\n",
-		      rt2x00dev->irq, status);
+		rt2x00_err(rt2x00dev, "IRQ %d allocation failed (error %d)\n",
+			   rt2x00dev->irq, status);
 		goto exit;
 	}
 
@@ -184,13 +184,13 @@ int rt2x00pci_initialize(struct rt2x00_dev *rt2x00dev)
 
 exit:
 	queue_for_each(rt2x00dev, queue)
-		rt2x00pci_free_queue_dma(rt2x00dev, queue);
+		rt2x00mmio_free_queue_dma(rt2x00dev, queue);
 
 	return status;
 }
-EXPORT_SYMBOL_GPL(rt2x00pci_initialize);
+EXPORT_SYMBOL_GPL(rt2x00mmio_initialize);
 
-void rt2x00pci_uninitialize(struct rt2x00_dev *rt2x00dev)
+void rt2x00mmio_uninitialize(struct rt2x00_dev *rt2x00dev)
 {
 	struct data_queue *queue;
 
@@ -203,9 +203,9 @@ void rt2x00pci_uninitialize(struct rt2x00_dev *rt2x00dev)
 	 * Free DMA
 	 */
 	queue_for_each(rt2x00dev, queue)
-		rt2x00pci_free_queue_dma(rt2x00dev, queue);
+		rt2x00mmio_free_queue_dma(rt2x00dev, queue);
 }
-EXPORT_SYMBOL_GPL(rt2x00pci_uninitialize);
+EXPORT_SYMBOL_GPL(rt2x00mmio_uninitialize);
 
 /*
  * rt2x00mmio module information.
diff --git a/drivers/net/wireless/rt2x00/rt2x00mmio.h b/drivers/net/wireless/rt2x00/rt2x00mmio.h
index 4ecaf60175bf..cda3dbcf7ead 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mmio.h
+++ b/drivers/net/wireless/rt2x00/rt2x00mmio.h
@@ -31,37 +31,37 @@
 /*
  * Register access.
  */
-static inline void rt2x00pci_register_read(struct rt2x00_dev *rt2x00dev,
-					   const unsigned int offset,
-					   u32 *value)
+static inline void rt2x00mmio_register_read(struct rt2x00_dev *rt2x00dev,
+					    const unsigned int offset,
+					    u32 *value)
 {
 	*value = readl(rt2x00dev->csr.base + offset);
 }
 
-static inline void rt2x00pci_register_multiread(struct rt2x00_dev *rt2x00dev,
-						const unsigned int offset,
-						void *value, const u32 length)
+static inline void rt2x00mmio_register_multiread(struct rt2x00_dev *rt2x00dev,
+						 const unsigned int offset,
+						 void *value, const u32 length)
 {
 	memcpy_fromio(value, rt2x00dev->csr.base + offset, length);
 }
 
-static inline void rt2x00pci_register_write(struct rt2x00_dev *rt2x00dev,
-					    const unsigned int offset,
-					    u32 value)
+static inline void rt2x00mmio_register_write(struct rt2x00_dev *rt2x00dev,
+					     const unsigned int offset,
+					     u32 value)
 {
 	writel(value, rt2x00dev->csr.base + offset);
 }
 
-static inline void rt2x00pci_register_multiwrite(struct rt2x00_dev *rt2x00dev,
-						 const unsigned int offset,
-						 const void *value,
-						 const u32 length)
+static inline void rt2x00mmio_register_multiwrite(struct rt2x00_dev *rt2x00dev,
+						  const unsigned int offset,
+						  const void *value,
+						  const u32 length)
 {
 	__iowrite32_copy(rt2x00dev->csr.base + offset, value, length >> 2);
 }
 
 /**
- * rt2x00pci_regbusy_read - Read from register with busy check
+ * rt2x00mmio_regbusy_read - Read from register with busy check
  * @rt2x00dev: Device pointer, see &struct rt2x00_dev.
  * @offset: Register offset
  * @field: Field to check if register is busy
@@ -73,47 +73,47 @@ static inline void rt2x00pci_register_multiwrite(struct rt2x00_dev *rt2x00dev,
  * is not read after a certain timeout, this function will return
  * FALSE.
  */
-int rt2x00pci_regbusy_read(struct rt2x00_dev *rt2x00dev,
-			   const unsigned int offset,
-			   const struct rt2x00_field32 field,
-			   u32 *reg);
+int rt2x00mmio_regbusy_read(struct rt2x00_dev *rt2x00dev,
+			    const unsigned int offset,
+			    const struct rt2x00_field32 field,
+			    u32 *reg);
 
 /**
- * struct queue_entry_priv_pci: Per entry PCI specific information
+ * struct queue_entry_priv_mmio: Per entry PCI specific information
  *
  * @desc: Pointer to device descriptor
  * @desc_dma: DMA pointer to &desc.
  * @data: Pointer to device's entry memory.
  * @data_dma: DMA pointer to &data.
  */
-struct queue_entry_priv_pci {
+struct queue_entry_priv_mmio {
 	__le32 *desc;
 	dma_addr_t desc_dma;
 };
 
 /**
- * rt2x00pci_rxdone - Handle RX done events
+ * rt2x00mmio_rxdone - Handle RX done events
  * @rt2x00dev: Device pointer, see &struct rt2x00_dev.
  *
  * Returns true if there are still rx frames pending and false if all
  * pending rx frames were processed.
  */
-bool rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev);
+bool rt2x00mmio_rxdone(struct rt2x00_dev *rt2x00dev);
 
 /**
- * rt2x00pci_flush_queue - Flush data queue
+ * rt2x00mmio_flush_queue - Flush data queue
  * @queue: Data queue to stop
  * @drop: True to drop all pending frames.
  *
  * This will wait for a maximum of 100ms, waiting for the queues
  * to become empty.
  */
-void rt2x00pci_flush_queue(struct data_queue *queue, bool drop);
+void rt2x00mmio_flush_queue(struct data_queue *queue, bool drop);
 
 /*
  * Device initialization handlers.
  */
-int rt2x00pci_initialize(struct rt2x00_dev *rt2x00dev);
-void rt2x00pci_uninitialize(struct rt2x00_dev *rt2x00dev);
+int rt2x00mmio_initialize(struct rt2x00_dev *rt2x00dev);
+void rt2x00mmio_uninitialize(struct rt2x00_dev *rt2x00dev);
 
 #endif /* RT2X00MMIO_H */
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.c b/drivers/net/wireless/rt2x00/rt2x00pci.c
index e87865e33113..dc49e525ae5e 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.c
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.c
@@ -68,7 +68,7 @@ static int rt2x00pci_alloc_reg(struct rt2x00_dev *rt2x00dev)
 	return 0;
 
 exit:
-	ERROR_PROBE("Failed to allocate registers.\n");
+	rt2x00_probe_err("Failed to allocate registers\n");
 
 	rt2x00pci_free_reg(rt2x00dev);
 
@@ -84,30 +84,30 @@ int rt2x00pci_probe(struct pci_dev *pci_dev, const struct rt2x00_ops *ops)
 
 	retval = pci_enable_device(pci_dev);
 	if (retval) {
-		ERROR_PROBE("Enable device failed.\n");
+		rt2x00_probe_err("Enable device failed\n");
 		return retval;
 	}
 
 	retval = pci_request_regions(pci_dev, pci_name(pci_dev));
 	if (retval) {
-		ERROR_PROBE("PCI request regions failed.\n");
+		rt2x00_probe_err("PCI request regions failed\n");
 		goto exit_disable_device;
 	}
 
 	pci_set_master(pci_dev);
 
 	if (pci_set_mwi(pci_dev))
-		ERROR_PROBE("MWI not available.\n");
+		rt2x00_probe_err("MWI not available\n");
 
 	if (dma_set_mask(&pci_dev->dev, DMA_BIT_MASK(32))) {
-		ERROR_PROBE("PCI DMA not supported.\n");
+		rt2x00_probe_err("PCI DMA not supported\n");
 		retval = -EIO;
 		goto exit_release_regions;
 	}
 
 	hw = ieee80211_alloc_hw(sizeof(struct rt2x00_dev), ops->hw);
 	if (!hw) {
-		ERROR_PROBE("Failed to allocate hardware.\n");
+		rt2x00_probe_err("Failed to allocate hardware\n");
 		retval = -ENOMEM;
 		goto exit_release_regions;
 	}
@@ -207,7 +207,7 @@ int rt2x00pci_resume(struct pci_dev *pci_dev)
 
 	if (pci_set_power_state(pci_dev, PCI_D0) ||
 	    pci_enable_device(pci_dev)) {
-		ERROR(rt2x00dev, "Failed to resume device.\n");
+		rt2x00_err(rt2x00dev, "Failed to resume device\n");
 		return -EIO;
 	}
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index 4d91795dc6a2..2c12311467a9 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -35,7 +35,8 @@
 
 struct sk_buff *rt2x00queue_alloc_rxskb(struct queue_entry *entry, gfp_t gfp)
 {
-	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+	struct data_queue *queue = entry->queue;
+	struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
 	struct sk_buff *skb;
 	struct skb_frame_desc *skbdesc;
 	unsigned int frame_size;
@@ -46,7 +47,7 @@ struct sk_buff *rt2x00queue_alloc_rxskb(struct queue_entry *entry, gfp_t gfp)
 	 * The frame size includes descriptor size, because the
 	 * hardware directly receive the frame into the skbuffer.
 	 */
-	frame_size = entry->queue->data_size + entry->queue->desc_size;
+	frame_size = queue->data_size + queue->desc_size + queue->winfo_size;
 
 	/*
 	 * The payload should be aligned to a 4-byte boundary,
@@ -531,10 +532,10 @@ static int rt2x00queue_write_tx_data(struct queue_entry *entry,
 	 */
 	if (unlikely(rt2x00dev->ops->lib->get_entry_state &&
 		     rt2x00dev->ops->lib->get_entry_state(entry))) {
-		ERROR(rt2x00dev,
-		      "Corrupt queue %d, accessing entry which is not ours.\n"
-		      "Please file bug report to %s.\n",
-		      entry->queue->qid, DRV_PROJECT);
+		rt2x00_err(rt2x00dev,
+			   "Corrupt queue %d, accessing entry which is not ours\n"
+			   "Please file bug report to %s\n",
+			   entry->queue->qid, DRV_PROJECT);
 		return -EINVAL;
 	}
 
@@ -698,8 +699,8 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb,
 	spin_lock(&queue->tx_lock);
 
 	if (unlikely(rt2x00queue_full(queue))) {
-		ERROR(queue->rt2x00dev,
-		      "Dropping frame due to full tx queue %d.\n", queue->qid);
+		rt2x00_err(queue->rt2x00dev, "Dropping frame due to full tx queue %d\n",
+			   queue->qid);
 		ret = -ENOBUFS;
 		goto out;
 	}
@@ -708,10 +709,10 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb,
 
 	if (unlikely(test_and_set_bit(ENTRY_OWNER_DEVICE_DATA,
 				      &entry->flags))) {
-		ERROR(queue->rt2x00dev,
-		      "Arrived at non-free entry in the non-full queue %d.\n"
-		      "Please file bug report to %s.\n",
-		      queue->qid, DRV_PROJECT);
+		rt2x00_err(queue->rt2x00dev,
+			   "Arrived at non-free entry in the non-full queue %d\n"
+			   "Please file bug report to %s\n",
+			   queue->qid, DRV_PROJECT);
 		ret = -EINVAL;
 		goto out;
 	}
@@ -832,7 +833,9 @@ int rt2x00queue_update_beacon(struct rt2x00_dev *rt2x00dev,
 bool rt2x00queue_for_each_entry(struct data_queue *queue,
 				enum queue_index start,
 				enum queue_index end,
-				bool (*fn)(struct queue_entry *entry))
+				void *data,
+				bool (*fn)(struct queue_entry *entry,
+					   void *data))
 {
 	unsigned long irqflags;
 	unsigned int index_start;
@@ -840,9 +843,9 @@ bool rt2x00queue_for_each_entry(struct data_queue *queue,
 	unsigned int i;
 
 	if (unlikely(start >= Q_INDEX_MAX || end >= Q_INDEX_MAX)) {
-		ERROR(queue->rt2x00dev,
-		      "Entry requested from invalid index range (%d - %d)\n",
-		      start, end);
+		rt2x00_err(queue->rt2x00dev,
+			   "Entry requested from invalid index range (%d - %d)\n",
+			   start, end);
 		return true;
 	}
 
@@ -863,17 +866,17 @@ bool rt2x00queue_for_each_entry(struct data_queue *queue,
 	 */
 	if (index_start < index_end) {
 		for (i = index_start; i < index_end; i++) {
-			if (fn(&queue->entries[i]))
+			if (fn(&queue->entries[i], data))
 				return true;
 		}
 	} else {
 		for (i = index_start; i < queue->limit; i++) {
-			if (fn(&queue->entries[i]))
+			if (fn(&queue->entries[i], data))
 				return true;
 		}
 
 		for (i = 0; i < index_end; i++) {
-			if (fn(&queue->entries[i]))
+			if (fn(&queue->entries[i], data))
 				return true;
 		}
 	}
@@ -889,8 +892,8 @@ struct queue_entry *rt2x00queue_get_entry(struct data_queue *queue,
 	unsigned long irqflags;
 
 	if (unlikely(index >= Q_INDEX_MAX)) {
-		ERROR(queue->rt2x00dev,
-		      "Entry requested from invalid index type (%d)\n", index);
+		rt2x00_err(queue->rt2x00dev, "Entry requested from invalid index type (%d)\n",
+			   index);
 		return NULL;
 	}
 
@@ -910,8 +913,8 @@ void rt2x00queue_index_inc(struct queue_entry *entry, enum queue_index index)
 	unsigned long irqflags;
 
 	if (unlikely(index >= Q_INDEX_MAX)) {
-		ERROR(queue->rt2x00dev,
-		      "Index change on invalid index type (%d)\n", index);
+		rt2x00_err(queue->rt2x00dev,
+			   "Index change on invalid index type (%d)\n", index);
 		return;
 	}
 
@@ -1071,7 +1074,8 @@ void rt2x00queue_flush_queue(struct data_queue *queue, bool drop)
 	 * The queue flush has failed...
 	 */
 	if (unlikely(!rt2x00queue_empty(queue)))
-		WARNING(queue->rt2x00dev, "Queue %d failed to flush\n", queue->qid);
+		rt2x00_warn(queue->rt2x00dev, "Queue %d failed to flush\n",
+			    queue->qid);
 
 	/*
 	 * Restore the queue to the previous status
@@ -1170,6 +1174,7 @@ static int rt2x00queue_alloc_entries(struct data_queue *queue,
 	queue->threshold = DIV_ROUND_UP(qdesc->entry_num, 10);
 	queue->data_size = qdesc->data_size;
 	queue->desc_size = qdesc->desc_size;
+	queue->winfo_size = qdesc->winfo_size;
 
 	/*
 	 * Allocate all queue entries.
@@ -1260,7 +1265,7 @@ int rt2x00queue_initialize(struct rt2x00_dev *rt2x00dev)
 	return 0;
 
 exit:
-	ERROR(rt2x00dev, "Queue entries allocation failed.\n");
+	rt2x00_err(rt2x00dev, "Queue entries allocation failed\n");
 
 	rt2x00queue_uninitialize(rt2x00dev);
 
@@ -1312,7 +1317,7 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
 
 	queue = kcalloc(rt2x00dev->data_queues, sizeof(*queue), GFP_KERNEL);
 	if (!queue) {
-		ERROR(rt2x00dev, "Queue allocation failed.\n");
+		rt2x00_err(rt2x00dev, "Queue allocation failed\n");
 		return -ENOMEM;
 	}
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.h b/drivers/net/wireless/rt2x00/rt2x00queue.h
index 9b8c10a86dee..4a7b34e9261b 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.h
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.h
@@ -359,6 +359,7 @@ enum queue_entry_flags {
 	ENTRY_DATA_PENDING,
 	ENTRY_DATA_IO_FAILED,
 	ENTRY_DATA_STATUS_PENDING,
+	ENTRY_DATA_STATUS_SET,
 };
 
 /**
@@ -372,6 +373,7 @@ enum queue_entry_flags {
  * @entry_idx: The entry index number.
  * @priv_data: Private data belonging to this queue entry. The pointer
  *	points to data specific to a particular driver and queue type.
+ * @status: Device specific status
  */
 struct queue_entry {
 	unsigned long flags;
@@ -383,6 +385,8 @@ struct queue_entry {
 
 	unsigned int entry_idx;
 
+	u32 status;
+
 	void *priv_data;
 };
 
@@ -475,7 +479,8 @@ struct data_queue {
 	unsigned short cw_max;
 
 	unsigned short data_size;
-	unsigned short desc_size;
+	unsigned char  desc_size;
+	unsigned char  winfo_size;
 
 	unsigned short usb_endpoint;
 	unsigned short usb_maxpacket;
@@ -495,7 +500,8 @@ struct data_queue {
 struct data_queue_desc {
 	unsigned short entry_num;
 	unsigned short data_size;
-	unsigned short desc_size;
+	unsigned char  desc_size;
+	unsigned char  winfo_size;
 	unsigned short priv_size;
 };
 
@@ -584,6 +590,7 @@ struct data_queue_desc {
  * @queue: Pointer to @data_queue
  * @start: &enum queue_index Pointer to start index
  * @end: &enum queue_index Pointer to end index
+ * @data: Data to pass to the callback function
  * @fn: The function to call for each &struct queue_entry
  *
  * This will walk through all entries in the queue, in chronological
@@ -596,7 +603,9 @@ struct data_queue_desc {
 bool rt2x00queue_for_each_entry(struct data_queue *queue,
 				enum queue_index start,
 				enum queue_index end,
-				bool (*fn)(struct queue_entry *entry));
+				void *data,
+				bool (*fn)(struct queue_entry *entry,
+					   void *data));
 
 /**
  * rt2x00queue_empty - Check if the queue is empty.
diff --git a/drivers/net/wireless/rt2x00/rt2x00soc.c b/drivers/net/wireless/rt2x00/rt2x00soc.c
index 2aa5c38022f3..9271a5fce0a8 100644
--- a/drivers/net/wireless/rt2x00/rt2x00soc.c
+++ b/drivers/net/wireless/rt2x00/rt2x00soc.c
@@ -68,7 +68,7 @@ static int rt2x00soc_alloc_reg(struct rt2x00_dev *rt2x00dev)
 	return 0;
 
 exit:
-	ERROR_PROBE("Failed to allocate registers.\n");
+	rt2x00_probe_err("Failed to allocate registers\n");
 	rt2x00soc_free_reg(rt2x00dev);
 
 	return -ENOMEM;
@@ -82,7 +82,7 @@ int rt2x00soc_probe(struct platform_device *pdev, const struct rt2x00_ops *ops)
 
 	hw = ieee80211_alloc_hw(sizeof(struct rt2x00_dev), ops->hw);
 	if (!hw) {
-		ERROR_PROBE("Failed to allocate hardware.\n");
+		rt2x00_probe_err("Failed to allocate hardware\n");
 		return -ENOMEM;
 	}
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.c b/drivers/net/wireless/rt2x00/rt2x00usb.c
index 40ea80725a96..88289873c0cf 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.c
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.c
@@ -70,9 +70,9 @@ int rt2x00usb_vendor_request(struct rt2x00_dev *rt2x00dev,
 		}
 	}
 
-	ERROR(rt2x00dev,
-	      "Vendor Request 0x%02x failed for offset 0x%04x with error %d.\n",
-	      request, offset, status);
+	rt2x00_err(rt2x00dev,
+		   "Vendor Request 0x%02x failed for offset 0x%04x with error %d\n",
+		   request, offset, status);
 
 	return status;
 }
@@ -91,7 +91,7 @@ int rt2x00usb_vendor_req_buff_lock(struct rt2x00_dev *rt2x00dev,
 	 * Check for Cache availability.
 	 */
 	if (unlikely(!rt2x00dev->csr.cache || buffer_length > CSR_CACHE_SIZE)) {
-		ERROR(rt2x00dev, "CSR cache not available.\n");
+		rt2x00_err(rt2x00dev, "CSR cache not available\n");
 		return -ENOMEM;
 	}
 
@@ -157,8 +157,8 @@ int rt2x00usb_regbusy_read(struct rt2x00_dev *rt2x00dev,
 		udelay(REGISTER_BUSY_DELAY);
 	}
 
-	ERROR(rt2x00dev, "Indirect register access failed: "
-	      "offset=0x%.08x, value=0x%.08x\n", offset, *reg);
+	rt2x00_err(rt2x00dev, "Indirect register access failed: offset=0x%.08x, value=0x%.08x\n",
+		   offset, *reg);
 	*reg = ~0;
 
 	return 0;
@@ -285,7 +285,7 @@ static void rt2x00usb_interrupt_txdone(struct urb *urb)
 		queue_work(rt2x00dev->workqueue, &rt2x00dev->txdone_work);
 }
 
-static bool rt2x00usb_kick_tx_entry(struct queue_entry *entry)
+static bool rt2x00usb_kick_tx_entry(struct queue_entry *entry, void *data)
 {
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
 	struct usb_device *usb_dev = to_usb_device_intf(rt2x00dev->dev);
@@ -307,7 +307,7 @@ static bool rt2x00usb_kick_tx_entry(struct queue_entry *entry)
 	status = skb_padto(entry->skb, length);
 	if (unlikely(status)) {
 		/* TODO: report something more appropriate than IO_FAILED. */
-		WARNING(rt2x00dev, "TX SKB padding error, out of memory\n");
+		rt2x00_warn(rt2x00dev, "TX SKB padding error, out of memory\n");
 		set_bit(ENTRY_DATA_IO_FAILED, &entry->flags);
 		rt2x00lib_dmadone(entry);
 
@@ -390,7 +390,7 @@ static void rt2x00usb_interrupt_rxdone(struct urb *urb)
 	queue_work(rt2x00dev->workqueue, &rt2x00dev->rxdone_work);
 }
 
-static bool rt2x00usb_kick_rx_entry(struct queue_entry *entry)
+static bool rt2x00usb_kick_rx_entry(struct queue_entry *entry, void *data)
 {
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
 	struct usb_device *usb_dev = to_usb_device_intf(rt2x00dev->dev);
@@ -427,12 +427,18 @@ void rt2x00usb_kick_queue(struct data_queue *queue)
 	case QID_AC_BE:
 	case QID_AC_BK:
 		if (!rt2x00queue_empty(queue))
-			rt2x00queue_for_each_entry(queue, Q_INDEX_DONE, Q_INDEX,
+			rt2x00queue_for_each_entry(queue,
+						   Q_INDEX_DONE,
+						   Q_INDEX,
+						   NULL,
 						   rt2x00usb_kick_tx_entry);
 		break;
 	case QID_RX:
 		if (!rt2x00queue_full(queue))
-			rt2x00queue_for_each_entry(queue, Q_INDEX, Q_INDEX_DONE,
+			rt2x00queue_for_each_entry(queue,
+						   Q_INDEX,
+						   Q_INDEX_DONE,
+						   NULL,
 						   rt2x00usb_kick_rx_entry);
 		break;
 	default:
@@ -441,7 +447,7 @@ void rt2x00usb_kick_queue(struct data_queue *queue)
 }
 EXPORT_SYMBOL_GPL(rt2x00usb_kick_queue);
 
-static bool rt2x00usb_flush_entry(struct queue_entry *entry)
+static bool rt2x00usb_flush_entry(struct queue_entry *entry, void *data)
 {
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
 	struct queue_entry_priv_usb *entry_priv = entry->priv_data;
@@ -468,7 +474,7 @@ void rt2x00usb_flush_queue(struct data_queue *queue, bool drop)
 	unsigned int i;
 
 	if (drop)
-		rt2x00queue_for_each_entry(queue, Q_INDEX_DONE, Q_INDEX,
+		rt2x00queue_for_each_entry(queue, Q_INDEX_DONE, Q_INDEX, NULL,
 					   rt2x00usb_flush_entry);
 
 	/*
@@ -514,8 +520,8 @@ EXPORT_SYMBOL_GPL(rt2x00usb_flush_queue);
 
 static void rt2x00usb_watchdog_tx_dma(struct data_queue *queue)
 {
-	WARNING(queue->rt2x00dev, "TX queue %d DMA timed out,"
-		" invoke forced forced reset\n", queue->qid);
+	rt2x00_warn(queue->rt2x00dev, "TX queue %d DMA timed out, invoke forced forced reset\n",
+		    queue->qid);
 
 	rt2x00queue_flush_queue(queue, true);
 }
@@ -559,7 +565,7 @@ void rt2x00usb_clear_entry(struct queue_entry *entry)
 	entry->flags = 0;
 
 	if (entry->queue->qid == QID_RX)
-		rt2x00usb_kick_rx_entry(entry);
+		rt2x00usb_kick_rx_entry(entry, NULL);
 }
 EXPORT_SYMBOL_GPL(rt2x00usb_clear_entry);
 
@@ -616,7 +622,7 @@ static int rt2x00usb_find_endpoints(struct rt2x00_dev *rt2x00dev)
 	 * At least 1 endpoint for RX and 1 endpoint for TX must be available.
 	 */
 	if (!rt2x00dev->rx->usb_endpoint || !rt2x00dev->tx->usb_endpoint) {
-		ERROR(rt2x00dev, "Bulk-in/Bulk-out endpoints not found\n");
+		rt2x00_err(rt2x00dev, "Bulk-in/Bulk-out endpoints not found\n");
 		return -EPIPE;
 	}
 
@@ -769,7 +775,7 @@ static int rt2x00usb_alloc_reg(struct rt2x00_dev *rt2x00dev)
 	return 0;
 
 exit:
-	ERROR_PROBE("Failed to allocate registers.\n");
+	rt2x00_probe_err("Failed to allocate registers\n");
 
 	rt2x00usb_free_reg(rt2x00dev);
 
@@ -789,7 +795,7 @@ int rt2x00usb_probe(struct usb_interface *usb_intf,
 
 	hw = ieee80211_alloc_hw(sizeof(struct rt2x00_dev), ops->hw);
 	if (!hw) {
-		ERROR_PROBE("Failed to allocate hardware.\n");
+		rt2x00_probe_err("Failed to allocate hardware\n");
 		retval = -ENOMEM;
 		goto exit_put_device;
 	}
diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
index 9e3c8ff53e3f..0dc8180e251b 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -58,12 +58,12 @@ MODULE_PARM_DESC(nohwcrypt, "Disable hardware encryption.");
  * and we will print an error.
  */
 #define WAIT_FOR_BBP(__dev, __reg) \
-	rt2x00pci_regbusy_read((__dev), PHY_CSR3, PHY_CSR3_BUSY, (__reg))
+	rt2x00mmio_regbusy_read((__dev), PHY_CSR3, PHY_CSR3_BUSY, (__reg))
 #define WAIT_FOR_RF(__dev, __reg) \
-	rt2x00pci_regbusy_read((__dev), PHY_CSR4, PHY_CSR4_BUSY, (__reg))
+	rt2x00mmio_regbusy_read((__dev), PHY_CSR4, PHY_CSR4_BUSY, (__reg))
 #define WAIT_FOR_MCU(__dev, __reg) \
-	rt2x00pci_regbusy_read((__dev), H2M_MAILBOX_CSR, \
-			       H2M_MAILBOX_CSR_OWNER, (__reg))
+	rt2x00mmio_regbusy_read((__dev), H2M_MAILBOX_CSR, \
+				H2M_MAILBOX_CSR_OWNER, (__reg))
 
 static void rt61pci_bbp_write(struct rt2x00_dev *rt2x00dev,
 			      const unsigned int word, const u8 value)
@@ -83,7 +83,7 @@ static void rt61pci_bbp_write(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, PHY_CSR3_BUSY, 1);
 		rt2x00_set_field32(&reg, PHY_CSR3_READ_CONTROL, 0);
 
-		rt2x00pci_register_write(rt2x00dev, PHY_CSR3, reg);
+		rt2x00mmio_register_write(rt2x00dev, PHY_CSR3, reg);
 	}
 
 	mutex_unlock(&rt2x00dev->csr_mutex);
@@ -110,7 +110,7 @@ static void rt61pci_bbp_read(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, PHY_CSR3_BUSY, 1);
 		rt2x00_set_field32(&reg, PHY_CSR3_READ_CONTROL, 1);
 
-		rt2x00pci_register_write(rt2x00dev, PHY_CSR3, reg);
+		rt2x00mmio_register_write(rt2x00dev, PHY_CSR3, reg);
 
 		WAIT_FOR_BBP(rt2x00dev, &reg);
 	}
@@ -138,7 +138,7 @@ static void rt61pci_rf_write(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, PHY_CSR4_IF_SELECT, 0);
 		rt2x00_set_field32(&reg, PHY_CSR4_BUSY, 1);
 
-		rt2x00pci_register_write(rt2x00dev, PHY_CSR4, reg);
+		rt2x00mmio_register_write(rt2x00dev, PHY_CSR4, reg);
 		rt2x00_rf_write(rt2x00dev, word, value);
 	}
 
@@ -162,12 +162,12 @@ static void rt61pci_mcu_request(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_CMD_TOKEN, token);
 		rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_ARG0, arg0);
 		rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_ARG1, arg1);
-		rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_CSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, H2M_MAILBOX_CSR, reg);
 
-		rt2x00pci_register_read(rt2x00dev, HOST_CMD_CSR, &reg);
+		rt2x00mmio_register_read(rt2x00dev, HOST_CMD_CSR, &reg);
 		rt2x00_set_field32(&reg, HOST_CMD_CSR_HOST_COMMAND, command);
 		rt2x00_set_field32(&reg, HOST_CMD_CSR_INTERRUPT_MCU, 1);
-		rt2x00pci_register_write(rt2x00dev, HOST_CMD_CSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, HOST_CMD_CSR, reg);
 	}
 
 	mutex_unlock(&rt2x00dev->csr_mutex);
@@ -179,7 +179,7 @@ static void rt61pci_eepromregister_read(struct eeprom_93cx6 *eeprom)
 	struct rt2x00_dev *rt2x00dev = eeprom->data;
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, E2PROM_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, E2PROM_CSR, &reg);
 
 	eeprom->reg_data_in = !!rt2x00_get_field32(reg, E2PROM_CSR_DATA_IN);
 	eeprom->reg_data_out = !!rt2x00_get_field32(reg, E2PROM_CSR_DATA_OUT);
@@ -201,15 +201,15 @@ static void rt61pci_eepromregister_write(struct eeprom_93cx6 *eeprom)
 	rt2x00_set_field32(&reg, E2PROM_CSR_CHIP_SELECT,
 			   !!eeprom->reg_chip_select);
 
-	rt2x00pci_register_write(rt2x00dev, E2PROM_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, E2PROM_CSR, reg);
 }
 
 #ifdef CONFIG_RT2X00_LIB_DEBUGFS
 static const struct rt2x00debug rt61pci_rt2x00debug = {
 	.owner	= THIS_MODULE,
 	.csr	= {
-		.read		= rt2x00pci_register_read,
-		.write		= rt2x00pci_register_write,
+		.read		= rt2x00mmio_register_read,
+		.write		= rt2x00mmio_register_write,
 		.flags		= RT2X00DEBUGFS_OFFSET,
 		.word_base	= CSR_REG_BASE,
 		.word_size	= sizeof(u32),
@@ -243,7 +243,7 @@ static int rt61pci_rfkill_poll(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, MAC_CSR13, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MAC_CSR13, &reg);
 	return rt2x00_get_field32(reg, MAC_CSR13_VAL5);
 }
 
@@ -294,10 +294,10 @@ static int rt61pci_blink_set(struct led_classdev *led_cdev,
 	    container_of(led_cdev, struct rt2x00_led, led_dev);
 	u32 reg;
 
-	rt2x00pci_register_read(led->rt2x00dev, MAC_CSR14, &reg);
+	rt2x00mmio_register_read(led->rt2x00dev, MAC_CSR14, &reg);
 	rt2x00_set_field32(&reg, MAC_CSR14_ON_PERIOD, *delay_on);
 	rt2x00_set_field32(&reg, MAC_CSR14_OFF_PERIOD, *delay_off);
-	rt2x00pci_register_write(led->rt2x00dev, MAC_CSR14, reg);
+	rt2x00mmio_register_write(led->rt2x00dev, MAC_CSR14, reg);
 
 	return 0;
 }
@@ -339,7 +339,7 @@ static int rt61pci_config_shared_key(struct rt2x00_dev *rt2x00dev,
 		 */
 		mask = (0xf << crypto->bssidx);
 
-		rt2x00pci_register_read(rt2x00dev, SEC_CSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, SEC_CSR0, &reg);
 		reg &= mask;
 
 		if (reg && reg == mask)
@@ -358,8 +358,8 @@ static int rt61pci_config_shared_key(struct rt2x00_dev *rt2x00dev,
 		       sizeof(key_entry.rx_mic));
 
 		reg = SHARED_KEY_ENTRY(key->hw_key_idx);
-		rt2x00pci_register_multiwrite(rt2x00dev, reg,
-					      &key_entry, sizeof(key_entry));
+		rt2x00mmio_register_multiwrite(rt2x00dev, reg,
+					       &key_entry, sizeof(key_entry));
 
 		/*
 		 * The cipher types are stored over 2 registers.
@@ -372,16 +372,16 @@ static int rt61pci_config_shared_key(struct rt2x00_dev *rt2x00dev,
 			field.bit_offset = (3 * key->hw_key_idx);
 			field.bit_mask = 0x7 << field.bit_offset;
 
-			rt2x00pci_register_read(rt2x00dev, SEC_CSR1, &reg);
+			rt2x00mmio_register_read(rt2x00dev, SEC_CSR1, &reg);
 			rt2x00_set_field32(&reg, field, crypto->cipher);
-			rt2x00pci_register_write(rt2x00dev, SEC_CSR1, reg);
+			rt2x00mmio_register_write(rt2x00dev, SEC_CSR1, reg);
 		} else {
 			field.bit_offset = (3 * (key->hw_key_idx - 8));
 			field.bit_mask = 0x7 << field.bit_offset;
 
-			rt2x00pci_register_read(rt2x00dev, SEC_CSR5, &reg);
+			rt2x00mmio_register_read(rt2x00dev, SEC_CSR5, &reg);
 			rt2x00_set_field32(&reg, field, crypto->cipher);
-			rt2x00pci_register_write(rt2x00dev, SEC_CSR5, reg);
+			rt2x00mmio_register_write(rt2x00dev, SEC_CSR5, reg);
 		}
 
 		/*
@@ -404,12 +404,12 @@ static int rt61pci_config_shared_key(struct rt2x00_dev *rt2x00dev,
 	 */
 	mask = 1 << key->hw_key_idx;
 
-	rt2x00pci_register_read(rt2x00dev, SEC_CSR0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, SEC_CSR0, &reg);
 	if (crypto->cmd == SET_KEY)
 		reg |= mask;
 	else if (crypto->cmd == DISABLE_KEY)
 		reg &= ~mask;
-	rt2x00pci_register_write(rt2x00dev, SEC_CSR0, reg);
+	rt2x00mmio_register_write(rt2x00dev, SEC_CSR0, reg);
 
 	return 0;
 }
@@ -433,10 +433,10 @@ static int rt61pci_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
 		 * When both registers are full, we drop the key.
 		 * Otherwise, we use the first invalid entry.
 		 */
-		rt2x00pci_register_read(rt2x00dev, SEC_CSR2, &reg);
+		rt2x00mmio_register_read(rt2x00dev, SEC_CSR2, &reg);
 		if (reg && reg == ~0) {
 			key->hw_key_idx = 32;
-			rt2x00pci_register_read(rt2x00dev, SEC_CSR3, &reg);
+			rt2x00mmio_register_read(rt2x00dev, SEC_CSR3, &reg);
 			if (reg && reg == ~0)
 				return -ENOSPC;
 		}
@@ -458,21 +458,21 @@ static int rt61pci_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
 		addr_entry.cipher = crypto->cipher;
 
 		reg = PAIRWISE_KEY_ENTRY(key->hw_key_idx);
-		rt2x00pci_register_multiwrite(rt2x00dev, reg,
-					      &key_entry, sizeof(key_entry));
+		rt2x00mmio_register_multiwrite(rt2x00dev, reg,
+					       &key_entry, sizeof(key_entry));
 
 		reg = PAIRWISE_TA_ENTRY(key->hw_key_idx);
-		rt2x00pci_register_multiwrite(rt2x00dev, reg,
-					      &addr_entry, sizeof(addr_entry));
+		rt2x00mmio_register_multiwrite(rt2x00dev, reg,
+					       &addr_entry, sizeof(addr_entry));
 
 		/*
 		 * Enable pairwise lookup table for given BSS idx.
 		 * Without this, received frames will not be decrypted
 		 * by the hardware.
 		 */
-		rt2x00pci_register_read(rt2x00dev, SEC_CSR4, &reg);
+		rt2x00mmio_register_read(rt2x00dev, SEC_CSR4, &reg);
 		reg |= (1 << crypto->bssidx);
-		rt2x00pci_register_write(rt2x00dev, SEC_CSR4, reg);
+		rt2x00mmio_register_write(rt2x00dev, SEC_CSR4, reg);
 
 		/*
 		 * The driver does not support the IV/EIV generation
@@ -495,21 +495,21 @@ static int rt61pci_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
 	if (key->hw_key_idx < 32) {
 		mask = 1 << key->hw_key_idx;
 
-		rt2x00pci_register_read(rt2x00dev, SEC_CSR2, &reg);
+		rt2x00mmio_register_read(rt2x00dev, SEC_CSR2, &reg);
 		if (crypto->cmd == SET_KEY)
 			reg |= mask;
 		else if (crypto->cmd == DISABLE_KEY)
 			reg &= ~mask;
-		rt2x00pci_register_write(rt2x00dev, SEC_CSR2, reg);
+		rt2x00mmio_register_write(rt2x00dev, SEC_CSR2, reg);
 	} else {
 		mask = 1 << (key->hw_key_idx - 32);
 
-		rt2x00pci_register_read(rt2x00dev, SEC_CSR3, &reg);
+		rt2x00mmio_register_read(rt2x00dev, SEC_CSR3, &reg);
 		if (crypto->cmd == SET_KEY)
 			reg |= mask;
 		else if (crypto->cmd == DISABLE_KEY)
 			reg &= ~mask;
-		rt2x00pci_register_write(rt2x00dev, SEC_CSR3, reg);
+		rt2x00mmio_register_write(rt2x00dev, SEC_CSR3, reg);
 	}
 
 	return 0;
@@ -526,7 +526,7 @@ static void rt61pci_config_filter(struct rt2x00_dev *rt2x00dev,
 	 * and broadcast frames will always be accepted since
 	 * there is no filter for it at this time.
 	 */
-	rt2x00pci_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR0, &reg);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_CRC,
 			   !(filter_flags & FIF_FCSFAIL));
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_PHYSICAL,
@@ -544,7 +544,7 @@ static void rt61pci_config_filter(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_BROADCAST, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_ACK_CTS,
 			   !(filter_flags & FIF_CONTROL));
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR0, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR0, reg);
 }
 
 static void rt61pci_config_intf(struct rt2x00_dev *rt2x00dev,
@@ -558,9 +558,9 @@ static void rt61pci_config_intf(struct rt2x00_dev *rt2x00dev,
 		/*
 		 * Enable synchronisation.
 		 */
-		rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9, &reg);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TSF_SYNC, conf->sync);
-		rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXRX_CSR9, reg);
 	}
 
 	if (flags & CONFIG_UPDATE_MAC) {
@@ -568,8 +568,8 @@ static void rt61pci_config_intf(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, MAC_CSR3_UNICAST_TO_ME_MASK, 0xff);
 		conf->mac[1] = cpu_to_le32(reg);
 
-		rt2x00pci_register_multiwrite(rt2x00dev, MAC_CSR2,
-					      conf->mac, sizeof(conf->mac));
+		rt2x00mmio_register_multiwrite(rt2x00dev, MAC_CSR2,
+					       conf->mac, sizeof(conf->mac));
 	}
 
 	if (flags & CONFIG_UPDATE_BSSID) {
@@ -577,8 +577,9 @@ static void rt61pci_config_intf(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, MAC_CSR5_BSS_ID_MASK, 3);
 		conf->bssid[1] = cpu_to_le32(reg);
 
-		rt2x00pci_register_multiwrite(rt2x00dev, MAC_CSR4,
-					      conf->bssid, sizeof(conf->bssid));
+		rt2x00mmio_register_multiwrite(rt2x00dev, MAC_CSR4,
+					       conf->bssid,
+					       sizeof(conf->bssid));
 	}
 }
 
@@ -588,40 +589,40 @@ static void rt61pci_config_erp(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR0, &reg);
 	rt2x00_set_field32(&reg, TXRX_CSR0_RX_ACK_TIMEOUT, 0x32);
 	rt2x00_set_field32(&reg, TXRX_CSR0_TSF_OFFSET, IEEE80211_HEADER);
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR0, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR0, reg);
 
 	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
-		rt2x00pci_register_read(rt2x00dev, TXRX_CSR4, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXRX_CSR4, &reg);
 		rt2x00_set_field32(&reg, TXRX_CSR4_AUTORESPOND_ENABLE, 1);
 		rt2x00_set_field32(&reg, TXRX_CSR4_AUTORESPOND_PREAMBLE,
 				   !!erp->short_preamble);
-		rt2x00pci_register_write(rt2x00dev, TXRX_CSR4, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXRX_CSR4, reg);
 	}
 
 	if (changed & BSS_CHANGED_BASIC_RATES)
-		rt2x00pci_register_write(rt2x00dev, TXRX_CSR5,
-					 erp->basic_rates);
+		rt2x00mmio_register_write(rt2x00dev, TXRX_CSR5,
+					  erp->basic_rates);
 
 	if (changed & BSS_CHANGED_BEACON_INT) {
-		rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9, &reg);
 		rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_INTERVAL,
 				   erp->beacon_int * 16);
-		rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXRX_CSR9, reg);
 	}
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
-		rt2x00pci_register_read(rt2x00dev, MAC_CSR9, &reg);
+		rt2x00mmio_register_read(rt2x00dev, MAC_CSR9, &reg);
 		rt2x00_set_field32(&reg, MAC_CSR9_SLOT_TIME, erp->slot_time);
-		rt2x00pci_register_write(rt2x00dev, MAC_CSR9, reg);
+		rt2x00mmio_register_write(rt2x00dev, MAC_CSR9, reg);
 
-		rt2x00pci_register_read(rt2x00dev, MAC_CSR8, &reg);
+		rt2x00mmio_register_read(rt2x00dev, MAC_CSR8, &reg);
 		rt2x00_set_field32(&reg, MAC_CSR8_SIFS, erp->sifs);
 		rt2x00_set_field32(&reg, MAC_CSR8_SIFS_AFTER_RX_OFDM, 3);
 		rt2x00_set_field32(&reg, MAC_CSR8_EIFS, erp->eifs);
-		rt2x00pci_register_write(rt2x00dev, MAC_CSR8, reg);
+		rt2x00mmio_register_write(rt2x00dev, MAC_CSR8, reg);
 	}
 }
 
@@ -714,7 +715,7 @@ static void rt61pci_config_antenna_2529_rx(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, MAC_CSR13, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MAC_CSR13, &reg);
 
 	rt2x00_set_field32(&reg, MAC_CSR13_DIR4, 0);
 	rt2x00_set_field32(&reg, MAC_CSR13_VAL4, p1);
@@ -722,7 +723,7 @@ static void rt61pci_config_antenna_2529_rx(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field32(&reg, MAC_CSR13_DIR3, 0);
 	rt2x00_set_field32(&reg, MAC_CSR13_VAL3, !p2);
 
-	rt2x00pci_register_write(rt2x00dev, MAC_CSR13, reg);
+	rt2x00mmio_register_write(rt2x00dev, MAC_CSR13, reg);
 }
 
 static void rt61pci_config_antenna_2529(struct rt2x00_dev *rt2x00dev,
@@ -821,14 +822,14 @@ static void rt61pci_config_ant(struct rt2x00_dev *rt2x00dev,
 	for (i = 0; i < ARRAY_SIZE(antenna_sel_a); i++)
 		rt61pci_bbp_write(rt2x00dev, sel[i].word, sel[i].value[lna]);
 
-	rt2x00pci_register_read(rt2x00dev, PHY_CSR0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, PHY_CSR0, &reg);
 
 	rt2x00_set_field32(&reg, PHY_CSR0_PA_PE_BG,
 			   rt2x00dev->curr_band == IEEE80211_BAND_2GHZ);
 	rt2x00_set_field32(&reg, PHY_CSR0_PA_PE_A,
 			   rt2x00dev->curr_band == IEEE80211_BAND_5GHZ);
 
-	rt2x00pci_register_write(rt2x00dev, PHY_CSR0, reg);
+	rt2x00mmio_register_write(rt2x00dev, PHY_CSR0, reg);
 
 	if (rt2x00_rf(rt2x00dev, RF5225) || rt2x00_rf(rt2x00dev, RF5325))
 		rt61pci_config_antenna_5x(rt2x00dev, ant);
@@ -848,7 +849,7 @@ static void rt61pci_config_lna_gain(struct rt2x00_dev *rt2x00dev,
 	u16 eeprom;
 	short lna_gain = 0;
 
-	if (libconf->conf->channel->band == IEEE80211_BAND_2GHZ) {
+	if (libconf->conf->chandef.chan->band == IEEE80211_BAND_2GHZ) {
 		if (test_bit(CAPABILITY_EXTERNAL_LNA_BG, &rt2x00dev->cap_flags))
 			lna_gain += 14;
 
@@ -928,7 +929,7 @@ static void rt61pci_config_retry_limit(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, TXRX_CSR4, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR4, &reg);
 	rt2x00_set_field32(&reg, TXRX_CSR4_OFDM_TX_RATE_DOWN, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR4_OFDM_TX_RATE_STEP, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR4_OFDM_TX_FALLBACK_CCK, 0);
@@ -936,7 +937,7 @@ static void rt61pci_config_retry_limit(struct rt2x00_dev *rt2x00dev,
 			   libconf->conf->long_frame_max_tx_count);
 	rt2x00_set_field32(&reg, TXRX_CSR4_SHORT_RETRY_LIMIT,
 			   libconf->conf->short_frame_max_tx_count);
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR4, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR4, reg);
 }
 
 static void rt61pci_config_ps(struct rt2x00_dev *rt2x00dev,
@@ -948,7 +949,7 @@ static void rt61pci_config_ps(struct rt2x00_dev *rt2x00dev,
 	u32 reg;
 
 	if (state == STATE_SLEEP) {
-		rt2x00pci_register_read(rt2x00dev, MAC_CSR11, &reg);
+		rt2x00mmio_register_read(rt2x00dev, MAC_CSR11, &reg);
 		rt2x00_set_field32(&reg, MAC_CSR11_DELAY_AFTER_TBCN,
 				   rt2x00dev->beacon_int - 10);
 		rt2x00_set_field32(&reg, MAC_CSR11_TBCN_BEFORE_WAKEUP,
@@ -957,27 +958,29 @@ static void rt61pci_config_ps(struct rt2x00_dev *rt2x00dev,
 
 		/* We must first disable autowake before it can be enabled */
 		rt2x00_set_field32(&reg, MAC_CSR11_AUTOWAKE, 0);
-		rt2x00pci_register_write(rt2x00dev, MAC_CSR11, reg);
+		rt2x00mmio_register_write(rt2x00dev, MAC_CSR11, reg);
 
 		rt2x00_set_field32(&reg, MAC_CSR11_AUTOWAKE, 1);
-		rt2x00pci_register_write(rt2x00dev, MAC_CSR11, reg);
+		rt2x00mmio_register_write(rt2x00dev, MAC_CSR11, reg);
 
-		rt2x00pci_register_write(rt2x00dev, SOFT_RESET_CSR, 0x00000005);
-		rt2x00pci_register_write(rt2x00dev, IO_CNTL_CSR, 0x0000001c);
-		rt2x00pci_register_write(rt2x00dev, PCI_USEC_CSR, 0x00000060);
+		rt2x00mmio_register_write(rt2x00dev, SOFT_RESET_CSR,
+					  0x00000005);
+		rt2x00mmio_register_write(rt2x00dev, IO_CNTL_CSR, 0x0000001c);
+		rt2x00mmio_register_write(rt2x00dev, PCI_USEC_CSR, 0x00000060);
 
 		rt61pci_mcu_request(rt2x00dev, MCU_SLEEP, 0xff, 0, 0);
 	} else {
-		rt2x00pci_register_read(rt2x00dev, MAC_CSR11, &reg);
+		rt2x00mmio_register_read(rt2x00dev, MAC_CSR11, &reg);
 		rt2x00_set_field32(&reg, MAC_CSR11_DELAY_AFTER_TBCN, 0);
 		rt2x00_set_field32(&reg, MAC_CSR11_TBCN_BEFORE_WAKEUP, 0);
 		rt2x00_set_field32(&reg, MAC_CSR11_AUTOWAKE, 0);
 		rt2x00_set_field32(&reg, MAC_CSR11_WAKEUP_LATENCY, 0);
-		rt2x00pci_register_write(rt2x00dev, MAC_CSR11, reg);
+		rt2x00mmio_register_write(rt2x00dev, MAC_CSR11, reg);
 
-		rt2x00pci_register_write(rt2x00dev, SOFT_RESET_CSR, 0x00000007);
-		rt2x00pci_register_write(rt2x00dev, IO_CNTL_CSR, 0x00000018);
-		rt2x00pci_register_write(rt2x00dev, PCI_USEC_CSR, 0x00000020);
+		rt2x00mmio_register_write(rt2x00dev, SOFT_RESET_CSR,
+					  0x00000007);
+		rt2x00mmio_register_write(rt2x00dev, IO_CNTL_CSR, 0x00000018);
+		rt2x00mmio_register_write(rt2x00dev, PCI_USEC_CSR, 0x00000020);
 
 		rt61pci_mcu_request(rt2x00dev, MCU_WAKEUP, 0xff, 0, 0);
 	}
@@ -1013,13 +1016,13 @@ static void rt61pci_link_stats(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Update FCS error count from register.
 	 */
-	rt2x00pci_register_read(rt2x00dev, STA_CSR0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, STA_CSR0, &reg);
 	qual->rx_failed = rt2x00_get_field32(reg, STA_CSR0_FCS_ERROR);
 
 	/*
 	 * Update False CCA count from register.
 	 */
-	rt2x00pci_register_read(rt2x00dev, STA_CSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, STA_CSR1, &reg);
 	qual->false_cca = rt2x00_get_field32(reg, STA_CSR1_FALSE_CCA_ERROR);
 }
 
@@ -1138,16 +1141,16 @@ static void rt61pci_start_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2x00pci_register_read(rt2x00dev, TXRX_CSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXRX_CSR0, &reg);
 		rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX, 0);
-		rt2x00pci_register_write(rt2x00dev, TXRX_CSR0, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXRX_CSR0, reg);
 		break;
 	case QID_BEACON:
-		rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9, &reg);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 1);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 1);
 		rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 1);
-		rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXRX_CSR9, reg);
 		break;
 	default:
 		break;
@@ -1161,24 +1164,24 @@ static void rt61pci_kick_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_AC_VO:
-		rt2x00pci_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC0, 1);
-		rt2x00pci_register_write(rt2x00dev, TX_CNTL_CSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
 	case QID_AC_VI:
-		rt2x00pci_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC1, 1);
-		rt2x00pci_register_write(rt2x00dev, TX_CNTL_CSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
 	case QID_AC_BE:
-		rt2x00pci_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC2, 1);
-		rt2x00pci_register_write(rt2x00dev, TX_CNTL_CSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
 	case QID_AC_BK:
-		rt2x00pci_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC3, 1);
-		rt2x00pci_register_write(rt2x00dev, TX_CNTL_CSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
 	default:
 		break;
@@ -1192,36 +1195,36 @@ static void rt61pci_stop_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_AC_VO:
-		rt2x00pci_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC0, 1);
-		rt2x00pci_register_write(rt2x00dev, TX_CNTL_CSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
 	case QID_AC_VI:
-		rt2x00pci_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC1, 1);
-		rt2x00pci_register_write(rt2x00dev, TX_CNTL_CSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
 	case QID_AC_BE:
-		rt2x00pci_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC2, 1);
-		rt2x00pci_register_write(rt2x00dev, TX_CNTL_CSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
 	case QID_AC_BK:
-		rt2x00pci_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC3, 1);
-		rt2x00pci_register_write(rt2x00dev, TX_CNTL_CSR, reg);
+		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
 	case QID_RX:
-		rt2x00pci_register_read(rt2x00dev, TXRX_CSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXRX_CSR0, &reg);
 		rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX, 1);
-		rt2x00pci_register_write(rt2x00dev, TXRX_CSR0, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXRX_CSR0, reg);
 		break;
 	case QID_BEACON:
-		rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
+		rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9, &reg);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 0);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 0);
 		rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
-		rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+		rt2x00mmio_register_write(rt2x00dev, TXRX_CSR9, reg);
 
 		/*
 		 * Wait for possibly running tbtt tasklets.
@@ -1299,14 +1302,14 @@ static int rt61pci_load_firmware(struct rt2x00_dev *rt2x00dev,
 	 * Wait for stable hardware.
 	 */
 	for (i = 0; i < 100; i++) {
-		rt2x00pci_register_read(rt2x00dev, MAC_CSR0, &reg);
+		rt2x00mmio_register_read(rt2x00dev, MAC_CSR0, &reg);
 		if (reg)
 			break;
 		msleep(1);
 	}
 
 	if (!reg) {
-		ERROR(rt2x00dev, "Unstable hardware.\n");
+		rt2x00_err(rt2x00dev, "Unstable hardware\n");
 		return -EBUSY;
 	}
 
@@ -1315,10 +1318,10 @@ static int rt61pci_load_firmware(struct rt2x00_dev *rt2x00dev,
 	 */
 	reg = 0;
 	rt2x00_set_field32(&reg, MCU_CNTL_CSR_RESET, 1);
-	rt2x00pci_register_write(rt2x00dev, MCU_CNTL_CSR, reg);
-	rt2x00pci_register_write(rt2x00dev, M2H_CMD_DONE_CSR, 0xffffffff);
-	rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_CSR, 0);
-	rt2x00pci_register_write(rt2x00dev, HOST_CMD_CSR, 0);
+	rt2x00mmio_register_write(rt2x00dev, MCU_CNTL_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, M2H_CMD_DONE_CSR, 0xffffffff);
+	rt2x00mmio_register_write(rt2x00dev, H2M_MAILBOX_CSR, 0);
+	rt2x00mmio_register_write(rt2x00dev, HOST_CMD_CSR, 0);
 
 	/*
 	 * Write firmware to device.
@@ -1326,26 +1329,26 @@ static int rt61pci_load_firmware(struct rt2x00_dev *rt2x00dev,
 	reg = 0;
 	rt2x00_set_field32(&reg, MCU_CNTL_CSR_RESET, 1);
 	rt2x00_set_field32(&reg, MCU_CNTL_CSR_SELECT_BANK, 1);
-	rt2x00pci_register_write(rt2x00dev, MCU_CNTL_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, MCU_CNTL_CSR, reg);
 
-	rt2x00pci_register_multiwrite(rt2x00dev, FIRMWARE_IMAGE_BASE,
-				      data, len);
+	rt2x00mmio_register_multiwrite(rt2x00dev, FIRMWARE_IMAGE_BASE,
+				       data, len);
 
 	rt2x00_set_field32(&reg, MCU_CNTL_CSR_SELECT_BANK, 0);
-	rt2x00pci_register_write(rt2x00dev, MCU_CNTL_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, MCU_CNTL_CSR, reg);
 
 	rt2x00_set_field32(&reg, MCU_CNTL_CSR_RESET, 0);
-	rt2x00pci_register_write(rt2x00dev, MCU_CNTL_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, MCU_CNTL_CSR, reg);
 
 	for (i = 0; i < 100; i++) {
-		rt2x00pci_register_read(rt2x00dev, MCU_CNTL_CSR, &reg);
+		rt2x00mmio_register_read(rt2x00dev, MCU_CNTL_CSR, &reg);
 		if (rt2x00_get_field32(reg, MCU_CNTL_CSR_READY))
 			break;
 		msleep(1);
 	}
 
 	if (i == 100) {
-		ERROR(rt2x00dev, "MCU Control register not ready.\n");
+		rt2x00_err(rt2x00dev, "MCU Control register not ready\n");
 		return -EBUSY;
 	}
 
@@ -1360,16 +1363,16 @@ static int rt61pci_load_firmware(struct rt2x00_dev *rt2x00dev,
 	reg = 0;
 	rt2x00_set_field32(&reg, MAC_CSR1_SOFT_RESET, 1);
 	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 1);
-	rt2x00pci_register_write(rt2x00dev, MAC_CSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, MAC_CSR1, reg);
 
-	rt2x00pci_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MAC_CSR1, &reg);
 	rt2x00_set_field32(&reg, MAC_CSR1_SOFT_RESET, 0);
 	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 0);
-	rt2x00pci_register_write(rt2x00dev, MAC_CSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, MAC_CSR1, reg);
 
-	rt2x00pci_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MAC_CSR1, &reg);
 	rt2x00_set_field32(&reg, MAC_CSR1_HOST_READY, 1);
-	rt2x00pci_register_write(rt2x00dev, MAC_CSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, MAC_CSR1, reg);
 
 	return 0;
 }
@@ -1379,7 +1382,7 @@ static int rt61pci_load_firmware(struct rt2x00_dev *rt2x00dev,
  */
 static bool rt61pci_get_entry_state(struct queue_entry *entry)
 {
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	u32 word;
 
 	if (entry->queue->qid == QID_RX) {
@@ -1396,7 +1399,7 @@ static bool rt61pci_get_entry_state(struct queue_entry *entry)
 
 static void rt61pci_clear_entry(struct queue_entry *entry)
 {
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
 	u32 word;
 
@@ -1419,13 +1422,13 @@ static void rt61pci_clear_entry(struct queue_entry *entry)
 
 static int rt61pci_init_queues(struct rt2x00_dev *rt2x00dev)
 {
-	struct queue_entry_priv_pci *entry_priv;
+	struct queue_entry_priv_mmio *entry_priv;
 	u32 reg;
 
 	/*
 	 * Initialize registers.
 	 */
-	rt2x00pci_register_read(rt2x00dev, TX_RING_CSR0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TX_RING_CSR0, &reg);
 	rt2x00_set_field32(&reg, TX_RING_CSR0_AC0_RING_SIZE,
 			   rt2x00dev->tx[0].limit);
 	rt2x00_set_field32(&reg, TX_RING_CSR0_AC1_RING_SIZE,
@@ -1434,67 +1437,67 @@ static int rt61pci_init_queues(struct rt2x00_dev *rt2x00dev)
 			   rt2x00dev->tx[2].limit);
 	rt2x00_set_field32(&reg, TX_RING_CSR0_AC3_RING_SIZE,
 			   rt2x00dev->tx[3].limit);
-	rt2x00pci_register_write(rt2x00dev, TX_RING_CSR0, reg);
+	rt2x00mmio_register_write(rt2x00dev, TX_RING_CSR0, reg);
 
-	rt2x00pci_register_read(rt2x00dev, TX_RING_CSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TX_RING_CSR1, &reg);
 	rt2x00_set_field32(&reg, TX_RING_CSR1_TXD_SIZE,
 			   rt2x00dev->tx[0].desc_size / 4);
-	rt2x00pci_register_write(rt2x00dev, TX_RING_CSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, TX_RING_CSR1, reg);
 
 	entry_priv = rt2x00dev->tx[0].entries[0].priv_data;
-	rt2x00pci_register_read(rt2x00dev, AC0_BASE_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, AC0_BASE_CSR, &reg);
 	rt2x00_set_field32(&reg, AC0_BASE_CSR_RING_REGISTER,
 			   entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, AC0_BASE_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, AC0_BASE_CSR, reg);
 
 	entry_priv = rt2x00dev->tx[1].entries[0].priv_data;
-	rt2x00pci_register_read(rt2x00dev, AC1_BASE_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, AC1_BASE_CSR, &reg);
 	rt2x00_set_field32(&reg, AC1_BASE_CSR_RING_REGISTER,
 			   entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, AC1_BASE_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, AC1_BASE_CSR, reg);
 
 	entry_priv = rt2x00dev->tx[2].entries[0].priv_data;
-	rt2x00pci_register_read(rt2x00dev, AC2_BASE_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, AC2_BASE_CSR, &reg);
 	rt2x00_set_field32(&reg, AC2_BASE_CSR_RING_REGISTER,
 			   entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, AC2_BASE_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, AC2_BASE_CSR, reg);
 
 	entry_priv = rt2x00dev->tx[3].entries[0].priv_data;
-	rt2x00pci_register_read(rt2x00dev, AC3_BASE_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, AC3_BASE_CSR, &reg);
 	rt2x00_set_field32(&reg, AC3_BASE_CSR_RING_REGISTER,
 			   entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, AC3_BASE_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, AC3_BASE_CSR, reg);
 
-	rt2x00pci_register_read(rt2x00dev, RX_RING_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, RX_RING_CSR, &reg);
 	rt2x00_set_field32(&reg, RX_RING_CSR_RING_SIZE, rt2x00dev->rx->limit);
 	rt2x00_set_field32(&reg, RX_RING_CSR_RXD_SIZE,
 			   rt2x00dev->rx->desc_size / 4);
 	rt2x00_set_field32(&reg, RX_RING_CSR_RXD_WRITEBACK_SIZE, 4);
-	rt2x00pci_register_write(rt2x00dev, RX_RING_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, RX_RING_CSR, reg);
 
 	entry_priv = rt2x00dev->rx->entries[0].priv_data;
-	rt2x00pci_register_read(rt2x00dev, RX_BASE_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, RX_BASE_CSR, &reg);
 	rt2x00_set_field32(&reg, RX_BASE_CSR_RING_REGISTER,
 			   entry_priv->desc_dma);
-	rt2x00pci_register_write(rt2x00dev, RX_BASE_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, RX_BASE_CSR, reg);
 
-	rt2x00pci_register_read(rt2x00dev, TX_DMA_DST_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TX_DMA_DST_CSR, &reg);
 	rt2x00_set_field32(&reg, TX_DMA_DST_CSR_DEST_AC0, 2);
 	rt2x00_set_field32(&reg, TX_DMA_DST_CSR_DEST_AC1, 2);
 	rt2x00_set_field32(&reg, TX_DMA_DST_CSR_DEST_AC2, 2);
 	rt2x00_set_field32(&reg, TX_DMA_DST_CSR_DEST_AC3, 2);
-	rt2x00pci_register_write(rt2x00dev, TX_DMA_DST_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, TX_DMA_DST_CSR, reg);
 
-	rt2x00pci_register_read(rt2x00dev, LOAD_TX_RING_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, LOAD_TX_RING_CSR, &reg);
 	rt2x00_set_field32(&reg, LOAD_TX_RING_CSR_LOAD_TXD_AC0, 1);
 	rt2x00_set_field32(&reg, LOAD_TX_RING_CSR_LOAD_TXD_AC1, 1);
 	rt2x00_set_field32(&reg, LOAD_TX_RING_CSR_LOAD_TXD_AC2, 1);
 	rt2x00_set_field32(&reg, LOAD_TX_RING_CSR_LOAD_TXD_AC3, 1);
-	rt2x00pci_register_write(rt2x00dev, LOAD_TX_RING_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, LOAD_TX_RING_CSR, reg);
 
-	rt2x00pci_register_read(rt2x00dev, RX_CNTL_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, RX_CNTL_CSR, &reg);
 	rt2x00_set_field32(&reg, RX_CNTL_CSR_LOAD_RXD, 1);
-	rt2x00pci_register_write(rt2x00dev, RX_CNTL_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, RX_CNTL_CSR, reg);
 
 	return 0;
 }
@@ -1503,13 +1506,13 @@ static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR0, &reg);
 	rt2x00_set_field32(&reg, TXRX_CSR0_AUTO_TX_SEQ, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR0_TX_WITHOUT_WAITING, 0);
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR0, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR0, reg);
 
-	rt2x00pci_register_read(rt2x00dev, TXRX_CSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR1, &reg);
 	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID0, 47); /* CCK Signal */
 	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID0_VALID, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID1, 30); /* Rssi */
@@ -1518,12 +1521,12 @@ static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID2_VALID, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID3, 30); /* Rssi */
 	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID3_VALID, 1);
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR1, reg);
 
 	/*
 	 * CCK TXD BBP registers
 	 */
-	rt2x00pci_register_read(rt2x00dev, TXRX_CSR2, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR2, &reg);
 	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID0, 13);
 	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID0_VALID, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID1, 12);
@@ -1532,76 +1535,76 @@ static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID2_VALID, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID3, 10);
 	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID3_VALID, 1);
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR2, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR2, reg);
 
 	/*
 	 * OFDM TXD BBP registers
 	 */
-	rt2x00pci_register_read(rt2x00dev, TXRX_CSR3, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR3, &reg);
 	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID0, 7);
 	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID0_VALID, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID1, 6);
 	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID1_VALID, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID2, 5);
 	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID2_VALID, 1);
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR3, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR3, reg);
 
-	rt2x00pci_register_read(rt2x00dev, TXRX_CSR7, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR7, &reg);
 	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_6MBS, 59);
 	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_9MBS, 53);
 	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_12MBS, 49);
 	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_18MBS, 46);
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR7, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR7, reg);
 
-	rt2x00pci_register_read(rt2x00dev, TXRX_CSR8, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR8, &reg);
 	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_24MBS, 44);
 	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_36MBS, 42);
 	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_48MBS, 42);
 	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_54MBS, 42);
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR8, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR8, reg);
 
-	rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9, &reg);
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_INTERVAL, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_SYNC, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR9_TIMESTAMP_COMPENSATE, 0);
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR9, reg);
 
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR15, 0x0000000f);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR15, 0x0000000f);
 
-	rt2x00pci_register_write(rt2x00dev, MAC_CSR6, 0x00000fff);
+	rt2x00mmio_register_write(rt2x00dev, MAC_CSR6, 0x00000fff);
 
-	rt2x00pci_register_read(rt2x00dev, MAC_CSR9, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MAC_CSR9, &reg);
 	rt2x00_set_field32(&reg, MAC_CSR9_CW_SELECT, 0);
-	rt2x00pci_register_write(rt2x00dev, MAC_CSR9, reg);
+	rt2x00mmio_register_write(rt2x00dev, MAC_CSR9, reg);
 
-	rt2x00pci_register_write(rt2x00dev, MAC_CSR10, 0x0000071c);
+	rt2x00mmio_register_write(rt2x00dev, MAC_CSR10, 0x0000071c);
 
 	if (rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_AWAKE))
 		return -EBUSY;
 
-	rt2x00pci_register_write(rt2x00dev, MAC_CSR13, 0x0000e000);
+	rt2x00mmio_register_write(rt2x00dev, MAC_CSR13, 0x0000e000);
 
 	/*
 	 * Invalidate all Shared Keys (SEC_CSR0),
 	 * and clear the Shared key Cipher algorithms (SEC_CSR1 & SEC_CSR5)
 	 */
-	rt2x00pci_register_write(rt2x00dev, SEC_CSR0, 0x00000000);
-	rt2x00pci_register_write(rt2x00dev, SEC_CSR1, 0x00000000);
-	rt2x00pci_register_write(rt2x00dev, SEC_CSR5, 0x00000000);
+	rt2x00mmio_register_write(rt2x00dev, SEC_CSR0, 0x00000000);
+	rt2x00mmio_register_write(rt2x00dev, SEC_CSR1, 0x00000000);
+	rt2x00mmio_register_write(rt2x00dev, SEC_CSR5, 0x00000000);
 
-	rt2x00pci_register_write(rt2x00dev, PHY_CSR1, 0x000023b0);
-	rt2x00pci_register_write(rt2x00dev, PHY_CSR5, 0x060a100c);
-	rt2x00pci_register_write(rt2x00dev, PHY_CSR6, 0x00080606);
-	rt2x00pci_register_write(rt2x00dev, PHY_CSR7, 0x00000a08);
+	rt2x00mmio_register_write(rt2x00dev, PHY_CSR1, 0x000023b0);
+	rt2x00mmio_register_write(rt2x00dev, PHY_CSR5, 0x060a100c);
+	rt2x00mmio_register_write(rt2x00dev, PHY_CSR6, 0x00080606);
+	rt2x00mmio_register_write(rt2x00dev, PHY_CSR7, 0x00000a08);
 
-	rt2x00pci_register_write(rt2x00dev, PCI_CFG_CSR, 0x28ca4404);
+	rt2x00mmio_register_write(rt2x00dev, PCI_CFG_CSR, 0x28ca4404);
 
-	rt2x00pci_register_write(rt2x00dev, TEST_MODE_CSR, 0x00000200);
+	rt2x00mmio_register_write(rt2x00dev, TEST_MODE_CSR, 0x00000200);
 
-	rt2x00pci_register_write(rt2x00dev, M2H_CMD_DONE_CSR, 0xffffffff);
+	rt2x00mmio_register_write(rt2x00dev, M2H_CMD_DONE_CSR, 0xffffffff);
 
 	/*
 	 * Clear all beacons
@@ -1609,36 +1612,36 @@ static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	 * the first byte since that byte contains the VALID and OWNER
 	 * bits which (when set to 0) will invalidate the entire beacon.
 	 */
-	rt2x00pci_register_write(rt2x00dev, HW_BEACON_BASE0, 0);
-	rt2x00pci_register_write(rt2x00dev, HW_BEACON_BASE1, 0);
-	rt2x00pci_register_write(rt2x00dev, HW_BEACON_BASE2, 0);
-	rt2x00pci_register_write(rt2x00dev, HW_BEACON_BASE3, 0);
+	rt2x00mmio_register_write(rt2x00dev, HW_BEACON_BASE0, 0);
+	rt2x00mmio_register_write(rt2x00dev, HW_BEACON_BASE1, 0);
+	rt2x00mmio_register_write(rt2x00dev, HW_BEACON_BASE2, 0);
+	rt2x00mmio_register_write(rt2x00dev, HW_BEACON_BASE3, 0);
 
 	/*
 	 * We must clear the error counters.
 	 * These registers are cleared on read,
 	 * so we may pass a useless variable to store the value.
 	 */
-	rt2x00pci_register_read(rt2x00dev, STA_CSR0, &reg);
-	rt2x00pci_register_read(rt2x00dev, STA_CSR1, &reg);
-	rt2x00pci_register_read(rt2x00dev, STA_CSR2, &reg);
+	rt2x00mmio_register_read(rt2x00dev, STA_CSR0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, STA_CSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, STA_CSR2, &reg);
 
 	/*
 	 * Reset MAC and BBP registers.
 	 */
-	rt2x00pci_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MAC_CSR1, &reg);
 	rt2x00_set_field32(&reg, MAC_CSR1_SOFT_RESET, 1);
 	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 1);
-	rt2x00pci_register_write(rt2x00dev, MAC_CSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, MAC_CSR1, reg);
 
-	rt2x00pci_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MAC_CSR1, &reg);
 	rt2x00_set_field32(&reg, MAC_CSR1_SOFT_RESET, 0);
 	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 0);
-	rt2x00pci_register_write(rt2x00dev, MAC_CSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, MAC_CSR1, reg);
 
-	rt2x00pci_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MAC_CSR1, &reg);
 	rt2x00_set_field32(&reg, MAC_CSR1_HOST_READY, 1);
-	rt2x00pci_register_write(rt2x00dev, MAC_CSR1, reg);
+	rt2x00mmio_register_write(rt2x00dev, MAC_CSR1, reg);
 
 	return 0;
 }
@@ -1655,7 +1658,7 @@ static int rt61pci_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 		udelay(REGISTER_BUSY_DELAY);
 	}
 
-	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
+	rt2x00_err(rt2x00dev, "BBP register access failed, aborting\n");
 	return -EACCES;
 }
 
@@ -1722,11 +1725,11 @@ static void rt61pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	 * should clear the register to assure a clean state.
 	 */
 	if (state == STATE_RADIO_IRQ_ON) {
-		rt2x00pci_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
-		rt2x00pci_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
+		rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
+		rt2x00mmio_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
 
-		rt2x00pci_register_read(rt2x00dev, MCU_INT_SOURCE_CSR, &reg);
-		rt2x00pci_register_write(rt2x00dev, MCU_INT_SOURCE_CSR, reg);
+		rt2x00mmio_register_read(rt2x00dev, MCU_INT_SOURCE_CSR, &reg);
+		rt2x00mmio_register_write(rt2x00dev, MCU_INT_SOURCE_CSR, reg);
 	}
 
 	/*
@@ -1735,15 +1738,15 @@ static void rt61pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	 */
 	spin_lock_irqsave(&rt2x00dev->irqmask_lock, flags);
 
-	rt2x00pci_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR, &reg);
 	rt2x00_set_field32(&reg, INT_MASK_CSR_TXDONE, mask);
 	rt2x00_set_field32(&reg, INT_MASK_CSR_RXDONE, mask);
 	rt2x00_set_field32(&reg, INT_MASK_CSR_BEACON_DONE, mask);
 	rt2x00_set_field32(&reg, INT_MASK_CSR_ENABLE_MITIGATION, mask);
 	rt2x00_set_field32(&reg, INT_MASK_CSR_MITIGATION_PERIOD, 0xff);
-	rt2x00pci_register_write(rt2x00dev, INT_MASK_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
 
-	rt2x00pci_register_read(rt2x00dev, MCU_INT_MASK_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MCU_INT_MASK_CSR, &reg);
 	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_0, mask);
 	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_1, mask);
 	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_2, mask);
@@ -1753,7 +1756,7 @@ static void rt61pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_6, mask);
 	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_7, mask);
 	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_TWAKEUP, mask);
-	rt2x00pci_register_write(rt2x00dev, MCU_INT_MASK_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, MCU_INT_MASK_CSR, reg);
 
 	spin_unlock_irqrestore(&rt2x00dev->irqmask_lock, flags);
 
@@ -1783,9 +1786,9 @@ static int rt61pci_enable_radio(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Enable RX.
 	 */
-	rt2x00pci_register_read(rt2x00dev, RX_CNTL_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, RX_CNTL_CSR, &reg);
 	rt2x00_set_field32(&reg, RX_CNTL_CSR_ENABLE_RX_DMA, 1);
-	rt2x00pci_register_write(rt2x00dev, RX_CNTL_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, RX_CNTL_CSR, reg);
 
 	return 0;
 }
@@ -1795,7 +1798,7 @@ static void rt61pci_disable_radio(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Disable power
 	 */
-	rt2x00pci_register_write(rt2x00dev, MAC_CSR10, 0x00001818);
+	rt2x00mmio_register_write(rt2x00dev, MAC_CSR10, 0x00001818);
 }
 
 static int rt61pci_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state state)
@@ -1806,10 +1809,10 @@ static int rt61pci_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state state)
 
 	put_to_sleep = (state != STATE_AWAKE);
 
-	rt2x00pci_register_read(rt2x00dev, MAC_CSR12, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MAC_CSR12, &reg);
 	rt2x00_set_field32(&reg, MAC_CSR12_FORCE_WAKEUP, !put_to_sleep);
 	rt2x00_set_field32(&reg, MAC_CSR12_PUT_TO_SLEEP, put_to_sleep);
-	rt2x00pci_register_write(rt2x00dev, MAC_CSR12, reg);
+	rt2x00mmio_register_write(rt2x00dev, MAC_CSR12, reg);
 
 	/*
 	 * Device is not guaranteed to be in the requested state yet.
@@ -1817,11 +1820,11 @@ static int rt61pci_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state state)
 	 * device has entered the correct state.
 	 */
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2x00pci_register_read(rt2x00dev, MAC_CSR12, &reg2);
+		rt2x00mmio_register_read(rt2x00dev, MAC_CSR12, &reg2);
 		state = rt2x00_get_field32(reg2, MAC_CSR12_BBP_CURRENT_STATE);
 		if (state == !put_to_sleep)
 			return 0;
-		rt2x00pci_register_write(rt2x00dev, MAC_CSR12, reg);
+		rt2x00mmio_register_write(rt2x00dev, MAC_CSR12, reg);
 		msleep(10);
 	}
 
@@ -1856,8 +1859,8 @@ static int rt61pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 	}
 
 	if (unlikely(retval))
-		ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
-		      state, retval);
+		rt2x00_err(rt2x00dev, "Device failed to enter state %d (%d)\n",
+			   state, retval);
 
 	return retval;
 }
@@ -1869,7 +1872,7 @@ static void rt61pci_write_tx_desc(struct queue_entry *entry,
 				  struct txentry_desc *txdesc)
 {
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	__le32 *txd = entry_priv->desc;
 	u32 word;
 
@@ -1967,7 +1970,7 @@ static void rt61pci_write_beacon(struct queue_entry *entry,
 				 struct txentry_desc *txdesc)
 {
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	unsigned int beacon_base;
 	unsigned int padding_len;
 	u32 orig_reg, reg;
@@ -1976,10 +1979,10 @@ static void rt61pci_write_beacon(struct queue_entry *entry,
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
-	rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9, &reg);
 	orig_reg = reg;
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR9, reg);
 
 	/*
 	 * Write the TX descriptor for the beacon.
@@ -1996,19 +1999,19 @@ static void rt61pci_write_beacon(struct queue_entry *entry,
 	 */
 	padding_len = roundup(entry->skb->len, 4) - entry->skb->len;
 	if (padding_len && skb_pad(entry->skb, padding_len)) {
-		ERROR(rt2x00dev, "Failure padding beacon, aborting\n");
+		rt2x00_err(rt2x00dev, "Failure padding beacon, aborting\n");
 		/* skb freed by skb_pad() on failure */
 		entry->skb = NULL;
-		rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, orig_reg);
+		rt2x00mmio_register_write(rt2x00dev, TXRX_CSR9, orig_reg);
 		return;
 	}
 
 	beacon_base = HW_BEACON_OFFSET(entry->entry_idx);
-	rt2x00pci_register_multiwrite(rt2x00dev, beacon_base,
-				      entry_priv->desc, TXINFO_SIZE);
-	rt2x00pci_register_multiwrite(rt2x00dev, beacon_base + TXINFO_SIZE,
-				      entry->skb->data,
-				      entry->skb->len + padding_len);
+	rt2x00mmio_register_multiwrite(rt2x00dev, beacon_base,
+				       entry_priv->desc, TXINFO_SIZE);
+	rt2x00mmio_register_multiwrite(rt2x00dev, beacon_base + TXINFO_SIZE,
+				       entry->skb->data,
+				       entry->skb->len + padding_len);
 
 	/*
 	 * Enable beaconing again.
@@ -2016,10 +2019,10 @@ static void rt61pci_write_beacon(struct queue_entry *entry,
 	 * For Wi-Fi faily generated beacons between participating
 	 * stations. Set TBTT phase adaptive adjustment step to 8us.
 	 */
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR10, 0x00001008);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR10, 0x00001008);
 
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 1);
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR9, reg);
 
 	/*
 	 * Clean up beacon skb.
@@ -2037,21 +2040,21 @@ static void rt61pci_clear_beacon(struct queue_entry *entry)
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
-	rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9, &reg);
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR9, reg);
 
 	/*
 	 * Clear beacon.
 	 */
-	rt2x00pci_register_write(rt2x00dev,
-				 HW_BEACON_OFFSET(entry->entry_idx), 0);
+	rt2x00mmio_register_write(rt2x00dev,
+				  HW_BEACON_OFFSET(entry->entry_idx), 0);
 
 	/*
 	 * Enable beaconing again.
 	 */
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 1);
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR9, reg);
 }
 
 /*
@@ -2089,7 +2092,7 @@ static void rt61pci_fill_rxdone(struct queue_entry *entry,
 				struct rxdone_entry_desc *rxdesc)
 {
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
-	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
 	u32 word0;
 	u32 word1;
 
@@ -2155,7 +2158,7 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
 	struct data_queue *queue;
 	struct queue_entry *entry;
 	struct queue_entry *entry_done;
-	struct queue_entry_priv_pci *entry_priv;
+	struct queue_entry_priv_mmio *entry_priv;
 	struct txdone_entry_desc txdesc;
 	u32 word;
 	u32 reg;
@@ -2173,7 +2176,7 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
 	 * tx ring size for now.
 	 */
 	for (i = 0; i < rt2x00dev->ops->tx->entry_num; i++) {
-		rt2x00pci_register_read(rt2x00dev, STA_CSR4, &reg);
+		rt2x00mmio_register_read(rt2x00dev, STA_CSR4, &reg);
 		if (!rt2x00_get_field32(reg, STA_CSR4_VALID))
 			break;
 
@@ -2207,9 +2210,8 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
 			/* Catch up.
 			 * Just report any entries we missed as failed.
 			 */
-			WARNING(rt2x00dev,
-				"TX status report missed for entry %d\n",
-				entry_done->entry_idx);
+			rt2x00_warn(rt2x00dev, "TX status report missed for entry %d\n",
+				    entry_done->entry_idx);
 
 			rt2x00lib_txdone_noinfo(entry_done, TXDONE_UNKNOWN);
 			entry_done = rt2x00queue_get_entry(queue, Q_INDEX_DONE);
@@ -2260,9 +2262,9 @@ static inline void rt61pci_enable_interrupt(struct rt2x00_dev *rt2x00dev,
 	 */
 	spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-	rt2x00pci_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR, &reg);
 	rt2x00_set_field32(&reg, irq_field, 0);
-	rt2x00pci_register_write(rt2x00dev, INT_MASK_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
 
 	spin_unlock_irq(&rt2x00dev->irqmask_lock);
 }
@@ -2278,9 +2280,9 @@ static void rt61pci_enable_mcu_interrupt(struct rt2x00_dev *rt2x00dev,
 	 */
 	spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-	rt2x00pci_register_read(rt2x00dev, MCU_INT_MASK_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MCU_INT_MASK_CSR, &reg);
 	rt2x00_set_field32(&reg, irq_field, 0);
-	rt2x00pci_register_write(rt2x00dev, MCU_INT_MASK_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, MCU_INT_MASK_CSR, reg);
 
 	spin_unlock_irq(&rt2x00dev->irqmask_lock);
 }
@@ -2304,7 +2306,7 @@ static void rt61pci_tbtt_tasklet(unsigned long data)
 static void rt61pci_rxdone_tasklet(unsigned long data)
 {
 	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
-	if (rt2x00pci_rxdone(rt2x00dev))
+	if (rt2x00mmio_rxdone(rt2x00dev))
 		tasklet_schedule(&rt2x00dev->rxdone_tasklet);
 	else if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
 		rt61pci_enable_interrupt(rt2x00dev, INT_MASK_CSR_RXDONE);
@@ -2314,8 +2316,8 @@ static void rt61pci_autowake_tasklet(unsigned long data)
 {
 	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
 	rt61pci_wakeup(rt2x00dev);
-	rt2x00pci_register_write(rt2x00dev,
-				 M2H_CMD_DONE_CSR, 0xffffffff);
+	rt2x00mmio_register_write(rt2x00dev,
+				  M2H_CMD_DONE_CSR, 0xffffffff);
 	if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
 		rt61pci_enable_mcu_interrupt(rt2x00dev, MCU_INT_MASK_CSR_TWAKEUP);
 }
@@ -2330,11 +2332,11 @@ static irqreturn_t rt61pci_interrupt(int irq, void *dev_instance)
 	 * Get the interrupt sources & saved to local variable.
 	 * Write register value back to clear pending interrupts.
 	 */
-	rt2x00pci_register_read(rt2x00dev, MCU_INT_SOURCE_CSR, &reg_mcu);
-	rt2x00pci_register_write(rt2x00dev, MCU_INT_SOURCE_CSR, reg_mcu);
+	rt2x00mmio_register_read(rt2x00dev, MCU_INT_SOURCE_CSR, &reg_mcu);
+	rt2x00mmio_register_write(rt2x00dev, MCU_INT_SOURCE_CSR, reg_mcu);
 
-	rt2x00pci_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
-	rt2x00pci_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
+	rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
+	rt2x00mmio_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
 
 	if (!reg && !reg_mcu)
 		return IRQ_NONE;
@@ -2371,13 +2373,13 @@ static irqreturn_t rt61pci_interrupt(int irq, void *dev_instance)
 	 */
 	spin_lock(&rt2x00dev->irqmask_lock);
 
-	rt2x00pci_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR, &reg);
 	reg |= mask;
-	rt2x00pci_register_write(rt2x00dev, INT_MASK_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
 
-	rt2x00pci_register_read(rt2x00dev, MCU_INT_MASK_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MCU_INT_MASK_CSR, &reg);
 	reg |= mask_mcu;
-	rt2x00pci_register_write(rt2x00dev, MCU_INT_MASK_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, MCU_INT_MASK_CSR, reg);
 
 	spin_unlock(&rt2x00dev->irqmask_lock);
 
@@ -2395,7 +2397,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	u8 *mac;
 	s8 value;
 
-	rt2x00pci_register_read(rt2x00dev, E2PROM_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, E2PROM_CSR, &reg);
 
 	eeprom.data = rt2x00dev;
 	eeprom.register_read = rt61pci_eepromregister_read;
@@ -2416,7 +2418,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	if (!is_valid_ether_addr(mac)) {
 		eth_random_addr(mac);
-		EEPROM(rt2x00dev, "MAC: %pM\n", mac);
+		rt2x00_eeprom_dbg(rt2x00dev, "MAC: %pM\n", mac);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &word);
@@ -2431,7 +2433,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_HARDWARE_RADIO, 0);
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_RF_TYPE, RF5225);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_ANTENNA, word);
-		EEPROM(rt2x00dev, "Antenna: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "Antenna: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &word);
@@ -2444,7 +2446,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_NIC_CARDBUS_ACCEL, 0);
 		rt2x00_set_field16(&word, EEPROM_NIC_EXTERNAL_LNA_A, 0);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_NIC, word);
-		EEPROM(rt2x00dev, "NIC: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "NIC: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &word);
@@ -2452,7 +2454,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_LED_LED_MODE,
 				   LED_MODE_DEFAULT);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_LED, word);
-		EEPROM(rt2x00dev, "Led: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "Led: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ, &word);
@@ -2460,7 +2462,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_FREQ_OFFSET, 0);
 		rt2x00_set_field16(&word, EEPROM_FREQ_SEQ, 0);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_FREQ, word);
-		EEPROM(rt2x00dev, "Freq: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "Freq: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_BG, &word);
@@ -2468,7 +2470,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_1, 0);
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_2, 0);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_OFFSET_BG, word);
-		EEPROM(rt2x00dev, "RSSI OFFSET BG: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "RSSI OFFSET BG: 0x%04x\n", word);
 	} else {
 		value = rt2x00_get_field16(word, EEPROM_RSSI_OFFSET_BG_1);
 		if (value < -10 || value > 10)
@@ -2484,7 +2486,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_1, 0);
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_2, 0);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_OFFSET_A, word);
-		EEPROM(rt2x00dev, "RSSI OFFSET A: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "RSSI OFFSET A: 0x%04x\n", word);
 	} else {
 		value = rt2x00_get_field16(word, EEPROM_RSSI_OFFSET_A_1);
 		if (value < -10 || value > 10)
@@ -2513,7 +2515,7 @@ static int rt61pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	 * Identify RF chipset.
 	 */
 	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RF_TYPE);
-	rt2x00pci_register_read(rt2x00dev, MAC_CSR0, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MAC_CSR0, &reg);
 	rt2x00_set_chip(rt2x00dev, rt2x00_get_field32(reg, MAC_CSR0_CHIPSET),
 			value, rt2x00_get_field32(reg, MAC_CSR0_REVISION));
 
@@ -2521,7 +2523,7 @@ static int rt61pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	    !rt2x00_rf(rt2x00dev, RF5325) &&
 	    !rt2x00_rf(rt2x00dev, RF2527) &&
 	    !rt2x00_rf(rt2x00dev, RF2529)) {
-		ERROR(rt2x00dev, "Invalid RF chipset detected.\n");
+		rt2x00_err(rt2x00dev, "Invalid RF chipset detected\n");
 		return -ENODEV;
 	}
 
@@ -2838,7 +2840,7 @@ static int rt61pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Disable power saving.
 	 */
-	rt2x00pci_register_write(rt2x00dev, SOFT_RESET_CSR, 0x00000007);
+	rt2x00mmio_register_write(rt2x00dev, SOFT_RESET_CSR, 0x00000007);
 
 	/*
 	 * Allocate eeprom data.
@@ -2855,9 +2857,9 @@ static int rt61pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 	 * Enable rfkill polling by setting GPIO direction of the
 	 * rfkill switch GPIO pin correctly.
 	 */
-	rt2x00pci_register_read(rt2x00dev, MAC_CSR13, &reg);
+	rt2x00mmio_register_read(rt2x00dev, MAC_CSR13, &reg);
 	rt2x00_set_field32(&reg, MAC_CSR13_DIR5, 1);
-	rt2x00pci_register_write(rt2x00dev, MAC_CSR13, reg);
+	rt2x00mmio_register_write(rt2x00dev, MAC_CSR13, reg);
 
 	/*
 	 * Initialize hw specifications.
@@ -2927,25 +2929,25 @@ static int rt61pci_conf_tx(struct ieee80211_hw *hw,
 	field.bit_offset = (queue_idx & 1) * 16;
 	field.bit_mask = 0xffff << field.bit_offset;
 
-	rt2x00pci_register_read(rt2x00dev, offset, &reg);
+	rt2x00mmio_register_read(rt2x00dev, offset, &reg);
 	rt2x00_set_field32(&reg, field, queue->txop);
-	rt2x00pci_register_write(rt2x00dev, offset, reg);
+	rt2x00mmio_register_write(rt2x00dev, offset, reg);
 
 	/* Update WMM registers */
 	field.bit_offset = queue_idx * 4;
 	field.bit_mask = 0xf << field.bit_offset;
 
-	rt2x00pci_register_read(rt2x00dev, AIFSN_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, AIFSN_CSR, &reg);
 	rt2x00_set_field32(&reg, field, queue->aifs);
-	rt2x00pci_register_write(rt2x00dev, AIFSN_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, AIFSN_CSR, reg);
 
-	rt2x00pci_register_read(rt2x00dev, CWMIN_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CWMIN_CSR, &reg);
 	rt2x00_set_field32(&reg, field, queue->cw_min);
-	rt2x00pci_register_write(rt2x00dev, CWMIN_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, CWMIN_CSR, reg);
 
-	rt2x00pci_register_read(rt2x00dev, CWMAX_CSR, &reg);
+	rt2x00mmio_register_read(rt2x00dev, CWMAX_CSR, &reg);
 	rt2x00_set_field32(&reg, field, queue->cw_max);
-	rt2x00pci_register_write(rt2x00dev, CWMAX_CSR, reg);
+	rt2x00mmio_register_write(rt2x00dev, CWMAX_CSR, reg);
 
 	return 0;
 }
@@ -2956,9 +2958,9 @@ static u64 rt61pci_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	u64 tsf;
 	u32 reg;
 
-	rt2x00pci_register_read(rt2x00dev, TXRX_CSR13, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR13, &reg);
 	tsf = (u64) rt2x00_get_field32(reg, TXRX_CSR13_HIGH_TSFTIMER) << 32;
-	rt2x00pci_register_read(rt2x00dev, TXRX_CSR12, &reg);
+	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR12, &reg);
 	tsf |= rt2x00_get_field32(reg, TXRX_CSR12_LOW_TSFTIMER);
 
 	return tsf;
@@ -2997,8 +2999,8 @@ static const struct rt2x00lib_ops rt61pci_rt2x00_ops = {
 	.get_firmware_name	= rt61pci_get_firmware_name,
 	.check_firmware		= rt61pci_check_firmware,
 	.load_firmware		= rt61pci_load_firmware,
-	.initialize		= rt2x00pci_initialize,
-	.uninitialize		= rt2x00pci_uninitialize,
+	.initialize		= rt2x00mmio_initialize,
+	.uninitialize		= rt2x00mmio_uninitialize,
 	.get_entry_state	= rt61pci_get_entry_state,
 	.clear_entry		= rt61pci_clear_entry,
 	.set_device_state	= rt61pci_set_device_state,
@@ -3009,7 +3011,7 @@ static const struct rt2x00lib_ops rt61pci_rt2x00_ops = {
 	.start_queue		= rt61pci_start_queue,
 	.kick_queue		= rt61pci_kick_queue,
 	.stop_queue		= rt61pci_stop_queue,
-	.flush_queue		= rt2x00pci_flush_queue,
+	.flush_queue		= rt2x00mmio_flush_queue,
 	.write_tx_desc		= rt61pci_write_tx_desc,
 	.write_beacon		= rt61pci_write_beacon,
 	.clear_beacon		= rt61pci_clear_beacon,
@@ -3027,21 +3029,21 @@ static const struct data_queue_desc rt61pci_queue_rx = {
 	.entry_num		= 32,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= RXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci),
+	.priv_size		= sizeof(struct queue_entry_priv_mmio),
 };
 
 static const struct data_queue_desc rt61pci_queue_tx = {
 	.entry_num		= 32,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci),
+	.priv_size		= sizeof(struct queue_entry_priv_mmio),
 };
 
 static const struct data_queue_desc rt61pci_queue_bcn = {
 	.entry_num		= 4,
 	.data_size		= 0, /* No DMA required for beacons */
 	.desc_size		= TXINFO_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci),
+	.priv_size		= sizeof(struct queue_entry_priv_mmio),
 };
 
 static const struct rt2x00_ops rt61pci_ops = {
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index 24eec66e9fd2..377e09bb0b81 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -739,7 +739,7 @@ static void rt73usb_config_lna_gain(struct rt2x00_dev *rt2x00dev,
 	u16 eeprom;
 	short lna_gain = 0;
 
-	if (libconf->conf->channel->band == IEEE80211_BAND_2GHZ) {
+	if (libconf->conf->chandef.chan->band == IEEE80211_BAND_2GHZ) {
 		if (test_bit(CAPABILITY_EXTERNAL_LNA_BG, &rt2x00dev->cap_flags))
 			lna_gain += 14;
 
@@ -1122,7 +1122,7 @@ static int rt73usb_load_firmware(struct rt2x00_dev *rt2x00dev,
 	}
 
 	if (!reg) {
-		ERROR(rt2x00dev, "Unstable hardware.\n");
+		rt2x00_err(rt2x00dev, "Unstable hardware\n");
 		return -EBUSY;
 	}
 
@@ -1139,7 +1139,7 @@ static int rt73usb_load_firmware(struct rt2x00_dev *rt2x00dev,
 					     0, USB_MODE_FIRMWARE,
 					     REGISTER_TIMEOUT_FIRMWARE);
 	if (status < 0) {
-		ERROR(rt2x00dev, "Failed to write Firmware to device.\n");
+		rt2x00_err(rt2x00dev, "Failed to write Firmware to device\n");
 		return status;
 	}
 
@@ -1305,7 +1305,7 @@ static int rt73usb_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 		udelay(REGISTER_BUSY_DELAY);
 	}
 
-	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
+	rt2x00_err(rt2x00dev, "BBP register access failed, aborting\n");
 	return -EACCES;
 }
 
@@ -1443,8 +1443,8 @@ static int rt73usb_set_device_state(struct rt2x00_dev *rt2x00dev,
 	}
 
 	if (unlikely(retval))
-		ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
-		      state, retval);
+		rt2x00_err(rt2x00dev, "Device failed to enter state %d (%d)\n",
+			   state, retval);
 
 	return retval;
 }
@@ -1567,7 +1567,7 @@ static void rt73usb_write_beacon(struct queue_entry *entry,
 	 */
 	padding_len = roundup(entry->skb->len, 4) - entry->skb->len;
 	if (padding_len && skb_pad(entry->skb, padding_len)) {
-		ERROR(rt2x00dev, "Failure padding beacon, aborting\n");
+		rt2x00_err(rt2x00dev, "Failure padding beacon, aborting\n");
 		/* skb freed by skb_pad() on failure */
 		entry->skb = NULL;
 		rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, orig_reg);
@@ -1771,7 +1771,7 @@ static int rt73usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	if (!is_valid_ether_addr(mac)) {
 		eth_random_addr(mac);
-		EEPROM(rt2x00dev, "MAC: %pM\n", mac);
+		rt2x00_eeprom_dbg(rt2x00dev, "MAC: %pM\n", mac);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &word);
@@ -1786,14 +1786,14 @@ static int rt73usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_HARDWARE_RADIO, 0);
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_RF_TYPE, RF5226);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_ANTENNA, word);
-		EEPROM(rt2x00dev, "Antenna: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "Antenna: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &word);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_NIC_EXTERNAL_LNA, 0);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_NIC, word);
-		EEPROM(rt2x00dev, "NIC: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "NIC: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &word);
@@ -1809,7 +1809,7 @@ static int rt73usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_LED_LED_MODE,
 				   LED_MODE_DEFAULT);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_LED, word);
-		EEPROM(rt2x00dev, "Led: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "Led: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ, &word);
@@ -1817,7 +1817,7 @@ static int rt73usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_FREQ_OFFSET, 0);
 		rt2x00_set_field16(&word, EEPROM_FREQ_SEQ, 0);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_FREQ, word);
-		EEPROM(rt2x00dev, "Freq: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "Freq: 0x%04x\n", word);
 	}
 
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_BG, &word);
@@ -1825,7 +1825,7 @@ static int rt73usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_1, 0);
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_2, 0);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_OFFSET_BG, word);
-		EEPROM(rt2x00dev, "RSSI OFFSET BG: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "RSSI OFFSET BG: 0x%04x\n", word);
 	} else {
 		value = rt2x00_get_field16(word, EEPROM_RSSI_OFFSET_BG_1);
 		if (value < -10 || value > 10)
@@ -1841,7 +1841,7 @@ static int rt73usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_1, 0);
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_2, 0);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_OFFSET_A, word);
-		EEPROM(rt2x00dev, "RSSI OFFSET A: 0x%04x\n", word);
+		rt2x00_eeprom_dbg(rt2x00dev, "RSSI OFFSET A: 0x%04x\n", word);
 	} else {
 		value = rt2x00_get_field16(word, EEPROM_RSSI_OFFSET_A_1);
 		if (value < -10 || value > 10)
@@ -1875,7 +1875,7 @@ static int rt73usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
 			value, rt2x00_get_field32(reg, MAC_CSR0_REVISION));
 
 	if (!rt2x00_rt(rt2x00dev, RT2573) || (rt2x00_rev(rt2x00dev) == 0)) {
-		ERROR(rt2x00dev, "Invalid RT chipset detected.\n");
+		rt2x00_err(rt2x00dev, "Invalid RT chipset detected\n");
 		return -ENODEV;
 	}
 
@@ -1883,7 +1883,7 @@ static int rt73usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	    !rt2x00_rf(rt2x00dev, RF2528) &&
 	    !rt2x00_rf(rt2x00dev, RF5225) &&
 	    !rt2x00_rf(rt2x00dev, RF2527)) {
-		ERROR(rt2x00dev, "Invalid RF chipset detected.\n");
+		rt2x00_err(rt2x00dev, "Invalid RF chipset detected\n");
 		return -ENODEV;
 	}
 
diff --git a/drivers/net/wireless/rtl818x/rtl8180/dev.c b/drivers/net/wireless/rtl818x/rtl8180/dev.c
index 1b3c2843221d..91a04e2b8ece 100644
--- a/drivers/net/wireless/rtl818x/rtl8180/dev.c
+++ b/drivers/net/wireless/rtl818x/rtl8180/dev.c
@@ -147,8 +147,8 @@ static void rtl8180_handle_rx(struct ieee80211_hw *dev)
 				signal = priv->rf->calc_rssi(agc, sq);
 			}
 			rx_status.signal = signal;
-			rx_status.freq = dev->conf.channel->center_freq;
-			rx_status.band = dev->conf.channel->band;
+			rx_status.freq = dev->conf.chandef.chan->center_freq;
+			rx_status.band = dev->conf.chandef.chan->band;
 			rx_status.mactime = le64_to_cpu(entry->tsft);
 			rx_status.flag |= RX_FLAG_MACTIME_START;
 			if (flags & RTL818X_RX_DESC_FLAG_CRC32_ERR)
diff --git a/drivers/net/wireless/rtl818x/rtl8180/grf5101.c b/drivers/net/wireless/rtl818x/rtl8180/grf5101.c
index 5ee7589dd546..077ff92cc139 100644
--- a/drivers/net/wireless/rtl818x/rtl8180/grf5101.c
+++ b/drivers/net/wireless/rtl818x/rtl8180/grf5101.c
@@ -82,7 +82,8 @@ static void grf5101_rf_set_channel(struct ieee80211_hw *dev,
 				   struct ieee80211_conf *conf)
 {
 	struct rtl8180_priv *priv = dev->priv;
-	int channel = ieee80211_frequency_to_channel(conf->channel->center_freq);
+	int channel =
+		ieee80211_frequency_to_channel(conf->chandef.chan->center_freq);
 	u32 txpw = priv->channels[channel - 1].hw_value & 0xFF;
 	u32 chan = channel - 1;
 
diff --git a/drivers/net/wireless/rtl818x/rtl8180/max2820.c b/drivers/net/wireless/rtl818x/rtl8180/max2820.c
index 667b3363d437..4715000c94dd 100644
--- a/drivers/net/wireless/rtl818x/rtl8180/max2820.c
+++ b/drivers/net/wireless/rtl818x/rtl8180/max2820.c
@@ -95,7 +95,7 @@ static void max2820_rf_set_channel(struct ieee80211_hw *dev,
 {
 	struct rtl8180_priv *priv = dev->priv;
 	int channel = conf ?
-		ieee80211_frequency_to_channel(conf->channel->center_freq) : 1;
+		ieee80211_frequency_to_channel(conf->chandef.chan->center_freq) : 1;
 	unsigned int chan_idx = channel - 1;
 	u32 txpw = priv->channels[chan_idx].hw_value & 0xFF;
 	u32 chan = max2820_chan[chan_idx];
diff --git a/drivers/net/wireless/rtl818x/rtl8180/rtl8225.c b/drivers/net/wireless/rtl818x/rtl8180/rtl8225.c
index 7c4574ba9d75..cc2a5412c1f0 100644
--- a/drivers/net/wireless/rtl818x/rtl8180/rtl8225.c
+++ b/drivers/net/wireless/rtl818x/rtl8180/rtl8225.c
@@ -719,7 +719,8 @@ static void rtl8225_rf_set_channel(struct ieee80211_hw *dev,
 				   struct ieee80211_conf *conf)
 {
 	struct rtl8180_priv *priv = dev->priv;
-	int chan = ieee80211_frequency_to_channel(conf->channel->center_freq);
+	int chan =
+		ieee80211_frequency_to_channel(conf->chandef.chan->center_freq);
 
 	if (priv->rf->init == rtl8225_rf_init)
 		rtl8225_rf_set_tx_power(dev, chan);
diff --git a/drivers/net/wireless/rtl818x/rtl8180/sa2400.c b/drivers/net/wireless/rtl818x/rtl8180/sa2400.c
index 44771a6286af..b3ec40f6bd23 100644
--- a/drivers/net/wireless/rtl818x/rtl8180/sa2400.c
+++ b/drivers/net/wireless/rtl818x/rtl8180/sa2400.c
@@ -105,7 +105,8 @@ static void sa2400_rf_set_channel(struct ieee80211_hw *dev,
 				  struct ieee80211_conf *conf)
 {
 	struct rtl8180_priv *priv = dev->priv;
-	int channel = ieee80211_frequency_to_channel(conf->channel->center_freq);
+	int channel =
+		ieee80211_frequency_to_channel(conf->chandef.chan->center_freq);
 	u32 txpw = priv->channels[channel - 1].hw_value & 0xFF;
 	u32 chan = sa2400_chan[channel - 1];
 
diff --git a/drivers/net/wireless/rtl818x/rtl8187/dev.c b/drivers/net/wireless/rtl818x/rtl8187/dev.c
index 4574bd213705..f49220e234b0 100644
--- a/drivers/net/wireless/rtl818x/rtl8187/dev.c
+++ b/drivers/net/wireless/rtl818x/rtl8187/dev.c
@@ -379,8 +379,8 @@ static void rtl8187_rx_cb(struct urb *urb)
 	rate = (flags >> 20) & 0xF;
 	skb_trim(skb, flags & 0x0FFF);
 	rx_status.rate_idx = rate;
-	rx_status.freq = dev->conf.channel->center_freq;
-	rx_status.band = dev->conf.channel->band;
+	rx_status.freq = dev->conf.chandef.chan->center_freq;
+	rx_status.band = dev->conf.chandef.chan->band;
 	rx_status.flag |= RX_FLAG_MACTIME_START;
 	if (flags & RTL818X_RX_DESC_FLAG_CRC32_ERR)
 		rx_status.flag |= RX_FLAG_FAILED_FCS_CRC;
diff --git a/drivers/net/wireless/rtl818x/rtl8187/rtl8225.c b/drivers/net/wireless/rtl818x/rtl8187/rtl8225.c
index 908903f721f5..f0bf35fedbaf 100644
--- a/drivers/net/wireless/rtl818x/rtl8187/rtl8225.c
+++ b/drivers/net/wireless/rtl818x/rtl8187/rtl8225.c
@@ -905,7 +905,8 @@ static void rtl8225_rf_set_channel(struct ieee80211_hw *dev,
 				   struct ieee80211_conf *conf)
 {
 	struct rtl8187_priv *priv = dev->priv;
-	int chan = ieee80211_frequency_to_channel(conf->channel->center_freq);
+	int chan =
+		ieee80211_frequency_to_channel(conf->chandef.chan->center_freq);
 
 	if (priv->rf->init == rtl8225_rf_init)
 		rtl8225_rf_set_tx_power(dev, chan);
diff --git a/drivers/net/wireless/rtlwifi/Kconfig b/drivers/net/wireless/rtlwifi/Kconfig
index b6aa0c40658f..7253de3d8c66 100644
--- a/drivers/net/wireless/rtlwifi/Kconfig
+++ b/drivers/net/wireless/rtlwifi/Kconfig
@@ -55,6 +55,15 @@ config RTL8723AE
 
 	If you choose to build it as a module, it will be called rtl8723ae
 
+config RTL8188EE
+	tristate "Realtek RTL8188EE Wireless Network Adapter"
+	depends on RTLWIFI && PCI
+	---help---
+	This is the driver for Realtek RTL8188EE 802.11n PCIe
+	wireless network adapters.
+
+	If you choose to build it as a module, it will be called rtl8188ee
+
 config RTL8192CU
 	tristate "Realtek RTL8192CU/RTL8188CU USB Wireless Network Adapter"
 	depends on RTLWIFI && USB
diff --git a/drivers/net/wireless/rtlwifi/Makefile b/drivers/net/wireless/rtlwifi/Makefile
index 3b1cbac741e3..ff02b874f8d8 100644
--- a/drivers/net/wireless/rtlwifi/Makefile
+++ b/drivers/net/wireless/rtlwifi/Makefile
@@ -26,5 +26,6 @@ obj-$(CONFIG_RTL8192CU)		+= rtl8192cu/
 obj-$(CONFIG_RTL8192SE)		+= rtl8192se/
 obj-$(CONFIG_RTL8192DE)		+= rtl8192de/
 obj-$(CONFIG_RTL8723AE)		+= rtl8723ae/
+obj-$(CONFIG_RTL8188EE)		+= rtl8188ee/
 
 ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/rtlwifi/base.c b/drivers/net/wireless/rtlwifi/base.c
index 99c5cea3fe21..af59dd5718e1 100644
--- a/drivers/net/wireless/rtlwifi/base.c
+++ b/drivers/net/wireless/rtlwifi/base.c
@@ -54,7 +54,8 @@
  *5) frame process functions
  *6) IOT functions
  *7) sysfs functions
- *8) ...
+ *8) vif functions
+ *9) ...
  */
 
 /*********************************************************
@@ -198,34 +199,46 @@ static void _rtl_init_hw_ht_capab(struct ieee80211_hw *hw,
 
 	ht_cap->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
 
-	/*
-	 *hw->wiphy->bands[IEEE80211_BAND_2GHZ]
+	/*hw->wiphy->bands[IEEE80211_BAND_2GHZ]
 	 *base on ant_num
 	 *rx_mask: RX mask
-	 *if rx_ant =1 rx_mask[0]=0xff;==>MCS0-MCS7
-	 *if rx_ant =2 rx_mask[1]=0xff;==>MCS8-MCS15
-	 *if rx_ant >=3 rx_mask[2]=0xff;
-	 *if BW_40 rx_mask[4]=0x01;
+	 *if rx_ant = 1 rx_mask[0]= 0xff;==>MCS0-MCS7
+	 *if rx_ant = 2 rx_mask[1]= 0xff;==>MCS8-MCS15
+	 *if rx_ant >= 3 rx_mask[2]= 0xff;
+	 *if BW_40 rx_mask[4]= 0x01;
 	 *highest supported RX rate
 	 */
-	if (get_rf_type(rtlphy) == RF_1T2R || get_rf_type(rtlphy) == RF_2T2R) {
+	if (rtlpriv->dm.supp_phymode_switch) {
 
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "1T2R or 2T2R\n");
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_EMERG,
+			 "Support phy mode switch\n");
 
 		ht_cap->mcs.rx_mask[0] = 0xFF;
 		ht_cap->mcs.rx_mask[1] = 0xFF;
 		ht_cap->mcs.rx_mask[4] = 0x01;
 
 		ht_cap->mcs.rx_highest = cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS15);
-	} else if (get_rf_type(rtlphy) == RF_1T1R) {
-
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "1T1R\n");
-
-		ht_cap->mcs.rx_mask[0] = 0xFF;
-		ht_cap->mcs.rx_mask[1] = 0x00;
-		ht_cap->mcs.rx_mask[4] = 0x01;
-
-		ht_cap->mcs.rx_highest = cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS7);
+	} else {
+		if (get_rf_type(rtlphy) == RF_1T2R ||
+		    get_rf_type(rtlphy) == RF_2T2R) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+				 "1T2R or 2T2R\n");
+			ht_cap->mcs.rx_mask[0] = 0xFF;
+			ht_cap->mcs.rx_mask[1] = 0xFF;
+			ht_cap->mcs.rx_mask[4] = 0x01;
+
+			ht_cap->mcs.rx_highest =
+				 cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS15);
+		} else if (get_rf_type(rtlphy) == RF_1T1R) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "1T1R\n");
+
+			ht_cap->mcs.rx_mask[0] = 0xFF;
+			ht_cap->mcs.rx_mask[1] = 0x00;
+			ht_cap->mcs.rx_mask[4] = 0x01;
+
+			ht_cap->mcs.rx_highest =
+				 cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS7);
+		}
 	}
 }
 
@@ -311,6 +324,8 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 	    IEEE80211_HW_AMPDU_AGGREGATION |
 	    IEEE80211_HW_CONNECTION_MONITOR |
 	    /* IEEE80211_HW_SUPPORTS_CQM_RSSI | */
+	    IEEE80211_HW_CONNECTION_MONITOR |
+	    IEEE80211_HW_MFP_CAPABLE |
 	    IEEE80211_HW_REPORTS_TX_ACK_STATUS | 0;
 
 	/* swlps or hwlps has been set in diff chip in init_sw_vars */
@@ -323,8 +338,12 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 	hw->wiphy->interface_modes =
 	    BIT(NL80211_IFTYPE_AP) |
 	    BIT(NL80211_IFTYPE_STATION) |
-	    BIT(NL80211_IFTYPE_ADHOC);
+	    BIT(NL80211_IFTYPE_ADHOC) |
+	    BIT(NL80211_IFTYPE_MESH_POINT) |
+	    BIT(NL80211_IFTYPE_P2P_CLIENT) |
+	    BIT(NL80211_IFTYPE_P2P_GO);
 
+	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN;
 	hw->wiphy->rts_threshold = 2347;
 
 	hw->queues = AC_MAX;
@@ -354,9 +373,10 @@ static void _rtl_init_deferred_work(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	/* <1> timer */
-	init_timer(&rtlpriv->works.watchdog_timer);
 	setup_timer(&rtlpriv->works.watchdog_timer,
 		    rtl_watch_dog_timer_callback, (unsigned long)hw);
+	setup_timer(&rtlpriv->works.dualmac_easyconcurrent_retrytimer,
+		    rtl_easy_concurrent_retrytimer_callback, (unsigned long)hw);
 
 	/* <2> work queue */
 	rtlpriv->works.hw = hw;
@@ -369,6 +389,8 @@ static void _rtl_init_deferred_work(struct ieee80211_hw *hw)
 			  (void *)rtl_swlps_wq_callback);
 	INIT_DELAYED_WORK(&rtlpriv->works.ps_rfon_wq,
 			  (void *)rtl_swlps_rfon_wq_callback);
+	INIT_DELAYED_WORK(&rtlpriv->works.fwevt_wq,
+			  (void *)rtl_fwevt_wq_callback);
 
 }
 
@@ -382,6 +404,7 @@ void rtl_deinit_deferred_work(struct ieee80211_hw *hw)
 	cancel_delayed_work(&rtlpriv->works.ips_nic_off_wq);
 	cancel_delayed_work(&rtlpriv->works.ps_work);
 	cancel_delayed_work(&rtlpriv->works.ps_rfon_wq);
+	cancel_delayed_work(&rtlpriv->works.fwevt_wq);
 }
 
 void rtl_init_rfkill(struct ieee80211_hw *hw)
@@ -436,12 +459,6 @@ int rtl_init_core(struct ieee80211_hw *hw)
 	if (rtl_regd_init(hw, rtl_reg_notifier)) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "REGD init failed\n");
 		return 1;
-	} else {
-		/* CRDA regd hint must after init CRDA */
-		if (regulatory_hint(hw->wiphy, rtlpriv->regd.alpha2)) {
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-				 "regulatory_hint fail\n");
-		}
 	}
 
 	/* <4> locks */
@@ -449,15 +466,25 @@ int rtl_init_core(struct ieee80211_hw *hw)
 	mutex_init(&rtlpriv->locks.ps_mutex);
 	spin_lock_init(&rtlpriv->locks.ips_lock);
 	spin_lock_init(&rtlpriv->locks.irq_th_lock);
+	spin_lock_init(&rtlpriv->locks.irq_pci_lock);
+	spin_lock_init(&rtlpriv->locks.tx_lock);
 	spin_lock_init(&rtlpriv->locks.h2c_lock);
 	spin_lock_init(&rtlpriv->locks.rf_ps_lock);
 	spin_lock_init(&rtlpriv->locks.rf_lock);
 	spin_lock_init(&rtlpriv->locks.waitq_lock);
+	spin_lock_init(&rtlpriv->locks.entry_list_lock);
+	spin_lock_init(&rtlpriv->locks.fw_ps_lock);
 	spin_lock_init(&rtlpriv->locks.cck_and_rw_pagea_lock);
+	spin_lock_init(&rtlpriv->locks.check_sendpkt_lock);
+	spin_lock_init(&rtlpriv->locks.fw_ps_lock);
+	spin_lock_init(&rtlpriv->locks.lps_lock);
+
+	/* <5> init list */
+	INIT_LIST_HEAD(&rtlpriv->entry_list);
 
 	rtlmac->link_state = MAC80211_NOLINK;
 
-	/* <5> init deferred work */
+	/* <6> init deferred work */
 	_rtl_init_deferred_work(hw);
 
 	return 0;
@@ -523,7 +550,8 @@ static void _rtl_query_shortgi(struct ieee80211_hw *hw,
 	if (mac->opmode == NL80211_IFTYPE_STATION)
 		bw_40 = mac->bw_40;
 	else if (mac->opmode == NL80211_IFTYPE_AP ||
-		 mac->opmode == NL80211_IFTYPE_ADHOC)
+		 mac->opmode == NL80211_IFTYPE_ADHOC ||
+		 mac->opmode == NL80211_IFTYPE_MESH_POINT)
 		bw_40 = sta->bandwidth >= IEEE80211_STA_RX_BW_40;
 
 	if (bw_40 && sgi_40)
@@ -578,23 +606,26 @@ static void _rtl_txrate_selectmode(struct ieee80211_hw *hw,
 	if (!tcb_desc->disable_ratefallback || !tcb_desc->use_driver_rate) {
 		if (mac->opmode == NL80211_IFTYPE_STATION) {
 			tcb_desc->ratr_index = 0;
-		} else if (mac->opmode == NL80211_IFTYPE_ADHOC) {
+		} else if (mac->opmode == NL80211_IFTYPE_ADHOC ||
+			   mac->opmode == NL80211_IFTYPE_MESH_POINT) {
 			if (tcb_desc->multicast || tcb_desc->broadcast) {
 				tcb_desc->hw_rate =
 				    rtlpriv->cfg->maps[RTL_RC_CCK_RATE2M];
 				tcb_desc->use_driver_rate = 1;
+				tcb_desc->ratr_index = RATR_INX_WIRELESS_MC;
 			} else {
-				/* TODO */
+				tcb_desc->ratr_index = ratr_index;
 			}
-			tcb_desc->ratr_index = ratr_index;
 		} else if (mac->opmode == NL80211_IFTYPE_AP) {
 			tcb_desc->ratr_index = ratr_index;
 		}
 	}
 
 	if (rtlpriv->dm.useramask) {
-		/* TODO we will differentiate adhoc and station futrue  */
-		if (mac->opmode == NL80211_IFTYPE_STATION) {
+		tcb_desc->ratr_index = ratr_index;
+		/* TODO we will differentiate adhoc and station future  */
+		if (mac->opmode == NL80211_IFTYPE_STATION ||
+		    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
 			tcb_desc->mac_id = 0;
 
 			if (mac->mode == WIRELESS_MODE_N_24G)
@@ -608,7 +639,7 @@ static void _rtl_txrate_selectmode(struct ieee80211_hw *hw,
 			else if (mac->mode & WIRELESS_MODE_A)
 				tcb_desc->ratr_index = RATR_INX_WIRELESS_G;
 		} else if (mac->opmode == NL80211_IFTYPE_AP ||
-			mac->opmode == NL80211_IFTYPE_ADHOC) {
+			   mac->opmode == NL80211_IFTYPE_ADHOC) {
 			if (NULL != sta) {
 				if (sta->aid > 0)
 					tcb_desc->mac_id = sta->aid + 1;
@@ -619,7 +650,6 @@ static void _rtl_txrate_selectmode(struct ieee80211_hw *hw,
 			}
 		}
 	}
-
 }
 
 static void _rtl_query_bandwidth_mode(struct ieee80211_hw *hw,
@@ -633,7 +663,8 @@ static void _rtl_query_bandwidth_mode(struct ieee80211_hw *hw,
 	if (!sta)
 		return;
 	if (mac->opmode == NL80211_IFTYPE_AP ||
-	    mac->opmode == NL80211_IFTYPE_ADHOC) {
+	    mac->opmode == NL80211_IFTYPE_ADHOC ||
+	    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
 		if (sta->bandwidth == IEEE80211_STA_RX_BW_20)
 			return;
 	} else if (mac->opmode == NL80211_IFTYPE_STATION) {
@@ -691,7 +722,7 @@ int rtlwifi_rate_mapping(struct ieee80211_hw *hw,
 	int rate_idx;
 
 	if (false == isht) {
-		if (IEEE80211_BAND_2GHZ == hw->conf.channel->band) {
+		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
 			switch (desc_rate) {
 			case DESC92_RATE1M:
 				rate_idx = 0;
@@ -834,8 +865,8 @@ bool rtl_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb)
 	if (rtlpriv->dm.supp_phymode_switch &&
 	    mac->link_state < MAC80211_LINKED &&
 	    (ieee80211_is_auth(fc) || ieee80211_is_probe_req(fc))) {
-		if (rtlpriv->cfg->ops->check_switch_to_dmdp)
-			rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
+		if (rtlpriv->cfg->ops->chk_switch_dmdp)
+			rtlpriv->cfg->ops->chk_switch_dmdp(hw);
 	}
 	if (ieee80211_is_auth(fc)) {
 		RT_TRACE(rtlpriv, COMP_SEND, DBG_DMESG, "MAC80211_LINKING\n");
@@ -924,6 +955,56 @@ void rtl_get_tcb_desc(struct ieee80211_hw *hw,
 }
 EXPORT_SYMBOL(rtl_get_tcb_desc);
 
+static bool addbareq_rx(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ieee80211_sta *sta = NULL;
+	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
+	struct rtl_sta_info *sta_entry = NULL;
+	struct ieee80211_mgmt *mgmt = (void *)skb->data;
+	u16 capab = 0, tid = 0;
+	struct rtl_tid_data *tid_data;
+	struct sk_buff *skb_delba = NULL;
+	struct ieee80211_rx_status rx_status = { 0 };
+
+	rcu_read_lock();
+	sta = rtl_find_sta(hw, hdr->addr3);
+	if (sta == NULL) {
+		RT_TRACE(rtlpriv, (COMP_SEND | COMP_RECV), DBG_EMERG,
+			 "sta is NULL\n");
+		rcu_read_unlock();
+		return true;
+	}
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	if (!sta_entry) {
+		rcu_read_unlock();
+		return true;
+	}
+	capab = le16_to_cpu(mgmt->u.action.u.addba_req.capab);
+	tid = (capab & IEEE80211_ADDBA_PARAM_TID_MASK) >> 2;
+	tid_data = &sta_entry->tids[tid];
+	if (tid_data->agg.rx_agg_state == RTL_RX_AGG_START) {
+		skb_delba = rtl_make_del_ba(hw, hdr->addr2, hdr->addr3, tid);
+		if (skb_delba) {
+			rx_status.freq = hw->conf.chandef.chan->center_freq;
+			rx_status.band = hw->conf.chandef.chan->band;
+			rx_status.flag |= RX_FLAG_DECRYPTED;
+			rx_status.flag |= RX_FLAG_MACTIME_END;
+			rx_status.rate_idx = 0;
+			rx_status.signal = 50 + 10;
+			memcpy(IEEE80211_SKB_RXCB(skb_delba), &rx_status,
+			       sizeof(rx_status));
+			RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG,
+				      "fake del\n", skb_delba->data,
+				      skb_delba->len);
+			ieee80211_rx_irqsafe(hw, skb_delba);
+		}
+	}
+	rcu_read_unlock();
+	return false;
+}
+
 bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 {
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
@@ -948,6 +1029,11 @@ bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 			RT_TRACE(rtlpriv, (COMP_SEND | COMP_RECV), DBG_DMESG,
 				 "%s ACT_ADDBAREQ From :%pM\n",
 				 is_tx ? "Tx" : "Rx", hdr->addr2);
+			RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, "req\n",
+				      skb->data, skb->len);
+			if (!is_tx)
+				if (addbareq_rx(hw, skb))
+					return true;
 			break;
 		case ACT_ADDBARSP:
 			RT_TRACE(rtlpriv, (COMP_SEND | COMP_RECV), DBG_DMESG,
@@ -1003,8 +1089,9 @@ u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 					 is_tx ? "Tx" : "Rx");
 
 				if (is_tx) {
+					rtlpriv->enter_ps = false;
 					schedule_work(&rtlpriv->
-						      works.lps_leave_work);
+						      works.lps_change_work);
 					ppsc->last_delaylps_stamp_jiffies =
 					    jiffies;
 				}
@@ -1014,7 +1101,8 @@ u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 		}
 	} else if (ETH_P_ARP == ether_type) {
 		if (is_tx) {
-			schedule_work(&rtlpriv->works.lps_leave_work);
+			rtlpriv->enter_ps = false;
+			schedule_work(&rtlpriv->works.lps_change_work);
 			ppsc->last_delaylps_stamp_jiffies = jiffies;
 		}
 
@@ -1024,7 +1112,8 @@ u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 			 "802.1X %s EAPOL pkt!!\n", is_tx ? "Tx" : "Rx");
 
 		if (is_tx) {
-			schedule_work(&rtlpriv->works.lps_leave_work);
+			rtlpriv->enter_ps = false;
+			schedule_work(&rtlpriv->works.lps_change_work);
 			ppsc->last_delaylps_stamp_jiffies = jiffies;
 		}
 
@@ -1101,6 +1190,58 @@ int rtl_tx_agg_stop(struct ieee80211_hw *hw,
 	return 0;
 }
 
+int rtl_rx_agg_start(struct ieee80211_hw *hw,
+		     struct ieee80211_sta *sta, u16 tid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tid_data *tid_data;
+	struct rtl_sta_info *sta_entry = NULL;
+
+	if (sta == NULL)
+		return -EINVAL;
+
+	if (unlikely(tid >= MAX_TID_COUNT))
+		return -EINVAL;
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	if (!sta_entry)
+		return -ENXIO;
+	tid_data = &sta_entry->tids[tid];
+
+	RT_TRACE(rtlpriv, COMP_RECV, DBG_DMESG,
+		 "on ra = %pM tid = %d seq:%d\n", sta->addr, tid,
+		 tid_data->seq_number);
+
+	tid_data->agg.rx_agg_state = RTL_RX_AGG_START;
+	return 0;
+}
+
+int rtl_rx_agg_stop(struct ieee80211_hw *hw,
+		    struct ieee80211_sta *sta, u16 tid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_sta_info *sta_entry = NULL;
+
+	if (sta == NULL)
+		return -EINVAL;
+
+	if (!sta->addr) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "ra = NULL\n");
+		return -EINVAL;
+	}
+
+	RT_TRACE(rtlpriv, COMP_SEND, DBG_DMESG,
+		 "on ra = %pM tid = %d\n", sta->addr, tid);
+
+	if (unlikely(tid >= MAX_TID_COUNT))
+		return -EINVAL;
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	sta_entry->tids[tid].agg.rx_agg_state = RTL_RX_AGG_STOP;
+
+	return 0;
+}
+
 int rtl_tx_agg_oper(struct ieee80211_hw *hw,
 		struct ieee80211_sta *sta, u16 tid)
 {
@@ -1132,6 +1273,34 @@ int rtl_tx_agg_oper(struct ieee80211_hw *hw,
  * wq & timer callback functions
  *
  *********************************************************/
+/* this function is used for roaming */
+void rtl_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+
+	if (rtlpriv->mac80211.opmode != NL80211_IFTYPE_STATION)
+		return;
+
+	if (rtlpriv->mac80211.link_state < MAC80211_LINKED)
+		return;
+
+	/* check if this really is a beacon */
+	if (!ieee80211_is_beacon(hdr->frame_control) &&
+	    !ieee80211_is_probe_resp(hdr->frame_control))
+		return;
+
+	/* min. beacon length + FCS_LEN */
+	if (skb->len <= 40 + FCS_LEN)
+		return;
+
+	/* and only beacons from the associated BSSID, please */
+	if (compare_ether_addr(hdr->addr3, rtlpriv->mac80211.bssid))
+		return;
+
+	rtlpriv->link_info.bcn_rx_inperiod++;
+}
+
 void rtl_watchdog_wq_callback(void *data)
 {
 	struct rtl_works *rtlworks = container_of_dwork_rtl(data,
@@ -1142,6 +1311,8 @@ void rtl_watchdog_wq_callback(void *data)
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	bool busytraffic = false;
+	bool tx_busy_traffic = false;
+	bool rx_busy_traffic = false;
 	bool higher_busytraffic = false;
 	bool higher_busyrxtraffic = false;
 	u8 idx, tid;
@@ -1151,7 +1322,6 @@ void rtl_watchdog_wq_callback(void *data)
 	u32 aver_tx_cnt_inperiod = 0;
 	u32 aver_tidtx_inperiod[MAX_TID_COUNT] = {0};
 	u32 tidtx_inp4eriod[MAX_TID_COUNT] = {0};
-	bool enter_ps = false;
 
 	if (is_hal_stop(rtlhal))
 		return;
@@ -1191,8 +1361,13 @@ void rtl_watchdog_wq_callback(void *data)
 		aver_tx_cnt_inperiod = tx_cnt_inp4eriod / 4;
 
 		/* (2) check traffic busy */
-		if (aver_rx_cnt_inperiod > 100 || aver_tx_cnt_inperiod > 100)
+		if (aver_rx_cnt_inperiod > 100 || aver_tx_cnt_inperiod > 100) {
 			busytraffic = true;
+			if (aver_rx_cnt_inperiod > aver_tx_cnt_inperiod)
+				rx_busy_traffic = true;
+			else
+				tx_busy_traffic = false;
+		}
 
 		/* Higher Tx/Rx data. */
 		if (aver_rx_cnt_inperiod > 4000 ||
@@ -1228,15 +1403,12 @@ void rtl_watchdog_wq_callback(void *data)
 		if (((rtlpriv->link_info.num_rx_inperiod +
 		      rtlpriv->link_info.num_tx_inperiod) > 8) ||
 		    (rtlpriv->link_info.num_rx_inperiod > 2))
-			enter_ps = false;
+			rtlpriv->enter_ps = true;
 		else
-			enter_ps = true;
+			rtlpriv->enter_ps = false;
 
 		/* LeisurePS only work in infra mode. */
-		if (enter_ps)
-			rtl_lps_enter(hw);
-		else
-			rtl_lps_leave(hw);
+		schedule_work(&rtlpriv->works.lps_change_work);
 	}
 
 	rtlpriv->link_info.num_rx_inperiod = 0;
@@ -1246,10 +1418,37 @@ void rtl_watchdog_wq_callback(void *data)
 
 	rtlpriv->link_info.busytraffic = busytraffic;
 	rtlpriv->link_info.higher_busytraffic = higher_busytraffic;
+	rtlpriv->link_info.rx_busy_traffic = rx_busy_traffic;
+	rtlpriv->link_info.tx_busy_traffic = tx_busy_traffic;
 	rtlpriv->link_info.higher_busyrxtraffic = higher_busyrxtraffic;
 
 	/* <3> DM */
 	rtlpriv->cfg->ops->dm_watchdog(hw);
+
+	/* <4> roaming */
+	if (mac->link_state == MAC80211_LINKED &&
+	    mac->opmode == NL80211_IFTYPE_STATION) {
+		if ((rtlpriv->link_info.bcn_rx_inperiod +
+		     rtlpriv->link_info.num_rx_inperiod) == 0) {
+			rtlpriv->link_info.roam_times++;
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_DMESG,
+				 "AP off for %d s\n",
+				 (rtlpriv->link_info.roam_times * 2));
+
+			/* if we can't recv beacon for 6s, we should
+			 * reconnect this AP
+			 */
+			if (rtlpriv->link_info.roam_times >= 3) {
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+					 "AP off, try to reconnect now\n");
+				rtlpriv->link_info.roam_times = 0;
+				ieee80211_connection_loss(rtlpriv->mac80211.vif);
+			}
+		} else {
+			rtlpriv->link_info.roam_times = 0;
+		}
+	}
+	rtlpriv->link_info.bcn_rx_inperiod = 0;
 }
 
 void rtl_watch_dog_timer_callback(unsigned long data)
@@ -1264,6 +1463,28 @@ void rtl_watch_dog_timer_callback(unsigned long data)
 		  jiffies + MSECS(RTL_WATCH_DOG_TIME));
 }
 
+void rtl_fwevt_wq_callback(void *data)
+{
+	struct rtl_works *rtlworks =
+		container_of_dwork_rtl(data, struct rtl_works, fwevt_wq);
+	struct ieee80211_hw *hw = rtlworks->hw;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->cfg->ops->c2h_command_handle(hw);
+}
+
+void rtl_easy_concurrent_retrytimer_callback(unsigned long data)
+{
+	struct ieee80211_hw *hw = (struct ieee80211_hw *)data;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_priv *buddy_priv = rtlpriv->buddy_priv;
+
+	if (buddy_priv == NULL)
+		return;
+
+	rtlpriv->cfg->ops->dualmac_easy_concurrent(hw);
+}
+
 /*********************************************************
  *
  * frame process functions
@@ -1334,14 +1555,16 @@ static struct sk_buff *rtl_make_smps_action(struct ieee80211_hw *hw,
 }
 
 int rtl_send_smps_action(struct ieee80211_hw *hw,
-		struct ieee80211_sta *sta, u8 *da, u8 *bssid,
+		struct ieee80211_sta *sta,
 		enum ieee80211_smps_mode smps)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct sk_buff *skb = rtl_make_smps_action(hw, smps, da, bssid);
+	struct sk_buff *skb = NULL;
 	struct rtl_tcb_desc tcb_desc;
+	u8 bssid[ETH_ALEN] = {0};
+
 	memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
 
 	if (rtlpriv->mac80211.act_scanning)
@@ -1356,21 +1579,67 @@ int rtl_send_smps_action(struct ieee80211_hw *hw,
 	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
 		goto err_free;
 
+	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP)
+		memcpy(bssid, rtlpriv->efuse.dev_addr, ETH_ALEN);
+	else
+		memcpy(bssid, rtlpriv->mac80211.bssid, ETH_ALEN);
+
+	skb = rtl_make_smps_action(hw, smps, sta->addr, bssid);
 	/* this is a type = mgmt * stype = action frame */
 	if (skb) {
 		struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 		struct rtl_sta_info *sta_entry =
 			(struct rtl_sta_info *) sta->drv_priv;
 		sta_entry->mimo_ps = smps;
-		rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0);
 
 		info->control.rates[0].idx = 0;
-		info->band = hw->conf.channel->band;
+		info->band = hw->conf.chandef.chan->band;
 		rtlpriv->intf_ops->adapter_tx(hw, sta, skb, &tcb_desc);
 	}
+	return 1;
+
 err_free:
 	return 0;
 }
+EXPORT_SYMBOL(rtl_send_smps_action);
+
+/* There seem to be issues in mac80211 regarding when del ba frames can be
+ * received. As a work around, we make a fake del_ba if we receive a ba_req;
+ * however, rx_agg was opened to let mac80211 release some ba related
+ * resources. This del_ba is for tx only.
+ */
+struct sk_buff *rtl_make_del_ba(struct ieee80211_hw *hw,
+				u8 *sa, u8 *bssid, u16 tid)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct sk_buff *skb;
+	struct ieee80211_mgmt *action_frame;
+	u16 params;
+
+	/* 27 = header + category + action + smps mode */
+	skb = dev_alloc_skb(34 + hw->extra_tx_headroom);
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, hw->extra_tx_headroom);
+	action_frame = (void *)skb_put(skb, 34);
+	memset(action_frame, 0, 34);
+	memcpy(action_frame->sa, sa, ETH_ALEN);
+	memcpy(action_frame->da, rtlefuse->dev_addr, ETH_ALEN);
+	memcpy(action_frame->bssid, bssid, ETH_ALEN);
+	action_frame->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+						  IEEE80211_STYPE_ACTION);
+	action_frame->u.action.category = WLAN_CATEGORY_BACK;
+	action_frame->u.action.u.delba.action_code = WLAN_ACTION_DELBA;
+	params = (u16)(1 << 11);	/* bit 11 initiator */
+	params |= (u16)(tid << 12);		/* bit 15:12 TID number */
+
+	action_frame->u.action.u.delba.params = cpu_to_le16(params);
+	action_frame->u.action.u.delba.reason_code =
+		cpu_to_le16(WLAN_REASON_QSTA_TIMEOUT);
+
+	return skb;
+}
 
 /*********************************************************
  *
@@ -1587,11 +1856,17 @@ MODULE_AUTHOR("Larry Finger	<Larry.FInger@lwfinger.net>");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Realtek 802.11n PCI wireless core");
 
+struct rtl_global_var global_var = {};
+
 static int __init rtl_core_module_init(void)
 {
 	if (rtl_rate_control_register())
 		pr_err("Unable to register rtl_rc, use default RC !!\n");
 
+	/* init some global vars */
+	INIT_LIST_HEAD(&global_var.glb_priv_list);
+	spin_lock_init(&global_var.glb_list_lock);
+
 	return 0;
 }
 
diff --git a/drivers/net/wireless/rtlwifi/base.h b/drivers/net/wireless/rtlwifi/base.h
index 5a8c80e259f7..8576bc34b032 100644
--- a/drivers/net/wireless/rtlwifi/base.h
+++ b/drivers/net/wireless/rtlwifi/base.h
@@ -113,6 +113,7 @@ void rtl_init_rx_config(struct ieee80211_hw *hw);
 void rtl_init_rfkill(struct ieee80211_hw *hw);
 void rtl_deinit_rfkill(struct ieee80211_hw *hw);
 
+void rtl_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb);
 void rtl_watch_dog_timer_callback(unsigned long data);
 void rtl_deinit_deferred_work(struct ieee80211_hw *hw);
 
@@ -126,7 +127,12 @@ int rtl_tx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
 		    u16 tid);
 int rtl_tx_agg_oper(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
 		    u16 tid);
+int rtl_rx_agg_start(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
+		     u16 tid);
+int rtl_rx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
+		    u16 tid);
 void rtl_watchdog_wq_callback(void *data);
+void rtl_fwevt_wq_callback(void *data);
 
 void rtl_get_tcb_desc(struct ieee80211_hw *hw,
 		      struct ieee80211_tx_info *info,
@@ -134,14 +140,18 @@ void rtl_get_tcb_desc(struct ieee80211_hw *hw,
 		      struct sk_buff *skb, struct rtl_tcb_desc *tcb_desc);
 
 int rtl_send_smps_action(struct ieee80211_hw *hw,
-		struct ieee80211_sta *sta, u8 *da, u8 *bssid,
-		enum ieee80211_smps_mode smps);
+			 struct ieee80211_sta *sta,
+			 enum ieee80211_smps_mode smps);
 u8 *rtl_find_ie(u8 *data, unsigned int len, u8 ie);
 void rtl_recognize_peer(struct ieee80211_hw *hw, u8 *data, unsigned int len);
 u8 rtl_tid_to_ac(u8 tid);
 extern struct attribute_group rtl_attribute_group;
+void rtl_easy_concurrent_retrytimer_callback(unsigned long data);
+extern struct rtl_global_var global_var;
 int rtlwifi_rate_mapping(struct ieee80211_hw *hw,
 			 bool isht, u8 desc_rate, bool first_ampdu);
 bool rtl_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb);
+struct sk_buff *rtl_make_del_ba(struct ieee80211_hw *hw,
+				u8 *sa, u8 *bssid, u16 tid);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/core.c b/drivers/net/wireless/rtlwifi/core.c
index d3ce9fbef00e..ee84844be008 100644
--- a/drivers/net/wireless/rtlwifi/core.c
+++ b/drivers/net/wireless/rtlwifi/core.c
@@ -104,9 +104,12 @@ static void rtl_op_stop(struct ieee80211_hw *hw)
 	if (is_hal_stop(rtlhal))
 		return;
 
+	/* here is must, because adhoc do stop and start,
+	 * but stop with RFOFF may cause something wrong,
+	 * like adhoc TP
+	 */
 	if (unlikely(ppsc->rfpwr_state == ERFOFF)) {
 		rtl_ips_nic_on(hw);
-		mdelay(1);
 	}
 
 	mutex_lock(&rtlpriv->locks.conf_mutex);
@@ -167,7 +170,11 @@ static int rtl_op_add_interface(struct ieee80211_hw *hw,
 	rtl_ips_nic_on(hw);
 
 	mutex_lock(&rtlpriv->locks.conf_mutex);
-	switch (vif->type) {
+
+	switch (ieee80211_vif_type_p2p(vif)) {
+	case NL80211_IFTYPE_P2P_CLIENT:
+		mac->p2p = P2P_ROLE_CLIENT;
+		/*fall through*/
 	case NL80211_IFTYPE_STATION:
 		if (mac->beacon_enabled == 1) {
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
@@ -192,6 +199,9 @@ static int rtl_op_add_interface(struct ieee80211_hw *hw,
 				(u8 *) (&mac->basic_rates));
 
 		break;
+	case NL80211_IFTYPE_P2P_GO:
+		mac->p2p = P2P_ROLE_GO;
+		/*fall through*/
 	case NL80211_IFTYPE_AP:
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
 			 "NL80211_IFTYPE_AP\n");
@@ -205,6 +215,19 @@ static int rtl_op_add_interface(struct ieee80211_hw *hw,
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
 				(u8 *) (&mac->basic_rates));
 		break;
+	case NL80211_IFTYPE_MESH_POINT:
+		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
+			 "NL80211_IFTYPE_MESH_POINT\n");
+
+		mac->link_state = MAC80211_LINKED;
+		rtlpriv->cfg->ops->set_bcn_reg(hw);
+		if (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G)
+			mac->basic_rates = 0xfff;
+		else
+			mac->basic_rates = 0xff0;
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
+				(u8 *)(&mac->basic_rates));
+		break;
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "operation mode %d is not supported!\n", vif->type);
@@ -212,6 +235,13 @@ static int rtl_op_add_interface(struct ieee80211_hw *hw,
 		goto out;
 	}
 
+	if (mac->p2p) {
+		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
+			 "p2p role %x\n", vif->type);
+		mac->basic_rates = 0xff0;/*disable cck rate for p2p*/
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
+				(u8 *)(&mac->basic_rates));
+	}
 	mac->vif = vif;
 	mac->opmode = vif->type;
 	rtlpriv->cfg->ops->set_network_type(hw, vif->type);
@@ -232,9 +262,9 @@ static void rtl_op_remove_interface(struct ieee80211_hw *hw,
 	mutex_lock(&rtlpriv->locks.conf_mutex);
 
 	/* Free beacon resources */
-	if ((mac->opmode == NL80211_IFTYPE_AP) ||
-	    (mac->opmode == NL80211_IFTYPE_ADHOC) ||
-	    (mac->opmode == NL80211_IFTYPE_MESH_POINT)) {
+	if ((vif->type == NL80211_IFTYPE_AP) ||
+	    (vif->type == NL80211_IFTYPE_ADHOC) ||
+	    (vif->type == NL80211_IFTYPE_MESH_POINT)) {
 		if (mac->beacon_enabled == 1) {
 			mac->beacon_enabled = 0;
 			rtlpriv->cfg->ops->update_interrupt_mask(hw, 0,
@@ -247,6 +277,7 @@ static void rtl_op_remove_interface(struct ieee80211_hw *hw,
 	 *Note: We assume NL80211_IFTYPE_UNSPECIFIED as
 	 *NO LINK for our hardware.
 	 */
+	mac->p2p = 0;
 	mac->vif = NULL;
 	mac->link_state = MAC80211_NOLINK;
 	memset(mac->bssid, 0, 6);
@@ -256,6 +287,22 @@ static void rtl_op_remove_interface(struct ieee80211_hw *hw,
 	mutex_unlock(&rtlpriv->locks.conf_mutex);
 }
 
+static int rtl_op_change_interface(struct ieee80211_hw *hw,
+				      struct ieee80211_vif *vif,
+				      enum nl80211_iftype new_type, bool p2p)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int ret;
+	rtl_op_remove_interface(hw, vif);
+
+	vif->type = new_type;
+	vif->p2p = p2p;
+	ret = rtl_op_add_interface(hw, vif);
+	RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
+		 "p2p %x\n", p2p);
+	return ret;
+}
+
 static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -264,6 +311,9 @@ static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	struct ieee80211_conf *conf = &hw->conf;
 
+	if (mac->skip_scan)
+		return 1;
+
 	mutex_lock(&rtlpriv->locks.conf_mutex);
 	if (changed & IEEE80211_CONF_CHANGE_LISTEN_INTERVAL) {	/*BIT(2)*/
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
@@ -320,9 +370,19 @@ static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 	}
 
 	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
-		struct ieee80211_channel *channel = hw->conf.channel;
+		struct ieee80211_channel *channel = hw->conf.chandef.chan;
 		u8 wide_chan = (u8) channel->hw_value;
 
+		if (mac->act_scanning)
+			mac->n_channels++;
+
+		if (rtlpriv->dm.supp_phymode_switch &&
+		    mac->link_state < MAC80211_LINKED &&
+		    !mac->act_scanning) {
+			if (rtlpriv->cfg->ops->chk_switch_dmdp)
+				rtlpriv->cfg->ops->chk_switch_dmdp(hw);
+		}
+
 		/*
 		 *because we should back channel to
 		 *current_network.chan in in scanning,
@@ -332,7 +392,7 @@ static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 		 *info for cisco1253 bw20, so we modify
 		 *it here based on UPPER & LOWER
 		 */
-		switch (hw->conf.channel_type) {
+		switch (cfg80211_get_chandef_type(&hw->conf.chandef)) {
 		case NL80211_CHAN_HT20:
 		case NL80211_CHAN_NO_HT:
 			/* SC */
@@ -373,13 +433,13 @@ static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 		if (wide_chan <= 0)
 			wide_chan = 1;
 
-		/* In scanning, before we go offchannel we may send a ps=1 null
-		 * to AP, and then we may send a ps = 0 null to AP quickly, but
-		 * first null may have caused AP to put lots of packet to hw tx
-		 * buffer. These packets must be tx'd before we go off channel
-		 * so we must delay more time to let AP flush these packets
-		 * before going offchannel, or dis-association or delete BA will
-		 * happen by AP
+		/* In scanning, before we go offchannel we may send a ps = 1
+		 * null to AP, and then we may send a ps = 0 null to AP quickly,
+		 * but first null may have caused AP to put lots of packet to
+		 * hw tx buffer. These packets must be tx'd before we go off
+		 * channel so we must delay more time to let AP flush these
+		 * packets before going offchannel, or dis-association or
+		 * delete BA will be caused by AP
 		 */
 		if (rtlpriv->mac80211.offchan_delay) {
 			rtlpriv->mac80211.offchan_delay = false;
@@ -390,7 +450,7 @@ static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 		rtlpriv->cfg->ops->switch_channel(hw);
 		rtlpriv->cfg->ops->set_channel_access(hw);
 		rtlpriv->cfg->ops->set_bw_mode(hw,
-					       hw->conf.channel_type);
+				cfg80211_get_chandef_type(&hw->conf.chandef));
 	}
 
 	mutex_unlock(&rtlpriv->locks.conf_mutex);
@@ -441,7 +501,8 @@ static void rtl_op_configure_filter(struct ieee80211_hw *hw,
 	 * and nolink check bssid is set in set network_type */
 	if ((changed_flags & FIF_BCN_PRBRESP_PROMISC) &&
 		(mac->link_state >= MAC80211_LINKED)) {
-		if (mac->opmode != NL80211_IFTYPE_AP) {
+		if (mac->opmode != NL80211_IFTYPE_AP &&
+		    mac->opmode != NL80211_IFTYPE_MESH_POINT) {
 			if (*new_flags & FIF_BCN_PRBRESP_PROMISC) {
 				rtlpriv->cfg->ops->set_chk_bssid(hw, false);
 			} else {
@@ -481,32 +542,43 @@ static int rtl_op_sta_add(struct ieee80211_hw *hw,
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_sta_info *sta_entry;
 
 	if (sta) {
 		sta_entry = (struct rtl_sta_info *) sta->drv_priv;
+		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+		list_add_tail(&sta_entry->list, &rtlpriv->entry_list);
+		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
 		if (rtlhal->current_bandtype == BAND_ON_2_4G) {
 			sta_entry->wireless_mode = WIRELESS_MODE_G;
 			if (sta->supp_rates[0] <= 0xf)
 				sta_entry->wireless_mode = WIRELESS_MODE_B;
-			if (sta->ht_cap.ht_supported)
+			if (sta->ht_cap.ht_supported == true)
 				sta_entry->wireless_mode = WIRELESS_MODE_N_24G;
+
+			if (vif->type == NL80211_IFTYPE_ADHOC)
+				sta_entry->wireless_mode = WIRELESS_MODE_G;
 		} else if (rtlhal->current_bandtype == BAND_ON_5G) {
 			sta_entry->wireless_mode = WIRELESS_MODE_A;
-			if (sta->ht_cap.ht_supported)
+			if (sta->ht_cap.ht_supported == true)
 				sta_entry->wireless_mode = WIRELESS_MODE_N_24G;
-		}
 
-		/* I found some times mac80211 give wrong supp_rates for adhoc*/
-		if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_ADHOC)
-			sta_entry->wireless_mode = WIRELESS_MODE_G;
+			if (vif->type == NL80211_IFTYPE_ADHOC)
+				sta_entry->wireless_mode = WIRELESS_MODE_A;
+		}
+		/*disable cck rate for p2p*/
+		if (mac->p2p)
+			sta->supp_rates[0] &= 0xfffffff0;
 
+		memcpy(sta_entry->mac_addr, sta->addr, ETH_ALEN);
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_DMESG,
 			 "Add sta addr is %pM\n", sta->addr);
 		rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0);
 	}
 	return 0;
 }
+
 static int rtl_op_sta_remove(struct ieee80211_hw *hw,
 				struct ieee80211_vif *vif,
 				struct ieee80211_sta *sta)
@@ -519,9 +591,14 @@ static int rtl_op_sta_remove(struct ieee80211_hw *hw,
 		sta_entry = (struct rtl_sta_info *) sta->drv_priv;
 		sta_entry->wireless_mode = 0;
 		sta_entry->ratr_index = 0;
+
+		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+		list_del(&sta_entry->list);
+		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
 	}
 	return 0;
 }
+
 static int _rtl_get_hal_qnum(u16 queue)
 {
 	int qnum;
@@ -547,8 +624,8 @@ static int _rtl_get_hal_qnum(u16 queue)
 }
 
 /*
- *for mac80211 VO=0, VI=1, BE=2, BK=3
- *for rtl819x  BE=0, BK=1, VI=2, VO=3
+ *for mac80211 VO = 0, VI = 1, BE = 2, BK = 3
+ *for rtl819x  BE = 0, BK = 1, VI = 2, VO = 3
  */
 static int rtl_op_conf_tx(struct ieee80211_hw *hw,
 		   struct ieee80211_vif *vif, u16 queue,
@@ -630,6 +707,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 	/*TODO: reference to enum ieee80211_bss_change */
 	if (changed & BSS_CHANGED_ASSOC) {
 		if (bss_conf->assoc) {
+			struct ieee80211_sta *sta = NULL;
 			/* we should reset all sec info & cam
 			 * before set cam after linked, we should not
 			 * reset in disassoc, that will cause tkip->wep
@@ -647,23 +725,39 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 
 			if (rtlpriv->cfg->ops->linked_set_reg)
 				rtlpriv->cfg->ops->linked_set_reg(hw);
-			if (mac->opmode == NL80211_IFTYPE_STATION && sta)
+			rcu_read_lock();
+			sta = ieee80211_find_sta(vif, (u8 *)bss_conf->bssid);
+
+			if (vif->type == NL80211_IFTYPE_STATION && sta)
 				rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0);
+			RT_TRACE(rtlpriv, COMP_EASY_CONCURRENT, DBG_LOUD,
+				 "send PS STATIC frame\n");
+			if (rtlpriv->dm.supp_phymode_switch) {
+				if (sta->ht_cap.ht_supported)
+					rtl_send_smps_action(hw, sta,
+						 IEEE80211_SMPS_STATIC);
+			}
+			rcu_read_unlock();
+
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_DMESG,
 				 "BSS_CHANGED_ASSOC\n");
 		} else {
-			if (mac->link_state == MAC80211_LINKED)
-				rtl_lps_leave(hw);
+			if (mac->link_state == MAC80211_LINKED) {
+				rtlpriv->enter_ps = false;
+				schedule_work(&rtlpriv->works.lps_change_work);
+			}
 
+			if (ppsc->p2p_ps_info.p2p_ps_mode > P2P_PS_NONE)
+				rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
 			mac->link_state = MAC80211_NOLINK;
 			memset(mac->bssid, 0, 6);
-
-			/* reset sec info */
-			rtl_cam_reset_sec_info(hw);
-
-			rtl_cam_reset_all_entry(hw);
 			mac->vendor = PEER_UNKNOWN;
 
+			if (rtlpriv->dm.supp_phymode_switch) {
+				if (rtlpriv->cfg->ops->chk_switch_dmdp)
+					rtlpriv->cfg->ops->chk_switch_dmdp(hw);
+			}
+
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_DMESG,
 				 "BSS_CHANGED_UN_ASSOC\n");
 		}
@@ -778,7 +872,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 		}
 
 		if (changed & BSS_CHANGED_BASIC_RATES) {
-			/* for 5G must << RATE_6M_INDEX=4,
+			/* for 5G must << RATE_6M_INDEX = 4,
 			 * because 5G have no cck rate*/
 			if (rtlhal->current_bandtype == BAND_ON_5G)
 				basic_rates = sta->supp_rates[1] << 4;
@@ -815,6 +909,9 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 				ppsc->report_linked = false;
 			}
 		}
+		if (rtlpriv->cfg->ops->bt_wifi_media_status_notify)
+			rtlpriv->cfg->ops->bt_wifi_media_status_notify(hw,
+							 ppsc->report_linked);
 	}
 
 out:
@@ -885,7 +982,6 @@ static int rtl_op_ampdu_action(struct ieee80211_hw *hw,
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_TRACE,
 			 "IEEE80211_AMPDU_TX_STOP: TID:%d\n", tid);
 		return rtl_tx_agg_stop(hw, sta, tid);
-		break;
 	case IEEE80211_AMPDU_TX_OPERATIONAL:
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_TRACE,
 			 "IEEE80211_AMPDU_TX_OPERATIONAL:TID:%d\n", tid);
@@ -894,11 +990,11 @@ static int rtl_op_ampdu_action(struct ieee80211_hw *hw,
 	case IEEE80211_AMPDU_RX_START:
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_TRACE,
 			 "IEEE80211_AMPDU_RX_START:TID:%d\n", tid);
-		break;
+		return rtl_rx_agg_start(hw, sta, tid);
 	case IEEE80211_AMPDU_RX_STOP:
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_TRACE,
 			 "IEEE80211_AMPDU_RX_STOP:TID:%d\n", tid);
-		break;
+		return rtl_rx_agg_stop(hw, sta, tid);
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "IEEE80211_AMPDU_ERR!!!!:\n");
@@ -912,12 +1008,20 @@ static void rtl_op_sw_scan_start(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 
-	mac->act_scanning = true;
-
 	RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD, "\n");
+	mac->act_scanning = true;
+	if (rtlpriv->link_info.higher_busytraffic) {
+		mac->skip_scan = true;
+		return;
+	}
 
+	if (rtlpriv->dm.supp_phymode_switch) {
+		if (rtlpriv->cfg->ops->chk_switch_dmdp)
+			rtlpriv->cfg->ops->chk_switch_dmdp(hw);
+	}
 	if (mac->link_state == MAC80211_LINKED) {
-		rtl_lps_leave(hw);
+		rtlpriv->enter_ps = false;
+		schedule_work(&rtlpriv->works.lps_change_work);
 		mac->link_state = MAC80211_LINKED_SCANNING;
 	} else {
 		rtl_ips_nic_on(hw);
@@ -937,6 +1041,16 @@ static void rtl_op_sw_scan_complete(struct ieee80211_hw *hw)
 
 	RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD, "\n");
 	mac->act_scanning = false;
+	mac->skip_scan = false;
+	if (rtlpriv->link_info.higher_busytraffic)
+		return;
+
+	/*p2p will use 1/6/11 to scan */
+	if (mac->n_channels == 3)
+		mac->p2p_in_use = true;
+	else
+		mac->p2p_in_use = false;
+	mac->n_channels = 0;
 	/* Dual mac */
 	rtlpriv->rtlhal.load_imrandiqk_setting_for2g = false;
 
@@ -970,6 +1084,11 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 			 "not open hw encryption\n");
 		return -ENOSPC;	/*User disabled HW-crypto */
 	}
+	/* To support IBSS, use sw-crypto for GTK */
+	if (((vif->type == NL80211_IFTYPE_ADHOC) ||
+	     (vif->type == NL80211_IFTYPE_MESH_POINT)) &&
+	      !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE))
+		return -ENOSPC;
 	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
 		 "%s hardware based encryption for keyidx: %d, mac: %pM\n",
 		 cmd == SET_KEY ? "Using" : "Disabling", key->keyidx,
@@ -996,6 +1115,14 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		key_type = AESCCMP_ENCRYPTION;
 		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "alg:CCMP\n");
 		break;
+	case WLAN_CIPHER_SUITE_AES_CMAC:
+		/*HW doesn't support CMAC encryption, use software CMAC */
+		key_type = AESCMAC_ENCRYPTION;
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "alg:CMAC\n");
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+			 "HW don't support CMAC encryption, use software CMAC\n");
+		err = -EOPNOTSUPP;
+		goto out_unlock;
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "alg_err:%x!!!!\n",
 			 key->cipher);
@@ -1017,13 +1144,14 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	 * 1) wep only: is just for wep enc, in this condition
 	 * rtlpriv->sec.pairwise_enc_algorithm == NO_ENCRYPTION
 	 * will be true & enable_hw_sec will be set when wep
-	 * ke setting.
+	 * key setting.
 	 * 2) wep(group) + AES(pairwise): some AP like cisco
 	 * may use it, in this condition enable_hw_sec will not
 	 * be set when wep key setting */
 	/* we must reset sec_info after lingked before set key,
 	 * or some flag will be wrong*/
-	if (mac->opmode == NL80211_IFTYPE_AP) {
+	if (vif->type == NL80211_IFTYPE_AP ||
+	    vif->type == NL80211_IFTYPE_MESH_POINT) {
 		if (!group_key || key_type == WEP40_ENCRYPTION ||
 			key_type == WEP104_ENCRYPTION) {
 			if (group_key)
@@ -1098,12 +1226,16 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		key->hw_key_idx = key_idx;
 		if (key_type == TKIP_ENCRYPTION)
 			key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
+		/*use software CCMP encryption for management frames (MFP) */
+		if (key_type == AESCCMP_ENCRYPTION)
+			key->flags |= IEEE80211_KEY_FLAG_SW_MGMT_TX;
 		break;
 	case DISABLE_KEY:
 		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
 			 "disable key delete one entry\n");
 		/*set local buf about wep key. */
-		if (mac->opmode == NL80211_IFTYPE_AP) {
+		if (vif->type == NL80211_IFTYPE_AP ||
+		    vif->type == NL80211_IFTYPE_MESH_POINT) {
 			if (sta)
 				rtl_cam_del_entry(hw, sta->addr);
 		}
@@ -1163,10 +1295,10 @@ static void rtl_op_rfkill_poll(struct ieee80211_hw *hw)
 }
 
 /* this function is called by mac80211 to flush tx buffer
- * before switch channle or power save, or tx buffer packet
+ * before switch channel or power save, or tx buffer packet
  * maybe send after offchannel or rf sleep, this may cause
  * dis-association by AP */
-static void rtl_op_flush(struct ieee80211_hw *hw, bool drop)
+static void rtl_op_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
@@ -1180,6 +1312,7 @@ const struct ieee80211_ops rtl_ops = {
 	.tx = rtl_op_tx,
 	.add_interface = rtl_op_add_interface,
 	.remove_interface = rtl_op_remove_interface,
+	.change_interface = rtl_op_change_interface,
 	.config = rtl_op_config,
 	.configure_filter = rtl_op_configure_filter,
 	.sta_add = rtl_op_sta_add,
diff --git a/drivers/net/wireless/rtlwifi/debug.c b/drivers/net/wireless/rtlwifi/debug.c
index bdda9b2fffe1..7d52d3d7769f 100644
--- a/drivers/net/wireless/rtlwifi/debug.c
+++ b/drivers/net/wireless/rtlwifi/debug.c
@@ -41,7 +41,10 @@ void rtl_dbgp_flag_init(struct ieee80211_hw *hw)
 	    COMP_BEACON | COMP_RATE | COMP_RXDESC | COMP_DIG | COMP_TXAGC |
 	    COMP_POWER | COMP_POWER_TRACKING | COMP_BB_POWERSAVING | COMP_SWAS |
 	    COMP_RF | COMP_TURBO | COMP_RATR | COMP_CMD |
-	    COMP_EFUSE | COMP_QOS | COMP_MAC80211 | COMP_REGD | COMP_CHAN;
+	    COMP_EFUSE | COMP_QOS | COMP_MAC80211 | COMP_REGD | COMP_CHAN |
+	    COMP_EASY_CONCURRENT | COMP_EFUSE | COMP_QOS | COMP_MAC80211 |
+	    COMP_REGD | COMP_CHAN | COMP_BT_COEXIST;
+
 
 	for (i = 0; i < DBGP_TYPE_MAX; i++)
 		rtlpriv->dbg.dbgp_type[i] = 0;
diff --git a/drivers/net/wireless/rtlwifi/debug.h b/drivers/net/wireless/rtlwifi/debug.h
index fd3269f47685..6d669364e3d9 100644
--- a/drivers/net/wireless/rtlwifi/debug.h
+++ b/drivers/net/wireless/rtlwifi/debug.h
@@ -115,11 +115,11 @@
 /* Define EEPROM and EFUSE  check module bit*/
 #define EEPROM_W			BIT(0)
 #define EFUSE_PG			BIT(1)
-#define EFUSE_READ_ALL		BIT(2)
+#define EFUSE_READ_ALL			BIT(2)
 
 /* Define init check for module bit*/
 #define	INIT_EEPROM			BIT(0)
-#define	INIT_TxPower		BIT(1)
+#define	INIT_TXPOWER			BIT(1)
 #define	INIT_IQK			BIT(2)
 #define	INIT_RF				BIT(3)
 
@@ -135,6 +135,15 @@
 #define	PHY_TXPWR			BIT(8)
 #define	PHY_PWRDIFF			BIT(9)
 
+/* Define Dynamic Mechanism check module bit --> FDM */
+#define WA_IOT				BIT(0)
+#define DM_PWDB				BIT(1)
+#define DM_MONITOR			BIT(2)
+#define DM_DIG				BIT(3)
+#define DM_EDCA_TURBO			BIT(4)
+
+#define DM_PWDB				BIT(1)
+
 enum dbgp_flag_e {
 	FQOS = 0,
 	FTX = 1,
diff --git a/drivers/net/wireless/rtlwifi/efuse.c b/drivers/net/wireless/rtlwifi/efuse.c
index 8e2f9afb125a..9e3894178e77 100644
--- a/drivers/net/wireless/rtlwifi/efuse.c
+++ b/drivers/net/wireless/rtlwifi/efuse.c
@@ -35,8 +35,6 @@ static const u8 MAX_PGPKT_SIZE = 9;
 static const u8 PGPKT_DATA_SIZE = 8;
 static const int EFUSE_MAX_SIZE = 512;
 
-static const u8 EFUSE_OOB_PROTECT_BYTES = 15;
-
 static const struct efuse_map RTL8712_SDIO_EFUSE_TABLE[] = {
 	{0, 0, 0, 2},
 	{0, 1, 0, 2},
@@ -240,6 +238,7 @@ void read_efuse(struct ieee80211_hw *hw, u16 _offset, u16 _size_byte, u8 *pbuf)
 	u8 rtemp8[1];
 	u16 efuse_addr = 0;
 	u8 offset, wren;
+	u8 u1temp = 0;
 	u16 i;
 	u16 j;
 	const u16 efuse_max_section =
@@ -285,10 +284,31 @@ void read_efuse(struct ieee80211_hw *hw, u16 _offset, u16 _size_byte, u8 *pbuf)
 	}
 
 	while ((*rtemp8 != 0xFF) && (efuse_addr < efuse_len)) {
-		offset = ((*rtemp8 >> 4) & 0x0f);
+		/*  Check PG header for section num.  */
+		if ((*rtemp8 & 0x1F) == 0x0F) {/* extended header */
+			u1temp = ((*rtemp8 & 0xE0) >> 5);
+			read_efuse_byte(hw, efuse_addr, rtemp8);
 
-		if (offset < efuse_max_section) {
+			if ((*rtemp8 & 0x0F) == 0x0F) {
+				efuse_addr++;
+				read_efuse_byte(hw, efuse_addr, rtemp8);
+
+				if (*rtemp8 != 0xFF &&
+				    (efuse_addr < efuse_len)) {
+					efuse_addr++;
+				}
+				continue;
+			} else {
+				offset = ((*rtemp8 & 0xF0) >> 1) | u1temp;
+				wren = (*rtemp8 & 0x0F);
+				efuse_addr++;
+			}
+		} else {
+			offset = ((*rtemp8 >> 4) & 0x0f);
 			wren = (*rtemp8 & 0x0f);
+		}
+
+		if (offset < efuse_max_section) {
 			RTPRINT(rtlpriv, FEEPROM, EFUSE_READ_ALL,
 				"offset-%d Worden=%x\n", offset, wren);
 
@@ -391,7 +411,8 @@ bool efuse_shadow_update_chk(struct ieee80211_hw *hw)
 	efuse_used = rtlefuse->efuse_usedbytes;
 
 	if ((totalbytes + efuse_used) >=
-	    (EFUSE_MAX_SIZE - EFUSE_OOB_PROTECT_BYTES))
+	    (EFUSE_MAX_SIZE -
+	     rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN]))
 		result = false;
 
 	RT_TRACE(rtlpriv, COMP_EFUSE, DBG_LOUD,
@@ -932,8 +953,8 @@ static int efuse_pg_packet_write(struct ieee80211_hw *hw,
 	u8 badworden = 0x0F;
 	static int repeat_times;
 
-	if (efuse_get_current_size(hw) >=
-	    (EFUSE_MAX_SIZE - EFUSE_OOB_PROTECT_BYTES)) {
+	if (efuse_get_current_size(hw) >= (EFUSE_MAX_SIZE -
+	    rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN])) {
 		RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
 			"efuse_pg_packet_write error\n");
 		return false;
@@ -949,8 +970,8 @@ static int efuse_pg_packet_write(struct ieee80211_hw *hw,
 
 	RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,  "efuse Power ON\n");
 
-	while (continual && (efuse_addr <
-	       (EFUSE_MAX_SIZE - EFUSE_OOB_PROTECT_BYTES))) {
+	while (continual && (efuse_addr < (EFUSE_MAX_SIZE -
+	       rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN]))) {
 
 		if (write_state == PG_STATE_HEADER) {
 			badworden = 0x0F;
@@ -1003,7 +1024,8 @@ static int efuse_pg_packet_write(struct ieee80211_hw *hw,
 		}
 	}
 
-	if (efuse_addr >= (EFUSE_MAX_SIZE - EFUSE_OOB_PROTECT_BYTES)) {
+	if (efuse_addr >= (EFUSE_MAX_SIZE -
+	    rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN])) {
 		RT_TRACE(rtlpriv, COMP_EFUSE, DBG_LOUD,
 			 "efuse_addr(%#x) Out of size!!\n", efuse_addr);
 	}
@@ -1102,8 +1124,11 @@ static void efuse_power_switch(struct ieee80211_hw *hw, u8 write, u8 pwrstate)
 	u8 tempval;
 	u16 tmpV16;
 
-	if (pwrstate && (rtlhal->hw_type !=
-		HARDWARE_TYPE_RTL8192SE)) {
+	if (pwrstate && (rtlhal->hw_type != HARDWARE_TYPE_RTL8192SE)) {
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8188EE)
+			rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_ACCESS],
+				       0x69);
+
 		tmpV16 = rtl_read_word(rtlpriv,
 				       rtlpriv->cfg->maps[SYS_ISO_CTRL]);
 		if (!(tmpV16 & rtlpriv->cfg->maps[EFUSE_PWC_EV12V])) {
@@ -1153,6 +1178,10 @@ static void efuse_power_switch(struct ieee80211_hw *hw, u8 write, u8 pwrstate)
 		}
 
 	} else {
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8188EE)
+			rtl_write_byte(rtlpriv,
+				       rtlpriv->cfg->maps[EFUSE_ACCESS], 0);
+
 		if (write) {
 			tempval = rtl_read_byte(rtlpriv,
 						rtlpriv->cfg->maps[EFUSE_TEST] +
diff --git a/drivers/net/wireless/rtlwifi/efuse.h b/drivers/net/wireless/rtlwifi/efuse.h
index 2bdea9a8699e..395a326acfb4 100644
--- a/drivers/net/wireless/rtlwifi/efuse.h
+++ b/drivers/net/wireless/rtlwifi/efuse.h
@@ -32,7 +32,6 @@
 
 #define EFUSE_IC_ID_OFFSET		506
 
-#define EFUSE_REAL_CONTENT_LEN		512
 #define EFUSE_MAP_LEN			128
 #define EFUSE_MAX_WORD_UNIT		4
 
diff --git a/drivers/net/wireless/rtlwifi/pci.c b/drivers/net/wireless/rtlwifi/pci.c
index 4261e8ecc4c3..999ffc12578b 100644
--- a/drivers/net/wireless/rtlwifi/pci.c
+++ b/drivers/net/wireless/rtlwifi/pci.c
@@ -59,7 +59,7 @@ static u8 _rtl_mac_to_hwqueue(struct ieee80211_hw *hw,
 
 	if (unlikely(ieee80211_is_beacon(fc)))
 		return BEACON_QUEUE;
-	if (ieee80211_is_mgmt(fc))
+	if (ieee80211_is_mgmt(fc) || ieee80211_is_ctl(fc))
 		return MGNT_QUEUE;
 	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE)
 		if (ieee80211_is_nullfunc(fc))
@@ -271,9 +271,6 @@ static void rtl_pci_enable_aspm(struct ieee80211_hw *hw)
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	u8 pcibridge_busnum = pcipriv->ndis_adapter.pcibridge_busnum;
-	u8 pcibridge_devnum = pcipriv->ndis_adapter.pcibridge_devnum;
-	u8 pcibridge_funcnum = pcipriv->ndis_adapter.pcibridge_funcnum;
 	u8 pcibridge_vendor = pcipriv->ndis_adapter.pcibridge_vendor;
 	u8 num4bytes = pcipriv->ndis_adapter.num4bytes;
 	u16 aspmlevel;
@@ -302,8 +299,7 @@ static void rtl_pci_enable_aspm(struct ieee80211_hw *hw)
 			      u_pcibridge_aspmsetting);
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-		 "PlatformEnableASPM():PciBridge busnumber[%x], DevNumbe[%x], funcnumber[%x], Write reg[%x] = %x\n",
-		 pcibridge_busnum, pcibridge_devnum, pcibridge_funcnum,
+		 "PlatformEnableASPM(): Write reg[%x] = %x\n",
 		 (pcipriv->ndis_adapter.pcibridge_pciehdr_offset + 0x10),
 		 u_pcibridge_aspmsetting);
 
@@ -349,6 +345,49 @@ static bool rtl_pci_get_amd_l1_patch(struct ieee80211_hw *hw)
 	return status;
 }
 
+static bool rtl_pci_check_buddy_priv(struct ieee80211_hw *hw,
+				     struct rtl_priv **buddy_priv)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	bool find_buddy_priv = false;
+	struct rtl_priv *tpriv = NULL;
+	struct rtl_pci_priv *tpcipriv = NULL;
+
+	if (!list_empty(&rtlpriv->glb_var->glb_priv_list)) {
+		list_for_each_entry(tpriv, &rtlpriv->glb_var->glb_priv_list,
+				    list) {
+			if (tpriv) {
+				tpcipriv = (struct rtl_pci_priv *)tpriv->priv;
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+					 "pcipriv->ndis_adapter.funcnumber %x\n",
+					pcipriv->ndis_adapter.funcnumber);
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+					 "tpcipriv->ndis_adapter.funcnumber %x\n",
+					tpcipriv->ndis_adapter.funcnumber);
+
+				if ((pcipriv->ndis_adapter.busnumber ==
+				     tpcipriv->ndis_adapter.busnumber) &&
+				    (pcipriv->ndis_adapter.devnumber ==
+				    tpcipriv->ndis_adapter.devnumber) &&
+				    (pcipriv->ndis_adapter.funcnumber !=
+				    tpcipriv->ndis_adapter.funcnumber)) {
+					find_buddy_priv = true;
+					break;
+				}
+			}
+		}
+	}
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "find_buddy_priv %d\n", find_buddy_priv);
+
+	if (find_buddy_priv)
+		*buddy_priv = tpriv;
+
+	return find_buddy_priv;
+}
+
 static void rtl_pci_get_linkcontrol_field(struct ieee80211_hw *hw)
 {
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
@@ -420,17 +459,14 @@ static void _rtl_pci_io_handler_init(struct device *dev,
 
 }
 
-static void _rtl_pci_io_handler_release(struct ieee80211_hw *hw)
-{
-}
-
 static bool _rtl_update_earlymode_info(struct ieee80211_hw *hw,
 		struct sk_buff *skb, struct rtl_tcb_desc *tcb_desc, u8 tid)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-	u8 additionlen = FCS_LEN;
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct sk_buff *next_skb;
+	u8 additionlen = FCS_LEN;
 
 	/* here open is 4, wep/tkip is 8, aes is 12*/
 	if (info->control.hw_key)
@@ -455,7 +491,7 @@ static bool _rtl_update_earlymode_info(struct ieee80211_hw *hw,
 				      next_skb))
 			break;
 
-		if (tcb_desc->empkt_num >= 5)
+		if (tcb_desc->empkt_num >= rtlhal->max_earlymode_num)
 			break;
 	}
 	spin_unlock_bh(&rtlpriv->locks.waitq_lock);
@@ -471,11 +507,17 @@ static void _rtl_pci_tx_chk_waitq(struct ieee80211_hw *hw)
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct sk_buff *skb = NULL;
 	struct ieee80211_tx_info *info = NULL;
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	int tid;
 
 	if (!rtlpriv->rtlhal.earlymode_enable)
 		return;
 
+	if (rtlpriv->dm.supp_phymode_switch &&
+	    (rtlpriv->easy_concurrent_ctl.switch_in_process ||
+	    (rtlpriv->buddy_priv &&
+	    rtlpriv->buddy_priv->easy_concurrent_ctl.switch_in_process)))
+		return;
 	/* we juse use em for BE/BK/VI/VO */
 	for (tid = 7; tid >= 0; tid--) {
 		u8 hw_queue = ac_to_hwq[rtl_tid_to_ac(tid)];
@@ -487,7 +529,8 @@ static void _rtl_pci_tx_chk_waitq(struct ieee80211_hw *hw)
 
 			spin_lock_bh(&rtlpriv->locks.waitq_lock);
 			if (!skb_queue_empty(&mac->skb_waitq[tid]) &&
-			   (ring->entries - skb_queue_len(&ring->queue) > 5)) {
+			    (ring->entries - skb_queue_len(&ring->queue) >
+			     rtlhal->max_earlymode_num)) {
 				skb = skb_dequeue(&mac->skb_waitq[tid]);
 			} else {
 				spin_unlock_bh(&rtlpriv->locks.waitq_lock);
@@ -525,9 +568,8 @@ static void _rtl_pci_tx_isr(struct ieee80211_hw *hw, int prio)
 		u8 own = (u8) rtlpriv->cfg->ops->get_desc((u8 *) entry, true,
 							  HW_DESC_OWN);
 
-		/*
-		 *beacon packet will only use the first
-		 *descriptor defautly,and the own may not
+		/*beacon packet will only use the first
+		 *descriptor by defaut, and the own may not
 		 *be cleared by the hardware
 		 */
 		if (own)
@@ -558,8 +600,9 @@ static void _rtl_pci_tx_isr(struct ieee80211_hw *hw, int prio)
 		}
 
 		/* for sw LPS, just after NULL skb send out, we can
-		 * sure AP kown we are sleeped, our we should not let
-		 * rf to sleep*/
+		 * sure AP knows we are sleeping, we should not let
+		 * rf sleep
+		 */
 		fc = rtl_get_fc(skb);
 		if (ieee80211_is_nullfunc(fc)) {
 			if (ieee80211_has_pm(fc)) {
@@ -569,6 +612,15 @@ static void _rtl_pci_tx_isr(struct ieee80211_hw *hw, int prio)
 				rtlpriv->psc.state_inap = false;
 			}
 		}
+		if (ieee80211_is_action(fc)) {
+			struct ieee80211_mgmt *action_frame =
+				(struct ieee80211_mgmt *)skb->data;
+			if (action_frame->u.action.u.ht_smps.action ==
+			    WLAN_HT_ACTION_SMPS) {
+				dev_kfree_skb(skb);
+				goto tx_status_ok;
+			}
+		}
 
 		/* update tid tx pkt num */
 		tid = rtl_get_tid(skb);
@@ -602,7 +654,8 @@ tx_status_ok:
 	if (((rtlpriv->link_info.num_rx_inperiod +
 		rtlpriv->link_info.num_tx_inperiod) > 8) ||
 		(rtlpriv->link_info.num_rx_inperiod > 2)) {
-		schedule_work(&rtlpriv->works.lps_leave_work);
+		rtlpriv->enter_ps = false;
+		schedule_work(&rtlpriv->works.lps_change_work);
 	}
 }
 
@@ -637,6 +690,10 @@ static void _rtl_receive_one(struct ieee80211_hw *hw, struct sk_buff *skb,
 			rtlpriv->link_info.num_rx_inperiod++;
 	}
 
+	/* static bcn for roaming */
+	rtl_beacon_statistic(hw, skb);
+	rtl_p2p_info(hw, (void *)skb->data, skb->len);
+
 	/* for sw lps */
 	rtl_swlps_beacon(hw, (void *)skb->data, skb->len);
 	rtl_recognize_peer(hw, (void *)skb->data, skb->len);
@@ -727,9 +784,10 @@ static void _rtl_pci_rx_interrupt(struct ieee80211_hw *hw)
 		_rtl_receive_one(hw, skb, rx_status);
 
 		if (((rtlpriv->link_info.num_rx_inperiod +
-			rtlpriv->link_info.num_tx_inperiod) > 8) ||
-			(rtlpriv->link_info.num_rx_inperiod > 2)) {
-			schedule_work(&rtlpriv->works.lps_leave_work);
+		      rtlpriv->link_info.num_tx_inperiod) > 8) ||
+		      (rtlpriv->link_info.num_rx_inperiod > 2)) {
+			rtlpriv->enter_ps = false;
+			schedule_work(&rtlpriv->works.lps_change_work);
 		}
 
 		dev_kfree_skb_any(skb);
@@ -803,7 +861,7 @@ static irqreturn_t _rtl_pci_interrupt(int irq, void *dev_id)
 		RT_TRACE(rtlpriv, COMP_INTR, DBG_TRACE, "beacon interrupt!\n");
 	}
 
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_BcnInt]) {
+	if (inta & rtlpriv->cfg->maps[RTL_IMR_BCNINT]) {
 		RT_TRACE(rtlpriv, COMP_INTR, DBG_TRACE,
 			 "prepare beacon for interrupt!\n");
 		tasklet_schedule(&rtlpriv->works.irq_prepare_bcn_tasklet);
@@ -884,6 +942,16 @@ static irqreturn_t _rtl_pci_interrupt(int irq, void *dev_id)
 		_rtl_pci_rx_interrupt(hw);
 	}
 
+	/*fw related*/
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723AE) {
+		if (inta & rtlpriv->cfg->maps[RTL_IMR_C2HCMD]) {
+			RT_TRACE(rtlpriv, COMP_INTR, DBG_TRACE,
+				 "firmware interrupt!\n");
+			queue_delayed_work(rtlpriv->works.rtl_wq,
+					   &rtlpriv->works.fwevt_wq, 0);
+		}
+	}
+
 	if (rtlpriv->rtlhal.earlymode_enable)
 		tasklet_schedule(&rtlpriv->works.irq_tasklet);
 
@@ -939,13 +1007,17 @@ static void _rtl_pci_prepare_bcn_tasklet(struct ieee80211_hw *hw)
 	return;
 }
 
-static void rtl_lps_leave_work_callback(struct work_struct *work)
+static void rtl_lps_change_work_callback(struct work_struct *work)
 {
 	struct rtl_works *rtlworks =
-	    container_of(work, struct rtl_works, lps_leave_work);
+	    container_of(work, struct rtl_works, lps_change_work);
 	struct ieee80211_hw *hw = rtlworks->hw;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	rtl_lps_leave(hw);
+	if (rtlpriv->enter_ps)
+		rtl_lps_enter(hw);
+	else
+		rtl_lps_leave(hw);
 }
 
 static void _rtl_pci_init_trx_var(struct ieee80211_hw *hw)
@@ -1009,7 +1081,8 @@ static void _rtl_pci_init_struct(struct ieee80211_hw *hw,
 	tasklet_init(&rtlpriv->works.irq_prepare_bcn_tasklet,
 		     (void (*)(unsigned long))_rtl_pci_prepare_bcn_tasklet,
 		     (unsigned long)hw);
-	INIT_WORK(&rtlpriv->works.lps_leave_work, rtl_lps_leave_work_callback);
+	INIT_WORK(&rtlpriv->works.lps_change_work,
+		  rtl_lps_change_work_callback);
 }
 
 static int _rtl_pci_init_tx_ring(struct ieee80211_hw *hw,
@@ -1458,10 +1531,14 @@ static void rtl_pci_flush(struct ieee80211_hw *hw, bool drop)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	u16 i = 0;
 	int queue_id;
 	struct rtl8192_tx_ring *ring;
 
+	if (mac->skip_scan)
+		return;
+
 	for (queue_id = RTL_PCI_MAX_TX_QUEUE_COUNT - 1; queue_id >= 0;) {
 		u32 queue_len;
 		ring = &pcipriv->dev.tx_ring[queue_id];
@@ -1491,7 +1568,7 @@ static void rtl_pci_deinit(struct ieee80211_hw *hw)
 
 	synchronize_irq(rtlpci->pdev->irq);
 	tasklet_kill(&rtlpriv->works.irq_tasklet);
-	cancel_work_sync(&rtlpriv->works.lps_leave_work);
+	cancel_work_sync(&rtlpriv->works.lps_change_work);
 
 	flush_workqueue(rtlpriv->works.rtl_wq);
 	destroy_workqueue(rtlpriv->works.rtl_wq);
@@ -1566,7 +1643,7 @@ static void rtl_pci_stop(struct ieee80211_hw *hw)
 	set_hal_stop(rtlhal);
 
 	rtlpriv->cfg->ops->disable_interrupt(hw);
-	cancel_work_sync(&rtlpriv->works.lps_leave_work);
+	cancel_work_sync(&rtlpriv->works.lps_change_work);
 
 	spin_lock_irqsave(&rtlpriv->locks.rf_ps_lock, flags);
 	while (ppsc->rfchange_inprogress) {
@@ -1673,6 +1750,10 @@ static bool _rtl_pci_find_adapter(struct pci_dev *pdev,
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 			 "8192D PCI-E is found - vid/did=%x/%x\n",
 			 venderid, deviceid);
+	} else if (deviceid == RTL_PCI_8188EE_DID) {
+		rtlhal->hw_type = HARDWARE_TYPE_RTL8188EE;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Find adapter, Hardware type is 8188EE\n");
 	} else {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 			 "Err: Unknown device - vid/did=%x/%x\n",
@@ -1704,6 +1785,9 @@ static bool _rtl_pci_find_adapter(struct pci_dev *pdev,
 	pcipriv->ndis_adapter.devnumber = PCI_SLOT(pdev->devfn);
 	pcipriv->ndis_adapter.funcnumber = PCI_FUNC(pdev->devfn);
 
+	/* some ARM have no bridge_pdev and will crash here
+	 * so we should check if bridge_pdev is NULL
+	 */
 	if (bridge_pdev) {
 		/*find bridge info if available */
 		pcipriv->ndis_adapter.pcibridge_vendorid = bridge_pdev->vendor;
@@ -1758,6 +1842,7 @@ static bool _rtl_pci_find_adapter(struct pci_dev *pdev,
 		 pcipriv->ndis_adapter.amd_l1_patch);
 
 	rtl_pci_parse_configuration(pdev, hw);
+	list_add_tail(&rtlpriv->list, &rtlpriv->glb_var->glb_priv_list);
 
 	return true;
 }
@@ -1804,6 +1889,7 @@ int rtl_pci_probe(struct pci_dev *pdev,
 	pci_set_drvdata(pdev, hw);
 
 	rtlpriv = hw->priv;
+	rtlpriv->hw = hw;
 	pcipriv = (void *)rtlpriv->priv;
 	pcipriv->dev.pdev = pdev;
 	init_completion(&rtlpriv->firmware_loading_complete);
@@ -1812,6 +1898,7 @@ int rtl_pci_probe(struct pci_dev *pdev,
 	rtlpriv->rtlhal.interface = INTF_PCI;
 	rtlpriv->cfg = (struct rtl_hal_cfg *)(id->driver_data);
 	rtlpriv->intf_ops = &rtl_pci_ops;
+	rtlpriv->glb_var = &global_var;
 
 	/*
 	 *init dbgp flags before all
@@ -1916,7 +2003,6 @@ int rtl_pci_probe(struct pci_dev *pdev,
 
 fail3:
 	rtl_deinit_core(hw);
-	_rtl_pci_io_handler_release(hw);
 
 	if (rtlpriv->io.pci_mem_start != 0)
 		pci_iounmap(pdev, (void __iomem *)rtlpriv->io.pci_mem_start);
@@ -1965,14 +2051,15 @@ void rtl_pci_disconnect(struct pci_dev *pdev)
 
 	rtl_pci_deinit(hw);
 	rtl_deinit_core(hw);
-	_rtl_pci_io_handler_release(hw);
 	rtlpriv->cfg->ops->deinit_sw_vars(hw);
 
 	if (rtlpci->irq_alloc) {
+		synchronize_irq(rtlpci->pdev->irq);
 		free_irq(rtlpci->pdev->irq, hw);
 		rtlpci->irq_alloc = 0;
 	}
 
+	list_del(&rtlpriv->list);
 	if (rtlpriv->io.pci_mem_start != 0) {
 		pci_iounmap(pdev, (void __iomem *)rtlpriv->io.pci_mem_start);
 		pci_release_regions(pdev);
@@ -2034,6 +2121,7 @@ struct rtl_intf_ops rtl_pci_ops = {
 	.read_efuse_byte = read_efuse_byte,
 	.adapter_start = rtl_pci_start,
 	.adapter_stop = rtl_pci_stop,
+	.check_buddy_priv = rtl_pci_check_buddy_priv,
 	.adapter_tx = rtl_pci_tx,
 	.flush = rtl_pci_flush,
 	.reset_trx_ring = rtl_pci_reset_trx_ring,
diff --git a/drivers/net/wireless/rtlwifi/pci.h b/drivers/net/wireless/rtlwifi/pci.h
index 65b08f50022e..d3262ec45d23 100644
--- a/drivers/net/wireless/rtlwifi/pci.h
+++ b/drivers/net/wireless/rtlwifi/pci.h
@@ -94,6 +94,7 @@
 #define RTL_PCI_8192CU_DID	0x8191	/*8192ce */
 #define RTL_PCI_8192DE_DID	0x8193	/*8192de */
 #define RTL_PCI_8192DE_DID2	0x002B	/*92DE*/
+#define RTL_PCI_8188EE_DID	0x8179  /*8188ee*/
 
 /*8192 support 16 pages of IO registers*/
 #define RTL_MEM_MAPPED_IO_RANGE_8190PCI		0x1000
@@ -175,6 +176,7 @@ struct rtl_pci {
 	/*irq */
 	u8 irq_alloc;
 	u32 irq_mask[2];
+	u32 sys_irq_mask;
 
 	/*Bcn control register setting */
 	u32 reg_bcn_ctrl_val;
diff --git a/drivers/net/wireless/rtlwifi/ps.c b/drivers/net/wireless/rtlwifi/ps.c
index 13ad33e85577..884bceae38a9 100644
--- a/drivers/net/wireless/rtlwifi/ps.c
+++ b/drivers/net/wireless/rtlwifi/ps.c
@@ -180,6 +180,9 @@ void rtl_ips_nic_off_wq_callback(void *data)
 		return;
 	}
 
+	if (mac->p2p_in_use)
+		return;
+
 	if (mac->link_state > MAC80211_NOLINK)
 		return;
 
@@ -189,6 +192,9 @@ void rtl_ips_nic_off_wq_callback(void *data)
 	if (rtlpriv->sec.being_setkey)
 		return;
 
+	if (rtlpriv->cfg->ops->bt_coex_off_before_lps)
+		rtlpriv->cfg->ops->bt_coex_off_before_lps(hw);
+
 	if (ppsc->inactiveps) {
 		rtstate = ppsc->rfpwr_state;
 
@@ -231,6 +237,9 @@ void rtl_ips_nic_off(struct ieee80211_hw *hw)
 			   &rtlpriv->works.ips_nic_off_wq, MSECS(100));
 }
 
+/* NOTICE: any opmode should exc nic_on, or disable without
+ * nic_on may something wrong, like adhoc TP
+ */
 void rtl_ips_nic_on(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -299,7 +308,7 @@ static void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	u8 rpwm_val, fw_pwrmode;
+	bool enter_fwlps;
 
 	if (mac->opmode == NL80211_IFTYPE_ADHOC)
 		return;
@@ -324,43 +333,31 @@ static void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
 	 */
 
 	if ((ppsc->fwctrl_lps) && ppsc->report_linked) {
-		bool fw_current_inps;
 		if (ppsc->dot11_psmode == EACTIVE) {
 			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 				 "FW LPS leave ps_mode:%x\n",
 				 FW_PS_ACTIVE_MODE);
-
-			rpwm_val = 0x0C;	/* RF on */
-			fw_pwrmode = FW_PS_ACTIVE_MODE;
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
-					&rpwm_val);
+			enter_fwlps = false;
+			ppsc->pwr_mode = FW_PS_ACTIVE_MODE;
+			ppsc->smart_ps = 0;
 			rtlpriv->cfg->ops->set_hw_reg(hw,
-					HW_VAR_H2C_FW_PWRMODE,
-					&fw_pwrmode);
-			fw_current_inps = false;
-
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-					HW_VAR_FW_PSMODE_STATUS,
-					(u8 *) (&fw_current_inps));
+						HW_VAR_FW_LPS_ACTION,
+						(u8 *)(&enter_fwlps));
+			if (ppsc->p2p_ps_info.opp_ps)
+				rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
 
 		} else {
 			if (rtl_get_fwlps_doze(hw)) {
 				RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 					 "FW LPS enter ps_mode:%x\n",
 					 ppsc->fwctrl_psmode);
-
-				rpwm_val = 0x02;	/* RF off */
-				fw_current_inps = true;
+				enter_fwlps = true;
+				ppsc->pwr_mode = ppsc->fwctrl_psmode;
+				ppsc->smart_ps = 2;
 				rtlpriv->cfg->ops->set_hw_reg(hw,
-						HW_VAR_FW_PSMODE_STATUS,
-						(u8 *) (&fw_current_inps));
-				rtlpriv->cfg->ops->set_hw_reg(hw,
-						HW_VAR_H2C_FW_PWRMODE,
-						&ppsc->fwctrl_psmode);
+							HW_VAR_FW_LPS_ACTION,
+							(u8 *)(&enter_fwlps));
 
-				rtlpriv->cfg->ops->set_hw_reg(hw,
-						HW_VAR_SET_RPWM,
-						&rpwm_val);
 			} else {
 				/* Reset the power save related parameters. */
 				ppsc->dot11_psmode = EACTIVE;
@@ -642,3 +639,286 @@ void rtl_swlps_wq_callback(void *data)
 		rtlpriv->psc.state = ps;
 	}
 }
+
+static void rtl_p2p_noa_ie(struct ieee80211_hw *hw, void *data,
+			   unsigned int len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ieee80211_mgmt *mgmt = (void *)data;
+	struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
+	u8 *pos, *end, *ie;
+	u16 noa_len;
+	static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
+	u8 noa_num, index, i, noa_index = 0;
+	bool find_p2p_ie = false , find_p2p_ps_ie = false;
+	pos = (u8 *)mgmt->u.beacon.variable;
+	end = data + len;
+	ie = NULL;
+
+	while (pos + 1 < end) {
+		if (pos + 2 + pos[1] > end)
+			return;
+
+		if (pos[0] == 221 && pos[1] > 4) {
+			if (memcmp(&pos[2], p2p_oui_ie_type, 4) == 0) {
+				ie = pos + 2+4;
+				break;
+			}
+		}
+		pos += 2 + pos[1];
+	}
+
+	if (ie == NULL)
+		return;
+	find_p2p_ie = true;
+	/*to find noa ie*/
+	while (ie + 1 < end) {
+		noa_len = READEF2BYTE(&ie[1]);
+		if (ie + 3 + ie[1] > end)
+			return;
+
+		if (ie[0] == 12) {
+			find_p2p_ps_ie = true;
+			if ((noa_len - 2) % 13 != 0) {
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+					 "P2P notice of absence: invalid length.%d\n",
+					 noa_len);
+				return;
+			} else {
+				noa_num = (noa_len - 2) / 13;
+			}
+			noa_index = ie[3];
+			if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode ==
+			    P2P_PS_NONE || noa_index != p2pinfo->noa_index) {
+				RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+					 "update NOA ie.\n");
+				p2pinfo->noa_index = noa_index;
+				p2pinfo->opp_ps = (ie[4] >> 7);
+				p2pinfo->ctwindow = ie[4] & 0x7F;
+				p2pinfo->noa_num = noa_num;
+				index = 5;
+				for (i = 0; i < noa_num; i++) {
+					p2pinfo->noa_count_type[i] =
+						 READEF1BYTE(ie+index);
+					index += 1;
+					p2pinfo->noa_duration[i] =
+						 READEF4BYTE(ie+index);
+					index += 4;
+					p2pinfo->noa_interval[i] =
+						 READEF4BYTE(ie+index);
+					index += 4;
+					p2pinfo->noa_start_time[i] =
+						 READEF4BYTE(ie+index);
+					index += 4;
+				}
+
+				if (p2pinfo->opp_ps == 1) {
+					p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
+					/* Driver should wait LPS entering
+					 * CTWindow
+					 */
+					if (rtlpriv->psc.fw_current_inpsmode)
+						rtl_p2p_ps_cmd(hw,
+							       P2P_PS_ENABLE);
+				} else if (p2pinfo->noa_num > 0) {
+					p2pinfo->p2p_ps_mode = P2P_PS_NOA;
+					rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
+				} else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
+					rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
+				}
+			}
+		break;
+		}
+		ie += 3 + noa_len;
+	}
+
+	if (find_p2p_ie == true) {
+		if ((p2pinfo->p2p_ps_mode > P2P_PS_NONE) &&
+		    (find_p2p_ps_ie == false))
+			rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
+	}
+}
+
+static void rtl_p2p_action_ie(struct ieee80211_hw *hw, void *data,
+			      unsigned int len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ieee80211_mgmt *mgmt = (void *)data;
+	struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
+	u8 noa_num, index, i, noa_index = 0;
+	u8 *pos, *end, *ie;
+	u16 noa_len;
+	static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
+
+	pos = (u8 *)&mgmt->u.action.category;
+	end = data + len;
+	ie = NULL;
+
+	if (pos[0] == 0x7f) {
+		if (memcmp(&pos[1], p2p_oui_ie_type, 4) == 0)
+			ie = pos + 3+4;
+	}
+
+	if (ie == NULL)
+		return;
+
+	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "action frame find P2P IE.\n");
+	/*to find noa ie*/
+	while (ie + 1 < end) {
+		noa_len = READEF2BYTE(&ie[1]);
+		if (ie + 3 + ie[1] > end)
+			return;
+
+		if (ie[0] == 12) {
+			RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "find NOA IE.\n");
+			RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_LOUD, "noa ie ",
+				      ie, noa_len);
+			if ((noa_len - 2) % 13 != 0) {
+				RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+					 "P2P notice of absence: invalid length.%d\n",
+					 noa_len);
+				return;
+			} else {
+				noa_num = (noa_len - 2) / 13;
+			}
+			noa_index = ie[3];
+			if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode ==
+			    P2P_PS_NONE || noa_index != p2pinfo->noa_index) {
+				p2pinfo->noa_index = noa_index;
+				p2pinfo->opp_ps = (ie[4] >> 7);
+				p2pinfo->ctwindow = ie[4] & 0x7F;
+				p2pinfo->noa_num = noa_num;
+				index = 5;
+				for (i = 0; i < noa_num; i++) {
+					p2pinfo->noa_count_type[i] =
+							 READEF1BYTE(ie+index);
+					index += 1;
+					p2pinfo->noa_duration[i] =
+							 READEF4BYTE(ie+index);
+					index += 4;
+					p2pinfo->noa_interval[i] =
+							 READEF4BYTE(ie+index);
+					index += 4;
+					p2pinfo->noa_start_time[i] =
+							 READEF4BYTE(ie+index);
+					index += 4;
+				}
+
+				if (p2pinfo->opp_ps == 1) {
+					p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
+					/* Driver should wait LPS entering
+					 * CTWindow
+					 */
+					if (rtlpriv->psc.fw_current_inpsmode)
+						rtl_p2p_ps_cmd(hw,
+							       P2P_PS_ENABLE);
+				} else if (p2pinfo->noa_num > 0) {
+					p2pinfo->p2p_ps_mode = P2P_PS_NOA;
+					rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
+				} else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
+					rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
+				}
+			}
+		break;
+		}
+		ie += 3 + noa_len;
+	}
+}
+
+void rtl_p2p_ps_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
+	struct rtl_p2p_ps_info  *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
+
+	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, " p2p state %x\n", p2p_ps_state);
+	switch (p2p_ps_state) {
+	case P2P_PS_DISABLE:
+		p2pinfo->p2p_ps_state = p2p_ps_state;
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+				 HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
+				 (u8 *)(&p2p_ps_state));
+
+		p2pinfo->noa_index = 0;
+		p2pinfo->ctwindow = 0;
+		p2pinfo->opp_ps = 0;
+		p2pinfo->noa_num = 0;
+		p2pinfo->p2p_ps_mode = P2P_PS_NONE;
+		if (rtlps->fw_current_inpsmode == true) {
+			if (rtlps->smart_ps == 0) {
+				rtlps->smart_ps = 2;
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+					 HW_VAR_H2C_FW_PWRMODE,
+					 (u8 *)(&rtlps->pwr_mode));
+			}
+		}
+		break;
+	case P2P_PS_ENABLE:
+		if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
+			p2pinfo->p2p_ps_state = p2p_ps_state;
+
+			if (p2pinfo->ctwindow > 0) {
+				if (rtlps->smart_ps != 0) {
+					rtlps->smart_ps = 0;
+					rtlpriv->cfg->ops->set_hw_reg(hw,
+						 HW_VAR_H2C_FW_PWRMODE,
+						 (u8 *)(&rtlps->pwr_mode));
+				}
+			}
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+				 HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
+				 (u8 *)(&p2p_ps_state));
+		}
+		break;
+	case P2P_PS_SCAN:
+	case P2P_PS_SCAN_DONE:
+	case P2P_PS_ALLSTASLEEP:
+		if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
+			p2pinfo->p2p_ps_state = p2p_ps_state;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+				 HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
+				 (u8 *)(&p2p_ps_state));
+		}
+		break;
+	default:
+		break;
+	}
+	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+		 "ctwindow %x oppps %x\n", p2pinfo->ctwindow, p2pinfo->opp_ps);
+	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+		 "count %x duration %x index %x interval %x start time %x noa num %x\n",
+		 p2pinfo->noa_count_type[0], p2pinfo->noa_duration[0],
+		 p2pinfo->noa_index, p2pinfo->noa_interval[0],
+		 p2pinfo->noa_start_time[0], p2pinfo->noa_num);
+	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "end\n");
+}
+
+void rtl_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct ieee80211_hdr *hdr = (void *)data;
+
+	if (!mac->p2p)
+		return;
+	if (mac->link_state != MAC80211_LINKED)
+		return;
+	/* min. beacon length + FCS_LEN */
+	if (len <= 40 + FCS_LEN)
+		return;
+
+	/* and only beacons from the associated BSSID, please */
+	if (compare_ether_addr(hdr->addr3, rtlpriv->mac80211.bssid))
+		return;
+
+	/* check if this really is a beacon */
+	if (!(ieee80211_is_beacon(hdr->frame_control) ||
+	      ieee80211_is_probe_resp(hdr->frame_control) ||
+	      ieee80211_is_action(hdr->frame_control)))
+		return;
+
+	if (ieee80211_is_action(hdr->frame_control))
+		rtl_p2p_action_ie(hw, data, len - FCS_LEN);
+	else
+		rtl_p2p_noa_ie(hw, data, len - FCS_LEN);
+}
diff --git a/drivers/net/wireless/rtlwifi/ps.h b/drivers/net/wireless/rtlwifi/ps.h
index 1357856998c2..4d682b753f50 100644
--- a/drivers/net/wireless/rtlwifi/ps.h
+++ b/drivers/net/wireless/rtlwifi/ps.h
@@ -47,5 +47,7 @@ void rtl_swlps_wq_callback(void *data);
 void rtl_swlps_rfon_wq_callback(void *data);
 void rtl_swlps_rf_awake(struct ieee80211_hw *hw);
 void rtl_swlps_rf_sleep(struct ieee80211_hw *hw);
+void rtl_p2p_ps_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state);
+void rtl_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/Makefile b/drivers/net/wireless/rtlwifi/rtl8188ee/Makefile
new file mode 100644
index 000000000000..5b194e97f4b3
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/Makefile
@@ -0,0 +1,16 @@
+rtl8188ee-objs :=		\
+		dm.o		\
+		fw.o		\
+		hw.o		\
+		led.o		\
+		phy.o		\
+		pwrseq.o	\
+		pwrseqcmd.o	\
+		rf.o		\
+		sw.o		\
+		table.o		\
+		trx.o
+
+obj-$(CONFIG_RTL8188EE) += rtl8188ee.o
+
+ccflags-y += -Idrivers/net/wireless/rtlwifi -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/def.h b/drivers/net/wireless/rtlwifi/rtl8188ee/def.h
new file mode 100644
index 000000000000..c764fff9ebe6
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/def.h
@@ -0,0 +1,324 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92C_DEF_H__
+#define __RTL92C_DEF_H__
+
+#define HAL_RETRY_LIMIT_INFRA				48
+#define HAL_RETRY_LIMIT_AP_ADHOC			7
+
+#define RESET_DELAY_8185				20
+
+#define RT_IBSS_INT_MASKS	(IMR_BCNINT | IMR_TBDOK | IMR_TBDER)
+#define RT_AC_INT_MASKS		(IMR_VIDOK | IMR_VODOK | IMR_BEDOK|IMR_BKDOK)
+
+#define NUM_OF_FIRMWARE_QUEUE				10
+#define NUM_OF_PAGES_IN_FW				0x100
+#define NUM_OF_PAGE_IN_FW_QUEUE_BK			0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_BE			0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_VI			0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_VO			0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_HCCA			0x0
+#define NUM_OF_PAGE_IN_FW_QUEUE_CMD			0x0
+#define NUM_OF_PAGE_IN_FW_QUEUE_MGNT			0x02
+#define NUM_OF_PAGE_IN_FW_QUEUE_HIGH			0x02
+#define NUM_OF_PAGE_IN_FW_QUEUE_BCN			0x2
+#define NUM_OF_PAGE_IN_FW_QUEUE_PUB			0xA1
+
+#define NUM_OF_PAGE_IN_FW_QUEUE_BK_DTM			0x026
+#define NUM_OF_PAGE_IN_FW_QUEUE_BE_DTM			0x048
+#define NUM_OF_PAGE_IN_FW_QUEUE_VI_DTM			0x048
+#define NUM_OF_PAGE_IN_FW_QUEUE_VO_DTM			0x026
+#define NUM_OF_PAGE_IN_FW_QUEUE_PUB_DTM			0x00
+
+#define MAX_LINES_HWCONFIG_TXT				1000
+#define MAX_BYTES_LINE_HWCONFIG_TXT			256
+
+#define SW_THREE_WIRE					0
+#define HW_THREE_WIRE					2
+
+#define BT_DEMO_BOARD					0
+#define BT_QA_BOARD					1
+#define BT_FPGA						2
+
+#define HAL_PRIME_CHNL_OFFSET_DONT_CARE			0
+#define HAL_PRIME_CHNL_OFFSET_LOWER			1
+#define HAL_PRIME_CHNL_OFFSET_UPPER			2
+
+#define MAX_H2C_QUEUE_NUM				10
+
+#define RX_MPDU_QUEUE					0
+#define RX_CMD_QUEUE					1
+#define RX_MAX_QUEUE					2
+#define AC2QUEUEID(_AC)					(_AC)
+
+#define	C2H_RX_CMD_HDR_LEN				8
+#define	GET_C2H_CMD_CMD_LEN(__prxhdr)			\
+	LE_BITS_TO_4BYTE((__prxhdr), 0, 16)
+#define	GET_C2H_CMD_ELEMENT_ID(__prxhdr)		\
+	LE_BITS_TO_4BYTE((__prxhdr), 16, 8)
+#define	GET_C2H_CMD_CMD_SEQ(__prxhdr)			\
+	LE_BITS_TO_4BYTE((__prxhdr), 24, 7)
+#define	GET_C2H_CMD_CONTINUE(__prxhdr)			\
+	LE_BITS_TO_4BYTE((__prxhdr), 31, 1)
+#define	GET_C2H_CMD_CONTENT(__prxhdr)			\
+	((u8 *)(__prxhdr) + C2H_RX_CMD_HDR_LEN)
+
+#define	GET_C2H_CMD_FEEDBACK_ELEMENT_ID(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE((__pcmdfbhdr), 0, 8)
+#define	GET_C2H_CMD_FEEDBACK_CCX_LEN(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE((__pcmdfbhdr), 8, 8)
+#define	GET_C2H_CMD_FEEDBACK_CCX_CMD_CNT(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE((__pcmdfbhdr), 16, 16)
+#define	GET_C2H_CMD_FEEDBACK_CCX_MAC_ID(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 0, 5)
+#define	GET_C2H_CMD_FEEDBACK_CCX_VALID(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 7, 1)
+#define	GET_C2H_CMD_FEEDBACK_CCX_RETRY_CNT(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 8, 5)
+#define	GET_C2H_CMD_FEEDBACK_CCX_TOK(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 15, 1)
+#define	GET_C2H_CMD_FEEDBACK_CCX_QSEL(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 16, 4)
+#define	GET_C2H_CMD_FEEDBACK_CCX_SEQ(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 20, 12)
+
+#define CHIP_BONDING_IDENTIFIER(_value)	(((_value)>>22)&0x3)
+
+
+/* [15:12] IC version(CUT): A-cut=0, B-cut=1, C-cut=2, D-cut=3
+ * [7] Manufacturer: TSMC=0, UMC=1
+ * [6:4] RF type: 1T1R=0, 1T2R=1, 2T2R=2
+ * [3] Chip type: TEST=0, NORMAL=1
+ * [2:0] IC type: 81xxC=0, 8723=1, 92D=2
+ */
+#define CHIP_8723			BIT(0)
+#define CHIP_92D			BIT(1)
+#define NORMAL_CHIP			BIT(3)
+#define RF_TYPE_1T1R			(~(BIT(4)|BIT(5)|BIT(6)))
+#define RF_TYPE_1T2R			BIT(4)
+#define RF_TYPE_2T2R			BIT(5)
+#define CHIP_VENDOR_UMC			BIT(7)
+#define B_CUT_VERSION			BIT(12)
+#define C_CUT_VERSION			BIT(13)
+#define D_CUT_VERSION			((BIT(12)|BIT(13)))
+#define E_CUT_VERSION			BIT(14)
+
+
+/* MASK */
+#define IC_TYPE_MASK			(BIT(0)|BIT(1)|BIT(2))
+#define CHIP_TYPE_MASK			BIT(3)
+#define RF_TYPE_MASK			(BIT(4)|BIT(5)|BIT(6))
+#define MANUFACTUER_MASK		BIT(7)
+#define ROM_VERSION_MASK		(BIT(11)|BIT(10)|BIT(9)|BIT(8))
+#define CUT_VERSION_MASK		(BIT(15)|BIT(14)|BIT(13)|BIT(12))
+
+/* Get element */
+#define GET_CVID_IC_TYPE(version)	((version) & IC_TYPE_MASK)
+#define GET_CVID_CHIP_TYPE(version)	((version) & CHIP_TYPE_MASK)
+#define GET_CVID_RF_TYPE(version)	((version) & RF_TYPE_MASK)
+#define GET_CVID_MANUFACTUER(version)	((version) & MANUFACTUER_MASK)
+#define GET_CVID_ROM_VERSION(version)	((version) & ROM_VERSION_MASK)
+#define GET_CVID_CUT_VERSION(version)	((version) & CUT_VERSION_MASK)
+
+
+#define IS_81XXC(version)						\
+	((GET_CVID_IC_TYPE(version) == 0) ? true : false)
+#define IS_8723_SERIES(version)						\
+	((GET_CVID_IC_TYPE(version) == CHIP_8723) ? true : false)
+#define IS_92D(version)							\
+	((GET_CVID_IC_TYPE(version) == CHIP_92D) ? true : false)
+
+#define IS_NORMAL_CHIP(version)						\
+	((GET_CVID_CHIP_TYPE(version)) ? true : false)
+#define IS_NORMAL_CHIP92D(version)					\
+	((GET_CVID_CHIP_TYPE(version)) ? true : false)
+
+#define IS_1T1R(version)						\
+	((GET_CVID_RF_TYPE(version)) ? false : true)
+#define IS_1T2R(version)						\
+	((GET_CVID_RF_TYPE(version) == RF_TYPE_1T2R) ? true : false)
+#define IS_2T2R(version)						\
+	((GET_CVID_RF_TYPE(version) == RF_TYPE_2T2R) ? true : false)
+#define IS_CHIP_VENDOR_UMC(version)					\
+	((GET_CVID_MANUFACTUER(version)) ? true : false)
+
+#define IS_92C_SERIAL(version)						\
+	((IS_81XXC(version) && IS_2T2R(version)) ? true : false)
+#define IS_81xxC_VENDOR_UMC_A_CUT(version)				\
+	(IS_81XXC(version) ? ((IS_CHIP_VENDOR_UMC(version)) ?		\
+	 ((GET_CVID_CUT_VERSION(version)) ? false : true) : false) : false)
+#define IS_81xxC_VENDOR_UMC_B_CUT(version)				\
+	(IS_81XXC(version) ? (IS_CHIP_VENDOR_UMC(version) ?		\
+	((GET_CVID_CUT_VERSION(version) == B_CUT_VERSION) ? true	\
+	: false) : false) : false)
+
+enum version_8188e {
+	VERSION_TEST_CHIP_88E = 0x00,
+	VERSION_NORMAL_CHIP_88E = 0x01,
+	VERSION_UNKNOWN = 0xFF,
+};
+
+enum rx_packet_type {
+	NORMAL_RX,
+	TX_REPORT1,
+	TX_REPORT2,
+	HIS_REPORT,
+};
+
+enum rtl819x_loopback_e {
+	RTL819X_NO_LOOPBACK = 0,
+	RTL819X_MAC_LOOPBACK = 1,
+	RTL819X_DMA_LOOPBACK = 2,
+	RTL819X_CCK_LOOPBACK = 3,
+};
+
+enum rf_optype {
+	RF_OP_BY_SW_3WIRE = 0,
+	RF_OP_BY_FW,
+	RF_OP_MAX
+};
+
+enum rf_power_state {
+	RF_ON,
+	RF_OFF,
+	RF_SLEEP,
+	RF_SHUT_DOWN,
+};
+
+enum power_save_mode {
+	POWER_SAVE_MODE_ACTIVE,
+	POWER_SAVE_MODE_SAVE,
+};
+
+enum power_polocy_config {
+	POWERCFG_MAX_POWER_SAVINGS,
+	POWERCFG_GLOBAL_POWER_SAVINGS,
+	POWERCFG_LOCAL_POWER_SAVINGS,
+	POWERCFG_LENOVO,
+};
+
+enum interface_select_pci {
+	INTF_SEL1_MINICARD,
+	INTF_SEL0_PCIE,
+	INTF_SEL2_RSV,
+	INTF_SEL3_RSV,
+};
+
+enum hal_fw_c2h_cmd_id {
+	HAL_FW_C2H_CMD_Read_MACREG,
+	HAL_FW_C2H_CMD_Read_BBREG,
+	HAL_FW_C2H_CMD_Read_RFREG,
+	HAL_FW_C2H_CMD_Read_EEPROM,
+	HAL_FW_C2H_CMD_Read_EFUSE,
+	HAL_FW_C2H_CMD_Read_CAM,
+	HAL_FW_C2H_CMD_Get_BasicRate,
+	HAL_FW_C2H_CMD_Get_DataRate,
+	HAL_FW_C2H_CMD_Survey,
+	HAL_FW_C2H_CMD_SurveyDone,
+	HAL_FW_C2H_CMD_JoinBss,
+	HAL_FW_C2H_CMD_AddSTA,
+	HAL_FW_C2H_CMD_DelSTA,
+	HAL_FW_C2H_CMD_AtimDone,
+	HAL_FW_C2H_CMD_TX_Report,
+	HAL_FW_C2H_CMD_CCX_Report,
+	HAL_FW_C2H_CMD_DTM_Report,
+	HAL_FW_C2H_CMD_TX_Rate_Statistics,
+	HAL_FW_C2H_CMD_C2HLBK,
+	HAL_FW_C2H_CMD_C2HDBG,
+	HAL_FW_C2H_CMD_C2HFEEDBACK,
+	HAL_FW_C2H_CMD_MAX
+};
+
+enum wake_on_wlan_mode {
+	ewowlandisable,
+	ewakeonmagicpacketonly,
+	ewakeonpatternmatchonly,
+	ewakeonbothtypepacket
+};
+
+enum rtl_desc_qsel {
+	QSLT_BK = 0x2,
+	QSLT_BE = 0x0,
+	QSLT_VI = 0x5,
+	QSLT_VO = 0x7,
+	QSLT_BEACON = 0x10,
+	QSLT_HIGH = 0x11,
+	QSLT_MGNT = 0x12,
+	QSLT_CMD = 0x13,
+};
+
+enum rtl_desc92c_rate {
+	DESC92C_RATE1M = 0x00,
+	DESC92C_RATE2M = 0x01,
+	DESC92C_RATE5_5M = 0x02,
+	DESC92C_RATE11M = 0x03,
+
+	DESC92C_RATE6M = 0x04,
+	DESC92C_RATE9M = 0x05,
+	DESC92C_RATE12M = 0x06,
+	DESC92C_RATE18M = 0x07,
+	DESC92C_RATE24M = 0x08,
+	DESC92C_RATE36M = 0x09,
+	DESC92C_RATE48M = 0x0a,
+	DESC92C_RATE54M = 0x0b,
+
+	DESC92C_RATEMCS0 = 0x0c,
+	DESC92C_RATEMCS1 = 0x0d,
+	DESC92C_RATEMCS2 = 0x0e,
+	DESC92C_RATEMCS3 = 0x0f,
+	DESC92C_RATEMCS4 = 0x10,
+	DESC92C_RATEMCS5 = 0x11,
+	DESC92C_RATEMCS6 = 0x12,
+	DESC92C_RATEMCS7 = 0x13,
+	DESC92C_RATEMCS8 = 0x14,
+	DESC92C_RATEMCS9 = 0x15,
+	DESC92C_RATEMCS10 = 0x16,
+	DESC92C_RATEMCS11 = 0x17,
+	DESC92C_RATEMCS12 = 0x18,
+	DESC92C_RATEMCS13 = 0x19,
+	DESC92C_RATEMCS14 = 0x1a,
+	DESC92C_RATEMCS15 = 0x1b,
+	DESC92C_RATEMCS15_SG = 0x1c,
+	DESC92C_RATEMCS32 = 0x20,
+};
+
+struct phy_sts_cck_8192s_t {
+	u8 adc_pwdb_X[4];
+	u8 sq_rpt;
+	u8 cck_agc_rpt;
+};
+
+struct h2c_cmd_8192c {
+	u8 element_id;
+	u32 cmd_len;
+	u8 *p_cmdbuffer;
+};
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c
new file mode 100644
index 000000000000..21a5cf060677
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c
@@ -0,0 +1,1794 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../base.h"
+#include "../pci.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "dm.h"
+#include "fw.h"
+#include "trx.h"
+
+static const u32 ofdmswing_table[OFDM_TABLE_SIZE] = {
+	0x7f8001fe,		/* 0, +6.0dB */
+	0x788001e2,		/* 1, +5.5dB */
+	0x71c001c7,		/* 2, +5.0dB */
+	0x6b8001ae,		/* 3, +4.5dB */
+	0x65400195,		/* 4, +4.0dB */
+	0x5fc0017f,		/* 5, +3.5dB */
+	0x5a400169,		/* 6, +3.0dB */
+	0x55400155,		/* 7, +2.5dB */
+	0x50800142,		/* 8, +2.0dB */
+	0x4c000130,		/* 9, +1.5dB */
+	0x47c0011f,		/* 10, +1.0dB */
+	0x43c0010f,		/* 11, +0.5dB */
+	0x40000100,		/* 12, +0dB */
+	0x3c8000f2,		/* 13, -0.5dB */
+	0x390000e4,		/* 14, -1.0dB */
+	0x35c000d7,		/* 15, -1.5dB */
+	0x32c000cb,		/* 16, -2.0dB */
+	0x300000c0,		/* 17, -2.5dB */
+	0x2d4000b5,		/* 18, -3.0dB */
+	0x2ac000ab,		/* 19, -3.5dB */
+	0x288000a2,		/* 20, -4.0dB */
+	0x26000098,		/* 21, -4.5dB */
+	0x24000090,		/* 22, -5.0dB */
+	0x22000088,		/* 23, -5.5dB */
+	0x20000080,		/* 24, -6.0dB */
+	0x1e400079,		/* 25, -6.5dB */
+	0x1c800072,		/* 26, -7.0dB */
+	0x1b00006c,		/* 27. -7.5dB */
+	0x19800066,		/* 28, -8.0dB */
+	0x18000060,		/* 29, -8.5dB */
+	0x16c0005b,		/* 30, -9.0dB */
+	0x15800056,		/* 31, -9.5dB */
+	0x14400051,		/* 32, -10.0dB */
+	0x1300004c,		/* 33, -10.5dB */
+	0x12000048,		/* 34, -11.0dB */
+	0x11000044,		/* 35, -11.5dB */
+	0x10000040,		/* 36, -12.0dB */
+	0x0f00003c,		/* 37, -12.5dB */
+	0x0e400039,		/* 38, -13.0dB */
+	0x0d800036,		/* 39, -13.5dB */
+	0x0cc00033,		/* 40, -14.0dB */
+	0x0c000030,		/* 41, -14.5dB */
+	0x0b40002d,		/* 42, -15.0dB */
+};
+
+static const u8 cck_tbl_ch1_13[CCK_TABLE_SIZE][8] = {
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04},	/* 0, +0dB */
+	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04},	/* 1, -0.5dB */
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03},	/* 2, -1.0dB */
+	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03},	/* 3, -1.5dB */
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03},	/* 4, -2.0dB */
+	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03},	/* 5, -2.5dB */
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03},	/* 6, -3.0dB */
+	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03},	/* 7, -3.5dB */
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02},	/* 8, -4.0dB */
+	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02},	/* 9, -4.5dB */
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02},	/* 10, -5.0dB */
+	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02},	/* 11, -5.5dB */
+	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02},	/* 12, -6.0dB */
+	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02},	/* 13, -6.5dB */
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02},	/* 14, -7.0dB */
+	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02},	/* 15, -7.5dB */
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01},	/* 16, -8.0dB */
+	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02},	/* 17, -8.5dB */
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01},	/* 18, -9.0dB */
+	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},	/* 19, -9.5dB */
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},	/* 20, -10.0dB*/
+	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01},	/* 21, -10.5dB*/
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01},	/* 22, -11.0dB*/
+	{0x0e, 0x0e, 0x0c, 0x0a, 0x08, 0x05, 0x02, 0x01},	/* 23, -11.5dB*/
+	{0x0d, 0x0d, 0x0c, 0x0a, 0x07, 0x05, 0x02, 0x01},	/* 24, -12.0dB*/
+	{0x0d, 0x0c, 0x0b, 0x09, 0x07, 0x04, 0x02, 0x01},	/* 25, -12.5dB*/
+	{0x0c, 0x0c, 0x0a, 0x09, 0x06, 0x04, 0x02, 0x01},	/* 26, -13.0dB*/
+	{0x0b, 0x0b, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x01},	/* 27, -13.5dB*/
+	{0x0b, 0x0a, 0x09, 0x08, 0x06, 0x04, 0x02, 0x01},	/* 28, -14.0dB*/
+	{0x0a, 0x0a, 0x09, 0x07, 0x05, 0x03, 0x02, 0x01},	/* 29, -14.5dB*/
+	{0x0a, 0x09, 0x08, 0x07, 0x05, 0x03, 0x02, 0x01},	/* 30, -15.0dB*/
+	{0x09, 0x09, 0x08, 0x06, 0x05, 0x03, 0x01, 0x01},	/* 31, -15.5dB*/
+	{0x09, 0x08, 0x07, 0x06, 0x04, 0x03, 0x01, 0x01}	/* 32, -16.0dB*/
+};
+
+static const u8 cck_tbl_ch14[CCK_TABLE_SIZE][8] = {
+	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00},	/* 0, +0dB */
+	{0x33, 0x32, 0x2b, 0x19, 0x00, 0x00, 0x00, 0x00},	/* 1, -0.5dB */
+	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00},	/* 2, -1.0dB */
+	{0x2d, 0x2d, 0x17, 0x17, 0x00, 0x00, 0x00, 0x00},	/* 3, -1.5dB */
+	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00},	/* 4, -2.0dB */
+	{0x28, 0x28, 0x24, 0x14, 0x00, 0x00, 0x00, 0x00},	/* 5, -2.5dB */
+	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00},	/* 6, -3.0dB */
+	{0x24, 0x23, 0x1f, 0x12, 0x00, 0x00, 0x00, 0x00},	/* 7, -3.5dB */
+	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00},	/* 8, -4.0dB */
+	{0x20, 0x20, 0x1b, 0x10, 0x00, 0x00, 0x00, 0x00},	/* 9, -4.5dB */
+	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00},	/* 10, -5.0dB */
+	{0x1d, 0x1c, 0x18, 0x0e, 0x00, 0x00, 0x00, 0x00},	/* 11, -5.5dB */
+	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00},	/* 12, -6.0dB */
+	{0x1a, 0x19, 0x16, 0x0d, 0x00, 0x00, 0x00, 0x00},	/* 13, -6.5dB */
+	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00},	/* 14, -7.0dB */
+	{0x17, 0x16, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00},	/* 15, -7.5dB */
+	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00},	/* 16, -8.0dB */
+	{0x14, 0x14, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x00},	/* 17, -8.5dB */
+	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00},	/* 18, -9.0dB */
+	{0x12, 0x12, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},	/* 19, -9.5dB */
+	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},	/* 20, -10.0dB*/
+	{0x10, 0x10, 0x0e, 0x08, 0x00, 0x00, 0x00, 0x00},	/* 21, -10.5dB*/
+	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00},	/* 22, -11.0dB*/
+	{0x0e, 0x0e, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00},	/* 23, -11.5dB*/
+	{0x0d, 0x0d, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00},	/* 24, -12.0dB*/
+	{0x0d, 0x0c, 0x0b, 0x06, 0x00, 0x00, 0x00, 0x00},	/* 25, -12.5dB*/
+	{0x0c, 0x0c, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00},	/* 26, -13.0dB*/
+	{0x0b, 0x0b, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00},	/* 27, -13.5dB*/
+	{0x0b, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00},	/* 28, -14.0dB*/
+	{0x0a, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00},	/* 29, -14.5dB*/
+	{0x0a, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00},	/* 30, -15.0dB*/
+	{0x09, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00},	/* 31, -15.5dB*/
+	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00}	/* 32, -16.0dB*/
+};
+
+#define	CAL_SWING_OFF(_off, _dir, _size, _del)				\
+	do {								\
+		for (_off = 0; _off < _size; _off++) {			\
+			if (_del < thermal_threshold[_dir][_off]) {	\
+				if (_off != 0)				\
+					_off--;				\
+				break;					\
+			}						\
+		}							\
+		if (_off >= _size)					\
+			_off = _size - 1;				\
+	} while (0)
+
+static void rtl88e_set_iqk_matrix(struct ieee80211_hw *hw,
+				  u8 ofdm_index, u8 rfpath,
+				  long iqk_result_x, long iqk_result_y)
+{
+	long ele_a = 0, ele_d, ele_c = 0, value32;
+
+	ele_d = (ofdmswing_table[ofdm_index] & 0xFFC00000)>>22;
+
+	if (iqk_result_x != 0) {
+		if ((iqk_result_x & 0x00000200) != 0)
+			iqk_result_x = iqk_result_x | 0xFFFFFC00;
+		ele_a = ((iqk_result_x * ele_d)>>8)&0x000003FF;
+
+		if ((iqk_result_y & 0x00000200) != 0)
+			iqk_result_y = iqk_result_y | 0xFFFFFC00;
+		ele_c = ((iqk_result_y * ele_d)>>8)&0x000003FF;
+
+		switch (rfpath) {
+		case RF90_PATH_A:
+			value32 = (ele_d << 22)|((ele_c & 0x3F)<<16) | ele_a;
+			rtl_set_bbreg(hw, ROFDM0_XATXIQIMBAL, MASKDWORD,
+				      value32);
+			value32 = (ele_c & 0x000003C0) >> 6;
+			rtl_set_bbreg(hw, ROFDM0_XCTXAFE, MASKH4BITS, value32);
+			value32 = ((iqk_result_x * ele_d) >> 7) & 0x01;
+			rtl_set_bbreg(hw, ROFDM0_ECCATHRES, BIT(24), value32);
+			break;
+		case RF90_PATH_B:
+			value32 = (ele_d << 22)|((ele_c & 0x3F)<<16) | ele_a;
+			rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBAL,
+				      MASKDWORD, value32);
+			value32 = (ele_c & 0x000003C0) >> 6;
+			rtl_set_bbreg(hw, ROFDM0_XDTXAFE, MASKH4BITS, value32);
+			value32 = ((iqk_result_x * ele_d) >> 7) & 0x01;
+			rtl_set_bbreg(hw, ROFDM0_ECCATHRES, BIT(28), value32);
+			break;
+		default:
+			break;
+		}
+	} else {
+		switch (rfpath) {
+		case RF90_PATH_A:
+			rtl_set_bbreg(hw, ROFDM0_XATXIQIMBAL, MASKDWORD,
+				      ofdmswing_table[ofdm_index]);
+			rtl_set_bbreg(hw, ROFDM0_XCTXAFE, MASKH4BITS, 0x00);
+			rtl_set_bbreg(hw, ROFDM0_ECCATHRES, BIT(24), 0x00);
+			break;
+		case RF90_PATH_B:
+			rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBAL, MASKDWORD,
+				      ofdmswing_table[ofdm_index]);
+			rtl_set_bbreg(hw, ROFDM0_XDTXAFE, MASKH4BITS, 0x00);
+			rtl_set_bbreg(hw, ROFDM0_ECCATHRES, BIT(28), 0x00);
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+void rtl88e_dm_txpower_track_adjust(struct ieee80211_hw *hw,
+	u8 type, u8 *pdirection, u32 *poutwrite_val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	u8 pwr_val = 0;
+	u8 cck_base = rtldm->swing_idx_cck_base;
+	u8 cck_val = rtldm->swing_idx_cck;
+	u8 ofdm_base = rtldm->swing_idx_ofdm_base;
+	u8 ofdm_val = rtlpriv->dm.swing_idx_ofdm[RF90_PATH_A];
+
+	if (type == 0) {
+		if (ofdm_val <= ofdm_base) {
+			*pdirection = 1;
+			pwr_val = ofdm_base - ofdm_val;
+		} else {
+			*pdirection = 2;
+			pwr_val = ofdm_val - ofdm_base;
+		}
+	} else if (type == 1) {
+		if (cck_val <= cck_base) {
+			*pdirection = 1;
+			pwr_val = cck_base - cck_val;
+		} else {
+			*pdirection = 2;
+			pwr_val = cck_val - cck_base;
+		}
+	}
+
+	if (pwr_val >= TXPWRTRACK_MAX_IDX && (*pdirection == 1))
+		pwr_val = TXPWRTRACK_MAX_IDX;
+
+	*poutwrite_val = pwr_val | (pwr_val << 8) | (pwr_val << 16) |
+			 (pwr_val << 24);
+}
+
+
+static void rtl88e_chk_tx_track(struct ieee80211_hw *hw,
+				enum pwr_track_control_method method,
+				u8 rfpath, u8 index)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	int jj = rtldm->swing_idx_cck;
+	int i;
+
+	if (method == TXAGC) {
+		if (rtldm->swing_flag_ofdm == true ||
+		    rtldm->swing_flag_cck == true) {
+			u8 chan = rtlphy->current_channel;
+			rtl88e_phy_set_txpower_level(hw, chan);
+			rtldm->swing_flag_ofdm = false;
+			rtldm->swing_flag_cck = false;
+		}
+	} else if (method == BBSWING) {
+		if (!rtldm->cck_inch14) {
+			for (i = 0; i < 8; i++)
+				rtl_write_byte(rtlpriv, 0xa22 + i,
+					       cck_tbl_ch1_13[jj][i]);
+		} else {
+			for (i = 0; i < 8; i++)
+				rtl_write_byte(rtlpriv, 0xa22 + i,
+					       cck_tbl_ch14[jj][i]);
+		}
+
+		if (rfpath == RF90_PATH_A) {
+			long x = rtlphy->iqk_matrix[index].value[0][0];
+			long y = rtlphy->iqk_matrix[index].value[0][1];
+			u8 indx = rtldm->swing_idx_ofdm[rfpath];
+			rtl88e_set_iqk_matrix(hw, indx, rfpath, x, y);
+		} else if (rfpath == RF90_PATH_B) {
+			u8 indx = rtldm->swing_idx_ofdm[rfpath];
+			long x = rtlphy->iqk_matrix[indx].value[0][4];
+			long y = rtlphy->iqk_matrix[indx].value[0][5];
+			rtl88e_set_iqk_matrix(hw, indx, rfpath, x, y);
+		}
+	} else {
+		return;
+	}
+}
+
+static void rtl88e_dm_diginit(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
+
+	dm_dig->dig_enable_flag = true;
+	dm_dig->cur_igvalue = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f);
+	dm_dig->pre_igvalue = 0;
+	dm_dig->cursta_cstate = DIG_STA_DISCONNECT;
+	dm_dig->presta_cstate = DIG_STA_DISCONNECT;
+	dm_dig->curmultista_cstate = DIG_MULTISTA_DISCONNECT;
+	dm_dig->rssi_lowthresh = DM_DIG_THRESH_LOW;
+	dm_dig->rssi_highthresh = DM_DIG_THRESH_HIGH;
+	dm_dig->fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
+	dm_dig->fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
+	dm_dig->rx_gain_max = DM_DIG_MAX;
+	dm_dig->rx_gain_min = DM_DIG_MIN;
+	dm_dig->back_val = DM_DIG_BACKOFF_DEFAULT;
+	dm_dig->back_range_max = DM_DIG_BACKOFF_MAX;
+	dm_dig->back_range_min = DM_DIG_BACKOFF_MIN;
+	dm_dig->pre_cck_cca_thres = 0xff;
+	dm_dig->cur_cck_cca_thres = 0x83;
+	dm_dig->forbidden_igi = DM_DIG_MIN;
+	dm_dig->large_fa_hit = 0;
+	dm_dig->recover_cnt = 0;
+	dm_dig->dig_min_0 = 0x25;
+	dm_dig->dig_min_1 = 0x25;
+	dm_dig->media_connect_0 = false;
+	dm_dig->media_connect_1 = false;
+	rtlpriv->dm.dm_initialgain_enable = true;
+}
+
+static u8 rtl88e_dm_initial_gain_min_pwdb(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
+	long rssi_val_min = 0;
+
+	if ((dm_dig->curmultista_cstate == DIG_MULTISTA_CONNECT) &&
+	    (dm_dig->cursta_cstate == DIG_STA_CONNECT)) {
+		if (rtlpriv->dm.entry_min_undec_sm_pwdb != 0)
+			rssi_val_min =
+			    (rtlpriv->dm.entry_min_undec_sm_pwdb >
+			    rtlpriv->dm.undec_sm_pwdb) ?
+			    rtlpriv->dm.undec_sm_pwdb :
+			    rtlpriv->dm.entry_min_undec_sm_pwdb;
+		else
+			rssi_val_min = rtlpriv->dm.undec_sm_pwdb;
+	} else if (dm_dig->cursta_cstate == DIG_STA_CONNECT ||
+		   dm_dig->cursta_cstate == DIG_STA_BEFORE_CONNECT) {
+		rssi_val_min = rtlpriv->dm.undec_sm_pwdb;
+	} else if (dm_dig->curmultista_cstate ==
+		DIG_MULTISTA_CONNECT) {
+		rssi_val_min = rtlpriv->dm.entry_min_undec_sm_pwdb;
+	}
+	return (u8)rssi_val_min;
+}
+
+static void rtl88e_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
+{
+	u32 ret_value;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct false_alarm_statistics *alm_cnt = &(rtlpriv->falsealm_cnt);
+
+	rtl_set_bbreg(hw, ROFDM0_LSTF, BIT(31), 1);
+	rtl_set_bbreg(hw, ROFDM1_LSTF, BIT(31), 1);
+
+	ret_value = rtl_get_bbreg(hw, ROFDM0_FRAMESYNC, MASKDWORD);
+	alm_cnt->cnt_fast_fsync_fail = (ret_value&0xffff);
+	alm_cnt->cnt_sb_search_fail = ((ret_value&0xffff0000)>>16);
+
+	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER1, MASKDWORD);
+	alm_cnt->cnt_ofdm_cca = (ret_value&0xffff);
+	alm_cnt->cnt_parity_fail = ((ret_value & 0xffff0000) >> 16);
+
+	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER2, MASKDWORD);
+	alm_cnt->cnt_rate_illegal = (ret_value & 0xffff);
+	alm_cnt->cnt_crc8_fail = ((ret_value & 0xffff0000) >> 16);
+
+	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER3, MASKDWORD);
+	alm_cnt->cnt_mcs_fail = (ret_value & 0xffff);
+	alm_cnt->cnt_ofdm_fail = alm_cnt->cnt_parity_fail +
+				 alm_cnt->cnt_rate_illegal +
+				 alm_cnt->cnt_crc8_fail +
+				 alm_cnt->cnt_mcs_fail +
+				 alm_cnt->cnt_fast_fsync_fail +
+				 alm_cnt->cnt_sb_search_fail;
+
+	ret_value = rtl_get_bbreg(hw, REG_SC_CNT, MASKDWORD);
+	alm_cnt->cnt_bw_lsc = (ret_value & 0xffff);
+	alm_cnt->cnt_bw_usc = ((ret_value & 0xffff0000) >> 16);
+
+	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, BIT(12), 1);
+	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, BIT(14), 1);
+
+	ret_value = rtl_get_bbreg(hw, RCCK0_FACOUNTERLOWER, MASKBYTE0);
+	alm_cnt->cnt_cck_fail = ret_value;
+
+	ret_value = rtl_get_bbreg(hw, RCCK0_FACOUNTERUPPER, MASKBYTE3);
+	alm_cnt->cnt_cck_fail += (ret_value & 0xff) << 8;
+
+	ret_value = rtl_get_bbreg(hw, RCCK0_CCA_CNT, MASKDWORD);
+	alm_cnt->cnt_cck_cca = ((ret_value & 0xff) << 8) |
+				((ret_value&0xFF00)>>8);
+
+	alm_cnt->cnt_all = alm_cnt->cnt_fast_fsync_fail +
+			   alm_cnt->cnt_sb_search_fail +
+			   alm_cnt->cnt_parity_fail +
+			   alm_cnt->cnt_rate_illegal +
+			   alm_cnt->cnt_crc8_fail +
+			   alm_cnt->cnt_mcs_fail +
+			   alm_cnt->cnt_cck_fail;
+	alm_cnt->cnt_cca_all = alm_cnt->cnt_ofdm_cca + alm_cnt->cnt_cck_cca;
+
+	rtl_set_bbreg(hw, ROFDM0_TRSWISOLATION, BIT(31), 1);
+	rtl_set_bbreg(hw, ROFDM0_TRSWISOLATION, BIT(31), 0);
+	rtl_set_bbreg(hw, ROFDM1_LSTF, BIT(27), 1);
+	rtl_set_bbreg(hw, ROFDM1_LSTF, BIT(27), 0);
+	rtl_set_bbreg(hw, ROFDM0_LSTF, BIT(31), 0);
+	rtl_set_bbreg(hw, ROFDM1_LSTF, BIT(31), 0);
+	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, BIT(13)|BIT(12), 0);
+	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, BIT(13)|BIT(12), 2);
+	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, BIT(15)|BIT(14), 0);
+	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, BIT(15)|BIT(14), 2);
+
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
+		 "cnt_parity_fail = %d, cnt_rate_illegal = %d, "
+		 "cnt_crc8_fail = %d, cnt_mcs_fail = %d\n",
+		 alm_cnt->cnt_parity_fail,
+		 alm_cnt->cnt_rate_illegal,
+		 alm_cnt->cnt_crc8_fail, alm_cnt->cnt_mcs_fail);
+
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
+		 "cnt_ofdm_fail = %x, cnt_cck_fail = %x, cnt_all = %x\n",
+		 alm_cnt->cnt_ofdm_fail,
+		 alm_cnt->cnt_cck_fail, alm_cnt->cnt_all);
+}
+
+static void rtl88e_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
+	u8 cur_cck_cca_thresh;
+
+	if (dm_dig->cursta_cstate == DIG_STA_CONNECT) {
+		dm_dig->rssi_val_min = rtl88e_dm_initial_gain_min_pwdb(hw);
+		if (dm_dig->rssi_val_min > 25) {
+			cur_cck_cca_thresh = 0xcd;
+		} else if ((dm_dig->rssi_val_min <= 25) &&
+			   (dm_dig->rssi_val_min > 10)) {
+			cur_cck_cca_thresh = 0x83;
+		} else {
+			if (rtlpriv->falsealm_cnt.cnt_cck_fail > 1000)
+				cur_cck_cca_thresh = 0x83;
+			else
+				cur_cck_cca_thresh = 0x40;
+		}
+
+	} else {
+		if (rtlpriv->falsealm_cnt.cnt_cck_fail > 1000)
+			cur_cck_cca_thresh = 0x83;
+		else
+			cur_cck_cca_thresh = 0x40;
+	}
+
+	if (dm_dig->cur_cck_cca_thres != cur_cck_cca_thresh)
+		rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2, cur_cck_cca_thresh);
+
+	dm_dig->cur_cck_cca_thres = cur_cck_cca_thresh;
+	dm_dig->pre_cck_cca_thres = dm_dig->cur_cck_cca_thres;
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
+		 "CCK cca thresh hold =%x\n", dm_dig->cur_cck_cca_thres);
+}
+
+static void rtl88e_dm_dig(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 dig_min, dig_maxofmin;
+	bool bfirstconnect;
+	u8 dm_dig_max, dm_dig_min;
+	u8 current_igi = dm_dig->cur_igvalue;
+
+	if (rtlpriv->dm.dm_initialgain_enable == false)
+		return;
+	if (dm_dig->dig_enable_flag == false)
+		return;
+	if (mac->act_scanning == true)
+		return;
+
+	if (mac->link_state >= MAC80211_LINKED)
+		dm_dig->cursta_cstate = DIG_STA_CONNECT;
+	else
+		dm_dig->cursta_cstate = DIG_STA_DISCONNECT;
+	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP ||
+	    rtlpriv->mac80211.opmode == NL80211_IFTYPE_ADHOC)
+		dm_dig->cursta_cstate = DIG_STA_DISCONNECT;
+
+	dm_dig_max = DM_DIG_MAX;
+	dm_dig_min = DM_DIG_MIN;
+	dig_maxofmin = DM_DIG_MAX_AP;
+	dig_min = dm_dig->dig_min_0;
+	bfirstconnect = ((mac->link_state >= MAC80211_LINKED) ? true : false) &&
+			 (dm_dig->media_connect_0 == false);
+
+	dm_dig->rssi_val_min =
+		rtl88e_dm_initial_gain_min_pwdb(hw);
+
+	if (mac->link_state >= MAC80211_LINKED) {
+		if ((dm_dig->rssi_val_min + 20) > dm_dig_max)
+			dm_dig->rx_gain_max = dm_dig_max;
+		else if ((dm_dig->rssi_val_min + 20) < dm_dig_min)
+			dm_dig->rx_gain_max = dm_dig_min;
+		else
+			dm_dig->rx_gain_max = dm_dig->rssi_val_min + 20;
+
+		if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) {
+			dig_min  = dm_dig->antdiv_rssi_max;
+		} else {
+			if (dm_dig->rssi_val_min < dm_dig_min)
+				dig_min = dm_dig_min;
+			else if (dm_dig->rssi_val_min < dig_maxofmin)
+				dig_min = dig_maxofmin;
+			else
+				dig_min = dm_dig->rssi_val_min;
+		}
+	} else {
+		dm_dig->rx_gain_max = dm_dig_max;
+		dig_min = dm_dig_min;
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "no link\n");
+	}
+
+	if (rtlpriv->falsealm_cnt.cnt_all > 10000) {
+		dm_dig->large_fa_hit++;
+		if (dm_dig->forbidden_igi < current_igi) {
+			dm_dig->forbidden_igi = current_igi;
+			dm_dig->large_fa_hit = 1;
+		}
+
+		if (dm_dig->large_fa_hit >= 3) {
+			if ((dm_dig->forbidden_igi + 1) > dm_dig->rx_gain_max)
+				dm_dig->rx_gain_min = dm_dig->rx_gain_max;
+			else
+				dm_dig->rx_gain_min = dm_dig->forbidden_igi + 1;
+			dm_dig->recover_cnt = 3600;
+		}
+	} else {
+		if (dm_dig->recover_cnt != 0) {
+			dm_dig->recover_cnt--;
+		} else {
+			if (dm_dig->large_fa_hit == 0) {
+				if ((dm_dig->forbidden_igi - 1) < dig_min) {
+					dm_dig->forbidden_igi = dig_min;
+					dm_dig->rx_gain_min = dig_min;
+				} else {
+					dm_dig->forbidden_igi--;
+					dm_dig->rx_gain_min =
+						 dm_dig->forbidden_igi + 1;
+				}
+			} else if (dm_dig->large_fa_hit == 3) {
+				dm_dig->large_fa_hit = 0;
+			}
+		}
+	}
+
+	if (dm_dig->cursta_cstate == DIG_STA_CONNECT) {
+		if (bfirstconnect) {
+			current_igi = dm_dig->rssi_val_min;
+		} else {
+			if (rtlpriv->falsealm_cnt.cnt_all > DM_DIG_FA_TH2)
+				current_igi += 2;
+			else if (rtlpriv->falsealm_cnt.cnt_all > DM_DIG_FA_TH1)
+				current_igi++;
+			else if (rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH0)
+				current_igi--;
+		}
+	} else {
+		if (rtlpriv->falsealm_cnt.cnt_all > 10000)
+			current_igi += 2;
+		else if (rtlpriv->falsealm_cnt.cnt_all > 8000)
+			current_igi++;
+		else if (rtlpriv->falsealm_cnt.cnt_all < 500)
+			current_igi--;
+	}
+
+	if (current_igi > DM_DIG_FA_UPPER)
+		current_igi = DM_DIG_FA_UPPER;
+	else if (current_igi < DM_DIG_FA_LOWER)
+		current_igi = DM_DIG_FA_LOWER;
+
+	if (rtlpriv->falsealm_cnt.cnt_all > 10000)
+		current_igi = DM_DIG_FA_UPPER;
+
+	dm_dig->cur_igvalue = current_igi;
+	rtl88e_dm_write_dig(hw);
+	dm_dig->media_connect_0 = ((mac->link_state >= MAC80211_LINKED) ?
+				    true : false);
+	dm_dig->dig_min_0 = dig_min;
+
+	rtl88e_dm_cck_packet_detection_thresh(hw);
+}
+
+static void rtl88e_dm_init_dynamic_txpower(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.dynamic_txpower_enable = false;
+
+	rtlpriv->dm.last_dtp_lvl = TXHIGHPWRLEVEL_NORMAL;
+	rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
+}
+
+static void rtl92c_dm_dynamic_txpower(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	long undec_sm_pwdb;
+
+	if (!rtlpriv->dm.dynamic_txpower_enable)
+		return;
+
+	if (rtlpriv->dm.dm_flag & HAL_DM_HIPWR_DISABLE) {
+		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
+		return;
+	}
+
+	if ((mac->link_state < MAC80211_LINKED) &&
+	    (rtlpriv->dm.entry_min_undec_sm_pwdb == 0)) {
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
+			 "Not connected\n");
+
+		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
+
+		rtlpriv->dm.last_dtp_lvl = TXHIGHPWRLEVEL_NORMAL;
+		return;
+	}
+
+	if (mac->link_state >= MAC80211_LINKED) {
+		if (mac->opmode == NL80211_IFTYPE_ADHOC) {
+			undec_sm_pwdb =
+			    rtlpriv->dm.entry_min_undec_sm_pwdb;
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+				 "AP Client PWDB = 0x%lx\n",
+				  undec_sm_pwdb);
+		} else {
+			undec_sm_pwdb =
+			    rtlpriv->dm.undec_sm_pwdb;
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+				 "STA Default Port PWDB = 0x%lx\n",
+				  undec_sm_pwdb);
+		}
+	} else {
+		undec_sm_pwdb = rtlpriv->dm.entry_min_undec_sm_pwdb;
+
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 "AP Ext Port PWDB = 0x%lx\n", undec_sm_pwdb);
+	}
+
+	if (undec_sm_pwdb >= TX_POWER_NEAR_FIELD_THRESH_LVL2) {
+		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_LEVEL1;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 "TXHIGHPWRLEVEL_LEVEL1 (TxPwr = 0x0)\n");
+	} else if ((undec_sm_pwdb <
+		    (TX_POWER_NEAR_FIELD_THRESH_LVL2 - 3)) &&
+		   (undec_sm_pwdb >= TX_POWER_NEAR_FIELD_THRESH_LVL1)) {
+		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_LEVEL1;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 "TXHIGHPWRLEVEL_LEVEL1 (TxPwr = 0x10)\n");
+	} else if (undec_sm_pwdb < (TX_POWER_NEAR_FIELD_THRESH_LVL1 - 5)) {
+		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 "TXHIGHPWRLEVEL_NORMAL\n");
+	}
+
+	if ((rtlpriv->dm.dynamic_txhighpower_lvl != rtlpriv->dm.last_dtp_lvl)) {
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 "PHY_SetTxPowerLevel8192S() Channel = %d\n",
+			  rtlphy->current_channel);
+		rtl88e_phy_set_txpower_level(hw, rtlphy->current_channel);
+	}
+
+	rtlpriv->dm.last_dtp_lvl = rtlpriv->dm.dynamic_txhighpower_lvl;
+}
+
+void rtl88e_dm_write_dig(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
+
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+		 "cur_igvalue = 0x%x, "
+		  "pre_igvalue = 0x%x, back_val = %d\n",
+		  dm_dig->cur_igvalue, dm_dig->pre_igvalue,
+		  dm_dig->back_val);
+
+	if (dm_dig->cur_igvalue > 0x3f)
+		dm_dig->cur_igvalue = 0x3f;
+	if (dm_dig->pre_igvalue != dm_dig->cur_igvalue) {
+		rtl_set_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f,
+			      dm_dig->cur_igvalue);
+
+		dm_dig->pre_igvalue = dm_dig->cur_igvalue;
+	}
+}
+
+static void rtl88e_dm_pwdb_monitor(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_sta_info *drv_priv;
+	static u64 last_txok;
+	static u64 last_rx;
+	long tmp_entry_max_pwdb = 0, tmp_entry_min_pwdb = 0xff;
+
+	if (rtlhal->oem_id == RT_CID_819x_HP) {
+		u64 cur_txok_cnt = 0;
+		u64 cur_rxok_cnt = 0;
+		cur_txok_cnt = rtlpriv->stats.txbytesunicast - last_txok;
+		cur_rxok_cnt = rtlpriv->stats.rxbytesunicast - last_rx;
+		last_txok = cur_txok_cnt;
+		last_rx = cur_rxok_cnt;
+
+		if (cur_rxok_cnt > (cur_txok_cnt * 6))
+			rtl_write_dword(rtlpriv, REG_ARFR0, 0x8f015);
+		else
+			rtl_write_dword(rtlpriv, REG_ARFR0, 0xff015);
+	}
+
+	/* AP & ADHOC & MESH */
+	spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+	list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
+		if (drv_priv->rssi_stat.undec_sm_pwdb < tmp_entry_min_pwdb)
+			tmp_entry_min_pwdb = drv_priv->rssi_stat.undec_sm_pwdb;
+		if (drv_priv->rssi_stat.undec_sm_pwdb > tmp_entry_max_pwdb)
+			tmp_entry_max_pwdb = drv_priv->rssi_stat.undec_sm_pwdb;
+	}
+	spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+
+	/* If associated entry is found */
+	if (tmp_entry_max_pwdb != 0) {
+		rtlpriv->dm.entry_max_undec_sm_pwdb = tmp_entry_max_pwdb;
+		RTPRINT(rtlpriv, FDM, DM_PWDB, "EntryMaxPWDB = 0x%lx(%ld)\n",
+			tmp_entry_max_pwdb, tmp_entry_max_pwdb);
+	} else {
+		rtlpriv->dm.entry_max_undec_sm_pwdb = 0;
+	}
+	/* If associated entry is found */
+	if (tmp_entry_min_pwdb != 0xff) {
+		rtlpriv->dm.entry_min_undec_sm_pwdb = tmp_entry_min_pwdb;
+		RTPRINT(rtlpriv, FDM, DM_PWDB, "EntryMinPWDB = 0x%lx(%ld)\n",
+			tmp_entry_min_pwdb, tmp_entry_min_pwdb);
+	} else {
+		rtlpriv->dm.entry_min_undec_sm_pwdb = 0;
+	}
+	/* Indicate Rx signal strength to FW. */
+	if (!rtlpriv->dm.useramask)
+		rtl_write_byte(rtlpriv, 0x4fe, rtlpriv->dm.undec_sm_pwdb);
+}
+
+void rtl88e_dm_init_edca_turbo(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.current_turbo_edca = false;
+	rtlpriv->dm.is_any_nonbepkts = false;
+	rtlpriv->dm.is_cur_rdlstate = false;
+}
+
+static void rtl88e_dm_check_edca_turbo(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	static u64 last_txok_cnt;
+	static u64 last_rxok_cnt;
+	static u32 last_bt_edca_ul;
+	static u32 last_bt_edca_dl;
+	u64 cur_txok_cnt = 0;
+	u64 cur_rxok_cnt = 0;
+	u32 edca_be_ul = 0x5ea42b;
+	u32 edca_be_dl = 0x5ea42b;
+	bool change_edca = false;
+
+	if ((last_bt_edca_ul != rtlpcipriv->bt_coexist.bt_edca_ul) ||
+	    (last_bt_edca_dl != rtlpcipriv->bt_coexist.bt_edca_dl)) {
+		rtlpriv->dm.current_turbo_edca = false;
+		last_bt_edca_ul = rtlpcipriv->bt_coexist.bt_edca_ul;
+		last_bt_edca_dl = rtlpcipriv->bt_coexist.bt_edca_dl;
+	}
+
+	if (rtlpcipriv->bt_coexist.bt_edca_ul != 0) {
+		edca_be_ul = rtlpcipriv->bt_coexist.bt_edca_ul;
+		change_edca = true;
+	}
+
+	if (rtlpcipriv->bt_coexist.bt_edca_dl != 0) {
+		edca_be_ul = rtlpcipriv->bt_coexist.bt_edca_dl;
+		change_edca = true;
+	}
+
+	if (mac->link_state != MAC80211_LINKED) {
+		rtlpriv->dm.current_turbo_edca = false;
+		return;
+	}
+
+	if ((!mac->ht_enable) && (!rtlpcipriv->bt_coexist.bt_coexistence)) {
+		if (!(edca_be_ul & 0xffff0000))
+			edca_be_ul |= 0x005e0000;
+
+		if (!(edca_be_dl & 0xffff0000))
+			edca_be_dl |= 0x005e0000;
+	}
+
+	if ((change_edca) || ((!rtlpriv->dm.is_any_nonbepkts) &&
+			      (!rtlpriv->dm.disable_framebursting))) {
+		cur_txok_cnt = rtlpriv->stats.txbytesunicast - last_txok_cnt;
+		cur_rxok_cnt = rtlpriv->stats.rxbytesunicast - last_rxok_cnt;
+
+		if (cur_rxok_cnt > 4 * cur_txok_cnt) {
+			if (!rtlpriv->dm.is_cur_rdlstate ||
+			    !rtlpriv->dm.current_turbo_edca) {
+				rtl_write_dword(rtlpriv,
+						REG_EDCA_BE_PARAM,
+						edca_be_dl);
+				rtlpriv->dm.is_cur_rdlstate = true;
+			}
+		} else {
+			if (rtlpriv->dm.is_cur_rdlstate ||
+			    !rtlpriv->dm.current_turbo_edca) {
+				rtl_write_dword(rtlpriv,
+						REG_EDCA_BE_PARAM,
+						edca_be_ul);
+				rtlpriv->dm.is_cur_rdlstate = false;
+			}
+		}
+		rtlpriv->dm.current_turbo_edca = true;
+	} else {
+		if (rtlpriv->dm.current_turbo_edca) {
+			u8 tmp = AC0_BE;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_AC_PARAM,
+						      (u8 *)(&tmp));
+			rtlpriv->dm.current_turbo_edca = false;
+		}
+	}
+
+	rtlpriv->dm.is_any_nonbepkts = false;
+	last_txok_cnt = rtlpriv->stats.txbytesunicast;
+	last_rxok_cnt = rtlpriv->stats.rxbytesunicast;
+}
+
+static void rtl88e_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
+							     *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 thermalvalue = 0, delta, delta_lck, delta_iqk, off;
+	u8 th_avg_cnt = 0;
+	u32 thermalvalue_avg = 0;
+	long  ele_d, temp_cck;
+	char ofdm_index[2], cck_index = 0, ofdm_old[2] = {0, 0}, cck_old = 0;
+	int i = 0;
+	bool is2t = false;
+
+	u8 ofdm_min_index = 6, rf = (is2t) ? 2 : 1;
+	u8 index_for_channel;
+	enum _dec_inc {dec, power_inc};
+
+	/* 0.1 the following TWO tables decide the final index of
+	 * OFDM/CCK swing table
+	 */
+	char del_tbl_idx[2][15] = {
+		{0, 0, 2, 3, 4, 4, 5, 6, 7, 7, 8, 9, 10, 10, 11},
+		{0, 0, -1, -2, -3, -4, -4, -4, -4, -5, -7, -8, -9, -9, -10}
+	};
+	u8 thermal_threshold[2][15] = {
+		{0, 2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24, 26, 27},
+		{0, 2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 25, 25, 25}
+	};
+
+	/*Initilization (7 steps in total) */
+	rtlpriv->dm.txpower_trackinginit = true;
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "rtl88e_dm_txpower_tracking_callback_thermalmeter\n");
+
+	thermalvalue = (u8) rtl_get_rfreg(hw, RF90_PATH_A, RF_T_METER, 0xfc00);
+	if (!thermalvalue)
+		return;
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "Readback Thermal Meter = 0x%x pre thermal meter 0x%x eeprom_thermalmeter 0x%x\n",
+		 thermalvalue, rtlpriv->dm.thermalvalue,
+		 rtlefuse->eeprom_thermalmeter);
+
+	/*1. Query OFDM Default Setting: Path A*/
+	ele_d = rtl_get_bbreg(hw, ROFDM0_XATXIQIMBAL, MASKDWORD) & MASKOFDM_D;
+	for (i = 0; i < OFDM_TABLE_LENGTH; i++) {
+		if (ele_d == (ofdmswing_table[i] & MASKOFDM_D)) {
+			ofdm_old[0] = (u8) i;
+			rtldm->swing_idx_ofdm_base = (u8)i;
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "Initial pathA ele_d reg0x%x = 0x%lx, ofdm_index = 0x%x\n",
+				 ROFDM0_XATXIQIMBAL,
+				 ele_d, ofdm_old[0]);
+			break;
+		}
+	}
+
+	if (is2t) {
+		ele_d = rtl_get_bbreg(hw, ROFDM0_XBTXIQIMBAL,
+				      MASKDWORD) & MASKOFDM_D;
+		for (i = 0; i < OFDM_TABLE_LENGTH; i++) {
+			if (ele_d == (ofdmswing_table[i] & MASKOFDM_D)) {
+				ofdm_old[1] = (u8)i;
+
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING,
+					 DBG_LOUD,
+					 "Initial pathB ele_d reg0x%x = 0x%lx, ofdm_index = 0x%x\n",
+					 ROFDM0_XBTXIQIMBAL, ele_d,
+					 ofdm_old[1]);
+				break;
+			}
+		}
+	}
+	/*2.Query CCK default setting From 0xa24*/
+	temp_cck = rtl_get_bbreg(hw, RCCK0_TXFILTER2, MASKDWORD) & MASKCCK;
+	for (i = 0; i < CCK_TABLE_LENGTH; i++) {
+		if (rtlpriv->dm.cck_inch14) {
+			if (memcmp(&temp_cck, &cck_tbl_ch14[i][2], 4) == 0) {
+				cck_old = (u8)i;
+				rtldm->swing_idx_cck_base = (u8)i;
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					 "Initial reg0x%x = 0x%lx, cck_index = 0x%x, ch 14 %d\n",
+					 RCCK0_TXFILTER2, temp_cck, cck_old,
+					 rtlpriv->dm.cck_inch14);
+				break;
+			}
+		} else {
+			if (memcmp(&temp_cck, &cck_tbl_ch1_13[i][2], 4) == 0) {
+				cck_old = (u8)i;
+				rtldm->swing_idx_cck_base = (u8)i;
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					 "Initial reg0x%x = 0x%lx, cck_index = 0x%x, ch14 %d\n",
+					 RCCK0_TXFILTER2, temp_cck, cck_old,
+					 rtlpriv->dm.cck_inch14);
+				break;
+			}
+		}
+	}
+
+	/*3 Initialize ThermalValues of RFCalibrateInfo*/
+	if (!rtldm->thermalvalue) {
+		rtlpriv->dm.thermalvalue = rtlefuse->eeprom_thermalmeter;
+		rtlpriv->dm.thermalvalue_lck = thermalvalue;
+		rtlpriv->dm.thermalvalue_iqk = thermalvalue;
+		for (i = 0; i < rf; i++)
+			rtlpriv->dm.ofdm_index[i] = ofdm_old[i];
+		rtlpriv->dm.cck_index = cck_old;
+	}
+
+	/*4 Calculate average thermal meter*/
+	rtldm->thermalvalue_avg[rtldm->thermalvalue_avg_index] = thermalvalue;
+	rtldm->thermalvalue_avg_index++;
+	if (rtldm->thermalvalue_avg_index == AVG_THERMAL_NUM_88E)
+		rtldm->thermalvalue_avg_index = 0;
+
+	for (i = 0; i < AVG_THERMAL_NUM_88E; i++) {
+		if (rtldm->thermalvalue_avg[i]) {
+			thermalvalue_avg += rtldm->thermalvalue_avg[i];
+			th_avg_cnt++;
+		}
+	}
+
+	if (th_avg_cnt)
+		thermalvalue = (u8)(thermalvalue_avg / th_avg_cnt);
+
+	/* 5 Calculate delta, delta_LCK, delta_IQK.*/
+	if (rtlhal->reloadtxpowerindex) {
+		delta = (thermalvalue > rtlefuse->eeprom_thermalmeter) ?
+		    (thermalvalue - rtlefuse->eeprom_thermalmeter) :
+		    (rtlefuse->eeprom_thermalmeter - thermalvalue);
+		rtlhal->reloadtxpowerindex = false;
+		rtlpriv->dm.done_txpower = false;
+	} else if (rtlpriv->dm.done_txpower) {
+		delta = (thermalvalue > rtlpriv->dm.thermalvalue) ?
+			(thermalvalue - rtlpriv->dm.thermalvalue) :
+			(rtlpriv->dm.thermalvalue - thermalvalue);
+	} else {
+		delta = (thermalvalue > rtlefuse->eeprom_thermalmeter) ?
+			(thermalvalue - rtlefuse->eeprom_thermalmeter) :
+			(rtlefuse->eeprom_thermalmeter - thermalvalue);
+	}
+	delta_lck = (thermalvalue > rtlpriv->dm.thermalvalue_lck) ?
+		    (thermalvalue - rtlpriv->dm.thermalvalue_lck) :
+		    (rtlpriv->dm.thermalvalue_lck - thermalvalue);
+	delta_iqk = (thermalvalue > rtlpriv->dm.thermalvalue_iqk) ?
+		    (thermalvalue - rtlpriv->dm.thermalvalue_iqk) :
+		    (rtlpriv->dm.thermalvalue_iqk - thermalvalue);
+
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "Readback Thermal Meter = 0x%x pre thermal meter 0x%x "
+		 "eeprom_thermalmeter 0x%x delta 0x%x "
+		 "delta_lck 0x%x delta_iqk 0x%x\n",
+		 thermalvalue, rtlpriv->dm.thermalvalue,
+		 rtlefuse->eeprom_thermalmeter, delta, delta_lck,
+		 delta_iqk);
+	/* 6 If necessary, do LCK.*/
+	if (delta_lck >= 8) {
+		rtlpriv->dm.thermalvalue_lck = thermalvalue;
+		rtl88e_phy_lc_calibrate(hw);
+	}
+
+	/* 7 If necessary, move the index of swing table to adjust Tx power. */
+	if (delta > 0 && rtlpriv->dm.txpower_track_control) {
+		delta = (thermalvalue > rtlefuse->eeprom_thermalmeter) ?
+			(thermalvalue - rtlefuse->eeprom_thermalmeter) :
+			(rtlefuse->eeprom_thermalmeter - thermalvalue);
+
+		/* 7.1 Get the final CCK_index and OFDM_index for each
+		 * swing table.
+		 */
+		if (thermalvalue > rtlefuse->eeprom_thermalmeter) {
+			CAL_SWING_OFF(off, power_inc, IDX_MAP, delta);
+			for (i = 0; i < rf; i++)
+				ofdm_index[i] = rtldm->ofdm_index[i] +
+						del_tbl_idx[power_inc][off];
+			cck_index = rtldm->cck_index +
+				    del_tbl_idx[power_inc][off];
+		} else {
+			CAL_SWING_OFF(off, dec, IDX_MAP, delta);
+			for (i = 0; i < rf; i++)
+				ofdm_index[i] = rtldm->ofdm_index[i] +
+						del_tbl_idx[dec][off];
+			cck_index = rtldm->cck_index + del_tbl_idx[dec][off];
+		}
+
+		/* 7.2 Handle boundary conditions of index.*/
+		for (i = 0; i < rf; i++) {
+			if (ofdm_index[i] > OFDM_TABLE_SIZE-1)
+				ofdm_index[i] = OFDM_TABLE_SIZE-1;
+			else if (rtldm->ofdm_index[i] < ofdm_min_index)
+				ofdm_index[i] = ofdm_min_index;
+		}
+
+		if (cck_index > CCK_TABLE_SIZE - 1)
+			cck_index = CCK_TABLE_SIZE - 1;
+		else if (cck_index < 0)
+			cck_index = 0;
+
+		/*7.3Configure the Swing Table to adjust Tx Power.*/
+		if (rtlpriv->dm.txpower_track_control) {
+			rtldm->done_txpower = true;
+			rtldm->swing_idx_ofdm[RF90_PATH_A] =
+				 (u8)ofdm_index[RF90_PATH_A];
+			if (is2t)
+				rtldm->swing_idx_ofdm[RF90_PATH_B] =
+					 (u8)ofdm_index[RF90_PATH_B];
+			rtldm->swing_idx_cck = cck_index;
+			if (rtldm->swing_idx_ofdm_cur !=
+			    rtldm->swing_idx_ofdm[0]) {
+				rtldm->swing_idx_ofdm_cur =
+					 rtldm->swing_idx_ofdm[0];
+				rtldm->swing_flag_ofdm = true;
+			}
+
+			if (rtldm->swing_idx_cck != rtldm->swing_idx_cck) {
+				rtldm->swing_idx_cck_cur = rtldm->swing_idx_cck;
+				rtldm->swing_flag_cck = true;
+			}
+
+			rtl88e_chk_tx_track(hw, TXAGC, 0, 0);
+
+			if (is2t)
+				rtl88e_chk_tx_track(hw, BBSWING,
+						    RF90_PATH_B,
+						    index_for_channel);
+		}
+	}
+
+	if (delta_iqk >= 8) {
+		rtlpriv->dm.thermalvalue_iqk = thermalvalue;
+		rtl88e_phy_iq_calibrate(hw, false);
+	}
+
+	if (rtldm->txpower_track_control)
+		rtldm->thermalvalue = thermalvalue;
+	rtldm->txpowercount = 0;
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD, "end\n");
+}
+
+static void rtl88e_dm_init_txpower_tracking(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.txpower_tracking = true;
+	rtlpriv->dm.txpower_trackinginit = false;
+	rtlpriv->dm.txpowercount = 0;
+	rtlpriv->dm.txpower_track_control = true;
+
+	rtlpriv->dm.swing_idx_ofdm[RF90_PATH_A] = 12;
+	rtlpriv->dm.swing_idx_ofdm_cur = 12;
+	rtlpriv->dm.swing_flag_ofdm = false;
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "  rtlpriv->dm.txpower_tracking = %d\n",
+		 rtlpriv->dm.txpower_tracking);
+}
+
+void rtl88e_dm_check_txpower_tracking(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	static u8 tm_trigger;
+
+	if (!rtlpriv->dm.txpower_tracking)
+		return;
+
+	if (!tm_trigger) {
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER, BIT(17)|BIT(16),
+			      0x03);
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "Trigger 88E Thermal Meter!!\n");
+		tm_trigger = 1;
+		return;
+	} else {
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "Schedule TxPowerTracking !!\n");
+		rtl88e_dm_txpower_tracking_callback_thermalmeter(hw);
+		tm_trigger = 0;
+	}
+}
+
+void rtl88e_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rate_adaptive *p_ra = &(rtlpriv->ra);
+
+	p_ra->ratr_state = DM_RATR_STA_INIT;
+	p_ra->pre_ratr_state = DM_RATR_STA_INIT;
+
+	if (rtlpriv->dm.dm_type == DM_TYPE_BYDRIVER)
+		rtlpriv->dm.useramask = true;
+	else
+		rtlpriv->dm.useramask = false;
+}
+
+static void rtl88e_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rate_adaptive *p_ra = &(rtlpriv->ra);
+	struct ieee80211_sta *sta = NULL;
+	u32 low_rssi, hi_rssi;
+
+	if (is_hal_stop(rtlhal)) {
+		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+			 "driver is going to unload\n");
+		return;
+	}
+
+	if (!rtlpriv->dm.useramask) {
+		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+			 "driver does not control rate adaptive mask\n");
+		return;
+	}
+
+	if (mac->link_state == MAC80211_LINKED &&
+	    mac->opmode == NL80211_IFTYPE_STATION) {
+		switch (p_ra->pre_ratr_state) {
+		case DM_RATR_STA_HIGH:
+			hi_rssi = 50;
+			low_rssi = 20;
+			break;
+		case DM_RATR_STA_MIDDLE:
+			hi_rssi = 55;
+			low_rssi = 20;
+			break;
+		case DM_RATR_STA_LOW:
+			hi_rssi = 50;
+			low_rssi = 25;
+			break;
+		default:
+			hi_rssi = 50;
+			low_rssi = 20;
+			break;
+		}
+
+		if (rtlpriv->dm.undec_sm_pwdb > (long)hi_rssi)
+			p_ra->ratr_state = DM_RATR_STA_HIGH;
+		else if (rtlpriv->dm.undec_sm_pwdb > (long)low_rssi)
+			p_ra->ratr_state = DM_RATR_STA_MIDDLE;
+		else
+			p_ra->ratr_state = DM_RATR_STA_LOW;
+
+		if (p_ra->pre_ratr_state != p_ra->ratr_state) {
+			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+				 "RSSI = %ld\n",
+				 rtlpriv->dm.undec_sm_pwdb);
+			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+				 "RSSI_LEVEL = %d\n", p_ra->ratr_state);
+			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+				 "PreState = %d, CurState = %d\n",
+				  p_ra->pre_ratr_state, p_ra->ratr_state);
+
+			rcu_read_lock();
+			sta = rtl_find_sta(hw, mac->bssid);
+			if (sta)
+				rtlpriv->cfg->ops->update_rate_tbl(hw, sta,
+						   p_ra->ratr_state);
+			rcu_read_unlock();
+
+			p_ra->pre_ratr_state = p_ra->ratr_state;
+		}
+	}
+}
+
+static void rtl92c_dm_init_dynamic_bb_powersaving(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ps_t *dm_pstable = &rtlpriv->dm_pstable;
+
+	dm_pstable->pre_ccastate = CCA_MAX;
+	dm_pstable->cur_ccasate = CCA_MAX;
+	dm_pstable->pre_rfstate = RF_MAX;
+	dm_pstable->cur_rfstate = RF_MAX;
+	dm_pstable->rssi_val_min = 0;
+}
+
+static void rtl88e_dm_update_rx_idle_ant(struct ieee80211_hw *hw, u8 ant)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
+	u32 def_ant, opt_ant;
+
+	if (fat_tbl->rx_idle_ant != ant) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "need to update rx idle ant\n");
+		if (ant == MAIN_ANT) {
+			def_ant = (fat_tbl->rx_idle_ant == CG_TRX_HW_ANTDIV) ?
+				   MAIN_ANT_CG_TRX : MAIN_ANT_CGCS_RX;
+			opt_ant = (fat_tbl->rx_idle_ant == CG_TRX_HW_ANTDIV) ?
+				   AUX_ANT_CG_TRX : AUX_ANT_CGCS_RX;
+		} else {
+			def_ant = (fat_tbl->rx_idle_ant == CG_TRX_HW_ANTDIV) ?
+				   AUX_ANT_CG_TRX : AUX_ANT_CGCS_RX;
+			opt_ant = (fat_tbl->rx_idle_ant == CG_TRX_HW_ANTDIV) ?
+				   MAIN_ANT_CG_TRX : MAIN_ANT_CGCS_RX;
+		}
+
+		if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) {
+			rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(5) |
+				      BIT(4) | BIT(3), def_ant);
+			rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(8) |
+				      BIT(7) | BIT(6), opt_ant);
+			rtl_set_bbreg(hw, DM_REG_ANTSEL_CTRL_11N, BIT(14) |
+				      BIT(13) | BIT(12), def_ant);
+			rtl_set_bbreg(hw, DM_REG_RESP_TX_11N, BIT(6) | BIT(7),
+				      def_ant);
+		} else if (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV) {
+			rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(5) |
+				      BIT(4) | BIT(3), def_ant);
+			rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(8) |
+				      BIT(7) | BIT(6), opt_ant);
+		}
+	}
+	fat_tbl->rx_idle_ant = ant;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "RxIdleAnt %s\n",
+		 ((ant == MAIN_ANT) ? ("MAIN_ANT") : ("AUX_ANT")));
+}
+
+static void rtl88e_dm_update_tx_ant(struct ieee80211_hw *hw,
+	u8 ant, u32 mac_id)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
+	u8 target_ant;
+
+	if (ant == MAIN_ANT)
+		target_ant = MAIN_ANT_CG_TRX;
+	else
+		target_ant = AUX_ANT_CG_TRX;
+
+	fat_tbl->antsel_a[mac_id] = target_ant & BIT(0);
+	fat_tbl->antsel_b[mac_id] = (target_ant & BIT(1)) >> 1;
+	fat_tbl->antsel_c[mac_id] = (target_ant & BIT(2)) >> 2;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "txfrominfo target ant %s\n",
+		 ((ant == MAIN_ANT) ? ("MAIN_ANT") : ("AUX_ANT")));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "antsel_tr_mux = 3'b%d%d%d\n",
+		 fat_tbl->antsel_c[mac_id],
+		 fat_tbl->antsel_b[mac_id], fat_tbl->antsel_a[mac_id]);
+}
+
+static void rtl88e_dm_rx_hw_antena_div_init(struct ieee80211_hw *hw)
+{
+	u32  value32;
+	/*MAC Setting*/
+	value32 = rtl_get_bbreg(hw, DM_REG_ANTSEL_PIN_11N, MASKDWORD);
+	rtl_set_bbreg(hw, DM_REG_ANTSEL_PIN_11N, MASKDWORD, value32 |
+		     (BIT(23) | BIT(25)));
+	/*Pin Setting*/
+	rtl_set_bbreg(hw, DM_REG_PIN_CTRL_11N, BIT(9) | BIT(8), 0);
+	rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(10), 0);
+	rtl_set_bbreg(hw, DM_REG_LNA_SWITCH_11N, BIT(22), 1);
+	rtl_set_bbreg(hw, DM_REG_LNA_SWITCH_11N, BIT(31), 1);
+	/*OFDM Setting*/
+	rtl_set_bbreg(hw, DM_REG_ANTDIV_PARA1_11N, MASKDWORD, 0x000000a0);
+	/*CCK Setting*/
+	rtl_set_bbreg(hw, DM_REG_BB_PWR_SAV4_11N, BIT(7), 1);
+	rtl_set_bbreg(hw, DM_REG_CCK_ANTDIV_PARA2_11N, BIT(4), 1);
+	rtl88e_dm_update_rx_idle_ant(hw, MAIN_ANT);
+	rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKLWORD, 0x0201);
+}
+
+static void rtl88e_dm_trx_hw_antenna_div_init(struct ieee80211_hw *hw)
+{
+	u32  value32;
+
+	/*MAC Setting*/
+	value32 = rtl_get_bbreg(hw, DM_REG_ANTSEL_PIN_11N, MASKDWORD);
+	rtl_set_bbreg(hw, DM_REG_ANTSEL_PIN_11N, MASKDWORD, value32 |
+		     (BIT(23) | BIT(25)));
+	/*Pin Setting*/
+	rtl_set_bbreg(hw, DM_REG_PIN_CTRL_11N, BIT(9) | BIT(8), 0);
+	rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(10), 0);
+	rtl_set_bbreg(hw, DM_REG_LNA_SWITCH_11N, BIT(22), 0);
+	rtl_set_bbreg(hw, DM_REG_LNA_SWITCH_11N, BIT(31), 1);
+	/*OFDM Setting*/
+	rtl_set_bbreg(hw, DM_REG_ANTDIV_PARA1_11N, MASKDWORD, 0x000000a0);
+	/*CCK Setting*/
+	rtl_set_bbreg(hw, DM_REG_BB_PWR_SAV4_11N, BIT(7), 1);
+	rtl_set_bbreg(hw, DM_REG_CCK_ANTDIV_PARA2_11N, BIT(4), 1);
+	/*TX Setting*/
+	rtl_set_bbreg(hw, DM_REG_TX_ANT_CTRL_11N, BIT(21), 0);
+	rtl88e_dm_update_rx_idle_ant(hw, MAIN_ANT);
+	rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKLWORD, 0x0201);
+}
+
+static void rtl88e_dm_fast_training_init(struct ieee80211_hw *hw)
+{
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
+	u32 ant_combo = 2;
+	u32 value32, i;
+
+	for (i = 0; i < 6; i++) {
+		fat_tbl->bssid[i] = 0;
+		fat_tbl->ant_sum[i] = 0;
+		fat_tbl->ant_cnt[i] = 0;
+		fat_tbl->ant_ave[i] = 0;
+	}
+	fat_tbl->train_idx = 0;
+	fat_tbl->fat_state = FAT_NORMAL_STATE;
+
+	/*MAC Setting*/
+	value32 = rtl_get_bbreg(hw, DM_REG_ANTSEL_PIN_11N, MASKDWORD);
+	rtl_set_bbreg(hw, DM_REG_ANTSEL_PIN_11N, MASKDWORD, value32 | (BIT(23) |
+		      BIT(25)));
+	value32 = rtl_get_bbreg(hw, DM_REG_ANT_TRAIN_2, MASKDWORD);
+	rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_2, MASKDWORD, value32 | (BIT(16) |
+		      BIT(17)));
+	rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_2, MASKLWORD, 0);
+	rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_1, MASKDWORD, 0);
+
+	/*Pin Setting*/
+	rtl_set_bbreg(hw, DM_REG_PIN_CTRL_11N, BIT(9) | BIT(8), 0);
+	rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(10), 0);
+	rtl_set_bbreg(hw, DM_REG_LNA_SWITCH_11N, BIT(22), 0);
+	rtl_set_bbreg(hw, DM_REG_LNA_SWITCH_11N, BIT(31), 1);
+
+	/*OFDM Setting*/
+	rtl_set_bbreg(hw, DM_REG_ANTDIV_PARA1_11N, MASKDWORD, 0x000000a0);
+	/*antenna mapping table*/
+	if (ant_combo == 2) {
+		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKBYTE0, 1);
+		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKBYTE1, 2);
+	} else if (ant_combo == 7) {
+		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKBYTE0, 1);
+		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKBYTE1, 2);
+		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKBYTE2, 2);
+		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKBYTE3, 3);
+		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING2_11N, MASKBYTE0, 4);
+		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING2_11N, MASKBYTE1, 5);
+		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING2_11N, MASKBYTE2, 6);
+		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING2_11N, MASKBYTE3, 7);
+	}
+
+	/*TX Setting*/
+	rtl_set_bbreg(hw, DM_REG_TX_ANT_CTRL_11N, BIT(21), 1);
+	rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(5) | BIT(4) | BIT(3), 0);
+	rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(8) | BIT(7) | BIT(6), 1);
+	rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(2) | BIT(1) | BIT(0),
+		      (ant_combo - 1));
+
+	rtl_set_bbreg(hw, DM_REG_IGI_A_11N, BIT(7), 1);
+}
+
+static void rtl88e_dm_antenna_div_init(struct ieee80211_hw *hw)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	if (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
+		rtl88e_dm_rx_hw_antena_div_init(hw);
+	else if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV)
+		rtl88e_dm_trx_hw_antenna_div_init(hw);
+	else if (rtlefuse->antenna_div_type == CG_TRX_SMART_ANTDIV)
+		rtl88e_dm_fast_training_init(hw);
+}
+
+void rtl88e_dm_set_tx_ant_by_tx_info(struct ieee80211_hw *hw,
+				     u8 *pdesc, u32 mac_id)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
+
+	if ((rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) ||
+	    (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV)) {
+		SET_TX_DESC_ANTSEL_A(pdesc, fat_tbl->antsel_a[mac_id]);
+		SET_TX_DESC_ANTSEL_B(pdesc, fat_tbl->antsel_b[mac_id]);
+		SET_TX_DESC_ANTSEL_C(pdesc, fat_tbl->antsel_c[mac_id]);
+	}
+}
+
+void rtl88e_dm_ant_sel_statistics(struct ieee80211_hw *hw,
+				  u8 antsel_tr_mux, u32 mac_id, u32 rx_pwdb_all)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
+
+	if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) {
+		if (antsel_tr_mux == MAIN_ANT_CG_TRX) {
+			fat_tbl->main_ant_sum[mac_id] += rx_pwdb_all;
+			fat_tbl->main_ant_cnt[mac_id]++;
+		} else {
+			fat_tbl->aux_ant_sum[mac_id] += rx_pwdb_all;
+			fat_tbl->aux_ant_cnt[mac_id]++;
+		}
+	} else if (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV) {
+		if (antsel_tr_mux == MAIN_ANT_CGCS_RX) {
+			fat_tbl->main_ant_sum[mac_id] += rx_pwdb_all;
+			fat_tbl->main_ant_cnt[mac_id]++;
+		} else {
+			fat_tbl->aux_ant_sum[mac_id] += rx_pwdb_all;
+			fat_tbl->aux_ant_cnt[mac_id]++;
+		}
+	}
+}
+
+static void rtl88e_dm_hw_ant_div(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_sta_info *drv_priv;
+	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
+	u32 i, min_rssi = 0xff, ant_div_max_rssi = 0, max_rssi = 0;
+	u32 local_min_rssi, local_max_rssi;
+	u32 main_rssi, aux_rssi;
+	u8 rx_idle_ant = 0, target_ant = 7;
+
+	i = 0;
+	main_rssi = (fat_tbl->main_ant_cnt[i] != 0) ?
+		    (fat_tbl->main_ant_sum[i] /
+		     fat_tbl->main_ant_cnt[i]) : 0;
+	aux_rssi = (fat_tbl->aux_ant_cnt[i] != 0) ?
+		(fat_tbl->aux_ant_sum[i] / fat_tbl->aux_ant_cnt[i]) : 0;
+	target_ant = (main_rssi == aux_rssi) ?
+		     fat_tbl->rx_idle_ant : ((main_rssi >= aux_rssi) ?
+		     MAIN_ANT : AUX_ANT);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "main_ant_sum %d main_ant_cnt %d\n",
+		 fat_tbl->main_ant_sum[i], fat_tbl->main_ant_cnt[i]);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "aux_ant_sum %d aux_ant_cnt %d\n",
+		 fat_tbl->aux_ant_sum[i],
+		 fat_tbl->aux_ant_cnt[i]);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "main_rssi %d aux_rssi%d\n", main_rssi, aux_rssi);
+	local_max_rssi = (main_rssi > aux_rssi) ? main_rssi : aux_rssi;
+	if ((local_max_rssi > ant_div_max_rssi) && (local_max_rssi < 40))
+		ant_div_max_rssi = local_max_rssi;
+	if (local_max_rssi > max_rssi)
+		max_rssi = local_max_rssi;
+
+	if ((fat_tbl->rx_idle_ant == MAIN_ANT) && (main_rssi == 0))
+		main_rssi = aux_rssi;
+	else if ((fat_tbl->rx_idle_ant == AUX_ANT) && (aux_rssi == 0))
+		aux_rssi = main_rssi;
+
+	local_min_rssi = (main_rssi > aux_rssi) ? aux_rssi : main_rssi;
+	if (local_min_rssi < min_rssi) {
+		min_rssi = local_min_rssi;
+		rx_idle_ant = target_ant;
+	}
+	if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV)
+		rtl88e_dm_update_tx_ant(hw, target_ant, i);
+
+	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP ||
+	    rtlpriv->mac80211.opmode == NL80211_IFTYPE_ADHOC) {
+		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+		list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
+			i++;
+			main_rssi = (fat_tbl->main_ant_cnt[i] != 0) ?
+				(fat_tbl->main_ant_sum[i] /
+				 fat_tbl->main_ant_cnt[i]) : 0;
+			aux_rssi = (fat_tbl->aux_ant_cnt[i] != 0) ?
+				   (fat_tbl->aux_ant_sum[i] /
+				    fat_tbl->aux_ant_cnt[i]) : 0;
+			target_ant = (main_rssi == aux_rssi) ?
+				      fat_tbl->rx_idle_ant : ((main_rssi >=
+				      aux_rssi) ? MAIN_ANT : AUX_ANT);
+
+
+			local_max_rssi = max_t(u32, main_rssi, aux_rssi);
+			if ((local_max_rssi > ant_div_max_rssi) &&
+			    (local_max_rssi < 40))
+				ant_div_max_rssi = local_max_rssi;
+			if (local_max_rssi > max_rssi)
+				max_rssi = local_max_rssi;
+
+			if ((fat_tbl->rx_idle_ant == MAIN_ANT) && !main_rssi)
+				main_rssi = aux_rssi;
+			else if ((fat_tbl->rx_idle_ant == AUX_ANT) &&
+				 (aux_rssi == 0))
+				aux_rssi = main_rssi;
+
+			local_min_rssi = (main_rssi > aux_rssi) ?
+					  aux_rssi : main_rssi;
+			if (local_min_rssi < min_rssi) {
+				min_rssi = local_min_rssi;
+				rx_idle_ant = target_ant;
+			}
+			if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV)
+				rtl88e_dm_update_tx_ant(hw, target_ant, i);
+		}
+		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+	}
+
+	for (i = 0; i < ASSOCIATE_ENTRY_NUM; i++) {
+		fat_tbl->main_ant_sum[i] = 0;
+		fat_tbl->aux_ant_sum[i] = 0;
+		fat_tbl->main_ant_cnt[i] = 0;
+		fat_tbl->aux_ant_cnt[i] = 0;
+	}
+
+	rtl88e_dm_update_rx_idle_ant(hw, rx_idle_ant);
+
+	dm_dig->antdiv_rssi_max = ant_div_max_rssi;
+	dm_dig->rssi_max = max_rssi;
+}
+
+static void rtl88e_set_next_mac_address_target(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_sta_info *drv_priv;
+	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
+	u32 value32, i, j = 0;
+
+	if (mac->link_state >= MAC80211_LINKED) {
+		for (i = 0; i < ASSOCIATE_ENTRY_NUM; i++) {
+			if ((fat_tbl->train_idx + 1) == ASSOCIATE_ENTRY_NUM)
+				fat_tbl->train_idx = 0;
+			else
+				fat_tbl->train_idx++;
+
+			if (fat_tbl->train_idx == 0) {
+				value32 = (mac->mac_addr[5] << 8) |
+					   mac->mac_addr[4];
+				rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_2,
+					      MASKLWORD, value32);
+
+				value32 = (mac->mac_addr[3] << 24) |
+					  (mac->mac_addr[2] << 16) |
+					  (mac->mac_addr[1] << 8) |
+					   mac->mac_addr[0];
+				rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_1,
+					      MASKDWORD, value32);
+				break;
+			}
+
+			if (rtlpriv->mac80211.opmode !=
+			    NL80211_IFTYPE_STATION) {
+				spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+				list_for_each_entry(drv_priv,
+						    &rtlpriv->entry_list,
+						    list) {
+					j++;
+					if (j != fat_tbl->train_idx)
+						continue;
+
+					value32 = (drv_priv->mac_addr[5] << 8) |
+						   drv_priv->mac_addr[4];
+					rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_2,
+						      MASKLWORD, value32);
+
+					value32 = (drv_priv->mac_addr[3]<<24) |
+						  (drv_priv->mac_addr[2]<<16) |
+						  (drv_priv->mac_addr[1]<<8) |
+						   drv_priv->mac_addr[0];
+					rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_1,
+						      MASKDWORD, value32);
+					break;
+				}
+				spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+				/*find entry, break*/
+				if (j == fat_tbl->train_idx)
+					break;
+			}
+		}
+	}
+}
+
+static void rtl88e_dm_fast_ant_training(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
+	u32 i, max_rssi = 0;
+	u8 target_ant = 2;
+	bool bpkt_filter_match = false;
+
+	if (fat_tbl->fat_state == FAT_TRAINING_STATE) {
+		for (i = 0; i < 7; i++) {
+			if (fat_tbl->ant_cnt[i] == 0) {
+				fat_tbl->ant_ave[i] = 0;
+			} else {
+				fat_tbl->ant_ave[i] = fat_tbl->ant_sum[i] /
+					fat_tbl->ant_cnt[i];
+				bpkt_filter_match = true;
+			}
+
+			if (fat_tbl->ant_ave[i] > max_rssi) {
+				max_rssi = fat_tbl->ant_ave[i];
+				target_ant = (u8) i;
+			}
+		}
+
+		if (bpkt_filter_match == false) {
+			rtl_set_bbreg(hw, DM_REG_TXAGC_A_1_MCS32_11N,
+				      BIT(16), 0);
+			rtl_set_bbreg(hw, DM_REG_IGI_A_11N, BIT(7), 0);
+		} else {
+			rtl_set_bbreg(hw, DM_REG_TXAGC_A_1_MCS32_11N,
+				      BIT(16), 0);
+			rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(8) |
+				      BIT(7) | BIT(6), target_ant);
+			rtl_set_bbreg(hw, DM_REG_TX_ANT_CTRL_11N, BIT(21), 1);
+
+			fat_tbl->antsel_a[fat_tbl->train_idx] =
+				  target_ant & BIT(0);
+			fat_tbl->antsel_b[fat_tbl->train_idx] =
+				 (target_ant & BIT(1)) >> 1;
+			fat_tbl->antsel_c[fat_tbl->train_idx] =
+				 (target_ant & BIT(2)) >> 2;
+
+			if (target_ant == 0)
+				rtl_set_bbreg(hw, DM_REG_IGI_A_11N, BIT(7), 0);
+		}
+
+		for (i = 0; i < 7; i++) {
+			fat_tbl->ant_sum[i] = 0;
+			fat_tbl->ant_cnt[i] = 0;
+		}
+
+		fat_tbl->fat_state = FAT_NORMAL_STATE;
+		return;
+	}
+
+	if (fat_tbl->fat_state == FAT_NORMAL_STATE) {
+		rtl88e_set_next_mac_address_target(hw);
+
+		fat_tbl->fat_state = FAT_TRAINING_STATE;
+		rtl_set_bbreg(hw, DM_REG_TXAGC_A_1_MCS32_11N, BIT(16), 1);
+		rtl_set_bbreg(hw, DM_REG_IGI_A_11N, BIT(7), 1);
+
+		mod_timer(&rtlpriv->works.fast_antenna_training_timer,
+			  jiffies + MSECS(RTL_WATCH_DOG_TIME));
+	}
+}
+
+void rtl88e_dm_fast_antenna_training_callback(unsigned long data)
+{
+	struct ieee80211_hw *hw = (struct ieee80211_hw *)data;
+
+	rtl88e_dm_fast_ant_training(hw);
+}
+
+static void rtl88e_dm_antenna_diversity(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
+
+	if (mac->link_state < MAC80211_LINKED) {
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "No Link\n");
+		if (fat_tbl->becomelinked == true) {
+			RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+				 "need to turn off HW AntDiv\n");
+			rtl_set_bbreg(hw, DM_REG_IGI_A_11N, BIT(7), 0);
+			rtl_set_bbreg(hw, DM_REG_CCK_ANTDIV_PARA1_11N,
+				      BIT(15), 0);
+			if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV)
+				rtl_set_bbreg(hw, DM_REG_TX_ANT_CTRL_11N,
+					      BIT(21), 0);
+			fat_tbl->becomelinked =
+			  (mac->link_state == MAC80211_LINKED) ? true : false;
+		}
+		return;
+	} else {
+		if (fat_tbl->becomelinked == false) {
+			RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+				 "Need to turn on HW AntDiv\n");
+			rtl_set_bbreg(hw, DM_REG_IGI_A_11N, BIT(7), 1);
+			rtl_set_bbreg(hw, DM_REG_CCK_ANTDIV_PARA1_11N,
+				      BIT(15), 1);
+			if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV)
+				rtl_set_bbreg(hw, DM_REG_TX_ANT_CTRL_11N,
+					      BIT(21), 1);
+			fat_tbl->becomelinked =
+			   (mac->link_state >= MAC80211_LINKED) ? true : false;
+		}
+	}
+
+	if ((rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) ||
+	    (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV))
+		rtl88e_dm_hw_ant_div(hw);
+	else if (rtlefuse->antenna_div_type == CG_TRX_SMART_ANTDIV)
+		rtl88e_dm_fast_ant_training(hw);
+}
+
+void rtl88e_dm_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
+	rtl88e_dm_diginit(hw);
+	rtl88e_dm_init_dynamic_txpower(hw);
+	rtl88e_dm_init_edca_turbo(hw);
+	rtl88e_dm_init_rate_adaptive_mask(hw);
+	rtl88e_dm_init_txpower_tracking(hw);
+	rtl92c_dm_init_dynamic_bb_powersaving(hw);
+	rtl88e_dm_antenna_div_init(hw);
+}
+
+void rtl88e_dm_watchdog(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool fw_current_inpsmode = false;
+	bool fw_ps_awake = true;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+				      (u8 *)(&fw_current_inpsmode));
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FWLPS_RF_ON,
+				      (u8 *)(&fw_ps_awake));
+	if (ppsc->p2p_ps_info.p2p_ps_mode)
+		fw_ps_awake = false;
+
+	if ((ppsc->rfpwr_state == ERFON) &&
+	    ((!fw_current_inpsmode) && fw_ps_awake) &&
+	    (!ppsc->rfchange_inprogress)) {
+		rtl88e_dm_pwdb_monitor(hw);
+		rtl88e_dm_dig(hw);
+		rtl88e_dm_false_alarm_counter_statistics(hw);
+		rtl92c_dm_dynamic_txpower(hw);
+		rtl88e_dm_check_txpower_tracking(hw);
+		rtl88e_dm_refresh_rate_adaptive_mask(hw);
+		rtl88e_dm_check_edca_turbo(hw);
+		rtl88e_dm_antenna_diversity(hw);
+	}
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/dm.h b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.h
new file mode 100644
index 000000000000..0e07f72ea158
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.h
@@ -0,0 +1,326 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef	__RTL88E_DM_H__
+#define __RTL88E_DM_H__
+
+#define	MAIN_ANT					0
+#define	AUX_ANT						1
+#define	MAIN_ANT_CG_TRX					1
+#define	AUX_ANT_CG_TRX					0
+#define	MAIN_ANT_CGCS_RX				0
+#define	AUX_ANT_CGCS_RX					1
+
+/*RF REG LIST*/
+#define	DM_REG_RF_MODE_11N				0x00
+#define	DM_REG_RF_0B_11N				0x0B
+#define	DM_REG_CHNBW_11N				0x18
+#define	DM_REG_T_METER_11N				0x24
+#define	DM_REG_RF_25_11N				0x25
+#define	DM_REG_RF_26_11N				0x26
+#define	DM_REG_RF_27_11N				0x27
+#define	DM_REG_RF_2B_11N				0x2B
+#define	DM_REG_RF_2C_11N				0x2C
+#define	DM_REG_RXRF_A3_11N				0x3C
+#define	DM_REG_T_METER_92D_11N				0x42
+#define	DM_REG_T_METER_88E_11N				0x42
+
+/*BB REG LIST*/
+/*PAGE 8 */
+#define	DM_REG_BB_CTRL_11N				0x800
+#define	DM_REG_RF_PIN_11N				0x804
+#define	DM_REG_PSD_CTRL_11N				0x808
+#define	DM_REG_TX_ANT_CTRL_11N				0x80C
+#define	DM_REG_BB_PWR_SAV5_11N				0x818
+#define	DM_REG_CCK_RPT_FORMAT_11N			0x824
+#define	DM_REG_RX_DEFAULT_A_11N				0x858
+#define	DM_REG_RX_DEFAULT_B_11N				0x85A
+#define	DM_REG_BB_PWR_SAV3_11N				0x85C
+#define	DM_REG_ANTSEL_CTRL_11N				0x860
+#define	DM_REG_RX_ANT_CTRL_11N				0x864
+#define	DM_REG_PIN_CTRL_11N				0x870
+#define	DM_REG_BB_PWR_SAV1_11N				0x874
+#define	DM_REG_ANTSEL_PATH_11N				0x878
+#define	DM_REG_BB_3WIRE_11N				0x88C
+#define	DM_REG_SC_CNT_11N				0x8C4
+#define	DM_REG_PSD_DATA_11N				0x8B4
+/*PAGE 9*/
+#define	DM_REG_ANT_MAPPING1_11N				0x914
+#define	DM_REG_ANT_MAPPING2_11N				0x918
+/*PAGE A*/
+#define	DM_REG_CCK_ANTDIV_PARA1_11N			0xA00
+#define	DM_REG_CCK_CCA_11N				0xA0A
+#define	DM_REG_CCK_ANTDIV_PARA2_11N			0xA0C
+#define	DM_REG_CCK_ANTDIV_PARA3_11N			0xA10
+#define	DM_REG_CCK_ANTDIV_PARA4_11N			0xA14
+#define	DM_REG_CCK_FILTER_PARA1_11N			0xA22
+#define	DM_REG_CCK_FILTER_PARA2_11N			0xA23
+#define	DM_REG_CCK_FILTER_PARA3_11N			0xA24
+#define	DM_REG_CCK_FILTER_PARA4_11N			0xA25
+#define	DM_REG_CCK_FILTER_PARA5_11N			0xA26
+#define	DM_REG_CCK_FILTER_PARA6_11N			0xA27
+#define	DM_REG_CCK_FILTER_PARA7_11N			0xA28
+#define	DM_REG_CCK_FILTER_PARA8_11N			0xA29
+#define	DM_REG_CCK_FA_RST_11N				0xA2C
+#define	DM_REG_CCK_FA_MSB_11N				0xA58
+#define	DM_REG_CCK_FA_LSB_11N				0xA5C
+#define	DM_REG_CCK_CCA_CNT_11N				0xA60
+#define	DM_REG_BB_PWR_SAV4_11N				0xA74
+/*PAGE B */
+#define	DM_REG_LNA_SWITCH_11N				0xB2C
+#define	DM_REG_PATH_SWITCH_11N				0xB30
+#define	DM_REG_RSSI_CTRL_11N				0xB38
+#define	DM_REG_CONFIG_ANTA_11N				0xB68
+#define	DM_REG_RSSI_BT_11N				0xB9C
+/*PAGE C */
+#define	DM_REG_OFDM_FA_HOLDC_11N			0xC00
+#define	DM_REG_RX_PATH_11N				0xC04
+#define	DM_REG_TRMUX_11N				0xC08
+#define	DM_REG_OFDM_FA_RSTC_11N				0xC0C
+#define	DM_REG_RXIQI_MATRIX_11N				0xC14
+#define	DM_REG_TXIQK_MATRIX_LSB1_11N			0xC4C
+#define	DM_REG_IGI_A_11N				0xC50
+#define	DM_REG_ANTDIV_PARA2_11N				0xC54
+#define	DM_REG_IGI_B_11N				0xC58
+#define	DM_REG_ANTDIV_PARA3_11N				0xC5C
+#define	DM_REG_BB_PWR_SAV2_11N				0xC70
+#define	DM_REG_RX_OFF_11N				0xC7C
+#define	DM_REG_TXIQK_MATRIXA_11N			0xC80
+#define	DM_REG_TXIQK_MATRIXB_11N			0xC88
+#define	DM_REG_TXIQK_MATRIXA_LSB2_11N			0xC94
+#define	DM_REG_TXIQK_MATRIXB_LSB2_11N			0xC9C
+#define	DM_REG_RXIQK_MATRIX_LSB_11N			0xCA0
+#define	DM_REG_ANTDIV_PARA1_11N				0xCA4
+#define	DM_REG_OFDM_FA_TYPE1_11N			0xCF0
+/*PAGE D */
+#define	DM_REG_OFDM_FA_RSTD_11N				0xD00
+#define	DM_REG_OFDM_FA_TYPE2_11N			0xDA0
+#define	DM_REG_OFDM_FA_TYPE3_11N			0xDA4
+#define	DM_REG_OFDM_FA_TYPE4_11N			0xDA8
+/*PAGE E */
+#define	DM_REG_TXAGC_A_6_18_11N				0xE00
+#define	DM_REG_TXAGC_A_24_54_11N			0xE04
+#define	DM_REG_TXAGC_A_1_MCS32_11N			0xE08
+#define	DM_REG_TXAGC_A_MCS0_3_11N			0xE10
+#define	DM_REG_TXAGC_A_MCS4_7_11N			0xE14
+#define	DM_REG_TXAGC_A_MCS8_11_11N			0xE18
+#define	DM_REG_TXAGC_A_MCS12_15_11N			0xE1C
+#define	DM_REG_FPGA0_IQK_11N				0xE28
+#define	DM_REG_TXIQK_TONE_A_11N				0xE30
+#define	DM_REG_RXIQK_TONE_A_11N				0xE34
+#define	DM_REG_TXIQK_PI_A_11N				0xE38
+#define	DM_REG_RXIQK_PI_A_11N				0xE3C
+#define	DM_REG_TXIQK_11N				0xE40
+#define	DM_REG_RXIQK_11N				0xE44
+#define	DM_REG_IQK_AGC_PTS_11N				0xE48
+#define	DM_REG_IQK_AGC_RSP_11N				0xE4C
+#define	DM_REG_BLUETOOTH_11N				0xE6C
+#define	DM_REG_RX_WAIT_CCA_11N				0xE70
+#define	DM_REG_TX_CCK_RFON_11N				0xE74
+#define	DM_REG_TX_CCK_BBON_11N				0xE78
+#define	DM_REG_OFDM_RFON_11N				0xE7C
+#define	DM_REG_OFDM_BBON_11N				0xE80
+#define DM_REG_TX2RX_11N				0xE84
+#define	DM_REG_TX2TX_11N				0xE88
+#define	DM_REG_RX_CCK_11N				0xE8C
+#define	DM_REG_RX_OFDM_11N				0xED0
+#define	DM_REG_RX_WAIT_RIFS_11N				0xED4
+#define	DM_REG_RX2RX_11N				0xED8
+#define	DM_REG_STANDBY_11N				0xEDC
+#define	DM_REG_SLEEP_11N				0xEE0
+#define	DM_REG_PMPD_ANAEN_11N				0xEEC
+
+
+/*MAC REG LIST*/
+#define	DM_REG_BB_RST_11N				0x02
+#define	DM_REG_ANTSEL_PIN_11N				0x4C
+#define	DM_REG_EARLY_MODE_11N				0x4D0
+#define	DM_REG_RSSI_MONITOR_11N				0x4FE
+#define	DM_REG_EDCA_VO_11N				0x500
+#define	DM_REG_EDCA_VI_11N				0x504
+#define	DM_REG_EDCA_BE_11N				0x508
+#define	DM_REG_EDCA_BK_11N				0x50C
+#define	DM_REG_TXPAUSE_11N				0x522
+#define	DM_REG_RESP_TX_11N				0x6D8
+#define	DM_REG_ANT_TRAIN_1				0x7b0
+#define	DM_REG_ANT_TRAIN_2				0x7b4
+
+/*DIG Related*/
+#define	DM_BIT_IGI_11N					0x0000007F
+
+#define HAL_DM_DIG_DISABLE				BIT(0)
+#define HAL_DM_HIPWR_DISABLE				BIT(1)
+
+#define OFDM_TABLE_LENGTH				43
+#define CCK_TABLE_LENGTH				33
+
+#define OFDM_TABLE_SIZE					43
+#define CCK_TABLE_SIZE					33
+
+#define BW_AUTO_SWITCH_HIGH_LOW				25
+#define BW_AUTO_SWITCH_LOW_HIGH				30
+
+#define DM_DIG_THRESH_HIGH				40
+#define DM_DIG_THRESH_LOW				35
+
+#define DM_FALSEALARM_THRESH_LOW			400
+#define DM_FALSEALARM_THRESH_HIGH			1000
+
+#define DM_DIG_MAX					0x3e
+#define DM_DIG_MIN					0x1e
+
+#define DM_DIG_MAX_AP					0x32
+#define DM_DIG_MIN_AP					0x20
+
+#define DM_DIG_FA_UPPER					0x3e
+#define DM_DIG_FA_LOWER					0x1e
+#define DM_DIG_FA_TH0					0x200
+#define DM_DIG_FA_TH1					0x300
+#define DM_DIG_FA_TH2					0x400
+
+#define DM_DIG_BACKOFF_MAX				12
+#define DM_DIG_BACKOFF_MIN				-4
+#define DM_DIG_BACKOFF_DEFAULT				10
+
+#define RXPATHSELECTION_SS_TH_LOW			30
+#define RXPATHSELECTION_DIFF_TH				18
+
+#define DM_RATR_STA_INIT				0
+#define DM_RATR_STA_HIGH				1
+#define DM_RATR_STA_MIDDLE				2
+#define DM_RATR_STA_LOW					3
+
+#define CTS2SELF_THVAL					30
+#define REGC38_TH					20
+
+#define WAIOTTHVAL					25
+
+#define TXHIGHPWRLEVEL_NORMAL				0
+#define TXHIGHPWRLEVEL_LEVEL1				1
+#define TXHIGHPWRLEVEL_LEVEL2				2
+#define TXHIGHPWRLEVEL_BT1				3
+#define TXHIGHPWRLEVEL_BT2				4
+
+#define DM_TYPE_BYFW					0
+#define DM_TYPE_BYDRIVER				1
+
+#define TX_POWER_NEAR_FIELD_THRESH_LVL2			74
+#define TX_POWER_NEAR_FIELD_THRESH_LVL1			67
+#define TXPWRTRACK_MAX_IDX				6
+
+struct swat_t {
+	u8 failure_cnt;
+	u8 try_flag;
+	u8 stop_trying;
+	long pre_rssi;
+	long trying_threshold;
+	u8 cur_antenna;
+	u8 pre_antenna;
+};
+
+enum FAT_STATE {
+	FAT_NORMAL_STATE	= 0,
+	FAT_TRAINING_STATE = 1,
+};
+
+enum tag_dynamic_init_gain_operation_type_definition {
+	DIG_TYPE_THRESH_HIGH = 0,
+	DIG_TYPE_THRESH_LOW = 1,
+	DIG_TYPE_BACKOFF = 2,
+	DIG_TYPE_RX_GAIN_MIN = 3,
+	DIG_TYPE_RX_GAIN_MAX = 4,
+	DIG_TYPE_ENABLE = 5,
+	DIG_TYPE_DISABLE = 6,
+	DIG_OP_TYPE_MAX
+};
+
+enum tag_cck_packet_detection_threshold_type_definition {
+	CCK_PD_STAGE_LOWRSSI = 0,
+	CCK_PD_STAGE_HIGHRSSI = 1,
+	CCK_FA_STAGE_LOW = 2,
+	CCK_FA_STAGE_HIGH = 3,
+	CCK_PD_STAGE_MAX = 4,
+};
+
+enum dm_1r_cca_e {
+	CCA_1R = 0,
+	CCA_2R = 1,
+	CCA_MAX = 2,
+};
+
+enum dm_rf_e {
+	RF_SAVE = 0,
+	RF_NORMAL = 1,
+	RF_MAX = 2,
+};
+
+enum dm_sw_ant_switch_e {
+	ANS_ANTENNA_B = 1,
+	ANS_ANTENNA_A = 2,
+	ANS_ANTENNA_MAX = 3,
+};
+
+enum dm_dig_ext_port_alg_e {
+	DIG_EXT_PORT_STAGE_0 = 0,
+	DIG_EXT_PORT_STAGE_1 = 1,
+	DIG_EXT_PORT_STAGE_2 = 2,
+	DIG_EXT_PORT_STAGE_3 = 3,
+	DIG_EXT_PORT_STAGE_MAX = 4,
+};
+
+enum dm_dig_connect_e {
+	DIG_STA_DISCONNECT = 0,
+	DIG_STA_CONNECT = 1,
+	DIG_STA_BEFORE_CONNECT = 2,
+	DIG_MULTISTA_DISCONNECT = 3,
+	DIG_MULTISTA_CONNECT = 4,
+	DIG_CONNECT_MAX
+};
+
+enum pwr_track_control_method {
+	BBSWING,
+	TXAGC
+};
+
+void rtl88e_dm_set_tx_ant_by_tx_info(struct ieee80211_hw *hw,
+				     u8 *pdesc, u32 mac_id);
+void rtl88e_dm_ant_sel_statistics(struct ieee80211_hw *hw, u8 antsel_tr_mux,
+				  u32 mac_id, u32 rx_pwdb_all);
+void rtl88e_dm_fast_antenna_training_callback(unsigned long data);
+void rtl88e_dm_init(struct ieee80211_hw *hw);
+void rtl88e_dm_watchdog(struct ieee80211_hw *hw);
+void rtl88e_dm_write_dig(struct ieee80211_hw *hw);
+void rtl88e_dm_init_edca_turbo(struct ieee80211_hw *hw);
+void rtl88e_dm_check_txpower_tracking(struct ieee80211_hw *hw);
+void rtl88e_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw);
+void rtl88e_dm_txpower_track_adjust(struct ieee80211_hw *hw,
+				    u8 type, u8 *pdirection,
+				    u32 *poutwrite_val);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/fw.c b/drivers/net/wireless/rtlwifi/rtl8188ee/fw.c
new file mode 100644
index 000000000000..57e4cc5833a9
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/fw.c
@@ -0,0 +1,830 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../base.h"
+#include "reg.h"
+#include "def.h"
+#include "fw.h"
+
+#include <linux/kmemleak.h>
+
+static void _rtl88e_enable_fw_download(struct ieee80211_hw *hw, bool enable)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp;
+
+	if (enable) {
+		tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, tmp | 0x04);
+
+		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, tmp | 0x01);
+
+		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL + 2);
+		rtl_write_byte(rtlpriv, REG_MCUFWDL + 2, tmp & 0xf7);
+	} else {
+		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, tmp & 0xfe);
+
+		rtl_write_byte(rtlpriv, REG_MCUFWDL + 1, 0x00);
+	}
+}
+
+static void _rtl88e_fw_block_write(struct ieee80211_hw *hw,
+				   const u8 *buffer, u32 size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 blk_sz = sizeof(u32);
+	u8 *buf_ptr = (u8 *)buffer;
+	u32 *pu4BytePtr = (u32 *)buffer;
+	u32 i, offset, blk_cnt, remain;
+
+	blk_cnt = size / blk_sz;
+	remain = size % blk_sz;
+
+	for (i = 0; i < blk_cnt; i++) {
+		offset = i * blk_sz;
+		rtl_write_dword(rtlpriv, (FW_8192C_START_ADDRESS + offset),
+				*(pu4BytePtr + i));
+	}
+
+	if (remain) {
+		offset = blk_cnt * blk_sz;
+		buf_ptr += offset;
+		for (i = 0; i < remain; i++) {
+			rtl_write_byte(rtlpriv, (FW_8192C_START_ADDRESS +
+						 offset + i), *(buf_ptr + i));
+		}
+	}
+}
+
+static void _rtl88e_fw_page_write(struct ieee80211_hw *hw,
+				  u32 page, const u8 *buffer, u32 size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 value8;
+	u8 u8page = (u8) (page & 0x07);
+
+	value8 = (rtl_read_byte(rtlpriv, REG_MCUFWDL + 2) & 0xF8) | u8page;
+
+	rtl_write_byte(rtlpriv, (REG_MCUFWDL + 2), value8);
+	_rtl88e_fw_block_write(hw, buffer, size);
+}
+
+static void _rtl88e_fill_dummy(u8 *pfwbuf, u32 *pfwlen)
+{
+	u32 fwlen = *pfwlen;
+	u8 remain = (u8) (fwlen % 4);
+
+	remain = (remain == 0) ? 0 : (4 - remain);
+
+	while (remain > 0) {
+		pfwbuf[fwlen] = 0;
+		fwlen++;
+		remain--;
+	}
+
+	*pfwlen = fwlen;
+}
+
+static void _rtl88e_write_fw(struct ieee80211_hw *hw,
+			     enum version_8188e version, u8 *buffer, u32 size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 *buf_ptr = (u8 *)buffer;
+	u32 page_no, remain;
+	u32 page, offset;
+
+	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "FW size is %d bytes,\n", size);
+
+	_rtl88e_fill_dummy(buf_ptr, &size);
+
+	page_no = size / FW_8192C_PAGE_SIZE;
+	remain = size % FW_8192C_PAGE_SIZE;
+
+	if (page_no > 8) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Page numbers should not greater then 8\n");
+	}
+
+	for (page = 0; page < page_no; page++) {
+		offset = page * FW_8192C_PAGE_SIZE;
+		_rtl88e_fw_page_write(hw, page, (buf_ptr + offset),
+				      FW_8192C_PAGE_SIZE);
+	}
+
+	if (remain) {
+		offset = page_no * FW_8192C_PAGE_SIZE;
+		page = page_no;
+		_rtl88e_fw_page_write(hw, page, (buf_ptr + offset), remain);
+	}
+}
+
+static int _rtl88e_fw_free_to_go(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int err = -EIO;
+	u32 counter = 0;
+	u32 value32;
+
+	do {
+		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
+	} while ((counter++ < FW_8192C_POLLING_TIMEOUT_COUNT) &&
+		 (!(value32 & FWDL_CHKSUM_RPT)));
+
+	if (counter >= FW_8192C_POLLING_TIMEOUT_COUNT) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "chksum report faill ! REG_MCUFWDL:0x%08x .\n",
+			  value32);
+		goto exit;
+	}
+
+	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+		 "Checksum report OK ! REG_MCUFWDL:0x%08x .\n", value32);
+
+	value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
+	value32 |= MCUFWDL_RDY;
+	value32 &= ~WINTINI_RDY;
+	rtl_write_dword(rtlpriv, REG_MCUFWDL, value32);
+
+	rtl88e_firmware_selfreset(hw);
+	counter = 0;
+
+	do {
+		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
+		if (value32 & WINTINI_RDY) {
+			RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+				 "Polling FW ready success!! REG_MCUFWDL:0x%08x.\n",
+				  value32);
+			err = 0;
+			goto exit;
+		}
+
+		udelay(FW_8192C_POLLING_DELAY);
+
+	} while (counter++ < FW_8192C_POLLING_TIMEOUT_COUNT);
+
+	RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+		 "Polling FW ready fail!! REG_MCUFWDL:0x%08x .\n", value32);
+
+exit:
+	return err;
+}
+
+int rtl88e_download_fw(struct ieee80211_hw *hw, bool buse_wake_on_wlan_fw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl92c_firmware_header *pfwheader;
+	u8 *pfwdata;
+	u32 fwsize;
+	int err;
+	enum version_8188e version = rtlhal->version;
+
+	if (!rtlhal->pfirmware)
+		return 1;
+
+	pfwheader = (struct rtl92c_firmware_header *)rtlhal->pfirmware;
+	pfwdata = (u8 *)rtlhal->pfirmware;
+	fwsize = rtlhal->fwsize;
+	RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
+		 "normal Firmware SIZE %d\n", fwsize);
+
+	if (IS_FW_HEADER_EXIST(pfwheader)) {
+		RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
+			 "Firmware Version(%d), Signature(%#x), Size(%d)\n",
+			 pfwheader->version, pfwheader->signature,
+			 (int)sizeof(struct rtl92c_firmware_header));
+
+		pfwdata = pfwdata + sizeof(struct rtl92c_firmware_header);
+		fwsize = fwsize - sizeof(struct rtl92c_firmware_header);
+	}
+
+	if (rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) {
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, 0);
+		rtl88e_firmware_selfreset(hw);
+	}
+	_rtl88e_enable_fw_download(hw, true);
+	_rtl88e_write_fw(hw, version, pfwdata, fwsize);
+	_rtl88e_enable_fw_download(hw, false);
+
+	err = _rtl88e_fw_free_to_go(hw);
+
+	RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
+		 "Firmware is%s ready to run!\n", err ? " not" : "");
+	return 0;
+}
+
+static bool _rtl88e_check_fw_read_last_h2c(struct ieee80211_hw *hw, u8 boxnum)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 val_hmetfr;
+
+	val_hmetfr = rtl_read_byte(rtlpriv, REG_HMETFR);
+	if (((val_hmetfr >> boxnum) & BIT(0)) == 0)
+		return true;
+	return false;
+}
+
+static void _rtl88e_fill_h2c_command(struct ieee80211_hw *hw,
+				     u8 element_id, u32 cmd_len,
+				     u8 *cmd_b)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 boxnum;
+	u16 box_reg = 0, box_extreg = 0;
+	u8 u1b_tmp;
+	bool isfw_read = false;
+	u8 buf_index = 0;
+	bool write_sucess = false;
+	u8 wait_h2c_limit = 100;
+	u8 wait_writeh2c_limit = 100;
+	u8 boxc[4], boxext[2];
+	u32 h2c_waitcounter = 0;
+	unsigned long flag;
+	u8 idx;
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "come in\n");
+
+	while (true) {
+		spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
+		if (rtlhal->h2c_setinprogress) {
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+				 "H2C set in progress! Wait to set..element_id(%d).\n",
+				 element_id);
+
+			while (rtlhal->h2c_setinprogress) {
+				spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
+						       flag);
+				h2c_waitcounter++;
+				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+					 "Wait 100 us (%d times)...\n",
+					 h2c_waitcounter);
+				udelay(100);
+
+				if (h2c_waitcounter > 1000)
+					return;
+				spin_lock_irqsave(&rtlpriv->locks.h2c_lock,
+						  flag);
+			}
+			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
+		} else {
+			rtlhal->h2c_setinprogress = true;
+			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
+			break;
+		}
+	}
+
+	while (!write_sucess) {
+		wait_writeh2c_limit--;
+		if (wait_writeh2c_limit == 0) {
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Write H2C fail because no trigger for FW INT!\n");
+			break;
+		}
+
+		boxnum = rtlhal->last_hmeboxnum;
+		switch (boxnum) {
+		case 0:
+			box_reg = REG_HMEBOX_0;
+			box_extreg = REG_HMEBOX_EXT_0;
+			break;
+		case 1:
+			box_reg = REG_HMEBOX_1;
+			box_extreg = REG_HMEBOX_EXT_1;
+			break;
+		case 2:
+			box_reg = REG_HMEBOX_2;
+			box_extreg = REG_HMEBOX_EXT_2;
+			break;
+		case 3:
+			box_reg = REG_HMEBOX_3;
+			box_extreg = REG_HMEBOX_EXT_3;
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "switch case not processed\n");
+			break;
+		}
+
+		isfw_read = _rtl88e_check_fw_read_last_h2c(hw, boxnum);
+		while (!isfw_read) {
+			wait_h2c_limit--;
+			if (wait_h2c_limit == 0) {
+				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+					 "Wating too long for FW read "
+					 "clear HMEBox(%d)!\n", boxnum);
+				break;
+			}
+
+			udelay(10);
+
+			isfw_read = _rtl88e_check_fw_read_last_h2c(hw, boxnum);
+			u1b_tmp = rtl_read_byte(rtlpriv, 0x130);
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+				 "Wating for FW read clear HMEBox(%d)!!! "
+				 "0x130 = %2x\n", boxnum, u1b_tmp);
+		}
+
+		if (!isfw_read) {
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+				 "Write H2C register BOX[%d] fail!!!!! "
+				 "Fw do not read.\n", boxnum);
+			break;
+		}
+
+		memset(boxc, 0, sizeof(boxc));
+		memset(boxext, 0, sizeof(boxext));
+		boxc[0] = element_id;
+		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+			 "Write element_id box_reg(%4x) = %2x\n",
+			 box_reg, element_id);
+
+		switch (cmd_len) {
+		case 1:
+		case 2:
+		case 3:
+			/*boxc[0] &= ~(BIT(7));*/
+			memcpy((u8 *)(boxc) + 1, cmd_b + buf_index, cmd_len);
+
+			for (idx = 0; idx < 4; idx++)
+				rtl_write_byte(rtlpriv, box_reg+idx, boxc[idx]);
+			break;
+		case 4:
+		case 5:
+		case 6:
+		case 7:
+			/*boxc[0] |= (BIT(7));*/
+			memcpy((u8 *)(boxext), cmd_b + buf_index+3, cmd_len-3);
+			memcpy((u8 *)(boxc) + 1, cmd_b + buf_index, 3);
+
+			for (idx = 0; idx < 2; idx++) {
+				rtl_write_byte(rtlpriv, box_extreg + idx,
+					       boxext[idx]);
+			}
+
+			for (idx = 0; idx < 4; idx++) {
+				rtl_write_byte(rtlpriv, box_reg + idx,
+					       boxc[idx]);
+			}
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "switch case not processed\n");
+			break;
+		}
+
+		write_sucess = true;
+
+		rtlhal->last_hmeboxnum = boxnum + 1;
+		if (rtlhal->last_hmeboxnum == 4)
+			rtlhal->last_hmeboxnum = 0;
+
+		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+			 "pHalData->last_hmeboxnum  = %d\n",
+			 rtlhal->last_hmeboxnum);
+	}
+
+	spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
+	rtlhal->h2c_setinprogress = false;
+	spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "go out\n");
+}
+
+void rtl88e_fill_h2c_cmd(struct ieee80211_hw *hw,
+			 u8 element_id, u32 cmd_len, u8 *cmd_b)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 tmp_cmdbuf[2];
+
+	if (rtlhal->fw_ready == false) {
+		RT_ASSERT(false, "fail H2C cmd - Fw download fail!!!\n");
+		return;
+	}
+
+	memset(tmp_cmdbuf, 0, 8);
+	memcpy(tmp_cmdbuf, cmd_b, cmd_len);
+	_rtl88e_fill_h2c_command(hw, element_id, cmd_len, (u8 *)&tmp_cmdbuf);
+
+	return;
+}
+
+void rtl88e_firmware_selfreset(struct ieee80211_hw *hw)
+{
+	u8 u1b_tmp;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp & (~BIT(2))));
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp | BIT(2)));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "8051Reset88E(): 8051 reset success.\n");
+}
+
+void rtl88e_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 u1_h2c_set_pwrmode[H2C_88E_PWEMODE_LENGTH] = { 0 };
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u8 power_state = 0;
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
+	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
+	SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, 0);
+	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
+					 (rtlpriv->mac80211.p2p) ?
+					 ppsc->smart_ps : 1);
+	SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(u1_h2c_set_pwrmode,
+					       ppsc->reg_max_lps_awakeintvl);
+	SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1_h2c_set_pwrmode, 0);
+	if (mode == FW_PS_ACTIVE_MODE)
+		power_state |= FW_PWR_STATE_ACTIVE;
+	else
+		power_state |= FW_PWR_STATE_RF_OFF;
+	SET_H2CCMD_PWRMODE_PARM_PWR_STATE(u1_h2c_set_pwrmode, power_state);
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+		      "rtl92c_set_fw_pwrmode(): u1_h2c_set_pwrmode\n",
+		      u1_h2c_set_pwrmode, H2C_88E_PWEMODE_LENGTH);
+	rtl88e_fill_h2c_cmd(hw, H2C_88E_SETPWRMODE, H2C_88E_PWEMODE_LENGTH,
+			    u1_h2c_set_pwrmode);
+}
+
+void rtl88e_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
+{
+	u8 u1_joinbssrpt_parm[1] = { 0 };
+
+	SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(u1_joinbssrpt_parm, mstatus);
+
+	rtl88e_fill_h2c_cmd(hw, H2C_88E_JOINBSSRPT, 1, u1_joinbssrpt_parm);
+}
+
+void rtl88e_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,
+				   u8 ap_offload_enable)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u8 u1_apoffload_parm[H2C_88E_AP_OFFLOAD_LENGTH] = { 0 };
+
+	SET_H2CCMD_AP_OFFLOAD_ON(u1_apoffload_parm, ap_offload_enable);
+	SET_H2CCMD_AP_OFFLOAD_HIDDEN(u1_apoffload_parm, mac->hiddenssid);
+	SET_H2CCMD_AP_OFFLOAD_DENYANY(u1_apoffload_parm, 0);
+
+	rtl88e_fill_h2c_cmd(hw, H2C_88E_AP_OFFLOAD, H2C_88E_AP_OFFLOAD_LENGTH,
+			    u1_apoffload_parm);
+}
+
+static bool _rtl88e_cmd_send_packet(struct ieee80211_hw *hw,
+				    struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring;
+	struct rtl_tx_desc *pdesc;
+	struct sk_buff *pskb = NULL;
+	unsigned long flags;
+
+	ring = &rtlpci->tx_ring[BEACON_QUEUE];
+
+	pskb = __skb_dequeue(&ring->queue);
+	if (pskb)
+		kfree_skb(pskb);
+
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
+
+	pdesc = &ring->desc[0];
+
+	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *)pdesc, 1, 1, skb);
+
+	__skb_queue_tail(&ring->queue, skb);
+
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+
+	rtlpriv->cfg->ops->tx_polling(hw, BEACON_QUEUE);
+
+	return true;
+}
+
+#define BEACON_PG		0 /* ->1 */
+#define PSPOLL_PG		2
+#define NULL_PG			3
+#define PROBERSP_PG		4 /* ->5 */
+
+#define TOTAL_RESERVED_PKT_LEN	768
+
+static u8 reserved_page_packet[TOTAL_RESERVED_PKT_LEN] = {
+	/* page 0 beacon */
+	0x80, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
+	0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 0x50, 0x08,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x64, 0x00, 0x00, 0x04, 0x00, 0x0C, 0x6C, 0x69,
+	0x6E, 0x6B, 0x73, 0x79, 0x73, 0x5F, 0x77, 0x6C,
+	0x61, 0x6E, 0x01, 0x04, 0x82, 0x84, 0x8B, 0x96,
+	0x03, 0x01, 0x01, 0x06, 0x02, 0x00, 0x00, 0x2A,
+	0x01, 0x00, 0x32, 0x08, 0x24, 0x30, 0x48, 0x6C,
+	0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C, 0x18,
+	0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x3D, 0x00, 0xDD, 0x06, 0x00, 0xE0, 0x4C, 0x02,
+	0x01, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* page 1 beacon */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x10, 0x00, 0x20, 0x8C, 0x00, 0x12, 0x10, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* page 2  ps-poll */
+	0xA4, 0x10, 0x01, 0xC0, 0x00, 0x40, 0x10, 0x10,
+	0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x18, 0x00, 0x20, 0x8C, 0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80,
+	0x80, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* page 3  null */
+	0x48, 0x01, 0x00, 0x00, 0x00, 0x40, 0x10, 0x10,
+	0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
+	0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x72, 0x00, 0x20, 0x8C, 0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80,
+	0x80, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* page 4  probe_resp */
+	0x50, 0x00, 0x00, 0x00, 0x00, 0x40, 0x10, 0x10,
+	0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
+	0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 0x00, 0x00,
+	0x9E, 0x46, 0x15, 0x32, 0x27, 0xF2, 0x2D, 0x00,
+	0x64, 0x00, 0x00, 0x04, 0x00, 0x0C, 0x6C, 0x69,
+	0x6E, 0x6B, 0x73, 0x79, 0x73, 0x5F, 0x77, 0x6C,
+	0x61, 0x6E, 0x01, 0x04, 0x82, 0x84, 0x8B, 0x96,
+	0x03, 0x01, 0x01, 0x06, 0x02, 0x00, 0x00, 0x2A,
+	0x01, 0x00, 0x32, 0x08, 0x24, 0x30, 0x48, 0x6C,
+	0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C, 0x18,
+	0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x3D, 0x00, 0xDD, 0x06, 0x00, 0xE0, 0x4C, 0x02,
+	0x01, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* page 5  probe_resp */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+};
+
+void rtl88e_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct sk_buff *skb = NULL;
+
+	u32 totalpacketlen;
+	u8 u1RsvdPageLoc[5] = { 0 };
+
+	u8 *beacon;
+	u8 *pspoll;
+	u8 *nullfunc;
+	u8 *probersp;
+	/*---------------------------------------------------------
+	 *			(1) beacon
+	 *---------------------------------------------------------
+	 */
+	beacon = &reserved_page_packet[BEACON_PG * 128];
+	SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
+
+	/*-------------------------------------------------------
+	 *			(2) ps-poll
+	 *--------------------------------------------------------
+	 */
+	pspoll = &reserved_page_packet[PSPOLL_PG * 128];
+	SET_80211_PS_POLL_AID(pspoll, (mac->assoc_id | 0xc000));
+	SET_80211_PS_POLL_BSSID(pspoll, mac->bssid);
+	SET_80211_PS_POLL_TA(pspoll, mac->mac_addr);
+
+	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1RsvdPageLoc, PSPOLL_PG);
+
+	/*--------------------------------------------------------
+	 *			(3) null data
+	 *---------------------------------------------------------
+	 */
+	nullfunc = &reserved_page_packet[NULL_PG * 128];
+	SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
+	SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
+
+	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1RsvdPageLoc, NULL_PG);
+
+	/*---------------------------------------------------------
+	 *			(4) probe response
+	 *----------------------------------------------------------
+	 */
+	probersp = &reserved_page_packet[PROBERSP_PG * 128];
+	SET_80211_HDR_ADDRESS1(probersp, mac->bssid);
+	SET_80211_HDR_ADDRESS2(probersp, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(probersp, mac->bssid);
+
+	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1RsvdPageLoc, PROBERSP_PG);
+
+	totalpacketlen = TOTAL_RESERVED_PKT_LEN;
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
+		      "rtl88e_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
+		      &reserved_page_packet[0], totalpacketlen);
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+		      "rtl88e_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
+		      u1RsvdPageLoc, 3);
+
+	skb = dev_alloc_skb(totalpacketlen);
+	if (!skb)
+		return;
+	kmemleak_not_leak(skb);
+	memcpy(skb_put(skb, totalpacketlen),
+	       &reserved_page_packet, totalpacketlen);
+
+	if (_rtl88e_cmd_send_packet(hw, skb)) {
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 "Set RSVD page location to Fw.\n");
+		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+			      "H2C_RSVDPAGE:\n", u1RsvdPageLoc, 3);
+		rtl88e_fill_h2c_cmd(hw, H2C_88E_RSVDPAGE,
+				    sizeof(u1RsvdPageLoc), u1RsvdPageLoc);
+	} else
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Set RSVD page location to Fw FAIL!!!!!!.\n");
+}
+
+/*Shoud check FW support p2p or not.*/
+static void rtl88e_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw, u8 ctwindow)
+{
+	u8 u1_ctwindow_period[1] = {ctwindow};
+
+	rtl88e_fill_h2c_cmd(hw, H2C_88E_P2P_PS_CTW_CMD, 1, u1_ctwindow_period);
+}
+
+void rtl88e_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
+	struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
+	u8	i;
+	u16	ctwindow;
+	u32	start_time, tsf_low;
+
+	switch (p2p_ps_state) {
+	case P2P_PS_DISABLE:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
+		memset(p2p_ps_offload, 0, sizeof(struct p2p_ps_offload_t));
+		break;
+	case P2P_PS_ENABLE:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
+		/* update CTWindow value. */
+		if (p2pinfo->ctwindow > 0) {
+			p2p_ps_offload->ctwindow_en = 1;
+			ctwindow = p2pinfo->ctwindow;
+			rtl88e_set_p2p_ctw_period_cmd(hw, ctwindow);
+		}
+		/* hw only support 2 set of NoA */
+		for (i = 0; i < p2pinfo->noa_num; i++) {
+			/* To control the register setting for which NOA*/
+			rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
+			if (i == 0)
+				p2p_ps_offload->noa0_en = 1;
+			else
+				p2p_ps_offload->noa1_en = 1;
+
+			/* config P2P NoA Descriptor Register */
+			rtl_write_dword(rtlpriv, 0x5E0,
+					p2pinfo->noa_duration[i]);
+			rtl_write_dword(rtlpriv, 0x5E4,
+					p2pinfo->noa_interval[i]);
+
+			/*Get Current TSF value */
+			tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
+
+			start_time = p2pinfo->noa_start_time[i];
+			if (p2pinfo->noa_count_type[i] != 1) {
+				while (start_time <= (tsf_low + (50 * 1024))) {
+					start_time += p2pinfo->noa_interval[i];
+					if (p2pinfo->noa_count_type[i] != 255)
+						p2pinfo->noa_count_type[i]--;
+				}
+			}
+			rtl_write_dword(rtlpriv, 0x5E8, start_time);
+			rtl_write_dword(rtlpriv, 0x5EC,
+					p2pinfo->noa_count_type[i]);
+		}
+
+		if ((p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0)) {
+			/* rst p2p circuit */
+			rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
+
+			p2p_ps_offload->offload_en = 1;
+
+			if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
+				p2p_ps_offload->role = 1;
+				p2p_ps_offload->allstasleep = 0;
+			} else {
+				p2p_ps_offload->role = 0;
+			}
+
+			p2p_ps_offload->discovery = 0;
+		}
+		break;
+	case P2P_PS_SCAN:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN\n");
+		p2p_ps_offload->discovery = 1;
+		break;
+	case P2P_PS_SCAN_DONE:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN_DONE\n");
+		p2p_ps_offload->discovery = 0;
+		p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
+		break;
+	default:
+		break;
+	}
+
+	rtl88e_fill_h2c_cmd(hw, H2C_88E_P2P_PS_OFFLOAD, 1,
+			    (u8 *)p2p_ps_offload);
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/fw.h b/drivers/net/wireless/rtlwifi/rtl8188ee/fw.h
new file mode 100644
index 000000000000..854a9875cd5f
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/fw.h
@@ -0,0 +1,301 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92C__FW__H__
+#define __RTL92C__FW__H__
+
+#define FW_8192C_SIZE				0x8000
+#define FW_8192C_START_ADDRESS			0x1000
+#define FW_8192C_END_ADDRESS			0x5FFF
+#define FW_8192C_PAGE_SIZE			4096
+#define FW_8192C_POLLING_DELAY			5
+#define FW_8192C_POLLING_TIMEOUT_COUNT		3000
+
+#define IS_FW_HEADER_EXIST(_pfwhdr)		\
+	((_pfwhdr->signature&0xFFFF) == 0x88E1)
+#define USE_OLD_WOWLAN_DEBUG_FW			0
+
+#define H2C_88E_RSVDPAGE_LOC_LEN		5
+#define H2C_88E_PWEMODE_LENGTH			5
+#define H2C_88E_JOINBSSRPT_LENGTH		1
+#define H2C_88E_AP_OFFLOAD_LENGTH		3
+#define H2C_88E_WOWLAN_LENGTH			3
+#define H2C_88E_KEEP_ALIVE_CTRL_LENGTH		3
+#if (USE_OLD_WOWLAN_DEBUG_FW == 0)
+#define H2C_88E_REMOTE_WAKE_CTRL_LEN		1
+#else
+#define H2C_88E_REMOTE_WAKE_CTRL_LEN		3
+#endif
+#define H2C_88E_AOAC_GLOBAL_INFO_LEN		2
+#define H2C_88E_AOAC_RSVDPAGE_LOC_LEN		7
+
+/* Fw PS state for RPWM.
+ * BIT[2:0] = HW state
+ * BIT[3] = Protocol PS state, 1: register active state, 0: register sleep state
+ * BIT[4] = sub-state
+ */
+#define	FW_PS_GO_ON			BIT(0)
+#define	FW_PS_TX_NULL			BIT(1)
+#define	FW_PS_RF_ON			BIT(2)
+#define	FW_PS_REGISTER_ACTIVE		BIT(3)
+
+#define	FW_PS_DPS			BIT(0)
+#define	FW_PS_LCLK			(FW_PS_DPS)
+#define	FW_PS_RF_OFF			BIT(1)
+#define	FW_PS_ALL_ON			BIT(2)
+#define	FW_PS_ST_ACTIVE			BIT(3)
+#define	FW_PS_ISR_ENABLE		BIT(4)
+#define	FW_PS_IMR_ENABLE		BIT(5)
+
+
+#define	FW_PS_ACK			BIT(6)
+#define	FW_PS_TOGGLE			BIT(7)
+
+ /* 88E RPWM value*/
+ /* BIT[0] = 1: 32k, 0: 40M*/
+#define	FW_PS_CLOCK_OFF			BIT(0)		/* 32k*/
+#define	FW_PS_CLOCK_ON			0		/*40M*/
+
+#define	FW_PS_STATE_MASK		(0x0F)
+#define	FW_PS_STATE_HW_MASK		(0x07)
+/*ISR_ENABLE, IMR_ENABLE, and PS mode should be inherited.*/
+#define	FW_PS_STATE_INT_MASK		(0x3F)
+
+#define	FW_PS_STATE(x)			(FW_PS_STATE_MASK & (x))
+#define	FW_PS_STATE_HW(x)		(FW_PS_STATE_HW_MASK & (x))
+#define	FW_PS_STATE_INT(x)		(FW_PS_STATE_INT_MASK & (x))
+#define	FW_PS_ISR_VAL(x)		((x) & 0x70)
+#define	FW_PS_IMR_MASK(x)		((x) & 0xDF)
+#define	FW_PS_KEEP_IMR(x)		((x) & 0x20)
+
+#define	FW_PS_STATE_S0			(FW_PS_DPS)
+#define	FW_PS_STATE_S1			(FW_PS_LCLK)
+#define	FW_PS_STATE_S2			(FW_PS_RF_OFF)
+#define	FW_PS_STATE_S3			(FW_PS_ALL_ON)
+#define	FW_PS_STATE_S4			((FW_PS_ST_ACTIVE) | (FW_PS_ALL_ON))
+
+#define	FW_PS_STATE_ALL_ON_88E		(FW_PS_CLOCK_ON)
+#define	FW_PS_STATE_RF_ON_88E		(FW_PS_CLOCK_ON)
+#define	FW_PS_STATE_RF_OFF_88E		(FW_PS_CLOCK_ON)
+#define	FW_PS_STATE_RF_OFF_LOW_PWR_88E	(FW_PS_CLOCK_OFF)
+
+#define	FW_PS_STATE_ALL_ON_92C		(FW_PS_STATE_S4)
+#define	FW_PS_STATE_RF_ON_92C		(FW_PS_STATE_S3)
+#define	FW_PS_STATE_RF_OFF_92C		(FW_PS_STATE_S2)
+#define	FW_PS_STATE_RF_OFF_LOW_PWR_92C	(FW_PS_STATE_S1)
+
+/* For 88E H2C PwrMode Cmd ID 5.*/
+#define	FW_PWR_STATE_ACTIVE	((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))
+#define	FW_PWR_STATE_RF_OFF		0
+
+#define	FW_PS_IS_ACK(x)			((x) & FW_PS_ACK)
+#define	FW_PS_IS_CLK_ON(x)		((x) & (FW_PS_RF_OFF | FW_PS_ALL_ON))
+#define	FW_PS_IS_RF_ON(x)		((x) & (FW_PS_ALL_ON))
+#define	FW_PS_IS_ACTIVE(x)		((x) & (FW_PS_ST_ACTIVE))
+#define	FW_PS_IS_CPWM_INT(x)		((x) & 0x40)
+
+#define	FW_CLR_PS_STATE(x)		((x) = ((x) & (0xF0)))
+
+#define	IS_IN_LOW_POWER_STATE_88E(fwpsstate)		\
+	(FW_PS_STATE(fwpsstate) == FW_PS_CLOCK_OFF)
+
+#define	FW_PWR_STATE_ACTIVE	((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))
+#define	FW_PWR_STATE_RF_OFF		0
+
+struct rtl92c_firmware_header {
+	u16 signature;
+	u8 category;
+	u8 function;
+	u16 version;
+	u8 subversion;
+	u8 rsvd1;
+	u8 month;
+	u8 date;
+	u8 hour;
+	u8 minute;
+	u16 ramcodesize;
+	u16 rsvd2;
+	u32 svnindex;
+	u32 rsvd3;
+	u32 rsvd4;
+	u32 rsvd5;
+};
+
+enum rtl8192c_h2c_cmd {
+	H2C_88E_RSVDPAGE = 0,
+	H2C_88E_JOINBSSRPT = 1,
+	H2C_88E_SCAN = 2,
+	H2C_88E_KEEP_ALIVE_CTRL = 3,
+	H2C_88E_DISCONNECT_DECISION = 4,
+#if (USE_OLD_WOWLAN_DEBUG_FW == 1)
+	H2C_88E_WO_WLAN = 5,
+#endif
+	H2C_88E_INIT_OFFLOAD = 6,
+#if (USE_OLD_WOWLAN_DEBUG_FW == 1)
+	H2C_88E_REMOTE_WAKE_CTRL = 7,
+#endif
+	H2C_88E_AP_OFFLOAD = 8,
+	H2C_88E_BCN_RSVDPAGE = 9,
+	H2C_88E_PROBERSP_RSVDPAGE = 10,
+
+	H2C_88E_SETPWRMODE = 0x20,
+	H2C_88E_PS_TUNING_PARA = 0x21,
+	H2C_88E_PS_TUNING_PARA2 = 0x22,
+	H2C_88E_PS_LPS_PARA = 0x23,
+	H2C_88E_P2P_PS_OFFLOAD = 024,
+
+#if (USE_OLD_WOWLAN_DEBUG_FW == 0)
+	H2C_88E_WO_WLAN = 0x80,
+	H2C_88E_REMOTE_WAKE_CTRL = 0x81,
+	H2C_88E_AOAC_GLOBAL_INFO = 0x82,
+	H2C_88E_AOAC_RSVDPAGE = 0x83,
+#endif
+	/* Not defined in new 88E H2C CMD Format */
+	H2C_88E_RA_MASK,
+	H2C_88E_SELECTIVE_SUSPEND_ROF_CMD,
+	H2C_88E_P2P_PS_MODE,
+	H2C_88E_PSD_RESULT,
+	/*Not defined CTW CMD for P2P yet*/
+	H2C_88E_P2P_PS_CTW_CMD,
+	MAX_88E_H2CCMD
+};
+
+#define pagenum_128(_len)	(u32)(((_len)>>7) + ((_len)&0x7F ? 1 : 0))
+
+#define SET_88E_H2CCMD_WOWLAN_FUNC_ENABLE(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE(__cmd, 0, 1, __value)
+#define SET_88E_H2CCMD_WOWLAN_PATTERN_MATCH_ENABLE(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 1, 1, __value)
+#define SET_88E_H2CCMD_WOWLAN_MAGIC_PKT_ENABLE(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 2, 1, __value)
+#define SET_88E_H2CCMD_WOWLAN_UNICAST_PKT_ENABLE(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 3, 1, __value)
+#define SET_88E_H2CCMD_WOWLAN_ALL_PKT_DROP(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE(__cmd, 4, 1, __value)
+#define SET_88E_H2CCMD_WOWLAN_GPIO_ACTIVE(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE(__cmd, 5, 1, __value)
+#define SET_88E_H2CCMD_WOWLAN_REKEY_WAKE_UP(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE(__cmd, 6, 1, __value)
+#define SET_88E_H2CCMD_WOWLAN_DISCONNECT_WAKE_UP(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 7, 1, __value)
+#define SET_88E_H2CCMD_WOWLAN_GPIONUM(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 0, 8, __value)
+#define SET_88E_H2CCMD_WOWLAN_GPIO_DURATION(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE((__cmd)+2, 0, 8, __value)
+
+
+#define SET_H2CCMD_PWRMODE_PARM_MODE(__ph2ccmd, __val)			\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_H2CCMD_PWRMODE_PARM_RLBM(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 0, 4, __value)
+#define SET_H2CCMD_PWRMODE_PARM_SMART_PS(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 4, 4, __value)
+#define SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+2, 0, 8, __value)
+#define SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+3, 0, 8, __value)
+#define SET_H2CCMD_PWRMODE_PARM_PWR_STATE(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE((__cmd)+4, 0, 8, __value)
+#define GET_88E_H2CCMD_PWRMODE_PARM_MODE(__cmd)			\
+	LE_BITS_TO_1BYTE(__cmd, 0, 8)
+
+#define SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
+#define SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
+
+/* AP_OFFLOAD */
+#define SET_H2CCMD_AP_OFFLOAD_ON(__cmd, __value)			\
+	SET_BITS_TO_LE_1BYTE(__cmd, 0, 8, __value)
+#define SET_H2CCMD_AP_OFFLOAD_HIDDEN(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 0, 8, __value)
+#define SET_H2CCMD_AP_OFFLOAD_DENYANY(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE((__cmd)+2, 0, 8, __value)
+#define SET_H2CCMD_AP_OFFLOAD_WAKEUP_EVT_RPT(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+3, 0, 8, __value)
+
+/* Keep Alive Control*/
+#define SET_88E_H2CCMD_KEEP_ALIVE_ENABLE(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE(__cmd, 0, 1, __value)
+#define SET_88E_H2CCMD_KEEP_ALIVE_ACCPEPT_USER_DEFINED(__cmd, __value) \
+	SET_BITS_TO_LE_1BYTE(__cmd, 1, 1, __value)
+#define SET_88E_H2CCMD_KEEP_ALIVE_PERIOD(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 0, 8, __value)
+
+/*REMOTE_WAKE_CTRL */
+#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_EN(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE(__cmd, 0, 1, __value)
+#if (USE_OLD_WOWLAN_DEBUG_FW == 0)
+#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_ARP_OFFLOAD_EN(__cmd, __value) \
+	SET_BITS_TO_LE_1BYTE(__cmd, 1, 1, __value)
+#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_NDP_OFFLOAD_EN(__cmd, __value) \
+	SET_BITS_TO_LE_1BYTE(__cmd, 2, 1, __value)
+#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_GTK_OFFLOAD_EN(__cmd, __value) \
+	SET_BITS_TO_LE_1BYTE(__cmd, 3, 1, __value)
+#else
+#define SET_88E_H2_REM_WAKE_ENC_ALG(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 0, 8, __value)
+#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_GROUP_ENC_ALG(__cmd, __value) \
+	SET_BITS_TO_LE_1BYTE((__cmd)+2, 0, 8, __value)
+#endif
+
+/* GTK_OFFLOAD */
+#define SET_88E_H2CCMD_AOAC_GLOBAL_INFO_PAIRWISE_ENC_ALG(__cmd, __value) \
+	SET_BITS_TO_LE_1BYTE(__cmd, 0, 8, __value)
+#define SET_88E_H2CCMD_AOAC_GLOBAL_INFO_GROUP_ENC_ALG(__cmd, __value) \
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 0, 8, __value)
+
+/* AOAC_RSVDPAGE_LOC */
+#define SET_88E_H2CCMD_AOAC_RSVD_LOC_REM_WAKE_CTRL_INFO(__cmd, __value) \
+	SET_BITS_TO_LE_1BYTE((__cmd), 0, 8, __value)
+#define SET_88E_H2CCMD_AOAC_RSVDPAGE_LOC_ARP_RSP(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 0, 8, __value)
+#define SET_88E_H2CCMD_AOAC_RSVDPAGE_LOC_NEIGHBOR_ADV(__cmd, __value) \
+	SET_BITS_TO_LE_1BYTE((__cmd)+2, 0, 8, __value)
+#define SET_88E_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_RSP(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+3, 0, 8, __value)
+#define SET_88E_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_INFO(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+4, 0, 8, __value)
+
+int rtl88e_download_fw(struct ieee80211_hw *hw,
+		       bool buse_wake_on_wlan_fw);
+void rtl88e_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
+			 u32 cmd_len, u8 *p_cmdbuffer);
+void rtl88e_firmware_selfreset(struct ieee80211_hw *hw);
+void rtl88e_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
+void rtl88e_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw,
+				      u8 mstatus);
+void rtl88e_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,  u8 enable);
+void rtl88e_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
+void rtl88e_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c b/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c
new file mode 100644
index 000000000000..b68cae3024fc
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c
@@ -0,0 +1,2530 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../efuse.h"
+#include "../base.h"
+#include "../regd.h"
+#include "../cam.h"
+#include "../ps.h"
+#include "../pci.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "dm.h"
+#include "fw.h"
+#include "led.h"
+#include "hw.h"
+#include "pwrseqcmd.h"
+#include "pwrseq.h"
+
+#define LLT_CONFIG		5
+
+static void _rtl88ee_set_bcn_ctrl_reg(struct ieee80211_hw *hw,
+				      u8 set_bits, u8 clear_bits)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpci->reg_bcn_ctrl_val |= set_bits;
+	rtlpci->reg_bcn_ctrl_val &= ~clear_bits;
+
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8) rtlpci->reg_bcn_ctrl_val);
+}
+
+static void _rtl88ee_stop_tx_beacon(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp1byte;
+
+	tmp1byte = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp1byte & (~BIT(6)));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0x64);
+	tmp1byte = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
+	tmp1byte &= ~(BIT(0));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp1byte);
+}
+
+static void _rtl88ee_resume_tx_beacon(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp1byte;
+
+	tmp1byte = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp1byte | BIT(6));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0xff);
+	tmp1byte = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
+	tmp1byte |= BIT(0);
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp1byte);
+}
+
+static void _rtl88ee_enable_bcn_sub_func(struct ieee80211_hw *hw)
+{
+	_rtl88ee_set_bcn_ctrl_reg(hw, 0, BIT(1));
+}
+
+static void _rtl88ee_return_beacon_queue_skb(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[BEACON_QUEUE];
+
+	while (skb_queue_len(&ring->queue)) {
+		struct rtl_tx_desc *entry = &ring->desc[ring->idx];
+		struct sk_buff *skb = __skb_dequeue(&ring->queue);
+
+		pci_unmap_single(rtlpci->pdev,
+				 rtlpriv->cfg->ops->get_desc(
+				 (u8 *)entry, true, HW_DESC_TXBUFF_ADDR),
+				 skb->len, PCI_DMA_TODEVICE);
+		kfree_skb(skb);
+		ring->idx = (ring->idx + 1) % ring->entries;
+	}
+}
+
+static void _rtl88ee_disable_bcn_sub_func(struct ieee80211_hw *hw)
+{
+	_rtl88ee_set_bcn_ctrl_reg(hw, BIT(1), 0);
+}
+
+static void _rtl88ee_set_fw_clock_on(struct ieee80211_hw *hw,
+				     u8 rpwm_val, bool need_turn_off_ckk)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool support_remote_wake_up;
+	u32 count = 0, isr_regaddr, content;
+	bool schedule_timer = need_turn_off_ckk;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
+				      (u8 *)(&support_remote_wake_up));
+	if (!rtlhal->fw_ready)
+		return;
+	if (!rtlpriv->psc.fw_current_inpsmode)
+		return;
+
+	while (1) {
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		if (rtlhal->fw_clk_change_in_progress) {
+			while (rtlhal->fw_clk_change_in_progress) {
+				spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+				udelay(100);
+				if (++count > 1000)
+					return;
+				spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+			}
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		} else {
+			rtlhal->fw_clk_change_in_progress = false;
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		}
+	}
+
+	if (IS_IN_LOW_POWER_STATE_88E(rtlhal->fw_ps_state)) {
+		rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_SET_RPWM,
+					      (u8 *)(&rpwm_val));
+		if (FW_PS_IS_ACK(rpwm_val)) {
+			isr_regaddr = REG_HISR;
+			content = rtl_read_dword(rtlpriv, isr_regaddr);
+			while (!(content & IMR_CPWM) && (count < 500)) {
+				udelay(50);
+				count++;
+				content = rtl_read_dword(rtlpriv, isr_regaddr);
+			}
+
+			if (content & IMR_CPWM) {
+				rtl_write_word(rtlpriv, isr_regaddr, 0x0100);
+				rtlhal->fw_ps_state = FW_PS_STATE_RF_ON_88E;
+				RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+					 "Receive CPWM INT!!! Set pHalData->FwPSState = %X\n",
+					 rtlhal->fw_ps_state);
+			}
+		}
+
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		rtlhal->fw_clk_change_in_progress = false;
+		spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		if (schedule_timer) {
+			mod_timer(&rtlpriv->works.fw_clockoff_timer,
+				  jiffies + MSECS(10));
+		}
+	} else  {
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		rtlhal->fw_clk_change_in_progress = false;
+		spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+	}
+}
+
+static void _rtl88ee_set_fw_clock_off(struct ieee80211_hw *hw,
+				      u8 rpwm_val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring;
+	enum rf_pwrstate rtstate;
+	bool schedule_timer = false;
+	u8 queue;
+
+	if (!rtlhal->fw_ready)
+		return;
+	if (!rtlpriv->psc.fw_current_inpsmode)
+		return;
+	if (!rtlhal->allow_sw_to_change_hwclc)
+		return;
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RF_STATE, (u8 *)(&rtstate));
+	if (rtstate == ERFOFF || rtlpriv->psc.inactive_pwrstate == ERFOFF)
+		return;
+
+	for (queue = 0; queue < RTL_PCI_MAX_TX_QUEUE_COUNT; queue++) {
+		ring = &rtlpci->tx_ring[queue];
+		if (skb_queue_len(&ring->queue)) {
+			schedule_timer = true;
+			break;
+		}
+	}
+
+	if (schedule_timer) {
+		mod_timer(&rtlpriv->works.fw_clockoff_timer,
+			  jiffies + MSECS(10));
+		return;
+	}
+
+	if (FW_PS_STATE(rtlhal->fw_ps_state) !=
+	    FW_PS_STATE_RF_OFF_LOW_PWR_88E) {
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		if (!rtlhal->fw_clk_change_in_progress) {
+			rtlhal->fw_clk_change_in_progress = true;
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+			rtlhal->fw_ps_state = FW_PS_STATE(rpwm_val);
+			rtl_write_word(rtlpriv, REG_HISR, 0x0100);
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+						      (u8 *)(&rpwm_val));
+			spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+			rtlhal->fw_clk_change_in_progress = false;
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		} else {
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+			mod_timer(&rtlpriv->works.fw_clockoff_timer,
+				  jiffies + MSECS(10));
+		}
+	}
+}
+
+static void _rtl88ee_set_fw_ps_rf_on(struct ieee80211_hw *hw)
+{
+	u8 rpwm_val = 0;
+
+	rpwm_val |= (FW_PS_STATE_RF_OFF_88E | FW_PS_ACK);
+	_rtl88ee_set_fw_clock_on(hw, rpwm_val, true);
+}
+
+static void _rtl88ee_set_fw_ps_rf_off_low_power(struct ieee80211_hw *hw)
+{
+	u8 rpwm_val = 0;
+
+	rpwm_val |= FW_PS_STATE_RF_OFF_LOW_PWR_88E;
+	_rtl88ee_set_fw_clock_off(hw, rpwm_val);
+}
+
+void rtl88ee_fw_clk_off_timer_callback(unsigned long data)
+{
+	struct ieee80211_hw *hw = (struct ieee80211_hw *)data;
+
+	_rtl88ee_set_fw_ps_rf_off_low_power(hw);
+}
+
+static void _rtl88ee_fwlps_leave(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool fw_current_inps = false;
+	u8 rpwm_val = 0, fw_pwrmode = FW_PS_ACTIVE_MODE;
+
+	if (ppsc->low_power_enable) {
+		rpwm_val = (FW_PS_STATE_ALL_ON_88E|FW_PS_ACK);/* RF on */
+		_rtl88ee_set_fw_clock_on(hw, rpwm_val, false);
+		rtlhal->allow_sw_to_change_hwclc = false;
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+					      (u8 *)(&fw_pwrmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+					      (u8 *)(&fw_current_inps));
+	} else {
+		rpwm_val = FW_PS_STATE_ALL_ON_88E;	/* RF on */
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+					      (u8 *)(&rpwm_val));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+					      (u8 *)(&fw_pwrmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+					      (u8 *)(&fw_current_inps));
+	}
+}
+
+static void _rtl88ee_fwlps_enter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool fw_current_inps = true;
+	u8 rpwm_val;
+
+	if (ppsc->low_power_enable) {
+		rpwm_val = FW_PS_STATE_RF_OFF_LOW_PWR_88E;	/* RF off */
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+					      (u8 *)(&fw_current_inps));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+					      (u8 *)(&ppsc->fwctrl_psmode));
+		rtlhal->allow_sw_to_change_hwclc = true;
+		_rtl88ee_set_fw_clock_off(hw, rpwm_val);
+	} else {
+		rpwm_val = FW_PS_STATE_RF_OFF_88E;	/* RF off */
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+					      (u8 *)(&fw_current_inps));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+					      (u8 *)(&ppsc->fwctrl_psmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+					      (u8 *)(&rpwm_val));
+	}
+}
+
+void rtl88ee_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	switch (variable) {
+	case HW_VAR_RCR:
+		*((u32 *)(val)) = rtlpci->receive_config;
+		break;
+	case HW_VAR_RF_STATE:
+		*((enum rf_pwrstate *)(val)) = ppsc->rfpwr_state;
+		break;
+	case HW_VAR_FWLPS_RF_ON:{
+			enum rf_pwrstate rfstate;
+			u32 val_rcr;
+
+			rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RF_STATE,
+						      (u8 *)(&rfstate));
+			if (rfstate == ERFOFF) {
+				*((bool *)(val)) = true;
+			} else {
+				val_rcr = rtl_read_dword(rtlpriv, REG_RCR);
+				val_rcr &= 0x00070000;
+				if (val_rcr)
+					*((bool *)(val)) = false;
+				else
+					*((bool *)(val)) = true;
+			}
+			break;
+		}
+	case HW_VAR_FW_PSMODE_STATUS:
+		*((bool *)(val)) = ppsc->fw_current_inpsmode;
+		break;
+	case HW_VAR_CORRECT_TSF:{
+		u64 tsf;
+		u32 *ptsf_low = (u32 *)&tsf;
+		u32 *ptsf_high = ((u32 *)&tsf) + 1;
+
+		*ptsf_high = rtl_read_dword(rtlpriv, (REG_TSFTR + 4));
+		*ptsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
+
+		*((u64 *)(val)) = tsf;
+		break; }
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process %x\n", variable);
+		break;
+	}
+}
+
+void rtl88ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u8 idx;
+
+	switch (variable) {
+	case HW_VAR_ETHER_ADDR:
+		for (idx = 0; idx < ETH_ALEN; idx++)
+			rtl_write_byte(rtlpriv, (REG_MACID + idx), val[idx]);
+		break;
+	case HW_VAR_BASIC_RATE:{
+		u16 rate_cfg = ((u16 *)val)[0];
+		u8 rate_index = 0;
+		rate_cfg = rate_cfg & 0x15f;
+		rate_cfg |= 0x01;
+		rtl_write_byte(rtlpriv, REG_RRSR, rate_cfg & 0xff);
+		rtl_write_byte(rtlpriv, REG_RRSR + 1, (rate_cfg >> 8) & 0xff);
+		while (rate_cfg > 0x1) {
+			rate_cfg = (rate_cfg >> 1);
+			rate_index++;
+		}
+		rtl_write_byte(rtlpriv, REG_INIRTS_RATE_SEL, rate_index);
+		break; }
+	case HW_VAR_BSSID:
+		for (idx = 0; idx < ETH_ALEN; idx++)
+			rtl_write_byte(rtlpriv, (REG_BSSID + idx), val[idx]);
+		break;
+	case HW_VAR_SIFS:
+		rtl_write_byte(rtlpriv, REG_SIFS_CTX + 1, val[0]);
+		rtl_write_byte(rtlpriv, REG_SIFS_TRX + 1, val[1]);
+
+		rtl_write_byte(rtlpriv, REG_SPEC_SIFS + 1, val[0]);
+		rtl_write_byte(rtlpriv, REG_MAC_SPEC_SIFS + 1, val[0]);
+
+		if (!mac->ht_enable)
+			rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM, 0x0e0e);
+		else
+			rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM,
+				       *((u16 *)val));
+		break;
+	case HW_VAR_SLOT_TIME:{
+		u8 e_aci;
+
+		RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+			 "HW_VAR_SLOT_TIME %x\n", val[0]);
+
+		rtl_write_byte(rtlpriv, REG_SLOT, val[0]);
+
+		for (e_aci = 0; e_aci < AC_MAX; e_aci++) {
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
+						      (u8 *)(&e_aci));
+		}
+		break; }
+	case HW_VAR_ACK_PREAMBLE:{
+		u8 reg_tmp;
+		u8 short_preamble = (bool) (*(u8 *)val);
+		reg_tmp = rtl_read_byte(rtlpriv, REG_TRXPTCL_CTL+2);
+		if (short_preamble) {
+			reg_tmp |= 0x02;
+			rtl_write_byte(rtlpriv, REG_TRXPTCL_CTL + 2, reg_tmp);
+		} else {
+			reg_tmp |= 0xFD;
+			rtl_write_byte(rtlpriv, REG_TRXPTCL_CTL + 2, reg_tmp);
+		}
+		break; }
+	case HW_VAR_WPA_CONFIG:
+		rtl_write_byte(rtlpriv, REG_SECCFG, *((u8 *)val));
+		break;
+	case HW_VAR_AMPDU_MIN_SPACE:{
+		u8 min_spacing_to_set;
+		u8 sec_min_space;
+
+		min_spacing_to_set = *((u8 *)val);
+		if (min_spacing_to_set <= 7) {
+			sec_min_space = 0;
+
+			if (min_spacing_to_set < sec_min_space)
+				min_spacing_to_set = sec_min_space;
+
+			mac->min_space_cfg = ((mac->min_space_cfg &
+					       0xf8) | min_spacing_to_set);
+
+			*val = min_spacing_to_set;
+
+			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+				 "Set HW_VAR_AMPDU_MIN_SPACE: %#x\n",
+				  mac->min_space_cfg);
+
+			rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
+				       mac->min_space_cfg);
+		}
+		break; }
+	case HW_VAR_SHORTGI_DENSITY:{
+		u8 density_to_set;
+
+		density_to_set = *((u8 *)val);
+		mac->min_space_cfg |= (density_to_set << 3);
+
+		RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+			 "Set HW_VAR_SHORTGI_DENSITY: %#x\n",
+			  mac->min_space_cfg);
+
+		rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
+			       mac->min_space_cfg);
+		break; }
+	case HW_VAR_AMPDU_FACTOR:{
+		u8 regtoset_normal[4] = { 0x41, 0xa8, 0x72, 0xb9 };
+		u8 factor;
+		u8 *reg = NULL;
+		u8 id = 0;
+
+		reg = regtoset_normal;
+
+		factor = *((u8 *)val);
+		if (factor <= 3) {
+			factor = (1 << (factor + 2));
+			if (factor > 0xf)
+				factor = 0xf;
+
+			for (id = 0; id < 4; id++) {
+				if ((reg[id] & 0xf0) > (factor << 4))
+					reg[id] = (reg[id] & 0x0f) |
+						  (factor << 4);
+
+				if ((reg[id] & 0x0f) > factor)
+					reg[id] = (reg[id] & 0xf0) | (factor);
+
+				rtl_write_byte(rtlpriv, (REG_AGGLEN_LMT + id),
+					       reg[id]);
+			}
+
+			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+				 "Set HW_VAR_AMPDU_FACTOR: %#x\n", factor);
+		}
+		break; }
+	case HW_VAR_AC_PARAM:{
+		u8 e_aci = *((u8 *)val);
+		rtl88e_dm_init_edca_turbo(hw);
+
+		if (rtlpci->acm_method != eAcmWay2_SW)
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACM_CTRL,
+						      (u8 *)(&e_aci));
+		break; }
+	case HW_VAR_ACM_CTRL:{
+		u8 e_aci = *((u8 *)val);
+		union aci_aifsn *p_aci_aifsn =
+		    (union aci_aifsn *)(&(mac->ac[0].aifs));
+		u8 acm = p_aci_aifsn->f.acm;
+		u8 acm_ctrl = rtl_read_byte(rtlpriv, REG_ACMHWCTRL);
+
+		acm_ctrl = acm_ctrl | ((rtlpci->acm_method == 2) ? 0x0 : 0x1);
+
+		if (acm) {
+			switch (e_aci) {
+			case AC0_BE:
+				acm_ctrl |= ACMHW_BEQEN;
+				break;
+			case AC2_VI:
+				acm_ctrl |= ACMHW_VIQEN;
+				break;
+			case AC3_VO:
+				acm_ctrl |= ACMHW_VOQEN;
+				break;
+			default:
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "HW_VAR_ACM_CTRL acm set failed: eACI is %d\n",
+					 acm);
+				break;
+			}
+		} else {
+			switch (e_aci) {
+			case AC0_BE:
+				acm_ctrl &= (~ACMHW_BEQEN);
+				break;
+			case AC2_VI:
+				acm_ctrl &= (~ACMHW_VIQEN);
+				break;
+			case AC3_VO:
+				acm_ctrl &= (~ACMHW_BEQEN);
+				break;
+			default:
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+					 "switch case not process\n");
+				break;
+			}
+		}
+
+		RT_TRACE(rtlpriv, COMP_QOS, DBG_TRACE,
+			 "SetHwReg8190pci(): [HW_VAR_ACM_CTRL] Write 0x%X\n",
+			 acm_ctrl);
+		rtl_write_byte(rtlpriv, REG_ACMHWCTRL, acm_ctrl);
+		break; }
+	case HW_VAR_RCR:
+		rtl_write_dword(rtlpriv, REG_RCR, ((u32 *)(val))[0]);
+		rtlpci->receive_config = ((u32 *)(val))[0];
+		break;
+	case HW_VAR_RETRY_LIMIT:{
+		u8 retry_limit = ((u8 *)(val))[0];
+
+		rtl_write_word(rtlpriv, REG_RL,
+			       retry_limit << RETRY_LIMIT_SHORT_SHIFT |
+			       retry_limit << RETRY_LIMIT_LONG_SHIFT);
+		break; }
+	case HW_VAR_DUAL_TSF_RST:
+		rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, (BIT(0) | BIT(1)));
+		break;
+	case HW_VAR_EFUSE_BYTES:
+		rtlefuse->efuse_usedbytes = *((u16 *)val);
+		break;
+	case HW_VAR_EFUSE_USAGE:
+		rtlefuse->efuse_usedpercentage = *((u8 *)val);
+		break;
+	case HW_VAR_IO_CMD:
+		rtl88e_phy_set_io_cmd(hw, (*(enum io_type *)val));
+		break;
+	case HW_VAR_SET_RPWM:{
+		u8 rpwm_val;
+
+		rpwm_val = rtl_read_byte(rtlpriv, REG_PCIE_HRPWM);
+		udelay(1);
+
+		if (rpwm_val & BIT(7)) {
+			rtl_write_byte(rtlpriv, REG_PCIE_HRPWM,
+				       (*(u8 *)val));
+		} else {
+			rtl_write_byte(rtlpriv, REG_PCIE_HRPWM,
+				       ((*(u8 *)val) | BIT(7)));
+		}
+		break; }
+	case HW_VAR_H2C_FW_PWRMODE:
+		rtl88e_set_fw_pwrmode_cmd(hw, (*(u8 *)val));
+		break;
+	case HW_VAR_FW_PSMODE_STATUS:
+		ppsc->fw_current_inpsmode = *((bool *)val);
+		break;
+	case HW_VAR_RESUME_CLK_ON:
+		_rtl88ee_set_fw_ps_rf_on(hw);
+		break;
+	case HW_VAR_FW_LPS_ACTION:{
+		bool enter_fwlps = *((bool *)val);
+
+		if (enter_fwlps)
+			_rtl88ee_fwlps_enter(hw);
+		 else
+			_rtl88ee_fwlps_leave(hw);
+		 break; }
+	case HW_VAR_H2C_FW_JOINBSSRPT:{
+		u8 mstatus = (*(u8 *)val);
+		u8 tmp, tmp_reg422, uval;
+		u8 count = 0, dlbcn_count = 0;
+		bool recover = false;
+
+		if (mstatus == RT_MEDIA_CONNECT) {
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AID, NULL);
+
+			tmp = rtl_read_byte(rtlpriv, REG_CR + 1);
+			rtl_write_byte(rtlpriv, REG_CR + 1, (tmp | BIT(0)));
+
+			_rtl88ee_set_bcn_ctrl_reg(hw, 0, BIT(3));
+			_rtl88ee_set_bcn_ctrl_reg(hw, BIT(4), 0);
+
+			tmp_reg422 = rtl_read_byte(rtlpriv,
+						   REG_FWHW_TXQ_CTRL + 2);
+			rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
+				       tmp_reg422 & (~BIT(6)));
+			if (tmp_reg422 & BIT(6))
+				recover = true;
+
+			do {
+				uval = rtl_read_byte(rtlpriv, REG_TDECTRL+2);
+				rtl_write_byte(rtlpriv, REG_TDECTRL+2,
+					       (uval | BIT(0)));
+				_rtl88ee_return_beacon_queue_skb(hw);
+
+				rtl88e_set_fw_rsvdpagepkt(hw, 0);
+				uval = rtl_read_byte(rtlpriv, REG_TDECTRL+2);
+				count = 0;
+				while (!(uval & BIT(0)) && count < 20) {
+					count++;
+					udelay(10);
+					uval = rtl_read_byte(rtlpriv,
+							     REG_TDECTRL+2);
+				}
+				dlbcn_count++;
+			} while (!(uval & BIT(0)) && dlbcn_count < 5);
+
+			if (uval & BIT(0))
+				rtl_write_byte(rtlpriv, REG_TDECTRL+2, BIT(0));
+
+			_rtl88ee_set_bcn_ctrl_reg(hw, BIT(3), 0);
+			_rtl88ee_set_bcn_ctrl_reg(hw, 0, BIT(4));
+
+			if (recover) {
+				rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
+					       tmp_reg422);
+			}
+			rtl_write_byte(rtlpriv, REG_CR + 1, (tmp & ~(BIT(0))));
+		}
+		rtl88e_set_fw_joinbss_report_cmd(hw, (*(u8 *)val));
+		break; }
+	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:
+		rtl88e_set_p2p_ps_offload_cmd(hw, (*(u8 *)val));
+		break;
+	case HW_VAR_AID:{
+		u16 u2btmp;
+		u2btmp = rtl_read_word(rtlpriv, REG_BCN_PSR_RPT);
+		u2btmp &= 0xC000;
+		rtl_write_word(rtlpriv, REG_BCN_PSR_RPT, (u2btmp |
+			       mac->assoc_id));
+		break; }
+	case HW_VAR_CORRECT_TSF:{
+		u8 btype_ibss = ((u8 *)(val))[0];
+
+		if (btype_ibss == true)
+			_rtl88ee_stop_tx_beacon(hw);
+
+		_rtl88ee_set_bcn_ctrl_reg(hw, 0, BIT(3));
+
+		rtl_write_dword(rtlpriv, REG_TSFTR,
+				(u32) (mac->tsf & 0xffffffff));
+		rtl_write_dword(rtlpriv, REG_TSFTR + 4,
+				(u32) ((mac->tsf >> 32) & 0xffffffff));
+
+		_rtl88ee_set_bcn_ctrl_reg(hw, BIT(3), 0);
+
+		if (btype_ibss == true)
+			_rtl88ee_resume_tx_beacon(hw);
+		break; }
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process %x\n", variable);
+		break;
+	}
+}
+
+static bool _rtl88ee_llt_write(struct ieee80211_hw *hw, u32 address, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	bool status = true;
+	long count = 0;
+	u32 value = _LLT_INIT_ADDR(address) | _LLT_INIT_DATA(data) |
+		    _LLT_OP(_LLT_WRITE_ACCESS);
+
+	rtl_write_dword(rtlpriv, REG_LLT_INIT, value);
+
+	do {
+		value = rtl_read_dword(rtlpriv, REG_LLT_INIT);
+		if (_LLT_NO_ACTIVE == _LLT_OP_VALUE(value))
+			break;
+
+		if (count > POLLING_LLT_THRESHOLD) {
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Failed to polling write LLT done at address %d!\n",
+				 address);
+			status = false;
+			break;
+		}
+	} while (++count);
+
+	return status;
+}
+
+static bool _rtl88ee_llt_table_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	unsigned short i;
+	u8 txpktbuf_bndy;
+	u8 maxpage;
+	bool status;
+
+	maxpage = 0xAF;
+	txpktbuf_bndy = 0xAB;
+
+	rtl_write_byte(rtlpriv, REG_RQPN_NPQ, 0x01);
+	rtl_write_dword(rtlpriv, REG_RQPN, 0x80730d29);
+
+
+	rtl_write_dword(rtlpriv, REG_TRXFF_BNDY, (0x25FF0000 | txpktbuf_bndy));
+	rtl_write_byte(rtlpriv, REG_TDECTRL + 1, txpktbuf_bndy);
+
+	rtl_write_byte(rtlpriv, REG_TXPKTBUF_BCNQ_BDNY, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, REG_TXPKTBUF_MGQ_BDNY, txpktbuf_bndy);
+
+	rtl_write_byte(rtlpriv, 0x45D, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, REG_PBP, 0x11);
+	rtl_write_byte(rtlpriv, REG_RX_DRVINFO_SZ, 0x4);
+
+	for (i = 0; i < (txpktbuf_bndy - 1); i++) {
+		status = _rtl88ee_llt_write(hw, i, i + 1);
+		if (true != status)
+			return status;
+	}
+
+	status = _rtl88ee_llt_write(hw, (txpktbuf_bndy - 1), 0xFF);
+	if (true != status)
+		return status;
+
+	for (i = txpktbuf_bndy; i < maxpage; i++) {
+		status = _rtl88ee_llt_write(hw, i, (i + 1));
+		if (true != status)
+			return status;
+	}
+
+	status = _rtl88ee_llt_write(hw, maxpage, txpktbuf_bndy);
+	if (true != status)
+		return status;
+
+	return true;
+}
+
+static void _rtl88ee_gen_refresh_led_state(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_led *pLed0 = &(pcipriv->ledctl.sw_led0);
+
+	if (rtlpriv->rtlhal.up_first_time)
+		return;
+
+	if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS)
+		rtl88ee_sw_led_on(hw, pLed0);
+	else if (ppsc->rfoff_reason == RF_CHANGE_BY_INIT)
+		rtl88ee_sw_led_on(hw, pLed0);
+	else
+		rtl88ee_sw_led_off(hw, pLed0);
+}
+
+static bool _rtl88ee_init_mac(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 bytetmp;
+	u16 wordtmp;
+
+	/*Disable XTAL OUTPUT for power saving. YJ, add, 111206. */
+	bytetmp = rtl_read_byte(rtlpriv, REG_XCK_OUT_CTRL) & (~BIT(0));
+	rtl_write_byte(rtlpriv, REG_XCK_OUT_CTRL, bytetmp);
+	/*Auto Power Down to CHIP-off State*/
+	bytetmp = rtl_read_byte(rtlpriv, REG_APS_FSMCO + 1) & (~BIT(7));
+	rtl_write_byte(rtlpriv, REG_APS_FSMCO + 1, bytetmp);
+
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x00);
+	/* HW Power on sequence */
+	if (!rtl88_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK,
+					PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,
+					Rtl8188E_NIC_ENABLE_FLOW)) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "init MAC Fail as rtl88_hal_pwrseqcmdparsing\n");
+		return false;
+	}
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_APS_FSMCO) | BIT(4);
+	rtl_write_byte(rtlpriv, REG_APS_FSMCO, bytetmp);
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_PCIE_CTRL_REG+2);
+	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG+2, bytetmp|BIT(2));
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_WATCH_DOG+1);
+	rtl_write_byte(rtlpriv, REG_WATCH_DOG+1, bytetmp|BIT(7));
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_AFE_XTAL_CTRL_EXT+1);
+	rtl_write_byte(rtlpriv, REG_AFE_XTAL_CTRL_EXT+1, bytetmp|BIT(1));
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_TX_RPT_CTRL);
+	rtl_write_byte(rtlpriv, REG_TX_RPT_CTRL, bytetmp|BIT(1)|BIT(0));
+	rtl_write_byte(rtlpriv, REG_TX_RPT_CTRL+1, 2);
+	rtl_write_word(rtlpriv, REG_TX_RPT_TIME, 0xcdf0);
+
+	/*Add for wake up online*/
+	bytetmp = rtl_read_byte(rtlpriv, REG_SYS_CLKR);
+
+	rtl_write_byte(rtlpriv, REG_SYS_CLKR, bytetmp|BIT(3));
+	bytetmp = rtl_read_byte(rtlpriv, REG_GPIO_MUXCFG+1);
+	rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG+1, (bytetmp & (~BIT(4))));
+	rtl_write_byte(rtlpriv, 0x367, 0x80);
+
+	rtl_write_word(rtlpriv, REG_CR, 0x2ff);
+	rtl_write_byte(rtlpriv, REG_CR+1, 0x06);
+	rtl_write_byte(rtlpriv, REG_CR+2, 0x00);
+
+	if (!rtlhal->mac_func_enable) {
+		if (_rtl88ee_llt_table_init(hw) == false) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "LLT table init fail\n");
+			return false;
+		}
+	}
+
+
+	rtl_write_dword(rtlpriv, REG_HISR, 0xffffffff);
+	rtl_write_dword(rtlpriv, REG_HISRE, 0xffffffff);
+
+	wordtmp = rtl_read_word(rtlpriv, REG_TRXDMA_CTRL);
+	wordtmp &= 0xf;
+	wordtmp |= 0xE771;
+	rtl_write_word(rtlpriv, REG_TRXDMA_CTRL, wordtmp);
+
+	rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
+	rtl_write_word(rtlpriv, REG_RXFLTMAP2, 0xffff);
+	rtl_write_dword(rtlpriv, REG_TCR, rtlpci->transmit_config);
+
+	rtl_write_dword(rtlpriv, REG_BCNQ_DESA,
+			((u64) rtlpci->tx_ring[BEACON_QUEUE].dma) &
+			DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_MGQ_DESA,
+			(u64) rtlpci->tx_ring[MGNT_QUEUE].dma &
+			DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_VOQ_DESA,
+			(u64) rtlpci->tx_ring[VO_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_VIQ_DESA,
+			(u64) rtlpci->tx_ring[VI_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_BEQ_DESA,
+			(u64) rtlpci->tx_ring[BE_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_BKQ_DESA,
+			(u64) rtlpci->tx_ring[BK_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_HQ_DESA,
+			(u64) rtlpci->tx_ring[HIGH_QUEUE].dma &
+			DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_RX_DESA,
+			(u64) rtlpci->rx_ring[RX_MPDU_QUEUE].dma &
+			DMA_BIT_MASK(32));
+
+	/* if we want to support 64 bit DMA, we should set it here,
+	 * but at the moment we do not support 64 bit DMA
+	 */
+
+	rtl_write_dword(rtlpriv, REG_INT_MIG, 0);
+
+	rtl_write_dword(rtlpriv, REG_MCUTST_1, 0x0);
+	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG+1, 0);/*Enable RX DMA */
+
+	if (rtlhal->earlymode_enable) {/*Early mode enable*/
+		bytetmp = rtl_read_byte(rtlpriv, REG_EARLY_MODE_CONTROL);
+		bytetmp |= 0x1f;
+		rtl_write_byte(rtlpriv, REG_EARLY_MODE_CONTROL, bytetmp);
+		rtl_write_byte(rtlpriv, REG_EARLY_MODE_CONTROL+3, 0x81);
+	}
+	_rtl88ee_gen_refresh_led_state(hw);
+	return true;
+}
+
+static void _rtl88ee_hw_configure(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 reg_prsr;
+
+	reg_prsr = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+
+	rtl_write_dword(rtlpriv, REG_RRSR, reg_prsr);
+	rtl_write_byte(rtlpriv, REG_HWSEQ_CTRL, 0xFF);
+}
+
+static void _rtl88ee_enable_aspm_back_door(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u8 tmp1byte = 0;
+	u32 tmp4Byte = 0, count;
+
+	rtl_write_word(rtlpriv, 0x354, 0x8104);
+	rtl_write_word(rtlpriv, 0x358, 0x24);
+
+	rtl_write_word(rtlpriv, 0x350, 0x70c);
+	rtl_write_byte(rtlpriv, 0x352, 0x2);
+	tmp1byte = rtl_read_byte(rtlpriv, 0x352);
+	count = 0;
+	while (tmp1byte && count < 20) {
+		udelay(10);
+		tmp1byte = rtl_read_byte(rtlpriv, 0x352);
+		count++;
+	}
+	if (0 == tmp1byte) {
+		tmp4Byte = rtl_read_dword(rtlpriv, 0x34c);
+		rtl_write_dword(rtlpriv, 0x348, tmp4Byte|BIT(31));
+		rtl_write_word(rtlpriv, 0x350, 0xf70c);
+		rtl_write_byte(rtlpriv, 0x352, 0x1);
+	}
+
+	tmp1byte = rtl_read_byte(rtlpriv, 0x352);
+	count = 0;
+	while (tmp1byte && count < 20) {
+		udelay(10);
+		tmp1byte = rtl_read_byte(rtlpriv, 0x352);
+		count++;
+	}
+
+	rtl_write_word(rtlpriv, 0x350, 0x718);
+	rtl_write_byte(rtlpriv, 0x352, 0x2);
+	tmp1byte = rtl_read_byte(rtlpriv, 0x352);
+	count = 0;
+	while (tmp1byte && count < 20) {
+		udelay(10);
+		tmp1byte = rtl_read_byte(rtlpriv, 0x352);
+		count++;
+	}
+	if (ppsc->support_backdoor || (0 == tmp1byte)) {
+		tmp4Byte = rtl_read_dword(rtlpriv, 0x34c);
+		rtl_write_dword(rtlpriv, 0x348, tmp4Byte|BIT(11)|BIT(12));
+		rtl_write_word(rtlpriv, 0x350, 0xf718);
+		rtl_write_byte(rtlpriv, 0x352, 0x1);
+	}
+	tmp1byte = rtl_read_byte(rtlpriv, 0x352);
+	count = 0;
+	while (tmp1byte && count < 20) {
+		udelay(10);
+		tmp1byte = rtl_read_byte(rtlpriv, 0x352);
+		count++;
+	}
+}
+
+void rtl88ee_enable_hw_security_config(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 sec_reg_value;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 "PairwiseEncAlgorithm = %d GroupEncAlgorithm = %d\n",
+		 rtlpriv->sec.pairwise_enc_algorithm,
+		 rtlpriv->sec.group_enc_algorithm);
+
+	if (rtlpriv->cfg->mod_params->sw_crypto || rtlpriv->sec.use_sw_sec) {
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+			 "not open hw encryption\n");
+		return;
+	}
+	sec_reg_value = SCR_TXENCENABLE | SCR_RXDECENABLE;
+
+	if (rtlpriv->sec.use_defaultkey) {
+		sec_reg_value |= SCR_TXUSEDK;
+		sec_reg_value |= SCR_RXUSEDK;
+	}
+
+	sec_reg_value |= (SCR_RXBCUSEDK | SCR_TXBCUSEDK);
+
+	rtl_write_byte(rtlpriv, REG_CR + 1, 0x02);
+
+	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+		 "The SECR-value %x\n", sec_reg_value);
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_WPA_CONFIG, &sec_reg_value);
+}
+
+int rtl88ee_hw_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	bool rtstatus = true;
+	int err = 0;
+	u8 tmp_u1b, u1byte;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Rtl8188EE hw init\n");
+	rtlpriv->rtlhal.being_init_adapter = true;
+	rtlpriv->intf_ops->disable_aspm(hw);
+
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_SYS_CLKR+1);
+	u1byte = rtl_read_byte(rtlpriv, REG_CR);
+	if ((tmp_u1b & BIT(3)) && (u1byte != 0 && u1byte != 0xEA)) {
+		rtlhal->mac_func_enable = true;
+	} else {
+		rtlhal->mac_func_enable = false;
+		rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON_88E;
+	}
+
+	rtstatus = _rtl88ee_init_mac(hw);
+	if (rtstatus != true) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Init MAC failed\n");
+		err = 1;
+		return err;
+	}
+
+	err = rtl88e_download_fw(hw, false);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Failed to download FW. Init HW without FW now..\n");
+		err = 1;
+		rtlhal->fw_ready = false;
+		return err;
+	} else {
+		rtlhal->fw_ready = true;
+	}
+	/*fw related variable initialize */
+	rtlhal->last_hmeboxnum = 0;
+	rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON_88E;
+	rtlhal->fw_clk_change_in_progress = false;
+	rtlhal->allow_sw_to_change_hwclc = false;
+	ppsc->fw_current_inpsmode = false;
+
+	rtl88e_phy_mac_config(hw);
+	/* because last function modifies RCR, we update
+	 * rcr var here, or TP will be unstable for receive_config
+	 * is wrong, RX RCR_ACRC32 will cause TP unstable & Rx
+	 * RCR_APP_ICV will cause mac80211 disassoc for cisco 1252
+	 */
+	rtlpci->receive_config &= ~(RCR_ACRC32 | RCR_AICV);
+	rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
+
+	rtl88e_phy_bb_config(hw);
+	rtl_set_bbreg(hw, RFPGA0_RFMOD, BCCKEN, 0x1);
+	rtl_set_bbreg(hw, RFPGA0_RFMOD, BOFDMEN, 0x1);
+
+	rtlphy->rf_mode = RF_OP_BY_SW_3WIRE;
+	rtl88e_phy_rf_config(hw);
+
+	rtlphy->rfreg_chnlval[0] = rtl_get_rfreg(hw, (enum radio_path)0,
+						 RF_CHNLBW, RFREG_OFFSET_MASK);
+	rtlphy->rfreg_chnlval[0] = rtlphy->rfreg_chnlval[0] & 0xfff00fff;
+
+	_rtl88ee_hw_configure(hw);
+	rtl_cam_reset_all_entry(hw);
+	rtl88ee_enable_hw_security_config(hw);
+
+	rtlhal->mac_func_enable = true;
+	ppsc->rfpwr_state = ERFON;
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ETHER_ADDR, mac->mac_addr);
+	_rtl88ee_enable_aspm_back_door(hw);
+	rtlpriv->intf_ops->enable_aspm(hw);
+
+	if (ppsc->rfpwr_state == ERFON) {
+		if ((rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV) ||
+		    ((rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) &&
+		    (rtlhal->oem_id == RT_CID_819x_HP))) {
+			rtl88e_phy_set_rfpath_switch(hw, true);
+			rtlpriv->dm.fat_table.rx_idle_ant = MAIN_ANT;
+		} else {
+			rtl88e_phy_set_rfpath_switch(hw, false);
+			rtlpriv->dm.fat_table.rx_idle_ant = AUX_ANT;
+		}
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "rx idle ant %s\n",
+			 (rtlpriv->dm.fat_table.rx_idle_ant == MAIN_ANT) ?
+			 ("MAIN_ANT") : ("AUX_ANT"));
+
+		if (rtlphy->iqk_initialized) {
+			rtl88e_phy_iq_calibrate(hw, true);
+		} else {
+			rtl88e_phy_iq_calibrate(hw, false);
+			rtlphy->iqk_initialized = true;
+		}
+		rtl88e_dm_check_txpower_tracking(hw);
+		rtl88e_phy_lc_calibrate(hw);
+	}
+
+	tmp_u1b = efuse_read_1byte(hw, 0x1FA);
+	if (!(tmp_u1b & BIT(0))) {
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x15, 0x0F, 0x05);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "PA BIAS path A\n");
+	}
+
+	if (!(tmp_u1b & BIT(4))) {
+		tmp_u1b = rtl_read_byte(rtlpriv, 0x16);
+		tmp_u1b &= 0x0F;
+		rtl_write_byte(rtlpriv, 0x16, tmp_u1b | 0x80);
+		udelay(10);
+		rtl_write_byte(rtlpriv, 0x16, tmp_u1b | 0x90);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "under 1.5V\n");
+	}
+	rtl_write_byte(rtlpriv, REG_NAV_CTRL+2,  ((30000+127)/128));
+	rtl88e_dm_init(hw);
+	rtlpriv->rtlhal.being_init_adapter = false;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "end of Rtl8188EE hw init %x\n",
+		 err);
+	return 0;
+}
+
+static enum version_8188e _rtl88ee_read_chip_version(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	enum version_8188e version = VERSION_UNKNOWN;
+	u32 value32;
+
+	value32 = rtl_read_dword(rtlpriv, REG_SYS_CFG);
+	if (value32 & TRP_VAUX_EN) {
+		version = (enum version_8188e) VERSION_TEST_CHIP_88E;
+	} else {
+		version = NORMAL_CHIP;
+		version = version | ((value32 & TYPE_ID) ? RF_TYPE_2T2R : 0);
+		version = version | ((value32 & VENDOR_ID) ?
+			  CHIP_VENDOR_UMC : 0);
+	}
+
+	rtlphy->rf_type = RF_1T1R;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Chip RF Type: %s\n", (rtlphy->rf_type == RF_2T2R) ?
+		 "RF_2T2R" : "RF_1T1R");
+
+	return version;
+}
+
+static int _rtl88ee_set_media_status(struct ieee80211_hw *hw,
+				     enum nl80211_iftype type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 bt_msr = rtl_read_byte(rtlpriv, MSR);
+	enum led_ctl_mode ledaction = LED_CTL_NO_LINK;
+	bt_msr &= 0xfc;
+
+	if (type == NL80211_IFTYPE_UNSPECIFIED ||
+	    type == NL80211_IFTYPE_STATION) {
+		_rtl88ee_stop_tx_beacon(hw);
+		_rtl88ee_enable_bcn_sub_func(hw);
+	} else if (type == NL80211_IFTYPE_ADHOC ||
+		type == NL80211_IFTYPE_AP ||
+		type == NL80211_IFTYPE_MESH_POINT) {
+		_rtl88ee_resume_tx_beacon(hw);
+		_rtl88ee_disable_bcn_sub_func(hw);
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Set HW_VAR_MEDIA_STATUS: No such media status(%x).\n",
+			 type);
+	}
+
+	switch (type) {
+	case NL80211_IFTYPE_UNSPECIFIED:
+		bt_msr |= MSR_NOLINK;
+		ledaction = LED_CTL_LINK;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to NO LINK!\n");
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		bt_msr |= MSR_ADHOC;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to Ad Hoc!\n");
+		break;
+	case NL80211_IFTYPE_STATION:
+		bt_msr |= MSR_INFRA;
+		ledaction = LED_CTL_LINK;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to STA!\n");
+		break;
+	case NL80211_IFTYPE_AP:
+		bt_msr |= MSR_AP;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to AP!\n");
+		break;
+	case NL80211_IFTYPE_MESH_POINT:
+		bt_msr |= MSR_ADHOC;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to Mesh Point!\n");
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Network type %d not support!\n", type);
+		return 1;
+	}
+
+	rtl_write_byte(rtlpriv, (MSR), bt_msr);
+	rtlpriv->cfg->ops->led_control(hw, ledaction);
+	if ((bt_msr & 0xfc) == MSR_AP)
+		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
+	else
+		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x66);
+	return 0;
+}
+
+void rtl88ee_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u32 reg_rcr = rtlpci->receive_config;
+
+	if (rtlpriv->psc.rfpwr_state != ERFON)
+		return;
+
+	if (check_bssid == true) {
+		reg_rcr |= (RCR_CBSSID_DATA | RCR_CBSSID_BCN);
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
+					      (u8 *)(&reg_rcr));
+		_rtl88ee_set_bcn_ctrl_reg(hw, 0, BIT(4));
+	} else if (check_bssid == false) {
+		reg_rcr &= (~(RCR_CBSSID_DATA | RCR_CBSSID_BCN));
+		_rtl88ee_set_bcn_ctrl_reg(hw, BIT(4), 0);
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+			HW_VAR_RCR, (u8 *)(&reg_rcr));
+	}
+}
+
+int rtl88ee_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (_rtl88ee_set_media_status(hw, type))
+		return -EOPNOTSUPP;
+
+	if (rtlpriv->mac80211.link_state == MAC80211_LINKED) {
+		if (type != NL80211_IFTYPE_AP &&
+		    type != NL80211_IFTYPE_MESH_POINT)
+			rtl88ee_set_check_bssid(hw, true);
+	} else {
+		rtl88ee_set_check_bssid(hw, false);
+	}
+
+	return 0;
+}
+
+/* don't set REG_EDCA_BE_PARAM here because mac80211 will send pkt when scan */
+void rtl88ee_set_qos(struct ieee80211_hw *hw, int aci)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	rtl88e_dm_init_edca_turbo(hw);
+	switch (aci) {
+	case AC1_BK:
+		rtl_write_dword(rtlpriv, REG_EDCA_BK_PARAM, 0xa44f);
+		break;
+	case AC0_BE:
+		break;
+	case AC2_VI:
+		rtl_write_dword(rtlpriv, REG_EDCA_VI_PARAM, 0x5e4322);
+		break;
+	case AC3_VO:
+		rtl_write_dword(rtlpriv, REG_EDCA_VO_PARAM, 0x2f3222);
+		break;
+	default:
+		RT_ASSERT(false, "invalid aci: %d !\n", aci);
+		break;
+	}
+}
+
+void rtl88ee_enable_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	rtl_write_dword(rtlpriv, REG_HIMR, rtlpci->irq_mask[0] & 0xFFFFFFFF);
+	rtl_write_dword(rtlpriv, REG_HIMRE, rtlpci->irq_mask[1] & 0xFFFFFFFF);
+	rtlpci->irq_enabled = true;
+	/* there are some C2H CMDs have been sent before system interrupt
+	 * is enabled, e.g., C2H, CPWM.
+	 * So we need to clear all C2H events that FW has notified, otherwise
+	 * FW won't schedule any commands anymore.
+	 */
+	rtl_write_byte(rtlpriv, REG_C2HEVT_CLEAR, 0);
+	/*enable system interrupt*/
+	rtl_write_dword(rtlpriv, REG_HSIMR, rtlpci->sys_irq_mask & 0xFFFFFFFF);
+}
+
+void rtl88ee_disable_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	rtl_write_dword(rtlpriv, REG_HIMR, IMR_DISABLED);
+	rtl_write_dword(rtlpriv, REG_HIMRE, IMR_DISABLED);
+	rtlpci->irq_enabled = false;
+	synchronize_irq(rtlpci->pdev->irq);
+}
+
+static void _rtl88ee_poweroff_adapter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 u1b_tmp;
+	u32 count = 0;
+	rtlhal->mac_func_enable = false;
+	rtlpriv->intf_ops->enable_aspm(hw);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "POWER OFF adapter\n");
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_TX_RPT_CTRL);
+	rtl_write_byte(rtlpriv, REG_TX_RPT_CTRL, u1b_tmp & (~BIT(1)));
+
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+	while (!(u1b_tmp & BIT(1)) && (count++ < 100)) {
+		udelay(10);
+		u1b_tmp = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+		count++;
+	}
+	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG+1, 0xFF);
+
+	rtl88_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+				   PWR_INTF_PCI_MSK,
+				   Rtl8188E_NIC_LPS_ENTER_FLOW);
+
+	rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x00);
+
+	if ((rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) && rtlhal->fw_ready)
+		rtl88e_firmware_selfreset(hw);
+
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, (u1b_tmp & (~BIT(2))));
+	rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x00);
+
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_32K_CTRL);
+	rtl_write_byte(rtlpriv, REG_32K_CTRL, (u1b_tmp & (~BIT(0))));
+
+	rtl88_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+				   PWR_INTF_PCI_MSK, Rtl8188E_NIC_DISABLE_FLOW);
+
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp & (~BIT(3))));
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp | BIT(3)));
+
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x0E);
+
+	u1b_tmp = rtl_read_byte(rtlpriv, GPIO_IN);
+	rtl_write_byte(rtlpriv, GPIO_OUT, u1b_tmp);
+	rtl_write_byte(rtlpriv, GPIO_IO_SEL, 0x7F);
+
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_GPIO_IO_SEL);
+	rtl_write_byte(rtlpriv, REG_GPIO_IO_SEL, (u1b_tmp << 4) | u1b_tmp);
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_GPIO_IO_SEL+1);
+	rtl_write_byte(rtlpriv, REG_GPIO_IO_SEL+1, u1b_tmp | 0x0F);
+
+	rtl_write_dword(rtlpriv, REG_GPIO_IO_SEL_2+2, 0x00080808);
+}
+
+void rtl88ee_card_disable(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	enum nl80211_iftype opmode;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "RTL8188ee card disable\n");
+
+	mac->link_state = MAC80211_NOLINK;
+	opmode = NL80211_IFTYPE_UNSPECIFIED;
+
+	_rtl88ee_set_media_status(hw, opmode);
+
+	if (rtlpriv->rtlhal.driver_is_goingto_unload ||
+	    ppsc->rfoff_reason > RF_CHANGE_BY_PS)
+		rtlpriv->cfg->ops->led_control(hw, LED_CTL_POWER_OFF);
+
+	RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+	_rtl88ee_poweroff_adapter(hw);
+
+	/* after power off we should do iqk again */
+	rtlpriv->phy.iqk_initialized = false;
+}
+
+void rtl88ee_interrupt_recognized(struct ieee80211_hw *hw,
+				  u32 *p_inta, u32 *p_intb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	*p_inta = rtl_read_dword(rtlpriv, ISR) & rtlpci->irq_mask[0];
+	rtl_write_dword(rtlpriv, ISR, *p_inta);
+
+	*p_intb = rtl_read_dword(rtlpriv, REG_HISRE) & rtlpci->irq_mask[1];
+	rtl_write_dword(rtlpriv, REG_HISRE, *p_intb);
+}
+
+void rtl88ee_set_beacon_related_registers(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u16 bcn_interval, atim_window;
+
+	bcn_interval = mac->beacon_interval;
+	atim_window = 2;	/*FIX MERGE */
+	rtl88ee_disable_interrupt(hw);
+	rtl_write_word(rtlpriv, REG_ATIMWND, atim_window);
+	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
+	rtl_write_word(rtlpriv, REG_BCNTCFG, 0x660f);
+	rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_CCK, 0x18);
+	rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_OFDM, 0x18);
+	rtl_write_byte(rtlpriv, 0x606, 0x30);
+	rtlpci->reg_bcn_ctrl_val |= BIT(3);
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8) rtlpci->reg_bcn_ctrl_val);
+	/*rtl88ee_enable_interrupt(hw);*/
+}
+
+void rtl88ee_set_beacon_interval(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u16 bcn_interval = mac->beacon_interval;
+
+	RT_TRACE(rtlpriv, COMP_BEACON, DBG_DMESG,
+		 "beacon_interval:%d\n", bcn_interval);
+	/*rtl88ee_disable_interrupt(hw);*/
+	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
+	/*rtl88ee_enable_interrupt(hw);*/
+}
+
+void rtl88ee_update_interrupt_mask(struct ieee80211_hw *hw,
+				   u32 add_msr, u32 rm_msr)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	RT_TRACE(rtlpriv, COMP_INTR, DBG_LOUD,
+		 "add_msr:%x, rm_msr:%x\n", add_msr, rm_msr);
+
+	rtl88ee_disable_interrupt(hw);
+	if (add_msr)
+		rtlpci->irq_mask[0] |= add_msr;
+	if (rm_msr)
+		rtlpci->irq_mask[0] &= (~rm_msr);
+	rtl88ee_enable_interrupt(hw);
+}
+
+static inline u8 get_chnl_group(u8 chnl)
+{
+	u8 group;
+
+	group = chnl / 3;
+	if (chnl == 14)
+		group = 5;
+
+	return group;
+}
+
+static void set_diff0_2g(struct txpower_info_2g *pwr2g, u8 *hwinfo, u32 path,
+			 u32 i, u32 eadr)
+{
+	pwr2g->bw40_diff[path][i] = 0;
+	if (hwinfo[eadr] == 0xFF) {
+		pwr2g->bw20_diff[path][i] = 0x02;
+	} else {
+		pwr2g->bw20_diff[path][i] = (hwinfo[eadr]&0xf0)>>4;
+		/*bit sign number to 8 bit sign number*/
+		if (pwr2g->bw20_diff[path][i] & BIT(3))
+			pwr2g->bw20_diff[path][i] |= 0xF0;
+	}
+
+	if (hwinfo[eadr] == 0xFF) {
+		pwr2g->ofdm_diff[path][i] = 0x04;
+	} else {
+		pwr2g->ofdm_diff[path][i] = (hwinfo[eadr] & 0x0f);
+		/*bit sign number to 8 bit sign number*/
+		if (pwr2g->ofdm_diff[path][i] & BIT(3))
+			pwr2g->ofdm_diff[path][i] |= 0xF0;
+	}
+	pwr2g->cck_diff[path][i] = 0;
+}
+
+static void set_diff0_5g(struct txpower_info_5g *pwr5g, u8 *hwinfo, u32 path,
+			 u32 i, u32 eadr)
+{
+	pwr5g->bw40_diff[path][i] = 0;
+	if (hwinfo[eadr] == 0xFF) {
+		pwr5g->bw20_diff[path][i] = 0;
+	} else {
+		pwr5g->bw20_diff[path][i] = (hwinfo[eadr]&0xf0)>>4;
+		/*bit sign number to 8 bit sign number*/
+		if (pwr5g->bw20_diff[path][i] & BIT(3))
+			pwr5g->bw20_diff[path][i] |= 0xF0;
+	}
+
+	if (hwinfo[eadr] == 0xFF) {
+		pwr5g->ofdm_diff[path][i] = 0x04;
+	} else {
+		pwr5g->ofdm_diff[path][i] = (hwinfo[eadr] & 0x0f);
+		/*bit sign number to 8 bit sign number*/
+		if (pwr5g->ofdm_diff[path][i] & BIT(3))
+			pwr5g->ofdm_diff[path][i] |= 0xF0;
+	}
+}
+
+static void set_diff1_2g(struct txpower_info_2g *pwr2g, u8 *hwinfo, u32 path,
+			 u32 i, u32 eadr)
+{
+	if (hwinfo[eadr] == 0xFF) {
+		pwr2g->bw40_diff[path][i] = 0xFE;
+	} else {
+		pwr2g->bw40_diff[path][i] = (hwinfo[eadr]&0xf0)>>4;
+		if (pwr2g->bw40_diff[path][i] & BIT(3))
+			pwr2g->bw40_diff[path][i] |= 0xF0;
+	}
+
+	if (hwinfo[eadr] == 0xFF) {
+		pwr2g->bw20_diff[path][i] = 0xFE;
+	} else {
+		pwr2g->bw20_diff[path][i] = (hwinfo[eadr]&0x0f);
+		if (pwr2g->bw20_diff[path][i] & BIT(3))
+			pwr2g->bw20_diff[path][i] |= 0xF0;
+	}
+}
+
+static void set_diff1_5g(struct txpower_info_5g *pwr5g, u8 *hwinfo, u32 path,
+			 u32 i, u32 eadr)
+{
+	if (hwinfo[eadr] == 0xFF) {
+		pwr5g->bw40_diff[path][i] = 0xFE;
+	} else {
+		pwr5g->bw40_diff[path][i] = (hwinfo[eadr]&0xf0)>>4;
+		if (pwr5g->bw40_diff[path][i] & BIT(3))
+			pwr5g->bw40_diff[path][i] |= 0xF0;
+	}
+
+	if (hwinfo[eadr] == 0xFF) {
+		pwr5g->bw20_diff[path][i] = 0xFE;
+	} else {
+		pwr5g->bw20_diff[path][i] = (hwinfo[eadr] & 0x0f);
+		if (pwr5g->bw20_diff[path][i] & BIT(3))
+			pwr5g->bw20_diff[path][i] |= 0xF0;
+	}
+}
+
+static void set_diff2_2g(struct txpower_info_2g *pwr2g, u8 *hwinfo, u32 path,
+			 u32 i, u32 eadr)
+{
+	if (hwinfo[eadr] == 0xFF) {
+		pwr2g->ofdm_diff[path][i] = 0xFE;
+	} else {
+		pwr2g->ofdm_diff[path][i] = (hwinfo[eadr]&0xf0)>>4;
+		if (pwr2g->ofdm_diff[path][i] & BIT(3))
+			pwr2g->ofdm_diff[path][i] |= 0xF0;
+	}
+
+	if (hwinfo[eadr] == 0xFF) {
+		pwr2g->cck_diff[path][i] = 0xFE;
+	} else {
+		pwr2g->cck_diff[path][i] = (hwinfo[eadr]&0x0f);
+		if (pwr2g->cck_diff[path][i] & BIT(3))
+			pwr2g->cck_diff[path][i] |= 0xF0;
+	}
+}
+
+static void _rtl8188e_read_power_value_fromprom(struct ieee80211_hw *hw,
+						struct txpower_info_2g *pwr2g,
+						struct txpower_info_5g *pwr5g,
+						bool autoload_fail,
+						u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 path, eadr = EEPROM_TX_PWR_INX, i;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "hal_ReadPowerValueFromPROM88E(): PROMContent[0x%x]= 0x%x\n",
+		 (eadr+1), hwinfo[eadr+1]);
+	if (0xFF == hwinfo[eadr+1])
+		autoload_fail = true;
+
+	if (autoload_fail) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "auto load fail : Use Default value!\n");
+		for (path = 0; path < MAX_RF_PATH; path++) {
+			/* 2.4G default value */
+			for (i = 0; i < MAX_CHNL_GROUP_24G; i++) {
+				pwr2g->index_cck_base[path][i] = 0x2D;
+				pwr2g->index_bw40_base[path][i] = 0x2D;
+			}
+			for (i = 0; i < MAX_TX_COUNT; i++) {
+				if (i == 0) {
+					pwr2g->bw20_diff[path][0] = 0x02;
+					pwr2g->ofdm_diff[path][0] = 0x04;
+				} else {
+					pwr2g->bw20_diff[path][i] = 0xFE;
+					pwr2g->bw40_diff[path][i] = 0xFE;
+					pwr2g->cck_diff[path][i] = 0xFE;
+					pwr2g->ofdm_diff[path][i] = 0xFE;
+				}
+			}
+		}
+		return;
+	}
+
+	for (path = 0; path < MAX_RF_PATH; path++) {
+		/*2.4G default value*/
+		for (i = 0; i < MAX_CHNL_GROUP_24G; i++) {
+			pwr2g->index_cck_base[path][i] = hwinfo[eadr++];
+			if (pwr2g->index_cck_base[path][i] == 0xFF)
+				pwr2g->index_cck_base[path][i] = 0x2D;
+		}
+		for (i = 0; i < MAX_CHNL_GROUP_24G; i++) {
+			pwr2g->index_bw40_base[path][i] = hwinfo[eadr++];
+			if (pwr2g->index_bw40_base[path][i] == 0xFF)
+				pwr2g->index_bw40_base[path][i] = 0x2D;
+		}
+		for (i = 0; i < MAX_TX_COUNT; i++) {
+			if (i == 0) {
+				set_diff0_2g(pwr2g, hwinfo, path, i, eadr);
+				eadr++;
+			} else {
+				set_diff1_2g(pwr2g, hwinfo, path, i, eadr);
+				eadr++;
+
+				set_diff2_2g(pwr2g, hwinfo, path, i, eadr);
+				eadr++;
+			}
+		}
+
+		/*5G default value*/
+		for (i = 0; i < MAX_CHNL_GROUP_5G; i++) {
+			pwr5g->index_bw40_base[path][i] = hwinfo[eadr++];
+			if (pwr5g->index_bw40_base[path][i] == 0xFF)
+				pwr5g->index_bw40_base[path][i] = 0xFE;
+		}
+
+		for (i = 0; i < MAX_TX_COUNT; i++) {
+			if (i == 0) {
+				set_diff0_5g(pwr5g, hwinfo, path, i, eadr);
+				eadr++;
+			} else {
+				set_diff1_5g(pwr5g, hwinfo, path, i, eadr);
+				eadr++;
+			}
+		}
+
+		if (hwinfo[eadr] == 0xFF) {
+			pwr5g->ofdm_diff[path][1] = 0xFE;
+			pwr5g->ofdm_diff[path][2] = 0xFE;
+		} else {
+			pwr5g->ofdm_diff[path][1] = (hwinfo[eadr] & 0xf0) >> 4;
+			pwr5g->ofdm_diff[path][2] = (hwinfo[eadr] & 0x0f);
+		}
+		eadr++;
+
+		if (hwinfo[eadr] == 0xFF)
+			pwr5g->ofdm_diff[path][3] = 0xFE;
+		else
+			pwr5g->ofdm_diff[path][3] = (hwinfo[eadr]&0x0f);
+		eadr++;
+
+		for (i = 1; i < MAX_TX_COUNT; i++) {
+			if (pwr5g->ofdm_diff[path][i] == 0xFF)
+				pwr5g->ofdm_diff[path][i] = 0xFE;
+			else if (pwr5g->ofdm_diff[path][i] & BIT(3))
+				pwr5g->ofdm_diff[path][i] |= 0xF0;
+		}
+	}
+}
+
+static void _rtl88ee_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
+						 bool autoload_fail,
+						 u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct txpower_info_2g pwrinfo24g;
+	struct txpower_info_5g pwrinfo5g;
+	u8 rf_path, index;
+	u8 i;
+	int jj = EEPROM_RF_BOARD_OPTION_88E;
+	int kk = EEPROM_THERMAL_METER_88E;
+
+	_rtl8188e_read_power_value_fromprom(hw, &pwrinfo24g, &pwrinfo5g,
+					    autoload_fail, hwinfo);
+
+	for (rf_path = 0; rf_path < 2; rf_path++) {
+		for (i = 0; i < 14; i++) {
+			index = get_chnl_group(i+1);
+
+			rtlefuse->txpwrlevel_cck[rf_path][i] =
+				 pwrinfo24g.index_cck_base[rf_path][index];
+			if (i == 13)
+				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
+				     pwrinfo24g.index_bw40_base[rf_path][4];
+			else
+				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
+				     pwrinfo24g.index_bw40_base[rf_path][index];
+			rtlefuse->txpwr_ht20diff[rf_path][i] =
+				 pwrinfo24g.bw20_diff[rf_path][0];
+			rtlefuse->txpwr_legacyhtdiff[rf_path][i] =
+				 pwrinfo24g.ofdm_diff[rf_path][0];
+		}
+
+		for (i = 0; i < 14; i++) {
+			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
+				"RF(%d)-Ch(%d) [CCK / HT40_1S ] = "
+				"[0x%x / 0x%x ]\n", rf_path, i,
+				rtlefuse->txpwrlevel_cck[rf_path][i],
+				rtlefuse->txpwrlevel_ht40_1s[rf_path][i]);
+		}
+	}
+
+	if (!autoload_fail)
+		rtlefuse->eeprom_thermalmeter = hwinfo[kk];
+	else
+		rtlefuse->eeprom_thermalmeter = EEPROM_DEFAULT_THERMALMETER;
+
+	if (rtlefuse->eeprom_thermalmeter == 0xff || autoload_fail) {
+		rtlefuse->apk_thermalmeterignore = true;
+		rtlefuse->eeprom_thermalmeter = EEPROM_DEFAULT_THERMALMETER;
+	}
+
+	rtlefuse->thermalmeter[0] = rtlefuse->eeprom_thermalmeter;
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
+		"thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter);
+
+	if (!autoload_fail) {
+		rtlefuse->eeprom_regulatory = hwinfo[jj] & 0x07;/*bit0~2*/
+		if (hwinfo[jj] == 0xFF)
+			rtlefuse->eeprom_regulatory = 0;
+	} else {
+		rtlefuse->eeprom_regulatory = 0;
+	}
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
+		"eeprom_regulatory = 0x%x\n", rtlefuse->eeprom_regulatory);
+}
+
+static void _rtl88ee_read_adapter_info(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_pci_priv *rppriv = rtl_pcipriv(hw);
+	u16 i, usvalue;
+	u8 hwinfo[HWSET_MAX_SIZE];
+	u16 eeprom_id;
+	int jj = EEPROM_RF_BOARD_OPTION_88E;
+	int kk = EEPROM_RF_FEATURE_OPTION_88E;
+
+	if (rtlefuse->epromtype == EEPROM_BOOT_EFUSE) {
+		rtl_efuse_shadow_map_update(hw);
+
+		memcpy(hwinfo, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
+		       HWSET_MAX_SIZE);
+	} else if (rtlefuse->epromtype == EEPROM_93C46) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "RTL819X Not boot from eeprom, check it !!");
+	}
+
+	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, ("MAP\n"),
+		      hwinfo, HWSET_MAX_SIZE);
+
+	eeprom_id = *((u16 *)&hwinfo[0]);
+	if (eeprom_id != RTL8188E_EEPROM_ID) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "EEPROM ID(%#x) is invalid!!\n", eeprom_id);
+		rtlefuse->autoload_failflag = true;
+	} else {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Autoload OK\n");
+		rtlefuse->autoload_failflag = false;
+	}
+
+	if (rtlefuse->autoload_failflag == true)
+		return;
+	/*VID DID SVID SDID*/
+	rtlefuse->eeprom_vid = *(u16 *)&hwinfo[EEPROM_VID];
+	rtlefuse->eeprom_did = *(u16 *)&hwinfo[EEPROM_DID];
+	rtlefuse->eeprom_svid = *(u16 *)&hwinfo[EEPROM_SVID];
+	rtlefuse->eeprom_smid = *(u16 *)&hwinfo[EEPROM_SMID];
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROMId = 0x%4x\n", eeprom_id);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM VID = 0x%4x\n", rtlefuse->eeprom_vid);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM DID = 0x%4x\n", rtlefuse->eeprom_did);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM SVID = 0x%4x\n", rtlefuse->eeprom_svid);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM SMID = 0x%4x\n", rtlefuse->eeprom_smid);
+	/*customer ID*/
+	rtlefuse->eeprom_oemid = *(u8 *)&hwinfo[EEPROM_CUSTOMER_ID];
+	if (rtlefuse->eeprom_oemid == 0xFF)
+		rtlefuse->eeprom_oemid = 0;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM Customer ID: 0x%2x\n", rtlefuse->eeprom_oemid);
+	/*EEPROM version*/
+	rtlefuse->eeprom_version = *(u16 *)&hwinfo[EEPROM_VERSION];
+	/*mac address*/
+	for (i = 0; i < 6; i += 2) {
+		usvalue = *(u16 *)&hwinfo[EEPROM_MAC_ADDR + i];
+		*((u16 *)(&rtlefuse->dev_addr[i])) = usvalue;
+	}
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 "dev_addr: %pM\n", rtlefuse->dev_addr);
+	/*channel plan */
+	rtlefuse->eeprom_channelplan = *(u8 *)&hwinfo[EEPROM_CHANNELPLAN];
+	/* set channel paln to world wide 13 */
+	rtlefuse->channel_plan = COUNTRY_CODE_WORLD_WIDE_13;
+	/*tx power*/
+	_rtl88ee_read_txpower_info_from_hwpg(hw, rtlefuse->autoload_failflag,
+					     hwinfo);
+	rtlefuse->txpwr_fromeprom = true;
+
+	rtl8188ee_read_bt_coexist_info_from_hwpg(hw,
+						 rtlefuse->autoload_failflag,
+						 hwinfo);
+	/*board type*/
+	rtlefuse->board_type = (((*(u8 *)&hwinfo[jj]) & 0xE0) >> 5);
+	/*Wake on wlan*/
+	rtlefuse->wowlan_enable = ((hwinfo[kk] & 0x40) >> 6);
+	/*parse xtal*/
+	rtlefuse->crystalcap = hwinfo[EEPROM_XTAL_88E];
+	if (hwinfo[EEPROM_XTAL_88E])
+		rtlefuse->crystalcap = 0x20;
+	/*antenna diversity*/
+	rtlefuse->antenna_div_cfg = (hwinfo[jj] & 0x18) >> 3;
+	if (hwinfo[jj] == 0xFF)
+		rtlefuse->antenna_div_cfg = 0;
+	if (rppriv->bt_coexist.eeprom_bt_coexist != 0 &&
+	    rppriv->bt_coexist.eeprom_bt_ant_num == ANT_X1)
+		rtlefuse->antenna_div_cfg = 0;
+
+	rtlefuse->antenna_div_type = hwinfo[EEPROM_RF_ANTENNA_OPT_88E];
+	if (rtlefuse->antenna_div_type == 0xFF)
+		rtlefuse->antenna_div_type = 0x01;
+	if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV ||
+	    rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
+		rtlefuse->antenna_div_cfg = 1;
+
+	if (rtlhal->oem_id == RT_CID_DEFAULT) {
+		switch (rtlefuse->eeprom_oemid) {
+		case EEPROM_CID_DEFAULT:
+			if (rtlefuse->eeprom_did == 0x8179) {
+				if (rtlefuse->eeprom_svid == 0x1025) {
+					rtlhal->oem_id = RT_CID_819x_Acer;
+				} else if ((rtlefuse->eeprom_svid == 0x10EC &&
+					    rtlefuse->eeprom_smid == 0x0179) ||
+					    (rtlefuse->eeprom_svid == 0x17AA &&
+					    rtlefuse->eeprom_smid == 0x0179)) {
+					rtlhal->oem_id = RT_CID_819x_Lenovo;
+				} else if (rtlefuse->eeprom_svid == 0x103c &&
+					 rtlefuse->eeprom_smid == 0x197d) {
+					rtlhal->oem_id = RT_CID_819x_HP;
+				} else {
+					rtlhal->oem_id = RT_CID_DEFAULT;
+				}
+			} else {
+				rtlhal->oem_id = RT_CID_DEFAULT;
+			}
+			break;
+		case EEPROM_CID_TOSHIBA:
+			rtlhal->oem_id = RT_CID_TOSHIBA;
+			break;
+		case EEPROM_CID_QMI:
+			rtlhal->oem_id = RT_CID_819x_QMI;
+			break;
+		case EEPROM_CID_WHQL:
+		default:
+			rtlhal->oem_id = RT_CID_DEFAULT;
+			break;
+		}
+	}
+}
+
+static void _rtl88ee_hal_customized_behavior(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	pcipriv->ledctl.led_opendrain = true;
+
+	switch (rtlhal->oem_id) {
+	case RT_CID_819x_HP:
+		pcipriv->ledctl.led_opendrain = true;
+		break;
+	case RT_CID_819x_Lenovo:
+	case RT_CID_DEFAULT:
+	case RT_CID_TOSHIBA:
+	case RT_CID_CCX:
+	case RT_CID_819x_Acer:
+	case RT_CID_WHQL:
+	default:
+		break;
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 "RT Customized ID: 0x%02X\n", rtlhal->oem_id);
+}
+
+void rtl88ee_read_eeprom_info(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tmp_u1b;
+
+	rtlhal->version = _rtl88ee_read_chip_version(hw);
+	if (get_rf_type(rtlphy) == RF_1T1R) {
+		rtlpriv->dm.rfpath_rxenable[0] = true;
+	} else {
+		rtlpriv->dm.rfpath_rxenable[0] = true;
+		rtlpriv->dm.rfpath_rxenable[1] = true;
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "VersionID = 0x%4x\n",
+		 rtlhal->version);
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_9346CR);
+	if (tmp_u1b & BIT(4)) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "Boot from EEPROM\n");
+		rtlefuse->epromtype = EEPROM_93C46;
+	} else {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "Boot from EFUSE\n");
+		rtlefuse->epromtype = EEPROM_BOOT_EFUSE;
+	}
+	if (tmp_u1b & BIT(5)) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Autoload OK\n");
+		rtlefuse->autoload_failflag = false;
+		_rtl88ee_read_adapter_info(hw);
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Autoload ERR!!\n");
+	}
+	_rtl88ee_hal_customized_behavior(hw);
+}
+
+static void rtl88ee_update_hal_rate_table(struct ieee80211_hw *hw,
+					  struct ieee80211_sta *sta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rppriv = rtl_pcipriv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 ratr_value;
+	u8 ratr_index = 0;
+	u8 nmode = mac->ht_enable;
+	u8 mimo_ps = IEEE80211_SMPS_OFF;
+	u16 shortgi_rate;
+	u32 tmp_ratr_value;
+	u8 ctx40 = mac->bw_40;
+	u16 cap = sta->ht_cap.cap;
+	u8 short40 = (cap & IEEE80211_HT_CAP_SGI_40) ?  1 : 0;
+	u8 short20 = (cap & IEEE80211_HT_CAP_SGI_20) ?  1 : 0;
+	enum wireless_mode wirelessmode = mac->mode;
+
+	if (rtlhal->current_bandtype == BAND_ON_5G)
+		ratr_value = sta->supp_rates[1] << 4;
+	else
+		ratr_value = sta->supp_rates[0];
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		ratr_value = 0xfff;
+	ratr_value |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
+			sta->ht_cap.mcs.rx_mask[0] << 12);
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		if (ratr_value & 0x0000000c)
+			ratr_value &= 0x0000000d;
+		else
+			ratr_value &= 0x0000000f;
+		break;
+	case WIRELESS_MODE_G:
+		ratr_value &= 0x00000FF5;
+		break;
+	case WIRELESS_MODE_N_24G:
+	case WIRELESS_MODE_N_5G:
+		nmode = 1;
+		if (mimo_ps == IEEE80211_SMPS_STATIC) {
+			ratr_value &= 0x0007F005;
+		} else {
+			u32 ratr_mask;
+
+			if (get_rf_type(rtlphy) == RF_1T2R ||
+			    get_rf_type(rtlphy) == RF_1T1R)
+				ratr_mask = 0x000ff005;
+			else
+				ratr_mask = 0x0f0ff005;
+
+			ratr_value &= ratr_mask;
+		}
+		break;
+	default:
+		if (rtlphy->rf_type == RF_1T2R)
+			ratr_value &= 0x000ff0ff;
+		else
+			ratr_value &= 0x0f0ff0ff;
+
+		break;
+	}
+
+	if ((rppriv->bt_coexist.bt_coexistence) &&
+	    (rppriv->bt_coexist.bt_coexist_type == BT_CSR_BC4) &&
+	    (rppriv->bt_coexist.bt_cur_state) &&
+	    (rppriv->bt_coexist.bt_ant_isolation) &&
+	    ((rppriv->bt_coexist.bt_service == BT_SCO) ||
+	    (rppriv->bt_coexist.bt_service == BT_BUSY)))
+		ratr_value &= 0x0fffcfc0;
+	else
+		ratr_value &= 0x0FFFFFFF;
+
+	if (nmode && ((ctx40 && short40) ||
+		      (!ctx40 && short20))) {
+		ratr_value |= 0x10000000;
+		tmp_ratr_value = (ratr_value >> 12);
+
+		for (shortgi_rate = 15; shortgi_rate > 0; shortgi_rate--) {
+			if ((1 << shortgi_rate) & tmp_ratr_value)
+				break;
+		}
+
+		shortgi_rate = (shortgi_rate << 12) | (shortgi_rate << 8) |
+		    (shortgi_rate << 4) | (shortgi_rate);
+	}
+
+	rtl_write_dword(rtlpriv, REG_ARFR0 + ratr_index * 4, ratr_value);
+
+	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
+		 "%x\n", rtl_read_dword(rtlpriv, REG_ARFR0));
+}
+
+static void rtl88ee_update_hal_rate_mask(struct ieee80211_hw *hw,
+					 struct ieee80211_sta *sta, u8 rssi)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_sta_info *sta_entry = NULL;
+	u32 ratr_bitmap;
+	u8 ratr_index;
+	u16 cap = sta->ht_cap.cap;
+	u8 ctx40 = (cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40) ? 1 : 0;
+	u8 short40 = (cap & IEEE80211_HT_CAP_SGI_40) ?  1 : 0;
+	u8 short20 = (cap & IEEE80211_HT_CAP_SGI_20) ?  1 : 0;
+	enum wireless_mode wirelessmode = 0;
+	bool shortgi = false;
+	u8 rate_mask[5];
+	u8 macid = 0;
+	u8 mimo_ps = IEEE80211_SMPS_OFF;
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	wirelessmode = sta_entry->wireless_mode;
+	if (mac->opmode == NL80211_IFTYPE_STATION ||
+	    mac->opmode == NL80211_IFTYPE_MESH_POINT)
+		ctx40 = mac->bw_40;
+	else if (mac->opmode == NL80211_IFTYPE_AP ||
+		 mac->opmode == NL80211_IFTYPE_ADHOC)
+		macid = sta->aid + 1;
+
+	if (rtlhal->current_bandtype == BAND_ON_5G)
+		ratr_bitmap = sta->supp_rates[1] << 4;
+	else
+		ratr_bitmap = sta->supp_rates[0];
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		ratr_bitmap = 0xfff;
+	ratr_bitmap |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
+			sta->ht_cap.mcs.rx_mask[0] << 12);
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		ratr_index = RATR_INX_WIRELESS_B;
+		if (ratr_bitmap & 0x0000000c)
+			ratr_bitmap &= 0x0000000d;
+		else
+			ratr_bitmap &= 0x0000000f;
+		break;
+	case WIRELESS_MODE_G:
+		ratr_index = RATR_INX_WIRELESS_GB;
+
+		if (rssi == 1)
+			ratr_bitmap &= 0x00000f00;
+		else if (rssi == 2)
+			ratr_bitmap &= 0x00000ff0;
+		else
+			ratr_bitmap &= 0x00000ff5;
+		break;
+	case WIRELESS_MODE_A:
+		ratr_index = RATR_INX_WIRELESS_A;
+		ratr_bitmap &= 0x00000ff0;
+		break;
+	case WIRELESS_MODE_N_24G:
+	case WIRELESS_MODE_N_5G:
+		ratr_index = RATR_INX_WIRELESS_NGB;
+
+		if (mimo_ps == IEEE80211_SMPS_STATIC) {
+			if (rssi == 1)
+				ratr_bitmap &= 0x00070000;
+			else if (rssi == 2)
+				ratr_bitmap &= 0x0007f000;
+			else
+				ratr_bitmap &= 0x0007f005;
+		} else {
+			if (rtlphy->rf_type == RF_1T2R ||
+			    rtlphy->rf_type == RF_1T1R) {
+				if (ctx40) {
+					if (rssi == 1)
+						ratr_bitmap &= 0x000f0000;
+					else if (rssi == 2)
+						ratr_bitmap &= 0x000ff000;
+					else
+						ratr_bitmap &= 0x000ff015;
+				} else {
+					if (rssi == 1)
+						ratr_bitmap &= 0x000f0000;
+					else if (rssi == 2)
+						ratr_bitmap &= 0x000ff000;
+					else
+						ratr_bitmap &= 0x000ff005;
+				}
+			} else {
+				if (ctx40) {
+					if (rssi == 1)
+						ratr_bitmap &= 0x0f8f0000;
+					else if (rssi == 2)
+						ratr_bitmap &= 0x0f8ff000;
+					else
+						ratr_bitmap &= 0x0f8ff015;
+				} else {
+					if (rssi == 1)
+						ratr_bitmap &= 0x0f8f0000;
+					else if (rssi == 2)
+						ratr_bitmap &= 0x0f8ff000;
+					else
+						ratr_bitmap &= 0x0f8ff005;
+				}
+			}
+		}
+
+		if ((ctx40 && short40) || (!ctx40 && short20)) {
+			if (macid == 0)
+				shortgi = true;
+			else if (macid == 1)
+				shortgi = false;
+		}
+		break;
+	default:
+		ratr_index = RATR_INX_WIRELESS_NGB;
+
+		if (rtlphy->rf_type == RF_1T2R)
+			ratr_bitmap &= 0x000ff0ff;
+		else
+			ratr_bitmap &= 0x0f0ff0ff;
+		break;
+	}
+	sta_entry->ratr_index = ratr_index;
+
+	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
+		 "ratr_bitmap :%x\n", ratr_bitmap);
+	*(u32 *)&rate_mask = (ratr_bitmap & 0x0fffffff) |
+			     (ratr_index << 28);
+	rate_mask[4] = macid | (shortgi ? 0x20 : 0x00) | 0x80;
+	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
+		 "Rate_index:%x, ratr_val:%x, %x:%x:%x:%x:%x\n",
+		 ratr_index, ratr_bitmap, rate_mask[0], rate_mask[1],
+		 rate_mask[2], rate_mask[3], rate_mask[4]);
+	rtl88e_fill_h2c_cmd(hw, H2C_88E_RA_MASK, 5, rate_mask);
+	_rtl88ee_set_bcn_ctrl_reg(hw, BIT(3), 0);
+}
+
+void rtl88ee_update_hal_rate_tbl(struct ieee80211_hw *hw,
+		struct ieee80211_sta *sta, u8 rssi)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->dm.useramask)
+		rtl88ee_update_hal_rate_mask(hw, sta, rssi);
+	else
+		rtl88ee_update_hal_rate_table(hw, sta);
+}
+
+void rtl88ee_update_channel_access_setting(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u16 sifs_timer;
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
+				      (u8 *)&mac->slot_time);
+	if (!mac->ht_enable)
+		sifs_timer = 0x0a0a;
+	else
+		sifs_timer = 0x0e0e;
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SIFS, (u8 *)&sifs_timer);
+}
+
+bool rtl88ee_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	enum rf_pwrstate state_toset;
+	u32 u4tmp;
+	bool actuallyset = false;
+
+	if (rtlpriv->rtlhal.being_init_adapter)
+		return false;
+
+	if (ppsc->swrf_processing)
+		return false;
+
+	spin_lock(&rtlpriv->locks.rf_ps_lock);
+	if (ppsc->rfchange_inprogress) {
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+		return false;
+	} else {
+		ppsc->rfchange_inprogress = true;
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+	}
+
+	u4tmp = rtl_read_dword(rtlpriv, REG_GPIO_OUTPUT);
+	state_toset = (u4tmp & BIT(31)) ? ERFON : ERFOFF;
+
+
+	if ((ppsc->hwradiooff == true) && (state_toset == ERFON)) {
+		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+			 "GPIOChangeRF  - HW Radio ON, RF ON\n");
+
+		state_toset = ERFON;
+		ppsc->hwradiooff = false;
+		actuallyset = true;
+	} else if ((ppsc->hwradiooff == false) && (state_toset == ERFOFF)) {
+		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+			 "GPIOChangeRF  - HW Radio OFF, RF OFF\n");
+
+		state_toset = ERFOFF;
+		ppsc->hwradiooff = true;
+		actuallyset = true;
+	}
+
+	if (actuallyset) {
+		spin_lock(&rtlpriv->locks.rf_ps_lock);
+		ppsc->rfchange_inprogress = false;
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+	} else {
+		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC)
+			RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+
+		spin_lock(&rtlpriv->locks.rf_ps_lock);
+		ppsc->rfchange_inprogress = false;
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+	}
+
+	*valid = 1;
+	return !ppsc->hwradiooff;
+}
+
+static void add_one_key(struct ieee80211_hw *hw, u8 *macaddr,
+			struct rtl_mac *mac, u32 key, u32 id,
+			u8 enc_algo, bool is_pairwise)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "add one entry\n");
+	if (is_pairwise) {
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "set Pairwise key\n");
+
+		rtl_cam_add_one_entry(hw, macaddr, key, id, enc_algo,
+				      CAM_CONFIG_NO_USEDK,
+				      rtlpriv->sec.key_buf[key]);
+	} else {
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "set group key\n");
+
+		if (mac->opmode == NL80211_IFTYPE_ADHOC) {
+			rtl_cam_add_one_entry(hw, rtlefuse->dev_addr,
+					      PAIRWISE_KEYIDX,
+					      CAM_PAIRWISE_KEY_POSITION,
+					      enc_algo,
+					      CAM_CONFIG_NO_USEDK,
+					      rtlpriv->sec.key_buf[id]);
+		}
+
+		rtl_cam_add_one_entry(hw, macaddr, key, id, enc_algo,
+				      CAM_CONFIG_NO_USEDK,
+				      rtlpriv->sec.key_buf[id]);
+	}
+}
+
+void rtl88ee_set_key(struct ieee80211_hw *hw, u32 key,
+		     u8 *mac_ad, bool is_group, u8 enc_algo,
+		     bool is_wepkey, bool clear_all)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u8 *macaddr = mac_ad;
+	u32 id = 0;
+	bool is_pairwise = false;
+
+	static u8 cam_const_addr[4][6] = {
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x01},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x02},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x03}
+	};
+	static u8 cam_const_broad[] = {
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff
+	};
+
+	if (clear_all) {
+		u8 idx = 0;
+		u8 cam_offset = 0;
+		u8 clear_number = 5;
+
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "clear_all\n");
+
+		for (idx = 0; idx < clear_number; idx++) {
+			rtl_cam_mark_invalid(hw, cam_offset + idx);
+			rtl_cam_empty_entry(hw, cam_offset + idx);
+
+			if (idx < 5) {
+				memset(rtlpriv->sec.key_buf[idx], 0,
+				       MAX_KEY_LEN);
+				rtlpriv->sec.key_len[idx] = 0;
+			}
+		}
+
+	} else {
+		switch (enc_algo) {
+		case WEP40_ENCRYPTION:
+			enc_algo = CAM_WEP40;
+			break;
+		case WEP104_ENCRYPTION:
+			enc_algo = CAM_WEP104;
+			break;
+		case TKIP_ENCRYPTION:
+			enc_algo = CAM_TKIP;
+			break;
+		case AESCCMP_ENCRYPTION:
+			enc_algo = CAM_AES;
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "switch case not processed\n");
+			enc_algo = CAM_TKIP;
+			break;
+		}
+
+		if (is_wepkey || rtlpriv->sec.use_defaultkey) {
+			macaddr = cam_const_addr[key];
+			id = key;
+		} else {
+			if (is_group) {
+				macaddr = cam_const_broad;
+				id = key;
+			} else {
+				if (mac->opmode == NL80211_IFTYPE_AP ||
+				    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+					id = rtl_cam_get_free_entry(hw, mac_ad);
+					if (id >=  TOTAL_CAM_ENTRY) {
+						RT_TRACE(rtlpriv, COMP_SEC,
+							 DBG_EMERG,
+							 "Can not find free hw security cam entry\n");
+						return;
+					}
+				} else {
+					id = CAM_PAIRWISE_KEY_POSITION;
+				}
+
+				key = PAIRWISE_KEYIDX;
+				is_pairwise = true;
+			}
+		}
+
+		if (rtlpriv->sec.key_len[key] == 0) {
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+				 "delete one entry, id is %d\n", id);
+			if (mac->opmode == NL80211_IFTYPE_AP ||
+			    mac->opmode == NL80211_IFTYPE_MESH_POINT)
+				rtl_cam_del_entry(hw, mac_ad);
+			rtl_cam_delete_one_entry(hw, mac_ad, id);
+		} else {
+			add_one_key(hw, macaddr, mac, key, id, enc_algo,
+				    is_pairwise);
+		}
+	}
+}
+
+static void rtl8188ee_bt_var_init(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *rppriv = rtl_pcipriv(hw);
+	struct bt_coexist_info coexist = rppriv->bt_coexist;
+
+	coexist.bt_coexistence = rppriv->bt_coexist.eeprom_bt_coexist;
+	coexist.bt_ant_num = coexist.eeprom_bt_ant_num;
+	coexist.bt_coexist_type = coexist.eeprom_bt_type;
+
+	if (coexist.reg_bt_iso == 2)
+		coexist.bt_ant_isolation = coexist.eeprom_bt_ant_isol;
+	else
+		coexist.bt_ant_isolation = coexist.reg_bt_iso;
+
+	coexist.bt_radio_shared_type = coexist.eeprom_bt_radio_shared;
+
+	if (coexist.bt_coexistence) {
+		if (coexist.reg_bt_sco == 1)
+			coexist.bt_service = BT_OTHER_ACTION;
+		else if (coexist.reg_bt_sco == 2)
+			coexist.bt_service = BT_SCO;
+		else if (coexist.reg_bt_sco == 4)
+			coexist.bt_service = BT_BUSY;
+		else if (coexist.reg_bt_sco == 5)
+			coexist.bt_service = BT_OTHERBUSY;
+		else
+			coexist.bt_service = BT_IDLE;
+
+		coexist.bt_edca_ul = 0;
+		coexist.bt_edca_dl = 0;
+		coexist.bt_rssi_state = 0xff;
+	}
+}
+
+void rtl8188ee_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
+					      bool auto_load_fail, u8 *hwinfo)
+{
+	rtl8188ee_bt_var_init(hw);
+}
+
+void rtl8188ee_bt_reg_init(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *rppriv = rtl_pcipriv(hw);
+
+	/* 0:Low, 1:High, 2:From Efuse. */
+	rppriv->bt_coexist.reg_bt_iso = 2;
+	/* 0:Idle, 1:None-SCO, 2:SCO, 3:From Counter. */
+	rppriv->bt_coexist.reg_bt_sco = 3;
+	/* 0:Disable BT control A-MPDU, 1:Enable BT control A-MPDU. */
+	rppriv->bt_coexist.reg_bt_sco = 0;
+}
+
+void rtl8188ee_bt_hw_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_pci_priv *rppriv = rtl_pcipriv(hw);
+	struct bt_coexist_info coexist = rppriv->bt_coexist;
+	u8 u1_tmp;
+
+	if (coexist.bt_coexistence &&
+	    ((coexist.bt_coexist_type == BT_CSR_BC4) ||
+	      coexist.bt_coexist_type == BT_CSR_BC8)) {
+		if (coexist.bt_ant_isolation)
+			rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG, 0xa0);
+
+		u1_tmp = rtl_read_byte(rtlpriv, 0x4fd) &
+				       BIT_OFFSET_LEN_MASK_32(0, 1);
+		u1_tmp = u1_tmp | ((coexist.bt_ant_isolation == 1) ?
+			 0 : BIT_OFFSET_LEN_MASK_32(1, 1)) |
+			 ((coexist.bt_service == BT_SCO) ?
+			 0 : BIT_OFFSET_LEN_MASK_32(2, 1));
+		rtl_write_byte(rtlpriv, 0x4fd, u1_tmp);
+
+		rtl_write_dword(rtlpriv, REG_BT_COEX_TABLE+4, 0xaaaa9aaa);
+		rtl_write_dword(rtlpriv, REG_BT_COEX_TABLE+8, 0xffbd0040);
+		rtl_write_dword(rtlpriv, REG_BT_COEX_TABLE+0xc, 0x40000010);
+
+		/* Config to 1T1R. */
+		if (rtlphy->rf_type == RF_1T1R) {
+			u1_tmp = rtl_read_byte(rtlpriv, ROFDM0_TRXPATHENABLE);
+			u1_tmp &= ~(BIT_OFFSET_LEN_MASK_32(1, 1));
+			rtl_write_byte(rtlpriv, ROFDM0_TRXPATHENABLE, u1_tmp);
+
+			u1_tmp = rtl_read_byte(rtlpriv, ROFDM1_TRXPATHENABLE);
+			u1_tmp &= ~(BIT_OFFSET_LEN_MASK_32(1, 1));
+			rtl_write_byte(rtlpriv, ROFDM1_TRXPATHENABLE, u1_tmp);
+		}
+	}
+}
+
+void rtl88ee_suspend(struct ieee80211_hw *hw)
+{
+}
+
+void rtl88ee_resume(struct ieee80211_hw *hw)
+{
+}
+
+/* Turn on AAP (RCR:bit 0) for promicuous mode. */
+void rtl88ee_allow_all_destaddr(struct ieee80211_hw *hw,
+				bool allow_all_da, bool write_into_reg)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	if (allow_all_da) /* Set BIT0 */
+		rtlpci->receive_config |= RCR_AAP;
+	 else /* Clear BIT0 */
+		rtlpci->receive_config &= ~RCR_AAP;
+
+	if (write_into_reg)
+		rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
+
+	RT_TRACE(rtlpriv, COMP_TURBO | COMP_INIT, DBG_LOUD,
+		 "receive_config = 0x%08X, write_into_reg =%d\n",
+		 rtlpci->receive_config, write_into_reg);
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/hw.h b/drivers/net/wireless/rtlwifi/rtl8188ee/hw.h
new file mode 100644
index 000000000000..b4460a41bd01
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/hw.h
@@ -0,0 +1,68 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92CE_HW_H__
+#define __RTL92CE_HW_H__
+
+void rtl88ee_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
+void rtl88ee_read_eeprom_info(struct ieee80211_hw *hw);
+void rtl88ee_interrupt_recognized(struct ieee80211_hw *hw,
+				  u32 *p_inta, u32 *p_intb);
+int rtl88ee_hw_init(struct ieee80211_hw *hw);
+void rtl88ee_card_disable(struct ieee80211_hw *hw);
+void rtl88ee_enable_interrupt(struct ieee80211_hw *hw);
+void rtl88ee_disable_interrupt(struct ieee80211_hw *hw);
+int rtl88ee_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type);
+void rtl88ee_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid);
+void rtl88ee_set_qos(struct ieee80211_hw *hw, int aci);
+void rtl88ee_set_beacon_related_registers(struct ieee80211_hw *hw);
+void rtl88ee_set_beacon_interval(struct ieee80211_hw *hw);
+void rtl88ee_update_interrupt_mask(struct ieee80211_hw *hw,
+				   u32 add_msr, u32 rm_msr);
+void rtl88ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
+void rtl88ee_update_hal_rate_tbl(struct ieee80211_hw *hw,
+				 struct ieee80211_sta *sta, u8 rssi_level);
+void rtl88ee_update_channel_access_setting(struct ieee80211_hw *hw);
+bool rtl88ee_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid);
+void rtl88ee_enable_hw_security_config(struct ieee80211_hw *hw);
+void rtl88ee_set_key(struct ieee80211_hw *hw, u32 key_index,
+		     u8 *p_macaddr, bool is_group, u8 enc_algo,
+		     bool is_wepkey, bool clear_all);
+
+void rtl8188ee_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
+					      bool autoload_fail, u8 *hwinfo);
+void rtl8188ee_bt_reg_init(struct ieee80211_hw *hw);
+void rtl8188ee_bt_hw_init(struct ieee80211_hw *hw);
+void rtl88ee_suspend(struct ieee80211_hw *hw);
+void rtl88ee_resume(struct ieee80211_hw *hw);
+void rtl88ee_allow_all_destaddr(struct ieee80211_hw *hw,
+				bool allow_all_da, bool write_into_reg);
+void rtl88ee_fw_clk_off_timer_callback(unsigned long data);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/led.c b/drivers/net/wireless/rtlwifi/rtl8188ee/led.c
new file mode 100644
index 000000000000..c81a9cb6894c
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/led.c
@@ -0,0 +1,157 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "reg.h"
+#include "led.h"
+
+static void rtl88ee_init_led(struct ieee80211_hw *hw,
+			     struct rtl_led *pled, enum rtl_led_pin ledpin)
+{
+	pled->hw = hw;
+	pled->ledpin = ledpin;
+	pled->ledon = false;
+}
+
+void rtl88ee_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
+{
+	u8 ledcfg;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
+		 "LedAddr:%X ledpin =%d\n", REG_LEDCFG2, pled->ledpin);
+
+	switch (pled->ledpin) {
+	case LED_PIN_GPIO0:
+		break;
+	case LED_PIN_LED0:
+		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG2);
+		rtl_write_byte(rtlpriv, REG_LEDCFG2,
+			       (ledcfg & 0xf0) | BIT(5) | BIT(6));
+		break;
+	case LED_PIN_LED1:
+		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG1);
+		rtl_write_byte(rtlpriv, REG_LEDCFG1, ledcfg & 0x10);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not processed\n");
+		break;
+	}
+	pled->ledon = true;
+}
+
+void rtl88ee_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	u8 ledcfg;
+	u8 val;
+
+	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
+		 "LedAddr:%X ledpin =%d\n", REG_LEDCFG2, pled->ledpin);
+
+	switch (pled->ledpin) {
+	case LED_PIN_GPIO0:
+		break;
+	case LED_PIN_LED0:
+		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG2);
+		ledcfg &= 0xf0;
+		val = ledcfg | BIT(3) | BIT(5) | BIT(6);
+		if (pcipriv->ledctl.led_opendrain == true) {
+			rtl_write_byte(rtlpriv, REG_LEDCFG2, val);
+			ledcfg = rtl_read_byte(rtlpriv, REG_MAC_PINMUX_CFG);
+			val = ledcfg & 0xFE;
+			rtl_write_byte(rtlpriv, REG_MAC_PINMUX_CFG, val);
+		} else {
+			rtl_write_byte(rtlpriv, REG_LEDCFG2, val);
+		}
+		break;
+	case LED_PIN_LED1:
+		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG1);
+		ledcfg &= 0x10;
+		rtl_write_byte(rtlpriv, REG_LEDCFG1, (ledcfg | BIT(3)));
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not processed\n");
+		break;
+	}
+	pled->ledon = false;
+}
+
+void rtl88ee_init_sw_leds(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+
+	rtl88ee_init_led(hw, &(pcipriv->ledctl.sw_led0), LED_PIN_LED0);
+	rtl88ee_init_led(hw, &(pcipriv->ledctl.sw_led1), LED_PIN_LED1);
+}
+
+static void rtl88ee_sw_led_control(struct ieee80211_hw *hw,
+				    enum led_ctl_mode ledaction)
+{
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_led *pLed0 = &(pcipriv->ledctl.sw_led0);
+
+	switch (ledaction) {
+	case LED_CTL_POWER_ON:
+	case LED_CTL_LINK:
+	case LED_CTL_NO_LINK:
+		rtl88ee_sw_led_on(hw, pLed0);
+		break;
+	case LED_CTL_POWER_OFF:
+		rtl88ee_sw_led_off(hw, pLed0);
+		break;
+	default:
+		break;
+	}
+}
+
+void rtl88ee_led_control(struct ieee80211_hw *hw,
+			enum led_ctl_mode ledaction)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	if ((ppsc->rfoff_reason > RF_CHANGE_BY_PS) &&
+	    (ledaction == LED_CTL_TX ||
+	     ledaction == LED_CTL_RX ||
+	     ledaction == LED_CTL_SITE_SURVEY ||
+	     ledaction == LED_CTL_LINK ||
+	     ledaction == LED_CTL_NO_LINK ||
+	     ledaction == LED_CTL_START_TO_LINK ||
+	     ledaction == LED_CTL_POWER_ON)) {
+		return;
+	}
+	RT_TRACE(rtlpriv, COMP_LED, DBG_TRACE, "ledaction %d,\n",
+		 ledaction);
+	rtl88ee_sw_led_control(hw, ledaction);
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/led.h b/drivers/net/wireless/rtlwifi/rtl8188ee/led.h
new file mode 100644
index 000000000000..4073f6f847b2
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/led.h
@@ -0,0 +1,38 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92CE_LED_H__
+#define __RTL92CE_LED_H__
+
+void rtl88ee_init_sw_leds(struct ieee80211_hw *hw);
+void rtl88ee_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled);
+void rtl88ee_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled);
+void rtl88ee_led_control(struct ieee80211_hw *hw, enum led_ctl_mode ledaction);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/phy.c b/drivers/net/wireless/rtlwifi/rtl8188ee/phy.c
new file mode 100644
index 000000000000..e655c0473225
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/phy.c
@@ -0,0 +1,2202 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../ps.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "rf.h"
+#include "dm.h"
+#include "table.h"
+
+static void set_baseband_phy_config(struct ieee80211_hw *hw);
+static void set_baseband_agc_config(struct ieee80211_hw *hw);
+static void store_pwrindex_offset(struct ieee80211_hw *hw,
+				  u32 regaddr, u32 bitmask,
+				  u32 data);
+static bool check_cond(struct ieee80211_hw *hw, const u32  condition);
+
+static u32 rf_serial_read(struct ieee80211_hw *hw,
+			  enum radio_path rfpath, u32 offset)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct bb_reg_def *phreg = &rtlphy->phyreg_def[rfpath];
+	u32 newoffset;
+	u32 tmplong, tmplong2;
+	u8 rfpi_enable = 0;
+	u32 ret;
+	int jj = RF90_PATH_A;
+	int kk = RF90_PATH_B;
+
+	offset &= 0xff;
+	newoffset = offset;
+	if (RT_CANNOT_IO(hw)) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "return all one\n");
+		return 0xFFFFFFFF;
+	}
+	tmplong = rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD);
+	if (rfpath == jj)
+		tmplong2 = tmplong;
+	else
+		tmplong2 = rtl_get_bbreg(hw, phreg->rfhssi_para2, MASKDWORD);
+	tmplong2 = (tmplong2 & (~BLSSIREADADDRESS)) |
+	    (newoffset << 23) | BLSSIREADEDGE;
+	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
+		      tmplong & (~BLSSIREADEDGE));
+	mdelay(1);
+	rtl_set_bbreg(hw, phreg->rfhssi_para2, MASKDWORD, tmplong2);
+	mdelay(2);
+	if (rfpath == jj)
+		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
+						 BIT(8));
+	else if (rfpath == kk)
+		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XB_HSSIPARAMETER1,
+						 BIT(8));
+	if (rfpi_enable)
+		ret = rtl_get_bbreg(hw, phreg->rf_rbpi, BLSSIREADBACKDATA);
+	else
+		ret = rtl_get_bbreg(hw, phreg->rf_rb, BLSSIREADBACKDATA);
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, "RFR-%d Addr[0x%x]= 0x%x\n",
+		 rfpath, phreg->rf_rb, ret);
+	return ret;
+}
+
+static void rf_serial_write(struct ieee80211_hw *hw,
+			    enum radio_path rfpath, u32 offset,
+			    u32 data)
+{
+	u32 data_and_addr;
+	u32 newoffset;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct bb_reg_def *phreg = &rtlphy->phyreg_def[rfpath];
+
+	if (RT_CANNOT_IO(hw)) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "stop\n");
+		return;
+	}
+	offset &= 0xff;
+	newoffset = offset;
+	data_and_addr = ((newoffset << 20) | (data & 0x000fffff)) & 0x0fffffff;
+	rtl_set_bbreg(hw, phreg->rf3wire_offset, MASKDWORD, data_and_addr);
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, "RFW-%d Addr[0x%x]= 0x%x\n",
+		 rfpath, phreg->rf3wire_offset, data_and_addr);
+}
+
+static u32 cal_bit_shift(u32 bitmask)
+{
+	u32 i;
+
+	for (i = 0; i <= 31; i++) {
+		if (((bitmask >> i) & 0x1) == 1)
+			break;
+	}
+	return i;
+}
+
+static bool config_bb_with_header(struct ieee80211_hw *hw,
+				  u8 configtype)
+{
+	if (configtype == BASEBAND_CONFIG_PHY_REG)
+		set_baseband_phy_config(hw);
+	else if (configtype == BASEBAND_CONFIG_AGC_TAB)
+		set_baseband_agc_config(hw);
+	return true;
+}
+
+static bool config_bb_with_pgheader(struct ieee80211_hw *hw,
+				    u8 configtype)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i;
+	u32 *table_pg;
+	u16 tbl_page_len;
+	u32 v1 = 0, v2 = 0;
+
+	tbl_page_len = RTL8188EEPHY_REG_ARRAY_PGLEN;
+	table_pg = RTL8188EEPHY_REG_ARRAY_PG;
+
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		for (i = 0; i < tbl_page_len; i = i + 3) {
+			v1 = table_pg[i];
+			v2 = table_pg[i + 1];
+
+			if (v1 < 0xcdcdcdcd) {
+				if (table_pg[i] == 0xfe)
+					mdelay(50);
+				else if (table_pg[i] == 0xfd)
+					mdelay(5);
+				else if (table_pg[i] == 0xfc)
+					mdelay(1);
+				else if (table_pg[i] == 0xfb)
+					udelay(50);
+				else if (table_pg[i] == 0xfa)
+					udelay(5);
+				else if (table_pg[i] == 0xf9)
+					udelay(1);
+
+				store_pwrindex_offset(hw, table_pg[i],
+						      table_pg[i + 1],
+						      table_pg[i + 2]);
+				continue;
+			} else {
+				if (!check_cond(hw, table_pg[i])) {
+					/*don't need the hw_body*/
+					i += 2; /* skip the pair of expression*/
+					v1 = table_pg[i];
+					v2 = table_pg[i + 1];
+					while (v2 != 0xDEAD) {
+						i += 3;
+						v1 = table_pg[i];
+						v2 = table_pg[i + 1];
+					}
+				}
+			}
+		}
+	} else {
+		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+			 "configtype != BaseBand_Config_PHY_REG\n");
+	}
+	return true;
+}
+
+static bool config_parafile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *fuse = rtl_efuse(rtl_priv(hw));
+	bool rtstatus;
+
+	rtstatus = config_bb_with_header(hw, BASEBAND_CONFIG_PHY_REG);
+	if (rtstatus != true) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Write BB Reg Fail!!");
+		return false;
+	}
+
+	if (fuse->autoload_failflag == false) {
+		rtlphy->pwrgroup_cnt = 0;
+		rtstatus = config_bb_with_pgheader(hw, BASEBAND_CONFIG_PHY_REG);
+	}
+	if (rtstatus != true) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "BB_PG Reg Fail!!");
+		return false;
+	}
+	rtstatus = config_bb_with_header(hw, BASEBAND_CONFIG_AGC_TAB);
+	if (rtstatus != true) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "AGC Table Fail\n");
+		return false;
+	}
+	rtlphy->cck_high_power = (bool) (rtl_get_bbreg(hw,
+				 RFPGA0_XA_HSSIPARAMETER2, 0x200));
+
+	return true;
+}
+
+static void rtl88e_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	int jj = RF90_PATH_A;
+	int kk = RF90_PATH_B;
+
+	rtlphy->phyreg_def[jj].rfintfs = RFPGA0_XAB_RFINTERFACESW;
+	rtlphy->phyreg_def[kk].rfintfs = RFPGA0_XAB_RFINTERFACESW;
+	rtlphy->phyreg_def[RF90_PATH_C].rfintfs = RFPGA0_XCD_RFINTERFACESW;
+	rtlphy->phyreg_def[RF90_PATH_D].rfintfs = RFPGA0_XCD_RFINTERFACESW;
+
+	rtlphy->phyreg_def[jj].rfintfi = RFPGA0_XAB_RFINTERFACERB;
+	rtlphy->phyreg_def[kk].rfintfi = RFPGA0_XAB_RFINTERFACERB;
+	rtlphy->phyreg_def[RF90_PATH_C].rfintfi = RFPGA0_XCD_RFINTERFACERB;
+	rtlphy->phyreg_def[RF90_PATH_D].rfintfi = RFPGA0_XCD_RFINTERFACERB;
+
+	rtlphy->phyreg_def[jj].rfintfo = RFPGA0_XA_RFINTERFACEOE;
+	rtlphy->phyreg_def[kk].rfintfo = RFPGA0_XB_RFINTERFACEOE;
+
+	rtlphy->phyreg_def[jj].rfintfe = RFPGA0_XA_RFINTERFACEOE;
+	rtlphy->phyreg_def[kk].rfintfe = RFPGA0_XB_RFINTERFACEOE;
+
+	rtlphy->phyreg_def[jj].rf3wire_offset = RFPGA0_XA_LSSIPARAMETER;
+	rtlphy->phyreg_def[kk].rf3wire_offset = RFPGA0_XB_LSSIPARAMETER;
+
+	rtlphy->phyreg_def[jj].rflssi_select = rFPGA0_XAB_RFPARAMETER;
+	rtlphy->phyreg_def[kk].rflssi_select = rFPGA0_XAB_RFPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_C].rflssi_select = rFPGA0_XCD_RFPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_D].rflssi_select = rFPGA0_XCD_RFPARAMETER;
+
+	rtlphy->phyreg_def[jj].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+	rtlphy->phyreg_def[kk].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+	rtlphy->phyreg_def[RF90_PATH_C].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+	rtlphy->phyreg_def[RF90_PATH_D].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+
+	rtlphy->phyreg_def[jj].rfhssi_para1 = RFPGA0_XA_HSSIPARAMETER1;
+	rtlphy->phyreg_def[kk].rfhssi_para1 = RFPGA0_XB_HSSIPARAMETER1;
+
+	rtlphy->phyreg_def[jj].rfhssi_para2 = RFPGA0_XA_HSSIPARAMETER2;
+	rtlphy->phyreg_def[kk].rfhssi_para2 = RFPGA0_XB_HSSIPARAMETER2;
+
+	rtlphy->phyreg_def[jj].rfsw_ctrl = RFPGA0_XAB_SWITCHCONTROL;
+	rtlphy->phyreg_def[kk].rfsw_ctrl = RFPGA0_XAB_SWITCHCONTROL;
+	rtlphy->phyreg_def[RF90_PATH_C].rfsw_ctrl = RFPGA0_XCD_SWITCHCONTROL;
+	rtlphy->phyreg_def[RF90_PATH_D].rfsw_ctrl = RFPGA0_XCD_SWITCHCONTROL;
+
+	rtlphy->phyreg_def[jj].rfagc_control1 = ROFDM0_XAAGCCORE1;
+	rtlphy->phyreg_def[kk].rfagc_control1 = ROFDM0_XBAGCCORE1;
+	rtlphy->phyreg_def[RF90_PATH_C].rfagc_control1 = ROFDM0_XCAGCCORE1;
+	rtlphy->phyreg_def[RF90_PATH_D].rfagc_control1 = ROFDM0_XDAGCCORE1;
+
+	rtlphy->phyreg_def[jj].rfagc_control2 = ROFDM0_XAAGCCORE2;
+	rtlphy->phyreg_def[kk].rfagc_control2 = ROFDM0_XBAGCCORE2;
+	rtlphy->phyreg_def[RF90_PATH_C].rfagc_control2 = ROFDM0_XCAGCCORE2;
+	rtlphy->phyreg_def[RF90_PATH_D].rfagc_control2 = ROFDM0_XDAGCCORE2;
+
+	rtlphy->phyreg_def[jj].rfrxiq_imbal = ROFDM0_XARXIQIMBAL;
+	rtlphy->phyreg_def[kk].rfrxiq_imbal = ROFDM0_XBRXIQIMBAL;
+	rtlphy->phyreg_def[RF90_PATH_C].rfrxiq_imbal = ROFDM0_XCRXIQIMBAL;
+	rtlphy->phyreg_def[RF90_PATH_D].rfrxiq_imbal = ROFDM0_XDRXIQIMBAL;
+
+	rtlphy->phyreg_def[jj].rfrx_afe = ROFDM0_XARXAFE;
+	rtlphy->phyreg_def[kk].rfrx_afe = ROFDM0_XBRXAFE;
+	rtlphy->phyreg_def[RF90_PATH_C].rfrx_afe = ROFDM0_XCRXAFE;
+	rtlphy->phyreg_def[RF90_PATH_D].rfrx_afe = ROFDM0_XDRXAFE;
+
+	rtlphy->phyreg_def[jj].rftxiq_imbal = ROFDM0_XATXIQIMBAL;
+	rtlphy->phyreg_def[kk].rftxiq_imbal = ROFDM0_XBTXIQIMBAL;
+	rtlphy->phyreg_def[RF90_PATH_C].rftxiq_imbal = ROFDM0_XCTXIQIMBAL;
+	rtlphy->phyreg_def[RF90_PATH_D].rftxiq_imbal = ROFDM0_XDTXIQIMBAL;
+
+	rtlphy->phyreg_def[jj].rftx_afe = ROFDM0_XATXAFE;
+	rtlphy->phyreg_def[kk].rftx_afe = ROFDM0_XBTXAFE;
+
+	rtlphy->phyreg_def[jj].rf_rb = RFPGA0_XA_LSSIREADBACK;
+	rtlphy->phyreg_def[kk].rf_rb = RFPGA0_XB_LSSIREADBACK;
+
+	rtlphy->phyreg_def[jj].rf_rbpi = TRANSCEIVEA_HSPI_READBACK;
+	rtlphy->phyreg_def[kk].rf_rbpi = TRANSCEIVEB_HSPI_READBACK;
+}
+
+static bool rtl88e_phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
+					    u32 cmdtableidx, u32 cmdtablesz,
+					    enum swchnlcmd_id cmdid,
+					    u32 para1, u32 para2, u32 msdelay)
+{
+	struct swchnlcmd *pcmd;
+
+	if (cmdtable == NULL) {
+		RT_ASSERT(false, "cmdtable cannot be NULL.\n");
+		return false;
+	}
+
+	if (cmdtableidx >= cmdtablesz)
+		return false;
+
+	pcmd = cmdtable + cmdtableidx;
+	pcmd->cmdid = cmdid;
+	pcmd->para1 = para1;
+	pcmd->para2 = para2;
+	pcmd->msdelay = msdelay;
+	return true;
+}
+
+static bool chnl_step_by_step(struct ieee80211_hw *hw,
+			      u8 channel, u8 *stage, u8 *step,
+			      u32 *delay)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct swchnlcmd precommoncmd[MAX_PRECMD_CNT];
+	u32 precommoncmdcnt;
+	struct swchnlcmd postcommoncmd[MAX_POSTCMD_CNT];
+	u32 postcommoncmdcnt;
+	struct swchnlcmd rfdependcmd[MAX_RFDEPENDCMD_CNT];
+	u32 rfdependcmdcnt;
+	struct swchnlcmd *currentcmd = NULL;
+	u8 rfpath;
+	u8 num_total_rfpath = rtlphy->num_total_rfpath;
+
+	precommoncmdcnt = 0;
+	rtl88e_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
+					MAX_PRECMD_CNT,
+					CMDID_SET_TXPOWEROWER_LEVEL, 0, 0, 0);
+	rtl88e_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
+					MAX_PRECMD_CNT, CMDID_END, 0, 0, 0);
+
+	postcommoncmdcnt = 0;
+
+	rtl88e_phy_set_sw_chnl_cmdarray(postcommoncmd, postcommoncmdcnt++,
+					MAX_POSTCMD_CNT, CMDID_END, 0, 0, 0);
+
+	rfdependcmdcnt = 0;
+
+	RT_ASSERT((channel >= 1 && channel <= 14),
+		  "illegal channel for Zebra: %d\n", channel);
+
+	rtl88e_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
+					MAX_RFDEPENDCMD_CNT, CMDID_RF_WRITEREG,
+					RF_CHNLBW, channel, 10);
+
+	rtl88e_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
+					MAX_RFDEPENDCMD_CNT, CMDID_END, 0, 0,
+					 0);
+
+	do {
+		switch (*stage) {
+		case 0:
+			currentcmd = &precommoncmd[*step];
+			break;
+		case 1:
+			currentcmd = &rfdependcmd[*step];
+			break;
+		case 2:
+			currentcmd = &postcommoncmd[*step];
+			break;
+		}
+
+		if (currentcmd->cmdid == CMDID_END) {
+			if ((*stage) == 2) {
+				return true;
+			} else {
+				(*stage)++;
+				(*step) = 0;
+				continue;
+			}
+		}
+
+		switch (currentcmd->cmdid) {
+		case CMDID_SET_TXPOWEROWER_LEVEL:
+			rtl88e_phy_set_txpower_level(hw, channel);
+			break;
+		case CMDID_WRITEPORT_ULONG:
+			rtl_write_dword(rtlpriv, currentcmd->para1,
+					currentcmd->para2);
+			break;
+		case CMDID_WRITEPORT_USHORT:
+			rtl_write_word(rtlpriv, currentcmd->para1,
+				       (u16) currentcmd->para2);
+			break;
+		case CMDID_WRITEPORT_UCHAR:
+			rtl_write_byte(rtlpriv, currentcmd->para1,
+				       (u8) currentcmd->para2);
+			break;
+		case CMDID_RF_WRITEREG:
+			for (rfpath = 0; rfpath < num_total_rfpath; rfpath++) {
+				rtlphy->rfreg_chnlval[rfpath] =
+				    ((rtlphy->rfreg_chnlval[rfpath] &
+				      0xfffffc00) | currentcmd->para2);
+
+				rtl_set_rfreg(hw, (enum radio_path)rfpath,
+					      currentcmd->para1,
+					      RFREG_OFFSET_MASK,
+					      rtlphy->rfreg_chnlval[rfpath]);
+			}
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "switch case not processed\n");
+			break;
+		}
+
+		break;
+	} while (true);
+
+	(*delay) = currentcmd->msdelay;
+	(*step)++;
+	return false;
+}
+
+static long rtl88e_pwr_idx_dbm(struct ieee80211_hw *hw,
+			       enum wireless_mode wirelessmode,
+			       u8 txpwridx)
+{
+	long offset;
+	long pwrout_dbm;
+
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		offset = -7;
+		break;
+	case WIRELESS_MODE_G:
+	case WIRELESS_MODE_N_24G:
+		offset = -8;
+		break;
+	default:
+		offset = -8;
+		break;
+	}
+	pwrout_dbm = txpwridx / 2 + offset;
+	return pwrout_dbm;
+}
+
+static void rtl88e_phy_set_io(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "--->Cmd(%#x), set_io_inprogress(%d)\n",
+		 rtlphy->current_io_type, rtlphy->set_io_inprogress);
+	switch (rtlphy->current_io_type) {
+	case IO_CMD_RESUME_DM_BY_SCAN:
+		dm_digtable->cur_igvalue = rtlphy->initgain_backup.xaagccore1;
+		/*rtl92c_dm_write_dig(hw);*/
+		rtl88e_phy_set_txpower_level(hw, rtlphy->current_channel);
+		rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x83);
+		break;
+	case IO_CMD_PAUSE_DM_BY_SCAN:
+		rtlphy->initgain_backup.xaagccore1 = dm_digtable->cur_igvalue;
+		dm_digtable->cur_igvalue = 0x17;
+		rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x40);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not processed\n");
+		break;
+	}
+	rtlphy->set_io_inprogress = false;
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "(%#x)\n", rtlphy->current_io_type);
+}
+
+u32 rtl88e_phy_query_bb_reg(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 returnvalue, originalvalue, bitshift;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x)\n", regaddr, bitmask);
+	originalvalue = rtl_read_dword(rtlpriv, regaddr);
+	bitshift = cal_bit_shift(bitmask);
+	returnvalue = (originalvalue & bitmask) >> bitshift;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "BBR MASK = 0x%x Addr[0x%x]= 0x%x\n", bitmask,
+		 regaddr, originalvalue);
+
+	return returnvalue;
+}
+
+void rtl88e_phy_set_bb_reg(struct ieee80211_hw *hw,
+			   u32 regaddr, u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 originalvalue, bitshift;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x),data(%#x)\n",
+		 regaddr, bitmask, data);
+
+	if (bitmask != MASKDWORD) {
+		originalvalue = rtl_read_dword(rtlpriv, regaddr);
+		bitshift = cal_bit_shift(bitmask);
+		data = ((originalvalue & (~bitmask)) | (data << bitshift));
+	}
+
+	rtl_write_dword(rtlpriv, regaddr, data);
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x), data(%#x)\n",
+		 regaddr, bitmask, data);
+}
+
+u32 rtl88e_phy_query_rf_reg(struct ieee80211_hw *hw,
+			    enum radio_path rfpath, u32 regaddr, u32 bitmask)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 original_value, readback_value, bitshift;
+	unsigned long flags;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), rfpath(%#x), bitmask(%#x)\n",
+		 regaddr, rfpath, bitmask);
+
+	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
+
+
+	original_value = rf_serial_read(hw, rfpath, regaddr);
+	bitshift = cal_bit_shift(bitmask);
+	readback_value = (original_value & bitmask) >> bitshift;
+
+	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), rfpath(%#x), bitmask(%#x), original_value(%#x)\n",
+		  regaddr, rfpath, bitmask, original_value);
+
+	return readback_value;
+}
+
+void rtl88e_phy_set_rf_reg(struct ieee80211_hw *hw,
+			   enum radio_path rfpath,
+			   u32 regaddr, u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 original_value, bitshift;
+	unsigned long flags;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
+		  regaddr, bitmask, data, rfpath);
+
+	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
+
+	if (bitmask != RFREG_OFFSET_MASK) {
+			original_value = rf_serial_read(hw, rfpath, regaddr);
+			bitshift = cal_bit_shift(bitmask);
+			data = ((original_value & (~bitmask)) |
+				(data << bitshift));
+		}
+
+	rf_serial_write(hw, rfpath, regaddr, data);
+
+
+	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
+		 regaddr, bitmask, data, rfpath);
+}
+
+static bool config_mac_with_header(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+	u32 arraylength;
+	u32 *ptrarray;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "Read Rtl8188EMACPHY_Array\n");
+	arraylength = RTL8188EEMAC_1T_ARRAYLEN;
+	ptrarray = RTL8188EEMAC_1T_ARRAY;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Img:RTL8188EEMAC_1T_ARRAY LEN %d\n", arraylength);
+	for (i = 0; i < arraylength; i = i + 2)
+		rtl_write_byte(rtlpriv, ptrarray[i], (u8) ptrarray[i + 1]);
+	return true;
+}
+
+bool rtl88e_phy_mac_config(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	bool rtstatus = config_mac_with_header(hw);
+
+	rtl_write_byte(rtlpriv, 0x04CA, 0x0B);
+	return rtstatus;
+}
+
+bool rtl88e_phy_bb_config(struct ieee80211_hw *hw)
+{
+	bool rtstatus = true;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 regval;
+	u8 reg_hwparafile = 1;
+	u32 tmp;
+	rtl88e_phy_init_bb_rf_register_definition(hw);
+	regval = rtl_read_word(rtlpriv, REG_SYS_FUNC_EN);
+	rtl_write_word(rtlpriv, REG_SYS_FUNC_EN,
+		       regval | BIT(13) | BIT(0) | BIT(1));
+
+	rtl_write_byte(rtlpriv, REG_RF_CTRL, RF_EN | RF_RSTB | RF_SDMRSTB);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN,
+		       FEN_PPLL | FEN_PCIEA | FEN_DIO_PCIE |
+		       FEN_BB_GLB_RSTN | FEN_BBRSTB);
+	tmp = rtl_read_dword(rtlpriv, 0x4c);
+	rtl_write_dword(rtlpriv, 0x4c, tmp | BIT(23));
+	if (reg_hwparafile == 1)
+		rtstatus = config_parafile(hw);
+	return rtstatus;
+}
+
+bool rtl88e_phy_rf_config(struct ieee80211_hw *hw)
+{
+	return rtl88e_phy_rf6052_config(hw);
+}
+
+static bool check_cond(struct ieee80211_hw *hw,
+				    const u32  condition)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_efuse *fuse = rtl_efuse(rtl_priv(hw));
+	u32 _board = fuse->board_type; /*need efuse define*/
+	u32 _interface = rtlhal->interface;
+	u32 _platform = 0x08;/*SupportPlatform */
+	u32 cond = condition;
+
+	if (condition == 0xCDCDCDCD)
+		return true;
+
+	cond = condition & 0xFF;
+	if ((_board & cond) == 0 && cond != 0x1F)
+		return false;
+
+	cond = condition & 0xFF00;
+	cond = cond >> 8;
+	if ((_interface & cond) == 0 && cond != 0x07)
+		return false;
+
+	cond = condition & 0xFF0000;
+	cond = cond >> 16;
+	if ((_platform & cond) == 0 && cond != 0x0F)
+		return false;
+	return true;
+}
+
+static void _rtl8188e_config_rf_reg(struct ieee80211_hw *hw,
+				    u32 addr, u32 data, enum radio_path rfpath,
+				    u32 regaddr)
+{
+	if (addr == 0xffe) {
+		mdelay(50);
+	} else if (addr == 0xfd) {
+		mdelay(5);
+	} else if (addr == 0xfc) {
+		mdelay(1);
+	} else if (addr == 0xfb) {
+		udelay(50);
+	} else if (addr == 0xfa) {
+		udelay(5);
+	} else if (addr == 0xf9) {
+		udelay(1);
+	} else {
+		rtl_set_rfreg(hw, rfpath, regaddr,
+			      RFREG_OFFSET_MASK,
+			      data);
+		udelay(1);
+	}
+}
+
+static void rtl88_config_s(struct ieee80211_hw *hw,
+	u32 addr, u32 data)
+{
+	u32 content = 0x1000; /*RF Content: radio_a_txt*/
+	u32 maskforphyset = (u32)(content & 0xE000);
+
+	_rtl8188e_config_rf_reg(hw, addr, data, RF90_PATH_A,
+				addr | maskforphyset);
+}
+
+static void _rtl8188e_config_bb_reg(struct ieee80211_hw *hw,
+				    u32 addr, u32 data)
+{
+	if (addr == 0xfe) {
+		mdelay(50);
+	} else if (addr == 0xfd) {
+		mdelay(5);
+	} else if (addr == 0xfc) {
+		mdelay(1);
+	} else if (addr == 0xfb) {
+		udelay(50);
+	} else if (addr == 0xfa) {
+		udelay(5);
+	} else if (addr == 0xf9) {
+		udelay(1);
+	} else {
+		rtl_set_bbreg(hw, addr, MASKDWORD, data);
+		udelay(1);
+	}
+}
+
+
+#define NEXT_PAIR(v1, v2, i)				\
+	do {						\
+		i += 2; v1 = array_table[i];		\
+		v2 = array_table[i + 1];		\
+	} while (0)
+
+static void set_baseband_agc_config(struct ieee80211_hw *hw)
+{
+	int i;
+	u32 *array_table;
+	u16 arraylen;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 v1 = 0, v2 = 0;
+
+	arraylen = RTL8188EEAGCTAB_1TARRAYLEN;
+	array_table = RTL8188EEAGCTAB_1TARRAY;
+
+	for (i = 0; i < arraylen; i += 2) {
+		v1 = array_table[i];
+		v2 = array_table[i + 1];
+		if (v1 < 0xCDCDCDCD) {
+			rtl_set_bbreg(hw, array_table[i], MASKDWORD,
+				      array_table[i + 1]);
+			udelay(1);
+			continue;
+		} else {/*This line is the start line of branch.*/
+			if (!check_cond(hw, array_table[i])) {
+				/*Discard the following (offset, data) pairs*/
+				NEXT_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD && v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < arraylen - 2) {
+					NEXT_PAIR(v1, v2, i);
+				}
+				i -= 2; /* compensate for loop's += 2*/
+			} else {
+				/* Configure matched pairs and skip to end */
+				NEXT_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD && v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < arraylen - 2) {
+					rtl_set_bbreg(hw, array_table[i],
+						      MASKDWORD,
+						      array_table[i + 1]);
+					udelay(1);
+					NEXT_PAIR(v1, v2, i);
+				}
+
+				while (v2 != 0xDEAD && i < arraylen - 2)
+					NEXT_PAIR(v1, v2, i);
+			}
+		}
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "The agctab_array_table[0] is %x Rtl818EEPHY_REGArray[1] is %x\n",
+			 array_table[i],
+			 array_table[i + 1]);
+	}
+}
+
+static void set_baseband_phy_config(struct ieee80211_hw *hw)
+{
+	int i;
+	u32 *array_table;
+	u16 arraylen;
+	u32 v1 = 0, v2 = 0;
+
+	arraylen = RTL8188EEPHY_REG_1TARRAYLEN;
+	array_table = RTL8188EEPHY_REG_1TARRAY;
+
+	for (i = 0; i < arraylen; i += 2) {
+		v1 = array_table[i];
+		v2 = array_table[i + 1];
+		if (v1 < 0xcdcdcdcd) {
+			_rtl8188e_config_bb_reg(hw, v1, v2);
+		} else {/*This line is the start line of branch.*/
+			if (!check_cond(hw, array_table[i])) {
+				/*Discard the following (offset, data) pairs*/
+				NEXT_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < arraylen - 2)
+					NEXT_PAIR(v1, v2, i);
+				i -= 2; /* prevent from for-loop += 2*/
+			} else {
+				/* Configure matched pairs and skip to end */
+				NEXT_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < arraylen - 2) {
+					_rtl8188e_config_bb_reg(hw, v1, v2);
+					NEXT_PAIR(v1, v2, i);
+				}
+
+				while (v2 != 0xDEAD && i < arraylen - 2)
+					NEXT_PAIR(v1, v2, i);
+			}
+		}
+	}
+}
+
+static void store_pwrindex_offset(struct ieee80211_hw *hw,
+				  u32 regaddr, u32 bitmask,
+				  u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (regaddr == RTXAGC_A_RATE18_06) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][0] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][0] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][0]);
+	}
+	if (regaddr == RTXAGC_A_RATE54_24) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][1] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][1] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][1]);
+	}
+	if (regaddr == RTXAGC_A_CCK1_MCS32) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][6] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][6] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][6]);
+	}
+	if (regaddr == RTXAGC_B_CCK11_A_CCK2_11 && bitmask == 0xffffff00) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][7] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][7] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][7]);
+	}
+	if (regaddr == RTXAGC_A_MCS03_MCS00) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][2] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][2] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][2]);
+	}
+	if (regaddr == RTXAGC_A_MCS07_MCS04) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][3] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][3] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][3]);
+	}
+	if (regaddr == RTXAGC_A_MCS11_MCS08) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][4] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][4] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][4]);
+	}
+	if (regaddr == RTXAGC_A_MCS15_MCS12) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][5] = data;
+		if (get_rf_type(rtlphy) == RF_1T1R)
+			rtlphy->pwrgroup_cnt++;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][5] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][5]);
+	}
+	if (regaddr == RTXAGC_B_RATE18_06) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][8] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][8] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][8]);
+	}
+	if (regaddr == RTXAGC_B_RATE54_24) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][9] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][9] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][9]);
+	}
+	if (regaddr == RTXAGC_B_CCK1_55_MCS32) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][14] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][14] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][14]);
+	}
+	if (regaddr == RTXAGC_B_CCK11_A_CCK2_11 && bitmask == 0x000000ff) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][15] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][15] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][15]);
+	}
+	if (regaddr == RTXAGC_B_MCS03_MCS00) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][10] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][10] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][10]);
+	}
+	if (regaddr == RTXAGC_B_MCS07_MCS04) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][11] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][11] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][11]);
+	}
+	if (regaddr == RTXAGC_B_MCS11_MCS08) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][12] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][12] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][12]);
+	}
+	if (regaddr == RTXAGC_B_MCS15_MCS12) {
+		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][13] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][13] = 0x%x\n",
+			 rtlphy->pwrgroup_cnt,
+			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][13]);
+		if (get_rf_type(rtlphy) != RF_1T1R)
+			rtlphy->pwrgroup_cnt++;
+	}
+}
+
+#define READ_NEXT_RF_PAIR(v1, v2, i)		\
+	do {					\
+		i += 2; v1 = a_table[i];	\
+		v2 = a_table[i + 1];		\
+	} while (0)
+
+bool rtl88e_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+					  enum radio_path rfpath)
+{
+	int i;
+	u32 *a_table;
+	u16 a_len;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 v1 = 0, v2 = 0;
+
+	a_len = RTL8188EE_RADIOA_1TARRAYLEN;
+	a_table = RTL8188EE_RADIOA_1TARRAY;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Radio_A:RTL8188EE_RADIOA_1TARRAY %d\n", a_len);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Radio No %x\n", rfpath);
+	switch (rfpath) {
+	case RF90_PATH_A:
+		for (i = 0; i < a_len; i = i + 2) {
+			v1 = a_table[i];
+			v2 = a_table[i + 1];
+			if (v1 < 0xcdcdcdcd) {
+				rtl88_config_s(hw, v1, v2);
+			} else {/*This line is the start line of branch.*/
+				if (!check_cond(hw, a_table[i])) {
+					/* Discard the following (offset, data)
+					 * pairs
+					 */
+					READ_NEXT_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD && v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < a_len - 2)
+						READ_NEXT_RF_PAIR(v1, v2, i);
+					i -= 2; /* prevent from for-loop += 2*/
+				} else {
+					/* Configure matched pairs and skip to
+					 * end of if-else.
+					 */
+					READ_NEXT_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD && v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < a_len - 2) {
+						rtl88_config_s(hw, v1, v2);
+						READ_NEXT_RF_PAIR(v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < a_len - 2)
+						READ_NEXT_RF_PAIR(v1, v2, i);
+				}
+			}
+		}
+
+		if (rtlhal->oem_id == RT_CID_819x_HP)
+			rtl88_config_s(hw, 0x52, 0x7E4BD);
+
+		break;
+
+	case RF90_PATH_B:
+	case RF90_PATH_C:
+	case RF90_PATH_D:
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not processed\n");
+		break;
+	}
+	return true;
+}
+
+void rtl88e_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	rtlphy->default_initialgain[0] = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1,
+						       MASKBYTE0);
+	rtlphy->default_initialgain[1] = rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1,
+						       MASKBYTE0);
+	rtlphy->default_initialgain[2] = rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1,
+						       MASKBYTE0);
+	rtlphy->default_initialgain[3] = rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1,
+						       MASKBYTE0);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+		 "Default initial gain (c50 = 0x%x, c58 = 0x%x, c60 = 0x%x, c68 = 0x%x\n",
+		  rtlphy->default_initialgain[0],
+		  rtlphy->default_initialgain[1],
+		  rtlphy->default_initialgain[2],
+		  rtlphy->default_initialgain[3]);
+
+	rtlphy->framesync = rtl_get_bbreg(hw, ROFDM0_RXDETECTOR3,
+					  MASKBYTE0);
+	rtlphy->framesync_c34 = rtl_get_bbreg(hw, ROFDM0_RXDETECTOR2,
+					      MASKDWORD);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+		 "Default framesync (0x%x) = 0x%x\n",
+		 ROFDM0_RXDETECTOR3, rtlphy->framesync);
+}
+
+void rtl88e_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 level;
+	long dbm;
+
+	level = rtlphy->cur_cck_txpwridx;
+	dbm = rtl88e_pwr_idx_dbm(hw, WIRELESS_MODE_B, level);
+	level = rtlphy->cur_ofdm24g_txpwridx;
+	if (rtl88e_pwr_idx_dbm(hw, WIRELESS_MODE_G, level) > dbm)
+		dbm = rtl88e_pwr_idx_dbm(hw, WIRELESS_MODE_G, level);
+	level = rtlphy->cur_ofdm24g_txpwridx;
+	if (rtl88e_pwr_idx_dbm(hw, WIRELESS_MODE_N_24G, level) > dbm)
+		dbm = rtl88e_pwr_idx_dbm(hw, WIRELESS_MODE_N_24G, level);
+	*powerlevel = dbm;
+}
+
+static void _rtl88e_get_txpower_index(struct ieee80211_hw *hw, u8 channel,
+				      u8 *cckpower, u8 *ofdm, u8 *bw20_pwr,
+				      u8 *bw40_pwr)
+{
+	struct rtl_efuse *fuse = rtl_efuse(rtl_priv(hw));
+	u8 i = (channel - 1);
+	u8 rf_path = 0;
+	int jj = RF90_PATH_A;
+	int kk = RF90_PATH_B;
+
+	for (rf_path = 0; rf_path < 2; rf_path++) {
+		if (rf_path == jj) {
+			cckpower[jj] = fuse->txpwrlevel_cck[jj][i];
+			if (fuse->txpwr_ht20diff[jj][i] > 0x0f) /*-8~7 */
+				bw20_pwr[jj] = fuse->txpwrlevel_ht40_1s[jj][i] -
+					(~(fuse->txpwr_ht20diff[jj][i]) + 1);
+			else
+				bw20_pwr[jj] = fuse->txpwrlevel_ht40_1s[jj][i] +
+					 fuse->txpwr_ht20diff[jj][i];
+			if (fuse->txpwr_legacyhtdiff[jj][i] > 0xf)
+				ofdm[jj] = fuse->txpwrlevel_ht40_1s[jj][i] -
+					(~(fuse->txpwr_legacyhtdiff[jj][i])+1);
+			else
+				ofdm[jj] = fuse->txpwrlevel_ht40_1s[jj][i] +
+					   fuse->txpwr_legacyhtdiff[jj][i];
+			bw40_pwr[jj] = fuse->txpwrlevel_ht40_1s[jj][i];
+
+		} else if (rf_path == kk) {
+			cckpower[kk] = fuse->txpwrlevel_cck[kk][i];
+			bw20_pwr[kk] = fuse->txpwrlevel_ht40_1s[kk][i] +
+				       fuse->txpwr_ht20diff[kk][i];
+			ofdm[kk] = fuse->txpwrlevel_ht40_1s[kk][i] +
+					fuse->txpwr_legacyhtdiff[kk][i];
+			bw40_pwr[kk] = fuse->txpwrlevel_ht40_1s[kk][i];
+		}
+	}
+}
+
+static void _rtl88e_ccxpower_index_check(struct ieee80211_hw *hw,
+					 u8 channel, u8 *cckpower,
+					 u8 *ofdm, u8 *bw20_pwr,
+					 u8 *bw40_pwr)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	rtlphy->cur_cck_txpwridx = cckpower[0];
+	rtlphy->cur_ofdm24g_txpwridx = ofdm[0];
+	rtlphy->cur_bw20_txpwridx = bw20_pwr[0];
+	rtlphy->cur_bw40_txpwridx = bw40_pwr[0];
+}
+
+void rtl88e_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel)
+{
+	struct rtl_efuse *fuse = rtl_efuse(rtl_priv(hw));
+	u8 cckpower[MAX_TX_COUNT]  = {0}, ofdm[MAX_TX_COUNT] = {0};
+	u8 bw20_pwr[MAX_TX_COUNT] = {0}, bw40_pwr[MAX_TX_COUNT] = {0};
+
+	if (fuse->txpwr_fromeprom == false)
+		return;
+	_rtl88e_get_txpower_index(hw, channel, &cckpower[0], &ofdm[0],
+				  &bw20_pwr[0], &bw40_pwr[0]);
+	_rtl88e_ccxpower_index_check(hw, channel, &cckpower[0], &ofdm[0],
+				     &bw20_pwr[0], &bw40_pwr[0]);
+	rtl88e_phy_rf6052_set_cck_txpower(hw, &cckpower[0]);
+	rtl88e_phy_rf6052_set_ofdm_txpower(hw, &ofdm[0], &bw20_pwr[0],
+					   &bw40_pwr[0], channel);
+}
+
+void rtl88e_phy_scan_operation_backup(struct ieee80211_hw *hw, u8 operation)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	enum io_type iotype;
+
+	if (!is_hal_stop(rtlhal)) {
+		switch (operation) {
+		case SCAN_OPT_BACKUP:
+			iotype = IO_CMD_PAUSE_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_IO_CMD,
+						      (u8 *)&iotype);
+			break;
+		case SCAN_OPT_RESTORE:
+			iotype = IO_CMD_RESUME_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_IO_CMD,
+						      (u8 *)&iotype);
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Unknown Scan Backup operation.\n");
+			break;
+		}
+	}
+}
+
+void rtl88e_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u8 reg_bw_opmode;
+	u8 reg_prsr_rsc;
+
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
+		 "Switch to %s bandwidth\n",
+		 rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
+		 "20MHz" : "40MHz");
+
+	if (is_hal_stop(rtlhal)) {
+		rtlphy->set_bwmode_inprogress = false;
+		return;
+	}
+
+	reg_bw_opmode = rtl_read_byte(rtlpriv, REG_BWOPMODE);
+	reg_prsr_rsc = rtl_read_byte(rtlpriv, REG_RRSR + 2);
+
+	switch (rtlphy->current_chan_bw) {
+	case HT_CHANNEL_WIDTH_20:
+		reg_bw_opmode |= BW_OPMODE_20MHZ;
+		rtl_write_byte(rtlpriv, REG_BWOPMODE, reg_bw_opmode);
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		reg_bw_opmode &= ~BW_OPMODE_20MHZ;
+		rtl_write_byte(rtlpriv, REG_BWOPMODE, reg_bw_opmode);
+		reg_prsr_rsc =
+		    (reg_prsr_rsc & 0x90) | (mac->cur_40_prime_sc << 5);
+		rtl_write_byte(rtlpriv, REG_RRSR + 2, reg_prsr_rsc);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "unknown bandwidth: %#X\n", rtlphy->current_chan_bw);
+		break;
+	}
+
+	switch (rtlphy->current_chan_bw) {
+	case HT_CHANNEL_WIDTH_20:
+		rtl_set_bbreg(hw, RFPGA0_RFMOD, BRFMOD, 0x0);
+		rtl_set_bbreg(hw, RFPGA1_RFMOD, BRFMOD, 0x0);
+	/*	rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER2, BIT(10), 1);*/
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		rtl_set_bbreg(hw, RFPGA0_RFMOD, BRFMOD, 0x1);
+		rtl_set_bbreg(hw, RFPGA1_RFMOD, BRFMOD, 0x1);
+
+		rtl_set_bbreg(hw, RCCK0_SYSTEM, BCCK_SIDEBAND,
+			      (mac->cur_40_prime_sc >> 1));
+		rtl_set_bbreg(hw, ROFDM1_LSTF, 0xC00, mac->cur_40_prime_sc);
+		/*rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER2, BIT(10), 0);*/
+
+		rtl_set_bbreg(hw, 0x818, (BIT(26) | BIT(27)),
+			      (mac->cur_40_prime_sc ==
+			       HAL_PRIME_CHNL_OFFSET_LOWER) ? 2 : 1);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "unknown bandwidth: %#X\n", rtlphy->current_chan_bw);
+		break;
+	}
+	rtl88e_phy_rf6052_set_bandwidth(hw, rtlphy->current_chan_bw);
+	rtlphy->set_bwmode_inprogress = false;
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD, "\n");
+}
+
+void rtl88e_phy_set_bw_mode(struct ieee80211_hw *hw,
+			    enum nl80211_channel_type ch_type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tmp_bw = rtlphy->current_chan_bw;
+
+	if (rtlphy->set_bwmode_inprogress)
+		return;
+	rtlphy->set_bwmode_inprogress = true;
+	if ((!is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
+		rtl88e_phy_set_bw_mode_callback(hw);
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "FALSE driver sleep or unload\n");
+		rtlphy->set_bwmode_inprogress = false;
+		rtlphy->current_chan_bw = tmp_bw;
+	}
+}
+
+void rtl88e_phy_sw_chnl_callback(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 delay;
+
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
+		 "switch to channel%d\n", rtlphy->current_channel);
+	if (is_hal_stop(rtlhal))
+		return;
+	do {
+		if (!rtlphy->sw_chnl_inprogress)
+			break;
+		if (!chnl_step_by_step(hw, rtlphy->current_channel,
+				       &rtlphy->sw_chnl_stage,
+				       &rtlphy->sw_chnl_step, &delay)) {
+			if (delay > 0)
+				mdelay(delay);
+			else
+				continue;
+		} else {
+			rtlphy->sw_chnl_inprogress = false;
+		}
+		break;
+	} while (true);
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE, "\n");
+}
+
+u8 rtl88e_phy_sw_chnl(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (rtlphy->sw_chnl_inprogress)
+		return 0;
+	if (rtlphy->set_bwmode_inprogress)
+		return 0;
+	RT_ASSERT((rtlphy->current_channel <= 14),
+		  "WIRELESS_MODE_G but channel>14");
+	rtlphy->sw_chnl_inprogress = true;
+	rtlphy->sw_chnl_stage = 0;
+	rtlphy->sw_chnl_step = 0;
+	if (!(is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
+		rtl88e_phy_sw_chnl_callback(hw);
+		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
+			 "sw_chnl_inprogress false schdule workitem current channel %d\n",
+			 rtlphy->current_channel);
+		rtlphy->sw_chnl_inprogress = false;
+	} else {
+		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
+			 "sw_chnl_inprogress false driver sleep or unload\n");
+		rtlphy->sw_chnl_inprogress = false;
+	}
+	return 1;
+}
+
+static u8 _rtl88e_phy_path_a_iqk(struct ieee80211_hw *hw, bool config_pathb)
+{
+	u32 reg_eac, reg_e94, reg_e9c;
+	u8 result = 0x00;
+
+	rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x10008c1c);
+	rtl_set_bbreg(hw, 0xe34, MASKDWORD, 0x30008c1c);
+	rtl_set_bbreg(hw, 0xe38, MASKDWORD, 0x8214032a);
+	rtl_set_bbreg(hw, 0xe3c, MASKDWORD, 0x28160000);
+
+	rtl_set_bbreg(hw, 0xe4c, MASKDWORD, 0x00462911);
+	rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+
+	reg_eac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
+	reg_e94 = rtl_get_bbreg(hw, 0xe94, MASKDWORD);
+	reg_e9c = rtl_get_bbreg(hw, 0xe9c, MASKDWORD);
+
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	return result;
+}
+
+static u8 _rtl88e_phy_path_b_iqk(struct ieee80211_hw *hw)
+{
+	u32 reg_eac, reg_eb4, reg_ebc, reg_ec4, reg_ecc;
+	u8 result = 0x00;
+
+	rtl_set_bbreg(hw, 0xe60, MASKDWORD, 0x00000002);
+	rtl_set_bbreg(hw, 0xe60, MASKDWORD, 0x00000000);
+	mdelay(IQK_DELAY_TIME);
+	reg_eac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
+	reg_eb4 = rtl_get_bbreg(hw, 0xeb4, MASKDWORD);
+	reg_ebc = rtl_get_bbreg(hw, 0xebc, MASKDWORD);
+	reg_ec4 = rtl_get_bbreg(hw, 0xec4, MASKDWORD);
+	reg_ecc = rtl_get_bbreg(hw, 0xecc, MASKDWORD);
+
+	if (!(reg_eac & BIT(31)) &&
+	    (((reg_eb4 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_ebc & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	else
+		return result;
+	if (!(reg_eac & BIT(30)) &&
+	    (((reg_ec4 & 0x03FF0000) >> 16) != 0x132) &&
+	    (((reg_ecc & 0x03FF0000) >> 16) != 0x36))
+		result |= 0x02;
+	return result;
+}
+
+static u8 _rtl88e_phy_path_a_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
+{
+	u32 reg_eac, reg_e94, reg_e9c, reg_ea4, u32temp;
+	u8 result = 0x00;
+	int jj = RF90_PATH_A;
+
+	/*Get TXIMR Setting*/
+	/*Modify RX IQK mode table*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	rtl_set_rfreg(hw, jj, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
+	rtl_set_rfreg(hw, jj, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, jj, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
+	rtl_set_rfreg(hw, jj, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf117b);
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/*IQK Setting*/
+	rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x81004800);
+
+	/*path a IQK setting*/
+	rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x10008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x30008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82160804);
+	rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x28160000);
+
+	/*LO calibration Setting*/
+	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a911);
+	/*one shot, path A LOK & iqk*/
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+
+	reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
+	reg_e94 = rtl_get_bbreg(hw, RTX_POWER_BEFORE_IQK_A, MASKDWORD);
+	reg_e9c = rtl_get_bbreg(hw, RTX_POWER_AFTER_IQK_A, MASKDWORD);
+
+
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	else
+		return result;
+
+	u32temp = 0x80007C00 | (reg_e94&0x3FF0000)  |
+		  ((reg_e9c&0x3FF0000) >> 16);
+	rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, u32temp);
+	/*RX IQK*/
+	/*Modify RX IQK mode table*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	rtl_set_rfreg(hw, jj, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
+	rtl_set_rfreg(hw, jj, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, jj, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
+	rtl_set_rfreg(hw, jj, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7ffa);
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/*IQK Setting*/
+	rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
+
+	/*path a IQK setting*/
+	rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x30008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x10008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82160c05);
+	rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x28160c05);
+
+	/*LO calibration Setting*/
+	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a911);
+	/*one shot, path A LOK & iqk*/
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+
+	reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
+	reg_e94 = rtl_get_bbreg(hw, RTX_POWER_BEFORE_IQK_A, MASKDWORD);
+	reg_e9c = rtl_get_bbreg(hw, RTX_POWER_AFTER_IQK_A, MASKDWORD);
+	reg_ea4 = rtl_get_bbreg(hw, RRX_POWER_BEFORE_IQK_A_2, MASKDWORD);
+
+	if (!(reg_eac & BIT(27)) &&
+	    (((reg_ea4 & 0x03FF0000) >> 16) != 0x132) &&
+	    (((reg_eac & 0x03FF0000) >> 16) != 0x36))
+		result |= 0x02;
+	return result;
+}
+
+static void fill_iqk(struct ieee80211_hw *hw, bool iqk_ok, long result[][8],
+		     u8 final, bool btxonly)
+{
+	u32 oldval_0, x, tx0_a, reg;
+	long y, tx0_c;
+
+	if (final == 0xFF) {
+		return;
+	} else if (iqk_ok) {
+		oldval_0 = (rtl_get_bbreg(hw, ROFDM0_XATXIQIMBAL,
+					  MASKDWORD) >> 22) & 0x3FF;
+		x = result[final][0];
+		if ((x & 0x00000200) != 0)
+			x = x | 0xFFFFFC00;
+		tx0_a = (x * oldval_0) >> 8;
+		rtl_set_bbreg(hw, ROFDM0_XATXIQIMBAL, 0x3FF, tx0_a);
+		rtl_set_bbreg(hw, ROFDM0_ECCATHRES, BIT(31),
+			      ((x * oldval_0 >> 7) & 0x1));
+		y = result[final][1];
+		if ((y & 0x00000200) != 0)
+			y |= 0xFFFFFC00;
+		tx0_c = (y * oldval_0) >> 8;
+		rtl_set_bbreg(hw, ROFDM0_XCTXAFE, 0xF0000000,
+			      ((tx0_c & 0x3C0) >> 6));
+		rtl_set_bbreg(hw, ROFDM0_XATXIQIMBAL, 0x003F0000,
+			      (tx0_c & 0x3F));
+		rtl_set_bbreg(hw, ROFDM0_ECCATHRES, BIT(29),
+			      ((y * oldval_0 >> 7) & 0x1));
+		if (btxonly)
+			return;
+		reg = result[final][2];
+		rtl_set_bbreg(hw, ROFDM0_XARXIQIMBAL, 0x3FF, reg);
+		reg = result[final][3] & 0x3F;
+		rtl_set_bbreg(hw, ROFDM0_XARXIQIMBAL, 0xFC00, reg);
+		reg = (result[final][3] >> 6) & 0xF;
+		rtl_set_bbreg(hw, 0xca0, 0xF0000000, reg);
+	}
+}
+
+static void save_adda_reg(struct ieee80211_hw *hw,
+			  const u32 *addareg, u32 *backup,
+			  u32 registernum)
+{
+	u32 i;
+
+	for (i = 0; i < registernum; i++)
+		backup[i] = rtl_get_bbreg(hw, addareg[i], MASKDWORD);
+}
+
+static void save_mac_reg(struct ieee80211_hw *hw, const u32 *macreg,
+			 u32 *macbackup)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++)
+		macbackup[i] = rtl_read_byte(rtlpriv, macreg[i]);
+	macbackup[i] = rtl_read_dword(rtlpriv, macreg[i]);
+}
+
+static void reload_adda(struct ieee80211_hw *hw, const u32 *addareg,
+		        u32 *backup, u32 reg_num)
+{
+	u32 i;
+
+	for (i = 0; i < reg_num; i++)
+		rtl_set_bbreg(hw, addareg[i], MASKDWORD, backup[i]);
+}
+
+static void reload_mac(struct ieee80211_hw *hw, const u32 *macreg,
+		       u32 *macbackup)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++)
+		rtl_write_byte(rtlpriv, macreg[i], (u8) macbackup[i]);
+	rtl_write_dword(rtlpriv, macreg[i], macbackup[i]);
+}
+
+static void _rtl88e_phy_path_adda_on(struct ieee80211_hw *hw,
+				     const u32 *addareg, bool is_patha_on,
+				     bool is2t)
+{
+	u32 pathon;
+	u32 i;
+
+	pathon = is_patha_on ? 0x04db25a4 : 0x0b1b25a4;
+	if (false == is2t) {
+		pathon = 0x0bdb25a0;
+		rtl_set_bbreg(hw, addareg[0], MASKDWORD, 0x0b1b25a0);
+	} else {
+		rtl_set_bbreg(hw, addareg[0], MASKDWORD, pathon);
+	}
+
+	for (i = 1; i < IQK_ADDA_REG_NUM; i++)
+		rtl_set_bbreg(hw, addareg[i], MASKDWORD, pathon);
+}
+
+static void _rtl88e_phy_mac_setting_calibration(struct ieee80211_hw *hw,
+						const u32 *macreg,
+						u32 *macbackup)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i = 0;
+
+	rtl_write_byte(rtlpriv, macreg[i], 0x3F);
+
+	for (i = 1; i < (IQK_MAC_REG_NUM - 1); i++)
+		rtl_write_byte(rtlpriv, macreg[i],
+			       (u8) (macbackup[i] & (~BIT(3))));
+	rtl_write_byte(rtlpriv, macreg[i], (u8) (macbackup[i] & (~BIT(5))));
+}
+
+static void _rtl88e_phy_path_a_standby(struct ieee80211_hw *hw)
+{
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x0);
+	rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00010000);
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
+}
+
+static void _rtl88e_phy_pi_mode_switch(struct ieee80211_hw *hw, bool pi_mode)
+{
+	u32 mode;
+
+	mode = pi_mode ? 0x01000100 : 0x01000000;
+	rtl_set_bbreg(hw, 0x820, MASKDWORD, mode);
+	rtl_set_bbreg(hw, 0x828, MASKDWORD, mode);
+}
+
+static bool sim_comp(struct ieee80211_hw *hw, long result[][8], u8 c1, u8 c2)
+{
+	u32 i, j, diff, bitmap, bound;
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	u8 final[2] = {0xFF, 0xFF};
+	bool bresult = true, is2t = IS_92C_SERIAL(rtlhal->version);
+
+	if (is2t)
+		bound = 8;
+	else
+		bound = 4;
+
+	bitmap = 0;
+
+	for (i = 0; i < bound; i++) {
+		diff = (result[c1][i] > result[c2][i]) ?
+		       (result[c1][i] - result[c2][i]) :
+		       (result[c2][i] - result[c1][i]);
+
+		if (diff > MAX_TOLERANCE) {
+			if ((i == 2 || i == 6) && !bitmap) {
+				if (result[c1][i] + result[c1][i + 1] == 0)
+					final[(i / 4)] = c2;
+				else if (result[c2][i] + result[c2][i + 1] == 0)
+					final[(i / 4)] = c1;
+				else
+					bitmap = bitmap | (1 << i);
+			} else {
+				bitmap = bitmap | (1 << i);
+			}
+		}
+	}
+
+	if (bitmap == 0) {
+		for (i = 0; i < (bound / 4); i++) {
+			if (final[i] != 0xFF) {
+				for (j = i * 4; j < (i + 1) * 4 - 2; j++)
+					result[3][j] = result[final[i]][j];
+				bresult = false;
+			}
+		}
+		return bresult;
+	} else if (!(bitmap & 0x0F)) {
+		for (i = 0; i < 4; i++)
+			result[3][i] = result[c1][i];
+		return false;
+	} else if (!(bitmap & 0xF0) && is2t) {
+		for (i = 4; i < 8; i++)
+			result[3][i] = result[c1][i];
+		return false;
+	} else {
+		return false;
+	}
+}
+
+static void _rtl88e_phy_iq_calibrate(struct ieee80211_hw *hw,
+				     long result[][8], u8 t, bool is2t)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 i;
+	u8 patha_ok, pathb_ok;
+	const u32 adda_reg[IQK_ADDA_REG_NUM] = {
+		0x85c, 0xe6c, 0xe70, 0xe74,
+		0xe78, 0xe7c, 0xe80, 0xe84,
+		0xe88, 0xe8c, 0xed0, 0xed4,
+		0xed8, 0xedc, 0xee0, 0xeec
+	};
+	const u32 iqk_mac_reg[IQK_MAC_REG_NUM] = {
+		0x522, 0x550, 0x551, 0x040
+	};
+	const u32 iqk_bb_reg[IQK_BB_REG_NUM] = {
+		ROFDM0_TRXPATHENABLE, ROFDM0_TRMUXPAR, RFPGA0_XCD_RFINTERFACESW,
+		0xb68, 0xb6c, 0x870, 0x860, 0x864, 0x800
+	};
+	const u32 retrycount = 2;
+
+	if (t == 0) {
+		save_adda_reg(hw, adda_reg, rtlphy->adda_backup, 16);
+		save_mac_reg(hw, iqk_mac_reg, rtlphy->iqk_mac_backup);
+		save_adda_reg(hw, iqk_bb_reg, rtlphy->iqk_bb_backup,
+			      IQK_BB_REG_NUM);
+	}
+	_rtl88e_phy_path_adda_on(hw, adda_reg, true, is2t);
+	if (t == 0) {
+		rtlphy->rfpi_enable = (u8) rtl_get_bbreg(hw,
+					   RFPGA0_XA_HSSIPARAMETER1, BIT(8));
+	}
+
+	if (!rtlphy->rfpi_enable)
+		_rtl88e_phy_pi_mode_switch(hw, true);
+	/*BB Setting*/
+	rtl_set_bbreg(hw, 0x800, BIT(24), 0x00);
+	rtl_set_bbreg(hw, 0xc04, MASKDWORD, 0x03a05600);
+	rtl_set_bbreg(hw, 0xc08, MASKDWORD, 0x000800e4);
+	rtl_set_bbreg(hw, 0x874, MASKDWORD, 0x22204000);
+
+	rtl_set_bbreg(hw, 0x870, BIT(10), 0x01);
+	rtl_set_bbreg(hw, 0x870, BIT(26), 0x01);
+	rtl_set_bbreg(hw, 0x860, BIT(10), 0x00);
+	rtl_set_bbreg(hw, 0x864, BIT(10), 0x00);
+
+	if (is2t) {
+		rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00010000);
+		rtl_set_bbreg(hw, 0x844, MASKDWORD, 0x00010000);
+	}
+	_rtl88e_phy_mac_setting_calibration(hw, iqk_mac_reg,
+					    rtlphy->iqk_mac_backup);
+	rtl_set_bbreg(hw, 0xb68, MASKDWORD, 0x0f600000);
+	if (is2t)
+		rtl_set_bbreg(hw, 0xb6c, MASKDWORD, 0x0f600000);
+
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
+	rtl_set_bbreg(hw, 0xe40, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, 0xe44, MASKDWORD, 0x81004800);
+	for (i = 0; i < retrycount; i++) {
+		patha_ok = _rtl88e_phy_path_a_iqk(hw, is2t);
+		if (patha_ok == 0x01) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Path A Tx IQK Success!!\n");
+			result[t][0] = (rtl_get_bbreg(hw, 0xe94, MASKDWORD) &
+					0x3FF0000) >> 16;
+			result[t][1] = (rtl_get_bbreg(hw, 0xe9c, MASKDWORD) &
+					0x3FF0000) >> 16;
+			break;
+		}
+	}
+
+	for (i = 0; i < retrycount; i++) {
+		patha_ok = _rtl88e_phy_path_a_rx_iqk(hw, is2t);
+		if (patha_ok == 0x03) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Path A Rx IQK Success!!\n");
+			result[t][2] = (rtl_get_bbreg(hw, 0xea4, MASKDWORD) &
+					0x3FF0000) >> 16;
+			result[t][3] = (rtl_get_bbreg(hw, 0xeac, MASKDWORD) &
+					0x3FF0000) >> 16;
+			break;
+		} else {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Path a RX iqk fail!!!\n");
+		}
+	}
+
+	if (0 == patha_ok) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Path A IQK Success!!\n");
+	}
+	if (is2t) {
+		_rtl88e_phy_path_a_standby(hw);
+		_rtl88e_phy_path_adda_on(hw, adda_reg, false, is2t);
+		for (i = 0; i < retrycount; i++) {
+			pathb_ok = _rtl88e_phy_path_b_iqk(hw);
+			if (pathb_ok == 0x03) {
+				result[t][4] = (rtl_get_bbreg(hw,
+						0xeb4, MASKDWORD) &
+						0x3FF0000) >> 16;
+				result[t][5] =
+				    (rtl_get_bbreg(hw, 0xebc, MASKDWORD) &
+						   0x3FF0000) >> 16;
+				result[t][6] =
+				    (rtl_get_bbreg(hw, 0xec4, MASKDWORD) &
+						   0x3FF0000) >> 16;
+				result[t][7] =
+				    (rtl_get_bbreg(hw, 0xecc, MASKDWORD) &
+						   0x3FF0000) >> 16;
+				break;
+			} else if (i == (retrycount - 1) && pathb_ok == 0x01) {
+				result[t][4] = (rtl_get_bbreg(hw,
+						0xeb4, MASKDWORD) &
+						0x3FF0000) >> 16;
+			}
+			result[t][5] = (rtl_get_bbreg(hw, 0xebc, MASKDWORD) &
+					0x3FF0000) >> 16;
+		}
+	}
+
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0);
+
+	if (t != 0) {
+		if (!rtlphy->rfpi_enable)
+			_rtl88e_phy_pi_mode_switch(hw, false);
+		reload_adda(hw, adda_reg, rtlphy->adda_backup, 16);
+		reload_mac(hw, iqk_mac_reg, rtlphy->iqk_mac_backup);
+		reload_adda(hw, iqk_bb_reg, rtlphy->iqk_bb_backup,
+			    IQK_BB_REG_NUM);
+
+		rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00032ed3);
+		if (is2t)
+			rtl_set_bbreg(hw, 0x844, MASKDWORD, 0x00032ed3);
+		rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x01008c00);
+		rtl_set_bbreg(hw, 0xe34, MASKDWORD, 0x01008c00);
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "88ee IQK Finish!!\n");
+}
+
+static void _rtl88e_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
+{
+	u8 tmpreg;
+	u32 rf_a_mode = 0, rf_b_mode = 0, lc_cal;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int jj = RF90_PATH_A;
+	int kk = RF90_PATH_B;
+
+	tmpreg = rtl_read_byte(rtlpriv, 0xd03);
+
+	if ((tmpreg & 0x70) != 0)
+		rtl_write_byte(rtlpriv, 0xd03, tmpreg & 0x8F);
+	else
+		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
+
+	if ((tmpreg & 0x70) != 0) {
+		rf_a_mode = rtl_get_rfreg(hw, jj, 0x00, MASK12BITS);
+
+		if (is2t)
+			rf_b_mode = rtl_get_rfreg(hw, kk, 0x00,
+						  MASK12BITS);
+
+		rtl_set_rfreg(hw, jj, 0x00, MASK12BITS,
+			      (rf_a_mode & 0x8FFFF) | 0x10000);
+
+		if (is2t)
+			rtl_set_rfreg(hw, kk, 0x00, MASK12BITS,
+				      (rf_b_mode & 0x8FFFF) | 0x10000);
+	}
+	lc_cal = rtl_get_rfreg(hw, jj, 0x18, MASK12BITS);
+
+	rtl_set_rfreg(hw, jj, 0x18, MASK12BITS, lc_cal | 0x08000);
+
+	mdelay(100);
+
+	if ((tmpreg & 0x70) != 0) {
+		rtl_write_byte(rtlpriv, 0xd03, tmpreg);
+		rtl_set_rfreg(hw, jj, 0x00, MASK12BITS, rf_a_mode);
+
+		if (is2t)
+			rtl_set_rfreg(hw, kk, 0x00, MASK12BITS,
+				      rf_b_mode);
+	} else {
+		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
+}
+
+static void rfpath_switch(struct ieee80211_hw *hw,
+			  bool bmain, bool is2t)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_efuse *fuse = rtl_efuse(rtl_priv(hw));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
+
+	if (is_hal_stop(rtlhal)) {
+		u8 u1btmp;
+		u1btmp = rtl_read_byte(rtlpriv, REG_LEDCFG0);
+		rtl_write_byte(rtlpriv, REG_LEDCFG0, u1btmp | BIT(7));
+		rtl_set_bbreg(hw, rFPGA0_XAB_RFPARAMETER, BIT(13), 0x01);
+	}
+	if (is2t) {
+		if (bmain)
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
+				      BIT(5) | BIT(6), 0x1);
+		else
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
+				      BIT(5) | BIT(6), 0x2);
+	} else {
+		rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, BIT(8) | BIT(9), 0);
+		rtl_set_bbreg(hw, 0x914, MASKLWORD, 0x0201);
+
+		/* We use the RF definition of MAIN and AUX, left antenna and
+		 * right antenna repectively.
+		 * Default output at AUX.
+		 */
+		if (bmain) {
+			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, BIT(14) |
+				      BIT(13) | BIT(12), 0);
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE, BIT(5) |
+				      BIT(4) | BIT(3), 0);
+			if (fuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
+				rtl_set_bbreg(hw, RCONFIG_RAM64X16, BIT(31), 0);
+		} else {
+			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, BIT(14) |
+				      BIT(13) | BIT(12), 1);
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE, BIT(5) |
+				      BIT(4) | BIT(3), 1);
+			if (fuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
+				rtl_set_bbreg(hw, RCONFIG_RAM64X16, BIT(31), 1);
+		}
+	}
+}
+
+#undef IQK_ADDA_REG_NUM
+#undef IQK_DELAY_TIME
+
+void rtl88e_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	long result[4][8];
+	u8 i, final;
+	bool patha_ok;
+	long reg_e94, reg_e9c, reg_ea4, reg_eb4, reg_ebc, reg_tmp = 0;
+	bool is12simular, is13simular, is23simular;
+	u32 iqk_bb_reg[9] = {
+		ROFDM0_XARXIQIMBAL,
+		ROFDM0_XBRXIQIMBAL,
+		ROFDM0_ECCATHRES,
+		ROFDM0_AGCRSSITABLE,
+		ROFDM0_XATXIQIMBAL,
+		ROFDM0_XBTXIQIMBAL,
+		ROFDM0_XCTXAFE,
+		ROFDM0_XDTXAFE,
+		ROFDM0_RXIQEXTANTA
+	};
+
+	if (recovery) {
+		reload_adda(hw, iqk_bb_reg, rtlphy->iqk_bb_backup, 9);
+		return;
+	}
+
+	memset(result, 0, 32 * sizeof(long));
+	final = 0xff;
+	patha_ok = false;
+	is12simular = false;
+	is23simular = false;
+	is13simular = false;
+	for (i = 0; i < 3; i++) {
+		if (get_rf_type(rtlphy) == RF_2T2R)
+			_rtl88e_phy_iq_calibrate(hw, result, i, true);
+		else
+			_rtl88e_phy_iq_calibrate(hw, result, i, false);
+		if (i == 1) {
+			is12simular = sim_comp(hw, result, 0, 1);
+			if (is12simular) {
+				final = 0;
+				break;
+			}
+		}
+		if (i == 2) {
+			is13simular = sim_comp(hw, result, 0, 2);
+			if (is13simular) {
+				final = 0;
+				break;
+			}
+			is23simular = sim_comp(hw, result, 1, 2);
+			if (is23simular) {
+				final = 1;
+			} else {
+				for (i = 0; i < 8; i++)
+					reg_tmp += result[3][i];
+
+				if (reg_tmp != 0)
+					final = 3;
+				else
+					final = 0xFF;
+			}
+		}
+	}
+	for (i = 0; i < 4; i++) {
+		reg_e94 = result[i][0];
+		reg_e9c = result[i][1];
+		reg_ea4 = result[i][2];
+		reg_eb4 = result[i][4];
+		reg_ebc = result[i][5];
+	}
+	if (final != 0xff) {
+		reg_e94 = result[final][0];
+		rtlphy->reg_e94 = reg_e94;
+		reg_e9c = result[final][1];
+		rtlphy->reg_e9c = reg_e9c;
+		reg_ea4 = result[final][2];
+		reg_eb4 = result[final][4];
+		rtlphy->reg_eb4 = reg_eb4;
+		reg_ebc = result[final][5];
+		rtlphy->reg_ebc = reg_ebc;
+		patha_ok = true;
+	} else {
+		rtlphy->reg_e94 = 0x100;
+		rtlphy->reg_eb4 = 0x100;
+		rtlphy->reg_ebc = 0x0;
+		rtlphy->reg_e9c = 0x0;
+	}
+	if (reg_e94 != 0) /*&&(reg_ea4 != 0) */
+		fill_iqk(hw, patha_ok, result, final, (reg_ea4 == 0));
+	if (final != 0xFF) {
+		for (i = 0; i < IQK_MATRIX_REG_NUM; i++)
+			rtlphy->iqk_matrix[0].value[0][i] = result[final][i];
+		rtlphy->iqk_matrix[0].iqk_done = true;
+	}
+	save_adda_reg(hw, iqk_bb_reg, rtlphy->iqk_bb_backup, 9);
+}
+
+void rtl88e_phy_lc_calibrate(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = &(rtlpriv->rtlhal);
+	bool start_conttx = false, singletone = false;
+	u32 timeout = 2000, timecount = 0;
+
+	if (start_conttx || singletone)
+		return;
+
+	while (rtlpriv->mac80211.act_scanning && timecount < timeout) {
+		udelay(50);
+		timecount += 50;
+	}
+
+	rtlphy->lck_inprogress = true;
+	RTPRINT(rtlpriv, FINIT, INIT_IQK,
+		"LCK:Start!!! currentband %x delay %d ms\n",
+		 rtlhal->current_bandtype, timecount);
+
+	_rtl88e_phy_lc_calibrate(hw, false);
+
+	rtlphy->lck_inprogress = false;
+}
+
+void rtl88e_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
+{
+	rfpath_switch(hw, bmain, false);
+}
+
+bool rtl88e_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	bool postprocessing = false;
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "-->IO Cmd(%#x), set_io_inprogress(%d)\n",
+		 iotype, rtlphy->set_io_inprogress);
+	do {
+		switch (iotype) {
+		case IO_CMD_RESUME_DM_BY_SCAN:
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+				 "[IO CMD] Resume DM after scan.\n");
+			postprocessing = true;
+			break;
+		case IO_CMD_PAUSE_DM_BY_SCAN:
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+				 "[IO CMD] Pause DM before scan.\n");
+			postprocessing = true;
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "switch case not processed\n");
+			break;
+		}
+	} while (false);
+	if (postprocessing && !rtlphy->set_io_inprogress) {
+		rtlphy->set_io_inprogress = true;
+		rtlphy->current_io_type = iotype;
+	} else {
+		return false;
+	}
+	rtl88e_phy_set_io(hw);
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE, "IO Type(%#x)\n", iotype);
+	return true;
+}
+
+static void rtl88ee_phy_set_rf_on(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x2b);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
+	/*rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x00);*/
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
+	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
+}
+
+static void _rtl88ee_phy_set_rf_sleep(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int jj = RF90_PATH_A;
+
+	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
+	rtl_set_rfreg(hw, jj, 0x00, RFREG_OFFSET_MASK, 0x00);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
+	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x22);
+}
+
+static bool _rtl88ee_phy_set_rf_power_state(struct ieee80211_hw *hw,
+					    enum rf_pwrstate rfpwr_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl8192_tx_ring *ring = NULL;
+	bool bresult = true;
+	u8 i, queue_id;
+
+	switch (rfpwr_state) {
+	case ERFON:{
+		if ((ppsc->rfpwr_state == ERFOFF) &&
+		    RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC)) {
+			bool rtstatus;
+			u32 init = 0;
+			do {
+				init++;
+				RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+					 "IPS Set eRf nic enable\n");
+				rtstatus = rtl_ps_enable_nic(hw);
+			} while ((rtstatus != true) && (init < 10));
+			RT_CLEAR_PS_LEVEL(ppsc,
+					  RT_RF_OFF_LEVL_HALT_NIC);
+		} else {
+			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+				 "Set ERFON sleeped:%d ms\n",
+				 jiffies_to_msecs(jiffies - ppsc->
+						  last_sleep_jiffies));
+			ppsc->last_awake_jiffies = jiffies;
+			rtl88ee_phy_set_rf_on(hw);
+		}
+		if (mac->link_state == MAC80211_LINKED)
+			rtlpriv->cfg->ops->led_control(hw, LED_CTL_LINK);
+		else
+			rtlpriv->cfg->ops->led_control(hw, LED_CTL_NO_LINK);
+		break; }
+	case ERFOFF:{
+		for (queue_id = 0, i = 0;
+		     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
+			ring = &pcipriv->dev.tx_ring[queue_id];
+			if (skb_queue_len(&ring->queue) == 0) {
+				queue_id++;
+				continue;
+			} else {
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before doze!\n",
+					 (i + 1), queue_id,
+					 skb_queue_len(&ring->queue));
+
+				udelay(10);
+				i++;
+			}
+			if (i >= MAX_DOZE_WAITING_TIMES_9x) {
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "\n ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
+					  MAX_DOZE_WAITING_TIMES_9x,
+					  queue_id,
+					  skb_queue_len(&ring->queue));
+				break;
+			}
+		}
+		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC) {
+			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+				 "IPS Set eRf nic disable\n");
+			rtl_ps_disable_nic(hw);
+			RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+		} else {
+			if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS) {
+				rtlpriv->cfg->ops->led_control(hw,
+						LED_CTL_NO_LINK);
+			} else {
+				rtlpriv->cfg->ops->led_control(hw,
+						LED_CTL_POWER_OFF);
+			}
+		}
+		break; }
+	case ERFSLEEP:{
+		if (ppsc->rfpwr_state == ERFOFF)
+			break;
+		for (queue_id = 0, i = 0;
+		     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
+			ring = &pcipriv->dev.tx_ring[queue_id];
+			if (skb_queue_len(&ring->queue) == 0) {
+				queue_id++;
+				continue;
+			} else {
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before doze!\n",
+					 (i + 1), queue_id,
+					 skb_queue_len(&ring->queue));
+
+				udelay(10);
+				i++;
+			}
+			if (i >= MAX_DOZE_WAITING_TIMES_9x) {
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "\n ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
+					 MAX_DOZE_WAITING_TIMES_9x,
+					 queue_id,
+					 skb_queue_len(&ring->queue));
+				break;
+			}
+		}
+		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+			 "Set ERFSLEEP awaked:%d ms\n",
+			 jiffies_to_msecs(jiffies - ppsc->last_awake_jiffies));
+		ppsc->last_sleep_jiffies = jiffies;
+		_rtl88ee_phy_set_rf_sleep(hw);
+		break; }
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not processed\n");
+		bresult = false;
+		break;
+	}
+	if (bresult)
+		ppsc->rfpwr_state = rfpwr_state;
+	return bresult;
+}
+
+bool rtl88e_phy_set_rf_power_state(struct ieee80211_hw *hw,
+				   enum rf_pwrstate rfpwr_state)
+{
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool bresult;
+
+	if (rfpwr_state == ppsc->rfpwr_state)
+		return false;
+	bresult = _rtl88ee_phy_set_rf_power_state(hw, rfpwr_state);
+	return bresult;
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/phy.h b/drivers/net/wireless/rtlwifi/rtl8188ee/phy.h
new file mode 100644
index 000000000000..f1acd6d27e44
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/phy.h
@@ -0,0 +1,236 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92C_PHY_H__
+#define __RTL92C_PHY_H__
+
+/*It must always set to 4, otherwise read efuse table secquence will be wrong.*/
+#define	MAX_TX_COUNT				4
+
+#define MAX_PRECMD_CNT				16
+#define MAX_RFDEPENDCMD_CNT			16
+#define MAX_POSTCMD_CNT				16
+
+#define MAX_DOZE_WAITING_TIMES_9x		64
+
+#define RT_CANNOT_IO(hw)			false
+#define HIGHPOWER_RADIOA_ARRAYLEN		22
+
+#define IQK_ADDA_REG_NUM			16
+#define IQK_BB_REG_NUM				9
+#define MAX_TOLERANCE				5
+#define	IQK_DELAY_TIME				10
+#define	IDX_MAP					15
+
+#define	APK_BB_REG_NUM				5
+#define	APK_AFE_REG_NUM				16
+#define	APK_CURVE_REG_NUM			4
+#define	PATH_NUM				2
+
+#define LOOP_LIMIT				5
+#define MAX_STALL_TIME				50
+#define ANTENNADIVERSITYVALUE			0x80
+#define MAX_TXPWR_IDX_NMODE_92S			63
+#define RESET_CNT_LIMIT				3
+
+#define IQK_ADDA_REG_NUM			16
+#define IQK_MAC_REG_NUM				4
+
+#define RF6052_MAX_PATH				2
+
+#define CT_OFFSET_MAC_ADDR			0X16
+
+#define CT_OFFSET_CCK_TX_PWR_IDX		0x5A
+#define CT_OFFSET_HT401S_TX_PWR_IDX		0x60
+#define CT_OFFSET_HT402S_TX_PWR_IDX_DIFF	0x66
+#define CT_OFFSET_HT20_TX_PWR_IDX_DIFF		0x69
+#define CT_OFFSET_OFDM_TX_PWR_IDX_DIFF		0x6C
+
+#define CT_OFFSET_HT40_MAX_PWR_OFFSET		0x6F
+#define CT_OFFSET_HT20_MAX_PWR_OFFSET		0x72
+
+#define CT_OFFSET_CHANNEL_PLAH			0x75
+#define CT_OFFSET_THERMAL_METER			0x78
+#define CT_OFFSET_RF_OPTION			0x79
+#define CT_OFFSET_VERSION			0x7E
+#define CT_OFFSET_CUSTOMER_ID			0x7F
+
+#define RTL92C_MAX_PATH_NUM			2
+
+enum swchnlcmd_id {
+	CMDID_END,
+	CMDID_SET_TXPOWEROWER_LEVEL,
+	CMDID_BBREGWRITE10,
+	CMDID_WRITEPORT_ULONG,
+	CMDID_WRITEPORT_USHORT,
+	CMDID_WRITEPORT_UCHAR,
+	CMDID_RF_WRITEREG,
+};
+
+struct swchnlcmd {
+	enum swchnlcmd_id cmdid;
+	u32 para1;
+	u32 para2;
+	u32 msdelay;
+};
+
+enum hw90_block_e {
+	HW90_BLOCK_MAC = 0,
+	HW90_BLOCK_PHY0 = 1,
+	HW90_BLOCK_PHY1 = 2,
+	HW90_BLOCK_RF = 3,
+	HW90_BLOCK_MAXIMUM = 4,
+};
+
+enum baseband_config_type {
+	BASEBAND_CONFIG_PHY_REG = 0,
+	BASEBAND_CONFIG_AGC_TAB = 1,
+};
+
+enum ra_offset_area {
+	RA_OFFSET_LEGACY_OFDM1,
+	RA_OFFSET_LEGACY_OFDM2,
+	RA_OFFSET_HT_OFDM1,
+	RA_OFFSET_HT_OFDM2,
+	RA_OFFSET_HT_OFDM3,
+	RA_OFFSET_HT_OFDM4,
+	RA_OFFSET_HT_CCK,
+};
+
+enum antenna_path {
+	ANTENNA_NONE,
+	ANTENNA_D,
+	ANTENNA_C,
+	ANTENNA_CD,
+	ANTENNA_B,
+	ANTENNA_BD,
+	ANTENNA_BC,
+	ANTENNA_BCD,
+	ANTENNA_A,
+	ANTENNA_AD,
+	ANTENNA_AC,
+	ANTENNA_ACD,
+	ANTENNA_AB,
+	ANTENNA_ABD,
+	ANTENNA_ABC,
+	ANTENNA_ABCD
+};
+
+struct r_antenna_select_ofdm {
+	u32 r_tx_antenna:4;
+	u32 r_ant_l:4;
+	u32 r_ant_non_ht:4;
+	u32 r_ant_ht1:4;
+	u32 r_ant_ht2:4;
+	u32 r_ant_ht_s1:4;
+	u32 r_ant_non_ht_s1:4;
+	u32 ofdm_txsc:2;
+	u32 reserved:2;
+};
+
+struct r_antenna_select_cck {
+	u8 r_cckrx_enable_2:2;
+	u8 r_cckrx_enable:2;
+	u8 r_ccktx_enable:4;
+};
+
+
+struct efuse_contents {
+	u8 mac_addr[ETH_ALEN];
+	u8 cck_tx_power_idx[6];
+	u8 ht40_1s_tx_power_idx[6];
+	u8 ht40_2s_tx_power_idx_diff[3];
+	u8 ht20_tx_power_idx_diff[3];
+	u8 ofdm_tx_power_idx_diff[3];
+	u8 ht40_max_power_offset[3];
+	u8 ht20_max_power_offset[3];
+	u8 channel_plan;
+	u8 thermal_meter;
+	u8 rf_option[5];
+	u8 version;
+	u8 oem_id;
+	u8 regulatory;
+};
+
+struct tx_power_struct {
+	u8 cck[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 ht40_1s[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 ht40_2s[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 ht20_diff[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 legacy_ht_diff[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 legacy_ht_txpowerdiff;
+	u8 groupht20[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 groupht40[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 pwrgroup_cnt;
+	u32 mcs_original_offset[4][16];
+};
+
+enum _ANT_DIV_TYPE {
+	NO_ANTDIV			= 0xFF,
+	CG_TRX_HW_ANTDIV		= 0x01,
+	CGCS_RX_HW_ANTDIV		= 0x02,
+	FIXED_HW_ANTDIV			= 0x03,
+	CG_TRX_SMART_ANTDIV		= 0x04,
+	CGCS_RX_SW_ANTDIV		= 0x05,
+};
+
+extern u32 rtl88e_phy_query_bb_reg(struct ieee80211_hw *hw,
+				   u32 regaddr, u32 bitmask);
+extern void rtl88e_phy_set_bb_reg(struct ieee80211_hw *hw,
+				  u32 regaddr, u32 bitmask, u32 data);
+extern u32 rtl88e_phy_query_rf_reg(struct ieee80211_hw *hw,
+				   enum radio_path rfpath, u32 regaddr,
+				   u32 bitmask);
+extern void rtl88e_phy_set_rf_reg(struct ieee80211_hw *hw,
+				  enum radio_path rfpath, u32 regaddr,
+				  u32 bitmask, u32 data);
+extern bool rtl88e_phy_mac_config(struct ieee80211_hw *hw);
+extern bool rtl88e_phy_bb_config(struct ieee80211_hw *hw);
+extern bool rtl88e_phy_rf_config(struct ieee80211_hw *hw);
+extern void rtl88e_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw);
+extern void rtl88e_phy_get_txpower_level(struct ieee80211_hw *hw,
+					 long *powerlevel);
+extern void rtl88e_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel);
+extern void rtl88e_phy_scan_operation_backup(struct ieee80211_hw *hw,
+					     u8 operation);
+extern void rtl88e_phy_set_bw_mode_callback(struct ieee80211_hw *hw);
+extern void rtl88e_phy_set_bw_mode(struct ieee80211_hw *hw,
+				   enum nl80211_channel_type ch_type);
+extern void rtl88e_phy_sw_chnl_callback(struct ieee80211_hw *hw);
+extern u8 rtl88e_phy_sw_chnl(struct ieee80211_hw *hw);
+extern void rtl88e_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery);
+void rtl88e_phy_lc_calibrate(struct ieee80211_hw *hw);
+void rtl88e_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain);
+bool rtl88e_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+					  enum radio_path rfpath);
+bool rtl88e_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype);
+extern bool rtl88e_phy_set_rf_power_state(struct ieee80211_hw *hw,
+					  enum rf_pwrstate rfpwr_state);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.c b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.c
new file mode 100644
index 000000000000..6dc4e3a954f6
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.c
@@ -0,0 +1,109 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "pwrseqcmd.h"
+#include "pwrseq.h"
+
+/* drivers should parse below arrays and do the corresponding actions */
+/*3 Power on  Array*/
+struct wlan_pwr_cfg rtl8188e_power_on_flow[RTL8188E_TRANS_CARDEMU_TO_ACT_STEPS +
+					   RTL8188E_TRANS_END_STEPS] = {
+	RTL8188E_TRANS_CARDEMU_TO_ACT
+	RTL8188E_TRANS_END
+};
+
+/*3Radio off GPIO Array */
+struct wlan_pwr_cfg rtl8188e_radio_off_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS
+					    + RTL8188E_TRANS_END_STEPS] = {
+	RTL8188E_TRANS_ACT_TO_CARDEMU
+	RTL8188E_TRANS_END
+};
+
+/*3Card Disable Array*/
+struct wlan_pwr_cfg rtl8188e_card_disable_flow
+	[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+	RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS +
+	RTL8188E_TRANS_END_STEPS] = {
+		RTL8188E_TRANS_ACT_TO_CARDEMU
+		RTL8188E_TRANS_CARDEMU_TO_CARDDIS
+		RTL8188E_TRANS_END
+};
+
+/*3 Card Enable Array*/
+struct wlan_pwr_cfg rtl8188e_card_enable_flow
+	[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+	RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS +
+	RTL8188E_TRANS_END_STEPS] = {
+		RTL8188E_TRANS_CARDDIS_TO_CARDEMU
+		RTL8188E_TRANS_CARDEMU_TO_ACT
+		RTL8188E_TRANS_END
+};
+
+/*3Suspend Array*/
+struct wlan_pwr_cfg rtl8188e_suspend_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS
+					+ RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS
+					+ RTL8188E_TRANS_END_STEPS] = {
+	RTL8188E_TRANS_ACT_TO_CARDEMU
+	RTL8188E_TRANS_CARDEMU_TO_SUS
+	RTL8188E_TRANS_END
+};
+
+/*3 Resume Array*/
+struct wlan_pwr_cfg rtl8188e_resume_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS
+					+ RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS
+					+ RTL8188E_TRANS_END_STEPS] = {
+	RTL8188E_TRANS_SUS_TO_CARDEMU
+	RTL8188E_TRANS_CARDEMU_TO_ACT
+	RTL8188E_TRANS_END
+};
+
+/*3HWPDN Array*/
+struct wlan_pwr_cfg rtl8188e_hwpdn_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS
+				+ RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS
+				+ RTL8188E_TRANS_END_STEPS] = {
+	RTL8188E_TRANS_ACT_TO_CARDEMU
+	RTL8188E_TRANS_CARDEMU_TO_PDN
+	RTL8188E_TRANS_END
+};
+
+/*3 Enter LPS */
+struct wlan_pwr_cfg rtl8188e_enter_lps_flow[RTL8188E_TRANS_ACT_TO_LPS_STEPS
+					+ RTL8188E_TRANS_END_STEPS] = {
+	/*FW behavior*/
+	RTL8188E_TRANS_ACT_TO_LPS
+	RTL8188E_TRANS_END
+};
+
+/*3 Leave LPS */
+struct wlan_pwr_cfg rtl8188e_leave_lps_flow[RTL8188E_TRANS_LPS_TO_ACT_STEPS
+					+ RTL8188E_TRANS_END_STEPS] = {
+	/*FW behavior*/
+	RTL8188E_TRANS_LPS_TO_ACT
+	RTL8188E_TRANS_END
+};
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.h b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.h
new file mode 100644
index 000000000000..028ec6dd52b4
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.h
@@ -0,0 +1,327 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8723E_PWRSEQ_H__
+#define __RTL8723E_PWRSEQ_H__
+
+#include "pwrseqcmd.h"
+/*
+	Check document WM-20110607-Paul-RTL8188E_Power_Architecture-R02.vsd
+	There are 6 HW Power States:
+	0: POFF--Power Off
+	1: PDN--Power Down
+	2: CARDEMU--Card Emulation
+	3: ACT--Active Mode
+	4: LPS--Low Power State
+	5: SUS--Suspend
+
+	The transision from different states are defined below
+	TRANS_CARDEMU_TO_ACT
+	TRANS_ACT_TO_CARDEMU
+	TRANS_CARDEMU_TO_SUS
+	TRANS_SUS_TO_CARDEMU
+	TRANS_CARDEMU_TO_PDN
+	TRANS_ACT_TO_LPS
+	TRANS_LPS_TO_ACT
+
+	TRANS_END
+	PWR SEQ Version: rtl8188e_PwrSeq_V09.h
+*/
+
+#define	RTL8188E_TRANS_CARDEMU_TO_ACT_STEPS	10
+#define	RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS	10
+#define	RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS	10
+#define	RTL8188E_TRANS_SUS_TO_CARDEMU_STEPS	10
+#define	RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS	10
+#define	RTL8188E_TRANS_PDN_TO_CARDEMU_STEPS	10
+#define	RTL8188E_TRANS_ACT_TO_LPS_STEPS		15
+#define	RTL8188E_TRANS_LPS_TO_ACT_STEPS		15
+#define	RTL8188E_TRANS_END_STEPS		1
+
+
+#define RTL8188E_TRANS_CARDEMU_TO_ACT					\
+	/* format */							\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/* wait till 0x04[17] = 1    power ready*/			\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), BIT(1)},		\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/* 0x02[1:0] = 0	reset BB*/				\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0)|BIT(1), 0},		\
+	{0x0026, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*0x24[23] = 2b'01 schmit trigger */				\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), BIT(7)},		\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/* 0x04[15] = 0 disable HWPDN (control by DRV)*/		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0},			\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*0x04[12:11] = 2b'00 disable WL suspend*/			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4)|BIT(3), 0},		\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*0x04[8] = 1 polling until return 0*/				\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*wait till 0x04[8] = 0*/					\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(0), 0},			\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0}, /*LDO normal mode*/\
+	{0x0074, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)}, /*SDIO Driving*/\
+
+#define RTL8188E_TRANS_ACT_TO_CARDEMU					\
+	/* format */							\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},/*0x1F[7:0] = 0 turn off RF*/\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)}, /*LDO Sleep mode*/\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*0x04[9] = 1 turn off MAC by HW state machine*/		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*wait till 0x04[9] = 0 polling until return 0 to disable*/	\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), 0},			\
+
+
+#define RTL8188E_TRANS_CARDEMU_TO_SUS					\
+	/* format */							\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,				\
+	/*0x04[12:11] = 2b'01enable WL suspend*/			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
+	/*0x04[12:11] = 2b'11enable WL suspend for PCIe*/		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)|BIT(4)},\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,				\
+	/*  0x04[31:30] = 2b'10 enable enable bandgap mbias in suspend */\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, BIT(7)},			\
+	{0x0041, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,				\
+	/*Clear SIC_EN register 0x40[12] = 1'b0 */			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0},			\
+	{0xfe10, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,				\
+	/*Set USB suspend enable local register  0xfe10[4]= 1 */	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)},		\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	/*Set SDIO suspend local register*/				\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	/*wait power state to suspend*/					\
+	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), 0},
+
+#define RTL8188E_TRANS_SUS_TO_CARDEMU					\
+	/* format */							\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	/*Set SDIO suspend local register*/				\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), 0},			\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	/*wait power state to suspend*/					\
+	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), BIT(1)},		\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*0x04[12:11] = 2b'01enable WL suspend*/			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), 0},
+
+#define RTL8188E_TRANS_CARDEMU_TO_CARDDIS				\
+	/* format */							\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+	{0x0026, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*0x24[23] = 2b'01 schmit trigger */				\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), BIT(7)},		\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,				\
+	/*0x04[12:11] = 2b'01 enable WL suspend*/			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)},	\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,				\
+	/*  0x04[31:30] = 2b'10 enable enable bandgap mbias in suspend */\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},			\
+	{0x0041, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,				\
+	/*Clear SIC_EN register 0x40[12] = 1'b0 */			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0},			\
+	{0xfe10, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,	\
+	/*Set USB suspend enable local register  0xfe10[4]= 1 */	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)},		\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	/*Set SDIO suspend local register*/				\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	PWR_CMD_POLLING, BIT(1), 0}, /*wait power state to suspend*/
+
+#define RTL8188E_TRANS_CARDDIS_TO_CARDEMU				\
+	/* format */							\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	PWR_BASEADDR_SDIO,\
+	PWR_CMD_WRITE, BIT(0), 0}, /*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	PWR_BASEADDR_SDIO,\
+	PWR_CMD_POLLING, BIT(1), BIT(1)}, /*wait power state to suspend*/\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC,						\
+	PWR_CMD_WRITE, BIT(3)|BIT(4), 0},				\
+	/*0x04[12:11] = 2b'01enable WL suspend*/
+
+
+#define RTL8188E_TRANS_CARDEMU_TO_PDN					\
+	/* format */							\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},/* 0x04[16] = 0*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), BIT(7)},/* 0x04[15] = 1*/
+
+
+#define RTL8188E_TRANS_PDN_TO_CARDEMU					\
+	/* format */							\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0},/* 0x04[15] = 0*/
+
+
+#define RTL8188E_TRANS_ACT_TO_LPS					\
+	/* format */							\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x7F},/*Tx Pause*/	\
+	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*zero if no pkt is tx*/\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*Should be zero if no packet is transmitting*/	\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*Should be zero if no packet is transmitting*/			\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*Should be zero if no packet is transmitting*/			\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*CCK and OFDM are disabled, and clock are gated*/		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},			\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},/*Delay 1us*/\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x3F},/*Reset MAC TRX*/	\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*check if removed later*/					\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), 0},			\
+	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*Respond TxOK to scheduler*/					\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(5), BIT(5)},		\
+
+
+#define RTL8188E_TRANS_LPS_TO_ACT					\
+	/* format */							\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, 0xFF, 0x84}, /*SDIO RPWM*/	\
+	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84}, /*USB RPWM*/	\
+	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84}, /*PCIe RPWM*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS}, /*Delay*/	\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*.	0x08[4] = 0		 switch TSF to 40M*/		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0},			\
+	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*Polling 0x109[7]= 0  TSF in 40M*/				\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(7), 0},			\
+	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*. 0x29[7:6] = 2b'00	 enable BB clock*/			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(6)|BIT(7), 0},		\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*.	0x101[1] = 1*/\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*.	0x100[7:0] = 0xFF	 enable WMAC TRX*/\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF},			\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	/*. 0x02[1:0] = 2b'11  enable BB macro*/\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1)|BIT(0), BIT(1)|BIT(0)},	\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0}, /*.	0x522 = 0*/
+
+
+#define RTL8188E_TRANS_END						\
+	/* format */							\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	0, PWR_CMD_END, 0, 0}
+
+extern struct wlan_pwr_cfg rtl8188e_power_on_flow
+		[RTL8188E_TRANS_CARDEMU_TO_ACT_STEPS +
+		RTL8188E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188e_radio_off_flow
+		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+		RTL8188E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188e_card_disable_flow
+		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+		RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS +
+		RTL8188E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188e_card_enable_flow
+		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+		RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS +
+		RTL8188E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188e_suspend_flow
+		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+		RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS +
+		RTL8188E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188e_resume_flow
+		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+		RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS +
+		RTL8188E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188e_hwpdn_flow
+		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+		RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS +
+		RTL8188E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188e_enter_lps_flow
+		[RTL8188E_TRANS_ACT_TO_LPS_STEPS +
+		RTL8188E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188e_leave_lps_flow
+		[RTL8188E_TRANS_LPS_TO_ACT_STEPS +
+		RTL8188E_TRANS_END_STEPS];
+
+/* RTL8723 Power Configuration CMDs for PCIe interface */
+#define Rtl8188E_NIC_PWR_ON_FLOW	rtl8188e_power_on_flow
+#define Rtl8188E_NIC_RF_OFF_FLOW	rtl8188e_radio_off_flow
+#define Rtl8188E_NIC_DISABLE_FLOW	rtl8188e_card_disable_flow
+#define Rtl8188E_NIC_ENABLE_FLOW	rtl8188e_card_enable_flow
+#define Rtl8188E_NIC_SUSPEND_FLOW	rtl8188e_suspend_flow
+#define Rtl8188E_NIC_RESUME_FLOW	rtl8188e_resume_flow
+#define Rtl8188E_NIC_PDN_FLOW		rtl8188e_hwpdn_flow
+#define Rtl8188E_NIC_LPS_ENTER_FLOW	rtl8188e_enter_lps_flow
+#define Rtl8188E_NIC_LPS_LEAVE_FLOW	rtl8188e_leave_lps_flow
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseqcmd.c b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseqcmd.c
new file mode 100644
index 000000000000..a9cfa13be3a8
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseqcmd.c
@@ -0,0 +1,140 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "pwrseq.h"
+
+
+/*	Description:
+ *		This routine deal with the Power Configuration CMDs
+ *		 parsing for RTL8723/RTL8188E Series IC.
+ *	Assumption:
+ *		We should follow specific format which was released from HW SD.
+ *
+ *	2011.07.07, added by Roger.
+ */
+
+bool rtl88_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
+				u8 fab_version, u8 interface_type,
+				struct wlan_pwr_cfg pwrcfgcmd[])
+{
+	struct wlan_pwr_cfg cmd = {0};
+	bool polling_bit = false;
+	u32 ary_idx = 0;
+	u8 val = 0;
+	u32 offset = 0;
+	u32 polling_count = 0;
+	u32 max_polling_cnt = 5000;
+
+	do {
+		cmd = pwrcfgcmd[ary_idx];
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "rtl88_hal_pwrseqcmdparsing(): offset(%#x), cut_msk(%#x), fab_msk(%#x),"
+			 "interface_msk(%#x), base(%#x), cmd(%#x), msk(%#x), val(%#x)\n",
+			 GET_PWR_CFG_OFFSET(cmd),
+			 GET_PWR_CFG_CUT_MASK(cmd),
+			 GET_PWR_CFG_FAB_MASK(cmd),
+			 GET_PWR_CFG_INTF_MASK(cmd),
+			 GET_PWR_CFG_BASE(cmd),
+			 GET_PWR_CFG_CMD(cmd),
+			 GET_PWR_CFG_MASK(cmd),
+			 GET_PWR_CFG_VALUE(cmd));
+
+		if ((GET_PWR_CFG_FAB_MASK(cmd) & fab_version) &&
+		    (GET_PWR_CFG_CUT_MASK(cmd) & cut_version) &&
+		    (GET_PWR_CFG_INTF_MASK(cmd) & interface_type)) {
+			switch (GET_PWR_CFG_CMD(cmd)) {
+			case PWR_CMD_READ:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					 "rtl88_hal_pwrseqcmdparsing(): PWR_CMD_READ\n");
+				break;
+			case PWR_CMD_WRITE: {
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					 "rtl88_hal_pwrseqcmdparsing(): PWR_CMD_WRITE\n");
+				offset = GET_PWR_CFG_OFFSET(cmd);
+
+					/*Read the val from system register*/
+					val = rtl_read_byte(rtlpriv, offset);
+					val &= (~(GET_PWR_CFG_MASK(cmd)));
+					val |= (GET_PWR_CFG_VALUE(cmd) &
+						GET_PWR_CFG_MASK(cmd));
+
+					/*Write the val back to sytem register*/
+					rtl_write_byte(rtlpriv, offset, val);
+				}
+				break;
+			case PWR_CMD_POLLING:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					 "rtl88_hal_pwrseqcmdparsing(): PWR_CMD_POLLING\n");
+				polling_bit = false;
+				offset = GET_PWR_CFG_OFFSET(cmd);
+
+				do {
+					val = rtl_read_byte(rtlpriv, offset);
+
+					val = val & GET_PWR_CFG_MASK(cmd);
+					if (val == (GET_PWR_CFG_VALUE(cmd) &
+						    GET_PWR_CFG_MASK(cmd)))
+						polling_bit = true;
+					else
+						udelay(10);
+
+					if (polling_count++ > max_polling_cnt) {
+						RT_TRACE(rtlpriv, COMP_INIT,
+							 DBG_LOUD,
+							 "polling fail in pwrseqcmd\n");
+						return false;
+					}
+				} while (!polling_bit);
+
+				break;
+			case PWR_CMD_DELAY:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					 "rtl88_hal_pwrseqcmdparsing(): PWR_CMD_DELAY\n");
+				if (GET_PWR_CFG_VALUE(cmd) == PWRSEQ_DELAY_US)
+					udelay(GET_PWR_CFG_OFFSET(cmd));
+				else
+					mdelay(GET_PWR_CFG_OFFSET(cmd));
+				break;
+			case PWR_CMD_END:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					 "rtl88_hal_pwrseqcmdparsing(): PWR_CMD_END\n");
+				return true;
+				break;
+			default:
+				RT_ASSERT(false,
+					  "rtl88_hal_pwrseqcmdparsing(): Unknown CMD!!\n");
+				break;
+			}
+		}
+
+		ary_idx++;
+	} while (1);
+
+	return true;
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseqcmd.h b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseqcmd.h
new file mode 100644
index 000000000000..d9ae280bb1a2
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseqcmd.h
@@ -0,0 +1,97 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8723E_PWRSEQCMD_H__
+#define __RTL8723E_PWRSEQCMD_H__
+
+#include "../wifi.h"
+/*---------------------------------------------*/
+/* The value of cmd: 4 bits */
+/*---------------------------------------------*/
+#define PWR_CMD_READ		0x00
+#define PWR_CMD_WRITE		0x01
+#define PWR_CMD_POLLING		0x02
+#define PWR_CMD_DELAY		0x03
+#define PWR_CMD_END		0x04
+
+/* define the base address of each block */
+#define PWR_BASEADDR_MAC	0x00
+#define PWR_BASEADDR_USB	0x01
+#define PWR_BASEADDR_PCIE	0x02
+#define PWR_BASEADDR_SDIO	0x03
+
+#define PWR_INTF_SDIO_MSK	BIT(0)
+#define PWR_INTF_USB_MSK	BIT(1)
+#define PWR_INTF_PCI_MSK	BIT(2)
+#define PWR_INTF_ALL_MSK	(BIT(0)|BIT(1)|BIT(2)|BIT(3))
+
+#define	PWR_FAB_TSMC_MSK	BIT(0)
+#define	PWR_FAB_UMC_MSK		BIT(1)
+#define	PWR_FAB_ALL_MSK		(BIT(0)|BIT(1)|BIT(2)|BIT(3))
+
+#define	PWR_CUT_TESTCHIP_MSK	BIT(0)
+#define	PWR_CUT_A_MSK		BIT(1)
+#define	PWR_CUT_B_MSK		BIT(2)
+#define	PWR_CUT_C_MSK		BIT(3)
+#define	PWR_CUT_D_MSK		BIT(4)
+#define	PWR_CUT_E_MSK		BIT(5)
+#define	PWR_CUT_F_MSK		BIT(6)
+#define	PWR_CUT_G_MSK		BIT(7)
+#define	PWR_CUT_ALL_MSK		0xFF
+
+enum pwrseq_delay_unit {
+	PWRSEQ_DELAY_US,
+	PWRSEQ_DELAY_MS,
+};
+
+struct wlan_pwr_cfg {
+	u16 offset;
+	u8 cut_msk;
+	u8 fab_msk:4;
+	u8 interface_msk:4;
+	u8 base:4;
+	u8 cmd:4;
+	u8 msk;
+	u8 value;
+};
+
+#define	GET_PWR_CFG_OFFSET(__PWR)	(__PWR.offset)
+#define	GET_PWR_CFG_CUT_MASK(__PWR)	(__PWR.cut_msk)
+#define	GET_PWR_CFG_FAB_MASK(__PWR)	(__PWR.fab_msk)
+#define	GET_PWR_CFG_INTF_MASK(__PWR)	(__PWR.interface_msk)
+#define	GET_PWR_CFG_BASE(__PWR)		(__PWR.base)
+#define	GET_PWR_CFG_CMD(__PWR)		(__PWR.cmd)
+#define	GET_PWR_CFG_MASK(__PWR)		(__PWR.msk)
+#define	GET_PWR_CFG_VALUE(__PWR)	(__PWR.value)
+
+bool rtl88_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
+				u8 fab_version, u8 interface_type,
+				struct wlan_pwr_cfg pwrcfgcmd[]);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/reg.h b/drivers/net/wireless/rtlwifi/rtl8188ee/reg.h
new file mode 100644
index 000000000000..d849abf7d94a
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/reg.h
@@ -0,0 +1,2258 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92C_REG_H__
+#define __RTL92C_REG_H__
+
+#define TXPKT_BUF_SELECT			0x69
+#define RXPKT_BUF_SELECT			0xA5
+#define DISABLE_TRXPKT_BUF_ACCESS		0x0
+
+#define REG_SYS_ISO_CTRL			0x0000
+#define REG_SYS_FUNC_EN				0x0002
+#define REG_APS_FSMCO				0x0004
+#define REG_SYS_CLKR				0x0008
+#define REG_9346CR				0x000A
+#define REG_EE_VPD				0x000C
+#define REG_AFE_MISC				0x0010
+#define REG_SPS0_CTRL				0x0011
+#define REG_SPS_OCP_CFG				0x0018
+#define REG_RSV_CTRL				0x001C
+#define REG_RF_CTRL				0x001F
+#define REG_LDOA15_CTRL				0x0020
+#define REG_LDOV12D_CTRL			0x0021
+#define REG_LDOHCI12_CTRL			0x0022
+#define REG_LPLDO_CTRL				0x0023
+#define REG_AFE_XTAL_CTRL			0x0024
+#define REG_AFE_LDO_CTRL			0x0027 /* 1.5v for 8188EE test
+							* chip, 1.4v for MP chip
+							*/
+#define REG_AFE_PLL_CTRL			0x0028
+#define REG_EFUSE_CTRL				0x0030
+#define REG_EFUSE_TEST				0x0034
+#define REG_PWR_DATA				0x0038
+#define REG_CAL_TIMER				0x003C
+#define REG_ACLK_MON				0x003E
+#define REG_GPIO_MUXCFG				0x0040
+#define REG_GPIO_IO_SEL				0x0042
+#define REG_MAC_PINMUX_CFG			0x0043
+#define REG_GPIO_PIN_CTRL			0x0044
+#define REG_GPIO_INTM				0x0048
+#define REG_LEDCFG0				0x004C
+#define REG_LEDCFG1				0x004D
+#define REG_LEDCFG2				0x004E
+#define REG_LEDCFG3				0x004F
+#define REG_FSIMR				0x0050
+#define REG_FSISR				0x0054
+#define REG_HSIMR				0x0058
+#define REG_HSISR				0x005c
+#define REG_GPIO_PIN_CTRL_2			0x0060
+#define REG_GPIO_IO_SEL_2			0x0062
+#define REG_GPIO_OUTPUT				0x006c
+#define REG_AFE_XTAL_CTRL_EXT			0x0078
+#define REG_XCK_OUT_CTRL			0x007c
+#define REG_MCUFWDL				0x0080
+#define REG_WOL_EVENT				0x0081
+#define REG_MCUTSTCFG				0x0084
+
+
+#define REG_HIMR				0x00B0
+#define REG_HISR				0x00B4
+#define REG_HIMRE				0x00B8
+#define REG_HISRE				0x00BC
+
+#define REG_EFUSE_ACCESS			0x00CF
+
+#define REG_BIST_SCAN				0x00D0
+#define REG_BIST_RPT				0x00D4
+#define REG_BIST_ROM_RPT			0x00D8
+#define REG_USB_SIE_INTF			0x00E0
+#define REG_PCIE_MIO_INTF			0x00E4
+#define REG_PCIE_MIO_INTD			0x00E8
+#define REG_HPON_FSM				0x00EC
+#define REG_SYS_CFG				0x00F0
+
+#define REG_CR					0x0100
+#define REG_PBP					0x0104
+#define REG_PKT_BUFF_ACCESS_CTRL		0x0106
+#define REG_TRXDMA_CTRL				0x010C
+#define REG_TRXFF_BNDY				0x0114
+#define REG_TRXFF_STATUS			0x0118
+#define REG_RXFF_PTR				0x011C
+
+#define REG_CPWM				0x012F
+#define REG_FWIMR				0x0130
+#define REG_FWISR				0x0134
+#define REG_PKTBUF_DBG_CTRL			0x0140
+#define REG_PKTBUF_DBG_DATA_L			0x0144
+#define REG_PKTBUF_DBG_DATA_H			0x0148
+#define REG_RXPKTBUF_CTRL			(REG_PKTBUF_DBG_CTRL+2)
+
+#define REG_TC0_CTRL				0x0150
+#define REG_TC1_CTRL				0x0154
+#define REG_TC2_CTRL				0x0158
+#define REG_TC3_CTRL				0x015C
+#define REG_TC4_CTRL				0x0160
+#define REG_TCUNIT_BASE				0x0164
+#define REG_MBIST_START				0x0174
+#define REG_MBIST_DONE				0x0178
+#define REG_MBIST_FAIL				0x017C
+#define REG_32K_CTRL				0x0194
+#define REG_C2HEVT_MSG_NORMAL			0x01A0
+#define REG_C2HEVT_CLEAR			0x01AF
+#define REG_C2HEVT_MSG_TEST			0x01B8
+#define REG_MCUTST_1				0x01c0
+#define REG_FMETHR				0x01C8
+#define REG_HMETFR				0x01CC
+#define REG_HMEBOX_0				0x01D0
+#define REG_HMEBOX_1				0x01D4
+#define REG_HMEBOX_2				0x01D8
+#define REG_HMEBOX_3				0x01DC
+
+#define REG_LLT_INIT				0x01E0
+#define REG_BB_ACCEESS_CTRL			0x01E8
+#define REG_BB_ACCESS_DATA			0x01EC
+
+#define REG_HMEBOX_EXT_0			0x01F0
+#define REG_HMEBOX_EXT_1			0x01F4
+#define REG_HMEBOX_EXT_2			0x01F8
+#define REG_HMEBOX_EXT_3			0x01FC
+
+#define REG_RQPN				0x0200
+#define REG_FIFOPAGE				0x0204
+#define REG_TDECTRL				0x0208
+#define REG_TXDMA_OFFSET_CHK			0x020C
+#define REG_TXDMA_STATUS			0x0210
+#define REG_RQPN_NPQ				0x0214
+
+#define REG_RXDMA_AGG_PG_TH			0x0280
+#define REG_FW_UPD_RDPTR			0x0284 /* FW shall update this
+						* register before FW * write
+						* RXPKT_RELEASE_POLL to 1
+						*/
+#define REG_RXDMA_CONTROL			0x0286 /* Control the RX DMA.*/
+#define REG_RXPKT_NUM				0x0287 /* The number of packets
+							* in RXPKTBUF.
+							 */
+#define	REG_PCIE_CTRL_REG			0x0300
+#define	REG_INT_MIG				0x0304
+#define	REG_BCNQ_DESA				0x0308
+#define	REG_HQ_DESA				0x0310
+#define	REG_MGQ_DESA				0x0318
+#define	REG_VOQ_DESA				0x0320
+#define	REG_VIQ_DESA				0x0328
+#define	REG_BEQ_DESA				0x0330
+#define	REG_BKQ_DESA				0x0338
+#define	REG_RX_DESA				0x0340
+
+#define	REG_DBI					0x0348
+#define	REG_MDIO				0x0354
+#define	REG_DBG_SEL				0x0360
+#define	REG_PCIE_HRPWM				0x0361
+#define	REG_PCIE_HCPWM				0x0363
+#define	REG_UART_CTRL				0x0364
+#define	REG_WATCH_DOG				0x0368
+#define	REG_UART_TX_DESA			0x0370
+#define	REG_UART_RX_DESA			0x0378
+
+
+#define	REG_HDAQ_DESA_NODEF			0x0000
+#define	REG_CMDQ_DESA_NODEF			0x0000
+
+#define REG_VOQ_INFORMATION			0x0400
+#define REG_VIQ_INFORMATION			0x0404
+#define REG_BEQ_INFORMATION			0x0408
+#define REG_BKQ_INFORMATION			0x040C
+#define REG_MGQ_INFORMATION			0x0410
+#define REG_HGQ_INFORMATION			0x0414
+#define REG_BCNQ_INFORMATION			0x0418
+#define REG_TXPKT_EMPTY				0x041A
+
+
+#define REG_CPU_MGQ_INFORMATION			0x041C
+#define REG_FWHW_TXQ_CTRL			0x0420
+#define REG_HWSEQ_CTRL				0x0423
+#define REG_TXPKTBUF_BCNQ_BDNY			0x0424
+#define REG_TXPKTBUF_MGQ_BDNY			0x0425
+#define REG_MULTI_BCNQ_EN			0x0426
+#define REG_MULTI_BCNQ_OFFSET			0x0427
+#define REG_SPEC_SIFS				0x0428
+#define REG_RL					0x042A
+#define REG_DARFRC				0x0430
+#define REG_RARFRC				0x0438
+#define REG_RRSR				0x0440
+#define REG_ARFR0				0x0444
+#define REG_ARFR1				0x0448
+#define REG_ARFR2				0x044C
+#define REG_ARFR3				0x0450
+#define REG_AGGLEN_LMT				0x0458
+#define REG_AMPDU_MIN_SPACE			0x045C
+#define REG_TXPKTBUF_WMAC_LBK_BF_HD		0x045D
+#define REG_FAST_EDCA_CTRL			0x0460
+#define REG_RD_RESP_PKT_TH			0x0463
+#define REG_INIRTS_RATE_SEL			0x0480
+#define REG_INIDATA_RATE_SEL			0x0484
+#define REG_POWER_STATUS			0x04A4
+#define REG_POWER_STAGE1			0x04B4
+#define REG_POWER_STAGE2			0x04B8
+#define REG_PKT_LIFE_TIME			0x04C0
+#define REG_STBC_SETTING			0x04C4
+#define REG_PROT_MODE_CTRL			0x04C8
+#define REG_BAR_MODE_CTRL			0x04CC
+#define REG_RA_TRY_RATE_AGG_LMT			0x04CF
+#define REG_EARLY_MODE_CONTROL			0x04D0
+#define REG_NQOS_SEQ				0x04DC
+#define REG_QOS_SEQ				0x04DE
+#define REG_NEED_CPU_HANDLE			0x04E0
+#define REG_PKT_LOSE_RPT			0x04E1
+#define REG_PTCL_ERR_STATUS			0x04E2
+#define REG_TX_RPT_CTRL				0x04EC
+#define REG_TX_RPT_TIME				0x04F0
+#define REG_DUMMY				0x04FC
+
+#define REG_EDCA_VO_PARAM			0x0500
+#define REG_EDCA_VI_PARAM			0x0504
+#define REG_EDCA_BE_PARAM			0x0508
+#define REG_EDCA_BK_PARAM			0x050C
+#define REG_BCNTCFG				0x0510
+#define REG_PIFS				0x0512
+#define REG_RDG_PIFS				0x0513
+#define REG_SIFS_CTX				0x0514
+#define REG_SIFS_TRX				0x0516
+#define REG_AGGR_BREAK_TIME			0x051A
+#define REG_SLOT				0x051B
+#define REG_TX_PTCL_CTRL			0x0520
+#define REG_TXPAUSE				0x0522
+#define REG_DIS_TXREQ_CLR			0x0523
+#define REG_RD_CTRL				0x0524
+#define REG_TBTT_PROHIBIT			0x0540
+#define REG_RD_NAV_NXT				0x0544
+#define REG_NAV_PROT_LEN			0x0546
+#define REG_BCN_CTRL				0x0550
+#define REG_USTIME_TSF				0x0551
+#define REG_MBID_NUM				0x0552
+#define REG_DUAL_TSF_RST			0x0553
+#define REG_BCN_INTERVAL			0x0554
+#define REG_MBSSID_BCN_SPACE			0x0554
+#define REG_DRVERLYINT				0x0558
+#define REG_BCNDMATIM				0x0559
+#define REG_ATIMWND				0x055A
+#define REG_BCN_MAX_ERR				0x055D
+#define REG_RXTSF_OFFSET_CCK			0x055E
+#define REG_RXTSF_OFFSET_OFDM			0x055F
+#define REG_TSFTR				0x0560
+#define REG_INIT_TSFTR				0x0564
+#define REG_PSTIMER				0x0580
+#define REG_TIMER0				0x0584
+#define REG_TIMER1				0x0588
+#define REG_ACMHWCTRL				0x05C0
+#define REG_ACMRSTCTRL				0x05C1
+#define REG_ACMAVG				0x05C2
+#define REG_VO_ADMTIME				0x05C4
+#define REG_VI_ADMTIME				0x05C6
+#define REG_BE_ADMTIME				0x05C8
+#define REG_EDCA_RANDOM_GEN			0x05CC
+#define REG_SCH_TXCMD				0x05D0
+
+#define REG_APSD_CTRL				0x0600
+#define REG_BWOPMODE				0x0603
+#define REG_TCR					0x0604
+#define REG_RCR					0x0608
+#define REG_RX_PKT_LIMIT			0x060C
+#define REG_RX_DLK_TIME				0x060D
+#define REG_RX_DRVINFO_SZ			0x060F
+
+#define REG_MACID				0x0610
+#define REG_BSSID				0x0618
+#define REG_MAR					0x0620
+#define REG_MBIDCAMCFG				0x0628
+
+#define REG_USTIME_EDCA				0x0638
+#define REG_MAC_SPEC_SIFS			0x063A
+#define REG_RESP_SIFS_CCK			0x063C
+#define REG_RESP_SIFS_OFDM			0x063E
+#define REG_ACKTO				0x0640
+#define REG_CTS2TO				0x0641
+#define REG_EIFS				0x0642
+
+#define REG_NAV_CTRL				0x0650
+#define REG_BACAMCMD				0x0654
+#define REG_BACAMCONTENT			0x0658
+#define REG_LBDLY				0x0660
+#define REG_FWDLY				0x0661
+#define REG_RXERR_RPT				0x0664
+#define REG_TRXPTCL_CTL				0x0668
+
+#define REG_CAMCMD				0x0670
+#define REG_CAMWRITE				0x0674
+#define REG_CAMREAD				0x0678
+#define REG_CAMDBG				0x067C
+#define REG_SECCFG				0x0680
+
+#define REG_WOW_CTRL				0x0690
+#define REG_PSSTATUS				0x0691
+#define REG_PS_RX_INFO				0x0692
+#define REG_UAPSD_TID				0x0693
+#define REG_LPNAV_CTRL				0x0694
+#define REG_WKFMCAM_NUM				0x0698
+#define REG_WKFMCAM_RWD				0x069C
+#define REG_RXFLTMAP0				0x06A0
+#define REG_RXFLTMAP1				0x06A2
+#define REG_RXFLTMAP2				0x06A4
+#define REG_BCN_PSR_RPT				0x06A8
+#define REG_CALB32K_CTRL			0x06AC
+#define REG_PKT_MON_CTRL			0x06B4
+#define REG_BT_COEX_TABLE			0x06C0
+#define REG_WMAC_RESP_TXINFO			0x06D8
+
+#define REG_USB_INFO				0xFE17
+#define REG_USB_SPECIAL_OPTION			0xFE55
+#define REG_USB_DMA_AGG_TO			0xFE5B
+#define REG_USB_AGG_TO				0xFE5C
+#define REG_USB_AGG_TH				0xFE5D
+
+#define REG_TEST_USB_TXQS			0xFE48
+#define REG_TEST_SIE_VID			0xFE60
+#define REG_TEST_SIE_PID			0xFE62
+#define REG_TEST_SIE_OPTIONAL			0xFE64
+#define REG_TEST_SIE_CHIRP_K			0xFE65
+#define REG_TEST_SIE_PHY			0xFE66
+#define REG_TEST_SIE_MAC_ADDR			0xFE70
+#define REG_TEST_SIE_STRING			0xFE80
+
+#define REG_NORMAL_SIE_VID			0xFE60
+#define REG_NORMAL_SIE_PID			0xFE62
+#define REG_NORMAL_SIE_OPTIONAL			0xFE64
+#define REG_NORMAL_SIE_EP			0xFE65
+#define REG_NORMAL_SIE_PHY			0xFE68
+#define REG_NORMAL_SIE_MAC_ADDR			0xFE70
+#define REG_NORMAL_SIE_STRING			0xFE80
+
+#define	CR9346					REG_9346CR
+#define	MSR					(REG_CR + 2)
+#define	ISR					REG_HISR
+#define	TSFR					REG_TSFTR
+
+#define	MACIDR0					REG_MACID
+#define	MACIDR4					(REG_MACID + 4)
+
+#define PBP					REG_PBP
+
+#define	IDR0					MACIDR0
+#define	IDR4					MACIDR4
+
+#define	UNUSED_REGISTER				0x1BF
+#define	DCAM					UNUSED_REGISTER
+#define	PSR					UNUSED_REGISTER
+#define BBADDR					UNUSED_REGISTER
+#define	PHYDATAR				UNUSED_REGISTER
+
+#define	INVALID_BBRF_VALUE			0x12345678
+
+#define	MAX_MSS_DENSITY_2T			0x13
+#define	MAX_MSS_DENSITY_1T			0x0A
+
+#define	CMDEEPROM_EN				BIT(5)
+#define	CMDEEPROM_SEL				BIT(4)
+#define	CMD9346CR_9356SEL			BIT(4)
+#define	AUTOLOAD_EEPROM				(CMDEEPROM_EN|CMDEEPROM_SEL)
+#define	AUTOLOAD_EFUSE				CMDEEPROM_EN
+
+#define	GPIOSEL_GPIO				0
+#define	GPIOSEL_ENBT				BIT(5)
+
+#define	GPIO_IN					REG_GPIO_PIN_CTRL
+#define	GPIO_OUT				(REG_GPIO_PIN_CTRL+1)
+#define	GPIO_IO_SEL				(REG_GPIO_PIN_CTRL+2)
+#define	GPIO_MOD				(REG_GPIO_PIN_CTRL+3)
+
+/* 8723/8188E Host System Interrupt Mask Register (offset 0x58, 32 byte) */
+#define	HSIMR_GPIO12_0_INT_EN			BIT(0)
+#define	HSIMR_SPS_OCP_INT_EN			BIT(5)
+#define	HSIMR_RON_INT_EN			BIT(6)
+#define	HSIMR_PDN_INT_EN			BIT(7)
+#define	HSIMR_GPIO9_INT_EN			BIT(25)
+
+
+/* 8723/8188E Host System Interrupt Status Register (offset 0x5C, 32 byte) */
+#define	HSISR_GPIO12_0_INT			BIT(0)
+#define	HSISR_SPS_OCP_INT			BIT(5)
+#define	HSISR_RON_INT_EN			BIT(6)
+#define	HSISR_PDNINT				BIT(7)
+#define	HSISR_GPIO9_INT				BIT(25)
+
+#define	MSR_NOLINK				0x00
+#define	MSR_ADHOC				0x01
+#define	MSR_INFRA				0x02
+#define	MSR_AP					0x03
+
+#define	RRSR_RSC_OFFSET				21
+#define	RRSR_SHORT_OFFSET			23
+#define	RRSR_RSC_BW_40M				0x600000
+#define	RRSR_RSC_UPSUBCHNL			0x400000
+#define	RRSR_RSC_LOWSUBCHNL			0x200000
+#define	RRSR_SHORT				0x800000
+#define	RRSR_1M					BIT(0)
+#define	RRSR_2M					BIT(1)
+#define	RRSR_5_5M				BIT(2)
+#define	RRSR_11M				BIT(3)
+#define	RRSR_6M					BIT(4)
+#define	RRSR_9M					BIT(5)
+#define	RRSR_12M				BIT(6)
+#define	RRSR_18M				BIT(7)
+#define	RRSR_24M				BIT(8)
+#define	RRSR_36M				BIT(9)
+#define	RRSR_48M				BIT(10)
+#define	RRSR_54M				BIT(11)
+#define	RRSR_MCS0				BIT(12)
+#define	RRSR_MCS1				BIT(13)
+#define	RRSR_MCS2				BIT(14)
+#define	RRSR_MCS3				BIT(15)
+#define	RRSR_MCS4				BIT(16)
+#define	RRSR_MCS5				BIT(17)
+#define	RRSR_MCS6				BIT(18)
+#define	RRSR_MCS7				BIT(19)
+#define	BRSR_ACKSHORTPMB			BIT(23)
+
+#define	RATR_1M					0x00000001
+#define	RATR_2M					0x00000002
+#define	RATR_55M				0x00000004
+#define	RATR_11M				0x00000008
+#define	RATR_6M					0x00000010
+#define	RATR_9M					0x00000020
+#define	RATR_12M				0x00000040
+#define	RATR_18M				0x00000080
+#define	RATR_24M				0x00000100
+#define	RATR_36M				0x00000200
+#define	RATR_48M				0x00000400
+#define	RATR_54M				0x00000800
+#define	RATR_MCS0				0x00001000
+#define	RATR_MCS1				0x00002000
+#define	RATR_MCS2				0x00004000
+#define	RATR_MCS3				0x00008000
+#define	RATR_MCS4				0x00010000
+#define	RATR_MCS5				0x00020000
+#define	RATR_MCS6				0x00040000
+#define	RATR_MCS7				0x00080000
+#define	RATR_MCS8				0x00100000
+#define	RATR_MCS9				0x00200000
+#define	RATR_MCS10				0x00400000
+#define	RATR_MCS11				0x00800000
+#define	RATR_MCS12				0x01000000
+#define	RATR_MCS13				0x02000000
+#define	RATR_MCS14				0x04000000
+#define	RATR_MCS15				0x08000000
+
+#define RATE_1M					BIT(0)
+#define RATE_2M					BIT(1)
+#define RATE_5_5M				BIT(2)
+#define RATE_11M				BIT(3)
+#define RATE_6M					BIT(4)
+#define RATE_9M					BIT(5)
+#define RATE_12M				BIT(6)
+#define RATE_18M				BIT(7)
+#define RATE_24M				BIT(8)
+#define RATE_36M				BIT(9)
+#define RATE_48M				BIT(10)
+#define RATE_54M				BIT(11)
+#define RATE_MCS0				BIT(12)
+#define RATE_MCS1				BIT(13)
+#define RATE_MCS2				BIT(14)
+#define RATE_MCS3				BIT(15)
+#define RATE_MCS4				BIT(16)
+#define RATE_MCS5				BIT(17)
+#define RATE_MCS6				BIT(18)
+#define RATE_MCS7				BIT(19)
+#define RATE_MCS8				BIT(20)
+#define RATE_MCS9				BIT(21)
+#define RATE_MCS10				BIT(22)
+#define RATE_MCS11				BIT(23)
+#define RATE_MCS12				BIT(24)
+#define RATE_MCS13				BIT(25)
+#define RATE_MCS14				BIT(26)
+#define RATE_MCS15				BIT(27)
+
+#define	RATE_ALL_CCK		(RATR_1M | RATR_2M | RATR_55M | RATR_11M)
+#define	RATE_ALL_OFDM_AG	(RATR_6M | RATR_9M | RATR_12M | RATR_18M | \
+				RATR_24M | RATR_36M | RATR_48M | RATR_54M)
+#define	RATE_ALL_OFDM_1SS	(RATR_MCS0 | RATR_MCS1 | RATR_MCS2 | \
+				RATR_MCS3 | RATR_MCS4 | RATR_MCS5 | \
+				RATR_MCS6 | RATR_MCS7)
+#define	RATE_ALL_OFDM_2SS	(RATR_MCS8 | RATR_MCS9 | RATR_MCS10 | \
+				RATR_MCS11 | RATR_MCS12 | RATR_MCS13 | \
+				RATR_MCS14 | RATR_MCS15)
+
+#define	BW_OPMODE_20MHZ				BIT(2)
+#define	BW_OPMODE_5G				BIT(1)
+#define	BW_OPMODE_11J				BIT(0)
+
+#define	CAM_VALID				BIT(15)
+#define	CAM_NOTVALID				0x0000
+#define	CAM_USEDK				BIT(5)
+
+#define	CAM_NONE				0x0
+#define	CAM_WEP40				0x01
+#define	CAM_TKIP				0x02
+#define	CAM_AES					0x04
+#define	CAM_WEP104				0x05
+
+#define	TOTAL_CAM_ENTRY				32
+#define	HALF_CAM_ENTRY				16
+
+#define	CAM_WRITE				BIT(16)
+#define	CAM_READ				0x00000000
+#define	CAM_POLLINIG				BIT(31)
+
+#define	SCR_USEDK				0x01
+#define	SCR_TXSEC_ENABLE			0x02
+#define	SCR_RXSEC_ENABLE			0x04
+
+#define	WOW_PMEN				BIT(0)
+#define	WOW_WOMEN				BIT(1)
+#define	WOW_MAGIC				BIT(2)
+#define	WOW_UWF					BIT(3)
+
+/*********************************************
+*       8188 IMR/ISR bits
+**********************************************/
+#define	IMR_DISABLED				0x0
+/* IMR DW0(0x0060-0063) Bit 0-31 */
+#define	IMR_TXCCK		BIT(30) /* TXRPT interrupt when CCX bit of
+					 * the packet is set
+					 */
+#define	IMR_PSTIMEOUT		BIT(29)	/* Power Save Time Out Interrupt */
+#define	IMR_GTINT4		BIT(28)	/* When GTIMER4 expires,
+					 * this bit is set to 1
+					 */
+#define	IMR_GTINT3		BIT(27)	/* When GTIMER3 expires,
+					 * this bit is set to 1
+					 */
+#define	IMR_TBDER		BIT(26)	/* Transmit Beacon0 Error */
+#define	IMR_TBDOK		BIT(25)	/* Transmit Beacon0 OK	*/
+#define	IMR_TSF_BIT32_TOGGLE	BIT(24)	/* TSF Timer BIT32 toggle ind int */
+#define	IMR_BCNDMAINT0		BIT(20)	/* Beacon DMA Interrupt 0 */
+#define	IMR_BCNDOK0		BIT(16)	/* Beacon Queue DMA OK0	*/
+#define	IMR_HSISR_IND_ON_INT	BIT(15)	/* HSISR Indicator (HSIMR & HSISR is
+					 * true, this bit is set to 1)
+					 */
+#define	IMR_BCNDMAINT_E		BIT(14)	/* Beacon DMA Int Extension for Win7 */
+#define	IMR_ATIMEND		BIT(12)	/* CTWidnow End or ATIM Window End */
+#define	IMR_HISR1_IND_INT	BIT(11)	/* HISR1 Indicator (HISR1 & HIMR1 is
+					 * true, this bit is set to 1)
+					 */
+#define	IMR_C2HCMD		BIT(10)	/* CPU to Host Command INT Status,
+					 * Write 1 clear
+					 */
+#define	IMR_CPWM2		BIT(9)	/* CPU power Mode exchange INT Status,
+					 * Write 1 clear
+					 */
+#define	IMR_CPWM		BIT(8)	/* CPU power Mode exchange INT Status,
+					 * Write 1 clear
+					 */
+#define	IMR_HIGHDOK		BIT(7)	/* High Queue DMA OK	*/
+#define	IMR_MGNTDOK		BIT(6)	/* Management Queue DMA OK */
+#define	IMR_BKDOK		BIT(5)	/* AC_BK DMA OK		*/
+#define	IMR_BEDOK		BIT(4)	/* AC_BE DMA OK	*/
+#define	IMR_VIDOK		BIT(3)	/* AC_VI DMA OK	*/
+#define	IMR_VODOK		BIT(2)	/* AC_VO DMA OK	*/
+#define	IMR_RDU			BIT(1)	/* Rx Descriptor Unavailable */
+#define	IMR_ROK			BIT(0)	/* Receive DMA OK */
+
+/* IMR DW1(0x00B4-00B7) Bit 0-31 */
+#define	IMR_BCNDMAINT7		BIT(27)	/* Beacon DMA Interrupt 7 */
+#define	IMR_BCNDMAINT6		BIT(26)	/* Beacon DMA Interrupt 6 */
+#define	IMR_BCNDMAINT5		BIT(25)	/* Beacon DMA Interrupt 5 */
+#define	IMR_BCNDMAINT4		BIT(24)	/* Beacon DMA Interrupt 4 */
+#define	IMR_BCNDMAINT3		BIT(23)	/* Beacon DMA Interrupt 3 */
+#define	IMR_BCNDMAINT2		BIT(22)	/* Beacon DMA Interrupt 2 */
+#define	IMR_BCNDMAINT1		BIT(21)	/* Beacon DMA Interrupt 1 */
+#define	IMR_BCNDOK7		BIT(20)	/* Beacon Queue DMA OK Interrup 7 */
+#define	IMR_BCNDOK6		BIT(19)	/* Beacon Queue DMA OK Interrup 6 */
+#define	IMR_BCNDOK5		BIT(18)	/* Beacon Queue DMA OK Interrup 5 */
+#define	IMR_BCNDOK4		BIT(17)	/* Beacon Queue DMA OK Interrup 4 */
+#define	IMR_BCNDOK3		BIT(16)	/* Beacon Queue DMA OK Interrup 3 */
+#define	IMR_BCNDOK2		BIT(15)	/* Beacon Queue DMA OK Interrup 2 */
+#define	IMR_BCNDOK1		BIT(14)	/* Beacon Queue DMA OK Interrup 1 */
+#define	IMR_ATIMEND_E		BIT(13)	/* ATIM Window End Extension for Win7 */
+#define	IMR_TXERR		BIT(11)	/* Tx Err Flag Int Status,
+					 * write 1 clear.
+					 */
+#define	IMR_RXERR		BIT(10)	/* Rx Err Flag INT Status,
+					 * Write 1 clear
+					 */
+#define	IMR_TXFOVW		BIT(9)	/* Transmit FIFO Overflow */
+#define	IMR_RXFOVW		BIT(8)	/* Receive FIFO Overflow */
+
+
+#define	HWSET_MAX_SIZE				512
+#define   EFUSE_MAX_SECTION			64
+#define   EFUSE_REAL_CONTENT_LEN		256
+#define	EFUSE_OOB_PROTECT_BYTES			18 /* PG data exclude header,
+						    * dummy 7 bytes frome CP
+						    * test and reserved 1byte.
+						    */
+
+#define	EEPROM_DEFAULT_TSSI			0x0
+#define EEPROM_DEFAULT_TXPOWERDIFF		0x0
+#define EEPROM_DEFAULT_CRYSTALCAP		0x5
+#define EEPROM_DEFAULT_BOARDTYPE		0x02
+#define EEPROM_DEFAULT_TXPOWER			0x1010
+#define	EEPROM_DEFAULT_HT2T_TXPWR		0x10
+
+#define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
+#define	EEPROM_DEFAULT_THERMALMETER		0x18
+#define	EEPROM_DEFAULT_ANTTXPOWERDIFF		0x0
+#define	EEPROM_DEFAULT_TXPWDIFF_CRYSTALCAP	0x5
+#define	EEPROM_DEFAULT_TXPOWERLEVEL		0x22
+#define	EEPROM_DEFAULT_HT40_2SDIFF		0x0
+#define EEPROM_DEFAULT_HT20_DIFF		2
+#define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
+#define EEPROM_DEFAULT_HT40_PWRMAXOFFSET	0
+#define EEPROM_DEFAULT_HT20_PWRMAXOFFSET	0
+
+#define RF_OPTION1				0x79
+#define RF_OPTION2				0x7A
+#define RF_OPTION3				0x7B
+#define RF_OPTION4				0x7C
+
+#define EEPROM_DEFAULT_PID			0x1234
+#define EEPROM_DEFAULT_VID			0x5678
+#define EEPROM_DEFAULT_CUSTOMERID		0xAB
+#define EEPROM_DEFAULT_SUBCUSTOMERID		0xCD
+#define EEPROM_DEFAULT_VERSION			0
+
+#define	EEPROM_CHANNEL_PLAN_FCC			0x0
+#define	EEPROM_CHANNEL_PLAN_IC			0x1
+#define	EEPROM_CHANNEL_PLAN_ETSI		0x2
+#define	EEPROM_CHANNEL_PLAN_SPAIN		0x3
+#define	EEPROM_CHANNEL_PLAN_FRANCE		0x4
+#define	EEPROM_CHANNEL_PLAN_MKK			0x5
+#define	EEPROM_CHANNEL_PLAN_MKK1		0x6
+#define	EEPROM_CHANNEL_PLAN_ISRAEL		0x7
+#define	EEPROM_CHANNEL_PLAN_TELEC		0x8
+#define	EEPROM_CHANNEL_PLAN_GLOBAL_DOMAIN	0x9
+#define	EEPROM_CHANNEL_PLAN_WORLD_WIDE_13	0xA
+#define	EEPROM_CHANNEL_PLAN_NCC			0xB
+#define	EEPROM_CHANNEL_PLAN_BY_HW_MASK		0x80
+
+#define EEPROM_CID_DEFAULT			0x0
+#define EEPROM_CID_TOSHIBA			0x4
+#define	EEPROM_CID_CCX				0x10
+#define	EEPROM_CID_QMI				0x0D
+#define EEPROM_CID_WHQL				0xFE
+
+#define	RTL8188E_EEPROM_ID			0x8129
+
+#define EEPROM_HPON				0x02
+#define EEPROM_CLK				0x06
+#define EEPROM_TESTR				0x08
+
+#define EEPROM_TXPOWERCCK			0x10
+#define	EEPROM_TXPOWERHT40_1S			0x16
+#define EEPROM_TXPOWERHT20DIFF			0x1B
+#define EEPROM_TXPOWER_OFDMDIFF			0x1B
+
+#define	EEPROM_TX_PWR_INX			0x10
+
+#define	EEPROM_CHANNELPLAN			0xB8
+#define	EEPROM_XTAL_88E				0xB9
+#define	EEPROM_THERMAL_METER_88E		0xBA
+#define	EEPROM_IQK_LCK_88E			0xBB
+
+#define	EEPROM_RF_BOARD_OPTION_88E		0xC1
+#define	EEPROM_RF_FEATURE_OPTION_88E		0xC2
+#define	EEPROM_RF_BT_SETTING_88E		0xC3
+#define	EEPROM_VERSION				0xC4
+#define	EEPROM_CUSTOMER_ID			0xC5
+#define	EEPROM_RF_ANTENNA_OPT_88E		0xC9
+
+#define	EEPROM_MAC_ADDR				0xD0
+#define EEPROM_VID				0xD6
+#define EEPROM_DID				0xD8
+#define EEPROM_SVID				0xDA
+#define EEPROM_SMID				0xDC
+
+#define	STOPBECON				BIT(6)
+#define	STOPHIGHT				BIT(5)
+#define	STOPMGT					BIT(4)
+#define	STOPVO					BIT(3)
+#define	STOPVI					BIT(2)
+#define	STOPBE					BIT(1)
+#define	STOPBK					BIT(0)
+
+#define	RCR_APPFCS				BIT(31)
+#define	RCR_APP_MIC				BIT(30)
+#define	RCR_APP_ICV				BIT(29)
+#define	RCR_APP_PHYST_RXFF			BIT(28)
+#define	RCR_APP_BA_SSN				BIT(27)
+#define	RCR_ENMBID				BIT(24)
+#define	RCR_LSIGEN				BIT(23)
+#define	RCR_MFBEN				BIT(22)
+#define	RCR_HTC_LOC_CTRL			BIT(14)
+#define	RCR_AMF					BIT(13)
+#define	RCR_ACF					BIT(12)
+#define	RCR_ADF					BIT(11)
+#define	RCR_AICV				BIT(9)
+#define	RCR_ACRC32				BIT(8)
+#define	RCR_CBSSID_BCN				BIT(7)
+#define	RCR_CBSSID_DATA				BIT(6)
+#define	RCR_CBSSID				RCR_CBSSID_DATA
+#define	RCR_APWRMGT				BIT(5)
+#define	RCR_ADD3				BIT(4)
+#define	RCR_AB					BIT(3)
+#define	RCR_AM					BIT(2)
+#define	RCR_APM					BIT(1)
+#define	RCR_AAP					BIT(0)
+#define	RCR_MXDMA_OFFSET			8
+#define	RCR_FIFO_OFFSET				13
+
+#define RSV_CTRL				0x001C
+#define RD_CTRL					0x0524
+
+#define REG_USB_INFO				0xFE17
+#define REG_USB_SPECIAL_OPTION			0xFE55
+#define REG_USB_DMA_AGG_TO			0xFE5B
+#define REG_USB_AGG_TO				0xFE5C
+#define REG_USB_AGG_TH				0xFE5D
+
+#define REG_USB_VID				0xFE60
+#define REG_USB_PID				0xFE62
+#define REG_USB_OPTIONAL			0xFE64
+#define REG_USB_CHIRP_K				0xFE65
+#define REG_USB_PHY				0xFE66
+#define REG_USB_MAC_ADDR			0xFE70
+#define REG_USB_HRPWM				0xFE58
+#define REG_USB_HCPWM				0xFE57
+
+#define SW18_FPWM				BIT(3)
+
+#define ISO_MD2PP				BIT(0)
+#define ISO_UA2USB				BIT(1)
+#define ISO_UD2CORE				BIT(2)
+#define ISO_PA2PCIE				BIT(3)
+#define ISO_PD2CORE				BIT(4)
+#define ISO_IP2MAC				BIT(5)
+#define ISO_DIOP				BIT(6)
+#define ISO_DIOE				BIT(7)
+#define ISO_EB2CORE				BIT(8)
+#define ISO_DIOR				BIT(9)
+
+#define PWC_EV25V				BIT(14)
+#define PWC_EV12V				BIT(15)
+
+#define FEN_BBRSTB				BIT(0)
+#define FEN_BB_GLB_RSTN				BIT(1)
+#define FEN_USBA				BIT(2)
+#define FEN_UPLL				BIT(3)
+#define FEN_USBD				BIT(4)
+#define FEN_DIO_PCIE				BIT(5)
+#define FEN_PCIEA				BIT(6)
+#define FEN_PPLL				BIT(7)
+#define FEN_PCIED				BIT(8)
+#define FEN_DIOE				BIT(9)
+#define FEN_CPUEN				BIT(10)
+#define FEN_DCORE				BIT(11)
+#define FEN_ELDR				BIT(12)
+#define FEN_DIO_RF				BIT(13)
+#define FEN_HWPDN				BIT(14)
+#define FEN_MREGEN				BIT(15)
+
+#define PFM_LDALL				BIT(0)
+#define PFM_ALDN				BIT(1)
+#define PFM_LDKP				BIT(2)
+#define PFM_WOWL				BIT(3)
+#define ENPDN					BIT(4)
+#define PDN_PL					BIT(5)
+#define APFM_ONMAC				BIT(8)
+#define APFM_OFF				BIT(9)
+#define APFM_RSM				BIT(10)
+#define AFSM_HSUS				BIT(11)
+#define AFSM_PCIE				BIT(12)
+#define APDM_MAC				BIT(13)
+#define APDM_HOST				BIT(14)
+#define APDM_HPDN				BIT(15)
+#define RDY_MACON				BIT(16)
+#define SUS_HOST				BIT(17)
+#define ROP_ALD					BIT(20)
+#define ROP_PWR					BIT(21)
+#define ROP_SPS					BIT(22)
+#define SOP_MRST				BIT(25)
+#define SOP_FUSE				BIT(26)
+#define SOP_ABG					BIT(27)
+#define SOP_AMB					BIT(28)
+#define SOP_RCK					BIT(29)
+#define SOP_A8M					BIT(30)
+#define XOP_BTCK				BIT(31)
+
+#define ANAD16V_EN				BIT(0)
+#define ANA8M					BIT(1)
+#define MACSLP					BIT(4)
+#define LOADER_CLK_EN				BIT(5)
+#define _80M_SSC_DIS				BIT(7)
+#define _80M_SSC_EN_HO				BIT(8)
+#define PHY_SSC_RSTB				BIT(9)
+#define SEC_CLK_EN				BIT(10)
+#define MAC_CLK_EN				BIT(11)
+#define SYS_CLK_EN				BIT(12)
+#define RING_CLK_EN				BIT(13)
+
+#define	BOOT_FROM_EEPROM			BIT(4)
+#define	EEPROM_EN				BIT(5)
+
+#define AFE_BGEN				BIT(0)
+#define AFE_MBEN				BIT(1)
+#define MAC_ID_EN				BIT(7)
+
+#define WLOCK_ALL				BIT(0)
+#define WLOCK_00				BIT(1)
+#define WLOCK_04				BIT(2)
+#define WLOCK_08				BIT(3)
+#define WLOCK_40				BIT(4)
+#define R_DIS_PRST_0				BIT(5)
+#define R_DIS_PRST_1				BIT(6)
+#define LOCK_ALL_EN				BIT(7)
+
+#define RF_EN					BIT(0)
+#define RF_RSTB					BIT(1)
+#define RF_SDMRSTB				BIT(2)
+
+#define LDA15_EN				BIT(0)
+#define LDA15_STBY				BIT(1)
+#define LDA15_OBUF				BIT(2)
+#define LDA15_REG_VOS				BIT(3)
+#define _LDA15_VOADJ(x)				(((x) & 0x7) << 4)
+
+#define LDV12_EN				BIT(0)
+#define LDV12_SDBY				BIT(1)
+#define LPLDO_HSM				BIT(2)
+#define LPLDO_LSM_DIS				BIT(3)
+#define _LDV12_VADJ(x)				(((x) & 0xF) << 4)
+
+#define XTAL_EN					BIT(0)
+#define XTAL_BSEL				BIT(1)
+#define _XTAL_BOSC(x)				(((x) & 0x3) << 2)
+#define _XTAL_CADJ(x)				(((x) & 0xF) << 4)
+#define XTAL_GATE_USB				BIT(8)
+#define _XTAL_USB_DRV(x)			(((x) & 0x3) << 9)
+#define XTAL_GATE_AFE				BIT(11)
+#define _XTAL_AFE_DRV(x)			(((x) & 0x3) << 12)
+#define XTAL_RF_GATE				BIT(14)
+#define _XTAL_RF_DRV(x)				(((x) & 0x3) << 15)
+#define XTAL_GATE_DIG				BIT(17)
+#define _XTAL_DIG_DRV(x)			(((x) & 0x3) << 18)
+#define XTAL_BT_GATE				BIT(20)
+#define _XTAL_BT_DRV(x)				(((x) & 0x3) << 21)
+#define _XTAL_GPIO(x)				(((x) & 0x7) << 23)
+
+#define CKDLY_AFE				BIT(26)
+#define CKDLY_USB				BIT(27)
+#define CKDLY_DIG				BIT(28)
+#define CKDLY_BT				BIT(29)
+
+#define APLL_EN					BIT(0)
+#define APLL_320_EN				BIT(1)
+#define APLL_FREF_SEL				BIT(2)
+#define APLL_EDGE_SEL				BIT(3)
+#define APLL_WDOGB				BIT(4)
+#define APLL_LPFEN				BIT(5)
+
+#define APLL_REF_CLK_13MHZ			0x1
+#define APLL_REF_CLK_19_2MHZ			0x2
+#define APLL_REF_CLK_20MHZ			0x3
+#define APLL_REF_CLK_25MHZ			0x4
+#define APLL_REF_CLK_26MHZ			0x5
+#define APLL_REF_CLK_38_4MHZ			0x6
+#define APLL_REF_CLK_40MHZ			0x7
+
+#define APLL_320EN				BIT(14)
+#define APLL_80EN				BIT(15)
+#define APLL_1MEN				BIT(24)
+
+#define ALD_EN					BIT(18)
+#define EF_PD					BIT(19)
+#define EF_FLAG					BIT(31)
+
+#define EF_TRPT					BIT(7)
+#define LDOE25_EN				BIT(31)
+
+#define RSM_EN					BIT(0)
+#define TIMER_EN				BIT(4)
+
+#define TRSW0EN					BIT(2)
+#define TRSW1EN					BIT(3)
+#define EROM_EN					BIT(4)
+#define ENBT					BIT(5)
+#define ENUART					BIT(8)
+#define UART_910				BIT(9)
+#define ENPMAC					BIT(10)
+#define SIC_SWRST				BIT(11)
+#define ENSIC					BIT(12)
+#define SIC_23					BIT(13)
+#define ENHDP					BIT(14)
+#define SIC_LBK					BIT(15)
+
+#define LED0PL					BIT(4)
+#define LED1PL					BIT(12)
+#define LED0DIS					BIT(7)
+
+#define MCUFWDL_EN				BIT(0)
+#define MCUFWDL_RDY				BIT(1)
+#define FWDL_CHKSUM_RPT				BIT(2)
+#define MACINI_RDY				BIT(3)
+#define BBINI_RDY				BIT(4)
+#define RFINI_RDY				BIT(5)
+#define WINTINI_RDY				BIT(6)
+#define CPRST					BIT(23)
+
+#define XCLK_VLD				BIT(0)
+#define ACLK_VLD				BIT(1)
+#define UCLK_VLD				BIT(2)
+#define PCLK_VLD				BIT(3)
+#define PCIRSTB					BIT(4)
+#define V15_VLD					BIT(5)
+#define TRP_B15V_EN				BIT(7)
+#define SIC_IDLE				BIT(8)
+#define BD_MAC2					BIT(9)
+#define BD_MAC1					BIT(10)
+#define IC_MACPHY_MODE				BIT(11)
+#define VENDOR_ID				BIT(19)
+#define PAD_HWPD_IDN				BIT(22)
+#define TRP_VAUX_EN				BIT(23)
+#define TRP_BT_EN				BIT(24)
+#define BD_PKG_SEL				BIT(25)
+#define BD_HCI_SEL				BIT(26)
+#define TYPE_ID					BIT(27)
+
+#define CHIP_VER_RTL_MASK			0xF000
+#define CHIP_VER_RTL_SHIFT			12
+
+#define REG_LBMODE				(REG_CR + 3)
+
+#define HCI_TXDMA_EN				BIT(0)
+#define HCI_RXDMA_EN				BIT(1)
+#define TXDMA_EN				BIT(2)
+#define RXDMA_EN				BIT(3)
+#define PROTOCOL_EN				BIT(4)
+#define SCHEDULE_EN				BIT(5)
+#define MACTXEN					BIT(6)
+#define MACRXEN					BIT(7)
+#define ENSWBCN					BIT(8)
+#define ENSEC					BIT(9)
+
+#define _NETTYPE(x)				(((x) & 0x3) << 16)
+#define MASK_NETTYPE				0x30000
+#define NT_NO_LINK				0x0
+#define NT_LINK_AD_HOC				0x1
+#define NT_LINK_AP				0x2
+#define NT_AS_AP				0x3
+
+#define _LBMODE(x)				(((x) & 0xF) << 24)
+#define MASK_LBMODE				0xF000000
+#define LOOPBACK_NORMAL				0x0
+#define LOOPBACK_IMMEDIATELY			0xB
+#define LOOPBACK_MAC_DELAY			0x3
+#define LOOPBACK_PHY				0x1
+#define LOOPBACK_DMA				0x7
+
+#define GET_RX_PAGE_SIZE(value)		((value) & 0xF)
+#define GET_TX_PAGE_SIZE(value)		(((value) & 0xF0) >> 4)
+#define _PSRX_MASK				0xF
+#define _PSTX_MASK				0xF0
+#define _PSRX(x)				(x)
+#define _PSTX(x)				((x) << 4)
+
+#define PBP_64					0x0
+#define PBP_128					0x1
+#define PBP_256					0x2
+#define PBP_512					0x3
+#define PBP_1024				0x4
+
+#define RXDMA_ARBBW_EN				BIT(0)
+#define RXSHFT_EN				BIT(1)
+#define RXDMA_AGG_EN				BIT(2)
+#define QS_VO_QUEUE				BIT(8)
+#define QS_VI_QUEUE				BIT(9)
+#define QS_BE_QUEUE				BIT(10)
+#define QS_BK_QUEUE				BIT(11)
+#define QS_MANAGER_QUEUE			BIT(12)
+#define QS_HIGH_QUEUE				BIT(13)
+
+#define HQSEL_VOQ				BIT(0)
+#define HQSEL_VIQ				BIT(1)
+#define HQSEL_BEQ				BIT(2)
+#define HQSEL_BKQ				BIT(3)
+#define HQSEL_MGTQ				BIT(4)
+#define HQSEL_HIQ				BIT(5)
+
+#define _TXDMA_HIQ_MAP(x)			(((x)&0x3) << 14)
+#define _TXDMA_MGQ_MAP(x)			(((x)&0x3) << 12)
+#define _TXDMA_BKQ_MAP(x)			(((x)&0x3) << 10)
+#define _TXDMA_BEQ_MAP(x)			(((x)&0x3) << 8)
+#define _TXDMA_VIQ_MAP(x)			(((x)&0x3) << 6)
+#define _TXDMA_VOQ_MAP(x)			(((x)&0x3) << 4)
+
+#define QUEUE_LOW				1
+#define QUEUE_NORMAL				2
+#define QUEUE_HIGH				3
+
+#define _LLT_NO_ACTIVE				0x0
+#define _LLT_WRITE_ACCESS			0x1
+#define _LLT_READ_ACCESS			0x2
+
+#define _LLT_INIT_DATA(x)			((x) & 0xFF)
+#define _LLT_INIT_ADDR(x)			(((x) & 0xFF) << 8)
+#define _LLT_OP(x)				(((x) & 0x3) << 30)
+#define _LLT_OP_VALUE(x)			(((x) >> 30) & 0x3)
+
+#define BB_WRITE_READ_MASK			(BIT(31) | BIT(30))
+#define BB_WRITE_EN				BIT(30)
+#define BB_READ_EN				BIT(31)
+
+#define _HPQ(x)					((x) & 0xFF)
+#define _LPQ(x)					(((x) & 0xFF) << 8)
+#define _PUBQ(x)				(((x) & 0xFF) << 16)
+#define _NPQ(x)					((x) & 0xFF)
+
+#define HPQ_PUBLIC_DIS				BIT(24)
+#define LPQ_PUBLIC_DIS				BIT(25)
+#define LD_RQPN					BIT(31)
+
+#define BCN_VALID				BIT(16)
+#define BCN_HEAD(x)				(((x) & 0xFF) << 8)
+#define	BCN_HEAD_MASK				0xFF00
+
+#define BLK_DESC_NUM_SHIFT			4
+#define BLK_DESC_NUM_MASK			0xF
+
+#define DROP_DATA_EN				BIT(9)
+
+#define EN_AMPDU_RTY_NEW			BIT(7)
+
+#define _INIRTSMCS_SEL(x)			((x) & 0x3F)
+
+#define _SPEC_SIFS_CCK(x)			((x) & 0xFF)
+#define _SPEC_SIFS_OFDM(x)			(((x) & 0xFF) << 8)
+
+#define RATE_REG_BITMAP_ALL			0xFFFFF
+
+#define _RRSC_BITMAP(x)				((x) & 0xFFFFF)
+
+#define _RRSR_RSC(x)				(((x) & 0x3) << 21)
+#define RRSR_RSC_RESERVED			0x0
+#define RRSR_RSC_UPPER_SUBCHANNEL		0x1
+#define RRSR_RSC_LOWER_SUBCHANNEL		0x2
+#define RRSR_RSC_DUPLICATE_MODE			0x3
+
+#define USE_SHORT_G1				BIT(20)
+
+#define _AGGLMT_MCS0(x)				((x) & 0xF)
+#define _AGGLMT_MCS1(x)				(((x) & 0xF) << 4)
+#define _AGGLMT_MCS2(x)				(((x) & 0xF) << 8)
+#define _AGGLMT_MCS3(x)				(((x) & 0xF) << 12)
+#define _AGGLMT_MCS4(x)				(((x) & 0xF) << 16)
+#define _AGGLMT_MCS5(x)				(((x) & 0xF) << 20)
+#define _AGGLMT_MCS6(x)				(((x) & 0xF) << 24)
+#define _AGGLMT_MCS7(x)				(((x) & 0xF) << 28)
+
+#define	RETRY_LIMIT_SHORT_SHIFT			8
+#define	RETRY_LIMIT_LONG_SHIFT			0
+
+#define _DARF_RC1(x)				((x) & 0x1F)
+#define _DARF_RC2(x)				(((x) & 0x1F) << 8)
+#define _DARF_RC3(x)				(((x) & 0x1F) << 16)
+#define _DARF_RC4(x)				(((x) & 0x1F) << 24)
+#define _DARF_RC5(x)				((x) & 0x1F)
+#define _DARF_RC6(x)				(((x) & 0x1F) << 8)
+#define _DARF_RC7(x)				(((x) & 0x1F) << 16)
+#define _DARF_RC8(x)				(((x) & 0x1F) << 24)
+
+#define _RARF_RC1(x)				((x) & 0x1F)
+#define _RARF_RC2(x)				(((x) & 0x1F) << 8)
+#define _RARF_RC3(x)				(((x) & 0x1F) << 16)
+#define _RARF_RC4(x)				(((x) & 0x1F) << 24)
+#define _RARF_RC5(x)				((x) & 0x1F)
+#define _RARF_RC6(x)				(((x) & 0x1F) << 8)
+#define _RARF_RC7(x)				(((x) & 0x1F) << 16)
+#define _RARF_RC8(x)				(((x) & 0x1F) << 24)
+
+#define AC_PARAM_TXOP_LIMIT_OFFSET		16
+#define AC_PARAM_ECW_MAX_OFFSET			12
+#define AC_PARAM_ECW_MIN_OFFSET			8
+#define AC_PARAM_AIFS_OFFSET			0
+
+#define _AIFS(x)				(x)
+#define _ECW_MAX_MIN(x)				((x) << 8)
+#define _TXOP_LIMIT(x)				((x) << 16)
+
+#define _BCNIFS(x)				((x) & 0xFF)
+#define _BCNECW(x)				((((x) & 0xF)) << 8)
+
+#define _LRL(x)					((x) & 0x3F)
+#define _SRL(x)					(((x) & 0x3F) << 8)
+
+#define _SIFS_CCK_CTX(x)			((x) & 0xFF)
+#define _SIFS_CCK_TRX(x)			(((x) & 0xFF) << 8);
+
+#define _SIFS_OFDM_CTX(x)			((x) & 0xFF)
+#define _SIFS_OFDM_TRX(x)			(((x) & 0xFF) << 8);
+
+#define _TBTT_PROHIBIT_HOLD(x)			(((x) & 0xFF) << 8)
+
+#define DIS_EDCA_CNT_DWN			BIT(11)
+
+#define EN_MBSSID				BIT(1)
+#define EN_TXBCN_RPT				BIT(2)
+#define	EN_BCN_FUNCTION				BIT(3)
+
+#define TSFTR_RST				BIT(0)
+#define TSFTR1_RST				BIT(1)
+
+#define STOP_BCNQ				BIT(6)
+
+#define	DIS_TSF_UDT0_NORMAL_CHIP		BIT(4)
+#define	DIS_TSF_UDT0_TEST_CHIP			BIT(5)
+
+#define	ACMHW_HWEN				BIT(0)
+#define	ACMHW_BEQEN				BIT(1)
+#define	ACMHW_VIQEN				BIT(2)
+#define	ACMHW_VOQEN				BIT(3)
+#define	ACMHW_BEQSTATUS				BIT(4)
+#define	ACMHW_VIQSTATUS				BIT(5)
+#define	ACMHW_VOQSTATUS				BIT(6)
+
+#define APSDOFF					BIT(6)
+#define APSDOFF_STATUS				BIT(7)
+
+#define BW_20MHZ				BIT(2)
+
+#define RATE_BITMAP_ALL				0xFFFFF
+
+#define RATE_RRSR_CCK_ONLY_1M			0xFFFF1
+
+#define TSFRST					BIT(0)
+#define DIS_GCLK				BIT(1)
+#define PAD_SEL					BIT(2)
+#define PWR_ST					BIT(6)
+#define PWRBIT_OW_EN				BIT(7)
+#define ACRC					BIT(8)
+#define CFENDFORM				BIT(9)
+#define ICV					BIT(10)
+
+#define AAP					BIT(0)
+#define APM					BIT(1)
+#define AM					BIT(2)
+#define AB					BIT(3)
+#define ADD3					BIT(4)
+#define APWRMGT					BIT(5)
+#define CBSSID					BIT(6)
+#define CBSSID_DATA				BIT(6)
+#define CBSSID_BCN				BIT(7)
+#define ACRC32					BIT(8)
+#define AICV					BIT(9)
+#define ADF					BIT(11)
+#define ACF					BIT(12)
+#define AMF					BIT(13)
+#define HTC_LOC_CTRL				BIT(14)
+#define UC_DATA_EN				BIT(16)
+#define BM_DATA_EN				BIT(17)
+#define MFBEN					BIT(22)
+#define LSIGEN					BIT(23)
+#define ENMBID					BIT(24)
+#define APP_BASSN				BIT(27)
+#define APP_PHYSTS				BIT(28)
+#define APP_ICV					BIT(29)
+#define APP_MIC					BIT(30)
+#define APP_FCS					BIT(31)
+
+#define _MIN_SPACE(x)				((x) & 0x7)
+#define _SHORT_GI_PADDING(x)			(((x) & 0x1F) << 3)
+
+#define RXERR_TYPE_OFDM_PPDU			0
+#define RXERR_TYPE_OFDM_FALSE_ALARM		1
+#define	RXERR_TYPE_OFDM_MPDU_OK			2
+#define RXERR_TYPE_OFDM_MPDU_FAIL		3
+#define RXERR_TYPE_CCK_PPDU			4
+#define RXERR_TYPE_CCK_FALSE_ALARM		5
+#define RXERR_TYPE_CCK_MPDU_OK			6
+#define RXERR_TYPE_CCK_MPDU_FAIL		7
+#define RXERR_TYPE_HT_PPDU			8
+#define RXERR_TYPE_HT_FALSE_ALARM		9
+#define RXERR_TYPE_HT_MPDU_TOTAL		10
+#define RXERR_TYPE_HT_MPDU_OK			11
+#define RXERR_TYPE_HT_MPDU_FAIL			12
+#define RXERR_TYPE_RX_FULL_DROP			15
+
+#define RXERR_COUNTER_MASK			0xFFFFF
+#define RXERR_RPT_RST				BIT(27)
+#define _RXERR_RPT_SEL(type)			((type) << 28)
+
+#define	SCR_TXUSEDK				BIT(0)
+#define	SCR_RXUSEDK				BIT(1)
+#define	SCR_TXENCENABLE				BIT(2)
+#define	SCR_RXDECENABLE				BIT(3)
+#define	SCR_SKBYA2				BIT(4)
+#define	SCR_NOSKMC				BIT(5)
+#define SCR_TXBCUSEDK				BIT(6)
+#define SCR_RXBCUSEDK				BIT(7)
+
+#define USB_IS_HIGH_SPEED			0
+#define USB_IS_FULL_SPEED			1
+#define USB_SPEED_MASK				BIT(5)
+
+#define USB_NORMAL_SIE_EP_MASK			0xF
+#define USB_NORMAL_SIE_EP_SHIFT			4
+
+#define USB_TEST_EP_MASK			0x30
+#define USB_TEST_EP_SHIFT			4
+
+#define USB_AGG_EN				BIT(3)
+
+#define MAC_ADDR_LEN				6
+#define LAST_ENTRY_OF_TX_PKT_BUFFER		175/*255    88e*/
+
+#define POLLING_LLT_THRESHOLD			20
+#define POLLING_READY_TIMEOUT_COUNT		3000
+
+#define	MAX_MSS_DENSITY_2T			0x13
+#define	MAX_MSS_DENSITY_1T			0x0A
+
+#define EPROM_CMD_OPERATING_MODE_MASK		((1<<7)|(1<<6))
+#define EPROM_CMD_CONFIG			0x3
+#define EPROM_CMD_LOAD				1
+
+#define	HWSET_MAX_SIZE_92S			HWSET_MAX_SIZE
+
+#define	HAL_8192C_HW_GPIO_WPS_BIT		BIT(2)
+
+#define	RPMAC_RESET				0x100
+#define	RPMAC_TXSTART				0x104
+#define	RPMAC_TXLEGACYSIG			0x108
+#define	RPMAC_TXHTSIG1				0x10c
+#define	RPMAC_TXHTSIG2				0x110
+#define	RPMAC_PHYDEBUG				0x114
+#define	RPMAC_TXPACKETNUM			0x118
+#define	RPMAC_TXIDLE				0x11c
+#define	RPMAC_TXMACHEADER0			0x120
+#define	RPMAC_TXMACHEADER1			0x124
+#define	RPMAC_TXMACHEADER2			0x128
+#define	RPMAC_TXMACHEADER3			0x12c
+#define	RPMAC_TXMACHEADER4			0x130
+#define	RPMAC_TXMACHEADER5			0x134
+#define	RPMAC_TXDADATYPE			0x138
+#define	RPMAC_TXRANDOMSEED			0x13c
+#define	RPMAC_CCKPLCPPREAMBLE			0x140
+#define	RPMAC_CCKPLCPHEADER			0x144
+#define	RPMAC_CCKCRC16				0x148
+#define	RPMAC_OFDMRXCRC32OK			0x170
+#define	RPMAC_OFDMRXCRC32Er			0x174
+#define	RPMAC_OFDMRXPARITYER			0x178
+#define	RPMAC_OFDMRXCRC8ER			0x17c
+#define	RPMAC_CCKCRXRC16ER			0x180
+#define	RPMAC_CCKCRXRC32ER			0x184
+#define	RPMAC_CCKCRXRC32OK			0x188
+#define	RPMAC_TXSTATUS				0x18c
+
+#define	RFPGA0_RFMOD				0x800
+
+#define	RFPGA0_TXINFO				0x804
+#define	RFPGA0_PSDFUNCTION			0x808
+
+#define	RFPGA0_TXGAINSTAGE			0x80c
+
+#define	RFPGA0_RFTIMING1			0x810
+#define	RFPGA0_RFTIMING2			0x814
+
+#define	RFPGA0_XA_HSSIPARAMETER1		0x820
+#define	RFPGA0_XA_HSSIPARAMETER2		0x824
+#define	RFPGA0_XB_HSSIPARAMETER1		0x828
+#define	RFPGA0_XB_HSSIPARAMETER2		0x82c
+
+#define	RFPGA0_XA_LSSIPARAMETER			0x840
+#define	RFPGA0_XB_LSSIPARAMETER			0x844
+
+#define	RFPGA0_RFWAKEUPPARAMETER		0x850
+#define	RFPGA0_RFSLEEPUPPARAMETER		0x854
+
+#define	RFPGA0_XAB_SWITCHCONTROL		0x858
+#define	RFPGA0_XCD_SWITCHCONTROL		0x85c
+
+#define	RFPGA0_XA_RFINTERFACEOE			0x860
+#define	RFPGA0_XB_RFINTERFACEOE			0x864
+
+#define	RFPGA0_XAB_RFINTERFACESW		0x870
+#define	RFPGA0_XCD_RFINTERFACESW		0x874
+
+#define	rFPGA0_XAB_RFPARAMETER			0x878
+#define	rFPGA0_XCD_RFPARAMETER			0x87c
+
+#define	RFPGA0_ANALOGPARAMETER1			0x880
+#define	RFPGA0_ANALOGPARAMETER2			0x884
+#define	RFPGA0_ANALOGPARAMETER3			0x888
+#define	RFPGA0_ANALOGPARAMETER4			0x88c
+
+#define	RFPGA0_XA_LSSIREADBACK			0x8a0
+#define	RFPGA0_XB_LSSIREADBACK			0x8a4
+#define	RFPGA0_XC_LSSIREADBACK			0x8a8
+#define	RFPGA0_XD_LSSIREADBACK			0x8ac
+
+#define	RFPGA0_PSDREPORT			0x8b4
+#define	TRANSCEIVEA_HSPI_READBACK		0x8b8
+#define	TRANSCEIVEB_HSPI_READBACK		0x8bc
+#define	REG_SC_CNT				0x8c4
+#define	RFPGA0_XAB_RFINTERFACERB		0x8e0
+#define	RFPGA0_XCD_RFINTERFACERB		0x8e4
+
+#define	RFPGA1_RFMOD				0x900
+
+#define	RFPGA1_TXBLOCK				0x904
+#define	RFPGA1_DEBUGSELECT			0x908
+#define	RFPGA1_TXINFO				0x90c
+
+#define	RCCK0_SYSTEM				0xa00
+
+#define	RCCK0_AFESETTING			0xa04
+#define	RCCK0_CCA				0xa08
+
+#define	RCCK0_RXAGC1				0xa0c
+#define	RCCK0_RXAGC2				0xa10
+
+#define	RCCK0_RXHP				0xa14
+
+#define	RCCK0_DSPPARAMETER1			0xa18
+#define	RCCK0_DSPPARAMETER2			0xa1c
+
+#define	RCCK0_TXFILTER1				0xa20
+#define	RCCK0_TXFILTER2				0xa24
+#define	RCCK0_DEBUGPORT				0xa28
+#define	RCCK0_FALSEALARMREPORT			0xa2c
+#define	RCCK0_TRSSIREPORT			0xa50
+#define	RCCK0_RXREPORT				0xa54
+#define	RCCK0_FACOUNTERLOWER			0xa5c
+#define	RCCK0_FACOUNTERUPPER			0xa58
+#define	RCCK0_CCA_CNT				0xa60
+
+
+/* PageB(0xB00) */
+#define	RPDP_ANTA				0xb00
+#define	RPDP_ANTA_4				0xb04
+#define	RPDP_ANTA_8				0xb08
+#define	RPDP_ANTA_C				0xb0c
+#define	RPDP_ANTA_10				0xb10
+#define	RPDP_ANTA_14				0xb14
+#define	RPDP_ANTA_18				0xb18
+#define	RPDP_ANTA_1C				0xb1c
+#define	RPDP_ANTA_20				0xb20
+#define	RPDP_ANTA_24				0xb24
+
+#define	RCONFIG_PMPD_ANTA			0xb28
+#define	RCONFIG_RAM64X16			0xb2c
+
+#define	RBNDA					0xb30
+#define	RHSSIPAR				0xb34
+
+#define	RCONFIG_ANTA				0xb68
+#define	RCONFIG_ANTB				0xb6c
+
+#define	RPDP_ANTB				0xb70
+#define	RPDP_ANTB_4				0xb74
+#define	RPDP_ANTB_8				0xb78
+#define	RPDP_ANTB_C				0xb7c
+#define	RPDP_ANTB_10				0xb80
+#define	RPDP_ANTB_14				0xb84
+#define	RPDP_ANTB_18				0xb88
+#define	RPDP_ANTB_1C				0xb8c
+#define	RPDP_ANTB_20				0xb90
+#define	RPDP_ANTB_24				0xb94
+
+#define	RCONFIG_PMPD_ANTB			0xb98
+
+#define	RBNDB					0xba0
+
+#define	RAPK					0xbd8
+#define	rPm_Rx0_AntA				0xbdc
+#define	rPm_Rx1_AntA				0xbe0
+#define	rPm_Rx2_AntA				0xbe4
+#define	rPm_Rx3_AntA				0xbe8
+#define	rPm_Rx0_AntB				0xbec
+#define	rPm_Rx1_AntB				0xbf0
+#define	rPm_Rx2_AntB				0xbf4
+#define	rPm_Rx3_AntB				0xbf8
+
+/*Page C*/
+#define	ROFDM0_LSTF				0xc00
+
+#define	ROFDM0_TRXPATHENABLE			0xc04
+#define	ROFDM0_TRMUXPAR				0xc08
+#define	ROFDM0_TRSWISOLATION			0xc0c
+
+#define	ROFDM0_XARXAFE				0xc10
+#define	ROFDM0_XARXIQIMBAL			0xc14
+#define	ROFDM0_XBRXAFE				0xc18
+#define	ROFDM0_XBRXIQIMBAL			0xc1c
+#define	ROFDM0_XCRXAFE				0xc20
+#define	ROFDM0_XCRXIQIMBAL			0xc24
+#define	ROFDM0_XDRXAFE				0xc28
+#define	ROFDM0_XDRXIQIMBAL			0xc2c
+
+#define	ROFDM0_RXDETECTOR1			0xc30
+#define	ROFDM0_RXDETECTOR2			0xc34
+#define	ROFDM0_RXDETECTOR3			0xc38
+#define	ROFDM0_RXDETECTOR4			0xc3c
+
+#define	ROFDM0_RXDSP				0xc40
+#define	ROFDM0_CFOANDDAGC			0xc44
+#define	ROFDM0_CCADROPTHRES			0xc48
+#define	ROFDM0_ECCATHRES			0xc4c
+
+#define	ROFDM0_XAAGCCORE1			0xc50
+#define	ROFDM0_XAAGCCORE2			0xc54
+#define	ROFDM0_XBAGCCORE1			0xc58
+#define	ROFDM0_XBAGCCORE2			0xc5c
+#define	ROFDM0_XCAGCCORE1			0xc60
+#define	ROFDM0_XCAGCCORE2			0xc64
+#define	ROFDM0_XDAGCCORE1			0xc68
+#define	ROFDM0_XDAGCCORE2			0xc6c
+
+#define	ROFDM0_AGCPARAMETER1			0xc70
+#define	ROFDM0_AGCPARAMETER2			0xc74
+#define	ROFDM0_AGCRSSITABLE			0xc78
+#define	ROFDM0_HTSTFAGC				0xc7c
+
+#define	ROFDM0_XATXIQIMBAL			0xc80
+#define	ROFDM0_XATXAFE				0xc84
+#define	ROFDM0_XBTXIQIMBAL			0xc88
+#define	ROFDM0_XBTXAFE				0xc8c
+#define	ROFDM0_XCTXIQIMBAL			0xc90
+#define	ROFDM0_XCTXAFE				0xc94
+#define	ROFDM0_XDTXIQIMBAL			0xc98
+#define	ROFDM0_XDTXAFE				0xc9c
+
+#define ROFDM0_RXIQEXTANTA			0xca0
+#define	ROFDM0_TXCOEFF1				0xca4
+#define	ROFDM0_TXCOEFF2				0xca8
+#define	ROFDM0_TXCOEFF3				0xcac
+#define	ROFDM0_TXCOEFF4				0xcb0
+#define	ROFDM0_TXCOEFF5				0xcb4
+#define	ROFDM0_TXCOEFF6				0xcb8
+
+#define	ROFDM0_RXHPPARAMETER			0xce0
+#define	ROFDM0_TXPSEUDONOISEWGT			0xce4
+#define	ROFDM0_FRAMESYNC			0xcf0
+#define	ROFDM0_DFSREPORT			0xcf4
+
+
+#define	ROFDM1_LSTF				0xd00
+#define	ROFDM1_TRXPATHENABLE			0xd04
+
+#define	ROFDM1_CF0				0xd08
+#define	ROFDM1_CSI1				0xd10
+#define	ROFDM1_SBD				0xd14
+#define	ROFDM1_CSI2				0xd18
+#define	ROFDM1_CFOTRACKING			0xd2c
+#define	ROFDM1_TRXMESAURE1			0xd34
+#define	ROFDM1_INTFDET				0xd3c
+#define	ROFDM1_PSEUDONOISESTATEAB		0xd50
+#define	ROFDM1_PSEUDONOISESTATECD		0xd54
+#define	ROFDM1_RXPSEUDONOISEWGT			0xd58
+
+#define	ROFDM_PHYCOUNTER1			0xda0
+#define	ROFDM_PHYCOUNTER2			0xda4
+#define	ROFDM_PHYCOUNTER3			0xda8
+
+#define	ROFDM_SHORTCFOAB			0xdac
+#define	ROFDM_SHORTCFOCD			0xdb0
+#define	ROFDM_LONGCFOAB				0xdb4
+#define	ROFDM_LONGCFOCD				0xdb8
+#define	ROFDM_TAILCF0AB				0xdbc
+#define	ROFDM_TAILCF0CD				0xdc0
+#define	ROFDM_PWMEASURE1			0xdc4
+#define	ROFDM_PWMEASURE2			0xdc8
+#define	ROFDM_BWREPORT				0xdcc
+#define	ROFDM_AGCREPORT				0xdd0
+#define	ROFDM_RXSNR				0xdd4
+#define	ROFDM_RXEVMCSI				0xdd8
+#define	ROFDM_SIGREPORT				0xddc
+
+#define	RTXAGC_A_RATE18_06			0xe00
+#define	RTXAGC_A_RATE54_24			0xe04
+#define	RTXAGC_A_CCK1_MCS32			0xe08
+#define	RTXAGC_A_MCS03_MCS00			0xe10
+#define	RTXAGC_A_MCS07_MCS04			0xe14
+#define	RTXAGC_A_MCS11_MCS08			0xe18
+#define	RTXAGC_A_MCS15_MCS12			0xe1c
+
+#define	RTXAGC_B_RATE18_06			0x830
+#define	RTXAGC_B_RATE54_24			0x834
+#define	RTXAGC_B_CCK1_55_MCS32			0x838
+#define	RTXAGC_B_MCS03_MCS00			0x83c
+#define	RTXAGC_B_MCS07_MCS04			0x848
+#define	RTXAGC_B_MCS11_MCS08			0x84c
+#define	RTXAGC_B_MCS15_MCS12			0x868
+#define	RTXAGC_B_CCK11_A_CCK2_11		0x86c
+
+#define	RFPGA0_IQK				0xe28
+#define	RTX_IQK_TONE_A				0xe30
+#define	RRX_IQK_TONE_A				0xe34
+#define	RTX_IQK_PI_A				0xe38
+#define	RRX_IQK_PI_A				0xe3c
+
+#define	RTX_IQK					0xe40
+#define	RRX_IQK					0xe44
+#define	RIQK_AGC_PTS				0xe48
+#define	RIQK_AGC_RSP				0xe4c
+#define	RTX_IQK_TONE_B				0xe50
+#define	RRX_IQK_TONE_B				0xe54
+#define	RTX_IQK_PI_B				0xe58
+#define	RRX_IQK_PI_B				0xe5c
+#define	RIQK_AGC_CONT				0xe60
+
+#define	RBLUE_TOOTH				0xe6c
+#define	RRX_WAIT_CCA				0xe70
+#define	RTX_CCK_RFON				0xe74
+#define	RTX_CCK_BBON				0xe78
+#define	RTX_OFDM_RFON				0xe7c
+#define	RTX_OFDM_BBON				0xe80
+#define	RTX_TO_RX				0xe84
+#define	RTX_TO_TX				0xe88
+#define	RRX_CCK					0xe8c
+
+#define	RTX_POWER_BEFORE_IQK_A			0xe94
+#define	RTX_POWER_AFTER_IQK_A			0xe9c
+
+#define	RRX_POWER_BEFORE_IQK_A			0xea0
+#define	RRX_POWER_BEFORE_IQK_A_2		0xea4
+#define	RRX_POWER_AFTER_IQK_A			0xea8
+#define	RRX_POWER_AFTER_IQK_A_2			0xeac
+
+#define	RTX_POWER_BEFORE_IQK_B			0xeb4
+#define	RTX_POWER_AFTER_IQK_B			0xebc
+
+#define	RRX_POWER_BEFORE_IQK_B			0xec0
+#define	RRX_POWER_BEFORE_IQK_B_2		0xec4
+#define	RRX_POWER_AFTER_IQK_B			0xec8
+#define	RRX_POWER_AFTER_IQK_B_2			0xecc
+
+#define	RRX_OFDM				0xed0
+#define	RRX_WAIT_RIFS				0xed4
+#define	RRX_TO_RX				0xed8
+#define	RSTANDBY				0xedc
+#define	RSLEEP					0xee0
+#define	RPMPD_ANAEN				0xeec
+
+#define	RZEBRA1_HSSIENABLE			0x0
+#define	RZEBRA1_TRXENABLE1			0x1
+#define	RZEBRA1_TRXENABLE2			0x2
+#define	RZEBRA1_AGC				0x4
+#define	RZEBRA1_CHARGEPUMP			0x5
+#define	RZEBRA1_CHANNEL				0x7
+
+#define	RZEBRA1_TXGAIN				0x8
+#define	RZEBRA1_TXLPF				0x9
+#define	RZEBRA1_RXLPF				0xb
+#define	RZEBRA1_RXHPFCORNER			0xc
+
+#define	RGLOBALCTRL				0
+#define	RRTL8256_TXLPF				19
+#define	RRTL8256_RXLPF				11
+#define	RRTL8258_TXLPF				0x11
+#define	RRTL8258_RXLPF				0x13
+#define	RRTL8258_RSSILPF			0xa
+
+#define	RF_AC					0x00
+
+#define	RF_IQADJ_G1				0x01
+#define	RF_IQADJ_G2				0x02
+#define	RF_POW_TRSW				0x05
+
+#define	RF_GAIN_RX				0x06
+#define	RF_GAIN_TX				0x07
+
+#define	RF_TXM_IDAC				0x08
+#define	RF_BS_IQGEN				0x0F
+
+#define	RF_MODE1				0x10
+#define	RF_MODE2				0x11
+
+#define	RF_RX_AGC_HP				0x12
+#define	RF_TX_AGC				0x13
+#define	RF_BIAS					0x14
+#define	RF_IPA					0x15
+#define	RF_POW_ABILITY				0x17
+#define	RF_MODE_AG				0x18
+#define	RRFCHANNEL				0x18
+#define	RF_CHNLBW				0x18
+#define	RF_TOP					0x19
+
+#define	RF_RX_G1				0x1A
+#define	RF_RX_G2				0x1B
+
+#define	RF_RX_BB2				0x1C
+#define	RF_RX_BB1				0x1D
+
+#define	RF_RCK1					0x1E
+#define	RF_RCK2					0x1F
+
+#define	RF_TX_G1				0x20
+#define	RF_TX_G2				0x21
+#define	RF_TX_G3				0x22
+
+#define	RF_TX_BB1				0x23
+#define	RF_T_METER				0x42
+
+#define	RF_SYN_G1				0x25
+#define	RF_SYN_G2				0x26
+#define	RF_SYN_G3				0x27
+#define	RF_SYN_G4				0x28
+#define	RF_SYN_G5				0x29
+#define	RF_SYN_G6				0x2A
+#define	RF_SYN_G7				0x2B
+#define	RF_SYN_G8				0x2C
+
+#define	RF_RCK_OS				0x30
+#define	RF_TXPA_G1				0x31
+#define	RF_TXPA_G2				0x32
+#define	RF_TXPA_G3				0x33
+
+#define	RF_TX_BIAS_A				0x35
+#define	RF_TX_BIAS_D				0x36
+#define	RF_LOBF_9				0x38
+#define	RF_RXRF_A3				0x3C
+#define	RF_TRSW					0x3F
+
+#define	RF_TXRF_A2				0x41
+#define	RF_TXPA_G4				0x46
+#define	RF_TXPA_A4				0x4B
+
+#define	RF_WE_LUT				0xEF
+
+#define	BBBRESETB				0x100
+#define	BGLOBALRESETB				0x200
+#define	BOFDMTXSTART				0x4
+#define	BCCKTXSTART				0x8
+#define	BCRC32DEBUG				0x100
+#define	BPMACLOOPBACK				0x10
+#define	BTXLSIG					0xffffff
+#define	BOFDMTXRATE				0xf
+#define	BOFDMTXRESERVED				0x10
+#define	BOFDMTXLENGTH				0x1ffe0
+#define	BOFDMTXPARITY				0x20000
+#define	BTXHTSIG1				0xffffff
+#define	BTXHTMCSRATE				0x7f
+#define	BTXHTBW					0x80
+#define	BTXHTLENGTH				0xffff00
+#define	BTXHTSIG2				0xffffff
+#define	BTXHTSMOOTHING				0x1
+#define	BTXHTSOUNDING				0x2
+#define	BTXHTRESERVED				0x4
+#define	BTXHTAGGREATION				0x8
+#define	BTXHTSTBC				0x30
+#define	BTXHTADVANCECODING			0x40
+#define	BTXHTSHORTGI				0x80
+#define	BTXHTNUMBERHT_LTF			0x300
+#define	BTXHTCRC8				0x3fc00
+#define	BCOUNTERRESET				0x10000
+#define	BNUMOFOFDMTX				0xffff
+#define	BNUMOFCCKTX				0xffff0000
+#define	BTXIDLEINTERVAL				0xffff
+#define	BOFDMSERVICE				0xffff0000
+#define	BTXMACHEADER				0xffffffff
+#define	BTXDATAINIT				0xff
+#define	BTXHTMODE				0x100
+#define	BTXDATATYPE				0x30000
+#define	BTXRANDOMSEED				0xffffffff
+#define	BCCKTXPREAMBLE				0x1
+#define	BCCKTXSFD				0xffff0000
+#define	BCCKTXSIG				0xff
+#define	BCCKTXSERVICE				0xff00
+#define	BCCKLENGTHEXT				0x8000
+#define	BCCKTXLENGHT				0xffff0000
+#define	BCCKTXCRC16				0xffff
+#define	BCCKTXSTATUS				0x1
+#define	BOFDMTXSTATUS				0x2
+#define IS_BB_REG_OFFSET_92S(_offset)	\
+	((_offset >= 0x800) && (_offset <= 0xfff))
+
+#define	BRFMOD					0x1
+#define	BJAPANMODE				0x2
+#define	BCCKTXSC				0x30
+#define	BCCKEN					0x1000000
+#define	BOFDMEN					0x2000000
+
+#define	BOFDMRXADCPHASE				0x10000
+#define	BOFDMTXDACPHASE				0x40000
+#define	BXATXAGC				0x3f
+
+#define	BXBTXAGC				0xf00
+#define	BXCTXAGC				0xf000
+#define	BXDTXAGC				0xf0000
+
+#define	BPASTART				0xf0000000
+#define	BTRSTART				0x00f00000
+#define	BRFSTART				0x0000f000
+#define	BBBSTART				0x000000f0
+#define	BBBCCKSTART				0x0000000f
+#define	BPAEND					0xf
+#define	BTREND					0x0f000000
+#define	BRFEND					0x000f0000
+#define	BCCAMASK				0x000000f0
+#define	BR2RCCAMASK				0x00000f00
+#define	BHSSI_R2TDELAY				0xf8000000
+#define	BHSSI_T2RDELAY				0xf80000
+#define	BCONTXHSSI				0x400
+#define	BIGFROMCCK				0x200
+#define	BAGCADDRESS				0x3f
+#define	BRXHPTX					0x7000
+#define	BRXHP2RX				0x38000
+#define	BRXHPCCKINI				0xc0000
+#define	BAGCTXCODE				0xc00000
+#define	BAGCRXCODE				0x300000
+
+#define	B3WIREDATALENGTH			0x800
+#define	B3WIREADDREAALENGTH			0x400
+
+#define	B3WIRERFPOWERDOWN			0x1
+#define	B5GPAPEPOLARITY				0x40000000
+#define	B2GPAPEPOLARITY				0x80000000
+#define	BRFSW_TXDEFAULTANT			0x3
+#define	BRFSW_TXOPTIONANT			0x30
+#define	BRFSW_RXDEFAULTANT			0x300
+#define	BRFSW_RXOPTIONANT			0x3000
+#define	BRFSI_3WIREDATA				0x1
+#define	BRFSI_3WIRECLOCK			0x2
+#define	BRFSI_3WIRELOAD				0x4
+#define	BRFSI_3WIRERW				0x8
+#define	BRFSI_3WIRE				0xf
+
+#define	BRFSI_RFENV				0x10
+
+#define	BRFSI_TRSW				0x20
+#define	BRFSI_TRSWB				0x40
+#define	BRFSI_ANTSW				0x100
+#define	BRFSI_ANTSWB				0x200
+#define	BRFSI_PAPE				0x400
+#define	BRFSI_PAPE5G				0x800
+#define	BBANDSELECT				0x1
+#define	BHTSIG2_GI				0x80
+#define	BHTSIG2_SMOOTHING			0x01
+#define	BHTSIG2_SOUNDING			0x02
+#define	BHTSIG2_AGGREATON			0x08
+#define	BHTSIG2_STBC				0x30
+#define	BHTSIG2_ADVCODING			0x40
+#define	BHTSIG2_NUMOFHTLTF			0x300
+#define	BHTSIG2_CRC8				0x3fc
+#define	BHTSIG1_MCS				0x7f
+#define	BHTSIG1_BANDWIDTH			0x80
+#define	BHTSIG1_HTLENGTH			0xffff
+#define	BLSIG_RATE				0xf
+#define	BLSIG_RESERVED				0x10
+#define	BLSIG_LENGTH				0x1fffe
+#define	BLSIG_PARITY				0x20
+#define	BCCKRXPHASE				0x4
+
+#define	BLSSIREADADDRESS			0x7f800000
+#define	BLSSIREADEDGE				0x80000000
+
+#define	BLSSIREADBACKDATA			0xfffff
+
+#define	BLSSIREADOKFLAG				0x1000
+#define	BCCKSAMPLERATE				0x8
+#define	BREGULATOR0STANDBY			0x1
+#define	BREGULATORPLLSTANDBY			0x2
+#define	BREGULATOR1STANDBY			0x4
+#define	BPLLPOWERUP				0x8
+#define	BDPLLPOWERUP				0x10
+#define	BDA10POWERUP				0x20
+#define	BAD7POWERUP				0x200
+#define	BDA6POWERUP				0x2000
+#define	BXTALPOWERUP				0x4000
+#define	B40MDCLKPOWERUP				0x8000
+#define	BDA6DEBUGMODE				0x20000
+#define	BDA6SWING				0x380000
+
+#define	BADCLKPHASE				0x4000000
+#define	B80MCLKDELAY				0x18000000
+#define	BAFEWATCHDOGENABLE			0x20000000
+
+#define	BXTALCAP01				0xc0000000
+#define	BXTALCAP23				0x3
+#define	BXTALCAP92X				0x0f000000
+#define BXTALCAP				0x0f000000
+
+#define	BINTDIFCLKENABLE			0x400
+#define	BEXTSIGCLKENABLE			0x800
+#define	BBANDGAP_MBIAS_POWERUP			0x10000
+#define	BAD11SH_GAIN				0xc0000
+#define	BAD11NPUT_RANGE				0x700000
+#define	BAD110P_CURRENT				0x3800000
+#define	BLPATH_LOOPBACK				0x4000000
+#define	BQPATH_LOOPBACK				0x8000000
+#define	BAFE_LOOPBACK				0x10000000
+#define	BDA10_SWING				0x7e0
+#define	BDA10_REVERSE				0x800
+#define	BDA_CLK_SOURCE				0x1000
+#define	BDA7INPUT_RANGE				0x6000
+#define	BDA7_GAIN				0x38000
+#define	BDA7OUTPUT_CM_MODE			0x40000
+#define	BDA7INPUT_CM_MODE			0x380000
+#define	BDA7CURRENT				0xc00000
+#define	BREGULATOR_ADJUST			0x7000000
+#define	BAD11POWERUP_ATTX			0x1
+#define	BDA10PS_ATTX				0x10
+#define	BAD11POWERUP_ATRX			0x100
+#define	BDA10PS_ATRX				0x1000
+#define	BCCKRX_AGC_FORMAT			0x200
+#define	BPSDFFT_SAMPLE_POINT			0xc000
+#define	BPSD_AVERAGE_NUM			0x3000
+#define	BIQPATH_CONTROL				0xc00
+#define	BPSD_FREQ				0x3ff
+#define	BPSD_ANTENNA_PATH			0x30
+#define	BPSD_IQ_SWITCH				0x40
+#define	BPSD_RX_TRIGGER				0x400000
+#define	BPSD_TX_TRIGGERCW			0x80000000
+#define	BPSD_SINE_TONE_SCALE			0x7f000000
+#define	BPSD_REPORT				0xffff
+
+#define	BOFDM_TXSC				0x30000000
+#define	BCCK_TXON				0x1
+#define	BOFDM_TXON				0x2
+#define	BDEBUG_PAGE				0xfff
+#define	BDEBUG_ITEM				0xff
+#define	BANTL					0x10
+#define	BANT_NONHT				0x100
+#define	BANT_HT1				0x1000
+#define	BANT_HT2				0x10000
+#define	BANT_HT1S1				0x100000
+#define	BANT_NONHTS1				0x1000000
+
+#define	BCCK_BBMODE				0x3
+#define	BCCK_TXPOWERSAVING			0x80
+#define	BCCK_RXPOWERSAVING			0x40
+
+#define	BCCK_SIDEBAND				0x10
+
+#define	BCCK_SCRAMBLE				0x8
+#define	BCCK_ANTDIVERSITY			0x8000
+#define	BCCK_CARRIER_RECOVERY			0x4000
+#define	BCCK_TXRATE				0x3000
+#define	BCCK_DCCANCEL				0x0800
+#define	BCCK_ISICANCEL				0x0400
+#define	BCCK_MATCH_FILTER			0x0200
+#define	BCCK_EQUALIZER				0x0100
+#define	BCCK_PREAMBLE_DETECT			0x800000
+#define	BCCK_FAST_FALSECCA			0x400000
+#define	BCCK_CH_ESTSTART			0x300000
+#define	BCCK_CCA_COUNT				0x080000
+#define	BCCK_CS_LIM				0x070000
+#define	BCCK_BIST_MODE				0x80000000
+#define	BCCK_CCAMASK				0x40000000
+#define	BCCK_TX_DAC_PHASE			0x4
+#define	BCCK_RX_ADC_PHASE			0x20000000
+#define	BCCKR_CP_MODE				0x0100
+#define	BCCK_TXDC_OFFSET			0xf0
+#define	BCCK_RXDC_OFFSET			0xf
+#define	BCCK_CCA_MODE				0xc000
+#define	BCCK_FALSECS_LIM			0x3f00
+#define	BCCK_CS_RATIO				0xc00000
+#define	BCCK_CORGBIT_SEL			0x300000
+#define	BCCK_PD_LIM				0x0f0000
+#define	BCCK_NEWCCA				0x80000000
+#define	BCCK_RXHP_OF_IG				0x8000
+#define	BCCK_RXIG				0x7f00
+#define	BCCK_LNA_POLARITY			0x800000
+#define	BCCK_RX1ST_BAIN				0x7f0000
+#define	BCCK_RF_EXTEND				0x20000000
+#define	BCCK_RXAGC_SATLEVEL			0x1f000000
+#define	BCCK_RXAGC_SATCOUNT			0xe0
+#define	BCCKRXRFSETTLE				0x1f
+#define	BCCK_FIXED_RXAGC			0x8000
+#define	BCCK_ANTENNA_POLARITY			0x2000
+#define	BCCK_TXFILTER_TYPE			0x0c00
+#define	BCCK_RXAGC_REPORTTYPE			0x0300
+#define	BCCK_RXDAGC_EN				0x80000000
+#define	BCCK_RXDAGC_PERIOD			0x20000000
+#define	BCCK_RXDAGC_SATLEVEL			0x1f000000
+#define	BCCK_TIMING_RECOVERY			0x800000
+#define	BCCK_TXC0				0x3f0000
+#define	BCCK_TXC1				0x3f000000
+#define	BCCK_TXC2				0x3f
+#define	BCCK_TXC3				0x3f00
+#define	BCCK_TXC4				0x3f0000
+#define	BCCK_TXC5				0x3f000000
+#define	BCCK_TXC6				0x3f
+#define	BCCK_TXC7				0x3f00
+#define	BCCK_DEBUGPORT				0xff0000
+#define	BCCK_DAC_DEBUG				0x0f000000
+#define	BCCK_FALSEALARM_ENABLE			0x8000
+#define	BCCK_FALSEALARM_READ			0x4000
+#define	BCCK_TRSSI				0x7f
+#define	BCCK_RXAGC_REPORT			0xfe
+#define	BCCK_RXREPORT_ANTSEL			0x80000000
+#define	BCCK_RXREPORT_MFOFF			0x40000000
+#define	BCCK_RXREPORT_SQLOSS			0x20000000
+#define	BCCK_RXREPORT_PKTLOSS			0x10000000
+#define	BCCK_RXREPORT_LOCKEDBIT			0x08000000
+#define	BCCK_RXREPORT_RATEERROR			0x04000000
+#define	BCCK_RXREPORT_RXRATE			0x03000000
+#define	BCCK_RXFA_COUNTER_LOWER			0xff
+#define	BCCK_RXFA_COUNTER_UPPER			0xff000000
+#define	BCCK_RXHPAGC_START			0xe000
+#define	BCCK_RXHPAGC_FINAL			0x1c00
+#define	BCCK_RXFALSEALARM_ENABLE		0x8000
+#define	BCCK_FACOUNTER_FREEZE			0x4000
+#define	BCCK_TXPATH_SEL				0x10000000
+#define	BCCK_DEFAULT_RXPATH			0xc000000
+#define	BCCK_OPTION_RXPATH			0x3000000
+
+#define	BNUM_OFSTF				0x3
+#define	BSHIFT_L				0xc0
+#define	BGI_TH					0xc
+#define	BRXPATH_A				0x1
+#define	BRXPATH_B				0x2
+#define	BRXPATH_C				0x4
+#define	BRXPATH_D				0x8
+#define	BTXPATH_A				0x1
+#define	BTXPATH_B				0x2
+#define	BTXPATH_C				0x4
+#define	BTXPATH_D				0x8
+#define	BTRSSI_FREQ				0x200
+#define	BADC_BACKOFF				0x3000
+#define	BDFIR_BACKOFF				0xc000
+#define	BTRSSI_LATCH_PHASE			0x10000
+#define	BRX_LDC_OFFSET				0xff
+#define	BRX_QDC_OFFSET				0xff00
+#define	BRX_DFIR_MODE				0x1800000
+#define	BRX_DCNF_TYPE				0xe000000
+#define	BRXIQIMB_A				0x3ff
+#define	BRXIQIMB_B				0xfc00
+#define	BRXIQIMB_C				0x3f0000
+#define	BRXIQIMB_D				0xffc00000
+#define	BDC_DC_NOTCH				0x60000
+#define	BRXNB_NOTCH				0x1f000000
+#define	BPD_TH					0xf
+#define	BPD_TH_OPT2				0xc000
+#define	BPWED_TH				0x700
+#define	BIFMF_WIN_L				0x800
+#define	BPD_OPTION				0x1000
+#define	BMF_WIN_L				0xe000
+#define	BBW_SEARCH_L				0x30000
+#define	BWIN_ENH_L				0xc0000
+#define	BBW_TH					0x700000
+#define	BED_TH2					0x3800000
+#define	BBW_OPTION				0x4000000
+#define	BRADIO_TH				0x18000000
+#define	BWINDOW_L				0xe0000000
+#define	BSBD_OPTION				0x1
+#define	BFRAME_TH				0x1c
+#define	BFS_OPTION				0x60
+#define	BDC_SLOPE_CHECK				0x80
+#define	BFGUARD_COUNTER_DC_L			0xe00
+#define	BFRAME_WEIGHT_SHORT			0x7000
+#define	BSUB_TUNE				0xe00000
+#define	BFRAME_DC_LENGTH			0xe000000
+#define	BSBD_START_OFFSET			0x30000000
+#define	BFRAME_TH_2				0x7
+#define	BFRAME_GI2_TH				0x38
+#define	BGI2_SYNC_EN				0x40
+#define	BSARCH_SHORT_EARLY			0x300
+#define	BSARCH_SHORT_LATE			0xc00
+#define	BSARCH_GI2_LATE				0x70000
+#define	BCFOANTSUM				0x1
+#define	BCFOACC					0x2
+#define	BCFOSTARTOFFSET				0xc
+#define	BCFOLOOPBACK				0x70
+#define	BCFOSUMWEIGHT				0x80
+#define	BDAGCENABLE				0x10000
+#define	BTXIQIMB_A				0x3ff
+#define	BTXIQIMB_B				0xfc00
+#define	BTXIQIMB_C				0x3f0000
+#define	BTXIQIMB_D				0xffc00000
+#define	BTXIDCOFFSET				0xff
+#define	BTXIQDCOFFSET				0xff00
+#define	BTXDFIRMODE				0x10000
+#define	BTXPESUDO_NOISEON			0x4000000
+#define	BTXPESUDO_NOISE_A			0xff
+#define	BTXPESUDO_NOISE_B			0xff00
+#define	BTXPESUDO_NOISE_C			0xff0000
+#define	BTXPESUDO_NOISE_D			0xff000000
+#define	BCCA_DROPOPTION				0x20000
+#define	BCCA_DROPTHRES				0xfff00000
+#define	BEDCCA_H				0xf
+#define	BEDCCA_L				0xf0
+#define	BLAMBDA_ED				0x300
+#define	BRX_INITIALGAIN				0x7f
+#define	BRX_ANTDIV_EN				0x80
+#define	BRX_AGC_ADDRESS_FOR_LNA			0x7f00
+#define	BRX_HIGHPOWER_FLOW			0x8000
+#define	BRX_AGC_FREEZE_THRES			0xc0000
+#define	BRX_FREEZESTEP_AGC1			0x300000
+#define	BRX_FREEZESTEP_AGC2			0xc00000
+#define	BRX_FREEZESTEP_AGC3			0x3000000
+#define	BRX_FREEZESTEP_AGC0			0xc000000
+#define	BRXRSSI_CMP_EN				0x10000000
+#define	BRXQUICK_AGCEN				0x20000000
+#define	BRXAGC_FREEZE_THRES_MODE		0x40000000
+#define	BRX_OVERFLOW_CHECKTYPE			0x80000000
+#define	BRX_AGCSHIFT				0x7f
+#define	BTRSW_TRI_ONLY				0x80
+#define	BPOWER_THRES				0x300
+#define	BRXAGC_EN				0x1
+#define	BRXAGC_TOGETHER_EN			0x2
+#define	BRXAGC_MIN				0x4
+#define	BRXHP_INI				0x7
+#define	BRXHP_TRLNA				0x70
+#define	BRXHP_RSSI				0x700
+#define	BRXHP_BBP1				0x7000
+#define	BRXHP_BBP2				0x70000
+#define	BRXHP_BBP3				0x700000
+#define	BRSSI_H					0x7f0000
+#define	BRSSI_GEN				0x7f000000
+#define	BRXSETTLE_TRSW				0x7
+#define	BRXSETTLE_LNA				0x38
+#define	BRXSETTLE_RSSI				0x1c0
+#define	BRXSETTLE_BBP				0xe00
+#define	BRXSETTLE_RXHP				0x7000
+#define	BRXSETTLE_ANTSW_RSSI			0x38000
+#define	BRXSETTLE_ANTSW				0xc0000
+#define	BRXPROCESS_TIME_DAGC			0x300000
+#define	BRXSETTLE_HSSI				0x400000
+#define	BRXPROCESS_TIME_BBPPW			0x800000
+#define	BRXANTENNA_POWER_SHIFT			0x3000000
+#define	BRSSI_TABLE_SELECT			0xc000000
+#define	BRXHP_FINAL				0x7000000
+#define	BRXHPSETTLE_BBP				0x7
+#define	BRXHTSETTLE_HSSI			0x8
+#define	BRXHTSETTLE_RXHP			0x70
+#define	BRXHTSETTLE_BBPPW			0x80
+#define	BRXHTSETTLE_IDLE			0x300
+#define	BRXHTSETTLE_RESERVED			0x1c00
+#define	BRXHT_RXHP_EN				0x8000
+#define	BRXAGC_FREEZE_THRES			0x30000
+#define	BRXAGC_TOGETHEREN			0x40000
+#define	BRXHTAGC_MIN				0x80000
+#define	BRXHTAGC_EN				0x100000
+#define	BRXHTDAGC_EN				0x200000
+#define	BRXHT_RXHP_BBP				0x1c00000
+#define	BRXHT_RXHP_FINAL			0xe0000000
+#define	BRXPW_RADIO_TH				0x3
+#define	BRXPW_RADIO_EN				0x4
+#define	BRXMF_HOLD				0x3800
+#define	BRXPD_DELAY_TH1				0x38
+#define	BRXPD_DELAY_TH2				0x1c0
+#define	BRXPD_DC_COUNT_MAX			0x600
+#define	BRXPD_DELAY_TH				0x8000
+#define	BRXPROCESS_DELAY			0xf0000
+#define	BRXSEARCHRANGE_GI2_EARLY		0x700000
+#define	BRXFRAME_FUARD_COUNTER_L		0x3800000
+#define	BRXSGI_GUARD_L				0xc000000
+#define	BRXSGI_SEARCH_L				0x30000000
+#define	BRXSGI_TH				0xc0000000
+#define	BDFSCNT0				0xff
+#define	BDFSCNT1				0xff00
+#define	BDFSFLAG				0xf0000
+#define	BMF_WEIGHT_SUM				0x300000
+#define	BMINIDX_TH				0x7f000000
+#define	BDAFORMAT				0x40000
+#define	BTXCH_EMU_ENABLE			0x01000000
+#define	BTRSW_ISOLATION_A			0x7f
+#define	BTRSW_ISOLATION_B			0x7f00
+#define	BTRSW_ISOLATION_C			0x7f0000
+#define	BTRSW_ISOLATION_D			0x7f000000
+#define	BEXT_LNA_GAIN				0x7c00
+
+#define	BSTBC_EN				0x4
+#define	BANTENNA_MAPPING			0x10
+#define	BNSS					0x20
+#define	BCFO_ANTSUM_ID				0x200
+#define	BPHY_COUNTER_RESET			0x8000000
+#define	BCFO_REPORT_GET				0x4000000
+#define	BOFDM_CONTINUE_TX			0x10000000
+#define	BOFDM_SINGLE_CARRIER			0x20000000
+#define	BOFDM_SINGLE_TONE			0x40000000
+#define	BHT_DETECT				0x100
+#define	BCFOEN					0x10000
+#define	BCFOVALUE				0xfff00000
+#define	BSIGTONE_RE				0x3f
+#define	BSIGTONE_IM				0x7f00
+#define	BCOUNTER_CCA				0xffff
+#define	BCOUNTER_PARITYFAIL			0xffff0000
+#define	BCOUNTER_RATEILLEGAL			0xffff
+#define	BCOUNTER_CRC8FAIL			0xffff0000
+#define	BCOUNTER_MCSNOSUPPORT			0xffff
+#define	BCOUNTER_FASTSYNC			0xffff
+#define	BSHORTCFO				0xfff
+#define	BSHORTCFOT_LENGTH			12
+#define	BSHORTCFOF_LENGTH			11
+#define	BLONGCFO				0x7ff
+#define	BLONGCFOT_LENGTH			11
+#define	BLONGCFOF_LENGTH			11
+#define	BTAILCFO				0x1fff
+#define	BTAILCFOT_LENGTH			13
+#define	BTAILCFOF_LENGTH			12
+#define	BNOISE_EN_PWDB				0xffff
+#define	BCC_POWER_DB				0xffff0000
+#define	BMOISE_PWDB				0xffff
+#define	BPOWERMEAST_LENGTH			10
+#define	BPOWERMEASF_LENGTH			3
+#define	BRX_HT_BW				0x1
+#define	BRXSC					0x6
+#define	BRX_HT					0x8
+#define	BNB_INTF_DET_ON				0x1
+#define	BINTF_WIN_LEN_CFG			0x30
+#define	BNB_INTF_TH_CFG				0x1c0
+#define	BRFGAIN					0x3f
+#define	BTABLESEL				0x40
+#define	BTRSW					0x80
+#define	BRXSNR_A				0xff
+#define	BRXSNR_B				0xff00
+#define	BRXSNR_C				0xff0000
+#define	BRXSNR_D				0xff000000
+#define	BSNR_EVMT_LENGTH			8
+#define	BSNR_EVMF_LENGTH			1
+#define	BCSI1ST					0xff
+#define	BCSI2ND					0xff00
+#define	BRXEVM1ST				0xff0000
+#define	BRXEVM2ND				0xff000000
+#define	BSIGEVM					0xff
+#define	BPWDB					0xff00
+#define	BSGIEN					0x10000
+
+#define	BSFACTOR_QMA1				0xf
+#define	BSFACTOR_QMA2				0xf0
+#define	BSFACTOR_QMA3				0xf00
+#define	BSFACTOR_QMA4				0xf000
+#define	BSFACTOR_QMA5				0xf0000
+#define	BSFACTOR_QMA6				0xf0000
+#define	BSFACTOR_QMA7				0xf00000
+#define	BSFACTOR_QMA8				0xf000000
+#define	BSFACTOR_QMA9				0xf0000000
+#define	BCSI_SCHEME				0x100000
+
+#define	BNOISE_LVL_TOP_SET			0x3
+#define	BCHSMOOTH				0x4
+#define	BCHSMOOTH_CFG1				0x38
+#define	BCHSMOOTH_CFG2				0x1c0
+#define	BCHSMOOTH_CFG3				0xe00
+#define	BCHSMOOTH_CFG4				0x7000
+#define	BMRCMODE				0x800000
+#define	BTHEVMCFG				0x7000000
+
+#define	BLOOP_FIT_TYPE				0x1
+#define	BUPD_CFO				0x40
+#define	BUPD_CFO_OFFDATA			0x80
+#define	BADV_UPD_CFO				0x100
+#define	BADV_TIME_CTRL				0x800
+#define	BUPD_CLKO				0x1000
+#define	BFC					0x6000
+#define	BTRACKING_MODE				0x8000
+#define	BPHCMP_ENABLE				0x10000
+#define	BUPD_CLKO_LTF				0x20000
+#define	BCOM_CH_CFO				0x40000
+#define	BCSI_ESTI_MODE				0x80000
+#define	BADV_UPD_EQZ				0x100000
+#define	BUCHCFG					0x7000000
+#define	BUPDEQZ					0x8000000
+
+#define	BRX_PESUDO_NOISE_ON			0x20000000
+#define	BRX_PESUDO_NOISE_A			0xff
+#define	BRX_PESUDO_NOISE_B			0xff00
+#define	BRX_PESUDO_NOISE_C			0xff0000
+#define	BRX_PESUDO_NOISE_D			0xff000000
+#define	BRX_PESUDO_NOISESTATE_A			0xffff
+#define	BRX_PESUDO_NOISESTATE_B			0xffff0000
+#define	BRX_PESUDO_NOISESTATE_C			0xffff
+#define	BRX_PESUDO_NOISESTATE_D			0xffff0000
+
+#define	BZEBRA1_HSSIENABLE			0x8
+#define	BZEBRA1_TRXCONTROL			0xc00
+#define	BZEBRA1_TRXGAINSETTING			0x07f
+#define	BZEBRA1_RXCOUNTER			0xc00
+#define	BZEBRA1_TXCHANGEPUMP			0x38
+#define	BZEBRA1_RXCHANGEPUMP			0x7
+#define	BZEBRA1_CHANNEL_NUM			0xf80
+#define	BZEBRA1_TXLPFBW				0x400
+#define	BZEBRA1_RXLPFBW				0x600
+
+#define	BRTL8256REG_MODE_CTRL1			0x100
+#define	BRTL8256REG_MODE_CTRL0			0x40
+#define	BRTL8256REG_TXLPFBW			0x18
+#define	BRTL8256REG_RXLPFBW			0x600
+
+#define	BRTL8258_TXLPFBW			0xc
+#define	BRTL8258_RXLPFBW			0xc00
+#define	BRTL8258_RSSILPFBW			0xc0
+
+#define	BBYTE0					0x1
+#define	BBYTE1					0x2
+#define	BBYTE2					0x4
+#define	BBYTE3					0x8
+#define	BWORD0					0x3
+#define	BWORD1					0xc
+#define	BWORD					0xf
+
+#define	MASKBYTE0				0xff
+#define	MASKBYTE1				0xff00
+#define	MASKBYTE2				0xff0000
+#define	MASKBYTE3				0xff000000
+#define	MASKHWORD				0xffff0000
+#define	MASKLWORD				0x0000ffff
+#define	MASKDWORD				0xffffffff
+#define	MASK12BITS				0xfff
+#define	MASKH4BITS				0xf0000000
+#define MASKOFDM_D				0xffc00000
+#define	MASKCCK					0x3f3f3f3f
+
+#define	MASK4BITS				0x0f
+#define	MASK20BITS				0xfffff
+#define RFREG_OFFSET_MASK			0xfffff
+
+#define	BENABLE					0x1
+#define	BDISABLE				0x0
+
+#define	LEFT_ANTENNA				0x0
+#define	RIGHT_ANTENNA				0x1
+
+#define	TCHECK_TXSTATUS				500
+#define	TUPDATE_RXCOUNTER			100
+
+#define	REG_UN_USED_REGISTER			0x01bf
+
+/* WOL bit information */
+#define	HAL92C_WOL_PTK_UPDATE_EVENT		BIT(0)
+#define	HAL92C_WOL_GTK_UPDATE_EVENT		BIT(1)
+#define	HAL92C_WOL_DISASSOC_EVENT		BIT(2)
+#define	HAL92C_WOL_DEAUTH_EVENT			BIT(3)
+#define	HAL92C_WOL_FW_DISCONNECT_EVENT		BIT(4)
+
+#define		WOL_REASON_PTK_UPDATE		BIT(0)
+#define		WOL_REASON_GTK_UPDATE		BIT(1)
+#define		WOL_REASON_DISASSOC		BIT(2)
+#define		WOL_REASON_DEAUTH		BIT(3)
+#define		WOL_REASON_FW_DISCONNECT	BIT(4)
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/rf.c b/drivers/net/wireless/rtlwifi/rtl8188ee/rf.c
new file mode 100644
index 000000000000..4faafdbab9c6
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/rf.c
@@ -0,0 +1,467 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "rf.h"
+#include "dm.h"
+
+void rtl88e_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	switch (bandwidth) {
+	case HT_CHANNEL_WIDTH_20:
+		rtlphy->rfreg_chnlval[0] = ((rtlphy->rfreg_chnlval[0] &
+					     0xfffff3ff) | BIT(10) | BIT(11));
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_CHNLBW, RFREG_OFFSET_MASK,
+			      rtlphy->rfreg_chnlval[0]);
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		rtlphy->rfreg_chnlval[0] = ((rtlphy->rfreg_chnlval[0] &
+					     0xfffff3ff) | BIT(10));
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_CHNLBW, RFREG_OFFSET_MASK,
+			      rtlphy->rfreg_chnlval[0]);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "unknown bandwidth: %#X\n", bandwidth);
+		break;
+	}
+}
+
+void rtl88e_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
+				       u8 *plevel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u32 tx_agc[2] = {0, 0}, tmpval;
+	bool turbo_scanoff = false;
+	u8 idx1, idx2;
+	u8 *ptr;
+	u8 direction;
+	u32 pwrtrac_value;
+
+	if (rtlefuse->eeprom_regulatory != 0)
+		turbo_scanoff = true;
+
+	if (mac->act_scanning == true) {
+		tx_agc[RF90_PATH_A] = 0x3f3f3f3f;
+		tx_agc[RF90_PATH_B] = 0x3f3f3f3f;
+
+		if (turbo_scanoff) {
+			for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
+				tx_agc[idx1] = plevel[idx1] |
+					       (plevel[idx1] << 8) |
+					       (plevel[idx1] << 16) |
+					       (plevel[idx1] << 24);
+			}
+		}
+	} else {
+		for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
+			tx_agc[idx1] = plevel[idx1] | (plevel[idx1] << 8) |
+				       (plevel[idx1] << 16) |
+				       (plevel[idx1] << 24);
+		}
+
+		if (rtlefuse->eeprom_regulatory == 0) {
+			tmpval = (rtlphy->mcs_offset[0][6]) +
+				 (rtlphy->mcs_offset[0][7] << 8);
+			tx_agc[RF90_PATH_A] += tmpval;
+
+			tmpval = (rtlphy->mcs_offset[0][14]) +
+				 (rtlphy->mcs_offset[0][15] << 24);
+			tx_agc[RF90_PATH_B] += tmpval;
+		}
+	}
+
+	for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
+		ptr = (u8 *)(&(tx_agc[idx1]));
+		for (idx2 = 0; idx2 < 4; idx2++) {
+			if (*ptr > RF6052_MAX_TX_PWR)
+				*ptr = RF6052_MAX_TX_PWR;
+			ptr++;
+		}
+	}
+	rtl88e_dm_txpower_track_adjust(hw, 1, &direction, &pwrtrac_value);
+	if (direction == 1) {
+		tx_agc[0] += pwrtrac_value;
+		tx_agc[1] += pwrtrac_value;
+	} else if (direction == 2) {
+		tx_agc[0] -= pwrtrac_value;
+		tx_agc[1] -= pwrtrac_value;
+	}
+	tmpval = tx_agc[RF90_PATH_A] & 0xff;
+	rtl_set_bbreg(hw, RTXAGC_A_CCK1_MCS32, MASKBYTE1, tmpval);
+
+	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+		"CCK PWR 1M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
+		RTXAGC_A_CCK1_MCS32);
+
+	tmpval = tx_agc[RF90_PATH_A] >> 8;
+
+	rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, 0xffffff00, tmpval);
+
+	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+		"CCK PWR 2~11M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
+		 RTXAGC_B_CCK11_A_CCK2_11);
+
+	tmpval = tx_agc[RF90_PATH_B] >> 24;
+	rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, MASKBYTE0, tmpval);
+
+	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+		"CCK PWR 11M (rf-B) = 0x%x (reg 0x%x)\n", tmpval,
+		 RTXAGC_B_CCK11_A_CCK2_11);
+
+	tmpval = tx_agc[RF90_PATH_B] & 0x00ffffff;
+	rtl_set_bbreg(hw, RTXAGC_B_CCK1_55_MCS32, 0xffffff00, tmpval);
+
+	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+		"CCK PWR 1~5.5M (rf-B) = 0x%x (reg 0x%x)\n", tmpval,
+		 RTXAGC_B_CCK1_55_MCS32);
+}
+
+static void rtl88e_phy_get_power_base(struct ieee80211_hw *hw,
+				      u8 *pwrlvlofdm, u8 *pwrlvlbw20,
+				      u8 *pwrlvlbw40, u8 channel,
+				      u32 *ofdmbase, u32 *mcsbase)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 base0, base1;
+	u8 i, powerlevel[2];
+
+	for (i = 0; i < 2; i++) {
+		base0 = pwrlvlofdm[i];
+
+		base0 = (base0 << 24) | (base0 << 16) |
+			     (base0 << 8) | base0;
+		*(ofdmbase + i) = base0;
+		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+			"[OFDM power base index rf(%c) = 0x%x]\n",
+			((i == 0) ? 'A' : 'B'), *(ofdmbase + i));
+	}
+
+	for (i = 0; i < 2; i++) {
+		if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20)
+			powerlevel[i] = pwrlvlbw20[i];
+		else
+			powerlevel[i] = pwrlvlbw40[i];
+		base1 = powerlevel[i];
+		base1 = (base1 << 24) |
+		    (base1 << 16) | (base1 << 8) | base1;
+
+		*(mcsbase + i) = base1;
+
+		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+			"[MCS power base index rf(%c) = 0x%x]\n",
+			((i == 0) ? 'A' : 'B'), *(mcsbase + i));
+	}
+}
+
+static void get_txpwr_by_reg(struct ieee80211_hw *hw, u8 chan, u8 index,
+			     u32 *base0, u32 *base1, u32 *outval)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 i, chg = 0, pwr_lim[4], pwr_diff = 0, cust_pwr_dif;
+	u32 writeval, cust_lim, rf, tmp;
+	u8 ch = chan - 1;
+	u8 j;
+
+	for (rf = 0; rf < 2; rf++) {
+		j = index + (rf ? 8 : 0);
+		tmp = ((index < 2) ? base0[rf] : base1[rf]);
+		switch (rtlefuse->eeprom_regulatory) {
+		case 0:
+			chg = 0;
+
+			writeval = rtlphy->mcs_offset[chg][j] + tmp;
+
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				"RTK better performance, "
+				"writeval(%c) = 0x%x\n",
+				((rf == 0) ? 'A' : 'B'), writeval);
+			break;
+		case 1:
+			if (rtlphy->pwrgroup_cnt == 1) {
+				chg = 0;
+			} else {
+				chg = chan / 3;
+				if (chan == 14)
+					chg = 5;
+			}
+			writeval = rtlphy->mcs_offset[chg][j] + tmp;
+
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				"Realtek regulatory, 20MHz, writeval(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeval);
+			break;
+		case 2:
+			writeval = ((index < 2) ? base0[rf] : base1[rf]);
+
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				"Better regulatory, writeval(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeval);
+			break;
+		case 3:
+			chg = 0;
+
+			if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
+				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+					"customer's limit, 40MHz rf(%c) = 0x%x\n",
+					 ((rf == 0) ? 'A' : 'B'),
+					 rtlefuse->pwrgroup_ht40[rf][ch]);
+			} else {
+				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+					"customer's limit, 20MHz rf(%c) = 0x%x\n",
+					 ((rf == 0) ? 'A' : 'B'),
+					 rtlefuse->pwrgroup_ht20[rf][ch]);
+			}
+
+			if (index < 2)
+				pwr_diff = rtlefuse->txpwr_legacyhtdiff[rf][ch];
+			else if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20)
+				pwr_diff = rtlefuse->txpwr_ht20diff[rf][ch];
+
+			if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40)
+				cust_pwr_dif = rtlefuse->pwrgroup_ht40[rf][ch];
+			else
+				cust_pwr_dif = rtlefuse->pwrgroup_ht20[rf][ch];
+
+			if (pwr_diff > cust_pwr_dif)
+				pwr_diff = 0;
+			else
+				pwr_diff = cust_pwr_dif - pwr_diff;
+
+			for (i = 0; i < 4; i++) {
+				pwr_lim[i] = (u8)((rtlphy->mcs_offset[chg][j] &
+					     (0x7f << (i * 8))) >> (i * 8));
+
+				if (pwr_lim[i] > pwr_diff)
+					pwr_lim[i] = pwr_diff;
+			}
+
+			cust_lim = (pwr_lim[3] << 24) | (pwr_lim[2] << 16) |
+				   (pwr_lim[1] << 8) | (pwr_lim[0]);
+
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				"Customer's limit rf(%c) = 0x%x\n",
+				((rf == 0) ? 'A' : 'B'), cust_lim);
+
+			writeval = cust_lim + tmp;
+
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				"Customer, writeval rf(%c) = 0x%x\n",
+				((rf == 0) ? 'A' : 'B'), writeval);
+			break;
+		default:
+			chg = 0;
+			writeval = rtlphy->mcs_offset[chg][j] + tmp;
+
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				"RTK better performance, writeval "
+				"rf(%c) = 0x%x\n",
+				((rf == 0) ? 'A' : 'B'), writeval);
+			break;
+		}
+
+		if (rtlpriv->dm.dynamic_txhighpower_lvl == TXHIGHPWRLEVEL_BT1)
+			writeval = writeval - 0x06060606;
+		else if (rtlpriv->dm.dynamic_txhighpower_lvl ==
+			 TXHIGHPWRLEVEL_BT2)
+			writeval -= 0x0c0c0c0c;
+		*(outval + rf) = writeval;
+	}
+}
+
+static void write_ofdm_pwr(struct ieee80211_hw *hw, u8 index, u32 *pvalue)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 regoffset_a[6] = {
+		RTXAGC_A_RATE18_06, RTXAGC_A_RATE54_24,
+		RTXAGC_A_MCS03_MCS00, RTXAGC_A_MCS07_MCS04,
+		RTXAGC_A_MCS11_MCS08, RTXAGC_A_MCS15_MCS12
+	};
+	u16 regoffset_b[6] = {
+		RTXAGC_B_RATE18_06, RTXAGC_B_RATE54_24,
+		RTXAGC_B_MCS03_MCS00, RTXAGC_B_MCS07_MCS04,
+		RTXAGC_B_MCS11_MCS08, RTXAGC_B_MCS15_MCS12
+	};
+	u8 i, rf, pwr_val[4];
+	u32 writeval;
+	u16 regoffset;
+
+	for (rf = 0; rf < 2; rf++) {
+		writeval = pvalue[rf];
+		for (i = 0; i < 4; i++) {
+			pwr_val[i] = (u8) ((writeval & (0x7f <<
+				     (i * 8))) >> (i * 8));
+
+			if (pwr_val[i] > RF6052_MAX_TX_PWR)
+				pwr_val[i] = RF6052_MAX_TX_PWR;
+		}
+		writeval = (pwr_val[3] << 24) | (pwr_val[2] << 16) |
+			   (pwr_val[1] << 8) | pwr_val[0];
+
+		if (rf == 0)
+			regoffset = regoffset_a[index];
+		else
+			regoffset = regoffset_b[index];
+		rtl_set_bbreg(hw, regoffset, MASKDWORD, writeval);
+
+		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+			"Set 0x%x = %08x\n", regoffset, writeval);
+	}
+}
+
+void rtl88e_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
+					u8 *pwrlvlofdm,
+					u8 *pwrlvlbw20,
+					u8 *pwrlvlbw40, u8 chan)
+{
+	u32 writeval[2], base0[2], base1[2];
+	u8 index;
+	u8 direction;
+	u32 pwrtrac_value;
+
+	rtl88e_phy_get_power_base(hw, pwrlvlofdm, pwrlvlbw20,
+				  pwrlvlbw40, chan, &base0[0],
+				  &base1[0]);
+
+	rtl88e_dm_txpower_track_adjust(hw, 1, &direction, &pwrtrac_value);
+
+	for (index = 0; index < 6; index++) {
+		get_txpwr_by_reg(hw, chan, index, &base0[0], &base1[0],
+				 &writeval[0]);
+		if (direction == 1) {
+			writeval[0] += pwrtrac_value;
+			writeval[1] += pwrtrac_value;
+		} else if (direction == 2) {
+			writeval[0] -= pwrtrac_value;
+			writeval[1] -= pwrtrac_value;
+		}
+		write_ofdm_pwr(hw, index, &writeval[0]);
+	}
+}
+
+static bool rf6052_conf_para(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 u4val = 0;
+	u8 rfpath;
+	bool rtstatus = true;
+	struct bb_reg_def *pphyreg;
+
+	for (rfpath = 0; rfpath < rtlphy->num_total_rfpath; rfpath++) {
+		pphyreg = &rtlphy->phyreg_def[rfpath];
+
+		switch (rfpath) {
+		case RF90_PATH_A:
+		case RF90_PATH_C:
+			u4val = rtl_get_bbreg(hw, pphyreg->rfintfs,
+						    BRFSI_RFENV);
+			break;
+		case RF90_PATH_B:
+		case RF90_PATH_D:
+			u4val = rtl_get_bbreg(hw, pphyreg->rfintfs,
+						    BRFSI_RFENV << 16);
+			break;
+		}
+
+		rtl_set_bbreg(hw, pphyreg->rfintfe, BRFSI_RFENV << 16, 0x1);
+		udelay(1);
+
+		rtl_set_bbreg(hw, pphyreg->rfintfo, BRFSI_RFENV, 0x1);
+		udelay(1);
+
+		rtl_set_bbreg(hw, pphyreg->rfhssi_para2,
+			      B3WIREADDREAALENGTH, 0x0);
+		udelay(1);
+
+		rtl_set_bbreg(hw, pphyreg->rfhssi_para2, B3WIREDATALENGTH, 0x0);
+		udelay(1);
+
+		switch (rfpath) {
+		case RF90_PATH_A:
+			rtstatus = rtl88e_phy_config_rf_with_headerfile(hw,
+					(enum radio_path)rfpath);
+			break;
+		case RF90_PATH_B:
+			rtstatus = rtl88e_phy_config_rf_with_headerfile(hw,
+					(enum radio_path)rfpath);
+			break;
+		case RF90_PATH_C:
+			break;
+		case RF90_PATH_D:
+			break;
+		}
+
+		switch (rfpath) {
+		case RF90_PATH_A:
+		case RF90_PATH_C:
+			rtl_set_bbreg(hw, pphyreg->rfintfs, BRFSI_RFENV, u4val);
+			break;
+		case RF90_PATH_B:
+		case RF90_PATH_D:
+			rtl_set_bbreg(hw, pphyreg->rfintfs, BRFSI_RFENV << 16,
+				      u4val);
+			break;
+		}
+
+		if (rtstatus != true) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+				 "Radio[%d] Fail!!", rfpath);
+			return false;
+		}
+	}
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "\n");
+	return rtstatus;
+}
+
+bool rtl88e_phy_rf6052_config(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (rtlphy->rf_type == RF_1T1R)
+		rtlphy->num_total_rfpath = 1;
+	else
+		rtlphy->num_total_rfpath = 2;
+
+	return rf6052_conf_para(hw);
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/rf.h b/drivers/net/wireless/rtlwifi/rtl8188ee/rf.h
new file mode 100644
index 000000000000..a39a2a3dbcc9
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/rf.h
@@ -0,0 +1,46 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92C_RF_H__
+#define __RTL92C_RF_H__
+
+#define RF6052_MAX_TX_PWR		0x3F
+#define RF6052_MAX_REG			0x3F
+
+void rtl88e_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw,
+				     u8 bandwidth);
+void rtl88e_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
+				       u8 *ppowerlevel);
+void rtl88e_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
+					u8 *ppowerlevel_ofdm,
+					u8 *ppowerlevel_bw20,
+					u8 *ppowerlevel_bw40, u8 channel);
+bool rtl88e_phy_rf6052_config(struct ieee80211_hw *hw);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/sw.c b/drivers/net/wireless/rtlwifi/rtl8188ee/sw.c
new file mode 100644
index 000000000000..c254693a1e6a
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/sw.c
@@ -0,0 +1,400 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../core.h"
+#include "../pci.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "dm.h"
+#include "hw.h"
+#include "sw.h"
+#include "trx.h"
+#include "led.h"
+#include "table.h"
+
+#include <linux/vmalloc.h>
+#include <linux/module.h>
+
+static void rtl88e_init_aspm_vars(struct ieee80211_hw *hw)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	/*close ASPM for AMD defaultly */
+	rtlpci->const_amdpci_aspm = 0;
+
+	/* ASPM PS mode.
+	 * 0 - Disable ASPM,
+	 * 1 - Enable ASPM without Clock Req,
+	 * 2 - Enable ASPM with Clock Req,
+	 * 3 - Alwyas Enable ASPM with Clock Req,
+	 * 4 - Always Enable ASPM without Clock Req.
+	 * set defult to RTL8192CE:3 RTL8192E:2
+	 */
+	rtlpci->const_pci_aspm = 3;
+
+	/*Setting for PCI-E device */
+	rtlpci->const_devicepci_aspm_setting = 0x03;
+
+	/*Setting for PCI-E bridge */
+	rtlpci->const_hostpci_aspm_setting = 0x02;
+
+	/* In Hw/Sw Radio Off situation.
+	 * 0 - Default,
+	 * 1 - From ASPM setting without low Mac Pwr,
+	 * 2 - From ASPM setting with low Mac Pwr,
+	 * 3 - Bus D3
+	 * set default to RTL8192CE:0 RTL8192SE:2
+	 */
+	rtlpci->const_hwsw_rfoff_d3 = 0;
+
+	/* This setting works for those device with
+	 * backdoor ASPM setting such as EPHY setting.
+	 * 0 - Not support ASPM,
+	 * 1 - Support ASPM,
+	 * 2 - According to chipset.
+	 */
+	rtlpci->const_support_pciaspm = 1;
+}
+
+int rtl88e_init_sw_vars(struct ieee80211_hw *hw)
+{
+	int err = 0;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u8 tid;
+
+	rtl8188ee_bt_reg_init(hw);
+
+	rtlpriv->dm.dm_initialgain_enable = 1;
+	rtlpriv->dm.dm_flag = 0;
+	rtlpriv->dm.disable_framebursting = 0;
+	rtlpriv->dm.thermalvalue = 0;
+	rtlpci->transmit_config = CFENDFORM | BIT(15);
+
+	/* compatible 5G band 88ce just 2.4G band & smsp */
+	rtlpriv->rtlhal.current_bandtype = BAND_ON_2_4G;
+	rtlpriv->rtlhal.bandset = BAND_ON_2_4G;
+	rtlpriv->rtlhal.macphymode = SINGLEMAC_SINGLEPHY;
+
+	rtlpci->receive_config = (RCR_APPFCS |
+				  RCR_APP_MIC |
+				  RCR_APP_ICV |
+				  RCR_APP_PHYST_RXFF |
+				  RCR_HTC_LOC_CTRL |
+				  RCR_AMF |
+				  RCR_ACF |
+				  RCR_ADF |
+				  RCR_AICV |
+				  RCR_ACRC32 |
+				  RCR_AB |
+				  RCR_AM |
+				  RCR_APM |
+				  0);
+
+	rtlpci->irq_mask[0] =
+				(u32) (IMR_PSTIMEOUT	|
+				IMR_HSISR_IND_ON_INT	|
+				IMR_C2HCMD		|
+				IMR_HIGHDOK		|
+				IMR_MGNTDOK		|
+				IMR_BKDOK		|
+				IMR_BEDOK		|
+				IMR_VIDOK		|
+				IMR_VODOK		|
+				IMR_RDU			|
+				IMR_ROK			|
+				0);
+	rtlpci->irq_mask[1] = (u32) (IMR_RXFOVW | 0);
+	rtlpci->sys_irq_mask = (u32) (HSIMR_PDN_INT_EN | HSIMR_RON_INT_EN);
+
+	/* for debug level */
+	rtlpriv->dbg.global_debuglevel = rtlpriv->cfg->mod_params->debug;
+	/* for LPS & IPS */
+	rtlpriv->psc.inactiveps = rtlpriv->cfg->mod_params->inactiveps;
+	rtlpriv->psc.swctrl_lps = rtlpriv->cfg->mod_params->swctrl_lps;
+	rtlpriv->psc.fwctrl_lps = rtlpriv->cfg->mod_params->fwctrl_lps;
+	if (!rtlpriv->psc.inactiveps)
+		pr_info("rtl8188ee: Power Save off (module option)\n");
+	if (!rtlpriv->psc.fwctrl_lps)
+		pr_info("rtl8188ee: FW Power Save off (module option)\n");
+	rtlpriv->psc.reg_fwctrl_lps = 3;
+	rtlpriv->psc.reg_max_lps_awakeintvl = 5;
+	/* for ASPM, you can close aspm through
+	 * set const_support_pciaspm = 0
+	 */
+	rtl88e_init_aspm_vars(hw);
+
+	if (rtlpriv->psc.reg_fwctrl_lps == 1)
+		rtlpriv->psc.fwctrl_psmode = FW_PS_MIN_MODE;
+	else if (rtlpriv->psc.reg_fwctrl_lps == 2)
+		rtlpriv->psc.fwctrl_psmode = FW_PS_MAX_MODE;
+	else if (rtlpriv->psc.reg_fwctrl_lps == 3)
+		rtlpriv->psc.fwctrl_psmode = FW_PS_DTIM_MODE;
+
+	/* for firmware buf */
+	rtlpriv->rtlhal.pfirmware = vmalloc(0x8000);
+	if (!rtlpriv->rtlhal.pfirmware) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Can't alloc buffer for fw.\n");
+		return 1;
+	}
+
+	rtlpriv->cfg->fw_name = "rtlwifi/rtl8188efw.bin";
+	rtlpriv->max_fw_size = 0x8000;
+	pr_info("Using firmware %s\n", rtlpriv->cfg->fw_name);
+	err = request_firmware_nowait(THIS_MODULE, 1, rtlpriv->cfg->fw_name,
+				      rtlpriv->io.dev, GFP_KERNEL, hw,
+				      rtl_fw_cb);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Failed to request firmware!\n");
+		return 1;
+	}
+
+	/* for early mode */
+	rtlpriv->rtlhal.earlymode_enable = false;
+	rtlpriv->rtlhal.max_earlymode_num = 10;
+	for (tid = 0; tid < 8; tid++)
+		skb_queue_head_init(&rtlpriv->mac80211.skb_waitq[tid]);
+
+	/*low power */
+	rtlpriv->psc.low_power_enable = false;
+	if (rtlpriv->psc.low_power_enable) {
+		init_timer(&rtlpriv->works.fw_clockoff_timer);
+		setup_timer(&rtlpriv->works.fw_clockoff_timer,
+			    rtl88ee_fw_clk_off_timer_callback,
+			    (unsigned long)hw);
+	}
+
+	init_timer(&rtlpriv->works.fast_antenna_training_timer);
+	setup_timer(&rtlpriv->works.fast_antenna_training_timer,
+		    rtl88e_dm_fast_antenna_training_callback,
+		    (unsigned long)hw);
+	return err;
+}
+
+void rtl88e_deinit_sw_vars(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->rtlhal.pfirmware) {
+		vfree(rtlpriv->rtlhal.pfirmware);
+		rtlpriv->rtlhal.pfirmware = NULL;
+	}
+
+	if (rtlpriv->psc.low_power_enable)
+		del_timer_sync(&rtlpriv->works.fw_clockoff_timer);
+
+	del_timer_sync(&rtlpriv->works.fast_antenna_training_timer);
+}
+
+static struct rtl_hal_ops rtl8188ee_hal_ops = {
+	.init_sw_vars = rtl88e_init_sw_vars,
+	.deinit_sw_vars = rtl88e_deinit_sw_vars,
+	.read_eeprom_info = rtl88ee_read_eeprom_info,
+	.interrupt_recognized = rtl88ee_interrupt_recognized,/*need check*/
+	.hw_init = rtl88ee_hw_init,
+	.hw_disable = rtl88ee_card_disable,
+	.hw_suspend = rtl88ee_suspend,
+	.hw_resume = rtl88ee_resume,
+	.enable_interrupt = rtl88ee_enable_interrupt,
+	.disable_interrupt = rtl88ee_disable_interrupt,
+	.set_network_type = rtl88ee_set_network_type,
+	.set_chk_bssid = rtl88ee_set_check_bssid,
+	.set_qos = rtl88ee_set_qos,
+	.set_bcn_reg = rtl88ee_set_beacon_related_registers,
+	.set_bcn_intv = rtl88ee_set_beacon_interval,
+	.update_interrupt_mask = rtl88ee_update_interrupt_mask,
+	.get_hw_reg = rtl88ee_get_hw_reg,
+	.set_hw_reg = rtl88ee_set_hw_reg,
+	.update_rate_tbl = rtl88ee_update_hal_rate_tbl,
+	.fill_tx_desc = rtl88ee_tx_fill_desc,
+	.fill_tx_cmddesc = rtl88ee_tx_fill_cmddesc,
+	.query_rx_desc = rtl88ee_rx_query_desc,
+	.set_channel_access = rtl88ee_update_channel_access_setting,
+	.radio_onoff_checking = rtl88ee_gpio_radio_on_off_checking,
+	.set_bw_mode = rtl88e_phy_set_bw_mode,
+	.switch_channel = rtl88e_phy_sw_chnl,
+	.dm_watchdog = rtl88e_dm_watchdog,
+	.scan_operation_backup = rtl88e_phy_scan_operation_backup,
+	.set_rf_power_state = rtl88e_phy_set_rf_power_state,
+	.led_control = rtl88ee_led_control,
+	.set_desc = rtl88ee_set_desc,
+	.get_desc = rtl88ee_get_desc,
+	.tx_polling = rtl88ee_tx_polling,
+	.enable_hw_sec = rtl88ee_enable_hw_security_config,
+	.set_key = rtl88ee_set_key,
+	.init_sw_leds = rtl88ee_init_sw_leds,
+	.allow_all_destaddr = rtl88ee_allow_all_destaddr,
+	.get_bbreg = rtl88e_phy_query_bb_reg,
+	.set_bbreg = rtl88e_phy_set_bb_reg,
+	.get_rfreg = rtl88e_phy_query_rf_reg,
+	.set_rfreg = rtl88e_phy_set_rf_reg,
+};
+
+static struct rtl_mod_params rtl88ee_mod_params = {
+	.sw_crypto = false,
+	.inactiveps = true,
+	.swctrl_lps = false,
+	.fwctrl_lps = true,
+	.debug = DBG_EMERG,
+};
+
+static struct rtl_hal_cfg rtl88ee_hal_cfg = {
+	.bar_id = 2,
+	.write_readback = true,
+	.name = "rtl88e_pci",
+	.ops = &rtl8188ee_hal_ops,
+	.mod_params = &rtl88ee_mod_params,
+
+	.maps[SYS_ISO_CTRL] = REG_SYS_ISO_CTRL,
+	.maps[SYS_FUNC_EN] = REG_SYS_FUNC_EN,
+	.maps[SYS_CLK] = REG_SYS_CLKR,
+	.maps[MAC_RCR_AM] = AM,
+	.maps[MAC_RCR_AB] = AB,
+	.maps[MAC_RCR_ACRC32] = ACRC32,
+	.maps[MAC_RCR_ACF] = ACF,
+	.maps[MAC_RCR_AAP] = AAP,
+
+	.maps[EFUSE_ACCESS] = REG_EFUSE_ACCESS,
+
+	.maps[EFUSE_TEST] = REG_EFUSE_TEST,
+	.maps[EFUSE_CTRL] = REG_EFUSE_CTRL,
+	.maps[EFUSE_CLK] = 0,
+	.maps[EFUSE_CLK_CTRL] = REG_EFUSE_CTRL,
+	.maps[EFUSE_PWC_EV12V] = PWC_EV12V,
+	.maps[EFUSE_FEN_ELDR] = FEN_ELDR,
+	.maps[EFUSE_LOADER_CLK_EN] = LOADER_CLK_EN,
+	.maps[EFUSE_ANA8M] = ANA8M,
+	.maps[EFUSE_HWSET_MAX_SIZE] = HWSET_MAX_SIZE,
+	.maps[EFUSE_MAX_SECTION_MAP] = EFUSE_MAX_SECTION,
+	.maps[EFUSE_REAL_CONTENT_SIZE] = EFUSE_REAL_CONTENT_LEN,
+	.maps[EFUSE_OOB_PROTECT_BYTES_LEN] = EFUSE_OOB_PROTECT_BYTES,
+
+	.maps[RWCAM] = REG_CAMCMD,
+	.maps[WCAMI] = REG_CAMWRITE,
+	.maps[RCAMO] = REG_CAMREAD,
+	.maps[CAMDBG] = REG_CAMDBG,
+	.maps[SECR] = REG_SECCFG,
+	.maps[SEC_CAM_NONE] = CAM_NONE,
+	.maps[SEC_CAM_WEP40] = CAM_WEP40,
+	.maps[SEC_CAM_TKIP] = CAM_TKIP,
+	.maps[SEC_CAM_AES] = CAM_AES,
+	.maps[SEC_CAM_WEP104] = CAM_WEP104,
+
+	.maps[RTL_IMR_BCNDMAINT6] = IMR_BCNDMAINT6,
+	.maps[RTL_IMR_BCNDMAINT5] = IMR_BCNDMAINT5,
+	.maps[RTL_IMR_BCNDMAINT4] = IMR_BCNDMAINT4,
+	.maps[RTL_IMR_BCNDMAINT3] = IMR_BCNDMAINT3,
+	.maps[RTL_IMR_BCNDMAINT2] = IMR_BCNDMAINT2,
+	.maps[RTL_IMR_BCNDMAINT1] = IMR_BCNDMAINT1,
+/*	.maps[RTL_IMR_BCNDOK8] = IMR_BCNDOK8,     */   /*need check*/
+	.maps[RTL_IMR_BCNDOK7] = IMR_BCNDOK7,
+	.maps[RTL_IMR_BCNDOK6] = IMR_BCNDOK6,
+	.maps[RTL_IMR_BCNDOK5] = IMR_BCNDOK5,
+	.maps[RTL_IMR_BCNDOK4] = IMR_BCNDOK4,
+	.maps[RTL_IMR_BCNDOK3] = IMR_BCNDOK3,
+	.maps[RTL_IMR_BCNDOK2] = IMR_BCNDOK2,
+	.maps[RTL_IMR_BCNDOK1] = IMR_BCNDOK1,
+/*	.maps[RTL_IMR_TIMEOUT2] = IMR_TIMEOUT2,*/
+/*	.maps[RTL_IMR_TIMEOUT1] = IMR_TIMEOUT1,*/
+
+	.maps[RTL_IMR_TXFOVW] = IMR_TXFOVW,
+	.maps[RTL_IMR_PSTIMEOUT] = IMR_PSTIMEOUT,
+	.maps[RTL_IMR_BCNINT] = IMR_BCNDMAINT0,
+	.maps[RTL_IMR_RXFOVW] = IMR_RXFOVW,
+	.maps[RTL_IMR_RDU] = IMR_RDU,
+	.maps[RTL_IMR_ATIMEND] = IMR_ATIMEND,
+	.maps[RTL_IMR_BDOK] = IMR_BCNDOK0,
+	.maps[RTL_IMR_MGNTDOK] = IMR_MGNTDOK,
+	.maps[RTL_IMR_TBDER] = IMR_TBDER,
+	.maps[RTL_IMR_HIGHDOK] = IMR_HIGHDOK,
+	.maps[RTL_IMR_TBDOK] = IMR_TBDOK,
+	.maps[RTL_IMR_BKDOK] = IMR_BKDOK,
+	.maps[RTL_IMR_BEDOK] = IMR_BEDOK,
+	.maps[RTL_IMR_VIDOK] = IMR_VIDOK,
+	.maps[RTL_IMR_VODOK] = IMR_VODOK,
+	.maps[RTL_IMR_ROK] = IMR_ROK,
+	.maps[RTL_IBSS_INT_MASKS] = (IMR_BCNDMAINT0 | IMR_TBDOK | IMR_TBDER),
+
+	.maps[RTL_RC_CCK_RATE1M] = DESC92C_RATE1M,
+	.maps[RTL_RC_CCK_RATE2M] = DESC92C_RATE2M,
+	.maps[RTL_RC_CCK_RATE5_5M] = DESC92C_RATE5_5M,
+	.maps[RTL_RC_CCK_RATE11M] = DESC92C_RATE11M,
+	.maps[RTL_RC_OFDM_RATE6M] = DESC92C_RATE6M,
+	.maps[RTL_RC_OFDM_RATE9M] = DESC92C_RATE9M,
+	.maps[RTL_RC_OFDM_RATE12M] = DESC92C_RATE12M,
+	.maps[RTL_RC_OFDM_RATE18M] = DESC92C_RATE18M,
+	.maps[RTL_RC_OFDM_RATE24M] = DESC92C_RATE24M,
+	.maps[RTL_RC_OFDM_RATE36M] = DESC92C_RATE36M,
+	.maps[RTL_RC_OFDM_RATE48M] = DESC92C_RATE48M,
+	.maps[RTL_RC_OFDM_RATE54M] = DESC92C_RATE54M,
+
+	.maps[RTL_RC_HT_RATEMCS7] = DESC92C_RATEMCS7,
+	.maps[RTL_RC_HT_RATEMCS15] = DESC92C_RATEMCS15,
+};
+
+static DEFINE_PCI_DEVICE_TABLE(rtl88ee_pci_ids) = {
+	{RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x8179, rtl88ee_hal_cfg)},
+	{},
+};
+
+MODULE_DEVICE_TABLE(pci, rtl88ee_pci_ids);
+
+MODULE_AUTHOR("zhiyuan_yang	<zhiyuan_yang@realsil.com.cn>");
+MODULE_AUTHOR("Realtek WlanFAE	<wlanfae@realtek.com>");
+MODULE_AUTHOR("Larry Finger	<Larry.Finger@lwfinger.net>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Realtek 8188E 802.11n PCI wireless");
+MODULE_FIRMWARE("rtlwifi/rtl8188efw.bin");
+
+module_param_named(swenc, rtl88ee_mod_params.sw_crypto, bool, 0444);
+module_param_named(debug, rtl88ee_mod_params.debug, int, 0444);
+module_param_named(ips, rtl88ee_mod_params.inactiveps, bool, 0444);
+module_param_named(swlps, rtl88ee_mod_params.swctrl_lps, bool, 0444);
+module_param_named(fwlps, rtl88ee_mod_params.fwctrl_lps, bool, 0444);
+MODULE_PARM_DESC(swenc, "Set to 1 for software crypto (default 0)\n");
+MODULE_PARM_DESC(ips, "Set to 0 to not use link power save (default 1)\n");
+MODULE_PARM_DESC(swlps, "Set to 1 to use SW control power save (default 0)\n");
+MODULE_PARM_DESC(fwlps, "Set to 1 to use FW control power save (default 1)\n");
+MODULE_PARM_DESC(debug, "Set debug level (0-5) (default 0)");
+
+static SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, rtl_pci_suspend, rtl_pci_resume);
+
+static struct pci_driver rtl88ee_driver = {
+	.name = KBUILD_MODNAME,
+	.id_table = rtl88ee_pci_ids,
+	.probe = rtl_pci_probe,
+	.remove = rtl_pci_disconnect,
+	.driver.pm = &rtlwifi_pm_ops,
+};
+
+module_pci_driver(rtl88ee_driver);
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/sw.h b/drivers/net/wireless/rtlwifi/rtl8188ee/sw.h
new file mode 100644
index 000000000000..85e02b3bdff8
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/sw.h
@@ -0,0 +1,36 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92CE_SW_H__
+#define __RTL92CE_SW_H__
+
+int rtl88e_init_sw_vars(struct ieee80211_hw *hw);
+void rtl88e_deinit_sw_vars(struct ieee80211_hw *hw);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/table.c b/drivers/net/wireless/rtlwifi/rtl8188ee/table.c
new file mode 100644
index 000000000000..fad373f97b2c
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/table.c
@@ -0,0 +1,643 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Created on  2010/ 5/18,  1:41
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "table.h"
+
+u32 RTL8188EEPHY_REG_1TARRAY[] = {
+		0x800, 0x80040000,
+		0x804, 0x00000003,
+		0x808, 0x0000FC00,
+		0x80C, 0x0000000A,
+		0x810, 0x10001331,
+		0x814, 0x020C3D10,
+		0x818, 0x02200385,
+		0x81C, 0x00000000,
+		0x820, 0x01000100,
+		0x824, 0x00390204,
+		0x828, 0x00000000,
+		0x82C, 0x00000000,
+		0x830, 0x00000000,
+		0x834, 0x00000000,
+		0x838, 0x00000000,
+		0x83C, 0x00000000,
+		0x840, 0x00010000,
+		0x844, 0x00000000,
+		0x848, 0x00000000,
+		0x84C, 0x00000000,
+		0x850, 0x00000000,
+		0x854, 0x00000000,
+		0x858, 0x569A11A9,
+		0x85C, 0x01000014,
+		0x860, 0x66F60110,
+		0x864, 0x061F0649,
+		0x868, 0x00000000,
+		0x86C, 0x27272700,
+		0x870, 0x07000760,
+		0x874, 0x25004000,
+		0x878, 0x00000808,
+		0x87C, 0x00000000,
+		0x880, 0xB0000C1C,
+		0x884, 0x00000001,
+		0x888, 0x00000000,
+		0x88C, 0xCCC000C0,
+		0x890, 0x00000800,
+		0x894, 0xFFFFFFFE,
+		0x898, 0x40302010,
+		0x89C, 0x00706050,
+		0x900, 0x00000000,
+		0x904, 0x00000023,
+		0x908, 0x00000000,
+		0x90C, 0x81121111,
+		0x910, 0x00000002,
+		0x914, 0x00000201,
+		0xA00, 0x00D047C8,
+		0xA04, 0x80FF000C,
+		0xA08, 0x8C838300,
+		0xA0C, 0x2E7F120F,
+		0xA10, 0x9500BB78,
+		0xA14, 0x1114D028,
+		0xA18, 0x00881117,
+		0xA1C, 0x89140F00,
+		0xA20, 0x1A1B0000,
+		0xA24, 0x090E1317,
+		0xA28, 0x00000204,
+		0xA2C, 0x00D30000,
+		0xA70, 0x101FBF00,
+		0xA74, 0x00000007,
+		0xA78, 0x00000900,
+		0xA7C, 0x225B0606,
+		0xA80, 0x218075B1,
+		0xB2C, 0x80000000,
+		0xC00, 0x48071D40,
+		0xC04, 0x03A05611,
+		0xC08, 0x000000E4,
+		0xC0C, 0x6C6C6C6C,
+		0xC10, 0x08800000,
+		0xC14, 0x40000100,
+		0xC18, 0x08800000,
+		0xC1C, 0x40000100,
+		0xC20, 0x00000000,
+		0xC24, 0x00000000,
+		0xC28, 0x00000000,
+		0xC2C, 0x00000000,
+		0xC30, 0x69E9AC47,
+		0xC34, 0x469652AF,
+		0xC38, 0x49795994,
+		0xC3C, 0x0A97971C,
+		0xC40, 0x1F7C403F,
+		0xC44, 0x000100B7,
+		0xC48, 0xEC020107,
+		0xC4C, 0x007F037F,
+		0xC50, 0x69553420,
+		0xC54, 0x43BC0094,
+		0xC58, 0x00013169,
+		0xC5C, 0x00250492,
+		0xC60, 0x00000000,
+		0xC64, 0x7112848B,
+		0xC68, 0x47C00BFF,
+		0xC6C, 0x00000036,
+		0xC70, 0x2C7F000D,
+		0xC74, 0x020610DB,
+		0xC78, 0x0000001F,
+		0xC7C, 0x00B91612,
+		0xC80, 0x390000E4,
+		0xC84, 0x20F60000,
+		0xC88, 0x40000100,
+		0xC8C, 0x20200000,
+		0xC90, 0x00091521,
+		0xC94, 0x00000000,
+		0xC98, 0x00121820,
+		0xC9C, 0x00007F7F,
+		0xCA0, 0x00000000,
+		0xCA4, 0x000300A0,
+		0xCA8, 0x00000000,
+		0xCAC, 0x00000000,
+		0xCB0, 0x00000000,
+		0xCB4, 0x00000000,
+		0xCB8, 0x00000000,
+		0xCBC, 0x28000000,
+		0xCC0, 0x00000000,
+		0xCC4, 0x00000000,
+		0xCC8, 0x00000000,
+		0xCCC, 0x00000000,
+		0xCD0, 0x00000000,
+		0xCD4, 0x00000000,
+		0xCD8, 0x64B22427,
+		0xCDC, 0x00766932,
+		0xCE0, 0x00222222,
+		0xCE4, 0x00000000,
+		0xCE8, 0x37644302,
+		0xCEC, 0x2F97D40C,
+		0xD00, 0x00000740,
+		0xD04, 0x00020401,
+		0xD08, 0x0000907F,
+		0xD0C, 0x20010201,
+		0xD10, 0xA0633333,
+		0xD14, 0x3333BC43,
+		0xD18, 0x7A8F5B6F,
+		0xD2C, 0xCC979975,
+		0xD30, 0x00000000,
+		0xD34, 0x80608000,
+		0xD38, 0x00000000,
+		0xD3C, 0x00127353,
+		0xD40, 0x00000000,
+		0xD44, 0x00000000,
+		0xD48, 0x00000000,
+		0xD4C, 0x00000000,
+		0xD50, 0x6437140A,
+		0xD54, 0x00000000,
+		0xD58, 0x00000282,
+		0xD5C, 0x30032064,
+		0xD60, 0x4653DE68,
+		0xD64, 0x04518A3C,
+		0xD68, 0x00002101,
+		0xD6C, 0x2A201C16,
+		0xD70, 0x1812362E,
+		0xD74, 0x322C2220,
+		0xD78, 0x000E3C24,
+		0xE00, 0x2D2D2D2D,
+		0xE04, 0x2D2D2D2D,
+		0xE08, 0x0390272D,
+		0xE10, 0x2D2D2D2D,
+		0xE14, 0x2D2D2D2D,
+		0xE18, 0x2D2D2D2D,
+		0xE1C, 0x2D2D2D2D,
+		0xE28, 0x00000000,
+		0xE30, 0x1000DC1F,
+		0xE34, 0x10008C1F,
+		0xE38, 0x02140102,
+		0xE3C, 0x681604C2,
+		0xE40, 0x01007C00,
+		0xE44, 0x01004800,
+		0xE48, 0xFB000000,
+		0xE4C, 0x000028D1,
+		0xE50, 0x1000DC1F,
+		0xE54, 0x10008C1F,
+		0xE58, 0x02140102,
+		0xE5C, 0x28160D05,
+		0xE60, 0x00000008,
+		0xE68, 0x001B25A4,
+		0xE6C, 0x00C00014,
+		0xE70, 0x00C00014,
+		0xE74, 0x01000014,
+		0xE78, 0x01000014,
+		0xE7C, 0x01000014,
+		0xE80, 0x01000014,
+		0xE84, 0x00C00014,
+		0xE88, 0x01000014,
+		0xE8C, 0x00C00014,
+		0xED0, 0x00C00014,
+		0xED4, 0x00C00014,
+		0xED8, 0x00C00014,
+		0xEDC, 0x00000014,
+		0xEE0, 0x00000014,
+		0xEEC, 0x01C00014,
+		0xF14, 0x00000003,
+		0xF4C, 0x00000000,
+		0xF00, 0x00000300,
+
+};
+
+u32 RTL8188EEPHY_REG_ARRAY_PG[] = {
+		0xE00, 0xFFFFFFFF, 0x06070809,
+		0xE04, 0xFFFFFFFF, 0x02020405,
+		0xE08, 0x0000FF00, 0x00000006,
+		0x86C, 0xFFFFFF00, 0x00020400,
+		0xE10, 0xFFFFFFFF, 0x08090A0B,
+		0xE14, 0xFFFFFFFF, 0x01030607,
+		0xE18, 0xFFFFFFFF, 0x08090A0B,
+		0xE1C, 0xFFFFFFFF, 0x01030607,
+		0xE00, 0xFFFFFFFF, 0x00000000,
+		0xE04, 0xFFFFFFFF, 0x00000000,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x00000000,
+		0xE14, 0xFFFFFFFF, 0x00000000,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x02020202,
+		0xE04, 0xFFFFFFFF, 0x00020202,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x04040404,
+		0xE14, 0xFFFFFFFF, 0x00020404,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x02020202,
+		0xE04, 0xFFFFFFFF, 0x00020202,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x04040404,
+		0xE14, 0xFFFFFFFF, 0x00020404,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x00000000,
+		0xE04, 0xFFFFFFFF, 0x00000000,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x00000000,
+		0xE14, 0xFFFFFFFF, 0x00000000,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x02020202,
+		0xE04, 0xFFFFFFFF, 0x00020202,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x04040404,
+		0xE14, 0xFFFFFFFF, 0x00020404,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x00000000,
+		0xE04, 0xFFFFFFFF, 0x00000000,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x00000000,
+		0xE14, 0xFFFFFFFF, 0x00000000,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x00000000,
+		0xE04, 0xFFFFFFFF, 0x00000000,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x00000000,
+		0xE14, 0xFFFFFFFF, 0x00000000,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x00000000,
+		0xE04, 0xFFFFFFFF, 0x00000000,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x00000000,
+		0xE14, 0xFFFFFFFF, 0x00000000,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x00000000,
+		0xE04, 0xFFFFFFFF, 0x00000000,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x00000000,
+		0xE14, 0xFFFFFFFF, 0x00000000,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+		0xE00, 0xFFFFFFFF, 0x00000000,
+		0xE04, 0xFFFFFFFF, 0x00000000,
+		0xE08, 0x0000FF00, 0x00000000,
+		0x86C, 0xFFFFFF00, 0x00000000,
+		0xE10, 0xFFFFFFFF, 0x00000000,
+		0xE14, 0xFFFFFFFF, 0x00000000,
+		0xE18, 0xFFFFFFFF, 0x00000000,
+		0xE1C, 0xFFFFFFFF, 0x00000000,
+
+};
+
+u32 RTL8188EE_RADIOA_1TARRAY[] = {
+		0x000, 0x00030000,
+		0x008, 0x00084000,
+		0x018, 0x00000407,
+		0x019, 0x00000012,
+		0x01E, 0x00080009,
+		0x01F, 0x00000880,
+		0x02F, 0x0001A060,
+		0x03F, 0x00000000,
+		0x042, 0x000060C0,
+		0x057, 0x000D0000,
+		0x058, 0x000BE180,
+		0x067, 0x00001552,
+		0x083, 0x00000000,
+		0x0B0, 0x000FF8FC,
+		0x0B1, 0x00054400,
+		0x0B2, 0x000CCC19,
+		0x0B4, 0x00043003,
+		0x0B6, 0x0004953E,
+		0x0B7, 0x0001C718,
+		0x0B8, 0x000060FF,
+		0x0B9, 0x00080001,
+		0x0BA, 0x00040000,
+		0x0BB, 0x00000400,
+		0x0BF, 0x000C0000,
+		0x0C2, 0x00002400,
+		0x0C3, 0x00000009,
+		0x0C4, 0x00040C91,
+		0x0C5, 0x00099999,
+		0x0C6, 0x000000A3,
+		0x0C7, 0x00088820,
+		0x0C8, 0x00076C06,
+		0x0C9, 0x00000000,
+		0x0CA, 0x00080000,
+		0x0DF, 0x00000180,
+		0x0EF, 0x000001A0,
+		0x051, 0x0006B27D,
+		0x052, 0x0007E49D,
+		0x053, 0x00000073,
+		0x056, 0x00051FF3,
+		0x035, 0x00000086,
+		0x035, 0x00000186,
+		0x035, 0x00000286,
+		0x036, 0x00001C25,
+		0x036, 0x00009C25,
+		0x036, 0x00011C25,
+		0x036, 0x00019C25,
+		0x0B6, 0x00048538,
+		0x018, 0x00000C07,
+		0x05A, 0x0004BD00,
+		0x019, 0x000739D0,
+		0x034, 0x0000ADF3,
+		0x034, 0x00009DF0,
+		0x034, 0x00008DED,
+		0x034, 0x00007DEA,
+		0x034, 0x00006DE7,
+		0x034, 0x000054EE,
+		0x034, 0x000044EB,
+		0x034, 0x000034E8,
+		0x034, 0x0000246B,
+		0x034, 0x00001468,
+		0x034, 0x0000006D,
+		0x000, 0x00030159,
+		0x084, 0x00068200,
+		0x086, 0x000000CE,
+		0x087, 0x00048A00,
+		0x08E, 0x00065540,
+		0x08F, 0x00088000,
+		0x0EF, 0x000020A0,
+		0x03B, 0x000F02B0,
+		0x03B, 0x000EF7B0,
+		0x03B, 0x000D4FB0,
+		0x03B, 0x000CF060,
+		0x03B, 0x000B0090,
+		0x03B, 0x000A0080,
+		0x03B, 0x00090080,
+		0x03B, 0x0008F780,
+		0x03B, 0x000722B0,
+		0x03B, 0x0006F7B0,
+		0x03B, 0x00054FB0,
+		0x03B, 0x0004F060,
+		0x03B, 0x00030090,
+		0x03B, 0x00020080,
+		0x03B, 0x00010080,
+		0x03B, 0x0000F780,
+		0x0EF, 0x000000A0,
+		0x000, 0x00010159,
+		0x018, 0x0000F407,
+		0xFFE, 0x00000000,
+		0xFFE, 0x00000000,
+		0x01F, 0x00080003,
+		0xFFE, 0x00000000,
+		0xFFE, 0x00000000,
+		0x01E, 0x00000001,
+		0x01F, 0x00080000,
+		0x000, 0x00033E60,
+
+};
+
+u32 RTL8188EEMAC_1T_ARRAY[] = {
+		0x026, 0x00000041,
+		0x027, 0x00000035,
+		0x428, 0x0000000A,
+		0x429, 0x00000010,
+		0x430, 0x00000000,
+		0x431, 0x00000001,
+		0x432, 0x00000002,
+		0x433, 0x00000004,
+		0x434, 0x00000005,
+		0x435, 0x00000006,
+		0x436, 0x00000007,
+		0x437, 0x00000008,
+		0x438, 0x00000000,
+		0x439, 0x00000000,
+		0x43A, 0x00000001,
+		0x43B, 0x00000002,
+		0x43C, 0x00000004,
+		0x43D, 0x00000005,
+		0x43E, 0x00000006,
+		0x43F, 0x00000007,
+		0x440, 0x0000005D,
+		0x441, 0x00000001,
+		0x442, 0x00000000,
+		0x444, 0x00000015,
+		0x445, 0x000000F0,
+		0x446, 0x0000000F,
+		0x447, 0x00000000,
+		0x458, 0x00000041,
+		0x459, 0x000000A8,
+		0x45A, 0x00000072,
+		0x45B, 0x000000B9,
+		0x460, 0x00000066,
+		0x461, 0x00000066,
+		0x480, 0x00000008,
+		0x4C8, 0x000000FF,
+		0x4C9, 0x00000008,
+		0x4CC, 0x000000FF,
+		0x4CD, 0x000000FF,
+		0x4CE, 0x00000001,
+		0x4D3, 0x00000001,
+		0x500, 0x00000026,
+		0x501, 0x000000A2,
+		0x502, 0x0000002F,
+		0x503, 0x00000000,
+		0x504, 0x00000028,
+		0x505, 0x000000A3,
+		0x506, 0x0000005E,
+		0x507, 0x00000000,
+		0x508, 0x0000002B,
+		0x509, 0x000000A4,
+		0x50A, 0x0000005E,
+		0x50B, 0x00000000,
+		0x50C, 0x0000004F,
+		0x50D, 0x000000A4,
+		0x50E, 0x00000000,
+		0x50F, 0x00000000,
+		0x512, 0x0000001C,
+		0x514, 0x0000000A,
+		0x516, 0x0000000A,
+		0x525, 0x0000004F,
+		0x550, 0x00000010,
+		0x551, 0x00000010,
+		0x559, 0x00000002,
+		0x55D, 0x000000FF,
+		0x605, 0x00000030,
+		0x608, 0x0000000E,
+		0x609, 0x0000002A,
+		0x620, 0x000000FF,
+		0x621, 0x000000FF,
+		0x622, 0x000000FF,
+		0x623, 0x000000FF,
+		0x624, 0x000000FF,
+		0x625, 0x000000FF,
+		0x626, 0x000000FF,
+		0x627, 0x000000FF,
+		0x652, 0x00000020,
+		0x63C, 0x0000000A,
+		0x63D, 0x0000000A,
+		0x63E, 0x0000000E,
+		0x63F, 0x0000000E,
+		0x640, 0x00000040,
+		0x66E, 0x00000005,
+		0x700, 0x00000021,
+		0x701, 0x00000043,
+		0x702, 0x00000065,
+		0x703, 0x00000087,
+		0x708, 0x00000021,
+		0x709, 0x00000043,
+		0x70A, 0x00000065,
+		0x70B, 0x00000087,
+
+};
+
+u32 RTL8188EEAGCTAB_1TARRAY[] = {
+		0xC78, 0xFB000001,
+		0xC78, 0xFB010001,
+		0xC78, 0xFB020001,
+		0xC78, 0xFB030001,
+		0xC78, 0xFB040001,
+		0xC78, 0xFB050001,
+		0xC78, 0xFA060001,
+		0xC78, 0xF9070001,
+		0xC78, 0xF8080001,
+		0xC78, 0xF7090001,
+		0xC78, 0xF60A0001,
+		0xC78, 0xF50B0001,
+		0xC78, 0xF40C0001,
+		0xC78, 0xF30D0001,
+		0xC78, 0xF20E0001,
+		0xC78, 0xF10F0001,
+		0xC78, 0xF0100001,
+		0xC78, 0xEF110001,
+		0xC78, 0xEE120001,
+		0xC78, 0xED130001,
+		0xC78, 0xEC140001,
+		0xC78, 0xEB150001,
+		0xC78, 0xEA160001,
+		0xC78, 0xE9170001,
+		0xC78, 0xE8180001,
+		0xC78, 0xE7190001,
+		0xC78, 0xE61A0001,
+		0xC78, 0xE51B0001,
+		0xC78, 0xE41C0001,
+		0xC78, 0xE31D0001,
+		0xC78, 0xE21E0001,
+		0xC78, 0xE11F0001,
+		0xC78, 0x8A200001,
+		0xC78, 0x89210001,
+		0xC78, 0x88220001,
+		0xC78, 0x87230001,
+		0xC78, 0x86240001,
+		0xC78, 0x85250001,
+		0xC78, 0x84260001,
+		0xC78, 0x83270001,
+		0xC78, 0x82280001,
+		0xC78, 0x6B290001,
+		0xC78, 0x6A2A0001,
+		0xC78, 0x692B0001,
+		0xC78, 0x682C0001,
+		0xC78, 0x672D0001,
+		0xC78, 0x662E0001,
+		0xC78, 0x652F0001,
+		0xC78, 0x64300001,
+		0xC78, 0x63310001,
+		0xC78, 0x62320001,
+		0xC78, 0x61330001,
+		0xC78, 0x46340001,
+		0xC78, 0x45350001,
+		0xC78, 0x44360001,
+		0xC78, 0x43370001,
+		0xC78, 0x42380001,
+		0xC78, 0x41390001,
+		0xC78, 0x403A0001,
+		0xC78, 0x403B0001,
+		0xC78, 0x403C0001,
+		0xC78, 0x403D0001,
+		0xC78, 0x403E0001,
+		0xC78, 0x403F0001,
+		0xC78, 0xFB400001,
+		0xC78, 0xFB410001,
+		0xC78, 0xFB420001,
+		0xC78, 0xFB430001,
+		0xC78, 0xFB440001,
+		0xC78, 0xFB450001,
+		0xC78, 0xFB460001,
+		0xC78, 0xFB470001,
+		0xC78, 0xFB480001,
+		0xC78, 0xFA490001,
+		0xC78, 0xF94A0001,
+		0xC78, 0xF84B0001,
+		0xC78, 0xF74C0001,
+		0xC78, 0xF64D0001,
+		0xC78, 0xF54E0001,
+		0xC78, 0xF44F0001,
+		0xC78, 0xF3500001,
+		0xC78, 0xF2510001,
+		0xC78, 0xF1520001,
+		0xC78, 0xF0530001,
+		0xC78, 0xEF540001,
+		0xC78, 0xEE550001,
+		0xC78, 0xED560001,
+		0xC78, 0xEC570001,
+		0xC78, 0xEB580001,
+		0xC78, 0xEA590001,
+		0xC78, 0xE95A0001,
+		0xC78, 0xE85B0001,
+		0xC78, 0xE75C0001,
+		0xC78, 0xE65D0001,
+		0xC78, 0xE55E0001,
+		0xC78, 0xE45F0001,
+		0xC78, 0xE3600001,
+		0xC78, 0xE2610001,
+		0xC78, 0xC3620001,
+		0xC78, 0xC2630001,
+		0xC78, 0xC1640001,
+		0xC78, 0x8B650001,
+		0xC78, 0x8A660001,
+		0xC78, 0x89670001,
+		0xC78, 0x88680001,
+		0xC78, 0x87690001,
+		0xC78, 0x866A0001,
+		0xC78, 0x856B0001,
+		0xC78, 0x846C0001,
+		0xC78, 0x676D0001,
+		0xC78, 0x666E0001,
+		0xC78, 0x656F0001,
+		0xC78, 0x64700001,
+		0xC78, 0x63710001,
+		0xC78, 0x62720001,
+		0xC78, 0x61730001,
+		0xC78, 0x60740001,
+		0xC78, 0x46750001,
+		0xC78, 0x45760001,
+		0xC78, 0x44770001,
+		0xC78, 0x43780001,
+		0xC78, 0x42790001,
+		0xC78, 0x417A0001,
+		0xC78, 0x407B0001,
+		0xC78, 0x407C0001,
+		0xC78, 0x407D0001,
+		0xC78, 0x407E0001,
+		0xC78, 0x407F0001,
+};
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/table.h b/drivers/net/wireless/rtlwifi/rtl8188ee/table.h
new file mode 100644
index 000000000000..c1218e835129
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/table.h
@@ -0,0 +1,47 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Created on  2010/ 5/18,  1:41
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92CE_TABLE__H_
+#define __RTL92CE_TABLE__H_
+
+#include <linux/types.h>
+#define  RTL8188EEPHY_REG_1TARRAYLEN	382
+extern u32 RTL8188EEPHY_REG_1TARRAY[];
+#define RTL8188EEPHY_REG_ARRAY_PGLEN	264
+extern u32 RTL8188EEPHY_REG_ARRAY_PG[];
+#define	RTL8188EE_RADIOA_1TARRAYLEN	190
+extern u32 RTL8188EE_RADIOA_1TARRAY[];
+#define RTL8188EEMAC_1T_ARRAYLEN	180
+extern u32 RTL8188EEMAC_1T_ARRAY[];
+#define RTL8188EEAGCTAB_1TARRAYLEN	256
+extern u32 RTL8188EEAGCTAB_1TARRAY[];
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c
new file mode 100644
index 000000000000..a8871d66d56a
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c
@@ -0,0 +1,817 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../base.h"
+#include "../stats.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "trx.h"
+#include "led.h"
+#include "dm.h"
+
+static u8 _rtl88ee_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
+{
+	__le16 fc = rtl_get_fc(skb);
+
+	if (unlikely(ieee80211_is_beacon(fc)))
+		return QSLT_BEACON;
+	if (ieee80211_is_mgmt(fc) || ieee80211_is_ctl(fc))
+		return QSLT_MGNT;
+
+	return skb->priority;
+}
+
+static void _rtl88ee_query_rxphystatus(struct ieee80211_hw *hw,
+			struct rtl_stats *pstatus, u8 *pdesc,
+			struct rx_fwinfo_88e *p_drvinfo,
+			bool bpacket_match_bssid,
+			bool bpacket_toself, bool packet_beacon)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtlpriv);
+	struct phy_sts_cck_8192s_t *cck_buf;
+	struct phy_status_rpt *phystrpt = (struct phy_status_rpt *)p_drvinfo;
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	char rx_pwr_all = 0, rx_pwr[4];
+	u8 rf_rx_num = 0, evm, pwdb_all;
+	u8 i, max_spatial_stream;
+	u32 rssi, total_rssi = 0;
+	bool is_cck = pstatus->is_cck;
+	u8 lan_idx, vga_idx;
+
+	/* Record it for next packet processing */
+	pstatus->packet_matchbssid = bpacket_match_bssid;
+	pstatus->packet_toself = bpacket_toself;
+	pstatus->packet_beacon = packet_beacon;
+	pstatus->rx_mimo_sig_qual[0] = -1;
+	pstatus->rx_mimo_sig_qual[1] = -1;
+
+	if (is_cck) {
+		u8 cck_hipwr;
+		u8 cck_agc_rpt;
+		/* CCK Driver info Structure is not the same as OFDM packet. */
+		cck_buf = (struct phy_sts_cck_8192s_t *)p_drvinfo;
+		cck_agc_rpt = cck_buf->cck_agc_rpt;
+
+		/* (1)Hardware does not provide RSSI for CCK
+		 * (2)PWDB, Average PWDB cacluated by
+		 * hardware (for rate adaptive)
+		 */
+		if (ppsc->rfpwr_state == ERFON)
+			cck_hipwr = rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2,
+						  BIT(9));
+		else
+			cck_hipwr = false;
+
+		lan_idx = ((cck_agc_rpt & 0xE0) >> 5);
+		vga_idx = (cck_agc_rpt & 0x1f);
+		switch (lan_idx) {
+		case 7:
+			if (vga_idx <= 27)
+				rx_pwr_all = -100 + 2 * (27 - vga_idx);
+			else
+				rx_pwr_all = -100;
+			break;
+		case 6:
+			rx_pwr_all = -48 + 2 * (2 - vga_idx); /*VGA_idx = 2~0*/
+			break;
+		case 5:
+			rx_pwr_all = -42 + 2 * (7 - vga_idx); /*VGA_idx = 7~5*/
+			break;
+		case 4:
+			rx_pwr_all = -36 + 2 * (7 - vga_idx); /*VGA_idx = 7~4*/
+			break;
+		case 3:
+			rx_pwr_all = -24 + 2 * (7 - vga_idx); /*VGA_idx = 7~0*/
+			break;
+		case 2:
+			if (cck_hipwr)
+				rx_pwr_all = -12 + 2 * (5 - vga_idx);
+			else
+				rx_pwr_all = -6 + 2 * (5 - vga_idx);
+			break;
+		case 1:
+			rx_pwr_all = 8 - 2 * vga_idx;
+			break;
+		case 0:
+			rx_pwr_all = 14 - 2 * vga_idx;
+			break;
+		default:
+			break;
+		}
+		rx_pwr_all += 6;
+		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
+		/* CCK gain is smaller than OFDM/MCS gain,
+		 * so we add gain diff by experiences,
+		 * the val is 6
+		 */
+		pwdb_all += 6;
+		if (pwdb_all > 100)
+			pwdb_all = 100;
+		/* modify the offset to make the same
+		 * gain index with OFDM.
+		 */
+		if (pwdb_all > 34 && pwdb_all <= 42)
+			pwdb_all -= 2;
+		else if (pwdb_all > 26 && pwdb_all <= 34)
+			pwdb_all -= 6;
+		else if (pwdb_all > 14 && pwdb_all <= 26)
+			pwdb_all -= 8;
+		else if (pwdb_all > 4 && pwdb_all <= 14)
+			pwdb_all -= 4;
+		if (cck_hipwr == false) {
+			if (pwdb_all >= 80)
+				pwdb_all = ((pwdb_all - 80)<<1) +
+					   ((pwdb_all - 80)>>1) + 80;
+			else if ((pwdb_all <= 78) && (pwdb_all >= 20))
+				pwdb_all += 3;
+			if (pwdb_all > 100)
+				pwdb_all = 100;
+		}
+
+		pstatus->rx_pwdb_all = pwdb_all;
+		pstatus->recvsignalpower = rx_pwr_all;
+
+		/* (3) Get Signal Quality (EVM) */
+		if (bpacket_match_bssid) {
+			u8 sq;
+
+			if (pstatus->rx_pwdb_all > 40) {
+				sq = 100;
+			} else {
+				sq = cck_buf->sq_rpt;
+				if (sq > 64)
+					sq = 0;
+				else if (sq < 20)
+					sq = 100;
+				else
+					sq = ((64 - sq) * 100) / 44;
+			}
+
+			pstatus->signalquality = sq;
+			pstatus->rx_mimo_sig_qual[0] = sq;
+			pstatus->rx_mimo_sig_qual[1] = -1;
+		}
+	} else {
+		rtlpriv->dm.rfpath_rxenable[0] =
+		    rtlpriv->dm.rfpath_rxenable[1] = true;
+
+		/* (1)Get RSSI for HT rate */
+		for (i = RF90_PATH_A; i < RF6052_MAX_PATH; i++) {
+			/* we will judge RF RX path now. */
+			if (rtlpriv->dm.rfpath_rxenable[i])
+				rf_rx_num++;
+
+			rx_pwr[i] = ((p_drvinfo->gain_trsw[i] & 0x3f) * 2)-110;
+
+			/* Translate DBM to percentage. */
+			rssi = rtl_query_rxpwrpercentage(rx_pwr[i]);
+			total_rssi += rssi;
+
+			/* Get Rx snr value in DB */
+			rtlpriv->stats.rx_snr_db[i] = p_drvinfo->rxsnr[i] / 2;
+
+			/* Record Signal Strength for next packet */
+			if (bpacket_match_bssid)
+				pstatus->rx_mimo_signalstrength[i] = (u8) rssi;
+		}
+
+		/* (2)PWDB, Average PWDB cacluated by
+		 * hardware (for rate adaptive)
+		 */
+		rx_pwr_all = ((p_drvinfo->pwdb_all >> 1) & 0x7f) - 110;
+
+		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
+		pstatus->rx_pwdb_all = pwdb_all;
+		pstatus->rxpower = rx_pwr_all;
+		pstatus->recvsignalpower = rx_pwr_all;
+
+		/* (3)EVM of HT rate */
+		if (pstatus->is_ht && pstatus->rate >= DESC92C_RATEMCS8 &&
+		    pstatus->rate <= DESC92C_RATEMCS15)
+			max_spatial_stream = 2;
+		else
+			max_spatial_stream = 1;
+
+		for (i = 0; i < max_spatial_stream; i++) {
+			evm = rtl_evm_db_to_percentage(p_drvinfo->rxevm[i]);
+
+			if (bpacket_match_bssid) {
+				/* Fill value in RFD, Get the first
+				 * spatial stream only
+				 */
+				if (i == 0)
+					pstatus->signalquality = evm & 0xff;
+				pstatus->rx_mimo_sig_qual[i] = evm & 0xff;
+			}
+		}
+	}
+
+	/* UI BSS List signal strength(in percentage),
+	 * make it good looking, from 0~100.
+	 */
+	if (is_cck)
+		pstatus->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
+					  pwdb_all));
+	else if (rf_rx_num != 0)
+		pstatus->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
+					  total_rssi /= rf_rx_num));
+	/*HW antenna diversity*/
+	rtldm->fat_table.antsel_rx_keep_0 = phystrpt->ant_sel;
+	rtldm->fat_table.antsel_rx_keep_1 = phystrpt->ant_sel_b;
+	rtldm->fat_table.antsel_rx_keep_2 = phystrpt->antsel_rx_keep_2;
+}
+
+static void _rtl88ee_smart_antenna(struct ieee80211_hw *hw,
+	struct rtl_stats *pstatus)
+{
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 ant_mux;
+	struct fast_ant_training *pfat = &(rtldm->fat_table);
+
+	if (rtlefuse->antenna_div_type == CG_TRX_SMART_ANTDIV) {
+		if (pfat->fat_state == FAT_TRAINING_STATE) {
+			if (pstatus->packet_toself) {
+				ant_mux = (pfat->antsel_rx_keep_2 << 2) |
+						(pfat->antsel_rx_keep_1 << 1) |
+						 pfat->antsel_rx_keep_0;
+				pfat->ant_sum[ant_mux] += pstatus->rx_pwdb_all;
+				pfat->ant_cnt[ant_mux]++;
+			}
+		}
+	} else if ((rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) ||
+		   (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)) {
+		if (pstatus->packet_toself || pstatus->packet_matchbssid) {
+			ant_mux = (pfat->antsel_rx_keep_2 << 2) |
+				  (pfat->antsel_rx_keep_1 << 1) |
+				   pfat->antsel_rx_keep_0;
+			rtl88e_dm_ant_sel_statistics(hw, ant_mux, 0,
+						     pstatus->rx_pwdb_all);
+		}
+	}
+}
+
+static void _rtl88ee_translate_rx_signal_stuff(struct ieee80211_hw *hw,
+		struct sk_buff *skb, struct rtl_stats *pstatus,
+		u8 *pdesc, struct rx_fwinfo_88e *p_drvinfo)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct ieee80211_hdr *hdr;
+	u8 *tmp_buf;
+	u8 *praddr;
+	u8 *psaddr;
+	__le16 fc;
+	u16 type, ufc;
+	bool match_bssid, packet_toself, packet_beacon, addr;
+
+	tmp_buf = skb->data + pstatus->rx_drvinfo_size + pstatus->rx_bufshift;
+
+	hdr = (struct ieee80211_hdr *)tmp_buf;
+	fc = hdr->frame_control;
+	ufc = le16_to_cpu(fc);
+	type = WLAN_FC_GET_TYPE(fc);
+	praddr = hdr->addr1;
+	psaddr = ieee80211_get_SA(hdr);
+	memcpy(pstatus->psaddr, psaddr, ETH_ALEN);
+
+	addr = (!compare_ether_addr(mac->bssid, (ufc & IEEE80211_FCTL_TODS) ?
+		hdr->addr1 : (ufc & IEEE80211_FCTL_FROMDS) ?
+		hdr->addr2 : hdr->addr3));
+	match_bssid = ((IEEE80211_FTYPE_CTL != type) && (!pstatus->hwerror) &&
+		       (!pstatus->crc) && (!pstatus->icv)) && addr;
+
+	addr = (!compare_ether_addr(praddr, rtlefuse->dev_addr));
+	packet_toself = match_bssid && addr;
+
+	if (ieee80211_is_beacon(fc))
+		packet_beacon = true;
+
+	_rtl88ee_query_rxphystatus(hw, pstatus, pdesc, p_drvinfo,
+				   match_bssid, packet_toself, packet_beacon);
+	_rtl88ee_smart_antenna(hw, pstatus);
+	rtl_process_phyinfo(hw, tmp_buf, pstatus);
+}
+
+static void insert_em(struct rtl_tcb_desc *ptcb_desc, u8 *virtualaddress)
+{
+	u32 dwtmp = 0;
+
+	memset(virtualaddress, 0, 8);
+
+	SET_EARLYMODE_PKTNUM(virtualaddress, ptcb_desc->empkt_num);
+	if (ptcb_desc->empkt_num == 1) {
+		dwtmp = ptcb_desc->empkt_len[0];
+	} else {
+		dwtmp = ptcb_desc->empkt_len[0];
+		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0) + 4;
+		dwtmp += ptcb_desc->empkt_len[1];
+	}
+	SET_EARLYMODE_LEN0(virtualaddress, dwtmp);
+
+	if (ptcb_desc->empkt_num <= 3) {
+		dwtmp = ptcb_desc->empkt_len[2];
+	} else {
+		dwtmp = ptcb_desc->empkt_len[2];
+		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0) + 4;
+		dwtmp += ptcb_desc->empkt_len[3];
+	}
+	SET_EARLYMODE_LEN1(virtualaddress, dwtmp);
+	if (ptcb_desc->empkt_num <= 5) {
+		dwtmp = ptcb_desc->empkt_len[4];
+	} else {
+		dwtmp = ptcb_desc->empkt_len[4];
+		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0) + 4;
+		dwtmp += ptcb_desc->empkt_len[5];
+	}
+	SET_EARLYMODE_LEN2_1(virtualaddress, dwtmp & 0xF);
+	SET_EARLYMODE_LEN2_2(virtualaddress, dwtmp >> 4);
+	if (ptcb_desc->empkt_num <= 7) {
+		dwtmp = ptcb_desc->empkt_len[6];
+	} else {
+		dwtmp = ptcb_desc->empkt_len[6];
+		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0) + 4;
+		dwtmp += ptcb_desc->empkt_len[7];
+	}
+	SET_EARLYMODE_LEN3(virtualaddress, dwtmp);
+	if (ptcb_desc->empkt_num <= 9) {
+		dwtmp = ptcb_desc->empkt_len[8];
+	} else {
+		dwtmp = ptcb_desc->empkt_len[8];
+		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0) + 4;
+		dwtmp += ptcb_desc->empkt_len[9];
+	}
+	SET_EARLYMODE_LEN4(virtualaddress, dwtmp);
+}
+
+bool rtl88ee_rx_query_desc(struct ieee80211_hw *hw,
+			   struct rtl_stats *status,
+			   struct ieee80211_rx_status *rx_status,
+			   u8 *pdesc, struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rx_fwinfo_88e *p_drvinfo;
+	struct ieee80211_hdr *hdr;
+
+	u32 phystatus = GET_RX_DESC_PHYST(pdesc);
+	status->packet_report_type = (u8)GET_RX_STATUS_DESC_RPT_SEL(pdesc);
+	if (status->packet_report_type == TX_REPORT2)
+		status->length = (u16) GET_RX_RPT2_DESC_PKT_LEN(pdesc);
+	else
+		status->length = (u16) GET_RX_DESC_PKT_LEN(pdesc);
+	status->rx_drvinfo_size = (u8) GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
+				       RX_DRV_INFO_SIZE_UNIT;
+	status->rx_bufshift = (u8) (GET_RX_DESC_SHIFT(pdesc) & 0x03);
+	status->icv = (u16) GET_RX_DESC_ICV(pdesc);
+	status->crc = (u16) GET_RX_DESC_CRC32(pdesc);
+	status->hwerror = (status->crc | status->icv);
+	status->decrypted = !GET_RX_DESC_SWDEC(pdesc);
+	status->rate = (u8) GET_RX_DESC_RXMCS(pdesc);
+	status->shortpreamble = (u16) GET_RX_DESC_SPLCP(pdesc);
+	status->isampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
+	status->isfirst_ampdu = (bool) ((GET_RX_DESC_PAGGR(pdesc) == 1) &&
+					(GET_RX_DESC_FAGGR(pdesc) == 1));
+	if (status->packet_report_type == NORMAL_RX)
+		status->timestamp_low = GET_RX_DESC_TSFL(pdesc);
+	status->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
+	status->is_ht = (bool)GET_RX_DESC_RXHT(pdesc);
+
+	status->is_cck = RTL8188_RX_HAL_IS_CCK_RATE(status->rate);
+
+	status->macid = GET_RX_DESC_MACID(pdesc);
+	if (GET_RX_STATUS_DESC_MAGIC_MATCH(pdesc))
+		status->wake_match = BIT(2);
+	else if (GET_RX_STATUS_DESC_MAGIC_MATCH(pdesc))
+		status->wake_match = BIT(1);
+	else if (GET_RX_STATUS_DESC_UNICAST_MATCH(pdesc))
+		status->wake_match = BIT(0);
+	else
+		status->wake_match = 0;
+	if (status->wake_match)
+		RT_TRACE(rtlpriv, COMP_RXDESC, DBG_LOUD,
+			 "Get Wakeup Packet!! WakeMatch =%d\n",
+			 status->wake_match);
+	rx_status->freq = hw->conf.chandef.chan->center_freq;
+	rx_status->band = hw->conf.chandef.chan->band;
+
+	if (status->crc)
+		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
+
+	if (status->rx_is40Mhzpacket)
+		rx_status->flag |= RX_FLAG_40MHZ;
+
+	if (status->is_ht)
+		rx_status->flag |= RX_FLAG_HT;
+
+	rx_status->flag |= RX_FLAG_MACTIME_START;
+
+	/* hw will set status->decrypted true, if it finds the
+	 * frame is open data frame or mgmt frame.
+	 * So hw will not decryption robust managment frame
+	 * for IEEE80211w but still set status->decrypted
+	 * true, so here we should set it back to undecrypted
+	 * for IEEE80211w frame, and mac80211 sw will help
+	 * to decrypt it
+	 */
+	if (status->decrypted) {
+		hdr = (struct ieee80211_hdr *)(skb->data +
+		       status->rx_drvinfo_size + status->rx_bufshift);
+
+		if (!hdr) {
+			/* During testing, hdr was NULL */
+			return false;
+		}
+		if ((ieee80211_is_robust_mgmt_frame(hdr)) &&
+		    (ieee80211_has_protected(hdr->frame_control)))
+			rx_status->flag &= ~RX_FLAG_DECRYPTED;
+		else
+			rx_status->flag |= RX_FLAG_DECRYPTED;
+	}
+
+	/* rate_idx: index of data rate into band's
+	 * supported rates or MCS index if HT rates
+	 * are use (RX_FLAG_HT)
+	 * Notice: this is diff with windows define
+	 */
+	rx_status->rate_idx = rtlwifi_rate_mapping(hw, status->is_ht,
+						   status->rate, false);
+
+	rx_status->mactime = status->timestamp_low;
+	if (phystatus == true) {
+		p_drvinfo = (struct rx_fwinfo_88e *)(skb->data +
+						     status->rx_bufshift);
+
+		_rtl88ee_translate_rx_signal_stuff(hw, skb, status, pdesc,
+						   p_drvinfo);
+	}
+
+	/*rx_status->qual = status->signal; */
+	rx_status->signal = status->recvsignalpower + 10;
+	/*rx_status->noise = -status->noise; */
+	if (status->packet_report_type == TX_REPORT2) {
+		status->macid_valid_entry[0] =
+			 GET_RX_RPT2_DESC_MACID_VALID_1(pdesc);
+		status->macid_valid_entry[1] =
+			 GET_RX_RPT2_DESC_MACID_VALID_2(pdesc);
+	}
+	return true;
+}
+
+void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
+			  struct ieee80211_hdr *hdr, u8 *pdesc_tx,
+			  struct ieee80211_tx_info *info,
+			  struct ieee80211_sta *sta,
+			  struct sk_buff *skb,
+			  u8 hw_queue, struct rtl_tcb_desc *ptcb_desc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 *pdesc = (u8 *)pdesc_tx;
+	u16 seq_number;
+	__le16 fc = hdr->frame_control;
+	unsigned int buf_len = 0;
+	unsigned int skb_len = skb->len;
+	u8 fw_qsel = _rtl88ee_map_hwqueue_to_fwqueue(skb, hw_queue);
+	bool firstseg = ((hdr->seq_ctrl &
+			    cpu_to_le16(IEEE80211_SCTL_FRAG)) == 0);
+	bool lastseg = ((hdr->frame_control &
+			   cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)) == 0);
+	dma_addr_t mapping;
+	u8 bw_40 = 0;
+	u8 short_gi = 0;
+
+	if (mac->opmode == NL80211_IFTYPE_STATION) {
+		bw_40 = mac->bw_40;
+	} else if (mac->opmode == NL80211_IFTYPE_AP ||
+		mac->opmode == NL80211_IFTYPE_ADHOC) {
+		if (sta)
+			bw_40 = sta->ht_cap.cap &
+				IEEE80211_HT_CAP_SUP_WIDTH_20_40;
+	}
+	seq_number = (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_SEQ) >> 4;
+	rtl_get_tcb_desc(hw, info, sta, skb, ptcb_desc);
+	/* reserve 8 byte for AMPDU early mode */
+	if (rtlhal->earlymode_enable) {
+		skb_push(skb, EM_HDR_LEN);
+		memset(skb->data, 0, EM_HDR_LEN);
+	}
+	buf_len = skb->len;
+	mapping = pci_map_single(rtlpci->pdev, skb->data, skb->len,
+				 PCI_DMA_TODEVICE);
+	if (pci_dma_mapping_error(rtlpci->pdev, mapping)) {
+		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+			 "DMA mapping error");
+		return;
+	}
+	CLEAR_PCI_TX_DESC_CONTENT(pdesc, sizeof(struct tx_desc_88e));
+	if (ieee80211_is_nullfunc(fc) || ieee80211_is_ctl(fc)) {
+		firstseg = true;
+		lastseg = true;
+	}
+	if (firstseg) {
+		if (rtlhal->earlymode_enable) {
+			SET_TX_DESC_PKT_OFFSET(pdesc, 1);
+			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN +
+					   EM_HDR_LEN);
+			if (ptcb_desc->empkt_num) {
+				RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+					 "Insert 8 byte.pTcb->EMPktNum:%d\n",
+					 ptcb_desc->empkt_num);
+				insert_em(ptcb_desc, (u8 *)(skb->data));
+			}
+		} else {
+			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
+		}
+
+		ptcb_desc->use_driver_rate = true;
+		SET_TX_DESC_TX_RATE(pdesc, ptcb_desc->hw_rate);
+		if (ptcb_desc->hw_rate > DESC92C_RATEMCS0)
+			short_gi = (ptcb_desc->use_shortgi) ? 1 : 0;
+		else
+			short_gi = (ptcb_desc->use_shortpreamble) ? 1 : 0;
+		SET_TX_DESC_DATA_SHORTGI(pdesc, short_gi);
+
+		if (info->flags & IEEE80211_TX_CTL_AMPDU) {
+			SET_TX_DESC_AGG_ENABLE(pdesc, 1);
+			SET_TX_DESC_MAX_AGG_NUM(pdesc, 0x14);
+		}
+		SET_TX_DESC_SEQ(pdesc, seq_number);
+		SET_TX_DESC_RTS_ENABLE(pdesc, ((ptcb_desc->rts_enable &&
+					      !ptcb_desc->cts_enable) ? 1 : 0));
+		SET_TX_DESC_HW_RTS_ENABLE(pdesc, 0);
+		SET_TX_DESC_CTS2SELF(pdesc, ((ptcb_desc->cts_enable) ? 1 : 0));
+		SET_TX_DESC_RTS_STBC(pdesc, ((ptcb_desc->rts_stbc) ? 1 : 0));
+
+		SET_TX_DESC_RTS_RATE(pdesc, ptcb_desc->rts_rate);
+		SET_TX_DESC_RTS_BW(pdesc, 0);
+		SET_TX_DESC_RTS_SC(pdesc, ptcb_desc->rts_sc);
+		SET_TX_DESC_RTS_SHORT(pdesc,
+			((ptcb_desc->rts_rate <= DESC92C_RATE54M) ?
+			(ptcb_desc->rts_use_shortpreamble ? 1 : 0) :
+			(ptcb_desc->rts_use_shortgi ? 1 : 0)));
+
+		if (ptcb_desc->btx_enable_sw_calc_duration)
+			SET_TX_DESC_NAV_USE_HDR(pdesc, 1);
+
+		if (bw_40) {
+			if (ptcb_desc->packet_bw) {
+				SET_TX_DESC_DATA_BW(pdesc, 1);
+				SET_TX_DESC_TX_SUB_CARRIER(pdesc, 3);
+			} else {
+				SET_TX_DESC_DATA_BW(pdesc, 0);
+				SET_TX_DESC_TX_SUB_CARRIER(pdesc,
+						   mac->cur_40_prime_sc);
+			}
+		} else {
+			SET_TX_DESC_DATA_BW(pdesc, 0);
+			SET_TX_DESC_TX_SUB_CARRIER(pdesc, 0);
+		}
+
+		SET_TX_DESC_LINIP(pdesc, 0);
+		SET_TX_DESC_PKT_SIZE(pdesc, (u16) skb_len);
+		if (sta) {
+			u8 ampdu_density = sta->ht_cap.ampdu_density;
+			SET_TX_DESC_AMPDU_DENSITY(pdesc, ampdu_density);
+		}
+		if (info->control.hw_key) {
+			struct ieee80211_key_conf *keyconf;
+			keyconf = info->control.hw_key;
+			switch (keyconf->cipher) {
+			case WLAN_CIPHER_SUITE_WEP40:
+			case WLAN_CIPHER_SUITE_WEP104:
+			case WLAN_CIPHER_SUITE_TKIP:
+				SET_TX_DESC_SEC_TYPE(pdesc, 0x1);
+				break;
+			case WLAN_CIPHER_SUITE_CCMP:
+				SET_TX_DESC_SEC_TYPE(pdesc, 0x3);
+				break;
+			default:
+				SET_TX_DESC_SEC_TYPE(pdesc, 0x0);
+				break;
+			}
+		}
+
+		SET_TX_DESC_QUEUE_SEL(pdesc, fw_qsel);
+		SET_TX_DESC_DATA_RATE_FB_LIMIT(pdesc, 0x1F);
+		SET_TX_DESC_RTS_RATE_FB_LIMIT(pdesc, 0xF);
+		SET_TX_DESC_DISABLE_FB(pdesc, ptcb_desc->disable_ratefallback ?
+				       1 : 0);
+		SET_TX_DESC_USE_RATE(pdesc, ptcb_desc->use_driver_rate ? 1 : 0);
+
+		/* Set TxRate and RTSRate in TxDesc  */
+		/* This prevent Tx initial rate of new-coming packets */
+		/* from being overwritten by retried  packet rate.*/
+		if (!ptcb_desc->use_driver_rate) {
+			/*SET_TX_DESC_RTS_RATE(pdesc, 0x08); */
+			/* SET_TX_DESC_TX_RATE(pdesc, 0x0b); */
+		}
+		if (ieee80211_is_data_qos(fc)) {
+			if (mac->rdg_en) {
+				RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+					 "Enable RDG function.\n");
+				SET_TX_DESC_RDG_ENABLE(pdesc, 1);
+				SET_TX_DESC_HTC(pdesc, 1);
+			}
+		}
+	}
+
+	SET_TX_DESC_FIRST_SEG(pdesc, (firstseg ? 1 : 0));
+	SET_TX_DESC_LAST_SEG(pdesc, (lastseg ? 1 : 0));
+	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16) buf_len);
+	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, mapping);
+	if (rtlpriv->dm.useramask) {
+		SET_TX_DESC_RATE_ID(pdesc, ptcb_desc->ratr_index);
+		SET_TX_DESC_MACID(pdesc, ptcb_desc->mac_id);
+	} else {
+		SET_TX_DESC_RATE_ID(pdesc, 0xC + ptcb_desc->ratr_index);
+		SET_TX_DESC_MACID(pdesc, ptcb_desc->ratr_index);
+	}
+	if (ieee80211_is_data_qos(fc))
+		SET_TX_DESC_QOS(pdesc, 1);
+
+	if (!ieee80211_is_data_qos(fc))
+		SET_TX_DESC_HWSEQ_EN(pdesc, 1);
+	SET_TX_DESC_MORE_FRAG(pdesc, (lastseg ? 0 : 1));
+	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
+	    is_broadcast_ether_addr(ieee80211_get_DA(hdr)))
+		SET_TX_DESC_BMC(pdesc, 1);
+
+	rtl88e_dm_set_tx_ant_by_tx_info(hw, pdesc, ptcb_desc->mac_id);
+	RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE, "\n");
+}
+
+void rtl88ee_tx_fill_cmddesc(struct ieee80211_hw *hw,
+			     u8 *pdesc, bool firstseg,
+			     bool lastseg, struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u8 fw_queue = QSLT_BEACON;
+
+	dma_addr_t mapping = pci_map_single(rtlpci->pdev,
+					    skb->data, skb->len,
+					    PCI_DMA_TODEVICE);
+
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(skb->data);
+	__le16 fc = hdr->frame_control;
+
+	if (pci_dma_mapping_error(rtlpci->pdev, mapping)) {
+		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+			 "DMA mapping error");
+		return;
+	}
+	CLEAR_PCI_TX_DESC_CONTENT(pdesc, TX_DESC_SIZE);
+
+	if (firstseg)
+		SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
+
+	SET_TX_DESC_TX_RATE(pdesc, DESC92C_RATE1M);
+
+	SET_TX_DESC_SEQ(pdesc, 0);
+
+	SET_TX_DESC_LINIP(pdesc, 0);
+
+	SET_TX_DESC_QUEUE_SEL(pdesc, fw_queue);
+
+	SET_TX_DESC_FIRST_SEG(pdesc, 1);
+	SET_TX_DESC_LAST_SEG(pdesc, 1);
+
+	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16)(skb->len));
+
+	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, mapping);
+
+	SET_TX_DESC_RATE_ID(pdesc, 7);
+	SET_TX_DESC_MACID(pdesc, 0);
+
+	SET_TX_DESC_OWN(pdesc, 1);
+
+	SET_TX_DESC_PKT_SIZE((u8 *)pdesc, (u16)(skb->len));
+
+	SET_TX_DESC_FIRST_SEG(pdesc, 1);
+	SET_TX_DESC_LAST_SEG(pdesc, 1);
+
+	SET_TX_DESC_OFFSET(pdesc, 0x20);
+
+	SET_TX_DESC_USE_RATE(pdesc, 1);
+
+	if (!ieee80211_is_data_qos(fc))
+		SET_TX_DESC_HWSEQ_EN(pdesc, 1);
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
+		      "H2C Tx Cmd Content\n",
+		      pdesc, TX_DESC_SIZE);
+}
+
+void rtl88ee_set_desc(u8 *pdesc, bool istx, u8 desc_name, u8 *val)
+{
+	if (istx == true) {
+		switch (desc_name) {
+		case HW_DESC_OWN:
+			SET_TX_DESC_OWN(pdesc, 1);
+			break;
+		case HW_DESC_TX_NEXTDESC_ADDR:
+			SET_TX_DESC_NEXT_DESC_ADDRESS(pdesc, *(u32 *)val);
+			break;
+		default:
+			RT_ASSERT(false, "ERR txdesc :%d not processed\n",
+				  desc_name);
+			break;
+		}
+	} else {
+		switch (desc_name) {
+		case HW_DESC_RXOWN:
+			SET_RX_DESC_OWN(pdesc, 1);
+			break;
+		case HW_DESC_RXBUFF_ADDR:
+			SET_RX_DESC_BUFF_ADDR(pdesc, *(u32 *)val);
+			break;
+		case HW_DESC_RXPKT_LEN:
+			SET_RX_DESC_PKT_LEN(pdesc, *(u32 *)val);
+			break;
+		case HW_DESC_RXERO:
+			SET_RX_DESC_EOR(pdesc, 1);
+			break;
+		default:
+			RT_ASSERT(false, "ERR rxdesc :%d not processed\n",
+				  desc_name);
+			break;
+		}
+	}
+}
+
+u32 rtl88ee_get_desc(u8 *pdesc, bool istx, u8 desc_name)
+{
+	u32 ret = 0;
+
+	if (istx == true) {
+		switch (desc_name) {
+		case HW_DESC_OWN:
+			ret = GET_TX_DESC_OWN(pdesc);
+			break;
+		case HW_DESC_TXBUFF_ADDR:
+			ret = GET_TX_DESC_TX_BUFFER_ADDRESS(pdesc);
+			break;
+		default:
+			RT_ASSERT(false, "ERR txdesc :%d not processed\n",
+				  desc_name);
+			break;
+		}
+	} else {
+		switch (desc_name) {
+		case HW_DESC_OWN:
+			ret = GET_RX_DESC_OWN(pdesc);
+			break;
+		case HW_DESC_RXPKT_LEN:
+			ret = GET_RX_DESC_PKT_LEN(pdesc);
+			break;
+		default:
+			RT_ASSERT(false, "ERR rxdesc :%d not processed\n",
+				  desc_name);
+			break;
+		}
+	}
+	return ret;
+}
+
+void rtl88ee_tx_polling(struct ieee80211_hw *hw, u8 hw_queue)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	if (hw_queue == BEACON_QUEUE) {
+		rtl_write_word(rtlpriv, REG_PCIE_CTRL_REG, BIT(4));
+	} else {
+		rtl_write_word(rtlpriv, REG_PCIE_CTRL_REG,
+			       BIT(0) << (hw_queue));
+	}
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/trx.h b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.h
new file mode 100644
index 000000000000..d3a02e73f53a
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.h
@@ -0,0 +1,795 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92CE_TRX_H__
+#define __RTL92CE_TRX_H__
+
+#define TX_DESC_SIZE				64
+#define TX_DESC_AGGR_SUBFRAME_SIZE		32
+
+#define RX_DESC_SIZE				32
+#define RX_DRV_INFO_SIZE_UNIT			8
+
+#define	TX_DESC_NEXT_DESC_OFFSET		40
+#define USB_HWDESC_HEADER_LEN			32
+#define CRCLENGTH				4
+
+#define SET_TX_DESC_PKT_SIZE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 16, __val)
+#define SET_TX_DESC_OFFSET(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 16, 8, __val)
+#define SET_TX_DESC_BMC(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 24, 1, __val)
+#define SET_TX_DESC_HTC(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 25, 1, __val)
+#define SET_TX_DESC_LAST_SEG(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 26, 1, __val)
+#define SET_TX_DESC_FIRST_SEG(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 27, 1, __val)
+#define SET_TX_DESC_LINIP(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 28, 1, __val)
+#define SET_TX_DESC_NO_ACM(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 29, 1, __val)
+#define SET_TX_DESC_GF(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 30, 1, __val)
+#define SET_TX_DESC_OWN(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 31, 1, __val)
+
+#define GET_TX_DESC_PKT_SIZE(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 0, 16)
+#define GET_TX_DESC_OFFSET(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 16, 8)
+#define GET_TX_DESC_BMC(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 24, 1)
+#define GET_TX_DESC_HTC(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 25, 1)
+#define GET_TX_DESC_LAST_SEG(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 26, 1)
+#define GET_TX_DESC_FIRST_SEG(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 27, 1)
+#define GET_TX_DESC_LINIP(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 28, 1)
+#define GET_TX_DESC_NO_ACM(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 29, 1)
+#define GET_TX_DESC_GF(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc, 30, 1)
+#define GET_TX_DESC_OWN(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 31, 1)
+
+#define SET_TX_DESC_MACID(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 0, 6, __val)
+#define SET_TX_DESC_QUEUE_SEL(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 8, 5, __val)
+#define SET_TX_DESC_RDG_NAV_EXT(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 13, 1, __val)
+#define SET_TX_DESC_LSIG_TXOP_EN(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 14, 1, __val)
+#define SET_TX_DESC_PIFS(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 15, 1, __val)
+#define SET_TX_DESC_RATE_ID(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 16, 4, __val)
+#define SET_TX_DESC_NAV_USE_HDR(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 20, 1, __val)
+#define SET_TX_DESC_EN_DESC_ID(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 21, 1, __val)
+#define SET_TX_DESC_SEC_TYPE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 22, 2, __val)
+#define SET_TX_DESC_PKT_OFFSET(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 26, 5, __val)
+#define SET_TX_DESC_PADDING_LEN(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 24, 8, __val)
+
+#define GET_TX_DESC_MACID(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 0, 5)
+#define GET_TX_DESC_AGG_ENABLE(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 5, 1)
+#define GET_TX_DESC_AGG_BREAK(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 6, 1)
+#define GET_TX_DESC_RDG_ENABLE(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 7, 1)
+#define GET_TX_DESC_QUEUE_SEL(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 8, 5)
+#define GET_TX_DESC_RDG_NAV_EXT(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 13, 1)
+#define GET_TX_DESC_LSIG_TXOP_EN(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 14, 1)
+#define GET_TX_DESC_PIFS(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 15, 1)
+#define GET_TX_DESC_RATE_ID(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 16, 4)
+#define GET_TX_DESC_NAV_USE_HDR(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 20, 1)
+#define GET_TX_DESC_EN_DESC_ID(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 21, 1)
+#define GET_TX_DESC_SEC_TYPE(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 22, 2)
+#define GET_TX_DESC_PKT_OFFSET(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 24, 8)
+
+#define SET_TX_DESC_RTS_RC(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 0, 6, __val)
+#define SET_TX_DESC_DATA_RC(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 6, 6, __val)
+#define SET_TX_DESC_AGG_ENABLE(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 12, 1, __val)
+#define SET_TX_DESC_RDG_ENABLE(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 13, 1, __val)
+#define SET_TX_DESC_BAR_RTY_TH(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 14, 2, __val)
+#define SET_TX_DESC_AGG_BREAK(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 16, 1, __val)
+#define SET_TX_DESC_MORE_FRAG(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 17, 1, __val)
+#define SET_TX_DESC_RAW(__pdesc, __val)				\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 18, 1, __val)
+#define SET_TX_DESC_CCX(__pdesc, __val)				\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 19, 1, __val)
+#define SET_TX_DESC_AMPDU_DENSITY(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 20, 3, __val)
+#define SET_TX_DESC_BT_INT(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 23, 1, __val)
+#define SET_TX_DESC_ANTSEL_A(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 24, 1, __val)
+#define SET_TX_DESC_ANTSEL_B(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 25, 1, __val)
+#define SET_TX_DESC_TX_ANT_CCK(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 26, 2, __val)
+#define SET_TX_DESC_TX_ANTL(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 28, 2, __val)
+#define SET_TX_DESC_TX_ANT_HT(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 30, 2, __val)
+
+#define GET_TX_DESC_RTS_RC(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+8, 0, 6)
+#define GET_TX_DESC_DATA_RC(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+8, 6, 6)
+#define GET_TX_DESC_BAR_RTY_TH(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+8, 14, 2)
+#define GET_TX_DESC_MORE_FRAG(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+8, 17, 1)
+#define GET_TX_DESC_RAW(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+8, 18, 1)
+#define GET_TX_DESC_CCX(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+8, 19, 1)
+#define GET_TX_DESC_AMPDU_DENSITY(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+8, 20, 3)
+#define GET_TX_DESC_ANTSEL_A(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+8, 24, 1)
+#define GET_TX_DESC_ANTSEL_B(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+8, 25, 1)
+#define GET_TX_DESC_TX_ANT_CCK(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+8, 26, 2)
+#define GET_TX_DESC_TX_ANTL(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+8, 28, 2)
+#define GET_TX_DESC_TX_ANT_HT(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+8, 30, 2)
+
+#define SET_TX_DESC_NEXT_HEAP_PAGE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 0, 8, __val)
+#define SET_TX_DESC_TAIL_PAGE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 8, 8, __val)
+#define SET_TX_DESC_SEQ(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 16, 12, __val)
+#define SET_TX_DESC_CPU_HANDLE(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 28, 1, __val)
+#define SET_TX_DESC_TAG1(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 29, 1, __val)
+#define SET_TX_DESC_TRIGGER_INT(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 30, 1, __val)
+#define SET_TX_DESC_HWSEQ_EN(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 31, 1, __val)
+
+
+#define GET_TX_DESC_NEXT_HEAP_PAGE(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+12, 0, 8)
+#define GET_TX_DESC_TAIL_PAGE(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+12, 8, 8)
+#define GET_TX_DESC_SEQ(__pdesc)					\
+	LE_BITS_TO_4BYTE(__pdesc+12, 16, 12)
+
+
+#define SET_TX_DESC_RTS_RATE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 0, 5, __val)
+#define SET_TX_DESC_AP_DCFE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 5, 1, __val)
+#define SET_TX_DESC_QOS(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 6, 1, __val)
+#define SET_TX_DESC_HWSEQ_SSN(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 7, 1, __val)
+#define SET_TX_DESC_USE_RATE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 8, 1, __val)
+#define SET_TX_DESC_DISABLE_RTS_FB(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 9, 1, __val)
+#define SET_TX_DESC_DISABLE_FB(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 10, 1, __val)
+#define SET_TX_DESC_CTS2SELF(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 11, 1, __val)
+#define SET_TX_DESC_RTS_ENABLE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 12, 1, __val)
+#define SET_TX_DESC_HW_RTS_ENABLE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 13, 1, __val)
+#define SET_TX_DESC_PORT_ID(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 14, 1, __val)
+#define SET_TX_DESC_PWR_STATUS(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 15, 3, __val)
+#define SET_TX_DESC_WAIT_DCTS(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 18, 1, __val)
+#define SET_TX_DESC_CTS2AP_EN(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 19, 1, __val)
+#define SET_TX_DESC_TX_SUB_CARRIER(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 20, 2, __val)
+#define SET_TX_DESC_TX_STBC(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 22, 2, __val)
+#define SET_TX_DESC_DATA_SHORT(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 24, 1, __val)
+#define SET_TX_DESC_DATA_BW(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 25, 1, __val)
+#define SET_TX_DESC_RTS_SHORT(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 26, 1, __val)
+#define SET_TX_DESC_RTS_BW(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 27, 1, __val)
+#define SET_TX_DESC_RTS_SC(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 28, 2, __val)
+#define SET_TX_DESC_RTS_STBC(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 30, 2, __val)
+
+#define GET_TX_DESC_RTS_RATE(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 0, 5)
+#define GET_TX_DESC_AP_DCFE(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 5, 1)
+#define GET_TX_DESC_QOS(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 6, 1)
+#define GET_TX_DESC_HWSEQ_EN(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 7, 1)
+#define GET_TX_DESC_USE_RATE(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 8, 1)
+#define GET_TX_DESC_DISABLE_RTS_FB(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+16, 9, 1)
+#define GET_TX_DESC_DISABLE_FB(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 10, 1)
+#define GET_TX_DESC_CTS2SELF(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 11, 1)
+#define GET_TX_DESC_RTS_ENABLE(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 12, 1)
+#define GET_TX_DESC_HW_RTS_ENABLE(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+16, 13, 1)
+#define GET_TX_DESC_PORT_ID(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 14, 1)
+#define GET_TX_DESC_WAIT_DCTS(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 18, 1)
+#define GET_TX_DESC_CTS2AP_EN(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 19, 1)
+#define GET_TX_DESC_TX_SUB_CARRIER(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+16, 20, 2)
+#define GET_TX_DESC_TX_STBC(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 22, 2)
+#define GET_TX_DESC_DATA_SHORT(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 24, 1)
+#define GET_TX_DESC_DATA_BW(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 25, 1)
+#define GET_TX_DESC_RTS_SHORT(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 26, 1)
+#define GET_TX_DESC_RTS_BW(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 27, 1)
+#define GET_TX_DESC_RTS_SC(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 28, 2)
+#define GET_TX_DESC_RTS_STBC(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 30, 2)
+
+#define SET_TX_DESC_TX_RATE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 0, 6, __val)
+#define SET_TX_DESC_DATA_SHORTGI(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 6, 1, __val)
+#define SET_TX_DESC_CCX_TAG(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 7, 1, __val)
+#define SET_TX_DESC_DATA_RATE_FB_LIMIT(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 8, 5, __val)
+#define SET_TX_DESC_RTS_RATE_FB_LIMIT(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 13, 4, __val)
+#define SET_TX_DESC_RETRY_LIMIT_ENABLE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 17, 1, __val)
+#define SET_TX_DESC_DATA_RETRY_LIMIT(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 18, 6, __val)
+#define SET_TX_DESC_USB_TXAGG_NUM(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 24, 8, __val)
+
+#define GET_TX_DESC_TX_RATE(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+20, 0, 6)
+#define GET_TX_DESC_DATA_SHORTGI(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+20, 6, 1)
+#define GET_TX_DESC_CCX_TAG(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+20, 7, 1)
+#define GET_TX_DESC_DATA_RATE_FB_LIMIT(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+20, 8, 5)
+#define GET_TX_DESC_RTS_RATE_FB_LIMIT(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+20, 13, 4)
+#define GET_TX_DESC_RETRY_LIMIT_ENABLE(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+20, 17, 1)
+#define GET_TX_DESC_DATA_RETRY_LIMIT(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+20, 18, 6)
+#define GET_TX_DESC_USB_TXAGG_NUM(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+20, 24, 8)
+
+#define SET_TX_DESC_TXAGC_A(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 0, 5, __val)
+#define SET_TX_DESC_TXAGC_B(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 5, 5, __val)
+#define SET_TX_DESC_USE_MAX_LEN(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 10, 1, __val)
+#define SET_TX_DESC_MAX_AGG_NUM(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 11, 5, __val)
+#define SET_TX_DESC_MCSG1_MAX_LEN(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 16, 4, __val)
+#define SET_TX_DESC_MCSG2_MAX_LEN(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 20, 4, __val)
+#define SET_TX_DESC_MCSG3_MAX_LEN(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 24, 4, __val)
+#define SET_TX_DESC_MCS7_SGI_MAX_LEN(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 28, 4, __val)
+
+#define GET_TX_DESC_TXAGC_A(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+24, 0, 5)
+#define GET_TX_DESC_TXAGC_B(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+24, 5, 5)
+#define GET_TX_DESC_USE_MAX_LEN(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+24, 10, 1)
+#define GET_TX_DESC_MAX_AGG_NUM(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+24, 11, 5)
+#define GET_TX_DESC_MCSG1_MAX_LEN(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+24, 16, 4)
+#define GET_TX_DESC_MCSG2_MAX_LEN(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+24, 20, 4)
+#define GET_TX_DESC_MCSG3_MAX_LEN(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+24, 24, 4)
+#define GET_TX_DESC_MCS7_SGI_MAX_LEN(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+24, 28, 4)
+
+#define SET_TX_DESC_TX_BUFFER_SIZE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 16, __val)
+#define SET_TX_DESC_SW_OFFSET30(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 16, 8, __val)
+#define SET_TX_DESC_SW_OFFSET31(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 24, 4, __val)
+#define SET_TX_DESC_ANTSEL_C(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 29, 1, __val)
+#define SET_TX_DESC_NULL_0(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 30, 1, __val)
+#define SET_TX_DESC_NULL_1(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 30, 1, __val)
+
+#define GET_TX_DESC_TX_BUFFER_SIZE(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+28, 0, 16)
+
+
+#define SET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+32, 0, 32, __val)
+#define SET_TX_DESC_TX_BUFFER_ADDRESS64(__pdesc, __val) \
+	SET_BITS_TO_LE_4BYTE(__pdesc+36, 0, 32, __val)
+
+#define GET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+32, 0, 32)
+#define GET_TX_DESC_TX_BUFFER_ADDRESS64(__pdesc)	\
+	LE_BITS_TO_4BYTE(__pdesc+36, 0, 32)
+
+#define SET_TX_DESC_NEXT_DESC_ADDRESS(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+40, 0, 32, __val)
+#define SET_TX_DESC_NEXT_DESC_ADDRESS64(__pdesc, __val) \
+	SET_BITS_TO_LE_4BYTE(__pdesc+44, 0, 32, __val)
+
+#define GET_TX_DESC_NEXT_DESC_ADDRESS(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+40, 0, 32)
+#define GET_TX_DESC_NEXT_DESC_ADDRESS64(__pdesc)	\
+	LE_BITS_TO_4BYTE(__pdesc+44, 0, 32)
+
+#define GET_RX_DESC_PKT_LEN(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 0, 14)
+#define GET_RX_DESC_CRC32(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 14, 1)
+#define GET_RX_DESC_ICV(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 15, 1)
+#define GET_RX_DESC_DRV_INFO_SIZE(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc, 16, 4)
+#define GET_RX_DESC_SECURITY(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 20, 3)
+#define GET_RX_DESC_QOS(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 23, 1)
+#define GET_RX_DESC_SHIFT(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 24, 2)
+#define GET_RX_DESC_PHYST(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 26, 1)
+#define GET_RX_DESC_SWDEC(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 27, 1)
+#define GET_RX_DESC_LS(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc, 28, 1)
+#define GET_RX_DESC_FS(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc, 29, 1)
+#define GET_RX_DESC_EOR(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 30, 1)
+#define GET_RX_DESC_OWN(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 31, 1)
+
+#define SET_RX_DESC_PKT_LEN(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 14, __val)
+#define SET_RX_DESC_EOR(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 30, 1, __val)
+#define SET_RX_DESC_OWN(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 31, 1, __val)
+
+#define GET_RX_DESC_MACID(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 0, 6)
+#define GET_RX_DESC_PAGGR(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 14, 1)
+#define GET_RX_DESC_FAGGR(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 15, 1)
+#define GET_RX_DESC_A1_FIT(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 16, 4)
+#define GET_RX_DESC_A2_FIT(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 20, 4)
+#define GET_RX_DESC_PAM(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 24, 1)
+#define GET_RX_DESC_PWR(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 25, 1)
+#define GET_RX_DESC_MD(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 26, 1)
+#define GET_RX_DESC_MF(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 27, 1)
+#define GET_RX_DESC_TYPE(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 28, 2)
+#define GET_RX_DESC_MC(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 30, 1)
+#define GET_RX_DESC_BC(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 31, 1)
+#define GET_RX_DESC_SEQ(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+8, 0, 12)
+#define GET_RX_DESC_FRAG(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+8, 12, 4)
+
+#define GET_RX_DESC_RXMCS(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+12, 0, 6)
+#define GET_RX_DESC_RXHT(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+12, 6, 1)
+#define GET_RX_STATUS_DESC_RX_GF(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+12, 7, 1)
+#define GET_RX_DESC_SPLCP(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+12, 8, 1)
+#define GET_RX_DESC_BW(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+12, 9, 1)
+#define GET_RX_DESC_HTC(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+12, 10, 1)
+#define GET_RX_STATUS_DESC_EOSP(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+12, 11, 1)
+#define GET_RX_STATUS_DESC_BSSID_FIT(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+12, 12, 2)
+#define GET_RX_STATUS_DESC_RPT_SEL(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+12, 14, 2)
+
+#define GET_RX_STATUS_DESC_PATTERN_MATCH(__pdesc)	\
+	LE_BITS_TO_4BYTE(__pdesc+12, 29, 1)
+#define GET_RX_STATUS_DESC_UNICAST_MATCH(__pdesc)	\
+	LE_BITS_TO_4BYTE(__pdesc+12, 30, 1)
+#define GET_RX_STATUS_DESC_MAGIC_MATCH(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+12, 31, 1)
+
+#define GET_RX_DESC_IV1(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 0, 32)
+#define GET_RX_DESC_TSFL(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+20, 0, 32)
+
+#define GET_RX_DESC_BUFF_ADDR(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+24, 0, 32)
+#define GET_RX_DESC_BUFF_ADDR64(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+28, 0, 32)
+
+#define SET_RX_DESC_BUFF_ADDR(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 0, 32, __val)
+#define SET_RX_DESC_BUFF_ADDR64(__pdesc, __val) \
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 32, __val)
+
+/* TX report 2 format in Rx desc*/
+
+#define GET_RX_RPT2_DESC_PKT_LEN(__status)	\
+	LE_BITS_TO_4BYTE(__status, 0, 9)
+#define GET_RX_RPT2_DESC_MACID_VALID_1(__status)	\
+	LE_BITS_TO_4BYTE(__status+16, 0, 32)
+#define GET_RX_RPT2_DESC_MACID_VALID_2(__status)	\
+	LE_BITS_TO_4BYTE(__status+20, 0, 32)
+
+#define SET_EARLYMODE_PKTNUM(__paddr, __value)	\
+	SET_BITS_TO_LE_4BYTE(__paddr, 0, 4, __value)
+#define SET_EARLYMODE_LEN0(__paddr, __value)	\
+	SET_BITS_TO_LE_4BYTE(__paddr, 4, 12, __value)
+#define SET_EARLYMODE_LEN1(__paddr, __value)	\
+	SET_BITS_TO_LE_4BYTE(__paddr, 16, 12, __value)
+#define SET_EARLYMODE_LEN2_1(__paddr, __value)	\
+	SET_BITS_TO_LE_4BYTE(__paddr, 28, 4, __value)
+#define SET_EARLYMODE_LEN2_2(__paddr, __value)	\
+	SET_BITS_TO_LE_4BYTE(__paddr+4, 0, 8, __value)
+#define SET_EARLYMODE_LEN3(__paddr, __value)	\
+	SET_BITS_TO_LE_4BYTE(__paddr+4, 8, 12, __value)
+#define SET_EARLYMODE_LEN4(__paddr, __value)	\
+	SET_BITS_TO_LE_4BYTE(__paddr+4, 20, 12, __value)
+
+#define CLEAR_PCI_TX_DESC_CONTENT(__pdesc, _size)		\
+do {								\
+	if (_size > TX_DESC_NEXT_DESC_OFFSET)			\
+		memset(__pdesc, 0, TX_DESC_NEXT_DESC_OFFSET);	\
+	else							\
+		memset(__pdesc, 0, _size);			\
+} while (0)
+
+#define RTL8188_RX_HAL_IS_CCK_RATE(rxmcs)\
+	(rxmcs == DESC92C_RATE1M ||\
+	 rxmcs == DESC92C_RATE2M ||\
+	 rxmcs == DESC92C_RATE5_5M ||\
+	 rxmcs == DESC92C_RATE11M)
+
+struct phy_rx_agc_info_t {
+	#if __LITTLE_ENDIAN
+		u8	gain:7, trsw:1;
+	#else
+		u8	trsw:1, gain:7;
+	#endif
+};
+struct phy_status_rpt {
+	struct phy_rx_agc_info_t path_agc[2];
+	u8	ch_corr[2];
+	u8	cck_sig_qual_ofdm_pwdb_all;
+	u8	cck_agc_rpt_ofdm_cfosho_a;
+	u8	cck_rpt_b_ofdm_cfosho_b;
+	u8	rsvd_1;
+	u8	noise_power_db_msb;
+	u8	path_cfotail[2];
+	u8	pcts_mask[2];
+	u8	stream_rxevm[2];
+	u8	path_rxsnr[2];
+	u8	noise_power_db_lsb;
+	u8	rsvd_2[3];
+	u8	stream_csi[2];
+	u8	stream_target_csi[2];
+	u8	sig_evm;
+	u8	rsvd_3;
+#if __LITTLE_ENDIAN
+	u8	antsel_rx_keep_2:1;	/*ex_intf_flg:1;*/
+	u8	sgi_en:1;
+	u8	rxsc:2;
+	u8	idle_long:1;
+	u8	r_ant_train_en:1;
+	u8	ant_sel_b:1;
+	u8	ant_sel:1;
+#else	/* _BIG_ENDIAN_	*/
+	u8	ant_sel:1;
+	u8	ant_sel_b:1;
+	u8	r_ant_train_en:1;
+	u8	idle_long:1;
+	u8	rxsc:2;
+	u8	sgi_en:1;
+	u8	antsel_rx_keep_2:1;	/*ex_intf_flg:1;*/
+#endif
+} __packed;
+
+struct rx_fwinfo_88e {
+	u8 gain_trsw[4];
+	u8 pwdb_all;
+	u8 cfosho[4];
+	u8 cfotail[4];
+	char rxevm[2];
+	char rxsnr[4];
+	u8 pdsnr[2];
+	u8 csi_current[2];
+	u8 csi_target[2];
+	u8 sigevm;
+	u8 max_ex_pwr;
+	u8 ex_intf_flag:1;
+	u8 sgi_en:1;
+	u8 rxsc:2;
+	u8 reserve:4;
+} __packed;
+
+struct tx_desc_88e {
+	u32 pktsize:16;
+	u32 offset:8;
+	u32 bmc:1;
+	u32 htc:1;
+	u32 lastseg:1;
+	u32 firstseg:1;
+	u32 linip:1;
+	u32 noacm:1;
+	u32 gf:1;
+	u32 own:1;
+
+	u32 macid:6;
+	u32 rsvd0:2;
+	u32 queuesel:5;
+	u32 rd_nav_ext:1;
+	u32 lsig_txop_en:1;
+	u32 pifs:1;
+	u32 rateid:4;
+	u32 nav_usehdr:1;
+	u32 en_descid:1;
+	u32 sectype:2;
+	u32 pktoffset:8;
+
+	u32 rts_rc:6;
+	u32 data_rc:6;
+	u32 agg_en:1;
+	u32 rdg_en:1;
+	u32 bar_retryht:2;
+	u32 agg_break:1;
+	u32 morefrag:1;
+	u32 raw:1;
+	u32 ccx:1;
+	u32 ampdudensity:3;
+	u32 bt_int:1;
+	u32 ant_sela:1;
+	u32 ant_selb:1;
+	u32 txant_cck:2;
+	u32 txant_l:2;
+	u32 txant_ht:2;
+
+	u32 nextheadpage:8;
+	u32 tailpage:8;
+	u32 seq:12;
+	u32 cpu_handle:1;
+	u32 tag1:1;
+	u32 trigger_int:1;
+	u32 hwseq_en:1;
+
+	u32 rtsrate:5;
+	u32 apdcfe:1;
+	u32 qos:1;
+	u32 hwseq_ssn:1;
+	u32 userrate:1;
+	u32 dis_rtsfb:1;
+	u32 dis_datafb:1;
+	u32 cts2self:1;
+	u32 rts_en:1;
+	u32 hwrts_en:1;
+	u32 portid:1;
+	u32 pwr_status:3;
+	u32 waitdcts:1;
+	u32 cts2ap_en:1;
+	u32 txsc:2;
+	u32 stbc:2;
+	u32 txshort:1;
+	u32 txbw:1;
+	u32 rtsshort:1;
+	u32 rtsbw:1;
+	u32 rtssc:2;
+	u32 rtsstbc:2;
+
+	u32 txrate:6;
+	u32 shortgi:1;
+	u32 ccxt:1;
+	u32 txrate_fb_lmt:5;
+	u32 rtsrate_fb_lmt:4;
+	u32 retrylmt_en:1;
+	u32 txretrylmt:6;
+	u32 usb_txaggnum:8;
+
+	u32 txagca:5;
+	u32 txagcb:5;
+	u32 usemaxlen:1;
+	u32 maxaggnum:5;
+	u32 mcsg1maxlen:4;
+	u32 mcsg2maxlen:4;
+	u32 mcsg3maxlen:4;
+	u32 mcs7sgimaxlen:4;
+
+	u32 txbuffersize:16;
+	u32 sw_offset30:8;
+	u32 sw_offset31:4;
+	u32 rsvd1:1;
+	u32 antsel_c:1;
+	u32 null_0:1;
+	u32 null_1:1;
+
+	u32 txbuffaddr;
+	u32 txbufferaddr64;
+	u32 nextdescaddress;
+	u32 nextdescaddress64;
+
+	u32 reserve_pass_pcie_mm_limit[4];
+} __packed;
+
+struct rx_desc_88e {
+	u32 length:14;
+	u32 crc32:1;
+	u32 icverror:1;
+	u32 drv_infosize:4;
+	u32 security:3;
+	u32 qos:1;
+	u32 shift:2;
+	u32 phystatus:1;
+	u32 swdec:1;
+	u32 lastseg:1;
+	u32 firstseg:1;
+	u32 eor:1;
+	u32 own:1;
+
+	u32 macid:6;
+	u32 tid:4;
+	u32 hwrsvd:5;
+	u32 paggr:1;
+	u32 faggr:1;
+	u32 a1_fit:4;
+	u32 a2_fit:4;
+	u32 pam:1;
+	u32 pwr:1;
+	u32 moredata:1;
+	u32 morefrag:1;
+	u32 type:2;
+	u32 mc:1;
+	u32 bc:1;
+
+	u32 seq:12;
+	u32 frag:4;
+	u32 nextpktlen:14;
+	u32 nextind:1;
+	u32 rsvd:1;
+
+	u32 rxmcs:6;
+	u32 rxht:1;
+	u32 amsdu:1;
+	u32 splcp:1;
+	u32 bandwidth:1;
+	u32 htc:1;
+	u32 tcpchk_rpt:1;
+	u32 ipcchk_rpt:1;
+	u32 tcpchk_valid:1;
+	u32 hwpcerr:1;
+	u32 hwpcind:1;
+	u32 iv0:16;
+
+	u32 iv1;
+
+	u32 tsfl;
+
+	u32 bufferaddress;
+	u32 bufferaddress64;
+
+} __packed;
+
+void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
+			  struct ieee80211_hdr *hdr, u8 *pdesc_tx,
+			  struct ieee80211_tx_info *info,
+			  struct ieee80211_sta *sta,
+			  struct sk_buff *skb,
+			  u8 hw_queue, struct rtl_tcb_desc *ptcb_desc);
+bool rtl88ee_rx_query_desc(struct ieee80211_hw *hw,
+			   struct rtl_stats *status,
+			   struct ieee80211_rx_status *rx_status,
+			   u8 *pdesc, struct sk_buff *skb);
+void rtl88ee_set_desc(u8 *pdesc, bool istx, u8 desc_name, u8 *val);
+u32 rtl88ee_get_desc(u8 *pdesc, bool istx, u8 desc_name);
+void rtl88ee_tx_polling(struct ieee80211_hw *hw, u8 hw_queue);
+void rtl88ee_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
+			     bool b_firstseg, bool b_lastseg,
+			     struct sk_buff *skb);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
index b793a659a465..d2d57a27a7c1 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
@@ -174,8 +174,8 @@ static void rtl92c_dm_diginit(struct ieee80211_hw *hw)
 	dm_digtable->rssi_highthresh = DM_DIG_THRESH_HIGH;
 	dm_digtable->fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
 	dm_digtable->fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
-	dm_digtable->rx_gain_range_max = DM_DIG_MAX;
-	dm_digtable->rx_gain_range_min = DM_DIG_MIN;
+	dm_digtable->rx_gain_max = DM_DIG_MAX;
+	dm_digtable->rx_gain_min = DM_DIG_MIN;
 	dm_digtable->back_val = DM_DIG_BACKOFF_DEFAULT;
 	dm_digtable->back_range_max = DM_DIG_BACKOFF_MAX;
 	dm_digtable->back_range_min = DM_DIG_BACKOFF_MIN;
@@ -300,11 +300,11 @@ static void rtl92c_dm_ctrl_initgain_by_rssi(struct ieee80211_hw *hw)
 	}
 
 	if ((digtable->rssi_val_min + 10 - digtable->back_val) >
-	    digtable->rx_gain_range_max)
-		digtable->cur_igvalue = digtable->rx_gain_range_max;
+	    digtable->rx_gain_max)
+		digtable->cur_igvalue = digtable->rx_gain_max;
 	else if ((digtable->rssi_val_min + 10 -
-		  digtable->back_val) < digtable->rx_gain_range_min)
-		digtable->cur_igvalue = digtable->rx_gain_range_min;
+		  digtable->back_val) < digtable->rx_gain_min)
+		digtable->cur_igvalue = digtable->rx_gain_min;
 	else
 		digtable->cur_igvalue = digtable->rssi_val_min + 10 -
 		    digtable->back_val;
@@ -669,7 +669,7 @@ static void rtl92c_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
 	u8 thermalvalue, delta, delta_lck, delta_iqk;
 	long ele_a, ele_d, temp_cck, val_x, value32;
 	long val_y, ele_c = 0;
-	u8 ofdm_index[2], ofdm_index_old[2], cck_index_old = 0;
+	u8 ofdm_index[2], ofdm_index_old[2] = {0, 0}, cck_index_old = 0;
 	s8 cck_index = 0;
 	int i;
 	bool is2t = IS_92C_SERIAL(rtlhal->version);
@@ -717,7 +717,7 @@ static void rtl92c_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
 			for (i = 0; i < OFDM_TABLE_LENGTH; i++) {
 				if (ele_d == (ofdmswing_table[i] &
 				    MASKOFDM_D)) {
-
+					ofdm_index_old[1] = (u8) i;
 					RT_TRACE(rtlpriv, COMP_POWER_TRACKING,
 						 DBG_LOUD,
 						 "Initial pathB ele_d reg0x%x = 0x%lx, ofdm_index=0x%x\n",
@@ -1147,75 +1147,6 @@ void rtl92c_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
 }
 EXPORT_SYMBOL(rtl92c_dm_init_rate_adaptive_mask);
 
-static void rtl92c_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rate_adaptive *p_ra = &(rtlpriv->ra);
-	u32 low_rssi_thresh, high_rssi_thresh;
-	struct ieee80211_sta *sta = NULL;
-
-	if (is_hal_stop(rtlhal)) {
-		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
-			 "<---- driver is going to unload\n");
-		return;
-	}
-
-	if (!rtlpriv->dm.useramask) {
-		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
-			 "<---- driver does not control rate adaptive mask\n");
-		return;
-	}
-
-	if (mac->link_state == MAC80211_LINKED &&
-	    mac->opmode == NL80211_IFTYPE_STATION) {
-		switch (p_ra->pre_ratr_state) {
-		case DM_RATR_STA_HIGH:
-			high_rssi_thresh = 50;
-			low_rssi_thresh = 20;
-			break;
-		case DM_RATR_STA_MIDDLE:
-			high_rssi_thresh = 55;
-			low_rssi_thresh = 20;
-			break;
-		case DM_RATR_STA_LOW:
-			high_rssi_thresh = 50;
-			low_rssi_thresh = 25;
-			break;
-		default:
-			high_rssi_thresh = 50;
-			low_rssi_thresh = 20;
-			break;
-		}
-
-		if (rtlpriv->dm.undec_sm_pwdb > (long)high_rssi_thresh)
-			p_ra->ratr_state = DM_RATR_STA_HIGH;
-		else if (rtlpriv->dm.undec_sm_pwdb > (long)low_rssi_thresh)
-			p_ra->ratr_state = DM_RATR_STA_MIDDLE;
-		else
-			p_ra->ratr_state = DM_RATR_STA_LOW;
-
-		if (p_ra->pre_ratr_state != p_ra->ratr_state) {
-			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD, "RSSI = %ld\n",
-				 rtlpriv->dm.undec_sm_pwdb);
-			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
-				 "RSSI_LEVEL = %d\n", p_ra->ratr_state);
-			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
-				 "PreState = %d, CurState = %d\n",
-				 p_ra->pre_ratr_state, p_ra->ratr_state);
-
-			rcu_read_lock();
-			sta = ieee80211_find_sta(mac->vif, mac->bssid);
-			rtlpriv->cfg->ops->update_rate_tbl(hw, sta,
-					p_ra->ratr_state);
-
-			p_ra->pre_ratr_state = p_ra->ratr_state;
-			rcu_read_unlock();
-		}
-	}
-}
-
 static void rtl92c_dm_init_dynamic_bb_powersaving(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -1437,6 +1368,9 @@ void rtl92c_dm_watchdog(struct ieee80211_hw *hw)
 	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FWLPS_RF_ON,
 				      (u8 *) (&fw_ps_awake));
 
+	if (ppsc->p2p_ps_info.p2p_ps_mode)
+		fw_ps_awake = false;
+
 	if ((ppsc->rfpwr_state == ERFON) && ((!fw_current_inpsmode) &&
 					     fw_ps_awake)
 	    && (!ppsc->rfchange_inprogress)) {
@@ -1446,7 +1380,7 @@ void rtl92c_dm_watchdog(struct ieee80211_hw *hw)
 		rtl92c_dm_dynamic_bb_powersaving(hw);
 		rtl92c_dm_dynamic_txpower(hw);
 		rtl92c_dm_check_txpower_tracking(hw);
-		rtl92c_dm_refresh_rate_adaptive_mask(hw);
+		/* rtl92c_dm_refresh_rate_adaptive_mask(hw); */
 		rtl92c_dm_bt_coexist(hw);
 		rtl92c_dm_check_edca_turbo(hw);
 	}
@@ -1651,7 +1585,7 @@ static void rtl92c_bt_set_normal(struct ieee80211_hw *hw)
 	}
 }
 
-static void rtl92c_bt_ant_isolation(struct ieee80211_hw *hw)
+static void rtl92c_bt_ant_isolation(struct ieee80211_hw *hw, u8 tmp1byte)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
@@ -1673,9 +1607,9 @@ static void rtl92c_bt_ant_isolation(struct ieee80211_hw *hw)
 			    BT_RSSI_STATE_SPECIAL_LOW)) {
 			rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG, 0xa0);
 		} else if (rtlpcipriv->bt_coexist.bt_service == BT_PAN) {
-			rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG, 0x00);
+			rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG, tmp1byte);
 		} else {
-			rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG, 0x00);
+			rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG, tmp1byte);
 		}
 	}
 
@@ -1726,12 +1660,17 @@ static void rtl92c_check_bt_change(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tmp1byte = 0;
 
+	if (IS_81xxC_VENDOR_UMC_B_CUT(rtlhal->version) &&
+	    rtlpcipriv->bt_coexist.bt_coexistence)
+		tmp1byte |= BIT(5);
 	if (rtlpcipriv->bt_coexist.bt_cur_state) {
 		if (rtlpcipriv->bt_coexist.bt_ant_isolation)
-			rtl92c_bt_ant_isolation(hw);
+			rtl92c_bt_ant_isolation(hw, tmp1byte);
 	} else {
-		rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG, 0x00);
+		rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG, tmp1byte);
 		rtlpriv->cfg->ops->set_rfreg(hw, RF90_PATH_A, 0x1e, 0xf0,
 				rtlpcipriv->bt_coexist.bt_rfreg_origin_1e);
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
index 883f23ae9519..04a41628ceed 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
@@ -552,7 +552,9 @@ void rtl92c_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
 
 	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, mode);
-	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode, 1);
+	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
+					 (rtlpriv->mac80211.p2p) ?
+					 ppsc->smart_ps : 1);
 	SET_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(u1_h2c_set_pwrmode,
 					      ppsc->reg_max_lps_awakeintvl);
 
@@ -808,3 +810,98 @@ void rtl92c_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
 	rtl92c_fill_h2c_cmd(hw, H2C_JOINBSSRPT, 1, u1_joinbssrpt_parm);
 }
 EXPORT_SYMBOL(rtl92c_set_fw_joinbss_report_cmd);
+
+static void rtl92c_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw, u8 ctwindow)
+{
+	u8 u1_ctwindow_period[1] = {ctwindow};
+
+	rtl92c_fill_h2c_cmd(hw, H2C_P2P_PS_CTW_CMD, 1, u1_ctwindow_period);
+}
+
+void rtl92c_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
+	struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
+	u8	i;
+	u16	ctwindow;
+	u32	start_time, tsf_low;
+
+	switch (p2p_ps_state) {
+	case P2P_PS_DISABLE:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
+		memset(p2p_ps_offload, 0, sizeof(struct p2p_ps_offload_t));
+		break;
+	case P2P_PS_ENABLE:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
+		/* update CTWindow value. */
+		if (p2pinfo->ctwindow > 0) {
+			p2p_ps_offload->ctwindow_en = 1;
+			ctwindow = p2pinfo->ctwindow;
+			rtl92c_set_p2p_ctw_period_cmd(hw, ctwindow);
+		}
+		/* hw only support 2 set of NoA */
+		for (i = 0; i < p2pinfo->noa_num; i++) {
+			/* To control the register setting for which NOA*/
+			rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
+			if (i == 0)
+				p2p_ps_offload->noa0_en = 1;
+			else
+				p2p_ps_offload->noa1_en = 1;
+
+			/* config P2P NoA Descriptor Register */
+			rtl_write_dword(rtlpriv, 0x5E0,
+					p2pinfo->noa_duration[i]);
+			rtl_write_dword(rtlpriv, 0x5E4,
+					p2pinfo->noa_interval[i]);
+
+			/*Get Current TSF value */
+			tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
+
+			start_time = p2pinfo->noa_start_time[i];
+			if (p2pinfo->noa_count_type[i] != 1) {
+				while (start_time <= (tsf_low+(50*1024))) {
+					start_time += p2pinfo->noa_interval[i];
+					if (p2pinfo->noa_count_type[i] != 255)
+						p2pinfo->noa_count_type[i]--;
+				}
+			}
+			rtl_write_dword(rtlpriv, 0x5E8, start_time);
+			rtl_write_dword(rtlpriv, 0x5EC,
+					p2pinfo->noa_count_type[i]);
+		}
+
+		if ((p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0)) {
+			/* rst p2p circuit */
+			rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
+
+			p2p_ps_offload->offload_en = 1;
+
+			if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
+				p2p_ps_offload->role = 1;
+				p2p_ps_offload->allstasleep = 0;
+			} else {
+				p2p_ps_offload->role = 0;
+			}
+
+			p2p_ps_offload->discovery = 0;
+		}
+		break;
+	case P2P_PS_SCAN:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN\n");
+		p2p_ps_offload->discovery = 1;
+		break;
+	case P2P_PS_SCAN_DONE:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN_DONE\n");
+		p2p_ps_offload->discovery = 0;
+		p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
+		break;
+	default:
+		break;
+	}
+
+	rtl92c_fill_h2c_cmd(hw, H2C_P2P_PS_OFFLOAD, 1, (u8 *)p2p_ps_offload);
+}
+EXPORT_SYMBOL_GPL(rtl92c_set_p2p_ps_offload_cmd);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.h b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.h
index 780ea5b1e24c..15b2055e6212 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.h
@@ -67,6 +67,9 @@ enum rtl8192c_h2c_cmd {
 	H2C_RSVDPAGE = 3,
 	H2C_RSSI_REPORT = 5,
 	H2C_RA_MASK = 6,
+	H2C_MACID_PS_MODE = 7,
+	H2C_P2P_PS_OFFLOAD = 8,
+	H2C_P2P_PS_CTW_CMD = 32,
 	MAX_H2CCMD
 };
 
@@ -95,5 +98,6 @@ void rtl92c_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
 void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
 void rtl92c_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus);
 void usb_writeN_async(struct rtl_priv *rtlpriv, u32 addr, void *data, u16 len);
+void rtl92c_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
index 1b65db7fd651..a82b30a1996c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
@@ -475,6 +475,9 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 
 			break;
 		}
+	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:
+		rtl92c_set_p2p_ps_offload_cmd(hw, (*(u8 *)val));
+		break;
 	case HW_VAR_AID:{
 			u16 u2btmp;
 			u2btmp = rtl_read_word(rtlpriv, REG_BCN_PSR_RPT);
@@ -505,6 +508,40 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			break;
 
 		}
+	case HW_VAR_FW_LPS_ACTION: {
+			bool enter_fwlps = *((bool *)val);
+			u8 rpwm_val, fw_pwrmode;
+			bool fw_current_inps;
+
+			if (enter_fwlps) {
+				rpwm_val = 0x02;	/* RF off */
+				fw_current_inps = true;
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						HW_VAR_FW_PSMODE_STATUS,
+						(u8 *)(&fw_current_inps));
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						HW_VAR_H2C_FW_PWRMODE,
+						(u8 *)(&ppsc->fwctrl_psmode));
+
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						HW_VAR_SET_RPWM,
+						(u8 *)(&rpwm_val));
+			} else {
+				rpwm_val = 0x0C;	/* RF on */
+				fw_pwrmode = FW_PS_ACTIVE_MODE;
+				fw_current_inps = false;
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						HW_VAR_SET_RPWM,
+						(u8 *)(&rpwm_val));
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						HW_VAR_H2C_FW_PWRMODE,
+						(u8 *)(&fw_pwrmode));
+
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						HW_VAR_FW_PSMODE_STATUS,
+						(u8 *)(&fw_current_inps));
+			}
+		break; }
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "switch case not processed\n");
@@ -1105,7 +1142,8 @@ static int _rtl92ce_set_media_status(struct ieee80211_hw *hw,
 	    type == NL80211_IFTYPE_STATION) {
 		_rtl92ce_stop_tx_beacon(hw);
 		_rtl92ce_enable_bcn_sub_func(hw);
-	} else if (type == NL80211_IFTYPE_ADHOC || type == NL80211_IFTYPE_AP) {
+	} else if (type == NL80211_IFTYPE_ADHOC || type == NL80211_IFTYPE_AP ||
+		   type == NL80211_IFTYPE_MESH_POINT) {
 		_rtl92ce_resume_tx_beacon(hw);
 		_rtl92ce_disable_bcn_sub_func(hw);
 	} else {
@@ -1137,6 +1175,11 @@ static int _rtl92ce_set_media_status(struct ieee80211_hw *hw,
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Set Network type to AP!\n");
 		break;
+	case NL80211_IFTYPE_MESH_POINT:
+		bt_msr |= MSR_ADHOC;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to Mesh Point!\n");
+		break;
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "Network type %d not supported!\n", type);
@@ -1184,7 +1227,8 @@ int rtl92ce_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type)
 		return -EOPNOTSUPP;
 
 	if (rtlpriv->mac80211.link_state == MAC80211_LINKED) {
-		if (type != NL80211_IFTYPE_AP)
+		if (type != NL80211_IFTYPE_AP &&
+		    type != NL80211_IFTYPE_MESH_POINT)
 			rtl92ce_set_check_bssid(hw, true);
 	} else {
 		rtl92ce_set_check_bssid(hw, false);
@@ -1459,7 +1503,7 @@ static void _rtl92ce_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		}
 
 		for (i = 0; i < 14; i++) {
-			RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 				"RF(%d)-Ch(%d) [CCK / HT40_1S / HT40_2S] = [0x%x / 0x%x / 0x%x]\n",
 				rf_path, i,
 				rtlefuse->txpwrlevel_cck[rf_path][i],
@@ -1500,11 +1544,11 @@ static void _rtl92ce_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 				      & 0xf0) >> 4);
 			}
 
-			RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 				"RF-%d pwrgroup_ht20[%d] = 0x%x\n",
 				rf_path, i,
 				rtlefuse->pwrgroup_ht20[rf_path][i]);
-			RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 				"RF-%d pwrgroup_ht40[%d] = 0x%x\n",
 				rf_path, i,
 				rtlefuse->pwrgroup_ht40[rf_path][i]);
@@ -1545,19 +1589,19 @@ static void _rtl92ce_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 	    rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][7];
 
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-A Ht20 to HT40 Diff[%d] = 0x%x\n",
 			i, rtlefuse->txpwr_ht20diff[RF90_PATH_A][i]);
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-A Legacy to Ht40 Diff[%d] = 0x%x\n",
 			i, rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][i]);
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-B Ht20 to HT40 Diff[%d] = 0x%x\n",
 			i, rtlefuse->txpwr_ht20diff[RF90_PATH_B][i]);
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-B Legacy to HT40 Diff[%d] = 0x%x\n",
 			i, rtlefuse->txpwr_legacyhtdiff[RF90_PATH_B][i]);
 
@@ -1565,7 +1609,7 @@ static void _rtl92ce_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		rtlefuse->eeprom_regulatory = (hwinfo[RF_OPTION1] & 0x7);
 	else
 		rtlefuse->eeprom_regulatory = 0;
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"eeprom_regulatory = 0x%x\n", rtlefuse->eeprom_regulatory);
 
 	if (!autoload_fail) {
@@ -1575,7 +1619,7 @@ static void _rtl92ce_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		rtlefuse->eeprom_tssi[RF90_PATH_A] = EEPROM_DEFAULT_TSSI;
 		rtlefuse->eeprom_tssi[RF90_PATH_B] = EEPROM_DEFAULT_TSSI;
 	}
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower, "TSSI_A = 0x%x, TSSI_B = 0x%x\n",
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER, "TSSI_A = 0x%x, TSSI_B = 0x%x\n",
 		rtlefuse->eeprom_tssi[RF90_PATH_A],
 		rtlefuse->eeprom_tssi[RF90_PATH_B]);
 
@@ -1589,7 +1633,7 @@ static void _rtl92ce_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		rtlefuse->apk_thermalmeterignore = true;
 
 	rtlefuse->thermalmeter[0] = rtlefuse->eeprom_thermalmeter;
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter);
 }
 
@@ -1629,6 +1673,21 @@ static void _rtl92ce_read_adapter_info(struct ieee80211_hw *hw)
 	if (rtlefuse->autoload_failflag)
 		return;
 
+	rtlefuse->eeprom_vid = *(u16 *)&hwinfo[EEPROM_VID];
+	rtlefuse->eeprom_did = *(u16 *)&hwinfo[EEPROM_DID];
+	rtlefuse->eeprom_svid = *(u16 *)&hwinfo[EEPROM_SVID];
+	rtlefuse->eeprom_smid = *(u16 *)&hwinfo[EEPROM_SMID];
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROMId = 0x%4x\n", eeprom_id);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM VID = 0x%4x\n", rtlefuse->eeprom_vid);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM DID = 0x%4x\n", rtlefuse->eeprom_did);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM SVID = 0x%4x\n", rtlefuse->eeprom_svid);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM SMID = 0x%4x\n", rtlefuse->eeprom_smid);
+
 	for (i = 0; i < 6; i += 2) {
 		usvalue = *(u16 *)&hwinfo[EEPROM_MAC_ADDR + i];
 		*((u16 *) (&rtlefuse->dev_addr[i])) = usvalue;
@@ -1766,6 +1825,9 @@ static void rtl92ce_update_hal_rate_table(struct ieee80211_hw *hw,
 		ratr_value = sta->supp_rates[1] << 4;
 	else
 		ratr_value = sta->supp_rates[0];
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		ratr_value = 0xfff;
+
 	ratr_value |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
 			sta->ht_cap.mcs.rx_mask[0] << 12);
 	switch (wirelessmode) {
@@ -1860,7 +1922,8 @@ static void rtl92ce_update_hal_rate_mask(struct ieee80211_hw *hw,
 
 	sta_entry = (struct rtl_sta_info *) sta->drv_priv;
 	wirelessmode = sta_entry->wireless_mode;
-	if (mac->opmode == NL80211_IFTYPE_STATION)
+	if (mac->opmode == NL80211_IFTYPE_STATION ||
+	    mac->opmode == NL80211_IFTYPE_MESH_POINT)
 		curtxbw_40mhz = mac->bw_40;
 	else if (mac->opmode == NL80211_IFTYPE_AP ||
 		mac->opmode == NL80211_IFTYPE_ADHOC)
@@ -1870,6 +1933,8 @@ static void rtl92ce_update_hal_rate_mask(struct ieee80211_hw *hw,
 		ratr_bitmap = sta->supp_rates[1] << 4;
 	else
 		ratr_bitmap = sta->supp_rates[0];
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		ratr_bitmap = 0xfff;
 	ratr_bitmap |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
 			sta->ht_cap.mcs.rx_mask[0] << 12);
 	switch (wirelessmode) {
@@ -2135,7 +2200,8 @@ void rtl92ce_set_key(struct ieee80211_hw *hw, u32 key_index,
 				macaddr = cam_const_broad;
 				entry_id = key_index;
 			} else {
-				if (mac->opmode == NL80211_IFTYPE_AP) {
+				if (mac->opmode == NL80211_IFTYPE_AP ||
+				    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
 					entry_id = rtl_cam_get_free_entry(hw,
 								 p_macaddr);
 					if (entry_id >=  TOTAL_CAM_ENTRY) {
@@ -2157,7 +2223,8 @@ void rtl92ce_set_key(struct ieee80211_hw *hw, u32 key_index,
 			RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
 				 "delete one entry, entry_id is %d\n",
 				 entry_id);
-			if (mac->opmode == NL80211_IFTYPE_AP)
+			if (mac->opmode == NL80211_IFTYPE_AP ||
+			    mac->opmode == NL80211_IFTYPE_MESH_POINT)
 				rtl_cam_del_entry(hw, p_macaddr);
 			rtl_cam_delete_one_entry(hw, p_macaddr, entry_id);
 		} else {
@@ -2338,3 +2405,24 @@ void rtl92ce_suspend(struct ieee80211_hw *hw)
 void rtl92ce_resume(struct ieee80211_hw *hw)
 {
 }
+
+/* Turn on AAP (RCR:bit 0) for promicuous mode. */
+void rtl92ce_allow_all_destaddr(struct ieee80211_hw *hw,
+	bool allow_all_da, bool write_into_reg)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	if (allow_all_da) {/* Set BIT0 */
+		rtlpci->receive_config |= RCR_AAP;
+	} else {/* Clear BIT0 */
+		rtlpci->receive_config &= ~RCR_AAP;
+	}
+
+	if (write_into_reg)
+		rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
+
+	RT_TRACE(rtlpriv, COMP_TURBO | COMP_INIT, DBG_LOUD,
+		 "receive_config=0x%08X, write_into_reg=%d\n",
+		 rtlpci->receive_config, write_into_reg);
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.h b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.h
index 52a3aea9b3de..2d063b0c7760 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.h
@@ -61,6 +61,8 @@ void rtl92ce_update_interrupt_mask(struct ieee80211_hw *hw,
 void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
 void rtl92ce_update_hal_rate_tbl(struct ieee80211_hw *hw,
 				 struct ieee80211_sta *sta, u8 rssi_level);
+void rtl92ce_update_hal_rate_tbl(struct ieee80211_hw *hw,
+				 struct ieee80211_sta *sta, u8 rssi_level);
 void rtl92ce_update_channel_access_setting(struct ieee80211_hw *hw);
 bool rtl92ce_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid);
 void rtl92ce_enable_hw_security_config(struct ieee80211_hw *hw);
@@ -74,5 +76,7 @@ void rtl8192ce_bt_reg_init(struct ieee80211_hw *hw);
 void rtl8192ce_bt_hw_init(struct ieee80211_hw *hw);
 void rtl92ce_suspend(struct ieee80211_hw *hw);
 void rtl92ce_resume(struct ieee80211_hw *hw);
+void rtl92ce_allow_all_destaddr(struct ieee80211_hw *hw,
+				bool allow_all_da, bool write_into_reg);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/reg.h b/drivers/net/wireless/rtlwifi/rtl8192ce/reg.h
index e4d738f6166d..bd4aef74c056 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/reg.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/reg.h
@@ -544,6 +544,7 @@
 #define	IMR_WLANOFF				BIT(0)
 
 #define EFUSE_REAL_CONTENT_LEN			512
+#define EFUSE_OOB_PROTECT_BYTES			15
 
 #define	EEPROM_DEFAULT_TSSI			0x0
 #define EEPROM_DEFAULT_TXPOWERDIFF		0x0
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c b/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
index 49f663bd93ff..14203561b6ee 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
@@ -228,6 +228,7 @@ static struct rtl_hal_ops rtl8192ce_hal_ops = {
 	.enable_hw_sec = rtl92ce_enable_hw_security_config,
 	.set_key = rtl92ce_set_key,
 	.init_sw_leds = rtl92ce_init_sw_leds,
+	.allow_all_destaddr = rtl92ce_allow_all_destaddr,
 	.get_bbreg = rtl92c_phy_query_bb_reg,
 	.set_bbreg = rtl92c_phy_set_bb_reg,
 	.set_rfreg = rtl92ce_phy_set_rf_reg,
@@ -278,6 +279,7 @@ static struct rtl_hal_cfg rtl92ce_hal_cfg = {
 	.maps[EFUSE_HWSET_MAX_SIZE] = HWSET_MAX_SIZE,
 	.maps[EFUSE_MAX_SECTION_MAP] = EFUSE_MAX_SECTION,
 	.maps[EFUSE_REAL_CONTENT_SIZE] = EFUSE_REAL_CONTENT_LEN,
+	.maps[EFUSE_OOB_PROTECT_BYTES_LEN] = EFUSE_OOB_PROTECT_BYTES,
 
 	.maps[RWCAM] = REG_CAMCMD,
 	.maps[WCAMI] = REG_CAMWRITE,
@@ -309,7 +311,7 @@ static struct rtl_hal_cfg rtl92ce_hal_cfg = {
 
 	.maps[RTL_IMR_TXFOVW] = IMR_TXFOVW,
 	.maps[RTL_IMR_PSTIMEOUT] = IMR_PSTIMEOUT,
-	.maps[RTL_IMR_BcnInt] = IMR_BCNINT,
+	.maps[RTL_IMR_BCNINT] = IMR_BCNINT,
 	.maps[RTL_IMR_RXFOVW] = IMR_RXFOVW,
 	.maps[RTL_IMR_RDU] = IMR_RDU,
 	.maps[RTL_IMR_ATIMEND] = IMR_ATIMEND,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
index b9b1a6e0b16e..6ad23b413eb3 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
@@ -30,6 +30,7 @@
 #include "../wifi.h"
 #include "../pci.h"
 #include "../base.h"
+#include "../stats.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
@@ -42,7 +43,7 @@ static u8 _rtl92ce_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
 
 	if (unlikely(ieee80211_is_beacon(fc)))
 		return QSLT_BEACON;
-	if (ieee80211_is_mgmt(fc))
+	if (ieee80211_is_mgmt(fc) || ieee80211_is_ctl(fc))
 		return QSLT_MGNT;
 
 	return skb->priority;
@@ -78,16 +79,6 @@ static u8 _rtl92c_evm_db_to_percentage(char value)
 	return ret_val;
 }
 
-static long _rtl92ce_translate_todbm(struct ieee80211_hw *hw,
-				     u8 signal_strength_index)
-{
-	long signal_power;
-
-	signal_power = (long)((signal_strength_index + 1) >> 1);
-	signal_power -= 95;
-	return signal_power;
-}
-
 static long _rtl92ce_signal_scale_mapping(struct ieee80211_hw *hw,
 		long currsig)
 {
@@ -139,7 +130,6 @@ static void _rtl92ce_query_rxphystatus(struct ieee80211_hw *hw,
 	pstats->packet_toself = packet_toself;
 	pstats->is_cck = is_cck_rate;
 	pstats->packet_beacon = packet_beacon;
-	pstats->is_cck = is_cck_rate;
 	pstats->rx_mimo_sig_qual[0] = -1;
 	pstats->rx_mimo_sig_qual[1] = -1;
 
@@ -192,10 +182,30 @@ static void _rtl92ce_query_rxphystatus(struct ieee80211_hw *hw,
 			}
 		}
 
-		pwdb_all = _rtl92c_query_rxpwrpercentage(rx_pwr_all);
+		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
+		/* CCK gain is smaller than OFDM/MCS gain,
+		 * so we add gain diff by experiences,
+		 * the val is 6
+		 */
+		pwdb_all += 6;
+		if (pwdb_all > 100)
+			pwdb_all = 100;
+		/* modify the offset to make the same
+		 * gain index with OFDM.
+		 */
+		if (pwdb_all > 34 && pwdb_all <= 42)
+			pwdb_all -= 2;
+		else if (pwdb_all > 26 && pwdb_all <= 34)
+			pwdb_all -= 6;
+		else if (pwdb_all > 14 && pwdb_all <= 26)
+			pwdb_all -= 8;
+		else if (pwdb_all > 4 && pwdb_all <= 14)
+			pwdb_all -= 4;
+
 		pstats->rx_pwdb_all = pwdb_all;
 		pstats->recvsignalpower = rx_pwr_all;
 
+		/* (3) Get Signal Quality (EVM) */
 		if (packet_match_bssid) {
 			u8 sq;
 			if (pstats->rx_pwdb_all > 40)
@@ -217,29 +227,38 @@ static void _rtl92ce_query_rxphystatus(struct ieee80211_hw *hw,
 	} else {
 		rtlpriv->dm.rfpath_rxenable[0] =
 		    rtlpriv->dm.rfpath_rxenable[1] = true;
+		/* (1)Get RSSI for HT rate */
 		for (i = RF90_PATH_A; i < RF90_PATH_MAX; i++) {
+			/* we will judge RF RX path now. */
 			if (rtlpriv->dm.rfpath_rxenable[i])
 				rf_rx_num++;
 
 			rx_pwr[i] =
 			    ((p_drvinfo->gain_trsw[i] & 0x3f) * 2) - 110;
+			/* Translate DBM to percentage. */
 			rssi = _rtl92c_query_rxpwrpercentage(rx_pwr[i]);
 			total_rssi += rssi;
+			/* Get Rx snr value in DB */
 			rtlpriv->stats.rx_snr_db[i] =
 			    (long)(p_drvinfo->rxsnr[i] / 2);
 
+			/* Record Signal Strength for next packet */
 			if (packet_match_bssid)
 				pstats->rx_mimo_signalstrength[i] = (u8) rssi;
 		}
 
+		/* (2)PWDB, Average PWDB cacluated by
+		 * hardware (for rate adaptive)
+		 */
 		rx_pwr_all = ((p_drvinfo->pwdb_all >> 1) & 0x7f) - 110;
 		pwdb_all = _rtl92c_query_rxpwrpercentage(rx_pwr_all);
 		pstats->rx_pwdb_all = pwdb_all;
 		pstats->rxpower = rx_pwr_all;
 		pstats->recvsignalpower = rx_pwr_all;
 
-		if (pdesc->rxht && pdesc->rxmcs >= DESC92_RATEMCS8 &&
-		    pdesc->rxmcs <= DESC92_RATEMCS15)
+		/* (3)EVM of HT rate */
+		if (pstats->is_ht && pstats->rate >= DESC92_RATEMCS8 &&
+		    pstats->rate <= DESC92_RATEMCS15)
 			max_spatial_stream = 2;
 		else
 			max_spatial_stream = 1;
@@ -248,6 +267,9 @@ static void _rtl92ce_query_rxphystatus(struct ieee80211_hw *hw,
 			evm = _rtl92c_evm_db_to_percentage(p_drvinfo->rxevm[i]);
 
 			if (packet_match_bssid) {
+				/* Fill value in RFD, Get the first
+				 * spatial stream only
+				 */
 				if (i == 0)
 					pstats->signalquality =
 					    (u8) (evm & 0xff);
@@ -256,6 +278,9 @@ static void _rtl92ce_query_rxphystatus(struct ieee80211_hw *hw,
 		}
 	}
 
+	/* UI BSS List signal strength(in percentage),
+	 * make it good looking, from 0~100.
+	 */
 	if (is_cck_rate)
 		pstats->signalstrength =
 		    (u8) (_rtl92ce_signal_scale_mapping(hw, pwdb_all));
@@ -265,215 +290,6 @@ static void _rtl92ce_query_rxphystatus(struct ieee80211_hw *hw,
 			  (hw, total_rssi /= rf_rx_num));
 }
 
-static void _rtl92ce_process_ui_rssi(struct ieee80211_hw *hw,
-		struct rtl_stats *pstats)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 rfpath;
-	u32 last_rssi, tmpval;
-
-	if (pstats->packet_toself || pstats->packet_beacon) {
-		rtlpriv->stats.rssi_calculate_cnt++;
-
-		if (rtlpriv->stats.ui_rssi.total_num++ >=
-		    PHY_RSSI_SLID_WIN_MAX) {
-
-			rtlpriv->stats.ui_rssi.total_num =
-			    PHY_RSSI_SLID_WIN_MAX;
-			last_rssi =
-			    rtlpriv->stats.ui_rssi.elements[rtlpriv->
-						    stats.ui_rssi.index];
-			rtlpriv->stats.ui_rssi.total_val -= last_rssi;
-		}
-
-		rtlpriv->stats.ui_rssi.total_val += pstats->signalstrength;
-		rtlpriv->stats.ui_rssi.elements[rtlpriv->stats.ui_rssi.
-						index++] =
-		    pstats->signalstrength;
-
-		if (rtlpriv->stats.ui_rssi.index >= PHY_RSSI_SLID_WIN_MAX)
-			rtlpriv->stats.ui_rssi.index = 0;
-
-		tmpval = rtlpriv->stats.ui_rssi.total_val /
-		    rtlpriv->stats.ui_rssi.total_num;
-		rtlpriv->stats.signal_strength =
-		    _rtl92ce_translate_todbm(hw, (u8) tmpval);
-		pstats->rssi = rtlpriv->stats.signal_strength;
-	}
-
-	if (!pstats->is_cck && pstats->packet_toself) {
-		for (rfpath = RF90_PATH_A; rfpath < rtlphy->num_total_rfpath;
-		     rfpath++) {
-			if (rtlpriv->stats.rx_rssi_percentage[rfpath] == 0) {
-				rtlpriv->stats.rx_rssi_percentage[rfpath] =
-				    pstats->rx_mimo_signalstrength[rfpath];
-
-			}
-
-			if (pstats->rx_mimo_signalstrength[rfpath] >
-			    rtlpriv->stats.rx_rssi_percentage[rfpath]) {
-				rtlpriv->stats.rx_rssi_percentage[rfpath] =
-				    ((rtlpriv->stats.
-				      rx_rssi_percentage[rfpath] *
-				      (RX_SMOOTH_FACTOR - 1)) +
-				     (pstats->rx_mimo_signalstrength[rfpath])) /
-				    (RX_SMOOTH_FACTOR);
-
-				rtlpriv->stats.rx_rssi_percentage[rfpath] =
-				    rtlpriv->stats.rx_rssi_percentage[rfpath] +
-				    1;
-			} else {
-				rtlpriv->stats.rx_rssi_percentage[rfpath] =
-				    ((rtlpriv->stats.
-				      rx_rssi_percentage[rfpath] *
-				      (RX_SMOOTH_FACTOR - 1)) +
-				     (pstats->rx_mimo_signalstrength[rfpath])) /
-				    (RX_SMOOTH_FACTOR);
-			}
-
-		}
-	}
-}
-
-static void _rtl92ce_update_rxsignalstatistics(struct ieee80211_hw *hw,
-					       struct rtl_stats *pstats)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int weighting = 0;
-
-	if (rtlpriv->stats.recv_signal_power == 0)
-		rtlpriv->stats.recv_signal_power = pstats->recvsignalpower;
-
-	if (pstats->recvsignalpower > rtlpriv->stats.recv_signal_power)
-		weighting = 5;
-
-	else if (pstats->recvsignalpower < rtlpriv->stats.recv_signal_power)
-		weighting = (-5);
-
-	rtlpriv->stats.recv_signal_power =
-	    (rtlpriv->stats.recv_signal_power * 5 +
-	     pstats->recvsignalpower + weighting) / 6;
-}
-
-static void _rtl92ce_process_pwdb(struct ieee80211_hw *hw,
-		struct rtl_stats *pstats)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	long undec_sm_pwdb;
-
-	if (mac->opmode == NL80211_IFTYPE_ADHOC) {
-		return;
-	} else {
-		undec_sm_pwdb = rtlpriv->dm.undec_sm_pwdb;
-	}
-
-	if (pstats->packet_toself || pstats->packet_beacon) {
-		if (undec_sm_pwdb < 0)
-			undec_sm_pwdb = pstats->rx_pwdb_all;
-
-		if (pstats->rx_pwdb_all > (u32) undec_sm_pwdb) {
-			undec_sm_pwdb = (((undec_sm_pwdb) *
-			      (RX_SMOOTH_FACTOR - 1)) +
-			     (pstats->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
-
-			undec_sm_pwdb += 1;
-		} else {
-			undec_sm_pwdb = (((undec_sm_pwdb) *
-			      (RX_SMOOTH_FACTOR - 1)) +
-			     (pstats->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
-		}
-
-		rtlpriv->dm.undec_sm_pwdb = undec_sm_pwdb;
-		_rtl92ce_update_rxsignalstatistics(hw, pstats);
-	}
-}
-
-static void _rtl92ce_process_ui_link_quality(struct ieee80211_hw *hw,
-					     struct rtl_stats *pstats)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 last_evm, n_spatialstream, tmpval;
-
-	if (pstats->signalquality != 0) {
-		if (pstats->packet_toself || pstats->packet_beacon) {
-
-			if (rtlpriv->stats.ui_link_quality.total_num++ >=
-			    PHY_LINKQUALITY_SLID_WIN_MAX) {
-				rtlpriv->stats.ui_link_quality.total_num =
-				    PHY_LINKQUALITY_SLID_WIN_MAX;
-				last_evm =
-				    rtlpriv->stats.
-				    ui_link_quality.elements[rtlpriv->
-							  stats.ui_link_quality.
-							  index];
-				rtlpriv->stats.ui_link_quality.total_val -=
-				    last_evm;
-			}
-
-			rtlpriv->stats.ui_link_quality.total_val +=
-			    pstats->signalquality;
-			rtlpriv->stats.ui_link_quality.elements[rtlpriv->stats.
-								ui_link_quality.
-								index++] =
-			    pstats->signalquality;
-
-			if (rtlpriv->stats.ui_link_quality.index >=
-			    PHY_LINKQUALITY_SLID_WIN_MAX)
-				rtlpriv->stats.ui_link_quality.index = 0;
-
-			tmpval = rtlpriv->stats.ui_link_quality.total_val /
-			    rtlpriv->stats.ui_link_quality.total_num;
-			rtlpriv->stats.signal_quality = tmpval;
-
-			rtlpriv->stats.last_sigstrength_inpercent = tmpval;
-
-			for (n_spatialstream = 0; n_spatialstream < 2;
-			     n_spatialstream++) {
-				if (pstats->
-				    rx_mimo_sig_qual[n_spatialstream] != -1) {
-					if (rtlpriv->stats.
-					    rx_evm_percentage[n_spatialstream]
-					    == 0) {
-						rtlpriv->stats.
-						   rx_evm_percentage
-						   [n_spatialstream] =
-						   pstats->rx_mimo_sig_qual
-						   [n_spatialstream];
-					}
-
-					rtlpriv->stats.
-					    rx_evm_percentage[n_spatialstream] =
-					    ((rtlpriv->
-					      stats.rx_evm_percentage
-					      [n_spatialstream] *
-					      (RX_SMOOTH_FACTOR - 1)) +
-					     (pstats->rx_mimo_sig_qual
-					      [n_spatialstream] * 1)) /
-					    (RX_SMOOTH_FACTOR);
-				}
-			}
-		}
-	} else {
-		;
-	}
-}
-
-static void _rtl92ce_process_phyinfo(struct ieee80211_hw *hw,
-				     u8 *buffer,
-				     struct rtl_stats *pcurrent_stats)
-{
-
-	if (!pcurrent_stats->packet_matchbssid &&
-	    !pcurrent_stats->packet_beacon)
-		return;
-
-	_rtl92ce_process_ui_rssi(hw, pcurrent_stats);
-	_rtl92ce_process_pwdb(hw, pcurrent_stats);
-	_rtl92ce_process_ui_link_quality(hw, pcurrent_stats);
-}
-
 static void _rtl92ce_translate_rx_signal_stuff(struct ieee80211_hw *hw,
 					       struct sk_buff *skb,
 					       struct rtl_stats *pstats,
@@ -516,7 +332,7 @@ static void _rtl92ce_translate_rx_signal_stuff(struct ieee80211_hw *hw,
 				   packet_matchbssid, packet_toself,
 				   packet_beacon);
 
-	_rtl92ce_process_phyinfo(hw, tmp_buf, pstats);
+	rtl_process_phyinfo(hw, tmp_buf, pstats);
 }
 
 bool rtl92ce_rx_query_desc(struct ieee80211_hw *hw,
@@ -526,7 +342,7 @@ bool rtl92ce_rx_query_desc(struct ieee80211_hw *hw,
 {
 	struct rx_fwinfo_92c *p_drvinfo;
 	struct rx_desc_92c *pdesc = (struct rx_desc_92c *)p_desc;
-
+	struct ieee80211_hdr *hdr;
 	u32 phystatus = GET_RX_DESC_PHYST(pdesc);
 	stats->length = (u16) GET_RX_DESC_PKT_LEN(pdesc);
 	stats->rx_drvinfo_size = (u8) GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
@@ -539,37 +355,60 @@ bool rtl92ce_rx_query_desc(struct ieee80211_hw *hw,
 	stats->rate = (u8) GET_RX_DESC_RXMCS(pdesc);
 	stats->shortpreamble = (u16) GET_RX_DESC_SPLCP(pdesc);
 	stats->isampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
-	stats->isampdu = (bool) ((GET_RX_DESC_PAGGR(pdesc) == 1)
+	stats->isfirst_ampdu = (bool) ((GET_RX_DESC_PAGGR(pdesc) == 1)
 				   && (GET_RX_DESC_FAGGR(pdesc) == 1));
 	stats->timestamp_low = GET_RX_DESC_TSFL(pdesc);
 	stats->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
+	stats->is_ht = (bool)GET_RX_DESC_RXHT(pdesc);
 
-	rx_status->freq = hw->conf.channel->center_freq;
-	rx_status->band = hw->conf.channel->band;
+	stats->is_cck = RX_HAL_IS_CCK_RATE(pdesc);
 
-	if (GET_RX_DESC_CRC32(pdesc))
-		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
+	rx_status->freq = hw->conf.chandef.chan->center_freq;
+	rx_status->band = hw->conf.chandef.chan->band;
 
-	if (!GET_RX_DESC_SWDEC(pdesc))
-		rx_status->flag |= RX_FLAG_DECRYPTED;
+	hdr = (struct ieee80211_hdr *)(skb->data + stats->rx_drvinfo_size
+			+ stats->rx_bufshift);
+
+	if (stats->crc)
+		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
 
-	if (GET_RX_DESC_BW(pdesc))
+	if (stats->rx_is40Mhzpacket)
 		rx_status->flag |= RX_FLAG_40MHZ;
 
-	if (GET_RX_DESC_RXHT(pdesc))
+	if (stats->is_ht)
 		rx_status->flag |= RX_FLAG_HT;
 
 	rx_status->flag |= RX_FLAG_MACTIME_START;
 
-	if (stats->decrypted)
-		rx_status->flag |= RX_FLAG_DECRYPTED;
-
+	/* hw will set stats->decrypted true, if it finds the
+	 * frame is open data frame or mgmt frame.
+	 * So hw will not decryption robust managment frame
+	 * for IEEE80211w but still set status->decrypted
+	 * true, so here we should set it back to undecrypted
+	 * for IEEE80211w frame, and mac80211 sw will help
+	 * to decrypt it
+	 */
+	if (stats->decrypted) {
+		if (!hdr) {
+			/* In testing, hdr was NULL here */
+			return false;
+		}
+		if ((ieee80211_is_robust_mgmt_frame(hdr)) &&
+		    (ieee80211_has_protected(hdr->frame_control)))
+			rx_status->flag &= ~RX_FLAG_DECRYPTED;
+		else
+			rx_status->flag |= RX_FLAG_DECRYPTED;
+	}
+	/* rate_idx: index of data rate into band's
+	 * supported rates or MCS index if HT rates
+	 * are use (RX_FLAG_HT)
+	 * Notice: this is diff with windows define
+	 */
 	rx_status->rate_idx = rtlwifi_rate_mapping(hw,
-				(bool)GET_RX_DESC_RXHT(pdesc),
-				(u8)GET_RX_DESC_RXMCS(pdesc),
-				(bool)GET_RX_DESC_PAGGR(pdesc));
+				stats->is_ht, stats->rate,
+				stats->isfirst_ampdu);
 
-	rx_status->mactime = GET_RX_DESC_TSFL(pdesc);
+	rx_status->mactime = stats->timestamp_low;
 	if (phystatus) {
 		p_drvinfo = (struct rx_fwinfo_92c *)(skb->data +
 						     stats->rx_bufshift);
@@ -580,7 +419,7 @@ bool rtl92ce_rx_query_desc(struct ieee80211_hw *hw,
 	}
 
 	/*rx_status->qual = stats->signal; */
-	rx_status->signal = stats->rssi + 10;
+	rx_status->signal = stats->recvsignalpower + 10;
 	/*rx_status->noise = -stats->noise; */
 
 	return true;
@@ -624,7 +463,8 @@ void rtl92ce_tx_fill_desc(struct ieee80211_hw *hw,
 	if (mac->opmode == NL80211_IFTYPE_STATION) {
 		bw_40 = mac->bw_40;
 	} else if (mac->opmode == NL80211_IFTYPE_AP ||
-		mac->opmode == NL80211_IFTYPE_ADHOC) {
+		   mac->opmode == NL80211_IFTYPE_ADHOC ||
+		   mac->opmode == NL80211_IFTYPE_MESH_POINT) {
 		if (sta)
 			bw_40 = sta->bandwidth >= IEEE80211_STA_RX_BW_40;
 	}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
index c08d0f4c5f3d..3d0498e69c8c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
@@ -202,7 +202,7 @@ static void _rtl92cu_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 			}
 		}
 		for (i = 0; i < 14; i++) {
-			RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 				"RF(%d)-Ch(%d) [CCK / HT40_1S / HT40_2S] = [0x%x / 0x%x / 0x%x]\n", rf_path, i,
 				rtlefuse->txpwrlevel_cck[rf_path][i],
 				rtlefuse->txpwrlevel_ht40_1s[rf_path][i],
@@ -238,11 +238,11 @@ static void _rtl92cu_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 				    ((rtlefuse->eeprom_pwrlimit_ht40[index]
 				      & 0xf0) >> 4);
 			}
-			RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 				"RF-%d pwrgroup_ht20[%d] = 0x%x\n",
 				rf_path, i,
 				rtlefuse->pwrgroup_ht20[rf_path][i]);
-			RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 				"RF-%d pwrgroup_ht40[%d] = 0x%x\n",
 				rf_path, i,
 				rtlefuse->pwrgroup_ht40[rf_path][i]);
@@ -273,26 +273,26 @@ static void _rtl92cu_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 	rtlefuse->legacy_ht_txpowerdiff =
 	    rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][7];
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-A Ht20 to HT40 Diff[%d] = 0x%x\n",
 			i, rtlefuse->txpwr_ht20diff[RF90_PATH_A][i]);
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-A Legacy to Ht40 Diff[%d] = 0x%x\n",
 			i, rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][i]);
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-B Ht20 to HT40 Diff[%d] = 0x%x\n",
 			i, rtlefuse->txpwr_ht20diff[RF90_PATH_B][i]);
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-B Legacy to HT40 Diff[%d] = 0x%x\n",
 			i, rtlefuse->txpwr_legacyhtdiff[RF90_PATH_B][i]);
 	if (!autoload_fail)
 		rtlefuse->eeprom_regulatory = (hwinfo[RF_OPTION1] & 0x7);
 	else
 		rtlefuse->eeprom_regulatory = 0;
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"eeprom_regulatory = 0x%x\n", rtlefuse->eeprom_regulatory);
 	if (!autoload_fail) {
 		rtlefuse->eeprom_tssi[RF90_PATH_A] = hwinfo[EEPROM_TSSI_A];
@@ -301,7 +301,7 @@ static void _rtl92cu_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		rtlefuse->eeprom_tssi[RF90_PATH_A] = EEPROM_DEFAULT_TSSI;
 		rtlefuse->eeprom_tssi[RF90_PATH_B] = EEPROM_DEFAULT_TSSI;
 	}
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"TSSI_A = 0x%x, TSSI_B = 0x%x\n",
 		rtlefuse->eeprom_tssi[RF90_PATH_A],
 		rtlefuse->eeprom_tssi[RF90_PATH_B]);
@@ -316,7 +316,7 @@ static void _rtl92cu_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 	if (rtlefuse->eeprom_thermalmeter == 0x1f || autoload_fail)
 		rtlefuse->apk_thermalmeterignore = true;
 	rtlefuse->thermalmeter[0] = rtlefuse->eeprom_thermalmeter;
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter);
 }
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
index a73a17bc56dd..23d640a4debd 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
@@ -223,7 +223,7 @@ static struct rtl_hal_cfg rtl92cu_hal_cfg = {
 
 	.maps[RTL_IMR_TXFOVW] = IMR_TXFOVW,
 	.maps[RTL_IMR_PSTIMEOUT] = IMR_PSTIMEOUT,
-	.maps[RTL_IMR_BcnInt] = IMR_BCNINT,
+	.maps[RTL_IMR_BCNINT] = IMR_BCNINT,
 	.maps[RTL_IMR_RXFOVW] = IMR_RXFOVW,
 	.maps[RTL_IMR_RDU] = IMR_RDU,
 	.maps[RTL_IMR_ATIMEND] = IMR_ATIMEND,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
index b6222eedb835..763cf1defab5 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
@@ -324,8 +324,8 @@ bool rtl92cu_rx_query_desc(struct ieee80211_hw *hw,
 				   && (GET_RX_DESC_FAGGR(pdesc) == 1));
 	stats->timestamp_low = GET_RX_DESC_TSFL(pdesc);
 	stats->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
-	rx_status->freq = hw->conf.channel->center_freq;
-	rx_status->band = hw->conf.channel->band;
+	rx_status->freq = hw->conf.chandef.chan->center_freq;
+	rx_status->band = hw->conf.chandef.chan->band;
 	if (GET_RX_DESC_CRC32(pdesc))
 		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
 	if (!GET_RX_DESC_SWDEC(pdesc))
@@ -395,8 +395,8 @@ static void _rtl_rx_process(struct ieee80211_hw *hw, struct sk_buff *skb)
 	stats.rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(rxdesc);
 	/* TODO: is center_freq changed when doing scan? */
 	/* TODO: Shall we add protection or just skip those two step? */
-	rx_status->freq = hw->conf.channel->center_freq;
-	rx_status->band = hw->conf.channel->band;
+	rx_status->freq = hw->conf.chandef.chan->center_freq;
+	rx_status->band = hw->conf.chandef.chan->band;
 	if (GET_RX_DESC_CRC32(rxdesc))
 		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
 	if (!GET_RX_DESC_SWDEC(rxdesc))
@@ -434,7 +434,7 @@ static void _rtl_rx_process(struct ieee80211_hw *hw, struct sk_buff *skb)
 		 (u32)hdr->addr1[2], (u32)hdr->addr1[3],
 		 (u32)hdr->addr1[4], (u32)hdr->addr1[5]);
 	memcpy(IEEE80211_SKB_RXCB(skb), rx_status, sizeof(*rx_status));
-	ieee80211_rx_irqsafe(hw, skb);
+	ieee80211_rx(hw, skb);
 }
 
 void  rtl8192cu_rx_hdl(struct ieee80211_hw *hw, struct sk_buff * skb)
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/dm.c b/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
index 5251fb8a111e..19a765532603 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
@@ -171,8 +171,8 @@ static void rtl92d_dm_diginit(struct ieee80211_hw *hw)
 	de_digtable->rssi_highthresh = DM_DIG_THRESH_HIGH;
 	de_digtable->fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
 	de_digtable->fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
-	de_digtable->rx_gain_range_max = DM_DIG_FA_UPPER;
-	de_digtable->rx_gain_range_min = DM_DIG_FA_LOWER;
+	de_digtable->rx_gain_max = DM_DIG_FA_UPPER;
+	de_digtable->rx_gain_min = DM_DIG_FA_LOWER;
 	de_digtable->back_val = DM_DIG_BACKOFF_DEFAULT;
 	de_digtable->back_range_max = DM_DIG_BACKOFF_MAX;
 	de_digtable->back_range_min = DM_DIG_BACKOFF_MIN;
@@ -444,8 +444,8 @@ static void rtl92d_dm_dig(struct ieee80211_hw *hw)
 		 "dm_DIG() Before: large_fa_hit=%d, forbidden_igi=%x\n",
 		 de_digtable->large_fa_hit, de_digtable->forbidden_igi);
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
-		 "dm_DIG() Before: Recover_cnt=%d, rx_gain_range_min=%x\n",
-		 de_digtable->recover_cnt, de_digtable->rx_gain_range_min);
+		 "dm_DIG() Before: Recover_cnt=%d, rx_gain_min=%x\n",
+		 de_digtable->recover_cnt, de_digtable->rx_gain_min);
 
 	/* deal with abnorally large false alarm */
 	if (falsealm_cnt->cnt_all > 10000) {
@@ -459,9 +459,9 @@ static void rtl92d_dm_dig(struct ieee80211_hw *hw)
 		}
 		if (de_digtable->large_fa_hit >= 3) {
 			if ((de_digtable->forbidden_igi + 1) > DM_DIG_MAX)
-				de_digtable->rx_gain_range_min = DM_DIG_MAX;
+				de_digtable->rx_gain_min = DM_DIG_MAX;
 			else
-				de_digtable->rx_gain_range_min =
+				de_digtable->rx_gain_min =
 				    (de_digtable->forbidden_igi + 1);
 			de_digtable->recover_cnt = 3600;	/* 3600=2hr */
 		}
@@ -475,12 +475,12 @@ static void rtl92d_dm_dig(struct ieee80211_hw *hw)
 				    DM_DIG_FA_LOWER) {
 					de_digtable->forbidden_igi =
 							 DM_DIG_FA_LOWER;
-					de_digtable->rx_gain_range_min =
+					de_digtable->rx_gain_min =
 							 DM_DIG_FA_LOWER;
 
 				} else {
 					de_digtable->forbidden_igi--;
-					de_digtable->rx_gain_range_min =
+					de_digtable->rx_gain_min =
 					    (de_digtable->forbidden_igi + 1);
 				}
 			} else if (de_digtable->large_fa_hit == 3) {
@@ -492,13 +492,13 @@ static void rtl92d_dm_dig(struct ieee80211_hw *hw)
 		 "dm_DIG() After: large_fa_hit=%d, forbidden_igi=%x\n",
 		 de_digtable->large_fa_hit, de_digtable->forbidden_igi);
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
-		 "dm_DIG() After: recover_cnt=%d, rx_gain_range_min=%x\n",
-		 de_digtable->recover_cnt, de_digtable->rx_gain_range_min);
+		 "dm_DIG() After: recover_cnt=%d, rx_gain_min=%x\n",
+		 de_digtable->recover_cnt, de_digtable->rx_gain_min);
 
 	if (value_igi > DM_DIG_MAX)
 		value_igi = DM_DIG_MAX;
-	else if (value_igi < de_digtable->rx_gain_range_min)
-		value_igi = de_digtable->rx_gain_range_min;
+	else if (value_igi < de_digtable->rx_gain_min)
+		value_igi = de_digtable->rx_gain_min;
 	de_digtable->cur_igvalue = value_igi;
 	rtl92d_dm_write_dig(hw);
 	if (rtlpriv->rtlhal.current_bandtype != BAND_ON_5G)
@@ -1071,9 +1071,9 @@ static void rtl92d_dm_txpower_tracking_callback_thermalmeter(
 			}
 			ele_d = (ofdmswing_table[(u8) ofdm_index[0]] &
 						 0xFFC00000) >> 22;
-			val_x = rtlphy->iqk_matrix_regsetting
+			val_x = rtlphy->iqk_matrix
 						[indexforchannel].value[0][0];
-			val_y = rtlphy->iqk_matrix_regsetting
+			val_y = rtlphy->iqk_matrix
 						[indexforchannel].value[0][1];
 			if (val_x != 0) {
 				if ((val_x & 0x00000200) != 0)
@@ -1175,9 +1175,9 @@ static void rtl92d_dm_txpower_tracking_callback_thermalmeter(
 			if (is2t) {
 				ele_d = (ofdmswing_table[(u8) ofdm_index[1]] &
 						0xFFC00000) >> 22;
-				val_x = rtlphy->iqk_matrix_regsetting
+				val_x = rtlphy->iqk_matrix
 						[indexforchannel].value[0][4];
-				val_y = rtlphy->iqk_matrix_regsetting
+				val_y = rtlphy->iqk_matrix
 						[indexforchannel].value[0][5];
 				if (val_x != 0) {
 					if ((val_x & 0x00000200) != 0)
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/hw.c b/drivers/net/wireless/rtlwifi/rtl8192de/hw.c
index aa5b42521bb4..7dd8f6de0550 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/hw.c
@@ -1183,7 +1183,7 @@ void rtl92d_linked_set_reg(struct ieee80211_hw *hw)
 	u8 channel = rtlphy->current_channel;
 
 	indexforchannel = rtl92d_get_rightchnlplace_for_iqk(channel);
-	if (!rtlphy->iqk_matrix_regsetting[indexforchannel].iqk_done) {
+	if (!rtlphy->iqk_matrix[indexforchannel].iqk_done) {
 		RT_TRACE(rtlpriv, COMP_SCAN | COMP_INIT, DBG_DMESG,
 			 "Do IQK for channel:%d\n", channel);
 		rtl92d_phy_iq_calibrate(hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/phy.c b/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
index 33041bd4da81..840bac5fa2f8 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
@@ -2479,9 +2479,9 @@ void rtl92d_phy_iq_calibrate(struct ieee80211_hw *hw)
 				  rtlphy->current_channel);
 
 		for (i = 0; i < IQK_MATRIX_REG_NUM; i++)
-			rtlphy->iqk_matrix_regsetting[indexforchannel].
+			rtlphy->iqk_matrix[indexforchannel].
 				value[0][i] = result[final_candidate][i];
-		rtlphy->iqk_matrix_regsetting[indexforchannel].iqk_done =
+		rtlphy->iqk_matrix[indexforchannel].iqk_done =
 			true;
 
 		RT_TRACE(rtlpriv, COMP_SCAN | COMP_MLME, DBG_LOUD,
@@ -2501,8 +2501,8 @@ void rtl92d_phy_reload_iqk_setting(struct ieee80211_hw *hw, u8 channel)
 	indexforchannel = rtl92d_get_rightchnlplace_for_iqk(channel);
 	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "indexforchannel %d done %d\n",
 		 indexforchannel,
-		 rtlphy->iqk_matrix_regsetting[indexforchannel].iqk_done);
-	if (0 && !rtlphy->iqk_matrix_regsetting[indexforchannel].iqk_done &&
+		 rtlphy->iqk_matrix[indexforchannel].iqk_done);
+	if (0 && !rtlphy->iqk_matrix[indexforchannel].iqk_done &&
 		rtlphy->need_iqk) {
 		/* Re Do IQK. */
 		RT_TRACE(rtlpriv, COMP_SCAN | COMP_INIT, DBG_LOUD,
@@ -2516,23 +2516,23 @@ void rtl92d_phy_reload_iqk_setting(struct ieee80211_hw *hw, u8 channel)
 			RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD,
 				 "Just Read IQK Matrix reg for channel:%d....\n",
 				 channel);
-			if ((rtlphy->iqk_matrix_regsetting[indexforchannel].
+			if ((rtlphy->iqk_matrix[indexforchannel].
 			     value[0] != NULL)
 				/*&&(regea4 != 0) */)
 				_rtl92d_phy_patha_fill_iqk_matrix(hw, true,
-					rtlphy->iqk_matrix_regsetting[
+					rtlphy->iqk_matrix[
 					indexforchannel].value,	0,
-					(rtlphy->iqk_matrix_regsetting[
+					(rtlphy->iqk_matrix[
 					indexforchannel].value[0][2] == 0));
 			if (IS_92D_SINGLEPHY(rtlhal->version)) {
-				if ((rtlphy->iqk_matrix_regsetting[
+				if ((rtlphy->iqk_matrix[
 					indexforchannel].value[0][4] != 0)
 					/*&&(regec4 != 0) */)
 					_rtl92d_phy_pathb_fill_iqk_matrix(hw,
 						true,
-						rtlphy->iqk_matrix_regsetting[
+						rtlphy->iqk_matrix[
 						indexforchannel].value, 0,
-						(rtlphy->iqk_matrix_regsetting[
+						(rtlphy->iqk_matrix[
 						indexforchannel].value[0][6]
 						== 0));
 			}
@@ -2830,20 +2830,20 @@ void rtl92d_phy_reset_iqk_result(struct ieee80211_hw *hw)
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 		 "settings regs %d default regs %d\n",
-		 (int)(sizeof(rtlphy->iqk_matrix_regsetting) /
+		 (int)(sizeof(rtlphy->iqk_matrix) /
 		       sizeof(struct iqk_matrix_regs)),
 		 IQK_MATRIX_REG_NUM);
 	/* 0xe94, 0xe9c, 0xea4, 0xeac, 0xeb4, 0xebc, 0xec4, 0xecc */
 	for (i = 0; i < IQK_MATRIX_SETTINGS_NUM; i++) {
-		rtlphy->iqk_matrix_regsetting[i].value[0][0] = 0x100;
-		rtlphy->iqk_matrix_regsetting[i].value[0][2] = 0x100;
-		rtlphy->iqk_matrix_regsetting[i].value[0][4] = 0x100;
-		rtlphy->iqk_matrix_regsetting[i].value[0][6] = 0x100;
-		rtlphy->iqk_matrix_regsetting[i].value[0][1] = 0x0;
-		rtlphy->iqk_matrix_regsetting[i].value[0][3] = 0x0;
-		rtlphy->iqk_matrix_regsetting[i].value[0][5] = 0x0;
-		rtlphy->iqk_matrix_regsetting[i].value[0][7] = 0x0;
-		rtlphy->iqk_matrix_regsetting[i].iqk_done = false;
+		rtlphy->iqk_matrix[i].value[0][0] = 0x100;
+		rtlphy->iqk_matrix[i].value[0][2] = 0x100;
+		rtlphy->iqk_matrix[i].value[0][4] = 0x100;
+		rtlphy->iqk_matrix[i].value[0][6] = 0x100;
+		rtlphy->iqk_matrix[i].value[0][1] = 0x0;
+		rtlphy->iqk_matrix[i].value[0][3] = 0x0;
+		rtlphy->iqk_matrix[i].value[0][5] = 0x0;
+		rtlphy->iqk_matrix[i].value[0][7] = 0x0;
+		rtlphy->iqk_matrix[i].iqk_done = false;
 	}
 }
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/reg.h b/drivers/net/wireless/rtlwifi/rtl8192de/reg.h
index ebb1d5f5e7b5..b7498c5bafc5 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/reg.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/reg.h
@@ -543,7 +543,7 @@
 #define	IMR_TIMEOUT1			BIT(16)
 #define	IMR_TXFOVW			BIT(15)
 #define	IMR_PSTIMEOUT			BIT(14)
-#define	IMR_BcnInt			BIT(13)
+#define	IMR_BCNINT			BIT(13)
 #define	IMR_RXFOVW			BIT(12)
 #define	IMR_RDU				BIT(11)
 #define	IMR_ATIMEND			BIT(10)
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/sw.c b/drivers/net/wireless/rtlwifi/rtl8192de/sw.c
index 03c6d18b2e07..c18c04bf0c13 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/sw.c
@@ -166,7 +166,7 @@ static int rtl92d_init_sw_vars(struct ieee80211_hw *hw)
 		rtlpriv->psc.fwctrl_psmode = FW_PS_DTIM_MODE;
 
 	/* for early mode */
-	rtlpriv->rtlhal.earlymode_enable = true;
+	rtlpriv->rtlhal.earlymode_enable = false;
 	for (tid = 0; tid < 8; tid++)
 		skb_queue_head_init(&rtlpriv->mac80211.skb_waitq[tid]);
 
@@ -319,7 +319,7 @@ static struct rtl_hal_cfg rtl92de_hal_cfg = {
 
 	.maps[RTL_IMR_TXFOVW] = IMR_TXFOVW,
 	.maps[RTL_IMR_PSTIMEOUT] = IMR_PSTIMEOUT,
-	.maps[RTL_IMR_BcnInt] = IMR_BcnInt,
+	.maps[RTL_IMR_BCNINT] = IMR_BCNINT,
 	.maps[RTL_IMR_RXFOVW] = IMR_RXFOVW,
 	.maps[RTL_IMR_RDU] = IMR_RDU,
 	.maps[RTL_IMR_ATIMEND] = IMR_ATIMEND,
@@ -333,7 +333,7 @@ static struct rtl_hal_cfg rtl92de_hal_cfg = {
 	.maps[RTL_IMR_VIDOK] = IMR_VIDOK,
 	.maps[RTL_IMR_VODOK] = IMR_VODOK,
 	.maps[RTL_IMR_ROK] = IMR_ROK,
-	.maps[RTL_IBSS_INT_MASKS] = (IMR_BcnInt | IMR_TBDOK | IMR_TBDER),
+	.maps[RTL_IBSS_INT_MASKS] = (IMR_BCNINT | IMR_TBDOK | IMR_TBDER),
 
 	.maps[RTL_RC_CCK_RATE1M] = DESC92_RATE1M,
 	.maps[RTL_RC_CCK_RATE2M] = DESC92_RATE2M,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/trx.c b/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
index 941080e03c06..b8ec718a0fab 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
@@ -499,8 +499,8 @@ bool rtl92de_rx_query_desc(struct ieee80211_hw *hw,	struct rtl_stats *stats,
 					 && (GET_RX_DESC_FAGGR(pdesc) == 1));
 	stats->timestamp_low = GET_RX_DESC_TSFL(pdesc);
 	stats->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
-	rx_status->freq = hw->conf.channel->center_freq;
-	rx_status->band = hw->conf.channel->band;
+	rx_status->freq = hw->conf.chandef.chan->center_freq;
+	rx_status->band = hw->conf.chandef.chan->band;
 	if (GET_RX_DESC_CRC32(pdesc))
 		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
 	if (!GET_RX_DESC_SWDEC(pdesc))
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/def.h b/drivers/net/wireless/rtlwifi/rtl8192se/def.h
index 2d255e02d795..83c98674bfd3 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/def.h
@@ -36,9 +36,6 @@
 #define SHORT_SLOT_TIME				9
 #define NON_SHORT_SLOT_TIME			20
 
-/* Rx smooth factor */
-#define	RX_SMOOTH_FACTOR			20
-
 /* Queue Select Value in TxDesc */
 #define QSLT_BK					0x2
 #define QSLT_BE					0x0
@@ -49,10 +46,6 @@
 #define QSLT_MGNT				0x12
 #define QSLT_CMD				0x13
 
-#define	PHY_RSSI_SLID_WIN_MAX			100
-#define	PHY_LINKQUALITY_SLID_WIN_MAX		20
-#define	PHY_BEACON_RSSI_SLID_WIN_MAX		10
-
 /* Tx Desc */
 #define TX_DESC_SIZE_RTL8192S			(16 * 4)
 #define TX_CMDDESC_SIZE_RTL8192S		(16 * 4)
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/dm.c b/drivers/net/wireless/rtlwifi/rtl8192se/dm.c
index e551fe5f9ccd..b3a2d5ec59e6 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/dm.c
@@ -163,6 +163,7 @@ static void _rtl92s_dm_txpowertracking_callback_thermalmeter(
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	u8 thermalvalue = 0;
+	u32 fw_cmd = 0;
 
 	rtlpriv->dm.txpower_trackinginit = true;
 
@@ -175,7 +176,19 @@ static void _rtl92s_dm_txpowertracking_callback_thermalmeter(
 
 	if (thermalvalue) {
 		rtlpriv->dm.thermalvalue = thermalvalue;
-		rtl92s_phy_set_fw_cmd(hw, FW_CMD_TXPWR_TRACK_THERMAL);
+		if (hal_get_firmwareversion(rtlpriv) >= 0x35) {
+			rtl92s_phy_set_fw_cmd(hw, FW_CMD_TXPWR_TRACK_THERMAL);
+		} else {
+			fw_cmd = (FW_TXPWR_TRACK_THERMAL |
+				 (rtlpriv->efuse.thermalmeter[0] << 8) |
+				 (thermalvalue << 16));
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "Write to FW Thermal Val = 0x%x\n", fw_cmd);
+
+			rtl_write_dword(rtlpriv, WFM5, fw_cmd);
+			rtl92s_phy_chk_fwcmd_iodone(hw);
+		}
 	}
 
 	rtlpriv->dm.txpowercount = 0;
@@ -217,11 +230,10 @@ static void _rtl92s_dm_refresh_rateadaptive_mask(struct ieee80211_hw *hw)
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rate_adaptive *ra = &(rtlpriv->ra);
-
+	struct ieee80211_sta *sta = NULL;
 	u32 low_rssi_thresh = 0;
 	u32 middle_rssi_thresh = 0;
 	u32 high_rssi_thresh = 0;
-	struct ieee80211_sta *sta = NULL;
 
 	if (is_hal_stop(rtlhal))
 		return;
@@ -229,14 +241,12 @@ static void _rtl92s_dm_refresh_rateadaptive_mask(struct ieee80211_hw *hw)
 	if (!rtlpriv->dm.useramask)
 		return;
 
-	if (!rtlpriv->dm.inform_fw_driverctrldm) {
+	if (hal_get_firmwareversion(rtlpriv) >= 61 &&
+	    !rtlpriv->dm.inform_fw_driverctrldm) {
 		rtl92s_phy_set_fw_cmd(hw, FW_CMD_CTRL_DM_BY_DRIVER);
 		rtlpriv->dm.inform_fw_driverctrldm = true;
 	}
 
-	rcu_read_lock();
-	if (mac->opmode == NL80211_IFTYPE_STATION)
-		sta = get_sta(hw, mac->vif, mac->bssid);
 	if ((mac->link_state == MAC80211_LINKED) &&
 	    (mac->opmode == NL80211_IFTYPE_STATION)) {
 		switch (ra->pre_ratr_state) {
@@ -285,12 +295,16 @@ static void _rtl92s_dm_refresh_rateadaptive_mask(struct ieee80211_hw *hw)
 				 rtlpriv->dm.undec_sm_pwdb, ra->ratr_state,
 				 ra->pre_ratr_state, ra->ratr_state);
 
-			rtlpriv->cfg->ops->update_rate_tbl(hw, sta,
+			rcu_read_lock();
+			sta = rtl_find_sta(hw, mac->bssid);
+			if (sta)
+				rtlpriv->cfg->ops->update_rate_tbl(hw, sta,
 							   ra->ratr_state);
+			rcu_read_unlock();
+
 			ra->pre_ratr_state = ra->ratr_state;
 		}
 	}
-	rcu_read_unlock();
 }
 
 static void _rtl92s_dm_switch_baseband_mrc(struct ieee80211_hw *hw)
@@ -370,7 +384,8 @@ static void _rtl92s_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
 	ra->ratr_state = DM_RATR_STA_MAX;
 	ra->pre_ratr_state = DM_RATR_STA_MAX;
 
-	if (rtlpriv->dm.dm_type == DM_TYPE_BYDRIVER)
+	if (rtlpriv->dm.dm_type == DM_TYPE_BYDRIVER &&
+	    hal_get_firmwareversion(rtlpriv) >= 60)
 		rtlpriv->dm.useramask = true;
 	else
 		rtlpriv->dm.useramask = false;
@@ -457,13 +472,13 @@ static void _rtl92s_dm_initial_gain_sta_beforeconnect(struct ieee80211_hw *hw)
 				digtable->back_val = DM_DIG_BACKOFF;
 
 			if ((digtable->rssi_val + 10 - digtable->back_val) >
-				digtable->rx_gain_range_max)
+				digtable->rx_gain_max)
 				digtable->cur_igvalue =
-						digtable->rx_gain_range_max;
+						digtable->rx_gain_max;
 			else if ((digtable->rssi_val + 10 - digtable->back_val)
-				 < digtable->rx_gain_range_min)
+				 < digtable->rx_gain_min)
 				digtable->cur_igvalue =
-						digtable->rx_gain_range_min;
+						digtable->rx_gain_min;
 			else
 				digtable->cur_igvalue = digtable->rssi_val + 10
 					- digtable->back_val;
@@ -475,7 +490,7 @@ static void _rtl92s_dm_initial_gain_sta_beforeconnect(struct ieee80211_hw *hw)
 
 			if (falsealm_cnt->cnt_all > 16000)
 				digtable->cur_igvalue =
-						 digtable->rx_gain_range_max;
+						 digtable->rx_gain_max;
 		/* connected -> connected or disconnected -> disconnected  */
 		} else {
 			/* Firmware control DIG, do nothing in driver dm */
@@ -677,9 +692,9 @@ static void _rtl92s_dm_init_dig(struct ieee80211_hw *hw)
 	/* for dig debug rssi value */
 	digtable->rssi_val = 50;
 	digtable->back_val = DM_DIG_BACKOFF;
-	digtable->rx_gain_range_max = DM_DIG_MAX;
+	digtable->rx_gain_max = DM_DIG_MAX;
 
-	digtable->rx_gain_range_min = DM_DIG_MIN;
+	digtable->rx_gain_min = DM_DIG_MIN;
 
 	digtable->backoffval_range_max = DM_DIG_BACKOFF_MAX;
 	digtable->backoffval_range_min = DM_DIG_BACKOFF_MIN;
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/hw.c b/drivers/net/wireless/rtlwifi/rtl8192se/hw.c
index 084e7773bce2..4f461786a7eb 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/hw.c
@@ -400,6 +400,39 @@ void rtl92se_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 
 			break;
 		}
+	case HW_VAR_FW_LPS_ACTION: {
+		bool enter_fwlps = *((bool *)val);
+		u8 rpwm_val, fw_pwrmode;
+		bool fw_current_inps;
+
+		if (enter_fwlps) {
+			rpwm_val = 0x02;	/* RF off */
+			fw_current_inps = true;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+					HW_VAR_FW_PSMODE_STATUS,
+					(u8 *)(&fw_current_inps));
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+					HW_VAR_H2C_FW_PWRMODE,
+					(u8 *)(&ppsc->fwctrl_psmode));
+
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+					HW_VAR_SET_RPWM,
+					(u8 *)(&rpwm_val));
+		} else {
+			rpwm_val = 0x0C;	/* RF on */
+			fw_pwrmode = FW_PS_ACTIVE_MODE;
+			fw_current_inps = false;
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+					(u8 *)(&rpwm_val));
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+					HW_VAR_H2C_FW_PWRMODE,
+					(u8 *)(&fw_pwrmode));
+
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+					HW_VAR_FW_PSMODE_STATUS,
+					(u8 *)(&fw_current_inps));
+		}
+		break; }
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "switch case not processed\n");
@@ -438,7 +471,7 @@ void rtl92se_enable_hw_security_config(struct ieee80211_hw *hw)
 
 }
 
-static u8 _rtl92ce_halset_sysclk(struct ieee80211_hw *hw, u8 data)
+static u8 _rtl92se_halset_sysclk(struct ieee80211_hw *hw, u8 data)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 waitcount = 100;
@@ -547,7 +580,7 @@ static void _rtl92se_macconfig_before_fwdownload(struct ieee80211_hw *hw)
 		tmpu1b &= ~(BIT(6) | BIT(7));
 
 		/* Set failed, return to prevent hang. */
-		if (!_rtl92ce_halset_sysclk(hw, tmpu1b))
+		if (!_rtl92se_halset_sysclk(hw, tmpu1b))
 			return;
 	}
 
@@ -650,7 +683,7 @@ static void _rtl92se_macconfig_before_fwdownload(struct ieee80211_hw *hw)
 
 	tmpu1b = rtl_read_byte(rtlpriv, (SYS_CLKR + 1));
 	tmpu1b = ((tmpu1b | BIT(7)) & (~BIT(6)));
-	if (!_rtl92ce_halset_sysclk(hw, tmpu1b))
+	if (!_rtl92se_halset_sysclk(hw, tmpu1b))
 		return; /* Set failed, return to prevent hang. */
 
 	rtl_write_word(rtlpriv, CMDR, 0x07FC);
@@ -967,6 +1000,15 @@ int rtl92se_hw_init(struct ieee80211_hw *hw)
 		return rtstatus;
 	}
 
+	/* because last function modify RCR, so we update
+	 * rcr var here, or TP will unstable for receive_config
+	 * is wrong, RX RCR_ACRC32 will cause TP unstabel & Rx
+	 * RCR_APP_ICV will cause mac80211 unassoc for cisco 1252
+	 */
+	rtlpci->receive_config = rtl_read_dword(rtlpriv, RCR);
+	rtlpci->receive_config &= ~(RCR_ACRC32 | RCR_AICV);
+	rtl_write_dword(rtlpriv, RCR, rtlpci->receive_config);
+
 	/* Make sure BB/RF write OK. We should prevent enter IPS. radio off. */
 	/* We must set flag avoid BB/RF config period later!! */
 	rtl_write_dword(rtlpriv, CMDR, 0x37FC);
@@ -982,25 +1024,6 @@ int rtl92se_hw_init(struct ieee80211_hw *hw)
 
 	rtlphy->rf_mode = RF_OP_BY_SW_3WIRE;
 
-	/* RF Power Save */
-#if 0
-	/* H/W or S/W RF OFF before sleep. */
-	if (rtlpriv->psc.rfoff_reason > RF_CHANGE_BY_PS) {
-		u32 rfoffreason = rtlpriv->psc.rfoff_reason;
-
-		rtlpriv->psc.rfoff_reason = RF_CHANGE_BY_INIT;
-		rtlpriv->psc.rfpwr_state = ERFON;
-		/* FIXME: check spinlocks if this block is uncommented */
-		rtl_ps_set_rf_state(hw, ERFOFF, rfoffreason);
-	} else {
-		/* gpio radio on/off is out of adapter start */
-		if (rtlpriv->psc.hwradiooff == false) {
-			rtlpriv->psc.rfpwr_state = ERFON;
-			rtlpriv->psc.rfoff_reason = 0;
-		}
-	}
-#endif
-
 	/* Before RF-R/W we must execute the IO from Scott's suggestion. */
 	rtl_write_byte(rtlpriv, AFE_XTAL_CTRL + 1, 0xDB);
 	if (rtlhal->version == VERSION_8192S_ACUT)
@@ -1058,7 +1081,22 @@ int rtl92se_hw_init(struct ieee80211_hw *hw)
 
 	/* We enable high power and RA related mechanism after NIC
 	 * initialized. */
-	rtl92s_phy_set_fw_cmd(hw, FW_CMD_RA_INIT);
+	if (hal_get_firmwareversion(rtlpriv) >= 0x35) {
+		/* Fw v.53 and later. */
+		rtl92s_phy_set_fw_cmd(hw, FW_CMD_RA_INIT);
+	} else if (hal_get_firmwareversion(rtlpriv) == 0x34) {
+		/* Fw v.52. */
+		rtl_write_dword(rtlpriv, WFM5, FW_RA_INIT);
+		rtl92s_phy_chk_fwcmd_iodone(hw);
+	} else {
+		/* Compatible earlier FW version. */
+		rtl_write_dword(rtlpriv, WFM5, FW_RA_RESET);
+		rtl92s_phy_chk_fwcmd_iodone(hw);
+		rtl_write_dword(rtlpriv, WFM5, FW_RA_ACTIVE);
+		rtl92s_phy_chk_fwcmd_iodone(hw);
+		rtl_write_dword(rtlpriv, WFM5, FW_RA_REFRESH);
+		rtl92s_phy_chk_fwcmd_iodone(hw);
+	}
 
 	/* Add to prevent ASPM bug. */
 	/* Always enable hst and NIC clock request. */
@@ -1229,7 +1267,6 @@ void rtl92se_disable_interrupt(struct ieee80211_hw *hw)
 	synchronize_irq(rtlpci->pdev->irq);
 }
 
-
 static u8 _rtl92s_set_sysclk(struct ieee80211_hw *hw, u8 data)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -1754,7 +1791,7 @@ static void _rtl92se_read_adapter_info(struct ieee80211_hw *hw)
 		}
 
 		for (i = 0; i < 14; i++) {
-			RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 				"RF(%d)-Ch(%d) [CCK / HT40_1S / HT40_2S] = [0x%x / 0x%x / 0x%x]\n",
 				rf_path, i,
 				rtlefuse->txpwrlevel_cck[rf_path][i],
@@ -1791,11 +1828,11 @@ static void _rtl92se_read_adapter_info(struct ieee80211_hw *hw)
 				((rtlefuse->eeprom_pwrgroup[rf_path][index] &
 				0xf0) >> 4);
 
-			RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 				"RF-%d pwrgroup_ht20[%d] = 0x%x\n",
 				rf_path, i,
 				rtlefuse->pwrgroup_ht20[rf_path][i]);
-			RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 				"RF-%d pwrgroup_ht40[%d] = 0x%x\n",
 				rf_path, i,
 				rtlefuse->pwrgroup_ht40[rf_path][i]);
@@ -1850,27 +1887,27 @@ static void _rtl92se_read_adapter_info(struct ieee80211_hw *hw)
 			rtlefuse->eeprom_regulatory =
 				 (hwinfo[EEPROM_REGULATORY] & 0x1);
 	}
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"eeprom_regulatory = 0x%x\n", rtlefuse->eeprom_regulatory);
 
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-A Ht20 to HT40 Diff[%d] = 0x%x\n",
 			i, rtlefuse->txpwr_ht20diff[RF90_PATH_A][i]);
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-A Legacy to Ht40 Diff[%d] = 0x%x\n",
 			i, rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][i]);
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-B Ht20 to HT40 Diff[%d] = 0x%x\n",
 			i, rtlefuse->txpwr_ht20diff[RF90_PATH_B][i]);
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-B Legacy to HT40 Diff[%d] = 0x%x\n",
 			i, rtlefuse->txpwr_legacyhtdiff[RF90_PATH_B][i]);
 
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"TxPwrSafetyFlag = %d\n", rtlefuse->txpwr_safetyflag);
 
 	/* Read RF-indication and Tx Power gain
@@ -1880,7 +1917,7 @@ static void _rtl92se_read_adapter_info(struct ieee80211_hw *hw)
 	rtlefuse->legacy_httxpowerdiff =
 		rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][0];
 
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"TxPowerDiff = %#x\n", rtlefuse->eeprom_txpowerdiff);
 
 	/* Get TSSI value for each path. */
@@ -1889,7 +1926,7 @@ static void _rtl92se_read_adapter_info(struct ieee80211_hw *hw)
 	usvalue = hwinfo[EEPROM_TSSI_B];
 	rtlefuse->eeprom_tssi[RF90_PATH_B] = (u8)(usvalue & 0xff);
 
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower, "TSSI_A = 0x%x, TSSI_B = 0x%x\n",
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER, "TSSI_A = 0x%x, TSSI_B = 0x%x\n",
 		rtlefuse->eeprom_tssi[RF90_PATH_A],
 		rtlefuse->eeprom_tssi[RF90_PATH_B]);
 
@@ -1897,7 +1934,7 @@ static void _rtl92se_read_adapter_info(struct ieee80211_hw *hw)
 	/* and read ThermalMeter from EEPROM */
 	tempval = hwinfo[EEPROM_THERMALMETER];
 	rtlefuse->eeprom_thermalmeter = tempval;
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter);
 
 	/* ThermalMeter, BIT(0)~3 for RFIC1, BIT(4)~7 for RFIC2 */
@@ -1914,7 +1951,7 @@ static void _rtl92se_read_adapter_info(struct ieee80211_hw *hw)
 	/* Version ID, Channel plan */
 	rtlefuse->eeprom_channelplan = hwinfo[EEPROM_CHANNELPLAN];
 	rtlefuse->txpwr_fromeprom = true;
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"EEPROM ChannelPlan = 0x%4x\n", rtlefuse->eeprom_channelplan);
 
 	/* Read Customer ID or Board Type!!! */
@@ -1999,6 +2036,8 @@ static void rtl92se_update_hal_rate_table(struct ieee80211_hw *hw,
 		ratr_value = sta->supp_rates[1] << 4;
 	else
 		ratr_value = sta->supp_rates[0];
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		ratr_value = 0xfff;
 	ratr_value |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
 			sta->ht_cap.mcs.rx_mask[0] << 12);
 	switch (wirelessmode) {
@@ -2112,6 +2151,8 @@ static void rtl92se_update_hal_rate_mask(struct ieee80211_hw *hw,
 		ratr_bitmap = sta->supp_rates[1] << 4;
 	else
 		ratr_bitmap = sta->supp_rates[0];
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		ratr_bitmap = 0xfff;
 	ratr_bitmap |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
 			sta->ht_cap.mcs.rx_mask[0] << 12);
 	switch (wirelessmode) {
@@ -2200,6 +2241,7 @@ static void rtl92se_update_hal_rate_mask(struct ieee80211_hw *hw,
 			ratr_bitmap &= 0x0f8ff0ff;
 		break;
 	}
+	sta_entry->ratr_index = ratr_index;
 
 	if (rtlpriv->rtlhal.version >= VERSION_8192S_BCUT)
 		ratr_bitmap &= 0x0FFFFFFF;
@@ -2438,23 +2480,9 @@ void rtl92se_set_key(struct ieee80211_hw *hw, u32 key_index, u8 *p_macaddr,
 				rtl_cam_del_entry(hw, p_macaddr);
 			rtl_cam_delete_one_entry(hw, p_macaddr, entry_id);
 		} else {
-			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
-				 "The insert KEY length is %d\n",
-				 rtlpriv->sec.key_len[PAIRWISE_KEYIDX]);
-			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
-				 "The insert KEY is %x %x\n",
-				 rtlpriv->sec.key_buf[0][0],
-				 rtlpriv->sec.key_buf[0][1]);
-
 			RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
 				 "add one entry\n");
 			if (is_pairwise) {
-				RT_PRINT_DATA(rtlpriv, COMP_SEC, DBG_LOUD,
-					      "Pairwise Key content",
-					      rtlpriv->sec.pairwise_key,
-					      rtlpriv->sec.
-					      key_len[PAIRWISE_KEYIDX]);
-
 				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
 					 "set Pairwise key\n");
 
@@ -2502,3 +2530,23 @@ void rtl92se_resume(struct ieee80211_hw *hw)
 		pci_write_config_dword(rtlpci->pdev, 0x40,
 			val & 0xffff00ff);
 }
+
+/* Turn on AAP (RCR:bit 0) for promicuous mode. */
+void rtl92se_allow_all_destaddr(struct ieee80211_hw *hw,
+				bool allow_all_da, bool write_into_reg)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	if (allow_all_da) /* Set BIT0 */
+		rtlpci->receive_config |= RCR_AAP;
+	else /* Clear BIT0 */
+		rtlpci->receive_config &= ~RCR_AAP;
+
+	if (write_into_reg)
+		rtl_write_dword(rtlpriv, RCR, rtlpci->receive_config);
+
+	RT_TRACE(rtlpriv, COMP_TURBO | COMP_INIT, DBG_LOUD,
+		 "receive_config=0x%08X, write_into_reg=%d\n",
+		 rtlpci->receive_config, write_into_reg);
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/hw.h b/drivers/net/wireless/rtlwifi/rtl8192se/hw.h
index a8e068c76e47..da48aa8cbe6f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/hw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/hw.h
@@ -74,6 +74,7 @@ void rtl92se_set_key(struct ieee80211_hw *hw,
 		     u8 enc_algo, bool is_wepkey, bool clear_all);
 void rtl92se_suspend(struct ieee80211_hw *hw);
 void rtl92se_resume(struct ieee80211_hw *hw);
+void rtl92se_allow_all_destaddr(struct ieee80211_hw *hw,
+				bool allow_all_da, bool write_into_reg);
 
 #endif
-
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/phy.c b/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
index 67404975e00b..9c092e6eb3fe 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
@@ -1307,6 +1307,8 @@ static void _rtl92s_phy_set_fwcmd_io(struct ieee80211_hw *hw)
 	if (is_hal_stop(rtlhal))
 		return;
 
+	if (hal_get_firmwareversion(rtlpriv) < 0x34)
+		goto skip;
 	/* We re-map RA related CMD IO to combinational ones */
 	/* if FW version is v.52 or later. */
 	switch (rtlhal->current_fwcmd_io) {
@@ -1320,6 +1322,7 @@ static void _rtl92s_phy_set_fwcmd_io(struct ieee80211_hw *hw)
 		break;
 	}
 
+skip:
 	switch (rtlhal->current_fwcmd_io) {
 	case FW_CMD_RA_RESET:
 		RT_TRACE(rtlpriv, COMP_CMD, DBG_DMESG, "FW_CMD_RA_RESET\n");
@@ -1440,7 +1443,7 @@ bool rtl92s_phy_set_fw_cmd(struct ieee80211_hw *hw, enum fwcmd_iotype fw_cmdio)
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	u32	fw_param = FW_CMD_IO_PARA_QUERY(rtlpriv);
 	u16	fw_cmdmap = FW_CMD_IO_QUERY(rtlpriv);
-	bool bPostProcessing = false;
+	bool postprocessing = false;
 
 	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 		 "Set FW Cmd(%#x), set_fwcmd_inprogress(%d)\n",
@@ -1449,15 +1452,24 @@ bool rtl92s_phy_set_fw_cmd(struct ieee80211_hw *hw, enum fwcmd_iotype fw_cmdio)
 	do {
 		/* We re-map to combined FW CMD ones if firmware version */
 		/* is v.53 or later. */
-		switch (fw_cmdio) {
-		case FW_CMD_RA_REFRESH_N:
-			fw_cmdio = FW_CMD_RA_REFRESH_N_COMB;
-			break;
-		case FW_CMD_RA_REFRESH_BG:
-			fw_cmdio = FW_CMD_RA_REFRESH_BG_COMB;
-			break;
-		default:
-			break;
+		if (hal_get_firmwareversion(rtlpriv) >= 0x35) {
+			switch (fw_cmdio) {
+			case FW_CMD_RA_REFRESH_N:
+				fw_cmdio = FW_CMD_RA_REFRESH_N_COMB;
+				break;
+			case FW_CMD_RA_REFRESH_BG:
+				fw_cmdio = FW_CMD_RA_REFRESH_BG_COMB;
+				break;
+			default:
+				break;
+			}
+		} else {
+			if ((fw_cmdio == FW_CMD_IQK_ENABLE) ||
+			    (fw_cmdio == FW_CMD_RA_REFRESH_N) ||
+			    (fw_cmdio == FW_CMD_RA_REFRESH_BG)) {
+				postprocessing = true;
+				break;
+			}
 		}
 
 		/* If firmware version is v.62 or later,
@@ -1588,19 +1600,19 @@ bool rtl92s_phy_set_fw_cmd(struct ieee80211_hw *hw, enum fwcmd_iotype fw_cmdio)
 				fw_cmdmap &= ~FW_DIG_ENABLE_CTL;
 
 			FW_CMD_IO_SET(rtlpriv, fw_cmdmap);
-			bPostProcessing = true;
+			postprocessing = true;
 			break;
 		case FW_CMD_PAUSE_DM_BY_SCAN:
 			fw_cmdmap &= ~(FW_DIG_ENABLE_CTL |
 				       FW_HIGH_PWR_ENABLE_CTL |
 				       FW_SS_CTL);
 			FW_CMD_IO_SET(rtlpriv, fw_cmdmap);
-			bPostProcessing = true;
+			postprocessing = true;
 			break;
 		case FW_CMD_HIGH_PWR_DISABLE:
 			fw_cmdmap &= ~FW_HIGH_PWR_ENABLE_CTL;
 			FW_CMD_IO_SET(rtlpriv, fw_cmdmap);
-			bPostProcessing = true;
+			postprocessing = true;
 			break;
 		case FW_CMD_HIGH_PWR_ENABLE:
 			if (!(rtlpriv->dm.dm_flag & HAL_DM_HIPWR_DISABLE) &&
@@ -1608,7 +1620,7 @@ bool rtl92s_phy_set_fw_cmd(struct ieee80211_hw *hw, enum fwcmd_iotype fw_cmdio)
 				fw_cmdmap |= (FW_HIGH_PWR_ENABLE_CTL |
 					      FW_SS_CTL);
 				FW_CMD_IO_SET(rtlpriv, fw_cmdmap);
-				bPostProcessing = true;
+				postprocessing = true;
 			}
 			break;
 		case FW_CMD_DIG_MODE_FA:
@@ -1629,14 +1641,15 @@ bool rtl92s_phy_set_fw_cmd(struct ieee80211_hw *hw, enum fwcmd_iotype fw_cmdio)
 		default:
 			/* Pass to original FW CMD processing callback
 			 * routine. */
-			bPostProcessing = true;
+			postprocessing = true;
 			break;
 		}
 	} while (false);
 
 	/* We shall post processing these FW CMD if
-	 * variable bPostProcessing is set. */
-	if (bPostProcessing && !rtlhal->set_fwcmd_inprogress) {
+	 * variable postprocessing is set.
+	 */
+	if (postprocessing && !rtlhal->set_fwcmd_inprogress) {
 		rtlhal->set_fwcmd_inprogress = true;
 		/* Update current FW Cmd for callback use. */
 		rtlhal->current_fwcmd_io = fw_cmdio;
@@ -1697,8 +1710,18 @@ void rtl92s_phy_switch_ephy_parameter(struct ieee80211_hw *hw)
 
 }
 
-void rtl92s_phy_set_beacon_hwreg(struct ieee80211_hw *hw, u16 BeaconInterval)
+void rtl92s_phy_set_beacon_hwreg(struct ieee80211_hw *hw, u16 beaconinterval)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	rtl_write_dword(rtlpriv, WFM5, 0xF1000000 | (BeaconInterval << 8));
+	u32 new_bcn_num = 0;
+
+	if (hal_get_firmwareversion(rtlpriv) >= 0x33) {
+		/* Fw v.51 and later. */
+		rtl_write_dword(rtlpriv, WFM5, 0xF1000000 |
+				(beaconinterval << 8));
+	} else {
+		new_bcn_num = beaconinterval * 32 - 64;
+		rtl_write_dword(rtlpriv, WFM3 + 4, new_bcn_num);
+		rtl_write_dword(rtlpriv, WFM3, 0xB026007C);
+	}
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/phy.h b/drivers/net/wireless/rtlwifi/rtl8192se/phy.h
index ac0387770630..8acf4765a7a6 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/phy.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/phy.h
@@ -39,6 +39,7 @@
 #define MAX_POSTCMD_CNT			16
 
 #define RF90_PATH_MAX			4
+#define RF6052_MAX_PATH			2
 
 enum version_8192s {
 	VERSION_8192S_ACUT,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/sw.c b/drivers/net/wireless/rtlwifi/rtl8192se/sw.c
index cecc377e9e61..2e8e6f8d2d51 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/sw.c
@@ -290,6 +290,7 @@ static struct rtl_hal_ops rtl8192se_hal_ops = {
 	.enable_hw_sec = rtl92se_enable_hw_security_config,
 	.set_key = rtl92se_set_key,
 	.init_sw_leds = rtl92se_init_sw_leds,
+	.allow_all_destaddr = rtl92se_allow_all_destaddr,
 	.get_bbreg = rtl92s_phy_query_bb_reg,
 	.set_bbreg = rtl92s_phy_set_bb_reg,
 	.get_rfreg = rtl92s_phy_query_rf_reg,
@@ -366,7 +367,7 @@ static struct rtl_hal_cfg rtl92se_hal_cfg = {
 
 	.maps[RTL_IMR_TXFOVW] = IMR_TXFOVW,
 	.maps[RTL_IMR_PSTIMEOUT] = IMR_PSTIMEOUT,
-	.maps[RTL_IMR_BcnInt] = IMR_BCNINT,
+	.maps[RTL_IMR_BCNINT] = IMR_BCNINT,
 	.maps[RTL_IMR_RXFOVW] = IMR_RXFOVW,
 	.maps[RTL_IMR_RDU] = IMR_RDU,
 	.maps[RTL_IMR_ATIMEND] = IMR_ATIMEND,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/trx.c b/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
index 7b0a2e75b8b8..c7095118de6e 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
@@ -30,6 +30,7 @@
 #include "../wifi.h"
 #include "../pci.h"
 #include "../base.h"
+#include "../stats.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
@@ -43,7 +44,7 @@ static u8 _rtl92se_map_hwqueue_to_fwqueue(struct sk_buff *skb,	u8 skb_queue)
 
 	if (unlikely(ieee80211_is_beacon(fc)))
 		return QSLT_BEACON;
-	if (ieee80211_is_mgmt(fc))
+	if (ieee80211_is_mgmt(fc) || ieee80211_is_ctl(fc))
 		return QSLT_MGNT;
 	if (ieee80211_is_nullfunc(fc))
 		return QSLT_HIGH;
@@ -51,65 +52,6 @@ static u8 _rtl92se_map_hwqueue_to_fwqueue(struct sk_buff *skb,	u8 skb_queue)
 	return skb->priority;
 }
 
-static u8 _rtl92s_query_rxpwrpercentage(char antpower)
-{
-	if ((antpower <= -100) || (antpower >= 20))
-		return 0;
-	else if (antpower >= 0)
-		return 100;
-	else
-		return 100 + antpower;
-}
-
-static u8 _rtl92s_evm_db_to_percentage(char value)
-{
-	char ret_val;
-	ret_val = value;
-
-	if (ret_val >= 0)
-		ret_val = 0;
-
-	if (ret_val <= -33)
-		ret_val = -33;
-
-	ret_val = 0 - ret_val;
-	ret_val *= 3;
-
-	if (ret_val == 99)
-		ret_val = 100;
-
-	return ret_val;
-}
-
-static long _rtl92se_translate_todbm(struct ieee80211_hw *hw,
-				     u8 signal_strength_index)
-{
-	long signal_power;
-
-	signal_power = (long)((signal_strength_index + 1) >> 1);
-	signal_power -= 95;
-	return signal_power;
-}
-
-static long _rtl92se_signal_scale_mapping(struct ieee80211_hw *hw,
-		long currsig)
-{
-	long retsig = 0;
-
-	/* Step 1. Scale mapping. */
-	if (currsig > 47)
-		retsig = 100;
-	else if (currsig > 14 && currsig <= 47)
-		retsig = 100 - ((47 - currsig) * 3) / 2;
-	else if (currsig > 2 && currsig <= 14)
-		retsig = 48 - ((14 - currsig) * 15) / 7;
-	else if (currsig >= 0)
-		retsig = currsig * 9 + 1;
-
-	return retsig;
-}
-
-
 static void _rtl92se_query_rxphystatus(struct ieee80211_hw *hw,
 				       struct rtl_stats *pstats, u8 *pdesc,
 				       struct rx_fwinfo *p_drvinfo,
@@ -119,11 +61,11 @@ static void _rtl92se_query_rxphystatus(struct ieee80211_hw *hw,
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct phy_sts_cck_8192s_t *cck_buf;
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtlpriv);
 	s8 rx_pwr_all = 0, rx_pwr[4];
 	u8 rf_rx_num = 0, evm, pwdb_all;
 	u8 i, max_spatial_stream;
 	u32 rssi, total_rssi = 0;
-	bool in_powersavemode = false;
 	bool is_cck = pstats->is_cck;
 
 	pstats->packet_matchbssid = packet_match_bssid;
@@ -136,7 +78,7 @@ static void _rtl92se_query_rxphystatus(struct ieee80211_hw *hw,
 		u8 report, cck_highpwr;
 		cck_buf = (struct phy_sts_cck_8192s_t *)p_drvinfo;
 
-		if (!in_powersavemode)
+		if (ppsc->rfpwr_state == ERFON)
 			cck_highpwr = (u8) rtl_get_bbreg(hw,
 						RFPGA0_XA_HSSIPARAMETER2,
 						0x200);
@@ -181,7 +123,7 @@ static void _rtl92se_query_rxphystatus(struct ieee80211_hw *hw,
 			}
 		}
 
-		pwdb_all = _rtl92s_query_rxpwrpercentage(rx_pwr_all);
+		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
 
 		/* CCK gain is smaller than OFDM/MCS gain,  */
 		/* so we add gain diff by experiences, the val is 6 */
@@ -222,13 +164,13 @@ static void _rtl92se_query_rxphystatus(struct ieee80211_hw *hw,
 	} else {
 		rtlpriv->dm.rfpath_rxenable[0] =
 		    rtlpriv->dm.rfpath_rxenable[1] = true;
-		for (i = RF90_PATH_A; i < RF90_PATH_MAX; i++) {
+		for (i = RF90_PATH_A; i < RF6052_MAX_PATH; i++) {
 			if (rtlpriv->dm.rfpath_rxenable[i])
 				rf_rx_num++;
 
 			rx_pwr[i] = ((p_drvinfo->gain_trsw[i] &
 				    0x3f) * 2) - 110;
-			rssi = _rtl92s_query_rxpwrpercentage(rx_pwr[i]);
+			rssi = rtl_query_rxpwrpercentage(rx_pwr[i]);
 			total_rssi += rssi;
 			rtlpriv->stats.rx_snr_db[i] =
 					 (long)(p_drvinfo->rxsnr[i] / 2);
@@ -238,7 +180,7 @@ static void _rtl92se_query_rxphystatus(struct ieee80211_hw *hw,
 		}
 
 		rx_pwr_all = ((p_drvinfo->pwdb_all >> 1) & 0x7f) - 110;
-		pwdb_all = _rtl92s_query_rxpwrpercentage(rx_pwr_all);
+		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
 		pstats->rx_pwdb_all = pwdb_all;
 		pstats->rxpower = rx_pwr_all;
 		pstats->recvsignalpower = rx_pwr_all;
@@ -250,7 +192,7 @@ static void _rtl92se_query_rxphystatus(struct ieee80211_hw *hw,
 			max_spatial_stream = 1;
 
 		for (i = 0; i < max_spatial_stream; i++) {
-			evm = _rtl92s_evm_db_to_percentage(p_drvinfo->rxevm[i]);
+			evm = rtl_evm_db_to_percentage(p_drvinfo->rxevm[i]);
 
 			if (packet_match_bssid) {
 				if (i == 0)
@@ -262,212 +204,13 @@ static void _rtl92se_query_rxphystatus(struct ieee80211_hw *hw,
 	}
 
 	if (is_cck)
-		pstats->signalstrength = (u8)(_rtl92se_signal_scale_mapping(hw,
+		pstats->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
 					 pwdb_all));
 	else if (rf_rx_num != 0)
-		pstats->signalstrength = (u8) (_rtl92se_signal_scale_mapping(hw,
+		pstats->signalstrength = (u8) (rtl_signal_scale_mapping(hw,
 				total_rssi /= rf_rx_num));
 }
 
-static void _rtl92se_process_ui_rssi(struct ieee80211_hw *hw,
-				     struct rtl_stats *pstats)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 rfpath;
-	u32 last_rssi, tmpval;
-
-	if (pstats->packet_toself || pstats->packet_beacon) {
-		rtlpriv->stats.rssi_calculate_cnt++;
-
-		if (rtlpriv->stats.ui_rssi.total_num++ >=
-		    PHY_RSSI_SLID_WIN_MAX) {
-			rtlpriv->stats.ui_rssi.total_num =
-					 PHY_RSSI_SLID_WIN_MAX;
-			last_rssi = rtlpriv->stats.ui_rssi.elements[
-				rtlpriv->stats.ui_rssi.index];
-			rtlpriv->stats.ui_rssi.total_val -= last_rssi;
-		}
-
-		rtlpriv->stats.ui_rssi.total_val += pstats->signalstrength;
-		rtlpriv->stats.ui_rssi.elements[rtlpriv->stats.ui_rssi.index++]
-			 = pstats->signalstrength;
-
-		if (rtlpriv->stats.ui_rssi.index >= PHY_RSSI_SLID_WIN_MAX)
-			rtlpriv->stats.ui_rssi.index = 0;
-
-		tmpval = rtlpriv->stats.ui_rssi.total_val /
-			rtlpriv->stats.ui_rssi.total_num;
-		rtlpriv->stats.signal_strength = _rtl92se_translate_todbm(hw,
-								(u8) tmpval);
-		pstats->rssi = rtlpriv->stats.signal_strength;
-	}
-
-	if (!pstats->is_cck && pstats->packet_toself) {
-		for (rfpath = RF90_PATH_A; rfpath < rtlphy->num_total_rfpath;
-		     rfpath++) {
-			if (rtlpriv->stats.rx_rssi_percentage[rfpath] == 0) {
-				rtlpriv->stats.rx_rssi_percentage[rfpath] =
-				    pstats->rx_mimo_signalstrength[rfpath];
-
-			}
-
-			if (pstats->rx_mimo_signalstrength[rfpath] >
-			    rtlpriv->stats.rx_rssi_percentage[rfpath]) {
-				rtlpriv->stats.rx_rssi_percentage[rfpath] =
-				    ((rtlpriv->stats.rx_rssi_percentage[rfpath]
-				    * (RX_SMOOTH_FACTOR - 1)) +
-				    (pstats->rx_mimo_signalstrength[rfpath])) /
-				    (RX_SMOOTH_FACTOR);
-
-				rtlpriv->stats.rx_rssi_percentage[rfpath] =
-				    rtlpriv->stats.rx_rssi_percentage[rfpath]
-				    + 1;
-			} else {
-				rtlpriv->stats.rx_rssi_percentage[rfpath] =
-				    ((rtlpriv->stats.rx_rssi_percentage[rfpath]
-				    * (RX_SMOOTH_FACTOR - 1)) +
-				    (pstats->rx_mimo_signalstrength[rfpath])) /
-				    (RX_SMOOTH_FACTOR);
-			}
-
-		}
-	}
-}
-
-static void _rtl92se_update_rxsignalstatistics(struct ieee80211_hw *hw,
-					       struct rtl_stats *pstats)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int weighting = 0;
-
-	if (rtlpriv->stats.recv_signal_power == 0)
-		rtlpriv->stats.recv_signal_power = pstats->recvsignalpower;
-
-	if (pstats->recvsignalpower > rtlpriv->stats.recv_signal_power)
-		weighting = 5;
-	else if (pstats->recvsignalpower < rtlpriv->stats.recv_signal_power)
-		weighting = (-5);
-
-	rtlpriv->stats.recv_signal_power = (rtlpriv->stats.recv_signal_power * 5
-					   + pstats->recvsignalpower +
-					   weighting) / 6;
-}
-
-static void _rtl92se_process_pwdb(struct ieee80211_hw *hw,
-				  struct rtl_stats *pstats)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	long undec_sm_pwdb = 0;
-
-	if (mac->opmode == NL80211_IFTYPE_ADHOC) {
-		return;
-	} else {
-		undec_sm_pwdb =
-		    rtlpriv->dm.undec_sm_pwdb;
-	}
-
-	if (pstats->packet_toself || pstats->packet_beacon) {
-		if (undec_sm_pwdb < 0)
-			undec_sm_pwdb = pstats->rx_pwdb_all;
-
-		if (pstats->rx_pwdb_all > (u32) undec_sm_pwdb) {
-			undec_sm_pwdb =
-			    (((undec_sm_pwdb) *
-			    (RX_SMOOTH_FACTOR - 1)) +
-			    (pstats->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
-
-			undec_sm_pwdb = undec_sm_pwdb + 1;
-		} else {
-			undec_sm_pwdb = (((undec_sm_pwdb) *
-			      (RX_SMOOTH_FACTOR - 1)) + (pstats->rx_pwdb_all)) /
-			      (RX_SMOOTH_FACTOR);
-		}
-
-		rtlpriv->dm.undec_sm_pwdb = undec_sm_pwdb;
-		_rtl92se_update_rxsignalstatistics(hw, pstats);
-	}
-}
-
-static void rtl_92s_process_streams(struct ieee80211_hw *hw,
-				    struct rtl_stats *pstats)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 stream;
-
-	for (stream = 0; stream < 2; stream++) {
-		if (pstats->rx_mimo_sig_qual[stream] != -1) {
-			if (rtlpriv->stats.rx_evm_percentage[stream] == 0) {
-				rtlpriv->stats.rx_evm_percentage[stream] =
-				    pstats->rx_mimo_sig_qual[stream];
-			}
-
-			rtlpriv->stats.rx_evm_percentage[stream] =
-			    ((rtlpriv->stats.rx_evm_percentage[stream] *
-					(RX_SMOOTH_FACTOR - 1)) +
-			     (pstats->rx_mimo_sig_qual[stream] *
-					1)) / (RX_SMOOTH_FACTOR);
-		}
-	}
-}
-
-static void _rtl92se_process_ui_link_quality(struct ieee80211_hw *hw,
-					     struct rtl_stats *pstats)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 last_evm = 0, tmpval;
-
-	if (pstats->signalquality != 0) {
-		if (pstats->packet_toself || pstats->packet_beacon) {
-
-			if (rtlpriv->stats.ui_link_quality.total_num++ >=
-			    PHY_LINKQUALITY_SLID_WIN_MAX) {
-				rtlpriv->stats.ui_link_quality.total_num =
-				    PHY_LINKQUALITY_SLID_WIN_MAX;
-				last_evm =
-				    rtlpriv->stats.ui_link_quality.elements[
-				    rtlpriv->stats.ui_link_quality.index];
-				rtlpriv->stats.ui_link_quality.total_val -=
-				    last_evm;
-			}
-
-			rtlpriv->stats.ui_link_quality.total_val +=
-			    pstats->signalquality;
-			rtlpriv->stats.ui_link_quality.elements[
-				rtlpriv->stats.ui_link_quality.index++] =
-			    pstats->signalquality;
-
-			if (rtlpriv->stats.ui_link_quality.index >=
-			    PHY_LINKQUALITY_SLID_WIN_MAX)
-				rtlpriv->stats.ui_link_quality.index = 0;
-
-			tmpval = rtlpriv->stats.ui_link_quality.total_val /
-			    rtlpriv->stats.ui_link_quality.total_num;
-			rtlpriv->stats.signal_quality = tmpval;
-
-			rtlpriv->stats.last_sigstrength_inpercent = tmpval;
-
-			rtl_92s_process_streams(hw, pstats);
-
-		}
-	}
-}
-
-static void _rtl92se_process_phyinfo(struct ieee80211_hw *hw,
-				     u8 *buffer,
-				     struct rtl_stats *pcurrent_stats)
-{
-
-	if (!pcurrent_stats->packet_matchbssid &&
-	    !pcurrent_stats->packet_beacon)
-		return;
-
-	_rtl92se_process_ui_rssi(hw, pcurrent_stats);
-	_rtl92se_process_pwdb(hw, pcurrent_stats);
-	_rtl92se_process_ui_link_quality(hw, pcurrent_stats);
-}
-
 static void _rtl92se_translate_rx_signal_stuff(struct ieee80211_hw *hw,
 		struct sk_buff *skb, struct rtl_stats *pstats,
 		u8 *pdesc, struct rx_fwinfo *p_drvinfo)
@@ -505,7 +248,7 @@ static void _rtl92se_translate_rx_signal_stuff(struct ieee80211_hw *hw,
 
 	_rtl92se_query_rxphystatus(hw, pstats, pdesc, p_drvinfo,
 			packet_matchbssid, packet_toself, packet_beacon);
-	_rtl92se_process_phyinfo(hw, tmp_buf, pstats);
+	rtl_process_phyinfo(hw, tmp_buf, pstats);
 }
 
 bool rtl92se_rx_query_desc(struct ieee80211_hw *hw, struct rtl_stats *stats,
@@ -538,11 +281,8 @@ bool rtl92se_rx_query_desc(struct ieee80211_hw *hw, struct rtl_stats *stats,
 	if (stats->hwerror)
 		return false;
 
-	rx_status->freq = hw->conf.channel->center_freq;
-	rx_status->band = hw->conf.channel->band;
-
-	hdr = (struct ieee80211_hdr *)(skb->data + stats->rx_drvinfo_size
-	      + stats->rx_bufshift);
+	rx_status->freq = hw->conf.chandef.chan->center_freq;
+	rx_status->band = hw->conf.chandef.chan->band;
 
 	if (stats->crc)
 		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
@@ -563,6 +303,13 @@ bool rtl92se_rx_query_desc(struct ieee80211_hw *hw, struct rtl_stats *stats,
 	 * for IEEE80211w frame, and mac80211 sw will help
 	 * to decrypt it */
 	if (stats->decrypted) {
+		hdr = (struct ieee80211_hdr *)(skb->data +
+		       stats->rx_drvinfo_size + stats->rx_bufshift);
+
+		if (!hdr) {
+			/* during testing, hdr was NULL here */
+			return false;
+		}
 		if ((ieee80211_is_robust_mgmt_frame(hdr)) &&
 			(ieee80211_has_protected(hdr->frame_control)))
 			rx_status->flag &= ~RX_FLAG_DECRYPTED;
@@ -630,6 +377,11 @@ void rtl92se_tx_fill_desc(struct ieee80211_hw *hw,
 
 	CLEAR_PCI_TX_DESC_CONTENT(pdesc, TX_DESC_SIZE_RTL8192S);
 
+	if (ieee80211_is_nullfunc(fc) || ieee80211_is_ctl(fc)) {
+		firstseg = true;
+		lastseg = true;
+	}
+
 	if (firstseg) {
 		if (rtlpriv->dm.useramask) {
 			/* set txdesc macId */
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c
index 12e2a3cb0701..a36eee28f9e7 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c
@@ -166,8 +166,8 @@ static void rtl8723ae_dm_diginit(struct ieee80211_hw *hw)
 	dm_digtable->rssi_highthresh = DM_DIG_THRESH_HIGH;
 	dm_digtable->fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
 	dm_digtable->fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
-	dm_digtable->rx_gain_range_max = DM_DIG_MAX;
-	dm_digtable->rx_gain_range_min = DM_DIG_MIN;
+	dm_digtable->rx_gain_max = DM_DIG_MAX;
+	dm_digtable->rx_gain_min = DM_DIG_MIN;
 	dm_digtable->back_val = DM_DIG_BACKOFF_DEFAULT;
 	dm_digtable->back_range_max = DM_DIG_BACKOFF_MAX;
 	dm_digtable->back_range_min = DM_DIG_BACKOFF_MIN;
@@ -291,11 +291,11 @@ static void rtl92c_dm_ctrl_initgain_by_rssi(struct ieee80211_hw *hw)
 	}
 
 	if ((dgtbl->rssi_val_min + 10 - dgtbl->back_val) >
-	    dgtbl->rx_gain_range_max)
-		dgtbl->cur_igvalue = dgtbl->rx_gain_range_max;
+	    dgtbl->rx_gain_max)
+		dgtbl->cur_igvalue = dgtbl->rx_gain_max;
 	else if ((dgtbl->rssi_val_min + 10 -
-		  dgtbl->back_val) < dgtbl->rx_gain_range_min)
-		dgtbl->cur_igvalue = dgtbl->rx_gain_range_min;
+		  dgtbl->back_val) < dgtbl->rx_gain_min)
+		dgtbl->cur_igvalue = dgtbl->rx_gain_min;
 	else
 		dgtbl->cur_igvalue = dgtbl->rssi_val_min + 10 - dgtbl->back_val;
 
@@ -707,6 +707,77 @@ void rtl8723ae_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
 		rtlpriv->dm.useramask = false;
 }
 
+static void rtl8723ae_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rate_adaptive *p_ra = &(rtlpriv->ra);
+	u32 low_rssithresh_for_ra, high_rssithresh_for_ra;
+	struct ieee80211_sta *sta = NULL;
+
+	if (is_hal_stop(rtlhal)) {
+		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+			 " driver is going to unload\n");
+		return;
+	}
+
+	if (!rtlpriv->dm.useramask) {
+		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+			 " driver does not control rate adaptive mask\n");
+		return;
+	}
+
+	if (mac->link_state == MAC80211_LINKED &&
+	    mac->opmode == NL80211_IFTYPE_STATION) {
+		switch (p_ra->pre_ratr_state) {
+		case DM_RATR_STA_HIGH:
+			high_rssithresh_for_ra = 50;
+			low_rssithresh_for_ra = 20;
+			break;
+		case DM_RATR_STA_MIDDLE:
+			high_rssithresh_for_ra = 55;
+			low_rssithresh_for_ra = 20;
+			break;
+		case DM_RATR_STA_LOW:
+			high_rssithresh_for_ra = 50;
+			low_rssithresh_for_ra = 25;
+			break;
+		default:
+			high_rssithresh_for_ra = 50;
+			low_rssithresh_for_ra = 20;
+			break;
+		}
+
+		if (rtlpriv->dm.undec_sm_pwdb > high_rssithresh_for_ra)
+			p_ra->ratr_state = DM_RATR_STA_HIGH;
+		else if (rtlpriv->dm.undec_sm_pwdb > low_rssithresh_for_ra)
+			p_ra->ratr_state = DM_RATR_STA_MIDDLE;
+		else
+			p_ra->ratr_state = DM_RATR_STA_LOW;
+
+		if (p_ra->pre_ratr_state != p_ra->ratr_state) {
+			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+				 "RSSI = %ld\n",
+				 rtlpriv->dm.undec_sm_pwdb);
+			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+				 "RSSI_LEVEL = %d\n", p_ra->ratr_state);
+			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+				 "PreState = %d, CurState = %d\n",
+				 p_ra->pre_ratr_state, p_ra->ratr_state);
+
+			rcu_read_lock();
+			sta = rtl_find_sta(hw, mac->bssid);
+			if (sta)
+				rtlpriv->cfg->ops->update_rate_tbl(hw, sta,
+							   p_ra->ratr_state);
+			rcu_read_unlock();
+
+			p_ra->pre_ratr_state = p_ra->ratr_state;
+		}
+	}
+}
+
 static void rtl8723ae_dm_init_dynamic_bpowersaving(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -853,6 +924,9 @@ void rtl8723ae_dm_watchdog(struct ieee80211_hw *hw)
 	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FWLPS_RF_ON,
 				      (u8 *) (&fw_ps_awake));
 
+	if (ppsc->p2p_ps_info.p2p_ps_mode)
+		fw_ps_awake = false;
+
 	if ((ppsc->rfpwr_state == ERFON) &&
 	    ((!fw_current_inpsmode) && fw_ps_awake) &&
 	    (!ppsc->rfchange_inprogress)) {
@@ -861,7 +935,7 @@ void rtl8723ae_dm_watchdog(struct ieee80211_hw *hw)
 		rtl8723ae_dm_false_alarm_counter_statistics(hw);
 		rtl8723ae_dm_dynamic_bpowersaving(hw);
 		rtl8723ae_dm_dynamic_txpower(hw);
-		/* rtl92c_dm_refresh_rate_adaptive_mask(hw); */
+		rtl8723ae_dm_refresh_rate_adaptive_mask(hw);
 		rtl8723ae_dm_bt_coexist(hw);
 		rtl8723ae_dm_check_edca_turbo(hw);
 	}
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h
index 39d246196247..a372b0204456 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h
@@ -55,7 +55,13 @@
 #define DM_DIG_BACKOFF_MIN			-4
 #define DM_DIG_BACKOFF_DEFAULT			10
 
+#define RXPATHSELECTION_SS_TH_LOW		30
+#define RXPATHSELECTION_DIFF_TH			18
+
 #define DM_RATR_STA_INIT			0
+#define DM_RATR_STA_HIGH			1
+#define DM_RATR_STA_MIDDLE			2
+#define DM_RATR_STA_LOW				3
 
 #define TXHIGHPWRLEVEL_NORMAL			0
 #define TXHIGHPWRLEVEL_LEVEL1			1
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/fw.c b/drivers/net/wireless/rtlwifi/rtl8723ae/fw.c
index 35cb8f83eed4..dedfa1ed3e02 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/fw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/fw.c
@@ -494,7 +494,9 @@ void rtl8723ae_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
 
 	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, mode);
-	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode, 1);
+	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
+					 (rtlpriv->mac80211.p2p) ?
+					 ppsc->smart_ps : 1);
 	SET_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(u1_h2c_set_pwrmode,
 					      ppsc->reg_max_lps_awakeintvl);
 
@@ -741,3 +743,96 @@ void rtl8723ae_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
 
 	rtl8723ae_fill_h2c_cmd(hw, H2C_JOINBSSRPT, 1, u1_joinbssrpt_parm);
 }
+
+static void rtl8723e_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw,
+					    u8 ctwindow)
+{
+	u8 u1_ctwindow_period[1] = {ctwindow};
+
+	rtl8723ae_fill_h2c_cmd(hw, H2C_P2P_PS_CTW_CMD, 1, u1_ctwindow_period);
+}
+
+void rtl8723ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
+	struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
+	u8	i;
+	u16	ctwindow;
+	u32	start_time, tsf_low;
+
+	switch (p2p_ps_state) {
+	case P2P_PS_DISABLE:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
+		memset(p2p_ps_offload, 0, sizeof(struct p2p_ps_offload_t));
+		break;
+	case P2P_PS_ENABLE:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
+		/* update CTWindow value. */
+		if (p2pinfo->ctwindow > 0) {
+			p2p_ps_offload->ctwindow_en = 1;
+			ctwindow = p2pinfo->ctwindow;
+			rtl8723e_set_p2p_ctw_period_cmd(hw, ctwindow);
+		}
+
+		/* hw only support 2 set of NoA */
+		for (i = 0; i < p2pinfo->noa_num; i++) {
+			/* To control the register setting for which NOA*/
+			rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
+			if (i == 0)
+				p2p_ps_offload->noa0_en = 1;
+			else
+				p2p_ps_offload->noa1_en = 1;
+
+			/* config P2P NoA Descriptor Register */
+			rtl_write_dword(rtlpriv, 0x5E0,
+					p2pinfo->noa_duration[i]);
+			rtl_write_dword(rtlpriv, 0x5E4,
+					p2pinfo->noa_interval[i]);
+
+			/*Get Current TSF value */
+			tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
+
+			start_time = p2pinfo->noa_start_time[i];
+			if (p2pinfo->noa_count_type[i] != 1) {
+				while (start_time <= (tsf_low+(50*1024))) {
+					start_time += p2pinfo->noa_interval[i];
+					if (p2pinfo->noa_count_type[i] != 255)
+						p2pinfo->noa_count_type[i]--;
+				}
+			}
+			rtl_write_dword(rtlpriv, 0x5E8, start_time);
+			rtl_write_dword(rtlpriv, 0x5EC,
+					p2pinfo->noa_count_type[i]);
+		}
+		if ((p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0)) {
+			/* rst p2p circuit */
+			rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
+
+			p2p_ps_offload->offload_en = 1;
+
+			if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
+				p2p_ps_offload->role = 1;
+				p2p_ps_offload->allstasleep = 0;
+			} else {
+				p2p_ps_offload->role = 0;
+			}
+			p2p_ps_offload->discovery = 0;
+		}
+		break;
+	case P2P_PS_SCAN:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN\n");
+		p2p_ps_offload->discovery = 1;
+		break;
+	case P2P_PS_SCAN_DONE:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN_DONE\n");
+		p2p_ps_offload->discovery = 0;
+		p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
+		break;
+	default:
+		break;
+	}
+	rtl8723ae_fill_h2c_cmd(hw, H2C_P2P_PS_OFFLOAD, 1, (u8 *)p2p_ps_offload);
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/fw.h b/drivers/net/wireless/rtlwifi/rtl8723ae/fw.h
index 89994e16dc83..ed3b795e6980 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/fw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/fw.h
@@ -70,8 +70,10 @@ enum rtl8192c_h2c_cmd {
 	H2C_SETPWRMODE = 1,
 	H2C_JOINBSSRPT = 2,
 	H2C_RSVDPAGE = 3,
-	H2C_RSSI_REPORT = 5,
-	H2C_RA_MASK = 6,
+	H2C_RSSI_REPORT = 4,
+	H2C_P2P_PS_CTW_CMD = 5,
+	H2C_P2P_PS_OFFLOAD = 6,
+	H2C_RA_MASK = 7,
 	MAX_H2CCMD
 };
 
@@ -97,5 +99,6 @@ void rtl8723ae_firmware_selfreset(struct ieee80211_hw *hw);
 void rtl8723ae_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
 void rtl8723ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
 void rtl8723ae_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus);
+void rtl8723ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c b/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c
index 9a0c71c2e15e..c333dfd116b8 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c
@@ -449,6 +449,9 @@ void rtl8723ae_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		rtl8723ae_set_fw_joinbss_report_cmd(hw, (*(u8 *) val));
 
 		break; }
+	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:
+		rtl8723ae_set_p2p_ps_offload_cmd(hw, (*(u8 *)val));
+		break;
 	case HW_VAR_AID:{
 		u16 u2btmp;
 		u2btmp = rtl_read_word(rtlpriv, REG_BCN_PSR_RPT);
@@ -474,6 +477,39 @@ void rtl8723ae_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		if (btype_ibss == true)
 			_rtl8723ae_resume_tx_beacon(hw);
 		break; }
+	case HW_VAR_FW_LPS_ACTION: {
+		bool enter_fwlps = *((bool *)val);
+		u8 rpwm_val, fw_pwrmode;
+		bool fw_current_inps;
+
+		if (enter_fwlps) {
+			rpwm_val = 0x02;	/* RF off */
+			fw_current_inps = true;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+					HW_VAR_FW_PSMODE_STATUS,
+					(u8 *)(&fw_current_inps));
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+					HW_VAR_H2C_FW_PWRMODE,
+					(u8 *)(&ppsc->fwctrl_psmode));
+
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+					HW_VAR_SET_RPWM,
+					(u8 *)(&rpwm_val));
+		} else {
+			rpwm_val = 0x0C;	/* RF on */
+			fw_pwrmode = FW_PS_ACTIVE_MODE;
+			fw_current_inps = false;
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+					(u8 *)(&rpwm_val));
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+					HW_VAR_H2C_FW_PWRMODE,
+					(u8 *)(&fw_pwrmode));
+
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+					HW_VAR_FW_PSMODE_STATUS,
+					(u8 *)(&fw_current_inps));
+		}
+		break; }
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "switch case not processed\n");
@@ -1379,7 +1415,7 @@ static void _rtl8723ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		}
 
 		for (i = 0; i < 14; i++) {
-			RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 				"RF(%d)-Ch(%d) [CCK / HT40_1S / HT40_2S] = "
 				"[0x%x / 0x%x / 0x%x]\n", rf_path, i,
 				rtlefuse->txpwrlevel_cck[rf_path][i],
@@ -1420,10 +1456,10 @@ static void _rtl8723ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 				    0xf0) >> 4);
 			}
 
-			RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 				"RF-%d pwrgroup_ht20[%d] = 0x%x\n", rf_path, i,
 				rtlefuse->pwrgroup_ht20[rf_path][i]);
-			RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 				"RF-%d pwrgroup_ht40[%d] = 0x%x\n", rf_path, i,
 				rtlefuse->pwrgroup_ht40[rf_path][i]);
 		}
@@ -1463,19 +1499,19 @@ static void _rtl8723ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 	    rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][7];
 
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-A Ht20 to HT40 Diff[%d] = 0x%x\n", i,
 			rtlefuse->txpwr_ht20diff[RF90_PATH_A][i]);
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-A Legacy to Ht40 Diff[%d] = 0x%x\n", i,
 			rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][i]);
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-B Ht20 to HT40 Diff[%d] = 0x%x\n", i,
 			rtlefuse->txpwr_ht20diff[RF90_PATH_B][i]);
 	for (i = 0; i < 14; i++)
-		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-B Legacy to HT40 Diff[%d] = 0x%x\n", i,
 			rtlefuse->txpwr_legacyhtdiff[RF90_PATH_B][i]);
 
@@ -1483,14 +1519,14 @@ static void _rtl8723ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		rtlefuse->eeprom_regulatory = (hwinfo[RF_OPTION1] & 0x7);
 	else
 		rtlefuse->eeprom_regulatory = 0;
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"eeprom_regulatory = 0x%x\n", rtlefuse->eeprom_regulatory);
 
 	if (!autoload_fail)
 		rtlefuse->eeprom_tssi[RF90_PATH_A] = hwinfo[EEPROM_TSSI_A];
 	else
 		rtlefuse->eeprom_tssi[RF90_PATH_A] = EEPROM_DEFAULT_TSSI;
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"TSSI_A = 0x%x, TSSI_B = 0x%x\n",
 		rtlefuse->eeprom_tssi[RF90_PATH_A],
 		rtlefuse->eeprom_tssi[RF90_PATH_B]);
@@ -1505,7 +1541,7 @@ static void _rtl8723ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		rtlefuse->apk_thermalmeterignore = true;
 
 	rtlefuse->thermalmeter[0] = rtlefuse->eeprom_thermalmeter;
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter);
 }
 
@@ -1713,19 +1749,7 @@ static void _rtl8723ae_hal_customized_behavior(struct ieee80211_hw *hw)
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
-	switch (rtlhal->oem_id) {
-	case RT_CID_819x_HP:
-		pcipriv->ledctl.led_opendrain = true;
-		break;
-	case RT_CID_819x_Lenovo:
-	case RT_CID_DEFAULT:
-	case RT_CID_TOSHIBA:
-	case RT_CID_CCX:
-	case RT_CID_819x_Acer:
-	case RT_CID_WHQL:
-	default:
-		break;
-	}
+	pcipriv->ledctl.led_opendrain = true;
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		 "RT Customized ID: 0x%02X\n", rtlhal->oem_id);
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/led.c b/drivers/net/wireless/rtlwifi/rtl8723ae/led.c
index 9c4e1d811187..061526fe6e2d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/led.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/led.c
@@ -54,8 +54,9 @@ void rtl8723ae_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
 	case LED_PIN_GPIO0:
 		break;
 	case LED_PIN_LED0:
+		ledcfg &= ~BIT(6);
 		rtl_write_byte(rtlpriv,
-			       REG_LEDCFG2, (ledcfg & 0xf0) | BIT(5) | BIT(6));
+			       REG_LEDCFG2, (ledcfg & 0xf0) | BIT(5));
 		break;
 	case LED_PIN_LED1:
 		rtl_write_byte(rtlpriv, REG_LEDCFG2, (ledcfg & 0x0f) | BIT(5));
@@ -84,16 +85,21 @@ void rtl8723ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
 		break;
 	case LED_PIN_LED0:
 		ledcfg &= 0xf0;
-		if (pcipriv->ledctl.led_opendrain)
+		if (pcipriv->ledctl.led_opendrain) {
+			ledcfg &= 0x90;
+			rtl_write_byte(rtlpriv, REG_LEDCFG2, (ledcfg|BIT(3)));
+			ledcfg = rtl_read_byte(rtlpriv, REG_MAC_PINMUX_CFG);
+			ledcfg &= 0xFE;
+			rtl_write_byte(rtlpriv, REG_MAC_PINMUX_CFG, ledcfg);
+		} else {
+			ledcfg &= ~BIT(6);
 			rtl_write_byte(rtlpriv, REG_LEDCFG2,
-				       (ledcfg | BIT(1) | BIT(5) | BIT(6)));
-		else
-			rtl_write_byte(rtlpriv, REG_LEDCFG2,
-				       (ledcfg | BIT(3) | BIT(5) | BIT(6)));
+				       (ledcfg | BIT(3) | BIT(5)));
+		}
 		break;
 	case LED_PIN_LED1:
-		ledcfg &= 0x0f;
-		rtl_write_byte(rtlpriv, REG_LEDCFG2, (ledcfg | BIT(3)));
+		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG1) & 0x10;
+		rtl_write_byte(rtlpriv, REG_LEDCFG1, (ledcfg | BIT(3)));
 		break;
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/sw.c b/drivers/net/wireless/rtlwifi/rtl8723ae/sw.c
index bb7cc90bafb2..e4c4cdc3eb67 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/sw.c
@@ -305,7 +305,7 @@ static struct rtl_hal_cfg rtl8723ae_hal_cfg = {
 
 	.maps[RTL_IMR_TXFOVW] = PHIMR_TXFOVW,
 	.maps[RTL_IMR_PSTIMEOUT] = PHIMR_PSTIMEOUT,
-	.maps[RTL_IMR_BcnInt] = PHIMR_BCNDMAINT0,
+	.maps[RTL_IMR_BCNINT] = PHIMR_BCNDMAINT0,
 	.maps[RTL_IMR_RXFOVW] = PHIMR_RXFOVW,
 	.maps[RTL_IMR_RDU] = PHIMR_RDU,
 	.maps[RTL_IMR_ATIMEND] = PHIMR_ATIMEND_E,
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c b/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c
index ac081297db50..c72758d8f4ed 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c
@@ -304,11 +304,8 @@ bool rtl8723ae_rx_query_desc(struct ieee80211_hw *hw,
 
 	status->is_cck = RTL8723E_RX_HAL_IS_CCK_RATE(status->rate);
 
-	rx_status->freq = hw->conf.channel->center_freq;
-	rx_status->band = hw->conf.channel->band;
-
-	hdr = (struct ieee80211_hdr *)(skb->data + status->rx_drvinfo_size
-		+ status->rx_bufshift);
+	rx_status->freq = hw->conf.chandef.chan->center_freq;
+	rx_status->band = hw->conf.chandef.chan->band;
 
 	if (status->crc)
 		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
@@ -330,6 +327,13 @@ bool rtl8723ae_rx_query_desc(struct ieee80211_hw *hw,
 	 * to decrypt it
 	 */
 	if (status->decrypted) {
+		hdr = (struct ieee80211_hdr *)(skb->data +
+		       status->rx_drvinfo_size + status->rx_bufshift);
+
+		if (!hdr) {
+			/* during testing, hdr could be NULL here */
+			return false;
+		}
 		if ((ieee80211_is_robust_mgmt_frame(hdr)) &&
 			(ieee80211_has_protected(hdr->frame_control)))
 			rx_status->flag &= ~RX_FLAG_DECRYPTED;
diff --git a/drivers/net/wireless/rtlwifi/usb.c b/drivers/net/wireless/rtlwifi/usb.c
index 5847d6d0881e..76732b0cd221 100644
--- a/drivers/net/wireless/rtlwifi/usb.c
+++ b/drivers/net/wireless/rtlwifi/usb.c
@@ -224,10 +224,9 @@ static void _usb_writeN_sync(struct rtl_priv *rtlpriv, u32 addr, void *data,
 	u8 *buffer;
 
 	wvalue = (u16)(addr & 0x0000ffff);
-	buffer = kmalloc(len, GFP_ATOMIC);
+	buffer = kmemdup(data, len, GFP_ATOMIC);
 	if (!buffer)
 		return;
-	memcpy(buffer, data, len);
 	usb_control_msg(udev, pipe, request, reqtype, wvalue,
 			index, buffer, len, 50);
 
@@ -309,6 +308,8 @@ static int _rtl_usb_init_tx(struct ieee80211_hw *hw)
 	return 0;
 }
 
+static void _rtl_rx_work(unsigned long param);
+
 static int _rtl_usb_init_rx(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -325,6 +326,12 @@ static int _rtl_usb_init_rx(struct ieee80211_hw *hw)
 	pr_info("rx_max_size %d, rx_urb_num %d, in_ep %d\n",
 		rtlusb->rx_max_size, rtlusb->rx_urb_num, rtlusb->in_ep);
 	init_usb_anchor(&rtlusb->rx_submitted);
+	init_usb_anchor(&rtlusb->rx_cleanup_urbs);
+
+	skb_queue_head_init(&rtlusb->rx_queue);
+	rtlusb->rx_work_tasklet.func = _rtl_rx_work;
+	rtlusb->rx_work_tasklet.data = (unsigned long)rtlusb;
+
 	return 0;
 }
 
@@ -406,40 +413,30 @@ static void rtl_usb_init_sw(struct ieee80211_hw *hw)
 	rtlusb->disableHWSM =  true;
 }
 
-#define __RADIO_TAP_SIZE_RSV	32
-
 static void _rtl_rx_completed(struct urb *urb);
 
-static struct sk_buff *_rtl_prep_rx_urb(struct ieee80211_hw *hw,
-					struct rtl_usb *rtlusb,
-					struct urb *urb,
-					gfp_t gfp_mask)
+static int _rtl_prep_rx_urb(struct ieee80211_hw *hw, struct rtl_usb *rtlusb,
+			      struct urb *urb, gfp_t gfp_mask)
 {
-	struct sk_buff *skb;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	void *buf;
 
-	skb = __dev_alloc_skb((rtlusb->rx_max_size + __RADIO_TAP_SIZE_RSV),
-			       gfp_mask);
-	if (!skb) {
+	buf = usb_alloc_coherent(rtlusb->udev, rtlusb->rx_max_size, gfp_mask,
+				 &urb->transfer_dma);
+	if (!buf) {
 		RT_TRACE(rtlpriv, COMP_USB, DBG_EMERG,
-			 "Failed to __dev_alloc_skb!!\n");
-		return ERR_PTR(-ENOMEM);
+			 "Failed to usb_alloc_coherent!!\n");
+		return -ENOMEM;
 	}
 
-	/* reserve some space for mac80211's radiotap */
-	skb_reserve(skb, __RADIO_TAP_SIZE_RSV);
 	usb_fill_bulk_urb(urb, rtlusb->udev,
 			  usb_rcvbulkpipe(rtlusb->udev, rtlusb->in_ep),
-			  skb->data, min(skb_tailroom(skb),
-			  (int)rtlusb->rx_max_size),
-			  _rtl_rx_completed, skb);
+			  buf, rtlusb->rx_max_size, _rtl_rx_completed, rtlusb);
+	urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
 
-	_rtl_install_trx_info(rtlusb, skb, rtlusb->in_ep);
-	return skb;
+	return 0;
 }
 
-#undef __RADIO_TAP_SIZE_RSV
-
 static void _rtl_usb_rx_process_agg(struct ieee80211_hw *hw,
 				    struct sk_buff *skb)
 {
@@ -523,22 +520,14 @@ static void _rtl_usb_rx_process_noagg(struct ieee80211_hw *hw,
 			if (unicast)
 				rtlpriv->link_info.num_rx_inperiod++;
 		}
-		if (likely(rtl_action_proc(hw, skb, false))) {
-			struct sk_buff *uskb = NULL;
-			u8 *pdata;
-
-			uskb = dev_alloc_skb(skb->len + 128);
-			if (uskb) {	/* drop packet on allocation failure */
-				memcpy(IEEE80211_SKB_RXCB(uskb), &rx_status,
-				       sizeof(rx_status));
-				pdata = (u8 *)skb_put(uskb, skb->len);
-				memcpy(pdata, skb->data, skb->len);
-				ieee80211_rx_irqsafe(hw, uskb);
-			}
-			dev_kfree_skb_any(skb);
-		} else {
+
+		/* static bcn for roaming */
+		rtl_beacon_statistic(hw, skb);
+
+		if (likely(rtl_action_proc(hw, skb, false)))
+			ieee80211_rx(hw, skb);
+		else
 			dev_kfree_skb_any(skb);
-		}
 	}
 }
 
@@ -555,15 +544,70 @@ static void _rtl_rx_pre_process(struct ieee80211_hw *hw, struct sk_buff *skb)
 	while (!skb_queue_empty(&rx_queue)) {
 		_skb = skb_dequeue(&rx_queue);
 		_rtl_usb_rx_process_agg(hw, _skb);
-		ieee80211_rx_irqsafe(hw, _skb);
+		ieee80211_rx(hw, _skb);
+	}
+}
+
+#define __RX_SKB_MAX_QUEUED	32
+
+static void _rtl_rx_work(unsigned long param)
+{
+	struct rtl_usb *rtlusb = (struct rtl_usb *)param;
+	struct ieee80211_hw *hw = usb_get_intfdata(rtlusb->intf);
+	struct sk_buff *skb;
+
+	while ((skb = skb_dequeue(&rtlusb->rx_queue))) {
+		if (unlikely(IS_USB_STOP(rtlusb))) {
+			dev_kfree_skb_any(skb);
+			continue;
+		}
+
+		if (likely(!rtlusb->usb_rx_segregate_hdl)) {
+			_rtl_usb_rx_process_noagg(hw, skb);
+		} else {
+			/* TO DO */
+			_rtl_rx_pre_process(hw, skb);
+			pr_err("rx agg not supported\n");
+		}
+	}
+}
+
+static unsigned int _rtl_rx_get_padding(struct ieee80211_hdr *hdr,
+					unsigned int len)
+{
+	unsigned int padding = 0;
+
+	/* make function no-op when possible */
+	if (NET_IP_ALIGN == 0 || len < sizeof(*hdr))
+		return 0;
+
+	/* alignment calculation as in lbtf_rx() / carl9170_rx_copy_data() */
+	/* TODO: deduplicate common code, define helper function instead? */
+
+	if (ieee80211_is_data_qos(hdr->frame_control)) {
+		u8 *qc = ieee80211_get_qos_ctl(hdr);
+
+		padding ^= NET_IP_ALIGN;
+
+		/* Input might be invalid, avoid accessing memory outside
+		 * the buffer.
+		 */
+		if ((unsigned long)qc - (unsigned long)hdr < len &&
+		    *qc & IEEE80211_QOS_CTL_A_MSDU_PRESENT)
+			padding ^= NET_IP_ALIGN;
 	}
+
+	if (ieee80211_has_a4(hdr->frame_control))
+		padding ^= NET_IP_ALIGN;
+
+	return padding;
 }
 
+#define __RADIO_TAP_SIZE_RSV	32
+
 static void _rtl_rx_completed(struct urb *_urb)
 {
-	struct sk_buff *skb = (struct sk_buff *)_urb->context;
-	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-	struct rtl_usb *rtlusb = (struct rtl_usb *)info->rate_driver_data[0];
+	struct rtl_usb *rtlusb = (struct rtl_usb *)_urb->context;
 	struct ieee80211_hw *hw = usb_get_intfdata(rtlusb->intf);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	int err = 0;
@@ -572,28 +616,50 @@ static void _rtl_rx_completed(struct urb *_urb)
 		goto free;
 
 	if (likely(0 == _urb->status)) {
-		/* If this code were moved to work queue, would CPU
-		 * utilization be improved?  NOTE: We shall allocate another skb
-		 * and reuse the original one.
-		 */
-		skb_put(skb, _urb->actual_length);
+		unsigned int padding;
+		struct sk_buff *skb;
+		unsigned int qlen;
+		unsigned int size = _urb->actual_length;
+		struct ieee80211_hdr *hdr;
 
-		if (likely(!rtlusb->usb_rx_segregate_hdl)) {
-			struct sk_buff *_skb;
-			_rtl_usb_rx_process_noagg(hw, skb);
-			_skb = _rtl_prep_rx_urb(hw, rtlusb, _urb, GFP_ATOMIC);
-			if (IS_ERR(_skb)) {
-				err = PTR_ERR(_skb);
-				RT_TRACE(rtlpriv, COMP_USB, DBG_EMERG,
-					 "Can't allocate skb for bulk IN!\n");
-				return;
-			}
-			skb = _skb;
-		} else{
-			/* TO DO */
-			_rtl_rx_pre_process(hw, skb);
-			pr_err("rx agg not supported\n");
+		if (size < RTL_RX_DESC_SIZE + sizeof(struct ieee80211_hdr)) {
+			RT_TRACE(rtlpriv, COMP_USB, DBG_EMERG,
+				 "Too short packet from bulk IN! (len: %d)\n",
+				 size);
+			goto resubmit;
+		}
+
+		qlen = skb_queue_len(&rtlusb->rx_queue);
+		if (qlen >= __RX_SKB_MAX_QUEUED) {
+			RT_TRACE(rtlpriv, COMP_USB, DBG_EMERG,
+				 "Pending RX skbuff queue full! (qlen: %d)\n",
+				 qlen);
+			goto resubmit;
 		}
+
+		hdr = (void *)(_urb->transfer_buffer + RTL_RX_DESC_SIZE);
+		padding = _rtl_rx_get_padding(hdr, size - RTL_RX_DESC_SIZE);
+
+		skb = dev_alloc_skb(size + __RADIO_TAP_SIZE_RSV + padding);
+		if (!skb) {
+			RT_TRACE(rtlpriv, COMP_USB, DBG_EMERG,
+				 "Can't allocate skb for bulk IN!\n");
+			goto resubmit;
+		}
+
+		_rtl_install_trx_info(rtlusb, skb, rtlusb->in_ep);
+
+		/* Make sure the payload data is 4 byte aligned. */
+		skb_reserve(skb, padding);
+
+		/* reserve some space for mac80211's radiotap */
+		skb_reserve(skb, __RADIO_TAP_SIZE_RSV);
+
+		memcpy(skb_put(skb, size), _urb->transfer_buffer, size);
+
+		skb_queue_tail(&rtlusb->rx_queue, skb);
+		tasklet_schedule(&rtlusb->rx_work_tasklet);
+
 		goto resubmit;
 	}
 
@@ -609,9 +675,6 @@ static void _rtl_rx_completed(struct urb *_urb)
 	}
 
 resubmit:
-	skb_reset_tail_pointer(skb);
-	skb_trim(skb, 0);
-
 	usb_anchor_urb(_urb, &rtlusb->rx_submitted);
 	err = usb_submit_urb(_urb, GFP_ATOMIC);
 	if (unlikely(err)) {
@@ -621,13 +684,34 @@ resubmit:
 	return;
 
 free:
-	dev_kfree_skb_irq(skb);
+	/* On some architectures, usb_free_coherent must not be called from
+	 * hardirq context. Queue urb to cleanup list.
+	 */
+	usb_anchor_urb(_urb, &rtlusb->rx_cleanup_urbs);
+}
+
+#undef __RADIO_TAP_SIZE_RSV
+
+static void _rtl_usb_cleanup_rx(struct ieee80211_hw *hw)
+{
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+	struct urb *urb;
+
+	usb_kill_anchored_urbs(&rtlusb->rx_submitted);
+
+	tasklet_kill(&rtlusb->rx_work_tasklet);
+	skb_queue_purge(&rtlusb->rx_queue);
+
+	while ((urb = usb_get_from_anchor(&rtlusb->rx_cleanup_urbs))) {
+		usb_free_coherent(urb->dev, urb->transfer_buffer_length,
+				urb->transfer_buffer, urb->transfer_dma);
+		usb_free_urb(urb);
+	}
 }
 
 static int _rtl_usb_receive(struct ieee80211_hw *hw)
 {
 	struct urb *urb;
-	struct sk_buff *skb;
 	int err;
 	int i;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -646,11 +730,10 @@ static int _rtl_usb_receive(struct ieee80211_hw *hw)
 			goto err_out;
 		}
 
-		skb = _rtl_prep_rx_urb(hw, rtlusb, urb, GFP_KERNEL);
-		if (IS_ERR(skb)) {
+		err = _rtl_prep_rx_urb(hw, rtlusb, urb, GFP_KERNEL);
+		if (err < 0) {
 			RT_TRACE(rtlpriv, COMP_USB, DBG_EMERG,
 				 "Failed to prep_rx_urb!!\n");
-			err = PTR_ERR(skb);
 			usb_free_urb(urb);
 			goto err_out;
 		}
@@ -665,6 +748,7 @@ static int _rtl_usb_receive(struct ieee80211_hw *hw)
 
 err_out:
 	usb_kill_anchored_urbs(&rtlusb->rx_submitted);
+	_rtl_usb_cleanup_rx(hw);
 	return err;
 }
 
@@ -706,7 +790,7 @@ static void rtl_usb_cleanup(struct ieee80211_hw *hw)
 	SET_USB_STOP(rtlusb);
 
 	/* clean up rx stuff. */
-	usb_kill_anchored_urbs(&rtlusb->rx_submitted);
+	_rtl_usb_cleanup_rx(hw);
 
 	/* clean up tx stuff */
 	for (i = 0; i < RTL_USB_MAX_EP_NUM; i++) {
diff --git a/drivers/net/wireless/rtlwifi/usb.h b/drivers/net/wireless/rtlwifi/usb.h
index fb986f98d1df..685273ca9561 100644
--- a/drivers/net/wireless/rtlwifi/usb.h
+++ b/drivers/net/wireless/rtlwifi/usb.h
@@ -136,11 +136,14 @@ struct rtl_usb {
 	void (*usb_tx_cleanup)(struct ieee80211_hw *, struct sk_buff *);
 
 	/* Rx */
-	u8 in_ep_nums ;
+	u8 in_ep_nums;
 	u32 in_ep;		/* Bulk IN endpoint number */
 	u32 rx_max_size;	/* Bulk IN max buffer size */
 	u32 rx_urb_num;		/* How many Bulk INs are submitted to host. */
 	struct usb_anchor	rx_submitted;
+	struct usb_anchor	rx_cleanup_urbs;
+	struct tasklet_struct   rx_work_tasklet;
+	struct sk_buff_head	rx_queue;
 	void (*usb_rx_segregate_hdl)(struct ieee80211_hw *, struct sk_buff *,
 				     struct sk_buff_head *);
 	void (*usb_rx_hdl)(struct ieee80211_hw *, struct sk_buff *);
diff --git a/drivers/net/wireless/rtlwifi/wifi.h b/drivers/net/wireless/rtlwifi/wifi.h
index f13258a8d995..44328baa6389 100644
--- a/drivers/net/wireless/rtlwifi/wifi.h
+++ b/drivers/net/wireless/rtlwifi/wifi.h
@@ -99,11 +99,36 @@
 #define	CHANNEL_GROUP_MAX_5G		9
 #define CHANNEL_MAX_NUMBER_2G		14
 #define AVG_THERMAL_NUM			8
+#define AVG_THERMAL_NUM_88E		4
 #define MAX_TID_COUNT			9
 
 /* for early mode */
 #define FCS_LEN				4
 #define EM_HDR_LEN			8
+
+#define MAX_TX_COUNT			4
+#define	MAX_RF_PATH			4
+#define	MAX_CHNL_GROUP_24G		6
+#define	MAX_CHNL_GROUP_5G		14
+
+struct txpower_info_2g {
+	u8 index_cck_base[MAX_RF_PATH][MAX_CHNL_GROUP_24G];
+	u8 index_bw40_base[MAX_RF_PATH][MAX_CHNL_GROUP_24G];
+	/*If only one tx, only BW20 and OFDM are used.*/
+	u8 cck_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 ofdm_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw20_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw40_diff[MAX_RF_PATH][MAX_TX_COUNT];
+};
+
+struct txpower_info_5g {
+	u8 index_bw40_base[MAX_RF_PATH][MAX_CHNL_GROUP_5G];
+	/*If only one tx, only BW20, OFDM, BW80 and BW160 are used.*/
+	u8 ofdm_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw20_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw40_diff[MAX_RF_PATH][MAX_TX_COUNT];
+};
+
 enum intf_type {
 	INTF_PCI = 0,
 	INTF_USB = 1,
@@ -137,6 +162,7 @@ enum hardware_type {
 	HARDWARE_TYPE_RTL8192DU,
 	HARDWARE_TYPE_RTL8723AE,
 	HARDWARE_TYPE_RTL8723U,
+	HARDWARE_TYPE_RTL8188EE,
 
 	/* keep it last */
 	HARDWARE_TYPE_NUM
@@ -263,7 +289,7 @@ enum hw_variables {
 	HW_VAR_RATR_0,
 	HW_VAR_RRSR,
 	HW_VAR_CPU_RST,
-	HW_VAR_CECHK_BSSID,
+	HW_VAR_CHECK_BSSID,
 	HW_VAR_LBK_MODE,
 	HW_VAR_AES_11N_FIX,
 	HW_VAR_USB_RX_AGGR,
@@ -278,7 +304,10 @@ enum hw_variables {
 	HW_VAR_SET_RPWM,
 	HW_VAR_H2C_FW_PWRMODE,
 	HW_VAR_H2C_FW_JOINBSSRPT,
+	HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
 	HW_VAR_FW_PSMODE_STATUS,
+	HW_VAR_RESUME_CLK_ON,
+	HW_VAR_FW_LPS_ACTION,
 	HW_VAR_1X1_RECV_COMBINE,
 	HW_VAR_STOP_SEND_BEACON,
 	HW_VAR_TSF_TIMER,
@@ -305,6 +334,7 @@ enum hw_variables {
 	HW_VAR_INT_AC,
 	HW_VAR_RF_TIMING,
 
+	HAL_DEF_WOWLAN,
 	HW_VAR_MRC,
 
 	HW_VAR_MGT_FILTER,
@@ -461,6 +491,7 @@ enum rtl_var_map {
 	EFUSE_MAX_SECTION_MAP,
 	EFUSE_REAL_CONTENT_SIZE,
 	EFUSE_OOB_PROTECT_BYTES_LEN,
+	EFUSE_ACCESS,
 
 	/*CAM map */
 	RWCAM,
@@ -493,7 +524,7 @@ enum rtl_var_map {
 	RTL_IMR_TIMEOUT1,	/*Timeout interrupt 1 */
 	RTL_IMR_TXFOVW,		/*Transmit FIFO Overflow */
 	RTL_IMR_PSTIMEOUT,	/*Power save time out interrupt */
-	RTL_IMR_BcnInt,		/*Beacon DMA Interrupt 0 */
+	RTL_IMR_BCNINT,		/*Beacon DMA Interrupt 0 */
 	RTL_IMR_RXFOVW,		/*Receive FIFO Overflow */
 	RTL_IMR_RDU,		/*Receive Descriptor Unavailable */
 	RTL_IMR_ATIMEND,	/*For 92C,ATIM Window End Interrupt */
@@ -508,7 +539,7 @@ enum rtl_var_map {
 	RTL_IMR_VIDOK,		/*AC_VI DMA OK Interrupt */
 	RTL_IMR_VODOK,		/*AC_VO DMA Interrupt */
 	RTL_IMR_ROK,		/*Receive DMA OK Interrupt */
-	RTL_IBSS_INT_MASKS,	/*(RTL_IMR_BcnInt | RTL_IMR_TBDOK |
+	RTL_IBSS_INT_MASKS,	/*(RTL_IMR_BCNINT | RTL_IMR_TBDOK |
 				 * RTL_IMR_TBDER) */
 	RTL_IMR_C2HCMD,		/*fw interrupt*/
 
@@ -742,6 +773,11 @@ struct false_alarm_statistics {
 	u32 cnt_ofdm_fail;
 	u32 cnt_cck_fail;
 	u32 cnt_all;
+	u32 cnt_ofdm_cca;
+	u32 cnt_cck_cca;
+	u32 cnt_cca_all;
+	u32 cnt_bw_usc;
+	u32 cnt_bw_lsc;
 };
 
 struct init_gain {
@@ -826,8 +862,67 @@ struct rtl_rfkill {
 	bool rfkill_state;	/*0 is off, 1 is on */
 };
 
+/*for P2P PS**/
+#define	P2P_MAX_NOA_NUM		2
+
+enum p2p_role {
+	P2P_ROLE_DISABLE = 0,
+	P2P_ROLE_DEVICE = 1,
+	P2P_ROLE_CLIENT = 2,
+	P2P_ROLE_GO = 3
+};
+
+enum p2p_ps_state {
+	P2P_PS_DISABLE = 0,
+	P2P_PS_ENABLE = 1,
+	P2P_PS_SCAN = 2,
+	P2P_PS_SCAN_DONE = 3,
+	P2P_PS_ALLSTASLEEP = 4, /* for P2P GO */
+};
+
+enum p2p_ps_mode {
+	P2P_PS_NONE = 0,
+	P2P_PS_CTWINDOW = 1,
+	P2P_PS_NOA	 = 2,
+	P2P_PS_MIX = 3, /* CTWindow and NoA */
+};
+
+struct rtl_p2p_ps_info {
+	enum p2p_ps_mode p2p_ps_mode; /* indicate p2p ps mode */
+	enum p2p_ps_state p2p_ps_state; /*  indicate p2p ps state */
+	u8 noa_index; /*  Identifies instance of Notice of Absence timing. */
+	/*  Client traffic window. A period of time in TU after TBTT. */
+	u8 ctwindow;
+	u8 opp_ps; /*  opportunistic power save. */
+	u8 noa_num; /*  number of NoA descriptor in P2P IE. */
+	/*  Count for owner, Type of client. */
+	u8 noa_count_type[P2P_MAX_NOA_NUM];
+	/*  Max duration for owner, preferred or min acceptable duration
+	 * for client.
+	 */
+	u32 noa_duration[P2P_MAX_NOA_NUM];
+	/*  Length of interval for owner, preferred or max acceptable intervali
+	 * of client.
+	 */
+	u32 noa_interval[P2P_MAX_NOA_NUM];
+	/*  schedule in terms of the lower 4 bytes of the TSF timer. */
+	u32 noa_start_time[P2P_MAX_NOA_NUM];
+};
+
+struct p2p_ps_offload_t {
+	u8 offload_en:1;
+	u8 role:1; /* 1: Owner, 0: Client */
+	u8 ctwindow_en:1;
+	u8 noa0_en:1;
+	u8 noa1_en:1;
+	u8 allstasleep:1;
+	u8 discovery:1;
+	u8 reserved:1;
+};
+
 #define IQK_MATRIX_REG_NUM	8
 #define IQK_MATRIX_SETTINGS_NUM	(1 + 24 + 21)
+
 struct iqk_matrix_regs {
 	bool iqk_done;
 	long value[1][IQK_MATRIX_REG_NUM];
@@ -889,7 +984,7 @@ struct rtl_phy {
 
 	/* Dual mac */
 	bool need_iqk;
-	struct iqk_matrix_regs iqk_matrix_regsetting[IQK_MATRIX_SETTINGS_NUM];
+	struct iqk_matrix_regs iqk_matrix[IQK_MATRIX_SETTINGS_NUM];
 
 	bool rfpi_enable;
 
@@ -902,6 +997,8 @@ struct rtl_phy {
 	/* the current Tx power level */
 	u8 cur_cck_txpwridx;
 	u8 cur_ofdm24g_txpwridx;
+	u8 cur_bw20_txpwridx;
+	u8 cur_bw40_txpwridx;
 
 	u32 rfreg_chnlval[2];
 	bool apk_done;
@@ -940,20 +1037,21 @@ struct rtl_ht_agg {
 	u8 rx_agg_state;
 };
 
+struct rssi_sta {
+	long undec_sm_pwdb;
+};
+
 struct rtl_tid_data {
 	u16 seq_number;
 	struct rtl_ht_agg agg;
 };
 
-struct rssi_sta {
-	long undec_sm_pwdb;
-};
-
 struct rtl_sta_info {
 	struct list_head list;
 	u8 ratr_index;
 	u8 wireless_mode;
 	u8 mimo_ps;
+	u8 mac_addr[ETH_ALEN];
 	struct rtl_tid_data tids[MAX_TID_COUNT];
 
 	/* just used for ap adhoc or mesh*/
@@ -1005,6 +1103,8 @@ struct rtl_mac {
 	int n_bitrates;
 
 	bool offchan_delay;
+	u8 p2p;	/*using p2p role*/
+	bool p2p_in_use;
 
 	/*filters */
 	u32 rx_conf;
@@ -1014,11 +1114,11 @@ struct rtl_mac {
 
 	bool act_scanning;
 	u8 cnt_after_linked;
+	bool skip_scan;
 
 	/* early mode */
 	/* skb wait queue */
 	struct sk_buff_head skb_waitq[MAX_TID_COUNT];
-	u8 earlymode_threshold;
 
 	/*RDG*/
 	bool rdg_en;
@@ -1042,6 +1142,7 @@ struct rtl_mac {
 	u8 retry_short;
 	u8 retry_long;
 	u16 assoc_id;
+	bool hiddenssid;
 
 	/*IBSS*/
 	int beacon_interval;
@@ -1111,10 +1212,13 @@ struct bt_coexist_8723 {
 
 struct rtl_hal {
 	struct ieee80211_hw *hw;
-	struct bt_coexist_8723 hal_coex_8723;
+	bool driver_is_goingto_unload;
 	bool up_first_time;
+	bool first_init;
 	bool being_init_adapter;
 	bool bbrf_ready;
+	bool mac_func_enable;
+	struct bt_coexist_8723 hal_coex_8723;
 
 	enum intf_type interface;
 	u16 hw_type;		/*92c or 92d or 92s and so on */
@@ -1122,6 +1226,7 @@ struct rtl_hal {
 	u8 oem_id;
 	u32 version;		/*version of chip */
 	u8 state;		/*stop 0, start 1 */
+	u8 board_type;
 
 	/*firmware */
 	u32 fwsize;
@@ -1141,6 +1246,10 @@ struct rtl_hal {
 	bool set_fwcmd_inprogress;
 	u8 current_fwcmd_io;
 
+	struct p2p_ps_offload_t p2p_ps_offload;
+	bool fw_clk_change_in_progress;
+	bool allow_sw_to_change_hwclc;
+	u8 fw_ps_state;
 	/**/
 	bool driver_going2unload;
 
@@ -1157,6 +1266,7 @@ struct rtl_hal {
 	/* just for DualMac S3S4 */
 	u8 macphyctl_reg;
 	bool earlymode_enable;
+	u8 max_earlymode_num;
 	/* Dual mac*/
 	bool during_mac0init_radiob;
 	bool during_mac1init_radioa;
@@ -1193,6 +1303,29 @@ struct rtl_security {
 	u8 *pairwise_key;
 };
 
+#define ASSOCIATE_ENTRY_NUM	33
+
+struct fast_ant_training {
+	u8	bssid[6];
+	u8	antsel_rx_keep_0;
+	u8	antsel_rx_keep_1;
+	u8	antsel_rx_keep_2;
+	u32	ant_sum[7];
+	u32	ant_cnt[7];
+	u32	ant_ave[7];
+	u8	fat_state;
+	u32	train_idx;
+	u8	antsel_a[ASSOCIATE_ENTRY_NUM];
+	u8	antsel_b[ASSOCIATE_ENTRY_NUM];
+	u8	antsel_c[ASSOCIATE_ENTRY_NUM];
+	u32	main_ant_sum[ASSOCIATE_ENTRY_NUM];
+	u32	aux_ant_sum[ASSOCIATE_ENTRY_NUM];
+	u32	main_ant_cnt[ASSOCIATE_ENTRY_NUM];
+	u32	aux_ant_cnt[ASSOCIATE_ENTRY_NUM];
+	u8	rx_idle_ant;
+	bool	becomelinked;
+};
+
 struct rtl_dm {
 	/*PHY status for Dynamic Management */
 	long entry_min_undec_sm_pwdb;
@@ -1229,9 +1362,24 @@ struct rtl_dm {
 	bool disable_tx_int;
 	char ofdm_index[2];
 	char cck_index;
+	char delta_power_index;
+	char delta_power_index_last;
+	char power_index_offset;
+
+	/*88e tx power tracking*/
+	u8	swing_idx_ofdm[2];
+	u8	swing_idx_ofdm_cur;
+	u8	swing_idx_ofdm_base;
+	bool	swing_flag_ofdm;
+	u8	swing_idx_cck;
+	u8	swing_idx_cck_cur;
+	u8	swing_idx_cck_base;
+	bool	swing_flag_cck;
 
 	/* DMSP */
 	bool supp_phymode_switch;
+
+	struct fast_ant_training fat_table;
 };
 
 #define	EFUSE_MAX_LOGICAL_SIZE			256
@@ -1264,6 +1412,9 @@ struct rtl_efuse {
 	u8 external_pa;
 
 	u8 dev_addr[6];
+	u8 wowlan_enable;
+	u8 antenna_div_cfg;
+	u8 antenna_div_type;
 
 	bool txpwr_fromeprom;
 	u8 eeprom_crystalcap;
@@ -1319,14 +1470,12 @@ struct rtl_ps_ctl {
 	bool rfchange_inprogress;
 	bool swrf_processing;
 	bool hwradiooff;
-
 	/*
 	 * just for PCIE ASPM
 	 * If it supports ASPM, Offset[560h] = 0x40,
 	 * otherwise Offset[560h] = 0x00.
 	 * */
 	bool support_aspm;
-
 	bool support_backdoor;
 
 	/*for LPS */
@@ -1341,6 +1490,7 @@ struct rtl_ps_ctl {
 	bool fw_current_inpsmode;
 	u8 reg_max_lps_awakeintvl;
 	bool report_linked;
+	bool low_power_enable;/*for 32k*/
 
 	/*for IPS */
 	bool inactiveps;
@@ -1373,6 +1523,11 @@ struct rtl_ps_ctl {
 	unsigned long last_beacon;
 	unsigned long last_action;
 	unsigned long last_slept;
+
+	/*For P2P PS */
+	struct rtl_p2p_ps_info p2p_ps_info;
+	u8 pwr_mode;
+	u8 smart_ps;
 };
 
 struct rtl_stats {
@@ -1381,7 +1536,7 @@ struct rtl_stats {
 	s8 rssi;
 	u8 signal;
 	u8 noise;
-	u16 rate;		/*in 100 kbps */
+	u8 rate;		/* hw desc rate */
 	u8 received_channel;
 	u8 control;
 	u8 mask;
@@ -1423,8 +1578,16 @@ struct rtl_stats {
 	bool packet_toself;
 	bool packet_beacon;	/*for rssi */
 	char cck_adc_pwdb[4];	/*for rx path selection */
+
+	u8 packet_report_type;
+
+	u32 macid;
+	u8 wake_match;
+	u32 bt_rx_rssi_percentage;
+	u32 macid_valid_entry[2];
 };
 
+
 struct rt_link_detect {
 	/* count for roaming */
 	u32 bcn_rx_inperiod;
@@ -1477,7 +1640,8 @@ struct rtl_tcb_desc {
 	/* early mode */
 	u8 empkt_num;
 	/* The max value by HW */
-	u32 empkt_len[5];
+	u32 empkt_len[10];
+	bool btx_enable_sw_calc_duration;
 };
 
 struct rtl_hal_ops {
@@ -1553,7 +1717,7 @@ struct rtl_hal_ops {
 	void (*allow_all_destaddr)(struct ieee80211_hw *hw,
 		bool allow_all_da, bool write_into_reg);
 	void (*linked_set_reg) (struct ieee80211_hw *hw);
-	void (*check_switch_to_dmdp) (struct ieee80211_hw *hw);
+	void (*chk_switch_dmdp) (struct ieee80211_hw *hw);
 	void (*dualmac_easy_concurrent) (struct ieee80211_hw *hw);
 	void (*dualmac_switch_to_dmdp) (struct ieee80211_hw *hw);
 	bool (*phy_rf6052_config) (struct ieee80211_hw *hw);
@@ -1662,6 +1826,8 @@ struct rtl_locks {
 	/*spin lock */
 	spinlock_t ips_lock;
 	spinlock_t irq_th_lock;
+	spinlock_t irq_pci_lock;
+	spinlock_t tx_lock;
 	spinlock_t h2c_lock;
 	spinlock_t rf_ps_lock;
 	spinlock_t rf_lock;
@@ -1670,6 +1836,9 @@ struct rtl_locks {
 	spinlock_t entry_list_lock;
 	spinlock_t usb_lock;
 
+	/*FW clock change */
+	spinlock_t fw_ps_lock;
+
 	/*Dual mac*/
 	spinlock_t cck_and_rw_pagea_lock;
 
@@ -1683,7 +1852,8 @@ struct rtl_works {
 	/*timer */
 	struct timer_list watchdog_timer;
 	struct timer_list dualmac_easyconcurrent_retrytimer;
-
+	struct timer_list fw_clockoff_timer;
+	struct timer_list fast_antenna_training_timer;
 	/*task */
 	struct tasklet_struct irq_tasklet;
 	struct tasklet_struct irq_prepare_bcn_tasklet;
@@ -1696,8 +1866,9 @@ struct rtl_works {
 	/* For SW LPS */
 	struct delayed_work ps_work;
 	struct delayed_work ps_rfon_wq;
+	struct delayed_work fwevt_wq;
 
-	struct work_struct lps_leave_work;
+	struct work_struct lps_change_work;
 };
 
 struct rtl_debug {
@@ -1767,10 +1938,12 @@ struct dig_t {
 	char back_val;
 	char back_range_max;
 	char back_range_min;
-	u8 rx_gain_range_max;
-	u8 rx_gain_range_min;
+	u8 rx_gain_max;
+	u8 rx_gain_min;
 	u8 min_undec_pwdb_for_dm;
 	u8 rssi_val_min;
+	u8 pre_cck_cca_thres;
+	u8 cur_cck_cca_thres;
 	u8 pre_cck_pd_state;
 	u8 cur_cck_pd_state;
 	u8 pre_cck_fa_state;
@@ -1792,6 +1965,13 @@ struct dig_t {
 	u8 backoff_enable_flag;
 	char backoffval_range_max;
 	char backoffval_range_min;
+	u8 dig_min_0;
+	u8 dig_min_1;
+	bool media_connect_0;
+	bool media_connect_1;
+
+	u32 antdiv_rssi_max;
+	u32 rssi_max;
 };
 
 struct rtl_global_var {
@@ -1802,6 +1982,7 @@ struct rtl_global_var {
 };
 
 struct rtl_priv {
+	struct ieee80211_hw *hw;
 	struct completion firmware_loading_complete;
 	struct list_head list;
 	struct rtl_priv *buddy_priv;
@@ -1866,6 +2047,7 @@ struct rtl_priv {
 			bool bt_operation_on;
 		};
 	};
+	bool enter_ps;	/* true when entering PS */
 
 	/*This must be the last item so
 	   that it points to the data allocated
@@ -2127,9 +2309,7 @@ value to host byte ordering.*/
 #define WLAN_FC_GET_TYPE(fc)	(le16_to_cpu(fc) & IEEE80211_FCTL_FTYPE)
 #define WLAN_FC_GET_STYPE(fc)	(le16_to_cpu(fc) & IEEE80211_FCTL_STYPE)
 #define WLAN_FC_MORE_DATA(fc)	(le16_to_cpu(fc) & IEEE80211_FCTL_MOREDATA)
-#define SEQ_TO_SN(seq)		(((seq) & IEEE80211_SCTL_SEQ) >> 4)
-#define SN_TO_SEQ(ssn)		(((ssn) << 4) & IEEE80211_SCTL_SEQ)
-#define MAX_SN			((IEEE80211_SCTL_SEQ) >> 4)
+#define rtl_dm(rtlpriv)		(&((rtlpriv)->dm))
 
 #define	RT_RF_OFF_LEVL_ASPM		BIT(0)	/*PCI ASPM */
 #define	RT_RF_OFF_LEVL_CLK_REQ		BIT(1)	/*PCI clock request */
diff --git a/drivers/net/wireless/ti/wl1251/main.c b/drivers/net/wireless/ti/wl1251/main.c
index bbbf68cf50a7..3291ffa95273 100644
--- a/drivers/net/wireless/ti/wl1251/main.c
+++ b/drivers/net/wireless/ti/wl1251/main.c
@@ -572,7 +572,8 @@ static int wl1251_op_config(struct ieee80211_hw *hw, u32 changed)
 	struct ieee80211_conf *conf = &hw->conf;
 	int channel, ret = 0;
 
-	channel = ieee80211_frequency_to_channel(conf->channel->center_freq);
+	channel = ieee80211_frequency_to_channel(
+			conf->chandef.chan->center_freq);
 
 	wl1251_debug(DEBUG_MAC80211, "mac80211 config ch %d psm %s power %d",
 		     channel,
@@ -1223,7 +1224,7 @@ static int wl1251_op_get_survey(struct ieee80211_hw *hw, int idx,
 	if (idx != 0)
 		return -ENOENT;
  
-	survey->channel = conf->channel;
+	survey->channel = conf->chandef.chan;
 	survey->filled = SURVEY_INFO_NOISE_DBM;
 	survey->noise = wl->noise;
  
diff --git a/drivers/net/wireless/ti/wl1251/sdio.c b/drivers/net/wireless/ti/wl1251/sdio.c
index e57ee48edff6..e2b3d9c541e8 100644
--- a/drivers/net/wireless/ti/wl1251/sdio.c
+++ b/drivers/net/wireless/ti/wl1251/sdio.c
@@ -186,8 +186,10 @@ static int wl1251_sdio_set_power(struct wl1251 *wl, bool enable)
 			wl->set_power(true);
 
 		ret = pm_runtime_get_sync(&func->dev);
-		if (ret < 0)
+		if (ret < 0) {
+			pm_runtime_put_sync(&func->dev);
 			goto out;
+		}
 
 		sdio_claim_host(func);
 		sdio_enable_func(func);
diff --git a/drivers/net/wireless/ti/wl1251/spi.c b/drivers/net/wireless/ti/wl1251/spi.c
index 3b266d3231a3..4c67c2f9ea71 100644
--- a/drivers/net/wireless/ti/wl1251/spi.c
+++ b/drivers/net/wireless/ti/wl1251/spi.c
@@ -257,7 +257,7 @@ static int wl1251_spi_probe(struct spi_device *spi)
 	wl = hw->priv;
 
 	SET_IEEE80211_DEV(hw, &spi->dev);
-	dev_set_drvdata(&spi->dev, wl);
+	spi_set_drvdata(spi, wl);
 	wl->if_priv = spi;
 	wl->if_ops = &wl1251_spi_ops;
 
@@ -311,7 +311,7 @@ static int wl1251_spi_probe(struct spi_device *spi)
 
 static int wl1251_spi_remove(struct spi_device *spi)
 {
-	struct wl1251 *wl = dev_get_drvdata(&spi->dev);
+	struct wl1251 *wl = spi_get_drvdata(spi);
 
 	free_irq(wl->irq, wl);
 	wl1251_free_hw(wl);
diff --git a/drivers/net/wireless/ti/wl12xx/cmd.c b/drivers/net/wireless/ti/wl12xx/cmd.c
index 7dc9f965037d..7485dbae8c4b 100644
--- a/drivers/net/wireless/ti/wl12xx/cmd.c
+++ b/drivers/net/wireless/ti/wl12xx/cmd.c
@@ -301,7 +301,7 @@ int wl12xx_cmd_channel_switch(struct wl1271 *wl,
 	}
 
 	cmd->role_id = wlvif->role_id;
-	cmd->channel = ch_switch->channel->hw_value;
+	cmd->channel = ch_switch->chandef.chan->hw_value;
 	cmd->switch_time = ch_switch->count;
 	cmd->stop_tx = ch_switch->block_tx;
 
diff --git a/drivers/net/wireless/ti/wl12xx/main.c b/drivers/net/wireless/ti/wl12xx/main.c
index 09694e39bb14..1c627da85083 100644
--- a/drivers/net/wireless/ti/wl12xx/main.c
+++ b/drivers/net/wireless/ti/wl12xx/main.c
@@ -723,6 +723,7 @@ static int wl12xx_identify_chip(struct wl1271 *wl)
 	wl->sched_scan_templ_id_2_4 = CMD_TEMPL_CFG_PROBE_REQ_2_4;
 	wl->sched_scan_templ_id_5 = CMD_TEMPL_CFG_PROBE_REQ_5;
 	wl->max_channels_5 = WL12XX_MAX_CHANNELS_5GHZ;
+	wl->ba_rx_session_count_max = WL12XX_RX_BA_MAX_SESSIONS;
 out:
 	return ret;
 }
diff --git a/drivers/net/wireless/ti/wl12xx/wl12xx.h b/drivers/net/wireless/ti/wl12xx/wl12xx.h
index d4552857480c..222d03540200 100644
--- a/drivers/net/wireless/ti/wl12xx/wl12xx.h
+++ b/drivers/net/wireless/ti/wl12xx/wl12xx.h
@@ -63,6 +63,8 @@
 
 #define WL12XX_NUM_MAC_ADDRESSES 2
 
+#define WL12XX_RX_BA_MAX_SESSIONS 3
+
 struct wl127x_rx_mem_pool_addr {
 	u32 addr;
 	u32 addr_extra;
diff --git a/drivers/net/wireless/ti/wl18xx/cmd.c b/drivers/net/wireless/ti/wl18xx/cmd.c
index 1d1f6cc7a50a..7649c75cd68d 100644
--- a/drivers/net/wireless/ti/wl18xx/cmd.c
+++ b/drivers/net/wireless/ti/wl18xx/cmd.c
@@ -42,11 +42,11 @@ int wl18xx_cmd_channel_switch(struct wl1271 *wl,
 	}
 
 	cmd->role_id = wlvif->role_id;
-	cmd->channel = ch_switch->channel->hw_value;
+	cmd->channel = ch_switch->chandef.chan->hw_value;
 	cmd->switch_time = ch_switch->count;
 	cmd->stop_tx = ch_switch->block_tx;
 
-	switch (ch_switch->channel->band) {
+	switch (ch_switch->chandef.chan->band) {
 	case IEEE80211_BAND_2GHZ:
 		cmd->band = WLCORE_BAND_2_4GHZ;
 		break;
@@ -55,7 +55,7 @@ int wl18xx_cmd_channel_switch(struct wl1271 *wl,
 		break;
 	default:
 		wl1271_error("invalid channel switch band: %d",
-			     ch_switch->channel->band);
+			     ch_switch->chandef.chan->band);
 		ret = -EINVAL;
 		goto out_free;
 	}
diff --git a/drivers/net/wireless/ti/wl18xx/main.c b/drivers/net/wireless/ti/wl18xx/main.c
index da3ef1b10a9c..9fa692d11025 100644
--- a/drivers/net/wireless/ti/wl18xx/main.c
+++ b/drivers/net/wireless/ti/wl18xx/main.c
@@ -678,6 +678,7 @@ static int wl18xx_identify_chip(struct wl1271 *wl)
 	wl->sched_scan_templ_id_2_4 = CMD_TEMPL_PROBE_REQ_2_4_PERIODIC;
 	wl->sched_scan_templ_id_5 = CMD_TEMPL_PROBE_REQ_5_PERIODIC;
 	wl->max_channels_5 = WL18XX_MAX_CHANNELS_5GHZ;
+	wl->ba_rx_session_count_max = WL18XX_RX_BA_MAX_SESSIONS;
 out:
 	return ret;
 }
@@ -1144,6 +1145,7 @@ static u32 wl18xx_ap_get_mimo_wide_rate_mask(struct wl1271 *wl,
 static int wl18xx_get_pg_ver(struct wl1271 *wl, s8 *ver)
 {
 	u32 fuse;
+	s8 rom = 0, metal = 0, pg_ver = 0, rdl_ver = 0;
 	int ret;
 
 	ret = wlcore_set_partition(wl, &wl->ptable[PART_TOP_PRCM_ELP_SOC]);
@@ -1154,8 +1156,29 @@ static int wl18xx_get_pg_ver(struct wl1271 *wl, s8 *ver)
 	if (ret < 0)
 		goto out;
 
+	pg_ver = (fuse & WL18XX_PG_VER_MASK) >> WL18XX_PG_VER_OFFSET;
+	rom = (fuse & WL18XX_ROM_VER_MASK) >> WL18XX_ROM_VER_OFFSET;
+
+	if (rom <= 0xE)
+		metal = (fuse & WL18XX_METAL_VER_MASK) >>
+			WL18XX_METAL_VER_OFFSET;
+	else
+		metal = (fuse & WL18XX_NEW_METAL_VER_MASK) >>
+			WL18XX_NEW_METAL_VER_OFFSET;
+
+	ret = wlcore_read32(wl, WL18XX_REG_FUSE_DATA_2_3, &fuse);
+	if (ret < 0)
+		goto out;
+
+	rdl_ver = (fuse & WL18XX_RDL_VER_MASK) >> WL18XX_RDL_VER_OFFSET;
+	if (rdl_ver > RDL_MAX)
+		rdl_ver = RDL_NONE;
+
+	wl1271_info("wl18xx HW: RDL %d, %s, PG %x.%x (ROM %x)",
+		    rdl_ver, rdl_names[rdl_ver], pg_ver, metal, rom);
+
 	if (ver)
-		*ver = (fuse & WL18XX_PG_VER_MASK) >> WL18XX_PG_VER_OFFSET;
+		*ver = pg_ver;
 
 	ret = wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
 
diff --git a/drivers/net/wireless/ti/wl18xx/reg.h b/drivers/net/wireless/ti/wl18xx/reg.h
index 937b71d8783f..6306e04cd258 100644
--- a/drivers/net/wireless/ti/wl18xx/reg.h
+++ b/drivers/net/wireless/ti/wl18xx/reg.h
@@ -131,6 +131,16 @@
 #define WL18XX_REG_FUSE_DATA_1_3	0xA0260C
 #define WL18XX_PG_VER_MASK		0x70
 #define WL18XX_PG_VER_OFFSET		4
+#define WL18XX_ROM_VER_MASK		0x3
+#define WL18XX_ROM_VER_OFFSET		0
+#define WL18XX_METAL_VER_MASK		0xC
+#define WL18XX_METAL_VER_OFFSET		2
+#define WL18XX_NEW_METAL_VER_MASK	0x180
+#define WL18XX_NEW_METAL_VER_OFFSET	7
+
+#define WL18XX_REG_FUSE_DATA_2_3	0xA02614
+#define WL18XX_RDL_VER_MASK		0x1f00
+#define WL18XX_RDL_VER_OFFSET		8
 
 #define WL18XX_REG_FUSE_BD_ADDR_1	0xA02602
 #define WL18XX_REG_FUSE_BD_ADDR_2	0xA02606
@@ -188,4 +198,23 @@ enum {
 	NUM_BOARD_TYPES,
 };
 
+enum {
+	RDL_NONE	= 0,
+	RDL_1_HP	= 1,
+	RDL_2_SP	= 2,
+	RDL_3_HP	= 3,
+	RDL_4_SP	= 4,
+
+	_RDL_LAST,
+	RDL_MAX = _RDL_LAST - 1,
+};
+
+static const char * const rdl_names[] = {
+	[RDL_NONE]	= "",
+	[RDL_1_HP]	= "1853 SISO",
+	[RDL_2_SP]	= "1857 MIMO",
+	[RDL_3_HP]	= "1893 SISO",
+	[RDL_4_SP]	= "1897 MIMO",
+};
+
 #endif /* __REG_H__ */
diff --git a/drivers/net/wireless/ti/wl18xx/wl18xx.h b/drivers/net/wireless/ti/wl18xx/wl18xx.h
index b6739e79efcf..9204e07ee432 100644
--- a/drivers/net/wireless/ti/wl18xx/wl18xx.h
+++ b/drivers/net/wireless/ti/wl18xx/wl18xx.h
@@ -29,7 +29,7 @@
 #define WL18XX_IFTYPE_VER	5
 #define WL18XX_MAJOR_VER	WLCORE_FW_VER_IGNORE
 #define WL18XX_SUBTYPE_VER	WLCORE_FW_VER_IGNORE
-#define WL18XX_MINOR_VER	28
+#define WL18XX_MINOR_VER	39
 
 #define WL18XX_CMD_MAX_SIZE          740
 
@@ -40,6 +40,8 @@
 
 #define WL18XX_NUM_MAC_ADDRESSES 3
 
+#define WL18XX_RX_BA_MAX_SESSIONS 5
+
 struct wl18xx_priv {
 	/* buffer for sending commands to FW */
 	u8 cmd_buf[WL18XX_CMD_MAX_SIZE];
diff --git a/drivers/net/wireless/ti/wlcore/acx.c b/drivers/net/wireless/ti/wlcore/acx.c
index c79654323396..7a970cd9c555 100644
--- a/drivers/net/wireless/ti/wlcore/acx.c
+++ b/drivers/net/wireless/ti/wlcore/acx.c
@@ -1736,6 +1736,35 @@ out:
 
 }
 
+int wlcore_acx_average_rssi(struct wl1271 *wl, struct wl12xx_vif *wlvif,
+			    s8 *avg_rssi)
+{
+	struct acx_roaming_stats *acx;
+	int ret = 0;
+
+	wl1271_debug(DEBUG_ACX, "acx roaming statistics");
+
+	acx = kzalloc(sizeof(*acx), GFP_KERNEL);
+	if (!acx) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	acx->role_id = wlvif->role_id;
+	ret = wl1271_cmd_interrogate(wl, ACX_ROAMING_STATISTICS_TBL,
+				     acx, sizeof(*acx));
+	if (ret	< 0) {
+		wl1271_warning("acx roaming statistics failed: %d", ret);
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	*avg_rssi = acx->rssi_beacon;
+out:
+	kfree(acx);
+	return ret;
+}
+
 #ifdef CONFIG_PM
 /* Set the global behaviour of RX filters - On/Off + default action */
 int wl1271_acx_default_rx_filter_enable(struct wl1271 *wl, bool enable,
diff --git a/drivers/net/wireless/ti/wlcore/acx.h b/drivers/net/wireless/ti/wlcore/acx.h
index 126536c6a393..6dcfad9b0472 100644
--- a/drivers/net/wireless/ti/wlcore/acx.h
+++ b/drivers/net/wireless/ti/wlcore/acx.h
@@ -728,8 +728,6 @@ struct wl1271_acx_ht_information {
 	u8 padding[2];
 } __packed;
 
-#define RX_BA_MAX_SESSIONS 3
-
 struct wl1271_acx_ba_initiator_policy {
 	struct acx_header header;
 
@@ -955,6 +953,18 @@ struct acx_rx_filter_cfg {
 	u8 fields[0];
 } __packed;
 
+struct acx_roaming_stats {
+	struct acx_header header;
+
+	u8	role_id;
+	u8	pad[3];
+	u32	missed_beacons;
+	u8	snr_data;
+	u8	snr_bacon;
+	s8	rssi_data;
+	s8	rssi_beacon;
+} __packed;
+
 enum {
 	ACX_WAKE_UP_CONDITIONS           = 0x0000,
 	ACX_MEM_CFG                      = 0x0001,
@@ -1112,6 +1122,8 @@ int wl1271_acx_set_inconnection_sta(struct wl1271 *wl, u8 *addr);
 int wl1271_acx_fm_coex(struct wl1271 *wl);
 int wl12xx_acx_set_rate_mgmt_params(struct wl1271 *wl);
 int wl12xx_acx_config_hangover(struct wl1271 *wl);
+int wlcore_acx_average_rssi(struct wl1271 *wl, struct wl12xx_vif *wlvif,
+			    s8 *avg_rssi);
 
 #ifdef CONFIG_PM
 int wl1271_acx_default_rx_filter_enable(struct wl1271 *wl, bool enable,
diff --git a/drivers/net/wireless/ti/wlcore/cmd.c b/drivers/net/wireless/ti/wlcore/cmd.c
index 6331f9e1cb39..c9e060795d13 100644
--- a/drivers/net/wireless/ti/wlcore/cmd.c
+++ b/drivers/net/wireless/ti/wlcore/cmd.c
@@ -327,6 +327,14 @@ int wl12xx_allocate_link(struct wl1271 *wl, struct wl12xx_vif *wlvif, u8 *hlid)
 	wl->links[link].prev_freed_pkts =
 			wl->fw_status_2->counters.tx_lnk_free_pkts[link];
 	wl->links[link].wlvif = wlvif;
+
+	/*
+	 * Take saved value for total freed packets from wlvif, in case this is
+	 * recovery/resume
+	 */
+	if (wlvif->bss_type != BSS_TYPE_AP_BSS)
+		wl->links[link].total_freed_pkts = wlvif->total_freed_pkts;
+
 	*hlid = link;
 
 	wl->active_link_count++;
@@ -358,6 +366,26 @@ void wl12xx_free_link(struct wl1271 *wl, struct wl12xx_vif *wlvif, u8 *hlid)
 	wl1271_tx_reset_link_queues(wl, *hlid);
 	wl->links[*hlid].wlvif = NULL;
 
+	if (wlvif->bss_type == BSS_TYPE_STA_BSS ||
+	    (wlvif->bss_type == BSS_TYPE_AP_BSS &&
+	     *hlid == wlvif->ap.bcast_hlid)) {
+		/*
+		 * save the total freed packets in the wlvif, in case this is
+		 * recovery or suspend
+		 */
+		wlvif->total_freed_pkts = wl->links[*hlid].total_freed_pkts;
+
+		/*
+		 * increment the initial seq number on recovery to account for
+		 * transmitted packets that we haven't yet got in the FW status
+		 */
+		if (test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags))
+			wlvif->total_freed_pkts +=
+					WL1271_TX_SQN_POST_RECOVERY_PADDING;
+	}
+
+	wl->links[*hlid].total_freed_pkts = 0;
+
 	*hlid = WL12XX_INVALID_LINK_ID;
 	wl->active_link_count--;
 	WARN_ON_ONCE(wl->active_link_count < 0);
@@ -609,6 +637,10 @@ int wl12xx_cmd_role_start_ap(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	if (ret < 0)
 		goto out_free_global;
 
+	/* use the previous security seq, if this is a recovery/resume */
+	wl->links[wlvif->ap.bcast_hlid].total_freed_pkts =
+						wlvif->total_freed_pkts;
+
 	cmd->role_id = wlvif->role_id;
 	cmd->ap.aging_period = cpu_to_le16(wl->conf.tx.ap_aging_period);
 	cmd->ap.bss_index = WL1271_AP_BSS_INDEX;
diff --git a/drivers/net/wireless/ti/wlcore/debug.h b/drivers/net/wireless/ti/wlcore/debug.h
index db4bf5a68ce2..0420bd45e4ee 100644
--- a/drivers/net/wireless/ti/wlcore/debug.h
+++ b/drivers/net/wireless/ti/wlcore/debug.h
@@ -89,25 +89,24 @@ extern u32 wl12xx_debug_level;
 	} while (0)
 #endif
 
-/* TODO: use pr_debug_hex_dump when it becomes available */
-#define wl1271_dump(level, prefix, buf, len)	\
-	do { \
-		if (level & wl12xx_debug_level) \
-			print_hex_dump(KERN_DEBUG, DRIVER_PREFIX prefix, \
-				       DUMP_PREFIX_OFFSET, 16, 1,	\
-				       buf,				\
-				       min_t(size_t, len, DEBUG_DUMP_LIMIT), \
-				       0);				\
+#define wl1271_dump(level, prefix, buf, len)				      \
+	do {								      \
+		if (level & wl12xx_debug_level)				      \
+			print_hex_dump_debug(DRIVER_PREFIX prefix,	      \
+					DUMP_PREFIX_OFFSET, 16, 1,	      \
+					buf,				      \
+					min_t(size_t, len, DEBUG_DUMP_LIMIT), \
+					0);				      \
 	} while (0)
 
-#define wl1271_dump_ascii(level, prefix, buf, len)	\
-	do { \
-		if (level & wl12xx_debug_level) \
-			print_hex_dump(KERN_DEBUG, DRIVER_PREFIX prefix, \
-				       DUMP_PREFIX_OFFSET, 16, 1,	\
-				       buf,				\
-				       min_t(size_t, len, DEBUG_DUMP_LIMIT), \
-				       true);				\
+#define wl1271_dump_ascii(level, prefix, buf, len)			      \
+	do {								      \
+		if (level & wl12xx_debug_level)				      \
+			print_hex_dump_debug(DRIVER_PREFIX prefix,	      \
+					DUMP_PREFIX_OFFSET, 16, 1,	      \
+					buf,				      \
+					min_t(size_t, len, DEBUG_DUMP_LIMIT), \
+					true);				      \
 	} while (0)
 
 #endif /* __DEBUG_H__ */
diff --git a/drivers/net/wireless/ti/wlcore/debugfs.c b/drivers/net/wireless/ti/wlcore/debugfs.c
index e70a7c864865..c3e1f79c7856 100644
--- a/drivers/net/wireless/ti/wlcore/debugfs.c
+++ b/drivers/net/wireless/ti/wlcore/debugfs.c
@@ -598,8 +598,7 @@ static ssize_t vifs_state_read(struct file *file, char __user *user_buf,
 		VIF_STATE_PRINT_INT(last_rssi_event);
 		VIF_STATE_PRINT_INT(ba_support);
 		VIF_STATE_PRINT_INT(ba_allowed);
-		VIF_STATE_PRINT_LLHEX(tx_security_seq);
-		VIF_STATE_PRINT_INT(tx_security_last_seq_lsb);
+		VIF_STATE_PRINT_LLHEX(total_freed_pkts);
 	}
 
 #undef VIF_STATE_PRINT_INT
diff --git a/drivers/net/wireless/ti/wlcore/event.c b/drivers/net/wireless/ti/wlcore/event.c
index 70f289aa1bc6..67f61689b49e 100644
--- a/drivers/net/wireless/ti/wlcore/event.c
+++ b/drivers/net/wireless/ti/wlcore/event.c
@@ -237,6 +237,14 @@ void wlcore_event_beacon_loss(struct wl1271 *wl, unsigned long roles_bitmap)
 		    !test_bit(wlvif->role_id , &roles_bitmap))
 			continue;
 
+		vif = wl12xx_wlvif_to_vif(wlvif);
+
+		/* don't attempt roaming in case of p2p */
+		if (wlvif->p2p) {
+			ieee80211_connection_loss(vif);
+			continue;
+		}
+
 		/*
 		 * if the work is already queued, it should take place.
 		 * We don't want to delay the connection loss
@@ -246,7 +254,6 @@ void wlcore_event_beacon_loss(struct wl1271 *wl, unsigned long roles_bitmap)
 					     &wlvif->connection_loss_work,
 					     msecs_to_jiffies(delay));
 
-		vif = wl12xx_wlvif_to_vif(wlvif);
 		ieee80211_cqm_rssi_notify(
 				vif,
 				NL80211_CQM_RSSI_BEACON_LOSS_EVENT,
diff --git a/drivers/net/wireless/ti/wlcore/main.c b/drivers/net/wireless/ti/wlcore/main.c
index 2c2ff3e1f849..953111a502ee 100644
--- a/drivers/net/wireless/ti/wlcore/main.c
+++ b/drivers/net/wireless/ti/wlcore/main.c
@@ -108,8 +108,7 @@ static void wl1271_reg_notify(struct wiphy *wiphy,
 
 	}
 
-	if (likely(wl->state == WLCORE_STATE_ON))
-		wlcore_regdomain_config(wl);
+	wlcore_regdomain_config(wl);
 }
 
 static int wl1271_set_rx_streaming(struct wl1271 *wl, struct wl12xx_vif *wlvif,
@@ -332,10 +331,9 @@ static void wl12xx_irq_ps_regulate_link(struct wl1271 *wl,
 					struct wl12xx_vif *wlvif,
 					u8 hlid, u8 tx_pkts)
 {
-	bool fw_ps, single_link;
+	bool fw_ps;
 
 	fw_ps = test_bit(hlid, (unsigned long *)&wl->ap_fw_ps_map);
-	single_link = (wl->active_link_count == 1);
 
 	/*
 	 * Wake up from high level PS if the STA is asleep with too little
@@ -348,8 +346,13 @@ static void wl12xx_irq_ps_regulate_link(struct wl1271 *wl,
 	 * Start high-level PS if the STA is asleep with enough blocks in FW.
 	 * Make an exception if this is the only connected link. In this
 	 * case FW-memory congestion is less of a problem.
+	 * Note that a single connected STA means 3 active links, since we must
+	 * account for the global and broadcast AP links. The "fw_ps" check
+	 * assures us the third link is a STA connected to the AP. Otherwise
+	 * the FW would not set the PSM bit.
 	 */
-	else if (!single_link && fw_ps && tx_pkts >= WL1271_PS_STA_MAX_PACKETS)
+	else if (wl->active_link_count > 3 && fw_ps &&
+		 tx_pkts >= WL1271_PS_STA_MAX_PACKETS)
 		wl12xx_ps_link_start(wl, wlvif, hlid, true);
 }
 
@@ -414,13 +417,21 @@ static int wlcore_fw_status(struct wl1271 *wl,
 
 
 	for_each_set_bit(i, wl->links_map, WL12XX_MAX_LINKS) {
+		u8 diff;
 		lnk = &wl->links[i];
+
 		/* prevent wrap-around in freed-packets counter */
-		lnk->allocated_pkts -=
-			(status_2->counters.tx_lnk_free_pkts[i] -
-			 lnk->prev_freed_pkts) & 0xff;
+		diff = (status_2->counters.tx_lnk_free_pkts[i] -
+		       lnk->prev_freed_pkts) & 0xff;
+
+		if (diff == 0)
+			continue;
 
+		lnk->allocated_pkts -= diff;
 		lnk->prev_freed_pkts = status_2->counters.tx_lnk_free_pkts[i];
+
+		/* accumulate the prev_freed_pkts counter */
+		lnk->total_freed_pkts += diff;
 	}
 
 	/* prevent wrap-around in total blocks counter */
@@ -640,6 +651,25 @@ static irqreturn_t wlcore_irq(int irq, void *cookie)
 	unsigned long flags;
 	struct wl1271 *wl = cookie;
 
+	/* complete the ELP completion */
+	spin_lock_irqsave(&wl->wl_lock, flags);
+	set_bit(WL1271_FLAG_IRQ_RUNNING, &wl->flags);
+	if (wl->elp_compl) {
+		complete(wl->elp_compl);
+		wl->elp_compl = NULL;
+	}
+
+	if (test_bit(WL1271_FLAG_SUSPENDED, &wl->flags)) {
+		/* don't enqueue a work right now. mark it as pending */
+		set_bit(WL1271_FLAG_PENDING_WORK, &wl->flags);
+		wl1271_debug(DEBUG_IRQ, "should not enqueue work");
+		disable_irq_nosync(wl->irq);
+		pm_wakeup_event(wl->dev, 0);
+		spin_unlock_irqrestore(&wl->wl_lock, flags);
+		return IRQ_HANDLED;
+	}
+	spin_unlock_irqrestore(&wl->wl_lock, flags);
+
 	/* TX might be handled here, avoid redundant work */
 	set_bit(WL1271_FLAG_TX_PENDING, &wl->flags);
 	cancel_work_sync(&wl->tx_work);
@@ -919,18 +949,6 @@ static void wl1271_recovery_work(struct work_struct *work)
 		goto out_unlock;
 	}
 
-	/*
-	 * Advance security sequence number to overcome potential progress
-	 * in the firmware during recovery. This doens't hurt if the network is
-	 * not encrypted.
-	 */
-	wl12xx_for_each_wlvif(wl, wlvif) {
-		if (test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags) ||
-		    test_bit(WLVIF_FLAG_AP_STARTED, &wlvif->flags))
-			wlvif->tx_security_seq +=
-				WL1271_TX_SQN_POST_RECOVERY_PADDING;
-	}
-
 	/* Prevent spurious TX during FW restart */
 	wlcore_stop_queues(wl, WLCORE_QUEUE_STOP_REASON_FW_RESTART);
 
@@ -2523,6 +2541,8 @@ static void __wl1271_op_remove_interface(struct wl1271 *wl,
 		wl1271_ps_elp_sleep(wl);
 	}
 deinit:
+	wl12xx_tx_reset_wlvif(wl, wlvif);
+
 	/* clear all hlids (except system_hlid) */
 	wlvif->dev_hlid = WL12XX_INVALID_LINK_ID;
 
@@ -2546,7 +2566,6 @@ deinit:
 
 	dev_kfree_skb(wlvif->probereq);
 	wlvif->probereq = NULL;
-	wl12xx_tx_reset_wlvif(wl, wlvif);
 	if (wl->last_wlvif == wlvif)
 		wl->last_wlvif = NULL;
 	list_del(&wlvif->list);
@@ -2860,10 +2879,6 @@ static int wlcore_unset_assoc(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 				     wlvif->sta.klv_template_id,
 				     ACX_KEEP_ALIVE_TPL_INVALID);
 
-	/* reset TX security counters on a clean disconnect */
-	wlvif->tx_security_last_seq_lsb = 0;
-	wlvif->tx_security_seq = 0;
-
 	return 0;
 }
 
@@ -3262,6 +3277,7 @@ int wlcore_set_key(struct wl1271 *wl, enum set_key_cmd cmd,
 	u32 tx_seq_32 = 0;
 	u16 tx_seq_16 = 0;
 	u8 key_type;
+	u8 hlid;
 
 	wl1271_debug(DEBUG_MAC80211, "mac80211 set key");
 
@@ -3271,6 +3287,22 @@ int wlcore_set_key(struct wl1271 *wl, enum set_key_cmd cmd,
 		     key_conf->keylen, key_conf->flags);
 	wl1271_dump(DEBUG_CRYPT, "KEY: ", key_conf->key, key_conf->keylen);
 
+	if (wlvif->bss_type == BSS_TYPE_AP_BSS)
+		if (sta) {
+			struct wl1271_station *wl_sta = (void *)sta->drv_priv;
+			hlid = wl_sta->hlid;
+		} else {
+			hlid = wlvif->ap.bcast_hlid;
+		}
+	else
+		hlid = wlvif->sta.hlid;
+
+	if (hlid != WL12XX_INVALID_LINK_ID) {
+		u64 tx_seq = wl->links[hlid].total_freed_pkts;
+		tx_seq_32 = WL1271_TX_SECURITY_HI32(tx_seq);
+		tx_seq_16 = WL1271_TX_SECURITY_LO16(tx_seq);
+	}
+
 	switch (key_conf->cipher) {
 	case WLAN_CIPHER_SUITE_WEP40:
 	case WLAN_CIPHER_SUITE_WEP104:
@@ -3280,22 +3312,14 @@ int wlcore_set_key(struct wl1271 *wl, enum set_key_cmd cmd,
 		break;
 	case WLAN_CIPHER_SUITE_TKIP:
 		key_type = KEY_TKIP;
-
 		key_conf->hw_key_idx = key_conf->keyidx;
-		tx_seq_32 = WL1271_TX_SECURITY_HI32(wlvif->tx_security_seq);
-		tx_seq_16 = WL1271_TX_SECURITY_LO16(wlvif->tx_security_seq);
 		break;
 	case WLAN_CIPHER_SUITE_CCMP:
 		key_type = KEY_AES;
-
 		key_conf->flags |= IEEE80211_KEY_FLAG_PUT_IV_SPACE;
-		tx_seq_32 = WL1271_TX_SECURITY_HI32(wlvif->tx_security_seq);
-		tx_seq_16 = WL1271_TX_SECURITY_LO16(wlvif->tx_security_seq);
 		break;
 	case WL1271_CIPHER_SUITE_GEM:
 		key_type = KEY_GEM;
-		tx_seq_32 = WL1271_TX_SECURITY_HI32(wlvif->tx_security_seq);
-		tx_seq_16 = WL1271_TX_SECURITY_LO16(wlvif->tx_security_seq);
 		break;
 	default:
 		wl1271_error("Unknown key algo 0x%x", key_conf->cipher);
@@ -3358,6 +3382,10 @@ void wlcore_regdomain_config(struct wl1271 *wl)
 		return;
 
 	mutex_lock(&wl->mutex);
+
+	if (unlikely(wl->state != WLCORE_STATE_ON))
+		goto out;
+
 	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
 		goto out;
@@ -4474,7 +4502,7 @@ static int wl1271_op_get_survey(struct ieee80211_hw *hw, int idx,
 	if (idx != 0)
 		return -ENOENT;
 
-	survey->channel = conf->channel;
+	survey->channel = conf->chandef.chan;
 	survey->filled = 0;
 	return 0;
 }
@@ -4499,6 +4527,9 @@ static int wl1271_allocate_sta(struct wl1271 *wl,
 		return -EBUSY;
 	}
 
+	/* use the previous security seq, if this is a recovery/resume */
+	wl->links[wl_sta->hlid].total_freed_pkts = wl_sta->total_freed_pkts;
+
 	set_bit(wl_sta->hlid, wlvif->ap.sta_hlid_map);
 	memcpy(wl->links[wl_sta->hlid].addr, sta->addr, ETH_ALEN);
 	wl->active_sta_count++;
@@ -4507,12 +4538,37 @@ static int wl1271_allocate_sta(struct wl1271 *wl,
 
 void wl1271_free_sta(struct wl1271 *wl, struct wl12xx_vif *wlvif, u8 hlid)
 {
+	struct wl1271_station *wl_sta;
+	struct ieee80211_sta *sta;
+	struct ieee80211_vif *vif = wl12xx_wlvif_to_vif(wlvif);
+
 	if (!test_bit(hlid, wlvif->ap.sta_hlid_map))
 		return;
 
 	clear_bit(hlid, wlvif->ap.sta_hlid_map);
 	__clear_bit(hlid, &wl->ap_ps_map);
 	__clear_bit(hlid, (unsigned long *)&wl->ap_fw_ps_map);
+
+	/*
+	 * save the last used PN in the private part of iee80211_sta,
+	 * in case of recovery/suspend
+	 */
+	rcu_read_lock();
+	sta = ieee80211_find_sta(vif, wl->links[hlid].addr);
+	if (sta) {
+		wl_sta = (void *)sta->drv_priv;
+		wl_sta->total_freed_pkts = wl->links[hlid].total_freed_pkts;
+
+		/*
+		 * increment the initial seq number on recovery to account for
+		 * transmitted packets that we haven't yet got in the FW status
+		 */
+		if (test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags))
+			wl_sta->total_freed_pkts +=
+					WL1271_TX_SQN_POST_RECOVERY_PADDING;
+	}
+	rcu_read_unlock();
+
 	wl12xx_free_link(wl, wlvif, &hlid);
 	wl->active_sta_count--;
 
@@ -4616,13 +4672,11 @@ static int wl12xx_update_sta_state(struct wl1271 *wl,
 				   enum ieee80211_sta_state new_state)
 {
 	struct wl1271_station *wl_sta;
-	u8 hlid;
 	bool is_ap = wlvif->bss_type == BSS_TYPE_AP_BSS;
 	bool is_sta = wlvif->bss_type == BSS_TYPE_STA_BSS;
 	int ret;
 
 	wl_sta = (struct wl1271_station *)sta->drv_priv;
-	hlid = wl_sta->hlid;
 
 	/* Add station (AP mode) */
 	if (is_ap &&
@@ -4648,12 +4702,12 @@ static int wl12xx_update_sta_state(struct wl1271 *wl,
 	/* Authorize station (AP mode) */
 	if (is_ap &&
 	    new_state == IEEE80211_STA_AUTHORIZED) {
-		ret = wl12xx_cmd_set_peer_state(wl, wlvif, hlid);
+		ret = wl12xx_cmd_set_peer_state(wl, wlvif, wl_sta->hlid);
 		if (ret < 0)
 			return ret;
 
 		ret = wl1271_acx_set_ht_capabilities(wl, &sta->ht_cap, true,
-						     hlid);
+						     wl_sta->hlid);
 		if (ret)
 			return ret;
 
@@ -4784,7 +4838,7 @@ static int wl1271_op_ampdu_action(struct ieee80211_hw *hw,
 			break;
 		}
 
-		if (wl->ba_rx_session_count >= RX_BA_MAX_SESSIONS) {
+		if (wl->ba_rx_session_count >= wl->ba_rx_session_count_max) {
 			ret = -EBUSY;
 			wl1271_error("exceeded max RX BA sessions");
 			break;
@@ -4946,7 +5000,7 @@ out:
 	mutex_unlock(&wl->mutex);
 }
 
-static void wlcore_op_flush(struct ieee80211_hw *hw, bool drop)
+static void wlcore_op_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 {
 	struct wl1271 *wl = hw->priv;
 
@@ -4956,7 +5010,8 @@ static void wlcore_op_flush(struct ieee80211_hw *hw, bool drop)
 static int wlcore_op_remain_on_channel(struct ieee80211_hw *hw,
 				       struct ieee80211_vif *vif,
 				       struct ieee80211_channel *chan,
-				       int duration)
+				       int duration,
+				       enum ieee80211_roc_type type)
 {
 	struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
 	struct wl1271 *wl = hw->priv;
@@ -5091,6 +5146,39 @@ static void wlcore_op_sta_rc_update(struct ieee80211_hw *hw,
 	wlcore_hw_sta_rc_update(wl, wlvif, sta, changed);
 }
 
+static int wlcore_op_get_rssi(struct ieee80211_hw *hw,
+			       struct ieee80211_vif *vif,
+			       struct ieee80211_sta *sta,
+			       s8 *rssi_dbm)
+{
+	struct wl1271 *wl = hw->priv;
+	struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
+	int ret = 0;
+
+	wl1271_debug(DEBUG_MAC80211, "mac80211 get_rssi");
+
+	mutex_lock(&wl->mutex);
+
+	if (unlikely(wl->state != WLCORE_STATE_ON))
+		goto out;
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out_sleep;
+
+	ret = wlcore_acx_average_rssi(wl, wlvif, rssi_dbm);
+	if (ret < 0)
+		goto out_sleep;
+
+out_sleep:
+	wl1271_ps_elp_sleep(wl);
+
+out:
+	mutex_unlock(&wl->mutex);
+
+	return ret;
+}
+
 static bool wl1271_tx_frames_pending(struct ieee80211_hw *hw)
 {
 	struct wl1271 *wl = hw->priv;
@@ -5290,6 +5378,7 @@ static const struct ieee80211_ops wl1271_ops = {
 	.assign_vif_chanctx = wlcore_op_assign_vif_chanctx,
 	.unassign_vif_chanctx = wlcore_op_unassign_vif_chanctx,
 	.sta_rc_update = wlcore_op_sta_rc_update,
+	.get_rssi = wlcore_op_get_rssi,
 	CFG80211_TESTMODE_CMD(wl1271_tm_cmd)
 };
 
@@ -5929,35 +6018,6 @@ int wlcore_free_hw(struct wl1271 *wl)
 }
 EXPORT_SYMBOL_GPL(wlcore_free_hw);
 
-static irqreturn_t wl12xx_hardirq(int irq, void *cookie)
-{
-	struct wl1271 *wl = cookie;
-	unsigned long flags;
-
-	wl1271_debug(DEBUG_IRQ, "IRQ");
-
-	/* complete the ELP completion */
-	spin_lock_irqsave(&wl->wl_lock, flags);
-	set_bit(WL1271_FLAG_IRQ_RUNNING, &wl->flags);
-	if (wl->elp_compl) {
-		complete(wl->elp_compl);
-		wl->elp_compl = NULL;
-	}
-
-	if (test_bit(WL1271_FLAG_SUSPENDED, &wl->flags)) {
-		/* don't enqueue a work right now. mark it as pending */
-		set_bit(WL1271_FLAG_PENDING_WORK, &wl->flags);
-		wl1271_debug(DEBUG_IRQ, "should not enqueue work");
-		disable_irq_nosync(wl->irq);
-		pm_wakeup_event(wl->dev, 0);
-		spin_unlock_irqrestore(&wl->wl_lock, flags);
-		return IRQ_HANDLED;
-	}
-	spin_unlock_irqrestore(&wl->wl_lock, flags);
-
-	return IRQ_WAKE_THREAD;
-}
-
 static void wlcore_nvs_cb(const struct firmware *fw, void *context)
 {
 	struct wl1271 *wl = context;
@@ -5999,9 +6059,8 @@ static void wlcore_nvs_cb(const struct firmware *fw, void *context)
 	else
 		irqflags = IRQF_TRIGGER_HIGH | IRQF_ONESHOT;
 
-	ret = request_threaded_irq(wl->irq, wl12xx_hardirq, wlcore_irq,
-				   irqflags,
-				   pdev->name, wl);
+	ret = request_threaded_irq(wl->irq, NULL, wlcore_irq,
+				   irqflags, pdev->name, wl);
 	if (ret < 0) {
 		wl1271_error("request_irq() failed: %d", ret);
 		goto out_free_nvs;
diff --git a/drivers/net/wireless/ti/wlcore/ps.c b/drivers/net/wireless/ti/wlcore/ps.c
index 9b7b6e2e4fbc..9654577efd01 100644
--- a/drivers/net/wireless/ti/wlcore/ps.c
+++ b/drivers/net/wireless/ti/wlcore/ps.c
@@ -29,6 +29,7 @@
 #define WL1271_WAKEUP_TIMEOUT 500
 
 #define ELP_ENTRY_DELAY  30
+#define ELP_ENTRY_DELAY_FORCE_PS  5
 
 void wl1271_elp_work(struct work_struct *work)
 {
@@ -98,7 +99,8 @@ void wl1271_ps_elp_sleep(struct wl1271 *wl)
 			return;
 	}
 
-	timeout = ELP_ENTRY_DELAY;
+	timeout = wl->conf.conn.forced_ps ?
+			ELP_ENTRY_DELAY_FORCE_PS : ELP_ENTRY_DELAY;
 	ieee80211_queue_delayed_work(wl->hw, &wl->elp_work,
 				     msecs_to_jiffies(timeout));
 }
diff --git a/drivers/net/wireless/ti/wlcore/tx.c b/drivers/net/wireless/ti/wlcore/tx.c
index ece392c54d9c..004d02e71f01 100644
--- a/drivers/net/wireless/ti/wlcore/tx.c
+++ b/drivers/net/wireless/ti/wlcore/tx.c
@@ -24,6 +24,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/etherdevice.h>
+#include <linux/spinlock.h>
 
 #include "wlcore.h"
 #include "debug.h"
@@ -104,7 +105,7 @@ static void wl1271_tx_regulate_link(struct wl1271 *wl,
 				    struct wl12xx_vif *wlvif,
 				    u8 hlid)
 {
-	bool fw_ps, single_link;
+	bool fw_ps;
 	u8 tx_pkts;
 
 	if (WARN_ON(!test_bit(hlid, wlvif->links_map)))
@@ -112,15 +113,19 @@ static void wl1271_tx_regulate_link(struct wl1271 *wl,
 
 	fw_ps = test_bit(hlid, (unsigned long *)&wl->ap_fw_ps_map);
 	tx_pkts = wl->links[hlid].allocated_pkts;
-	single_link = (wl->active_link_count == 1);
 
 	/*
 	 * if in FW PS and there is enough data in FW we can put the link
 	 * into high-level PS and clean out its TX queues.
 	 * Make an exception if this is the only connected link. In this
 	 * case FW-memory congestion is less of a problem.
+	 * Note that a single connected STA means 3 active links, since we must
+	 * account for the global and broadcast AP links. The "fw_ps" check
+	 * assures us the third link is a STA connected to the AP. Otherwise
+	 * the FW would not set the PSM bit.
 	 */
-	if (!single_link && fw_ps && tx_pkts >= WL1271_PS_STA_MAX_PACKETS)
+	if (wl->active_link_count > 3 && fw_ps &&
+	    tx_pkts >= WL1271_PS_STA_MAX_PACKETS)
 		wl12xx_ps_link_start(wl, wlvif, hlid, true);
 }
 
@@ -639,6 +644,7 @@ next:
 
 	}
 
+out:
 	if (!skb &&
 	    test_and_clear_bit(WL1271_FLAG_DUMMY_PACKET_PENDING, &wl->flags)) {
 		int q;
@@ -652,7 +658,6 @@ next:
 		spin_unlock_irqrestore(&wl->wl_lock, flags);
 	}
 
-out:
 	return skb;
 }
 
@@ -928,25 +933,6 @@ static void wl1271_tx_complete_packet(struct wl1271 *wl,
 
 	wl->stats.retry_count += result->ack_failures;
 
-	/*
-	 * update sequence number only when relevant, i.e. only in
-	 * sessions of TKIP, AES and GEM (not in open or WEP sessions)
-	 */
-	if (info->control.hw_key &&
-	    (info->control.hw_key->cipher == WLAN_CIPHER_SUITE_TKIP ||
-	     info->control.hw_key->cipher == WLAN_CIPHER_SUITE_CCMP ||
-	     info->control.hw_key->cipher == WL1271_CIPHER_SUITE_GEM)) {
-		u8 fw_lsb = result->tx_security_sequence_number_lsb;
-		u8 cur_lsb = wlvif->tx_security_last_seq_lsb;
-
-		/*
-		 * update security sequence number, taking care of potential
-		 * wrap-around
-		 */
-		wlvif->tx_security_seq += (fw_lsb - cur_lsb) & 0xff;
-		wlvif->tx_security_last_seq_lsb = fw_lsb;
-	}
-
 	/* remove private header from packet */
 	skb_pull(skb, sizeof(struct wl1271_tx_hw_descr));
 
@@ -1061,7 +1047,8 @@ void wl12xx_tx_reset_wlvif(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 
 	/* TX failure */
 	for_each_set_bit(i, wlvif->links_map, WL12XX_MAX_LINKS) {
-		if (wlvif->bss_type == BSS_TYPE_AP_BSS) {
+		if (wlvif->bss_type == BSS_TYPE_AP_BSS &&
+		    i != wlvif->ap.bcast_hlid && i != wlvif->ap.global_hlid) {
 			/* this calls wl12xx_free_link */
 			wl1271_free_sta(wl, wlvif, i);
 		} else {
@@ -1304,7 +1291,7 @@ bool wlcore_is_queue_stopped_by_reason_locked(struct wl1271 *wl,
 {
 	int hwq = wlcore_tx_get_mac80211_queue(wlvif, queue);
 
-	WARN_ON_ONCE(!spin_is_locked(&wl->wl_lock));
+	assert_spin_locked(&wl->wl_lock);
 	return test_bit(reason, &wl->queue_stop_reasons[hwq]);
 }
 
@@ -1313,6 +1300,6 @@ bool wlcore_is_queue_stopped_locked(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 {
 	int hwq = wlcore_tx_get_mac80211_queue(wlvif, queue);
 
-	WARN_ON_ONCE(!spin_is_locked(&wl->wl_lock));
+	assert_spin_locked(&wl->wl_lock);
 	return !!wl->queue_stop_reasons[hwq];
 }
diff --git a/drivers/net/wireless/ti/wlcore/wlcore.h b/drivers/net/wireless/ti/wlcore/wlcore.h
index af9fecaefc30..0034979e97cb 100644
--- a/drivers/net/wireless/ti/wlcore/wlcore.h
+++ b/drivers/net/wireless/ti/wlcore/wlcore.h
@@ -390,6 +390,9 @@ struct wl1271 {
 	/* number of currently active RX BA sessions */
 	int ba_rx_session_count;
 
+	/* Maximum number of supported RX BA sessions */
+	int ba_rx_session_count_max;
+
 	/* AP-mode - number of currently connected stations */
 	int active_sta_count;
 
diff --git a/drivers/net/wireless/ti/wlcore/wlcore_i.h b/drivers/net/wireless/ti/wlcore/wlcore_i.h
index 508f5b0f8a70..e5e146435fe7 100644
--- a/drivers/net/wireless/ti/wlcore/wlcore_i.h
+++ b/drivers/net/wireless/ti/wlcore/wlcore_i.h
@@ -274,6 +274,13 @@ struct wl1271_link {
 
 	/* The wlvif this link belongs to. Might be null for global links */
 	struct wl12xx_vif *wlvif;
+
+	/*
+	 * total freed FW packets on the link - used for tracking the
+	 * AES/TKIP PN across recoveries. Re-initialized each time
+	 * from the wl1271_station structure.
+	 */
+	u64 total_freed_pkts;
 };
 
 #define WL1271_MAX_RX_FILTERS 5
@@ -318,6 +325,13 @@ struct wl12xx_rx_filter {
 struct wl1271_station {
 	u8 hlid;
 	bool in_connection;
+
+	/*
+	 * total freed FW packets on the link to the STA - used for tracking the
+	 * AES/TKIP PN across recoveries. Re-initialized each time from the
+	 * wl1271_station structure.
+	 */
+	u64 total_freed_pkts;
 };
 
 struct wl12xx_vif {
@@ -449,16 +463,15 @@ struct wl12xx_vif {
 	 */
 	struct {
 		u8 persistent[0];
+
 		/*
-		 * Security sequence number
-		 *     bits 0-15: lower 16 bits part of sequence number
-		 *     bits 16-47: higher 32 bits part of sequence number
-		 *     bits 48-63: not in use
+		 * total freed FW packets on the link - used for
+		 * storing the AES/TKIP PN during recovery, as this
+		 * structure is not zeroed out.
+		 * For STA this holds the PN of the link to the AP.
+		 * For AP this holds the PN of the broadcast link.
 		 */
-		u64 tx_security_seq;
-
-		/* 8 bits of the last sequence number in use */
-		u8 tx_security_last_seq_lsb;
+		u64 total_freed_pkts;
 	};
 };
 
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.c b/drivers/net/wireless/zd1211rw/zd_mac.c
index 114364b5d466..c6208a7988e4 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.c
+++ b/drivers/net/wireless/zd1211rw/zd_mac.c
@@ -1156,10 +1156,10 @@ static int zd_op_config(struct ieee80211_hw *hw, u32 changed)
 	struct ieee80211_conf *conf = &hw->conf;
 
 	spin_lock_irq(&mac->lock);
-	mac->channel = conf->channel->hw_value;
+	mac->channel = conf->chandef.chan->hw_value;
 	spin_unlock_irq(&mac->lock);
 
-	return zd_chip_set_channel(&mac->chip, conf->channel->hw_value);
+	return zd_chip_set_channel(&mac->chip, conf->chandef.chan->hw_value);
 }
 
 static void zd_beacon_done(struct zd_mac *mac)
diff --git a/drivers/net/xen-netback/netback.c b/drivers/net/xen-netback/netback.c
index cd49ba949636..37984e6d4e99 100644
--- a/drivers/net/xen-netback/netback.c
+++ b/drivers/net/xen-netback/netback.c
@@ -47,11 +47,33 @@
 #include <asm/xen/hypercall.h>
 #include <asm/xen/page.h>
 
+/*
+ * This is the maximum slots a skb can have. If a guest sends a skb
+ * which exceeds this limit it is considered malicious.
+ */
+#define FATAL_SKB_SLOTS_DEFAULT 20
+static unsigned int fatal_skb_slots = FATAL_SKB_SLOTS_DEFAULT;
+module_param(fatal_skb_slots, uint, 0444);
+
+/*
+ * To avoid confusion, we define XEN_NETBK_LEGACY_SLOTS_MAX indicating
+ * the maximum slots a valid packet can use. Now this value is defined
+ * to be XEN_NETIF_NR_SLOTS_MIN, which is supposed to be supported by
+ * all backend.
+ */
+#define XEN_NETBK_LEGACY_SLOTS_MAX XEN_NETIF_NR_SLOTS_MIN
+
+typedef unsigned int pending_ring_idx_t;
+#define INVALID_PENDING_RING_IDX (~0U)
+
 struct pending_tx_info {
-	struct xen_netif_tx_request req;
+	struct xen_netif_tx_request req; /* coalesced tx request */
 	struct xenvif *vif;
+	pending_ring_idx_t head; /* head != INVALID_PENDING_RING_IDX
+				  * if it is head of one or more tx
+				  * reqs
+				  */
 };
-typedef unsigned int pending_ring_idx_t;
 
 struct netbk_rx_meta {
 	int id;
@@ -102,7 +124,11 @@ struct xen_netbk {
 	atomic_t netfront_count;
 
 	struct pending_tx_info pending_tx_info[MAX_PENDING_REQS];
-	struct gnttab_copy tx_copy_ops[MAX_PENDING_REQS];
+	/* Coalescing tx requests before copying makes number of grant
+	 * copy ops greater or equal to number of slots required. In
+	 * worst case a tx request consumes 2 gnttab_copy.
+	 */
+	struct gnttab_copy tx_copy_ops[2*MAX_PENDING_REQS];
 
 	u16 pending_ring[MAX_PENDING_REQS];
 
@@ -118,6 +144,16 @@ struct xen_netbk {
 static struct xen_netbk *xen_netbk;
 static int xen_netbk_group_nr;
 
+/*
+ * If head != INVALID_PENDING_RING_IDX, it means this tx request is head of
+ * one or more merged tx requests, otherwise it is the continuation of
+ * previous tx request.
+ */
+static inline int pending_tx_is_head(struct xen_netbk *netbk, RING_IDX idx)
+{
+	return netbk->pending_tx_info[idx].head != INVALID_PENDING_RING_IDX;
+}
+
 void xen_netbk_add_xenvif(struct xenvif *vif)
 {
 	int i;
@@ -250,6 +286,7 @@ static int max_required_rx_slots(struct xenvif *vif)
 {
 	int max = DIV_ROUND_UP(vif->dev->mtu, PAGE_SIZE);
 
+	/* XXX FIXME: RX path dependent on MAX_SKB_FRAGS */
 	if (vif->can_sg || vif->gso || vif->gso_prefix)
 		max += MAX_SKB_FRAGS + 1; /* extra_info + frags */
 
@@ -657,6 +694,7 @@ static void xen_netbk_rx_action(struct xen_netbk *netbk)
 		__skb_queue_tail(&rxq, skb);
 
 		/* Filled the batch queue? */
+		/* XXX FIXME: RX path dependent on MAX_SKB_FRAGS */
 		if (count + MAX_SKB_FRAGS >= XEN_NETIF_RX_RING_SIZE)
 			break;
 	}
@@ -902,47 +940,99 @@ static int netbk_count_requests(struct xenvif *vif,
 				int work_to_do)
 {
 	RING_IDX cons = vif->tx.req_cons;
-	int frags = 0;
+	int slots = 0;
+	int drop_err = 0;
+	int more_data;
 
 	if (!(first->flags & XEN_NETTXF_more_data))
 		return 0;
 
 	do {
-		if (frags >= work_to_do) {
-			netdev_err(vif->dev, "Need more frags\n");
+		struct xen_netif_tx_request dropped_tx = { 0 };
+
+		if (slots >= work_to_do) {
+			netdev_err(vif->dev,
+				   "Asked for %d slots but exceeds this limit\n",
+				   work_to_do);
 			netbk_fatal_tx_err(vif);
 			return -ENODATA;
 		}
 
-		if (unlikely(frags >= MAX_SKB_FRAGS)) {
-			netdev_err(vif->dev, "Too many frags\n");
+		/* This guest is really using too many slots and
+		 * considered malicious.
+		 */
+		if (unlikely(slots >= fatal_skb_slots)) {
+			netdev_err(vif->dev,
+				   "Malicious frontend using %d slots, threshold %u\n",
+				   slots, fatal_skb_slots);
 			netbk_fatal_tx_err(vif);
 			return -E2BIG;
 		}
 
-		memcpy(txp, RING_GET_REQUEST(&vif->tx, cons + frags),
+		/* Xen network protocol had implicit dependency on
+		 * MAX_SKB_FRAGS. XEN_NETBK_LEGACY_SLOTS_MAX is set to
+		 * the historical MAX_SKB_FRAGS value 18 to honor the
+		 * same behavior as before. Any packet using more than
+		 * 18 slots but less than fatal_skb_slots slots is
+		 * dropped
+		 */
+		if (!drop_err && slots >= XEN_NETBK_LEGACY_SLOTS_MAX) {
+			if (net_ratelimit())
+				netdev_dbg(vif->dev,
+					   "Too many slots (%d) exceeding limit (%d), dropping packet\n",
+					   slots, XEN_NETBK_LEGACY_SLOTS_MAX);
+			drop_err = -E2BIG;
+		}
+
+		if (drop_err)
+			txp = &dropped_tx;
+
+		memcpy(txp, RING_GET_REQUEST(&vif->tx, cons + slots),
 		       sizeof(*txp));
-		if (txp->size > first->size) {
-			netdev_err(vif->dev, "Frag is bigger than frame.\n");
-			netbk_fatal_tx_err(vif);
-			return -EIO;
+
+		/* If the guest submitted a frame >= 64 KiB then
+		 * first->size overflowed and following slots will
+		 * appear to be larger than the frame.
+		 *
+		 * This cannot be fatal error as there are buggy
+		 * frontends that do this.
+		 *
+		 * Consume all slots and drop the packet.
+		 */
+		if (!drop_err && txp->size > first->size) {
+			if (net_ratelimit())
+				netdev_dbg(vif->dev,
+					   "Invalid tx request, slot size %u > remaining size %u\n",
+					   txp->size, first->size);
+			drop_err = -EIO;
 		}
 
 		first->size -= txp->size;
-		frags++;
+		slots++;
 
 		if (unlikely((txp->offset + txp->size) > PAGE_SIZE)) {
-			netdev_err(vif->dev, "txp->offset: %x, size: %u\n",
+			netdev_err(vif->dev, "Cross page boundary, txp->offset: %x, size: %u\n",
 				 txp->offset, txp->size);
 			netbk_fatal_tx_err(vif);
 			return -EINVAL;
 		}
-	} while ((txp++)->flags & XEN_NETTXF_more_data);
-	return frags;
+
+		more_data = txp->flags & XEN_NETTXF_more_data;
+
+		if (!drop_err)
+			txp++;
+
+	} while (more_data);
+
+	if (drop_err) {
+		netbk_tx_err(vif, first, cons + slots);
+		return drop_err;
+	}
+
+	return slots;
 }
 
 static struct page *xen_netbk_alloc_page(struct xen_netbk *netbk,
-					 struct sk_buff *skb,
 					 u16 pending_idx)
 {
 	struct page *page;
@@ -963,48 +1053,114 @@ static struct gnttab_copy *xen_netbk_get_requests(struct xen_netbk *netbk,
 	struct skb_shared_info *shinfo = skb_shinfo(skb);
 	skb_frag_t *frags = shinfo->frags;
 	u16 pending_idx = *((u16 *)skb->data);
-	int i, start;
+	u16 head_idx = 0;
+	int slot, start;
+	struct page *page;
+	pending_ring_idx_t index, start_idx = 0;
+	uint16_t dst_offset;
+	unsigned int nr_slots;
+	struct pending_tx_info *first = NULL;
+
+	/* At this point shinfo->nr_frags is in fact the number of
+	 * slots, which can be as large as XEN_NETBK_LEGACY_SLOTS_MAX.
+	 */
+	nr_slots = shinfo->nr_frags;
 
 	/* Skip first skb fragment if it is on same page as header fragment. */
 	start = (frag_get_pending_idx(&shinfo->frags[0]) == pending_idx);
 
-	for (i = start; i < shinfo->nr_frags; i++, txp++) {
-		struct page *page;
-		pending_ring_idx_t index;
+	/* Coalesce tx requests, at this point the packet passed in
+	 * should be <= 64K. Any packets larger than 64K have been
+	 * handled in netbk_count_requests().
+	 */
+	for (shinfo->nr_frags = slot = start; slot < nr_slots;
+	     shinfo->nr_frags++) {
 		struct pending_tx_info *pending_tx_info =
 			netbk->pending_tx_info;
 
-		index = pending_index(netbk->pending_cons++);
-		pending_idx = netbk->pending_ring[index];
-		page = xen_netbk_alloc_page(netbk, skb, pending_idx);
+		page = alloc_page(GFP_KERNEL|__GFP_COLD);
 		if (!page)
 			goto err;
 
-		gop->source.u.ref = txp->gref;
-		gop->source.domid = vif->domid;
-		gop->source.offset = txp->offset;
-
-		gop->dest.u.gmfn = virt_to_mfn(page_address(page));
-		gop->dest.domid = DOMID_SELF;
-		gop->dest.offset = txp->offset;
-
-		gop->len = txp->size;
-		gop->flags = GNTCOPY_source_gref;
+		dst_offset = 0;
+		first = NULL;
+		while (dst_offset < PAGE_SIZE && slot < nr_slots) {
+			gop->flags = GNTCOPY_source_gref;
+
+			gop->source.u.ref = txp->gref;
+			gop->source.domid = vif->domid;
+			gop->source.offset = txp->offset;
+
+			gop->dest.domid = DOMID_SELF;
+
+			gop->dest.offset = dst_offset;
+			gop->dest.u.gmfn = virt_to_mfn(page_address(page));
+
+			if (dst_offset + txp->size > PAGE_SIZE) {
+				/* This page can only merge a portion
+				 * of tx request. Do not increment any
+				 * pointer / counter here. The txp
+				 * will be dealt with in future
+				 * rounds, eventually hitting the
+				 * `else` branch.
+				 */
+				gop->len = PAGE_SIZE - dst_offset;
+				txp->offset += gop->len;
+				txp->size -= gop->len;
+				dst_offset += gop->len; /* quit loop */
+			} else {
+				/* This tx request can be merged in the page */
+				gop->len = txp->size;
+				dst_offset += gop->len;
+
+				index = pending_index(netbk->pending_cons++);
+
+				pending_idx = netbk->pending_ring[index];
+
+				memcpy(&pending_tx_info[pending_idx].req, txp,
+				       sizeof(*txp));
+				xenvif_get(vif);
+
+				pending_tx_info[pending_idx].vif = vif;
+
+				/* Poison these fields, corresponding
+				 * fields for head tx req will be set
+				 * to correct values after the loop.
+				 */
+				netbk->mmap_pages[pending_idx] = (void *)(~0UL);
+				pending_tx_info[pending_idx].head =
+					INVALID_PENDING_RING_IDX;
+
+				if (!first) {
+					first = &pending_tx_info[pending_idx];
+					start_idx = index;
+					head_idx = pending_idx;
+				}
+
+				txp++;
+				slot++;
+			}
 
-		gop++;
+			gop++;
+		}
 
-		memcpy(&pending_tx_info[pending_idx].req, txp, sizeof(*txp));
-		xenvif_get(vif);
-		pending_tx_info[pending_idx].vif = vif;
-		frag_set_pending_idx(&frags[i], pending_idx);
+		first->req.offset = 0;
+		first->req.size = dst_offset;
+		first->head = start_idx;
+		set_page_ext(page, netbk, head_idx);
+		netbk->mmap_pages[head_idx] = page;
+		frag_set_pending_idx(&frags[shinfo->nr_frags], head_idx);
 	}
 
+	BUG_ON(shinfo->nr_frags > MAX_SKB_FRAGS);
+
 	return gop;
 err:
 	/* Unwind, freeing all pages and sending error responses. */
-	while (i-- > start) {
-		xen_netbk_idx_release(netbk, frag_get_pending_idx(&frags[i]),
-				      XEN_NETIF_RSP_ERROR);
+	while (shinfo->nr_frags-- > start) {
+		xen_netbk_idx_release(netbk,
+				frag_get_pending_idx(&frags[shinfo->nr_frags]),
+				XEN_NETIF_RSP_ERROR);
 	}
 	/* The head too, if necessary. */
 	if (start)
@@ -1020,8 +1176,10 @@ static int xen_netbk_tx_check_gop(struct xen_netbk *netbk,
 	struct gnttab_copy *gop = *gopp;
 	u16 pending_idx = *((u16 *)skb->data);
 	struct skb_shared_info *shinfo = skb_shinfo(skb);
+	struct pending_tx_info *tx_info;
 	int nr_frags = shinfo->nr_frags;
 	int i, err, start;
+	u16 peek; /* peek into next tx request */
 
 	/* Check status of header. */
 	err = gop->status;
@@ -1033,11 +1191,20 @@ static int xen_netbk_tx_check_gop(struct xen_netbk *netbk,
 
 	for (i = start; i < nr_frags; i++) {
 		int j, newerr;
+		pending_ring_idx_t head;
 
 		pending_idx = frag_get_pending_idx(&shinfo->frags[i]);
+		tx_info = &netbk->pending_tx_info[pending_idx];
+		head = tx_info->head;
 
 		/* Check error status: if okay then remember grant handle. */
-		newerr = (++gop)->status;
+		do {
+			newerr = (++gop)->status;
+			if (newerr)
+				break;
+			peek = netbk->pending_ring[pending_index(++head)];
+		} while (!pending_tx_is_head(netbk, peek));
+
 		if (likely(!newerr)) {
 			/* Had a previous error? Invalidate this fragment. */
 			if (unlikely(err))
@@ -1157,7 +1324,6 @@ static int netbk_set_skb_gso(struct xenvif *vif,
 static int checksum_setup(struct xenvif *vif, struct sk_buff *skb)
 {
 	struct iphdr *iph;
-	unsigned char *th;
 	int err = -EPROTO;
 	int recalculate_partial_csum = 0;
 
@@ -1181,27 +1347,26 @@ static int checksum_setup(struct xenvif *vif, struct sk_buff *skb)
 		goto out;
 
 	iph = (void *)skb->data;
-	th = skb->data + 4 * iph->ihl;
-	if (th >= skb_tail_pointer(skb))
-		goto out;
-
-	skb->csum_start = th - skb->head;
 	switch (iph->protocol) {
 	case IPPROTO_TCP:
-		skb->csum_offset = offsetof(struct tcphdr, check);
+		if (!skb_partial_csum_set(skb, 4 * iph->ihl,
+					  offsetof(struct tcphdr, check)))
+			goto out;
 
 		if (recalculate_partial_csum) {
-			struct tcphdr *tcph = (struct tcphdr *)th;
+			struct tcphdr *tcph = tcp_hdr(skb);
 			tcph->check = ~csum_tcpudp_magic(iph->saddr, iph->daddr,
 							 skb->len - iph->ihl*4,
 							 IPPROTO_TCP, 0);
 		}
 		break;
 	case IPPROTO_UDP:
-		skb->csum_offset = offsetof(struct udphdr, check);
+		if (!skb_partial_csum_set(skb, 4 * iph->ihl,
+					  offsetof(struct udphdr, check)))
+			goto out;
 
 		if (recalculate_partial_csum) {
-			struct udphdr *udph = (struct udphdr *)th;
+			struct udphdr *udph = udp_hdr(skb);
 			udph->check = ~csum_tcpudp_magic(iph->saddr, iph->daddr,
 							 skb->len - iph->ihl*4,
 							 IPPROTO_UDP, 0);
@@ -1215,9 +1380,6 @@ static int checksum_setup(struct xenvif *vif, struct sk_buff *skb)
 		goto out;
 	}
 
-	if ((th + skb->csum_offset + 2) > skb_tail_pointer(skb))
-		goto out;
-
 	err = 0;
 
 out:
@@ -1262,11 +1424,12 @@ static unsigned xen_netbk_tx_build_gops(struct xen_netbk *netbk)
 	struct sk_buff *skb;
 	int ret;
 
-	while (((nr_pending_reqs(netbk) + MAX_SKB_FRAGS) < MAX_PENDING_REQS) &&
+	while ((nr_pending_reqs(netbk) + XEN_NETBK_LEGACY_SLOTS_MAX
+		< MAX_PENDING_REQS) &&
 		!list_empty(&netbk->net_schedule_list)) {
 		struct xenvif *vif;
 		struct xen_netif_tx_request txreq;
-		struct xen_netif_tx_request txfrags[MAX_SKB_FRAGS];
+		struct xen_netif_tx_request txfrags[XEN_NETBK_LEGACY_SLOTS_MAX];
 		struct page *page;
 		struct xen_netif_extra_info extras[XEN_NETIF_EXTRA_TYPE_MAX-1];
 		u16 pending_idx;
@@ -1354,7 +1517,7 @@ static unsigned xen_netbk_tx_build_gops(struct xen_netbk *netbk)
 		pending_idx = netbk->pending_ring[index];
 
 		data_len = (txreq.size > PKT_PROT_LEN &&
-			    ret < MAX_SKB_FRAGS) ?
+			    ret < XEN_NETBK_LEGACY_SLOTS_MAX) ?
 			PKT_PROT_LEN : txreq.size;
 
 		skb = alloc_skb(data_len + NET_SKB_PAD + NET_IP_ALIGN,
@@ -1381,7 +1544,7 @@ static unsigned xen_netbk_tx_build_gops(struct xen_netbk *netbk)
 		}
 
 		/* XXX could copy straight to head */
-		page = xen_netbk_alloc_page(netbk, skb, pending_idx);
+		page = xen_netbk_alloc_page(netbk, pending_idx);
 		if (!page) {
 			kfree_skb(skb);
 			netbk_tx_err(vif, &txreq, idx);
@@ -1404,6 +1567,7 @@ static unsigned xen_netbk_tx_build_gops(struct xen_netbk *netbk)
 		memcpy(&netbk->pending_tx_info[pending_idx].req,
 		       &txreq, sizeof(txreq));
 		netbk->pending_tx_info[pending_idx].vif = vif;
+		netbk->pending_tx_info[pending_idx].head = index;
 		*((u16 *)skb->data) = pending_idx;
 
 		__skb_put(skb, data_len);
@@ -1496,6 +1660,7 @@ static void xen_netbk_tx_submit(struct xen_netbk *netbk)
 
 		skb->dev      = vif->dev;
 		skb->protocol = eth_type_trans(skb, skb->dev);
+		skb_reset_network_header(skb);
 
 		if (checksum_setup(vif, skb)) {
 			netdev_dbg(vif->dev,
@@ -1504,6 +1669,8 @@ static void xen_netbk_tx_submit(struct xen_netbk *netbk)
 			continue;
 		}
 
+		skb_probe_transport_header(skb, 0);
+
 		vif->dev->stats.rx_bytes += skb->len;
 		vif->dev->stats.rx_packets++;
 
@@ -1531,7 +1698,10 @@ static void xen_netbk_idx_release(struct xen_netbk *netbk, u16 pending_idx,
 {
 	struct xenvif *vif;
 	struct pending_tx_info *pending_tx_info;
-	pending_ring_idx_t index;
+	pending_ring_idx_t head;
+	u16 peek; /* peek into next tx request */
+
+	BUG_ON(netbk->mmap_pages[pending_idx] == (void *)(~0UL));
 
 	/* Already complete? */
 	if (netbk->mmap_pages[pending_idx] == NULL)
@@ -1540,19 +1710,40 @@ static void xen_netbk_idx_release(struct xen_netbk *netbk, u16 pending_idx,
 	pending_tx_info = &netbk->pending_tx_info[pending_idx];
 
 	vif = pending_tx_info->vif;
+	head = pending_tx_info->head;
 
-	make_tx_response(vif, &pending_tx_info->req, status);
+	BUG_ON(!pending_tx_is_head(netbk, head));
+	BUG_ON(netbk->pending_ring[pending_index(head)] != pending_idx);
 
-	index = pending_index(netbk->pending_prod++);
-	netbk->pending_ring[index] = pending_idx;
+	do {
+		pending_ring_idx_t index;
+		pending_ring_idx_t idx = pending_index(head);
+		u16 info_idx = netbk->pending_ring[idx];
 
-	xenvif_put(vif);
+		pending_tx_info = &netbk->pending_tx_info[info_idx];
+		make_tx_response(vif, &pending_tx_info->req, status);
+
+		/* Setting any number other than
+		 * INVALID_PENDING_RING_IDX indicates this slot is
+		 * starting a new packet / ending a previous packet.
+		 */
+		pending_tx_info->head = 0;
+
+		index = pending_index(netbk->pending_prod++);
+		netbk->pending_ring[index] = netbk->pending_ring[info_idx];
+
+		xenvif_put(vif);
+
+		peek = netbk->pending_ring[pending_index(++head)];
+
+	} while (!pending_tx_is_head(netbk, peek));
 
 	netbk->mmap_pages[pending_idx]->mapping = 0;
 	put_page(netbk->mmap_pages[pending_idx]);
 	netbk->mmap_pages[pending_idx] = NULL;
 }
 
+
 static void make_tx_response(struct xenvif *vif,
 			     struct xen_netif_tx_request *txp,
 			     s8       st)
@@ -1605,8 +1796,9 @@ static inline int rx_work_todo(struct xen_netbk *netbk)
 static inline int tx_work_todo(struct xen_netbk *netbk)
 {
 
-	if (((nr_pending_reqs(netbk) + MAX_SKB_FRAGS) < MAX_PENDING_REQS) &&
-			!list_empty(&netbk->net_schedule_list))
+	if ((nr_pending_reqs(netbk) + XEN_NETBK_LEGACY_SLOTS_MAX
+	     < MAX_PENDING_REQS) &&
+	     !list_empty(&netbk->net_schedule_list))
 		return 1;
 
 	return 0;
@@ -1689,6 +1881,13 @@ static int __init netback_init(void)
 	if (!xen_domain())
 		return -ENODEV;
 
+	if (fatal_skb_slots < XEN_NETBK_LEGACY_SLOTS_MAX) {
+		printk(KERN_INFO
+		       "xen-netback: fatal_skb_slots too small (%d), bump it to XEN_NETBK_LEGACY_SLOTS_MAX (%d)\n",
+		       fatal_skb_slots, XEN_NETBK_LEGACY_SLOTS_MAX);
+		fatal_skb_slots = XEN_NETBK_LEGACY_SLOTS_MAX;
+	}
+
 	xen_netbk_group_nr = num_online_cpus();
 	xen_netbk = vzalloc(sizeof(struct xen_netbk) * xen_netbk_group_nr);
 	if (!xen_netbk)
diff --git a/drivers/net/xen-netfront.c b/drivers/net/xen-netfront.c
index 7ffa43bd7cf9..1db101415069 100644
--- a/drivers/net/xen-netfront.c
+++ b/drivers/net/xen-netfront.c
@@ -36,7 +36,7 @@
 #include <linux/skbuff.h>
 #include <linux/ethtool.h>
 #include <linux/if_ether.h>
-#include <linux/tcp.h>
+#include <net/tcp.h>
 #include <linux/udp.h>
 #include <linux/moduleparam.h>
 #include <linux/mm.h>
@@ -537,7 +537,6 @@ static int xennet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct netfront_info *np = netdev_priv(dev);
 	struct netfront_stats *stats = this_cpu_ptr(np->stats);
 	struct xen_netif_tx_request *tx;
-	struct xen_netif_extra_info *extra;
 	char *data = skb->data;
 	RING_IDX i;
 	grant_ref_t ref;
@@ -548,6 +547,16 @@ static int xennet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	unsigned int len = skb_headlen(skb);
 	unsigned long flags;
 
+	/* If skb->len is too big for wire format, drop skb and alert
+	 * user about misconfiguration.
+	 */
+	if (unlikely(skb->len > XEN_NETIF_MAX_TX_SIZE)) {
+		net_alert_ratelimited(
+			"xennet: skb->len = %u, too big for wire format\n",
+			skb->len);
+		goto drop;
+	}
+
 	slots = DIV_ROUND_UP(offset + len, PAGE_SIZE) +
 		xennet_count_skb_frag_slots(skb);
 	if (unlikely(slots > MAX_SKB_FRAGS + 1)) {
@@ -581,7 +590,6 @@ static int xennet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	tx->gref = np->grant_tx_ref[id] = ref;
 	tx->offset = offset;
 	tx->size = len;
-	extra = NULL;
 
 	tx->flags = 0;
 	if (skb->ip_summed == CHECKSUM_PARTIAL)
@@ -597,10 +605,7 @@ static int xennet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		gso = (struct xen_netif_extra_info *)
 			RING_GET_REQUEST(&np->tx, ++i);
 
-		if (extra)
-			extra->flags |= XEN_NETIF_EXTRA_FLAG_MORE;
-		else
-			tx->flags |= XEN_NETTXF_extra_info;
+		tx->flags |= XEN_NETTXF_extra_info;
 
 		gso->u.gso.size = skb_shinfo(skb)->gso_size;
 		gso->u.gso.type = XEN_NETIF_GSO_TYPE_TCPV4;
@@ -609,7 +614,6 @@ static int xennet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 		gso->type = XEN_NETIF_EXTRA_TYPE_GSO;
 		gso->flags = 0;
-		extra = gso;
 	}
 
 	np->tx.req_prod_pvt = i + 1;
@@ -718,7 +722,7 @@ static int xennet_get_responses(struct netfront_info *np,
 	struct sk_buff *skb = xennet_get_rx_skb(np, cons);
 	grant_ref_t ref = xennet_get_rx_ref(np, cons);
 	int max = MAX_SKB_FRAGS + (rx->status <= RX_COPY_THRESHOLD);
-	int frags = 1;
+	int slots = 1;
 	int err = 0;
 	unsigned long ret;
 
@@ -741,7 +745,7 @@ static int xennet_get_responses(struct netfront_info *np,
 		/*
 		 * This definitely indicates a bug, either in this driver or in
 		 * the backend driver. In future this should flag the bad
-		 * situation to the system controller to reboot the backed.
+		 * situation to the system controller to reboot the backend.
 		 */
 		if (ref == GRANT_INVALID_REF) {
 			if (net_ratelimit())
@@ -762,27 +766,27 @@ next:
 		if (!(rx->flags & XEN_NETRXF_more_data))
 			break;
 
-		if (cons + frags == rp) {
+		if (cons + slots == rp) {
 			if (net_ratelimit())
-				dev_warn(dev, "Need more frags\n");
+				dev_warn(dev, "Need more slots\n");
 			err = -ENOENT;
 			break;
 		}
 
-		rx = RING_GET_RESPONSE(&np->rx, cons + frags);
-		skb = xennet_get_rx_skb(np, cons + frags);
-		ref = xennet_get_rx_ref(np, cons + frags);
-		frags++;
+		rx = RING_GET_RESPONSE(&np->rx, cons + slots);
+		skb = xennet_get_rx_skb(np, cons + slots);
+		ref = xennet_get_rx_ref(np, cons + slots);
+		slots++;
 	}
 
-	if (unlikely(frags > max)) {
+	if (unlikely(slots > max)) {
 		if (net_ratelimit())
-			dev_warn(dev, "Too many frags\n");
+			dev_warn(dev, "Too many slots\n");
 		err = -E2BIG;
 	}
 
 	if (unlikely(err))
-		np->rx.rsp_cons = cons + frags;
+		np->rx.rsp_cons = cons + slots;
 
 	return err;
 }
@@ -1064,7 +1068,8 @@ err:
 
 static int xennet_change_mtu(struct net_device *dev, int mtu)
 {
-	int max = xennet_can_sg(dev) ? 65535 - ETH_HLEN : ETH_DATA_LEN;
+	int max = xennet_can_sg(dev) ?
+		XEN_NETIF_MAX_TX_SIZE - MAX_TCP_HEADER : ETH_DATA_LEN;
 
 	if (mtu > max)
 		return -EINVAL;
@@ -1368,6 +1373,8 @@ static struct net_device *xennet_create_dev(struct xenbus_device *dev)
 	SET_ETHTOOL_OPS(netdev, &xennet_ethtool_ops);
 	SET_NETDEV_DEV(netdev, &dev->dev);
 
+	netif_set_gso_max_size(netdev, XEN_NETIF_MAX_TX_SIZE - MAX_TCP_HEADER);
+
 	np->netdev = netdev;
 
 	netif_carrier_off(netdev);
diff --git a/drivers/nfc/Kconfig b/drivers/nfc/Kconfig
index e57034971ccc..4775d4e61b88 100644
--- a/drivers/nfc/Kconfig
+++ b/drivers/nfc/Kconfig
@@ -26,6 +26,16 @@ config NFC_WILINK
 	  Say Y here to compile support for Texas Instrument's NFC WiLink driver
 	  into the kernel or say M to compile it as module.
 
+config NFC_MEI_PHY
+	tristate "MEI bus NFC device support"
+	depends on INTEL_MEI_BUS_NFC && NFC_HCI
+	help
+	  This adds support to use an mei bus nfc device. Select this if you
+	  will use an HCI NFC driver for an NFC chip connected behind an
+	  Intel's Management Engine chip.
+
+	  If unsure, say N.
+
 source "drivers/nfc/pn544/Kconfig"
 source "drivers/nfc/microread/Kconfig"
 
diff --git a/drivers/nfc/Makefile b/drivers/nfc/Makefile
index a189ada0926a..aa6bd657ef40 100644
--- a/drivers/nfc/Makefile
+++ b/drivers/nfc/Makefile
@@ -6,5 +6,6 @@ obj-$(CONFIG_NFC_PN544)		+= pn544/
 obj-$(CONFIG_NFC_MICROREAD)	+= microread/
 obj-$(CONFIG_NFC_PN533)		+= pn533.o
 obj-$(CONFIG_NFC_WILINK)	+= nfcwilink.o
+obj-$(CONFIG_NFC_MEI_PHY)	+= mei_phy.o
 
 ccflags-$(CONFIG_NFC_DEBUG) := -DDEBUG
diff --git a/drivers/nfc/mei_phy.c b/drivers/nfc/mei_phy.c
new file mode 100644
index 000000000000..b8f8abc422f0
--- /dev/null
+++ b/drivers/nfc/mei_phy.c
@@ -0,0 +1,164 @@
+/*
+ * MEI Library for mei bus nfc device access
+ *
+ * Copyright (C) 2013  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/nfc.h>
+
+#include "mei_phy.h"
+
+struct mei_nfc_hdr {
+	u8 cmd;
+	u8 status;
+	u16 req_id;
+	u32 reserved;
+	u16 data_size;
+} __attribute__((packed));
+
+#define MEI_NFC_MAX_READ (MEI_NFC_HEADER_SIZE + MEI_NFC_MAX_HCI_PAYLOAD)
+
+#define MEI_DUMP_SKB_IN(info, skb)				\
+do {								\
+	pr_debug("%s:\n", info);				\
+	print_hex_dump_debug("mei in : ", DUMP_PREFIX_OFFSET,	\
+			16, 1, (skb)->data, (skb)->len, false);	\
+} while (0)
+
+#define MEI_DUMP_SKB_OUT(info, skb)				\
+do {								\
+	pr_debug("%s:\n", info);				\
+	print_hex_dump_debug("mei out: ", DUMP_PREFIX_OFFSET,	\
+		       16, 1, (skb)->data, (skb)->len, false);	\
+} while (0)
+
+int nfc_mei_phy_enable(void *phy_id)
+{
+	int r;
+	struct nfc_mei_phy *phy = phy_id;
+
+	pr_info("%s\n", __func__);
+
+	if (phy->powered == 1)
+		return 0;
+
+	r = mei_cl_enable_device(phy->device);
+	if (r < 0) {
+                pr_err("MEI_PHY: Could not enable device\n");
+                return r;
+	}
+
+	phy->powered = 1;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(nfc_mei_phy_enable);
+
+void nfc_mei_phy_disable(void *phy_id)
+{
+	struct nfc_mei_phy *phy = phy_id;
+
+	pr_info("%s\n", __func__);
+
+	mei_cl_disable_device(phy->device);
+
+	phy->powered = 0;
+}
+EXPORT_SYMBOL_GPL(nfc_mei_phy_disable);
+
+/*
+ * Writing a frame must not return the number of written bytes.
+ * It must return either zero for success, or <0 for error.
+ * In addition, it must not alter the skb
+ */
+static int nfc_mei_phy_write(void *phy_id, struct sk_buff *skb)
+{
+	struct nfc_mei_phy *phy = phy_id;
+	int r;
+
+	MEI_DUMP_SKB_OUT("mei frame sent", skb);
+
+	r = mei_cl_send(phy->device, skb->data, skb->len);
+	if (r > 0)
+		r = 0;
+
+	return r;
+}
+
+void nfc_mei_event_cb(struct mei_cl_device *device, u32 events, void *context)
+{
+	struct nfc_mei_phy *phy = context;
+
+	if (phy->hard_fault != 0)
+		return;
+
+	if (events & BIT(MEI_CL_EVENT_RX)) {
+		struct sk_buff *skb;
+		int reply_size;
+
+		skb = alloc_skb(MEI_NFC_MAX_READ, GFP_KERNEL);
+		if (!skb)
+			return;
+
+		reply_size = mei_cl_recv(device, skb->data, MEI_NFC_MAX_READ);
+		if (reply_size < MEI_NFC_HEADER_SIZE) {
+			kfree(skb);
+			return;
+		}
+
+		skb_put(skb, reply_size);
+		skb_pull(skb, MEI_NFC_HEADER_SIZE);
+
+		MEI_DUMP_SKB_IN("mei frame read", skb);
+
+		nfc_hci_recv_frame(phy->hdev, skb);
+	}
+}
+EXPORT_SYMBOL_GPL(nfc_mei_event_cb);
+
+struct nfc_phy_ops mei_phy_ops = {
+	.write = nfc_mei_phy_write,
+	.enable = nfc_mei_phy_enable,
+	.disable = nfc_mei_phy_disable,
+};
+EXPORT_SYMBOL_GPL(mei_phy_ops);
+
+struct nfc_mei_phy *nfc_mei_phy_alloc(struct mei_cl_device *device)
+{
+	struct nfc_mei_phy *phy;
+
+	phy = kzalloc(sizeof(struct nfc_mei_phy), GFP_KERNEL);
+	if (!phy)
+		return NULL;
+
+	phy->device = device;
+	mei_cl_set_drvdata(device, phy);
+
+	return phy;
+}
+EXPORT_SYMBOL_GPL(nfc_mei_phy_alloc);
+
+void nfc_mei_phy_free(struct nfc_mei_phy *phy)
+{
+	kfree(phy);
+}
+EXPORT_SYMBOL_GPL(nfc_mei_phy_free);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("mei bus NFC device interface");
diff --git a/drivers/nfc/mei_phy.h b/drivers/nfc/mei_phy.h
new file mode 100644
index 000000000000..d669900f8278
--- /dev/null
+++ b/drivers/nfc/mei_phy.h
@@ -0,0 +1,30 @@
+#ifndef __LOCAL_MEI_PHY_H_
+#define __LOCAL_MEI_PHY_H_
+
+#include <linux/mei_cl_bus.h>
+#include <net/nfc/hci.h>
+
+#define MEI_NFC_HEADER_SIZE 10
+#define MEI_NFC_MAX_HCI_PAYLOAD 300
+
+struct nfc_mei_phy {
+	struct mei_cl_device *device;
+	struct nfc_hci_dev *hdev;
+
+	int powered;
+
+	int hard_fault;		/*
+				 * < 0 if hardware error occured
+				 * and prevents normal operation.
+				 */
+};
+
+extern struct nfc_phy_ops mei_phy_ops;
+
+int nfc_mei_phy_enable(void *phy_id);
+void nfc_mei_phy_disable(void *phy_id);
+void nfc_mei_event_cb(struct mei_cl_device *device, u32 events, void *context);
+struct nfc_mei_phy *nfc_mei_phy_alloc(struct mei_cl_device *device);
+void nfc_mei_phy_free(struct nfc_mei_phy *phy);
+
+#endif /* __LOCAL_MEI_PHY_H_ */
diff --git a/drivers/nfc/microread/Kconfig b/drivers/nfc/microread/Kconfig
index 572305be6e37..951d5542f6bc 100644
--- a/drivers/nfc/microread/Kconfig
+++ b/drivers/nfc/microread/Kconfig
@@ -25,7 +25,7 @@ config NFC_MICROREAD_I2C
 
 config NFC_MICROREAD_MEI
 	tristate "NFC Microread MEI support"
-	depends on NFC_MICROREAD && INTEL_MEI_BUS_NFC
+	depends on NFC_MICROREAD && NFC_MEI_PHY
 	---help---
 	  This module adds support for the mei interface of adapters using
 	  Inside microread chipsets.  Select this if your microread chipset
diff --git a/drivers/nfc/microread/mei.c b/drivers/nfc/microread/mei.c
index ca33ae193935..1ad044dce7b6 100644
--- a/drivers/nfc/microread/mei.c
+++ b/drivers/nfc/microread/mei.c
@@ -19,151 +19,31 @@
  */
 
 #include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/interrupt.h>
-#include <linux/gpio.h>
-#include <linux/mei_cl_bus.h>
-
+#include <linux/mod_devicetable.h>
 #include <linux/nfc.h>
 #include <net/nfc/hci.h>
 #include <net/nfc/llc.h>
 
+#include "../mei_phy.h"
 #include "microread.h"
 
 #define MICROREAD_DRIVER_NAME "microread"
 
-struct mei_nfc_hdr {
-	u8 cmd;
-	u8 status;
-	u16 req_id;
-	u32 reserved;
-	u16 data_size;
-} __attribute__((packed));
-
-#define MEI_NFC_HEADER_SIZE 10
-#define MEI_NFC_MAX_HCI_PAYLOAD 300
-#define MEI_NFC_MAX_READ (MEI_NFC_HEADER_SIZE + MEI_NFC_MAX_HCI_PAYLOAD)
-
-struct microread_mei_phy {
-	struct mei_cl_device *device;
-	struct nfc_hci_dev *hdev;
-
-	int powered;
-
-	int hard_fault;		/*
-				 * < 0 if hardware error occured (e.g. i2c err)
-				 * and prevents normal operation.
-				 */
-};
-
-#define MEI_DUMP_SKB_IN(info, skb)					\
-do {								\
-	pr_debug("%s:\n", info);				\
-	print_hex_dump(KERN_DEBUG, "mei in : ", DUMP_PREFIX_OFFSET,	\
-		       16, 1, (skb)->data, (skb)->len, 0);	\
-} while (0)
-
-#define MEI_DUMP_SKB_OUT(info, skb)					\
-do {								\
-	pr_debug("%s:\n", info);				\
-	print_hex_dump(KERN_DEBUG, "mei out: ", DUMP_PREFIX_OFFSET,	\
-		       16, 1, (skb)->data, (skb)->len, 0);	\
-} while (0)
-
-static int microread_mei_enable(void *phy_id)
-{
-	struct microread_mei_phy *phy = phy_id;
-
-	pr_info(DRIVER_DESC ": %s\n", __func__);
-
-	phy->powered = 1;
-
-	return 0;
-}
-
-static void microread_mei_disable(void *phy_id)
-{
-	struct microread_mei_phy *phy = phy_id;
-
-	pr_info(DRIVER_DESC ": %s\n", __func__);
-
-	phy->powered = 0;
-}
-
-/*
- * Writing a frame must not return the number of written bytes.
- * It must return either zero for success, or <0 for error.
- * In addition, it must not alter the skb
- */
-static int microread_mei_write(void *phy_id, struct sk_buff *skb)
-{
-	struct microread_mei_phy *phy = phy_id;
-	int r;
-
-	MEI_DUMP_SKB_OUT("mei frame sent", skb);
-
-	r = mei_cl_send(phy->device, skb->data, skb->len);
-	if (r > 0)
-		r = 0;
-
-	return r;
-}
-
-static void microread_event_cb(struct mei_cl_device *device, u32 events,
-			       void *context)
-{
-	struct microread_mei_phy *phy = context;
-
-	if (phy->hard_fault != 0)
-		return;
-
-	if (events & BIT(MEI_CL_EVENT_RX)) {
-		struct sk_buff *skb;
-		int reply_size;
-
-		skb = alloc_skb(MEI_NFC_MAX_READ, GFP_KERNEL);
-		if (!skb)
-			return;
-
-		reply_size = mei_cl_recv(device, skb->data, MEI_NFC_MAX_READ);
-		if (reply_size < MEI_NFC_HEADER_SIZE) {
-			kfree(skb);
-			return;
-		}
-
-		skb_put(skb, reply_size);
-		skb_pull(skb, MEI_NFC_HEADER_SIZE);
-
-		MEI_DUMP_SKB_IN("mei frame read", skb);
-
-		nfc_hci_recv_frame(phy->hdev, skb);
-	}
-}
-
-static struct nfc_phy_ops mei_phy_ops = {
-	.write = microread_mei_write,
-	.enable = microread_mei_enable,
-	.disable = microread_mei_disable,
-};
-
 static int microread_mei_probe(struct mei_cl_device *device,
 			       const struct mei_cl_device_id *id)
 {
-	struct microread_mei_phy *phy;
+	struct nfc_mei_phy *phy;
 	int r;
 
 	pr_info("Probing NFC microread\n");
 
-	phy = kzalloc(sizeof(struct microread_mei_phy), GFP_KERNEL);
+	phy = nfc_mei_phy_alloc(device);
 	if (!phy) {
 		pr_err("Cannot allocate memory for microread mei phy.\n");
 		return -ENOMEM;
 	}
 
-	phy->device = device;
-	mei_cl_set_drvdata(device, phy);
-
-	r = mei_cl_register_event_cb(device, microread_event_cb, phy);
+	r = mei_cl_register_event_cb(device, nfc_mei_event_cb, phy);
 	if (r) {
 		pr_err(MICROREAD_DRIVER_NAME ": event cb registration failed\n");
 		goto err_out;
@@ -178,23 +58,22 @@ static int microread_mei_probe(struct mei_cl_device *device,
 	return 0;
 
 err_out:
-	kfree(phy);
+	nfc_mei_phy_free(phy);
 
 	return r;
 }
 
 static int microread_mei_remove(struct mei_cl_device *device)
 {
-	struct microread_mei_phy *phy = mei_cl_get_drvdata(device);
+	struct nfc_mei_phy *phy = mei_cl_get_drvdata(device);
 
 	pr_info("Removing microread\n");
 
 	microread_remove(phy->hdev);
 
-	if (phy->powered)
-		microread_mei_disable(phy);
+	nfc_mei_phy_disable(phy);
 
-	kfree(phy);
+	nfc_mei_phy_free(phy);
 
 	return 0;
 }
diff --git a/drivers/nfc/pn533.c b/drivers/nfc/pn533.c
index f0f6763d67ae..8f6f2baa930d 100644
--- a/drivers/nfc/pn533.c
+++ b/drivers/nfc/pn533.c
@@ -1,9 +1,6 @@
 /*
  * Copyright (C) 2011 Instituto Nokia de Tecnologia
- *
- * Authors:
- *    Lauro Ramos Venancio <lauro.venancio@openbossa.org>
- *    Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
+ * Copyright (C) 2012-2013 Tieto Poland
  *
  * 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
@@ -30,7 +27,7 @@
 #include <linux/netdevice.h>
 #include <net/nfc/nfc.h>
 
-#define VERSION "0.1"
+#define VERSION "0.2"
 
 #define PN533_VENDOR_ID 0x4CC
 #define PN533_PRODUCT_ID 0x2533
@@ -41,8 +38,12 @@
 #define SONY_VENDOR_ID         0x054c
 #define PASORI_PRODUCT_ID      0x02e1
 
-#define PN533_DEVICE_STD    0x1
-#define PN533_DEVICE_PASORI 0x2
+#define ACS_VENDOR_ID 0x072f
+#define ACR122U_PRODUCT_ID 0x2200
+
+#define PN533_DEVICE_STD     0x1
+#define PN533_DEVICE_PASORI  0x2
+#define PN533_DEVICE_ACR122U 0x3
 
 #define PN533_ALL_PROTOCOLS (NFC_PROTO_JEWEL_MASK | NFC_PROTO_MIFARE_MASK |\
 			     NFC_PROTO_FELICA_MASK | NFC_PROTO_ISO14443_MASK |\
@@ -71,6 +72,11 @@ static const struct usb_device_id pn533_table[] = {
 	  .idProduct		= PASORI_PRODUCT_ID,
 	  .driver_info		= PN533_DEVICE_PASORI,
 	},
+	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE,
+	  .idVendor		= ACS_VENDOR_ID,
+	  .idProduct		= ACR122U_PRODUCT_ID,
+	  .driver_info		= PN533_DEVICE_ACR122U,
+	},
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, pn533_table);
@@ -78,32 +84,47 @@ MODULE_DEVICE_TABLE(usb, pn533_table);
 /* How much time we spend listening for initiators */
 #define PN533_LISTEN_TIME 2
 
-/* frame definitions */
-#define PN533_FRAME_HEADER_LEN (sizeof(struct pn533_frame) \
+/* Standard pn533 frame definitions */
+#define PN533_STD_FRAME_HEADER_LEN (sizeof(struct pn533_std_frame) \
 					+ 2) /* data[0] TFI, data[1] CC */
-#define PN533_FRAME_TAIL_LEN 2 /* data[len] DCS, data[len + 1] postamble*/
+#define PN533_STD_FRAME_TAIL_LEN 2 /* data[len] DCS, data[len + 1] postamble*/
 
 /*
  * Max extended frame payload len, excluding TFI and CC
  * which are already in PN533_FRAME_HEADER_LEN.
  */
-#define PN533_FRAME_MAX_PAYLOAD_LEN 263
+#define PN533_STD_FRAME_MAX_PAYLOAD_LEN 263
 
-#define PN533_FRAME_ACK_SIZE 6 /* Preamble (1), SoPC (2), ACK Code (2),
+#define PN533_STD_FRAME_ACK_SIZE 6 /* Preamble (1), SoPC (2), ACK Code (2),
 				  Postamble (1) */
-#define PN533_FRAME_CHECKSUM(f) (f->data[f->datalen])
-#define PN533_FRAME_POSTAMBLE(f) (f->data[f->datalen + 1])
+#define PN533_STD_FRAME_CHECKSUM(f) (f->data[f->datalen])
+#define PN533_STD_FRAME_POSTAMBLE(f) (f->data[f->datalen + 1])
 
 /* start of frame */
-#define PN533_SOF 0x00FF
+#define PN533_STD_FRAME_SOF 0x00FF
+
+/* standard frame identifier: in/out/error */
+#define PN533_STD_FRAME_IDENTIFIER(f) (f->data[0]) /* TFI */
+#define PN533_STD_FRAME_DIR_OUT 0xD4
+#define PN533_STD_FRAME_DIR_IN 0xD5
+
+/* ACS ACR122 pn533 frame definitions */
+#define PN533_ACR122_TX_FRAME_HEADER_LEN (sizeof(struct pn533_acr122_tx_frame) \
+					  + 2)
+#define PN533_ACR122_TX_FRAME_TAIL_LEN 0
+#define PN533_ACR122_RX_FRAME_HEADER_LEN (sizeof(struct pn533_acr122_rx_frame) \
+					  + 2)
+#define PN533_ACR122_RX_FRAME_TAIL_LEN 2
+#define PN533_ACR122_FRAME_MAX_PAYLOAD_LEN PN533_STD_FRAME_MAX_PAYLOAD_LEN
+
+/* CCID messages types */
+#define PN533_ACR122_PC_TO_RDR_ICCPOWERON 0x62
+#define PN533_ACR122_PC_TO_RDR_ESCAPE 0x6B
 
-/* frame identifier: in/out/error */
-#define PN533_FRAME_IDENTIFIER(f) (f->data[0])
-#define PN533_DIR_OUT 0xD4
-#define PN533_DIR_IN 0xD5
+#define PN533_ACR122_RDR_TO_PC_ESCAPE 0x83
 
 /* PN533 Commands */
-#define PN533_FRAME_CMD(f) (f->data[1])
+#define PN533_STD_FRAME_CMD(f) (f->data[1])
 
 #define PN533_CMD_GET_FIRMWARE_VERSION 0x02
 #define PN533_CMD_RF_CONFIGURATION 0x32
@@ -128,8 +149,6 @@ MODULE_DEVICE_TABLE(usb, pn533_table);
 
 struct pn533;
 
-typedef int (*pn533_cmd_complete_t) (struct pn533 *dev, void *arg, int status);
-
 typedef int (*pn533_send_async_complete_t) (struct pn533 *dev, void *arg,
 					struct sk_buff *resp);
 
@@ -144,9 +163,13 @@ struct pn533_fw_version {
 };
 
 /* PN533_CMD_RF_CONFIGURATION */
-#define PN533_CFGITEM_TIMING 0x02
+#define PN533_CFGITEM_RF_FIELD    0x01
+#define PN533_CFGITEM_TIMING      0x02
 #define PN533_CFGITEM_MAX_RETRIES 0x05
-#define PN533_CFGITEM_PASORI 0x82
+#define PN533_CFGITEM_PASORI      0x82
+
+#define PN533_CFGITEM_RF_FIELD_ON  0x1
+#define PN533_CFGITEM_RF_FIELD_OFF 0x0
 
 #define PN533_CONFIG_TIMING_102 0xb
 #define PN533_CONFIG_TIMING_204 0xc
@@ -313,10 +336,17 @@ struct pn533_cmd_jump_dep_response {
 #define PN533_INIT_TARGET_RESP_ACTIVE     0x1
 #define PN533_INIT_TARGET_RESP_DEP        0x4
 
+enum  pn533_protocol_type {
+	PN533_PROTO_REQ_ACK_RESP = 0,
+	PN533_PROTO_REQ_RESP
+};
+
 struct pn533 {
 	struct usb_device *udev;
 	struct usb_interface *interface;
 	struct nfc_dev *nfc_dev;
+	u32 device_type;
+	enum pn533_protocol_type protocol_type;
 
 	struct urb *out_urb;
 	struct urb *in_urb;
@@ -329,21 +359,21 @@ struct pn533 {
 	struct work_struct poll_work;
 	struct work_struct mi_work;
 	struct work_struct tg_work;
-	struct timer_list listen_timer;
-	int wq_in_error;
-	int cancel_listen;
 
-	pn533_cmd_complete_t cmd_complete;
-	void *cmd_complete_arg;
+	struct list_head cmd_queue;
+	struct pn533_cmd *cmd;
+	u8 cmd_pending;
+	struct mutex cmd_lock;  /* protects cmd queue */
+
 	void *cmd_complete_mi_arg;
-	struct mutex cmd_lock;
-	u8 cmd;
 
 	struct pn533_poll_modulations *poll_mod_active[PN533_POLL_MOD_MAX + 1];
 	u8 poll_mod_count;
 	u8 poll_mod_curr;
 	u32 poll_protocols;
 	u32 listen_protocols;
+	struct timer_list listen_timer;
+	int cancel_listen;
 
 	u8 *gb;
 	size_t gb_len;
@@ -352,24 +382,21 @@ struct pn533 {
 	u8 tgt_active_prot;
 	u8 tgt_mode;
 
-	u32 device_type;
-
-	struct list_head cmd_queue;
-	u8 cmd_pending;
-
 	struct pn533_frame_ops *ops;
 };
 
 struct pn533_cmd {
 	struct list_head queue;
-	u8 cmd_code;
+	u8 code;
+	int status;
 	struct sk_buff *req;
 	struct sk_buff *resp;
 	int resp_len;
-	void *arg;
+	pn533_send_async_complete_t  complete_cb;
+	void *complete_cb_context;
 };
 
-struct pn533_frame {
+struct pn533_std_frame {
 	u8 preamble;
 	__be16 start_frame;
 	u8 datalen;
@@ -393,14 +420,124 @@ struct pn533_frame_ops {
 	u8 (*get_cmd_code)(void *frame);
 };
 
+struct pn533_acr122_ccid_hdr {
+	u8 type;
+	u32 datalen;
+	u8 slot;
+	u8 seq;
+	u8 params[3]; /* 3 msg specific bytes or status, error and 1 specific
+			 byte for reposnse msg */
+	u8 data[]; /* payload */
+} __packed;
+
+struct pn533_acr122_apdu_hdr {
+	u8 class;
+	u8 ins;
+	u8 p1;
+	u8 p2;
+} __packed;
+
+struct pn533_acr122_tx_frame {
+	struct pn533_acr122_ccid_hdr ccid;
+	struct pn533_acr122_apdu_hdr apdu;
+	u8 datalen;
+	u8 data[]; /* pn533 frame: TFI ... */
+} __packed;
+
+struct pn533_acr122_rx_frame {
+	struct pn533_acr122_ccid_hdr ccid;
+	u8 data[]; /* pn533 frame : TFI ... */
+} __packed;
+
+static void pn533_acr122_tx_frame_init(void *_frame, u8 cmd_code)
+{
+	struct pn533_acr122_tx_frame *frame = _frame;
+
+	frame->ccid.type = PN533_ACR122_PC_TO_RDR_ESCAPE;
+	frame->ccid.datalen = sizeof(frame->apdu) + 1; /* sizeof(apdu_hdr) +
+							  sizeof(datalen) */
+	frame->ccid.slot = 0;
+	frame->ccid.seq = 0;
+	frame->ccid.params[0] = 0;
+	frame->ccid.params[1] = 0;
+	frame->ccid.params[2] = 0;
+
+	frame->data[0] = PN533_STD_FRAME_DIR_OUT;
+	frame->data[1] = cmd_code;
+	frame->datalen = 2;  /* data[0] + data[1] */
+
+	frame->apdu.class = 0xFF;
+	frame->apdu.ins = 0;
+	frame->apdu.p1 = 0;
+	frame->apdu.p2 = 0;
+}
+
+static void pn533_acr122_tx_frame_finish(void *_frame)
+{
+	struct pn533_acr122_tx_frame *frame = _frame;
+
+	frame->ccid.datalen += frame->datalen;
+}
+
+static void pn533_acr122_tx_update_payload_len(void *_frame, int len)
+{
+	struct pn533_acr122_tx_frame *frame = _frame;
+
+	frame->datalen += len;
+}
+
+static bool pn533_acr122_is_rx_frame_valid(void *_frame)
+{
+	struct pn533_acr122_rx_frame *frame = _frame;
+
+	if (frame->ccid.type != 0x83)
+		return false;
+
+	if (frame->data[frame->ccid.datalen - 2] == 0x63)
+		return false;
+
+	return true;
+}
+
+static int pn533_acr122_rx_frame_size(void *frame)
+{
+	struct pn533_acr122_rx_frame *f = frame;
+
+	/* f->ccid.datalen already includes tail length */
+	return sizeof(struct pn533_acr122_rx_frame) + f->ccid.datalen;
+}
+
+static u8 pn533_acr122_get_cmd_code(void *frame)
+{
+	struct pn533_acr122_rx_frame *f = frame;
+
+	return PN533_STD_FRAME_CMD(f);
+}
+
+static struct pn533_frame_ops pn533_acr122_frame_ops = {
+	.tx_frame_init = pn533_acr122_tx_frame_init,
+	.tx_frame_finish = pn533_acr122_tx_frame_finish,
+	.tx_update_payload_len = pn533_acr122_tx_update_payload_len,
+	.tx_header_len = PN533_ACR122_TX_FRAME_HEADER_LEN,
+	.tx_tail_len = PN533_ACR122_TX_FRAME_TAIL_LEN,
+
+	.rx_is_frame_valid = pn533_acr122_is_rx_frame_valid,
+	.rx_header_len = PN533_ACR122_RX_FRAME_HEADER_LEN,
+	.rx_tail_len = PN533_ACR122_RX_FRAME_TAIL_LEN,
+	.rx_frame_size = pn533_acr122_rx_frame_size,
+
+	.max_payload_len = PN533_ACR122_FRAME_MAX_PAYLOAD_LEN,
+	.get_cmd_code = pn533_acr122_get_cmd_code,
+};
+
 /* The rule: value + checksum = 0 */
-static inline u8 pn533_checksum(u8 value)
+static inline u8 pn533_std_checksum(u8 value)
 {
 	return ~value + 1;
 }
 
 /* The rule: sum(data elements) + checksum = 0 */
-static u8 pn533_data_checksum(u8 *data, int datalen)
+static u8 pn533_std_data_checksum(u8 *data, int datalen)
 {
 	u8 sum = 0;
 	int i;
@@ -408,61 +545,61 @@ static u8 pn533_data_checksum(u8 *data, int datalen)
 	for (i = 0; i < datalen; i++)
 		sum += data[i];
 
-	return pn533_checksum(sum);
+	return pn533_std_checksum(sum);
 }
 
-static void pn533_tx_frame_init(void *_frame, u8 cmd_code)
+static void pn533_std_tx_frame_init(void *_frame, u8 cmd_code)
 {
-	struct pn533_frame *frame = _frame;
+	struct pn533_std_frame *frame = _frame;
 
 	frame->preamble = 0;
-	frame->start_frame = cpu_to_be16(PN533_SOF);
-	PN533_FRAME_IDENTIFIER(frame) = PN533_DIR_OUT;
-	PN533_FRAME_CMD(frame) = cmd_code;
+	frame->start_frame = cpu_to_be16(PN533_STD_FRAME_SOF);
+	PN533_STD_FRAME_IDENTIFIER(frame) = PN533_STD_FRAME_DIR_OUT;
+	PN533_STD_FRAME_CMD(frame) = cmd_code;
 	frame->datalen = 2;
 }
 
-static void pn533_tx_frame_finish(void *_frame)
+static void pn533_std_tx_frame_finish(void *_frame)
 {
-	struct pn533_frame *frame = _frame;
+	struct pn533_std_frame *frame = _frame;
 
-	frame->datalen_checksum = pn533_checksum(frame->datalen);
+	frame->datalen_checksum = pn533_std_checksum(frame->datalen);
 
-	PN533_FRAME_CHECKSUM(frame) =
-		pn533_data_checksum(frame->data, frame->datalen);
+	PN533_STD_FRAME_CHECKSUM(frame) =
+		pn533_std_data_checksum(frame->data, frame->datalen);
 
-	PN533_FRAME_POSTAMBLE(frame) = 0;
+	PN533_STD_FRAME_POSTAMBLE(frame) = 0;
 }
 
-static void pn533_tx_update_payload_len(void *_frame, int len)
+static void pn533_std_tx_update_payload_len(void *_frame, int len)
 {
-	struct pn533_frame *frame = _frame;
+	struct pn533_std_frame *frame = _frame;
 
 	frame->datalen += len;
 }
 
-static bool pn533_rx_frame_is_valid(void *_frame)
+static bool pn533_std_rx_frame_is_valid(void *_frame)
 {
 	u8 checksum;
-	struct pn533_frame *frame = _frame;
+	struct pn533_std_frame *frame = _frame;
 
-	if (frame->start_frame != cpu_to_be16(PN533_SOF))
+	if (frame->start_frame != cpu_to_be16(PN533_STD_FRAME_SOF))
 		return false;
 
-	checksum = pn533_checksum(frame->datalen);
+	checksum = pn533_std_checksum(frame->datalen);
 	if (checksum != frame->datalen_checksum)
 		return false;
 
-	checksum = pn533_data_checksum(frame->data, frame->datalen);
-	if (checksum != PN533_FRAME_CHECKSUM(frame))
+	checksum = pn533_std_data_checksum(frame->data, frame->datalen);
+	if (checksum != PN533_STD_FRAME_CHECKSUM(frame))
 		return false;
 
 	return true;
 }
 
-static bool pn533_rx_frame_is_ack(struct pn533_frame *frame)
+static bool pn533_std_rx_frame_is_ack(struct pn533_std_frame *frame)
 {
-	if (frame->start_frame != cpu_to_be16(PN533_SOF))
+	if (frame->start_frame != cpu_to_be16(PN533_STD_FRAME_SOF))
 		return false;
 
 	if (frame->datalen != 0 || frame->datalen_checksum != 0xFF)
@@ -471,57 +608,51 @@ static bool pn533_rx_frame_is_ack(struct pn533_frame *frame)
 	return true;
 }
 
-static inline int pn533_rx_frame_size(void *frame)
+static inline int pn533_std_rx_frame_size(void *frame)
 {
-	struct pn533_frame *f = frame;
+	struct pn533_std_frame *f = frame;
 
-	return sizeof(struct pn533_frame) + f->datalen + PN533_FRAME_TAIL_LEN;
+	return sizeof(struct pn533_std_frame) + f->datalen +
+	       PN533_STD_FRAME_TAIL_LEN;
 }
 
-static u8 pn533_get_cmd_code(void *frame)
+static u8 pn533_std_get_cmd_code(void *frame)
 {
-	struct pn533_frame *f = frame;
+	struct pn533_std_frame *f = frame;
 
-	return PN533_FRAME_CMD(f);
+	return PN533_STD_FRAME_CMD(f);
 }
 
 static struct pn533_frame_ops pn533_std_frame_ops = {
-	.tx_frame_init = pn533_tx_frame_init,
-	.tx_frame_finish = pn533_tx_frame_finish,
-	.tx_update_payload_len = pn533_tx_update_payload_len,
-	.tx_header_len = PN533_FRAME_HEADER_LEN,
-	.tx_tail_len = PN533_FRAME_TAIL_LEN,
-
-	.rx_is_frame_valid = pn533_rx_frame_is_valid,
-	.rx_frame_size = pn533_rx_frame_size,
-	.rx_header_len = PN533_FRAME_HEADER_LEN,
-	.rx_tail_len = PN533_FRAME_TAIL_LEN,
-
-	.max_payload_len =  PN533_FRAME_MAX_PAYLOAD_LEN,
-	.get_cmd_code = pn533_get_cmd_code,
+	.tx_frame_init = pn533_std_tx_frame_init,
+	.tx_frame_finish = pn533_std_tx_frame_finish,
+	.tx_update_payload_len = pn533_std_tx_update_payload_len,
+	.tx_header_len = PN533_STD_FRAME_HEADER_LEN,
+	.tx_tail_len = PN533_STD_FRAME_TAIL_LEN,
+
+	.rx_is_frame_valid = pn533_std_rx_frame_is_valid,
+	.rx_frame_size = pn533_std_rx_frame_size,
+	.rx_header_len = PN533_STD_FRAME_HEADER_LEN,
+	.rx_tail_len = PN533_STD_FRAME_TAIL_LEN,
+
+	.max_payload_len =  PN533_STD_FRAME_MAX_PAYLOAD_LEN,
+	.get_cmd_code = pn533_std_get_cmd_code,
 };
 
 static bool pn533_rx_frame_is_cmd_response(struct pn533 *dev, void *frame)
 {
-	return (dev->ops->get_cmd_code(frame) == PN533_CMD_RESPONSE(dev->cmd));
-}
-
-
-static void pn533_wq_cmd_complete(struct work_struct *work)
-{
-	struct pn533 *dev = container_of(work, struct pn533, cmd_complete_work);
-	int rc;
-
-	rc = dev->cmd_complete(dev, dev->cmd_complete_arg, dev->wq_in_error);
-	if (rc != -EINPROGRESS)
-		queue_work(dev->wq, &dev->cmd_work);
+	return (dev->ops->get_cmd_code(frame) ==
+				PN533_CMD_RESPONSE(dev->cmd->code));
 }
 
 static void pn533_recv_response(struct urb *urb)
 {
 	struct pn533 *dev = urb->context;
+	struct pn533_cmd *cmd = dev->cmd;
 	u8 *in_frame;
 
+	cmd->status = urb->status;
+
 	switch (urb->status) {
 	case 0:
 		break; /* success */
@@ -530,37 +661,33 @@ static void pn533_recv_response(struct urb *urb)
 		nfc_dev_dbg(&dev->interface->dev,
 			    "The urb has been canceled (status %d)",
 			    urb->status);
-		dev->wq_in_error = urb->status;
 		goto sched_wq;
 	case -ESHUTDOWN:
 	default:
 		nfc_dev_err(&dev->interface->dev,
 			    "Urb failure (status %d)", urb->status);
-		dev->wq_in_error = urb->status;
 		goto sched_wq;
 	}
 
 	in_frame = dev->in_urb->transfer_buffer;
 
 	nfc_dev_dbg(&dev->interface->dev, "Received a frame.");
-	print_hex_dump(KERN_DEBUG, "PN533 RX: ", DUMP_PREFIX_NONE, 16, 1,
-		       in_frame, dev->ops->rx_frame_size(in_frame), false);
+	print_hex_dump_debug("PN533 RX: ", DUMP_PREFIX_NONE, 16, 1, in_frame,
+			     dev->ops->rx_frame_size(in_frame), false);
 
 	if (!dev->ops->rx_is_frame_valid(in_frame)) {
 		nfc_dev_err(&dev->interface->dev, "Received an invalid frame");
-		dev->wq_in_error = -EIO;
+		cmd->status = -EIO;
 		goto sched_wq;
 	}
 
 	if (!pn533_rx_frame_is_cmd_response(dev, in_frame)) {
 		nfc_dev_err(&dev->interface->dev,
 			    "It it not the response to the last command");
-		dev->wq_in_error = -EIO;
+		cmd->status = -EIO;
 		goto sched_wq;
 	}
 
-	dev->wq_in_error = 0;
-
 sched_wq:
 	queue_work(dev->wq, &dev->cmd_complete_work);
 }
@@ -575,9 +702,12 @@ static int pn533_submit_urb_for_response(struct pn533 *dev, gfp_t flags)
 static void pn533_recv_ack(struct urb *urb)
 {
 	struct pn533 *dev = urb->context;
-	struct pn533_frame *in_frame;
+	struct pn533_cmd *cmd = dev->cmd;
+	struct pn533_std_frame *in_frame;
 	int rc;
 
+	cmd->status = urb->status;
+
 	switch (urb->status) {
 	case 0:
 		break; /* success */
@@ -586,21 +716,19 @@ static void pn533_recv_ack(struct urb *urb)
 		nfc_dev_dbg(&dev->interface->dev,
 			    "The urb has been stopped (status %d)",
 			    urb->status);
-		dev->wq_in_error = urb->status;
 		goto sched_wq;
 	case -ESHUTDOWN:
 	default:
 		nfc_dev_err(&dev->interface->dev,
 			    "Urb failure (status %d)", urb->status);
-		dev->wq_in_error = urb->status;
 		goto sched_wq;
 	}
 
 	in_frame = dev->in_urb->transfer_buffer;
 
-	if (!pn533_rx_frame_is_ack(in_frame)) {
+	if (!pn533_std_rx_frame_is_ack(in_frame)) {
 		nfc_dev_err(&dev->interface->dev, "Received an invalid ack");
-		dev->wq_in_error = -EIO;
+		cmd->status = -EIO;
 		goto sched_wq;
 	}
 
@@ -608,7 +736,7 @@ static void pn533_recv_ack(struct urb *urb)
 	if (rc) {
 		nfc_dev_err(&dev->interface->dev,
 			    "usb_submit_urb failed with result %d", rc);
-		dev->wq_in_error = rc;
+		cmd->status = rc;
 		goto sched_wq;
 	}
 
@@ -627,7 +755,7 @@ static int pn533_submit_urb_for_ack(struct pn533 *dev, gfp_t flags)
 
 static int pn533_send_ack(struct pn533 *dev, gfp_t flags)
 {
-	u8 ack[PN533_FRAME_ACK_SIZE] = {0x00, 0x00, 0xff, 0x00, 0xff, 0x00};
+	u8 ack[PN533_STD_FRAME_ACK_SIZE] = {0x00, 0x00, 0xff, 0x00, 0xff, 0x00};
 	/* spec 7.1.1.3:  Preamble, SoPC (2), ACK Code (2), Postamble */
 	int rc;
 
@@ -643,32 +771,34 @@ static int pn533_send_ack(struct pn533 *dev, gfp_t flags)
 static int __pn533_send_frame_async(struct pn533 *dev,
 					struct sk_buff *out,
 					struct sk_buff *in,
-					int in_len,
-					pn533_cmd_complete_t cmd_complete,
-					void *arg)
+					int in_len)
 {
 	int rc;
 
-	dev->cmd = dev->ops->get_cmd_code(out->data);
-	dev->cmd_complete = cmd_complete;
-	dev->cmd_complete_arg = arg;
-
 	dev->out_urb->transfer_buffer = out->data;
 	dev->out_urb->transfer_buffer_length = out->len;
 
 	dev->in_urb->transfer_buffer = in->data;
 	dev->in_urb->transfer_buffer_length = in_len;
 
-	print_hex_dump(KERN_DEBUG, "PN533 TX: ", DUMP_PREFIX_NONE, 16, 1,
-		       out->data, out->len, false);
+	print_hex_dump_debug("PN533 TX: ", DUMP_PREFIX_NONE, 16, 1,
+			     out->data, out->len, false);
 
 	rc = usb_submit_urb(dev->out_urb, GFP_KERNEL);
 	if (rc)
 		return rc;
 
-	rc = pn533_submit_urb_for_ack(dev, GFP_KERNEL);
-	if (rc)
-		goto error;
+	if (dev->protocol_type == PN533_PROTO_REQ_RESP) {
+		/* request for response for sent packet directly */
+		rc = pn533_submit_urb_for_response(dev, GFP_ATOMIC);
+		if (rc)
+			goto error;
+	} else if (dev->protocol_type == PN533_PROTO_REQ_ACK_RESP) {
+		/* request for ACK if that's the case */
+		rc = pn533_submit_urb_for_ack(dev, GFP_KERNEL);
+		if (rc)
+			goto error;
+	}
 
 	return 0;
 
@@ -693,39 +823,34 @@ static void pn533_build_cmd_frame(struct pn533 *dev, u8 cmd_code,
 	ops->tx_frame_finish(skb->data);
 }
 
-struct pn533_send_async_complete_arg {
-	pn533_send_async_complete_t  complete_cb;
-	void *complete_cb_context;
-	struct sk_buff *resp;
-	struct sk_buff *req;
-};
-
-static int pn533_send_async_complete(struct pn533 *dev, void *_arg, int status)
+static int pn533_send_async_complete(struct pn533 *dev)
 {
-	struct pn533_send_async_complete_arg *arg = _arg;
+	struct pn533_cmd *cmd = dev->cmd;
+	int status = cmd->status;
 
-	struct sk_buff *req = arg->req;
-	struct sk_buff *resp = arg->resp;
+	struct sk_buff *req = cmd->req;
+	struct sk_buff *resp = cmd->resp;
 
 	int rc;
 
 	dev_kfree_skb(req);
 
 	if (status < 0) {
-		arg->complete_cb(dev, arg->complete_cb_context,
-				 ERR_PTR(status));
+		rc = cmd->complete_cb(dev, cmd->complete_cb_context,
+				      ERR_PTR(status));
 		dev_kfree_skb(resp);
-		kfree(arg);
-		return status;
+		goto done;
 	}
 
 	skb_put(resp, dev->ops->rx_frame_size(resp->data));
 	skb_pull(resp, dev->ops->rx_header_len);
 	skb_trim(resp, resp->len - dev->ops->rx_tail_len);
 
-	rc = arg->complete_cb(dev, arg->complete_cb_context, resp);
+	rc = cmd->complete_cb(dev, cmd->complete_cb_context, resp);
 
-	kfree(arg);
+done:
+	kfree(cmd);
+	dev->cmd = NULL;
 	return rc;
 }
 
@@ -736,56 +861,45 @@ static int __pn533_send_async(struct pn533 *dev, u8 cmd_code,
 			      void *complete_cb_context)
 {
 	struct pn533_cmd *cmd;
-	struct pn533_send_async_complete_arg *arg;
 	int rc = 0;
 
 	nfc_dev_dbg(&dev->interface->dev, "Sending command 0x%x", cmd_code);
 
-	arg = kzalloc(sizeof(*arg), GFP_KERNEL);
-	if (!arg)
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (!cmd)
 		return -ENOMEM;
 
-	arg->complete_cb = complete_cb;
-	arg->complete_cb_context = complete_cb_context;
-	arg->resp = resp;
-	arg->req = req;
+	cmd->code = cmd_code;
+	cmd->req = req;
+	cmd->resp = resp;
+	cmd->resp_len = resp_len;
+	cmd->complete_cb = complete_cb;
+	cmd->complete_cb_context = complete_cb_context;
 
 	pn533_build_cmd_frame(dev, cmd_code, req);
 
 	mutex_lock(&dev->cmd_lock);
 
 	if (!dev->cmd_pending) {
-		rc = __pn533_send_frame_async(dev, req, resp, resp_len,
-					      pn533_send_async_complete, arg);
+		rc = __pn533_send_frame_async(dev, req, resp, resp_len);
 		if (rc)
 			goto error;
 
 		dev->cmd_pending = 1;
+		dev->cmd = cmd;
 		goto unlock;
 	}
 
 	nfc_dev_dbg(&dev->interface->dev, "%s Queueing command 0x%x", __func__,
 		    cmd_code);
 
-	cmd = kzalloc(sizeof(struct pn533_cmd), GFP_KERNEL);
-	if (!cmd) {
-		rc = -ENOMEM;
-		goto error;
-	}
-
 	INIT_LIST_HEAD(&cmd->queue);
-	cmd->cmd_code = cmd_code;
-	cmd->req = req;
-	cmd->resp = resp;
-	cmd->resp_len = resp_len;
-	cmd->arg = arg;
-
 	list_add_tail(&cmd->queue, &dev->cmd_queue);
 
 	goto unlock;
 
 error:
-	kfree(arg);
+	kfree(cmd);
 unlock:
 	mutex_unlock(&dev->cmd_lock);
 	return rc;
@@ -850,8 +964,8 @@ static int pn533_send_cmd_direct_async(struct pn533 *dev, u8 cmd_code,
 				       pn533_send_async_complete_t complete_cb,
 				       void *complete_cb_context)
 {
-	struct pn533_send_async_complete_arg *arg;
 	struct sk_buff *resp;
+	struct pn533_cmd *cmd;
 	int rc;
 	int resp_len = dev->ops->rx_header_len +
 		       dev->ops->max_payload_len +
@@ -861,33 +975,47 @@ static int pn533_send_cmd_direct_async(struct pn533 *dev, u8 cmd_code,
 	if (!resp)
 		return -ENOMEM;
 
-	arg = kzalloc(sizeof(*arg), GFP_KERNEL);
-	if (!arg) {
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (!cmd) {
 		dev_kfree_skb(resp);
 		return -ENOMEM;
 	}
 
-	arg->complete_cb = complete_cb;
-	arg->complete_cb_context = complete_cb_context;
-	arg->resp = resp;
-	arg->req = req;
+	cmd->code = cmd_code;
+	cmd->req = req;
+	cmd->resp = resp;
+	cmd->resp_len = resp_len;
+	cmd->complete_cb = complete_cb;
+	cmd->complete_cb_context = complete_cb_context;
 
 	pn533_build_cmd_frame(dev, cmd_code, req);
 
-	rc = __pn533_send_frame_async(dev, req, resp, resp_len,
-				      pn533_send_async_complete, arg);
+	rc = __pn533_send_frame_async(dev, req, resp, resp_len);
 	if (rc < 0) {
 		dev_kfree_skb(resp);
-		kfree(arg);
+		kfree(cmd);
+	} else {
+		dev->cmd = cmd;
 	}
 
 	return rc;
 }
 
+static void pn533_wq_cmd_complete(struct work_struct *work)
+{
+	struct pn533 *dev = container_of(work, struct pn533, cmd_complete_work);
+	int rc;
+
+	rc = pn533_send_async_complete(dev);
+	if (rc != -EINPROGRESS)
+		queue_work(dev->wq, &dev->cmd_work);
+}
+
 static void pn533_wq_cmd(struct work_struct *work)
 {
 	struct pn533 *dev = container_of(work, struct pn533, cmd_work);
 	struct pn533_cmd *cmd;
+	int rc;
 
 	mutex_lock(&dev->cmd_lock);
 
@@ -903,10 +1031,15 @@ static void pn533_wq_cmd(struct work_struct *work)
 
 	mutex_unlock(&dev->cmd_lock);
 
-	__pn533_send_frame_async(dev, cmd->req, cmd->resp, cmd->resp_len,
-				 pn533_send_async_complete, cmd->arg);
+	rc = __pn533_send_frame_async(dev, cmd->req, cmd->resp, cmd->resp_len);
+	if (rc < 0) {
+		dev_kfree_skb(cmd->req);
+		dev_kfree_skb(cmd->resp);
+		kfree(cmd);
+		return;
+	}
 
-	kfree(cmd);
+	dev->cmd = cmd;
 }
 
 struct pn533_sync_cmd_response {
@@ -982,6 +1115,23 @@ static void pn533_send_complete(struct urb *urb)
 	}
 }
 
+static void pn533_abort_cmd(struct pn533 *dev, gfp_t flags)
+{
+	/* ACR122U does not support any command which aborts last
+	 * issued command i.e. as ACK for standard PN533. Additionally,
+	 * it behaves stange, sending broken or incorrect responses,
+	 * when we cancel urb before the chip will send response.
+	 */
+	if (dev->device_type == PN533_DEVICE_ACR122U)
+		return;
+
+	/* An ack will cancel the last issued command */
+	pn533_send_ack(dev, flags);
+
+	/* cancel the urb request */
+	usb_kill_urb(dev->in_urb);
+}
+
 static struct sk_buff *pn533_alloc_skb(struct pn533 *dev, unsigned int size)
 {
 	struct sk_buff *skb;
@@ -1500,9 +1650,6 @@ static void pn533_listen_mode_timer(unsigned long data)
 
 	nfc_dev_dbg(&dev->interface->dev, "Listen mode timeout");
 
-	/* An ack will cancel the last issued command (poll) */
-	pn533_send_ack(dev, GFP_ATOMIC);
-
 	dev->cancel_listen = 1;
 
 	pn533_poll_next_mod(dev);
@@ -1549,6 +1696,11 @@ static int pn533_poll_complete(struct pn533 *dev, void *arg,
 	if (!rc)
 		goto done;
 
+	if (!dev->poll_mod_count) {
+		nfc_dev_dbg(&dev->interface->dev, "Polling has been stoped.");
+		goto done;
+	}
+
 	pn533_poll_next_mod(dev);
 	queue_work(dev->wq, &dev->poll_work);
 
@@ -1627,7 +1779,7 @@ static void pn533_wq_poll(struct work_struct *work)
 
 	if (dev->cancel_listen == 1) {
 		dev->cancel_listen = 0;
-		usb_kill_urb(dev->in_urb);
+		pn533_abort_cmd(dev, GFP_ATOMIC);
 	}
 
 	rc = pn533_send_poll_frame(dev);
@@ -1689,12 +1841,7 @@ static void pn533_stop_poll(struct nfc_dev *nfc_dev)
 		return;
 	}
 
-	/* An ack will cancel the last issued command (poll) */
-	pn533_send_ack(dev, GFP_KERNEL);
-
-	/* prevent pn533_start_poll_complete to issue a new poll meanwhile */
-	usb_kill_urb(dev->in_urb);
-
+	pn533_abort_cmd(dev, GFP_KERNEL);
 	pn533_poll_reset_mod_list(dev);
 }
 
@@ -1723,6 +1870,8 @@ static int pn533_activate_target_nfcdep(struct pn533 *dev)
 	rsp = (struct pn533_cmd_activate_response *)resp->data;
 	rc = rsp->status & PN533_CMD_RET_MASK;
 	if (rc != PN533_CMD_RET_SUCCESS) {
+		nfc_dev_err(&dev->interface->dev,
+			    "Target activation failed (error 0x%x)", rc);
 		dev_kfree_skb(resp);
 		return -EIO;
 	}
@@ -1850,7 +1999,7 @@ static int pn533_in_dep_link_up_complete(struct pn533 *dev, void *arg,
 	rc = rsp->status & PN533_CMD_RET_MASK;
 	if (rc != PN533_CMD_RET_SUCCESS) {
 		nfc_dev_err(&dev->interface->dev,
-			    "Bringing DEP link up failed %d", rc);
+			    "Bringing DEP link up failed (error 0x%x)", rc);
 		goto error;
 	}
 
@@ -1985,10 +2134,8 @@ static int pn533_dep_link_down(struct nfc_dev *nfc_dev)
 
 	pn533_poll_reset_mod_list(dev);
 
-	if (dev->tgt_mode || dev->tgt_active_prot) {
-		pn533_send_ack(dev, GFP_KERNEL);
-		usb_kill_urb(dev->in_urb);
-	}
+	if (dev->tgt_mode || dev->tgt_active_prot)
+		pn533_abort_cmd(dev, GFP_KERNEL);
 
 	dev->tgt_active_prot = 0;
 	dev->tgt_mode = 0;
@@ -2064,8 +2211,7 @@ static int pn533_data_exchange_complete(struct pn533 *dev, void *_arg,
 
 	if (ret != PN533_CMD_RET_SUCCESS) {
 		nfc_dev_err(&dev->interface->dev,
-			    "PN533 reported error %d when exchanging data",
-			    ret);
+			    "Exchanging data failed (error 0x%x)", ret);
 		rc = -EIO;
 		goto error;
 	}
@@ -2253,7 +2399,7 @@ static void pn533_wq_mi_recv(struct work_struct *work)
 		    "Error %d when trying to perform data_exchange", rc);
 
 	dev_kfree_skb(skb);
-	kfree(dev->cmd_complete_arg);
+	kfree(dev->cmd_complete_mi_arg);
 
 error:
 	pn533_send_ack(dev, GFP_KERNEL);
@@ -2310,7 +2456,7 @@ static int pn533_get_firmware_version(struct pn533 *dev,
 	return 0;
 }
 
-static int pn533_fw_reset(struct pn533 *dev)
+static int pn533_pasori_fw_reset(struct pn533 *dev)
 {
 	struct sk_buff *skb;
 	struct sk_buff *resp;
@@ -2332,9 +2478,102 @@ static int pn533_fw_reset(struct pn533 *dev)
 	return 0;
 }
 
+struct pn533_acr122_poweron_rdr_arg {
+	int rc;
+	struct completion done;
+};
+
+static void pn533_acr122_poweron_rdr_resp(struct urb *urb)
+{
+	struct pn533_acr122_poweron_rdr_arg *arg = urb->context;
+
+	nfc_dev_dbg(&urb->dev->dev, "%s", __func__);
+
+	print_hex_dump(KERN_ERR, "ACR122 RX: ", DUMP_PREFIX_NONE, 16, 1,
+		       urb->transfer_buffer, urb->transfer_buffer_length,
+		       false);
+
+	arg->rc = urb->status;
+	complete(&arg->done);
+}
+
+static int pn533_acr122_poweron_rdr(struct pn533 *dev)
+{
+	/* Power on th reader (CCID cmd) */
+	u8 cmd[10] = {PN533_ACR122_PC_TO_RDR_ICCPOWERON,
+		      0, 0, 0, 0, 0, 0, 3, 0, 0};
+	u8 buf[255];
+	int rc;
+	void *cntx;
+	struct pn533_acr122_poweron_rdr_arg arg;
+
+	nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
+	init_completion(&arg.done);
+	cntx = dev->in_urb->context;  /* backup context */
+
+	dev->in_urb->transfer_buffer = buf;
+	dev->in_urb->transfer_buffer_length = 255;
+	dev->in_urb->complete = pn533_acr122_poweron_rdr_resp;
+	dev->in_urb->context = &arg;
+
+	dev->out_urb->transfer_buffer = cmd;
+	dev->out_urb->transfer_buffer_length = sizeof(cmd);
+
+	print_hex_dump(KERN_ERR, "ACR122 TX: ", DUMP_PREFIX_NONE, 16, 1,
+		       cmd, sizeof(cmd), false);
+
+	rc = usb_submit_urb(dev->out_urb, GFP_KERNEL);
+	if (rc) {
+		nfc_dev_err(&dev->interface->dev,
+			    "Reader power on cmd error %d", rc);
+		return rc;
+	}
+
+	rc =  usb_submit_urb(dev->in_urb, GFP_KERNEL);
+	if (rc) {
+		nfc_dev_err(&dev->interface->dev,
+			    "Can't submit for reader power on cmd response %d",
+			    rc);
+		return rc;
+	}
+
+	wait_for_completion(&arg.done);
+	dev->in_urb->context = cntx; /* restore context */
+
+	return arg.rc;
+}
+
+static int pn533_rf_field(struct nfc_dev *nfc_dev, u8 rf)
+{
+	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
+	u8 rf_field = !!rf;
+	int rc;
+
+	rc = pn533_set_configuration(dev, PN533_CFGITEM_RF_FIELD,
+				     (u8 *)&rf_field, 1);
+	if (rc) {
+		nfc_dev_err(&dev->interface->dev,
+			    "Error on setting RF field");
+		return rc;
+	}
+
+	return rc;
+}
+
+int pn533_dev_up(struct nfc_dev *nfc_dev)
+{
+	return pn533_rf_field(nfc_dev, 1);
+}
+
+int pn533_dev_down(struct nfc_dev *nfc_dev)
+{
+	return pn533_rf_field(nfc_dev, 0);
+}
+
 static struct nfc_ops pn533_nfc_ops = {
-	.dev_up = NULL,
-	.dev_down = NULL,
+	.dev_up = pn533_dev_up,
+	.dev_down = pn533_dev_down,
 	.dep_link_up = pn533_dep_link_up,
 	.dep_link_down = pn533_dep_link_down,
 	.start_poll = pn533_start_poll,
@@ -2366,6 +2605,7 @@ static int pn533_setup(struct pn533 *dev)
 		break;
 
 	case PN533_DEVICE_PASORI:
+	case PN533_DEVICE_ACR122U:
 		max_retries.mx_rty_atr = 0x2;
 		max_retries.mx_rty_psl = 0x1;
 		max_retries.mx_rty_passive_act =
@@ -2405,7 +2645,7 @@ static int pn533_setup(struct pn533 *dev)
 		break;
 
 	case PN533_DEVICE_PASORI:
-		pn533_fw_reset(dev);
+		pn533_pasori_fw_reset(dev);
 
 		rc = pn533_set_configuration(dev, PN533_CFGITEM_PASORI,
 					     pasori_cfg, 3);
@@ -2415,7 +2655,7 @@ static int pn533_setup(struct pn533 *dev)
 			return rc;
 		}
 
-		pn533_fw_reset(dev);
+		pn533_pasori_fw_reset(dev);
 
 		break;
 	}
@@ -2496,6 +2736,7 @@ static int pn533_probe(struct usb_interface *interface,
 
 	dev->ops = &pn533_std_frame_ops;
 
+	dev->protocol_type = PN533_PROTO_REQ_ACK_RESP;
 	dev->device_type = id->driver_info;
 	switch (dev->device_type) {
 	case PN533_DEVICE_STD:
@@ -2506,6 +2747,20 @@ static int pn533_probe(struct usb_interface *interface,
 		protocols = PN533_NO_TYPE_B_PROTOCOLS;
 		break;
 
+	case PN533_DEVICE_ACR122U:
+		protocols = PN533_NO_TYPE_B_PROTOCOLS;
+		dev->ops = &pn533_acr122_frame_ops;
+		dev->protocol_type = PN533_PROTO_REQ_RESP,
+
+		rc = pn533_acr122_poweron_rdr(dev);
+		if (rc < 0) {
+			nfc_dev_err(&dev->interface->dev,
+				    "Couldn't poweron the reader (error %d)",
+				    rc);
+			goto destroy_wq;
+		}
+		break;
+
 	default:
 		nfc_dev_err(&dev->interface->dev, "Unknown device type %d\n",
 			    dev->device_type);
@@ -2555,6 +2810,7 @@ destroy_wq:
 error:
 	usb_free_urb(dev->in_urb);
 	usb_free_urb(dev->out_urb);
+	usb_put_dev(dev->udev);
 	kfree(dev);
 	return rc;
 }
@@ -2600,8 +2856,9 @@ static struct usb_driver pn533_driver = {
 
 module_usb_driver(pn533_driver);
 
-MODULE_AUTHOR("Lauro Ramos Venancio <lauro.venancio@openbossa.org>,"
-			" Aloisio Almeida Jr <aloisio.almeida@openbossa.org>");
+MODULE_AUTHOR("Lauro Ramos Venancio <lauro.venancio@openbossa.org>");
+MODULE_AUTHOR("Aloisio Almeida Jr <aloisio.almeida@openbossa.org>");
+MODULE_AUTHOR("Waldemar Rymarkiewicz <waldemar.rymarkiewicz@tieto.com>");
 MODULE_DESCRIPTION("PN533 usb driver ver " VERSION);
 MODULE_VERSION(VERSION);
 MODULE_LICENSE("GPL");
diff --git a/drivers/nfc/pn544/Kconfig b/drivers/nfc/pn544/Kconfig
index c277790ac71c..ccf06f5f6ebb 100644
--- a/drivers/nfc/pn544/Kconfig
+++ b/drivers/nfc/pn544/Kconfig
@@ -20,4 +20,15 @@ config NFC_PN544_I2C
 	  Select this if your platform is using the i2c bus.
 
 	  If you choose to build a module, it'll be called pn544_i2c.
-	  Say N if unsure.
\ No newline at end of file
+	  Say N if unsure.
+
+config NFC_PN544_MEI
+	tristate "NFC PN544 MEI support"
+	depends on NFC_PN544 && NFC_MEI_PHY
+	---help---
+	  This module adds support for the mei interface of adapters using
+	  NXP pn544 chipsets.  Select this if your pn544 chipset
+	  is handled by Intel's Management Engine Interface on your platform.
+
+	  If you choose to build a module, it'll be called pn544_mei.
+	  Say N if unsure.
diff --git a/drivers/nfc/pn544/Makefile b/drivers/nfc/pn544/Makefile
index ac076793687d..29fb5a174036 100644
--- a/drivers/nfc/pn544/Makefile
+++ b/drivers/nfc/pn544/Makefile
@@ -3,6 +3,8 @@
 #
 
 pn544_i2c-objs  = i2c.o
+pn544_mei-objs  = mei.o
 
 obj-$(CONFIG_NFC_PN544)     += pn544.o
 obj-$(CONFIG_NFC_PN544_I2C) += pn544_i2c.o
+obj-$(CONFIG_NFC_PN544_MEI) += pn544_mei.o
diff --git a/drivers/nfc/pn544/mei.c b/drivers/nfc/pn544/mei.c
new file mode 100644
index 000000000000..1eb48848a35a
--- /dev/null
+++ b/drivers/nfc/pn544/mei.c
@@ -0,0 +1,121 @@
+/*
+ * HCI based Driver for NXP pn544 NFC Chip
+ *
+ * Copyright (C) 2013  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/mod_devicetable.h>
+#include <linux/nfc.h>
+#include <net/nfc/hci.h>
+#include <net/nfc/llc.h>
+
+#include "../mei_phy.h"
+#include "pn544.h"
+
+#define PN544_DRIVER_NAME "pn544"
+
+static int pn544_mei_probe(struct mei_cl_device *device,
+			       const struct mei_cl_device_id *id)
+{
+	struct nfc_mei_phy *phy;
+	int r;
+
+	pr_info("Probing NFC pn544\n");
+
+	phy = nfc_mei_phy_alloc(device);
+	if (!phy) {
+		pr_err("Cannot allocate memory for pn544 mei phy.\n");
+		return -ENOMEM;
+	}
+
+	r = mei_cl_register_event_cb(device, nfc_mei_event_cb, phy);
+	if (r) {
+		pr_err(PN544_DRIVER_NAME ": event cb registration failed\n");
+		goto err_out;
+	}
+
+	r = pn544_hci_probe(phy, &mei_phy_ops, LLC_NOP_NAME,
+			    MEI_NFC_HEADER_SIZE, 0, MEI_NFC_MAX_HCI_PAYLOAD,
+			    &phy->hdev);
+	if (r < 0)
+		goto err_out;
+
+	return 0;
+
+err_out:
+	nfc_mei_phy_free(phy);
+
+	return r;
+}
+
+static int pn544_mei_remove(struct mei_cl_device *device)
+{
+	struct nfc_mei_phy *phy = mei_cl_get_drvdata(device);
+
+	pr_info("Removing pn544\n");
+
+	pn544_hci_remove(phy->hdev);
+
+	nfc_mei_phy_disable(phy);
+
+	nfc_mei_phy_free(phy);
+
+	return 0;
+}
+
+static struct mei_cl_device_id pn544_mei_tbl[] = {
+	{ PN544_DRIVER_NAME },
+
+	/* required last entry */
+	{ }
+};
+MODULE_DEVICE_TABLE(mei, pn544_mei_tbl);
+
+static struct mei_cl_driver pn544_driver = {
+	.id_table = pn544_mei_tbl,
+	.name = PN544_DRIVER_NAME,
+
+	.probe = pn544_mei_probe,
+	.remove = pn544_mei_remove,
+};
+
+static int pn544_mei_init(void)
+{
+	int r;
+
+	pr_debug(DRIVER_DESC ": %s\n", __func__);
+
+	r = mei_cl_driver_register(&pn544_driver);
+	if (r) {
+		pr_err(PN544_DRIVER_NAME ": driver registration failed\n");
+		return r;
+	}
+
+	return 0;
+}
+
+static void pn544_mei_exit(void)
+{
+	mei_cl_driver_unregister(&pn544_driver);
+}
+
+module_init(pn544_mei_init);
+module_exit(pn544_mei_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/nubus/nubus.c b/drivers/nubus/nubus.c
index 44d01afafe9c..43926cd25ae8 100644
--- a/drivers/nubus/nubus.c
+++ b/drivers/nubus/nubus.c
@@ -19,7 +19,6 @@
 #include <asm/setup.h>
 #include <asm/page.h>
 #include <asm/hwtest.h>
-#include <linux/proc_fs.h>
 #include <asm/mac_via.h>
 #include <asm/mac_oss.h>
 
@@ -954,56 +953,6 @@ void __init nubus_probe_slot(int slot)
 	}
 }
 
-#if defined(CONFIG_PROC_FS)
-
-/* /proc/nubus stuff */
-
-static int sprint_nubus_board(struct nubus_board* board, char* ptr, int len)
-{
-	if(len < 100)
-		return -1;
-	
-	sprintf(ptr, "Slot %X: %s\n",
-		board->slot, board->name);
-	
-	return strlen(ptr);
-}
-
-static int nubus_read_proc(char *page, char **start, off_t off,
-				int count, int *eof, void *data)
-{
-	int nprinted, len, begin = 0;
-	int size = PAGE_SIZE;
-	struct nubus_board* board;
-	
-	len   = sprintf(page, "Nubus devices found:\n");
-	/* Walk the list of NuBus boards */
-	for (board = nubus_boards; board != NULL; board = board->next)
-	{
-		nprinted = sprint_nubus_board(board, page + len, size - len);
-		if (nprinted < 0)
-			break;
-		len += nprinted;
-		if (len+begin < off) {
-			begin += len;
-			len = 0;
-		}
-		if (len+begin >= off+count)
-			break;
-	}
-	if (len+begin < off)
-		*eof = 1;
-	off -= begin;
-	*start = page + off;
-	len -= off;
-	if (len>count)
-		len = count;
-	if (len<0)
-		len = 0;
-	return len;
-}
-#endif
-
 void __init nubus_scan_bus(void)
 {
 	int slot;
@@ -1041,11 +990,7 @@ static int __init nubus_init(void)
 	nubus_devices = NULL;
 	nubus_boards  = NULL;
 	nubus_scan_bus();
-
-#ifdef CONFIG_PROC_FS
-	create_proc_read_entry("nubus", 0, NULL, nubus_read_proc, NULL);
 	nubus_proc_init();
-#endif
 	return 0;
 }
 
diff --git a/drivers/nubus/proc.c b/drivers/nubus/proc.c
index 208dd12825bc..5371b374f1fe 100644
--- a/drivers/nubus/proc.c
+++ b/drivers/nubus/proc.c
@@ -52,7 +52,6 @@ static int nubus_devices_proc_open(struct inode *inode, struct file *file)
 }
 
 static const struct file_operations nubus_devices_proc_fops = {
-	.owner		= THIS_MODULE,
 	.open		= nubus_devices_proc_open,
 	.read		= seq_read,
 	.llseek		= seq_lseek,
@@ -61,6 +60,10 @@ static const struct file_operations nubus_devices_proc_fops = {
 
 static struct proc_dir_entry *proc_bus_nubus_dir;
 
+static const struct file_operations nubus_proc_subdir_fops = {
+#warning Need to set some I/O handlers here
+};
+
 static void nubus_proc_subdir(struct nubus_dev* dev,
 			      struct proc_dir_entry* parent,
 			      struct nubus_dir* dir)
@@ -73,9 +76,10 @@ static void nubus_proc_subdir(struct nubus_dev* dev,
 		struct proc_dir_entry* e;
 		
 		sprintf(name, "%x", ent.type);
-		e = create_proc_entry(name, S_IFREG | S_IRUGO |
-				      S_IWUSR, parent);
-		if (!e) return;
+		e = proc_create(name, S_IFREG | S_IRUGO | S_IWUSR, parent,
+				&nubus_proc_subdir_fops);
+		if (!e)
+			return;
 	}
 }
 
@@ -143,20 +147,72 @@ int nubus_proc_attach_device(struct nubus_dev *dev)
 }
 EXPORT_SYMBOL(nubus_proc_attach_device);
 
-/* FIXME: this is certainly broken! */
-int nubus_proc_detach_device(struct nubus_dev *dev)
+/*
+ * /proc/nubus stuff
+ */
+static int nubus_proc_show(struct seq_file *m, void *v)
 {
-	struct proc_dir_entry *e;
+	const struct nubus_board *board = v;
 
-	if ((e = dev->procdir)) {
-		if (atomic_read(&e->count))
-			return -EBUSY;
-		remove_proc_entry(e->name, proc_bus_nubus_dir);
-		dev->procdir = NULL;
-	}
+	/* Display header on line 1 */
+	if (v == SEQ_START_TOKEN)
+		seq_puts(m, "Nubus devices found:\n");
+	else
+		seq_printf(m, "Slot %X: %s\n", board->slot, board->name);
 	return 0;
 }
-EXPORT_SYMBOL(nubus_proc_detach_device);
+
+static void *nubus_proc_start(struct seq_file *m, loff_t *_pos)
+{
+	struct nubus_board *board;
+	unsigned pos;
+
+	if (*_pos > LONG_MAX)
+		return NULL;
+	pos = *_pos;
+	if (pos == 0)
+		return SEQ_START_TOKEN;
+	for (board = nubus_boards; board; board = board->next)
+		if (--pos == 0)
+			break;
+	return board;
+}
+
+static void *nubus_proc_next(struct seq_file *p, void *v, loff_t *_pos)
+{
+	/* Walk the list of NuBus boards */
+	struct nubus_board *board = v;
+
+	++*_pos;
+	if (v == SEQ_START_TOKEN)
+		board = nubus_boards;
+	else if (board)
+		board = board->next;
+	return board;
+}
+
+static void nubus_proc_stop(struct seq_file *p, void *v)
+{
+}
+
+static const struct seq_operations nubus_proc_seqops = {
+	.start	= nubus_proc_start,
+	.next	= nubus_proc_next,
+	.stop	= nubus_proc_stop,
+	.show	= nubus_proc_show,
+};
+
+static int nubus_proc_open(struct inode *inode, struct file *file)
+{
+	return seq_open(file, &nubus_proc_seqops);
+}
+
+static const struct file_operations nubus_proc_fops = {
+	.open		= nubus_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= seq_release,
+};
 
 void __init proc_bus_nubus_add_devices(void)
 {
@@ -168,6 +224,7 @@ void __init proc_bus_nubus_add_devices(void)
 
 void __init nubus_proc_init(void)
 {
+	proc_create("nubus", 0, NULL, &nubus_proc_fops);
 	if (!MACH_IS_MAC)
 		return;
 	proc_bus_nubus_dir = proc_mkdir("bus/nubus", NULL);
diff --git a/drivers/of/base.c b/drivers/of/base.c
index 321d3ef05006..c76d16c972cc 100644
--- a/drivers/of/base.c
+++ b/drivers/of/base.c
@@ -382,6 +382,7 @@ struct device_node *of_get_next_parent(struct device_node *node)
 	raw_spin_unlock_irqrestore(&devtree_lock, flags);
 	return parent;
 }
+EXPORT_SYMBOL(of_get_next_parent);
 
 /**
  *	of_get_next_child - Iterate a node childs
@@ -746,6 +747,64 @@ struct device_node *of_find_node_by_phandle(phandle handle)
 EXPORT_SYMBOL(of_find_node_by_phandle);
 
 /**
+ * of_find_property_value_of_size
+ *
+ * @np:		device node from which the property value is to be read.
+ * @propname:	name of the property to be searched.
+ * @len:	requested length of property value
+ *
+ * Search for a property in a device node and valid the requested size.
+ * Returns the property value on success, -EINVAL if the property does not
+ *  exist, -ENODATA if property does not have a value, and -EOVERFLOW if the
+ * property data isn't large enough.
+ *
+ */
+static void *of_find_property_value_of_size(const struct device_node *np,
+			const char *propname, u32 len)
+{
+	struct property *prop = of_find_property(np, propname, NULL);
+
+	if (!prop)
+		return ERR_PTR(-EINVAL);
+	if (!prop->value)
+		return ERR_PTR(-ENODATA);
+	if (len > prop->length)
+		return ERR_PTR(-EOVERFLOW);
+
+	return prop->value;
+}
+
+/**
+ * of_property_read_u32_index - Find and read a u32 from a multi-value property.
+ *
+ * @np:		device node from which the property value is to be read.
+ * @propname:	name of the property to be searched.
+ * @index:	index of the u32 in the list of values
+ * @out_value:	pointer to return value, modified only if no error.
+ *
+ * Search for a property in a device node and read nth 32-bit value from
+ * it. Returns 0 on success, -EINVAL if the property does not exist,
+ * -ENODATA if property does not have a value, and -EOVERFLOW if the
+ * property data isn't large enough.
+ *
+ * The out_value is modified only if a valid u32 value can be decoded.
+ */
+int of_property_read_u32_index(const struct device_node *np,
+				       const char *propname,
+				       u32 index, u32 *out_value)
+{
+	const u32 *val = of_find_property_value_of_size(np, propname,
+					((index + 1) * sizeof(*out_value)));
+
+	if (IS_ERR(val))
+		return PTR_ERR(val);
+
+	*out_value = be32_to_cpup(((__be32 *)val) + index);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(of_property_read_u32_index);
+
+/**
  * of_property_read_u8_array - Find and read an array of u8 from a property.
  *
  * @np:		device node from which the property value is to be read.
@@ -766,17 +825,12 @@ EXPORT_SYMBOL(of_find_node_by_phandle);
 int of_property_read_u8_array(const struct device_node *np,
 			const char *propname, u8 *out_values, size_t sz)
 {
-	struct property *prop = of_find_property(np, propname, NULL);
-	const u8 *val;
+	const u8 *val = of_find_property_value_of_size(np, propname,
+						(sz * sizeof(*out_values)));
 
-	if (!prop)
-		return -EINVAL;
-	if (!prop->value)
-		return -ENODATA;
-	if ((sz * sizeof(*out_values)) > prop->length)
-		return -EOVERFLOW;
+	if (IS_ERR(val))
+		return PTR_ERR(val);
 
-	val = prop->value;
 	while (sz--)
 		*out_values++ = *val++;
 	return 0;
@@ -804,17 +858,12 @@ EXPORT_SYMBOL_GPL(of_property_read_u8_array);
 int of_property_read_u16_array(const struct device_node *np,
 			const char *propname, u16 *out_values, size_t sz)
 {
-	struct property *prop = of_find_property(np, propname, NULL);
-	const __be16 *val;
+	const __be16 *val = of_find_property_value_of_size(np, propname,
+						(sz * sizeof(*out_values)));
 
-	if (!prop)
-		return -EINVAL;
-	if (!prop->value)
-		return -ENODATA;
-	if ((sz * sizeof(*out_values)) > prop->length)
-		return -EOVERFLOW;
+	if (IS_ERR(val))
+		return PTR_ERR(val);
 
-	val = prop->value;
 	while (sz--)
 		*out_values++ = be16_to_cpup(val++);
 	return 0;
@@ -841,17 +890,12 @@ int of_property_read_u32_array(const struct device_node *np,
 			       const char *propname, u32 *out_values,
 			       size_t sz)
 {
-	struct property *prop = of_find_property(np, propname, NULL);
-	const __be32 *val;
+	const __be32 *val = of_find_property_value_of_size(np, propname,
+						(sz * sizeof(*out_values)));
 
-	if (!prop)
-		return -EINVAL;
-	if (!prop->value)
-		return -ENODATA;
-	if ((sz * sizeof(*out_values)) > prop->length)
-		return -EOVERFLOW;
+	if (IS_ERR(val))
+		return PTR_ERR(val);
 
-	val = prop->value;
 	while (sz--)
 		*out_values++ = be32_to_cpup(val++);
 	return 0;
@@ -874,15 +918,13 @@ EXPORT_SYMBOL_GPL(of_property_read_u32_array);
 int of_property_read_u64(const struct device_node *np, const char *propname,
 			 u64 *out_value)
 {
-	struct property *prop = of_find_property(np, propname, NULL);
+	const __be32 *val = of_find_property_value_of_size(np, propname,
+						sizeof(*out_value));
 
-	if (!prop)
-		return -EINVAL;
-	if (!prop->value)
-		return -ENODATA;
-	if (sizeof(*out_value) > prop->length)
-		return -EOVERFLOW;
-	*out_value = of_read_number(prop->value, 2);
+	if (IS_ERR(val))
+		return PTR_ERR(val);
+
+	*out_value = of_read_number(val, 2);
 	return 0;
 }
 EXPORT_SYMBOL_GPL(of_property_read_u64);
@@ -1452,16 +1494,7 @@ int of_attach_node(struct device_node *np)
 #ifdef CONFIG_PROC_DEVICETREE
 static void of_remove_proc_dt_entry(struct device_node *dn)
 {
-	struct device_node *parent = dn->parent;
-	struct property *prop = dn->properties;
-
-	while (prop) {
-		remove_proc_entry(prop->name, dn->pde);
-		prop = prop->next;
-	}
-
-	if (dn->pde)
-		remove_proc_entry(dn->pde->name, parent->pde);
+	proc_remove(dn->pde);
 }
 #else
 static void of_remove_proc_dt_entry(struct device_node *dn)
diff --git a/drivers/of/of_mdio.c b/drivers/of/of_mdio.c
index e3a8b22ef9dd..d5a57a9e329c 100644
--- a/drivers/of/of_mdio.c
+++ b/drivers/of/of_mdio.c
@@ -34,7 +34,10 @@ int of_mdiobus_register(struct mii_bus *mdio, struct device_node *np)
 {
 	struct phy_device *phy;
 	struct device_node *child;
-	int rc, i;
+	const __be32 *paddr;
+	u32 addr;
+	bool is_c45, scanphys = false;
+	int rc, i, len;
 
 	/* Mask out all PHYs from auto probing.  Instead the PHYs listed in
 	 * the device tree are populated after the bus has been registered */
@@ -54,14 +57,10 @@ int of_mdiobus_register(struct mii_bus *mdio, struct device_node *np)
 
 	/* Loop over the child nodes and register a phy_device for each one */
 	for_each_available_child_of_node(np, child) {
-		const __be32 *paddr;
-		u32 addr;
-		int len;
-		bool is_c45;
-
 		/* A PHY must have a reg property in the range [0-31] */
 		paddr = of_get_property(child, "reg", &len);
 		if (!paddr || len < sizeof(*paddr)) {
+			scanphys = true;
 			dev_err(&mdio->dev, "%s has invalid PHY address\n",
 				child->full_name);
 			continue;
@@ -85,13 +84,10 @@ int of_mdiobus_register(struct mii_bus *mdio, struct device_node *np)
 		phy = get_phy_device(mdio, addr, is_c45);
 
 		if (!phy || IS_ERR(phy)) {
-			phy = phy_device_create(mdio, addr, 0, false, NULL);
-			if (!phy || IS_ERR(phy)) {
-				dev_err(&mdio->dev,
-					"error creating PHY at address %i\n",
-					addr);
-				continue;
-			}
+			dev_err(&mdio->dev,
+				"cannot get PHY at address %i\n",
+				addr);
+			continue;
 		}
 
 		/* Associate the OF node with the device structure so it
@@ -111,6 +107,59 @@ int of_mdiobus_register(struct mii_bus *mdio, struct device_node *np)
 			child->name, addr);
 	}
 
+	if (!scanphys)
+		return 0;
+
+	/* auto scan for PHYs with empty reg property */
+	for_each_available_child_of_node(np, child) {
+		/* Skip PHYs with reg property set */
+		paddr = of_get_property(child, "reg", &len);
+		if (paddr)
+			continue;
+
+		is_c45 = of_device_is_compatible(child,
+						 "ethernet-phy-ieee802.3-c45");
+
+		for (addr = 0; addr < PHY_MAX_ADDR; addr++) {
+			/* skip already registered PHYs */
+			if (mdio->phy_map[addr])
+				continue;
+
+			/* be noisy to encourage people to set reg property */
+			dev_info(&mdio->dev, "scan phy %s at address %i\n",
+				 child->name, addr);
+
+			phy = get_phy_device(mdio, addr, is_c45);
+			if (!phy || IS_ERR(phy))
+				continue;
+
+			if (mdio->irq) {
+				mdio->irq[addr] =
+					irq_of_parse_and_map(child, 0);
+				if (!mdio->irq[addr])
+					mdio->irq[addr] = PHY_POLL;
+			}
+
+			/* Associate the OF node with the device structure so it
+			 * can be looked up later */
+			of_node_get(child);
+			phy->dev.of_node = child;
+
+			/* All data is now stored in the phy struct;
+			 * register it */
+			rc = phy_device_register(phy);
+			if (rc) {
+				phy_device_free(phy);
+				of_node_put(child);
+				continue;
+			}
+
+			dev_info(&mdio->dev, "registered phy %s at address %i\n",
+				 child->name, addr);
+			break;
+		}
+	}
+
 	return 0;
 }
 EXPORT_SYMBOL(of_mdiobus_register);
diff --git a/drivers/parisc/led.c b/drivers/parisc/led.c
index d4d800c54d86..b48243131993 100644
--- a/drivers/parisc/led.c
+++ b/drivers/parisc/led.c
@@ -172,14 +172,14 @@ static int led_proc_show(struct seq_file *m, void *v)
 
 static int led_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, led_proc_show, PDE(inode)->data);
+	return single_open(file, led_proc_show, PDE_DATA(inode));
 }
 
 
 static ssize_t led_proc_write(struct file *file, const char *buf,
 	size_t count, loff_t *pos)
 {
-	void *data = PDE(file_inode(file))->data;
+	void *data = PDE_DATA(file_inode(file));
 	char *cur, lbuf[32];
 	int d;
 
diff --git a/drivers/parisc/sba_iommu.c b/drivers/parisc/sba_iommu.c
index 42cfcd9eb9aa..1ff1b67e8b27 100644
--- a/drivers/parisc/sba_iommu.c
+++ b/drivers/parisc/sba_iommu.c
@@ -575,7 +575,7 @@ sba_io_pdir_entry(u64 *pdir_ptr, space_t sid, unsigned long vba,
 
 	mtsp(sid,1);
 	asm("lci 0(%%sr1, %1), %0" : "=r" (ci) : "r" (vba));
-	pa |= (ci >> 12) & 0xff;  /* move CI (8 bits) into lowest byte */
+	pa |= (ci >> PAGE_SHIFT) & 0xff;  /* move CI (8 bits) into lowest byte */
 
 	pa |= SBA_PDIR_VALID_BIT;	/* set "valid" bit */
 	*pdir_ptr = cpu_to_le64(pa);	/* swap and store into I/O Pdir */
@@ -1376,7 +1376,7 @@ static void
 sba_ioc_init(struct parisc_device *sba, struct ioc *ioc, int ioc_num)
 {
 	u32 iova_space_size, iova_space_mask;
-	unsigned int pdir_size, iov_order;
+	unsigned int pdir_size, iov_order, tcnfg;
 
 	/*
 	** Determine IOVA Space size from memory size.
@@ -1468,8 +1468,19 @@ sba_ioc_init(struct parisc_device *sba, struct ioc *ioc, int ioc_num)
 	WRITE_REG(ioc->ibase | 1, ioc->ioc_hpa+IOC_IBASE);
 	WRITE_REG(ioc->imask, ioc->ioc_hpa+IOC_IMASK);
 
-	/* Set I/O PDIR Page size to 4K */
-	WRITE_REG(0, ioc->ioc_hpa+IOC_TCNFG);
+	/* Set I/O PDIR Page size to system page size */
+	switch (PAGE_SHIFT) {
+		case 12: tcnfg = 0; break;	/*  4K */
+		case 13: tcnfg = 1; break;	/*  8K */
+		case 14: tcnfg = 2; break;	/* 16K */
+		case 16: tcnfg = 3; break;	/* 64K */
+		default:
+			panic(__FILE__ "Unsupported system page size %d",
+				1 << PAGE_SHIFT);
+			break;
+	}
+	/* Set I/O PDIR Page size to PAGE_SIZE (4k/16k/...) */
+	WRITE_REG(tcnfg, ioc->ioc_hpa+IOC_TCNFG);
 
 	/*
 	** Clear I/O TLB of any possible entries.
diff --git a/drivers/pci/bus.c b/drivers/pci/bus.c
index 748f8f3e9ff5..32e66a6f12d9 100644
--- a/drivers/pci/bus.c
+++ b/drivers/pci/bus.c
@@ -174,6 +174,7 @@ int pci_bus_add_device(struct pci_dev *dev)
 	 * Can not put in pci_device_add yet because resources
 	 * are not assigned yet for some devices.
 	 */
+	pci_fixup_device(pci_fixup_final, dev);
 	pci_create_sysfs_dev_files(dev);
 
 	dev->match_driver = true;
diff --git a/drivers/pci/iov.c b/drivers/pci/iov.c
index ee599f274f05..c93071d428f5 100644
--- a/drivers/pci/iov.c
+++ b/drivers/pci/iov.c
@@ -729,6 +729,47 @@ int pci_num_vf(struct pci_dev *dev)
 EXPORT_SYMBOL_GPL(pci_num_vf);
 
 /**
+ * pci_vfs_assigned - returns number of VFs are assigned to a guest
+ * @dev: the PCI device
+ *
+ * Returns number of VFs belonging to this device that are assigned to a guest.
+ * If device is not a physical function returns -ENODEV.
+ */
+int pci_vfs_assigned(struct pci_dev *dev)
+{
+	struct pci_dev *vfdev;
+	unsigned int vfs_assigned = 0;
+	unsigned short dev_id;
+
+	/* only search if we are a PF */
+	if (!dev->is_physfn)
+		return 0;
+
+	/*
+	 * determine the device ID for the VFs, the vendor ID will be the
+	 * same as the PF so there is no need to check for that one
+	 */
+	pci_read_config_word(dev, dev->sriov->pos + PCI_SRIOV_VF_DID, &dev_id);
+
+	/* loop through all the VFs to see if we own any that are assigned */
+	vfdev = pci_get_device(dev->vendor, dev_id, NULL);
+	while (vfdev) {
+		/*
+		 * It is considered assigned if it is a virtual function with
+		 * our dev as the physical function and the assigned bit is set
+		 */
+		if (vfdev->is_virtfn && (vfdev->physfn == dev) &&
+		    (vfdev->dev_flags & PCI_DEV_FLAGS_ASSIGNED))
+			vfs_assigned++;
+
+		vfdev = pci_get_device(dev->vendor, dev_id, vfdev);
+	}
+
+	return vfs_assigned;
+}
+EXPORT_SYMBOL_GPL(pci_vfs_assigned);
+
+/**
  * pci_sriov_set_totalvfs -- reduce the TotalVFs available
  * @dev: the PCI PF device
  * @numvfs: number that should be used for TotalVFs supported
diff --git a/drivers/pci/msi.c b/drivers/pci/msi.c
index d40bed726769..2c1075213bec 100644
--- a/drivers/pci/msi.c
+++ b/drivers/pci/msi.c
@@ -563,8 +563,10 @@ static int msi_capability_init(struct pci_dev *dev, int nvec)
 	entry->msi_attrib.default_irq	= dev->irq;	/* Save IOAPIC IRQ */
 	entry->msi_attrib.pos		= dev->msi_cap;
 
-	entry->mask_pos = dev->msi_cap + (control & PCI_MSI_FLAGS_64BIT) ?
-		PCI_MSI_MASK_64 : PCI_MSI_MASK_32;
+	if (control & PCI_MSI_FLAGS_64BIT)
+		entry->mask_pos = dev->msi_cap + PCI_MSI_MASK_64;
+	else
+		entry->mask_pos = dev->msi_cap + PCI_MSI_MASK_32;
 	/* All MSIs are unmasked by default, Mask them all */
 	if (entry->msi_attrib.maskbit)
 		pci_read_config_dword(dev, entry->mask_pos, &entry->masked);
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index 43ece5d41d36..70f10fa3c1b2 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -990,7 +990,6 @@ int pci_setup_device(struct pci_dev *dev)
 	dev->sysdata = dev->bus->sysdata;
 	dev->dev.parent = dev->bus->bridge;
 	dev->dev.bus = &pci_bus_type;
-	dev->dev.type = &pci_dev_type;
 	dev->hdr_type = hdr_type & 0x7f;
 	dev->multifunction = !!(hdr_type & 0x80);
 	dev->error_state = pci_channel_io_normal;
@@ -1210,6 +1209,7 @@ struct pci_dev *alloc_pci_dev(void)
 		return NULL;
 
 	INIT_LIST_HEAD(&dev->bus_list);
+	dev->dev.type = &pci_dev_type;
 
 	return dev;
 }
@@ -1341,7 +1341,6 @@ void pci_device_add(struct pci_dev *dev, struct pci_bus *bus)
 	list_add_tail(&dev->bus_list, &bus->devices);
 	up_write(&pci_bus_sem);
 
-	pci_fixup_device(pci_fixup_final, dev);
 	ret = pcibios_add_device(dev);
 	WARN_ON(ret < 0);
 
diff --git a/drivers/pci/proc.c b/drivers/pci/proc.c
index 0b009470e6db..08126087ec31 100644
--- a/drivers/pci/proc.c
+++ b/drivers/pci/proc.c
@@ -46,9 +46,7 @@ proc_bus_pci_lseek(struct file *file, loff_t off, int whence)
 static ssize_t
 proc_bus_pci_read(struct file *file, char __user *buf, size_t nbytes, loff_t *ppos)
 {
-	const struct inode *ino = file_inode(file);
-	const struct proc_dir_entry *dp = PDE(ino);
-	struct pci_dev *dev = dp->data;
+	struct pci_dev *dev = PDE_DATA(file_inode(file));
 	unsigned int pos = *ppos;
 	unsigned int cnt, size;
 
@@ -59,7 +57,7 @@ proc_bus_pci_read(struct file *file, char __user *buf, size_t nbytes, loff_t *pp
 	 */
 
 	if (capable(CAP_SYS_ADMIN))
-		size = dp->size;
+		size = dev->cfg_size;
 	else if (dev->hdr_type == PCI_HEADER_TYPE_CARDBUS)
 		size = 128;
 	else
@@ -133,10 +131,9 @@ static ssize_t
 proc_bus_pci_write(struct file *file, const char __user *buf, size_t nbytes, loff_t *ppos)
 {
 	struct inode *ino = file_inode(file);
-	const struct proc_dir_entry *dp = PDE(ino);
-	struct pci_dev *dev = dp->data;
+	struct pci_dev *dev = PDE_DATA(ino);
 	int pos = *ppos;
-	int size = dp->size;
+	int size = dev->cfg_size;
 	int cnt;
 
 	if (pos >= size)
@@ -200,7 +197,7 @@ proc_bus_pci_write(struct file *file, const char __user *buf, size_t nbytes, lof
 	pci_config_pm_runtime_put(dev);
 
 	*ppos = pos;
-	i_size_write(ino, dp->size);
+	i_size_write(ino, dev->cfg_size);
 	return nbytes;
 }
 
@@ -212,8 +209,7 @@ struct pci_filp_private {
 static long proc_bus_pci_ioctl(struct file *file, unsigned int cmd,
 			       unsigned long arg)
 {
-	const struct proc_dir_entry *dp = PDE(file_inode(file));
-	struct pci_dev *dev = dp->data;
+	struct pci_dev *dev = PDE_DATA(file_inode(file));
 #ifdef HAVE_PCI_MMAP
 	struct pci_filp_private *fpriv = file->private_data;
 #endif /* HAVE_PCI_MMAP */
@@ -253,9 +249,7 @@ static long proc_bus_pci_ioctl(struct file *file, unsigned int cmd,
 #ifdef HAVE_PCI_MMAP
 static int proc_bus_pci_mmap(struct file *file, struct vm_area_struct *vma)
 {
-	struct inode *inode = file_inode(file);
-	const struct proc_dir_entry *dp = PDE(inode);
-	struct pci_dev *dev = dp->data;
+	struct pci_dev *dev = PDE_DATA(file_inode(file));
 	struct pci_filp_private *fpriv = file->private_data;
 	int i, ret;
 
@@ -425,7 +419,7 @@ int pci_proc_attach_device(struct pci_dev *dev)
 			     &proc_bus_pci_operations, dev);
 	if (!e)
 		return -ENOMEM;
-	e->size = dev->cfg_size;
+	proc_set_size(e, dev->cfg_size);
 	dev->procent = e;
 
 	return 0;
@@ -433,20 +427,14 @@ int pci_proc_attach_device(struct pci_dev *dev)
 
 int pci_proc_detach_device(struct pci_dev *dev)
 {
-	struct proc_dir_entry *e;
-
-	if ((e = dev->procent)) {
-		remove_proc_entry(e->name, dev->bus->procdir);
-		dev->procent = NULL;
-	}
+	proc_remove(dev->procent);
+	dev->procent = NULL;
 	return 0;
 }
 
 int pci_proc_detach_bus(struct pci_bus* bus)
 {
-	struct proc_dir_entry *de = bus->procdir;
-	if (de)
-		remove_proc_entry(de->name, proc_bus_pci_dir);
+	proc_remove(bus->procdir);
 	return 0;
 }
 
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index f910962baaa7..8f6692438149 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -93,12 +93,20 @@ config PINCTRL_IMX53
 	  Say Y here to enable the imx53 pinctrl driver
 
 config PINCTRL_IMX6Q
-	bool "IMX6Q pinctrl driver"
+	bool "IMX6Q/DL pinctrl driver"
 	depends on OF
 	depends on SOC_IMX6Q
 	select PINCTRL_IMX
 	help
-	  Say Y here to enable the imx6q pinctrl driver
+	  Say Y here to enable the imx6q/dl pinctrl driver
+
+config PINCTRL_IMX6SL
+	bool "IMX6SL pinctrl driver"
+	depends on OF
+	depends on SOC_IMX6SL
+	select PINCTRL_IMX
+	help
+	  Say Y here to enable the imx6sl pinctrl driver
 
 config PINCTRL_LANTIQ
 	bool
@@ -216,6 +224,7 @@ config PINCTRL_S3C64XX
 source "drivers/pinctrl/mvebu/Kconfig"
 source "drivers/pinctrl/sh-pfc/Kconfig"
 source "drivers/pinctrl/spear/Kconfig"
+source "drivers/pinctrl/vt8500/Kconfig"
 
 config PINCTRL_XWAY
 	bool
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index 988279ae23cd..9bdaeb8785ce 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -21,6 +21,7 @@ obj-$(CONFIG_PINCTRL_IMX35)	+= pinctrl-imx35.o
 obj-$(CONFIG_PINCTRL_IMX51)	+= pinctrl-imx51.o
 obj-$(CONFIG_PINCTRL_IMX53)	+= pinctrl-imx53.o
 obj-$(CONFIG_PINCTRL_IMX6Q)	+= pinctrl-imx6q.o
+obj-$(CONFIG_PINCTRL_IMX6Q)	+= pinctrl-imx6dl.o
 obj-$(CONFIG_PINCTRL_FALCON)	+= pinctrl-falcon.o
 obj-$(CONFIG_PINCTRL_MXS)	+= pinctrl-mxs.o
 obj-$(CONFIG_PINCTRL_IMX23)	+= pinctrl-imx23.o
@@ -49,3 +50,4 @@ obj-$(CONFIG_PLAT_ORION)        += mvebu/
 obj-$(CONFIG_ARCH_SHMOBILE)	+= sh-pfc/
 obj-$(CONFIG_SUPERH)		+= sh-pfc/
 obj-$(CONFIG_PLAT_SPEAR)	+= spear/
+obj-$(CONFIG_ARCH_VT8500)	+= vt8500/
diff --git a/drivers/pinctrl/core.c b/drivers/pinctrl/core.c
index c3d222ed39a2..5327f35d9b5c 100644
--- a/drivers/pinctrl/core.c
+++ b/drivers/pinctrl/core.c
@@ -129,7 +129,7 @@ struct pinctrl_dev *get_pinctrl_dev_from_of_node(struct device_node *np)
 			return pctldev;
 		}
 
-	mutex_lock(&pinctrldev_list_mutex);
+	mutex_unlock(&pinctrldev_list_mutex);
 
 	return NULL;
 }
diff --git a/drivers/pinctrl/pinctrl-at91.c b/drivers/pinctrl/pinctrl-at91.c
index 4d7f531e945d..5d7529ed5392 100644
--- a/drivers/pinctrl/pinctrl-at91.c
+++ b/drivers/pinctrl/pinctrl-at91.c
@@ -18,6 +18,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/irqdomain.h>
+#include <linux/irqchip/chained_irq.h>
 #include <linux/io.h>
 #include <linux/gpio.h>
 #include <linux/pinctrl/machine.h>
@@ -27,8 +28,6 @@
 /* Since we request GPIOs from ourself */
 #include <linux/pinctrl/consumer.h>
 
-#include <asm/mach/irq.h>
-
 #include <mach/hardware.h>
 #include <mach/at91_pio.h>
 
diff --git a/drivers/pinctrl/pinctrl-bcm2835.c b/drivers/pinctrl/pinctrl-bcm2835.c
index f28d4b08771a..c8f20a3d8f88 100644
--- a/drivers/pinctrl/pinctrl-bcm2835.c
+++ b/drivers/pinctrl/pinctrl-bcm2835.c
@@ -699,11 +699,6 @@ static int bcm2835_pctl_dt_node_to_map_pull(struct bcm2835_pinctrl *pc,
 	return 0;
 }
 
-static inline u32 prop_u32(struct property *p, int i)
-{
-	return be32_to_cpup(((__be32 *)p->value) + i);
-}
-
 static int bcm2835_pctl_dt_node_to_map(struct pinctrl_dev *pctldev,
 		struct device_node *np,
 		struct pinctrl_map **map, unsigned *num_maps)
@@ -761,7 +756,9 @@ static int bcm2835_pctl_dt_node_to_map(struct pinctrl_dev *pctldev,
 		return -ENOMEM;
 
 	for (i = 0; i < num_pins; i++) {
-		pin = prop_u32(pins, i);
+		err = of_property_read_u32_index(np, "brcm,pins", i, &pin);
+		if (err)
+			goto out;
 		if (pin >= ARRAY_SIZE(bcm2835_gpio_pins)) {
 			dev_err(pc->dev, "%s: invalid brcm,pins value %d\n",
 				of_node_full_name(np), pin);
@@ -770,14 +767,20 @@ static int bcm2835_pctl_dt_node_to_map(struct pinctrl_dev *pctldev,
 		}
 
 		if (num_funcs) {
-			func = prop_u32(funcs, (num_funcs > 1) ? i : 0);
+			err = of_property_read_u32_index(np, "brcm,function",
+					(num_funcs > 1) ? i : 0, &func);
+			if (err)
+				goto out;
 			err = bcm2835_pctl_dt_node_to_map_func(pc, np, pin,
 							func, &cur_map);
 			if (err)
 				goto out;
 		}
 		if (num_pulls) {
-			pull = prop_u32(pulls, (num_pulls > 1) ? i : 0);
+			err = of_property_read_u32_index(np, "brcm,pull",
+					(num_funcs > 1) ? i : 0, &pull);
+			if (err)
+				goto out;
 			err = bcm2835_pctl_dt_node_to_map_pull(pc, np, pin,
 							pull, &cur_map);
 			if (err)
diff --git a/drivers/pinctrl/pinctrl-exynos.c b/drivers/pinctrl/pinctrl-exynos.c
index 8b10b1ac9071..ac742817ebce 100644
--- a/drivers/pinctrl/pinctrl-exynos.c
+++ b/drivers/pinctrl/pinctrl-exynos.c
@@ -23,14 +23,13 @@
 #include <linux/interrupt.h>
 #include <linux/irqdomain.h>
 #include <linux/irq.h>
+#include <linux/irqchip/chained_irq.h>
 #include <linux/of_irq.h>
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/err.h>
 
-#include <asm/mach/irq.h>
-
 #include "pinctrl-samsung.h"
 #include "pinctrl-exynos.h"
 
@@ -701,3 +700,111 @@ struct samsung_pin_ctrl exynos4x12_pin_ctrl[] = {
 		.label		= "exynos4x12-gpio-ctrl3",
 	},
 };
+
+/* pin banks of exynos5250 pin-controller 0 */
+static struct samsung_pin_bank exynos5250_pin_banks0[] = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpa2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpb0", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpb1", 0x10),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0A0, "gpb2", 0x14),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0C0, "gpb3", 0x18),
+	EXYNOS_PIN_BANK_EINTG(7, 0x0E0, "gpc0", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(4, 0x100, "gpc1", 0x20),
+	EXYNOS_PIN_BANK_EINTG(7, 0x120, "gpc2", 0x24),
+	EXYNOS_PIN_BANK_EINTG(7, 0x140, "gpc3", 0x28),
+	EXYNOS_PIN_BANK_EINTG(4, 0x160, "gpd0", 0x2c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x180, "gpd1", 0x30),
+	EXYNOS_PIN_BANK_EINTG(7, 0x2E0, "gpc4", 0x34),
+	EXYNOS_PIN_BANK_EINTN(6, 0x1A0, "gpy0"),
+	EXYNOS_PIN_BANK_EINTN(4, 0x1C0, "gpy1"),
+	EXYNOS_PIN_BANK_EINTN(6, 0x1E0, "gpy2"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x200, "gpy3"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x220, "gpy4"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x240, "gpy5"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x260, "gpy6"),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC00, "gpx0", 0x00),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC20, "gpx1", 0x04),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC40, "gpx2", 0x08),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC60, "gpx3", 0x0c),
+};
+
+/* pin banks of exynos5250 pin-controller 1 */
+static struct samsung_pin_bank exynos5250_pin_banks1[] = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpe0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(2, 0x020, "gpe1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(4, 0x040, "gpf0", 0x08),
+	EXYNOS_PIN_BANK_EINTG(4, 0x060, "gpf1", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x080, "gpg0", 0x10),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0A0, "gpg1", 0x14),
+	EXYNOS_PIN_BANK_EINTG(2, 0x0C0, "gpg2", 0x18),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0E0, "gph0", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x100, "gph1", 0x20),
+};
+
+/* pin banks of exynos5250 pin-controller 2 */
+static struct samsung_pin_bank exynos5250_pin_banks2[] = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpv0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpv1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpv2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(8, 0x080, "gpv3", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(2, 0x0C0, "gpv4", 0x10),
+};
+
+/* pin banks of exynos5250 pin-controller 3 */
+static struct samsung_pin_bank exynos5250_pin_banks3[] = {
+	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpz", 0x00),
+};
+
+/*
+ * Samsung pinctrl driver data for Exynos5250 SoC. Exynos5250 SoC includes
+ * four gpio/pin-mux/pinconfig controllers.
+ */
+struct samsung_pin_ctrl exynos5250_pin_ctrl[] = {
+	{
+		/* pin-controller instance 0 data */
+		.pin_banks	= exynos5250_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(exynos5250_pin_banks0),
+		.geint_con	= EXYNOS_GPIO_ECON_OFFSET,
+		.geint_mask	= EXYNOS_GPIO_EMASK_OFFSET,
+		.geint_pend	= EXYNOS_GPIO_EPEND_OFFSET,
+		.weint_con	= EXYNOS_WKUP_ECON_OFFSET,
+		.weint_mask	= EXYNOS_WKUP_EMASK_OFFSET,
+		.weint_pend	= EXYNOS_WKUP_EPEND_OFFSET,
+		.svc		= EXYNOS_SVC_OFFSET,
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.eint_wkup_init = exynos_eint_wkup_init,
+		.label		= "exynos5250-gpio-ctrl0",
+	}, {
+		/* pin-controller instance 1 data */
+		.pin_banks	= exynos5250_pin_banks1,
+		.nr_banks	= ARRAY_SIZE(exynos5250_pin_banks1),
+		.geint_con	= EXYNOS_GPIO_ECON_OFFSET,
+		.geint_mask	= EXYNOS_GPIO_EMASK_OFFSET,
+		.geint_pend	= EXYNOS_GPIO_EPEND_OFFSET,
+		.svc		= EXYNOS_SVC_OFFSET,
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.label		= "exynos5250-gpio-ctrl1",
+	}, {
+		/* pin-controller instance 2 data */
+		.pin_banks	= exynos5250_pin_banks2,
+		.nr_banks	= ARRAY_SIZE(exynos5250_pin_banks2),
+		.geint_con	= EXYNOS_GPIO_ECON_OFFSET,
+		.geint_mask	= EXYNOS_GPIO_EMASK_OFFSET,
+		.geint_pend	= EXYNOS_GPIO_EPEND_OFFSET,
+		.svc		= EXYNOS_SVC_OFFSET,
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.label		= "exynos5250-gpio-ctrl2",
+	}, {
+		/* pin-controller instance 3 data */
+		.pin_banks	= exynos5250_pin_banks3,
+		.nr_banks	= ARRAY_SIZE(exynos5250_pin_banks3),
+		.geint_con	= EXYNOS_GPIO_ECON_OFFSET,
+		.geint_mask	= EXYNOS_GPIO_EMASK_OFFSET,
+		.geint_pend	= EXYNOS_GPIO_EPEND_OFFSET,
+		.svc		= EXYNOS_SVC_OFFSET,
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.label		= "exynos5250-gpio-ctrl3",
+	},
+};
diff --git a/drivers/pinctrl/pinctrl-imx.c b/drivers/pinctrl/pinctrl-imx.c
index 0ef190449eab..4fcfff9243be 100644
--- a/drivers/pinctrl/pinctrl-imx.c
+++ b/drivers/pinctrl/pinctrl-imx.c
@@ -54,32 +54,6 @@ struct imx_pinctrl {
 	const struct imx_pinctrl_soc_info *info;
 };
 
-static const struct imx_pin_reg *imx_find_pin_reg(
-				const struct imx_pinctrl_soc_info *info,
-				unsigned pin, bool is_mux, unsigned mux)
-{
-	const struct imx_pin_reg *pin_reg = NULL;
-	int i;
-
-	for (i = 0; i < info->npin_regs; i++) {
-		pin_reg = &info->pin_regs[i];
-		if (pin_reg->pid != pin)
-			continue;
-		if (!is_mux)
-			break;
-		else if (pin_reg->mux_mode == (mux & IMX_MUX_MASK))
-			break;
-	}
-
-	if (i == info->npin_regs) {
-		dev_err(info->dev, "Pin(%s): unable to find pin reg map\n",
-			info->pins[pin].name);
-		return NULL;
-	}
-
-	return pin_reg;
-}
-
 static const inline struct imx_pin_group *imx_pinctrl_find_group_by_name(
 				const struct imx_pinctrl_soc_info *info,
 				const char *name)
@@ -223,7 +197,8 @@ static int imx_pmx_enable(struct pinctrl_dev *pctldev, unsigned selector,
 	struct imx_pinctrl *ipctl = pinctrl_dev_get_drvdata(pctldev);
 	const struct imx_pinctrl_soc_info *info = ipctl->info;
 	const struct imx_pin_reg *pin_reg;
-	const unsigned *pins, *mux;
+	const unsigned *pins, *mux, *input_val;
+	u16 *input_reg;
 	unsigned int npins, pin_id;
 	int i;
 
@@ -234,18 +209,17 @@ static int imx_pmx_enable(struct pinctrl_dev *pctldev, unsigned selector,
 	pins = info->groups[group].pins;
 	npins = info->groups[group].npins;
 	mux = info->groups[group].mux_mode;
+	input_val = info->groups[group].input_val;
+	input_reg = info->groups[group].input_reg;
 
-	WARN_ON(!pins || !npins || !mux);
+	WARN_ON(!pins || !npins || !mux || !input_val || !input_reg);
 
 	dev_dbg(ipctl->dev, "enable function %s group %s\n",
 		info->functions[selector].name, info->groups[group].name);
 
 	for (i = 0; i < npins; i++) {
 		pin_id = pins[i];
-
-		pin_reg = imx_find_pin_reg(info, pin_id, 1, mux[i]);
-		if (!pin_reg)
-			return -EINVAL;
+		pin_reg = &info->pin_regs[pin_id];
 
 		if (!pin_reg->mux_reg) {
 			dev_err(ipctl->dev, "Pin(%s) does not support mux function\n",
@@ -258,11 +232,11 @@ static int imx_pmx_enable(struct pinctrl_dev *pctldev, unsigned selector,
 			pin_reg->mux_reg, mux[i]);
 
 		/* some pins also need select input setting, set it if found */
-		if (pin_reg->input_reg) {
-			writel(pin_reg->input_val, ipctl->base + pin_reg->input_reg);
+		if (input_reg[i]) {
+			writel(input_val[i], ipctl->base + input_reg[i]);
 			dev_dbg(ipctl->dev,
 				"==>select_input: offset 0x%x val 0x%x\n",
-				pin_reg->input_reg, pin_reg->input_val);
+				input_reg[i], input_val[i]);
 		}
 	}
 
@@ -311,11 +285,7 @@ static int imx_pinconf_get(struct pinctrl_dev *pctldev,
 {
 	struct imx_pinctrl *ipctl = pinctrl_dev_get_drvdata(pctldev);
 	const struct imx_pinctrl_soc_info *info = ipctl->info;
-	const struct imx_pin_reg *pin_reg;
-
-	pin_reg = imx_find_pin_reg(info, pin_id, 0, 0);
-	if (!pin_reg)
-		return -EINVAL;
+	const struct imx_pin_reg *pin_reg = &info->pin_regs[pin_id];
 
 	if (!pin_reg->conf_reg) {
 		dev_err(info->dev, "Pin(%s) does not support config function\n",
@@ -333,11 +303,7 @@ static int imx_pinconf_set(struct pinctrl_dev *pctldev,
 {
 	struct imx_pinctrl *ipctl = pinctrl_dev_get_drvdata(pctldev);
 	const struct imx_pinctrl_soc_info *info = ipctl->info;
-	const struct imx_pin_reg *pin_reg;
-
-	pin_reg = imx_find_pin_reg(info, pin_id, 0, 0);
-	if (!pin_reg)
-		return -EINVAL;
+	const struct imx_pin_reg *pin_reg = &info->pin_regs[pin_id];
 
 	if (!pin_reg->conf_reg) {
 		dev_err(info->dev, "Pin(%s) does not support config function\n",
@@ -360,10 +326,9 @@ static void imx_pinconf_dbg_show(struct pinctrl_dev *pctldev,
 {
 	struct imx_pinctrl *ipctl = pinctrl_dev_get_drvdata(pctldev);
 	const struct imx_pinctrl_soc_info *info = ipctl->info;
-	const struct imx_pin_reg *pin_reg;
+	const struct imx_pin_reg *pin_reg = &info->pin_regs[pin_id];
 	unsigned long config;
 
-	pin_reg = imx_find_pin_reg(info, pin_id, 0, 0);
 	if (!pin_reg || !pin_reg->conf_reg) {
 		seq_printf(s, "N/A");
 		return;
@@ -411,29 +376,20 @@ static struct pinctrl_desc imx_pinctrl_desc = {
 	.owner = THIS_MODULE,
 };
 
-/* decode pin id and mux from pin function id got from device tree*/
-static int imx_pinctrl_get_pin_id_and_mux(const struct imx_pinctrl_soc_info *info,
-				unsigned int pin_func_id, unsigned int *pin_id,
-				unsigned int *mux)
-{
-	if (pin_func_id > info->npin_regs)
-		return -EINVAL;
-
-	*pin_id = info->pin_regs[pin_func_id].pid;
-	*mux = info->pin_regs[pin_func_id].mux_mode;
-
-	return 0;
-}
+/*
+ * Each pin represented in fsl,pins consists of 5 u32 PIN_FUNC_ID and
+ * 1 u32 CONFIG, so 24 types in total for each pin.
+ */
+#define FSL_PIN_SIZE 24
 
 static int imx_pinctrl_parse_groups(struct device_node *np,
 				    struct imx_pin_group *grp,
 				    struct imx_pinctrl_soc_info *info,
 				    u32 index)
 {
-	unsigned int pin_func_id;
-	int ret, size;
+	int size;
 	const __be32 *list;
-	int i, j;
+	int i;
 	u32 config;
 
 	dev_dbg(info->dev, "group(%d): %s\n", index, np->name);
@@ -447,32 +403,40 @@ static int imx_pinctrl_parse_groups(struct device_node *np,
 	 */
 	list = of_get_property(np, "fsl,pins", &size);
 	/* we do not check return since it's safe node passed down */
-	size /= sizeof(*list);
-	if (!size || size % 2) {
-		dev_err(info->dev, "wrong pins number or pins and configs should be pairs\n");
+	if (!size || size % FSL_PIN_SIZE) {
+		dev_err(info->dev, "Invalid fsl,pins property\n");
 		return -EINVAL;
 	}
 
-	grp->npins = size / 2;
+	grp->npins = size / FSL_PIN_SIZE;
 	grp->pins = devm_kzalloc(info->dev, grp->npins * sizeof(unsigned int),
 				GFP_KERNEL);
 	grp->mux_mode = devm_kzalloc(info->dev, grp->npins * sizeof(unsigned int),
 				GFP_KERNEL);
+	grp->input_reg = devm_kzalloc(info->dev, grp->npins * sizeof(u16),
+				GFP_KERNEL);
+	grp->input_val = devm_kzalloc(info->dev, grp->npins * sizeof(unsigned int),
+				GFP_KERNEL);
 	grp->configs = devm_kzalloc(info->dev, grp->npins * sizeof(unsigned long),
 				GFP_KERNEL);
-	for (i = 0, j = 0; i < size; i += 2, j++) {
-		pin_func_id = be32_to_cpu(*list++);
-		ret = imx_pinctrl_get_pin_id_and_mux(info, pin_func_id,
-					&grp->pins[j], &grp->mux_mode[j]);
-		if (ret) {
-			dev_err(info->dev, "get invalid pin function id\n");
-			return -EINVAL;
-		}
+	for (i = 0; i < grp->npins; i++) {
+		u32 mux_reg = be32_to_cpu(*list++);
+		u32 conf_reg = be32_to_cpu(*list++);
+		unsigned int pin_id = mux_reg ? mux_reg / 4 : conf_reg / 4;
+		struct imx_pin_reg *pin_reg = &info->pin_regs[pin_id];
+
+		grp->pins[i] = pin_id;
+		pin_reg->mux_reg = mux_reg;
+		pin_reg->conf_reg = conf_reg;
+		grp->input_reg[i] = be32_to_cpu(*list++);
+		grp->mux_mode[i] = be32_to_cpu(*list++);
+		grp->input_val[i] = be32_to_cpu(*list++);
+
 		/* SION bit is in mux register */
 		config = be32_to_cpu(*list++);
 		if (config & IMX_PAD_SION)
-			grp->mux_mode[j] |= IOMUXC_CONFIG_SION;
-		grp->configs[j] = config & ~IMX_PAD_SION;
+			grp->mux_mode[i] |= IOMUXC_CONFIG_SION;
+		grp->configs[i] = config & ~IMX_PAD_SION;
 	}
 
 #ifdef DEBUG
@@ -568,8 +532,7 @@ int imx_pinctrl_probe(struct platform_device *pdev,
 	struct resource *res;
 	int ret;
 
-	if (!info || !info->pins || !info->npins
-		  || !info->pin_regs || !info->npin_regs) {
+	if (!info || !info->pins || !info->npins) {
 		dev_err(&pdev->dev, "wrong pinctrl info\n");
 		return -EINVAL;
 	}
@@ -580,6 +543,11 @@ int imx_pinctrl_probe(struct platform_device *pdev,
 	if (!ipctl)
 		return -ENOMEM;
 
+	info->pin_regs = devm_kzalloc(&pdev->dev, sizeof(*info->pin_regs) *
+				      info->npins, GFP_KERNEL);
+	if (!info->pin_regs)
+		return -ENOMEM;
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res)
 		return -ENOENT;
diff --git a/drivers/pinctrl/pinctrl-imx.h b/drivers/pinctrl/pinctrl-imx.h
index 9b65e7828f1d..607ef5497552 100644
--- a/drivers/pinctrl/pinctrl-imx.h
+++ b/drivers/pinctrl/pinctrl-imx.h
@@ -26,6 +26,10 @@ struct platform_device;
  *	elements in .pins so we can iterate over that array
  * @mux_mode: the mux mode for each pin in this group. The size of this
  *	array is the same as pins.
+ * @input_reg: select input register offset for this mux if any
+ *	0 if no select input setting needed.
+ * @input_val: the select input value for each pin in this group. The size of
+ *	this array is the same as pins.
  * @configs: the config for each pin in this group. The size of this
  *	array is the same as pins.
  */
@@ -34,6 +38,8 @@ struct imx_pin_group {
 	unsigned int *pins;
 	unsigned npins;
 	unsigned int *mux_mode;
+	u16 *input_reg;
+	unsigned int *input_val;
 	unsigned long *configs;
 };
 
@@ -51,30 +57,19 @@ struct imx_pmx_func {
 
 /**
  * struct imx_pin_reg - describe a pin reg map
- * The last 3 members are used for select input setting
- * @pid: pin id
  * @mux_reg: mux register offset
  * @conf_reg: config register offset
- * @mux_mode: mux mode
- * @input_reg: select input register offset for this mux if any
- *  0 if no select input setting needed.
- * @input_val: the value set to select input register
  */
 struct imx_pin_reg {
-	u16 pid;
 	u16 mux_reg;
 	u16 conf_reg;
-	u8 mux_mode;
-	u16 input_reg;
-	u8 input_val;
 };
 
 struct imx_pinctrl_soc_info {
 	struct device *dev;
 	const struct pinctrl_pin_desc *pins;
 	unsigned int npins;
-	const struct imx_pin_reg *pin_regs;
-	unsigned int npin_regs;
+	struct imx_pin_reg *pin_regs;
 	struct imx_pin_group *groups;
 	unsigned int ngroups;
 	struct imx_pmx_func *functions;
@@ -84,16 +79,6 @@ struct imx_pinctrl_soc_info {
 #define NO_MUX		0x0
 #define NO_PAD		0x0
 
-#define IMX_PIN_REG(id, conf, mux, mode, input, val)	\
-	{						\
-		.pid = id,				\
-		.conf_reg = conf,			\
-		.mux_reg = mux,				\
-		.mux_mode  = mode,			\
-		.input_reg = input,			\
-		.input_val = val,			\
-	}
-
 #define IMX_PINCTRL_PIN(pin) PINCTRL_PIN(pin, #pin)
 
 #define PAD_CTL_MASK(len)	((1 << len) - 1)
diff --git a/drivers/pinctrl/pinctrl-imx35.c b/drivers/pinctrl/pinctrl-imx35.c
index 6e214110e3d5..c4549829fc47 100644
--- a/drivers/pinctrl/pinctrl-imx35.c
+++ b/drivers/pinctrl/pinctrl-imx35.c
@@ -24,1249 +24,496 @@
 #include "pinctrl-imx.h"
 
 enum imx35_pads {
-	MX35_PAD_CAPTURE = 0,
-	MX35_PAD_COMPARE = 1,
-	MX35_PAD_WDOG_RST = 2,
-	MX35_PAD_GPIO1_0 = 3,
-	MX35_PAD_GPIO1_1 = 4,
-	MX35_PAD_GPIO2_0 = 5,
-	MX35_PAD_GPIO3_0 = 6,
-	MX35_PAD_RESET_IN_B = 7,
-	MX35_PAD_POR_B = 8,
-	MX35_PAD_CLKO = 9,
-	MX35_PAD_BOOT_MODE0 = 10,
-	MX35_PAD_BOOT_MODE1 = 11,
-	MX35_PAD_CLK_MODE0 = 12,
-	MX35_PAD_CLK_MODE1 = 13,
-	MX35_PAD_POWER_FAIL = 14,
-	MX35_PAD_VSTBY = 15,
-	MX35_PAD_A0 = 16,
-	MX35_PAD_A1 = 17,
-	MX35_PAD_A2 = 18,
-	MX35_PAD_A3 = 19,
-	MX35_PAD_A4 = 20,
-	MX35_PAD_A5 = 21,
-	MX35_PAD_A6 = 22,
-	MX35_PAD_A7 = 23,
-	MX35_PAD_A8 = 24,
-	MX35_PAD_A9 = 25,
-	MX35_PAD_A10 = 26,
-	MX35_PAD_MA10 = 27,
-	MX35_PAD_A11 = 28,
-	MX35_PAD_A12 = 29,
-	MX35_PAD_A13 = 30,
-	MX35_PAD_A14 = 31,
-	MX35_PAD_A15 = 32,
-	MX35_PAD_A16 = 33,
-	MX35_PAD_A17 = 34,
-	MX35_PAD_A18 = 35,
-	MX35_PAD_A19 = 36,
-	MX35_PAD_A20 = 37,
-	MX35_PAD_A21 = 38,
-	MX35_PAD_A22 = 39,
-	MX35_PAD_A23 = 40,
-	MX35_PAD_A24 = 41,
-	MX35_PAD_A25 = 42,
-	MX35_PAD_SDBA1 = 43,
-	MX35_PAD_SDBA0 = 44,
-	MX35_PAD_SD0 = 45,
-	MX35_PAD_SD1 = 46,
-	MX35_PAD_SD2 = 47,
-	MX35_PAD_SD3 = 48,
-	MX35_PAD_SD4 = 49,
-	MX35_PAD_SD5 = 50,
-	MX35_PAD_SD6 = 51,
-	MX35_PAD_SD7 = 52,
-	MX35_PAD_SD8 = 53,
-	MX35_PAD_SD9 = 54,
-	MX35_PAD_SD10 = 55,
-	MX35_PAD_SD11 = 56,
-	MX35_PAD_SD12 = 57,
-	MX35_PAD_SD13 = 58,
-	MX35_PAD_SD14 = 59,
-	MX35_PAD_SD15 = 60,
-	MX35_PAD_SD16 = 61,
-	MX35_PAD_SD17 = 62,
-	MX35_PAD_SD18 = 63,
-	MX35_PAD_SD19 = 64,
-	MX35_PAD_SD20 = 65,
-	MX35_PAD_SD21 = 66,
-	MX35_PAD_SD22 = 67,
-	MX35_PAD_SD23 = 68,
-	MX35_PAD_SD24 = 69,
-	MX35_PAD_SD25 = 70,
-	MX35_PAD_SD26 = 71,
-	MX35_PAD_SD27 = 72,
-	MX35_PAD_SD28 = 73,
-	MX35_PAD_SD29 = 74,
-	MX35_PAD_SD30 = 75,
-	MX35_PAD_SD31 = 76,
-	MX35_PAD_DQM0 = 77,
-	MX35_PAD_DQM1 = 78,
-	MX35_PAD_DQM2 = 79,
-	MX35_PAD_DQM3 = 80,
-	MX35_PAD_EB0 = 81,
-	MX35_PAD_EB1 = 82,
-	MX35_PAD_OE = 83,
-	MX35_PAD_CS0 = 84,
-	MX35_PAD_CS1 = 85,
-	MX35_PAD_CS2 = 86,
-	MX35_PAD_CS3 = 87,
-	MX35_PAD_CS4 = 88,
-	MX35_PAD_CS5 = 89,
-	MX35_PAD_NF_CE0 = 90,
-	MX35_PAD_ECB = 91,
-	MX35_PAD_LBA = 92,
-	MX35_PAD_BCLK = 93,
-	MX35_PAD_RW = 94,
-	MX35_PAD_RAS = 95,
-	MX35_PAD_CAS = 96,
-	MX35_PAD_SDWE = 97,
-	MX35_PAD_SDCKE0 = 98,
-	MX35_PAD_SDCKE1 = 99,
-	MX35_PAD_SDCLK = 100,
-	MX35_PAD_SDQS0 = 101,
-	MX35_PAD_SDQS1 = 102,
-	MX35_PAD_SDQS2 = 103,
-	MX35_PAD_SDQS3 = 104,
-	MX35_PAD_NFWE_B = 105,
-	MX35_PAD_NFRE_B = 106,
-	MX35_PAD_NFALE = 107,
-	MX35_PAD_NFCLE = 108,
-	MX35_PAD_NFWP_B = 109,
-	MX35_PAD_NFRB = 110,
-	MX35_PAD_D15 = 111,
-	MX35_PAD_D14 = 112,
-	MX35_PAD_D13 = 113,
-	MX35_PAD_D12 = 114,
-	MX35_PAD_D11 = 115,
-	MX35_PAD_D10 = 116,
-	MX35_PAD_D9 = 117,
-	MX35_PAD_D8 = 118,
-	MX35_PAD_D7 = 119,
-	MX35_PAD_D6 = 120,
-	MX35_PAD_D5 = 121,
-	MX35_PAD_D4 = 122,
-	MX35_PAD_D3 = 123,
-	MX35_PAD_D2 = 124,
-	MX35_PAD_D1 = 125,
-	MX35_PAD_D0 = 126,
-	MX35_PAD_CSI_D8 = 127,
-	MX35_PAD_CSI_D9 = 128,
-	MX35_PAD_CSI_D10 = 129,
-	MX35_PAD_CSI_D11 = 130,
-	MX35_PAD_CSI_D12 = 131,
-	MX35_PAD_CSI_D13 = 132,
-	MX35_PAD_CSI_D14 = 133,
-	MX35_PAD_CSI_D15 = 134,
-	MX35_PAD_CSI_MCLK = 135,
-	MX35_PAD_CSI_VSYNC = 136,
-	MX35_PAD_CSI_HSYNC = 137,
-	MX35_PAD_CSI_PIXCLK = 138,
-	MX35_PAD_I2C1_CLK = 139,
-	MX35_PAD_I2C1_DAT = 140,
-	MX35_PAD_I2C2_CLK = 141,
-	MX35_PAD_I2C2_DAT = 142,
-	MX35_PAD_STXD4 = 143,
-	MX35_PAD_SRXD4 = 144,
-	MX35_PAD_SCK4 = 145,
-	MX35_PAD_STXFS4 = 146,
-	MX35_PAD_STXD5 = 147,
-	MX35_PAD_SRXD5 = 148,
-	MX35_PAD_SCK5 = 149,
-	MX35_PAD_STXFS5 = 150,
-	MX35_PAD_SCKR = 151,
-	MX35_PAD_FSR = 152,
-	MX35_PAD_HCKR = 153,
-	MX35_PAD_SCKT = 154,
-	MX35_PAD_FST = 155,
-	MX35_PAD_HCKT = 156,
-	MX35_PAD_TX5_RX0 = 157,
-	MX35_PAD_TX4_RX1 = 158,
-	MX35_PAD_TX3_RX2 = 159,
-	MX35_PAD_TX2_RX3 = 160,
-	MX35_PAD_TX1 = 161,
-	MX35_PAD_TX0 = 162,
-	MX35_PAD_CSPI1_MOSI = 163,
-	MX35_PAD_CSPI1_MISO = 164,
-	MX35_PAD_CSPI1_SS0 = 165,
-	MX35_PAD_CSPI1_SS1 = 166,
-	MX35_PAD_CSPI1_SCLK = 167,
-	MX35_PAD_CSPI1_SPI_RDY = 168,
-	MX35_PAD_RXD1 = 169,
-	MX35_PAD_TXD1 = 170,
-	MX35_PAD_RTS1 = 171,
-	MX35_PAD_CTS1 = 172,
-	MX35_PAD_RXD2 = 173,
-	MX35_PAD_TXD2 = 174,
-	MX35_PAD_RTS2 = 175,
-	MX35_PAD_CTS2 = 176,
-	MX35_PAD_RTCK = 177,
-	MX35_PAD_TCK = 178,
-	MX35_PAD_TMS = 179,
-	MX35_PAD_TDI = 180,
-	MX35_PAD_TDO = 181,
-	MX35_PAD_TRSTB = 182,
-	MX35_PAD_DE_B = 183,
-	MX35_PAD_SJC_MOD = 184,
-	MX35_PAD_USBOTG_PWR = 185,
-	MX35_PAD_USBOTG_OC = 186,
-	MX35_PAD_LD0 = 187,
-	MX35_PAD_LD1 = 188,
-	MX35_PAD_LD2 = 189,
-	MX35_PAD_LD3 = 190,
-	MX35_PAD_LD4 = 191,
-	MX35_PAD_LD5 = 192,
-	MX35_PAD_LD6 = 193,
-	MX35_PAD_LD7 = 194,
-	MX35_PAD_LD8 = 195,
-	MX35_PAD_LD9 = 196,
-	MX35_PAD_LD10 = 197,
-	MX35_PAD_LD11 = 198,
-	MX35_PAD_LD12 = 199,
-	MX35_PAD_LD13 = 200,
-	MX35_PAD_LD14 = 201,
-	MX35_PAD_LD15 = 202,
-	MX35_PAD_LD16 = 203,
-	MX35_PAD_LD17 = 204,
-	MX35_PAD_LD18 = 205,
-	MX35_PAD_LD19 = 206,
-	MX35_PAD_LD20 = 207,
-	MX35_PAD_LD21 = 208,
-	MX35_PAD_LD22 = 209,
-	MX35_PAD_LD23 = 210,
-	MX35_PAD_D3_HSYNC = 211,
-	MX35_PAD_D3_FPSHIFT = 212,
-	MX35_PAD_D3_DRDY = 213,
-	MX35_PAD_CONTRAST = 214,
-	MX35_PAD_D3_VSYNC = 215,
-	MX35_PAD_D3_REV = 216,
-	MX35_PAD_D3_CLS = 217,
-	MX35_PAD_D3_SPL = 218,
-	MX35_PAD_SD1_CMD = 219,
-	MX35_PAD_SD1_CLK = 220,
-	MX35_PAD_SD1_DATA0 = 221,
-	MX35_PAD_SD1_DATA1 = 222,
-	MX35_PAD_SD1_DATA2 = 223,
-	MX35_PAD_SD1_DATA3 = 224,
-	MX35_PAD_SD2_CMD = 225,
-	MX35_PAD_SD2_CLK = 226,
-	MX35_PAD_SD2_DATA0 = 227,
-	MX35_PAD_SD2_DATA1 = 228,
-	MX35_PAD_SD2_DATA2 = 229,
-	MX35_PAD_SD2_DATA3 = 230,
-	MX35_PAD_ATA_CS0 = 231,
-	MX35_PAD_ATA_CS1 = 232,
-	MX35_PAD_ATA_DIOR = 233,
-	MX35_PAD_ATA_DIOW = 234,
-	MX35_PAD_ATA_DMACK = 235,
-	MX35_PAD_ATA_RESET_B = 236,
-	MX35_PAD_ATA_IORDY = 237,
-	MX35_PAD_ATA_DATA0 = 238,
-	MX35_PAD_ATA_DATA1 = 239,
-	MX35_PAD_ATA_DATA2 = 240,
-	MX35_PAD_ATA_DATA3 = 241,
-	MX35_PAD_ATA_DATA4 = 242,
-	MX35_PAD_ATA_DATA5 = 243,
-	MX35_PAD_ATA_DATA6 = 244,
-	MX35_PAD_ATA_DATA7 = 245,
-	MX35_PAD_ATA_DATA8 = 246,
-	MX35_PAD_ATA_DATA9 = 247,
-	MX35_PAD_ATA_DATA10 = 248,
-	MX35_PAD_ATA_DATA11 = 249,
-	MX35_PAD_ATA_DATA12 = 250,
-	MX35_PAD_ATA_DATA13 = 251,
-	MX35_PAD_ATA_DATA14 = 252,
-	MX35_PAD_ATA_DATA15 = 253,
-	MX35_PAD_ATA_INTRQ = 254,
-	MX35_PAD_ATA_BUFF_EN = 255,
-	MX35_PAD_ATA_DMARQ = 256,
-	MX35_PAD_ATA_DA0 = 257,
-	MX35_PAD_ATA_DA1 = 258,
-	MX35_PAD_ATA_DA2 = 259,
-	MX35_PAD_MLB_CLK = 260,
-	MX35_PAD_MLB_DAT = 261,
-	MX35_PAD_MLB_SIG = 262,
-	MX35_PAD_FEC_TX_CLK = 263,
-	MX35_PAD_FEC_RX_CLK = 264,
-	MX35_PAD_FEC_RX_DV = 265,
-	MX35_PAD_FEC_COL = 266,
-	MX35_PAD_FEC_RDATA0 = 267,
-	MX35_PAD_FEC_TDATA0 = 268,
-	MX35_PAD_FEC_TX_EN = 269,
-	MX35_PAD_FEC_MDC = 270,
-	MX35_PAD_FEC_MDIO = 271,
-	MX35_PAD_FEC_TX_ERR = 272,
-	MX35_PAD_FEC_RX_ERR = 273,
-	MX35_PAD_FEC_CRS = 274,
-	MX35_PAD_FEC_RDATA1 = 275,
-	MX35_PAD_FEC_TDATA1 = 276,
-	MX35_PAD_FEC_RDATA2 = 277,
-	MX35_PAD_FEC_TDATA2 = 278,
-	MX35_PAD_FEC_RDATA3 = 279,
-	MX35_PAD_FEC_TDATA3 = 280,
-	MX35_PAD_EXT_ARMCLK = 281,
-	MX35_PAD_TEST_MODE = 282,
-};
-
-/* imx35 register maps */
-static struct imx_pin_reg imx35_pin_regs[] = {
-	[0] = IMX_PIN_REG(MX35_PAD_CAPTURE, 0x328, 0x004, 0, 0x0, 0), /* MX35_PAD_CAPTURE__GPT_CAPIN1 */
-	[1] = IMX_PIN_REG(MX35_PAD_CAPTURE, 0x328, 0x004, 1, 0x0, 0), /* MX35_PAD_CAPTURE__GPT_CMPOUT2 */
-	[2] = IMX_PIN_REG(MX35_PAD_CAPTURE, 0x328, 0x004, 2, 0x7f4, 0), /* MX35_PAD_CAPTURE__CSPI2_SS1 */
-	[3] = IMX_PIN_REG(MX35_PAD_CAPTURE, 0x328, 0x004, 3, 0x0, 0), /* MX35_PAD_CAPTURE__EPIT1_EPITO */
-	[4] = IMX_PIN_REG(MX35_PAD_CAPTURE, 0x328, 0x004, 4, 0x7d0, 0), /* MX35_PAD_CAPTURE__CCM_CLK32K */
-	[5] = IMX_PIN_REG(MX35_PAD_CAPTURE, 0x328, 0x004, 5, 0x850, 0), /* MX35_PAD_CAPTURE__GPIO1_4 */
-	[6] = IMX_PIN_REG(MX35_PAD_COMPARE, 0x32c, 0x008, 0, 0x0, 0), /* MX35_PAD_COMPARE__GPT_CMPOUT1 */
-	[7] = IMX_PIN_REG(MX35_PAD_COMPARE, 0x32c, 0x008, 1, 0x0, 0), /* MX35_PAD_COMPARE__GPT_CAPIN2 */
-	[8] = IMX_PIN_REG(MX35_PAD_COMPARE, 0x32c, 0x008, 2, 0x0, 0), /* MX35_PAD_COMPARE__GPT_CMPOUT3 */
-	[9] = IMX_PIN_REG(MX35_PAD_COMPARE, 0x32c, 0x008, 3, 0x0, 0), /* MX35_PAD_COMPARE__EPIT2_EPITO */
-	[10] = IMX_PIN_REG(MX35_PAD_COMPARE, 0x32c, 0x008, 5, 0x854, 0), /* MX35_PAD_COMPARE__GPIO1_5 */
-	[11] = IMX_PIN_REG(MX35_PAD_COMPARE, 0x32c, 0x008, 7, 0x0, 0), /* MX35_PAD_COMPARE__SDMA_EXTDMA_2 */
-	[12] = IMX_PIN_REG(MX35_PAD_WDOG_RST, 0x330, 0x00c, 0, 0x0, 0), /* MX35_PAD_WDOG_RST__WDOG_WDOG_B */
-	[13] = IMX_PIN_REG(MX35_PAD_WDOG_RST, 0x330, 0x00c, 3, 0x0, 0), /* MX35_PAD_WDOG_RST__IPU_FLASH_STROBE */
-	[14] = IMX_PIN_REG(MX35_PAD_WDOG_RST, 0x330, 0x00c, 5, 0x858, 0), /* MX35_PAD_WDOG_RST__GPIO1_6 */
-	[15] = IMX_PIN_REG(MX35_PAD_GPIO1_0, 0x334, 0x010, 0, 0x82c, 0), /* MX35_PAD_GPIO1_0__GPIO1_0 */
-	[16] = IMX_PIN_REG(MX35_PAD_GPIO1_0, 0x334, 0x010, 1, 0x7d4, 0), /* MX35_PAD_GPIO1_0__CCM_PMIC_RDY */
-	[17] = IMX_PIN_REG(MX35_PAD_GPIO1_0, 0x334, 0x010, 2, 0x990, 0), /* MX35_PAD_GPIO1_0__OWIRE_LINE */
-	[18] = IMX_PIN_REG(MX35_PAD_GPIO1_0, 0x334, 0x010, 7, 0x0, 0), /* MX35_PAD_GPIO1_0__SDMA_EXTDMA_0 */
-	[19] = IMX_PIN_REG(MX35_PAD_GPIO1_1, 0x338, 0x014, 0, 0x838, 0), /* MX35_PAD_GPIO1_1__GPIO1_1 */
-	[20] = IMX_PIN_REG(MX35_PAD_GPIO1_1, 0x338, 0x014, 2, 0x0, 0), /* MX35_PAD_GPIO1_1__PWM_PWMO */
-	[21] = IMX_PIN_REG(MX35_PAD_GPIO1_1, 0x338, 0x014, 3, 0x7d8, 0), /* MX35_PAD_GPIO1_1__CSPI1_SS2 */
-	[22] = IMX_PIN_REG(MX35_PAD_GPIO1_1, 0x338, 0x014, 6, 0x0, 0), /* MX35_PAD_GPIO1_1__SCC_TAMPER_DETECT */
-	[23] = IMX_PIN_REG(MX35_PAD_GPIO1_1, 0x338, 0x014, 7, 0x0, 0), /* MX35_PAD_GPIO1_1__SDMA_EXTDMA_1 */
-	[24] = IMX_PIN_REG(MX35_PAD_GPIO2_0, 0x33c, 0x018, 0, 0x868, 0), /* MX35_PAD_GPIO2_0__GPIO2_0 */
-	[25] = IMX_PIN_REG(MX35_PAD_GPIO2_0, 0x33c, 0x018, 1, 0x0, 0), /* MX35_PAD_GPIO2_0__USB_TOP_USBOTG_CLK */
-	[26] = IMX_PIN_REG(MX35_PAD_GPIO3_0, 0x340, 0x01c, 0, 0x8e8, 0), /* MX35_PAD_GPIO3_0__GPIO3_0 */
-	[27] = IMX_PIN_REG(MX35_PAD_GPIO3_0, 0x340, 0x01c, 1, 0x0, 0), /* MX35_PAD_GPIO3_0__USB_TOP_USBH2_CLK */
-	[28] = IMX_PIN_REG(MX35_PAD_RESET_IN_B, 0x344, 0x0, 0, 0x0, 0), /* MX35_PAD_RESET_IN_B__CCM_RESET_IN_B */
-	[29] = IMX_PIN_REG(MX35_PAD_POR_B, 0x348, 0x0, 0, 0x0, 0), /* MX35_PAD_POR_B__CCM_POR_B */
-	[30] = IMX_PIN_REG(MX35_PAD_CLKO, 0x34c, 0x020, 0, 0x0, 0), /* MX35_PAD_CLKO__CCM_CLKO */
-	[31] = IMX_PIN_REG(MX35_PAD_CLKO, 0x34c, 0x020, 5, 0x860, 0), /* MX35_PAD_CLKO__GPIO1_8 */
-	[32] = IMX_PIN_REG(MX35_PAD_BOOT_MODE0, 0x350, 0x0, 0, 0x0, 0), /* MX35_PAD_BOOT_MODE0__CCM_BOOT_MODE_0 */
-	[33] = IMX_PIN_REG(MX35_PAD_BOOT_MODE1, 0x354, 0x0, 0, 0x0, 0), /* MX35_PAD_BOOT_MODE1__CCM_BOOT_MODE_1 */
-	[34] = IMX_PIN_REG(MX35_PAD_CLK_MODE0, 0x358, 0x0, 0, 0x0, 0), /* MX35_PAD_CLK_MODE0__CCM_CLK_MODE_0 */
-	[35] = IMX_PIN_REG(MX35_PAD_CLK_MODE1, 0x35c, 0x0, 0, 0x0, 0), /* MX35_PAD_CLK_MODE1__CCM_CLK_MODE_1 */
-	[36] = IMX_PIN_REG(MX35_PAD_POWER_FAIL, 0x360, 0x0, 0, 0x0, 0), /* MX35_PAD_POWER_FAIL__CCM_DSM_WAKEUP_INT_26 */
-	[37] = IMX_PIN_REG(MX35_PAD_VSTBY, 0x364, 0x024, 0, 0x0, 0), /* MX35_PAD_VSTBY__CCM_VSTBY */
-	[38] = IMX_PIN_REG(MX35_PAD_VSTBY, 0x364, 0x024, 5, 0x85c, 0), /* MX35_PAD_VSTBY__GPIO1_7 */
-	[39] = IMX_PIN_REG(MX35_PAD_A0, 0x368, 0x028, 0, 0x0, 0), /* MX35_PAD_A0__EMI_EIM_DA_L_0 */
-	[40] = IMX_PIN_REG(MX35_PAD_A1, 0x36c, 0x02c, 0, 0x0, 0), /* MX35_PAD_A1__EMI_EIM_DA_L_1 */
-	[41] = IMX_PIN_REG(MX35_PAD_A2, 0x370, 0x030, 0, 0x0, 0), /* MX35_PAD_A2__EMI_EIM_DA_L_2 */
-	[42] = IMX_PIN_REG(MX35_PAD_A3, 0x374, 0x034, 0, 0x0, 0), /* MX35_PAD_A3__EMI_EIM_DA_L_3 */
-	[43] = IMX_PIN_REG(MX35_PAD_A4, 0x378, 0x038, 0, 0x0, 0), /* MX35_PAD_A4__EMI_EIM_DA_L_4 */
-	[44] = IMX_PIN_REG(MX35_PAD_A5, 0x37c, 0x03c, 0, 0x0, 0), /* MX35_PAD_A5__EMI_EIM_DA_L_5 */
-	[45] = IMX_PIN_REG(MX35_PAD_A6, 0x380, 0x040, 0, 0x0, 0), /* MX35_PAD_A6__EMI_EIM_DA_L_6 */
-	[46] = IMX_PIN_REG(MX35_PAD_A7, 0x384, 0x044, 0, 0x0, 0), /* MX35_PAD_A7__EMI_EIM_DA_L_7 */
-	[47] = IMX_PIN_REG(MX35_PAD_A8, 0x388, 0x048, 0, 0x0, 0), /* MX35_PAD_A8__EMI_EIM_DA_H_8 */
-	[48] = IMX_PIN_REG(MX35_PAD_A9, 0x38c, 0x04c, 0, 0x0, 0), /* MX35_PAD_A9__EMI_EIM_DA_H_9 */
-	[49] = IMX_PIN_REG(MX35_PAD_A10, 0x390, 0x050, 0, 0x0, 0), /* MX35_PAD_A10__EMI_EIM_DA_H_10 */
-	[50] = IMX_PIN_REG(MX35_PAD_MA10, 0x394, 0x054, 0, 0x0, 0), /* MX35_PAD_MA10__EMI_MA10 */
-	[51] = IMX_PIN_REG(MX35_PAD_A11, 0x398, 0x058, 0, 0x0, 0), /* MX35_PAD_A11__EMI_EIM_DA_H_11 */
-	[52] = IMX_PIN_REG(MX35_PAD_A12, 0x39c, 0x05c, 0, 0x0, 0), /* MX35_PAD_A12__EMI_EIM_DA_H_12 */
-	[53] = IMX_PIN_REG(MX35_PAD_A13, 0x3a0, 0x060, 0, 0x0, 0), /* MX35_PAD_A13__EMI_EIM_DA_H_13 */
-	[54] = IMX_PIN_REG(MX35_PAD_A14, 0x3a4, 0x064, 0, 0x0, 0), /* MX35_PAD_A14__EMI_EIM_DA_H2_14 */
-	[55] = IMX_PIN_REG(MX35_PAD_A15, 0x3a8, 0x068, 0, 0x0, 0), /* MX35_PAD_A15__EMI_EIM_DA_H2_15 */
-	[56] = IMX_PIN_REG(MX35_PAD_A16, 0x3ac, 0x06c, 0, 0x0, 0), /* MX35_PAD_A16__EMI_EIM_A_16 */
-	[57] = IMX_PIN_REG(MX35_PAD_A17, 0x3b0, 0x070, 0, 0x0, 0), /* MX35_PAD_A17__EMI_EIM_A_17 */
-	[58] = IMX_PIN_REG(MX35_PAD_A18, 0x3b4, 0x074, 0, 0x0, 0), /* MX35_PAD_A18__EMI_EIM_A_18 */
-	[59] = IMX_PIN_REG(MX35_PAD_A19, 0x3b8, 0x078, 0, 0x0, 0), /* MX35_PAD_A19__EMI_EIM_A_19 */
-	[60] = IMX_PIN_REG(MX35_PAD_A20, 0x3bc, 0x07c, 0, 0x0, 0), /* MX35_PAD_A20__EMI_EIM_A_20 */
-	[61] = IMX_PIN_REG(MX35_PAD_A21, 0x3c0, 0x080, 0, 0x0, 0), /* MX35_PAD_A21__EMI_EIM_A_21 */
-	[62] = IMX_PIN_REG(MX35_PAD_A22, 0x3c4, 0x084, 0, 0x0, 0), /* MX35_PAD_A22__EMI_EIM_A_22 */
-	[63] = IMX_PIN_REG(MX35_PAD_A23, 0x3c8, 0x088, 0, 0x0, 0), /* MX35_PAD_A23__EMI_EIM_A_23 */
-	[64] = IMX_PIN_REG(MX35_PAD_A24, 0x3cc, 0x08c, 0, 0x0, 0), /* MX35_PAD_A24__EMI_EIM_A_24 */
-	[65] = IMX_PIN_REG(MX35_PAD_A25, 0x3d0, 0x090, 0, 0x0, 0), /* MX35_PAD_A25__EMI_EIM_A_25 */
-	[66] = IMX_PIN_REG(MX35_PAD_SDBA1, 0x3d4, 0x0, 0, 0x0, 0), /* MX35_PAD_SDBA1__EMI_EIM_SDBA1 */
-	[67] = IMX_PIN_REG(MX35_PAD_SDBA0, 0x3d8, 0x0, 0, 0x0, 0), /* MX35_PAD_SDBA0__EMI_EIM_SDBA0 */
-	[68] = IMX_PIN_REG(MX35_PAD_SD0, 0x3dc, 0x0, 0, 0x0, 0), /* MX35_PAD_SD0__EMI_DRAM_D_0 */
-	[69] = IMX_PIN_REG(MX35_PAD_SD1, 0x3e0, 0x0, 0, 0x0, 0), /* MX35_PAD_SD1__EMI_DRAM_D_1 */
-	[70] = IMX_PIN_REG(MX35_PAD_SD2, 0x3e4, 0x0, 0, 0x0, 0), /* MX35_PAD_SD2__EMI_DRAM_D_2 */
-	[71] = IMX_PIN_REG(MX35_PAD_SD3, 0x3e8, 0x0, 0, 0x0, 0), /* MX35_PAD_SD3__EMI_DRAM_D_3 */
-	[72] = IMX_PIN_REG(MX35_PAD_SD4, 0x3ec, 0x0, 0, 0x0, 0), /* MX35_PAD_SD4__EMI_DRAM_D_4 */
-	[73] = IMX_PIN_REG(MX35_PAD_SD5, 0x3f0, 0x0, 0, 0x0, 0), /* MX35_PAD_SD5__EMI_DRAM_D_5 */
-	[74] = IMX_PIN_REG(MX35_PAD_SD6, 0x3f4, 0x0, 0, 0x0, 0), /* MX35_PAD_SD6__EMI_DRAM_D_6 */
-	[75] = IMX_PIN_REG(MX35_PAD_SD7, 0x3f8, 0x0, 0, 0x0, 0), /* MX35_PAD_SD7__EMI_DRAM_D_7 */
-	[76] = IMX_PIN_REG(MX35_PAD_SD8, 0x3fc, 0x0, 0, 0x0, 0), /* MX35_PAD_SD8__EMI_DRAM_D_8 */
-	[77] = IMX_PIN_REG(MX35_PAD_SD9, 0x400, 0x0, 0, 0x0, 0), /* MX35_PAD_SD9__EMI_DRAM_D_9 */
-	[78] = IMX_PIN_REG(MX35_PAD_SD10, 0x404, 0x0, 0, 0x0, 0), /* MX35_PAD_SD10__EMI_DRAM_D_10 */
-	[79] = IMX_PIN_REG(MX35_PAD_SD11, 0x408, 0x0, 0, 0x0, 0), /* MX35_PAD_SD11__EMI_DRAM_D_11 */
-	[80] = IMX_PIN_REG(MX35_PAD_SD12, 0x40c, 0x0, 0, 0x0, 0), /* MX35_PAD_SD12__EMI_DRAM_D_12 */
-	[81] = IMX_PIN_REG(MX35_PAD_SD13, 0x410, 0x0, 0, 0x0, 0), /* MX35_PAD_SD13__EMI_DRAM_D_13 */
-	[82] = IMX_PIN_REG(MX35_PAD_SD14, 0x414, 0x0, 0, 0x0, 0), /* MX35_PAD_SD14__EMI_DRAM_D_14 */
-	[83] = IMX_PIN_REG(MX35_PAD_SD15, 0x418, 0x0, 0, 0x0, 0), /* MX35_PAD_SD15__EMI_DRAM_D_15 */
-	[84] = IMX_PIN_REG(MX35_PAD_SD16, 0x41c, 0x0, 0, 0x0, 0), /* MX35_PAD_SD16__EMI_DRAM_D_16 */
-	[85] = IMX_PIN_REG(MX35_PAD_SD17, 0x420, 0x0, 0, 0x0, 0), /* MX35_PAD_SD17__EMI_DRAM_D_17 */
-	[86] = IMX_PIN_REG(MX35_PAD_SD18, 0x424, 0x0, 0, 0x0, 0), /* MX35_PAD_SD18__EMI_DRAM_D_18 */
-	[87] = IMX_PIN_REG(MX35_PAD_SD19, 0x428, 0x0, 0, 0x0, 0), /* MX35_PAD_SD19__EMI_DRAM_D_19 */
-	[88] = IMX_PIN_REG(MX35_PAD_SD20, 0x42c, 0x0, 0, 0x0, 0), /* MX35_PAD_SD20__EMI_DRAM_D_20 */
-	[89] = IMX_PIN_REG(MX35_PAD_SD21, 0x430, 0x0, 0, 0x0, 0), /* MX35_PAD_SD21__EMI_DRAM_D_21 */
-	[90] = IMX_PIN_REG(MX35_PAD_SD22, 0x434, 0x0, 0, 0x0, 0), /* MX35_PAD_SD22__EMI_DRAM_D_22 */
-	[91] = IMX_PIN_REG(MX35_PAD_SD23, 0x438, 0x0, 0, 0x0, 0), /* MX35_PAD_SD23__EMI_DRAM_D_23 */
-	[92] = IMX_PIN_REG(MX35_PAD_SD24, 0x43c, 0x0, 0, 0x0, 0), /* MX35_PAD_SD24__EMI_DRAM_D_24 */
-	[93] = IMX_PIN_REG(MX35_PAD_SD25, 0x440, 0x0, 0, 0x0, 0), /* MX35_PAD_SD25__EMI_DRAM_D_25 */
-	[94] = IMX_PIN_REG(MX35_PAD_SD26, 0x444, 0x0, 0, 0x0, 0), /* MX35_PAD_SD26__EMI_DRAM_D_26 */
-	[95] = IMX_PIN_REG(MX35_PAD_SD27, 0x448, 0x0, 0, 0x0, 0), /* MX35_PAD_SD27__EMI_DRAM_D_27 */
-	[96] = IMX_PIN_REG(MX35_PAD_SD28, 0x44c, 0x0, 0, 0x0, 0), /* MX35_PAD_SD28__EMI_DRAM_D_28 */
-	[97] = IMX_PIN_REG(MX35_PAD_SD29, 0x450, 0x0, 0, 0x0, 0), /* MX35_PAD_SD29__EMI_DRAM_D_29 */
-	[98] = IMX_PIN_REG(MX35_PAD_SD30, 0x454, 0x0, 0, 0x0, 0), /* MX35_PAD_SD30__EMI_DRAM_D_30 */
-	[99] = IMX_PIN_REG(MX35_PAD_SD31, 0x458, 0x0, 0, 0x0, 0), /* MX35_PAD_SD31__EMI_DRAM_D_31 */
-	[100] = IMX_PIN_REG(MX35_PAD_DQM0, 0x45c, 0x0, 0, 0x0, 0), /* MX35_PAD_DQM0__EMI_DRAM_DQM_0 */
-	[101] = IMX_PIN_REG(MX35_PAD_DQM1, 0x460, 0x0, 0, 0x0, 0), /* MX35_PAD_DQM1__EMI_DRAM_DQM_1 */
-	[102] = IMX_PIN_REG(MX35_PAD_DQM2, 0x464, 0x0, 0, 0x0, 0), /* MX35_PAD_DQM2__EMI_DRAM_DQM_2 */
-	[103] = IMX_PIN_REG(MX35_PAD_DQM3, 0x468, 0x0, 0, 0x0, 0), /* MX35_PAD_DQM3__EMI_DRAM_DQM_3 */
-	[104] = IMX_PIN_REG(MX35_PAD_EB0, 0x46c, 0x094, 0, 0x0, 0), /* MX35_PAD_EB0__EMI_EIM_EB0_B */
-	[105] = IMX_PIN_REG(MX35_PAD_EB1, 0x470, 0x098, 0, 0x0, 0), /* MX35_PAD_EB1__EMI_EIM_EB1_B */
-	[106] = IMX_PIN_REG(MX35_PAD_OE, 0x474, 0x09c, 0, 0x0, 0), /* MX35_PAD_OE__EMI_EIM_OE */
-	[107] = IMX_PIN_REG(MX35_PAD_CS0, 0x478, 0x0a0, 0, 0x0, 0), /* MX35_PAD_CS0__EMI_EIM_CS0 */
-	[108] = IMX_PIN_REG(MX35_PAD_CS1, 0x47c, 0x0a4, 0, 0x0, 0), /* MX35_PAD_CS1__EMI_EIM_CS1 */
-	[109] = IMX_PIN_REG(MX35_PAD_CS1, 0x47c, 0x0a4, 3, 0x0, 0), /* MX35_PAD_CS1__EMI_NANDF_CE3 */
-	[110] = IMX_PIN_REG(MX35_PAD_CS2, 0x480, 0x0a8, 0, 0x0, 0), /* MX35_PAD_CS2__EMI_EIM_CS2 */
-	[111] = IMX_PIN_REG(MX35_PAD_CS3, 0x484, 0x0ac, 0, 0x0, 0), /* MX35_PAD_CS3__EMI_EIM_CS3 */
-	[112] = IMX_PIN_REG(MX35_PAD_CS4, 0x488, 0x0b0, 0, 0x0, 0), /* MX35_PAD_CS4__EMI_EIM_CS4 */
-	[113] = IMX_PIN_REG(MX35_PAD_CS4, 0x488, 0x0b0, 1, 0x800, 0), /* MX35_PAD_CS4__EMI_DTACK_B */
-	[114] = IMX_PIN_REG(MX35_PAD_CS4, 0x488, 0x0b0, 3, 0x0, 0), /* MX35_PAD_CS4__EMI_NANDF_CE1 */
-	[115] = IMX_PIN_REG(MX35_PAD_CS4, 0x488, 0x0b0, 5, 0x83c, 0), /* MX35_PAD_CS4__GPIO1_20 */
-	[116] = IMX_PIN_REG(MX35_PAD_CS5, 0x48c, 0x0b4, 0, 0x0, 0), /* MX35_PAD_CS5__EMI_EIM_CS5 */
-	[117] = IMX_PIN_REG(MX35_PAD_CS5, 0x48c, 0x0b4, 1, 0x7f8, 0), /* MX35_PAD_CS5__CSPI2_SS2 */
-	[118] = IMX_PIN_REG(MX35_PAD_CS5, 0x48c, 0x0b4, 2, 0x7d8, 1), /* MX35_PAD_CS5__CSPI1_SS2 */
-	[119] = IMX_PIN_REG(MX35_PAD_CS5, 0x48c, 0x0b4, 3, 0x0, 0), /* MX35_PAD_CS5__EMI_NANDF_CE2 */
-	[120] = IMX_PIN_REG(MX35_PAD_CS5, 0x48c, 0x0b4, 5, 0x840, 0), /* MX35_PAD_CS5__GPIO1_21 */
-	[121] = IMX_PIN_REG(MX35_PAD_NF_CE0, 0x490, 0x0b8, 0, 0x0, 0), /* MX35_PAD_NF_CE0__EMI_NANDF_CE0 */
-	[122] = IMX_PIN_REG(MX35_PAD_NF_CE0, 0x490, 0x0b8, 5, 0x844, 0), /* MX35_PAD_NF_CE0__GPIO1_22 */
-	[123] = IMX_PIN_REG(MX35_PAD_ECB, 0x494, 0x0, 0, 0x0, 0), /* MX35_PAD_ECB__EMI_EIM_ECB */
-	[124] = IMX_PIN_REG(MX35_PAD_LBA, 0x498, 0x0bc, 0, 0x0, 0), /* MX35_PAD_LBA__EMI_EIM_LBA */
-	[125] = IMX_PIN_REG(MX35_PAD_BCLK, 0x49c, 0x0c0, 0, 0x0, 0), /* MX35_PAD_BCLK__EMI_EIM_BCLK */
-	[126] = IMX_PIN_REG(MX35_PAD_RW, 0x4a0, 0x0c4, 0, 0x0, 0), /* MX35_PAD_RW__EMI_EIM_RW */
-	[127] = IMX_PIN_REG(MX35_PAD_RAS, 0x4a4, 0x0, 0, 0x0, 0), /* MX35_PAD_RAS__EMI_DRAM_RAS */
-	[128] = IMX_PIN_REG(MX35_PAD_CAS, 0x4a8, 0x0, 0, 0x0, 0), /* MX35_PAD_CAS__EMI_DRAM_CAS */
-	[129] = IMX_PIN_REG(MX35_PAD_SDWE, 0x4ac, 0x0, 0, 0x0, 0), /* MX35_PAD_SDWE__EMI_DRAM_SDWE */
-	[130] = IMX_PIN_REG(MX35_PAD_SDCKE0, 0x4b0, 0x0, 0, 0x0, 0), /* MX35_PAD_SDCKE0__EMI_DRAM_SDCKE_0 */
-	[131] = IMX_PIN_REG(MX35_PAD_SDCKE1, 0x4b4, 0x0, 0, 0x0, 0), /* MX35_PAD_SDCKE1__EMI_DRAM_SDCKE_1 */
-	[132] = IMX_PIN_REG(MX35_PAD_SDCLK, 0x4b8, 0x0, 0, 0x0, 0), /* MX35_PAD_SDCLK__EMI_DRAM_SDCLK */
-	[133] = IMX_PIN_REG(MX35_PAD_SDQS0, 0x4bc, 0x0, 0, 0x0, 0), /* MX35_PAD_SDQS0__EMI_DRAM_SDQS_0 */
-	[134] = IMX_PIN_REG(MX35_PAD_SDQS1, 0x4c0, 0x0, 0, 0x0, 0), /* MX35_PAD_SDQS1__EMI_DRAM_SDQS_1 */
-	[135] = IMX_PIN_REG(MX35_PAD_SDQS2, 0x4c4, 0x0, 0, 0x0, 0), /* MX35_PAD_SDQS2__EMI_DRAM_SDQS_2 */
-	[136] = IMX_PIN_REG(MX35_PAD_SDQS3, 0x4c8, 0x0, 0, 0x0, 0), /* MX35_PAD_SDQS3__EMI_DRAM_SDQS_3 */
-	[137] = IMX_PIN_REG(MX35_PAD_NFWE_B, 0x4cc, 0x0c8, 0, 0x0, 0), /* MX35_PAD_NFWE_B__EMI_NANDF_WE_B */
-	[138] = IMX_PIN_REG(MX35_PAD_NFWE_B, 0x4cc, 0x0c8, 1, 0x9d8, 0), /* MX35_PAD_NFWE_B__USB_TOP_USBH2_DATA_3 */
-	[139] = IMX_PIN_REG(MX35_PAD_NFWE_B, 0x4cc, 0x0c8, 2, 0x924, 0), /* MX35_PAD_NFWE_B__IPU_DISPB_D0_VSYNC */
-	[140] = IMX_PIN_REG(MX35_PAD_NFWE_B, 0x4cc, 0x0c8, 5, 0x88c, 0), /* MX35_PAD_NFWE_B__GPIO2_18 */
-	[141] = IMX_PIN_REG(MX35_PAD_NFWE_B, 0x4cc, 0x0c8, 7, 0x0, 0), /* MX35_PAD_NFWE_B__ARM11P_TOP_TRACE_0 */
-	[142] = IMX_PIN_REG(MX35_PAD_NFRE_B, 0x4d0, 0x0cc, 0, 0x0, 0), /* MX35_PAD_NFRE_B__EMI_NANDF_RE_B */
-	[143] = IMX_PIN_REG(MX35_PAD_NFRE_B, 0x4d0, 0x0cc, 1, 0x9ec, 0), /* MX35_PAD_NFRE_B__USB_TOP_USBH2_DIR */
-	[144] = IMX_PIN_REG(MX35_PAD_NFRE_B, 0x4d0, 0x0cc, 2, 0x0, 0), /* MX35_PAD_NFRE_B__IPU_DISPB_BCLK */
-	[145] = IMX_PIN_REG(MX35_PAD_NFRE_B, 0x4d0, 0x0cc, 5, 0x890, 0), /* MX35_PAD_NFRE_B__GPIO2_19 */
-	[146] = IMX_PIN_REG(MX35_PAD_NFRE_B, 0x4d0, 0x0cc, 7, 0x0, 0), /* MX35_PAD_NFRE_B__ARM11P_TOP_TRACE_1 */
-	[147] = IMX_PIN_REG(MX35_PAD_NFALE, 0x4d4, 0x0d0, 0, 0x0, 0), /* MX35_PAD_NFALE__EMI_NANDF_ALE */
-	[148] = IMX_PIN_REG(MX35_PAD_NFALE, 0x4d4, 0x0d0, 1, 0x0, 0), /* MX35_PAD_NFALE__USB_TOP_USBH2_STP */
-	[149] = IMX_PIN_REG(MX35_PAD_NFALE, 0x4d4, 0x0d0, 2, 0x0, 0), /* MX35_PAD_NFALE__IPU_DISPB_CS0 */
-	[150] = IMX_PIN_REG(MX35_PAD_NFALE, 0x4d4, 0x0d0, 5, 0x898, 0), /* MX35_PAD_NFALE__GPIO2_20 */
-	[151] = IMX_PIN_REG(MX35_PAD_NFALE, 0x4d4, 0x0d0, 7, 0x0, 0), /* MX35_PAD_NFALE__ARM11P_TOP_TRACE_2 */
-	[152] = IMX_PIN_REG(MX35_PAD_NFCLE, 0x4d8, 0x0d4, 0, 0x0, 0), /* MX35_PAD_NFCLE__EMI_NANDF_CLE */
-	[153] = IMX_PIN_REG(MX35_PAD_NFCLE, 0x4d8, 0x0d4, 1, 0x9f0, 0), /* MX35_PAD_NFCLE__USB_TOP_USBH2_NXT */
-	[154] = IMX_PIN_REG(MX35_PAD_NFCLE, 0x4d8, 0x0d4, 2, 0x0, 0), /* MX35_PAD_NFCLE__IPU_DISPB_PAR_RS */
-	[155] = IMX_PIN_REG(MX35_PAD_NFCLE, 0x4d8, 0x0d4, 5, 0x89c, 0), /* MX35_PAD_NFCLE__GPIO2_21 */
-	[156] = IMX_PIN_REG(MX35_PAD_NFCLE, 0x4d8, 0x0d4, 7, 0x0, 0), /* MX35_PAD_NFCLE__ARM11P_TOP_TRACE_3 */
-	[157] = IMX_PIN_REG(MX35_PAD_NFWP_B, 0x4dc, 0x0d8, 0, 0x0, 0), /* MX35_PAD_NFWP_B__EMI_NANDF_WP_B */
-	[158] = IMX_PIN_REG(MX35_PAD_NFWP_B, 0x4dc, 0x0d8, 1, 0x9e8, 0), /* MX35_PAD_NFWP_B__USB_TOP_USBH2_DATA_7 */
-	[159] = IMX_PIN_REG(MX35_PAD_NFWP_B, 0x4dc, 0x0d8, 2, 0x0, 0), /* MX35_PAD_NFWP_B__IPU_DISPB_WR */
-	[160] = IMX_PIN_REG(MX35_PAD_NFWP_B, 0x4dc, 0x0d8, 5, 0x8a0, 0), /* MX35_PAD_NFWP_B__GPIO2_22 */
-	[161] = IMX_PIN_REG(MX35_PAD_NFWP_B, 0x4dc, 0x0d8, 7, 0x0, 0), /* MX35_PAD_NFWP_B__ARM11P_TOP_TRCTL */
-	[162] = IMX_PIN_REG(MX35_PAD_NFRB, 0x4e0, 0x0dc, 0, 0x0, 0), /* MX35_PAD_NFRB__EMI_NANDF_RB */
-	[163] = IMX_PIN_REG(MX35_PAD_NFRB, 0x4e0, 0x0dc, 2, 0x0, 0), /* MX35_PAD_NFRB__IPU_DISPB_RD */
-	[164] = IMX_PIN_REG(MX35_PAD_NFRB, 0x4e0, 0x0dc, 5, 0x8a4, 0), /* MX35_PAD_NFRB__GPIO2_23 */
-	[165] = IMX_PIN_REG(MX35_PAD_NFRB, 0x4e0, 0x0dc, 7, 0x0, 0), /* MX35_PAD_NFRB__ARM11P_TOP_TRCLK */
-	[166] = IMX_PIN_REG(MX35_PAD_D15, 0x4e4, 0x0, 0, 0x0, 0), /* MX35_PAD_D15__EMI_EIM_D_15 */
-	[167] = IMX_PIN_REG(MX35_PAD_D14, 0x4e8, 0x0, 0, 0x0, 0), /* MX35_PAD_D14__EMI_EIM_D_14 */
-	[168] = IMX_PIN_REG(MX35_PAD_D13, 0x4ec, 0x0, 0, 0x0, 0), /* MX35_PAD_D13__EMI_EIM_D_13 */
-	[169] = IMX_PIN_REG(MX35_PAD_D12, 0x4f0, 0x0, 0, 0x0, 0), /* MX35_PAD_D12__EMI_EIM_D_12 */
-	[170] = IMX_PIN_REG(MX35_PAD_D11, 0x4f4, 0x0, 0, 0x0, 0), /* MX35_PAD_D11__EMI_EIM_D_11 */
-	[171] = IMX_PIN_REG(MX35_PAD_D10, 0x4f8, 0x0, 0, 0x0, 0), /* MX35_PAD_D10__EMI_EIM_D_10 */
-	[172] = IMX_PIN_REG(MX35_PAD_D9, 0x4fc, 0x0, 0, 0x0, 0), /* MX35_PAD_D9__EMI_EIM_D_9 */
-	[173] = IMX_PIN_REG(MX35_PAD_D8, 0x500, 0x0, 0, 0x0, 0), /* MX35_PAD_D8__EMI_EIM_D_8 */
-	[174] = IMX_PIN_REG(MX35_PAD_D7, 0x504, 0x0, 0, 0x0, 0), /* MX35_PAD_D7__EMI_EIM_D_7 */
-	[175] = IMX_PIN_REG(MX35_PAD_D6, 0x508, 0x0, 0, 0x0, 0), /* MX35_PAD_D6__EMI_EIM_D_6 */
-	[176] = IMX_PIN_REG(MX35_PAD_D5, 0x50c, 0x0, 0, 0x0, 0), /* MX35_PAD_D5__EMI_EIM_D_5 */
-	[177] = IMX_PIN_REG(MX35_PAD_D4, 0x510, 0x0, 0, 0x0, 0), /* MX35_PAD_D4__EMI_EIM_D_4 */
-	[178] = IMX_PIN_REG(MX35_PAD_D3, 0x514, 0x0, 0, 0x0, 0), /* MX35_PAD_D3__EMI_EIM_D_3 */
-	[179] = IMX_PIN_REG(MX35_PAD_D2, 0x518, 0x0, 0, 0x0, 0), /* MX35_PAD_D2__EMI_EIM_D_2 */
-	[180] = IMX_PIN_REG(MX35_PAD_D1, 0x51c, 0x0, 0, 0x0, 0), /* MX35_PAD_D1__EMI_EIM_D_1 */
-	[181] = IMX_PIN_REG(MX35_PAD_D0, 0x520, 0x0, 0, 0x0, 0), /* MX35_PAD_D0__EMI_EIM_D_0 */
-	[182] = IMX_PIN_REG(MX35_PAD_CSI_D8, 0x524, 0x0e0, 0, 0x0, 0), /* MX35_PAD_CSI_D8__IPU_CSI_D_8 */
-	[183] = IMX_PIN_REG(MX35_PAD_CSI_D8, 0x524, 0x0e0, 1, 0x950, 0), /* MX35_PAD_CSI_D8__KPP_COL_0 */
-	[184] = IMX_PIN_REG(MX35_PAD_CSI_D8, 0x524, 0x0e0, 5, 0x83c, 1), /* MX35_PAD_CSI_D8__GPIO1_20 */
-	[185] = IMX_PIN_REG(MX35_PAD_CSI_D8, 0x524, 0x0e0, 7, 0x0, 0), /* MX35_PAD_CSI_D8__ARM11P_TOP_EVNTBUS_13 */
-	[186] = IMX_PIN_REG(MX35_PAD_CSI_D9, 0x528, 0x0e4, 0, 0x0, 0), /* MX35_PAD_CSI_D9__IPU_CSI_D_9 */
-	[187] = IMX_PIN_REG(MX35_PAD_CSI_D9, 0x528, 0x0e4, 1, 0x954, 0), /* MX35_PAD_CSI_D9__KPP_COL_1 */
-	[188] = IMX_PIN_REG(MX35_PAD_CSI_D9, 0x528, 0x0e4, 5, 0x840, 1), /* MX35_PAD_CSI_D9__GPIO1_21 */
-	[189] = IMX_PIN_REG(MX35_PAD_CSI_D9, 0x528, 0x0e4, 7, 0x0, 0), /* MX35_PAD_CSI_D9__ARM11P_TOP_EVNTBUS_14 */
-	[190] = IMX_PIN_REG(MX35_PAD_CSI_D10, 0x52c, 0x0e8, 0, 0x0, 0), /* MX35_PAD_CSI_D10__IPU_CSI_D_10 */
-	[191] = IMX_PIN_REG(MX35_PAD_CSI_D10, 0x52c, 0x0e8, 1, 0x958, 0), /* MX35_PAD_CSI_D10__KPP_COL_2 */
-	[192] = IMX_PIN_REG(MX35_PAD_CSI_D10, 0x52c, 0x0e8, 5, 0x844, 1), /* MX35_PAD_CSI_D10__GPIO1_22 */
-	[193] = IMX_PIN_REG(MX35_PAD_CSI_D10, 0x52c, 0x0e8, 7, 0x0, 0), /* MX35_PAD_CSI_D10__ARM11P_TOP_EVNTBUS_15 */
-	[194] = IMX_PIN_REG(MX35_PAD_CSI_D11, 0x530, 0x0ec, 0, 0x0, 0), /* MX35_PAD_CSI_D11__IPU_CSI_D_11 */
-	[195] = IMX_PIN_REG(MX35_PAD_CSI_D11, 0x530, 0x0ec, 1, 0x95c, 0), /* MX35_PAD_CSI_D11__KPP_COL_3 */
-	[196] = IMX_PIN_REG(MX35_PAD_CSI_D11, 0x530, 0x0ec, 5, 0x0, 0), /* MX35_PAD_CSI_D11__GPIO1_23 */
-	[197] = IMX_PIN_REG(MX35_PAD_CSI_D12, 0x534, 0x0f0, 0, 0x0, 0), /* MX35_PAD_CSI_D12__IPU_CSI_D_12 */
-	[198] = IMX_PIN_REG(MX35_PAD_CSI_D12, 0x534, 0x0f0, 1, 0x970, 0), /* MX35_PAD_CSI_D12__KPP_ROW_0 */
-	[199] = IMX_PIN_REG(MX35_PAD_CSI_D12, 0x534, 0x0f0, 5, 0x0, 0), /* MX35_PAD_CSI_D12__GPIO1_24 */
-	[200] = IMX_PIN_REG(MX35_PAD_CSI_D13, 0x538, 0x0f4, 0, 0x0, 0), /* MX35_PAD_CSI_D13__IPU_CSI_D_13 */
-	[201] = IMX_PIN_REG(MX35_PAD_CSI_D13, 0x538, 0x0f4, 1, 0x974, 0), /* MX35_PAD_CSI_D13__KPP_ROW_1 */
-	[202] = IMX_PIN_REG(MX35_PAD_CSI_D13, 0x538, 0x0f4, 5, 0x0, 0), /* MX35_PAD_CSI_D13__GPIO1_25 */
-	[203] = IMX_PIN_REG(MX35_PAD_CSI_D14, 0x53c, 0x0f8, 0, 0x0, 0), /* MX35_PAD_CSI_D14__IPU_CSI_D_14 */
-	[204] = IMX_PIN_REG(MX35_PAD_CSI_D14, 0x53c, 0x0f8, 1, 0x978, 0), /* MX35_PAD_CSI_D14__KPP_ROW_2 */
-	[205] = IMX_PIN_REG(MX35_PAD_CSI_D14, 0x53c, 0x0f8, 5, 0x0, 0), /* MX35_PAD_CSI_D14__GPIO1_26 */
-	[206] = IMX_PIN_REG(MX35_PAD_CSI_D15, 0x540, 0x0fc, 0, 0x97c, 0), /* MX35_PAD_CSI_D15__IPU_CSI_D_15 */
-	[207] = IMX_PIN_REG(MX35_PAD_CSI_D15, 0x540, 0x0fc, 1, 0x0, 0), /* MX35_PAD_CSI_D15__KPP_ROW_3 */
-	[208] = IMX_PIN_REG(MX35_PAD_CSI_D15, 0x540, 0x0fc, 5, 0x0, 0), /* MX35_PAD_CSI_D15__GPIO1_27 */
-	[209] = IMX_PIN_REG(MX35_PAD_CSI_MCLK, 0x544, 0x100, 0, 0x0, 0), /* MX35_PAD_CSI_MCLK__IPU_CSI_MCLK */
-	[210] = IMX_PIN_REG(MX35_PAD_CSI_MCLK, 0x544, 0x100, 5, 0x0, 0), /* MX35_PAD_CSI_MCLK__GPIO1_28 */
-	[211] = IMX_PIN_REG(MX35_PAD_CSI_VSYNC, 0x548, 0x104, 0, 0x0, 0), /* MX35_PAD_CSI_VSYNC__IPU_CSI_VSYNC */
-	[212] = IMX_PIN_REG(MX35_PAD_CSI_VSYNC, 0x548, 0x104, 5, 0x0, 0), /* MX35_PAD_CSI_VSYNC__GPIO1_29 */
-	[213] = IMX_PIN_REG(MX35_PAD_CSI_HSYNC, 0x54c, 0x108, 0, 0x0, 0), /* MX35_PAD_CSI_HSYNC__IPU_CSI_HSYNC */
-	[214] = IMX_PIN_REG(MX35_PAD_CSI_HSYNC, 0x54c, 0x108, 5, 0x0, 0), /* MX35_PAD_CSI_HSYNC__GPIO1_30 */
-	[215] = IMX_PIN_REG(MX35_PAD_CSI_PIXCLK, 0x550, 0x10c, 0, 0x0, 0), /* MX35_PAD_CSI_PIXCLK__IPU_CSI_PIXCLK */
-	[216] = IMX_PIN_REG(MX35_PAD_CSI_PIXCLK, 0x550, 0x10c, 5, 0x0, 0), /* MX35_PAD_CSI_PIXCLK__GPIO1_31 */
-	[217] = IMX_PIN_REG(MX35_PAD_I2C1_CLK, 0x554, 0x110, 0, 0x0, 0), /* MX35_PAD_I2C1_CLK__I2C1_SCL */
-	[218] = IMX_PIN_REG(MX35_PAD_I2C1_CLK, 0x554, 0x110, 5, 0x8a8, 0), /* MX35_PAD_I2C1_CLK__GPIO2_24 */
-	[219] = IMX_PIN_REG(MX35_PAD_I2C1_CLK, 0x554, 0x110, 6, 0x0, 0), /* MX35_PAD_I2C1_CLK__CCM_USB_BYP_CLK */
-	[220] = IMX_PIN_REG(MX35_PAD_I2C1_DAT, 0x558, 0x114, 0, 0x0, 0), /* MX35_PAD_I2C1_DAT__I2C1_SDA */
-	[221] = IMX_PIN_REG(MX35_PAD_I2C1_DAT, 0x558, 0x114, 5, 0x8ac, 0), /* MX35_PAD_I2C1_DAT__GPIO2_25 */
-	[222] = IMX_PIN_REG(MX35_PAD_I2C2_CLK, 0x55c, 0x118, 0, 0x0, 0), /* MX35_PAD_I2C2_CLK__I2C2_SCL */
-	[223] = IMX_PIN_REG(MX35_PAD_I2C2_CLK, 0x55c, 0x118, 1, 0x0, 0), /* MX35_PAD_I2C2_CLK__CAN1_TXCAN */
-	[224] = IMX_PIN_REG(MX35_PAD_I2C2_CLK, 0x55c, 0x118, 2, 0x0, 0), /* MX35_PAD_I2C2_CLK__USB_TOP_USBH2_PWR */
-	[225] = IMX_PIN_REG(MX35_PAD_I2C2_CLK, 0x55c, 0x118, 5, 0x8b0, 0), /* MX35_PAD_I2C2_CLK__GPIO2_26 */
-	[226] = IMX_PIN_REG(MX35_PAD_I2C2_CLK, 0x55c, 0x118, 6, 0x0, 0), /* MX35_PAD_I2C2_CLK__SDMA_DEBUG_BUS_DEVICE_2 */
-	[227] = IMX_PIN_REG(MX35_PAD_I2C2_DAT, 0x560, 0x11c, 0, 0x0, 0), /* MX35_PAD_I2C2_DAT__I2C2_SDA */
-	[228] = IMX_PIN_REG(MX35_PAD_I2C2_DAT, 0x560, 0x11c, 1, 0x7c8, 0), /* MX35_PAD_I2C2_DAT__CAN1_RXCAN */
-	[229] = IMX_PIN_REG(MX35_PAD_I2C2_DAT, 0x560, 0x11c, 2, 0x9f4, 0), /* MX35_PAD_I2C2_DAT__USB_TOP_USBH2_OC */
-	[230] = IMX_PIN_REG(MX35_PAD_I2C2_DAT, 0x560, 0x11c, 5, 0x8b4, 0), /* MX35_PAD_I2C2_DAT__GPIO2_27 */
-	[231] = IMX_PIN_REG(MX35_PAD_I2C2_DAT, 0x560, 0x11c, 6, 0x0, 0), /* MX35_PAD_I2C2_DAT__SDMA_DEBUG_BUS_DEVICE_3 */
-	[232] = IMX_PIN_REG(MX35_PAD_STXD4, 0x564, 0x120, 0, 0x0, 0), /* MX35_PAD_STXD4__AUDMUX_AUD4_TXD */
-	[233] = IMX_PIN_REG(MX35_PAD_STXD4, 0x564, 0x120, 5, 0x8b8, 0), /* MX35_PAD_STXD4__GPIO2_28 */
-	[234] = IMX_PIN_REG(MX35_PAD_STXD4, 0x564, 0x120, 7, 0x0, 0), /* MX35_PAD_STXD4__ARM11P_TOP_ARM_COREASID0 */
-	[235] = IMX_PIN_REG(MX35_PAD_SRXD4, 0x568, 0x124, 0, 0x0, 0), /* MX35_PAD_SRXD4__AUDMUX_AUD4_RXD */
-	[236] = IMX_PIN_REG(MX35_PAD_SRXD4, 0x568, 0x124, 5, 0x8bc, 0), /* MX35_PAD_SRXD4__GPIO2_29 */
-	[237] = IMX_PIN_REG(MX35_PAD_SRXD4, 0x568, 0x124, 7, 0x0, 0), /* MX35_PAD_SRXD4__ARM11P_TOP_ARM_COREASID1 */
-	[238] = IMX_PIN_REG(MX35_PAD_SCK4, 0x56c, 0x128, 0, 0x0, 0), /* MX35_PAD_SCK4__AUDMUX_AUD4_TXC */
-	[239] = IMX_PIN_REG(MX35_PAD_SCK4, 0x56c, 0x128, 5, 0x8c4, 0), /* MX35_PAD_SCK4__GPIO2_30 */
-	[240] = IMX_PIN_REG(MX35_PAD_SCK4, 0x56c, 0x128, 7, 0x0, 0), /* MX35_PAD_SCK4__ARM11P_TOP_ARM_COREASID2 */
-	[241] = IMX_PIN_REG(MX35_PAD_STXFS4, 0x570, 0x12c, 0, 0x0, 0), /* MX35_PAD_STXFS4__AUDMUX_AUD4_TXFS */
-	[242] = IMX_PIN_REG(MX35_PAD_STXFS4, 0x570, 0x12c, 5, 0x8c8, 0), /* MX35_PAD_STXFS4__GPIO2_31 */
-	[243] = IMX_PIN_REG(MX35_PAD_STXFS4, 0x570, 0x12c, 7, 0x0, 0), /* MX35_PAD_STXFS4__ARM11P_TOP_ARM_COREASID3 */
-	[244] = IMX_PIN_REG(MX35_PAD_STXD5, 0x574, 0x130, 0, 0x0, 0), /* MX35_PAD_STXD5__AUDMUX_AUD5_TXD */
-	[245] = IMX_PIN_REG(MX35_PAD_STXD5, 0x574, 0x130, 1, 0x0, 0), /* MX35_PAD_STXD5__SPDIF_SPDIF_OUT1 */
-	[246] = IMX_PIN_REG(MX35_PAD_STXD5, 0x574, 0x130, 2, 0x7ec, 0), /* MX35_PAD_STXD5__CSPI2_MOSI */
-	[247] = IMX_PIN_REG(MX35_PAD_STXD5, 0x574, 0x130, 5, 0x82c, 1), /* MX35_PAD_STXD5__GPIO1_0 */
-	[248] = IMX_PIN_REG(MX35_PAD_STXD5, 0x574, 0x130, 7, 0x0, 0), /* MX35_PAD_STXD5__ARM11P_TOP_ARM_COREASID4 */
-	[249] = IMX_PIN_REG(MX35_PAD_SRXD5, 0x578, 0x134, 0, 0x0, 0), /* MX35_PAD_SRXD5__AUDMUX_AUD5_RXD */
-	[250] = IMX_PIN_REG(MX35_PAD_SRXD5, 0x578, 0x134, 1, 0x998, 0), /* MX35_PAD_SRXD5__SPDIF_SPDIF_IN1 */
-	[251] = IMX_PIN_REG(MX35_PAD_SRXD5, 0x578, 0x134, 2, 0x7e8, 0), /* MX35_PAD_SRXD5__CSPI2_MISO */
-	[252] = IMX_PIN_REG(MX35_PAD_SRXD5, 0x578, 0x134, 5, 0x838, 1), /* MX35_PAD_SRXD5__GPIO1_1 */
-	[253] = IMX_PIN_REG(MX35_PAD_SRXD5, 0x578, 0x134, 7, 0x0, 0), /* MX35_PAD_SRXD5__ARM11P_TOP_ARM_COREASID5 */
-	[254] = IMX_PIN_REG(MX35_PAD_SCK5, 0x57c, 0x138, 0, 0x0, 0), /* MX35_PAD_SCK5__AUDMUX_AUD5_TXC */
-	[255] = IMX_PIN_REG(MX35_PAD_SCK5, 0x57c, 0x138, 1, 0x994, 0), /* MX35_PAD_SCK5__SPDIF_SPDIF_EXTCLK */
-	[256] = IMX_PIN_REG(MX35_PAD_SCK5, 0x57c, 0x138, 2, 0x7e0, 0), /* MX35_PAD_SCK5__CSPI2_SCLK */
-	[257] = IMX_PIN_REG(MX35_PAD_SCK5, 0x57c, 0x138, 5, 0x848, 0), /* MX35_PAD_SCK5__GPIO1_2 */
-	[258] = IMX_PIN_REG(MX35_PAD_SCK5, 0x57c, 0x138, 7, 0x0, 0), /* MX35_PAD_SCK5__ARM11P_TOP_ARM_COREASID6 */
-	[259] = IMX_PIN_REG(MX35_PAD_STXFS5, 0x580, 0x13c, 0, 0x0, 0), /* MX35_PAD_STXFS5__AUDMUX_AUD5_TXFS */
-	[260] = IMX_PIN_REG(MX35_PAD_STXFS5, 0x580, 0x13c, 2, 0x7e4, 0), /* MX35_PAD_STXFS5__CSPI2_RDY */
-	[261] = IMX_PIN_REG(MX35_PAD_STXFS5, 0x580, 0x13c, 5, 0x84c, 0), /* MX35_PAD_STXFS5__GPIO1_3 */
-	[262] = IMX_PIN_REG(MX35_PAD_STXFS5, 0x580, 0x13c, 7, 0x0, 0), /* MX35_PAD_STXFS5__ARM11P_TOP_ARM_COREASID7 */
-	[263] = IMX_PIN_REG(MX35_PAD_SCKR, 0x584, 0x140, 0, 0x0, 0), /* MX35_PAD_SCKR__ESAI_SCKR */
-	[264] = IMX_PIN_REG(MX35_PAD_SCKR, 0x584, 0x140, 5, 0x850, 1), /* MX35_PAD_SCKR__GPIO1_4 */
-	[265] = IMX_PIN_REG(MX35_PAD_SCKR, 0x584, 0x140, 7, 0x0, 0), /* MX35_PAD_SCKR__ARM11P_TOP_EVNTBUS_10 */
-	[266] = IMX_PIN_REG(MX35_PAD_FSR, 0x588, 0x144, 0, 0x0, 0), /* MX35_PAD_FSR__ESAI_FSR */
-	[267] = IMX_PIN_REG(MX35_PAD_FSR, 0x588, 0x144, 5, 0x854, 1), /* MX35_PAD_FSR__GPIO1_5 */
-	[268] = IMX_PIN_REG(MX35_PAD_FSR, 0x588, 0x144, 7, 0x0, 0), /* MX35_PAD_FSR__ARM11P_TOP_EVNTBUS_11 */
-	[269] = IMX_PIN_REG(MX35_PAD_HCKR, 0x58c, 0x148, 0, 0x0, 0), /* MX35_PAD_HCKR__ESAI_HCKR */
-	[270] = IMX_PIN_REG(MX35_PAD_HCKR, 0x58c, 0x148, 1, 0x0, 0), /* MX35_PAD_HCKR__AUDMUX_AUD5_RXFS */
-	[271] = IMX_PIN_REG(MX35_PAD_HCKR, 0x58c, 0x148, 2, 0x7f0, 0), /* MX35_PAD_HCKR__CSPI2_SS0 */
-	[272] = IMX_PIN_REG(MX35_PAD_HCKR, 0x58c, 0x148, 3, 0x0, 0), /* MX35_PAD_HCKR__IPU_FLASH_STROBE */
-	[273] = IMX_PIN_REG(MX35_PAD_HCKR, 0x58c, 0x148, 5, 0x858, 1), /* MX35_PAD_HCKR__GPIO1_6 */
-	[274] = IMX_PIN_REG(MX35_PAD_HCKR, 0x58c, 0x148, 7, 0x0, 0), /* MX35_PAD_HCKR__ARM11P_TOP_EVNTBUS_12 */
-	[275] = IMX_PIN_REG(MX35_PAD_SCKT, 0x590, 0x14c, 0, 0x0, 0), /* MX35_PAD_SCKT__ESAI_SCKT */
-	[276] = IMX_PIN_REG(MX35_PAD_SCKT, 0x590, 0x14c, 5, 0x85c, 1), /* MX35_PAD_SCKT__GPIO1_7 */
-	[277] = IMX_PIN_REG(MX35_PAD_SCKT, 0x590, 0x14c, 6, 0x930, 0), /* MX35_PAD_SCKT__IPU_CSI_D_0 */
-	[278] = IMX_PIN_REG(MX35_PAD_SCKT, 0x590, 0x14c, 7, 0x978, 1), /* MX35_PAD_SCKT__KPP_ROW_2 */
-	[279] = IMX_PIN_REG(MX35_PAD_FST, 0x594, 0x150, 0, 0x0, 0), /* MX35_PAD_FST__ESAI_FST */
-	[280] = IMX_PIN_REG(MX35_PAD_FST, 0x594, 0x150, 5, 0x860, 1), /* MX35_PAD_FST__GPIO1_8 */
-	[281] = IMX_PIN_REG(MX35_PAD_FST, 0x594, 0x150, 6, 0x934, 0), /* MX35_PAD_FST__IPU_CSI_D_1 */
-	[282] = IMX_PIN_REG(MX35_PAD_FST, 0x594, 0x150, 7, 0x97c, 1), /* MX35_PAD_FST__KPP_ROW_3 */
-	[283] = IMX_PIN_REG(MX35_PAD_HCKT, 0x598, 0x154, 0, 0x0, 0), /* MX35_PAD_HCKT__ESAI_HCKT */
-	[284] = IMX_PIN_REG(MX35_PAD_HCKT, 0x598, 0x154, 1, 0x7a8, 0), /* MX35_PAD_HCKT__AUDMUX_AUD5_RXC */
-	[285] = IMX_PIN_REG(MX35_PAD_HCKT, 0x598, 0x154, 5, 0x864, 0), /* MX35_PAD_HCKT__GPIO1_9 */
-	[286] = IMX_PIN_REG(MX35_PAD_HCKT, 0x598, 0x154, 6, 0x938, 0), /* MX35_PAD_HCKT__IPU_CSI_D_2 */
-	[287] = IMX_PIN_REG(MX35_PAD_HCKT, 0x598, 0x154, 7, 0x95c, 1), /* MX35_PAD_HCKT__KPP_COL_3 */
-	[288] = IMX_PIN_REG(MX35_PAD_TX5_RX0, 0x59c, 0x158, 0, 0x0, 0), /* MX35_PAD_TX5_RX0__ESAI_TX5_RX0 */
-	[289] = IMX_PIN_REG(MX35_PAD_TX5_RX0, 0x59c, 0x158, 1, 0x0, 0), /* MX35_PAD_TX5_RX0__AUDMUX_AUD4_RXC */
-	[290] = IMX_PIN_REG(MX35_PAD_TX5_RX0, 0x59c, 0x158, 2, 0x7f8, 1), /* MX35_PAD_TX5_RX0__CSPI2_SS2 */
-	[291] = IMX_PIN_REG(MX35_PAD_TX5_RX0, 0x59c, 0x158, 3, 0x0, 0), /* MX35_PAD_TX5_RX0__CAN2_TXCAN */
-	[292] = IMX_PIN_REG(MX35_PAD_TX5_RX0, 0x59c, 0x158, 4, 0x0, 0), /* MX35_PAD_TX5_RX0__UART2_DTR */
-	[293] = IMX_PIN_REG(MX35_PAD_TX5_RX0, 0x59c, 0x158, 5, 0x830, 0), /* MX35_PAD_TX5_RX0__GPIO1_10 */
-	[294] = IMX_PIN_REG(MX35_PAD_TX5_RX0, 0x59c, 0x158, 7, 0x0, 0), /* MX35_PAD_TX5_RX0__EMI_M3IF_CHOSEN_MASTER_0 */
-	[295] = IMX_PIN_REG(MX35_PAD_TX4_RX1, 0x5a0, 0x15c, 0, 0x0, 0), /* MX35_PAD_TX4_RX1__ESAI_TX4_RX1 */
-	[296] = IMX_PIN_REG(MX35_PAD_TX4_RX1, 0x5a0, 0x15c, 1, 0x0, 0), /* MX35_PAD_TX4_RX1__AUDMUX_AUD4_RXFS */
-	[297] = IMX_PIN_REG(MX35_PAD_TX4_RX1, 0x5a0, 0x15c, 2, 0x7fc, 0), /* MX35_PAD_TX4_RX1__CSPI2_SS3 */
-	[298] = IMX_PIN_REG(MX35_PAD_TX4_RX1, 0x5a0, 0x15c, 3, 0x7cc, 0), /* MX35_PAD_TX4_RX1__CAN2_RXCAN */
-	[299] = IMX_PIN_REG(MX35_PAD_TX4_RX1, 0x5a0, 0x15c, 4, 0x0, 0), /* MX35_PAD_TX4_RX1__UART2_DSR */
-	[300] = IMX_PIN_REG(MX35_PAD_TX4_RX1, 0x5a0, 0x15c, 5, 0x834, 0), /* MX35_PAD_TX4_RX1__GPIO1_11 */
-	[301] = IMX_PIN_REG(MX35_PAD_TX4_RX1, 0x5a0, 0x15c, 6, 0x93c, 0), /* MX35_PAD_TX4_RX1__IPU_CSI_D_3 */
-	[302] = IMX_PIN_REG(MX35_PAD_TX4_RX1, 0x5a0, 0x15c, 7, 0x970, 1), /* MX35_PAD_TX4_RX1__KPP_ROW_0 */
-	[303] = IMX_PIN_REG(MX35_PAD_TX3_RX2, 0x5a4, 0x160, 0, 0x0, 0), /* MX35_PAD_TX3_RX2__ESAI_TX3_RX2 */
-	[304] = IMX_PIN_REG(MX35_PAD_TX3_RX2, 0x5a4, 0x160, 1, 0x91c, 0), /* MX35_PAD_TX3_RX2__I2C3_SCL */
-	[305] = IMX_PIN_REG(MX35_PAD_TX3_RX2, 0x5a4, 0x160, 3, 0x0, 0), /* MX35_PAD_TX3_RX2__EMI_NANDF_CE1 */
-	[306] = IMX_PIN_REG(MX35_PAD_TX3_RX2, 0x5a4, 0x160, 5, 0x0, 0), /* MX35_PAD_TX3_RX2__GPIO1_12 */
-	[307] = IMX_PIN_REG(MX35_PAD_TX3_RX2, 0x5a4, 0x160, 6, 0x940, 0), /* MX35_PAD_TX3_RX2__IPU_CSI_D_4 */
-	[308] = IMX_PIN_REG(MX35_PAD_TX3_RX2, 0x5a4, 0x160, 7, 0x974, 1), /* MX35_PAD_TX3_RX2__KPP_ROW_1 */
-	[309] = IMX_PIN_REG(MX35_PAD_TX2_RX3, 0x5a8, 0x164, 0, 0x0, 0), /* MX35_PAD_TX2_RX3__ESAI_TX2_RX3 */
-	[310] = IMX_PIN_REG(MX35_PAD_TX2_RX3, 0x5a8, 0x164, 1, 0x920, 0), /* MX35_PAD_TX2_RX3__I2C3_SDA */
-	[311] = IMX_PIN_REG(MX35_PAD_TX2_RX3, 0x5a8, 0x164, 3, 0x0, 0), /* MX35_PAD_TX2_RX3__EMI_NANDF_CE2 */
-	[312] = IMX_PIN_REG(MX35_PAD_TX2_RX3, 0x5a8, 0x164, 5, 0x0, 0), /* MX35_PAD_TX2_RX3__GPIO1_13 */
-	[313] = IMX_PIN_REG(MX35_PAD_TX2_RX3, 0x5a8, 0x164, 6, 0x944, 0), /* MX35_PAD_TX2_RX3__IPU_CSI_D_5 */
-	[314] = IMX_PIN_REG(MX35_PAD_TX2_RX3, 0x5a8, 0x164, 7, 0x950, 1), /* MX35_PAD_TX2_RX3__KPP_COL_0 */
-	[315] = IMX_PIN_REG(MX35_PAD_TX1, 0x5ac, 0x168, 0, 0x0, 0), /* MX35_PAD_TX1__ESAI_TX1 */
-	[316] = IMX_PIN_REG(MX35_PAD_TX1, 0x5ac, 0x168, 1, 0x7d4, 1), /* MX35_PAD_TX1__CCM_PMIC_RDY */
-	[317] = IMX_PIN_REG(MX35_PAD_TX1, 0x5ac, 0x168, 2, 0x7d8, 2), /* MX35_PAD_TX1__CSPI1_SS2 */
-	[318] = IMX_PIN_REG(MX35_PAD_TX1, 0x5ac, 0x168, 3, 0x0, 0), /* MX35_PAD_TX1__EMI_NANDF_CE3 */
-	[319] = IMX_PIN_REG(MX35_PAD_TX1, 0x5ac, 0x168, 4, 0x0, 0), /* MX35_PAD_TX1__UART2_RI */
-	[320] = IMX_PIN_REG(MX35_PAD_TX1, 0x5ac, 0x168, 5, 0x0, 0), /* MX35_PAD_TX1__GPIO1_14 */
-	[321] = IMX_PIN_REG(MX35_PAD_TX1, 0x5ac, 0x168, 6, 0x948, 0), /* MX35_PAD_TX1__IPU_CSI_D_6 */
-	[322] = IMX_PIN_REG(MX35_PAD_TX1, 0x5ac, 0x168, 7, 0x954, 1), /* MX35_PAD_TX1__KPP_COL_1 */
-	[323] = IMX_PIN_REG(MX35_PAD_TX0, 0x5b0, 0x16c, 0, 0x0, 0), /* MX35_PAD_TX0__ESAI_TX0 */
-	[324] = IMX_PIN_REG(MX35_PAD_TX0, 0x5b0, 0x16c, 1, 0x994, 1), /* MX35_PAD_TX0__SPDIF_SPDIF_EXTCLK */
-	[325] = IMX_PIN_REG(MX35_PAD_TX0, 0x5b0, 0x16c, 2, 0x7dc, 0), /* MX35_PAD_TX0__CSPI1_SS3 */
-	[326] = IMX_PIN_REG(MX35_PAD_TX0, 0x5b0, 0x16c, 3, 0x800, 1), /* MX35_PAD_TX0__EMI_DTACK_B */
-	[327] = IMX_PIN_REG(MX35_PAD_TX0, 0x5b0, 0x16c, 4, 0x0, 0), /* MX35_PAD_TX0__UART2_DCD */
-	[328] = IMX_PIN_REG(MX35_PAD_TX0, 0x5b0, 0x16c, 5, 0x0, 0), /* MX35_PAD_TX0__GPIO1_15 */
-	[329] = IMX_PIN_REG(MX35_PAD_TX0, 0x5b0, 0x16c, 6, 0x94c, 0), /* MX35_PAD_TX0__IPU_CSI_D_7 */
-	[330] = IMX_PIN_REG(MX35_PAD_TX0, 0x5b0, 0x16c, 7, 0x958, 1), /* MX35_PAD_TX0__KPP_COL_2 */
-	[331] = IMX_PIN_REG(MX35_PAD_CSPI1_MOSI, 0x5b4, 0x170, 0, 0x0, 0), /* MX35_PAD_CSPI1_MOSI__CSPI1_MOSI */
-	[332] = IMX_PIN_REG(MX35_PAD_CSPI1_MOSI, 0x5b4, 0x170, 5, 0x0, 0), /* MX35_PAD_CSPI1_MOSI__GPIO1_16 */
-	[333] = IMX_PIN_REG(MX35_PAD_CSPI1_MOSI, 0x5b4, 0x170, 7, 0x0, 0), /* MX35_PAD_CSPI1_MOSI__ECT_CTI_TRIG_OUT1_2 */
-	[334] = IMX_PIN_REG(MX35_PAD_CSPI1_MISO, 0x5b8, 0x174, 0, 0x0, 0), /* MX35_PAD_CSPI1_MISO__CSPI1_MISO */
-	[335] = IMX_PIN_REG(MX35_PAD_CSPI1_MISO, 0x5b8, 0x174, 5, 0x0, 0), /* MX35_PAD_CSPI1_MISO__GPIO1_17 */
-	[336] = IMX_PIN_REG(MX35_PAD_CSPI1_MISO, 0x5b8, 0x174, 7, 0x0, 0), /* MX35_PAD_CSPI1_MISO__ECT_CTI_TRIG_OUT1_3 */
-	[337] = IMX_PIN_REG(MX35_PAD_CSPI1_SS0, 0x5bc, 0x178, 0, 0x0, 0), /* MX35_PAD_CSPI1_SS0__CSPI1_SS0 */
-	[338] = IMX_PIN_REG(MX35_PAD_CSPI1_SS0, 0x5bc, 0x178, 1, 0x990, 1), /* MX35_PAD_CSPI1_SS0__OWIRE_LINE */
-	[339] = IMX_PIN_REG(MX35_PAD_CSPI1_SS0, 0x5bc, 0x178, 2, 0x7fc, 1), /* MX35_PAD_CSPI1_SS0__CSPI2_SS3 */
-	[340] = IMX_PIN_REG(MX35_PAD_CSPI1_SS0, 0x5bc, 0x178, 5, 0x0, 0), /* MX35_PAD_CSPI1_SS0__GPIO1_18 */
-	[341] = IMX_PIN_REG(MX35_PAD_CSPI1_SS0, 0x5bc, 0x178, 7, 0x0, 0), /* MX35_PAD_CSPI1_SS0__ECT_CTI_TRIG_OUT1_4 */
-	[342] = IMX_PIN_REG(MX35_PAD_CSPI1_SS1, 0x5c0, 0x17c, 0, 0x0, 0), /* MX35_PAD_CSPI1_SS1__CSPI1_SS1 */
-	[343] = IMX_PIN_REG(MX35_PAD_CSPI1_SS1, 0x5c0, 0x17c, 1, 0x0, 0), /* MX35_PAD_CSPI1_SS1__PWM_PWMO */
-	[344] = IMX_PIN_REG(MX35_PAD_CSPI1_SS1, 0x5c0, 0x17c, 2, 0x7d0, 1), /* MX35_PAD_CSPI1_SS1__CCM_CLK32K */
-	[345] = IMX_PIN_REG(MX35_PAD_CSPI1_SS1, 0x5c0, 0x17c, 5, 0x0, 0), /* MX35_PAD_CSPI1_SS1__GPIO1_19 */
-	[346] = IMX_PIN_REG(MX35_PAD_CSPI1_SS1, 0x5c0, 0x17c, 6, 0x0, 0), /* MX35_PAD_CSPI1_SS1__IPU_DIAGB_29 */
-	[347] = IMX_PIN_REG(MX35_PAD_CSPI1_SS1, 0x5c0, 0x17c, 7, 0x0, 0), /* MX35_PAD_CSPI1_SS1__ECT_CTI_TRIG_OUT1_5 */
-	[348] = IMX_PIN_REG(MX35_PAD_CSPI1_SCLK, 0x5c4, 0x180, 0, 0x0, 0), /* MX35_PAD_CSPI1_SCLK__CSPI1_SCLK */
-	[349] = IMX_PIN_REG(MX35_PAD_CSPI1_SCLK, 0x5c4, 0x180, 5, 0x904, 0), /* MX35_PAD_CSPI1_SCLK__GPIO3_4 */
-	[350] = IMX_PIN_REG(MX35_PAD_CSPI1_SCLK, 0x5c4, 0x180, 6, 0x0, 0), /* MX35_PAD_CSPI1_SCLK__IPU_DIAGB_30 */
-	[351] = IMX_PIN_REG(MX35_PAD_CSPI1_SCLK, 0x5c4, 0x180, 7, 0x0, 0), /* MX35_PAD_CSPI1_SCLK__EMI_M3IF_CHOSEN_MASTER_1 */
-	[352] = IMX_PIN_REG(MX35_PAD_CSPI1_SPI_RDY, 0x5c8, 0x184, 0, 0x0, 0), /* MX35_PAD_CSPI1_SPI_RDY__CSPI1_RDY */
-	[353] = IMX_PIN_REG(MX35_PAD_CSPI1_SPI_RDY, 0x5c8, 0x184, 5, 0x908, 0), /* MX35_PAD_CSPI1_SPI_RDY__GPIO3_5 */
-	[354] = IMX_PIN_REG(MX35_PAD_CSPI1_SPI_RDY, 0x5c8, 0x184, 6, 0x0, 0), /* MX35_PAD_CSPI1_SPI_RDY__IPU_DIAGB_31 */
-	[355] = IMX_PIN_REG(MX35_PAD_CSPI1_SPI_RDY, 0x5c8, 0x184, 7, 0x0, 0), /* MX35_PAD_CSPI1_SPI_RDY__EMI_M3IF_CHOSEN_MASTER_2 */
-	[356] = IMX_PIN_REG(MX35_PAD_RXD1, 0x5cc, 0x188, 0, 0x0, 0), /* MX35_PAD_RXD1__UART1_RXD_MUX */
-	[357] = IMX_PIN_REG(MX35_PAD_RXD1, 0x5cc, 0x188, 1, 0x7ec, 1), /* MX35_PAD_RXD1__CSPI2_MOSI */
-	[358] = IMX_PIN_REG(MX35_PAD_RXD1, 0x5cc, 0x188, 4, 0x960, 0), /* MX35_PAD_RXD1__KPP_COL_4 */
-	[359] = IMX_PIN_REG(MX35_PAD_RXD1, 0x5cc, 0x188, 5, 0x90c, 0), /* MX35_PAD_RXD1__GPIO3_6 */
-	[360] = IMX_PIN_REG(MX35_PAD_RXD1, 0x5cc, 0x188, 7, 0x0, 0), /* MX35_PAD_RXD1__ARM11P_TOP_EVNTBUS_16 */
-	[361] = IMX_PIN_REG(MX35_PAD_TXD1, 0x5d0, 0x18c, 0, 0x0, 0), /* MX35_PAD_TXD1__UART1_TXD_MUX */
-	[362] = IMX_PIN_REG(MX35_PAD_TXD1, 0x5d0, 0x18c, 1, 0x7e8, 1), /* MX35_PAD_TXD1__CSPI2_MISO */
-	[363] = IMX_PIN_REG(MX35_PAD_TXD1, 0x5d0, 0x18c, 4, 0x964, 0), /* MX35_PAD_TXD1__KPP_COL_5 */
-	[364] = IMX_PIN_REG(MX35_PAD_TXD1, 0x5d0, 0x18c, 5, 0x910, 0), /* MX35_PAD_TXD1__GPIO3_7 */
-	[365] = IMX_PIN_REG(MX35_PAD_TXD1, 0x5d0, 0x18c, 7, 0x0, 0), /* MX35_PAD_TXD1__ARM11P_TOP_EVNTBUS_17 */
-	[366] = IMX_PIN_REG(MX35_PAD_RTS1, 0x5d4, 0x190, 0, 0x0, 0), /* MX35_PAD_RTS1__UART1_RTS */
-	[367] = IMX_PIN_REG(MX35_PAD_RTS1, 0x5d4, 0x190, 1, 0x7e0, 1), /* MX35_PAD_RTS1__CSPI2_SCLK */
-	[368] = IMX_PIN_REG(MX35_PAD_RTS1, 0x5d4, 0x190, 2, 0x91c, 1), /* MX35_PAD_RTS1__I2C3_SCL */
-	[369] = IMX_PIN_REG(MX35_PAD_RTS1, 0x5d4, 0x190, 3, 0x930, 1), /* MX35_PAD_RTS1__IPU_CSI_D_0 */
-	[370] = IMX_PIN_REG(MX35_PAD_RTS1, 0x5d4, 0x190, 4, 0x968, 0), /* MX35_PAD_RTS1__KPP_COL_6 */
-	[371] = IMX_PIN_REG(MX35_PAD_RTS1, 0x5d4, 0x190, 5, 0x914, 0), /* MX35_PAD_RTS1__GPIO3_8 */
-	[372] = IMX_PIN_REG(MX35_PAD_RTS1, 0x5d4, 0x190, 6, 0x0, 0), /* MX35_PAD_RTS1__EMI_NANDF_CE1 */
-	[373] = IMX_PIN_REG(MX35_PAD_RTS1, 0x5d4, 0x190, 7, 0x0, 0), /* MX35_PAD_RTS1__ARM11P_TOP_EVNTBUS_18 */
-	[374] = IMX_PIN_REG(MX35_PAD_CTS1, 0x5d8, 0x194, 0, 0x0, 0), /* MX35_PAD_CTS1__UART1_CTS */
-	[375] = IMX_PIN_REG(MX35_PAD_CTS1, 0x5d8, 0x194, 1, 0x7e4, 1), /* MX35_PAD_CTS1__CSPI2_RDY */
-	[376] = IMX_PIN_REG(MX35_PAD_CTS1, 0x5d8, 0x194, 2, 0x920, 1), /* MX35_PAD_CTS1__I2C3_SDA */
-	[377] = IMX_PIN_REG(MX35_PAD_CTS1, 0x5d8, 0x194, 3, 0x934, 1), /* MX35_PAD_CTS1__IPU_CSI_D_1 */
-	[378] = IMX_PIN_REG(MX35_PAD_CTS1, 0x5d8, 0x194, 4, 0x96c, 0), /* MX35_PAD_CTS1__KPP_COL_7 */
-	[379] = IMX_PIN_REG(MX35_PAD_CTS1, 0x5d8, 0x194, 5, 0x918, 0), /* MX35_PAD_CTS1__GPIO3_9 */
-	[380] = IMX_PIN_REG(MX35_PAD_CTS1, 0x5d8, 0x194, 6, 0x0, 0), /* MX35_PAD_CTS1__EMI_NANDF_CE2 */
-	[381] = IMX_PIN_REG(MX35_PAD_CTS1, 0x5d8, 0x194, 7, 0x0, 0), /* MX35_PAD_CTS1__ARM11P_TOP_EVNTBUS_19 */
-	[382] = IMX_PIN_REG(MX35_PAD_RXD2, 0x5dc, 0x198, 0, 0x0, 0), /* MX35_PAD_RXD2__UART2_RXD_MUX */
-	[383] = IMX_PIN_REG(MX35_PAD_RXD2, 0x5dc, 0x198, 4, 0x980, 0), /* MX35_PAD_RXD2__KPP_ROW_4 */
-	[384] = IMX_PIN_REG(MX35_PAD_RXD2, 0x5dc, 0x198, 5, 0x8ec, 0), /* MX35_PAD_RXD2__GPIO3_10 */
-	[385] = IMX_PIN_REG(MX35_PAD_TXD2, 0x5e0, 0x19c, 0, 0x0, 0), /* MX35_PAD_TXD2__UART2_TXD_MUX */
-	[386] = IMX_PIN_REG(MX35_PAD_TXD2, 0x5e0, 0x19c, 1, 0x994, 2), /* MX35_PAD_TXD2__SPDIF_SPDIF_EXTCLK */
-	[387] = IMX_PIN_REG(MX35_PAD_TXD2, 0x5e0, 0x19c, 4, 0x984, 0), /* MX35_PAD_TXD2__KPP_ROW_5 */
-	[388] = IMX_PIN_REG(MX35_PAD_TXD2, 0x5e0, 0x19c, 5, 0x8f0, 0), /* MX35_PAD_TXD2__GPIO3_11 */
-	[389] = IMX_PIN_REG(MX35_PAD_RTS2, 0x5e4, 0x1a0, 0, 0x0, 0), /* MX35_PAD_RTS2__UART2_RTS */
-	[390] = IMX_PIN_REG(MX35_PAD_RTS2, 0x5e4, 0x1a0, 1, 0x998, 1), /* MX35_PAD_RTS2__SPDIF_SPDIF_IN1 */
-	[391] = IMX_PIN_REG(MX35_PAD_RTS2, 0x5e4, 0x1a0, 2, 0x7cc, 1), /* MX35_PAD_RTS2__CAN2_RXCAN */
-	[392] = IMX_PIN_REG(MX35_PAD_RTS2, 0x5e4, 0x1a0, 3, 0x938, 1), /* MX35_PAD_RTS2__IPU_CSI_D_2 */
-	[393] = IMX_PIN_REG(MX35_PAD_RTS2, 0x5e4, 0x1a0, 4, 0x988, 0), /* MX35_PAD_RTS2__KPP_ROW_6 */
-	[394] = IMX_PIN_REG(MX35_PAD_RTS2, 0x5e4, 0x1a0, 5, 0x8f4, 0), /* MX35_PAD_RTS2__GPIO3_12 */
-	[395] = IMX_PIN_REG(MX35_PAD_RTS2, 0x5e4, 0x1a0, 6, 0x0, 0), /* MX35_PAD_RTS2__AUDMUX_AUD5_RXC */
-	[396] = IMX_PIN_REG(MX35_PAD_RTS2, 0x5e4, 0x1a0, 7, 0x9a0, 0), /* MX35_PAD_RTS2__UART3_RXD_MUX */
-	[397] = IMX_PIN_REG(MX35_PAD_CTS2, 0x5e8, 0x1a4, 0, 0x0, 0), /* MX35_PAD_CTS2__UART2_CTS */
-	[398] = IMX_PIN_REG(MX35_PAD_CTS2, 0x5e8, 0x1a4, 1, 0x0, 0), /* MX35_PAD_CTS2__SPDIF_SPDIF_OUT1 */
-	[399] = IMX_PIN_REG(MX35_PAD_CTS2, 0x5e8, 0x1a4, 2, 0x0, 0), /* MX35_PAD_CTS2__CAN2_TXCAN */
-	[400] = IMX_PIN_REG(MX35_PAD_CTS2, 0x5e8, 0x1a4, 3, 0x93c, 1), /* MX35_PAD_CTS2__IPU_CSI_D_3 */
-	[401] = IMX_PIN_REG(MX35_PAD_CTS2, 0x5e8, 0x1a4, 4, 0x98c, 0), /* MX35_PAD_CTS2__KPP_ROW_7 */
-	[402] = IMX_PIN_REG(MX35_PAD_CTS2, 0x5e8, 0x1a4, 5, 0x8f8, 0), /* MX35_PAD_CTS2__GPIO3_13 */
-	[403] = IMX_PIN_REG(MX35_PAD_CTS2, 0x5e8, 0x1a4, 6, 0x0, 0), /* MX35_PAD_CTS2__AUDMUX_AUD5_RXFS */
-	[404] = IMX_PIN_REG(MX35_PAD_CTS2, 0x5e8, 0x1a4, 7, 0x0, 0), /* MX35_PAD_CTS2__UART3_TXD_MUX */
-	[405] = IMX_PIN_REG(MX35_PAD_RTCK, 0x5ec, 0x0, 0, 0x0, 0), /* MX35_PAD_RTCK__ARM11P_TOP_RTCK */
-	[406] = IMX_PIN_REG(MX35_PAD_TCK, 0x5f0, 0x0, 0, 0x0, 0), /* MX35_PAD_TCK__SJC_TCK */
-	[407] = IMX_PIN_REG(MX35_PAD_TMS, 0x5f4, 0x0, 0, 0x0, 0), /* MX35_PAD_TMS__SJC_TMS */
-	[408] = IMX_PIN_REG(MX35_PAD_TDI, 0x5f8, 0x0, 0, 0x0, 0), /* MX35_PAD_TDI__SJC_TDI */
-	[409] = IMX_PIN_REG(MX35_PAD_TDO, 0x5fc, 0x0, 0, 0x0, 0), /* MX35_PAD_TDO__SJC_TDO */
-	[410] = IMX_PIN_REG(MX35_PAD_TRSTB, 0x600, 0x0, 0, 0x0, 0), /* MX35_PAD_TRSTB__SJC_TRSTB */
-	[411] = IMX_PIN_REG(MX35_PAD_DE_B, 0x604, 0x0, 0, 0x0, 0), /* MX35_PAD_DE_B__SJC_DE_B */
-	[412] = IMX_PIN_REG(MX35_PAD_SJC_MOD, 0x608, 0x0, 0, 0x0, 0), /* MX35_PAD_SJC_MOD__SJC_MOD */
-	[413] = IMX_PIN_REG(MX35_PAD_USBOTG_PWR, 0x60c, 0x1a8, 0, 0x0, 0), /* MX35_PAD_USBOTG_PWR__USB_TOP_USBOTG_PWR */
-	[414] = IMX_PIN_REG(MX35_PAD_USBOTG_PWR, 0x60c, 0x1a8, 1, 0x0, 0), /* MX35_PAD_USBOTG_PWR__USB_TOP_USBH2_PWR */
-	[415] = IMX_PIN_REG(MX35_PAD_USBOTG_PWR, 0x60c, 0x1a8, 5, 0x8fc, 0), /* MX35_PAD_USBOTG_PWR__GPIO3_14 */
-	[416] = IMX_PIN_REG(MX35_PAD_USBOTG_OC, 0x610, 0x1ac, 0, 0x0, 0), /* MX35_PAD_USBOTG_OC__USB_TOP_USBOTG_OC */
-	[417] = IMX_PIN_REG(MX35_PAD_USBOTG_OC, 0x610, 0x1ac, 1, 0x9f4, 1), /* MX35_PAD_USBOTG_OC__USB_TOP_USBH2_OC */
-	[418] = IMX_PIN_REG(MX35_PAD_USBOTG_OC, 0x610, 0x1ac, 5, 0x900, 0), /* MX35_PAD_USBOTG_OC__GPIO3_15 */
-	[419] = IMX_PIN_REG(MX35_PAD_LD0, 0x614, 0x1b0, 0, 0x0, 0), /* MX35_PAD_LD0__IPU_DISPB_DAT_0 */
-	[420] = IMX_PIN_REG(MX35_PAD_LD0, 0x614, 0x1b0, 5, 0x868, 1), /* MX35_PAD_LD0__GPIO2_0 */
-	[421] = IMX_PIN_REG(MX35_PAD_LD0, 0x614, 0x1b0, 6, 0x0, 0), /* MX35_PAD_LD0__SDMA_SDMA_DEBUG_PC_0 */
-	[422] = IMX_PIN_REG(MX35_PAD_LD1, 0x618, 0x1b4, 0, 0x0, 0), /* MX35_PAD_LD1__IPU_DISPB_DAT_1 */
-	[423] = IMX_PIN_REG(MX35_PAD_LD1, 0x618, 0x1b4, 5, 0x894, 0), /* MX35_PAD_LD1__GPIO2_1 */
-	[424] = IMX_PIN_REG(MX35_PAD_LD1, 0x618, 0x1b4, 6, 0x0, 0), /* MX35_PAD_LD1__SDMA_SDMA_DEBUG_PC_1 */
-	[425] = IMX_PIN_REG(MX35_PAD_LD2, 0x61c, 0x1b8, 0, 0x0, 0), /* MX35_PAD_LD2__IPU_DISPB_DAT_2 */
-	[426] = IMX_PIN_REG(MX35_PAD_LD2, 0x61c, 0x1b8, 5, 0x8c0, 0), /* MX35_PAD_LD2__GPIO2_2 */
-	[427] = IMX_PIN_REG(MX35_PAD_LD2, 0x61c, 0x1b8, 6, 0x0, 0), /* MX35_PAD_LD2__SDMA_SDMA_DEBUG_PC_2 */
-	[428] = IMX_PIN_REG(MX35_PAD_LD3, 0x620, 0x1bc, 0, 0x0, 0), /* MX35_PAD_LD3__IPU_DISPB_DAT_3 */
-	[429] = IMX_PIN_REG(MX35_PAD_LD3, 0x620, 0x1bc, 5, 0x8cc, 0), /* MX35_PAD_LD3__GPIO2_3 */
-	[430] = IMX_PIN_REG(MX35_PAD_LD3, 0x620, 0x1bc, 6, 0x0, 0), /* MX35_PAD_LD3__SDMA_SDMA_DEBUG_PC_3 */
-	[431] = IMX_PIN_REG(MX35_PAD_LD4, 0x624, 0x1c0, 0, 0x0, 0), /* MX35_PAD_LD4__IPU_DISPB_DAT_4 */
-	[432] = IMX_PIN_REG(MX35_PAD_LD4, 0x624, 0x1c0, 5, 0x8d0, 0), /* MX35_PAD_LD4__GPIO2_4 */
-	[433] = IMX_PIN_REG(MX35_PAD_LD4, 0x624, 0x1c0, 6, 0x0, 0), /* MX35_PAD_LD4__SDMA_SDMA_DEBUG_PC_4 */
-	[434] = IMX_PIN_REG(MX35_PAD_LD5, 0x628, 0x1c4, 0, 0x0, 0), /* MX35_PAD_LD5__IPU_DISPB_DAT_5 */
-	[435] = IMX_PIN_REG(MX35_PAD_LD5, 0x628, 0x1c4, 5, 0x8d4, 0), /* MX35_PAD_LD5__GPIO2_5 */
-	[436] = IMX_PIN_REG(MX35_PAD_LD5, 0x628, 0x1c4, 6, 0x0, 0), /* MX35_PAD_LD5__SDMA_SDMA_DEBUG_PC_5 */
-	[437] = IMX_PIN_REG(MX35_PAD_LD6, 0x62c, 0x1c8, 0, 0x0, 0), /* MX35_PAD_LD6__IPU_DISPB_DAT_6 */
-	[438] = IMX_PIN_REG(MX35_PAD_LD6, 0x62c, 0x1c8, 5, 0x8d8, 0), /* MX35_PAD_LD6__GPIO2_6 */
-	[439] = IMX_PIN_REG(MX35_PAD_LD6, 0x62c, 0x1c8, 6, 0x0, 0), /* MX35_PAD_LD6__SDMA_SDMA_DEBUG_PC_6 */
-	[440] = IMX_PIN_REG(MX35_PAD_LD7, 0x630, 0x1cc, 0, 0x0, 0), /* MX35_PAD_LD7__IPU_DISPB_DAT_7 */
-	[441] = IMX_PIN_REG(MX35_PAD_LD7, 0x630, 0x1cc, 5, 0x8dc, 0), /* MX35_PAD_LD7__GPIO2_7 */
-	[442] = IMX_PIN_REG(MX35_PAD_LD7, 0x630, 0x1cc, 6, 0x0, 0), /* MX35_PAD_LD7__SDMA_SDMA_DEBUG_PC_7 */
-	[443] = IMX_PIN_REG(MX35_PAD_LD8, 0x634, 0x1d0, 0, 0x0, 0), /* MX35_PAD_LD8__IPU_DISPB_DAT_8 */
-	[444] = IMX_PIN_REG(MX35_PAD_LD8, 0x634, 0x1d0, 5, 0x8e0, 0), /* MX35_PAD_LD8__GPIO2_8 */
-	[445] = IMX_PIN_REG(MX35_PAD_LD8, 0x634, 0x1d0, 6, 0x0, 0), /* MX35_PAD_LD8__SDMA_SDMA_DEBUG_PC_8 */
-	[446] = IMX_PIN_REG(MX35_PAD_LD9, 0x638, 0x1d4, 0, 0x0, 0), /* MX35_PAD_LD9__IPU_DISPB_DAT_9 */
-	[447] = IMX_PIN_REG(MX35_PAD_LD9, 0x638, 0x1d4, 5, 0x8e4, 0), /* MX35_PAD_LD9__GPIO2_9 */
-	[448] = IMX_PIN_REG(MX35_PAD_LD9, 0x638, 0x1d4, 6, 0x0, 0), /* MX35_PAD_LD9__SDMA_SDMA_DEBUG_PC_9 */
-	[449] = IMX_PIN_REG(MX35_PAD_LD10, 0x63c, 0x1d8, 0, 0x0, 0), /* MX35_PAD_LD10__IPU_DISPB_DAT_10 */
-	[450] = IMX_PIN_REG(MX35_PAD_LD10, 0x63c, 0x1d8, 5, 0x86c, 0), /* MX35_PAD_LD10__GPIO2_10 */
-	[451] = IMX_PIN_REG(MX35_PAD_LD10, 0x63c, 0x1d8, 6, 0x0, 0), /* MX35_PAD_LD10__SDMA_SDMA_DEBUG_PC_10 */
-	[452] = IMX_PIN_REG(MX35_PAD_LD11, 0x640, 0x1dc, 0, 0x0, 0), /* MX35_PAD_LD11__IPU_DISPB_DAT_11 */
-	[453] = IMX_PIN_REG(MX35_PAD_LD11, 0x640, 0x1dc, 5, 0x870, 0), /* MX35_PAD_LD11__GPIO2_11 */
-	[454] = IMX_PIN_REG(MX35_PAD_LD11, 0x640, 0x1dc, 6, 0x0, 0), /* MX35_PAD_LD11__SDMA_SDMA_DEBUG_PC_11 */
-	[455] = IMX_PIN_REG(MX35_PAD_LD11, 0x640, 0x1dc, 7, 0x0, 0), /* MX35_PAD_LD11__ARM11P_TOP_TRACE_4 */
-	[456] = IMX_PIN_REG(MX35_PAD_LD12, 0x644, 0x1e0, 0, 0x0, 0), /* MX35_PAD_LD12__IPU_DISPB_DAT_12 */
-	[457] = IMX_PIN_REG(MX35_PAD_LD12, 0x644, 0x1e0, 5, 0x874, 0), /* MX35_PAD_LD12__GPIO2_12 */
-	[458] = IMX_PIN_REG(MX35_PAD_LD12, 0x644, 0x1e0, 6, 0x0, 0), /* MX35_PAD_LD12__SDMA_SDMA_DEBUG_PC_12 */
-	[459] = IMX_PIN_REG(MX35_PAD_LD12, 0x644, 0x1e0, 7, 0x0, 0), /* MX35_PAD_LD12__ARM11P_TOP_TRACE_5 */
-	[460] = IMX_PIN_REG(MX35_PAD_LD13, 0x648, 0x1e4, 0, 0x0, 0), /* MX35_PAD_LD13__IPU_DISPB_DAT_13 */
-	[461] = IMX_PIN_REG(MX35_PAD_LD13, 0x648, 0x1e4, 5, 0x878, 0), /* MX35_PAD_LD13__GPIO2_13 */
-	[462] = IMX_PIN_REG(MX35_PAD_LD13, 0x648, 0x1e4, 6, 0x0, 0), /* MX35_PAD_LD13__SDMA_SDMA_DEBUG_PC_13 */
-	[463] = IMX_PIN_REG(MX35_PAD_LD13, 0x648, 0x1e4, 7, 0x0, 0), /* MX35_PAD_LD13__ARM11P_TOP_TRACE_6 */
-	[464] = IMX_PIN_REG(MX35_PAD_LD14, 0x64c, 0x1e8, 0, 0x0, 0), /* MX35_PAD_LD14__IPU_DISPB_DAT_14 */
-	[465] = IMX_PIN_REG(MX35_PAD_LD14, 0x64c, 0x1e8, 5, 0x87c, 0), /* MX35_PAD_LD14__GPIO2_14 */
-	[466] = IMX_PIN_REG(MX35_PAD_LD14, 0x64c, 0x1e8, 6, 0x0, 0), /* MX35_PAD_LD14__SDMA_SDMA_DEBUG_EVENT_CHANNEL_0 */
-	[467] = IMX_PIN_REG(MX35_PAD_LD14, 0x64c, 0x1e8, 7, 0x0, 0), /* MX35_PAD_LD14__ARM11P_TOP_TRACE_7 */
-	[468] = IMX_PIN_REG(MX35_PAD_LD15, 0x650, 0x1ec, 0, 0x0, 0), /* MX35_PAD_LD15__IPU_DISPB_DAT_15 */
-	[469] = IMX_PIN_REG(MX35_PAD_LD15, 0x650, 0x1ec, 5, 0x880, 0), /* MX35_PAD_LD15__GPIO2_15 */
-	[470] = IMX_PIN_REG(MX35_PAD_LD15, 0x650, 0x1ec, 6, 0x0, 0), /* MX35_PAD_LD15__SDMA_SDMA_DEBUG_EVENT_CHANNEL_1 */
-	[471] = IMX_PIN_REG(MX35_PAD_LD15, 0x650, 0x1ec, 7, 0x0, 0), /* MX35_PAD_LD15__ARM11P_TOP_TRACE_8 */
-	[472] = IMX_PIN_REG(MX35_PAD_LD16, 0x654, 0x1f0, 0, 0x0, 0), /* MX35_PAD_LD16__IPU_DISPB_DAT_16 */
-	[473] = IMX_PIN_REG(MX35_PAD_LD16, 0x654, 0x1f0, 2, 0x928, 0), /* MX35_PAD_LD16__IPU_DISPB_D12_VSYNC */
-	[474] = IMX_PIN_REG(MX35_PAD_LD16, 0x654, 0x1f0, 5, 0x884, 0), /* MX35_PAD_LD16__GPIO2_16 */
-	[475] = IMX_PIN_REG(MX35_PAD_LD16, 0x654, 0x1f0, 6, 0x0, 0), /* MX35_PAD_LD16__SDMA_SDMA_DEBUG_EVENT_CHANNEL_2 */
-	[476] = IMX_PIN_REG(MX35_PAD_LD16, 0x654, 0x1f0, 7, 0x0, 0), /* MX35_PAD_LD16__ARM11P_TOP_TRACE_9 */
-	[477] = IMX_PIN_REG(MX35_PAD_LD17, 0x658, 0x1f4, 0, 0x0, 0), /* MX35_PAD_LD17__IPU_DISPB_DAT_17 */
-	[478] = IMX_PIN_REG(MX35_PAD_LD17, 0x658, 0x1f4, 2, 0x0, 0), /* MX35_PAD_LD17__IPU_DISPB_CS2 */
-	[479] = IMX_PIN_REG(MX35_PAD_LD17, 0x658, 0x1f4, 5, 0x888, 0), /* MX35_PAD_LD17__GPIO2_17 */
-	[480] = IMX_PIN_REG(MX35_PAD_LD17, 0x658, 0x1f4, 6, 0x0, 0), /* MX35_PAD_LD17__SDMA_SDMA_DEBUG_EVENT_CHANNEL_3 */
-	[481] = IMX_PIN_REG(MX35_PAD_LD17, 0x658, 0x1f4, 7, 0x0, 0), /* MX35_PAD_LD17__ARM11P_TOP_TRACE_10 */
-	[482] = IMX_PIN_REG(MX35_PAD_LD18, 0x65c, 0x1f8, 0, 0x0, 0), /* MX35_PAD_LD18__IPU_DISPB_DAT_18 */
-	[483] = IMX_PIN_REG(MX35_PAD_LD18, 0x65c, 0x1f8, 1, 0x924, 1), /* MX35_PAD_LD18__IPU_DISPB_D0_VSYNC */
-	[484] = IMX_PIN_REG(MX35_PAD_LD18, 0x65c, 0x1f8, 2, 0x928, 1), /* MX35_PAD_LD18__IPU_DISPB_D12_VSYNC */
-	[485] = IMX_PIN_REG(MX35_PAD_LD18, 0x65c, 0x1f8, 3, 0x818, 0), /* MX35_PAD_LD18__ESDHC3_CMD */
-	[486] = IMX_PIN_REG(MX35_PAD_LD18, 0x65c, 0x1f8, 4, 0x9b0, 0), /* MX35_PAD_LD18__USB_TOP_USBOTG_DATA_3 */
-	[487] = IMX_PIN_REG(MX35_PAD_LD18, 0x65c, 0x1f8, 5, 0x0, 0), /* MX35_PAD_LD18__GPIO3_24 */
-	[488] = IMX_PIN_REG(MX35_PAD_LD18, 0x65c, 0x1f8, 6, 0x0, 0), /* MX35_PAD_LD18__SDMA_SDMA_DEBUG_EVENT_CHANNEL_4 */
-	[489] = IMX_PIN_REG(MX35_PAD_LD18, 0x65c, 0x1f8, 7, 0x0, 0), /* MX35_PAD_LD18__ARM11P_TOP_TRACE_11 */
-	[490] = IMX_PIN_REG(MX35_PAD_LD19, 0x660, 0x1fc, 0, 0x0, 0), /* MX35_PAD_LD19__IPU_DISPB_DAT_19 */
-	[491] = IMX_PIN_REG(MX35_PAD_LD19, 0x660, 0x1fc, 1, 0x0, 0), /* MX35_PAD_LD19__IPU_DISPB_BCLK */
-	[492] = IMX_PIN_REG(MX35_PAD_LD19, 0x660, 0x1fc, 2, 0x0, 0), /* MX35_PAD_LD19__IPU_DISPB_CS1 */
-	[493] = IMX_PIN_REG(MX35_PAD_LD19, 0x660, 0x1fc, 3, 0x814, 0), /* MX35_PAD_LD19__ESDHC3_CLK */
-	[494] = IMX_PIN_REG(MX35_PAD_LD19, 0x660, 0x1fc, 4, 0x9c4, 0), /* MX35_PAD_LD19__USB_TOP_USBOTG_DIR */
-	[495] = IMX_PIN_REG(MX35_PAD_LD19, 0x660, 0x1fc, 5, 0x0, 0), /* MX35_PAD_LD19__GPIO3_25 */
-	[496] = IMX_PIN_REG(MX35_PAD_LD19, 0x660, 0x1fc, 6, 0x0, 0), /* MX35_PAD_LD19__SDMA_SDMA_DEBUG_EVENT_CHANNEL_5 */
-	[497] = IMX_PIN_REG(MX35_PAD_LD19, 0x660, 0x1fc, 7, 0x0, 0), /* MX35_PAD_LD19__ARM11P_TOP_TRACE_12 */
-	[498] = IMX_PIN_REG(MX35_PAD_LD20, 0x664, 0x200, 0, 0x0, 0), /* MX35_PAD_LD20__IPU_DISPB_DAT_20 */
-	[499] = IMX_PIN_REG(MX35_PAD_LD20, 0x664, 0x200, 1, 0x0, 0), /* MX35_PAD_LD20__IPU_DISPB_CS0 */
-	[500] = IMX_PIN_REG(MX35_PAD_LD20, 0x664, 0x200, 2, 0x0, 0), /* MX35_PAD_LD20__IPU_DISPB_SD_CLK */
-	[501] = IMX_PIN_REG(MX35_PAD_LD20, 0x664, 0x200, 3, 0x81c, 0), /* MX35_PAD_LD20__ESDHC3_DAT0 */
-	[502] = IMX_PIN_REG(MX35_PAD_LD20, 0x664, 0x200, 5, 0x0, 0), /* MX35_PAD_LD20__GPIO3_26 */
-	[503] = IMX_PIN_REG(MX35_PAD_LD20, 0x664, 0x200, 6, 0x0, 0), /* MX35_PAD_LD20__SDMA_SDMA_DEBUG_CORE_STATUS_3 */
-	[504] = IMX_PIN_REG(MX35_PAD_LD20, 0x664, 0x200, 7, 0x0, 0), /* MX35_PAD_LD20__ARM11P_TOP_TRACE_13 */
-	[505] = IMX_PIN_REG(MX35_PAD_LD21, 0x668, 0x204, 0, 0x0, 0), /* MX35_PAD_LD21__IPU_DISPB_DAT_21 */
-	[506] = IMX_PIN_REG(MX35_PAD_LD21, 0x668, 0x204, 1, 0x0, 0), /* MX35_PAD_LD21__IPU_DISPB_PAR_RS */
-	[507] = IMX_PIN_REG(MX35_PAD_LD21, 0x668, 0x204, 2, 0x0, 0), /* MX35_PAD_LD21__IPU_DISPB_SER_RS */
-	[508] = IMX_PIN_REG(MX35_PAD_LD21, 0x668, 0x204, 3, 0x820, 0), /* MX35_PAD_LD21__ESDHC3_DAT1 */
-	[509] = IMX_PIN_REG(MX35_PAD_LD21, 0x668, 0x204, 4, 0x0, 0), /* MX35_PAD_LD21__USB_TOP_USBOTG_STP */
-	[510] = IMX_PIN_REG(MX35_PAD_LD21, 0x668, 0x204, 5, 0x0, 0), /* MX35_PAD_LD21__GPIO3_27 */
-	[511] = IMX_PIN_REG(MX35_PAD_LD21, 0x668, 0x204, 6, 0x0, 0), /* MX35_PAD_LD21__SDMA_DEBUG_EVENT_CHANNEL_SEL */
-	[512] = IMX_PIN_REG(MX35_PAD_LD21, 0x668, 0x204, 7, 0x0, 0), /* MX35_PAD_LD21__ARM11P_TOP_TRACE_14 */
-	[513] = IMX_PIN_REG(MX35_PAD_LD22, 0x66c, 0x208, 0, 0x0, 0), /* MX35_PAD_LD22__IPU_DISPB_DAT_22 */
-	[514] = IMX_PIN_REG(MX35_PAD_LD22, 0x66c, 0x208, 1, 0x0, 0), /* MX35_PAD_LD22__IPU_DISPB_WR */
-	[515] = IMX_PIN_REG(MX35_PAD_LD22, 0x66c, 0x208, 2, 0x92c, 0), /* MX35_PAD_LD22__IPU_DISPB_SD_D_I */
-	[516] = IMX_PIN_REG(MX35_PAD_LD22, 0x66c, 0x208, 3, 0x824, 0), /* MX35_PAD_LD22__ESDHC3_DAT2 */
-	[517] = IMX_PIN_REG(MX35_PAD_LD22, 0x66c, 0x208, 4, 0x9c8, 0), /* MX35_PAD_LD22__USB_TOP_USBOTG_NXT */
-	[518] = IMX_PIN_REG(MX35_PAD_LD22, 0x66c, 0x208, 5, 0x0, 0), /* MX35_PAD_LD22__GPIO3_28 */
-	[519] = IMX_PIN_REG(MX35_PAD_LD22, 0x66c, 0x208, 6, 0x0, 0), /* MX35_PAD_LD22__SDMA_DEBUG_BUS_ERROR */
-	[520] = IMX_PIN_REG(MX35_PAD_LD22, 0x66c, 0x208, 7, 0x0, 0), /* MX35_PAD_LD22__ARM11P_TOP_TRCTL */
-	[521] = IMX_PIN_REG(MX35_PAD_LD23, 0x670, 0x20c, 0, 0x0, 0), /* MX35_PAD_LD23__IPU_DISPB_DAT_23 */
-	[522] = IMX_PIN_REG(MX35_PAD_LD23, 0x670, 0x20c, 1, 0x0, 0), /* MX35_PAD_LD23__IPU_DISPB_RD */
-	[523] = IMX_PIN_REG(MX35_PAD_LD23, 0x670, 0x20c, 2, 0x92c, 1), /* MX35_PAD_LD23__IPU_DISPB_SD_D_IO */
-	[524] = IMX_PIN_REG(MX35_PAD_LD23, 0x670, 0x20c, 3, 0x828, 0), /* MX35_PAD_LD23__ESDHC3_DAT3 */
-	[525] = IMX_PIN_REG(MX35_PAD_LD23, 0x670, 0x20c, 4, 0x9c0, 0), /* MX35_PAD_LD23__USB_TOP_USBOTG_DATA_7 */
-	[526] = IMX_PIN_REG(MX35_PAD_LD23, 0x670, 0x20c, 5, 0x0, 0), /* MX35_PAD_LD23__GPIO3_29 */
-	[527] = IMX_PIN_REG(MX35_PAD_LD23, 0x670, 0x20c, 6, 0x0, 0), /* MX35_PAD_LD23__SDMA_DEBUG_MATCHED_DMBUS */
-	[528] = IMX_PIN_REG(MX35_PAD_LD23, 0x670, 0x20c, 7, 0x0, 0), /* MX35_PAD_LD23__ARM11P_TOP_TRCLK */
-	[529] = IMX_PIN_REG(MX35_PAD_D3_HSYNC, 0x674, 0x210, 0, 0x0, 0), /* MX35_PAD_D3_HSYNC__IPU_DISPB_D3_HSYNC */
-	[530] = IMX_PIN_REG(MX35_PAD_D3_HSYNC, 0x674, 0x210, 2, 0x92c, 2), /* MX35_PAD_D3_HSYNC__IPU_DISPB_SD_D_IO */
-	[531] = IMX_PIN_REG(MX35_PAD_D3_HSYNC, 0x674, 0x210, 5, 0x0, 0), /* MX35_PAD_D3_HSYNC__GPIO3_30 */
-	[532] = IMX_PIN_REG(MX35_PAD_D3_HSYNC, 0x674, 0x210, 6, 0x0, 0), /* MX35_PAD_D3_HSYNC__SDMA_DEBUG_RTBUFFER_WRITE */
-	[533] = IMX_PIN_REG(MX35_PAD_D3_HSYNC, 0x674, 0x210, 7, 0x0, 0), /* MX35_PAD_D3_HSYNC__ARM11P_TOP_TRACE_15 */
-	[534] = IMX_PIN_REG(MX35_PAD_D3_FPSHIFT, 0x678, 0x214, 0, 0x0, 0), /* MX35_PAD_D3_FPSHIFT__IPU_DISPB_D3_CLK */
-	[535] = IMX_PIN_REG(MX35_PAD_D3_FPSHIFT, 0x678, 0x214, 2, 0x0, 0), /* MX35_PAD_D3_FPSHIFT__IPU_DISPB_SD_CLK */
-	[536] = IMX_PIN_REG(MX35_PAD_D3_FPSHIFT, 0x678, 0x214, 5, 0x0, 0), /* MX35_PAD_D3_FPSHIFT__GPIO3_31 */
-	[537] = IMX_PIN_REG(MX35_PAD_D3_FPSHIFT, 0x678, 0x214, 6, 0x0, 0), /* MX35_PAD_D3_FPSHIFT__SDMA_SDMA_DEBUG_CORE_STATUS_0 */
-	[538] = IMX_PIN_REG(MX35_PAD_D3_FPSHIFT, 0x678, 0x214, 7, 0x0, 0), /* MX35_PAD_D3_FPSHIFT__ARM11P_TOP_TRACE_16 */
-	[539] = IMX_PIN_REG(MX35_PAD_D3_DRDY, 0x67c, 0x218, 0, 0x0, 0), /* MX35_PAD_D3_DRDY__IPU_DISPB_D3_DRDY */
-	[540] = IMX_PIN_REG(MX35_PAD_D3_DRDY, 0x67c, 0x218, 2, 0x0, 0), /* MX35_PAD_D3_DRDY__IPU_DISPB_SD_D_O */
-	[541] = IMX_PIN_REG(MX35_PAD_D3_DRDY, 0x67c, 0x218, 5, 0x82c, 2), /* MX35_PAD_D3_DRDY__GPIO1_0 */
-	[542] = IMX_PIN_REG(MX35_PAD_D3_DRDY, 0x67c, 0x218, 6, 0x0, 0), /* MX35_PAD_D3_DRDY__SDMA_SDMA_DEBUG_CORE_STATUS_1 */
-	[543] = IMX_PIN_REG(MX35_PAD_D3_DRDY, 0x67c, 0x218, 7, 0x0, 0), /* MX35_PAD_D3_DRDY__ARM11P_TOP_TRACE_17 */
-	[544] = IMX_PIN_REG(MX35_PAD_CONTRAST, 0x680, 0x21c, 0, 0x0, 0), /* MX35_PAD_CONTRAST__IPU_DISPB_CONTR */
-	[545] = IMX_PIN_REG(MX35_PAD_CONTRAST, 0x680, 0x21c, 5, 0x838, 2), /* MX35_PAD_CONTRAST__GPIO1_1 */
-	[546] = IMX_PIN_REG(MX35_PAD_CONTRAST, 0x680, 0x21c, 6, 0x0, 0), /* MX35_PAD_CONTRAST__SDMA_SDMA_DEBUG_CORE_STATUS_2 */
-	[547] = IMX_PIN_REG(MX35_PAD_CONTRAST, 0x680, 0x21c, 7, 0x0, 0), /* MX35_PAD_CONTRAST__ARM11P_TOP_TRACE_18 */
-	[548] = IMX_PIN_REG(MX35_PAD_D3_VSYNC, 0x684, 0x220, 0, 0x0, 0), /* MX35_PAD_D3_VSYNC__IPU_DISPB_D3_VSYNC */
-	[549] = IMX_PIN_REG(MX35_PAD_D3_VSYNC, 0x684, 0x220, 2, 0x0, 0), /* MX35_PAD_D3_VSYNC__IPU_DISPB_CS1 */
-	[550] = IMX_PIN_REG(MX35_PAD_D3_VSYNC, 0x684, 0x220, 5, 0x848, 1), /* MX35_PAD_D3_VSYNC__GPIO1_2 */
-	[551] = IMX_PIN_REG(MX35_PAD_D3_VSYNC, 0x684, 0x220, 6, 0x0, 0), /* MX35_PAD_D3_VSYNC__SDMA_DEBUG_YIELD */
-	[552] = IMX_PIN_REG(MX35_PAD_D3_VSYNC, 0x684, 0x220, 7, 0x0, 0), /* MX35_PAD_D3_VSYNC__ARM11P_TOP_TRACE_19 */
-	[553] = IMX_PIN_REG(MX35_PAD_D3_REV, 0x688, 0x224, 0, 0x0, 0), /* MX35_PAD_D3_REV__IPU_DISPB_D3_REV */
-	[554] = IMX_PIN_REG(MX35_PAD_D3_REV, 0x688, 0x224, 2, 0x0, 0), /* MX35_PAD_D3_REV__IPU_DISPB_SER_RS */
-	[555] = IMX_PIN_REG(MX35_PAD_D3_REV, 0x688, 0x224, 5, 0x84c, 1), /* MX35_PAD_D3_REV__GPIO1_3 */
-	[556] = IMX_PIN_REG(MX35_PAD_D3_REV, 0x688, 0x224, 6, 0x0, 0), /* MX35_PAD_D3_REV__SDMA_DEBUG_BUS_RWB */
-	[557] = IMX_PIN_REG(MX35_PAD_D3_REV, 0x688, 0x224, 7, 0x0, 0), /* MX35_PAD_D3_REV__ARM11P_TOP_TRACE_20 */
-	[558] = IMX_PIN_REG(MX35_PAD_D3_CLS, 0x68c, 0x228, 0, 0x0, 0), /* MX35_PAD_D3_CLS__IPU_DISPB_D3_CLS */
-	[559] = IMX_PIN_REG(MX35_PAD_D3_CLS, 0x68c, 0x228, 2, 0x0, 0), /* MX35_PAD_D3_CLS__IPU_DISPB_CS2 */
-	[560] = IMX_PIN_REG(MX35_PAD_D3_CLS, 0x68c, 0x228, 5, 0x850, 2), /* MX35_PAD_D3_CLS__GPIO1_4 */
-	[561] = IMX_PIN_REG(MX35_PAD_D3_CLS, 0x68c, 0x228, 6, 0x0, 0), /* MX35_PAD_D3_CLS__SDMA_DEBUG_BUS_DEVICE_0 */
-	[562] = IMX_PIN_REG(MX35_PAD_D3_CLS, 0x68c, 0x228, 7, 0x0, 0), /* MX35_PAD_D3_CLS__ARM11P_TOP_TRACE_21 */
-	[563] = IMX_PIN_REG(MX35_PAD_D3_SPL, 0x690, 0x22c, 0, 0x0, 0), /* MX35_PAD_D3_SPL__IPU_DISPB_D3_SPL */
-	[564] = IMX_PIN_REG(MX35_PAD_D3_SPL, 0x690, 0x22c, 2, 0x928, 2), /* MX35_PAD_D3_SPL__IPU_DISPB_D12_VSYNC */
-	[565] = IMX_PIN_REG(MX35_PAD_D3_SPL, 0x690, 0x22c, 5, 0x854, 2), /* MX35_PAD_D3_SPL__GPIO1_5 */
-	[566] = IMX_PIN_REG(MX35_PAD_D3_SPL, 0x690, 0x22c, 6, 0x0, 0), /* MX35_PAD_D3_SPL__SDMA_DEBUG_BUS_DEVICE_1 */
-	[567] = IMX_PIN_REG(MX35_PAD_D3_SPL, 0x690, 0x22c, 7, 0x0, 0), /* MX35_PAD_D3_SPL__ARM11P_TOP_TRACE_22 */
-	[568] = IMX_PIN_REG(MX35_PAD_SD1_CMD, 0x694, 0x230, 0, 0x0, 0), /* MX35_PAD_SD1_CMD__ESDHC1_CMD */
-	[569] = IMX_PIN_REG(MX35_PAD_SD1_CMD, 0x694, 0x230, 1, 0x0, 0), /* MX35_PAD_SD1_CMD__MSHC_SCLK */
-	[570] = IMX_PIN_REG(MX35_PAD_SD1_CMD, 0x694, 0x230, 3, 0x924, 2), /* MX35_PAD_SD1_CMD__IPU_DISPB_D0_VSYNC */
-	[571] = IMX_PIN_REG(MX35_PAD_SD1_CMD, 0x694, 0x230, 4, 0x9b4, 0), /* MX35_PAD_SD1_CMD__USB_TOP_USBOTG_DATA_4 */
-	[572] = IMX_PIN_REG(MX35_PAD_SD1_CMD, 0x694, 0x230, 5, 0x858, 2), /* MX35_PAD_SD1_CMD__GPIO1_6 */
-	[573] = IMX_PIN_REG(MX35_PAD_SD1_CMD, 0x694, 0x230, 7, 0x0, 0), /* MX35_PAD_SD1_CMD__ARM11P_TOP_TRCTL */
-	[574] = IMX_PIN_REG(MX35_PAD_SD1_CLK, 0x698, 0x234, 0, 0x0, 0), /* MX35_PAD_SD1_CLK__ESDHC1_CLK */
-	[575] = IMX_PIN_REG(MX35_PAD_SD1_CLK, 0x698, 0x234, 1, 0x0, 0), /* MX35_PAD_SD1_CLK__MSHC_BS */
-	[576] = IMX_PIN_REG(MX35_PAD_SD1_CLK, 0x698, 0x234, 3, 0x0, 0), /* MX35_PAD_SD1_CLK__IPU_DISPB_BCLK */
-	[577] = IMX_PIN_REG(MX35_PAD_SD1_CLK, 0x698, 0x234, 4, 0x9b8, 0), /* MX35_PAD_SD1_CLK__USB_TOP_USBOTG_DATA_5 */
-	[578] = IMX_PIN_REG(MX35_PAD_SD1_CLK, 0x698, 0x234, 5, 0x85c, 2), /* MX35_PAD_SD1_CLK__GPIO1_7 */
-	[579] = IMX_PIN_REG(MX35_PAD_SD1_CLK, 0x698, 0x234, 7, 0x0, 0), /* MX35_PAD_SD1_CLK__ARM11P_TOP_TRCLK */
-	[580] = IMX_PIN_REG(MX35_PAD_SD1_DATA0, 0x69c, 0x238, 0, 0x0, 0), /* MX35_PAD_SD1_DATA0__ESDHC1_DAT0 */
-	[581] = IMX_PIN_REG(MX35_PAD_SD1_DATA0, 0x69c, 0x238, 1, 0x0, 0), /* MX35_PAD_SD1_DATA0__MSHC_DATA_0 */
-	[582] = IMX_PIN_REG(MX35_PAD_SD1_DATA0, 0x69c, 0x238, 3, 0x0, 0), /* MX35_PAD_SD1_DATA0__IPU_DISPB_CS0 */
-	[583] = IMX_PIN_REG(MX35_PAD_SD1_DATA0, 0x69c, 0x238, 4, 0x9bc, 0), /* MX35_PAD_SD1_DATA0__USB_TOP_USBOTG_DATA_6 */
-	[584] = IMX_PIN_REG(MX35_PAD_SD1_DATA0, 0x69c, 0x238, 5, 0x860, 2), /* MX35_PAD_SD1_DATA0__GPIO1_8 */
-	[585] = IMX_PIN_REG(MX35_PAD_SD1_DATA0, 0x69c, 0x238, 7, 0x0, 0), /* MX35_PAD_SD1_DATA0__ARM11P_TOP_TRACE_23 */
-	[586] = IMX_PIN_REG(MX35_PAD_SD1_DATA1, 0x6a0, 0x23c, 0, 0x0, 0), /* MX35_PAD_SD1_DATA1__ESDHC1_DAT1 */
-	[587] = IMX_PIN_REG(MX35_PAD_SD1_DATA1, 0x6a0, 0x23c, 1, 0x0, 0), /* MX35_PAD_SD1_DATA1__MSHC_DATA_1 */
-	[588] = IMX_PIN_REG(MX35_PAD_SD1_DATA1, 0x6a0, 0x23c, 3, 0x0, 0), /* MX35_PAD_SD1_DATA1__IPU_DISPB_PAR_RS */
-	[589] = IMX_PIN_REG(MX35_PAD_SD1_DATA1, 0x6a0, 0x23c, 4, 0x9a4, 0), /* MX35_PAD_SD1_DATA1__USB_TOP_USBOTG_DATA_0 */
-	[590] = IMX_PIN_REG(MX35_PAD_SD1_DATA1, 0x6a0, 0x23c, 5, 0x864, 1), /* MX35_PAD_SD1_DATA1__GPIO1_9 */
-	[591] = IMX_PIN_REG(MX35_PAD_SD1_DATA1, 0x6a0, 0x23c, 7, 0x0, 0), /* MX35_PAD_SD1_DATA1__ARM11P_TOP_TRACE_24 */
-	[592] = IMX_PIN_REG(MX35_PAD_SD1_DATA2, 0x6a4, 0x240, 0, 0x0, 0), /* MX35_PAD_SD1_DATA2__ESDHC1_DAT2 */
-	[593] = IMX_PIN_REG(MX35_PAD_SD1_DATA2, 0x6a4, 0x240, 1, 0x0, 0), /* MX35_PAD_SD1_DATA2__MSHC_DATA_2 */
-	[594] = IMX_PIN_REG(MX35_PAD_SD1_DATA2, 0x6a4, 0x240, 3, 0x0, 0), /* MX35_PAD_SD1_DATA2__IPU_DISPB_WR */
-	[595] = IMX_PIN_REG(MX35_PAD_SD1_DATA2, 0x6a4, 0x240, 4, 0x9a8, 0), /* MX35_PAD_SD1_DATA2__USB_TOP_USBOTG_DATA_1 */
-	[596] = IMX_PIN_REG(MX35_PAD_SD1_DATA2, 0x6a4, 0x240, 5, 0x830, 1), /* MX35_PAD_SD1_DATA2__GPIO1_10 */
-	[597] = IMX_PIN_REG(MX35_PAD_SD1_DATA2, 0x6a4, 0x240, 7, 0x0, 0), /* MX35_PAD_SD1_DATA2__ARM11P_TOP_TRACE_25 */
-	[598] = IMX_PIN_REG(MX35_PAD_SD1_DATA3, 0x6a8, 0x244, 0, 0x0, 0), /* MX35_PAD_SD1_DATA3__ESDHC1_DAT3 */
-	[599] = IMX_PIN_REG(MX35_PAD_SD1_DATA3, 0x6a8, 0x244, 1, 0x0, 0), /* MX35_PAD_SD1_DATA3__MSHC_DATA_3 */
-	[600] = IMX_PIN_REG(MX35_PAD_SD1_DATA3, 0x6a8, 0x244, 3, 0x0, 0), /* MX35_PAD_SD1_DATA3__IPU_DISPB_RD */
-	[601] = IMX_PIN_REG(MX35_PAD_SD1_DATA3, 0x6a8, 0x244, 4, 0x9ac, 0), /* MX35_PAD_SD1_DATA3__USB_TOP_USBOTG_DATA_2 */
-	[602] = IMX_PIN_REG(MX35_PAD_SD1_DATA3, 0x6a8, 0x244, 5, 0x834, 1), /* MX35_PAD_SD1_DATA3__GPIO1_11 */
-	[603] = IMX_PIN_REG(MX35_PAD_SD1_DATA3, 0x6a8, 0x244, 7, 0x0, 0), /* MX35_PAD_SD1_DATA3__ARM11P_TOP_TRACE_26 */
-	[604] = IMX_PIN_REG(MX35_PAD_SD2_CMD, 0x6ac, 0x248, 0, 0x0, 0), /* MX35_PAD_SD2_CMD__ESDHC2_CMD */
-	[605] = IMX_PIN_REG(MX35_PAD_SD2_CMD, 0x6ac, 0x248, 1, 0x91c, 2), /* MX35_PAD_SD2_CMD__I2C3_SCL */
-	[606] = IMX_PIN_REG(MX35_PAD_SD2_CMD, 0x6ac, 0x248, 2, 0x804, 0), /* MX35_PAD_SD2_CMD__ESDHC1_DAT4 */
-	[607] = IMX_PIN_REG(MX35_PAD_SD2_CMD, 0x6ac, 0x248, 3, 0x938, 2), /* MX35_PAD_SD2_CMD__IPU_CSI_D_2 */
-	[608] = IMX_PIN_REG(MX35_PAD_SD2_CMD, 0x6ac, 0x248, 4, 0x9dc, 0), /* MX35_PAD_SD2_CMD__USB_TOP_USBH2_DATA_4 */
-	[609] = IMX_PIN_REG(MX35_PAD_SD2_CMD, 0x6ac, 0x248, 5, 0x868, 2), /* MX35_PAD_SD2_CMD__GPIO2_0 */
-	[610] = IMX_PIN_REG(MX35_PAD_SD2_CMD, 0x6ac, 0x248, 6, 0x0, 0), /* MX35_PAD_SD2_CMD__SPDIF_SPDIF_OUT1 */
-	[611] = IMX_PIN_REG(MX35_PAD_SD2_CMD, 0x6ac, 0x248, 7, 0x928, 3), /* MX35_PAD_SD2_CMD__IPU_DISPB_D12_VSYNC */
-	[612] = IMX_PIN_REG(MX35_PAD_SD2_CLK, 0x6b0, 0x24c, 0, 0x0, 0), /* MX35_PAD_SD2_CLK__ESDHC2_CLK */
-	[613] = IMX_PIN_REG(MX35_PAD_SD2_CLK, 0x6b0, 0x24c, 1, 0x920, 2), /* MX35_PAD_SD2_CLK__I2C3_SDA */
-	[614] = IMX_PIN_REG(MX35_PAD_SD2_CLK, 0x6b0, 0x24c, 2, 0x808, 0), /* MX35_PAD_SD2_CLK__ESDHC1_DAT5 */
-	[615] = IMX_PIN_REG(MX35_PAD_SD2_CLK, 0x6b0, 0x24c, 3, 0x93c, 2), /* MX35_PAD_SD2_CLK__IPU_CSI_D_3 */
-	[616] = IMX_PIN_REG(MX35_PAD_SD2_CLK, 0x6b0, 0x24c, 4, 0x9e0, 0), /* MX35_PAD_SD2_CLK__USB_TOP_USBH2_DATA_5 */
-	[617] = IMX_PIN_REG(MX35_PAD_SD2_CLK, 0x6b0, 0x24c, 5, 0x894, 1), /* MX35_PAD_SD2_CLK__GPIO2_1 */
-	[618] = IMX_PIN_REG(MX35_PAD_SD2_CLK, 0x6b0, 0x24c, 6, 0x998, 2), /* MX35_PAD_SD2_CLK__SPDIF_SPDIF_IN1 */
-	[619] = IMX_PIN_REG(MX35_PAD_SD2_CLK, 0x6b0, 0x24c, 7, 0x0, 0), /* MX35_PAD_SD2_CLK__IPU_DISPB_CS2 */
-	[620] = IMX_PIN_REG(MX35_PAD_SD2_DATA0, 0x6b4, 0x250, 0, 0x0, 0), /* MX35_PAD_SD2_DATA0__ESDHC2_DAT0 */
-	[621] = IMX_PIN_REG(MX35_PAD_SD2_DATA0, 0x6b4, 0x250, 1, 0x9a0, 1), /* MX35_PAD_SD2_DATA0__UART3_RXD_MUX */
-	[622] = IMX_PIN_REG(MX35_PAD_SD2_DATA0, 0x6b4, 0x250, 2, 0x80c, 0), /* MX35_PAD_SD2_DATA0__ESDHC1_DAT6 */
-	[623] = IMX_PIN_REG(MX35_PAD_SD2_DATA0, 0x6b4, 0x250, 3, 0x940, 1), /* MX35_PAD_SD2_DATA0__IPU_CSI_D_4 */
-	[624] = IMX_PIN_REG(MX35_PAD_SD2_DATA0, 0x6b4, 0x250, 4, 0x9e4, 0), /* MX35_PAD_SD2_DATA0__USB_TOP_USBH2_DATA_6 */
-	[625] = IMX_PIN_REG(MX35_PAD_SD2_DATA0, 0x6b4, 0x250, 5, 0x8c0, 1), /* MX35_PAD_SD2_DATA0__GPIO2_2 */
-	[626] = IMX_PIN_REG(MX35_PAD_SD2_DATA0, 0x6b4, 0x250, 6, 0x994, 3), /* MX35_PAD_SD2_DATA0__SPDIF_SPDIF_EXTCLK */
-	[627] = IMX_PIN_REG(MX35_PAD_SD2_DATA1, 0x6b8, 0x254, 0, 0x0, 0), /* MX35_PAD_SD2_DATA1__ESDHC2_DAT1 */
-	[628] = IMX_PIN_REG(MX35_PAD_SD2_DATA1, 0x6b8, 0x254, 1, 0x0, 0), /* MX35_PAD_SD2_DATA1__UART3_TXD_MUX */
-	[629] = IMX_PIN_REG(MX35_PAD_SD2_DATA1, 0x6b8, 0x254, 2, 0x810, 0), /* MX35_PAD_SD2_DATA1__ESDHC1_DAT7 */
-	[630] = IMX_PIN_REG(MX35_PAD_SD2_DATA1, 0x6b8, 0x254, 3, 0x944, 1), /* MX35_PAD_SD2_DATA1__IPU_CSI_D_5 */
-	[631] = IMX_PIN_REG(MX35_PAD_SD2_DATA1, 0x6b8, 0x254, 4, 0x9cc, 0), /* MX35_PAD_SD2_DATA1__USB_TOP_USBH2_DATA_0 */
-	[632] = IMX_PIN_REG(MX35_PAD_SD2_DATA1, 0x6b8, 0x254, 5, 0x8cc, 1), /* MX35_PAD_SD2_DATA1__GPIO2_3 */
-	[633] = IMX_PIN_REG(MX35_PAD_SD2_DATA2, 0x6bc, 0x258, 0, 0x0, 0), /* MX35_PAD_SD2_DATA2__ESDHC2_DAT2 */
-	[634] = IMX_PIN_REG(MX35_PAD_SD2_DATA2, 0x6bc, 0x258, 1, 0x99c, 0), /* MX35_PAD_SD2_DATA2__UART3_RTS */
-	[635] = IMX_PIN_REG(MX35_PAD_SD2_DATA2, 0x6bc, 0x258, 2, 0x7c8, 1), /* MX35_PAD_SD2_DATA2__CAN1_RXCAN */
-	[636] = IMX_PIN_REG(MX35_PAD_SD2_DATA2, 0x6bc, 0x258, 3, 0x948, 1), /* MX35_PAD_SD2_DATA2__IPU_CSI_D_6 */
-	[637] = IMX_PIN_REG(MX35_PAD_SD2_DATA2, 0x6bc, 0x258, 4, 0x9d0, 0), /* MX35_PAD_SD2_DATA2__USB_TOP_USBH2_DATA_1 */
-	[638] = IMX_PIN_REG(MX35_PAD_SD2_DATA2, 0x6bc, 0x258, 5, 0x8d0, 1), /* MX35_PAD_SD2_DATA2__GPIO2_4 */
-	[639] = IMX_PIN_REG(MX35_PAD_SD2_DATA3, 0x6c0, 0x25c, 0, 0x0, 0), /* MX35_PAD_SD2_DATA3__ESDHC2_DAT3 */
-	[640] = IMX_PIN_REG(MX35_PAD_SD2_DATA3, 0x6c0, 0x25c, 1, 0x0, 0), /* MX35_PAD_SD2_DATA3__UART3_CTS */
-	[641] = IMX_PIN_REG(MX35_PAD_SD2_DATA3, 0x6c0, 0x25c, 2, 0x0, 0), /* MX35_PAD_SD2_DATA3__CAN1_TXCAN */
-	[642] = IMX_PIN_REG(MX35_PAD_SD2_DATA3, 0x6c0, 0x25c, 3, 0x94c, 1), /* MX35_PAD_SD2_DATA3__IPU_CSI_D_7 */
-	[643] = IMX_PIN_REG(MX35_PAD_SD2_DATA3, 0x6c0, 0x25c, 4, 0x9d4, 0), /* MX35_PAD_SD2_DATA3__USB_TOP_USBH2_DATA_2 */
-	[644] = IMX_PIN_REG(MX35_PAD_SD2_DATA3, 0x6c0, 0x25c, 5, 0x8d4, 1), /* MX35_PAD_SD2_DATA3__GPIO2_5 */
-	[645] = IMX_PIN_REG(MX35_PAD_ATA_CS0, 0x6c4, 0x260, 0, 0x0, 0), /* MX35_PAD_ATA_CS0__ATA_CS0 */
-	[646] = IMX_PIN_REG(MX35_PAD_ATA_CS0, 0x6c4, 0x260, 1, 0x7dc, 1), /* MX35_PAD_ATA_CS0__CSPI1_SS3 */
-	[647] = IMX_PIN_REG(MX35_PAD_ATA_CS0, 0x6c4, 0x260, 3, 0x0, 0), /* MX35_PAD_ATA_CS0__IPU_DISPB_CS1 */
-	[648] = IMX_PIN_REG(MX35_PAD_ATA_CS0, 0x6c4, 0x260, 5, 0x8d8, 1), /* MX35_PAD_ATA_CS0__GPIO2_6 */
-	[649] = IMX_PIN_REG(MX35_PAD_ATA_CS0, 0x6c4, 0x260, 6, 0x0, 0), /* MX35_PAD_ATA_CS0__IPU_DIAGB_0 */
-	[650] = IMX_PIN_REG(MX35_PAD_ATA_CS0, 0x6c4, 0x260, 7, 0x0, 0), /* MX35_PAD_ATA_CS0__ARM11P_TOP_MAX1_HMASTER_0 */
-	[651] = IMX_PIN_REG(MX35_PAD_ATA_CS1, 0x6c8, 0x264, 0, 0x0, 0), /* MX35_PAD_ATA_CS1__ATA_CS1 */
-	[652] = IMX_PIN_REG(MX35_PAD_ATA_CS1, 0x6c8, 0x264, 3, 0x0, 0), /* MX35_PAD_ATA_CS1__IPU_DISPB_CS2 */
-	[653] = IMX_PIN_REG(MX35_PAD_ATA_CS1, 0x6c8, 0x264, 4, 0x7f0, 1), /* MX35_PAD_ATA_CS1__CSPI2_SS0 */
-	[654] = IMX_PIN_REG(MX35_PAD_ATA_CS1, 0x6c8, 0x264, 5, 0x8dc, 1), /* MX35_PAD_ATA_CS1__GPIO2_7 */
-	[655] = IMX_PIN_REG(MX35_PAD_ATA_CS1, 0x6c8, 0x264, 6, 0x0, 0), /* MX35_PAD_ATA_CS1__IPU_DIAGB_1 */
-	[656] = IMX_PIN_REG(MX35_PAD_ATA_CS1, 0x6c8, 0x264, 7, 0x0, 0), /* MX35_PAD_ATA_CS1__ARM11P_TOP_MAX1_HMASTER_1 */
-	[657] = IMX_PIN_REG(MX35_PAD_ATA_DIOR, 0x6cc, 0x268, 0, 0x0, 0), /* MX35_PAD_ATA_DIOR__ATA_DIOR */
-	[658] = IMX_PIN_REG(MX35_PAD_ATA_DIOR, 0x6cc, 0x268, 1, 0x81c, 1), /* MX35_PAD_ATA_DIOR__ESDHC3_DAT0 */
-	[659] = IMX_PIN_REG(MX35_PAD_ATA_DIOR, 0x6cc, 0x268, 2, 0x9c4, 1), /* MX35_PAD_ATA_DIOR__USB_TOP_USBOTG_DIR */
-	[660] = IMX_PIN_REG(MX35_PAD_ATA_DIOR, 0x6cc, 0x268, 3, 0x0, 0), /* MX35_PAD_ATA_DIOR__IPU_DISPB_BE0 */
-	[661] = IMX_PIN_REG(MX35_PAD_ATA_DIOR, 0x6cc, 0x268, 4, 0x7f4, 1), /* MX35_PAD_ATA_DIOR__CSPI2_SS1 */
-	[662] = IMX_PIN_REG(MX35_PAD_ATA_DIOR, 0x6cc, 0x268, 5, 0x8e0, 1), /* MX35_PAD_ATA_DIOR__GPIO2_8 */
-	[663] = IMX_PIN_REG(MX35_PAD_ATA_DIOR, 0x6cc, 0x268, 6, 0x0, 0), /* MX35_PAD_ATA_DIOR__IPU_DIAGB_2 */
-	[664] = IMX_PIN_REG(MX35_PAD_ATA_DIOR, 0x6cc, 0x268, 7, 0x0, 0), /* MX35_PAD_ATA_DIOR__ARM11P_TOP_MAX1_HMASTER_2 */
-	[665] = IMX_PIN_REG(MX35_PAD_ATA_DIOW, 0x6d0, 0x26c, 0, 0x0, 0), /* MX35_PAD_ATA_DIOW__ATA_DIOW */
-	[666] = IMX_PIN_REG(MX35_PAD_ATA_DIOW, 0x6d0, 0x26c, 1, 0x820, 1), /* MX35_PAD_ATA_DIOW__ESDHC3_DAT1 */
-	[667] = IMX_PIN_REG(MX35_PAD_ATA_DIOW, 0x6d0, 0x26c, 2, 0x0, 0), /* MX35_PAD_ATA_DIOW__USB_TOP_USBOTG_STP */
-	[668] = IMX_PIN_REG(MX35_PAD_ATA_DIOW, 0x6d0, 0x26c, 3, 0x0, 0), /* MX35_PAD_ATA_DIOW__IPU_DISPB_BE1 */
-	[669] = IMX_PIN_REG(MX35_PAD_ATA_DIOW, 0x6d0, 0x26c, 4, 0x7ec, 2), /* MX35_PAD_ATA_DIOW__CSPI2_MOSI */
-	[670] = IMX_PIN_REG(MX35_PAD_ATA_DIOW, 0x6d0, 0x26c, 5, 0x8e4, 1), /* MX35_PAD_ATA_DIOW__GPIO2_9 */
-	[671] = IMX_PIN_REG(MX35_PAD_ATA_DIOW, 0x6d0, 0x26c, 6, 0x0, 0), /* MX35_PAD_ATA_DIOW__IPU_DIAGB_3 */
-	[672] = IMX_PIN_REG(MX35_PAD_ATA_DIOW, 0x6d0, 0x26c, 7, 0x0, 0), /* MX35_PAD_ATA_DIOW__ARM11P_TOP_MAX1_HMASTER_3 */
-	[673] = IMX_PIN_REG(MX35_PAD_ATA_DMACK, 0x6d4, 0x270, 0, 0x0, 0), /* MX35_PAD_ATA_DMACK__ATA_DMACK */
-	[674] = IMX_PIN_REG(MX35_PAD_ATA_DMACK, 0x6d4, 0x270, 1, 0x824, 1), /* MX35_PAD_ATA_DMACK__ESDHC3_DAT2 */
-	[675] = IMX_PIN_REG(MX35_PAD_ATA_DMACK, 0x6d4, 0x270, 2, 0x9c8, 1), /* MX35_PAD_ATA_DMACK__USB_TOP_USBOTG_NXT */
-	[676] = IMX_PIN_REG(MX35_PAD_ATA_DMACK, 0x6d4, 0x270, 4, 0x7e8, 2), /* MX35_PAD_ATA_DMACK__CSPI2_MISO */
-	[677] = IMX_PIN_REG(MX35_PAD_ATA_DMACK, 0x6d4, 0x270, 5, 0x86c, 1), /* MX35_PAD_ATA_DMACK__GPIO2_10 */
-	[678] = IMX_PIN_REG(MX35_PAD_ATA_DMACK, 0x6d4, 0x270, 6, 0x0, 0), /* MX35_PAD_ATA_DMACK__IPU_DIAGB_4 */
-	[679] = IMX_PIN_REG(MX35_PAD_ATA_DMACK, 0x6d4, 0x270, 7, 0x0, 0), /* MX35_PAD_ATA_DMACK__ARM11P_TOP_MAX0_HMASTER_0 */
-	[680] = IMX_PIN_REG(MX35_PAD_ATA_RESET_B, 0x6d8, 0x274, 0, 0x0, 0), /* MX35_PAD_ATA_RESET_B__ATA_RESET_B */
-	[681] = IMX_PIN_REG(MX35_PAD_ATA_RESET_B, 0x6d8, 0x274, 1, 0x828, 1), /* MX35_PAD_ATA_RESET_B__ESDHC3_DAT3 */
-	[682] = IMX_PIN_REG(MX35_PAD_ATA_RESET_B, 0x6d8, 0x274, 2, 0x9a4, 1), /* MX35_PAD_ATA_RESET_B__USB_TOP_USBOTG_DATA_0 */
-	[683] = IMX_PIN_REG(MX35_PAD_ATA_RESET_B, 0x6d8, 0x274, 3, 0x0, 0), /* MX35_PAD_ATA_RESET_B__IPU_DISPB_SD_D_O */
-	[684] = IMX_PIN_REG(MX35_PAD_ATA_RESET_B, 0x6d8, 0x274, 4, 0x7e4, 2), /* MX35_PAD_ATA_RESET_B__CSPI2_RDY */
-	[685] = IMX_PIN_REG(MX35_PAD_ATA_RESET_B, 0x6d8, 0x274, 5, 0x870, 1), /* MX35_PAD_ATA_RESET_B__GPIO2_11 */
-	[686] = IMX_PIN_REG(MX35_PAD_ATA_RESET_B, 0x6d8, 0x274, 6, 0x0, 0), /* MX35_PAD_ATA_RESET_B__IPU_DIAGB_5 */
-	[687] = IMX_PIN_REG(MX35_PAD_ATA_RESET_B, 0x6d8, 0x274, 7, 0x0, 0), /* MX35_PAD_ATA_RESET_B__ARM11P_TOP_MAX0_HMASTER_1 */
-	[688] = IMX_PIN_REG(MX35_PAD_ATA_IORDY, 0x6dc, 0x278, 0, 0x0, 0), /* MX35_PAD_ATA_IORDY__ATA_IORDY */
-	[689] = IMX_PIN_REG(MX35_PAD_ATA_IORDY, 0x6dc, 0x278, 1, 0x0, 0), /* MX35_PAD_ATA_IORDY__ESDHC3_DAT4 */
-	[690] = IMX_PIN_REG(MX35_PAD_ATA_IORDY, 0x6dc, 0x278, 2, 0x9a8, 1), /* MX35_PAD_ATA_IORDY__USB_TOP_USBOTG_DATA_1 */
-	[691] = IMX_PIN_REG(MX35_PAD_ATA_IORDY, 0x6dc, 0x278, 3, 0x92c, 3), /* MX35_PAD_ATA_IORDY__IPU_DISPB_SD_D_IO */
-	[692] = IMX_PIN_REG(MX35_PAD_ATA_IORDY, 0x6dc, 0x278, 4, 0x0, 0), /* MX35_PAD_ATA_IORDY__ESDHC2_DAT4 */
-	[693] = IMX_PIN_REG(MX35_PAD_ATA_IORDY, 0x6dc, 0x278, 5, 0x874, 1), /* MX35_PAD_ATA_IORDY__GPIO2_12 */
-	[694] = IMX_PIN_REG(MX35_PAD_ATA_IORDY, 0x6dc, 0x278, 6, 0x0, 0), /* MX35_PAD_ATA_IORDY__IPU_DIAGB_6 */
-	[695] = IMX_PIN_REG(MX35_PAD_ATA_IORDY, 0x6dc, 0x278, 7, 0x0, 0), /* MX35_PAD_ATA_IORDY__ARM11P_TOP_MAX0_HMASTER_2 */
-	[696] = IMX_PIN_REG(MX35_PAD_ATA_DATA0, 0x6e0, 0x27c, 0, 0x0, 0), /* MX35_PAD_ATA_DATA0__ATA_DATA_0 */
-	[697] = IMX_PIN_REG(MX35_PAD_ATA_DATA0, 0x6e0, 0x27c, 1, 0x0, 0), /* MX35_PAD_ATA_DATA0__ESDHC3_DAT5 */
-	[698] = IMX_PIN_REG(MX35_PAD_ATA_DATA0, 0x6e0, 0x27c, 2, 0x9ac, 1), /* MX35_PAD_ATA_DATA0__USB_TOP_USBOTG_DATA_2 */
-	[699] = IMX_PIN_REG(MX35_PAD_ATA_DATA0, 0x6e0, 0x27c, 3, 0x928, 4), /* MX35_PAD_ATA_DATA0__IPU_DISPB_D12_VSYNC */
-	[700] = IMX_PIN_REG(MX35_PAD_ATA_DATA0, 0x6e0, 0x27c, 4, 0x0, 0), /* MX35_PAD_ATA_DATA0__ESDHC2_DAT5 */
-	[701] = IMX_PIN_REG(MX35_PAD_ATA_DATA0, 0x6e0, 0x27c, 5, 0x878, 1), /* MX35_PAD_ATA_DATA0__GPIO2_13 */
-	[702] = IMX_PIN_REG(MX35_PAD_ATA_DATA0, 0x6e0, 0x27c, 6, 0x0, 0), /* MX35_PAD_ATA_DATA0__IPU_DIAGB_7 */
-	[703] = IMX_PIN_REG(MX35_PAD_ATA_DATA0, 0x6e0, 0x27c, 7, 0x0, 0), /* MX35_PAD_ATA_DATA0__ARM11P_TOP_MAX0_HMASTER_3 */
-	[704] = IMX_PIN_REG(MX35_PAD_ATA_DATA1, 0x6e4, 0x280, 0, 0x0, 0), /* MX35_PAD_ATA_DATA1__ATA_DATA_1 */
-	[705] = IMX_PIN_REG(MX35_PAD_ATA_DATA1, 0x6e4, 0x280, 1, 0x0, 0), /* MX35_PAD_ATA_DATA1__ESDHC3_DAT6 */
-	[706] = IMX_PIN_REG(MX35_PAD_ATA_DATA1, 0x6e4, 0x280, 2, 0x9b0, 1), /* MX35_PAD_ATA_DATA1__USB_TOP_USBOTG_DATA_3 */
-	[707] = IMX_PIN_REG(MX35_PAD_ATA_DATA1, 0x6e4, 0x280, 3, 0x0, 0), /* MX35_PAD_ATA_DATA1__IPU_DISPB_SD_CLK */
-	[708] = IMX_PIN_REG(MX35_PAD_ATA_DATA1, 0x6e4, 0x280, 4, 0x0, 0), /* MX35_PAD_ATA_DATA1__ESDHC2_DAT6 */
-	[709] = IMX_PIN_REG(MX35_PAD_ATA_DATA1, 0x6e4, 0x280, 5, 0x87c, 1), /* MX35_PAD_ATA_DATA1__GPIO2_14 */
-	[710] = IMX_PIN_REG(MX35_PAD_ATA_DATA1, 0x6e4, 0x280, 6, 0x0, 0), /* MX35_PAD_ATA_DATA1__IPU_DIAGB_8 */
-	[711] = IMX_PIN_REG(MX35_PAD_ATA_DATA1, 0x6e4, 0x280, 7, 0x0, 0), /* MX35_PAD_ATA_DATA1__ARM11P_TOP_TRACE_27 */
-	[712] = IMX_PIN_REG(MX35_PAD_ATA_DATA2, 0x6e8, 0x284, 0, 0x0, 0), /* MX35_PAD_ATA_DATA2__ATA_DATA_2 */
-	[713] = IMX_PIN_REG(MX35_PAD_ATA_DATA2, 0x6e8, 0x284, 1, 0x0, 0), /* MX35_PAD_ATA_DATA2__ESDHC3_DAT7 */
-	[714] = IMX_PIN_REG(MX35_PAD_ATA_DATA2, 0x6e8, 0x284, 2, 0x9b4, 1), /* MX35_PAD_ATA_DATA2__USB_TOP_USBOTG_DATA_4 */
-	[715] = IMX_PIN_REG(MX35_PAD_ATA_DATA2, 0x6e8, 0x284, 3, 0x0, 0), /* MX35_PAD_ATA_DATA2__IPU_DISPB_SER_RS */
-	[716] = IMX_PIN_REG(MX35_PAD_ATA_DATA2, 0x6e8, 0x284, 4, 0x0, 0), /* MX35_PAD_ATA_DATA2__ESDHC2_DAT7 */
-	[717] = IMX_PIN_REG(MX35_PAD_ATA_DATA2, 0x6e8, 0x284, 5, 0x880, 1), /* MX35_PAD_ATA_DATA2__GPIO2_15 */
-	[718] = IMX_PIN_REG(MX35_PAD_ATA_DATA2, 0x6e8, 0x284, 6, 0x0, 0), /* MX35_PAD_ATA_DATA2__IPU_DIAGB_9 */
-	[719] = IMX_PIN_REG(MX35_PAD_ATA_DATA2, 0x6e8, 0x284, 7, 0x0, 0), /* MX35_PAD_ATA_DATA2__ARM11P_TOP_TRACE_28 */
-	[720] = IMX_PIN_REG(MX35_PAD_ATA_DATA3, 0x6ec, 0x288, 0, 0x0, 0), /* MX35_PAD_ATA_DATA3__ATA_DATA_3 */
-	[721] = IMX_PIN_REG(MX35_PAD_ATA_DATA3, 0x6ec, 0x288, 1, 0x814, 1), /* MX35_PAD_ATA_DATA3__ESDHC3_CLK */
-	[722] = IMX_PIN_REG(MX35_PAD_ATA_DATA3, 0x6ec, 0x288, 2, 0x9b8, 1), /* MX35_PAD_ATA_DATA3__USB_TOP_USBOTG_DATA_5 */
-	[723] = IMX_PIN_REG(MX35_PAD_ATA_DATA3, 0x6ec, 0x288, 4, 0x7e0, 2), /* MX35_PAD_ATA_DATA3__CSPI2_SCLK */
-	[724] = IMX_PIN_REG(MX35_PAD_ATA_DATA3, 0x6ec, 0x288, 5, 0x884, 1), /* MX35_PAD_ATA_DATA3__GPIO2_16 */
-	[725] = IMX_PIN_REG(MX35_PAD_ATA_DATA3, 0x6ec, 0x288, 6, 0x0, 0), /* MX35_PAD_ATA_DATA3__IPU_DIAGB_10 */
-	[726] = IMX_PIN_REG(MX35_PAD_ATA_DATA3, 0x6ec, 0x288, 7, 0x0, 0), /* MX35_PAD_ATA_DATA3__ARM11P_TOP_TRACE_29 */
-	[727] = IMX_PIN_REG(MX35_PAD_ATA_DATA4, 0x6f0, 0x28c, 0, 0x0, 0), /* MX35_PAD_ATA_DATA4__ATA_DATA_4 */
-	[728] = IMX_PIN_REG(MX35_PAD_ATA_DATA4, 0x6f0, 0x28c, 1, 0x818, 1), /* MX35_PAD_ATA_DATA4__ESDHC3_CMD */
-	[729] = IMX_PIN_REG(MX35_PAD_ATA_DATA4, 0x6f0, 0x28c, 2, 0x9bc, 1), /* MX35_PAD_ATA_DATA4__USB_TOP_USBOTG_DATA_6 */
-	[730] = IMX_PIN_REG(MX35_PAD_ATA_DATA4, 0x6f0, 0x28c, 5, 0x888, 1), /* MX35_PAD_ATA_DATA4__GPIO2_17 */
-	[731] = IMX_PIN_REG(MX35_PAD_ATA_DATA4, 0x6f0, 0x28c, 6, 0x0, 0), /* MX35_PAD_ATA_DATA4__IPU_DIAGB_11 */
-	[732] = IMX_PIN_REG(MX35_PAD_ATA_DATA4, 0x6f0, 0x28c, 7, 0x0, 0), /* MX35_PAD_ATA_DATA4__ARM11P_TOP_TRACE_30 */
-	[733] = IMX_PIN_REG(MX35_PAD_ATA_DATA5, 0x6f4, 0x290, 0, 0x0, 0), /* MX35_PAD_ATA_DATA5__ATA_DATA_5 */
-	[734] = IMX_PIN_REG(MX35_PAD_ATA_DATA5, 0x6f4, 0x290, 2, 0x9c0, 1), /* MX35_PAD_ATA_DATA5__USB_TOP_USBOTG_DATA_7 */
-	[735] = IMX_PIN_REG(MX35_PAD_ATA_DATA5, 0x6f4, 0x290, 5, 0x88c, 1), /* MX35_PAD_ATA_DATA5__GPIO2_18 */
-	[736] = IMX_PIN_REG(MX35_PAD_ATA_DATA5, 0x6f4, 0x290, 6, 0x0, 0), /* MX35_PAD_ATA_DATA5__IPU_DIAGB_12 */
-	[737] = IMX_PIN_REG(MX35_PAD_ATA_DATA5, 0x6f4, 0x290, 7, 0x0, 0), /* MX35_PAD_ATA_DATA5__ARM11P_TOP_TRACE_31 */
-	[738] = IMX_PIN_REG(MX35_PAD_ATA_DATA6, 0x6f8, 0x294, 0, 0x0, 0), /* MX35_PAD_ATA_DATA6__ATA_DATA_6 */
-	[739] = IMX_PIN_REG(MX35_PAD_ATA_DATA6, 0x6f8, 0x294, 1, 0x0, 0), /* MX35_PAD_ATA_DATA6__CAN1_TXCAN */
-	[740] = IMX_PIN_REG(MX35_PAD_ATA_DATA6, 0x6f8, 0x294, 2, 0x0, 0), /* MX35_PAD_ATA_DATA6__UART1_DTR */
-	[741] = IMX_PIN_REG(MX35_PAD_ATA_DATA6, 0x6f8, 0x294, 3, 0x7b4, 0), /* MX35_PAD_ATA_DATA6__AUDMUX_AUD6_TXD */
-	[742] = IMX_PIN_REG(MX35_PAD_ATA_DATA6, 0x6f8, 0x294, 5, 0x890, 1), /* MX35_PAD_ATA_DATA6__GPIO2_19 */
-	[743] = IMX_PIN_REG(MX35_PAD_ATA_DATA6, 0x6f8, 0x294, 6, 0x0, 0), /* MX35_PAD_ATA_DATA6__IPU_DIAGB_13 */
-	[744] = IMX_PIN_REG(MX35_PAD_ATA_DATA7, 0x6fc, 0x298, 0, 0x0, 0), /* MX35_PAD_ATA_DATA7__ATA_DATA_7 */
-	[745] = IMX_PIN_REG(MX35_PAD_ATA_DATA7, 0x6fc, 0x298, 1, 0x7c8, 2), /* MX35_PAD_ATA_DATA7__CAN1_RXCAN */
-	[746] = IMX_PIN_REG(MX35_PAD_ATA_DATA7, 0x6fc, 0x298, 2, 0x0, 0), /* MX35_PAD_ATA_DATA7__UART1_DSR */
-	[747] = IMX_PIN_REG(MX35_PAD_ATA_DATA7, 0x6fc, 0x298, 3, 0x7b0, 0), /* MX35_PAD_ATA_DATA7__AUDMUX_AUD6_RXD */
-	[748] = IMX_PIN_REG(MX35_PAD_ATA_DATA7, 0x6fc, 0x298, 5, 0x898, 1), /* MX35_PAD_ATA_DATA7__GPIO2_20 */
-	[749] = IMX_PIN_REG(MX35_PAD_ATA_DATA7, 0x6fc, 0x298, 6, 0x0, 0), /* MX35_PAD_ATA_DATA7__IPU_DIAGB_14 */
-	[750] = IMX_PIN_REG(MX35_PAD_ATA_DATA8, 0x700, 0x29c, 0, 0x0, 0), /* MX35_PAD_ATA_DATA8__ATA_DATA_8 */
-	[751] = IMX_PIN_REG(MX35_PAD_ATA_DATA8, 0x700, 0x29c, 1, 0x99c, 1), /* MX35_PAD_ATA_DATA8__UART3_RTS */
-	[752] = IMX_PIN_REG(MX35_PAD_ATA_DATA8, 0x700, 0x29c, 2, 0x0, 0), /* MX35_PAD_ATA_DATA8__UART1_RI */
-	[753] = IMX_PIN_REG(MX35_PAD_ATA_DATA8, 0x700, 0x29c, 3, 0x7c0, 0), /* MX35_PAD_ATA_DATA8__AUDMUX_AUD6_TXC */
-	[754] = IMX_PIN_REG(MX35_PAD_ATA_DATA8, 0x700, 0x29c, 5, 0x89c, 1), /* MX35_PAD_ATA_DATA8__GPIO2_21 */
-	[755] = IMX_PIN_REG(MX35_PAD_ATA_DATA8, 0x700, 0x29c, 6, 0x0, 0), /* MX35_PAD_ATA_DATA8__IPU_DIAGB_15 */
-	[756] = IMX_PIN_REG(MX35_PAD_ATA_DATA9, 0x704, 0x2a0, 0, 0x0, 0), /* MX35_PAD_ATA_DATA9__ATA_DATA_9 */
-	[757] = IMX_PIN_REG(MX35_PAD_ATA_DATA9, 0x704, 0x2a0, 1, 0x0, 0), /* MX35_PAD_ATA_DATA9__UART3_CTS */
-	[758] = IMX_PIN_REG(MX35_PAD_ATA_DATA9, 0x704, 0x2a0, 2, 0x0, 0), /* MX35_PAD_ATA_DATA9__UART1_DCD */
-	[759] = IMX_PIN_REG(MX35_PAD_ATA_DATA9, 0x704, 0x2a0, 3, 0x7c4, 0), /* MX35_PAD_ATA_DATA9__AUDMUX_AUD6_TXFS */
-	[760] = IMX_PIN_REG(MX35_PAD_ATA_DATA9, 0x704, 0x2a0, 5, 0x8a0, 1), /* MX35_PAD_ATA_DATA9__GPIO2_22 */
-	[761] = IMX_PIN_REG(MX35_PAD_ATA_DATA9, 0x704, 0x2a0, 6, 0x0, 0), /* MX35_PAD_ATA_DATA9__IPU_DIAGB_16 */
-	[762] = IMX_PIN_REG(MX35_PAD_ATA_DATA10, 0x708, 0x2a4, 0, 0x0, 0), /* MX35_PAD_ATA_DATA10__ATA_DATA_10 */
-	[763] = IMX_PIN_REG(MX35_PAD_ATA_DATA10, 0x708, 0x2a4, 1, 0x9a0, 2), /* MX35_PAD_ATA_DATA10__UART3_RXD_MUX */
-	[764] = IMX_PIN_REG(MX35_PAD_ATA_DATA10, 0x708, 0x2a4, 3, 0x7b8, 0), /* MX35_PAD_ATA_DATA10__AUDMUX_AUD6_RXC */
-	[765] = IMX_PIN_REG(MX35_PAD_ATA_DATA10, 0x708, 0x2a4, 5, 0x8a4, 1), /* MX35_PAD_ATA_DATA10__GPIO2_23 */
-	[766] = IMX_PIN_REG(MX35_PAD_ATA_DATA10, 0x708, 0x2a4, 6, 0x0, 0), /* MX35_PAD_ATA_DATA10__IPU_DIAGB_17 */
-	[767] = IMX_PIN_REG(MX35_PAD_ATA_DATA11, 0x70c, 0x2a8, 0, 0x0, 0), /* MX35_PAD_ATA_DATA11__ATA_DATA_11 */
-	[768] = IMX_PIN_REG(MX35_PAD_ATA_DATA11, 0x70c, 0x2a8, 1, 0x0, 0), /* MX35_PAD_ATA_DATA11__UART3_TXD_MUX */
-	[769] = IMX_PIN_REG(MX35_PAD_ATA_DATA11, 0x70c, 0x2a8, 3, 0x7bc, 0), /* MX35_PAD_ATA_DATA11__AUDMUX_AUD6_RXFS */
-	[770] = IMX_PIN_REG(MX35_PAD_ATA_DATA11, 0x70c, 0x2a8, 5, 0x8a8, 1), /* MX35_PAD_ATA_DATA11__GPIO2_24 */
-	[771] = IMX_PIN_REG(MX35_PAD_ATA_DATA11, 0x70c, 0x2a8, 6, 0x0, 0), /* MX35_PAD_ATA_DATA11__IPU_DIAGB_18 */
-	[772] = IMX_PIN_REG(MX35_PAD_ATA_DATA12, 0x710, 0x2ac, 0, 0x0, 0), /* MX35_PAD_ATA_DATA12__ATA_DATA_12 */
-	[773] = IMX_PIN_REG(MX35_PAD_ATA_DATA12, 0x710, 0x2ac, 1, 0x91c, 3), /* MX35_PAD_ATA_DATA12__I2C3_SCL */
-	[774] = IMX_PIN_REG(MX35_PAD_ATA_DATA12, 0x710, 0x2ac, 5, 0x8ac, 1), /* MX35_PAD_ATA_DATA12__GPIO2_25 */
-	[775] = IMX_PIN_REG(MX35_PAD_ATA_DATA12, 0x710, 0x2ac, 6, 0x0, 0), /* MX35_PAD_ATA_DATA12__IPU_DIAGB_19 */
-	[776] = IMX_PIN_REG(MX35_PAD_ATA_DATA13, 0x714, 0x2b0, 0, 0x0, 0), /* MX35_PAD_ATA_DATA13__ATA_DATA_13 */
-	[777] = IMX_PIN_REG(MX35_PAD_ATA_DATA13, 0x714, 0x2b0, 1, 0x920, 3), /* MX35_PAD_ATA_DATA13__I2C3_SDA */
-	[778] = IMX_PIN_REG(MX35_PAD_ATA_DATA13, 0x714, 0x2b0, 5, 0x8b0, 1), /* MX35_PAD_ATA_DATA13__GPIO2_26 */
-	[779] = IMX_PIN_REG(MX35_PAD_ATA_DATA13, 0x714, 0x2b0, 6, 0x0, 0), /* MX35_PAD_ATA_DATA13__IPU_DIAGB_20 */
-	[780] = IMX_PIN_REG(MX35_PAD_ATA_DATA14, 0x718, 0x2b4, 0, 0x0, 0), /* MX35_PAD_ATA_DATA14__ATA_DATA_14 */
-	[781] = IMX_PIN_REG(MX35_PAD_ATA_DATA14, 0x718, 0x2b4, 1, 0x930, 2), /* MX35_PAD_ATA_DATA14__IPU_CSI_D_0 */
-	[782] = IMX_PIN_REG(MX35_PAD_ATA_DATA14, 0x718, 0x2b4, 3, 0x970, 2), /* MX35_PAD_ATA_DATA14__KPP_ROW_0 */
-	[783] = IMX_PIN_REG(MX35_PAD_ATA_DATA14, 0x718, 0x2b4, 5, 0x8b4, 1), /* MX35_PAD_ATA_DATA14__GPIO2_27 */
-	[784] = IMX_PIN_REG(MX35_PAD_ATA_DATA14, 0x718, 0x2b4, 6, 0x0, 0), /* MX35_PAD_ATA_DATA14__IPU_DIAGB_21 */
-	[785] = IMX_PIN_REG(MX35_PAD_ATA_DATA15, 0x71c, 0x2b8, 0, 0x0, 0), /* MX35_PAD_ATA_DATA15__ATA_DATA_15 */
-	[786] = IMX_PIN_REG(MX35_PAD_ATA_DATA15, 0x71c, 0x2b8, 1, 0x934, 2), /* MX35_PAD_ATA_DATA15__IPU_CSI_D_1 */
-	[787] = IMX_PIN_REG(MX35_PAD_ATA_DATA15, 0x71c, 0x2b8, 3, 0x974, 2), /* MX35_PAD_ATA_DATA15__KPP_ROW_1 */
-	[788] = IMX_PIN_REG(MX35_PAD_ATA_DATA15, 0x71c, 0x2b8, 5, 0x8b8, 1), /* MX35_PAD_ATA_DATA15__GPIO2_28 */
-	[789] = IMX_PIN_REG(MX35_PAD_ATA_DATA15, 0x71c, 0x2b8, 6, 0x0, 0), /* MX35_PAD_ATA_DATA15__IPU_DIAGB_22 */
-	[790] = IMX_PIN_REG(MX35_PAD_ATA_INTRQ, 0x720, 0x2bc, 0, 0x0, 0), /* MX35_PAD_ATA_INTRQ__ATA_INTRQ */
-	[791] = IMX_PIN_REG(MX35_PAD_ATA_INTRQ, 0x720, 0x2bc, 1, 0x938, 3), /* MX35_PAD_ATA_INTRQ__IPU_CSI_D_2 */
-	[792] = IMX_PIN_REG(MX35_PAD_ATA_INTRQ, 0x720, 0x2bc, 3, 0x978, 2), /* MX35_PAD_ATA_INTRQ__KPP_ROW_2 */
-	[793] = IMX_PIN_REG(MX35_PAD_ATA_INTRQ, 0x720, 0x2bc, 5, 0x8bc, 1), /* MX35_PAD_ATA_INTRQ__GPIO2_29 */
-	[794] = IMX_PIN_REG(MX35_PAD_ATA_INTRQ, 0x720, 0x2bc, 6, 0x0, 0), /* MX35_PAD_ATA_INTRQ__IPU_DIAGB_23 */
-	[795] = IMX_PIN_REG(MX35_PAD_ATA_BUFF_EN, 0x724, 0x2c0, 0, 0x0, 0), /* MX35_PAD_ATA_BUFF_EN__ATA_BUFFER_EN */
-	[796] = IMX_PIN_REG(MX35_PAD_ATA_BUFF_EN, 0x724, 0x2c0, 1, 0x93c, 3), /* MX35_PAD_ATA_BUFF_EN__IPU_CSI_D_3 */
-	[797] = IMX_PIN_REG(MX35_PAD_ATA_BUFF_EN, 0x724, 0x2c0, 3, 0x97c, 2), /* MX35_PAD_ATA_BUFF_EN__KPP_ROW_3 */
-	[798] = IMX_PIN_REG(MX35_PAD_ATA_BUFF_EN, 0x724, 0x2c0, 5, 0x8c4, 1), /* MX35_PAD_ATA_BUFF_EN__GPIO2_30 */
-	[799] = IMX_PIN_REG(MX35_PAD_ATA_BUFF_EN, 0x724, 0x2c0, 6, 0x0, 0), /* MX35_PAD_ATA_BUFF_EN__IPU_DIAGB_24 */
-	[800] = IMX_PIN_REG(MX35_PAD_ATA_DMARQ, 0x728, 0x2c4, 0, 0x0, 0), /* MX35_PAD_ATA_DMARQ__ATA_DMARQ */
-	[801] = IMX_PIN_REG(MX35_PAD_ATA_DMARQ, 0x728, 0x2c4, 1, 0x940, 2), /* MX35_PAD_ATA_DMARQ__IPU_CSI_D_4 */
-	[802] = IMX_PIN_REG(MX35_PAD_ATA_DMARQ, 0x728, 0x2c4, 3, 0x950, 2), /* MX35_PAD_ATA_DMARQ__KPP_COL_0 */
-	[803] = IMX_PIN_REG(MX35_PAD_ATA_DMARQ, 0x728, 0x2c4, 5, 0x8c8, 1), /* MX35_PAD_ATA_DMARQ__GPIO2_31 */
-	[804] = IMX_PIN_REG(MX35_PAD_ATA_DMARQ, 0x728, 0x2c4, 6, 0x0, 0), /* MX35_PAD_ATA_DMARQ__IPU_DIAGB_25 */
-	[805] = IMX_PIN_REG(MX35_PAD_ATA_DMARQ, 0x728, 0x2c4, 7, 0x0, 0), /* MX35_PAD_ATA_DMARQ__ECT_CTI_TRIG_IN1_4 */
-	[806] = IMX_PIN_REG(MX35_PAD_ATA_DA0, 0x72c, 0x2c8, 0, 0x0, 0), /* MX35_PAD_ATA_DA0__ATA_DA_0 */
-	[807] = IMX_PIN_REG(MX35_PAD_ATA_DA0, 0x72c, 0x2c8, 1, 0x944, 2), /* MX35_PAD_ATA_DA0__IPU_CSI_D_5 */
-	[808] = IMX_PIN_REG(MX35_PAD_ATA_DA0, 0x72c, 0x2c8, 3, 0x954, 2), /* MX35_PAD_ATA_DA0__KPP_COL_1 */
-	[809] = IMX_PIN_REG(MX35_PAD_ATA_DA0, 0x72c, 0x2c8, 5, 0x8e8, 1), /* MX35_PAD_ATA_DA0__GPIO3_0 */
-	[810] = IMX_PIN_REG(MX35_PAD_ATA_DA0, 0x72c, 0x2c8, 6, 0x0, 0), /* MX35_PAD_ATA_DA0__IPU_DIAGB_26 */
-	[811] = IMX_PIN_REG(MX35_PAD_ATA_DA0, 0x72c, 0x2c8, 7, 0x0, 0), /* MX35_PAD_ATA_DA0__ECT_CTI_TRIG_IN1_5 */
-	[812] = IMX_PIN_REG(MX35_PAD_ATA_DA1, 0x730, 0x2cc, 0, 0x0, 0), /* MX35_PAD_ATA_DA1__ATA_DA_1 */
-	[813] = IMX_PIN_REG(MX35_PAD_ATA_DA1, 0x730, 0x2cc, 1, 0x948, 2), /* MX35_PAD_ATA_DA1__IPU_CSI_D_6 */
-	[814] = IMX_PIN_REG(MX35_PAD_ATA_DA1, 0x730, 0x2cc, 3, 0x958, 2), /* MX35_PAD_ATA_DA1__KPP_COL_2 */
-	[815] = IMX_PIN_REG(MX35_PAD_ATA_DA1, 0x730, 0x2cc, 5, 0x0, 0), /* MX35_PAD_ATA_DA1__GPIO3_1 */
-	[816] = IMX_PIN_REG(MX35_PAD_ATA_DA1, 0x730, 0x2cc, 6, 0x0, 0), /* MX35_PAD_ATA_DA1__IPU_DIAGB_27 */
-	[817] = IMX_PIN_REG(MX35_PAD_ATA_DA1, 0x730, 0x2cc, 7, 0x0, 0), /* MX35_PAD_ATA_DA1__ECT_CTI_TRIG_IN1_6 */
-	[818] = IMX_PIN_REG(MX35_PAD_ATA_DA2, 0x734, 0x2d0, 0, 0x0, 0), /* MX35_PAD_ATA_DA2__ATA_DA_2 */
-	[819] = IMX_PIN_REG(MX35_PAD_ATA_DA2, 0x734, 0x2d0, 1, 0x94c, 2), /* MX35_PAD_ATA_DA2__IPU_CSI_D_7 */
-	[820] = IMX_PIN_REG(MX35_PAD_ATA_DA2, 0x734, 0x2d0, 3, 0x95c, 2), /* MX35_PAD_ATA_DA2__KPP_COL_3 */
-	[821] = IMX_PIN_REG(MX35_PAD_ATA_DA2, 0x734, 0x2d0, 5, 0x0, 0), /* MX35_PAD_ATA_DA2__GPIO3_2 */
-	[822] = IMX_PIN_REG(MX35_PAD_ATA_DA2, 0x734, 0x2d0, 6, 0x0, 0), /* MX35_PAD_ATA_DA2__IPU_DIAGB_28 */
-	[823] = IMX_PIN_REG(MX35_PAD_ATA_DA2, 0x734, 0x2d0, 7, 0x0, 0), /* MX35_PAD_ATA_DA2__ECT_CTI_TRIG_IN1_7 */
-	[824] = IMX_PIN_REG(MX35_PAD_MLB_CLK, 0x738, 0x2d4, 0, 0x0, 0), /* MX35_PAD_MLB_CLK__MLB_MLBCLK */
-	[825] = IMX_PIN_REG(MX35_PAD_MLB_CLK, 0x738, 0x2d4, 5, 0x0, 0), /* MX35_PAD_MLB_CLK__GPIO3_3 */
-	[826] = IMX_PIN_REG(MX35_PAD_MLB_DAT, 0x73c, 0x2d8, 0, 0x0, 0), /* MX35_PAD_MLB_DAT__MLB_MLBDAT */
-	[827] = IMX_PIN_REG(MX35_PAD_MLB_DAT, 0x73c, 0x2d8, 5, 0x904, 1), /* MX35_PAD_MLB_DAT__GPIO3_4 */
-	[828] = IMX_PIN_REG(MX35_PAD_MLB_SIG, 0x740, 0x2dc, 0, 0x0, 0), /* MX35_PAD_MLB_SIG__MLB_MLBSIG */
-	[829] = IMX_PIN_REG(MX35_PAD_MLB_SIG, 0x740, 0x2dc, 5, 0x908, 1), /* MX35_PAD_MLB_SIG__GPIO3_5 */
-	[830] = IMX_PIN_REG(MX35_PAD_FEC_TX_CLK, 0x744, 0x2e0, 0, 0x0, 0), /* MX35_PAD_FEC_TX_CLK__FEC_TX_CLK */
-	[831] = IMX_PIN_REG(MX35_PAD_FEC_TX_CLK, 0x744, 0x2e0, 1, 0x804, 1), /* MX35_PAD_FEC_TX_CLK__ESDHC1_DAT4 */
-	[832] = IMX_PIN_REG(MX35_PAD_FEC_TX_CLK, 0x744, 0x2e0, 2, 0x9a0, 3), /* MX35_PAD_FEC_TX_CLK__UART3_RXD_MUX */
-	[833] = IMX_PIN_REG(MX35_PAD_FEC_TX_CLK, 0x744, 0x2e0, 3, 0x9ec, 1), /* MX35_PAD_FEC_TX_CLK__USB_TOP_USBH2_DIR */
-	[834] = IMX_PIN_REG(MX35_PAD_FEC_TX_CLK, 0x744, 0x2e0, 4, 0x7ec, 3), /* MX35_PAD_FEC_TX_CLK__CSPI2_MOSI */
-	[835] = IMX_PIN_REG(MX35_PAD_FEC_TX_CLK, 0x744, 0x2e0, 5, 0x90c, 1), /* MX35_PAD_FEC_TX_CLK__GPIO3_6 */
-	[836] = IMX_PIN_REG(MX35_PAD_FEC_TX_CLK, 0x744, 0x2e0, 6, 0x928, 5), /* MX35_PAD_FEC_TX_CLK__IPU_DISPB_D12_VSYNC */
-	[837] = IMX_PIN_REG(MX35_PAD_FEC_TX_CLK, 0x744, 0x2e0, 7, 0x0, 0), /* MX35_PAD_FEC_TX_CLK__ARM11P_TOP_EVNTBUS_0 */
-	[838] = IMX_PIN_REG(MX35_PAD_FEC_RX_CLK, 0x748, 0x2e4, 0, 0x0, 0), /* MX35_PAD_FEC_RX_CLK__FEC_RX_CLK */
-	[839] = IMX_PIN_REG(MX35_PAD_FEC_RX_CLK, 0x748, 0x2e4, 1, 0x808, 1), /* MX35_PAD_FEC_RX_CLK__ESDHC1_DAT5 */
-	[840] = IMX_PIN_REG(MX35_PAD_FEC_RX_CLK, 0x748, 0x2e4, 2, 0x0, 0), /* MX35_PAD_FEC_RX_CLK__UART3_TXD_MUX */
-	[841] = IMX_PIN_REG(MX35_PAD_FEC_RX_CLK, 0x748, 0x2e4, 3, 0x0, 0), /* MX35_PAD_FEC_RX_CLK__USB_TOP_USBH2_STP */
-	[842] = IMX_PIN_REG(MX35_PAD_FEC_RX_CLK, 0x748, 0x2e4, 4, 0x7e8, 3), /* MX35_PAD_FEC_RX_CLK__CSPI2_MISO */
-	[843] = IMX_PIN_REG(MX35_PAD_FEC_RX_CLK, 0x748, 0x2e4, 5, 0x910, 1), /* MX35_PAD_FEC_RX_CLK__GPIO3_7 */
-	[844] = IMX_PIN_REG(MX35_PAD_FEC_RX_CLK, 0x748, 0x2e4, 6, 0x92c, 4), /* MX35_PAD_FEC_RX_CLK__IPU_DISPB_SD_D_I */
-	[845] = IMX_PIN_REG(MX35_PAD_FEC_RX_CLK, 0x748, 0x2e4, 7, 0x0, 0), /* MX35_PAD_FEC_RX_CLK__ARM11P_TOP_EVNTBUS_1 */
-	[846] = IMX_PIN_REG(MX35_PAD_FEC_RX_DV, 0x74c, 0x2e8, 0, 0x0, 0), /* MX35_PAD_FEC_RX_DV__FEC_RX_DV */
-	[847] = IMX_PIN_REG(MX35_PAD_FEC_RX_DV, 0x74c, 0x2e8, 1, 0x80c, 1), /* MX35_PAD_FEC_RX_DV__ESDHC1_DAT6 */
-	[848] = IMX_PIN_REG(MX35_PAD_FEC_RX_DV, 0x74c, 0x2e8, 2, 0x99c, 2), /* MX35_PAD_FEC_RX_DV__UART3_RTS */
-	[849] = IMX_PIN_REG(MX35_PAD_FEC_RX_DV, 0x74c, 0x2e8, 3, 0x9f0, 1), /* MX35_PAD_FEC_RX_DV__USB_TOP_USBH2_NXT */
-	[850] = IMX_PIN_REG(MX35_PAD_FEC_RX_DV, 0x74c, 0x2e8, 4, 0x7e0, 3), /* MX35_PAD_FEC_RX_DV__CSPI2_SCLK */
-	[851] = IMX_PIN_REG(MX35_PAD_FEC_RX_DV, 0x74c, 0x2e8, 5, 0x914, 1), /* MX35_PAD_FEC_RX_DV__GPIO3_8 */
-	[852] = IMX_PIN_REG(MX35_PAD_FEC_RX_DV, 0x74c, 0x2e8, 6, 0x0, 0), /* MX35_PAD_FEC_RX_DV__IPU_DISPB_SD_CLK */
-	[853] = IMX_PIN_REG(MX35_PAD_FEC_RX_DV, 0x74c, 0x2e8, 7, 0x0, 0), /* MX35_PAD_FEC_RX_DV__ARM11P_TOP_EVNTBUS_2 */
-	[854] = IMX_PIN_REG(MX35_PAD_FEC_COL, 0x750, 0x2ec, 0, 0x0, 0), /* MX35_PAD_FEC_COL__FEC_COL */
-	[855] = IMX_PIN_REG(MX35_PAD_FEC_COL, 0x750, 0x2ec, 1, 0x810, 1), /* MX35_PAD_FEC_COL__ESDHC1_DAT7 */
-	[856] = IMX_PIN_REG(MX35_PAD_FEC_COL, 0x750, 0x2ec, 2, 0x0, 0), /* MX35_PAD_FEC_COL__UART3_CTS */
-	[857] = IMX_PIN_REG(MX35_PAD_FEC_COL, 0x750, 0x2ec, 3, 0x9cc, 1), /* MX35_PAD_FEC_COL__USB_TOP_USBH2_DATA_0 */
-	[858] = IMX_PIN_REG(MX35_PAD_FEC_COL, 0x750, 0x2ec, 4, 0x7e4, 3), /* MX35_PAD_FEC_COL__CSPI2_RDY */
-	[859] = IMX_PIN_REG(MX35_PAD_FEC_COL, 0x750, 0x2ec, 5, 0x918, 1), /* MX35_PAD_FEC_COL__GPIO3_9 */
-	[860] = IMX_PIN_REG(MX35_PAD_FEC_COL, 0x750, 0x2ec, 6, 0x0, 0), /* MX35_PAD_FEC_COL__IPU_DISPB_SER_RS */
-	[861] = IMX_PIN_REG(MX35_PAD_FEC_COL, 0x750, 0x2ec, 7, 0x0, 0), /* MX35_PAD_FEC_COL__ARM11P_TOP_EVNTBUS_3 */
-	[862] = IMX_PIN_REG(MX35_PAD_FEC_RDATA0, 0x754, 0x2f0, 0, 0x0, 0), /* MX35_PAD_FEC_RDATA0__FEC_RDATA_0 */
-	[863] = IMX_PIN_REG(MX35_PAD_FEC_RDATA0, 0x754, 0x2f0, 1, 0x0, 0), /* MX35_PAD_FEC_RDATA0__PWM_PWMO */
-	[864] = IMX_PIN_REG(MX35_PAD_FEC_RDATA0, 0x754, 0x2f0, 2, 0x0, 0), /* MX35_PAD_FEC_RDATA0__UART3_DTR */
-	[865] = IMX_PIN_REG(MX35_PAD_FEC_RDATA0, 0x754, 0x2f0, 3, 0x9d0, 1), /* MX35_PAD_FEC_RDATA0__USB_TOP_USBH2_DATA_1 */
-	[866] = IMX_PIN_REG(MX35_PAD_FEC_RDATA0, 0x754, 0x2f0, 4, 0x7f0, 2), /* MX35_PAD_FEC_RDATA0__CSPI2_SS0 */
-	[867] = IMX_PIN_REG(MX35_PAD_FEC_RDATA0, 0x754, 0x2f0, 5, 0x8ec, 1), /* MX35_PAD_FEC_RDATA0__GPIO3_10 */
-	[868] = IMX_PIN_REG(MX35_PAD_FEC_RDATA0, 0x754, 0x2f0, 6, 0x0, 0), /* MX35_PAD_FEC_RDATA0__IPU_DISPB_CS1 */
-	[869] = IMX_PIN_REG(MX35_PAD_FEC_RDATA0, 0x754, 0x2f0, 7, 0x0, 0), /* MX35_PAD_FEC_RDATA0__ARM11P_TOP_EVNTBUS_4 */
-	[870] = IMX_PIN_REG(MX35_PAD_FEC_TDATA0, 0x758, 0x2f4, 0, 0x0, 0), /* MX35_PAD_FEC_TDATA0__FEC_TDATA_0 */
-	[871] = IMX_PIN_REG(MX35_PAD_FEC_TDATA0, 0x758, 0x2f4, 1, 0x0, 0), /* MX35_PAD_FEC_TDATA0__SPDIF_SPDIF_OUT1 */
-	[872] = IMX_PIN_REG(MX35_PAD_FEC_TDATA0, 0x758, 0x2f4, 2, 0x0, 0), /* MX35_PAD_FEC_TDATA0__UART3_DSR */
-	[873] = IMX_PIN_REG(MX35_PAD_FEC_TDATA0, 0x758, 0x2f4, 3, 0x9d4, 1), /* MX35_PAD_FEC_TDATA0__USB_TOP_USBH2_DATA_2 */
-	[874] = IMX_PIN_REG(MX35_PAD_FEC_TDATA0, 0x758, 0x2f4, 4, 0x7f4, 2), /* MX35_PAD_FEC_TDATA0__CSPI2_SS1 */
-	[875] = IMX_PIN_REG(MX35_PAD_FEC_TDATA0, 0x758, 0x2f4, 5, 0x8f0, 1), /* MX35_PAD_FEC_TDATA0__GPIO3_11 */
-	[876] = IMX_PIN_REG(MX35_PAD_FEC_TDATA0, 0x758, 0x2f4, 6, 0x0, 0), /* MX35_PAD_FEC_TDATA0__IPU_DISPB_CS0 */
-	[877] = IMX_PIN_REG(MX35_PAD_FEC_TDATA0, 0x758, 0x2f4, 7, 0x0, 0), /* MX35_PAD_FEC_TDATA0__ARM11P_TOP_EVNTBUS_5 */
-	[878] = IMX_PIN_REG(MX35_PAD_FEC_TX_EN, 0x75c, 0x2f8, 0, 0x0, 0), /* MX35_PAD_FEC_TX_EN__FEC_TX_EN */
-	[879] = IMX_PIN_REG(MX35_PAD_FEC_TX_EN, 0x75c, 0x2f8, 1, 0x998, 3), /* MX35_PAD_FEC_TX_EN__SPDIF_SPDIF_IN1 */
-	[880] = IMX_PIN_REG(MX35_PAD_FEC_TX_EN, 0x75c, 0x2f8, 2, 0x0, 0), /* MX35_PAD_FEC_TX_EN__UART3_RI */
-	[881] = IMX_PIN_REG(MX35_PAD_FEC_TX_EN, 0x75c, 0x2f8, 3, 0x9d8, 1), /* MX35_PAD_FEC_TX_EN__USB_TOP_USBH2_DATA_3 */
-	[882] = IMX_PIN_REG(MX35_PAD_FEC_TX_EN, 0x75c, 0x2f8, 5, 0x8f4, 1), /* MX35_PAD_FEC_TX_EN__GPIO3_12 */
-	[883] = IMX_PIN_REG(MX35_PAD_FEC_TX_EN, 0x75c, 0x2f8, 6, 0x0, 0), /* MX35_PAD_FEC_TX_EN__IPU_DISPB_PAR_RS */
-	[884] = IMX_PIN_REG(MX35_PAD_FEC_TX_EN, 0x75c, 0x2f8, 7, 0x0, 0), /* MX35_PAD_FEC_TX_EN__ARM11P_TOP_EVNTBUS_6 */
-	[885] = IMX_PIN_REG(MX35_PAD_FEC_MDC, 0x760, 0x2fc, 0, 0x0, 0), /* MX35_PAD_FEC_MDC__FEC_MDC */
-	[886] = IMX_PIN_REG(MX35_PAD_FEC_MDC, 0x760, 0x2fc, 1, 0x0, 0), /* MX35_PAD_FEC_MDC__CAN2_TXCAN */
-	[887] = IMX_PIN_REG(MX35_PAD_FEC_MDC, 0x760, 0x2fc, 2, 0x0, 0), /* MX35_PAD_FEC_MDC__UART3_DCD */
-	[888] = IMX_PIN_REG(MX35_PAD_FEC_MDC, 0x760, 0x2fc, 3, 0x9dc, 1), /* MX35_PAD_FEC_MDC__USB_TOP_USBH2_DATA_4 */
-	[889] = IMX_PIN_REG(MX35_PAD_FEC_MDC, 0x760, 0x2fc, 5, 0x8f8, 1), /* MX35_PAD_FEC_MDC__GPIO3_13 */
-	[890] = IMX_PIN_REG(MX35_PAD_FEC_MDC, 0x760, 0x2fc, 6, 0x0, 0), /* MX35_PAD_FEC_MDC__IPU_DISPB_WR */
-	[891] = IMX_PIN_REG(MX35_PAD_FEC_MDC, 0x760, 0x2fc, 7, 0x0, 0), /* MX35_PAD_FEC_MDC__ARM11P_TOP_EVNTBUS_7 */
-	[892] = IMX_PIN_REG(MX35_PAD_FEC_MDIO, 0x764, 0x300, 0, 0x0, 0), /* MX35_PAD_FEC_MDIO__FEC_MDIO */
-	[893] = IMX_PIN_REG(MX35_PAD_FEC_MDIO, 0x764, 0x300, 1, 0x7cc, 2), /* MX35_PAD_FEC_MDIO__CAN2_RXCAN */
-	[894] = IMX_PIN_REG(MX35_PAD_FEC_MDIO, 0x764, 0x300, 3, 0x9e0, 1), /* MX35_PAD_FEC_MDIO__USB_TOP_USBH2_DATA_5 */
-	[895] = IMX_PIN_REG(MX35_PAD_FEC_MDIO, 0x764, 0x300, 5, 0x8fc, 1), /* MX35_PAD_FEC_MDIO__GPIO3_14 */
-	[896] = IMX_PIN_REG(MX35_PAD_FEC_MDIO, 0x764, 0x300, 6, 0x0, 0), /* MX35_PAD_FEC_MDIO__IPU_DISPB_RD */
-	[897] = IMX_PIN_REG(MX35_PAD_FEC_MDIO, 0x764, 0x300, 7, 0x0, 0), /* MX35_PAD_FEC_MDIO__ARM11P_TOP_EVNTBUS_8 */
-	[898] = IMX_PIN_REG(MX35_PAD_FEC_TX_ERR, 0x768, 0x304, 0, 0x0, 0), /* MX35_PAD_FEC_TX_ERR__FEC_TX_ERR */
-	[899] = IMX_PIN_REG(MX35_PAD_FEC_TX_ERR, 0x768, 0x304, 1, 0x990, 2), /* MX35_PAD_FEC_TX_ERR__OWIRE_LINE */
-	[900] = IMX_PIN_REG(MX35_PAD_FEC_TX_ERR, 0x768, 0x304, 2, 0x994, 4), /* MX35_PAD_FEC_TX_ERR__SPDIF_SPDIF_EXTCLK */
-	[901] = IMX_PIN_REG(MX35_PAD_FEC_TX_ERR, 0x768, 0x304, 3, 0x9e4, 1), /* MX35_PAD_FEC_TX_ERR__USB_TOP_USBH2_DATA_6 */
-	[902] = IMX_PIN_REG(MX35_PAD_FEC_TX_ERR, 0x768, 0x304, 5, 0x900, 1), /* MX35_PAD_FEC_TX_ERR__GPIO3_15 */
-	[903] = IMX_PIN_REG(MX35_PAD_FEC_TX_ERR, 0x768, 0x304, 6, 0x924, 3), /* MX35_PAD_FEC_TX_ERR__IPU_DISPB_D0_VSYNC */
-	[904] = IMX_PIN_REG(MX35_PAD_FEC_TX_ERR, 0x768, 0x304, 7, 0x0, 0), /* MX35_PAD_FEC_TX_ERR__ARM11P_TOP_EVNTBUS_9 */
-	[905] = IMX_PIN_REG(MX35_PAD_FEC_RX_ERR, 0x76c, 0x308, 0, 0x0, 0), /* MX35_PAD_FEC_RX_ERR__FEC_RX_ERR */
-	[906] = IMX_PIN_REG(MX35_PAD_FEC_RX_ERR, 0x76c, 0x308, 1, 0x930, 3), /* MX35_PAD_FEC_RX_ERR__IPU_CSI_D_0 */
-	[907] = IMX_PIN_REG(MX35_PAD_FEC_RX_ERR, 0x76c, 0x308, 3, 0x9e8, 1), /* MX35_PAD_FEC_RX_ERR__USB_TOP_USBH2_DATA_7 */
-	[908] = IMX_PIN_REG(MX35_PAD_FEC_RX_ERR, 0x76c, 0x308, 4, 0x960, 1), /* MX35_PAD_FEC_RX_ERR__KPP_COL_4 */
-	[909] = IMX_PIN_REG(MX35_PAD_FEC_RX_ERR, 0x76c, 0x308, 5, 0x0, 0), /* MX35_PAD_FEC_RX_ERR__GPIO3_16 */
-	[910] = IMX_PIN_REG(MX35_PAD_FEC_RX_ERR, 0x76c, 0x308, 6, 0x92c, 5), /* MX35_PAD_FEC_RX_ERR__IPU_DISPB_SD_D_IO */
-	[911] = IMX_PIN_REG(MX35_PAD_FEC_CRS, 0x770, 0x30c, 0, 0x0, 0), /* MX35_PAD_FEC_CRS__FEC_CRS */
-	[912] = IMX_PIN_REG(MX35_PAD_FEC_CRS, 0x770, 0x30c, 1, 0x934, 3), /* MX35_PAD_FEC_CRS__IPU_CSI_D_1 */
-	[913] = IMX_PIN_REG(MX35_PAD_FEC_CRS, 0x770, 0x30c, 3, 0x0, 0), /* MX35_PAD_FEC_CRS__USB_TOP_USBH2_PWR */
-	[914] = IMX_PIN_REG(MX35_PAD_FEC_CRS, 0x770, 0x30c, 4, 0x964, 1), /* MX35_PAD_FEC_CRS__KPP_COL_5 */
-	[915] = IMX_PIN_REG(MX35_PAD_FEC_CRS, 0x770, 0x30c, 5, 0x0, 0), /* MX35_PAD_FEC_CRS__GPIO3_17 */
-	[916] = IMX_PIN_REG(MX35_PAD_FEC_CRS, 0x770, 0x30c, 6, 0x0, 0), /* MX35_PAD_FEC_CRS__IPU_FLASH_STROBE */
-	[917] = IMX_PIN_REG(MX35_PAD_FEC_RDATA1, 0x774, 0x310, 0, 0x0, 0), /* MX35_PAD_FEC_RDATA1__FEC_RDATA_1 */
-	[918] = IMX_PIN_REG(MX35_PAD_FEC_RDATA1, 0x774, 0x310, 1, 0x938, 4), /* MX35_PAD_FEC_RDATA1__IPU_CSI_D_2 */
-	[919] = IMX_PIN_REG(MX35_PAD_FEC_RDATA1, 0x774, 0x310, 2, 0x0, 0), /* MX35_PAD_FEC_RDATA1__AUDMUX_AUD6_RXC */
-	[920] = IMX_PIN_REG(MX35_PAD_FEC_RDATA1, 0x774, 0x310, 3, 0x9f4, 2), /* MX35_PAD_FEC_RDATA1__USB_TOP_USBH2_OC */
-	[921] = IMX_PIN_REG(MX35_PAD_FEC_RDATA1, 0x774, 0x310, 4, 0x968, 1), /* MX35_PAD_FEC_RDATA1__KPP_COL_6 */
-	[922] = IMX_PIN_REG(MX35_PAD_FEC_RDATA1, 0x774, 0x310, 5, 0x0, 0), /* MX35_PAD_FEC_RDATA1__GPIO3_18 */
-	[923] = IMX_PIN_REG(MX35_PAD_FEC_RDATA1, 0x774, 0x310, 6, 0x0, 0), /* MX35_PAD_FEC_RDATA1__IPU_DISPB_BE0 */
-	[924] = IMX_PIN_REG(MX35_PAD_FEC_TDATA1, 0x778, 0x314, 0, 0x0, 0), /* MX35_PAD_FEC_TDATA1__FEC_TDATA_1 */
-	[925] = IMX_PIN_REG(MX35_PAD_FEC_TDATA1, 0x778, 0x314, 1, 0x93c, 4), /* MX35_PAD_FEC_TDATA1__IPU_CSI_D_3 */
-	[926] = IMX_PIN_REG(MX35_PAD_FEC_TDATA1, 0x778, 0x314, 2, 0x7bc, 1), /* MX35_PAD_FEC_TDATA1__AUDMUX_AUD6_RXFS */
-	[927] = IMX_PIN_REG(MX35_PAD_FEC_TDATA1, 0x778, 0x314, 4, 0x96c, 1), /* MX35_PAD_FEC_TDATA1__KPP_COL_7 */
-	[928] = IMX_PIN_REG(MX35_PAD_FEC_TDATA1, 0x778, 0x314, 5, 0x0, 0), /* MX35_PAD_FEC_TDATA1__GPIO3_19 */
-	[929] = IMX_PIN_REG(MX35_PAD_FEC_TDATA1, 0x778, 0x314, 6, 0x0, 0), /* MX35_PAD_FEC_TDATA1__IPU_DISPB_BE1 */
-	[930] = IMX_PIN_REG(MX35_PAD_FEC_RDATA2, 0x77c, 0x318, 0, 0x0, 0), /* MX35_PAD_FEC_RDATA2__FEC_RDATA_2 */
-	[931] = IMX_PIN_REG(MX35_PAD_FEC_RDATA2, 0x77c, 0x318, 1, 0x940, 3), /* MX35_PAD_FEC_RDATA2__IPU_CSI_D_4 */
-	[932] = IMX_PIN_REG(MX35_PAD_FEC_RDATA2, 0x77c, 0x318, 2, 0x7b4, 1), /* MX35_PAD_FEC_RDATA2__AUDMUX_AUD6_TXD */
-	[933] = IMX_PIN_REG(MX35_PAD_FEC_RDATA2, 0x77c, 0x318, 4, 0x980, 1), /* MX35_PAD_FEC_RDATA2__KPP_ROW_4 */
-	[934] = IMX_PIN_REG(MX35_PAD_FEC_RDATA2, 0x77c, 0x318, 5, 0x0, 0), /* MX35_PAD_FEC_RDATA2__GPIO3_20 */
-	[935] = IMX_PIN_REG(MX35_PAD_FEC_TDATA2, 0x780, 0x31c, 0, 0x0, 0), /* MX35_PAD_FEC_TDATA2__FEC_TDATA_2 */
-	[936] = IMX_PIN_REG(MX35_PAD_FEC_TDATA2, 0x780, 0x31c, 1, 0x944, 3), /* MX35_PAD_FEC_TDATA2__IPU_CSI_D_5 */
-	[937] = IMX_PIN_REG(MX35_PAD_FEC_TDATA2, 0x780, 0x31c, 2, 0x7b0, 1), /* MX35_PAD_FEC_TDATA2__AUDMUX_AUD6_RXD */
-	[938] = IMX_PIN_REG(MX35_PAD_FEC_TDATA2, 0x780, 0x31c, 4, 0x984, 1), /* MX35_PAD_FEC_TDATA2__KPP_ROW_5 */
-	[939] = IMX_PIN_REG(MX35_PAD_FEC_TDATA2, 0x780, 0x31c, 5, 0x0, 0), /* MX35_PAD_FEC_TDATA2__GPIO3_21 */
-	[940] = IMX_PIN_REG(MX35_PAD_FEC_RDATA3, 0x784, 0x320, 0, 0x0, 0), /* MX35_PAD_FEC_RDATA3__FEC_RDATA_3 */
-	[941] = IMX_PIN_REG(MX35_PAD_FEC_RDATA3, 0x784, 0x320, 1, 0x948, 3), /* MX35_PAD_FEC_RDATA3__IPU_CSI_D_6 */
-	[942] = IMX_PIN_REG(MX35_PAD_FEC_RDATA3, 0x784, 0x320, 2, 0x7c0, 1), /* MX35_PAD_FEC_RDATA3__AUDMUX_AUD6_TXC */
-	[943] = IMX_PIN_REG(MX35_PAD_FEC_RDATA3, 0x784, 0x320, 4, 0x988, 1), /* MX35_PAD_FEC_RDATA3__KPP_ROW_6 */
-	[944] = IMX_PIN_REG(MX35_PAD_FEC_RDATA3, 0x784, 0x320, 6, 0x0, 0), /* MX35_PAD_FEC_RDATA3__GPIO3_22 */
-	[945] = IMX_PIN_REG(MX35_PAD_FEC_TDATA3, 0x788, 0x324, 0, 0x0, 0), /* MX35_PAD_FEC_TDATA3__FEC_TDATA_3 */
-	[946] = IMX_PIN_REG(MX35_PAD_FEC_TDATA3, 0x788, 0x324, 1, 0x94c, 3), /* MX35_PAD_FEC_TDATA3__IPU_CSI_D_7 */
-	[947] = IMX_PIN_REG(MX35_PAD_FEC_TDATA3, 0x788, 0x324, 2, 0x7c4, 1), /* MX35_PAD_FEC_TDATA3__AUDMUX_AUD6_TXFS */
-	[948] = IMX_PIN_REG(MX35_PAD_FEC_TDATA3, 0x788, 0x324, 4, 0x98c, 1), /* MX35_PAD_FEC_TDATA3__KPP_ROW_7 */
-	[949] = IMX_PIN_REG(MX35_PAD_FEC_TDATA3, 0x788, 0x324, 5, 0x0, 0), /* MX35_PAD_FEC_TDATA3__GPIO3_23 */
-	[950] = IMX_PIN_REG(MX35_PAD_EXT_ARMCLK, 0x78c, 0x0, 0, 0x0, 0), /* MX35_PAD_EXT_ARMCLK__CCM_EXT_ARMCLK */
-	[951] = IMX_PIN_REG(MX35_PAD_TEST_MODE, 0x790, 0x0, 0, 0x0, 0), /* MX35_PAD_TEST_MODE__TCU_TEST_MODE */
+	MX35_PAD_RESERVE0 = 0,
+	MX35_PAD_CAPTURE = 1,
+	MX35_PAD_COMPARE = 2,
+	MX35_PAD_WDOG_RST = 3,
+	MX35_PAD_GPIO1_0 = 4,
+	MX35_PAD_GPIO1_1 = 5,
+	MX35_PAD_GPIO2_0 = 6,
+	MX35_PAD_GPIO3_0 = 7,
+	MX35_PAD_CLKO = 8,
+	MX35_PAD_VSTBY = 9,
+	MX35_PAD_A0 = 10,
+	MX35_PAD_A1 = 11,
+	MX35_PAD_A2 = 12,
+	MX35_PAD_A3 = 13,
+	MX35_PAD_A4 = 14,
+	MX35_PAD_A5 = 15,
+	MX35_PAD_A6 = 16,
+	MX35_PAD_A7 = 17,
+	MX35_PAD_A8 = 18,
+	MX35_PAD_A9 = 19,
+	MX35_PAD_A10 = 20,
+	MX35_PAD_MA10 = 21,
+	MX35_PAD_A11 = 22,
+	MX35_PAD_A12 = 23,
+	MX35_PAD_A13 = 24,
+	MX35_PAD_A14 = 25,
+	MX35_PAD_A15 = 26,
+	MX35_PAD_A16 = 27,
+	MX35_PAD_A17 = 28,
+	MX35_PAD_A18 = 29,
+	MX35_PAD_A19 = 30,
+	MX35_PAD_A20 = 31,
+	MX35_PAD_A21 = 32,
+	MX35_PAD_A22 = 33,
+	MX35_PAD_A23 = 34,
+	MX35_PAD_A24 = 35,
+	MX35_PAD_A25 = 36,
+	MX35_PAD_EB0 = 37,
+	MX35_PAD_EB1 = 38,
+	MX35_PAD_OE = 39,
+	MX35_PAD_CS0 = 40,
+	MX35_PAD_CS1 = 41,
+	MX35_PAD_CS2 = 42,
+	MX35_PAD_CS3 = 43,
+	MX35_PAD_CS4 = 44,
+	MX35_PAD_CS5 = 45,
+	MX35_PAD_NF_CE0 = 46,
+	MX35_PAD_LBA = 47,
+	MX35_PAD_BCLK = 48,
+	MX35_PAD_RW = 49,
+	MX35_PAD_NFWE_B = 50,
+	MX35_PAD_NFRE_B = 51,
+	MX35_PAD_NFALE = 52,
+	MX35_PAD_NFCLE = 53,
+	MX35_PAD_NFWP_B = 54,
+	MX35_PAD_NFRB = 55,
+	MX35_PAD_CSI_D8 = 56,
+	MX35_PAD_CSI_D9 = 57,
+	MX35_PAD_CSI_D10 = 58,
+	MX35_PAD_CSI_D11 = 59,
+	MX35_PAD_CSI_D12 = 60,
+	MX35_PAD_CSI_D13 = 61,
+	MX35_PAD_CSI_D14 = 62,
+	MX35_PAD_CSI_D15 = 63,
+	MX35_PAD_CSI_MCLK = 64,
+	MX35_PAD_CSI_VSYNC = 65,
+	MX35_PAD_CSI_HSYNC = 66,
+	MX35_PAD_CSI_PIXCLK = 67,
+	MX35_PAD_I2C1_CLK = 68,
+	MX35_PAD_I2C1_DAT = 69,
+	MX35_PAD_I2C2_CLK = 70,
+	MX35_PAD_I2C2_DAT = 71,
+	MX35_PAD_STXD4 = 72,
+	MX35_PAD_SRXD4 = 73,
+	MX35_PAD_SCK4 = 74,
+	MX35_PAD_STXFS4 = 75,
+	MX35_PAD_STXD5 = 76,
+	MX35_PAD_SRXD5 = 77,
+	MX35_PAD_SCK5 = 78,
+	MX35_PAD_STXFS5 = 79,
+	MX35_PAD_SCKR = 80,
+	MX35_PAD_FSR = 81,
+	MX35_PAD_HCKR = 82,
+	MX35_PAD_SCKT = 83,
+	MX35_PAD_FST = 84,
+	MX35_PAD_HCKT = 85,
+	MX35_PAD_TX5_RX0 = 86,
+	MX35_PAD_TX4_RX1 = 87,
+	MX35_PAD_TX3_RX2 = 88,
+	MX35_PAD_TX2_RX3 = 89,
+	MX35_PAD_TX1 = 90,
+	MX35_PAD_TX0 = 91,
+	MX35_PAD_CSPI1_MOSI = 92,
+	MX35_PAD_CSPI1_MISO = 93,
+	MX35_PAD_CSPI1_SS0 = 94,
+	MX35_PAD_CSPI1_SS1 = 95,
+	MX35_PAD_CSPI1_SCLK = 96,
+	MX35_PAD_CSPI1_SPI_RDY = 97,
+	MX35_PAD_RXD1 = 98,
+	MX35_PAD_TXD1 = 99,
+	MX35_PAD_RTS1 = 100,
+	MX35_PAD_CTS1 = 101,
+	MX35_PAD_RXD2 = 102,
+	MX35_PAD_TXD2 = 103,
+	MX35_PAD_RTS2 = 104,
+	MX35_PAD_CTS2 = 105,
+	MX35_PAD_USBOTG_PWR = 106,
+	MX35_PAD_USBOTG_OC = 107,
+	MX35_PAD_LD0 = 108,
+	MX35_PAD_LD1 = 109,
+	MX35_PAD_LD2 = 110,
+	MX35_PAD_LD3 = 111,
+	MX35_PAD_LD4 = 112,
+	MX35_PAD_LD5 = 113,
+	MX35_PAD_LD6 = 114,
+	MX35_PAD_LD7 = 115,
+	MX35_PAD_LD8 = 116,
+	MX35_PAD_LD9 = 117,
+	MX35_PAD_LD10 = 118,
+	MX35_PAD_LD11 = 119,
+	MX35_PAD_LD12 = 120,
+	MX35_PAD_LD13 = 121,
+	MX35_PAD_LD14 = 122,
+	MX35_PAD_LD15 = 123,
+	MX35_PAD_LD16 = 124,
+	MX35_PAD_LD17 = 125,
+	MX35_PAD_LD18 = 126,
+	MX35_PAD_LD19 = 127,
+	MX35_PAD_LD20 = 128,
+	MX35_PAD_LD21 = 129,
+	MX35_PAD_LD22 = 130,
+	MX35_PAD_LD23 = 131,
+	MX35_PAD_D3_HSYNC = 132,
+	MX35_PAD_D3_FPSHIFT = 133,
+	MX35_PAD_D3_DRDY = 134,
+	MX35_PAD_CONTRAST = 135,
+	MX35_PAD_D3_VSYNC = 136,
+	MX35_PAD_D3_REV = 137,
+	MX35_PAD_D3_CLS = 138,
+	MX35_PAD_D3_SPL = 139,
+	MX35_PAD_SD1_CMD = 140,
+	MX35_PAD_SD1_CLK = 141,
+	MX35_PAD_SD1_DATA0 = 142,
+	MX35_PAD_SD1_DATA1 = 143,
+	MX35_PAD_SD1_DATA2 = 144,
+	MX35_PAD_SD1_DATA3 = 145,
+	MX35_PAD_SD2_CMD = 146,
+	MX35_PAD_SD2_CLK = 147,
+	MX35_PAD_SD2_DATA0 = 148,
+	MX35_PAD_SD2_DATA1 = 149,
+	MX35_PAD_SD2_DATA2 = 150,
+	MX35_PAD_SD2_DATA3 = 151,
+	MX35_PAD_ATA_CS0 = 152,
+	MX35_PAD_ATA_CS1 = 153,
+	MX35_PAD_ATA_DIOR = 154,
+	MX35_PAD_ATA_DIOW = 155,
+	MX35_PAD_ATA_DMACK = 156,
+	MX35_PAD_ATA_RESET_B = 157,
+	MX35_PAD_ATA_IORDY = 158,
+	MX35_PAD_ATA_DATA0 = 159,
+	MX35_PAD_ATA_DATA1 = 160,
+	MX35_PAD_ATA_DATA2 = 161,
+	MX35_PAD_ATA_DATA3 = 162,
+	MX35_PAD_ATA_DATA4 = 163,
+	MX35_PAD_ATA_DATA5 = 164,
+	MX35_PAD_ATA_DATA6 = 165,
+	MX35_PAD_ATA_DATA7 = 166,
+	MX35_PAD_ATA_DATA8 = 167,
+	MX35_PAD_ATA_DATA9 = 168,
+	MX35_PAD_ATA_DATA10 = 169,
+	MX35_PAD_ATA_DATA11 = 170,
+	MX35_PAD_ATA_DATA12 = 171,
+	MX35_PAD_ATA_DATA13 = 172,
+	MX35_PAD_ATA_DATA14 = 173,
+	MX35_PAD_ATA_DATA15 = 174,
+	MX35_PAD_ATA_INTRQ = 175,
+	MX35_PAD_ATA_BUFF_EN = 176,
+	MX35_PAD_ATA_DMARQ = 177,
+	MX35_PAD_ATA_DA0 = 178,
+	MX35_PAD_ATA_DA1 = 179,
+	MX35_PAD_ATA_DA2 = 180,
+	MX35_PAD_MLB_CLK = 181,
+	MX35_PAD_MLB_DAT = 182,
+	MX35_PAD_MLB_SIG = 183,
+	MX35_PAD_FEC_TX_CLK = 184,
+	MX35_PAD_FEC_RX_CLK = 185,
+	MX35_PAD_FEC_RX_DV = 186,
+	MX35_PAD_FEC_COL = 187,
+	MX35_PAD_FEC_RDATA0 = 188,
+	MX35_PAD_FEC_TDATA0 = 189,
+	MX35_PAD_FEC_TX_EN = 190,
+	MX35_PAD_FEC_MDC = 191,
+	MX35_PAD_FEC_MDIO = 192,
+	MX35_PAD_FEC_TX_ERR = 193,
+	MX35_PAD_FEC_RX_ERR = 194,
+	MX35_PAD_FEC_CRS = 195,
+	MX35_PAD_FEC_RDATA1 = 196,
+	MX35_PAD_FEC_TDATA1 = 197,
+	MX35_PAD_FEC_RDATA2 = 198,
+	MX35_PAD_FEC_TDATA2 = 199,
+	MX35_PAD_FEC_RDATA3 = 200,
+	MX35_PAD_FEC_TDATA3 = 201,
+	MX35_PAD_RESERVE1 = 202,
+	MX35_PAD_RESERVE2 = 203,
+	MX35_PAD_RESERVE3 = 204,
+	MX35_PAD_RESERVE4 = 205,
+	MX35_PAD_RESERVE5 = 206,
+	MX35_PAD_RESERVE6 = 207,
+	MX35_PAD_RESERVE7 = 208,
+	MX35_PAD_RESET_IN_B = 209,
+	MX35_PAD_POR_B = 210,
+	MX35_PAD_RESERVE8 = 211,
+	MX35_PAD_BOOT_MODE0 = 212,
+	MX35_PAD_BOOT_MODE1 = 213,
+	MX35_PAD_CLK_MODE0 = 214,
+	MX35_PAD_CLK_MODE1 = 215,
+	MX35_PAD_POWER_FAIL = 216,
+	MX35_PAD_RESERVE9 = 217,
+	MX35_PAD_RESERVE10 = 218,
+	MX35_PAD_RESERVE11 = 219,
+	MX35_PAD_RESERVE12 = 220,
+	MX35_PAD_RESERVE13 = 221,
+	MX35_PAD_RESERVE14 = 222,
+	MX35_PAD_RESERVE15 = 223,
+	MX35_PAD_RESERVE16 = 224,
+	MX35_PAD_RESERVE17 = 225,
+	MX35_PAD_RESERVE18 = 226,
+	MX35_PAD_RESERVE19 = 227,
+	MX35_PAD_RESERVE20 = 228,
+	MX35_PAD_RESERVE21 = 229,
+	MX35_PAD_RESERVE22 = 230,
+	MX35_PAD_RESERVE23 = 231,
+	MX35_PAD_RESERVE24 = 232,
+	MX35_PAD_RESERVE25 = 233,
+	MX35_PAD_RESERVE26 = 234,
+	MX35_PAD_RESERVE27 = 235,
+	MX35_PAD_RESERVE28 = 236,
+	MX35_PAD_RESERVE29 = 237,
+	MX35_PAD_RESERVE30 = 238,
+	MX35_PAD_RESERVE31 = 239,
+	MX35_PAD_RESERVE32 = 240,
+	MX35_PAD_RESERVE33 = 241,
+	MX35_PAD_RESERVE34 = 242,
+	MX35_PAD_RESERVE35 = 243,
+	MX35_PAD_RESERVE36 = 244,
+	MX35_PAD_SDBA1 = 245,
+	MX35_PAD_SDBA0 = 246,
+	MX35_PAD_SD0 = 247,
+	MX35_PAD_SD1 = 248,
+	MX35_PAD_SD2 = 249,
+	MX35_PAD_SD3 = 250,
+	MX35_PAD_SD4 = 251,
+	MX35_PAD_SD5 = 252,
+	MX35_PAD_SD6 = 253,
+	MX35_PAD_SD7 = 254,
+	MX35_PAD_SD8 = 255,
+	MX35_PAD_SD9 = 256,
+	MX35_PAD_SD10 = 257,
+	MX35_PAD_SD11 = 258,
+	MX35_PAD_SD12 = 259,
+	MX35_PAD_SD13 = 260,
+	MX35_PAD_SD14 = 261,
+	MX35_PAD_SD15 = 262,
+	MX35_PAD_SD16 = 263,
+	MX35_PAD_SD17 = 264,
+	MX35_PAD_SD18 = 265,
+	MX35_PAD_SD19 = 266,
+	MX35_PAD_SD20 = 267,
+	MX35_PAD_SD21 = 268,
+	MX35_PAD_SD22 = 269,
+	MX35_PAD_SD23 = 270,
+	MX35_PAD_SD24 = 271,
+	MX35_PAD_SD25 = 272,
+	MX35_PAD_SD26 = 273,
+	MX35_PAD_SD27 = 274,
+	MX35_PAD_SD28 = 275,
+	MX35_PAD_SD29 = 276,
+	MX35_PAD_SD30 = 277,
+	MX35_PAD_SD31 = 278,
+	MX35_PAD_DQM0 = 279,
+	MX35_PAD_DQM1 = 280,
+	MX35_PAD_DQM2 = 281,
+	MX35_PAD_DQM3 = 282,
+	MX35_PAD_RESERVE37 = 283,
+	MX35_PAD_RESERVE38 = 284,
+	MX35_PAD_RESERVE39 = 285,
+	MX35_PAD_RESERVE40 = 286,
+	MX35_PAD_RESERVE41 = 287,
+	MX35_PAD_RESERVE42 = 288,
+	MX35_PAD_RESERVE43 = 289,
+	MX35_PAD_RESERVE44 = 290,
+	MX35_PAD_RESERVE45 = 291,
+	MX35_PAD_RESERVE46 = 292,
+	MX35_PAD_ECB = 293,
+	MX35_PAD_RESERVE47 = 294,
+	MX35_PAD_RESERVE48 = 295,
+	MX35_PAD_RESERVE49 = 296,
+	MX35_PAD_RAS = 297,
+	MX35_PAD_CAS = 298,
+	MX35_PAD_SDWE = 299,
+	MX35_PAD_SDCKE0 = 300,
+	MX35_PAD_SDCKE1 = 301,
+	MX35_PAD_SDCLK = 302,
+	MX35_PAD_SDQS0 = 303,
+	MX35_PAD_SDQS1 = 304,
+	MX35_PAD_SDQS2 = 305,
+	MX35_PAD_SDQS3 = 306,
+	MX35_PAD_RESERVE50 = 307,
+	MX35_PAD_RESERVE51 = 308,
+	MX35_PAD_RESERVE52 = 309,
+	MX35_PAD_RESERVE53 = 310,
+	MX35_PAD_RESERVE54 = 311,
+	MX35_PAD_RESERVE55 = 312,
+	MX35_PAD_D15 = 313,
+	MX35_PAD_D14 = 314,
+	MX35_PAD_D13 = 315,
+	MX35_PAD_D12 = 316,
+	MX35_PAD_D11 = 317,
+	MX35_PAD_D10 = 318,
+	MX35_PAD_D9 = 319,
+	MX35_PAD_D8 = 320,
+	MX35_PAD_D7 = 321,
+	MX35_PAD_D6 = 322,
+	MX35_PAD_D5 = 323,
+	MX35_PAD_D4 = 324,
+	MX35_PAD_D3 = 325,
+	MX35_PAD_D2 = 326,
+	MX35_PAD_D1 = 327,
+	MX35_PAD_D0 = 328,
+	MX35_PAD_RESERVE56 = 329,
+	MX35_PAD_RESERVE57 = 330,
+	MX35_PAD_RESERVE58 = 331,
+	MX35_PAD_RESERVE59 = 332,
+	MX35_PAD_RESERVE60 = 333,
+	MX35_PAD_RESERVE61 = 334,
+	MX35_PAD_RESERVE62 = 335,
+	MX35_PAD_RESERVE63 = 336,
+	MX35_PAD_RESERVE64 = 337,
+	MX35_PAD_RESERVE65 = 338,
+	MX35_PAD_RESERVE66 = 339,
+	MX35_PAD_RESERVE67 = 340,
+	MX35_PAD_RESERVE68 = 341,
+	MX35_PAD_RESERVE69 = 342,
+	MX35_PAD_RESERVE70 = 343,
+	MX35_PAD_RESERVE71 = 344,
+	MX35_PAD_RESERVE72 = 345,
+	MX35_PAD_RESERVE73 = 346,
+	MX35_PAD_RESERVE74 = 347,
+	MX35_PAD_RESERVE75 = 348,
+	MX35_PAD_RESERVE76 = 349,
+	MX35_PAD_RESERVE77 = 350,
+	MX35_PAD_RESERVE78 = 351,
+	MX35_PAD_RESERVE79 = 352,
+	MX35_PAD_RESERVE80 = 353,
+	MX35_PAD_RESERVE81 = 354,
+	MX35_PAD_RESERVE82 = 355,
+	MX35_PAD_RESERVE83 = 356,
+	MX35_PAD_RESERVE84 = 357,
+	MX35_PAD_RESERVE85 = 358,
+	MX35_PAD_RESERVE86 = 359,
+	MX35_PAD_RESERVE87 = 360,
+	MX35_PAD_RESERVE88 = 361,
+	MX35_PAD_RESERVE89 = 362,
+	MX35_PAD_RESERVE90 = 363,
+	MX35_PAD_RESERVE91 = 364,
+	MX35_PAD_RESERVE92 = 365,
+	MX35_PAD_RESERVE93 = 366,
+	MX35_PAD_RESERVE94 = 367,
+	MX35_PAD_RESERVE95 = 368,
+	MX35_PAD_RESERVE96 = 369,
+	MX35_PAD_RESERVE97 = 370,
+	MX35_PAD_RESERVE98 = 371,
+	MX35_PAD_RESERVE99 = 372,
+	MX35_PAD_RESERVE100 = 373,
+	MX35_PAD_RESERVE101 = 374,
+	MX35_PAD_RESERVE102 = 375,
+	MX35_PAD_RESERVE103 = 376,
+	MX35_PAD_RESERVE104 = 377,
+	MX35_PAD_RESERVE105 = 378,
+	MX35_PAD_RTCK = 379,
+	MX35_PAD_TCK = 380,
+	MX35_PAD_TMS = 381,
+	MX35_PAD_TDI = 382,
+	MX35_PAD_TDO = 383,
+	MX35_PAD_TRSTB = 384,
+	MX35_PAD_DE_B = 385,
+	MX35_PAD_SJC_MOD = 386,
+	MX35_PAD_RESERVE106 = 387,
+	MX35_PAD_RESERVE107 = 388,
+	MX35_PAD_RESERVE108 = 389,
+	MX35_PAD_RESERVE109 = 390,
+	MX35_PAD_RESERVE110 = 391,
+	MX35_PAD_RESERVE111 = 392,
+	MX35_PAD_RESERVE112 = 393,
+	MX35_PAD_RESERVE113 = 394,
+	MX35_PAD_RESERVE114 = 395,
+	MX35_PAD_RESERVE115 = 396,
+	MX35_PAD_RESERVE116 = 397,
+	MX35_PAD_RESERVE117 = 398,
+	MX35_PAD_RESERVE118 = 399,
+	MX35_PAD_RESERVE119 = 400,
+	MX35_PAD_RESERVE120 = 401,
+	MX35_PAD_RESERVE121 = 402,
+	MX35_PAD_RESERVE122 = 403,
+	MX35_PAD_RESERVE123 = 404,
+	MX35_PAD_RESERVE124 = 405,
+	MX35_PAD_RESERVE125 = 406,
+	MX35_PAD_RESERVE126 = 407,
+	MX35_PAD_RESERVE127 = 408,
+	MX35_PAD_RESERVE128 = 409,
+	MX35_PAD_RESERVE129 = 410,
+	MX35_PAD_RESERVE130 = 411,
+	MX35_PAD_RESERVE131 = 412,
+	MX35_PAD_RESERVE132 = 413,
+	MX35_PAD_RESERVE133 = 414,
+	MX35_PAD_RESERVE134 = 415,
+	MX35_PAD_RESERVE135 = 416,
+	MX35_PAD_RESERVE136 = 417,
+	MX35_PAD_RESERVE137 = 418,
+	MX35_PAD_RESERVE138 = 419,
+	MX35_PAD_RESERVE139 = 420,
+	MX35_PAD_RESERVE140 = 421,
+	MX35_PAD_RESERVE141 = 422,
+	MX35_PAD_RESERVE142 = 423,
+	MX35_PAD_RESERVE143 = 424,
+	MX35_PAD_RESERVE144 = 425,
+	MX35_PAD_RESERVE145 = 426,
+	MX35_PAD_RESERVE146 = 427,
+	MX35_PAD_RESERVE147 = 428,
+	MX35_PAD_RESERVE148 = 429,
+	MX35_PAD_RESERVE149 = 430,
+	MX35_PAD_RESERVE150 = 431,
+	MX35_PAD_RESERVE151 = 432,
+	MX35_PAD_RESERVE152 = 433,
+	MX35_PAD_RESERVE153 = 434,
+	MX35_PAD_RESERVE154 = 435,
+	MX35_PAD_RESERVE155 = 436,
+	MX35_PAD_RESERVE156 = 437,
+	MX35_PAD_RESERVE157 = 438,
+	MX35_PAD_RESERVE158 = 439,
+	MX35_PAD_RESERVE159 = 440,
+	MX35_PAD_RESERVE160 = 441,
+	MX35_PAD_RESERVE161 = 442,
+	MX35_PAD_RESERVE162 = 443,
+	MX35_PAD_RESERVE163 = 444,
+	MX35_PAD_RESERVE164 = 445,
+	MX35_PAD_RESERVE165 = 446,
+	MX35_PAD_RESERVE166 = 447,
+	MX35_PAD_RESERVE167 = 448,
+	MX35_PAD_RESERVE168 = 449,
+	MX35_PAD_RESERVE169 = 450,
+	MX35_PAD_RESERVE170 = 451,
+	MX35_PAD_RESERVE171 = 452,
+	MX35_PAD_RESERVE172 = 453,
+	MX35_PAD_RESERVE173 = 454,
+	MX35_PAD_RESERVE174 = 455,
+	MX35_PAD_RESERVE175 = 456,
+	MX35_PAD_RESERVE176 = 457,
+	MX35_PAD_RESERVE177 = 458,
+	MX35_PAD_RESERVE178 = 459,
+	MX35_PAD_RESERVE179 = 460,
+	MX35_PAD_RESERVE180 = 461,
+	MX35_PAD_RESERVE181 = 462,
+	MX35_PAD_RESERVE182 = 463,
+	MX35_PAD_RESERVE183 = 464,
+	MX35_PAD_RESERVE184 = 465,
+	MX35_PAD_RESERVE185 = 466,
+	MX35_PAD_RESERVE186 = 467,
+	MX35_PAD_RESERVE187 = 468,
+	MX35_PAD_RESERVE188 = 469,
+	MX35_PAD_RESERVE189 = 470,
+	MX35_PAD_RESERVE190 = 471,
+	MX35_PAD_RESERVE191 = 472,
+	MX35_PAD_RESERVE192 = 473,
+	MX35_PAD_RESERVE193 = 474,
+	MX35_PAD_RESERVE194 = 475,
+	MX35_PAD_RESERVE195 = 476,
+	MX35_PAD_RESERVE196 = 477,
+	MX35_PAD_RESERVE197 = 478,
+	MX35_PAD_RESERVE198 = 479,
+	MX35_PAD_RESERVE199 = 480,
+	MX35_PAD_RESERVE200 = 481,
+	MX35_PAD_RESERVE201 = 482,
+	MX35_PAD_EXT_ARMCLK = 483,
+	MX35_PAD_TEST_MODE = 484,
 };
 
 /* Pad names for the pinmux subsystem */
 static const struct pinctrl_pin_desc imx35_pinctrl_pads[] = {
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE0),
 	IMX_PINCTRL_PIN(MX35_PAD_CAPTURE),
 	IMX_PINCTRL_PIN(MX35_PAD_COMPARE),
 	IMX_PINCTRL_PIN(MX35_PAD_WDOG_RST),
@@ -1274,14 +521,7 @@ static const struct pinctrl_pin_desc imx35_pinctrl_pads[] = {
 	IMX_PINCTRL_PIN(MX35_PAD_GPIO1_1),
 	IMX_PINCTRL_PIN(MX35_PAD_GPIO2_0),
 	IMX_PINCTRL_PIN(MX35_PAD_GPIO3_0),
-	IMX_PINCTRL_PIN(MX35_PAD_RESET_IN_B),
-	IMX_PINCTRL_PIN(MX35_PAD_POR_B),
 	IMX_PINCTRL_PIN(MX35_PAD_CLKO),
-	IMX_PINCTRL_PIN(MX35_PAD_BOOT_MODE0),
-	IMX_PINCTRL_PIN(MX35_PAD_BOOT_MODE1),
-	IMX_PINCTRL_PIN(MX35_PAD_CLK_MODE0),
-	IMX_PINCTRL_PIN(MX35_PAD_CLK_MODE1),
-	IMX_PINCTRL_PIN(MX35_PAD_POWER_FAIL),
 	IMX_PINCTRL_PIN(MX35_PAD_VSTBY),
 	IMX_PINCTRL_PIN(MX35_PAD_A0),
 	IMX_PINCTRL_PIN(MX35_PAD_A1),
@@ -1310,44 +550,6 @@ static const struct pinctrl_pin_desc imx35_pinctrl_pads[] = {
 	IMX_PINCTRL_PIN(MX35_PAD_A23),
 	IMX_PINCTRL_PIN(MX35_PAD_A24),
 	IMX_PINCTRL_PIN(MX35_PAD_A25),
-	IMX_PINCTRL_PIN(MX35_PAD_SDBA1),
-	IMX_PINCTRL_PIN(MX35_PAD_SDBA0),
-	IMX_PINCTRL_PIN(MX35_PAD_SD0),
-	IMX_PINCTRL_PIN(MX35_PAD_SD1),
-	IMX_PINCTRL_PIN(MX35_PAD_SD2),
-	IMX_PINCTRL_PIN(MX35_PAD_SD3),
-	IMX_PINCTRL_PIN(MX35_PAD_SD4),
-	IMX_PINCTRL_PIN(MX35_PAD_SD5),
-	IMX_PINCTRL_PIN(MX35_PAD_SD6),
-	IMX_PINCTRL_PIN(MX35_PAD_SD7),
-	IMX_PINCTRL_PIN(MX35_PAD_SD8),
-	IMX_PINCTRL_PIN(MX35_PAD_SD9),
-	IMX_PINCTRL_PIN(MX35_PAD_SD10),
-	IMX_PINCTRL_PIN(MX35_PAD_SD11),
-	IMX_PINCTRL_PIN(MX35_PAD_SD12),
-	IMX_PINCTRL_PIN(MX35_PAD_SD13),
-	IMX_PINCTRL_PIN(MX35_PAD_SD14),
-	IMX_PINCTRL_PIN(MX35_PAD_SD15),
-	IMX_PINCTRL_PIN(MX35_PAD_SD16),
-	IMX_PINCTRL_PIN(MX35_PAD_SD17),
-	IMX_PINCTRL_PIN(MX35_PAD_SD18),
-	IMX_PINCTRL_PIN(MX35_PAD_SD19),
-	IMX_PINCTRL_PIN(MX35_PAD_SD20),
-	IMX_PINCTRL_PIN(MX35_PAD_SD21),
-	IMX_PINCTRL_PIN(MX35_PAD_SD22),
-	IMX_PINCTRL_PIN(MX35_PAD_SD23),
-	IMX_PINCTRL_PIN(MX35_PAD_SD24),
-	IMX_PINCTRL_PIN(MX35_PAD_SD25),
-	IMX_PINCTRL_PIN(MX35_PAD_SD26),
-	IMX_PINCTRL_PIN(MX35_PAD_SD27),
-	IMX_PINCTRL_PIN(MX35_PAD_SD28),
-	IMX_PINCTRL_PIN(MX35_PAD_SD29),
-	IMX_PINCTRL_PIN(MX35_PAD_SD30),
-	IMX_PINCTRL_PIN(MX35_PAD_SD31),
-	IMX_PINCTRL_PIN(MX35_PAD_DQM0),
-	IMX_PINCTRL_PIN(MX35_PAD_DQM1),
-	IMX_PINCTRL_PIN(MX35_PAD_DQM2),
-	IMX_PINCTRL_PIN(MX35_PAD_DQM3),
 	IMX_PINCTRL_PIN(MX35_PAD_EB0),
 	IMX_PINCTRL_PIN(MX35_PAD_EB1),
 	IMX_PINCTRL_PIN(MX35_PAD_OE),
@@ -1358,42 +560,15 @@ static const struct pinctrl_pin_desc imx35_pinctrl_pads[] = {
 	IMX_PINCTRL_PIN(MX35_PAD_CS4),
 	IMX_PINCTRL_PIN(MX35_PAD_CS5),
 	IMX_PINCTRL_PIN(MX35_PAD_NF_CE0),
-	IMX_PINCTRL_PIN(MX35_PAD_ECB),
 	IMX_PINCTRL_PIN(MX35_PAD_LBA),
 	IMX_PINCTRL_PIN(MX35_PAD_BCLK),
 	IMX_PINCTRL_PIN(MX35_PAD_RW),
-	IMX_PINCTRL_PIN(MX35_PAD_RAS),
-	IMX_PINCTRL_PIN(MX35_PAD_CAS),
-	IMX_PINCTRL_PIN(MX35_PAD_SDWE),
-	IMX_PINCTRL_PIN(MX35_PAD_SDCKE0),
-	IMX_PINCTRL_PIN(MX35_PAD_SDCKE1),
-	IMX_PINCTRL_PIN(MX35_PAD_SDCLK),
-	IMX_PINCTRL_PIN(MX35_PAD_SDQS0),
-	IMX_PINCTRL_PIN(MX35_PAD_SDQS1),
-	IMX_PINCTRL_PIN(MX35_PAD_SDQS2),
-	IMX_PINCTRL_PIN(MX35_PAD_SDQS3),
 	IMX_PINCTRL_PIN(MX35_PAD_NFWE_B),
 	IMX_PINCTRL_PIN(MX35_PAD_NFRE_B),
 	IMX_PINCTRL_PIN(MX35_PAD_NFALE),
 	IMX_PINCTRL_PIN(MX35_PAD_NFCLE),
 	IMX_PINCTRL_PIN(MX35_PAD_NFWP_B),
 	IMX_PINCTRL_PIN(MX35_PAD_NFRB),
-	IMX_PINCTRL_PIN(MX35_PAD_D15),
-	IMX_PINCTRL_PIN(MX35_PAD_D14),
-	IMX_PINCTRL_PIN(MX35_PAD_D13),
-	IMX_PINCTRL_PIN(MX35_PAD_D12),
-	IMX_PINCTRL_PIN(MX35_PAD_D11),
-	IMX_PINCTRL_PIN(MX35_PAD_D10),
-	IMX_PINCTRL_PIN(MX35_PAD_D9),
-	IMX_PINCTRL_PIN(MX35_PAD_D8),
-	IMX_PINCTRL_PIN(MX35_PAD_D7),
-	IMX_PINCTRL_PIN(MX35_PAD_D6),
-	IMX_PINCTRL_PIN(MX35_PAD_D5),
-	IMX_PINCTRL_PIN(MX35_PAD_D4),
-	IMX_PINCTRL_PIN(MX35_PAD_D3),
-	IMX_PINCTRL_PIN(MX35_PAD_D2),
-	IMX_PINCTRL_PIN(MX35_PAD_D1),
-	IMX_PINCTRL_PIN(MX35_PAD_D0),
 	IMX_PINCTRL_PIN(MX35_PAD_CSI_D8),
 	IMX_PINCTRL_PIN(MX35_PAD_CSI_D9),
 	IMX_PINCTRL_PIN(MX35_PAD_CSI_D10),
@@ -1444,14 +619,6 @@ static const struct pinctrl_pin_desc imx35_pinctrl_pads[] = {
 	IMX_PINCTRL_PIN(MX35_PAD_TXD2),
 	IMX_PINCTRL_PIN(MX35_PAD_RTS2),
 	IMX_PINCTRL_PIN(MX35_PAD_CTS2),
-	IMX_PINCTRL_PIN(MX35_PAD_RTCK),
-	IMX_PINCTRL_PIN(MX35_PAD_TCK),
-	IMX_PINCTRL_PIN(MX35_PAD_TMS),
-	IMX_PINCTRL_PIN(MX35_PAD_TDI),
-	IMX_PINCTRL_PIN(MX35_PAD_TDO),
-	IMX_PINCTRL_PIN(MX35_PAD_TRSTB),
-	IMX_PINCTRL_PIN(MX35_PAD_DE_B),
-	IMX_PINCTRL_PIN(MX35_PAD_SJC_MOD),
 	IMX_PINCTRL_PIN(MX35_PAD_USBOTG_PWR),
 	IMX_PINCTRL_PIN(MX35_PAD_USBOTG_OC),
 	IMX_PINCTRL_PIN(MX35_PAD_LD0),
@@ -1548,6 +715,287 @@ static const struct pinctrl_pin_desc imx35_pinctrl_pads[] = {
 	IMX_PINCTRL_PIN(MX35_PAD_FEC_TDATA2),
 	IMX_PINCTRL_PIN(MX35_PAD_FEC_RDATA3),
 	IMX_PINCTRL_PIN(MX35_PAD_FEC_TDATA3),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE1),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE2),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE3),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE4),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE5),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE6),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE7),
+	IMX_PINCTRL_PIN(MX35_PAD_RESET_IN_B),
+	IMX_PINCTRL_PIN(MX35_PAD_POR_B),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE8),
+	IMX_PINCTRL_PIN(MX35_PAD_BOOT_MODE0),
+	IMX_PINCTRL_PIN(MX35_PAD_BOOT_MODE1),
+	IMX_PINCTRL_PIN(MX35_PAD_CLK_MODE0),
+	IMX_PINCTRL_PIN(MX35_PAD_CLK_MODE1),
+	IMX_PINCTRL_PIN(MX35_PAD_POWER_FAIL),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE9),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE10),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE11),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE12),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE13),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE14),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE15),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE16),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE17),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE18),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE19),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE20),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE21),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE22),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE23),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE24),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE25),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE26),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE27),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE28),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE29),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE30),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE31),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE32),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE33),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE34),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE35),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE36),
+	IMX_PINCTRL_PIN(MX35_PAD_SDBA1),
+	IMX_PINCTRL_PIN(MX35_PAD_SDBA0),
+	IMX_PINCTRL_PIN(MX35_PAD_SD0),
+	IMX_PINCTRL_PIN(MX35_PAD_SD1),
+	IMX_PINCTRL_PIN(MX35_PAD_SD2),
+	IMX_PINCTRL_PIN(MX35_PAD_SD3),
+	IMX_PINCTRL_PIN(MX35_PAD_SD4),
+	IMX_PINCTRL_PIN(MX35_PAD_SD5),
+	IMX_PINCTRL_PIN(MX35_PAD_SD6),
+	IMX_PINCTRL_PIN(MX35_PAD_SD7),
+	IMX_PINCTRL_PIN(MX35_PAD_SD8),
+	IMX_PINCTRL_PIN(MX35_PAD_SD9),
+	IMX_PINCTRL_PIN(MX35_PAD_SD10),
+	IMX_PINCTRL_PIN(MX35_PAD_SD11),
+	IMX_PINCTRL_PIN(MX35_PAD_SD12),
+	IMX_PINCTRL_PIN(MX35_PAD_SD13),
+	IMX_PINCTRL_PIN(MX35_PAD_SD14),
+	IMX_PINCTRL_PIN(MX35_PAD_SD15),
+	IMX_PINCTRL_PIN(MX35_PAD_SD16),
+	IMX_PINCTRL_PIN(MX35_PAD_SD17),
+	IMX_PINCTRL_PIN(MX35_PAD_SD18),
+	IMX_PINCTRL_PIN(MX35_PAD_SD19),
+	IMX_PINCTRL_PIN(MX35_PAD_SD20),
+	IMX_PINCTRL_PIN(MX35_PAD_SD21),
+	IMX_PINCTRL_PIN(MX35_PAD_SD22),
+	IMX_PINCTRL_PIN(MX35_PAD_SD23),
+	IMX_PINCTRL_PIN(MX35_PAD_SD24),
+	IMX_PINCTRL_PIN(MX35_PAD_SD25),
+	IMX_PINCTRL_PIN(MX35_PAD_SD26),
+	IMX_PINCTRL_PIN(MX35_PAD_SD27),
+	IMX_PINCTRL_PIN(MX35_PAD_SD28),
+	IMX_PINCTRL_PIN(MX35_PAD_SD29),
+	IMX_PINCTRL_PIN(MX35_PAD_SD30),
+	IMX_PINCTRL_PIN(MX35_PAD_SD31),
+	IMX_PINCTRL_PIN(MX35_PAD_DQM0),
+	IMX_PINCTRL_PIN(MX35_PAD_DQM1),
+	IMX_PINCTRL_PIN(MX35_PAD_DQM2),
+	IMX_PINCTRL_PIN(MX35_PAD_DQM3),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE37),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE38),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE39),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE40),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE41),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE42),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE43),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE44),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE45),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE46),
+	IMX_PINCTRL_PIN(MX35_PAD_ECB),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE47),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE48),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE49),
+	IMX_PINCTRL_PIN(MX35_PAD_RAS),
+	IMX_PINCTRL_PIN(MX35_PAD_CAS),
+	IMX_PINCTRL_PIN(MX35_PAD_SDWE),
+	IMX_PINCTRL_PIN(MX35_PAD_SDCKE0),
+	IMX_PINCTRL_PIN(MX35_PAD_SDCKE1),
+	IMX_PINCTRL_PIN(MX35_PAD_SDCLK),
+	IMX_PINCTRL_PIN(MX35_PAD_SDQS0),
+	IMX_PINCTRL_PIN(MX35_PAD_SDQS1),
+	IMX_PINCTRL_PIN(MX35_PAD_SDQS2),
+	IMX_PINCTRL_PIN(MX35_PAD_SDQS3),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE50),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE51),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE52),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE53),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE54),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE55),
+	IMX_PINCTRL_PIN(MX35_PAD_D15),
+	IMX_PINCTRL_PIN(MX35_PAD_D14),
+	IMX_PINCTRL_PIN(MX35_PAD_D13),
+	IMX_PINCTRL_PIN(MX35_PAD_D12),
+	IMX_PINCTRL_PIN(MX35_PAD_D11),
+	IMX_PINCTRL_PIN(MX35_PAD_D10),
+	IMX_PINCTRL_PIN(MX35_PAD_D9),
+	IMX_PINCTRL_PIN(MX35_PAD_D8),
+	IMX_PINCTRL_PIN(MX35_PAD_D7),
+	IMX_PINCTRL_PIN(MX35_PAD_D6),
+	IMX_PINCTRL_PIN(MX35_PAD_D5),
+	IMX_PINCTRL_PIN(MX35_PAD_D4),
+	IMX_PINCTRL_PIN(MX35_PAD_D3),
+	IMX_PINCTRL_PIN(MX35_PAD_D2),
+	IMX_PINCTRL_PIN(MX35_PAD_D1),
+	IMX_PINCTRL_PIN(MX35_PAD_D0),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE56),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE57),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE58),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE59),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE60),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE61),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE62),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE63),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE64),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE65),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE66),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE67),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE68),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE69),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE70),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE71),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE72),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE73),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE74),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE75),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE76),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE77),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE78),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE79),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE80),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE81),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE82),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE83),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE84),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE85),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE86),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE87),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE88),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE89),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE90),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE91),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE92),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE93),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE94),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE95),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE96),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE97),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE98),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE99),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE100),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE101),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE102),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE103),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE104),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE105),
+	IMX_PINCTRL_PIN(MX35_PAD_RTCK),
+	IMX_PINCTRL_PIN(MX35_PAD_TCK),
+	IMX_PINCTRL_PIN(MX35_PAD_TMS),
+	IMX_PINCTRL_PIN(MX35_PAD_TDI),
+	IMX_PINCTRL_PIN(MX35_PAD_TDO),
+	IMX_PINCTRL_PIN(MX35_PAD_TRSTB),
+	IMX_PINCTRL_PIN(MX35_PAD_DE_B),
+	IMX_PINCTRL_PIN(MX35_PAD_SJC_MOD),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE106),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE107),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE108),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE109),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE110),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE111),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE112),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE113),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE114),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE115),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE116),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE117),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE118),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE119),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE120),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE121),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE122),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE123),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE124),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE125),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE126),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE127),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE128),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE129),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE130),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE131),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE132),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE133),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE134),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE135),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE136),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE137),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE138),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE139),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE140),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE141),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE142),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE143),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE144),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE145),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE146),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE147),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE148),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE149),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE150),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE151),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE152),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE153),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE154),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE155),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE156),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE157),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE158),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE159),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE160),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE161),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE162),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE163),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE164),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE165),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE166),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE167),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE168),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE169),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE170),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE171),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE172),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE173),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE174),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE175),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE176),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE177),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE178),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE179),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE180),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE181),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE182),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE183),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE184),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE185),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE186),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE187),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE188),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE189),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE190),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE191),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE192),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE193),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE194),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE195),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE196),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE197),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE198),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE199),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE200),
+	IMX_PINCTRL_PIN(MX35_PAD_RESERVE201),
 	IMX_PINCTRL_PIN(MX35_PAD_EXT_ARMCLK),
 	IMX_PINCTRL_PIN(MX35_PAD_TEST_MODE),
 };
@@ -1555,8 +1003,6 @@ static const struct pinctrl_pin_desc imx35_pinctrl_pads[] = {
 static struct imx_pinctrl_soc_info imx35_pinctrl_info = {
 	.pins = imx35_pinctrl_pads,
 	.npins = ARRAY_SIZE(imx35_pinctrl_pads),
-	.pin_regs = imx35_pin_regs,
-	.npin_regs = ARRAY_SIZE(imx35_pin_regs),
 };
 
 static struct of_device_id imx35_pinctrl_of_match[] = {
diff --git a/drivers/pinctrl/pinctrl-imx51.c b/drivers/pinctrl/pinctrl-imx51.c
index 9a92aaad150f..db268b920079 100644
--- a/drivers/pinctrl/pinctrl-imx51.c
+++ b/drivers/pinctrl/pinctrl-imx51.c
@@ -23,1015 +23,400 @@
 #include "pinctrl-imx.h"
 
 enum imx51_pads {
-	MX51_PAD_EIM_D16 = 0,
-	MX51_PAD_EIM_D17 = 1,
-	MX51_PAD_EIM_D18 = 2,
-	MX51_PAD_EIM_D19 = 3,
-	MX51_PAD_EIM_D20 = 4,
-	MX51_PAD_EIM_D21 = 5,
-	MX51_PAD_EIM_D22 = 6,
-	MX51_PAD_EIM_D23 = 7,
-	MX51_PAD_EIM_D24 = 8,
-	MX51_PAD_EIM_D25 = 9,
-	MX51_PAD_EIM_D26 = 10,
-	MX51_PAD_EIM_D27 = 11,
-	MX51_PAD_EIM_D28 = 12,
-	MX51_PAD_EIM_D29 = 13,
-	MX51_PAD_EIM_D30 = 14,
-	MX51_PAD_EIM_D31 = 15,
-	MX51_PAD_EIM_A16 = 16,
-	MX51_PAD_EIM_A17 = 17,
-	MX51_PAD_EIM_A18 = 18,
-	MX51_PAD_EIM_A19 = 19,
-	MX51_PAD_EIM_A20 = 20,
-	MX51_PAD_EIM_A21 = 21,
-	MX51_PAD_EIM_A22 = 22,
-	MX51_PAD_EIM_A23 = 23,
-	MX51_PAD_EIM_A24 = 24,
-	MX51_PAD_EIM_A25 = 25,
-	MX51_PAD_EIM_A26 = 26,
-	MX51_PAD_EIM_A27 = 27,
-	MX51_PAD_EIM_EB0 = 28,
-	MX51_PAD_EIM_EB1 = 29,
-	MX51_PAD_EIM_EB2 = 30,
-	MX51_PAD_EIM_EB3 = 31,
-	MX51_PAD_EIM_OE = 32,
-	MX51_PAD_EIM_CS0 = 33,
-	MX51_PAD_EIM_CS1 = 34,
-	MX51_PAD_EIM_CS2 = 35,
-	MX51_PAD_EIM_CS3 = 36,
-	MX51_PAD_EIM_CS4 = 37,
-	MX51_PAD_EIM_CS5 = 38,
-	MX51_PAD_EIM_DTACK = 39,
-	MX51_PAD_EIM_LBA = 40,
-	MX51_PAD_EIM_CRE = 41,
-	MX51_PAD_DRAM_CS1 = 42,
-	MX51_PAD_NANDF_WE_B = 43,
-	MX51_PAD_NANDF_RE_B = 44,
-	MX51_PAD_NANDF_ALE = 45,
-	MX51_PAD_NANDF_CLE = 46,
-	MX51_PAD_NANDF_WP_B = 47,
-	MX51_PAD_NANDF_RB0 = 48,
-	MX51_PAD_NANDF_RB1 = 49,
-	MX51_PAD_NANDF_RB2 = 50,
-	MX51_PAD_NANDF_RB3 = 51,
-	MX51_PAD_GPIO_NAND = 52,
-	MX51_PAD_NANDF_CS0 = 53,
-	MX51_PAD_NANDF_CS1 = 54,
-	MX51_PAD_NANDF_CS2 = 55,
-	MX51_PAD_NANDF_CS3 = 56,
-	MX51_PAD_NANDF_CS4 = 57,
-	MX51_PAD_NANDF_CS5 = 58,
-	MX51_PAD_NANDF_CS6 = 59,
-	MX51_PAD_NANDF_CS7 = 60,
-	MX51_PAD_NANDF_RDY_INT = 61,
-	MX51_PAD_NANDF_D15 = 62,
-	MX51_PAD_NANDF_D14 = 63,
-	MX51_PAD_NANDF_D13 = 64,
-	MX51_PAD_NANDF_D12 = 65,
-	MX51_PAD_NANDF_D11 = 66,
-	MX51_PAD_NANDF_D10 = 67,
-	MX51_PAD_NANDF_D9 = 68,
-	MX51_PAD_NANDF_D8 = 69,
-	MX51_PAD_NANDF_D7 = 70,
-	MX51_PAD_NANDF_D6 = 71,
-	MX51_PAD_NANDF_D5 = 72,
-	MX51_PAD_NANDF_D4 = 73,
-	MX51_PAD_NANDF_D3 = 74,
-	MX51_PAD_NANDF_D2 = 75,
-	MX51_PAD_NANDF_D1 = 76,
-	MX51_PAD_NANDF_D0 = 77,
-	MX51_PAD_CSI1_D8 = 78,
-	MX51_PAD_CSI1_D9 = 79,
-	MX51_PAD_CSI1_D10 = 80,
-	MX51_PAD_CSI1_D11 = 81,
-	MX51_PAD_CSI1_D12 = 82,
-	MX51_PAD_CSI1_D13 = 83,
-	MX51_PAD_CSI1_D14 = 84,
-	MX51_PAD_CSI1_D15 = 85,
-	MX51_PAD_CSI1_D16 = 86,
-	MX51_PAD_CSI1_D17 = 87,
-	MX51_PAD_CSI1_D18 = 88,
-	MX51_PAD_CSI1_D19 = 89,
-	MX51_PAD_CSI1_VSYNC = 90,
-	MX51_PAD_CSI1_HSYNC = 91,
-	MX51_PAD_CSI1_PIXCLK = 92,
-	MX51_PAD_CSI1_MCLK = 93,
-	MX51_PAD_CSI2_D12 = 94,
-	MX51_PAD_CSI2_D13 = 95,
-	MX51_PAD_CSI2_D14 = 96,
-	MX51_PAD_CSI2_D15 = 97,
-	MX51_PAD_CSI2_D16 = 98,
-	MX51_PAD_CSI2_D17 = 99,
-	MX51_PAD_CSI2_D18 = 100,
-	MX51_PAD_CSI2_D19 = 101,
-	MX51_PAD_CSI2_VSYNC = 102,
-	MX51_PAD_CSI2_HSYNC = 103,
-	MX51_PAD_CSI2_PIXCLK = 104,
-	MX51_PAD_I2C1_CLK = 105,
-	MX51_PAD_I2C1_DAT = 106,
-	MX51_PAD_AUD3_BB_TXD = 107,
-	MX51_PAD_AUD3_BB_RXD = 108,
-	MX51_PAD_AUD3_BB_CK = 109,
-	MX51_PAD_AUD3_BB_FS = 110,
-	MX51_PAD_CSPI1_MOSI = 111,
-	MX51_PAD_CSPI1_MISO = 112,
-	MX51_PAD_CSPI1_SS0 = 113,
-	MX51_PAD_CSPI1_SS1 = 114,
-	MX51_PAD_CSPI1_RDY = 115,
-	MX51_PAD_CSPI1_SCLK = 116,
-	MX51_PAD_UART1_RXD = 117,
-	MX51_PAD_UART1_TXD = 118,
-	MX51_PAD_UART1_RTS = 119,
-	MX51_PAD_UART1_CTS = 120,
-	MX51_PAD_UART2_RXD = 121,
-	MX51_PAD_UART2_TXD = 122,
-	MX51_PAD_UART3_RXD = 123,
-	MX51_PAD_UART3_TXD = 124,
-	MX51_PAD_OWIRE_LINE = 125,
-	MX51_PAD_KEY_ROW0 = 126,
-	MX51_PAD_KEY_ROW1 = 127,
-	MX51_PAD_KEY_ROW2 = 128,
-	MX51_PAD_KEY_ROW3 = 129,
-	MX51_PAD_KEY_COL0 = 130,
-	MX51_PAD_KEY_COL1 = 131,
-	MX51_PAD_KEY_COL2 = 132,
-	MX51_PAD_KEY_COL3 = 133,
-	MX51_PAD_KEY_COL4 = 134,
-	MX51_PAD_KEY_COL5 = 135,
-	MX51_PAD_USBH1_CLK = 136,
-	MX51_PAD_USBH1_DIR = 137,
-	MX51_PAD_USBH1_STP = 138,
-	MX51_PAD_USBH1_NXT = 139,
-	MX51_PAD_USBH1_DATA0 = 140,
-	MX51_PAD_USBH1_DATA1 = 141,
-	MX51_PAD_USBH1_DATA2 = 142,
-	MX51_PAD_USBH1_DATA3 = 143,
-	MX51_PAD_USBH1_DATA4 = 144,
-	MX51_PAD_USBH1_DATA5 = 145,
-	MX51_PAD_USBH1_DATA6 = 146,
-	MX51_PAD_USBH1_DATA7 = 147,
-	MX51_PAD_DI1_PIN11 = 148,
-	MX51_PAD_DI1_PIN12 = 149,
-	MX51_PAD_DI1_PIN13 = 150,
-	MX51_PAD_DI1_D0_CS = 151,
-	MX51_PAD_DI1_D1_CS = 152,
-	MX51_PAD_DISPB2_SER_DIN = 153,
-	MX51_PAD_DISPB2_SER_DIO = 154,
-	MX51_PAD_DISPB2_SER_CLK = 155,
-	MX51_PAD_DISPB2_SER_RS = 156,
-	MX51_PAD_DISP1_DAT0 = 157,
-	MX51_PAD_DISP1_DAT1 = 158,
-	MX51_PAD_DISP1_DAT2 = 159,
-	MX51_PAD_DISP1_DAT3 = 160,
-	MX51_PAD_DISP1_DAT4 = 161,
-	MX51_PAD_DISP1_DAT5 = 162,
-	MX51_PAD_DISP1_DAT6 = 163,
-	MX51_PAD_DISP1_DAT7 = 164,
-	MX51_PAD_DISP1_DAT8 = 165,
-	MX51_PAD_DISP1_DAT9 = 166,
-	MX51_PAD_DISP1_DAT10 = 167,
-	MX51_PAD_DISP1_DAT11 = 168,
-	MX51_PAD_DISP1_DAT12 = 169,
-	MX51_PAD_DISP1_DAT13 = 170,
-	MX51_PAD_DISP1_DAT14 = 171,
-	MX51_PAD_DISP1_DAT15 = 172,
-	MX51_PAD_DISP1_DAT16 = 173,
-	MX51_PAD_DISP1_DAT17 = 174,
-	MX51_PAD_DISP1_DAT18 = 175,
-	MX51_PAD_DISP1_DAT19 = 176,
-	MX51_PAD_DISP1_DAT20 = 177,
-	MX51_PAD_DISP1_DAT21 = 178,
-	MX51_PAD_DISP1_DAT22 = 179,
-	MX51_PAD_DISP1_DAT23 = 180,
-	MX51_PAD_DI1_PIN3 = 181,
-	MX51_PAD_DI1_PIN2 = 182,
-	MX51_PAD_DI_GP2 = 183,
-	MX51_PAD_DI_GP3 = 184,
-	MX51_PAD_DI2_PIN4 = 185,
-	MX51_PAD_DI2_PIN2 = 186,
-	MX51_PAD_DI2_PIN3 = 187,
-	MX51_PAD_DI2_DISP_CLK = 188,
-	MX51_PAD_DI_GP4 = 189,
-	MX51_PAD_DISP2_DAT0 = 190,
-	MX51_PAD_DISP2_DAT1 = 191,
-	MX51_PAD_DISP2_DAT2 = 192,
-	MX51_PAD_DISP2_DAT3 = 193,
-	MX51_PAD_DISP2_DAT4 = 194,
-	MX51_PAD_DISP2_DAT5 = 195,
-	MX51_PAD_DISP2_DAT6 = 196,
-	MX51_PAD_DISP2_DAT7 = 197,
-	MX51_PAD_DISP2_DAT8 = 198,
-	MX51_PAD_DISP2_DAT9 = 199,
-	MX51_PAD_DISP2_DAT10 = 200,
-	MX51_PAD_DISP2_DAT11 = 201,
-	MX51_PAD_DISP2_DAT12 = 202,
-	MX51_PAD_DISP2_DAT13 = 203,
-	MX51_PAD_DISP2_DAT14 = 204,
-	MX51_PAD_DISP2_DAT15 = 205,
-	MX51_PAD_SD1_CMD = 206,
-	MX51_PAD_SD1_CLK = 207,
-	MX51_PAD_SD1_DATA0 = 208,
-	MX51_PAD_EIM_DA0 = 209,
-	MX51_PAD_EIM_DA1 = 210,
-	MX51_PAD_EIM_DA2 = 211,
-	MX51_PAD_EIM_DA3 = 212,
-	MX51_PAD_SD1_DATA1 = 213,
-	MX51_PAD_EIM_DA4 = 214,
-	MX51_PAD_EIM_DA5 = 215,
-	MX51_PAD_EIM_DA6 = 216,
-	MX51_PAD_EIM_DA7 = 217,
-	MX51_PAD_SD1_DATA2 = 218,
-	MX51_PAD_EIM_DA10 = 219,
-	MX51_PAD_EIM_DA11 = 220,
-	MX51_PAD_EIM_DA8 = 221,
-	MX51_PAD_EIM_DA9 = 222,
-	MX51_PAD_SD1_DATA3 = 223,
-	MX51_PAD_GPIO1_0 = 224,
-	MX51_PAD_GPIO1_1 = 225,
-	MX51_PAD_EIM_DA12 = 226,
-	MX51_PAD_EIM_DA13 = 227,
-	MX51_PAD_EIM_DA14 = 228,
-	MX51_PAD_EIM_DA15 = 229,
-	MX51_PAD_SD2_CMD = 230,
-	MX51_PAD_SD2_CLK = 231,
-	MX51_PAD_SD2_DATA0 = 232,
-	MX51_PAD_SD2_DATA1 = 233,
-	MX51_PAD_SD2_DATA2 = 234,
-	MX51_PAD_SD2_DATA3 = 235,
-	MX51_PAD_GPIO1_2 = 236,
-	MX51_PAD_GPIO1_3 = 237,
-	MX51_PAD_PMIC_INT_REQ = 238,
-	MX51_PAD_GPIO1_4 = 239,
-	MX51_PAD_GPIO1_5 = 240,
-	MX51_PAD_GPIO1_6 = 241,
-	MX51_PAD_GPIO1_7 = 242,
-	MX51_PAD_GPIO1_8 = 243,
-	MX51_PAD_GPIO1_9 = 244,
-};
-
-/* imx51 register maps */
-static struct imx_pin_reg imx51_pin_regs[] = {
-	IMX_PIN_REG(MX51_PAD_EIM_D16, 0x3f0, 0x05c, 5, 0x000, 0), /* MX51_PAD_EIM_D16__AUD4_RXFS */
-	IMX_PIN_REG(MX51_PAD_EIM_D16, 0x3f0, 0x05c, 7, 0x8d8, 0), /* MX51_PAD_EIM_D16__AUD5_TXD */
-	IMX_PIN_REG(MX51_PAD_EIM_D16, 0x3f0, 0x05c, 0, 0x000, 0), /* MX51_PAD_EIM_D16__EIM_D16 */
-	IMX_PIN_REG(MX51_PAD_EIM_D16, 0x3f0, 0x05c, 1, 0x000, 0), /* MX51_PAD_EIM_D16__GPIO2_0 */
-	IMX_PIN_REG(MX51_PAD_EIM_D16, 0x3f0, 0x05c, 4, 0x9b4, 0), /* MX51_PAD_EIM_D16__I2C1_SDA */
-	IMX_PIN_REG(MX51_PAD_EIM_D16, 0x3f0, 0x05c, 3, 0x000, 0), /* MX51_PAD_EIM_D16__UART2_CTS */
-	IMX_PIN_REG(MX51_PAD_EIM_D16, 0x3f0, 0x05c, 2, 0x000, 0), /* MX51_PAD_EIM_D16__USBH2_DATA0 */
-	IMX_PIN_REG(MX51_PAD_EIM_D17, 0x3f4, 0x060, 7, 0x8d4, 0), /* MX51_PAD_EIM_D17__AUD5_RXD */
-	IMX_PIN_REG(MX51_PAD_EIM_D17, 0x3f4, 0x060, 0, 0x000, 0), /* MX51_PAD_EIM_D17__EIM_D17 */
-	IMX_PIN_REG(MX51_PAD_EIM_D17, 0x3f4, 0x060, 1, 0x000, 0), /* MX51_PAD_EIM_D17__GPIO2_1 */
-	IMX_PIN_REG(MX51_PAD_EIM_D17, 0x3f4, 0x060, 3, 0x9ec, 0), /* MX51_PAD_EIM_D17__UART2_RXD */
-	IMX_PIN_REG(MX51_PAD_EIM_D17, 0x3f4, 0x060, 4, 0x000, 0), /* MX51_PAD_EIM_D17__UART3_CTS */
-	IMX_PIN_REG(MX51_PAD_EIM_D17, 0x3f4, 0x060, 2, 0x000, 0), /* MX51_PAD_EIM_D17__USBH2_DATA1 */
-	IMX_PIN_REG(MX51_PAD_EIM_D18, 0x3f8, 0x064, 7, 0x8e4, 0), /* MX51_PAD_EIM_D18__AUD5_TXC */
-	IMX_PIN_REG(MX51_PAD_EIM_D18, 0x3f8, 0x064, 0, 0x000, 0), /* MX51_PAD_EIM_D18__EIM_D18 */
-	IMX_PIN_REG(MX51_PAD_EIM_D18, 0x3f8, 0x064, 1, 0x000, 0), /* MX51_PAD_EIM_D18__GPIO2_2 */
-	IMX_PIN_REG(MX51_PAD_EIM_D18, 0x3f8, 0x064, 3, 0x000, 0), /* MX51_PAD_EIM_D18__UART2_TXD */
-	IMX_PIN_REG(MX51_PAD_EIM_D18, 0x3f8, 0x064, 4, 0x9f0, 1), /* MX51_PAD_EIM_D18__UART3_RTS */
-	IMX_PIN_REG(MX51_PAD_EIM_D18, 0x3f8, 0x064, 2, 0x000, 0), /* MX51_PAD_EIM_D18__USBH2_DATA2 */
-	IMX_PIN_REG(MX51_PAD_EIM_D19, 0x3fc, 0x068, 5, 0x000, 0), /* MX51_PAD_EIM_D19__AUD4_RXC */
-	IMX_PIN_REG(MX51_PAD_EIM_D19, 0x3fc, 0x068, 7, 0x8e8, 0), /* MX51_PAD_EIM_D19__AUD5_TXFS */
-	IMX_PIN_REG(MX51_PAD_EIM_D19, 0x3fc, 0x068, 0, 0x000, 0), /* MX51_PAD_EIM_D19__EIM_D19 */
-	IMX_PIN_REG(MX51_PAD_EIM_D19, 0x3fc, 0x068, 1, 0x000, 0), /* MX51_PAD_EIM_D19__GPIO2_3 */
-	IMX_PIN_REG(MX51_PAD_EIM_D19, 0x3fc, 0x068, 4, 0x9b0, 0), /* MX51_PAD_EIM_D19__I2C1_SCL */
-	IMX_PIN_REG(MX51_PAD_EIM_D19, 0x3fc, 0x068, 3, 0x9e8, 1), /* MX51_PAD_EIM_D19__UART2_RTS */
-	IMX_PIN_REG(MX51_PAD_EIM_D19, 0x3fc, 0x068, 2, 0x000, 0), /* MX51_PAD_EIM_D19__USBH2_DATA3 */
-	IMX_PIN_REG(MX51_PAD_EIM_D20, 0x400, 0x06c, 5, 0x8c8, 0), /* MX51_PAD_EIM_D20__AUD4_TXD */
-	IMX_PIN_REG(MX51_PAD_EIM_D20, 0x400, 0x06c, 0, 0x000, 0), /* MX51_PAD_EIM_D20__EIM_D20 */
-	IMX_PIN_REG(MX51_PAD_EIM_D20, 0x400, 0x06c, 1, 0x000, 0), /* MX51_PAD_EIM_D20__GPIO2_4 */
-	IMX_PIN_REG(MX51_PAD_EIM_D20, 0x400, 0x06c, 4, 0x000, 0), /* MX51_PAD_EIM_D20__SRTC_ALARM_DEB */
-	IMX_PIN_REG(MX51_PAD_EIM_D20, 0x400, 0x06c, 2, 0x000, 0), /* MX51_PAD_EIM_D20__USBH2_DATA4 */
-	IMX_PIN_REG(MX51_PAD_EIM_D21, 0x404, 0x070, 5, 0x8c4, 0), /* MX51_PAD_EIM_D21__AUD4_RXD */
-	IMX_PIN_REG(MX51_PAD_EIM_D21, 0x404, 0x070, 0, 0x000, 0), /* MX51_PAD_EIM_D21__EIM_D21 */
-	IMX_PIN_REG(MX51_PAD_EIM_D21, 0x404, 0x070, 1, 0x000, 0), /* MX51_PAD_EIM_D21__GPIO2_5 */
-	IMX_PIN_REG(MX51_PAD_EIM_D21, 0x404, 0x070, 3, 0x000, 0), /* MX51_PAD_EIM_D21__SRTC_ALARM_DEB */
-	IMX_PIN_REG(MX51_PAD_EIM_D21, 0x404, 0x070, 2, 0x000, 0), /* MX51_PAD_EIM_D21__USBH2_DATA5 */
-	IMX_PIN_REG(MX51_PAD_EIM_D22, 0x408, 0x074, 5, 0x8cc, 0), /* MX51_PAD_EIM_D22__AUD4_TXC */
-	IMX_PIN_REG(MX51_PAD_EIM_D22, 0x408, 0x074, 0, 0x000, 0), /* MX51_PAD_EIM_D22__EIM_D22 */
-	IMX_PIN_REG(MX51_PAD_EIM_D22, 0x408, 0x074, 1, 0x000, 0), /* MX51_PAD_EIM_D22__GPIO2_6 */
-	IMX_PIN_REG(MX51_PAD_EIM_D22, 0x408, 0x074, 2, 0x000, 0), /* MX51_PAD_EIM_D22__USBH2_DATA6 */
-	IMX_PIN_REG(MX51_PAD_EIM_D23, 0x40c, 0x078, 5, 0x8d0, 0), /* MX51_PAD_EIM_D23__AUD4_TXFS */
-	IMX_PIN_REG(MX51_PAD_EIM_D23, 0x40c, 0x078, 0, 0x000, 0), /* MX51_PAD_EIM_D23__EIM_D23 */
-	IMX_PIN_REG(MX51_PAD_EIM_D23, 0x40c, 0x078, 1, 0x000, 0), /* MX51_PAD_EIM_D23__GPIO2_7 */
-	IMX_PIN_REG(MX51_PAD_EIM_D23, 0x40c, 0x078, 4, 0x000, 0), /* MX51_PAD_EIM_D23__SPDIF_OUT1 */
-	IMX_PIN_REG(MX51_PAD_EIM_D23, 0x40c, 0x078, 2, 0x000, 0), /* MX51_PAD_EIM_D23__USBH2_DATA7 */
-	IMX_PIN_REG(MX51_PAD_EIM_D24, 0x410, 0x07c, 5, 0x8f8, 0), /* MX51_PAD_EIM_D24__AUD6_RXFS */
-	IMX_PIN_REG(MX51_PAD_EIM_D24, 0x410, 0x07c, 0, 0x000, 0), /* MX51_PAD_EIM_D24__EIM_D24 */
-	IMX_PIN_REG(MX51_PAD_EIM_D24, 0x410, 0x07c, 1, 0x000, 0), /* MX51_PAD_EIM_D24__GPIO2_8 */
-	IMX_PIN_REG(MX51_PAD_EIM_D24, 0x410, 0x07c, 4, 0x9bc, 0), /* MX51_PAD_EIM_D24__I2C2_SDA */
-	IMX_PIN_REG(MX51_PAD_EIM_D24, 0x410, 0x07c, 3, 0x000, 0), /* MX51_PAD_EIM_D24__UART3_CTS */
-	IMX_PIN_REG(MX51_PAD_EIM_D24, 0x410, 0x07c, 2, 0x000, 0), /* MX51_PAD_EIM_D24__USBOTG_DATA0 */
-	IMX_PIN_REG(MX51_PAD_EIM_D25, 0x414, 0x080, 0, 0x000, 0), /* MX51_PAD_EIM_D25__EIM_D25 */
-	IMX_PIN_REG(MX51_PAD_EIM_D25, 0x414, 0x080, 1, 0x9c8, 0), /* MX51_PAD_EIM_D25__KEY_COL6 */
-	IMX_PIN_REG(MX51_PAD_EIM_D25, 0x414, 0x080, 4, 0x000, 0), /* MX51_PAD_EIM_D25__UART2_CTS */
-	IMX_PIN_REG(MX51_PAD_EIM_D25, 0x414, 0x080, 3, 0x9f4, 0), /* MX51_PAD_EIM_D25__UART3_RXD */
-	IMX_PIN_REG(MX51_PAD_EIM_D25, 0x414, 0x080, 2, 0x000, 0), /* MX51_PAD_EIM_D25__USBOTG_DATA1 */
-	IMX_PIN_REG(MX51_PAD_EIM_D26, 0x418, 0x084, 0, 0x000, 0), /* MX51_PAD_EIM_D26__EIM_D26 */
-	IMX_PIN_REG(MX51_PAD_EIM_D26, 0x418, 0x084, 1, 0x9cc, 0), /* MX51_PAD_EIM_D26__KEY_COL7 */
-	IMX_PIN_REG(MX51_PAD_EIM_D26, 0x418, 0x084, 4, 0x9e8, 3), /* MX51_PAD_EIM_D26__UART2_RTS */
-	IMX_PIN_REG(MX51_PAD_EIM_D26, 0x418, 0x084, 3, 0x000, 0), /* MX51_PAD_EIM_D26__UART3_TXD */
-	IMX_PIN_REG(MX51_PAD_EIM_D26, 0x418, 0x084, 2, 0x000, 0), /* MX51_PAD_EIM_D26__USBOTG_DATA2 */
-	IMX_PIN_REG(MX51_PAD_EIM_D27, 0x41c, 0x088, 5, 0x8f4, 0), /* MX51_PAD_EIM_D27__AUD6_RXC */
-	IMX_PIN_REG(MX51_PAD_EIM_D27, 0x41c, 0x088, 0, 0x000, 0), /* MX51_PAD_EIM_D27__EIM_D27 */
-	IMX_PIN_REG(MX51_PAD_EIM_D27, 0x41c, 0x088, 1, 0x000, 0), /* MX51_PAD_EIM_D27__GPIO2_9 */
-	IMX_PIN_REG(MX51_PAD_EIM_D27, 0x41c, 0x088, 4, 0x9b8, 0), /* MX51_PAD_EIM_D27__I2C2_SCL */
-	IMX_PIN_REG(MX51_PAD_EIM_D27, 0x41c, 0x088, 3, 0x9f0, 3), /* MX51_PAD_EIM_D27__UART3_RTS */
-	IMX_PIN_REG(MX51_PAD_EIM_D27, 0x41c, 0x088, 2, 0x000, 0), /* MX51_PAD_EIM_D27__USBOTG_DATA3 */
-	IMX_PIN_REG(MX51_PAD_EIM_D28, 0x420, 0x08c, 5, 0x8f0, 0), /* MX51_PAD_EIM_D28__AUD6_TXD */
-	IMX_PIN_REG(MX51_PAD_EIM_D28, 0x420, 0x08c, 0, 0x000, 0), /* MX51_PAD_EIM_D28__EIM_D28 */
-	IMX_PIN_REG(MX51_PAD_EIM_D28, 0x420, 0x08c, 1, 0x9d0, 0), /* MX51_PAD_EIM_D28__KEY_ROW4 */
-	IMX_PIN_REG(MX51_PAD_EIM_D28, 0x420, 0x08c, 2, 0x000, 0), /* MX51_PAD_EIM_D28__USBOTG_DATA4 */
-	IMX_PIN_REG(MX51_PAD_EIM_D29, 0x424, 0x090, 5, 0x8ec, 0), /* MX51_PAD_EIM_D29__AUD6_RXD */
-	IMX_PIN_REG(MX51_PAD_EIM_D29, 0x424, 0x090, 0, 0x000, 0), /* MX51_PAD_EIM_D29__EIM_D29 */
-	IMX_PIN_REG(MX51_PAD_EIM_D29, 0x424, 0x090, 1, 0x9d4, 0), /* MX51_PAD_EIM_D29__KEY_ROW5 */
-	IMX_PIN_REG(MX51_PAD_EIM_D29, 0x424, 0x090, 2, 0x000, 0), /* MX51_PAD_EIM_D29__USBOTG_DATA5 */
-	IMX_PIN_REG(MX51_PAD_EIM_D30, 0x428, 0x094, 5, 0x8fc, 0), /* MX51_PAD_EIM_D30__AUD6_TXC */
-	IMX_PIN_REG(MX51_PAD_EIM_D30, 0x428, 0x094, 0, 0x000, 0), /* MX51_PAD_EIM_D30__EIM_D30 */
-	IMX_PIN_REG(MX51_PAD_EIM_D30, 0x428, 0x094, 1, 0x9d8, 0), /* MX51_PAD_EIM_D30__KEY_ROW6 */
-	IMX_PIN_REG(MX51_PAD_EIM_D30, 0x428, 0x094, 2, 0x000, 0), /* MX51_PAD_EIM_D30__USBOTG_DATA6 */
-	IMX_PIN_REG(MX51_PAD_EIM_D31, 0x42c, 0x098, 5, 0x900, 0), /* MX51_PAD_EIM_D31__AUD6_TXFS */
-	IMX_PIN_REG(MX51_PAD_EIM_D31, 0x42c, 0x098, 0, 0x000, 0), /* MX51_PAD_EIM_D31__EIM_D31 */
-	IMX_PIN_REG(MX51_PAD_EIM_D31, 0x42c, 0x098, 1, 0x9dc, 0), /* MX51_PAD_EIM_D31__KEY_ROW7 */
-	IMX_PIN_REG(MX51_PAD_EIM_D31, 0x42c, 0x098, 2, 0x000, 0), /* MX51_PAD_EIM_D31__USBOTG_DATA7 */
-	IMX_PIN_REG(MX51_PAD_EIM_A16, 0x430, 0x09c, 0, 0x000, 0), /* MX51_PAD_EIM_A16__EIM_A16 */
-	IMX_PIN_REG(MX51_PAD_EIM_A16, 0x430, 0x09c, 1, 0x000, 0), /* MX51_PAD_EIM_A16__GPIO2_10 */
-	IMX_PIN_REG(MX51_PAD_EIM_A16, 0x430, 0x09c, 7, 0x000, 0), /* MX51_PAD_EIM_A16__OSC_FREQ_SEL0 */
-	IMX_PIN_REG(MX51_PAD_EIM_A17, 0x434, 0x0a0, 0, 0x000, 0), /* MX51_PAD_EIM_A17__EIM_A17 */
-	IMX_PIN_REG(MX51_PAD_EIM_A17, 0x434, 0x0a0, 1, 0x000, 0), /* MX51_PAD_EIM_A17__GPIO2_11 */
-	IMX_PIN_REG(MX51_PAD_EIM_A17, 0x434, 0x0a0, 7, 0x000, 0), /* MX51_PAD_EIM_A17__OSC_FREQ_SEL1 */
-	IMX_PIN_REG(MX51_PAD_EIM_A18, 0x438, 0x0a4, 7, 0x000, 0), /* MX51_PAD_EIM_A18__BOOT_LPB0 */
-	IMX_PIN_REG(MX51_PAD_EIM_A18, 0x438, 0x0a4, 0, 0x000, 0), /* MX51_PAD_EIM_A18__EIM_A18 */
-	IMX_PIN_REG(MX51_PAD_EIM_A18, 0x438, 0x0a4, 1, 0x000, 0), /* MX51_PAD_EIM_A18__GPIO2_12 */
-	IMX_PIN_REG(MX51_PAD_EIM_A19, 0x43c, 0x0a8, 7, 0x000, 0), /* MX51_PAD_EIM_A19__BOOT_LPB1 */
-	IMX_PIN_REG(MX51_PAD_EIM_A19, 0x43c, 0x0a8, 0, 0x000, 0), /* MX51_PAD_EIM_A19__EIM_A19 */
-	IMX_PIN_REG(MX51_PAD_EIM_A19, 0x43c, 0x0a8, 1, 0x000, 0), /* MX51_PAD_EIM_A19__GPIO2_13 */
-	IMX_PIN_REG(MX51_PAD_EIM_A20, 0x440, 0x0ac, 7, 0x000, 0), /* MX51_PAD_EIM_A20__BOOT_UART_SRC0 */
-	IMX_PIN_REG(MX51_PAD_EIM_A20, 0x440, 0x0ac, 0, 0x000, 0), /* MX51_PAD_EIM_A20__EIM_A20 */
-	IMX_PIN_REG(MX51_PAD_EIM_A20, 0x440, 0x0ac, 1, 0x000, 0), /* MX51_PAD_EIM_A20__GPIO2_14 */
-	IMX_PIN_REG(MX51_PAD_EIM_A21, 0x444, 0x0b0, 7, 0x000, 0), /* MX51_PAD_EIM_A21__BOOT_UART_SRC1 */
-	IMX_PIN_REG(MX51_PAD_EIM_A21, 0x444, 0x0b0, 0, 0x000, 0), /* MX51_PAD_EIM_A21__EIM_A21 */
-	IMX_PIN_REG(MX51_PAD_EIM_A21, 0x444, 0x0b0, 1, 0x000, 0), /* MX51_PAD_EIM_A21__GPIO2_15 */
-	IMX_PIN_REG(MX51_PAD_EIM_A22, 0x448, 0x0b4, 0, 0x000, 0), /* MX51_PAD_EIM_A22__EIM_A22 */
-	IMX_PIN_REG(MX51_PAD_EIM_A22, 0x448, 0x0b4, 1, 0x000, 0), /* MX51_PAD_EIM_A22__GPIO2_16 */
-	IMX_PIN_REG(MX51_PAD_EIM_A23, 0x44c, 0x0b8, 7, 0x000, 0), /* MX51_PAD_EIM_A23__BOOT_HPN_EN */
-	IMX_PIN_REG(MX51_PAD_EIM_A23, 0x44c, 0x0b8, 0, 0x000, 0), /* MX51_PAD_EIM_A23__EIM_A23 */
-	IMX_PIN_REG(MX51_PAD_EIM_A23, 0x44c, 0x0b8, 1, 0x000, 0), /* MX51_PAD_EIM_A23__GPIO2_17 */
-	IMX_PIN_REG(MX51_PAD_EIM_A24, 0x450, 0x0bc, 0, 0x000, 0), /* MX51_PAD_EIM_A24__EIM_A24 */
-	IMX_PIN_REG(MX51_PAD_EIM_A24, 0x450, 0x0bc, 1, 0x000, 0), /* MX51_PAD_EIM_A24__GPIO2_18 */
-	IMX_PIN_REG(MX51_PAD_EIM_A24, 0x450, 0x0bc, 2, 0x000, 0), /* MX51_PAD_EIM_A24__USBH2_CLK */
-	IMX_PIN_REG(MX51_PAD_EIM_A25, 0x454, 0x0c0, 6, 0x000, 0), /* MX51_PAD_EIM_A25__DISP1_PIN4 */
-	IMX_PIN_REG(MX51_PAD_EIM_A25, 0x454, 0x0c0, 0, 0x000, 0), /* MX51_PAD_EIM_A25__EIM_A25 */
-	IMX_PIN_REG(MX51_PAD_EIM_A25, 0x454, 0x0c0, 1, 0x000, 0), /* MX51_PAD_EIM_A25__GPIO2_19 */
-	IMX_PIN_REG(MX51_PAD_EIM_A25, 0x454, 0x0c0, 2, 0x000, 0), /* MX51_PAD_EIM_A25__USBH2_DIR */
-	IMX_PIN_REG(MX51_PAD_EIM_A26, 0x458, 0x0c4, 5, 0x9a0, 0), /* MX51_PAD_EIM_A26__CSI1_DATA_EN */
-	IMX_PIN_REG(MX51_PAD_EIM_A26, 0x458, 0x0c4, 6, 0x908, 0), /* MX51_PAD_EIM_A26__DISP2_EXT_CLK */
-	IMX_PIN_REG(MX51_PAD_EIM_A26, 0x458, 0x0c4, 0, 0x000, 0), /* MX51_PAD_EIM_A26__EIM_A26 */
-	IMX_PIN_REG(MX51_PAD_EIM_A26, 0x458, 0x0c4, 1, 0x000, 0), /* MX51_PAD_EIM_A26__GPIO2_20 */
-	IMX_PIN_REG(MX51_PAD_EIM_A26, 0x458, 0x0c4, 2, 0x000, 0), /* MX51_PAD_EIM_A26__USBH2_STP */
-	IMX_PIN_REG(MX51_PAD_EIM_A27, 0x45c, 0x0c8, 5, 0x99c, 0), /* MX51_PAD_EIM_A27__CSI2_DATA_EN */
-	IMX_PIN_REG(MX51_PAD_EIM_A27, 0x45c, 0x0c8, 6, 0x9a4, 0), /* MX51_PAD_EIM_A27__DISP1_PIN1 */
-	IMX_PIN_REG(MX51_PAD_EIM_A27, 0x45c, 0x0c8, 0, 0x000, 0), /* MX51_PAD_EIM_A27__EIM_A27 */
-	IMX_PIN_REG(MX51_PAD_EIM_A27, 0x45c, 0x0c8, 1, 0x000, 0), /* MX51_PAD_EIM_A27__GPIO2_21 */
-	IMX_PIN_REG(MX51_PAD_EIM_A27, 0x45c, 0x0c8, 2, 0x000, 0), /* MX51_PAD_EIM_A27__USBH2_NXT */
-	IMX_PIN_REG(MX51_PAD_EIM_EB0, 0x460, 0x0cc, 0, 0x000, 0), /* MX51_PAD_EIM_EB0__EIM_EB0 */
-	IMX_PIN_REG(MX51_PAD_EIM_EB1, 0x464, 0x0d0, 0, 0x000, 0), /* MX51_PAD_EIM_EB1__EIM_EB1 */
-	IMX_PIN_REG(MX51_PAD_EIM_EB2, 0x468, 0x0d4, 6, 0x8e0, 0), /* MX51_PAD_EIM_EB2__AUD5_RXFS */
-	IMX_PIN_REG(MX51_PAD_EIM_EB2, 0x468, 0x0d4, 5, 0x000, 0), /* MX51_PAD_EIM_EB2__CSI1_D2 */
-	IMX_PIN_REG(MX51_PAD_EIM_EB2, 0x468, 0x0d4, 0, 0x000, 0), /* MX51_PAD_EIM_EB2__EIM_EB2 */
-	IMX_PIN_REG(MX51_PAD_EIM_EB2, 0x468, 0x0d4, 3, 0x954, 0), /* MX51_PAD_EIM_EB2__FEC_MDIO */
-	IMX_PIN_REG(MX51_PAD_EIM_EB2, 0x468, 0x0d4, 1, 0x000, 0), /* MX51_PAD_EIM_EB2__GPIO2_22 */
-	IMX_PIN_REG(MX51_PAD_EIM_EB2, 0x468, 0x0d4, 7, 0x000, 0), /* MX51_PAD_EIM_EB2__GPT_CMPOUT1 */
-	IMX_PIN_REG(MX51_PAD_EIM_EB3, 0x46c, 0x0d8, 6, 0x8dc, 0), /* MX51_PAD_EIM_EB3__AUD5_RXC */
-	IMX_PIN_REG(MX51_PAD_EIM_EB3, 0x46c, 0x0d8, 5, 0x000, 0), /* MX51_PAD_EIM_EB3__CSI1_D3 */
-	IMX_PIN_REG(MX51_PAD_EIM_EB3, 0x46c, 0x0d8, 0, 0x000, 0), /* MX51_PAD_EIM_EB3__EIM_EB3 */
-	IMX_PIN_REG(MX51_PAD_EIM_EB3, 0x46c, 0x0d8, 3, 0x95c, 0), /* MX51_PAD_EIM_EB3__FEC_RDATA1 */
-	IMX_PIN_REG(MX51_PAD_EIM_EB3, 0x46c, 0x0d8, 1, 0x000, 0), /* MX51_PAD_EIM_EB3__GPIO2_23 */
-	IMX_PIN_REG(MX51_PAD_EIM_EB3, 0x46c, 0x0d8, 7, 0x000, 0), /* MX51_PAD_EIM_EB3__GPT_CMPOUT2 */
-	IMX_PIN_REG(MX51_PAD_EIM_OE, 0x470, 0x0dc, 0, 0x000, 0), /* MX51_PAD_EIM_OE__EIM_OE */
-	IMX_PIN_REG(MX51_PAD_EIM_OE, 0x470, 0x0dc, 1, 0x000, 0), /* MX51_PAD_EIM_OE__GPIO2_24 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS0, 0x474, 0x0e0, 0, 0x000, 0), /* MX51_PAD_EIM_CS0__EIM_CS0 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS0, 0x474, 0x0e0, 1, 0x000, 0), /* MX51_PAD_EIM_CS0__GPIO2_25 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS1, 0x478, 0x0e4, 0, 0x000, 0), /* MX51_PAD_EIM_CS1__EIM_CS1 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS1, 0x478, 0x0e4, 1, 0x000, 0), /* MX51_PAD_EIM_CS1__GPIO2_26 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS2, 0x47c, 0x0e8, 6, 0x8d8, 1), /* MX51_PAD_EIM_CS2__AUD5_TXD */
-	IMX_PIN_REG(MX51_PAD_EIM_CS2, 0x47c, 0x0e8, 5, 0x000, 0), /* MX51_PAD_EIM_CS2__CSI1_D4 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS2, 0x47c, 0x0e8, 0, 0x000, 0), /* MX51_PAD_EIM_CS2__EIM_CS2 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS2, 0x47c, 0x0e8, 3, 0x960, 0), /* MX51_PAD_EIM_CS2__FEC_RDATA2 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS2, 0x47c, 0x0e8, 1, 0x000, 0), /* MX51_PAD_EIM_CS2__GPIO2_27 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS2, 0x47c, 0x0e8, 2, 0x000, 0), /* MX51_PAD_EIM_CS2__USBOTG_STP */
-	IMX_PIN_REG(MX51_PAD_EIM_CS3, 0x480, 0x0ec, 6, 0x8d4, 1), /* MX51_PAD_EIM_CS3__AUD5_RXD */
-	IMX_PIN_REG(MX51_PAD_EIM_CS3, 0x480, 0x0ec, 5, 0x000, 0), /* MX51_PAD_EIM_CS3__CSI1_D5 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS3, 0x480, 0x0ec, 0, 0x000, 0), /* MX51_PAD_EIM_CS3__EIM_CS3 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS3, 0x480, 0x0ec, 3, 0x964, 0), /* MX51_PAD_EIM_CS3__FEC_RDATA3 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS3, 0x480, 0x0ec, 1, 0x000, 0), /* MX51_PAD_EIM_CS3__GPIO2_28 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS3, 0x480, 0x0ec, 2, 0x000, 0), /* MX51_PAD_EIM_CS3__USBOTG_NXT */
-	IMX_PIN_REG(MX51_PAD_EIM_CS4, 0x484, 0x0f0, 6, 0x8e4, 1), /* MX51_PAD_EIM_CS4__AUD5_TXC */
-	IMX_PIN_REG(MX51_PAD_EIM_CS4, 0x484, 0x0f0, 5, 0x000, 0), /* MX51_PAD_EIM_CS4__CSI1_D6 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS4, 0x484, 0x0f0, 0, 0x000, 0), /* MX51_PAD_EIM_CS4__EIM_CS4 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS4, 0x484, 0x0f0, 3, 0x970, 0), /* MX51_PAD_EIM_CS4__FEC_RX_ER */
-	IMX_PIN_REG(MX51_PAD_EIM_CS4, 0x484, 0x0f0, 1, 0x000, 0), /* MX51_PAD_EIM_CS4__GPIO2_29 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS4, 0x484, 0x0f0, 2, 0x000, 0), /* MX51_PAD_EIM_CS4__USBOTG_CLK */
-	IMX_PIN_REG(MX51_PAD_EIM_CS5, 0x488, 0x0f4, 6, 0x8e8, 1), /* MX51_PAD_EIM_CS5__AUD5_TXFS */
-	IMX_PIN_REG(MX51_PAD_EIM_CS5, 0x488, 0x0f4, 5, 0x000, 0), /* MX51_PAD_EIM_CS5__CSI1_D7 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS5, 0x488, 0x0f4, 4, 0x904, 0), /* MX51_PAD_EIM_CS5__DISP1_EXT_CLK */
-	IMX_PIN_REG(MX51_PAD_EIM_CS5, 0x488, 0x0f4, 0, 0x000, 0), /* MX51_PAD_EIM_CS5__EIM_CS5 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS5, 0x488, 0x0f4, 3, 0x950, 0), /* MX51_PAD_EIM_CS5__FEC_CRS */
-	IMX_PIN_REG(MX51_PAD_EIM_CS5, 0x488, 0x0f4, 1, 0x000, 0), /* MX51_PAD_EIM_CS5__GPIO2_30 */
-	IMX_PIN_REG(MX51_PAD_EIM_CS5, 0x488, 0x0f4, 2, 0x000, 0), /* MX51_PAD_EIM_CS5__USBOTG_DIR */
-	IMX_PIN_REG(MX51_PAD_EIM_DTACK, 0x48c, 0x0f8, 0, 0x000, 0), /* MX51_PAD_EIM_DTACK__EIM_DTACK */
-	IMX_PIN_REG(MX51_PAD_EIM_DTACK, 0x48c, 0x0f8, 1, 0x000, 0), /* MX51_PAD_EIM_DTACK__GPIO2_31 */
-	IMX_PIN_REG(MX51_PAD_EIM_LBA, 0x494, 0x0fc, 0, 0x000, 0), /* MX51_PAD_EIM_LBA__EIM_LBA */
-	IMX_PIN_REG(MX51_PAD_EIM_LBA, 0x494, 0x0fc, 1, 0x978, 0), /* MX51_PAD_EIM_LBA__GPIO3_1 */
-	IMX_PIN_REG(MX51_PAD_EIM_CRE, 0x4a0, 0x100, 0, 0x000, 0), /* MX51_PAD_EIM_CRE__EIM_CRE */
-	IMX_PIN_REG(MX51_PAD_EIM_CRE, 0x4a0, 0x100, 1, 0x97c, 0), /* MX51_PAD_EIM_CRE__GPIO3_2 */
-	IMX_PIN_REG(MX51_PAD_DRAM_CS1, 0x4d0, 0x104, 0, 0x000, 0), /* MX51_PAD_DRAM_CS1__DRAM_CS1 */
-	IMX_PIN_REG(MX51_PAD_NANDF_WE_B, 0x4e4, 0x108, 3, 0x980, 0), /* MX51_PAD_NANDF_WE_B__GPIO3_3 */
-	IMX_PIN_REG(MX51_PAD_NANDF_WE_B, 0x4e4, 0x108, 0, 0x000, 0), /* MX51_PAD_NANDF_WE_B__NANDF_WE_B */
-	IMX_PIN_REG(MX51_PAD_NANDF_WE_B, 0x4e4, 0x108, 1, 0x000, 0), /* MX51_PAD_NANDF_WE_B__PATA_DIOW */
-	IMX_PIN_REG(MX51_PAD_NANDF_WE_B, 0x4e4, 0x108, 2, 0x93c, 0), /* MX51_PAD_NANDF_WE_B__SD3_DATA0 */
-	IMX_PIN_REG(MX51_PAD_NANDF_RE_B, 0x4e8, 0x10c, 3, 0x984, 0), /* MX51_PAD_NANDF_RE_B__GPIO3_4 */
-	IMX_PIN_REG(MX51_PAD_NANDF_RE_B, 0x4e8, 0x10c, 0, 0x000, 0), /* MX51_PAD_NANDF_RE_B__NANDF_RE_B */
-	IMX_PIN_REG(MX51_PAD_NANDF_RE_B, 0x4e8, 0x10c, 1, 0x000, 0), /* MX51_PAD_NANDF_RE_B__PATA_DIOR */
-	IMX_PIN_REG(MX51_PAD_NANDF_RE_B, 0x4e8, 0x10c, 2, 0x940, 0), /* MX51_PAD_NANDF_RE_B__SD3_DATA1 */
-	IMX_PIN_REG(MX51_PAD_NANDF_ALE, 0x4ec, 0x110, 3, 0x988, 0), /* MX51_PAD_NANDF_ALE__GPIO3_5 */
-	IMX_PIN_REG(MX51_PAD_NANDF_ALE, 0x4ec, 0x110, 0, 0x000, 0), /* MX51_PAD_NANDF_ALE__NANDF_ALE */
-	IMX_PIN_REG(MX51_PAD_NANDF_ALE, 0x4ec, 0x110, 1, 0x000, 0), /* MX51_PAD_NANDF_ALE__PATA_BUFFER_EN */
-	IMX_PIN_REG(MX51_PAD_NANDF_CLE, 0x4f0, 0x114, 3, 0x98c, 0), /* MX51_PAD_NANDF_CLE__GPIO3_6 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CLE, 0x4f0, 0x114, 0, 0x000, 0), /* MX51_PAD_NANDF_CLE__NANDF_CLE */
-	IMX_PIN_REG(MX51_PAD_NANDF_CLE, 0x4f0, 0x114, 1, 0x000, 0), /* MX51_PAD_NANDF_CLE__PATA_RESET_B */
-	IMX_PIN_REG(MX51_PAD_NANDF_WP_B, 0x4f4, 0x118, 3, 0x990, 0), /* MX51_PAD_NANDF_WP_B__GPIO3_7 */
-	IMX_PIN_REG(MX51_PAD_NANDF_WP_B, 0x4f4, 0x118, 0, 0x000, 0), /* MX51_PAD_NANDF_WP_B__NANDF_WP_B */
-	IMX_PIN_REG(MX51_PAD_NANDF_WP_B, 0x4f4, 0x118, 1, 0x000, 0), /* MX51_PAD_NANDF_WP_B__PATA_DMACK */
-	IMX_PIN_REG(MX51_PAD_NANDF_WP_B, 0x4f4, 0x118, 2, 0x944, 0), /* MX51_PAD_NANDF_WP_B__SD3_DATA2 */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB0, 0x4f8, 0x11c, 5, 0x930, 0), /* MX51_PAD_NANDF_RB0__ECSPI2_SS1 */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB0, 0x4f8, 0x11c, 3, 0x994, 0), /* MX51_PAD_NANDF_RB0__GPIO3_8 */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB0, 0x4f8, 0x11c, 0, 0x000, 0), /* MX51_PAD_NANDF_RB0__NANDF_RB0 */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB0, 0x4f8, 0x11c, 1, 0x000, 0), /* MX51_PAD_NANDF_RB0__PATA_DMARQ */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB0, 0x4f8, 0x11c, 2, 0x948, 0), /* MX51_PAD_NANDF_RB0__SD3_DATA3 */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB1, 0x4fc, 0x120, 6, 0x91c, 0), /* MX51_PAD_NANDF_RB1__CSPI_MOSI */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB1, 0x4fc, 0x120, 2, 0x000, 0), /* MX51_PAD_NANDF_RB1__ECSPI2_RDY */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB1, 0x4fc, 0x120, 3, 0x000, 0), /* MX51_PAD_NANDF_RB1__GPIO3_9 */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB1, 0x4fc, 0x120, 0, 0x000, 0), /* MX51_PAD_NANDF_RB1__NANDF_RB1 */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB1, 0x4fc, 0x120, 1, 0x000, 0), /* MX51_PAD_NANDF_RB1__PATA_IORDY */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB1, 0x4fc, 0x120, 5, 0x000, 0), /* MX51_PAD_NANDF_RB1__SD4_CMD */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB2, 0x500, 0x124, 5, 0x9a8, 0), /* MX51_PAD_NANDF_RB2__DISP2_WAIT */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB2, 0x500, 0x124, 2, 0x000, 0), /* MX51_PAD_NANDF_RB2__ECSPI2_SCLK */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB2, 0x500, 0x124, 1, 0x94c, 0), /* MX51_PAD_NANDF_RB2__FEC_COL */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB2, 0x500, 0x124, 3, 0x000, 0), /* MX51_PAD_NANDF_RB2__GPIO3_10 */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB2, 0x500, 0x124, 0, 0x000, 0), /* MX51_PAD_NANDF_RB2__NANDF_RB2 */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB2, 0x500, 0x124, 7, 0x000, 0), /* MX51_PAD_NANDF_RB2__USBH3_H3_DP */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB2, 0x500, 0x124, 6, 0xa20, 0), /* MX51_PAD_NANDF_RB2__USBH3_NXT */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB3, 0x504, 0x128, 5, 0x000, 0), /* MX51_PAD_NANDF_RB3__DISP1_WAIT */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB3, 0x504, 0x128, 2, 0x000, 0), /* MX51_PAD_NANDF_RB3__ECSPI2_MISO */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB3, 0x504, 0x128, 1, 0x968, 0), /* MX51_PAD_NANDF_RB3__FEC_RX_CLK */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB3, 0x504, 0x128, 3, 0x000, 0), /* MX51_PAD_NANDF_RB3__GPIO3_11 */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB3, 0x504, 0x128, 0, 0x000, 0), /* MX51_PAD_NANDF_RB3__NANDF_RB3 */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB3, 0x504, 0x128, 6, 0x9f8, 0), /* MX51_PAD_NANDF_RB3__USBH3_CLK */
-	IMX_PIN_REG(MX51_PAD_NANDF_RB3, 0x504, 0x128, 7, 0x000, 0), /* MX51_PAD_NANDF_RB3__USBH3_H3_DM */
-	IMX_PIN_REG(MX51_PAD_GPIO_NAND, 0x514, 0x12c, 0, 0x998, 0), /* MX51_PAD_GPIO_NAND__GPIO_NAND */
-	IMX_PIN_REG(MX51_PAD_GPIO_NAND, 0x514, 0x12c, 1, 0x000, 0), /* MX51_PAD_GPIO_NAND__PATA_INTRQ */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS0, 0x518, 0x130, 3, 0x000, 0), /* MX51_PAD_NANDF_CS0__GPIO3_16 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS0, 0x518, 0x130, 0, 0x000, 0), /* MX51_PAD_NANDF_CS0__NANDF_CS0 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS1, 0x51c, 0x134, 3, 0x000, 0), /* MX51_PAD_NANDF_CS1__GPIO3_17 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS1, 0x51c, 0x134, 0, 0x000, 0), /* MX51_PAD_NANDF_CS1__NANDF_CS1 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS2, 0x520, 0x138, 6, 0x914, 0), /* MX51_PAD_NANDF_CS2__CSPI_SCLK */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS2, 0x520, 0x138, 2, 0x000, 0), /* MX51_PAD_NANDF_CS2__FEC_TX_ER */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS2, 0x520, 0x138, 3, 0x000, 0), /* MX51_PAD_NANDF_CS2__GPIO3_18 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS2, 0x520, 0x138, 0, 0x000, 0), /* MX51_PAD_NANDF_CS2__NANDF_CS2 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS2, 0x520, 0x138, 1, 0x000, 0), /* MX51_PAD_NANDF_CS2__PATA_CS_0 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS2, 0x520, 0x138, 5, 0x000, 0), /* MX51_PAD_NANDF_CS2__SD4_CLK */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS2, 0x520, 0x138, 7, 0x000, 0), /* MX51_PAD_NANDF_CS2__USBH3_H1_DP */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS3, 0x524, 0x13c, 2, 0x000, 0), /* MX51_PAD_NANDF_CS3__FEC_MDC */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS3, 0x524, 0x13c, 3, 0x000, 0), /* MX51_PAD_NANDF_CS3__GPIO3_19 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS3, 0x524, 0x13c, 0, 0x000, 0), /* MX51_PAD_NANDF_CS3__NANDF_CS3 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS3, 0x524, 0x13c, 1, 0x000, 0), /* MX51_PAD_NANDF_CS3__PATA_CS_1 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS3, 0x524, 0x13c, 5, 0x000, 0), /* MX51_PAD_NANDF_CS3__SD4_DAT0 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS3, 0x524, 0x13c, 7, 0x000, 0), /* MX51_PAD_NANDF_CS3__USBH3_H1_DM */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS4, 0x528, 0x140, 2, 0x000, 0), /* MX51_PAD_NANDF_CS4__FEC_TDATA1 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS4, 0x528, 0x140, 3, 0x000, 0), /* MX51_PAD_NANDF_CS4__GPIO3_20 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS4, 0x528, 0x140, 0, 0x000, 0), /* MX51_PAD_NANDF_CS4__NANDF_CS4 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS4, 0x528, 0x140, 1, 0x000, 0), /* MX51_PAD_NANDF_CS4__PATA_DA_0 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS4, 0x528, 0x140, 5, 0x000, 0), /* MX51_PAD_NANDF_CS4__SD4_DAT1 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS4, 0x528, 0x140, 7, 0xa24, 0), /* MX51_PAD_NANDF_CS4__USBH3_STP */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS5, 0x52c, 0x144, 2, 0x000, 0), /* MX51_PAD_NANDF_CS5__FEC_TDATA2 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS5, 0x52c, 0x144, 3, 0x000, 0), /* MX51_PAD_NANDF_CS5__GPIO3_21 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS5, 0x52c, 0x144, 0, 0x000, 0), /* MX51_PAD_NANDF_CS5__NANDF_CS5 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS5, 0x52c, 0x144, 1, 0x000, 0), /* MX51_PAD_NANDF_CS5__PATA_DA_1 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS5, 0x52c, 0x144, 5, 0x000, 0), /* MX51_PAD_NANDF_CS5__SD4_DAT2 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS5, 0x52c, 0x144, 7, 0xa1c, 0), /* MX51_PAD_NANDF_CS5__USBH3_DIR */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS6, 0x530, 0x148, 7, 0x928, 0), /* MX51_PAD_NANDF_CS6__CSPI_SS3 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS6, 0x530, 0x148, 2, 0x000, 0), /* MX51_PAD_NANDF_CS6__FEC_TDATA3 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS6, 0x530, 0x148, 3, 0x000, 0), /* MX51_PAD_NANDF_CS6__GPIO3_22 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS6, 0x530, 0x148, 0, 0x000, 0), /* MX51_PAD_NANDF_CS6__NANDF_CS6 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS6, 0x530, 0x148, 1, 0x000, 0), /* MX51_PAD_NANDF_CS6__PATA_DA_2 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS6, 0x530, 0x148, 5, 0x000, 0), /* MX51_PAD_NANDF_CS6__SD4_DAT3 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS7, 0x534, 0x14c, 1, 0x000, 0), /* MX51_PAD_NANDF_CS7__FEC_TX_EN */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS7, 0x534, 0x14c, 3, 0x000, 0), /* MX51_PAD_NANDF_CS7__GPIO3_23 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS7, 0x534, 0x14c, 0, 0x000, 0), /* MX51_PAD_NANDF_CS7__NANDF_CS7 */
-	IMX_PIN_REG(MX51_PAD_NANDF_CS7, 0x534, 0x14c, 5, 0x000, 0), /* MX51_PAD_NANDF_CS7__SD3_CLK */
-	IMX_PIN_REG(MX51_PAD_NANDF_RDY_INT, 0x538, 0x150, 2, 0x000, 0), /* MX51_PAD_NANDF_RDY_INT__ECSPI2_SS0 */
-	IMX_PIN_REG(MX51_PAD_NANDF_RDY_INT, 0x538, 0x150, 1, 0x974, 0), /* MX51_PAD_NANDF_RDY_INT__FEC_TX_CLK */
-	IMX_PIN_REG(MX51_PAD_NANDF_RDY_INT, 0x538, 0x150, 3, 0x000, 0), /* MX51_PAD_NANDF_RDY_INT__GPIO3_24 */
-	IMX_PIN_REG(MX51_PAD_NANDF_RDY_INT, 0x538, 0x150, 0, 0x938, 0), /* MX51_PAD_NANDF_RDY_INT__NANDF_RDY_INT */
-	IMX_PIN_REG(MX51_PAD_NANDF_RDY_INT, 0x538, 0x150, 5, 0x000, 0), /* MX51_PAD_NANDF_RDY_INT__SD3_CMD */
-	IMX_PIN_REG(MX51_PAD_NANDF_D15, 0x53c, 0x154, 2, 0x000, 0), /* MX51_PAD_NANDF_D15__ECSPI2_MOSI */
-	IMX_PIN_REG(MX51_PAD_NANDF_D15, 0x53c, 0x154, 3, 0x000, 0), /* MX51_PAD_NANDF_D15__GPIO3_25 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D15, 0x53c, 0x154, 0, 0x000, 0), /* MX51_PAD_NANDF_D15__NANDF_D15 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D15, 0x53c, 0x154, 1, 0x000, 0), /* MX51_PAD_NANDF_D15__PATA_DATA15 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D15, 0x53c, 0x154, 5, 0x000, 0), /* MX51_PAD_NANDF_D15__SD3_DAT7 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D14, 0x540, 0x158, 2, 0x934, 0), /* MX51_PAD_NANDF_D14__ECSPI2_SS3 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D14, 0x540, 0x158, 3, 0x000, 0), /* MX51_PAD_NANDF_D14__GPIO3_26 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D14, 0x540, 0x158, 0, 0x000, 0), /* MX51_PAD_NANDF_D14__NANDF_D14 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D14, 0x540, 0x158, 1, 0x000, 0), /* MX51_PAD_NANDF_D14__PATA_DATA14 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D14, 0x540, 0x158, 5, 0x000, 0), /* MX51_PAD_NANDF_D14__SD3_DAT6 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D13, 0x544, 0x15c, 2, 0x000, 0), /* MX51_PAD_NANDF_D13__ECSPI2_SS2 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D13, 0x544, 0x15c, 3, 0x000, 0), /* MX51_PAD_NANDF_D13__GPIO3_27 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D13, 0x544, 0x15c, 0, 0x000, 0), /* MX51_PAD_NANDF_D13__NANDF_D13 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D13, 0x544, 0x15c, 1, 0x000, 0), /* MX51_PAD_NANDF_D13__PATA_DATA13 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D13, 0x544, 0x15c, 5, 0x000, 0), /* MX51_PAD_NANDF_D13__SD3_DAT5 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D12, 0x548, 0x160, 2, 0x930, 1), /* MX51_PAD_NANDF_D12__ECSPI2_SS1 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D12, 0x548, 0x160, 3, 0x000, 0), /* MX51_PAD_NANDF_D12__GPIO3_28 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D12, 0x548, 0x160, 0, 0x000, 0), /* MX51_PAD_NANDF_D12__NANDF_D12 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D12, 0x548, 0x160, 1, 0x000, 0), /* MX51_PAD_NANDF_D12__PATA_DATA12 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D12, 0x548, 0x160, 5, 0x000, 0), /* MX51_PAD_NANDF_D12__SD3_DAT4 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D11, 0x54c, 0x164, 2, 0x96c, 0), /* MX51_PAD_NANDF_D11__FEC_RX_DV */
-	IMX_PIN_REG(MX51_PAD_NANDF_D11, 0x54c, 0x164, 3, 0x000, 0), /* MX51_PAD_NANDF_D11__GPIO3_29 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D11, 0x54c, 0x164, 0, 0x000, 0), /* MX51_PAD_NANDF_D11__NANDF_D11 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D11, 0x54c, 0x164, 1, 0x000, 0), /* MX51_PAD_NANDF_D11__PATA_DATA11 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D11, 0x54c, 0x164, 5, 0x948, 1), /* MX51_PAD_NANDF_D11__SD3_DATA3 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D10, 0x550, 0x168, 3, 0x000, 0), /* MX51_PAD_NANDF_D10__GPIO3_30 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D10, 0x550, 0x168, 0, 0x000, 0), /* MX51_PAD_NANDF_D10__NANDF_D10 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D10, 0x550, 0x168, 1, 0x000, 0), /* MX51_PAD_NANDF_D10__PATA_DATA10 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D10, 0x550, 0x168, 5, 0x944, 1), /* MX51_PAD_NANDF_D10__SD3_DATA2 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D9, 0x554, 0x16c, 2, 0x958, 0), /* MX51_PAD_NANDF_D9__FEC_RDATA0 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D9, 0x554, 0x16c, 3, 0x000, 0), /* MX51_PAD_NANDF_D9__GPIO3_31 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D9, 0x554, 0x16c, 0, 0x000, 0), /* MX51_PAD_NANDF_D9__NANDF_D9 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D9, 0x554, 0x16c, 1, 0x000, 0), /* MX51_PAD_NANDF_D9__PATA_DATA9 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D9, 0x554, 0x16c, 5, 0x940, 1), /* MX51_PAD_NANDF_D9__SD3_DATA1 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D8, 0x558, 0x170, 2, 0x000, 0), /* MX51_PAD_NANDF_D8__FEC_TDATA0 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D8, 0x558, 0x170, 3, 0x000, 0), /* MX51_PAD_NANDF_D8__GPIO4_0 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D8, 0x558, 0x170, 0, 0x000, 0), /* MX51_PAD_NANDF_D8__NANDF_D8 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D8, 0x558, 0x170, 1, 0x000, 0), /* MX51_PAD_NANDF_D8__PATA_DATA8 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D8, 0x558, 0x170, 5, 0x93c, 1), /* MX51_PAD_NANDF_D8__SD3_DATA0 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D7, 0x55c, 0x174, 3, 0x000, 0), /* MX51_PAD_NANDF_D7__GPIO4_1 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D7, 0x55c, 0x174, 0, 0x000, 0), /* MX51_PAD_NANDF_D7__NANDF_D7 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D7, 0x55c, 0x174, 1, 0x000, 0), /* MX51_PAD_NANDF_D7__PATA_DATA7 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D7, 0x55c, 0x174, 5, 0x9fc, 0), /* MX51_PAD_NANDF_D7__USBH3_DATA0 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D6, 0x560, 0x178, 3, 0x000, 0), /* MX51_PAD_NANDF_D6__GPIO4_2 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D6, 0x560, 0x178, 0, 0x000, 0), /* MX51_PAD_NANDF_D6__NANDF_D6 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D6, 0x560, 0x178, 1, 0x000, 0), /* MX51_PAD_NANDF_D6__PATA_DATA6 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D6, 0x560, 0x178, 2, 0x000, 0), /* MX51_PAD_NANDF_D6__SD4_LCTL */
-	IMX_PIN_REG(MX51_PAD_NANDF_D6, 0x560, 0x178, 5, 0xa00, 0), /* MX51_PAD_NANDF_D6__USBH3_DATA1 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D5, 0x564, 0x17c, 3, 0x000, 0), /* MX51_PAD_NANDF_D5__GPIO4_3 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D5, 0x564, 0x17c, 0, 0x000, 0), /* MX51_PAD_NANDF_D5__NANDF_D5 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D5, 0x564, 0x17c, 1, 0x000, 0), /* MX51_PAD_NANDF_D5__PATA_DATA5 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D5, 0x564, 0x17c, 2, 0x000, 0), /* MX51_PAD_NANDF_D5__SD4_WP */
-	IMX_PIN_REG(MX51_PAD_NANDF_D5, 0x564, 0x17c, 5, 0xa04, 0), /* MX51_PAD_NANDF_D5__USBH3_DATA2 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D4, 0x568, 0x180, 3, 0x000, 0), /* MX51_PAD_NANDF_D4__GPIO4_4 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D4, 0x568, 0x180, 0, 0x000, 0), /* MX51_PAD_NANDF_D4__NANDF_D4 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D4, 0x568, 0x180, 1, 0x000, 0), /* MX51_PAD_NANDF_D4__PATA_DATA4 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D4, 0x568, 0x180, 2, 0x000, 0), /* MX51_PAD_NANDF_D4__SD4_CD */
-	IMX_PIN_REG(MX51_PAD_NANDF_D4, 0x568, 0x180, 5, 0xa08, 0), /* MX51_PAD_NANDF_D4__USBH3_DATA3 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D3, 0x56c, 0x184, 3, 0x000, 0), /* MX51_PAD_NANDF_D3__GPIO4_5 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D3, 0x56c, 0x184, 0, 0x000, 0), /* MX51_PAD_NANDF_D3__NANDF_D3 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D3, 0x56c, 0x184, 1, 0x000, 0), /* MX51_PAD_NANDF_D3__PATA_DATA3 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D3, 0x56c, 0x184, 2, 0x000, 0), /* MX51_PAD_NANDF_D3__SD4_DAT4 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D3, 0x56c, 0x184, 5, 0xa0c, 0), /* MX51_PAD_NANDF_D3__USBH3_DATA4 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D2, 0x570, 0x188, 3, 0x000, 0), /* MX51_PAD_NANDF_D2__GPIO4_6 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D2, 0x570, 0x188, 0, 0x000, 0), /* MX51_PAD_NANDF_D2__NANDF_D2 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D2, 0x570, 0x188, 1, 0x000, 0), /* MX51_PAD_NANDF_D2__PATA_DATA2 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D2, 0x570, 0x188, 2, 0x000, 0), /* MX51_PAD_NANDF_D2__SD4_DAT5 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D2, 0x570, 0x188, 5, 0xa10, 0), /* MX51_PAD_NANDF_D2__USBH3_DATA5 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D1, 0x574, 0x18c, 3, 0x000, 0), /* MX51_PAD_NANDF_D1__GPIO4_7 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D1, 0x574, 0x18c, 0, 0x000, 0), /* MX51_PAD_NANDF_D1__NANDF_D1 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D1, 0x574, 0x18c, 1, 0x000, 0), /* MX51_PAD_NANDF_D1__PATA_DATA1 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D1, 0x574, 0x18c, 2, 0x000, 0), /* MX51_PAD_NANDF_D1__SD4_DAT6 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D1, 0x574, 0x18c, 5, 0xa14, 0), /* MX51_PAD_NANDF_D1__USBH3_DATA6 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D0, 0x578, 0x190, 3, 0x000, 0), /* MX51_PAD_NANDF_D0__GPIO4_8 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D0, 0x578, 0x190, 0, 0x000, 0), /* MX51_PAD_NANDF_D0__NANDF_D0 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D0, 0x578, 0x190, 1, 0x000, 0), /* MX51_PAD_NANDF_D0__PATA_DATA0 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D0, 0x578, 0x190, 2, 0x000, 0), /* MX51_PAD_NANDF_D0__SD4_DAT7 */
-	IMX_PIN_REG(MX51_PAD_NANDF_D0, 0x578, 0x190, 5, 0xa18, 0), /* MX51_PAD_NANDF_D0__USBH3_DATA7 */
-	IMX_PIN_REG(MX51_PAD_CSI1_D8, 0x57c, 0x194, 0, 0x000, 0), /* MX51_PAD_CSI1_D8__CSI1_D8 */
-	IMX_PIN_REG(MX51_PAD_CSI1_D8, 0x57c, 0x194, 3, 0x998, 1), /* MX51_PAD_CSI1_D8__GPIO3_12 */
-	IMX_PIN_REG(MX51_PAD_CSI1_D9, 0x580, 0x198, 0, 0x000, 0), /* MX51_PAD_CSI1_D9__CSI1_D9 */
-	IMX_PIN_REG(MX51_PAD_CSI1_D9, 0x580, 0x198, 3, 0x000, 0), /* MX51_PAD_CSI1_D9__GPIO3_13 */
-	IMX_PIN_REG(MX51_PAD_CSI1_D10, 0x584, 0x19c, 0, 0x000, 0), /* MX51_PAD_CSI1_D10__CSI1_D10 */
-	IMX_PIN_REG(MX51_PAD_CSI1_D11, 0x588, 0x1a0, 0, 0x000, 0), /* MX51_PAD_CSI1_D11__CSI1_D11 */
-	IMX_PIN_REG(MX51_PAD_CSI1_D12, 0x58c, 0x1a4, 0, 0x000, 0), /* MX51_PAD_CSI1_D12__CSI1_D12 */
-	IMX_PIN_REG(MX51_PAD_CSI1_D13, 0x590, 0x1a8, 0, 0x000, 0), /* MX51_PAD_CSI1_D13__CSI1_D13 */
-	IMX_PIN_REG(MX51_PAD_CSI1_D14, 0x594, 0x1ac, 0, 0x000, 0), /* MX51_PAD_CSI1_D14__CSI1_D14 */
-	IMX_PIN_REG(MX51_PAD_CSI1_D15, 0x598, 0x1b0, 0, 0x000, 0), /* MX51_PAD_CSI1_D15__CSI1_D15 */
-	IMX_PIN_REG(MX51_PAD_CSI1_D16, 0x59c, 0x1b4, 0, 0x000, 0), /* MX51_PAD_CSI1_D16__CSI1_D16 */
-	IMX_PIN_REG(MX51_PAD_CSI1_D17, 0x5a0, 0x1b8, 0, 0x000, 0), /* MX51_PAD_CSI1_D17__CSI1_D17 */
-	IMX_PIN_REG(MX51_PAD_CSI1_D18, 0x5a4, 0x1bc, 0, 0x000, 0), /* MX51_PAD_CSI1_D18__CSI1_D18 */
-	IMX_PIN_REG(MX51_PAD_CSI1_D19, 0x5a8, 0x1c0, 0, 0x000, 0), /* MX51_PAD_CSI1_D19__CSI1_D19 */
-	IMX_PIN_REG(MX51_PAD_CSI1_VSYNC, 0x5ac, 0x1c4, 0, 0x000, 0), /* MX51_PAD_CSI1_VSYNC__CSI1_VSYNC */
-	IMX_PIN_REG(MX51_PAD_CSI1_VSYNC, 0x5ac, 0x1c4, 3, 0x000, 0), /* MX51_PAD_CSI1_VSYNC__GPIO3_14 */
-	IMX_PIN_REG(MX51_PAD_CSI1_HSYNC, 0x5b0, 0x1c8, 0, 0x000, 0), /* MX51_PAD_CSI1_HSYNC__CSI1_HSYNC */
-	IMX_PIN_REG(MX51_PAD_CSI1_HSYNC, 0x5b0, 0x1c8, 3, 0x000, 0), /* MX51_PAD_CSI1_HSYNC__GPIO3_15 */
-	IMX_PIN_REG(MX51_PAD_CSI1_PIXCLK, 0x5b4, NO_MUX, 0, 0x000, 0), /* MX51_PAD_CSI1_PIXCLK__CSI1_PIXCLK */
-	IMX_PIN_REG(MX51_PAD_CSI1_MCLK, 0x5b8, NO_MUX, 0, 0x000, 0), /* MX51_PAD_CSI1_MCLK__CSI1_MCLK */
-	IMX_PIN_REG(MX51_PAD_CSI2_D12, 0x5bc, 0x1cc, 0, 0x000, 0), /* MX51_PAD_CSI2_D12__CSI2_D12 */
-	IMX_PIN_REG(MX51_PAD_CSI2_D12, 0x5bc, 0x1cc, 3, 0x000, 0), /* MX51_PAD_CSI2_D12__GPIO4_9 */
-	IMX_PIN_REG(MX51_PAD_CSI2_D13, 0x5c0, 0x1d0, 0, 0x000, 0), /* MX51_PAD_CSI2_D13__CSI2_D13 */
-	IMX_PIN_REG(MX51_PAD_CSI2_D13, 0x5c0, 0x1d0, 3, 0x000, 0), /* MX51_PAD_CSI2_D13__GPIO4_10 */
-	IMX_PIN_REG(MX51_PAD_CSI2_D14, 0x5c4, 0x1d4, 0, 0x000, 0), /* MX51_PAD_CSI2_D14__CSI2_D14 */
-	IMX_PIN_REG(MX51_PAD_CSI2_D15, 0x5c8, 0x1d8, 0, 0x000, 0), /* MX51_PAD_CSI2_D15__CSI2_D15 */
-	IMX_PIN_REG(MX51_PAD_CSI2_D16, 0x5cc, 0x1dc, 0, 0x000, 0), /* MX51_PAD_CSI2_D16__CSI2_D16 */
-	IMX_PIN_REG(MX51_PAD_CSI2_D17, 0x5d0, 0x1e0, 0, 0x000, 0), /* MX51_PAD_CSI2_D17__CSI2_D17 */
-	IMX_PIN_REG(MX51_PAD_CSI2_D18, 0x5d4, 0x1e4, 0, 0x000, 0), /* MX51_PAD_CSI2_D18__CSI2_D18 */
-	IMX_PIN_REG(MX51_PAD_CSI2_D18, 0x5d4, 0x1e4, 3, 0x000, 0), /* MX51_PAD_CSI2_D18__GPIO4_11 */
-	IMX_PIN_REG(MX51_PAD_CSI2_D19, 0x5d8, 0x1e8, 0, 0x000, 0), /* MX51_PAD_CSI2_D19__CSI2_D19 */
-	IMX_PIN_REG(MX51_PAD_CSI2_D19, 0x5d8, 0x1e8, 3, 0x000, 0), /* MX51_PAD_CSI2_D19__GPIO4_12 */
-	IMX_PIN_REG(MX51_PAD_CSI2_VSYNC, 0x5dc, 0x1ec, 0, 0x000, 0), /* MX51_PAD_CSI2_VSYNC__CSI2_VSYNC */
-	IMX_PIN_REG(MX51_PAD_CSI2_VSYNC, 0x5dc, 0x1ec, 3, 0x000, 0), /* MX51_PAD_CSI2_VSYNC__GPIO4_13 */
-	IMX_PIN_REG(MX51_PAD_CSI2_HSYNC, 0x5e0, 0x1f0, 0, 0x000, 0), /* MX51_PAD_CSI2_HSYNC__CSI2_HSYNC */
-	IMX_PIN_REG(MX51_PAD_CSI2_HSYNC, 0x5e0, 0x1f0, 3, 0x000, 0), /* MX51_PAD_CSI2_HSYNC__GPIO4_14 */
-	IMX_PIN_REG(MX51_PAD_CSI2_PIXCLK, 0x5e4, 0x1f4, 0, 0x000, 0), /* MX51_PAD_CSI2_PIXCLK__CSI2_PIXCLK */
-	IMX_PIN_REG(MX51_PAD_CSI2_PIXCLK, 0x5e4, 0x1f4, 3, 0x000, 0), /* MX51_PAD_CSI2_PIXCLK__GPIO4_15 */
-	IMX_PIN_REG(MX51_PAD_I2C1_CLK, 0x5e8, 0x1f8, 3, 0x000, 0), /* MX51_PAD_I2C1_CLK__GPIO4_16 */
-	IMX_PIN_REG(MX51_PAD_I2C1_CLK, 0x5e8, 0x1f8, 0, 0x000, 0), /* MX51_PAD_I2C1_CLK__I2C1_CLK */
-	IMX_PIN_REG(MX51_PAD_I2C1_DAT, 0x5ec, 0x1fc, 3, 0x000, 0), /* MX51_PAD_I2C1_DAT__GPIO4_17 */
-	IMX_PIN_REG(MX51_PAD_I2C1_DAT, 0x5ec, 0x1fc, 0, 0x000, 0), /* MX51_PAD_I2C1_DAT__I2C1_DAT */
-	IMX_PIN_REG(MX51_PAD_AUD3_BB_TXD, 0x5f0, 0x200, 0, 0x000, 0), /* MX51_PAD_AUD3_BB_TXD__AUD3_TXD */
-	IMX_PIN_REG(MX51_PAD_AUD3_BB_TXD, 0x5f0, 0x200, 3, 0x000, 0), /* MX51_PAD_AUD3_BB_TXD__GPIO4_18 */
-	IMX_PIN_REG(MX51_PAD_AUD3_BB_RXD, 0x5f4, 0x204, 0, 0x000, 0), /* MX51_PAD_AUD3_BB_RXD__AUD3_RXD */
-	IMX_PIN_REG(MX51_PAD_AUD3_BB_RXD, 0x5f4, 0x204, 3, 0x000, 0), /* MX51_PAD_AUD3_BB_RXD__GPIO4_19 */
-	IMX_PIN_REG(MX51_PAD_AUD3_BB_RXD, 0x5f4, 0x204, 1, 0x9f4, 2), /* MX51_PAD_AUD3_BB_RXD__UART3_RXD */
-	IMX_PIN_REG(MX51_PAD_AUD3_BB_CK, 0x5f8, 0x208, 0, 0x000, 0), /* MX51_PAD_AUD3_BB_CK__AUD3_TXC */
-	IMX_PIN_REG(MX51_PAD_AUD3_BB_CK, 0x5f8, 0x208, 3, 0x000, 0), /* MX51_PAD_AUD3_BB_CK__GPIO4_20 */
-	IMX_PIN_REG(MX51_PAD_AUD3_BB_FS, 0x5fc, 0x20c, 0, 0x000, 0), /* MX51_PAD_AUD3_BB_FS__AUD3_TXFS */
-	IMX_PIN_REG(MX51_PAD_AUD3_BB_FS, 0x5fc, 0x20c, 3, 0x000, 0), /* MX51_PAD_AUD3_BB_FS__GPIO4_21 */
-	IMX_PIN_REG(MX51_PAD_AUD3_BB_FS, 0x5fc, 0x20c, 1, 0x000, 0), /* MX51_PAD_AUD3_BB_FS__UART3_TXD */
-	IMX_PIN_REG(MX51_PAD_CSPI1_MOSI, 0x600, 0x210, 0, 0x000, 0), /* MX51_PAD_CSPI1_MOSI__ECSPI1_MOSI */
-	IMX_PIN_REG(MX51_PAD_CSPI1_MOSI, 0x600, 0x210, 3, 0x000, 0), /* MX51_PAD_CSPI1_MOSI__GPIO4_22 */
-	IMX_PIN_REG(MX51_PAD_CSPI1_MOSI, 0x600, 0x210, 1, 0x9b4, 1), /* MX51_PAD_CSPI1_MOSI__I2C1_SDA */
-	IMX_PIN_REG(MX51_PAD_CSPI1_MISO, 0x604, 0x214, 1, 0x8c4, 1), /* MX51_PAD_CSPI1_MISO__AUD4_RXD */
-	IMX_PIN_REG(MX51_PAD_CSPI1_MISO, 0x604, 0x214, 0, 0x000, 0), /* MX51_PAD_CSPI1_MISO__ECSPI1_MISO */
-	IMX_PIN_REG(MX51_PAD_CSPI1_MISO, 0x604, 0x214, 3, 0x000, 0), /* MX51_PAD_CSPI1_MISO__GPIO4_23 */
-	IMX_PIN_REG(MX51_PAD_CSPI1_SS0, 0x608, 0x218, 1, 0x8cc, 1), /* MX51_PAD_CSPI1_SS0__AUD4_TXC */
-	IMX_PIN_REG(MX51_PAD_CSPI1_SS0, 0x608, 0x218, 0, 0x000, 0), /* MX51_PAD_CSPI1_SS0__ECSPI1_SS0 */
-	IMX_PIN_REG(MX51_PAD_CSPI1_SS0, 0x608, 0x218, 3, 0x000, 0), /* MX51_PAD_CSPI1_SS0__GPIO4_24 */
-	IMX_PIN_REG(MX51_PAD_CSPI1_SS1, 0x60c, 0x21c, 1, 0x8c8, 1), /* MX51_PAD_CSPI1_SS1__AUD4_TXD */
-	IMX_PIN_REG(MX51_PAD_CSPI1_SS1, 0x60c, 0x21c, 0, 0x000, 0), /* MX51_PAD_CSPI1_SS1__ECSPI1_SS1 */
-	IMX_PIN_REG(MX51_PAD_CSPI1_SS1, 0x60c, 0x21c, 3, 0x000, 0), /* MX51_PAD_CSPI1_SS1__GPIO4_25 */
-	IMX_PIN_REG(MX51_PAD_CSPI1_RDY, 0x610, 0x220, 1, 0x8d0, 1), /* MX51_PAD_CSPI1_RDY__AUD4_TXFS */
-	IMX_PIN_REG(MX51_PAD_CSPI1_RDY, 0x610, 0x220, 0, 0x000, 0), /* MX51_PAD_CSPI1_RDY__ECSPI1_RDY */
-	IMX_PIN_REG(MX51_PAD_CSPI1_RDY, 0x610, 0x220, 3, 0x000, 0), /* MX51_PAD_CSPI1_RDY__GPIO4_26 */
-	IMX_PIN_REG(MX51_PAD_CSPI1_SCLK, 0x614, 0x224, 0, 0x000, 0), /* MX51_PAD_CSPI1_SCLK__ECSPI1_SCLK */
-	IMX_PIN_REG(MX51_PAD_CSPI1_SCLK, 0x614, 0x224, 3, 0x000, 0), /* MX51_PAD_CSPI1_SCLK__GPIO4_27 */
-	IMX_PIN_REG(MX51_PAD_CSPI1_SCLK, 0x614, 0x224, 1, 0x9b0, 1), /* MX51_PAD_CSPI1_SCLK__I2C1_SCL */
-	IMX_PIN_REG(MX51_PAD_UART1_RXD, 0x618, 0x228, 3, 0x000, 0), /* MX51_PAD_UART1_RXD__GPIO4_28 */
-	IMX_PIN_REG(MX51_PAD_UART1_RXD, 0x618, 0x228, 0, 0x9e4, 0), /* MX51_PAD_UART1_RXD__UART1_RXD */
-	IMX_PIN_REG(MX51_PAD_UART1_TXD, 0x61c, 0x22c, 3, 0x000, 0), /* MX51_PAD_UART1_TXD__GPIO4_29 */
-	IMX_PIN_REG(MX51_PAD_UART1_TXD, 0x61c, 0x22c, 1, 0x000, 0), /* MX51_PAD_UART1_TXD__PWM2_PWMO */
-	IMX_PIN_REG(MX51_PAD_UART1_TXD, 0x61c, 0x22c, 0, 0x000, 0), /* MX51_PAD_UART1_TXD__UART1_TXD */
-	IMX_PIN_REG(MX51_PAD_UART1_RTS, 0x620, 0x230, 3, 0x000, 0), /* MX51_PAD_UART1_RTS__GPIO4_30 */
-	IMX_PIN_REG(MX51_PAD_UART1_RTS, 0x620, 0x230, 0, 0x9e0, 0), /* MX51_PAD_UART1_RTS__UART1_RTS */
-	IMX_PIN_REG(MX51_PAD_UART1_CTS, 0x624, 0x234, 3, 0x000, 0), /* MX51_PAD_UART1_CTS__GPIO4_31 */
-	IMX_PIN_REG(MX51_PAD_UART1_CTS, 0x624, 0x234, 0, 0x000, 0), /* MX51_PAD_UART1_CTS__UART1_CTS */
-	IMX_PIN_REG(MX51_PAD_UART2_RXD, 0x628, 0x238, 1, 0x000, 0), /* MX51_PAD_UART2_RXD__FIRI_TXD */
-	IMX_PIN_REG(MX51_PAD_UART2_RXD, 0x628, 0x238, 3, 0x000, 0), /* MX51_PAD_UART2_RXD__GPIO1_20 */
-	IMX_PIN_REG(MX51_PAD_UART2_RXD, 0x628, 0x238, 0, 0x9ec, 2), /* MX51_PAD_UART2_RXD__UART2_RXD */
-	IMX_PIN_REG(MX51_PAD_UART2_TXD, 0x62c, 0x23c, 1, 0x000, 0), /* MX51_PAD_UART2_TXD__FIRI_RXD */
-	IMX_PIN_REG(MX51_PAD_UART2_TXD, 0x62c, 0x23c, 3, 0x000, 0), /* MX51_PAD_UART2_TXD__GPIO1_21 */
-	IMX_PIN_REG(MX51_PAD_UART2_TXD, 0x62c, 0x23c, 0, 0x000, 0), /* MX51_PAD_UART2_TXD__UART2_TXD */
-	IMX_PIN_REG(MX51_PAD_UART3_RXD, 0x630, 0x240, 2, 0x000, 0), /* MX51_PAD_UART3_RXD__CSI1_D0 */
-	IMX_PIN_REG(MX51_PAD_UART3_RXD, 0x630, 0x240, 3, 0x000, 0), /* MX51_PAD_UART3_RXD__GPIO1_22 */
-	IMX_PIN_REG(MX51_PAD_UART3_RXD, 0x630, 0x240, 0, 0x000, 0), /* MX51_PAD_UART3_RXD__UART1_DTR */
-	IMX_PIN_REG(MX51_PAD_UART3_RXD, 0x630, 0x240, 1, 0x9f4, 4), /* MX51_PAD_UART3_RXD__UART3_RXD */
-	IMX_PIN_REG(MX51_PAD_UART3_TXD, 0x634, 0x244, 2, 0x000, 0), /* MX51_PAD_UART3_TXD__CSI1_D1 */
-	IMX_PIN_REG(MX51_PAD_UART3_TXD, 0x634, 0x244, 3, 0x000, 0), /* MX51_PAD_UART3_TXD__GPIO1_23 */
-	IMX_PIN_REG(MX51_PAD_UART3_TXD, 0x634, 0x244, 0, 0x000, 0), /* MX51_PAD_UART3_TXD__UART1_DSR */
-	IMX_PIN_REG(MX51_PAD_UART3_TXD, 0x634, 0x244, 1, 0x000, 0), /* MX51_PAD_UART3_TXD__UART3_TXD */
-	IMX_PIN_REG(MX51_PAD_OWIRE_LINE, 0x638, 0x248, 3, 0x000, 0), /* MX51_PAD_OWIRE_LINE__GPIO1_24 */
-	IMX_PIN_REG(MX51_PAD_OWIRE_LINE, 0x638, 0x248, 0, 0x000, 0), /* MX51_PAD_OWIRE_LINE__OWIRE_LINE */
-	IMX_PIN_REG(MX51_PAD_OWIRE_LINE, 0x638, 0x248, 6, 0x000, 0), /* MX51_PAD_OWIRE_LINE__SPDIF_OUT */
-	IMX_PIN_REG(MX51_PAD_KEY_ROW0, 0x63c, 0x24c, 0, 0x000, 0), /* MX51_PAD_KEY_ROW0__KEY_ROW0 */
-	IMX_PIN_REG(MX51_PAD_KEY_ROW1, 0x640, 0x250, 0, 0x000, 0), /* MX51_PAD_KEY_ROW1__KEY_ROW1 */
-	IMX_PIN_REG(MX51_PAD_KEY_ROW2, 0x644, 0x254, 0, 0x000, 0), /* MX51_PAD_KEY_ROW2__KEY_ROW2 */
-	IMX_PIN_REG(MX51_PAD_KEY_ROW3, 0x648, 0x258, 0, 0x000, 0), /* MX51_PAD_KEY_ROW3__KEY_ROW3 */
-	IMX_PIN_REG(MX51_PAD_KEY_COL0, 0x64c, 0x25c, 0, 0x000, 0), /* MX51_PAD_KEY_COL0__KEY_COL0 */
-	IMX_PIN_REG(MX51_PAD_KEY_COL0, 0x64c, 0x25c, 7, 0x90c, 0), /* MX51_PAD_KEY_COL0__PLL1_BYP */
-	IMX_PIN_REG(MX51_PAD_KEY_COL1, 0x650, 0x260, 0, 0x000, 0), /* MX51_PAD_KEY_COL1__KEY_COL1 */
-	IMX_PIN_REG(MX51_PAD_KEY_COL1, 0x650, 0x260, 7, 0x910, 0), /* MX51_PAD_KEY_COL1__PLL2_BYP */
-	IMX_PIN_REG(MX51_PAD_KEY_COL2, 0x654, 0x264, 0, 0x000, 0), /* MX51_PAD_KEY_COL2__KEY_COL2 */
-	IMX_PIN_REG(MX51_PAD_KEY_COL2, 0x654, 0x264, 7, 0x000, 0), /* MX51_PAD_KEY_COL2__PLL3_BYP */
-	IMX_PIN_REG(MX51_PAD_KEY_COL3, 0x658, 0x268, 0, 0x000, 0), /* MX51_PAD_KEY_COL3__KEY_COL3 */
-	IMX_PIN_REG(MX51_PAD_KEY_COL4, 0x65c, 0x26c, 3, 0x9b8, 1), /* MX51_PAD_KEY_COL4__I2C2_SCL */
-	IMX_PIN_REG(MX51_PAD_KEY_COL4, 0x65c, 0x26c, 0, 0x000, 0), /* MX51_PAD_KEY_COL4__KEY_COL4 */
-	IMX_PIN_REG(MX51_PAD_KEY_COL4, 0x65c, 0x26c, 6, 0x000, 0), /* MX51_PAD_KEY_COL4__SPDIF_OUT1 */
-	IMX_PIN_REG(MX51_PAD_KEY_COL4, 0x65c, 0x26c, 1, 0x000, 0), /* MX51_PAD_KEY_COL4__UART1_RI */
-	IMX_PIN_REG(MX51_PAD_KEY_COL4, 0x65c, 0x26c, 2, 0x9f0, 4), /* MX51_PAD_KEY_COL4__UART3_RTS */
-	IMX_PIN_REG(MX51_PAD_KEY_COL5, 0x660, 0x270, 3, 0x9bc, 1), /* MX51_PAD_KEY_COL5__I2C2_SDA */
-	IMX_PIN_REG(MX51_PAD_KEY_COL5, 0x660, 0x270, 0, 0x000, 0), /* MX51_PAD_KEY_COL5__KEY_COL5 */
-	IMX_PIN_REG(MX51_PAD_KEY_COL5, 0x660, 0x270, 1, 0x000, 0), /* MX51_PAD_KEY_COL5__UART1_DCD */
-	IMX_PIN_REG(MX51_PAD_KEY_COL5, 0x660, 0x270, 2, 0x000, 0), /* MX51_PAD_KEY_COL5__UART3_CTS */
-	IMX_PIN_REG(MX51_PAD_USBH1_CLK, 0x678, 0x278, 1, 0x914, 1), /* MX51_PAD_USBH1_CLK__CSPI_SCLK */
-	IMX_PIN_REG(MX51_PAD_USBH1_CLK, 0x678, 0x278, 2, 0x000, 0), /* MX51_PAD_USBH1_CLK__GPIO1_25 */
-	IMX_PIN_REG(MX51_PAD_USBH1_CLK, 0x678, 0x278, 5, 0x9b8, 2), /* MX51_PAD_USBH1_CLK__I2C2_SCL */
-	IMX_PIN_REG(MX51_PAD_USBH1_CLK, 0x678, 0x278, 0, 0x000, 0), /* MX51_PAD_USBH1_CLK__USBH1_CLK */
-	IMX_PIN_REG(MX51_PAD_USBH1_DIR, 0x67c, 0x27c, 1, 0x91c, 1), /* MX51_PAD_USBH1_DIR__CSPI_MOSI */
-	IMX_PIN_REG(MX51_PAD_USBH1_DIR, 0x67c, 0x27c, 2, 0x000, 0), /* MX51_PAD_USBH1_DIR__GPIO1_26 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DIR, 0x67c, 0x27c, 5, 0x9bc, 2), /* MX51_PAD_USBH1_DIR__I2C2_SDA */
-	IMX_PIN_REG(MX51_PAD_USBH1_DIR, 0x67c, 0x27c, 0, 0x000, 0), /* MX51_PAD_USBH1_DIR__USBH1_DIR */
-	IMX_PIN_REG(MX51_PAD_USBH1_STP, 0x680, 0x280, 1, 0x000, 0), /* MX51_PAD_USBH1_STP__CSPI_RDY */
-	IMX_PIN_REG(MX51_PAD_USBH1_STP, 0x680, 0x280, 2, 0x000, 0), /* MX51_PAD_USBH1_STP__GPIO1_27 */
-	IMX_PIN_REG(MX51_PAD_USBH1_STP, 0x680, 0x280, 5, 0x9f4, 6), /* MX51_PAD_USBH1_STP__UART3_RXD */
-	IMX_PIN_REG(MX51_PAD_USBH1_STP, 0x680, 0x280, 0, 0x000, 0), /* MX51_PAD_USBH1_STP__USBH1_STP */
-	IMX_PIN_REG(MX51_PAD_USBH1_NXT, 0x684, 0x284, 1, 0x918, 0), /* MX51_PAD_USBH1_NXT__CSPI_MISO */
-	IMX_PIN_REG(MX51_PAD_USBH1_NXT, 0x684, 0x284, 2, 0x000, 0), /* MX51_PAD_USBH1_NXT__GPIO1_28 */
-	IMX_PIN_REG(MX51_PAD_USBH1_NXT, 0x684, 0x284, 5, 0x000, 0), /* MX51_PAD_USBH1_NXT__UART3_TXD */
-	IMX_PIN_REG(MX51_PAD_USBH1_NXT, 0x684, 0x284, 0, 0x000, 0), /* MX51_PAD_USBH1_NXT__USBH1_NXT */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA0, 0x688, 0x288, 2, 0x000, 0), /* MX51_PAD_USBH1_DATA0__GPIO1_11 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA0, 0x688, 0x288, 1, 0x000, 0), /* MX51_PAD_USBH1_DATA0__UART2_CTS */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA0, 0x688, 0x288, 0, 0x000, 0), /* MX51_PAD_USBH1_DATA0__USBH1_DATA0 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA1, 0x68c, 0x28c, 2, 0x000, 0), /* MX51_PAD_USBH1_DATA1__GPIO1_12 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA1, 0x68c, 0x28c, 1, 0x9ec, 4), /* MX51_PAD_USBH1_DATA1__UART2_RXD */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA1, 0x68c, 0x28c, 0, 0x000, 0), /* MX51_PAD_USBH1_DATA1__USBH1_DATA1 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA2, 0x690, 0x290, 2, 0x000, 0), /* MX51_PAD_USBH1_DATA2__GPIO1_13 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA2, 0x690, 0x290, 1, 0x000, 0), /* MX51_PAD_USBH1_DATA2__UART2_TXD */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA2, 0x690, 0x290, 0, 0x000, 0), /* MX51_PAD_USBH1_DATA2__USBH1_DATA2 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA3, 0x694, 0x294, 2, 0x000, 0), /* MX51_PAD_USBH1_DATA3__GPIO1_14 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA3, 0x694, 0x294, 1, 0x9e8, 5), /* MX51_PAD_USBH1_DATA3__UART2_RTS */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA3, 0x694, 0x294, 0, 0x000, 0), /* MX51_PAD_USBH1_DATA3__USBH1_DATA3 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA4, 0x698, 0x298, 1, 0x000, 0), /* MX51_PAD_USBH1_DATA4__CSPI_SS0 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA4, 0x698, 0x298, 2, 0x000, 0), /* MX51_PAD_USBH1_DATA4__GPIO1_15 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA4, 0x698, 0x298, 0, 0x000, 0), /* MX51_PAD_USBH1_DATA4__USBH1_DATA4 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA5, 0x69c, 0x29c, 1, 0x920, 0), /* MX51_PAD_USBH1_DATA5__CSPI_SS1 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA5, 0x69c, 0x29c, 2, 0x000, 0), /* MX51_PAD_USBH1_DATA5__GPIO1_16 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA5, 0x69c, 0x29c, 0, 0x000, 0), /* MX51_PAD_USBH1_DATA5__USBH1_DATA5 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA6, 0x6a0, 0x2a0, 1, 0x928, 1), /* MX51_PAD_USBH1_DATA6__CSPI_SS3 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA6, 0x6a0, 0x2a0, 2, 0x000, 0), /* MX51_PAD_USBH1_DATA6__GPIO1_17 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA6, 0x6a0, 0x2a0, 0, 0x000, 0), /* MX51_PAD_USBH1_DATA6__USBH1_DATA6 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA7, 0x6a4, 0x2a4, 1, 0x000, 0), /* MX51_PAD_USBH1_DATA7__ECSPI1_SS3 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA7, 0x6a4, 0x2a4, 5, 0x934, 1), /* MX51_PAD_USBH1_DATA7__ECSPI2_SS3 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA7, 0x6a4, 0x2a4, 2, 0x000, 0), /* MX51_PAD_USBH1_DATA7__GPIO1_18 */
-	IMX_PIN_REG(MX51_PAD_USBH1_DATA7, 0x6a4, 0x2a4, 0, 0x000, 0), /* MX51_PAD_USBH1_DATA7__USBH1_DATA7 */
-	IMX_PIN_REG(MX51_PAD_DI1_PIN11, 0x6a8, 0x2a8, 0, 0x000, 0), /* MX51_PAD_DI1_PIN11__DI1_PIN11 */
-	IMX_PIN_REG(MX51_PAD_DI1_PIN11, 0x6a8, 0x2a8, 7, 0x000, 0), /* MX51_PAD_DI1_PIN11__ECSPI1_SS2 */
-	IMX_PIN_REG(MX51_PAD_DI1_PIN11, 0x6a8, 0x2a8, 4, 0x000, 0), /* MX51_PAD_DI1_PIN11__GPIO3_0 */
-	IMX_PIN_REG(MX51_PAD_DI1_PIN12, 0x6ac, 0x2ac, 0, 0x000, 0), /* MX51_PAD_DI1_PIN12__DI1_PIN12 */
-	IMX_PIN_REG(MX51_PAD_DI1_PIN12, 0x6ac, 0x2ac, 4, 0x978, 1), /* MX51_PAD_DI1_PIN12__GPIO3_1 */
-	IMX_PIN_REG(MX51_PAD_DI1_PIN13, 0x6b0, 0x2b0, 0, 0x000, 0), /* MX51_PAD_DI1_PIN13__DI1_PIN13 */
-	IMX_PIN_REG(MX51_PAD_DI1_PIN13, 0x6b0, 0x2b0, 4, 0x97c, 1), /* MX51_PAD_DI1_PIN13__GPIO3_2 */
-	IMX_PIN_REG(MX51_PAD_DI1_D0_CS, 0x6b4, 0x2b4, 0, 0x000, 0), /* MX51_PAD_DI1_D0_CS__DI1_D0_CS */
-	IMX_PIN_REG(MX51_PAD_DI1_D0_CS, 0x6b4, 0x2b4, 4, 0x980, 1), /* MX51_PAD_DI1_D0_CS__GPIO3_3 */
-	IMX_PIN_REG(MX51_PAD_DI1_D1_CS, 0x6b8, 0x2b8, 0, 0x000, 0), /* MX51_PAD_DI1_D1_CS__DI1_D1_CS */
-	IMX_PIN_REG(MX51_PAD_DI1_D1_CS, 0x6b8, 0x2b8, 2, 0x000, 0), /* MX51_PAD_DI1_D1_CS__DISP1_PIN14 */
-	IMX_PIN_REG(MX51_PAD_DI1_D1_CS, 0x6b8, 0x2b8, 3, 0x000, 0), /* MX51_PAD_DI1_D1_CS__DISP1_PIN5 */
-	IMX_PIN_REG(MX51_PAD_DI1_D1_CS, 0x6b8, 0x2b8, 4, 0x984, 1), /* MX51_PAD_DI1_D1_CS__GPIO3_4 */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_DIN, 0x6bc, 0x2bc, 2, 0x9a4, 1), /* MX51_PAD_DISPB2_SER_DIN__DISP1_PIN1 */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_DIN, 0x6bc, 0x2bc, 0, 0x9c4, 0), /* MX51_PAD_DISPB2_SER_DIN__DISPB2_SER_DIN */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_DIN, 0x6bc, 0x2bc, 4, 0x988, 1), /* MX51_PAD_DISPB2_SER_DIN__GPIO3_5 */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_DIO, 0x6c0, 0x2c0, 3, 0x000, 0), /* MX51_PAD_DISPB2_SER_DIO__DISP1_PIN6 */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_DIO, 0x6c0, 0x2c0, 0, 0x9c4, 1), /* MX51_PAD_DISPB2_SER_DIO__DISPB2_SER_DIO */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_DIO, 0x6c0, 0x2c0, 4, 0x98c, 1), /* MX51_PAD_DISPB2_SER_DIO__GPIO3_6 */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_CLK, 0x6c4, 0x2c4, 2, 0x000, 0), /* MX51_PAD_DISPB2_SER_CLK__DISP1_PIN17 */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_CLK, 0x6c4, 0x2c4, 3, 0x000, 0), /* MX51_PAD_DISPB2_SER_CLK__DISP1_PIN7 */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_CLK, 0x6c4, 0x2c4, 0, 0x000, 0), /* MX51_PAD_DISPB2_SER_CLK__DISPB2_SER_CLK */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_CLK, 0x6c4, 0x2c4, 4, 0x990, 1), /* MX51_PAD_DISPB2_SER_CLK__GPIO3_7 */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_RS, 0x6c8, 0x2c8, 2, 0x000, 0), /* MX51_PAD_DISPB2_SER_RS__DISP1_EXT_CLK */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_RS, 0x6c8, 0x2c8, 2, 0x000, 0), /* MX51_PAD_DISPB2_SER_RS__DISP1_PIN16 */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_RS, 0x6c8, 0x2c8, 3, 0x000, 0), /* MX51_PAD_DISPB2_SER_RS__DISP1_PIN8 */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_RS, 0x6c8, 0x2c8, 0, 0x000, 0), /* MX51_PAD_DISPB2_SER_RS__DISPB2_SER_RS */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_RS, 0x6c8, 0x2c8, 0, 0x000, 0), /* MX51_PAD_DISPB2_SER_RS__DISPB2_SER_RS */
-	IMX_PIN_REG(MX51_PAD_DISPB2_SER_RS, 0x6c8, 0x2c8, 4, 0x994, 1), /* MX51_PAD_DISPB2_SER_RS__GPIO3_8 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT0, 0x6cc, 0x2cc, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT0__DISP1_DAT0 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT1, 0x6d0, 0x2d0, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT1__DISP1_DAT1 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT2, 0x6d4, 0x2d4, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT2__DISP1_DAT2 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT3, 0x6d8, 0x2d8, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT3__DISP1_DAT3 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT4, 0x6dc, 0x2dc, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT4__DISP1_DAT4 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT5, 0x6e0, 0x2e0, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT5__DISP1_DAT5 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT6, 0x6e4, 0x2e4, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT6__BOOT_USB_SRC */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT6, 0x6e4, 0x2e4, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT6__DISP1_DAT6 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT7, 0x6e8, 0x2e8, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT7__BOOT_EEPROM_CFG */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT7, 0x6e8, 0x2e8, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT7__DISP1_DAT7 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT8, 0x6ec, 0x2ec, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT8__BOOT_SRC0 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT8, 0x6ec, 0x2ec, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT8__DISP1_DAT8 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT9, 0x6f0, 0x2f0, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT9__BOOT_SRC1 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT9, 0x6f0, 0x2f0, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT9__DISP1_DAT9 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT10, 0x6f4, 0x2f4, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT10__BOOT_SPARE_SIZE */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT10, 0x6f4, 0x2f4, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT10__DISP1_DAT10 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT11, 0x6f8, 0x2f8, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT11__BOOT_LPB_FREQ2 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT11, 0x6f8, 0x2f8, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT11__DISP1_DAT11 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT12, 0x6fc, 0x2fc, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT12__BOOT_MLC_SEL */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT12, 0x6fc, 0x2fc, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT12__DISP1_DAT12 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT13, 0x700, 0x300, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT13__BOOT_MEM_CTL0 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT13, 0x700, 0x300, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT13__DISP1_DAT13 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT14, 0x704, 0x304, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT14__BOOT_MEM_CTL1 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT14, 0x704, 0x304, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT14__DISP1_DAT14 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT15, 0x708, 0x308, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT15__BOOT_BUS_WIDTH */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT15, 0x708, 0x308, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT15__DISP1_DAT15 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT16, 0x70c, 0x30c, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT16__BOOT_PAGE_SIZE0 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT16, 0x70c, 0x30c, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT16__DISP1_DAT16 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT17, 0x710, 0x310, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT17__BOOT_PAGE_SIZE1 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT17, 0x710, 0x310, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT17__DISP1_DAT17 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT18, 0x714, 0x314, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT18__BOOT_WEIM_MUXED0 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT18, 0x714, 0x314, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT18__DISP1_DAT18 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT18, 0x714, 0x314, 5, 0x000, 0), /* MX51_PAD_DISP1_DAT18__DISP2_PIN11 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT18, 0x714, 0x314, 4, 0x000, 0), /* MX51_PAD_DISP1_DAT18__DISP2_PIN5 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT19, 0x718, 0x318, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT19__BOOT_WEIM_MUXED1 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT19, 0x718, 0x318, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT19__DISP1_DAT19 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT19, 0x718, 0x318, 5, 0x000, 0), /* MX51_PAD_DISP1_DAT19__DISP2_PIN12 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT19, 0x718, 0x318, 4, 0x000, 0), /* MX51_PAD_DISP1_DAT19__DISP2_PIN6 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT20, 0x71c, 0x31c, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT20__BOOT_MEM_TYPE0 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT20, 0x71c, 0x31c, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT20__DISP1_DAT20 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT20, 0x71c, 0x31c, 5, 0x000, 0), /* MX51_PAD_DISP1_DAT20__DISP2_PIN13 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT20, 0x71c, 0x31c, 4, 0x000, 0), /* MX51_PAD_DISP1_DAT20__DISP2_PIN7 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT21, 0x720, 0x320, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT21__BOOT_MEM_TYPE1 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT21, 0x720, 0x320, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT21__DISP1_DAT21 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT21, 0x720, 0x320, 5, 0x000, 0), /* MX51_PAD_DISP1_DAT21__DISP2_PIN14 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT21, 0x720, 0x320, 4, 0x000, 0), /* MX51_PAD_DISP1_DAT21__DISP2_PIN8 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT22, 0x724, 0x324, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT22__BOOT_LPB_FREQ0 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT22, 0x724, 0x324, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT22__DISP1_DAT22 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT22, 0x724, 0x324, 6, 0x000, 0), /* MX51_PAD_DISP1_DAT22__DISP2_D0_CS */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT22, 0x724, 0x324, 5, 0x000, 0), /* MX51_PAD_DISP1_DAT22__DISP2_DAT16 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT23, 0x728, 0x328, 7, 0x000, 0), /* MX51_PAD_DISP1_DAT23__BOOT_LPB_FREQ1 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT23, 0x728, 0x328, 0, 0x000, 0), /* MX51_PAD_DISP1_DAT23__DISP1_DAT23 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT23, 0x728, 0x328, 6, 0x000, 0), /* MX51_PAD_DISP1_DAT23__DISP2_D1_CS */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT23, 0x728, 0x328, 5, 0x000, 0), /* MX51_PAD_DISP1_DAT23__DISP2_DAT17 */
-	IMX_PIN_REG(MX51_PAD_DISP1_DAT23, 0x728, 0x328, 4, 0x000, 0), /* MX51_PAD_DISP1_DAT23__DISP2_SER_CS */
-	IMX_PIN_REG(MX51_PAD_DI1_PIN3, 0x72c, 0x32c, 0, 0x000, 0), /* MX51_PAD_DI1_PIN3__DI1_PIN3 */
-	IMX_PIN_REG(MX51_PAD_DI1_PIN2, 0x734, 0x330, 0, 0x000, 0), /* MX51_PAD_DI1_PIN2__DI1_PIN2 */
-	IMX_PIN_REG(MX51_PAD_DI_GP2, 0x740, 0x338, 0, 0x000, 0), /* MX51_PAD_DI_GP2__DISP1_SER_CLK */
-	IMX_PIN_REG(MX51_PAD_DI_GP2, 0x740, 0x338, 2, 0x9a8, 1), /* MX51_PAD_DI_GP2__DISP2_WAIT */
-	IMX_PIN_REG(MX51_PAD_DI_GP3, 0x744, 0x33c, 3, 0x9a0, 1), /* MX51_PAD_DI_GP3__CSI1_DATA_EN */
-	IMX_PIN_REG(MX51_PAD_DI_GP3, 0x744, 0x33c, 0, 0x9c0, 0), /* MX51_PAD_DI_GP3__DISP1_SER_DIO */
-	IMX_PIN_REG(MX51_PAD_DI_GP3, 0x744, 0x33c, 2, 0x000, 0), /* MX51_PAD_DI_GP3__FEC_TX_ER */
-	IMX_PIN_REG(MX51_PAD_DI2_PIN4, 0x748, 0x340, 3, 0x99c, 1), /* MX51_PAD_DI2_PIN4__CSI2_DATA_EN */
-	IMX_PIN_REG(MX51_PAD_DI2_PIN4, 0x748, 0x340, 0, 0x000, 0), /* MX51_PAD_DI2_PIN4__DI2_PIN4 */
-	IMX_PIN_REG(MX51_PAD_DI2_PIN4, 0x748, 0x340, 2, 0x950, 1), /* MX51_PAD_DI2_PIN4__FEC_CRS */
-	IMX_PIN_REG(MX51_PAD_DI2_PIN2, 0x74c, 0x344, 0, 0x000, 0), /* MX51_PAD_DI2_PIN2__DI2_PIN2 */
-	IMX_PIN_REG(MX51_PAD_DI2_PIN2, 0x74c, 0x344, 2, 0x000, 0), /* MX51_PAD_DI2_PIN2__FEC_MDC */
-	IMX_PIN_REG(MX51_PAD_DI2_PIN3, 0x750, 0x348, 0, 0x000, 0), /* MX51_PAD_DI2_PIN3__DI2_PIN3 */
-	IMX_PIN_REG(MX51_PAD_DI2_PIN3, 0x750, 0x348, 2, 0x954, 1), /* MX51_PAD_DI2_PIN3__FEC_MDIO */
-	IMX_PIN_REG(MX51_PAD_DI2_DISP_CLK, 0x754, 0x34c, 0, 0x000, 0), /* MX51_PAD_DI2_DISP_CLK__DI2_DISP_CLK */
-	IMX_PIN_REG(MX51_PAD_DI2_DISP_CLK, 0x754, 0x34c, 2, 0x95c, 1), /* MX51_PAD_DI2_DISP_CLK__FEC_RDATA1 */
-	IMX_PIN_REG(MX51_PAD_DI_GP4, 0x758, 0x350, 4, 0x000, 0), /* MX51_PAD_DI_GP4__DI2_PIN15 */
-	IMX_PIN_REG(MX51_PAD_DI_GP4, 0x758, 0x350, 0, 0x9c0, 1), /* MX51_PAD_DI_GP4__DISP1_SER_DIN */
-	IMX_PIN_REG(MX51_PAD_DI_GP4, 0x758, 0x350, 3, 0x000, 0), /* MX51_PAD_DI_GP4__DISP2_PIN1 */
-	IMX_PIN_REG(MX51_PAD_DI_GP4, 0x758, 0x350, 2, 0x960, 1), /* MX51_PAD_DI_GP4__FEC_RDATA2 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT0, 0x75c, 0x354, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT0__DISP2_DAT0 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT0, 0x75c, 0x354, 2, 0x964, 1), /* MX51_PAD_DISP2_DAT0__FEC_RDATA3 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT0, 0x75c, 0x354, 4, 0x9c8, 1), /* MX51_PAD_DISP2_DAT0__KEY_COL6 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT0, 0x75c, 0x354, 5, 0x9f4, 8), /* MX51_PAD_DISP2_DAT0__UART3_RXD */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT0, 0x75c, 0x354, 3, 0x9f8, 1), /* MX51_PAD_DISP2_DAT0__USBH3_CLK */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT1, 0x760, 0x358, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT1__DISP2_DAT1 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT1, 0x760, 0x358, 2, 0x970, 1), /* MX51_PAD_DISP2_DAT1__FEC_RX_ER */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT1, 0x760, 0x358, 4, 0x9cc, 1), /* MX51_PAD_DISP2_DAT1__KEY_COL7 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT1, 0x760, 0x358, 5, 0x000, 0), /* MX51_PAD_DISP2_DAT1__UART3_TXD */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT1, 0x760, 0x358, 3, 0xa1c, 1), /* MX51_PAD_DISP2_DAT1__USBH3_DIR */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT2, 0x764, 0x35c, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT2__DISP2_DAT2 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT3, 0x768, 0x360, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT3__DISP2_DAT3 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT4, 0x76c, 0x364, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT4__DISP2_DAT4 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT5, 0x770, 0x368, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT5__DISP2_DAT5 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT6, 0x774, 0x36c, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT6__DISP2_DAT6 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT6, 0x774, 0x36c, 2, 0x000, 0), /* MX51_PAD_DISP2_DAT6__FEC_TDATA1 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT6, 0x774, 0x36c, 5, 0x000, 0), /* MX51_PAD_DISP2_DAT6__GPIO1_19 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT6, 0x774, 0x36c, 4, 0x9d0, 1), /* MX51_PAD_DISP2_DAT6__KEY_ROW4 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT6, 0x774, 0x36c, 3, 0xa24, 1), /* MX51_PAD_DISP2_DAT6__USBH3_STP */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT7, 0x778, 0x370, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT7__DISP2_DAT7 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT7, 0x778, 0x370, 2, 0x000, 0), /* MX51_PAD_DISP2_DAT7__FEC_TDATA2 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT7, 0x778, 0x370, 5, 0x000, 0), /* MX51_PAD_DISP2_DAT7__GPIO1_29 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT7, 0x778, 0x370, 4, 0x9d4, 1), /* MX51_PAD_DISP2_DAT7__KEY_ROW5 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT7, 0x778, 0x370, 3, 0xa20, 1), /* MX51_PAD_DISP2_DAT7__USBH3_NXT */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT8, 0x77c, 0x374, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT8__DISP2_DAT8 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT8, 0x77c, 0x374, 2, 0x000, 0), /* MX51_PAD_DISP2_DAT8__FEC_TDATA3 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT8, 0x77c, 0x374, 5, 0x000, 0), /* MX51_PAD_DISP2_DAT8__GPIO1_30 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT8, 0x77c, 0x374, 4, 0x9d8, 1), /* MX51_PAD_DISP2_DAT8__KEY_ROW6 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT8, 0x77c, 0x374, 3, 0x9fc, 1), /* MX51_PAD_DISP2_DAT8__USBH3_DATA0 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT9, 0x780, 0x378, 4, 0x8f4, 1), /* MX51_PAD_DISP2_DAT9__AUD6_RXC */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT9, 0x780, 0x378, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT9__DISP2_DAT9 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT9, 0x780, 0x378, 2, 0x000, 0), /* MX51_PAD_DISP2_DAT9__FEC_TX_EN */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT9, 0x780, 0x378, 5, 0x000, 0), /* MX51_PAD_DISP2_DAT9__GPIO1_31 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT9, 0x780, 0x378, 3, 0xa00, 1), /* MX51_PAD_DISP2_DAT9__USBH3_DATA1 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT10, 0x784, 0x37c, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT10__DISP2_DAT10 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT10, 0x784, 0x37c, 5, 0x000, 0), /* MX51_PAD_DISP2_DAT10__DISP2_SER_CS */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT10, 0x784, 0x37c, 2, 0x94c, 1), /* MX51_PAD_DISP2_DAT10__FEC_COL */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT10, 0x784, 0x37c, 4, 0x9dc, 1), /* MX51_PAD_DISP2_DAT10__KEY_ROW7 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT10, 0x784, 0x37c, 3, 0xa04, 1), /* MX51_PAD_DISP2_DAT10__USBH3_DATA2 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT11, 0x788, 0x380, 4, 0x8f0, 1), /* MX51_PAD_DISP2_DAT11__AUD6_TXD */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT11, 0x788, 0x380, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT11__DISP2_DAT11 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT11, 0x788, 0x380, 2, 0x968, 1), /* MX51_PAD_DISP2_DAT11__FEC_RX_CLK */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT11, 0x788, 0x380, 7, 0x000, 0), /* MX51_PAD_DISP2_DAT11__GPIO1_10 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT11, 0x788, 0x380, 3, 0xa08, 1), /* MX51_PAD_DISP2_DAT11__USBH3_DATA3 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT12, 0x78c, 0x384, 4, 0x8ec, 1), /* MX51_PAD_DISP2_DAT12__AUD6_RXD */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT12, 0x78c, 0x384, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT12__DISP2_DAT12 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT12, 0x78c, 0x384, 2, 0x96c, 1), /* MX51_PAD_DISP2_DAT12__FEC_RX_DV */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT12, 0x78c, 0x384, 3, 0xa0c, 1), /* MX51_PAD_DISP2_DAT12__USBH3_DATA4 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT13, 0x790, 0x388, 4, 0x8fc, 1), /* MX51_PAD_DISP2_DAT13__AUD6_TXC */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT13, 0x790, 0x388, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT13__DISP2_DAT13 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT13, 0x790, 0x388, 2, 0x974, 1), /* MX51_PAD_DISP2_DAT13__FEC_TX_CLK */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT13, 0x790, 0x388, 3, 0xa10, 1), /* MX51_PAD_DISP2_DAT13__USBH3_DATA5 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT14, 0x794, 0x38c, 4, 0x900, 1), /* MX51_PAD_DISP2_DAT14__AUD6_TXFS */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT14, 0x794, 0x38c, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT14__DISP2_DAT14 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT14, 0x794, 0x38c, 2, 0x958, 1), /* MX51_PAD_DISP2_DAT14__FEC_RDATA0 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT14, 0x794, 0x38c, 3, 0xa14, 1), /* MX51_PAD_DISP2_DAT14__USBH3_DATA6 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT15, 0x798, 0x390, 4, 0x8f8, 1), /* MX51_PAD_DISP2_DAT15__AUD6_RXFS */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT15, 0x798, 0x390, 5, 0x000, 0), /* MX51_PAD_DISP2_DAT15__DISP1_SER_CS */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT15, 0x798, 0x390, 0, 0x000, 0), /* MX51_PAD_DISP2_DAT15__DISP2_DAT15 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT15, 0x798, 0x390, 2, 0x000, 0), /* MX51_PAD_DISP2_DAT15__FEC_TDATA0 */
-	IMX_PIN_REG(MX51_PAD_DISP2_DAT15, 0x798, 0x390, 3, 0xa18, 1), /* MX51_PAD_DISP2_DAT15__USBH3_DATA7 */
-	IMX_PIN_REG(MX51_PAD_SD1_CMD, 0x79c, 0x394, 1, 0x8e0, 1), /* MX51_PAD_SD1_CMD__AUD5_RXFS */
-	IMX_PIN_REG(MX51_PAD_SD1_CMD, 0x79c, 0x394, 2, 0x91c, 2), /* MX51_PAD_SD1_CMD__CSPI_MOSI */
-	IMX_PIN_REG(MX51_PAD_SD1_CMD, 0x79c, 0x394, 0, 0x000, 0), /* MX51_PAD_SD1_CMD__SD1_CMD */
-	IMX_PIN_REG(MX51_PAD_SD1_CLK, 0x7a0, 0x398, 1, 0x8dc, 1), /* MX51_PAD_SD1_CLK__AUD5_RXC */
-	IMX_PIN_REG(MX51_PAD_SD1_CLK, 0x7a0, 0x398, 2, 0x914, 2), /* MX51_PAD_SD1_CLK__CSPI_SCLK */
-	IMX_PIN_REG(MX51_PAD_SD1_CLK, 0x7a0, 0x398, 0, 0x000, 0), /* MX51_PAD_SD1_CLK__SD1_CLK */
-	IMX_PIN_REG(MX51_PAD_SD1_DATA0, 0x7a4, 0x39c, 1, 0x8d8, 2), /* MX51_PAD_SD1_DATA0__AUD5_TXD */
-	IMX_PIN_REG(MX51_PAD_SD1_DATA0, 0x7a4, 0x39c, 2, 0x918, 1), /* MX51_PAD_SD1_DATA0__CSPI_MISO */
-	IMX_PIN_REG(MX51_PAD_SD1_DATA0, 0x7a4, 0x39c, 0, 0x000, 0), /* MX51_PAD_SD1_DATA0__SD1_DATA0 */
-	IMX_PIN_REG(MX51_PAD_EIM_DA0, NO_PAD, 0x01c, 0, 0x000, 0), /* MX51_PAD_EIM_DA0__EIM_DA0 */
-	IMX_PIN_REG(MX51_PAD_EIM_DA1, NO_PAD, 0x020, 0, 0x000, 0), /* MX51_PAD_EIM_DA1__EIM_DA1 */
-	IMX_PIN_REG(MX51_PAD_EIM_DA2, NO_PAD, 0x024, 0, 0x000, 0), /* MX51_PAD_EIM_DA2__EIM_DA2 */
-	IMX_PIN_REG(MX51_PAD_EIM_DA3, NO_PAD, 0x028, 0, 0x000, 0), /* MX51_PAD_EIM_DA3__EIM_DA3 */
-	IMX_PIN_REG(MX51_PAD_SD1_DATA1, 0x7a8, 0x3a0, 1, 0x8d4, 2), /* MX51_PAD_SD1_DATA1__AUD5_RXD */
-	IMX_PIN_REG(MX51_PAD_SD1_DATA1, 0x7a8, 0x3a0, 0, 0x000, 0), /* MX51_PAD_SD1_DATA1__SD1_DATA1 */
-	IMX_PIN_REG(MX51_PAD_EIM_DA4, NO_PAD, 0x02c, 0, 0x000, 0), /* MX51_PAD_EIM_DA4__EIM_DA4 */
-	IMX_PIN_REG(MX51_PAD_EIM_DA5, NO_PAD, 0x030, 0, 0x000, 0), /* MX51_PAD_EIM_DA5__EIM_DA5 */
-	IMX_PIN_REG(MX51_PAD_EIM_DA6, NO_PAD, 0x034, 0, 0x000, 0), /* MX51_PAD_EIM_DA6__EIM_DA6 */
-	IMX_PIN_REG(MX51_PAD_EIM_DA7, NO_PAD, 0x038, 0, 0x000, 0), /* MX51_PAD_EIM_DA7__EIM_DA7 */
-	IMX_PIN_REG(MX51_PAD_SD1_DATA2, 0x7ac, 0x3a4, 1, 0x8e4, 2), /* MX51_PAD_SD1_DATA2__AUD5_TXC */
-	IMX_PIN_REG(MX51_PAD_SD1_DATA2, 0x7ac, 0x3a4, 0, 0x000, 0), /* MX51_PAD_SD1_DATA2__SD1_DATA2 */
-	IMX_PIN_REG(MX51_PAD_EIM_DA10, NO_PAD, 0x044, 0, 0x000, 0), /* MX51_PAD_EIM_DA10__EIM_DA10 */
-	IMX_PIN_REG(MX51_PAD_EIM_DA11, NO_PAD, 0x048, 0, 0x000, 0), /* MX51_PAD_EIM_DA11__EIM_DA11 */
-	IMX_PIN_REG(MX51_PAD_EIM_DA8, NO_PAD, 0x03c, 0, 0x000, 0), /* MX51_PAD_EIM_DA8__EIM_DA8 */
-	IMX_PIN_REG(MX51_PAD_EIM_DA9, NO_PAD, 0x040, 0, 0x000, 0), /* MX51_PAD_EIM_DA9__EIM_DA9 */
-	IMX_PIN_REG(MX51_PAD_SD1_DATA3, 0x7b0, 0x3a8, 1, 0x8e8, 2), /* MX51_PAD_SD1_DATA3__AUD5_TXFS */
-	IMX_PIN_REG(MX51_PAD_SD1_DATA3, 0x7b0, 0x3a8, 2, 0x920, 1), /* MX51_PAD_SD1_DATA3__CSPI_SS1 */
-	IMX_PIN_REG(MX51_PAD_SD1_DATA3, 0x7b0, 0x3a8, 0, 0x000, 0), /* MX51_PAD_SD1_DATA3__SD1_DATA3 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_0, 0x7b4, 0x3ac, 2, 0x924, 0), /* MX51_PAD_GPIO1_0__CSPI_SS2 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_0, 0x7b4, 0x3ac, 1, 0x000, 0), /* MX51_PAD_GPIO1_0__GPIO1_0 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_0, 0x7b4, 0x3ac, 0, 0x000, 0), /* MX51_PAD_GPIO1_0__SD1_CD */
-	IMX_PIN_REG(MX51_PAD_GPIO1_1, 0x7b8, 0x3b0, 2, 0x918, 2), /* MX51_PAD_GPIO1_1__CSPI_MISO */
-	IMX_PIN_REG(MX51_PAD_GPIO1_1, 0x7b8, 0x3b0, 1, 0x000, 0), /* MX51_PAD_GPIO1_1__GPIO1_1 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_1, 0x7b8, 0x3b0, 0, 0x000, 0), /* MX51_PAD_GPIO1_1__SD1_WP */
-	IMX_PIN_REG(MX51_PAD_EIM_DA12, NO_PAD, 0x04c, 0, 0x000, 0), /* MX51_PAD_EIM_DA12__EIM_DA12 */
-	IMX_PIN_REG(MX51_PAD_EIM_DA13, NO_PAD, 0x050, 0, 0x000, 0), /* MX51_PAD_EIM_DA13__EIM_DA13 */
-	IMX_PIN_REG(MX51_PAD_EIM_DA14, NO_PAD, 0x054, 0, 0x000, 0), /* MX51_PAD_EIM_DA14__EIM_DA14 */
-	IMX_PIN_REG(MX51_PAD_EIM_DA15, NO_PAD, 0x058, 0, 0x000, 0), /* MX51_PAD_EIM_DA15__EIM_DA15 */
-	IMX_PIN_REG(MX51_PAD_SD2_CMD, 0x7bc, 0x3b4, 2, 0x91c, 3), /* MX51_PAD_SD2_CMD__CSPI_MOSI */
-	IMX_PIN_REG(MX51_PAD_SD2_CMD, 0x7bc, 0x3b4, 1, 0x9b0, 2), /* MX51_PAD_SD2_CMD__I2C1_SCL */
-	IMX_PIN_REG(MX51_PAD_SD2_CMD, 0x7bc, 0x3b4, 0, 0x000, 0), /* MX51_PAD_SD2_CMD__SD2_CMD */
-	IMX_PIN_REG(MX51_PAD_SD2_CLK, 0x7c0, 0x3b8, 2, 0x914, 3), /* MX51_PAD_SD2_CLK__CSPI_SCLK */
-	IMX_PIN_REG(MX51_PAD_SD2_CLK, 0x7c0, 0x3b8, 1, 0x9b4, 2), /* MX51_PAD_SD2_CLK__I2C1_SDA */
-	IMX_PIN_REG(MX51_PAD_SD2_CLK, 0x7c0, 0x3b8, 0, 0x000, 0), /* MX51_PAD_SD2_CLK__SD2_CLK */
-	IMX_PIN_REG(MX51_PAD_SD2_DATA0, 0x7c4, 0x3bc, 2, 0x918, 3), /* MX51_PAD_SD2_DATA0__CSPI_MISO */
-	IMX_PIN_REG(MX51_PAD_SD2_DATA0, 0x7c4, 0x3bc, 1, 0x000, 0), /* MX51_PAD_SD2_DATA0__SD1_DAT4 */
-	IMX_PIN_REG(MX51_PAD_SD2_DATA0, 0x7c4, 0x3bc, 0, 0x000, 0), /* MX51_PAD_SD2_DATA0__SD2_DATA0 */
-	IMX_PIN_REG(MX51_PAD_SD2_DATA1, 0x7c8, 0x3c0, 1, 0x000, 0), /* MX51_PAD_SD2_DATA1__SD1_DAT5 */
-	IMX_PIN_REG(MX51_PAD_SD2_DATA1, 0x7c8, 0x3c0, 0, 0x000, 0), /* MX51_PAD_SD2_DATA1__SD2_DATA1 */
-	IMX_PIN_REG(MX51_PAD_SD2_DATA1, 0x7c8, 0x3c0, 2, 0x000, 0), /* MX51_PAD_SD2_DATA1__USBH3_H2_DP */
-	IMX_PIN_REG(MX51_PAD_SD2_DATA2, 0x7cc, 0x3c4, 1, 0x000, 0), /* MX51_PAD_SD2_DATA2__SD1_DAT6 */
-	IMX_PIN_REG(MX51_PAD_SD2_DATA2, 0x7cc, 0x3c4, 0, 0x000, 0), /* MX51_PAD_SD2_DATA2__SD2_DATA2 */
-	IMX_PIN_REG(MX51_PAD_SD2_DATA2, 0x7cc, 0x3c4, 2, 0x000, 0), /* MX51_PAD_SD2_DATA2__USBH3_H2_DM */
-	IMX_PIN_REG(MX51_PAD_SD2_DATA3, 0x7d0, 0x3c8, 2, 0x924, 1), /* MX51_PAD_SD2_DATA3__CSPI_SS2 */
-	IMX_PIN_REG(MX51_PAD_SD2_DATA3, 0x7d0, 0x3c8, 1, 0x000, 0), /* MX51_PAD_SD2_DATA3__SD1_DAT7 */
-	IMX_PIN_REG(MX51_PAD_SD2_DATA3, 0x7d0, 0x3c8, 0, 0x000, 0), /* MX51_PAD_SD2_DATA3__SD2_DATA3 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_2, 0x7d4, 0x3cc, 5, 0x000, 0), /* MX51_PAD_GPIO1_2__CCM_OUT_2 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_2, 0x7d4, 0x3cc, 0, 0x000, 0), /* MX51_PAD_GPIO1_2__GPIO1_2 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_2, 0x7d4, 0x3cc, 2, 0x9b8, 3), /* MX51_PAD_GPIO1_2__I2C2_SCL */
-	IMX_PIN_REG(MX51_PAD_GPIO1_2, 0x7d4, 0x3cc, 7, 0x90c, 1), /* MX51_PAD_GPIO1_2__PLL1_BYP */
-	IMX_PIN_REG(MX51_PAD_GPIO1_2, 0x7d4, 0x3cc, 1, 0x000, 0), /* MX51_PAD_GPIO1_2__PWM1_PWMO */
-	IMX_PIN_REG(MX51_PAD_GPIO1_3, 0x7d8, 0x3d0, 0, 0x000, 0), /* MX51_PAD_GPIO1_3__GPIO1_3 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_3, 0x7d8, 0x3d0, 2, 0x9bc, 3), /* MX51_PAD_GPIO1_3__I2C2_SDA */
-	IMX_PIN_REG(MX51_PAD_GPIO1_3, 0x7d8, 0x3d0, 7, 0x910, 1), /* MX51_PAD_GPIO1_3__PLL2_BYP */
-	IMX_PIN_REG(MX51_PAD_GPIO1_3, 0x7d8, 0x3d0, 1, 0x000, 0), /* MX51_PAD_GPIO1_3__PWM2_PWMO */
-	IMX_PIN_REG(MX51_PAD_PMIC_INT_REQ, 0x7fc, 0x3d4, 0, 0x000, 0), /* MX51_PAD_PMIC_INT_REQ__PMIC_INT_REQ */
-	IMX_PIN_REG(MX51_PAD_PMIC_INT_REQ, 0x7fc, 0x3d4, 1, 0x000, 0), /* MX51_PAD_PMIC_INT_REQ__PMIC_PMU_IRQ_B */
-	IMX_PIN_REG(MX51_PAD_GPIO1_4, 0x804, 0x3d8, 4, 0x908, 1), /* MX51_PAD_GPIO1_4__DISP2_EXT_CLK */
-	IMX_PIN_REG(MX51_PAD_GPIO1_4, 0x804, 0x3d8, 3, 0x938, 1), /* MX51_PAD_GPIO1_4__EIM_RDY */
-	IMX_PIN_REG(MX51_PAD_GPIO1_4, 0x804, 0x3d8, 0, 0x000, 0), /* MX51_PAD_GPIO1_4__GPIO1_4 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_4, 0x804, 0x3d8, 2, 0x000, 0), /* MX51_PAD_GPIO1_4__WDOG1_WDOG_B */
-	IMX_PIN_REG(MX51_PAD_GPIO1_5, 0x808, 0x3dc, 6, 0x000, 0), /* MX51_PAD_GPIO1_5__CSI2_MCLK */
-	IMX_PIN_REG(MX51_PAD_GPIO1_5, 0x808, 0x3dc, 3, 0x000, 0), /* MX51_PAD_GPIO1_5__DISP2_PIN16 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_5, 0x808, 0x3dc, 0, 0x000, 0), /* MX51_PAD_GPIO1_5__GPIO1_5 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_5, 0x808, 0x3dc, 2, 0x000, 0), /* MX51_PAD_GPIO1_5__WDOG2_WDOG_B */
-	IMX_PIN_REG(MX51_PAD_GPIO1_6, 0x80c, 0x3e0, 4, 0x000, 0), /* MX51_PAD_GPIO1_6__DISP2_PIN17 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_6, 0x80c, 0x3e0, 0, 0x000, 0), /* MX51_PAD_GPIO1_6__GPIO1_6 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_6, 0x80c, 0x3e0, 3, 0x000, 0), /* MX51_PAD_GPIO1_6__REF_EN_B */
-	IMX_PIN_REG(MX51_PAD_GPIO1_7, 0x810, 0x3e4, 3, 0x000, 0), /* MX51_PAD_GPIO1_7__CCM_OUT_0 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_7, 0x810, 0x3e4, 0, 0x000, 0), /* MX51_PAD_GPIO1_7__GPIO1_7 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_7, 0x810, 0x3e4, 6, 0x000, 0), /* MX51_PAD_GPIO1_7__SD2_WP */
-	IMX_PIN_REG(MX51_PAD_GPIO1_7, 0x810, 0x3e4, 2, 0x000, 0), /* MX51_PAD_GPIO1_7__SPDIF_OUT1 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_8, 0x814, 0x3e8, 2, 0x99c, 2), /* MX51_PAD_GPIO1_8__CSI2_DATA_EN */
-	IMX_PIN_REG(MX51_PAD_GPIO1_8, 0x814, 0x3e8, 0, 0x000, 0), /* MX51_PAD_GPIO1_8__GPIO1_8 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_8, 0x814, 0x3e8, 6, 0x000, 0), /* MX51_PAD_GPIO1_8__SD2_CD */
-	IMX_PIN_REG(MX51_PAD_GPIO1_8, 0x814, 0x3e8, 1, 0x000, 0), /* MX51_PAD_GPIO1_8__USBH3_PWR */
-	IMX_PIN_REG(MX51_PAD_GPIO1_9, 0x818, 0x3ec, 3, 0x000, 0), /* MX51_PAD_GPIO1_9__CCM_OUT_1 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_9, 0x818, 0x3ec, 2, 0x000, 0), /* MX51_PAD_GPIO1_9__DISP2_D1_CS */
-	IMX_PIN_REG(MX51_PAD_GPIO1_9, 0x818, 0x3ec, 7, 0x000, 0), /* MX51_PAD_GPIO1_9__DISP2_SER_CS */
-	IMX_PIN_REG(MX51_PAD_GPIO1_9, 0x818, 0x3ec, 0, 0x000, 0), /* MX51_PAD_GPIO1_9__GPIO1_9 */
-	IMX_PIN_REG(MX51_PAD_GPIO1_9, 0x818, 0x3ec, 6, 0x000, 0), /* MX51_PAD_GPIO1_9__SD2_LCTL */
-	IMX_PIN_REG(MX51_PAD_GPIO1_9, 0x818, 0x3ec, 1, 0x000, 0), /* MX51_PAD_GPIO1_9__USBH3_OC */
+	MX51_PAD_RESERVE0 = 0,
+	MX51_PAD_RESERVE1 = 1,
+	MX51_PAD_RESERVE2 = 2,
+	MX51_PAD_RESERVE3 = 3,
+	MX51_PAD_RESERVE4 = 4,
+	MX51_PAD_RESERVE5 = 5,
+	MX51_PAD_RESERVE6 = 6,
+	MX51_PAD_EIM_DA0 = 7,
+	MX51_PAD_EIM_DA1 = 8,
+	MX51_PAD_EIM_DA2 = 9,
+	MX51_PAD_EIM_DA3 = 10,
+	MX51_PAD_EIM_DA4 = 11,
+	MX51_PAD_EIM_DA5 = 12,
+	MX51_PAD_EIM_DA6 = 13,
+	MX51_PAD_EIM_DA7 = 14,
+	MX51_PAD_EIM_DA8 = 15,
+	MX51_PAD_EIM_DA9 = 16,
+	MX51_PAD_EIM_DA10 = 17,
+	MX51_PAD_EIM_DA11 = 18,
+	MX51_PAD_EIM_DA12 = 19,
+	MX51_PAD_EIM_DA13 = 20,
+	MX51_PAD_EIM_DA14 = 21,
+	MX51_PAD_EIM_DA15 = 22,
+	MX51_PAD_EIM_D16 = 23,
+	MX51_PAD_EIM_D17 = 24,
+	MX51_PAD_EIM_D18 = 25,
+	MX51_PAD_EIM_D19 = 26,
+	MX51_PAD_EIM_D20 = 27,
+	MX51_PAD_EIM_D21 = 28,
+	MX51_PAD_EIM_D22 = 29,
+	MX51_PAD_EIM_D23 = 30,
+	MX51_PAD_EIM_D24 = 31,
+	MX51_PAD_EIM_D25 = 32,
+	MX51_PAD_EIM_D26 = 33,
+	MX51_PAD_EIM_D27 = 34,
+	MX51_PAD_EIM_D28 = 35,
+	MX51_PAD_EIM_D29 = 36,
+	MX51_PAD_EIM_D30 = 37,
+	MX51_PAD_EIM_D31 = 38,
+	MX51_PAD_EIM_A16 = 39,
+	MX51_PAD_EIM_A17 = 40,
+	MX51_PAD_EIM_A18 = 41,
+	MX51_PAD_EIM_A19 = 42,
+	MX51_PAD_EIM_A20 = 43,
+	MX51_PAD_EIM_A21 = 44,
+	MX51_PAD_EIM_A22 = 45,
+	MX51_PAD_EIM_A23 = 46,
+	MX51_PAD_EIM_A24 = 47,
+	MX51_PAD_EIM_A25 = 48,
+	MX51_PAD_EIM_A26 = 49,
+	MX51_PAD_EIM_A27 = 50,
+	MX51_PAD_EIM_EB0 = 51,
+	MX51_PAD_EIM_EB1 = 52,
+	MX51_PAD_EIM_EB2 = 53,
+	MX51_PAD_EIM_EB3 = 54,
+	MX51_PAD_EIM_OE = 55,
+	MX51_PAD_EIM_CS0 = 56,
+	MX51_PAD_EIM_CS1 = 57,
+	MX51_PAD_EIM_CS2 = 58,
+	MX51_PAD_EIM_CS3 = 59,
+	MX51_PAD_EIM_CS4 = 60,
+	MX51_PAD_EIM_CS5 = 61,
+	MX51_PAD_EIM_DTACK = 62,
+	MX51_PAD_EIM_LBA = 63,
+	MX51_PAD_EIM_CRE = 64,
+	MX51_PAD_DRAM_CS1 = 65,
+	MX51_PAD_NANDF_WE_B = 66,
+	MX51_PAD_NANDF_RE_B = 67,
+	MX51_PAD_NANDF_ALE = 68,
+	MX51_PAD_NANDF_CLE = 69,
+	MX51_PAD_NANDF_WP_B = 70,
+	MX51_PAD_NANDF_RB0 = 71,
+	MX51_PAD_NANDF_RB1 = 72,
+	MX51_PAD_NANDF_RB2 = 73,
+	MX51_PAD_NANDF_RB3 = 74,
+	MX51_PAD_GPIO_NAND = 75,
+	MX51_PAD_NANDF_CS0 = 76,
+	MX51_PAD_NANDF_CS1 = 77,
+	MX51_PAD_NANDF_CS2 = 78,
+	MX51_PAD_NANDF_CS3 = 79,
+	MX51_PAD_NANDF_CS4 = 80,
+	MX51_PAD_NANDF_CS5 = 81,
+	MX51_PAD_NANDF_CS6 = 82,
+	MX51_PAD_NANDF_CS7 = 83,
+	MX51_PAD_NANDF_RDY_INT = 84,
+	MX51_PAD_NANDF_D15 = 85,
+	MX51_PAD_NANDF_D14 = 86,
+	MX51_PAD_NANDF_D13 = 87,
+	MX51_PAD_NANDF_D12 = 88,
+	MX51_PAD_NANDF_D11 = 89,
+	MX51_PAD_NANDF_D10 = 90,
+	MX51_PAD_NANDF_D9 = 91,
+	MX51_PAD_NANDF_D8 = 92,
+	MX51_PAD_NANDF_D7 = 93,
+	MX51_PAD_NANDF_D6 = 94,
+	MX51_PAD_NANDF_D5 = 95,
+	MX51_PAD_NANDF_D4 = 96,
+	MX51_PAD_NANDF_D3 = 97,
+	MX51_PAD_NANDF_D2 = 98,
+	MX51_PAD_NANDF_D1 = 99,
+	MX51_PAD_NANDF_D0 = 100,
+	MX51_PAD_CSI1_D8 = 101,
+	MX51_PAD_CSI1_D9 = 102,
+	MX51_PAD_CSI1_D10 = 103,
+	MX51_PAD_CSI1_D11 = 104,
+	MX51_PAD_CSI1_D12 = 105,
+	MX51_PAD_CSI1_D13 = 106,
+	MX51_PAD_CSI1_D14 = 107,
+	MX51_PAD_CSI1_D15 = 108,
+	MX51_PAD_CSI1_D16 = 109,
+	MX51_PAD_CSI1_D17 = 110,
+	MX51_PAD_CSI1_D18 = 111,
+	MX51_PAD_CSI1_D19 = 112,
+	MX51_PAD_CSI1_VSYNC = 113,
+	MX51_PAD_CSI1_HSYNC = 114,
+	MX51_PAD_CSI2_D12 = 115,
+	MX51_PAD_CSI2_D13 = 116,
+	MX51_PAD_CSI2_D14 = 117,
+	MX51_PAD_CSI2_D15 = 118,
+	MX51_PAD_CSI2_D16 = 119,
+	MX51_PAD_CSI2_D17 = 120,
+	MX51_PAD_CSI2_D18 = 121,
+	MX51_PAD_CSI2_D19 = 122,
+	MX51_PAD_CSI2_VSYNC = 123,
+	MX51_PAD_CSI2_HSYNC = 124,
+	MX51_PAD_CSI2_PIXCLK = 125,
+	MX51_PAD_I2C1_CLK = 126,
+	MX51_PAD_I2C1_DAT = 127,
+	MX51_PAD_AUD3_BB_TXD = 128,
+	MX51_PAD_AUD3_BB_RXD = 129,
+	MX51_PAD_AUD3_BB_CK = 130,
+	MX51_PAD_AUD3_BB_FS = 131,
+	MX51_PAD_CSPI1_MOSI = 132,
+	MX51_PAD_CSPI1_MISO = 133,
+	MX51_PAD_CSPI1_SS0 = 134,
+	MX51_PAD_CSPI1_SS1 = 135,
+	MX51_PAD_CSPI1_RDY = 136,
+	MX51_PAD_CSPI1_SCLK = 137,
+	MX51_PAD_UART1_RXD = 138,
+	MX51_PAD_UART1_TXD = 139,
+	MX51_PAD_UART1_RTS = 140,
+	MX51_PAD_UART1_CTS = 141,
+	MX51_PAD_UART2_RXD = 142,
+	MX51_PAD_UART2_TXD = 143,
+	MX51_PAD_UART3_RXD = 144,
+	MX51_PAD_UART3_TXD = 145,
+	MX51_PAD_OWIRE_LINE = 146,
+	MX51_PAD_KEY_ROW0 = 147,
+	MX51_PAD_KEY_ROW1 = 148,
+	MX51_PAD_KEY_ROW2 = 149,
+	MX51_PAD_KEY_ROW3 = 150,
+	MX51_PAD_KEY_COL0 = 151,
+	MX51_PAD_KEY_COL1 = 152,
+	MX51_PAD_KEY_COL2 = 153,
+	MX51_PAD_KEY_COL3 = 154,
+	MX51_PAD_KEY_COL4 = 155,
+	MX51_PAD_KEY_COL5 = 156,
+	MX51_PAD_RESERVE7 = 157,
+	MX51_PAD_USBH1_CLK = 158,
+	MX51_PAD_USBH1_DIR = 159,
+	MX51_PAD_USBH1_STP = 160,
+	MX51_PAD_USBH1_NXT = 161,
+	MX51_PAD_USBH1_DATA0 = 162,
+	MX51_PAD_USBH1_DATA1 = 163,
+	MX51_PAD_USBH1_DATA2 = 164,
+	MX51_PAD_USBH1_DATA3 = 165,
+	MX51_PAD_USBH1_DATA4 = 166,
+	MX51_PAD_USBH1_DATA5 = 167,
+	MX51_PAD_USBH1_DATA6 = 168,
+	MX51_PAD_USBH1_DATA7 = 169,
+	MX51_PAD_DI1_PIN11 = 170,
+	MX51_PAD_DI1_PIN12 = 171,
+	MX51_PAD_DI1_PIN13 = 172,
+	MX51_PAD_DI1_D0_CS = 173,
+	MX51_PAD_DI1_D1_CS = 174,
+	MX51_PAD_DISPB2_SER_DIN = 175,
+	MX51_PAD_DISPB2_SER_DIO = 176,
+	MX51_PAD_DISPB2_SER_CLK = 177,
+	MX51_PAD_DISPB2_SER_RS = 178,
+	MX51_PAD_DISP1_DAT0 = 179,
+	MX51_PAD_DISP1_DAT1 = 180,
+	MX51_PAD_DISP1_DAT2 = 181,
+	MX51_PAD_DISP1_DAT3 = 182,
+	MX51_PAD_DISP1_DAT4 = 183,
+	MX51_PAD_DISP1_DAT5 = 184,
+	MX51_PAD_DISP1_DAT6 = 185,
+	MX51_PAD_DISP1_DAT7 = 186,
+	MX51_PAD_DISP1_DAT8 = 187,
+	MX51_PAD_DISP1_DAT9 = 188,
+	MX51_PAD_DISP1_DAT10 = 189,
+	MX51_PAD_DISP1_DAT11 = 190,
+	MX51_PAD_DISP1_DAT12 = 191,
+	MX51_PAD_DISP1_DAT13 = 192,
+	MX51_PAD_DISP1_DAT14 = 193,
+	MX51_PAD_DISP1_DAT15 = 194,
+	MX51_PAD_DISP1_DAT16 = 195,
+	MX51_PAD_DISP1_DAT17 = 196,
+	MX51_PAD_DISP1_DAT18 = 197,
+	MX51_PAD_DISP1_DAT19 = 198,
+	MX51_PAD_DISP1_DAT20 = 199,
+	MX51_PAD_DISP1_DAT21 = 200,
+	MX51_PAD_DISP1_DAT22 = 201,
+	MX51_PAD_DISP1_DAT23 = 202,
+	MX51_PAD_DI1_PIN3 = 203,
+	MX51_PAD_DI1_PIN2 = 204,
+	MX51_PAD_RESERVE8 = 205,
+	MX51_PAD_DI_GP2 = 206,
+	MX51_PAD_DI_GP3 = 207,
+	MX51_PAD_DI2_PIN4 = 208,
+	MX51_PAD_DI2_PIN2 = 209,
+	MX51_PAD_DI2_PIN3 = 210,
+	MX51_PAD_DI2_DISP_CLK = 211,
+	MX51_PAD_DI_GP4 = 212,
+	MX51_PAD_DISP2_DAT0 = 213,
+	MX51_PAD_DISP2_DAT1 = 214,
+	MX51_PAD_DISP2_DAT2 = 215,
+	MX51_PAD_DISP2_DAT3 = 216,
+	MX51_PAD_DISP2_DAT4 = 217,
+	MX51_PAD_DISP2_DAT5 = 218,
+	MX51_PAD_DISP2_DAT6 = 219,
+	MX51_PAD_DISP2_DAT7 = 220,
+	MX51_PAD_DISP2_DAT8 = 221,
+	MX51_PAD_DISP2_DAT9 = 222,
+	MX51_PAD_DISP2_DAT10 = 223,
+	MX51_PAD_DISP2_DAT11 = 224,
+	MX51_PAD_DISP2_DAT12 = 225,
+	MX51_PAD_DISP2_DAT13 = 226,
+	MX51_PAD_DISP2_DAT14 = 227,
+	MX51_PAD_DISP2_DAT15 = 228,
+	MX51_PAD_SD1_CMD = 229,
+	MX51_PAD_SD1_CLK = 230,
+	MX51_PAD_SD1_DATA0 = 231,
+	MX51_PAD_SD1_DATA1 = 232,
+	MX51_PAD_SD1_DATA2 = 233,
+	MX51_PAD_SD1_DATA3 = 234,
+	MX51_PAD_GPIO1_0 = 235,
+	MX51_PAD_GPIO1_1 = 236,
+	MX51_PAD_SD2_CMD = 237,
+	MX51_PAD_SD2_CLK = 238,
+	MX51_PAD_SD2_DATA0 = 239,
+	MX51_PAD_SD2_DATA1 = 240,
+	MX51_PAD_SD2_DATA2 = 241,
+	MX51_PAD_SD2_DATA3 = 242,
+	MX51_PAD_GPIO1_2 = 243,
+	MX51_PAD_GPIO1_3 = 244,
+	MX51_PAD_PMIC_INT_REQ = 245,
+	MX51_PAD_GPIO1_4 = 246,
+	MX51_PAD_GPIO1_5 = 247,
+	MX51_PAD_GPIO1_6 = 248,
+	MX51_PAD_GPIO1_7 = 249,
+	MX51_PAD_GPIO1_8 = 250,
+	MX51_PAD_GPIO1_9 = 251,
+	MX51_PAD_RESERVE9 = 252,
+	MX51_PAD_RESERVE10 = 253,
+	MX51_PAD_RESERVE11 = 254,
+	MX51_PAD_RESERVE12 = 255,
+	MX51_PAD_RESERVE13 = 256,
+	MX51_PAD_RESERVE14 = 257,
+	MX51_PAD_RESERVE15 = 258,
+	MX51_PAD_RESERVE16 = 259,
+	MX51_PAD_RESERVE17 = 260,
+	MX51_PAD_RESERVE18 = 261,
+	MX51_PAD_RESERVE19 = 262,
+	MX51_PAD_RESERVE20 = 263,
+	MX51_PAD_RESERVE21 = 264,
+	MX51_PAD_RESERVE22 = 265,
+	MX51_PAD_RESERVE23 = 266,
+	MX51_PAD_RESERVE24 = 267,
+	MX51_PAD_RESERVE25 = 268,
+	MX51_PAD_RESERVE26 = 269,
+	MX51_PAD_RESERVE27 = 270,
+	MX51_PAD_RESERVE28 = 271,
+	MX51_PAD_RESERVE29 = 272,
+	MX51_PAD_RESERVE30 = 273,
+	MX51_PAD_RESERVE31 = 274,
+	MX51_PAD_RESERVE32 = 275,
+	MX51_PAD_RESERVE33 = 276,
+	MX51_PAD_RESERVE34 = 277,
+	MX51_PAD_RESERVE35 = 278,
+	MX51_PAD_RESERVE36 = 279,
+	MX51_PAD_RESERVE37 = 280,
+	MX51_PAD_RESERVE38 = 281,
+	MX51_PAD_RESERVE39 = 282,
+	MX51_PAD_RESERVE40 = 283,
+	MX51_PAD_RESERVE41 = 284,
+	MX51_PAD_RESERVE42 = 285,
+	MX51_PAD_RESERVE43 = 286,
+	MX51_PAD_RESERVE44 = 287,
+	MX51_PAD_RESERVE45 = 288,
+	MX51_PAD_RESERVE46 = 289,
+	MX51_PAD_RESERVE47 = 290,
+	MX51_PAD_RESERVE48 = 291,
+	MX51_PAD_RESERVE49 = 292,
+	MX51_PAD_RESERVE50 = 293,
+	MX51_PAD_RESERVE51 = 294,
+	MX51_PAD_RESERVE52 = 295,
+	MX51_PAD_RESERVE53 = 296,
+	MX51_PAD_RESERVE54 = 297,
+	MX51_PAD_RESERVE55 = 298,
+	MX51_PAD_RESERVE56 = 299,
+	MX51_PAD_RESERVE57 = 300,
+	MX51_PAD_RESERVE58 = 301,
+	MX51_PAD_RESERVE59 = 302,
+	MX51_PAD_RESERVE60 = 303,
+	MX51_PAD_RESERVE61 = 304,
+	MX51_PAD_RESERVE62 = 305,
+	MX51_PAD_RESERVE63 = 306,
+	MX51_PAD_RESERVE64 = 307,
+	MX51_PAD_RESERVE65 = 308,
+	MX51_PAD_RESERVE66 = 309,
+	MX51_PAD_RESERVE67 = 310,
+	MX51_PAD_RESERVE68 = 311,
+	MX51_PAD_RESERVE69 = 312,
+	MX51_PAD_RESERVE70 = 313,
+	MX51_PAD_RESERVE71 = 314,
+	MX51_PAD_RESERVE72 = 315,
+	MX51_PAD_RESERVE73 = 316,
+	MX51_PAD_RESERVE74 = 317,
+	MX51_PAD_RESERVE75 = 318,
+	MX51_PAD_RESERVE76 = 319,
+	MX51_PAD_RESERVE77 = 320,
+	MX51_PAD_RESERVE78 = 321,
+	MX51_PAD_RESERVE79 = 322,
+	MX51_PAD_RESERVE80 = 323,
+	MX51_PAD_RESERVE81 = 324,
+	MX51_PAD_RESERVE82 = 325,
+	MX51_PAD_RESERVE83 = 326,
+	MX51_PAD_RESERVE84 = 327,
+	MX51_PAD_RESERVE85 = 328,
+	MX51_PAD_RESERVE86 = 329,
+	MX51_PAD_RESERVE87 = 330,
+	MX51_PAD_RESERVE88 = 331,
+	MX51_PAD_RESERVE89 = 332,
+	MX51_PAD_RESERVE90 = 333,
+	MX51_PAD_RESERVE91 = 334,
+	MX51_PAD_RESERVE92 = 335,
+	MX51_PAD_RESERVE93 = 336,
+	MX51_PAD_RESERVE94 = 337,
+	MX51_PAD_RESERVE95 = 338,
+	MX51_PAD_RESERVE96 = 339,
+	MX51_PAD_RESERVE97 = 340,
+	MX51_PAD_RESERVE98 = 341,
+	MX51_PAD_RESERVE99 = 342,
+	MX51_PAD_RESERVE100 = 343,
+	MX51_PAD_RESERVE101 = 344,
+	MX51_PAD_RESERVE102 = 345,
+	MX51_PAD_RESERVE103 = 346,
+	MX51_PAD_RESERVE104 = 347,
+	MX51_PAD_RESERVE105 = 348,
+	MX51_PAD_RESERVE106 = 349,
+	MX51_PAD_RESERVE107 = 350,
+	MX51_PAD_RESERVE108 = 351,
+	MX51_PAD_RESERVE109 = 352,
+	MX51_PAD_RESERVE110 = 353,
+	MX51_PAD_RESERVE111 = 354,
+	MX51_PAD_RESERVE112 = 355,
+	MX51_PAD_RESERVE113 = 356,
+	MX51_PAD_RESERVE114 = 357,
+	MX51_PAD_RESERVE115 = 358,
+	MX51_PAD_RESERVE116 = 359,
+	MX51_PAD_RESERVE117 = 360,
+	MX51_PAD_RESERVE118 = 361,
+	MX51_PAD_RESERVE119 = 362,
+	MX51_PAD_RESERVE120 = 363,
+	MX51_PAD_RESERVE121 = 364,
+	MX51_PAD_CSI1_PIXCLK = 365,
+	MX51_PAD_CSI1_MCLK = 366,
 };
 
 /* Pad names for the pinmux subsystem */
 static const struct pinctrl_pin_desc imx51_pinctrl_pads[] = {
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE0),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE1),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE2),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE3),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE4),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE5),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE6),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA0),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA1),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA2),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA3),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA4),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA5),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA6),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA7),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA8),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA9),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA10),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA11),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA12),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA13),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA14),
+	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA15),
 	IMX_PINCTRL_PIN(MX51_PAD_EIM_D16),
 	IMX_PINCTRL_PIN(MX51_PAD_EIM_D17),
 	IMX_PINCTRL_PIN(MX51_PAD_EIM_D18),
@@ -1124,8 +509,6 @@ static const struct pinctrl_pin_desc imx51_pinctrl_pads[] = {
 	IMX_PINCTRL_PIN(MX51_PAD_CSI1_D19),
 	IMX_PINCTRL_PIN(MX51_PAD_CSI1_VSYNC),
 	IMX_PINCTRL_PIN(MX51_PAD_CSI1_HSYNC),
-	IMX_PINCTRL_PIN(MX51_PAD_CSI1_PIXCLK),
-	IMX_PINCTRL_PIN(MX51_PAD_CSI1_MCLK),
 	IMX_PINCTRL_PIN(MX51_PAD_CSI2_D12),
 	IMX_PINCTRL_PIN(MX51_PAD_CSI2_D13),
 	IMX_PINCTRL_PIN(MX51_PAD_CSI2_D14),
@@ -1168,6 +551,7 @@ static const struct pinctrl_pin_desc imx51_pinctrl_pads[] = {
 	IMX_PINCTRL_PIN(MX51_PAD_KEY_COL3),
 	IMX_PINCTRL_PIN(MX51_PAD_KEY_COL4),
 	IMX_PINCTRL_PIN(MX51_PAD_KEY_COL5),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE7),
 	IMX_PINCTRL_PIN(MX51_PAD_USBH1_CLK),
 	IMX_PINCTRL_PIN(MX51_PAD_USBH1_DIR),
 	IMX_PINCTRL_PIN(MX51_PAD_USBH1_STP),
@@ -1215,6 +599,7 @@ static const struct pinctrl_pin_desc imx51_pinctrl_pads[] = {
 	IMX_PINCTRL_PIN(MX51_PAD_DISP1_DAT23),
 	IMX_PINCTRL_PIN(MX51_PAD_DI1_PIN3),
 	IMX_PINCTRL_PIN(MX51_PAD_DI1_PIN2),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE8),
 	IMX_PINCTRL_PIN(MX51_PAD_DI_GP2),
 	IMX_PINCTRL_PIN(MX51_PAD_DI_GP3),
 	IMX_PINCTRL_PIN(MX51_PAD_DI2_PIN4),
@@ -1241,27 +626,11 @@ static const struct pinctrl_pin_desc imx51_pinctrl_pads[] = {
 	IMX_PINCTRL_PIN(MX51_PAD_SD1_CMD),
 	IMX_PINCTRL_PIN(MX51_PAD_SD1_CLK),
 	IMX_PINCTRL_PIN(MX51_PAD_SD1_DATA0),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA0),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA1),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA2),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA3),
 	IMX_PINCTRL_PIN(MX51_PAD_SD1_DATA1),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA4),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA5),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA6),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA7),
 	IMX_PINCTRL_PIN(MX51_PAD_SD1_DATA2),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA10),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA11),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA8),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA9),
 	IMX_PINCTRL_PIN(MX51_PAD_SD1_DATA3),
 	IMX_PINCTRL_PIN(MX51_PAD_GPIO1_0),
 	IMX_PINCTRL_PIN(MX51_PAD_GPIO1_1),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA12),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA13),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA14),
-	IMX_PINCTRL_PIN(MX51_PAD_EIM_DA15),
 	IMX_PINCTRL_PIN(MX51_PAD_SD2_CMD),
 	IMX_PINCTRL_PIN(MX51_PAD_SD2_CLK),
 	IMX_PINCTRL_PIN(MX51_PAD_SD2_DATA0),
@@ -1277,13 +646,126 @@ static const struct pinctrl_pin_desc imx51_pinctrl_pads[] = {
 	IMX_PINCTRL_PIN(MX51_PAD_GPIO1_7),
 	IMX_PINCTRL_PIN(MX51_PAD_GPIO1_8),
 	IMX_PINCTRL_PIN(MX51_PAD_GPIO1_9),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE9),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE10),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE11),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE12),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE13),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE14),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE15),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE16),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE17),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE18),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE19),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE20),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE21),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE22),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE23),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE24),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE25),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE26),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE27),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE28),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE29),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE30),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE31),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE32),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE33),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE34),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE35),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE36),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE37),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE38),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE39),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE40),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE41),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE42),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE43),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE44),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE45),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE46),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE47),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE48),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE49),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE50),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE51),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE52),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE53),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE54),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE55),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE56),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE57),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE58),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE59),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE60),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE61),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE62),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE63),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE64),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE65),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE66),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE67),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE68),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE69),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE70),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE71),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE72),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE73),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE74),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE75),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE76),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE77),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE78),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE79),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE80),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE81),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE82),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE83),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE84),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE85),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE86),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE87),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE88),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE89),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE90),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE91),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE92),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE93),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE94),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE95),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE96),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE97),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE98),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE99),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE100),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE101),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE102),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE103),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE104),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE105),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE106),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE107),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE108),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE109),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE110),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE111),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE112),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE113),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE114),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE115),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE116),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE117),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE118),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE119),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE120),
+	IMX_PINCTRL_PIN(MX51_PAD_RESERVE121),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI1_PIXCLK),
+	IMX_PINCTRL_PIN(MX51_PAD_CSI1_MCLK),
 };
 
 static struct imx_pinctrl_soc_info imx51_pinctrl_info = {
 	.pins = imx51_pinctrl_pads,
 	.npins = ARRAY_SIZE(imx51_pinctrl_pads),
-	.pin_regs = imx51_pin_regs,
-	.npin_regs = ARRAY_SIZE(imx51_pin_regs),
 };
 
 static struct of_device_id imx51_pinctrl_of_match[] = {
diff --git a/drivers/pinctrl/pinctrl-imx53.c b/drivers/pinctrl/pinctrl-imx53.c
index 2c9c8e2334da..17562ae9005c 100644
--- a/drivers/pinctrl/pinctrl-imx53.c
+++ b/drivers/pinctrl/pinctrl-imx53.c
@@ -23,1386 +23,228 @@
 #include "pinctrl-imx.h"
 
 enum imx53_pads {
-	MX53_PAD_GPIO_19 = 0,
-	MX53_PAD_KEY_COL0 = 1,
-	MX53_PAD_KEY_ROW0 = 2,
-	MX53_PAD_KEY_COL1 = 3,
-	MX53_PAD_KEY_ROW1 = 4,
-	MX53_PAD_KEY_COL2 = 5,
-	MX53_PAD_KEY_ROW2 = 6,
-	MX53_PAD_KEY_COL3 = 7,
-	MX53_PAD_KEY_ROW3 = 8,
-	MX53_PAD_KEY_COL4 = 9,
-	MX53_PAD_KEY_ROW4 = 10,
-	MX53_PAD_DI0_DISP_CLK = 11,
-	MX53_PAD_DI0_PIN15 = 12,
-	MX53_PAD_DI0_PIN2 = 13,
-	MX53_PAD_DI0_PIN3 = 14,
-	MX53_PAD_DI0_PIN4 = 15,
-	MX53_PAD_DISP0_DAT0 = 16,
-	MX53_PAD_DISP0_DAT1 = 17,
-	MX53_PAD_DISP0_DAT2 = 18,
-	MX53_PAD_DISP0_DAT3 = 19,
-	MX53_PAD_DISP0_DAT4 = 20,
-	MX53_PAD_DISP0_DAT5 = 21,
-	MX53_PAD_DISP0_DAT6 = 22,
-	MX53_PAD_DISP0_DAT7 = 23,
-	MX53_PAD_DISP0_DAT8 = 24,
-	MX53_PAD_DISP0_DAT9 = 25,
-	MX53_PAD_DISP0_DAT10 = 26,
-	MX53_PAD_DISP0_DAT11 = 27,
-	MX53_PAD_DISP0_DAT12 = 28,
-	MX53_PAD_DISP0_DAT13 = 29,
-	MX53_PAD_DISP0_DAT14 = 30,
-	MX53_PAD_DISP0_DAT15 = 31,
-	MX53_PAD_DISP0_DAT16 = 32,
-	MX53_PAD_DISP0_DAT17 = 33,
-	MX53_PAD_DISP0_DAT18 = 34,
-	MX53_PAD_DISP0_DAT19 = 35,
-	MX53_PAD_DISP0_DAT20 = 36,
-	MX53_PAD_DISP0_DAT21 = 37,
-	MX53_PAD_DISP0_DAT22 = 38,
-	MX53_PAD_DISP0_DAT23 = 39,
-	MX53_PAD_CSI0_PIXCLK = 40,
-	MX53_PAD_CSI0_MCLK = 41,
-	MX53_PAD_CSI0_DATA_EN = 42,
-	MX53_PAD_CSI0_VSYNC = 43,
-	MX53_PAD_CSI0_DAT4 = 44,
-	MX53_PAD_CSI0_DAT5 = 45,
-	MX53_PAD_CSI0_DAT6 = 46,
-	MX53_PAD_CSI0_DAT7 = 47,
-	MX53_PAD_CSI0_DAT8 = 48,
-	MX53_PAD_CSI0_DAT9 = 49,
-	MX53_PAD_CSI0_DAT10 = 50,
-	MX53_PAD_CSI0_DAT11 = 51,
-	MX53_PAD_CSI0_DAT12 = 52,
-	MX53_PAD_CSI0_DAT13 = 53,
-	MX53_PAD_CSI0_DAT14 = 54,
-	MX53_PAD_CSI0_DAT15 = 55,
-	MX53_PAD_CSI0_DAT16 = 56,
-	MX53_PAD_CSI0_DAT17 = 57,
-	MX53_PAD_CSI0_DAT18 = 58,
-	MX53_PAD_CSI0_DAT19 = 59,
-	MX53_PAD_EIM_A25 = 60,
-	MX53_PAD_EIM_EB2 = 61,
-	MX53_PAD_EIM_D16 = 62,
-	MX53_PAD_EIM_D17 = 63,
-	MX53_PAD_EIM_D18 = 64,
-	MX53_PAD_EIM_D19 = 65,
-	MX53_PAD_EIM_D20 = 66,
-	MX53_PAD_EIM_D21 = 67,
-	MX53_PAD_EIM_D22 = 68,
-	MX53_PAD_EIM_D23 = 69,
-	MX53_PAD_EIM_EB3 = 70,
-	MX53_PAD_EIM_D24 = 71,
-	MX53_PAD_EIM_D25 = 72,
-	MX53_PAD_EIM_D26 = 73,
-	MX53_PAD_EIM_D27 = 74,
-	MX53_PAD_EIM_D28 = 75,
-	MX53_PAD_EIM_D29 = 76,
-	MX53_PAD_EIM_D30 = 77,
-	MX53_PAD_EIM_D31 = 78,
-	MX53_PAD_EIM_A24 = 79,
-	MX53_PAD_EIM_A23 = 80,
-	MX53_PAD_EIM_A22 = 81,
-	MX53_PAD_EIM_A21 = 82,
-	MX53_PAD_EIM_A20 = 83,
-	MX53_PAD_EIM_A19 = 84,
-	MX53_PAD_EIM_A18 = 85,
-	MX53_PAD_EIM_A17 = 86,
-	MX53_PAD_EIM_A16 = 87,
-	MX53_PAD_EIM_CS0 = 88,
-	MX53_PAD_EIM_CS1 = 89,
-	MX53_PAD_EIM_OE = 90,
-	MX53_PAD_EIM_RW = 91,
-	MX53_PAD_EIM_LBA = 92,
-	MX53_PAD_EIM_EB0 = 93,
-	MX53_PAD_EIM_EB1 = 94,
-	MX53_PAD_EIM_DA0 = 95,
-	MX53_PAD_EIM_DA1 = 96,
-	MX53_PAD_EIM_DA2 = 97,
-	MX53_PAD_EIM_DA3 = 98,
-	MX53_PAD_EIM_DA4 = 99,
-	MX53_PAD_EIM_DA5 = 100,
-	MX53_PAD_EIM_DA6 = 101,
-	MX53_PAD_EIM_DA7 = 102,
-	MX53_PAD_EIM_DA8 = 103,
-	MX53_PAD_EIM_DA9 = 104,
-	MX53_PAD_EIM_DA10 = 105,
-	MX53_PAD_EIM_DA11 = 106,
-	MX53_PAD_EIM_DA12 = 107,
-	MX53_PAD_EIM_DA13 = 108,
-	MX53_PAD_EIM_DA14 = 109,
-	MX53_PAD_EIM_DA15 = 110,
-	MX53_PAD_NANDF_WE_B = 111,
-	MX53_PAD_NANDF_RE_B = 112,
-	MX53_PAD_EIM_WAIT = 113,
-	MX53_PAD_LVDS1_TX3_P = 114,
-	MX53_PAD_LVDS1_TX2_P = 115,
-	MX53_PAD_LVDS1_CLK_P = 116,
-	MX53_PAD_LVDS1_TX1_P = 117,
-	MX53_PAD_LVDS1_TX0_P = 118,
-	MX53_PAD_LVDS0_TX3_P = 119,
-	MX53_PAD_LVDS0_CLK_P = 120,
-	MX53_PAD_LVDS0_TX2_P = 121,
-	MX53_PAD_LVDS0_TX1_P = 122,
-	MX53_PAD_LVDS0_TX0_P = 123,
-	MX53_PAD_GPIO_10 = 124,
-	MX53_PAD_GPIO_11 = 125,
-	MX53_PAD_GPIO_12 = 126,
-	MX53_PAD_GPIO_13 = 127,
-	MX53_PAD_GPIO_14 = 128,
-	MX53_PAD_NANDF_CLE = 129,
-	MX53_PAD_NANDF_ALE = 130,
-	MX53_PAD_NANDF_WP_B = 131,
-	MX53_PAD_NANDF_RB0 = 132,
-	MX53_PAD_NANDF_CS0 = 133,
-	MX53_PAD_NANDF_CS1 = 134,
-	MX53_PAD_NANDF_CS2 = 135,
-	MX53_PAD_NANDF_CS3 = 136,
-	MX53_PAD_FEC_MDIO = 137,
-	MX53_PAD_FEC_REF_CLK = 138,
-	MX53_PAD_FEC_RX_ER = 139,
-	MX53_PAD_FEC_CRS_DV = 140,
-	MX53_PAD_FEC_RXD1 = 141,
-	MX53_PAD_FEC_RXD0 = 142,
-	MX53_PAD_FEC_TX_EN = 143,
-	MX53_PAD_FEC_TXD1 = 144,
-	MX53_PAD_FEC_TXD0 = 145,
-	MX53_PAD_FEC_MDC = 146,
-	MX53_PAD_PATA_DIOW = 147,
-	MX53_PAD_PATA_DMACK = 148,
-	MX53_PAD_PATA_DMARQ = 149,
-	MX53_PAD_PATA_BUFFER_EN = 150,
-	MX53_PAD_PATA_INTRQ = 151,
-	MX53_PAD_PATA_DIOR = 152,
-	MX53_PAD_PATA_RESET_B = 153,
-	MX53_PAD_PATA_IORDY = 154,
-	MX53_PAD_PATA_DA_0 = 155,
-	MX53_PAD_PATA_DA_1 = 156,
-	MX53_PAD_PATA_DA_2 = 157,
-	MX53_PAD_PATA_CS_0 = 158,
-	MX53_PAD_PATA_CS_1 = 159,
-	MX53_PAD_PATA_DATA0 = 160,
-	MX53_PAD_PATA_DATA1 = 161,
-	MX53_PAD_PATA_DATA2 = 162,
-	MX53_PAD_PATA_DATA3 = 163,
-	MX53_PAD_PATA_DATA4 = 164,
-	MX53_PAD_PATA_DATA5 = 165,
-	MX53_PAD_PATA_DATA6 = 166,
-	MX53_PAD_PATA_DATA7 = 167,
-	MX53_PAD_PATA_DATA8 = 168,
-	MX53_PAD_PATA_DATA9 = 169,
-	MX53_PAD_PATA_DATA10 = 170,
-	MX53_PAD_PATA_DATA11 = 171,
-	MX53_PAD_PATA_DATA12 = 172,
-	MX53_PAD_PATA_DATA13 = 173,
-	MX53_PAD_PATA_DATA14 = 174,
-	MX53_PAD_PATA_DATA15 = 175,
-	MX53_PAD_SD1_DATA0 = 176,
-	MX53_PAD_SD1_DATA1 = 177,
-	MX53_PAD_SD1_CMD = 178,
-	MX53_PAD_SD1_DATA2 = 179,
-	MX53_PAD_SD1_CLK = 180,
-	MX53_PAD_SD1_DATA3 = 181,
-	MX53_PAD_SD2_CLK = 182,
-	MX53_PAD_SD2_CMD = 183,
-	MX53_PAD_SD2_DATA3 = 184,
-	MX53_PAD_SD2_DATA2 = 185,
-	MX53_PAD_SD2_DATA1 = 186,
-	MX53_PAD_SD2_DATA0 = 187,
-	MX53_PAD_GPIO_0 = 188,
-	MX53_PAD_GPIO_1 = 189,
-	MX53_PAD_GPIO_9 = 190,
-	MX53_PAD_GPIO_3 = 191,
-	MX53_PAD_GPIO_6 = 192,
-	MX53_PAD_GPIO_2 = 193,
-	MX53_PAD_GPIO_4 = 194,
-	MX53_PAD_GPIO_5 = 195,
-	MX53_PAD_GPIO_7 = 196,
-	MX53_PAD_GPIO_8 = 197,
-	MX53_PAD_GPIO_16 = 198,
-	MX53_PAD_GPIO_17 = 199,
-	MX53_PAD_GPIO_18 = 200,
-};
-
-/* imx53 register maps */
-static struct imx_pin_reg imx53_pin_regs[] = {
-	IMX_PIN_REG(MX53_PAD_GPIO_19, 0x348, 0x020, 0, 0x840, 0), /* MX53_PAD_GPIO_19__KPP_COL_5 */
-	IMX_PIN_REG(MX53_PAD_GPIO_19, 0x348, 0x020, 1, 0x000, 0), /* MX53_PAD_GPIO_19__GPIO4_5 */
-	IMX_PIN_REG(MX53_PAD_GPIO_19, 0x348, 0x020, 2, 0x000, 0), /* MX53_PAD_GPIO_19__CCM_CLKO */
-	IMX_PIN_REG(MX53_PAD_GPIO_19, 0x348, 0x020, 3, 0x000, 0), /* MX53_PAD_GPIO_19__SPDIF_OUT1 */
-	IMX_PIN_REG(MX53_PAD_GPIO_19, 0x348, 0x020, 4, 0x000, 0), /* MX53_PAD_GPIO_19__RTC_CE_RTC_EXT_TRIG2 */
-	IMX_PIN_REG(MX53_PAD_GPIO_19, 0x348, 0x020, 5, 0x000, 0), /* MX53_PAD_GPIO_19__ECSPI1_RDY */
-	IMX_PIN_REG(MX53_PAD_GPIO_19, 0x348, 0x020, 6, 0x000, 0), /* MX53_PAD_GPIO_19__FEC_TDATA_3 */
-	IMX_PIN_REG(MX53_PAD_GPIO_19, 0x348, 0x020, 7, 0x000, 0), /* MX53_PAD_GPIO_19__SRC_INT_BOOT */
-	IMX_PIN_REG(MX53_PAD_KEY_COL0, 0x34C, 0x024, 0, 0x000, 0), /* MX53_PAD_KEY_COL0__KPP_COL_0 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL0, 0x34C, 0x024, 1, 0x000, 0), /* MX53_PAD_KEY_COL0__GPIO4_6 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL0, 0x34C, 0x024, 2, 0x758, 0), /* MX53_PAD_KEY_COL0__AUDMUX_AUD5_TXC */
-	IMX_PIN_REG(MX53_PAD_KEY_COL0, 0x34C, 0x024, 4, 0x000, 0), /* MX53_PAD_KEY_COL0__UART4_TXD_MUX */
-	IMX_PIN_REG(MX53_PAD_KEY_COL0, 0x34C, 0x024, 5, 0x79C, 0), /* MX53_PAD_KEY_COL0__ECSPI1_SCLK */
-	IMX_PIN_REG(MX53_PAD_KEY_COL0, 0x34C, 0x024, 6, 0x000, 0), /* MX53_PAD_KEY_COL0__FEC_RDATA_3 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL0, 0x34C, 0x024, 7, 0x000, 0), /* MX53_PAD_KEY_COL0__SRC_ANY_PU_RST */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW0, 0x350, 0x028, 0, 0x000, 0), /* MX53_PAD_KEY_ROW0__KPP_ROW_0 */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW0, 0x350, 0x028, 1, 0x000, 0), /* MX53_PAD_KEY_ROW0__GPIO4_7 */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW0, 0x350, 0x028, 2, 0x74C, 0), /* MX53_PAD_KEY_ROW0__AUDMUX_AUD5_TXD */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW0, 0x350, 0x028, 4, 0x890, 1), /* MX53_PAD_KEY_ROW0__UART4_RXD_MUX */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW0, 0x350, 0x028, 5, 0x7A4, 0), /* MX53_PAD_KEY_ROW0__ECSPI1_MOSI */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW0, 0x350, 0x028, 6, 0x000, 0), /* MX53_PAD_KEY_ROW0__FEC_TX_ER */
-	IMX_PIN_REG(MX53_PAD_KEY_COL1, 0x354, 0x02C, 0, 0x000, 0), /* MX53_PAD_KEY_COL1__KPP_COL_1 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL1, 0x354, 0x02C, 1, 0x000, 0), /* MX53_PAD_KEY_COL1__GPIO4_8 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL1, 0x354, 0x02C, 2, 0x75C, 0), /* MX53_PAD_KEY_COL1__AUDMUX_AUD5_TXFS */
-	IMX_PIN_REG(MX53_PAD_KEY_COL1, 0x354, 0x02C, 4, 0x000, 0), /* MX53_PAD_KEY_COL1__UART5_TXD_MUX */
-	IMX_PIN_REG(MX53_PAD_KEY_COL1, 0x354, 0x02C, 5, 0x7A0, 0), /* MX53_PAD_KEY_COL1__ECSPI1_MISO */
-	IMX_PIN_REG(MX53_PAD_KEY_COL1, 0x354, 0x02C, 6, 0x808, 0), /* MX53_PAD_KEY_COL1__FEC_RX_CLK */
-	IMX_PIN_REG(MX53_PAD_KEY_COL1, 0x354, 0x02C, 7, 0x000, 0), /* MX53_PAD_KEY_COL1__USBPHY1_TXREADY */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW1, 0x358, 0x030, 0, 0x000, 0), /* MX53_PAD_KEY_ROW1__KPP_ROW_1 */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW1, 0x358, 0x030, 1, 0x000, 0), /* MX53_PAD_KEY_ROW1__GPIO4_9 */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW1, 0x358, 0x030, 2, 0x748, 0), /* MX53_PAD_KEY_ROW1__AUDMUX_AUD5_RXD */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW1, 0x358, 0x030, 4, 0x898, 1), /* MX53_PAD_KEY_ROW1__UART5_RXD_MUX */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW1, 0x358, 0x030, 5, 0x7A8, 0), /* MX53_PAD_KEY_ROW1__ECSPI1_SS0 */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW1, 0x358, 0x030, 6, 0x800, 0), /* MX53_PAD_KEY_ROW1__FEC_COL */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW1, 0x358, 0x030, 7, 0x000, 0), /* MX53_PAD_KEY_ROW1__USBPHY1_RXVALID */
-	IMX_PIN_REG(MX53_PAD_KEY_COL2, 0x35C, 0x034, 0, 0x000, 0), /* MX53_PAD_KEY_COL2__KPP_COL_2 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL2, 0x35C, 0x034, 1, 0x000, 0), /* MX53_PAD_KEY_COL2__GPIO4_10 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL2, 0x35C, 0x034, 2, 0x000, 0), /* MX53_PAD_KEY_COL2__CAN1_TXCAN */
-	IMX_PIN_REG(MX53_PAD_KEY_COL2, 0x35C, 0x034, 4, 0x804, 0), /* MX53_PAD_KEY_COL2__FEC_MDIO */
-	IMX_PIN_REG(MX53_PAD_KEY_COL2, 0x35C, 0x034, 5, 0x7AC, 0), /* MX53_PAD_KEY_COL2__ECSPI1_SS1 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL2, 0x35C, 0x034, 6, 0x000, 0), /* MX53_PAD_KEY_COL2__FEC_RDATA_2 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL2, 0x35C, 0x034, 7, 0x000, 0), /* MX53_PAD_KEY_COL2__USBPHY1_RXACTIVE */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW2, 0x360, 0x038, 0, 0x000, 0), /* MX53_PAD_KEY_ROW2__KPP_ROW_2 */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW2, 0x360, 0x038, 1, 0x000, 0), /* MX53_PAD_KEY_ROW2__GPIO4_11 */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW2, 0x360, 0x038, 2, 0x760, 0), /* MX53_PAD_KEY_ROW2__CAN1_RXCAN */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW2, 0x360, 0x038, 4, 0x000, 0), /* MX53_PAD_KEY_ROW2__FEC_MDC */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW2, 0x360, 0x038, 5, 0x7B0, 0), /* MX53_PAD_KEY_ROW2__ECSPI1_SS2 */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW2, 0x360, 0x038, 6, 0x000, 0), /* MX53_PAD_KEY_ROW2__FEC_TDATA_2 */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW2, 0x360, 0x038, 7, 0x000, 0), /* MX53_PAD_KEY_ROW2__USBPHY1_RXERROR */
-	IMX_PIN_REG(MX53_PAD_KEY_COL3, 0x364, 0x03C, 0, 0x000, 0), /* MX53_PAD_KEY_COL3__KPP_COL_3 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL3, 0x364, 0x03C, 1, 0x000, 0), /* MX53_PAD_KEY_COL3__GPIO4_12 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL3, 0x364, 0x03C, 2, 0x000, 0), /* MX53_PAD_KEY_COL3__USBOH3_H2_DP */
-	IMX_PIN_REG(MX53_PAD_KEY_COL3, 0x364, 0x03C, 3, 0x870, 0), /* MX53_PAD_KEY_COL3__SPDIF_IN1 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL3, 0x364, 0x03C, 4, 0x81C, 0), /* MX53_PAD_KEY_COL3__I2C2_SCL */
-	IMX_PIN_REG(MX53_PAD_KEY_COL3, 0x364, 0x03C, 5, 0x7B4, 0), /* MX53_PAD_KEY_COL3__ECSPI1_SS3 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL3, 0x364, 0x03C, 6, 0x000, 0), /* MX53_PAD_KEY_COL3__FEC_CRS */
-	IMX_PIN_REG(MX53_PAD_KEY_COL3, 0x364, 0x03C, 7, 0x000, 0), /* MX53_PAD_KEY_COL3__USBPHY1_SIECLOCK */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW3, 0x368, 0x040, 0, 0x000, 0), /* MX53_PAD_KEY_ROW3__KPP_ROW_3 */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW3, 0x368, 0x040, 1, 0x000, 0), /* MX53_PAD_KEY_ROW3__GPIO4_13 */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW3, 0x368, 0x040, 2, 0x000, 0), /* MX53_PAD_KEY_ROW3__USBOH3_H2_DM */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW3, 0x368, 0x040, 3, 0x768, 0), /* MX53_PAD_KEY_ROW3__CCM_ASRC_EXT_CLK */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW3, 0x368, 0x040, 4, 0x820, 0), /* MX53_PAD_KEY_ROW3__I2C2_SDA */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW3, 0x368, 0x040, 5, 0x000, 0), /* MX53_PAD_KEY_ROW3__OSC32K_32K_OUT */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW3, 0x368, 0x040, 6, 0x77C, 0), /* MX53_PAD_KEY_ROW3__CCM_PLL4_BYP */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW3, 0x368, 0x040, 7, 0x000, 0), /* MX53_PAD_KEY_ROW3__USBPHY1_LINESTATE_0 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL4, 0x36C, 0x044, 0, 0x000, 0), /* MX53_PAD_KEY_COL4__KPP_COL_4 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL4, 0x36C, 0x044, 1, 0x000, 0), /* MX53_PAD_KEY_COL4__GPIO4_14 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL4, 0x36C, 0x044, 2, 0x000, 0), /* MX53_PAD_KEY_COL4__CAN2_TXCAN */
-	IMX_PIN_REG(MX53_PAD_KEY_COL4, 0x36C, 0x044, 3, 0x000, 0), /* MX53_PAD_KEY_COL4__IPU_SISG_4 */
-	IMX_PIN_REG(MX53_PAD_KEY_COL4, 0x36C, 0x044, 4, 0x894, 0), /* MX53_PAD_KEY_COL4__UART5_RTS */
-	IMX_PIN_REG(MX53_PAD_KEY_COL4, 0x36C, 0x044, 5, 0x89C, 0), /* MX53_PAD_KEY_COL4__USBOH3_USBOTG_OC */
-	IMX_PIN_REG(MX53_PAD_KEY_COL4, 0x36C, 0x044, 7, 0x000, 0), /* MX53_PAD_KEY_COL4__USBPHY1_LINESTATE_1 */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW4, 0x370, 0x048, 0, 0x000, 0), /* MX53_PAD_KEY_ROW4__KPP_ROW_4 */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW4, 0x370, 0x048, 1, 0x000, 0), /* MX53_PAD_KEY_ROW4__GPIO4_15 */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW4, 0x370, 0x048, 2, 0x764, 0), /* MX53_PAD_KEY_ROW4__CAN2_RXCAN */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW4, 0x370, 0x048, 3, 0x000, 0), /* MX53_PAD_KEY_ROW4__IPU_SISG_5 */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW4, 0x370, 0x048, 4, 0x000, 0), /* MX53_PAD_KEY_ROW4__UART5_CTS */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW4, 0x370, 0x048, 5, 0x000, 0), /* MX53_PAD_KEY_ROW4__USBOH3_USBOTG_PWR */
-	IMX_PIN_REG(MX53_PAD_KEY_ROW4, 0x370, 0x048, 7, 0x000, 0), /* MX53_PAD_KEY_ROW4__USBPHY1_VBUSVALID */
-	IMX_PIN_REG(MX53_PAD_DI0_DISP_CLK, 0x378, 0x04C, 0, 0x000, 0), /* MX53_PAD_DI0_DISP_CLK__IPU_DI0_DISP_CLK */
-	IMX_PIN_REG(MX53_PAD_DI0_DISP_CLK, 0x378, 0x04C, 1, 0x000, 0), /* MX53_PAD_DI0_DISP_CLK__GPIO4_16 */
-	IMX_PIN_REG(MX53_PAD_DI0_DISP_CLK, 0x378, 0x04C, 2, 0x000, 0), /* MX53_PAD_DI0_DISP_CLK__USBOH3_USBH2_DIR */
-	IMX_PIN_REG(MX53_PAD_DI0_DISP_CLK, 0x378, 0x04C, 5, 0x000, 0), /* MX53_PAD_DI0_DISP_CLK__SDMA_DEBUG_CORE_STATE_0 */
-	IMX_PIN_REG(MX53_PAD_DI0_DISP_CLK, 0x378, 0x04C, 6, 0x000, 0), /* MX53_PAD_DI0_DISP_CLK__EMI_EMI_DEBUG_0 */
-	IMX_PIN_REG(MX53_PAD_DI0_DISP_CLK, 0x378, 0x04C, 7, 0x000, 0), /* MX53_PAD_DI0_DISP_CLK__USBPHY1_AVALID */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN15, 0x37C, 0x050, 0, 0x000, 0), /* MX53_PAD_DI0_PIN15__IPU_DI0_PIN15 */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN15, 0x37C, 0x050, 1, 0x000, 0), /* MX53_PAD_DI0_PIN15__GPIO4_17 */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN15, 0x37C, 0x050, 2, 0x000, 0), /* MX53_PAD_DI0_PIN15__AUDMUX_AUD6_TXC */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN15, 0x37C, 0x050, 5, 0x000, 0), /* MX53_PAD_DI0_PIN15__SDMA_DEBUG_CORE_STATE_1 */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN15, 0x37C, 0x050, 6, 0x000, 0), /* MX53_PAD_DI0_PIN15__EMI_EMI_DEBUG_1 */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN15, 0x37C, 0x050, 7, 0x000, 0), /* MX53_PAD_DI0_PIN15__USBPHY1_BVALID */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN2, 0x380, 0x054, 0, 0x000, 0), /* MX53_PAD_DI0_PIN2__IPU_DI0_PIN2 */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN2, 0x380, 0x054, 1, 0x000, 0), /* MX53_PAD_DI0_PIN2__GPIO4_18 */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN2, 0x380, 0x054, 2, 0x000, 0), /* MX53_PAD_DI0_PIN2__AUDMUX_AUD6_TXD */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN2, 0x380, 0x054, 5, 0x000, 0), /* MX53_PAD_DI0_PIN2__SDMA_DEBUG_CORE_STATE_2 */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN2, 0x380, 0x054, 6, 0x000, 0), /* MX53_PAD_DI0_PIN2__EMI_EMI_DEBUG_2 */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN2, 0x380, 0x054, 7, 0x000, 0), /* MX53_PAD_DI0_PIN2__USBPHY1_ENDSESSION */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN3, 0x384, 0x058, 0, 0x000, 0), /* MX53_PAD_DI0_PIN3__IPU_DI0_PIN3 */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN3, 0x384, 0x058, 1, 0x000, 0), /* MX53_PAD_DI0_PIN3__GPIO4_19 */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN3, 0x384, 0x058, 2, 0x000, 0), /* MX53_PAD_DI0_PIN3__AUDMUX_AUD6_TXFS */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN3, 0x384, 0x058, 5, 0x000, 0), /* MX53_PAD_DI0_PIN3__SDMA_DEBUG_CORE_STATE_3 */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN3, 0x384, 0x058, 6, 0x000, 0), /* MX53_PAD_DI0_PIN3__EMI_EMI_DEBUG_3 */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN3, 0x384, 0x058, 7, 0x000, 0), /* MX53_PAD_DI0_PIN3__USBPHY1_IDDIG */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN4, 0x388, 0x05C, 0, 0x000, 0), /* MX53_PAD_DI0_PIN4__IPU_DI0_PIN4 */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN4, 0x388, 0x05C, 1, 0x000, 0), /* MX53_PAD_DI0_PIN4__GPIO4_20 */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN4, 0x388, 0x05C, 2, 0x000, 0), /* MX53_PAD_DI0_PIN4__AUDMUX_AUD6_RXD */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN4, 0x388, 0x05C, 3, 0x7FC, 0), /* MX53_PAD_DI0_PIN4__ESDHC1_WP */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN4, 0x388, 0x05C, 5, 0x000, 0), /* MX53_PAD_DI0_PIN4__SDMA_DEBUG_YIELD */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN4, 0x388, 0x05C, 6, 0x000, 0), /* MX53_PAD_DI0_PIN4__EMI_EMI_DEBUG_4 */
-	IMX_PIN_REG(MX53_PAD_DI0_PIN4, 0x388, 0x05C, 7, 0x000, 0), /* MX53_PAD_DI0_PIN4__USBPHY1_HOSTDISCONNECT */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT0, 0x38C, 0x060, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT0__IPU_DISP0_DAT_0 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT0, 0x38C, 0x060, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT0__GPIO4_21 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT0, 0x38C, 0x060, 2, 0x780, 0), /* MX53_PAD_DISP0_DAT0__CSPI_SCLK */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT0, 0x38C, 0x060, 3, 0x000, 0), /* MX53_PAD_DISP0_DAT0__USBOH3_USBH2_DATA_0 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT0, 0x38C, 0x060, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT0__SDMA_DEBUG_CORE_RUN */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT0, 0x38C, 0x060, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT0__EMI_EMI_DEBUG_5 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT0, 0x38C, 0x060, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT0__USBPHY2_TXREADY */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT1, 0x390, 0x064, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT1__IPU_DISP0_DAT_1 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT1, 0x390, 0x064, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT1__GPIO4_22 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT1, 0x390, 0x064, 2, 0x788, 0), /* MX53_PAD_DISP0_DAT1__CSPI_MOSI */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT1, 0x390, 0x064, 3, 0x000, 0), /* MX53_PAD_DISP0_DAT1__USBOH3_USBH2_DATA_1 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT1, 0x390, 0x064, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT1__SDMA_DEBUG_EVENT_CHANNEL_SEL */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT1, 0x390, 0x064, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT1__EMI_EMI_DEBUG_6 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT1, 0x390, 0x064, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT1__USBPHY2_RXVALID */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT2, 0x394, 0x068, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT2__IPU_DISP0_DAT_2 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT2, 0x394, 0x068, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT2__GPIO4_23 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT2, 0x394, 0x068, 2, 0x784, 0), /* MX53_PAD_DISP0_DAT2__CSPI_MISO */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT2, 0x394, 0x068, 3, 0x000, 0), /* MX53_PAD_DISP0_DAT2__USBOH3_USBH2_DATA_2 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT2, 0x394, 0x068, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT2__SDMA_DEBUG_MODE */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT2, 0x394, 0x068, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT2__EMI_EMI_DEBUG_7 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT2, 0x394, 0x068, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT2__USBPHY2_RXACTIVE */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT3, 0x398, 0x06C, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT3__IPU_DISP0_DAT_3 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT3, 0x398, 0x06C, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT3__GPIO4_24 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT3, 0x398, 0x06C, 2, 0x78C, 0), /* MX53_PAD_DISP0_DAT3__CSPI_SS0 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT3, 0x398, 0x06C, 3, 0x000, 0), /* MX53_PAD_DISP0_DAT3__USBOH3_USBH2_DATA_3 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT3, 0x398, 0x06C, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT3__SDMA_DEBUG_BUS_ERROR */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT3, 0x398, 0x06C, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT3__EMI_EMI_DEBUG_8 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT3, 0x398, 0x06C, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT3__USBPHY2_RXERROR */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT4, 0x39C, 0x070, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT4__IPU_DISP0_DAT_4 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT4, 0x39C, 0x070, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT4__GPIO4_25 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT4, 0x39C, 0x070, 2, 0x790, 0), /* MX53_PAD_DISP0_DAT4__CSPI_SS1 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT4, 0x39C, 0x070, 3, 0x000, 0), /* MX53_PAD_DISP0_DAT4__USBOH3_USBH2_DATA_4 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT4, 0x39C, 0x070, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT4__SDMA_DEBUG_BUS_RWB */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT4, 0x39C, 0x070, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT4__EMI_EMI_DEBUG_9 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT4, 0x39C, 0x070, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT4__USBPHY2_SIECLOCK */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT5, 0x3A0, 0x074, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT5__IPU_DISP0_DAT_5 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT5, 0x3A0, 0x074, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT5__GPIO4_26 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT5, 0x3A0, 0x074, 2, 0x794, 0), /* MX53_PAD_DISP0_DAT5__CSPI_SS2 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT5, 0x3A0, 0x074, 3, 0x000, 0), /* MX53_PAD_DISP0_DAT5__USBOH3_USBH2_DATA_5 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT5, 0x3A0, 0x074, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT5__SDMA_DEBUG_MATCHED_DMBUS */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT5, 0x3A0, 0x074, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT5__EMI_EMI_DEBUG_10 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT5, 0x3A0, 0x074, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT5__USBPHY2_LINESTATE_0 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT6, 0x3A4, 0x078, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT6__IPU_DISP0_DAT_6 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT6, 0x3A4, 0x078, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT6__GPIO4_27 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT6, 0x3A4, 0x078, 2, 0x798, 0), /* MX53_PAD_DISP0_DAT6__CSPI_SS3 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT6, 0x3A4, 0x078, 3, 0x000, 0), /* MX53_PAD_DISP0_DAT6__USBOH3_USBH2_DATA_6 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT6, 0x3A4, 0x078, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT6__SDMA_DEBUG_RTBUFFER_WRITE */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT6, 0x3A4, 0x078, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT6__EMI_EMI_DEBUG_11 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT6, 0x3A4, 0x078, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT6__USBPHY2_LINESTATE_1 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT7, 0x3A8, 0x07C, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT7__IPU_DISP0_DAT_7 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT7, 0x3A8, 0x07C, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT7__GPIO4_28 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT7, 0x3A8, 0x07C, 2, 0x000, 0), /* MX53_PAD_DISP0_DAT7__CSPI_RDY */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT7, 0x3A8, 0x07C, 3, 0x000, 0), /* MX53_PAD_DISP0_DAT7__USBOH3_USBH2_DATA_7 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT7, 0x3A8, 0x07C, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT7__SDMA_DEBUG_EVENT_CHANNEL_0 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT7, 0x3A8, 0x07C, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT7__EMI_EMI_DEBUG_12 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT7, 0x3A8, 0x07C, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT7__USBPHY2_VBUSVALID */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT8, 0x3AC, 0x080, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT8__IPU_DISP0_DAT_8 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT8, 0x3AC, 0x080, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT8__GPIO4_29 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT8, 0x3AC, 0x080, 2, 0x000, 0), /* MX53_PAD_DISP0_DAT8__PWM1_PWMO */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT8, 0x3AC, 0x080, 3, 0x000, 0), /* MX53_PAD_DISP0_DAT8__WDOG1_WDOG_B */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT8, 0x3AC, 0x080, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT8__SDMA_DEBUG_EVENT_CHANNEL_1 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT8, 0x3AC, 0x080, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT8__EMI_EMI_DEBUG_13 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT8, 0x3AC, 0x080, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT8__USBPHY2_AVALID */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT9, 0x3B0, 0x084, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT9__IPU_DISP0_DAT_9 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT9, 0x3B0, 0x084, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT9__GPIO4_30 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT9, 0x3B0, 0x084, 2, 0x000, 0), /* MX53_PAD_DISP0_DAT9__PWM2_PWMO */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT9, 0x3B0, 0x084, 3, 0x000, 0), /* MX53_PAD_DISP0_DAT9__WDOG2_WDOG_B */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT9, 0x3B0, 0x084, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT9__SDMA_DEBUG_EVENT_CHANNEL_2 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT9, 0x3B0, 0x084, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT9__EMI_EMI_DEBUG_14 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT9, 0x3B0, 0x084, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT9__USBPHY2_VSTATUS_0 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT10, 0x3B4, 0x088, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT10__IPU_DISP0_DAT_10 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT10, 0x3B4, 0x088, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT10__GPIO4_31 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT10, 0x3B4, 0x088, 2, 0x000, 0), /* MX53_PAD_DISP0_DAT10__USBOH3_USBH2_STP */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT10, 0x3B4, 0x088, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT10__SDMA_DEBUG_EVENT_CHANNEL_3 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT10, 0x3B4, 0x088, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT10__EMI_EMI_DEBUG_15 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT10, 0x3B4, 0x088, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT10__USBPHY2_VSTATUS_1 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT11, 0x3B8, 0x08C, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT11__IPU_DISP0_DAT_11 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT11, 0x3B8, 0x08C, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT11__GPIO5_5 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT11, 0x3B8, 0x08C, 2, 0x000, 0), /* MX53_PAD_DISP0_DAT11__USBOH3_USBH2_NXT */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT11, 0x3B8, 0x08C, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT11__SDMA_DEBUG_EVENT_CHANNEL_4 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT11, 0x3B8, 0x08C, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT11__EMI_EMI_DEBUG_16 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT11, 0x3B8, 0x08C, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT11__USBPHY2_VSTATUS_2 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT12, 0x3BC, 0x090, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT12__IPU_DISP0_DAT_12 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT12, 0x3BC, 0x090, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT12__GPIO5_6 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT12, 0x3BC, 0x090, 2, 0x000, 0), /* MX53_PAD_DISP0_DAT12__USBOH3_USBH2_CLK */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT12, 0x3BC, 0x090, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT12__SDMA_DEBUG_EVENT_CHANNEL_5 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT12, 0x3BC, 0x090, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT12__EMI_EMI_DEBUG_17 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT12, 0x3BC, 0x090, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT12__USBPHY2_VSTATUS_3 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT13, 0x3C0, 0x094, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT13__IPU_DISP0_DAT_13 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT13, 0x3C0, 0x094, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT13__GPIO5_7 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT13, 0x3C0, 0x094, 3, 0x754, 0), /* MX53_PAD_DISP0_DAT13__AUDMUX_AUD5_RXFS */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT13, 0x3C0, 0x094, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT13__SDMA_DEBUG_EVT_CHN_LINES_0 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT13, 0x3C0, 0x094, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT13__EMI_EMI_DEBUG_18 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT13, 0x3C0, 0x094, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT13__USBPHY2_VSTATUS_4 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT14, 0x3C4, 0x098, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT14__IPU_DISP0_DAT_14 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT14, 0x3C4, 0x098, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT14__GPIO5_8 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT14, 0x3C4, 0x098, 3, 0x750, 0), /* MX53_PAD_DISP0_DAT14__AUDMUX_AUD5_RXC */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT14, 0x3C4, 0x098, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT14__SDMA_DEBUG_EVT_CHN_LINES_1 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT14, 0x3C4, 0x098, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT14__EMI_EMI_DEBUG_19 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT14, 0x3C4, 0x098, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT14__USBPHY2_VSTATUS_5 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT15, 0x3C8, 0x09C, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT15__IPU_DISP0_DAT_15 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT15, 0x3C8, 0x09C, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT15__GPIO5_9 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT15, 0x3C8, 0x09C, 2, 0x7AC, 1), /* MX53_PAD_DISP0_DAT15__ECSPI1_SS1 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT15, 0x3C8, 0x09C, 3, 0x7C8, 0), /* MX53_PAD_DISP0_DAT15__ECSPI2_SS1 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT15, 0x3C8, 0x09C, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT15__SDMA_DEBUG_EVT_CHN_LINES_2 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT15, 0x3C8, 0x09C, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT15__EMI_EMI_DEBUG_20 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT15, 0x3C8, 0x09C, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT15__USBPHY2_VSTATUS_6 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT16, 0x3CC, 0x0A0, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT16__IPU_DISP0_DAT_16 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT16, 0x3CC, 0x0A0, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT16__GPIO5_10 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT16, 0x3CC, 0x0A0, 2, 0x7C0, 0), /* MX53_PAD_DISP0_DAT16__ECSPI2_MOSI */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT16, 0x3CC, 0x0A0, 3, 0x758, 1), /* MX53_PAD_DISP0_DAT16__AUDMUX_AUD5_TXC */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT16, 0x3CC, 0x0A0, 4, 0x868, 0), /* MX53_PAD_DISP0_DAT16__SDMA_EXT_EVENT_0 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT16, 0x3CC, 0x0A0, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT16__SDMA_DEBUG_EVT_CHN_LINES_3 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT16, 0x3CC, 0x0A0, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT16__EMI_EMI_DEBUG_21 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT16, 0x3CC, 0x0A0, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT16__USBPHY2_VSTATUS_7 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT17, 0x3D0, 0x0A4, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT17__IPU_DISP0_DAT_17 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT17, 0x3D0, 0x0A4, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT17__GPIO5_11 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT17, 0x3D0, 0x0A4, 2, 0x7BC, 0), /* MX53_PAD_DISP0_DAT17__ECSPI2_MISO */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT17, 0x3D0, 0x0A4, 3, 0x74C, 1), /* MX53_PAD_DISP0_DAT17__AUDMUX_AUD5_TXD */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT17, 0x3D0, 0x0A4, 4, 0x86C, 0), /* MX53_PAD_DISP0_DAT17__SDMA_EXT_EVENT_1 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT17, 0x3D0, 0x0A4, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT17__SDMA_DEBUG_EVT_CHN_LINES_4 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT17, 0x3D0, 0x0A4, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT17__EMI_EMI_DEBUG_22 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT18, 0x3D4, 0x0A8, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT18__IPU_DISP0_DAT_18 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT18, 0x3D4, 0x0A8, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT18__GPIO5_12 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT18, 0x3D4, 0x0A8, 2, 0x7C4, 0), /* MX53_PAD_DISP0_DAT18__ECSPI2_SS0 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT18, 0x3D4, 0x0A8, 3, 0x75C, 1), /* MX53_PAD_DISP0_DAT18__AUDMUX_AUD5_TXFS */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT18, 0x3D4, 0x0A8, 4, 0x73C, 0), /* MX53_PAD_DISP0_DAT18__AUDMUX_AUD4_RXFS */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT18, 0x3D4, 0x0A8, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT18__SDMA_DEBUG_EVT_CHN_LINES_5 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT18, 0x3D4, 0x0A8, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT18__EMI_EMI_DEBUG_23 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT18, 0x3D4, 0x0A8, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT18__EMI_WEIM_CS_2 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT19, 0x3D8, 0x0AC, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT19__IPU_DISP0_DAT_19 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT19, 0x3D8, 0x0AC, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT19__GPIO5_13 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT19, 0x3D8, 0x0AC, 2, 0x7B8, 0), /* MX53_PAD_DISP0_DAT19__ECSPI2_SCLK */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT19, 0x3D8, 0x0AC, 3, 0x748, 1), /* MX53_PAD_DISP0_DAT19__AUDMUX_AUD5_RXD */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT19, 0x3D8, 0x0AC, 4, 0x738, 0), /* MX53_PAD_DISP0_DAT19__AUDMUX_AUD4_RXC */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT19, 0x3D8, 0x0AC, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT19__SDMA_DEBUG_EVT_CHN_LINES_6 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT19, 0x3D8, 0x0AC, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT19__EMI_EMI_DEBUG_24 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT19, 0x3D8, 0x0AC, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT19__EMI_WEIM_CS_3 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT20, 0x3DC, 0x0B0, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT20__IPU_DISP0_DAT_20 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT20, 0x3DC, 0x0B0, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT20__GPIO5_14 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT20, 0x3DC, 0x0B0, 2, 0x79C, 1), /* MX53_PAD_DISP0_DAT20__ECSPI1_SCLK */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT20, 0x3DC, 0x0B0, 3, 0x740, 0), /* MX53_PAD_DISP0_DAT20__AUDMUX_AUD4_TXC */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT20, 0x3DC, 0x0B0, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT20__SDMA_DEBUG_EVT_CHN_LINES_7 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT20, 0x3DC, 0x0B0, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT20__EMI_EMI_DEBUG_25 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT20, 0x3DC, 0x0B0, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT20__SATA_PHY_TDI */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT21, 0x3E0, 0x0B4, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT21__IPU_DISP0_DAT_21 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT21, 0x3E0, 0x0B4, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT21__GPIO5_15 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT21, 0x3E0, 0x0B4, 2, 0x7A4, 1), /* MX53_PAD_DISP0_DAT21__ECSPI1_MOSI */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT21, 0x3E0, 0x0B4, 3, 0x734, 0), /* MX53_PAD_DISP0_DAT21__AUDMUX_AUD4_TXD */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT21, 0x3E0, 0x0B4, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT21__SDMA_DEBUG_BUS_DEVICE_0 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT21, 0x3E0, 0x0B4, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT21__EMI_EMI_DEBUG_26 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT21, 0x3E0, 0x0B4, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT21__SATA_PHY_TDO */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT22, 0x3E4, 0x0B8, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT22__IPU_DISP0_DAT_22 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT22, 0x3E4, 0x0B8, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT22__GPIO5_16 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT22, 0x3E4, 0x0B8, 2, 0x7A0, 1), /* MX53_PAD_DISP0_DAT22__ECSPI1_MISO */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT22, 0x3E4, 0x0B8, 3, 0x744, 0), /* MX53_PAD_DISP0_DAT22__AUDMUX_AUD4_TXFS */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT22, 0x3E4, 0x0B8, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT22__SDMA_DEBUG_BUS_DEVICE_1 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT22, 0x3E4, 0x0B8, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT22__EMI_EMI_DEBUG_27 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT22, 0x3E4, 0x0B8, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT22__SATA_PHY_TCK */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT23, 0x3E8, 0x0BC, 0, 0x000, 0), /* MX53_PAD_DISP0_DAT23__IPU_DISP0_DAT_23 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT23, 0x3E8, 0x0BC, 1, 0x000, 0), /* MX53_PAD_DISP0_DAT23__GPIO5_17 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT23, 0x3E8, 0x0BC, 2, 0x7A8, 1), /* MX53_PAD_DISP0_DAT23__ECSPI1_SS0 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT23, 0x3E8, 0x0BC, 3, 0x730, 0), /* MX53_PAD_DISP0_DAT23__AUDMUX_AUD4_RXD */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT23, 0x3E8, 0x0BC, 5, 0x000, 0), /* MX53_PAD_DISP0_DAT23__SDMA_DEBUG_BUS_DEVICE_2 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT23, 0x3E8, 0x0BC, 6, 0x000, 0), /* MX53_PAD_DISP0_DAT23__EMI_EMI_DEBUG_28 */
-	IMX_PIN_REG(MX53_PAD_DISP0_DAT23, 0x3E8, 0x0BC, 7, 0x000, 0), /* MX53_PAD_DISP0_DAT23__SATA_PHY_TMS */
-	IMX_PIN_REG(MX53_PAD_CSI0_PIXCLK, 0x3EC, 0x0C0, 0, 0x000, 0), /* MX53_PAD_CSI0_PIXCLK__IPU_CSI0_PIXCLK */
-	IMX_PIN_REG(MX53_PAD_CSI0_PIXCLK, 0x3EC, 0x0C0, 1, 0x000, 0), /* MX53_PAD_CSI0_PIXCLK__GPIO5_18 */
-	IMX_PIN_REG(MX53_PAD_CSI0_PIXCLK, 0x3EC, 0x0C0, 5, 0x000, 0), /* MX53_PAD_CSI0_PIXCLK__SDMA_DEBUG_PC_0 */
-	IMX_PIN_REG(MX53_PAD_CSI0_PIXCLK, 0x3EC, 0x0C0, 6, 0x000, 0), /* MX53_PAD_CSI0_PIXCLK__EMI_EMI_DEBUG_29 */
-	IMX_PIN_REG(MX53_PAD_CSI0_MCLK, 0x3F0, 0x0C4, 0, 0x000, 0), /* MX53_PAD_CSI0_MCLK__IPU_CSI0_HSYNC */
-	IMX_PIN_REG(MX53_PAD_CSI0_MCLK, 0x3F0, 0x0C4, 1, 0x000, 0), /* MX53_PAD_CSI0_MCLK__GPIO5_19 */
-	IMX_PIN_REG(MX53_PAD_CSI0_MCLK, 0x3F0, 0x0C4, 2, 0x000, 0), /* MX53_PAD_CSI0_MCLK__CCM_CSI0_MCLK */
-	IMX_PIN_REG(MX53_PAD_CSI0_MCLK, 0x3F0, 0x0C4, 5, 0x000, 0), /* MX53_PAD_CSI0_MCLK__SDMA_DEBUG_PC_1 */
-	IMX_PIN_REG(MX53_PAD_CSI0_MCLK, 0x3F0, 0x0C4, 6, 0x000, 0), /* MX53_PAD_CSI0_MCLK__EMI_EMI_DEBUG_30 */
-	IMX_PIN_REG(MX53_PAD_CSI0_MCLK, 0x3F0, 0x0C4, 7, 0x000, 0), /* MX53_PAD_CSI0_MCLK__TPIU_TRCTL */
-	IMX_PIN_REG(MX53_PAD_CSI0_DATA_EN, 0x3F4, 0x0C8, 0, 0x000, 0), /* MX53_PAD_CSI0_DATA_EN__IPU_CSI0_DATA_EN */
-	IMX_PIN_REG(MX53_PAD_CSI0_DATA_EN, 0x3F4, 0x0C8, 1, 0x000, 0), /* MX53_PAD_CSI0_DATA_EN__GPIO5_20 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DATA_EN, 0x3F4, 0x0C8, 5, 0x000, 0), /* MX53_PAD_CSI0_DATA_EN__SDMA_DEBUG_PC_2 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DATA_EN, 0x3F4, 0x0C8, 6, 0x000, 0), /* MX53_PAD_CSI0_DATA_EN__EMI_EMI_DEBUG_31 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DATA_EN, 0x3F4, 0x0C8, 7, 0x000, 0), /* MX53_PAD_CSI0_DATA_EN__TPIU_TRCLK */
-	IMX_PIN_REG(MX53_PAD_CSI0_VSYNC, 0x3F8, 0x0CC, 0, 0x000, 0), /* MX53_PAD_CSI0_VSYNC__IPU_CSI0_VSYNC */
-	IMX_PIN_REG(MX53_PAD_CSI0_VSYNC, 0x3F8, 0x0CC, 1, 0x000, 0), /* MX53_PAD_CSI0_VSYNC__GPIO5_21 */
-	IMX_PIN_REG(MX53_PAD_CSI0_VSYNC, 0x3F8, 0x0CC, 5, 0x000, 0), /* MX53_PAD_CSI0_VSYNC__SDMA_DEBUG_PC_3 */
-	IMX_PIN_REG(MX53_PAD_CSI0_VSYNC, 0x3F8, 0x0CC, 6, 0x000, 0), /* MX53_PAD_CSI0_VSYNC__EMI_EMI_DEBUG_32 */
-	IMX_PIN_REG(MX53_PAD_CSI0_VSYNC, 0x3F8, 0x0CC, 7, 0x000, 0), /* MX53_PAD_CSI0_VSYNC__TPIU_TRACE_0 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT4, 0x3FC, 0x0D0, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT4__IPU_CSI0_D_4 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT4, 0x3FC, 0x0D0, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT4__GPIO5_22 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT4, 0x3FC, 0x0D0, 2, 0x840, 1), /* MX53_PAD_CSI0_DAT4__KPP_COL_5 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT4, 0x3FC, 0x0D0, 3, 0x79C, 2), /* MX53_PAD_CSI0_DAT4__ECSPI1_SCLK */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT4, 0x3FC, 0x0D0, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT4__USBOH3_USBH3_STP */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT4, 0x3FC, 0x0D0, 5, 0x000, 0), /* MX53_PAD_CSI0_DAT4__AUDMUX_AUD3_TXC */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT4, 0x3FC, 0x0D0, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT4__EMI_EMI_DEBUG_33 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT4, 0x3FC, 0x0D0, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT4__TPIU_TRACE_1 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT5, 0x400, 0x0D4, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT5__IPU_CSI0_D_5 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT5, 0x400, 0x0D4, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT5__GPIO5_23 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT5, 0x400, 0x0D4, 2, 0x84C, 0), /* MX53_PAD_CSI0_DAT5__KPP_ROW_5 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT5, 0x400, 0x0D4, 3, 0x7A4, 2), /* MX53_PAD_CSI0_DAT5__ECSPI1_MOSI */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT5, 0x400, 0x0D4, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT5__USBOH3_USBH3_NXT */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT5, 0x400, 0x0D4, 5, 0x000, 0), /* MX53_PAD_CSI0_DAT5__AUDMUX_AUD3_TXD */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT5, 0x400, 0x0D4, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT5__EMI_EMI_DEBUG_34 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT5, 0x400, 0x0D4, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT5__TPIU_TRACE_2 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT6, 0x404, 0x0D8, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT6__IPU_CSI0_D_6 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT6, 0x404, 0x0D8, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT6__GPIO5_24 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT6, 0x404, 0x0D8, 2, 0x844, 0), /* MX53_PAD_CSI0_DAT6__KPP_COL_6 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT6, 0x404, 0x0D8, 3, 0x7A0, 2), /* MX53_PAD_CSI0_DAT6__ECSPI1_MISO */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT6, 0x404, 0x0D8, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT6__USBOH3_USBH3_CLK */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT6, 0x404, 0x0D8, 5, 0x000, 0), /* MX53_PAD_CSI0_DAT6__AUDMUX_AUD3_TXFS */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT6, 0x404, 0x0D8, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT6__EMI_EMI_DEBUG_35 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT6, 0x404, 0x0D8, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT6__TPIU_TRACE_3 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT7, 0x408, 0x0DC, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT7__IPU_CSI0_D_7 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT7, 0x408, 0x0DC, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT7__GPIO5_25 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT7, 0x408, 0x0DC, 2, 0x850, 0), /* MX53_PAD_CSI0_DAT7__KPP_ROW_6 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT7, 0x408, 0x0DC, 3, 0x7A8, 2), /* MX53_PAD_CSI0_DAT7__ECSPI1_SS0 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT7, 0x408, 0x0DC, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT7__USBOH3_USBH3_DIR */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT7, 0x408, 0x0DC, 5, 0x000, 0), /* MX53_PAD_CSI0_DAT7__AUDMUX_AUD3_RXD */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT7, 0x408, 0x0DC, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT7__EMI_EMI_DEBUG_36 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT7, 0x408, 0x0DC, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT7__TPIU_TRACE_4 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT8, 0x40C, 0x0E0, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT8__IPU_CSI0_D_8 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT8, 0x40C, 0x0E0, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT8__GPIO5_26 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT8, 0x40C, 0x0E0, 2, 0x848, 0), /* MX53_PAD_CSI0_DAT8__KPP_COL_7 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT8, 0x40C, 0x0E0, 3, 0x7B8, 1), /* MX53_PAD_CSI0_DAT8__ECSPI2_SCLK */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT8, 0x40C, 0x0E0, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT8__USBOH3_USBH3_OC */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT8, 0x40C, 0x0E0, 5, 0x818, 0), /* MX53_PAD_CSI0_DAT8__I2C1_SDA */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT8, 0x40C, 0x0E0, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT8__EMI_EMI_DEBUG_37 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT8, 0x40C, 0x0E0, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT8__TPIU_TRACE_5 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT9, 0x410, 0x0E4, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT9__IPU_CSI0_D_9 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT9, 0x410, 0x0E4, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT9__GPIO5_27 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT9, 0x410, 0x0E4, 2, 0x854, 0), /* MX53_PAD_CSI0_DAT9__KPP_ROW_7 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT9, 0x410, 0x0E4, 3, 0x7C0, 1), /* MX53_PAD_CSI0_DAT9__ECSPI2_MOSI */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT9, 0x410, 0x0E4, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT9__USBOH3_USBH3_PWR */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT9, 0x410, 0x0E4, 5, 0x814, 0), /* MX53_PAD_CSI0_DAT9__I2C1_SCL */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT9, 0x410, 0x0E4, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT9__EMI_EMI_DEBUG_38 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT9, 0x410, 0x0E4, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT9__TPIU_TRACE_6 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT10, 0x414, 0x0E8, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT10__IPU_CSI0_D_10 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT10, 0x414, 0x0E8, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT10__GPIO5_28 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT10, 0x414, 0x0E8, 2, 0x000, 0), /* MX53_PAD_CSI0_DAT10__UART1_TXD_MUX */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT10, 0x414, 0x0E8, 3, 0x7BC, 1), /* MX53_PAD_CSI0_DAT10__ECSPI2_MISO */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT10, 0x414, 0x0E8, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT10__AUDMUX_AUD3_RXC */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT10, 0x414, 0x0E8, 5, 0x000, 0), /* MX53_PAD_CSI0_DAT10__SDMA_DEBUG_PC_4 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT10, 0x414, 0x0E8, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT10__EMI_EMI_DEBUG_39 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT10, 0x414, 0x0E8, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT10__TPIU_TRACE_7 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT11, 0x418, 0x0EC, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT11__IPU_CSI0_D_11 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT11, 0x418, 0x0EC, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT11__GPIO5_29 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT11, 0x418, 0x0EC, 2, 0x878, 1), /* MX53_PAD_CSI0_DAT11__UART1_RXD_MUX */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT11, 0x418, 0x0EC, 3, 0x7C4, 1), /* MX53_PAD_CSI0_DAT11__ECSPI2_SS0 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT11, 0x418, 0x0EC, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT11__AUDMUX_AUD3_RXFS */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT11, 0x418, 0x0EC, 5, 0x000, 0), /* MX53_PAD_CSI0_DAT11__SDMA_DEBUG_PC_5 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT11, 0x418, 0x0EC, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT11__EMI_EMI_DEBUG_40 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT11, 0x418, 0x0EC, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT11__TPIU_TRACE_8 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT12, 0x41C, 0x0F0, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT12__IPU_CSI0_D_12 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT12, 0x41C, 0x0F0, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT12__GPIO5_30 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT12, 0x41C, 0x0F0, 2, 0x000, 0), /* MX53_PAD_CSI0_DAT12__UART4_TXD_MUX */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT12, 0x41C, 0x0F0, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT12__USBOH3_USBH3_DATA_0 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT12, 0x41C, 0x0F0, 5, 0x000, 0), /* MX53_PAD_CSI0_DAT12__SDMA_DEBUG_PC_6 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT12, 0x41C, 0x0F0, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT12__EMI_EMI_DEBUG_41 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT12, 0x41C, 0x0F0, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT12__TPIU_TRACE_9 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT13, 0x420, 0x0F4, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT13__IPU_CSI0_D_13 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT13, 0x420, 0x0F4, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT13__GPIO5_31 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT13, 0x420, 0x0F4, 2, 0x890, 3), /* MX53_PAD_CSI0_DAT13__UART4_RXD_MUX */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT13, 0x420, 0x0F4, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT13__USBOH3_USBH3_DATA_1 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT13, 0x420, 0x0F4, 5, 0x000, 0), /* MX53_PAD_CSI0_DAT13__SDMA_DEBUG_PC_7 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT13, 0x420, 0x0F4, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT13__EMI_EMI_DEBUG_42 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT13, 0x420, 0x0F4, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT13__TPIU_TRACE_10 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT14, 0x424, 0x0F8, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT14__IPU_CSI0_D_14 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT14, 0x424, 0x0F8, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT14__GPIO6_0 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT14, 0x424, 0x0F8, 2, 0x000, 0), /* MX53_PAD_CSI0_DAT14__UART5_TXD_MUX */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT14, 0x424, 0x0F8, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT14__USBOH3_USBH3_DATA_2 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT14, 0x424, 0x0F8, 5, 0x000, 0), /* MX53_PAD_CSI0_DAT14__SDMA_DEBUG_PC_8 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT14, 0x424, 0x0F8, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT14__EMI_EMI_DEBUG_43 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT14, 0x424, 0x0F8, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT14__TPIU_TRACE_11 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT15, 0x428, 0x0FC, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT15__IPU_CSI0_D_15 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT15, 0x428, 0x0FC, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT15__GPIO6_1 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT15, 0x428, 0x0FC, 2, 0x898, 3), /* MX53_PAD_CSI0_DAT15__UART5_RXD_MUX */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT15, 0x428, 0x0FC, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT15__USBOH3_USBH3_DATA_3 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT15, 0x428, 0x0FC, 5, 0x000, 0), /* MX53_PAD_CSI0_DAT15__SDMA_DEBUG_PC_9 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT15, 0x428, 0x0FC, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT15__EMI_EMI_DEBUG_44 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT15, 0x428, 0x0FC, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT15__TPIU_TRACE_12 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT16, 0x42C, 0x100, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT16__IPU_CSI0_D_16 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT16, 0x42C, 0x100, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT16__GPIO6_2 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT16, 0x42C, 0x100, 2, 0x88C, 0), /* MX53_PAD_CSI0_DAT16__UART4_RTS */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT16, 0x42C, 0x100, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT16__USBOH3_USBH3_DATA_4 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT16, 0x42C, 0x100, 5, 0x000, 0), /* MX53_PAD_CSI0_DAT16__SDMA_DEBUG_PC_10 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT16, 0x42C, 0x100, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT16__EMI_EMI_DEBUG_45 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT16, 0x42C, 0x100, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT16__TPIU_TRACE_13 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT17, 0x430, 0x104, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT17__IPU_CSI0_D_17 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT17, 0x430, 0x104, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT17__GPIO6_3 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT17, 0x430, 0x104, 2, 0x000, 0), /* MX53_PAD_CSI0_DAT17__UART4_CTS */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT17, 0x430, 0x104, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT17__USBOH3_USBH3_DATA_5 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT17, 0x430, 0x104, 5, 0x000, 0), /* MX53_PAD_CSI0_DAT17__SDMA_DEBUG_PC_11 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT17, 0x430, 0x104, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT17__EMI_EMI_DEBUG_46 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT17, 0x430, 0x104, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT17__TPIU_TRACE_14 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT18, 0x434, 0x108, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT18__IPU_CSI0_D_18 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT18, 0x434, 0x108, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT18__GPIO6_4 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT18, 0x434, 0x108, 2, 0x894, 2), /* MX53_PAD_CSI0_DAT18__UART5_RTS */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT18, 0x434, 0x108, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT18__USBOH3_USBH3_DATA_6 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT18, 0x434, 0x108, 5, 0x000, 0), /* MX53_PAD_CSI0_DAT18__SDMA_DEBUG_PC_12 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT18, 0x434, 0x108, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT18__EMI_EMI_DEBUG_47 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT18, 0x434, 0x108, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT18__TPIU_TRACE_15 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT19, 0x438, 0x10C, 0, 0x000, 0), /* MX53_PAD_CSI0_DAT19__IPU_CSI0_D_19 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT19, 0x438, 0x10C, 1, 0x000, 0), /* MX53_PAD_CSI0_DAT19__GPIO6_5 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT19, 0x438, 0x10C, 2, 0x000, 0), /* MX53_PAD_CSI0_DAT19__UART5_CTS */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT19, 0x438, 0x10C, 4, 0x000, 0), /* MX53_PAD_CSI0_DAT19__USBOH3_USBH3_DATA_7 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT19, 0x438, 0x10C, 5, 0x000, 0), /* MX53_PAD_CSI0_DAT19__SDMA_DEBUG_PC_13 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT19, 0x438, 0x10C, 6, 0x000, 0), /* MX53_PAD_CSI0_DAT19__EMI_EMI_DEBUG_48 */
-	IMX_PIN_REG(MX53_PAD_CSI0_DAT19, 0x438, 0x10C, 7, 0x000, 0), /* MX53_PAD_CSI0_DAT19__USBPHY2_BISTOK */
-	IMX_PIN_REG(MX53_PAD_EIM_A25, 0x458, 0x110, 0, 0x000, 0), /* MX53_PAD_EIM_A25__EMI_WEIM_A_25 */
-	IMX_PIN_REG(MX53_PAD_EIM_A25, 0x458, 0x110, 1, 0x000, 0), /* MX53_PAD_EIM_A25__GPIO5_2 */
-	IMX_PIN_REG(MX53_PAD_EIM_A25, 0x458, 0x110, 2, 0x000, 0), /* MX53_PAD_EIM_A25__ECSPI2_RDY */
-	IMX_PIN_REG(MX53_PAD_EIM_A25, 0x458, 0x110, 3, 0x000, 0), /* MX53_PAD_EIM_A25__IPU_DI1_PIN12 */
-	IMX_PIN_REG(MX53_PAD_EIM_A25, 0x458, 0x110, 4, 0x790, 1), /* MX53_PAD_EIM_A25__CSPI_SS1 */
-	IMX_PIN_REG(MX53_PAD_EIM_A25, 0x458, 0x110, 6, 0x000, 0), /* MX53_PAD_EIM_A25__IPU_DI0_D1_CS */
-	IMX_PIN_REG(MX53_PAD_EIM_A25, 0x458, 0x110, 7, 0x000, 0), /* MX53_PAD_EIM_A25__USBPHY1_BISTOK */
-	IMX_PIN_REG(MX53_PAD_EIM_EB2, 0x45C, 0x114, 0, 0x000, 0), /* MX53_PAD_EIM_EB2__EMI_WEIM_EB_2 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB2, 0x45C, 0x114, 1, 0x000, 0), /* MX53_PAD_EIM_EB2__GPIO2_30 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB2, 0x45C, 0x114, 2, 0x76C, 0), /* MX53_PAD_EIM_EB2__CCM_DI1_EXT_CLK */
-	IMX_PIN_REG(MX53_PAD_EIM_EB2, 0x45C, 0x114, 3, 0x000, 0), /* MX53_PAD_EIM_EB2__IPU_SER_DISP1_CS */
-	IMX_PIN_REG(MX53_PAD_EIM_EB2, 0x45C, 0x114, 4, 0x7A8, 3), /* MX53_PAD_EIM_EB2__ECSPI1_SS0 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB2, 0x45C, 0x114, 5, 0x81C, 1), /* MX53_PAD_EIM_EB2__I2C2_SCL */
-	IMX_PIN_REG(MX53_PAD_EIM_D16, 0x460, 0x118, 0, 0x000, 0), /* MX53_PAD_EIM_D16__EMI_WEIM_D_16 */
-	IMX_PIN_REG(MX53_PAD_EIM_D16, 0x460, 0x118, 1, 0x000, 0), /* MX53_PAD_EIM_D16__GPIO3_16 */
-	IMX_PIN_REG(MX53_PAD_EIM_D16, 0x460, 0x118, 2, 0x000, 0), /* MX53_PAD_EIM_D16__IPU_DI0_PIN5 */
-	IMX_PIN_REG(MX53_PAD_EIM_D16, 0x460, 0x118, 3, 0x000, 0), /* MX53_PAD_EIM_D16__IPU_DISPB1_SER_CLK */
-	IMX_PIN_REG(MX53_PAD_EIM_D16, 0x460, 0x118, 4, 0x79C, 3), /* MX53_PAD_EIM_D16__ECSPI1_SCLK */
-	IMX_PIN_REG(MX53_PAD_EIM_D16, 0x460, 0x118, 5, 0x820, 1), /* MX53_PAD_EIM_D16__I2C2_SDA */
-	IMX_PIN_REG(MX53_PAD_EIM_D17, 0x464, 0x11C, 0, 0x000, 0), /* MX53_PAD_EIM_D17__EMI_WEIM_D_17 */
-	IMX_PIN_REG(MX53_PAD_EIM_D17, 0x464, 0x11C, 1, 0x000, 0), /* MX53_PAD_EIM_D17__GPIO3_17 */
-	IMX_PIN_REG(MX53_PAD_EIM_D17, 0x464, 0x11C, 2, 0x000, 0), /* MX53_PAD_EIM_D17__IPU_DI0_PIN6 */
-	IMX_PIN_REG(MX53_PAD_EIM_D17, 0x464, 0x11C, 3, 0x830, 0), /* MX53_PAD_EIM_D17__IPU_DISPB1_SER_DIN */
-	IMX_PIN_REG(MX53_PAD_EIM_D17, 0x464, 0x11C, 4, 0x7A0, 3), /* MX53_PAD_EIM_D17__ECSPI1_MISO */
-	IMX_PIN_REG(MX53_PAD_EIM_D17, 0x464, 0x11C, 5, 0x824, 0), /* MX53_PAD_EIM_D17__I2C3_SCL */
-	IMX_PIN_REG(MX53_PAD_EIM_D18, 0x468, 0x120, 0, 0x000, 0), /* MX53_PAD_EIM_D18__EMI_WEIM_D_18 */
-	IMX_PIN_REG(MX53_PAD_EIM_D18, 0x468, 0x120, 1, 0x000, 0), /* MX53_PAD_EIM_D18__GPIO3_18 */
-	IMX_PIN_REG(MX53_PAD_EIM_D18, 0x468, 0x120, 2, 0x000, 0), /* MX53_PAD_EIM_D18__IPU_DI0_PIN7 */
-	IMX_PIN_REG(MX53_PAD_EIM_D18, 0x468, 0x120, 3, 0x830, 1), /* MX53_PAD_EIM_D18__IPU_DISPB1_SER_DIO */
-	IMX_PIN_REG(MX53_PAD_EIM_D18, 0x468, 0x120, 4, 0x7A4, 3), /* MX53_PAD_EIM_D18__ECSPI1_MOSI */
-	IMX_PIN_REG(MX53_PAD_EIM_D18, 0x468, 0x120, 5, 0x828, 0), /* MX53_PAD_EIM_D18__I2C3_SDA */
-	IMX_PIN_REG(MX53_PAD_EIM_D18, 0x468, 0x120, 6, 0x000, 0), /* MX53_PAD_EIM_D18__IPU_DI1_D0_CS */
-	IMX_PIN_REG(MX53_PAD_EIM_D19, 0x46C, 0x124, 0, 0x000, 0), /* MX53_PAD_EIM_D19__EMI_WEIM_D_19 */
-	IMX_PIN_REG(MX53_PAD_EIM_D19, 0x46C, 0x124, 1, 0x000, 0), /* MX53_PAD_EIM_D19__GPIO3_19 */
-	IMX_PIN_REG(MX53_PAD_EIM_D19, 0x46C, 0x124, 2, 0x000, 0), /* MX53_PAD_EIM_D19__IPU_DI0_PIN8 */
-	IMX_PIN_REG(MX53_PAD_EIM_D19, 0x46C, 0x124, 3, 0x000, 0), /* MX53_PAD_EIM_D19__IPU_DISPB1_SER_RS */
-	IMX_PIN_REG(MX53_PAD_EIM_D19, 0x46C, 0x124, 4, 0x7AC, 2), /* MX53_PAD_EIM_D19__ECSPI1_SS1 */
-	IMX_PIN_REG(MX53_PAD_EIM_D19, 0x46C, 0x124, 5, 0x000, 0), /* MX53_PAD_EIM_D19__EPIT1_EPITO */
-	IMX_PIN_REG(MX53_PAD_EIM_D19, 0x46C, 0x124, 6, 0x000, 0), /* MX53_PAD_EIM_D19__UART1_CTS */
-	IMX_PIN_REG(MX53_PAD_EIM_D19, 0x46C, 0x124, 7, 0x8A4, 0), /* MX53_PAD_EIM_D19__USBOH3_USBH2_OC */
-	IMX_PIN_REG(MX53_PAD_EIM_D20, 0x470, 0x128, 0, 0x000, 0), /* MX53_PAD_EIM_D20__EMI_WEIM_D_20 */
-	IMX_PIN_REG(MX53_PAD_EIM_D20, 0x470, 0x128, 1, 0x000, 0), /* MX53_PAD_EIM_D20__GPIO3_20 */
-	IMX_PIN_REG(MX53_PAD_EIM_D20, 0x470, 0x128, 2, 0x000, 0), /* MX53_PAD_EIM_D20__IPU_DI0_PIN16 */
-	IMX_PIN_REG(MX53_PAD_EIM_D20, 0x470, 0x128, 3, 0x000, 0), /* MX53_PAD_EIM_D20__IPU_SER_DISP0_CS */
-	IMX_PIN_REG(MX53_PAD_EIM_D20, 0x470, 0x128, 4, 0x78C, 1), /* MX53_PAD_EIM_D20__CSPI_SS0 */
-	IMX_PIN_REG(MX53_PAD_EIM_D20, 0x470, 0x128, 5, 0x000, 0), /* MX53_PAD_EIM_D20__EPIT2_EPITO */
-	IMX_PIN_REG(MX53_PAD_EIM_D20, 0x470, 0x128, 6, 0x874, 1), /* MX53_PAD_EIM_D20__UART1_RTS */
-	IMX_PIN_REG(MX53_PAD_EIM_D20, 0x470, 0x128, 7, 0x000, 0), /* MX53_PAD_EIM_D20__USBOH3_USBH2_PWR */
-	IMX_PIN_REG(MX53_PAD_EIM_D21, 0x474, 0x12C, 0, 0x000, 0), /* MX53_PAD_EIM_D21__EMI_WEIM_D_21 */
-	IMX_PIN_REG(MX53_PAD_EIM_D21, 0x474, 0x12C, 1, 0x000, 0), /* MX53_PAD_EIM_D21__GPIO3_21 */
-	IMX_PIN_REG(MX53_PAD_EIM_D21, 0x474, 0x12C, 2, 0x000, 0), /* MX53_PAD_EIM_D21__IPU_DI0_PIN17 */
-	IMX_PIN_REG(MX53_PAD_EIM_D21, 0x474, 0x12C, 3, 0x000, 0), /* MX53_PAD_EIM_D21__IPU_DISPB0_SER_CLK */
-	IMX_PIN_REG(MX53_PAD_EIM_D21, 0x474, 0x12C, 4, 0x780, 1), /* MX53_PAD_EIM_D21__CSPI_SCLK */
-	IMX_PIN_REG(MX53_PAD_EIM_D21, 0x474, 0x12C, 5, 0x814, 1), /* MX53_PAD_EIM_D21__I2C1_SCL */
-	IMX_PIN_REG(MX53_PAD_EIM_D21, 0x474, 0x12C, 6, 0x89C, 1), /* MX53_PAD_EIM_D21__USBOH3_USBOTG_OC */
-	IMX_PIN_REG(MX53_PAD_EIM_D22, 0x478, 0x130, 0, 0x000, 0), /* MX53_PAD_EIM_D22__EMI_WEIM_D_22 */
-	IMX_PIN_REG(MX53_PAD_EIM_D22, 0x478, 0x130, 1, 0x000, 0), /* MX53_PAD_EIM_D22__GPIO3_22 */
-	IMX_PIN_REG(MX53_PAD_EIM_D22, 0x478, 0x130, 2, 0x000, 0), /* MX53_PAD_EIM_D22__IPU_DI0_PIN1 */
-	IMX_PIN_REG(MX53_PAD_EIM_D22, 0x478, 0x130, 3, 0x82C, 0), /* MX53_PAD_EIM_D22__IPU_DISPB0_SER_DIN */
-	IMX_PIN_REG(MX53_PAD_EIM_D22, 0x478, 0x130, 4, 0x784, 1), /* MX53_PAD_EIM_D22__CSPI_MISO */
-	IMX_PIN_REG(MX53_PAD_EIM_D22, 0x478, 0x130, 6, 0x000, 0), /* MX53_PAD_EIM_D22__USBOH3_USBOTG_PWR */
-	IMX_PIN_REG(MX53_PAD_EIM_D23, 0x47C, 0x134, 0, 0x000, 0), /* MX53_PAD_EIM_D23__EMI_WEIM_D_23 */
-	IMX_PIN_REG(MX53_PAD_EIM_D23, 0x47C, 0x134, 1, 0x000, 0), /* MX53_PAD_EIM_D23__GPIO3_23 */
-	IMX_PIN_REG(MX53_PAD_EIM_D23, 0x47C, 0x134, 2, 0x000, 0), /* MX53_PAD_EIM_D23__UART3_CTS */
-	IMX_PIN_REG(MX53_PAD_EIM_D23, 0x47C, 0x134, 3, 0x000, 0), /* MX53_PAD_EIM_D23__UART1_DCD */
-	IMX_PIN_REG(MX53_PAD_EIM_D23, 0x47C, 0x134, 4, 0x000, 0), /* MX53_PAD_EIM_D23__IPU_DI0_D0_CS */
-	IMX_PIN_REG(MX53_PAD_EIM_D23, 0x47C, 0x134, 5, 0x000, 0), /* MX53_PAD_EIM_D23__IPU_DI1_PIN2 */
-	IMX_PIN_REG(MX53_PAD_EIM_D23, 0x47C, 0x134, 6, 0x834, 0), /* MX53_PAD_EIM_D23__IPU_CSI1_DATA_EN */
-	IMX_PIN_REG(MX53_PAD_EIM_D23, 0x47C, 0x134, 7, 0x000, 0), /* MX53_PAD_EIM_D23__IPU_DI1_PIN14 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB3, 0x480, 0x138, 0, 0x000, 0), /* MX53_PAD_EIM_EB3__EMI_WEIM_EB_3 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB3, 0x480, 0x138, 1, 0x000, 0), /* MX53_PAD_EIM_EB3__GPIO2_31 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB3, 0x480, 0x138, 2, 0x884, 1), /* MX53_PAD_EIM_EB3__UART3_RTS */
-	IMX_PIN_REG(MX53_PAD_EIM_EB3, 0x480, 0x138, 3, 0x000, 0), /* MX53_PAD_EIM_EB3__UART1_RI */
-	IMX_PIN_REG(MX53_PAD_EIM_EB3, 0x480, 0x138, 5, 0x000, 0), /* MX53_PAD_EIM_EB3__IPU_DI1_PIN3 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB3, 0x480, 0x138, 6, 0x838, 0), /* MX53_PAD_EIM_EB3__IPU_CSI1_HSYNC */
-	IMX_PIN_REG(MX53_PAD_EIM_EB3, 0x480, 0x138, 7, 0x000, 0), /* MX53_PAD_EIM_EB3__IPU_DI1_PIN16 */
-	IMX_PIN_REG(MX53_PAD_EIM_D24, 0x484, 0x13C, 0, 0x000, 0), /* MX53_PAD_EIM_D24__EMI_WEIM_D_24 */
-	IMX_PIN_REG(MX53_PAD_EIM_D24, 0x484, 0x13C, 1, 0x000, 0), /* MX53_PAD_EIM_D24__GPIO3_24 */
-	IMX_PIN_REG(MX53_PAD_EIM_D24, 0x484, 0x13C, 2, 0x000, 0), /* MX53_PAD_EIM_D24__UART3_TXD_MUX */
-	IMX_PIN_REG(MX53_PAD_EIM_D24, 0x484, 0x13C, 3, 0x7B0, 1), /* MX53_PAD_EIM_D24__ECSPI1_SS2 */
-	IMX_PIN_REG(MX53_PAD_EIM_D24, 0x484, 0x13C, 4, 0x794, 1), /* MX53_PAD_EIM_D24__CSPI_SS2 */
-	IMX_PIN_REG(MX53_PAD_EIM_D24, 0x484, 0x13C, 5, 0x754, 1), /* MX53_PAD_EIM_D24__AUDMUX_AUD5_RXFS */
-	IMX_PIN_REG(MX53_PAD_EIM_D24, 0x484, 0x13C, 6, 0x000, 0), /* MX53_PAD_EIM_D24__ECSPI2_SS2 */
-	IMX_PIN_REG(MX53_PAD_EIM_D24, 0x484, 0x13C, 7, 0x000, 0), /* MX53_PAD_EIM_D24__UART1_DTR */
-	IMX_PIN_REG(MX53_PAD_EIM_D25, 0x488, 0x140, 0, 0x000, 0), /* MX53_PAD_EIM_D25__EMI_WEIM_D_25 */
-	IMX_PIN_REG(MX53_PAD_EIM_D25, 0x488, 0x140, 1, 0x000, 0), /* MX53_PAD_EIM_D25__GPIO3_25 */
-	IMX_PIN_REG(MX53_PAD_EIM_D25, 0x488, 0x140, 2, 0x888, 1), /* MX53_PAD_EIM_D25__UART3_RXD_MUX */
-	IMX_PIN_REG(MX53_PAD_EIM_D25, 0x488, 0x140, 3, 0x7B4, 1), /* MX53_PAD_EIM_D25__ECSPI1_SS3 */
-	IMX_PIN_REG(MX53_PAD_EIM_D25, 0x488, 0x140, 4, 0x798, 1), /* MX53_PAD_EIM_D25__CSPI_SS3 */
-	IMX_PIN_REG(MX53_PAD_EIM_D25, 0x488, 0x140, 5, 0x750, 1), /* MX53_PAD_EIM_D25__AUDMUX_AUD5_RXC */
-	IMX_PIN_REG(MX53_PAD_EIM_D25, 0x488, 0x140, 6, 0x000, 0), /* MX53_PAD_EIM_D25__ECSPI2_SS3 */
-	IMX_PIN_REG(MX53_PAD_EIM_D25, 0x488, 0x140, 7, 0x000, 0), /* MX53_PAD_EIM_D25__UART1_DSR */
-	IMX_PIN_REG(MX53_PAD_EIM_D26, 0x48C, 0x144, 0, 0x000, 0), /* MX53_PAD_EIM_D26__EMI_WEIM_D_26 */
-	IMX_PIN_REG(MX53_PAD_EIM_D26, 0x48C, 0x144, 1, 0x000, 0), /* MX53_PAD_EIM_D26__GPIO3_26 */
-	IMX_PIN_REG(MX53_PAD_EIM_D26, 0x48C, 0x144, 2, 0x000, 0), /* MX53_PAD_EIM_D26__UART2_TXD_MUX */
-	IMX_PIN_REG(MX53_PAD_EIM_D26, 0x48C, 0x144, 3, 0x80C, 0), /* MX53_PAD_EIM_D26__FIRI_RXD */
-	IMX_PIN_REG(MX53_PAD_EIM_D26, 0x48C, 0x144, 4, 0x000, 0), /* MX53_PAD_EIM_D26__IPU_CSI0_D_1 */
-	IMX_PIN_REG(MX53_PAD_EIM_D26, 0x48C, 0x144, 5, 0x000, 0), /* MX53_PAD_EIM_D26__IPU_DI1_PIN11 */
-	IMX_PIN_REG(MX53_PAD_EIM_D26, 0x48C, 0x144, 6, 0x000, 0), /* MX53_PAD_EIM_D26__IPU_SISG_2 */
-	IMX_PIN_REG(MX53_PAD_EIM_D26, 0x48C, 0x144, 7, 0x000, 0), /* MX53_PAD_EIM_D26__IPU_DISP1_DAT_22 */
-	IMX_PIN_REG(MX53_PAD_EIM_D27, 0x490, 0x148, 0, 0x000, 0), /* MX53_PAD_EIM_D27__EMI_WEIM_D_27 */
-	IMX_PIN_REG(MX53_PAD_EIM_D27, 0x490, 0x148, 1, 0x000, 0), /* MX53_PAD_EIM_D27__GPIO3_27 */
-	IMX_PIN_REG(MX53_PAD_EIM_D27, 0x490, 0x148, 2, 0x880, 1), /* MX53_PAD_EIM_D27__UART2_RXD_MUX */
-	IMX_PIN_REG(MX53_PAD_EIM_D27, 0x490, 0x148, 3, 0x000, 0), /* MX53_PAD_EIM_D27__FIRI_TXD */
-	IMX_PIN_REG(MX53_PAD_EIM_D27, 0x490, 0x148, 4, 0x000, 0), /* MX53_PAD_EIM_D27__IPU_CSI0_D_0 */
-	IMX_PIN_REG(MX53_PAD_EIM_D27, 0x490, 0x148, 5, 0x000, 0), /* MX53_PAD_EIM_D27__IPU_DI1_PIN13 */
-	IMX_PIN_REG(MX53_PAD_EIM_D27, 0x490, 0x148, 6, 0x000, 0), /* MX53_PAD_EIM_D27__IPU_SISG_3 */
-	IMX_PIN_REG(MX53_PAD_EIM_D27, 0x490, 0x148, 7, 0x000, 0), /* MX53_PAD_EIM_D27__IPU_DISP1_DAT_23 */
-	IMX_PIN_REG(MX53_PAD_EIM_D28, 0x494, 0x14C, 0, 0x000, 0), /* MX53_PAD_EIM_D28__EMI_WEIM_D_28 */
-	IMX_PIN_REG(MX53_PAD_EIM_D28, 0x494, 0x14C, 1, 0x000, 0), /* MX53_PAD_EIM_D28__GPIO3_28 */
-	IMX_PIN_REG(MX53_PAD_EIM_D28, 0x494, 0x14C, 2, 0x000, 0), /* MX53_PAD_EIM_D28__UART2_CTS */
-	IMX_PIN_REG(MX53_PAD_EIM_D28, 0x494, 0x14C, 3, 0x82C, 1), /* MX53_PAD_EIM_D28__IPU_DISPB0_SER_DIO */
-	IMX_PIN_REG(MX53_PAD_EIM_D28, 0x494, 0x14C, 4, 0x788, 1), /* MX53_PAD_EIM_D28__CSPI_MOSI */
-	IMX_PIN_REG(MX53_PAD_EIM_D28, 0x494, 0x14C, 5, 0x818, 1), /* MX53_PAD_EIM_D28__I2C1_SDA */
-	IMX_PIN_REG(MX53_PAD_EIM_D28, 0x494, 0x14C, 6, 0x000, 0), /* MX53_PAD_EIM_D28__IPU_EXT_TRIG */
-	IMX_PIN_REG(MX53_PAD_EIM_D28, 0x494, 0x14C, 7, 0x000, 0), /* MX53_PAD_EIM_D28__IPU_DI0_PIN13 */
-	IMX_PIN_REG(MX53_PAD_EIM_D29, 0x498, 0x150, 0, 0x000, 0), /* MX53_PAD_EIM_D29__EMI_WEIM_D_29 */
-	IMX_PIN_REG(MX53_PAD_EIM_D29, 0x498, 0x150, 1, 0x000, 0), /* MX53_PAD_EIM_D29__GPIO3_29 */
-	IMX_PIN_REG(MX53_PAD_EIM_D29, 0x498, 0x150, 2, 0x87C, 1), /* MX53_PAD_EIM_D29__UART2_RTS */
-	IMX_PIN_REG(MX53_PAD_EIM_D29, 0x498, 0x150, 3, 0x000, 0), /* MX53_PAD_EIM_D29__IPU_DISPB0_SER_RS */
-	IMX_PIN_REG(MX53_PAD_EIM_D29, 0x498, 0x150, 4, 0x78C, 2), /* MX53_PAD_EIM_D29__CSPI_SS0 */
-	IMX_PIN_REG(MX53_PAD_EIM_D29, 0x498, 0x150, 5, 0x000, 0), /* MX53_PAD_EIM_D29__IPU_DI1_PIN15 */
-	IMX_PIN_REG(MX53_PAD_EIM_D29, 0x498, 0x150, 6, 0x83C, 0), /* MX53_PAD_EIM_D29__IPU_CSI1_VSYNC */
-	IMX_PIN_REG(MX53_PAD_EIM_D29, 0x498, 0x150, 7, 0x000, 0), /* MX53_PAD_EIM_D29__IPU_DI0_PIN14 */
-	IMX_PIN_REG(MX53_PAD_EIM_D30, 0x49C, 0x154, 0, 0x000, 0), /* MX53_PAD_EIM_D30__EMI_WEIM_D_30 */
-	IMX_PIN_REG(MX53_PAD_EIM_D30, 0x49C, 0x154, 1, 0x000, 0), /* MX53_PAD_EIM_D30__GPIO3_30 */
-	IMX_PIN_REG(MX53_PAD_EIM_D30, 0x49C, 0x154, 2, 0x000, 0), /* MX53_PAD_EIM_D30__UART3_CTS */
-	IMX_PIN_REG(MX53_PAD_EIM_D30, 0x49C, 0x154, 3, 0x000, 0), /* MX53_PAD_EIM_D30__IPU_CSI0_D_3 */
-	IMX_PIN_REG(MX53_PAD_EIM_D30, 0x49C, 0x154, 4, 0x000, 0), /* MX53_PAD_EIM_D30__IPU_DI0_PIN11 */
-	IMX_PIN_REG(MX53_PAD_EIM_D30, 0x49C, 0x154, 5, 0x000, 0), /* MX53_PAD_EIM_D30__IPU_DISP1_DAT_21 */
-	IMX_PIN_REG(MX53_PAD_EIM_D30, 0x49C, 0x154, 6, 0x8A0, 0), /* MX53_PAD_EIM_D30__USBOH3_USBH1_OC */
-	IMX_PIN_REG(MX53_PAD_EIM_D30, 0x49C, 0x154, 7, 0x8A4, 1), /* MX53_PAD_EIM_D30__USBOH3_USBH2_OC */
-	IMX_PIN_REG(MX53_PAD_EIM_D31, 0x4A0, 0x158, 0, 0x000, 0), /* MX53_PAD_EIM_D31__EMI_WEIM_D_31 */
-	IMX_PIN_REG(MX53_PAD_EIM_D31, 0x4A0, 0x158, 1, 0x000, 0), /* MX53_PAD_EIM_D31__GPIO3_31 */
-	IMX_PIN_REG(MX53_PAD_EIM_D31, 0x4A0, 0x158, 2, 0x884, 3), /* MX53_PAD_EIM_D31__UART3_RTS */
-	IMX_PIN_REG(MX53_PAD_EIM_D31, 0x4A0, 0x158, 3, 0x000, 0), /* MX53_PAD_EIM_D31__IPU_CSI0_D_2 */
-	IMX_PIN_REG(MX53_PAD_EIM_D31, 0x4A0, 0x158, 4, 0x000, 0), /* MX53_PAD_EIM_D31__IPU_DI0_PIN12 */
-	IMX_PIN_REG(MX53_PAD_EIM_D31, 0x4A0, 0x158, 5, 0x000, 0), /* MX53_PAD_EIM_D31__IPU_DISP1_DAT_20 */
-	IMX_PIN_REG(MX53_PAD_EIM_D31, 0x4A0, 0x158, 6, 0x000, 0), /* MX53_PAD_EIM_D31__USBOH3_USBH1_PWR */
-	IMX_PIN_REG(MX53_PAD_EIM_D31, 0x4A0, 0x158, 7, 0x000, 0), /* MX53_PAD_EIM_D31__USBOH3_USBH2_PWR */
-	IMX_PIN_REG(MX53_PAD_EIM_A24, 0x4A8, 0x15C, 0, 0x000, 0), /* MX53_PAD_EIM_A24__EMI_WEIM_A_24 */
-	IMX_PIN_REG(MX53_PAD_EIM_A24, 0x4A8, 0x15C, 1, 0x000, 0), /* MX53_PAD_EIM_A24__GPIO5_4 */
-	IMX_PIN_REG(MX53_PAD_EIM_A24, 0x4A8, 0x15C, 2, 0x000, 0), /* MX53_PAD_EIM_A24__IPU_DISP1_DAT_19 */
-	IMX_PIN_REG(MX53_PAD_EIM_A24, 0x4A8, 0x15C, 3, 0x000, 0), /* MX53_PAD_EIM_A24__IPU_CSI1_D_19 */
-	IMX_PIN_REG(MX53_PAD_EIM_A24, 0x4A8, 0x15C, 6, 0x000, 0), /* MX53_PAD_EIM_A24__IPU_SISG_2 */
-	IMX_PIN_REG(MX53_PAD_EIM_A24, 0x4A8, 0x15C, 7, 0x000, 0), /* MX53_PAD_EIM_A24__USBPHY2_BVALID */
-	IMX_PIN_REG(MX53_PAD_EIM_A23, 0x4AC, 0x160, 0, 0x000, 0), /* MX53_PAD_EIM_A23__EMI_WEIM_A_23 */
-	IMX_PIN_REG(MX53_PAD_EIM_A23, 0x4AC, 0x160, 1, 0x000, 0), /* MX53_PAD_EIM_A23__GPIO6_6 */
-	IMX_PIN_REG(MX53_PAD_EIM_A23, 0x4AC, 0x160, 2, 0x000, 0), /* MX53_PAD_EIM_A23__IPU_DISP1_DAT_18 */
-	IMX_PIN_REG(MX53_PAD_EIM_A23, 0x4AC, 0x160, 3, 0x000, 0), /* MX53_PAD_EIM_A23__IPU_CSI1_D_18 */
-	IMX_PIN_REG(MX53_PAD_EIM_A23, 0x4AC, 0x160, 6, 0x000, 0), /* MX53_PAD_EIM_A23__IPU_SISG_3 */
-	IMX_PIN_REG(MX53_PAD_EIM_A23, 0x4AC, 0x160, 7, 0x000, 0), /* MX53_PAD_EIM_A23__USBPHY2_ENDSESSION */
-	IMX_PIN_REG(MX53_PAD_EIM_A22, 0x4B0, 0x164, 0, 0x000, 0), /* MX53_PAD_EIM_A22__EMI_WEIM_A_22 */
-	IMX_PIN_REG(MX53_PAD_EIM_A22, 0x4B0, 0x164, 1, 0x000, 0), /* MX53_PAD_EIM_A22__GPIO2_16 */
-	IMX_PIN_REG(MX53_PAD_EIM_A22, 0x4B0, 0x164, 2, 0x000, 0), /* MX53_PAD_EIM_A22__IPU_DISP1_DAT_17 */
-	IMX_PIN_REG(MX53_PAD_EIM_A22, 0x4B0, 0x164, 3, 0x000, 0), /* MX53_PAD_EIM_A22__IPU_CSI1_D_17 */
-	IMX_PIN_REG(MX53_PAD_EIM_A22, 0x4B0, 0x164, 7, 0x000, 0), /* MX53_PAD_EIM_A22__SRC_BT_CFG1_7 */
-	IMX_PIN_REG(MX53_PAD_EIM_A21, 0x4B4, 0x168, 0, 0x000, 0), /* MX53_PAD_EIM_A21__EMI_WEIM_A_21 */
-	IMX_PIN_REG(MX53_PAD_EIM_A21, 0x4B4, 0x168, 1, 0x000, 0), /* MX53_PAD_EIM_A21__GPIO2_17 */
-	IMX_PIN_REG(MX53_PAD_EIM_A21, 0x4B4, 0x168, 2, 0x000, 0), /* MX53_PAD_EIM_A21__IPU_DISP1_DAT_16 */
-	IMX_PIN_REG(MX53_PAD_EIM_A21, 0x4B4, 0x168, 3, 0x000, 0), /* MX53_PAD_EIM_A21__IPU_CSI1_D_16 */
-	IMX_PIN_REG(MX53_PAD_EIM_A21, 0x4B4, 0x168, 7, 0x000, 0), /* MX53_PAD_EIM_A21__SRC_BT_CFG1_6 */
-	IMX_PIN_REG(MX53_PAD_EIM_A20, 0x4B8, 0x16C, 0, 0x000, 0), /* MX53_PAD_EIM_A20__EMI_WEIM_A_20 */
-	IMX_PIN_REG(MX53_PAD_EIM_A20, 0x4B8, 0x16C, 1, 0x000, 0), /* MX53_PAD_EIM_A20__GPIO2_18 */
-	IMX_PIN_REG(MX53_PAD_EIM_A20, 0x4B8, 0x16C, 2, 0x000, 0), /* MX53_PAD_EIM_A20__IPU_DISP1_DAT_15 */
-	IMX_PIN_REG(MX53_PAD_EIM_A20, 0x4B8, 0x16C, 3, 0x000, 0), /* MX53_PAD_EIM_A20__IPU_CSI1_D_15 */
-	IMX_PIN_REG(MX53_PAD_EIM_A20, 0x4B8, 0x16C, 7, 0x000, 0), /* MX53_PAD_EIM_A20__SRC_BT_CFG1_5 */
-	IMX_PIN_REG(MX53_PAD_EIM_A19, 0x4BC, 0x170, 0, 0x000, 0), /* MX53_PAD_EIM_A19__EMI_WEIM_A_19 */
-	IMX_PIN_REG(MX53_PAD_EIM_A19, 0x4BC, 0x170, 1, 0x000, 0), /* MX53_PAD_EIM_A19__GPIO2_19 */
-	IMX_PIN_REG(MX53_PAD_EIM_A19, 0x4BC, 0x170, 2, 0x000, 0), /* MX53_PAD_EIM_A19__IPU_DISP1_DAT_14 */
-	IMX_PIN_REG(MX53_PAD_EIM_A19, 0x4BC, 0x170, 3, 0x000, 0), /* MX53_PAD_EIM_A19__IPU_CSI1_D_14 */
-	IMX_PIN_REG(MX53_PAD_EIM_A19, 0x4BC, 0x170, 7, 0x000, 0), /* MX53_PAD_EIM_A19__SRC_BT_CFG1_4 */
-	IMX_PIN_REG(MX53_PAD_EIM_A18, 0x4C0, 0x174, 0, 0x000, 0), /* MX53_PAD_EIM_A18__EMI_WEIM_A_18 */
-	IMX_PIN_REG(MX53_PAD_EIM_A18, 0x4C0, 0x174, 1, 0x000, 0), /* MX53_PAD_EIM_A18__GPIO2_20 */
-	IMX_PIN_REG(MX53_PAD_EIM_A18, 0x4C0, 0x174, 2, 0x000, 0), /* MX53_PAD_EIM_A18__IPU_DISP1_DAT_13 */
-	IMX_PIN_REG(MX53_PAD_EIM_A18, 0x4C0, 0x174, 3, 0x000, 0), /* MX53_PAD_EIM_A18__IPU_CSI1_D_13 */
-	IMX_PIN_REG(MX53_PAD_EIM_A18, 0x4C0, 0x174, 7, 0x000, 0), /* MX53_PAD_EIM_A18__SRC_BT_CFG1_3 */
-	IMX_PIN_REG(MX53_PAD_EIM_A17, 0x4C4, 0x178, 0, 0x000, 0), /* MX53_PAD_EIM_A17__EMI_WEIM_A_17 */
-	IMX_PIN_REG(MX53_PAD_EIM_A17, 0x4C4, 0x178, 1, 0x000, 0), /* MX53_PAD_EIM_A17__GPIO2_21 */
-	IMX_PIN_REG(MX53_PAD_EIM_A17, 0x4C4, 0x178, 2, 0x000, 0), /* MX53_PAD_EIM_A17__IPU_DISP1_DAT_12 */
-	IMX_PIN_REG(MX53_PAD_EIM_A17, 0x4C4, 0x178, 3, 0x000, 0), /* MX53_PAD_EIM_A17__IPU_CSI1_D_12 */
-	IMX_PIN_REG(MX53_PAD_EIM_A17, 0x4C4, 0x178, 7, 0x000, 0), /* MX53_PAD_EIM_A17__SRC_BT_CFG1_2 */
-	IMX_PIN_REG(MX53_PAD_EIM_A16, 0x4C8, 0x17C, 0, 0x000, 0), /* MX53_PAD_EIM_A16__EMI_WEIM_A_16 */
-	IMX_PIN_REG(MX53_PAD_EIM_A16, 0x4C8, 0x17C, 1, 0x000, 0), /* MX53_PAD_EIM_A16__GPIO2_22 */
-	IMX_PIN_REG(MX53_PAD_EIM_A16, 0x4C8, 0x17C, 2, 0x000, 0), /* MX53_PAD_EIM_A16__IPU_DI1_DISP_CLK */
-	IMX_PIN_REG(MX53_PAD_EIM_A16, 0x4C8, 0x17C, 3, 0x000, 0), /* MX53_PAD_EIM_A16__IPU_CSI1_PIXCLK */
-	IMX_PIN_REG(MX53_PAD_EIM_A16, 0x4C8, 0x17C, 7, 0x000, 0), /* MX53_PAD_EIM_A16__SRC_BT_CFG1_1 */
-	IMX_PIN_REG(MX53_PAD_EIM_CS0, 0x4CC, 0x180, 0, 0x000, 0), /* MX53_PAD_EIM_CS0__EMI_WEIM_CS_0 */
-	IMX_PIN_REG(MX53_PAD_EIM_CS0, 0x4CC, 0x180, 1, 0x000, 0), /* MX53_PAD_EIM_CS0__GPIO2_23 */
-	IMX_PIN_REG(MX53_PAD_EIM_CS0, 0x4CC, 0x180, 2, 0x7B8, 2), /* MX53_PAD_EIM_CS0__ECSPI2_SCLK */
-	IMX_PIN_REG(MX53_PAD_EIM_CS0, 0x4CC, 0x180, 3, 0x000, 0), /* MX53_PAD_EIM_CS0__IPU_DI1_PIN5 */
-	IMX_PIN_REG(MX53_PAD_EIM_CS1, 0x4D0, 0x184, 0, 0x000, 0), /* MX53_PAD_EIM_CS1__EMI_WEIM_CS_1 */
-	IMX_PIN_REG(MX53_PAD_EIM_CS1, 0x4D0, 0x184, 1, 0x000, 0), /* MX53_PAD_EIM_CS1__GPIO2_24 */
-	IMX_PIN_REG(MX53_PAD_EIM_CS1, 0x4D0, 0x184, 2, 0x7C0, 2), /* MX53_PAD_EIM_CS1__ECSPI2_MOSI */
-	IMX_PIN_REG(MX53_PAD_EIM_CS1, 0x4D0, 0x184, 3, 0x000, 0), /* MX53_PAD_EIM_CS1__IPU_DI1_PIN6 */
-	IMX_PIN_REG(MX53_PAD_EIM_OE, 0x4D4, 0x188, 0, 0x000, 0), /* MX53_PAD_EIM_OE__EMI_WEIM_OE */
-	IMX_PIN_REG(MX53_PAD_EIM_OE, 0x4D4, 0x188, 1, 0x000, 0), /* MX53_PAD_EIM_OE__GPIO2_25 */
-	IMX_PIN_REG(MX53_PAD_EIM_OE, 0x4D4, 0x188, 2, 0x7BC, 2), /* MX53_PAD_EIM_OE__ECSPI2_MISO */
-	IMX_PIN_REG(MX53_PAD_EIM_OE, 0x4D4, 0x188, 3, 0x000, 0), /* MX53_PAD_EIM_OE__IPU_DI1_PIN7 */
-	IMX_PIN_REG(MX53_PAD_EIM_OE, 0x4D4, 0x188, 7, 0x000, 0), /* MX53_PAD_EIM_OE__USBPHY2_IDDIG */
-	IMX_PIN_REG(MX53_PAD_EIM_RW, 0x4D8, 0x18C, 0, 0x000, 0), /* MX53_PAD_EIM_RW__EMI_WEIM_RW */
-	IMX_PIN_REG(MX53_PAD_EIM_RW, 0x4D8, 0x18C, 1, 0x000, 0), /* MX53_PAD_EIM_RW__GPIO2_26 */
-	IMX_PIN_REG(MX53_PAD_EIM_RW, 0x4D8, 0x18C, 2, 0x7C4, 2), /* MX53_PAD_EIM_RW__ECSPI2_SS0 */
-	IMX_PIN_REG(MX53_PAD_EIM_RW, 0x4D8, 0x18C, 3, 0x000, 0), /* MX53_PAD_EIM_RW__IPU_DI1_PIN8 */
-	IMX_PIN_REG(MX53_PAD_EIM_RW, 0x4D8, 0x18C, 7, 0x000, 0), /* MX53_PAD_EIM_RW__USBPHY2_HOSTDISCONNECT */
-	IMX_PIN_REG(MX53_PAD_EIM_LBA, 0x4DC, 0x190, 0, 0x000, 0), /* MX53_PAD_EIM_LBA__EMI_WEIM_LBA */
-	IMX_PIN_REG(MX53_PAD_EIM_LBA, 0x4DC, 0x190, 1, 0x000, 0), /* MX53_PAD_EIM_LBA__GPIO2_27 */
-	IMX_PIN_REG(MX53_PAD_EIM_LBA, 0x4DC, 0x190, 2, 0x7C8, 1), /* MX53_PAD_EIM_LBA__ECSPI2_SS1 */
-	IMX_PIN_REG(MX53_PAD_EIM_LBA, 0x4DC, 0x190, 3, 0x000, 0), /* MX53_PAD_EIM_LBA__IPU_DI1_PIN17 */
-	IMX_PIN_REG(MX53_PAD_EIM_LBA, 0x4DC, 0x190, 7, 0x000, 0), /* MX53_PAD_EIM_LBA__SRC_BT_CFG1_0 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB0, 0x4E4, 0x194, 0, 0x000, 0), /* MX53_PAD_EIM_EB0__EMI_WEIM_EB_0 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB0, 0x4E4, 0x194, 1, 0x000, 0), /* MX53_PAD_EIM_EB0__GPIO2_28 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB0, 0x4E4, 0x194, 3, 0x000, 0), /* MX53_PAD_EIM_EB0__IPU_DISP1_DAT_11 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB0, 0x4E4, 0x194, 4, 0x000, 0), /* MX53_PAD_EIM_EB0__IPU_CSI1_D_11 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB0, 0x4E4, 0x194, 5, 0x810, 0), /* MX53_PAD_EIM_EB0__GPC_PMIC_RDY */
-	IMX_PIN_REG(MX53_PAD_EIM_EB0, 0x4E4, 0x194, 7, 0x000, 0), /* MX53_PAD_EIM_EB0__SRC_BT_CFG2_7 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB1, 0x4E8, 0x198, 0, 0x000, 0), /* MX53_PAD_EIM_EB1__EMI_WEIM_EB_1 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB1, 0x4E8, 0x198, 1, 0x000, 0), /* MX53_PAD_EIM_EB1__GPIO2_29 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB1, 0x4E8, 0x198, 3, 0x000, 0), /* MX53_PAD_EIM_EB1__IPU_DISP1_DAT_10 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB1, 0x4E8, 0x198, 4, 0x000, 0), /* MX53_PAD_EIM_EB1__IPU_CSI1_D_10 */
-	IMX_PIN_REG(MX53_PAD_EIM_EB1, 0x4E8, 0x198, 7, 0x000, 0), /* MX53_PAD_EIM_EB1__SRC_BT_CFG2_6 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA0, 0x4EC, 0x19C, 0, 0x000, 0), /* MX53_PAD_EIM_DA0__EMI_NAND_WEIM_DA_0 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA0, 0x4EC, 0x19C, 1, 0x000, 0), /* MX53_PAD_EIM_DA0__GPIO3_0 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA0, 0x4EC, 0x19C, 3, 0x000, 0), /* MX53_PAD_EIM_DA0__IPU_DISP1_DAT_9 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA0, 0x4EC, 0x19C, 4, 0x000, 0), /* MX53_PAD_EIM_DA0__IPU_CSI1_D_9 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA0, 0x4EC, 0x19C, 7, 0x000, 0), /* MX53_PAD_EIM_DA0__SRC_BT_CFG2_5 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA1, 0x4F0, 0x1A0, 0, 0x000, 0), /* MX53_PAD_EIM_DA1__EMI_NAND_WEIM_DA_1 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA1, 0x4F0, 0x1A0, 1, 0x000, 0), /* MX53_PAD_EIM_DA1__GPIO3_1 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA1, 0x4F0, 0x1A0, 3, 0x000, 0), /* MX53_PAD_EIM_DA1__IPU_DISP1_DAT_8 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA1, 0x4F0, 0x1A0, 4, 0x000, 0), /* MX53_PAD_EIM_DA1__IPU_CSI1_D_8 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA1, 0x4F0, 0x1A0, 7, 0x000, 0), /* MX53_PAD_EIM_DA1__SRC_BT_CFG2_4 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA2, 0x4F4, 0x1A4, 0, 0x000, 0), /* MX53_PAD_EIM_DA2__EMI_NAND_WEIM_DA_2 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA2, 0x4F4, 0x1A4, 1, 0x000, 0), /* MX53_PAD_EIM_DA2__GPIO3_2 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA2, 0x4F4, 0x1A4, 3, 0x000, 0), /* MX53_PAD_EIM_DA2__IPU_DISP1_DAT_7 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA2, 0x4F4, 0x1A4, 4, 0x000, 0), /* MX53_PAD_EIM_DA2__IPU_CSI1_D_7 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA2, 0x4F4, 0x1A4, 7, 0x000, 0), /* MX53_PAD_EIM_DA2__SRC_BT_CFG2_3 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA3, 0x4F8, 0x1A8, 0, 0x000, 0), /* MX53_PAD_EIM_DA3__EMI_NAND_WEIM_DA_3 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA3, 0x4F8, 0x1A8, 1, 0x000, 0), /* MX53_PAD_EIM_DA3__GPIO3_3 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA3, 0x4F8, 0x1A8, 3, 0x000, 0), /* MX53_PAD_EIM_DA3__IPU_DISP1_DAT_6 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA3, 0x4F8, 0x1A8, 4, 0x000, 0), /* MX53_PAD_EIM_DA3__IPU_CSI1_D_6 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA3, 0x4F8, 0x1A8, 7, 0x000, 0), /* MX53_PAD_EIM_DA3__SRC_BT_CFG2_2 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA4, 0x4FC, 0x1AC, 0, 0x000, 0), /* MX53_PAD_EIM_DA4__EMI_NAND_WEIM_DA_4 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA4, 0x4FC, 0x1AC, 1, 0x000, 0), /* MX53_PAD_EIM_DA4__GPIO3_4 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA4, 0x4FC, 0x1AC, 3, 0x000, 0), /* MX53_PAD_EIM_DA4__IPU_DISP1_DAT_5 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA4, 0x4FC, 0x1AC, 4, 0x000, 0), /* MX53_PAD_EIM_DA4__IPU_CSI1_D_5 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA4, 0x4FC, 0x1AC, 7, 0x000, 0), /* MX53_PAD_EIM_DA4__SRC_BT_CFG3_7 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA5, 0x500, 0x1B0, 0, 0x000, 0), /* MX53_PAD_EIM_DA5__EMI_NAND_WEIM_DA_5 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA5, 0x500, 0x1B0, 1, 0x000, 0), /* MX53_PAD_EIM_DA5__GPIO3_5 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA5, 0x500, 0x1B0, 3, 0x000, 0), /* MX53_PAD_EIM_DA5__IPU_DISP1_DAT_4 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA5, 0x500, 0x1B0, 4, 0x000, 0), /* MX53_PAD_EIM_DA5__IPU_CSI1_D_4 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA5, 0x500, 0x1B0, 7, 0x000, 0), /* MX53_PAD_EIM_DA5__SRC_BT_CFG3_6 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA6, 0x504, 0x1B4, 0, 0x000, 0), /* MX53_PAD_EIM_DA6__EMI_NAND_WEIM_DA_6 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA6, 0x504, 0x1B4, 1, 0x000, 0), /* MX53_PAD_EIM_DA6__GPIO3_6 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA6, 0x504, 0x1B4, 3, 0x000, 0), /* MX53_PAD_EIM_DA6__IPU_DISP1_DAT_3 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA6, 0x504, 0x1B4, 4, 0x000, 0), /* MX53_PAD_EIM_DA6__IPU_CSI1_D_3 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA6, 0x504, 0x1B4, 7, 0x000, 0), /* MX53_PAD_EIM_DA6__SRC_BT_CFG3_5 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA7, 0x508, 0x1B8, 0, 0x000, 0), /* MX53_PAD_EIM_DA7__EMI_NAND_WEIM_DA_7 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA7, 0x508, 0x1B8, 1, 0x000, 0), /* MX53_PAD_EIM_DA7__GPIO3_7 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA7, 0x508, 0x1B8, 3, 0x000, 0), /* MX53_PAD_EIM_DA7__IPU_DISP1_DAT_2 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA7, 0x508, 0x1B8, 4, 0x000, 0), /* MX53_PAD_EIM_DA7__IPU_CSI1_D_2 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA7, 0x508, 0x1B8, 7, 0x000, 0), /* MX53_PAD_EIM_DA7__SRC_BT_CFG3_4 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA8, 0x50C, 0x1BC, 0, 0x000, 0), /* MX53_PAD_EIM_DA8__EMI_NAND_WEIM_DA_8 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA8, 0x50C, 0x1BC, 1, 0x000, 0), /* MX53_PAD_EIM_DA8__GPIO3_8 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA8, 0x50C, 0x1BC, 3, 0x000, 0), /* MX53_PAD_EIM_DA8__IPU_DISP1_DAT_1 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA8, 0x50C, 0x1BC, 4, 0x000, 0), /* MX53_PAD_EIM_DA8__IPU_CSI1_D_1 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA8, 0x50C, 0x1BC, 7, 0x000, 0), /* MX53_PAD_EIM_DA8__SRC_BT_CFG3_3 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA9, 0x510, 0x1C0, 0, 0x000, 0), /* MX53_PAD_EIM_DA9__EMI_NAND_WEIM_DA_9 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA9, 0x510, 0x1C0, 1, 0x000, 0), /* MX53_PAD_EIM_DA9__GPIO3_9 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA9, 0x510, 0x1C0, 3, 0x000, 0), /* MX53_PAD_EIM_DA9__IPU_DISP1_DAT_0 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA9, 0x510, 0x1C0, 4, 0x000, 0), /* MX53_PAD_EIM_DA9__IPU_CSI1_D_0 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA9, 0x510, 0x1C0, 7, 0x000, 0), /* MX53_PAD_EIM_DA9__SRC_BT_CFG3_2 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA10, 0x514, 0x1C4, 0, 0x000, 0), /* MX53_PAD_EIM_DA10__EMI_NAND_WEIM_DA_10 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA10, 0x514, 0x1C4, 1, 0x000, 0), /* MX53_PAD_EIM_DA10__GPIO3_10 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA10, 0x514, 0x1C4, 3, 0x000, 0), /* MX53_PAD_EIM_DA10__IPU_DI1_PIN15 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA10, 0x514, 0x1C4, 4, 0x834, 1), /* MX53_PAD_EIM_DA10__IPU_CSI1_DATA_EN */
-	IMX_PIN_REG(MX53_PAD_EIM_DA10, 0x514, 0x1C4, 7, 0x000, 0), /* MX53_PAD_EIM_DA10__SRC_BT_CFG3_1 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA11, 0x518, 0x1C8, 0, 0x000, 0), /* MX53_PAD_EIM_DA11__EMI_NAND_WEIM_DA_11 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA11, 0x518, 0x1C8, 1, 0x000, 0), /* MX53_PAD_EIM_DA11__GPIO3_11 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA11, 0x518, 0x1C8, 3, 0x000, 0), /* MX53_PAD_EIM_DA11__IPU_DI1_PIN2 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA11, 0x518, 0x1C8, 4, 0x838, 1), /* MX53_PAD_EIM_DA11__IPU_CSI1_HSYNC */
-	IMX_PIN_REG(MX53_PAD_EIM_DA12, 0x51C, 0x1CC, 0, 0x000, 0), /* MX53_PAD_EIM_DA12__EMI_NAND_WEIM_DA_12 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA12, 0x51C, 0x1CC, 1, 0x000, 0), /* MX53_PAD_EIM_DA12__GPIO3_12 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA12, 0x51C, 0x1CC, 3, 0x000, 0), /* MX53_PAD_EIM_DA12__IPU_DI1_PIN3 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA12, 0x51C, 0x1CC, 4, 0x83C, 1), /* MX53_PAD_EIM_DA12__IPU_CSI1_VSYNC */
-	IMX_PIN_REG(MX53_PAD_EIM_DA13, 0x520, 0x1D0, 0, 0x000, 0), /* MX53_PAD_EIM_DA13__EMI_NAND_WEIM_DA_13 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA13, 0x520, 0x1D0, 1, 0x000, 0), /* MX53_PAD_EIM_DA13__GPIO3_13 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA13, 0x520, 0x1D0, 3, 0x000, 0), /* MX53_PAD_EIM_DA13__IPU_DI1_D0_CS */
-	IMX_PIN_REG(MX53_PAD_EIM_DA13, 0x520, 0x1D0, 4, 0x76C, 1), /* MX53_PAD_EIM_DA13__CCM_DI1_EXT_CLK */
-	IMX_PIN_REG(MX53_PAD_EIM_DA14, 0x524, 0x1D4, 0, 0x000, 0), /* MX53_PAD_EIM_DA14__EMI_NAND_WEIM_DA_14 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA14, 0x524, 0x1D4, 1, 0x000, 0), /* MX53_PAD_EIM_DA14__GPIO3_14 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA14, 0x524, 0x1D4, 3, 0x000, 0), /* MX53_PAD_EIM_DA14__IPU_DI1_D1_CS */
-	IMX_PIN_REG(MX53_PAD_EIM_DA14, 0x524, 0x1D4, 4, 0x000, 0), /* MX53_PAD_EIM_DA14__CCM_DI0_EXT_CLK */
-	IMX_PIN_REG(MX53_PAD_EIM_DA15, 0x528, 0x1D8, 0, 0x000, 0), /* MX53_PAD_EIM_DA15__EMI_NAND_WEIM_DA_15 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA15, 0x528, 0x1D8, 1, 0x000, 0), /* MX53_PAD_EIM_DA15__GPIO3_15 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA15, 0x528, 0x1D8, 3, 0x000, 0), /* MX53_PAD_EIM_DA15__IPU_DI1_PIN1 */
-	IMX_PIN_REG(MX53_PAD_EIM_DA15, 0x528, 0x1D8, 4, 0x000, 0), /* MX53_PAD_EIM_DA15__IPU_DI1_PIN4 */
-	IMX_PIN_REG(MX53_PAD_NANDF_WE_B, 0x52C, 0x1DC, 0, 0x000, 0), /* MX53_PAD_NANDF_WE_B__EMI_NANDF_WE_B */
-	IMX_PIN_REG(MX53_PAD_NANDF_WE_B, 0x52C, 0x1DC, 1, 0x000, 0), /* MX53_PAD_NANDF_WE_B__GPIO6_12 */
-	IMX_PIN_REG(MX53_PAD_NANDF_RE_B, 0x530, 0x1E0, 0, 0x000, 0), /* MX53_PAD_NANDF_RE_B__EMI_NANDF_RE_B */
-	IMX_PIN_REG(MX53_PAD_NANDF_RE_B, 0x530, 0x1E0, 1, 0x000, 0), /* MX53_PAD_NANDF_RE_B__GPIO6_13 */
-	IMX_PIN_REG(MX53_PAD_EIM_WAIT, 0x534, 0x1E4, 0, 0x000, 0), /* MX53_PAD_EIM_WAIT__EMI_WEIM_WAIT */
-	IMX_PIN_REG(MX53_PAD_EIM_WAIT, 0x534, 0x1E4, 1, 0x000, 0), /* MX53_PAD_EIM_WAIT__GPIO5_0 */
-	IMX_PIN_REG(MX53_PAD_EIM_WAIT, 0x534, 0x1E4, 2, 0x000, 0), /* MX53_PAD_EIM_WAIT__EMI_WEIM_DTACK_B */
-	IMX_PIN_REG(MX53_PAD_LVDS1_TX3_P, NO_PAD, 0x1EC, 0, 0x000, 0), /* MX53_PAD_LVDS1_TX3_P__GPIO6_22 */
-	IMX_PIN_REG(MX53_PAD_LVDS1_TX3_P, NO_PAD, 0x1EC, 1, 0x000, 0), /* MX53_PAD_LVDS1_TX3_P__LDB_LVDS1_TX3 */
-	IMX_PIN_REG(MX53_PAD_LVDS1_TX2_P, NO_PAD, 0x1F0, 0, 0x000, 0), /* MX53_PAD_LVDS1_TX2_P__GPIO6_24 */
-	IMX_PIN_REG(MX53_PAD_LVDS1_TX2_P, NO_PAD, 0x1F0, 1, 0x000, 0), /* MX53_PAD_LVDS1_TX2_P__LDB_LVDS1_TX2 */
-	IMX_PIN_REG(MX53_PAD_LVDS1_CLK_P, NO_PAD, 0x1F4, 0, 0x000, 0), /* MX53_PAD_LVDS1_CLK_P__GPIO6_26 */
-	IMX_PIN_REG(MX53_PAD_LVDS1_CLK_P, NO_PAD, 0x1F4, 1, 0x000, 0), /* MX53_PAD_LVDS1_CLK_P__LDB_LVDS1_CLK */
-	IMX_PIN_REG(MX53_PAD_LVDS1_TX1_P, NO_PAD, 0x1F8, 0, 0x000, 0), /* MX53_PAD_LVDS1_TX1_P__GPIO6_28 */
-	IMX_PIN_REG(MX53_PAD_LVDS1_TX1_P, NO_PAD, 0x1F8, 1, 0x000, 0), /* MX53_PAD_LVDS1_TX1_P__LDB_LVDS1_TX1 */
-	IMX_PIN_REG(MX53_PAD_LVDS1_TX0_P, NO_PAD, 0x1FC, 0, 0x000, 0), /* MX53_PAD_LVDS1_TX0_P__GPIO6_30 */
-	IMX_PIN_REG(MX53_PAD_LVDS1_TX0_P, NO_PAD, 0x1FC, 1, 0x000, 0), /* MX53_PAD_LVDS1_TX0_P__LDB_LVDS1_TX0 */
-	IMX_PIN_REG(MX53_PAD_LVDS0_TX3_P, NO_PAD, 0x200, 0, 0x000, 0), /* MX53_PAD_LVDS0_TX3_P__GPIO7_22 */
-	IMX_PIN_REG(MX53_PAD_LVDS0_TX3_P, NO_PAD, 0x200, 1, 0x000, 0), /* MX53_PAD_LVDS0_TX3_P__LDB_LVDS0_TX3 */
-	IMX_PIN_REG(MX53_PAD_LVDS0_CLK_P, NO_PAD, 0x204, 0, 0x000, 0), /* MX53_PAD_LVDS0_CLK_P__GPIO7_24 */
-	IMX_PIN_REG(MX53_PAD_LVDS0_CLK_P, NO_PAD, 0x204, 1, 0x000, 0), /* MX53_PAD_LVDS0_CLK_P__LDB_LVDS0_CLK */
-	IMX_PIN_REG(MX53_PAD_LVDS0_TX2_P, NO_PAD, 0x208, 0, 0x000, 0), /* MX53_PAD_LVDS0_TX2_P__GPIO7_26 */
-	IMX_PIN_REG(MX53_PAD_LVDS0_TX2_P, NO_PAD, 0x208, 1, 0x000, 0), /* MX53_PAD_LVDS0_TX2_P__LDB_LVDS0_TX2 */
-	IMX_PIN_REG(MX53_PAD_LVDS0_TX1_P, NO_PAD, 0x20C, 0, 0x000, 0), /* MX53_PAD_LVDS0_TX1_P__GPIO7_28 */
-	IMX_PIN_REG(MX53_PAD_LVDS0_TX1_P, NO_PAD, 0x20C, 1, 0x000, 0), /* MX53_PAD_LVDS0_TX1_P__LDB_LVDS0_TX1 */
-	IMX_PIN_REG(MX53_PAD_LVDS0_TX0_P, NO_PAD, 0x210, 0, 0x000, 0), /* MX53_PAD_LVDS0_TX0_P__GPIO7_30 */
-	IMX_PIN_REG(MX53_PAD_LVDS0_TX0_P, NO_PAD, 0x210, 1, 0x000, 0), /* MX53_PAD_LVDS0_TX0_P__LDB_LVDS0_TX0 */
-	IMX_PIN_REG(MX53_PAD_GPIO_10, 0x540, 0x214, 0, 0x000, 0), /* MX53_PAD_GPIO_10__GPIO4_0 */
-	IMX_PIN_REG(MX53_PAD_GPIO_10, 0x540, 0x214, 1, 0x000, 0), /* MX53_PAD_GPIO_10__OSC32k_32K_OUT */
-	IMX_PIN_REG(MX53_PAD_GPIO_11, 0x544, 0x218, 0, 0x000, 0), /* MX53_PAD_GPIO_11__GPIO4_1 */
-	IMX_PIN_REG(MX53_PAD_GPIO_12, 0x548, 0x21C, 0, 0x000, 0), /* MX53_PAD_GPIO_12__GPIO4_2 */
-	IMX_PIN_REG(MX53_PAD_GPIO_13, 0x54C, 0x220, 0, 0x000, 0), /* MX53_PAD_GPIO_13__GPIO4_3 */
-	IMX_PIN_REG(MX53_PAD_GPIO_14, 0x550, 0x224, 0, 0x000, 0), /* MX53_PAD_GPIO_14__GPIO4_4 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CLE, 0x5A0, 0x228, 0, 0x000, 0), /* MX53_PAD_NANDF_CLE__EMI_NANDF_CLE */
-	IMX_PIN_REG(MX53_PAD_NANDF_CLE, 0x5A0, 0x228, 1, 0x000, 0), /* MX53_PAD_NANDF_CLE__GPIO6_7 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CLE, 0x5A0, 0x228, 7, 0x000, 0), /* MX53_PAD_NANDF_CLE__USBPHY1_VSTATUS_0 */
-	IMX_PIN_REG(MX53_PAD_NANDF_ALE, 0x5A4, 0x22C, 0, 0x000, 0), /* MX53_PAD_NANDF_ALE__EMI_NANDF_ALE */
-	IMX_PIN_REG(MX53_PAD_NANDF_ALE, 0x5A4, 0x22C, 1, 0x000, 0), /* MX53_PAD_NANDF_ALE__GPIO6_8 */
-	IMX_PIN_REG(MX53_PAD_NANDF_ALE, 0x5A4, 0x22C, 7, 0x000, 0), /* MX53_PAD_NANDF_ALE__USBPHY1_VSTATUS_1 */
-	IMX_PIN_REG(MX53_PAD_NANDF_WP_B, 0x5A8, 0x230, 0, 0x000, 0), /* MX53_PAD_NANDF_WP_B__EMI_NANDF_WP_B */
-	IMX_PIN_REG(MX53_PAD_NANDF_WP_B, 0x5A8, 0x230, 1, 0x000, 0), /* MX53_PAD_NANDF_WP_B__GPIO6_9 */
-	IMX_PIN_REG(MX53_PAD_NANDF_WP_B, 0x5A8, 0x230, 7, 0x000, 0), /* MX53_PAD_NANDF_WP_B__USBPHY1_VSTATUS_2 */
-	IMX_PIN_REG(MX53_PAD_NANDF_RB0, 0x5AC, 0x234, 0, 0x000, 0), /* MX53_PAD_NANDF_RB0__EMI_NANDF_RB_0 */
-	IMX_PIN_REG(MX53_PAD_NANDF_RB0, 0x5AC, 0x234, 1, 0x000, 0), /* MX53_PAD_NANDF_RB0__GPIO6_10 */
-	IMX_PIN_REG(MX53_PAD_NANDF_RB0, 0x5AC, 0x234, 7, 0x000, 0), /* MX53_PAD_NANDF_RB0__USBPHY1_VSTATUS_3 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS0, 0x5B0, 0x238, 0, 0x000, 0), /* MX53_PAD_NANDF_CS0__EMI_NANDF_CS_0 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS0, 0x5B0, 0x238, 1, 0x000, 0), /* MX53_PAD_NANDF_CS0__GPIO6_11 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS0, 0x5B0, 0x238, 7, 0x000, 0), /* MX53_PAD_NANDF_CS0__USBPHY1_VSTATUS_4 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS1, 0x5B4, 0x23C, 0, 0x000, 0), /* MX53_PAD_NANDF_CS1__EMI_NANDF_CS_1 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS1, 0x5B4, 0x23C, 1, 0x000, 0), /* MX53_PAD_NANDF_CS1__GPIO6_14 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS1, 0x5B4, 0x23C, 6, 0x858, 0), /* MX53_PAD_NANDF_CS1__MLB_MLBCLK */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS1, 0x5B4, 0x23C, 7, 0x000, 0), /* MX53_PAD_NANDF_CS1__USBPHY1_VSTATUS_5 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS2, 0x5B8, 0x240, 0, 0x000, 0), /* MX53_PAD_NANDF_CS2__EMI_NANDF_CS_2 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS2, 0x5B8, 0x240, 1, 0x000, 0), /* MX53_PAD_NANDF_CS2__GPIO6_15 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS2, 0x5B8, 0x240, 2, 0x000, 0), /* MX53_PAD_NANDF_CS2__IPU_SISG_0 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS2, 0x5B8, 0x240, 3, 0x7E4, 0), /* MX53_PAD_NANDF_CS2__ESAI1_TX0 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS2, 0x5B8, 0x240, 4, 0x000, 0), /* MX53_PAD_NANDF_CS2__EMI_WEIM_CRE */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS2, 0x5B8, 0x240, 5, 0x000, 0), /* MX53_PAD_NANDF_CS2__CCM_CSI0_MCLK */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS2, 0x5B8, 0x240, 6, 0x860, 0), /* MX53_PAD_NANDF_CS2__MLB_MLBSIG */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS2, 0x5B8, 0x240, 7, 0x000, 0), /* MX53_PAD_NANDF_CS2__USBPHY1_VSTATUS_6 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS3, 0x5BC, 0x244, 0, 0x000, 0), /* MX53_PAD_NANDF_CS3__EMI_NANDF_CS_3 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS3, 0x5BC, 0x244, 1, 0x000, 0), /* MX53_PAD_NANDF_CS3__GPIO6_16 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS3, 0x5BC, 0x244, 2, 0x000, 0), /* MX53_PAD_NANDF_CS3__IPU_SISG_1 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS3, 0x5BC, 0x244, 3, 0x7E8, 0), /* MX53_PAD_NANDF_CS3__ESAI1_TX1 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS3, 0x5BC, 0x244, 4, 0x000, 0), /* MX53_PAD_NANDF_CS3__EMI_WEIM_A_26 */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS3, 0x5BC, 0x244, 6, 0x85C, 0), /* MX53_PAD_NANDF_CS3__MLB_MLBDAT */
-	IMX_PIN_REG(MX53_PAD_NANDF_CS3, 0x5BC, 0x244, 7, 0x000, 0), /* MX53_PAD_NANDF_CS3__USBPHY1_VSTATUS_7 */
-	IMX_PIN_REG(MX53_PAD_FEC_MDIO, 0x5C4, 0x248, 0, 0x804, 1), /* MX53_PAD_FEC_MDIO__FEC_MDIO */
-	IMX_PIN_REG(MX53_PAD_FEC_MDIO, 0x5C4, 0x248, 1, 0x000, 0), /* MX53_PAD_FEC_MDIO__GPIO1_22 */
-	IMX_PIN_REG(MX53_PAD_FEC_MDIO, 0x5C4, 0x248, 2, 0x7DC, 0), /* MX53_PAD_FEC_MDIO__ESAI1_SCKR */
-	IMX_PIN_REG(MX53_PAD_FEC_MDIO, 0x5C4, 0x248, 3, 0x800, 1), /* MX53_PAD_FEC_MDIO__FEC_COL */
-	IMX_PIN_REG(MX53_PAD_FEC_MDIO, 0x5C4, 0x248, 4, 0x000, 0), /* MX53_PAD_FEC_MDIO__RTC_CE_RTC_PS2 */
-	IMX_PIN_REG(MX53_PAD_FEC_MDIO, 0x5C4, 0x248, 5, 0x000, 0), /* MX53_PAD_FEC_MDIO__SDMA_DEBUG_BUS_DEVICE_3 */
-	IMX_PIN_REG(MX53_PAD_FEC_MDIO, 0x5C4, 0x248, 6, 0x000, 0), /* MX53_PAD_FEC_MDIO__EMI_EMI_DEBUG_49 */
-	IMX_PIN_REG(MX53_PAD_FEC_REF_CLK, 0x5C8, 0x24C, 0, 0x000, 0), /* MX53_PAD_FEC_REF_CLK__FEC_TX_CLK */
-	IMX_PIN_REG(MX53_PAD_FEC_REF_CLK, 0x5C8, 0x24C, 1, 0x000, 0), /* MX53_PAD_FEC_REF_CLK__GPIO1_23 */
-	IMX_PIN_REG(MX53_PAD_FEC_REF_CLK, 0x5C8, 0x24C, 2, 0x7CC, 0), /* MX53_PAD_FEC_REF_CLK__ESAI1_FSR */
-	IMX_PIN_REG(MX53_PAD_FEC_REF_CLK, 0x5C8, 0x24C, 5, 0x000, 0), /* MX53_PAD_FEC_REF_CLK__SDMA_DEBUG_BUS_DEVICE_4 */
-	IMX_PIN_REG(MX53_PAD_FEC_REF_CLK, 0x5C8, 0x24C, 6, 0x000, 0), /* MX53_PAD_FEC_REF_CLK__EMI_EMI_DEBUG_50 */
-	IMX_PIN_REG(MX53_PAD_FEC_RX_ER, 0x5CC, 0x250, 0, 0x000, 0), /* MX53_PAD_FEC_RX_ER__FEC_RX_ER */
-	IMX_PIN_REG(MX53_PAD_FEC_RX_ER, 0x5CC, 0x250, 1, 0x000, 0), /* MX53_PAD_FEC_RX_ER__GPIO1_24 */
-	IMX_PIN_REG(MX53_PAD_FEC_RX_ER, 0x5CC, 0x250, 2, 0x7D4, 0), /* MX53_PAD_FEC_RX_ER__ESAI1_HCKR */
-	IMX_PIN_REG(MX53_PAD_FEC_RX_ER, 0x5CC, 0x250, 3, 0x808, 1), /* MX53_PAD_FEC_RX_ER__FEC_RX_CLK */
-	IMX_PIN_REG(MX53_PAD_FEC_RX_ER, 0x5CC, 0x250, 4, 0x000, 0), /* MX53_PAD_FEC_RX_ER__RTC_CE_RTC_PS3 */
-	IMX_PIN_REG(MX53_PAD_FEC_CRS_DV, 0x5D0, 0x254, 0, 0x000, 0), /* MX53_PAD_FEC_CRS_DV__FEC_RX_DV */
-	IMX_PIN_REG(MX53_PAD_FEC_CRS_DV, 0x5D0, 0x254, 1, 0x000, 0), /* MX53_PAD_FEC_CRS_DV__GPIO1_25 */
-	IMX_PIN_REG(MX53_PAD_FEC_CRS_DV, 0x5D0, 0x254, 2, 0x7E0, 0), /* MX53_PAD_FEC_CRS_DV__ESAI1_SCKT */
-	IMX_PIN_REG(MX53_PAD_FEC_RXD1, 0x5D4, 0x258, 0, 0x000, 0), /* MX53_PAD_FEC_RXD1__FEC_RDATA_1 */
-	IMX_PIN_REG(MX53_PAD_FEC_RXD1, 0x5D4, 0x258, 1, 0x000, 0), /* MX53_PAD_FEC_RXD1__GPIO1_26 */
-	IMX_PIN_REG(MX53_PAD_FEC_RXD1, 0x5D4, 0x258, 2, 0x7D0, 0), /* MX53_PAD_FEC_RXD1__ESAI1_FST */
-	IMX_PIN_REG(MX53_PAD_FEC_RXD1, 0x5D4, 0x258, 3, 0x860, 1), /* MX53_PAD_FEC_RXD1__MLB_MLBSIG */
-	IMX_PIN_REG(MX53_PAD_FEC_RXD1, 0x5D4, 0x258, 4, 0x000, 0), /* MX53_PAD_FEC_RXD1__RTC_CE_RTC_PS1 */
-	IMX_PIN_REG(MX53_PAD_FEC_RXD0, 0x5D8, 0x25C, 0, 0x000, 0), /* MX53_PAD_FEC_RXD0__FEC_RDATA_0 */
-	IMX_PIN_REG(MX53_PAD_FEC_RXD0, 0x5D8, 0x25C, 1, 0x000, 0), /* MX53_PAD_FEC_RXD0__GPIO1_27 */
-	IMX_PIN_REG(MX53_PAD_FEC_RXD0, 0x5D8, 0x25C, 2, 0x7D8, 0), /* MX53_PAD_FEC_RXD0__ESAI1_HCKT */
-	IMX_PIN_REG(MX53_PAD_FEC_RXD0, 0x5D8, 0x25C, 3, 0x000, 0), /* MX53_PAD_FEC_RXD0__OSC32k_32K_OUT */
-	IMX_PIN_REG(MX53_PAD_FEC_TX_EN, 0x5DC, 0x260, 0, 0x000, 0), /* MX53_PAD_FEC_TX_EN__FEC_TX_EN */
-	IMX_PIN_REG(MX53_PAD_FEC_TX_EN, 0x5DC, 0x260, 1, 0x000, 0), /* MX53_PAD_FEC_TX_EN__GPIO1_28 */
-	IMX_PIN_REG(MX53_PAD_FEC_TX_EN, 0x5DC, 0x260, 2, 0x7F0, 0), /* MX53_PAD_FEC_TX_EN__ESAI1_TX3_RX2 */
-	IMX_PIN_REG(MX53_PAD_FEC_TXD1, 0x5E0, 0x264, 0, 0x000, 0), /* MX53_PAD_FEC_TXD1__FEC_TDATA_1 */
-	IMX_PIN_REG(MX53_PAD_FEC_TXD1, 0x5E0, 0x264, 1, 0x000, 0), /* MX53_PAD_FEC_TXD1__GPIO1_29 */
-	IMX_PIN_REG(MX53_PAD_FEC_TXD1, 0x5E0, 0x264, 2, 0x7EC, 0), /* MX53_PAD_FEC_TXD1__ESAI1_TX2_RX3 */
-	IMX_PIN_REG(MX53_PAD_FEC_TXD1, 0x5E0, 0x264, 3, 0x858, 1), /* MX53_PAD_FEC_TXD1__MLB_MLBCLK */
-	IMX_PIN_REG(MX53_PAD_FEC_TXD1, 0x5E0, 0x264, 4, 0x000, 0), /* MX53_PAD_FEC_TXD1__RTC_CE_RTC_PRSC_CLK */
-	IMX_PIN_REG(MX53_PAD_FEC_TXD0, 0x5E4, 0x268, 0, 0x000, 0), /* MX53_PAD_FEC_TXD0__FEC_TDATA_0 */
-	IMX_PIN_REG(MX53_PAD_FEC_TXD0, 0x5E4, 0x268, 1, 0x000, 0), /* MX53_PAD_FEC_TXD0__GPIO1_30 */
-	IMX_PIN_REG(MX53_PAD_FEC_TXD0, 0x5E4, 0x268, 2, 0x7F4, 0), /* MX53_PAD_FEC_TXD0__ESAI1_TX4_RX1 */
-	IMX_PIN_REG(MX53_PAD_FEC_TXD0, 0x5E4, 0x268, 7, 0x000, 0), /* MX53_PAD_FEC_TXD0__USBPHY2_DATAOUT_0 */
-	IMX_PIN_REG(MX53_PAD_FEC_MDC, 0x5E8, 0x26C, 0, 0x000, 0), /* MX53_PAD_FEC_MDC__FEC_MDC */
-	IMX_PIN_REG(MX53_PAD_FEC_MDC, 0x5E8, 0x26C, 1, 0x000, 0), /* MX53_PAD_FEC_MDC__GPIO1_31 */
-	IMX_PIN_REG(MX53_PAD_FEC_MDC, 0x5E8, 0x26C, 2, 0x7F8, 0), /* MX53_PAD_FEC_MDC__ESAI1_TX5_RX0 */
-	IMX_PIN_REG(MX53_PAD_FEC_MDC, 0x5E8, 0x26C, 3, 0x85C, 1), /* MX53_PAD_FEC_MDC__MLB_MLBDAT */
-	IMX_PIN_REG(MX53_PAD_FEC_MDC, 0x5E8, 0x26C, 4, 0x000, 0), /* MX53_PAD_FEC_MDC__RTC_CE_RTC_ALARM1_TRIG */
-	IMX_PIN_REG(MX53_PAD_FEC_MDC, 0x5E8, 0x26C, 7, 0x000, 0), /* MX53_PAD_FEC_MDC__USBPHY2_DATAOUT_1 */
-	IMX_PIN_REG(MX53_PAD_PATA_DIOW, 0x5F0, 0x270, 0, 0x000, 0), /* MX53_PAD_PATA_DIOW__PATA_DIOW */
-	IMX_PIN_REG(MX53_PAD_PATA_DIOW, 0x5F0, 0x270, 1, 0x000, 0), /* MX53_PAD_PATA_DIOW__GPIO6_17 */
-	IMX_PIN_REG(MX53_PAD_PATA_DIOW, 0x5F0, 0x270, 3, 0x000, 0), /* MX53_PAD_PATA_DIOW__UART1_TXD_MUX */
-	IMX_PIN_REG(MX53_PAD_PATA_DIOW, 0x5F0, 0x270, 7, 0x000, 0), /* MX53_PAD_PATA_DIOW__USBPHY2_DATAOUT_2 */
-	IMX_PIN_REG(MX53_PAD_PATA_DMACK, 0x5F4, 0x274, 0, 0x000, 0), /* MX53_PAD_PATA_DMACK__PATA_DMACK */
-	IMX_PIN_REG(MX53_PAD_PATA_DMACK, 0x5F4, 0x274, 1, 0x000, 0), /* MX53_PAD_PATA_DMACK__GPIO6_18 */
-	IMX_PIN_REG(MX53_PAD_PATA_DMACK, 0x5F4, 0x274, 3, 0x878, 3), /* MX53_PAD_PATA_DMACK__UART1_RXD_MUX */
-	IMX_PIN_REG(MX53_PAD_PATA_DMACK, 0x5F4, 0x274, 7, 0x000, 0), /* MX53_PAD_PATA_DMACK__USBPHY2_DATAOUT_3 */
-	IMX_PIN_REG(MX53_PAD_PATA_DMARQ, 0x5F8, 0x278, 0, 0x000, 0), /* MX53_PAD_PATA_DMARQ__PATA_DMARQ */
-	IMX_PIN_REG(MX53_PAD_PATA_DMARQ, 0x5F8, 0x278, 1, 0x000, 0), /* MX53_PAD_PATA_DMARQ__GPIO7_0 */
-	IMX_PIN_REG(MX53_PAD_PATA_DMARQ, 0x5F8, 0x278, 3, 0x000, 0), /* MX53_PAD_PATA_DMARQ__UART2_TXD_MUX */
-	IMX_PIN_REG(MX53_PAD_PATA_DMARQ, 0x5F8, 0x278, 5, 0x000, 0), /* MX53_PAD_PATA_DMARQ__CCM_CCM_OUT_0 */
-	IMX_PIN_REG(MX53_PAD_PATA_DMARQ, 0x5F8, 0x278, 7, 0x000, 0), /* MX53_PAD_PATA_DMARQ__USBPHY2_DATAOUT_4 */
-	IMX_PIN_REG(MX53_PAD_PATA_BUFFER_EN, 0x5FC, 0x27C, 0, 0x000, 0), /* MX53_PAD_PATA_BUFFER_EN__PATA_BUFFER_EN */
-	IMX_PIN_REG(MX53_PAD_PATA_BUFFER_EN, 0x5FC, 0x27C, 1, 0x000, 0), /* MX53_PAD_PATA_BUFFER_EN__GPIO7_1 */
-	IMX_PIN_REG(MX53_PAD_PATA_BUFFER_EN, 0x5FC, 0x27C, 3, 0x880, 3), /* MX53_PAD_PATA_BUFFER_EN__UART2_RXD_MUX */
-	IMX_PIN_REG(MX53_PAD_PATA_BUFFER_EN, 0x5FC, 0x27C, 5, 0x000, 0), /* MX53_PAD_PATA_BUFFER_EN__CCM_CCM_OUT_1 */
-	IMX_PIN_REG(MX53_PAD_PATA_BUFFER_EN, 0x5FC, 0x27C, 7, 0x000, 0), /* MX53_PAD_PATA_BUFFER_EN__USBPHY2_DATAOUT_5 */
-	IMX_PIN_REG(MX53_PAD_PATA_INTRQ, 0x600, 0x280, 0, 0x000, 0), /* MX53_PAD_PATA_INTRQ__PATA_INTRQ */
-	IMX_PIN_REG(MX53_PAD_PATA_INTRQ, 0x600, 0x280, 1, 0x000, 0), /* MX53_PAD_PATA_INTRQ__GPIO7_2 */
-	IMX_PIN_REG(MX53_PAD_PATA_INTRQ, 0x600, 0x280, 3, 0x000, 0), /* MX53_PAD_PATA_INTRQ__UART2_CTS */
-	IMX_PIN_REG(MX53_PAD_PATA_INTRQ, 0x600, 0x280, 4, 0x000, 0), /* MX53_PAD_PATA_INTRQ__CAN1_TXCAN */
-	IMX_PIN_REG(MX53_PAD_PATA_INTRQ, 0x600, 0x280, 5, 0x000, 0), /* MX53_PAD_PATA_INTRQ__CCM_CCM_OUT_2 */
-	IMX_PIN_REG(MX53_PAD_PATA_INTRQ, 0x600, 0x280, 7, 0x000, 0), /* MX53_PAD_PATA_INTRQ__USBPHY2_DATAOUT_6 */
-	IMX_PIN_REG(MX53_PAD_PATA_DIOR, 0x604, 0x284, 0, 0x000, 0), /* MX53_PAD_PATA_DIOR__PATA_DIOR */
-	IMX_PIN_REG(MX53_PAD_PATA_DIOR, 0x604, 0x284, 1, 0x000, 0), /* MX53_PAD_PATA_DIOR__GPIO7_3 */
-	IMX_PIN_REG(MX53_PAD_PATA_DIOR, 0x604, 0x284, 3, 0x87C, 3), /* MX53_PAD_PATA_DIOR__UART2_RTS */
-	IMX_PIN_REG(MX53_PAD_PATA_DIOR, 0x604, 0x284, 4, 0x760, 1), /* MX53_PAD_PATA_DIOR__CAN1_RXCAN */
-	IMX_PIN_REG(MX53_PAD_PATA_DIOR, 0x604, 0x284, 7, 0x000, 0), /* MX53_PAD_PATA_DIOR__USBPHY2_DATAOUT_7 */
-	IMX_PIN_REG(MX53_PAD_PATA_RESET_B, 0x608, 0x288, 0, 0x000, 0), /* MX53_PAD_PATA_RESET_B__PATA_PATA_RESET_B */
-	IMX_PIN_REG(MX53_PAD_PATA_RESET_B, 0x608, 0x288, 1, 0x000, 0), /* MX53_PAD_PATA_RESET_B__GPIO7_4 */
-	IMX_PIN_REG(MX53_PAD_PATA_RESET_B, 0x608, 0x288, 2, 0x000, 0), /* MX53_PAD_PATA_RESET_B__ESDHC3_CMD */
-	IMX_PIN_REG(MX53_PAD_PATA_RESET_B, 0x608, 0x288, 3, 0x000, 0), /* MX53_PAD_PATA_RESET_B__UART1_CTS */
-	IMX_PIN_REG(MX53_PAD_PATA_RESET_B, 0x608, 0x288, 4, 0x000, 0), /* MX53_PAD_PATA_RESET_B__CAN2_TXCAN */
-	IMX_PIN_REG(MX53_PAD_PATA_RESET_B, 0x608, 0x288, 7, 0x000, 0), /* MX53_PAD_PATA_RESET_B__USBPHY1_DATAOUT_0 */
-	IMX_PIN_REG(MX53_PAD_PATA_IORDY, 0x60C, 0x28C, 0, 0x000, 0), /* MX53_PAD_PATA_IORDY__PATA_IORDY */
-	IMX_PIN_REG(MX53_PAD_PATA_IORDY, 0x60C, 0x28C, 1, 0x000, 0), /* MX53_PAD_PATA_IORDY__GPIO7_5 */
-	IMX_PIN_REG(MX53_PAD_PATA_IORDY, 0x60C, 0x28C, 2, 0x000, 0), /* MX53_PAD_PATA_IORDY__ESDHC3_CLK */
-	IMX_PIN_REG(MX53_PAD_PATA_IORDY, 0x60C, 0x28C, 3, 0x874, 3), /* MX53_PAD_PATA_IORDY__UART1_RTS */
-	IMX_PIN_REG(MX53_PAD_PATA_IORDY, 0x60C, 0x28C, 4, 0x764, 1), /* MX53_PAD_PATA_IORDY__CAN2_RXCAN */
-	IMX_PIN_REG(MX53_PAD_PATA_IORDY, 0x60C, 0x28C, 7, 0x000, 0), /* MX53_PAD_PATA_IORDY__USBPHY1_DATAOUT_1 */
-	IMX_PIN_REG(MX53_PAD_PATA_DA_0, 0x610, 0x290, 0, 0x000, 0), /* MX53_PAD_PATA_DA_0__PATA_DA_0 */
-	IMX_PIN_REG(MX53_PAD_PATA_DA_0, 0x610, 0x290, 1, 0x000, 0), /* MX53_PAD_PATA_DA_0__GPIO7_6 */
-	IMX_PIN_REG(MX53_PAD_PATA_DA_0, 0x610, 0x290, 2, 0x000, 0), /* MX53_PAD_PATA_DA_0__ESDHC3_RST */
-	IMX_PIN_REG(MX53_PAD_PATA_DA_0, 0x610, 0x290, 4, 0x864, 0), /* MX53_PAD_PATA_DA_0__OWIRE_LINE */
-	IMX_PIN_REG(MX53_PAD_PATA_DA_0, 0x610, 0x290, 7, 0x000, 0), /* MX53_PAD_PATA_DA_0__USBPHY1_DATAOUT_2 */
-	IMX_PIN_REG(MX53_PAD_PATA_DA_1, 0x614, 0x294, 0, 0x000, 0), /* MX53_PAD_PATA_DA_1__PATA_DA_1 */
-	IMX_PIN_REG(MX53_PAD_PATA_DA_1, 0x614, 0x294, 1, 0x000, 0), /* MX53_PAD_PATA_DA_1__GPIO7_7 */
-	IMX_PIN_REG(MX53_PAD_PATA_DA_1, 0x614, 0x294, 2, 0x000, 0), /* MX53_PAD_PATA_DA_1__ESDHC4_CMD */
-	IMX_PIN_REG(MX53_PAD_PATA_DA_1, 0x614, 0x294, 4, 0x000, 0), /* MX53_PAD_PATA_DA_1__UART3_CTS */
-	IMX_PIN_REG(MX53_PAD_PATA_DA_1, 0x614, 0x294, 7, 0x000, 0), /* MX53_PAD_PATA_DA_1__USBPHY1_DATAOUT_3 */
-	IMX_PIN_REG(MX53_PAD_PATA_DA_2, 0x618, 0x298, 0, 0x000, 0), /* MX53_PAD_PATA_DA_2__PATA_DA_2 */
-	IMX_PIN_REG(MX53_PAD_PATA_DA_2, 0x618, 0x298, 1, 0x000, 0), /* MX53_PAD_PATA_DA_2__GPIO7_8 */
-	IMX_PIN_REG(MX53_PAD_PATA_DA_2, 0x618, 0x298, 2, 0x000, 0), /* MX53_PAD_PATA_DA_2__ESDHC4_CLK */
-	IMX_PIN_REG(MX53_PAD_PATA_DA_2, 0x618, 0x298, 4, 0x884, 5), /* MX53_PAD_PATA_DA_2__UART3_RTS */
-	IMX_PIN_REG(MX53_PAD_PATA_DA_2, 0x618, 0x298, 7, 0x000, 0), /* MX53_PAD_PATA_DA_2__USBPHY1_DATAOUT_4 */
-	IMX_PIN_REG(MX53_PAD_PATA_CS_0, 0x61C, 0x29C, 0, 0x000, 0), /* MX53_PAD_PATA_CS_0__PATA_CS_0 */
-	IMX_PIN_REG(MX53_PAD_PATA_CS_0, 0x61C, 0x29C, 1, 0x000, 0), /* MX53_PAD_PATA_CS_0__GPIO7_9 */
-	IMX_PIN_REG(MX53_PAD_PATA_CS_0, 0x61C, 0x29C, 4, 0x000, 0), /* MX53_PAD_PATA_CS_0__UART3_TXD_MUX */
-	IMX_PIN_REG(MX53_PAD_PATA_CS_0, 0x61C, 0x29C, 7, 0x000, 0), /* MX53_PAD_PATA_CS_0__USBPHY1_DATAOUT_5 */
-	IMX_PIN_REG(MX53_PAD_PATA_CS_1, 0x620, 0x2A0, 0, 0x000, 0), /* MX53_PAD_PATA_CS_1__PATA_CS_1 */
-	IMX_PIN_REG(MX53_PAD_PATA_CS_1, 0x620, 0x2A0, 1, 0x000, 0), /* MX53_PAD_PATA_CS_1__GPIO7_10 */
-	IMX_PIN_REG(MX53_PAD_PATA_CS_1, 0x620, 0x2A0, 4, 0x888, 3), /* MX53_PAD_PATA_CS_1__UART3_RXD_MUX */
-	IMX_PIN_REG(MX53_PAD_PATA_CS_1, 0x620, 0x2A0, 7, 0x000, 0), /* MX53_PAD_PATA_CS_1__USBPHY1_DATAOUT_6 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA0, 0x628, 0x2A4, 0, 0x000, 0), /* MX53_PAD_PATA_DATA0__PATA_DATA_0 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA0, 0x628, 0x2A4, 1, 0x000, 0), /* MX53_PAD_PATA_DATA0__GPIO2_0 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA0, 0x628, 0x2A4, 3, 0x000, 0), /* MX53_PAD_PATA_DATA0__EMI_NANDF_D_0 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA0, 0x628, 0x2A4, 4, 0x000, 0), /* MX53_PAD_PATA_DATA0__ESDHC3_DAT4 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA0, 0x628, 0x2A4, 5, 0x000, 0), /* MX53_PAD_PATA_DATA0__GPU3d_GPU_DEBUG_OUT_0 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA0, 0x628, 0x2A4, 6, 0x000, 0), /* MX53_PAD_PATA_DATA0__IPU_DIAG_BUS_0 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA0, 0x628, 0x2A4, 7, 0x000, 0), /* MX53_PAD_PATA_DATA0__USBPHY1_DATAOUT_7 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA1, 0x62C, 0x2A8, 0, 0x000, 0), /* MX53_PAD_PATA_DATA1__PATA_DATA_1 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA1, 0x62C, 0x2A8, 1, 0x000, 0), /* MX53_PAD_PATA_DATA1__GPIO2_1 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA1, 0x62C, 0x2A8, 3, 0x000, 0), /* MX53_PAD_PATA_DATA1__EMI_NANDF_D_1 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA1, 0x62C, 0x2A8, 4, 0x000, 0), /* MX53_PAD_PATA_DATA1__ESDHC3_DAT5 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA1, 0x62C, 0x2A8, 5, 0x000, 0), /* MX53_PAD_PATA_DATA1__GPU3d_GPU_DEBUG_OUT_1 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA1, 0x62C, 0x2A8, 6, 0x000, 0), /* MX53_PAD_PATA_DATA1__IPU_DIAG_BUS_1 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA2, 0x630, 0x2AC, 0, 0x000, 0), /* MX53_PAD_PATA_DATA2__PATA_DATA_2 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA2, 0x630, 0x2AC, 1, 0x000, 0), /* MX53_PAD_PATA_DATA2__GPIO2_2 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA2, 0x630, 0x2AC, 3, 0x000, 0), /* MX53_PAD_PATA_DATA2__EMI_NANDF_D_2 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA2, 0x630, 0x2AC, 4, 0x000, 0), /* MX53_PAD_PATA_DATA2__ESDHC3_DAT6 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA2, 0x630, 0x2AC, 5, 0x000, 0), /* MX53_PAD_PATA_DATA2__GPU3d_GPU_DEBUG_OUT_2 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA2, 0x630, 0x2AC, 6, 0x000, 0), /* MX53_PAD_PATA_DATA2__IPU_DIAG_BUS_2 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA3, 0x634, 0x2B0, 0, 0x000, 0), /* MX53_PAD_PATA_DATA3__PATA_DATA_3 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA3, 0x634, 0x2B0, 1, 0x000, 0), /* MX53_PAD_PATA_DATA3__GPIO2_3 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA3, 0x634, 0x2B0, 3, 0x000, 0), /* MX53_PAD_PATA_DATA3__EMI_NANDF_D_3 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA3, 0x634, 0x2B0, 4, 0x000, 0), /* MX53_PAD_PATA_DATA3__ESDHC3_DAT7 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA3, 0x634, 0x2B0, 5, 0x000, 0), /* MX53_PAD_PATA_DATA3__GPU3d_GPU_DEBUG_OUT_3 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA3, 0x634, 0x2B0, 6, 0x000, 0), /* MX53_PAD_PATA_DATA3__IPU_DIAG_BUS_3 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA4, 0x638, 0x2B4, 0, 0x000, 0), /* MX53_PAD_PATA_DATA4__PATA_DATA_4 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA4, 0x638, 0x2B4, 1, 0x000, 0), /* MX53_PAD_PATA_DATA4__GPIO2_4 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA4, 0x638, 0x2B4, 3, 0x000, 0), /* MX53_PAD_PATA_DATA4__EMI_NANDF_D_4 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA4, 0x638, 0x2B4, 4, 0x000, 0), /* MX53_PAD_PATA_DATA4__ESDHC4_DAT4 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA4, 0x638, 0x2B4, 5, 0x000, 0), /* MX53_PAD_PATA_DATA4__GPU3d_GPU_DEBUG_OUT_4 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA4, 0x638, 0x2B4, 6, 0x000, 0), /* MX53_PAD_PATA_DATA4__IPU_DIAG_BUS_4 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA5, 0x63C, 0x2B8, 0, 0x000, 0), /* MX53_PAD_PATA_DATA5__PATA_DATA_5 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA5, 0x63C, 0x2B8, 1, 0x000, 0), /* MX53_PAD_PATA_DATA5__GPIO2_5 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA5, 0x63C, 0x2B8, 3, 0x000, 0), /* MX53_PAD_PATA_DATA5__EMI_NANDF_D_5 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA5, 0x63C, 0x2B8, 4, 0x000, 0), /* MX53_PAD_PATA_DATA5__ESDHC4_DAT5 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA5, 0x63C, 0x2B8, 5, 0x000, 0), /* MX53_PAD_PATA_DATA5__GPU3d_GPU_DEBUG_OUT_5 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA5, 0x63C, 0x2B8, 6, 0x000, 0), /* MX53_PAD_PATA_DATA5__IPU_DIAG_BUS_5 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA6, 0x640, 0x2BC, 0, 0x000, 0), /* MX53_PAD_PATA_DATA6__PATA_DATA_6 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA6, 0x640, 0x2BC, 1, 0x000, 0), /* MX53_PAD_PATA_DATA6__GPIO2_6 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA6, 0x640, 0x2BC, 3, 0x000, 0), /* MX53_PAD_PATA_DATA6__EMI_NANDF_D_6 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA6, 0x640, 0x2BC, 4, 0x000, 0), /* MX53_PAD_PATA_DATA6__ESDHC4_DAT6 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA6, 0x640, 0x2BC, 5, 0x000, 0), /* MX53_PAD_PATA_DATA6__GPU3d_GPU_DEBUG_OUT_6 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA6, 0x640, 0x2BC, 6, 0x000, 0), /* MX53_PAD_PATA_DATA6__IPU_DIAG_BUS_6 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA7, 0x644, 0x2C0, 0, 0x000, 0), /* MX53_PAD_PATA_DATA7__PATA_DATA_7 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA7, 0x644, 0x2C0, 1, 0x000, 0), /* MX53_PAD_PATA_DATA7__GPIO2_7 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA7, 0x644, 0x2C0, 3, 0x000, 0), /* MX53_PAD_PATA_DATA7__EMI_NANDF_D_7 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA7, 0x644, 0x2C0, 4, 0x000, 0), /* MX53_PAD_PATA_DATA7__ESDHC4_DAT7 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA7, 0x644, 0x2C0, 5, 0x000, 0), /* MX53_PAD_PATA_DATA7__GPU3d_GPU_DEBUG_OUT_7 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA7, 0x644, 0x2C0, 6, 0x000, 0), /* MX53_PAD_PATA_DATA7__IPU_DIAG_BUS_7 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA8, 0x648, 0x2C4, 0, 0x000, 0), /* MX53_PAD_PATA_DATA8__PATA_DATA_8 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA8, 0x648, 0x2C4, 1, 0x000, 0), /* MX53_PAD_PATA_DATA8__GPIO2_8 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA8, 0x648, 0x2C4, 2, 0x000, 0), /* MX53_PAD_PATA_DATA8__ESDHC1_DAT4 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA8, 0x648, 0x2C4, 3, 0x000, 0), /* MX53_PAD_PATA_DATA8__EMI_NANDF_D_8 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA8, 0x648, 0x2C4, 4, 0x000, 0), /* MX53_PAD_PATA_DATA8__ESDHC3_DAT0 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA8, 0x648, 0x2C4, 5, 0x000, 0), /* MX53_PAD_PATA_DATA8__GPU3d_GPU_DEBUG_OUT_8 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA8, 0x648, 0x2C4, 6, 0x000, 0), /* MX53_PAD_PATA_DATA8__IPU_DIAG_BUS_8 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA9, 0x64C, 0x2C8, 0, 0x000, 0), /* MX53_PAD_PATA_DATA9__PATA_DATA_9 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA9, 0x64C, 0x2C8, 1, 0x000, 0), /* MX53_PAD_PATA_DATA9__GPIO2_9 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA9, 0x64C, 0x2C8, 2, 0x000, 0), /* MX53_PAD_PATA_DATA9__ESDHC1_DAT5 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA9, 0x64C, 0x2C8, 3, 0x000, 0), /* MX53_PAD_PATA_DATA9__EMI_NANDF_D_9 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA9, 0x64C, 0x2C8, 4, 0x000, 0), /* MX53_PAD_PATA_DATA9__ESDHC3_DAT1 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA9, 0x64C, 0x2C8, 5, 0x000, 0), /* MX53_PAD_PATA_DATA9__GPU3d_GPU_DEBUG_OUT_9 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA9, 0x64C, 0x2C8, 6, 0x000, 0), /* MX53_PAD_PATA_DATA9__IPU_DIAG_BUS_9 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA10, 0x650, 0x2CC, 0, 0x000, 0), /* MX53_PAD_PATA_DATA10__PATA_DATA_10 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA10, 0x650, 0x2CC, 1, 0x000, 0), /* MX53_PAD_PATA_DATA10__GPIO2_10 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA10, 0x650, 0x2CC, 2, 0x000, 0), /* MX53_PAD_PATA_DATA10__ESDHC1_DAT6 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA10, 0x650, 0x2CC, 3, 0x000, 0), /* MX53_PAD_PATA_DATA10__EMI_NANDF_D_10 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA10, 0x650, 0x2CC, 4, 0x000, 0), /* MX53_PAD_PATA_DATA10__ESDHC3_DAT2 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA10, 0x650, 0x2CC, 5, 0x000, 0), /* MX53_PAD_PATA_DATA10__GPU3d_GPU_DEBUG_OUT_10 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA10, 0x650, 0x2CC, 6, 0x000, 0), /* MX53_PAD_PATA_DATA10__IPU_DIAG_BUS_10 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA11, 0x654, 0x2D0, 0, 0x000, 0), /* MX53_PAD_PATA_DATA11__PATA_DATA_11 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA11, 0x654, 0x2D0, 1, 0x000, 0), /* MX53_PAD_PATA_DATA11__GPIO2_11 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA11, 0x654, 0x2D0, 2, 0x000, 0), /* MX53_PAD_PATA_DATA11__ESDHC1_DAT7 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA11, 0x654, 0x2D0, 3, 0x000, 0), /* MX53_PAD_PATA_DATA11__EMI_NANDF_D_11 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA11, 0x654, 0x2D0, 4, 0x000, 0), /* MX53_PAD_PATA_DATA11__ESDHC3_DAT3 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA11, 0x654, 0x2D0, 5, 0x000, 0), /* MX53_PAD_PATA_DATA11__GPU3d_GPU_DEBUG_OUT_11 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA11, 0x654, 0x2D0, 6, 0x000, 0), /* MX53_PAD_PATA_DATA11__IPU_DIAG_BUS_11 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA12, 0x658, 0x2D4, 0, 0x000, 0), /* MX53_PAD_PATA_DATA12__PATA_DATA_12 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA12, 0x658, 0x2D4, 1, 0x000, 0), /* MX53_PAD_PATA_DATA12__GPIO2_12 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA12, 0x658, 0x2D4, 2, 0x000, 0), /* MX53_PAD_PATA_DATA12__ESDHC2_DAT4 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA12, 0x658, 0x2D4, 3, 0x000, 0), /* MX53_PAD_PATA_DATA12__EMI_NANDF_D_12 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA12, 0x658, 0x2D4, 4, 0x000, 0), /* MX53_PAD_PATA_DATA12__ESDHC4_DAT0 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA12, 0x658, 0x2D4, 5, 0x000, 0), /* MX53_PAD_PATA_DATA12__GPU3d_GPU_DEBUG_OUT_12 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA12, 0x658, 0x2D4, 6, 0x000, 0), /* MX53_PAD_PATA_DATA12__IPU_DIAG_BUS_12 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA13, 0x65C, 0x2D8, 0, 0x000, 0), /* MX53_PAD_PATA_DATA13__PATA_DATA_13 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA13, 0x65C, 0x2D8, 1, 0x000, 0), /* MX53_PAD_PATA_DATA13__GPIO2_13 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA13, 0x65C, 0x2D8, 2, 0x000, 0), /* MX53_PAD_PATA_DATA13__ESDHC2_DAT5 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA13, 0x65C, 0x2D8, 3, 0x000, 0), /* MX53_PAD_PATA_DATA13__EMI_NANDF_D_13 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA13, 0x65C, 0x2D8, 4, 0x000, 0), /* MX53_PAD_PATA_DATA13__ESDHC4_DAT1 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA13, 0x65C, 0x2D8, 5, 0x000, 0), /* MX53_PAD_PATA_DATA13__GPU3d_GPU_DEBUG_OUT_13 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA13, 0x65C, 0x2D8, 6, 0x000, 0), /* MX53_PAD_PATA_DATA13__IPU_DIAG_BUS_13 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA14, 0x660, 0x2DC, 0, 0x000, 0), /* MX53_PAD_PATA_DATA14__PATA_DATA_14 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA14, 0x660, 0x2DC, 1, 0x000, 0), /* MX53_PAD_PATA_DATA14__GPIO2_14 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA14, 0x660, 0x2DC, 2, 0x000, 0), /* MX53_PAD_PATA_DATA14__ESDHC2_DAT6 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA14, 0x660, 0x2DC, 3, 0x000, 0), /* MX53_PAD_PATA_DATA14__EMI_NANDF_D_14 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA14, 0x660, 0x2DC, 4, 0x000, 0), /* MX53_PAD_PATA_DATA14__ESDHC4_DAT2 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA14, 0x660, 0x2DC, 5, 0x000, 0), /* MX53_PAD_PATA_DATA14__GPU3d_GPU_DEBUG_OUT_14 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA14, 0x660, 0x2DC, 6, 0x000, 0), /* MX53_PAD_PATA_DATA14__IPU_DIAG_BUS_14 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA15, 0x664, 0x2E0, 0, 0x000, 0), /* MX53_PAD_PATA_DATA15__PATA_DATA_15 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA15, 0x664, 0x2E0, 1, 0x000, 0), /* MX53_PAD_PATA_DATA15__GPIO2_15 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA15, 0x664, 0x2E0, 2, 0x000, 0), /* MX53_PAD_PATA_DATA15__ESDHC2_DAT7 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA15, 0x664, 0x2E0, 3, 0x000, 0), /* MX53_PAD_PATA_DATA15__EMI_NANDF_D_15 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA15, 0x664, 0x2E0, 4, 0x000, 0), /* MX53_PAD_PATA_DATA15__ESDHC4_DAT3 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA15, 0x664, 0x2E0, 5, 0x000, 0), /* MX53_PAD_PATA_DATA15__GPU3d_GPU_DEBUG_OUT_15 */
-	IMX_PIN_REG(MX53_PAD_PATA_DATA15, 0x664, 0x2E0, 6, 0x000, 0), /* MX53_PAD_PATA_DATA15__IPU_DIAG_BUS_15 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA0, 0x66C, 0x2E4, 0, 0x000, 0), /* MX53_PAD_SD1_DATA0__ESDHC1_DAT0 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA0, 0x66C, 0x2E4, 1, 0x000, 0), /* MX53_PAD_SD1_DATA0__GPIO1_16 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA0, 0x66C, 0x2E4, 3, 0x000, 0), /* MX53_PAD_SD1_DATA0__GPT_CAPIN1 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA0, 0x66C, 0x2E4, 5, 0x784, 2), /* MX53_PAD_SD1_DATA0__CSPI_MISO */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA0, 0x66C, 0x2E4, 7, 0x778, 0), /* MX53_PAD_SD1_DATA0__CCM_PLL3_BYP */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA1, 0x670, 0x2E8, 0, 0x000, 0), /* MX53_PAD_SD1_DATA1__ESDHC1_DAT1 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA1, 0x670, 0x2E8, 1, 0x000, 0), /* MX53_PAD_SD1_DATA1__GPIO1_17 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA1, 0x670, 0x2E8, 3, 0x000, 0), /* MX53_PAD_SD1_DATA1__GPT_CAPIN2 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA1, 0x670, 0x2E8, 5, 0x78C, 3), /* MX53_PAD_SD1_DATA1__CSPI_SS0 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA1, 0x670, 0x2E8, 7, 0x77C, 1), /* MX53_PAD_SD1_DATA1__CCM_PLL4_BYP */
-	IMX_PIN_REG(MX53_PAD_SD1_CMD, 0x674, 0x2EC, 0, 0x000, 0), /* MX53_PAD_SD1_CMD__ESDHC1_CMD */
-	IMX_PIN_REG(MX53_PAD_SD1_CMD, 0x674, 0x2EC, 1, 0x000, 0), /* MX53_PAD_SD1_CMD__GPIO1_18 */
-	IMX_PIN_REG(MX53_PAD_SD1_CMD, 0x674, 0x2EC, 3, 0x000, 0), /* MX53_PAD_SD1_CMD__GPT_CMPOUT1 */
-	IMX_PIN_REG(MX53_PAD_SD1_CMD, 0x674, 0x2EC, 5, 0x788, 2), /* MX53_PAD_SD1_CMD__CSPI_MOSI */
-	IMX_PIN_REG(MX53_PAD_SD1_CMD, 0x674, 0x2EC, 7, 0x770, 0), /* MX53_PAD_SD1_CMD__CCM_PLL1_BYP */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA2, 0x678, 0x2F0, 0, 0x000, 0), /* MX53_PAD_SD1_DATA2__ESDHC1_DAT2 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA2, 0x678, 0x2F0, 1, 0x000, 0), /* MX53_PAD_SD1_DATA2__GPIO1_19 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA2, 0x678, 0x2F0, 2, 0x000, 0), /* MX53_PAD_SD1_DATA2__GPT_CMPOUT2 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA2, 0x678, 0x2F0, 3, 0x000, 0), /* MX53_PAD_SD1_DATA2__PWM2_PWMO */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA2, 0x678, 0x2F0, 4, 0x000, 0), /* MX53_PAD_SD1_DATA2__WDOG1_WDOG_B */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA2, 0x678, 0x2F0, 5, 0x790, 2), /* MX53_PAD_SD1_DATA2__CSPI_SS1 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA2, 0x678, 0x2F0, 6, 0x000, 0), /* MX53_PAD_SD1_DATA2__WDOG1_WDOG_RST_B_DEB */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA2, 0x678, 0x2F0, 7, 0x774, 0), /* MX53_PAD_SD1_DATA2__CCM_PLL2_BYP */
-	IMX_PIN_REG(MX53_PAD_SD1_CLK, 0x67C, 0x2F4, 0, 0x000, 0), /* MX53_PAD_SD1_CLK__ESDHC1_CLK */
-	IMX_PIN_REG(MX53_PAD_SD1_CLK, 0x67C, 0x2F4, 1, 0x000, 0), /* MX53_PAD_SD1_CLK__GPIO1_20 */
-	IMX_PIN_REG(MX53_PAD_SD1_CLK, 0x67C, 0x2F4, 2, 0x000, 0), /* MX53_PAD_SD1_CLK__OSC32k_32K_OUT */
-	IMX_PIN_REG(MX53_PAD_SD1_CLK, 0x67C, 0x2F4, 3, 0x000, 0), /* MX53_PAD_SD1_CLK__GPT_CLKIN */
-	IMX_PIN_REG(MX53_PAD_SD1_CLK, 0x67C, 0x2F4, 5, 0x780, 2), /* MX53_PAD_SD1_CLK__CSPI_SCLK */
-	IMX_PIN_REG(MX53_PAD_SD1_CLK, 0x67C, 0x2F4, 7, 0x000, 0), /* MX53_PAD_SD1_CLK__SATA_PHY_DTB_0 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA3, 0x680, 0x2F8, 0, 0x000, 0), /* MX53_PAD_SD1_DATA3__ESDHC1_DAT3 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA3, 0x680, 0x2F8, 1, 0x000, 0), /* MX53_PAD_SD1_DATA3__GPIO1_21 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA3, 0x680, 0x2F8, 2, 0x000, 0), /* MX53_PAD_SD1_DATA3__GPT_CMPOUT3 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA3, 0x680, 0x2F8, 3, 0x000, 0), /* MX53_PAD_SD1_DATA3__PWM1_PWMO */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA3, 0x680, 0x2F8, 4, 0x000, 0), /* MX53_PAD_SD1_DATA3__WDOG2_WDOG_B */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA3, 0x680, 0x2F8, 5, 0x794, 2), /* MX53_PAD_SD1_DATA3__CSPI_SS2 */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA3, 0x680, 0x2F8, 6, 0x000, 0), /* MX53_PAD_SD1_DATA3__WDOG2_WDOG_RST_B_DEB */
-	IMX_PIN_REG(MX53_PAD_SD1_DATA3, 0x680, 0x2F8, 7, 0x000, 0), /* MX53_PAD_SD1_DATA3__SATA_PHY_DTB_1 */
-	IMX_PIN_REG(MX53_PAD_SD2_CLK, 0x688, 0x2FC, 0, 0x000, 0), /* MX53_PAD_SD2_CLK__ESDHC2_CLK */
-	IMX_PIN_REG(MX53_PAD_SD2_CLK, 0x688, 0x2FC, 1, 0x000, 0), /* MX53_PAD_SD2_CLK__GPIO1_10 */
-	IMX_PIN_REG(MX53_PAD_SD2_CLK, 0x688, 0x2FC, 2, 0x840, 2), /* MX53_PAD_SD2_CLK__KPP_COL_5 */
-	IMX_PIN_REG(MX53_PAD_SD2_CLK, 0x688, 0x2FC, 3, 0x73C, 1), /* MX53_PAD_SD2_CLK__AUDMUX_AUD4_RXFS */
-	IMX_PIN_REG(MX53_PAD_SD2_CLK, 0x688, 0x2FC, 5, 0x780, 3), /* MX53_PAD_SD2_CLK__CSPI_SCLK */
-	IMX_PIN_REG(MX53_PAD_SD2_CLK, 0x688, 0x2FC, 7, 0x000, 0), /* MX53_PAD_SD2_CLK__SCC_RANDOM_V */
-	IMX_PIN_REG(MX53_PAD_SD2_CMD, 0x68C, 0x300, 0, 0x000, 0), /* MX53_PAD_SD2_CMD__ESDHC2_CMD */
-	IMX_PIN_REG(MX53_PAD_SD2_CMD, 0x68C, 0x300, 1, 0x000, 0), /* MX53_PAD_SD2_CMD__GPIO1_11 */
-	IMX_PIN_REG(MX53_PAD_SD2_CMD, 0x68C, 0x300, 2, 0x84C, 1), /* MX53_PAD_SD2_CMD__KPP_ROW_5 */
-	IMX_PIN_REG(MX53_PAD_SD2_CMD, 0x68C, 0x300, 3, 0x738, 1), /* MX53_PAD_SD2_CMD__AUDMUX_AUD4_RXC */
-	IMX_PIN_REG(MX53_PAD_SD2_CMD, 0x68C, 0x300, 5, 0x788, 3), /* MX53_PAD_SD2_CMD__CSPI_MOSI */
-	IMX_PIN_REG(MX53_PAD_SD2_CMD, 0x68C, 0x300, 7, 0x000, 0), /* MX53_PAD_SD2_CMD__SCC_RANDOM */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA3, 0x690, 0x304, 0, 0x000, 0), /* MX53_PAD_SD2_DATA3__ESDHC2_DAT3 */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA3, 0x690, 0x304, 1, 0x000, 0), /* MX53_PAD_SD2_DATA3__GPIO1_12 */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA3, 0x690, 0x304, 2, 0x844, 1), /* MX53_PAD_SD2_DATA3__KPP_COL_6 */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA3, 0x690, 0x304, 3, 0x740, 1), /* MX53_PAD_SD2_DATA3__AUDMUX_AUD4_TXC */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA3, 0x690, 0x304, 5, 0x794, 3), /* MX53_PAD_SD2_DATA3__CSPI_SS2 */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA3, 0x690, 0x304, 7, 0x000, 0), /* MX53_PAD_SD2_DATA3__SJC_DONE */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA2, 0x694, 0x308, 0, 0x000, 0), /* MX53_PAD_SD2_DATA2__ESDHC2_DAT2 */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA2, 0x694, 0x308, 1, 0x000, 0), /* MX53_PAD_SD2_DATA2__GPIO1_13 */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA2, 0x694, 0x308, 2, 0x850, 1), /* MX53_PAD_SD2_DATA2__KPP_ROW_6 */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA2, 0x694, 0x308, 3, 0x734, 1), /* MX53_PAD_SD2_DATA2__AUDMUX_AUD4_TXD */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA2, 0x694, 0x308, 5, 0x790, 3), /* MX53_PAD_SD2_DATA2__CSPI_SS1 */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA2, 0x694, 0x308, 7, 0x000, 0), /* MX53_PAD_SD2_DATA2__SJC_FAIL */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA1, 0x698, 0x30C, 0, 0x000, 0), /* MX53_PAD_SD2_DATA1__ESDHC2_DAT1 */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA1, 0x698, 0x30C, 1, 0x000, 0), /* MX53_PAD_SD2_DATA1__GPIO1_14 */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA1, 0x698, 0x30C, 2, 0x848, 1), /* MX53_PAD_SD2_DATA1__KPP_COL_7 */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA1, 0x698, 0x30C, 3, 0x744, 0), /* MX53_PAD_SD2_DATA1__AUDMUX_AUD4_TXFS */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA1, 0x698, 0x30C, 5, 0x78C, 4), /* MX53_PAD_SD2_DATA1__CSPI_SS0 */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA1, 0x698, 0x30C, 7, 0x000, 0), /* MX53_PAD_SD2_DATA1__RTIC_SEC_VIO */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA0, 0x69C, 0x310, 0, 0x000, 0), /* MX53_PAD_SD2_DATA0__ESDHC2_DAT0 */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA0, 0x69C, 0x310, 1, 0x000, 0), /* MX53_PAD_SD2_DATA0__GPIO1_15 */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA0, 0x69C, 0x310, 2, 0x854, 1), /* MX53_PAD_SD2_DATA0__KPP_ROW_7 */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA0, 0x69C, 0x310, 3, 0x730, 1), /* MX53_PAD_SD2_DATA0__AUDMUX_AUD4_RXD */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA0, 0x69C, 0x310, 5, 0x784, 3), /* MX53_PAD_SD2_DATA0__CSPI_MISO */
-	IMX_PIN_REG(MX53_PAD_SD2_DATA0, 0x69C, 0x310, 7, 0x000, 0), /* MX53_PAD_SD2_DATA0__RTIC_DONE_INT */
-	IMX_PIN_REG(MX53_PAD_GPIO_0, 0x6A4, 0x314, 0, 0x000, 0), /* MX53_PAD_GPIO_0__CCM_CLKO */
-	IMX_PIN_REG(MX53_PAD_GPIO_0, 0x6A4, 0x314, 1, 0x000, 0), /* MX53_PAD_GPIO_0__GPIO1_0 */
-	IMX_PIN_REG(MX53_PAD_GPIO_0, 0x6A4, 0x314, 2, 0x840, 3), /* MX53_PAD_GPIO_0__KPP_COL_5 */
-	IMX_PIN_REG(MX53_PAD_GPIO_0, 0x6A4, 0x314, 3, 0x000, 0), /* MX53_PAD_GPIO_0__CCM_SSI_EXT1_CLK */
-	IMX_PIN_REG(MX53_PAD_GPIO_0, 0x6A4, 0x314, 4, 0x000, 0), /* MX53_PAD_GPIO_0__EPIT1_EPITO */
-	IMX_PIN_REG(MX53_PAD_GPIO_0, 0x6A4, 0x314, 5, 0x000, 0), /* MX53_PAD_GPIO_0__SRTC_ALARM_DEB */
-	IMX_PIN_REG(MX53_PAD_GPIO_0, 0x6A4, 0x314, 6, 0x000, 0), /* MX53_PAD_GPIO_0__USBOH3_USBH1_PWR */
-	IMX_PIN_REG(MX53_PAD_GPIO_0, 0x6A4, 0x314, 7, 0x000, 0), /* MX53_PAD_GPIO_0__CSU_TD */
-	IMX_PIN_REG(MX53_PAD_GPIO_1, 0x6A8, 0x318, 0, 0x7DC, 1), /* MX53_PAD_GPIO_1__ESAI1_SCKR */
-	IMX_PIN_REG(MX53_PAD_GPIO_1, 0x6A8, 0x318, 1, 0x000, 0), /* MX53_PAD_GPIO_1__GPIO1_1 */
-	IMX_PIN_REG(MX53_PAD_GPIO_1, 0x6A8, 0x318, 2, 0x84C, 2), /* MX53_PAD_GPIO_1__KPP_ROW_5 */
-	IMX_PIN_REG(MX53_PAD_GPIO_1, 0x6A8, 0x318, 3, 0x000, 0), /* MX53_PAD_GPIO_1__CCM_SSI_EXT2_CLK */
-	IMX_PIN_REG(MX53_PAD_GPIO_1, 0x6A8, 0x318, 4, 0x000, 0), /* MX53_PAD_GPIO_1__PWM2_PWMO */
-	IMX_PIN_REG(MX53_PAD_GPIO_1, 0x6A8, 0x318, 5, 0x000, 0), /* MX53_PAD_GPIO_1__WDOG2_WDOG_B */
-	IMX_PIN_REG(MX53_PAD_GPIO_1, 0x6A8, 0x318, 6, 0x000, 0), /* MX53_PAD_GPIO_1__ESDHC1_CD */
-	IMX_PIN_REG(MX53_PAD_GPIO_1, 0x6A8, 0x318, 7, 0x000, 0), /* MX53_PAD_GPIO_1__SRC_TESTER_ACK */
-	IMX_PIN_REG(MX53_PAD_GPIO_9, 0x6AC, 0x31C, 0, 0x7CC, 1), /* MX53_PAD_GPIO_9__ESAI1_FSR */
-	IMX_PIN_REG(MX53_PAD_GPIO_9, 0x6AC, 0x31C, 1, 0x000, 0), /* MX53_PAD_GPIO_9__GPIO1_9 */
-	IMX_PIN_REG(MX53_PAD_GPIO_9, 0x6AC, 0x31C, 2, 0x844, 2), /* MX53_PAD_GPIO_9__KPP_COL_6 */
-	IMX_PIN_REG(MX53_PAD_GPIO_9, 0x6AC, 0x31C, 3, 0x000, 0), /* MX53_PAD_GPIO_9__CCM_REF_EN_B */
-	IMX_PIN_REG(MX53_PAD_GPIO_9, 0x6AC, 0x31C, 4, 0x000, 0), /* MX53_PAD_GPIO_9__PWM1_PWMO */
-	IMX_PIN_REG(MX53_PAD_GPIO_9, 0x6AC, 0x31C, 5, 0x000, 0), /* MX53_PAD_GPIO_9__WDOG1_WDOG_B */
-	IMX_PIN_REG(MX53_PAD_GPIO_9, 0x6AC, 0x31C, 6, 0x7FC, 1), /* MX53_PAD_GPIO_9__ESDHC1_WP */
-	IMX_PIN_REG(MX53_PAD_GPIO_9, 0x6AC, 0x31C, 7, 0x000, 0), /* MX53_PAD_GPIO_9__SCC_FAIL_STATE */
-	IMX_PIN_REG(MX53_PAD_GPIO_3, 0x6B0, 0x320, 0, 0x7D4, 1), /* MX53_PAD_GPIO_3__ESAI1_HCKR */
-	IMX_PIN_REG(MX53_PAD_GPIO_3, 0x6B0, 0x320, 1, 0x000, 0), /* MX53_PAD_GPIO_3__GPIO1_3 */
-	IMX_PIN_REG(MX53_PAD_GPIO_3, 0x6B0, 0x320, 2, 0x824, 1), /* MX53_PAD_GPIO_3__I2C3_SCL */
-	IMX_PIN_REG(MX53_PAD_GPIO_3, 0x6B0, 0x320, 3, 0x000, 0), /* MX53_PAD_GPIO_3__DPLLIP1_TOG_EN */
-	IMX_PIN_REG(MX53_PAD_GPIO_3, 0x6B0, 0x320, 4, 0x000, 0), /* MX53_PAD_GPIO_3__CCM_CLKO2 */
-	IMX_PIN_REG(MX53_PAD_GPIO_3, 0x6B0, 0x320, 5, 0x000, 0), /* MX53_PAD_GPIO_3__OBSERVE_MUX_OBSRV_INT_OUT0 */
-	IMX_PIN_REG(MX53_PAD_GPIO_3, 0x6B0, 0x320, 6, 0x8A0, 1), /* MX53_PAD_GPIO_3__USBOH3_USBH1_OC */
-	IMX_PIN_REG(MX53_PAD_GPIO_3, 0x6B0, 0x320, 7, 0x858, 2), /* MX53_PAD_GPIO_3__MLB_MLBCLK */
-	IMX_PIN_REG(MX53_PAD_GPIO_6, 0x6B4, 0x324, 0, 0x7E0, 1), /* MX53_PAD_GPIO_6__ESAI1_SCKT */
-	IMX_PIN_REG(MX53_PAD_GPIO_6, 0x6B4, 0x324, 1, 0x000, 0), /* MX53_PAD_GPIO_6__GPIO1_6 */
-	IMX_PIN_REG(MX53_PAD_GPIO_6, 0x6B4, 0x324, 2, 0x828, 1), /* MX53_PAD_GPIO_6__I2C3_SDA */
-	IMX_PIN_REG(MX53_PAD_GPIO_6, 0x6B4, 0x324, 3, 0x000, 0), /* MX53_PAD_GPIO_6__CCM_CCM_OUT_0 */
-	IMX_PIN_REG(MX53_PAD_GPIO_6, 0x6B4, 0x324, 4, 0x000, 0), /* MX53_PAD_GPIO_6__CSU_CSU_INT_DEB */
-	IMX_PIN_REG(MX53_PAD_GPIO_6, 0x6B4, 0x324, 5, 0x000, 0), /* MX53_PAD_GPIO_6__OBSERVE_MUX_OBSRV_INT_OUT1 */
-	IMX_PIN_REG(MX53_PAD_GPIO_6, 0x6B4, 0x324, 6, 0x000, 0), /* MX53_PAD_GPIO_6__ESDHC2_LCTL */
-	IMX_PIN_REG(MX53_PAD_GPIO_6, 0x6B4, 0x324, 7, 0x860, 2), /* MX53_PAD_GPIO_6__MLB_MLBSIG */
-	IMX_PIN_REG(MX53_PAD_GPIO_2, 0x6B8, 0x328, 0, 0x7D0, 1), /* MX53_PAD_GPIO_2__ESAI1_FST */
-	IMX_PIN_REG(MX53_PAD_GPIO_2, 0x6B8, 0x328, 1, 0x000, 0), /* MX53_PAD_GPIO_2__GPIO1_2 */
-	IMX_PIN_REG(MX53_PAD_GPIO_2, 0x6B8, 0x328, 2, 0x850, 2), /* MX53_PAD_GPIO_2__KPP_ROW_6 */
-	IMX_PIN_REG(MX53_PAD_GPIO_2, 0x6B8, 0x328, 3, 0x000, 0), /* MX53_PAD_GPIO_2__CCM_CCM_OUT_1 */
-	IMX_PIN_REG(MX53_PAD_GPIO_2, 0x6B8, 0x328, 4, 0x000, 0), /* MX53_PAD_GPIO_2__CSU_CSU_ALARM_AUT_0 */
-	IMX_PIN_REG(MX53_PAD_GPIO_2, 0x6B8, 0x328, 5, 0x000, 0), /* MX53_PAD_GPIO_2__OBSERVE_MUX_OBSRV_INT_OUT2 */
-	IMX_PIN_REG(MX53_PAD_GPIO_2, 0x6B8, 0x328, 6, 0x000, 0), /* MX53_PAD_GPIO_2__ESDHC2_WP */
-	IMX_PIN_REG(MX53_PAD_GPIO_2, 0x6B8, 0x328, 7, 0x85C, 2), /* MX53_PAD_GPIO_2__MLB_MLBDAT */
-	IMX_PIN_REG(MX53_PAD_GPIO_4, 0x6BC, 0x32C, 0, 0x7D8, 1), /* MX53_PAD_GPIO_4__ESAI1_HCKT */
-	IMX_PIN_REG(MX53_PAD_GPIO_4, 0x6BC, 0x32C, 1, 0x000, 0), /* MX53_PAD_GPIO_4__GPIO1_4 */
-	IMX_PIN_REG(MX53_PAD_GPIO_4, 0x6BC, 0x32C, 2, 0x848, 2), /* MX53_PAD_GPIO_4__KPP_COL_7 */
-	IMX_PIN_REG(MX53_PAD_GPIO_4, 0x6BC, 0x32C, 3, 0x000, 0), /* MX53_PAD_GPIO_4__CCM_CCM_OUT_2 */
-	IMX_PIN_REG(MX53_PAD_GPIO_4, 0x6BC, 0x32C, 4, 0x000, 0), /* MX53_PAD_GPIO_4__CSU_CSU_ALARM_AUT_1 */
-	IMX_PIN_REG(MX53_PAD_GPIO_4, 0x6BC, 0x32C, 5, 0x000, 0), /* MX53_PAD_GPIO_4__OBSERVE_MUX_OBSRV_INT_OUT3 */
-	IMX_PIN_REG(MX53_PAD_GPIO_4, 0x6BC, 0x32C, 6, 0x000, 0), /* MX53_PAD_GPIO_4__ESDHC2_CD */
-	IMX_PIN_REG(MX53_PAD_GPIO_4, 0x6BC, 0x32C, 7, 0x000, 0), /* MX53_PAD_GPIO_4__SCC_SEC_STATE */
-	IMX_PIN_REG(MX53_PAD_GPIO_5, 0x6C0, 0x330, 0, 0x7EC, 1), /* MX53_PAD_GPIO_5__ESAI1_TX2_RX3 */
-	IMX_PIN_REG(MX53_PAD_GPIO_5, 0x6C0, 0x330, 1, 0x000, 0), /* MX53_PAD_GPIO_5__GPIO1_5 */
-	IMX_PIN_REG(MX53_PAD_GPIO_5, 0x6C0, 0x330, 2, 0x854, 2), /* MX53_PAD_GPIO_5__KPP_ROW_7 */
-	IMX_PIN_REG(MX53_PAD_GPIO_5, 0x6C0, 0x330, 3, 0x000, 0), /* MX53_PAD_GPIO_5__CCM_CLKO */
-	IMX_PIN_REG(MX53_PAD_GPIO_5, 0x6C0, 0x330, 4, 0x000, 0), /* MX53_PAD_GPIO_5__CSU_CSU_ALARM_AUT_2 */
-	IMX_PIN_REG(MX53_PAD_GPIO_5, 0x6C0, 0x330, 5, 0x000, 0), /* MX53_PAD_GPIO_5__OBSERVE_MUX_OBSRV_INT_OUT4 */
-	IMX_PIN_REG(MX53_PAD_GPIO_5, 0x6C0, 0x330, 6, 0x824, 2), /* MX53_PAD_GPIO_5__I2C3_SCL */
-	IMX_PIN_REG(MX53_PAD_GPIO_5, 0x6C0, 0x330, 7, 0x770, 1), /* MX53_PAD_GPIO_5__CCM_PLL1_BYP */
-	IMX_PIN_REG(MX53_PAD_GPIO_7, 0x6C4, 0x334, 0, 0x7F4, 1), /* MX53_PAD_GPIO_7__ESAI1_TX4_RX1 */
-	IMX_PIN_REG(MX53_PAD_GPIO_7, 0x6C4, 0x334, 1, 0x000, 0), /* MX53_PAD_GPIO_7__GPIO1_7 */
-	IMX_PIN_REG(MX53_PAD_GPIO_7, 0x6C4, 0x334, 2, 0x000, 0), /* MX53_PAD_GPIO_7__EPIT1_EPITO */
-	IMX_PIN_REG(MX53_PAD_GPIO_7, 0x6C4, 0x334, 3, 0x000, 0), /* MX53_PAD_GPIO_7__CAN1_TXCAN */
-	IMX_PIN_REG(MX53_PAD_GPIO_7, 0x6C4, 0x334, 4, 0x000, 0), /* MX53_PAD_GPIO_7__UART2_TXD_MUX */
-	IMX_PIN_REG(MX53_PAD_GPIO_7, 0x6C4, 0x334, 5, 0x80C, 1), /* MX53_PAD_GPIO_7__FIRI_RXD */
-	IMX_PIN_REG(MX53_PAD_GPIO_7, 0x6C4, 0x334, 6, 0x000, 0), /* MX53_PAD_GPIO_7__SPDIF_PLOCK */
-	IMX_PIN_REG(MX53_PAD_GPIO_7, 0x6C4, 0x334, 7, 0x774, 1), /* MX53_PAD_GPIO_7__CCM_PLL2_BYP */
-	IMX_PIN_REG(MX53_PAD_GPIO_8, 0x6C8, 0x338, 0, 0x7F8, 1), /* MX53_PAD_GPIO_8__ESAI1_TX5_RX0 */
-	IMX_PIN_REG(MX53_PAD_GPIO_8, 0x6C8, 0x338, 1, 0x000, 0), /* MX53_PAD_GPIO_8__GPIO1_8 */
-	IMX_PIN_REG(MX53_PAD_GPIO_8, 0x6C8, 0x338, 2, 0x000, 0), /* MX53_PAD_GPIO_8__EPIT2_EPITO */
-	IMX_PIN_REG(MX53_PAD_GPIO_8, 0x6C8, 0x338, 3, 0x760, 2), /* MX53_PAD_GPIO_8__CAN1_RXCAN */
-	IMX_PIN_REG(MX53_PAD_GPIO_8, 0x6C8, 0x338, 4, 0x880, 5), /* MX53_PAD_GPIO_8__UART2_RXD_MUX */
-	IMX_PIN_REG(MX53_PAD_GPIO_8, 0x6C8, 0x338, 5, 0x000, 0), /* MX53_PAD_GPIO_8__FIRI_TXD */
-	IMX_PIN_REG(MX53_PAD_GPIO_8, 0x6C8, 0x338, 6, 0x000, 0), /* MX53_PAD_GPIO_8__SPDIF_SRCLK */
-	IMX_PIN_REG(MX53_PAD_GPIO_8, 0x6C8, 0x338, 7, 0x778, 1), /* MX53_PAD_GPIO_8__CCM_PLL3_BYP */
-	IMX_PIN_REG(MX53_PAD_GPIO_16, 0x6CC, 0x33C, 0, 0x7F0, 1), /* MX53_PAD_GPIO_16__ESAI1_TX3_RX2 */
-	IMX_PIN_REG(MX53_PAD_GPIO_16, 0x6CC, 0x33C, 1, 0x000, 0), /* MX53_PAD_GPIO_16__GPIO7_11 */
-	IMX_PIN_REG(MX53_PAD_GPIO_16, 0x6CC, 0x33C, 2, 0x000, 0), /* MX53_PAD_GPIO_16__TZIC_PWRFAIL_INT */
-	IMX_PIN_REG(MX53_PAD_GPIO_16, 0x6CC, 0x33C, 4, 0x000, 0), /* MX53_PAD_GPIO_16__RTC_CE_RTC_EXT_TRIG1 */
-	IMX_PIN_REG(MX53_PAD_GPIO_16, 0x6CC, 0x33C, 5, 0x870, 1), /* MX53_PAD_GPIO_16__SPDIF_IN1 */
-	IMX_PIN_REG(MX53_PAD_GPIO_16, 0x6CC, 0x33C, 6, 0x828, 2), /* MX53_PAD_GPIO_16__I2C3_SDA */
-	IMX_PIN_REG(MX53_PAD_GPIO_16, 0x6CC, 0x33C, 7, 0x000, 0), /* MX53_PAD_GPIO_16__SJC_DE_B */
-	IMX_PIN_REG(MX53_PAD_GPIO_17, 0x6D0, 0x340, 0, 0x7E4, 1), /* MX53_PAD_GPIO_17__ESAI1_TX0 */
-	IMX_PIN_REG(MX53_PAD_GPIO_17, 0x6D0, 0x340, 1, 0x000, 0), /* MX53_PAD_GPIO_17__GPIO7_12 */
-	IMX_PIN_REG(MX53_PAD_GPIO_17, 0x6D0, 0x340, 2, 0x868, 1), /* MX53_PAD_GPIO_17__SDMA_EXT_EVENT_0 */
-	IMX_PIN_REG(MX53_PAD_GPIO_17, 0x6D0, 0x340, 3, 0x810, 1), /* MX53_PAD_GPIO_17__GPC_PMIC_RDY */
-	IMX_PIN_REG(MX53_PAD_GPIO_17, 0x6D0, 0x340, 4, 0x000, 0), /* MX53_PAD_GPIO_17__RTC_CE_RTC_FSV_TRIG */
-	IMX_PIN_REG(MX53_PAD_GPIO_17, 0x6D0, 0x340, 5, 0x000, 0), /* MX53_PAD_GPIO_17__SPDIF_OUT1 */
-	IMX_PIN_REG(MX53_PAD_GPIO_17, 0x6D0, 0x340, 6, 0x000, 0), /* MX53_PAD_GPIO_17__IPU_SNOOP2 */
-	IMX_PIN_REG(MX53_PAD_GPIO_17, 0x6D0, 0x340, 7, 0x000, 0), /* MX53_PAD_GPIO_17__SJC_JTAG_ACT */
-	IMX_PIN_REG(MX53_PAD_GPIO_18, 0x6D4, 0x344, 0, 0x7E8, 1), /* MX53_PAD_GPIO_18__ESAI1_TX1 */
-	IMX_PIN_REG(MX53_PAD_GPIO_18, 0x6D4, 0x344, 1, 0x000, 0), /* MX53_PAD_GPIO_18__GPIO7_13 */
-	IMX_PIN_REG(MX53_PAD_GPIO_18, 0x6D4, 0x344, 2, 0x86C, 1), /* MX53_PAD_GPIO_18__SDMA_EXT_EVENT_1 */
-	IMX_PIN_REG(MX53_PAD_GPIO_18, 0x6D4, 0x344, 3, 0x864, 1), /* MX53_PAD_GPIO_18__OWIRE_LINE */
-	IMX_PIN_REG(MX53_PAD_GPIO_18, 0x6D4, 0x344, 4, 0x000, 0), /* MX53_PAD_GPIO_18__RTC_CE_RTC_ALARM2_TRIG */
-	IMX_PIN_REG(MX53_PAD_GPIO_18, 0x6D4, 0x344, 5, 0x768, 1), /* MX53_PAD_GPIO_18__CCM_ASRC_EXT_CLK */
-	IMX_PIN_REG(MX53_PAD_GPIO_18, 0x6D4, 0x344, 6, 0x000, 0), /* MX53_PAD_GPIO_18__ESDHC1_LCTL */
-	IMX_PIN_REG(MX53_PAD_GPIO_18, 0x6D4, 0x344, 7, 0x000, 0), /* MX53_PAD_GPIO_18__SRC_SYSTEM_RST */
+	MX53_PAD_RESERVE0 = 0,
+	MX53_PAD_RESERVE1 = 1,
+	MX53_PAD_RESERVE2 = 2,
+	MX53_PAD_RESERVE3 = 3,
+	MX53_PAD_RESERVE4 = 4,
+	MX53_PAD_RESERVE5 = 5,
+	MX53_PAD_RESERVE6 = 6,
+	MX53_PAD_RESERVE7 = 7,
+	MX53_PAD_GPIO_19 = 8,
+	MX53_PAD_KEY_COL0 = 9,
+	MX53_PAD_KEY_ROW0 = 10,
+	MX53_PAD_KEY_COL1 = 11,
+	MX53_PAD_KEY_ROW1 = 12,
+	MX53_PAD_KEY_COL2 = 13,
+	MX53_PAD_KEY_ROW2 = 14,
+	MX53_PAD_KEY_COL3 = 15,
+	MX53_PAD_KEY_ROW3 = 16,
+	MX53_PAD_KEY_COL4 = 17,
+	MX53_PAD_KEY_ROW4 = 18,
+	MX53_PAD_DI0_DISP_CLK = 19,
+	MX53_PAD_DI0_PIN15 = 20,
+	MX53_PAD_DI0_PIN2 = 21,
+	MX53_PAD_DI0_PIN3 = 22,
+	MX53_PAD_DI0_PIN4 = 23,
+	MX53_PAD_DISP0_DAT0 = 24,
+	MX53_PAD_DISP0_DAT1 = 25,
+	MX53_PAD_DISP0_DAT2 = 26,
+	MX53_PAD_DISP0_DAT3 = 27,
+	MX53_PAD_DISP0_DAT4 = 28,
+	MX53_PAD_DISP0_DAT5 = 29,
+	MX53_PAD_DISP0_DAT6 = 30,
+	MX53_PAD_DISP0_DAT7 = 31,
+	MX53_PAD_DISP0_DAT8 = 32,
+	MX53_PAD_DISP0_DAT9 = 33,
+	MX53_PAD_DISP0_DAT10 = 34,
+	MX53_PAD_DISP0_DAT11 = 35,
+	MX53_PAD_DISP0_DAT12 = 36,
+	MX53_PAD_DISP0_DAT13 = 37,
+	MX53_PAD_DISP0_DAT14 = 38,
+	MX53_PAD_DISP0_DAT15 = 39,
+	MX53_PAD_DISP0_DAT16 = 40,
+	MX53_PAD_DISP0_DAT17 = 41,
+	MX53_PAD_DISP0_DAT18 = 42,
+	MX53_PAD_DISP0_DAT19 = 43,
+	MX53_PAD_DISP0_DAT20 = 44,
+	MX53_PAD_DISP0_DAT21 = 45,
+	MX53_PAD_DISP0_DAT22 = 46,
+	MX53_PAD_DISP0_DAT23 = 47,
+	MX53_PAD_CSI0_PIXCLK = 48,
+	MX53_PAD_CSI0_MCLK = 49,
+	MX53_PAD_CSI0_DATA_EN = 50,
+	MX53_PAD_CSI0_VSYNC = 51,
+	MX53_PAD_CSI0_DAT4 = 52,
+	MX53_PAD_CSI0_DAT5 = 53,
+	MX53_PAD_CSI0_DAT6 = 54,
+	MX53_PAD_CSI0_DAT7 = 55,
+	MX53_PAD_CSI0_DAT8 = 56,
+	MX53_PAD_CSI0_DAT9 = 57,
+	MX53_PAD_CSI0_DAT10 = 58,
+	MX53_PAD_CSI0_DAT11 = 59,
+	MX53_PAD_CSI0_DAT12 = 60,
+	MX53_PAD_CSI0_DAT13 = 61,
+	MX53_PAD_CSI0_DAT14 = 62,
+	MX53_PAD_CSI0_DAT15 = 63,
+	MX53_PAD_CSI0_DAT16 = 64,
+	MX53_PAD_CSI0_DAT17 = 65,
+	MX53_PAD_CSI0_DAT18 = 66,
+	MX53_PAD_CSI0_DAT19 = 67,
+	MX53_PAD_EIM_A25 = 68,
+	MX53_PAD_EIM_EB2 = 69,
+	MX53_PAD_EIM_D16 = 70,
+	MX53_PAD_EIM_D17 = 71,
+	MX53_PAD_EIM_D18 = 72,
+	MX53_PAD_EIM_D19 = 73,
+	MX53_PAD_EIM_D20 = 74,
+	MX53_PAD_EIM_D21 = 75,
+	MX53_PAD_EIM_D22 = 76,
+	MX53_PAD_EIM_D23 = 77,
+	MX53_PAD_EIM_EB3 = 78,
+	MX53_PAD_EIM_D24 = 79,
+	MX53_PAD_EIM_D25 = 80,
+	MX53_PAD_EIM_D26 = 81,
+	MX53_PAD_EIM_D27 = 82,
+	MX53_PAD_EIM_D28 = 83,
+	MX53_PAD_EIM_D29 = 84,
+	MX53_PAD_EIM_D30 = 85,
+	MX53_PAD_EIM_D31 = 86,
+	MX53_PAD_EIM_A24 = 87,
+	MX53_PAD_EIM_A23 = 88,
+	MX53_PAD_EIM_A22 = 89,
+	MX53_PAD_EIM_A21 = 90,
+	MX53_PAD_EIM_A20 = 91,
+	MX53_PAD_EIM_A19 = 92,
+	MX53_PAD_EIM_A18 = 93,
+	MX53_PAD_EIM_A17 = 94,
+	MX53_PAD_EIM_A16 = 95,
+	MX53_PAD_EIM_CS0 = 96,
+	MX53_PAD_EIM_CS1 = 97,
+	MX53_PAD_EIM_OE = 98,
+	MX53_PAD_EIM_RW = 99,
+	MX53_PAD_EIM_LBA = 100,
+	MX53_PAD_EIM_EB0 = 101,
+	MX53_PAD_EIM_EB1 = 102,
+	MX53_PAD_EIM_DA0 = 103,
+	MX53_PAD_EIM_DA1 = 104,
+	MX53_PAD_EIM_DA2 = 105,
+	MX53_PAD_EIM_DA3 = 106,
+	MX53_PAD_EIM_DA4 = 107,
+	MX53_PAD_EIM_DA5 = 108,
+	MX53_PAD_EIM_DA6 = 109,
+	MX53_PAD_EIM_DA7 = 110,
+	MX53_PAD_EIM_DA8 = 111,
+	MX53_PAD_EIM_DA9 = 112,
+	MX53_PAD_EIM_DA10 = 113,
+	MX53_PAD_EIM_DA11 = 114,
+	MX53_PAD_EIM_DA12 = 115,
+	MX53_PAD_EIM_DA13 = 116,
+	MX53_PAD_EIM_DA14 = 117,
+	MX53_PAD_EIM_DA15 = 118,
+	MX53_PAD_NANDF_WE_B = 119,
+	MX53_PAD_NANDF_RE_B = 120,
+	MX53_PAD_EIM_WAIT = 121,
+	MX53_PAD_RESERVE8 = 122,
+	MX53_PAD_LVDS1_TX3_P = 123,
+	MX53_PAD_LVDS1_TX2_P = 124,
+	MX53_PAD_LVDS1_CLK_P = 125,
+	MX53_PAD_LVDS1_TX1_P = 126,
+	MX53_PAD_LVDS1_TX0_P = 127,
+	MX53_PAD_LVDS0_TX3_P = 128,
+	MX53_PAD_LVDS0_CLK_P = 129,
+	MX53_PAD_LVDS0_TX2_P = 130,
+	MX53_PAD_LVDS0_TX1_P = 131,
+	MX53_PAD_LVDS0_TX0_P = 132,
+	MX53_PAD_GPIO_10 = 133,
+	MX53_PAD_GPIO_11 = 134,
+	MX53_PAD_GPIO_12 = 135,
+	MX53_PAD_GPIO_13 = 136,
+	MX53_PAD_GPIO_14 = 137,
+	MX53_PAD_NANDF_CLE = 138,
+	MX53_PAD_NANDF_ALE = 139,
+	MX53_PAD_NANDF_WP_B = 140,
+	MX53_PAD_NANDF_RB0 = 141,
+	MX53_PAD_NANDF_CS0 = 142,
+	MX53_PAD_NANDF_CS1 = 143,
+	MX53_PAD_NANDF_CS2 = 144,
+	MX53_PAD_NANDF_CS3 = 145,
+	MX53_PAD_FEC_MDIO = 146,
+	MX53_PAD_FEC_REF_CLK = 147,
+	MX53_PAD_FEC_RX_ER = 148,
+	MX53_PAD_FEC_CRS_DV = 149,
+	MX53_PAD_FEC_RXD1 = 150,
+	MX53_PAD_FEC_RXD0 = 151,
+	MX53_PAD_FEC_TX_EN = 152,
+	MX53_PAD_FEC_TXD1 = 153,
+	MX53_PAD_FEC_TXD0 = 154,
+	MX53_PAD_FEC_MDC = 155,
+	MX53_PAD_PATA_DIOW = 156,
+	MX53_PAD_PATA_DMACK = 157,
+	MX53_PAD_PATA_DMARQ = 158,
+	MX53_PAD_PATA_BUFFER_EN = 159,
+	MX53_PAD_PATA_INTRQ = 160,
+	MX53_PAD_PATA_DIOR = 161,
+	MX53_PAD_PATA_RESET_B = 162,
+	MX53_PAD_PATA_IORDY = 163,
+	MX53_PAD_PATA_DA_0 = 164,
+	MX53_PAD_PATA_DA_1 = 165,
+	MX53_PAD_PATA_DA_2 = 166,
+	MX53_PAD_PATA_CS_0 = 167,
+	MX53_PAD_PATA_CS_1 = 168,
+	MX53_PAD_PATA_DATA0 = 169,
+	MX53_PAD_PATA_DATA1 = 170,
+	MX53_PAD_PATA_DATA2 = 171,
+	MX53_PAD_PATA_DATA3 = 172,
+	MX53_PAD_PATA_DATA4 = 173,
+	MX53_PAD_PATA_DATA5 = 174,
+	MX53_PAD_PATA_DATA6 = 175,
+	MX53_PAD_PATA_DATA7 = 176,
+	MX53_PAD_PATA_DATA8 = 177,
+	MX53_PAD_PATA_DATA9 = 178,
+	MX53_PAD_PATA_DATA10 = 179,
+	MX53_PAD_PATA_DATA11 = 180,
+	MX53_PAD_PATA_DATA12 = 181,
+	MX53_PAD_PATA_DATA13 = 182,
+	MX53_PAD_PATA_DATA14 = 183,
+	MX53_PAD_PATA_DATA15 = 184,
+	MX53_PAD_SD1_DATA0 = 185,
+	MX53_PAD_SD1_DATA1 = 186,
+	MX53_PAD_SD1_CMD = 187,
+	MX53_PAD_SD1_DATA2 = 188,
+	MX53_PAD_SD1_CLK = 189,
+	MX53_PAD_SD1_DATA3 = 190,
+	MX53_PAD_SD2_CLK = 191,
+	MX53_PAD_SD2_CMD = 192,
+	MX53_PAD_SD2_DATA3 = 193,
+	MX53_PAD_SD2_DATA2 = 194,
+	MX53_PAD_SD2_DATA1 = 195,
+	MX53_PAD_SD2_DATA0 = 196,
+	MX53_PAD_GPIO_0 = 197,
+	MX53_PAD_GPIO_1 = 198,
+	MX53_PAD_GPIO_9 = 199,
+	MX53_PAD_GPIO_3 = 200,
+	MX53_PAD_GPIO_6 = 201,
+	MX53_PAD_GPIO_2 = 202,
+	MX53_PAD_GPIO_4 = 203,
+	MX53_PAD_GPIO_5 = 204,
+	MX53_PAD_GPIO_7 = 205,
+	MX53_PAD_GPIO_8 = 206,
+	MX53_PAD_GPIO_16 = 207,
+	MX53_PAD_GPIO_17 = 208,
+	MX53_PAD_GPIO_18 = 209,
 };
 
 /* Pad names for the pinmux subsystem */
 static const struct pinctrl_pin_desc imx53_pinctrl_pads[] = {
+	IMX_PINCTRL_PIN(MX53_PAD_RESERVE0),
+	IMX_PINCTRL_PIN(MX53_PAD_RESERVE1),
+	IMX_PINCTRL_PIN(MX53_PAD_RESERVE2),
+	IMX_PINCTRL_PIN(MX53_PAD_RESERVE3),
+	IMX_PINCTRL_PIN(MX53_PAD_RESERVE4),
+	IMX_PINCTRL_PIN(MX53_PAD_RESERVE5),
+	IMX_PINCTRL_PIN(MX53_PAD_RESERVE6),
+	IMX_PINCTRL_PIN(MX53_PAD_RESERVE7),
 	IMX_PINCTRL_PIN(MX53_PAD_GPIO_19),
 	IMX_PINCTRL_PIN(MX53_PAD_KEY_COL0),
 	IMX_PINCTRL_PIN(MX53_PAD_KEY_ROW0),
@@ -1517,6 +359,7 @@ static const struct pinctrl_pin_desc imx53_pinctrl_pads[] = {
 	IMX_PINCTRL_PIN(MX53_PAD_NANDF_WE_B),
 	IMX_PINCTRL_PIN(MX53_PAD_NANDF_RE_B),
 	IMX_PINCTRL_PIN(MX53_PAD_EIM_WAIT),
+	IMX_PINCTRL_PIN(MX53_PAD_RESERVE8),
 	IMX_PINCTRL_PIN(MX53_PAD_LVDS1_TX3_P),
 	IMX_PINCTRL_PIN(MX53_PAD_LVDS1_TX2_P),
 	IMX_PINCTRL_PIN(MX53_PAD_LVDS1_CLK_P),
@@ -1609,8 +452,6 @@ static const struct pinctrl_pin_desc imx53_pinctrl_pads[] = {
 static struct imx_pinctrl_soc_info imx53_pinctrl_info = {
 	.pins = imx53_pinctrl_pads,
 	.npins = ARRAY_SIZE(imx53_pinctrl_pads),
-	.pin_regs = imx53_pin_regs,
-	.npin_regs = ARRAY_SIZE(imx53_pin_regs),
 };
 
 static struct of_device_id imx53_pinctrl_of_match[] = {
diff --git a/drivers/pinctrl/pinctrl-imx6dl.c b/drivers/pinctrl/pinctrl-imx6dl.c
new file mode 100644
index 000000000000..a76b72427936
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-imx6dl.c
@@ -0,0 +1,497 @@
+/*
+ * Copyright (C) 2013 Freescale Semiconductor, Inc.
+ *
+ * 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/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-imx.h"
+
+enum imx6dl_pads {
+	MX6DL_PAD_RESERVE0 = 0,
+	MX6DL_PAD_RESERVE1 = 1,
+	MX6DL_PAD_RESERVE2 = 2,
+	MX6DL_PAD_RESERVE3 = 3,
+	MX6DL_PAD_RESERVE4 = 4,
+	MX6DL_PAD_RESERVE5 = 5,
+	MX6DL_PAD_RESERVE6 = 6,
+	MX6DL_PAD_RESERVE7 = 7,
+	MX6DL_PAD_RESERVE8 = 8,
+	MX6DL_PAD_RESERVE9 = 9,
+	MX6DL_PAD_RESERVE10 = 10,
+	MX6DL_PAD_RESERVE11 = 11,
+	MX6DL_PAD_RESERVE12 = 12,
+	MX6DL_PAD_RESERVE13 = 13,
+	MX6DL_PAD_RESERVE14 = 14,
+	MX6DL_PAD_RESERVE15 = 15,
+	MX6DL_PAD_RESERVE16 = 16,
+	MX6DL_PAD_RESERVE17 = 17,
+	MX6DL_PAD_RESERVE18 = 18,
+	MX6DL_PAD_CSI0_DAT10 = 19,
+	MX6DL_PAD_CSI0_DAT11 = 20,
+	MX6DL_PAD_CSI0_DAT12 = 21,
+	MX6DL_PAD_CSI0_DAT13 = 22,
+	MX6DL_PAD_CSI0_DAT14 = 23,
+	MX6DL_PAD_CSI0_DAT15 = 24,
+	MX6DL_PAD_CSI0_DAT16 = 25,
+	MX6DL_PAD_CSI0_DAT17 = 26,
+	MX6DL_PAD_CSI0_DAT18 = 27,
+	MX6DL_PAD_CSI0_DAT19 = 28,
+	MX6DL_PAD_CSI0_DAT4 = 29,
+	MX6DL_PAD_CSI0_DAT5 = 30,
+	MX6DL_PAD_CSI0_DAT6 = 31,
+	MX6DL_PAD_CSI0_DAT7 = 32,
+	MX6DL_PAD_CSI0_DAT8 = 33,
+	MX6DL_PAD_CSI0_DAT9 = 34,
+	MX6DL_PAD_CSI0_DATA_EN = 35,
+	MX6DL_PAD_CSI0_MCLK = 36,
+	MX6DL_PAD_CSI0_PIXCLK = 37,
+	MX6DL_PAD_CSI0_VSYNC = 38,
+	MX6DL_PAD_DI0_DISP_CLK = 39,
+	MX6DL_PAD_DI0_PIN15 = 40,
+	MX6DL_PAD_DI0_PIN2 = 41,
+	MX6DL_PAD_DI0_PIN3 = 42,
+	MX6DL_PAD_DI0_PIN4 = 43,
+	MX6DL_PAD_DISP0_DAT0 = 44,
+	MX6DL_PAD_DISP0_DAT1 = 45,
+	MX6DL_PAD_DISP0_DAT10 = 46,
+	MX6DL_PAD_DISP0_DAT11 = 47,
+	MX6DL_PAD_DISP0_DAT12 = 48,
+	MX6DL_PAD_DISP0_DAT13 = 49,
+	MX6DL_PAD_DISP0_DAT14 = 50,
+	MX6DL_PAD_DISP0_DAT15 = 51,
+	MX6DL_PAD_DISP0_DAT16 = 52,
+	MX6DL_PAD_DISP0_DAT17 = 53,
+	MX6DL_PAD_DISP0_DAT18 = 54,
+	MX6DL_PAD_DISP0_DAT19 = 55,
+	MX6DL_PAD_DISP0_DAT2 = 56,
+	MX6DL_PAD_DISP0_DAT20 = 57,
+	MX6DL_PAD_DISP0_DAT21 = 58,
+	MX6DL_PAD_DISP0_DAT22 = 59,
+	MX6DL_PAD_DISP0_DAT23 = 60,
+	MX6DL_PAD_DISP0_DAT3 = 61,
+	MX6DL_PAD_DISP0_DAT4 = 62,
+	MX6DL_PAD_DISP0_DAT5 = 63,
+	MX6DL_PAD_DISP0_DAT6 = 64,
+	MX6DL_PAD_DISP0_DAT7 = 65,
+	MX6DL_PAD_DISP0_DAT8 = 66,
+	MX6DL_PAD_DISP0_DAT9 = 67,
+	MX6DL_PAD_EIM_A16 = 68,
+	MX6DL_PAD_EIM_A17 = 69,
+	MX6DL_PAD_EIM_A18 = 70,
+	MX6DL_PAD_EIM_A19 = 71,
+	MX6DL_PAD_EIM_A20 = 72,
+	MX6DL_PAD_EIM_A21 = 73,
+	MX6DL_PAD_EIM_A22 = 74,
+	MX6DL_PAD_EIM_A23 = 75,
+	MX6DL_PAD_EIM_A24 = 76,
+	MX6DL_PAD_EIM_A25 = 77,
+	MX6DL_PAD_EIM_BCLK = 78,
+	MX6DL_PAD_EIM_CS0 = 79,
+	MX6DL_PAD_EIM_CS1 = 80,
+	MX6DL_PAD_EIM_D16 = 81,
+	MX6DL_PAD_EIM_D17 = 82,
+	MX6DL_PAD_EIM_D18 = 83,
+	MX6DL_PAD_EIM_D19 = 84,
+	MX6DL_PAD_EIM_D20 = 85,
+	MX6DL_PAD_EIM_D21 = 86,
+	MX6DL_PAD_EIM_D22 = 87,
+	MX6DL_PAD_EIM_D23 = 88,
+	MX6DL_PAD_EIM_D24 = 89,
+	MX6DL_PAD_EIM_D25 = 90,
+	MX6DL_PAD_EIM_D26 = 91,
+	MX6DL_PAD_EIM_D27 = 92,
+	MX6DL_PAD_EIM_D28 = 93,
+	MX6DL_PAD_EIM_D29 = 94,
+	MX6DL_PAD_EIM_D30 = 95,
+	MX6DL_PAD_EIM_D31 = 96,
+	MX6DL_PAD_EIM_DA0 = 97,
+	MX6DL_PAD_EIM_DA1 = 98,
+	MX6DL_PAD_EIM_DA10 = 99,
+	MX6DL_PAD_EIM_DA11 = 100,
+	MX6DL_PAD_EIM_DA12 = 101,
+	MX6DL_PAD_EIM_DA13 = 102,
+	MX6DL_PAD_EIM_DA14 = 103,
+	MX6DL_PAD_EIM_DA15 = 104,
+	MX6DL_PAD_EIM_DA2 = 105,
+	MX6DL_PAD_EIM_DA3 = 106,
+	MX6DL_PAD_EIM_DA4 = 107,
+	MX6DL_PAD_EIM_DA5 = 108,
+	MX6DL_PAD_EIM_DA6 = 109,
+	MX6DL_PAD_EIM_DA7 = 110,
+	MX6DL_PAD_EIM_DA8 = 111,
+	MX6DL_PAD_EIM_DA9 = 112,
+	MX6DL_PAD_EIM_EB0 = 113,
+	MX6DL_PAD_EIM_EB1 = 114,
+	MX6DL_PAD_EIM_EB2 = 115,
+	MX6DL_PAD_EIM_EB3 = 116,
+	MX6DL_PAD_EIM_LBA = 117,
+	MX6DL_PAD_EIM_OE = 118,
+	MX6DL_PAD_EIM_RW = 119,
+	MX6DL_PAD_EIM_WAIT = 120,
+	MX6DL_PAD_ENET_CRS_DV = 121,
+	MX6DL_PAD_ENET_MDC = 122,
+	MX6DL_PAD_ENET_MDIO = 123,
+	MX6DL_PAD_ENET_REF_CLK = 124,
+	MX6DL_PAD_ENET_RX_ER = 125,
+	MX6DL_PAD_ENET_RXD0 = 126,
+	MX6DL_PAD_ENET_RXD1 = 127,
+	MX6DL_PAD_ENET_TX_EN = 128,
+	MX6DL_PAD_ENET_TXD0 = 129,
+	MX6DL_PAD_ENET_TXD1 = 130,
+	MX6DL_PAD_GPIO_0 = 131,
+	MX6DL_PAD_GPIO_1 = 132,
+	MX6DL_PAD_GPIO_16 = 133,
+	MX6DL_PAD_GPIO_17 = 134,
+	MX6DL_PAD_GPIO_18 = 135,
+	MX6DL_PAD_GPIO_19 = 136,
+	MX6DL_PAD_GPIO_2 = 137,
+	MX6DL_PAD_GPIO_3 = 138,
+	MX6DL_PAD_GPIO_4 = 139,
+	MX6DL_PAD_GPIO_5 = 140,
+	MX6DL_PAD_GPIO_6 = 141,
+	MX6DL_PAD_GPIO_7 = 142,
+	MX6DL_PAD_GPIO_8 = 143,
+	MX6DL_PAD_GPIO_9 = 144,
+	MX6DL_PAD_KEY_COL0 = 145,
+	MX6DL_PAD_KEY_COL1 = 146,
+	MX6DL_PAD_KEY_COL2 = 147,
+	MX6DL_PAD_KEY_COL3 = 148,
+	MX6DL_PAD_KEY_COL4 = 149,
+	MX6DL_PAD_KEY_ROW0 = 150,
+	MX6DL_PAD_KEY_ROW1 = 151,
+	MX6DL_PAD_KEY_ROW2 = 152,
+	MX6DL_PAD_KEY_ROW3 = 153,
+	MX6DL_PAD_KEY_ROW4 = 154,
+	MX6DL_PAD_NANDF_ALE = 155,
+	MX6DL_PAD_NANDF_CLE = 156,
+	MX6DL_PAD_NANDF_CS0 = 157,
+	MX6DL_PAD_NANDF_CS1 = 158,
+	MX6DL_PAD_NANDF_CS2 = 159,
+	MX6DL_PAD_NANDF_CS3 = 160,
+	MX6DL_PAD_NANDF_D0 = 161,
+	MX6DL_PAD_NANDF_D1 = 162,
+	MX6DL_PAD_NANDF_D2 = 163,
+	MX6DL_PAD_NANDF_D3 = 164,
+	MX6DL_PAD_NANDF_D4 = 165,
+	MX6DL_PAD_NANDF_D5 = 166,
+	MX6DL_PAD_NANDF_D6 = 167,
+	MX6DL_PAD_NANDF_D7 = 168,
+	MX6DL_PAD_NANDF_RB0 = 169,
+	MX6DL_PAD_NANDF_WP_B = 170,
+	MX6DL_PAD_RGMII_RD0 = 171,
+	MX6DL_PAD_RGMII_RD1 = 172,
+	MX6DL_PAD_RGMII_RD2 = 173,
+	MX6DL_PAD_RGMII_RD3 = 174,
+	MX6DL_PAD_RGMII_RX_CTL = 175,
+	MX6DL_PAD_RGMII_RXC = 176,
+	MX6DL_PAD_RGMII_TD0 = 177,
+	MX6DL_PAD_RGMII_TD1 = 178,
+	MX6DL_PAD_RGMII_TD2 = 179,
+	MX6DL_PAD_RGMII_TD3 = 180,
+	MX6DL_PAD_RGMII_TX_CTL = 181,
+	MX6DL_PAD_RGMII_TXC = 182,
+	MX6DL_PAD_SD1_CLK = 183,
+	MX6DL_PAD_SD1_CMD = 184,
+	MX6DL_PAD_SD1_DAT0 = 185,
+	MX6DL_PAD_SD1_DAT1 = 186,
+	MX6DL_PAD_SD1_DAT2 = 187,
+	MX6DL_PAD_SD1_DAT3 = 188,
+	MX6DL_PAD_SD2_CLK = 189,
+	MX6DL_PAD_SD2_CMD = 190,
+	MX6DL_PAD_SD2_DAT0 = 191,
+	MX6DL_PAD_SD2_DAT1 = 192,
+	MX6DL_PAD_SD2_DAT2 = 193,
+	MX6DL_PAD_SD2_DAT3 = 194,
+	MX6DL_PAD_SD3_CLK = 195,
+	MX6DL_PAD_SD3_CMD = 196,
+	MX6DL_PAD_SD3_DAT0 = 197,
+	MX6DL_PAD_SD3_DAT1 = 198,
+	MX6DL_PAD_SD3_DAT2 = 199,
+	MX6DL_PAD_SD3_DAT3 = 200,
+	MX6DL_PAD_SD3_DAT4 = 201,
+	MX6DL_PAD_SD3_DAT5 = 202,
+	MX6DL_PAD_SD3_DAT6 = 203,
+	MX6DL_PAD_SD3_DAT7 = 204,
+	MX6DL_PAD_SD3_RST = 205,
+	MX6DL_PAD_SD4_CLK = 206,
+	MX6DL_PAD_SD4_CMD = 207,
+	MX6DL_PAD_SD4_DAT0 = 208,
+	MX6DL_PAD_SD4_DAT1 = 209,
+	MX6DL_PAD_SD4_DAT2 = 210,
+	MX6DL_PAD_SD4_DAT3 = 211,
+	MX6DL_PAD_SD4_DAT4 = 212,
+	MX6DL_PAD_SD4_DAT5 = 213,
+	MX6DL_PAD_SD4_DAT6 = 214,
+	MX6DL_PAD_SD4_DAT7 = 215,
+};
+
+/* Pad names for the pinmux subsystem */
+static const struct pinctrl_pin_desc imx6dl_pinctrl_pads[] = {
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE4),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE5),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE6),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE7),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE8),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE9),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE10),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE11),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE12),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE13),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE14),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE15),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE16),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE17),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RESERVE18),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT10),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT11),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT12),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT13),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT14),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT15),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT16),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT17),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT18),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT19),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT4),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT5),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT6),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT7),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT8),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DAT9),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_DATA_EN),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_MCLK),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_PIXCLK),
+	IMX_PINCTRL_PIN(MX6DL_PAD_CSI0_VSYNC),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DI0_DISP_CLK),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DI0_PIN15),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DI0_PIN2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DI0_PIN3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DI0_PIN4),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT10),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT11),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT12),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT13),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT14),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT15),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT16),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT17),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT18),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT19),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT20),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT21),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT22),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT23),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT4),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT5),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT6),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT7),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT8),
+	IMX_PINCTRL_PIN(MX6DL_PAD_DISP0_DAT9),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_A16),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_A17),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_A18),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_A19),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_A20),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_A21),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_A22),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_A23),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_A24),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_A25),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_BCLK),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_CS0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_CS1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D16),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D17),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D18),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D19),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D20),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D21),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D22),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D23),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D24),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D25),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D26),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D27),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D28),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D29),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D30),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_D31),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA10),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA11),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA12),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA13),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA14),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA15),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA4),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA5),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA6),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA7),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA8),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_DA9),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_EB0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_EB1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_EB2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_EB3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_LBA),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_OE),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_RW),
+	IMX_PINCTRL_PIN(MX6DL_PAD_EIM_WAIT),
+	IMX_PINCTRL_PIN(MX6DL_PAD_ENET_CRS_DV),
+	IMX_PINCTRL_PIN(MX6DL_PAD_ENET_MDC),
+	IMX_PINCTRL_PIN(MX6DL_PAD_ENET_MDIO),
+	IMX_PINCTRL_PIN(MX6DL_PAD_ENET_REF_CLK),
+	IMX_PINCTRL_PIN(MX6DL_PAD_ENET_RX_ER),
+	IMX_PINCTRL_PIN(MX6DL_PAD_ENET_RXD0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_ENET_RXD1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_ENET_TX_EN),
+	IMX_PINCTRL_PIN(MX6DL_PAD_ENET_TXD0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_ENET_TXD1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_GPIO_0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_GPIO_1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_GPIO_16),
+	IMX_PINCTRL_PIN(MX6DL_PAD_GPIO_17),
+	IMX_PINCTRL_PIN(MX6DL_PAD_GPIO_18),
+	IMX_PINCTRL_PIN(MX6DL_PAD_GPIO_19),
+	IMX_PINCTRL_PIN(MX6DL_PAD_GPIO_2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_GPIO_3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_GPIO_4),
+	IMX_PINCTRL_PIN(MX6DL_PAD_GPIO_5),
+	IMX_PINCTRL_PIN(MX6DL_PAD_GPIO_6),
+	IMX_PINCTRL_PIN(MX6DL_PAD_GPIO_7),
+	IMX_PINCTRL_PIN(MX6DL_PAD_GPIO_8),
+	IMX_PINCTRL_PIN(MX6DL_PAD_GPIO_9),
+	IMX_PINCTRL_PIN(MX6DL_PAD_KEY_COL0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_KEY_COL1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_KEY_COL2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_KEY_COL3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_KEY_COL4),
+	IMX_PINCTRL_PIN(MX6DL_PAD_KEY_ROW0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_KEY_ROW1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_KEY_ROW2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_KEY_ROW3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_KEY_ROW4),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_ALE),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_CLE),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_CS0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_CS1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_CS2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_CS3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_D0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_D1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_D2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_D3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_D4),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_D5),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_D6),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_D7),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_RB0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_NANDF_WP_B),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RGMII_RD0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RGMII_RD1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RGMII_RD2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RGMII_RD3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RGMII_RX_CTL),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RGMII_RXC),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RGMII_TD0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RGMII_TD1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RGMII_TD2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RGMII_TD3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RGMII_TX_CTL),
+	IMX_PINCTRL_PIN(MX6DL_PAD_RGMII_TXC),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD1_CLK),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD1_CMD),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD1_DAT0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD1_DAT1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD1_DAT2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD1_DAT3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD2_CLK),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD2_CMD),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD2_DAT0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD2_DAT1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD2_DAT2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD2_DAT3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD3_CLK),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD3_CMD),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD3_DAT0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD3_DAT1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD3_DAT2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD3_DAT3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD3_DAT4),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD3_DAT5),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD3_DAT6),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD3_DAT7),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD3_RST),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD4_CLK),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD4_CMD),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD4_DAT0),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD4_DAT1),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD4_DAT2),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD4_DAT3),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD4_DAT4),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD4_DAT5),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD4_DAT6),
+	IMX_PINCTRL_PIN(MX6DL_PAD_SD4_DAT7),
+};
+
+static struct imx_pinctrl_soc_info imx6dl_pinctrl_info = {
+	.pins = imx6dl_pinctrl_pads,
+	.npins = ARRAY_SIZE(imx6dl_pinctrl_pads),
+};
+
+static struct of_device_id imx6dl_pinctrl_of_match[] = {
+	{ .compatible = "fsl,imx6dl-iomuxc", },
+	{ /* sentinel */ }
+};
+
+static int imx6dl_pinctrl_probe(struct platform_device *pdev)
+{
+	return imx_pinctrl_probe(pdev, &imx6dl_pinctrl_info);
+}
+
+static struct platform_driver imx6dl_pinctrl_driver = {
+	.driver = {
+		.name = "imx6dl-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(imx6dl_pinctrl_of_match),
+	},
+	.probe = imx6dl_pinctrl_probe,
+	.remove = imx_pinctrl_remove,
+};
+
+static int __init imx6dl_pinctrl_init(void)
+{
+	return platform_driver_register(&imx6dl_pinctrl_driver);
+}
+arch_initcall(imx6dl_pinctrl_init);
+
+static void __exit imx6dl_pinctrl_exit(void)
+{
+	platform_driver_unregister(&imx6dl_pinctrl_driver);
+}
+module_exit(imx6dl_pinctrl_exit);
+
+MODULE_AUTHOR("Shawn Guo <shawn.guo@linaro.org>");
+MODULE_DESCRIPTION("Freescale imx6dl pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-imx6q.c b/drivers/pinctrl/pinctrl-imx6q.c
index 663346bb765e..76dd9c4949f4 100644
--- a/drivers/pinctrl/pinctrl-imx6q.c
+++ b/drivers/pinctrl/pinctrl-imx6q.c
@@ -23,1939 +23,245 @@
 #include "pinctrl-imx.h"
 
 enum imx6q_pads {
-	MX6Q_PAD_SD2_DAT1 = 0,
-	MX6Q_PAD_SD2_DAT2 = 1,
-	MX6Q_PAD_SD2_DAT0 = 2,
-	MX6Q_PAD_RGMII_TXC = 3,
-	MX6Q_PAD_RGMII_TD0 = 4,
-	MX6Q_PAD_RGMII_TD1 = 5,
-	MX6Q_PAD_RGMII_TD2 = 6,
-	MX6Q_PAD_RGMII_TD3 = 7,
-	MX6Q_PAD_RGMII_RX_CTL = 8,
-	MX6Q_PAD_RGMII_RD0 = 9,
-	MX6Q_PAD_RGMII_TX_CTL = 10,
-	MX6Q_PAD_RGMII_RD1 = 11,
-	MX6Q_PAD_RGMII_RD2 = 12,
-	MX6Q_PAD_RGMII_RD3 = 13,
-	MX6Q_PAD_RGMII_RXC = 14,
-	MX6Q_PAD_EIM_A25 = 15,
-	MX6Q_PAD_EIM_EB2 = 16,
-	MX6Q_PAD_EIM_D16 = 17,
-	MX6Q_PAD_EIM_D17 = 18,
-	MX6Q_PAD_EIM_D18 = 19,
-	MX6Q_PAD_EIM_D19 = 20,
-	MX6Q_PAD_EIM_D20 = 21,
-	MX6Q_PAD_EIM_D21 = 22,
-	MX6Q_PAD_EIM_D22 = 23,
-	MX6Q_PAD_EIM_D23 = 24,
-	MX6Q_PAD_EIM_EB3 = 25,
-	MX6Q_PAD_EIM_D24 = 26,
-	MX6Q_PAD_EIM_D25 = 27,
-	MX6Q_PAD_EIM_D26 = 28,
-	MX6Q_PAD_EIM_D27 = 29,
-	MX6Q_PAD_EIM_D28 = 30,
-	MX6Q_PAD_EIM_D29 = 31,
-	MX6Q_PAD_EIM_D30 = 32,
-	MX6Q_PAD_EIM_D31 = 33,
-	MX6Q_PAD_EIM_A24 = 34,
-	MX6Q_PAD_EIM_A23 = 35,
-	MX6Q_PAD_EIM_A22 = 36,
-	MX6Q_PAD_EIM_A21 = 37,
-	MX6Q_PAD_EIM_A20 = 38,
-	MX6Q_PAD_EIM_A19 = 39,
-	MX6Q_PAD_EIM_A18 = 40,
-	MX6Q_PAD_EIM_A17 = 41,
-	MX6Q_PAD_EIM_A16 = 42,
-	MX6Q_PAD_EIM_CS0 = 43,
-	MX6Q_PAD_EIM_CS1 = 44,
-	MX6Q_PAD_EIM_OE = 45,
-	MX6Q_PAD_EIM_RW = 46,
-	MX6Q_PAD_EIM_LBA = 47,
-	MX6Q_PAD_EIM_EB0 = 48,
-	MX6Q_PAD_EIM_EB1 = 49,
-	MX6Q_PAD_EIM_DA0 = 50,
-	MX6Q_PAD_EIM_DA1 = 51,
-	MX6Q_PAD_EIM_DA2 = 52,
-	MX6Q_PAD_EIM_DA3 = 53,
-	MX6Q_PAD_EIM_DA4 = 54,
-	MX6Q_PAD_EIM_DA5 = 55,
-	MX6Q_PAD_EIM_DA6 = 56,
-	MX6Q_PAD_EIM_DA7 = 57,
-	MX6Q_PAD_EIM_DA8 = 58,
-	MX6Q_PAD_EIM_DA9 = 59,
-	MX6Q_PAD_EIM_DA10 = 60,
-	MX6Q_PAD_EIM_DA11 = 61,
-	MX6Q_PAD_EIM_DA12 = 62,
-	MX6Q_PAD_EIM_DA13 = 63,
-	MX6Q_PAD_EIM_DA14 = 64,
-	MX6Q_PAD_EIM_DA15 = 65,
-	MX6Q_PAD_EIM_WAIT = 66,
-	MX6Q_PAD_EIM_BCLK = 67,
-	MX6Q_PAD_DI0_DISP_CLK = 68,
-	MX6Q_PAD_DI0_PIN15 = 69,
-	MX6Q_PAD_DI0_PIN2 = 70,
-	MX6Q_PAD_DI0_PIN3 = 71,
-	MX6Q_PAD_DI0_PIN4 = 72,
-	MX6Q_PAD_DISP0_DAT0 = 73,
-	MX6Q_PAD_DISP0_DAT1 = 74,
-	MX6Q_PAD_DISP0_DAT2 = 75,
-	MX6Q_PAD_DISP0_DAT3 = 76,
-	MX6Q_PAD_DISP0_DAT4 = 77,
-	MX6Q_PAD_DISP0_DAT5 = 78,
-	MX6Q_PAD_DISP0_DAT6 = 79,
-	MX6Q_PAD_DISP0_DAT7 = 80,
-	MX6Q_PAD_DISP0_DAT8 = 81,
-	MX6Q_PAD_DISP0_DAT9 = 82,
-	MX6Q_PAD_DISP0_DAT10 = 83,
-	MX6Q_PAD_DISP0_DAT11 = 84,
-	MX6Q_PAD_DISP0_DAT12 = 85,
-	MX6Q_PAD_DISP0_DAT13 = 86,
-	MX6Q_PAD_DISP0_DAT14 = 87,
-	MX6Q_PAD_DISP0_DAT15 = 88,
-	MX6Q_PAD_DISP0_DAT16 = 89,
-	MX6Q_PAD_DISP0_DAT17 = 90,
-	MX6Q_PAD_DISP0_DAT18 = 91,
-	MX6Q_PAD_DISP0_DAT19 = 92,
-	MX6Q_PAD_DISP0_DAT20 = 93,
-	MX6Q_PAD_DISP0_DAT21 = 94,
-	MX6Q_PAD_DISP0_DAT22 = 95,
-	MX6Q_PAD_DISP0_DAT23 = 96,
-	MX6Q_PAD_ENET_MDIO = 97,
-	MX6Q_PAD_ENET_REF_CLK = 98,
-	MX6Q_PAD_ENET_RX_ER = 99,
-	MX6Q_PAD_ENET_CRS_DV = 100,
-	MX6Q_PAD_ENET_RXD1 = 101,
-	MX6Q_PAD_ENET_RXD0 = 102,
-	MX6Q_PAD_ENET_TX_EN = 103,
-	MX6Q_PAD_ENET_TXD1 = 104,
-	MX6Q_PAD_ENET_TXD0 = 105,
-	MX6Q_PAD_ENET_MDC = 106,
-	MX6Q_PAD_DRAM_D40 = 107,
-	MX6Q_PAD_DRAM_D41 = 108,
-	MX6Q_PAD_DRAM_D42 = 109,
-	MX6Q_PAD_DRAM_D43 = 110,
-	MX6Q_PAD_DRAM_D44 = 111,
-	MX6Q_PAD_DRAM_D45 = 112,
-	MX6Q_PAD_DRAM_D46 = 113,
-	MX6Q_PAD_DRAM_D47 = 114,
-	MX6Q_PAD_DRAM_SDQS5 = 115,
-	MX6Q_PAD_DRAM_DQM5 = 116,
-	MX6Q_PAD_DRAM_D32 = 117,
-	MX6Q_PAD_DRAM_D33 = 118,
-	MX6Q_PAD_DRAM_D34 = 119,
-	MX6Q_PAD_DRAM_D35 = 120,
-	MX6Q_PAD_DRAM_D36 = 121,
-	MX6Q_PAD_DRAM_D37 = 122,
-	MX6Q_PAD_DRAM_D38 = 123,
-	MX6Q_PAD_DRAM_D39 = 124,
-	MX6Q_PAD_DRAM_DQM4 = 125,
-	MX6Q_PAD_DRAM_SDQS4 = 126,
-	MX6Q_PAD_DRAM_D24 = 127,
-	MX6Q_PAD_DRAM_D25 = 128,
-	MX6Q_PAD_DRAM_D26 = 129,
-	MX6Q_PAD_DRAM_D27 = 130,
-	MX6Q_PAD_DRAM_D28 = 131,
-	MX6Q_PAD_DRAM_D29 = 132,
-	MX6Q_PAD_DRAM_SDQS3 = 133,
-	MX6Q_PAD_DRAM_D30 = 134,
-	MX6Q_PAD_DRAM_D31 = 135,
-	MX6Q_PAD_DRAM_DQM3 = 136,
-	MX6Q_PAD_DRAM_D16 = 137,
-	MX6Q_PAD_DRAM_D17 = 138,
-	MX6Q_PAD_DRAM_D18 = 139,
-	MX6Q_PAD_DRAM_D19 = 140,
-	MX6Q_PAD_DRAM_D20 = 141,
-	MX6Q_PAD_DRAM_D21 = 142,
-	MX6Q_PAD_DRAM_D22 = 143,
-	MX6Q_PAD_DRAM_SDQS2 = 144,
-	MX6Q_PAD_DRAM_D23 = 145,
-	MX6Q_PAD_DRAM_DQM2 = 146,
-	MX6Q_PAD_DRAM_A0 = 147,
-	MX6Q_PAD_DRAM_A1 = 148,
-	MX6Q_PAD_DRAM_A2 = 149,
-	MX6Q_PAD_DRAM_A3 = 150,
-	MX6Q_PAD_DRAM_A4 = 151,
-	MX6Q_PAD_DRAM_A5 = 152,
-	MX6Q_PAD_DRAM_A6 = 153,
-	MX6Q_PAD_DRAM_A7 = 154,
-	MX6Q_PAD_DRAM_A8 = 155,
-	MX6Q_PAD_DRAM_A9 = 156,
-	MX6Q_PAD_DRAM_A10 = 157,
-	MX6Q_PAD_DRAM_A11 = 158,
-	MX6Q_PAD_DRAM_A12 = 159,
-	MX6Q_PAD_DRAM_A13 = 160,
-	MX6Q_PAD_DRAM_A14 = 161,
-	MX6Q_PAD_DRAM_A15 = 162,
-	MX6Q_PAD_DRAM_CAS = 163,
-	MX6Q_PAD_DRAM_CS0 = 164,
-	MX6Q_PAD_DRAM_CS1 = 165,
-	MX6Q_PAD_DRAM_RAS = 166,
-	MX6Q_PAD_DRAM_RESET = 167,
-	MX6Q_PAD_DRAM_SDBA0 = 168,
-	MX6Q_PAD_DRAM_SDBA1 = 169,
-	MX6Q_PAD_DRAM_SDCLK_0 = 170,
-	MX6Q_PAD_DRAM_SDBA2 = 171,
-	MX6Q_PAD_DRAM_SDCKE0 = 172,
-	MX6Q_PAD_DRAM_SDCLK_1 = 173,
-	MX6Q_PAD_DRAM_SDCKE1 = 174,
-	MX6Q_PAD_DRAM_SDODT0 = 175,
-	MX6Q_PAD_DRAM_SDODT1 = 176,
-	MX6Q_PAD_DRAM_SDWE = 177,
-	MX6Q_PAD_DRAM_D0 = 178,
-	MX6Q_PAD_DRAM_D1 = 179,
-	MX6Q_PAD_DRAM_D2 = 180,
-	MX6Q_PAD_DRAM_D3 = 181,
-	MX6Q_PAD_DRAM_D4 = 182,
-	MX6Q_PAD_DRAM_D5 = 183,
-	MX6Q_PAD_DRAM_SDQS0 = 184,
-	MX6Q_PAD_DRAM_D6 = 185,
-	MX6Q_PAD_DRAM_D7 = 186,
-	MX6Q_PAD_DRAM_DQM0 = 187,
-	MX6Q_PAD_DRAM_D8 = 188,
-	MX6Q_PAD_DRAM_D9 = 189,
-	MX6Q_PAD_DRAM_D10 = 190,
-	MX6Q_PAD_DRAM_D11 = 191,
-	MX6Q_PAD_DRAM_D12 = 192,
-	MX6Q_PAD_DRAM_D13 = 193,
-	MX6Q_PAD_DRAM_D14 = 194,
-	MX6Q_PAD_DRAM_SDQS1 = 195,
-	MX6Q_PAD_DRAM_D15 = 196,
-	MX6Q_PAD_DRAM_DQM1 = 197,
-	MX6Q_PAD_DRAM_D48 = 198,
-	MX6Q_PAD_DRAM_D49 = 199,
-	MX6Q_PAD_DRAM_D50 = 200,
-	MX6Q_PAD_DRAM_D51 = 201,
-	MX6Q_PAD_DRAM_D52 = 202,
-	MX6Q_PAD_DRAM_D53 = 203,
-	MX6Q_PAD_DRAM_D54 = 204,
-	MX6Q_PAD_DRAM_D55 = 205,
-	MX6Q_PAD_DRAM_SDQS6 = 206,
-	MX6Q_PAD_DRAM_DQM6 = 207,
-	MX6Q_PAD_DRAM_D56 = 208,
-	MX6Q_PAD_DRAM_SDQS7 = 209,
-	MX6Q_PAD_DRAM_D57 = 210,
-	MX6Q_PAD_DRAM_D58 = 211,
-	MX6Q_PAD_DRAM_D59 = 212,
-	MX6Q_PAD_DRAM_D60 = 213,
-	MX6Q_PAD_DRAM_DQM7 = 214,
-	MX6Q_PAD_DRAM_D61 = 215,
-	MX6Q_PAD_DRAM_D62 = 216,
-	MX6Q_PAD_DRAM_D63 = 217,
-	MX6Q_PAD_KEY_COL0 = 218,
-	MX6Q_PAD_KEY_ROW0 = 219,
-	MX6Q_PAD_KEY_COL1 = 220,
-	MX6Q_PAD_KEY_ROW1 = 221,
-	MX6Q_PAD_KEY_COL2 = 222,
-	MX6Q_PAD_KEY_ROW2 = 223,
-	MX6Q_PAD_KEY_COL3 = 224,
-	MX6Q_PAD_KEY_ROW3 = 225,
-	MX6Q_PAD_KEY_COL4 = 226,
-	MX6Q_PAD_KEY_ROW4 = 227,
-	MX6Q_PAD_GPIO_0 = 228,
-	MX6Q_PAD_GPIO_1 = 229,
-	MX6Q_PAD_GPIO_9 = 230,
-	MX6Q_PAD_GPIO_3 = 231,
-	MX6Q_PAD_GPIO_6 = 232,
-	MX6Q_PAD_GPIO_2 = 233,
-	MX6Q_PAD_GPIO_4 = 234,
-	MX6Q_PAD_GPIO_5 = 235,
-	MX6Q_PAD_GPIO_7 = 236,
-	MX6Q_PAD_GPIO_8 = 237,
-	MX6Q_PAD_GPIO_16 = 238,
-	MX6Q_PAD_GPIO_17 = 239,
-	MX6Q_PAD_GPIO_18 = 240,
-	MX6Q_PAD_GPIO_19 = 241,
-	MX6Q_PAD_CSI0_PIXCLK = 242,
-	MX6Q_PAD_CSI0_MCLK = 243,
-	MX6Q_PAD_CSI0_DATA_EN = 244,
-	MX6Q_PAD_CSI0_VSYNC = 245,
-	MX6Q_PAD_CSI0_DAT4 = 246,
-	MX6Q_PAD_CSI0_DAT5 = 247,
-	MX6Q_PAD_CSI0_DAT6 = 248,
-	MX6Q_PAD_CSI0_DAT7 = 249,
-	MX6Q_PAD_CSI0_DAT8 = 250,
-	MX6Q_PAD_CSI0_DAT9 = 251,
-	MX6Q_PAD_CSI0_DAT10 = 252,
-	MX6Q_PAD_CSI0_DAT11 = 253,
-	MX6Q_PAD_CSI0_DAT12 = 254,
-	MX6Q_PAD_CSI0_DAT13 = 255,
-	MX6Q_PAD_CSI0_DAT14 = 256,
-	MX6Q_PAD_CSI0_DAT15 = 257,
-	MX6Q_PAD_CSI0_DAT16 = 258,
-	MX6Q_PAD_CSI0_DAT17 = 259,
-	MX6Q_PAD_CSI0_DAT18 = 260,
-	MX6Q_PAD_CSI0_DAT19 = 261,
-	MX6Q_PAD_JTAG_TMS = 262,
-	MX6Q_PAD_JTAG_MOD = 263,
-	MX6Q_PAD_JTAG_TRSTB = 264,
-	MX6Q_PAD_JTAG_TDI = 265,
-	MX6Q_PAD_JTAG_TCK = 266,
-	MX6Q_PAD_JTAG_TDO = 267,
-	MX6Q_PAD_LVDS1_TX3_P = 268,
-	MX6Q_PAD_LVDS1_TX2_P = 269,
-	MX6Q_PAD_LVDS1_CLK_P = 270,
-	MX6Q_PAD_LVDS1_TX1_P = 271,
-	MX6Q_PAD_LVDS1_TX0_P = 272,
-	MX6Q_PAD_LVDS0_TX3_P = 273,
-	MX6Q_PAD_LVDS0_CLK_P = 274,
-	MX6Q_PAD_LVDS0_TX2_P = 275,
-	MX6Q_PAD_LVDS0_TX1_P = 276,
-	MX6Q_PAD_LVDS0_TX0_P = 277,
-	MX6Q_PAD_TAMPER = 278,
-	MX6Q_PAD_PMIC_ON_REQ = 279,
-	MX6Q_PAD_PMIC_STBY_REQ = 280,
-	MX6Q_PAD_POR_B = 281,
-	MX6Q_PAD_BOOT_MODE1 = 282,
-	MX6Q_PAD_RESET_IN_B = 283,
-	MX6Q_PAD_BOOT_MODE0 = 284,
-	MX6Q_PAD_TEST_MODE = 285,
-	MX6Q_PAD_SD3_DAT7 = 286,
-	MX6Q_PAD_SD3_DAT6 = 287,
-	MX6Q_PAD_SD3_DAT5 = 288,
-	MX6Q_PAD_SD3_DAT4 = 289,
-	MX6Q_PAD_SD3_CMD = 290,
-	MX6Q_PAD_SD3_CLK = 291,
-	MX6Q_PAD_SD3_DAT0 = 292,
-	MX6Q_PAD_SD3_DAT1 = 293,
-	MX6Q_PAD_SD3_DAT2 = 294,
-	MX6Q_PAD_SD3_DAT3 = 295,
-	MX6Q_PAD_SD3_RST = 296,
-	MX6Q_PAD_NANDF_CLE = 297,
-	MX6Q_PAD_NANDF_ALE = 298,
-	MX6Q_PAD_NANDF_WP_B = 299,
-	MX6Q_PAD_NANDF_RB0 = 300,
-	MX6Q_PAD_NANDF_CS0 = 301,
-	MX6Q_PAD_NANDF_CS1 = 302,
-	MX6Q_PAD_NANDF_CS2 = 303,
-	MX6Q_PAD_NANDF_CS3 = 304,
-	MX6Q_PAD_SD4_CMD = 305,
-	MX6Q_PAD_SD4_CLK = 306,
-	MX6Q_PAD_NANDF_D0 = 307,
-	MX6Q_PAD_NANDF_D1 = 308,
-	MX6Q_PAD_NANDF_D2 = 309,
-	MX6Q_PAD_NANDF_D3 = 310,
-	MX6Q_PAD_NANDF_D4 = 311,
-	MX6Q_PAD_NANDF_D5 = 312,
-	MX6Q_PAD_NANDF_D6 = 313,
-	MX6Q_PAD_NANDF_D7 = 314,
-	MX6Q_PAD_SD4_DAT0 = 315,
-	MX6Q_PAD_SD4_DAT1 = 316,
-	MX6Q_PAD_SD4_DAT2 = 317,
-	MX6Q_PAD_SD4_DAT3 = 318,
-	MX6Q_PAD_SD4_DAT4 = 319,
-	MX6Q_PAD_SD4_DAT5 = 320,
-	MX6Q_PAD_SD4_DAT6 = 321,
-	MX6Q_PAD_SD4_DAT7 = 322,
-	MX6Q_PAD_SD1_DAT1 = 323,
-	MX6Q_PAD_SD1_DAT0 = 324,
-	MX6Q_PAD_SD1_DAT3 = 325,
-	MX6Q_PAD_SD1_CMD = 326,
-	MX6Q_PAD_SD1_DAT2 = 327,
-	MX6Q_PAD_SD1_CLK = 328,
-	MX6Q_PAD_SD2_CLK = 329,
-	MX6Q_PAD_SD2_CMD = 330,
-	MX6Q_PAD_SD2_DAT3 = 331,
-};
-
-/* imx6q register maps */
-static struct imx_pin_reg imx6q_pin_regs[] = {
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT1, 0x0360, 0x004C, 0, 0x0000, 0), /* MX6Q_PAD_SD2_DAT1__USDHC2_DAT1 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT1, 0x0360, 0x004C, 1, 0x0834, 0), /* MX6Q_PAD_SD2_DAT1__ECSPI5_SS0 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT1, 0x0360, 0x004C, 2, 0x0000, 0), /* MX6Q_PAD_SD2_DAT1__WEIM_WEIM_CS_2 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT1, 0x0360, 0x004C, 3, 0x07C8, 0), /* MX6Q_PAD_SD2_DAT1__AUDMUX_AUD4_TXFS */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT1, 0x0360, 0x004C, 4, 0x08F0, 0), /* MX6Q_PAD_SD2_DAT1__KPP_COL_7 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT1, 0x0360, 0x004C, 5, 0x0000, 0), /* MX6Q_PAD_SD2_DAT1__GPIO_1_14 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT1, 0x0360, 0x004C, 6, 0x0000, 0), /* MX6Q_PAD_SD2_DAT1__CCM_WAIT */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT1, 0x0360, 0x004C, 7, 0x0000, 0), /* MX6Q_PAD_SD2_DAT1__ANATOP_TESTO_0 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT2, 0x0364, 0x0050, 0, 0x0000, 0), /* MX6Q_PAD_SD2_DAT2__USDHC2_DAT2 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT2, 0x0364, 0x0050, 1, 0x0838, 0), /* MX6Q_PAD_SD2_DAT2__ECSPI5_SS1 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT2, 0x0364, 0x0050, 2, 0x0000, 0), /* MX6Q_PAD_SD2_DAT2__WEIM_WEIM_CS_3 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT2, 0x0364, 0x0050, 3, 0x07B8, 0), /* MX6Q_PAD_SD2_DAT2__AUDMUX_AUD4_TXD */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT2, 0x0364, 0x0050, 4, 0x08F8, 0), /* MX6Q_PAD_SD2_DAT2__KPP_ROW_6 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT2, 0x0364, 0x0050, 5, 0x0000, 0), /* MX6Q_PAD_SD2_DAT2__GPIO_1_13 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT2, 0x0364, 0x0050, 6, 0x0000, 0), /* MX6Q_PAD_SD2_DAT2__CCM_STOP */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT2, 0x0364, 0x0050, 7, 0x0000, 0), /* MX6Q_PAD_SD2_DAT2__ANATOP_TESTO_1 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT0, 0x0368, 0x0054, 0, 0x0000, 0), /* MX6Q_PAD_SD2_DAT0__USDHC2_DAT0 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT0, 0x0368, 0x0054, 1, 0x082C, 0), /* MX6Q_PAD_SD2_DAT0__ECSPI5_MISO */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT0, 0x0368, 0x0054, 3, 0x07B4, 0), /* MX6Q_PAD_SD2_DAT0__AUDMUX_AUD4_RXD */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT0, 0x0368, 0x0054, 4, 0x08FC, 0), /* MX6Q_PAD_SD2_DAT0__KPP_ROW_7 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT0, 0x0368, 0x0054, 5, 0x0000, 0), /* MX6Q_PAD_SD2_DAT0__GPIO_1_15 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT0, 0x0368, 0x0054, 6, 0x0000, 0), /* MX6Q_PAD_SD2_DAT0__DCIC2_DCIC_OUT */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT0, 0x0368, 0x0054, 7, 0x0000, 0), /* MX6Q_PAD_SD2_DAT0__TESTO_2 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TXC, 0x036C, 0x0058, 0, 0x0000, 0), /* MX6Q_PAD_RGMII_TXC__USBOH3_H2_DATA */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TXC, 0x036C, 0x0058, 1, 0x0000, 0), /* MX6Q_PAD_RGMII_TXC__ENET_RGMII_TXC */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TXC, 0x036C, 0x0058, 2, 0x0918, 0), /* MX6Q_PAD_RGMII_TXC__SPDIF_SPDIF_EXTCLK */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TXC, 0x036C, 0x0058, 5, 0x0000, 0), /* MX6Q_PAD_RGMII_TXC__GPIO_6_19 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TXC, 0x036C, 0x0058, 6, 0x0000, 0), /* MX6Q_PAD_RGMII_TXC__MIPI_CORE_DPHY_IN_0 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TXC, 0x036C, 0x0058, 7, 0x0000, 0), /* MX6Q_PAD_RGMII_TXC__ANATOP_24M_OUT */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD0, 0x0370, 0x005C, 0, 0x0000, 0), /* MX6Q_PAD_RGMII_TD0__MIPI_HSI_CRL_TX_RDY */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD0, 0x0370, 0x005C, 1, 0x0000, 0), /* MX6Q_PAD_RGMII_TD0__ENET_RGMII_TD0 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD0, 0x0370, 0x005C, 5, 0x0000, 0), /* MX6Q_PAD_RGMII_TD0__GPIO_6_20 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD0, 0x0370, 0x005C, 6, 0x0000, 0), /* MX6Q_PAD_RGMII_TD0__MIPI_CORE_DPHY_IN_1 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD1, 0x0374, 0x0060, 0, 0x0000, 0), /* MX6Q_PAD_RGMII_TD1__MIPI_HSI_CRL_RX_FLG */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD1, 0x0374, 0x0060, 1, 0x0000, 0), /* MX6Q_PAD_RGMII_TD1__ENET_RGMII_TD1 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD1, 0x0374, 0x0060, 5, 0x0000, 0), /* MX6Q_PAD_RGMII_TD1__GPIO_6_21 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD1, 0x0374, 0x0060, 6, 0x0000, 0), /* MX6Q_PAD_RGMII_TD1__MIPI_CORE_DPHY_IN_2 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD1, 0x0374, 0x0060, 7, 0x0000, 0), /* MX6Q_PAD_RGMII_TD1__CCM_PLL3_BYP */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD2, 0x0378, 0x0064, 0, 0x0000, 0), /* MX6Q_PAD_RGMII_TD2__MIPI_HSI_CRL_RX_DTA */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD2, 0x0378, 0x0064, 1, 0x0000, 0), /* MX6Q_PAD_RGMII_TD2__ENET_RGMII_TD2 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD2, 0x0378, 0x0064, 5, 0x0000, 0), /* MX6Q_PAD_RGMII_TD2__GPIO_6_22 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD2, 0x0378, 0x0064, 6, 0x0000, 0), /* MX6Q_PAD_RGMII_TD2__MIPI_CORE_DPHY_IN_3 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD2, 0x0378, 0x0064, 7, 0x0000, 0), /* MX6Q_PAD_RGMII_TD2__CCM_PLL2_BYP */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD3, 0x037C, 0x0068, 0, 0x0000, 0), /* MX6Q_PAD_RGMII_TD3__MIPI_HSI_CRL_RX_WAK */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD3, 0x037C, 0x0068, 1, 0x0000, 0), /* MX6Q_PAD_RGMII_TD3__ENET_RGMII_TD3 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD3, 0x037C, 0x0068, 5, 0x0000, 0), /* MX6Q_PAD_RGMII_TD3__GPIO_6_23 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TD3, 0x037C, 0x0068, 6, 0x0000, 0), /* MX6Q_PAD_RGMII_TD3__MIPI_CORE_DPHY_IN_4 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RX_CTL, 0x0380, 0x006C, 0, 0x0000, 0), /* MX6Q_PAD_RGMII_RX_CTL__USBOH3_H3_DATA */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RX_CTL, 0x0380, 0x006C, 1, 0x0858, 0), /* MX6Q_PAD_RGMII_RX_CTL__RGMII_RX_CTL */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RX_CTL, 0x0380, 0x006C, 5, 0x0000, 0), /* MX6Q_PAD_RGMII_RX_CTL__GPIO_6_24 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RX_CTL, 0x0380, 0x006C, 6, 0x0000, 0), /* MX6Q_PAD_RGMII_RX_CTL__MIPI_DPHY_IN_5 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD0, 0x0384, 0x0070, 0, 0x0000, 0), /* MX6Q_PAD_RGMII_RD0__MIPI_HSI_CRL_RX_RDY */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD0, 0x0384, 0x0070, 1, 0x0848, 0), /* MX6Q_PAD_RGMII_RD0__ENET_RGMII_RD0 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD0, 0x0384, 0x0070, 5, 0x0000, 0), /* MX6Q_PAD_RGMII_RD0__GPIO_6_25 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD0, 0x0384, 0x0070, 6, 0x0000, 0), /* MX6Q_PAD_RGMII_RD0__MIPI_CORE_DPHY_IN_6 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TX_CTL, 0x0388, 0x0074, 0, 0x0000, 0), /* MX6Q_PAD_RGMII_TX_CTL__USBOH3_H2_STROBE */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TX_CTL, 0x0388, 0x0074, 1, 0x0000, 0), /* MX6Q_PAD_RGMII_TX_CTL__RGMII_TX_CTL */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TX_CTL, 0x0388, 0x0074, 5, 0x0000, 0), /* MX6Q_PAD_RGMII_TX_CTL__GPIO_6_26 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TX_CTL, 0x0388, 0x0074, 6, 0x0000, 0), /* MX6Q_PAD_RGMII_TX_CTL__CORE_DPHY_IN_7 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_TX_CTL, 0x0388, 0x0074, 7, 0x083C, 0), /* MX6Q_PAD_RGMII_TX_CTL__ANATOP_REF_OUT */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD1, 0x038C, 0x0078, 0, 0x0000, 0), /* MX6Q_PAD_RGMII_RD1__MIPI_HSI_CTRL_TX_FL */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD1, 0x038C, 0x0078, 1, 0x084C, 0), /* MX6Q_PAD_RGMII_RD1__ENET_RGMII_RD1 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD1, 0x038C, 0x0078, 5, 0x0000, 0), /* MX6Q_PAD_RGMII_RD1__GPIO_6_27 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD1, 0x038C, 0x0078, 6, 0x0000, 0), /* MX6Q_PAD_RGMII_RD1__CORE_DPHY_TEST_IN_8 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD1, 0x038C, 0x0078, 7, 0x0000, 0), /* MX6Q_PAD_RGMII_RD1__SJC_FAIL */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD2, 0x0390, 0x007C, 0, 0x0000, 0), /* MX6Q_PAD_RGMII_RD2__MIPI_HSI_CRL_TX_DTA */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD2, 0x0390, 0x007C, 1, 0x0850, 0), /* MX6Q_PAD_RGMII_RD2__ENET_RGMII_RD2 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD2, 0x0390, 0x007C, 5, 0x0000, 0), /* MX6Q_PAD_RGMII_RD2__GPIO_6_28 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD2, 0x0390, 0x007C, 6, 0x0000, 0), /* MX6Q_PAD_RGMII_RD2__MIPI_CORE_DPHY_IN_9 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD3, 0x0394, 0x0080, 0, 0x0000, 0), /* MX6Q_PAD_RGMII_RD3__MIPI_HSI_CRL_TX_WAK */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD3, 0x0394, 0x0080, 1, 0x0854, 0), /* MX6Q_PAD_RGMII_RD3__ENET_RGMII_RD3 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD3, 0x0394, 0x0080, 5, 0x0000, 0), /* MX6Q_PAD_RGMII_RD3__GPIO_6_29 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RD3, 0x0394, 0x0080, 6, 0x0000, 0), /* MX6Q_PAD_RGMII_RD3__MIPI_CORE_DPHY_IN10 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RXC, 0x0398, 0x0084, 0, 0x0000, 0), /* MX6Q_PAD_RGMII_RXC__USBOH3_H3_STROBE */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RXC, 0x0398, 0x0084, 1, 0x0844, 0), /* MX6Q_PAD_RGMII_RXC__ENET_RGMII_RXC */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RXC, 0x0398, 0x0084, 5, 0x0000, 0), /* MX6Q_PAD_RGMII_RXC__GPIO_6_30 */
-	IMX_PIN_REG(MX6Q_PAD_RGMII_RXC, 0x0398, 0x0084, 6, 0x0000, 0), /* MX6Q_PAD_RGMII_RXC__MIPI_CORE_DPHY_IN11 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A25, 0x039C, 0x0088, 0, 0x0000, 0), /* MX6Q_PAD_EIM_A25__WEIM_WEIM_A_25 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A25, 0x039C, 0x0088, 1, 0x0000, 0), /* MX6Q_PAD_EIM_A25__ECSPI4_SS1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A25, 0x039C, 0x0088, 2, 0x0000, 0), /* MX6Q_PAD_EIM_A25__ECSPI2_RDY */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A25, 0x039C, 0x0088, 3, 0x0000, 0), /* MX6Q_PAD_EIM_A25__IPU1_DI1_PIN12 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A25, 0x039C, 0x0088, 4, 0x0000, 0), /* MX6Q_PAD_EIM_A25__IPU1_DI0_D1_CS */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A25, 0x039C, 0x0088, 5, 0x0000, 0), /* MX6Q_PAD_EIM_A25__GPIO_5_2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A25, 0x039C, 0x0088, 6, 0x088C, 0), /* MX6Q_PAD_EIM_A25__HDMI_TX_CEC_LINE */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A25, 0x039C, 0x0088, 7, 0x0000, 0), /* MX6Q_PAD_EIM_A25__PL301_PER1_HBURST_0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB2, 0x03A0, 0x008C, 0, 0x0000, 0), /* MX6Q_PAD_EIM_EB2__WEIM_WEIM_EB_2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB2, 0x03A0, 0x008C, 1, 0x0800, 0), /* MX6Q_PAD_EIM_EB2__ECSPI1_SS0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB2, 0x03A0, 0x008C, 2, 0x07EC, 0), /* MX6Q_PAD_EIM_EB2__CCM_DI1_EXT_CLK */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB2, 0x03A0, 0x008C, 3, 0x08D4, 0), /* MX6Q_PAD_EIM_EB2__IPU2_CSI1_D_19 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB2, 0x03A0, 0x008C, 4, 0x0890, 0), /* MX6Q_PAD_EIM_EB2__HDMI_TX_DDC_SCL */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB2, 0x03A0, 0x008C, 5, 0x0000, 0), /* MX6Q_PAD_EIM_EB2__GPIO_2_30 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB2, 0x03A0, 0x008C, 6, 0x08A0, 0), /* MX6Q_PAD_EIM_EB2__I2C2_SCL */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB2, 0x03A0, 0x008C, 7, 0x0000, 0), /* MX6Q_PAD_EIM_EB2__SRC_BT_CFG_30 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D16, 0x03A4, 0x0090, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D16__WEIM_WEIM_D_16 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D16, 0x03A4, 0x0090, 1, 0x07F4, 0), /* MX6Q_PAD_EIM_D16__ECSPI1_SCLK */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D16, 0x03A4, 0x0090, 2, 0x0000, 0), /* MX6Q_PAD_EIM_D16__IPU1_DI0_PIN5 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D16, 0x03A4, 0x0090, 3, 0x08D0, 0), /* MX6Q_PAD_EIM_D16__IPU2_CSI1_D_18 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D16, 0x03A4, 0x0090, 4, 0x0894, 0), /* MX6Q_PAD_EIM_D16__HDMI_TX_DDC_SDA */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D16, 0x03A4, 0x0090, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D16__GPIO_3_16 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D16, 0x03A4, 0x0090, 6, 0x08A4, 0), /* MX6Q_PAD_EIM_D16__I2C2_SDA */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D17, 0x03A8, 0x0094, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D17__WEIM_WEIM_D_17 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D17, 0x03A8, 0x0094, 1, 0x07F8, 0), /* MX6Q_PAD_EIM_D17__ECSPI1_MISO */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D17, 0x03A8, 0x0094, 2, 0x0000, 0), /* MX6Q_PAD_EIM_D17__IPU1_DI0_PIN6 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D17, 0x03A8, 0x0094, 3, 0x08E0, 0), /* MX6Q_PAD_EIM_D17__IPU2_CSI1_PIXCLK */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D17, 0x03A8, 0x0094, 4, 0x0000, 0), /* MX6Q_PAD_EIM_D17__DCIC1_DCIC_OUT */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D17, 0x03A8, 0x0094, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D17__GPIO_3_17 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D17, 0x03A8, 0x0094, 6, 0x08A8, 0), /* MX6Q_PAD_EIM_D17__I2C3_SCL */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D17, 0x03A8, 0x0094, 7, 0x0000, 0), /* MX6Q_PAD_EIM_D17__PL301_PER1_HBURST_1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D18, 0x03AC, 0x0098, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D18__WEIM_WEIM_D_18 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D18, 0x03AC, 0x0098, 1, 0x07FC, 0), /* MX6Q_PAD_EIM_D18__ECSPI1_MOSI */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D18, 0x03AC, 0x0098, 2, 0x0000, 0), /* MX6Q_PAD_EIM_D18__IPU1_DI0_PIN7 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D18, 0x03AC, 0x0098, 3, 0x08CC, 0), /* MX6Q_PAD_EIM_D18__IPU2_CSI1_D_17 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D18, 0x03AC, 0x0098, 4, 0x0000, 0), /* MX6Q_PAD_EIM_D18__IPU1_DI1_D0_CS */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D18, 0x03AC, 0x0098, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D18__GPIO_3_18 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D18, 0x03AC, 0x0098, 6, 0x08AC, 0), /* MX6Q_PAD_EIM_D18__I2C3_SDA */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D18, 0x03AC, 0x0098, 7, 0x0000, 0), /* MX6Q_PAD_EIM_D18__PL301_PER1_HBURST_2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D19, 0x03B0, 0x009C, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D19__WEIM_WEIM_D_19 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D19, 0x03B0, 0x009C, 1, 0x0804, 0), /* MX6Q_PAD_EIM_D19__ECSPI1_SS1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D19, 0x03B0, 0x009C, 2, 0x0000, 0), /* MX6Q_PAD_EIM_D19__IPU1_DI0_PIN8 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D19, 0x03B0, 0x009C, 3, 0x08C8, 0), /* MX6Q_PAD_EIM_D19__IPU2_CSI1_D_16 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D19, 0x03B0, 0x009C, 4, 0x091C, 0), /* MX6Q_PAD_EIM_D19__UART1_CTS */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D19, 0x03B0, 0x009C, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D19__GPIO_3_19 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D19, 0x03B0, 0x009C, 6, 0x0000, 0), /* MX6Q_PAD_EIM_D19__EPIT1_EPITO */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D19, 0x03B0, 0x009C, 7, 0x0000, 0), /* MX6Q_PAD_EIM_D19__PL301_PER1_HRESP */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D20, 0x03B4, 0x00A0, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D20__WEIM_WEIM_D_20 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D20, 0x03B4, 0x00A0, 1, 0x0824, 0), /* MX6Q_PAD_EIM_D20__ECSPI4_SS0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D20, 0x03B4, 0x00A0, 2, 0x0000, 0), /* MX6Q_PAD_EIM_D20__IPU1_DI0_PIN16 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D20, 0x03B4, 0x00A0, 3, 0x08C4, 0), /* MX6Q_PAD_EIM_D20__IPU2_CSI1_D_15 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D20, 0x03B4, 0x00A0, 4, 0x091C, 1), /* MX6Q_PAD_EIM_D20__UART1_RTS */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D20, 0x03B4, 0x00A0, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D20__GPIO_3_20 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D20, 0x03B4, 0x00A0, 6, 0x0000, 0), /* MX6Q_PAD_EIM_D20__EPIT2_EPITO */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D21, 0x03B8, 0x00A4, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D21__WEIM_WEIM_D_21 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D21, 0x03B8, 0x00A4, 1, 0x0000, 0), /* MX6Q_PAD_EIM_D21__ECSPI4_SCLK */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D21, 0x03B8, 0x00A4, 2, 0x0000, 0), /* MX6Q_PAD_EIM_D21__IPU1_DI0_PIN17 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D21, 0x03B8, 0x00A4, 3, 0x08B4, 0), /* MX6Q_PAD_EIM_D21__IPU2_CSI1_D_11 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D21, 0x03B8, 0x00A4, 4, 0x0944, 0), /* MX6Q_PAD_EIM_D21__USBOH3_USBOTG_OC */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D21, 0x03B8, 0x00A4, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D21__GPIO_3_21 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D21, 0x03B8, 0x00A4, 6, 0x0898, 0), /* MX6Q_PAD_EIM_D21__I2C1_SCL */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D21, 0x03B8, 0x00A4, 7, 0x0914, 0), /* MX6Q_PAD_EIM_D21__SPDIF_IN1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D22, 0x03BC, 0x00A8, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D22__WEIM_WEIM_D_22 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D22, 0x03BC, 0x00A8, 1, 0x0000, 0), /* MX6Q_PAD_EIM_D22__ECSPI4_MISO */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D22, 0x03BC, 0x00A8, 2, 0x0000, 0), /* MX6Q_PAD_EIM_D22__IPU1_DI0_PIN1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D22, 0x03BC, 0x00A8, 3, 0x08B0, 0), /* MX6Q_PAD_EIM_D22__IPU2_CSI1_D_10 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D22, 0x03BC, 0x00A8, 4, 0x0000, 0), /* MX6Q_PAD_EIM_D22__USBOH3_USBOTG_PWR */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D22, 0x03BC, 0x00A8, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D22__GPIO_3_22 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D22, 0x03BC, 0x00A8, 6, 0x0000, 0), /* MX6Q_PAD_EIM_D22__SPDIF_OUT1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D22, 0x03BC, 0x00A8, 7, 0x0000, 0), /* MX6Q_PAD_EIM_D22__PL301_PER1_HWRITE */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D23, 0x03C0, 0x00AC, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D23__WEIM_WEIM_D_23 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D23, 0x03C0, 0x00AC, 1, 0x0000, 0), /* MX6Q_PAD_EIM_D23__IPU1_DI0_D0_CS */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D23, 0x03C0, 0x00AC, 2, 0x092C, 0), /* MX6Q_PAD_EIM_D23__UART3_CTS */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D23, 0x03C0, 0x00AC, 3, 0x0000, 0), /* MX6Q_PAD_EIM_D23__UART1_DCD */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D23, 0x03C0, 0x00AC, 4, 0x08D8, 0), /* MX6Q_PAD_EIM_D23__IPU2_CSI1_DATA_EN */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D23, 0x03C0, 0x00AC, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D23__GPIO_3_23 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D23, 0x03C0, 0x00AC, 6, 0x0000, 0), /* MX6Q_PAD_EIM_D23__IPU1_DI1_PIN2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D23, 0x03C0, 0x00AC, 7, 0x0000, 0), /* MX6Q_PAD_EIM_D23__IPU1_DI1_PIN14 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB3, 0x03C4, 0x00B0, 0, 0x0000, 0), /* MX6Q_PAD_EIM_EB3__WEIM_WEIM_EB_3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB3, 0x03C4, 0x00B0, 1, 0x0000, 0), /* MX6Q_PAD_EIM_EB3__ECSPI4_RDY */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB3, 0x03C4, 0x00B0, 2, 0x092C, 1), /* MX6Q_PAD_EIM_EB3__UART3_RTS */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB3, 0x03C4, 0x00B0, 3, 0x0000, 0), /* MX6Q_PAD_EIM_EB3__UART1_RI */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB3, 0x03C4, 0x00B0, 4, 0x08DC, 0), /* MX6Q_PAD_EIM_EB3__IPU2_CSI1_HSYNC */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB3, 0x03C4, 0x00B0, 5, 0x0000, 0), /* MX6Q_PAD_EIM_EB3__GPIO_2_31 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB3, 0x03C4, 0x00B0, 6, 0x0000, 0), /* MX6Q_PAD_EIM_EB3__IPU1_DI1_PIN3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB3, 0x03C4, 0x00B0, 7, 0x0000, 0), /* MX6Q_PAD_EIM_EB3__SRC_BT_CFG_31 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D24, 0x03C8, 0x00B4, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D24__WEIM_WEIM_D_24 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D24, 0x03C8, 0x00B4, 1, 0x0000, 0), /* MX6Q_PAD_EIM_D24__ECSPI4_SS2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D24, 0x03C8, 0x00B4, 2, 0x0000, 0), /* MX6Q_PAD_EIM_D24__UART3_TXD */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D24, 0x03C8, 0x00B4, 3, 0x0808, 0), /* MX6Q_PAD_EIM_D24__ECSPI1_SS2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D24, 0x03C8, 0x00B4, 4, 0x0000, 0), /* MX6Q_PAD_EIM_D24__ECSPI2_SS2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D24, 0x03C8, 0x00B4, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D24__GPIO_3_24 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D24, 0x03C8, 0x00B4, 6, 0x07D8, 0), /* MX6Q_PAD_EIM_D24__AUDMUX_AUD5_RXFS */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D24, 0x03C8, 0x00B4, 7, 0x0000, 0), /* MX6Q_PAD_EIM_D24__UART1_DTR */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D25, 0x03CC, 0x00B8, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D25__WEIM_WEIM_D_25 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D25, 0x03CC, 0x00B8, 1, 0x0000, 0), /* MX6Q_PAD_EIM_D25__ECSPI4_SS3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D25, 0x03CC, 0x00B8, 2, 0x0930, 1), /* MX6Q_PAD_EIM_D25__UART3_RXD */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D25, 0x03CC, 0x00B8, 3, 0x080C, 0), /* MX6Q_PAD_EIM_D25__ECSPI1_SS3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D25, 0x03CC, 0x00B8, 4, 0x0000, 0), /* MX6Q_PAD_EIM_D25__ECSPI2_SS3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D25, 0x03CC, 0x00B8, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D25__GPIO_3_25 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D25, 0x03CC, 0x00B8, 6, 0x07D4, 0), /* MX6Q_PAD_EIM_D25__AUDMUX_AUD5_RXC */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D25, 0x03CC, 0x00B8, 7, 0x0000, 0), /* MX6Q_PAD_EIM_D25__UART1_DSR */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D26, 0x03D0, 0x00BC, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D26__WEIM_WEIM_D_26 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D26, 0x03D0, 0x00BC, 1, 0x0000, 0), /* MX6Q_PAD_EIM_D26__IPU1_DI1_PIN11 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D26, 0x03D0, 0x00BC, 2, 0x0000, 0), /* MX6Q_PAD_EIM_D26__IPU1_CSI0_D_1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D26, 0x03D0, 0x00BC, 3, 0x08C0, 0), /* MX6Q_PAD_EIM_D26__IPU2_CSI1_D_14 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D26, 0x03D0, 0x00BC, 4, 0x0000, 0), /* MX6Q_PAD_EIM_D26__UART2_TXD */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D26, 0x03D0, 0x00BC, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D26__GPIO_3_26 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D26, 0x03D0, 0x00BC, 6, 0x0000, 0), /* MX6Q_PAD_EIM_D26__IPU1_SISG_2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D26, 0x03D0, 0x00BC, 7, 0x0000, 0), /* MX6Q_PAD_EIM_D26__IPU1_DISP1_DAT_22 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D27, 0x03D4, 0x00C0, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D27__WEIM_WEIM_D_27 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D27, 0x03D4, 0x00C0, 1, 0x0000, 0), /* MX6Q_PAD_EIM_D27__IPU1_DI1_PIN13 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D27, 0x03D4, 0x00C0, 2, 0x0000, 0), /* MX6Q_PAD_EIM_D27__IPU1_CSI0_D_0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D27, 0x03D4, 0x00C0, 3, 0x08BC, 0), /* MX6Q_PAD_EIM_D27__IPU2_CSI1_D_13 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D27, 0x03D4, 0x00C0, 4, 0x0928, 1), /* MX6Q_PAD_EIM_D27__UART2_RXD */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D27, 0x03D4, 0x00C0, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D27__GPIO_3_27 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D27, 0x03D4, 0x00C0, 6, 0x0000, 0), /* MX6Q_PAD_EIM_D27__IPU1_SISG_3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D27, 0x03D4, 0x00C0, 7, 0x0000, 0), /* MX6Q_PAD_EIM_D27__IPU1_DISP1_DAT_23 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D28, 0x03D8, 0x00C4, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D28__WEIM_WEIM_D_28 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D28, 0x03D8, 0x00C4, 1, 0x089C, 0), /* MX6Q_PAD_EIM_D28__I2C1_SDA */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D28, 0x03D8, 0x00C4, 2, 0x0000, 0), /* MX6Q_PAD_EIM_D28__ECSPI4_MOSI */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D28, 0x03D8, 0x00C4, 3, 0x08B8, 0), /* MX6Q_PAD_EIM_D28__IPU2_CSI1_D_12 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D28, 0x03D8, 0x00C4, 4, 0x0924, 0), /* MX6Q_PAD_EIM_D28__UART2_CTS */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D28, 0x03D8, 0x00C4, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D28__GPIO_3_28 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D28, 0x03D8, 0x00C4, 6, 0x0000, 0), /* MX6Q_PAD_EIM_D28__IPU1_EXT_TRIG */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D28, 0x03D8, 0x00C4, 7, 0x0000, 0), /* MX6Q_PAD_EIM_D28__IPU1_DI0_PIN13 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D29, 0x03DC, 0x00C8, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D29__WEIM_WEIM_D_29 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D29, 0x03DC, 0x00C8, 1, 0x0000, 0), /* MX6Q_PAD_EIM_D29__IPU1_DI1_PIN15 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D29, 0x03DC, 0x00C8, 2, 0x0824, 1), /* MX6Q_PAD_EIM_D29__ECSPI4_SS0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D29, 0x03DC, 0x00C8, 4, 0x0924, 1), /* MX6Q_PAD_EIM_D29__UART2_RTS */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D29, 0x03DC, 0x00C8, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D29__GPIO_3_29 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D29, 0x03DC, 0x00C8, 6, 0x08E4, 0), /* MX6Q_PAD_EIM_D29__IPU2_CSI1_VSYNC */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D29, 0x03DC, 0x00C8, 7, 0x0000, 0), /* MX6Q_PAD_EIM_D29__IPU1_DI0_PIN14 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D30, 0x03E0, 0x00CC, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D30__WEIM_WEIM_D_30 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D30, 0x03E0, 0x00CC, 1, 0x0000, 0), /* MX6Q_PAD_EIM_D30__IPU1_DISP1_DAT_21 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D30, 0x03E0, 0x00CC, 2, 0x0000, 0), /* MX6Q_PAD_EIM_D30__IPU1_DI0_PIN11 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D30, 0x03E0, 0x00CC, 3, 0x0000, 0), /* MX6Q_PAD_EIM_D30__IPU1_CSI0_D_3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D30, 0x03E0, 0x00CC, 4, 0x092C, 2), /* MX6Q_PAD_EIM_D30__UART3_CTS */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D30, 0x03E0, 0x00CC, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D30__GPIO_3_30 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D30, 0x03E0, 0x00CC, 6, 0x0948, 0), /* MX6Q_PAD_EIM_D30__USBOH3_USBH1_OC */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D30, 0x03E0, 0x00CC, 7, 0x0000, 0), /* MX6Q_PAD_EIM_D30__PL301_PER1_HPROT_0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D31, 0x03E4, 0x00D0, 0, 0x0000, 0), /* MX6Q_PAD_EIM_D31__WEIM_WEIM_D_31 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D31, 0x03E4, 0x00D0, 1, 0x0000, 0), /* MX6Q_PAD_EIM_D31__IPU1_DISP1_DAT_20 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D31, 0x03E4, 0x00D0, 2, 0x0000, 0), /* MX6Q_PAD_EIM_D31__IPU1_DI0_PIN12 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D31, 0x03E4, 0x00D0, 3, 0x0000, 0), /* MX6Q_PAD_EIM_D31__IPU1_CSI0_D_2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D31, 0x03E4, 0x00D0, 4, 0x092C, 3), /* MX6Q_PAD_EIM_D31__UART3_RTS */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D31, 0x03E4, 0x00D0, 5, 0x0000, 0), /* MX6Q_PAD_EIM_D31__GPIO_3_31 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D31, 0x03E4, 0x00D0, 6, 0x0000, 0), /* MX6Q_PAD_EIM_D31__USBOH3_USBH1_PWR */
-	IMX_PIN_REG(MX6Q_PAD_EIM_D31, 0x03E4, 0x00D0, 7, 0x0000, 0), /* MX6Q_PAD_EIM_D31__PL301_PER1_HPROT_1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A24, 0x03E8, 0x00D4, 0, 0x0000, 0), /* MX6Q_PAD_EIM_A24__WEIM_WEIM_A_24 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A24, 0x03E8, 0x00D4, 1, 0x0000, 0), /* MX6Q_PAD_EIM_A24__IPU1_DISP1_DAT_19 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A24, 0x03E8, 0x00D4, 2, 0x08D4, 1), /* MX6Q_PAD_EIM_A24__IPU2_CSI1_D_19 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A24, 0x03E8, 0x00D4, 3, 0x0000, 0), /* MX6Q_PAD_EIM_A24__IPU2_SISG_2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A24, 0x03E8, 0x00D4, 4, 0x0000, 0), /* MX6Q_PAD_EIM_A24__IPU1_SISG_2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A24, 0x03E8, 0x00D4, 5, 0x0000, 0), /* MX6Q_PAD_EIM_A24__GPIO_5_4 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A24, 0x03E8, 0x00D4, 6, 0x0000, 0), /* MX6Q_PAD_EIM_A24__PL301_PER1_HPROT_2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A24, 0x03E8, 0x00D4, 7, 0x0000, 0), /* MX6Q_PAD_EIM_A24__SRC_BT_CFG_24 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A23, 0x03EC, 0x00D8, 0, 0x0000, 0), /* MX6Q_PAD_EIM_A23__WEIM_WEIM_A_23 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A23, 0x03EC, 0x00D8, 1, 0x0000, 0), /* MX6Q_PAD_EIM_A23__IPU1_DISP1_DAT_18 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A23, 0x03EC, 0x00D8, 2, 0x08D0, 1), /* MX6Q_PAD_EIM_A23__IPU2_CSI1_D_18 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A23, 0x03EC, 0x00D8, 3, 0x0000, 0), /* MX6Q_PAD_EIM_A23__IPU2_SISG_3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A23, 0x03EC, 0x00D8, 4, 0x0000, 0), /* MX6Q_PAD_EIM_A23__IPU1_SISG_3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A23, 0x03EC, 0x00D8, 5, 0x0000, 0), /* MX6Q_PAD_EIM_A23__GPIO_6_6 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A23, 0x03EC, 0x00D8, 6, 0x0000, 0), /* MX6Q_PAD_EIM_A23__PL301_PER1_HPROT_3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A23, 0x03EC, 0x00D8, 7, 0x0000, 0), /* MX6Q_PAD_EIM_A23__SRC_BT_CFG_23 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A22, 0x03F0, 0x00DC, 0, 0x0000, 0), /* MX6Q_PAD_EIM_A22__WEIM_WEIM_A_22 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A22, 0x03F0, 0x00DC, 1, 0x0000, 0), /* MX6Q_PAD_EIM_A22__IPU1_DISP1_DAT_17 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A22, 0x03F0, 0x00DC, 2, 0x08CC, 1), /* MX6Q_PAD_EIM_A22__IPU2_CSI1_D_17 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A22, 0x03F0, 0x00DC, 5, 0x0000, 0), /* MX6Q_PAD_EIM_A22__GPIO_2_16 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A22, 0x03F0, 0x00DC, 6, 0x0000, 0), /* MX6Q_PAD_EIM_A22__TPSMP_HDATA_0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A22, 0x03F0, 0x00DC, 7, 0x0000, 0), /* MX6Q_PAD_EIM_A22__SRC_BT_CFG_22 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A21, 0x03F4, 0x00E0, 0, 0x0000, 0), /* MX6Q_PAD_EIM_A21__WEIM_WEIM_A_21 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A21, 0x03F4, 0x00E0, 1, 0x0000, 0), /* MX6Q_PAD_EIM_A21__IPU1_DISP1_DAT_16 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A21, 0x03F4, 0x00E0, 2, 0x08C8, 1), /* MX6Q_PAD_EIM_A21__IPU2_CSI1_D_16 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A21, 0x03F4, 0x00E0, 3, 0x0000, 0), /* MX6Q_PAD_EIM_A21__RESERVED_RESERVED */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A21, 0x03F4, 0x00E0, 4, 0x0000, 0), /* MX6Q_PAD_EIM_A21__MIPI_CORE_DPHY_OUT_18 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A21, 0x03F4, 0x00E0, 5, 0x0000, 0), /* MX6Q_PAD_EIM_A21__GPIO_2_17 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A21, 0x03F4, 0x00E0, 6, 0x0000, 0), /* MX6Q_PAD_EIM_A21__TPSMP_HDATA_1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A21, 0x03F4, 0x00E0, 7, 0x0000, 0), /* MX6Q_PAD_EIM_A21__SRC_BT_CFG_21 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A20, 0x03F8, 0x00E4, 0, 0x0000, 0), /* MX6Q_PAD_EIM_A20__WEIM_WEIM_A_20 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A20, 0x03F8, 0x00E4, 1, 0x0000, 0), /* MX6Q_PAD_EIM_A20__IPU1_DISP1_DAT_15 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A20, 0x03F8, 0x00E4, 2, 0x08C4, 1), /* MX6Q_PAD_EIM_A20__IPU2_CSI1_D_15 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A20, 0x03F8, 0x00E4, 3, 0x0000, 0), /* MX6Q_PAD_EIM_A20__RESERVED_RESERVED */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A20, 0x03F8, 0x00E4, 4, 0x0000, 0), /* MX6Q_PAD_EIM_A20__MIPI_CORE_DPHY_OUT_19 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A20, 0x03F8, 0x00E4, 5, 0x0000, 0), /* MX6Q_PAD_EIM_A20__GPIO_2_18 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A20, 0x03F8, 0x00E4, 6, 0x0000, 0), /* MX6Q_PAD_EIM_A20__TPSMP_HDATA_2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A20, 0x03F8, 0x00E4, 7, 0x0000, 0), /* MX6Q_PAD_EIM_A20__SRC_BT_CFG_20 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A19, 0x03FC, 0x00E8, 0, 0x0000, 0), /* MX6Q_PAD_EIM_A19__WEIM_WEIM_A_19 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A19, 0x03FC, 0x00E8, 1, 0x0000, 0), /* MX6Q_PAD_EIM_A19__IPU1_DISP1_DAT_14 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A19, 0x03FC, 0x00E8, 2, 0x08C0, 1), /* MX6Q_PAD_EIM_A19__IPU2_CSI1_D_14 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A19, 0x03FC, 0x00E8, 3, 0x0000, 0), /* MX6Q_PAD_EIM_A19__RESERVED_RESERVED */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A19, 0x03FC, 0x00E8, 4, 0x0000, 0), /* MX6Q_PAD_EIM_A19__MIPI_CORE_DPHY_OUT_20 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A19, 0x03FC, 0x00E8, 5, 0x0000, 0), /* MX6Q_PAD_EIM_A19__GPIO_2_19 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A19, 0x03FC, 0x00E8, 6, 0x0000, 0), /* MX6Q_PAD_EIM_A19__TPSMP_HDATA_3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A19, 0x03FC, 0x00E8, 7, 0x0000, 0), /* MX6Q_PAD_EIM_A19__SRC_BT_CFG_19 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A18, 0x0400, 0x00EC, 0, 0x0000, 0), /* MX6Q_PAD_EIM_A18__WEIM_WEIM_A_18 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A18, 0x0400, 0x00EC, 1, 0x0000, 0), /* MX6Q_PAD_EIM_A18__IPU1_DISP1_DAT_13 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A18, 0x0400, 0x00EC, 2, 0x08BC, 1), /* MX6Q_PAD_EIM_A18__IPU2_CSI1_D_13 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A18, 0x0400, 0x00EC, 3, 0x0000, 0), /* MX6Q_PAD_EIM_A18__RESERVED_RESERVED */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A18, 0x0400, 0x00EC, 4, 0x0000, 0), /* MX6Q_PAD_EIM_A18__MIPI_CORE_DPHY_OUT_21 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A18, 0x0400, 0x00EC, 5, 0x0000, 0), /* MX6Q_PAD_EIM_A18__GPIO_2_20 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A18, 0x0400, 0x00EC, 6, 0x0000, 0), /* MX6Q_PAD_EIM_A18__TPSMP_HDATA_4 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A18, 0x0400, 0x00EC, 7, 0x0000, 0), /* MX6Q_PAD_EIM_A18__SRC_BT_CFG_18 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A17, 0x0404, 0x00F0, 0, 0x0000, 0), /* MX6Q_PAD_EIM_A17__WEIM_WEIM_A_17 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A17, 0x0404, 0x00F0, 1, 0x0000, 0), /* MX6Q_PAD_EIM_A17__IPU1_DISP1_DAT_12 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A17, 0x0404, 0x00F0, 2, 0x08B8, 1), /* MX6Q_PAD_EIM_A17__IPU2_CSI1_D_12 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A17, 0x0404, 0x00F0, 3, 0x0000, 0), /* MX6Q_PAD_EIM_A17__RESERVED_RESERVED */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A17, 0x0404, 0x00F0, 4, 0x0000, 0), /* MX6Q_PAD_EIM_A17__MIPI_CORE_DPHY_OUT_22 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A17, 0x0404, 0x00F0, 5, 0x0000, 0), /* MX6Q_PAD_EIM_A17__GPIO_2_21 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A17, 0x0404, 0x00F0, 6, 0x0000, 0), /* MX6Q_PAD_EIM_A17__TPSMP_HDATA_5 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A17, 0x0404, 0x00F0, 7, 0x0000, 0), /* MX6Q_PAD_EIM_A17__SRC_BT_CFG_17 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A16, 0x0408, 0x00F4, 0, 0x0000, 0), /* MX6Q_PAD_EIM_A16__WEIM_WEIM_A_16 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A16, 0x0408, 0x00F4, 1, 0x0000, 0), /* MX6Q_PAD_EIM_A16__IPU1_DI1_DISP_CLK */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A16, 0x0408, 0x00F4, 2, 0x08E0, 1), /* MX6Q_PAD_EIM_A16__IPU2_CSI1_PIXCLK */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A16, 0x0408, 0x00F4, 4, 0x0000, 0), /* MX6Q_PAD_EIM_A16__MIPI_CORE_DPHY_OUT_23 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A16, 0x0408, 0x00F4, 5, 0x0000, 0), /* MX6Q_PAD_EIM_A16__GPIO_2_22 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A16, 0x0408, 0x00F4, 6, 0x0000, 0), /* MX6Q_PAD_EIM_A16__TPSMP_HDATA_6 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_A16, 0x0408, 0x00F4, 7, 0x0000, 0), /* MX6Q_PAD_EIM_A16__SRC_BT_CFG_16 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_CS0, 0x040C, 0x00F8, 0, 0x0000, 0), /* MX6Q_PAD_EIM_CS0__WEIM_WEIM_CS_0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_CS0, 0x040C, 0x00F8, 1, 0x0000, 0), /* MX6Q_PAD_EIM_CS0__IPU1_DI1_PIN5 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_CS0, 0x040C, 0x00F8, 2, 0x0810, 0), /* MX6Q_PAD_EIM_CS0__ECSPI2_SCLK */
-	IMX_PIN_REG(MX6Q_PAD_EIM_CS0, 0x040C, 0x00F8, 4, 0x0000, 0), /* MX6Q_PAD_EIM_CS0__MIPI_CORE_DPHY_OUT_24 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_CS0, 0x040C, 0x00F8, 5, 0x0000, 0), /* MX6Q_PAD_EIM_CS0__GPIO_2_23 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_CS0, 0x040C, 0x00F8, 6, 0x0000, 0), /* MX6Q_PAD_EIM_CS0__TPSMP_HDATA_7 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_CS1, 0x0410, 0x00FC, 0, 0x0000, 0), /* MX6Q_PAD_EIM_CS1__WEIM_WEIM_CS_1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_CS1, 0x0410, 0x00FC, 1, 0x0000, 0), /* MX6Q_PAD_EIM_CS1__IPU1_DI1_PIN6 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_CS1, 0x0410, 0x00FC, 2, 0x0818, 0), /* MX6Q_PAD_EIM_CS1__ECSPI2_MOSI */
-	IMX_PIN_REG(MX6Q_PAD_EIM_CS1, 0x0410, 0x00FC, 4, 0x0000, 0), /* MX6Q_PAD_EIM_CS1__MIPI_CORE_DPHY_OUT_25 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_CS1, 0x0410, 0x00FC, 5, 0x0000, 0), /* MX6Q_PAD_EIM_CS1__GPIO_2_24 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_CS1, 0x0410, 0x00FC, 6, 0x0000, 0), /* MX6Q_PAD_EIM_CS1__TPSMP_HDATA_8 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_OE, 0x0414, 0x0100, 0, 0x0000, 0), /* MX6Q_PAD_EIM_OE__WEIM_WEIM_OE */
-	IMX_PIN_REG(MX6Q_PAD_EIM_OE, 0x0414, 0x0100, 1, 0x0000, 0), /* MX6Q_PAD_EIM_OE__IPU1_DI1_PIN7 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_OE, 0x0414, 0x0100, 2, 0x0814, 0), /* MX6Q_PAD_EIM_OE__ECSPI2_MISO */
-	IMX_PIN_REG(MX6Q_PAD_EIM_OE, 0x0414, 0x0100, 4, 0x0000, 0), /* MX6Q_PAD_EIM_OE__MIPI_CORE_DPHY_OUT_26 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_OE, 0x0414, 0x0100, 5, 0x0000, 0), /* MX6Q_PAD_EIM_OE__GPIO_2_25 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_OE, 0x0414, 0x0100, 6, 0x0000, 0), /* MX6Q_PAD_EIM_OE__TPSMP_HDATA_9 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_RW, 0x0418, 0x0104, 0, 0x0000, 0), /* MX6Q_PAD_EIM_RW__WEIM_WEIM_RW */
-	IMX_PIN_REG(MX6Q_PAD_EIM_RW, 0x0418, 0x0104, 1, 0x0000, 0), /* MX6Q_PAD_EIM_RW__IPU1_DI1_PIN8 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_RW, 0x0418, 0x0104, 2, 0x081C, 0), /* MX6Q_PAD_EIM_RW__ECSPI2_SS0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_RW, 0x0418, 0x0104, 4, 0x0000, 0), /* MX6Q_PAD_EIM_RW__MIPI_CORE_DPHY_OUT_27 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_RW, 0x0418, 0x0104, 5, 0x0000, 0), /* MX6Q_PAD_EIM_RW__GPIO_2_26 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_RW, 0x0418, 0x0104, 6, 0x0000, 0), /* MX6Q_PAD_EIM_RW__TPSMP_HDATA_10 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_RW, 0x0418, 0x0104, 7, 0x0000, 0), /* MX6Q_PAD_EIM_RW__SRC_BT_CFG_29 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_LBA, 0x041C, 0x0108, 0, 0x0000, 0), /* MX6Q_PAD_EIM_LBA__WEIM_WEIM_LBA */
-	IMX_PIN_REG(MX6Q_PAD_EIM_LBA, 0x041C, 0x0108, 1, 0x0000, 0), /* MX6Q_PAD_EIM_LBA__IPU1_DI1_PIN17 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_LBA, 0x041C, 0x0108, 2, 0x0820, 0), /* MX6Q_PAD_EIM_LBA__ECSPI2_SS1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_LBA, 0x041C, 0x0108, 5, 0x0000, 0), /* MX6Q_PAD_EIM_LBA__GPIO_2_27 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_LBA, 0x041C, 0x0108, 6, 0x0000, 0), /* MX6Q_PAD_EIM_LBA__TPSMP_HDATA_11 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_LBA, 0x041C, 0x0108, 7, 0x0000, 0), /* MX6Q_PAD_EIM_LBA__SRC_BT_CFG_26 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB0, 0x0420, 0x010C, 0, 0x0000, 0), /* MX6Q_PAD_EIM_EB0__WEIM_WEIM_EB_0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB0, 0x0420, 0x010C, 1, 0x0000, 0), /* MX6Q_PAD_EIM_EB0__IPU1_DISP1_DAT_11 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB0, 0x0420, 0x010C, 2, 0x08B4, 1), /* MX6Q_PAD_EIM_EB0__IPU2_CSI1_D_11 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB0, 0x0420, 0x010C, 3, 0x0000, 0), /* MX6Q_PAD_EIM_EB0__MIPI_CORE_DPHY_OUT_0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB0, 0x0420, 0x010C, 4, 0x07F0, 0), /* MX6Q_PAD_EIM_EB0__CCM_PMIC_RDY */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB0, 0x0420, 0x010C, 5, 0x0000, 0), /* MX6Q_PAD_EIM_EB0__GPIO_2_28 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB0, 0x0420, 0x010C, 6, 0x0000, 0), /* MX6Q_PAD_EIM_EB0__TPSMP_HDATA_12 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB0, 0x0420, 0x010C, 7, 0x0000, 0), /* MX6Q_PAD_EIM_EB0__SRC_BT_CFG_27 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB1, 0x0424, 0x0110, 0, 0x0000, 0), /* MX6Q_PAD_EIM_EB1__WEIM_WEIM_EB_1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB1, 0x0424, 0x0110, 1, 0x0000, 0), /* MX6Q_PAD_EIM_EB1__IPU1_DISP1_DAT_10 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB1, 0x0424, 0x0110, 2, 0x08B0, 1), /* MX6Q_PAD_EIM_EB1__IPU2_CSI1_D_10 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB1, 0x0424, 0x0110, 3, 0x0000, 0), /* MX6Q_PAD_EIM_EB1__MIPI_CORE_DPHY__OUT_1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB1, 0x0424, 0x0110, 5, 0x0000, 0), /* MX6Q_PAD_EIM_EB1__GPIO_2_29 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB1, 0x0424, 0x0110, 6, 0x0000, 0), /* MX6Q_PAD_EIM_EB1__TPSMP_HDATA_13 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_EB1, 0x0424, 0x0110, 7, 0x0000, 0), /* MX6Q_PAD_EIM_EB1__SRC_BT_CFG_28 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA0, 0x0428, 0x0114, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA0__WEIM_WEIM_DA_A_0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA0, 0x0428, 0x0114, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA0__IPU1_DISP1_DAT_9 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA0, 0x0428, 0x0114, 2, 0x0000, 0), /* MX6Q_PAD_EIM_DA0__IPU2_CSI1_D_9 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA0, 0x0428, 0x0114, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA0__MIPI_CORE_DPHY__OUT_2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA0, 0x0428, 0x0114, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA0__GPIO_3_0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA0, 0x0428, 0x0114, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA0__TPSMP_HDATA_14 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA0, 0x0428, 0x0114, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA0__SRC_BT_CFG_0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA1, 0x042C, 0x0118, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA1__WEIM_WEIM_DA_A_1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA1, 0x042C, 0x0118, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA1__IPU1_DISP1_DAT_8 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA1, 0x042C, 0x0118, 2, 0x0000, 0), /* MX6Q_PAD_EIM_DA1__IPU2_CSI1_D_8 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA1, 0x042C, 0x0118, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA1__MIPI_CORE_DPHY_OUT_3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA1, 0x042C, 0x0118, 4, 0x0000, 0), /* MX6Q_PAD_EIM_DA1__USBPHY1_TX_LS_MODE */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA1, 0x042C, 0x0118, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA1__GPIO_3_1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA1, 0x042C, 0x0118, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA1__TPSMP_HDATA_15 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA1, 0x042C, 0x0118, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA1__SRC_BT_CFG_1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA2, 0x0430, 0x011C, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA2__WEIM_WEIM_DA_A_2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA2, 0x0430, 0x011C, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA2__IPU1_DISP1_DAT_7 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA2, 0x0430, 0x011C, 2, 0x0000, 0), /* MX6Q_PAD_EIM_DA2__IPU2_CSI1_D_7 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA2, 0x0430, 0x011C, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA2__MIPI_CORE_DPHY_OUT_4 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA2, 0x0430, 0x011C, 4, 0x0000, 0), /* MX6Q_PAD_EIM_DA2__USBPHY1_TX_HS_MODE */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA2, 0x0430, 0x011C, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA2__GPIO_3_2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA2, 0x0430, 0x011C, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA2__TPSMP_HDATA_16 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA2, 0x0430, 0x011C, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA2__SRC_BT_CFG_2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA3, 0x0434, 0x0120, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA3__WEIM_WEIM_DA_A_3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA3, 0x0434, 0x0120, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA3__IPU1_DISP1_DAT_6 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA3, 0x0434, 0x0120, 2, 0x0000, 0), /* MX6Q_PAD_EIM_DA3__IPU2_CSI1_D_6 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA3, 0x0434, 0x0120, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA3__MIPI_CORE_DPHY_OUT_5 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA3, 0x0434, 0x0120, 4, 0x0000, 0), /* MX6Q_PAD_EIM_DA3__USBPHY1_TX_HIZ */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA3, 0x0434, 0x0120, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA3__GPIO_3_3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA3, 0x0434, 0x0120, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA3__TPSMP_HDATA_17 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA3, 0x0434, 0x0120, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA3__SRC_BT_CFG_3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA4, 0x0438, 0x0124, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA4__WEIM_WEIM_DA_A_4 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA4, 0x0438, 0x0124, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA4__IPU1_DISP1_DAT_5 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA4, 0x0438, 0x0124, 2, 0x0000, 0), /* MX6Q_PAD_EIM_DA4__IPU2_CSI1_D_5 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA4, 0x0438, 0x0124, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA4__MIPI_CORE_DPHY_OUT_6 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA4, 0x0438, 0x0124, 4, 0x0000, 0), /* MX6Q_PAD_EIM_DA4__ANATOP_USBPHY1_TX_EN */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA4, 0x0438, 0x0124, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA4__GPIO_3_4 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA4, 0x0438, 0x0124, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA4__TPSMP_HDATA_18 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA4, 0x0438, 0x0124, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA4__SRC_BT_CFG_4 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA5, 0x043C, 0x0128, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA5__WEIM_WEIM_DA_A_5 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA5, 0x043C, 0x0128, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA5__IPU1_DISP1_DAT_4 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA5, 0x043C, 0x0128, 2, 0x0000, 0), /* MX6Q_PAD_EIM_DA5__IPU2_CSI1_D_4 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA5, 0x043C, 0x0128, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA5__MIPI_CORE_DPHY_OUT_7 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA5, 0x043C, 0x0128, 4, 0x0000, 0), /* MX6Q_PAD_EIM_DA5__ANATOP_USBPHY1_TX_DP */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA5, 0x043C, 0x0128, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA5__GPIO_3_5 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA5, 0x043C, 0x0128, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA5__TPSMP_HDATA_19 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA5, 0x043C, 0x0128, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA5__SRC_BT_CFG_5 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA6, 0x0440, 0x012C, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA6__WEIM_WEIM_DA_A_6 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA6, 0x0440, 0x012C, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA6__IPU1_DISP1_DAT_3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA6, 0x0440, 0x012C, 2, 0x0000, 0), /* MX6Q_PAD_EIM_DA6__IPU2_CSI1_D_3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA6, 0x0440, 0x012C, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA6__MIPI_CORE_DPHY_OUT_8 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA6, 0x0440, 0x012C, 4, 0x0000, 0), /* MX6Q_PAD_EIM_DA6__ANATOP_USBPHY1_TX_DN */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA6, 0x0440, 0x012C, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA6__GPIO_3_6 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA6, 0x0440, 0x012C, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA6__TPSMP_HDATA_20 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA6, 0x0440, 0x012C, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA6__SRC_BT_CFG_6 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA7, 0x0444, 0x0130, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA7__WEIM_WEIM_DA_A_7 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA7, 0x0444, 0x0130, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA7__IPU1_DISP1_DAT_2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA7, 0x0444, 0x0130, 2, 0x0000, 0), /* MX6Q_PAD_EIM_DA7__IPU2_CSI1_D_2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA7, 0x0444, 0x0130, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA7__MIPI_CORE_DPHY_OUT_9 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA7, 0x0444, 0x0130, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA7__GPIO_3_7 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA7, 0x0444, 0x0130, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA7__TPSMP_HDATA_21 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA7, 0x0444, 0x0130, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA7__SRC_BT_CFG_7 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA8, 0x0448, 0x0134, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA8__WEIM_WEIM_DA_A_8 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA8, 0x0448, 0x0134, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA8__IPU1_DISP1_DAT_1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA8, 0x0448, 0x0134, 2, 0x0000, 0), /* MX6Q_PAD_EIM_DA8__IPU2_CSI1_D_1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA8, 0x0448, 0x0134, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA8__MIPI_CORE_DPHY_OUT_10 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA8, 0x0448, 0x0134, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA8__GPIO_3_8 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA8, 0x0448, 0x0134, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA8__TPSMP_HDATA_22 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA8, 0x0448, 0x0134, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA8__SRC_BT_CFG_8 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA9, 0x044C, 0x0138, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA9__WEIM_WEIM_DA_A_9 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA9, 0x044C, 0x0138, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA9__IPU1_DISP1_DAT_0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA9, 0x044C, 0x0138, 2, 0x0000, 0), /* MX6Q_PAD_EIM_DA9__IPU2_CSI1_D_0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA9, 0x044C, 0x0138, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA9__MIPI_CORE_DPHY_OUT_11 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA9, 0x044C, 0x0138, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA9__GPIO_3_9 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA9, 0x044C, 0x0138, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA9__TPSMP_HDATA_23 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA9, 0x044C, 0x0138, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA9__SRC_BT_CFG_9 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA10, 0x0450, 0x013C, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA10__WEIM_WEIM_DA_A_10 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA10, 0x0450, 0x013C, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA10__IPU1_DI1_PIN15 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA10, 0x0450, 0x013C, 2, 0x08D8, 1), /* MX6Q_PAD_EIM_DA10__IPU2_CSI1_DATA_EN */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA10, 0x0450, 0x013C, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA10__MIPI_CORE_DPHY_OUT12 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA10, 0x0450, 0x013C, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA10__GPIO_3_10 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA10, 0x0450, 0x013C, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA10__TPSMP_HDATA_24 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA10, 0x0450, 0x013C, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA10__SRC_BT_CFG_10 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA11, 0x0454, 0x0140, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA11__WEIM_WEIM_DA_A_11 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA11, 0x0454, 0x0140, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA11__IPU1_DI1_PIN2 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA11, 0x0454, 0x0140, 2, 0x08DC, 1), /* MX6Q_PAD_EIM_DA11__IPU2_CSI1_HSYNC */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA11, 0x0454, 0x0140, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA11__MIPI_CORE_DPHY_OUT13 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA11, 0x0454, 0x0140, 4, 0x0000, 0), /* MX6Q_PAD_EIM_DA11__SDMA_DBG_EVT_CHN_6 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA11, 0x0454, 0x0140, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA11__GPIO_3_11 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA11, 0x0454, 0x0140, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA11__TPSMP_HDATA_25 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA11, 0x0454, 0x0140, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA11__SRC_BT_CFG_11 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA12, 0x0458, 0x0144, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA12__WEIM_WEIM_DA_A_12 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA12, 0x0458, 0x0144, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA12__IPU1_DI1_PIN3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA12, 0x0458, 0x0144, 2, 0x08E4, 1), /* MX6Q_PAD_EIM_DA12__IPU2_CSI1_VSYNC */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA12, 0x0458, 0x0144, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA12__MIPI_CORE_DPHY_OUT14 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA12, 0x0458, 0x0144, 4, 0x0000, 0), /* MX6Q_PAD_EIM_DA12__SDMA_DEBUG_EVT_CHN_3 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA12, 0x0458, 0x0144, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA12__GPIO_3_12 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA12, 0x0458, 0x0144, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA12__TPSMP_HDATA_26 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA12, 0x0458, 0x0144, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA12__SRC_BT_CFG_12 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA13, 0x045C, 0x0148, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA13__WEIM_WEIM_DA_A_13 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA13, 0x045C, 0x0148, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA13__IPU1_DI1_D0_CS */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA13, 0x045C, 0x0148, 2, 0x07EC, 1), /* MX6Q_PAD_EIM_DA13__CCM_DI1_EXT_CLK */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA13, 0x045C, 0x0148, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA13__MIPI_CORE_DPHY_OUT15 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA13, 0x045C, 0x0148, 4, 0x0000, 0), /* MX6Q_PAD_EIM_DA13__SDMA_DEBUG_EVT_CHN_4 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA13, 0x045C, 0x0148, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA13__GPIO_3_13 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA13, 0x045C, 0x0148, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA13__TPSMP_HDATA_27 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA13, 0x045C, 0x0148, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA13__SRC_BT_CFG_13 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA14, 0x0460, 0x014C, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA14__WEIM_WEIM_DA_A_14 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA14, 0x0460, 0x014C, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA14__IPU1_DI1_D1_CS */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA14, 0x0460, 0x014C, 2, 0x0000, 0), /* MX6Q_PAD_EIM_DA14__CCM_DI0_EXT_CLK */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA14, 0x0460, 0x014C, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA14__MIPI_CORE_DPHY_OUT16 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA14, 0x0460, 0x014C, 4, 0x0000, 0), /* MX6Q_PAD_EIM_DA14__SDMA_DEBUG_EVT_CHN_5 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA14, 0x0460, 0x014C, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA14__GPIO_3_14 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA14, 0x0460, 0x014C, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA14__TPSMP_HDATA_28 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA14, 0x0460, 0x014C, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA14__SRC_BT_CFG_14 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA15, 0x0464, 0x0150, 0, 0x0000, 0), /* MX6Q_PAD_EIM_DA15__WEIM_WEIM_DA_A_15 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA15, 0x0464, 0x0150, 1, 0x0000, 0), /* MX6Q_PAD_EIM_DA15__IPU1_DI1_PIN1 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA15, 0x0464, 0x0150, 2, 0x0000, 0), /* MX6Q_PAD_EIM_DA15__IPU1_DI1_PIN4 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA15, 0x0464, 0x0150, 3, 0x0000, 0), /* MX6Q_PAD_EIM_DA15__MIPI_CORE_DPHY_OUT17 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA15, 0x0464, 0x0150, 5, 0x0000, 0), /* MX6Q_PAD_EIM_DA15__GPIO_3_15 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA15, 0x0464, 0x0150, 6, 0x0000, 0), /* MX6Q_PAD_EIM_DA15__TPSMP_HDATA_29 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_DA15, 0x0464, 0x0150, 7, 0x0000, 0), /* MX6Q_PAD_EIM_DA15__SRC_BT_CFG_15 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_WAIT, 0x0468, 0x0154, 0, 0x0000, 0), /* MX6Q_PAD_EIM_WAIT__WEIM_WEIM_WAIT */
-	IMX_PIN_REG(MX6Q_PAD_EIM_WAIT, 0x0468, 0x0154, 1, 0x0000, 0), /* MX6Q_PAD_EIM_WAIT__WEIM_WEIM_DTACK_B */
-	IMX_PIN_REG(MX6Q_PAD_EIM_WAIT, 0x0468, 0x0154, 5, 0x0000, 0), /* MX6Q_PAD_EIM_WAIT__GPIO_5_0 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_WAIT, 0x0468, 0x0154, 6, 0x0000, 0), /* MX6Q_PAD_EIM_WAIT__TPSMP_HDATA_30 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_WAIT, 0x0468, 0x0154, 7, 0x0000, 0), /* MX6Q_PAD_EIM_WAIT__SRC_BT_CFG_25 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_BCLK, 0x046C, 0x0158, 0, 0x0000, 0), /* MX6Q_PAD_EIM_BCLK__WEIM_WEIM_BCLK */
-	IMX_PIN_REG(MX6Q_PAD_EIM_BCLK, 0x046C, 0x0158, 1, 0x0000, 0), /* MX6Q_PAD_EIM_BCLK__IPU1_DI1_PIN16 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_BCLK, 0x046C, 0x0158, 5, 0x0000, 0), /* MX6Q_PAD_EIM_BCLK__GPIO_6_31 */
-	IMX_PIN_REG(MX6Q_PAD_EIM_BCLK, 0x046C, 0x0158, 6, 0x0000, 0), /* MX6Q_PAD_EIM_BCLK__TPSMP_HDATA_31 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_DISP_CLK, 0x0470, 0x015C, 0, 0x0000, 0), /* MX6Q_PAD_DI0_DISP_CLK__IPU1_DI0_DSP_CLK */
-	IMX_PIN_REG(MX6Q_PAD_DI0_DISP_CLK, 0x0470, 0x015C, 1, 0x0000, 0), /* MX6Q_PAD_DI0_DISP_CLK__IPU2_DI0_DSP_CLK */
-	IMX_PIN_REG(MX6Q_PAD_DI0_DISP_CLK, 0x0470, 0x015C, 3, 0x0000, 0), /* MX6Q_PAD_DI0_DISP_CLK__MIPI_CR_DPY_OT28 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_DISP_CLK, 0x0470, 0x015C, 4, 0x0000, 0), /* MX6Q_PAD_DI0_DISP_CLK__SDMA_DBG_CR_STA0 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_DISP_CLK, 0x0470, 0x015C, 5, 0x0000, 0), /* MX6Q_PAD_DI0_DISP_CLK__GPIO_4_16 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_DISP_CLK, 0x0470, 0x015C, 6, 0x0000, 0), /* MX6Q_PAD_DI0_DISP_CLK__MMDC_DEBUG_0 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN15, 0x0474, 0x0160, 0, 0x0000, 0), /* MX6Q_PAD_DI0_PIN15__IPU1_DI0_PIN15 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN15, 0x0474, 0x0160, 1, 0x0000, 0), /* MX6Q_PAD_DI0_PIN15__IPU2_DI0_PIN15 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN15, 0x0474, 0x0160, 2, 0x0000, 0), /* MX6Q_PAD_DI0_PIN15__AUDMUX_AUD6_TXC */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN15, 0x0474, 0x0160, 3, 0x0000, 0), /* MX6Q_PAD_DI0_PIN15__MIPI_CR_DPHY_OUT_29 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN15, 0x0474, 0x0160, 4, 0x0000, 0), /* MX6Q_PAD_DI0_PIN15__SDMA_DBG_CORE_STA_1 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN15, 0x0474, 0x0160, 5, 0x0000, 0), /* MX6Q_PAD_DI0_PIN15__GPIO_4_17 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN15, 0x0474, 0x0160, 6, 0x0000, 0), /* MX6Q_PAD_DI0_PIN15__MMDC_MMDC_DEBUG_1 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN2, 0x0478, 0x0164, 0, 0x0000, 0), /* MX6Q_PAD_DI0_PIN2__IPU1_DI0_PIN2 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN2, 0x0478, 0x0164, 1, 0x0000, 0), /* MX6Q_PAD_DI0_PIN2__IPU2_DI0_PIN2 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN2, 0x0478, 0x0164, 2, 0x0000, 0), /* MX6Q_PAD_DI0_PIN2__AUDMUX_AUD6_TXD */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN2, 0x0478, 0x0164, 3, 0x0000, 0), /* MX6Q_PAD_DI0_PIN2__MIPI_CR_DPHY_OUT_30 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN2, 0x0478, 0x0164, 4, 0x0000, 0), /* MX6Q_PAD_DI0_PIN2__SDMA_DBG_CORE_STA_2 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN2, 0x0478, 0x0164, 5, 0x0000, 0), /* MX6Q_PAD_DI0_PIN2__GPIO_4_18 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN2, 0x0478, 0x0164, 6, 0x0000, 0), /* MX6Q_PAD_DI0_PIN2__MMDC_DEBUG_2 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN2, 0x0478, 0x0164, 7, 0x0000, 0), /* MX6Q_PAD_DI0_PIN2__PL301_PER1_HADDR_9 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN3, 0x047C, 0x0168, 0, 0x0000, 0), /* MX6Q_PAD_DI0_PIN3__IPU1_DI0_PIN3 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN3, 0x047C, 0x0168, 1, 0x0000, 0), /* MX6Q_PAD_DI0_PIN3__IPU2_DI0_PIN3 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN3, 0x047C, 0x0168, 2, 0x0000, 0), /* MX6Q_PAD_DI0_PIN3__AUDMUX_AUD6_TXFS */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN3, 0x047C, 0x0168, 3, 0x0000, 0), /* MX6Q_PAD_DI0_PIN3__MIPI_CORE_DPHY_OUT31 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN3, 0x047C, 0x0168, 4, 0x0000, 0), /* MX6Q_PAD_DI0_PIN3__SDMA_DBG_CORE_STA_3 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN3, 0x047C, 0x0168, 5, 0x0000, 0), /* MX6Q_PAD_DI0_PIN3__GPIO_4_19 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN3, 0x047C, 0x0168, 6, 0x0000, 0), /* MX6Q_PAD_DI0_PIN3__MMDC_MMDC_DEBUG_3 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN3, 0x047C, 0x0168, 7, 0x0000, 0), /* MX6Q_PAD_DI0_PIN3__PL301_PER1_HADDR_10 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN4, 0x0480, 0x016C, 0, 0x0000, 0), /* MX6Q_PAD_DI0_PIN4__IPU1_DI0_PIN4 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN4, 0x0480, 0x016C, 1, 0x0000, 0), /* MX6Q_PAD_DI0_PIN4__IPU2_DI0_PIN4 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN4, 0x0480, 0x016C, 2, 0x0000, 0), /* MX6Q_PAD_DI0_PIN4__AUDMUX_AUD6_RXD */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN4, 0x0480, 0x016C, 3, 0x094C, 0), /* MX6Q_PAD_DI0_PIN4__USDHC1_WP */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN4, 0x0480, 0x016C, 4, 0x0000, 0), /* MX6Q_PAD_DI0_PIN4__SDMA_DEBUG_YIELD */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN4, 0x0480, 0x016C, 5, 0x0000, 0), /* MX6Q_PAD_DI0_PIN4__GPIO_4_20 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN4, 0x0480, 0x016C, 6, 0x0000, 0), /* MX6Q_PAD_DI0_PIN4__MMDC_MMDC_DEBUG_4 */
-	IMX_PIN_REG(MX6Q_PAD_DI0_PIN4, 0x0480, 0x016C, 7, 0x0000, 0), /* MX6Q_PAD_DI0_PIN4__PL301_PER1_HADDR_11 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT0, 0x0484, 0x0170, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT0__IPU1_DISP0_DAT_0 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT0, 0x0484, 0x0170, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT0__IPU2_DISP0_DAT_0 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT0, 0x0484, 0x0170, 2, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT0__ECSPI3_SCLK */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT0, 0x0484, 0x0170, 3, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT0__USDHC1_USDHC_DBG_0 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT0, 0x0484, 0x0170, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT0__SDMA_DBG_CORE_RUN */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT0, 0x0484, 0x0170, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT0__GPIO_4_21 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT0, 0x0484, 0x0170, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT0__MMDC_MMDC_DEBUG_5 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT1, 0x0488, 0x0174, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT1__IPU1_DISP0_DAT_1 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT1, 0x0488, 0x0174, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT1__IPU2_DISP0_DAT_1 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT1, 0x0488, 0x0174, 2, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT1__ECSPI3_MOSI */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT1, 0x0488, 0x0174, 3, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT1__USDHC1_USDHC_DBG_1 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT1, 0x0488, 0x0174, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT1__SDMA_DBG_EVT_CHNSL */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT1, 0x0488, 0x0174, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT1__GPIO_4_22 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT1, 0x0488, 0x0174, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT1__MMDC_DEBUG_6 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT1, 0x0488, 0x0174, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT1__PL301_PER1_HADR_12 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT2, 0x048C, 0x0178, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT2__IPU1_DISP0_DAT_2 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT2, 0x048C, 0x0178, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT2__IPU2_DISP0_DAT_2 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT2, 0x048C, 0x0178, 2, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT2__ECSPI3_MISO */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT2, 0x048C, 0x0178, 3, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT2__USDHC1_USDHC_DBG_2 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT2, 0x048C, 0x0178, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT2__SDMA_DEBUG_MODE */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT2, 0x048C, 0x0178, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT2__GPIO_4_23 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT2, 0x048C, 0x0178, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT2__MMDC_DEBUG_7 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT2, 0x048C, 0x0178, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT2__PL301_PER1_HADR_13 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT3, 0x0490, 0x017C, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT3__IPU1_DISP0_DAT_3 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT3, 0x0490, 0x017C, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT3__IPU2_DISP0_DAT_3 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT3, 0x0490, 0x017C, 2, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT3__ECSPI3_SS0 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT3, 0x0490, 0x017C, 3, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT3__USDHC1_USDHC_DBG_3 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT3, 0x0490, 0x017C, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT3__SDMA_DBG_BUS_ERROR */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT3, 0x0490, 0x017C, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT3__GPIO_4_24 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT3, 0x0490, 0x017C, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT3__MMDC_MMDC_DBG_8 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT3, 0x0490, 0x017C, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT3__PL301_PER1_HADR_14 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT4, 0x0494, 0x0180, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT4__IPU1_DISP0_DAT_4 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT4, 0x0494, 0x0180, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT4__IPU2_DISP0_DAT_4 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT4, 0x0494, 0x0180, 2, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT4__ECSPI3_SS1 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT4, 0x0494, 0x0180, 3, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT4__USDHC1_USDHC_DBG_4 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT4, 0x0494, 0x0180, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT4__SDMA_DEBUG_BUS_RWB */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT4, 0x0494, 0x0180, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT4__GPIO_4_25 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT4, 0x0494, 0x0180, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT4__MMDC_MMDC_DEBUG_9 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT4, 0x0494, 0x0180, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT4__PL301_PER1_HADR_15 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT5, 0x0498, 0x0184, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT5__IPU1_DISP0_DAT_5 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT5, 0x0498, 0x0184, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT5__IPU2_DISP0_DAT_5 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT5, 0x0498, 0x0184, 2, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT5__ECSPI3_SS2 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT5, 0x0498, 0x0184, 3, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT5__AUDMUX_AUD6_RXFS */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT5, 0x0498, 0x0184, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT5__SDMA_DBG_MCH_DMBUS */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT5, 0x0498, 0x0184, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT5__GPIO_4_26 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT5, 0x0498, 0x0184, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT5__MMDC_DEBUG_10 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT5, 0x0498, 0x0184, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT5__PL301_PER1_HADR_16 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT6, 0x049C, 0x0188, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT6__IPU1_DISP0_DAT_6 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT6, 0x049C, 0x0188, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT6__IPU2_DISP0_DAT_6 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT6, 0x049C, 0x0188, 2, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT6__ECSPI3_SS3 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT6, 0x049C, 0x0188, 3, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT6__AUDMUX_AUD6_RXC */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT6, 0x049C, 0x0188, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT6__SDMA_DBG_RTBUF_WRT */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT6, 0x049C, 0x0188, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT6__GPIO_4_27 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT6, 0x049C, 0x0188, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT6__MMDC_DEBUG_11 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT6, 0x049C, 0x0188, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT6__PL301_PER1_HADR_17 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT7, 0x04A0, 0x018C, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT7__IPU1_DISP0_DAT_7 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT7, 0x04A0, 0x018C, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT7__IPU2_DISP0_DAT_7 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT7, 0x04A0, 0x018C, 2, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT7__ECSPI3_RDY */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT7, 0x04A0, 0x018C, 3, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT7__USDHC1_USDHC_DBG_5 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT7, 0x04A0, 0x018C, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT7__SDMA_DBG_EVT_CHN_0 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT7, 0x04A0, 0x018C, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT7__GPIO_4_28 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT7, 0x04A0, 0x018C, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT7__MMDC_DEBUG_12 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT7, 0x04A0, 0x018C, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT7__PL301_PER1_HADR_18 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT8, 0x04A4, 0x0190, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT8__IPU1_DISP0_DAT_8 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT8, 0x04A4, 0x0190, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT8__IPU2_DISP0_DAT_8 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT8, 0x04A4, 0x0190, 2, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT8__PWM1_PWMO */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT8, 0x04A4, 0x0190, 3, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT8__WDOG1_WDOG_B */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT8, 0x04A4, 0x0190, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT8__SDMA_DBG_EVT_CHN_1 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT8, 0x04A4, 0x0190, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT8__GPIO_4_29 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT8, 0x04A4, 0x0190, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT8__MMDC_DEBUG_13 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT8, 0x04A4, 0x0190, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT8__PL301_PER1_HADR_19 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT9, 0x04A8, 0x0194, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT9__IPU1_DISP0_DAT_9 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT9, 0x04A8, 0x0194, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT9__IPU2_DISP0_DAT_9 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT9, 0x04A8, 0x0194, 2, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT9__PWM2_PWMO */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT9, 0x04A8, 0x0194, 3, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT9__WDOG2_WDOG_B */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT9, 0x04A8, 0x0194, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT9__SDMA_DBG_EVT_CHN_2 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT9, 0x04A8, 0x0194, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT9__GPIO_4_30 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT9, 0x04A8, 0x0194, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT9__MMDC_DEBUG_14 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT9, 0x04A8, 0x0194, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT9__PL301_PER1_HADR_20 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT10, 0x04AC, 0x0198, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT10__IPU1_DISP0_DAT_10 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT10, 0x04AC, 0x0198, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT10__IPU2_DISP0_DAT_10 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT10, 0x04AC, 0x0198, 3, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT10__USDHC1_DBG_6 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT10, 0x04AC, 0x0198, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT10__SDMA_DBG_EVT_CHN3 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT10, 0x04AC, 0x0198, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT10__GPIO_4_31 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT10, 0x04AC, 0x0198, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT10__MMDC_DEBUG_15 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT10, 0x04AC, 0x0198, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT10__PL301_PER1_HADR21 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT11, 0x04B0, 0x019C, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT11__IPU1_DISP0_DAT_11 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT11, 0x04B0, 0x019C, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT11__IPU2_DISP0_DAT_11 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT11, 0x04B0, 0x019C, 3, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT11__USDHC1_USDHC_DBG7 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT11, 0x04B0, 0x019C, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT11__SDMA_DBG_EVT_CHN4 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT11, 0x04B0, 0x019C, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT11__GPIO_5_5 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT11, 0x04B0, 0x019C, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT11__MMDC_DEBUG_16 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT11, 0x04B0, 0x019C, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT11__PL301_PER1_HADR22 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT12, 0x04B4, 0x01A0, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT12__IPU1_DISP0_DAT_12 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT12, 0x04B4, 0x01A0, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT12__IPU2_DISP0_DAT_12 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT12, 0x04B4, 0x01A0, 3, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT12__RESERVED_RESERVED */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT12, 0x04B4, 0x01A0, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT12__SDMA_DBG_EVT_CHN5 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT12, 0x04B4, 0x01A0, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT12__GPIO_5_6 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT12, 0x04B4, 0x01A0, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT12__MMDC_DEBUG_17 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT12, 0x04B4, 0x01A0, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT12__PL301_PER1_HADR23 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT13, 0x04B8, 0x01A4, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT13__IPU1_DISP0_DAT_13 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT13, 0x04B8, 0x01A4, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT13__IPU2_DISP0_DAT_13 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT13, 0x04B8, 0x01A4, 3, 0x07D8, 1), /* MX6Q_PAD_DISP0_DAT13__AUDMUX_AUD5_RXFS */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT13, 0x04B8, 0x01A4, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT13__SDMA_DBG_EVT_CHN0 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT13, 0x04B8, 0x01A4, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT13__GPIO_5_7 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT13, 0x04B8, 0x01A4, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT13__MMDC_DEBUG_18 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT13, 0x04B8, 0x01A4, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT13__PL301_PER1_HADR24 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT14, 0x04BC, 0x01A8, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT14__IPU1_DISP0_DAT_14 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT14, 0x04BC, 0x01A8, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT14__IPU2_DISP0_DAT_14 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT14, 0x04BC, 0x01A8, 3, 0x07D4, 1), /* MX6Q_PAD_DISP0_DAT14__AUDMUX_AUD5_RXC */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT14, 0x04BC, 0x01A8, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT14__SDMA_DBG_EVT_CHN1 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT14, 0x04BC, 0x01A8, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT14__GPIO_5_8 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT14, 0x04BC, 0x01A8, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT14__MMDC_DEBUG_19 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT15, 0x04C0, 0x01AC, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT15__IPU1_DISP0_DAT_15 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT15, 0x04C0, 0x01AC, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT15__IPU2_DISP0_DAT_15 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT15, 0x04C0, 0x01AC, 2, 0x0804, 1), /* MX6Q_PAD_DISP0_DAT15__ECSPI1_SS1 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT15, 0x04C0, 0x01AC, 3, 0x0820, 1), /* MX6Q_PAD_DISP0_DAT15__ECSPI2_SS1 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT15, 0x04C0, 0x01AC, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT15__SDMA_DBG_EVT_CHN2 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT15, 0x04C0, 0x01AC, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT15__GPIO_5_9 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT15, 0x04C0, 0x01AC, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT15__MMDC_DEBUG_20 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT15, 0x04C0, 0x01AC, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT15__PL301_PER1_HADR25 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT16, 0x04C4, 0x01B0, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT16__IPU1_DISP0_DAT_16 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT16, 0x04C4, 0x01B0, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT16__IPU2_DISP0_DAT_16 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT16, 0x04C4, 0x01B0, 2, 0x0818, 1), /* MX6Q_PAD_DISP0_DAT16__ECSPI2_MOSI */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT16, 0x04C4, 0x01B0, 3, 0x07DC, 0), /* MX6Q_PAD_DISP0_DAT16__AUDMUX_AUD5_TXC */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT16, 0x04C4, 0x01B0, 4, 0x090C, 0), /* MX6Q_PAD_DISP0_DAT16__SDMA_EXT_EVENT_0 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT16, 0x04C4, 0x01B0, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT16__GPIO_5_10 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT16, 0x04C4, 0x01B0, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT16__MMDC_DEBUG_21 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT16, 0x04C4, 0x01B0, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT16__PL301_PER1_HADR26 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT17, 0x04C8, 0x01B4, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT17__IPU1_DISP0_DAT_17 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT17, 0x04C8, 0x01B4, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT17__IPU2_DISP0_DAT_17 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT17, 0x04C8, 0x01B4, 2, 0x0814, 1), /* MX6Q_PAD_DISP0_DAT17__ECSPI2_MISO */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT17, 0x04C8, 0x01B4, 3, 0x07D0, 0), /* MX6Q_PAD_DISP0_DAT17__AUDMUX_AUD5_TXD */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT17, 0x04C8, 0x01B4, 4, 0x0910, 0), /* MX6Q_PAD_DISP0_DAT17__SDMA_EXT_EVENT_1 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT17, 0x04C8, 0x01B4, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT17__GPIO_5_11 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT17, 0x04C8, 0x01B4, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT17__MMDC_DEBUG_22 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT17, 0x04C8, 0x01B4, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT17__PL301_PER1_HADR27 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT18, 0x04CC, 0x01B8, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT18__IPU1_DISP0_DAT_18 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT18, 0x04CC, 0x01B8, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT18__IPU2_DISP0_DAT_18 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT18, 0x04CC, 0x01B8, 2, 0x081C, 1), /* MX6Q_PAD_DISP0_DAT18__ECSPI2_SS0 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT18, 0x04CC, 0x01B8, 3, 0x07E0, 0), /* MX6Q_PAD_DISP0_DAT18__AUDMUX_AUD5_TXFS */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT18, 0x04CC, 0x01B8, 4, 0x07C0, 0), /* MX6Q_PAD_DISP0_DAT18__AUDMUX_AUD4_RXFS */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT18, 0x04CC, 0x01B8, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT18__GPIO_5_12 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT18, 0x04CC, 0x01B8, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT18__MMDC_DEBUG_23 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT18, 0x04CC, 0x01B8, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT18__WEIM_WEIM_CS_2 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT19, 0x04D0, 0x01BC, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT19__IPU1_DISP0_DAT_19 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT19, 0x04D0, 0x01BC, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT19__IPU2_DISP0_DAT_19 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT19, 0x04D0, 0x01BC, 2, 0x0810, 1), /* MX6Q_PAD_DISP0_DAT19__ECSPI2_SCLK */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT19, 0x04D0, 0x01BC, 3, 0x07CC, 0), /* MX6Q_PAD_DISP0_DAT19__AUDMUX_AUD5_RXD */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT19, 0x04D0, 0x01BC, 4, 0x07BC, 0), /* MX6Q_PAD_DISP0_DAT19__AUDMUX_AUD4_RXC */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT19, 0x04D0, 0x01BC, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT19__GPIO_5_13 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT19, 0x04D0, 0x01BC, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT19__MMDC_DEBUG_24 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT19, 0x04D0, 0x01BC, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT19__WEIM_WEIM_CS_3 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT20, 0x04D4, 0x01C0, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT20__IPU1_DISP0_DAT_20 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT20, 0x04D4, 0x01C0, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT20__IPU2_DISP0_DAT_20 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT20, 0x04D4, 0x01C0, 2, 0x07F4, 1), /* MX6Q_PAD_DISP0_DAT20__ECSPI1_SCLK */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT20, 0x04D4, 0x01C0, 3, 0x07C4, 0), /* MX6Q_PAD_DISP0_DAT20__AUDMUX_AUD4_TXC */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT20, 0x04D4, 0x01C0, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT20__SDMA_DBG_EVT_CHN7 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT20, 0x04D4, 0x01C0, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT20__GPIO_5_14 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT20, 0x04D4, 0x01C0, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT20__MMDC_DEBUG_25 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT20, 0x04D4, 0x01C0, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT20__PL301_PER1_HADR28 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT21, 0x04D8, 0x01C4, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT21__IPU1_DISP0_DAT_21 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT21, 0x04D8, 0x01C4, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT21__IPU2_DISP0_DAT_21 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT21, 0x04D8, 0x01C4, 2, 0x07FC, 1), /* MX6Q_PAD_DISP0_DAT21__ECSPI1_MOSI */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT21, 0x04D8, 0x01C4, 3, 0x07B8, 1), /* MX6Q_PAD_DISP0_DAT21__AUDMUX_AUD4_TXD */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT21, 0x04D8, 0x01C4, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT21__SDMA_DBG_BUS_DEV0 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT21, 0x04D8, 0x01C4, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT21__GPIO_5_15 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT21, 0x04D8, 0x01C4, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT21__MMDC_DEBUG_26 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT21, 0x04D8, 0x01C4, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT21__PL301_PER1_HADR29 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT22, 0x04DC, 0x01C8, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT22__IPU1_DISP0_DAT_22 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT22, 0x04DC, 0x01C8, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT22__IPU2_DISP0_DAT_22 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT22, 0x04DC, 0x01C8, 2, 0x07F8, 1), /* MX6Q_PAD_DISP0_DAT22__ECSPI1_MISO */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT22, 0x04DC, 0x01C8, 3, 0x07C8, 1), /* MX6Q_PAD_DISP0_DAT22__AUDMUX_AUD4_TXFS */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT22, 0x04DC, 0x01C8, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT22__SDMA_DBG_BUS_DEV1 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT22, 0x04DC, 0x01C8, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT22__GPIO_5_16 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT22, 0x04DC, 0x01C8, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT22__MMDC_DEBUG_27 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT22, 0x04DC, 0x01C8, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT22__PL301_PER1_HADR30 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT23, 0x04E0, 0x01CC, 0, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT23__IPU1_DISP0_DAT_23 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT23, 0x04E0, 0x01CC, 1, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT23__IPU2_DISP0_DAT_23 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT23, 0x04E0, 0x01CC, 2, 0x0800, 1), /* MX6Q_PAD_DISP0_DAT23__ECSPI1_SS0 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT23, 0x04E0, 0x01CC, 3, 0x07B4, 1), /* MX6Q_PAD_DISP0_DAT23__AUDMUX_AUD4_RXD */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT23, 0x04E0, 0x01CC, 4, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT23__SDMA_DBG_BUS_DEV2 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT23, 0x04E0, 0x01CC, 5, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT23__GPIO_5_17 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT23, 0x04E0, 0x01CC, 6, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT23__MMDC_DEBUG_28 */
-	IMX_PIN_REG(MX6Q_PAD_DISP0_DAT23, 0x04E0, 0x01CC, 7, 0x0000, 0), /* MX6Q_PAD_DISP0_DAT23__PL301_PER1_HADR31 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_MDIO, 0x04E4, 0x01D0, 0, 0x0000, 0), /* MX6Q_PAD_ENET_MDIO__RESERVED_RESERVED */
-	IMX_PIN_REG(MX6Q_PAD_ENET_MDIO, 0x04E4, 0x01D0, 1, 0x0840, 0), /* MX6Q_PAD_ENET_MDIO__ENET_MDIO */
-	IMX_PIN_REG(MX6Q_PAD_ENET_MDIO, 0x04E4, 0x01D0, 2, 0x086C, 0), /* MX6Q_PAD_ENET_MDIO__ESAI1_SCKR */
-	IMX_PIN_REG(MX6Q_PAD_ENET_MDIO, 0x04E4, 0x01D0, 3, 0x0000, 0), /* MX6Q_PAD_ENET_MDIO__SDMA_DEBUG_BUS_DEV3 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_MDIO, 0x04E4, 0x01D0, 4, 0x0000, 0), /* MX6Q_PAD_ENET_MDIO__ENET_1588_EVT1_OUT */
-	IMX_PIN_REG(MX6Q_PAD_ENET_MDIO, 0x04E4, 0x01D0, 5, 0x0000, 0), /* MX6Q_PAD_ENET_MDIO__GPIO_1_22 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_MDIO, 0x04E4, 0x01D0, 6, 0x0000, 0), /* MX6Q_PAD_ENET_MDIO__SPDIF_PLOCK */
-	IMX_PIN_REG(MX6Q_PAD_ENET_REF_CLK, 0x04E8, 0x01D4, 0, 0x0000, 0), /* MX6Q_PAD_ENET_REF_CLK__RESERVED_RSRVED */
-	IMX_PIN_REG(MX6Q_PAD_ENET_REF_CLK, 0x04E8, 0x01D4, 1, 0x0000, 0), /* MX6Q_PAD_ENET_REF_CLK__ENET_TX_CLK */
-	IMX_PIN_REG(MX6Q_PAD_ENET_REF_CLK, 0x04E8, 0x01D4, 2, 0x085C, 0), /* MX6Q_PAD_ENET_REF_CLK__ESAI1_FSR */
-	IMX_PIN_REG(MX6Q_PAD_ENET_REF_CLK, 0x04E8, 0x01D4, 3, 0x0000, 0), /* MX6Q_PAD_ENET_REF_CLK__SDMA_DBGBUS_DEV4 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_REF_CLK, 0x04E8, 0x01D4, 5, 0x0000, 0), /* MX6Q_PAD_ENET_REF_CLK__GPIO_1_23 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_REF_CLK, 0x04E8, 0x01D4, 6, 0x0000, 0), /* MX6Q_PAD_ENET_REF_CLK__SPDIF_SRCLK */
-	IMX_PIN_REG(MX6Q_PAD_ENET_REF_CLK, 0x04E8, 0x01D4, 7, 0x0000, 0), /* MX6Q_PAD_ENET_REF_CLK__USBPHY1_RX_SQH */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RX_ER, 0x04EC, 0x01D8, 1, 0x0000, 0), /* MX6Q_PAD_ENET_RX_ER__ENET_RX_ER */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RX_ER, 0x04EC, 0x01D8, 2, 0x0864, 0), /* MX6Q_PAD_ENET_RX_ER__ESAI1_HCKR */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RX_ER, 0x04EC, 0x01D8, 3, 0x0914, 1), /* MX6Q_PAD_ENET_RX_ER__SPDIF_IN1 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RX_ER, 0x04EC, 0x01D8, 4, 0x0000, 0), /* MX6Q_PAD_ENET_RX_ER__ENET_1588_EVT2_OUT */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RX_ER, 0x04EC, 0x01D8, 5, 0x0000, 0), /* MX6Q_PAD_ENET_RX_ER__GPIO_1_24 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RX_ER, 0x04EC, 0x01D8, 6, 0x0000, 0), /* MX6Q_PAD_ENET_RX_ER__PHY_TDI */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RX_ER, 0x04EC, 0x01D8, 7, 0x0000, 0), /* MX6Q_PAD_ENET_RX_ER__USBPHY1_RX_HS_RXD */
-	IMX_PIN_REG(MX6Q_PAD_ENET_CRS_DV, 0x04F0, 0x01DC, 0, 0x0000, 0), /* MX6Q_PAD_ENET_CRS_DV__RESERVED_RSRVED */
-	IMX_PIN_REG(MX6Q_PAD_ENET_CRS_DV, 0x04F0, 0x01DC, 1, 0x0858, 1), /* MX6Q_PAD_ENET_CRS_DV__ENET_RX_EN */
-	IMX_PIN_REG(MX6Q_PAD_ENET_CRS_DV, 0x04F0, 0x01DC, 2, 0x0870, 0), /* MX6Q_PAD_ENET_CRS_DV__ESAI1_SCKT */
-	IMX_PIN_REG(MX6Q_PAD_ENET_CRS_DV, 0x04F0, 0x01DC, 3, 0x0918, 1), /* MX6Q_PAD_ENET_CRS_DV__SPDIF_EXTCLK */
-	IMX_PIN_REG(MX6Q_PAD_ENET_CRS_DV, 0x04F0, 0x01DC, 5, 0x0000, 0), /* MX6Q_PAD_ENET_CRS_DV__GPIO_1_25 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_CRS_DV, 0x04F0, 0x01DC, 6, 0x0000, 0), /* MX6Q_PAD_ENET_CRS_DV__PHY_TDO */
-	IMX_PIN_REG(MX6Q_PAD_ENET_CRS_DV, 0x04F0, 0x01DC, 7, 0x0000, 0), /* MX6Q_PAD_ENET_CRS_DV__USBPHY1_RX_FS_RXD */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RXD1, 0x04F4, 0x01E0, 0, 0x0908, 0), /* MX6Q_PAD_ENET_RXD1__MLB_MLBSIG */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RXD1, 0x04F4, 0x01E0, 1, 0x084C, 1), /* MX6Q_PAD_ENET_RXD1__ENET_RDATA_1 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RXD1, 0x04F4, 0x01E0, 2, 0x0860, 0), /* MX6Q_PAD_ENET_RXD1__ESAI1_FST */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RXD1, 0x04F4, 0x01E0, 4, 0x0000, 0), /* MX6Q_PAD_ENET_RXD1__ENET_1588_EVT3_OUT */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RXD1, 0x04F4, 0x01E0, 5, 0x0000, 0), /* MX6Q_PAD_ENET_RXD1__GPIO_1_26 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RXD1, 0x04F4, 0x01E0, 6, 0x0000, 0), /* MX6Q_PAD_ENET_RXD1__PHY_TCK */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RXD1, 0x04F4, 0x01E0, 7, 0x0000, 0), /* MX6Q_PAD_ENET_RXD1__USBPHY1_RX_DISCON */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RXD0, 0x04F8, 0x01E4, 0, 0x0000, 0), /* MX6Q_PAD_ENET_RXD0__OSC32K_32K_OUT */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RXD0, 0x04F8, 0x01E4, 1, 0x0848, 1), /* MX6Q_PAD_ENET_RXD0__ENET_RDATA_0 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RXD0, 0x04F8, 0x01E4, 2, 0x0868, 0), /* MX6Q_PAD_ENET_RXD0__ESAI1_HCKT */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RXD0, 0x04F8, 0x01E4, 3, 0x0000, 0), /* MX6Q_PAD_ENET_RXD0__SPDIF_OUT1 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RXD0, 0x04F8, 0x01E4, 5, 0x0000, 0), /* MX6Q_PAD_ENET_RXD0__GPIO_1_27 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RXD0, 0x04F8, 0x01E4, 6, 0x0000, 0), /* MX6Q_PAD_ENET_RXD0__PHY_TMS */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RXD0, 0x04F8, 0x01E4, 7, 0x0000, 0), /* MX6Q_PAD_ENET_RXD0__USBPHY1_PLL_CK20DIV */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TX_EN, 0x04FC, 0x01E8, 0, 0x0000, 0), /* MX6Q_PAD_ENET_TX_EN__RESERVED_RSRVED */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TX_EN, 0x04FC, 0x01E8, 1, 0x0000, 0), /* MX6Q_PAD_ENET_TX_EN__ENET_TX_EN */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TX_EN, 0x04FC, 0x01E8, 2, 0x0880, 0), /* MX6Q_PAD_ENET_TX_EN__ESAI1_TX3_RX2 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TX_EN, 0x04FC, 0x01E8, 5, 0x0000, 0), /* MX6Q_PAD_ENET_TX_EN__GPIO_1_28 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TX_EN, 0x04FC, 0x01E8, 6, 0x0000, 0), /* MX6Q_PAD_ENET_TX_EN__SATA_PHY_TDI */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TX_EN, 0x04FC, 0x01E8, 7, 0x0000, 0), /* MX6Q_PAD_ENET_TX_EN__USBPHY2_RX_SQH */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TXD1, 0x0500, 0x01EC, 0, 0x0900, 0), /* MX6Q_PAD_ENET_TXD1__MLB_MLBCLK */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TXD1, 0x0500, 0x01EC, 1, 0x0000, 0), /* MX6Q_PAD_ENET_TXD1__ENET_TDATA_1 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TXD1, 0x0500, 0x01EC, 2, 0x087C, 0), /* MX6Q_PAD_ENET_TXD1__ESAI1_TX2_RX3 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TXD1, 0x0500, 0x01EC, 4, 0x0000, 0), /* MX6Q_PAD_ENET_TXD1__ENET_1588_EVENT0_IN */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TXD1, 0x0500, 0x01EC, 5, 0x0000, 0), /* MX6Q_PAD_ENET_TXD1__GPIO_1_29 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TXD1, 0x0500, 0x01EC, 6, 0x0000, 0), /* MX6Q_PAD_ENET_TXD1__SATA_PHY_TDO */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TXD1, 0x0500, 0x01EC, 7, 0x0000, 0), /* MX6Q_PAD_ENET_TXD1__USBPHY2_RX_HS_RXD */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TXD0, 0x0504, 0x01F0, 0, 0x0000, 0), /* MX6Q_PAD_ENET_TXD0__RESERVED_RSRVED */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TXD0, 0x0504, 0x01F0, 1, 0x0000, 0), /* MX6Q_PAD_ENET_TXD0__ENET_TDATA_0 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TXD0, 0x0504, 0x01F0, 2, 0x0884, 0), /* MX6Q_PAD_ENET_TXD0__ESAI1_TX4_RX1 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TXD0, 0x0504, 0x01F0, 5, 0x0000, 0), /* MX6Q_PAD_ENET_TXD0__GPIO_1_30 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TXD0, 0x0504, 0x01F0, 6, 0x0000, 0), /* MX6Q_PAD_ENET_TXD0__SATA_PHY_TCK */
-	IMX_PIN_REG(MX6Q_PAD_ENET_TXD0, 0x0504, 0x01F0, 7, 0x0000, 0), /* MX6Q_PAD_ENET_TXD0__USBPHY2_RX_FS_RXD */
-	IMX_PIN_REG(MX6Q_PAD_ENET_MDC, 0x0508, 0x01F4, 0, 0x0904, 0), /* MX6Q_PAD_ENET_MDC__MLB_MLBDAT */
-	IMX_PIN_REG(MX6Q_PAD_ENET_MDC, 0x0508, 0x01F4, 1, 0x0000, 0), /* MX6Q_PAD_ENET_MDC__ENET_MDC */
-	IMX_PIN_REG(MX6Q_PAD_ENET_MDC, 0x0508, 0x01F4, 2, 0x0888, 0), /* MX6Q_PAD_ENET_MDC__ESAI1_TX5_RX0 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_MDC, 0x0508, 0x01F4, 4, 0x0000, 0), /* MX6Q_PAD_ENET_MDC__ENET_1588_EVENT1_IN */
-	IMX_PIN_REG(MX6Q_PAD_ENET_MDC, 0x0508, 0x01F4, 5, 0x0000, 0), /* MX6Q_PAD_ENET_MDC__GPIO_1_31 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_MDC, 0x0508, 0x01F4, 6, 0x0000, 0), /* MX6Q_PAD_ENET_MDC__SATA_PHY_TMS */
-	IMX_PIN_REG(MX6Q_PAD_ENET_MDC, 0x0508, 0x01F4, 7, 0x0000, 0), /* MX6Q_PAD_ENET_MDC__USBPHY2_RX_DISCON */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D40, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D40__MMDC_DRAM_D_40 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D41, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D41__MMDC_DRAM_D_41 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D42, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D42__MMDC_DRAM_D_42 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D43, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D43__MMDC_DRAM_D_43 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D44, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D44__MMDC_DRAM_D_44 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D45, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D45__MMDC_DRAM_D_45 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D46, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D46__MMDC_DRAM_D_46 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D47, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D47__MMDC_DRAM_D_47 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDQS5, 0x050C, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDQS5__MMDC_DRAM_SDQS_5 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_DQM5, 0x0510, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_DQM5__MMDC_DRAM_DQM_5 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D32, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D32__MMDC_DRAM_D_32 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D33, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D33__MMDC_DRAM_D_33 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D34, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D34__MMDC_DRAM_D_34 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D35, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D35__MMDC_DRAM_D_35 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D36, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D36__MMDC_DRAM_D_36 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D37, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D37__MMDC_DRAM_D_37 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D38, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D38__MMDC_DRAM_D_38 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D39, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D39__MMDC_DRAM_D_39 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_DQM4, 0x0514, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_DQM4__MMDC_DRAM_DQM_4 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDQS4, 0x0518, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDQS4__MMDC_DRAM_SDQS_4 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D24, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D24__MMDC_DRAM_D_24 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D25, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D25__MMDC_DRAM_D_25 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D26, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D26__MMDC_DRAM_D_26 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D27, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D27__MMDC_DRAM_D_27 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D28, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D28__MMDC_DRAM_D_28 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D29, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D29__MMDC_DRAM_D_29 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDQS3, 0x051C, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDQS3__MMDC_DRAM_SDQS_3 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D30, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D30__MMDC_DRAM_D_30 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D31, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D31__MMDC_DRAM_D_31 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_DQM3, 0x0520, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_DQM3__MMDC_DRAM_DQM_3 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D16, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D16__MMDC_DRAM_D_16 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D17, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D17__MMDC_DRAM_D_17 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D18, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D18__MMDC_DRAM_D_18 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D19, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D19__MMDC_DRAM_D_19 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D20, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D20__MMDC_DRAM_D_20 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D21, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D21__MMDC_DRAM_D_21 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D22, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D22__MMDC_DRAM_D_22 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDQS2, 0x0524, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDQS2__MMDC_DRAM_SDQS_2 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D23, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D23__MMDC_DRAM_D_23 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_DQM2, 0x0528, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_DQM2__MMDC_DRAM_DQM_2 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A0, 0x052C, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A0__MMDC_DRAM_A_0 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A1, 0x0530, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A1__MMDC_DRAM_A_1 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A2, 0x0534, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A2__MMDC_DRAM_A_2 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A3, 0x0538, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A3__MMDC_DRAM_A_3 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A4, 0x053C, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A4__MMDC_DRAM_A_4 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A5, 0x0540, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A5__MMDC_DRAM_A_5 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A6, 0x0544, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A6__MMDC_DRAM_A_6 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A7, 0x0548, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A7__MMDC_DRAM_A_7 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A8, 0x054C, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A8__MMDC_DRAM_A_8 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A9, 0x0550, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A9__MMDC_DRAM_A_9 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A10, 0x0554, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A10__MMDC_DRAM_A_10 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A11, 0x0558, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A11__MMDC_DRAM_A_11 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A12, 0x055C, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A12__MMDC_DRAM_A_12 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A13, 0x0560, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A13__MMDC_DRAM_A_13 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A14, 0x0564, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A14__MMDC_DRAM_A_14 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_A15, 0x0568, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_A15__MMDC_DRAM_A_15 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_CAS, 0x056C, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_CAS__MMDC_DRAM_CAS */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_CS0, 0x0570, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_CS0__MMDC_DRAM_CS_0 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_CS1, 0x0574, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_CS1__MMDC_DRAM_CS_1 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_RAS, 0x0578, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_RAS__MMDC_DRAM_RAS */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_RESET, 0x057C, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_RESET__MMDC_DRAM_RESET */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDBA0, 0x0580, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDBA0__MMDC_DRAM_SDBA_0 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDBA1, 0x0584, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDBA1__MMDC_DRAM_SDBA_1 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDCLK_0, 0x0588, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDCLK_0__MMDC_DRAM_SDCLK0 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDBA2, 0x058C, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDBA2__MMDC_DRAM_SDBA_2 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDCKE0, 0x0590, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDCKE0__MMDC_DRAM_SDCKE_0 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDCLK_1, 0x0594, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDCLK_1__MMDC_DRAM_SDCLK1 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDCKE1, 0x0598, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDCKE1__MMDC_DRAM_SDCKE_1 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDODT0, 0x059C, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDODT0__MMDC_DRAM_ODT_0 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDODT1, 0x05A0, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDODT1__MMDC_DRAM_ODT_1 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDWE, 0x05A4, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDWE__MMDC_DRAM_SDWE */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D0, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D0__MMDC_DRAM_D_0 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D1, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D1__MMDC_DRAM_D_1 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D2, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D2__MMDC_DRAM_D_2 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D3, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D3__MMDC_DRAM_D_3 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D4, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D4__MMDC_DRAM_D_4 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D5, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D5__MMDC_DRAM_D_5 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDQS0, 0x05A8, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDQS0__MMDC_DRAM_SDQS_0 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D6, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D6__MMDC_DRAM_D_6 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D7, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D7__MMDC_DRAM_D_7 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_DQM0, 0x05AC, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_DQM0__MMDC_DRAM_DQM_0 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D8, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D8__MMDC_DRAM_D_8 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D9, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D9__MMDC_DRAM_D_9 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D10, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D10__MMDC_DRAM_D_10 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D11, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D11__MMDC_DRAM_D_11 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D12, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D12__MMDC_DRAM_D_12 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D13, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D13__MMDC_DRAM_D_13 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D14, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D14__MMDC_DRAM_D_14 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDQS1, 0x05B0, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDQS1__MMDC_DRAM_SDQS_1 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D15, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D15__MMDC_DRAM_D_15 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_DQM1, 0x05B4, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_DQM1__MMDC_DRAM_DQM_1 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D48, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D48__MMDC_DRAM_D_48 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D49, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D49__MMDC_DRAM_D_49 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D50, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D50__MMDC_DRAM_D_50 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D51, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D51__MMDC_DRAM_D_51 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D52, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D52__MMDC_DRAM_D_52 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D53, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D53__MMDC_DRAM_D_53 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D54, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D54__MMDC_DRAM_D_54 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D55, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D55__MMDC_DRAM_D_55 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDQS6, 0x05B8, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDQS6__MMDC_DRAM_SDQS_6 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_DQM6, 0x05BC, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_DQM6__MMDC_DRAM_DQM_6 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D56, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D56__MMDC_DRAM_D_56 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_SDQS7, 0x05C0, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_SDQS7__MMDC_DRAM_SDQS_7 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D57, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D57__MMDC_DRAM_D_57 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D58, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D58__MMDC_DRAM_D_58 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D59, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D59__MMDC_DRAM_D_59 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D60, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D60__MMDC_DRAM_D_60 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_DQM7, 0x05C4, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_DQM7__MMDC_DRAM_DQM_7 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D61, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D61__MMDC_DRAM_D_61 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D62, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D62__MMDC_DRAM_D_62 */
-	IMX_PIN_REG(MX6Q_PAD_DRAM_D63, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_DRAM_D63__MMDC_DRAM_D_63 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL0, 0x05C8, 0x01F8, 0, 0x07F4, 2), /* MX6Q_PAD_KEY_COL0__ECSPI1_SCLK */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL0, 0x05C8, 0x01F8, 1, 0x0854, 1), /* MX6Q_PAD_KEY_COL0__ENET_RDATA_3 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL0, 0x05C8, 0x01F8, 2, 0x07DC, 1), /* MX6Q_PAD_KEY_COL0__AUDMUX_AUD5_TXC */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL0, 0x05C8, 0x01F8, 3, 0x0000, 0), /* MX6Q_PAD_KEY_COL0__KPP_COL_0 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL0, 0x05C8, 0x01F8, 4, 0x0000, 0), /* MX6Q_PAD_KEY_COL0__UART4_TXD */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL0, 0x05C8, 0x01F8, 5, 0x0000, 0), /* MX6Q_PAD_KEY_COL0__GPIO_4_6 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL0, 0x05C8, 0x01F8, 6, 0x0000, 0), /* MX6Q_PAD_KEY_COL0__DCIC1_DCIC_OUT */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL0, 0x05C8, 0x01F8, 7, 0x0000, 0), /* MX6Q_PAD_KEY_COL0__SRC_ANY_PU_RST */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW0, 0x05CC, 0x01FC, 0, 0x07FC, 2), /* MX6Q_PAD_KEY_ROW0__ECSPI1_MOSI */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW0, 0x05CC, 0x01FC, 1, 0x0000, 0), /* MX6Q_PAD_KEY_ROW0__ENET_TDATA_3 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW0, 0x05CC, 0x01FC, 2, 0x07D0, 1), /* MX6Q_PAD_KEY_ROW0__AUDMUX_AUD5_TXD */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW0, 0x05CC, 0x01FC, 3, 0x0000, 0), /* MX6Q_PAD_KEY_ROW0__KPP_ROW_0 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW0, 0x05CC, 0x01FC, 4, 0x0938, 1), /* MX6Q_PAD_KEY_ROW0__UART4_RXD */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW0, 0x05CC, 0x01FC, 5, 0x0000, 0), /* MX6Q_PAD_KEY_ROW0__GPIO_4_7 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW0, 0x05CC, 0x01FC, 6, 0x0000, 0), /* MX6Q_PAD_KEY_ROW0__DCIC2_DCIC_OUT */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW0, 0x05CC, 0x01FC, 7, 0x0000, 0), /* MX6Q_PAD_KEY_ROW0__PL301_PER1_HADR_0 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL1, 0x05D0, 0x0200, 0, 0x07F8, 2), /* MX6Q_PAD_KEY_COL1__ECSPI1_MISO */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL1, 0x05D0, 0x0200, 1, 0x0840, 1), /* MX6Q_PAD_KEY_COL1__ENET_MDIO */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL1, 0x05D0, 0x0200, 2, 0x07E0, 1), /* MX6Q_PAD_KEY_COL1__AUDMUX_AUD5_TXFS */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL1, 0x05D0, 0x0200, 3, 0x0000, 0), /* MX6Q_PAD_KEY_COL1__KPP_COL_1 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL1, 0x05D0, 0x0200, 4, 0x0000, 0), /* MX6Q_PAD_KEY_COL1__UART5_TXD */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL1, 0x05D0, 0x0200, 5, 0x0000, 0), /* MX6Q_PAD_KEY_COL1__GPIO_4_8 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL1, 0x05D0, 0x0200, 6, 0x0000, 0), /* MX6Q_PAD_KEY_COL1__USDHC1_VSELECT */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL1, 0x05D0, 0x0200, 7, 0x0000, 0), /* MX6Q_PAD_KEY_COL1__PL301MX_PER1_HADR_1 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW1, 0x05D4, 0x0204, 0, 0x0800, 2), /* MX6Q_PAD_KEY_ROW1__ECSPI1_SS0 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW1, 0x05D4, 0x0204, 1, 0x0000, 0), /* MX6Q_PAD_KEY_ROW1__ENET_COL */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW1, 0x05D4, 0x0204, 2, 0x07CC, 1), /* MX6Q_PAD_KEY_ROW1__AUDMUX_AUD5_RXD */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW1, 0x05D4, 0x0204, 3, 0x0000, 0), /* MX6Q_PAD_KEY_ROW1__KPP_ROW_1 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW1, 0x05D4, 0x0204, 4, 0x0940, 1), /* MX6Q_PAD_KEY_ROW1__UART5_RXD */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW1, 0x05D4, 0x0204, 5, 0x0000, 0), /* MX6Q_PAD_KEY_ROW1__GPIO_4_9 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW1, 0x05D4, 0x0204, 6, 0x0000, 0), /* MX6Q_PAD_KEY_ROW1__USDHC2_VSELECT */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW1, 0x05D4, 0x0204, 7, 0x0000, 0), /* MX6Q_PAD_KEY_ROW1__PL301_PER1_HADDR_2 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL2, 0x05D8, 0x0208, 0, 0x0804, 2), /* MX6Q_PAD_KEY_COL2__ECSPI1_SS1 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL2, 0x05D8, 0x0208, 1, 0x0850, 1), /* MX6Q_PAD_KEY_COL2__ENET_RDATA_2 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL2, 0x05D8, 0x0208, 2, 0x0000, 0), /* MX6Q_PAD_KEY_COL2__CAN1_TXCAN */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL2, 0x05D8, 0x0208, 3, 0x0000, 0), /* MX6Q_PAD_KEY_COL2__KPP_COL_2 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL2, 0x05D8, 0x0208, 4, 0x0000, 0), /* MX6Q_PAD_KEY_COL2__ENET_MDC */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL2, 0x05D8, 0x0208, 5, 0x0000, 0), /* MX6Q_PAD_KEY_COL2__GPIO_4_10 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL2, 0x05D8, 0x0208, 6, 0x0000, 0), /* MX6Q_PAD_KEY_COL2__USBOH3_H1_PWRCTL_WKP */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL2, 0x05D8, 0x0208, 7, 0x0000, 0), /* MX6Q_PAD_KEY_COL2__PL301_PER1_HADDR_3 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW2, 0x05DC, 0x020C, 0, 0x0808, 1), /* MX6Q_PAD_KEY_ROW2__ECSPI1_SS2 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW2, 0x05DC, 0x020C, 1, 0x0000, 0), /* MX6Q_PAD_KEY_ROW2__ENET_TDATA_2 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW2, 0x05DC, 0x020C, 2, 0x07E4, 0), /* MX6Q_PAD_KEY_ROW2__CAN1_RXCAN */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW2, 0x05DC, 0x020C, 3, 0x0000, 0), /* MX6Q_PAD_KEY_ROW2__KPP_ROW_2 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW2, 0x05DC, 0x020C, 4, 0x0000, 0), /* MX6Q_PAD_KEY_ROW2__USDHC2_VSELECT */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW2, 0x05DC, 0x020C, 5, 0x0000, 0), /* MX6Q_PAD_KEY_ROW2__GPIO_4_11 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW2, 0x05DC, 0x020C, 6, 0x088C, 1), /* MX6Q_PAD_KEY_ROW2__HDMI_TX_CEC_LINE */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW2, 0x05DC, 0x020C, 7, 0x0000, 0), /* MX6Q_PAD_KEY_ROW2__PL301_PER1_HADR_4 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL3, 0x05E0, 0x0210, 0, 0x080C, 1), /* MX6Q_PAD_KEY_COL3__ECSPI1_SS3 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL3, 0x05E0, 0x0210, 1, 0x0000, 0), /* MX6Q_PAD_KEY_COL3__ENET_CRS */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL3, 0x05E0, 0x0210, 2, 0x0890, 1), /* MX6Q_PAD_KEY_COL3__HDMI_TX_DDC_SCL */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL3, 0x05E0, 0x0210, 3, 0x0000, 0), /* MX6Q_PAD_KEY_COL3__KPP_COL_3 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL3, 0x05E0, 0x0210, 4, 0x08A0, 1), /* MX6Q_PAD_KEY_COL3__I2C2_SCL */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL3, 0x05E0, 0x0210, 5, 0x0000, 0), /* MX6Q_PAD_KEY_COL3__GPIO_4_12 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL3, 0x05E0, 0x0210, 6, 0x0914, 2), /* MX6Q_PAD_KEY_COL3__SPDIF_IN1 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL3, 0x05E0, 0x0210, 7, 0x0000, 0), /* MX6Q_PAD_KEY_COL3__PL301_PER1_HADR_5 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW3, 0x05E4, 0x0214, 0, 0x0000, 0), /* MX6Q_PAD_KEY_ROW3__OSC32K_32K_OUT */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW3, 0x05E4, 0x0214, 1, 0x07B0, 0), /* MX6Q_PAD_KEY_ROW3__ASRC_ASRC_EXT_CLK */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW3, 0x05E4, 0x0214, 2, 0x0894, 1), /* MX6Q_PAD_KEY_ROW3__HDMI_TX_DDC_SDA */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW3, 0x05E4, 0x0214, 3, 0x0000, 0), /* MX6Q_PAD_KEY_ROW3__KPP_ROW_3 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW3, 0x05E4, 0x0214, 4, 0x08A4, 1), /* MX6Q_PAD_KEY_ROW3__I2C2_SDA */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW3, 0x05E4, 0x0214, 5, 0x0000, 0), /* MX6Q_PAD_KEY_ROW3__GPIO_4_13 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW3, 0x05E4, 0x0214, 6, 0x0000, 0), /* MX6Q_PAD_KEY_ROW3__USDHC1_VSELECT */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW3, 0x05E4, 0x0214, 7, 0x0000, 0), /* MX6Q_PAD_KEY_ROW3__PL301_PER1_HADR_6 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL4, 0x05E8, 0x0218, 0, 0x0000, 0), /* MX6Q_PAD_KEY_COL4__CAN2_TXCAN */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL4, 0x05E8, 0x0218, 1, 0x0000, 0), /* MX6Q_PAD_KEY_COL4__IPU1_SISG_4 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL4, 0x05E8, 0x0218, 2, 0x0944, 1), /* MX6Q_PAD_KEY_COL4__USBOH3_USBOTG_OC */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL4, 0x05E8, 0x0218, 3, 0x0000, 0), /* MX6Q_PAD_KEY_COL4__KPP_COL_4 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL4, 0x05E8, 0x0218, 4, 0x093C, 0), /* MX6Q_PAD_KEY_COL4__UART5_RTS */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL4, 0x05E8, 0x0218, 5, 0x0000, 0), /* MX6Q_PAD_KEY_COL4__GPIO_4_14 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL4, 0x05E8, 0x0218, 6, 0x0000, 0), /* MX6Q_PAD_KEY_COL4__MMDC_DEBUG_49 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_COL4, 0x05E8, 0x0218, 7, 0x0000, 0), /* MX6Q_PAD_KEY_COL4__PL301_PER1_HADDR_7 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW4, 0x05EC, 0x021C, 0, 0x07E8, 0), /* MX6Q_PAD_KEY_ROW4__CAN2_RXCAN */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW4, 0x05EC, 0x021C, 1, 0x0000, 0), /* MX6Q_PAD_KEY_ROW4__IPU1_SISG_5 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW4, 0x05EC, 0x021C, 2, 0x0000, 0), /* MX6Q_PAD_KEY_ROW4__USBOH3_USBOTG_PWR */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW4, 0x05EC, 0x021C, 3, 0x0000, 0), /* MX6Q_PAD_KEY_ROW4__KPP_ROW_4 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW4, 0x05EC, 0x021C, 4, 0x093C, 1), /* MX6Q_PAD_KEY_ROW4__UART5_CTS */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW4, 0x05EC, 0x021C, 5, 0x0000, 0), /* MX6Q_PAD_KEY_ROW4__GPIO_4_15 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW4, 0x05EC, 0x021C, 6, 0x0000, 0), /* MX6Q_PAD_KEY_ROW4__MMDC_DEBUG_50 */
-	IMX_PIN_REG(MX6Q_PAD_KEY_ROW4, 0x05EC, 0x021C, 7, 0x0000, 0), /* MX6Q_PAD_KEY_ROW4__PL301_PER1_HADR_8 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_0, 0x05F0, 0x0220, 0, 0x0000, 0), /* MX6Q_PAD_GPIO_0__CCM_CLKO */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_0, 0x05F0, 0x0220, 2, 0x08E8, 0), /* MX6Q_PAD_GPIO_0__KPP_COL_5 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_0, 0x05F0, 0x0220, 3, 0x07B0, 1), /* MX6Q_PAD_GPIO_0__ASRC_ASRC_EXT_CLK */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_0, 0x05F0, 0x0220, 4, 0x0000, 0), /* MX6Q_PAD_GPIO_0__EPIT1_EPITO */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_0, 0x05F0, 0x0220, 5, 0x0000, 0), /* MX6Q_PAD_GPIO_0__GPIO_1_0 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_0, 0x05F0, 0x0220, 6, 0x0000, 0), /* MX6Q_PAD_GPIO_0__USBOH3_USBH1_PWR */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_0, 0x05F0, 0x0220, 7, 0x0000, 0), /* MX6Q_PAD_GPIO_0__SNVS_HP_WRAP_SNVS_VIO5 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_1, 0x05F4, 0x0224, 0, 0x086C, 1), /* MX6Q_PAD_GPIO_1__ESAI1_SCKR */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_1, 0x05F4, 0x0224, 1, 0x0000, 0), /* MX6Q_PAD_GPIO_1__WDOG2_WDOG_B */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_1, 0x05F4, 0x0224, 2, 0x08F4, 0), /* MX6Q_PAD_GPIO_1__KPP_ROW_5 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_1, 0x05F4, 0x0224, 4, 0x0000, 0), /* MX6Q_PAD_GPIO_1__PWM2_PWMO */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_1, 0x05F4, 0x0224, 5, 0x0000, 0), /* MX6Q_PAD_GPIO_1__GPIO_1_1 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_1, 0x05F4, 0x0224, 6, 0x0000, 0), /* MX6Q_PAD_GPIO_1__USDHC1_CD */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_1, 0x05F4, 0x0224, 7, 0x0000, 0), /* MX6Q_PAD_GPIO_1__SRC_TESTER_ACK */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_9, 0x05F8, 0x0228, 0, 0x085C, 1), /* MX6Q_PAD_GPIO_9__ESAI1_FSR */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_9, 0x05F8, 0x0228, 1, 0x0000, 0), /* MX6Q_PAD_GPIO_9__WDOG1_WDOG_B */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_9, 0x05F8, 0x0228, 2, 0x08EC, 0), /* MX6Q_PAD_GPIO_9__KPP_COL_6 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_9, 0x05F8, 0x0228, 3, 0x0000, 0), /* MX6Q_PAD_GPIO_9__CCM_REF_EN_B */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_9, 0x05F8, 0x0228, 4, 0x0000, 0), /* MX6Q_PAD_GPIO_9__PWM1_PWMO */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_9, 0x05F8, 0x0228, 5, 0x0000, 0), /* MX6Q_PAD_GPIO_9__GPIO_1_9 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_9, 0x05F8, 0x0228, 6, 0x094C, 1), /* MX6Q_PAD_GPIO_9__USDHC1_WP */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_9, 0x05F8, 0x0228, 7, 0x0000, 0), /* MX6Q_PAD_GPIO_9__SRC_EARLY_RST */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_3, 0x05FC, 0x022C, 0, 0x0864, 1), /* MX6Q_PAD_GPIO_3__ESAI1_HCKR */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_3, 0x05FC, 0x022C, 1, 0x0000, 0), /* MX6Q_PAD_GPIO_3__OBSERVE_MUX_INT_OUT0 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_3, 0x05FC, 0x022C, 2, 0x08A8, 1), /* MX6Q_PAD_GPIO_3__I2C3_SCL */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_3, 0x05FC, 0x022C, 3, 0x0000, 0), /* MX6Q_PAD_GPIO_3__ANATOP_24M_OUT */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_3, 0x05FC, 0x022C, 4, 0x0000, 0), /* MX6Q_PAD_GPIO_3__CCM_CLKO2 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_3, 0x05FC, 0x022C, 5, 0x0000, 0), /* MX6Q_PAD_GPIO_3__GPIO_1_3 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_3, 0x05FC, 0x022C, 6, 0x0948, 1), /* MX6Q_PAD_GPIO_3__USBOH3_USBH1_OC */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_3, 0x05FC, 0x022C, 7, 0x0900, 1), /* MX6Q_PAD_GPIO_3__MLB_MLBCLK */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_6, 0x0600, 0x0230, 0, 0x0870, 1), /* MX6Q_PAD_GPIO_6__ESAI1_SCKT */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_6, 0x0600, 0x0230, 1, 0x0000, 0), /* MX6Q_PAD_GPIO_6__OBSERVE_MUX_INT_OUT1 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_6, 0x0600, 0x0230, 2, 0x08AC, 1), /* MX6Q_PAD_GPIO_6__I2C3_SDA */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_6, 0x0600, 0x0230, 3, 0x0000, 0), /* MX6Q_PAD_GPIO_6__CCM_CCM_OUT_0 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_6, 0x0600, 0x0230, 4, 0x0000, 0), /* MX6Q_PAD_GPIO_6__CSU_CSU_INT_DEB */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_6, 0x0600, 0x0230, 5, 0x0000, 0), /* MX6Q_PAD_GPIO_6__GPIO_1_6 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_6, 0x0600, 0x0230, 6, 0x0000, 0), /* MX6Q_PAD_GPIO_6__USDHC2_LCTL */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_6, 0x0600, 0x0230, 7, 0x0908, 1), /* MX6Q_PAD_GPIO_6__MLB_MLBSIG */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_2, 0x0604, 0x0234, 0, 0x0860, 1), /* MX6Q_PAD_GPIO_2__ESAI1_FST */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_2, 0x0604, 0x0234, 1, 0x0000, 0), /* MX6Q_PAD_GPIO_2__OBSERVE_MUX_INT_OUT2 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_2, 0x0604, 0x0234, 2, 0x08F8, 1), /* MX6Q_PAD_GPIO_2__KPP_ROW_6 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_2, 0x0604, 0x0234, 3, 0x0000, 0), /* MX6Q_PAD_GPIO_2__CCM_CCM_OUT_1 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_2, 0x0604, 0x0234, 4, 0x0000, 0), /* MX6Q_PAD_GPIO_2__CSU_CSU_ALARM_AUT_0 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_2, 0x0604, 0x0234, 5, 0x0000, 0), /* MX6Q_PAD_GPIO_2__GPIO_1_2 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_2, 0x0604, 0x0234, 6, 0x0000, 0), /* MX6Q_PAD_GPIO_2__USDHC2_WP */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_2, 0x0604, 0x0234, 7, 0x0904, 1), /* MX6Q_PAD_GPIO_2__MLB_MLBDAT */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_4, 0x0608, 0x0238, 0, 0x0868, 1), /* MX6Q_PAD_GPIO_4__ESAI1_HCKT */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_4, 0x0608, 0x0238, 1, 0x0000, 0), /* MX6Q_PAD_GPIO_4__OBSERVE_MUX_INT_OUT3 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_4, 0x0608, 0x0238, 2, 0x08F0, 1), /* MX6Q_PAD_GPIO_4__KPP_COL_7 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_4, 0x0608, 0x0238, 3, 0x0000, 0), /* MX6Q_PAD_GPIO_4__CCM_CCM_OUT_2 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_4, 0x0608, 0x0238, 4, 0x0000, 0), /* MX6Q_PAD_GPIO_4__CSU_CSU_ALARM_AUT_1 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_4, 0x0608, 0x0238, 5, 0x0000, 0), /* MX6Q_PAD_GPIO_4__GPIO_1_4 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_4, 0x0608, 0x0238, 6, 0x0000, 0), /* MX6Q_PAD_GPIO_4__USDHC2_CD */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_4, 0x0608, 0x0238, 7, 0x0000, 0), /* MX6Q_PAD_GPIO_4__OCOTP_CRL_WRAR_FUSE_LA */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_5, 0x060C, 0x023C, 0, 0x087C, 1), /* MX6Q_PAD_GPIO_5__ESAI1_TX2_RX3 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_5, 0x060C, 0x023C, 1, 0x0000, 0), /* MX6Q_PAD_GPIO_5__OBSERVE_MUX_INT_OUT4 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_5, 0x060C, 0x023C, 2, 0x08FC, 1), /* MX6Q_PAD_GPIO_5__KPP_ROW_7 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_5, 0x060C, 0x023C, 3, 0x0000, 0), /* MX6Q_PAD_GPIO_5__CCM_CLKO */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_5, 0x060C, 0x023C, 4, 0x0000, 0), /* MX6Q_PAD_GPIO_5__CSU_CSU_ALARM_AUT_2 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_5, 0x060C, 0x023C, 5, 0x0000, 0), /* MX6Q_PAD_GPIO_5__GPIO_1_5 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_5, 0x060C, 0x023C, 6, 0x08A8, 2), /* MX6Q_PAD_GPIO_5__I2C3_SCL */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_5, 0x060C, 0x023C, 7, 0x0000, 0), /* MX6Q_PAD_GPIO_5__CHEETAH_EVENTI */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_7, 0x0610, 0x0240, 0, 0x0884, 1), /* MX6Q_PAD_GPIO_7__ESAI1_TX4_RX1 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_7, 0x0610, 0x0240, 1, 0x0000, 0), /* MX6Q_PAD_GPIO_7__ECSPI5_RDY */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_7, 0x0610, 0x0240, 2, 0x0000, 0), /* MX6Q_PAD_GPIO_7__EPIT1_EPITO */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_7, 0x0610, 0x0240, 3, 0x0000, 0), /* MX6Q_PAD_GPIO_7__CAN1_TXCAN */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_7, 0x0610, 0x0240, 4, 0x0000, 0), /* MX6Q_PAD_GPIO_7__UART2_TXD */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_7, 0x0610, 0x0240, 5, 0x0000, 0), /* MX6Q_PAD_GPIO_7__GPIO_1_7 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_7, 0x0610, 0x0240, 6, 0x0000, 0), /* MX6Q_PAD_GPIO_7__SPDIF_PLOCK */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_7, 0x0610, 0x0240, 7, 0x0000, 0), /* MX6Q_PAD_GPIO_7__USBOH3_OTGUSB_HST_MODE */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_8, 0x0614, 0x0244, 0, 0x0888, 1), /* MX6Q_PAD_GPIO_8__ESAI1_TX5_RX0 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_8, 0x0614, 0x0244, 1, 0x0000, 0), /* MX6Q_PAD_GPIO_8__ANATOP_ANATOP_32K_OUT */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_8, 0x0614, 0x0244, 2, 0x0000, 0), /* MX6Q_PAD_GPIO_8__EPIT2_EPITO */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_8, 0x0614, 0x0244, 3, 0x07E4, 1), /* MX6Q_PAD_GPIO_8__CAN1_RXCAN */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_8, 0x0614, 0x0244, 4, 0x0928, 3), /* MX6Q_PAD_GPIO_8__UART2_RXD */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_8, 0x0614, 0x0244, 5, 0x0000, 0), /* MX6Q_PAD_GPIO_8__GPIO_1_8 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_8, 0x0614, 0x0244, 6, 0x0000, 0), /* MX6Q_PAD_GPIO_8__SPDIF_SRCLK */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_8, 0x0614, 0x0244, 7, 0x0000, 0), /* MX6Q_PAD_GPIO_8__USBOH3_OTG_PWRCTL_WAK */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_16, 0x0618, 0x0248, 0, 0x0880, 1), /* MX6Q_PAD_GPIO_16__ESAI1_TX3_RX2 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_16, 0x0618, 0x0248, 1, 0x0000, 0), /* MX6Q_PAD_GPIO_16__ENET_1588_EVENT2_IN */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_16, 0x0618, 0x0248, 2, 0x083C, 1), /* MX6Q_PAD_GPIO_16__ENET_ETHERNET_REF_OUT */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_16, 0x0618, 0x0248, 3, 0x0000, 0), /* MX6Q_PAD_GPIO_16__USDHC1_LCTL */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_16, 0x0618, 0x0248, 4, 0x0914, 3), /* MX6Q_PAD_GPIO_16__SPDIF_IN1 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_16, 0x0618, 0x0248, 5, 0x0000, 0), /* MX6Q_PAD_GPIO_16__GPIO_7_11 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_16, 0x0618, 0x0248, 6, 0x08AC, 2), /* MX6Q_PAD_GPIO_16__I2C3_SDA */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_16, 0x0618, 0x0248, 7, 0x0000, 0), /* MX6Q_PAD_GPIO_16__SJC_DE_B */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_17, 0x061C, 0x024C, 0, 0x0874, 0), /* MX6Q_PAD_GPIO_17__ESAI1_TX0 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_17, 0x061C, 0x024C, 1, 0x0000, 0), /* MX6Q_PAD_GPIO_17__ENET_1588_EVENT3_IN */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_17, 0x061C, 0x024C, 2, 0x07F0, 1), /* MX6Q_PAD_GPIO_17__CCM_PMIC_RDY */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_17, 0x061C, 0x024C, 3, 0x090C, 1), /* MX6Q_PAD_GPIO_17__SDMA_SDMA_EXT_EVENT_0 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_17, 0x061C, 0x024C, 4, 0x0000, 0), /* MX6Q_PAD_GPIO_17__SPDIF_OUT1 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_17, 0x061C, 0x024C, 5, 0x0000, 0), /* MX6Q_PAD_GPIO_17__GPIO_7_12 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_17, 0x061C, 0x024C, 7, 0x0000, 0), /* MX6Q_PAD_GPIO_17__SJC_JTAG_ACT */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_18, 0x0620, 0x0250, 0, 0x0878, 0), /* MX6Q_PAD_GPIO_18__ESAI1_TX1 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_18, 0x0620, 0x0250, 1, 0x0844, 1), /* MX6Q_PAD_GPIO_18__ENET_RX_CLK */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_18, 0x0620, 0x0250, 2, 0x0000, 0), /* MX6Q_PAD_GPIO_18__USDHC3_VSELECT */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_18, 0x0620, 0x0250, 3, 0x0910, 1), /* MX6Q_PAD_GPIO_18__SDMA_SDMA_EXT_EVENT_1 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_18, 0x0620, 0x0250, 4, 0x07B0, 2), /* MX6Q_PAD_GPIO_18__ASRC_ASRC_EXT_CLK */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_18, 0x0620, 0x0250, 5, 0x0000, 0), /* MX6Q_PAD_GPIO_18__GPIO_7_13 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_18, 0x0620, 0x0250, 6, 0x0000, 0), /* MX6Q_PAD_GPIO_18__SNVS_HP_WRA_SNVS_VIO5 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_18, 0x0620, 0x0250, 7, 0x0000, 0), /* MX6Q_PAD_GPIO_18__SRC_SYSTEM_RST */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_19, 0x0624, 0x0254, 0, 0x08E8, 1), /* MX6Q_PAD_GPIO_19__KPP_COL_5 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_19, 0x0624, 0x0254, 1, 0x0000, 0), /* MX6Q_PAD_GPIO_19__ENET_1588_EVENT0_OUT */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_19, 0x0624, 0x0254, 2, 0x0000, 0), /* MX6Q_PAD_GPIO_19__SPDIF_OUT1 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_19, 0x0624, 0x0254, 3, 0x0000, 0), /* MX6Q_PAD_GPIO_19__CCM_CLKO */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_19, 0x0624, 0x0254, 4, 0x0000, 0), /* MX6Q_PAD_GPIO_19__ECSPI1_RDY */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_19, 0x0624, 0x0254, 5, 0x0000, 0), /* MX6Q_PAD_GPIO_19__GPIO_4_5 */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_19, 0x0624, 0x0254, 6, 0x0000, 0), /* MX6Q_PAD_GPIO_19__ENET_TX_ER */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_19, 0x0624, 0x0254, 7, 0x0000, 0), /* MX6Q_PAD_GPIO_19__SRC_INT_BOOT */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_PIXCLK, 0x0628, 0x0258, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_PIXCLK__IPU1_CSI0_PIXCLK */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_PIXCLK, 0x0628, 0x0258, 2, 0x0000, 0), /* MX6Q_PAD_CSI0_PIXCLK__PCIE_CTRL_MUX_12 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_PIXCLK, 0x0628, 0x0258, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_PIXCLK__SDMA_DEBUG_PC_0 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_PIXCLK, 0x0628, 0x0258, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_PIXCLK__GPIO_5_18 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_PIXCLK, 0x0628, 0x0258, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_PIXCLK___MMDC_DEBUG_29 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_PIXCLK, 0x0628, 0x0258, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_PIXCLK__CHEETAH_EVENTO */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_MCLK, 0x062C, 0x025C, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_MCLK__IPU1_CSI0_HSYNC */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_MCLK, 0x062C, 0x025C, 2, 0x0000, 0), /* MX6Q_PAD_CSI0_MCLK__PCIE_CTRL_MUX_13 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_MCLK, 0x062C, 0x025C, 3, 0x0000, 0), /* MX6Q_PAD_CSI0_MCLK__CCM_CLKO */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_MCLK, 0x062C, 0x025C, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_MCLK__SDMA_DEBUG_PC_1 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_MCLK, 0x062C, 0x025C, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_MCLK__GPIO_5_19 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_MCLK, 0x062C, 0x025C, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_MCLK__MMDC_MMDC_DEBUG_30 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_MCLK, 0x062C, 0x025C, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_MCLK__CHEETAH_TRCTL */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DATA_EN, 0x0630, 0x0260, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DATA_EN__IPU1_CSI0_DA_EN */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DATA_EN, 0x0630, 0x0260, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DATA_EN__WEIM_WEIM_D_0 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DATA_EN, 0x0630, 0x0260, 2, 0x0000, 0), /* MX6Q_PAD_CSI0_DATA_EN__PCIE_CTRL_MUX_14 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DATA_EN, 0x0630, 0x0260, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_DATA_EN__SDMA_DEBUG_PC_2 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DATA_EN, 0x0630, 0x0260, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DATA_EN__GPIO_5_20 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DATA_EN, 0x0630, 0x0260, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DATA_EN__MMDC_DEBUG_31 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DATA_EN, 0x0630, 0x0260, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DATA_EN__CHEETAH_TRCLK */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_VSYNC, 0x0634, 0x0264, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_VSYNC__IPU1_CSI0_VSYNC */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_VSYNC, 0x0634, 0x0264, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_VSYNC__WEIM_WEIM_D_1 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_VSYNC, 0x0634, 0x0264, 2, 0x0000, 0), /* MX6Q_PAD_CSI0_VSYNC__PCIE_CTRL_MUX_15 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_VSYNC, 0x0634, 0x0264, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_VSYNC__SDMA_DEBUG_PC_3 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_VSYNC, 0x0634, 0x0264, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_VSYNC__GPIO_5_21 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_VSYNC, 0x0634, 0x0264, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_VSYNC__MMDC_DEBUG_32 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_VSYNC, 0x0634, 0x0264, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_VSYNC__CHEETAH_TRACE_0 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT4, 0x0638, 0x0268, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT4__IPU1_CSI0_D_4 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT4, 0x0638, 0x0268, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT4__WEIM_WEIM_D_2 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT4, 0x0638, 0x0268, 2, 0x07F4, 3), /* MX6Q_PAD_CSI0_DAT4__ECSPI1_SCLK */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT4, 0x0638, 0x0268, 3, 0x08E8, 2), /* MX6Q_PAD_CSI0_DAT4__KPP_COL_5 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT4, 0x0638, 0x0268, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT4__AUDMUX_AUD3_TXC */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT4, 0x0638, 0x0268, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT4__GPIO_5_22 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT4, 0x0638, 0x0268, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT4__MMDC_DEBUG_43 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT4, 0x0638, 0x0268, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT4__CHEETAH_TRACE_1 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT5, 0x063C, 0x026C, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT5__IPU1_CSI0_D_5 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT5, 0x063C, 0x026C, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT5__WEIM_WEIM_D_3 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT5, 0x063C, 0x026C, 2, 0x07FC, 3), /* MX6Q_PAD_CSI0_DAT5__ECSPI1_MOSI */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT5, 0x063C, 0x026C, 3, 0x08F4, 1), /* MX6Q_PAD_CSI0_DAT5__KPP_ROW_5 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT5, 0x063C, 0x026C, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT5__AUDMUX_AUD3_TXD */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT5, 0x063C, 0x026C, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT5__GPIO_5_23 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT5, 0x063C, 0x026C, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT5__MMDC_MMDC_DEBUG_44 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT5, 0x063C, 0x026C, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT5__CHEETAH_TRACE_2 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT6, 0x0640, 0x0270, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT6__IPU1_CSI0_D_6 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT6, 0x0640, 0x0270, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT6__WEIM_WEIM_D_4 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT6, 0x0640, 0x0270, 2, 0x07F8, 3), /* MX6Q_PAD_CSI0_DAT6__ECSPI1_MISO */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT6, 0x0640, 0x0270, 3, 0x08EC, 1), /* MX6Q_PAD_CSI0_DAT6__KPP_COL_6 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT6, 0x0640, 0x0270, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT6__AUDMUX_AUD3_TXFS */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT6, 0x0640, 0x0270, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT6__GPIO_5_24 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT6, 0x0640, 0x0270, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT6__MMDC_MMDC_DEBUG_45 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT6, 0x0640, 0x0270, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT6__CHEETAH_TRACE_3 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT7, 0x0644, 0x0274, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT7__IPU1_CSI0_D_7 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT7, 0x0644, 0x0274, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT7__WEIM_WEIM_D_5 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT7, 0x0644, 0x0274, 2, 0x0800, 3), /* MX6Q_PAD_CSI0_DAT7__ECSPI1_SS0 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT7, 0x0644, 0x0274, 3, 0x08F8, 2), /* MX6Q_PAD_CSI0_DAT7__KPP_ROW_6 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT7, 0x0644, 0x0274, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT7__AUDMUX_AUD3_RXD */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT7, 0x0644, 0x0274, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT7__GPIO_5_25 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT7, 0x0644, 0x0274, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT7__MMDC_MMDC_DEBUG_46 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT7, 0x0644, 0x0274, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT7__CHEETAH_TRACE_4 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT8, 0x0648, 0x0278, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT8__IPU1_CSI0_D_8 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT8, 0x0648, 0x0278, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT8__WEIM_WEIM_D_6 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT8, 0x0648, 0x0278, 2, 0x0810, 2), /* MX6Q_PAD_CSI0_DAT8__ECSPI2_SCLK */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT8, 0x0648, 0x0278, 3, 0x08F0, 2), /* MX6Q_PAD_CSI0_DAT8__KPP_COL_7 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT8, 0x0648, 0x0278, 4, 0x089C, 1), /* MX6Q_PAD_CSI0_DAT8__I2C1_SDA */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT8, 0x0648, 0x0278, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT8__GPIO_5_26 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT8, 0x0648, 0x0278, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT8__MMDC_MMDC_DEBUG_47 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT8, 0x0648, 0x0278, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT8__CHEETAH_TRACE_5 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT9, 0x064C, 0x027C, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT9__IPU1_CSI0_D_9 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT9, 0x064C, 0x027C, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT9__WEIM_WEIM_D_7 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT9, 0x064C, 0x027C, 2, 0x0818, 2), /* MX6Q_PAD_CSI0_DAT9__ECSPI2_MOSI */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT9, 0x064C, 0x027C, 3, 0x08FC, 2), /* MX6Q_PAD_CSI0_DAT9__KPP_ROW_7 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT9, 0x064C, 0x027C, 4, 0x0898, 1), /* MX6Q_PAD_CSI0_DAT9__I2C1_SCL */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT9, 0x064C, 0x027C, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT9__GPIO_5_27 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT9, 0x064C, 0x027C, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT9__MMDC_MMDC_DEBUG_48 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT9, 0x064C, 0x027C, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT9__CHEETAH_TRACE_6 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT10, 0x0650, 0x0280, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT10__IPU1_CSI0_D_10 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT10, 0x0650, 0x0280, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT10__AUDMUX_AUD3_RXC */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT10, 0x0650, 0x0280, 2, 0x0814, 2), /* MX6Q_PAD_CSI0_DAT10__ECSPI2_MISO */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT10, 0x0650, 0x0280, 3, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT10__UART1_TXD */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT10, 0x0650, 0x0280, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT10__SDMA_DEBUG_PC_4 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT10, 0x0650, 0x0280, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT10__GPIO_5_28 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT10, 0x0650, 0x0280, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT10__MMDC_MMDC_DEBUG_33 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT10, 0x0650, 0x0280, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT10__CHEETAH_TRACE_7 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT11, 0x0654, 0x0284, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT11__IPU1_CSI0_D_11 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT11, 0x0654, 0x0284, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT11__AUDMUX_AUD3_RXFS */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT11, 0x0654, 0x0284, 2, 0x081C, 2), /* MX6Q_PAD_CSI0_DAT11__ECSPI2_SS0 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT11, 0x0654, 0x0284, 3, 0x0920, 1), /* MX6Q_PAD_CSI0_DAT11__UART1_RXD */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT11, 0x0654, 0x0284, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT11__SDMA_DEBUG_PC_5 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT11, 0x0654, 0x0284, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT11__GPIO_5_29 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT11, 0x0654, 0x0284, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT11__MMDC_MMDC_DEBUG_34 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT11, 0x0654, 0x0284, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT11__CHEETAH_TRACE_8 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT12, 0x0658, 0x0288, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT12__IPU1_CSI0_D_12 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT12, 0x0658, 0x0288, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT12__WEIM_WEIM_D_8 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT12, 0x0658, 0x0288, 2, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT12__PCIE_CTRL_MUX_16 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT12, 0x0658, 0x0288, 3, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT12__UART4_TXD */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT12, 0x0658, 0x0288, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT12__SDMA_DEBUG_PC_6 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT12, 0x0658, 0x0288, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT12__GPIO_5_30 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT12, 0x0658, 0x0288, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT12__MMDC_MMDC_DEBUG_35 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT12, 0x0658, 0x0288, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT12__CHEETAH_TRACE_9 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT13, 0x065C, 0x028C, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT13__IPU1_CSI0_D_13 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT13, 0x065C, 0x028C, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT13__WEIM_WEIM_D_9 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT13, 0x065C, 0x028C, 2, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT13__PCIE_CTRL_MUX_17 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT13, 0x065C, 0x028C, 3, 0x0938, 3), /* MX6Q_PAD_CSI0_DAT13__UART4_RXD */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT13, 0x065C, 0x028C, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT13__SDMA_DEBUG_PC_7 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT13, 0x065C, 0x028C, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT13__GPIO_5_31 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT13, 0x065C, 0x028C, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT13__MMDC_MMDC_DEBUG_36 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT13, 0x065C, 0x028C, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT13__CHEETAH_TRACE_10 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT14, 0x0660, 0x0290, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT14__IPU1_CSI0_D_14 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT14, 0x0660, 0x0290, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT14__WEIM_WEIM_D_10 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT14, 0x0660, 0x0290, 2, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT14__PCIE_CTRL_MUX_18 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT14, 0x0660, 0x0290, 3, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT14__UART5_TXD */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT14, 0x0660, 0x0290, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT14__SDMA_DEBUG_PC_8 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT14, 0x0660, 0x0290, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT14__GPIO_6_0 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT14, 0x0660, 0x0290, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT14__MMDC_MMDC_DEBUG_37 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT14, 0x0660, 0x0290, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT14__CHEETAH_TRACE_11 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT15, 0x0664, 0x0294, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT15__IPU1_CSI0_D_15 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT15, 0x0664, 0x0294, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT15__WEIM_WEIM_D_11 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT15, 0x0664, 0x0294, 2, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT15__PCIE_CTRL_MUX_19 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT15, 0x0664, 0x0294, 3, 0x0940, 3), /* MX6Q_PAD_CSI0_DAT15__UART5_RXD */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT15, 0x0664, 0x0294, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT15__SDMA_DEBUG_PC_9 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT15, 0x0664, 0x0294, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT15__GPIO_6_1 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT15, 0x0664, 0x0294, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT15__MMDC_MMDC_DEBUG_38 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT15, 0x0664, 0x0294, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT15__CHEETAH_TRACE_12 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT16, 0x0668, 0x0298, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT16__IPU1_CSI0_D_16 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT16, 0x0668, 0x0298, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT16__WEIM_WEIM_D_12 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT16, 0x0668, 0x0298, 2, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT16__PCIE_CTRL_MUX_20 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT16, 0x0668, 0x0298, 3, 0x0934, 0), /* MX6Q_PAD_CSI0_DAT16__UART4_RTS */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT16, 0x0668, 0x0298, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT16__SDMA_DEBUG_PC_10 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT16, 0x0668, 0x0298, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT16__GPIO_6_2 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT16, 0x0668, 0x0298, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT16__MMDC_MMDC_DEBUG_39 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT16, 0x0668, 0x0298, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT16__CHEETAH_TRACE_13 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT17, 0x066C, 0x029C, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT17__IPU1_CSI0_D_17 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT17, 0x066C, 0x029C, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT17__WEIM_WEIM_D_13 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT17, 0x066C, 0x029C, 2, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT17__PCIE_CTRL_MUX_21 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT17, 0x066C, 0x029C, 3, 0x0934, 1), /* MX6Q_PAD_CSI0_DAT17__UART4_CTS */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT17, 0x066C, 0x029C, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT17__SDMA_DEBUG_PC_11 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT17, 0x066C, 0x029C, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT17__GPIO_6_3 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT17, 0x066C, 0x029C, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT17__MMDC_MMDC_DEBUG_40 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT17, 0x066C, 0x029C, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT17__CHEETAH_TRACE_14 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT18, 0x0670, 0x02A0, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT18__IPU1_CSI0_D_18 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT18, 0x0670, 0x02A0, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT18__WEIM_WEIM_D_14 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT18, 0x0670, 0x02A0, 2, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT18__PCIE_CTRL_MUX_22 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT18, 0x0670, 0x02A0, 3, 0x093C, 2), /* MX6Q_PAD_CSI0_DAT18__UART5_RTS */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT18, 0x0670, 0x02A0, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT18__SDMA_DEBUG_PC_12 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT18, 0x0670, 0x02A0, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT18__GPIO_6_4 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT18, 0x0670, 0x02A0, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT18__MMDC_MMDC_DEBUG_41 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT18, 0x0670, 0x02A0, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT18__CHEETAH_TRACE_15 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT19, 0x0674, 0x02A4, 0, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT19__IPU1_CSI0_D_19 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT19, 0x0674, 0x02A4, 1, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT19__WEIM_WEIM_D_15 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT19, 0x0674, 0x02A4, 2, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT19__PCIE_CTRL_MUX_23 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT19, 0x0674, 0x02A4, 3, 0x093C, 3), /* MX6Q_PAD_CSI0_DAT19__UART5_CTS */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT19, 0x0674, 0x02A4, 4, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT19__SDMA_DEBUG_PC_13 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT19, 0x0674, 0x02A4, 5, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT19__GPIO_6_5 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT19, 0x0674, 0x02A4, 6, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT19__MMDC_MMDC_DEBUG_42 */
-	IMX_PIN_REG(MX6Q_PAD_CSI0_DAT19, 0x0674, 0x02A4, 7, 0x0000, 0), /* MX6Q_PAD_CSI0_DAT19__ANATOP_TESTO_9 */
-	IMX_PIN_REG(MX6Q_PAD_JTAG_TMS, 0x0678, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_JTAG_TMS__SJC_TMS */
-	IMX_PIN_REG(MX6Q_PAD_JTAG_MOD, 0x067C, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_JTAG_MOD__SJC_MOD */
-	IMX_PIN_REG(MX6Q_PAD_JTAG_TRSTB, 0x0680, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_JTAG_TRSTB__SJC_TRSTB */
-	IMX_PIN_REG(MX6Q_PAD_JTAG_TDI, 0x0684, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_JTAG_TDI__SJC_TDI */
-	IMX_PIN_REG(MX6Q_PAD_JTAG_TCK, 0x0688, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_JTAG_TCK__SJC_TCK */
-	IMX_PIN_REG(MX6Q_PAD_JTAG_TDO, 0x068C, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_JTAG_TDO__SJC_TDO */
-	IMX_PIN_REG(MX6Q_PAD_LVDS1_TX3_P, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_LVDS1_TX3_P__LDB_LVDS1_TX3 */
-	IMX_PIN_REG(MX6Q_PAD_LVDS1_TX2_P, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_LVDS1_TX2_P__LDB_LVDS1_TX2 */
-	IMX_PIN_REG(MX6Q_PAD_LVDS1_CLK_P, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_LVDS1_CLK_P__LDB_LVDS1_CLK */
-	IMX_PIN_REG(MX6Q_PAD_LVDS1_TX1_P, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_LVDS1_TX1_P__LDB_LVDS1_TX1 */
-	IMX_PIN_REG(MX6Q_PAD_LVDS1_TX0_P, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_LVDS1_TX0_P__LDB_LVDS1_TX0 */
-	IMX_PIN_REG(MX6Q_PAD_LVDS0_TX3_P, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_LVDS0_TX3_P__LDB_LVDS0_TX3 */
-	IMX_PIN_REG(MX6Q_PAD_LVDS0_CLK_P, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_LVDS0_CLK_P__LDB_LVDS0_CLK */
-	IMX_PIN_REG(MX6Q_PAD_LVDS0_TX2_P, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_LVDS0_TX2_P__LDB_LVDS0_TX2 */
-	IMX_PIN_REG(MX6Q_PAD_LVDS0_TX1_P, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_LVDS0_TX1_P__LDB_LVDS0_TX1 */
-	IMX_PIN_REG(MX6Q_PAD_LVDS0_TX0_P, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_LVDS0_TX0_P__LDB_LVDS0_TX0 */
-	IMX_PIN_REG(MX6Q_PAD_TAMPER, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_TAMPER__SNVS_LP_WRAP_SNVS_TD1 */
-	IMX_PIN_REG(MX6Q_PAD_PMIC_ON_REQ, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_PMIC_ON_REQ__SNVS_LPWRAP_WKALM */
-	IMX_PIN_REG(MX6Q_PAD_PMIC_STBY_REQ, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_PMIC_STBY_REQ__CCM_PMIC_STBYRQ */
-	IMX_PIN_REG(MX6Q_PAD_POR_B, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_POR_B__SRC_POR_B */
-	IMX_PIN_REG(MX6Q_PAD_BOOT_MODE1, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_BOOT_MODE1__SRC_BOOT_MODE_1 */
-	IMX_PIN_REG(MX6Q_PAD_RESET_IN_B, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_RESET_IN_B__SRC_RESET_B */
-	IMX_PIN_REG(MX6Q_PAD_BOOT_MODE0, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_BOOT_MODE0__SRC_BOOT_MODE_0 */
-	IMX_PIN_REG(MX6Q_PAD_TEST_MODE, NO_PAD, NO_MUX, 0, 0x0000, 0), /* MX6Q_PAD_TEST_MODE__TCU_TEST_MODE */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT7, 0x0690, 0x02A8, 0, 0x0000, 0), /* MX6Q_PAD_SD3_DAT7__USDHC3_DAT7 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT7, 0x0690, 0x02A8, 1, 0x0000, 0), /* MX6Q_PAD_SD3_DAT7__UART1_TXD */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT7, 0x0690, 0x02A8, 2, 0x0000, 0), /* MX6Q_PAD_SD3_DAT7__PCIE_CTRL_MUX_24 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT7, 0x0690, 0x02A8, 3, 0x0000, 0), /* MX6Q_PAD_SD3_DAT7__USBOH3_UH3_DFD_OUT_0 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT7, 0x0690, 0x02A8, 4, 0x0000, 0), /* MX6Q_PAD_SD3_DAT7__USBOH3_UH2_DFD_OUT_0 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT7, 0x0690, 0x02A8, 5, 0x0000, 0), /* MX6Q_PAD_SD3_DAT7__GPIO_6_17 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT7, 0x0690, 0x02A8, 6, 0x0000, 0), /* MX6Q_PAD_SD3_DAT7__MIPI_CORE_DPHY_IN_12 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT7, 0x0690, 0x02A8, 7, 0x0000, 0), /* MX6Q_PAD_SD3_DAT7__USBPHY2_CLK20DIV */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT6, 0x0694, 0x02AC, 0, 0x0000, 0), /* MX6Q_PAD_SD3_DAT6__USDHC3_DAT6 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT6, 0x0694, 0x02AC, 1, 0x0920, 3), /* MX6Q_PAD_SD3_DAT6__UART1_RXD */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT6, 0x0694, 0x02AC, 2, 0x0000, 0), /* MX6Q_PAD_SD3_DAT6__PCIE_CTRL_MUX_25 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT6, 0x0694, 0x02AC, 3, 0x0000, 0), /* MX6Q_PAD_SD3_DAT6__USBOH3_UH3_DFD_OUT_1 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT6, 0x0694, 0x02AC, 4, 0x0000, 0), /* MX6Q_PAD_SD3_DAT6__USBOH3_UH2_DFD_OUT_1 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT6, 0x0694, 0x02AC, 5, 0x0000, 0), /* MX6Q_PAD_SD3_DAT6__GPIO_6_18 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT6, 0x0694, 0x02AC, 6, 0x0000, 0), /* MX6Q_PAD_SD3_DAT6__MIPI_CORE_DPHY_IN_13 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT6, 0x0694, 0x02AC, 7, 0x0000, 0), /* MX6Q_PAD_SD3_DAT6__ANATOP_TESTO_10 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT5, 0x0698, 0x02B0, 0, 0x0000, 0), /* MX6Q_PAD_SD3_DAT5__USDHC3_DAT5 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT5, 0x0698, 0x02B0, 1, 0x0000, 0), /* MX6Q_PAD_SD3_DAT5__UART2_TXD */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT5, 0x0698, 0x02B0, 2, 0x0000, 0), /* MX6Q_PAD_SD3_DAT5__PCIE_CTRL_MUX_26 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT5, 0x0698, 0x02B0, 3, 0x0000, 0), /* MX6Q_PAD_SD3_DAT5__USBOH3_UH3_DFD_OUT_2 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT5, 0x0698, 0x02B0, 4, 0x0000, 0), /* MX6Q_PAD_SD3_DAT5__USBOH3_UH2_DFD_OUT_2 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT5, 0x0698, 0x02B0, 5, 0x0000, 0), /* MX6Q_PAD_SD3_DAT5__GPIO_7_0 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT5, 0x0698, 0x02B0, 6, 0x0000, 0), /* MX6Q_PAD_SD3_DAT5__MIPI_CORE_DPHY_IN_14 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT5, 0x0698, 0x02B0, 7, 0x0000, 0), /* MX6Q_PAD_SD3_DAT5__ANATOP_TESTO_11 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT4, 0x069C, 0x02B4, 0, 0x0000, 0), /* MX6Q_PAD_SD3_DAT4__USDHC3_DAT4 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT4, 0x069C, 0x02B4, 1, 0x0928, 5), /* MX6Q_PAD_SD3_DAT4__UART2_RXD */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT4, 0x069C, 0x02B4, 2, 0x0000, 0), /* MX6Q_PAD_SD3_DAT4__PCIE_CTRL_MUX_27 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT4, 0x069C, 0x02B4, 3, 0x0000, 0), /* MX6Q_PAD_SD3_DAT4__USBOH3_UH3_DFD_OUT_3 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT4, 0x069C, 0x02B4, 4, 0x0000, 0), /* MX6Q_PAD_SD3_DAT4__USBOH3_UH2_DFD_OUT_3 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT4, 0x069C, 0x02B4, 5, 0x0000, 0), /* MX6Q_PAD_SD3_DAT4__GPIO_7_1 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT4, 0x069C, 0x02B4, 6, 0x0000, 0), /* MX6Q_PAD_SD3_DAT4__MIPI_CORE_DPHY_IN_15 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT4, 0x069C, 0x02B4, 7, 0x0000, 0), /* MX6Q_PAD_SD3_DAT4__ANATOP_TESTO_12 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CMD, 0x06A0, 0x02B8, 0, 0x0000, 0), /* MX6Q_PAD_SD3_CMD__USDHC3_CMD */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CMD, 0x06A0, 0x02B8, 1, 0x0924, 2), /* MX6Q_PAD_SD3_CMD__UART2_CTS */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CMD, 0x06A0, 0x02B8, 2, 0x0000, 0), /* MX6Q_PAD_SD3_CMD__CAN1_TXCAN */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CMD, 0x06A0, 0x02B8, 3, 0x0000, 0), /* MX6Q_PAD_SD3_CMD__USBOH3_UH3_DFD_OUT_4 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CMD, 0x06A0, 0x02B8, 4, 0x0000, 0), /* MX6Q_PAD_SD3_CMD__USBOH3_UH2_DFD_OUT_4 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CMD, 0x06A0, 0x02B8, 5, 0x0000, 0), /* MX6Q_PAD_SD3_CMD__GPIO_7_2 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CMD, 0x06A0, 0x02B8, 6, 0x0000, 0), /* MX6Q_PAD_SD3_CMD__MIPI_CORE_DPHY_IN_16 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CMD, 0x06A0, 0x02B8, 7, 0x0000, 0), /* MX6Q_PAD_SD3_CMD__ANATOP_TESTO_13 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CLK, 0x06A4, 0x02BC, 0, 0x0000, 0), /* MX6Q_PAD_SD3_CLK__USDHC3_CLK */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CLK, 0x06A4, 0x02BC, 1, 0x0924, 3), /* MX6Q_PAD_SD3_CLK__UART2_RTS */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CLK, 0x06A4, 0x02BC, 2, 0x07E4, 2), /* MX6Q_PAD_SD3_CLK__CAN1_RXCAN */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CLK, 0x06A4, 0x02BC, 3, 0x0000, 0), /* MX6Q_PAD_SD3_CLK__USBOH3_UH3_DFD_OUT_5 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CLK, 0x06A4, 0x02BC, 4, 0x0000, 0), /* MX6Q_PAD_SD3_CLK__USBOH3_UH2_DFD_OUT_5 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CLK, 0x06A4, 0x02BC, 5, 0x0000, 0), /* MX6Q_PAD_SD3_CLK__GPIO_7_3 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CLK, 0x06A4, 0x02BC, 6, 0x0000, 0), /* MX6Q_PAD_SD3_CLK__MIPI_CORE_DPHY_IN_17 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_CLK, 0x06A4, 0x02BC, 7, 0x0000, 0), /* MX6Q_PAD_SD3_CLK__ANATOP_TESTO_14 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT0, 0x06A8, 0x02C0, 0, 0x0000, 0), /* MX6Q_PAD_SD3_DAT0__USDHC3_DAT0 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT0, 0x06A8, 0x02C0, 1, 0x091C, 2), /* MX6Q_PAD_SD3_DAT0__UART1_CTS */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT0, 0x06A8, 0x02C0, 2, 0x0000, 0), /* MX6Q_PAD_SD3_DAT0__CAN2_TXCAN */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT0, 0x06A8, 0x02C0, 3, 0x0000, 0), /* MX6Q_PAD_SD3_DAT0__USBOH3_UH3_DFD_OUT_6 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT0, 0x06A8, 0x02C0, 4, 0x0000, 0), /* MX6Q_PAD_SD3_DAT0__USBOH3_UH2_DFD_OUT_6 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT0, 0x06A8, 0x02C0, 5, 0x0000, 0), /* MX6Q_PAD_SD3_DAT0__GPIO_7_4 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT0, 0x06A8, 0x02C0, 6, 0x0000, 0), /* MX6Q_PAD_SD3_DAT0__MIPI_CORE_DPHY_IN_18 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT0, 0x06A8, 0x02C0, 7, 0x0000, 0), /* MX6Q_PAD_SD3_DAT0__ANATOP_TESTO_15 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT1, 0x06AC, 0x02C4, 0, 0x0000, 0), /* MX6Q_PAD_SD3_DAT1__USDHC3_DAT1 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT1, 0x06AC, 0x02C4, 1, 0x091C, 3), /* MX6Q_PAD_SD3_DAT1__UART1_RTS */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT1, 0x06AC, 0x02C4, 2, 0x07E8, 1), /* MX6Q_PAD_SD3_DAT1__CAN2_RXCAN */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT1, 0x06AC, 0x02C4, 3, 0x0000, 0), /* MX6Q_PAD_SD3_DAT1__USBOH3_UH3_DFD_OUT_7 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT1, 0x06AC, 0x02C4, 4, 0x0000, 0), /* MX6Q_PAD_SD3_DAT1__USBOH3_UH2_DFD_OUT_7 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT1, 0x06AC, 0x02C4, 5, 0x0000, 0), /* MX6Q_PAD_SD3_DAT1__GPIO_7_5 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT1, 0x06AC, 0x02C4, 6, 0x0000, 0), /* MX6Q_PAD_SD3_DAT1__MIPI_CORE_DPHY_IN_19 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT1, 0x06AC, 0x02C4, 7, 0x0000, 0), /* MX6Q_PAD_SD3_DAT1__ANATOP_TESTI_0 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT2, 0x06B0, 0x02C8, 0, 0x0000, 0), /* MX6Q_PAD_SD3_DAT2__USDHC3_DAT2 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT2, 0x06B0, 0x02C8, 2, 0x0000, 0), /* MX6Q_PAD_SD3_DAT2__PCIE_CTRL_MUX_28 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT2, 0x06B0, 0x02C8, 3, 0x0000, 0), /* MX6Q_PAD_SD3_DAT2__USBOH3_UH3_DFD_OUT_8 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT2, 0x06B0, 0x02C8, 4, 0x0000, 0), /* MX6Q_PAD_SD3_DAT2__USBOH3_UH2_DFD_OUT_8 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT2, 0x06B0, 0x02C8, 5, 0x0000, 0), /* MX6Q_PAD_SD3_DAT2__GPIO_7_6 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT2, 0x06B0, 0x02C8, 6, 0x0000, 0), /* MX6Q_PAD_SD3_DAT2__MIPI_CORE_DPHY_IN_20 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT2, 0x06B0, 0x02C8, 7, 0x0000, 0), /* MX6Q_PAD_SD3_DAT2__ANATOP_TESTI_1 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT3, 0x06B4, 0x02CC, 0, 0x0000, 0), /* MX6Q_PAD_SD3_DAT3__USDHC3_DAT3 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT3, 0x06B4, 0x02CC, 1, 0x092C, 4), /* MX6Q_PAD_SD3_DAT3__UART3_CTS */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT3, 0x06B4, 0x02CC, 2, 0x0000, 0), /* MX6Q_PAD_SD3_DAT3__PCIE_CTRL_MUX_29 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT3, 0x06B4, 0x02CC, 3, 0x0000, 0), /* MX6Q_PAD_SD3_DAT3__USBOH3_UH3_DFD_OUT_9 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT3, 0x06B4, 0x02CC, 4, 0x0000, 0), /* MX6Q_PAD_SD3_DAT3__USBOH3_UH2_DFD_OUT_9 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT3, 0x06B4, 0x02CC, 5, 0x0000, 0), /* MX6Q_PAD_SD3_DAT3__GPIO_7_7 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT3, 0x06B4, 0x02CC, 6, 0x0000, 0), /* MX6Q_PAD_SD3_DAT3__MIPI_CORE_DPHY_IN_21 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_DAT3, 0x06B4, 0x02CC, 7, 0x0000, 0), /* MX6Q_PAD_SD3_DAT3__ANATOP_TESTI_2 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_RST, 0x06B8, 0x02D0, 0, 0x0000, 0), /* MX6Q_PAD_SD3_RST__USDHC3_RST */
-	IMX_PIN_REG(MX6Q_PAD_SD3_RST, 0x06B8, 0x02D0, 1, 0x092C, 5), /* MX6Q_PAD_SD3_RST__UART3_RTS */
-	IMX_PIN_REG(MX6Q_PAD_SD3_RST, 0x06B8, 0x02D0, 2, 0x0000, 0), /* MX6Q_PAD_SD3_RST__PCIE_CTRL_MUX_30 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_RST, 0x06B8, 0x02D0, 3, 0x0000, 0), /* MX6Q_PAD_SD3_RST__USBOH3_UH3_DFD_OUT_10 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_RST, 0x06B8, 0x02D0, 4, 0x0000, 0), /* MX6Q_PAD_SD3_RST__USBOH3_UH2_DFD_OUT_10 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_RST, 0x06B8, 0x02D0, 5, 0x0000, 0), /* MX6Q_PAD_SD3_RST__GPIO_7_8 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_RST, 0x06B8, 0x02D0, 6, 0x0000, 0), /* MX6Q_PAD_SD3_RST__MIPI_CORE_DPHY_IN_22 */
-	IMX_PIN_REG(MX6Q_PAD_SD3_RST, 0x06B8, 0x02D0, 7, 0x0000, 0), /* MX6Q_PAD_SD3_RST__ANATOP_ANATOP_TESTI_3 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CLE, 0x06BC, 0x02D4, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_CLE__RAWNAND_CLE */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CLE, 0x06BC, 0x02D4, 1, 0x0000, 0), /* MX6Q_PAD_NANDF_CLE__IPU2_SISG_4 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CLE, 0x06BC, 0x02D4, 2, 0x0000, 0), /* MX6Q_PAD_NANDF_CLE__PCIE_CTRL_MUX_31 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CLE, 0x06BC, 0x02D4, 3, 0x0000, 0), /* MX6Q_PAD_NANDF_CLE__USBOH3_UH3_DFD_OT11 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CLE, 0x06BC, 0x02D4, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_CLE__USBOH3_UH2_DFD_OT11 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CLE, 0x06BC, 0x02D4, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_CLE__GPIO_6_7 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CLE, 0x06BC, 0x02D4, 6, 0x0000, 0), /* MX6Q_PAD_NANDF_CLE__MIPI_CORE_DPHY_IN23 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CLE, 0x06BC, 0x02D4, 7, 0x0000, 0), /* MX6Q_PAD_NANDF_CLE__TPSMP_HTRANS_0 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_ALE, 0x06C0, 0x02D8, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_ALE__RAWNAND_ALE */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_ALE, 0x06C0, 0x02D8, 1, 0x0000, 0), /* MX6Q_PAD_NANDF_ALE__USDHC4_RST */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_ALE, 0x06C0, 0x02D8, 2, 0x0000, 0), /* MX6Q_PAD_NANDF_ALE__PCIE_CTRL_MUX_0 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_ALE, 0x06C0, 0x02D8, 3, 0x0000, 0), /* MX6Q_PAD_NANDF_ALE__USBOH3_UH3_DFD_OT12 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_ALE, 0x06C0, 0x02D8, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_ALE__USBOH3_UH2_DFD_OT12 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_ALE, 0x06C0, 0x02D8, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_ALE__GPIO_6_8 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_ALE, 0x06C0, 0x02D8, 6, 0x0000, 0), /* MX6Q_PAD_NANDF_ALE__MIPI_CR_DPHY_IN_24 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_ALE, 0x06C0, 0x02D8, 7, 0x0000, 0), /* MX6Q_PAD_NANDF_ALE__TPSMP_HTRANS_1 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_WP_B, 0x06C4, 0x02DC, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_WP_B__RAWNAND_RESETN */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_WP_B, 0x06C4, 0x02DC, 1, 0x0000, 0), /* MX6Q_PAD_NANDF_WP_B__IPU2_SISG_5 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_WP_B, 0x06C4, 0x02DC, 2, 0x0000, 0), /* MX6Q_PAD_NANDF_WP_B__PCIE_CTRL__MUX_1 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_WP_B, 0x06C4, 0x02DC, 3, 0x0000, 0), /* MX6Q_PAD_NANDF_WP_B__USBOH3_UH3_DFDOT13 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_WP_B, 0x06C4, 0x02DC, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_WP_B__USBOH3_UH2_DFDOT13 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_WP_B, 0x06C4, 0x02DC, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_WP_B__GPIO_6_9 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_WP_B, 0x06C4, 0x02DC, 6, 0x0000, 0), /* MX6Q_PAD_NANDF_WP_B__MIPI_CR_DPHY_OUT32 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_WP_B, 0x06C4, 0x02DC, 7, 0x0000, 0), /* MX6Q_PAD_NANDF_WP_B__PL301_PER1_HSIZE_0 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_RB0, 0x06C8, 0x02E0, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_RB0__RAWNAND_READY0 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_RB0, 0x06C8, 0x02E0, 1, 0x0000, 0), /* MX6Q_PAD_NANDF_RB0__IPU2_DI0_PIN1 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_RB0, 0x06C8, 0x02E0, 2, 0x0000, 0), /* MX6Q_PAD_NANDF_RB0__PCIE_CTRL_MUX_2 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_RB0, 0x06C8, 0x02E0, 3, 0x0000, 0), /* MX6Q_PAD_NANDF_RB0__USBOH3_UH3_DFD_OT14 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_RB0, 0x06C8, 0x02E0, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_RB0__USBOH3_UH2_DFD_OT14 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_RB0, 0x06C8, 0x02E0, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_RB0__GPIO_6_10 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_RB0, 0x06C8, 0x02E0, 6, 0x0000, 0), /* MX6Q_PAD_NANDF_RB0__MIPI_CR_DPHY_OUT_33 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_RB0, 0x06C8, 0x02E0, 7, 0x0000, 0), /* MX6Q_PAD_NANDF_RB0__PL301_PER1_HSIZE_1 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS0, 0x06CC, 0x02E4, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_CS0__RAWNAND_CE0N */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS0, 0x06CC, 0x02E4, 3, 0x0000, 0), /* MX6Q_PAD_NANDF_CS0__USBOH3_UH3_DFD_OT15 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS0, 0x06CC, 0x02E4, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_CS0__USBOH3_UH2_DFD_OT15 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS0, 0x06CC, 0x02E4, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_CS0__GPIO_6_11 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS0, 0x06CC, 0x02E4, 7, 0x0000, 0), /* MX6Q_PAD_NANDF_CS0__PL301_PER1_HSIZE_2 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS1, 0x06D0, 0x02E8, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_CS1__RAWNAND_CE1N */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS1, 0x06D0, 0x02E8, 1, 0x0000, 0), /* MX6Q_PAD_NANDF_CS1__USDHC4_VSELECT */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS1, 0x06D0, 0x02E8, 2, 0x0000, 0), /* MX6Q_PAD_NANDF_CS1__USDHC3_VSELECT */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS1, 0x06D0, 0x02E8, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_CS1__PCIE_CTRL_MUX_3 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS1, 0x06D0, 0x02E8, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_CS1__GPIO_6_14 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS1, 0x06D0, 0x02E8, 7, 0x0000, 0), /* MX6Q_PAD_NANDF_CS1__PL301_PER1_HRDYOUT */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS2, 0x06D4, 0x02EC, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_CS2__RAWNAND_CE2N */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS2, 0x06D4, 0x02EC, 1, 0x0000, 0), /* MX6Q_PAD_NANDF_CS2__IPU1_SISG_0 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS2, 0x06D4, 0x02EC, 2, 0x0874, 1), /* MX6Q_PAD_NANDF_CS2__ESAI1_TX0 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS2, 0x06D4, 0x02EC, 3, 0x0000, 0), /* MX6Q_PAD_NANDF_CS2__WEIM_WEIM_CRE */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS2, 0x06D4, 0x02EC, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_CS2__CCM_CLKO2 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS2, 0x06D4, 0x02EC, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_CS2__GPIO_6_15 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS2, 0x06D4, 0x02EC, 6, 0x0000, 0), /* MX6Q_PAD_NANDF_CS2__IPU2_SISG_0 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS3, 0x06D8, 0x02F0, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_CS3__RAWNAND_CE3N */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS3, 0x06D8, 0x02F0, 1, 0x0000, 0), /* MX6Q_PAD_NANDF_CS3__IPU1_SISG_1 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS3, 0x06D8, 0x02F0, 2, 0x0878, 1), /* MX6Q_PAD_NANDF_CS3__ESAI1_TX1 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS3, 0x06D8, 0x02F0, 3, 0x0000, 0), /* MX6Q_PAD_NANDF_CS3__WEIM_WEIM_A_26 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS3, 0x06D8, 0x02F0, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_CS3__PCIE_CTRL_MUX_4 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS3, 0x06D8, 0x02F0, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_CS3__GPIO_6_16 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS3, 0x06D8, 0x02F0, 6, 0x0000, 0), /* MX6Q_PAD_NANDF_CS3__IPU2_SISG_1 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_CS3, 0x06D8, 0x02F0, 7, 0x0000, 0), /* MX6Q_PAD_NANDF_CS3__TPSMP_CLK */
-	IMX_PIN_REG(MX6Q_PAD_SD4_CMD, 0x06DC, 0x02F4, 0, 0x0000, 0), /* MX6Q_PAD_SD4_CMD__USDHC4_CMD */
-	IMX_PIN_REG(MX6Q_PAD_SD4_CMD, 0x06DC, 0x02F4, 1, 0x0000, 0), /* MX6Q_PAD_SD4_CMD__RAWNAND_RDN */
-	IMX_PIN_REG(MX6Q_PAD_SD4_CMD, 0x06DC, 0x02F4, 2, 0x0000, 0), /* MX6Q_PAD_SD4_CMD__UART3_TXD */
-	IMX_PIN_REG(MX6Q_PAD_SD4_CMD, 0x06DC, 0x02F4, 4, 0x0000, 0), /* MX6Q_PAD_SD4_CMD__PCIE_CTRL_MUX_5 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_CMD, 0x06DC, 0x02F4, 5, 0x0000, 0), /* MX6Q_PAD_SD4_CMD__GPIO_7_9 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_CMD, 0x06DC, 0x02F4, 7, 0x0000, 0), /* MX6Q_PAD_SD4_CMD__TPSMP_HDATA_DIR */
-	IMX_PIN_REG(MX6Q_PAD_SD4_CLK, 0x06E0, 0x02F8, 0, 0x0000, 0), /* MX6Q_PAD_SD4_CLK__USDHC4_CLK */
-	IMX_PIN_REG(MX6Q_PAD_SD4_CLK, 0x06E0, 0x02F8, 1, 0x0000, 0), /* MX6Q_PAD_SD4_CLK__RAWNAND_WRN */
-	IMX_PIN_REG(MX6Q_PAD_SD4_CLK, 0x06E0, 0x02F8, 2, 0x0930, 3), /* MX6Q_PAD_SD4_CLK__UART3_RXD */
-	IMX_PIN_REG(MX6Q_PAD_SD4_CLK, 0x06E0, 0x02F8, 4, 0x0000, 0), /* MX6Q_PAD_SD4_CLK__PCIE_CTRL_MUX_6 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_CLK, 0x06E0, 0x02F8, 5, 0x0000, 0), /* MX6Q_PAD_SD4_CLK__GPIO_7_10 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D0, 0x06E4, 0x02FC, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_D0__RAWNAND_D0 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D0, 0x06E4, 0x02FC, 1, 0x0000, 0), /* MX6Q_PAD_NANDF_D0__USDHC1_DAT4 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D0, 0x06E4, 0x02FC, 2, 0x0000, 0), /* MX6Q_PAD_NANDF_D0__GPU3D_GPU_DBG_OUT_0 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D0, 0x06E4, 0x02FC, 3, 0x0000, 0), /* MX6Q_PAD_NANDF_D0__USBOH3_UH2_DFD_OUT16 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D0, 0x06E4, 0x02FC, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_D0__USBOH3_UH3_DFD_OUT16 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D0, 0x06E4, 0x02FC, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_D0__GPIO_2_0 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D0, 0x06E4, 0x02FC, 6, 0x0000, 0), /* MX6Q_PAD_NANDF_D0__IPU1_IPU_DIAG_BUS_0 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D0, 0x06E4, 0x02FC, 7, 0x0000, 0), /* MX6Q_PAD_NANDF_D0__IPU2_IPU_DIAG_BUS_0 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D1, 0x06E8, 0x0300, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_D1__RAWNAND_D1 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D1, 0x06E8, 0x0300, 1, 0x0000, 0), /* MX6Q_PAD_NANDF_D1__USDHC1_DAT5 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D1, 0x06E8, 0x0300, 2, 0x0000, 0), /* MX6Q_PAD_NANDF_D1__GPU3D_GPU_DEBUG_OUT1 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D1, 0x06E8, 0x0300, 3, 0x0000, 0), /* MX6Q_PAD_NANDF_D1__USBOH3_UH2_DFD_OUT17 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D1, 0x06E8, 0x0300, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_D1__USBOH3_UH3_DFD_OUT17 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D1, 0x06E8, 0x0300, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_D1__GPIO_2_1 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D1, 0x06E8, 0x0300, 6, 0x0000, 0), /* MX6Q_PAD_NANDF_D1__IPU1_IPU_DIAG_BUS_1 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D1, 0x06E8, 0x0300, 7, 0x0000, 0), /* MX6Q_PAD_NANDF_D1__IPU2_IPU_DIAG_BUS_1 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D2, 0x06EC, 0x0304, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_D2__RAWNAND_D2 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D2, 0x06EC, 0x0304, 1, 0x0000, 0), /* MX6Q_PAD_NANDF_D2__USDHC1_DAT6 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D2, 0x06EC, 0x0304, 2, 0x0000, 0), /* MX6Q_PAD_NANDF_D2__GPU3D_GPU_DBG_OUT_2 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D2, 0x06EC, 0x0304, 3, 0x0000, 0), /* MX6Q_PAD_NANDF_D2__USBOH3_UH2_DFD_OUT18 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D2, 0x06EC, 0x0304, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_D2__USBOH3_UH3_DFD_OUT18 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D2, 0x06EC, 0x0304, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_D2__GPIO_2_2 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D2, 0x06EC, 0x0304, 6, 0x0000, 0), /* MX6Q_PAD_NANDF_D2__IPU1_IPU_DIAG_BUS_2 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D2, 0x06EC, 0x0304, 7, 0x0000, 0), /* MX6Q_PAD_NANDF_D2__IPU2_IPU_DIAG_BUS_2 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D3, 0x06F0, 0x0308, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_D3__RAWNAND_D3 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D3, 0x06F0, 0x0308, 1, 0x0000, 0), /* MX6Q_PAD_NANDF_D3__USDHC1_DAT7 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D3, 0x06F0, 0x0308, 2, 0x0000, 0), /* MX6Q_PAD_NANDF_D3__GPU3D_GPU_DBG_OUT_3 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D3, 0x06F0, 0x0308, 3, 0x0000, 0), /* MX6Q_PAD_NANDF_D3__USBOH3_UH2_DFD_OUT19 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D3, 0x06F0, 0x0308, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_D3__USBOH3_UH3_DFD_OUT19 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D3, 0x06F0, 0x0308, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_D3__GPIO_2_3 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D3, 0x06F0, 0x0308, 6, 0x0000, 0), /* MX6Q_PAD_NANDF_D3__IPU1_IPU_DIAG_BUS_3 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D3, 0x06F0, 0x0308, 7, 0x0000, 0), /* MX6Q_PAD_NANDF_D3__IPU2_IPU_DIAG_BUS_3 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D4, 0x06F4, 0x030C, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_D4__RAWNAND_D4 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D4, 0x06F4, 0x030C, 1, 0x0000, 0), /* MX6Q_PAD_NANDF_D4__USDHC2_DAT4 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D4, 0x06F4, 0x030C, 2, 0x0000, 0), /* MX6Q_PAD_NANDF_D4__GPU3D_GPU_DBG_OUT_4 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D4, 0x06F4, 0x030C, 3, 0x0000, 0), /* MX6Q_PAD_NANDF_D4__USBOH3_UH2_DFD_OUT20 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D4, 0x06F4, 0x030C, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_D4__USBOH3_UH3_DFD_OUT20 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D4, 0x06F4, 0x030C, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_D4__GPIO_2_4 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D4, 0x06F4, 0x030C, 6, 0x0000, 0), /* MX6Q_PAD_NANDF_D4__IPU1_IPU_DIAG_BUS_4 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D4, 0x06F4, 0x030C, 7, 0x0000, 0), /* MX6Q_PAD_NANDF_D4__IPU2_IPU_DIAG_BUS_4 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D5, 0x06F8, 0x0310, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_D5__RAWNAND_D5 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D5, 0x06F8, 0x0310, 1, 0x0000, 0), /* MX6Q_PAD_NANDF_D5__USDHC2_DAT5 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D5, 0x06F8, 0x0310, 2, 0x0000, 0), /* MX6Q_PAD_NANDF_D5__GPU3D_GPU_DBG_OUT_5 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D5, 0x06F8, 0x0310, 3, 0x0000, 0), /* MX6Q_PAD_NANDF_D5__USBOH3_UH2_DFD_OUT21 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D5, 0x06F8, 0x0310, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_D5__USBOH3_UH3_DFD_OUT21 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D5, 0x06F8, 0x0310, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_D5__GPIO_2_5 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D5, 0x06F8, 0x0310, 6, 0x0000, 0), /* MX6Q_PAD_NANDF_D5__IPU1_IPU_DIAG_BUS_5 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D5, 0x06F8, 0x0310, 7, 0x0000, 0), /* MX6Q_PAD_NANDF_D5__IPU2_IPU_DIAG_BUS_5 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D6, 0x06FC, 0x0314, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_D6__RAWNAND_D6 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D6, 0x06FC, 0x0314, 1, 0x0000, 0), /* MX6Q_PAD_NANDF_D6__USDHC2_DAT6 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D6, 0x06FC, 0x0314, 2, 0x0000, 0), /* MX6Q_PAD_NANDF_D6__GPU3D_GPU_DBG_OUT_6 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D6, 0x06FC, 0x0314, 3, 0x0000, 0), /* MX6Q_PAD_NANDF_D6__USBOH3_UH2_DFD_OUT22 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D6, 0x06FC, 0x0314, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_D6__USBOH3_UH3_DFD_OUT22 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D6, 0x06FC, 0x0314, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_D6__GPIO_2_6 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D6, 0x06FC, 0x0314, 6, 0x0000, 0), /* MX6Q_PAD_NANDF_D6__IPU1_IPU_DIAG_BUS_6 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D6, 0x06FC, 0x0314, 7, 0x0000, 0), /* MX6Q_PAD_NANDF_D6__IPU2_IPU_DIAG_BUS_6 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D7, 0x0700, 0x0318, 0, 0x0000, 0), /* MX6Q_PAD_NANDF_D7__RAWNAND_D7 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D7, 0x0700, 0x0318, 1, 0x0000, 0), /* MX6Q_PAD_NANDF_D7__USDHC2_DAT7 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D7, 0x0700, 0x0318, 2, 0x0000, 0), /* MX6Q_PAD_NANDF_D7__GPU3D_GPU_DBG_OUT_7 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D7, 0x0700, 0x0318, 3, 0x0000, 0), /* MX6Q_PAD_NANDF_D7__USBOH3_UH2_DFD_OUT23 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D7, 0x0700, 0x0318, 4, 0x0000, 0), /* MX6Q_PAD_NANDF_D7__USBOH3_UH3_DFD_OUT23 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D7, 0x0700, 0x0318, 5, 0x0000, 0), /* MX6Q_PAD_NANDF_D7__GPIO_2_7 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D7, 0x0700, 0x0318, 6, 0x0000, 0), /* MX6Q_PAD_NANDF_D7__IPU1_IPU_DIAG_BUS_7 */
-	IMX_PIN_REG(MX6Q_PAD_NANDF_D7, 0x0700, 0x0318, 7, 0x0000, 0), /* MX6Q_PAD_NANDF_D7__IPU2_IPU_DIAG_BUS_7 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT0, 0x0704, 0x031C, 0, 0x0000, 0), /* MX6Q_PAD_SD4_DAT0__RAWNAND_D8 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT0, 0x0704, 0x031C, 1, 0x0000, 0), /* MX6Q_PAD_SD4_DAT0__USDHC4_DAT0 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT0, 0x0704, 0x031C, 2, 0x0000, 0), /* MX6Q_PAD_SD4_DAT0__RAWNAND_DQS */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT0, 0x0704, 0x031C, 3, 0x0000, 0), /* MX6Q_PAD_SD4_DAT0__USBOH3_UH2_DFD_OUT24 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT0, 0x0704, 0x031C, 4, 0x0000, 0), /* MX6Q_PAD_SD4_DAT0__USBOH3_UH3_DFD_OUT24 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT0, 0x0704, 0x031C, 5, 0x0000, 0), /* MX6Q_PAD_SD4_DAT0__GPIO_2_8 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT0, 0x0704, 0x031C, 6, 0x0000, 0), /* MX6Q_PAD_SD4_DAT0__IPU1_IPU_DIAG_BUS_8 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT0, 0x0704, 0x031C, 7, 0x0000, 0), /* MX6Q_PAD_SD4_DAT0__IPU2_IPU_DIAG_BUS_8 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT1, 0x0708, 0x0320, 0, 0x0000, 0), /* MX6Q_PAD_SD4_DAT1__RAWNAND_D9 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT1, 0x0708, 0x0320, 1, 0x0000, 0), /* MX6Q_PAD_SD4_DAT1__USDHC4_DAT1 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT1, 0x0708, 0x0320, 2, 0x0000, 0), /* MX6Q_PAD_SD4_DAT1__PWM3_PWMO */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT1, 0x0708, 0x0320, 3, 0x0000, 0), /* MX6Q_PAD_SD4_DAT1__USBOH3_UH2_DFD_OUT25 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT1, 0x0708, 0x0320, 4, 0x0000, 0), /* MX6Q_PAD_SD4_DAT1__USBOH3_UH3_DFD_OUT25 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT1, 0x0708, 0x0320, 5, 0x0000, 0), /* MX6Q_PAD_SD4_DAT1__GPIO_2_9 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT1, 0x0708, 0x0320, 6, 0x0000, 0), /* MX6Q_PAD_SD4_DAT1__IPU1_IPU_DIAG_BUS_9 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT1, 0x0708, 0x0320, 7, 0x0000, 0), /* MX6Q_PAD_SD4_DAT1__IPU2_IPU_DIAG_BUS_9 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT2, 0x070C, 0x0324, 0, 0x0000, 0), /* MX6Q_PAD_SD4_DAT2__RAWNAND_D10 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT2, 0x070C, 0x0324, 1, 0x0000, 0), /* MX6Q_PAD_SD4_DAT2__USDHC4_DAT2 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT2, 0x070C, 0x0324, 2, 0x0000, 0), /* MX6Q_PAD_SD4_DAT2__PWM4_PWMO */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT2, 0x070C, 0x0324, 3, 0x0000, 0), /* MX6Q_PAD_SD4_DAT2__USBOH3_UH2_DFD_OUT26 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT2, 0x070C, 0x0324, 4, 0x0000, 0), /* MX6Q_PAD_SD4_DAT2__USBOH3_UH3_DFD_OUT26 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT2, 0x070C, 0x0324, 5, 0x0000, 0), /* MX6Q_PAD_SD4_DAT2__GPIO_2_10 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT2, 0x070C, 0x0324, 6, 0x0000, 0), /* MX6Q_PAD_SD4_DAT2__IPU1_IPU_DIAG_BUS_10 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT2, 0x070C, 0x0324, 7, 0x0000, 0), /* MX6Q_PAD_SD4_DAT2__IPU2_IPU_DIAG_BUS_10 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT3, 0x0710, 0x0328, 0, 0x0000, 0), /* MX6Q_PAD_SD4_DAT3__RAWNAND_D11 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT3, 0x0710, 0x0328, 1, 0x0000, 0), /* MX6Q_PAD_SD4_DAT3__USDHC4_DAT3 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT3, 0x0710, 0x0328, 3, 0x0000, 0), /* MX6Q_PAD_SD4_DAT3__USBOH3_UH2_DFD_OUT27 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT3, 0x0710, 0x0328, 4, 0x0000, 0), /* MX6Q_PAD_SD4_DAT3__USBOH3_UH3_DFD_OUT27 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT3, 0x0710, 0x0328, 5, 0x0000, 0), /* MX6Q_PAD_SD4_DAT3__GPIO_2_11 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT3, 0x0710, 0x0328, 6, 0x0000, 0), /* MX6Q_PAD_SD4_DAT3__IPU1_IPU_DIAG_BUS_11 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT3, 0x0710, 0x0328, 7, 0x0000, 0), /* MX6Q_PAD_SD4_DAT3__IPU2_IPU_DIAG_BUS_11 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT4, 0x0714, 0x032C, 0, 0x0000, 0), /* MX6Q_PAD_SD4_DAT4__RAWNAND_D12 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT4, 0x0714, 0x032C, 1, 0x0000, 0), /* MX6Q_PAD_SD4_DAT4__USDHC4_DAT4 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT4, 0x0714, 0x032C, 2, 0x0928, 6), /* MX6Q_PAD_SD4_DAT4__UART2_RXD */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT4, 0x0714, 0x032C, 3, 0x0000, 0), /* MX6Q_PAD_SD4_DAT4__USBOH3_UH2_DFD_OUT28 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT4, 0x0714, 0x032C, 4, 0x0000, 0), /* MX6Q_PAD_SD4_DAT4__USBOH3_UH3_DFD_OUT28 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT4, 0x0714, 0x032C, 5, 0x0000, 0), /* MX6Q_PAD_SD4_DAT4__GPIO_2_12 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT4, 0x0714, 0x032C, 6, 0x0000, 0), /* MX6Q_PAD_SD4_DAT4__IPU1_IPU_DIAG_BUS_12 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT4, 0x0714, 0x032C, 7, 0x0000, 0), /* MX6Q_PAD_SD4_DAT4__IPU2_IPU_DIAG_BUS_12 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT5, 0x0718, 0x0330, 0, 0x0000, 0), /* MX6Q_PAD_SD4_DAT5__RAWNAND_D13 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT5, 0x0718, 0x0330, 1, 0x0000, 0), /* MX6Q_PAD_SD4_DAT5__USDHC4_DAT5 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT5, 0x0718, 0x0330, 2, 0x0924, 4), /* MX6Q_PAD_SD4_DAT5__UART2_RTS */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT5, 0x0718, 0x0330, 3, 0x0000, 0), /* MX6Q_PAD_SD4_DAT5__USBOH3_UH2_DFD_OUT29 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT5, 0x0718, 0x0330, 4, 0x0000, 0), /* MX6Q_PAD_SD4_DAT5__USBOH3_UH3_DFD_OUT29 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT5, 0x0718, 0x0330, 5, 0x0000, 0), /* MX6Q_PAD_SD4_DAT5__GPIO_2_13 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT5, 0x0718, 0x0330, 6, 0x0000, 0), /* MX6Q_PAD_SD4_DAT5__IPU1_IPU_DIAG_BUS_13 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT5, 0x0718, 0x0330, 7, 0x0000, 0), /* MX6Q_PAD_SD4_DAT5__IPU2_IPU_DIAG_BUS_13 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT6, 0x071C, 0x0334, 0, 0x0000, 0), /* MX6Q_PAD_SD4_DAT6__RAWNAND_D14 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT6, 0x071C, 0x0334, 1, 0x0000, 0), /* MX6Q_PAD_SD4_DAT6__USDHC4_DAT6 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT6, 0x071C, 0x0334, 2, 0x0924, 5), /* MX6Q_PAD_SD4_DAT6__UART2_CTS */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT6, 0x071C, 0x0334, 3, 0x0000, 0), /* MX6Q_PAD_SD4_DAT6__USBOH3_UH2_DFD_OUT30 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT6, 0x071C, 0x0334, 4, 0x0000, 0), /* MX6Q_PAD_SD4_DAT6__USBOH3_UH3_DFD_OUT30 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT6, 0x071C, 0x0334, 5, 0x0000, 0), /* MX6Q_PAD_SD4_DAT6__GPIO_2_14 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT6, 0x071C, 0x0334, 6, 0x0000, 0), /* MX6Q_PAD_SD4_DAT6__IPU1_IPU_DIAG_BUS_14 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT6, 0x071C, 0x0334, 7, 0x0000, 0), /* MX6Q_PAD_SD4_DAT6__IPU2_IPU_DIAG_BUS_14 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT7, 0x0720, 0x0338, 0, 0x0000, 0), /* MX6Q_PAD_SD4_DAT7__RAWNAND_D15 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT7, 0x0720, 0x0338, 1, 0x0000, 0), /* MX6Q_PAD_SD4_DAT7__USDHC4_DAT7 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT7, 0x0720, 0x0338, 2, 0x0000, 0), /* MX6Q_PAD_SD4_DAT7__UART2_TXD */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT7, 0x0720, 0x0338, 3, 0x0000, 0), /* MX6Q_PAD_SD4_DAT7__USBOH3_UH2_DFD_OUT31 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT7, 0x0720, 0x0338, 4, 0x0000, 0), /* MX6Q_PAD_SD4_DAT7__USBOH3_UH3_DFD_OUT31 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT7, 0x0720, 0x0338, 5, 0x0000, 0), /* MX6Q_PAD_SD4_DAT7__GPIO_2_15 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT7, 0x0720, 0x0338, 6, 0x0000, 0), /* MX6Q_PAD_SD4_DAT7__IPU1_IPU_DIAG_BUS_15 */
-	IMX_PIN_REG(MX6Q_PAD_SD4_DAT7, 0x0720, 0x0338, 7, 0x0000, 0), /* MX6Q_PAD_SD4_DAT7__IPU2_IPU_DIAG_BUS_15 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT1, 0x0724, 0x033C, 0, 0x0000, 0), /* MX6Q_PAD_SD1_DAT1__USDHC1_DAT1 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT1, 0x0724, 0x033C, 1, 0x0834, 1), /* MX6Q_PAD_SD1_DAT1__ECSPI5_SS0 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT1, 0x0724, 0x033C, 2, 0x0000, 0), /* MX6Q_PAD_SD1_DAT1__PWM3_PWMO */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT1, 0x0724, 0x033C, 3, 0x0000, 0), /* MX6Q_PAD_SD1_DAT1__GPT_CAPIN2 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT1, 0x0724, 0x033C, 4, 0x0000, 0), /* MX6Q_PAD_SD1_DAT1__PCIE_CTRL_MUX_7 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT1, 0x0724, 0x033C, 5, 0x0000, 0), /* MX6Q_PAD_SD1_DAT1__GPIO_1_17 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT1, 0x0724, 0x033C, 6, 0x0000, 0), /* MX6Q_PAD_SD1_DAT1__HDMI_TX_OPHYDTB_0 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT1, 0x0724, 0x033C, 7, 0x0000, 0), /* MX6Q_PAD_SD1_DAT1__ANATOP_TESTO_8 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT0, 0x0728, 0x0340, 0, 0x0000, 0), /* MX6Q_PAD_SD1_DAT0__USDHC1_DAT0 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT0, 0x0728, 0x0340, 1, 0x082C, 1), /* MX6Q_PAD_SD1_DAT0__ECSPI5_MISO */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT0, 0x0728, 0x0340, 2, 0x0000, 0), /* MX6Q_PAD_SD1_DAT0__CAAM_WRAP_RNG_OSCOBS */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT0, 0x0728, 0x0340, 3, 0x0000, 0), /* MX6Q_PAD_SD1_DAT0__GPT_CAPIN1 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT0, 0x0728, 0x0340, 4, 0x0000, 0), /* MX6Q_PAD_SD1_DAT0__PCIE_CTRL_MUX_8 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT0, 0x0728, 0x0340, 5, 0x0000, 0), /* MX6Q_PAD_SD1_DAT0__GPIO_1_16 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT0, 0x0728, 0x0340, 6, 0x0000, 0), /* MX6Q_PAD_SD1_DAT0__HDMI_TX_OPHYDTB_1 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT0, 0x0728, 0x0340, 7, 0x0000, 0), /* MX6Q_PAD_SD1_DAT0__ANATOP_TESTO_7 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT3, 0x072C, 0x0344, 0, 0x0000, 0), /* MX6Q_PAD_SD1_DAT3__USDHC1_DAT3 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT3, 0x072C, 0x0344, 1, 0x0000, 0), /* MX6Q_PAD_SD1_DAT3__ECSPI5_SS2 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT3, 0x072C, 0x0344, 2, 0x0000, 0), /* MX6Q_PAD_SD1_DAT3__GPT_CMPOUT3 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT3, 0x072C, 0x0344, 3, 0x0000, 0), /* MX6Q_PAD_SD1_DAT3__PWM1_PWMO */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT3, 0x072C, 0x0344, 4, 0x0000, 0), /* MX6Q_PAD_SD1_DAT3__WDOG2_WDOG_B */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT3, 0x072C, 0x0344, 5, 0x0000, 0), /* MX6Q_PAD_SD1_DAT3__GPIO_1_21 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT3, 0x072C, 0x0344, 6, 0x0000, 0), /* MX6Q_PAD_SD1_DAT3__WDOG2_WDOG_RST_B_DEB */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT3, 0x072C, 0x0344, 7, 0x0000, 0), /* MX6Q_PAD_SD1_DAT3__ANATOP_TESTO_6 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_CMD, 0x0730, 0x0348, 0, 0x0000, 0), /* MX6Q_PAD_SD1_CMD__USDHC1_CMD */
-	IMX_PIN_REG(MX6Q_PAD_SD1_CMD, 0x0730, 0x0348, 1, 0x0830, 0), /* MX6Q_PAD_SD1_CMD__ECSPI5_MOSI */
-	IMX_PIN_REG(MX6Q_PAD_SD1_CMD, 0x0730, 0x0348, 2, 0x0000, 0), /* MX6Q_PAD_SD1_CMD__PWM4_PWMO */
-	IMX_PIN_REG(MX6Q_PAD_SD1_CMD, 0x0730, 0x0348, 3, 0x0000, 0), /* MX6Q_PAD_SD1_CMD__GPT_CMPOUT1 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_CMD, 0x0730, 0x0348, 5, 0x0000, 0), /* MX6Q_PAD_SD1_CMD__GPIO_1_18 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_CMD, 0x0730, 0x0348, 7, 0x0000, 0), /* MX6Q_PAD_SD1_CMD__ANATOP_TESTO_5 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT2, 0x0734, 0x034C, 0, 0x0000, 0), /* MX6Q_PAD_SD1_DAT2__USDHC1_DAT2 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT2, 0x0734, 0x034C, 1, 0x0838, 1), /* MX6Q_PAD_SD1_DAT2__ECSPI5_SS1 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT2, 0x0734, 0x034C, 2, 0x0000, 0), /* MX6Q_PAD_SD1_DAT2__GPT_CMPOUT2 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT2, 0x0734, 0x034C, 3, 0x0000, 0), /* MX6Q_PAD_SD1_DAT2__PWM2_PWMO */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT2, 0x0734, 0x034C, 4, 0x0000, 0), /* MX6Q_PAD_SD1_DAT2__WDOG1_WDOG_B */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT2, 0x0734, 0x034C, 5, 0x0000, 0), /* MX6Q_PAD_SD1_DAT2__GPIO_1_19 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT2, 0x0734, 0x034C, 6, 0x0000, 0), /* MX6Q_PAD_SD1_DAT2__WDOG1_WDOG_RST_B_DEB */
-	IMX_PIN_REG(MX6Q_PAD_SD1_DAT2, 0x0734, 0x034C, 7, 0x0000, 0), /* MX6Q_PAD_SD1_DAT2__ANATOP_TESTO_4 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_CLK, 0x0738, 0x0350, 0, 0x0000, 0), /* MX6Q_PAD_SD1_CLK__USDHC1_CLK */
-	IMX_PIN_REG(MX6Q_PAD_SD1_CLK, 0x0738, 0x0350, 1, 0x0828, 0), /* MX6Q_PAD_SD1_CLK__ECSPI5_SCLK */
-	IMX_PIN_REG(MX6Q_PAD_SD1_CLK, 0x0738, 0x0350, 2, 0x0000, 0), /* MX6Q_PAD_SD1_CLK__OSC32K_32K_OUT */
-	IMX_PIN_REG(MX6Q_PAD_SD1_CLK, 0x0738, 0x0350, 3, 0x0000, 0), /* MX6Q_PAD_SD1_CLK__GPT_CLKIN */
-	IMX_PIN_REG(MX6Q_PAD_SD1_CLK, 0x0738, 0x0350, 5, 0x0000, 0), /* MX6Q_PAD_SD1_CLK__GPIO_1_20 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_CLK, 0x0738, 0x0350, 6, 0x0000, 0), /* MX6Q_PAD_SD1_CLK__PHY_DTB_0 */
-	IMX_PIN_REG(MX6Q_PAD_SD1_CLK, 0x0738, 0x0350, 7, 0x0000, 0), /* MX6Q_PAD_SD1_CLK__SATA_PHY_DTB_0 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_CLK, 0x073C, 0x0354, 0, 0x0000, 0), /* MX6Q_PAD_SD2_CLK__USDHC2_CLK */
-	IMX_PIN_REG(MX6Q_PAD_SD2_CLK, 0x073C, 0x0354, 1, 0x0828, 1), /* MX6Q_PAD_SD2_CLK__ECSPI5_SCLK */
-	IMX_PIN_REG(MX6Q_PAD_SD2_CLK, 0x073C, 0x0354, 2, 0x08E8, 3), /* MX6Q_PAD_SD2_CLK__KPP_COL_5 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_CLK, 0x073C, 0x0354, 3, 0x07C0, 1), /* MX6Q_PAD_SD2_CLK__AUDMUX_AUD4_RXFS */
-	IMX_PIN_REG(MX6Q_PAD_SD2_CLK, 0x073C, 0x0354, 4, 0x0000, 0), /* MX6Q_PAD_SD2_CLK__PCIE_CTRL_MUX_9 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_CLK, 0x073C, 0x0354, 5, 0x0000, 0), /* MX6Q_PAD_SD2_CLK__GPIO_1_10 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_CLK, 0x073C, 0x0354, 6, 0x0000, 0), /* MX6Q_PAD_SD2_CLK__PHY_DTB_1 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_CLK, 0x073C, 0x0354, 7, 0x0000, 0), /* MX6Q_PAD_SD2_CLK__SATA_PHY_DTB_1 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_CMD, 0x0740, 0x0358, 0, 0x0000, 0), /* MX6Q_PAD_SD2_CMD__USDHC2_CMD */
-	IMX_PIN_REG(MX6Q_PAD_SD2_CMD, 0x0740, 0x0358, 1, 0x0830, 1), /* MX6Q_PAD_SD2_CMD__ECSPI5_MOSI */
-	IMX_PIN_REG(MX6Q_PAD_SD2_CMD, 0x0740, 0x0358, 2, 0x08F4, 2), /* MX6Q_PAD_SD2_CMD__KPP_ROW_5 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_CMD, 0x0740, 0x0358, 3, 0x07BC, 1), /* MX6Q_PAD_SD2_CMD__AUDMUX_AUD4_RXC */
-	IMX_PIN_REG(MX6Q_PAD_SD2_CMD, 0x0740, 0x0358, 4, 0x0000, 0), /* MX6Q_PAD_SD2_CMD__PCIE_CTRL_MUX_10 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_CMD, 0x0740, 0x0358, 5, 0x0000, 0), /* MX6Q_PAD_SD2_CMD__GPIO_1_11 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT3, 0x0744, 0x035C, 0, 0x0000, 0), /* MX6Q_PAD_SD2_DAT3__USDHC2_DAT3 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT3, 0x0744, 0x035C, 1, 0x0000, 0), /* MX6Q_PAD_SD2_DAT3__ECSPI5_SS3 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT3, 0x0744, 0x035C, 2, 0x08EC, 2), /* MX6Q_PAD_SD2_DAT3__KPP_COL_6 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT3, 0x0744, 0x035C, 3, 0x07C4, 1), /* MX6Q_PAD_SD2_DAT3__AUDMUX_AUD4_TXC */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT3, 0x0744, 0x035C, 4, 0x0000, 0), /* MX6Q_PAD_SD2_DAT3__PCIE_CTRL_MUX_11 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT3, 0x0744, 0x035C, 5, 0x0000, 0), /* MX6Q_PAD_SD2_DAT3__GPIO_1_12 */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT3, 0x0744, 0x035C, 6, 0x0000, 0), /* MX6Q_PAD_SD2_DAT3__SJC_DONE */
-	IMX_PIN_REG(MX6Q_PAD_SD2_DAT3, 0x0744, 0x035C, 7, 0x0000, 0), /* MX6Q_PAD_SD2_DAT3__ANATOP_TESTO_3 */
-	IMX_PIN_REG(MX6Q_PAD_ENET_RX_ER, 0x04EC, 0x01D8, 0, 0x0000, 0), /* MX6Q_PAD_ENET_RX_ER__ANATOP_USBOTG_ID */
-	IMX_PIN_REG(MX6Q_PAD_GPIO_1, 0x05F4, 0x0224, 3, 0x0000, 0), /* MX6Q_PAD_GPIO_1__ANATOP_USBOTG_ID */
+	MX6Q_PAD_RESERVE0 = 0,
+	MX6Q_PAD_RESERVE1 = 1,
+	MX6Q_PAD_RESERVE2 = 2,
+	MX6Q_PAD_RESERVE3 = 3,
+	MX6Q_PAD_RESERVE4 = 4,
+	MX6Q_PAD_RESERVE5 = 5,
+	MX6Q_PAD_RESERVE6 = 6,
+	MX6Q_PAD_RESERVE7 = 7,
+	MX6Q_PAD_RESERVE8 = 8,
+	MX6Q_PAD_RESERVE9 = 9,
+	MX6Q_PAD_RESERVE10 = 10,
+	MX6Q_PAD_RESERVE11 = 11,
+	MX6Q_PAD_RESERVE12 = 12,
+	MX6Q_PAD_RESERVE13 = 13,
+	MX6Q_PAD_RESERVE14 = 14,
+	MX6Q_PAD_RESERVE15 = 15,
+	MX6Q_PAD_RESERVE16 = 16,
+	MX6Q_PAD_RESERVE17 = 17,
+	MX6Q_PAD_RESERVE18 = 18,
+	MX6Q_PAD_SD2_DAT1 = 19,
+	MX6Q_PAD_SD2_DAT2 = 20,
+	MX6Q_PAD_SD2_DAT0 = 21,
+	MX6Q_PAD_RGMII_TXC = 22,
+	MX6Q_PAD_RGMII_TD0 = 23,
+	MX6Q_PAD_RGMII_TD1 = 24,
+	MX6Q_PAD_RGMII_TD2 = 25,
+	MX6Q_PAD_RGMII_TD3 = 26,
+	MX6Q_PAD_RGMII_RX_CTL = 27,
+	MX6Q_PAD_RGMII_RD0 = 28,
+	MX6Q_PAD_RGMII_TX_CTL = 29,
+	MX6Q_PAD_RGMII_RD1 = 30,
+	MX6Q_PAD_RGMII_RD2 = 31,
+	MX6Q_PAD_RGMII_RD3 = 32,
+	MX6Q_PAD_RGMII_RXC = 33,
+	MX6Q_PAD_EIM_A25 = 34,
+	MX6Q_PAD_EIM_EB2 = 35,
+	MX6Q_PAD_EIM_D16 = 36,
+	MX6Q_PAD_EIM_D17 = 37,
+	MX6Q_PAD_EIM_D18 = 38,
+	MX6Q_PAD_EIM_D19 = 39,
+	MX6Q_PAD_EIM_D20 = 40,
+	MX6Q_PAD_EIM_D21 = 41,
+	MX6Q_PAD_EIM_D22 = 42,
+	MX6Q_PAD_EIM_D23 = 43,
+	MX6Q_PAD_EIM_EB3 = 44,
+	MX6Q_PAD_EIM_D24 = 45,
+	MX6Q_PAD_EIM_D25 = 46,
+	MX6Q_PAD_EIM_D26 = 47,
+	MX6Q_PAD_EIM_D27 = 48,
+	MX6Q_PAD_EIM_D28 = 49,
+	MX6Q_PAD_EIM_D29 = 50,
+	MX6Q_PAD_EIM_D30 = 51,
+	MX6Q_PAD_EIM_D31 = 52,
+	MX6Q_PAD_EIM_A24 = 53,
+	MX6Q_PAD_EIM_A23 = 54,
+	MX6Q_PAD_EIM_A22 = 55,
+	MX6Q_PAD_EIM_A21 = 56,
+	MX6Q_PAD_EIM_A20 = 57,
+	MX6Q_PAD_EIM_A19 = 58,
+	MX6Q_PAD_EIM_A18 = 59,
+	MX6Q_PAD_EIM_A17 = 60,
+	MX6Q_PAD_EIM_A16 = 61,
+	MX6Q_PAD_EIM_CS0 = 62,
+	MX6Q_PAD_EIM_CS1 = 63,
+	MX6Q_PAD_EIM_OE = 64,
+	MX6Q_PAD_EIM_RW = 65,
+	MX6Q_PAD_EIM_LBA = 66,
+	MX6Q_PAD_EIM_EB0 = 67,
+	MX6Q_PAD_EIM_EB1 = 68,
+	MX6Q_PAD_EIM_DA0 = 69,
+	MX6Q_PAD_EIM_DA1 = 70,
+	MX6Q_PAD_EIM_DA2 = 71,
+	MX6Q_PAD_EIM_DA3 = 72,
+	MX6Q_PAD_EIM_DA4 = 73,
+	MX6Q_PAD_EIM_DA5 = 74,
+	MX6Q_PAD_EIM_DA6 = 75,
+	MX6Q_PAD_EIM_DA7 = 76,
+	MX6Q_PAD_EIM_DA8 = 77,
+	MX6Q_PAD_EIM_DA9 = 78,
+	MX6Q_PAD_EIM_DA10 = 79,
+	MX6Q_PAD_EIM_DA11 = 80,
+	MX6Q_PAD_EIM_DA12 = 81,
+	MX6Q_PAD_EIM_DA13 = 82,
+	MX6Q_PAD_EIM_DA14 = 83,
+	MX6Q_PAD_EIM_DA15 = 84,
+	MX6Q_PAD_EIM_WAIT = 85,
+	MX6Q_PAD_EIM_BCLK = 86,
+	MX6Q_PAD_DI0_DISP_CLK = 87,
+	MX6Q_PAD_DI0_PIN15 = 88,
+	MX6Q_PAD_DI0_PIN2 = 89,
+	MX6Q_PAD_DI0_PIN3 = 90,
+	MX6Q_PAD_DI0_PIN4 = 91,
+	MX6Q_PAD_DISP0_DAT0 = 92,
+	MX6Q_PAD_DISP0_DAT1 = 93,
+	MX6Q_PAD_DISP0_DAT2 = 94,
+	MX6Q_PAD_DISP0_DAT3 = 95,
+	MX6Q_PAD_DISP0_DAT4 = 96,
+	MX6Q_PAD_DISP0_DAT5 = 97,
+	MX6Q_PAD_DISP0_DAT6 = 98,
+	MX6Q_PAD_DISP0_DAT7 = 99,
+	MX6Q_PAD_DISP0_DAT8 = 100,
+	MX6Q_PAD_DISP0_DAT9 = 101,
+	MX6Q_PAD_DISP0_DAT10 = 102,
+	MX6Q_PAD_DISP0_DAT11 = 103,
+	MX6Q_PAD_DISP0_DAT12 = 104,
+	MX6Q_PAD_DISP0_DAT13 = 105,
+	MX6Q_PAD_DISP0_DAT14 = 106,
+	MX6Q_PAD_DISP0_DAT15 = 107,
+	MX6Q_PAD_DISP0_DAT16 = 108,
+	MX6Q_PAD_DISP0_DAT17 = 109,
+	MX6Q_PAD_DISP0_DAT18 = 110,
+	MX6Q_PAD_DISP0_DAT19 = 111,
+	MX6Q_PAD_DISP0_DAT20 = 112,
+	MX6Q_PAD_DISP0_DAT21 = 113,
+	MX6Q_PAD_DISP0_DAT22 = 114,
+	MX6Q_PAD_DISP0_DAT23 = 115,
+	MX6Q_PAD_ENET_MDIO = 116,
+	MX6Q_PAD_ENET_REF_CLK = 117,
+	MX6Q_PAD_ENET_RX_ER = 118,
+	MX6Q_PAD_ENET_CRS_DV = 119,
+	MX6Q_PAD_ENET_RXD1 = 120,
+	MX6Q_PAD_ENET_RXD0 = 121,
+	MX6Q_PAD_ENET_TX_EN = 122,
+	MX6Q_PAD_ENET_TXD1 = 123,
+	MX6Q_PAD_ENET_TXD0 = 124,
+	MX6Q_PAD_ENET_MDC = 125,
+	MX6Q_PAD_KEY_COL0 = 126,
+	MX6Q_PAD_KEY_ROW0 = 127,
+	MX6Q_PAD_KEY_COL1 = 128,
+	MX6Q_PAD_KEY_ROW1 = 129,
+	MX6Q_PAD_KEY_COL2 = 130,
+	MX6Q_PAD_KEY_ROW2 = 131,
+	MX6Q_PAD_KEY_COL3 = 132,
+	MX6Q_PAD_KEY_ROW3 = 133,
+	MX6Q_PAD_KEY_COL4 = 134,
+	MX6Q_PAD_KEY_ROW4 = 135,
+	MX6Q_PAD_GPIO_0 = 136,
+	MX6Q_PAD_GPIO_1 = 137,
+	MX6Q_PAD_GPIO_9 = 138,
+	MX6Q_PAD_GPIO_3 = 139,
+	MX6Q_PAD_GPIO_6 = 140,
+	MX6Q_PAD_GPIO_2 = 141,
+	MX6Q_PAD_GPIO_4 = 142,
+	MX6Q_PAD_GPIO_5 = 143,
+	MX6Q_PAD_GPIO_7 = 144,
+	MX6Q_PAD_GPIO_8 = 145,
+	MX6Q_PAD_GPIO_16 = 146,
+	MX6Q_PAD_GPIO_17 = 147,
+	MX6Q_PAD_GPIO_18 = 148,
+	MX6Q_PAD_GPIO_19 = 149,
+	MX6Q_PAD_CSI0_PIXCLK = 150,
+	MX6Q_PAD_CSI0_MCLK = 151,
+	MX6Q_PAD_CSI0_DATA_EN = 152,
+	MX6Q_PAD_CSI0_VSYNC = 153,
+	MX6Q_PAD_CSI0_DAT4 = 154,
+	MX6Q_PAD_CSI0_DAT5 = 155,
+	MX6Q_PAD_CSI0_DAT6 = 156,
+	MX6Q_PAD_CSI0_DAT7 = 157,
+	MX6Q_PAD_CSI0_DAT8 = 158,
+	MX6Q_PAD_CSI0_DAT9 = 159,
+	MX6Q_PAD_CSI0_DAT10 = 160,
+	MX6Q_PAD_CSI0_DAT11 = 161,
+	MX6Q_PAD_CSI0_DAT12 = 162,
+	MX6Q_PAD_CSI0_DAT13 = 163,
+	MX6Q_PAD_CSI0_DAT14 = 164,
+	MX6Q_PAD_CSI0_DAT15 = 165,
+	MX6Q_PAD_CSI0_DAT16 = 166,
+	MX6Q_PAD_CSI0_DAT17 = 167,
+	MX6Q_PAD_CSI0_DAT18 = 168,
+	MX6Q_PAD_CSI0_DAT19 = 169,
+	MX6Q_PAD_SD3_DAT7 = 170,
+	MX6Q_PAD_SD3_DAT6 = 171,
+	MX6Q_PAD_SD3_DAT5 = 172,
+	MX6Q_PAD_SD3_DAT4 = 173,
+	MX6Q_PAD_SD3_CMD = 174,
+	MX6Q_PAD_SD3_CLK = 175,
+	MX6Q_PAD_SD3_DAT0 = 176,
+	MX6Q_PAD_SD3_DAT1 = 177,
+	MX6Q_PAD_SD3_DAT2 = 178,
+	MX6Q_PAD_SD3_DAT3 = 179,
+	MX6Q_PAD_SD3_RST = 180,
+	MX6Q_PAD_NANDF_CLE = 181,
+	MX6Q_PAD_NANDF_ALE = 182,
+	MX6Q_PAD_NANDF_WP_B = 183,
+	MX6Q_PAD_NANDF_RB0 = 184,
+	MX6Q_PAD_NANDF_CS0 = 185,
+	MX6Q_PAD_NANDF_CS1 = 186,
+	MX6Q_PAD_NANDF_CS2 = 187,
+	MX6Q_PAD_NANDF_CS3 = 188,
+	MX6Q_PAD_SD4_CMD = 189,
+	MX6Q_PAD_SD4_CLK = 190,
+	MX6Q_PAD_NANDF_D0 = 191,
+	MX6Q_PAD_NANDF_D1 = 192,
+	MX6Q_PAD_NANDF_D2 = 193,
+	MX6Q_PAD_NANDF_D3 = 194,
+	MX6Q_PAD_NANDF_D4 = 195,
+	MX6Q_PAD_NANDF_D5 = 196,
+	MX6Q_PAD_NANDF_D6 = 197,
+	MX6Q_PAD_NANDF_D7 = 198,
+	MX6Q_PAD_SD4_DAT0 = 199,
+	MX6Q_PAD_SD4_DAT1 = 200,
+	MX6Q_PAD_SD4_DAT2 = 201,
+	MX6Q_PAD_SD4_DAT3 = 202,
+	MX6Q_PAD_SD4_DAT4 = 203,
+	MX6Q_PAD_SD4_DAT5 = 204,
+	MX6Q_PAD_SD4_DAT6 = 205,
+	MX6Q_PAD_SD4_DAT7 = 206,
+	MX6Q_PAD_SD1_DAT1 = 207,
+	MX6Q_PAD_SD1_DAT0 = 208,
+	MX6Q_PAD_SD1_DAT3 = 209,
+	MX6Q_PAD_SD1_CMD = 210,
+	MX6Q_PAD_SD1_DAT2 = 211,
+	MX6Q_PAD_SD1_CLK = 212,
+	MX6Q_PAD_SD2_CLK = 213,
+	MX6Q_PAD_SD2_CMD = 214,
+	MX6Q_PAD_SD2_DAT3 = 215,
 };
 
 /* Pad names for the pinmux subsystem */
 static const struct pinctrl_pin_desc imx6q_pinctrl_pads[] = {
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE0),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE1),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE2),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE3),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE4),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE5),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE6),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE7),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE8),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE9),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE10),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE11),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE12),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE13),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE14),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE15),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE16),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE17),
+	IMX_PINCTRL_PIN(MX6Q_PAD_RESERVE18),
 	IMX_PINCTRL_PIN(MX6Q_PAD_SD2_DAT1),
 	IMX_PINCTRL_PIN(MX6Q_PAD_SD2_DAT2),
 	IMX_PINCTRL_PIN(MX6Q_PAD_SD2_DAT0),
@@ -2063,117 +369,6 @@ static const struct pinctrl_pin_desc imx6q_pinctrl_pads[] = {
 	IMX_PINCTRL_PIN(MX6Q_PAD_ENET_TXD1),
 	IMX_PINCTRL_PIN(MX6Q_PAD_ENET_TXD0),
 	IMX_PINCTRL_PIN(MX6Q_PAD_ENET_MDC),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D40),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D41),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D42),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D43),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D44),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D45),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D46),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D47),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDQS5),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_DQM5),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D32),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D33),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D34),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D35),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D36),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D37),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D38),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D39),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_DQM4),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDQS4),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D24),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D25),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D26),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D27),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D28),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D29),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDQS3),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D30),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D31),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_DQM3),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D16),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D17),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D18),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D19),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D20),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D21),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D22),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDQS2),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D23),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_DQM2),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A0),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A1),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A2),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A3),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A4),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A5),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A6),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A7),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A8),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A9),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A10),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A11),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A12),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A13),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A14),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_A15),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_CAS),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_CS0),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_CS1),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_RAS),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_RESET),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDBA0),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDBA1),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDCLK_0),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDBA2),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDCKE0),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDCLK_1),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDCKE1),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDODT0),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDODT1),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDWE),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D0),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D1),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D2),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D3),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D4),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D5),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDQS0),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D6),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D7),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_DQM0),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D8),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D9),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D10),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D11),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D12),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D13),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D14),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDQS1),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D15),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_DQM1),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D48),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D49),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D50),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D51),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D52),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D53),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D54),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D55),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDQS6),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_DQM6),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D56),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_SDQS7),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D57),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D58),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D59),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D60),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_DQM7),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D61),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D62),
-	IMX_PINCTRL_PIN(MX6Q_PAD_DRAM_D63),
 	IMX_PINCTRL_PIN(MX6Q_PAD_KEY_COL0),
 	IMX_PINCTRL_PIN(MX6Q_PAD_KEY_ROW0),
 	IMX_PINCTRL_PIN(MX6Q_PAD_KEY_COL1),
@@ -2218,30 +413,6 @@ static const struct pinctrl_pin_desc imx6q_pinctrl_pads[] = {
 	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT17),
 	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT18),
 	IMX_PINCTRL_PIN(MX6Q_PAD_CSI0_DAT19),
-	IMX_PINCTRL_PIN(MX6Q_PAD_JTAG_TMS),
-	IMX_PINCTRL_PIN(MX6Q_PAD_JTAG_MOD),
-	IMX_PINCTRL_PIN(MX6Q_PAD_JTAG_TRSTB),
-	IMX_PINCTRL_PIN(MX6Q_PAD_JTAG_TDI),
-	IMX_PINCTRL_PIN(MX6Q_PAD_JTAG_TCK),
-	IMX_PINCTRL_PIN(MX6Q_PAD_JTAG_TDO),
-	IMX_PINCTRL_PIN(MX6Q_PAD_LVDS1_TX3_P),
-	IMX_PINCTRL_PIN(MX6Q_PAD_LVDS1_TX2_P),
-	IMX_PINCTRL_PIN(MX6Q_PAD_LVDS1_CLK_P),
-	IMX_PINCTRL_PIN(MX6Q_PAD_LVDS1_TX1_P),
-	IMX_PINCTRL_PIN(MX6Q_PAD_LVDS1_TX0_P),
-	IMX_PINCTRL_PIN(MX6Q_PAD_LVDS0_TX3_P),
-	IMX_PINCTRL_PIN(MX6Q_PAD_LVDS0_CLK_P),
-	IMX_PINCTRL_PIN(MX6Q_PAD_LVDS0_TX2_P),
-	IMX_PINCTRL_PIN(MX6Q_PAD_LVDS0_TX1_P),
-	IMX_PINCTRL_PIN(MX6Q_PAD_LVDS0_TX0_P),
-	IMX_PINCTRL_PIN(MX6Q_PAD_TAMPER),
-	IMX_PINCTRL_PIN(MX6Q_PAD_PMIC_ON_REQ),
-	IMX_PINCTRL_PIN(MX6Q_PAD_PMIC_STBY_REQ),
-	IMX_PINCTRL_PIN(MX6Q_PAD_POR_B),
-	IMX_PINCTRL_PIN(MX6Q_PAD_BOOT_MODE1),
-	IMX_PINCTRL_PIN(MX6Q_PAD_RESET_IN_B),
-	IMX_PINCTRL_PIN(MX6Q_PAD_BOOT_MODE0),
-	IMX_PINCTRL_PIN(MX6Q_PAD_TEST_MODE),
 	IMX_PINCTRL_PIN(MX6Q_PAD_SD3_DAT7),
 	IMX_PINCTRL_PIN(MX6Q_PAD_SD3_DAT6),
 	IMX_PINCTRL_PIN(MX6Q_PAD_SD3_DAT5),
@@ -2293,8 +464,6 @@ static const struct pinctrl_pin_desc imx6q_pinctrl_pads[] = {
 static struct imx_pinctrl_soc_info imx6q_pinctrl_info = {
 	.pins = imx6q_pinctrl_pads,
 	.npins = ARRAY_SIZE(imx6q_pinctrl_pads),
-	.pin_regs = imx6q_pin_regs,
-	.npin_regs = ARRAY_SIZE(imx6q_pin_regs),
 };
 
 static struct of_device_id imx6q_pinctrl_of_match[] = {
diff --git a/drivers/pinctrl/pinctrl-imx6sl.c b/drivers/pinctrl/pinctrl-imx6sl.c
new file mode 100644
index 000000000000..4eb7ccab5f2d
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-imx6sl.c
@@ -0,0 +1,403 @@
+/*
+ * Copyright (C) 2013 Freescale Semiconductor, Inc.
+ *
+ * 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/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-imx.h"
+
+enum imx6sl_pads {
+	MX6SL_PAD_RESERVE0 = 0,
+	MX6SL_PAD_RESERVE1 = 1,
+	MX6SL_PAD_RESERVE2 = 2,
+	MX6SL_PAD_RESERVE3 = 3,
+	MX6SL_PAD_RESERVE4 = 4,
+	MX6SL_PAD_RESERVE5 = 5,
+	MX6SL_PAD_RESERVE6 = 6,
+	MX6SL_PAD_RESERVE7 = 7,
+	MX6SL_PAD_RESERVE8 = 8,
+	MX6SL_PAD_RESERVE9 = 9,
+	MX6SL_PAD_RESERVE10 = 10,
+	MX6SL_PAD_RESERVE11 = 11,
+	MX6SL_PAD_RESERVE12 = 12,
+	MX6SL_PAD_RESERVE13 = 13,
+	MX6SL_PAD_RESERVE14 = 14,
+	MX6SL_PAD_RESERVE15 = 15,
+	MX6SL_PAD_RESERVE16 = 16,
+	MX6SL_PAD_RESERVE17 = 17,
+	MX6SL_PAD_RESERVE18 = 18,
+	MX6SL_PAD_AUD_MCLK = 19,
+	MX6SL_PAD_AUD_RXC = 20,
+	MX6SL_PAD_AUD_RXD = 21,
+	MX6SL_PAD_AUD_RXFS = 22,
+	MX6SL_PAD_AUD_TXC = 23,
+	MX6SL_PAD_AUD_TXD = 24,
+	MX6SL_PAD_AUD_TXFS = 25,
+	MX6SL_PAD_ECSPI1_MISO = 26,
+	MX6SL_PAD_ECSPI1_MOSI = 27,
+	MX6SL_PAD_ECSPI1_SCLK = 28,
+	MX6SL_PAD_ECSPI1_SS0 = 29,
+	MX6SL_PAD_ECSPI2_MISO = 30,
+	MX6SL_PAD_ECSPI2_MOSI = 31,
+	MX6SL_PAD_ECSPI2_SCLK = 32,
+	MX6SL_PAD_ECSPI2_SS0 = 33,
+	MX6SL_PAD_EPDC_BDR0 = 34,
+	MX6SL_PAD_EPDC_BDR1 = 35,
+	MX6SL_PAD_EPDC_D0 = 36,
+	MX6SL_PAD_EPDC_D1 = 37,
+	MX6SL_PAD_EPDC_D10 = 38,
+	MX6SL_PAD_EPDC_D11 = 39,
+	MX6SL_PAD_EPDC_D12 = 40,
+	MX6SL_PAD_EPDC_D13 = 41,
+	MX6SL_PAD_EPDC_D14 = 42,
+	MX6SL_PAD_EPDC_D15 = 43,
+	MX6SL_PAD_EPDC_D2 = 44,
+	MX6SL_PAD_EPDC_D3 = 45,
+	MX6SL_PAD_EPDC_D4 = 46,
+	MX6SL_PAD_EPDC_D5 = 47,
+	MX6SL_PAD_EPDC_D6 = 48,
+	MX6SL_PAD_EPDC_D7 = 49,
+	MX6SL_PAD_EPDC_D8 = 50,
+	MX6SL_PAD_EPDC_D9 = 51,
+	MX6SL_PAD_EPDC_GDCLK = 52,
+	MX6SL_PAD_EPDC_GDOE = 53,
+	MX6SL_PAD_EPDC_GDRL = 54,
+	MX6SL_PAD_EPDC_GDSP = 55,
+	MX6SL_PAD_EPDC_PWRCOM = 56,
+	MX6SL_PAD_EPDC_PWRCTRL0 = 57,
+	MX6SL_PAD_EPDC_PWRCTRL1 = 58,
+	MX6SL_PAD_EPDC_PWRCTRL2 = 59,
+	MX6SL_PAD_EPDC_PWRCTRL3 = 60,
+	MX6SL_PAD_EPDC_PWRINT = 61,
+	MX6SL_PAD_EPDC_PWRSTAT = 62,
+	MX6SL_PAD_EPDC_PWRWAKEUP = 63,
+	MX6SL_PAD_EPDC_SDCE0 = 64,
+	MX6SL_PAD_EPDC_SDCE1 = 65,
+	MX6SL_PAD_EPDC_SDCE2 = 66,
+	MX6SL_PAD_EPDC_SDCE3 = 67,
+	MX6SL_PAD_EPDC_SDCLK = 68,
+	MX6SL_PAD_EPDC_SDLE = 69,
+	MX6SL_PAD_EPDC_SDOE = 70,
+	MX6SL_PAD_EPDC_SDSHR = 71,
+	MX6SL_PAD_EPDC_VCOM0 = 72,
+	MX6SL_PAD_EPDC_VCOM1 = 73,
+	MX6SL_PAD_FEC_CRS_DV = 74,
+	MX6SL_PAD_FEC_MDC = 75,
+	MX6SL_PAD_FEC_MDIO = 76,
+	MX6SL_PAD_FEC_REF_CLK = 77,
+	MX6SL_PAD_FEC_RX_ER = 78,
+	MX6SL_PAD_FEC_RXD0 = 79,
+	MX6SL_PAD_FEC_RXD1 = 80,
+	MX6SL_PAD_FEC_TX_CLK = 81,
+	MX6SL_PAD_FEC_TX_EN = 82,
+	MX6SL_PAD_FEC_TXD0 = 83,
+	MX6SL_PAD_FEC_TXD1 = 84,
+	MX6SL_PAD_HSIC_DAT = 85,
+	MX6SL_PAD_HSIC_STROBE = 86,
+	MX6SL_PAD_I2C1_SCL = 87,
+	MX6SL_PAD_I2C1_SDA = 88,
+	MX6SL_PAD_I2C2_SCL = 89,
+	MX6SL_PAD_I2C2_SDA = 90,
+	MX6SL_PAD_KEY_COL0 = 91,
+	MX6SL_PAD_KEY_COL1 = 92,
+	MX6SL_PAD_KEY_COL2 = 93,
+	MX6SL_PAD_KEY_COL3 = 94,
+	MX6SL_PAD_KEY_COL4 = 95,
+	MX6SL_PAD_KEY_COL5 = 96,
+	MX6SL_PAD_KEY_COL6 = 97,
+	MX6SL_PAD_KEY_COL7 = 98,
+	MX6SL_PAD_KEY_ROW0 = 99,
+	MX6SL_PAD_KEY_ROW1 = 100,
+	MX6SL_PAD_KEY_ROW2 = 101,
+	MX6SL_PAD_KEY_ROW3 = 102,
+	MX6SL_PAD_KEY_ROW4 = 103,
+	MX6SL_PAD_KEY_ROW5 = 104,
+	MX6SL_PAD_KEY_ROW6 = 105,
+	MX6SL_PAD_KEY_ROW7 = 106,
+	MX6SL_PAD_LCD_CLK = 107,
+	MX6SL_PAD_LCD_DAT0 = 108,
+	MX6SL_PAD_LCD_DAT1 = 109,
+	MX6SL_PAD_LCD_DAT10 = 110,
+	MX6SL_PAD_LCD_DAT11 = 111,
+	MX6SL_PAD_LCD_DAT12 = 112,
+	MX6SL_PAD_LCD_DAT13 = 113,
+	MX6SL_PAD_LCD_DAT14 = 114,
+	MX6SL_PAD_LCD_DAT15 = 115,
+	MX6SL_PAD_LCD_DAT16 = 116,
+	MX6SL_PAD_LCD_DAT17 = 117,
+	MX6SL_PAD_LCD_DAT18 = 118,
+	MX6SL_PAD_LCD_DAT19 = 119,
+	MX6SL_PAD_LCD_DAT2 = 120,
+	MX6SL_PAD_LCD_DAT20 = 121,
+	MX6SL_PAD_LCD_DAT21 = 122,
+	MX6SL_PAD_LCD_DAT22 = 123,
+	MX6SL_PAD_LCD_DAT23 = 124,
+	MX6SL_PAD_LCD_DAT3 = 125,
+	MX6SL_PAD_LCD_DAT4 = 126,
+	MX6SL_PAD_LCD_DAT5 = 127,
+	MX6SL_PAD_LCD_DAT6 = 128,
+	MX6SL_PAD_LCD_DAT7 = 129,
+	MX6SL_PAD_LCD_DAT8 = 130,
+	MX6SL_PAD_LCD_DAT9 = 131,
+	MX6SL_PAD_LCD_ENABLE = 132,
+	MX6SL_PAD_LCD_HSYNC = 133,
+	MX6SL_PAD_LCD_RESET = 134,
+	MX6SL_PAD_LCD_VSYNC = 135,
+	MX6SL_PAD_PWM1 = 136,
+	MX6SL_PAD_REF_CLK_24M = 137,
+	MX6SL_PAD_REF_CLK_32K = 138,
+	MX6SL_PAD_SD1_CLK = 139,
+	MX6SL_PAD_SD1_CMD = 140,
+	MX6SL_PAD_SD1_DAT0 = 141,
+	MX6SL_PAD_SD1_DAT1 = 142,
+	MX6SL_PAD_SD1_DAT2 = 143,
+	MX6SL_PAD_SD1_DAT3 = 144,
+	MX6SL_PAD_SD1_DAT4 = 145,
+	MX6SL_PAD_SD1_DAT5 = 146,
+	MX6SL_PAD_SD1_DAT6 = 147,
+	MX6SL_PAD_SD1_DAT7 = 148,
+	MX6SL_PAD_SD2_CLK = 149,
+	MX6SL_PAD_SD2_CMD = 150,
+	MX6SL_PAD_SD2_DAT0 = 151,
+	MX6SL_PAD_SD2_DAT1 = 152,
+	MX6SL_PAD_SD2_DAT2 = 153,
+	MX6SL_PAD_SD2_DAT3 = 154,
+	MX6SL_PAD_SD2_DAT4 = 155,
+	MX6SL_PAD_SD2_DAT5 = 156,
+	MX6SL_PAD_SD2_DAT6 = 157,
+	MX6SL_PAD_SD2_DAT7 = 158,
+	MX6SL_PAD_SD2_RST = 159,
+	MX6SL_PAD_SD3_CLK = 160,
+	MX6SL_PAD_SD3_CMD = 161,
+	MX6SL_PAD_SD3_DAT0 = 162,
+	MX6SL_PAD_SD3_DAT1 = 163,
+	MX6SL_PAD_SD3_DAT2 = 164,
+	MX6SL_PAD_SD3_DAT3 = 165,
+	MX6SL_PAD_UART1_RXD = 166,
+	MX6SL_PAD_UART1_TXD = 167,
+	MX6SL_PAD_WDOG_B = 168,
+};
+
+/* Pad names for the pinmux subsystem */
+static const struct pinctrl_pin_desc imx6sl_pinctrl_pads[] = {
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE1),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE2),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE3),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE4),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE5),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE6),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE7),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE8),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE9),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE10),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE11),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE12),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE13),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE14),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE15),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE16),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE17),
+	IMX_PINCTRL_PIN(MX6SL_PAD_RESERVE18),
+	IMX_PINCTRL_PIN(MX6SL_PAD_AUD_MCLK),
+	IMX_PINCTRL_PIN(MX6SL_PAD_AUD_RXC),
+	IMX_PINCTRL_PIN(MX6SL_PAD_AUD_RXD),
+	IMX_PINCTRL_PIN(MX6SL_PAD_AUD_RXFS),
+	IMX_PINCTRL_PIN(MX6SL_PAD_AUD_TXC),
+	IMX_PINCTRL_PIN(MX6SL_PAD_AUD_TXD),
+	IMX_PINCTRL_PIN(MX6SL_PAD_AUD_TXFS),
+	IMX_PINCTRL_PIN(MX6SL_PAD_ECSPI1_MISO),
+	IMX_PINCTRL_PIN(MX6SL_PAD_ECSPI1_MOSI),
+	IMX_PINCTRL_PIN(MX6SL_PAD_ECSPI1_SCLK),
+	IMX_PINCTRL_PIN(MX6SL_PAD_ECSPI1_SS0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_ECSPI2_MISO),
+	IMX_PINCTRL_PIN(MX6SL_PAD_ECSPI2_MOSI),
+	IMX_PINCTRL_PIN(MX6SL_PAD_ECSPI2_SCLK),
+	IMX_PINCTRL_PIN(MX6SL_PAD_ECSPI2_SS0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_BDR0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_BDR1),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D1),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D10),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D11),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D12),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D13),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D14),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D15),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D2),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D3),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D4),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D5),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D6),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D7),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D8),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_D9),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_GDCLK),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_GDOE),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_GDRL),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_GDSP),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_PWRCOM),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_PWRCTRL0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_PWRCTRL1),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_PWRCTRL2),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_PWRCTRL3),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_PWRINT),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_PWRSTAT),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_PWRWAKEUP),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_SDCE0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_SDCE1),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_SDCE2),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_SDCE3),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_SDCLK),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_SDLE),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_SDOE),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_SDSHR),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_VCOM0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_EPDC_VCOM1),
+	IMX_PINCTRL_PIN(MX6SL_PAD_FEC_CRS_DV),
+	IMX_PINCTRL_PIN(MX6SL_PAD_FEC_MDC),
+	IMX_PINCTRL_PIN(MX6SL_PAD_FEC_MDIO),
+	IMX_PINCTRL_PIN(MX6SL_PAD_FEC_REF_CLK),
+	IMX_PINCTRL_PIN(MX6SL_PAD_FEC_RX_ER),
+	IMX_PINCTRL_PIN(MX6SL_PAD_FEC_RXD0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_FEC_RXD1),
+	IMX_PINCTRL_PIN(MX6SL_PAD_FEC_TX_CLK),
+	IMX_PINCTRL_PIN(MX6SL_PAD_FEC_TX_EN),
+	IMX_PINCTRL_PIN(MX6SL_PAD_FEC_TXD0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_FEC_TXD1),
+	IMX_PINCTRL_PIN(MX6SL_PAD_HSIC_DAT),
+	IMX_PINCTRL_PIN(MX6SL_PAD_HSIC_STROBE),
+	IMX_PINCTRL_PIN(MX6SL_PAD_I2C1_SCL),
+	IMX_PINCTRL_PIN(MX6SL_PAD_I2C1_SDA),
+	IMX_PINCTRL_PIN(MX6SL_PAD_I2C2_SCL),
+	IMX_PINCTRL_PIN(MX6SL_PAD_I2C2_SDA),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_COL0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_COL1),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_COL2),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_COL3),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_COL4),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_COL5),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_COL6),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_COL7),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_ROW0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_ROW1),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_ROW2),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_ROW3),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_ROW4),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_ROW5),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_ROW6),
+	IMX_PINCTRL_PIN(MX6SL_PAD_KEY_ROW7),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_CLK),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT1),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT10),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT11),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT12),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT13),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT14),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT15),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT16),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT17),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT18),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT19),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT2),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT20),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT21),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT22),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT23),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT3),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT4),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT5),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT6),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT7),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT8),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_DAT9),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_ENABLE),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_HSYNC),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_RESET),
+	IMX_PINCTRL_PIN(MX6SL_PAD_LCD_VSYNC),
+	IMX_PINCTRL_PIN(MX6SL_PAD_PWM1),
+	IMX_PINCTRL_PIN(MX6SL_PAD_REF_CLK_24M),
+	IMX_PINCTRL_PIN(MX6SL_PAD_REF_CLK_32K),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD1_CLK),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD1_CMD),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD1_DAT0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD1_DAT1),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD1_DAT2),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD1_DAT3),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD1_DAT4),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD1_DAT5),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD1_DAT6),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD1_DAT7),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD2_CLK),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD2_CMD),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD2_DAT0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD2_DAT1),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD2_DAT2),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD2_DAT3),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD2_DAT4),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD2_DAT5),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD2_DAT6),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD2_DAT7),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD2_RST),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD3_CLK),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD3_CMD),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD3_DAT0),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD3_DAT1),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD3_DAT2),
+	IMX_PINCTRL_PIN(MX6SL_PAD_SD3_DAT3),
+	IMX_PINCTRL_PIN(MX6SL_PAD_UART1_RXD),
+	IMX_PINCTRL_PIN(MX6SL_PAD_UART1_TXD),
+	IMX_PINCTRL_PIN(MX6SL_PAD_WDOG_B),
+};
+
+static struct imx_pinctrl_soc_info imx6sl_pinctrl_info = {
+	.pins = imx6sl_pinctrl_pads,
+	.npins = ARRAY_SIZE(imx6sl_pinctrl_pads),
+};
+
+static struct of_device_id imx6sl_pinctrl_of_match[] = {
+	{ .compatible = "fsl,imx6sl-iomuxc", },
+	{ /* sentinel */ }
+};
+
+static int imx6sl_pinctrl_probe(struct platform_device *pdev)
+{
+	return imx_pinctrl_probe(pdev, &imx6sl_pinctrl_info);
+}
+
+static struct platform_driver imx6sl_pinctrl_driver = {
+	.driver = {
+		.name = "imx6sl-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(imx6sl_pinctrl_of_match),
+	},
+	.probe = imx6sl_pinctrl_probe,
+	.remove = imx_pinctrl_remove,
+};
+
+static int __init imx6sl_pinctrl_init(void)
+{
+	return platform_driver_register(&imx6sl_pinctrl_driver);
+}
+arch_initcall(imx6sl_pinctrl_init);
+
+static void __exit imx6sl_pinctrl_exit(void)
+{
+	platform_driver_unregister(&imx6sl_pinctrl_driver);
+}
+module_exit(imx6sl_pinctrl_exit);
+
+MODULE_AUTHOR("Shawn Guo <shawn.guo@linaro.org>");
+MODULE_DESCRIPTION("Freescale imx6sl pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-nomadik.c b/drivers/pinctrl/pinctrl-nomadik.c
index 435bf3078d2c..34281754b629 100644
--- a/drivers/pinctrl/pinctrl-nomadik.c
+++ b/drivers/pinctrl/pinctrl-nomadik.c
@@ -23,6 +23,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/irqdomain.h>
+#include <linux/irqchip/chained_irq.h>
 #include <linux/slab.h>
 #include <linux/of_device.h>
 #include <linux/of_address.h>
@@ -33,7 +34,6 @@
 /* Since we request GPIOs from ourself */
 #include <linux/pinctrl/consumer.h>
 #include <linux/platform_data/pinctrl-nomadik.h>
-#include <asm/mach/irq.h>
 #include "pinctrl-nomadik.h"
 #include "core.h"
 
diff --git a/drivers/pinctrl/pinctrl-samsung.c b/drivers/pinctrl/pinctrl-samsung.c
index 4f54faf2971f..976366899f68 100644
--- a/drivers/pinctrl/pinctrl-samsung.c
+++ b/drivers/pinctrl/pinctrl-samsung.c
@@ -970,6 +970,8 @@ static const struct of_device_id samsung_pinctrl_dt_match[] = {
 		.data = (void *)exynos4210_pin_ctrl },
 	{ .compatible = "samsung,exynos4x12-pinctrl",
 		.data = (void *)exynos4x12_pin_ctrl },
+	{ .compatible = "samsung,exynos5250-pinctrl",
+		.data = (void *)exynos5250_pin_ctrl },
 #endif
 #ifdef CONFIG_PINCTRL_S3C64XX
 	{ .compatible = "samsung,s3c64xx-pinctrl",
diff --git a/drivers/pinctrl/pinctrl-samsung.h b/drivers/pinctrl/pinctrl-samsung.h
index 45f27b41e30c..7c7f9ebcd05b 100644
--- a/drivers/pinctrl/pinctrl-samsung.h
+++ b/drivers/pinctrl/pinctrl-samsung.h
@@ -244,6 +244,7 @@ struct samsung_pmx_func {
 /* list of all exported SoC specific data */
 extern struct samsung_pin_ctrl exynos4210_pin_ctrl[];
 extern struct samsung_pin_ctrl exynos4x12_pin_ctrl[];
+extern struct samsung_pin_ctrl exynos5250_pin_ctrl[];
 extern struct samsung_pin_ctrl s3c64xx_pin_ctrl[];
 
 #endif /* __PINCTRL_SAMSUNG_H */
diff --git a/drivers/pinctrl/pinctrl-sirf.c b/drivers/pinctrl/pinctrl-sirf.c
index fb9062570745..bc9d1be27fb0 100644
--- a/drivers/pinctrl/pinctrl-sirf.c
+++ b/drivers/pinctrl/pinctrl-sirf.c
@@ -14,6 +14,7 @@
 #include <linux/slab.h>
 #include <linux/err.h>
 #include <linux/irqdomain.h>
+#include <linux/irqchip/chained_irq.h>
 #include <linux/pinctrl/pinctrl.h>
 #include <linux/pinctrl/pinmux.h>
 #include <linux/pinctrl/consumer.h>
@@ -25,7 +26,6 @@
 #include <linux/bitops.h>
 #include <linux/gpio.h>
 #include <linux/of_gpio.h>
-#include <asm/mach/irq.h>
 
 #define DRIVER_NAME "pinmux-sirf"
 
@@ -1347,7 +1347,7 @@ static inline int sirfsoc_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 	struct sirfsoc_gpio_bank *bank = container_of(to_of_mm_gpio_chip(chip),
 		struct sirfsoc_gpio_bank, chip);
 
-	return irq_find_mapping(bank->domain, offset);
+	return irq_create_mapping(bank->domain, offset);
 }
 
 static inline int sirfsoc_gpio_to_offset(unsigned int gpio)
@@ -1485,7 +1485,6 @@ static void sirfsoc_gpio_handle_irq(unsigned int irq, struct irq_desc *desc)
 	struct sirfsoc_gpio_bank *bank = irq_get_handler_data(irq);
 	u32 status, ctrl;
 	int idx = 0;
-	unsigned int first_irq;
 	struct irq_chip *chip = irq_get_chip(irq);
 
 	chained_irq_enter(chip, desc);
@@ -1499,8 +1498,6 @@ static void sirfsoc_gpio_handle_irq(unsigned int irq, struct irq_desc *desc)
 		return;
 	}
 
-	first_irq = bank->domain->revmap_data.legacy.first_irq;
-
 	while (status) {
 		ctrl = readl(bank->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, idx));
 
@@ -1511,7 +1508,7 @@ static void sirfsoc_gpio_handle_irq(unsigned int irq, struct irq_desc *desc)
 		if ((status & 0x1) && (ctrl & SIRFSOC_GPIO_CTL_INTR_EN_MASK)) {
 			pr_debug("%s: gpio id %d idx %d happens\n",
 				__func__, bank->id, idx);
-			generic_handle_irq(first_irq + idx);
+			generic_handle_irq(irq_find_mapping(bank->domain, idx));
 		}
 
 		idx++;
@@ -1764,9 +1761,8 @@ static int sirfsoc_gpio_probe(struct device_node *np)
 			goto out;
 		}
 
-		bank->domain = irq_domain_add_legacy(np, SIRFSOC_GPIO_BANK_SIZE,
-			SIRFSOC_GPIO_IRQ_START + i * SIRFSOC_GPIO_BANK_SIZE, 0,
-			&sirfsoc_gpio_irq_simple_ops, bank);
+		bank->domain = irq_domain_add_linear(np, SIRFSOC_GPIO_BANK_SIZE,
+						&sirfsoc_gpio_irq_simple_ops, bank);
 
 		if (!bank->domain) {
 			pr_err("%s: Failed to create irqdomain\n", np->full_name);
diff --git a/drivers/pinctrl/sh-pfc/Kconfig b/drivers/pinctrl/sh-pfc/Kconfig
index c3340f54d2ad..f8a2ae413c7f 100644
--- a/drivers/pinctrl/sh-pfc/Kconfig
+++ b/drivers/pinctrl/sh-pfc/Kconfig
@@ -6,10 +6,11 @@ if ARCH_SHMOBILE || SUPERH
 
 config PINCTRL_SH_PFC
 	# XXX move off the gpio dependency
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select GPIO_SH_PFC if ARCH_REQUIRE_GPIOLIB
 	select PINMUX
 	select PINCONF
+	select GENERIC_PINCONF
 	def_bool y
 	help
 	  This enables pin control drivers for SH and SH Mobile platforms
@@ -21,6 +22,11 @@ config GPIO_SH_PFC
 	  This enables support for GPIOs within the SoC's pin function
 	  controller.
 
+config PINCTRL_PFC_R8A73A4
+	def_bool y
+	depends on ARCH_R8A73A4
+	select PINCTRL_SH_PFC
+
 config PINCTRL_PFC_R8A7740
 	def_bool y
 	depends on ARCH_R8A7740
@@ -34,19 +40,19 @@ config PINCTRL_PFC_R8A7779
 config PINCTRL_PFC_SH7203
 	def_bool y
 	depends on CPU_SUBTYPE_SH7203
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select PINCTRL_SH_PFC
 
 config PINCTRL_PFC_SH7264
 	def_bool y
 	depends on CPU_SUBTYPE_SH7264
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select PINCTRL_SH_PFC
 
 config PINCTRL_PFC_SH7269
 	def_bool y
 	depends on CPU_SUBTYPE_SH7269
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select PINCTRL_SH_PFC
 
 config PINCTRL_PFC_SH7372
@@ -62,55 +68,55 @@ config PINCTRL_PFC_SH73A0
 config PINCTRL_PFC_SH7720
 	def_bool y
 	depends on CPU_SUBTYPE_SH7720
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select PINCTRL_SH_PFC
 
 config PINCTRL_PFC_SH7722
 	def_bool y
 	depends on CPU_SUBTYPE_SH7722
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select PINCTRL_SH_PFC
 
 config PINCTRL_PFC_SH7723
 	def_bool y
 	depends on CPU_SUBTYPE_SH7723
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select PINCTRL_SH_PFC
 
 config PINCTRL_PFC_SH7724
 	def_bool y
 	depends on CPU_SUBTYPE_SH7724
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select PINCTRL_SH_PFC
 
 config PINCTRL_PFC_SH7734
 	def_bool y
 	depends on CPU_SUBTYPE_SH7734
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select PINCTRL_SH_PFC
 
 config PINCTRL_PFC_SH7757
 	def_bool y
 	depends on CPU_SUBTYPE_SH7757
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select PINCTRL_SH_PFC
 
 config PINCTRL_PFC_SH7785
 	def_bool y
 	depends on CPU_SUBTYPE_SH7785
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select PINCTRL_SH_PFC
 
 config PINCTRL_PFC_SH7786
 	def_bool y
 	depends on CPU_SUBTYPE_SH7786
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select PINCTRL_SH_PFC
 
 config PINCTRL_PFC_SHX3
 	def_bool y
 	depends on CPU_SUBTYPE_SHX3
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select PINCTRL_SH_PFC
 
 endif
diff --git a/drivers/pinctrl/sh-pfc/Makefile b/drivers/pinctrl/sh-pfc/Makefile
index e8b9562c47e1..211cd8e98a8a 100644
--- a/drivers/pinctrl/sh-pfc/Makefile
+++ b/drivers/pinctrl/sh-pfc/Makefile
@@ -3,6 +3,7 @@ ifeq ($(CONFIG_GPIO_SH_PFC),y)
 sh-pfc-objs			+= gpio.o
 endif
 obj-$(CONFIG_PINCTRL_SH_PFC)	+= sh-pfc.o
+obj-$(CONFIG_PINCTRL_PFC_R8A73A4)	+= pfc-r8a73a4.o
 obj-$(CONFIG_PINCTRL_PFC_R8A7740)	+= pfc-r8a7740.o
 obj-$(CONFIG_PINCTRL_PFC_R8A7779)	+= pfc-r8a7779.o
 obj-$(CONFIG_PINCTRL_PFC_SH7203)	+= pfc-sh7203.o
diff --git a/drivers/pinctrl/sh-pfc/core.c b/drivers/pinctrl/sh-pfc/core.c
index 970ddff2b0b6..b551336924a5 100644
--- a/drivers/pinctrl/sh-pfc/core.c
+++ b/drivers/pinctrl/sh-pfc/core.c
@@ -10,7 +10,6 @@
  */
 
 #define DRV_NAME "sh-pfc"
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/bitops.h>
 #include <linux/err.h>
@@ -30,10 +29,8 @@ static int sh_pfc_ioremap(struct sh_pfc *pfc, struct platform_device *pdev)
 	struct resource *res;
 	int k;
 
-	if (pdev->num_resources == 0) {
-		pfc->num_windows = 0;
-		return 0;
-	}
+	if (pdev->num_resources == 0)
+		return -EINVAL;
 
 	pfc->window = devm_kzalloc(pfc->dev, pdev->num_resources *
 				   sizeof(*pfc->window), GFP_NOWAIT);
@@ -59,11 +56,11 @@ static void __iomem *sh_pfc_phys_to_virt(struct sh_pfc *pfc,
 					 unsigned long address)
 {
 	struct sh_pfc_window *window;
-	int k;
+	unsigned int i;
 
 	/* scan through physical windows and convert address */
-	for (k = 0; k < pfc->num_windows; k++) {
-		window = pfc->window + k;
+	for (i = 0; i < pfc->num_windows; i++) {
+		window = pfc->window + i;
 
 		if (address < window->phys)
 			continue;
@@ -74,11 +71,33 @@ static void __iomem *sh_pfc_phys_to_virt(struct sh_pfc *pfc,
 		return window->virt + (address - window->phys);
 	}
 
-	/* no windows defined, register must be 1:1 mapped virt:phys */
-	return (void __iomem *)address;
+	BUG();
+	return NULL;
+}
+
+int sh_pfc_get_pin_index(struct sh_pfc *pfc, unsigned int pin)
+{
+	unsigned int offset;
+	unsigned int i;
+
+	if (pfc->info->ranges == NULL)
+		return pin;
+
+	for (i = 0, offset = 0; i < pfc->info->nr_ranges; ++i) {
+		const struct pinmux_range *range = &pfc->info->ranges[i];
+
+		if (pin <= range->end)
+			return pin >= range->begin
+			     ? offset + pin - range->begin : -1;
+
+		offset += range->end - range->begin + 1;
+	}
+
+	return -EINVAL;
 }
 
-static int sh_pfc_enum_in_range(pinmux_enum_t enum_id, struct pinmux_range *r)
+static int sh_pfc_enum_in_range(pinmux_enum_t enum_id,
+				const struct pinmux_range *r)
 {
 	if (enum_id < r->begin)
 		return 0;
@@ -89,8 +108,8 @@ static int sh_pfc_enum_in_range(pinmux_enum_t enum_id, struct pinmux_range *r)
 	return 1;
 }
 
-static unsigned long sh_pfc_read_raw_reg(void __iomem *mapped_reg,
-					 unsigned long reg_width)
+unsigned long sh_pfc_read_raw_reg(void __iomem *mapped_reg,
+				  unsigned long reg_width)
 {
 	switch (reg_width) {
 	case 8:
@@ -105,8 +124,8 @@ static unsigned long sh_pfc_read_raw_reg(void __iomem *mapped_reg,
 	return 0;
 }
 
-static void sh_pfc_write_raw_reg(void __iomem *mapped_reg,
-				 unsigned long reg_width, unsigned long data)
+void sh_pfc_write_raw_reg(void __iomem *mapped_reg, unsigned long reg_width,
+			  unsigned long data)
 {
 	switch (reg_width) {
 	case 8:
@@ -123,39 +142,8 @@ static void sh_pfc_write_raw_reg(void __iomem *mapped_reg,
 	BUG();
 }
 
-int sh_pfc_read_bit(struct pinmux_data_reg *dr, unsigned long in_pos)
-{
-	unsigned long pos;
-
-	pos = dr->reg_width - (in_pos + 1);
-
-	pr_debug("read_bit: addr = %lx, pos = %ld, "
-		 "r_width = %ld\n", dr->reg, pos, dr->reg_width);
-
-	return (sh_pfc_read_raw_reg(dr->mapped_reg, dr->reg_width) >> pos) & 1;
-}
-
-void sh_pfc_write_bit(struct pinmux_data_reg *dr, unsigned long in_pos,
-		      unsigned long value)
-{
-	unsigned long pos;
-
-	pos = dr->reg_width - (in_pos + 1);
-
-	pr_debug("write_bit addr = %lx, value = %d, pos = %ld, "
-		 "r_width = %ld\n",
-		 dr->reg, !!value, pos, dr->reg_width);
-
-	if (value)
-		set_bit(pos, &dr->reg_shadow);
-	else
-		clear_bit(pos, &dr->reg_shadow);
-
-	sh_pfc_write_raw_reg(dr->mapped_reg, dr->reg_width, dr->reg_shadow);
-}
-
 static void sh_pfc_config_reg_helper(struct sh_pfc *pfc,
-				     struct pinmux_cfg_reg *crp,
+				     const struct pinmux_cfg_reg *crp,
 				     unsigned long in_pos,
 				     void __iomem **mapped_regp,
 				     unsigned long *maskp,
@@ -176,24 +164,8 @@ static void sh_pfc_config_reg_helper(struct sh_pfc *pfc,
 	}
 }
 
-static int sh_pfc_read_config_reg(struct sh_pfc *pfc,
-				  struct pinmux_cfg_reg *crp,
-				  unsigned long field)
-{
-	void __iomem *mapped_reg;
-	unsigned long mask, pos;
-
-	sh_pfc_config_reg_helper(pfc, crp, field, &mapped_reg, &mask, &pos);
-
-	pr_debug("read_reg: addr = %lx, field = %ld, "
-		 "r_width = %ld, f_width = %ld\n",
-		 crp->reg, field, crp->reg_width, crp->field_width);
-
-	return (sh_pfc_read_raw_reg(mapped_reg, crp->reg_width) >> pos) & mask;
-}
-
 static void sh_pfc_write_config_reg(struct sh_pfc *pfc,
-				    struct pinmux_cfg_reg *crp,
+				    const struct pinmux_cfg_reg *crp,
 				    unsigned long field, unsigned long value)
 {
 	void __iomem *mapped_reg;
@@ -201,9 +173,9 @@ static void sh_pfc_write_config_reg(struct sh_pfc *pfc,
 
 	sh_pfc_config_reg_helper(pfc, crp, field, &mapped_reg, &mask, &pos);
 
-	pr_debug("write_reg addr = %lx, value = %ld, field = %ld, "
-		 "r_width = %ld, f_width = %ld\n",
-		 crp->reg, value, field, crp->reg_width, crp->field_width);
+	dev_dbg(pfc->dev, "write_reg addr = %lx, value = %ld, field = %ld, "
+		"r_width = %ld, f_width = %ld\n",
+		crp->reg, value, field, crp->reg_width, crp->field_width);
 
 	mask = ~(mask << pos);
 	value = value << pos;
@@ -220,83 +192,11 @@ static void sh_pfc_write_config_reg(struct sh_pfc *pfc,
 	sh_pfc_write_raw_reg(mapped_reg, crp->reg_width, data);
 }
 
-static int sh_pfc_setup_data_reg(struct sh_pfc *pfc, unsigned gpio)
-{
-	struct pinmux_gpio *gpiop = &pfc->info->gpios[gpio];
-	struct pinmux_data_reg *data_reg;
-	int k, n;
-
-	if (!sh_pfc_enum_in_range(gpiop->enum_id, &pfc->info->data))
-		return -1;
-
-	k = 0;
-	while (1) {
-		data_reg = pfc->info->data_regs + k;
-
-		if (!data_reg->reg_width)
-			break;
-
-		data_reg->mapped_reg = sh_pfc_phys_to_virt(pfc, data_reg->reg);
-
-		for (n = 0; n < data_reg->reg_width; n++) {
-			if (data_reg->enum_ids[n] == gpiop->enum_id) {
-				gpiop->flags &= ~PINMUX_FLAG_DREG;
-				gpiop->flags |= (k << PINMUX_FLAG_DREG_SHIFT);
-				gpiop->flags &= ~PINMUX_FLAG_DBIT;
-				gpiop->flags |= (n << PINMUX_FLAG_DBIT_SHIFT);
-				return 0;
-			}
-		}
-		k++;
-	}
-
-	BUG();
-
-	return -1;
-}
-
-static void sh_pfc_setup_data_regs(struct sh_pfc *pfc)
-{
-	struct pinmux_data_reg *drp;
-	int k;
-
-	for (k = pfc->info->first_gpio; k <= pfc->info->last_gpio; k++)
-		sh_pfc_setup_data_reg(pfc, k);
-
-	k = 0;
-	while (1) {
-		drp = pfc->info->data_regs + k;
-
-		if (!drp->reg_width)
-			break;
-
-		drp->reg_shadow = sh_pfc_read_raw_reg(drp->mapped_reg,
-						      drp->reg_width);
-		k++;
-	}
-}
-
-int sh_pfc_get_data_reg(struct sh_pfc *pfc, unsigned gpio,
-			struct pinmux_data_reg **drp, int *bitp)
-{
-	struct pinmux_gpio *gpiop = &pfc->info->gpios[gpio];
-	int k, n;
-
-	if (!sh_pfc_enum_in_range(gpiop->enum_id, &pfc->info->data))
-		return -1;
-
-	k = (gpiop->flags & PINMUX_FLAG_DREG) >> PINMUX_FLAG_DREG_SHIFT;
-	n = (gpiop->flags & PINMUX_FLAG_DBIT) >> PINMUX_FLAG_DBIT_SHIFT;
-	*drp = pfc->info->data_regs + k;
-	*bitp = n;
-	return 0;
-}
-
 static int sh_pfc_get_config_reg(struct sh_pfc *pfc, pinmux_enum_t enum_id,
-				 struct pinmux_cfg_reg **crp, int *fieldp,
-				 int *valuep, unsigned long **cntp)
+				 const struct pinmux_cfg_reg **crp, int *fieldp,
+				 int *valuep)
 {
-	struct pinmux_cfg_reg *config_reg;
+	const struct pinmux_cfg_reg *config_reg;
 	unsigned long r_width, f_width, curr_width, ncomb;
 	int k, m, n, pos, bit_pos;
 
@@ -324,7 +224,6 @@ static int sh_pfc_get_config_reg(struct sh_pfc *pfc, pinmux_enum_t enum_id,
 					*crp = config_reg;
 					*fieldp = m;
 					*valuep = n;
-					*cntp = &config_reg->cnt[m];
 					return 0;
 				}
 			}
@@ -334,50 +233,42 @@ static int sh_pfc_get_config_reg(struct sh_pfc *pfc, pinmux_enum_t enum_id,
 		k++;
 	}
 
-	return -1;
+	return -EINVAL;
 }
 
-int sh_pfc_gpio_to_enum(struct sh_pfc *pfc, unsigned gpio, int pos,
-			pinmux_enum_t *enum_idp)
+static int sh_pfc_mark_to_enum(struct sh_pfc *pfc, pinmux_enum_t mark, int pos,
+			      pinmux_enum_t *enum_idp)
 {
-	pinmux_enum_t enum_id = pfc->info->gpios[gpio].enum_id;
-	pinmux_enum_t *data = pfc->info->gpio_data;
+	const pinmux_enum_t *data = pfc->info->gpio_data;
 	int k;
 
-	if (!sh_pfc_enum_in_range(enum_id, &pfc->info->data)) {
-		if (!sh_pfc_enum_in_range(enum_id, &pfc->info->mark)) {
-			pr_err("non data/mark enum_id for gpio %d\n", gpio);
-			return -1;
-		}
-	}
-
 	if (pos) {
 		*enum_idp = data[pos + 1];
 		return pos + 1;
 	}
 
 	for (k = 0; k < pfc->info->gpio_data_size; k++) {
-		if (data[k] == enum_id) {
+		if (data[k] == mark) {
 			*enum_idp = data[k + 1];
 			return k + 1;
 		}
 	}
 
-	pr_err("cannot locate data/mark enum_id for gpio %d\n", gpio);
-	return -1;
+	dev_err(pfc->dev, "cannot locate data/mark enum_id for mark %d\n",
+		mark);
+	return -EINVAL;
 }
 
-int sh_pfc_config_gpio(struct sh_pfc *pfc, unsigned gpio, int pinmux_type,
-		       int cfg_mode)
+int sh_pfc_config_mux(struct sh_pfc *pfc, unsigned mark, int pinmux_type)
 {
-	struct pinmux_cfg_reg *cr = NULL;
+	const struct pinmux_cfg_reg *cr = NULL;
 	pinmux_enum_t enum_id;
-	struct pinmux_range *range;
+	const struct pinmux_range *range;
 	int in_range, pos, field, value;
-	unsigned long *cntp;
+	int ret;
 
 	switch (pinmux_type) {
-
+	case PINMUX_TYPE_GPIO:
 	case PINMUX_TYPE_FUNCTION:
 		range = NULL;
 		break;
@@ -399,33 +290,37 @@ int sh_pfc_config_gpio(struct sh_pfc *pfc, unsigned gpio, int pinmux_type,
 		break;
 
 	default:
-		goto out_err;
+		return -EINVAL;
 	}
 
 	pos = 0;
 	enum_id = 0;
 	field = 0;
 	value = 0;
+
+	/* Iterate over all the configuration fields we need to update. */
 	while (1) {
-		pos = sh_pfc_gpio_to_enum(pfc, gpio, pos, &enum_id);
-		if (pos <= 0)
-			goto out_err;
+		pos = sh_pfc_mark_to_enum(pfc, mark, pos, &enum_id);
+		if (pos < 0)
+			return pos;
 
 		if (!enum_id)
 			break;
 
-		/* first check if this is a function enum */
+		/* Check if the configuration field selects a function. If it
+		 * doesn't, skip the field if it's not applicable to the
+		 * requested pinmux type.
+		 */
 		in_range = sh_pfc_enum_in_range(enum_id, &pfc->info->function);
 		if (!in_range) {
-			/* not a function enum */
-			if (range) {
-				/*
-				 * other range exists, so this pin is
-				 * a regular GPIO pin that now is being
-				 * bound to a specific direction.
-				 *
-				 * for this case we only allow function enums
-				 * and the enums that match the other range.
+			if (pinmux_type == PINMUX_TYPE_FUNCTION) {
+				/* Functions are allowed to modify all
+				 * fields.
+				 */
+				in_range = 1;
+			} else if (pinmux_type != PINMUX_TYPE_GPIO) {
+				/* Input/output types can only modify fields
+				 * that correspond to their respective ranges.
 				 */
 				in_range = sh_pfc_enum_in_range(enum_id, range);
 
@@ -436,60 +331,29 @@ int sh_pfc_config_gpio(struct sh_pfc *pfc, unsigned gpio, int pinmux_type,
 				 */
 				if (in_range && enum_id == range->force)
 					continue;
-			} else {
-				/*
-				 * no other range exists, so this pin
-				 * must then be of the function type.
-				 *
-				 * allow function type pins to select
-				 * any combination of function/in/out
-				 * in their MARK lists.
-				 */
-				in_range = 1;
 			}
+			/* GPIOs are only allowed to modify function fields. */
 		}
 
 		if (!in_range)
 			continue;
 
-		if (sh_pfc_get_config_reg(pfc, enum_id, &cr,
-					  &field, &value, &cntp) != 0)
-			goto out_err;
-
-		switch (cfg_mode) {
-		case GPIO_CFG_DRYRUN:
-			if (!*cntp ||
-			    (sh_pfc_read_config_reg(pfc, cr, field) != value))
-				continue;
-			break;
-
-		case GPIO_CFG_REQ:
-			sh_pfc_write_config_reg(pfc, cr, field, value);
-			*cntp = *cntp + 1;
-			break;
+		ret = sh_pfc_get_config_reg(pfc, enum_id, &cr, &field, &value);
+		if (ret < 0)
+			return ret;
 
-		case GPIO_CFG_FREE:
-			*cntp = *cntp - 1;
-			break;
-		}
+		sh_pfc_write_config_reg(pfc, cr, field, value);
 	}
 
 	return 0;
- out_err:
-	return -1;
 }
 
 static int sh_pfc_probe(struct platform_device *pdev)
 {
-	struct sh_pfc_soc_info *info;
+	const struct sh_pfc_soc_info *info;
 	struct sh_pfc *pfc;
 	int ret;
 
-	/*
-	 * Ensure that the type encoding fits
-	 */
-	BUILD_BUG_ON(PINMUX_FLAG_TYPE > ((1 << PINMUX_FLAG_DBIT_SHIFT) - 1));
-
 	info = pdev->id_entry->driver_data
 	      ? (void *)pdev->id_entry->driver_data : pdev->dev.platform_data;
 	if (info == NULL)
@@ -509,7 +373,6 @@ static int sh_pfc_probe(struct platform_device *pdev)
 	spin_lock_init(&pfc->lock);
 
 	pinctrl_provide_dummies();
-	sh_pfc_setup_data_regs(pfc);
 
 	/*
 	 * Initialize pinctrl bindings first
@@ -529,13 +392,13 @@ static int sh_pfc_probe(struct platform_device *pdev)
 		 * PFC state as it is, given that there are already
 		 * extant users of it that have succeeded by this point.
 		 */
-		pr_notice("failed to init GPIO chip, ignoring...\n");
+		dev_notice(pfc->dev, "failed to init GPIO chip, ignoring...\n");
 	}
 #endif
 
 	platform_set_drvdata(pdev, pfc);
 
-	pr_info("%s support registered\n", info->name);
+	dev_info(pfc->dev, "%s support registered\n", info->name);
 
 	return 0;
 }
@@ -555,6 +418,9 @@ static int sh_pfc_remove(struct platform_device *pdev)
 }
 
 static const struct platform_device_id sh_pfc_id_table[] = {
+#ifdef CONFIG_PINCTRL_PFC_R8A73A4
+	{ "pfc-r8a73a4", (kernel_ulong_t)&r8a73a4_pinmux_info },
+#endif
 #ifdef CONFIG_PINCTRL_PFC_R8A7740
 	{ "pfc-r8a7740", (kernel_ulong_t)&r8a7740_pinmux_info },
 #endif
diff --git a/drivers/pinctrl/sh-pfc/core.h b/drivers/pinctrl/sh-pfc/core.h
index ba7c33c33599..89cb4289d761 100644
--- a/drivers/pinctrl/sh-pfc/core.h
+++ b/drivers/pinctrl/sh-pfc/core.h
@@ -26,13 +26,17 @@ struct sh_pfc_pinctrl;
 
 struct sh_pfc {
 	struct device *dev;
-	struct sh_pfc_soc_info *info;
+	const struct sh_pfc_soc_info *info;
 	spinlock_t lock;
 
 	unsigned int num_windows;
 	struct sh_pfc_window *window;
 
+	unsigned int nr_pins;
+
 	struct sh_pfc_chip *gpio;
+	struct sh_pfc_chip *func;
+
 	struct sh_pfc_pinctrl *pinctrl;
 };
 
@@ -42,31 +46,30 @@ int sh_pfc_unregister_gpiochip(struct sh_pfc *pfc);
 int sh_pfc_register_pinctrl(struct sh_pfc *pfc);
 int sh_pfc_unregister_pinctrl(struct sh_pfc *pfc);
 
-int sh_pfc_read_bit(struct pinmux_data_reg *dr, unsigned long in_pos);
-void sh_pfc_write_bit(struct pinmux_data_reg *dr, unsigned long in_pos,
-		      unsigned long value);
-int sh_pfc_get_data_reg(struct sh_pfc *pfc, unsigned gpio,
-			struct pinmux_data_reg **drp, int *bitp);
-int sh_pfc_gpio_to_enum(struct sh_pfc *pfc, unsigned gpio, int pos,
-			pinmux_enum_t *enum_idp);
-int sh_pfc_config_gpio(struct sh_pfc *pfc, unsigned gpio, int pinmux_type,
-		       int cfg_mode);
+unsigned long sh_pfc_read_raw_reg(void __iomem *mapped_reg,
+				  unsigned long reg_width);
+void sh_pfc_write_raw_reg(void __iomem *mapped_reg, unsigned long reg_width,
+			  unsigned long data);
+
+int sh_pfc_get_pin_index(struct sh_pfc *pfc, unsigned int pin);
+int sh_pfc_config_mux(struct sh_pfc *pfc, unsigned mark, int pinmux_type);
 
-extern struct sh_pfc_soc_info r8a7740_pinmux_info;
-extern struct sh_pfc_soc_info r8a7779_pinmux_info;
-extern struct sh_pfc_soc_info sh7203_pinmux_info;
-extern struct sh_pfc_soc_info sh7264_pinmux_info;
-extern struct sh_pfc_soc_info sh7269_pinmux_info;
-extern struct sh_pfc_soc_info sh7372_pinmux_info;
-extern struct sh_pfc_soc_info sh73a0_pinmux_info;
-extern struct sh_pfc_soc_info sh7720_pinmux_info;
-extern struct sh_pfc_soc_info sh7722_pinmux_info;
-extern struct sh_pfc_soc_info sh7723_pinmux_info;
-extern struct sh_pfc_soc_info sh7724_pinmux_info;
-extern struct sh_pfc_soc_info sh7734_pinmux_info;
-extern struct sh_pfc_soc_info sh7757_pinmux_info;
-extern struct sh_pfc_soc_info sh7785_pinmux_info;
-extern struct sh_pfc_soc_info sh7786_pinmux_info;
-extern struct sh_pfc_soc_info shx3_pinmux_info;
+extern const struct sh_pfc_soc_info r8a73a4_pinmux_info;
+extern const struct sh_pfc_soc_info r8a7740_pinmux_info;
+extern const struct sh_pfc_soc_info r8a7779_pinmux_info;
+extern const struct sh_pfc_soc_info sh7203_pinmux_info;
+extern const struct sh_pfc_soc_info sh7264_pinmux_info;
+extern const struct sh_pfc_soc_info sh7269_pinmux_info;
+extern const struct sh_pfc_soc_info sh7372_pinmux_info;
+extern const struct sh_pfc_soc_info sh73a0_pinmux_info;
+extern const struct sh_pfc_soc_info sh7720_pinmux_info;
+extern const struct sh_pfc_soc_info sh7722_pinmux_info;
+extern const struct sh_pfc_soc_info sh7723_pinmux_info;
+extern const struct sh_pfc_soc_info sh7724_pinmux_info;
+extern const struct sh_pfc_soc_info sh7734_pinmux_info;
+extern const struct sh_pfc_soc_info sh7757_pinmux_info;
+extern const struct sh_pfc_soc_info sh7785_pinmux_info;
+extern const struct sh_pfc_soc_info sh7786_pinmux_info;
+extern const struct sh_pfc_soc_info shx3_pinmux_info;
 
 #endif /* __SH_PFC_CORE_H__ */
diff --git a/drivers/pinctrl/sh-pfc/gpio.c b/drivers/pinctrl/sh-pfc/gpio.c
index a535075c8b69..d37efa7dcf90 100644
--- a/drivers/pinctrl/sh-pfc/gpio.c
+++ b/drivers/pinctrl/sh-pfc/gpio.c
@@ -9,8 +9,6 @@
  * for more details.
  */
 
-#define pr_fmt(fmt) KBUILD_MODNAME " gpio: " fmt
-
 #include <linux/device.h>
 #include <linux/gpio.h>
 #include <linux/init.h>
@@ -21,9 +19,23 @@
 
 #include "core.h"
 
+struct sh_pfc_gpio_data_reg {
+	const struct pinmux_data_reg *info;
+	unsigned long shadow;
+};
+
+struct sh_pfc_gpio_pin {
+	u8 dbit;
+	u8 dreg;
+};
+
 struct sh_pfc_chip {
-	struct sh_pfc		*pfc;
-	struct gpio_chip	gpio_chip;
+	struct sh_pfc			*pfc;
+	struct gpio_chip		gpio_chip;
+
+	struct sh_pfc_window		*mem;
+	struct sh_pfc_gpio_data_reg	*regs;
+	struct sh_pfc_gpio_pin		*pins;
 };
 
 static struct sh_pfc_chip *gpio_to_pfc_chip(struct gpio_chip *gc)
@@ -36,143 +48,367 @@ static struct sh_pfc *gpio_to_pfc(struct gpio_chip *gc)
 	return gpio_to_pfc_chip(gc)->pfc;
 }
 
-static int sh_gpio_request(struct gpio_chip *gc, unsigned offset)
+static void gpio_get_data_reg(struct sh_pfc_chip *chip, unsigned int gpio,
+			      struct sh_pfc_gpio_data_reg **reg,
+			      unsigned int *bit)
 {
-	return pinctrl_request_gpio(offset);
+	int idx = sh_pfc_get_pin_index(chip->pfc, gpio);
+	struct sh_pfc_gpio_pin *gpio_pin = &chip->pins[idx];
+
+	*reg = &chip->regs[gpio_pin->dreg];
+	*bit = gpio_pin->dbit;
 }
 
-static void sh_gpio_free(struct gpio_chip *gc, unsigned offset)
+static unsigned long gpio_read_data_reg(struct sh_pfc_chip *chip,
+					const struct pinmux_data_reg *dreg)
 {
-	pinctrl_free_gpio(offset);
+	void __iomem *mem = dreg->reg - chip->mem->phys + chip->mem->virt;
+
+	return sh_pfc_read_raw_reg(mem, dreg->reg_width);
 }
 
-static void sh_gpio_set_value(struct sh_pfc *pfc, unsigned gpio, int value)
+static void gpio_write_data_reg(struct sh_pfc_chip *chip,
+				const struct pinmux_data_reg *dreg,
+				unsigned long value)
 {
-	struct pinmux_data_reg *dr = NULL;
-	int bit = 0;
+	void __iomem *mem = dreg->reg - chip->mem->phys + chip->mem->virt;
 
-	if (sh_pfc_get_data_reg(pfc, gpio, &dr, &bit) != 0)
-		BUG();
-	else
-		sh_pfc_write_bit(dr, bit, value);
+	sh_pfc_write_raw_reg(mem, dreg->reg_width, value);
 }
 
-static int sh_gpio_get_value(struct sh_pfc *pfc, unsigned gpio)
+static void gpio_setup_data_reg(struct sh_pfc_chip *chip, unsigned gpio)
 {
-	struct pinmux_data_reg *dr = NULL;
-	int bit = 0;
+	struct sh_pfc *pfc = chip->pfc;
+	struct sh_pfc_gpio_pin *gpio_pin = &chip->pins[gpio];
+	const struct sh_pfc_pin *pin = &pfc->info->pins[gpio];
+	const struct pinmux_data_reg *dreg;
+	unsigned int bit;
+	unsigned int i;
+
+	for (i = 0, dreg = pfc->info->data_regs; dreg->reg; ++i, ++dreg) {
+		for (bit = 0; bit < dreg->reg_width; bit++) {
+			if (dreg->enum_ids[bit] == pin->enum_id) {
+				gpio_pin->dreg = i;
+				gpio_pin->dbit = bit;
+				return;
+			}
+		}
+	}
 
-	if (sh_pfc_get_data_reg(pfc, gpio, &dr, &bit) != 0)
+	BUG();
+}
+
+static int gpio_setup_data_regs(struct sh_pfc_chip *chip)
+{
+	struct sh_pfc *pfc = chip->pfc;
+	const struct pinmux_data_reg *dreg;
+	unsigned int i;
+
+	/* Count the number of data registers, allocate memory and initialize
+	 * them.
+	 */
+	for (i = 0; pfc->info->data_regs[i].reg_width; ++i)
+		;
+
+	chip->regs = devm_kzalloc(pfc->dev, i * sizeof(*chip->regs),
+				  GFP_KERNEL);
+	if (chip->regs == NULL)
+		return -ENOMEM;
+
+	for (i = 0, dreg = pfc->info->data_regs; dreg->reg_width; ++i, ++dreg) {
+		chip->regs[i].info = dreg;
+		chip->regs[i].shadow = gpio_read_data_reg(chip, dreg);
+	}
+
+	for (i = 0; i < pfc->info->nr_pins; i++) {
+		if (pfc->info->pins[i].enum_id == 0)
+			continue;
+
+		gpio_setup_data_reg(chip, i);
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Pin GPIOs
+ */
+
+static int gpio_pin_request(struct gpio_chip *gc, unsigned offset)
+{
+	struct sh_pfc *pfc = gpio_to_pfc(gc);
+	int idx = sh_pfc_get_pin_index(pfc, offset);
+
+	if (idx < 0 || pfc->info->pins[idx].enum_id == 0)
 		return -EINVAL;
 
-	return sh_pfc_read_bit(dr, bit);
+	return pinctrl_request_gpio(offset);
+}
+
+static void gpio_pin_free(struct gpio_chip *gc, unsigned offset)
+{
+	return pinctrl_free_gpio(offset);
+}
+
+static void gpio_pin_set_value(struct sh_pfc_chip *chip, unsigned offset,
+			       int value)
+{
+	struct sh_pfc_gpio_data_reg *reg;
+	unsigned long pos;
+	unsigned int bit;
+
+	gpio_get_data_reg(chip, offset, &reg, &bit);
+
+	pos = reg->info->reg_width - (bit + 1);
+
+	if (value)
+		set_bit(pos, &reg->shadow);
+	else
+		clear_bit(pos, &reg->shadow);
+
+	gpio_write_data_reg(chip, reg->info, reg->shadow);
 }
 
-static int sh_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
+static int gpio_pin_direction_input(struct gpio_chip *gc, unsigned offset)
 {
 	return pinctrl_gpio_direction_input(offset);
 }
 
-static int sh_gpio_direction_output(struct gpio_chip *gc, unsigned offset,
+static int gpio_pin_direction_output(struct gpio_chip *gc, unsigned offset,
 				    int value)
 {
-	sh_gpio_set_value(gpio_to_pfc(gc), offset, value);
+	gpio_pin_set_value(gpio_to_pfc_chip(gc), offset, value);
 
 	return pinctrl_gpio_direction_output(offset);
 }
 
-static int sh_gpio_get(struct gpio_chip *gc, unsigned offset)
+static int gpio_pin_get(struct gpio_chip *gc, unsigned offset)
 {
-	return sh_gpio_get_value(gpio_to_pfc(gc), offset);
+	struct sh_pfc_chip *chip = gpio_to_pfc_chip(gc);
+	struct sh_pfc_gpio_data_reg *reg;
+	unsigned long pos;
+	unsigned int bit;
+
+	gpio_get_data_reg(chip, offset, &reg, &bit);
+
+	pos = reg->info->reg_width - (bit + 1);
+
+	return (gpio_read_data_reg(chip, reg->info) >> pos) & 1;
 }
 
-static void sh_gpio_set(struct gpio_chip *gc, unsigned offset, int value)
+static void gpio_pin_set(struct gpio_chip *gc, unsigned offset, int value)
 {
-	sh_gpio_set_value(gpio_to_pfc(gc), offset, value);
+	gpio_pin_set_value(gpio_to_pfc_chip(gc), offset, value);
 }
 
-static int sh_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
+static int gpio_pin_to_irq(struct gpio_chip *gc, unsigned offset)
 {
 	struct sh_pfc *pfc = gpio_to_pfc(gc);
-	pinmux_enum_t enum_id;
-	pinmux_enum_t *enum_ids;
-	int i, k, pos;
-
-	pos = 0;
-	enum_id = 0;
-	while (1) {
-		pos = sh_pfc_gpio_to_enum(pfc, offset, pos, &enum_id);
-		if (pos <= 0 || !enum_id)
-			break;
+	int i, k;
 
-		for (i = 0; i < pfc->info->gpio_irq_size; i++) {
-			enum_ids = pfc->info->gpio_irq[i].enum_ids;
-			for (k = 0; enum_ids[k]; k++) {
-				if (enum_ids[k] == enum_id)
-					return pfc->info->gpio_irq[i].irq;
-			}
+	for (i = 0; i < pfc->info->gpio_irq_size; i++) {
+		unsigned short *gpios = pfc->info->gpio_irq[i].gpios;
+
+		for (k = 0; gpios[k]; k++) {
+			if (gpios[k] == offset)
+				return pfc->info->gpio_irq[i].irq;
 		}
 	}
 
 	return -ENOSYS;
 }
 
-static void sh_pfc_gpio_setup(struct sh_pfc_chip *chip)
+static int gpio_pin_setup(struct sh_pfc_chip *chip)
 {
 	struct sh_pfc *pfc = chip->pfc;
 	struct gpio_chip *gc = &chip->gpio_chip;
+	int ret;
+
+	chip->pins = devm_kzalloc(pfc->dev, pfc->nr_pins * sizeof(*chip->pins),
+				  GFP_KERNEL);
+	if (chip->pins == NULL)
+		return -ENOMEM;
+
+	ret = gpio_setup_data_regs(chip);
+	if (ret < 0)
+		return ret;
+
+	gc->request = gpio_pin_request;
+	gc->free = gpio_pin_free;
+	gc->direction_input = gpio_pin_direction_input;
+	gc->get = gpio_pin_get;
+	gc->direction_output = gpio_pin_direction_output;
+	gc->set = gpio_pin_set;
+	gc->to_irq = gpio_pin_to_irq;
 
-	gc->request = sh_gpio_request;
-	gc->free = sh_gpio_free;
-	gc->direction_input = sh_gpio_direction_input;
-	gc->get = sh_gpio_get;
-	gc->direction_output = sh_gpio_direction_output;
-	gc->set = sh_gpio_set;
-	gc->to_irq = sh_gpio_to_irq;
+	gc->label = pfc->info->name;
+	gc->dev = pfc->dev;
+	gc->owner = THIS_MODULE;
+	gc->base = 0;
+	gc->ngpio = pfc->nr_pins;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Function GPIOs
+ */
+
+static int gpio_function_request(struct gpio_chip *gc, unsigned offset)
+{
+	static bool __print_once;
+	struct sh_pfc *pfc = gpio_to_pfc(gc);
+	unsigned int mark = pfc->info->func_gpios[offset].enum_id;
+	unsigned long flags;
+	int ret;
+
+	if (!__print_once) {
+		dev_notice(pfc->dev,
+			   "Use of GPIO API for function requests is deprecated."
+			   " Convert to pinctrl\n");
+		__print_once = true;
+	}
+
+	if (mark == 0)
+		return -EINVAL;
 
-	WARN_ON(pfc->info->first_gpio != 0); /* needs testing */
+	spin_lock_irqsave(&pfc->lock, flags);
+	ret = sh_pfc_config_mux(pfc, mark, PINMUX_TYPE_FUNCTION);
+	spin_unlock_irqrestore(&pfc->lock, flags);
+
+	return ret;
+}
+
+static void gpio_function_free(struct gpio_chip *gc, unsigned offset)
+{
+}
+
+static int gpio_function_setup(struct sh_pfc_chip *chip)
+{
+	struct sh_pfc *pfc = chip->pfc;
+	struct gpio_chip *gc = &chip->gpio_chip;
+
+	gc->request = gpio_function_request;
+	gc->free = gpio_function_free;
 
 	gc->label = pfc->info->name;
 	gc->owner = THIS_MODULE;
-	gc->base = pfc->info->first_gpio;
-	gc->ngpio = (pfc->info->last_gpio - pfc->info->first_gpio) + 1;
+	gc->base = pfc->nr_pins;
+	gc->ngpio = pfc->info->nr_func_gpios;
+
+	return 0;
 }
 
-int sh_pfc_register_gpiochip(struct sh_pfc *pfc)
+/* -----------------------------------------------------------------------------
+ * Register/unregister
+ */
+
+static struct sh_pfc_chip *
+sh_pfc_add_gpiochip(struct sh_pfc *pfc, int(*setup)(struct sh_pfc_chip *),
+		    struct sh_pfc_window *mem)
 {
 	struct sh_pfc_chip *chip;
 	int ret;
 
 	chip = devm_kzalloc(pfc->dev, sizeof(*chip), GFP_KERNEL);
 	if (unlikely(!chip))
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
+	chip->mem = mem;
 	chip->pfc = pfc;
 
-	sh_pfc_gpio_setup(chip);
+	ret = setup(chip);
+	if (ret < 0)
+		return ERR_PTR(ret);
 
 	ret = gpiochip_add(&chip->gpio_chip);
 	if (unlikely(ret < 0))
-		return ret;
+		return ERR_PTR(ret);
+
+	dev_info(pfc->dev, "%s handling gpio %u -> %u\n",
+		 chip->gpio_chip.label, chip->gpio_chip.base,
+		 chip->gpio_chip.base + chip->gpio_chip.ngpio - 1);
+
+	return chip;
+}
+
+int sh_pfc_register_gpiochip(struct sh_pfc *pfc)
+{
+	const struct pinmux_range *ranges;
+	struct pinmux_range def_range;
+	struct sh_pfc_chip *chip;
+	unsigned int nr_ranges;
+	unsigned int i;
+	int ret;
+
+	if (pfc->info->data_regs == NULL)
+		return 0;
+
+	/* Find the memory window that contain the GPIO registers. Boards that
+	 * register a separate GPIO device will not supply a memory resource
+	 * that covers the data registers. In that case don't try to handle
+	 * GPIOs.
+	 */
+	for (i = 0; i < pfc->num_windows; ++i) {
+		struct sh_pfc_window *window = &pfc->window[i];
+
+		if (pfc->info->data_regs[0].reg >= window->phys &&
+		    pfc->info->data_regs[0].reg < window->phys + window->size)
+			break;
+	}
+
+	if (i == pfc->num_windows)
+		return 0;
+
+	/* Register the real GPIOs chip. */
+	chip = sh_pfc_add_gpiochip(pfc, gpio_pin_setup, &pfc->window[i]);
+	if (IS_ERR(chip))
+		return PTR_ERR(chip);
 
 	pfc->gpio = chip;
 
-	pr_info("%s handling gpio %d -> %d\n",
-		pfc->info->name, pfc->info->first_gpio,
-		pfc->info->last_gpio);
+	/* Register the GPIO to pin mappings. */
+	if (pfc->info->ranges == NULL) {
+		def_range.begin = 0;
+		def_range.end = pfc->info->nr_pins - 1;
+		ranges = &def_range;
+		nr_ranges = 1;
+	} else {
+		ranges = pfc->info->ranges;
+		nr_ranges = pfc->info->nr_ranges;
+	}
+
+	for (i = 0; i < nr_ranges; ++i) {
+		const struct pinmux_range *range = &ranges[i];
+
+		ret = gpiochip_add_pin_range(&chip->gpio_chip,
+					     dev_name(pfc->dev),
+					     range->begin, range->begin,
+					     range->end - range->begin + 1);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* Register the function GPIOs chip. */
+	if (pfc->info->nr_func_gpios == 0)
+		return 0;
+
+	chip = sh_pfc_add_gpiochip(pfc, gpio_function_setup, NULL);
+	if (IS_ERR(chip))
+		return PTR_ERR(chip);
+
+	pfc->func = chip;
 
 	return 0;
 }
 
 int sh_pfc_unregister_gpiochip(struct sh_pfc *pfc)
 {
-	struct sh_pfc_chip *chip = pfc->gpio;
+	int err;
 	int ret;
 
-	ret = gpiochip_remove(&chip->gpio_chip);
-	if (unlikely(ret < 0))
-		return ret;
+	ret = gpiochip_remove(&pfc->gpio->gpio_chip);
+	err = gpiochip_remove(&pfc->func->gpio_chip);
 
-	pfc->gpio = NULL;
-	return 0;
+	return ret < 0 ? ret : err;
 }
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c b/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c
new file mode 100644
index 000000000000..bbff5596e922
--- /dev/null
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c
@@ -0,0 +1,2587 @@
+/*
+ * Copyright (C) 2012-2013  Renesas Solutions Corp.
+ * Copyright (C) 2013  Magnus Damm
+ * Copyright (C) 2012  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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/pinctrl/pinconf-generic.h>
+#include <mach/irqs.h>
+#include <mach/r8a73a4.h>
+
+#include "core.h"
+#include "sh_pfc.h"
+
+#define CPU_ALL_PORT(fn, pfx, sfx)					\
+	/*  Port0 - Port30 */						\
+	PORT_10(fn, pfx, sfx),						\
+	PORT_10(fn, pfx##1, sfx),					\
+	PORT_10(fn, pfx##2, sfx),					\
+	PORT_1(fn,  pfx##30, sfx),					\
+	/* Port32 - Port40 */						\
+	PORT_1(fn,  pfx##32, sfx),	PORT_1(fn,  pfx##33, sfx),	\
+	PORT_1(fn,  pfx##34, sfx),	PORT_1(fn,  pfx##35, sfx),	\
+	PORT_1(fn,  pfx##36, sfx),	PORT_1(fn,  pfx##37, sfx),	\
+	PORT_1(fn,  pfx##38, sfx),	PORT_1(fn,  pfx##39, sfx),	\
+	PORT_1(fn,  pfx##40, sfx),					\
+	/* Port64  - Port85 */						\
+	PORT_1(fn, pfx##64, sfx),	PORT_1(fn, pfx##65, sfx),	\
+	PORT_1(fn, pfx##66, sfx),	PORT_1(fn, pfx##67, sfx),	\
+	PORT_1(fn, pfx##68, sfx),	PORT_1(fn, pfx##69, sfx),	\
+	PORT_10(fn, pfx##7, sfx),					\
+	PORT_1(fn, pfx##80, sfx),	PORT_1(fn, pfx##81, sfx),	\
+	PORT_1(fn, pfx##82, sfx),	PORT_1(fn, pfx##83, sfx),	\
+	PORT_1(fn, pfx##84, sfx),	PORT_1(fn, pfx##85, sfx),	\
+	/* Port96  - Port126 */						\
+	PORT_1(fn, pfx##96, sfx),	PORT_1(fn, pfx##97, sfx),	\
+	PORT_1(fn, pfx##98, sfx),	PORT_1(fn, pfx##99, sfx),	\
+	PORT_10(fn, pfx##10, sfx),					\
+	PORT_10(fn, pfx##11, sfx),					\
+	PORT_1(fn, pfx##120, sfx),	PORT_1(fn, pfx##121, sfx),	\
+	PORT_1(fn, pfx##122, sfx),	PORT_1(fn, pfx##123, sfx),	\
+	PORT_1(fn, pfx##124, sfx),	PORT_1(fn, pfx##125, sfx),	\
+	PORT_1(fn, pfx##126, sfx),					\
+	/* Port128 - Port134 */						\
+	PORT_1(fn, pfx##128, sfx),	PORT_1(fn, pfx##129, sfx),	\
+	PORT_1(fn, pfx##130, sfx),	PORT_1(fn, pfx##131, sfx),	\
+	PORT_1(fn, pfx##132, sfx),	PORT_1(fn, pfx##133, sfx),	\
+	PORT_1(fn, pfx##134, sfx),					\
+	/* Port160 - Port178 */						\
+	PORT_10(fn, pfx##16, sfx),					\
+	PORT_1(fn, pfx##170, sfx),	PORT_1(fn, pfx##171, sfx),	\
+	PORT_1(fn, pfx##172, sfx),	PORT_1(fn, pfx##173, sfx),	\
+	PORT_1(fn, pfx##174, sfx),	PORT_1(fn, pfx##175, sfx),	\
+	PORT_1(fn, pfx##176, sfx),	PORT_1(fn, pfx##177, sfx),	\
+	PORT_1(fn, pfx##178, sfx),					\
+	/* Port192 - Port222 */						\
+	PORT_1(fn, pfx##192, sfx),	PORT_1(fn, pfx##193, sfx),	\
+	PORT_1(fn, pfx##194, sfx),	PORT_1(fn, pfx##195, sfx),	\
+	PORT_1(fn, pfx##196, sfx),	PORT_1(fn, pfx##197, sfx),	\
+	PORT_1(fn, pfx##198, sfx),	PORT_1(fn, pfx##199, sfx),	\
+	PORT_10(fn, pfx##20, sfx),					\
+	PORT_10(fn, pfx##21, sfx),					\
+	PORT_1(fn, pfx##220, sfx),	PORT_1(fn, pfx##221, sfx),	\
+	PORT_1(fn, pfx##222, sfx),					\
+	/* Port224 - Port250 */						\
+	PORT_1(fn, pfx##224, sfx),	PORT_1(fn, pfx##225, sfx),	\
+	PORT_1(fn, pfx##226, sfx),	PORT_1(fn, pfx##227, sfx),	\
+	PORT_1(fn, pfx##228, sfx),	PORT_1(fn, pfx##229, sfx),	\
+	PORT_10(fn, pfx##23, sfx),					\
+	PORT_10(fn, pfx##24, sfx),					\
+	PORT_1(fn, pfx##250, sfx),					\
+	/* Port256 - Port283 */						\
+	PORT_1(fn, pfx##256, sfx),	PORT_1(fn, pfx##257, sfx),	\
+	PORT_1(fn, pfx##258, sfx),	PORT_1(fn, pfx##259, sfx),	\
+	PORT_10(fn, pfx##26, sfx),					\
+	PORT_10(fn, pfx##27, sfx),					\
+	PORT_1(fn, pfx##280, sfx),	PORT_1(fn, pfx##281, sfx),	\
+	PORT_1(fn, pfx##282, sfx),	PORT_1(fn, pfx##283, sfx),	\
+	/* Port288 - Port308 */						\
+	PORT_1(fn, pfx##288, sfx),	PORT_1(fn, pfx##289, sfx),	\
+	PORT_10(fn, pfx##29, sfx),					\
+	PORT_1(fn, pfx##300, sfx),	PORT_1(fn, pfx##301, sfx),	\
+	PORT_1(fn, pfx##302, sfx),	PORT_1(fn, pfx##303, sfx),	\
+	PORT_1(fn, pfx##304, sfx),	PORT_1(fn, pfx##305, sfx),	\
+	PORT_1(fn, pfx##306, sfx),	PORT_1(fn, pfx##307, sfx),	\
+	PORT_1(fn, pfx##308, sfx),					\
+	/* Port320 - Port329 */						\
+	PORT_10(fn, pfx##32, sfx)
+
+
+enum {
+	PINMUX_RESERVED = 0,
+
+	/* PORT0_DATA -> PORT329_DATA */
+	PINMUX_DATA_BEGIN,
+	PORT_ALL(DATA),
+	PINMUX_DATA_END,
+
+	/* PORT0_IN -> PORT329_IN */
+	PINMUX_INPUT_BEGIN,
+	PORT_ALL(IN),
+	PINMUX_INPUT_END,
+
+	/* PORT0_OUT -> PORT329_OUT */
+	PINMUX_OUTPUT_BEGIN,
+	PORT_ALL(OUT),
+	PINMUX_OUTPUT_END,
+
+	PINMUX_FUNCTION_BEGIN,
+	PORT_ALL(FN_IN), /* PORT0_FN_IN -> PORT329_FN_IN */
+	PORT_ALL(FN_OUT), /* PORT0_FN_OUT -> PORT329_FN_OUT */
+	PORT_ALL(FN0), /* PORT0_FN0 -> PORT329_FN0 */
+	PORT_ALL(FN1), /* PORT0_FN1 -> PORT329_FN1 */
+	PORT_ALL(FN2), /* PORT0_FN2 -> PORT329_FN2 */
+	PORT_ALL(FN3), /* PORT0_FN3 -> PORT329_FN3 */
+	PORT_ALL(FN4), /* PORT0_FN4 -> PORT329_FN4 */
+	PORT_ALL(FN5), /* PORT0_FN5 -> PORT329_FN5 */
+	PORT_ALL(FN6), /* PORT0_FN6 -> PORT329_FN6 */
+	PORT_ALL(FN7), /* PORT0_FN7 -> PORT329_FN7 */
+
+	MSEL1CR_31_0, MSEL1CR_31_1,
+	MSEL1CR_27_0, MSEL1CR_27_1,
+	MSEL1CR_25_0, MSEL1CR_25_1,
+	MSEL1CR_24_0, MSEL1CR_24_1,
+	MSEL1CR_22_0, MSEL1CR_22_1,
+	MSEL1CR_21_0, MSEL1CR_21_1,
+	MSEL1CR_20_0, MSEL1CR_20_1,
+	MSEL1CR_19_0, MSEL1CR_19_1,
+	MSEL1CR_18_0, MSEL1CR_18_1,
+	MSEL1CR_17_0, MSEL1CR_17_1,
+	MSEL1CR_16_0, MSEL1CR_16_1,
+	MSEL1CR_15_0, MSEL1CR_15_1,
+	MSEL1CR_14_0, MSEL1CR_14_1,
+	MSEL1CR_13_0, MSEL1CR_13_1,
+	MSEL1CR_12_0, MSEL1CR_12_1,
+	MSEL1CR_11_0, MSEL1CR_11_1,
+	MSEL1CR_10_0, MSEL1CR_10_1,
+	MSEL1CR_09_0, MSEL1CR_09_1,
+	MSEL1CR_08_0, MSEL1CR_08_1,
+	MSEL1CR_07_0, MSEL1CR_07_1,
+	MSEL1CR_06_0, MSEL1CR_06_1,
+	MSEL1CR_05_0, MSEL1CR_05_1,
+	MSEL1CR_04_0, MSEL1CR_04_1,
+	MSEL1CR_03_0, MSEL1CR_03_1,
+	MSEL1CR_02_0, MSEL1CR_02_1,
+	MSEL1CR_01_0, MSEL1CR_01_1,
+	MSEL1CR_00_0, MSEL1CR_00_1,
+
+	MSEL3CR_31_0, MSEL3CR_31_1,
+	MSEL3CR_28_0, MSEL3CR_28_1,
+	MSEL3CR_27_0, MSEL3CR_27_1,
+	MSEL3CR_26_0, MSEL3CR_26_1,
+	MSEL3CR_23_0, MSEL3CR_23_1,
+	MSEL3CR_22_0, MSEL3CR_22_1,
+	MSEL3CR_21_0, MSEL3CR_21_1,
+	MSEL3CR_20_0, MSEL3CR_20_1,
+	MSEL3CR_19_0, MSEL3CR_19_1,
+	MSEL3CR_18_0, MSEL3CR_18_1,
+	MSEL3CR_17_0, MSEL3CR_17_1,
+	MSEL3CR_16_0, MSEL3CR_16_1,
+	MSEL3CR_15_0, MSEL3CR_15_1,
+	MSEL3CR_12_0, MSEL3CR_12_1,
+	MSEL3CR_11_0, MSEL3CR_11_1,
+	MSEL3CR_10_0, MSEL3CR_10_1,
+	MSEL3CR_09_0, MSEL3CR_09_1,
+	MSEL3CR_06_0, MSEL3CR_06_1,
+	MSEL3CR_03_0, MSEL3CR_03_1,
+	MSEL3CR_01_0, MSEL3CR_01_1,
+	MSEL3CR_00_0, MSEL3CR_00_1,
+
+	MSEL4CR_30_0, MSEL4CR_30_1,
+	MSEL4CR_29_0, MSEL4CR_29_1,
+	MSEL4CR_28_0, MSEL4CR_28_1,
+	MSEL4CR_27_0, MSEL4CR_27_1,
+	MSEL4CR_26_0, MSEL4CR_26_1,
+	MSEL4CR_25_0, MSEL4CR_25_1,
+	MSEL4CR_24_0, MSEL4CR_24_1,
+	MSEL4CR_23_0, MSEL4CR_23_1,
+	MSEL4CR_22_0, MSEL4CR_22_1,
+	MSEL4CR_21_0, MSEL4CR_21_1,
+	MSEL4CR_20_0, MSEL4CR_20_1,
+	MSEL4CR_19_0, MSEL4CR_19_1,
+	MSEL4CR_18_0, MSEL4CR_18_1,
+	MSEL4CR_17_0, MSEL4CR_17_1,
+	MSEL4CR_16_0, MSEL4CR_16_1,
+	MSEL4CR_15_0, MSEL4CR_15_1,
+	MSEL4CR_14_0, MSEL4CR_14_1,
+	MSEL4CR_13_0, MSEL4CR_13_1,
+	MSEL4CR_12_0, MSEL4CR_12_1,
+	MSEL4CR_11_0, MSEL4CR_11_1,
+	MSEL4CR_10_0, MSEL4CR_10_1,
+	MSEL4CR_09_0, MSEL4CR_09_1,
+	MSEL4CR_07_0, MSEL4CR_07_1,
+	MSEL4CR_04_0, MSEL4CR_04_1,
+	MSEL4CR_01_0, MSEL4CR_01_1,
+
+	MSEL5CR_31_0, MSEL5CR_31_1,
+	MSEL5CR_30_0, MSEL5CR_30_1,
+	MSEL5CR_29_0, MSEL5CR_29_1,
+	MSEL5CR_28_0, MSEL5CR_28_1,
+	MSEL5CR_27_0, MSEL5CR_27_1,
+	MSEL5CR_26_0, MSEL5CR_26_1,
+	MSEL5CR_25_0, MSEL5CR_25_1,
+	MSEL5CR_24_0, MSEL5CR_24_1,
+	MSEL5CR_23_0, MSEL5CR_23_1,
+	MSEL5CR_22_0, MSEL5CR_22_1,
+	MSEL5CR_21_0, MSEL5CR_21_1,
+	MSEL5CR_20_0, MSEL5CR_20_1,
+	MSEL5CR_19_0, MSEL5CR_19_1,
+	MSEL5CR_18_0, MSEL5CR_18_1,
+	MSEL5CR_17_0, MSEL5CR_17_1,
+	MSEL5CR_16_0, MSEL5CR_16_1,
+	MSEL5CR_15_0, MSEL5CR_15_1,
+	MSEL5CR_14_0, MSEL5CR_14_1,
+	MSEL5CR_13_0, MSEL5CR_13_1,
+	MSEL5CR_12_0, MSEL5CR_12_1,
+	MSEL5CR_11_0, MSEL5CR_11_1,
+	MSEL5CR_10_0, MSEL5CR_10_1,
+	MSEL5CR_09_0, MSEL5CR_09_1,
+	MSEL5CR_08_0, MSEL5CR_08_1,
+	MSEL5CR_07_0, MSEL5CR_07_1,
+	MSEL5CR_06_0, MSEL5CR_06_1,
+
+	MSEL8CR_16_0, MSEL8CR_16_1,
+	MSEL8CR_01_0, MSEL8CR_01_1,
+	MSEL8CR_00_0, MSEL8CR_00_1,
+
+	PINMUX_FUNCTION_END,
+
+	PINMUX_MARK_BEGIN,
+
+
+#define F1(a)	a##_MARK
+#define F2(a)	a##_MARK
+#define F3(a)	a##_MARK
+#define F4(a)	a##_MARK
+#define F5(a)	a##_MARK
+#define F6(a)	a##_MARK
+#define F7(a)	a##_MARK
+#define IRQ(a)	IRQ##a##_MARK
+
+	F1(LCDD0), F3(PDM2_CLK_0), F7(DU0_DR0), IRQ(0), /* Port0 */
+	F1(LCDD1), F3(PDM2_DATA_1), F7(DU0_DR19), IRQ(1),
+	F1(LCDD2), F3(PDM3_CLK_2), F7(DU0_DR2), IRQ(2),
+	F1(LCDD3), F3(PDM3_DATA_3), F7(DU0_DR3), IRQ(3),
+	F1(LCDD4), F3(PDM4_CLK_4), F7(DU0_DR4), IRQ(4),
+	F1(LCDD5), F3(PDM4_DATA_5), F7(DU0_DR5), IRQ(5),
+	F1(LCDD6), F3(PDM0_OUTCLK_6), F7(DU0_DR6), IRQ(6),
+	F1(LCDD7), F3(PDM0_OUTDATA_7), F7(DU0_DR7), IRQ(7),
+	F1(LCDD8), F3(PDM1_OUTCLK_8), F7(DU0_DG0), IRQ(8),
+	F1(LCDD9), F3(PDM1_OUTDATA_9), F7(DU0_DG1), IRQ(9),
+	F1(LCDD10), F3(FSICCK), F7(DU0_DG2), IRQ(10), /* Port10 */
+	F1(LCDD11), F3(FSICISLD), F7(DU0_DG3), IRQ(11),
+	F1(LCDD12), F3(FSICOMC), F7(DU0_DG4), IRQ(12),
+	F1(LCDD13), F3(FSICOLR), F4(FSICILR), F7(DU0_DG5), IRQ(13),
+	F1(LCDD14), F3(FSICOBT), F4(FSICIBT), F7(DU0_DG6), IRQ(14),
+	F1(LCDD15), F3(FSICOSLD), F7(DU0_DG7), IRQ(15),
+	F1(LCDD16), F4(TPU1TO1), F7(DU0_DB0),
+	F1(LCDD17), F4(SF_IRQ_00), F7(DU0_DB1),
+	F1(LCDD18), F4(SF_IRQ_01), F7(DU0_DB2),
+	F1(LCDD19), F3(SCIFB3_RTS_19), F7(DU0_DB3),
+	F1(LCDD20), F3(SCIFB3_CTS_20), F7(DU0_DB4), /* Port20 */
+	F1(LCDD21), F3(SCIFB3_TXD_21), F7(DU0_DB5),
+	F1(LCDD22), F3(SCIFB3_RXD_22), F7(DU0_DB6),
+	F1(LCDD23), F3(SCIFB3_SCK_23), F7(DU0_DB7),
+	F1(LCDHSYN), F2(LCDCS), F3(SCIFB1_RTS_24),
+	F7(DU0_EXHSYNC_N_CSYNC_N_HSYNC_N),
+	F1(LCDVSYN), F3(SCIFB1_CTS_25), F7(DU0_EXVSYNC_N_VSYNC_N_CSYNC_N),
+	F1(LCDDCK), F2(LCDWR), F3(SCIFB1_TXD_26), F7(DU0_DOTCLKIN),
+	F1(LCDDISP), F2(LCDRS), F3(SCIFB1_RXD_27), F7(DU0_DOTCLKOUT),
+	F1(LCDRD_N), F3(SCIFB1_SCK_28), F7(DU0_DOTCLKOUTB),
+	F1(LCDLCLK), F4(SF_IRQ_02), F7(DU0_DISP_CSYNC_N_DE),
+	F1(LCDDON), F4(SF_IRQ_03), F7(DU0_ODDF_N_CLAMP), /* Port30 */
+
+	F1(SCIFA0_RTS), F5(SIM0_DET), F7(CSCIF0_RTS), /* Port32 */
+	F1(SCIFA0_CTS), F5(SIM1_DET), F7(CSCIF0_CTS),
+	F1(SCIFA0_SCK), F5(SIM0_PWRON), F7(CSCIF0_SCK),
+	F1(SCIFA1_RTS), F7(CSCIF1_RTS),
+	F1(SCIFA1_CTS), F7(CSCIF1_CTS),
+	F1(SCIFA1_SCK), F7(CSCIF1_SCK),
+	F1(SCIFB0_RTS), F3(TPU0TO1), F4(SCIFB3_RTS_38), F7(CHSCIF0_HRTS),
+	F1(SCIFB0_CTS), F3(TPU0TO2), F4(SCIFB3_CTS_39), F7(CHSCIF0_HCTS),
+	F1(SCIFB0_SCK), F3(TPU0TO3), F4(SCIFB3_SCK_40),
+	F7(CHSCIF0_HSCK), /* Port40 */
+
+	F1(PDM0_DATA), /* Port64 */
+	F1(PDM1_DATA),
+	F1(HSI_RX_WAKE), F2(SCIFB2_CTS_66), F3(MSIOF3_SYNC), F5(GenIO4),
+	IRQ(40),
+	F1(HSI_RX_READY), F2(SCIFB1_TXD_67), F5(GIO_OUT3_67), F7(CHSCIF1_HTX),
+	F1(HSI_RX_FLAG), F2(SCIFB2_TXD_68), F3(MSIOF3_TXD), F5(GIO_OUT4_68),
+	F1(HSI_RX_DATA), F2(SCIFB2_RXD_69), F3(MSIOF3_RXD), F5(GIO_OUT5_69),
+	F1(HSI_TX_FLAG), F2(SCIFB1_RTS_70), F5(GIO_OUT1_70), F6(HSIC_TSTCLK0),
+	F7(CHSCIF1_HRTS), /* Port70 */
+	F1(HSI_TX_DATA), F2(SCIFB1_CTS_71), F5(GIO_OUT2_71), F6(HSIC_TSTCLK1),
+	F7(CHSCIF1_HCTS),
+	F1(HSI_TX_WAKE), F2(SCIFB1_RXD_72), F5(GenIO8), F7(CHSCIF1_HRX),
+	F1(HSI_TX_READY), F2(SCIFB2_RTS_73), F3(MSIOF3_SCK), F5(GIO_OUT0_73),
+	F1(IRDA_OUT), F1(IRDA_IN), F1(IRDA_FIRSEL), F1(TPU0TO0),
+	F1(DIGRFEN), F1(GPS_TIMESTAMP), F1(TXP), /* Port80 */
+	F1(TXP2), F1(COEX_0), F1(COEX_1), IRQ(19), IRQ(18), /* Port85 */
+
+	F1(KEYIN0), /* Port96 */
+	F1(KEYIN1), F1(KEYIN2), F1(KEYIN3), F1(KEYIN4), /* Port100 */
+	F1(KEYIN5), F1(KEYIN6), IRQ(41), F1(KEYIN7), IRQ(42),
+	F2(KEYOUT0), F2(KEYOUT1), F2(KEYOUT2), F2(KEYOUT3),
+	F2(KEYOUT4), F2(KEYOUT5), IRQ(43), F2(KEYOUT6), IRQ(44), /* Port110 */
+	F2(KEYOUT7), F5(RFANAEN), IRQ(45),
+	F1(KEYIN8), F2(KEYOUT8), F4(SF_IRQ_04), IRQ(46),
+	F1(KEYIN9), F2(KEYOUT9), F4(SF_IRQ_05), IRQ(47),
+	F1(KEYIN10), F2(KEYOUT10), F4(SF_IRQ_06), IRQ(48),
+	F1(KEYIN11), F2(KEYOUT11), F4(SF_IRQ_07), IRQ(49),
+	F1(SCIFA0_TXD), F7(CSCIF0_TX), F1(SCIFA0_RXD), F7(CSCIF0_RX),
+	F1(SCIFA1_TXD), F7(CSCIF1_TX), F1(SCIFA1_RXD), F7(CSCIF1_RX),
+	F3(SF_PORT_1_120), F4(SCIFB3_RXD_120), F7(DU0_CDE), /* Port120 */
+	F3(SF_PORT_0_121), F4(SCIFB3_TXD_121),
+	F1(SCIFB0_TXD), F7(CHSCIF0_HTX),
+	F1(SCIFB0_RXD), F7(CHSCIF0_HRX), F3(ISP_STROBE_124),
+	F1(STP_ISD_0), F2(PDM4_CLK_125), F3(MSIOF2_TXD), F5(SIM0_VOLTSEL0),
+	F1(TS_SDEN), F2(MSIOF7_SYNC), F3(STP_ISEN_1),
+	F1(STP_ISEN_0), F2(PDM1_OUTDATA_128), F3(MSIOF2_SYNC),
+	F5(SIM1_VOLTSEL1), F1(TS_SPSYNC), F2(MSIOF7_RXD), F3(STP_ISSYNC_1),
+	F1(STP_ISSYNC_0), F2(PDM4_DATA_130), F3(MSIOF2_RXD),
+	F5(SIM0_VOLTSEL1), /* Port130 */
+	F1(STP_OPWM_0), F5(SIM1_PWRON), F1(TS_SCK), F2(MSIOF7_SCK),
+	F3(STP_ISCLK_1), F1(STP_ISCLK_0), F2(PDM1_OUTCLK_133), F3(MSIOF2_SCK),
+	F5(SIM1_VOLTSEL0), F1(TS_SDAT), F2(MSIOF7_TXD), F3(STP_ISD_1),
+	IRQ(20), /* Port160 */
+	IRQ(21), IRQ(22), IRQ(23),
+	F1(MMCD0_0), F1(MMCD0_1), F1(MMCD0_2), F1(MMCD0_3),
+	F1(MMCD0_4), F1(MMCD0_5), F1(MMCD0_6), /* Port170 */
+	F1(MMCD0_7), F1(MMCCMD0), F1(MMCCLK0), F1(MMCRST),
+	IRQ(24), IRQ(25), IRQ(26), IRQ(27),
+	F1(A10), F2(MMCD1_7), IRQ(31), /* Port192 */
+	F1(A9), F2(MMCD1_6), IRQ(32),
+	F1(A8), F2(MMCD1_5), IRQ(33),
+	F1(A7), F2(MMCD1_4), IRQ(34),
+	F1(A6), F2(MMCD1_3), IRQ(35),
+	F1(A5), F2(MMCD1_2), IRQ(36),
+	F1(A4), F2(MMCD1_1), IRQ(37),
+	F1(A3), F2(MMCD1_0), IRQ(38),
+	F1(A2), F2(MMCCMD1), IRQ(39), /* Port200 */
+	F1(A1),
+	F1(A0), F2(BS),
+	F1(CKO), F2(MMCCLK1),
+	F1(CS0_N), F5(SIM0_GPO1),
+	F1(CS2_N), F5(SIM0_GPO2),
+	F1(CS4_N), F2(VIO_VD), F5(SIM1_GPO0),
+	F1(D15), F5(GIO_OUT15),
+	F1(D14), F5(GIO_OUT14),
+	F1(D13), F5(GIO_OUT13),
+	F1(D12), F5(GIO_OUT12), /* Port210 */
+	F1(D11), F5(WGM_TXP2),
+	F1(D10), F5(WGM_GPS_TIMEM_ASK_RFCLK),
+	F1(D9), F2(VIO_D9), F5(GIO_OUT9),
+	F1(D8), F2(VIO_D8), F5(GIO_OUT8),
+	F1(D7), F2(VIO_D7), F5(GIO_OUT7),
+	F1(D6), F2(VIO_D6), F5(GIO_OUT6),
+	F1(D5), F2(VIO_D5), F5(GIO_OUT5_217),
+	F1(D4), F2(VIO_D4), F5(GIO_OUT4_218),
+	F1(D3), F2(VIO_D3), F5(GIO_OUT3_219),
+	F1(D2), F2(VIO_D2), F5(GIO_OUT2_220), /* Port220 */
+	F1(D1), F2(VIO_D1), F5(GIO_OUT1_221),
+	F1(D0), F2(VIO_D0), F5(GIO_OUT0_222),
+	F1(RDWR_224), F2(VIO_HD), F5(SIM1_GPO2),
+	F1(RD_N), F1(WAIT_N), F2(VIO_CLK), F5(SIM1_GPO1),
+	F1(WE0_N), F2(RDWR_227),
+	F1(WE1_N), F5(SIM0_GPO0),
+	F1(PWMO), F2(VIO_CKO1_229),
+	F1(SLIM_CLK), F2(VIO_CKO4_230), /* Port230 */
+	F1(SLIM_DATA), F2(VIO_CKO5_231), F2(VIO_CKO2_232), F4(SF_PORT_0_232),
+	F2(VIO_CKO3_233), F4(SF_PORT_1_233),
+	F1(FSIACK), F2(PDM3_CLK_234), F3(ISP_IRIS1_234),
+	F1(FSIAISLD), F2(PDM3_DATA_235),
+	F1(FSIAOMC), F2(PDM0_OUTCLK_236), F3(ISP_IRIS0_236),
+	F1(FSIAOLR), F2(FSIAILR), F1(FSIAOBT), F2(FSIAIBT),
+	F1(FSIAOSLD), F2(PDM0_OUTDATA_239),
+	F1(FSIBISLD), /* Port240 */
+	F1(FSIBOLR), F2(FSIBILR), F1(FSIBOMC), F3(ISP_SHUTTER1_242),
+	F1(FSIBOBT), F2(FSIBIBT), F1(FSIBOSLD), F2(FSIASPDIF),
+	F1(FSIBCK), F3(ISP_SHUTTER0_245),
+	F1(ISP_IRIS1_246), F1(ISP_IRIS0_247), F1(ISP_SHUTTER1_248),
+	F1(ISP_SHUTTER0_249), F1(ISP_STROBE_250), /* Port250 */
+	F1(MSIOF0_SYNC), F1(MSIOF0_RXD), F1(MSIOF0_SCK), F1(MSIOF0_SS2),
+	F3(VIO_CKO3_259), F1(MSIOF0_TXD), /* Port260 */
+	F2(SCIFB1_SCK_261), F7(CHSCIF1_HSCK), F2(SCIFB2_SCK_262),
+	F1(MSIOF1_SS2), F4(MSIOF5_SS2), F1(MSIOF1_TXD), F4(MSIOF5_TXD),
+	F1(MSIOF1_RXD), F4(MSIOF5_RXD), F1(MSIOF1_SS1), F4(MSIOF5_SS1),
+	F1(MSIOF0_SS1), F1(MSIOF1_SCK), F4(MSIOF5_SCK),
+	F1(MSIOF1_SYNC), F4(MSIOF5_SYNC),
+	F1(MSIOF2_SS1), F3(VIO_CKO5_270), /* Port270 */
+	F1(MSIOF2_SS2), F3(VIO_CKO2_271), F1(MSIOF3_SS2), F3(VIO_CKO1_272),
+	F1(MSIOF3_SS1), F3(VIO_CKO4_273), F1(MSIOF4_SS2), F4(TPU1TO0),
+	F1(IC_DP), F1(SIM0_RST), F1(IC_DM), F1(SIM0_BSICOMP),
+	F1(SIM0_CLK), F1(SIM0_IO), /* Port280 */
+	F1(SIM1_IO), F2(PDM2_DATA_281), F1(SIM1_CLK), F2(PDM2_CLK_282),
+	F1(SIM1_RST), F1(SDHID1_0), F3(STMDATA0_2),
+	F1(SDHID1_1), F3(STMDATA1_2), IRQ(51), /* Port290 */
+	F1(SDHID1_2), F3(STMDATA2_2), F1(SDHID1_3), F3(STMDATA3_2),
+	F1(SDHICLK1), F3(STMCLK_2), F1(SDHICMD1), F3(STMSIDI_2),
+	F1(SDHID2_0), F2(MSIOF4_TXD), F3(SCIFB2_TXD_295), F4(MSIOF6_TXD),
+	F1(SDHID2_1), F4(MSIOF6_SS2), IRQ(52),
+	F1(SDHID2_2), F2(MSIOF4_RXD), F3(SCIFB2_RXD_297), F4(MSIOF6_RXD),
+	F1(SDHID2_3), F2(MSIOF4_SYNC), F3(SCIFB2_CTS_298), F4(MSIOF6_SYNC),
+	F1(SDHICLK2), F2(MSIOF4_SCK), F3(SCIFB2_SCK_299), F4(MSIOF6_SCK),
+	F1(SDHICMD2), F2(MSIOF4_SS1), F3(SCIFB2_RTS_300),
+	F4(MSIOF6_SS1), /* Port300 */
+	F1(SDHICD0), IRQ(50), F1(SDHID0_0), F3(STMDATA0_1),
+	F1(SDHID0_1), F3(STMDATA1_1), F1(SDHID0_2), F3(STMDATA2_1),
+	F1(SDHID0_3), F3(STMDATA3_1), F1(SDHICMD0), F3(STMSIDI_1),
+	F1(SDHIWP0), F1(SDHICLK0), F3(STMCLK_1), IRQ(16), /* Port320 */
+	IRQ(17), IRQ(28), IRQ(29), IRQ(30), IRQ(53), IRQ(54),
+	IRQ(55), IRQ(56), IRQ(57),
+	PINMUX_MARK_END,
+};
+
+#define _PORT_DATA(pfx, sfx)	PORT_DATA_IO(pfx)
+#define PINMUX_DATA_ALL()    CPU_ALL_PORT(_PORT_DATA, , unused)
+
+static const pinmux_enum_t pinmux_data[] = {
+	/* specify valid pin states for each pin in GPIO mode */
+	PINMUX_DATA_ALL(),
+
+	/* Port0 */
+	PINMUX_DATA(LCDD0_MARK,		PORT0_FN1),
+	PINMUX_DATA(PDM2_CLK_0_MARK,	PORT0_FN3),
+	PINMUX_DATA(DU0_DR0_MARK,	PORT0_FN7),
+	PINMUX_DATA(IRQ0_MARK,		PORT0_FN0),
+
+	/* Port1 */
+	PINMUX_DATA(LCDD1_MARK,		PORT1_FN1),
+	PINMUX_DATA(PDM2_DATA_1_MARK,	PORT1_FN3,	MSEL3CR_12_0),
+	PINMUX_DATA(DU0_DR19_MARK,	PORT1_FN7),
+	PINMUX_DATA(IRQ1_MARK,		PORT1_FN0),
+
+	/* Port2 */
+	PINMUX_DATA(LCDD2_MARK,		PORT2_FN1),
+	PINMUX_DATA(PDM3_CLK_2_MARK,	PORT2_FN3),
+	PINMUX_DATA(DU0_DR2_MARK,	PORT2_FN7),
+	PINMUX_DATA(IRQ2_MARK,		PORT2_FN0),
+
+	/* Port3 */
+	PINMUX_DATA(LCDD3_MARK,		PORT3_FN1),
+	PINMUX_DATA(PDM3_DATA_3_MARK,	PORT3_FN3,	MSEL3CR_12_0),
+	PINMUX_DATA(DU0_DR3_MARK,	PORT3_FN7),
+	PINMUX_DATA(IRQ3_MARK,		PORT3_FN0),
+
+	/* Port4 */
+	PINMUX_DATA(LCDD4_MARK,		PORT4_FN1),
+	PINMUX_DATA(PDM4_CLK_4_MARK,	PORT4_FN3),
+	PINMUX_DATA(DU0_DR4_MARK,	PORT4_FN7),
+	PINMUX_DATA(IRQ4_MARK,		PORT4_FN0),
+
+	/* Port5 */
+	PINMUX_DATA(LCDD5_MARK,		PORT5_FN1),
+	PINMUX_DATA(PDM4_DATA_5_MARK,	PORT5_FN3,	MSEL3CR_12_0),
+	PINMUX_DATA(DU0_DR5_MARK,	PORT5_FN7),
+	PINMUX_DATA(IRQ5_MARK,		PORT5_FN0),
+
+	/* Port6 */
+	PINMUX_DATA(LCDD6_MARK,		PORT6_FN1),
+	PINMUX_DATA(PDM0_OUTCLK_6_MARK,	PORT6_FN3),
+	PINMUX_DATA(DU0_DR6_MARK,	PORT6_FN7),
+	PINMUX_DATA(IRQ6_MARK,		PORT6_FN0),
+
+	/* Port7 */
+	PINMUX_DATA(LCDD7_MARK,			PORT7_FN1),
+	PINMUX_DATA(PDM0_OUTDATA_7_MARK,	PORT7_FN3),
+	PINMUX_DATA(DU0_DR7_MARK,		PORT7_FN7),
+	PINMUX_DATA(IRQ7_MARK,			PORT7_FN0),
+
+	/* Port8 */
+	PINMUX_DATA(LCDD8_MARK,		PORT8_FN1),
+	PINMUX_DATA(PDM1_OUTCLK_8_MARK,	PORT8_FN3),
+	PINMUX_DATA(DU0_DG0_MARK,	PORT8_FN7),
+	PINMUX_DATA(IRQ8_MARK,		PORT8_FN0),
+
+	/* Port9 */
+	PINMUX_DATA(LCDD9_MARK,		PORT9_FN1),
+	PINMUX_DATA(PDM1_OUTDATA_9_MARK, PORT9_FN3),
+	PINMUX_DATA(DU0_DG1_MARK,	PORT9_FN7),
+	PINMUX_DATA(IRQ9_MARK,		PORT9_FN0),
+
+	/* Port10 */
+	PINMUX_DATA(LCDD10_MARK,		PORT10_FN1),
+	PINMUX_DATA(FSICCK_MARK,		PORT10_FN3),
+	PINMUX_DATA(DU0_DG2_MARK,		PORT10_FN7),
+	PINMUX_DATA(IRQ10_MARK,			PORT10_FN0),
+
+	/* Port11 */
+	PINMUX_DATA(LCDD11_MARK,		PORT11_FN1),
+	PINMUX_DATA(FSICISLD_MARK,		PORT11_FN3),
+	PINMUX_DATA(DU0_DG3_MARK,		PORT11_FN7),
+	PINMUX_DATA(IRQ11_MARK,			PORT11_FN0),
+
+	/* Port12 */
+	PINMUX_DATA(LCDD12_MARK,		PORT12_FN1),
+	PINMUX_DATA(FSICOMC_MARK,		PORT12_FN3),
+	PINMUX_DATA(DU0_DG4_MARK,		PORT12_FN7),
+	PINMUX_DATA(IRQ12_MARK,			PORT12_FN0),
+
+	/* Port13 */
+	PINMUX_DATA(LCDD13_MARK,		PORT13_FN1),
+	PINMUX_DATA(FSICOLR_MARK,		PORT13_FN3),
+	PINMUX_DATA(FSICILR_MARK,		PORT13_FN4),
+	PINMUX_DATA(DU0_DG5_MARK,		PORT13_FN7),
+	PINMUX_DATA(IRQ13_MARK,			PORT13_FN0),
+
+	/* Port14 */
+	PINMUX_DATA(LCDD14_MARK,		PORT14_FN1),
+	PINMUX_DATA(FSICOBT_MARK,		PORT14_FN3),
+	PINMUX_DATA(FSICIBT_MARK,		PORT14_FN4),
+	PINMUX_DATA(DU0_DG6_MARK,		PORT14_FN7),
+	PINMUX_DATA(IRQ14_MARK,			PORT14_FN0),
+
+	/* Port15 */
+	PINMUX_DATA(LCDD15_MARK,		PORT15_FN1),
+	PINMUX_DATA(FSICOSLD_MARK,		PORT15_FN3),
+	PINMUX_DATA(DU0_DG7_MARK,		PORT15_FN7),
+	PINMUX_DATA(IRQ15_MARK,			PORT15_FN0),
+
+	/* Port16 */
+	PINMUX_DATA(LCDD16_MARK,		PORT16_FN1),
+	PINMUX_DATA(TPU1TO1_MARK,		PORT16_FN4),
+	PINMUX_DATA(DU0_DB0_MARK,		PORT16_FN7),
+
+	/* Port17 */
+	PINMUX_DATA(LCDD17_MARK,		PORT17_FN1),
+	PINMUX_DATA(SF_IRQ_00_MARK,		PORT17_FN4),
+	PINMUX_DATA(DU0_DB1_MARK,		PORT17_FN7),
+
+	/* Port18 */
+	PINMUX_DATA(LCDD18_MARK,		PORT18_FN1),
+	PINMUX_DATA(SF_IRQ_01_MARK,		PORT18_FN4),
+	PINMUX_DATA(DU0_DB2_MARK,		PORT18_FN7),
+
+	/* Port19 */
+	PINMUX_DATA(LCDD19_MARK,		PORT19_FN1),
+	PINMUX_DATA(SCIFB3_RTS_19_MARK,		PORT19_FN3),
+	PINMUX_DATA(DU0_DB3_MARK,		PORT19_FN7),
+
+	/* Port20 */
+	PINMUX_DATA(LCDD20_MARK,		PORT20_FN1),
+	PINMUX_DATA(SCIFB3_CTS_20_MARK,		PORT20_FN3,	MSEL3CR_09_0),
+	PINMUX_DATA(DU0_DB4_MARK,		PORT20_FN7),
+
+	/* Port21 */
+	PINMUX_DATA(LCDD21_MARK,		PORT21_FN1),
+	PINMUX_DATA(SCIFB3_TXD_21_MARK,		PORT21_FN3,	MSEL3CR_09_0),
+	PINMUX_DATA(DU0_DB5_MARK,		PORT21_FN7),
+
+	/* Port22 */
+	PINMUX_DATA(LCDD22_MARK,		PORT22_FN1),
+	PINMUX_DATA(SCIFB3_RXD_22_MARK,		PORT22_FN3,	MSEL3CR_09_0),
+	PINMUX_DATA(DU0_DB6_MARK,		PORT22_FN7),
+
+	/* Port23 */
+	PINMUX_DATA(LCDD23_MARK,		PORT23_FN1),
+	PINMUX_DATA(SCIFB3_SCK_23_MARK,		PORT23_FN3),
+	PINMUX_DATA(DU0_DB7_MARK,		PORT23_FN7),
+
+	/* Port24 */
+	PINMUX_DATA(LCDHSYN_MARK,			PORT24_FN1),
+	PINMUX_DATA(LCDCS_MARK,				PORT24_FN2),
+	PINMUX_DATA(SCIFB1_RTS_24_MARK,			PORT24_FN3),
+	PINMUX_DATA(DU0_EXHSYNC_N_CSYNC_N_HSYNC_N_MARK,	PORT24_FN7),
+
+	/* Port25 */
+	PINMUX_DATA(LCDVSYN_MARK,			PORT25_FN1),
+	PINMUX_DATA(SCIFB1_CTS_25_MARK, PORT25_FN3, MSEL3CR_11_0),
+	PINMUX_DATA(DU0_EXVSYNC_N_VSYNC_N_CSYNC_N_MARK,	PORT25_FN7),
+
+	/* Port26 */
+	PINMUX_DATA(LCDDCK_MARK,		PORT26_FN1),
+	PINMUX_DATA(LCDWR_MARK,			PORT26_FN2),
+	PINMUX_DATA(SCIFB1_TXD_26_MARK,		PORT26_FN3,	MSEL3CR_11_0),
+	PINMUX_DATA(DU0_DOTCLKIN_MARK,		PORT26_FN7),
+
+	/* Port27 */
+	PINMUX_DATA(LCDDISP_MARK,		PORT27_FN1),
+	PINMUX_DATA(LCDRS_MARK,			PORT27_FN2),
+	PINMUX_DATA(SCIFB1_RXD_27_MARK,		PORT27_FN3,	MSEL3CR_11_0),
+	PINMUX_DATA(DU0_DOTCLKOUT_MARK,		PORT27_FN7),
+
+	/* Port28 */
+	PINMUX_DATA(LCDRD_N_MARK,		PORT28_FN1),
+	PINMUX_DATA(SCIFB1_SCK_28_MARK,		PORT28_FN3),
+	PINMUX_DATA(DU0_DOTCLKOUTB_MARK,	PORT28_FN7),
+
+	/* Port29 */
+	PINMUX_DATA(LCDLCLK_MARK,		PORT29_FN1),
+	PINMUX_DATA(SF_IRQ_02_MARK,		PORT29_FN4),
+	PINMUX_DATA(DU0_DISP_CSYNC_N_DE_MARK,	PORT29_FN7),
+
+	/* Port30 */
+	PINMUX_DATA(LCDDON_MARK,		PORT30_FN1),
+	PINMUX_DATA(SF_IRQ_03_MARK,		PORT30_FN4),
+	PINMUX_DATA(DU0_ODDF_N_CLAMP_MARK,	PORT30_FN7),
+
+	/* Port32 */
+	PINMUX_DATA(SCIFA0_RTS_MARK,		PORT32_FN1),
+	PINMUX_DATA(SIM0_DET_MARK,		PORT32_FN5),
+	PINMUX_DATA(CSCIF0_RTS_MARK,		PORT32_FN7),
+
+	/* Port33 */
+	PINMUX_DATA(SCIFA0_CTS_MARK,		PORT33_FN1),
+	PINMUX_DATA(SIM1_DET_MARK,		PORT33_FN5),
+	PINMUX_DATA(CSCIF0_CTS_MARK,		PORT33_FN7),
+
+	/* Port34 */
+	PINMUX_DATA(SCIFA0_SCK_MARK,		PORT34_FN1),
+	PINMUX_DATA(SIM0_PWRON_MARK,		PORT34_FN5),
+	PINMUX_DATA(CSCIF0_SCK_MARK,		PORT34_FN7),
+
+	/* Port35 */
+	PINMUX_DATA(SCIFA1_RTS_MARK,		PORT35_FN1),
+	PINMUX_DATA(CSCIF1_RTS_MARK,		PORT35_FN7),
+
+	/* Port36 */
+	PINMUX_DATA(SCIFA1_CTS_MARK,		PORT36_FN1),
+	PINMUX_DATA(CSCIF1_CTS_MARK,		PORT36_FN7),
+
+	/* Port37 */
+	PINMUX_DATA(SCIFA1_SCK_MARK,		PORT37_FN1),
+	PINMUX_DATA(CSCIF1_SCK_MARK,		PORT37_FN7),
+
+	/* Port38 */
+	PINMUX_DATA(SCIFB0_RTS_MARK,		PORT38_FN1),
+	PINMUX_DATA(TPU0TO1_MARK,		PORT38_FN3),
+	PINMUX_DATA(SCIFB3_RTS_38_MARK,		PORT38_FN4),
+	PINMUX_DATA(CHSCIF0_HRTS_MARK,		PORT38_FN7),
+
+	/* Port39 */
+	PINMUX_DATA(SCIFB0_CTS_MARK,		PORT39_FN1),
+	PINMUX_DATA(TPU0TO2_MARK,		PORT39_FN3),
+	PINMUX_DATA(SCIFB3_CTS_39_MARK,		PORT39_FN4,	MSEL3CR_09_1),
+	PINMUX_DATA(CHSCIF0_HCTS_MARK,		PORT39_FN7),
+
+	/* Port40 */
+	PINMUX_DATA(SCIFB0_SCK_MARK,		PORT40_FN1),
+	PINMUX_DATA(TPU0TO3_MARK,		PORT40_FN3),
+	PINMUX_DATA(SCIFB3_SCK_40_MARK,		PORT40_FN4),
+	PINMUX_DATA(CHSCIF0_HSCK_MARK,		PORT40_FN7),
+
+	/* Port64 */
+	PINMUX_DATA(PDM0_DATA_MARK,		PORT64_FN1),
+
+	/* Port65 */
+	PINMUX_DATA(PDM1_DATA_MARK,		PORT65_FN1),
+
+	/* Port66 */
+	PINMUX_DATA(HSI_RX_WAKE_MARK,		PORT66_FN1),
+	PINMUX_DATA(SCIFB2_CTS_66_MARK,		PORT66_FN2,	MSEL3CR_10_0),
+	PINMUX_DATA(MSIOF3_SYNC_MARK,		PORT66_FN3),
+	PINMUX_DATA(GenIO4_MARK,		PORT66_FN5),
+	PINMUX_DATA(IRQ40_MARK,			PORT66_FN0),
+
+	/* Port67 */
+	PINMUX_DATA(HSI_RX_READY_MARK,		PORT67_FN1),
+	PINMUX_DATA(SCIFB1_TXD_67_MARK,		PORT67_FN2,	MSEL3CR_11_1),
+	PINMUX_DATA(GIO_OUT3_67_MARK,		PORT67_FN5),
+	PINMUX_DATA(CHSCIF1_HTX_MARK,		PORT67_FN7),
+
+	/* Port68 */
+	PINMUX_DATA(HSI_RX_FLAG_MARK,		PORT68_FN1),
+	PINMUX_DATA(SCIFB2_TXD_68_MARK,		PORT68_FN2,	MSEL3CR_10_0),
+	PINMUX_DATA(MSIOF3_TXD_MARK,		PORT68_FN3),
+	PINMUX_DATA(GIO_OUT4_68_MARK,		PORT68_FN5),
+
+	/* Port69 */
+	PINMUX_DATA(HSI_RX_DATA_MARK,		PORT69_FN1),
+	PINMUX_DATA(SCIFB2_RXD_69_MARK,		PORT69_FN2,	MSEL3CR_10_0),
+	PINMUX_DATA(MSIOF3_RXD_MARK,		PORT69_FN3),
+	PINMUX_DATA(GIO_OUT5_69_MARK,		PORT69_FN5),
+
+	/* Port70 */
+	PINMUX_DATA(HSI_TX_FLAG_MARK,		PORT70_FN1),
+	PINMUX_DATA(SCIFB1_RTS_70_MARK,		PORT70_FN2),
+	PINMUX_DATA(GIO_OUT1_70_MARK,		PORT70_FN5),
+	PINMUX_DATA(HSIC_TSTCLK0_MARK,		PORT70_FN6),
+	PINMUX_DATA(CHSCIF1_HRTS_MARK,		PORT70_FN7),
+
+	/* Port71 */
+	PINMUX_DATA(HSI_TX_DATA_MARK,		PORT71_FN1),
+	PINMUX_DATA(SCIFB1_CTS_71_MARK,		PORT71_FN2,	MSEL3CR_11_1),
+	PINMUX_DATA(GIO_OUT2_71_MARK,		PORT71_FN5),
+	PINMUX_DATA(HSIC_TSTCLK1_MARK,		PORT71_FN6),
+	PINMUX_DATA(CHSCIF1_HCTS_MARK,		PORT71_FN7),
+
+	/* Port72 */
+	PINMUX_DATA(HSI_TX_WAKE_MARK,		PORT72_FN1),
+	PINMUX_DATA(SCIFB1_RXD_72_MARK,		PORT72_FN2,	MSEL3CR_11_1),
+	PINMUX_DATA(GenIO8_MARK,		PORT72_FN5),
+	PINMUX_DATA(CHSCIF1_HRX_MARK,		PORT72_FN7),
+
+	/* Port73 */
+	PINMUX_DATA(HSI_TX_READY_MARK,		PORT73_FN1),
+	PINMUX_DATA(SCIFB2_RTS_73_MARK,		PORT73_FN2),
+	PINMUX_DATA(MSIOF3_SCK_MARK,		PORT73_FN3),
+	PINMUX_DATA(GIO_OUT0_73_MARK,		PORT73_FN5),
+
+	/* Port74 - Port85 */
+	PINMUX_DATA(IRDA_OUT_MARK,		PORT74_FN1),
+	PINMUX_DATA(IRDA_IN_MARK,		PORT75_FN1),
+	PINMUX_DATA(IRDA_FIRSEL_MARK,		PORT76_FN1),
+	PINMUX_DATA(TPU0TO0_MARK,		PORT77_FN1),
+	PINMUX_DATA(DIGRFEN_MARK,		PORT78_FN1),
+	PINMUX_DATA(GPS_TIMESTAMP_MARK,		PORT79_FN1),
+	PINMUX_DATA(TXP_MARK,			PORT80_FN1),
+	PINMUX_DATA(TXP2_MARK,			PORT81_FN1),
+	PINMUX_DATA(COEX_0_MARK,		PORT82_FN1),
+	PINMUX_DATA(COEX_1_MARK,		PORT83_FN1),
+	PINMUX_DATA(IRQ19_MARK,			PORT84_FN0),
+	PINMUX_DATA(IRQ18_MARK,			PORT85_FN0),
+
+	/* Port96 - Port101 */
+	PINMUX_DATA(KEYIN0_MARK,		PORT96_FN1),
+	PINMUX_DATA(KEYIN1_MARK,		PORT97_FN1),
+	PINMUX_DATA(KEYIN2_MARK,		PORT98_FN1),
+	PINMUX_DATA(KEYIN3_MARK,		PORT99_FN1),
+	PINMUX_DATA(KEYIN4_MARK,		PORT100_FN1),
+	PINMUX_DATA(KEYIN5_MARK,		PORT101_FN1),
+
+	/* Port102 */
+	PINMUX_DATA(KEYIN6_MARK,		PORT102_FN1),
+	PINMUX_DATA(IRQ41_MARK,			PORT102_FN0),
+
+	/* Port103 */
+	PINMUX_DATA(KEYIN7_MARK,		PORT103_FN1),
+	PINMUX_DATA(IRQ42_MARK,			PORT103_FN0),
+
+	/* Port104 - Port108 */
+	PINMUX_DATA(KEYOUT0_MARK,		PORT104_FN2),
+	PINMUX_DATA(KEYOUT1_MARK,		PORT105_FN2),
+	PINMUX_DATA(KEYOUT2_MARK,		PORT106_FN2),
+	PINMUX_DATA(KEYOUT3_MARK,		PORT107_FN2),
+	PINMUX_DATA(KEYOUT4_MARK,		PORT108_FN2),
+
+	/* Port109 */
+	PINMUX_DATA(KEYOUT5_MARK,		PORT109_FN2),
+	PINMUX_DATA(IRQ43_MARK,			PORT109_FN0),
+
+	/* Port110 */
+	PINMUX_DATA(KEYOUT6_MARK,		PORT110_FN2),
+	PINMUX_DATA(IRQ44_MARK,			PORT110_FN0),
+
+	/* Port111 */
+	PINMUX_DATA(KEYOUT7_MARK,		PORT111_FN2),
+	PINMUX_DATA(RFANAEN_MARK,		PORT111_FN5),
+	PINMUX_DATA(IRQ45_MARK,			PORT111_FN0),
+
+	/* Port112 */
+	PINMUX_DATA(KEYIN8_MARK,		PORT112_FN1),
+	PINMUX_DATA(KEYOUT8_MARK,		PORT112_FN2),
+	PINMUX_DATA(SF_IRQ_04_MARK,		PORT112_FN4),
+	PINMUX_DATA(IRQ46_MARK,			PORT112_FN0),
+
+	/* Port113 */
+	PINMUX_DATA(KEYIN9_MARK,		PORT113_FN1),
+	PINMUX_DATA(KEYOUT9_MARK,		PORT113_FN2),
+	PINMUX_DATA(SF_IRQ_05_MARK,		PORT113_FN4),
+	PINMUX_DATA(IRQ47_MARK,			PORT113_FN0),
+
+	/* Port114 */
+	PINMUX_DATA(KEYIN10_MARK,		PORT114_FN1),
+	PINMUX_DATA(KEYOUT10_MARK,		PORT114_FN2),
+	PINMUX_DATA(SF_IRQ_06_MARK,		PORT114_FN4),
+	PINMUX_DATA(IRQ48_MARK,			PORT114_FN0),
+
+	/* Port115 */
+	PINMUX_DATA(KEYIN11_MARK,		PORT115_FN1),
+	PINMUX_DATA(KEYOUT11_MARK,		PORT115_FN2),
+	PINMUX_DATA(SF_IRQ_07_MARK,		PORT115_FN4),
+	PINMUX_DATA(IRQ49_MARK,			PORT115_FN0),
+
+	/* Port116 */
+	PINMUX_DATA(SCIFA0_TXD_MARK,		PORT116_FN1),
+	PINMUX_DATA(CSCIF0_TX_MARK,		PORT116_FN7),
+
+	/* Port117 */
+	PINMUX_DATA(SCIFA0_RXD_MARK,		PORT117_FN1),
+	PINMUX_DATA(CSCIF0_RX_MARK,		PORT117_FN7),
+
+	/* Port118 */
+	PINMUX_DATA(SCIFA1_TXD_MARK,		PORT118_FN1),
+	PINMUX_DATA(CSCIF1_TX_MARK,		PORT118_FN7),
+
+	/* Port119 */
+	PINMUX_DATA(SCIFA1_RXD_MARK,		PORT119_FN1),
+	PINMUX_DATA(CSCIF1_RX_MARK,		PORT119_FN7),
+
+	/* Port120 */
+	PINMUX_DATA(SF_PORT_1_120_MARK,		PORT120_FN3),
+	PINMUX_DATA(SCIFB3_RXD_120_MARK,	PORT120_FN4,	MSEL3CR_09_1),
+	PINMUX_DATA(DU0_CDE_MARK,		PORT120_FN7),
+
+	/* Port121 */
+	PINMUX_DATA(SF_PORT_0_121_MARK,		PORT121_FN3),
+	PINMUX_DATA(SCIFB3_TXD_121_MARK,	PORT121_FN4,	MSEL3CR_09_1),
+
+	/* Port122 */
+	PINMUX_DATA(SCIFB0_TXD_MARK,		PORT122_FN1),
+	PINMUX_DATA(CHSCIF0_HTX_MARK,		PORT122_FN7),
+
+	/* Port123 */
+	PINMUX_DATA(SCIFB0_RXD_MARK,		PORT123_FN1),
+	PINMUX_DATA(CHSCIF0_HRX_MARK,		PORT123_FN7),
+
+	/* Port124 */
+	PINMUX_DATA(ISP_STROBE_124_MARK,	PORT124_FN3),
+
+	/* Port125 */
+	PINMUX_DATA(STP_ISD_0_MARK,		PORT125_FN1),
+	PINMUX_DATA(PDM4_CLK_125_MARK,		PORT125_FN2),
+	PINMUX_DATA(MSIOF2_TXD_MARK,		PORT125_FN3),
+	PINMUX_DATA(SIM0_VOLTSEL0_MARK,		PORT125_FN5),
+
+	/* Port126 */
+	PINMUX_DATA(TS_SDEN_MARK,		PORT126_FN1),
+	PINMUX_DATA(MSIOF7_SYNC_MARK,		PORT126_FN2),
+	PINMUX_DATA(STP_ISEN_1_MARK,		PORT126_FN3),
+
+	/* Port128 */
+	PINMUX_DATA(STP_ISEN_0_MARK,		PORT128_FN1),
+	PINMUX_DATA(PDM1_OUTDATA_128_MARK,	PORT128_FN2),
+	PINMUX_DATA(MSIOF2_SYNC_MARK,		PORT128_FN3),
+	PINMUX_DATA(SIM1_VOLTSEL1_MARK,		PORT128_FN5),
+
+	/* Port129 */
+	PINMUX_DATA(TS_SPSYNC_MARK,		PORT129_FN1),
+	PINMUX_DATA(MSIOF7_RXD_MARK,		PORT129_FN2),
+	PINMUX_DATA(STP_ISSYNC_1_MARK,		PORT129_FN3),
+
+	/* Port130 */
+	PINMUX_DATA(STP_ISSYNC_0_MARK,		PORT130_FN1),
+	PINMUX_DATA(PDM4_DATA_130_MARK,		PORT130_FN2,	MSEL3CR_12_1),
+	PINMUX_DATA(MSIOF2_RXD_MARK,		PORT130_FN3),
+	PINMUX_DATA(SIM0_VOLTSEL1_MARK,		PORT130_FN5),
+
+	/* Port131 */
+	PINMUX_DATA(STP_OPWM_0_MARK,		PORT131_FN1),
+	PINMUX_DATA(SIM1_PWRON_MARK,		PORT131_FN5),
+
+	/* Port132 */
+	PINMUX_DATA(TS_SCK_MARK,		PORT132_FN1),
+	PINMUX_DATA(MSIOF7_SCK_MARK,		PORT132_FN2),
+	PINMUX_DATA(STP_ISCLK_1_MARK,		PORT132_FN3),
+
+	/* Port133 */
+	PINMUX_DATA(STP_ISCLK_0_MARK,		PORT133_FN1),
+	PINMUX_DATA(PDM1_OUTCLK_133_MARK,	PORT133_FN2),
+	PINMUX_DATA(MSIOF2_SCK_MARK,		PORT133_FN3),
+	PINMUX_DATA(SIM1_VOLTSEL0_MARK,		PORT133_FN5),
+
+	/* Port134 */
+	PINMUX_DATA(TS_SDAT_MARK,		PORT134_FN1),
+	PINMUX_DATA(MSIOF7_TXD_MARK,		PORT134_FN2),
+	PINMUX_DATA(STP_ISD_1_MARK,		PORT134_FN3),
+
+	/* Port160 - Port178 */
+	PINMUX_DATA(IRQ20_MARK,			PORT160_FN0),
+	PINMUX_DATA(IRQ21_MARK,			PORT161_FN0),
+	PINMUX_DATA(IRQ22_MARK,			PORT162_FN0),
+	PINMUX_DATA(IRQ23_MARK,			PORT163_FN0),
+	PINMUX_DATA(MMCD0_0_MARK,		PORT164_FN1),
+	PINMUX_DATA(MMCD0_1_MARK,		PORT165_FN1),
+	PINMUX_DATA(MMCD0_2_MARK,		PORT166_FN1),
+	PINMUX_DATA(MMCD0_3_MARK,		PORT167_FN1),
+	PINMUX_DATA(MMCD0_4_MARK,		PORT168_FN1),
+	PINMUX_DATA(MMCD0_5_MARK,		PORT169_FN1),
+	PINMUX_DATA(MMCD0_6_MARK,		PORT170_FN1),
+	PINMUX_DATA(MMCD0_7_MARK,		PORT171_FN1),
+	PINMUX_DATA(MMCCMD0_MARK,		PORT172_FN1),
+	PINMUX_DATA(MMCCLK0_MARK,		PORT173_FN1),
+	PINMUX_DATA(MMCRST_MARK,		PORT174_FN1),
+	PINMUX_DATA(IRQ24_MARK,			PORT175_FN0),
+	PINMUX_DATA(IRQ25_MARK,			PORT176_FN0),
+	PINMUX_DATA(IRQ26_MARK,			PORT177_FN0),
+	PINMUX_DATA(IRQ27_MARK,			PORT178_FN0),
+
+	/* Port192 - Port200 FN1 */
+	PINMUX_DATA(A10_MARK,		PORT192_FN1),
+	PINMUX_DATA(A9_MARK,		PORT193_FN1),
+	PINMUX_DATA(A8_MARK,		PORT194_FN1),
+	PINMUX_DATA(A7_MARK,		PORT195_FN1),
+	PINMUX_DATA(A6_MARK,		PORT196_FN1),
+	PINMUX_DATA(A5_MARK,		PORT197_FN1),
+	PINMUX_DATA(A4_MARK,		PORT198_FN1),
+	PINMUX_DATA(A3_MARK,		PORT199_FN1),
+	PINMUX_DATA(A2_MARK,		PORT200_FN1),
+
+	/* Port192 - Port200 FN2 */
+	PINMUX_DATA(MMCD1_7_MARK,		PORT192_FN2),
+	PINMUX_DATA(MMCD1_6_MARK,		PORT193_FN2),
+	PINMUX_DATA(MMCD1_5_MARK,		PORT194_FN2),
+	PINMUX_DATA(MMCD1_4_MARK,		PORT195_FN2),
+	PINMUX_DATA(MMCD1_3_MARK,		PORT196_FN2),
+	PINMUX_DATA(MMCD1_2_MARK,		PORT197_FN2),
+	PINMUX_DATA(MMCD1_1_MARK,		PORT198_FN2),
+	PINMUX_DATA(MMCD1_0_MARK,		PORT199_FN2),
+	PINMUX_DATA(MMCCMD1_MARK,		PORT200_FN2),
+
+	/* Port192 - Port200 IRQ */
+	PINMUX_DATA(IRQ31_MARK,			PORT192_FN0),
+	PINMUX_DATA(IRQ32_MARK,			PORT193_FN0),
+	PINMUX_DATA(IRQ33_MARK,			PORT194_FN0),
+	PINMUX_DATA(IRQ34_MARK,			PORT195_FN0),
+	PINMUX_DATA(IRQ35_MARK,			PORT196_FN0),
+	PINMUX_DATA(IRQ36_MARK,			PORT197_FN0),
+	PINMUX_DATA(IRQ37_MARK,			PORT198_FN0),
+	PINMUX_DATA(IRQ38_MARK,			PORT199_FN0),
+	PINMUX_DATA(IRQ39_MARK,			PORT200_FN0),
+
+	/* Port201 */
+	PINMUX_DATA(A1_MARK,		PORT201_FN1),
+
+	/* Port202 */
+	PINMUX_DATA(A0_MARK,		PORT202_FN1),
+	PINMUX_DATA(BS_MARK,		PORT202_FN2),
+
+	/* Port203 */
+	PINMUX_DATA(CKO_MARK,		PORT203_FN1),
+	PINMUX_DATA(MMCCLK1_MARK,	PORT203_FN2),
+
+	/* Port204 */
+	PINMUX_DATA(CS0_N_MARK,		PORT204_FN1),
+	PINMUX_DATA(SIM0_GPO1_MARK,	PORT204_FN5),
+
+	/* Port205 */
+	PINMUX_DATA(CS2_N_MARK,		PORT205_FN1),
+	PINMUX_DATA(SIM0_GPO2_MARK,	PORT205_FN5),
+
+	/* Port206 */
+	PINMUX_DATA(CS4_N_MARK,		PORT206_FN1),
+	PINMUX_DATA(VIO_VD_MARK,	PORT206_FN2),
+	PINMUX_DATA(SIM1_GPO0_MARK,	PORT206_FN5),
+
+	/* Port207 - Port212 FN1 */
+	PINMUX_DATA(D15_MARK,		PORT207_FN1),
+	PINMUX_DATA(D14_MARK,		PORT208_FN1),
+	PINMUX_DATA(D13_MARK,		PORT209_FN1),
+	PINMUX_DATA(D12_MARK,		PORT210_FN1),
+	PINMUX_DATA(D11_MARK,		PORT211_FN1),
+	PINMUX_DATA(D10_MARK,		PORT212_FN1),
+
+	/* Port207 - Port212 FN5 */
+	PINMUX_DATA(GIO_OUT15_MARK,			PORT207_FN5),
+	PINMUX_DATA(GIO_OUT14_MARK,			PORT208_FN5),
+	PINMUX_DATA(GIO_OUT13_MARK,			PORT209_FN5),
+	PINMUX_DATA(GIO_OUT12_MARK,			PORT210_FN5),
+	PINMUX_DATA(WGM_TXP2_MARK,			PORT211_FN5),
+	PINMUX_DATA(WGM_GPS_TIMEM_ASK_RFCLK_MARK,	PORT212_FN5),
+
+	/* Port213 - Port222 FN1 */
+	PINMUX_DATA(D9_MARK,		PORT213_FN1),
+	PINMUX_DATA(D8_MARK,		PORT214_FN1),
+	PINMUX_DATA(D7_MARK,		PORT215_FN1),
+	PINMUX_DATA(D6_MARK,		PORT216_FN1),
+	PINMUX_DATA(D5_MARK,		PORT217_FN1),
+	PINMUX_DATA(D4_MARK,		PORT218_FN1),
+	PINMUX_DATA(D3_MARK,		PORT219_FN1),
+	PINMUX_DATA(D2_MARK,		PORT220_FN1),
+	PINMUX_DATA(D1_MARK,		PORT221_FN1),
+	PINMUX_DATA(D0_MARK,		PORT222_FN1),
+
+	/* Port213 - Port222 FN2 */
+	PINMUX_DATA(VIO_D9_MARK,	PORT213_FN2),
+	PINMUX_DATA(VIO_D8_MARK,	PORT214_FN2),
+	PINMUX_DATA(VIO_D7_MARK,	PORT215_FN2),
+	PINMUX_DATA(VIO_D6_MARK,	PORT216_FN2),
+	PINMUX_DATA(VIO_D5_MARK,	PORT217_FN2),
+	PINMUX_DATA(VIO_D4_MARK,	PORT218_FN2),
+	PINMUX_DATA(VIO_D3_MARK,	PORT219_FN2),
+	PINMUX_DATA(VIO_D2_MARK,	PORT220_FN2),
+	PINMUX_DATA(VIO_D1_MARK,	PORT221_FN2),
+	PINMUX_DATA(VIO_D0_MARK,	PORT222_FN2),
+
+	/* Port213 - Port222 FN5 */
+	PINMUX_DATA(GIO_OUT9_MARK,	PORT213_FN5),
+	PINMUX_DATA(GIO_OUT8_MARK,	PORT214_FN5),
+	PINMUX_DATA(GIO_OUT7_MARK,	PORT215_FN5),
+	PINMUX_DATA(GIO_OUT6_MARK,	PORT216_FN5),
+	PINMUX_DATA(GIO_OUT5_217_MARK,	PORT217_FN5),
+	PINMUX_DATA(GIO_OUT4_218_MARK,	PORT218_FN5),
+	PINMUX_DATA(GIO_OUT3_219_MARK,	PORT219_FN5),
+	PINMUX_DATA(GIO_OUT2_220_MARK,	PORT220_FN5),
+	PINMUX_DATA(GIO_OUT1_221_MARK,	PORT221_FN5),
+	PINMUX_DATA(GIO_OUT0_222_MARK,	PORT222_FN5),
+
+	/* Port224 */
+	PINMUX_DATA(RDWR_224_MARK,	PORT224_FN1),
+	PINMUX_DATA(VIO_HD_MARK,	PORT224_FN2),
+	PINMUX_DATA(SIM1_GPO2_MARK,	PORT224_FN5),
+
+	/* Port225 */
+	PINMUX_DATA(RD_N_MARK,		PORT225_FN1),
+
+	/* Port226 */
+	PINMUX_DATA(WAIT_N_MARK,	PORT226_FN1),
+	PINMUX_DATA(VIO_CLK_MARK,	PORT226_FN2),
+	PINMUX_DATA(SIM1_GPO1_MARK,	PORT226_FN5),
+
+	/* Port227 */
+	PINMUX_DATA(WE0_N_MARK,		PORT227_FN1),
+	PINMUX_DATA(RDWR_227_MARK,	PORT227_FN2),
+
+	/* Port228 */
+	PINMUX_DATA(WE1_N_MARK,		PORT228_FN1),
+	PINMUX_DATA(SIM0_GPO0_MARK,	PORT228_FN5),
+
+	/* Port229 */
+	PINMUX_DATA(PWMO_MARK,		PORT229_FN1),
+	PINMUX_DATA(VIO_CKO1_229_MARK,	PORT229_FN2),
+
+	/* Port230 */
+	PINMUX_DATA(SLIM_CLK_MARK,	PORT230_FN1),
+	PINMUX_DATA(VIO_CKO4_230_MARK,	PORT230_FN2),
+
+	/* Port231 */
+	PINMUX_DATA(SLIM_DATA_MARK,	PORT231_FN1),
+	PINMUX_DATA(VIO_CKO5_231_MARK,	PORT231_FN2),
+
+	/* Port232 */
+	PINMUX_DATA(VIO_CKO2_232_MARK,	PORT232_FN2),
+	PINMUX_DATA(SF_PORT_0_232_MARK,	PORT232_FN4),
+
+	/* Port233 */
+	PINMUX_DATA(VIO_CKO3_233_MARK,	PORT233_FN2),
+	PINMUX_DATA(SF_PORT_1_233_MARK,	PORT233_FN4),
+
+	/* Port234 */
+	PINMUX_DATA(FSIACK_MARK,	PORT234_FN1),
+	PINMUX_DATA(PDM3_CLK_234_MARK,	PORT234_FN2),
+	PINMUX_DATA(ISP_IRIS1_234_MARK,	PORT234_FN3),
+
+	/* Port235 */
+	PINMUX_DATA(FSIAISLD_MARK,	PORT235_FN1),
+	PINMUX_DATA(PDM3_DATA_235_MARK,	PORT235_FN2,	MSEL3CR_12_1),
+
+	/* Port236 */
+	PINMUX_DATA(FSIAOMC_MARK,		PORT236_FN1),
+	PINMUX_DATA(PDM0_OUTCLK_236_MARK,	PORT236_FN2),
+	PINMUX_DATA(ISP_IRIS0_236_MARK,		PORT236_FN3),
+
+	/* Port237 */
+	PINMUX_DATA(FSIAOLR_MARK,	PORT237_FN1),
+	PINMUX_DATA(FSIAILR_MARK,	PORT237_FN2),
+
+	/* Port238 */
+	PINMUX_DATA(FSIAOBT_MARK,	PORT238_FN1),
+	PINMUX_DATA(FSIAIBT_MARK,	PORT238_FN2),
+
+	/* Port239 */
+	PINMUX_DATA(FSIAOSLD_MARK,		PORT239_FN1),
+	PINMUX_DATA(PDM0_OUTDATA_239_MARK,	PORT239_FN2),
+
+	/* Port240 */
+	PINMUX_DATA(FSIBISLD_MARK,	PORT240_FN1),
+
+	/* Port241 */
+	PINMUX_DATA(FSIBOLR_MARK,	PORT241_FN1),
+	PINMUX_DATA(FSIBILR_MARK,	PORT241_FN2),
+
+	/* Port242 */
+	PINMUX_DATA(FSIBOMC_MARK,		PORT242_FN1),
+	PINMUX_DATA(ISP_SHUTTER1_242_MARK,	PORT242_FN3),
+
+	/* Port243 */
+	PINMUX_DATA(FSIBOBT_MARK,	PORT243_FN1),
+	PINMUX_DATA(FSIBIBT_MARK,	PORT243_FN2),
+
+	/* Port244 */
+	PINMUX_DATA(FSIBOSLD_MARK,	PORT244_FN1),
+	PINMUX_DATA(FSIASPDIF_MARK,	PORT244_FN2),
+
+	/* Port245 */
+	PINMUX_DATA(FSIBCK_MARK,		PORT245_FN1),
+	PINMUX_DATA(ISP_SHUTTER0_245_MARK,	PORT245_FN3),
+
+	/* Port246 - Port250 FN1 */
+	PINMUX_DATA(ISP_IRIS1_246_MARK,		PORT246_FN1),
+	PINMUX_DATA(ISP_IRIS0_247_MARK,		PORT247_FN1),
+	PINMUX_DATA(ISP_SHUTTER1_248_MARK,	PORT248_FN1),
+	PINMUX_DATA(ISP_SHUTTER0_249_MARK,	PORT249_FN1),
+	PINMUX_DATA(ISP_STROBE_250_MARK,	PORT250_FN1),
+
+	/* Port256 - Port258 */
+	PINMUX_DATA(MSIOF0_SYNC_MARK,		PORT256_FN1),
+	PINMUX_DATA(MSIOF0_RXD_MARK,		PORT257_FN1),
+	PINMUX_DATA(MSIOF0_SCK_MARK,		PORT258_FN1),
+
+	/* Port259 */
+	PINMUX_DATA(MSIOF0_SS2_MARK,		PORT259_FN1),
+	PINMUX_DATA(VIO_CKO3_259_MARK,		PORT259_FN3),
+
+	/* Port260 */
+	PINMUX_DATA(MSIOF0_TXD_MARK,		PORT260_FN1),
+
+	/* Port261 */
+	PINMUX_DATA(SCIFB1_SCK_261_MARK,	PORT261_FN2),
+	PINMUX_DATA(CHSCIF1_HSCK_MARK,		PORT261_FN7),
+
+	/* Port262 */
+	PINMUX_DATA(SCIFB2_SCK_262_MARK,	PORT262_FN2),
+
+	/* Port263 - Port266 FN1 */
+	PINMUX_DATA(MSIOF1_SS2_MARK,		PORT263_FN1),
+	PINMUX_DATA(MSIOF1_TXD_MARK,		PORT264_FN1),
+	PINMUX_DATA(MSIOF1_RXD_MARK,		PORT265_FN1),
+	PINMUX_DATA(MSIOF1_SS1_MARK,		PORT266_FN1),
+
+	/* Port263 - Port266 FN4 */
+	PINMUX_DATA(MSIOF5_SS2_MARK,		PORT263_FN4),
+	PINMUX_DATA(MSIOF5_TXD_MARK,		PORT264_FN4),
+	PINMUX_DATA(MSIOF5_RXD_MARK,		PORT265_FN4),
+	PINMUX_DATA(MSIOF5_SS1_MARK,		PORT266_FN4),
+
+	/* Port267 */
+	PINMUX_DATA(MSIOF0_SS1_MARK,		PORT267_FN1),
+
+	/* Port268 */
+	PINMUX_DATA(MSIOF1_SCK_MARK,		PORT268_FN1),
+	PINMUX_DATA(MSIOF5_SCK_MARK,		PORT268_FN4),
+
+	/* Port269 */
+	PINMUX_DATA(MSIOF1_SYNC_MARK,		PORT269_FN1),
+	PINMUX_DATA(MSIOF5_SYNC_MARK,		PORT269_FN4),
+
+	/* Port270 - Port273 FN1 */
+	PINMUX_DATA(MSIOF2_SS1_MARK,		PORT270_FN1),
+	PINMUX_DATA(MSIOF2_SS2_MARK,		PORT271_FN1),
+	PINMUX_DATA(MSIOF3_SS2_MARK,		PORT272_FN1),
+	PINMUX_DATA(MSIOF3_SS1_MARK,		PORT273_FN1),
+
+	/* Port270 - Port273 FN3 */
+	PINMUX_DATA(VIO_CKO5_270_MARK,		PORT270_FN3),
+	PINMUX_DATA(VIO_CKO2_271_MARK,		PORT271_FN3),
+	PINMUX_DATA(VIO_CKO1_272_MARK,		PORT272_FN3),
+	PINMUX_DATA(VIO_CKO4_273_MARK,		PORT273_FN3),
+
+	/* Port274 */
+	PINMUX_DATA(MSIOF4_SS2_MARK,		PORT274_FN1),
+	PINMUX_DATA(TPU1TO0_MARK,		PORT274_FN4),
+
+	/* Port275 - Port280 */
+	PINMUX_DATA(IC_DP_MARK,			PORT275_FN1),
+	PINMUX_DATA(SIM0_RST_MARK,		PORT276_FN1),
+	PINMUX_DATA(IC_DM_MARK,			PORT277_FN1),
+	PINMUX_DATA(SIM0_BSICOMP_MARK,		PORT278_FN1),
+	PINMUX_DATA(SIM0_CLK_MARK,		PORT279_FN1),
+	PINMUX_DATA(SIM0_IO_MARK,		PORT280_FN1),
+
+	/* Port281 */
+	PINMUX_DATA(SIM1_IO_MARK,		PORT281_FN1),
+	PINMUX_DATA(PDM2_DATA_281_MARK,		PORT281_FN2,	MSEL3CR_12_1),
+
+	/* Port282 */
+	PINMUX_DATA(SIM1_CLK_MARK,		PORT282_FN1),
+	PINMUX_DATA(PDM2_CLK_282_MARK,		PORT282_FN2),
+
+	/* Port283 */
+	PINMUX_DATA(SIM1_RST_MARK,		PORT283_FN1),
+
+	/* Port289 */
+	PINMUX_DATA(SDHID1_0_MARK,		PORT289_FN1),
+	PINMUX_DATA(STMDATA0_2_MARK,		PORT289_FN3),
+
+	/* Port290 */
+	PINMUX_DATA(SDHID1_1_MARK,		PORT290_FN1),
+	PINMUX_DATA(STMDATA1_2_MARK,		PORT290_FN3),
+	PINMUX_DATA(IRQ51_MARK,			PORT290_FN0),
+
+	/* Port291 - Port294 FN1 */
+	PINMUX_DATA(SDHID1_2_MARK,		PORT291_FN1),
+	PINMUX_DATA(SDHID1_3_MARK,		PORT292_FN1),
+	PINMUX_DATA(SDHICLK1_MARK,		PORT293_FN1),
+	PINMUX_DATA(SDHICMD1_MARK,		PORT294_FN1),
+
+	/* Port291 - Port294 FN3 */
+	PINMUX_DATA(STMDATA2_2_MARK,		PORT291_FN3),
+	PINMUX_DATA(STMDATA3_2_MARK,		PORT292_FN3),
+	PINMUX_DATA(STMCLK_2_MARK,		PORT293_FN3),
+	PINMUX_DATA(STMSIDI_2_MARK,		PORT294_FN3),
+
+	/* Port295 */
+	PINMUX_DATA(SDHID2_0_MARK,		PORT295_FN1),
+	PINMUX_DATA(MSIOF4_TXD_MARK,		PORT295_FN2),
+	PINMUX_DATA(SCIFB2_TXD_295_MARK,	PORT295_FN3,	MSEL3CR_10_1),
+	PINMUX_DATA(MSIOF6_TXD_MARK,		PORT295_FN4),
+
+	/* Port296 */
+	PINMUX_DATA(SDHID2_1_MARK,		PORT296_FN1),
+	PINMUX_DATA(MSIOF6_SS2_MARK,		PORT296_FN4),
+	PINMUX_DATA(IRQ52_MARK,			PORT296_FN0),
+
+	/* Port297 - Port300 FN1 */
+	PINMUX_DATA(SDHID2_2_MARK,		PORT297_FN1),
+	PINMUX_DATA(SDHID2_3_MARK,		PORT298_FN1),
+	PINMUX_DATA(SDHICLK2_MARK,		PORT299_FN1),
+	PINMUX_DATA(SDHICMD2_MARK,		PORT300_FN1),
+
+	/* Port297 - Port300 FN2 */
+	PINMUX_DATA(MSIOF4_RXD_MARK,		PORT297_FN2),
+	PINMUX_DATA(MSIOF4_SYNC_MARK,		PORT298_FN2),
+	PINMUX_DATA(MSIOF4_SCK_MARK,		PORT299_FN2),
+	PINMUX_DATA(MSIOF4_SS1_MARK,		PORT300_FN2),
+
+	/* Port297 - Port300 FN3 */
+	PINMUX_DATA(SCIFB2_RXD_297_MARK,	PORT297_FN3,	MSEL3CR_10_1),
+	PINMUX_DATA(SCIFB2_CTS_298_MARK,	PORT298_FN3,	MSEL3CR_10_1),
+	PINMUX_DATA(SCIFB2_SCK_299_MARK,	PORT299_FN3),
+	PINMUX_DATA(SCIFB2_RTS_300_MARK,	PORT300_FN3),
+
+	/* Port297 - Port300 FN4 */
+	PINMUX_DATA(MSIOF6_RXD_MARK,		PORT297_FN4),
+	PINMUX_DATA(MSIOF6_SYNC_MARK,		PORT298_FN4),
+	PINMUX_DATA(MSIOF6_SCK_MARK,		PORT299_FN4),
+	PINMUX_DATA(MSIOF6_SS1_MARK,		PORT300_FN4),
+
+	/* Port301 */
+	PINMUX_DATA(SDHICD0_MARK,		PORT301_FN1),
+	PINMUX_DATA(IRQ50_MARK,			PORT301_FN0),
+
+	/* Port302 - Port306 FN1 */
+	PINMUX_DATA(SDHID0_0_MARK,		PORT302_FN1),
+	PINMUX_DATA(SDHID0_1_MARK,		PORT303_FN1),
+	PINMUX_DATA(SDHID0_2_MARK,		PORT304_FN1),
+	PINMUX_DATA(SDHID0_3_MARK,		PORT305_FN1),
+	PINMUX_DATA(SDHICMD0_MARK,		PORT306_FN1),
+
+	/* Port302 - Port306 FN3 */
+	PINMUX_DATA(STMDATA0_1_MARK,		PORT302_FN3),
+	PINMUX_DATA(STMDATA1_1_MARK,		PORT303_FN3),
+	PINMUX_DATA(STMDATA2_1_MARK,		PORT304_FN3),
+	PINMUX_DATA(STMDATA3_1_MARK,		PORT305_FN3),
+	PINMUX_DATA(STMSIDI_1_MARK,		PORT306_FN3),
+
+	/* Port307 */
+	PINMUX_DATA(SDHIWP0_MARK,		PORT307_FN1),
+
+	/* Port308 */
+	PINMUX_DATA(SDHICLK0_MARK,		PORT308_FN1),
+	PINMUX_DATA(STMCLK_1_MARK,		PORT308_FN3),
+
+	/* Port320 - Port329 */
+	PINMUX_DATA(IRQ16_MARK,			PORT320_FN0),
+	PINMUX_DATA(IRQ17_MARK,			PORT321_FN0),
+	PINMUX_DATA(IRQ28_MARK,			PORT322_FN0),
+	PINMUX_DATA(IRQ29_MARK,			PORT323_FN0),
+	PINMUX_DATA(IRQ30_MARK,			PORT324_FN0),
+	PINMUX_DATA(IRQ53_MARK,			PORT325_FN0),
+	PINMUX_DATA(IRQ54_MARK,			PORT326_FN0),
+	PINMUX_DATA(IRQ55_MARK,			PORT327_FN0),
+	PINMUX_DATA(IRQ56_MARK,			PORT328_FN0),
+	PINMUX_DATA(IRQ57_MARK,			PORT329_FN0),
+};
+
+#define R8A73A4_PIN(pin, cfgs)			\
+	{					\
+		.name = __stringify(PORT##pin),	\
+		.enum_id = PORT##pin##_DATA,	\
+		.configs = cfgs,		\
+	}
+
+#define __O	(SH_PFC_PIN_CFG_OUTPUT)
+#define __IO	(SH_PFC_PIN_CFG_INPUT | SH_PFC_PIN_CFG_OUTPUT)
+#define __PUD	(SH_PFC_PIN_CFG_PULL_DOWN | SH_PFC_PIN_CFG_PULL_UP)
+
+#define R8A73A4_PIN_IO_PU_PD(pin)       R8A73A4_PIN(pin, __IO | __PUD)
+#define R8A73A4_PIN_O(pin)              R8A73A4_PIN(pin, __O)
+
+static struct sh_pfc_pin pinmux_pins[] = {
+	R8A73A4_PIN_IO_PU_PD(0), R8A73A4_PIN_IO_PU_PD(1),
+	R8A73A4_PIN_IO_PU_PD(2), R8A73A4_PIN_IO_PU_PD(3),
+	R8A73A4_PIN_IO_PU_PD(4), R8A73A4_PIN_IO_PU_PD(5),
+	R8A73A4_PIN_IO_PU_PD(6), R8A73A4_PIN_IO_PU_PD(7),
+	R8A73A4_PIN_IO_PU_PD(8), R8A73A4_PIN_IO_PU_PD(9),
+	R8A73A4_PIN_IO_PU_PD(10), R8A73A4_PIN_IO_PU_PD(11),
+	R8A73A4_PIN_IO_PU_PD(12), R8A73A4_PIN_IO_PU_PD(13),
+	R8A73A4_PIN_IO_PU_PD(14), R8A73A4_PIN_IO_PU_PD(15),
+	R8A73A4_PIN_IO_PU_PD(16), R8A73A4_PIN_IO_PU_PD(17),
+	R8A73A4_PIN_IO_PU_PD(18), R8A73A4_PIN_IO_PU_PD(19),
+	R8A73A4_PIN_IO_PU_PD(20), R8A73A4_PIN_IO_PU_PD(21),
+	R8A73A4_PIN_IO_PU_PD(22), R8A73A4_PIN_IO_PU_PD(23),
+	R8A73A4_PIN_IO_PU_PD(24), R8A73A4_PIN_IO_PU_PD(25),
+	R8A73A4_PIN_IO_PU_PD(26), R8A73A4_PIN_IO_PU_PD(27),
+	R8A73A4_PIN_IO_PU_PD(28), R8A73A4_PIN_IO_PU_PD(29),
+	R8A73A4_PIN_IO_PU_PD(30),
+	R8A73A4_PIN_IO_PU_PD(32), R8A73A4_PIN_IO_PU_PD(33),
+	R8A73A4_PIN_IO_PU_PD(34), R8A73A4_PIN_IO_PU_PD(35),
+	R8A73A4_PIN_IO_PU_PD(36), R8A73A4_PIN_IO_PU_PD(37),
+	R8A73A4_PIN_IO_PU_PD(38), R8A73A4_PIN_IO_PU_PD(39),
+	R8A73A4_PIN_IO_PU_PD(40),
+	R8A73A4_PIN_IO_PU_PD(64), R8A73A4_PIN_IO_PU_PD(65),
+	R8A73A4_PIN_IO_PU_PD(66), R8A73A4_PIN_IO_PU_PD(67),
+	R8A73A4_PIN_IO_PU_PD(68), R8A73A4_PIN_IO_PU_PD(69),
+	R8A73A4_PIN_IO_PU_PD(70), R8A73A4_PIN_IO_PU_PD(71),
+	R8A73A4_PIN_IO_PU_PD(72), R8A73A4_PIN_IO_PU_PD(73),
+	R8A73A4_PIN_O(74), R8A73A4_PIN_IO_PU_PD(75),
+	R8A73A4_PIN_IO_PU_PD(76), R8A73A4_PIN_IO_PU_PD(77),
+	R8A73A4_PIN_IO_PU_PD(78), R8A73A4_PIN_IO_PU_PD(79),
+	R8A73A4_PIN_IO_PU_PD(80), R8A73A4_PIN_IO_PU_PD(81),
+	R8A73A4_PIN_IO_PU_PD(82), R8A73A4_PIN_IO_PU_PD(83),
+	R8A73A4_PIN_IO_PU_PD(84), R8A73A4_PIN_IO_PU_PD(85),
+	R8A73A4_PIN_IO_PU_PD(96), R8A73A4_PIN_IO_PU_PD(97),
+	R8A73A4_PIN_IO_PU_PD(98), R8A73A4_PIN_IO_PU_PD(99),
+	R8A73A4_PIN_IO_PU_PD(100), R8A73A4_PIN_IO_PU_PD(101),
+	R8A73A4_PIN_IO_PU_PD(102), R8A73A4_PIN_IO_PU_PD(103),
+	R8A73A4_PIN_IO_PU_PD(104), R8A73A4_PIN_IO_PU_PD(105),
+	R8A73A4_PIN_IO_PU_PD(106), R8A73A4_PIN_IO_PU_PD(107),
+	R8A73A4_PIN_IO_PU_PD(108), R8A73A4_PIN_IO_PU_PD(109),
+	R8A73A4_PIN_IO_PU_PD(110), R8A73A4_PIN_IO_PU_PD(111),
+	R8A73A4_PIN_IO_PU_PD(112), R8A73A4_PIN_IO_PU_PD(113),
+	R8A73A4_PIN_IO_PU_PD(114), R8A73A4_PIN_IO_PU_PD(115),
+	R8A73A4_PIN_IO_PU_PD(116), R8A73A4_PIN_IO_PU_PD(117),
+	R8A73A4_PIN_IO_PU_PD(118), R8A73A4_PIN_IO_PU_PD(119),
+	R8A73A4_PIN_IO_PU_PD(120), R8A73A4_PIN_IO_PU_PD(121),
+	R8A73A4_PIN_IO_PU_PD(122), R8A73A4_PIN_IO_PU_PD(123),
+	R8A73A4_PIN_IO_PU_PD(124), R8A73A4_PIN_IO_PU_PD(125),
+	R8A73A4_PIN_IO_PU_PD(126),
+	R8A73A4_PIN_IO_PU_PD(128), R8A73A4_PIN_IO_PU_PD(129),
+	R8A73A4_PIN_IO_PU_PD(130), R8A73A4_PIN_IO_PU_PD(131),
+	R8A73A4_PIN_IO_PU_PD(132), R8A73A4_PIN_IO_PU_PD(133),
+	R8A73A4_PIN_IO_PU_PD(134),
+	R8A73A4_PIN_IO_PU_PD(160), R8A73A4_PIN_IO_PU_PD(161),
+	R8A73A4_PIN_IO_PU_PD(162), R8A73A4_PIN_IO_PU_PD(163),
+	R8A73A4_PIN_IO_PU_PD(164), R8A73A4_PIN_IO_PU_PD(165),
+	R8A73A4_PIN_IO_PU_PD(166), R8A73A4_PIN_IO_PU_PD(167),
+	R8A73A4_PIN_IO_PU_PD(168), R8A73A4_PIN_IO_PU_PD(169),
+	R8A73A4_PIN_IO_PU_PD(170), R8A73A4_PIN_IO_PU_PD(171),
+	R8A73A4_PIN_IO_PU_PD(172), R8A73A4_PIN_IO_PU_PD(173),
+	R8A73A4_PIN_IO_PU_PD(174), R8A73A4_PIN_IO_PU_PD(175),
+	R8A73A4_PIN_IO_PU_PD(176), R8A73A4_PIN_IO_PU_PD(177),
+	R8A73A4_PIN_IO_PU_PD(178),
+	R8A73A4_PIN_IO_PU_PD(192), R8A73A4_PIN_IO_PU_PD(193),
+	R8A73A4_PIN_IO_PU_PD(194), R8A73A4_PIN_IO_PU_PD(195),
+	R8A73A4_PIN_IO_PU_PD(196), R8A73A4_PIN_IO_PU_PD(197),
+	R8A73A4_PIN_IO_PU_PD(198), R8A73A4_PIN_IO_PU_PD(199),
+	R8A73A4_PIN_IO_PU_PD(200), R8A73A4_PIN_IO_PU_PD(201),
+	R8A73A4_PIN_IO_PU_PD(202), R8A73A4_PIN_IO_PU_PD(203),
+	R8A73A4_PIN_IO_PU_PD(204), R8A73A4_PIN_IO_PU_PD(205),
+	R8A73A4_PIN_IO_PU_PD(206), R8A73A4_PIN_IO_PU_PD(207),
+	R8A73A4_PIN_IO_PU_PD(208), R8A73A4_PIN_IO_PU_PD(209),
+	R8A73A4_PIN_IO_PU_PD(210), R8A73A4_PIN_IO_PU_PD(211),
+	R8A73A4_PIN_IO_PU_PD(212), R8A73A4_PIN_IO_PU_PD(213),
+	R8A73A4_PIN_IO_PU_PD(214), R8A73A4_PIN_IO_PU_PD(215),
+	R8A73A4_PIN_IO_PU_PD(216), R8A73A4_PIN_IO_PU_PD(217),
+	R8A73A4_PIN_IO_PU_PD(218), R8A73A4_PIN_IO_PU_PD(219),
+	R8A73A4_PIN_IO_PU_PD(220), R8A73A4_PIN_IO_PU_PD(221),
+	R8A73A4_PIN_IO_PU_PD(222),
+	R8A73A4_PIN_IO_PU_PD(224), R8A73A4_PIN_IO_PU_PD(225),
+	R8A73A4_PIN_IO_PU_PD(226), R8A73A4_PIN_IO_PU_PD(227),
+	R8A73A4_PIN_IO_PU_PD(228), R8A73A4_PIN_IO_PU_PD(229),
+	R8A73A4_PIN_IO_PU_PD(230), R8A73A4_PIN_IO_PU_PD(231),
+	R8A73A4_PIN_IO_PU_PD(232), R8A73A4_PIN_IO_PU_PD(233),
+	R8A73A4_PIN_IO_PU_PD(234), R8A73A4_PIN_IO_PU_PD(235),
+	R8A73A4_PIN_IO_PU_PD(236), R8A73A4_PIN_IO_PU_PD(237),
+	R8A73A4_PIN_IO_PU_PD(238), R8A73A4_PIN_IO_PU_PD(239),
+	R8A73A4_PIN_IO_PU_PD(240), R8A73A4_PIN_IO_PU_PD(241),
+	R8A73A4_PIN_IO_PU_PD(242), R8A73A4_PIN_IO_PU_PD(243),
+	R8A73A4_PIN_IO_PU_PD(244), R8A73A4_PIN_IO_PU_PD(245),
+	R8A73A4_PIN_IO_PU_PD(246), R8A73A4_PIN_IO_PU_PD(247),
+	R8A73A4_PIN_IO_PU_PD(248), R8A73A4_PIN_IO_PU_PD(249),
+	R8A73A4_PIN_IO_PU_PD(250),
+	R8A73A4_PIN_IO_PU_PD(256), R8A73A4_PIN_IO_PU_PD(257),
+	R8A73A4_PIN_IO_PU_PD(258), R8A73A4_PIN_IO_PU_PD(259),
+	R8A73A4_PIN_IO_PU_PD(260), R8A73A4_PIN_IO_PU_PD(261),
+	R8A73A4_PIN_IO_PU_PD(262), R8A73A4_PIN_IO_PU_PD(263),
+	R8A73A4_PIN_IO_PU_PD(264), R8A73A4_PIN_IO_PU_PD(265),
+	R8A73A4_PIN_IO_PU_PD(266), R8A73A4_PIN_IO_PU_PD(267),
+	R8A73A4_PIN_IO_PU_PD(268), R8A73A4_PIN_IO_PU_PD(269),
+	R8A73A4_PIN_IO_PU_PD(270), R8A73A4_PIN_IO_PU_PD(271),
+	R8A73A4_PIN_IO_PU_PD(272), R8A73A4_PIN_IO_PU_PD(273),
+	R8A73A4_PIN_IO_PU_PD(274), R8A73A4_PIN_IO_PU_PD(275),
+	R8A73A4_PIN_IO_PU_PD(276), R8A73A4_PIN_IO_PU_PD(277),
+	R8A73A4_PIN_IO_PU_PD(278), R8A73A4_PIN_IO_PU_PD(279),
+	R8A73A4_PIN_IO_PU_PD(280), R8A73A4_PIN_IO_PU_PD(281),
+	R8A73A4_PIN_IO_PU_PD(282), R8A73A4_PIN_IO_PU_PD(283),
+	R8A73A4_PIN_O(288), R8A73A4_PIN_IO_PU_PD(289),
+	R8A73A4_PIN_IO_PU_PD(290), R8A73A4_PIN_IO_PU_PD(291),
+	R8A73A4_PIN_IO_PU_PD(292), R8A73A4_PIN_IO_PU_PD(293),
+	R8A73A4_PIN_IO_PU_PD(294), R8A73A4_PIN_IO_PU_PD(295),
+	R8A73A4_PIN_IO_PU_PD(296), R8A73A4_PIN_IO_PU_PD(297),
+	R8A73A4_PIN_IO_PU_PD(298), R8A73A4_PIN_IO_PU_PD(299),
+	R8A73A4_PIN_IO_PU_PD(300), R8A73A4_PIN_IO_PU_PD(301),
+	R8A73A4_PIN_IO_PU_PD(302), R8A73A4_PIN_IO_PU_PD(303),
+	R8A73A4_PIN_IO_PU_PD(304), R8A73A4_PIN_IO_PU_PD(305),
+	R8A73A4_PIN_IO_PU_PD(306), R8A73A4_PIN_IO_PU_PD(307),
+	R8A73A4_PIN_IO_PU_PD(308),
+	R8A73A4_PIN_IO_PU_PD(320), R8A73A4_PIN_IO_PU_PD(321),
+	R8A73A4_PIN_IO_PU_PD(322), R8A73A4_PIN_IO_PU_PD(323),
+	R8A73A4_PIN_IO_PU_PD(324), R8A73A4_PIN_IO_PU_PD(325),
+	R8A73A4_PIN_IO_PU_PD(326), R8A73A4_PIN_IO_PU_PD(327),
+	R8A73A4_PIN_IO_PU_PD(328), R8A73A4_PIN_IO_PU_PD(329),
+};
+
+static const struct pinmux_range pinmux_ranges[] = {
+	{.begin = 0, .end = 30,},
+	{.begin = 32, .end = 40,},
+	{.begin = 64, .end = 85,},
+	{.begin = 96, .end = 126,},
+	{.begin = 128, .end = 134,},
+	{.begin = 160, .end = 178,},
+	{.begin = 192, .end = 222,},
+	{.begin = 224, .end = 250,},
+	{.begin = 256, .end = 283,},
+	{.begin = 288, .end = 308,},
+	{.begin = 320, .end = 329,},
+};
+
+/* - IRQC ------------------------------------------------------------------- */
+#define IRQC_PINS_MUX(pin, irq_mark)				\
+static const unsigned int irqc_irq##irq_mark##_pins[] = {	\
+	pin,							\
+};								\
+static const unsigned int irqc_irq##irq_mark##_mux[] = {	\
+	IRQ##irq_mark##_MARK,					\
+}
+IRQC_PINS_MUX(0, 0);
+IRQC_PINS_MUX(1, 1);
+IRQC_PINS_MUX(2, 2);
+IRQC_PINS_MUX(3, 3);
+IRQC_PINS_MUX(4, 4);
+IRQC_PINS_MUX(5, 5);
+IRQC_PINS_MUX(6, 6);
+IRQC_PINS_MUX(7, 7);
+IRQC_PINS_MUX(8, 8);
+IRQC_PINS_MUX(9, 9);
+IRQC_PINS_MUX(10, 10);
+IRQC_PINS_MUX(11, 11);
+IRQC_PINS_MUX(12, 12);
+IRQC_PINS_MUX(13, 13);
+IRQC_PINS_MUX(14, 14);
+IRQC_PINS_MUX(15, 15);
+IRQC_PINS_MUX(66, 40);
+IRQC_PINS_MUX(84, 19);
+IRQC_PINS_MUX(85, 18);
+IRQC_PINS_MUX(102, 41);
+IRQC_PINS_MUX(103, 42);
+IRQC_PINS_MUX(109, 43);
+IRQC_PINS_MUX(110, 44);
+IRQC_PINS_MUX(111, 45);
+IRQC_PINS_MUX(112, 46);
+IRQC_PINS_MUX(113, 47);
+IRQC_PINS_MUX(114, 48);
+IRQC_PINS_MUX(115, 49);
+IRQC_PINS_MUX(160, 20);
+IRQC_PINS_MUX(161, 21);
+IRQC_PINS_MUX(162, 22);
+IRQC_PINS_MUX(163, 23);
+IRQC_PINS_MUX(175, 24);
+IRQC_PINS_MUX(176, 25);
+IRQC_PINS_MUX(177, 26);
+IRQC_PINS_MUX(178, 27);
+IRQC_PINS_MUX(192, 31);
+IRQC_PINS_MUX(193, 32);
+IRQC_PINS_MUX(194, 33);
+IRQC_PINS_MUX(195, 34);
+IRQC_PINS_MUX(196, 35);
+IRQC_PINS_MUX(197, 36);
+IRQC_PINS_MUX(198, 37);
+IRQC_PINS_MUX(199, 38);
+IRQC_PINS_MUX(200, 39);
+IRQC_PINS_MUX(290, 51);
+IRQC_PINS_MUX(296, 52);
+IRQC_PINS_MUX(301, 50);
+IRQC_PINS_MUX(320, 16);
+IRQC_PINS_MUX(321, 17);
+IRQC_PINS_MUX(322, 28);
+IRQC_PINS_MUX(323, 29);
+IRQC_PINS_MUX(324, 30);
+IRQC_PINS_MUX(325, 53);
+IRQC_PINS_MUX(326, 54);
+IRQC_PINS_MUX(327, 55);
+IRQC_PINS_MUX(328, 56);
+IRQC_PINS_MUX(329, 57);
+/* - SCIFA0 ----------------------------------------------------------------- */
+static const unsigned int scifa0_data_pins[] = {
+	/* SCIFA0_RXD, SCIFA0_TXD */
+	117, 116,
+};
+static const unsigned int scifa0_data_mux[] = {
+	SCIFA0_RXD_MARK, SCIFA0_TXD_MARK,
+};
+static const unsigned int scifa0_clk_pins[] = {
+	/* SCIFA0_SCK */
+	34,
+};
+static const unsigned int scifa0_clk_mux[] = {
+	SCIFA0_SCK_MARK,
+};
+static const unsigned int scifa0_ctrl_pins[] = {
+	/* SCIFA0_RTS, SCIFA0_CTS */
+	32, 33,
+};
+static const unsigned int scifa0_ctrl_mux[] = {
+	SCIFA0_RTS_MARK, SCIFA0_CTS_MARK,
+};
+/* - SCIFA1 ----------------------------------------------------------------- */
+static const unsigned int scifa1_data_pins[] = {
+	/* SCIFA1_RXD, SCIFA1_TXD */
+	119, 118,
+};
+static const unsigned int scifa1_data_mux[] = {
+	SCIFA1_RXD_MARK, SCIFA1_TXD_MARK,
+};
+static const unsigned int scifa1_clk_pins[] = {
+	/* SCIFA1_SCK */
+	37,
+};
+static const unsigned int scifa1_clk_mux[] = {
+	SCIFA1_SCK_MARK,
+};
+static const unsigned int scifa1_ctrl_pins[] = {
+	/* SCIFA1_RTS, SCIFA1_CTS */
+	35, 36,
+};
+static const unsigned int scifa1_ctrl_mux[] = {
+	SCIFA1_RTS_MARK, SCIFA1_CTS_MARK,
+};
+/* - SCIFB0 ----------------------------------------------------------------- */
+static const unsigned int scifb0_data_pins[] = {
+	/* SCIFB0_RXD, SCIFB0_TXD */
+	123, 122,
+};
+static const unsigned int scifb0_data_mux[] = {
+	SCIFB0_RXD_MARK, SCIFB0_TXD_MARK,
+};
+static const unsigned int scifb0_clk_pins[] = {
+	/* SCIFB0_SCK */
+	40,
+};
+static const unsigned int scifb0_clk_mux[] = {
+	SCIFB0_SCK_MARK,
+};
+static const unsigned int scifb0_ctrl_pins[] = {
+	/* SCIFB0_RTS, SCIFB0_CTS */
+	38, 39,
+};
+static const unsigned int scifb0_ctrl_mux[] = {
+	SCIFB0_RTS_MARK, SCIFB0_CTS_MARK,
+};
+/* - SCIFB1 ----------------------------------------------------------------- */
+static const unsigned int scifb1_data_pins[] = {
+	/* SCIFB1_RXD, SCIFB1_TXD */
+	27, 26,
+};
+static const unsigned int scifb1_data_mux[] = {
+	SCIFB1_RXD_27_MARK, SCIFB1_TXD_26_MARK,
+};
+static const unsigned int scifb1_clk_pins[] = {
+	/* SCIFB1_SCK */
+	28,
+};
+static const unsigned int scifb1_clk_mux[] = {
+	SCIFB1_SCK_28_MARK,
+};
+static const unsigned int scifb1_ctrl_pins[] = {
+	/* SCIFB1_RTS, SCIFB1_CTS */
+	24, 25,
+};
+static const unsigned int scifb1_ctrl_mux[] = {
+	SCIFB1_RTS_24_MARK, SCIFB1_CTS_25_MARK,
+};
+static const unsigned int scifb1_data_b_pins[] = {
+	/* SCIFB1_RXD, SCIFB1_TXD */
+	72, 67,
+};
+static const unsigned int scifb1_data_b_mux[] = {
+	SCIFB1_RXD_72_MARK, SCIFB1_TXD_67_MARK,
+};
+static const unsigned int scifb1_clk_b_pins[] = {
+	/* SCIFB1_SCK */
+	261,
+};
+static const unsigned int scifb1_clk_b_mux[] = {
+	SCIFB1_SCK_261_MARK,
+};
+static const unsigned int scifb1_ctrl_b_pins[] = {
+	/* SCIFB1_RTS, SCIFB1_CTS */
+	70, 71,
+};
+static const unsigned int scifb1_ctrl_b_mux[] = {
+	SCIFB1_RTS_70_MARK, SCIFB1_CTS_71_MARK,
+};
+/* - SCIFB2 ----------------------------------------------------------------- */
+static const unsigned int scifb2_data_pins[] = {
+	/* SCIFB2_RXD, SCIFB2_TXD */
+	69, 68,
+};
+static const unsigned int scifb2_data_mux[] = {
+	SCIFB2_RXD_69_MARK, SCIFB2_TXD_68_MARK,
+};
+static const unsigned int scifb2_clk_pins[] = {
+	/* SCIFB2_SCK */
+	262,
+};
+static const unsigned int scifb2_clk_mux[] = {
+	SCIFB2_SCK_262_MARK,
+};
+static const unsigned int scifb2_ctrl_pins[] = {
+	/* SCIFB2_RTS, SCIFB2_CTS */
+	73, 66,
+};
+static const unsigned int scifb2_ctrl_mux[] = {
+	SCIFB2_RTS_73_MARK, SCIFB2_CTS_66_MARK,
+};
+static const unsigned int scifb2_data_b_pins[] = {
+	/* SCIFB2_RXD, SCIFB2_TXD */
+	297, 295,
+};
+static const unsigned int scifb2_data_b_mux[] = {
+	SCIFB2_RXD_297_MARK, SCIFB2_TXD_295_MARK,
+};
+static const unsigned int scifb2_clk_b_pins[] = {
+	/* SCIFB2_SCK */
+	299,
+};
+static const unsigned int scifb2_clk_b_mux[] = {
+	SCIFB2_SCK_299_MARK,
+};
+static const unsigned int scifb2_ctrl_b_pins[] = {
+	/* SCIFB2_RTS, SCIFB2_CTS */
+	300, 298,
+};
+static const unsigned int scifb2_ctrl_b_mux[] = {
+	SCIFB2_RTS_300_MARK, SCIFB2_CTS_298_MARK,
+};
+/* - SCIFB3 ----------------------------------------------------------------- */
+static const unsigned int scifb3_data_pins[] = {
+	/* SCIFB3_RXD, SCIFB3_TXD */
+	22, 21,
+};
+static const unsigned int scifb3_data_mux[] = {
+	SCIFB3_RXD_22_MARK, SCIFB3_TXD_21_MARK,
+};
+static const unsigned int scifb3_clk_pins[] = {
+	/* SCIFB3_SCK */
+	23,
+};
+static const unsigned int scifb3_clk_mux[] = {
+	SCIFB3_SCK_23_MARK,
+};
+static const unsigned int scifb3_ctrl_pins[] = {
+	/* SCIFB3_RTS, SCIFB3_CTS */
+	19, 20,
+};
+static const unsigned int scifb3_ctrl_mux[] = {
+	SCIFB3_RTS_19_MARK, SCIFB3_CTS_20_MARK,
+};
+static const unsigned int scifb3_data_b_pins[] = {
+	/* SCIFB3_RXD, SCIFB3_TXD */
+	120, 121,
+};
+static const unsigned int scifb3_data_b_mux[] = {
+	SCIFB3_RXD_120_MARK, SCIFB3_TXD_121_MARK,
+};
+static const unsigned int scifb3_clk_b_pins[] = {
+	/* SCIFB3_SCK */
+	40,
+};
+static const unsigned int scifb3_clk_b_mux[] = {
+	SCIFB3_SCK_40_MARK,
+};
+static const unsigned int scifb3_ctrl_b_pins[] = {
+	/* SCIFB3_RTS, SCIFB3_CTS */
+	38, 39,
+};
+static const unsigned int scifb3_ctrl_b_mux[] = {
+	SCIFB3_RTS_38_MARK, SCIFB3_CTS_39_MARK,
+};
+
+static const struct sh_pfc_pin_group pinmux_groups[] = {
+	SH_PFC_PIN_GROUP(irqc_irq0),
+	SH_PFC_PIN_GROUP(irqc_irq1),
+	SH_PFC_PIN_GROUP(irqc_irq2),
+	SH_PFC_PIN_GROUP(irqc_irq3),
+	SH_PFC_PIN_GROUP(irqc_irq4),
+	SH_PFC_PIN_GROUP(irqc_irq5),
+	SH_PFC_PIN_GROUP(irqc_irq6),
+	SH_PFC_PIN_GROUP(irqc_irq7),
+	SH_PFC_PIN_GROUP(irqc_irq8),
+	SH_PFC_PIN_GROUP(irqc_irq9),
+	SH_PFC_PIN_GROUP(irqc_irq10),
+	SH_PFC_PIN_GROUP(irqc_irq11),
+	SH_PFC_PIN_GROUP(irqc_irq12),
+	SH_PFC_PIN_GROUP(irqc_irq13),
+	SH_PFC_PIN_GROUP(irqc_irq14),
+	SH_PFC_PIN_GROUP(irqc_irq15),
+	SH_PFC_PIN_GROUP(irqc_irq16),
+	SH_PFC_PIN_GROUP(irqc_irq17),
+	SH_PFC_PIN_GROUP(irqc_irq18),
+	SH_PFC_PIN_GROUP(irqc_irq19),
+	SH_PFC_PIN_GROUP(irqc_irq20),
+	SH_PFC_PIN_GROUP(irqc_irq21),
+	SH_PFC_PIN_GROUP(irqc_irq22),
+	SH_PFC_PIN_GROUP(irqc_irq23),
+	SH_PFC_PIN_GROUP(irqc_irq24),
+	SH_PFC_PIN_GROUP(irqc_irq25),
+	SH_PFC_PIN_GROUP(irqc_irq26),
+	SH_PFC_PIN_GROUP(irqc_irq27),
+	SH_PFC_PIN_GROUP(irqc_irq28),
+	SH_PFC_PIN_GROUP(irqc_irq29),
+	SH_PFC_PIN_GROUP(irqc_irq30),
+	SH_PFC_PIN_GROUP(irqc_irq31),
+	SH_PFC_PIN_GROUP(irqc_irq32),
+	SH_PFC_PIN_GROUP(irqc_irq33),
+	SH_PFC_PIN_GROUP(irqc_irq34),
+	SH_PFC_PIN_GROUP(irqc_irq35),
+	SH_PFC_PIN_GROUP(irqc_irq36),
+	SH_PFC_PIN_GROUP(irqc_irq37),
+	SH_PFC_PIN_GROUP(irqc_irq38),
+	SH_PFC_PIN_GROUP(irqc_irq39),
+	SH_PFC_PIN_GROUP(irqc_irq40),
+	SH_PFC_PIN_GROUP(irqc_irq41),
+	SH_PFC_PIN_GROUP(irqc_irq42),
+	SH_PFC_PIN_GROUP(irqc_irq43),
+	SH_PFC_PIN_GROUP(irqc_irq44),
+	SH_PFC_PIN_GROUP(irqc_irq45),
+	SH_PFC_PIN_GROUP(irqc_irq46),
+	SH_PFC_PIN_GROUP(irqc_irq47),
+	SH_PFC_PIN_GROUP(irqc_irq48),
+	SH_PFC_PIN_GROUP(irqc_irq49),
+	SH_PFC_PIN_GROUP(irqc_irq50),
+	SH_PFC_PIN_GROUP(irqc_irq51),
+	SH_PFC_PIN_GROUP(irqc_irq52),
+	SH_PFC_PIN_GROUP(irqc_irq53),
+	SH_PFC_PIN_GROUP(irqc_irq54),
+	SH_PFC_PIN_GROUP(irqc_irq55),
+	SH_PFC_PIN_GROUP(irqc_irq56),
+	SH_PFC_PIN_GROUP(irqc_irq57),
+	SH_PFC_PIN_GROUP(scifa0_data),
+	SH_PFC_PIN_GROUP(scifa0_clk),
+	SH_PFC_PIN_GROUP(scifa0_ctrl),
+	SH_PFC_PIN_GROUP(scifa1_data),
+	SH_PFC_PIN_GROUP(scifa1_clk),
+	SH_PFC_PIN_GROUP(scifa1_ctrl),
+	SH_PFC_PIN_GROUP(scifb0_data),
+	SH_PFC_PIN_GROUP(scifb0_clk),
+	SH_PFC_PIN_GROUP(scifb0_ctrl),
+	SH_PFC_PIN_GROUP(scifb1_data),
+	SH_PFC_PIN_GROUP(scifb1_clk),
+	SH_PFC_PIN_GROUP(scifb1_ctrl),
+	SH_PFC_PIN_GROUP(scifb1_data_b),
+	SH_PFC_PIN_GROUP(scifb1_clk_b),
+	SH_PFC_PIN_GROUP(scifb1_ctrl_b),
+	SH_PFC_PIN_GROUP(scifb2_data),
+	SH_PFC_PIN_GROUP(scifb2_clk),
+	SH_PFC_PIN_GROUP(scifb2_ctrl),
+	SH_PFC_PIN_GROUP(scifb2_data_b),
+	SH_PFC_PIN_GROUP(scifb2_clk_b),
+	SH_PFC_PIN_GROUP(scifb2_ctrl_b),
+	SH_PFC_PIN_GROUP(scifb3_data),
+	SH_PFC_PIN_GROUP(scifb3_clk),
+	SH_PFC_PIN_GROUP(scifb3_ctrl),
+	SH_PFC_PIN_GROUP(scifb3_data_b),
+	SH_PFC_PIN_GROUP(scifb3_clk_b),
+	SH_PFC_PIN_GROUP(scifb3_ctrl_b),
+};
+
+static const char * const irqc_groups[] = {
+	"irqc_irq0",
+	"irqc_irq1",
+	"irqc_irq2",
+	"irqc_irq3",
+	"irqc_irq4",
+	"irqc_irq5",
+	"irqc_irq6",
+	"irqc_irq7",
+	"irqc_irq8",
+	"irqc_irq9",
+	"irqc_irq10",
+	"irqc_irq11",
+	"irqc_irq12",
+	"irqc_irq13",
+	"irqc_irq14",
+	"irqc_irq15",
+	"irqc_irq16",
+	"irqc_irq17",
+	"irqc_irq18",
+	"irqc_irq19",
+	"irqc_irq20",
+	"irqc_irq21",
+	"irqc_irq22",
+	"irqc_irq23",
+	"irqc_irq24",
+	"irqc_irq25",
+	"irqc_irq26",
+	"irqc_irq27",
+	"irqc_irq28",
+	"irqc_irq29",
+	"irqc_irq30",
+	"irqc_irq31",
+	"irqc_irq32",
+	"irqc_irq33",
+	"irqc_irq34",
+	"irqc_irq35",
+	"irqc_irq36",
+	"irqc_irq37",
+	"irqc_irq38",
+	"irqc_irq39",
+	"irqc_irq40",
+	"irqc_irq41",
+	"irqc_irq42",
+	"irqc_irq43",
+	"irqc_irq44",
+	"irqc_irq45",
+	"irqc_irq46",
+	"irqc_irq47",
+	"irqc_irq48",
+	"irqc_irq49",
+	"irqc_irq50",
+	"irqc_irq51",
+	"irqc_irq52",
+	"irqc_irq53",
+	"irqc_irq54",
+	"irqc_irq55",
+	"irqc_irq56",
+	"irqc_irq57",
+};
+
+static const char * const scifa0_groups[] = {
+	"scifa0_data",
+	"scifa0_clk",
+	"scifa0_ctrl",
+};
+
+static const char * const scifa1_groups[] = {
+	"scifa1_data",
+	"scifa1_clk",
+	"scifa1_ctrl",
+};
+
+static const char * const scifb0_groups[] = {
+	"scifb0_data",
+	"scifb0_clk",
+	"scifb0_ctrl",
+};
+
+static const char * const scifb1_groups[] = {
+	"scifb1_data",
+	"scifb1_clk",
+	"scifb1_ctrl",
+	"scifb1_data_b",
+	"scifb1_clk_b",
+	"scifb1_ctrl_b",
+};
+
+static const char * const scifb2_groups[] = {
+	"scifb2_data",
+	"scifb2_clk",
+	"scifb2_ctrl",
+	"scifb2_data_b",
+	"scifb2_clk_b",
+	"scifb2_ctrl_b",
+};
+
+static const char * const scifb3_groups[] = {
+	"scifb3_data",
+	"scifb3_clk",
+	"scifb3_ctrl",
+	"scifb3_data_b",
+	"scifb3_clk_b",
+	"scifb3_ctrl_b",
+};
+
+static const struct sh_pfc_function pinmux_functions[] = {
+	SH_PFC_FUNCTION(irqc),
+	SH_PFC_FUNCTION(scifa0),
+	SH_PFC_FUNCTION(scifa1),
+	SH_PFC_FUNCTION(scifb0),
+	SH_PFC_FUNCTION(scifb1),
+	SH_PFC_FUNCTION(scifb2),
+	SH_PFC_FUNCTION(scifb3),
+};
+
+#undef PORTCR
+#define PORTCR(nr, reg)							\
+	{								\
+		PINMUX_CFG_REG("PORT" nr "CR", reg, 8, 4) {		\
+			_PCRH(PORT##nr##_IN, 0, 0, PORT##nr##_OUT),	\
+				PORT##nr##_FN0, PORT##nr##_FN1,		\
+				PORT##nr##_FN2, PORT##nr##_FN3,		\
+				PORT##nr##_FN4, PORT##nr##_FN5,		\
+				PORT##nr##_FN6, PORT##nr##_FN7 }	\
+	}
+
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
+	PORTCR(0, 0xe6050000),
+	PORTCR(1, 0xe6050001),
+	PORTCR(2, 0xe6050002),
+	PORTCR(3, 0xe6050003),
+	PORTCR(4, 0xe6050004),
+	PORTCR(5, 0xe6050005),
+	PORTCR(6, 0xe6050006),
+	PORTCR(7, 0xe6050007),
+	PORTCR(8, 0xe6050008),
+	PORTCR(9, 0xe6050009),
+	PORTCR(10, 0xe605000A),
+	PORTCR(11, 0xe605000B),
+	PORTCR(12, 0xe605000C),
+	PORTCR(13, 0xe605000D),
+	PORTCR(14, 0xe605000E),
+	PORTCR(15, 0xe605000F),
+	PORTCR(16, 0xe6050010),
+	PORTCR(17, 0xe6050011),
+	PORTCR(18, 0xe6050012),
+	PORTCR(19, 0xe6050013),
+	PORTCR(20, 0xe6050014),
+	PORTCR(21, 0xe6050015),
+	PORTCR(22, 0xe6050016),
+	PORTCR(23, 0xe6050017),
+	PORTCR(24, 0xe6050018),
+	PORTCR(25, 0xe6050019),
+	PORTCR(26, 0xe605001A),
+	PORTCR(27, 0xe605001B),
+	PORTCR(28, 0xe605001C),
+	PORTCR(29, 0xe605001D),
+	PORTCR(30, 0xe605001E),
+	PORTCR(32, 0xe6051020),
+	PORTCR(33, 0xe6051021),
+	PORTCR(34, 0xe6051022),
+	PORTCR(35, 0xe6051023),
+	PORTCR(36, 0xe6051024),
+	PORTCR(37, 0xe6051025),
+	PORTCR(38, 0xe6051026),
+	PORTCR(39, 0xe6051027),
+	PORTCR(40, 0xe6051028),
+	PORTCR(64, 0xe6050040),
+	PORTCR(65, 0xe6050041),
+	PORTCR(66, 0xe6050042),
+	PORTCR(67, 0xe6050043),
+	PORTCR(68, 0xe6050044),
+	PORTCR(69, 0xe6050045),
+	PORTCR(70, 0xe6050046),
+	PORTCR(71, 0xe6050047),
+	PORTCR(72, 0xe6050048),
+	PORTCR(73, 0xe6050049),
+	PORTCR(74, 0xe605004A),
+	PORTCR(75, 0xe605004B),
+	PORTCR(76, 0xe605004C),
+	PORTCR(77, 0xe605004D),
+	PORTCR(78, 0xe605004E),
+	PORTCR(79, 0xe605004F),
+	PORTCR(80, 0xe6050050),
+	PORTCR(81, 0xe6050051),
+	PORTCR(82, 0xe6050052),
+	PORTCR(83, 0xe6050053),
+	PORTCR(84, 0xe6050054),
+	PORTCR(85, 0xe6050055),
+	PORTCR(96, 0xe6051060),
+	PORTCR(97, 0xe6051061),
+	PORTCR(98, 0xe6051062),
+	PORTCR(99, 0xe6051063),
+	PORTCR(100, 0xe6051064),
+	PORTCR(101, 0xe6051065),
+	PORTCR(102, 0xe6051066),
+	PORTCR(103, 0xe6051067),
+	PORTCR(104, 0xe6051068),
+	PORTCR(105, 0xe6051069),
+	PORTCR(106, 0xe605106A),
+	PORTCR(107, 0xe605106B),
+	PORTCR(108, 0xe605106C),
+	PORTCR(109, 0xe605106D),
+	PORTCR(110, 0xe605106E),
+	PORTCR(111, 0xe605106F),
+	PORTCR(112, 0xe6051070),
+	PORTCR(113, 0xe6051071),
+	PORTCR(114, 0xe6051072),
+	PORTCR(115, 0xe6051073),
+	PORTCR(116, 0xe6051074),
+	PORTCR(117, 0xe6051075),
+	PORTCR(118, 0xe6051076),
+	PORTCR(119, 0xe6051077),
+	PORTCR(120, 0xe6051078),
+	PORTCR(121, 0xe6051079),
+	PORTCR(122, 0xe605107A),
+	PORTCR(123, 0xe605107B),
+	PORTCR(124, 0xe605107C),
+	PORTCR(125, 0xe605107D),
+	PORTCR(126, 0xe605107E),
+	PORTCR(128, 0xe6051080),
+	PORTCR(129, 0xe6051081),
+	PORTCR(130, 0xe6051082),
+	PORTCR(131, 0xe6051083),
+	PORTCR(132, 0xe6051084),
+	PORTCR(133, 0xe6051085),
+	PORTCR(134, 0xe6051086),
+	PORTCR(160, 0xe60520A0),
+	PORTCR(161, 0xe60520A1),
+	PORTCR(162, 0xe60520A2),
+	PORTCR(163, 0xe60520A3),
+	PORTCR(164, 0xe60520A4),
+	PORTCR(165, 0xe60520A5),
+	PORTCR(166, 0xe60520A6),
+	PORTCR(167, 0xe60520A7),
+	PORTCR(168, 0xe60520A8),
+	PORTCR(169, 0xe60520A9),
+	PORTCR(170, 0xe60520AA),
+	PORTCR(171, 0xe60520AB),
+	PORTCR(172, 0xe60520AC),
+	PORTCR(173, 0xe60520AD),
+	PORTCR(174, 0xe60520AE),
+	PORTCR(175, 0xe60520AF),
+	PORTCR(176, 0xe60520B0),
+	PORTCR(177, 0xe60520B1),
+	PORTCR(178, 0xe60520B2),
+	PORTCR(192, 0xe60520C0),
+	PORTCR(193, 0xe60520C1),
+	PORTCR(194, 0xe60520C2),
+	PORTCR(195, 0xe60520C3),
+	PORTCR(196, 0xe60520C4),
+	PORTCR(197, 0xe60520C5),
+	PORTCR(198, 0xe60520C6),
+	PORTCR(199, 0xe60520C7),
+	PORTCR(200, 0xe60520C8),
+	PORTCR(201, 0xe60520C9),
+	PORTCR(202, 0xe60520CA),
+	PORTCR(203, 0xe60520CB),
+	PORTCR(204, 0xe60520CC),
+	PORTCR(205, 0xe60520CD),
+	PORTCR(206, 0xe60520CE),
+	PORTCR(207, 0xe60520CF),
+	PORTCR(208, 0xe60520D0),
+	PORTCR(209, 0xe60520D1),
+	PORTCR(210, 0xe60520D2),
+	PORTCR(211, 0xe60520D3),
+	PORTCR(212, 0xe60520D4),
+	PORTCR(213, 0xe60520D5),
+	PORTCR(214, 0xe60520D6),
+	PORTCR(215, 0xe60520D7),
+	PORTCR(216, 0xe60520D8),
+	PORTCR(217, 0xe60520D9),
+	PORTCR(218, 0xe60520DA),
+	PORTCR(219, 0xe60520DB),
+	PORTCR(220, 0xe60520DC),
+	PORTCR(221, 0xe60520DD),
+	PORTCR(222, 0xe60520DE),
+	PORTCR(224, 0xe60520E0),
+	PORTCR(225, 0xe60520E1),
+	PORTCR(226, 0xe60520E2),
+	PORTCR(227, 0xe60520E3),
+	PORTCR(228, 0xe60520E4),
+	PORTCR(229, 0xe60520E5),
+	PORTCR(230, 0xe60520e6),
+	PORTCR(231, 0xe60520E7),
+	PORTCR(232, 0xe60520E8),
+	PORTCR(233, 0xe60520E9),
+	PORTCR(234, 0xe60520EA),
+	PORTCR(235, 0xe60520EB),
+	PORTCR(236, 0xe60520EC),
+	PORTCR(237, 0xe60520ED),
+	PORTCR(238, 0xe60520EE),
+	PORTCR(239, 0xe60520EF),
+	PORTCR(240, 0xe60520F0),
+	PORTCR(241, 0xe60520F1),
+	PORTCR(242, 0xe60520F2),
+	PORTCR(243, 0xe60520F3),
+	PORTCR(244, 0xe60520F4),
+	PORTCR(245, 0xe60520F5),
+	PORTCR(246, 0xe60520F6),
+	PORTCR(247, 0xe60520F7),
+	PORTCR(248, 0xe60520F8),
+	PORTCR(249, 0xe60520F9),
+	PORTCR(250, 0xe60520FA),
+	PORTCR(256, 0xe6052100),
+	PORTCR(257, 0xe6052101),
+	PORTCR(258, 0xe6052102),
+	PORTCR(259, 0xe6052103),
+	PORTCR(260, 0xe6052104),
+	PORTCR(261, 0xe6052105),
+	PORTCR(262, 0xe6052106),
+	PORTCR(263, 0xe6052107),
+	PORTCR(264, 0xe6052108),
+	PORTCR(265, 0xe6052109),
+	PORTCR(266, 0xe605210A),
+	PORTCR(267, 0xe605210B),
+	PORTCR(268, 0xe605210C),
+	PORTCR(269, 0xe605210D),
+	PORTCR(270, 0xe605210E),
+	PORTCR(271, 0xe605210F),
+	PORTCR(272, 0xe6052110),
+	PORTCR(273, 0xe6052111),
+	PORTCR(274, 0xe6052112),
+	PORTCR(275, 0xe6052113),
+	PORTCR(276, 0xe6052114),
+	PORTCR(277, 0xe6052115),
+	PORTCR(278, 0xe6052116),
+	PORTCR(279, 0xe6052117),
+	PORTCR(280, 0xe6052118),
+	PORTCR(281, 0xe6052119),
+	PORTCR(282, 0xe605211A),
+	PORTCR(283, 0xe605211B),
+	PORTCR(288, 0xe6053120),
+	PORTCR(289, 0xe6053121),
+	PORTCR(290, 0xe6053122),
+	PORTCR(291, 0xe6053123),
+	PORTCR(292, 0xe6053124),
+	PORTCR(293, 0xe6053125),
+	PORTCR(294, 0xe6053126),
+	PORTCR(295, 0xe6053127),
+	PORTCR(296, 0xe6053128),
+	PORTCR(297, 0xe6053129),
+	PORTCR(298, 0xe605312A),
+	PORTCR(299, 0xe605312B),
+	PORTCR(300, 0xe605312C),
+	PORTCR(301, 0xe605312D),
+	PORTCR(302, 0xe605312E),
+	PORTCR(303, 0xe605312F),
+	PORTCR(304, 0xe6053130),
+	PORTCR(305, 0xe6053131),
+	PORTCR(306, 0xe6053132),
+	PORTCR(307, 0xe6053133),
+	PORTCR(308, 0xe6053134),
+	PORTCR(320, 0xe6053140),
+	PORTCR(321, 0xe6053141),
+	PORTCR(322, 0xe6053142),
+	PORTCR(323, 0xe6053143),
+	PORTCR(324, 0xe6053144),
+	PORTCR(325, 0xe6053145),
+	PORTCR(326, 0xe6053146),
+	PORTCR(327, 0xe6053147),
+	PORTCR(328, 0xe6053148),
+	PORTCR(329, 0xe6053149),
+
+	{ PINMUX_CFG_REG("MSEL1CR", 0xe605800c, 32, 1) {
+			MSEL1CR_31_0, MSEL1CR_31_1,
+			0, 0,
+			0, 0,
+			0, 0,
+			MSEL1CR_27_0, MSEL1CR_27_1,
+			0, 0,
+			MSEL1CR_25_0, MSEL1CR_25_1,
+			MSEL1CR_24_0, MSEL1CR_24_1,
+			0, 0,
+			MSEL1CR_22_0, MSEL1CR_22_1,
+			MSEL1CR_21_0, MSEL1CR_21_1,
+			MSEL1CR_20_0, MSEL1CR_20_1,
+			MSEL1CR_19_0, MSEL1CR_19_1,
+			MSEL1CR_18_0, MSEL1CR_18_1,
+			MSEL1CR_17_0, MSEL1CR_17_1,
+			MSEL1CR_16_0, MSEL1CR_16_1,
+			MSEL1CR_15_0, MSEL1CR_15_1,
+			MSEL1CR_14_0, MSEL1CR_14_1,
+			MSEL1CR_13_0, MSEL1CR_13_1,
+			MSEL1CR_12_0, MSEL1CR_12_1,
+			MSEL1CR_11_0, MSEL1CR_11_1,
+			MSEL1CR_10_0, MSEL1CR_10_1,
+			MSEL1CR_09_0, MSEL1CR_09_1,
+			MSEL1CR_08_0, MSEL1CR_08_1,
+			MSEL1CR_07_0, MSEL1CR_07_1,
+			MSEL1CR_06_0, MSEL1CR_06_1,
+			MSEL1CR_05_0, MSEL1CR_05_1,
+			MSEL1CR_04_0, MSEL1CR_04_1,
+			MSEL1CR_03_0, MSEL1CR_03_1,
+			MSEL1CR_02_0, MSEL1CR_02_1,
+			MSEL1CR_01_0, MSEL1CR_01_1,
+			MSEL1CR_00_0, MSEL1CR_00_1,
+		}
+	},
+	{ PINMUX_CFG_REG("MSEL3CR", 0xe6058020, 32, 1) {
+			MSEL3CR_31_0, MSEL3CR_31_1,
+			0, 0,
+			0, 0,
+			MSEL3CR_28_0, MSEL3CR_28_1,
+			MSEL3CR_27_0, MSEL3CR_27_1,
+			MSEL3CR_26_0, MSEL3CR_26_1,
+			0, 0,
+			0, 0,
+			MSEL3CR_23_0, MSEL3CR_23_1,
+			MSEL3CR_22_0, MSEL3CR_22_1,
+			MSEL3CR_21_0, MSEL3CR_21_1,
+			MSEL3CR_20_0, MSEL3CR_20_1,
+			MSEL3CR_19_0, MSEL3CR_19_1,
+			MSEL3CR_18_0, MSEL3CR_18_1,
+			MSEL3CR_17_0, MSEL3CR_17_1,
+			MSEL3CR_16_0, MSEL3CR_16_1,
+			MSEL3CR_15_0, MSEL3CR_15_1,
+			0, 0,
+			0, 0,
+			MSEL3CR_12_0, MSEL3CR_12_1,
+			MSEL3CR_11_0, MSEL3CR_11_1,
+			MSEL3CR_10_0, MSEL3CR_10_1,
+			MSEL3CR_09_0, MSEL3CR_09_1,
+			0, 0,
+			0, 0,
+			MSEL3CR_06_0, MSEL3CR_06_1,
+			0, 0,
+			0, 0,
+			MSEL3CR_03_0, MSEL3CR_03_1,
+			0, 0,
+			MSEL3CR_01_0, MSEL3CR_01_1,
+			MSEL3CR_00_0, MSEL3CR_00_1,
+			}
+	},
+	{ PINMUX_CFG_REG("MSEL4CR", 0xe6058024, 32, 1) {
+			0, 0,
+			MSEL4CR_30_0, MSEL4CR_30_1,
+			MSEL4CR_29_0, MSEL4CR_29_1,
+			MSEL4CR_28_0, MSEL4CR_28_1,
+			MSEL4CR_27_0, MSEL4CR_27_1,
+			MSEL4CR_26_0, MSEL4CR_26_1,
+			MSEL4CR_25_0, MSEL4CR_25_1,
+			MSEL4CR_24_0, MSEL4CR_24_1,
+			MSEL4CR_23_0, MSEL4CR_23_1,
+			MSEL4CR_22_0, MSEL4CR_22_1,
+			MSEL4CR_21_0, MSEL4CR_21_1,
+			MSEL4CR_20_0, MSEL4CR_20_1,
+			MSEL4CR_19_0, MSEL4CR_19_1,
+			MSEL4CR_18_0, MSEL4CR_18_1,
+			MSEL4CR_17_0, MSEL4CR_17_1,
+			MSEL4CR_16_0, MSEL4CR_16_1,
+			MSEL4CR_15_0, MSEL4CR_15_1,
+			MSEL4CR_14_0, MSEL4CR_14_1,
+			MSEL4CR_13_0, MSEL4CR_13_1,
+			MSEL4CR_12_0, MSEL4CR_12_1,
+			MSEL4CR_11_0, MSEL4CR_11_1,
+			MSEL4CR_10_0, MSEL4CR_10_1,
+			MSEL4CR_09_0, MSEL4CR_09_1,
+			0, 0,
+			MSEL4CR_07_0, MSEL4CR_07_1,
+			0, 0,
+			0, 0,
+			MSEL4CR_04_0, MSEL4CR_04_1,
+			0, 0,
+			0, 0,
+			MSEL4CR_01_0, MSEL4CR_01_1,
+			0, 0,
+		}
+	},
+	{ PINMUX_CFG_REG("MSEL5CR", 0xe6058028, 32, 1) {
+			MSEL5CR_31_0, MSEL5CR_31_1,
+			MSEL5CR_30_0, MSEL5CR_30_1,
+			MSEL5CR_29_0, MSEL5CR_29_1,
+			MSEL5CR_28_0, MSEL5CR_28_1,
+			MSEL5CR_27_0, MSEL5CR_27_1,
+			MSEL5CR_26_0, MSEL5CR_26_1,
+			MSEL5CR_25_0, MSEL5CR_25_1,
+			MSEL5CR_24_0, MSEL5CR_24_1,
+			MSEL5CR_23_0, MSEL5CR_23_1,
+			MSEL5CR_22_0, MSEL5CR_22_1,
+			MSEL5CR_21_0, MSEL5CR_21_1,
+			MSEL5CR_20_0, MSEL5CR_20_1,
+			MSEL5CR_19_0, MSEL5CR_19_1,
+			MSEL5CR_18_0, MSEL5CR_18_1,
+			MSEL5CR_17_0, MSEL5CR_17_1,
+			MSEL5CR_16_0, MSEL5CR_16_1,
+			MSEL5CR_15_0, MSEL5CR_15_1,
+			MSEL5CR_14_0, MSEL5CR_14_1,
+			MSEL5CR_13_0, MSEL5CR_13_1,
+			MSEL5CR_12_0, MSEL5CR_12_1,
+			MSEL5CR_11_0, MSEL5CR_11_1,
+			MSEL5CR_10_0, MSEL5CR_10_1,
+			MSEL5CR_09_0, MSEL5CR_09_1,
+			MSEL5CR_08_0, MSEL5CR_08_1,
+			MSEL5CR_07_0, MSEL5CR_07_1,
+			MSEL5CR_06_0, MSEL5CR_06_1,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+		}
+	},
+	{ PINMUX_CFG_REG("MSEL8CR", 0xe6058034, 32, 1) {
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			MSEL8CR_16_0, MSEL8CR_16_1,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			0, 0,
+			MSEL8CR_01_0, MSEL8CR_01_1,
+			MSEL8CR_00_0, MSEL8CR_00_1,
+		}
+	},
+	{ },
+};
+
+static const struct pinmux_data_reg pinmux_data_regs[] = {
+
+	{ PINMUX_DATA_REG("PORTL031_000DR", 0xe6054000, 32) {
+			0, PORT30_DATA, PORT29_DATA, PORT28_DATA,
+			PORT27_DATA, PORT26_DATA, PORT25_DATA, PORT24_DATA,
+			PORT23_DATA, PORT22_DATA, PORT21_DATA, PORT20_DATA,
+			PORT19_DATA, PORT18_DATA, PORT17_DATA, PORT16_DATA,
+			PORT15_DATA, PORT14_DATA, PORT13_DATA, PORT12_DATA,
+			PORT11_DATA, PORT10_DATA, PORT9_DATA, PORT8_DATA,
+			PORT7_DATA, PORT6_DATA, PORT5_DATA, PORT4_DATA,
+			PORT3_DATA, PORT2_DATA, PORT1_DATA, PORT0_DATA,
+		}
+	},
+	{ PINMUX_DATA_REG("PORTD063_032DR", 0xe6055000, 32) {
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, 0, 0, PORT40_DATA,
+			PORT39_DATA, PORT38_DATA, PORT37_DATA, PORT36_DATA,
+			PORT35_DATA, PORT34_DATA, PORT33_DATA, PORT32_DATA,
+		}
+	},
+	{ PINMUX_DATA_REG("PORTL095_064DR", 0xe6054004, 32) {
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, 0, PORT85_DATA, PORT84_DATA,
+			PORT83_DATA, PORT82_DATA, PORT81_DATA, PORT80_DATA,
+			PORT79_DATA, PORT78_DATA, PORT77_DATA, PORT76_DATA,
+			PORT75_DATA, PORT74_DATA, PORT73_DATA, PORT72_DATA,
+			PORT71_DATA, PORT70_DATA, PORT69_DATA, PORT68_DATA,
+			PORT67_DATA, PORT66_DATA, PORT65_DATA, PORT64_DATA,
+		}
+	},
+	{ PINMUX_DATA_REG("PORTD127_096DR", 0xe6055004, 32) {
+			0, PORT126_DATA, PORT125_DATA, PORT124_DATA,
+			PORT123_DATA, PORT122_DATA, PORT121_DATA, PORT120_DATA,
+			PORT119_DATA, PORT118_DATA, PORT117_DATA, PORT116_DATA,
+			PORT115_DATA, PORT114_DATA, PORT113_DATA, PORT112_DATA,
+			PORT111_DATA, PORT110_DATA, PORT109_DATA, PORT108_DATA,
+			PORT107_DATA, PORT106_DATA, PORT105_DATA, PORT104_DATA,
+			PORT103_DATA, PORT102_DATA, PORT101_DATA, PORT100_DATA,
+			PORT99_DATA, PORT98_DATA, PORT97_DATA, PORT96_DATA,
+		}
+	},
+	{ PINMUX_DATA_REG("PORTD159_128DR", 0xe6055008, 32) {
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, PORT134_DATA, PORT133_DATA, PORT132_DATA,
+			PORT131_DATA, PORT130_DATA, PORT129_DATA, PORT128_DATA,
+		}
+	},
+	{ PINMUX_DATA_REG("PORTR191_160DR", 0xe6056000, 32) {
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, PORT178_DATA, PORT177_DATA, PORT176_DATA,
+			PORT175_DATA, PORT174_DATA, PORT173_DATA, PORT172_DATA,
+			PORT171_DATA, PORT170_DATA, PORT169_DATA, PORT168_DATA,
+			PORT167_DATA, PORT166_DATA, PORT165_DATA, PORT164_DATA,
+			PORT163_DATA, PORT162_DATA, PORT161_DATA, PORT160_DATA,
+		}
+	},
+	{ PINMUX_DATA_REG("PORTR223_192DR", 0xe6056004, 32) {
+			0, PORT222_DATA, PORT221_DATA, PORT220_DATA,
+			PORT219_DATA, PORT218_DATA, PORT217_DATA, PORT216_DATA,
+			PORT215_DATA, PORT214_DATA, PORT213_DATA, PORT212_DATA,
+			PORT211_DATA, PORT210_DATA, PORT209_DATA, PORT208_DATA,
+			PORT207_DATA, PORT206_DATA, PORT205_DATA, PORT204_DATA,
+			PORT203_DATA, PORT202_DATA, PORT201_DATA, PORT200_DATA,
+			PORT199_DATA, PORT198_DATA, PORT197_DATA, PORT196_DATA,
+			PORT195_DATA, PORT194_DATA, PORT193_DATA, PORT192_DATA,
+		}
+	},
+	{ PINMUX_DATA_REG("PORTR255_224DR", 0xe6056008, 32) {
+			0, 0, 0, 0,
+			0, PORT250_DATA, PORT249_DATA, PORT248_DATA,
+			PORT247_DATA, PORT246_DATA, PORT245_DATA, PORT244_DATA,
+			PORT243_DATA, PORT242_DATA, PORT241_DATA, PORT240_DATA,
+			PORT239_DATA, PORT238_DATA, PORT237_DATA, PORT236_DATA,
+			PORT235_DATA, PORT234_DATA, PORT233_DATA, PORT232_DATA,
+			PORT231_DATA, PORT230_DATA, PORT229_DATA, PORT228_DATA,
+			PORT227_DATA, PORT226_DATA, PORT225_DATA, PORT224_DATA,
+		}
+	},
+	{ PINMUX_DATA_REG("PORTR287_256DR", 0xe605600C, 32) {
+			0, 0, 0, 0,
+			PORT283_DATA, PORT282_DATA, PORT281_DATA, PORT280_DATA,
+			PORT279_DATA, PORT278_DATA, PORT277_DATA, PORT276_DATA,
+			PORT275_DATA, PORT274_DATA, PORT273_DATA, PORT272_DATA,
+			PORT271_DATA, PORT270_DATA, PORT269_DATA, PORT268_DATA,
+			PORT267_DATA, PORT266_DATA, PORT265_DATA, PORT264_DATA,
+			PORT263_DATA, PORT262_DATA, PORT261_DATA, PORT260_DATA,
+			PORT259_DATA, PORT258_DATA, PORT257_DATA, PORT256_DATA,
+		}
+	},
+	{ PINMUX_DATA_REG("PORTU319_288DR", 0xe6057000, 32) {
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, 0, 0, PORT308_DATA,
+			PORT307_DATA, PORT306_DATA, PORT305_DATA, PORT304_DATA,
+			PORT303_DATA, PORT302_DATA, PORT301_DATA, PORT300_DATA,
+			PORT299_DATA, PORT298_DATA, PORT297_DATA, PORT296_DATA,
+			PORT295_DATA, PORT294_DATA, PORT293_DATA, PORT292_DATA,
+			PORT291_DATA, PORT290_DATA, PORT289_DATA, PORT288_DATA,
+		}
+	},
+	{ PINMUX_DATA_REG("PORTU351_320DR", 0xe6057004, 32) {
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, 0, 0, 0,
+			0, 0, PORT329_DATA, PORT328_DATA,
+			PORT327_DATA, PORT326_DATA, PORT325_DATA, PORT324_DATA,
+			PORT323_DATA, PORT322_DATA, PORT321_DATA, PORT320_DATA,
+		}
+	},
+	{ },
+};
+
+static const struct pinmux_irq pinmux_irqs[] = {
+	PINMUX_IRQ(irq_pin(0), 0),
+	PINMUX_IRQ(irq_pin(1), 1),
+	PINMUX_IRQ(irq_pin(2), 2),
+	PINMUX_IRQ(irq_pin(3), 3),
+	PINMUX_IRQ(irq_pin(4), 4),
+	PINMUX_IRQ(irq_pin(5), 5),
+	PINMUX_IRQ(irq_pin(6), 6),
+	PINMUX_IRQ(irq_pin(7), 7),
+	PINMUX_IRQ(irq_pin(8), 8),
+	PINMUX_IRQ(irq_pin(9), 9),
+	PINMUX_IRQ(irq_pin(10), 10),
+	PINMUX_IRQ(irq_pin(11), 11),
+	PINMUX_IRQ(irq_pin(12), 12),
+	PINMUX_IRQ(irq_pin(13), 13),
+	PINMUX_IRQ(irq_pin(14), 14),
+	PINMUX_IRQ(irq_pin(15), 15),
+	PINMUX_IRQ(irq_pin(16), 320),
+	PINMUX_IRQ(irq_pin(17), 321),
+	PINMUX_IRQ(irq_pin(18), 85),
+	PINMUX_IRQ(irq_pin(19), 84),
+	PINMUX_IRQ(irq_pin(20), 160),
+	PINMUX_IRQ(irq_pin(21), 161),
+	PINMUX_IRQ(irq_pin(22), 162),
+	PINMUX_IRQ(irq_pin(23), 163),
+	PINMUX_IRQ(irq_pin(24), 175),
+	PINMUX_IRQ(irq_pin(25), 176),
+	PINMUX_IRQ(irq_pin(26), 177),
+	PINMUX_IRQ(irq_pin(27), 178),
+	PINMUX_IRQ(irq_pin(28), 322),
+	PINMUX_IRQ(irq_pin(29), 323),
+	PINMUX_IRQ(irq_pin(30), 324),
+	PINMUX_IRQ(irq_pin(31), 192),
+	PINMUX_IRQ(irq_pin(32), 193),
+	PINMUX_IRQ(irq_pin(33), 194),
+	PINMUX_IRQ(irq_pin(34), 195),
+	PINMUX_IRQ(irq_pin(35), 196),
+	PINMUX_IRQ(irq_pin(36), 197),
+	PINMUX_IRQ(irq_pin(37), 198),
+	PINMUX_IRQ(irq_pin(38), 199),
+	PINMUX_IRQ(irq_pin(39), 200),
+	PINMUX_IRQ(irq_pin(40), 66),
+	PINMUX_IRQ(irq_pin(41), 102),
+	PINMUX_IRQ(irq_pin(42), 103),
+	PINMUX_IRQ(irq_pin(43), 109),
+	PINMUX_IRQ(irq_pin(44), 110),
+	PINMUX_IRQ(irq_pin(45), 111),
+	PINMUX_IRQ(irq_pin(46), 112),
+	PINMUX_IRQ(irq_pin(47), 113),
+	PINMUX_IRQ(irq_pin(48), 114),
+	PINMUX_IRQ(irq_pin(49), 115),
+	PINMUX_IRQ(irq_pin(50), 301),
+	PINMUX_IRQ(irq_pin(51), 290),
+	PINMUX_IRQ(irq_pin(52), 296),
+	PINMUX_IRQ(irq_pin(53), 325),
+	PINMUX_IRQ(irq_pin(54), 326),
+	PINMUX_IRQ(irq_pin(55), 327),
+	PINMUX_IRQ(irq_pin(56), 328),
+	PINMUX_IRQ(irq_pin(57), 329),
+};
+
+#define PORTCR_PULMD_OFF (0 << 6)
+#define PORTCR_PULMD_DOWN (2 << 6)
+#define PORTCR_PULMD_UP (3 << 6)
+#define PORTCR_PULMD_MASK (3 << 6)
+
+static const unsigned int r8a73a4_portcr_offsets[] = {
+	0x00000000, 0x00001000, 0x00000000, 0x00001000,
+	0x00001000, 0x00002000, 0x00002000, 0x00002000,
+	0x00002000, 0x00003000, 0x00003000,
+};
+
+static unsigned int r8a73a4_pinmux_get_bias(struct sh_pfc *pfc,
+					    unsigned int pin)
+{
+	void __iomem *addr;
+
+	addr = pfc->window->virt + r8a73a4_portcr_offsets[pin >> 5] + pin;
+
+	switch (ioread8(addr) & PORTCR_PULMD_MASK) {
+	case PORTCR_PULMD_UP:
+		return PIN_CONFIG_BIAS_PULL_UP;
+	case PORTCR_PULMD_DOWN:
+		return PIN_CONFIG_BIAS_PULL_DOWN;
+	case PORTCR_PULMD_OFF:
+	default:
+		return PIN_CONFIG_BIAS_DISABLE;
+	}
+}
+
+static void r8a73a4_pinmux_set_bias(struct sh_pfc *pfc, unsigned int pin,
+				   unsigned int bias)
+{
+	void __iomem *addr;
+	u32 value;
+
+	addr = pfc->window->virt + r8a73a4_portcr_offsets[pin >> 5] + pin;
+	value = ioread8(addr) & ~PORTCR_PULMD_MASK;
+
+	switch (bias) {
+	case PIN_CONFIG_BIAS_PULL_UP:
+		value |= PORTCR_PULMD_UP;
+		break;
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		value |= PORTCR_PULMD_DOWN;
+		break;
+	}
+
+	iowrite8(value, addr);
+}
+
+static const struct sh_pfc_soc_operations r8a73a4_pinmux_ops = {
+	.get_bias = r8a73a4_pinmux_get_bias,
+	.set_bias = r8a73a4_pinmux_set_bias,
+};
+
+const struct sh_pfc_soc_info r8a73a4_pinmux_info = {
+	.name		= "r8a73a4_pfc",
+	.ops		= &r8a73a4_pinmux_ops,
+
+	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END },
+	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END },
+	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
+
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+
+	.ranges = pinmux_ranges,
+	.nr_ranges = ARRAY_SIZE(pinmux_ranges),
+
+	.groups = pinmux_groups,
+	.nr_groups = ARRAY_SIZE(pinmux_groups),
+	.functions = pinmux_functions,
+	.nr_functions = ARRAY_SIZE(pinmux_functions),
+
+	.cfg_regs	= pinmux_config_regs,
+	.data_regs	= pinmux_data_regs,
+
+	.gpio_data	= pinmux_data,
+	.gpio_data_size	= ARRAY_SIZE(pinmux_data),
+
+	.gpio_irq = pinmux_irqs,
+	.gpio_irq_size = ARRAY_SIZE(pinmux_irqs),
+};
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7740.c b/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
index 214788c4a606..bbd87d29bfd0 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
@@ -577,7 +577,7 @@ enum {
 	PINMUX_MARK_END,
 };
 
-static pinmux_enum_t pinmux_data[] = {
+static const pinmux_enum_t pinmux_data[] = {
 	/* specify valid pin states for each pin in GPIO mode */
 
 	/* I/O and Pull U/D */
@@ -1654,11 +1654,532 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA(TRACEAUD_FROM_MEMC_MARK,			MSEL5CR_30_1,	MSEL5CR_29_0),
 };
 
-static struct pinmux_gpio pinmux_gpios[] = {
-
-	/* PORT */
+static struct sh_pfc_pin pinmux_pins[] = {
 	GPIO_PORT_ALL(),
+};
+
+/* - LCD0 ------------------------------------------------------------------- */
+static const unsigned int lcd0_data8_pins[] = {
+	/* D[0:7] */
+	58, 57, 56, 55, 54, 53, 52, 51,
+};
+static const unsigned int lcd0_data8_mux[] = {
+	LCD0_D0_MARK, LCD0_D1_MARK, LCD0_D2_MARK, LCD0_D3_MARK,
+	LCD0_D4_MARK, LCD0_D5_MARK, LCD0_D6_MARK, LCD0_D7_MARK,
+};
+static const unsigned int lcd0_data9_pins[] = {
+	/* D[0:8] */
+	58, 57, 56, 55, 54, 53, 52, 51,
+	50,
+};
+static const unsigned int lcd0_data9_mux[] = {
+	LCD0_D0_MARK, LCD0_D1_MARK, LCD0_D2_MARK, LCD0_D3_MARK,
+	LCD0_D4_MARK, LCD0_D5_MARK, LCD0_D6_MARK, LCD0_D7_MARK,
+	LCD0_D8_MARK,
+};
+static const unsigned int lcd0_data12_pins[] = {
+	/* D[0:11] */
+	58, 57, 56, 55, 54, 53, 52, 51,
+	50, 49, 48, 47,
+};
+static const unsigned int lcd0_data12_mux[] = {
+	LCD0_D0_MARK, LCD0_D1_MARK, LCD0_D2_MARK, LCD0_D3_MARK,
+	LCD0_D4_MARK, LCD0_D5_MARK, LCD0_D6_MARK, LCD0_D7_MARK,
+	LCD0_D8_MARK, LCD0_D9_MARK, LCD0_D10_MARK, LCD0_D11_MARK,
+};
+static const unsigned int lcd0_data16_pins[] = {
+	/* D[0:15] */
+	58, 57, 56, 55, 54, 53, 52, 51,
+	50, 49, 48, 47, 46, 45, 44, 43,
+};
+static const unsigned int lcd0_data16_mux[] = {
+	LCD0_D0_MARK, LCD0_D1_MARK, LCD0_D2_MARK, LCD0_D3_MARK,
+	LCD0_D4_MARK, LCD0_D5_MARK, LCD0_D6_MARK, LCD0_D7_MARK,
+	LCD0_D8_MARK, LCD0_D9_MARK, LCD0_D10_MARK, LCD0_D11_MARK,
+	LCD0_D12_MARK, LCD0_D13_MARK, LCD0_D14_MARK, LCD0_D15_MARK,
+};
+static const unsigned int lcd0_data18_pins[] = {
+	/* D[0:17] */
+	58, 57, 56, 55, 54, 53, 52, 51,
+	50, 49, 48, 47, 46, 45, 44, 43,
+	42, 41,
+};
+static const unsigned int lcd0_data18_mux[] = {
+	LCD0_D0_MARK, LCD0_D1_MARK, LCD0_D2_MARK, LCD0_D3_MARK,
+	LCD0_D4_MARK, LCD0_D5_MARK, LCD0_D6_MARK, LCD0_D7_MARK,
+	LCD0_D8_MARK, LCD0_D9_MARK, LCD0_D10_MARK, LCD0_D11_MARK,
+	LCD0_D12_MARK, LCD0_D13_MARK, LCD0_D14_MARK, LCD0_D15_MARK,
+	LCD0_D16_MARK, LCD0_D17_MARK,
+};
+static const unsigned int lcd0_data24_0_pins[] = {
+	/* D[0:23] */
+	58, 57, 56, 55, 54, 53, 52, 51,
+	50, 49, 48, 47, 46, 45, 44, 43,
+	42, 41, 40, 4, 3, 2, 0, 1,
+};
+static const unsigned int lcd0_data24_0_mux[] = {
+	LCD0_D0_MARK, LCD0_D1_MARK, LCD0_D2_MARK, LCD0_D3_MARK,
+	LCD0_D4_MARK, LCD0_D5_MARK, LCD0_D6_MARK, LCD0_D7_MARK,
+	LCD0_D8_MARK, LCD0_D9_MARK, LCD0_D10_MARK, LCD0_D11_MARK,
+	LCD0_D12_MARK, LCD0_D13_MARK, LCD0_D14_MARK, LCD0_D15_MARK,
+	LCD0_D16_MARK, LCD0_D17_MARK, LCD0_D18_PORT40_MARK, LCD0_D19_PORT4_MARK,
+	LCD0_D20_PORT3_MARK, LCD0_D21_PORT2_MARK, LCD0_D22_PORT0_MARK,
+	LCD0_D23_PORT1_MARK,
+};
+static const unsigned int lcd0_data24_1_pins[] = {
+	/* D[0:23] */
+	58, 57, 56, 55, 54, 53, 52, 51,
+	50, 49, 48, 47, 46, 45, 44, 43,
+	42, 41, 163, 162, 161, 158, 160, 159,
+};
+static const unsigned int lcd0_data24_1_mux[] = {
+	LCD0_D0_MARK, LCD0_D1_MARK, LCD0_D2_MARK, LCD0_D3_MARK,
+	LCD0_D4_MARK, LCD0_D5_MARK, LCD0_D6_MARK, LCD0_D7_MARK,
+	LCD0_D8_MARK, LCD0_D9_MARK, LCD0_D10_MARK, LCD0_D11_MARK,
+	LCD0_D16_MARK, LCD0_D17_MARK, LCD0_D18_PORT163_MARK,
+	LCD0_D19_PORT162_MARK, LCD0_D20_PORT161_MARK, LCD0_D21_PORT158_MARK,
+	LCD0_D22_PORT160_MARK, LCD0_D23_PORT159_MARK,
+};
+static const unsigned int lcd0_display_pins[] = {
+	/* DON, VCPWC, VEPWC */
+	61, 59, 60,
+};
+static const unsigned int lcd0_display_mux[] = {
+	LCD0_DON_MARK, LCD0_VCPWC_MARK, LCD0_VEPWC_MARK,
+};
+static const unsigned int lcd0_lclk_0_pins[] = {
+	/* LCLK */
+	102,
+};
+static const unsigned int lcd0_lclk_0_mux[] = {
+	LCD0_LCLK_PORT102_MARK,
+};
+static const unsigned int lcd0_lclk_1_pins[] = {
+	/* LCLK */
+	165,
+};
+static const unsigned int lcd0_lclk_1_mux[] = {
+	LCD0_LCLK_PORT165_MARK,
+};
+static const unsigned int lcd0_sync_pins[] = {
+	/* VSYN, HSYN, DCK, DISP */
+	63, 64, 62, 65,
+};
+static const unsigned int lcd0_sync_mux[] = {
+	LCD0_VSYN_MARK, LCD0_HSYN_MARK, LCD0_DCK_MARK, LCD0_DISP_MARK,
+};
+static const unsigned int lcd0_sys_pins[] = {
+	/* CS, WR, RD, RS */
+	64, 62, 164, 65,
+};
+static const unsigned int lcd0_sys_mux[] = {
+	LCD0_CS_MARK, LCD0_WR_MARK, LCD0_RD_MARK, LCD0_RS_MARK,
+};
+/* - LCD1 ------------------------------------------------------------------- */
+static const unsigned int lcd1_data8_pins[] = {
+	/* D[0:7] */
+	4, 3, 2, 1, 0, 91, 92, 23,
+};
+static const unsigned int lcd1_data8_mux[] = {
+	LCD1_D0_MARK, LCD1_D1_MARK, LCD1_D2_MARK, LCD1_D3_MARK,
+	LCD1_D4_MARK, LCD1_D5_MARK, LCD1_D6_MARK, LCD1_D7_MARK,
+};
+static const unsigned int lcd1_data9_pins[] = {
+	/* D[0:8] */
+	4, 3, 2, 1, 0, 91, 92, 23,
+	93,
+};
+static const unsigned int lcd1_data9_mux[] = {
+	LCD1_D0_MARK, LCD1_D1_MARK, LCD1_D2_MARK, LCD1_D3_MARK,
+	LCD1_D4_MARK, LCD1_D5_MARK, LCD1_D6_MARK, LCD1_D7_MARK,
+	LCD1_D8_MARK,
+};
+static const unsigned int lcd1_data12_pins[] = {
+	/* D[0:12] */
+	4, 3, 2, 1, 0, 91, 92, 23,
+	93, 94, 21, 201,
+};
+static const unsigned int lcd1_data12_mux[] = {
+	LCD1_D0_MARK, LCD1_D1_MARK, LCD1_D2_MARK, LCD1_D3_MARK,
+	LCD1_D4_MARK, LCD1_D5_MARK, LCD1_D6_MARK, LCD1_D7_MARK,
+	LCD1_D8_MARK, LCD1_D9_MARK, LCD1_D10_MARK, LCD1_D11_MARK,
+};
+static const unsigned int lcd1_data16_pins[] = {
+	/* D[0:15] */
+	4, 3, 2, 1, 0, 91, 92, 23,
+	93, 94, 21, 201, 200, 199, 196, 195,
+};
+static const unsigned int lcd1_data16_mux[] = {
+	LCD1_D0_MARK, LCD1_D1_MARK, LCD1_D2_MARK, LCD1_D3_MARK,
+	LCD1_D4_MARK, LCD1_D5_MARK, LCD1_D6_MARK, LCD1_D7_MARK,
+	LCD1_D8_MARK, LCD1_D9_MARK, LCD1_D10_MARK, LCD1_D11_MARK,
+	LCD1_D12_MARK, LCD1_D13_MARK, LCD1_D14_MARK, LCD1_D15_MARK,
+};
+static const unsigned int lcd1_data18_pins[] = {
+	/* D[0:17] */
+	4, 3, 2, 1, 0, 91, 92, 23,
+	93, 94, 21, 201, 200, 199, 196, 195,
+	194, 193,
+};
+static const unsigned int lcd1_data18_mux[] = {
+	LCD1_D0_MARK, LCD1_D1_MARK, LCD1_D2_MARK, LCD1_D3_MARK,
+	LCD1_D4_MARK, LCD1_D5_MARK, LCD1_D6_MARK, LCD1_D7_MARK,
+	LCD1_D8_MARK, LCD1_D9_MARK, LCD1_D10_MARK, LCD1_D11_MARK,
+	LCD1_D12_MARK, LCD1_D13_MARK, LCD1_D14_MARK, LCD1_D15_MARK,
+	LCD1_D16_MARK, LCD1_D17_MARK,
+};
+static const unsigned int lcd1_data24_pins[] = {
+	/* D[0:23] */
+	4, 3, 2, 1, 0, 91, 92, 23,
+	93, 94, 21, 201, 200, 199, 196, 195,
+	194, 193, 198, 197, 75, 74, 15, 14,
+};
+static const unsigned int lcd1_data24_mux[] = {
+	LCD1_D0_MARK, LCD1_D1_MARK, LCD1_D2_MARK, LCD1_D3_MARK,
+	LCD1_D4_MARK, LCD1_D5_MARK, LCD1_D6_MARK, LCD1_D7_MARK,
+	LCD1_D8_MARK, LCD1_D9_MARK, LCD1_D10_MARK, LCD1_D11_MARK,
+	LCD1_D12_MARK, LCD1_D13_MARK, LCD1_D14_MARK, LCD1_D15_MARK,
+	LCD1_D16_MARK, LCD1_D17_MARK, LCD1_D18_MARK, LCD1_D19_MARK,
+	LCD1_D20_MARK, LCD1_D21_MARK, LCD1_D22_MARK, LCD1_D23_MARK,
+};
+static const unsigned int lcd1_display_pins[] = {
+	/* DON, VCPWC, VEPWC */
+	100, 5, 6,
+};
+static const unsigned int lcd1_display_mux[] = {
+	LCD1_DON_MARK, LCD1_VCPWC_MARK, LCD1_VEPWC_MARK,
+};
+static const unsigned int lcd1_lclk_pins[] = {
+	/* LCLK */
+	40,
+};
+static const unsigned int lcd1_lclk_mux[] = {
+	LCD1_LCLK_MARK,
+};
+static const unsigned int lcd1_sync_pins[] = {
+	/* VSYN, HSYN, DCK, DISP */
+	98, 97, 99, 12,
+};
+static const unsigned int lcd1_sync_mux[] = {
+	LCD1_VSYN_MARK, LCD1_HSYN_MARK, LCD1_DCK_MARK, LCD1_DISP_MARK,
+};
+static const unsigned int lcd1_sys_pins[] = {
+	/* CS, WR, RD, RS */
+	97, 99, 13, 12,
+};
+static const unsigned int lcd1_sys_mux[] = {
+	LCD1_CS_MARK, LCD1_WR_MARK, LCD1_RD_MARK, LCD1_RS_MARK,
+};
+/* - MMCIF ------------------------------------------------------------------ */
+static const unsigned int mmc0_data1_0_pins[] = {
+	/* D[0] */
+	68,
+};
+static const unsigned int mmc0_data1_0_mux[] = {
+	MMC0_D0_PORT68_MARK,
+};
+static const unsigned int mmc0_data4_0_pins[] = {
+	/* D[0:3] */
+	68, 69, 70, 71,
+};
+static const unsigned int mmc0_data4_0_mux[] = {
+	MMC0_D0_PORT68_MARK, MMC0_D1_PORT69_MARK, MMC0_D2_PORT70_MARK, MMC0_D3_PORT71_MARK,
+};
+static const unsigned int mmc0_data8_0_pins[] = {
+	/* D[0:7] */
+	68, 69, 70, 71, 72, 73, 74, 75,
+};
+static const unsigned int mmc0_data8_0_mux[] = {
+	MMC0_D0_PORT68_MARK, MMC0_D1_PORT69_MARK, MMC0_D2_PORT70_MARK, MMC0_D3_PORT71_MARK,
+	MMC0_D4_PORT72_MARK, MMC0_D5_PORT73_MARK, MMC0_D6_PORT74_MARK, MMC0_D7_PORT75_MARK,
+};
+static const unsigned int mmc0_ctrl_0_pins[] = {
+	/* CMD, CLK */
+	67, 66,
+};
+static const unsigned int mmc0_ctrl_0_mux[] = {
+	MMC0_CMD_PORT67_MARK, MMC0_CLK_PORT66_MARK,
+};
+
+static const unsigned int mmc0_data1_1_pins[] = {
+	/* D[0] */
+	149,
+};
+static const unsigned int mmc0_data1_1_mux[] = {
+	MMC1_D0_PORT149_MARK,
+};
+static const unsigned int mmc0_data4_1_pins[] = {
+	/* D[0:3] */
+	149, 148, 147, 146,
+};
+static const unsigned int mmc0_data4_1_mux[] = {
+	MMC1_D0_PORT149_MARK, MMC1_D1_PORT148_MARK, MMC1_D2_PORT147_MARK, MMC1_D3_PORT146_MARK,
+};
+static const unsigned int mmc0_data8_1_pins[] = {
+	/* D[0:7] */
+	149, 148, 147, 146, 145, 144, 143, 142,
+};
+static const unsigned int mmc0_data8_1_mux[] = {
+	MMC1_D0_PORT149_MARK, MMC1_D1_PORT148_MARK, MMC1_D2_PORT147_MARK, MMC1_D3_PORT146_MARK,
+	MMC1_D4_PORT145_MARK, MMC1_D5_PORT144_MARK, MMC1_D6_PORT143_MARK, MMC1_D7_PORT142_MARK,
+};
+static const unsigned int mmc0_ctrl_1_pins[] = {
+	/* CMD, CLK */
+	104, 103,
+};
+static const unsigned int mmc0_ctrl_1_mux[] = {
+	MMC1_CMD_PORT104_MARK, MMC1_CLK_PORT103_MARK,
+};
+/* - SDHI0 ------------------------------------------------------------------ */
+static const unsigned int sdhi0_data1_pins[] = {
+	/* D0 */
+	77,
+};
+static const unsigned int sdhi0_data1_mux[] = {
+	SDHI0_D0_MARK,
+};
+static const unsigned int sdhi0_data4_pins[] = {
+	/* D[0:3] */
+	77, 78, 79, 80,
+};
+static const unsigned int sdhi0_data4_mux[] = {
+	SDHI0_D0_MARK, SDHI0_D1_MARK, SDHI0_D2_MARK, SDHI0_D3_MARK,
+};
+static const unsigned int sdhi0_ctrl_pins[] = {
+	/* CMD, CLK */
+	76, 82,
+};
+static const unsigned int sdhi0_ctrl_mux[] = {
+	SDHI0_CMD_MARK, SDHI0_CLK_MARK,
+};
+static const unsigned int sdhi0_cd_pins[] = {
+	/* CD */
+	81,
+};
+static const unsigned int sdhi0_cd_mux[] = {
+	SDHI0_CD_MARK,
+};
+static const unsigned int sdhi0_wp_pins[] = {
+	/* WP */
+	83,
+};
+static const unsigned int sdhi0_wp_mux[] = {
+	SDHI0_WP_MARK,
+};
+/* - SDHI1 ------------------------------------------------------------------ */
+static const unsigned int sdhi1_data1_pins[] = {
+	/* D0 */
+	68,
+};
+static const unsigned int sdhi1_data1_mux[] = {
+	SDHI1_D0_MARK,
+};
+static const unsigned int sdhi1_data4_pins[] = {
+	/* D[0:3] */
+	68, 69, 70, 71,
+};
+static const unsigned int sdhi1_data4_mux[] = {
+	SDHI1_D0_MARK, SDHI1_D1_MARK, SDHI1_D2_MARK, SDHI1_D3_MARK,
+};
+static const unsigned int sdhi1_ctrl_pins[] = {
+	/* CMD, CLK */
+	67, 66,
+};
+static const unsigned int sdhi1_ctrl_mux[] = {
+	SDHI1_CMD_MARK, SDHI1_CLK_MARK,
+};
+static const unsigned int sdhi1_cd_pins[] = {
+	/* CD */
+	72,
+};
+static const unsigned int sdhi1_cd_mux[] = {
+	SDHI1_CD_MARK,
+};
+static const unsigned int sdhi1_wp_pins[] = {
+	/* WP */
+	73,
+};
+static const unsigned int sdhi1_wp_mux[] = {
+	SDHI1_WP_MARK,
+};
+/* - SDHI2 ------------------------------------------------------------------ */
+static const unsigned int sdhi2_data1_pins[] = {
+	/* D0 */
+	205,
+};
+static const unsigned int sdhi2_data1_mux[] = {
+	SDHI2_D0_MARK,
+};
+static const unsigned int sdhi2_data4_pins[] = {
+	/* D[0:3] */
+	205, 206, 207, 208,
+};
+static const unsigned int sdhi2_data4_mux[] = {
+	SDHI2_D0_MARK, SDHI2_D1_MARK, SDHI2_D2_MARK, SDHI2_D3_MARK,
+};
+static const unsigned int sdhi2_ctrl_pins[] = {
+	/* CMD, CLK */
+	204, 203,
+};
+static const unsigned int sdhi2_ctrl_mux[] = {
+	SDHI2_CMD_MARK, SDHI2_CLK_MARK,
+};
+static const unsigned int sdhi2_cd_0_pins[] = {
+	/* CD */
+	202,
+};
+static const unsigned int sdhi2_cd_0_mux[] = {
+	SDHI2_CD_PORT202_MARK,
+};
+static const unsigned int sdhi2_wp_0_pins[] = {
+	/* WP */
+	177,
+};
+static const unsigned int sdhi2_wp_0_mux[] = {
+	SDHI2_WP_PORT177_MARK,
+};
+static const unsigned int sdhi2_cd_1_pins[] = {
+	/* CD */
+	24,
+};
+static const unsigned int sdhi2_cd_1_mux[] = {
+	SDHI2_CD_PORT24_MARK,
+};
+static const unsigned int sdhi2_wp_1_pins[] = {
+	/* WP */
+	25,
+};
+static const unsigned int sdhi2_wp_1_mux[] = {
+	SDHI2_WP_PORT25_MARK,
+};
 
+static const struct sh_pfc_pin_group pinmux_groups[] = {
+	SH_PFC_PIN_GROUP(lcd0_data8),
+	SH_PFC_PIN_GROUP(lcd0_data9),
+	SH_PFC_PIN_GROUP(lcd0_data12),
+	SH_PFC_PIN_GROUP(lcd0_data16),
+	SH_PFC_PIN_GROUP(lcd0_data18),
+	SH_PFC_PIN_GROUP(lcd0_data24_0),
+	SH_PFC_PIN_GROUP(lcd0_data24_1),
+	SH_PFC_PIN_GROUP(lcd0_display),
+	SH_PFC_PIN_GROUP(lcd0_lclk_0),
+	SH_PFC_PIN_GROUP(lcd0_lclk_1),
+	SH_PFC_PIN_GROUP(lcd0_sync),
+	SH_PFC_PIN_GROUP(lcd0_sys),
+	SH_PFC_PIN_GROUP(lcd1_data8),
+	SH_PFC_PIN_GROUP(lcd1_data9),
+	SH_PFC_PIN_GROUP(lcd1_data12),
+	SH_PFC_PIN_GROUP(lcd1_data16),
+	SH_PFC_PIN_GROUP(lcd1_data18),
+	SH_PFC_PIN_GROUP(lcd1_data24),
+	SH_PFC_PIN_GROUP(lcd1_display),
+	SH_PFC_PIN_GROUP(lcd1_lclk),
+	SH_PFC_PIN_GROUP(lcd1_sync),
+	SH_PFC_PIN_GROUP(lcd1_sys),
+	SH_PFC_PIN_GROUP(mmc0_data1_0),
+	SH_PFC_PIN_GROUP(mmc0_data4_0),
+	SH_PFC_PIN_GROUP(mmc0_data8_0),
+	SH_PFC_PIN_GROUP(mmc0_ctrl_0),
+	SH_PFC_PIN_GROUP(mmc0_data1_1),
+	SH_PFC_PIN_GROUP(mmc0_data4_1),
+	SH_PFC_PIN_GROUP(mmc0_data8_1),
+	SH_PFC_PIN_GROUP(mmc0_ctrl_1),
+	SH_PFC_PIN_GROUP(sdhi0_data1),
+	SH_PFC_PIN_GROUP(sdhi0_data4),
+	SH_PFC_PIN_GROUP(sdhi0_ctrl),
+	SH_PFC_PIN_GROUP(sdhi0_cd),
+	SH_PFC_PIN_GROUP(sdhi0_wp),
+	SH_PFC_PIN_GROUP(sdhi1_data1),
+	SH_PFC_PIN_GROUP(sdhi1_data4),
+	SH_PFC_PIN_GROUP(sdhi1_ctrl),
+	SH_PFC_PIN_GROUP(sdhi1_cd),
+	SH_PFC_PIN_GROUP(sdhi1_wp),
+	SH_PFC_PIN_GROUP(sdhi2_data1),
+	SH_PFC_PIN_GROUP(sdhi2_data4),
+	SH_PFC_PIN_GROUP(sdhi2_ctrl),
+	SH_PFC_PIN_GROUP(sdhi2_cd_0),
+	SH_PFC_PIN_GROUP(sdhi2_wp_0),
+	SH_PFC_PIN_GROUP(sdhi2_cd_1),
+	SH_PFC_PIN_GROUP(sdhi2_wp_1),
+};
+
+static const char * const lcd0_groups[] = {
+	"lcd0_data8",
+	"lcd0_data9",
+	"lcd0_data12",
+	"lcd0_data16",
+	"lcd0_data18",
+	"lcd0_data24_0",
+	"lcd0_data24_1",
+	"lcd0_display",
+	"lcd0_lclk_0",
+	"lcd0_lclk_1",
+	"lcd0_sync",
+	"lcd0_sys",
+};
+
+static const char * const lcd1_groups[] = {
+	"lcd1_data8",
+	"lcd1_data9",
+	"lcd1_data12",
+	"lcd1_data16",
+	"lcd1_data18",
+	"lcd1_data24",
+	"lcd1_display",
+	"lcd1_lclk",
+	"lcd1_sync",
+	"lcd1_sys",
+};
+
+static const char * const mmc0_groups[] = {
+	"mmc0_data1_0",
+	"mmc0_data4_0",
+	"mmc0_data8_0",
+	"mmc0_ctrl_0",
+	"mmc0_data1_1",
+	"mmc0_data4_1",
+	"mmc0_data8_1",
+	"mmc0_ctrl_1",
+};
+
+static const char * const sdhi0_groups[] = {
+	"sdhi0_data1",
+	"sdhi0_data4",
+	"sdhi0_ctrl",
+	"sdhi0_cd",
+	"sdhi0_wp",
+};
+
+static const char * const sdhi1_groups[] = {
+	"sdhi1_data1",
+	"sdhi1_data4",
+	"sdhi1_ctrl",
+	"sdhi1_cd",
+	"sdhi1_wp",
+};
+
+static const char * const sdhi2_groups[] = {
+	"sdhi2_data1",
+	"sdhi2_data4",
+	"sdhi2_ctrl",
+	"sdhi2_cd_0",
+	"sdhi2_wp_0",
+	"sdhi2_cd_1",
+	"sdhi2_wp_1",
+};
+
+static const struct sh_pfc_function pinmux_functions[] = {
+	SH_PFC_FUNCTION(lcd0),
+	SH_PFC_FUNCTION(lcd1),
+	SH_PFC_FUNCTION(mmc0),
+	SH_PFC_FUNCTION(sdhi0),
+	SH_PFC_FUNCTION(sdhi1),
+	SH_PFC_FUNCTION(sdhi2),
+};
+
+#define PINMUX_FN_BASE	ARRAY_SIZE(pinmux_pins)
+
+static const struct pinmux_func pinmux_func_gpios[] = {
 	/* IRQ */
 	GPIO_FN(IRQ0_PORT2),	GPIO_FN(IRQ0_PORT13),
 	GPIO_FN(IRQ1),
@@ -1792,43 +2313,6 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	GPIO_FN(SCIFB_RTS_PORT172),
 	GPIO_FN(SCIFB_CTS_PORT173),
 
-	/* LCD0 */
-	GPIO_FN(LCD0_D0),	GPIO_FN(LCD0_D1),	GPIO_FN(LCD0_D2),
-	GPIO_FN(LCD0_D3),	GPIO_FN(LCD0_D4),	GPIO_FN(LCD0_D5),
-	GPIO_FN(LCD0_D6),	GPIO_FN(LCD0_D7),	GPIO_FN(LCD0_D8),
-	GPIO_FN(LCD0_D9),	GPIO_FN(LCD0_D10),	GPIO_FN(LCD0_D11),
-	GPIO_FN(LCD0_D12),	GPIO_FN(LCD0_D13),	GPIO_FN(LCD0_D14),
-	GPIO_FN(LCD0_D15),	GPIO_FN(LCD0_D16),	GPIO_FN(LCD0_D17),
-	GPIO_FN(LCD0_DON),	GPIO_FN(LCD0_VCPWC),	GPIO_FN(LCD0_VEPWC),
-	GPIO_FN(LCD0_DCK),	GPIO_FN(LCD0_VSYN),
-	GPIO_FN(LCD0_HSYN),	GPIO_FN(LCD0_DISP),
-	GPIO_FN(LCD0_WR),	GPIO_FN(LCD0_RD),
-	GPIO_FN(LCD0_CS),	GPIO_FN(LCD0_RS),
-
-	GPIO_FN(LCD0_D18_PORT163),	GPIO_FN(LCD0_D19_PORT162),
-	GPIO_FN(LCD0_D20_PORT161),	GPIO_FN(LCD0_D21_PORT158),
-	GPIO_FN(LCD0_D22_PORT160),	GPIO_FN(LCD0_D23_PORT159),
-	GPIO_FN(LCD0_LCLK_PORT165),	/* MSEL5CR_6_1 */
-
-	GPIO_FN(LCD0_D18_PORT40),	GPIO_FN(LCD0_D19_PORT4),
-	GPIO_FN(LCD0_D20_PORT3),	GPIO_FN(LCD0_D21_PORT2),
-	GPIO_FN(LCD0_D22_PORT0),	GPIO_FN(LCD0_D23_PORT1),
-	GPIO_FN(LCD0_LCLK_PORT102),	/* MSEL5CR_6_0 */
-
-	/* LCD1 */
-	GPIO_FN(LCD1_D0),	GPIO_FN(LCD1_D1),	GPIO_FN(LCD1_D2),
-	GPIO_FN(LCD1_D3),	GPIO_FN(LCD1_D4),	GPIO_FN(LCD1_D5),
-	GPIO_FN(LCD1_D6),	GPIO_FN(LCD1_D7),	GPIO_FN(LCD1_D8),
-	GPIO_FN(LCD1_D9),	GPIO_FN(LCD1_D10),	GPIO_FN(LCD1_D11),
-	GPIO_FN(LCD1_D12),	GPIO_FN(LCD1_D13),	GPIO_FN(LCD1_D14),
-	GPIO_FN(LCD1_D15),	GPIO_FN(LCD1_D16),	GPIO_FN(LCD1_D17),
-	GPIO_FN(LCD1_D18),	GPIO_FN(LCD1_D19),	GPIO_FN(LCD1_D20),
-	GPIO_FN(LCD1_D21),	GPIO_FN(LCD1_D22),	GPIO_FN(LCD1_D23),
-	GPIO_FN(LCD1_RS),	GPIO_FN(LCD1_RD),	GPIO_FN(LCD1_CS),
-	GPIO_FN(LCD1_WR),	GPIO_FN(LCD1_DCK),	GPIO_FN(LCD1_DON),
-	GPIO_FN(LCD1_VCPWC),	GPIO_FN(LCD1_LCLK),	GPIO_FN(LCD1_HSYN),
-	GPIO_FN(LCD1_VSYN),	GPIO_FN(LCD1_VEPWC),	GPIO_FN(LCD1_DISP),
-
 	/* RSPI */
 	GPIO_FN(RSPI_SSL0_A),	GPIO_FN(RSPI_SSL1_A),	GPIO_FN(RSPI_SSL2_A),
 	GPIO_FN(RSPI_SSL3_A),	GPIO_FN(RSPI_CK_A),	GPIO_FN(RSPI_MOSI_A),
@@ -1889,26 +2373,6 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	GPIO_FN(SIM_D_PORT22), /* SIM_D  Port 22/199 */
 	GPIO_FN(SIM_D_PORT199),
 
-	/* SDHI0 */
-	GPIO_FN(SDHI0_D0),	GPIO_FN(SDHI0_D1),	GPIO_FN(SDHI0_D2),
-	GPIO_FN(SDHI0_D3),	GPIO_FN(SDHI0_CD),	GPIO_FN(SDHI0_WP),
-	GPIO_FN(SDHI0_CMD),	GPIO_FN(SDHI0_CLK),
-
-	/* SDHI1 */
-	GPIO_FN(SDHI1_D0),	GPIO_FN(SDHI1_D1),	GPIO_FN(SDHI1_D2),
-	GPIO_FN(SDHI1_D3),	GPIO_FN(SDHI1_CD),	GPIO_FN(SDHI1_WP),
-	GPIO_FN(SDHI1_CMD),	GPIO_FN(SDHI1_CLK),
-
-	/* SDHI2 */
-	GPIO_FN(SDHI2_D0),	GPIO_FN(SDHI2_D1),	GPIO_FN(SDHI2_D2),
-	GPIO_FN(SDHI2_D3),	GPIO_FN(SDHI2_CLK),	GPIO_FN(SDHI2_CMD),
-
-	GPIO_FN(SDHI2_CD_PORT24), /* MSEL5CR_19_0 */
-	GPIO_FN(SDHI2_WP_PORT25),
-
-	GPIO_FN(SDHI2_WP_PORT177), /* MSEL5CR_19_1 */
-	GPIO_FN(SDHI2_CD_PORT202),
-
 	/* MSIOF2 */
 	GPIO_FN(MSIOF2_TXD),	GPIO_FN(MSIOF2_RXD),	GPIO_FN(MSIOF2_TSCK),
 	GPIO_FN(MSIOF2_SS2),	GPIO_FN(MSIOF2_TSYNC),	GPIO_FN(MSIOF2_SS1),
@@ -1953,21 +2417,6 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	GPIO_FN(MEMC_WAIT),	GPIO_FN(MEMC_DREQ1),	GPIO_FN(MEMC_BUSCLK),
 	GPIO_FN(MEMC_A0),
 
-	/* MMC */
-	GPIO_FN(MMC0_D0_PORT68),	GPIO_FN(MMC0_D1_PORT69),
-	GPIO_FN(MMC0_D2_PORT70),	GPIO_FN(MMC0_D3_PORT71),
-	GPIO_FN(MMC0_D4_PORT72),	GPIO_FN(MMC0_D5_PORT73),
-	GPIO_FN(MMC0_D6_PORT74),	GPIO_FN(MMC0_D7_PORT75),
-	GPIO_FN(MMC0_CLK_PORT66),
-	GPIO_FN(MMC0_CMD_PORT67),	/* MSEL4CR_15_0 */
-
-	GPIO_FN(MMC1_D0_PORT149),	GPIO_FN(MMC1_D1_PORT148),
-	GPIO_FN(MMC1_D2_PORT147),	GPIO_FN(MMC1_D3_PORT146),
-	GPIO_FN(MMC1_D4_PORT145),	GPIO_FN(MMC1_D5_PORT144),
-	GPIO_FN(MMC1_D6_PORT143),	GPIO_FN(MMC1_D7_PORT142),
-	GPIO_FN(MMC1_CLK_PORT103),
-	GPIO_FN(MMC1_CMD_PORT104),	/* MSEL4CR_15_1 */
-
 	/* MSIOF0 */
 	GPIO_FN(MSIOF0_SS1),	GPIO_FN(MSIOF0_SS2),	GPIO_FN(MSIOF0_RXD),
 	GPIO_FN(MSIOF0_TXD),	GPIO_FN(MSIOF0_MCK0),	GPIO_FN(MSIOF0_MCK1),
@@ -2126,7 +2575,7 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	GPIO_FN(TRACEAUD_FROM_MEMC),
 };
 
-static struct pinmux_cfg_reg pinmux_config_regs[] = {
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	PORTCR(0,	0xe6050000), /* PORT0CR */
 	PORTCR(1,	0xe6050001), /* PORT1CR */
 	PORTCR(2,	0xe6050002), /* PORT2CR */
@@ -2440,7 +2889,7 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ },
 };
 
-static struct pinmux_data_reg pinmux_data_regs[] = {
+static const struct pinmux_data_reg pinmux_data_regs[] = {
 	{ PINMUX_DATA_REG("PORTL031_000DR", 0xe6054800, 32) {
 		PORT31_DATA,	PORT30_DATA,	PORT29_DATA,	PORT28_DATA,
 		PORT27_DATA,	PORT26_DATA,	PORT25_DATA,	PORT24_DATA,
@@ -2544,46 +2993,43 @@ static struct pinmux_data_reg pinmux_data_regs[] = {
 	{ },
 };
 
-static struct pinmux_irq pinmux_irqs[] = {
-	PINMUX_IRQ(evt2irq(0x0200), PORT2_FN0,	 PORT13_FN0),	/* IRQ0A */
-	PINMUX_IRQ(evt2irq(0x0220), PORT20_FN0),		/* IRQ1A */
-	PINMUX_IRQ(evt2irq(0x0240), PORT11_FN0,	 PORT12_FN0),	/* IRQ2A */
-	PINMUX_IRQ(evt2irq(0x0260), PORT10_FN0,	 PORT14_FN0),	/* IRQ3A */
-	PINMUX_IRQ(evt2irq(0x0280), PORT15_FN0,	 PORT172_FN0),	/* IRQ4A */
-	PINMUX_IRQ(evt2irq(0x02A0), PORT0_FN0,	 PORT1_FN0),	/* IRQ5A */
-	PINMUX_IRQ(evt2irq(0x02C0), PORT121_FN0, PORT173_FN0),	/* IRQ6A */
-	PINMUX_IRQ(evt2irq(0x02E0), PORT120_FN0, PORT209_FN0),	/* IRQ7A */
-	PINMUX_IRQ(evt2irq(0x0300), PORT119_FN0),		/* IRQ8A */
-	PINMUX_IRQ(evt2irq(0x0320), PORT118_FN0, PORT210_FN0),	/* IRQ9A */
-	PINMUX_IRQ(evt2irq(0x0340), PORT19_FN0),		/* IRQ10A */
-	PINMUX_IRQ(evt2irq(0x0360), PORT104_FN0),		/* IRQ11A */
-	PINMUX_IRQ(evt2irq(0x0380), PORT42_FN0,	 PORT97_FN0),	/* IRQ12A */
-	PINMUX_IRQ(evt2irq(0x03A0), PORT64_FN0,	 PORT98_FN0),	/* IRQ13A */
-	PINMUX_IRQ(evt2irq(0x03C0), PORT63_FN0,	 PORT99_FN0),	/* IRQ14A */
-	PINMUX_IRQ(evt2irq(0x03E0), PORT62_FN0,	 PORT100_FN0),	/* IRQ15A */
-	PINMUX_IRQ(evt2irq(0x3200), PORT68_FN0,	 PORT211_FN0),	/* IRQ16A */
-	PINMUX_IRQ(evt2irq(0x3220), PORT69_FN0),		/* IRQ17A */
-	PINMUX_IRQ(evt2irq(0x3240), PORT70_FN0),		/* IRQ18A */
-	PINMUX_IRQ(evt2irq(0x3260), PORT71_FN0),		/* IRQ19A */
-	PINMUX_IRQ(evt2irq(0x3280), PORT67_FN0),		/* IRQ20A */
-	PINMUX_IRQ(evt2irq(0x32A0), PORT202_FN0),		/* IRQ21A */
-	PINMUX_IRQ(evt2irq(0x32C0), PORT95_FN0),		/* IRQ22A */
-	PINMUX_IRQ(evt2irq(0x32E0), PORT96_FN0),		/* IRQ23A */
-	PINMUX_IRQ(evt2irq(0x3300), PORT180_FN0),		/* IRQ24A */
-	PINMUX_IRQ(evt2irq(0x3320), PORT38_FN0),		/* IRQ25A */
-	PINMUX_IRQ(evt2irq(0x3340), PORT58_FN0,	 PORT81_FN0),	/* IRQ26A */
-	PINMUX_IRQ(evt2irq(0x3360), PORT57_FN0,	 PORT168_FN0),	/* IRQ27A */
-	PINMUX_IRQ(evt2irq(0x3380), PORT56_FN0,	 PORT169_FN0),	/* IRQ28A */
-	PINMUX_IRQ(evt2irq(0x33A0), PORT50_FN0,	 PORT170_FN0),	/* IRQ29A */
-	PINMUX_IRQ(evt2irq(0x33C0), PORT49_FN0,	 PORT171_FN0),	/* IRQ30A */
-	PINMUX_IRQ(evt2irq(0x33E0), PORT41_FN0,	 PORT167_FN0),	/* IRQ31A */
-};
-
-struct sh_pfc_soc_info r8a7740_pinmux_info = {
+static const struct pinmux_irq pinmux_irqs[] = {
+	PINMUX_IRQ(irq_pin(0), GPIO_PORT2,   GPIO_PORT13),	/* IRQ0A */
+	PINMUX_IRQ(irq_pin(1), GPIO_PORT20),		/* IRQ1A */
+	PINMUX_IRQ(irq_pin(2), GPIO_PORT11,  GPIO_PORT12),	/* IRQ2A */
+	PINMUX_IRQ(irq_pin(3), GPIO_PORT10,  GPIO_PORT14),	/* IRQ3A */
+	PINMUX_IRQ(irq_pin(4), GPIO_PORT15,  GPIO_PORT172),/* IRQ4A */
+	PINMUX_IRQ(irq_pin(5), GPIO_PORT0,   GPIO_PORT1),	/* IRQ5A */
+	PINMUX_IRQ(irq_pin(6), GPIO_PORT121, GPIO_PORT173),/* IRQ6A */
+	PINMUX_IRQ(irq_pin(7), GPIO_PORT120, GPIO_PORT209),/* IRQ7A */
+	PINMUX_IRQ(irq_pin(8), GPIO_PORT119),		/* IRQ8A */
+	PINMUX_IRQ(irq_pin(9), GPIO_PORT118, GPIO_PORT210),/* IRQ9A */
+	PINMUX_IRQ(irq_pin(10), GPIO_PORT19),		/* IRQ10A */
+	PINMUX_IRQ(irq_pin(11), GPIO_PORT104),		/* IRQ11A */
+	PINMUX_IRQ(irq_pin(12), GPIO_PORT42,  GPIO_PORT97),	/* IRQ12A */
+	PINMUX_IRQ(irq_pin(13), GPIO_PORT64,  GPIO_PORT98),	/* IRQ13A */
+	PINMUX_IRQ(irq_pin(14), GPIO_PORT63,  GPIO_PORT99),	/* IRQ14A */
+	PINMUX_IRQ(irq_pin(15), GPIO_PORT62,  GPIO_PORT100),/* IRQ15A */
+	PINMUX_IRQ(irq_pin(16), GPIO_PORT68,  GPIO_PORT211),/* IRQ16A */
+	PINMUX_IRQ(irq_pin(17), GPIO_PORT69),		/* IRQ17A */
+	PINMUX_IRQ(irq_pin(18), GPIO_PORT70),		/* IRQ18A */
+	PINMUX_IRQ(irq_pin(19), GPIO_PORT71),		/* IRQ19A */
+	PINMUX_IRQ(irq_pin(20), GPIO_PORT67),		/* IRQ20A */
+	PINMUX_IRQ(irq_pin(21), GPIO_PORT202),		/* IRQ21A */
+	PINMUX_IRQ(irq_pin(22), GPIO_PORT95),		/* IRQ22A */
+	PINMUX_IRQ(irq_pin(23), GPIO_PORT96),		/* IRQ23A */
+	PINMUX_IRQ(irq_pin(24), GPIO_PORT180),		/* IRQ24A */
+	PINMUX_IRQ(irq_pin(25), GPIO_PORT38),		/* IRQ25A */
+	PINMUX_IRQ(irq_pin(26), GPIO_PORT58,  GPIO_PORT81),	/* IRQ26A */
+	PINMUX_IRQ(irq_pin(27), GPIO_PORT57,  GPIO_PORT168),/* IRQ27A */
+	PINMUX_IRQ(irq_pin(28), GPIO_PORT56,  GPIO_PORT169),/* IRQ28A */
+	PINMUX_IRQ(irq_pin(29), GPIO_PORT50,  GPIO_PORT170),/* IRQ29A */
+	PINMUX_IRQ(irq_pin(30), GPIO_PORT49,  GPIO_PORT171),/* IRQ30A */
+	PINMUX_IRQ(irq_pin(31), GPIO_PORT41,  GPIO_PORT167),/* IRQ31A */
+};
+
+const struct sh_pfc_soc_info r8a7740_pinmux_info = {
 	.name		= "r8a7740_pfc",
-	.reserved_id	= PINMUX_RESERVED,
-	.data		= { PINMUX_DATA_BEGIN,
-			    PINMUX_DATA_END },
 	.input		= { PINMUX_INPUT_BEGIN,
 			    PINMUX_INPUT_END },
 	.input_pu	= { PINMUX_INPUT_PULLUP_BEGIN,
@@ -2592,15 +3038,19 @@ struct sh_pfc_soc_info r8a7740_pinmux_info = {
 			    PINMUX_INPUT_PULLDOWN_END },
 	.output		= { PINMUX_OUTPUT_BEGIN,
 			    PINMUX_OUTPUT_END },
-	.mark		= { PINMUX_MARK_BEGIN,
-			    PINMUX_MARK_END },
 	.function	= { PINMUX_FUNCTION_BEGIN,
 			    PINMUX_FUNCTION_END },
 
-	.first_gpio	= GPIO_PORT0,
-	.last_gpio	= GPIO_FN_TRACEAUD_FROM_MEMC,
+	.pins		= pinmux_pins,
+	.nr_pins	= ARRAY_SIZE(pinmux_pins),
+	.groups		= pinmux_groups,
+	.nr_groups	= ARRAY_SIZE(pinmux_groups),
+	.functions	= pinmux_functions,
+	.nr_functions	= ARRAY_SIZE(pinmux_functions),
+
+	.func_gpios	= pinmux_func_gpios,
+	.nr_func_gpios	= ARRAY_SIZE(pinmux_func_gpios),
 
-	.gpios		= pinmux_gpios,
 	.cfg_regs	= pinmux_config_regs,
 	.data_regs	= pinmux_data_regs,
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7779.c b/drivers/pinctrl/sh-pfc/pfc-r8a7779.c
index 13feaa0c0eb7..791a6719d8a9 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7779.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7779.c
@@ -19,57 +19,77 @@
  */
 
 #include <linux/kernel.h>
-#include <mach/r8a7779.h>
 
 #include "sh_pfc.h"
 
-#define CPU_32_PORT(fn, pfx, sfx)				\
-	PORT_10(fn, pfx, sfx), PORT_10(fn, pfx##1, sfx),	\
-	PORT_10(fn, pfx##2, sfx), PORT_1(fn, pfx##30, sfx),	\
-	PORT_1(fn, pfx##31, sfx)
-
-#define CPU_32_PORT6(fn, pfx, sfx)				\
-	PORT_1(fn, pfx##0, sfx), PORT_1(fn, pfx##1, sfx),	\
-	PORT_1(fn, pfx##2, sfx), PORT_1(fn, pfx##3, sfx),	\
-	PORT_1(fn, pfx##4, sfx), PORT_1(fn, pfx##5, sfx),	\
-	PORT_1(fn, pfx##6, sfx), PORT_1(fn, pfx##7, sfx),	\
-	PORT_1(fn, pfx##8, sfx)
-
-#define CPU_ALL_PORT(fn, pfx, sfx)				\
-	CPU_32_PORT(fn, pfx##_0_, sfx),				\
-	CPU_32_PORT(fn, pfx##_1_, sfx),				\
-	CPU_32_PORT(fn, pfx##_2_, sfx),				\
-	CPU_32_PORT(fn, pfx##_3_, sfx),				\
-	CPU_32_PORT(fn, pfx##_4_, sfx),				\
-	CPU_32_PORT(fn, pfx##_5_, sfx),				\
-	CPU_32_PORT6(fn, pfx##_6_, sfx)
-
-#define _GP_GPIO(pfx, sfx) PINMUX_GPIO(GPIO_GP##pfx, GP##pfx##_DATA)
-#define _GP_DATA(pfx, sfx) PINMUX_DATA(GP##pfx##_DATA, GP##pfx##_FN,	\
-				       GP##pfx##_IN, GP##pfx##_OUT)
-
-#define _GP_INOUTSEL(pfx, sfx) GP##pfx##_IN, GP##pfx##_OUT
-#define _GP_INDT(pfx, sfx) GP##pfx##_DATA
-
-#define GP_ALL(str)	CPU_ALL_PORT(_PORT_ALL, GP, str)
-#define PINMUX_GPIO_GP_ALL()	CPU_ALL_PORT(_GP_GPIO, , unused)
-#define PINMUX_DATA_GP_ALL()	CPU_ALL_PORT(_GP_DATA, , unused)
-
-
-#define PORT_10_REV(fn, pfx, sfx)				\
-	PORT_1(fn, pfx##9, sfx), PORT_1(fn, pfx##8, sfx),	\
-	PORT_1(fn, pfx##7, sfx), PORT_1(fn, pfx##6, sfx),	\
-	PORT_1(fn, pfx##5, sfx), PORT_1(fn, pfx##4, sfx),	\
-	PORT_1(fn, pfx##3, sfx), PORT_1(fn, pfx##2, sfx),	\
-	PORT_1(fn, pfx##1, sfx), PORT_1(fn, pfx##0, sfx)
-
-#define CPU_32_PORT_REV(fn, pfx, sfx)					\
-	PORT_1(fn, pfx##31, sfx), PORT_1(fn, pfx##30, sfx),		\
-	PORT_10_REV(fn, pfx##2, sfx), PORT_10_REV(fn, pfx##1, sfx),	\
-	PORT_10_REV(fn, pfx, sfx)
-
-#define GP_INOUTSEL(bank) CPU_32_PORT_REV(_GP_INOUTSEL, _##bank##_, unused)
-#define GP_INDT(bank) CPU_32_PORT_REV(_GP_INDT, _##bank##_, unused)
+#define PORT_GP_1(bank, pin, fn, sfx) fn(bank, pin, GP_##bank##_##pin, sfx)
+
+#define PORT_GP_32(bank, fn, sfx)					\
+	PORT_GP_1(bank, 0,  fn, sfx), PORT_GP_1(bank, 1,  fn, sfx),	\
+	PORT_GP_1(bank, 2,  fn, sfx), PORT_GP_1(bank, 3,  fn, sfx),	\
+	PORT_GP_1(bank, 4,  fn, sfx), PORT_GP_1(bank, 5,  fn, sfx),	\
+	PORT_GP_1(bank, 6,  fn, sfx), PORT_GP_1(bank, 7,  fn, sfx),	\
+	PORT_GP_1(bank, 8,  fn, sfx), PORT_GP_1(bank, 9,  fn, sfx),	\
+	PORT_GP_1(bank, 10, fn, sfx), PORT_GP_1(bank, 11, fn, sfx),	\
+	PORT_GP_1(bank, 12, fn, sfx), PORT_GP_1(bank, 13, fn, sfx),	\
+	PORT_GP_1(bank, 14, fn, sfx), PORT_GP_1(bank, 15, fn, sfx),	\
+	PORT_GP_1(bank, 16, fn, sfx), PORT_GP_1(bank, 17, fn, sfx),	\
+	PORT_GP_1(bank, 18, fn, sfx), PORT_GP_1(bank, 19, fn, sfx),	\
+	PORT_GP_1(bank, 20, fn, sfx), PORT_GP_1(bank, 21, fn, sfx),	\
+	PORT_GP_1(bank, 22, fn, sfx), PORT_GP_1(bank, 23, fn, sfx),	\
+	PORT_GP_1(bank, 24, fn, sfx), PORT_GP_1(bank, 25, fn, sfx),	\
+	PORT_GP_1(bank, 26, fn, sfx), PORT_GP_1(bank, 27, fn, sfx),	\
+	PORT_GP_1(bank, 28, fn, sfx), PORT_GP_1(bank, 29, fn, sfx),	\
+	PORT_GP_1(bank, 30, fn, sfx), PORT_GP_1(bank, 31, fn, sfx)
+
+#define PORT_GP_32_9(bank, fn, sfx)					\
+	PORT_GP_1(bank, 0, fn, sfx), PORT_GP_1(bank, 1, fn, sfx),	\
+	PORT_GP_1(bank, 2, fn, sfx), PORT_GP_1(bank, 3, fn, sfx),	\
+	PORT_GP_1(bank, 4, fn, sfx), PORT_GP_1(bank, 5, fn, sfx),	\
+	PORT_GP_1(bank, 6, fn, sfx), PORT_GP_1(bank, 7, fn, sfx),	\
+	PORT_GP_1(bank, 8, fn, sfx)
+
+#define PORT_GP_32_REV(bank, fn, sfx)					\
+	PORT_GP_1(bank, 31, fn, sfx), PORT_GP_1(bank, 30, fn, sfx),	\
+	PORT_GP_1(bank, 29, fn, sfx), PORT_GP_1(bank, 28, fn, sfx),	\
+	PORT_GP_1(bank, 27, fn, sfx), PORT_GP_1(bank, 26, fn, sfx),	\
+	PORT_GP_1(bank, 25, fn, sfx), PORT_GP_1(bank, 24, fn, sfx),	\
+	PORT_GP_1(bank, 23, fn, sfx), PORT_GP_1(bank, 22, fn, sfx),	\
+	PORT_GP_1(bank, 21, fn, sfx), PORT_GP_1(bank, 20, fn, sfx),	\
+	PORT_GP_1(bank, 19, fn, sfx), PORT_GP_1(bank, 18, fn, sfx),	\
+	PORT_GP_1(bank, 17, fn, sfx), PORT_GP_1(bank, 16, fn, sfx),	\
+	PORT_GP_1(bank, 15, fn, sfx), PORT_GP_1(bank, 14, fn, sfx),	\
+	PORT_GP_1(bank, 13, fn, sfx), PORT_GP_1(bank, 12, fn, sfx),	\
+	PORT_GP_1(bank, 11, fn, sfx), PORT_GP_1(bank, 10, fn, sfx),	\
+	PORT_GP_1(bank, 9,  fn, sfx), PORT_GP_1(bank, 8,  fn, sfx),	\
+	PORT_GP_1(bank, 7,  fn, sfx), PORT_GP_1(bank, 6,  fn, sfx),	\
+	PORT_GP_1(bank, 5,  fn, sfx), PORT_GP_1(bank, 4,  fn, sfx),	\
+	PORT_GP_1(bank, 3,  fn, sfx), PORT_GP_1(bank, 2,  fn, sfx),	\
+	PORT_GP_1(bank, 1,  fn, sfx), PORT_GP_1(bank, 0,  fn, sfx)
+
+#define CPU_ALL_PORT(fn, sfx)						\
+	PORT_GP_32(0, fn, sfx),						\
+	PORT_GP_32(1, fn, sfx),						\
+	PORT_GP_32(2, fn, sfx),						\
+	PORT_GP_32(3, fn, sfx),						\
+	PORT_GP_32(4, fn, sfx),						\
+	PORT_GP_32(5, fn, sfx),						\
+	PORT_GP_32_9(6, fn, sfx)
+
+#define _GP_PORT_ALL(bank, pin, name, sfx)	name##_##sfx
+
+#define _GP_GPIO(bank, pin, _name, sfx)					\
+	[(bank * 32) + pin] = {						\
+		.name = __stringify(_name),				\
+		.enum_id = _name##_DATA,				\
+	}
+
+#define _GP_DATA(bank, pin, name, sfx)					\
+	PINMUX_DATA(name##_DATA, name##_FN)
+
+#define GP_ALL(str)		CPU_ALL_PORT(_GP_PORT_ALL, str)
+#define PINMUX_GPIO_GP_ALL()	CPU_ALL_PORT(_GP_GPIO, unused)
+#define PINMUX_DATA_GP_ALL()	CPU_ALL_PORT(_GP_DATA, unused)
 
 #define PINMUX_IPSR_DATA(ipsr, fn) PINMUX_DATA(fn##_MARK, FN_##ipsr, FN_##fn)
 #define PINMUX_IPSR_MODSEL_DATA(ipsr, fn, ms) PINMUX_DATA(fn##_MARK, FN_##ms, \
@@ -82,14 +102,6 @@ enum {
 	GP_ALL(DATA), /* GP_0_0_DATA -> GP_6_8_DATA */
 	PINMUX_DATA_END,
 
-	PINMUX_INPUT_BEGIN,
-	GP_ALL(IN), /* GP_0_0_IN -> GP_6_8_IN */
-	PINMUX_INPUT_END,
-
-	PINMUX_OUTPUT_BEGIN,
-	GP_ALL(OUT), /* GP_0_0_OUT -> GP_6_8_OUT */
-	PINMUX_OUTPUT_END,
-
 	PINMUX_FUNCTION_BEGIN,
 	GP_ALL(FN), /* GP_0_0_FN -> GP_6_8_FN */
 
@@ -371,7 +383,7 @@ enum {
 	FN_VI1_DATA6_VI1_B6, FN_SD2_CD, FN_MT0_VCXO, FN_SPA_TMS,
 	FN_HSPI_TX1_D, FN_VI1_DATA7_VI1_B7, FN_SD2_WP, FN_MT0_PWM,
 	FN_SPA_TDI, FN_HSPI_RX1_D, FN_VI1_G0, FN_VI3_DATA0,
-	FN_DU1_DOTCLKOUT1, FN_TS_SCK1, FN_DREQ2_B, FN_TX2,
+	FN_TS_SCK1, FN_DREQ2_B, FN_TX2,
 	FN_SPA_TDO, FN_HCTS0_B, FN_VI1_G1, FN_VI3_DATA1,
 	FN_SSI_SCK1, FN_TS_SDEN1, FN_DACK2_B, FN_RX2, FN_HRTS0_B,
 
@@ -447,7 +459,8 @@ enum {
 	A0_MARK, SD1_DAT3_MARK, MMC0_D3_MARK, FD3_MARK,
 	BS_MARK, SD1_DAT2_MARK, MMC0_D2_MARK, FD2_MARK,
 	ATADIR0_MARK, SDSELF_MARK, HCTS1_MARK, TX4_C_MARK,
-	USB_PENC2_MARK, SCK0_MARK, PWM1_MARK, PWMFSW0_MARK,
+	USB_PENC0_MARK, USB_PENC1_MARK, USB_PENC2_MARK,
+	SCK0_MARK, PWM1_MARK, PWMFSW0_MARK,
 	SCIF_CLK_MARK, TCLK0_C_MARK,
 
 	EX_CS0_MARK, RX3_C_IRDA_RX_C_MARK, MMC0_D6_MARK,
@@ -632,7 +645,7 @@ enum {
 	HSPI_CS1_D_MARK, ADICHS2_B_MARK, VI1_DATA6_VI1_B6_MARK, SD2_CD_MARK,
 	MT0_VCXO_MARK, SPA_TMS_MARK, HSPI_TX1_D_MARK, VI1_DATA7_VI1_B7_MARK,
 	SD2_WP_MARK, MT0_PWM_MARK, SPA_TDI_MARK, HSPI_RX1_D_MARK,
-	VI1_G0_MARK, VI3_DATA0_MARK, DU1_DOTCLKOUT1_MARK, TS_SCK1_MARK,
+	VI1_G0_MARK, VI3_DATA0_MARK, TS_SCK1_MARK,
 	DREQ2_B_MARK, TX2_MARK,	SPA_TDO_MARK, HCTS0_B_MARK,
 	VI1_G1_MARK, VI3_DATA1_MARK, SSI_SCK1_MARK, TS_SDEN1_MARK,
 	DACK2_B_MARK, RX2_MARK, HRTS0_B_MARK,
@@ -649,7 +662,7 @@ enum {
 	PINMUX_MARK_END,
 };
 
-static pinmux_enum_t pinmux_data[] = {
+static const pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA_GP_ALL(), /* PINMUX_DATA(GP_M_N_DATA, GP_M_N_FN...), */
 
 	PINMUX_DATA(AVS1_MARK, FN_AVS1),
@@ -658,6 +671,9 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA(A18_MARK, FN_A18),
 	PINMUX_DATA(A19_MARK, FN_A19),
 
+	PINMUX_DATA(USB_PENC0_MARK, FN_USB_PENC0),
+	PINMUX_DATA(USB_PENC1_MARK, FN_USB_PENC1),
+
 	PINMUX_IPSR_DATA(IP0_2_0, USB_PENC2),
 	PINMUX_IPSR_MODSEL_DATA(IP0_2_0, SCK0, SEL_SCIF0_0),
 	PINMUX_IPSR_DATA(IP0_2_0, PWM1),
@@ -1399,7 +1415,6 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_IPSR_MODSEL_DATA(IP11_23_21, HSPI_RX1_D, SEL_HSPI1_3),
 	PINMUX_IPSR_DATA(IP11_26_24, VI1_G0),
 	PINMUX_IPSR_DATA(IP11_26_24, VI3_DATA0),
-	PINMUX_IPSR_DATA(IP11_26_24, DU1_DOTCLKOUT1),
 	PINMUX_IPSR_DATA(IP11_26_24, TS_SCK1),
 	PINMUX_IPSR_MODSEL_DATA(IP11_26_24, DREQ2_B, SEL_EXBUS2_1),
 	PINMUX_IPSR_DATA(IP11_26_24, TX2),
@@ -1450,280 +1465,1260 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_IPSR_MODSEL_DATA(IP12_17_15, SCK4_B, SEL_SCIF4_1),
 };
 
-static struct pinmux_gpio pinmux_gpios[] = {
+static struct sh_pfc_pin pinmux_pins[] = {
 	PINMUX_GPIO_GP_ALL(),
-	GPIO_FN(AVS1), GPIO_FN(AVS2), GPIO_FN(A17), GPIO_FN(A18),
-	GPIO_FN(A19),
+};
 
-	/* IPSR0 */
-	GPIO_FN(USB_PENC2), GPIO_FN(SCK0), GPIO_FN(PWM1), GPIO_FN(PWMFSW0),
-	GPIO_FN(SCIF_CLK), GPIO_FN(TCLK0_C), GPIO_FN(BS), GPIO_FN(SD1_DAT2),
-	GPIO_FN(MMC0_D2), GPIO_FN(FD2), GPIO_FN(ATADIR0), GPIO_FN(SDSELF),
-	GPIO_FN(HCTS1), GPIO_FN(TX4_C), GPIO_FN(A0), GPIO_FN(SD1_DAT3),
-	GPIO_FN(MMC0_D3), GPIO_FN(FD3), GPIO_FN(A20), GPIO_FN(TX5_D),
-	GPIO_FN(HSPI_TX2_B), GPIO_FN(A21), GPIO_FN(SCK5_D),
-	GPIO_FN(HSPI_CLK2_B), GPIO_FN(A22), GPIO_FN(RX5_D),
-	GPIO_FN(HSPI_RX2_B), GPIO_FN(VI1_R0), GPIO_FN(A23), GPIO_FN(FCLE),
-	GPIO_FN(HSPI_CLK2), GPIO_FN(VI1_R1), GPIO_FN(A24), GPIO_FN(SD1_CD),
-	GPIO_FN(MMC0_D4), GPIO_FN(FD4),	GPIO_FN(HSPI_CS2), GPIO_FN(VI1_R2),
-	GPIO_FN(SSI_WS78_B), GPIO_FN(A25), GPIO_FN(SD1_WP), GPIO_FN(MMC0_D5),
-	GPIO_FN(FD5), GPIO_FN(HSPI_RX2), GPIO_FN(VI1_R3), GPIO_FN(TX5_B),
-	GPIO_FN(SSI_SDATA7_B), GPIO_FN(CTS0_B), GPIO_FN(CLKOUT),
-	GPIO_FN(TX3C_IRDA_TX_C), GPIO_FN(PWM0_B), GPIO_FN(CS0),
-	GPIO_FN(HSPI_CS2_B), GPIO_FN(CS1_A26), GPIO_FN(HSPI_TX2),
-	GPIO_FN(SDSELF_B), GPIO_FN(RD_WR), GPIO_FN(FWE), GPIO_FN(ATAG0),
-	GPIO_FN(VI1_R7), GPIO_FN(HRTS1), GPIO_FN(RX4_C),
+/* - DU0 -------------------------------------------------------------------- */
+static const unsigned int du0_rgb666_pins[] = {
+	/* R[7:2], G[7:2], B[7:2] */
+	188, 187, 186, 185, 184, 183,
+	194, 193, 192, 191, 190, 189,
+	200, 199, 198, 197, 196, 195,
+};
+static const unsigned int du0_rgb666_mux[] = {
+	DU0_DR7_MARK, DU0_DR6_MARK, DU0_DR5_MARK, DU0_DR4_MARK,
+	DU0_DR3_MARK, DU0_DR2_MARK,
+	DU0_DG7_MARK, DU0_DG6_MARK, DU0_DG5_MARK, DU0_DG4_MARK,
+	DU0_DG3_MARK, DU0_DG2_MARK,
+	DU0_DB7_MARK, DU0_DB6_MARK, DU0_DB5_MARK, DU0_DB4_MARK,
+	DU0_DB3_MARK, DU0_DB2_MARK,
+};
+static const unsigned int du0_rgb888_pins[] = {
+	/* R[7:0], G[7:0], B[7:0] */
+	188, 187, 186, 185, 184, 183, 24, 23,
+	194, 193, 192, 191, 190, 189, 26, 25,
+	200, 199, 198, 197, 196, 195, 28, 27,
+};
+static const unsigned int du0_rgb888_mux[] = {
+	DU0_DR7_MARK, DU0_DR6_MARK, DU0_DR5_MARK, DU0_DR4_MARK,
+	DU0_DR3_MARK, DU0_DR2_MARK, DU0_DR1_MARK, DU0_DR0_MARK,
+	DU0_DG7_MARK, DU0_DG6_MARK, DU0_DG5_MARK, DU0_DG4_MARK,
+	DU0_DG3_MARK, DU0_DG2_MARK, DU0_DG1_MARK, DU0_DG0_MARK,
+	DU0_DB7_MARK, DU0_DB6_MARK, DU0_DB5_MARK, DU0_DB4_MARK,
+	DU0_DB3_MARK, DU0_DB2_MARK, DU0_DB1_MARK, DU0_DB0_MARK,
+};
+static const unsigned int du0_clk_in_pins[] = {
+	/* CLKIN */
+	29,
+};
+static const unsigned int du0_clk_in_mux[] = {
+	DU0_DOTCLKIN_MARK,
+};
+static const unsigned int du0_clk_out_0_pins[] = {
+	/* CLKOUT */
+	180,
+};
+static const unsigned int du0_clk_out_0_mux[] = {
+	DU0_DOTCLKOUT0_MARK,
+};
+static const unsigned int du0_clk_out_1_pins[] = {
+	/* CLKOUT */
+	30,
+};
+static const unsigned int du0_clk_out_1_mux[] = {
+	DU0_DOTCLKOUT1_MARK,
+};
+static const unsigned int du0_sync_0_pins[] = {
+	/* VSYNC, HSYNC, DISP */
+	182, 181, 31,
+};
+static const unsigned int du0_sync_0_mux[] = {
+	DU0_EXHSYNC_DU0_HSYNC_MARK, DU0_EXVSYNC_DU0_VSYNC_MARK,
+	DU0_EXODDF_DU0_ODDF_DISP_CDE_MARK
+};
+static const unsigned int du0_sync_1_pins[] = {
+	/* VSYNC, HSYNC, DISP */
+	182, 181, 32,
+};
+static const unsigned int du0_sync_1_mux[] = {
+	DU0_EXHSYNC_DU0_HSYNC_MARK, DU0_EXVSYNC_DU0_VSYNC_MARK,
+	DU0_DISP_MARK
+};
+static const unsigned int du0_oddf_pins[] = {
+	/* ODDF */
+	31,
+};
+static const unsigned int du0_oddf_mux[] = {
+	DU0_EXODDF_DU0_ODDF_DISP_CDE_MARK
+};
+static const unsigned int du0_cde_pins[] = {
+	/* CDE */
+	33,
+};
+static const unsigned int du0_cde_mux[] = {
+	DU0_CDE_MARK
+};
+/* - DU1 -------------------------------------------------------------------- */
+static const unsigned int du1_rgb666_pins[] = {
+	/* R[7:2], G[7:2], B[7:2] */
+	41, 40, 39, 38, 37, 36,
+	49, 48, 47, 46, 45, 44,
+	57, 56, 55, 54, 53, 52,
+};
+static const unsigned int du1_rgb666_mux[] = {
+	DU1_DR7_MARK, DU1_DR6_MARK, DU1_DR5_MARK, DU1_DR4_MARK,
+	DU1_DR3_MARK, DU1_DR2_MARK,
+	DU1_DG7_MARK, DU1_DG6_MARK, DU1_DG5_MARK, DU1_DG4_MARK,
+	DU1_DG3_MARK, DU1_DG2_MARK,
+	DU1_DB7_MARK, DU1_DB6_MARK, DU1_DB5_MARK, DU1_DB4_MARK,
+	DU1_DB3_MARK, DU1_DB2_MARK,
+};
+static const unsigned int du1_rgb888_pins[] = {
+	/* R[7:0], G[7:0], B[7:0] */
+	41, 40, 39, 38, 37, 36, 35, 34,
+	49, 48, 47, 46, 45, 44, 43, 32,
+	57, 56, 55, 54, 53, 52, 51, 50,
+};
+static const unsigned int du1_rgb888_mux[] = {
+	DU1_DR7_MARK, DU1_DR6_MARK, DU1_DR5_MARK, DU1_DR4_MARK,
+	DU1_DR3_MARK, DU1_DR2_MARK, DU1_DR1_MARK, DU1_DR0_MARK,
+	DU1_DG7_MARK, DU1_DG6_MARK, DU1_DG5_MARK, DU1_DG4_MARK,
+	DU1_DG3_MARK, DU1_DG2_MARK, DU1_DG1_MARK, DU1_DG0_MARK,
+	DU1_DB7_MARK, DU1_DB6_MARK, DU1_DB5_MARK, DU1_DB4_MARK,
+	DU1_DB3_MARK, DU1_DB2_MARK, DU1_DB1_MARK, DU1_DB0_MARK,
+};
+static const unsigned int du1_clk_in_pins[] = {
+	/* CLKIN */
+	58,
+};
+static const unsigned int du1_clk_in_mux[] = {
+	DU1_DOTCLKIN_MARK,
+};
+static const unsigned int du1_clk_out_pins[] = {
+	/* CLKOUT */
+	59,
+};
+static const unsigned int du1_clk_out_mux[] = {
+	DU1_DOTCLKOUT_MARK,
+};
+static const unsigned int du1_sync_0_pins[] = {
+	/* VSYNC, HSYNC, DISP */
+	61, 60, 62,
+};
+static const unsigned int du1_sync_0_mux[] = {
+	DU1_EXVSYNC_DU1_VSYNC_MARK, DU1_EXHSYNC_DU1_HSYNC_MARK,
+	DU1_EXODDF_DU1_ODDF_DISP_CDE_MARK
+};
+static const unsigned int du1_sync_1_pins[] = {
+	/* VSYNC, HSYNC, DISP */
+	61, 60, 63,
+};
+static const unsigned int du1_sync_1_mux[] = {
+	DU1_EXVSYNC_DU1_VSYNC_MARK, DU1_EXHSYNC_DU1_HSYNC_MARK,
+	DU1_DISP_MARK
+};
+static const unsigned int du1_oddf_pins[] = {
+	/* ODDF */
+	62,
+};
+static const unsigned int du1_oddf_mux[] = {
+	DU1_EXODDF_DU1_ODDF_DISP_CDE_MARK
+};
+static const unsigned int du1_cde_pins[] = {
+	/* CDE */
+	64,
+};
+static const unsigned int du1_cde_mux[] = {
+	DU1_CDE_MARK
+};
+/* - HSPI0 ------------------------------------------------------------------ */
+static const unsigned int hspi0_pins[] = {
+	/* CLK, CS, RX, TX */
+	150, 151, 153, 152,
+};
+static const unsigned int hspi0_mux[] = {
+	HSPI_CLK0_MARK, HSPI_CS0_MARK, HSPI_RX0_MARK, HSPI_TX0_MARK,
+};
+/* - HSPI1 ------------------------------------------------------------------ */
+static const unsigned int hspi1_pins[] = {
+	/* CLK, CS, RX, TX */
+	63, 58, 64, 62,
+};
+static const unsigned int hspi1_mux[] = {
+	HSPI_CLK1_MARK, HSPI_CS1_MARK, HSPI_RX1_MARK, HSPI_TX1_MARK,
+};
+static const unsigned int hspi1_b_pins[] = {
+	/* CLK, CS, RX, TX */
+	90, 91, 93, 92,
+};
+static const unsigned int hspi1_b_mux[] = {
+	HSPI_CLK1_B_MARK, HSPI_CS1_B_MARK, HSPI_RX1_B_MARK, HSPI_TX1_B_MARK,
+};
+static const unsigned int hspi1_c_pins[] = {
+	/* CLK, CS, RX, TX */
+	141, 142, 144, 143,
+};
+static const unsigned int hspi1_c_mux[] = {
+	HSPI_CLK1_C_MARK, HSPI_CS1_C_MARK, HSPI_RX1_C_MARK, HSPI_TX1_C_MARK,
+};
+static const unsigned int hspi1_d_pins[] = {
+	/* CLK, CS, RX, TX */
+	101, 102, 104, 103,
+};
+static const unsigned int hspi1_d_mux[] = {
+	HSPI_CLK1_D_MARK, HSPI_CS1_D_MARK, HSPI_RX1_D_MARK, HSPI_TX1_D_MARK,
+};
+/* - HSPI2 ------------------------------------------------------------------ */
+static const unsigned int hspi2_pins[] = {
+	/* CLK, CS, RX, TX */
+	9, 10, 11, 14,
+};
+static const unsigned int hspi2_mux[] = {
+	HSPI_CLK2_MARK, HSPI_CS2_MARK, HSPI_RX2_MARK, HSPI_TX2_MARK,
+};
+static const unsigned int hspi2_b_pins[] = {
+	/* CLK, CS, RX, TX */
+	7, 13, 8, 6,
+};
+static const unsigned int hspi2_b_mux[] = {
+	HSPI_CLK2_B_MARK, HSPI_CS2_B_MARK, HSPI_RX2_B_MARK, HSPI_TX2_B_MARK,
+};
+/* - INTC ------------------------------------------------------------------- */
+static const unsigned int intc_irq0_pins[] = {
+	/* IRQ */
+	78,
+};
+static const unsigned int intc_irq0_mux[] = {
+	IRQ0_MARK,
+};
+static const unsigned int intc_irq0_b_pins[] = {
+	/* IRQ */
+	141,
+};
+static const unsigned int intc_irq0_b_mux[] = {
+	IRQ0_B_MARK,
+};
+static const unsigned int intc_irq1_pins[] = {
+	/* IRQ */
+	79,
+};
+static const unsigned int intc_irq1_mux[] = {
+	IRQ1_MARK,
+};
+static const unsigned int intc_irq1_b_pins[] = {
+	/* IRQ */
+	142,
+};
+static const unsigned int intc_irq1_b_mux[] = {
+	IRQ1_B_MARK,
+};
+static const unsigned int intc_irq2_pins[] = {
+	/* IRQ */
+	88,
+};
+static const unsigned int intc_irq2_mux[] = {
+	IRQ2_MARK,
+};
+static const unsigned int intc_irq2_b_pins[] = {
+	/* IRQ */
+	143,
+};
+static const unsigned int intc_irq2_b_mux[] = {
+	IRQ2_B_MARK,
+};
+static const unsigned int intc_irq3_pins[] = {
+	/* IRQ */
+	89,
+};
+static const unsigned int intc_irq3_mux[] = {
+	IRQ3_MARK,
+};
+static const unsigned int intc_irq3_b_pins[] = {
+	/* IRQ */
+	144,
+};
+static const unsigned int intc_irq3_b_mux[] = {
+	IRQ3_B_MARK,
+};
+/* - LSBC ------------------------------------------------------------------- */
+static const unsigned int lbsc_cs0_pins[] = {
+	/* CS */
+	13,
+};
+static const unsigned int lbsc_cs0_mux[] = {
+	CS0_MARK,
+};
+static const unsigned int lbsc_cs1_pins[] = {
+	/* CS */
+	14,
+};
+static const unsigned int lbsc_cs1_mux[] = {
+	CS1_A26_MARK,
+};
+static const unsigned int lbsc_ex_cs0_pins[] = {
+	/* CS */
+	15,
+};
+static const unsigned int lbsc_ex_cs0_mux[] = {
+	EX_CS0_MARK,
+};
+static const unsigned int lbsc_ex_cs1_pins[] = {
+	/* CS */
+	16,
+};
+static const unsigned int lbsc_ex_cs1_mux[] = {
+	EX_CS1_MARK,
+};
+static const unsigned int lbsc_ex_cs2_pins[] = {
+	/* CS */
+	17,
+};
+static const unsigned int lbsc_ex_cs2_mux[] = {
+	EX_CS2_MARK,
+};
+static const unsigned int lbsc_ex_cs3_pins[] = {
+	/* CS */
+	18,
+};
+static const unsigned int lbsc_ex_cs3_mux[] = {
+	EX_CS3_MARK,
+};
+static const unsigned int lbsc_ex_cs4_pins[] = {
+	/* CS */
+	19,
+};
+static const unsigned int lbsc_ex_cs4_mux[] = {
+	EX_CS4_MARK,
+};
+static const unsigned int lbsc_ex_cs5_pins[] = {
+	/* CS */
+	20,
+};
+static const unsigned int lbsc_ex_cs5_mux[] = {
+	EX_CS5_MARK,
+};
+/* - MMCIF ------------------------------------------------------------------ */
+static const unsigned int mmc0_data1_pins[] = {
+	/* D[0] */
+	19,
+};
+static const unsigned int mmc0_data1_mux[] = {
+	MMC0_D0_MARK,
+};
+static const unsigned int mmc0_data4_pins[] = {
+	/* D[0:3] */
+	19, 20, 21, 2,
+};
+static const unsigned int mmc0_data4_mux[] = {
+	MMC0_D0_MARK, MMC0_D1_MARK, MMC0_D2_MARK, MMC0_D3_MARK,
+};
+static const unsigned int mmc0_data8_pins[] = {
+	/* D[0:7] */
+	19, 20, 21, 2, 10, 11, 15, 16,
+};
+static const unsigned int mmc0_data8_mux[] = {
+	MMC0_D0_MARK, MMC0_D1_MARK, MMC0_D2_MARK, MMC0_D3_MARK,
+	MMC0_D4_MARK, MMC0_D5_MARK, MMC0_D6_MARK, MMC0_D7_MARK,
+};
+static const unsigned int mmc0_ctrl_pins[] = {
+	/* CMD, CLK */
+	18, 17,
+};
+static const unsigned int mmc0_ctrl_mux[] = {
+	MMC0_CMD_MARK, MMC0_CLK_MARK,
+};
+static const unsigned int mmc1_data1_pins[] = {
+	/* D[0] */
+	72,
+};
+static const unsigned int mmc1_data1_mux[] = {
+	MMC1_D0_MARK,
+};
+static const unsigned int mmc1_data4_pins[] = {
+	/* D[0:3] */
+	72, 73, 74, 75,
+};
+static const unsigned int mmc1_data4_mux[] = {
+	MMC1_D0_MARK, MMC1_D1_MARK, MMC1_D2_MARK, MMC1_D3_MARK,
+};
+static const unsigned int mmc1_data8_pins[] = {
+	/* D[0:7] */
+	72, 73, 74, 75, 76, 77, 80, 81,
+};
+static const unsigned int mmc1_data8_mux[] = {
+	MMC1_D0_MARK, MMC1_D1_MARK, MMC1_D2_MARK, MMC1_D3_MARK,
+	MMC1_D4_MARK, MMC1_D5_MARK, MMC1_D6_MARK, MMC1_D7_MARK,
+};
+static const unsigned int mmc1_ctrl_pins[] = {
+	/* CMD, CLK */
+	68, 65,
+};
+static const unsigned int mmc1_ctrl_mux[] = {
+	MMC1_CMD_MARK, MMC1_CLK_MARK,
+};
+/* - SCIF0 ------------------------------------------------------------------ */
+static const unsigned int scif0_data_pins[] = {
+	/* RXD, TXD */
+	153, 152,
+};
+static const unsigned int scif0_data_mux[] = {
+	RX0_MARK, TX0_MARK,
+};
+static const unsigned int scif0_clk_pins[] = {
+	/* SCK */
+	156,
+};
+static const unsigned int scif0_clk_mux[] = {
+	SCK0_MARK,
+};
+static const unsigned int scif0_ctrl_pins[] = {
+	/* RTS, CTS */
+	151, 150,
+};
+static const unsigned int scif0_ctrl_mux[] = {
+	RTS0_TANS_MARK, CTS0_MARK,
+};
+static const unsigned int scif0_data_b_pins[] = {
+	/* RXD, TXD */
+	20, 19,
+};
+static const unsigned int scif0_data_b_mux[] = {
+	RX0_B_MARK, TX0_B_MARK,
+};
+static const unsigned int scif0_clk_b_pins[] = {
+	/* SCK */
+	33,
+};
+static const unsigned int scif0_clk_b_mux[] = {
+	SCK0_B_MARK,
+};
+static const unsigned int scif0_ctrl_b_pins[] = {
+	/* RTS, CTS */
+	18, 11,
+};
+static const unsigned int scif0_ctrl_b_mux[] = {
+	RTS0_B_TANS_B_MARK, CTS0_B_MARK,
+};
+static const unsigned int scif0_data_c_pins[] = {
+	/* RXD, TXD */
+	146, 147,
+};
+static const unsigned int scif0_data_c_mux[] = {
+	RX0_C_MARK, TX0_C_MARK,
+};
+static const unsigned int scif0_clk_c_pins[] = {
+	/* SCK */
+	145,
+};
+static const unsigned int scif0_clk_c_mux[] = {
+	SCK0_C_MARK,
+};
+static const unsigned int scif0_ctrl_c_pins[] = {
+	/* RTS, CTS */
+	149, 148,
+};
+static const unsigned int scif0_ctrl_c_mux[] = {
+	RTS0_C_TANS_C_MARK, CTS0_C_MARK,
+};
+static const unsigned int scif0_data_d_pins[] = {
+	/* RXD, TXD */
+	43, 42,
+};
+static const unsigned int scif0_data_d_mux[] = {
+	RX0_D_MARK, TX0_D_MARK,
+};
+static const unsigned int scif0_clk_d_pins[] = {
+	/* SCK */
+	50,
+};
+static const unsigned int scif0_clk_d_mux[] = {
+	SCK0_D_MARK,
+};
+static const unsigned int scif0_ctrl_d_pins[] = {
+	/* RTS, CTS */
+	51, 35,
+};
+static const unsigned int scif0_ctrl_d_mux[] = {
+	RTS0_D_TANS_D_MARK, CTS0_D_MARK,
+};
+/* - SCIF1 ------------------------------------------------------------------ */
+static const unsigned int scif1_data_pins[] = {
+	/* RXD, TXD */
+	149, 148,
+};
+static const unsigned int scif1_data_mux[] = {
+	RX1_MARK, TX1_MARK,
+};
+static const unsigned int scif1_clk_pins[] = {
+	/* SCK */
+	145,
+};
+static const unsigned int scif1_clk_mux[] = {
+	SCK1_MARK,
+};
+static const unsigned int scif1_ctrl_pins[] = {
+	/* RTS, CTS */
+	147, 146,
+};
+static const unsigned int scif1_ctrl_mux[] = {
+	RTS1_TANS_MARK, CTS1_MARK,
+};
+static const unsigned int scif1_data_b_pins[] = {
+	/* RXD, TXD */
+	117, 114,
+};
+static const unsigned int scif1_data_b_mux[] = {
+	RX1_B_MARK, TX1_B_MARK,
+};
+static const unsigned int scif1_clk_b_pins[] = {
+	/* SCK */
+	113,
+};
+static const unsigned int scif1_clk_b_mux[] = {
+	SCK1_B_MARK,
+};
+static const unsigned int scif1_ctrl_b_pins[] = {
+	/* RTS, CTS */
+	115, 116,
+};
+static const unsigned int scif1_ctrl_b_mux[] = {
+	RTS1_B_TANS_B_MARK, CTS1_B_MARK,
+};
+static const unsigned int scif1_data_c_pins[] = {
+	/* RXD, TXD */
+	67, 66,
+};
+static const unsigned int scif1_data_c_mux[] = {
+	RX1_C_MARK, TX1_C_MARK,
+};
+static const unsigned int scif1_clk_c_pins[] = {
+	/* SCK */
+	86,
+};
+static const unsigned int scif1_clk_c_mux[] = {
+	SCK1_C_MARK,
+};
+static const unsigned int scif1_ctrl_c_pins[] = {
+	/* RTS, CTS */
+	69, 68,
+};
+static const unsigned int scif1_ctrl_c_mux[] = {
+	RTS1_C_TANS_C_MARK, CTS1_C_MARK,
+};
+/* - SCIF2 ------------------------------------------------------------------ */
+static const unsigned int scif2_data_pins[] = {
+	/* RXD, TXD */
+	106, 105,
+};
+static const unsigned int scif2_data_mux[] = {
+	RX2_MARK, TX2_MARK,
+};
+static const unsigned int scif2_clk_pins[] = {
+	/* SCK */
+	107,
+};
+static const unsigned int scif2_clk_mux[] = {
+	SCK2_MARK,
+};
+static const unsigned int scif2_data_b_pins[] = {
+	/* RXD, TXD */
+	120, 119,
+};
+static const unsigned int scif2_data_b_mux[] = {
+	RX2_B_MARK, TX2_B_MARK,
+};
+static const unsigned int scif2_clk_b_pins[] = {
+	/* SCK */
+	118,
+};
+static const unsigned int scif2_clk_b_mux[] = {
+	SCK2_B_MARK,
+};
+static const unsigned int scif2_data_c_pins[] = {
+	/* RXD, TXD */
+	33, 31,
+};
+static const unsigned int scif2_data_c_mux[] = {
+	RX2_C_MARK, TX2_C_MARK,
+};
+static const unsigned int scif2_clk_c_pins[] = {
+	/* SCK */
+	32,
+};
+static const unsigned int scif2_clk_c_mux[] = {
+	SCK2_C_MARK,
+};
+static const unsigned int scif2_data_d_pins[] = {
+	/* RXD, TXD */
+	64, 62,
+};
+static const unsigned int scif2_data_d_mux[] = {
+	RX2_D_MARK, TX2_D_MARK,
+};
+static const unsigned int scif2_clk_d_pins[] = {
+	/* SCK */
+	63,
+};
+static const unsigned int scif2_clk_d_mux[] = {
+	SCK2_D_MARK,
+};
+static const unsigned int scif2_data_e_pins[] = {
+	/* RXD, TXD */
+	20, 19,
+};
+static const unsigned int scif2_data_e_mux[] = {
+	RX2_E_MARK, TX2_E_MARK,
+};
+/* - SCIF3 ------------------------------------------------------------------ */
+static const unsigned int scif3_data_pins[] = {
+	/* RXD, TXD */
+	137, 136,
+};
+static const unsigned int scif3_data_mux[] = {
+	RX3_IRDA_RX_MARK, TX3_IRDA_TX_MARK,
+};
+static const unsigned int scif3_clk_pins[] = {
+	/* SCK */
+	135,
+};
+static const unsigned int scif3_clk_mux[] = {
+	SCK3_MARK,
+};
 
-	/* IPSR1 */
-	GPIO_FN(EX_CS0), GPIO_FN(RX3_C_IRDA_RX_C), GPIO_FN(MMC0_D6),
-	GPIO_FN(FD6), GPIO_FN(EX_CS1), GPIO_FN(MMC0_D7), GPIO_FN(FD7),
-	GPIO_FN(EX_CS2), GPIO_FN(SD1_CLK), GPIO_FN(MMC0_CLK), GPIO_FN(FALE),
-	GPIO_FN(ATACS00), GPIO_FN(EX_CS3), GPIO_FN(SD1_CMD), GPIO_FN(MMC0_CMD),
-	GPIO_FN(FRE), GPIO_FN(ATACS10), GPIO_FN(VI1_R4), GPIO_FN(RX5_B),
-	GPIO_FN(HSCK1), GPIO_FN(SSI_SDATA8_B), GPIO_FN(RTS0_B_TANS_B),
-	GPIO_FN(SSI_SDATA9), GPIO_FN(EX_CS4), GPIO_FN(SD1_DAT0),
-	GPIO_FN(MMC0_D0), GPIO_FN(FD0), GPIO_FN(ATARD0), GPIO_FN(VI1_R5),
-	GPIO_FN(SCK5_B), GPIO_FN(HTX1), GPIO_FN(TX2_E), GPIO_FN(TX0_B),
-	GPIO_FN(SSI_SCK9), GPIO_FN(EX_CS5), GPIO_FN(SD1_DAT1),
-	GPIO_FN(MMC0_D1), GPIO_FN(FD1),	GPIO_FN(ATAWR0), GPIO_FN(VI1_R6),
-	GPIO_FN(HRX1), GPIO_FN(RX2_E), GPIO_FN(RX0_B), GPIO_FN(SSI_WS9),
-	GPIO_FN(MLB_CLK), GPIO_FN(PWM2), GPIO_FN(SCK4), GPIO_FN(MLB_SIG),
-	GPIO_FN(PWM3), GPIO_FN(TX4), GPIO_FN(MLB_DAT), GPIO_FN(PWM4),
-	GPIO_FN(RX4), GPIO_FN(HTX0), GPIO_FN(TX1), GPIO_FN(SDATA),
-	GPIO_FN(CTS0_C), GPIO_FN(SUB_TCK), GPIO_FN(CC5_STATE2),
-	GPIO_FN(CC5_STATE10), GPIO_FN(CC5_STATE18), GPIO_FN(CC5_STATE26),
-	GPIO_FN(CC5_STATE34),
+static const unsigned int scif3_data_b_pins[] = {
+	/* RXD, TXD */
+	64, 62,
+};
+static const unsigned int scif3_data_b_mux[] = {
+	RX3_B_IRDA_RX_B_MARK, TX3_B_IRDA_TX_B_MARK,
+};
+static const unsigned int scif3_data_c_pins[] = {
+	/* RXD, TXD */
+	15, 12,
+};
+static const unsigned int scif3_data_c_mux[] = {
+	RX3_C_IRDA_RX_C_MARK, TX3C_IRDA_TX_C_MARK,
+};
+static const unsigned int scif3_data_d_pins[] = {
+	/* RXD, TXD */
+	30, 29,
+};
+static const unsigned int scif3_data_d_mux[] = {
+	RX3_D_IRDA_RX_D_MARK, TX3_D_IRDA_TX_D_MARK,
+};
+static const unsigned int scif3_data_e_pins[] = {
+	/* RXD, TXD */
+	35, 34,
+};
+static const unsigned int scif3_data_e_mux[] = {
+	RX3_E_IRDA_RX_E_MARK, TX3_E_IRDA_TX_E_MARK,
+};
+static const unsigned int scif3_clk_e_pins[] = {
+	/* SCK */
+	42,
+};
+static const unsigned int scif3_clk_e_mux[] = {
+	SCK3_E_MARK,
+};
+/* - SCIF4 ------------------------------------------------------------------ */
+static const unsigned int scif4_data_pins[] = {
+	/* RXD, TXD */
+	123, 122,
+};
+static const unsigned int scif4_data_mux[] = {
+	RX4_MARK, TX4_MARK,
+};
+static const unsigned int scif4_clk_pins[] = {
+	/* SCK */
+	121,
+};
+static const unsigned int scif4_clk_mux[] = {
+	SCK4_MARK,
+};
+static const unsigned int scif4_data_b_pins[] = {
+	/* RXD, TXD */
+	111, 110,
+};
+static const unsigned int scif4_data_b_mux[] = {
+	RX4_B_MARK, TX4_B_MARK,
+};
+static const unsigned int scif4_clk_b_pins[] = {
+	/* SCK */
+	112,
+};
+static const unsigned int scif4_clk_b_mux[] = {
+	SCK4_B_MARK,
+};
+static const unsigned int scif4_data_c_pins[] = {
+	/* RXD, TXD */
+	22, 21,
+};
+static const unsigned int scif4_data_c_mux[] = {
+	RX4_C_MARK, TX4_C_MARK,
+};
+static const unsigned int scif4_data_d_pins[] = {
+	/* RXD, TXD */
+	69, 68,
+};
+static const unsigned int scif4_data_d_mux[] = {
+	RX4_D_MARK, TX4_D_MARK,
+};
+/* - SCIF5 ------------------------------------------------------------------ */
+static const unsigned int scif5_data_pins[] = {
+	/* RXD, TXD */
+	51, 50,
+};
+static const unsigned int scif5_data_mux[] = {
+	RX5_MARK, TX5_MARK,
+};
+static const unsigned int scif5_clk_pins[] = {
+	/* SCK */
+	43,
+};
+static const unsigned int scif5_clk_mux[] = {
+	SCK5_MARK,
+};
+static const unsigned int scif5_data_b_pins[] = {
+	/* RXD, TXD */
+	18, 11,
+};
+static const unsigned int scif5_data_b_mux[] = {
+	RX5_B_MARK, TX5_B_MARK,
+};
+static const unsigned int scif5_clk_b_pins[] = {
+	/* SCK */
+	19,
+};
+static const unsigned int scif5_clk_b_mux[] = {
+	SCK5_B_MARK,
+};
+static const unsigned int scif5_data_c_pins[] = {
+	/* RXD, TXD */
+	24, 23,
+};
+static const unsigned int scif5_data_c_mux[] = {
+	RX5_C_MARK, TX5_C_MARK,
+};
+static const unsigned int scif5_clk_c_pins[] = {
+	/* SCK */
+	28,
+};
+static const unsigned int scif5_clk_c_mux[] = {
+	SCK5_C_MARK,
+};
+static const unsigned int scif5_data_d_pins[] = {
+	/* RXD, TXD */
+	8, 6,
+};
+static const unsigned int scif5_data_d_mux[] = {
+	RX5_D_MARK, TX5_D_MARK,
+};
+static const unsigned int scif5_clk_d_pins[] = {
+	/* SCK */
+	7,
+};
+static const unsigned int scif5_clk_d_mux[] = {
+	SCK5_D_MARK,
+};
+/* - SDHI0 ------------------------------------------------------------------ */
+static const unsigned int sdhi0_data1_pins[] = {
+	/* D0 */
+	117,
+};
+static const unsigned int sdhi0_data1_mux[] = {
+	SD0_DAT0_MARK,
+};
+static const unsigned int sdhi0_data4_pins[] = {
+	/* D[0:3] */
+	117, 118, 119, 120,
+};
+static const unsigned int sdhi0_data4_mux[] = {
+	SD0_DAT0_MARK, SD0_DAT1_MARK, SD0_DAT2_MARK, SD0_DAT3_MARK,
+};
+static const unsigned int sdhi0_ctrl_pins[] = {
+	/* CMD, CLK */
+	114, 113,
+};
+static const unsigned int sdhi0_ctrl_mux[] = {
+	SD0_CMD_MARK, SD0_CLK_MARK,
+};
+static const unsigned int sdhi0_cd_pins[] = {
+	/* CD */
+	115,
+};
+static const unsigned int sdhi0_cd_mux[] = {
+	SD0_CD_MARK,
+};
+static const unsigned int sdhi0_wp_pins[] = {
+	/* WP */
+	116,
+};
+static const unsigned int sdhi0_wp_mux[] = {
+	SD0_WP_MARK,
+};
+/* - SDHI1 ------------------------------------------------------------------ */
+static const unsigned int sdhi1_data1_pins[] = {
+	/* D0 */
+	19,
+};
+static const unsigned int sdhi1_data1_mux[] = {
+	SD1_DAT0_MARK,
+};
+static const unsigned int sdhi1_data4_pins[] = {
+	/* D[0:3] */
+	19, 20, 21, 2,
+};
+static const unsigned int sdhi1_data4_mux[] = {
+	SD1_DAT0_MARK, SD1_DAT1_MARK, SD1_DAT2_MARK, SD1_DAT3_MARK,
+};
+static const unsigned int sdhi1_ctrl_pins[] = {
+	/* CMD, CLK */
+	18, 17,
+};
+static const unsigned int sdhi1_ctrl_mux[] = {
+	SD1_CMD_MARK, SD1_CLK_MARK,
+};
+static const unsigned int sdhi1_cd_pins[] = {
+	/* CD */
+	10,
+};
+static const unsigned int sdhi1_cd_mux[] = {
+	SD1_CD_MARK,
+};
+static const unsigned int sdhi1_wp_pins[] = {
+	/* WP */
+	11,
+};
+static const unsigned int sdhi1_wp_mux[] = {
+	SD1_WP_MARK,
+};
+/* - SDHI2 ------------------------------------------------------------------ */
+static const unsigned int sdhi2_data1_pins[] = {
+	/* D0 */
+	97,
+};
+static const unsigned int sdhi2_data1_mux[] = {
+	SD2_DAT0_MARK,
+};
+static const unsigned int sdhi2_data4_pins[] = {
+	/* D[0:3] */
+	97, 98, 99, 100,
+};
+static const unsigned int sdhi2_data4_mux[] = {
+	SD2_DAT0_MARK, SD2_DAT1_MARK, SD2_DAT2_MARK, SD2_DAT3_MARK,
+};
+static const unsigned int sdhi2_ctrl_pins[] = {
+	/* CMD, CLK */
+	102, 101,
+};
+static const unsigned int sdhi2_ctrl_mux[] = {
+	SD2_CMD_MARK, SD2_CLK_MARK,
+};
+static const unsigned int sdhi2_cd_pins[] = {
+	/* CD */
+	103,
+};
+static const unsigned int sdhi2_cd_mux[] = {
+	SD2_CD_MARK,
+};
+static const unsigned int sdhi2_wp_pins[] = {
+	/* WP */
+	104,
+};
+static const unsigned int sdhi2_wp_mux[] = {
+	SD2_WP_MARK,
+};
+/* - SDHI3 ------------------------------------------------------------------ */
+static const unsigned int sdhi3_data1_pins[] = {
+	/* D0 */
+	50,
+};
+static const unsigned int sdhi3_data1_mux[] = {
+	SD3_DAT0_MARK,
+};
+static const unsigned int sdhi3_data4_pins[] = {
+	/* D[0:3] */
+	50, 51, 52, 53,
+};
+static const unsigned int sdhi3_data4_mux[] = {
+	SD3_DAT0_MARK, SD3_DAT1_MARK, SD3_DAT2_MARK, SD3_DAT3_MARK,
+};
+static const unsigned int sdhi3_ctrl_pins[] = {
+	/* CMD, CLK */
+	35, 34,
+};
+static const unsigned int sdhi3_ctrl_mux[] = {
+	SD3_CMD_MARK, SD3_CLK_MARK,
+};
+static const unsigned int sdhi3_cd_pins[] = {
+	/* CD */
+	62,
+};
+static const unsigned int sdhi3_cd_mux[] = {
+	SD3_CD_MARK,
+};
+static const unsigned int sdhi3_wp_pins[] = {
+	/* WP */
+	64,
+};
+static const unsigned int sdhi3_wp_mux[] = {
+	SD3_WP_MARK,
+};
+/* - USB0 ------------------------------------------------------------------- */
+static const unsigned int usb0_pins[] = {
+	/* OVC */
+	150, 154,
+};
+static const unsigned int usb0_mux[] = {
+	USB_OVC0_MARK, USB_PENC0_MARK,
+};
+/* - USB1 ------------------------------------------------------------------- */
+static const unsigned int usb1_pins[] = {
+	/* OVC */
+	152, 155,
+};
+static const unsigned int usb1_mux[] = {
+	USB_OVC1_MARK, USB_PENC1_MARK,
+};
+/* - USB2 ------------------------------------------------------------------- */
+static const unsigned int usb2_pins[] = {
+	/* OVC, PENC */
+	125, 156,
+};
+static const unsigned int usb2_mux[] = {
+	USB_OVC2_MARK, USB_PENC2_MARK,
+};
 
-	/* IPSR2 */
-	GPIO_FN(HRX0), GPIO_FN(RX1), GPIO_FN(SCKZ), GPIO_FN(RTS0_C_TANS_C),
-	GPIO_FN(SUB_TDI), GPIO_FN(CC5_STATE3), GPIO_FN(CC5_STATE11),
-	GPIO_FN(CC5_STATE19), GPIO_FN(CC5_STATE27), GPIO_FN(CC5_STATE35),
-	GPIO_FN(HSCK0), GPIO_FN(SCK1), GPIO_FN(MTS), GPIO_FN(PWM5),
-	GPIO_FN(SCK0_C), GPIO_FN(SSI_SDATA9_B), GPIO_FN(SUB_TDO),
-	GPIO_FN(CC5_STATE0), GPIO_FN(CC5_STATE8), GPIO_FN(CC5_STATE16),
-	GPIO_FN(CC5_STATE24), GPIO_FN(CC5_STATE32), GPIO_FN(HCTS0),
-	GPIO_FN(CTS1), GPIO_FN(STM), GPIO_FN(PWM0_D), GPIO_FN(RX0_C),
-	GPIO_FN(SCIF_CLK_C), GPIO_FN(SUB_TRST), GPIO_FN(TCLK1_B),
-	GPIO_FN(CC5_OSCOUT), GPIO_FN(HRTS0), GPIO_FN(RTS1_TANS),
-	GPIO_FN(MDATA), GPIO_FN(TX0_C), GPIO_FN(SUB_TMS), GPIO_FN(CC5_STATE1),
-	GPIO_FN(CC5_STATE9), GPIO_FN(CC5_STATE17), GPIO_FN(CC5_STATE25),
-	GPIO_FN(CC5_STATE33), GPIO_FN(DU0_DR0), GPIO_FN(LCDOUT0),
-	GPIO_FN(DREQ0), GPIO_FN(GPS_CLK_B), GPIO_FN(AUDATA0),
-	GPIO_FN(TX5_C), GPIO_FN(DU0_DR1), GPIO_FN(LCDOUT1), GPIO_FN(DACK0),
-	GPIO_FN(DRACK0), GPIO_FN(GPS_SIGN_B), GPIO_FN(AUDATA1), GPIO_FN(RX5_C),
-	GPIO_FN(DU0_DR2), GPIO_FN(LCDOUT2), GPIO_FN(DU0_DR3), GPIO_FN(LCDOUT3),
-	GPIO_FN(DU0_DR4), GPIO_FN(LCDOUT4), GPIO_FN(DU0_DR5), GPIO_FN(LCDOUT5),
-	GPIO_FN(DU0_DR6), GPIO_FN(LCDOUT6), GPIO_FN(DU0_DR7), GPIO_FN(LCDOUT7),
-	GPIO_FN(DU0_DG0), GPIO_FN(LCDOUT8), GPIO_FN(DREQ1), GPIO_FN(SCL2),
-	GPIO_FN(AUDATA2),
+static const struct sh_pfc_pin_group pinmux_groups[] = {
+	SH_PFC_PIN_GROUP(du0_rgb666),
+	SH_PFC_PIN_GROUP(du0_rgb888),
+	SH_PFC_PIN_GROUP(du0_clk_in),
+	SH_PFC_PIN_GROUP(du0_clk_out_0),
+	SH_PFC_PIN_GROUP(du0_clk_out_1),
+	SH_PFC_PIN_GROUP(du0_sync_0),
+	SH_PFC_PIN_GROUP(du0_sync_1),
+	SH_PFC_PIN_GROUP(du0_oddf),
+	SH_PFC_PIN_GROUP(du0_cde),
+	SH_PFC_PIN_GROUP(du1_rgb666),
+	SH_PFC_PIN_GROUP(du1_rgb888),
+	SH_PFC_PIN_GROUP(du1_clk_in),
+	SH_PFC_PIN_GROUP(du1_clk_out),
+	SH_PFC_PIN_GROUP(du1_sync_0),
+	SH_PFC_PIN_GROUP(du1_sync_1),
+	SH_PFC_PIN_GROUP(du1_oddf),
+	SH_PFC_PIN_GROUP(du1_cde),
+	SH_PFC_PIN_GROUP(hspi0),
+	SH_PFC_PIN_GROUP(hspi1),
+	SH_PFC_PIN_GROUP(hspi1_b),
+	SH_PFC_PIN_GROUP(hspi1_c),
+	SH_PFC_PIN_GROUP(hspi1_d),
+	SH_PFC_PIN_GROUP(hspi2),
+	SH_PFC_PIN_GROUP(hspi2_b),
+	SH_PFC_PIN_GROUP(intc_irq0),
+	SH_PFC_PIN_GROUP(intc_irq0_b),
+	SH_PFC_PIN_GROUP(intc_irq1),
+	SH_PFC_PIN_GROUP(intc_irq1_b),
+	SH_PFC_PIN_GROUP(intc_irq2),
+	SH_PFC_PIN_GROUP(intc_irq2_b),
+	SH_PFC_PIN_GROUP(intc_irq3),
+	SH_PFC_PIN_GROUP(intc_irq3_b),
+	SH_PFC_PIN_GROUP(lbsc_cs0),
+	SH_PFC_PIN_GROUP(lbsc_cs1),
+	SH_PFC_PIN_GROUP(lbsc_ex_cs0),
+	SH_PFC_PIN_GROUP(lbsc_ex_cs1),
+	SH_PFC_PIN_GROUP(lbsc_ex_cs2),
+	SH_PFC_PIN_GROUP(lbsc_ex_cs3),
+	SH_PFC_PIN_GROUP(lbsc_ex_cs4),
+	SH_PFC_PIN_GROUP(lbsc_ex_cs5),
+	SH_PFC_PIN_GROUP(mmc0_data1),
+	SH_PFC_PIN_GROUP(mmc0_data4),
+	SH_PFC_PIN_GROUP(mmc0_data8),
+	SH_PFC_PIN_GROUP(mmc0_ctrl),
+	SH_PFC_PIN_GROUP(mmc1_data1),
+	SH_PFC_PIN_GROUP(mmc1_data4),
+	SH_PFC_PIN_GROUP(mmc1_data8),
+	SH_PFC_PIN_GROUP(mmc1_ctrl),
+	SH_PFC_PIN_GROUP(scif0_data),
+	SH_PFC_PIN_GROUP(scif0_clk),
+	SH_PFC_PIN_GROUP(scif0_ctrl),
+	SH_PFC_PIN_GROUP(scif0_data_b),
+	SH_PFC_PIN_GROUP(scif0_clk_b),
+	SH_PFC_PIN_GROUP(scif0_ctrl_b),
+	SH_PFC_PIN_GROUP(scif0_data_c),
+	SH_PFC_PIN_GROUP(scif0_clk_c),
+	SH_PFC_PIN_GROUP(scif0_ctrl_c),
+	SH_PFC_PIN_GROUP(scif0_data_d),
+	SH_PFC_PIN_GROUP(scif0_clk_d),
+	SH_PFC_PIN_GROUP(scif0_ctrl_d),
+	SH_PFC_PIN_GROUP(scif1_data),
+	SH_PFC_PIN_GROUP(scif1_clk),
+	SH_PFC_PIN_GROUP(scif1_ctrl),
+	SH_PFC_PIN_GROUP(scif1_data_b),
+	SH_PFC_PIN_GROUP(scif1_clk_b),
+	SH_PFC_PIN_GROUP(scif1_ctrl_b),
+	SH_PFC_PIN_GROUP(scif1_data_c),
+	SH_PFC_PIN_GROUP(scif1_clk_c),
+	SH_PFC_PIN_GROUP(scif1_ctrl_c),
+	SH_PFC_PIN_GROUP(scif2_data),
+	SH_PFC_PIN_GROUP(scif2_clk),
+	SH_PFC_PIN_GROUP(scif2_data_b),
+	SH_PFC_PIN_GROUP(scif2_clk_b),
+	SH_PFC_PIN_GROUP(scif2_data_c),
+	SH_PFC_PIN_GROUP(scif2_clk_c),
+	SH_PFC_PIN_GROUP(scif2_data_d),
+	SH_PFC_PIN_GROUP(scif2_clk_d),
+	SH_PFC_PIN_GROUP(scif2_data_e),
+	SH_PFC_PIN_GROUP(scif3_data),
+	SH_PFC_PIN_GROUP(scif3_clk),
+	SH_PFC_PIN_GROUP(scif3_data_b),
+	SH_PFC_PIN_GROUP(scif3_data_c),
+	SH_PFC_PIN_GROUP(scif3_data_d),
+	SH_PFC_PIN_GROUP(scif3_data_e),
+	SH_PFC_PIN_GROUP(scif3_clk_e),
+	SH_PFC_PIN_GROUP(scif4_data),
+	SH_PFC_PIN_GROUP(scif4_clk),
+	SH_PFC_PIN_GROUP(scif4_data_b),
+	SH_PFC_PIN_GROUP(scif4_clk_b),
+	SH_PFC_PIN_GROUP(scif4_data_c),
+	SH_PFC_PIN_GROUP(scif4_data_d),
+	SH_PFC_PIN_GROUP(scif5_data),
+	SH_PFC_PIN_GROUP(scif5_clk),
+	SH_PFC_PIN_GROUP(scif5_data_b),
+	SH_PFC_PIN_GROUP(scif5_clk_b),
+	SH_PFC_PIN_GROUP(scif5_data_c),
+	SH_PFC_PIN_GROUP(scif5_clk_c),
+	SH_PFC_PIN_GROUP(scif5_data_d),
+	SH_PFC_PIN_GROUP(scif5_clk_d),
+	SH_PFC_PIN_GROUP(sdhi0_data1),
+	SH_PFC_PIN_GROUP(sdhi0_data4),
+	SH_PFC_PIN_GROUP(sdhi0_ctrl),
+	SH_PFC_PIN_GROUP(sdhi0_cd),
+	SH_PFC_PIN_GROUP(sdhi0_wp),
+	SH_PFC_PIN_GROUP(sdhi1_data1),
+	SH_PFC_PIN_GROUP(sdhi1_data4),
+	SH_PFC_PIN_GROUP(sdhi1_ctrl),
+	SH_PFC_PIN_GROUP(sdhi1_cd),
+	SH_PFC_PIN_GROUP(sdhi1_wp),
+	SH_PFC_PIN_GROUP(sdhi2_data1),
+	SH_PFC_PIN_GROUP(sdhi2_data4),
+	SH_PFC_PIN_GROUP(sdhi2_ctrl),
+	SH_PFC_PIN_GROUP(sdhi2_cd),
+	SH_PFC_PIN_GROUP(sdhi2_wp),
+	SH_PFC_PIN_GROUP(sdhi3_data1),
+	SH_PFC_PIN_GROUP(sdhi3_data4),
+	SH_PFC_PIN_GROUP(sdhi3_ctrl),
+	SH_PFC_PIN_GROUP(sdhi3_cd),
+	SH_PFC_PIN_GROUP(sdhi3_wp),
+	SH_PFC_PIN_GROUP(usb0),
+	SH_PFC_PIN_GROUP(usb1),
+	SH_PFC_PIN_GROUP(usb2),
+};
 
-	/* IPSR3 */
-	GPIO_FN(DU0_DG1), GPIO_FN(LCDOUT9), GPIO_FN(DACK1), GPIO_FN(SDA2),
-	GPIO_FN(AUDATA3), GPIO_FN(DU0_DG2), GPIO_FN(LCDOUT10),
-	GPIO_FN(DU0_DG3), GPIO_FN(LCDOUT11), GPIO_FN(DU0_DG4),
-	GPIO_FN(LCDOUT12), GPIO_FN(DU0_DG5), GPIO_FN(LCDOUT13),
-	GPIO_FN(DU0_DG6), GPIO_FN(LCDOUT14), GPIO_FN(DU0_DG7),
-	GPIO_FN(LCDOUT15), GPIO_FN(DU0_DB0), GPIO_FN(LCDOUT16),
-	GPIO_FN(EX_WAIT1), GPIO_FN(SCL1), GPIO_FN(TCLK1), GPIO_FN(AUDATA4),
-	GPIO_FN(DU0_DB1), GPIO_FN(LCDOUT17), GPIO_FN(EX_WAIT2), GPIO_FN(SDA1),
-	GPIO_FN(GPS_MAG_B), GPIO_FN(AUDATA5), GPIO_FN(SCK5_C),
-	GPIO_FN(DU0_DB2), GPIO_FN(LCDOUT18), GPIO_FN(DU0_DB3),
-	GPIO_FN(LCDOUT19), GPIO_FN(DU0_DB4), GPIO_FN(LCDOUT20),
-	GPIO_FN(DU0_DB5), GPIO_FN(LCDOUT21), GPIO_FN(DU0_DB6),
-	GPIO_FN(LCDOUT22), GPIO_FN(DU0_DB7), GPIO_FN(LCDOUT23),
-	GPIO_FN(DU0_DOTCLKIN), GPIO_FN(QSTVA_QVS), GPIO_FN(TX3_D_IRDA_TX_D),
-	GPIO_FN(SCL3_B), GPIO_FN(DU0_DOTCLKOUT0), GPIO_FN(QCLK),
-	GPIO_FN(DU0_DOTCLKOUT1), GPIO_FN(QSTVB_QVE), GPIO_FN(RX3_D_IRDA_RX_D),
-	GPIO_FN(SDA3_B), GPIO_FN(SDA2_C), GPIO_FN(DACK0_B), GPIO_FN(DRACK0_B),
-	GPIO_FN(DU0_EXHSYNC_DU0_HSYNC), GPIO_FN(QSTH_QHS),
-	GPIO_FN(DU0_EXVSYNC_DU0_VSYNC), GPIO_FN(QSTB_QHE),
-	GPIO_FN(DU0_EXODDF_DU0_ODDF_DISP_CDE), GPIO_FN(QCPV_QDE),
-	GPIO_FN(CAN1_TX), GPIO_FN(TX2_C), GPIO_FN(SCL2_C), GPIO_FN(REMOCON),
+static const char * const du0_groups[] = {
+	"du0_rgb666",
+	"du0_rgb888",
+	"du0_clk_in",
+	"du0_clk_out_0",
+	"du0_clk_out_1",
+	"du0_sync_0",
+	"du0_sync_1",
+	"du0_oddf",
+	"du0_cde",
+};
 
-	/* IPSR4 */
-	GPIO_FN(DU0_DISP), GPIO_FN(QPOLA), GPIO_FN(CAN_CLK_C), GPIO_FN(SCK2_C),
-	GPIO_FN(DU0_CDE), GPIO_FN(QPOLB), GPIO_FN(CAN1_RX), GPIO_FN(RX2_C),
-	GPIO_FN(DREQ0_B), GPIO_FN(SSI_SCK78_B), GPIO_FN(SCK0_B),
-	GPIO_FN(DU1_DR0), GPIO_FN(VI2_DATA0_VI2_B0), GPIO_FN(PWM6),
-	GPIO_FN(SD3_CLK), GPIO_FN(TX3_E_IRDA_TX_E), GPIO_FN(AUDCK),
-	GPIO_FN(PWMFSW0_B), GPIO_FN(DU1_DR1), GPIO_FN(VI2_DATA1_VI2_B1),
-	GPIO_FN(PWM0), GPIO_FN(SD3_CMD), GPIO_FN(RX3_E_IRDA_RX_E),
-	GPIO_FN(AUDSYNC), GPIO_FN(CTS0_D), GPIO_FN(DU1_DR2), GPIO_FN(VI2_G0),
-	GPIO_FN(DU1_DR3), GPIO_FN(VI2_G1), GPIO_FN(DU1_DR4), GPIO_FN(VI2_G2),
-	GPIO_FN(DU1_DR5), GPIO_FN(VI2_G3), GPIO_FN(DU1_DR6), GPIO_FN(VI2_G4),
-	GPIO_FN(DU1_DR7), GPIO_FN(VI2_G5), GPIO_FN(DU1_DG0),
-	GPIO_FN(VI2_DATA2_VI2_B2), GPIO_FN(SCL1_B), GPIO_FN(SD3_DAT2),
-	GPIO_FN(SCK3_E), GPIO_FN(AUDATA6), GPIO_FN(TX0_D), GPIO_FN(DU1_DG1),
-	GPIO_FN(VI2_DATA3_VI2_B3), GPIO_FN(SDA1_B), GPIO_FN(SD3_DAT3),
-	GPIO_FN(SCK5), GPIO_FN(AUDATA7), GPIO_FN(RX0_D), GPIO_FN(DU1_DG2),
-	GPIO_FN(VI2_G6), GPIO_FN(DU1_DG3), GPIO_FN(VI2_G7), GPIO_FN(DU1_DG4),
-	GPIO_FN(VI2_R0), GPIO_FN(DU1_DG5), GPIO_FN(VI2_R1), GPIO_FN(DU1_DG6),
-	GPIO_FN(VI2_R2), GPIO_FN(DU1_DG7), GPIO_FN(VI2_R3), GPIO_FN(DU1_DB0),
-	GPIO_FN(VI2_DATA4_VI2_B4), GPIO_FN(SCL2_B), GPIO_FN(SD3_DAT0),
-	GPIO_FN(TX5), GPIO_FN(SCK0_D),
+static const char * const du1_groups[] = {
+	"du1_rgb666",
+	"du1_rgb888",
+	"du1_clk_in",
+	"du1_clk_out",
+	"du1_sync_0",
+	"du1_sync_1",
+	"du1_oddf",
+	"du1_cde",
+};
 
-	/* IPSR5 */
-	GPIO_FN(DU1_DB1), GPIO_FN(VI2_DATA5_VI2_B5), GPIO_FN(SDA2_B),
-	GPIO_FN(SD3_DAT1), GPIO_FN(RX5), GPIO_FN(RTS0_D_TANS_D),
-	GPIO_FN(DU1_DB2), GPIO_FN(VI2_R4), GPIO_FN(DU1_DB3), GPIO_FN(VI2_R5),
-	GPIO_FN(DU1_DB4), GPIO_FN(VI2_R6), GPIO_FN(DU1_DB5), GPIO_FN(VI2_R7),
-	GPIO_FN(DU1_DB6), GPIO_FN(SCL2_D), GPIO_FN(DU1_DB7), GPIO_FN(SDA2_D),
-	GPIO_FN(DU1_DOTCLKIN), GPIO_FN(VI2_CLKENB), GPIO_FN(HSPI_CS1),
-	GPIO_FN(SCL1_D), GPIO_FN(DU1_DOTCLKOUT), GPIO_FN(VI2_FIELD),
-	GPIO_FN(SDA1_D), GPIO_FN(DU1_EXHSYNC_DU1_HSYNC), GPIO_FN(VI2_HSYNC),
-	GPIO_FN(VI3_HSYNC), GPIO_FN(DU1_EXVSYNC_DU1_VSYNC), GPIO_FN(VI2_VSYNC),
-	GPIO_FN(VI3_VSYNC), GPIO_FN(DU1_EXODDF_DU1_ODDF_DISP_CDE),
-	GPIO_FN(VI2_CLK), GPIO_FN(TX3_B_IRDA_TX_B), GPIO_FN(SD3_CD),
-	GPIO_FN(HSPI_TX1), GPIO_FN(VI1_CLKENB), GPIO_FN(VI3_CLKENB),
-	GPIO_FN(AUDIO_CLKC), GPIO_FN(TX2_D), GPIO_FN(SPEEDIN),
-	GPIO_FN(GPS_SIGN_D), GPIO_FN(DU1_DISP), GPIO_FN(VI2_DATA6_VI2_B6),
-	GPIO_FN(TCLK0), GPIO_FN(QSTVA_B_QVS_B), GPIO_FN(HSPI_CLK1),
-	GPIO_FN(SCK2_D), GPIO_FN(AUDIO_CLKOUT_B), GPIO_FN(GPS_MAG_D),
-	GPIO_FN(DU1_CDE), GPIO_FN(VI2_DATA7_VI2_B7), GPIO_FN(RX3_B_IRDA_RX_B),
-	GPIO_FN(SD3_WP), GPIO_FN(HSPI_RX1), GPIO_FN(VI1_FIELD),
-	GPIO_FN(VI3_FIELD), GPIO_FN(AUDIO_CLKOUT), GPIO_FN(RX2_D),
-	GPIO_FN(GPS_CLK_C), GPIO_FN(GPS_CLK_D), GPIO_FN(AUDIO_CLKA),
-	GPIO_FN(CAN_TXCLK), GPIO_FN(AUDIO_CLKB), GPIO_FN(USB_OVC2),
-	GPIO_FN(CAN_DEBUGOUT0), GPIO_FN(MOUT0),
+static const char * const hspi0_groups[] = {
+	"hspi0",
+};
 
-	/* IPSR6 */
-	GPIO_FN(SSI_SCK0129), GPIO_FN(CAN_DEBUGOUT1), GPIO_FN(MOUT1),
-	GPIO_FN(SSI_WS0129), GPIO_FN(CAN_DEBUGOUT2), GPIO_FN(MOUT2),
-	GPIO_FN(SSI_SDATA0), GPIO_FN(CAN_DEBUGOUT3), GPIO_FN(MOUT5),
-	GPIO_FN(SSI_SDATA1), GPIO_FN(CAN_DEBUGOUT4), GPIO_FN(MOUT6),
-	GPIO_FN(SSI_SDATA2), GPIO_FN(CAN_DEBUGOUT5), GPIO_FN(SSI_SCK34),
-	GPIO_FN(CAN_DEBUGOUT6), GPIO_FN(CAN0_TX_B), GPIO_FN(IERX),
-	GPIO_FN(SSI_SCK9_C), GPIO_FN(SSI_WS34), GPIO_FN(CAN_DEBUGOUT7),
-	GPIO_FN(CAN0_RX_B), GPIO_FN(IETX), GPIO_FN(SSI_WS9_C),
-	GPIO_FN(SSI_SDATA3), GPIO_FN(PWM0_C), GPIO_FN(CAN_DEBUGOUT8),
-	GPIO_FN(CAN_CLK_B), GPIO_FN(IECLK), GPIO_FN(SCIF_CLK_B),
-	GPIO_FN(TCLK0_B), GPIO_FN(SSI_SDATA4), GPIO_FN(CAN_DEBUGOUT9),
-	GPIO_FN(SSI_SDATA9_C), GPIO_FN(SSI_SCK5), GPIO_FN(ADICLK),
-	GPIO_FN(CAN_DEBUGOUT10), GPIO_FN(SCK3), GPIO_FN(TCLK0_D),
-	GPIO_FN(SSI_WS5), GPIO_FN(ADICS_SAMP), GPIO_FN(CAN_DEBUGOUT11),
-	GPIO_FN(TX3_IRDA_TX), GPIO_FN(SSI_SDATA5), GPIO_FN(ADIDATA),
-	GPIO_FN(CAN_DEBUGOUT12), GPIO_FN(RX3_IRDA_RX), GPIO_FN(SSI_SCK6),
-	GPIO_FN(ADICHS0), GPIO_FN(CAN0_TX), GPIO_FN(IERX_B),
+static const char * const hspi1_groups[] = {
+	"hspi1",
+	"hspi1_b",
+	"hspi1_c",
+	"hspi1_d",
+};
 
-	/* IPSR7 */
-	GPIO_FN(SSI_WS6), GPIO_FN(ADICHS1), GPIO_FN(CAN0_RX), GPIO_FN(IETX_B),
-	GPIO_FN(SSI_SDATA6), GPIO_FN(ADICHS2), GPIO_FN(CAN_CLK),
-	GPIO_FN(IECLK_B), GPIO_FN(SSI_SCK78), GPIO_FN(CAN_DEBUGOUT13),
-	GPIO_FN(IRQ0_B), GPIO_FN(SSI_SCK9_B), GPIO_FN(HSPI_CLK1_C),
-	GPIO_FN(SSI_WS78), GPIO_FN(CAN_DEBUGOUT14), GPIO_FN(IRQ1_B),
-	GPIO_FN(SSI_WS9_B), GPIO_FN(HSPI_CS1_C), GPIO_FN(SSI_SDATA7),
-	GPIO_FN(CAN_DEBUGOUT15), GPIO_FN(IRQ2_B), GPIO_FN(TCLK1_C),
-	GPIO_FN(HSPI_TX1_C), GPIO_FN(SSI_SDATA8), GPIO_FN(VSP),
-	GPIO_FN(IRQ3_B), GPIO_FN(HSPI_RX1_C), GPIO_FN(SD0_CLK),
-	GPIO_FN(ATACS01), GPIO_FN(SCK1_B), GPIO_FN(SD0_CMD), GPIO_FN(ATACS11),
-	GPIO_FN(TX1_B), GPIO_FN(CC5_TDO), GPIO_FN(SD0_DAT0), GPIO_FN(ATADIR1),
-	GPIO_FN(RX1_B), GPIO_FN(CC5_TRST), GPIO_FN(SD0_DAT1), GPIO_FN(ATAG1),
-	GPIO_FN(SCK2_B), GPIO_FN(CC5_TMS), GPIO_FN(SD0_DAT2), GPIO_FN(ATARD1),
-	GPIO_FN(TX2_B), GPIO_FN(CC5_TCK), GPIO_FN(SD0_DAT3), GPIO_FN(ATAWR1),
-	GPIO_FN(RX2_B), GPIO_FN(CC5_TDI), GPIO_FN(SD0_CD), GPIO_FN(DREQ2),
-	GPIO_FN(RTS1_B_TANS_B), GPIO_FN(SD0_WP), GPIO_FN(DACK2),
-	GPIO_FN(CTS1_B),
+static const char * const hspi2_groups[] = {
+	"hspi2",
+	"hspi2_b",
+};
 
-	/* IPSR8 */
-	GPIO_FN(HSPI_CLK0), GPIO_FN(CTS0), GPIO_FN(USB_OVC0), GPIO_FN(AD_CLK),
-	GPIO_FN(CC5_STATE4), GPIO_FN(CC5_STATE12), GPIO_FN(CC5_STATE20),
-	GPIO_FN(CC5_STATE28), GPIO_FN(CC5_STATE36), GPIO_FN(HSPI_CS0),
-	GPIO_FN(RTS0_TANS), GPIO_FN(USB_OVC1), GPIO_FN(AD_DI),
-	GPIO_FN(CC5_STATE5), GPIO_FN(CC5_STATE13), GPIO_FN(CC5_STATE21),
-	GPIO_FN(CC5_STATE29), GPIO_FN(CC5_STATE37), GPIO_FN(HSPI_TX0),
-	GPIO_FN(TX0), GPIO_FN(CAN_DEBUG_HW_TRIGGER), GPIO_FN(AD_DO),
-	GPIO_FN(CC5_STATE6), GPIO_FN(CC5_STATE14), GPIO_FN(CC5_STATE22),
-	GPIO_FN(CC5_STATE30), GPIO_FN(CC5_STATE38), GPIO_FN(HSPI_RX0),
-	GPIO_FN(RX0), GPIO_FN(CAN_STEP0), GPIO_FN(AD_NCS), GPIO_FN(CC5_STATE7),
-	GPIO_FN(CC5_STATE15), GPIO_FN(CC5_STATE23), GPIO_FN(CC5_STATE31),
-	GPIO_FN(CC5_STATE39), GPIO_FN(FMCLK), GPIO_FN(RDS_CLK), GPIO_FN(PCMOE),
-	GPIO_FN(BPFCLK), GPIO_FN(PCMWE), GPIO_FN(FMIN), GPIO_FN(RDS_DATA),
-	GPIO_FN(VI0_CLK), GPIO_FN(MMC1_CLK), GPIO_FN(VI0_CLKENB),
-	GPIO_FN(TX1_C), GPIO_FN(HTX1_B), GPIO_FN(MT1_SYNC),
-	GPIO_FN(VI0_FIELD), GPIO_FN(RX1_C), GPIO_FN(HRX1_B),
-	GPIO_FN(VI0_HSYNC), GPIO_FN(VI0_DATA0_B_VI0_B0_B), GPIO_FN(CTS1_C),
-	GPIO_FN(TX4_D), GPIO_FN(MMC1_CMD), GPIO_FN(HSCK1_B),
-	GPIO_FN(VI0_VSYNC), GPIO_FN(VI0_DATA1_B_VI0_B1_B),
-	GPIO_FN(RTS1_C_TANS_C), GPIO_FN(RX4_D), GPIO_FN(PWMFSW0_C),
+static const char * const intc_groups[] = {
+	"intc_irq0",
+	"intc_irq0_b",
+	"intc_irq1",
+	"intc_irq1_b",
+	"intc_irq2",
+	"intc_irq2_b",
+	"intc_irq3",
+	"intc_irq3_b",
+};
 
-	/* IPSR9 */
-	GPIO_FN(VI0_DATA0_VI0_B0), GPIO_FN(HRTS1_B), GPIO_FN(MT1_VCXO),
-	GPIO_FN(VI0_DATA1_VI0_B1), GPIO_FN(HCTS1_B), GPIO_FN(MT1_PWM),
-	GPIO_FN(VI0_DATA2_VI0_B2), GPIO_FN(MMC1_D0), GPIO_FN(VI0_DATA3_VI0_B3),
-	GPIO_FN(MMC1_D1), GPIO_FN(VI0_DATA4_VI0_B4), GPIO_FN(MMC1_D2),
-	GPIO_FN(VI0_DATA5_VI0_B5), GPIO_FN(MMC1_D3), GPIO_FN(VI0_DATA6_VI0_B6),
-	GPIO_FN(MMC1_D4), GPIO_FN(ARM_TRACEDATA_0), GPIO_FN(VI0_DATA7_VI0_B7),
-	GPIO_FN(MMC1_D5), GPIO_FN(ARM_TRACEDATA_1), GPIO_FN(VI0_G0),
-	GPIO_FN(SSI_SCK78_C), GPIO_FN(IRQ0), GPIO_FN(ARM_TRACEDATA_2),
-	GPIO_FN(VI0_G1), GPIO_FN(SSI_WS78_C), GPIO_FN(IRQ1),
-	GPIO_FN(ARM_TRACEDATA_3), GPIO_FN(VI0_G2), GPIO_FN(ETH_TXD1),
-	GPIO_FN(MMC1_D6), GPIO_FN(ARM_TRACEDATA_4), GPIO_FN(TS_SPSYNC0),
-	GPIO_FN(VI0_G3), GPIO_FN(ETH_CRS_DV), GPIO_FN(MMC1_D7),
-	GPIO_FN(ARM_TRACEDATA_5), GPIO_FN(TS_SDAT0), GPIO_FN(VI0_G4),
-	GPIO_FN(ETH_TX_EN), GPIO_FN(SD2_DAT0_B), GPIO_FN(ARM_TRACEDATA_6),
-	GPIO_FN(VI0_G5), GPIO_FN(ETH_RX_ER), GPIO_FN(SD2_DAT1_B),
-	GPIO_FN(ARM_TRACEDATA_7), GPIO_FN(VI0_G6), GPIO_FN(ETH_RXD0),
-	GPIO_FN(SD2_DAT2_B), GPIO_FN(ARM_TRACEDATA_8), GPIO_FN(VI0_G7),
-	GPIO_FN(ETH_RXD1), GPIO_FN(SD2_DAT3_B), GPIO_FN(ARM_TRACEDATA_9),
+static const char * const lbsc_groups[] = {
+	"lbsc_cs0",
+	"lbsc_cs1",
+	"lbsc_ex_cs0",
+	"lbsc_ex_cs1",
+	"lbsc_ex_cs2",
+	"lbsc_ex_cs3",
+	"lbsc_ex_cs4",
+	"lbsc_ex_cs5",
+};
 
-	/* IPSR10 */
-	GPIO_FN(VI0_R0), GPIO_FN(SSI_SDATA7_C), GPIO_FN(SCK1_C),
-	GPIO_FN(DREQ1_B), GPIO_FN(ARM_TRACEDATA_10), GPIO_FN(DREQ0_C),
-	GPIO_FN(VI0_R1), GPIO_FN(SSI_SDATA8_C), GPIO_FN(DACK1_B),
-	GPIO_FN(ARM_TRACEDATA_11), GPIO_FN(DACK0_C), GPIO_FN(DRACK0_C),
-	GPIO_FN(VI0_R2), GPIO_FN(ETH_LINK), GPIO_FN(SD2_CLK_B), GPIO_FN(IRQ2),
-	GPIO_FN(ARM_TRACEDATA_12), GPIO_FN(VI0_R3), GPIO_FN(ETH_MAGIC),
-	GPIO_FN(SD2_CMD_B), GPIO_FN(IRQ3), GPIO_FN(ARM_TRACEDATA_13),
-	GPIO_FN(VI0_R4), GPIO_FN(ETH_REFCLK), GPIO_FN(SD2_CD_B),
-	GPIO_FN(HSPI_CLK1_B), GPIO_FN(ARM_TRACEDATA_14), GPIO_FN(MT1_CLK),
-	GPIO_FN(TS_SCK0), GPIO_FN(VI0_R5), GPIO_FN(ETH_TXD0),
-	GPIO_FN(SD2_WP_B), GPIO_FN(HSPI_CS1_B), GPIO_FN(ARM_TRACEDATA_15),
-	GPIO_FN(MT1_D), GPIO_FN(TS_SDEN0), GPIO_FN(VI0_R6), GPIO_FN(ETH_MDC),
-	GPIO_FN(DREQ2_C), GPIO_FN(HSPI_TX1_B), GPIO_FN(TRACECLK),
-	GPIO_FN(MT1_BEN), GPIO_FN(PWMFSW0_D), GPIO_FN(VI0_R7),
-	GPIO_FN(ETH_MDIO), GPIO_FN(DACK2_C), GPIO_FN(HSPI_RX1_B),
-	GPIO_FN(SCIF_CLK_D), GPIO_FN(TRACECTL), GPIO_FN(MT1_PEN),
-	GPIO_FN(VI1_CLK), GPIO_FN(SIM_D), GPIO_FN(SDA3), GPIO_FN(VI1_HSYNC),
-	GPIO_FN(VI3_CLK), GPIO_FN(SSI_SCK4), GPIO_FN(GPS_SIGN_C),
-	GPIO_FN(PWMFSW0_E), GPIO_FN(VI1_VSYNC), GPIO_FN(AUDIO_CLKOUT_C),
-	GPIO_FN(SSI_WS4), GPIO_FN(SIM_CLK), GPIO_FN(GPS_MAG_C),
-	GPIO_FN(SPV_TRST), GPIO_FN(SCL3),
+static const char * const mmc0_groups[] = {
+	"mmc0_data1",
+	"mmc0_data4",
+	"mmc0_data8",
+	"mmc0_ctrl",
+};
 
-	/* IPSR11 */
-	GPIO_FN(VI1_DATA0_VI1_B0), GPIO_FN(SD2_DAT0), GPIO_FN(SIM_RST),
-	GPIO_FN(SPV_TCK), GPIO_FN(ADICLK_B), GPIO_FN(VI1_DATA1_VI1_B1),
-	GPIO_FN(SD2_DAT1), GPIO_FN(MT0_CLK), GPIO_FN(SPV_TMS),
-	GPIO_FN(ADICS_B_SAMP_B), GPIO_FN(VI1_DATA2_VI1_B2), GPIO_FN(SD2_DAT2),
-	GPIO_FN(MT0_D), GPIO_FN(SPVTDI), GPIO_FN(ADIDATA_B),
-	GPIO_FN(VI1_DATA3_VI1_B3), GPIO_FN(SD2_DAT3), GPIO_FN(MT0_BEN),
-	GPIO_FN(SPV_TDO), GPIO_FN(ADICHS0_B), GPIO_FN(VI1_DATA4_VI1_B4),
-	GPIO_FN(SD2_CLK), GPIO_FN(MT0_PEN), GPIO_FN(SPA_TRST),
-	GPIO_FN(HSPI_CLK1_D), GPIO_FN(ADICHS1_B), GPIO_FN(VI1_DATA5_VI1_B5),
-	GPIO_FN(SD2_CMD), GPIO_FN(MT0_SYNC), GPIO_FN(SPA_TCK),
-	GPIO_FN(HSPI_CS1_D), GPIO_FN(ADICHS2_B), GPIO_FN(VI1_DATA6_VI1_B6),
-	GPIO_FN(SD2_CD), GPIO_FN(MT0_VCXO), GPIO_FN(SPA_TMS),
-	GPIO_FN(HSPI_TX1_D), GPIO_FN(VI1_DATA7_VI1_B7), GPIO_FN(SD2_WP),
-	GPIO_FN(MT0_PWM), GPIO_FN(SPA_TDI), GPIO_FN(HSPI_RX1_D),
-	GPIO_FN(VI1_G0), GPIO_FN(VI3_DATA0), GPIO_FN(DU1_DOTCLKOUT1),
-	GPIO_FN(TS_SCK1), GPIO_FN(DREQ2_B), GPIO_FN(TX2), GPIO_FN(SPA_TDO),
-	GPIO_FN(HCTS0_B), GPIO_FN(VI1_G1), GPIO_FN(VI3_DATA1),
-	GPIO_FN(SSI_SCK1), GPIO_FN(TS_SDEN1), GPIO_FN(DACK2_B), GPIO_FN(RX2),
-	GPIO_FN(HRTS0_B),
+static const char * const mmc1_groups[] = {
+	"mmc1_data1",
+	"mmc1_data4",
+	"mmc1_data8",
+	"mmc1_ctrl",
+};
 
-	/* IPSR12 */
-	GPIO_FN(VI1_G2), GPIO_FN(VI3_DATA2), GPIO_FN(SSI_WS1),
-	GPIO_FN(TS_SPSYNC1), GPIO_FN(SCK2), GPIO_FN(HSCK0_B), GPIO_FN(VI1_G3),
-	GPIO_FN(VI3_DATA3), GPIO_FN(SSI_SCK2), GPIO_FN(TS_SDAT1),
-	GPIO_FN(SCL1_C), GPIO_FN(HTX0_B), GPIO_FN(VI1_G4), GPIO_FN(VI3_DATA4),
-	GPIO_FN(SSI_WS2), GPIO_FN(SDA1_C), GPIO_FN(SIM_RST_B),
-	GPIO_FN(HRX0_B), GPIO_FN(VI1_G5), GPIO_FN(VI3_DATA5),
-	GPIO_FN(GPS_CLK), GPIO_FN(FSE), GPIO_FN(TX4_B), GPIO_FN(SIM_D_B),
-	GPIO_FN(VI1_G6), GPIO_FN(VI3_DATA6), GPIO_FN(GPS_SIGN), GPIO_FN(FRB),
-	GPIO_FN(RX4_B), GPIO_FN(SIM_CLK_B), GPIO_FN(VI1_G7),
-	GPIO_FN(VI3_DATA7), GPIO_FN(GPS_MAG), GPIO_FN(FCE), GPIO_FN(SCK4_B),
-};
-
-static struct pinmux_cfg_reg pinmux_config_regs[] = {
+static const char * const scif0_groups[] = {
+	"scif0_data",
+	"scif0_clk",
+	"scif0_ctrl",
+	"scif0_data_b",
+	"scif0_clk_b",
+	"scif0_ctrl_b",
+	"scif0_data_c",
+	"scif0_clk_c",
+	"scif0_ctrl_c",
+	"scif0_data_d",
+	"scif0_clk_d",
+	"scif0_ctrl_d",
+};
+
+static const char * const scif1_groups[] = {
+	"scif1_data",
+	"scif1_clk",
+	"scif1_ctrl",
+	"scif1_data_b",
+	"scif1_clk_b",
+	"scif1_ctrl_b",
+	"scif1_data_c",
+	"scif1_clk_c",
+	"scif1_ctrl_c",
+};
+
+static const char * const scif2_groups[] = {
+	"scif2_data",
+	"scif2_clk",
+	"scif2_data_b",
+	"scif2_clk_b",
+	"scif2_data_c",
+	"scif2_clk_c",
+	"scif2_data_d",
+	"scif2_clk_d",
+	"scif2_data_e",
+};
+
+static const char * const scif3_groups[] = {
+	"scif3_data",
+	"scif3_clk",
+	"scif3_data_b",
+	"scif3_data_c",
+	"scif3_data_d",
+	"scif3_data_e",
+	"scif3_clk_e",
+};
+
+static const char * const scif4_groups[] = {
+	"scif4_data",
+	"scif4_clk",
+	"scif4_data_b",
+	"scif4_clk_b",
+	"scif4_data_c",
+	"scif4_data_d",
+};
+
+static const char * const scif5_groups[] = {
+	"scif5_data",
+	"scif5_clk",
+	"scif5_data_b",
+	"scif5_clk_b",
+	"scif5_data_c",
+	"scif5_clk_c",
+	"scif5_data_d",
+	"scif5_clk_d",
+};
+
+static const char * const sdhi0_groups[] = {
+	"sdhi0_data1",
+	"sdhi0_data4",
+	"sdhi0_ctrl",
+	"sdhi0_cd",
+	"sdhi0_wp",
+};
+
+static const char * const sdhi1_groups[] = {
+	"sdhi1_data1",
+	"sdhi1_data4",
+	"sdhi1_ctrl",
+	"sdhi1_cd",
+	"sdhi1_wp",
+};
+
+static const char * const sdhi2_groups[] = {
+	"sdhi2_data1",
+	"sdhi2_data4",
+	"sdhi2_ctrl",
+	"sdhi2_cd",
+	"sdhi2_wp",
+};
+
+static const char * const sdhi3_groups[] = {
+	"sdhi3_data1",
+	"sdhi3_data4",
+	"sdhi3_ctrl",
+	"sdhi3_cd",
+	"sdhi3_wp",
+};
+
+static const char * const usb0_groups[] = {
+	"usb0",
+};
+
+static const char * const usb1_groups[] = {
+	"usb1",
+};
+
+static const char * const usb2_groups[] = {
+	"usb2",
+};
+
+static const struct sh_pfc_function pinmux_functions[] = {
+	SH_PFC_FUNCTION(du0),
+	SH_PFC_FUNCTION(du1),
+	SH_PFC_FUNCTION(hspi0),
+	SH_PFC_FUNCTION(hspi1),
+	SH_PFC_FUNCTION(hspi2),
+	SH_PFC_FUNCTION(intc),
+	SH_PFC_FUNCTION(lbsc),
+	SH_PFC_FUNCTION(mmc0),
+	SH_PFC_FUNCTION(mmc1),
+	SH_PFC_FUNCTION(sdhi0),
+	SH_PFC_FUNCTION(sdhi1),
+	SH_PFC_FUNCTION(sdhi2),
+	SH_PFC_FUNCTION(sdhi3),
+	SH_PFC_FUNCTION(scif0),
+	SH_PFC_FUNCTION(scif1),
+	SH_PFC_FUNCTION(scif2),
+	SH_PFC_FUNCTION(scif3),
+	SH_PFC_FUNCTION(scif4),
+	SH_PFC_FUNCTION(scif5),
+	SH_PFC_FUNCTION(usb0),
+	SH_PFC_FUNCTION(usb1),
+	SH_PFC_FUNCTION(usb2),
+};
+
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ PINMUX_CFG_REG("GPSR0", 0xfffc0004, 32, 1) {
 		GP_0_31_FN, FN_IP3_31_29,
 		GP_0_30_FN, FN_IP3_26_24,
@@ -2412,7 +3407,7 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	    FN_VI1_G1, FN_VI3_DATA1, FN_SSI_SCK1, FN_TS_SDEN1,
 	    FN_DACK2_B, FN_RX2, FN_HRTS0_B, 0,
 	    /* IP11_26_24 [3] */
-	    FN_VI1_G0, FN_VI3_DATA0, FN_DU1_DOTCLKOUT1, FN_TS_SCK1,
+	    FN_VI1_G0, FN_VI3_DATA0, 0, FN_TS_SCK1,
 	    FN_DREQ2_B, FN_TX2, FN_SPA_TDO, FN_HCTS0_B,
 	    /* IP11_23_21 [3] */
 	    FN_VI1_DATA7_VI1_B7, FN_SD2_WP, FN_MT0_PWM, FN_SPA_TDI,
@@ -2558,66 +3553,24 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	    /* SEL_I2C1 [2] */
 	    FN_SEL_I2C1_0, FN_SEL_I2C1_1, FN_SEL_I2C1_2, FN_SEL_I2C1_3 }
 	},
-	{ PINMUX_CFG_REG("INOUTSEL0", 0xffc40004, 32, 1) { GP_INOUTSEL(0) } },
-	{ PINMUX_CFG_REG("INOUTSEL1", 0xffc41004, 32, 1) { GP_INOUTSEL(1) } },
-	{ PINMUX_CFG_REG("INOUTSEL2", 0xffc42004, 32, 1) { GP_INOUTSEL(2) } },
-	{ PINMUX_CFG_REG("INOUTSEL3", 0xffc43004, 32, 1) { GP_INOUTSEL(3) } },
-	{ PINMUX_CFG_REG("INOUTSEL4", 0xffc44004, 32, 1) { GP_INOUTSEL(4) } },
-	{ PINMUX_CFG_REG("INOUTSEL5", 0xffc45004, 32, 1) { GP_INOUTSEL(5) } },
-	{ PINMUX_CFG_REG("INOUTSEL6", 0xffc46004, 32, 1) {
-		0, 0, 0, 0, 0, 0, 0, 0,	0, 0, 0, 0, 0, 0, 0, 0,
-		0, 0, 0, 0, 0, 0, 0, 0,	0, 0, 0, 0, 0, 0, 0, 0,
-		0, 0, 0, 0, 0, 0, 0, 0,
-		0, 0,
-		0, 0,
-		0, 0,
-		GP_6_8_IN, GP_6_8_OUT,
-		GP_6_7_IN, GP_6_7_OUT,
-		GP_6_6_IN, GP_6_6_OUT,
-		GP_6_5_IN, GP_6_5_OUT,
-		GP_6_4_IN, GP_6_4_OUT,
-		GP_6_3_IN, GP_6_3_OUT,
-		GP_6_2_IN, GP_6_2_OUT,
-		GP_6_1_IN, GP_6_1_OUT,
-		GP_6_0_IN, GP_6_0_OUT, }
-	},
-	{ },
-};
-
-static struct pinmux_data_reg pinmux_data_regs[] = {
-	{ PINMUX_DATA_REG("INDT0", 0xffc40008, 32) { GP_INDT(0) } },
-	{ PINMUX_DATA_REG("INDT1", 0xffc41008, 32) { GP_INDT(1) } },
-	{ PINMUX_DATA_REG("INDT2", 0xffc42008, 32) { GP_INDT(2) } },
-	{ PINMUX_DATA_REG("INDT3", 0xffc43008, 32) { GP_INDT(3) } },
-	{ PINMUX_DATA_REG("INDT4", 0xffc44008, 32) { GP_INDT(4) } },
-	{ PINMUX_DATA_REG("INDT5", 0xffc45008, 32) { GP_INDT(5) } },
-	{ PINMUX_DATA_REG("INDT6", 0xffc46008, 32) {
-		0, 0, 0, 0, 0, 0, 0, 0,	0, 0, 0, 0, 0, 0, 0, 0,
-		0, 0, 0, 0, 0, 0, 0, GP_6_8_DATA,
-		GP_6_7_DATA, GP_6_6_DATA, GP_6_5_DATA, GP_6_4_DATA,
-		GP_6_3_DATA, GP_6_2_DATA, GP_6_1_DATA, GP_6_0_DATA }
-	},
 	{ },
 };
 
-struct sh_pfc_soc_info r8a7779_pinmux_info = {
+const struct sh_pfc_soc_info r8a7779_pinmux_info = {
 	.name = "r8a7779_pfc",
 
 	.unlock_reg = 0xfffc0000, /* PMMR */
 
-	.reserved_id = PINMUX_RESERVED,
-	.data = { PINMUX_DATA_BEGIN, PINMUX_DATA_END },
-	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END },
-	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END },
-	.mark = { PINMUX_MARK_BEGIN, PINMUX_MARK_END },
 	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
 
-	.first_gpio = GPIO_GP_0_0,
-	.last_gpio = GPIO_FN_SCK4_B,
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.groups = pinmux_groups,
+	.nr_groups = ARRAY_SIZE(pinmux_groups),
+	.functions = pinmux_functions,
+	.nr_functions = ARRAY_SIZE(pinmux_functions),
 
-	.gpios = pinmux_gpios,
 	.cfg_regs = pinmux_config_regs,
-	.data_regs = pinmux_data_regs,
 
 	.gpio_data = pinmux_data,
 	.gpio_data_size = ARRAY_SIZE(pinmux_data),
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh7203.c b/drivers/pinctrl/sh-pfc/pfc-sh7203.c
index 01b425dfd162..f63d51dc3f4c 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh7203.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh7203.c
@@ -272,7 +272,7 @@ enum {
 	PINMUX_MARK_END,
 };
 
-static pinmux_enum_t pinmux_data[] = {
+static const pinmux_enum_t pinmux_data[] = {
 
 	/* PA */
 	PINMUX_DATA(PA7_DATA, PA7_IN),
@@ -703,7 +703,7 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA(SSCK0_PF_MARK, PF0MD_11),
 };
 
-static struct pinmux_gpio pinmux_gpios[] = {
+static struct sh_pfc_pin pinmux_pins[] = {
 
 	/* PA */
 	PINMUX_GPIO(GPIO_PA7, PA7_DATA),
@@ -815,265 +815,269 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	PINMUX_GPIO(GPIO_PF2, PF2_DATA),
 	PINMUX_GPIO(GPIO_PF1, PF1_DATA),
 	PINMUX_GPIO(GPIO_PF0, PF0_DATA),
+};
+
+#define PINMUX_FN_BASE	ARRAY_SIZE(pinmux_pins)
 
+static const struct pinmux_func pinmux_func_gpios[] = {
 	/* INTC */
-	PINMUX_GPIO(GPIO_FN_PINT7_PB, PINT7_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT6_PB, PINT6_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT5_PB, PINT5_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT4_PB, PINT4_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT3_PB, PINT3_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT2_PB, PINT2_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT1_PB, PINT1_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT0_PB, PINT0_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT7_PD, PINT7_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT6_PD, PINT6_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT5_PD, PINT5_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT4_PD, PINT4_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT3_PD, PINT3_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT2_PD, PINT2_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT1_PD, PINT1_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT0_PD, PINT0_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ7_PB, IRQ7_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ6_PB, IRQ6_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ5_PB, IRQ5_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ4_PB, IRQ4_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ3_PB, IRQ3_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ2_PB, IRQ2_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ1_PB, IRQ1_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ0_PB, IRQ0_PB_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ7_PD, IRQ7_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ6_PD, IRQ6_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ5_PD, IRQ5_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ4_PD, IRQ4_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ3_PD, IRQ3_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ2_PD, IRQ2_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ1_PD, IRQ1_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ0_PD, IRQ0_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ7_PE, IRQ7_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ6_PE, IRQ6_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ5_PE, IRQ5_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ4_PE, IRQ4_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ3_PE, IRQ3_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ2_PE, IRQ2_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ1_PE, IRQ1_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ0_PE, IRQ0_PE_MARK),
-
-	PINMUX_GPIO(GPIO_FN_WDTOVF, WDTOVF_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQOUT, IRQOUT_MARK),
-	PINMUX_GPIO(GPIO_FN_REFOUT, REFOUT_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQOUT_REFOUT, IRQOUT_REFOUT_MARK),
-	PINMUX_GPIO(GPIO_FN_UBCTRG, UBCTRG_MARK),
+	GPIO_FN(PINT7_PB),
+	GPIO_FN(PINT6_PB),
+	GPIO_FN(PINT5_PB),
+	GPIO_FN(PINT4_PB),
+	GPIO_FN(PINT3_PB),
+	GPIO_FN(PINT2_PB),
+	GPIO_FN(PINT1_PB),
+	GPIO_FN(PINT0_PB),
+	GPIO_FN(PINT7_PD),
+	GPIO_FN(PINT6_PD),
+	GPIO_FN(PINT5_PD),
+	GPIO_FN(PINT4_PD),
+	GPIO_FN(PINT3_PD),
+	GPIO_FN(PINT2_PD),
+	GPIO_FN(PINT1_PD),
+	GPIO_FN(PINT0_PD),
+	GPIO_FN(IRQ7_PB),
+	GPIO_FN(IRQ6_PB),
+	GPIO_FN(IRQ5_PB),
+	GPIO_FN(IRQ4_PB),
+	GPIO_FN(IRQ3_PB),
+	GPIO_FN(IRQ2_PB),
+	GPIO_FN(IRQ1_PB),
+	GPIO_FN(IRQ0_PB),
+	GPIO_FN(IRQ7_PD),
+	GPIO_FN(IRQ6_PD),
+	GPIO_FN(IRQ5_PD),
+	GPIO_FN(IRQ4_PD),
+	GPIO_FN(IRQ3_PD),
+	GPIO_FN(IRQ2_PD),
+	GPIO_FN(IRQ1_PD),
+	GPIO_FN(IRQ0_PD),
+	GPIO_FN(IRQ7_PE),
+	GPIO_FN(IRQ6_PE),
+	GPIO_FN(IRQ5_PE),
+	GPIO_FN(IRQ4_PE),
+	GPIO_FN(IRQ3_PE),
+	GPIO_FN(IRQ2_PE),
+	GPIO_FN(IRQ1_PE),
+	GPIO_FN(IRQ0_PE),
+
+	GPIO_FN(WDTOVF),
+	GPIO_FN(IRQOUT),
+	GPIO_FN(REFOUT),
+	GPIO_FN(IRQOUT_REFOUT),
+	GPIO_FN(UBCTRG),
 
 	/* CAN */
-	PINMUX_GPIO(GPIO_FN_CTX1, CTX1_MARK),
-	PINMUX_GPIO(GPIO_FN_CRX1, CRX1_MARK),
-	PINMUX_GPIO(GPIO_FN_CTX0, CTX0_MARK),
-	PINMUX_GPIO(GPIO_FN_CTX0_CTX1, CTX0_CTX1_MARK),
-	PINMUX_GPIO(GPIO_FN_CRX0, CRX0_MARK),
-	PINMUX_GPIO(GPIO_FN_CRX0_CRX1, CRX0_CRX1_MARK),
+	GPIO_FN(CTX1),
+	GPIO_FN(CRX1),
+	GPIO_FN(CTX0),
+	GPIO_FN(CTX0_CTX1),
+	GPIO_FN(CRX0),
+	GPIO_FN(CRX0_CRX1),
 
 	/* IIC3 */
-	PINMUX_GPIO(GPIO_FN_SDA3, SDA3_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL3, SCL3_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA2, SDA2_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL2, SCL2_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA1, SDA1_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL1, SCL1_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA0, SDA0_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL0, SCL0_MARK),
+	GPIO_FN(SDA3),
+	GPIO_FN(SCL3),
+	GPIO_FN(SDA2),
+	GPIO_FN(SCL2),
+	GPIO_FN(SDA1),
+	GPIO_FN(SCL1),
+	GPIO_FN(SDA0),
+	GPIO_FN(SCL0),
 
 	/* DMAC */
-	PINMUX_GPIO(GPIO_FN_TEND0_PD, TEND0_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_TEND0_PE, TEND0_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK0_PD, DACK0_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK0_PE, DACK0_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ0_PD, DREQ0_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ0_PE, DREQ0_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_TEND1_PD, TEND1_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_TEND1_PE, TEND1_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK1_PD, DACK1_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK1_PE, DACK1_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ1_PD, DREQ1_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ1_PE, DREQ1_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK2, DACK2_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ2, DREQ2_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK3, DACK3_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ3, DREQ3_MARK),
+	GPIO_FN(TEND0_PD),
+	GPIO_FN(TEND0_PE),
+	GPIO_FN(DACK0_PD),
+	GPIO_FN(DACK0_PE),
+	GPIO_FN(DREQ0_PD),
+	GPIO_FN(DREQ0_PE),
+	GPIO_FN(TEND1_PD),
+	GPIO_FN(TEND1_PE),
+	GPIO_FN(DACK1_PD),
+	GPIO_FN(DACK1_PE),
+	GPIO_FN(DREQ1_PD),
+	GPIO_FN(DREQ1_PE),
+	GPIO_FN(DACK2),
+	GPIO_FN(DREQ2),
+	GPIO_FN(DACK3),
+	GPIO_FN(DREQ3),
 
 	/* ADC */
-	PINMUX_GPIO(GPIO_FN_ADTRG_PD, ADTRG_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_ADTRG_PE, ADTRG_PE_MARK),
+	GPIO_FN(ADTRG_PD),
+	GPIO_FN(ADTRG_PE),
 
 	/* BSC */
-	PINMUX_GPIO(GPIO_FN_D31, D31_MARK),
-	PINMUX_GPIO(GPIO_FN_D30, D30_MARK),
-	PINMUX_GPIO(GPIO_FN_D29, D29_MARK),
-	PINMUX_GPIO(GPIO_FN_D28, D28_MARK),
-	PINMUX_GPIO(GPIO_FN_D27, D27_MARK),
-	PINMUX_GPIO(GPIO_FN_D26, D26_MARK),
-	PINMUX_GPIO(GPIO_FN_D25, D25_MARK),
-	PINMUX_GPIO(GPIO_FN_D24, D24_MARK),
-	PINMUX_GPIO(GPIO_FN_D23, D23_MARK),
-	PINMUX_GPIO(GPIO_FN_D22, D22_MARK),
-	PINMUX_GPIO(GPIO_FN_D21, D21_MARK),
-	PINMUX_GPIO(GPIO_FN_D20, D20_MARK),
-	PINMUX_GPIO(GPIO_FN_D19, D19_MARK),
-	PINMUX_GPIO(GPIO_FN_D18, D18_MARK),
-	PINMUX_GPIO(GPIO_FN_D17, D17_MARK),
-	PINMUX_GPIO(GPIO_FN_D16, D16_MARK),
-	PINMUX_GPIO(GPIO_FN_A25, A25_MARK),
-	PINMUX_GPIO(GPIO_FN_A24, A24_MARK),
-	PINMUX_GPIO(GPIO_FN_A23, A23_MARK),
-	PINMUX_GPIO(GPIO_FN_A22, A22_MARK),
-	PINMUX_GPIO(GPIO_FN_A21, A21_MARK),
-	PINMUX_GPIO(GPIO_FN_CS4, CS4_MARK),
-	PINMUX_GPIO(GPIO_FN_MRES, MRES_MARK),
-	PINMUX_GPIO(GPIO_FN_BS, BS_MARK),
-	PINMUX_GPIO(GPIO_FN_IOIS16, IOIS16_MARK),
-	PINMUX_GPIO(GPIO_FN_CS1, CS1_MARK),
-	PINMUX_GPIO(GPIO_FN_CS6_CE1B, CS6_CE1B_MARK),
-	PINMUX_GPIO(GPIO_FN_CE2B, CE2B_MARK),
-	PINMUX_GPIO(GPIO_FN_CS5_CE1A, CS5_CE1A_MARK),
-	PINMUX_GPIO(GPIO_FN_CE2A, CE2A_MARK),
-	PINMUX_GPIO(GPIO_FN_FRAME, FRAME_MARK),
-	PINMUX_GPIO(GPIO_FN_WAIT, WAIT_MARK),
-	PINMUX_GPIO(GPIO_FN_RDWR, RDWR_MARK),
-	PINMUX_GPIO(GPIO_FN_CKE, CKE_MARK),
-	PINMUX_GPIO(GPIO_FN_CASU, CASU_MARK),
-	PINMUX_GPIO(GPIO_FN_BREQ, BREQ_MARK),
-	PINMUX_GPIO(GPIO_FN_RASU, RASU_MARK),
-	PINMUX_GPIO(GPIO_FN_BACK, BACK_MARK),
-	PINMUX_GPIO(GPIO_FN_CASL, CASL_MARK),
-	PINMUX_GPIO(GPIO_FN_RASL, RASL_MARK),
-	PINMUX_GPIO(GPIO_FN_WE3_DQMUU_AH_ICIO_WR, WE3_DQMUU_AH_ICIO_WR_MARK),
-	PINMUX_GPIO(GPIO_FN_WE2_DQMUL_ICIORD, WE2_DQMUL_ICIORD_MARK),
-	PINMUX_GPIO(GPIO_FN_WE1_DQMLU_WE, WE1_DQMLU_WE_MARK),
-	PINMUX_GPIO(GPIO_FN_WE0_DQMLL, WE0_DQMLL_MARK),
-	PINMUX_GPIO(GPIO_FN_CS3, CS3_MARK),
-	PINMUX_GPIO(GPIO_FN_CS2, CS2_MARK),
-	PINMUX_GPIO(GPIO_FN_A1, A1_MARK),
-	PINMUX_GPIO(GPIO_FN_A0, A0_MARK),
-	PINMUX_GPIO(GPIO_FN_CS7, CS7_MARK),
+	GPIO_FN(D31),
+	GPIO_FN(D30),
+	GPIO_FN(D29),
+	GPIO_FN(D28),
+	GPIO_FN(D27),
+	GPIO_FN(D26),
+	GPIO_FN(D25),
+	GPIO_FN(D24),
+	GPIO_FN(D23),
+	GPIO_FN(D22),
+	GPIO_FN(D21),
+	GPIO_FN(D20),
+	GPIO_FN(D19),
+	GPIO_FN(D18),
+	GPIO_FN(D17),
+	GPIO_FN(D16),
+	GPIO_FN(A25),
+	GPIO_FN(A24),
+	GPIO_FN(A23),
+	GPIO_FN(A22),
+	GPIO_FN(A21),
+	GPIO_FN(CS4),
+	GPIO_FN(MRES),
+	GPIO_FN(BS),
+	GPIO_FN(IOIS16),
+	GPIO_FN(CS1),
+	GPIO_FN(CS6_CE1B),
+	GPIO_FN(CE2B),
+	GPIO_FN(CS5_CE1A),
+	GPIO_FN(CE2A),
+	GPIO_FN(FRAME),
+	GPIO_FN(WAIT),
+	GPIO_FN(RDWR),
+	GPIO_FN(CKE),
+	GPIO_FN(CASU),
+	GPIO_FN(BREQ),
+	GPIO_FN(RASU),
+	GPIO_FN(BACK),
+	GPIO_FN(CASL),
+	GPIO_FN(RASL),
+	GPIO_FN(WE3_DQMUU_AH_ICIO_WR),
+	GPIO_FN(WE2_DQMUL_ICIORD),
+	GPIO_FN(WE1_DQMLU_WE),
+	GPIO_FN(WE0_DQMLL),
+	GPIO_FN(CS3),
+	GPIO_FN(CS2),
+	GPIO_FN(A1),
+	GPIO_FN(A0),
+	GPIO_FN(CS7),
 
 	/* TMU */
-	PINMUX_GPIO(GPIO_FN_TIOC4D, TIOC4D_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC4C, TIOC4C_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC4B, TIOC4B_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC4A, TIOC4A_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC3D, TIOC3D_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC3C, TIOC3C_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC3B, TIOC3B_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC3A, TIOC3A_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC2B, TIOC2B_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC1B, TIOC1B_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC2A, TIOC2A_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC1A, TIOC1A_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC0D, TIOC0D_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC0C, TIOC0C_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC0B, TIOC0B_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC0A, TIOC0A_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKD_PD, TCLKD_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKC_PD, TCLKC_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKB_PD, TCLKB_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKA_PD, TCLKA_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKD_PF, TCLKD_PF_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKC_PF, TCLKC_PF_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKB_PF, TCLKB_PF_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKA_PF, TCLKA_PF_MARK),
+	GPIO_FN(TIOC4D),
+	GPIO_FN(TIOC4C),
+	GPIO_FN(TIOC4B),
+	GPIO_FN(TIOC4A),
+	GPIO_FN(TIOC3D),
+	GPIO_FN(TIOC3C),
+	GPIO_FN(TIOC3B),
+	GPIO_FN(TIOC3A),
+	GPIO_FN(TIOC2B),
+	GPIO_FN(TIOC1B),
+	GPIO_FN(TIOC2A),
+	GPIO_FN(TIOC1A),
+	GPIO_FN(TIOC0D),
+	GPIO_FN(TIOC0C),
+	GPIO_FN(TIOC0B),
+	GPIO_FN(TIOC0A),
+	GPIO_FN(TCLKD_PD),
+	GPIO_FN(TCLKC_PD),
+	GPIO_FN(TCLKB_PD),
+	GPIO_FN(TCLKA_PD),
+	GPIO_FN(TCLKD_PF),
+	GPIO_FN(TCLKC_PF),
+	GPIO_FN(TCLKB_PF),
+	GPIO_FN(TCLKA_PF),
 
 	/* SSU */
-	PINMUX_GPIO(GPIO_FN_SCS0_PD, SCS0_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_SSO0_PD, SSO0_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI0_PD, SSI0_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_SSCK0_PD, SSCK0_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCS0_PF, SCS0_PF_MARK),
-	PINMUX_GPIO(GPIO_FN_SSO0_PF, SSO0_PF_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI0_PF, SSI0_PF_MARK),
-	PINMUX_GPIO(GPIO_FN_SSCK0_PF, SSCK0_PF_MARK),
-	PINMUX_GPIO(GPIO_FN_SCS1_PD, SCS1_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_SSO1_PD, SSO1_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI1_PD, SSI1_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_SSCK1_PD, SSCK1_PD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCS1_PF, SCS1_PF_MARK),
-	PINMUX_GPIO(GPIO_FN_SSO1_PF, SSO1_PF_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI1_PF, SSI1_PF_MARK),
-	PINMUX_GPIO(GPIO_FN_SSCK1_PF, SSCK1_PF_MARK),
+	GPIO_FN(SCS0_PD),
+	GPIO_FN(SSO0_PD),
+	GPIO_FN(SSI0_PD),
+	GPIO_FN(SSCK0_PD),
+	GPIO_FN(SCS0_PF),
+	GPIO_FN(SSO0_PF),
+	GPIO_FN(SSI0_PF),
+	GPIO_FN(SSCK0_PF),
+	GPIO_FN(SCS1_PD),
+	GPIO_FN(SSO1_PD),
+	GPIO_FN(SSI1_PD),
+	GPIO_FN(SSCK1_PD),
+	GPIO_FN(SCS1_PF),
+	GPIO_FN(SSO1_PF),
+	GPIO_FN(SSI1_PF),
+	GPIO_FN(SSCK1_PF),
 
 	/* SCIF */
-	PINMUX_GPIO(GPIO_FN_TXD0, TXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD0, RXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK0, SCK0_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD1, TXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD1, RXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK1, SCK1_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD2, TXD2_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD2, RXD2_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK2, SCK2_MARK),
-	PINMUX_GPIO(GPIO_FN_RTS3, RTS3_MARK),
-	PINMUX_GPIO(GPIO_FN_CTS3, CTS3_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD3, TXD3_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD3, RXD3_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK3, SCK3_MARK),
+	GPIO_FN(TXD0),
+	GPIO_FN(RXD0),
+	GPIO_FN(SCK0),
+	GPIO_FN(TXD1),
+	GPIO_FN(RXD1),
+	GPIO_FN(SCK1),
+	GPIO_FN(TXD2),
+	GPIO_FN(RXD2),
+	GPIO_FN(SCK2),
+	GPIO_FN(RTS3),
+	GPIO_FN(CTS3),
+	GPIO_FN(TXD3),
+	GPIO_FN(RXD3),
+	GPIO_FN(SCK3),
 
 	/* SSI */
-	PINMUX_GPIO(GPIO_FN_AUDIO_CLK, AUDIO_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIDATA3, SSIDATA3_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIWS3, SSIWS3_MARK),
-	PINMUX_GPIO(GPIO_FN_SSISCK3, SSISCK3_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIDATA2, SSIDATA2_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIWS2, SSIWS2_MARK),
-	PINMUX_GPIO(GPIO_FN_SSISCK2, SSISCK2_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIDATA1, SSIDATA1_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIWS1, SSIWS1_MARK),
-	PINMUX_GPIO(GPIO_FN_SSISCK1, SSISCK1_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIDATA0, SSIDATA0_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIWS0, SSIWS0_MARK),
-	PINMUX_GPIO(GPIO_FN_SSISCK0, SSISCK0_MARK),
+	GPIO_FN(AUDIO_CLK),
+	GPIO_FN(SSIDATA3),
+	GPIO_FN(SSIWS3),
+	GPIO_FN(SSISCK3),
+	GPIO_FN(SSIDATA2),
+	GPIO_FN(SSIWS2),
+	GPIO_FN(SSISCK2),
+	GPIO_FN(SSIDATA1),
+	GPIO_FN(SSIWS1),
+	GPIO_FN(SSISCK1),
+	GPIO_FN(SSIDATA0),
+	GPIO_FN(SSIWS0),
+	GPIO_FN(SSISCK0),
 
 	/* FLCTL */
-	PINMUX_GPIO(GPIO_FN_FCE, FCE_MARK),
-	PINMUX_GPIO(GPIO_FN_FRB, FRB_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF7, NAF7_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF6, NAF6_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF5, NAF5_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF4, NAF4_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF3, NAF3_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF2, NAF2_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF1, NAF1_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF0, NAF0_MARK),
-	PINMUX_GPIO(GPIO_FN_FSC, FSC_MARK),
-	PINMUX_GPIO(GPIO_FN_FOE, FOE_MARK),
-	PINMUX_GPIO(GPIO_FN_FCDE, FCDE_MARK),
-	PINMUX_GPIO(GPIO_FN_FWE, FWE_MARK),
+	GPIO_FN(FCE),
+	GPIO_FN(FRB),
+	GPIO_FN(NAF7),
+	GPIO_FN(NAF6),
+	GPIO_FN(NAF5),
+	GPIO_FN(NAF4),
+	GPIO_FN(NAF3),
+	GPIO_FN(NAF2),
+	GPIO_FN(NAF1),
+	GPIO_FN(NAF0),
+	GPIO_FN(FSC),
+	GPIO_FN(FOE),
+	GPIO_FN(FCDE),
+	GPIO_FN(FWE),
 
 	/* LCDC */
-	PINMUX_GPIO(GPIO_FN_LCD_VEPWC, LCD_VEPWC_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_VCPWC, LCD_VCPWC_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_CLK, LCD_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_FLM, LCD_FLM_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_M_DISP, LCD_M_DISP_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_CL2, LCD_CL2_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_CL1, LCD_CL1_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DON, LCD_DON_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA15, LCD_DATA15_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA14, LCD_DATA14_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA13, LCD_DATA13_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA12, LCD_DATA12_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA11, LCD_DATA11_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA10, LCD_DATA10_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA9, LCD_DATA9_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA8, LCD_DATA8_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA7, LCD_DATA7_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA6, LCD_DATA6_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA5, LCD_DATA5_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA4, LCD_DATA4_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA3, LCD_DATA3_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA2, LCD_DATA2_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA1, LCD_DATA1_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA0, LCD_DATA0_MARK),
+	GPIO_FN(LCD_VEPWC),
+	GPIO_FN(LCD_VCPWC),
+	GPIO_FN(LCD_CLK),
+	GPIO_FN(LCD_FLM),
+	GPIO_FN(LCD_M_DISP),
+	GPIO_FN(LCD_CL2),
+	GPIO_FN(LCD_CL1),
+	GPIO_FN(LCD_DON),
+	GPIO_FN(LCD_DATA15),
+	GPIO_FN(LCD_DATA14),
+	GPIO_FN(LCD_DATA13),
+	GPIO_FN(LCD_DATA12),
+	GPIO_FN(LCD_DATA11),
+	GPIO_FN(LCD_DATA10),
+	GPIO_FN(LCD_DATA9),
+	GPIO_FN(LCD_DATA8),
+	GPIO_FN(LCD_DATA7),
+	GPIO_FN(LCD_DATA6),
+	GPIO_FN(LCD_DATA5),
+	GPIO_FN(LCD_DATA4),
+	GPIO_FN(LCD_DATA3),
+	GPIO_FN(LCD_DATA2),
+	GPIO_FN(LCD_DATA1),
+	GPIO_FN(LCD_DATA0),
 };
 
-static struct pinmux_cfg_reg pinmux_config_regs[] = {
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ PINMUX_CFG_REG("PBIORL", 0xfffe3886, 16, 1) {
 		0, 0,
 		0, 0,
@@ -1525,7 +1529,7 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{}
 };
 
-static struct pinmux_data_reg pinmux_data_regs[] = {
+static const struct pinmux_data_reg pinmux_data_regs[] = {
 	{ PINMUX_DATA_REG("PADRL", 0xfffe3802, 16) {
 		0, 0, 0, 0,
 		0, 0, 0, 0,
@@ -1571,19 +1575,17 @@ static struct pinmux_data_reg pinmux_data_regs[] = {
 	{ },
 };
 
-struct sh_pfc_soc_info sh7203_pinmux_info = {
+const struct sh_pfc_soc_info sh7203_pinmux_info = {
 	.name = "sh7203_pfc",
-	.reserved_id = PINMUX_RESERVED,
-	.data = { PINMUX_DATA_BEGIN, PINMUX_DATA_END },
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END, FORCE_IN },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END, FORCE_OUT },
-	.mark = { PINMUX_MARK_BEGIN, PINMUX_MARK_END },
 	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
 
-	.first_gpio = GPIO_PA7,
-	.last_gpio = GPIO_FN_LCD_DATA0,
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.func_gpios = pinmux_func_gpios,
+	.nr_func_gpios = ARRAY_SIZE(pinmux_func_gpios),
 
-	.gpios = pinmux_gpios,
 	.cfg_regs = pinmux_config_regs,
 	.data_regs = pinmux_data_regs,
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh7264.c b/drivers/pinctrl/sh-pfc/pfc-sh7264.c
index 2ba5639dcf34..284675249ed9 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh7264.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh7264.c
@@ -604,7 +604,7 @@ enum {
 	PINMUX_MARK_END,
 };
 
-static pinmux_enum_t pinmux_data[] = {
+static const pinmux_enum_t pinmux_data[] = {
 
 	/* Port A */
 	PINMUX_DATA(PA3_DATA, PA3_IN),
@@ -1072,7 +1072,7 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA(SD_D2_MARK, PK0MD_10),
 };
 
-static struct pinmux_gpio pinmux_gpios[] = {
+static struct sh_pfc_pin pinmux_pins[] = {
 
 	/* Port A */
 	PINMUX_GPIO(GPIO_PA3, PA3_DATA),
@@ -1216,257 +1216,261 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	PINMUX_GPIO(GPIO_PK2, PK2_DATA),
 	PINMUX_GPIO(GPIO_PK1, PK1_DATA),
 	PINMUX_GPIO(GPIO_PK0, PK0_DATA),
+};
+
+#define PINMUX_FN_BASE	ARRAY_SIZE(pinmux_pins)
 
+static const struct pinmux_func pinmux_func_gpios[] = {
 	/* INTC */
-	PINMUX_GPIO(GPIO_FN_PINT7_PG, PINT7_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT6_PG, PINT6_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT5_PG, PINT5_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT4_PG, PINT4_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT3_PG, PINT3_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT2_PG, PINT2_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT1_PG, PINT1_PG_MARK),
-
-	PINMUX_GPIO(GPIO_FN_IRQ7_PC, IRQ7_PC_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ6_PC, IRQ6_PC_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ5_PC, IRQ5_PC_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ4_PC, IRQ4_PC_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ3_PG, IRQ3_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ2_PG, IRQ2_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ1_PJ, IRQ1_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ0_PJ, IRQ0_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ3_PE, IRQ3_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ2_PE, IRQ2_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ1_PE, IRQ1_PE_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ0_PE, IRQ0_PE_MARK),
+	GPIO_FN(PINT7_PG),
+	GPIO_FN(PINT6_PG),
+	GPIO_FN(PINT5_PG),
+	GPIO_FN(PINT4_PG),
+	GPIO_FN(PINT3_PG),
+	GPIO_FN(PINT2_PG),
+	GPIO_FN(PINT1_PG),
+
+	GPIO_FN(IRQ7_PC),
+	GPIO_FN(IRQ6_PC),
+	GPIO_FN(IRQ5_PC),
+	GPIO_FN(IRQ4_PC),
+	GPIO_FN(IRQ3_PG),
+	GPIO_FN(IRQ2_PG),
+	GPIO_FN(IRQ1_PJ),
+	GPIO_FN(IRQ0_PJ),
+	GPIO_FN(IRQ3_PE),
+	GPIO_FN(IRQ2_PE),
+	GPIO_FN(IRQ1_PE),
+	GPIO_FN(IRQ0_PE),
 
 	/* WDT */
-	PINMUX_GPIO(GPIO_FN_WDTOVF, WDTOVF_MARK),
+	GPIO_FN(WDTOVF),
 
 	/* CAN */
-	PINMUX_GPIO(GPIO_FN_CTX1, CTX1_MARK),
-	PINMUX_GPIO(GPIO_FN_CRX1, CRX1_MARK),
-	PINMUX_GPIO(GPIO_FN_CTX0, CTX0_MARK),
-	PINMUX_GPIO(GPIO_FN_CRX0, CRX0_MARK),
-	PINMUX_GPIO(GPIO_FN_CRX0_CRX1, CRX0_CRX1_MARK),
+	GPIO_FN(CTX1),
+	GPIO_FN(CRX1),
+	GPIO_FN(CTX0),
+	GPIO_FN(CRX0),
+	GPIO_FN(CRX0_CRX1),
 
 	/* DMAC */
-	PINMUX_GPIO(GPIO_FN_TEND0, TEND0_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK0, DACK0_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ0, DREQ0_MARK),
-	PINMUX_GPIO(GPIO_FN_TEND1, TEND1_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK1, DACK1_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ1, DREQ1_MARK),
+	GPIO_FN(TEND0),
+	GPIO_FN(DACK0),
+	GPIO_FN(DREQ0),
+	GPIO_FN(TEND1),
+	GPIO_FN(DACK1),
+	GPIO_FN(DREQ1),
 
 	/* ADC */
-	PINMUX_GPIO(GPIO_FN_ADTRG, ADTRG_MARK),
+	GPIO_FN(ADTRG),
 
 	/* BSCh */
-	PINMUX_GPIO(GPIO_FN_A25, A25_MARK),
-	PINMUX_GPIO(GPIO_FN_A24, A24_MARK),
-	PINMUX_GPIO(GPIO_FN_A23, A23_MARK),
-	PINMUX_GPIO(GPIO_FN_A22, A22_MARK),
-	PINMUX_GPIO(GPIO_FN_A21, A21_MARK),
-	PINMUX_GPIO(GPIO_FN_A20, A20_MARK),
-	PINMUX_GPIO(GPIO_FN_A19, A19_MARK),
-	PINMUX_GPIO(GPIO_FN_A18, A18_MARK),
-	PINMUX_GPIO(GPIO_FN_A17, A17_MARK),
-	PINMUX_GPIO(GPIO_FN_A16, A16_MARK),
-	PINMUX_GPIO(GPIO_FN_A15, A15_MARK),
-	PINMUX_GPIO(GPIO_FN_A14, A14_MARK),
-	PINMUX_GPIO(GPIO_FN_A13, A13_MARK),
-	PINMUX_GPIO(GPIO_FN_A12, A12_MARK),
-	PINMUX_GPIO(GPIO_FN_A11, A11_MARK),
-	PINMUX_GPIO(GPIO_FN_A10, A10_MARK),
-	PINMUX_GPIO(GPIO_FN_A9, A9_MARK),
-	PINMUX_GPIO(GPIO_FN_A8, A8_MARK),
-	PINMUX_GPIO(GPIO_FN_A7, A7_MARK),
-	PINMUX_GPIO(GPIO_FN_A6, A6_MARK),
-	PINMUX_GPIO(GPIO_FN_A5, A5_MARK),
-	PINMUX_GPIO(GPIO_FN_A4, A4_MARK),
-	PINMUX_GPIO(GPIO_FN_A3, A3_MARK),
-	PINMUX_GPIO(GPIO_FN_A2, A2_MARK),
-	PINMUX_GPIO(GPIO_FN_A1, A1_MARK),
-	PINMUX_GPIO(GPIO_FN_A0, A0_MARK),
-
-	PINMUX_GPIO(GPIO_FN_D15, D15_MARK),
-	PINMUX_GPIO(GPIO_FN_D14, D14_MARK),
-	PINMUX_GPIO(GPIO_FN_D13, D13_MARK),
-	PINMUX_GPIO(GPIO_FN_D12, D12_MARK),
-	PINMUX_GPIO(GPIO_FN_D11, D11_MARK),
-	PINMUX_GPIO(GPIO_FN_D10, D10_MARK),
-	PINMUX_GPIO(GPIO_FN_D9, D9_MARK),
-	PINMUX_GPIO(GPIO_FN_D8, D8_MARK),
-	PINMUX_GPIO(GPIO_FN_D7, D7_MARK),
-	PINMUX_GPIO(GPIO_FN_D6, D6_MARK),
-	PINMUX_GPIO(GPIO_FN_D5, D5_MARK),
-	PINMUX_GPIO(GPIO_FN_D4, D4_MARK),
-	PINMUX_GPIO(GPIO_FN_D3, D3_MARK),
-	PINMUX_GPIO(GPIO_FN_D2, D2_MARK),
-	PINMUX_GPIO(GPIO_FN_D1, D1_MARK),
-	PINMUX_GPIO(GPIO_FN_D0, D0_MARK),
-
-	PINMUX_GPIO(GPIO_FN_BS, BS_MARK),
-	PINMUX_GPIO(GPIO_FN_CS4, CS4_MARK),
-	PINMUX_GPIO(GPIO_FN_CS3, CS3_MARK),
-	PINMUX_GPIO(GPIO_FN_CS2, CS2_MARK),
-	PINMUX_GPIO(GPIO_FN_CS1, CS1_MARK),
-	PINMUX_GPIO(GPIO_FN_CS0, CS0_MARK),
-	PINMUX_GPIO(GPIO_FN_CS6CE1B, CS6CE1B_MARK),
-	PINMUX_GPIO(GPIO_FN_CS5CE1A, CS5CE1A_MARK),
-	PINMUX_GPIO(GPIO_FN_CE2A, CE2A_MARK),
-	PINMUX_GPIO(GPIO_FN_CE2B, CE2B_MARK),
-	PINMUX_GPIO(GPIO_FN_RD, RD_MARK),
-	PINMUX_GPIO(GPIO_FN_RDWR, RDWR_MARK),
-	PINMUX_GPIO(GPIO_FN_ICIOWRAH, ICIOWRAH_MARK),
-	PINMUX_GPIO(GPIO_FN_ICIORD, ICIORD_MARK),
-	PINMUX_GPIO(GPIO_FN_WE1DQMUWE, WE1DQMUWE_MARK),
-	PINMUX_GPIO(GPIO_FN_WE0DQML, WE0DQML_MARK),
-	PINMUX_GPIO(GPIO_FN_RAS, RAS_MARK),
-	PINMUX_GPIO(GPIO_FN_CAS, CAS_MARK),
-	PINMUX_GPIO(GPIO_FN_CKE, CKE_MARK),
-	PINMUX_GPIO(GPIO_FN_WAIT, WAIT_MARK),
-	PINMUX_GPIO(GPIO_FN_BREQ, BREQ_MARK),
-	PINMUX_GPIO(GPIO_FN_BACK, BACK_MARK),
-	PINMUX_GPIO(GPIO_FN_IOIS16, IOIS16_MARK),
+	GPIO_FN(A25),
+	GPIO_FN(A24),
+	GPIO_FN(A23),
+	GPIO_FN(A22),
+	GPIO_FN(A21),
+	GPIO_FN(A20),
+	GPIO_FN(A19),
+	GPIO_FN(A18),
+	GPIO_FN(A17),
+	GPIO_FN(A16),
+	GPIO_FN(A15),
+	GPIO_FN(A14),
+	GPIO_FN(A13),
+	GPIO_FN(A12),
+	GPIO_FN(A11),
+	GPIO_FN(A10),
+	GPIO_FN(A9),
+	GPIO_FN(A8),
+	GPIO_FN(A7),
+	GPIO_FN(A6),
+	GPIO_FN(A5),
+	GPIO_FN(A4),
+	GPIO_FN(A3),
+	GPIO_FN(A2),
+	GPIO_FN(A1),
+	GPIO_FN(A0),
+
+	GPIO_FN(D15),
+	GPIO_FN(D14),
+	GPIO_FN(D13),
+	GPIO_FN(D12),
+	GPIO_FN(D11),
+	GPIO_FN(D10),
+	GPIO_FN(D9),
+	GPIO_FN(D8),
+	GPIO_FN(D7),
+	GPIO_FN(D6),
+	GPIO_FN(D5),
+	GPIO_FN(D4),
+	GPIO_FN(D3),
+	GPIO_FN(D2),
+	GPIO_FN(D1),
+	GPIO_FN(D0),
+
+	GPIO_FN(BS),
+	GPIO_FN(CS4),
+	GPIO_FN(CS3),
+	GPIO_FN(CS2),
+	GPIO_FN(CS1),
+	GPIO_FN(CS0),
+	GPIO_FN(CS6CE1B),
+	GPIO_FN(CS5CE1A),
+	GPIO_FN(CE2A),
+	GPIO_FN(CE2B),
+	GPIO_FN(RD),
+	GPIO_FN(RDWR),
+	GPIO_FN(ICIOWRAH),
+	GPIO_FN(ICIORD),
+	GPIO_FN(WE1DQMUWE),
+	GPIO_FN(WE0DQML),
+	GPIO_FN(RAS),
+	GPIO_FN(CAS),
+	GPIO_FN(CKE),
+	GPIO_FN(WAIT),
+	GPIO_FN(BREQ),
+	GPIO_FN(BACK),
+	GPIO_FN(IOIS16),
 
 	/* TMU */
-	PINMUX_GPIO(GPIO_FN_TIOC4D, TIOC4D_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC4C, TIOC4C_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC4B, TIOC4B_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC4A, TIOC4A_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC3D, TIOC3D_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC3C, TIOC3C_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC3B, TIOC3B_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC3A, TIOC3A_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC2B, TIOC2B_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC1B, TIOC1B_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC2A, TIOC2A_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC1A, TIOC1A_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC0D, TIOC0D_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC0C, TIOC0C_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC0B, TIOC0B_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC0A, TIOC0A_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKD, TCLKD_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKC, TCLKC_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKB, TCLKB_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKA, TCLKA_MARK),
+	GPIO_FN(TIOC4D),
+	GPIO_FN(TIOC4C),
+	GPIO_FN(TIOC4B),
+	GPIO_FN(TIOC4A),
+	GPIO_FN(TIOC3D),
+	GPIO_FN(TIOC3C),
+	GPIO_FN(TIOC3B),
+	GPIO_FN(TIOC3A),
+	GPIO_FN(TIOC2B),
+	GPIO_FN(TIOC1B),
+	GPIO_FN(TIOC2A),
+	GPIO_FN(TIOC1A),
+	GPIO_FN(TIOC0D),
+	GPIO_FN(TIOC0C),
+	GPIO_FN(TIOC0B),
+	GPIO_FN(TIOC0A),
+	GPIO_FN(TCLKD),
+	GPIO_FN(TCLKC),
+	GPIO_FN(TCLKB),
+	GPIO_FN(TCLKA),
 
 	/* SCIF */
-	PINMUX_GPIO(GPIO_FN_TXD0, TXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD0, RXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK0, SCK0_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD1, TXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD1, RXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK1, SCK1_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD2, TXD2_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD2, RXD2_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK2, SCK2_MARK),
-	PINMUX_GPIO(GPIO_FN_RTS3, RTS3_MARK),
-	PINMUX_GPIO(GPIO_FN_CTS3, CTS3_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD3, TXD3_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD3, RXD3_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK3, SCK3_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD4, TXD4_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD4, RXD4_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD5, TXD5_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD5, RXD5_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD6, TXD6_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD6, RXD6_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD7, TXD7_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD7, RXD7_MARK),
-	PINMUX_GPIO(GPIO_FN_RTS1, RTS1_MARK),
-	PINMUX_GPIO(GPIO_FN_CTS1, CTS1_MARK),
+	GPIO_FN(TXD0),
+	GPIO_FN(RXD0),
+	GPIO_FN(SCK0),
+	GPIO_FN(TXD1),
+	GPIO_FN(RXD1),
+	GPIO_FN(SCK1),
+	GPIO_FN(TXD2),
+	GPIO_FN(RXD2),
+	GPIO_FN(SCK2),
+	GPIO_FN(RTS3),
+	GPIO_FN(CTS3),
+	GPIO_FN(TXD3),
+	GPIO_FN(RXD3),
+	GPIO_FN(SCK3),
+	GPIO_FN(TXD4),
+	GPIO_FN(RXD4),
+	GPIO_FN(TXD5),
+	GPIO_FN(RXD5),
+	GPIO_FN(TXD6),
+	GPIO_FN(RXD6),
+	GPIO_FN(TXD7),
+	GPIO_FN(RXD7),
+	GPIO_FN(RTS1),
+	GPIO_FN(CTS1),
 
 	/* RSPI */
-	PINMUX_GPIO(GPIO_FN_RSPCK0, RSPCK0_MARK),
-	PINMUX_GPIO(GPIO_FN_MOSI0, MOSI0_MARK),
-	PINMUX_GPIO(GPIO_FN_MISO0_PF12, MISO0_PF12_MARK),
-	PINMUX_GPIO(GPIO_FN_MISO1, MISO1_MARK),
-	PINMUX_GPIO(GPIO_FN_SSL00, SSL00_MARK),
-	PINMUX_GPIO(GPIO_FN_RSPCK1, RSPCK1_MARK),
-	PINMUX_GPIO(GPIO_FN_MOSI1, MOSI1_MARK),
-	PINMUX_GPIO(GPIO_FN_MISO1_PG19, MISO1_PG19_MARK),
-	PINMUX_GPIO(GPIO_FN_SSL10, SSL10_MARK),
+	GPIO_FN(RSPCK0),
+	GPIO_FN(MOSI0),
+	GPIO_FN(MISO0_PF12),
+	GPIO_FN(MISO1),
+	GPIO_FN(SSL00),
+	GPIO_FN(RSPCK1),
+	GPIO_FN(MOSI1),
+	GPIO_FN(MISO1_PG19),
+	GPIO_FN(SSL10),
 
 	/* IIC3 */
-	PINMUX_GPIO(GPIO_FN_SCL0, SCL0_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL1, SCL1_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL2, SCL2_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA0, SDA0_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA1, SDA1_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA2, SDA2_MARK),
+	GPIO_FN(SCL0),
+	GPIO_FN(SCL1),
+	GPIO_FN(SCL2),
+	GPIO_FN(SDA0),
+	GPIO_FN(SDA1),
+	GPIO_FN(SDA2),
 
 	/* SSI */
-	PINMUX_GPIO(GPIO_FN_SSISCK0, SSISCK0_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIWS0, SSIWS0_MARK),
-	PINMUX_GPIO(GPIO_FN_SSITXD0, SSITXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIRXD0, SSIRXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIWS1, SSIWS1_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIWS2, SSIWS2_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIWS3, SSIWS3_MARK),
-	PINMUX_GPIO(GPIO_FN_SSISCK1, SSISCK1_MARK),
-	PINMUX_GPIO(GPIO_FN_SSISCK2, SSISCK2_MARK),
-	PINMUX_GPIO(GPIO_FN_SSISCK3, SSISCK3_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIDATA1, SSIDATA1_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIDATA2, SSIDATA2_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIDATA3, SSIDATA3_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDIO_CLK, AUDIO_CLK_MARK),
+	GPIO_FN(SSISCK0),
+	GPIO_FN(SSIWS0),
+	GPIO_FN(SSITXD0),
+	GPIO_FN(SSIRXD0),
+	GPIO_FN(SSIWS1),
+	GPIO_FN(SSIWS2),
+	GPIO_FN(SSIWS3),
+	GPIO_FN(SSISCK1),
+	GPIO_FN(SSISCK2),
+	GPIO_FN(SSISCK3),
+	GPIO_FN(SSIDATA1),
+	GPIO_FN(SSIDATA2),
+	GPIO_FN(SSIDATA3),
+	GPIO_FN(AUDIO_CLK),
 
 	/* SIOF */ /* NOTE Shares AUDIO_CLK with SSI */
-	PINMUX_GPIO(GPIO_FN_SIOFTXD, SIOFTXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOFRXD, SIOFRXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOFSYNC, SIOFSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOFSCK, SIOFSCK_MARK),
+	GPIO_FN(SIOFTXD),
+	GPIO_FN(SIOFRXD),
+	GPIO_FN(SIOFSYNC),
+	GPIO_FN(SIOFSCK),
 
 	/* SPDIF */ /* NOTE Shares AUDIO_CLK with SSI */
-	PINMUX_GPIO(GPIO_FN_SPDIF_IN, SPDIF_IN_MARK),
-	PINMUX_GPIO(GPIO_FN_SPDIF_OUT, SPDIF_OUT_MARK),
+	GPIO_FN(SPDIF_IN),
+	GPIO_FN(SPDIF_OUT),
 
 	/* NANDFMC */ /* NOTE Controller is not available in boot mode 0 */
-	PINMUX_GPIO(GPIO_FN_FCE, FCE_MARK),
-	PINMUX_GPIO(GPIO_FN_FRB, FRB_MARK),
+	GPIO_FN(FCE),
+	GPIO_FN(FRB),
 
 	/* VDC3 */
-	PINMUX_GPIO(GPIO_FN_DV_CLK, DV_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_VSYNC, DV_VSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_HSYNC, DV_HSYNC_MARK),
-
-	PINMUX_GPIO(GPIO_FN_DV_DATA7, DV_DATA7_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA6, DV_DATA6_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA5, DV_DATA5_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA4, DV_DATA4_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA3, DV_DATA3_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA2, DV_DATA2_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA1, DV_DATA1_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA0, DV_DATA0_MARK),
-
-	PINMUX_GPIO(GPIO_FN_LCD_CLK, LCD_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_EXTCLK, LCD_EXTCLK_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_VSYNC, LCD_VSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_HSYNC, LCD_HSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DE, LCD_DE_MARK),
-
-	PINMUX_GPIO(GPIO_FN_LCD_DATA15, LCD_DATA15_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA14, LCD_DATA14_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA13, LCD_DATA13_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA12, LCD_DATA12_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA11, LCD_DATA11_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA10, LCD_DATA10_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA9, LCD_DATA9_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA8, LCD_DATA8_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA7, LCD_DATA7_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA6, LCD_DATA6_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA5, LCD_DATA5_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA4, LCD_DATA4_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA3, LCD_DATA3_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA2, LCD_DATA2_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA1, LCD_DATA1_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA0, LCD_DATA0_MARK),
-
-	PINMUX_GPIO(GPIO_FN_LCD_M_DISP, LCD_M_DISP_MARK),
+	GPIO_FN(DV_CLK),
+	GPIO_FN(DV_VSYNC),
+	GPIO_FN(DV_HSYNC),
+
+	GPIO_FN(DV_DATA7),
+	GPIO_FN(DV_DATA6),
+	GPIO_FN(DV_DATA5),
+	GPIO_FN(DV_DATA4),
+	GPIO_FN(DV_DATA3),
+	GPIO_FN(DV_DATA2),
+	GPIO_FN(DV_DATA1),
+	GPIO_FN(DV_DATA0),
+
+	GPIO_FN(LCD_CLK),
+	GPIO_FN(LCD_EXTCLK),
+	GPIO_FN(LCD_VSYNC),
+	GPIO_FN(LCD_HSYNC),
+	GPIO_FN(LCD_DE),
+
+	GPIO_FN(LCD_DATA15),
+	GPIO_FN(LCD_DATA14),
+	GPIO_FN(LCD_DATA13),
+	GPIO_FN(LCD_DATA12),
+	GPIO_FN(LCD_DATA11),
+	GPIO_FN(LCD_DATA10),
+	GPIO_FN(LCD_DATA9),
+	GPIO_FN(LCD_DATA8),
+	GPIO_FN(LCD_DATA7),
+	GPIO_FN(LCD_DATA6),
+	GPIO_FN(LCD_DATA5),
+	GPIO_FN(LCD_DATA4),
+	GPIO_FN(LCD_DATA3),
+	GPIO_FN(LCD_DATA2),
+	GPIO_FN(LCD_DATA1),
+	GPIO_FN(LCD_DATA0),
+
+	GPIO_FN(LCD_M_DISP),
 };
 
-static struct pinmux_cfg_reg pinmux_config_regs[] = {
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ PINMUX_CFG_REG("PAIOR0", 0xfffe3812, 16, 1) {
 		0, 0, 0, 0, 0, 0, 0, 0,
 		0, 0, 0, 0, 0, 0, 0, 0,
@@ -2032,7 +2036,7 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{}
 };
 
-static struct pinmux_data_reg pinmux_data_regs[] = {
+static const struct pinmux_data_reg pinmux_data_regs[] = {
 	{ PINMUX_DATA_REG("PADR1", 0xfffe3814, 16) {
 		0, 0, 0, 0, 0, 0, 0, PA3_DATA,
 		0, 0, 0, 0, 0, 0, 0, PA2_DATA }
@@ -2110,19 +2114,17 @@ static struct pinmux_data_reg pinmux_data_regs[] = {
 	{ }
 };
 
-struct sh_pfc_soc_info sh7264_pinmux_info = {
+const struct sh_pfc_soc_info sh7264_pinmux_info = {
 	.name = "sh7264_pfc",
-	.reserved_id = PINMUX_RESERVED,
-	.data = { PINMUX_DATA_BEGIN, PINMUX_DATA_END },
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END, FORCE_IN },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END, FORCE_OUT },
-	.mark = { PINMUX_MARK_BEGIN, PINMUX_MARK_END },
 	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
 
-	.first_gpio = GPIO_PA3,
-	.last_gpio = GPIO_FN_LCD_M_DISP,
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.func_gpios = pinmux_func_gpios,
+	.nr_func_gpios = ARRAY_SIZE(pinmux_func_gpios),
 
-	.gpios = pinmux_gpios,
 	.cfg_regs = pinmux_config_regs,
 	.data_regs = pinmux_data_regs,
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh7269.c b/drivers/pinctrl/sh-pfc/pfc-sh7269.c
index b1b5d6d4ad76..4c401a74acd5 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh7269.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh7269.c
@@ -781,7 +781,7 @@ enum {
 	PINMUX_MARK_END,
 };
 
-static pinmux_enum_t pinmux_data[] = {
+static const pinmux_enum_t pinmux_data[] = {
 
 	/* Port A */
 	PINMUX_DATA(PA1_DATA, PA1_IN),
@@ -1452,7 +1452,7 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA(PWM1A_MARK, PJ0MD_100),
 };
 
-static struct pinmux_gpio pinmux_gpios[] = {
+static struct sh_pfc_pin pinmux_pins[] = {
 	/* Port A */
 	PINMUX_GPIO(GPIO_PA1, PA1_DATA),
 	PINMUX_GPIO(GPIO_PA0, PA0_DATA),
@@ -1613,339 +1613,343 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	PINMUX_GPIO(GPIO_PJ2, PJ2_DATA),
 	PINMUX_GPIO(GPIO_PJ1, PJ1_DATA),
 	PINMUX_GPIO(GPIO_PJ0, PJ0_DATA),
+};
+
+#define PINMUX_FN_BASE	ARRAY_SIZE(pinmux_pins)
 
+static const struct pinmux_func pinmux_func_gpios[] = {
 	/* INTC */
-	PINMUX_GPIO(GPIO_FN_IRQ7_PG, IRQ7_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ6_PG, IRQ6_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ5_PG, IRQ5_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ4_PG, IRQ4_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ3_PG, IRQ3_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ2_PG, IRQ2_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ1_PG, IRQ1_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ0_PG, IRQ0_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ7_PF, IRQ7_PF_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ6_PF, IRQ6_PF_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ5_PF, IRQ5_PF_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ4_PF, IRQ4_PF_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ3_PJ, IRQ3_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ2_PJ, IRQ2_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ1_PJ, IRQ1_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ0_PJ, IRQ0_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ1_PC, IRQ1_PC_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ0_PC, IRQ0_PC_MARK),
-
-	PINMUX_GPIO(GPIO_FN_PINT7_PG, PINT7_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT6_PG, PINT6_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT5_PG, PINT5_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT4_PG, PINT4_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT3_PG, PINT3_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT2_PG, PINT2_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT1_PG, PINT1_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT0_PG, PINT0_PG_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT7_PH, PINT7_PH_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT6_PH, PINT6_PH_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT5_PH, PINT5_PH_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT4_PH, PINT4_PH_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT3_PH, PINT3_PH_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT2_PH, PINT2_PH_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT1_PH, PINT1_PH_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT0_PH, PINT0_PH_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT7_PJ, PINT7_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT6_PJ, PINT6_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT5_PJ, PINT5_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT4_PJ, PINT4_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT3_PJ, PINT3_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT2_PJ, PINT2_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT1_PJ, PINT1_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_PINT0_PJ, PINT0_PJ_MARK),
+	GPIO_FN(IRQ7_PG),
+	GPIO_FN(IRQ6_PG),
+	GPIO_FN(IRQ5_PG),
+	GPIO_FN(IRQ4_PG),
+	GPIO_FN(IRQ3_PG),
+	GPIO_FN(IRQ2_PG),
+	GPIO_FN(IRQ1_PG),
+	GPIO_FN(IRQ0_PG),
+	GPIO_FN(IRQ7_PF),
+	GPIO_FN(IRQ6_PF),
+	GPIO_FN(IRQ5_PF),
+	GPIO_FN(IRQ4_PF),
+	GPIO_FN(IRQ3_PJ),
+	GPIO_FN(IRQ2_PJ),
+	GPIO_FN(IRQ1_PJ),
+	GPIO_FN(IRQ0_PJ),
+	GPIO_FN(IRQ1_PC),
+	GPIO_FN(IRQ0_PC),
+
+	GPIO_FN(PINT7_PG),
+	GPIO_FN(PINT6_PG),
+	GPIO_FN(PINT5_PG),
+	GPIO_FN(PINT4_PG),
+	GPIO_FN(PINT3_PG),
+	GPIO_FN(PINT2_PG),
+	GPIO_FN(PINT1_PG),
+	GPIO_FN(PINT0_PG),
+	GPIO_FN(PINT7_PH),
+	GPIO_FN(PINT6_PH),
+	GPIO_FN(PINT5_PH),
+	GPIO_FN(PINT4_PH),
+	GPIO_FN(PINT3_PH),
+	GPIO_FN(PINT2_PH),
+	GPIO_FN(PINT1_PH),
+	GPIO_FN(PINT0_PH),
+	GPIO_FN(PINT7_PJ),
+	GPIO_FN(PINT6_PJ),
+	GPIO_FN(PINT5_PJ),
+	GPIO_FN(PINT4_PJ),
+	GPIO_FN(PINT3_PJ),
+	GPIO_FN(PINT2_PJ),
+	GPIO_FN(PINT1_PJ),
+	GPIO_FN(PINT0_PJ),
 
 	/* WDT */
-	PINMUX_GPIO(GPIO_FN_WDTOVF, WDTOVF_MARK),
+	GPIO_FN(WDTOVF),
 
 	/* CAN */
-	PINMUX_GPIO(GPIO_FN_CTX1, CTX1_MARK),
-	PINMUX_GPIO(GPIO_FN_CRX1, CRX1_MARK),
-	PINMUX_GPIO(GPIO_FN_CTX0, CTX0_MARK),
-	PINMUX_GPIO(GPIO_FN_CRX0, CRX0_MARK),
-	PINMUX_GPIO(GPIO_FN_CRX0_CRX1, CRX0_CRX1_MARK),
-	PINMUX_GPIO(GPIO_FN_CRX0_CRX1_CRX2, CRX0_CRX1_CRX2_MARK),
+	GPIO_FN(CTX1),
+	GPIO_FN(CRX1),
+	GPIO_FN(CTX0),
+	GPIO_FN(CRX0),
+	GPIO_FN(CRX0_CRX1),
+	GPIO_FN(CRX0_CRX1_CRX2),
 
 	/* DMAC */
-	PINMUX_GPIO(GPIO_FN_TEND0, TEND0_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK0, DACK0_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ0, DREQ0_MARK),
-	PINMUX_GPIO(GPIO_FN_TEND1, TEND1_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK1, DACK1_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ1, DREQ1_MARK),
+	GPIO_FN(TEND0),
+	GPIO_FN(DACK0),
+	GPIO_FN(DREQ0),
+	GPIO_FN(TEND1),
+	GPIO_FN(DACK1),
+	GPIO_FN(DREQ1),
 
 	/* ADC */
-	PINMUX_GPIO(GPIO_FN_ADTRG, ADTRG_MARK),
+	GPIO_FN(ADTRG),
 
 	/* BSCh */
-	PINMUX_GPIO(GPIO_FN_A25, A25_MARK),
-	PINMUX_GPIO(GPIO_FN_A24, A24_MARK),
-	PINMUX_GPIO(GPIO_FN_A23, A23_MARK),
-	PINMUX_GPIO(GPIO_FN_A22, A22_MARK),
-	PINMUX_GPIO(GPIO_FN_A21, A21_MARK),
-	PINMUX_GPIO(GPIO_FN_A20, A20_MARK),
-	PINMUX_GPIO(GPIO_FN_A19, A19_MARK),
-	PINMUX_GPIO(GPIO_FN_A18, A18_MARK),
-	PINMUX_GPIO(GPIO_FN_A17, A17_MARK),
-	PINMUX_GPIO(GPIO_FN_A16, A16_MARK),
-	PINMUX_GPIO(GPIO_FN_A15, A15_MARK),
-	PINMUX_GPIO(GPIO_FN_A14, A14_MARK),
-	PINMUX_GPIO(GPIO_FN_A13, A13_MARK),
-	PINMUX_GPIO(GPIO_FN_A12, A12_MARK),
-	PINMUX_GPIO(GPIO_FN_A11, A11_MARK),
-	PINMUX_GPIO(GPIO_FN_A10, A10_MARK),
-	PINMUX_GPIO(GPIO_FN_A9, A9_MARK),
-	PINMUX_GPIO(GPIO_FN_A8, A8_MARK),
-	PINMUX_GPIO(GPIO_FN_A7, A7_MARK),
-	PINMUX_GPIO(GPIO_FN_A6, A6_MARK),
-	PINMUX_GPIO(GPIO_FN_A5, A5_MARK),
-	PINMUX_GPIO(GPIO_FN_A4, A4_MARK),
-	PINMUX_GPIO(GPIO_FN_A3, A3_MARK),
-	PINMUX_GPIO(GPIO_FN_A2, A2_MARK),
-	PINMUX_GPIO(GPIO_FN_A1, A1_MARK),
-	PINMUX_GPIO(GPIO_FN_A0, A0_MARK),
-
-	PINMUX_GPIO(GPIO_FN_D15, D15_MARK),
-	PINMUX_GPIO(GPIO_FN_D14, D14_MARK),
-	PINMUX_GPIO(GPIO_FN_D13, D13_MARK),
-	PINMUX_GPIO(GPIO_FN_D12, D12_MARK),
-	PINMUX_GPIO(GPIO_FN_D11, D11_MARK),
-	PINMUX_GPIO(GPIO_FN_D10, D10_MARK),
-	PINMUX_GPIO(GPIO_FN_D9, D9_MARK),
-	PINMUX_GPIO(GPIO_FN_D8, D8_MARK),
-	PINMUX_GPIO(GPIO_FN_D7, D7_MARK),
-	PINMUX_GPIO(GPIO_FN_D6, D6_MARK),
-	PINMUX_GPIO(GPIO_FN_D5, D5_MARK),
-	PINMUX_GPIO(GPIO_FN_D4, D4_MARK),
-	PINMUX_GPIO(GPIO_FN_D3, D3_MARK),
-	PINMUX_GPIO(GPIO_FN_D2, D2_MARK),
-	PINMUX_GPIO(GPIO_FN_D1, D1_MARK),
-	PINMUX_GPIO(GPIO_FN_D0, D0_MARK),
-
-	PINMUX_GPIO(GPIO_FN_BS, BS_MARK),
-	PINMUX_GPIO(GPIO_FN_CS4, CS4_MARK),
-	PINMUX_GPIO(GPIO_FN_CS3, CS3_MARK),
-	PINMUX_GPIO(GPIO_FN_CS2, CS2_MARK),
-	PINMUX_GPIO(GPIO_FN_CS1, CS1_MARK),
-	PINMUX_GPIO(GPIO_FN_CS0, CS0_MARK),
-	PINMUX_GPIO(GPIO_FN_CS5CE1A, CS5CE1A_MARK),
-	PINMUX_GPIO(GPIO_FN_CE2A, CE2A_MARK),
-	PINMUX_GPIO(GPIO_FN_CE2B, CE2B_MARK),
-	PINMUX_GPIO(GPIO_FN_RD, RD_MARK),
-	PINMUX_GPIO(GPIO_FN_RDWR, RDWR_MARK),
-	PINMUX_GPIO(GPIO_FN_WE3ICIOWRAHDQMUU, WE3ICIOWRAHDQMUU_MARK),
-	PINMUX_GPIO(GPIO_FN_WE2ICIORDDQMUL, WE2ICIORDDQMUL_MARK),
-	PINMUX_GPIO(GPIO_FN_WE1DQMUWE, WE1DQMUWE_MARK),
-	PINMUX_GPIO(GPIO_FN_WE0DQML, WE0DQML_MARK),
-	PINMUX_GPIO(GPIO_FN_RAS, RAS_MARK),
-	PINMUX_GPIO(GPIO_FN_CAS, CAS_MARK),
-	PINMUX_GPIO(GPIO_FN_CKE, CKE_MARK),
-	PINMUX_GPIO(GPIO_FN_WAIT, WAIT_MARK),
-	PINMUX_GPIO(GPIO_FN_BREQ, BREQ_MARK),
-	PINMUX_GPIO(GPIO_FN_BACK, BACK_MARK),
-	PINMUX_GPIO(GPIO_FN_IOIS16, IOIS16_MARK),
+	GPIO_FN(A25),
+	GPIO_FN(A24),
+	GPIO_FN(A23),
+	GPIO_FN(A22),
+	GPIO_FN(A21),
+	GPIO_FN(A20),
+	GPIO_FN(A19),
+	GPIO_FN(A18),
+	GPIO_FN(A17),
+	GPIO_FN(A16),
+	GPIO_FN(A15),
+	GPIO_FN(A14),
+	GPIO_FN(A13),
+	GPIO_FN(A12),
+	GPIO_FN(A11),
+	GPIO_FN(A10),
+	GPIO_FN(A9),
+	GPIO_FN(A8),
+	GPIO_FN(A7),
+	GPIO_FN(A6),
+	GPIO_FN(A5),
+	GPIO_FN(A4),
+	GPIO_FN(A3),
+	GPIO_FN(A2),
+	GPIO_FN(A1),
+	GPIO_FN(A0),
+
+	GPIO_FN(D15),
+	GPIO_FN(D14),
+	GPIO_FN(D13),
+	GPIO_FN(D12),
+	GPIO_FN(D11),
+	GPIO_FN(D10),
+	GPIO_FN(D9),
+	GPIO_FN(D8),
+	GPIO_FN(D7),
+	GPIO_FN(D6),
+	GPIO_FN(D5),
+	GPIO_FN(D4),
+	GPIO_FN(D3),
+	GPIO_FN(D2),
+	GPIO_FN(D1),
+	GPIO_FN(D0),
+
+	GPIO_FN(BS),
+	GPIO_FN(CS4),
+	GPIO_FN(CS3),
+	GPIO_FN(CS2),
+	GPIO_FN(CS1),
+	GPIO_FN(CS0),
+	GPIO_FN(CS5CE1A),
+	GPIO_FN(CE2A),
+	GPIO_FN(CE2B),
+	GPIO_FN(RD),
+	GPIO_FN(RDWR),
+	GPIO_FN(WE3ICIOWRAHDQMUU),
+	GPIO_FN(WE2ICIORDDQMUL),
+	GPIO_FN(WE1DQMUWE),
+	GPIO_FN(WE0DQML),
+	GPIO_FN(RAS),
+	GPIO_FN(CAS),
+	GPIO_FN(CKE),
+	GPIO_FN(WAIT),
+	GPIO_FN(BREQ),
+	GPIO_FN(BACK),
+	GPIO_FN(IOIS16),
 
 	/* TMU */
-	PINMUX_GPIO(GPIO_FN_TIOC4D, TIOC4D_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC4C, TIOC4C_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC4B, TIOC4B_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC4A, TIOC4A_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC3D, TIOC3D_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC3C, TIOC3C_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC3B, TIOC3B_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC3A, TIOC3A_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC2B, TIOC2B_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC1B, TIOC1B_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC2A, TIOC2A_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC1A, TIOC1A_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC0D, TIOC0D_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC0C, TIOC0C_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC0B, TIOC0B_MARK),
-	PINMUX_GPIO(GPIO_FN_TIOC0A, TIOC0A_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKD, TCLKD_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKC, TCLKC_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKB, TCLKB_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLKA, TCLKA_MARK),
+	GPIO_FN(TIOC4D),
+	GPIO_FN(TIOC4C),
+	GPIO_FN(TIOC4B),
+	GPIO_FN(TIOC4A),
+	GPIO_FN(TIOC3D),
+	GPIO_FN(TIOC3C),
+	GPIO_FN(TIOC3B),
+	GPIO_FN(TIOC3A),
+	GPIO_FN(TIOC2B),
+	GPIO_FN(TIOC1B),
+	GPIO_FN(TIOC2A),
+	GPIO_FN(TIOC1A),
+	GPIO_FN(TIOC0D),
+	GPIO_FN(TIOC0C),
+	GPIO_FN(TIOC0B),
+	GPIO_FN(TIOC0A),
+	GPIO_FN(TCLKD),
+	GPIO_FN(TCLKC),
+	GPIO_FN(TCLKB),
+	GPIO_FN(TCLKA),
 
 	/* SCIF */
-	PINMUX_GPIO(GPIO_FN_SCK0, SCK0_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD0, TXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD0, RXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK1, SCK1_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD1, TXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD1, RXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_RTS1, RTS1_MARK),
-	PINMUX_GPIO(GPIO_FN_CTS1, CTS1_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK2, SCK2_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD2, TXD2_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD2, RXD2_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK3, SCK3_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD3, TXD3_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD3, RXD3_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK4, SCK4_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD4, TXD4_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD4, RXD4_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK5, SCK5_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD5, TXD5_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD5, RXD5_MARK),
-	PINMUX_GPIO(GPIO_FN_RTS5, RTS5_MARK),
-	PINMUX_GPIO(GPIO_FN_CTS5, CTS5_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK6, SCK6_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD6, TXD6_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD6, RXD6_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK7, SCK7_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD7, TXD7_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD7, RXD7_MARK),
-	PINMUX_GPIO(GPIO_FN_RTS7, RTS7_MARK),
-	PINMUX_GPIO(GPIO_FN_CTS7, CTS7_MARK),
+	GPIO_FN(SCK0),
+	GPIO_FN(TXD0),
+	GPIO_FN(RXD0),
+	GPIO_FN(SCK1),
+	GPIO_FN(TXD1),
+	GPIO_FN(RXD1),
+	GPIO_FN(RTS1),
+	GPIO_FN(CTS1),
+	GPIO_FN(SCK2),
+	GPIO_FN(TXD2),
+	GPIO_FN(RXD2),
+	GPIO_FN(SCK3),
+	GPIO_FN(TXD3),
+	GPIO_FN(RXD3),
+	GPIO_FN(SCK4),
+	GPIO_FN(TXD4),
+	GPIO_FN(RXD4),
+	GPIO_FN(SCK5),
+	GPIO_FN(TXD5),
+	GPIO_FN(RXD5),
+	GPIO_FN(RTS5),
+	GPIO_FN(CTS5),
+	GPIO_FN(SCK6),
+	GPIO_FN(TXD6),
+	GPIO_FN(RXD6),
+	GPIO_FN(SCK7),
+	GPIO_FN(TXD7),
+	GPIO_FN(RXD7),
+	GPIO_FN(RTS7),
+	GPIO_FN(CTS7),
 
 	/* RSPI */
-	PINMUX_GPIO(GPIO_FN_RSPCK0_PJ16, RSPCK0_PJ16_MARK),
-	PINMUX_GPIO(GPIO_FN_SSL00_PJ17, SSL00_PJ17_MARK),
-	PINMUX_GPIO(GPIO_FN_MOSI0_PJ18, MOSI0_PJ18_MARK),
-	PINMUX_GPIO(GPIO_FN_MISO0_PJ19, MISO0_PJ19_MARK),
-	PINMUX_GPIO(GPIO_FN_RSPCK0_PB17, RSPCK0_PB17_MARK),
-	PINMUX_GPIO(GPIO_FN_SSL00_PB18, SSL00_PB18_MARK),
-	PINMUX_GPIO(GPIO_FN_MOSI0_PB19, MOSI0_PB19_MARK),
-	PINMUX_GPIO(GPIO_FN_MISO0_PB20, MISO0_PB20_MARK),
-	PINMUX_GPIO(GPIO_FN_RSPCK1, RSPCK1_MARK),
-	PINMUX_GPIO(GPIO_FN_MOSI1, MOSI1_MARK),
-	PINMUX_GPIO(GPIO_FN_MISO1, MISO1_MARK),
-	PINMUX_GPIO(GPIO_FN_SSL10, SSL10_MARK),
+	GPIO_FN(RSPCK0_PJ16),
+	GPIO_FN(SSL00_PJ17),
+	GPIO_FN(MOSI0_PJ18),
+	GPIO_FN(MISO0_PJ19),
+	GPIO_FN(RSPCK0_PB17),
+	GPIO_FN(SSL00_PB18),
+	GPIO_FN(MOSI0_PB19),
+	GPIO_FN(MISO0_PB20),
+	GPIO_FN(RSPCK1),
+	GPIO_FN(MOSI1),
+	GPIO_FN(MISO1),
+	GPIO_FN(SSL10),
 
 	/* IIC3 */
-	PINMUX_GPIO(GPIO_FN_SCL0, SCL0_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL1, SCL1_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL2, SCL2_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA0, SDA0_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA1, SDA1_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA2, SDA2_MARK),
+	GPIO_FN(SCL0),
+	GPIO_FN(SCL1),
+	GPIO_FN(SCL2),
+	GPIO_FN(SDA0),
+	GPIO_FN(SDA1),
+	GPIO_FN(SDA2),
 
 	/* SSI */
-	PINMUX_GPIO(GPIO_FN_SSISCK0, SSISCK0_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIWS0, SSIWS0_MARK),
-	PINMUX_GPIO(GPIO_FN_SSITXD0, SSITXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIRXD0, SSIRXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIWS1, SSIWS1_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIWS2, SSIWS2_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIWS3, SSIWS3_MARK),
-	PINMUX_GPIO(GPIO_FN_SSISCK1, SSISCK1_MARK),
-	PINMUX_GPIO(GPIO_FN_SSISCK2, SSISCK2_MARK),
-	PINMUX_GPIO(GPIO_FN_SSISCK3, SSISCK3_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIDATA1, SSIDATA1_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIDATA2, SSIDATA2_MARK),
-	PINMUX_GPIO(GPIO_FN_SSIDATA3, SSIDATA3_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDIO_CLK, AUDIO_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDIO_XOUT, AUDIO_XOUT_MARK),
+	GPIO_FN(SSISCK0),
+	GPIO_FN(SSIWS0),
+	GPIO_FN(SSITXD0),
+	GPIO_FN(SSIRXD0),
+	GPIO_FN(SSIWS1),
+	GPIO_FN(SSIWS2),
+	GPIO_FN(SSIWS3),
+	GPIO_FN(SSISCK1),
+	GPIO_FN(SSISCK2),
+	GPIO_FN(SSISCK3),
+	GPIO_FN(SSIDATA1),
+	GPIO_FN(SSIDATA2),
+	GPIO_FN(SSIDATA3),
+	GPIO_FN(AUDIO_CLK),
+	GPIO_FN(AUDIO_XOUT),
 
 	/* SIOF */ /* NOTE Shares AUDIO_CLK with SSI */
-	PINMUX_GPIO(GPIO_FN_SIOFTXD, SIOFTXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOFRXD, SIOFRXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOFSYNC, SIOFSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOFSCK, SIOFSCK_MARK),
+	GPIO_FN(SIOFTXD),
+	GPIO_FN(SIOFRXD),
+	GPIO_FN(SIOFSYNC),
+	GPIO_FN(SIOFSCK),
 
 	/* SPDIF */ /* NOTE Shares AUDIO_CLK with SSI */
-	PINMUX_GPIO(GPIO_FN_SPDIF_IN, SPDIF_IN_MARK),
-	PINMUX_GPIO(GPIO_FN_SPDIF_OUT, SPDIF_OUT_MARK),
+	GPIO_FN(SPDIF_IN),
+	GPIO_FN(SPDIF_OUT),
 
 	/* NANDFMC */ /* NOTE Controller is not available in boot mode 0 */
-	PINMUX_GPIO(GPIO_FN_FCE, FCE_MARK),
-	PINMUX_GPIO(GPIO_FN_FRB, FRB_MARK),
+	GPIO_FN(FCE),
+	GPIO_FN(FRB),
 
 	/* VDC3 */
-	PINMUX_GPIO(GPIO_FN_DV_CLK, DV_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_VSYNC, DV_VSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_HSYNC, DV_HSYNC_MARK),
-
-	PINMUX_GPIO(GPIO_FN_DV_DATA23, DV_DATA23_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA22, DV_DATA22_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA21, DV_DATA21_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA20, DV_DATA20_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA19, DV_DATA19_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA18, DV_DATA18_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA17, DV_DATA17_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA16, DV_DATA16_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA15, DV_DATA15_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA14, DV_DATA14_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA13, DV_DATA13_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA12, DV_DATA12_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA11, DV_DATA11_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA10, DV_DATA10_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA9, DV_DATA9_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA8, DV_DATA8_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA7, DV_DATA7_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA6, DV_DATA6_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA5, DV_DATA5_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA4, DV_DATA4_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA3, DV_DATA3_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA2, DV_DATA2_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA1, DV_DATA1_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_DATA0, DV_DATA0_MARK),
-
-	PINMUX_GPIO(GPIO_FN_LCD_CLK, LCD_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_EXTCLK, LCD_EXTCLK_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_VSYNC, LCD_VSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_HSYNC, LCD_HSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DE, LCD_DE_MARK),
-
-	PINMUX_GPIO(GPIO_FN_LCD_DATA23_PG23, LCD_DATA23_PG23_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA22_PG22, LCD_DATA22_PG22_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA21_PG21, LCD_DATA21_PG21_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA20_PG20, LCD_DATA20_PG20_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA19_PG19, LCD_DATA19_PG19_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA18_PG18, LCD_DATA18_PG18_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA17_PG17, LCD_DATA17_PG17_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA16_PG16, LCD_DATA16_PG16_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA15_PG15, LCD_DATA15_PG15_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA14_PG14, LCD_DATA14_PG14_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA13_PG13, LCD_DATA13_PG13_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA12_PG12, LCD_DATA12_PG12_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA11_PG11, LCD_DATA11_PG11_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA10_PG10, LCD_DATA10_PG10_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA9_PG9, LCD_DATA9_PG9_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA8_PG8, LCD_DATA8_PG8_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA7_PG7, LCD_DATA7_PG7_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA6_PG6, LCD_DATA6_PG6_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA5_PG5, LCD_DATA5_PG5_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA4_PG4, LCD_DATA4_PG4_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA3_PG3, LCD_DATA3_PG3_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA2_PG2, LCD_DATA2_PG2_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA1_PG1, LCD_DATA1_PG1_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA0_PG0, LCD_DATA0_PG0_MARK),
-
-	PINMUX_GPIO(GPIO_FN_LCD_DATA23_PJ23, LCD_DATA23_PJ23_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA22_PJ22, LCD_DATA22_PJ22_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA21_PJ21, LCD_DATA21_PJ21_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA20_PJ20, LCD_DATA20_PJ20_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA19_PJ19, LCD_DATA19_PJ19_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA18_PJ18, LCD_DATA18_PJ18_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA17_PJ17, LCD_DATA17_PJ17_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA16_PJ16, LCD_DATA16_PJ16_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA15_PJ15, LCD_DATA15_PJ15_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA14_PJ14, LCD_DATA14_PJ14_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA13_PJ13, LCD_DATA13_PJ13_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA12_PJ12, LCD_DATA12_PJ12_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA11_PJ11, LCD_DATA11_PJ11_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA10_PJ10, LCD_DATA10_PJ10_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA9_PJ9, LCD_DATA9_PJ9_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA8_PJ8, LCD_DATA8_PJ8_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA7_PJ7, LCD_DATA7_PJ7_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA6_PJ6, LCD_DATA6_PJ6_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA5_PJ5, LCD_DATA5_PJ5_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA4_PJ4, LCD_DATA4_PJ4_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA3_PJ3, LCD_DATA3_PJ3_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA2_PJ2, LCD_DATA2_PJ2_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA1_PJ1, LCD_DATA1_PJ1_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA0_PJ0, LCD_DATA0_PJ0_MARK),
-
-	PINMUX_GPIO(GPIO_FN_LCD_M_DISP, LCD_M_DISP_MARK),
+	GPIO_FN(DV_CLK),
+	GPIO_FN(DV_VSYNC),
+	GPIO_FN(DV_HSYNC),
+
+	GPIO_FN(DV_DATA23),
+	GPIO_FN(DV_DATA22),
+	GPIO_FN(DV_DATA21),
+	GPIO_FN(DV_DATA20),
+	GPIO_FN(DV_DATA19),
+	GPIO_FN(DV_DATA18),
+	GPIO_FN(DV_DATA17),
+	GPIO_FN(DV_DATA16),
+	GPIO_FN(DV_DATA15),
+	GPIO_FN(DV_DATA14),
+	GPIO_FN(DV_DATA13),
+	GPIO_FN(DV_DATA12),
+	GPIO_FN(DV_DATA11),
+	GPIO_FN(DV_DATA10),
+	GPIO_FN(DV_DATA9),
+	GPIO_FN(DV_DATA8),
+	GPIO_FN(DV_DATA7),
+	GPIO_FN(DV_DATA6),
+	GPIO_FN(DV_DATA5),
+	GPIO_FN(DV_DATA4),
+	GPIO_FN(DV_DATA3),
+	GPIO_FN(DV_DATA2),
+	GPIO_FN(DV_DATA1),
+	GPIO_FN(DV_DATA0),
+
+	GPIO_FN(LCD_CLK),
+	GPIO_FN(LCD_EXTCLK),
+	GPIO_FN(LCD_VSYNC),
+	GPIO_FN(LCD_HSYNC),
+	GPIO_FN(LCD_DE),
+
+	GPIO_FN(LCD_DATA23_PG23),
+	GPIO_FN(LCD_DATA22_PG22),
+	GPIO_FN(LCD_DATA21_PG21),
+	GPIO_FN(LCD_DATA20_PG20),
+	GPIO_FN(LCD_DATA19_PG19),
+	GPIO_FN(LCD_DATA18_PG18),
+	GPIO_FN(LCD_DATA17_PG17),
+	GPIO_FN(LCD_DATA16_PG16),
+	GPIO_FN(LCD_DATA15_PG15),
+	GPIO_FN(LCD_DATA14_PG14),
+	GPIO_FN(LCD_DATA13_PG13),
+	GPIO_FN(LCD_DATA12_PG12),
+	GPIO_FN(LCD_DATA11_PG11),
+	GPIO_FN(LCD_DATA10_PG10),
+	GPIO_FN(LCD_DATA9_PG9),
+	GPIO_FN(LCD_DATA8_PG8),
+	GPIO_FN(LCD_DATA7_PG7),
+	GPIO_FN(LCD_DATA6_PG6),
+	GPIO_FN(LCD_DATA5_PG5),
+	GPIO_FN(LCD_DATA4_PG4),
+	GPIO_FN(LCD_DATA3_PG3),
+	GPIO_FN(LCD_DATA2_PG2),
+	GPIO_FN(LCD_DATA1_PG1),
+	GPIO_FN(LCD_DATA0_PG0),
+
+	GPIO_FN(LCD_DATA23_PJ23),
+	GPIO_FN(LCD_DATA22_PJ22),
+	GPIO_FN(LCD_DATA21_PJ21),
+	GPIO_FN(LCD_DATA20_PJ20),
+	GPIO_FN(LCD_DATA19_PJ19),
+	GPIO_FN(LCD_DATA18_PJ18),
+	GPIO_FN(LCD_DATA17_PJ17),
+	GPIO_FN(LCD_DATA16_PJ16),
+	GPIO_FN(LCD_DATA15_PJ15),
+	GPIO_FN(LCD_DATA14_PJ14),
+	GPIO_FN(LCD_DATA13_PJ13),
+	GPIO_FN(LCD_DATA12_PJ12),
+	GPIO_FN(LCD_DATA11_PJ11),
+	GPIO_FN(LCD_DATA10_PJ10),
+	GPIO_FN(LCD_DATA9_PJ9),
+	GPIO_FN(LCD_DATA8_PJ8),
+	GPIO_FN(LCD_DATA7_PJ7),
+	GPIO_FN(LCD_DATA6_PJ6),
+	GPIO_FN(LCD_DATA5_PJ5),
+	GPIO_FN(LCD_DATA4_PJ4),
+	GPIO_FN(LCD_DATA3_PJ3),
+	GPIO_FN(LCD_DATA2_PJ2),
+	GPIO_FN(LCD_DATA1_PJ1),
+	GPIO_FN(LCD_DATA0_PJ0),
+
+	GPIO_FN(LCD_M_DISP),
 };
 
-static struct pinmux_cfg_reg pinmux_config_regs[] = {
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	/* "name" addr register_size Field_Width */
 
 	/* where Field_Width is 1 for single mode registers or 4 for upto 16
@@ -2734,7 +2738,7 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{}
 };
 
-static struct pinmux_data_reg pinmux_data_regs[] = {
+static const struct pinmux_data_reg pinmux_data_regs[] = {
 	{ PINMUX_DATA_REG("PADR0", 0xfffe3816, 16) {
 		0, 0, 0, 0, 0, 0, 0, PA1_DATA,
 		0, 0, 0, 0, 0, 0, 0, PA0_DATA }
@@ -2813,19 +2817,17 @@ static struct pinmux_data_reg pinmux_data_regs[] = {
 	{ }
 };
 
-struct sh_pfc_soc_info sh7269_pinmux_info = {
+const struct sh_pfc_soc_info sh7269_pinmux_info = {
 	.name = "sh7269_pfc",
-	.reserved_id = PINMUX_RESERVED,
-	.data = { PINMUX_DATA_BEGIN, PINMUX_DATA_END },
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END, FORCE_IN },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END, FORCE_OUT },
-	.mark = { PINMUX_MARK_BEGIN, PINMUX_MARK_END },
 	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
 
-	.first_gpio = GPIO_PA1,
-	.last_gpio = GPIO_FN_LCD_M_DISP,
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.func_gpios = pinmux_func_gpios,
+	.nr_func_gpios = ARRAY_SIZE(pinmux_func_gpios),
 
-	.gpios = pinmux_gpios,
 	.cfg_regs = pinmux_config_regs,
 	.data_regs = pinmux_data_regs,
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh7372.c b/drivers/pinctrl/sh-pfc/pfc-sh7372.c
index d44e7f02069b..df0ae21a5ac8 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh7372.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh7372.c
@@ -368,7 +368,7 @@ enum {
 	PINMUX_MARK_END,
 };
 
-static pinmux_enum_t pinmux_data[] = {
+static const pinmux_enum_t pinmux_data[] = {
 
 	/* specify valid pin states for each pin in GPIO mode */
 	PORT_DATA_IO_PD(0),		PORT_DATA_IO_PD(1),
@@ -929,11 +929,214 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA(MFIv4_MARK,		MSEL4CR_6_1),
 };
 
-static struct pinmux_gpio pinmux_gpios[] = {
-
-	/* PORT */
+static struct sh_pfc_pin pinmux_pins[] = {
 	GPIO_PORT_ALL(),
+};
 
+/* - MMCIF ------------------------------------------------------------------ */
+static const unsigned int mmc0_data1_0_pins[] = {
+	/* D[0] */
+	84,
+};
+static const unsigned int mmc0_data1_0_mux[] = {
+	MMCD0_0_MARK,
+};
+static const unsigned int mmc0_data4_0_pins[] = {
+	/* D[0:3] */
+	84, 85, 86, 87,
+};
+static const unsigned int mmc0_data4_0_mux[] = {
+	MMCD0_0_MARK, MMCD0_1_MARK, MMCD0_2_MARK, MMCD0_3_MARK,
+};
+static const unsigned int mmc0_data8_0_pins[] = {
+	/* D[0:7] */
+	84, 85, 86, 87, 88, 89, 90, 91,
+};
+static const unsigned int mmc0_data8_0_mux[] = {
+	MMCD0_0_MARK, MMCD0_1_MARK, MMCD0_2_MARK, MMCD0_3_MARK,
+	MMCD0_4_MARK, MMCD0_5_MARK, MMCD0_6_MARK, MMCD0_7_MARK,
+};
+static const unsigned int mmc0_ctrl_0_pins[] = {
+	/* CMD, CLK */
+	92, 99,
+};
+static const unsigned int mmc0_ctrl_0_mux[] = {
+	MMCCMD0_MARK, MMCCLK0_MARK,
+};
+
+static const unsigned int mmc0_data1_1_pins[] = {
+	/* D[0] */
+	54,
+};
+static const unsigned int mmc0_data1_1_mux[] = {
+	MMCD1_0_MARK,
+};
+static const unsigned int mmc0_data4_1_pins[] = {
+	/* D[0:3] */
+	54, 55, 56, 57,
+};
+static const unsigned int mmc0_data4_1_mux[] = {
+	MMCD1_0_MARK, MMCD1_1_MARK, MMCD1_2_MARK, MMCD1_3_MARK,
+};
+static const unsigned int mmc0_data8_1_pins[] = {
+	/* D[0:7] */
+	54, 55, 56, 57, 58, 59, 60, 61,
+};
+static const unsigned int mmc0_data8_1_mux[] = {
+	MMCD1_0_MARK, MMCD1_1_MARK, MMCD1_2_MARK, MMCD1_3_MARK,
+	MMCD1_4_MARK, MMCD1_5_MARK, MMCD1_6_MARK, MMCD1_7_MARK,
+};
+static const unsigned int mmc0_ctrl_1_pins[] = {
+	/* CMD, CLK */
+	67, 66,
+};
+static const unsigned int mmc0_ctrl_1_mux[] = {
+	MMCCMD1_MARK, MMCCLK1_MARK,
+};
+/* - SDHI0 ------------------------------------------------------------------ */
+static const unsigned int sdhi0_data1_pins[] = {
+	/* D0 */
+	173,
+};
+static const unsigned int sdhi0_data1_mux[] = {
+	SDHID0_0_MARK,
+};
+static const unsigned int sdhi0_data4_pins[] = {
+	/* D[0:3] */
+	173, 174, 175, 176,
+};
+static const unsigned int sdhi0_data4_mux[] = {
+	SDHID0_0_MARK, SDHID0_1_MARK, SDHID0_2_MARK, SDHID0_3_MARK,
+};
+static const unsigned int sdhi0_ctrl_pins[] = {
+	/* CMD, CLK */
+	177, 171,
+};
+static const unsigned int sdhi0_ctrl_mux[] = {
+	SDHICMD0_MARK, SDHICLK0_MARK,
+};
+static const unsigned int sdhi0_cd_pins[] = {
+	/* CD */
+	172,
+};
+static const unsigned int sdhi0_cd_mux[] = {
+	SDHICD0_MARK,
+};
+static const unsigned int sdhi0_wp_pins[] = {
+	/* WP */
+	178,
+};
+static const unsigned int sdhi0_wp_mux[] = {
+	SDHIWP0_MARK,
+};
+/* - SDHI1 ------------------------------------------------------------------ */
+static const unsigned int sdhi1_data1_pins[] = {
+	/* D0 */
+	180,
+};
+static const unsigned int sdhi1_data1_mux[] = {
+	SDHID1_0_MARK,
+};
+static const unsigned int sdhi1_data4_pins[] = {
+	/* D[0:3] */
+	180, 181, 182, 183,
+};
+static const unsigned int sdhi1_data4_mux[] = {
+	SDHID1_0_MARK, SDHID1_1_MARK, SDHID1_2_MARK, SDHID1_3_MARK,
+};
+static const unsigned int sdhi1_ctrl_pins[] = {
+	/* CMD, CLK */
+	184, 179,
+};
+static const unsigned int sdhi1_ctrl_mux[] = {
+	SDHICMD1_MARK, SDHICLK1_MARK,
+};
+
+static const unsigned int sdhi2_data1_pins[] = {
+	/* D0 */
+	186,
+};
+static const unsigned int sdhi2_data1_mux[] = {
+	SDHID2_0_MARK,
+};
+static const unsigned int sdhi2_data4_pins[] = {
+	/* D[0:3] */
+	186, 187, 188, 189,
+};
+static const unsigned int sdhi2_data4_mux[] = {
+	SDHID2_0_MARK, SDHID2_1_MARK, SDHID2_2_MARK, SDHID2_3_MARK,
+};
+static const unsigned int sdhi2_ctrl_pins[] = {
+	/* CMD, CLK */
+	190, 185,
+};
+static const unsigned int sdhi2_ctrl_mux[] = {
+	SDHICMD2_MARK, SDHICLK2_MARK,
+};
+
+static const struct sh_pfc_pin_group pinmux_groups[] = {
+	SH_PFC_PIN_GROUP(mmc0_data1_0),
+	SH_PFC_PIN_GROUP(mmc0_data4_0),
+	SH_PFC_PIN_GROUP(mmc0_data8_0),
+	SH_PFC_PIN_GROUP(mmc0_ctrl_0),
+	SH_PFC_PIN_GROUP(mmc0_data1_1),
+	SH_PFC_PIN_GROUP(mmc0_data4_1),
+	SH_PFC_PIN_GROUP(mmc0_data8_1),
+	SH_PFC_PIN_GROUP(mmc0_ctrl_1),
+	SH_PFC_PIN_GROUP(sdhi0_data1),
+	SH_PFC_PIN_GROUP(sdhi0_data4),
+	SH_PFC_PIN_GROUP(sdhi0_ctrl),
+	SH_PFC_PIN_GROUP(sdhi0_cd),
+	SH_PFC_PIN_GROUP(sdhi0_wp),
+	SH_PFC_PIN_GROUP(sdhi1_data1),
+	SH_PFC_PIN_GROUP(sdhi1_data4),
+	SH_PFC_PIN_GROUP(sdhi1_ctrl),
+	SH_PFC_PIN_GROUP(sdhi2_data1),
+	SH_PFC_PIN_GROUP(sdhi2_data4),
+	SH_PFC_PIN_GROUP(sdhi2_ctrl),
+};
+
+static const char * const mmc0_groups[] = {
+	"mmc0_data1_0",
+	"mmc0_data4_0",
+	"mmc0_data8_0",
+	"mmc0_ctrl_0",
+	"mmc0_data1_1",
+	"mmc0_data4_1",
+	"mmc0_data8_1",
+	"mmc0_ctrl_1",
+};
+
+static const char * const sdhi0_groups[] = {
+	"sdhi0_data1",
+	"sdhi0_data4",
+	"sdhi0_ctrl",
+	"sdhi0_cd",
+	"sdhi0_wp",
+};
+
+static const char * const sdhi1_groups[] = {
+	"sdhi1_data1",
+	"sdhi1_data4",
+	"sdhi1_ctrl",
+};
+
+static const char * const sdhi2_groups[] = {
+	"sdhi2_data1",
+	"sdhi2_data4",
+	"sdhi2_ctrl",
+};
+
+static const struct sh_pfc_function pinmux_functions[] = {
+	SH_PFC_FUNCTION(mmc0),
+	SH_PFC_FUNCTION(sdhi0),
+	SH_PFC_FUNCTION(sdhi1),
+	SH_PFC_FUNCTION(sdhi2),
+};
+
+#define PINMUX_FN_BASE	ARRAY_SIZE(pinmux_pins)
+
+static const struct pinmux_func pinmux_func_gpios[] = {
 	/* IRQ */
 	GPIO_FN(IRQ0_6),	GPIO_FN(IRQ0_162),	GPIO_FN(IRQ1),
 	GPIO_FN(IRQ2_4),	GPIO_FN(IRQ2_5),	GPIO_FN(IRQ3_8),
@@ -1074,18 +1277,6 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	GPIO_FN(D11_NAF11),	GPIO_FN(D12_NAF12),	GPIO_FN(D13_NAF13),
 	GPIO_FN(D14_NAF14),	GPIO_FN(D15_NAF15),
 
-	/* MMCIF(1) */
-	GPIO_FN(MMCD0_0),	GPIO_FN(MMCD0_1),	GPIO_FN(MMCD0_2),
-	GPIO_FN(MMCD0_3),	GPIO_FN(MMCD0_4),	GPIO_FN(MMCD0_5),
-	GPIO_FN(MMCD0_6),	GPIO_FN(MMCD0_7),	GPIO_FN(MMCCMD0),
-	GPIO_FN(MMCCLK0),
-
-	/* MMCIF(2) */
-	GPIO_FN(MMCD1_0),	GPIO_FN(MMCD1_1),	GPIO_FN(MMCD1_2),
-	GPIO_FN(MMCD1_3),	GPIO_FN(MMCD1_4),	GPIO_FN(MMCD1_5),
-	GPIO_FN(MMCD1_6),	GPIO_FN(MMCD1_7),	GPIO_FN(MMCCLK1),
-	GPIO_FN(MMCCMD1),
-
 	/* SPU2 */
 	GPIO_FN(VINT_I),
 
@@ -1182,25 +1373,12 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	/* HDMI */
 	GPIO_FN(HDMI_HPD),	GPIO_FN(HDMI_CEC),
 
-	/* SDHI0 */
-	GPIO_FN(SDHICLK0),	GPIO_FN(SDHICD0),	GPIO_FN(SDHICMD0),
-	GPIO_FN(SDHIWP0),	GPIO_FN(SDHID0_0),	GPIO_FN(SDHID0_1),
-	GPIO_FN(SDHID0_2),	GPIO_FN(SDHID0_3),
-
-	/* SDHI1 */
-	GPIO_FN(SDHICLK1),	GPIO_FN(SDHICMD1),	GPIO_FN(SDHID1_0),
-	GPIO_FN(SDHID1_1),	GPIO_FN(SDHID1_2),	GPIO_FN(SDHID1_3),
-
-	/* SDHI2 */
-	GPIO_FN(SDHICLK2),	GPIO_FN(SDHICMD2),	GPIO_FN(SDHID2_0),
-	GPIO_FN(SDHID2_1),	GPIO_FN(SDHID2_2),	GPIO_FN(SDHID2_3),
-
 	/* SDENC */
 	GPIO_FN(SDENC_CPG),
 	GPIO_FN(SDENC_DV_CLKI),
 };
 
-static struct pinmux_cfg_reg pinmux_config_regs[] = {
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	PORTCR(0,	0xE6051000), /* PORT0CR */
 	PORTCR(1,	0xE6051001), /* PORT1CR */
 	PORTCR(2,	0xE6051002), /* PORT2CR */
@@ -1472,7 +1650,7 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ },
 };
 
-static struct pinmux_data_reg pinmux_data_regs[] = {
+static const struct pinmux_data_reg pinmux_data_regs[] = {
 	{ PINMUX_DATA_REG("PORTL095_064DR", 0xE6054008, 32) {
 			PORT95_DATA, PORT94_DATA, PORT93_DATA, PORT92_DATA,
 			PORT91_DATA, PORT90_DATA, PORT89_DATA, PORT88_DATA,
@@ -1597,56 +1775,59 @@ static struct pinmux_data_reg pinmux_data_regs[] = {
 
 #define EXT_IRQ16L(n) evt2irq(0x200 + ((n) << 5))
 #define EXT_IRQ16H(n) evt2irq(0x3200 + (((n) - 16) << 5))
-static struct pinmux_irq pinmux_irqs[] = {
-	PINMUX_IRQ(EXT_IRQ16L(0), PORT6_FN0, PORT162_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(1), PORT12_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(2), PORT4_FN0, PORT5_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(3), PORT8_FN0, PORT16_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(4), PORT17_FN0, PORT163_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(5), PORT18_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(6), PORT39_FN0, PORT164_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(7), PORT40_FN0, PORT167_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(8), PORT41_FN0, PORT168_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(9), PORT42_FN0, PORT169_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(10), PORT65_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(11), PORT67_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(12), PORT80_FN0, PORT137_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(13), PORT81_FN0, PORT145_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(14), PORT82_FN0, PORT146_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(15), PORT83_FN0, PORT147_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(16), PORT84_FN0, PORT170_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(17), PORT85_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(18), PORT86_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(19), PORT87_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(20), PORT92_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(21), PORT93_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(22), PORT94_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(23), PORT95_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(24), PORT112_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(25), PORT119_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(26), PORT121_FN0, PORT172_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(27), PORT122_FN0, PORT180_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(28), PORT123_FN0, PORT181_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(29), PORT129_FN0, PORT182_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(30), PORT130_FN0, PORT183_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(31), PORT138_FN0, PORT184_FN0),
+static const struct pinmux_irq pinmux_irqs[] = {
+	PINMUX_IRQ(EXT_IRQ16L(0), GPIO_PORT6, GPIO_PORT162),
+	PINMUX_IRQ(EXT_IRQ16L(1), GPIO_PORT12),
+	PINMUX_IRQ(EXT_IRQ16L(2), GPIO_PORT4, GPIO_PORT5),
+	PINMUX_IRQ(EXT_IRQ16L(3), GPIO_PORT8, GPIO_PORT16),
+	PINMUX_IRQ(EXT_IRQ16L(4), GPIO_PORT17, GPIO_PORT163),
+	PINMUX_IRQ(EXT_IRQ16L(5), GPIO_PORT18),
+	PINMUX_IRQ(EXT_IRQ16L(6), GPIO_PORT39, GPIO_PORT164),
+	PINMUX_IRQ(EXT_IRQ16L(7), GPIO_PORT40, GPIO_PORT167),
+	PINMUX_IRQ(EXT_IRQ16L(8), GPIO_PORT41, GPIO_PORT168),
+	PINMUX_IRQ(EXT_IRQ16L(9), GPIO_PORT42, GPIO_PORT169),
+	PINMUX_IRQ(EXT_IRQ16L(10), GPIO_PORT65),
+	PINMUX_IRQ(EXT_IRQ16L(11), GPIO_PORT67),
+	PINMUX_IRQ(EXT_IRQ16L(12), GPIO_PORT80, GPIO_PORT137),
+	PINMUX_IRQ(EXT_IRQ16L(13), GPIO_PORT81, GPIO_PORT145),
+	PINMUX_IRQ(EXT_IRQ16L(14), GPIO_PORT82, GPIO_PORT146),
+	PINMUX_IRQ(EXT_IRQ16L(15), GPIO_PORT83, GPIO_PORT147),
+	PINMUX_IRQ(EXT_IRQ16H(16), GPIO_PORT84, GPIO_PORT170),
+	PINMUX_IRQ(EXT_IRQ16H(17), GPIO_PORT85),
+	PINMUX_IRQ(EXT_IRQ16H(18), GPIO_PORT86),
+	PINMUX_IRQ(EXT_IRQ16H(19), GPIO_PORT87),
+	PINMUX_IRQ(EXT_IRQ16H(20), GPIO_PORT92),
+	PINMUX_IRQ(EXT_IRQ16H(21), GPIO_PORT93),
+	PINMUX_IRQ(EXT_IRQ16H(22), GPIO_PORT94),
+	PINMUX_IRQ(EXT_IRQ16H(23), GPIO_PORT95),
+	PINMUX_IRQ(EXT_IRQ16H(24), GPIO_PORT112),
+	PINMUX_IRQ(EXT_IRQ16H(25), GPIO_PORT119),
+	PINMUX_IRQ(EXT_IRQ16H(26), GPIO_PORT121, GPIO_PORT172),
+	PINMUX_IRQ(EXT_IRQ16H(27), GPIO_PORT122, GPIO_PORT180),
+	PINMUX_IRQ(EXT_IRQ16H(28), GPIO_PORT123, GPIO_PORT181),
+	PINMUX_IRQ(EXT_IRQ16H(29), GPIO_PORT129, GPIO_PORT182),
+	PINMUX_IRQ(EXT_IRQ16H(30), GPIO_PORT130, GPIO_PORT183),
+	PINMUX_IRQ(EXT_IRQ16H(31), GPIO_PORT138, GPIO_PORT184),
 };
 
-struct sh_pfc_soc_info sh7372_pinmux_info = {
+const struct sh_pfc_soc_info sh7372_pinmux_info = {
 	.name = "sh7372_pfc",
-	.reserved_id = PINMUX_RESERVED,
-	.data = { PINMUX_DATA_BEGIN, PINMUX_DATA_END },
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END },
 	.input_pu = { PINMUX_INPUT_PULLUP_BEGIN, PINMUX_INPUT_PULLUP_END },
 	.input_pd = { PINMUX_INPUT_PULLDOWN_BEGIN, PINMUX_INPUT_PULLDOWN_END },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END },
-	.mark = { PINMUX_MARK_BEGIN, PINMUX_MARK_END },
 	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
 
-	.first_gpio = GPIO_PORT0,
-	.last_gpio = GPIO_FN_SDENC_DV_CLKI,
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.groups = pinmux_groups,
+	.nr_groups = ARRAY_SIZE(pinmux_groups),
+	.functions = pinmux_functions,
+	.nr_functions = ARRAY_SIZE(pinmux_functions),
+
+	.func_gpios = pinmux_func_gpios,
+	.nr_func_gpios = ARRAY_SIZE(pinmux_func_gpios),
 
-	.gpios = pinmux_gpios,
 	.cfg_regs = pinmux_config_regs,
 	.data_regs = pinmux_data_regs,
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh73a0.c b/drivers/pinctrl/sh-pfc/pfc-sh73a0.c
index 709008e94124..587f7772abf2 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh73a0.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh73a0.c
@@ -18,18 +18,18 @@
  * along with this program; if not, write to the Free Software
  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  */
+#include <linux/io.h>
 #include <linux/kernel.h>
+#include <linux/pinctrl/pinconf-generic.h>
+
 #include <mach/sh73a0.h>
 #include <mach/irqs.h>
 
+#include "core.h"
 #include "sh_pfc.h"
 
 #define CPU_ALL_PORT(fn, pfx, sfx)				\
-	PORT_10(fn, pfx,    sfx), PORT_10(fn, pfx##1, sfx),	\
-	PORT_10(fn, pfx##2, sfx), PORT_10(fn, pfx##3, sfx),	\
-	PORT_10(fn, pfx##4, sfx), PORT_10(fn, pfx##5, sfx),	\
-	PORT_10(fn, pfx##6, sfx), PORT_10(fn, pfx##7, sfx),	\
-	PORT_10(fn, pfx##8, sfx), PORT_10(fn, pfx##9, sfx),	\
+	PORT_10(fn, pfx,    sfx), PORT_90(fn, pfx, sfx),	\
 	PORT_10(fn, pfx##10, sfx),				\
 	PORT_1(fn, pfx##110, sfx), PORT_1(fn, pfx##111, sfx),	\
 	PORT_1(fn, pfx##112, sfx), PORT_1(fn, pfx##113, sfx),	\
@@ -66,14 +66,6 @@ enum {
 	PORT_ALL(IN),			/* PORT0_IN -> PORT309_IN */
 	PINMUX_INPUT_END,
 
-	PINMUX_INPUT_PULLUP_BEGIN,
-	PORT_ALL(IN_PU),		/* PORT0_IN_PU -> PORT309_IN_PU */
-	PINMUX_INPUT_PULLUP_END,
-
-	PINMUX_INPUT_PULLDOWN_BEGIN,
-	PORT_ALL(IN_PD),		/* PORT0_IN_PD -> PORT309_IN_PD */
-	PINMUX_INPUT_PULLDOWN_END,
-
 	PINMUX_OUTPUT_BEGIN,
 	PORT_ALL(OUT),			/* PORT0_OUT -> PORT309_OUT */
 	PINMUX_OUTPUT_END,
@@ -468,328 +460,15 @@ enum {
 	EDBGREQ_PD_MARK,
 	EDBGREQ_PU_MARK,
 
-	/* Functions with pull-ups */
-	KEYIN0_PU_MARK,
-	KEYIN1_PU_MARK,
-	KEYIN2_PU_MARK,
-	KEYIN3_PU_MARK,
-	KEYIN4_PU_MARK,
-	KEYIN5_PU_MARK,
-	KEYIN6_PU_MARK,
-	KEYIN7_PU_MARK,
-	SDHICD0_PU_MARK,
-	SDHID0_0_PU_MARK,
-	SDHID0_1_PU_MARK,
-	SDHID0_2_PU_MARK,
-	SDHID0_3_PU_MARK,
-	SDHICMD0_PU_MARK,
-	SDHIWP0_PU_MARK,
-	SDHID1_0_PU_MARK,
-	SDHID1_1_PU_MARK,
-	SDHID1_2_PU_MARK,
-	SDHID1_3_PU_MARK,
-	SDHICMD1_PU_MARK,
-	SDHID2_0_PU_MARK,
-	SDHID2_1_PU_MARK,
-	SDHID2_2_PU_MARK,
-	SDHID2_3_PU_MARK,
-	SDHICMD2_PU_MARK,
-	MMCCMD0_PU_MARK,
-	MMCCMD1_PU_MARK,
-	MMCD0_0_PU_MARK,
-	MMCD0_1_PU_MARK,
-	MMCD0_2_PU_MARK,
-	MMCD0_3_PU_MARK,
-	MMCD0_4_PU_MARK,
-	MMCD0_5_PU_MARK,
-	MMCD0_6_PU_MARK,
-	MMCD0_7_PU_MARK,
-	FSIBISLD_PU_MARK,
-	FSIACK_PU_MARK,
-	FSIAILR_PU_MARK,
-	FSIAIBT_PU_MARK,
-	FSIAISLD_PU_MARK,
-
 	PINMUX_MARK_END,
 };
 
-static pinmux_enum_t pinmux_data[] = {
-	/* specify valid pin states for each pin in GPIO mode */
+#define _PORT_DATA(pfx, sfx)	PORT_DATA_IO(pfx)
+#define PINMUX_DATA_GP_ALL()    CPU_ALL_PORT(_PORT_DATA, , unused)
 
-	/* Table 25-1 (I/O and Pull U/D) */
-	PORT_DATA_I_PD(0),
-	PORT_DATA_I_PU(1),
-	PORT_DATA_I_PU(2),
-	PORT_DATA_I_PU(3),
-	PORT_DATA_I_PU(4),
-	PORT_DATA_I_PU(5),
-	PORT_DATA_I_PU(6),
-	PORT_DATA_I_PU(7),
-	PORT_DATA_I_PU(8),
-	PORT_DATA_I_PD(9),
-	PORT_DATA_I_PD(10),
-	PORT_DATA_I_PU_PD(11),
-	PORT_DATA_IO_PU_PD(12),
-	PORT_DATA_IO_PU_PD(13),
-	PORT_DATA_IO_PU_PD(14),
-	PORT_DATA_IO_PU_PD(15),
-	PORT_DATA_IO_PD(16),
-	PORT_DATA_IO_PD(17),
-	PORT_DATA_IO_PU(18),
-	PORT_DATA_IO_PU(19),
-	PORT_DATA_O(20),
-	PORT_DATA_O(21),
-	PORT_DATA_O(22),
-	PORT_DATA_O(23),
-	PORT_DATA_O(24),
-	PORT_DATA_I_PD(25),
-	PORT_DATA_I_PD(26),
-	PORT_DATA_IO_PU(27),
-	PORT_DATA_IO_PU(28),
-	PORT_DATA_IO_PD(29),
-	PORT_DATA_IO_PD(30),
-	PORT_DATA_IO_PU(31),
-	PORT_DATA_IO_PD(32),
-	PORT_DATA_I_PU_PD(33),
-	PORT_DATA_IO_PD(34),
-	PORT_DATA_I_PU_PD(35),
-	PORT_DATA_IO_PD(36),
-	PORT_DATA_IO(37),
-	PORT_DATA_O(38),
-	PORT_DATA_I_PU(39),
-	PORT_DATA_I_PU_PD(40),
-	PORT_DATA_O(41),
-	PORT_DATA_IO_PD(42),
-	PORT_DATA_IO_PU_PD(43),
-	PORT_DATA_IO_PU_PD(44),
-	PORT_DATA_IO_PD(45),
-	PORT_DATA_IO_PD(46),
-	PORT_DATA_IO_PD(47),
-	PORT_DATA_I_PD(48),
-	PORT_DATA_IO_PU_PD(49),
-	PORT_DATA_IO_PD(50),
-
-	PORT_DATA_IO_PD(51),
-	PORT_DATA_O(52),
-	PORT_DATA_IO_PU_PD(53),
-	PORT_DATA_IO_PU_PD(54),
-	PORT_DATA_IO_PD(55),
-	PORT_DATA_I_PU_PD(56),
-	PORT_DATA_IO(57),
-	PORT_DATA_IO(58),
-	PORT_DATA_IO(59),
-	PORT_DATA_IO(60),
-	PORT_DATA_IO(61),
-	PORT_DATA_IO_PD(62),
-	PORT_DATA_IO_PD(63),
-	PORT_DATA_IO_PU_PD(64),
-	PORT_DATA_IO_PD(65),
-	PORT_DATA_IO_PU_PD(66),
-	PORT_DATA_IO_PU_PD(67),
-	PORT_DATA_IO_PU_PD(68),
-	PORT_DATA_IO_PU_PD(69),
-	PORT_DATA_IO_PU_PD(70),
-	PORT_DATA_IO_PU_PD(71),
-	PORT_DATA_IO_PU_PD(72),
-	PORT_DATA_I_PU_PD(73),
-	PORT_DATA_IO_PU(74),
-	PORT_DATA_IO_PU(75),
-	PORT_DATA_IO_PU(76),
-	PORT_DATA_IO_PU(77),
-	PORT_DATA_IO_PU(78),
-	PORT_DATA_IO_PU(79),
-	PORT_DATA_IO_PU(80),
-	PORT_DATA_IO_PU(81),
-	PORT_DATA_IO_PU(82),
-	PORT_DATA_IO_PU(83),
-	PORT_DATA_IO_PU(84),
-	PORT_DATA_IO_PU(85),
-	PORT_DATA_IO_PU(86),
-	PORT_DATA_IO_PU(87),
-	PORT_DATA_IO_PU(88),
-	PORT_DATA_IO_PU(89),
-	PORT_DATA_O(90),
-	PORT_DATA_IO_PU(91),
-	PORT_DATA_O(92),
-	PORT_DATA_IO_PU(93),
-	PORT_DATA_O(94),
-	PORT_DATA_I_PU_PD(95),
-	PORT_DATA_IO(96),
-	PORT_DATA_IO(97),
-	PORT_DATA_IO(98),
-	PORT_DATA_I_PU(99),
-	PORT_DATA_O(100),
-	PORT_DATA_O(101),
-	PORT_DATA_I_PU(102),
-	PORT_DATA_IO_PD(103),
-	PORT_DATA_I_PU_PD(104),
-	PORT_DATA_I_PD(105),
-	PORT_DATA_I_PD(106),
-	PORT_DATA_I_PU_PD(107),
-	PORT_DATA_I_PU_PD(108),
-	PORT_DATA_IO_PD(109),
-	PORT_DATA_IO_PD(110),
-	PORT_DATA_IO_PU_PD(111),
-	PORT_DATA_IO_PU_PD(112),
-	PORT_DATA_IO_PU_PD(113),
-	PORT_DATA_IO_PD(114),
-	PORT_DATA_IO_PU(115),
-	PORT_DATA_IO_PU(116),
-	PORT_DATA_IO_PU_PD(117),
-	PORT_DATA_IO_PU_PD(118),
-	PORT_DATA_IO_PD(128),
-
-	PORT_DATA_IO_PD(129),
-	PORT_DATA_IO_PU_PD(130),
-	PORT_DATA_IO_PD(131),
-	PORT_DATA_IO_PD(132),
-	PORT_DATA_IO_PD(133),
-	PORT_DATA_IO_PU_PD(134),
-	PORT_DATA_IO_PU_PD(135),
-	PORT_DATA_IO_PU_PD(136),
-	PORT_DATA_IO_PU_PD(137),
-	PORT_DATA_IO_PD(138),
-	PORT_DATA_IO_PD(139),
-	PORT_DATA_IO_PD(140),
-	PORT_DATA_IO_PD(141),
-	PORT_DATA_IO_PD(142),
-	PORT_DATA_IO_PD(143),
-	PORT_DATA_IO_PU_PD(144),
-	PORT_DATA_IO_PD(145),
-	PORT_DATA_IO_PU_PD(146),
-	PORT_DATA_IO_PU_PD(147),
-	PORT_DATA_IO_PU_PD(148),
-	PORT_DATA_IO_PU_PD(149),
-	PORT_DATA_I_PU_PD(150),
-	PORT_DATA_IO_PU_PD(151),
-	PORT_DATA_IO_PU_PD(152),
-	PORT_DATA_IO_PD(153),
-	PORT_DATA_IO_PD(154),
-	PORT_DATA_I_PU_PD(155),
-	PORT_DATA_IO_PU_PD(156),
-	PORT_DATA_I_PD(157),
-	PORT_DATA_IO_PD(158),
-	PORT_DATA_IO_PU_PD(159),
-	PORT_DATA_IO_PU_PD(160),
-	PORT_DATA_I_PU_PD(161),
-	PORT_DATA_I_PU_PD(162),
-	PORT_DATA_IO_PU_PD(163),
-	PORT_DATA_I_PU_PD(164),
-	PORT_DATA_IO_PD(192),
-	PORT_DATA_IO_PU_PD(193),
-	PORT_DATA_IO_PD(194),
-	PORT_DATA_IO_PU_PD(195),
-	PORT_DATA_IO_PD(196),
-	PORT_DATA_IO_PD(197),
-	PORT_DATA_IO_PD(198),
-	PORT_DATA_IO_PD(199),
-	PORT_DATA_IO_PU_PD(200),
-	PORT_DATA_IO_PU_PD(201),
-	PORT_DATA_IO_PU_PD(202),
-	PORT_DATA_IO_PU_PD(203),
-	PORT_DATA_IO_PU_PD(204),
-	PORT_DATA_IO_PU_PD(205),
-	PORT_DATA_IO_PU_PD(206),
-	PORT_DATA_IO_PD(207),
-	PORT_DATA_IO_PD(208),
-	PORT_DATA_IO_PD(209),
-	PORT_DATA_IO_PD(210),
-	PORT_DATA_IO_PD(211),
-	PORT_DATA_IO_PD(212),
-	PORT_DATA_IO_PD(213),
-	PORT_DATA_IO_PU_PD(214),
-	PORT_DATA_IO_PU_PD(215),
-	PORT_DATA_IO_PD(216),
-	PORT_DATA_IO_PD(217),
-	PORT_DATA_O(218),
-	PORT_DATA_IO_PD(219),
-	PORT_DATA_IO_PD(220),
-	PORT_DATA_IO_PU_PD(221),
-	PORT_DATA_IO_PU_PD(222),
-	PORT_DATA_I_PU_PD(223),
-	PORT_DATA_I_PU_PD(224),
-
-	PORT_DATA_IO_PU_PD(225),
-	PORT_DATA_O(226),
-	PORT_DATA_IO_PU_PD(227),
-	PORT_DATA_I_PU_PD(228),
-	PORT_DATA_I_PD(229),
-	PORT_DATA_IO(230),
-	PORT_DATA_IO_PU_PD(231),
-	PORT_DATA_IO_PU_PD(232),
-	PORT_DATA_I_PU_PD(233),
-	PORT_DATA_IO_PU_PD(234),
-	PORT_DATA_IO_PU_PD(235),
-	PORT_DATA_IO_PU_PD(236),
-	PORT_DATA_IO_PD(237),
-	PORT_DATA_IO_PU_PD(238),
-	PORT_DATA_IO_PU_PD(239),
-	PORT_DATA_IO_PU_PD(240),
-	PORT_DATA_O(241),
-	PORT_DATA_I_PD(242),
-	PORT_DATA_IO_PU_PD(243),
-	PORT_DATA_IO_PU_PD(244),
-	PORT_DATA_IO_PU_PD(245),
-	PORT_DATA_IO_PU_PD(246),
-	PORT_DATA_IO_PU_PD(247),
-	PORT_DATA_IO_PU_PD(248),
-	PORT_DATA_IO_PU_PD(249),
-	PORT_DATA_IO_PU_PD(250),
-	PORT_DATA_IO_PU_PD(251),
-	PORT_DATA_IO_PU_PD(252),
-	PORT_DATA_IO_PU_PD(253),
-	PORT_DATA_IO_PU_PD(254),
-	PORT_DATA_IO_PU_PD(255),
-	PORT_DATA_IO_PU_PD(256),
-	PORT_DATA_IO_PU_PD(257),
-	PORT_DATA_IO_PU_PD(258),
-	PORT_DATA_IO_PU_PD(259),
-	PORT_DATA_IO_PU_PD(260),
-	PORT_DATA_IO_PU_PD(261),
-	PORT_DATA_IO_PU_PD(262),
-	PORT_DATA_IO_PU_PD(263),
-	PORT_DATA_IO_PU_PD(264),
-	PORT_DATA_IO_PU_PD(265),
-	PORT_DATA_IO_PU_PD(266),
-	PORT_DATA_IO_PU_PD(267),
-	PORT_DATA_IO_PU_PD(268),
-	PORT_DATA_IO_PU_PD(269),
-	PORT_DATA_IO_PU_PD(270),
-	PORT_DATA_IO_PU_PD(271),
-	PORT_DATA_IO_PU_PD(272),
-	PORT_DATA_IO_PU_PD(273),
-	PORT_DATA_IO_PU_PD(274),
-	PORT_DATA_IO_PU_PD(275),
-	PORT_DATA_IO_PU_PD(276),
-	PORT_DATA_IO_PU_PD(277),
-	PORT_DATA_IO_PU_PD(278),
-	PORT_DATA_IO_PU_PD(279),
-	PORT_DATA_IO_PU_PD(280),
-	PORT_DATA_O(281),
-	PORT_DATA_O(282),
-	PORT_DATA_I_PU(288),
-	PORT_DATA_IO_PU_PD(289),
-	PORT_DATA_IO_PU_PD(290),
-	PORT_DATA_IO_PU_PD(291),
-	PORT_DATA_IO_PU_PD(292),
-	PORT_DATA_IO_PU_PD(293),
-	PORT_DATA_IO_PU_PD(294),
-	PORT_DATA_IO_PU_PD(295),
-	PORT_DATA_IO_PU_PD(296),
-	PORT_DATA_IO_PU_PD(297),
-	PORT_DATA_IO_PU_PD(298),
-
-	PORT_DATA_IO_PU_PD(299),
-	PORT_DATA_IO_PU_PD(300),
-	PORT_DATA_IO_PU_PD(301),
-	PORT_DATA_IO_PU_PD(302),
-	PORT_DATA_IO_PU_PD(303),
-	PORT_DATA_IO_PU_PD(304),
-	PORT_DATA_IO_PU_PD(305),
-	PORT_DATA_O(306),
-	PORT_DATA_O(307),
-	PORT_DATA_I_PU(308),
-	PORT_DATA_O(309),
+static const pinmux_enum_t pinmux_data[] = {
+	/* specify valid pin states for each pin in GPIO mode */
+	PINMUX_DATA_GP_ALL(),
 
 	/* Table 25-1 (Function 0-7) */
 	PINMUX_DATA(VBUS_0_MARK, PORT0_FN1),
@@ -1358,28 +1037,19 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA(TS_SCK4_MARK, PORT268_FN3),
 	PINMUX_DATA(SDHICMD2_MARK, PORT269_FN1),
 	PINMUX_DATA(MMCCLK0_MARK, PORT270_FN1, MSEL4CR_MSEL15_0),
-	PINMUX_DATA(MMCD0_0_MARK, PORT271_FN1, PORT271_IN_PU,
-		MSEL4CR_MSEL15_0),
-	PINMUX_DATA(MMCD0_1_MARK, PORT272_FN1, PORT272_IN_PU,
-		MSEL4CR_MSEL15_0),
-	PINMUX_DATA(MMCD0_2_MARK, PORT273_FN1, PORT273_IN_PU,
-		MSEL4CR_MSEL15_0),
-	PINMUX_DATA(MMCD0_3_MARK, PORT274_FN1, PORT274_IN_PU,
-		MSEL4CR_MSEL15_0),
-	PINMUX_DATA(MMCD0_4_MARK, PORT275_FN1, PORT275_IN_PU,
-		MSEL4CR_MSEL15_0), \
+	PINMUX_DATA(MMCD0_0_MARK, PORT271_FN1, MSEL4CR_MSEL15_0),
+	PINMUX_DATA(MMCD0_1_MARK, PORT272_FN1, MSEL4CR_MSEL15_0),
+	PINMUX_DATA(MMCD0_2_MARK, PORT273_FN1, MSEL4CR_MSEL15_0),
+	PINMUX_DATA(MMCD0_3_MARK, PORT274_FN1, MSEL4CR_MSEL15_0),
+	PINMUX_DATA(MMCD0_4_MARK, PORT275_FN1, MSEL4CR_MSEL15_0),
 	PINMUX_DATA(TS_SPSYNC5_MARK, PORT275_FN3),
-	PINMUX_DATA(MMCD0_5_MARK, PORT276_FN1, PORT276_IN_PU,
-		MSEL4CR_MSEL15_0), \
+	PINMUX_DATA(MMCD0_5_MARK, PORT276_FN1, MSEL4CR_MSEL15_0),
 	PINMUX_DATA(TS_SDAT5_MARK, PORT276_FN3),
-	PINMUX_DATA(MMCD0_6_MARK, PORT277_FN1, PORT277_IN_PU,
-		MSEL4CR_MSEL15_0), \
+	PINMUX_DATA(MMCD0_6_MARK, PORT277_FN1, MSEL4CR_MSEL15_0),
 	PINMUX_DATA(TS_SDEN5_MARK, PORT277_FN3),
-	PINMUX_DATA(MMCD0_7_MARK, PORT278_FN1, PORT278_IN_PU,
-		MSEL4CR_MSEL15_0), \
+	PINMUX_DATA(MMCD0_7_MARK, PORT278_FN1, MSEL4CR_MSEL15_0),
 	PINMUX_DATA(TS_SCK5_MARK, PORT278_FN3),
-	PINMUX_DATA(MMCCMD0_MARK, PORT279_FN1, PORT279_IN_PU,
-		MSEL4CR_MSEL15_0),
+	PINMUX_DATA(MMCCMD0_MARK, PORT279_FN1, MSEL4CR_MSEL15_0),
 	PINMUX_DATA(RESETOUTS__MARK, PORT281_FN1), \
 	PINMUX_DATA(EXTAL2OUT_MARK, PORT281_FN2),
 	PINMUX_DATA(MCP_WAIT__MCP_FRB_MARK, PORT288_FN1),
@@ -1485,69 +1155,1791 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA(RESETA_N_PU_OFF_MARK, MSEL4CR_MSEL4_1),
 	PINMUX_DATA(EDBGREQ_PD_MARK, MSEL4CR_MSEL1_0),
 	PINMUX_DATA(EDBGREQ_PU_MARK, MSEL4CR_MSEL1_1),
+};
+
+#define SH73A0_PIN(pin, cfgs)						\
+	{								\
+		.name = __stringify(PORT##pin),				\
+		.enum_id = PORT##pin##_DATA,				\
+		.configs = cfgs,					\
+	}
+
+#define __I		(SH_PFC_PIN_CFG_INPUT)
+#define __O		(SH_PFC_PIN_CFG_OUTPUT)
+#define __IO		(SH_PFC_PIN_CFG_INPUT | SH_PFC_PIN_CFG_OUTPUT)
+#define __PD		(SH_PFC_PIN_CFG_PULL_DOWN)
+#define __PU		(SH_PFC_PIN_CFG_PULL_UP)
+#define __PUD		(SH_PFC_PIN_CFG_PULL_DOWN | SH_PFC_PIN_CFG_PULL_UP)
+
+#define SH73A0_PIN_I_PD(pin)		SH73A0_PIN(pin, __I | __PD)
+#define SH73A0_PIN_I_PU(pin)		SH73A0_PIN(pin, __I | __PU)
+#define SH73A0_PIN_I_PU_PD(pin)		SH73A0_PIN(pin, __I | __PUD)
+#define SH73A0_PIN_IO(pin)		SH73A0_PIN(pin, __IO)
+#define SH73A0_PIN_IO_PD(pin)		SH73A0_PIN(pin, __IO | __PD)
+#define SH73A0_PIN_IO_PU(pin)		SH73A0_PIN(pin, __IO | __PU)
+#define SH73A0_PIN_IO_PU_PD(pin)	SH73A0_PIN(pin, __IO | __PUD)
+#define SH73A0_PIN_O(pin)		SH73A0_PIN(pin, __O)
+
+static struct sh_pfc_pin pinmux_pins[] = {
+	/* Table 25-1 (I/O and Pull U/D) */
+	SH73A0_PIN_I_PD(0),
+	SH73A0_PIN_I_PU(1),
+	SH73A0_PIN_I_PU(2),
+	SH73A0_PIN_I_PU(3),
+	SH73A0_PIN_I_PU(4),
+	SH73A0_PIN_I_PU(5),
+	SH73A0_PIN_I_PU(6),
+	SH73A0_PIN_I_PU(7),
+	SH73A0_PIN_I_PU(8),
+	SH73A0_PIN_I_PD(9),
+	SH73A0_PIN_I_PD(10),
+	SH73A0_PIN_I_PU_PD(11),
+	SH73A0_PIN_IO_PU_PD(12),
+	SH73A0_PIN_IO_PU_PD(13),
+	SH73A0_PIN_IO_PU_PD(14),
+	SH73A0_PIN_IO_PU_PD(15),
+	SH73A0_PIN_IO_PD(16),
+	SH73A0_PIN_IO_PD(17),
+	SH73A0_PIN_IO_PU(18),
+	SH73A0_PIN_IO_PU(19),
+	SH73A0_PIN_O(20),
+	SH73A0_PIN_O(21),
+	SH73A0_PIN_O(22),
+	SH73A0_PIN_O(23),
+	SH73A0_PIN_O(24),
+	SH73A0_PIN_I_PD(25),
+	SH73A0_PIN_I_PD(26),
+	SH73A0_PIN_IO_PU(27),
+	SH73A0_PIN_IO_PU(28),
+	SH73A0_PIN_IO_PD(29),
+	SH73A0_PIN_IO_PD(30),
+	SH73A0_PIN_IO_PU(31),
+	SH73A0_PIN_IO_PD(32),
+	SH73A0_PIN_I_PU_PD(33),
+	SH73A0_PIN_IO_PD(34),
+	SH73A0_PIN_I_PU_PD(35),
+	SH73A0_PIN_IO_PD(36),
+	SH73A0_PIN_IO(37),
+	SH73A0_PIN_O(38),
+	SH73A0_PIN_I_PU(39),
+	SH73A0_PIN_I_PU_PD(40),
+	SH73A0_PIN_O(41),
+	SH73A0_PIN_IO_PD(42),
+	SH73A0_PIN_IO_PU_PD(43),
+	SH73A0_PIN_IO_PU_PD(44),
+	SH73A0_PIN_IO_PD(45),
+	SH73A0_PIN_IO_PD(46),
+	SH73A0_PIN_IO_PD(47),
+	SH73A0_PIN_I_PD(48),
+	SH73A0_PIN_IO_PU_PD(49),
+	SH73A0_PIN_IO_PD(50),
+	SH73A0_PIN_IO_PD(51),
+	SH73A0_PIN_O(52),
+	SH73A0_PIN_IO_PU_PD(53),
+	SH73A0_PIN_IO_PU_PD(54),
+	SH73A0_PIN_IO_PD(55),
+	SH73A0_PIN_I_PU_PD(56),
+	SH73A0_PIN_IO(57),
+	SH73A0_PIN_IO(58),
+	SH73A0_PIN_IO(59),
+	SH73A0_PIN_IO(60),
+	SH73A0_PIN_IO(61),
+	SH73A0_PIN_IO_PD(62),
+	SH73A0_PIN_IO_PD(63),
+	SH73A0_PIN_IO_PU_PD(64),
+	SH73A0_PIN_IO_PD(65),
+	SH73A0_PIN_IO_PU_PD(66),
+	SH73A0_PIN_IO_PU_PD(67),
+	SH73A0_PIN_IO_PU_PD(68),
+	SH73A0_PIN_IO_PU_PD(69),
+	SH73A0_PIN_IO_PU_PD(70),
+	SH73A0_PIN_IO_PU_PD(71),
+	SH73A0_PIN_IO_PU_PD(72),
+	SH73A0_PIN_I_PU_PD(73),
+	SH73A0_PIN_IO_PU(74),
+	SH73A0_PIN_IO_PU(75),
+	SH73A0_PIN_IO_PU(76),
+	SH73A0_PIN_IO_PU(77),
+	SH73A0_PIN_IO_PU(78),
+	SH73A0_PIN_IO_PU(79),
+	SH73A0_PIN_IO_PU(80),
+	SH73A0_PIN_IO_PU(81),
+	SH73A0_PIN_IO_PU(82),
+	SH73A0_PIN_IO_PU(83),
+	SH73A0_PIN_IO_PU(84),
+	SH73A0_PIN_IO_PU(85),
+	SH73A0_PIN_IO_PU(86),
+	SH73A0_PIN_IO_PU(87),
+	SH73A0_PIN_IO_PU(88),
+	SH73A0_PIN_IO_PU(89),
+	SH73A0_PIN_O(90),
+	SH73A0_PIN_IO_PU(91),
+	SH73A0_PIN_O(92),
+	SH73A0_PIN_IO_PU(93),
+	SH73A0_PIN_O(94),
+	SH73A0_PIN_I_PU_PD(95),
+	SH73A0_PIN_IO(96),
+	SH73A0_PIN_IO(97),
+	SH73A0_PIN_IO(98),
+	SH73A0_PIN_I_PU(99),
+	SH73A0_PIN_O(100),
+	SH73A0_PIN_O(101),
+	SH73A0_PIN_I_PU(102),
+	SH73A0_PIN_IO_PD(103),
+	SH73A0_PIN_I_PU_PD(104),
+	SH73A0_PIN_I_PD(105),
+	SH73A0_PIN_I_PD(106),
+	SH73A0_PIN_I_PU_PD(107),
+	SH73A0_PIN_I_PU_PD(108),
+	SH73A0_PIN_IO_PD(109),
+	SH73A0_PIN_IO_PD(110),
+	SH73A0_PIN_IO_PU_PD(111),
+	SH73A0_PIN_IO_PU_PD(112),
+	SH73A0_PIN_IO_PU_PD(113),
+	SH73A0_PIN_IO_PD(114),
+	SH73A0_PIN_IO_PU(115),
+	SH73A0_PIN_IO_PU(116),
+	SH73A0_PIN_IO_PU_PD(117),
+	SH73A0_PIN_IO_PU_PD(118),
+	SH73A0_PIN_IO_PD(128),
+	SH73A0_PIN_IO_PD(129),
+	SH73A0_PIN_IO_PU_PD(130),
+	SH73A0_PIN_IO_PD(131),
+	SH73A0_PIN_IO_PD(132),
+	SH73A0_PIN_IO_PD(133),
+	SH73A0_PIN_IO_PU_PD(134),
+	SH73A0_PIN_IO_PU_PD(135),
+	SH73A0_PIN_IO_PU_PD(136),
+	SH73A0_PIN_IO_PU_PD(137),
+	SH73A0_PIN_IO_PD(138),
+	SH73A0_PIN_IO_PD(139),
+	SH73A0_PIN_IO_PD(140),
+	SH73A0_PIN_IO_PD(141),
+	SH73A0_PIN_IO_PD(142),
+	SH73A0_PIN_IO_PD(143),
+	SH73A0_PIN_IO_PU_PD(144),
+	SH73A0_PIN_IO_PD(145),
+	SH73A0_PIN_IO_PU_PD(146),
+	SH73A0_PIN_IO_PU_PD(147),
+	SH73A0_PIN_IO_PU_PD(148),
+	SH73A0_PIN_IO_PU_PD(149),
+	SH73A0_PIN_I_PU_PD(150),
+	SH73A0_PIN_IO_PU_PD(151),
+	SH73A0_PIN_IO_PU_PD(152),
+	SH73A0_PIN_IO_PD(153),
+	SH73A0_PIN_IO_PD(154),
+	SH73A0_PIN_I_PU_PD(155),
+	SH73A0_PIN_IO_PU_PD(156),
+	SH73A0_PIN_I_PD(157),
+	SH73A0_PIN_IO_PD(158),
+	SH73A0_PIN_IO_PU_PD(159),
+	SH73A0_PIN_IO_PU_PD(160),
+	SH73A0_PIN_I_PU_PD(161),
+	SH73A0_PIN_I_PU_PD(162),
+	SH73A0_PIN_IO_PU_PD(163),
+	SH73A0_PIN_I_PU_PD(164),
+	SH73A0_PIN_IO_PD(192),
+	SH73A0_PIN_IO_PU_PD(193),
+	SH73A0_PIN_IO_PD(194),
+	SH73A0_PIN_IO_PU_PD(195),
+	SH73A0_PIN_IO_PD(196),
+	SH73A0_PIN_IO_PD(197),
+	SH73A0_PIN_IO_PD(198),
+	SH73A0_PIN_IO_PD(199),
+	SH73A0_PIN_IO_PU_PD(200),
+	SH73A0_PIN_IO_PU_PD(201),
+	SH73A0_PIN_IO_PU_PD(202),
+	SH73A0_PIN_IO_PU_PD(203),
+	SH73A0_PIN_IO_PU_PD(204),
+	SH73A0_PIN_IO_PU_PD(205),
+	SH73A0_PIN_IO_PU_PD(206),
+	SH73A0_PIN_IO_PD(207),
+	SH73A0_PIN_IO_PD(208),
+	SH73A0_PIN_IO_PD(209),
+	SH73A0_PIN_IO_PD(210),
+	SH73A0_PIN_IO_PD(211),
+	SH73A0_PIN_IO_PD(212),
+	SH73A0_PIN_IO_PD(213),
+	SH73A0_PIN_IO_PU_PD(214),
+	SH73A0_PIN_IO_PU_PD(215),
+	SH73A0_PIN_IO_PD(216),
+	SH73A0_PIN_IO_PD(217),
+	SH73A0_PIN_O(218),
+	SH73A0_PIN_IO_PD(219),
+	SH73A0_PIN_IO_PD(220),
+	SH73A0_PIN_IO_PU_PD(221),
+	SH73A0_PIN_IO_PU_PD(222),
+	SH73A0_PIN_I_PU_PD(223),
+	SH73A0_PIN_I_PU_PD(224),
+	SH73A0_PIN_IO_PU_PD(225),
+	SH73A0_PIN_O(226),
+	SH73A0_PIN_IO_PU_PD(227),
+	SH73A0_PIN_I_PU_PD(228),
+	SH73A0_PIN_I_PD(229),
+	SH73A0_PIN_IO(230),
+	SH73A0_PIN_IO_PU_PD(231),
+	SH73A0_PIN_IO_PU_PD(232),
+	SH73A0_PIN_I_PU_PD(233),
+	SH73A0_PIN_IO_PU_PD(234),
+	SH73A0_PIN_IO_PU_PD(235),
+	SH73A0_PIN_IO_PU_PD(236),
+	SH73A0_PIN_IO_PD(237),
+	SH73A0_PIN_IO_PU_PD(238),
+	SH73A0_PIN_IO_PU_PD(239),
+	SH73A0_PIN_IO_PU_PD(240),
+	SH73A0_PIN_O(241),
+	SH73A0_PIN_I_PD(242),
+	SH73A0_PIN_IO_PU_PD(243),
+	SH73A0_PIN_IO_PU_PD(244),
+	SH73A0_PIN_IO_PU_PD(245),
+	SH73A0_PIN_IO_PU_PD(246),
+	SH73A0_PIN_IO_PU_PD(247),
+	SH73A0_PIN_IO_PU_PD(248),
+	SH73A0_PIN_IO_PU_PD(249),
+	SH73A0_PIN_IO_PU_PD(250),
+	SH73A0_PIN_IO_PU_PD(251),
+	SH73A0_PIN_IO_PU_PD(252),
+	SH73A0_PIN_IO_PU_PD(253),
+	SH73A0_PIN_IO_PU_PD(254),
+	SH73A0_PIN_IO_PU_PD(255),
+	SH73A0_PIN_IO_PU_PD(256),
+	SH73A0_PIN_IO_PU_PD(257),
+	SH73A0_PIN_IO_PU_PD(258),
+	SH73A0_PIN_IO_PU_PD(259),
+	SH73A0_PIN_IO_PU_PD(260),
+	SH73A0_PIN_IO_PU_PD(261),
+	SH73A0_PIN_IO_PU_PD(262),
+	SH73A0_PIN_IO_PU_PD(263),
+	SH73A0_PIN_IO_PU_PD(264),
+	SH73A0_PIN_IO_PU_PD(265),
+	SH73A0_PIN_IO_PU_PD(266),
+	SH73A0_PIN_IO_PU_PD(267),
+	SH73A0_PIN_IO_PU_PD(268),
+	SH73A0_PIN_IO_PU_PD(269),
+	SH73A0_PIN_IO_PU_PD(270),
+	SH73A0_PIN_IO_PU_PD(271),
+	SH73A0_PIN_IO_PU_PD(272),
+	SH73A0_PIN_IO_PU_PD(273),
+	SH73A0_PIN_IO_PU_PD(274),
+	SH73A0_PIN_IO_PU_PD(275),
+	SH73A0_PIN_IO_PU_PD(276),
+	SH73A0_PIN_IO_PU_PD(277),
+	SH73A0_PIN_IO_PU_PD(278),
+	SH73A0_PIN_IO_PU_PD(279),
+	SH73A0_PIN_IO_PU_PD(280),
+	SH73A0_PIN_O(281),
+	SH73A0_PIN_O(282),
+	SH73A0_PIN_I_PU(288),
+	SH73A0_PIN_IO_PU_PD(289),
+	SH73A0_PIN_IO_PU_PD(290),
+	SH73A0_PIN_IO_PU_PD(291),
+	SH73A0_PIN_IO_PU_PD(292),
+	SH73A0_PIN_IO_PU_PD(293),
+	SH73A0_PIN_IO_PU_PD(294),
+	SH73A0_PIN_IO_PU_PD(295),
+	SH73A0_PIN_IO_PU_PD(296),
+	SH73A0_PIN_IO_PU_PD(297),
+	SH73A0_PIN_IO_PU_PD(298),
+	SH73A0_PIN_IO_PU_PD(299),
+	SH73A0_PIN_IO_PU_PD(300),
+	SH73A0_PIN_IO_PU_PD(301),
+	SH73A0_PIN_IO_PU_PD(302),
+	SH73A0_PIN_IO_PU_PD(303),
+	SH73A0_PIN_IO_PU_PD(304),
+	SH73A0_PIN_IO_PU_PD(305),
+	SH73A0_PIN_O(306),
+	SH73A0_PIN_O(307),
+	SH73A0_PIN_I_PU(308),
+	SH73A0_PIN_O(309),
+};
+
+static const struct pinmux_range pinmux_ranges[] = {
+	{.begin = 0, .end = 118,},
+	{.begin = 128, .end = 164,},
+	{.begin = 192, .end = 282,},
+	{.begin = 288, .end = 309,},
+};
+
+/* Pin numbers for pins without a corresponding GPIO port number are computed
+ * from the row and column numbers with a 1000 offset to avoid collisions with
+ * GPIO port numbers.
+ */
+#define PIN_NUMBER(row, col)		(1000+((row)-1)*34+(col)-1)
+
+/* - BSC -------------------------------------------------------------------- */
+static const unsigned int bsc_data_0_7_pins[] = {
+	/* D[0:7] */
+	74, 75, 76, 77, 78, 79, 80, 81,
+};
+static const unsigned int bsc_data_0_7_mux[] = {
+	D0_NAF0_MARK, D1_NAF1_MARK, D2_NAF2_MARK, D3_NAF3_MARK,
+	D4_NAF4_MARK, D5_NAF5_MARK, D6_NAF6_MARK, D7_NAF7_MARK,
+};
+static const unsigned int bsc_data_8_15_pins[] = {
+	/* D[8:15] */
+	82, 83, 84, 85, 86, 87, 88, 89,
+};
+static const unsigned int bsc_data_8_15_mux[] = {
+	D8_NAF8_MARK, D9_NAF9_MARK, D10_NAF10_MARK, D11_NAF11_MARK,
+	D12_NAF12_MARK, D13_NAF13_MARK, D14_NAF14_MARK, D15_NAF15_MARK,
+};
+static const unsigned int bsc_cs4_pins[] = {
+	/* CS */
+	90,
+};
+static const unsigned int bsc_cs4_mux[] = {
+	CS4__MARK,
+};
+static const unsigned int bsc_cs5_a_pins[] = {
+	/* CS */
+	91,
+};
+static const unsigned int bsc_cs5_a_mux[] = {
+	CS5A__MARK,
+};
+static const unsigned int bsc_cs5_b_pins[] = {
+	/* CS */
+	92,
+};
+static const unsigned int bsc_cs5_b_mux[] = {
+	CS5B__MARK,
+};
+static const unsigned int bsc_cs6_a_pins[] = {
+	/* CS */
+	94,
+};
+static const unsigned int bsc_cs6_a_mux[] = {
+	CS6A__MARK,
+};
+static const unsigned int bsc_cs6_b_pins[] = {
+	/* CS */
+	93,
+};
+static const unsigned int bsc_cs6_b_mux[] = {
+	CS6B__MARK,
+};
+static const unsigned int bsc_rd_pins[] = {
+	/* RD */
+	96,
+};
+static const unsigned int bsc_rd_mux[] = {
+	RD__FSC_MARK,
+};
+static const unsigned int bsc_rdwr_0_pins[] = {
+	/* RDWR */
+	91,
+};
+static const unsigned int bsc_rdwr_0_mux[] = {
+	PORT91_RDWR_MARK,
+};
+static const unsigned int bsc_rdwr_1_pins[] = {
+	/* RDWR */
+	97,
+};
+static const unsigned int bsc_rdwr_1_mux[] = {
+	RDWR_FWE_MARK,
+};
+static const unsigned int bsc_rdwr_2_pins[] = {
+	/* RDWR */
+	149,
+};
+static const unsigned int bsc_rdwr_2_mux[] = {
+	PORT149_RDWR_MARK,
+};
+static const unsigned int bsc_we0_pins[] = {
+	/* WE0 */
+	97,
+};
+static const unsigned int bsc_we0_mux[] = {
+	WE0__FWE_MARK,
+};
+static const unsigned int bsc_we1_pins[] = {
+	/* WE1 */
+	98,
+};
+static const unsigned int bsc_we1_mux[] = {
+	WE1__MARK,
+};
+/* - FSIA ------------------------------------------------------------------- */
+static const unsigned int fsia_mclk_in_pins[] = {
+	/* CK */
+	49,
+};
+static const unsigned int fsia_mclk_in_mux[] = {
+	FSIACK_MARK,
+};
+static const unsigned int fsia_mclk_out_pins[] = {
+	/* OMC */
+	49,
+};
+static const unsigned int fsia_mclk_out_mux[] = {
+	FSIAOMC_MARK,
+};
+static const unsigned int fsia_sclk_in_pins[] = {
+	/* ILR, IBT */
+	50, 51,
+};
+static const unsigned int fsia_sclk_in_mux[] = {
+	FSIAILR_MARK, FSIAIBT_MARK,
+};
+static const unsigned int fsia_sclk_out_pins[] = {
+	/* OLR, OBT */
+	50, 51,
+};
+static const unsigned int fsia_sclk_out_mux[] = {
+	FSIAOLR_MARK, FSIAOBT_MARK,
+};
+static const unsigned int fsia_data_in_pins[] = {
+	/* ISLD */
+	55,
+};
+static const unsigned int fsia_data_in_mux[] = {
+	FSIAISLD_MARK,
+};
+static const unsigned int fsia_data_out_pins[] = {
+	/* OSLD */
+	52,
+};
+static const unsigned int fsia_data_out_mux[] = {
+	FSIAOSLD_MARK,
+};
+static const unsigned int fsia_spdif_pins[] = {
+	/* SPDIF */
+	53,
+};
+static const unsigned int fsia_spdif_mux[] = {
+	FSIASPDIF_MARK,
+};
+/* - FSIB ------------------------------------------------------------------- */
+static const unsigned int fsib_mclk_in_pins[] = {
+	/* CK */
+	54,
+};
+static const unsigned int fsib_mclk_in_mux[] = {
+	FSIBCK_MARK,
+};
+static const unsigned int fsib_mclk_out_pins[] = {
+	/* OMC */
+	54,
+};
+static const unsigned int fsib_mclk_out_mux[] = {
+	FSIBOMC_MARK,
+};
+static const unsigned int fsib_sclk_in_pins[] = {
+	/* ILR, IBT */
+	37, 36,
+};
+static const unsigned int fsib_sclk_in_mux[] = {
+	FSIBILR_MARK, FSIBIBT_MARK,
+};
+static const unsigned int fsib_sclk_out_pins[] = {
+	/* OLR, OBT */
+	37, 36,
+};
+static const unsigned int fsib_sclk_out_mux[] = {
+	FSIBOLR_MARK, FSIBOBT_MARK,
+};
+static const unsigned int fsib_data_in_pins[] = {
+	/* ISLD */
+	39,
+};
+static const unsigned int fsib_data_in_mux[] = {
+	FSIBISLD_MARK,
+};
+static const unsigned int fsib_data_out_pins[] = {
+	/* OSLD */
+	38,
+};
+static const unsigned int fsib_data_out_mux[] = {
+	FSIBOSLD_MARK,
+};
+static const unsigned int fsib_spdif_pins[] = {
+	/* SPDIF */
+	53,
+};
+static const unsigned int fsib_spdif_mux[] = {
+	FSIBSPDIF_MARK,
+};
+/* - FSIC ------------------------------------------------------------------- */
+static const unsigned int fsic_mclk_in_pins[] = {
+	/* CK */
+	54,
+};
+static const unsigned int fsic_mclk_in_mux[] = {
+	FSICCK_MARK,
+};
+static const unsigned int fsic_mclk_out_pins[] = {
+	/* OMC */
+	54,
+};
+static const unsigned int fsic_mclk_out_mux[] = {
+	FSICOMC_MARK,
+};
+static const unsigned int fsic_sclk_in_pins[] = {
+	/* ILR, IBT */
+	46, 45,
+};
+static const unsigned int fsic_sclk_in_mux[] = {
+	FSICILR_MARK, FSICIBT_MARK,
+};
+static const unsigned int fsic_sclk_out_pins[] = {
+	/* OLR, OBT */
+	46, 45,
+};
+static const unsigned int fsic_sclk_out_mux[] = {
+	FSICOLR_MARK, FSICOBT_MARK,
+};
+static const unsigned int fsic_data_in_pins[] = {
+	/* ISLD */
+	48,
+};
+static const unsigned int fsic_data_in_mux[] = {
+	FSICISLD_MARK,
+};
+static const unsigned int fsic_data_out_pins[] = {
+	/* OSLD, OSLDT1, OSLDT2, OSLDT3 */
+	47, 44, 42, 16,
+};
+static const unsigned int fsic_data_out_mux[] = {
+	FSICOSLD_MARK, FSICOSLDT1_MARK, FSICOSLDT2_MARK, FSICOSLDT3_MARK,
+};
+static const unsigned int fsic_spdif_0_pins[] = {
+	/* SPDIF */
+	53,
+};
+static const unsigned int fsic_spdif_0_mux[] = {
+	PORT53_FSICSPDIF_MARK,
+};
+static const unsigned int fsic_spdif_1_pins[] = {
+	/* SPDIF */
+	47,
+};
+static const unsigned int fsic_spdif_1_mux[] = {
+	PORT47_FSICSPDIF_MARK,
+};
+/* - FSID ------------------------------------------------------------------- */
+static const unsigned int fsid_sclk_in_pins[] = {
+	/* ILR, IBT */
+	46, 45,
+};
+static const unsigned int fsid_sclk_in_mux[] = {
+	FSIDILR_MARK, FSIDIBT_MARK,
+};
+static const unsigned int fsid_sclk_out_pins[] = {
+	/* OLR, OBT */
+	46, 45,
+};
+static const unsigned int fsid_sclk_out_mux[] = {
+	FSIDOLR_MARK, FSIDOBT_MARK,
+};
+static const unsigned int fsid_data_in_pins[] = {
+	/* ISLD */
+	48,
+};
+static const unsigned int fsid_data_in_mux[] = {
+	FSIDISLD_MARK,
+};
+/* - I2C2 ------------------------------------------------------------------- */
+static const unsigned int i2c2_0_pins[] = {
+	/* SCL, SDA */
+	237, 236,
+};
+static const unsigned int i2c2_0_mux[] = {
+	PORT237_I2C_SCL2_MARK, PORT236_I2C_SDA2_MARK,
+};
+static const unsigned int i2c2_1_pins[] = {
+	/* SCL, SDA */
+	27, 28,
+};
+static const unsigned int i2c2_1_mux[] = {
+	PORT27_I2C_SCL2_MARK, PORT28_I2C_SDA2_MARK,
+};
+static const unsigned int i2c2_2_pins[] = {
+	/* SCL, SDA */
+	115, 116,
+};
+static const unsigned int i2c2_2_mux[] = {
+	PORT115_I2C_SCL2_MARK, PORT116_I2C_SDA2_MARK,
+};
+/* - I2C3 ------------------------------------------------------------------- */
+static const unsigned int i2c3_0_pins[] = {
+	/* SCL, SDA */
+	248, 249,
+};
+static const unsigned int i2c3_0_mux[] = {
+	PORT248_I2C_SCL3_MARK, PORT249_I2C_SDA3_MARK,
+};
+static const unsigned int i2c3_1_pins[] = {
+	/* SCL, SDA */
+	27, 28,
+};
+static const unsigned int i2c3_1_mux[] = {
+	PORT27_I2C_SCL3_MARK, PORT28_I2C_SDA3_MARK,
+};
+static const unsigned int i2c3_2_pins[] = {
+	/* SCL, SDA */
+	115, 116,
+};
+static const unsigned int i2c3_2_mux[] = {
+	PORT115_I2C_SCL3_MARK, PORT116_I2C_SDA3_MARK,
+};
+/* - IrDA ------------------------------------------------------------------- */
+static const unsigned int irda_0_pins[] = {
+	/* OUT, IN, FIRSEL */
+	241, 242, 243,
+};
+static const unsigned int irda_0_mux[] = {
+	PORT241_IRDA_OUT_MARK, PORT242_IRDA_IN_MARK, PORT243_IRDA_FIRSEL_MARK,
+};
+static const unsigned int irda_1_pins[] = {
+	/* OUT, IN, FIRSEL */
+	49, 53, 54,
+};
+static const unsigned int irda_1_mux[] = {
+	PORT49_IRDA_OUT_MARK, PORT53_IRDA_IN_MARK, PORT54_IRDA_FIRSEL_MARK,
+};
+/* - KEYSC ------------------------------------------------------------------ */
+static const unsigned int keysc_in5_pins[] = {
+	/* KEYIN[0:4] */
+	66, 67, 68, 69, 70,
+};
+static const unsigned int keysc_in5_mux[] = {
+	KEYIN0_MARK, KEYIN1_MARK, KEYIN2_MARK, KEYIN3_MARK,
+	KEYIN4_MARK,
+};
+static const unsigned int keysc_in6_pins[] = {
+	/* KEYIN[0:5] */
+	66, 67, 68, 69, 70, 71,
+};
+static const unsigned int keysc_in6_mux[] = {
+	KEYIN0_MARK, KEYIN1_MARK, KEYIN2_MARK, KEYIN3_MARK,
+	KEYIN4_MARK, KEYIN5_MARK,
+};
+static const unsigned int keysc_in7_pins[] = {
+	/* KEYIN[0:6] */
+	66, 67, 68, 69, 70, 71, 72,
+};
+static const unsigned int keysc_in7_mux[] = {
+	KEYIN0_MARK, KEYIN1_MARK, KEYIN2_MARK, KEYIN3_MARK,
+	KEYIN4_MARK, KEYIN5_MARK, KEYIN6_MARK,
+};
+static const unsigned int keysc_in8_pins[] = {
+	/* KEYIN[0:7] */
+	66, 67, 68, 69, 70, 71, 72, 73,
+};
+static const unsigned int keysc_in8_mux[] = {
+	KEYIN0_MARK, KEYIN1_MARK, KEYIN2_MARK, KEYIN3_MARK,
+	KEYIN4_MARK, KEYIN5_MARK, KEYIN6_MARK, KEYIN7_MARK,
+};
+static const unsigned int keysc_out04_pins[] = {
+	/* KEYOUT[0:4] */
+	65, 64, 63, 62, 61,
+};
+static const unsigned int keysc_out04_mux[] = {
+	KEYOUT0_MARK, KEYOUT1_MARK, KEYOUT2_MARK, KEYOUT3_MARK, KEYOUT4_MARK,
+};
+static const unsigned int keysc_out5_pins[] = {
+	/* KEYOUT5 */
+	60,
+};
+static const unsigned int keysc_out5_mux[] = {
+	KEYOUT5_MARK,
+};
+static const unsigned int keysc_out6_0_pins[] = {
+	/* KEYOUT6 */
+	59,
+};
+static const unsigned int keysc_out6_0_mux[] = {
+	PORT59_KEYOUT6_MARK,
+};
+static const unsigned int keysc_out6_1_pins[] = {
+	/* KEYOUT6 */
+	131,
+};
+static const unsigned int keysc_out6_1_mux[] = {
+	PORT131_KEYOUT6_MARK,
+};
+static const unsigned int keysc_out6_2_pins[] = {
+	/* KEYOUT6 */
+	143,
+};
+static const unsigned int keysc_out6_2_mux[] = {
+	PORT143_KEYOUT6_MARK,
+};
+static const unsigned int keysc_out7_0_pins[] = {
+	/* KEYOUT7 */
+	58,
+};
+static const unsigned int keysc_out7_0_mux[] = {
+	PORT58_KEYOUT7_MARK,
+};
+static const unsigned int keysc_out7_1_pins[] = {
+	/* KEYOUT7 */
+	132,
+};
+static const unsigned int keysc_out7_1_mux[] = {
+	PORT132_KEYOUT7_MARK,
+};
+static const unsigned int keysc_out7_2_pins[] = {
+	/* KEYOUT7 */
+	144,
+};
+static const unsigned int keysc_out7_2_mux[] = {
+	PORT144_KEYOUT7_MARK,
+};
+static const unsigned int keysc_out8_0_pins[] = {
+	/* KEYOUT8 */
+	PIN_NUMBER(6, 26),
+};
+static const unsigned int keysc_out8_0_mux[] = {
+	KEYOUT8_MARK,
+};
+static const unsigned int keysc_out8_1_pins[] = {
+	/* KEYOUT8 */
+	136,
+};
+static const unsigned int keysc_out8_1_mux[] = {
+	PORT136_KEYOUT8_MARK,
+};
+static const unsigned int keysc_out8_2_pins[] = {
+	/* KEYOUT8 */
+	138,
+};
+static const unsigned int keysc_out8_2_mux[] = {
+	PORT138_KEYOUT8_MARK,
+};
+static const unsigned int keysc_out9_0_pins[] = {
+	/* KEYOUT9 */
+	137,
+};
+static const unsigned int keysc_out9_0_mux[] = {
+	PORT137_KEYOUT9_MARK,
+};
+static const unsigned int keysc_out9_1_pins[] = {
+	/* KEYOUT9 */
+	139,
+};
+static const unsigned int keysc_out9_1_mux[] = {
+	PORT139_KEYOUT9_MARK,
+};
+static const unsigned int keysc_out9_2_pins[] = {
+	/* KEYOUT9 */
+	149,
+};
+static const unsigned int keysc_out9_2_mux[] = {
+	PORT149_KEYOUT9_MARK,
+};
+static const unsigned int keysc_out10_0_pins[] = {
+	/* KEYOUT10 */
+	132,
+};
+static const unsigned int keysc_out10_0_mux[] = {
+	PORT132_KEYOUT10_MARK,
+};
+static const unsigned int keysc_out10_1_pins[] = {
+	/* KEYOUT10 */
+	142,
+};
+static const unsigned int keysc_out10_1_mux[] = {
+	PORT142_KEYOUT10_MARK,
+};
+static const unsigned int keysc_out11_0_pins[] = {
+	/* KEYOUT11 */
+	131,
+};
+static const unsigned int keysc_out11_0_mux[] = {
+	PORT131_KEYOUT11_MARK,
+};
+static const unsigned int keysc_out11_1_pins[] = {
+	/* KEYOUT11 */
+	143,
+};
+static const unsigned int keysc_out11_1_mux[] = {
+	PORT143_KEYOUT11_MARK,
+};
+/* - LCD -------------------------------------------------------------------- */
+static const unsigned int lcd_data8_pins[] = {
+	/* D[0:7] */
+	192, 193, 194, 195, 196, 197, 198, 199,
+};
+static const unsigned int lcd_data8_mux[] = {
+	LCDD0_MARK, LCDD1_MARK, LCDD2_MARK, LCDD3_MARK,
+	LCDD4_MARK, LCDD5_MARK, LCDD6_MARK, LCDD7_MARK,
+};
+static const unsigned int lcd_data9_pins[] = {
+	/* D[0:8] */
+	192, 193, 194, 195, 196, 197, 198, 199,
+	200,
+};
+static const unsigned int lcd_data9_mux[] = {
+	LCDD0_MARK, LCDD1_MARK, LCDD2_MARK, LCDD3_MARK,
+	LCDD4_MARK, LCDD5_MARK, LCDD6_MARK, LCDD7_MARK,
+	LCDD8_MARK,
+};
+static const unsigned int lcd_data12_pins[] = {
+	/* D[0:11] */
+	192, 193, 194, 195, 196, 197, 198, 199,
+	200, 201, 202, 203,
+};
+static const unsigned int lcd_data12_mux[] = {
+	LCDD0_MARK, LCDD1_MARK, LCDD2_MARK, LCDD3_MARK,
+	LCDD4_MARK, LCDD5_MARK, LCDD6_MARK, LCDD7_MARK,
+	LCDD8_MARK, LCDD9_MARK, LCDD10_MARK, LCDD11_MARK,
+};
+static const unsigned int lcd_data16_pins[] = {
+	/* D[0:15] */
+	192, 193, 194, 195, 196, 197, 198, 199,
+	200, 201, 202, 203, 204, 205, 206, 207,
+};
+static const unsigned int lcd_data16_mux[] = {
+	LCDD0_MARK, LCDD1_MARK, LCDD2_MARK, LCDD3_MARK,
+	LCDD4_MARK, LCDD5_MARK, LCDD6_MARK, LCDD7_MARK,
+	LCDD8_MARK, LCDD9_MARK, LCDD10_MARK, LCDD11_MARK,
+	LCDD12_MARK, LCDD13_MARK, LCDD14_MARK, LCDD15_MARK,
+};
+static const unsigned int lcd_data18_pins[] = {
+	/* D[0:17] */
+	192, 193, 194, 195, 196, 197, 198, 199,
+	200, 201, 202, 203, 204, 205, 206, 207,
+	208, 209,
+};
+static const unsigned int lcd_data18_mux[] = {
+	LCDD0_MARK, LCDD1_MARK, LCDD2_MARK, LCDD3_MARK,
+	LCDD4_MARK, LCDD5_MARK, LCDD6_MARK, LCDD7_MARK,
+	LCDD8_MARK, LCDD9_MARK, LCDD10_MARK, LCDD11_MARK,
+	LCDD12_MARK, LCDD13_MARK, LCDD14_MARK, LCDD15_MARK,
+	LCDD16_MARK, LCDD17_MARK,
+};
+static const unsigned int lcd_data24_pins[] = {
+	/* D[0:23] */
+	192, 193, 194, 195, 196, 197, 198, 199,
+	200, 201, 202, 203, 204, 205, 206, 207,
+	208, 209, 210, 211, 212, 213, 214, 215
+};
+static const unsigned int lcd_data24_mux[] = {
+	LCDD0_MARK, LCDD1_MARK, LCDD2_MARK, LCDD3_MARK,
+	LCDD4_MARK, LCDD5_MARK, LCDD6_MARK, LCDD7_MARK,
+	LCDD8_MARK, LCDD9_MARK, LCDD10_MARK, LCDD11_MARK,
+	LCDD12_MARK, LCDD13_MARK, LCDD14_MARK, LCDD15_MARK,
+	LCDD16_MARK, LCDD17_MARK, LCDD18_MARK, LCDD19_MARK,
+	LCDD20_MARK, LCDD21_MARK, LCDD22_MARK, LCDD23_MARK,
+};
+static const unsigned int lcd_display_pins[] = {
+	/* DON */
+	222,
+};
+static const unsigned int lcd_display_mux[] = {
+	LCDDON_MARK,
+};
+static const unsigned int lcd_lclk_pins[] = {
+	/* LCLK */
+	221,
+};
+static const unsigned int lcd_lclk_mux[] = {
+	LCDLCLK_MARK,
+};
+static const unsigned int lcd_sync_pins[] = {
+	/* VSYN, HSYN, DCK, DISP */
+	220, 218, 216, 219,
+};
+static const unsigned int lcd_sync_mux[] = {
+	LCDVSYN_MARK, LCDHSYN_MARK, LCDDCK_MARK, LCDDISP_MARK,
+};
+static const unsigned int lcd_sys_pins[] = {
+	/* CS, WR, RD, RS */
+	218, 216, 217, 219,
+};
+static const unsigned int lcd_sys_mux[] = {
+	LCDCS__MARK, LCDWR__MARK, LCDRD__MARK, LCDRS_MARK,
+};
+/* - LCD2 ------------------------------------------------------------------- */
+static const unsigned int lcd2_data8_pins[] = {
+	/* D[0:7] */
+	128, 129, 142, 143, 144, 145, 138, 139,
+};
+static const unsigned int lcd2_data8_mux[] = {
+	LCD2D0_MARK, LCD2D1_MARK, LCD2D2_MARK, LCD2D3_MARK,
+	LCD2D4_MARK, LCD2D5_MARK, LCD2D6_MARK, LCD2D7_MARK,
+};
+static const unsigned int lcd2_data9_pins[] = {
+	/* D[0:8] */
+	128, 129, 142, 143, 144, 145, 138, 139,
+	140,
+};
+static const unsigned int lcd2_data9_mux[] = {
+	LCD2D0_MARK, LCD2D1_MARK, LCD2D2_MARK, LCD2D3_MARK,
+	LCD2D4_MARK, LCD2D5_MARK, LCD2D6_MARK, LCD2D7_MARK,
+	LCD2D8_MARK,
+};
+static const unsigned int lcd2_data12_pins[] = {
+	/* D[0:12] */
+	128, 129, 142, 143, 144, 145, 138, 139,
+	140, 141, 130, 131,
+};
+static const unsigned int lcd2_data12_mux[] = {
+	LCD2D0_MARK, LCD2D1_MARK, LCD2D2_MARK, LCD2D3_MARK,
+	LCD2D4_MARK, LCD2D5_MARK, LCD2D6_MARK, LCD2D7_MARK,
+	LCD2D8_MARK, LCD2D9_MARK, LCD2D10_MARK, LCD2D11_MARK,
+};
+static const unsigned int lcd2_data16_pins[] = {
+	/* D[0:15] */
+	128, 129, 142, 143, 144, 145, 138, 139,
+	140, 141, 130, 131, 132, 133, 134, 135,
+};
+static const unsigned int lcd2_data16_mux[] = {
+	LCD2D0_MARK, LCD2D1_MARK, LCD2D2_MARK, LCD2D3_MARK,
+	LCD2D4_MARK, LCD2D5_MARK, LCD2D6_MARK, LCD2D7_MARK,
+	LCD2D8_MARK, LCD2D9_MARK, LCD2D10_MARK, LCD2D11_MARK,
+	LCD2D12_MARK, LCD2D13_MARK, LCD2D14_MARK, LCD2D15_MARK,
+};
+static const unsigned int lcd2_data18_pins[] = {
+	/* D[0:17] */
+	128, 129, 142, 143, 144, 145, 138, 139,
+	140, 141, 130, 131, 132, 133, 134, 135,
+	136, 137,
+};
+static const unsigned int lcd2_data18_mux[] = {
+	LCD2D0_MARK, LCD2D1_MARK, LCD2D2_MARK, LCD2D3_MARK,
+	LCD2D4_MARK, LCD2D5_MARK, LCD2D6_MARK, LCD2D7_MARK,
+	LCD2D8_MARK, LCD2D9_MARK, LCD2D10_MARK, LCD2D11_MARK,
+	LCD2D12_MARK, LCD2D13_MARK, LCD2D14_MARK, LCD2D15_MARK,
+	LCD2D16_MARK, LCD2D17_MARK,
+};
+static const unsigned int lcd2_data24_pins[] = {
+	/* D[0:23] */
+	128, 129, 142, 143, 144, 145, 138, 139,
+	140, 141, 130, 131, 132, 133, 134, 135,
+	136, 137, 146, 147, 234, 235, 238, 239
+};
+static const unsigned int lcd2_data24_mux[] = {
+	LCD2D0_MARK, LCD2D1_MARK, LCD2D2_MARK, LCD2D3_MARK,
+	LCD2D4_MARK, LCD2D5_MARK, LCD2D6_MARK, LCD2D7_MARK,
+	LCD2D8_MARK, LCD2D9_MARK, LCD2D10_MARK, LCD2D11_MARK,
+	LCD2D12_MARK, LCD2D13_MARK, LCD2D14_MARK, LCD2D15_MARK,
+	LCD2D16_MARK, LCD2D17_MARK, LCD2D18_MARK, LCD2D19_MARK,
+	LCD2D20_MARK, LCD2D21_MARK, LCD2D22_MARK, LCD2D23_MARK,
+};
+static const unsigned int lcd2_sync_0_pins[] = {
+	/* VSYN, HSYN, DCK, DISP */
+	128, 129, 146, 145,
+};
+static const unsigned int lcd2_sync_0_mux[] = {
+	PORT128_LCD2VSYN_MARK, PORT129_LCD2HSYN_MARK,
+	LCD2DCK_MARK, PORT145_LCD2DISP_MARK,
+};
+static const unsigned int lcd2_sync_1_pins[] = {
+	/* VSYN, HSYN, DCK, DISP */
+	222, 221, 219, 217,
+};
+static const unsigned int lcd2_sync_1_mux[] = {
+	PORT222_LCD2VSYN_MARK, PORT221_LCD2HSYN_MARK,
+	LCD2DCK_2_MARK, PORT217_LCD2DISP_MARK,
+};
+static const unsigned int lcd2_sys_0_pins[] = {
+	/* CS, WR, RD, RS */
+	129, 146, 147, 145,
+};
+static const unsigned int lcd2_sys_0_mux[] = {
+	PORT129_LCD2CS__MARK, PORT146_LCD2WR__MARK,
+	LCD2RD__MARK, PORT145_LCD2RS_MARK,
+};
+static const unsigned int lcd2_sys_1_pins[] = {
+	/* CS, WR, RD, RS */
+	221, 219, 147, 217,
+};
+static const unsigned int lcd2_sys_1_mux[] = {
+	PORT221_LCD2CS__MARK, PORT219_LCD2WR__MARK,
+	LCD2RD__MARK, PORT217_LCD2RS_MARK,
+};
+/* - MMCIF ------------------------------------------------------------------ */
+static const unsigned int mmc0_data1_0_pins[] = {
+	/* D[0] */
+	271,
+};
+static const unsigned int mmc0_data1_0_mux[] = {
+	MMCD0_0_MARK,
+};
+static const unsigned int mmc0_data4_0_pins[] = {
+	/* D[0:3] */
+	271, 272, 273, 274,
+};
+static const unsigned int mmc0_data4_0_mux[] = {
+	MMCD0_0_MARK, MMCD0_1_MARK, MMCD0_2_MARK, MMCD0_3_MARK,
+};
+static const unsigned int mmc0_data8_0_pins[] = {
+	/* D[0:7] */
+	271, 272, 273, 274, 275, 276, 277, 278,
+};
+static const unsigned int mmc0_data8_0_mux[] = {
+	MMCD0_0_MARK, MMCD0_1_MARK, MMCD0_2_MARK, MMCD0_3_MARK,
+	MMCD0_4_MARK, MMCD0_5_MARK, MMCD0_6_MARK, MMCD0_7_MARK,
+};
+static const unsigned int mmc0_ctrl_0_pins[] = {
+	/* CMD, CLK */
+	279, 270,
+};
+static const unsigned int mmc0_ctrl_0_mux[] = {
+	MMCCMD0_MARK, MMCCLK0_MARK,
+};
 
-	/* Functions with pull-ups */
-	PINMUX_DATA(KEYIN0_PU_MARK, PORT66_FN2, PORT66_IN_PU),
-	PINMUX_DATA(KEYIN1_PU_MARK, PORT67_FN2, PORT67_IN_PU),
-	PINMUX_DATA(KEYIN2_PU_MARK, PORT68_FN2, PORT68_IN_PU),
-	PINMUX_DATA(KEYIN3_PU_MARK, PORT69_FN2, PORT69_IN_PU),
-	PINMUX_DATA(KEYIN4_PU_MARK, PORT70_FN2, PORT70_IN_PU),
-	PINMUX_DATA(KEYIN5_PU_MARK, PORT71_FN2, PORT71_IN_PU),
-	PINMUX_DATA(KEYIN6_PU_MARK, PORT72_FN2, PORT72_IN_PU),
-	PINMUX_DATA(KEYIN7_PU_MARK, PORT73_FN2, PORT73_IN_PU),
-
-	PINMUX_DATA(SDHICD0_PU_MARK,  PORT251_FN1, PORT251_IN_PU),
-	PINMUX_DATA(SDHID0_0_PU_MARK, PORT252_FN1, PORT252_IN_PU),
-	PINMUX_DATA(SDHID0_1_PU_MARK, PORT253_FN1, PORT253_IN_PU),
-	PINMUX_DATA(SDHID0_2_PU_MARK, PORT254_FN1, PORT254_IN_PU),
-	PINMUX_DATA(SDHID0_3_PU_MARK, PORT255_FN1, PORT255_IN_PU),
-	PINMUX_DATA(SDHICMD0_PU_MARK, PORT256_FN1, PORT256_IN_PU),
-	PINMUX_DATA(SDHIWP0_PU_MARK,  PORT257_FN1, PORT256_IN_PU),
-	PINMUX_DATA(SDHID1_0_PU_MARK, PORT259_FN1, PORT259_IN_PU),
-	PINMUX_DATA(SDHID1_1_PU_MARK, PORT260_FN1, PORT260_IN_PU),
-	PINMUX_DATA(SDHID1_2_PU_MARK, PORT261_FN1, PORT261_IN_PU),
-	PINMUX_DATA(SDHID1_3_PU_MARK, PORT262_FN1, PORT262_IN_PU),
-	PINMUX_DATA(SDHICMD1_PU_MARK, PORT263_FN1, PORT263_IN_PU),
-	PINMUX_DATA(SDHID2_0_PU_MARK, PORT265_FN1, PORT265_IN_PU),
-	PINMUX_DATA(SDHID2_1_PU_MARK, PORT266_FN1, PORT266_IN_PU),
-	PINMUX_DATA(SDHID2_2_PU_MARK, PORT267_FN1, PORT267_IN_PU),
-	PINMUX_DATA(SDHID2_3_PU_MARK, PORT268_FN1, PORT268_IN_PU),
-	PINMUX_DATA(SDHICMD2_PU_MARK, PORT269_FN1, PORT269_IN_PU),
-
-	PINMUX_DATA(MMCCMD0_PU_MARK, PORT279_FN1, PORT279_IN_PU,
-		MSEL4CR_MSEL15_0),
-	PINMUX_DATA(MMCCMD1_PU_MARK, PORT297_FN2, PORT297_IN_PU,
-		MSEL4CR_MSEL15_1),
-
-	PINMUX_DATA(MMCD0_0_PU_MARK,
-		    PORT271_FN1, PORT271_IN_PU, MSEL4CR_MSEL15_0),
-	PINMUX_DATA(MMCD0_1_PU_MARK,
-		    PORT272_FN1, PORT272_IN_PU, MSEL4CR_MSEL15_0),
-	PINMUX_DATA(MMCD0_2_PU_MARK,
-		    PORT273_FN1, PORT273_IN_PU, MSEL4CR_MSEL15_0),
-	PINMUX_DATA(MMCD0_3_PU_MARK,
-		    PORT274_FN1, PORT274_IN_PU, MSEL4CR_MSEL15_0),
-	PINMUX_DATA(MMCD0_4_PU_MARK,
-		    PORT275_FN1, PORT275_IN_PU, MSEL4CR_MSEL15_0),
-	PINMUX_DATA(MMCD0_5_PU_MARK,
-		    PORT276_FN1, PORT276_IN_PU, MSEL4CR_MSEL15_0),
-	PINMUX_DATA(MMCD0_6_PU_MARK,
-		    PORT277_FN1, PORT277_IN_PU, MSEL4CR_MSEL15_0),
-	PINMUX_DATA(MMCD0_7_PU_MARK,
-		    PORT278_FN1, PORT278_IN_PU, MSEL4CR_MSEL15_0),
-
-	PINMUX_DATA(FSIBISLD_PU_MARK, PORT39_FN1, PORT39_IN_PU),
-	PINMUX_DATA(FSIACK_PU_MARK, PORT49_FN1, PORT49_IN_PU),
-	PINMUX_DATA(FSIAILR_PU_MARK, PORT50_FN5, PORT50_IN_PU),
-	PINMUX_DATA(FSIAIBT_PU_MARK, PORT51_FN5, PORT51_IN_PU),
-	PINMUX_DATA(FSIAISLD_PU_MARK, PORT55_FN1, PORT55_IN_PU),
-};
-
-static struct pinmux_gpio pinmux_gpios[] = {
-	GPIO_PORT_ALL(),
+static const unsigned int mmc0_data1_1_pins[] = {
+	/* D[0] */
+	305,
+};
+static const unsigned int mmc0_data1_1_mux[] = {
+	MMCD1_0_MARK,
+};
+static const unsigned int mmc0_data4_1_pins[] = {
+	/* D[0:3] */
+	305, 304, 303, 302,
+};
+static const unsigned int mmc0_data4_1_mux[] = {
+	MMCD1_0_MARK, MMCD1_1_MARK, MMCD1_2_MARK, MMCD1_3_MARK,
+};
+static const unsigned int mmc0_data8_1_pins[] = {
+	/* D[0:7] */
+	305, 304, 303, 302, 301, 300, 299, 298,
+};
+static const unsigned int mmc0_data8_1_mux[] = {
+	MMCD1_0_MARK, MMCD1_1_MARK, MMCD1_2_MARK, MMCD1_3_MARK,
+	MMCD1_4_MARK, MMCD1_5_MARK, MMCD1_6_MARK, MMCD1_7_MARK,
+};
+static const unsigned int mmc0_ctrl_1_pins[] = {
+	/* CMD, CLK */
+	297, 289,
+};
+static const unsigned int mmc0_ctrl_1_mux[] = {
+	MMCCMD1_MARK, MMCCLK1_MARK,
+};
+/* - SCIFA0 ----------------------------------------------------------------- */
+static const unsigned int scifa0_data_pins[] = {
+	/* RXD, TXD */
+	43, 17,
+};
+static const unsigned int scifa0_data_mux[] = {
+	SCIFA0_RXD_MARK, SCIFA0_TXD_MARK,
+};
+static const unsigned int scifa0_clk_pins[] = {
+	/* SCK */
+	16,
+};
+static const unsigned int scifa0_clk_mux[] = {
+	SCIFA0_SCK_MARK,
+};
+static const unsigned int scifa0_ctrl_pins[] = {
+	/* RTS, CTS */
+	42, 44,
+};
+static const unsigned int scifa0_ctrl_mux[] = {
+	SCIFA0_RTS__MARK, SCIFA0_CTS__MARK,
+};
+/* - SCIFA1 ----------------------------------------------------------------- */
+static const unsigned int scifa1_data_pins[] = {
+	/* RXD, TXD */
+	228, 225,
+};
+static const unsigned int scifa1_data_mux[] = {
+	SCIFA1_RXD_MARK, SCIFA1_TXD_MARK,
+};
+static const unsigned int scifa1_clk_pins[] = {
+	/* SCK */
+	226,
+};
+static const unsigned int scifa1_clk_mux[] = {
+	SCIFA1_SCK_MARK,
+};
+static const unsigned int scifa1_ctrl_pins[] = {
+	/* RTS, CTS */
+	227, 229,
+};
+static const unsigned int scifa1_ctrl_mux[] = {
+	SCIFA1_RTS__MARK, SCIFA1_CTS__MARK,
+};
+/* - SCIFA2 ----------------------------------------------------------------- */
+static const unsigned int scifa2_data_0_pins[] = {
+	/* RXD, TXD */
+	155, 154,
+};
+static const unsigned int scifa2_data_0_mux[] = {
+	SCIFA2_RXD1_MARK, SCIFA2_TXD1_MARK,
+};
+static const unsigned int scifa2_clk_0_pins[] = {
+	/* SCK */
+	158,
+};
+static const unsigned int scifa2_clk_0_mux[] = {
+	SCIFA2_SCK1_MARK,
+};
+static const unsigned int scifa2_ctrl_0_pins[] = {
+	/* RTS, CTS */
+	156, 157,
+};
+static const unsigned int scifa2_ctrl_0_mux[] = {
+	SCIFA2_RTS1__MARK, SCIFA2_CTS1__MARK,
+};
+static const unsigned int scifa2_data_1_pins[] = {
+	/* RXD, TXD */
+	233, 230,
+};
+static const unsigned int scifa2_data_1_mux[] = {
+	SCIFA2_RXD2_MARK, SCIFA2_TXD2_MARK,
+};
+static const unsigned int scifa2_clk_1_pins[] = {
+	/* SCK */
+	232,
+};
+static const unsigned int scifa2_clk_1_mux[] = {
+	SCIFA2_SCK2_MARK,
+};
+static const unsigned int scifa2_ctrl_1_pins[] = {
+	/* RTS, CTS */
+	234, 231,
+};
+static const unsigned int scifa2_ctrl_1_mux[] = {
+	SCIFA2_RTS2__MARK, SCIFA2_CTS2__MARK,
+};
+/* - SCIFA3 ----------------------------------------------------------------- */
+static const unsigned int scifa3_data_pins[] = {
+	/* RXD, TXD */
+	108, 110,
+};
+static const unsigned int scifa3_data_mux[] = {
+	SCIFA3_RXD_MARK, SCIFA3_TXD_MARK,
+};
+static const unsigned int scifa3_ctrl_pins[] = {
+	/* RTS, CTS */
+	109, 107,
+};
+static const unsigned int scifa3_ctrl_mux[] = {
+	SCIFA3_RTS__MARK, SCIFA3_CTS__MARK,
+};
+/* - SCIFA4 ----------------------------------------------------------------- */
+static const unsigned int scifa4_data_pins[] = {
+	/* RXD, TXD */
+	33, 32,
+};
+static const unsigned int scifa4_data_mux[] = {
+	SCIFA4_RXD_MARK, SCIFA4_TXD_MARK,
+};
+static const unsigned int scifa4_ctrl_pins[] = {
+	/* RTS, CTS */
+	34, 35,
+};
+static const unsigned int scifa4_ctrl_mux[] = {
+	SCIFA4_RTS__MARK, SCIFA4_CTS__MARK,
+};
+/* - SCIFA5 ----------------------------------------------------------------- */
+static const unsigned int scifa5_data_0_pins[] = {
+	/* RXD, TXD */
+	246, 247,
+};
+static const unsigned int scifa5_data_0_mux[] = {
+	PORT246_SCIFA5_RXD_MARK, PORT247_SCIFA5_TXD_MARK,
+};
+static const unsigned int scifa5_clk_0_pins[] = {
+	/* SCK */
+	248,
+};
+static const unsigned int scifa5_clk_0_mux[] = {
+	PORT248_SCIFA5_SCK_MARK,
+};
+static const unsigned int scifa5_ctrl_0_pins[] = {
+	/* RTS, CTS */
+	245, 244,
+};
+static const unsigned int scifa5_ctrl_0_mux[] = {
+	PORT245_SCIFA5_RTS__MARK, PORT244_SCIFA5_CTS__MARK,
+};
+static const unsigned int scifa5_data_1_pins[] = {
+	/* RXD, TXD */
+	195, 196,
+};
+static const unsigned int scifa5_data_1_mux[] = {
+	PORT195_SCIFA5_RXD_MARK, PORT196_SCIFA5_TXD_MARK,
+};
+static const unsigned int scifa5_clk_1_pins[] = {
+	/* SCK */
+	197,
+};
+static const unsigned int scifa5_clk_1_mux[] = {
+	PORT197_SCIFA5_SCK_MARK,
+};
+static const unsigned int scifa5_ctrl_1_pins[] = {
+	/* RTS, CTS */
+	194, 193,
+};
+static const unsigned int scifa5_ctrl_1_mux[] = {
+	PORT194_SCIFA5_RTS__MARK, PORT193_SCIFA5_CTS__MARK,
+};
+static const unsigned int scifa5_data_2_pins[] = {
+	/* RXD, TXD */
+	162, 160,
+};
+static const unsigned int scifa5_data_2_mux[] = {
+	PORT162_SCIFA5_RXD_MARK, PORT160_SCIFA5_TXD_MARK,
+};
+static const unsigned int scifa5_clk_2_pins[] = {
+	/* SCK */
+	159,
+};
+static const unsigned int scifa5_clk_2_mux[] = {
+	PORT159_SCIFA5_SCK_MARK,
+};
+static const unsigned int scifa5_ctrl_2_pins[] = {
+	/* RTS, CTS */
+	163, 161,
+};
+static const unsigned int scifa5_ctrl_2_mux[] = {
+	PORT163_SCIFA5_RTS__MARK, PORT161_SCIFA5_CTS__MARK,
+};
+/* - SCIFA6 ----------------------------------------------------------------- */
+static const unsigned int scifa6_pins[] = {
+	/* TXD */
+	240,
+};
+static const unsigned int scifa6_mux[] = {
+	SCIFA6_TXD_MARK,
+};
+/* - SCIFA7 ----------------------------------------------------------------- */
+static const unsigned int scifa7_data_pins[] = {
+	/* RXD, TXD */
+	12, 18,
+};
+static const unsigned int scifa7_data_mux[] = {
+	SCIFA7_RXD_MARK, SCIFA7_TXD_MARK,
+};
+static const unsigned int scifa7_ctrl_pins[] = {
+	/* RTS, CTS */
+	19, 13,
+};
+static const unsigned int scifa7_ctrl_mux[] = {
+	SCIFA7_RTS__MARK, SCIFA7_CTS__MARK,
+};
+/* - SCIFB ------------------------------------------------------------------ */
+static const unsigned int scifb_data_0_pins[] = {
+	/* RXD, TXD */
+	162, 160,
+};
+static const unsigned int scifb_data_0_mux[] = {
+	PORT162_SCIFB_RXD_MARK, PORT160_SCIFB_TXD_MARK,
+};
+static const unsigned int scifb_clk_0_pins[] = {
+	/* SCK */
+	159,
+};
+static const unsigned int scifb_clk_0_mux[] = {
+	PORT159_SCIFB_SCK_MARK,
+};
+static const unsigned int scifb_ctrl_0_pins[] = {
+	/* RTS, CTS */
+	163, 161,
+};
+static const unsigned int scifb_ctrl_0_mux[] = {
+	PORT163_SCIFB_RTS__MARK, PORT161_SCIFB_CTS__MARK,
+};
+static const unsigned int scifb_data_1_pins[] = {
+	/* RXD, TXD */
+	246, 247,
+};
+static const unsigned int scifb_data_1_mux[] = {
+	PORT246_SCIFB_RXD_MARK, PORT247_SCIFB_TXD_MARK,
+};
+static const unsigned int scifb_clk_1_pins[] = {
+	/* SCK */
+	248,
+};
+static const unsigned int scifb_clk_1_mux[] = {
+	PORT248_SCIFB_SCK_MARK,
+};
+static const unsigned int scifb_ctrl_1_pins[] = {
+	/* RTS, CTS */
+	245, 244,
+};
+static const unsigned int scifb_ctrl_1_mux[] = {
+	PORT245_SCIFB_RTS__MARK, PORT244_SCIFB_CTS__MARK,
+};
+/* - SDHI0 ------------------------------------------------------------------ */
+static const unsigned int sdhi0_data1_pins[] = {
+	/* D0 */
+	252,
+};
+static const unsigned int sdhi0_data1_mux[] = {
+	SDHID0_0_MARK,
+};
+static const unsigned int sdhi0_data4_pins[] = {
+	/* D[0:3] */
+	252, 253, 254, 255,
+};
+static const unsigned int sdhi0_data4_mux[] = {
+	SDHID0_0_MARK, SDHID0_1_MARK, SDHID0_2_MARK, SDHID0_3_MARK,
+};
+static const unsigned int sdhi0_ctrl_pins[] = {
+	/* CMD, CLK */
+	256, 250,
+};
+static const unsigned int sdhi0_ctrl_mux[] = {
+	SDHICMD0_MARK, SDHICLK0_MARK,
+};
+static const unsigned int sdhi0_cd_pins[] = {
+	/* CD */
+	251,
+};
+static const unsigned int sdhi0_cd_mux[] = {
+	SDHICD0_MARK,
+};
+static const unsigned int sdhi0_wp_pins[] = {
+	/* WP */
+	257,
+};
+static const unsigned int sdhi0_wp_mux[] = {
+	SDHIWP0_MARK,
+};
+/* - SDHI1 ------------------------------------------------------------------ */
+static const unsigned int sdhi1_data1_pins[] = {
+	/* D0 */
+	259,
+};
+static const unsigned int sdhi1_data1_mux[] = {
+	SDHID1_0_MARK,
+};
+static const unsigned int sdhi1_data4_pins[] = {
+	/* D[0:3] */
+	259, 260, 261, 262,
+};
+static const unsigned int sdhi1_data4_mux[] = {
+	SDHID1_0_MARK, SDHID1_1_MARK, SDHID1_2_MARK, SDHID1_3_MARK,
+};
+static const unsigned int sdhi1_ctrl_pins[] = {
+	/* CMD, CLK */
+	263, 258,
+};
+static const unsigned int sdhi1_ctrl_mux[] = {
+	SDHICMD1_MARK, SDHICLK1_MARK,
+};
+/* - SDHI2 ------------------------------------------------------------------ */
+static const unsigned int sdhi2_data1_pins[] = {
+	/* D0 */
+	265,
+};
+static const unsigned int sdhi2_data1_mux[] = {
+	SDHID2_0_MARK,
+};
+static const unsigned int sdhi2_data4_pins[] = {
+	/* D[0:3] */
+	265, 266, 267, 268,
+};
+static const unsigned int sdhi2_data4_mux[] = {
+	SDHID2_0_MARK, SDHID2_1_MARK, SDHID2_2_MARK, SDHID2_3_MARK,
+};
+static const unsigned int sdhi2_ctrl_pins[] = {
+	/* CMD, CLK */
+	269, 264,
+};
+static const unsigned int sdhi2_ctrl_mux[] = {
+	SDHICMD2_MARK, SDHICLK2_MARK,
+};
+/* - USB -------------------------------------------------------------------- */
+static const unsigned int usb_vbus_pins[] = {
+	/* VBUS */
+	0,
+};
+static const unsigned int usb_vbus_mux[] = {
+	VBUS_0_MARK,
+};
+
+static const struct sh_pfc_pin_group pinmux_groups[] = {
+	SH_PFC_PIN_GROUP(bsc_data_0_7),
+	SH_PFC_PIN_GROUP(bsc_data_8_15),
+	SH_PFC_PIN_GROUP(bsc_cs4),
+	SH_PFC_PIN_GROUP(bsc_cs5_a),
+	SH_PFC_PIN_GROUP(bsc_cs5_b),
+	SH_PFC_PIN_GROUP(bsc_cs6_a),
+	SH_PFC_PIN_GROUP(bsc_cs6_b),
+	SH_PFC_PIN_GROUP(bsc_rd),
+	SH_PFC_PIN_GROUP(bsc_rdwr_0),
+	SH_PFC_PIN_GROUP(bsc_rdwr_1),
+	SH_PFC_PIN_GROUP(bsc_rdwr_2),
+	SH_PFC_PIN_GROUP(bsc_we0),
+	SH_PFC_PIN_GROUP(bsc_we1),
+	SH_PFC_PIN_GROUP(fsia_mclk_in),
+	SH_PFC_PIN_GROUP(fsia_mclk_out),
+	SH_PFC_PIN_GROUP(fsia_sclk_in),
+	SH_PFC_PIN_GROUP(fsia_sclk_out),
+	SH_PFC_PIN_GROUP(fsia_data_in),
+	SH_PFC_PIN_GROUP(fsia_data_out),
+	SH_PFC_PIN_GROUP(fsia_spdif),
+	SH_PFC_PIN_GROUP(fsib_mclk_in),
+	SH_PFC_PIN_GROUP(fsib_mclk_out),
+	SH_PFC_PIN_GROUP(fsib_sclk_in),
+	SH_PFC_PIN_GROUP(fsib_sclk_out),
+	SH_PFC_PIN_GROUP(fsib_data_in),
+	SH_PFC_PIN_GROUP(fsib_data_out),
+	SH_PFC_PIN_GROUP(fsib_spdif),
+	SH_PFC_PIN_GROUP(fsic_mclk_in),
+	SH_PFC_PIN_GROUP(fsic_mclk_out),
+	SH_PFC_PIN_GROUP(fsic_sclk_in),
+	SH_PFC_PIN_GROUP(fsic_sclk_out),
+	SH_PFC_PIN_GROUP(fsic_data_in),
+	SH_PFC_PIN_GROUP(fsic_data_out),
+	SH_PFC_PIN_GROUP(fsic_spdif_0),
+	SH_PFC_PIN_GROUP(fsic_spdif_1),
+	SH_PFC_PIN_GROUP(fsid_sclk_in),
+	SH_PFC_PIN_GROUP(fsid_sclk_out),
+	SH_PFC_PIN_GROUP(fsid_data_in),
+	SH_PFC_PIN_GROUP(i2c2_0),
+	SH_PFC_PIN_GROUP(i2c2_1),
+	SH_PFC_PIN_GROUP(i2c2_2),
+	SH_PFC_PIN_GROUP(i2c3_0),
+	SH_PFC_PIN_GROUP(i2c3_1),
+	SH_PFC_PIN_GROUP(i2c3_2),
+	SH_PFC_PIN_GROUP(irda_0),
+	SH_PFC_PIN_GROUP(irda_1),
+	SH_PFC_PIN_GROUP(keysc_in5),
+	SH_PFC_PIN_GROUP(keysc_in6),
+	SH_PFC_PIN_GROUP(keysc_in7),
+	SH_PFC_PIN_GROUP(keysc_in8),
+	SH_PFC_PIN_GROUP(keysc_out04),
+	SH_PFC_PIN_GROUP(keysc_out5),
+	SH_PFC_PIN_GROUP(keysc_out6_0),
+	SH_PFC_PIN_GROUP(keysc_out6_1),
+	SH_PFC_PIN_GROUP(keysc_out6_2),
+	SH_PFC_PIN_GROUP(keysc_out7_0),
+	SH_PFC_PIN_GROUP(keysc_out7_1),
+	SH_PFC_PIN_GROUP(keysc_out7_2),
+	SH_PFC_PIN_GROUP(keysc_out8_0),
+	SH_PFC_PIN_GROUP(keysc_out8_1),
+	SH_PFC_PIN_GROUP(keysc_out8_2),
+	SH_PFC_PIN_GROUP(keysc_out9_0),
+	SH_PFC_PIN_GROUP(keysc_out9_1),
+	SH_PFC_PIN_GROUP(keysc_out9_2),
+	SH_PFC_PIN_GROUP(keysc_out10_0),
+	SH_PFC_PIN_GROUP(keysc_out10_1),
+	SH_PFC_PIN_GROUP(keysc_out11_0),
+	SH_PFC_PIN_GROUP(keysc_out11_1),
+	SH_PFC_PIN_GROUP(lcd_data8),
+	SH_PFC_PIN_GROUP(lcd_data9),
+	SH_PFC_PIN_GROUP(lcd_data12),
+	SH_PFC_PIN_GROUP(lcd_data16),
+	SH_PFC_PIN_GROUP(lcd_data18),
+	SH_PFC_PIN_GROUP(lcd_data24),
+	SH_PFC_PIN_GROUP(lcd_display),
+	SH_PFC_PIN_GROUP(lcd_lclk),
+	SH_PFC_PIN_GROUP(lcd_sync),
+	SH_PFC_PIN_GROUP(lcd_sys),
+	SH_PFC_PIN_GROUP(lcd2_data8),
+	SH_PFC_PIN_GROUP(lcd2_data9),
+	SH_PFC_PIN_GROUP(lcd2_data12),
+	SH_PFC_PIN_GROUP(lcd2_data16),
+	SH_PFC_PIN_GROUP(lcd2_data18),
+	SH_PFC_PIN_GROUP(lcd2_data24),
+	SH_PFC_PIN_GROUP(lcd2_sync_0),
+	SH_PFC_PIN_GROUP(lcd2_sync_1),
+	SH_PFC_PIN_GROUP(lcd2_sys_0),
+	SH_PFC_PIN_GROUP(lcd2_sys_1),
+	SH_PFC_PIN_GROUP(mmc0_data1_0),
+	SH_PFC_PIN_GROUP(mmc0_data4_0),
+	SH_PFC_PIN_GROUP(mmc0_data8_0),
+	SH_PFC_PIN_GROUP(mmc0_ctrl_0),
+	SH_PFC_PIN_GROUP(mmc0_data1_1),
+	SH_PFC_PIN_GROUP(mmc0_data4_1),
+	SH_PFC_PIN_GROUP(mmc0_data8_1),
+	SH_PFC_PIN_GROUP(mmc0_ctrl_1),
+	SH_PFC_PIN_GROUP(scifa0_data),
+	SH_PFC_PIN_GROUP(scifa0_clk),
+	SH_PFC_PIN_GROUP(scifa0_ctrl),
+	SH_PFC_PIN_GROUP(scifa1_data),
+	SH_PFC_PIN_GROUP(scifa1_clk),
+	SH_PFC_PIN_GROUP(scifa1_ctrl),
+	SH_PFC_PIN_GROUP(scifa2_data_0),
+	SH_PFC_PIN_GROUP(scifa2_clk_0),
+	SH_PFC_PIN_GROUP(scifa2_ctrl_0),
+	SH_PFC_PIN_GROUP(scifa2_data_1),
+	SH_PFC_PIN_GROUP(scifa2_clk_1),
+	SH_PFC_PIN_GROUP(scifa2_ctrl_1),
+	SH_PFC_PIN_GROUP(scifa3_data),
+	SH_PFC_PIN_GROUP(scifa3_ctrl),
+	SH_PFC_PIN_GROUP(scifa4_data),
+	SH_PFC_PIN_GROUP(scifa4_ctrl),
+	SH_PFC_PIN_GROUP(scifa5_data_0),
+	SH_PFC_PIN_GROUP(scifa5_clk_0),
+	SH_PFC_PIN_GROUP(scifa5_ctrl_0),
+	SH_PFC_PIN_GROUP(scifa5_data_1),
+	SH_PFC_PIN_GROUP(scifa5_clk_1),
+	SH_PFC_PIN_GROUP(scifa5_ctrl_1),
+	SH_PFC_PIN_GROUP(scifa5_data_2),
+	SH_PFC_PIN_GROUP(scifa5_clk_2),
+	SH_PFC_PIN_GROUP(scifa5_ctrl_2),
+	SH_PFC_PIN_GROUP(scifa6),
+	SH_PFC_PIN_GROUP(scifa7_data),
+	SH_PFC_PIN_GROUP(scifa7_ctrl),
+	SH_PFC_PIN_GROUP(scifb_data_0),
+	SH_PFC_PIN_GROUP(scifb_clk_0),
+	SH_PFC_PIN_GROUP(scifb_ctrl_0),
+	SH_PFC_PIN_GROUP(scifb_data_1),
+	SH_PFC_PIN_GROUP(scifb_clk_1),
+	SH_PFC_PIN_GROUP(scifb_ctrl_1),
+	SH_PFC_PIN_GROUP(sdhi0_data1),
+	SH_PFC_PIN_GROUP(sdhi0_data4),
+	SH_PFC_PIN_GROUP(sdhi0_ctrl),
+	SH_PFC_PIN_GROUP(sdhi0_cd),
+	SH_PFC_PIN_GROUP(sdhi0_wp),
+	SH_PFC_PIN_GROUP(sdhi1_data1),
+	SH_PFC_PIN_GROUP(sdhi1_data4),
+	SH_PFC_PIN_GROUP(sdhi1_ctrl),
+	SH_PFC_PIN_GROUP(sdhi2_data1),
+	SH_PFC_PIN_GROUP(sdhi2_data4),
+	SH_PFC_PIN_GROUP(sdhi2_ctrl),
+	SH_PFC_PIN_GROUP(usb_vbus),
+};
+
+static const char * const bsc_groups[] = {
+	"bsc_data_0_7",
+	"bsc_data_8_15",
+	"bsc_cs4",
+	"bsc_cs5_a",
+	"bsc_cs5_b",
+	"bsc_cs6_a",
+	"bsc_cs6_b",
+	"bsc_rd",
+	"bsc_rdwr_0",
+	"bsc_rdwr_1",
+	"bsc_rdwr_2",
+	"bsc_we0",
+	"bsc_we1",
+};
+
+static const char * const fsia_groups[] = {
+	"fsia_mclk_in",
+	"fsia_mclk_out",
+	"fsia_sclk_in",
+	"fsia_sclk_out",
+	"fsia_data_in",
+	"fsia_data_out",
+	"fsia_spdif",
+};
+
+static const char * const fsib_groups[] = {
+	"fsib_mclk_in",
+	"fsib_mclk_out",
+	"fsib_sclk_in",
+	"fsib_sclk_out",
+	"fsib_data_in",
+	"fsib_data_out",
+	"fsib_spdif",
+};
+
+static const char * const fsic_groups[] = {
+	"fsic_mclk_in",
+	"fsic_mclk_out",
+	"fsic_sclk_in",
+	"fsic_sclk_out",
+	"fsic_data_in",
+	"fsic_data_out",
+	"fsic_spdif",
+};
+
+static const char * const fsid_groups[] = {
+	"fsid_sclk_in",
+	"fsid_sclk_out",
+	"fsid_data_in",
+};
+
+static const char * const i2c2_groups[] = {
+	"i2c2_0",
+	"i2c2_1",
+	"i2c2_2",
+};
+
+static const char * const i2c3_groups[] = {
+	"i2c3_0",
+	"i2c3_1",
+	"i2c3_2",
+};
+
+static const char * const irda_groups[] = {
+	"irda_0",
+	"irda_1",
+};
 
+static const char * const keysc_groups[] = {
+	"keysc_in5",
+	"keysc_in6",
+	"keysc_in7",
+	"keysc_in8",
+	"keysc_out04",
+	"keysc_out5",
+	"keysc_out6_0",
+	"keysc_out6_1",
+	"keysc_out6_2",
+	"keysc_out7_0",
+	"keysc_out7_1",
+	"keysc_out7_2",
+	"keysc_out8_0",
+	"keysc_out8_1",
+	"keysc_out8_2",
+	"keysc_out9_0",
+	"keysc_out9_1",
+	"keysc_out9_2",
+	"keysc_out10_0",
+	"keysc_out10_1",
+	"keysc_out11_0",
+	"keysc_out11_1",
+};
+
+static const char * const lcd_groups[] = {
+	"lcd_data8",
+	"lcd_data9",
+	"lcd_data12",
+	"lcd_data16",
+	"lcd_data18",
+	"lcd_data24",
+	"lcd_display",
+	"lcd_lclk",
+	"lcd_sync",
+	"lcd_sys",
+};
+
+static const char * const lcd2_groups[] = {
+	"lcd2_data8",
+	"lcd2_data9",
+	"lcd2_data12",
+	"lcd2_data16",
+	"lcd2_data18",
+	"lcd2_data24",
+	"lcd2_sync_0",
+	"lcd2_sync_1",
+	"lcd2_sys_0",
+	"lcd2_sys_1",
+};
+
+static const char * const mmc0_groups[] = {
+	"mmc0_data1_0",
+	"mmc0_data4_0",
+	"mmc0_data8_0",
+	"mmc0_ctrl_0",
+	"mmc0_data1_1",
+	"mmc0_data4_1",
+	"mmc0_data8_1",
+	"mmc0_ctrl_1",
+};
+
+static const char * const scifa0_groups[] = {
+	"scifa0_data",
+	"scifa0_clk",
+	"scifa0_ctrl",
+};
+
+static const char * const scifa1_groups[] = {
+	"scifa1_data",
+	"scifa1_clk",
+	"scifa1_ctrl",
+};
+
+static const char * const scifa2_groups[] = {
+	"scifa2_data_0",
+	"scifa2_clk_0",
+	"scifa2_ctrl_0",
+	"scifa2_data_1",
+	"scifa2_clk_1",
+	"scifa2_ctrl_1",
+};
+
+static const char * const scifa3_groups[] = {
+	"scifa3_data",
+	"scifa3_ctrl",
+};
+
+static const char * const scifa4_groups[] = {
+	"scifa4_data",
+	"scifa4_ctrl",
+};
+
+static const char * const scifa5_groups[] = {
+	"scifa5_data_0",
+	"scifa5_clk_0",
+	"scifa5_ctrl_0",
+	"scifa5_data_1",
+	"scifa5_clk_1",
+	"scifa5_ctrl_1",
+	"scifa5_data_2",
+	"scifa5_clk_2",
+	"scifa5_ctrl_2",
+};
+
+static const char * const scifa6_groups[] = {
+	"scifa6",
+};
+
+static const char * const scifa7_groups[] = {
+	"scifa7_data",
+	"scifa7_ctrl",
+};
+
+static const char * const scifb_groups[] = {
+	"scifb_data_0",
+	"scifb_clk_0",
+	"scifb_ctrl_0",
+	"scifb_data_1",
+	"scifb_clk_1",
+	"scifb_ctrl_1",
+};
+
+static const char * const sdhi0_groups[] = {
+	"sdhi0_data1",
+	"sdhi0_data4",
+	"sdhi0_ctrl",
+	"sdhi0_cd",
+	"sdhi0_wp",
+};
+
+static const char * const sdhi1_groups[] = {
+	"sdhi1_data1",
+	"sdhi1_data4",
+	"sdhi1_ctrl",
+};
+
+static const char * const sdhi2_groups[] = {
+	"sdhi2_data1",
+	"sdhi2_data4",
+	"sdhi2_ctrl",
+};
+
+static const char * const usb_groups[] = {
+	"usb_vbus",
+};
+
+static const struct sh_pfc_function pinmux_functions[] = {
+	SH_PFC_FUNCTION(bsc),
+	SH_PFC_FUNCTION(fsia),
+	SH_PFC_FUNCTION(fsib),
+	SH_PFC_FUNCTION(fsic),
+	SH_PFC_FUNCTION(fsid),
+	SH_PFC_FUNCTION(i2c2),
+	SH_PFC_FUNCTION(i2c3),
+	SH_PFC_FUNCTION(irda),
+	SH_PFC_FUNCTION(keysc),
+	SH_PFC_FUNCTION(lcd),
+	SH_PFC_FUNCTION(lcd2),
+	SH_PFC_FUNCTION(mmc0),
+	SH_PFC_FUNCTION(scifa0),
+	SH_PFC_FUNCTION(scifa1),
+	SH_PFC_FUNCTION(scifa2),
+	SH_PFC_FUNCTION(scifa3),
+	SH_PFC_FUNCTION(scifa4),
+	SH_PFC_FUNCTION(scifa5),
+	SH_PFC_FUNCTION(scifa6),
+	SH_PFC_FUNCTION(scifa7),
+	SH_PFC_FUNCTION(scifb),
+	SH_PFC_FUNCTION(sdhi0),
+	SH_PFC_FUNCTION(sdhi1),
+	SH_PFC_FUNCTION(sdhi2),
+	SH_PFC_FUNCTION(usb),
+};
+
+#define PINMUX_FN_BASE	GPIO_FN_GPI0
+
+static const struct pinmux_func pinmux_func_gpios[] = {
 	/* Table 25-1 (Functions 0-7) */
-	GPIO_FN(VBUS_0),
 	GPIO_FN(GPI0),
 	GPIO_FN(GPI1),
 	GPIO_FN(GPI2),
@@ -1556,19 +2948,12 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	GPIO_FN(GPI5),
 	GPIO_FN(GPI6),
 	GPIO_FN(GPI7),
-	GPIO_FN(SCIFA7_RXD),
-	GPIO_FN(SCIFA7_CTS_),
 	GPIO_FN(GPO7), \
 	GPIO_FN(MFG0_OUT2),
 	GPIO_FN(GPO6), \
 	GPIO_FN(MFG1_OUT2),
 	GPIO_FN(GPO5), \
-	GPIO_FN(SCIFA0_SCK), \
-	GPIO_FN(FSICOSLDT3), \
 	GPIO_FN(PORT16_VIO_CKOR),
-	GPIO_FN(SCIFA0_TXD),
-	GPIO_FN(SCIFA7_TXD),
-	GPIO_FN(SCIFA7_RTS_), \
 	GPIO_FN(PORT19_VIO_CKO2),
 	GPIO_FN(GPO0),
 	GPIO_FN(GPO1),
@@ -1581,13 +2966,9 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	GPIO_FN(VINT),
 	GPIO_FN(TCKON),
 	GPIO_FN(XDVFS1), \
-	GPIO_FN(PORT27_I2C_SCL2), \
-	GPIO_FN(PORT27_I2C_SCL3), \
 	GPIO_FN(MFG0_OUT1), \
 	GPIO_FN(PORT27_IROUT),
 	GPIO_FN(XDVFS2), \
-	GPIO_FN(PORT28_I2C_SDA2), \
-	GPIO_FN(PORT28_I2C_SDA3), \
 	GPIO_FN(PORT28_TPU1TO1),
 	GPIO_FN(SIM_RST), \
 	GPIO_FN(PORT29_TPU1TO1),
@@ -1595,140 +2976,53 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	GPIO_FN(PORT30_VIO_CKOR),
 	GPIO_FN(SIM_D), \
 	GPIO_FN(PORT31_IROUT),
-	GPIO_FN(SCIFA4_TXD),
-	GPIO_FN(SCIFA4_RXD), \
 	GPIO_FN(XWUP),
-	GPIO_FN(SCIFA4_RTS_),
-	GPIO_FN(SCIFA4_CTS_),
-	GPIO_FN(FSIBOBT), \
-	GPIO_FN(FSIBIBT),
-	GPIO_FN(FSIBOLR), \
-	GPIO_FN(FSIBILR),
-	GPIO_FN(FSIBOSLD),
-	GPIO_FN(FSIBISLD),
 	GPIO_FN(VACK),
 	GPIO_FN(XTAL1L),
-	GPIO_FN(SCIFA0_RTS_), \
-	GPIO_FN(FSICOSLDT2),
-	GPIO_FN(SCIFA0_RXD),
-	GPIO_FN(SCIFA0_CTS_), \
-	GPIO_FN(FSICOSLDT1),
-	GPIO_FN(FSICOBT), \
-	GPIO_FN(FSICIBT), \
-	GPIO_FN(FSIDOBT), \
-	GPIO_FN(FSIDIBT),
-	GPIO_FN(FSICOLR), \
-	GPIO_FN(FSICILR), \
-	GPIO_FN(FSIDOLR), \
-	GPIO_FN(FSIDILR),
-	GPIO_FN(FSICOSLD), \
-	GPIO_FN(PORT47_FSICSPDIF),
-	GPIO_FN(FSICISLD), \
-	GPIO_FN(FSIDISLD),
-	GPIO_FN(FSIACK), \
-	GPIO_FN(PORT49_IRDA_OUT), \
 	GPIO_FN(PORT49_IROUT), \
-	GPIO_FN(FSIAOMC),
-	GPIO_FN(FSIAOLR), \
 	GPIO_FN(BBIF2_TSYNC2), \
 	GPIO_FN(TPU2TO2), \
-	GPIO_FN(FSIAILR),
 
-	GPIO_FN(FSIAOBT), \
 	GPIO_FN(BBIF2_TSCK2), \
 	GPIO_FN(TPU2TO3), \
-	GPIO_FN(FSIAIBT),
-	GPIO_FN(FSIAOSLD), \
 	GPIO_FN(BBIF2_TXD2),
-	GPIO_FN(FSIASPDIF), \
-	GPIO_FN(PORT53_IRDA_IN), \
 	GPIO_FN(TPU3TO3), \
-	GPIO_FN(FSIBSPDIF), \
-	GPIO_FN(PORT53_FSICSPDIF),
-	GPIO_FN(FSIBCK), \
-	GPIO_FN(PORT54_IRDA_FIRSEL), \
 	GPIO_FN(TPU3TO2), \
-	GPIO_FN(FSIBOMC), \
-	GPIO_FN(FSICCK), \
-	GPIO_FN(FSICOMC),
-	GPIO_FN(FSIAISLD), \
 	GPIO_FN(TPU0TO0),
 	GPIO_FN(A0), \
 	GPIO_FN(BS_),
 	GPIO_FN(A12), \
-	GPIO_FN(PORT58_KEYOUT7), \
 	GPIO_FN(TPU4TO2),
 	GPIO_FN(A13), \
-	GPIO_FN(PORT59_KEYOUT6), \
 	GPIO_FN(TPU0TO1),
 	GPIO_FN(A14), \
-	GPIO_FN(KEYOUT5),
 	GPIO_FN(A15), \
-	GPIO_FN(KEYOUT4),
 	GPIO_FN(A16), \
-	GPIO_FN(KEYOUT3), \
 	GPIO_FN(MSIOF0_SS1),
 	GPIO_FN(A17), \
-	GPIO_FN(KEYOUT2), \
 	GPIO_FN(MSIOF0_TSYNC),
 	GPIO_FN(A18), \
-	GPIO_FN(KEYOUT1), \
 	GPIO_FN(MSIOF0_TSCK),
 	GPIO_FN(A19), \
-	GPIO_FN(KEYOUT0), \
 	GPIO_FN(MSIOF0_TXD),
 	GPIO_FN(A20), \
-	GPIO_FN(KEYIN0), \
 	GPIO_FN(MSIOF0_RSCK),
 	GPIO_FN(A21), \
-	GPIO_FN(KEYIN1), \
 	GPIO_FN(MSIOF0_RSYNC),
 	GPIO_FN(A22), \
-	GPIO_FN(KEYIN2), \
 	GPIO_FN(MSIOF0_MCK0),
 	GPIO_FN(A23), \
-	GPIO_FN(KEYIN3), \
 	GPIO_FN(MSIOF0_MCK1),
 	GPIO_FN(A24), \
-	GPIO_FN(KEYIN4), \
 	GPIO_FN(MSIOF0_RXD),
 	GPIO_FN(A25), \
-	GPIO_FN(KEYIN5), \
 	GPIO_FN(MSIOF0_SS2),
 	GPIO_FN(A26), \
-	GPIO_FN(KEYIN6),
-	GPIO_FN(KEYIN7),
-	GPIO_FN(D0_NAF0),
-	GPIO_FN(D1_NAF1),
-	GPIO_FN(D2_NAF2),
-	GPIO_FN(D3_NAF3),
-	GPIO_FN(D4_NAF4),
-	GPIO_FN(D5_NAF5),
-	GPIO_FN(D6_NAF6),
-	GPIO_FN(D7_NAF7),
-	GPIO_FN(D8_NAF8),
-	GPIO_FN(D9_NAF9),
-	GPIO_FN(D10_NAF10),
-	GPIO_FN(D11_NAF11),
-	GPIO_FN(D12_NAF12),
-	GPIO_FN(D13_NAF13),
-	GPIO_FN(D14_NAF14),
-	GPIO_FN(D15_NAF15),
-	GPIO_FN(CS4_),
-	GPIO_FN(CS5A_), \
-	GPIO_FN(PORT91_RDWR),
-	GPIO_FN(CS5B_), \
 	GPIO_FN(FCE1_),
-	GPIO_FN(CS6B_), \
 	GPIO_FN(DACK0),
 	GPIO_FN(FCE0_), \
-	GPIO_FN(CS6A_),
 	GPIO_FN(WAIT_), \
 	GPIO_FN(DREQ0),
-	GPIO_FN(RD__FSC),
-	GPIO_FN(WE0__FWE), \
-	GPIO_FN(RDWR_FWE),
-	GPIO_FN(WE1_),
 	GPIO_FN(FRB),
 	GPIO_FN(CKO),
 	GPIO_FN(NBRSTOUT_),
@@ -1737,14 +3031,10 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	GPIO_FN(BBIF2_RXD),
 	GPIO_FN(BBIF2_SYNC),
 	GPIO_FN(BBIF2_SCK),
-	GPIO_FN(SCIFA3_CTS_), \
 	GPIO_FN(MFG3_IN2),
-	GPIO_FN(SCIFA3_RXD), \
 	GPIO_FN(MFG3_IN1),
 	GPIO_FN(BBIF1_SS2), \
-	GPIO_FN(SCIFA3_RTS_), \
 	GPIO_FN(MFG3_OUT1),
-	GPIO_FN(SCIFA3_TXD),
 	GPIO_FN(HSI_RX_DATA), \
 	GPIO_FN(BBIF1_RXD),
 	GPIO_FN(HSI_TX_WAKE), \
@@ -1755,103 +3045,57 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	GPIO_FN(BBIF1_TXD),
 	GPIO_FN(HSI_RX_READY), \
 	GPIO_FN(BBIF1_RSCK), \
-	GPIO_FN(PORT115_I2C_SCL2), \
-	GPIO_FN(PORT115_I2C_SCL3),
 	GPIO_FN(HSI_RX_WAKE), \
 	GPIO_FN(BBIF1_RSYNC), \
-	GPIO_FN(PORT116_I2C_SDA2), \
-	GPIO_FN(PORT116_I2C_SDA3),
 	GPIO_FN(HSI_RX_FLAG), \
 	GPIO_FN(BBIF1_SS1), \
 	GPIO_FN(BBIF1_FLOW),
 	GPIO_FN(HSI_TX_FLAG),
 	GPIO_FN(VIO_VD), \
-	GPIO_FN(PORT128_LCD2VSYN), \
 	GPIO_FN(VIO2_VD), \
-	GPIO_FN(LCD2D0),
 
 	GPIO_FN(VIO_HD), \
-	GPIO_FN(PORT129_LCD2HSYN), \
-	GPIO_FN(PORT129_LCD2CS_), \
 	GPIO_FN(VIO2_HD), \
-	GPIO_FN(LCD2D1),
 	GPIO_FN(VIO_D0), \
 	GPIO_FN(PORT130_MSIOF2_RXD), \
-	GPIO_FN(LCD2D10),
 	GPIO_FN(VIO_D1), \
-	GPIO_FN(PORT131_KEYOUT6), \
 	GPIO_FN(PORT131_MSIOF2_SS1), \
-	GPIO_FN(PORT131_KEYOUT11), \
-	GPIO_FN(LCD2D11),
 	GPIO_FN(VIO_D2), \
-	GPIO_FN(PORT132_KEYOUT7), \
 	GPIO_FN(PORT132_MSIOF2_SS2), \
-	GPIO_FN(PORT132_KEYOUT10), \
-	GPIO_FN(LCD2D12),
 	GPIO_FN(VIO_D3), \
 	GPIO_FN(MSIOF2_TSYNC), \
-	GPIO_FN(LCD2D13),
 	GPIO_FN(VIO_D4), \
 	GPIO_FN(MSIOF2_TXD), \
-	GPIO_FN(LCD2D14),
 	GPIO_FN(VIO_D5), \
 	GPIO_FN(MSIOF2_TSCK), \
-	GPIO_FN(LCD2D15),
 	GPIO_FN(VIO_D6), \
-	GPIO_FN(PORT136_KEYOUT8), \
-	GPIO_FN(LCD2D16),
 	GPIO_FN(VIO_D7), \
-	GPIO_FN(PORT137_KEYOUT9), \
-	GPIO_FN(LCD2D17),
 	GPIO_FN(VIO_D8), \
-	GPIO_FN(PORT138_KEYOUT8), \
 	GPIO_FN(VIO2_D0), \
-	GPIO_FN(LCD2D6),
 	GPIO_FN(VIO_D9), \
-	GPIO_FN(PORT139_KEYOUT9), \
 	GPIO_FN(VIO2_D1), \
-	GPIO_FN(LCD2D7),
 	GPIO_FN(VIO_D10), \
 	GPIO_FN(TPU0TO2), \
 	GPIO_FN(VIO2_D2), \
-	GPIO_FN(LCD2D8),
 	GPIO_FN(VIO_D11), \
 	GPIO_FN(TPU0TO3), \
 	GPIO_FN(VIO2_D3), \
-	GPIO_FN(LCD2D9),
 	GPIO_FN(VIO_D12), \
-	GPIO_FN(PORT142_KEYOUT10), \
 	GPIO_FN(VIO2_D4), \
-	GPIO_FN(LCD2D2),
 	GPIO_FN(VIO_D13), \
-	GPIO_FN(PORT143_KEYOUT11), \
-	GPIO_FN(PORT143_KEYOUT6), \
 	GPIO_FN(VIO2_D5), \
-	GPIO_FN(LCD2D3),
 	GPIO_FN(VIO_D14), \
-	GPIO_FN(PORT144_KEYOUT7), \
 	GPIO_FN(VIO2_D6), \
-	GPIO_FN(LCD2D4),
 	GPIO_FN(VIO_D15), \
 	GPIO_FN(TPU1TO3), \
-	GPIO_FN(PORT145_LCD2DISP), \
-	GPIO_FN(PORT145_LCD2RS), \
 	GPIO_FN(VIO2_D7), \
-	GPIO_FN(LCD2D5),
 	GPIO_FN(VIO_CLK), \
-	GPIO_FN(LCD2DCK), \
-	GPIO_FN(PORT146_LCD2WR_), \
 	GPIO_FN(VIO2_CLK), \
-	GPIO_FN(LCD2D18),
 	GPIO_FN(VIO_FIELD), \
-	GPIO_FN(LCD2RD_), \
 	GPIO_FN(VIO2_FIELD), \
-	GPIO_FN(LCD2D19),
 	GPIO_FN(VIO_CKO),
 	GPIO_FN(A27), \
-	GPIO_FN(PORT149_RDWR), \
 	GPIO_FN(MFG0_IN1), \
-	GPIO_FN(PORT149_KEYOUT9),
 	GPIO_FN(MFG0_IN2),
 	GPIO_FN(TS_SPSYNC3), \
 	GPIO_FN(MSIOF2_RSCK),
@@ -1860,201 +3104,105 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	GPIO_FN(TPU1TO2), \
 	GPIO_FN(TS_SDEN3), \
 	GPIO_FN(PORT153_MSIOF2_SS1),
-	GPIO_FN(SCIFA2_TXD1), \
 	GPIO_FN(MSIOF2_MCK0),
-	GPIO_FN(SCIFA2_RXD1), \
 	GPIO_FN(MSIOF2_MCK1),
-	GPIO_FN(SCIFA2_RTS1_), \
 	GPIO_FN(PORT156_MSIOF2_SS2),
-	GPIO_FN(SCIFA2_CTS1_), \
 	GPIO_FN(PORT157_MSIOF2_RXD),
 	GPIO_FN(DINT_), \
-	GPIO_FN(SCIFA2_SCK1), \
 	GPIO_FN(TS_SCK3),
-	GPIO_FN(PORT159_SCIFB_SCK), \
-	GPIO_FN(PORT159_SCIFA5_SCK), \
 	GPIO_FN(NMI),
-	GPIO_FN(PORT160_SCIFB_TXD), \
-	GPIO_FN(PORT160_SCIFA5_TXD),
-	GPIO_FN(PORT161_SCIFB_CTS_), \
-	GPIO_FN(PORT161_SCIFA5_CTS_),
-	GPIO_FN(PORT162_SCIFB_RXD), \
-	GPIO_FN(PORT162_SCIFA5_RXD),
-	GPIO_FN(PORT163_SCIFB_RTS_), \
-	GPIO_FN(PORT163_SCIFA5_RTS_), \
 	GPIO_FN(TPU3TO0),
-	GPIO_FN(LCDD0),
-	GPIO_FN(LCDD1), \
-	GPIO_FN(PORT193_SCIFA5_CTS_), \
 	GPIO_FN(BBIF2_TSYNC1),
-	GPIO_FN(LCDD2), \
-	GPIO_FN(PORT194_SCIFA5_RTS_), \
 	GPIO_FN(BBIF2_TSCK1),
-	GPIO_FN(LCDD3), \
-	GPIO_FN(PORT195_SCIFA5_RXD), \
 	GPIO_FN(BBIF2_TXD1),
-	GPIO_FN(LCDD4), \
-	GPIO_FN(PORT196_SCIFA5_TXD),
-	GPIO_FN(LCDD5), \
-	GPIO_FN(PORT197_SCIFA5_SCK), \
 	GPIO_FN(MFG2_OUT2), \
 	GPIO_FN(TPU2TO1),
-	GPIO_FN(LCDD6),
-	GPIO_FN(LCDD7), \
 	GPIO_FN(TPU4TO1), \
 	GPIO_FN(MFG4_OUT2),
-	GPIO_FN(LCDD8), \
 	GPIO_FN(D16),
-	GPIO_FN(LCDD9), \
 	GPIO_FN(D17),
-	GPIO_FN(LCDD10), \
 	GPIO_FN(D18),
-	GPIO_FN(LCDD11), \
 	GPIO_FN(D19),
-	GPIO_FN(LCDD12), \
 	GPIO_FN(D20),
-	GPIO_FN(LCDD13), \
 	GPIO_FN(D21),
-	GPIO_FN(LCDD14), \
 	GPIO_FN(D22),
-	GPIO_FN(LCDD15), \
 	GPIO_FN(PORT207_MSIOF0L_SS1), \
 	GPIO_FN(D23),
-	GPIO_FN(LCDD16), \
 	GPIO_FN(PORT208_MSIOF0L_SS2), \
 	GPIO_FN(D24),
-	GPIO_FN(LCDD17), \
 	GPIO_FN(D25),
-	GPIO_FN(LCDD18), \
 	GPIO_FN(DREQ2), \
 	GPIO_FN(PORT210_MSIOF0L_SS1), \
 	GPIO_FN(D26),
-	GPIO_FN(LCDD19), \
 	GPIO_FN(PORT211_MSIOF0L_SS2), \
 	GPIO_FN(D27),
-	GPIO_FN(LCDD20), \
 	GPIO_FN(TS_SPSYNC1), \
 	GPIO_FN(MSIOF0L_MCK0), \
 	GPIO_FN(D28),
-	GPIO_FN(LCDD21), \
 	GPIO_FN(TS_SDAT1), \
 	GPIO_FN(MSIOF0L_MCK1), \
 	GPIO_FN(D29),
-	GPIO_FN(LCDD22), \
 	GPIO_FN(TS_SDEN1), \
 	GPIO_FN(MSIOF0L_RSCK), \
 	GPIO_FN(D30),
-	GPIO_FN(LCDD23), \
 	GPIO_FN(TS_SCK1), \
 	GPIO_FN(MSIOF0L_RSYNC), \
 	GPIO_FN(D31),
-	GPIO_FN(LCDDCK), \
-	GPIO_FN(LCDWR_),
-	GPIO_FN(LCDRD_), \
 	GPIO_FN(DACK2), \
-	GPIO_FN(PORT217_LCD2RS), \
 	GPIO_FN(MSIOF0L_TSYNC), \
 	GPIO_FN(VIO2_FIELD3), \
-	GPIO_FN(PORT217_LCD2DISP),
-	GPIO_FN(LCDHSYN), \
-	GPIO_FN(LCDCS_), \
-	GPIO_FN(LCDCS2_), \
 	GPIO_FN(DACK3), \
 	GPIO_FN(PORT218_VIO_CKOR),
-	GPIO_FN(LCDDISP), \
-	GPIO_FN(LCDRS), \
-	GPIO_FN(PORT219_LCD2WR_), \
 	GPIO_FN(DREQ3), \
 	GPIO_FN(MSIOF0L_TSCK), \
 	GPIO_FN(VIO2_CLK3), \
-	GPIO_FN(LCD2DCK_2),
-	GPIO_FN(LCDVSYN), \
-	GPIO_FN(LCDVSYN2),
-	GPIO_FN(LCDLCLK), \
 	GPIO_FN(DREQ1), \
-	GPIO_FN(PORT221_LCD2CS_), \
 	GPIO_FN(PWEN), \
 	GPIO_FN(MSIOF0L_RXD), \
 	GPIO_FN(VIO2_HD3), \
-	GPIO_FN(PORT221_LCD2HSYN),
-	GPIO_FN(LCDDON), \
-	GPIO_FN(LCDDON2), \
 	GPIO_FN(DACK1), \
 	GPIO_FN(OVCN), \
 	GPIO_FN(MSIOF0L_TXD), \
 	GPIO_FN(VIO2_VD3), \
-	GPIO_FN(PORT222_LCD2VSYN),
 
-	GPIO_FN(SCIFA1_TXD), \
 	GPIO_FN(OVCN2),
 	GPIO_FN(EXTLP), \
-	GPIO_FN(SCIFA1_SCK), \
 	GPIO_FN(PORT226_VIO_CKO2),
-	GPIO_FN(SCIFA1_RTS_), \
 	GPIO_FN(IDIN),
-	GPIO_FN(SCIFA1_RXD),
-	GPIO_FN(SCIFA1_CTS_), \
 	GPIO_FN(MFG1_IN1),
 	GPIO_FN(MSIOF1_TXD), \
-	GPIO_FN(SCIFA2_TXD2),
 	GPIO_FN(MSIOF1_TSYNC), \
-	GPIO_FN(SCIFA2_CTS2_),
 	GPIO_FN(MSIOF1_TSCK), \
-	GPIO_FN(SCIFA2_SCK2),
 	GPIO_FN(MSIOF1_RXD), \
-	GPIO_FN(SCIFA2_RXD2),
 	GPIO_FN(MSIOF1_RSCK), \
-	GPIO_FN(SCIFA2_RTS2_), \
 	GPIO_FN(VIO2_CLK2), \
-	GPIO_FN(LCD2D20),
 	GPIO_FN(MSIOF1_RSYNC), \
 	GPIO_FN(MFG1_IN2), \
 	GPIO_FN(VIO2_VD2), \
-	GPIO_FN(LCD2D21),
 	GPIO_FN(MSIOF1_MCK0), \
-	GPIO_FN(PORT236_I2C_SDA2),
 	GPIO_FN(MSIOF1_MCK1), \
-	GPIO_FN(PORT237_I2C_SCL2),
 	GPIO_FN(MSIOF1_SS1), \
 	GPIO_FN(VIO2_FIELD2), \
-	GPIO_FN(LCD2D22),
 	GPIO_FN(MSIOF1_SS2), \
 	GPIO_FN(VIO2_HD2), \
-	GPIO_FN(LCD2D23),
-	GPIO_FN(SCIFA6_TXD),
-	GPIO_FN(PORT241_IRDA_OUT), \
 	GPIO_FN(PORT241_IROUT), \
 	GPIO_FN(MFG4_OUT1), \
 	GPIO_FN(TPU4TO0),
-	GPIO_FN(PORT242_IRDA_IN), \
 	GPIO_FN(MFG4_IN2),
-	GPIO_FN(PORT243_IRDA_FIRSEL), \
 	GPIO_FN(PORT243_VIO_CKO2),
-	GPIO_FN(PORT244_SCIFA5_CTS_), \
 	GPIO_FN(MFG2_IN1), \
-	GPIO_FN(PORT244_SCIFB_CTS_), \
 	GPIO_FN(MSIOF2R_RXD),
-	GPIO_FN(PORT245_SCIFA5_RTS_), \
 	GPIO_FN(MFG2_IN2), \
-	GPIO_FN(PORT245_SCIFB_RTS_), \
 	GPIO_FN(MSIOF2R_TXD),
-	GPIO_FN(PORT246_SCIFA5_RXD), \
 	GPIO_FN(MFG1_OUT1), \
-	GPIO_FN(PORT246_SCIFB_RXD), \
 	GPIO_FN(TPU1TO0),
-	GPIO_FN(PORT247_SCIFA5_TXD), \
 	GPIO_FN(MFG3_OUT2), \
-	GPIO_FN(PORT247_SCIFB_TXD), \
 	GPIO_FN(TPU3TO1),
-	GPIO_FN(PORT248_SCIFA5_SCK), \
 	GPIO_FN(MFG2_OUT1), \
-	GPIO_FN(PORT248_SCIFB_SCK), \
 	GPIO_FN(TPU2TO0), \
-	GPIO_FN(PORT248_I2C_SCL3), \
 	GPIO_FN(MSIOF2R_TSCK),
 	GPIO_FN(PORT249_IROUT), \
 	GPIO_FN(MFG4_IN1), \
-	GPIO_FN(PORT249_I2C_SDA3), \
 	GPIO_FN(MSIOF2R_TSYNC),
 	GPIO_FN(SDHICLK0),
 	GPIO_FN(SDHICD0),
@@ -2172,56 +3320,24 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	GPIO_FN(IRQ9_MEM_INT),
 	GPIO_FN(IRQ9_MCP_INT),
 	GPIO_FN(A11),
-	GPIO_FN(KEYOUT8),
 	GPIO_FN(TPU4TO3),
 	GPIO_FN(RESETA_N_PU_ON),
 	GPIO_FN(RESETA_N_PU_OFF),
 	GPIO_FN(EDBGREQ_PD),
 	GPIO_FN(EDBGREQ_PU),
+};
 
-	/* Functions with pull-ups */
-	GPIO_FN(KEYIN0_PU),
-	GPIO_FN(KEYIN1_PU),
-	GPIO_FN(KEYIN2_PU),
-	GPIO_FN(KEYIN3_PU),
-	GPIO_FN(KEYIN4_PU),
-	GPIO_FN(KEYIN5_PU),
-	GPIO_FN(KEYIN6_PU),
-	GPIO_FN(KEYIN7_PU),
-	GPIO_FN(SDHICD0_PU),
-	GPIO_FN(SDHID0_0_PU),
-	GPIO_FN(SDHID0_1_PU),
-	GPIO_FN(SDHID0_2_PU),
-	GPIO_FN(SDHID0_3_PU),
-	GPIO_FN(SDHICMD0_PU),
-	GPIO_FN(SDHIWP0_PU),
-	GPIO_FN(SDHID1_0_PU),
-	GPIO_FN(SDHID1_1_PU),
-	GPIO_FN(SDHID1_2_PU),
-	GPIO_FN(SDHID1_3_PU),
-	GPIO_FN(SDHICMD1_PU),
-	GPIO_FN(SDHID2_0_PU),
-	GPIO_FN(SDHID2_1_PU),
-	GPIO_FN(SDHID2_2_PU),
-	GPIO_FN(SDHID2_3_PU),
-	GPIO_FN(SDHICMD2_PU),
-	GPIO_FN(MMCCMD0_PU),
-	GPIO_FN(MMCCMD1_PU),
-	GPIO_FN(MMCD0_0_PU),
-	GPIO_FN(MMCD0_1_PU),
-	GPIO_FN(MMCD0_2_PU),
-	GPIO_FN(MMCD0_3_PU),
-	GPIO_FN(MMCD0_4_PU),
-	GPIO_FN(MMCD0_5_PU),
-	GPIO_FN(MMCD0_6_PU),
-	GPIO_FN(MMCD0_7_PU),
-	GPIO_FN(FSIACK_PU),
-	GPIO_FN(FSIAILR_PU),
-	GPIO_FN(FSIAIBT_PU),
-	GPIO_FN(FSIAISLD_PU),
-};
-
-static struct pinmux_cfg_reg pinmux_config_regs[] = {
+#undef PORTCR
+#define PORTCR(nr, reg)							\
+	{								\
+		PINMUX_CFG_REG("PORT" nr "CR", reg, 8, 4) {		\
+			_PCRH(PORT##nr##_IN, 0, 0, PORT##nr##_OUT),	\
+				PORT##nr##_FN0, PORT##nr##_FN1,		\
+				PORT##nr##_FN2, PORT##nr##_FN3,		\
+				PORT##nr##_FN4, PORT##nr##_FN5,		\
+				PORT##nr##_FN6, PORT##nr##_FN7 }	\
+	}
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	PORTCR(0, 0xe6050000), /* PORT0CR */
 	PORTCR(1, 0xe6050001), /* PORT1CR */
 	PORTCR(2, 0xe6050002), /* PORT2CR */
@@ -2629,7 +3745,7 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ },
 };
 
-static struct pinmux_data_reg pinmux_data_regs[] = {
+static const struct pinmux_data_reg pinmux_data_regs[] = {
 	{ PINMUX_DATA_REG("PORTL031_000DR", 0xe6054000, 32) {
 			PORT31_DATA, PORT30_DATA, PORT29_DATA, PORT28_DATA,
 			PORT27_DATA, PORT26_DATA, PORT25_DATA, PORT24_DATA,
@@ -2733,60 +3849,116 @@ static struct pinmux_data_reg pinmux_data_regs[] = {
 	{ },
 };
 
-/* IRQ pins through INTCS with IRQ0->15 from 0x200 and IRQ16-31 from 0x3200 */
-#define EXT_IRQ16L(n) intcs_evt2irq(0x200 + ((n) << 5))
-#define EXT_IRQ16H(n) intcs_evt2irq(0x3200 + ((n - 16) << 5))
-
-static struct pinmux_irq pinmux_irqs[] = {
-	PINMUX_IRQ(EXT_IRQ16H(19), PORT9_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(1), PORT10_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(0), PORT11_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(18), PORT13_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(20), PORT14_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(21), PORT15_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(31), PORT26_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(30), PORT27_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(29), PORT28_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(22), PORT40_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(23), PORT53_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(10), PORT54_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(9), PORT56_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(26), PORT115_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(27), PORT116_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(28), PORT117_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(24), PORT118_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(6), PORT147_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(2), PORT149_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(7), PORT150_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(12), PORT156_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(4), PORT159_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(25), PORT164_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(8), PORT223_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(3), PORT224_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(5), PORT227_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(17), PORT234_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(11), PORT238_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(13), PORT239_FN0),
-	PINMUX_IRQ(EXT_IRQ16H(16), PORT249_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(14), PORT251_FN0),
-	PINMUX_IRQ(EXT_IRQ16L(9), PORT308_FN0),
-};
-
-struct sh_pfc_soc_info sh73a0_pinmux_info = {
+/* External IRQ pins mapped at IRQPIN_BASE */
+#define EXT_IRQ16L(n) irq_pin(n)
+#define EXT_IRQ16H(n) irq_pin(n)
+
+static const struct pinmux_irq pinmux_irqs[] = {
+	PINMUX_IRQ(EXT_IRQ16H(19), 9),
+	PINMUX_IRQ(EXT_IRQ16L(1), 10),
+	PINMUX_IRQ(EXT_IRQ16L(0), 11),
+	PINMUX_IRQ(EXT_IRQ16H(18), 13),
+	PINMUX_IRQ(EXT_IRQ16H(20), 14),
+	PINMUX_IRQ(EXT_IRQ16H(21), 15),
+	PINMUX_IRQ(EXT_IRQ16H(31), 26),
+	PINMUX_IRQ(EXT_IRQ16H(30), 27),
+	PINMUX_IRQ(EXT_IRQ16H(29), 28),
+	PINMUX_IRQ(EXT_IRQ16H(22), 40),
+	PINMUX_IRQ(EXT_IRQ16H(23), 53),
+	PINMUX_IRQ(EXT_IRQ16L(10), 54),
+	PINMUX_IRQ(EXT_IRQ16L(9), 56),
+	PINMUX_IRQ(EXT_IRQ16H(26), 115),
+	PINMUX_IRQ(EXT_IRQ16H(27), 116),
+	PINMUX_IRQ(EXT_IRQ16H(28), 117),
+	PINMUX_IRQ(EXT_IRQ16H(24), 118),
+	PINMUX_IRQ(EXT_IRQ16L(6), 147),
+	PINMUX_IRQ(EXT_IRQ16L(2), 149),
+	PINMUX_IRQ(EXT_IRQ16L(7), 150),
+	PINMUX_IRQ(EXT_IRQ16L(12), 156),
+	PINMUX_IRQ(EXT_IRQ16L(4), 159),
+	PINMUX_IRQ(EXT_IRQ16H(25), 164),
+	PINMUX_IRQ(EXT_IRQ16L(8), 223),
+	PINMUX_IRQ(EXT_IRQ16L(3), 224),
+	PINMUX_IRQ(EXT_IRQ16L(5), 227),
+	PINMUX_IRQ(EXT_IRQ16H(17), 234),
+	PINMUX_IRQ(EXT_IRQ16L(11), 238),
+	PINMUX_IRQ(EXT_IRQ16L(13), 239),
+	PINMUX_IRQ(EXT_IRQ16H(16), 249),
+	PINMUX_IRQ(EXT_IRQ16L(14), 251),
+	PINMUX_IRQ(EXT_IRQ16L(9), 308),
+};
+
+#define PORTnCR_PULMD_OFF	(0 << 6)
+#define PORTnCR_PULMD_DOWN	(2 << 6)
+#define PORTnCR_PULMD_UP	(3 << 6)
+#define PORTnCR_PULMD_MASK	(3 << 6)
+
+static const unsigned int sh73a0_portcr_offsets[] = {
+	0x00000000, 0x00001000, 0x00001000, 0x00002000, 0x00002000,
+	0x00002000, 0x00002000, 0x00003000, 0x00003000, 0x00002000,
+};
+
+static unsigned int sh73a0_pinmux_get_bias(struct sh_pfc *pfc, unsigned int pin)
+{
+	void __iomem *addr = pfc->window->virt
+			   + sh73a0_portcr_offsets[pin >> 5] + pin;
+	u32 value = ioread8(addr) & PORTnCR_PULMD_MASK;
+
+	switch (value) {
+	case PORTnCR_PULMD_UP:
+		return PIN_CONFIG_BIAS_PULL_UP;
+	case PORTnCR_PULMD_DOWN:
+		return PIN_CONFIG_BIAS_PULL_DOWN;
+	case PORTnCR_PULMD_OFF:
+	default:
+		return PIN_CONFIG_BIAS_DISABLE;
+	}
+}
+
+static void sh73a0_pinmux_set_bias(struct sh_pfc *pfc, unsigned int pin,
+				   unsigned int bias)
+{
+	void __iomem *addr = pfc->window->virt
+			   + sh73a0_portcr_offsets[pin >> 5] + pin;
+	u32 value = ioread8(addr) & ~PORTnCR_PULMD_MASK;
+
+	switch (bias) {
+	case PIN_CONFIG_BIAS_PULL_UP:
+		value |= PORTnCR_PULMD_UP;
+		break;
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		value |= PORTnCR_PULMD_DOWN;
+		break;
+	}
+
+	iowrite8(value, addr);
+}
+
+static const struct sh_pfc_soc_operations sh73a0_pinmux_ops = {
+	.get_bias = sh73a0_pinmux_get_bias,
+	.set_bias = sh73a0_pinmux_set_bias,
+};
+
+const struct sh_pfc_soc_info sh73a0_pinmux_info = {
 	.name = "sh73a0_pfc",
-	.reserved_id = PINMUX_RESERVED,
-	.data = { PINMUX_DATA_BEGIN, PINMUX_DATA_END },
+	.ops = &sh73a0_pinmux_ops,
+
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END },
-	.input_pu = { PINMUX_INPUT_PULLUP_BEGIN, PINMUX_INPUT_PULLUP_END },
-	.input_pd = { PINMUX_INPUT_PULLDOWN_BEGIN, PINMUX_INPUT_PULLDOWN_END },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END },
-	.mark = { PINMUX_MARK_BEGIN, PINMUX_MARK_END },
 	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
 
-	.first_gpio = GPIO_PORT0,
-	.last_gpio = GPIO_FN_FSIAISLD_PU,
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.ranges = pinmux_ranges,
+	.nr_ranges = ARRAY_SIZE(pinmux_ranges),
+	.groups = pinmux_groups,
+	.nr_groups = ARRAY_SIZE(pinmux_groups),
+	.functions = pinmux_functions,
+	.nr_functions = ARRAY_SIZE(pinmux_functions),
+
+	.func_gpios = pinmux_func_gpios,
+	.nr_func_gpios = ARRAY_SIZE(pinmux_func_gpios),
 
-	.gpios = pinmux_gpios,
 	.cfg_regs = pinmux_config_regs,
 	.data_regs = pinmux_data_regs,
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh7720.c b/drivers/pinctrl/sh-pfc/pfc-sh7720.c
index 10872ed688a6..52e9f6be665f 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh7720.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh7720.c
@@ -262,7 +262,7 @@ enum {
 	PINMUX_MARK_END,
 };
 
-static pinmux_enum_t pinmux_data[] = {
+static const pinmux_enum_t pinmux_data[] = {
 	/* PTA GPIO */
 	PINMUX_DATA(PTA7_DATA, PTA7_IN, PTA7_OUT, PTA7_IN_PU),
 	PINMUX_DATA(PTA6_DATA, PTA6_IN, PTA6_OUT, PTA6_IN_PU),
@@ -606,7 +606,7 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA(SIM_CLK_MARK, PSELD_1_0_10, PTV0_FN),
 };
 
-static struct pinmux_gpio pinmux_gpios[] = {
+static struct sh_pfc_pin pinmux_pins[] = {
 	/* PTA */
 	PINMUX_GPIO(GPIO_PTA7, PTA7_DATA),
 	PINMUX_GPIO(GPIO_PTA6, PTA6_DATA),
@@ -759,202 +759,205 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	PINMUX_GPIO(GPIO_PTV2, PTV2_DATA),
 	PINMUX_GPIO(GPIO_PTV1, PTV1_DATA),
 	PINMUX_GPIO(GPIO_PTV0, PTV0_DATA),
+};
+
+#define PINMUX_FN_BASE	ARRAY_SIZE(pinmux_pins)
 
+static const struct pinmux_func pinmux_func_gpios[] = {
 	/* BSC */
-	PINMUX_GPIO(GPIO_FN_D31, D31_MARK),
-	PINMUX_GPIO(GPIO_FN_D30, D30_MARK),
-	PINMUX_GPIO(GPIO_FN_D29, D29_MARK),
-	PINMUX_GPIO(GPIO_FN_D28, D28_MARK),
-	PINMUX_GPIO(GPIO_FN_D27, D27_MARK),
-	PINMUX_GPIO(GPIO_FN_D26, D26_MARK),
-	PINMUX_GPIO(GPIO_FN_D25, D25_MARK),
-	PINMUX_GPIO(GPIO_FN_D24, D24_MARK),
-	PINMUX_GPIO(GPIO_FN_D23, D23_MARK),
-	PINMUX_GPIO(GPIO_FN_D22, D22_MARK),
-	PINMUX_GPIO(GPIO_FN_D21, D21_MARK),
-	PINMUX_GPIO(GPIO_FN_D20, D20_MARK),
-	PINMUX_GPIO(GPIO_FN_D19, D19_MARK),
-	PINMUX_GPIO(GPIO_FN_D18, D18_MARK),
-	PINMUX_GPIO(GPIO_FN_D17, D17_MARK),
-	PINMUX_GPIO(GPIO_FN_D16, D16_MARK),
-	PINMUX_GPIO(GPIO_FN_IOIS16, IOIS16_MARK),
-	PINMUX_GPIO(GPIO_FN_RAS, RAS_MARK),
-	PINMUX_GPIO(GPIO_FN_CAS, CAS_MARK),
-	PINMUX_GPIO(GPIO_FN_CKE, CKE_MARK),
-	PINMUX_GPIO(GPIO_FN_CS5B_CE1A, CS5B_CE1A_MARK),
-	PINMUX_GPIO(GPIO_FN_CS6B_CE1B, CS6B_CE1B_MARK),
-	PINMUX_GPIO(GPIO_FN_A25, A25_MARK),
-	PINMUX_GPIO(GPIO_FN_A24, A24_MARK),
-	PINMUX_GPIO(GPIO_FN_A23, A23_MARK),
-	PINMUX_GPIO(GPIO_FN_A22, A22_MARK),
-	PINMUX_GPIO(GPIO_FN_A21, A21_MARK),
-	PINMUX_GPIO(GPIO_FN_A20, A20_MARK),
-	PINMUX_GPIO(GPIO_FN_A19, A19_MARK),
-	PINMUX_GPIO(GPIO_FN_A0, A0_MARK),
-	PINMUX_GPIO(GPIO_FN_REFOUT, REFOUT_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQOUT, IRQOUT_MARK),
+	GPIO_FN(D31),
+	GPIO_FN(D30),
+	GPIO_FN(D29),
+	GPIO_FN(D28),
+	GPIO_FN(D27),
+	GPIO_FN(D26),
+	GPIO_FN(D25),
+	GPIO_FN(D24),
+	GPIO_FN(D23),
+	GPIO_FN(D22),
+	GPIO_FN(D21),
+	GPIO_FN(D20),
+	GPIO_FN(D19),
+	GPIO_FN(D18),
+	GPIO_FN(D17),
+	GPIO_FN(D16),
+	GPIO_FN(IOIS16),
+	GPIO_FN(RAS),
+	GPIO_FN(CAS),
+	GPIO_FN(CKE),
+	GPIO_FN(CS5B_CE1A),
+	GPIO_FN(CS6B_CE1B),
+	GPIO_FN(A25),
+	GPIO_FN(A24),
+	GPIO_FN(A23),
+	GPIO_FN(A22),
+	GPIO_FN(A21),
+	GPIO_FN(A20),
+	GPIO_FN(A19),
+	GPIO_FN(A0),
+	GPIO_FN(REFOUT),
+	GPIO_FN(IRQOUT),
 
 	/* LCDC */
-	PINMUX_GPIO(GPIO_FN_LCD_DATA15, LCD_DATA15_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA14, LCD_DATA14_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA13, LCD_DATA13_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA12, LCD_DATA12_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA11, LCD_DATA11_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA10, LCD_DATA10_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA9, LCD_DATA9_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA8, LCD_DATA8_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA7, LCD_DATA7_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA6, LCD_DATA6_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA5, LCD_DATA5_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA4, LCD_DATA4_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA3, LCD_DATA3_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA2, LCD_DATA2_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA1, LCD_DATA1_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DATA0, LCD_DATA0_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_M_DISP, LCD_M_DISP_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_CL1, LCD_CL1_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_CL2, LCD_CL2_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_DON, LCD_DON_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_FLM, LCD_FLM_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_VEPWC, LCD_VEPWC_MARK),
-	PINMUX_GPIO(GPIO_FN_LCD_VCPWC, LCD_VCPWC_MARK),
+	GPIO_FN(LCD_DATA15),
+	GPIO_FN(LCD_DATA14),
+	GPIO_FN(LCD_DATA13),
+	GPIO_FN(LCD_DATA12),
+	GPIO_FN(LCD_DATA11),
+	GPIO_FN(LCD_DATA10),
+	GPIO_FN(LCD_DATA9),
+	GPIO_FN(LCD_DATA8),
+	GPIO_FN(LCD_DATA7),
+	GPIO_FN(LCD_DATA6),
+	GPIO_FN(LCD_DATA5),
+	GPIO_FN(LCD_DATA4),
+	GPIO_FN(LCD_DATA3),
+	GPIO_FN(LCD_DATA2),
+	GPIO_FN(LCD_DATA1),
+	GPIO_FN(LCD_DATA0),
+	GPIO_FN(LCD_M_DISP),
+	GPIO_FN(LCD_CL1),
+	GPIO_FN(LCD_CL2),
+	GPIO_FN(LCD_DON),
+	GPIO_FN(LCD_FLM),
+	GPIO_FN(LCD_VEPWC),
+	GPIO_FN(LCD_VCPWC),
 
 	/* AFEIF */
-	PINMUX_GPIO(GPIO_FN_AFE_RXIN, AFE_RXIN_MARK),
-	PINMUX_GPIO(GPIO_FN_AFE_RDET, AFE_RDET_MARK),
-	PINMUX_GPIO(GPIO_FN_AFE_FS, AFE_FS_MARK),
-	PINMUX_GPIO(GPIO_FN_AFE_TXOUT, AFE_TXOUT_MARK),
-	PINMUX_GPIO(GPIO_FN_AFE_SCLK, AFE_SCLK_MARK),
-	PINMUX_GPIO(GPIO_FN_AFE_RLYCNT, AFE_RLYCNT_MARK),
-	PINMUX_GPIO(GPIO_FN_AFE_HC1, AFE_HC1_MARK),
+	GPIO_FN(AFE_RXIN),
+	GPIO_FN(AFE_RDET),
+	GPIO_FN(AFE_FS),
+	GPIO_FN(AFE_TXOUT),
+	GPIO_FN(AFE_SCLK),
+	GPIO_FN(AFE_RLYCNT),
+	GPIO_FN(AFE_HC1),
 
 	/* IIC */
-	PINMUX_GPIO(GPIO_FN_IIC_SCL, IIC_SCL_MARK),
-	PINMUX_GPIO(GPIO_FN_IIC_SDA, IIC_SDA_MARK),
+	GPIO_FN(IIC_SCL),
+	GPIO_FN(IIC_SDA),
 
 	/* DAC */
-	PINMUX_GPIO(GPIO_FN_DA1, DA1_MARK),
-	PINMUX_GPIO(GPIO_FN_DA0, DA0_MARK),
+	GPIO_FN(DA1),
+	GPIO_FN(DA0),
 
 	/* ADC */
-	PINMUX_GPIO(GPIO_FN_AN3, AN3_MARK),
-	PINMUX_GPIO(GPIO_FN_AN2, AN2_MARK),
-	PINMUX_GPIO(GPIO_FN_AN1, AN1_MARK),
-	PINMUX_GPIO(GPIO_FN_AN0, AN0_MARK),
-	PINMUX_GPIO(GPIO_FN_ADTRG, ADTRG_MARK),
+	GPIO_FN(AN3),
+	GPIO_FN(AN2),
+	GPIO_FN(AN1),
+	GPIO_FN(AN0),
+	GPIO_FN(ADTRG),
 
 	/* USB */
-	PINMUX_GPIO(GPIO_FN_USB1D_RCV, USB1D_RCV_MARK),
-	PINMUX_GPIO(GPIO_FN_USB1D_TXSE0, USB1D_TXSE0_MARK),
-	PINMUX_GPIO(GPIO_FN_USB1D_TXDPLS, USB1D_TXDPLS_MARK),
-	PINMUX_GPIO(GPIO_FN_USB1D_DMNS, USB1D_DMNS_MARK),
-	PINMUX_GPIO(GPIO_FN_USB1D_DPLS, USB1D_DPLS_MARK),
-	PINMUX_GPIO(GPIO_FN_USB1D_SPEED, USB1D_SPEED_MARK),
-	PINMUX_GPIO(GPIO_FN_USB1D_TXENL, USB1D_TXENL_MARK),
-
-	PINMUX_GPIO(GPIO_FN_USB2_PWR_EN, USB2_PWR_EN_MARK),
-	PINMUX_GPIO(GPIO_FN_USB1_PWR_EN_USBF_UPLUP,
-		    USB1_PWR_EN_USBF_UPLUP_MARK),
-	PINMUX_GPIO(GPIO_FN_USB1D_SUSPEND, USB1D_SUSPEND_MARK),
+	GPIO_FN(USB1D_RCV),
+	GPIO_FN(USB1D_TXSE0),
+	GPIO_FN(USB1D_TXDPLS),
+	GPIO_FN(USB1D_DMNS),
+	GPIO_FN(USB1D_DPLS),
+	GPIO_FN(USB1D_SPEED),
+	GPIO_FN(USB1D_TXENL),
+
+	GPIO_FN(USB2_PWR_EN),
+	GPIO_FN(USB1_PWR_EN_USBF_UPLUP),
+	GPIO_FN(USB1D_SUSPEND),
 
 	/* INTC */
-	PINMUX_GPIO(GPIO_FN_IRQ5, IRQ5_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ4, IRQ4_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ3_IRL3, IRQ3_IRL3_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ2_IRL2, IRQ2_IRL2_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ1_IRL1, IRQ1_IRL1_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ0_IRL0, IRQ0_IRL0_MARK),
+	GPIO_FN(IRQ5),
+	GPIO_FN(IRQ4),
+	GPIO_FN(IRQ3_IRL3),
+	GPIO_FN(IRQ2_IRL2),
+	GPIO_FN(IRQ1_IRL1),
+	GPIO_FN(IRQ0_IRL0),
 
 	/* PCC */
-	PINMUX_GPIO(GPIO_FN_PCC_REG, PCC_REG_MARK),
-	PINMUX_GPIO(GPIO_FN_PCC_DRV, PCC_DRV_MARK),
-	PINMUX_GPIO(GPIO_FN_PCC_BVD2, PCC_BVD2_MARK),
-	PINMUX_GPIO(GPIO_FN_PCC_BVD1, PCC_BVD1_MARK),
-	PINMUX_GPIO(GPIO_FN_PCC_CD2, PCC_CD2_MARK),
-	PINMUX_GPIO(GPIO_FN_PCC_CD1, PCC_CD1_MARK),
-	PINMUX_GPIO(GPIO_FN_PCC_RESET, PCC_RESET_MARK),
-	PINMUX_GPIO(GPIO_FN_PCC_RDY, PCC_RDY_MARK),
-	PINMUX_GPIO(GPIO_FN_PCC_VS2, PCC_VS2_MARK),
-	PINMUX_GPIO(GPIO_FN_PCC_VS1, PCC_VS1_MARK),
+	GPIO_FN(PCC_REG),
+	GPIO_FN(PCC_DRV),
+	GPIO_FN(PCC_BVD2),
+	GPIO_FN(PCC_BVD1),
+	GPIO_FN(PCC_CD2),
+	GPIO_FN(PCC_CD1),
+	GPIO_FN(PCC_RESET),
+	GPIO_FN(PCC_RDY),
+	GPIO_FN(PCC_VS2),
+	GPIO_FN(PCC_VS1),
 
 	/* HUDI */
-	PINMUX_GPIO(GPIO_FN_AUDATA3, AUDATA3_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA2, AUDATA2_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA1, AUDATA1_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA0, AUDATA0_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDCK, AUDCK_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDSYNC, AUDSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_ASEBRKAK, ASEBRKAK_MARK),
-	PINMUX_GPIO(GPIO_FN_TRST, TRST_MARK),
-	PINMUX_GPIO(GPIO_FN_TMS, TMS_MARK),
-	PINMUX_GPIO(GPIO_FN_TDO, TDO_MARK),
-	PINMUX_GPIO(GPIO_FN_TDI, TDI_MARK),
-	PINMUX_GPIO(GPIO_FN_TCK, TCK_MARK),
+	GPIO_FN(AUDATA3),
+	GPIO_FN(AUDATA2),
+	GPIO_FN(AUDATA1),
+	GPIO_FN(AUDATA0),
+	GPIO_FN(AUDCK),
+	GPIO_FN(AUDSYNC),
+	GPIO_FN(ASEBRKAK),
+	GPIO_FN(TRST),
+	GPIO_FN(TMS),
+	GPIO_FN(TDO),
+	GPIO_FN(TDI),
+	GPIO_FN(TCK),
 
 	/* DMAC */
-	PINMUX_GPIO(GPIO_FN_DACK1, DACK1_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ1, DREQ1_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK0, DACK0_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ0, DREQ0_MARK),
-	PINMUX_GPIO(GPIO_FN_TEND1, TEND1_MARK),
-	PINMUX_GPIO(GPIO_FN_TEND0, TEND0_MARK),
+	GPIO_FN(DACK1),
+	GPIO_FN(DREQ1),
+	GPIO_FN(DACK0),
+	GPIO_FN(DREQ0),
+	GPIO_FN(TEND1),
+	GPIO_FN(TEND0),
 
 	/* SIOF0 */
-	PINMUX_GPIO(GPIO_FN_SIOF0_SYNC, SIOF0_SYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF0_MCLK, SIOF0_MCLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF0_TXD, SIOF0_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF0_RXD, SIOF0_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF0_SCK, SIOF0_SCK_MARK),
+	GPIO_FN(SIOF0_SYNC),
+	GPIO_FN(SIOF0_MCLK),
+	GPIO_FN(SIOF0_TXD),
+	GPIO_FN(SIOF0_RXD),
+	GPIO_FN(SIOF0_SCK),
 
 	/* SIOF1 */
-	PINMUX_GPIO(GPIO_FN_SIOF1_SYNC, SIOF1_SYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF1_MCLK, SIOF1_MCLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF1_TXD, SIOF1_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF1_RXD, SIOF1_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF1_SCK, SIOF1_SCK_MARK),
+	GPIO_FN(SIOF1_SYNC),
+	GPIO_FN(SIOF1_MCLK),
+	GPIO_FN(SIOF1_TXD),
+	GPIO_FN(SIOF1_RXD),
+	GPIO_FN(SIOF1_SCK),
 
 	/* SCIF0 */
-	PINMUX_GPIO(GPIO_FN_SCIF0_TXD, SCIF0_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_RXD, SCIF0_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_RTS, SCIF0_RTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_CTS, SCIF0_CTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_SCK, SCIF0_SCK_MARK),
+	GPIO_FN(SCIF0_TXD),
+	GPIO_FN(SCIF0_RXD),
+	GPIO_FN(SCIF0_RTS),
+	GPIO_FN(SCIF0_CTS),
+	GPIO_FN(SCIF0_SCK),
 
 	/* SCIF1 */
-	PINMUX_GPIO(GPIO_FN_SCIF1_TXD, SCIF1_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_RXD, SCIF1_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_RTS, SCIF1_RTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_CTS, SCIF1_CTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_SCK, SCIF1_SCK_MARK),
+	GPIO_FN(SCIF1_TXD),
+	GPIO_FN(SCIF1_RXD),
+	GPIO_FN(SCIF1_RTS),
+	GPIO_FN(SCIF1_CTS),
+	GPIO_FN(SCIF1_SCK),
 
 	/* TPU */
-	PINMUX_GPIO(GPIO_FN_TPU_TO1, TPU_TO1_MARK),
-	PINMUX_GPIO(GPIO_FN_TPU_TO0, TPU_TO0_MARK),
-	PINMUX_GPIO(GPIO_FN_TPU_TI3B, TPU_TI3B_MARK),
-	PINMUX_GPIO(GPIO_FN_TPU_TI3A, TPU_TI3A_MARK),
-	PINMUX_GPIO(GPIO_FN_TPU_TI2B, TPU_TI2B_MARK),
-	PINMUX_GPIO(GPIO_FN_TPU_TI2A, TPU_TI2A_MARK),
-	PINMUX_GPIO(GPIO_FN_TPU_TO3, TPU_TO3_MARK),
-	PINMUX_GPIO(GPIO_FN_TPU_TO2, TPU_TO2_MARK),
+	GPIO_FN(TPU_TO1),
+	GPIO_FN(TPU_TO0),
+	GPIO_FN(TPU_TI3B),
+	GPIO_FN(TPU_TI3A),
+	GPIO_FN(TPU_TI2B),
+	GPIO_FN(TPU_TI2A),
+	GPIO_FN(TPU_TO3),
+	GPIO_FN(TPU_TO2),
 
 	/* SIM */
-	PINMUX_GPIO(GPIO_FN_SIM_D, SIM_D_MARK),
-	PINMUX_GPIO(GPIO_FN_SIM_CLK, SIM_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SIM_RST, SIM_RST_MARK),
+	GPIO_FN(SIM_D),
+	GPIO_FN(SIM_CLK),
+	GPIO_FN(SIM_RST),
 
 	/* MMC */
-	PINMUX_GPIO(GPIO_FN_MMC_DAT, MMC_DAT_MARK),
-	PINMUX_GPIO(GPIO_FN_MMC_CMD, MMC_CMD_MARK),
-	PINMUX_GPIO(GPIO_FN_MMC_CLK, MMC_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_MMC_VDDON, MMC_VDDON_MARK),
-	PINMUX_GPIO(GPIO_FN_MMC_ODMOD, MMC_ODMOD_MARK),
+	GPIO_FN(MMC_DAT),
+	GPIO_FN(MMC_CMD),
+	GPIO_FN(MMC_CLK),
+	GPIO_FN(MMC_VDDON),
+	GPIO_FN(MMC_ODMOD),
 
 	/* SYSC */
-	PINMUX_GPIO(GPIO_FN_STATUS0, STATUS0_MARK),
-	PINMUX_GPIO(GPIO_FN_STATUS1, STATUS1_MARK),
+	GPIO_FN(STATUS0),
+	GPIO_FN(STATUS1),
 };
 
-static struct pinmux_cfg_reg pinmux_config_regs[] = {
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ PINMUX_CFG_REG("PACR", 0xa4050100, 16, 2) {
 		PTA7_FN, PTA7_OUT, PTA7_IN_PU, PTA7_IN,
 		PTA6_FN, PTA6_OUT, PTA6_IN_PU, PTA6_IN,
@@ -1138,7 +1141,7 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{}
 };
 
-static struct pinmux_data_reg pinmux_data_regs[] = {
+static const struct pinmux_data_reg pinmux_data_regs[] = {
 	{ PINMUX_DATA_REG("PADR", 0xa4050140, 8) {
 		PTA7_DATA, PTA6_DATA, PTA5_DATA, PTA4_DATA,
 		PTA3_DATA, PTA2_DATA, PTA1_DATA, PTA0_DATA }
@@ -1214,20 +1217,18 @@ static struct pinmux_data_reg pinmux_data_regs[] = {
 	{ },
 };
 
-struct sh_pfc_soc_info sh7720_pinmux_info = {
+const struct sh_pfc_soc_info sh7720_pinmux_info = {
 	.name = "sh7720_pfc",
-	.reserved_id = PINMUX_RESERVED,
-	.data = { PINMUX_DATA_BEGIN, PINMUX_DATA_END },
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END },
 	.input_pu = { PINMUX_INPUT_PULLUP_BEGIN, PINMUX_INPUT_PULLUP_END },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END },
-	.mark = { PINMUX_MARK_BEGIN, PINMUX_MARK_END },
 	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
 
-	.first_gpio = GPIO_PTA7,
-	.last_gpio = GPIO_FN_STATUS1,
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.func_gpios = pinmux_func_gpios,
+	.nr_func_gpios = ARRAY_SIZE(pinmux_func_gpios),
 
-	.gpios = pinmux_gpios,
 	.cfg_regs = pinmux_config_regs,
 	.data_regs = pinmux_data_regs,
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh7722.c b/drivers/pinctrl/sh-pfc/pfc-sh7722.c
index 2de0929315e6..32034387477b 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh7722.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh7722.c
@@ -296,7 +296,7 @@ enum {
 	PINMUX_FUNCTION_END,
 };
 
-static pinmux_enum_t pinmux_data[] = {
+static const pinmux_enum_t pinmux_data[] = {
 	/* PTA */
 	PINMUX_DATA(PTA7_DATA, PTA7_IN, PTA7_IN_PD, PTA7_OUT),
 	PINMUX_DATA(PTA6_DATA, PTA6_IN, PTA6_IN_PD),
@@ -787,7 +787,7 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA(KEYOUT5_IN5_MARK, HIZA14_KEYSC, KEYOUT5_IN5),
 };
 
-static struct pinmux_gpio pinmux_gpios[] = {
+static struct sh_pfc_pin pinmux_pins[] = {
 	/* PTA */
 	PINMUX_GPIO(GPIO_PTA7, PTA7_DATA),
 	PINMUX_GPIO(GPIO_PTA6, PTA6_DATA),
@@ -982,289 +982,293 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	PINMUX_GPIO(GPIO_PTZ3, PTZ3_DATA),
 	PINMUX_GPIO(GPIO_PTZ2, PTZ2_DATA),
 	PINMUX_GPIO(GPIO_PTZ1, PTZ1_DATA),
+};
+
+#define PINMUX_FN_BASE	ARRAY_SIZE(pinmux_pins)
 
+static const struct pinmux_func pinmux_func_gpios[] = {
 	/* SCIF0 */
-	PINMUX_GPIO(GPIO_FN_SCIF0_TXD, SCIF0_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_RXD, SCIF0_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_RTS, SCIF0_RTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_CTS, SCIF0_CTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_SCK, SCIF0_SCK_MARK),
+	GPIO_FN(SCIF0_TXD),
+	GPIO_FN(SCIF0_RXD),
+	GPIO_FN(SCIF0_RTS),
+	GPIO_FN(SCIF0_CTS),
+	GPIO_FN(SCIF0_SCK),
 
 	/* SCIF1 */
-	PINMUX_GPIO(GPIO_FN_SCIF1_TXD, SCIF1_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_RXD, SCIF1_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_RTS, SCIF1_RTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_CTS, SCIF1_CTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_SCK, SCIF1_SCK_MARK),
+	GPIO_FN(SCIF1_TXD),
+	GPIO_FN(SCIF1_RXD),
+	GPIO_FN(SCIF1_RTS),
+	GPIO_FN(SCIF1_CTS),
+	GPIO_FN(SCIF1_SCK),
 
 	/* SCIF2 */
-	PINMUX_GPIO(GPIO_FN_SCIF2_TXD, SCIF2_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_RXD, SCIF2_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_RTS, SCIF2_RTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_CTS, SCIF2_CTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_SCK, SCIF2_SCK_MARK),
+	GPIO_FN(SCIF2_TXD),
+	GPIO_FN(SCIF2_RXD),
+	GPIO_FN(SCIF2_RTS),
+	GPIO_FN(SCIF2_CTS),
+	GPIO_FN(SCIF2_SCK),
 
 	/* SIO */
-	PINMUX_GPIO(GPIO_FN_SIOTXD, SIOTXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIORXD, SIORXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOD, SIOD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOSTRB0, SIOSTRB0_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOSTRB1, SIOSTRB1_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOSCK, SIOSCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOMCK, SIOMCK_MARK),
+	GPIO_FN(SIOTXD),
+	GPIO_FN(SIORXD),
+	GPIO_FN(SIOD),
+	GPIO_FN(SIOSTRB0),
+	GPIO_FN(SIOSTRB1),
+	GPIO_FN(SIOSCK),
+	GPIO_FN(SIOMCK),
 
 	/* CEU */
-	PINMUX_GPIO(GPIO_FN_VIO_D15, VIO_D15_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D14, VIO_D14_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D13, VIO_D13_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D12, VIO_D12_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D11, VIO_D11_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D10, VIO_D10_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D9, VIO_D9_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D8, VIO_D8_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D7, VIO_D7_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D6, VIO_D6_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D5, VIO_D5_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D4, VIO_D4_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D3, VIO_D3_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D2, VIO_D2_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D1, VIO_D1_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D0, VIO_D0_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_CLK, VIO_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_VD, VIO_VD_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_HD, VIO_HD_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_FLD, VIO_FLD_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_CKO, VIO_CKO_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_STEX, VIO_STEX_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_STEM, VIO_STEM_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_VD2, VIO_VD2_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_HD2, VIO_HD2_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_CLK2, VIO_CLK2_MARK),
+	GPIO_FN(VIO_D15),
+	GPIO_FN(VIO_D14),
+	GPIO_FN(VIO_D13),
+	GPIO_FN(VIO_D12),
+	GPIO_FN(VIO_D11),
+	GPIO_FN(VIO_D10),
+	GPIO_FN(VIO_D9),
+	GPIO_FN(VIO_D8),
+	GPIO_FN(VIO_D7),
+	GPIO_FN(VIO_D6),
+	GPIO_FN(VIO_D5),
+	GPIO_FN(VIO_D4),
+	GPIO_FN(VIO_D3),
+	GPIO_FN(VIO_D2),
+	GPIO_FN(VIO_D1),
+	GPIO_FN(VIO_D0),
+	GPIO_FN(VIO_CLK),
+	GPIO_FN(VIO_VD),
+	GPIO_FN(VIO_HD),
+	GPIO_FN(VIO_FLD),
+	GPIO_FN(VIO_CKO),
+	GPIO_FN(VIO_STEX),
+	GPIO_FN(VIO_STEM),
+	GPIO_FN(VIO_VD2),
+	GPIO_FN(VIO_HD2),
+	GPIO_FN(VIO_CLK2),
 
 	/* LCDC */
-	PINMUX_GPIO(GPIO_FN_LCDD23, LCDD23_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD22, LCDD22_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD21, LCDD21_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD20, LCDD20_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD19, LCDD19_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD18, LCDD18_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD17, LCDD17_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD16, LCDD16_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD15, LCDD15_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD14, LCDD14_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD13, LCDD13_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD12, LCDD12_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD11, LCDD11_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD10, LCDD10_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD9, LCDD9_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD8, LCDD8_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD7, LCDD7_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD6, LCDD6_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD5, LCDD5_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD4, LCDD4_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD3, LCDD3_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD2, LCDD2_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD1, LCDD1_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD0, LCDD0_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDLCLK, LCDLCLK_MARK),
+	GPIO_FN(LCDD23),
+	GPIO_FN(LCDD22),
+	GPIO_FN(LCDD21),
+	GPIO_FN(LCDD20),
+	GPIO_FN(LCDD19),
+	GPIO_FN(LCDD18),
+	GPIO_FN(LCDD17),
+	GPIO_FN(LCDD16),
+	GPIO_FN(LCDD15),
+	GPIO_FN(LCDD14),
+	GPIO_FN(LCDD13),
+	GPIO_FN(LCDD12),
+	GPIO_FN(LCDD11),
+	GPIO_FN(LCDD10),
+	GPIO_FN(LCDD9),
+	GPIO_FN(LCDD8),
+	GPIO_FN(LCDD7),
+	GPIO_FN(LCDD6),
+	GPIO_FN(LCDD5),
+	GPIO_FN(LCDD4),
+	GPIO_FN(LCDD3),
+	GPIO_FN(LCDD2),
+	GPIO_FN(LCDD1),
+	GPIO_FN(LCDD0),
+	GPIO_FN(LCDLCLK),
 	/* Main LCD */
-	PINMUX_GPIO(GPIO_FN_LCDDON, LCDDON_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDVCPWC, LCDVCPWC_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDVEPWC, LCDVEPWC_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDVSYN, LCDVSYN_MARK),
+	GPIO_FN(LCDDON),
+	GPIO_FN(LCDVCPWC),
+	GPIO_FN(LCDVEPWC),
+	GPIO_FN(LCDVSYN),
 	/* Main LCD - RGB Mode */
-	PINMUX_GPIO(GPIO_FN_LCDDCK, LCDDCK_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDHSYN, LCDHSYN_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDDISP, LCDDISP_MARK),
+	GPIO_FN(LCDDCK),
+	GPIO_FN(LCDHSYN),
+	GPIO_FN(LCDDISP),
 	/* Main LCD - SYS Mode */
-	PINMUX_GPIO(GPIO_FN_LCDRS, LCDRS_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDCS, LCDCS_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDWR, LCDWR_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDRD, LCDRD_MARK),
+	GPIO_FN(LCDRS),
+	GPIO_FN(LCDCS),
+	GPIO_FN(LCDWR),
+	GPIO_FN(LCDRD),
 	/* Sub LCD - SYS Mode */
-	PINMUX_GPIO(GPIO_FN_LCDDON2, LCDDON2_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDVCPWC2, LCDVCPWC2_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDVEPWC2, LCDVEPWC2_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDVSYN2, LCDVSYN2_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDCS2, LCDCS2_MARK),
+	GPIO_FN(LCDDON2),
+	GPIO_FN(LCDVCPWC2),
+	GPIO_FN(LCDVEPWC2),
+	GPIO_FN(LCDVSYN2),
+	GPIO_FN(LCDCS2),
 
 	/* BSC */
-	PINMUX_GPIO(GPIO_FN_IOIS16, IOIS16_MARK),
-	PINMUX_GPIO(GPIO_FN_A25, A25_MARK),
-	PINMUX_GPIO(GPIO_FN_A24, A24_MARK),
-	PINMUX_GPIO(GPIO_FN_A23, A23_MARK),
-	PINMUX_GPIO(GPIO_FN_A22, A22_MARK),
-	PINMUX_GPIO(GPIO_FN_BS, BS_MARK),
-	PINMUX_GPIO(GPIO_FN_CS6B_CE1B, CS6B_CE1B_MARK),
-	PINMUX_GPIO(GPIO_FN_WAIT, WAIT_MARK),
-	PINMUX_GPIO(GPIO_FN_CS6A_CE2B, CS6A_CE2B_MARK),
+	GPIO_FN(IOIS16),
+	GPIO_FN(A25),
+	GPIO_FN(A24),
+	GPIO_FN(A23),
+	GPIO_FN(A22),
+	GPIO_FN(BS),
+	GPIO_FN(CS6B_CE1B),
+	GPIO_FN(WAIT),
+	GPIO_FN(CS6A_CE2B),
 
 	/* SBSC */
-	PINMUX_GPIO(GPIO_FN_HPD63, HPD63_MARK),
-	PINMUX_GPIO(GPIO_FN_HPD62, HPD62_MARK),
-	PINMUX_GPIO(GPIO_FN_HPD61, HPD61_MARK),
-	PINMUX_GPIO(GPIO_FN_HPD60, HPD60_MARK),
-	PINMUX_GPIO(GPIO_FN_HPD59, HPD59_MARK),
-	PINMUX_GPIO(GPIO_FN_HPD58, HPD58_MARK),
-	PINMUX_GPIO(GPIO_FN_HPD57, HPD57_MARK),
-	PINMUX_GPIO(GPIO_FN_HPD56, HPD56_MARK),
-	PINMUX_GPIO(GPIO_FN_HPD55, HPD55_MARK),
-	PINMUX_GPIO(GPIO_FN_HPD54, HPD54_MARK),
-	PINMUX_GPIO(GPIO_FN_HPD53, HPD53_MARK),
-	PINMUX_GPIO(GPIO_FN_HPD52, HPD52_MARK),
-	PINMUX_GPIO(GPIO_FN_HPD51, HPD51_MARK),
-	PINMUX_GPIO(GPIO_FN_HPD50, HPD50_MARK),
-	PINMUX_GPIO(GPIO_FN_HPD49, HPD49_MARK),
-	PINMUX_GPIO(GPIO_FN_HPD48, HPD48_MARK),
-	PINMUX_GPIO(GPIO_FN_HPDQM7, HPDQM7_MARK),
-	PINMUX_GPIO(GPIO_FN_HPDQM6, HPDQM6_MARK),
-	PINMUX_GPIO(GPIO_FN_HPDQM5, HPDQM5_MARK),
-	PINMUX_GPIO(GPIO_FN_HPDQM4, HPDQM4_MARK),
+	GPIO_FN(HPD63),
+	GPIO_FN(HPD62),
+	GPIO_FN(HPD61),
+	GPIO_FN(HPD60),
+	GPIO_FN(HPD59),
+	GPIO_FN(HPD58),
+	GPIO_FN(HPD57),
+	GPIO_FN(HPD56),
+	GPIO_FN(HPD55),
+	GPIO_FN(HPD54),
+	GPIO_FN(HPD53),
+	GPIO_FN(HPD52),
+	GPIO_FN(HPD51),
+	GPIO_FN(HPD50),
+	GPIO_FN(HPD49),
+	GPIO_FN(HPD48),
+	GPIO_FN(HPDQM7),
+	GPIO_FN(HPDQM6),
+	GPIO_FN(HPDQM5),
+	GPIO_FN(HPDQM4),
 
 	/* IRQ */
-	PINMUX_GPIO(GPIO_FN_IRQ0, IRQ0_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ1, IRQ1_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ2, IRQ2_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ3, IRQ3_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ4, IRQ4_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ5, IRQ5_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ6, IRQ6_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ7, IRQ7_MARK),
+	GPIO_FN(IRQ0),
+	GPIO_FN(IRQ1),
+	GPIO_FN(IRQ2),
+	GPIO_FN(IRQ3),
+	GPIO_FN(IRQ4),
+	GPIO_FN(IRQ5),
+	GPIO_FN(IRQ6),
+	GPIO_FN(IRQ7),
 
 	/* SDHI */
-	PINMUX_GPIO(GPIO_FN_SDHICD, SDHICD_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHIWP, SDHIWP_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHID3, SDHID3_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHID2, SDHID2_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHID1, SDHID1_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHID0, SDHID0_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHICMD, SDHICMD_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHICLK, SDHICLK_MARK),
+	GPIO_FN(SDHICD),
+	GPIO_FN(SDHIWP),
+	GPIO_FN(SDHID3),
+	GPIO_FN(SDHID2),
+	GPIO_FN(SDHID1),
+	GPIO_FN(SDHID0),
+	GPIO_FN(SDHICMD),
+	GPIO_FN(SDHICLK),
 
 	/* SIU - Port A */
-	PINMUX_GPIO(GPIO_FN_SIUAOLR, SIUAOLR_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUAOBT, SIUAOBT_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUAISLD, SIUAISLD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUAILR, SIUAILR_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUAIBT, SIUAIBT_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUAOSLD, SIUAOSLD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUMCKA, SIUMCKA_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUFCKA, SIUFCKA_MARK),
+	GPIO_FN(SIUAOLR),
+	GPIO_FN(SIUAOBT),
+	GPIO_FN(SIUAISLD),
+	GPIO_FN(SIUAILR),
+	GPIO_FN(SIUAIBT),
+	GPIO_FN(SIUAOSLD),
+	GPIO_FN(SIUMCKA),
+	GPIO_FN(SIUFCKA),
 
 	/* SIU - Port B */
-	PINMUX_GPIO(GPIO_FN_SIUBOLR, SIUBOLR_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUBOBT, SIUBOBT_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUBISLD, SIUBISLD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUBILR, SIUBILR_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUBIBT, SIUBIBT_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUBOSLD, SIUBOSLD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUMCKB, SIUMCKB_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUFCKB, SIUFCKB_MARK),
+	GPIO_FN(SIUBOLR),
+	GPIO_FN(SIUBOBT),
+	GPIO_FN(SIUBISLD),
+	GPIO_FN(SIUBILR),
+	GPIO_FN(SIUBIBT),
+	GPIO_FN(SIUBOSLD),
+	GPIO_FN(SIUMCKB),
+	GPIO_FN(SIUFCKB),
 
 	/* AUD */
-	PINMUX_GPIO(GPIO_FN_AUDSYNC, AUDSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA3, AUDATA3_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA2, AUDATA2_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA1, AUDATA1_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA0, AUDATA0_MARK),
+	GPIO_FN(AUDSYNC),
+	GPIO_FN(AUDATA3),
+	GPIO_FN(AUDATA2),
+	GPIO_FN(AUDATA1),
+	GPIO_FN(AUDATA0),
 
 	/* DMAC */
-	PINMUX_GPIO(GPIO_FN_DACK, DACK_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ0, DREQ0_MARK),
+	GPIO_FN(DACK),
+	GPIO_FN(DREQ0),
 
 	/* VOU */
-	PINMUX_GPIO(GPIO_FN_DV_CLKI, DV_CLKI_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_CLK, DV_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_HSYNC, DV_HSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_VSYNC, DV_VSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D15, DV_D15_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D14, DV_D14_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D13, DV_D13_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D12, DV_D12_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D11, DV_D11_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D10, DV_D10_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D9, DV_D9_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D8, DV_D8_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D7, DV_D7_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D6, DV_D6_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D5, DV_D5_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D4, DV_D4_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D3, DV_D3_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D2, DV_D2_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D1, DV_D1_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D0, DV_D0_MARK),
+	GPIO_FN(DV_CLKI),
+	GPIO_FN(DV_CLK),
+	GPIO_FN(DV_HSYNC),
+	GPIO_FN(DV_VSYNC),
+	GPIO_FN(DV_D15),
+	GPIO_FN(DV_D14),
+	GPIO_FN(DV_D13),
+	GPIO_FN(DV_D12),
+	GPIO_FN(DV_D11),
+	GPIO_FN(DV_D10),
+	GPIO_FN(DV_D9),
+	GPIO_FN(DV_D8),
+	GPIO_FN(DV_D7),
+	GPIO_FN(DV_D6),
+	GPIO_FN(DV_D5),
+	GPIO_FN(DV_D4),
+	GPIO_FN(DV_D3),
+	GPIO_FN(DV_D2),
+	GPIO_FN(DV_D1),
+	GPIO_FN(DV_D0),
 
 	/* CPG */
-	PINMUX_GPIO(GPIO_FN_STATUS0, STATUS0_MARK),
-	PINMUX_GPIO(GPIO_FN_PDSTATUS, PDSTATUS_MARK),
+	GPIO_FN(STATUS0),
+	GPIO_FN(PDSTATUS),
 
 	/* SIOF0 */
-	PINMUX_GPIO(GPIO_FN_SIOF0_MCK, SIOF0_MCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF0_SCK, SIOF0_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF0_SYNC, SIOF0_SYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF0_SS1, SIOF0_SS1_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF0_SS2, SIOF0_SS2_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF0_TXD, SIOF0_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF0_RXD, SIOF0_RXD_MARK),
+	GPIO_FN(SIOF0_MCK),
+	GPIO_FN(SIOF0_SCK),
+	GPIO_FN(SIOF0_SYNC),
+	GPIO_FN(SIOF0_SS1),
+	GPIO_FN(SIOF0_SS2),
+	GPIO_FN(SIOF0_TXD),
+	GPIO_FN(SIOF0_RXD),
 
 	/* SIOF1 */
-	PINMUX_GPIO(GPIO_FN_SIOF1_MCK, SIOF1_MCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF1_SCK, SIOF1_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF1_SYNC, SIOF1_SYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF1_SS1, SIOF1_SS1_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF1_SS2, SIOF1_SS2_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF1_TXD, SIOF1_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF1_RXD, SIOF1_RXD_MARK),
+	GPIO_FN(SIOF1_MCK),
+	GPIO_FN(SIOF1_SCK),
+	GPIO_FN(SIOF1_SYNC),
+	GPIO_FN(SIOF1_SS1),
+	GPIO_FN(SIOF1_SS2),
+	GPIO_FN(SIOF1_TXD),
+	GPIO_FN(SIOF1_RXD),
 
 	/* SIM */
-	PINMUX_GPIO(GPIO_FN_SIM_D, SIM_D_MARK),
-	PINMUX_GPIO(GPIO_FN_SIM_CLK, SIM_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SIM_RST, SIM_RST_MARK),
+	GPIO_FN(SIM_D),
+	GPIO_FN(SIM_CLK),
+	GPIO_FN(SIM_RST),
 
 	/* TSIF */
-	PINMUX_GPIO(GPIO_FN_TS_SDAT, TS_SDAT_MARK),
-	PINMUX_GPIO(GPIO_FN_TS_SCK, TS_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_TS_SDEN, TS_SDEN_MARK),
-	PINMUX_GPIO(GPIO_FN_TS_SPSYNC, TS_SPSYNC_MARK),
+	GPIO_FN(TS_SDAT),
+	GPIO_FN(TS_SCK),
+	GPIO_FN(TS_SDEN),
+	GPIO_FN(TS_SPSYNC),
 
 	/* IRDA */
-	PINMUX_GPIO(GPIO_FN_IRDA_IN, IRDA_IN_MARK),
-	PINMUX_GPIO(GPIO_FN_IRDA_OUT, IRDA_OUT_MARK),
+	GPIO_FN(IRDA_IN),
+	GPIO_FN(IRDA_OUT),
 
 	/* TPU */
-	PINMUX_GPIO(GPIO_FN_TPUTO, TPUTO_MARK),
+	GPIO_FN(TPUTO),
 
 	/* FLCTL */
-	PINMUX_GPIO(GPIO_FN_FCE, FCE_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF7, NAF7_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF6, NAF6_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF5, NAF5_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF4, NAF4_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF3, NAF3_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF2, NAF2_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF1, NAF1_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF0, NAF0_MARK),
-	PINMUX_GPIO(GPIO_FN_FCDE, FCDE_MARK),
-	PINMUX_GPIO(GPIO_FN_FOE, FOE_MARK),
-	PINMUX_GPIO(GPIO_FN_FSC, FSC_MARK),
-	PINMUX_GPIO(GPIO_FN_FWE, FWE_MARK),
-	PINMUX_GPIO(GPIO_FN_FRB, FRB_MARK),
+	GPIO_FN(FCE),
+	GPIO_FN(NAF7),
+	GPIO_FN(NAF6),
+	GPIO_FN(NAF5),
+	GPIO_FN(NAF4),
+	GPIO_FN(NAF3),
+	GPIO_FN(NAF2),
+	GPIO_FN(NAF1),
+	GPIO_FN(NAF0),
+	GPIO_FN(FCDE),
+	GPIO_FN(FOE),
+	GPIO_FN(FSC),
+	GPIO_FN(FWE),
+	GPIO_FN(FRB),
 
 	/* KEYSC */
-	PINMUX_GPIO(GPIO_FN_KEYIN0, KEYIN0_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYIN1, KEYIN1_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYIN2, KEYIN2_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYIN3, KEYIN3_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYIN4, KEYIN4_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT0, KEYOUT0_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT1, KEYOUT1_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT2, KEYOUT2_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT3, KEYOUT3_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT4_IN6, KEYOUT4_IN6_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT5_IN5, KEYOUT5_IN5_MARK),
+	GPIO_FN(KEYIN0),
+	GPIO_FN(KEYIN1),
+	GPIO_FN(KEYIN2),
+	GPIO_FN(KEYIN3),
+	GPIO_FN(KEYIN4),
+	GPIO_FN(KEYOUT0),
+	GPIO_FN(KEYOUT1),
+	GPIO_FN(KEYOUT2),
+	GPIO_FN(KEYOUT3),
+	GPIO_FN(KEYOUT4_IN6),
+	GPIO_FN(KEYOUT5_IN5),
 };
 
-static struct pinmux_cfg_reg pinmux_config_regs[] = {
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ PINMUX_CFG_REG("PACR", 0xa4050100, 16, 2) {
 		VIO_D7_SCIF1_SCK, PTA7_OUT, PTA7_IN_PD, PTA7_IN,
 		VIO_D6_SCIF1_RXD, 0, PTA6_IN_PD, PTA6_IN,
@@ -1660,7 +1664,7 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{}
 };
 
-static struct pinmux_data_reg pinmux_data_regs[] = {
+static const struct pinmux_data_reg pinmux_data_regs[] = {
 	{ PINMUX_DATA_REG("PADR", 0xa4050120, 8) {
 		PTA7_DATA, PTA6_DATA, PTA5_DATA, PTA4_DATA,
 		PTA3_DATA, PTA2_DATA, PTA1_DATA, PTA0_DATA }
@@ -1756,21 +1760,19 @@ static struct pinmux_data_reg pinmux_data_regs[] = {
 	{ },
 };
 
-struct sh_pfc_soc_info sh7722_pinmux_info = {
+const struct sh_pfc_soc_info sh7722_pinmux_info = {
 	.name = "sh7722_pfc",
-	.reserved_id = PINMUX_RESERVED,
-	.data = { PINMUX_DATA_BEGIN, PINMUX_DATA_END },
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END },
 	.input_pd = { PINMUX_INPUT_PULLDOWN_BEGIN, PINMUX_INPUT_PULLDOWN_END },
 	.input_pu = { PINMUX_INPUT_PULLUP_BEGIN, PINMUX_INPUT_PULLUP_END },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END },
-	.mark = { PINMUX_MARK_BEGIN, PINMUX_MARK_END },
 	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
 
-	.first_gpio = GPIO_PTA7,
-	.last_gpio = GPIO_FN_KEYOUT5_IN5,
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.func_gpios = pinmux_func_gpios,
+	.nr_func_gpios = ARRAY_SIZE(pinmux_func_gpios),
 
-	.gpios = pinmux_gpios,
 	.cfg_regs = pinmux_config_regs,
 	.data_regs = pinmux_data_regs,
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh7723.c b/drivers/pinctrl/sh-pfc/pfc-sh7723.c
index 609673d3d70e..07ad1d8d6c8b 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh7723.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh7723.c
@@ -350,7 +350,7 @@ enum {
 	PINMUX_MARK_END,
 };
 
-static pinmux_enum_t pinmux_data[] = {
+static const pinmux_enum_t pinmux_data[] = {
 	/* PTA GPIO */
 	PINMUX_DATA(PTA7_DATA, PTA7_IN, PTA7_OUT),
 	PINMUX_DATA(PTA6_DATA, PTA6_IN, PTA6_OUT),
@@ -923,7 +923,7 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA(SIUBISLD_MARK, PSD1_PSD0_FN2, PTZ0_FN),
 };
 
-static struct pinmux_gpio pinmux_gpios[] = {
+static struct sh_pfc_pin pinmux_pins[] = {
 	/* PTA */
 	PINMUX_GPIO(GPIO_PTA7, PTA7_DATA),
 	PINMUX_GPIO(GPIO_PTA6, PTA6_DATA),
@@ -1139,379 +1139,383 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	PINMUX_GPIO(GPIO_PTZ2, PTZ2_DATA),
 	PINMUX_GPIO(GPIO_PTZ1, PTZ1_DATA),
 	PINMUX_GPIO(GPIO_PTZ0, PTZ0_DATA),
+};
+
+#define PINMUX_FN_BASE	ARRAY_SIZE(pinmux_pins)
 
+static const struct pinmux_func pinmux_func_gpios[] = {
 	/* SCIF0 */
-	PINMUX_GPIO(GPIO_FN_SCIF0_PTT_TXD, SCIF0_PTT_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_PTT_RXD, SCIF0_PTT_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_PTT_SCK, SCIF0_PTT_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_PTU_TXD, SCIF0_PTU_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_PTU_RXD, SCIF0_PTU_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_PTU_SCK, SCIF0_PTU_SCK_MARK),
+	GPIO_FN(SCIF0_PTT_TXD),
+	GPIO_FN(SCIF0_PTT_RXD),
+	GPIO_FN(SCIF0_PTT_SCK),
+	GPIO_FN(SCIF0_PTU_TXD),
+	GPIO_FN(SCIF0_PTU_RXD),
+	GPIO_FN(SCIF0_PTU_SCK),
 
 	/* SCIF1 */
-	PINMUX_GPIO(GPIO_FN_SCIF1_PTS_TXD, SCIF1_PTS_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_PTS_RXD, SCIF1_PTS_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_PTS_SCK, SCIF1_PTS_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_PTV_TXD, SCIF1_PTV_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_PTV_RXD, SCIF1_PTV_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_PTV_SCK, SCIF1_PTV_SCK_MARK),
+	GPIO_FN(SCIF1_PTS_TXD),
+	GPIO_FN(SCIF1_PTS_RXD),
+	GPIO_FN(SCIF1_PTS_SCK),
+	GPIO_FN(SCIF1_PTV_TXD),
+	GPIO_FN(SCIF1_PTV_RXD),
+	GPIO_FN(SCIF1_PTV_SCK),
 
 	/* SCIF2 */
-	PINMUX_GPIO(GPIO_FN_SCIF2_PTT_TXD, SCIF2_PTT_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_PTT_RXD, SCIF2_PTT_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_PTT_SCK, SCIF2_PTT_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_PTU_TXD, SCIF2_PTU_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_PTU_RXD, SCIF2_PTU_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_PTU_SCK, SCIF2_PTU_SCK_MARK),
+	GPIO_FN(SCIF2_PTT_TXD),
+	GPIO_FN(SCIF2_PTT_RXD),
+	GPIO_FN(SCIF2_PTT_SCK),
+	GPIO_FN(SCIF2_PTU_TXD),
+	GPIO_FN(SCIF2_PTU_RXD),
+	GPIO_FN(SCIF2_PTU_SCK),
 
 	/* SCIF3 */
-	PINMUX_GPIO(GPIO_FN_SCIF3_PTS_TXD, SCIF3_PTS_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_PTS_RXD, SCIF3_PTS_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_PTS_SCK, SCIF3_PTS_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_PTS_RTS, SCIF3_PTS_RTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_PTS_CTS, SCIF3_PTS_CTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_PTV_TXD, SCIF3_PTV_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_PTV_RXD, SCIF3_PTV_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_PTV_SCK, SCIF3_PTV_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_PTV_RTS, SCIF3_PTV_RTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_PTV_CTS, SCIF3_PTV_CTS_MARK),
+	GPIO_FN(SCIF3_PTS_TXD),
+	GPIO_FN(SCIF3_PTS_RXD),
+	GPIO_FN(SCIF3_PTS_SCK),
+	GPIO_FN(SCIF3_PTS_RTS),
+	GPIO_FN(SCIF3_PTS_CTS),
+	GPIO_FN(SCIF3_PTV_TXD),
+	GPIO_FN(SCIF3_PTV_RXD),
+	GPIO_FN(SCIF3_PTV_SCK),
+	GPIO_FN(SCIF3_PTV_RTS),
+	GPIO_FN(SCIF3_PTV_CTS),
 
 	/* SCIF4 */
-	PINMUX_GPIO(GPIO_FN_SCIF4_PTE_TXD, SCIF4_PTE_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF4_PTE_RXD, SCIF4_PTE_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF4_PTE_SCK, SCIF4_PTE_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF4_PTN_TXD, SCIF4_PTN_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF4_PTN_RXD, SCIF4_PTN_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF4_PTN_SCK, SCIF4_PTN_SCK_MARK),
+	GPIO_FN(SCIF4_PTE_TXD),
+	GPIO_FN(SCIF4_PTE_RXD),
+	GPIO_FN(SCIF4_PTE_SCK),
+	GPIO_FN(SCIF4_PTN_TXD),
+	GPIO_FN(SCIF4_PTN_RXD),
+	GPIO_FN(SCIF4_PTN_SCK),
 
 	/* SCIF5 */
-	PINMUX_GPIO(GPIO_FN_SCIF5_PTE_TXD, SCIF5_PTE_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF5_PTE_RXD, SCIF5_PTE_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF5_PTE_SCK, SCIF5_PTE_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF5_PTN_TXD, SCIF5_PTN_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF5_PTN_RXD, SCIF5_PTN_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF5_PTN_SCK, SCIF5_PTN_SCK_MARK),
+	GPIO_FN(SCIF5_PTE_TXD),
+	GPIO_FN(SCIF5_PTE_RXD),
+	GPIO_FN(SCIF5_PTE_SCK),
+	GPIO_FN(SCIF5_PTN_TXD),
+	GPIO_FN(SCIF5_PTN_RXD),
+	GPIO_FN(SCIF5_PTN_SCK),
 
 	/* CEU */
-	PINMUX_GPIO(GPIO_FN_VIO_D15, VIO_D15_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D14, VIO_D14_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D13, VIO_D13_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D12, VIO_D12_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D11, VIO_D11_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D10, VIO_D10_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D9, VIO_D9_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D8, VIO_D8_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D7, VIO_D7_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D6, VIO_D6_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D5, VIO_D5_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D4, VIO_D4_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D3, VIO_D3_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D2, VIO_D2_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D1, VIO_D1_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_D0, VIO_D0_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_CLK1, VIO_CLK1_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_VD1, VIO_VD1_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_HD1, VIO_HD1_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_FLD, VIO_FLD_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_CKO, VIO_CKO_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_VD2, VIO_VD2_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_HD2, VIO_HD2_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO_CLK2, VIO_CLK2_MARK),
+	GPIO_FN(VIO_D15),
+	GPIO_FN(VIO_D14),
+	GPIO_FN(VIO_D13),
+	GPIO_FN(VIO_D12),
+	GPIO_FN(VIO_D11),
+	GPIO_FN(VIO_D10),
+	GPIO_FN(VIO_D9),
+	GPIO_FN(VIO_D8),
+	GPIO_FN(VIO_D7),
+	GPIO_FN(VIO_D6),
+	GPIO_FN(VIO_D5),
+	GPIO_FN(VIO_D4),
+	GPIO_FN(VIO_D3),
+	GPIO_FN(VIO_D2),
+	GPIO_FN(VIO_D1),
+	GPIO_FN(VIO_D0),
+	GPIO_FN(VIO_CLK1),
+	GPIO_FN(VIO_VD1),
+	GPIO_FN(VIO_HD1),
+	GPIO_FN(VIO_FLD),
+	GPIO_FN(VIO_CKO),
+	GPIO_FN(VIO_VD2),
+	GPIO_FN(VIO_HD2),
+	GPIO_FN(VIO_CLK2),
 
 	/* LCDC */
-	PINMUX_GPIO(GPIO_FN_LCDD23, LCDD23_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD22, LCDD22_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD21, LCDD21_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD20, LCDD20_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD19, LCDD19_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD18, LCDD18_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD17, LCDD17_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD16, LCDD16_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD15, LCDD15_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD14, LCDD14_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD13, LCDD13_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD12, LCDD12_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD11, LCDD11_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD10, LCDD10_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD9, LCDD9_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD8, LCDD8_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD7, LCDD7_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD6, LCDD6_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD5, LCDD5_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD4, LCDD4_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD3, LCDD3_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD2, LCDD2_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD1, LCDD1_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD0, LCDD0_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDLCLK_PTR, LCDLCLK_PTR_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDLCLK_PTW, LCDLCLK_PTW_MARK),
+	GPIO_FN(LCDD23),
+	GPIO_FN(LCDD22),
+	GPIO_FN(LCDD21),
+	GPIO_FN(LCDD20),
+	GPIO_FN(LCDD19),
+	GPIO_FN(LCDD18),
+	GPIO_FN(LCDD17),
+	GPIO_FN(LCDD16),
+	GPIO_FN(LCDD15),
+	GPIO_FN(LCDD14),
+	GPIO_FN(LCDD13),
+	GPIO_FN(LCDD12),
+	GPIO_FN(LCDD11),
+	GPIO_FN(LCDD10),
+	GPIO_FN(LCDD9),
+	GPIO_FN(LCDD8),
+	GPIO_FN(LCDD7),
+	GPIO_FN(LCDD6),
+	GPIO_FN(LCDD5),
+	GPIO_FN(LCDD4),
+	GPIO_FN(LCDD3),
+	GPIO_FN(LCDD2),
+	GPIO_FN(LCDD1),
+	GPIO_FN(LCDD0),
+	GPIO_FN(LCDLCLK_PTR),
+	GPIO_FN(LCDLCLK_PTW),
 	/* Main LCD */
-	PINMUX_GPIO(GPIO_FN_LCDDON, LCDDON_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDVCPWC, LCDVCPWC_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDVEPWC, LCDVEPWC_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDVSYN, LCDVSYN_MARK),
+	GPIO_FN(LCDDON),
+	GPIO_FN(LCDVCPWC),
+	GPIO_FN(LCDVEPWC),
+	GPIO_FN(LCDVSYN),
 	/* Main LCD - RGB Mode */
-	PINMUX_GPIO(GPIO_FN_LCDDCK, LCDDCK_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDHSYN, LCDHSYN_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDDISP, LCDDISP_MARK),
+	GPIO_FN(LCDDCK),
+	GPIO_FN(LCDHSYN),
+	GPIO_FN(LCDDISP),
 	/* Main LCD - SYS Mode */
-	PINMUX_GPIO(GPIO_FN_LCDRS, LCDRS_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDCS, LCDCS_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDWR, LCDWR_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDRD, LCDRD_MARK),
+	GPIO_FN(LCDRS),
+	GPIO_FN(LCDCS),
+	GPIO_FN(LCDWR),
+	GPIO_FN(LCDRD),
 
 	/* IRQ */
-	PINMUX_GPIO(GPIO_FN_IRQ0, IRQ0_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ1, IRQ1_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ2, IRQ2_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ3, IRQ3_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ4, IRQ4_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ5, IRQ5_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ6, IRQ6_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ7, IRQ7_MARK),
+	GPIO_FN(IRQ0),
+	GPIO_FN(IRQ1),
+	GPIO_FN(IRQ2),
+	GPIO_FN(IRQ3),
+	GPIO_FN(IRQ4),
+	GPIO_FN(IRQ5),
+	GPIO_FN(IRQ6),
+	GPIO_FN(IRQ7),
 
 	/* AUD */
-	PINMUX_GPIO(GPIO_FN_AUDCK, AUDCK_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDSYNC, AUDSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA3, AUDATA3_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA2, AUDATA2_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA1, AUDATA1_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA0, AUDATA0_MARK),
+	GPIO_FN(AUDCK),
+	GPIO_FN(AUDSYNC),
+	GPIO_FN(AUDATA3),
+	GPIO_FN(AUDATA2),
+	GPIO_FN(AUDATA1),
+	GPIO_FN(AUDATA0),
 
 	/* SDHI0 (PTD) */
-	PINMUX_GPIO(GPIO_FN_SDHI0CD_PTD, SDHI0CD_PTD_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0WP_PTD, SDHI0WP_PTD_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0D3_PTD, SDHI0D3_PTD_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0D2_PTD, SDHI0D2_PTD_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0D1_PTD, SDHI0D1_PTD_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0D0_PTD, SDHI0D0_PTD_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0CMD_PTD, SDHI0CMD_PTD_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0CLK_PTD, SDHI0CLK_PTD_MARK),
+	GPIO_FN(SDHI0CD_PTD),
+	GPIO_FN(SDHI0WP_PTD),
+	GPIO_FN(SDHI0D3_PTD),
+	GPIO_FN(SDHI0D2_PTD),
+	GPIO_FN(SDHI0D1_PTD),
+	GPIO_FN(SDHI0D0_PTD),
+	GPIO_FN(SDHI0CMD_PTD),
+	GPIO_FN(SDHI0CLK_PTD),
 
 	/* SDHI0 (PTS) */
-	PINMUX_GPIO(GPIO_FN_SDHI0CD_PTS, SDHI0CD_PTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0WP_PTS, SDHI0WP_PTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0D3_PTS, SDHI0D3_PTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0D2_PTS, SDHI0D2_PTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0D1_PTS, SDHI0D1_PTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0D0_PTS, SDHI0D0_PTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0CMD_PTS, SDHI0CMD_PTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0CLK_PTS, SDHI0CLK_PTS_MARK),
+	GPIO_FN(SDHI0CD_PTS),
+	GPIO_FN(SDHI0WP_PTS),
+	GPIO_FN(SDHI0D3_PTS),
+	GPIO_FN(SDHI0D2_PTS),
+	GPIO_FN(SDHI0D1_PTS),
+	GPIO_FN(SDHI0D0_PTS),
+	GPIO_FN(SDHI0CMD_PTS),
+	GPIO_FN(SDHI0CLK_PTS),
 
 	/* SDHI1 */
-	PINMUX_GPIO(GPIO_FN_SDHI1CD, SDHI1CD_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI1WP, SDHI1WP_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI1D3, SDHI1D3_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI1D2, SDHI1D2_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI1D1, SDHI1D1_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI1D0, SDHI1D0_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI1CMD, SDHI1CMD_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI1CLK, SDHI1CLK_MARK),
+	GPIO_FN(SDHI1CD),
+	GPIO_FN(SDHI1WP),
+	GPIO_FN(SDHI1D3),
+	GPIO_FN(SDHI1D2),
+	GPIO_FN(SDHI1D1),
+	GPIO_FN(SDHI1D0),
+	GPIO_FN(SDHI1CMD),
+	GPIO_FN(SDHI1CLK),
 
 	/* SIUA */
-	PINMUX_GPIO(GPIO_FN_SIUAFCK, SIUAFCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUAILR, SIUAILR_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUAIBT, SIUAIBT_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUAISLD, SIUAISLD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUAOLR, SIUAOLR_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUAOBT, SIUAOBT_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUAOSLD, SIUAOSLD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUAMCK, SIUAMCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUAISPD, SIUAISPD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUAOSPD, SIUAOSPD_MARK),
+	GPIO_FN(SIUAFCK),
+	GPIO_FN(SIUAILR),
+	GPIO_FN(SIUAIBT),
+	GPIO_FN(SIUAISLD),
+	GPIO_FN(SIUAOLR),
+	GPIO_FN(SIUAOBT),
+	GPIO_FN(SIUAOSLD),
+	GPIO_FN(SIUAMCK),
+	GPIO_FN(SIUAISPD),
+	GPIO_FN(SIUAOSPD),
 
 	/* SIUB */
-	PINMUX_GPIO(GPIO_FN_SIUBFCK, SIUBFCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUBILR, SIUBILR_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUBIBT, SIUBIBT_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUBISLD, SIUBISLD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUBOLR, SIUBOLR_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUBOBT, SIUBOBT_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUBOSLD, SIUBOSLD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIUBMCK, SIUBMCK_MARK),
+	GPIO_FN(SIUBFCK),
+	GPIO_FN(SIUBILR),
+	GPIO_FN(SIUBIBT),
+	GPIO_FN(SIUBISLD),
+	GPIO_FN(SIUBOLR),
+	GPIO_FN(SIUBOBT),
+	GPIO_FN(SIUBOSLD),
+	GPIO_FN(SIUBMCK),
 
 	/* IRDA */
-	PINMUX_GPIO(GPIO_FN_IRDA_IN, IRDA_IN_MARK),
-	PINMUX_GPIO(GPIO_FN_IRDA_OUT, IRDA_OUT_MARK),
+	GPIO_FN(IRDA_IN),
+	GPIO_FN(IRDA_OUT),
 
 	/* VOU */
-	PINMUX_GPIO(GPIO_FN_DV_CLKI, DV_CLKI_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_CLK, DV_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_HSYNC, DV_HSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_VSYNC, DV_VSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D15, DV_D15_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D14, DV_D14_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D13, DV_D13_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D12, DV_D12_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D11, DV_D11_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D10, DV_D10_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D9, DV_D9_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D8, DV_D8_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D7, DV_D7_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D6, DV_D6_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D5, DV_D5_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D4, DV_D4_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D3, DV_D3_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D2, DV_D2_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D1, DV_D1_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D0, DV_D0_MARK),
+	GPIO_FN(DV_CLKI),
+	GPIO_FN(DV_CLK),
+	GPIO_FN(DV_HSYNC),
+	GPIO_FN(DV_VSYNC),
+	GPIO_FN(DV_D15),
+	GPIO_FN(DV_D14),
+	GPIO_FN(DV_D13),
+	GPIO_FN(DV_D12),
+	GPIO_FN(DV_D11),
+	GPIO_FN(DV_D10),
+	GPIO_FN(DV_D9),
+	GPIO_FN(DV_D8),
+	GPIO_FN(DV_D7),
+	GPIO_FN(DV_D6),
+	GPIO_FN(DV_D5),
+	GPIO_FN(DV_D4),
+	GPIO_FN(DV_D3),
+	GPIO_FN(DV_D2),
+	GPIO_FN(DV_D1),
+	GPIO_FN(DV_D0),
 
 	/* KEYSC */
-	PINMUX_GPIO(GPIO_FN_KEYIN0, KEYIN0_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYIN1, KEYIN1_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYIN2, KEYIN2_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYIN3, KEYIN3_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYIN4, KEYIN4_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT0, KEYOUT0_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT1, KEYOUT1_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT2, KEYOUT2_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT3, KEYOUT3_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT4_IN6, KEYOUT4_IN6_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT5_IN5, KEYOUT5_IN5_MARK),
+	GPIO_FN(KEYIN0),
+	GPIO_FN(KEYIN1),
+	GPIO_FN(KEYIN2),
+	GPIO_FN(KEYIN3),
+	GPIO_FN(KEYIN4),
+	GPIO_FN(KEYOUT0),
+	GPIO_FN(KEYOUT1),
+	GPIO_FN(KEYOUT2),
+	GPIO_FN(KEYOUT3),
+	GPIO_FN(KEYOUT4_IN6),
+	GPIO_FN(KEYOUT5_IN5),
 
 	/* MSIOF0 (PTF) */
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTF_TXD, MSIOF0_PTF_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTF_RXD, MSIOF0_PTF_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTF_MCK, MSIOF0_PTF_MCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTF_TSYNC, MSIOF0_PTF_TSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTF_TSCK, MSIOF0_PTF_TSCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTF_RSYNC, MSIOF0_PTF_RSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTF_RSCK, MSIOF0_PTF_RSCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTF_SS1, MSIOF0_PTF_SS1_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTF_SS2, MSIOF0_PTF_SS2_MARK),
+	GPIO_FN(MSIOF0_PTF_TXD),
+	GPIO_FN(MSIOF0_PTF_RXD),
+	GPIO_FN(MSIOF0_PTF_MCK),
+	GPIO_FN(MSIOF0_PTF_TSYNC),
+	GPIO_FN(MSIOF0_PTF_TSCK),
+	GPIO_FN(MSIOF0_PTF_RSYNC),
+	GPIO_FN(MSIOF0_PTF_RSCK),
+	GPIO_FN(MSIOF0_PTF_SS1),
+	GPIO_FN(MSIOF0_PTF_SS2),
 
 	/* MSIOF0 (PTT+PTX) */
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTT_TXD, MSIOF0_PTT_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTT_RXD, MSIOF0_PTT_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTX_MCK, MSIOF0_PTX_MCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTT_TSYNC, MSIOF0_PTT_TSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTT_TSCK, MSIOF0_PTT_TSCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTT_RSYNC, MSIOF0_PTT_RSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTT_RSCK, MSIOF0_PTT_RSCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTT_SS1, MSIOF0_PTT_SS1_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_PTT_SS2, MSIOF0_PTT_SS2_MARK),
+	GPIO_FN(MSIOF0_PTT_TXD),
+	GPIO_FN(MSIOF0_PTT_RXD),
+	GPIO_FN(MSIOF0_PTX_MCK),
+	GPIO_FN(MSIOF0_PTT_TSYNC),
+	GPIO_FN(MSIOF0_PTT_TSCK),
+	GPIO_FN(MSIOF0_PTT_RSYNC),
+	GPIO_FN(MSIOF0_PTT_RSCK),
+	GPIO_FN(MSIOF0_PTT_SS1),
+	GPIO_FN(MSIOF0_PTT_SS2),
 
 	/* MSIOF1 */
-	PINMUX_GPIO(GPIO_FN_MSIOF1_TXD, MSIOF1_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_RXD, MSIOF1_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_MCK, MSIOF1_MCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_TSYNC, MSIOF1_TSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_TSCK, MSIOF1_TSCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_RSYNC, MSIOF1_RSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_RSCK, MSIOF1_RSCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_SS1, MSIOF1_SS1_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_SS2, MSIOF1_SS2_MARK),
+	GPIO_FN(MSIOF1_TXD),
+	GPIO_FN(MSIOF1_RXD),
+	GPIO_FN(MSIOF1_MCK),
+	GPIO_FN(MSIOF1_TSYNC),
+	GPIO_FN(MSIOF1_TSCK),
+	GPIO_FN(MSIOF1_RSYNC),
+	GPIO_FN(MSIOF1_RSCK),
+	GPIO_FN(MSIOF1_SS1),
+	GPIO_FN(MSIOF1_SS2),
 
 	/* TSIF */
-	PINMUX_GPIO(GPIO_FN_TS0_SDAT, TS0_SDAT_MARK),
-	PINMUX_GPIO(GPIO_FN_TS0_SCK, TS0_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_TS0_SDEN, TS0_SDEN_MARK),
-	PINMUX_GPIO(GPIO_FN_TS0_SPSYNC, TS0_SPSYNC_MARK),
+	GPIO_FN(TS0_SDAT),
+	GPIO_FN(TS0_SCK),
+	GPIO_FN(TS0_SDEN),
+	GPIO_FN(TS0_SPSYNC),
 
 	/* FLCTL */
-	PINMUX_GPIO(GPIO_FN_FCE, FCE_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF7, NAF7_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF6, NAF6_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF5, NAF5_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF4, NAF4_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF3, NAF3_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF2, NAF2_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF1, NAF1_MARK),
-	PINMUX_GPIO(GPIO_FN_NAF0, NAF0_MARK),
-	PINMUX_GPIO(GPIO_FN_FCDE, FCDE_MARK),
-	PINMUX_GPIO(GPIO_FN_FOE, FOE_MARK),
-	PINMUX_GPIO(GPIO_FN_FSC, FSC_MARK),
-	PINMUX_GPIO(GPIO_FN_FWE, FWE_MARK),
-	PINMUX_GPIO(GPIO_FN_FRB, FRB_MARK),
+	GPIO_FN(FCE),
+	GPIO_FN(NAF7),
+	GPIO_FN(NAF6),
+	GPIO_FN(NAF5),
+	GPIO_FN(NAF4),
+	GPIO_FN(NAF3),
+	GPIO_FN(NAF2),
+	GPIO_FN(NAF1),
+	GPIO_FN(NAF0),
+	GPIO_FN(FCDE),
+	GPIO_FN(FOE),
+	GPIO_FN(FSC),
+	GPIO_FN(FWE),
+	GPIO_FN(FRB),
 
 	/* DMAC */
-	PINMUX_GPIO(GPIO_FN_DACK1, DACK1_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ1, DREQ1_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK0, DACK0_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ0, DREQ0_MARK),
+	GPIO_FN(DACK1),
+	GPIO_FN(DREQ1),
+	GPIO_FN(DACK0),
+	GPIO_FN(DREQ0),
 
 	/* ADC */
-	PINMUX_GPIO(GPIO_FN_AN3, AN3_MARK),
-	PINMUX_GPIO(GPIO_FN_AN2, AN2_MARK),
-	PINMUX_GPIO(GPIO_FN_AN1, AN1_MARK),
-	PINMUX_GPIO(GPIO_FN_AN0, AN0_MARK),
-	PINMUX_GPIO(GPIO_FN_ADTRG, ADTRG_MARK),
+	GPIO_FN(AN3),
+	GPIO_FN(AN2),
+	GPIO_FN(AN1),
+	GPIO_FN(AN0),
+	GPIO_FN(ADTRG),
 
 	/* CPG */
-	PINMUX_GPIO(GPIO_FN_STATUS0, STATUS0_MARK),
-	PINMUX_GPIO(GPIO_FN_PDSTATUS, PDSTATUS_MARK),
+	GPIO_FN(STATUS0),
+	GPIO_FN(PDSTATUS),
 
 	/* TPU */
-	PINMUX_GPIO(GPIO_FN_TPUTO0, TPUTO0_MARK),
-	PINMUX_GPIO(GPIO_FN_TPUTO1, TPUTO1_MARK),
-	PINMUX_GPIO(GPIO_FN_TPUTO2, TPUTO2_MARK),
-	PINMUX_GPIO(GPIO_FN_TPUTO3, TPUTO3_MARK),
+	GPIO_FN(TPUTO0),
+	GPIO_FN(TPUTO1),
+	GPIO_FN(TPUTO2),
+	GPIO_FN(TPUTO3),
 
 	/* BSC */
-	PINMUX_GPIO(GPIO_FN_D31, D31_MARK),
-	PINMUX_GPIO(GPIO_FN_D30, D30_MARK),
-	PINMUX_GPIO(GPIO_FN_D29, D29_MARK),
-	PINMUX_GPIO(GPIO_FN_D28, D28_MARK),
-	PINMUX_GPIO(GPIO_FN_D27, D27_MARK),
-	PINMUX_GPIO(GPIO_FN_D26, D26_MARK),
-	PINMUX_GPIO(GPIO_FN_D25, D25_MARK),
-	PINMUX_GPIO(GPIO_FN_D24, D24_MARK),
-	PINMUX_GPIO(GPIO_FN_D23, D23_MARK),
-	PINMUX_GPIO(GPIO_FN_D22, D22_MARK),
-	PINMUX_GPIO(GPIO_FN_D21, D21_MARK),
-	PINMUX_GPIO(GPIO_FN_D20, D20_MARK),
-	PINMUX_GPIO(GPIO_FN_D19, D19_MARK),
-	PINMUX_GPIO(GPIO_FN_D18, D18_MARK),
-	PINMUX_GPIO(GPIO_FN_D17, D17_MARK),
-	PINMUX_GPIO(GPIO_FN_D16, D16_MARK),
-	PINMUX_GPIO(GPIO_FN_IOIS16, IOIS16_MARK),
-	PINMUX_GPIO(GPIO_FN_WAIT, WAIT_MARK),
-	PINMUX_GPIO(GPIO_FN_BS, BS_MARK),
-	PINMUX_GPIO(GPIO_FN_A25, A25_MARK),
-	PINMUX_GPIO(GPIO_FN_A24, A24_MARK),
-	PINMUX_GPIO(GPIO_FN_A23, A23_MARK),
-	PINMUX_GPIO(GPIO_FN_A22, A22_MARK),
-	PINMUX_GPIO(GPIO_FN_CS6B_CE1B, CS6B_CE1B_MARK),
-	PINMUX_GPIO(GPIO_FN_CS6A_CE2B, CS6A_CE2B_MARK),
-	PINMUX_GPIO(GPIO_FN_CS5B_CE1A, CS5B_CE1A_MARK),
-	PINMUX_GPIO(GPIO_FN_CS5A_CE2A, CS5A_CE2A_MARK),
-	PINMUX_GPIO(GPIO_FN_WE3_ICIOWR, WE3_ICIOWR_MARK),
-	PINMUX_GPIO(GPIO_FN_WE2_ICIORD, WE2_ICIORD_MARK),
+	GPIO_FN(D31),
+	GPIO_FN(D30),
+	GPIO_FN(D29),
+	GPIO_FN(D28),
+	GPIO_FN(D27),
+	GPIO_FN(D26),
+	GPIO_FN(D25),
+	GPIO_FN(D24),
+	GPIO_FN(D23),
+	GPIO_FN(D22),
+	GPIO_FN(D21),
+	GPIO_FN(D20),
+	GPIO_FN(D19),
+	GPIO_FN(D18),
+	GPIO_FN(D17),
+	GPIO_FN(D16),
+	GPIO_FN(IOIS16),
+	GPIO_FN(WAIT),
+	GPIO_FN(BS),
+	GPIO_FN(A25),
+	GPIO_FN(A24),
+	GPIO_FN(A23),
+	GPIO_FN(A22),
+	GPIO_FN(CS6B_CE1B),
+	GPIO_FN(CS6A_CE2B),
+	GPIO_FN(CS5B_CE1A),
+	GPIO_FN(CS5A_CE2A),
+	GPIO_FN(WE3_ICIOWR),
+	GPIO_FN(WE2_ICIORD),
 
 	/* ATAPI */
-	PINMUX_GPIO(GPIO_FN_IDED15, IDED15_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED14, IDED14_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED13, IDED13_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED12, IDED12_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED11, IDED11_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED10, IDED10_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED9, IDED9_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED8, IDED8_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED7, IDED7_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED6, IDED6_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED5, IDED5_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED4, IDED4_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED3, IDED3_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED2, IDED2_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED1, IDED1_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED0, IDED0_MARK),
-	PINMUX_GPIO(GPIO_FN_DIRECTION, DIRECTION_MARK),
-	PINMUX_GPIO(GPIO_FN_EXBUF_ENB, EXBUF_ENB_MARK),
-	PINMUX_GPIO(GPIO_FN_IDERST, IDERST_MARK),
-	PINMUX_GPIO(GPIO_FN_IODACK, IODACK_MARK),
-	PINMUX_GPIO(GPIO_FN_IODREQ, IODREQ_MARK),
-	PINMUX_GPIO(GPIO_FN_IDEIORDY, IDEIORDY_MARK),
-	PINMUX_GPIO(GPIO_FN_IDEINT, IDEINT_MARK),
-	PINMUX_GPIO(GPIO_FN_IDEIOWR, IDEIOWR_MARK),
-	PINMUX_GPIO(GPIO_FN_IDEIORD, IDEIORD_MARK),
-	PINMUX_GPIO(GPIO_FN_IDECS1, IDECS1_MARK),
-	PINMUX_GPIO(GPIO_FN_IDECS0, IDECS0_MARK),
-	PINMUX_GPIO(GPIO_FN_IDEA2, IDEA2_MARK),
-	PINMUX_GPIO(GPIO_FN_IDEA1, IDEA1_MARK),
-	PINMUX_GPIO(GPIO_FN_IDEA0, IDEA0_MARK),
+	GPIO_FN(IDED15),
+	GPIO_FN(IDED14),
+	GPIO_FN(IDED13),
+	GPIO_FN(IDED12),
+	GPIO_FN(IDED11),
+	GPIO_FN(IDED10),
+	GPIO_FN(IDED9),
+	GPIO_FN(IDED8),
+	GPIO_FN(IDED7),
+	GPIO_FN(IDED6),
+	GPIO_FN(IDED5),
+	GPIO_FN(IDED4),
+	GPIO_FN(IDED3),
+	GPIO_FN(IDED2),
+	GPIO_FN(IDED1),
+	GPIO_FN(IDED0),
+	GPIO_FN(DIRECTION),
+	GPIO_FN(EXBUF_ENB),
+	GPIO_FN(IDERST),
+	GPIO_FN(IODACK),
+	GPIO_FN(IODREQ),
+	GPIO_FN(IDEIORDY),
+	GPIO_FN(IDEINT),
+	GPIO_FN(IDEIOWR),
+	GPIO_FN(IDEIORD),
+	GPIO_FN(IDECS1),
+	GPIO_FN(IDECS0),
+	GPIO_FN(IDEA2),
+	GPIO_FN(IDEA1),
+	GPIO_FN(IDEA0),
 };
 
-static struct pinmux_cfg_reg pinmux_config_regs[] = {
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ PINMUX_CFG_REG("PACR", 0xa4050100, 16, 2) {
 		PTA7_FN, PTA7_OUT, 0, PTA7_IN,
 		PTA6_FN, PTA6_OUT, 0, PTA6_IN,
@@ -1785,7 +1789,7 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{}
 };
 
-static struct pinmux_data_reg pinmux_data_regs[] = {
+static const struct pinmux_data_reg pinmux_data_regs[] = {
 	{ PINMUX_DATA_REG("PADR", 0xa4050120, 8) {
 		PTA7_DATA, PTA6_DATA, PTA5_DATA, PTA4_DATA,
 		PTA3_DATA, PTA2_DATA, PTA1_DATA, PTA0_DATA }
@@ -1881,20 +1885,18 @@ static struct pinmux_data_reg pinmux_data_regs[] = {
 	{ },
 };
 
-struct sh_pfc_soc_info sh7723_pinmux_info = {
+const struct sh_pfc_soc_info sh7723_pinmux_info = {
 	.name = "sh7723_pfc",
-	.reserved_id = PINMUX_RESERVED,
-	.data = { PINMUX_DATA_BEGIN, PINMUX_DATA_END },
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END },
 	.input_pu = { PINMUX_INPUT_PULLUP_BEGIN, PINMUX_INPUT_PULLUP_END },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END },
-	.mark = { PINMUX_MARK_BEGIN, PINMUX_MARK_END },
 	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
 
-	.first_gpio = GPIO_PTA7,
-	.last_gpio = GPIO_FN_IDEA0,
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.func_gpios = pinmux_func_gpios,
+	.nr_func_gpios = ARRAY_SIZE(pinmux_func_gpios),
 
-	.gpios = pinmux_gpios,
 	.cfg_regs = pinmux_config_regs,
 	.data_regs = pinmux_data_regs,
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh7724.c b/drivers/pinctrl/sh-pfc/pfc-sh7724.c
index 233fbf750b39..35e551609805 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh7724.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh7724.c
@@ -572,7 +572,7 @@ enum {
 	PINMUX_MARK_END,
 };
 
-static pinmux_enum_t pinmux_data[] = {
+static const pinmux_enum_t pinmux_data[] = {
 	/* PTA GPIO */
 	PINMUX_DATA(PTA7_DATA, PTA7_IN, PTA7_OUT, PTA7_IN_PU),
 	PINMUX_DATA(PTA6_DATA, PTA6_IN, PTA6_OUT, PTA6_IN_PU),
@@ -1192,7 +1192,7 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA(SCIF3_I_TXD_MARK,	PSB14_1, PTZ3_FN),
 };
 
-static struct pinmux_gpio pinmux_gpios[] = {
+static struct sh_pfc_pin pinmux_pins[] = {
 	/* PTA */
 	PINMUX_GPIO(GPIO_PTA7, PTA7_DATA),
 	PINMUX_GPIO(GPIO_PTA6, PTA6_DATA),
@@ -1418,372 +1418,376 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	PINMUX_GPIO(GPIO_PTZ2, PTZ2_DATA),
 	PINMUX_GPIO(GPIO_PTZ1, PTZ1_DATA),
 	PINMUX_GPIO(GPIO_PTZ0, PTZ0_DATA),
+};
+
+#define PINMUX_FN_BASE	ARRAY_SIZE(pinmux_pins)
 
+static const struct pinmux_func pinmux_func_gpios[] = {
 	/* BSC */
-	PINMUX_GPIO(GPIO_FN_D31,	D31_MARK),
-	PINMUX_GPIO(GPIO_FN_D30,	D30_MARK),
-	PINMUX_GPIO(GPIO_FN_D29,	D29_MARK),
-	PINMUX_GPIO(GPIO_FN_D28,	D28_MARK),
-	PINMUX_GPIO(GPIO_FN_D27,	D27_MARK),
-	PINMUX_GPIO(GPIO_FN_D26,	D26_MARK),
-	PINMUX_GPIO(GPIO_FN_D25,	D25_MARK),
-	PINMUX_GPIO(GPIO_FN_D24,	D24_MARK),
-	PINMUX_GPIO(GPIO_FN_D23,	D23_MARK),
-	PINMUX_GPIO(GPIO_FN_D22,	D22_MARK),
-	PINMUX_GPIO(GPIO_FN_D21,	D21_MARK),
-	PINMUX_GPIO(GPIO_FN_D20,	D20_MARK),
-	PINMUX_GPIO(GPIO_FN_D19,	D19_MARK),
-	PINMUX_GPIO(GPIO_FN_D18,	D18_MARK),
-	PINMUX_GPIO(GPIO_FN_D17,	D17_MARK),
-	PINMUX_GPIO(GPIO_FN_D16,	D16_MARK),
-	PINMUX_GPIO(GPIO_FN_D15,	D15_MARK),
-	PINMUX_GPIO(GPIO_FN_D14,	D14_MARK),
-	PINMUX_GPIO(GPIO_FN_D13,	D13_MARK),
-	PINMUX_GPIO(GPIO_FN_D12,	D12_MARK),
-	PINMUX_GPIO(GPIO_FN_D11,	D11_MARK),
-	PINMUX_GPIO(GPIO_FN_D10,	D10_MARK),
-	PINMUX_GPIO(GPIO_FN_D9,		D9_MARK),
-	PINMUX_GPIO(GPIO_FN_D8,		D8_MARK),
-	PINMUX_GPIO(GPIO_FN_D7,		D7_MARK),
-	PINMUX_GPIO(GPIO_FN_D6,		D6_MARK),
-	PINMUX_GPIO(GPIO_FN_D5,		D5_MARK),
-	PINMUX_GPIO(GPIO_FN_D4,		D4_MARK),
-	PINMUX_GPIO(GPIO_FN_D3,		D3_MARK),
-	PINMUX_GPIO(GPIO_FN_D2,		D2_MARK),
-	PINMUX_GPIO(GPIO_FN_D1,		D1_MARK),
-	PINMUX_GPIO(GPIO_FN_D0,		D0_MARK),
-	PINMUX_GPIO(GPIO_FN_A25,	A25_MARK),
-	PINMUX_GPIO(GPIO_FN_A24,	A24_MARK),
-	PINMUX_GPIO(GPIO_FN_A23,	A23_MARK),
-	PINMUX_GPIO(GPIO_FN_A22,	A22_MARK),
-	PINMUX_GPIO(GPIO_FN_CS6B_CE1B,	CS6B_CE1B_MARK),
-	PINMUX_GPIO(GPIO_FN_CS6A_CE2B,	CS6A_CE2B_MARK),
-	PINMUX_GPIO(GPIO_FN_CS5B_CE1A,	CS5B_CE1A_MARK),
-	PINMUX_GPIO(GPIO_FN_CS5A_CE2A,	CS5A_CE2A_MARK),
-	PINMUX_GPIO(GPIO_FN_WE3_ICIOWR,	WE3_ICIOWR_MARK),
-	PINMUX_GPIO(GPIO_FN_WE2_ICIORD,	WE2_ICIORD_MARK),
-	PINMUX_GPIO(GPIO_FN_IOIS16,	IOIS16_MARK),
-	PINMUX_GPIO(GPIO_FN_WAIT,	WAIT_MARK),
-	PINMUX_GPIO(GPIO_FN_BS,		BS_MARK),
+	GPIO_FN(D31),
+	GPIO_FN(D30),
+	GPIO_FN(D29),
+	GPIO_FN(D28),
+	GPIO_FN(D27),
+	GPIO_FN(D26),
+	GPIO_FN(D25),
+	GPIO_FN(D24),
+	GPIO_FN(D23),
+	GPIO_FN(D22),
+	GPIO_FN(D21),
+	GPIO_FN(D20),
+	GPIO_FN(D19),
+	GPIO_FN(D18),
+	GPIO_FN(D17),
+	GPIO_FN(D16),
+	GPIO_FN(D15),
+	GPIO_FN(D14),
+	GPIO_FN(D13),
+	GPIO_FN(D12),
+	GPIO_FN(D11),
+	GPIO_FN(D10),
+	GPIO_FN(D9),
+	GPIO_FN(D8),
+	GPIO_FN(D7),
+	GPIO_FN(D6),
+	GPIO_FN(D5),
+	GPIO_FN(D4),
+	GPIO_FN(D3),
+	GPIO_FN(D2),
+	GPIO_FN(D1),
+	GPIO_FN(D0),
+	GPIO_FN(A25),
+	GPIO_FN(A24),
+	GPIO_FN(A23),
+	GPIO_FN(A22),
+	GPIO_FN(CS6B_CE1B),
+	GPIO_FN(CS6A_CE2B),
+	GPIO_FN(CS5B_CE1A),
+	GPIO_FN(CS5A_CE2A),
+	GPIO_FN(WE3_ICIOWR),
+	GPIO_FN(WE2_ICIORD),
+	GPIO_FN(IOIS16),
+	GPIO_FN(WAIT),
+	GPIO_FN(BS),
 
 	/* KEYSC */
-	PINMUX_GPIO(GPIO_FN_KEYOUT5_IN5,	KEYOUT5_IN5_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT4_IN6,	KEYOUT4_IN6_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYIN4,		KEYIN4_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYIN3,		KEYIN3_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYIN2,		KEYIN2_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYIN1,		KEYIN1_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYIN0,		KEYIN0_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT3,		KEYOUT3_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT2,		KEYOUT2_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT1,		KEYOUT1_MARK),
-	PINMUX_GPIO(GPIO_FN_KEYOUT0,		KEYOUT0_MARK),
+	GPIO_FN(KEYOUT5_IN5),
+	GPIO_FN(KEYOUT4_IN6),
+	GPIO_FN(KEYIN4),
+	GPIO_FN(KEYIN3),
+	GPIO_FN(KEYIN2),
+	GPIO_FN(KEYIN1),
+	GPIO_FN(KEYIN0),
+	GPIO_FN(KEYOUT3),
+	GPIO_FN(KEYOUT2),
+	GPIO_FN(KEYOUT1),
+	GPIO_FN(KEYOUT0),
 
 	/* ATAPI */
-	PINMUX_GPIO(GPIO_FN_IDED15,	IDED15_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED14,	IDED14_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED13,	IDED13_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED12,	IDED12_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED11,	IDED11_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED10,	IDED10_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED9,	IDED9_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED8,	IDED8_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED7,	IDED7_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED6,	IDED6_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED5,	IDED5_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED4,	IDED4_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED3,	IDED3_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED2,	IDED2_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED1,	IDED1_MARK),
-	PINMUX_GPIO(GPIO_FN_IDED0,	IDED0_MARK),
-	PINMUX_GPIO(GPIO_FN_IDEA2,	IDEA2_MARK),
-	PINMUX_GPIO(GPIO_FN_IDEA1,	IDEA1_MARK),
-	PINMUX_GPIO(GPIO_FN_IDEA0,	IDEA0_MARK),
-	PINMUX_GPIO(GPIO_FN_IDEIOWR,	IDEIOWR_MARK),
-	PINMUX_GPIO(GPIO_FN_IODREQ,	IODREQ_MARK),
-	PINMUX_GPIO(GPIO_FN_IDECS0,	IDECS0_MARK),
-	PINMUX_GPIO(GPIO_FN_IDECS1,	IDECS1_MARK),
-	PINMUX_GPIO(GPIO_FN_IDEIORD,	IDEIORD_MARK),
-	PINMUX_GPIO(GPIO_FN_DIRECTION,	DIRECTION_MARK),
-	PINMUX_GPIO(GPIO_FN_EXBUF_ENB,	EXBUF_ENB_MARK),
-	PINMUX_GPIO(GPIO_FN_IDERST,	IDERST_MARK),
-	PINMUX_GPIO(GPIO_FN_IODACK,	IODACK_MARK),
-	PINMUX_GPIO(GPIO_FN_IDEINT,	IDEINT_MARK),
-	PINMUX_GPIO(GPIO_FN_IDEIORDY,	IDEIORDY_MARK),
+	GPIO_FN(IDED15),
+	GPIO_FN(IDED14),
+	GPIO_FN(IDED13),
+	GPIO_FN(IDED12),
+	GPIO_FN(IDED11),
+	GPIO_FN(IDED10),
+	GPIO_FN(IDED9),
+	GPIO_FN(IDED8),
+	GPIO_FN(IDED7),
+	GPIO_FN(IDED6),
+	GPIO_FN(IDED5),
+	GPIO_FN(IDED4),
+	GPIO_FN(IDED3),
+	GPIO_FN(IDED2),
+	GPIO_FN(IDED1),
+	GPIO_FN(IDED0),
+	GPIO_FN(IDEA2),
+	GPIO_FN(IDEA1),
+	GPIO_FN(IDEA0),
+	GPIO_FN(IDEIOWR),
+	GPIO_FN(IODREQ),
+	GPIO_FN(IDECS0),
+	GPIO_FN(IDECS1),
+	GPIO_FN(IDEIORD),
+	GPIO_FN(DIRECTION),
+	GPIO_FN(EXBUF_ENB),
+	GPIO_FN(IDERST),
+	GPIO_FN(IODACK),
+	GPIO_FN(IDEINT),
+	GPIO_FN(IDEIORDY),
 
 	/* TPU */
-	PINMUX_GPIO(GPIO_FN_TPUTO3,	TPUTO3_MARK),
-	PINMUX_GPIO(GPIO_FN_TPUTO2,	TPUTO2_MARK),
-	PINMUX_GPIO(GPIO_FN_TPUTO1,	TPUTO1_MARK),
-	PINMUX_GPIO(GPIO_FN_TPUTO0,	TPUTO0_MARK),
-	PINMUX_GPIO(GPIO_FN_TPUTI3,	TPUTI3_MARK),
-	PINMUX_GPIO(GPIO_FN_TPUTI2,	TPUTI2_MARK),
+	GPIO_FN(TPUTO3),
+	GPIO_FN(TPUTO2),
+	GPIO_FN(TPUTO1),
+	GPIO_FN(TPUTO0),
+	GPIO_FN(TPUTI3),
+	GPIO_FN(TPUTI2),
 
 	/* LCDC */
-	PINMUX_GPIO(GPIO_FN_LCDD23,	LCDD23_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD22,	LCDD22_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD21,	LCDD21_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD20,	LCDD20_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD19,	LCDD19_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD18,	LCDD18_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD17,	LCDD17_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD16,	LCDD16_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD15,	LCDD15_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD14,	LCDD14_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD13,	LCDD13_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD12,	LCDD12_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD11,	LCDD11_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD10,	LCDD10_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD9,	LCDD9_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD8,	LCDD8_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD7,	LCDD7_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD6,	LCDD6_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD5,	LCDD5_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD4,	LCDD4_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD3,	LCDD3_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD2,	LCDD2_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD1,	LCDD1_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDD0,	LCDD0_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDVSYN,	LCDVSYN_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDDISP,	LCDDISP_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDRS,	LCDRS_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDHSYN,	LCDHSYN_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDCS,	LCDCS_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDDON,	LCDDON_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDDCK,	LCDDCK_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDWR,	LCDWR_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDVEPWC,	LCDVEPWC_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDVCPWC,	LCDVCPWC_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDRD,	LCDRD_MARK),
-	PINMUX_GPIO(GPIO_FN_LCDLCLK,	LCDLCLK_MARK),
+	GPIO_FN(LCDD23),
+	GPIO_FN(LCDD22),
+	GPIO_FN(LCDD21),
+	GPIO_FN(LCDD20),
+	GPIO_FN(LCDD19),
+	GPIO_FN(LCDD18),
+	GPIO_FN(LCDD17),
+	GPIO_FN(LCDD16),
+	GPIO_FN(LCDD15),
+	GPIO_FN(LCDD14),
+	GPIO_FN(LCDD13),
+	GPIO_FN(LCDD12),
+	GPIO_FN(LCDD11),
+	GPIO_FN(LCDD10),
+	GPIO_FN(LCDD9),
+	GPIO_FN(LCDD8),
+	GPIO_FN(LCDD7),
+	GPIO_FN(LCDD6),
+	GPIO_FN(LCDD5),
+	GPIO_FN(LCDD4),
+	GPIO_FN(LCDD3),
+	GPIO_FN(LCDD2),
+	GPIO_FN(LCDD1),
+	GPIO_FN(LCDD0),
+	GPIO_FN(LCDVSYN),
+	GPIO_FN(LCDDISP),
+	GPIO_FN(LCDRS),
+	GPIO_FN(LCDHSYN),
+	GPIO_FN(LCDCS),
+	GPIO_FN(LCDDON),
+	GPIO_FN(LCDDCK),
+	GPIO_FN(LCDWR),
+	GPIO_FN(LCDVEPWC),
+	GPIO_FN(LCDVCPWC),
+	GPIO_FN(LCDRD),
+	GPIO_FN(LCDLCLK),
 
 	/* SCIF0 */
-	PINMUX_GPIO(GPIO_FN_SCIF0_TXD,	SCIF0_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_RXD,	SCIF0_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_SCK,	SCIF0_SCK_MARK),
+	GPIO_FN(SCIF0_TXD),
+	GPIO_FN(SCIF0_RXD),
+	GPIO_FN(SCIF0_SCK),
 
 	/* SCIF1 */
-	PINMUX_GPIO(GPIO_FN_SCIF1_SCK,	SCIF1_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_RXD,	SCIF1_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_TXD,	SCIF1_TXD_MARK),
+	GPIO_FN(SCIF1_SCK),
+	GPIO_FN(SCIF1_RXD),
+	GPIO_FN(SCIF1_TXD),
 
 	/* SCIF2 */
-	PINMUX_GPIO(GPIO_FN_SCIF2_L_TXD,	SCIF2_L_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_L_SCK,	SCIF2_L_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_L_RXD,	SCIF2_L_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_V_TXD,	SCIF2_V_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_V_SCK,	SCIF2_V_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_V_RXD,	SCIF2_V_RXD_MARK),
+	GPIO_FN(SCIF2_L_TXD),
+	GPIO_FN(SCIF2_L_SCK),
+	GPIO_FN(SCIF2_L_RXD),
+	GPIO_FN(SCIF2_V_TXD),
+	GPIO_FN(SCIF2_V_SCK),
+	GPIO_FN(SCIF2_V_RXD),
 
 	/* SCIF3 */
-	PINMUX_GPIO(GPIO_FN_SCIF3_V_SCK,	SCIF3_V_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_V_RXD,	SCIF3_V_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_V_TXD,	SCIF3_V_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_V_CTS,	SCIF3_V_CTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_V_RTS,	SCIF3_V_RTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_I_SCK,	SCIF3_I_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_I_RXD,	SCIF3_I_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_I_TXD,	SCIF3_I_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_I_CTS,	SCIF3_I_CTS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_I_RTS,	SCIF3_I_RTS_MARK),
+	GPIO_FN(SCIF3_V_SCK),
+	GPIO_FN(SCIF3_V_RXD),
+	GPIO_FN(SCIF3_V_TXD),
+	GPIO_FN(SCIF3_V_CTS),
+	GPIO_FN(SCIF3_V_RTS),
+	GPIO_FN(SCIF3_I_SCK),
+	GPIO_FN(SCIF3_I_RXD),
+	GPIO_FN(SCIF3_I_TXD),
+	GPIO_FN(SCIF3_I_CTS),
+	GPIO_FN(SCIF3_I_RTS),
 
 	/* SCIF4 */
-	PINMUX_GPIO(GPIO_FN_SCIF4_SCK,	SCIF4_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF4_RXD,	SCIF4_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF4_TXD,	SCIF4_TXD_MARK),
+	GPIO_FN(SCIF4_SCK),
+	GPIO_FN(SCIF4_RXD),
+	GPIO_FN(SCIF4_TXD),
 
 	/* SCIF5 */
-	PINMUX_GPIO(GPIO_FN_SCIF5_SCK,	SCIF5_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF5_RXD,	SCIF5_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF5_TXD,	SCIF5_TXD_MARK),
+	GPIO_FN(SCIF5_SCK),
+	GPIO_FN(SCIF5_RXD),
+	GPIO_FN(SCIF5_TXD),
 
 	/* FSI */
-	PINMUX_GPIO(GPIO_FN_FSIMCKB,	FSIMCKB_MARK),
-	PINMUX_GPIO(GPIO_FN_FSIMCKA,	FSIMCKA_MARK),
-	PINMUX_GPIO(GPIO_FN_FSIOASD,	FSIOASD_MARK),
-	PINMUX_GPIO(GPIO_FN_FSIIABCK,	FSIIABCK_MARK),
-	PINMUX_GPIO(GPIO_FN_FSIIALRCK,	FSIIALRCK_MARK),
-	PINMUX_GPIO(GPIO_FN_FSIOABCK,	FSIOABCK_MARK),
-	PINMUX_GPIO(GPIO_FN_FSIOALRCK,	FSIOALRCK_MARK),
-	PINMUX_GPIO(GPIO_FN_CLKAUDIOAO,	CLKAUDIOAO_MARK),
-	PINMUX_GPIO(GPIO_FN_FSIIBSD,	FSIIBSD_MARK),
-	PINMUX_GPIO(GPIO_FN_FSIOBSD,	FSIOBSD_MARK),
-	PINMUX_GPIO(GPIO_FN_FSIIBBCK,	FSIIBBCK_MARK),
-	PINMUX_GPIO(GPIO_FN_FSIIBLRCK,	FSIIBLRCK_MARK),
-	PINMUX_GPIO(GPIO_FN_FSIOBBCK,	FSIOBBCK_MARK),
-	PINMUX_GPIO(GPIO_FN_FSIOBLRCK,	FSIOBLRCK_MARK),
-	PINMUX_GPIO(GPIO_FN_CLKAUDIOBO,	CLKAUDIOBO_MARK),
-	PINMUX_GPIO(GPIO_FN_FSIIASD,	FSIIASD_MARK),
+	GPIO_FN(FSIMCKB),
+	GPIO_FN(FSIMCKA),
+	GPIO_FN(FSIOASD),
+	GPIO_FN(FSIIABCK),
+	GPIO_FN(FSIIALRCK),
+	GPIO_FN(FSIOABCK),
+	GPIO_FN(FSIOALRCK),
+	GPIO_FN(CLKAUDIOAO),
+	GPIO_FN(FSIIBSD),
+	GPIO_FN(FSIOBSD),
+	GPIO_FN(FSIIBBCK),
+	GPIO_FN(FSIIBLRCK),
+	GPIO_FN(FSIOBBCK),
+	GPIO_FN(FSIOBLRCK),
+	GPIO_FN(CLKAUDIOBO),
+	GPIO_FN(FSIIASD),
 
 	/* AUD */
-	PINMUX_GPIO(GPIO_FN_AUDCK,	AUDCK_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDSYNC,	AUDSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA3,	AUDATA3_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA2,	AUDATA2_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA1,	AUDATA1_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA0,	AUDATA0_MARK),
+	GPIO_FN(AUDCK),
+	GPIO_FN(AUDSYNC),
+	GPIO_FN(AUDATA3),
+	GPIO_FN(AUDATA2),
+	GPIO_FN(AUDATA1),
+	GPIO_FN(AUDATA0),
 
 	/* VIO */
-	PINMUX_GPIO(GPIO_FN_VIO_CKO,	VIO_CKO_MARK),
+	GPIO_FN(VIO_CKO),
 
 	/* VIO0 */
-	PINMUX_GPIO(GPIO_FN_VIO0_D15,	VIO0_D15_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_D14,	VIO0_D14_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_D13,	VIO0_D13_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_D12,	VIO0_D12_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_D11,	VIO0_D11_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_D10,	VIO0_D10_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_D9,	VIO0_D9_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_D8,	VIO0_D8_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_D7,	VIO0_D7_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_D6,	VIO0_D6_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_D5,	VIO0_D5_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_D4,	VIO0_D4_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_D3,	VIO0_D3_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_D2,	VIO0_D2_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_D1,	VIO0_D1_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_D0,	VIO0_D0_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_VD,	VIO0_VD_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_CLK,	VIO0_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_FLD,	VIO0_FLD_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO0_HD,	VIO0_HD_MARK),
+	GPIO_FN(VIO0_D15),
+	GPIO_FN(VIO0_D14),
+	GPIO_FN(VIO0_D13),
+	GPIO_FN(VIO0_D12),
+	GPIO_FN(VIO0_D11),
+	GPIO_FN(VIO0_D10),
+	GPIO_FN(VIO0_D9),
+	GPIO_FN(VIO0_D8),
+	GPIO_FN(VIO0_D7),
+	GPIO_FN(VIO0_D6),
+	GPIO_FN(VIO0_D5),
+	GPIO_FN(VIO0_D4),
+	GPIO_FN(VIO0_D3),
+	GPIO_FN(VIO0_D2),
+	GPIO_FN(VIO0_D1),
+	GPIO_FN(VIO0_D0),
+	GPIO_FN(VIO0_VD),
+	GPIO_FN(VIO0_CLK),
+	GPIO_FN(VIO0_FLD),
+	GPIO_FN(VIO0_HD),
 
 	/* VIO1 */
-	PINMUX_GPIO(GPIO_FN_VIO1_D7,	VIO1_D7_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO1_D6,	VIO1_D6_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO1_D5,	VIO1_D5_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO1_D4,	VIO1_D4_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO1_D3,	VIO1_D3_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO1_D2,	VIO1_D2_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO1_D1,	VIO1_D1_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO1_D0,	VIO1_D0_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO1_FLD,	VIO1_FLD_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO1_HD,	VIO1_HD_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO1_VD,	VIO1_VD_MARK),
-	PINMUX_GPIO(GPIO_FN_VIO1_CLK,	VIO1_CLK_MARK),
+	GPIO_FN(VIO1_D7),
+	GPIO_FN(VIO1_D6),
+	GPIO_FN(VIO1_D5),
+	GPIO_FN(VIO1_D4),
+	GPIO_FN(VIO1_D3),
+	GPIO_FN(VIO1_D2),
+	GPIO_FN(VIO1_D1),
+	GPIO_FN(VIO1_D0),
+	GPIO_FN(VIO1_FLD),
+	GPIO_FN(VIO1_HD),
+	GPIO_FN(VIO1_VD),
+	GPIO_FN(VIO1_CLK),
 
 	/* Eth */
-	PINMUX_GPIO(GPIO_FN_RMII_RXD0,		RMII_RXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII_RXD1,		RMII_RXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII_TXD0,		RMII_TXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII_TXD1,		RMII_TXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII_REF_CLK,	RMII_REF_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII_TX_EN,		RMII_TX_EN_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII_RX_ER,		RMII_RX_ER_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII_CRS_DV,	RMII_CRS_DV_MARK),
-	PINMUX_GPIO(GPIO_FN_LNKSTA,		LNKSTA_MARK),
-	PINMUX_GPIO(GPIO_FN_MDIO,		MDIO_MARK),
-	PINMUX_GPIO(GPIO_FN_MDC,		MDC_MARK),
+	GPIO_FN(RMII_RXD0),
+	GPIO_FN(RMII_RXD1),
+	GPIO_FN(RMII_TXD0),
+	GPIO_FN(RMII_TXD1),
+	GPIO_FN(RMII_REF_CLK),
+	GPIO_FN(RMII_TX_EN),
+	GPIO_FN(RMII_RX_ER),
+	GPIO_FN(RMII_CRS_DV),
+	GPIO_FN(LNKSTA),
+	GPIO_FN(MDIO),
+	GPIO_FN(MDC),
 
 	/* System */
-	PINMUX_GPIO(GPIO_FN_PDSTATUS,	PDSTATUS_MARK),
-	PINMUX_GPIO(GPIO_FN_STATUS2,	STATUS2_MARK),
-	PINMUX_GPIO(GPIO_FN_STATUS0,	STATUS0_MARK),
+	GPIO_FN(PDSTATUS),
+	GPIO_FN(STATUS2),
+	GPIO_FN(STATUS0),
 
 	/* VOU */
-	PINMUX_GPIO(GPIO_FN_DV_D15,	DV_D15_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D14,	DV_D14_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D13,	DV_D13_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D12,	DV_D12_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D11,	DV_D11_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D10,	DV_D10_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D9,	DV_D9_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D8,	DV_D8_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D7,	DV_D7_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D6,	DV_D6_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D5,	DV_D5_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D4,	DV_D4_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D3,	DV_D3_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D2,	DV_D2_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D1,	DV_D1_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_D0,	DV_D0_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_CLKI,	DV_CLKI_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_CLK,	DV_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_VSYNC,	DV_VSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_DV_HSYNC,	DV_HSYNC_MARK),
+	GPIO_FN(DV_D15),
+	GPIO_FN(DV_D14),
+	GPIO_FN(DV_D13),
+	GPIO_FN(DV_D12),
+	GPIO_FN(DV_D11),
+	GPIO_FN(DV_D10),
+	GPIO_FN(DV_D9),
+	GPIO_FN(DV_D8),
+	GPIO_FN(DV_D7),
+	GPIO_FN(DV_D6),
+	GPIO_FN(DV_D5),
+	GPIO_FN(DV_D4),
+	GPIO_FN(DV_D3),
+	GPIO_FN(DV_D2),
+	GPIO_FN(DV_D1),
+	GPIO_FN(DV_D0),
+	GPIO_FN(DV_CLKI),
+	GPIO_FN(DV_CLK),
+	GPIO_FN(DV_VSYNC),
+	GPIO_FN(DV_HSYNC),
 
 	/* MSIOF0 */
-	PINMUX_GPIO(GPIO_FN_MSIOF0_RXD,		MSIOF0_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_TXD,		MSIOF0_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_MCK,		MSIOF0_MCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_TSCK,	MSIOF0_TSCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_SS1,		MSIOF0_SS1_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_SS2,		MSIOF0_SS2_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_TSYNC,	MSIOF0_TSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_RSCK,	MSIOF0_RSCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF0_RSYNC,	MSIOF0_RSYNC_MARK),
+	GPIO_FN(MSIOF0_RXD),
+	GPIO_FN(MSIOF0_TXD),
+	GPIO_FN(MSIOF0_MCK),
+	GPIO_FN(MSIOF0_TSCK),
+	GPIO_FN(MSIOF0_SS1),
+	GPIO_FN(MSIOF0_SS2),
+	GPIO_FN(MSIOF0_TSYNC),
+	GPIO_FN(MSIOF0_RSCK),
+	GPIO_FN(MSIOF0_RSYNC),
 
 	/* MSIOF1 */
-	PINMUX_GPIO(GPIO_FN_MSIOF1_RXD,		MSIOF1_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_TXD,		MSIOF1_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_MCK,		MSIOF1_MCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_TSCK,	MSIOF1_TSCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_SS1,		MSIOF1_SS1_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_SS2,		MSIOF1_SS2_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_TSYNC,	MSIOF1_TSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_RSCK,	MSIOF1_RSCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MSIOF1_RSYNC,	MSIOF1_RSYNC_MARK),
+	GPIO_FN(MSIOF1_RXD),
+	GPIO_FN(MSIOF1_TXD),
+	GPIO_FN(MSIOF1_MCK),
+	GPIO_FN(MSIOF1_TSCK),
+	GPIO_FN(MSIOF1_SS1),
+	GPIO_FN(MSIOF1_SS2),
+	GPIO_FN(MSIOF1_TSYNC),
+	GPIO_FN(MSIOF1_RSCK),
+	GPIO_FN(MSIOF1_RSYNC),
 
 	/* DMAC */
-	PINMUX_GPIO(GPIO_FN_DMAC_DACK0,	DMAC_DACK0_MARK),
-	PINMUX_GPIO(GPIO_FN_DMAC_DREQ0,	DMAC_DREQ0_MARK),
-	PINMUX_GPIO(GPIO_FN_DMAC_DACK1,	DMAC_DACK1_MARK),
-	PINMUX_GPIO(GPIO_FN_DMAC_DREQ1,	DMAC_DREQ1_MARK),
+	GPIO_FN(DMAC_DACK0),
+	GPIO_FN(DMAC_DREQ0),
+	GPIO_FN(DMAC_DACK1),
+	GPIO_FN(DMAC_DREQ1),
 
 	/* SDHI0 */
-	PINMUX_GPIO(GPIO_FN_SDHI0CD,	SDHI0CD_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0WP,	SDHI0WP_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0CMD,	SDHI0CMD_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0CLK,	SDHI0CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0D3,	SDHI0D3_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0D2,	SDHI0D2_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0D1,	SDHI0D1_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI0D0,	SDHI0D0_MARK),
+	GPIO_FN(SDHI0CD),
+	GPIO_FN(SDHI0WP),
+	GPIO_FN(SDHI0CMD),
+	GPIO_FN(SDHI0CLK),
+	GPIO_FN(SDHI0D3),
+	GPIO_FN(SDHI0D2),
+	GPIO_FN(SDHI0D1),
+	GPIO_FN(SDHI0D0),
 
 	/* SDHI1 */
-	PINMUX_GPIO(GPIO_FN_SDHI1CD,	SDHI1CD_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI1WP,	SDHI1WP_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI1CMD,	SDHI1CMD_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI1CLK,	SDHI1CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI1D3,	SDHI1D3_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI1D2,	SDHI1D2_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI1D1,	SDHI1D1_MARK),
-	PINMUX_GPIO(GPIO_FN_SDHI1D0,	SDHI1D0_MARK),
+	GPIO_FN(SDHI1CD),
+	GPIO_FN(SDHI1WP),
+	GPIO_FN(SDHI1CMD),
+	GPIO_FN(SDHI1CLK),
+	GPIO_FN(SDHI1D3),
+	GPIO_FN(SDHI1D2),
+	GPIO_FN(SDHI1D1),
+	GPIO_FN(SDHI1D0),
 
 	/* MMC */
-	PINMUX_GPIO(GPIO_FN_MMC_D7,	MMC_D7_MARK),
-	PINMUX_GPIO(GPIO_FN_MMC_D6,	MMC_D6_MARK),
-	PINMUX_GPIO(GPIO_FN_MMC_D5,	MMC_D5_MARK),
-	PINMUX_GPIO(GPIO_FN_MMC_D4,	MMC_D4_MARK),
-	PINMUX_GPIO(GPIO_FN_MMC_D3,	MMC_D3_MARK),
-	PINMUX_GPIO(GPIO_FN_MMC_D2,	MMC_D2_MARK),
-	PINMUX_GPIO(GPIO_FN_MMC_D1,	MMC_D1_MARK),
-	PINMUX_GPIO(GPIO_FN_MMC_D0,	MMC_D0_MARK),
-	PINMUX_GPIO(GPIO_FN_MMC_CLK,	MMC_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_MMC_CMD,	MMC_CMD_MARK),
+	GPIO_FN(MMC_D7),
+	GPIO_FN(MMC_D6),
+	GPIO_FN(MMC_D5),
+	GPIO_FN(MMC_D4),
+	GPIO_FN(MMC_D3),
+	GPIO_FN(MMC_D2),
+	GPIO_FN(MMC_D1),
+	GPIO_FN(MMC_D0),
+	GPIO_FN(MMC_CLK),
+	GPIO_FN(MMC_CMD),
 
 	/* IrDA */
-	PINMUX_GPIO(GPIO_FN_IRDA_OUT,	IRDA_OUT_MARK),
-	PINMUX_GPIO(GPIO_FN_IRDA_IN,	IRDA_IN_MARK),
+	GPIO_FN(IRDA_OUT),
+	GPIO_FN(IRDA_IN),
 
 	/* TSIF */
-	PINMUX_GPIO(GPIO_FN_TSIF_TS0_SDAT,	TSIF_TS0_SDAT_MARK),
-	PINMUX_GPIO(GPIO_FN_TSIF_TS0_SCK,	TSIF_TS0_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_TSIF_TS0_SDEN,	TSIF_TS0_SDEN_MARK),
-	PINMUX_GPIO(GPIO_FN_TSIF_TS0_SPSYNC,	TSIF_TS0_SPSYNC_MARK),
+	GPIO_FN(TSIF_TS0_SDAT),
+	GPIO_FN(TSIF_TS0_SCK),
+	GPIO_FN(TSIF_TS0_SDEN),
+	GPIO_FN(TSIF_TS0_SPSYNC),
 
 	/* IRQ */
-	PINMUX_GPIO(GPIO_FN_INTC_IRQ7,	INTC_IRQ7_MARK),
-	PINMUX_GPIO(GPIO_FN_INTC_IRQ6,	INTC_IRQ6_MARK),
-	PINMUX_GPIO(GPIO_FN_INTC_IRQ5,	INTC_IRQ5_MARK),
-	PINMUX_GPIO(GPIO_FN_INTC_IRQ4,	INTC_IRQ4_MARK),
-	PINMUX_GPIO(GPIO_FN_INTC_IRQ3,	INTC_IRQ3_MARK),
-	PINMUX_GPIO(GPIO_FN_INTC_IRQ2,	INTC_IRQ2_MARK),
-	PINMUX_GPIO(GPIO_FN_INTC_IRQ1,	INTC_IRQ1_MARK),
-	PINMUX_GPIO(GPIO_FN_INTC_IRQ0,	INTC_IRQ0_MARK),
+	GPIO_FN(INTC_IRQ7),
+	GPIO_FN(INTC_IRQ6),
+	GPIO_FN(INTC_IRQ5),
+	GPIO_FN(INTC_IRQ4),
+	GPIO_FN(INTC_IRQ3),
+	GPIO_FN(INTC_IRQ2),
+	GPIO_FN(INTC_IRQ1),
+	GPIO_FN(INTC_IRQ0),
 };
 
-static struct pinmux_cfg_reg pinmux_config_regs[] = {
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ PINMUX_CFG_REG("PACR", 0xa4050100, 16, 2) {
 		PTA7_FN, PTA7_OUT, PTA7_IN_PU, PTA7_IN,
 		PTA6_FN, PTA6_OUT, PTA6_IN_PU, PTA6_IN,
@@ -2107,7 +2111,7 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{}
 };
 
-static struct pinmux_data_reg pinmux_data_regs[] = {
+static const struct pinmux_data_reg pinmux_data_regs[] = {
 	{ PINMUX_DATA_REG("PADR", 0xa4050120, 8) {
 		PTA7_DATA, PTA6_DATA, PTA5_DATA, PTA4_DATA,
 		PTA3_DATA, PTA2_DATA, PTA1_DATA, PTA0_DATA }
@@ -2203,20 +2207,18 @@ static struct pinmux_data_reg pinmux_data_regs[] = {
 	{ },
 };
 
-struct sh_pfc_soc_info sh7724_pinmux_info = {
+const struct sh_pfc_soc_info sh7724_pinmux_info = {
 	.name = "sh7724_pfc",
-	.reserved_id = PINMUX_RESERVED,
-	.data = { PINMUX_DATA_BEGIN, PINMUX_DATA_END },
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END },
 	.input_pu = { PINMUX_INPUT_PULLUP_BEGIN, PINMUX_INPUT_PULLUP_END },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END },
-	.mark = { PINMUX_MARK_BEGIN, PINMUX_MARK_END },
 	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
 
-	.first_gpio = GPIO_PTA7,
-	.last_gpio = GPIO_FN_INTC_IRQ0,
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.func_gpios = pinmux_func_gpios,
+	.nr_func_gpios = ARRAY_SIZE(pinmux_func_gpios),
 
-	.gpios = pinmux_gpios,
 	.cfg_regs = pinmux_config_regs,
 	.data_regs = pinmux_data_regs,
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh7734.c b/drivers/pinctrl/sh-pfc/pfc-sh7734.c
index 23d76d262c32..2fd5b7d4cb94 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh7734.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh7734.c
@@ -14,11 +14,6 @@
 
 #include "sh_pfc.h"
 
-#define CPU_32_PORT(fn, pfx, sfx)				\
-	PORT_10(fn, pfx, sfx), PORT_10(fn, pfx##1, sfx),	\
-	PORT_10(fn, pfx##2, sfx), PORT_1(fn, pfx##30, sfx),	\
-	PORT_1(fn, pfx##31, sfx)
-
 #define CPU_32_PORT5(fn, pfx, sfx)				\
 	PORT_1(fn, pfx##0, sfx), PORT_1(fn, pfx##1, sfx),	\
 	PORT_1(fn, pfx##2, sfx), PORT_1(fn, pfx##3, sfx),	\
@@ -29,11 +24,11 @@
 
 /* GPSR0 - GPSR5 */
 #define CPU_ALL_PORT(fn, pfx, sfx)				\
-	CPU_32_PORT(fn, pfx##_0_, sfx),			\
-	CPU_32_PORT(fn, pfx##_1_, sfx),				\
-	CPU_32_PORT(fn, pfx##_2_, sfx),				\
-	CPU_32_PORT(fn, pfx##_3_, sfx),				\
-	CPU_32_PORT(fn, pfx##_4_, sfx),				\
+	PORT_32(fn, pfx##_0_, sfx),			\
+	PORT_32(fn, pfx##_1_, sfx),				\
+	PORT_32(fn, pfx##_2_, sfx),				\
+	PORT_32(fn, pfx##_3_, sfx),				\
+	PORT_32(fn, pfx##_4_, sfx),				\
 	CPU_32_PORT5(fn, pfx##_5_, sfx)
 
 #define _GP_GPIO(pfx, sfx) PINMUX_GPIO(GPIO_GP##pfx, GP##pfx##_DATA)
@@ -47,20 +42,8 @@
 #define PINMUX_GPIO_GP_ALL()	CPU_ALL_PORT(_GP_GPIO, , unused)
 #define PINMUX_DATA_GP_ALL()	CPU_ALL_PORT(_GP_DATA, , unused)
 
-#define PORT_10_REV(fn, pfx, sfx)	\
-	PORT_1(fn, pfx##9, sfx), PORT_1(fn, pfx##8, sfx),	\
-	PORT_1(fn, pfx##7, sfx), PORT_1(fn, pfx##6, sfx),	\
-	PORT_1(fn, pfx##5, sfx), PORT_1(fn, pfx##4, sfx),	\
-	PORT_1(fn, pfx##3, sfx), PORT_1(fn, pfx##2, sfx),	\
-	PORT_1(fn, pfx##1, sfx), PORT_1(fn, pfx##0, sfx)
-
-#define CPU_32_PORT_REV(fn, pfx, sfx)	\
-	PORT_1(fn, pfx##31, sfx), PORT_1(fn, pfx##30, sfx),	\
-	PORT_10_REV(fn, pfx##2, sfx), PORT_10_REV(fn, pfx##1, sfx),	\
-	PORT_10_REV(fn, pfx, sfx)
-
-#define GP_INOUTSEL(bank) CPU_32_PORT_REV(_GP_INOUTSEL, _##bank##_, unused)
-#define GP_INDT(bank) CPU_32_PORT_REV(_GP_INDT, _##bank##_, unused)
+#define GP_INOUTSEL(bank) PORT_32_REV(_GP_INOUTSEL, _##bank##_, unused)
+#define GP_INDT(bank) PORT_32_REV(_GP_INDT, _##bank##_, unused)
 
 #define PINMUX_IPSR_DATA(ipsr, fn) PINMUX_DATA(fn##_MARK, FN_##ipsr, FN_##fn)
 #define PINMUX_IPSR_MODSEL_DATA(ipsr, fn, ms) PINMUX_DATA(fn##_MARK, FN_##ms, \
@@ -609,7 +592,7 @@ enum {
 	PINMUX_MARK_END,
 };
 
-static pinmux_enum_t pinmux_data[] = {
+static const pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA_GP_ALL(), /* PINMUX_DATA(GP_M_N_DATA, GP_M_N_FN...), */
 
 	PINMUX_DATA(CLKOUT_MARK, FN_CLKOUT),
@@ -1384,9 +1367,13 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_IPSR_DATA(IP11_28, ST_CLKOUT),
 };
 
-static struct pinmux_gpio pinmux_gpios[] = {
+static struct sh_pfc_pin pinmux_pins[] = {
 	PINMUX_GPIO_GP_ALL(),
+};
+
+#define PINMUX_FN_BASE	ARRAY_SIZE(pinmux_pins)
 
+static const struct pinmux_func pinmux_func_gpios[] = {
 	GPIO_FN(CLKOUT), GPIO_FN(BS), GPIO_FN(CS0), GPIO_FN(EX_CS0),
 	GPIO_FN(RD), GPIO_FN(WE0), GPIO_FN(WE1),
 	GPIO_FN(SCL0), GPIO_FN(PENC0), GPIO_FN(USB_OVC0),
@@ -1665,7 +1652,7 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	GPIO_FN(SCL1), GPIO_FN(SCIF_CLK_C),
 };
 
-static struct pinmux_cfg_reg pinmux_config_regs[] = {
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ PINMUX_CFG_REG("GPSR0", 0xFFFC0004, 32, 1) {
 		GP_0_31_FN, FN_IP2_2_0,
 		GP_0_30_FN, FN_IP1_31_29,
@@ -2434,7 +2421,7 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ },
 };
 
-static struct pinmux_data_reg pinmux_data_regs[] = {
+static const struct pinmux_data_reg pinmux_data_regs[] = {
 	/* GPIO 0 - 5*/
 	{ PINMUX_DATA_REG("INDT0", 0xFFC4000C, 32) { GP_INDT(0) } },
 	{ PINMUX_DATA_REG("INDT1", 0xFFC4100C, 32) { GP_INDT(1) } },
@@ -2451,22 +2438,20 @@ static struct pinmux_data_reg pinmux_data_regs[] = {
 	{ },
 };
 
-struct sh_pfc_soc_info sh7734_pinmux_info = {
+const struct sh_pfc_soc_info sh7734_pinmux_info = {
 	.name = "sh7734_pfc",
 
 	.unlock_reg = 0xFFFC0000,
 
-	.reserved_id = PINMUX_RESERVED,
-	.data = { PINMUX_DATA_BEGIN, PINMUX_DATA_END },
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END },
-	.mark = { PINMUX_MARK_BEGIN, PINMUX_MARK_END },
 	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
 
-	.first_gpio = GPIO_GP_0_0,
-	.last_gpio = GPIO_FN_ST_CLKOUT,
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.func_gpios = pinmux_func_gpios,
+	.nr_func_gpios = ARRAY_SIZE(pinmux_func_gpios),
 
-	.gpios = pinmux_gpios,
 	.cfg_regs = pinmux_config_regs,
 	.data_regs = pinmux_data_regs,
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh7757.c b/drivers/pinctrl/sh-pfc/pfc-sh7757.c
index 5ed74cd0ba99..e074230e6243 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh7757.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh7757.c
@@ -526,7 +526,7 @@ enum {
 	PINMUX_MARK_END,
 };
 
-static pinmux_enum_t pinmux_data[] = {
+static const pinmux_enum_t pinmux_data[] = {
 	/* PTA GPIO */
 	PINMUX_DATA(PTA7_DATA, PTA7_IN, PTA7_OUT),
 	PINMUX_DATA(PTA6_DATA, PTA6_IN, PTA6_OUT),
@@ -1114,7 +1114,7 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA(ON_DQ0_MARK, PS8_8_FN2, PTZ0_FN),
 };
 
-static struct pinmux_gpio pinmux_gpios[] = {
+static struct sh_pfc_pin pinmux_pins[] = {
 	/* PTA */
 	PINMUX_GPIO(GPIO_PTA7, PTA7_DATA),
 	PINMUX_GPIO(GPIO_PTA6, PTA6_DATA),
@@ -1370,359 +1370,363 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	PINMUX_GPIO(GPIO_PTZ2, PTZ2_DATA),
 	PINMUX_GPIO(GPIO_PTZ1, PTZ1_DATA),
 	PINMUX_GPIO(GPIO_PTZ0, PTZ0_DATA),
+};
+
+#define PINMUX_FN_BASE	ARRAY_SIZE(pinmux_pins)
 
+static const struct pinmux_func pinmux_func_gpios[] = {
 	/* PTA (mobule: LBSC, RGMII) */
-	PINMUX_GPIO(GPIO_FN_BS, BS_MARK),
-	PINMUX_GPIO(GPIO_FN_RDWR, RDWR_MARK),
-	PINMUX_GPIO(GPIO_FN_WE1, WE1_MARK),
-	PINMUX_GPIO(GPIO_FN_RDY, RDY_MARK),
-	PINMUX_GPIO(GPIO_FN_ET0_MDC, ET0_MDC_MARK),
-	PINMUX_GPIO(GPIO_FN_ET0_MDIO, ET0_MDIO_MARK),
-	PINMUX_GPIO(GPIO_FN_ET1_MDC, ET1_MDC_MARK),
-	PINMUX_GPIO(GPIO_FN_ET1_MDIO, ET1_MDIO_MARK),
+	GPIO_FN(BS),
+	GPIO_FN(RDWR),
+	GPIO_FN(WE1),
+	GPIO_FN(RDY),
+	GPIO_FN(ET0_MDC),
+	GPIO_FN(ET0_MDIO),
+	GPIO_FN(ET1_MDC),
+	GPIO_FN(ET1_MDIO),
 
 	/* PTB (mobule: INTC, ONFI, TMU) */
-	PINMUX_GPIO(GPIO_FN_IRQ15, IRQ15_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ14, IRQ14_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ13, IRQ13_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ12, IRQ12_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ11, IRQ11_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ10, IRQ10_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ9, IRQ9_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ8, IRQ8_MARK),
-	PINMUX_GPIO(GPIO_FN_ON_NRE, ON_NRE_MARK),
-	PINMUX_GPIO(GPIO_FN_ON_NWE, ON_NWE_MARK),
-	PINMUX_GPIO(GPIO_FN_ON_NWP, ON_NWP_MARK),
-	PINMUX_GPIO(GPIO_FN_ON_NCE0, ON_NCE0_MARK),
-	PINMUX_GPIO(GPIO_FN_ON_R_B0, ON_R_B0_MARK),
-	PINMUX_GPIO(GPIO_FN_ON_ALE, ON_ALE_MARK),
-	PINMUX_GPIO(GPIO_FN_ON_CLE, ON_CLE_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLK, TCLK_MARK),
+	GPIO_FN(IRQ15),
+	GPIO_FN(IRQ14),
+	GPIO_FN(IRQ13),
+	GPIO_FN(IRQ12),
+	GPIO_FN(IRQ11),
+	GPIO_FN(IRQ10),
+	GPIO_FN(IRQ9),
+	GPIO_FN(IRQ8),
+	GPIO_FN(ON_NRE),
+	GPIO_FN(ON_NWE),
+	GPIO_FN(ON_NWP),
+	GPIO_FN(ON_NCE0),
+	GPIO_FN(ON_R_B0),
+	GPIO_FN(ON_ALE),
+	GPIO_FN(ON_CLE),
+	GPIO_FN(TCLK),
 
 	/* PTC (mobule: IRQ, PWMU) */
-	PINMUX_GPIO(GPIO_FN_IRQ7, IRQ7_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ6, IRQ6_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ5, IRQ5_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ4, IRQ4_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ3, IRQ3_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ2, IRQ2_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ1, IRQ1_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ0, IRQ0_MARK),
-	PINMUX_GPIO(GPIO_FN_PWMU0, PWMU0_MARK),
-	PINMUX_GPIO(GPIO_FN_PWMU1, PWMU1_MARK),
-	PINMUX_GPIO(GPIO_FN_PWMU2, PWMU2_MARK),
-	PINMUX_GPIO(GPIO_FN_PWMU3, PWMU3_MARK),
-	PINMUX_GPIO(GPIO_FN_PWMU4, PWMU4_MARK),
-	PINMUX_GPIO(GPIO_FN_PWMU5, PWMU5_MARK),
+	GPIO_FN(IRQ7),
+	GPIO_FN(IRQ6),
+	GPIO_FN(IRQ5),
+	GPIO_FN(IRQ4),
+	GPIO_FN(IRQ3),
+	GPIO_FN(IRQ2),
+	GPIO_FN(IRQ1),
+	GPIO_FN(IRQ0),
+	GPIO_FN(PWMU0),
+	GPIO_FN(PWMU1),
+	GPIO_FN(PWMU2),
+	GPIO_FN(PWMU3),
+	GPIO_FN(PWMU4),
+	GPIO_FN(PWMU5),
 
 	/* PTD (mobule: SPI0, DMAC) */
-	PINMUX_GPIO(GPIO_FN_SP0_MOSI, SP0_MOSI_MARK),
-	PINMUX_GPIO(GPIO_FN_SP0_MISO, SP0_MISO_MARK),
-	PINMUX_GPIO(GPIO_FN_SP0_SCK, SP0_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SP0_SCK_FB, SP0_SCK_FB_MARK),
-	PINMUX_GPIO(GPIO_FN_SP0_SS0, SP0_SS0_MARK),
-	PINMUX_GPIO(GPIO_FN_SP0_SS1, SP0_SS1_MARK),
-	PINMUX_GPIO(GPIO_FN_SP0_SS2, SP0_SS2_MARK),
-	PINMUX_GPIO(GPIO_FN_SP0_SS3, SP0_SS3_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ0, DREQ0_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK0, DACK0_MARK),
-	PINMUX_GPIO(GPIO_FN_TEND0, TEND0_MARK),
+	GPIO_FN(SP0_MOSI),
+	GPIO_FN(SP0_MISO),
+	GPIO_FN(SP0_SCK),
+	GPIO_FN(SP0_SCK_FB),
+	GPIO_FN(SP0_SS0),
+	GPIO_FN(SP0_SS1),
+	GPIO_FN(SP0_SS2),
+	GPIO_FN(SP0_SS3),
+	GPIO_FN(DREQ0),
+	GPIO_FN(DACK0),
+	GPIO_FN(TEND0),
 
 	/* PTE (mobule: RMII) */
-	PINMUX_GPIO(GPIO_FN_RMII0_CRS_DV, RMII0_CRS_DV_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII0_TXD1, RMII0_TXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII0_TXD0, RMII0_TXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII0_TXEN, RMII0_TXEN_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII0_REFCLK, RMII0_REFCLK_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII0_RXD1, RMII0_RXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII0_RXD0, RMII0_RXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII0_RX_ER, RMII0_RX_ER_MARK),
+	GPIO_FN(RMII0_CRS_DV),
+	GPIO_FN(RMII0_TXD1),
+	GPIO_FN(RMII0_TXD0),
+	GPIO_FN(RMII0_TXEN),
+	GPIO_FN(RMII0_REFCLK),
+	GPIO_FN(RMII0_RXD1),
+	GPIO_FN(RMII0_RXD0),
+	GPIO_FN(RMII0_RX_ER),
 
 	/* PTF (mobule: RMII, SerMux) */
-	PINMUX_GPIO(GPIO_FN_RMII1_CRS_DV, RMII1_CRS_DV_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII1_TXD1, RMII1_TXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII1_TXD0, RMII1_TXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII1_TXEN, RMII1_TXEN_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII1_REFCLK, RMII1_REFCLK_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII1_RXD1, RMII1_RXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII1_RXD0, RMII1_RXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_RMII1_RX_ER, RMII1_RX_ER_MARK),
-	PINMUX_GPIO(GPIO_FN_RAC_RI, RAC_RI_MARK),
+	GPIO_FN(RMII1_CRS_DV),
+	GPIO_FN(RMII1_TXD1),
+	GPIO_FN(RMII1_TXD0),
+	GPIO_FN(RMII1_TXEN),
+	GPIO_FN(RMII1_REFCLK),
+	GPIO_FN(RMII1_RXD1),
+	GPIO_FN(RMII1_RXD0),
+	GPIO_FN(RMII1_RX_ER),
+	GPIO_FN(RAC_RI),
 
 	/* PTG (mobule: system, LBSC, LPC, WDT, LPC, eMMC) */
-	PINMUX_GPIO(GPIO_FN_BOOTFMS, BOOTFMS_MARK),
-	PINMUX_GPIO(GPIO_FN_BOOTWP, BOOTWP_MARK),
-	PINMUX_GPIO(GPIO_FN_A25, A25_MARK),
-	PINMUX_GPIO(GPIO_FN_A24, A24_MARK),
-	PINMUX_GPIO(GPIO_FN_SERIRQ, SERIRQ_MARK),
-	PINMUX_GPIO(GPIO_FN_WDTOVF, WDTOVF_MARK),
-	PINMUX_GPIO(GPIO_FN_LPCPD, LPCPD_MARK),
-	PINMUX_GPIO(GPIO_FN_LDRQ, LDRQ_MARK),
-	PINMUX_GPIO(GPIO_FN_MMCCLK, MMCCLK_MARK),
-	PINMUX_GPIO(GPIO_FN_MMCCMD, MMCCMD_MARK),
+	GPIO_FN(BOOTFMS),
+	GPIO_FN(BOOTWP),
+	GPIO_FN(A25),
+	GPIO_FN(A24),
+	GPIO_FN(SERIRQ),
+	GPIO_FN(WDTOVF),
+	GPIO_FN(LPCPD),
+	GPIO_FN(LDRQ),
+	GPIO_FN(MMCCLK),
+	GPIO_FN(MMCCMD),
 
 	/* PTH (mobule: SPI1, LPC, DMAC, ADC) */
-	PINMUX_GPIO(GPIO_FN_SP1_MOSI, SP1_MOSI_MARK),
-	PINMUX_GPIO(GPIO_FN_SP1_MISO, SP1_MISO_MARK),
-	PINMUX_GPIO(GPIO_FN_SP1_SCK, SP1_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SP1_SCK_FB, SP1_SCK_FB_MARK),
-	PINMUX_GPIO(GPIO_FN_SP1_SS0, SP1_SS0_MARK),
-	PINMUX_GPIO(GPIO_FN_SP1_SS1, SP1_SS1_MARK),
-	PINMUX_GPIO(GPIO_FN_WP, WP_MARK),
-	PINMUX_GPIO(GPIO_FN_FMS0, FMS0_MARK),
-	PINMUX_GPIO(GPIO_FN_TEND1, TEND1_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ1, DREQ1_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK1, DACK1_MARK),
-	PINMUX_GPIO(GPIO_FN_ADTRG1, ADTRG1_MARK),
-	PINMUX_GPIO(GPIO_FN_ADTRG0, ADTRG0_MARK),
+	GPIO_FN(SP1_MOSI),
+	GPIO_FN(SP1_MISO),
+	GPIO_FN(SP1_SCK),
+	GPIO_FN(SP1_SCK_FB),
+	GPIO_FN(SP1_SS0),
+	GPIO_FN(SP1_SS1),
+	GPIO_FN(WP),
+	GPIO_FN(FMS0),
+	GPIO_FN(TEND1),
+	GPIO_FN(DREQ1),
+	GPIO_FN(DACK1),
+	GPIO_FN(ADTRG1),
+	GPIO_FN(ADTRG0),
 
 	/* PTI (mobule: LBSC, SDHI) */
-	PINMUX_GPIO(GPIO_FN_D15, D15_MARK),
-	PINMUX_GPIO(GPIO_FN_D14, D14_MARK),
-	PINMUX_GPIO(GPIO_FN_D13, D13_MARK),
-	PINMUX_GPIO(GPIO_FN_D12, D12_MARK),
-	PINMUX_GPIO(GPIO_FN_D11, D11_MARK),
-	PINMUX_GPIO(GPIO_FN_D10, D10_MARK),
-	PINMUX_GPIO(GPIO_FN_D9, D9_MARK),
-	PINMUX_GPIO(GPIO_FN_D8, D8_MARK),
-	PINMUX_GPIO(GPIO_FN_SD_WP, SD_WP_MARK),
-	PINMUX_GPIO(GPIO_FN_SD_CD, SD_CD_MARK),
-	PINMUX_GPIO(GPIO_FN_SD_CLK, SD_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SD_CMD, SD_CMD_MARK),
-	PINMUX_GPIO(GPIO_FN_SD_D3, SD_D3_MARK),
-	PINMUX_GPIO(GPIO_FN_SD_D2, SD_D2_MARK),
-	PINMUX_GPIO(GPIO_FN_SD_D1, SD_D1_MARK),
-	PINMUX_GPIO(GPIO_FN_SD_D0, SD_D0_MARK),
+	GPIO_FN(D15),
+	GPIO_FN(D14),
+	GPIO_FN(D13),
+	GPIO_FN(D12),
+	GPIO_FN(D11),
+	GPIO_FN(D10),
+	GPIO_FN(D9),
+	GPIO_FN(D8),
+	GPIO_FN(SD_WP),
+	GPIO_FN(SD_CD),
+	GPIO_FN(SD_CLK),
+	GPIO_FN(SD_CMD),
+	GPIO_FN(SD_D3),
+	GPIO_FN(SD_D2),
+	GPIO_FN(SD_D1),
+	GPIO_FN(SD_D0),
 
 	/* PTJ (mobule: SCIF234, SERMUX) */
-	PINMUX_GPIO(GPIO_FN_RTS3, RTS3_MARK),
-	PINMUX_GPIO(GPIO_FN_CTS3, CTS3_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD3, TXD3_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD3, RXD3_MARK),
-	PINMUX_GPIO(GPIO_FN_RTS4, RTS4_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD4, RXD4_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD4, TXD4_MARK),
+	GPIO_FN(RTS3),
+	GPIO_FN(CTS3),
+	GPIO_FN(TXD3),
+	GPIO_FN(RXD3),
+	GPIO_FN(RTS4),
+	GPIO_FN(RXD4),
+	GPIO_FN(TXD4),
 
 	/* PTK (mobule: SERMUX, LBSC, SCIF) */
-	PINMUX_GPIO(GPIO_FN_COM2_TXD, COM2_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_COM2_RXD, COM2_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_COM2_RTS, COM2_RTS_MARK),
-	PINMUX_GPIO(GPIO_FN_COM2_CTS, COM2_CTS_MARK),
-	PINMUX_GPIO(GPIO_FN_COM2_DTR, COM2_DTR_MARK),
-	PINMUX_GPIO(GPIO_FN_COM2_DSR, COM2_DSR_MARK),
-	PINMUX_GPIO(GPIO_FN_COM2_DCD, COM2_DCD_MARK),
-	PINMUX_GPIO(GPIO_FN_CLKOUT, CLKOUT_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK2, SCK2_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK4, SCK4_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK3, SCK3_MARK),
+	GPIO_FN(COM2_TXD),
+	GPIO_FN(COM2_RXD),
+	GPIO_FN(COM2_RTS),
+	GPIO_FN(COM2_CTS),
+	GPIO_FN(COM2_DTR),
+	GPIO_FN(COM2_DSR),
+	GPIO_FN(COM2_DCD),
+	GPIO_FN(CLKOUT),
+	GPIO_FN(SCK2),
+	GPIO_FN(SCK4),
+	GPIO_FN(SCK3),
 
 	/* PTL (mobule: SERMUX, SCIF, LBSC, AUD) */
-	PINMUX_GPIO(GPIO_FN_RAC_RXD, RAC_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_RAC_RTS, RAC_RTS_MARK),
-	PINMUX_GPIO(GPIO_FN_RAC_CTS, RAC_CTS_MARK),
-	PINMUX_GPIO(GPIO_FN_RAC_DTR, RAC_DTR_MARK),
-	PINMUX_GPIO(GPIO_FN_RAC_DSR, RAC_DSR_MARK),
-	PINMUX_GPIO(GPIO_FN_RAC_DCD, RAC_DCD_MARK),
-	PINMUX_GPIO(GPIO_FN_RAC_TXD, RAC_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD2, RXD2_MARK),
-	PINMUX_GPIO(GPIO_FN_CS5, CS5_MARK),
-	PINMUX_GPIO(GPIO_FN_CS6, CS6_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDSYNC, AUDSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDCK, AUDCK_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD2, TXD2_MARK),
+	GPIO_FN(RAC_RXD),
+	GPIO_FN(RAC_RTS),
+	GPIO_FN(RAC_CTS),
+	GPIO_FN(RAC_DTR),
+	GPIO_FN(RAC_DSR),
+	GPIO_FN(RAC_DCD),
+	GPIO_FN(RAC_TXD),
+	GPIO_FN(RXD2),
+	GPIO_FN(CS5),
+	GPIO_FN(CS6),
+	GPIO_FN(AUDSYNC),
+	GPIO_FN(AUDCK),
+	GPIO_FN(TXD2),
 
 	/* PTM (mobule: LBSC, IIC) */
-	PINMUX_GPIO(GPIO_FN_CS4, CS4_MARK),
-	PINMUX_GPIO(GPIO_FN_RD, RD_MARK),
-	PINMUX_GPIO(GPIO_FN_WE0, WE0_MARK),
-	PINMUX_GPIO(GPIO_FN_CS0, CS0_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA6, SDA6_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL6, SCL6_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA7, SDA7_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL7, SCL7_MARK),
+	GPIO_FN(CS4),
+	GPIO_FN(RD),
+	GPIO_FN(WE0),
+	GPIO_FN(CS0),
+	GPIO_FN(SDA6),
+	GPIO_FN(SCL6),
+	GPIO_FN(SDA7),
+	GPIO_FN(SCL7),
 
 	/* PTN (mobule: USB, JMC, SGPIO, WDT) */
-	PINMUX_GPIO(GPIO_FN_VBUS_EN, VBUS_EN_MARK),
-	PINMUX_GPIO(GPIO_FN_VBUS_OC, VBUS_OC_MARK),
-	PINMUX_GPIO(GPIO_FN_JMCTCK, JMCTCK_MARK),
-	PINMUX_GPIO(GPIO_FN_JMCTMS, JMCTMS_MARK),
-	PINMUX_GPIO(GPIO_FN_JMCTDO, JMCTDO_MARK),
-	PINMUX_GPIO(GPIO_FN_JMCTDI, JMCTDI_MARK),
-	PINMUX_GPIO(GPIO_FN_JMCTRST, JMCTRST_MARK),
-	PINMUX_GPIO(GPIO_FN_SGPIO1_CLK, SGPIO1_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SGPIO1_LOAD, SGPIO1_LOAD_MARK),
-	PINMUX_GPIO(GPIO_FN_SGPIO1_DI, SGPIO1_DI_MARK),
-	PINMUX_GPIO(GPIO_FN_SGPIO1_DO, SGPIO1_DO_MARK),
-	PINMUX_GPIO(GPIO_FN_SUB_CLKIN, SUB_CLKIN_MARK),
+	GPIO_FN(VBUS_EN),
+	GPIO_FN(VBUS_OC),
+	GPIO_FN(JMCTCK),
+	GPIO_FN(JMCTMS),
+	GPIO_FN(JMCTDO),
+	GPIO_FN(JMCTDI),
+	GPIO_FN(JMCTRST),
+	GPIO_FN(SGPIO1_CLK),
+	GPIO_FN(SGPIO1_LOAD),
+	GPIO_FN(SGPIO1_DI),
+	GPIO_FN(SGPIO1_DO),
+	GPIO_FN(SUB_CLKIN),
 
 	/* PTO (mobule: SGPIO, SerMux) */
-	PINMUX_GPIO(GPIO_FN_SGPIO0_CLK, SGPIO0_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SGPIO0_LOAD, SGPIO0_LOAD_MARK),
-	PINMUX_GPIO(GPIO_FN_SGPIO0_DI, SGPIO0_DI_MARK),
-	PINMUX_GPIO(GPIO_FN_SGPIO0_DO, SGPIO0_DO_MARK),
-	PINMUX_GPIO(GPIO_FN_SGPIO2_CLK, SGPIO2_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SGPIO2_LOAD, SGPIO2_LOAD_MARK),
-	PINMUX_GPIO(GPIO_FN_SGPIO2_DI, SGPIO2_DI_MARK),
-	PINMUX_GPIO(GPIO_FN_SGPIO2_DO, SGPIO2_DO_MARK),
-	PINMUX_GPIO(GPIO_FN_COM1_TXD, COM1_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_COM1_RXD, COM1_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_COM1_RTS, COM1_RTS_MARK),
-	PINMUX_GPIO(GPIO_FN_COM1_CTS, COM1_CTS_MARK),
+	GPIO_FN(SGPIO0_CLK),
+	GPIO_FN(SGPIO0_LOAD),
+	GPIO_FN(SGPIO0_DI),
+	GPIO_FN(SGPIO0_DO),
+	GPIO_FN(SGPIO2_CLK),
+	GPIO_FN(SGPIO2_LOAD),
+	GPIO_FN(SGPIO2_DI),
+	GPIO_FN(SGPIO2_DO),
+	GPIO_FN(COM1_TXD),
+	GPIO_FN(COM1_RXD),
+	GPIO_FN(COM1_RTS),
+	GPIO_FN(COM1_CTS),
 
 	/* PTP (mobule: EVC, ADC) */
 
 	/* PTQ (mobule: LPC) */
-	PINMUX_GPIO(GPIO_FN_LAD3, LAD3_MARK),
-	PINMUX_GPIO(GPIO_FN_LAD2, LAD2_MARK),
-	PINMUX_GPIO(GPIO_FN_LAD1, LAD1_MARK),
-	PINMUX_GPIO(GPIO_FN_LAD0, LAD0_MARK),
-	PINMUX_GPIO(GPIO_FN_LFRAME, LFRAME_MARK),
-	PINMUX_GPIO(GPIO_FN_LRESET, LRESET_MARK),
-	PINMUX_GPIO(GPIO_FN_LCLK, LCLK_MARK),
+	GPIO_FN(LAD3),
+	GPIO_FN(LAD2),
+	GPIO_FN(LAD1),
+	GPIO_FN(LAD0),
+	GPIO_FN(LFRAME),
+	GPIO_FN(LRESET),
+	GPIO_FN(LCLK),
 
 	/* PTR (mobule: GRA, IIC) */
-	PINMUX_GPIO(GPIO_FN_DDC3, DDC3_MARK),
-	PINMUX_GPIO(GPIO_FN_DDC2, DDC2_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA8, SDA8_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL8, SCL8_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA2, SDA2_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL2, SCL2_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA1, SDA1_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL1, SCL1_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA0, SDA0_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL0, SCL0_MARK),
+	GPIO_FN(DDC3),
+	GPIO_FN(DDC2),
+	GPIO_FN(SDA8),
+	GPIO_FN(SCL8),
+	GPIO_FN(SDA2),
+	GPIO_FN(SCL2),
+	GPIO_FN(SDA1),
+	GPIO_FN(SCL1),
+	GPIO_FN(SDA0),
+	GPIO_FN(SCL0),
 
 	/* PTS (mobule: GRA, IIC) */
-	PINMUX_GPIO(GPIO_FN_DDC1, DDC1_MARK),
-	PINMUX_GPIO(GPIO_FN_DDC0, DDC0_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA9, SDA9_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL9, SCL9_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA5, SDA5_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL5, SCL5_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA4, SDA4_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL4, SCL4_MARK),
-	PINMUX_GPIO(GPIO_FN_SDA3, SDA3_MARK),
-	PINMUX_GPIO(GPIO_FN_SCL3, SCL3_MARK),
+	GPIO_FN(DDC1),
+	GPIO_FN(DDC0),
+	GPIO_FN(SDA9),
+	GPIO_FN(SCL9),
+	GPIO_FN(SDA5),
+	GPIO_FN(SCL5),
+	GPIO_FN(SDA4),
+	GPIO_FN(SCL4),
+	GPIO_FN(SDA3),
+	GPIO_FN(SCL3),
 
 	/* PTT (mobule: PWMX, AUD) */
-	PINMUX_GPIO(GPIO_FN_PWMX7, PWMX7_MARK),
-	PINMUX_GPIO(GPIO_FN_PWMX6, PWMX6_MARK),
-	PINMUX_GPIO(GPIO_FN_PWMX5, PWMX5_MARK),
-	PINMUX_GPIO(GPIO_FN_PWMX4, PWMX4_MARK),
-	PINMUX_GPIO(GPIO_FN_PWMX3, PWMX3_MARK),
-	PINMUX_GPIO(GPIO_FN_PWMX2, PWMX2_MARK),
-	PINMUX_GPIO(GPIO_FN_PWMX1, PWMX1_MARK),
-	PINMUX_GPIO(GPIO_FN_PWMX0, PWMX0_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA3, AUDATA3_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA2, AUDATA2_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA1, AUDATA1_MARK),
-	PINMUX_GPIO(GPIO_FN_AUDATA0, AUDATA0_MARK),
-	PINMUX_GPIO(GPIO_FN_STATUS1, STATUS1_MARK),
-	PINMUX_GPIO(GPIO_FN_STATUS0, STATUS0_MARK),
+	GPIO_FN(PWMX7),
+	GPIO_FN(PWMX6),
+	GPIO_FN(PWMX5),
+	GPIO_FN(PWMX4),
+	GPIO_FN(PWMX3),
+	GPIO_FN(PWMX2),
+	GPIO_FN(PWMX1),
+	GPIO_FN(PWMX0),
+	GPIO_FN(AUDATA3),
+	GPIO_FN(AUDATA2),
+	GPIO_FN(AUDATA1),
+	GPIO_FN(AUDATA0),
+	GPIO_FN(STATUS1),
+	GPIO_FN(STATUS0),
 
 	/* PTU (mobule: LPC, APM) */
-	PINMUX_GPIO(GPIO_FN_LGPIO7, LGPIO7_MARK),
-	PINMUX_GPIO(GPIO_FN_LGPIO6, LGPIO6_MARK),
-	PINMUX_GPIO(GPIO_FN_LGPIO5, LGPIO5_MARK),
-	PINMUX_GPIO(GPIO_FN_LGPIO4, LGPIO4_MARK),
-	PINMUX_GPIO(GPIO_FN_LGPIO3, LGPIO3_MARK),
-	PINMUX_GPIO(GPIO_FN_LGPIO2, LGPIO2_MARK),
-	PINMUX_GPIO(GPIO_FN_LGPIO1, LGPIO1_MARK),
-	PINMUX_GPIO(GPIO_FN_LGPIO0, LGPIO0_MARK),
-	PINMUX_GPIO(GPIO_FN_APMONCTL_O, APMONCTL_O_MARK),
-	PINMUX_GPIO(GPIO_FN_APMPWBTOUT_O, APMPWBTOUT_O_MARK),
-	PINMUX_GPIO(GPIO_FN_APMSCI_O, APMSCI_O_MARK),
-	PINMUX_GPIO(GPIO_FN_APMVDDON, APMVDDON_MARK),
-	PINMUX_GPIO(GPIO_FN_APMSLPBTN, APMSLPBTN_MARK),
-	PINMUX_GPIO(GPIO_FN_APMPWRBTN, APMPWRBTN_MARK),
-	PINMUX_GPIO(GPIO_FN_APMS5N, APMS5N_MARK),
-	PINMUX_GPIO(GPIO_FN_APMS3N, APMS3N_MARK),
+	GPIO_FN(LGPIO7),
+	GPIO_FN(LGPIO6),
+	GPIO_FN(LGPIO5),
+	GPIO_FN(LGPIO4),
+	GPIO_FN(LGPIO3),
+	GPIO_FN(LGPIO2),
+	GPIO_FN(LGPIO1),
+	GPIO_FN(LGPIO0),
+	GPIO_FN(APMONCTL_O),
+	GPIO_FN(APMPWBTOUT_O),
+	GPIO_FN(APMSCI_O),
+	GPIO_FN(APMVDDON),
+	GPIO_FN(APMSLPBTN),
+	GPIO_FN(APMPWRBTN),
+	GPIO_FN(APMS5N),
+	GPIO_FN(APMS3N),
 
 	/* PTV (mobule: LBSC, SerMux, R-SPI, EVC, GRA) */
-	PINMUX_GPIO(GPIO_FN_A23, A23_MARK),
-	PINMUX_GPIO(GPIO_FN_A22, A22_MARK),
-	PINMUX_GPIO(GPIO_FN_A21, A21_MARK),
-	PINMUX_GPIO(GPIO_FN_A20, A20_MARK),
-	PINMUX_GPIO(GPIO_FN_A19, A19_MARK),
-	PINMUX_GPIO(GPIO_FN_A18, A18_MARK),
-	PINMUX_GPIO(GPIO_FN_A17, A17_MARK),
-	PINMUX_GPIO(GPIO_FN_A16, A16_MARK),
-	PINMUX_GPIO(GPIO_FN_COM2_RI, COM2_RI_MARK),
-	PINMUX_GPIO(GPIO_FN_R_SPI_MOSI, R_SPI_MOSI_MARK),
-	PINMUX_GPIO(GPIO_FN_R_SPI_MISO, R_SPI_MISO_MARK),
-	PINMUX_GPIO(GPIO_FN_R_SPI_RSPCK, R_SPI_RSPCK_MARK),
-	PINMUX_GPIO(GPIO_FN_R_SPI_SSL0, R_SPI_SSL0_MARK),
-	PINMUX_GPIO(GPIO_FN_R_SPI_SSL1, R_SPI_SSL1_MARK),
-	PINMUX_GPIO(GPIO_FN_EVENT7, EVENT7_MARK),
-	PINMUX_GPIO(GPIO_FN_EVENT6, EVENT6_MARK),
-	PINMUX_GPIO(GPIO_FN_VBIOS_DI, VBIOS_DI_MARK),
-	PINMUX_GPIO(GPIO_FN_VBIOS_DO, VBIOS_DO_MARK),
-	PINMUX_GPIO(GPIO_FN_VBIOS_CLK, VBIOS_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_VBIOS_CS, VBIOS_CS_MARK),
+	GPIO_FN(A23),
+	GPIO_FN(A22),
+	GPIO_FN(A21),
+	GPIO_FN(A20),
+	GPIO_FN(A19),
+	GPIO_FN(A18),
+	GPIO_FN(A17),
+	GPIO_FN(A16),
+	GPIO_FN(COM2_RI),
+	GPIO_FN(R_SPI_MOSI),
+	GPIO_FN(R_SPI_MISO),
+	GPIO_FN(R_SPI_RSPCK),
+	GPIO_FN(R_SPI_SSL0),
+	GPIO_FN(R_SPI_SSL1),
+	GPIO_FN(EVENT7),
+	GPIO_FN(EVENT6),
+	GPIO_FN(VBIOS_DI),
+	GPIO_FN(VBIOS_DO),
+	GPIO_FN(VBIOS_CLK),
+	GPIO_FN(VBIOS_CS),
 
 	/* PTW (mobule: LBSC, EVC, SCIF) */
-	PINMUX_GPIO(GPIO_FN_A16, A16_MARK),
-	PINMUX_GPIO(GPIO_FN_A15, A15_MARK),
-	PINMUX_GPIO(GPIO_FN_A14, A14_MARK),
-	PINMUX_GPIO(GPIO_FN_A13, A13_MARK),
-	PINMUX_GPIO(GPIO_FN_A12, A12_MARK),
-	PINMUX_GPIO(GPIO_FN_A11, A11_MARK),
-	PINMUX_GPIO(GPIO_FN_A10, A10_MARK),
-	PINMUX_GPIO(GPIO_FN_A9, A9_MARK),
-	PINMUX_GPIO(GPIO_FN_A8, A8_MARK),
-	PINMUX_GPIO(GPIO_FN_EVENT5, EVENT5_MARK),
-	PINMUX_GPIO(GPIO_FN_EVENT4, EVENT4_MARK),
-	PINMUX_GPIO(GPIO_FN_EVENT3, EVENT3_MARK),
-	PINMUX_GPIO(GPIO_FN_EVENT2, EVENT2_MARK),
-	PINMUX_GPIO(GPIO_FN_EVENT1, EVENT1_MARK),
-	PINMUX_GPIO(GPIO_FN_EVENT0, EVENT0_MARK),
-	PINMUX_GPIO(GPIO_FN_CTS4, CTS4_MARK),
-	PINMUX_GPIO(GPIO_FN_CTS2, CTS2_MARK),
+	GPIO_FN(A16),
+	GPIO_FN(A15),
+	GPIO_FN(A14),
+	GPIO_FN(A13),
+	GPIO_FN(A12),
+	GPIO_FN(A11),
+	GPIO_FN(A10),
+	GPIO_FN(A9),
+	GPIO_FN(A8),
+	GPIO_FN(EVENT5),
+	GPIO_FN(EVENT4),
+	GPIO_FN(EVENT3),
+	GPIO_FN(EVENT2),
+	GPIO_FN(EVENT1),
+	GPIO_FN(EVENT0),
+	GPIO_FN(CTS4),
+	GPIO_FN(CTS2),
 
 	/* PTX (mobule: LBSC) */
-	PINMUX_GPIO(GPIO_FN_A7, A7_MARK),
-	PINMUX_GPIO(GPIO_FN_A6, A6_MARK),
-	PINMUX_GPIO(GPIO_FN_A5, A5_MARK),
-	PINMUX_GPIO(GPIO_FN_A4, A4_MARK),
-	PINMUX_GPIO(GPIO_FN_A3, A3_MARK),
-	PINMUX_GPIO(GPIO_FN_A2, A2_MARK),
-	PINMUX_GPIO(GPIO_FN_A1, A1_MARK),
-	PINMUX_GPIO(GPIO_FN_A0, A0_MARK),
-	PINMUX_GPIO(GPIO_FN_RTS2, RTS2_MARK),
-	PINMUX_GPIO(GPIO_FN_SIM_D, SIM_D_MARK),
-	PINMUX_GPIO(GPIO_FN_SIM_CLK, SIM_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SIM_RST, SIM_RST_MARK),
+	GPIO_FN(A7),
+	GPIO_FN(A6),
+	GPIO_FN(A5),
+	GPIO_FN(A4),
+	GPIO_FN(A3),
+	GPIO_FN(A2),
+	GPIO_FN(A1),
+	GPIO_FN(A0),
+	GPIO_FN(RTS2),
+	GPIO_FN(SIM_D),
+	GPIO_FN(SIM_CLK),
+	GPIO_FN(SIM_RST),
 
 	/* PTY (mobule: LBSC) */
-	PINMUX_GPIO(GPIO_FN_D7, D7_MARK),
-	PINMUX_GPIO(GPIO_FN_D6, D6_MARK),
-	PINMUX_GPIO(GPIO_FN_D5, D5_MARK),
-	PINMUX_GPIO(GPIO_FN_D4, D4_MARK),
-	PINMUX_GPIO(GPIO_FN_D3, D3_MARK),
-	PINMUX_GPIO(GPIO_FN_D2, D2_MARK),
-	PINMUX_GPIO(GPIO_FN_D1, D1_MARK),
-	PINMUX_GPIO(GPIO_FN_D0, D0_MARK),
+	GPIO_FN(D7),
+	GPIO_FN(D6),
+	GPIO_FN(D5),
+	GPIO_FN(D4),
+	GPIO_FN(D3),
+	GPIO_FN(D2),
+	GPIO_FN(D1),
+	GPIO_FN(D0),
 
 	/* PTZ (mobule: eMMC, ONFI) */
-	PINMUX_GPIO(GPIO_FN_MMCDAT7, MMCDAT7_MARK),
-	PINMUX_GPIO(GPIO_FN_MMCDAT6, MMCDAT6_MARK),
-	PINMUX_GPIO(GPIO_FN_MMCDAT5, MMCDAT5_MARK),
-	PINMUX_GPIO(GPIO_FN_MMCDAT4, MMCDAT4_MARK),
-	PINMUX_GPIO(GPIO_FN_MMCDAT3, MMCDAT3_MARK),
-	PINMUX_GPIO(GPIO_FN_MMCDAT2, MMCDAT2_MARK),
-	PINMUX_GPIO(GPIO_FN_MMCDAT1, MMCDAT1_MARK),
-	PINMUX_GPIO(GPIO_FN_MMCDAT0, MMCDAT0_MARK),
-	PINMUX_GPIO(GPIO_FN_ON_DQ7, ON_DQ7_MARK),
-	PINMUX_GPIO(GPIO_FN_ON_DQ6, ON_DQ6_MARK),
-	PINMUX_GPIO(GPIO_FN_ON_DQ5, ON_DQ5_MARK),
-	PINMUX_GPIO(GPIO_FN_ON_DQ4, ON_DQ4_MARK),
-	PINMUX_GPIO(GPIO_FN_ON_DQ3, ON_DQ3_MARK),
-	PINMUX_GPIO(GPIO_FN_ON_DQ2, ON_DQ2_MARK),
-	PINMUX_GPIO(GPIO_FN_ON_DQ1, ON_DQ1_MARK),
-	PINMUX_GPIO(GPIO_FN_ON_DQ0, ON_DQ0_MARK),
+	GPIO_FN(MMCDAT7),
+	GPIO_FN(MMCDAT6),
+	GPIO_FN(MMCDAT5),
+	GPIO_FN(MMCDAT4),
+	GPIO_FN(MMCDAT3),
+	GPIO_FN(MMCDAT2),
+	GPIO_FN(MMCDAT1),
+	GPIO_FN(MMCDAT0),
+	GPIO_FN(ON_DQ7),
+	GPIO_FN(ON_DQ6),
+	GPIO_FN(ON_DQ5),
+	GPIO_FN(ON_DQ4),
+	GPIO_FN(ON_DQ3),
+	GPIO_FN(ON_DQ2),
+	GPIO_FN(ON_DQ1),
+	GPIO_FN(ON_DQ0),
 };
 
-static struct pinmux_cfg_reg pinmux_config_regs[] = {
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ PINMUX_CFG_REG("PACR", 0xffec0000, 16, 2) {
 		PTA7_FN, PTA7_OUT, PTA7_IN, PTA7_IN_PU,
 		PTA6_FN, PTA6_OUT, PTA6_IN, PTA6_IN_PU,
@@ -2152,7 +2156,7 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{}
 };
 
-static struct pinmux_data_reg pinmux_data_regs[] = {
+static const struct pinmux_data_reg pinmux_data_regs[] = {
 	{ PINMUX_DATA_REG("PADR", 0xffec0034, 8) {
 		PTA7_DATA, PTA6_DATA, PTA5_DATA, PTA4_DATA,
 		PTA3_DATA, PTA2_DATA, PTA1_DATA, PTA0_DATA }
@@ -2260,20 +2264,18 @@ static struct pinmux_data_reg pinmux_data_regs[] = {
 	{ },
 };
 
-struct sh_pfc_soc_info sh7757_pinmux_info = {
+const struct sh_pfc_soc_info sh7757_pinmux_info = {
 	.name = "sh7757_pfc",
-	.reserved_id = PINMUX_RESERVED,
-	.data = { PINMUX_DATA_BEGIN, PINMUX_DATA_END },
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END },
 	.input_pu = { PINMUX_INPUT_PULLUP_BEGIN, PINMUX_INPUT_PULLUP_END },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END },
-	.mark = { PINMUX_MARK_BEGIN, PINMUX_MARK_END },
 	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
 
-	.first_gpio = GPIO_PTA0,
-	.last_gpio = GPIO_FN_ON_DQ0,
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.func_gpios = pinmux_func_gpios,
+	.nr_func_gpios = ARRAY_SIZE(pinmux_func_gpios),
 
-	.gpios = pinmux_gpios,
 	.cfg_regs = pinmux_config_regs,
 	.data_regs = pinmux_data_regs,
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh7785.c b/drivers/pinctrl/sh-pfc/pfc-sh7785.c
index 3b1825d925bb..c176b794f240 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh7785.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh7785.c
@@ -355,7 +355,7 @@ enum {
 	PINMUX_MARK_END,
 };
 
-static pinmux_enum_t pinmux_data[] = {
+static const pinmux_enum_t pinmux_data[] = {
 
 	/* PA GPIO */
 	PINMUX_DATA(PA7_DATA, PA7_IN, PA7_OUT, PA7_IN_PU),
@@ -702,7 +702,7 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA(IRQOUT_MARK, P2MSEL2_1),
 };
 
-static struct pinmux_gpio pinmux_gpios[] = {
+static struct sh_pfc_pin pinmux_pins[] = {
 	/* PA */
 	PINMUX_GPIO(GPIO_PA7, PA7_DATA),
 	PINMUX_GPIO(GPIO_PA6, PA6_DATA),
@@ -845,176 +845,180 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	PINMUX_GPIO(GPIO_PR2, PR2_DATA),
 	PINMUX_GPIO(GPIO_PR1, PR1_DATA),
 	PINMUX_GPIO(GPIO_PR0, PR0_DATA),
+};
+
+#define PINMUX_FN_BASE	ARRAY_SIZE(pinmux_pins)
 
+static const struct pinmux_func pinmux_func_gpios[] = {
 	/* FN */
-	PINMUX_GPIO(GPIO_FN_D63_AD31, D63_AD31_MARK),
-	PINMUX_GPIO(GPIO_FN_D62_AD30, D62_AD30_MARK),
-	PINMUX_GPIO(GPIO_FN_D61_AD29, D61_AD29_MARK),
-	PINMUX_GPIO(GPIO_FN_D60_AD28, D60_AD28_MARK),
-	PINMUX_GPIO(GPIO_FN_D59_AD27, D59_AD27_MARK),
-	PINMUX_GPIO(GPIO_FN_D58_AD26, D58_AD26_MARK),
-	PINMUX_GPIO(GPIO_FN_D57_AD25, D57_AD25_MARK),
-	PINMUX_GPIO(GPIO_FN_D56_AD24, D56_AD24_MARK),
-	PINMUX_GPIO(GPIO_FN_D55_AD23, D55_AD23_MARK),
-	PINMUX_GPIO(GPIO_FN_D54_AD22, D54_AD22_MARK),
-	PINMUX_GPIO(GPIO_FN_D53_AD21, D53_AD21_MARK),
-	PINMUX_GPIO(GPIO_FN_D52_AD20, D52_AD20_MARK),
-	PINMUX_GPIO(GPIO_FN_D51_AD19, D51_AD19_MARK),
-	PINMUX_GPIO(GPIO_FN_D50_AD18, D50_AD18_MARK),
-	PINMUX_GPIO(GPIO_FN_D49_AD17_DB5, D49_AD17_DB5_MARK),
-	PINMUX_GPIO(GPIO_FN_D48_AD16_DB4, D48_AD16_DB4_MARK),
-	PINMUX_GPIO(GPIO_FN_D47_AD15_DB3, D47_AD15_DB3_MARK),
-	PINMUX_GPIO(GPIO_FN_D46_AD14_DB2, D46_AD14_DB2_MARK),
-	PINMUX_GPIO(GPIO_FN_D45_AD13_DB1, D45_AD13_DB1_MARK),
-	PINMUX_GPIO(GPIO_FN_D44_AD12_DB0, D44_AD12_DB0_MARK),
-	PINMUX_GPIO(GPIO_FN_D43_AD11_DG5, D43_AD11_DG5_MARK),
-	PINMUX_GPIO(GPIO_FN_D42_AD10_DG4, D42_AD10_DG4_MARK),
-	PINMUX_GPIO(GPIO_FN_D41_AD9_DG3, D41_AD9_DG3_MARK),
-	PINMUX_GPIO(GPIO_FN_D40_AD8_DG2, D40_AD8_DG2_MARK),
-	PINMUX_GPIO(GPIO_FN_D39_AD7_DG1, D39_AD7_DG1_MARK),
-	PINMUX_GPIO(GPIO_FN_D38_AD6_DG0, D38_AD6_DG0_MARK),
-	PINMUX_GPIO(GPIO_FN_D37_AD5_DR5, D37_AD5_DR5_MARK),
-	PINMUX_GPIO(GPIO_FN_D36_AD4_DR4, D36_AD4_DR4_MARK),
-	PINMUX_GPIO(GPIO_FN_D35_AD3_DR3, D35_AD3_DR3_MARK),
-	PINMUX_GPIO(GPIO_FN_D34_AD2_DR2, D34_AD2_DR2_MARK),
-	PINMUX_GPIO(GPIO_FN_D33_AD1_DR1, D33_AD1_DR1_MARK),
-	PINMUX_GPIO(GPIO_FN_D32_AD0_DR0, D32_AD0_DR0_MARK),
-	PINMUX_GPIO(GPIO_FN_REQ1, REQ1_MARK),
-	PINMUX_GPIO(GPIO_FN_REQ2, REQ2_MARK),
-	PINMUX_GPIO(GPIO_FN_REQ3, REQ3_MARK),
-	PINMUX_GPIO(GPIO_FN_GNT1, GNT1_MARK),
-	PINMUX_GPIO(GPIO_FN_GNT2, GNT2_MARK),
-	PINMUX_GPIO(GPIO_FN_GNT3, GNT3_MARK),
-	PINMUX_GPIO(GPIO_FN_MMCCLK, MMCCLK_MARK),
-	PINMUX_GPIO(GPIO_FN_D31, D31_MARK),
-	PINMUX_GPIO(GPIO_FN_D30, D30_MARK),
-	PINMUX_GPIO(GPIO_FN_D29, D29_MARK),
-	PINMUX_GPIO(GPIO_FN_D28, D28_MARK),
-	PINMUX_GPIO(GPIO_FN_D27, D27_MARK),
-	PINMUX_GPIO(GPIO_FN_D26, D26_MARK),
-	PINMUX_GPIO(GPIO_FN_D25, D25_MARK),
-	PINMUX_GPIO(GPIO_FN_D24, D24_MARK),
-	PINMUX_GPIO(GPIO_FN_D23, D23_MARK),
-	PINMUX_GPIO(GPIO_FN_D22, D22_MARK),
-	PINMUX_GPIO(GPIO_FN_D21, D21_MARK),
-	PINMUX_GPIO(GPIO_FN_D20, D20_MARK),
-	PINMUX_GPIO(GPIO_FN_D19, D19_MARK),
-	PINMUX_GPIO(GPIO_FN_D18, D18_MARK),
-	PINMUX_GPIO(GPIO_FN_D17, D17_MARK),
-	PINMUX_GPIO(GPIO_FN_D16, D16_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_SCK, SCIF1_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_RXD, SCIF1_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_TXD, SCIF1_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_CTS, SCIF0_CTS_MARK),
-	PINMUX_GPIO(GPIO_FN_INTD, INTD_MARK),
-	PINMUX_GPIO(GPIO_FN_FCE, FCE_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_RTS, SCIF0_RTS_MARK),
-	PINMUX_GPIO(GPIO_FN_HSPI_CS, HSPI_CS_MARK),
-	PINMUX_GPIO(GPIO_FN_FSE, FSE_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_SCK, SCIF0_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_HSPI_CLK, HSPI_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_FRE, FRE_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_RXD, SCIF0_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_HSPI_RX, HSPI_RX_MARK),
-	PINMUX_GPIO(GPIO_FN_FRB, FRB_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_TXD, SCIF0_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_HSPI_TX, HSPI_TX_MARK),
-	PINMUX_GPIO(GPIO_FN_FWE, FWE_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF5_TXD, SCIF5_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC1_SYNC, HAC1_SYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI1_WS, SSI1_WS_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF_TXD_PJ, SIOF_TXD_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC0_SDOUT, HAC0_SDOUT_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI0_SDATA, SSI0_SDATA_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF_RXD_PJ, SIOF_RXD_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC0_SDIN, HAC0_SDIN_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI0_SCK, SSI0_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF_SYNC_PJ, SIOF_SYNC_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC0_SYNC, HAC0_SYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI0_WS, SSI0_WS_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF_MCLK_PJ, SIOF_MCLK_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC_RES, HAC_RES_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF_SCK_PJ, SIOF_SCK_PJ_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC0_BITCLK, HAC0_BITCLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI0_CLK, SSI0_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC1_BITCLK, HAC1_BITCLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI1_CLK, SSI1_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLK, TCLK_MARK),
-	PINMUX_GPIO(GPIO_FN_IOIS16, IOIS16_MARK),
-	PINMUX_GPIO(GPIO_FN_STATUS0, STATUS0_MARK),
-	PINMUX_GPIO(GPIO_FN_DRAK0_PK3, DRAK0_PK3_MARK),
-	PINMUX_GPIO(GPIO_FN_STATUS1, STATUS1_MARK),
-	PINMUX_GPIO(GPIO_FN_DRAK1_PK2, DRAK1_PK2_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK2, DACK2_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_TXD, SCIF2_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_MMCCMD, MMCCMD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF_TXD_PK, SIOF_TXD_PK_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK3, DACK3_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_SCK, SCIF2_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_MMCDAT, MMCDAT_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF_SCK_PK, SIOF_SCK_PK_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ0, DREQ0_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ1, DREQ1_MARK),
-	PINMUX_GPIO(GPIO_FN_DRAK0_PK1, DRAK0_PK1_MARK),
-	PINMUX_GPIO(GPIO_FN_DRAK1_PK0, DRAK1_PK0_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ2, DREQ2_MARK),
-	PINMUX_GPIO(GPIO_FN_INTB, INTB_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ3, DREQ3_MARK),
-	PINMUX_GPIO(GPIO_FN_INTC, INTC_MARK),
-	PINMUX_GPIO(GPIO_FN_DRAK2, DRAK2_MARK),
-	PINMUX_GPIO(GPIO_FN_CE2A, CE2A_MARK),
-	PINMUX_GPIO(GPIO_FN_IRL4, IRL4_MARK),
-	PINMUX_GPIO(GPIO_FN_FD4, FD4_MARK),
-	PINMUX_GPIO(GPIO_FN_IRL5, IRL5_MARK),
-	PINMUX_GPIO(GPIO_FN_FD5, FD5_MARK),
-	PINMUX_GPIO(GPIO_FN_IRL6, IRL6_MARK),
-	PINMUX_GPIO(GPIO_FN_FD6, FD6_MARK),
-	PINMUX_GPIO(GPIO_FN_IRL7, IRL7_MARK),
-	PINMUX_GPIO(GPIO_FN_FD7, FD7_MARK),
-	PINMUX_GPIO(GPIO_FN_DRAK3, DRAK3_MARK),
-	PINMUX_GPIO(GPIO_FN_CE2B, CE2B_MARK),
-	PINMUX_GPIO(GPIO_FN_BREQ_BSACK, BREQ_BSACK_MARK),
-	PINMUX_GPIO(GPIO_FN_BACK_BSREQ, BACK_BSREQ_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF5_RXD, SCIF5_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC1_SDIN, HAC1_SDIN_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI1_SCK, SSI1_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF5_SCK, SCIF5_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC1_SDOUT, HAC1_SDOUT_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI1_SDATA, SSI1_SDATA_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_TXD, SCIF3_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_FCLE, FCLE_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_RXD, SCIF3_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_FALE, FALE_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_SCK, SCIF3_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_FD0, FD0_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF4_TXD, SCIF4_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_FD1, FD1_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF4_RXD, SCIF4_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_FD2, FD2_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF4_SCK, SCIF4_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_FD3, FD3_MARK),
-	PINMUX_GPIO(GPIO_FN_DEVSEL_DCLKOUT, DEVSEL_DCLKOUT_MARK),
-	PINMUX_GPIO(GPIO_FN_STOP_CDE, STOP_CDE_MARK),
-	PINMUX_GPIO(GPIO_FN_LOCK_ODDF, LOCK_ODDF_MARK),
-	PINMUX_GPIO(GPIO_FN_TRDY_DISPL, TRDY_DISPL_MARK),
-	PINMUX_GPIO(GPIO_FN_IRDY_HSYNC, IRDY_HSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_PCIFRAME_VSYNC, PCIFRAME_VSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_INTA, INTA_MARK),
-	PINMUX_GPIO(GPIO_FN_GNT0_GNTIN, GNT0_GNTIN_MARK),
-	PINMUX_GPIO(GPIO_FN_REQ0_REQOUT, REQ0_REQOUT_MARK),
-	PINMUX_GPIO(GPIO_FN_PERR, PERR_MARK),
-	PINMUX_GPIO(GPIO_FN_SERR, SERR_MARK),
-	PINMUX_GPIO(GPIO_FN_WE7_CBE3, WE7_CBE3_MARK),
-	PINMUX_GPIO(GPIO_FN_WE6_CBE2, WE6_CBE2_MARK),
-	PINMUX_GPIO(GPIO_FN_WE5_CBE1, WE5_CBE1_MARK),
-	PINMUX_GPIO(GPIO_FN_WE4_CBE0, WE4_CBE0_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF2_RXD, SCIF2_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SIOF_RXD, SIOF_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_MRESETOUT, MRESETOUT_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQOUT, IRQOUT_MARK),
+	GPIO_FN(D63_AD31),
+	GPIO_FN(D62_AD30),
+	GPIO_FN(D61_AD29),
+	GPIO_FN(D60_AD28),
+	GPIO_FN(D59_AD27),
+	GPIO_FN(D58_AD26),
+	GPIO_FN(D57_AD25),
+	GPIO_FN(D56_AD24),
+	GPIO_FN(D55_AD23),
+	GPIO_FN(D54_AD22),
+	GPIO_FN(D53_AD21),
+	GPIO_FN(D52_AD20),
+	GPIO_FN(D51_AD19),
+	GPIO_FN(D50_AD18),
+	GPIO_FN(D49_AD17_DB5),
+	GPIO_FN(D48_AD16_DB4),
+	GPIO_FN(D47_AD15_DB3),
+	GPIO_FN(D46_AD14_DB2),
+	GPIO_FN(D45_AD13_DB1),
+	GPIO_FN(D44_AD12_DB0),
+	GPIO_FN(D43_AD11_DG5),
+	GPIO_FN(D42_AD10_DG4),
+	GPIO_FN(D41_AD9_DG3),
+	GPIO_FN(D40_AD8_DG2),
+	GPIO_FN(D39_AD7_DG1),
+	GPIO_FN(D38_AD6_DG0),
+	GPIO_FN(D37_AD5_DR5),
+	GPIO_FN(D36_AD4_DR4),
+	GPIO_FN(D35_AD3_DR3),
+	GPIO_FN(D34_AD2_DR2),
+	GPIO_FN(D33_AD1_DR1),
+	GPIO_FN(D32_AD0_DR0),
+	GPIO_FN(REQ1),
+	GPIO_FN(REQ2),
+	GPIO_FN(REQ3),
+	GPIO_FN(GNT1),
+	GPIO_FN(GNT2),
+	GPIO_FN(GNT3),
+	GPIO_FN(MMCCLK),
+	GPIO_FN(D31),
+	GPIO_FN(D30),
+	GPIO_FN(D29),
+	GPIO_FN(D28),
+	GPIO_FN(D27),
+	GPIO_FN(D26),
+	GPIO_FN(D25),
+	GPIO_FN(D24),
+	GPIO_FN(D23),
+	GPIO_FN(D22),
+	GPIO_FN(D21),
+	GPIO_FN(D20),
+	GPIO_FN(D19),
+	GPIO_FN(D18),
+	GPIO_FN(D17),
+	GPIO_FN(D16),
+	GPIO_FN(SCIF1_SCK),
+	GPIO_FN(SCIF1_RXD),
+	GPIO_FN(SCIF1_TXD),
+	GPIO_FN(SCIF0_CTS),
+	GPIO_FN(INTD),
+	GPIO_FN(FCE),
+	GPIO_FN(SCIF0_RTS),
+	GPIO_FN(HSPI_CS),
+	GPIO_FN(FSE),
+	GPIO_FN(SCIF0_SCK),
+	GPIO_FN(HSPI_CLK),
+	GPIO_FN(FRE),
+	GPIO_FN(SCIF0_RXD),
+	GPIO_FN(HSPI_RX),
+	GPIO_FN(FRB),
+	GPIO_FN(SCIF0_TXD),
+	GPIO_FN(HSPI_TX),
+	GPIO_FN(FWE),
+	GPIO_FN(SCIF5_TXD),
+	GPIO_FN(HAC1_SYNC),
+	GPIO_FN(SSI1_WS),
+	GPIO_FN(SIOF_TXD_PJ),
+	GPIO_FN(HAC0_SDOUT),
+	GPIO_FN(SSI0_SDATA),
+	GPIO_FN(SIOF_RXD_PJ),
+	GPIO_FN(HAC0_SDIN),
+	GPIO_FN(SSI0_SCK),
+	GPIO_FN(SIOF_SYNC_PJ),
+	GPIO_FN(HAC0_SYNC),
+	GPIO_FN(SSI0_WS),
+	GPIO_FN(SIOF_MCLK_PJ),
+	GPIO_FN(HAC_RES),
+	GPIO_FN(SIOF_SCK_PJ),
+	GPIO_FN(HAC0_BITCLK),
+	GPIO_FN(SSI0_CLK),
+	GPIO_FN(HAC1_BITCLK),
+	GPIO_FN(SSI1_CLK),
+	GPIO_FN(TCLK),
+	GPIO_FN(IOIS16),
+	GPIO_FN(STATUS0),
+	GPIO_FN(DRAK0_PK3),
+	GPIO_FN(STATUS1),
+	GPIO_FN(DRAK1_PK2),
+	GPIO_FN(DACK2),
+	GPIO_FN(SCIF2_TXD),
+	GPIO_FN(MMCCMD),
+	GPIO_FN(SIOF_TXD_PK),
+	GPIO_FN(DACK3),
+	GPIO_FN(SCIF2_SCK),
+	GPIO_FN(MMCDAT),
+	GPIO_FN(SIOF_SCK_PK),
+	GPIO_FN(DREQ0),
+	GPIO_FN(DREQ1),
+	GPIO_FN(DRAK0_PK1),
+	GPIO_FN(DRAK1_PK0),
+	GPIO_FN(DREQ2),
+	GPIO_FN(INTB),
+	GPIO_FN(DREQ3),
+	GPIO_FN(INTC),
+	GPIO_FN(DRAK2),
+	GPIO_FN(CE2A),
+	GPIO_FN(IRL4),
+	GPIO_FN(FD4),
+	GPIO_FN(IRL5),
+	GPIO_FN(FD5),
+	GPIO_FN(IRL6),
+	GPIO_FN(FD6),
+	GPIO_FN(IRL7),
+	GPIO_FN(FD7),
+	GPIO_FN(DRAK3),
+	GPIO_FN(CE2B),
+	GPIO_FN(BREQ_BSACK),
+	GPIO_FN(BACK_BSREQ),
+	GPIO_FN(SCIF5_RXD),
+	GPIO_FN(HAC1_SDIN),
+	GPIO_FN(SSI1_SCK),
+	GPIO_FN(SCIF5_SCK),
+	GPIO_FN(HAC1_SDOUT),
+	GPIO_FN(SSI1_SDATA),
+	GPIO_FN(SCIF3_TXD),
+	GPIO_FN(FCLE),
+	GPIO_FN(SCIF3_RXD),
+	GPIO_FN(FALE),
+	GPIO_FN(SCIF3_SCK),
+	GPIO_FN(FD0),
+	GPIO_FN(SCIF4_TXD),
+	GPIO_FN(FD1),
+	GPIO_FN(SCIF4_RXD),
+	GPIO_FN(FD2),
+	GPIO_FN(SCIF4_SCK),
+	GPIO_FN(FD3),
+	GPIO_FN(DEVSEL_DCLKOUT),
+	GPIO_FN(STOP_CDE),
+	GPIO_FN(LOCK_ODDF),
+	GPIO_FN(TRDY_DISPL),
+	GPIO_FN(IRDY_HSYNC),
+	GPIO_FN(PCIFRAME_VSYNC),
+	GPIO_FN(INTA),
+	GPIO_FN(GNT0_GNTIN),
+	GPIO_FN(REQ0_REQOUT),
+	GPIO_FN(PERR),
+	GPIO_FN(SERR),
+	GPIO_FN(WE7_CBE3),
+	GPIO_FN(WE6_CBE2),
+	GPIO_FN(WE5_CBE1),
+	GPIO_FN(WE4_CBE0),
+	GPIO_FN(SCIF2_RXD),
+	GPIO_FN(SIOF_RXD),
+	GPIO_FN(MRESETOUT),
+	GPIO_FN(IRQOUT),
 };
 
-static struct pinmux_cfg_reg pinmux_config_regs[] = {
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ PINMUX_CFG_REG("PACR", 0xffe70000, 16, 2) {
 		PA7_FN, PA7_OUT, PA7_IN, PA7_IN_PU,
 		PA6_FN, PA6_OUT, PA6_IN, PA6_IN_PU,
@@ -1214,7 +1218,7 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{}
 };
 
-static struct pinmux_data_reg pinmux_data_regs[] = {
+static const struct pinmux_data_reg pinmux_data_regs[] = {
 	{ PINMUX_DATA_REG("PADR", 0xffe70020, 8) {
 		PA7_DATA, PA6_DATA, PA5_DATA, PA4_DATA,
 		PA3_DATA, PA2_DATA, PA1_DATA, PA0_DATA }
@@ -1282,20 +1286,18 @@ static struct pinmux_data_reg pinmux_data_regs[] = {
 	{ },
 };
 
-struct sh_pfc_soc_info sh7785_pinmux_info = {
+const struct sh_pfc_soc_info sh7785_pinmux_info = {
 	.name = "sh7785_pfc",
-	.reserved_id = PINMUX_RESERVED,
-	.data = { PINMUX_DATA_BEGIN, PINMUX_DATA_END },
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END },
 	.input_pu = { PINMUX_INPUT_PULLUP_BEGIN, PINMUX_INPUT_PULLUP_END },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END },
-	.mark = { PINMUX_MARK_BEGIN, PINMUX_MARK_END },
 	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
 
-	.first_gpio = GPIO_PA7,
-	.last_gpio = GPIO_FN_IRQOUT,
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.func_gpios = pinmux_func_gpios,
+	.nr_func_gpios = ARRAY_SIZE(pinmux_func_gpios),
 
-	.gpios = pinmux_gpios,
 	.cfg_regs = pinmux_config_regs,
 	.data_regs = pinmux_data_regs,
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh7786.c b/drivers/pinctrl/sh-pfc/pfc-sh7786.c
index 1e18b58f9e5f..8ae0e32844e9 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh7786.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh7786.c
@@ -191,7 +191,7 @@ enum {
 	PINMUX_MARK_END,
 };
 
-static pinmux_enum_t pinmux_data[] = {
+static const pinmux_enum_t pinmux_data[] = {
 
 	/* PA GPIO */
 	PINMUX_DATA(PA7_DATA, PA7_IN, PA7_OUT, PA7_IN_PU),
@@ -427,7 +427,7 @@ static pinmux_enum_t pinmux_data[] = {
 	PINMUX_DATA(SSI3_SCK_MARK,	P2MSEL6_1, P2MSEL5_1, PJ1_FN),
 };
 
-static struct pinmux_gpio pinmux_gpios[] = {
+static struct sh_pfc_pin pinmux_pins[] = {
 	/* PA */
 	PINMUX_GPIO(GPIO_PA7, PA7_DATA),
 	PINMUX_GPIO(GPIO_PA6, PA6_DATA),
@@ -505,147 +505,151 @@ static struct pinmux_gpio pinmux_gpios[] = {
 	PINMUX_GPIO(GPIO_PJ3, PJ3_DATA),
 	PINMUX_GPIO(GPIO_PJ2, PJ2_DATA),
 	PINMUX_GPIO(GPIO_PJ1, PJ1_DATA),
+};
+
+#define PINMUX_FN_BASE	ARRAY_SIZE(pinmux_pins)
 
+static const struct pinmux_func pinmux_func_gpios[] = {
 	/* FN */
-	PINMUX_GPIO(GPIO_FN_CDE,		CDE_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_MAGIC,		ETH_MAGIC_MARK),
-	PINMUX_GPIO(GPIO_FN_DISP,		DISP_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_LINK,		ETH_LINK_MARK),
-	PINMUX_GPIO(GPIO_FN_DR5,		DR5_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_TX_ER,		ETH_TX_ER_MARK),
-	PINMUX_GPIO(GPIO_FN_DR4,		DR4_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_TX_EN,		ETH_TX_EN_MARK),
-	PINMUX_GPIO(GPIO_FN_DR3,		DR3_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_TXD3,		ETH_TXD3_MARK),
-	PINMUX_GPIO(GPIO_FN_DR2,		DR2_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_TXD2,		ETH_TXD2_MARK),
-	PINMUX_GPIO(GPIO_FN_DR1,		DR1_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_TXD1,		ETH_TXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_DR0,		DR0_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_TXD0,		ETH_TXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_VSYNC,		VSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_HSPI_CLK,		HSPI_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_ODDF,		ODDF_MARK),
-	PINMUX_GPIO(GPIO_FN_HSPI_CS,		HSPI_CS_MARK),
-	PINMUX_GPIO(GPIO_FN_DG5,		DG5_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_MDIO,		ETH_MDIO_MARK),
-	PINMUX_GPIO(GPIO_FN_DG4,		DG4_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_RX_CLK,		ETH_RX_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_DG3,		DG3_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_MDC,		ETH_MDC_MARK),
-	PINMUX_GPIO(GPIO_FN_DG2,		DG2_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_COL,		ETH_COL_MARK),
-	PINMUX_GPIO(GPIO_FN_DG1,		DG1_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_TX_CLK,		ETH_TX_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_DG0,		DG0_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_CRS,		ETH_CRS_MARK),
-	PINMUX_GPIO(GPIO_FN_DCLKIN,		DCLKIN_MARK),
-	PINMUX_GPIO(GPIO_FN_HSPI_RX,		HSPI_RX_MARK),
-	PINMUX_GPIO(GPIO_FN_HSYNC,		HSYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_HSPI_TX,		HSPI_TX_MARK),
-	PINMUX_GPIO(GPIO_FN_DB5,		DB5_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_RXD3,		ETH_RXD3_MARK),
-	PINMUX_GPIO(GPIO_FN_DB4,		DB4_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_RXD2,		ETH_RXD2_MARK),
-	PINMUX_GPIO(GPIO_FN_DB3,		DB3_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_RXD1,		ETH_RXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_DB2,		DB2_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_RXD0,		ETH_RXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_DB1,		DB1_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_RX_DV,		ETH_RX_DV_MARK),
-	PINMUX_GPIO(GPIO_FN_DB0,		DB0_MARK),
-	PINMUX_GPIO(GPIO_FN_ETH_RX_ER,		ETH_RX_ER_MARK),
-	PINMUX_GPIO(GPIO_FN_DCLKOUT,		DCLKOUT_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_SCK,		SCIF1_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_RXD,		SCIF1_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF1_TXD,		SCIF1_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK1,		DACK1_MARK),
-	PINMUX_GPIO(GPIO_FN_BACK,		BACK_MARK),
-	PINMUX_GPIO(GPIO_FN_FALE,		FALE_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK0,		DACK0_MARK),
-	PINMUX_GPIO(GPIO_FN_FCLE,		FCLE_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ1,		DREQ1_MARK),
-	PINMUX_GPIO(GPIO_FN_BREQ,		BREQ_MARK),
-	PINMUX_GPIO(GPIO_FN_USB_OVC1,		USB_OVC1_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ0,		DREQ0_MARK),
-	PINMUX_GPIO(GPIO_FN_USB_OVC0,		USB_OVC0_MARK),
-	PINMUX_GPIO(GPIO_FN_USB_PENC1,		USB_PENC1_MARK),
-	PINMUX_GPIO(GPIO_FN_USB_PENC0,		USB_PENC0_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC1_SDOUT,		HAC1_SDOUT_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI1_SDATA,		SSI1_SDATA_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF1CMD,		SDIF1CMD_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC1_SDIN,		HAC1_SDIN_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI1_SCK,		SSI1_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF1CD,		SDIF1CD_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC1_SYNC,		HAC1_SYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI1_WS,		SSI1_WS_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF1WP,		SDIF1WP_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC1_BITCLK,	HAC1_BITCLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI1_CLK,		SSI1_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF1CLK,		SDIF1CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC0_SDOUT,		HAC0_SDOUT_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI0_SDATA,		SSI0_SDATA_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF1D3,		SDIF1D3_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC0_SDIN,		HAC0_SDIN_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI0_SCK,		SSI0_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF1D2,		SDIF1D2_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC0_SYNC,		HAC0_SYNC_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI0_WS,		SSI0_WS_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF1D1,		SDIF1D1_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC0_BITCLK,	HAC0_BITCLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI0_CLK,		SSI0_CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF1D0,		SDIF1D0_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_SCK,		SCIF3_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI2_SDATA,		SSI2_SDATA_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_RXD,		SCIF3_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_TCLK,		TCLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI2_SCK,		SSI2_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF3_TXD,		SCIF3_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_HAC_RES,		HAC_RES_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI2_WS,		SSI2_WS_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK3,		DACK3_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF0CMD,		SDIF0CMD_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK2,		DACK2_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF0CD,		SDIF0CD_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ3,		DREQ3_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF0WP,		SDIF0WP_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_CTS,		SCIF0_CTS_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ2,		DREQ2_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF0CLK,		SDIF0CLK_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_RTS,		SCIF0_RTS_MARK),
-	PINMUX_GPIO(GPIO_FN_IRL7,		IRL7_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF0D3,		SDIF0D3_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_SCK,		SCIF0_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_IRL6,		IRL6_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF0D2,		SDIF0D2_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_RXD,		SCIF0_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_IRL5,		IRL5_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF0D1,		SDIF0D1_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF0_TXD,		SCIF0_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_IRL4,		IRL4_MARK),
-	PINMUX_GPIO(GPIO_FN_SDIF0D0,		SDIF0D0_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF5_SCK,		SCIF5_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_FRB,		FRB_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF5_RXD,		SCIF5_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_IOIS16,		IOIS16_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF5_TXD,		SCIF5_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_CE2B,		CE2B_MARK),
-	PINMUX_GPIO(GPIO_FN_DRAK3,		DRAK3_MARK),
-	PINMUX_GPIO(GPIO_FN_CE2A,		CE2A_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF4_SCK,		SCIF4_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_DRAK2,		DRAK2_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI3_WS,		SSI3_WS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF4_RXD,		SCIF4_RXD_MARK),
-	PINMUX_GPIO(GPIO_FN_DRAK1,		DRAK1_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI3_SDATA,		SSI3_SDATA_MARK),
-	PINMUX_GPIO(GPIO_FN_FSTATUS,		FSTATUS_MARK),
-	PINMUX_GPIO(GPIO_FN_SCIF4_TXD,		SCIF4_TXD_MARK),
-	PINMUX_GPIO(GPIO_FN_DRAK0,		DRAK0_MARK),
-	PINMUX_GPIO(GPIO_FN_SSI3_SCK,		SSI3_SCK_MARK),
-	PINMUX_GPIO(GPIO_FN_FSE,		FSE_MARK),
+	GPIO_FN(CDE),
+	GPIO_FN(ETH_MAGIC),
+	GPIO_FN(DISP),
+	GPIO_FN(ETH_LINK),
+	GPIO_FN(DR5),
+	GPIO_FN(ETH_TX_ER),
+	GPIO_FN(DR4),
+	GPIO_FN(ETH_TX_EN),
+	GPIO_FN(DR3),
+	GPIO_FN(ETH_TXD3),
+	GPIO_FN(DR2),
+	GPIO_FN(ETH_TXD2),
+	GPIO_FN(DR1),
+	GPIO_FN(ETH_TXD1),
+	GPIO_FN(DR0),
+	GPIO_FN(ETH_TXD0),
+	GPIO_FN(VSYNC),
+	GPIO_FN(HSPI_CLK),
+	GPIO_FN(ODDF),
+	GPIO_FN(HSPI_CS),
+	GPIO_FN(DG5),
+	GPIO_FN(ETH_MDIO),
+	GPIO_FN(DG4),
+	GPIO_FN(ETH_RX_CLK),
+	GPIO_FN(DG3),
+	GPIO_FN(ETH_MDC),
+	GPIO_FN(DG2),
+	GPIO_FN(ETH_COL),
+	GPIO_FN(DG1),
+	GPIO_FN(ETH_TX_CLK),
+	GPIO_FN(DG0),
+	GPIO_FN(ETH_CRS),
+	GPIO_FN(DCLKIN),
+	GPIO_FN(HSPI_RX),
+	GPIO_FN(HSYNC),
+	GPIO_FN(HSPI_TX),
+	GPIO_FN(DB5),
+	GPIO_FN(ETH_RXD3),
+	GPIO_FN(DB4),
+	GPIO_FN(ETH_RXD2),
+	GPIO_FN(DB3),
+	GPIO_FN(ETH_RXD1),
+	GPIO_FN(DB2),
+	GPIO_FN(ETH_RXD0),
+	GPIO_FN(DB1),
+	GPIO_FN(ETH_RX_DV),
+	GPIO_FN(DB0),
+	GPIO_FN(ETH_RX_ER),
+	GPIO_FN(DCLKOUT),
+	GPIO_FN(SCIF1_SCK),
+	GPIO_FN(SCIF1_RXD),
+	GPIO_FN(SCIF1_TXD),
+	GPIO_FN(DACK1),
+	GPIO_FN(BACK),
+	GPIO_FN(FALE),
+	GPIO_FN(DACK0),
+	GPIO_FN(FCLE),
+	GPIO_FN(DREQ1),
+	GPIO_FN(BREQ),
+	GPIO_FN(USB_OVC1),
+	GPIO_FN(DREQ0),
+	GPIO_FN(USB_OVC0),
+	GPIO_FN(USB_PENC1),
+	GPIO_FN(USB_PENC0),
+	GPIO_FN(HAC1_SDOUT),
+	GPIO_FN(SSI1_SDATA),
+	GPIO_FN(SDIF1CMD),
+	GPIO_FN(HAC1_SDIN),
+	GPIO_FN(SSI1_SCK),
+	GPIO_FN(SDIF1CD),
+	GPIO_FN(HAC1_SYNC),
+	GPIO_FN(SSI1_WS),
+	GPIO_FN(SDIF1WP),
+	GPIO_FN(HAC1_BITCLK),
+	GPIO_FN(SSI1_CLK),
+	GPIO_FN(SDIF1CLK),
+	GPIO_FN(HAC0_SDOUT),
+	GPIO_FN(SSI0_SDATA),
+	GPIO_FN(SDIF1D3),
+	GPIO_FN(HAC0_SDIN),
+	GPIO_FN(SSI0_SCK),
+	GPIO_FN(SDIF1D2),
+	GPIO_FN(HAC0_SYNC),
+	GPIO_FN(SSI0_WS),
+	GPIO_FN(SDIF1D1),
+	GPIO_FN(HAC0_BITCLK),
+	GPIO_FN(SSI0_CLK),
+	GPIO_FN(SDIF1D0),
+	GPIO_FN(SCIF3_SCK),
+	GPIO_FN(SSI2_SDATA),
+	GPIO_FN(SCIF3_RXD),
+	GPIO_FN(TCLK),
+	GPIO_FN(SSI2_SCK),
+	GPIO_FN(SCIF3_TXD),
+	GPIO_FN(HAC_RES),
+	GPIO_FN(SSI2_WS),
+	GPIO_FN(DACK3),
+	GPIO_FN(SDIF0CMD),
+	GPIO_FN(DACK2),
+	GPIO_FN(SDIF0CD),
+	GPIO_FN(DREQ3),
+	GPIO_FN(SDIF0WP),
+	GPIO_FN(SCIF0_CTS),
+	GPIO_FN(DREQ2),
+	GPIO_FN(SDIF0CLK),
+	GPIO_FN(SCIF0_RTS),
+	GPIO_FN(IRL7),
+	GPIO_FN(SDIF0D3),
+	GPIO_FN(SCIF0_SCK),
+	GPIO_FN(IRL6),
+	GPIO_FN(SDIF0D2),
+	GPIO_FN(SCIF0_RXD),
+	GPIO_FN(IRL5),
+	GPIO_FN(SDIF0D1),
+	GPIO_FN(SCIF0_TXD),
+	GPIO_FN(IRL4),
+	GPIO_FN(SDIF0D0),
+	GPIO_FN(SCIF5_SCK),
+	GPIO_FN(FRB),
+	GPIO_FN(SCIF5_RXD),
+	GPIO_FN(IOIS16),
+	GPIO_FN(SCIF5_TXD),
+	GPIO_FN(CE2B),
+	GPIO_FN(DRAK3),
+	GPIO_FN(CE2A),
+	GPIO_FN(SCIF4_SCK),
+	GPIO_FN(DRAK2),
+	GPIO_FN(SSI3_WS),
+	GPIO_FN(SCIF4_RXD),
+	GPIO_FN(DRAK1),
+	GPIO_FN(SSI3_SDATA),
+	GPIO_FN(FSTATUS),
+	GPIO_FN(SCIF4_TXD),
+	GPIO_FN(DRAK0),
+	GPIO_FN(SSI3_SCK),
+	GPIO_FN(FSE),
 };
 
-static struct pinmux_cfg_reg pinmux_config_regs[] = {
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ PINMUX_CFG_REG("PACR", 0xffcc0000, 16, 2) {
 		PA7_FN, PA7_OUT, PA7_IN, PA7_IN_PU,
 		PA6_FN, PA6_OUT, PA6_IN, PA6_IN_PU,
@@ -775,7 +779,7 @@ static struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{}
 };
 
-static struct pinmux_data_reg pinmux_data_regs[] = {
+static const struct pinmux_data_reg pinmux_data_regs[] = {
 	{ PINMUX_DATA_REG("PADR", 0xffcc0020, 8) {
 		PA7_DATA, PA6_DATA, PA5_DATA, PA4_DATA,
 		PA3_DATA, PA2_DATA, PA1_DATA, PA0_DATA }
@@ -815,20 +819,18 @@ static struct pinmux_data_reg pinmux_data_regs[] = {
 	{ },
 };
 
-struct sh_pfc_soc_info sh7786_pinmux_info = {
+const struct sh_pfc_soc_info sh7786_pinmux_info = {
 	.name = "sh7786_pfc",
-	.reserved_id = PINMUX_RESERVED,
-	.data = { PINMUX_DATA_BEGIN, PINMUX_DATA_END },
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END },
 	.input_pu = { PINMUX_INPUT_PULLUP_BEGIN, PINMUX_INPUT_PULLUP_END },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END },
-	.mark = { PINMUX_MARK_BEGIN, PINMUX_MARK_END },
 	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
 
-	.first_gpio = GPIO_PA7,
-	.last_gpio = GPIO_FN_IRL4,
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.func_gpios = pinmux_func_gpios,
+	.nr_func_gpios = ARRAY_SIZE(pinmux_func_gpios),
 
-	.gpios = pinmux_gpios,
 	.cfg_regs = pinmux_config_regs,
 	.data_regs = pinmux_data_regs,
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-shx3.c b/drivers/pinctrl/sh-pfc/pfc-shx3.c
index ccf6918b03c6..6594c8c48747 100644
--- a/drivers/pinctrl/sh-pfc/pfc-shx3.c
+++ b/drivers/pinctrl/sh-pfc/pfc-shx3.c
@@ -147,7 +147,7 @@ enum {
 	PINMUX_MARK_END,
 };
 
-static pinmux_enum_t shx3_pinmux_data[] = {
+static const pinmux_enum_t shx3_pinmux_data[] = {
 
 	/* PA GPIO */
 	PINMUX_DATA(PA7_DATA, PA7_IN, PA7_OUT, PA7_IN_PU),
@@ -306,7 +306,7 @@ static pinmux_enum_t shx3_pinmux_data[] = {
 	PINMUX_DATA(IRQOUT_MARK,	PH0_FN),
 };
 
-static struct pinmux_gpio shx3_pinmux_gpios[] = {
+static struct sh_pfc_pin shx3_pinmux_pins[] = {
 	/* PA */
 	PINMUX_GPIO(GPIO_PA7, PA7_DATA),
 	PINMUX_GPIO(GPIO_PA6, PA6_DATA),
@@ -384,73 +384,77 @@ static struct pinmux_gpio shx3_pinmux_gpios[] = {
 	PINMUX_GPIO(GPIO_PH2, PH2_DATA),
 	PINMUX_GPIO(GPIO_PH1, PH1_DATA),
 	PINMUX_GPIO(GPIO_PH0, PH0_DATA),
+};
+
+#define PINMUX_FN_BASE	ARRAY_SIZE(shx3_pinmux_pins)
 
+static const struct pinmux_func shx3_pinmux_func_gpios[] = {
 	/* FN */
-	PINMUX_GPIO(GPIO_FN_D31,	D31_MARK),
-	PINMUX_GPIO(GPIO_FN_D30,	D30_MARK),
-	PINMUX_GPIO(GPIO_FN_D29,	D29_MARK),
-	PINMUX_GPIO(GPIO_FN_D28,	D28_MARK),
-	PINMUX_GPIO(GPIO_FN_D27,	D27_MARK),
-	PINMUX_GPIO(GPIO_FN_D26,	D26_MARK),
-	PINMUX_GPIO(GPIO_FN_D25,	D25_MARK),
-	PINMUX_GPIO(GPIO_FN_D24,	D24_MARK),
-	PINMUX_GPIO(GPIO_FN_D23,	D23_MARK),
-	PINMUX_GPIO(GPIO_FN_D22,	D22_MARK),
-	PINMUX_GPIO(GPIO_FN_D21,	D21_MARK),
-	PINMUX_GPIO(GPIO_FN_D20,	D20_MARK),
-	PINMUX_GPIO(GPIO_FN_D19,	D19_MARK),
-	PINMUX_GPIO(GPIO_FN_D18,	D18_MARK),
-	PINMUX_GPIO(GPIO_FN_D17,	D17_MARK),
-	PINMUX_GPIO(GPIO_FN_D16,	D16_MARK),
-	PINMUX_GPIO(GPIO_FN_BACK,	BACK_MARK),
-	PINMUX_GPIO(GPIO_FN_BREQ,	BREQ_MARK),
-	PINMUX_GPIO(GPIO_FN_WE3,	WE3_MARK),
-	PINMUX_GPIO(GPIO_FN_WE2,	WE2_MARK),
-	PINMUX_GPIO(GPIO_FN_CS6,	CS6_MARK),
-	PINMUX_GPIO(GPIO_FN_CS5,	CS5_MARK),
-	PINMUX_GPIO(GPIO_FN_CS4,	CS4_MARK),
-	PINMUX_GPIO(GPIO_FN_CLKOUTENB,	CLKOUTENB_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK3,	DACK3_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK2,	DACK2_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK1,	DACK1_MARK),
-	PINMUX_GPIO(GPIO_FN_DACK0,	DACK0_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ3,	DREQ3_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ2,	DREQ2_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ1,	DREQ1_MARK),
-	PINMUX_GPIO(GPIO_FN_DREQ0,	DREQ0_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ3,	IRQ3_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ2,	IRQ2_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ1,	IRQ1_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQ0,	IRQ0_MARK),
-	PINMUX_GPIO(GPIO_FN_DRAK3,	DRAK3_MARK),
-	PINMUX_GPIO(GPIO_FN_DRAK2,	DRAK2_MARK),
-	PINMUX_GPIO(GPIO_FN_DRAK1,	DRAK1_MARK),
-	PINMUX_GPIO(GPIO_FN_DRAK0,	DRAK0_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK3,	SCK3_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK2,	SCK2_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK1,	SCK1_MARK),
-	PINMUX_GPIO(GPIO_FN_SCK0,	SCK0_MARK),
-	PINMUX_GPIO(GPIO_FN_IRL3,	IRL3_MARK),
-	PINMUX_GPIO(GPIO_FN_IRL2,	IRL2_MARK),
-	PINMUX_GPIO(GPIO_FN_IRL1,	IRL1_MARK),
-	PINMUX_GPIO(GPIO_FN_IRL0,	IRL0_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD3,	TXD3_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD2,	TXD2_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD1,	TXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_TXD0,	TXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD3,	RXD3_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD2,	RXD2_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD1,	RXD1_MARK),
-	PINMUX_GPIO(GPIO_FN_RXD0,	RXD0_MARK),
-	PINMUX_GPIO(GPIO_FN_CE2B,	CE2B_MARK),
-	PINMUX_GPIO(GPIO_FN_CE2A,	CE2A_MARK),
-	PINMUX_GPIO(GPIO_FN_IOIS16,	IOIS16_MARK),
-	PINMUX_GPIO(GPIO_FN_STATUS1,	STATUS1_MARK),
-	PINMUX_GPIO(GPIO_FN_STATUS0,	STATUS0_MARK),
-	PINMUX_GPIO(GPIO_FN_IRQOUT,	IRQOUT_MARK),
+	GPIO_FN(D31),
+	GPIO_FN(D30),
+	GPIO_FN(D29),
+	GPIO_FN(D28),
+	GPIO_FN(D27),
+	GPIO_FN(D26),
+	GPIO_FN(D25),
+	GPIO_FN(D24),
+	GPIO_FN(D23),
+	GPIO_FN(D22),
+	GPIO_FN(D21),
+	GPIO_FN(D20),
+	GPIO_FN(D19),
+	GPIO_FN(D18),
+	GPIO_FN(D17),
+	GPIO_FN(D16),
+	GPIO_FN(BACK),
+	GPIO_FN(BREQ),
+	GPIO_FN(WE3),
+	GPIO_FN(WE2),
+	GPIO_FN(CS6),
+	GPIO_FN(CS5),
+	GPIO_FN(CS4),
+	GPIO_FN(CLKOUTENB),
+	GPIO_FN(DACK3),
+	GPIO_FN(DACK2),
+	GPIO_FN(DACK1),
+	GPIO_FN(DACK0),
+	GPIO_FN(DREQ3),
+	GPIO_FN(DREQ2),
+	GPIO_FN(DREQ1),
+	GPIO_FN(DREQ0),
+	GPIO_FN(IRQ3),
+	GPIO_FN(IRQ2),
+	GPIO_FN(IRQ1),
+	GPIO_FN(IRQ0),
+	GPIO_FN(DRAK3),
+	GPIO_FN(DRAK2),
+	GPIO_FN(DRAK1),
+	GPIO_FN(DRAK0),
+	GPIO_FN(SCK3),
+	GPIO_FN(SCK2),
+	GPIO_FN(SCK1),
+	GPIO_FN(SCK0),
+	GPIO_FN(IRL3),
+	GPIO_FN(IRL2),
+	GPIO_FN(IRL1),
+	GPIO_FN(IRL0),
+	GPIO_FN(TXD3),
+	GPIO_FN(TXD2),
+	GPIO_FN(TXD1),
+	GPIO_FN(TXD0),
+	GPIO_FN(RXD3),
+	GPIO_FN(RXD2),
+	GPIO_FN(RXD1),
+	GPIO_FN(RXD0),
+	GPIO_FN(CE2B),
+	GPIO_FN(CE2A),
+	GPIO_FN(IOIS16),
+	GPIO_FN(STATUS1),
+	GPIO_FN(STATUS0),
+	GPIO_FN(IRQOUT),
 };
 
-static struct pinmux_cfg_reg shx3_pinmux_config_regs[] = {
+static const struct pinmux_cfg_reg shx3_pinmux_config_regs[] = {
 	{ PINMUX_CFG_REG("PABCR", 0xffc70000, 32, 2) {
 		PA7_FN, PA7_OUT, PA7_IN, PA7_IN_PU,
 		PA6_FN, PA6_OUT, PA6_IN, PA6_IN_PU,
@@ -526,7 +530,7 @@ static struct pinmux_cfg_reg shx3_pinmux_config_regs[] = {
 	{ },
 };
 
-static struct pinmux_data_reg shx3_pinmux_data_regs[] = {
+static const struct pinmux_data_reg shx3_pinmux_data_regs[] = {
 	{ PINMUX_DATA_REG("PABDR", 0xffc70010, 32) {
 		0, 0, 0, 0, 0, 0, 0, 0,
 		PA7_DATA, PA6_DATA, PA5_DATA, PA4_DATA,
@@ -562,19 +566,17 @@ static struct pinmux_data_reg shx3_pinmux_data_regs[] = {
 	{ },
 };
 
-struct sh_pfc_soc_info shx3_pinmux_info = {
+const struct sh_pfc_soc_info shx3_pinmux_info = {
 	.name		= "shx3_pfc",
-	.reserved_id	= PINMUX_RESERVED,
-	.data		= { PINMUX_DATA_BEGIN,	   PINMUX_DATA_END },
 	.input		= { PINMUX_INPUT_BEGIN,	   PINMUX_INPUT_END },
 	.input_pu	= { PINMUX_INPUT_PULLUP_BEGIN,
 			    PINMUX_INPUT_PULLUP_END },
 	.output		= { PINMUX_OUTPUT_BEGIN,   PINMUX_OUTPUT_END },
-	.mark		= { PINMUX_MARK_BEGIN,     PINMUX_MARK_END },
 	.function	= { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
-	.first_gpio	= GPIO_PA7,
-	.last_gpio	= GPIO_FN_STATUS0,
-	.gpios		= shx3_pinmux_gpios,
+	.pins		= shx3_pinmux_pins,
+	.nr_pins	= ARRAY_SIZE(shx3_pinmux_pins),
+	.func_gpios	= shx3_pinmux_func_gpios,
+	.nr_func_gpios	= ARRAY_SIZE(shx3_pinmux_func_gpios),
 	.gpio_data	= shx3_pinmux_data,
 	.gpio_data_size	= ARRAY_SIZE(shx3_pinmux_data),
 	.cfg_regs	= shx3_pinmux_config_regs,
diff --git a/drivers/pinctrl/sh-pfc/pinctrl.c b/drivers/pinctrl/sh-pfc/pinctrl.c
index 11e0e1374d65..3492ec9a33b7 100644
--- a/drivers/pinctrl/sh-pfc/pinctrl.c
+++ b/drivers/pinctrl/sh-pfc/pinctrl.c
@@ -9,7 +9,6 @@
  */
 
 #define DRV_NAME "sh-pfc"
-#define pr_fmt(fmt) KBUILD_MODNAME " pinctrl: " fmt
 
 #include <linux/device.h>
 #include <linux/err.h>
@@ -24,25 +23,28 @@
 #include <linux/spinlock.h>
 
 #include "core.h"
+#include "../core.h"
+#include "../pinconf.h"
+
+struct sh_pfc_pin_config {
+	u32 type;
+};
 
 struct sh_pfc_pinctrl {
 	struct pinctrl_dev *pctl;
-	struct sh_pfc *pfc;
-
-	struct pinmux_gpio **functions;
-	unsigned int nr_functions;
+	struct pinctrl_desc pctl_desc;
 
-	struct pinctrl_pin_desc *pads;
-	unsigned int nr_pads;
+	struct sh_pfc *pfc;
 
-	spinlock_t lock;
+	struct pinctrl_pin_desc *pins;
+	struct sh_pfc_pin_config *configs;
 };
 
 static int sh_pfc_get_groups_count(struct pinctrl_dev *pctldev)
 {
 	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
 
-	return pmx->nr_pads;
+	return pmx->pfc->info->nr_groups;
 }
 
 static const char *sh_pfc_get_group_name(struct pinctrl_dev *pctldev,
@@ -50,16 +52,16 @@ static const char *sh_pfc_get_group_name(struct pinctrl_dev *pctldev,
 {
 	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
 
-	return pmx->pads[selector].name;
+	return pmx->pfc->info->groups[selector].name;
 }
 
-static int sh_pfc_get_group_pins(struct pinctrl_dev *pctldev, unsigned group,
+static int sh_pfc_get_group_pins(struct pinctrl_dev *pctldev, unsigned selector,
 				 const unsigned **pins, unsigned *num_pins)
 {
 	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
 
-	*pins = &pmx->pads[group].number;
-	*num_pins = 1;
+	*pins = pmx->pfc->info->groups[selector].pins;
+	*num_pins = pmx->pfc->info->groups[selector].nr_pins;
 
 	return 0;
 }
@@ -70,7 +72,7 @@ static void sh_pfc_pin_dbg_show(struct pinctrl_dev *pctldev, struct seq_file *s,
 	seq_printf(s, "%s", DRV_NAME);
 }
 
-static struct pinctrl_ops sh_pfc_pinctrl_ops = {
+static const struct pinctrl_ops sh_pfc_pinctrl_ops = {
 	.get_groups_count	= sh_pfc_get_groups_count,
 	.get_group_name		= sh_pfc_get_group_name,
 	.get_group_pins		= sh_pfc_get_group_pins,
@@ -81,7 +83,7 @@ static int sh_pfc_get_functions_count(struct pinctrl_dev *pctldev)
 {
 	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
 
-	return pmx->nr_functions;
+	return pmx->pfc->info->nr_functions;
 }
 
 static const char *sh_pfc_get_function_name(struct pinctrl_dev *pctldev,
@@ -89,136 +91,113 @@ static const char *sh_pfc_get_function_name(struct pinctrl_dev *pctldev,
 {
 	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
 
-	return pmx->functions[selector]->name;
+	return pmx->pfc->info->functions[selector].name;
 }
 
-static int sh_pfc_get_function_groups(struct pinctrl_dev *pctldev, unsigned func,
+static int sh_pfc_get_function_groups(struct pinctrl_dev *pctldev,
+				      unsigned selector,
 				      const char * const **groups,
 				      unsigned * const num_groups)
 {
 	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
 
-	*groups = &pmx->functions[func]->name;
-	*num_groups = 1;
+	*groups = pmx->pfc->info->functions[selector].groups;
+	*num_groups = pmx->pfc->info->functions[selector].nr_groups;
 
 	return 0;
 }
 
-static int sh_pfc_noop_enable(struct pinctrl_dev *pctldev, unsigned func,
+static int sh_pfc_func_enable(struct pinctrl_dev *pctldev, unsigned selector,
 			      unsigned group)
 {
-	return 0;
-}
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+	struct sh_pfc *pfc = pmx->pfc;
+	const struct sh_pfc_pin_group *grp = &pfc->info->groups[group];
+	unsigned long flags;
+	unsigned int i;
+	int ret = 0;
 
-static void sh_pfc_noop_disable(struct pinctrl_dev *pctldev, unsigned func,
-				unsigned group)
-{
-}
+	spin_lock_irqsave(&pfc->lock, flags);
 
-static int sh_pfc_config_function(struct sh_pfc *pfc, unsigned offset)
-{
-	if (sh_pfc_config_gpio(pfc, offset,
-			       PINMUX_TYPE_FUNCTION,
-			       GPIO_CFG_DRYRUN) != 0)
-		return -EINVAL;
+	for (i = 0; i < grp->nr_pins; ++i) {
+		int idx = sh_pfc_get_pin_index(pfc, grp->pins[i]);
+		struct sh_pfc_pin_config *cfg = &pmx->configs[idx];
 
-	if (sh_pfc_config_gpio(pfc, offset,
-			       PINMUX_TYPE_FUNCTION,
-			       GPIO_CFG_REQ) != 0)
-		return -EINVAL;
+		if (cfg->type != PINMUX_TYPE_NONE) {
+			ret = -EBUSY;
+			goto done;
+		}
+	}
 
-	return 0;
+	for (i = 0; i < grp->nr_pins; ++i) {
+		ret = sh_pfc_config_mux(pfc, grp->mux[i], PINMUX_TYPE_FUNCTION);
+		if (ret < 0)
+			break;
+	}
+
+done:
+	spin_unlock_irqrestore(&pfc->lock, flags);
+	return ret;
 }
 
-static int sh_pfc_reconfig_pin(struct sh_pfc *pfc, unsigned offset,
-			       int new_type)
+static void sh_pfc_func_disable(struct pinctrl_dev *pctldev, unsigned selector,
+				unsigned group)
 {
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+	struct sh_pfc *pfc = pmx->pfc;
+	const struct sh_pfc_pin_group *grp = &pfc->info->groups[group];
 	unsigned long flags;
-	int pinmux_type;
-	int ret = -EINVAL;
+	unsigned int i;
 
 	spin_lock_irqsave(&pfc->lock, flags);
 
-	pinmux_type = pfc->info->gpios[offset].flags & PINMUX_FLAG_TYPE;
+	for (i = 0; i < grp->nr_pins; ++i) {
+		int idx = sh_pfc_get_pin_index(pfc, grp->pins[i]);
+		struct sh_pfc_pin_config *cfg = &pmx->configs[idx];
 
-	/*
-	 * See if the present config needs to first be de-configured.
-	 */
-	switch (pinmux_type) {
-	case PINMUX_TYPE_GPIO:
-		break;
-	case PINMUX_TYPE_OUTPUT:
-	case PINMUX_TYPE_INPUT:
-	case PINMUX_TYPE_INPUT_PULLUP:
-	case PINMUX_TYPE_INPUT_PULLDOWN:
-		sh_pfc_config_gpio(pfc, offset, pinmux_type, GPIO_CFG_FREE);
-		break;
-	default:
-		goto err;
+		cfg->type = PINMUX_TYPE_NONE;
 	}
 
-	/*
-	 * Dry run
-	 */
-	if (sh_pfc_config_gpio(pfc, offset, new_type,
-			       GPIO_CFG_DRYRUN) != 0)
-		goto err;
-
-	/*
-	 * Request
-	 */
-	if (sh_pfc_config_gpio(pfc, offset, new_type,
-			       GPIO_CFG_REQ) != 0)
-		goto err;
-
-	pfc->info->gpios[offset].flags &= ~PINMUX_FLAG_TYPE;
-	pfc->info->gpios[offset].flags |= new_type;
-
-	ret = 0;
-
-err:
 	spin_unlock_irqrestore(&pfc->lock, flags);
-
-	return ret;
 }
 
-
 static int sh_pfc_gpio_request_enable(struct pinctrl_dev *pctldev,
 				      struct pinctrl_gpio_range *range,
 				      unsigned offset)
 {
 	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
 	struct sh_pfc *pfc = pmx->pfc;
+	int idx = sh_pfc_get_pin_index(pfc, offset);
+	struct sh_pfc_pin_config *cfg = &pmx->configs[idx];
 	unsigned long flags;
-	int ret, pinmux_type;
+	int ret;
 
 	spin_lock_irqsave(&pfc->lock, flags);
 
-	pinmux_type = pfc->info->gpios[offset].flags & PINMUX_FLAG_TYPE;
+	if (cfg->type != PINMUX_TYPE_NONE) {
+		dev_err(pfc->dev,
+			"Pin %u is busy, can't configure it as GPIO.\n",
+			offset);
+		ret = -EBUSY;
+		goto done;
+	}
 
-	switch (pinmux_type) {
-	case PINMUX_TYPE_FUNCTION:
-		pr_notice_once("Use of GPIO API for function requests is "
-			       "deprecated, convert to pinctrl\n");
-		/* handle for now */
-		ret = sh_pfc_config_function(pfc, offset);
-		if (unlikely(ret < 0))
-			goto err;
+	if (!pfc->gpio) {
+		/* If GPIOs are handled externally the pin mux type need to be
+		 * set to GPIO here.
+		 */
+		const struct sh_pfc_pin *pin = &pfc->info->pins[idx];
 
-		break;
-	case PINMUX_TYPE_GPIO:
-	case PINMUX_TYPE_INPUT:
-	case PINMUX_TYPE_OUTPUT:
-		break;
-	default:
-		pr_err("Unsupported mux type (%d), bailing...\n", pinmux_type);
-		ret = -ENOTSUPP;
-		goto err;
+		ret = sh_pfc_config_mux(pfc, pin->enum_id, PINMUX_TYPE_GPIO);
+		if (ret < 0)
+			goto done;
 	}
 
+	cfg->type = PINMUX_TYPE_GPIO;
+
 	ret = 0;
 
-err:
+done:
 	spin_unlock_irqrestore(&pfc->lock, flags);
 
 	return ret;
@@ -230,15 +209,12 @@ static void sh_pfc_gpio_disable_free(struct pinctrl_dev *pctldev,
 {
 	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
 	struct sh_pfc *pfc = pmx->pfc;
+	int idx = sh_pfc_get_pin_index(pfc, offset);
+	struct sh_pfc_pin_config *cfg = &pmx->configs[idx];
 	unsigned long flags;
-	int pinmux_type;
 
 	spin_lock_irqsave(&pfc->lock, flags);
-
-	pinmux_type = pfc->info->gpios[offset].flags & PINMUX_FLAG_TYPE;
-
-	sh_pfc_config_gpio(pfc, offset, pinmux_type, GPIO_CFG_FREE);
-
+	cfg->type = PINMUX_TYPE_NONE;
 	spin_unlock_irqrestore(&pfc->lock, flags);
 }
 
@@ -247,207 +223,242 @@ static int sh_pfc_gpio_set_direction(struct pinctrl_dev *pctldev,
 				     unsigned offset, bool input)
 {
 	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
-	int type = input ? PINMUX_TYPE_INPUT : PINMUX_TYPE_OUTPUT;
+	struct sh_pfc *pfc = pmx->pfc;
+	int new_type = input ? PINMUX_TYPE_INPUT : PINMUX_TYPE_OUTPUT;
+	int idx = sh_pfc_get_pin_index(pfc, offset);
+	const struct sh_pfc_pin *pin = &pfc->info->pins[idx];
+	struct sh_pfc_pin_config *cfg = &pmx->configs[idx];
+	unsigned long flags;
+	unsigned int dir;
+	int ret;
+
+	/* Check if the requested direction is supported by the pin. Not all SoC
+	 * provide pin config data, so perform the check conditionally.
+	 */
+	if (pin->configs) {
+		dir = input ? SH_PFC_PIN_CFG_INPUT : SH_PFC_PIN_CFG_OUTPUT;
+		if (!(pin->configs & dir))
+			return -EINVAL;
+	}
+
+	spin_lock_irqsave(&pfc->lock, flags);
+
+	ret = sh_pfc_config_mux(pfc, pin->enum_id, new_type);
+	if (ret < 0)
+		goto done;
+
+	cfg->type = new_type;
 
-	return sh_pfc_reconfig_pin(pmx->pfc, offset, type);
+done:
+	spin_unlock_irqrestore(&pfc->lock, flags);
+	return ret;
 }
 
-static struct pinmux_ops sh_pfc_pinmux_ops = {
+static const struct pinmux_ops sh_pfc_pinmux_ops = {
 	.get_functions_count	= sh_pfc_get_functions_count,
 	.get_function_name	= sh_pfc_get_function_name,
 	.get_function_groups	= sh_pfc_get_function_groups,
-	.enable			= sh_pfc_noop_enable,
-	.disable		= sh_pfc_noop_disable,
+	.enable			= sh_pfc_func_enable,
+	.disable		= sh_pfc_func_disable,
 	.gpio_request_enable	= sh_pfc_gpio_request_enable,
 	.gpio_disable_free	= sh_pfc_gpio_disable_free,
 	.gpio_set_direction	= sh_pfc_gpio_set_direction,
 };
 
-static int sh_pfc_pinconf_get(struct pinctrl_dev *pctldev, unsigned pin,
-			      unsigned long *config)
+/* Check whether the requested parameter is supported for a pin. */
+static bool sh_pfc_pinconf_validate(struct sh_pfc *pfc, unsigned int _pin,
+				    enum pin_config_param param)
 {
-	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
-	struct sh_pfc *pfc = pmx->pfc;
+	int idx = sh_pfc_get_pin_index(pfc, _pin);
+	const struct sh_pfc_pin *pin = &pfc->info->pins[idx];
 
-	*config = pfc->info->gpios[pin].flags & PINMUX_FLAG_TYPE;
+	switch (param) {
+	case PIN_CONFIG_BIAS_DISABLE:
+		return true;
 
-	return 0;
-}
+	case PIN_CONFIG_BIAS_PULL_UP:
+		return pin->configs & SH_PFC_PIN_CFG_PULL_UP;
 
-static int sh_pfc_pinconf_set(struct pinctrl_dev *pctldev, unsigned pin,
-			      unsigned long config)
-{
-	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
-
-	/* Validate the new type */
-	if (config >= PINMUX_FLAG_TYPE)
-		return -EINVAL;
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		return pin->configs & SH_PFC_PIN_CFG_PULL_DOWN;
 
-	return sh_pfc_reconfig_pin(pmx->pfc, pin, config);
+	default:
+		return false;
+	}
 }
 
-static void sh_pfc_pinconf_dbg_show(struct pinctrl_dev *pctldev,
-				    struct seq_file *s, unsigned pin)
+static int sh_pfc_pinconf_get(struct pinctrl_dev *pctldev, unsigned _pin,
+			      unsigned long *config)
 {
-	const char *pinmux_type_str[] = {
-		[PINMUX_TYPE_NONE]		= "none",
-		[PINMUX_TYPE_FUNCTION]		= "function",
-		[PINMUX_TYPE_GPIO]		= "gpio",
-		[PINMUX_TYPE_OUTPUT]		= "output",
-		[PINMUX_TYPE_INPUT]		= "input",
-		[PINMUX_TYPE_INPUT_PULLUP]	= "input bias pull up",
-		[PINMUX_TYPE_INPUT_PULLDOWN]	= "input bias pull down",
-	};
-	unsigned long config;
-	int rc;
-
-	rc = sh_pfc_pinconf_get(pctldev, pin, &config);
-	if (unlikely(rc != 0))
-		return;
-
-	seq_printf(s, " %s", pinmux_type_str[config]);
-}
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+	struct sh_pfc *pfc = pmx->pfc;
+	enum pin_config_param param = pinconf_to_config_param(*config);
+	unsigned long flags;
+	unsigned int bias;
 
-static struct pinconf_ops sh_pfc_pinconf_ops = {
-	.pin_config_get		= sh_pfc_pinconf_get,
-	.pin_config_set		= sh_pfc_pinconf_set,
-	.pin_config_dbg_show	= sh_pfc_pinconf_dbg_show,
-};
+	if (!sh_pfc_pinconf_validate(pfc, _pin, param))
+		return -ENOTSUPP;
 
-static struct pinctrl_gpio_range sh_pfc_gpio_range = {
-	.name		= DRV_NAME,
-	.id		= 0,
-};
+	switch (param) {
+	case PIN_CONFIG_BIAS_DISABLE:
+	case PIN_CONFIG_BIAS_PULL_UP:
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		if (!pfc->info->ops || !pfc->info->ops->get_bias)
+			return -ENOTSUPP;
 
-static struct pinctrl_desc sh_pfc_pinctrl_desc = {
-	.name		= DRV_NAME,
-	.owner		= THIS_MODULE,
-	.pctlops	= &sh_pfc_pinctrl_ops,
-	.pmxops		= &sh_pfc_pinmux_ops,
-	.confops	= &sh_pfc_pinconf_ops,
-};
+		spin_lock_irqsave(&pfc->lock, flags);
+		bias = pfc->info->ops->get_bias(pfc, _pin);
+		spin_unlock_irqrestore(&pfc->lock, flags);
 
-static void sh_pfc_map_one_gpio(struct sh_pfc *pfc, struct sh_pfc_pinctrl *pmx,
-				struct pinmux_gpio *gpio, unsigned offset)
-{
-	struct pinmux_data_reg *dummy;
-	unsigned long flags;
-	int bit;
-
-	gpio->flags &= ~PINMUX_FLAG_TYPE;
+		if (bias != param)
+			return -EINVAL;
 
-	if (sh_pfc_get_data_reg(pfc, offset, &dummy, &bit) == 0)
-		gpio->flags |= PINMUX_TYPE_GPIO;
-	else {
-		gpio->flags |= PINMUX_TYPE_FUNCTION;
+		*config = 0;
+		break;
 
-		spin_lock_irqsave(&pmx->lock, flags);
-		pmx->nr_functions++;
-		spin_unlock_irqrestore(&pmx->lock, flags);
+	default:
+		return -ENOTSUPP;
 	}
+
+	return 0;
 }
 
-/* pinmux ranges -> pinctrl pin descs */
-static int sh_pfc_map_gpios(struct sh_pfc *pfc, struct sh_pfc_pinctrl *pmx)
+static int sh_pfc_pinconf_set(struct pinctrl_dev *pctldev, unsigned _pin,
+			      unsigned long config)
 {
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+	struct sh_pfc *pfc = pmx->pfc;
+	enum pin_config_param param = pinconf_to_config_param(config);
 	unsigned long flags;
-	int i;
 
-	pmx->nr_pads = pfc->info->last_gpio - pfc->info->first_gpio + 1;
+	if (!sh_pfc_pinconf_validate(pfc, _pin, param))
+		return -ENOTSUPP;
 
-	pmx->pads = devm_kzalloc(pfc->dev, sizeof(*pmx->pads) * pmx->nr_pads,
-				 GFP_KERNEL);
-	if (unlikely(!pmx->pads)) {
-		pmx->nr_pads = 0;
-		return -ENOMEM;
-	}
+	switch (param) {
+	case PIN_CONFIG_BIAS_PULL_UP:
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+	case PIN_CONFIG_BIAS_DISABLE:
+		if (!pfc->info->ops || !pfc->info->ops->set_bias)
+			return -ENOTSUPP;
 
-	spin_lock_irqsave(&pfc->lock, flags);
+		spin_lock_irqsave(&pfc->lock, flags);
+		pfc->info->ops->set_bias(pfc, _pin, param);
+		spin_unlock_irqrestore(&pfc->lock, flags);
 
-	/*
-	 * We don't necessarily have a 1:1 mapping between pin and linux
-	 * GPIO number, as the latter maps to the associated enum_id.
-	 * Care needs to be taken to translate back to pin space when
-	 * dealing with any pin configurations.
-	 */
-	for (i = 0; i < pmx->nr_pads; i++) {
-		struct pinctrl_pin_desc *pin = pmx->pads + i;
-		struct pinmux_gpio *gpio = pfc->info->gpios + i;
+		break;
 
-		pin->number = pfc->info->first_gpio + i;
-		pin->name = gpio->name;
+	default:
+		return -ENOTSUPP;
+	}
 
-		/* XXX */
-		if (unlikely(!gpio->enum_id))
-			continue;
+	return 0;
+}
 
-		sh_pfc_map_one_gpio(pfc, pmx, gpio, i);
-	}
+static int sh_pfc_pinconf_group_set(struct pinctrl_dev *pctldev, unsigned group,
+				    unsigned long config)
+{
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+	const unsigned int *pins;
+	unsigned int num_pins;
+	unsigned int i;
 
-	spin_unlock_irqrestore(&pfc->lock, flags);
+	pins = pmx->pfc->info->groups[group].pins;
+	num_pins = pmx->pfc->info->groups[group].nr_pins;
 
-	sh_pfc_pinctrl_desc.pins = pmx->pads;
-	sh_pfc_pinctrl_desc.npins = pmx->nr_pads;
+	for (i = 0; i < num_pins; ++i)
+		sh_pfc_pinconf_set(pctldev, pins[i], config);
 
 	return 0;
 }
 
-static int sh_pfc_map_functions(struct sh_pfc *pfc, struct sh_pfc_pinctrl *pmx)
+static const struct pinconf_ops sh_pfc_pinconf_ops = {
+	.is_generic			= true,
+	.pin_config_get			= sh_pfc_pinconf_get,
+	.pin_config_set			= sh_pfc_pinconf_set,
+	.pin_config_group_set		= sh_pfc_pinconf_group_set,
+	.pin_config_config_dbg_show	= pinconf_generic_dump_config,
+};
+
+/* PFC ranges -> pinctrl pin descs */
+static int sh_pfc_map_pins(struct sh_pfc *pfc, struct sh_pfc_pinctrl *pmx)
 {
-	unsigned long flags;
-	int i, fn;
+	const struct pinmux_range *ranges;
+	struct pinmux_range def_range;
+	unsigned int nr_ranges;
+	unsigned int nr_pins;
+	unsigned int i;
+
+	if (pfc->info->ranges == NULL) {
+		def_range.begin = 0;
+		def_range.end = pfc->info->nr_pins - 1;
+		ranges = &def_range;
+		nr_ranges = 1;
+	} else {
+		ranges = pfc->info->ranges;
+		nr_ranges = pfc->info->nr_ranges;
+	}
 
-	pmx->functions = devm_kzalloc(pfc->dev, pmx->nr_functions *
-				      sizeof(*pmx->functions), GFP_KERNEL);
-	if (unlikely(!pmx->functions))
+	pmx->pins = devm_kzalloc(pfc->dev,
+				 sizeof(*pmx->pins) * pfc->info->nr_pins,
+				 GFP_KERNEL);
+	if (unlikely(!pmx->pins))
 		return -ENOMEM;
 
-	spin_lock_irqsave(&pmx->lock, flags);
-
-	for (i = fn = 0; i < pmx->nr_pads; i++) {
-		struct pinmux_gpio *gpio = pfc->info->gpios + i;
+	pmx->configs = devm_kzalloc(pfc->dev,
+				    sizeof(*pmx->configs) * pfc->info->nr_pins,
+				    GFP_KERNEL);
+	if (unlikely(!pmx->configs))
+		return -ENOMEM;
 
-		if ((gpio->flags & PINMUX_FLAG_TYPE) == PINMUX_TYPE_FUNCTION)
-			pmx->functions[fn++] = gpio;
+	for (i = 0, nr_pins = 0; i < nr_ranges; ++i) {
+		const struct pinmux_range *range = &ranges[i];
+		unsigned int number;
+
+		for (number = range->begin; number <= range->end;
+		     number++, nr_pins++) {
+			struct sh_pfc_pin_config *cfg = &pmx->configs[nr_pins];
+			struct pinctrl_pin_desc *pin = &pmx->pins[nr_pins];
+			const struct sh_pfc_pin *info =
+				&pfc->info->pins[nr_pins];
+
+			pin->number = number;
+			pin->name = info->name;
+			cfg->type = PINMUX_TYPE_NONE;
+		}
 	}
 
-	spin_unlock_irqrestore(&pmx->lock, flags);
+	pfc->nr_pins = ranges[nr_ranges-1].end + 1;
 
-	return 0;
+	return nr_ranges;
 }
 
 int sh_pfc_register_pinctrl(struct sh_pfc *pfc)
 {
 	struct sh_pfc_pinctrl *pmx;
-	int ret;
+	int nr_ranges;
 
 	pmx = devm_kzalloc(pfc->dev, sizeof(*pmx), GFP_KERNEL);
 	if (unlikely(!pmx))
 		return -ENOMEM;
 
-	spin_lock_init(&pmx->lock);
-
 	pmx->pfc = pfc;
 	pfc->pinctrl = pmx;
 
-	ret = sh_pfc_map_gpios(pfc, pmx);
-	if (unlikely(ret != 0))
-		return ret;
+	nr_ranges = sh_pfc_map_pins(pfc, pmx);
+	if (unlikely(nr_ranges < 0))
+		return nr_ranges;
 
-	ret = sh_pfc_map_functions(pfc, pmx);
-	if (unlikely(ret != 0))
-		return ret;
+	pmx->pctl_desc.name = DRV_NAME;
+	pmx->pctl_desc.owner = THIS_MODULE;
+	pmx->pctl_desc.pctlops = &sh_pfc_pinctrl_ops;
+	pmx->pctl_desc.pmxops = &sh_pfc_pinmux_ops;
+	pmx->pctl_desc.confops = &sh_pfc_pinconf_ops;
+	pmx->pctl_desc.pins = pmx->pins;
+	pmx->pctl_desc.npins = pfc->info->nr_pins;
 
-	pmx->pctl = pinctrl_register(&sh_pfc_pinctrl_desc, pfc->dev, pmx);
-	if (IS_ERR(pmx->pctl))
-		return PTR_ERR(pmx->pctl);
-
-	sh_pfc_gpio_range.npins = pfc->info->last_gpio
-				- pfc->info->first_gpio + 1;
-	sh_pfc_gpio_range.base = pfc->info->first_gpio;
-	sh_pfc_gpio_range.pin_base = pfc->info->first_gpio;
-
-	pinctrl_add_gpio_range(pmx->pctl, &sh_pfc_gpio_range);
+	pmx->pctl = pinctrl_register(&pmx->pctl_desc, pfc->dev, pmx);
+	if (pmx->pctl == NULL)
+		return -EINVAL;
 
 	return 0;
 }
diff --git a/drivers/pinctrl/sh-pfc/sh_pfc.h b/drivers/pinctrl/sh-pfc/sh_pfc.h
index 13049c4c8d30..3b785fc428d5 100644
--- a/drivers/pinctrl/sh-pfc/sh_pfc.h
+++ b/drivers/pinctrl/sh-pfc/sh_pfc.h
@@ -15,7 +15,8 @@
 #include <asm-generic/gpio.h>
 
 typedef unsigned short pinmux_enum_t;
-typedef unsigned short pinmux_flag_t;
+
+#define SH_PFC_MARK_INVALID	((pinmux_enum_t)-1)
 
 enum {
 	PINMUX_TYPE_NONE,
@@ -30,44 +31,81 @@ enum {
 	PINMUX_FLAG_TYPE,	/* must be last */
 };
 
-#define PINMUX_FLAG_DBIT_SHIFT      5
-#define PINMUX_FLAG_DBIT            (0x1f << PINMUX_FLAG_DBIT_SHIFT)
-#define PINMUX_FLAG_DREG_SHIFT      10
-#define PINMUX_FLAG_DREG            (0x3f << PINMUX_FLAG_DREG_SHIFT)
+#define SH_PFC_PIN_CFG_INPUT		(1 << 0)
+#define SH_PFC_PIN_CFG_OUTPUT		(1 << 1)
+#define SH_PFC_PIN_CFG_PULL_UP		(1 << 2)
+#define SH_PFC_PIN_CFG_PULL_DOWN	(1 << 3)
 
-struct pinmux_gpio {
-	pinmux_enum_t enum_id;
-	pinmux_flag_t flags;
+struct sh_pfc_pin {
+	const pinmux_enum_t enum_id;
 	const char *name;
+	unsigned int configs;
 };
 
-#define PINMUX_GPIO(gpio, data_or_mark) \
-	[gpio] = { .name = __stringify(gpio), .enum_id = data_or_mark, .flags = PINMUX_TYPE_NONE }
+#define SH_PFC_PIN_GROUP(n)				\
+	{						\
+		.name = #n,				\
+		.pins = n##_pins,			\
+		.mux = n##_mux,				\
+		.nr_pins = ARRAY_SIZE(n##_pins),	\
+	}
+
+struct sh_pfc_pin_group {
+	const char *name;
+	const unsigned int *pins;
+	const unsigned int *mux;
+	unsigned int nr_pins;
+};
+
+#define SH_PFC_FUNCTION(n)				\
+	{						\
+		.name = #n,				\
+		.groups = n##_groups,			\
+		.nr_groups = ARRAY_SIZE(n##_groups),	\
+	}
+
+struct sh_pfc_function {
+	const char *name;
+	const char * const *groups;
+	unsigned int nr_groups;
+};
+
+struct pinmux_func {
+	const pinmux_enum_t enum_id;
+	const char *name;
+};
+
+#define PINMUX_GPIO(gpio, data_or_mark)			\
+	[gpio] = {					\
+		.name = __stringify(gpio),		\
+		.enum_id = data_or_mark,		\
+	}
+#define PINMUX_GPIO_FN(gpio, base, data_or_mark)	\
+	[gpio - (base)] = {				\
+		.name = __stringify(gpio),		\
+		.enum_id = data_or_mark,		\
+	}
 
 #define PINMUX_DATA(data_or_mark, ids...) data_or_mark, ids, 0
 
 struct pinmux_cfg_reg {
 	unsigned long reg, reg_width, field_width;
-	unsigned long *cnt;
-	pinmux_enum_t *enum_ids;
-	unsigned long *var_field_width;
+	const pinmux_enum_t *enum_ids;
+	const unsigned long *var_field_width;
 };
 
 #define PINMUX_CFG_REG(name, r, r_width, f_width) \
 	.reg = r, .reg_width = r_width, .field_width = f_width,		\
-	.cnt = (unsigned long [r_width / f_width]) {}, \
 	.enum_ids = (pinmux_enum_t [(r_width / f_width) * (1 << f_width)])
 
 #define PINMUX_CFG_REG_VAR(name, r, r_width, var_fw0, var_fwn...) \
 	.reg = r, .reg_width = r_width,	\
-	.cnt = (unsigned long [r_width]) {}, \
 	.var_field_width = (unsigned long [r_width]) { var_fw0, var_fwn, 0 }, \
 	.enum_ids = (pinmux_enum_t [])
 
 struct pinmux_data_reg {
-	unsigned long reg, reg_width, reg_shadow;
-	pinmux_enum_t *enum_ids;
-	void __iomem *mapped_reg;
+	unsigned long reg, reg_width;
+	const pinmux_enum_t *enum_ids;
 };
 
 #define PINMUX_DATA_REG(name, r, r_width) \
@@ -76,11 +114,11 @@ struct pinmux_data_reg {
 
 struct pinmux_irq {
 	int irq;
-	pinmux_enum_t *enum_ids;
+	unsigned short *gpios;
 };
 
 #define PINMUX_IRQ(irq_nr, ids...)			   \
-	{ .irq = irq_nr, .enum_ids = (pinmux_enum_t []) { ids, 0 } }	\
+	{ .irq = irq_nr, .gpios = (unsigned short []) { ids, 0 } }	\
 
 struct pinmux_range {
 	pinmux_enum_t begin;
@@ -88,33 +126,49 @@ struct pinmux_range {
 	pinmux_enum_t force;
 };
 
+struct sh_pfc;
+
+struct sh_pfc_soc_operations {
+	unsigned int (*get_bias)(struct sh_pfc *pfc, unsigned int pin);
+	void (*set_bias)(struct sh_pfc *pfc, unsigned int pin,
+			 unsigned int bias);
+};
+
 struct sh_pfc_soc_info {
-	char *name;
-	pinmux_enum_t reserved_id;
-	struct pinmux_range data;
+	const char *name;
+	const struct sh_pfc_soc_operations *ops;
+
 	struct pinmux_range input;
 	struct pinmux_range input_pd;
 	struct pinmux_range input_pu;
 	struct pinmux_range output;
-	struct pinmux_range mark;
 	struct pinmux_range function;
 
-	unsigned first_gpio, last_gpio;
+	const struct sh_pfc_pin *pins;
+	unsigned int nr_pins;
+	const struct pinmux_range *ranges;
+	unsigned int nr_ranges;
+	const struct sh_pfc_pin_group *groups;
+	unsigned int nr_groups;
+	const struct sh_pfc_function *functions;
+	unsigned int nr_functions;
+
+	const struct pinmux_func *func_gpios;
+	unsigned int nr_func_gpios;
 
-	struct pinmux_gpio *gpios;
-	struct pinmux_cfg_reg *cfg_regs;
-	struct pinmux_data_reg *data_regs;
+	const struct pinmux_cfg_reg *cfg_regs;
+	const struct pinmux_data_reg *data_regs;
 
-	pinmux_enum_t *gpio_data;
+	const pinmux_enum_t *gpio_data;
 	unsigned int gpio_data_size;
 
-	struct pinmux_irq *gpio_irq;
+	const struct pinmux_irq *gpio_irq;
 	unsigned int gpio_irq_size;
 
 	unsigned long unlock_reg;
 };
 
-enum { GPIO_CFG_DRYRUN, GPIO_CFG_REQ, GPIO_CFG_FREE };
+enum { GPIO_CFG_REQ, GPIO_CFG_FREE };
 
 /* helper macro for port */
 #define PORT_1(fn, pfx, sfx) fn(pfx, sfx)
@@ -126,6 +180,23 @@ enum { GPIO_CFG_DRYRUN, GPIO_CFG_REQ, GPIO_CFG_FREE };
 	PORT_1(fn, pfx##6, sfx), PORT_1(fn, pfx##7, sfx),	\
 	PORT_1(fn, pfx##8, sfx), PORT_1(fn, pfx##9, sfx)
 
+#define PORT_10_REV(fn, pfx, sfx)	\
+	PORT_1(fn, pfx##9, sfx), PORT_1(fn, pfx##8, sfx),	\
+	PORT_1(fn, pfx##7, sfx), PORT_1(fn, pfx##6, sfx),	\
+	PORT_1(fn, pfx##5, sfx), PORT_1(fn, pfx##4, sfx),	\
+	PORT_1(fn, pfx##3, sfx), PORT_1(fn, pfx##2, sfx),	\
+	PORT_1(fn, pfx##1, sfx), PORT_1(fn, pfx##0, sfx)
+
+#define PORT_32(fn, pfx, sfx)					\
+	PORT_10(fn, pfx, sfx), PORT_10(fn, pfx##1, sfx),	\
+	PORT_10(fn, pfx##2, sfx), PORT_1(fn, pfx##30, sfx),	\
+	PORT_1(fn, pfx##31, sfx)
+
+#define PORT_32_REV(fn, pfx, sfx)					\
+	PORT_1(fn, pfx##31, sfx), PORT_1(fn, pfx##30, sfx),		\
+	PORT_10_REV(fn, pfx##2, sfx), PORT_10_REV(fn, pfx##1, sfx),	\
+	PORT_10_REV(fn, pfx, sfx)
+
 #define PORT_90(fn, pfx, sfx) \
 	PORT_10(fn, pfx##1, sfx), PORT_10(fn, pfx##2, sfx),	\
 	PORT_10(fn, pfx##3, sfx), PORT_10(fn, pfx##4, sfx),	\
@@ -137,7 +208,7 @@ enum { GPIO_CFG_DRYRUN, GPIO_CFG_REQ, GPIO_CFG_FREE };
 #define _GPIO_PORT(pfx, sfx) PINMUX_GPIO(GPIO_PORT##pfx, PORT##pfx##_DATA)
 #define PORT_ALL(str)	CPU_ALL_PORT(_PORT_ALL, PORT, str)
 #define GPIO_PORT_ALL()	CPU_ALL_PORT(_GPIO_PORT, , unused)
-#define GPIO_FN(str) PINMUX_GPIO(GPIO_FN_##str, str##_MARK)
+#define GPIO_FN(str) PINMUX_GPIO_FN(GPIO_FN_##str, PINMUX_FN_BASE, str##_MARK)
 
 /* helper macro for pinmux_enum_t */
 #define PORT_DATA_I(nr)	\
diff --git a/drivers/pinctrl/spear/pinctrl-plgpio.c b/drivers/pinctrl/spear/pinctrl-plgpio.c
index 295b349a05cf..3e5a887216b8 100644
--- a/drivers/pinctrl/spear/pinctrl-plgpio.c
+++ b/drivers/pinctrl/spear/pinctrl-plgpio.c
@@ -15,12 +15,12 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 #include <linux/irqdomain.h>
+#include <linux/irqchip/chained_irq.h>
 #include <linux/module.h>
 #include <linux/pinctrl/consumer.h>
 #include <linux/platform_device.h>
 #include <linux/pm.h>
 #include <linux/spinlock.h>
-#include <asm/mach/irq.h>
 
 #define MAX_GPIO_PER_REG		32
 #define PIN_OFFSET(pin)			(pin % MAX_GPIO_PER_REG)
@@ -75,7 +75,7 @@ struct plgpio {
 	int			(*o2p)(int offset);	/* offset_to_pin */
 	u32			p2o_regs;
 	struct plgpio_regs	regs;
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	struct plgpio_regs	*csave_regs;
 #endif
 };
@@ -554,7 +554,7 @@ static int plgpio_probe(struct platform_device *pdev)
 	if (IS_ERR(plgpio->clk))
 		dev_warn(&pdev->dev, "clk_get() failed, work without it\n");
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	plgpio->csave_regs = devm_kzalloc(&pdev->dev,
 			sizeof(*plgpio->csave_regs) *
 			DIV_ROUND_UP(plgpio->chip.ngpio, MAX_GPIO_PER_REG),
@@ -641,7 +641,7 @@ unprepare_clk:
 	return ret;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int plgpio_suspend(struct device *dev)
 {
 	struct plgpio *plgpio = dev_get_drvdata(dev);
diff --git a/drivers/pinctrl/vt8500/Kconfig b/drivers/pinctrl/vt8500/Kconfig
new file mode 100644
index 000000000000..55724a73d94a
--- /dev/null
+++ b/drivers/pinctrl/vt8500/Kconfig
@@ -0,0 +1,52 @@
+#
+# VIA/Wondermedia PINCTRL drivers
+#
+
+if ARCH_VT8500
+
+config PINCTRL_WMT
+	bool
+	select PINMUX
+	select GENERIC_PINCONF
+
+config PINCTRL_VT8500
+	bool "VIA VT8500 pin controller driver"
+	depends on ARCH_WM8505
+	select PINCTRL_WMT
+	help
+	  Say yes here to support the gpio/pin control module on
+	  VIA VT8500 SoCs.
+
+config PINCTRL_WM8505
+	bool "Wondermedia WM8505 pin controller driver"
+	depends on ARCH_WM8505
+	select PINCTRL_WMT
+	help
+	  Say yes here to support the gpio/pin control module on
+	  Wondermedia WM8505 SoCs.
+
+config PINCTRL_WM8650
+	bool "Wondermedia WM8650 pin controller driver"
+	depends on ARCH_WM8505
+	select PINCTRL_WMT
+	help
+	  Say yes here to support the gpio/pin control module on
+	  Wondermedia WM8650 SoCs.
+
+config PINCTRL_WM8750
+	bool "Wondermedia WM8750 pin controller driver"
+	depends on ARCH_WM8750
+	select PINCTRL_WMT
+	help
+	  Say yes here to support the gpio/pin control module on
+	  Wondermedia WM8750 SoCs.
+
+config PINCTRL_WM8850
+	bool "Wondermedia WM8850 pin controller driver"
+	depends on ARCH_WM8850
+	select PINCTRL_WMT
+	help
+	  Say yes here to support the gpio/pin control module on
+	  Wondermedia WM8850 SoCs.
+
+endif
diff --git a/drivers/pinctrl/vt8500/Makefile b/drivers/pinctrl/vt8500/Makefile
new file mode 100644
index 000000000000..24ec45dd0d80
--- /dev/null
+++ b/drivers/pinctrl/vt8500/Makefile
@@ -0,0 +1,8 @@
+# VIA/Wondermedia pinctrl support
+
+obj-$(CONFIG_PINCTRL_WMT)	+= pinctrl-wmt.o
+obj-$(CONFIG_PINCTRL_VT8500)	+= pinctrl-vt8500.o
+obj-$(CONFIG_PINCTRL_WM8505)	+= pinctrl-wm8505.o
+obj-$(CONFIG_PINCTRL_WM8650)	+= pinctrl-wm8650.o
+obj-$(CONFIG_PINCTRL_WM8750)	+= pinctrl-wm8750.o
+obj-$(CONFIG_PINCTRL_WM8850)	+= pinctrl-wm8850.o
diff --git a/drivers/pinctrl/vt8500/pinctrl-vt8500.c b/drivers/pinctrl/vt8500/pinctrl-vt8500.c
new file mode 100644
index 000000000000..f2fe9f85cfa6
--- /dev/null
+++ b/drivers/pinctrl/vt8500/pinctrl-vt8500.c
@@ -0,0 +1,501 @@
+/*
+ * Pinctrl data for VIA VT8500 SoC
+ *
+ * Copyright (c) 2013 Tony Prisk <linux@prisktech.co.nz>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include "pinctrl-wmt.h"
+
+/*
+ * Describe the register offsets within the GPIO memory space
+ * The dedicated external GPIO's should always be listed in bank 0
+ * so they are exported in the 0..31 range which is what users
+ * expect.
+ *
+ * Do not reorder these banks as it will change the pin numbering
+ */
+static const struct wmt_pinctrl_bank_registers vt8500_banks[] = {
+	WMT_PINCTRL_BANK(NO_REG, 0x3C, 0x5C, 0x7C, NO_REG, NO_REG),	/* 0 */
+	WMT_PINCTRL_BANK(0x00, 0x20, 0x40, 0x60, NO_REG, NO_REG),	/* 1 */
+	WMT_PINCTRL_BANK(0x04, 0x24, 0x44, 0x64, NO_REG, NO_REG),	/* 2 */
+	WMT_PINCTRL_BANK(0x08, 0x28, 0x48, 0x68, NO_REG, NO_REG),	/* 3 */
+	WMT_PINCTRL_BANK(0x0C, 0x2C, 0x4C, 0x6C, NO_REG, NO_REG),	/* 4 */
+	WMT_PINCTRL_BANK(0x10, 0x30, 0x50, 0x70, NO_REG, NO_REG),	/* 5 */
+	WMT_PINCTRL_BANK(0x14, 0x34, 0x54, 0x74, NO_REG, NO_REG),	/* 6 */
+};
+
+/* Please keep sorted by bank/bit */
+#define WMT_PIN_EXTGPIO0	WMT_PIN(0, 0)
+#define WMT_PIN_EXTGPIO1	WMT_PIN(0, 1)
+#define WMT_PIN_EXTGPIO2	WMT_PIN(0, 2)
+#define WMT_PIN_EXTGPIO3	WMT_PIN(0, 3)
+#define WMT_PIN_EXTGPIO4	WMT_PIN(0, 4)
+#define WMT_PIN_EXTGPIO5	WMT_PIN(0, 5)
+#define WMT_PIN_EXTGPIO6	WMT_PIN(0, 6)
+#define WMT_PIN_EXTGPIO7	WMT_PIN(0, 7)
+#define WMT_PIN_EXTGPIO8	WMT_PIN(0, 8)
+#define WMT_PIN_UART0RTS	WMT_PIN(1, 0)
+#define WMT_PIN_UART0TXD	WMT_PIN(1, 1)
+#define WMT_PIN_UART0CTS	WMT_PIN(1, 2)
+#define WMT_PIN_UART0RXD	WMT_PIN(1, 3)
+#define WMT_PIN_UART1RTS	WMT_PIN(1, 4)
+#define WMT_PIN_UART1TXD	WMT_PIN(1, 5)
+#define WMT_PIN_UART1CTS	WMT_PIN(1, 6)
+#define WMT_PIN_UART1RXD	WMT_PIN(1, 7)
+#define WMT_PIN_SPI0CLK		WMT_PIN(1, 8)
+#define WMT_PIN_SPI0SS		WMT_PIN(1, 9)
+#define WMT_PIN_SPI0MISO	WMT_PIN(1, 10)
+#define WMT_PIN_SPI0MOSI	WMT_PIN(1, 11)
+#define WMT_PIN_SPI1CLK		WMT_PIN(1, 12)
+#define WMT_PIN_SPI1SS		WMT_PIN(1, 13)
+#define WMT_PIN_SPI1MISO	WMT_PIN(1, 14)
+#define WMT_PIN_SPI1MOSI	WMT_PIN(1, 15)
+#define WMT_PIN_SPI2CLK		WMT_PIN(1, 16)
+#define WMT_PIN_SPI2SS		WMT_PIN(1, 17)
+#define WMT_PIN_SPI2MISO	WMT_PIN(1, 18)
+#define WMT_PIN_SPI2MOSI	WMT_PIN(1, 19)
+#define WMT_PIN_SDDATA0		WMT_PIN(2, 0)
+#define WMT_PIN_SDDATA1		WMT_PIN(2, 1)
+#define WMT_PIN_SDDATA2		WMT_PIN(2, 2)
+#define WMT_PIN_SDDATA3		WMT_PIN(2, 3)
+#define WMT_PIN_MMCDATA0	WMT_PIN(2, 4)
+#define WMT_PIN_MMCDATA1	WMT_PIN(2, 5)
+#define WMT_PIN_MMCDATA2	WMT_PIN(2, 6)
+#define WMT_PIN_MMCDATA3	WMT_PIN(2, 7)
+#define WMT_PIN_SDCLK		WMT_PIN(2, 8)
+#define WMT_PIN_SDWP		WMT_PIN(2, 9)
+#define WMT_PIN_SDCMD		WMT_PIN(2, 10)
+#define WMT_PIN_MSDATA0		WMT_PIN(2, 16)
+#define WMT_PIN_MSDATA1		WMT_PIN(2, 17)
+#define WMT_PIN_MSDATA2		WMT_PIN(2, 18)
+#define WMT_PIN_MSDATA3		WMT_PIN(2, 19)
+#define WMT_PIN_MSCLK		WMT_PIN(2, 20)
+#define WMT_PIN_MSBS		WMT_PIN(2, 21)
+#define WMT_PIN_MSINS		WMT_PIN(2, 22)
+#define WMT_PIN_I2C0SCL		WMT_PIN(2, 24)
+#define WMT_PIN_I2C0SDA		WMT_PIN(2, 25)
+#define WMT_PIN_I2C1SCL		WMT_PIN(2, 26)
+#define WMT_PIN_I2C1SDA		WMT_PIN(2, 27)
+#define WMT_PIN_MII0RXD0	WMT_PIN(3, 0)
+#define WMT_PIN_MII0RXD1	WMT_PIN(3, 1)
+#define WMT_PIN_MII0RXD2	WMT_PIN(3, 2)
+#define WMT_PIN_MII0RXD3	WMT_PIN(3, 3)
+#define WMT_PIN_MII0RXCLK	WMT_PIN(3, 4)
+#define WMT_PIN_MII0RXDV	WMT_PIN(3, 5)
+#define WMT_PIN_MII0RXERR	WMT_PIN(3, 6)
+#define WMT_PIN_MII0PHYRST	WMT_PIN(3, 7)
+#define WMT_PIN_MII0TXD0	WMT_PIN(3, 8)
+#define WMT_PIN_MII0TXD1	WMT_PIN(3, 9)
+#define WMT_PIN_MII0TXD2	WMT_PIN(3, 10)
+#define WMT_PIN_MII0TXD3	WMT_PIN(3, 11)
+#define WMT_PIN_MII0TXCLK	WMT_PIN(3, 12)
+#define WMT_PIN_MII0TXEN	WMT_PIN(3, 13)
+#define WMT_PIN_MII0TXERR	WMT_PIN(3, 14)
+#define WMT_PIN_MII0PHYPD	WMT_PIN(3, 15)
+#define WMT_PIN_MII0COL		WMT_PIN(3, 16)
+#define WMT_PIN_MII0CRS		WMT_PIN(3, 17)
+#define WMT_PIN_MII0MDIO	WMT_PIN(3, 18)
+#define WMT_PIN_MII0MDC		WMT_PIN(3, 19)
+#define WMT_PIN_SEECS		WMT_PIN(3, 20)
+#define WMT_PIN_SEECK		WMT_PIN(3, 21)
+#define WMT_PIN_SEEDI		WMT_PIN(3, 22)
+#define WMT_PIN_SEEDO		WMT_PIN(3, 23)
+#define WMT_PIN_IDEDREQ0	WMT_PIN(3, 24)
+#define WMT_PIN_IDEDREQ1	WMT_PIN(3, 25)
+#define WMT_PIN_IDEIOW		WMT_PIN(3, 26)
+#define WMT_PIN_IDEIOR		WMT_PIN(3, 27)
+#define WMT_PIN_IDEDACK		WMT_PIN(3, 28)
+#define WMT_PIN_IDEIORDY	WMT_PIN(3, 29)
+#define WMT_PIN_IDEINTRQ	WMT_PIN(3, 30)
+#define WMT_PIN_VDIN0		WMT_PIN(4, 0)
+#define WMT_PIN_VDIN1		WMT_PIN(4, 1)
+#define WMT_PIN_VDIN2		WMT_PIN(4, 2)
+#define WMT_PIN_VDIN3		WMT_PIN(4, 3)
+#define WMT_PIN_VDIN4		WMT_PIN(4, 4)
+#define WMT_PIN_VDIN5		WMT_PIN(4, 5)
+#define WMT_PIN_VDIN6		WMT_PIN(4, 6)
+#define WMT_PIN_VDIN7		WMT_PIN(4, 7)
+#define WMT_PIN_VDOUT0		WMT_PIN(4, 8)
+#define WMT_PIN_VDOUT1		WMT_PIN(4, 9)
+#define WMT_PIN_VDOUT2		WMT_PIN(4, 10)
+#define WMT_PIN_VDOUT3		WMT_PIN(4, 11)
+#define WMT_PIN_VDOUT4		WMT_PIN(4, 12)
+#define WMT_PIN_VDOUT5		WMT_PIN(4, 13)
+#define WMT_PIN_NANDCLE0	WMT_PIN(4, 14)
+#define WMT_PIN_NANDCLE1	WMT_PIN(4, 15)
+#define WMT_PIN_VDOUT6_7	WMT_PIN(4, 16)
+#define WMT_PIN_VHSYNC		WMT_PIN(4, 17)
+#define WMT_PIN_VVSYNC		WMT_PIN(4, 18)
+#define WMT_PIN_TSDIN0		WMT_PIN(5, 8)
+#define WMT_PIN_TSDIN1		WMT_PIN(5, 9)
+#define WMT_PIN_TSDIN2		WMT_PIN(5, 10)
+#define WMT_PIN_TSDIN3		WMT_PIN(5, 11)
+#define WMT_PIN_TSDIN4		WMT_PIN(5, 12)
+#define WMT_PIN_TSDIN5		WMT_PIN(5, 13)
+#define WMT_PIN_TSDIN6		WMT_PIN(5, 14)
+#define WMT_PIN_TSDIN7		WMT_PIN(5, 15)
+#define WMT_PIN_TSSYNC		WMT_PIN(5, 16)
+#define WMT_PIN_TSVALID		WMT_PIN(5, 17)
+#define WMT_PIN_TSCLK		WMT_PIN(5, 18)
+#define WMT_PIN_LCDD0		WMT_PIN(6, 0)
+#define WMT_PIN_LCDD1		WMT_PIN(6, 1)
+#define WMT_PIN_LCDD2		WMT_PIN(6, 2)
+#define WMT_PIN_LCDD3		WMT_PIN(6, 3)
+#define WMT_PIN_LCDD4		WMT_PIN(6, 4)
+#define WMT_PIN_LCDD5		WMT_PIN(6, 5)
+#define WMT_PIN_LCDD6		WMT_PIN(6, 6)
+#define WMT_PIN_LCDD7		WMT_PIN(6, 7)
+#define WMT_PIN_LCDD8		WMT_PIN(6, 8)
+#define WMT_PIN_LCDD9		WMT_PIN(6, 9)
+#define WMT_PIN_LCDD10		WMT_PIN(6, 10)
+#define WMT_PIN_LCDD11		WMT_PIN(6, 11)
+#define WMT_PIN_LCDD12		WMT_PIN(6, 12)
+#define WMT_PIN_LCDD13		WMT_PIN(6, 13)
+#define WMT_PIN_LCDD14		WMT_PIN(6, 14)
+#define WMT_PIN_LCDD15		WMT_PIN(6, 15)
+#define WMT_PIN_LCDD16		WMT_PIN(6, 16)
+#define WMT_PIN_LCDD17		WMT_PIN(6, 17)
+#define WMT_PIN_LCDCLK		WMT_PIN(6, 18)
+#define WMT_PIN_LCDDEN		WMT_PIN(6, 19)
+#define WMT_PIN_LCDLINE		WMT_PIN(6, 20)
+#define WMT_PIN_LCDFRM		WMT_PIN(6, 21)
+#define WMT_PIN_LCDBIAS		WMT_PIN(6, 22)
+
+static const struct pinctrl_pin_desc vt8500_pins[] = {
+	PINCTRL_PIN(WMT_PIN_EXTGPIO0, "extgpio0"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO1, "extgpio1"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO2, "extgpio2"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO3, "extgpio3"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO4, "extgpio4"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO5, "extgpio5"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO6, "extgpio6"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO7, "extgpio7"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO8, "extgpio8"),
+	PINCTRL_PIN(WMT_PIN_UART0RTS, "uart0_rts"),
+	PINCTRL_PIN(WMT_PIN_UART0TXD, "uart0_txd"),
+	PINCTRL_PIN(WMT_PIN_UART0CTS, "uart0_cts"),
+	PINCTRL_PIN(WMT_PIN_UART0RXD, "uart0_rxd"),
+	PINCTRL_PIN(WMT_PIN_UART1RTS, "uart1_rts"),
+	PINCTRL_PIN(WMT_PIN_UART1TXD, "uart1_txd"),
+	PINCTRL_PIN(WMT_PIN_UART1CTS, "uart1_cts"),
+	PINCTRL_PIN(WMT_PIN_UART1RXD, "uart1_rxd"),
+	PINCTRL_PIN(WMT_PIN_SPI0CLK, "spi0_clk"),
+	PINCTRL_PIN(WMT_PIN_SPI0SS, "spi0_ss"),
+	PINCTRL_PIN(WMT_PIN_SPI0MISO, "spi0_miso"),
+	PINCTRL_PIN(WMT_PIN_SPI0MOSI, "spi0_mosi"),
+	PINCTRL_PIN(WMT_PIN_SPI1CLK, "spi1_clk"),
+	PINCTRL_PIN(WMT_PIN_SPI1SS, "spi1_ss"),
+	PINCTRL_PIN(WMT_PIN_SPI1MISO, "spi1_miso"),
+	PINCTRL_PIN(WMT_PIN_SPI1MOSI, "spi1_mosi"),
+	PINCTRL_PIN(WMT_PIN_SPI2CLK, "spi2_clk"),
+	PINCTRL_PIN(WMT_PIN_SPI2SS, "spi2_ss"),
+	PINCTRL_PIN(WMT_PIN_SPI2MISO, "spi2_miso"),
+	PINCTRL_PIN(WMT_PIN_SPI2MOSI, "spi2_mosi"),
+	PINCTRL_PIN(WMT_PIN_SDDATA0, "sd_data0"),
+	PINCTRL_PIN(WMT_PIN_SDDATA1, "sd_data1"),
+	PINCTRL_PIN(WMT_PIN_SDDATA2, "sd_data2"),
+	PINCTRL_PIN(WMT_PIN_SDDATA3, "sd_data3"),
+	PINCTRL_PIN(WMT_PIN_MMCDATA0, "mmc_data0"),
+	PINCTRL_PIN(WMT_PIN_MMCDATA1, "mmc_data1"),
+	PINCTRL_PIN(WMT_PIN_MMCDATA2, "mmc_data2"),
+	PINCTRL_PIN(WMT_PIN_MMCDATA3, "mmc_data3"),
+	PINCTRL_PIN(WMT_PIN_SDCLK, "sd_clk"),
+	PINCTRL_PIN(WMT_PIN_SDWP, "sd_wp"),
+	PINCTRL_PIN(WMT_PIN_SDCMD, "sd_cmd"),
+	PINCTRL_PIN(WMT_PIN_MSDATA0, "ms_data0"),
+	PINCTRL_PIN(WMT_PIN_MSDATA1, "ms_data1"),
+	PINCTRL_PIN(WMT_PIN_MSDATA2, "ms_data2"),
+	PINCTRL_PIN(WMT_PIN_MSDATA3, "ms_data3"),
+	PINCTRL_PIN(WMT_PIN_MSCLK, "ms_clk"),
+	PINCTRL_PIN(WMT_PIN_MSBS, "ms_bs"),
+	PINCTRL_PIN(WMT_PIN_MSINS, "ms_ins"),
+	PINCTRL_PIN(WMT_PIN_I2C0SCL, "i2c0_scl"),
+	PINCTRL_PIN(WMT_PIN_I2C0SDA, "i2c0_sda"),
+	PINCTRL_PIN(WMT_PIN_I2C1SCL, "i2c1_scl"),
+	PINCTRL_PIN(WMT_PIN_I2C1SDA, "i2c1_sda"),
+	PINCTRL_PIN(WMT_PIN_MII0RXD0, "mii0_rxd0"),
+	PINCTRL_PIN(WMT_PIN_MII0RXD1, "mii0_rxd1"),
+	PINCTRL_PIN(WMT_PIN_MII0RXD2, "mii0_rxd2"),
+	PINCTRL_PIN(WMT_PIN_MII0RXD3, "mii0_rxd3"),
+	PINCTRL_PIN(WMT_PIN_MII0RXCLK, "mii0_rxclk"),
+	PINCTRL_PIN(WMT_PIN_MII0RXDV, "mii0_rxdv"),
+	PINCTRL_PIN(WMT_PIN_MII0RXERR, "mii0_rxerr"),
+	PINCTRL_PIN(WMT_PIN_MII0PHYRST, "mii0_phyrst"),
+	PINCTRL_PIN(WMT_PIN_MII0TXD0, "mii0_txd0"),
+	PINCTRL_PIN(WMT_PIN_MII0TXD1, "mii0_txd1"),
+	PINCTRL_PIN(WMT_PIN_MII0TXD2, "mii0_txd2"),
+	PINCTRL_PIN(WMT_PIN_MII0TXD3, "mii0_txd3"),
+	PINCTRL_PIN(WMT_PIN_MII0TXCLK, "mii0_txclk"),
+	PINCTRL_PIN(WMT_PIN_MII0TXEN, "mii0_txen"),
+	PINCTRL_PIN(WMT_PIN_MII0TXERR, "mii0_txerr"),
+	PINCTRL_PIN(WMT_PIN_MII0PHYPD, "mii0_phypd"),
+	PINCTRL_PIN(WMT_PIN_MII0COL, "mii0_col"),
+	PINCTRL_PIN(WMT_PIN_MII0CRS, "mii0_crs"),
+	PINCTRL_PIN(WMT_PIN_MII0MDIO, "mii0_mdio"),
+	PINCTRL_PIN(WMT_PIN_MII0MDC, "mii0_mdc"),
+	PINCTRL_PIN(WMT_PIN_SEECS, "see_cs"),
+	PINCTRL_PIN(WMT_PIN_SEECK, "see_ck"),
+	PINCTRL_PIN(WMT_PIN_SEEDI, "see_di"),
+	PINCTRL_PIN(WMT_PIN_SEEDO, "see_do"),
+	PINCTRL_PIN(WMT_PIN_IDEDREQ0, "ide_dreq0"),
+	PINCTRL_PIN(WMT_PIN_IDEDREQ1, "ide_dreq1"),
+	PINCTRL_PIN(WMT_PIN_IDEIOW, "ide_iow"),
+	PINCTRL_PIN(WMT_PIN_IDEIOR, "ide_ior"),
+	PINCTRL_PIN(WMT_PIN_IDEDACK, "ide_dack"),
+	PINCTRL_PIN(WMT_PIN_IDEIORDY, "ide_iordy"),
+	PINCTRL_PIN(WMT_PIN_IDEINTRQ, "ide_intrq"),
+	PINCTRL_PIN(WMT_PIN_VDIN0, "vdin0"),
+	PINCTRL_PIN(WMT_PIN_VDIN1, "vdin1"),
+	PINCTRL_PIN(WMT_PIN_VDIN2, "vdin2"),
+	PINCTRL_PIN(WMT_PIN_VDIN3, "vdin3"),
+	PINCTRL_PIN(WMT_PIN_VDIN4, "vdin4"),
+	PINCTRL_PIN(WMT_PIN_VDIN5, "vdin5"),
+	PINCTRL_PIN(WMT_PIN_VDIN6, "vdin6"),
+	PINCTRL_PIN(WMT_PIN_VDIN7, "vdin7"),
+	PINCTRL_PIN(WMT_PIN_VDOUT0, "vdout0"),
+	PINCTRL_PIN(WMT_PIN_VDOUT1, "vdout1"),
+	PINCTRL_PIN(WMT_PIN_VDOUT2, "vdout2"),
+	PINCTRL_PIN(WMT_PIN_VDOUT3, "vdout3"),
+	PINCTRL_PIN(WMT_PIN_VDOUT4, "vdout4"),
+	PINCTRL_PIN(WMT_PIN_VDOUT5, "vdout5"),
+	PINCTRL_PIN(WMT_PIN_NANDCLE0, "nand_cle0"),
+	PINCTRL_PIN(WMT_PIN_NANDCLE1, "nand_cle1"),
+	PINCTRL_PIN(WMT_PIN_VDOUT6_7, "vdout6_7"),
+	PINCTRL_PIN(WMT_PIN_VHSYNC, "vhsync"),
+	PINCTRL_PIN(WMT_PIN_VVSYNC, "vvsync"),
+	PINCTRL_PIN(WMT_PIN_TSDIN0, "tsdin0"),
+	PINCTRL_PIN(WMT_PIN_TSDIN1, "tsdin1"),
+	PINCTRL_PIN(WMT_PIN_TSDIN2, "tsdin2"),
+	PINCTRL_PIN(WMT_PIN_TSDIN3, "tsdin3"),
+	PINCTRL_PIN(WMT_PIN_TSDIN4, "tsdin4"),
+	PINCTRL_PIN(WMT_PIN_TSDIN5, "tsdin5"),
+	PINCTRL_PIN(WMT_PIN_TSDIN6, "tsdin6"),
+	PINCTRL_PIN(WMT_PIN_TSDIN7, "tsdin7"),
+	PINCTRL_PIN(WMT_PIN_TSSYNC, "tssync"),
+	PINCTRL_PIN(WMT_PIN_TSVALID, "tsvalid"),
+	PINCTRL_PIN(WMT_PIN_TSCLK, "tsclk"),
+	PINCTRL_PIN(WMT_PIN_LCDD0, "lcd_d0"),
+	PINCTRL_PIN(WMT_PIN_LCDD1, "lcd_d1"),
+	PINCTRL_PIN(WMT_PIN_LCDD2, "lcd_d2"),
+	PINCTRL_PIN(WMT_PIN_LCDD3, "lcd_d3"),
+	PINCTRL_PIN(WMT_PIN_LCDD4, "lcd_d4"),
+	PINCTRL_PIN(WMT_PIN_LCDD5, "lcd_d5"),
+	PINCTRL_PIN(WMT_PIN_LCDD6, "lcd_d6"),
+	PINCTRL_PIN(WMT_PIN_LCDD7, "lcd_d7"),
+	PINCTRL_PIN(WMT_PIN_LCDD8, "lcd_d8"),
+	PINCTRL_PIN(WMT_PIN_LCDD9, "lcd_d9"),
+	PINCTRL_PIN(WMT_PIN_LCDD10, "lcd_d10"),
+	PINCTRL_PIN(WMT_PIN_LCDD11, "lcd_d11"),
+	PINCTRL_PIN(WMT_PIN_LCDD12, "lcd_d12"),
+	PINCTRL_PIN(WMT_PIN_LCDD13, "lcd_d13"),
+	PINCTRL_PIN(WMT_PIN_LCDD14, "lcd_d14"),
+	PINCTRL_PIN(WMT_PIN_LCDD15, "lcd_d15"),
+	PINCTRL_PIN(WMT_PIN_LCDD16, "lcd_d16"),
+	PINCTRL_PIN(WMT_PIN_LCDD17, "lcd_d17"),
+	PINCTRL_PIN(WMT_PIN_LCDCLK, "lcd_clk"),
+	PINCTRL_PIN(WMT_PIN_LCDDEN, "lcd_den"),
+	PINCTRL_PIN(WMT_PIN_LCDLINE, "lcd_line"),
+	PINCTRL_PIN(WMT_PIN_LCDFRM, "lcd_frm"),
+	PINCTRL_PIN(WMT_PIN_LCDBIAS, "lcd_bias"),
+};
+
+/* Order of these names must match the above list */
+static const char * const vt8500_groups[] = {
+	"extgpio0",
+	"extgpio1",
+	"extgpio2",
+	"extgpio3",
+	"extgpio4",
+	"extgpio5",
+	"extgpio6",
+	"extgpio7",
+	"extgpio8",
+	"uart0_rts",
+	"uart0_txd",
+	"uart0_cts",
+	"uart0_rxd",
+	"uart1_rts",
+	"uart1_txd",
+	"uart1_cts",
+	"uart1_rxd",
+	"spi0_clk",
+	"spi0_ss",
+	"spi0_miso",
+	"spi0_mosi",
+	"spi1_clk",
+	"spi1_ss",
+	"spi1_miso",
+	"spi1_mosi",
+	"spi2_clk",
+	"spi2_ss",
+	"spi2_miso",
+	"spi2_mosi",
+	"sd_data0",
+	"sd_data1",
+	"sd_data2",
+	"sd_data3",
+	"mmc_data0",
+	"mmc_data1",
+	"mmc_data2",
+	"mmc_data3",
+	"sd_clk",
+	"sd_wp",
+	"sd_cmd",
+	"ms_data0",
+	"ms_data1",
+	"ms_data2",
+	"ms_data3",
+	"ms_clk",
+	"ms_bs",
+	"ms_ins",
+	"i2c0_scl",
+	"i2c0_sda",
+	"i2c1_scl",
+	"i2c1_sda",
+	"mii0_rxd0",
+	"mii0_rxd1",
+	"mii0_rxd2",
+	"mii0_rxd3",
+	"mii0_rxclk",
+	"mii0_rxdv",
+	"mii0_rxerr",
+	"mii0_phyrst",
+	"mii0_txd0",
+	"mii0_txd1",
+	"mii0_txd2",
+	"mii0_txd3",
+	"mii0_txclk",
+	"mii0_txen",
+	"mii0_txerr",
+	"mii0_phypd",
+	"mii0_col",
+	"mii0_crs",
+	"mii0_mdio",
+	"mii0_mdc",
+	"see_cs",
+	"see_ck",
+	"see_di",
+	"see_do",
+	"ide_dreq0",
+	"ide_dreq1",
+	"ide_iow",
+	"ide_ior",
+	"ide_dack",
+	"ide_iordy",
+	"ide_intrq",
+	"vdin0",
+	"vdin1",
+	"vdin2",
+	"vdin3",
+	"vdin4",
+	"vdin5",
+	"vdin6",
+	"vdin7",
+	"vdout0",
+	"vdout1",
+	"vdout2",
+	"vdout3",
+	"vdout4",
+	"vdout5",
+	"nand_cle0",
+	"nand_cle1",
+	"vdout6_7",
+	"vhsync",
+	"vvsync",
+	"tsdin0",
+	"tsdin1",
+	"tsdin2",
+	"tsdin3",
+	"tsdin4",
+	"tsdin5",
+	"tsdin6",
+	"tsdin7",
+	"tssync",
+	"tsvalid",
+	"tsclk",
+	"lcd_d0",
+	"lcd_d1",
+	"lcd_d2",
+	"lcd_d3",
+	"lcd_d4",
+	"lcd_d5",
+	"lcd_d6",
+	"lcd_d7",
+	"lcd_d8",
+	"lcd_d9",
+	"lcd_d10",
+	"lcd_d11",
+	"lcd_d12",
+	"lcd_d13",
+	"lcd_d14",
+	"lcd_d15",
+	"lcd_d16",
+	"lcd_d17",
+	"lcd_clk",
+	"lcd_den",
+	"lcd_line",
+	"lcd_frm",
+	"lcd_bias",
+};
+
+static int vt8500_pinctrl_probe(struct platform_device *pdev)
+{
+	struct wmt_pinctrl_data *data;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		dev_err(&pdev->dev, "failed to allocate data\n");
+		return -ENOMEM;
+	}
+
+	data->banks = vt8500_banks;
+	data->nbanks = ARRAY_SIZE(vt8500_banks);
+	data->pins = vt8500_pins;
+	data->npins = ARRAY_SIZE(vt8500_pins);
+	data->groups = vt8500_groups;
+	data->ngroups = ARRAY_SIZE(vt8500_groups);
+
+	return wmt_pinctrl_probe(pdev, data);
+}
+
+static int vt8500_pinctrl_remove(struct platform_device *pdev)
+{
+	return wmt_pinctrl_remove(pdev);
+}
+
+static struct of_device_id wmt_pinctrl_of_match[] = {
+	{ .compatible = "via,vt8500-pinctrl" },
+	{ /* sentinel */ },
+};
+
+static struct platform_driver wmt_pinctrl_driver = {
+	.probe	= vt8500_pinctrl_probe,
+	.remove	= vt8500_pinctrl_remove,
+	.driver = {
+		.name	= "pinctrl-vt8500",
+		.owner	= THIS_MODULE,
+		.of_match_table	= wmt_pinctrl_of_match,
+	},
+};
+
+module_platform_driver(wmt_pinctrl_driver);
+
+MODULE_AUTHOR("Tony Prisk <linux@prisktech.co.nz>");
+MODULE_DESCRIPTION("VIA VT8500 Pincontrol driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, wmt_pinctrl_of_match);
diff --git a/drivers/pinctrl/vt8500/pinctrl-wm8505.c b/drivers/pinctrl/vt8500/pinctrl-wm8505.c
new file mode 100644
index 000000000000..483ba732694e
--- /dev/null
+++ b/drivers/pinctrl/vt8500/pinctrl-wm8505.c
@@ -0,0 +1,532 @@
+/*
+ * Pinctrl data for Wondermedia WM8505 SoC
+ *
+ * Copyright (c) 2013 Tony Prisk <linux@prisktech.co.nz>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include "pinctrl-wmt.h"
+
+/*
+ * Describe the register offsets within the GPIO memory space
+ * The dedicated external GPIO's should always be listed in bank 0
+ * so they are exported in the 0..31 range which is what users
+ * expect.
+ *
+ * Do not reorder these banks as it will change the pin numbering
+ */
+static const struct wmt_pinctrl_bank_registers wm8505_banks[] = {
+	WMT_PINCTRL_BANK(0x64, 0x8C, 0xB4, 0xDC, NO_REG, NO_REG),	/* 0 */
+	WMT_PINCTRL_BANK(0x40, 0x68, 0x90, 0xB8, NO_REG, NO_REG),	/* 1 */
+	WMT_PINCTRL_BANK(0x44, 0x6C, 0x94, 0xBC, NO_REG, NO_REG),	/* 2 */
+	WMT_PINCTRL_BANK(0x48, 0x70, 0x98, 0xC0, NO_REG, NO_REG),	/* 3 */
+	WMT_PINCTRL_BANK(0x4C, 0x74, 0x9C, 0xC4, NO_REG, NO_REG),	/* 4 */
+	WMT_PINCTRL_BANK(0x50, 0x78, 0xA0, 0xC8, NO_REG, NO_REG),	/* 5 */
+	WMT_PINCTRL_BANK(0x54, 0x7C, 0xA4, 0xD0, NO_REG, NO_REG),	/* 6 */
+	WMT_PINCTRL_BANK(0x58, 0x80, 0xA8, 0xD4, NO_REG, NO_REG),	/* 7 */
+	WMT_PINCTRL_BANK(0x5C, 0x84, 0xAC, 0xD8, NO_REG, NO_REG),	/* 8 */
+	WMT_PINCTRL_BANK(0x60, 0x88, 0xB0, 0xDC, NO_REG, NO_REG),	/* 9 */
+	WMT_PINCTRL_BANK(0x500, 0x504, 0x508, 0x50C, NO_REG, NO_REG),	/* 10 */
+};
+
+/* Please keep sorted by bank/bit */
+#define WMT_PIN_EXTGPIO0	WMT_PIN(0, 0)
+#define WMT_PIN_EXTGPIO1	WMT_PIN(0, 1)
+#define WMT_PIN_EXTGPIO2	WMT_PIN(0, 2)
+#define WMT_PIN_EXTGPIO3	WMT_PIN(0, 3)
+#define WMT_PIN_EXTGPIO4	WMT_PIN(0, 4)
+#define WMT_PIN_EXTGPIO5	WMT_PIN(0, 5)
+#define WMT_PIN_EXTGPIO6	WMT_PIN(0, 6)
+#define WMT_PIN_EXTGPIO7	WMT_PIN(0, 7)
+#define WMT_PIN_WAKEUP0		WMT_PIN(0, 16)
+#define WMT_PIN_WAKEUP1		WMT_PIN(0, 17)
+#define WMT_PIN_WAKEUP2		WMT_PIN(0, 18)
+#define WMT_PIN_WAKEUP3		WMT_PIN(0, 19)
+#define WMT_PIN_SUSGPIO0	WMT_PIN(0, 21)
+#define WMT_PIN_SDDATA0		WMT_PIN(1, 0)
+#define WMT_PIN_SDDATA1		WMT_PIN(1, 1)
+#define WMT_PIN_SDDATA2		WMT_PIN(1, 2)
+#define WMT_PIN_SDDATA3		WMT_PIN(1, 3)
+#define WMT_PIN_MMCDATA0	WMT_PIN(1, 4)
+#define WMT_PIN_MMCDATA1	WMT_PIN(1, 5)
+#define WMT_PIN_MMCDATA2	WMT_PIN(1, 6)
+#define WMT_PIN_MMCDATA3	WMT_PIN(1, 7)
+#define WMT_PIN_VDIN0		WMT_PIN(2, 0)
+#define WMT_PIN_VDIN1		WMT_PIN(2, 1)
+#define WMT_PIN_VDIN2		WMT_PIN(2, 2)
+#define WMT_PIN_VDIN3		WMT_PIN(2, 3)
+#define WMT_PIN_VDIN4		WMT_PIN(2, 4)
+#define WMT_PIN_VDIN5		WMT_PIN(2, 5)
+#define WMT_PIN_VDIN6		WMT_PIN(2, 6)
+#define WMT_PIN_VDIN7		WMT_PIN(2, 7)
+#define WMT_PIN_VDOUT0		WMT_PIN(2, 8)
+#define WMT_PIN_VDOUT1		WMT_PIN(2, 9)
+#define WMT_PIN_VDOUT2		WMT_PIN(2, 10)
+#define WMT_PIN_VDOUT3		WMT_PIN(2, 11)
+#define WMT_PIN_VDOUT4		WMT_PIN(2, 12)
+#define WMT_PIN_VDOUT5		WMT_PIN(2, 13)
+#define WMT_PIN_VDOUT6		WMT_PIN(2, 14)
+#define WMT_PIN_VDOUT7		WMT_PIN(2, 15)
+#define WMT_PIN_VDOUT8		WMT_PIN(2, 16)
+#define WMT_PIN_VDOUT9		WMT_PIN(2, 17)
+#define WMT_PIN_VDOUT10		WMT_PIN(2, 18)
+#define WMT_PIN_VDOUT11		WMT_PIN(2, 19)
+#define WMT_PIN_VDOUT12		WMT_PIN(2, 20)
+#define WMT_PIN_VDOUT13		WMT_PIN(2, 21)
+#define WMT_PIN_VDOUT14		WMT_PIN(2, 22)
+#define WMT_PIN_VDOUT15		WMT_PIN(2, 23)
+#define WMT_PIN_VDOUT16		WMT_PIN(2, 24)
+#define WMT_PIN_VDOUT17		WMT_PIN(2, 25)
+#define WMT_PIN_VDOUT18		WMT_PIN(2, 26)
+#define WMT_PIN_VDOUT19		WMT_PIN(2, 27)
+#define WMT_PIN_VDOUT20		WMT_PIN(2, 28)
+#define WMT_PIN_VDOUT21		WMT_PIN(2, 29)
+#define WMT_PIN_VDOUT22		WMT_PIN(2, 30)
+#define WMT_PIN_VDOUT23		WMT_PIN(2, 31)
+#define WMT_PIN_VHSYNC		WMT_PIN(3, 0)
+#define WMT_PIN_VVSYNC		WMT_PIN(3, 1)
+#define WMT_PIN_VGAHSYNC	WMT_PIN(3, 2)
+#define WMT_PIN_VGAVSYNC	WMT_PIN(3, 3)
+#define WMT_PIN_VDHSYNC		WMT_PIN(3, 4)
+#define WMT_PIN_VDVSYNC		WMT_PIN(3, 5)
+#define WMT_PIN_NORD0		WMT_PIN(4, 0)
+#define WMT_PIN_NORD1		WMT_PIN(4, 1)
+#define WMT_PIN_NORD2		WMT_PIN(4, 2)
+#define WMT_PIN_NORD3		WMT_PIN(4, 3)
+#define WMT_PIN_NORD4		WMT_PIN(4, 4)
+#define WMT_PIN_NORD5		WMT_PIN(4, 5)
+#define WMT_PIN_NORD6		WMT_PIN(4, 6)
+#define WMT_PIN_NORD7		WMT_PIN(4, 7)
+#define WMT_PIN_NORD8		WMT_PIN(4, 8)
+#define WMT_PIN_NORD9		WMT_PIN(4, 9)
+#define WMT_PIN_NORD10		WMT_PIN(4, 10)
+#define WMT_PIN_NORD11		WMT_PIN(4, 11)
+#define WMT_PIN_NORD12		WMT_PIN(4, 12)
+#define WMT_PIN_NORD13		WMT_PIN(4, 13)
+#define WMT_PIN_NORD14		WMT_PIN(4, 14)
+#define WMT_PIN_NORD15		WMT_PIN(4, 15)
+#define WMT_PIN_NORA0		WMT_PIN(5, 0)
+#define WMT_PIN_NORA1		WMT_PIN(5, 1)
+#define WMT_PIN_NORA2		WMT_PIN(5, 2)
+#define WMT_PIN_NORA3		WMT_PIN(5, 3)
+#define WMT_PIN_NORA4		WMT_PIN(5, 4)
+#define WMT_PIN_NORA5		WMT_PIN(5, 5)
+#define WMT_PIN_NORA6		WMT_PIN(5, 6)
+#define WMT_PIN_NORA7		WMT_PIN(5, 7)
+#define WMT_PIN_NORA8		WMT_PIN(5, 8)
+#define WMT_PIN_NORA9		WMT_PIN(5, 9)
+#define WMT_PIN_NORA10		WMT_PIN(5, 10)
+#define WMT_PIN_NORA11		WMT_PIN(5, 11)
+#define WMT_PIN_NORA12		WMT_PIN(5, 12)
+#define WMT_PIN_NORA13		WMT_PIN(5, 13)
+#define WMT_PIN_NORA14		WMT_PIN(5, 14)
+#define WMT_PIN_NORA15		WMT_PIN(5, 15)
+#define WMT_PIN_NORA16		WMT_PIN(5, 16)
+#define WMT_PIN_NORA17		WMT_PIN(5, 17)
+#define WMT_PIN_NORA18		WMT_PIN(5, 18)
+#define WMT_PIN_NORA19		WMT_PIN(5, 19)
+#define WMT_PIN_NORA20		WMT_PIN(5, 20)
+#define WMT_PIN_NORA21		WMT_PIN(5, 21)
+#define WMT_PIN_NORA22		WMT_PIN(5, 22)
+#define WMT_PIN_NORA23		WMT_PIN(5, 23)
+#define WMT_PIN_NORA24		WMT_PIN(5, 24)
+#define WMT_PIN_AC97SDI		WMT_PIN(6, 0)
+#define WMT_PIN_AC97SYNC	WMT_PIN(6, 1)
+#define WMT_PIN_AC97SDO		WMT_PIN(6, 2)
+#define WMT_PIN_AC97BCLK	WMT_PIN(6, 3)
+#define WMT_PIN_AC97RST		WMT_PIN(6, 4)
+#define WMT_PIN_SFDO		WMT_PIN(7, 0)
+#define WMT_PIN_SFCS0		WMT_PIN(7, 1)
+#define WMT_PIN_SFCS1		WMT_PIN(7, 2)
+#define WMT_PIN_SFCLK		WMT_PIN(7, 3)
+#define WMT_PIN_SFDI		WMT_PIN(7, 4)
+#define WMT_PIN_SPI0CLK		WMT_PIN(8, 0)
+#define WMT_PIN_SPI0MISO	WMT_PIN(8, 1)
+#define WMT_PIN_SPI0MOSI	WMT_PIN(8, 2)
+#define WMT_PIN_SPI0SS		WMT_PIN(8, 3)
+#define WMT_PIN_SPI1CLK		WMT_PIN(8, 4)
+#define WMT_PIN_SPI1MISO	WMT_PIN(8, 5)
+#define WMT_PIN_SPI1MOSI	WMT_PIN(8, 6)
+#define WMT_PIN_SPI1SS		WMT_PIN(8, 7)
+#define WMT_PIN_SPI2CLK		WMT_PIN(8, 8)
+#define WMT_PIN_SPI2MISO	WMT_PIN(8, 9)
+#define WMT_PIN_SPI2MOSI	WMT_PIN(8, 10)
+#define WMT_PIN_SPI2SS		WMT_PIN(8, 11)
+#define WMT_PIN_UART0_RTS	WMT_PIN(9, 0)
+#define WMT_PIN_UART0_TXD	WMT_PIN(9, 1)
+#define WMT_PIN_UART0_CTS	WMT_PIN(9, 2)
+#define WMT_PIN_UART0_RXD	WMT_PIN(9, 3)
+#define WMT_PIN_UART1_RTS	WMT_PIN(9, 4)
+#define WMT_PIN_UART1_TXD	WMT_PIN(9, 5)
+#define WMT_PIN_UART1_CTS	WMT_PIN(9, 6)
+#define WMT_PIN_UART1_RXD	WMT_PIN(9, 7)
+#define WMT_PIN_UART2_RTS	WMT_PIN(9, 8)
+#define WMT_PIN_UART2_TXD	WMT_PIN(9, 9)
+#define WMT_PIN_UART2_CTS	WMT_PIN(9, 10)
+#define WMT_PIN_UART2_RXD	WMT_PIN(9, 11)
+#define WMT_PIN_UART3_RTS	WMT_PIN(9, 12)
+#define WMT_PIN_UART3_TXD	WMT_PIN(9, 13)
+#define WMT_PIN_UART3_CTS	WMT_PIN(9, 14)
+#define WMT_PIN_UART3_RXD	WMT_PIN(9, 15)
+#define WMT_PIN_I2C0SCL		WMT_PIN(10, 0)
+#define WMT_PIN_I2C0SDA		WMT_PIN(10, 1)
+#define WMT_PIN_I2C1SCL		WMT_PIN(10, 2)
+#define WMT_PIN_I2C1SDA		WMT_PIN(10, 3)
+#define WMT_PIN_I2C2SCL		WMT_PIN(10, 4)
+#define WMT_PIN_I2C2SDA		WMT_PIN(10, 5)
+
+static const struct pinctrl_pin_desc wm8505_pins[] = {
+	PINCTRL_PIN(WMT_PIN_EXTGPIO0, "extgpio0"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO1, "extgpio1"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO2, "extgpio2"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO3, "extgpio3"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO4, "extgpio4"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO5, "extgpio5"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO6, "extgpio6"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO7, "extgpio7"),
+	PINCTRL_PIN(WMT_PIN_WAKEUP0, "wakeup0"),
+	PINCTRL_PIN(WMT_PIN_WAKEUP1, "wakeup1"),
+	PINCTRL_PIN(WMT_PIN_WAKEUP2, "wakeup2"),
+	PINCTRL_PIN(WMT_PIN_WAKEUP3, "wakeup3"),
+	PINCTRL_PIN(WMT_PIN_SUSGPIO0, "susgpio0"),
+	PINCTRL_PIN(WMT_PIN_SDDATA0, "sd_data0"),
+	PINCTRL_PIN(WMT_PIN_SDDATA1, "sd_data1"),
+	PINCTRL_PIN(WMT_PIN_SDDATA2, "sd_data2"),
+	PINCTRL_PIN(WMT_PIN_SDDATA3, "sd_data3"),
+	PINCTRL_PIN(WMT_PIN_MMCDATA0, "mmc_data0"),
+	PINCTRL_PIN(WMT_PIN_MMCDATA1, "mmc_data1"),
+	PINCTRL_PIN(WMT_PIN_MMCDATA2, "mmc_data2"),
+	PINCTRL_PIN(WMT_PIN_MMCDATA3, "mmc_data3"),
+	PINCTRL_PIN(WMT_PIN_VDIN0, "vdin0"),
+	PINCTRL_PIN(WMT_PIN_VDIN1, "vdin1"),
+	PINCTRL_PIN(WMT_PIN_VDIN2, "vdin2"),
+	PINCTRL_PIN(WMT_PIN_VDIN3, "vdin3"),
+	PINCTRL_PIN(WMT_PIN_VDIN4, "vdin4"),
+	PINCTRL_PIN(WMT_PIN_VDIN5, "vdin5"),
+	PINCTRL_PIN(WMT_PIN_VDIN6, "vdin6"),
+	PINCTRL_PIN(WMT_PIN_VDIN7, "vdin7"),
+	PINCTRL_PIN(WMT_PIN_VDOUT0, "vdout0"),
+	PINCTRL_PIN(WMT_PIN_VDOUT1, "vdout1"),
+	PINCTRL_PIN(WMT_PIN_VDOUT2, "vdout2"),
+	PINCTRL_PIN(WMT_PIN_VDOUT3, "vdout3"),
+	PINCTRL_PIN(WMT_PIN_VDOUT4, "vdout4"),
+	PINCTRL_PIN(WMT_PIN_VDOUT5, "vdout5"),
+	PINCTRL_PIN(WMT_PIN_VDOUT6, "vdout6"),
+	PINCTRL_PIN(WMT_PIN_VDOUT7, "vdout7"),
+	PINCTRL_PIN(WMT_PIN_VDOUT8, "vdout8"),
+	PINCTRL_PIN(WMT_PIN_VDOUT9, "vdout9"),
+	PINCTRL_PIN(WMT_PIN_VDOUT10, "vdout10"),
+	PINCTRL_PIN(WMT_PIN_VDOUT11, "vdout11"),
+	PINCTRL_PIN(WMT_PIN_VDOUT12, "vdout12"),
+	PINCTRL_PIN(WMT_PIN_VDOUT13, "vdout13"),
+	PINCTRL_PIN(WMT_PIN_VDOUT14, "vdout14"),
+	PINCTRL_PIN(WMT_PIN_VDOUT15, "vdout15"),
+	PINCTRL_PIN(WMT_PIN_VDOUT16, "vdout16"),
+	PINCTRL_PIN(WMT_PIN_VDOUT17, "vdout17"),
+	PINCTRL_PIN(WMT_PIN_VDOUT18, "vdout18"),
+	PINCTRL_PIN(WMT_PIN_VDOUT19, "vdout19"),
+	PINCTRL_PIN(WMT_PIN_VDOUT20, "vdout20"),
+	PINCTRL_PIN(WMT_PIN_VDOUT21, "vdout21"),
+	PINCTRL_PIN(WMT_PIN_VDOUT22, "vdout22"),
+	PINCTRL_PIN(WMT_PIN_VDOUT23, "vdout23"),
+	PINCTRL_PIN(WMT_PIN_VHSYNC, "v_hsync"),
+	PINCTRL_PIN(WMT_PIN_VVSYNC, "v_vsync"),
+	PINCTRL_PIN(WMT_PIN_VGAHSYNC, "vga_hsync"),
+	PINCTRL_PIN(WMT_PIN_VGAVSYNC, "vga_vsync"),
+	PINCTRL_PIN(WMT_PIN_VDHSYNC, "vd_hsync"),
+	PINCTRL_PIN(WMT_PIN_VDVSYNC, "vd_vsync"),
+	PINCTRL_PIN(WMT_PIN_NORD0, "nor_d0"),
+	PINCTRL_PIN(WMT_PIN_NORD1, "nor_d1"),
+	PINCTRL_PIN(WMT_PIN_NORD2, "nor_d2"),
+	PINCTRL_PIN(WMT_PIN_NORD3, "nor_d3"),
+	PINCTRL_PIN(WMT_PIN_NORD4, "nor_d4"),
+	PINCTRL_PIN(WMT_PIN_NORD5, "nor_d5"),
+	PINCTRL_PIN(WMT_PIN_NORD6, "nor_d6"),
+	PINCTRL_PIN(WMT_PIN_NORD7, "nor_d7"),
+	PINCTRL_PIN(WMT_PIN_NORD8, "nor_d8"),
+	PINCTRL_PIN(WMT_PIN_NORD9, "nor_d9"),
+	PINCTRL_PIN(WMT_PIN_NORD10, "nor_d10"),
+	PINCTRL_PIN(WMT_PIN_NORD11, "nor_d11"),
+	PINCTRL_PIN(WMT_PIN_NORD12, "nor_d12"),
+	PINCTRL_PIN(WMT_PIN_NORD13, "nor_d13"),
+	PINCTRL_PIN(WMT_PIN_NORD14, "nor_d14"),
+	PINCTRL_PIN(WMT_PIN_NORD15, "nor_d15"),
+	PINCTRL_PIN(WMT_PIN_NORA0, "nor_a0"),
+	PINCTRL_PIN(WMT_PIN_NORA1, "nor_a1"),
+	PINCTRL_PIN(WMT_PIN_NORA2, "nor_a2"),
+	PINCTRL_PIN(WMT_PIN_NORA3, "nor_a3"),
+	PINCTRL_PIN(WMT_PIN_NORA4, "nor_a4"),
+	PINCTRL_PIN(WMT_PIN_NORA5, "nor_a5"),
+	PINCTRL_PIN(WMT_PIN_NORA6, "nor_a6"),
+	PINCTRL_PIN(WMT_PIN_NORA7, "nor_a7"),
+	PINCTRL_PIN(WMT_PIN_NORA8, "nor_a8"),
+	PINCTRL_PIN(WMT_PIN_NORA9, "nor_a9"),
+	PINCTRL_PIN(WMT_PIN_NORA10, "nor_a10"),
+	PINCTRL_PIN(WMT_PIN_NORA11, "nor_a11"),
+	PINCTRL_PIN(WMT_PIN_NORA12, "nor_a12"),
+	PINCTRL_PIN(WMT_PIN_NORA13, "nor_a13"),
+	PINCTRL_PIN(WMT_PIN_NORA14, "nor_a14"),
+	PINCTRL_PIN(WMT_PIN_NORA15, "nor_a15"),
+	PINCTRL_PIN(WMT_PIN_NORA16, "nor_a16"),
+	PINCTRL_PIN(WMT_PIN_NORA17, "nor_a17"),
+	PINCTRL_PIN(WMT_PIN_NORA18, "nor_a18"),
+	PINCTRL_PIN(WMT_PIN_NORA19, "nor_a19"),
+	PINCTRL_PIN(WMT_PIN_NORA20, "nor_a20"),
+	PINCTRL_PIN(WMT_PIN_NORA21, "nor_a21"),
+	PINCTRL_PIN(WMT_PIN_NORA22, "nor_a22"),
+	PINCTRL_PIN(WMT_PIN_NORA23, "nor_a23"),
+	PINCTRL_PIN(WMT_PIN_NORA24, "nor_a24"),
+	PINCTRL_PIN(WMT_PIN_AC97SDI, "ac97_sdi"),
+	PINCTRL_PIN(WMT_PIN_AC97SYNC, "ac97_sync"),
+	PINCTRL_PIN(WMT_PIN_AC97SDO, "ac97_sdo"),
+	PINCTRL_PIN(WMT_PIN_AC97BCLK, "ac97_bclk"),
+	PINCTRL_PIN(WMT_PIN_AC97RST, "ac97_rst"),
+	PINCTRL_PIN(WMT_PIN_SFDO, "sf_do"),
+	PINCTRL_PIN(WMT_PIN_SFCS0, "sf_cs0"),
+	PINCTRL_PIN(WMT_PIN_SFCS1, "sf_cs1"),
+	PINCTRL_PIN(WMT_PIN_SFCLK, "sf_clk"),
+	PINCTRL_PIN(WMT_PIN_SFDI, "sf_di"),
+	PINCTRL_PIN(WMT_PIN_SPI0CLK, "spi0_clk"),
+	PINCTRL_PIN(WMT_PIN_SPI0MISO, "spi0_miso"),
+	PINCTRL_PIN(WMT_PIN_SPI0MOSI, "spi0_mosi"),
+	PINCTRL_PIN(WMT_PIN_SPI0SS, "spi0_ss"),
+	PINCTRL_PIN(WMT_PIN_SPI1CLK, "spi1_clk"),
+	PINCTRL_PIN(WMT_PIN_SPI1MISO, "spi1_miso"),
+	PINCTRL_PIN(WMT_PIN_SPI1MOSI, "spi1_mosi"),
+	PINCTRL_PIN(WMT_PIN_SPI1SS, "spi1_ss"),
+	PINCTRL_PIN(WMT_PIN_SPI2CLK, "spi2_clk"),
+	PINCTRL_PIN(WMT_PIN_SPI2MISO, "spi2_miso"),
+	PINCTRL_PIN(WMT_PIN_SPI2MOSI, "spi2_mosi"),
+	PINCTRL_PIN(WMT_PIN_SPI2SS, "spi2_ss"),
+	PINCTRL_PIN(WMT_PIN_UART0_RTS, "uart0_rts"),
+	PINCTRL_PIN(WMT_PIN_UART0_TXD, "uart0_txd"),
+	PINCTRL_PIN(WMT_PIN_UART0_CTS, "uart0_cts"),
+	PINCTRL_PIN(WMT_PIN_UART0_RXD, "uart0_rxd"),
+	PINCTRL_PIN(WMT_PIN_UART1_RTS, "uart1_rts"),
+	PINCTRL_PIN(WMT_PIN_UART1_TXD, "uart1_txd"),
+	PINCTRL_PIN(WMT_PIN_UART1_CTS, "uart1_cts"),
+	PINCTRL_PIN(WMT_PIN_UART1_RXD, "uart1_rxd"),
+	PINCTRL_PIN(WMT_PIN_UART2_RTS, "uart2_rts"),
+	PINCTRL_PIN(WMT_PIN_UART2_TXD, "uart2_txd"),
+	PINCTRL_PIN(WMT_PIN_UART2_CTS, "uart2_cts"),
+	PINCTRL_PIN(WMT_PIN_UART2_RXD, "uart2_rxd"),
+	PINCTRL_PIN(WMT_PIN_UART3_RTS, "uart3_rts"),
+	PINCTRL_PIN(WMT_PIN_UART3_TXD, "uart3_txd"),
+	PINCTRL_PIN(WMT_PIN_UART3_CTS, "uart3_cts"),
+	PINCTRL_PIN(WMT_PIN_UART3_RXD, "uart3_rxd"),
+	PINCTRL_PIN(WMT_PIN_I2C0SCL, "i2c0_scl"),
+	PINCTRL_PIN(WMT_PIN_I2C0SDA, "i2c0_sda"),
+	PINCTRL_PIN(WMT_PIN_I2C1SCL, "i2c1_scl"),
+	PINCTRL_PIN(WMT_PIN_I2C1SDA, "i2c1_sda"),
+	PINCTRL_PIN(WMT_PIN_I2C2SCL, "i2c2_scl"),
+	PINCTRL_PIN(WMT_PIN_I2C2SDA, "i2c2_sda"),
+};
+
+/* Order of these names must match the above list */
+static const char * const wm8505_groups[] = {
+	"extgpio0",
+	"extgpio1",
+	"extgpio2",
+	"extgpio3",
+	"extgpio4",
+	"extgpio5",
+	"extgpio6",
+	"extgpio7",
+	"wakeup0",
+	"wakeup1",
+	"wakeup2",
+	"wakeup3",
+	"susgpio0",
+	"sd_data0",
+	"sd_data1",
+	"sd_data2",
+	"sd_data3",
+	"mmc_data0",
+	"mmc_data1",
+	"mmc_data2",
+	"mmc_data3",
+	"vdin0",
+	"vdin1",
+	"vdin2",
+	"vdin3",
+	"vdin4",
+	"vdin5",
+	"vdin6",
+	"vdin7",
+	"vdout0",
+	"vdout1",
+	"vdout2",
+	"vdout3",
+	"vdout4",
+	"vdout5",
+	"vdout6",
+	"vdout7",
+	"vdout8",
+	"vdout9",
+	"vdout10",
+	"vdout11",
+	"vdout12",
+	"vdout13",
+	"vdout14",
+	"vdout15",
+	"vdout16",
+	"vdout17",
+	"vdout18",
+	"vdout19",
+	"vdout20",
+	"vdout21",
+	"vdout22",
+	"vdout23",
+	"v_hsync",
+	"v_vsync",
+	"vga_hsync",
+	"vga_vsync",
+	"vd_hsync",
+	"vd_vsync",
+	"nor_d0",
+	"nor_d1",
+	"nor_d2",
+	"nor_d3",
+	"nor_d4",
+	"nor_d5",
+	"nor_d6",
+	"nor_d7",
+	"nor_d8",
+	"nor_d9",
+	"nor_d10",
+	"nor_d11",
+	"nor_d12",
+	"nor_d13",
+	"nor_d14",
+	"nor_d15",
+	"nor_a0",
+	"nor_a1",
+	"nor_a2",
+	"nor_a3",
+	"nor_a4",
+	"nor_a5",
+	"nor_a6",
+	"nor_a7",
+	"nor_a8",
+	"nor_a9",
+	"nor_a10",
+	"nor_a11",
+	"nor_a12",
+	"nor_a13",
+	"nor_a14",
+	"nor_a15",
+	"nor_a16",
+	"nor_a17",
+	"nor_a18",
+	"nor_a19",
+	"nor_a20",
+	"nor_a21",
+	"nor_a22",
+	"nor_a23",
+	"nor_a24",
+	"ac97_sdi",
+	"ac97_sync",
+	"ac97_sdo",
+	"ac97_bclk",
+	"ac97_rst",
+	"sf_do",
+	"sf_cs0",
+	"sf_cs1",
+	"sf_clk",
+	"sf_di",
+	"spi0_clk",
+	"spi0_miso",
+	"spi0_mosi",
+	"spi0_ss",
+	"spi1_clk",
+	"spi1_miso",
+	"spi1_mosi",
+	"spi1_ss",
+	"spi2_clk",
+	"spi2_miso",
+	"spi2_mosi",
+	"spi2_ss",
+	"uart0_rts",
+	"uart0_txd",
+	"uart0_cts",
+	"uart0_rxd",
+	"uart1_rts",
+	"uart1_txd",
+	"uart1_cts",
+	"uart1_rxd",
+	"uart2_rts",
+	"uart2_txd",
+	"uart2_cts",
+	"uart2_rxd",
+	"uart3_rts",
+	"uart3_txd",
+	"uart3_cts",
+	"uart3_rxd",
+	"i2c0_scl",
+	"i2c0_sda",
+	"i2c1_scl",
+	"i2c1_sda",
+	"i2c2_scl",
+	"i2c2_sda",
+};
+
+static int wm8505_pinctrl_probe(struct platform_device *pdev)
+{
+	struct wmt_pinctrl_data *data;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		dev_err(&pdev->dev, "failed to allocate data\n");
+		return -ENOMEM;
+	}
+
+	data->banks = wm8505_banks;
+	data->nbanks = ARRAY_SIZE(wm8505_banks);
+	data->pins = wm8505_pins;
+	data->npins = ARRAY_SIZE(wm8505_pins);
+	data->groups = wm8505_groups;
+	data->ngroups = ARRAY_SIZE(wm8505_groups);
+
+	return wmt_pinctrl_probe(pdev, data);
+}
+
+static int wm8505_pinctrl_remove(struct platform_device *pdev)
+{
+	return wmt_pinctrl_remove(pdev);
+}
+
+static struct of_device_id wmt_pinctrl_of_match[] = {
+	{ .compatible = "wm,wm8505-pinctrl" },
+	{ /* sentinel */ },
+};
+
+static struct platform_driver wmt_pinctrl_driver = {
+	.probe	= wm8505_pinctrl_probe,
+	.remove	= wm8505_pinctrl_remove,
+	.driver = {
+		.name	= "pinctrl-wm8505",
+		.owner	= THIS_MODULE,
+		.of_match_table	= wmt_pinctrl_of_match,
+	},
+};
+
+module_platform_driver(wmt_pinctrl_driver);
+
+MODULE_AUTHOR("Tony Prisk <linux@prisktech.co.nz>");
+MODULE_DESCRIPTION("Wondermedia WM8505 Pincontrol driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, wmt_pinctrl_of_match);
diff --git a/drivers/pinctrl/vt8500/pinctrl-wm8650.c b/drivers/pinctrl/vt8500/pinctrl-wm8650.c
new file mode 100644
index 000000000000..7de57f063153
--- /dev/null
+++ b/drivers/pinctrl/vt8500/pinctrl-wm8650.c
@@ -0,0 +1,370 @@
+/*
+ * Pinctrl data for Wondermedia WM8650 SoC
+ *
+ * Copyright (c) 2013 Tony Prisk <linux@prisktech.co.nz>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include "pinctrl-wmt.h"
+
+/*
+ * Describe the register offsets within the GPIO memory space
+ * The dedicated external GPIO's should always be listed in bank 0
+ * so they are exported in the 0..31 range which is what users
+ * expect.
+ *
+ * Do not reorder these banks as it will change the pin numbering
+ */
+static const struct wmt_pinctrl_bank_registers wm8650_banks[] = {
+	WMT_PINCTRL_BANK(0x40, 0x80, 0xC0, 0x00, 0x480, 0x4C0),		/* 0 */
+	WMT_PINCTRL_BANK(0x44, 0x84, 0xC4, 0x04, 0x484, 0x4C4),		/* 1 */
+	WMT_PINCTRL_BANK(0x48, 0x88, 0xC8, 0x08, 0x488, 0x4C8),		/* 2 */
+	WMT_PINCTRL_BANK(0x4C, 0x8C, 0xCC, 0x0C, 0x48C, 0x4CC),		/* 3 */
+	WMT_PINCTRL_BANK(0x50, 0x90, 0xD0, 0x10, 0x490, 0x4D0),		/* 4 */
+	WMT_PINCTRL_BANK(0x54, 0x94, 0xD4, 0x14, 0x494, 0x4D4),		/* 5 */
+	WMT_PINCTRL_BANK(0x58, 0x98, 0xD8, 0x18, 0x498, 0x4D8),		/* 6 */
+	WMT_PINCTRL_BANK(0x5C, 0x9C, 0xDC, 0x1C, 0x49C, 0x4DC),		/* 7 */
+};
+
+/* Please keep sorted by bank/bit */
+#define WMT_PIN_EXTGPIO0	WMT_PIN(0, 0)
+#define WMT_PIN_EXTGPIO1	WMT_PIN(0, 1)
+#define WMT_PIN_EXTGPIO2	WMT_PIN(0, 2)
+#define WMT_PIN_EXTGPIO3	WMT_PIN(0, 3)
+#define WMT_PIN_EXTGPIO4	WMT_PIN(0, 4)
+#define WMT_PIN_EXTGPIO5	WMT_PIN(0, 5)
+#define WMT_PIN_EXTGPIO6	WMT_PIN(0, 6)
+#define WMT_PIN_EXTGPIO7	WMT_PIN(0, 7)
+#define WMT_PIN_WAKEUP0		WMT_PIN(0, 16)
+#define WMT_PIN_WAKEUP1		WMT_PIN(0, 17)
+#define WMT_PIN_SUSGPIO0	WMT_PIN(0, 21)
+#define WMT_PIN_SD0CD		WMT_PIN(0, 28)
+#define WMT_PIN_SD1CD		WMT_PIN(0, 29)
+#define WMT_PIN_VDOUT0		WMT_PIN(1, 0)
+#define WMT_PIN_VDOUT1		WMT_PIN(1, 1)
+#define WMT_PIN_VDOUT2		WMT_PIN(1, 2)
+#define WMT_PIN_VDOUT3		WMT_PIN(1, 3)
+#define WMT_PIN_VDOUT4		WMT_PIN(1, 4)
+#define WMT_PIN_VDOUT5		WMT_PIN(1, 5)
+#define WMT_PIN_VDOUT6		WMT_PIN(1, 6)
+#define WMT_PIN_VDOUT7		WMT_PIN(1, 7)
+#define WMT_PIN_VDOUT8		WMT_PIN(1, 8)
+#define WMT_PIN_VDOUT9		WMT_PIN(1, 9)
+#define WMT_PIN_VDOUT10		WMT_PIN(1, 10)
+#define WMT_PIN_VDOUT11		WMT_PIN(1, 11)
+#define WMT_PIN_VDOUT12		WMT_PIN(1, 12)
+#define WMT_PIN_VDOUT13		WMT_PIN(1, 13)
+#define WMT_PIN_VDOUT14		WMT_PIN(1, 14)
+#define WMT_PIN_VDOUT15		WMT_PIN(1, 15)
+#define WMT_PIN_VDOUT16		WMT_PIN(1, 16)
+#define WMT_PIN_VDOUT17		WMT_PIN(1, 17)
+#define WMT_PIN_VDOUT18		WMT_PIN(1, 18)
+#define WMT_PIN_VDOUT19		WMT_PIN(1, 19)
+#define WMT_PIN_VDOUT20		WMT_PIN(1, 20)
+#define WMT_PIN_VDOUT21		WMT_PIN(1, 21)
+#define WMT_PIN_VDOUT22		WMT_PIN(1, 22)
+#define WMT_PIN_VDOUT23		WMT_PIN(1, 23)
+#define WMT_PIN_VDIN0		WMT_PIN(2, 0)
+#define WMT_PIN_VDIN1		WMT_PIN(2, 1)
+#define WMT_PIN_VDIN2		WMT_PIN(2, 2)
+#define WMT_PIN_VDIN3		WMT_PIN(2, 3)
+#define WMT_PIN_VDIN4		WMT_PIN(2, 4)
+#define WMT_PIN_VDIN5		WMT_PIN(2, 5)
+#define WMT_PIN_VDIN6		WMT_PIN(2, 6)
+#define WMT_PIN_VDIN7		WMT_PIN(2, 7)
+#define WMT_PIN_I2C1SCL		WMT_PIN(2, 12)
+#define WMT_PIN_I2C1SDA		WMT_PIN(2, 13)
+#define WMT_PIN_SPI0MOSI	WMT_PIN(2, 24)
+#define WMT_PIN_SPI0MISO	WMT_PIN(2, 25)
+#define WMT_PIN_SPI0SS0		WMT_PIN(2, 26)
+#define WMT_PIN_SPI0CLK		WMT_PIN(2, 27)
+#define WMT_PIN_SD0DATA0	WMT_PIN(3, 8)
+#define WMT_PIN_SD0DATA1	WMT_PIN(3, 9)
+#define WMT_PIN_SD0DATA2	WMT_PIN(3, 10)
+#define WMT_PIN_SD0DATA3	WMT_PIN(3, 11)
+#define WMT_PIN_SD0CLK		WMT_PIN(3, 12)
+#define WMT_PIN_SD0WP		WMT_PIN(3, 13)
+#define WMT_PIN_SD0CMD		WMT_PIN(3, 14)
+#define WMT_PIN_SD1DATA0	WMT_PIN(3, 24)
+#define WMT_PIN_SD1DATA1	WMT_PIN(3, 25)
+#define WMT_PIN_SD1DATA2	WMT_PIN(3, 26)
+#define WMT_PIN_SD1DATA3	WMT_PIN(3, 27)
+#define WMT_PIN_SD1DATA4	WMT_PIN(3, 28)
+#define WMT_PIN_SD1DATA5	WMT_PIN(3, 29)
+#define WMT_PIN_SD1DATA6	WMT_PIN(3, 30)
+#define WMT_PIN_SD1DATA7	WMT_PIN(3, 31)
+#define WMT_PIN_I2C0SCL		WMT_PIN(5, 8)
+#define WMT_PIN_I2C0SDA		WMT_PIN(5, 9)
+#define WMT_PIN_UART0RTS	WMT_PIN(5, 16)
+#define WMT_PIN_UART0TXD	WMT_PIN(5, 17)
+#define WMT_PIN_UART0CTS	WMT_PIN(5, 18)
+#define WMT_PIN_UART0RXD	WMT_PIN(5, 19)
+#define WMT_PIN_UART1RTS	WMT_PIN(5, 20)
+#define WMT_PIN_UART1TXD	WMT_PIN(5, 21)
+#define WMT_PIN_UART1CTS	WMT_PIN(5, 22)
+#define WMT_PIN_UART1RXD	WMT_PIN(5, 23)
+#define WMT_PIN_UART2RTS	WMT_PIN(5, 24)
+#define WMT_PIN_UART2TXD	WMT_PIN(5, 25)
+#define WMT_PIN_UART2CTS	WMT_PIN(5, 26)
+#define WMT_PIN_UART2RXD	WMT_PIN(5, 27)
+#define WMT_PIN_UART3RTS	WMT_PIN(5, 28)
+#define WMT_PIN_UART3TXD	WMT_PIN(5, 29)
+#define WMT_PIN_UART3CTS	WMT_PIN(5, 30)
+#define WMT_PIN_UART3RXD	WMT_PIN(5, 31)
+#define WMT_PIN_KPADROW0	WMT_PIN(6, 16)
+#define WMT_PIN_KPADROW1	WMT_PIN(6, 17)
+#define WMT_PIN_KPADCOL0	WMT_PIN(6, 18)
+#define WMT_PIN_KPADCOL1	WMT_PIN(6, 19)
+#define WMT_PIN_SD1CLK		WMT_PIN(7, 0)
+#define WMT_PIN_SD1CMD		WMT_PIN(7, 1)
+#define WMT_PIN_SD1WP		WMT_PIN(7, 13)
+
+static const struct pinctrl_pin_desc wm8650_pins[] = {
+	PINCTRL_PIN(WMT_PIN_EXTGPIO0, "extgpio0"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO1, "extgpio1"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO2, "extgpio2"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO3, "extgpio3"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO4, "extgpio4"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO5, "extgpio5"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO6, "extgpio6"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO7, "extgpio7"),
+	PINCTRL_PIN(WMT_PIN_WAKEUP0, "wakeup0"),
+	PINCTRL_PIN(WMT_PIN_WAKEUP1, "wakeup1"),
+	PINCTRL_PIN(WMT_PIN_SUSGPIO0, "susgpio0"),
+	PINCTRL_PIN(WMT_PIN_SD0CD, "sd0_cd"),
+	PINCTRL_PIN(WMT_PIN_SD1CD, "sd1_cd"),
+	PINCTRL_PIN(WMT_PIN_VDOUT0, "vdout0"),
+	PINCTRL_PIN(WMT_PIN_VDOUT1, "vdout1"),
+	PINCTRL_PIN(WMT_PIN_VDOUT2, "vdout2"),
+	PINCTRL_PIN(WMT_PIN_VDOUT3, "vdout3"),
+	PINCTRL_PIN(WMT_PIN_VDOUT4, "vdout4"),
+	PINCTRL_PIN(WMT_PIN_VDOUT5, "vdout5"),
+	PINCTRL_PIN(WMT_PIN_VDOUT6, "vdout6"),
+	PINCTRL_PIN(WMT_PIN_VDOUT7, "vdout7"),
+	PINCTRL_PIN(WMT_PIN_VDOUT8, "vdout8"),
+	PINCTRL_PIN(WMT_PIN_VDOUT9, "vdout9"),
+	PINCTRL_PIN(WMT_PIN_VDOUT10, "vdout10"),
+	PINCTRL_PIN(WMT_PIN_VDOUT11, "vdout11"),
+	PINCTRL_PIN(WMT_PIN_VDOUT12, "vdout12"),
+	PINCTRL_PIN(WMT_PIN_VDOUT13, "vdout13"),
+	PINCTRL_PIN(WMT_PIN_VDOUT14, "vdout14"),
+	PINCTRL_PIN(WMT_PIN_VDOUT15, "vdout15"),
+	PINCTRL_PIN(WMT_PIN_VDOUT16, "vdout16"),
+	PINCTRL_PIN(WMT_PIN_VDOUT17, "vdout17"),
+	PINCTRL_PIN(WMT_PIN_VDOUT18, "vdout18"),
+	PINCTRL_PIN(WMT_PIN_VDOUT19, "vdout19"),
+	PINCTRL_PIN(WMT_PIN_VDOUT20, "vdout20"),
+	PINCTRL_PIN(WMT_PIN_VDOUT21, "vdout21"),
+	PINCTRL_PIN(WMT_PIN_VDOUT22, "vdout22"),
+	PINCTRL_PIN(WMT_PIN_VDOUT23, "vdout23"),
+	PINCTRL_PIN(WMT_PIN_VDIN0, "vdin0"),
+	PINCTRL_PIN(WMT_PIN_VDIN1, "vdin1"),
+	PINCTRL_PIN(WMT_PIN_VDIN2, "vdin2"),
+	PINCTRL_PIN(WMT_PIN_VDIN3, "vdin3"),
+	PINCTRL_PIN(WMT_PIN_VDIN4, "vdin4"),
+	PINCTRL_PIN(WMT_PIN_VDIN5, "vdin5"),
+	PINCTRL_PIN(WMT_PIN_VDIN6, "vdin6"),
+	PINCTRL_PIN(WMT_PIN_VDIN7, "vdin7"),
+	PINCTRL_PIN(WMT_PIN_I2C1SCL, "i2c1_scl"),
+	PINCTRL_PIN(WMT_PIN_I2C1SDA, "i2c1_sda"),
+	PINCTRL_PIN(WMT_PIN_SPI0MOSI, "spi0_mosi"),
+	PINCTRL_PIN(WMT_PIN_SPI0MISO, "spi0_miso"),
+	PINCTRL_PIN(WMT_PIN_SPI0SS0, "spi0_ss0"),
+	PINCTRL_PIN(WMT_PIN_SPI0CLK, "spi0_clk"),
+	PINCTRL_PIN(WMT_PIN_SD0DATA0, "sd0_data0"),
+	PINCTRL_PIN(WMT_PIN_SD0DATA1, "sd0_data1"),
+	PINCTRL_PIN(WMT_PIN_SD0DATA2, "sd0_data2"),
+	PINCTRL_PIN(WMT_PIN_SD0DATA3, "sd0_data3"),
+	PINCTRL_PIN(WMT_PIN_SD0CLK, "sd0_clk"),
+	PINCTRL_PIN(WMT_PIN_SD0WP, "sd0_wp"),
+	PINCTRL_PIN(WMT_PIN_SD0CMD, "sd0_cmd"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA0, "sd1_data0"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA1, "sd1_data1"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA2, "sd1_data2"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA3, "sd1_data3"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA4, "sd1_data4"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA5, "sd1_data5"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA6, "sd1_data6"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA7, "sd1_data7"),
+	PINCTRL_PIN(WMT_PIN_I2C0SCL, "i2c0_scl"),
+	PINCTRL_PIN(WMT_PIN_I2C0SDA, "i2c0_sda"),
+	PINCTRL_PIN(WMT_PIN_UART0RTS, "uart0_rts"),
+	PINCTRL_PIN(WMT_PIN_UART0TXD, "uart0_txd"),
+	PINCTRL_PIN(WMT_PIN_UART0CTS, "uart0_cts"),
+	PINCTRL_PIN(WMT_PIN_UART0RXD, "uart0_rxd"),
+	PINCTRL_PIN(WMT_PIN_UART1RTS, "uart1_rts"),
+	PINCTRL_PIN(WMT_PIN_UART1TXD, "uart1_txd"),
+	PINCTRL_PIN(WMT_PIN_UART1CTS, "uart1_cts"),
+	PINCTRL_PIN(WMT_PIN_UART1RXD, "uart1_rxd"),
+	PINCTRL_PIN(WMT_PIN_UART2RTS, "uart2_rts"),
+	PINCTRL_PIN(WMT_PIN_UART2TXD, "uart2_txd"),
+	PINCTRL_PIN(WMT_PIN_UART2CTS, "uart2_cts"),
+	PINCTRL_PIN(WMT_PIN_UART2RXD, "uart2_rxd"),
+	PINCTRL_PIN(WMT_PIN_UART3RTS, "uart3_rts"),
+	PINCTRL_PIN(WMT_PIN_UART3TXD, "uart3_txd"),
+	PINCTRL_PIN(WMT_PIN_UART3CTS, "uart3_cts"),
+	PINCTRL_PIN(WMT_PIN_UART3RXD, "uart3_rxd"),
+	PINCTRL_PIN(WMT_PIN_KPADROW0, "kpadrow0"),
+	PINCTRL_PIN(WMT_PIN_KPADROW1, "kpadrow1"),
+	PINCTRL_PIN(WMT_PIN_KPADCOL0, "kpadcol0"),
+	PINCTRL_PIN(WMT_PIN_KPADCOL1, "kpadcol1"),
+	PINCTRL_PIN(WMT_PIN_SD1CLK, "sd1_clk"),
+	PINCTRL_PIN(WMT_PIN_SD1CMD, "sd1_cmd"),
+	PINCTRL_PIN(WMT_PIN_SD1WP, "sd1_wp"),
+};
+
+/* Order of these names must match the above list */
+static const char * const wm8650_groups[] = {
+	"extgpio0",
+	"extgpio1",
+	"extgpio2",
+	"extgpio3",
+	"extgpio4",
+	"extgpio5",
+	"extgpio6",
+	"extgpio7",
+	"wakeup0",
+	"wakeup1",
+	"susgpio0",
+	"sd0_cd",
+	"sd1_cd",
+	"vdout0",
+	"vdout1",
+	"vdout2",
+	"vdout3",
+	"vdout4",
+	"vdout5",
+	"vdout6",
+	"vdout7",
+	"vdout8",
+	"vdout9",
+	"vdout10",
+	"vdout11",
+	"vdout12",
+	"vdout13",
+	"vdout14",
+	"vdout15",
+	"vdout16",
+	"vdout17",
+	"vdout18",
+	"vdout19",
+	"vdout20",
+	"vdout21",
+	"vdout22",
+	"vdout23",
+	"vdin0",
+	"vdin1",
+	"vdin2",
+	"vdin3",
+	"vdin4",
+	"vdin5",
+	"vdin6",
+	"vdin7",
+	"i2c1_scl",
+	"i2c1_sda",
+	"spi0_mosi",
+	"spi0_miso",
+	"spi0_ss0",
+	"spi0_clk",
+	"sd0_data0",
+	"sd0_data1",
+	"sd0_data2",
+	"sd0_data3",
+	"sd0_clk",
+	"sd0_wp",
+	"sd0_cmd",
+	"sd1_data0",
+	"sd1_data1",
+	"sd1_data2",
+	"sd1_data3",
+	"sd1_data4",
+	"sd1_data5",
+	"sd1_data6",
+	"sd1_data7",
+	"i2c0_scl",
+	"i2c0_sda",
+	"uart0_rts",
+	"uart0_txd",
+	"uart0_cts",
+	"uart0_rxd",
+	"uart1_rts",
+	"uart1_txd",
+	"uart1_cts",
+	"uart1_rxd",
+	"uart2_rts",
+	"uart2_txd",
+	"uart2_cts",
+	"uart2_rxd",
+	"uart3_rts",
+	"uart3_txd",
+	"uart3_cts",
+	"uart3_rxd",
+	"kpadrow0",
+	"kpadrow1",
+	"kpadcol0",
+	"kpadcol1",
+	"sd1_clk",
+	"sd1_cmd",
+	"sd1_wp",
+};
+
+static int wm8650_pinctrl_probe(struct platform_device *pdev)
+{
+	struct wmt_pinctrl_data *data;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		dev_err(&pdev->dev, "failed to allocate data\n");
+		return -ENOMEM;
+	}
+
+	data->banks = wm8650_banks;
+	data->nbanks = ARRAY_SIZE(wm8650_banks);
+	data->pins = wm8650_pins;
+	data->npins = ARRAY_SIZE(wm8650_pins);
+	data->groups = wm8650_groups;
+	data->ngroups = ARRAY_SIZE(wm8650_groups);
+
+	return wmt_pinctrl_probe(pdev, data);
+}
+
+static int wm8650_pinctrl_remove(struct platform_device *pdev)
+{
+	return wmt_pinctrl_remove(pdev);
+}
+
+static struct of_device_id wmt_pinctrl_of_match[] = {
+	{ .compatible = "wm,wm8650-pinctrl" },
+	{ /* sentinel */ },
+};
+
+static struct platform_driver wmt_pinctrl_driver = {
+	.probe	= wm8650_pinctrl_probe,
+	.remove	= wm8650_pinctrl_remove,
+	.driver = {
+		.name	= "pinctrl-wm8650",
+		.owner	= THIS_MODULE,
+		.of_match_table	= wmt_pinctrl_of_match,
+	},
+};
+
+module_platform_driver(wmt_pinctrl_driver);
+
+MODULE_AUTHOR("Tony Prisk <linux@prisktech.co.nz>");
+MODULE_DESCRIPTION("Wondermedia WM8650 Pincontrol driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, wmt_pinctrl_of_match);
diff --git a/drivers/pinctrl/vt8500/pinctrl-wm8750.c b/drivers/pinctrl/vt8500/pinctrl-wm8750.c
new file mode 100644
index 000000000000..b964cc550568
--- /dev/null
+++ b/drivers/pinctrl/vt8500/pinctrl-wm8750.c
@@ -0,0 +1,409 @@
+/*
+ * Pinctrl data for Wondermedia WM8750 SoC
+ *
+ * Copyright (c) 2013 Tony Prisk <linux@prisktech.co.nz>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include "pinctrl-wmt.h"
+
+/*
+ * Describe the register offsets within the GPIO memory space
+ * The dedicated external GPIO's should always be listed in bank 0
+ * so they are exported in the 0..31 range which is what users
+ * expect.
+ *
+ * Do not reorder these banks as it will change the pin numbering
+ */
+static const struct wmt_pinctrl_bank_registers wm8750_banks[] = {
+	WMT_PINCTRL_BANK(0x40, 0x80, 0xC0, 0x00, 0x480, 0x4C0),	/* 0 */
+	WMT_PINCTRL_BANK(0x44, 0x84, 0xC4, 0x04, 0x484, 0x4C4),	/* 1 */
+	WMT_PINCTRL_BANK(0x48, 0x88, 0xC8, 0x08, 0x488, 0x4C8),	/* 2 */
+	WMT_PINCTRL_BANK(0x4C, 0x8C, 0xCC, 0x0C, 0x48C, 0x4CC),	/* 3 */
+	WMT_PINCTRL_BANK(0x50, 0x90, 0xD0, 0x10, 0x490, 0x4D0),	/* 4 */
+	WMT_PINCTRL_BANK(0x54, 0x94, 0xD4, 0x14, 0x494, 0x4D4),	/* 5 */
+	WMT_PINCTRL_BANK(0x58, 0x98, 0xD8, 0x18, 0x498, 0x4D8),	/* 6 */
+	WMT_PINCTRL_BANK(0x5C, 0x9C, 0xDC, 0x1C, 0x49C, 0x4DC),	/* 7 */
+	WMT_PINCTRL_BANK(0x60, 0xA0, 0xE0, 0x20, 0x4A0, 0x4E0),	/* 8 */
+	WMT_PINCTRL_BANK(0x70, 0xB0, 0xF0, 0x30, 0x4B0, 0x4F0),	/* 9 */
+	WMT_PINCTRL_BANK(0x7C, 0xBC, 0xDC, 0x3C, 0x4BC, 0x4FC),	/* 10 */
+};
+
+/* Please keep sorted by bank/bit */
+#define WMT_PIN_EXTGPIO0	WMT_PIN(0, 0)
+#define WMT_PIN_EXTGPIO1	WMT_PIN(0, 1)
+#define WMT_PIN_EXTGPIO2	WMT_PIN(0, 2)
+#define WMT_PIN_EXTGPIO3	WMT_PIN(0, 3)
+#define WMT_PIN_EXTGPIO4	WMT_PIN(0, 4)
+#define WMT_PIN_EXTGPIO5	WMT_PIN(0, 5)
+#define WMT_PIN_EXTGPIO6	WMT_PIN(0, 6)
+#define WMT_PIN_EXTGPIO7	WMT_PIN(0, 7)
+#define WMT_PIN_WAKEUP0		WMT_PIN(0, 16)
+#define WMT_PIN_WAKEUP1		WMT_PIN(0, 16)
+#define WMT_PIN_SD0CD		WMT_PIN(0, 28)
+#define WMT_PIN_VDOUT0		WMT_PIN(1, 0)
+#define WMT_PIN_VDOUT1		WMT_PIN(1, 1)
+#define WMT_PIN_VDOUT2		WMT_PIN(1, 2)
+#define WMT_PIN_VDOUT3		WMT_PIN(1, 3)
+#define WMT_PIN_VDOUT4		WMT_PIN(1, 4)
+#define WMT_PIN_VDOUT5		WMT_PIN(1, 5)
+#define WMT_PIN_VDOUT6		WMT_PIN(1, 6)
+#define WMT_PIN_VDOUT7		WMT_PIN(1, 7)
+#define WMT_PIN_VDOUT8		WMT_PIN(1, 8)
+#define WMT_PIN_VDOUT9		WMT_PIN(1, 9)
+#define WMT_PIN_VDOUT10		WMT_PIN(1, 10)
+#define WMT_PIN_VDOUT11		WMT_PIN(1, 11)
+#define WMT_PIN_VDOUT12		WMT_PIN(1, 12)
+#define WMT_PIN_VDOUT13		WMT_PIN(1, 13)
+#define WMT_PIN_VDOUT14		WMT_PIN(1, 14)
+#define WMT_PIN_VDOUT15		WMT_PIN(1, 15)
+#define WMT_PIN_VDOUT16		WMT_PIN(1, 16)
+#define WMT_PIN_VDOUT17		WMT_PIN(1, 17)
+#define WMT_PIN_VDOUT18		WMT_PIN(1, 18)
+#define WMT_PIN_VDOUT19		WMT_PIN(1, 19)
+#define WMT_PIN_VDOUT20		WMT_PIN(1, 20)
+#define WMT_PIN_VDOUT21		WMT_PIN(1, 21)
+#define WMT_PIN_VDOUT22		WMT_PIN(1, 22)
+#define WMT_PIN_VDOUT23		WMT_PIN(1, 23)
+#define WMT_PIN_VDIN0		WMT_PIN(2, 0)
+#define WMT_PIN_VDIN1		WMT_PIN(2, 1)
+#define WMT_PIN_VDIN2		WMT_PIN(2, 2)
+#define WMT_PIN_VDIN3		WMT_PIN(2, 3)
+#define WMT_PIN_VDIN4		WMT_PIN(2, 4)
+#define WMT_PIN_VDIN5		WMT_PIN(2, 5)
+#define WMT_PIN_VDIN6		WMT_PIN(2, 6)
+#define WMT_PIN_VDIN7		WMT_PIN(2, 7)
+#define WMT_PIN_SPI0_MOSI	WMT_PIN(2, 24)
+#define WMT_PIN_SPI0_MISO	WMT_PIN(2, 25)
+#define WMT_PIN_SPI0_SS		WMT_PIN(2, 26)
+#define WMT_PIN_SPI0_CLK	WMT_PIN(2, 27)
+#define WMT_PIN_SPI0_SSB	WMT_PIN(2, 28)
+#define WMT_PIN_SD0CLK		WMT_PIN(3, 17)
+#define WMT_PIN_SD0CMD		WMT_PIN(3, 18)
+#define WMT_PIN_SD0WP		WMT_PIN(3, 19)
+#define WMT_PIN_SD0DATA0	WMT_PIN(3, 20)
+#define WMT_PIN_SD0DATA1	WMT_PIN(3, 21)
+#define WMT_PIN_SD0DATA2	WMT_PIN(3, 22)
+#define WMT_PIN_SD0DATA3	WMT_PIN(3, 23)
+#define WMT_PIN_SD1DATA0	WMT_PIN(3, 24)
+#define WMT_PIN_SD1DATA1	WMT_PIN(3, 25)
+#define WMT_PIN_SD1DATA2	WMT_PIN(3, 26)
+#define WMT_PIN_SD1DATA3	WMT_PIN(3, 27)
+#define WMT_PIN_SD1DATA4	WMT_PIN(3, 28)
+#define WMT_PIN_SD1DATA5	WMT_PIN(3, 29)
+#define WMT_PIN_SD1DATA6	WMT_PIN(3, 30)
+#define WMT_PIN_SD1DATA7	WMT_PIN(3, 31)
+#define WMT_PIN_I2C0_SCL	WMT_PIN(5, 8)
+#define WMT_PIN_I2C0_SDA	WMT_PIN(5, 9)
+#define WMT_PIN_I2C1_SCL	WMT_PIN(5, 10)
+#define WMT_PIN_I2C1_SDA	WMT_PIN(5, 11)
+#define WMT_PIN_I2C2_SCL	WMT_PIN(5, 12)
+#define WMT_PIN_I2C2_SDA	WMT_PIN(5, 13)
+#define WMT_PIN_UART0_RTS	WMT_PIN(5, 16)
+#define WMT_PIN_UART0_TXD	WMT_PIN(5, 17)
+#define WMT_PIN_UART0_CTS	WMT_PIN(5, 18)
+#define WMT_PIN_UART0_RXD	WMT_PIN(5, 19)
+#define WMT_PIN_UART1_RTS	WMT_PIN(5, 20)
+#define WMT_PIN_UART1_TXD	WMT_PIN(5, 21)
+#define WMT_PIN_UART1_CTS	WMT_PIN(5, 22)
+#define WMT_PIN_UART1_RXD	WMT_PIN(5, 23)
+#define WMT_PIN_UART2_RTS	WMT_PIN(5, 24)
+#define WMT_PIN_UART2_TXD	WMT_PIN(5, 25)
+#define WMT_PIN_UART2_CTS	WMT_PIN(5, 26)
+#define WMT_PIN_UART2_RXD	WMT_PIN(5, 27)
+#define WMT_PIN_UART3_RTS	WMT_PIN(5, 28)
+#define WMT_PIN_UART3_TXD	WMT_PIN(5, 29)
+#define WMT_PIN_UART3_CTS	WMT_PIN(5, 30)
+#define WMT_PIN_UART3_RXD	WMT_PIN(5, 31)
+#define WMT_PIN_SD2CD		WMT_PIN(6, 0)
+#define WMT_PIN_SD2DATA3	WMT_PIN(6, 1)
+#define WMT_PIN_SD2DATA0	WMT_PIN(6, 2)
+#define WMT_PIN_SD2WP		WMT_PIN(6, 3)
+#define WMT_PIN_SD2DATA1	WMT_PIN(6, 4)
+#define WMT_PIN_SD2DATA2	WMT_PIN(6, 5)
+#define WMT_PIN_SD2CMD		WMT_PIN(6, 6)
+#define WMT_PIN_SD2CLK		WMT_PIN(6, 7)
+#define WMT_PIN_SD2PWR		WMT_PIN(6, 9)
+#define WMT_PIN_SD1CLK		WMT_PIN(7, 0)
+#define WMT_PIN_SD1CMD		WMT_PIN(7, 1)
+#define WMT_PIN_SD1PWR		WMT_PIN(7, 10)
+#define WMT_PIN_SD1WP		WMT_PIN(7, 11)
+#define WMT_PIN_SD1CD		WMT_PIN(7, 12)
+#define WMT_PIN_SPI0SS3		WMT_PIN(7, 24)
+#define WMT_PIN_SPI0SS2		WMT_PIN(7, 25)
+#define WMT_PIN_PWMOUT1		WMT_PIN(7, 26)
+#define WMT_PIN_PWMOUT0		WMT_PIN(7, 27)
+
+static const struct pinctrl_pin_desc wm8750_pins[] = {
+	PINCTRL_PIN(WMT_PIN_EXTGPIO0, "extgpio0"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO1, "extgpio1"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO2, "extgpio2"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO3, "extgpio3"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO4, "extgpio4"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO5, "extgpio5"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO6, "extgpio6"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO7, "extgpio7"),
+	PINCTRL_PIN(WMT_PIN_WAKEUP0, "wakeup0"),
+	PINCTRL_PIN(WMT_PIN_WAKEUP1, "wakeup1"),
+	PINCTRL_PIN(WMT_PIN_SD0CD, "sd0_cd"),
+	PINCTRL_PIN(WMT_PIN_VDOUT0, "vdout0"),
+	PINCTRL_PIN(WMT_PIN_VDOUT1, "vdout1"),
+	PINCTRL_PIN(WMT_PIN_VDOUT2, "vdout2"),
+	PINCTRL_PIN(WMT_PIN_VDOUT3, "vdout3"),
+	PINCTRL_PIN(WMT_PIN_VDOUT4, "vdout4"),
+	PINCTRL_PIN(WMT_PIN_VDOUT5, "vdout5"),
+	PINCTRL_PIN(WMT_PIN_VDOUT6, "vdout6"),
+	PINCTRL_PIN(WMT_PIN_VDOUT7, "vdout7"),
+	PINCTRL_PIN(WMT_PIN_VDOUT8, "vdout8"),
+	PINCTRL_PIN(WMT_PIN_VDOUT9, "vdout9"),
+	PINCTRL_PIN(WMT_PIN_VDOUT10, "vdout10"),
+	PINCTRL_PIN(WMT_PIN_VDOUT11, "vdout11"),
+	PINCTRL_PIN(WMT_PIN_VDOUT12, "vdout12"),
+	PINCTRL_PIN(WMT_PIN_VDOUT13, "vdout13"),
+	PINCTRL_PIN(WMT_PIN_VDOUT14, "vdout14"),
+	PINCTRL_PIN(WMT_PIN_VDOUT15, "vdout15"),
+	PINCTRL_PIN(WMT_PIN_VDOUT16, "vdout16"),
+	PINCTRL_PIN(WMT_PIN_VDOUT17, "vdout17"),
+	PINCTRL_PIN(WMT_PIN_VDOUT18, "vdout18"),
+	PINCTRL_PIN(WMT_PIN_VDOUT19, "vdout19"),
+	PINCTRL_PIN(WMT_PIN_VDOUT20, "vdout20"),
+	PINCTRL_PIN(WMT_PIN_VDOUT21, "vdout21"),
+	PINCTRL_PIN(WMT_PIN_VDOUT22, "vdout22"),
+	PINCTRL_PIN(WMT_PIN_VDOUT23, "vdout23"),
+	PINCTRL_PIN(WMT_PIN_VDIN0, "vdin0"),
+	PINCTRL_PIN(WMT_PIN_VDIN1, "vdin1"),
+	PINCTRL_PIN(WMT_PIN_VDIN2, "vdin2"),
+	PINCTRL_PIN(WMT_PIN_VDIN3, "vdin3"),
+	PINCTRL_PIN(WMT_PIN_VDIN4, "vdin4"),
+	PINCTRL_PIN(WMT_PIN_VDIN5, "vdin5"),
+	PINCTRL_PIN(WMT_PIN_VDIN6, "vdin6"),
+	PINCTRL_PIN(WMT_PIN_VDIN7, "vdin7"),
+	PINCTRL_PIN(WMT_PIN_SPI0_MOSI, "spi0_mosi"),
+	PINCTRL_PIN(WMT_PIN_SPI0_MISO, "spi0_miso"),
+	PINCTRL_PIN(WMT_PIN_SPI0_SS, "spi0_ss"),
+	PINCTRL_PIN(WMT_PIN_SPI0_CLK, "spi0_clk"),
+	PINCTRL_PIN(WMT_PIN_SPI0_SSB, "spi0_ssb"),
+	PINCTRL_PIN(WMT_PIN_SD0CLK, "sd0_clk"),
+	PINCTRL_PIN(WMT_PIN_SD0CMD, "sd0_cmd"),
+	PINCTRL_PIN(WMT_PIN_SD0WP, "sd0_wp"),
+	PINCTRL_PIN(WMT_PIN_SD0DATA0, "sd0_data0"),
+	PINCTRL_PIN(WMT_PIN_SD0DATA1, "sd0_data1"),
+	PINCTRL_PIN(WMT_PIN_SD0DATA2, "sd0_data2"),
+	PINCTRL_PIN(WMT_PIN_SD0DATA3, "sd0_data3"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA0, "sd1_data0"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA1, "sd1_data1"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA2, "sd1_data2"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA3, "sd1_data3"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA4, "sd1_data4"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA5, "sd1_data5"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA6, "sd1_data6"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA7, "sd1_data7"),
+	PINCTRL_PIN(WMT_PIN_I2C0_SCL, "i2c0_scl"),
+	PINCTRL_PIN(WMT_PIN_I2C0_SDA, "i2c0_sda"),
+	PINCTRL_PIN(WMT_PIN_I2C1_SCL, "i2c1_scl"),
+	PINCTRL_PIN(WMT_PIN_I2C1_SDA, "i2c1_sda"),
+	PINCTRL_PIN(WMT_PIN_I2C2_SCL, "i2c2_scl"),
+	PINCTRL_PIN(WMT_PIN_I2C2_SDA, "i2c2_sda"),
+	PINCTRL_PIN(WMT_PIN_UART0_RTS, "uart0_rts"),
+	PINCTRL_PIN(WMT_PIN_UART0_TXD, "uart0_txd"),
+	PINCTRL_PIN(WMT_PIN_UART0_CTS, "uart0_cts"),
+	PINCTRL_PIN(WMT_PIN_UART0_RXD, "uart0_rxd"),
+	PINCTRL_PIN(WMT_PIN_UART1_RTS, "uart1_rts"),
+	PINCTRL_PIN(WMT_PIN_UART1_TXD, "uart1_txd"),
+	PINCTRL_PIN(WMT_PIN_UART1_CTS, "uart1_cts"),
+	PINCTRL_PIN(WMT_PIN_UART1_RXD, "uart1_rxd"),
+	PINCTRL_PIN(WMT_PIN_UART2_RTS, "uart2_rts"),
+	PINCTRL_PIN(WMT_PIN_UART2_TXD, "uart2_txd"),
+	PINCTRL_PIN(WMT_PIN_UART2_CTS, "uart2_cts"),
+	PINCTRL_PIN(WMT_PIN_UART2_RXD, "uart2_rxd"),
+	PINCTRL_PIN(WMT_PIN_UART3_RTS, "uart3_rts"),
+	PINCTRL_PIN(WMT_PIN_UART3_TXD, "uart3_txd"),
+	PINCTRL_PIN(WMT_PIN_UART3_CTS, "uart3_cts"),
+	PINCTRL_PIN(WMT_PIN_UART3_RXD, "uart3_rxd"),
+	PINCTRL_PIN(WMT_PIN_SD2CD, "sd2_cd"),
+	PINCTRL_PIN(WMT_PIN_SD2DATA3, "sd2_data3"),
+	PINCTRL_PIN(WMT_PIN_SD2DATA0, "sd2_data0"),
+	PINCTRL_PIN(WMT_PIN_SD2WP, "sd2_wp"),
+	PINCTRL_PIN(WMT_PIN_SD2DATA1, "sd2_data1"),
+	PINCTRL_PIN(WMT_PIN_SD2DATA2, "sd2_data2"),
+	PINCTRL_PIN(WMT_PIN_SD2CMD, "sd2_cmd"),
+	PINCTRL_PIN(WMT_PIN_SD2CLK, "sd2_clk"),
+	PINCTRL_PIN(WMT_PIN_SD2PWR, "sd2_pwr"),
+	PINCTRL_PIN(WMT_PIN_SD1CLK, "sd1_clk"),
+	PINCTRL_PIN(WMT_PIN_SD1CMD, "sd1_cmd"),
+	PINCTRL_PIN(WMT_PIN_SD1PWR, "sd1_pwr"),
+	PINCTRL_PIN(WMT_PIN_SD1WP, "sd1_wp"),
+	PINCTRL_PIN(WMT_PIN_SD1CD, "sd1_cd"),
+	PINCTRL_PIN(WMT_PIN_SPI0SS3, "spi0_ss3"),
+	PINCTRL_PIN(WMT_PIN_SPI0SS2, "spi0_ss2"),
+	PINCTRL_PIN(WMT_PIN_PWMOUT1, "pwmout1"),
+	PINCTRL_PIN(WMT_PIN_PWMOUT0, "pwmout0"),
+};
+
+/* Order of these names must match the above list */
+static const char * const wm8750_groups[] = {
+	"extgpio0",
+	"extgpio1",
+	"extgpio2",
+	"extgpio3",
+	"extgpio4",
+	"extgpio5",
+	"extgpio6",
+	"extgpio7",
+	"wakeup0",
+	"wakeup1",
+	"sd0_cd",
+	"vdout0",
+	"vdout1",
+	"vdout2",
+	"vdout3",
+	"vdout4",
+	"vdout5",
+	"vdout6",
+	"vdout7",
+	"vdout8",
+	"vdout9",
+	"vdout10",
+	"vdout11",
+	"vdout12",
+	"vdout13",
+	"vdout14",
+	"vdout15",
+	"vdout16",
+	"vdout17",
+	"vdout18",
+	"vdout19",
+	"vdout20",
+	"vdout21",
+	"vdout22",
+	"vdout23",
+	"vdin0",
+	"vdin1",
+	"vdin2",
+	"vdin3",
+	"vdin4",
+	"vdin5",
+	"vdin6",
+	"vdin7",
+	"spi0_mosi",
+	"spi0_miso",
+	"spi0_ss",
+	"spi0_clk",
+	"spi0_ssb",
+	"sd0_clk",
+	"sd0_cmd",
+	"sd0_wp",
+	"sd0_data0",
+	"sd0_data1",
+	"sd0_data2",
+	"sd0_data3",
+	"sd1_data0",
+	"sd1_data1",
+	"sd1_data2",
+	"sd1_data3",
+	"sd1_data4",
+	"sd1_data5",
+	"sd1_data6",
+	"sd1_data7",
+	"i2c0_scl",
+	"i2c0_sda",
+	"i2c1_scl",
+	"i2c1_sda",
+	"i2c2_scl",
+	"i2c2_sda",
+	"uart0_rts",
+	"uart0_txd",
+	"uart0_cts",
+	"uart0_rxd",
+	"uart1_rts",
+	"uart1_txd",
+	"uart1_cts",
+	"uart1_rxd",
+	"uart2_rts",
+	"uart2_txd",
+	"uart2_cts",
+	"uart2_rxd",
+	"uart3_rts",
+	"uart3_txd",
+	"uart3_cts",
+	"uart3_rxd",
+	"sd2_cd",
+	"sd2_data3",
+	"sd2_data0",
+	"sd2_wp",
+	"sd2_data1",
+	"sd2_data2",
+	"sd2_cmd",
+	"sd2_clk",
+	"sd2_pwr",
+	"sd1_clk",
+	"sd1_cmd",
+	"sd1_pwr",
+	"sd1_wp",
+	"sd1_cd",
+	"spi0_ss3",
+	"spi0_ss2",
+	"pwmout1",
+	"pwmout0",
+};
+
+static int wm8750_pinctrl_probe(struct platform_device *pdev)
+{
+	struct wmt_pinctrl_data *data;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		dev_err(&pdev->dev, "failed to allocate data\n");
+		return -ENOMEM;
+	}
+
+	data->banks = wm8750_banks;
+	data->nbanks = ARRAY_SIZE(wm8750_banks);
+	data->pins = wm8750_pins;
+	data->npins = ARRAY_SIZE(wm8750_pins);
+	data->groups = wm8750_groups;
+	data->ngroups = ARRAY_SIZE(wm8750_groups);
+
+	return wmt_pinctrl_probe(pdev, data);
+}
+
+static int wm8750_pinctrl_remove(struct platform_device *pdev)
+{
+	return wmt_pinctrl_remove(pdev);
+}
+
+static struct of_device_id wmt_pinctrl_of_match[] = {
+	{ .compatible = "wm,wm8750-pinctrl" },
+	{ /* sentinel */ },
+};
+
+static struct platform_driver wmt_pinctrl_driver = {
+	.probe	= wm8750_pinctrl_probe,
+	.remove	= wm8750_pinctrl_remove,
+	.driver = {
+		.name	= "pinctrl-wm8750",
+		.owner	= THIS_MODULE,
+		.of_match_table	= wmt_pinctrl_of_match,
+	},
+};
+
+module_platform_driver(wmt_pinctrl_driver);
+
+MODULE_AUTHOR("Tony Prisk <linux@prisktech.co.nz>");
+MODULE_DESCRIPTION("Wondermedia WM8750 Pincontrol driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, wmt_pinctrl_of_match);
diff --git a/drivers/pinctrl/vt8500/pinctrl-wm8850.c b/drivers/pinctrl/vt8500/pinctrl-wm8850.c
new file mode 100644
index 000000000000..ecadce9c91d5
--- /dev/null
+++ b/drivers/pinctrl/vt8500/pinctrl-wm8850.c
@@ -0,0 +1,388 @@
+/*
+ * Pinctrl data for Wondermedia WM8850 SoC
+ *
+ * Copyright (c) 2013 Tony Prisk <linux@prisktech.co.nz>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include "pinctrl-wmt.h"
+
+/*
+ * Describe the register offsets within the GPIO memory space
+ * The dedicated external GPIO's should always be listed in bank 0
+ * so they are exported in the 0..31 range which is what users
+ * expect.
+ *
+ * Do not reorder these banks as it will change the pin numbering
+ */
+static const struct wmt_pinctrl_bank_registers wm8850_banks[] = {
+	WMT_PINCTRL_BANK(0x40, 0x80, 0xC0, 0x00, 0x480, 0x4C0),		/* 0 */
+	WMT_PINCTRL_BANK(0x44, 0x84, 0xC4, 0x04, 0x484, 0x4C4),		/* 1 */
+	WMT_PINCTRL_BANK(0x48, 0x88, 0xC8, 0x08, 0x488, 0x4C8),		/* 2 */
+	WMT_PINCTRL_BANK(0x4C, 0x8C, 0xCC, 0x0C, 0x48C, 0x4CC),		/* 3 */
+	WMT_PINCTRL_BANK(0x50, 0x90, 0xD0, 0x10, 0x490, 0x4D0),		/* 4 */
+	WMT_PINCTRL_BANK(0x54, 0x94, 0xD4, 0x14, 0x494, 0x4D4),		/* 5 */
+	WMT_PINCTRL_BANK(0x58, 0x98, 0xD8, 0x18, 0x498, 0x4D8),		/* 6 */
+	WMT_PINCTRL_BANK(0x5C, 0x9C, 0xDC, 0x1C, 0x49C, 0x4DC),		/* 7 */
+	WMT_PINCTRL_BANK(0x60, 0xA0, 0xE0, 0x20, 0x4A0, 0x4E0),		/* 8 */
+	WMT_PINCTRL_BANK(0x70, 0xB0, 0xF0, 0x30, 0x4B0, 0x4F0),		/* 9 */
+	WMT_PINCTRL_BANK(0x7C, 0xBC, 0xDC, 0x3C, 0x4BC, 0x4FC),		/* 10 */
+};
+
+/* Please keep sorted by bank/bit */
+#define WMT_PIN_EXTGPIO0	WMT_PIN(0, 0)
+#define WMT_PIN_EXTGPIO1	WMT_PIN(0, 1)
+#define WMT_PIN_EXTGPIO2	WMT_PIN(0, 2)
+#define WMT_PIN_EXTGPIO3	WMT_PIN(0, 3)
+#define WMT_PIN_EXTGPIO4	WMT_PIN(0, 4)
+#define WMT_PIN_EXTGPIO5	WMT_PIN(0, 5)
+#define WMT_PIN_EXTGPIO6	WMT_PIN(0, 6)
+#define WMT_PIN_EXTGPIO7	WMT_PIN(0, 7)
+#define WMT_PIN_WAKEUP0		WMT_PIN(0, 16)
+#define WMT_PIN_WAKEUP1		WMT_PIN(0, 17)
+#define WMT_PIN_WAKEUP2		WMT_PIN(0, 18)
+#define WMT_PIN_WAKEUP3		WMT_PIN(0, 19)
+#define WMT_PIN_SUSGPIO0	WMT_PIN(0, 21)
+#define WMT_PIN_SUSGPIO1	WMT_PIN(0, 22)
+#define WMT_PIN_SD0CD		WMT_PIN(0, 28)
+#define WMT_PIN_VDOUT0		WMT_PIN(1, 0)
+#define WMT_PIN_VDOUT1		WMT_PIN(1, 1)
+#define WMT_PIN_VDOUT2		WMT_PIN(1, 2)
+#define WMT_PIN_VDOUT3		WMT_PIN(1, 3)
+#define WMT_PIN_VDOUT4		WMT_PIN(1, 4)
+#define WMT_PIN_VDOUT5		WMT_PIN(1, 5)
+#define WMT_PIN_VDOUT6		WMT_PIN(1, 6)
+#define WMT_PIN_VDOUT7		WMT_PIN(1, 7)
+#define WMT_PIN_VDOUT8		WMT_PIN(1, 8)
+#define WMT_PIN_VDOUT9		WMT_PIN(1, 9)
+#define WMT_PIN_VDOUT10		WMT_PIN(1, 10)
+#define WMT_PIN_VDOUT11		WMT_PIN(1, 11)
+#define WMT_PIN_VDOUT12		WMT_PIN(1, 12)
+#define WMT_PIN_VDOUT13		WMT_PIN(1, 13)
+#define WMT_PIN_VDOUT14		WMT_PIN(1, 14)
+#define WMT_PIN_VDOUT15		WMT_PIN(1, 15)
+#define WMT_PIN_VDOUT16		WMT_PIN(1, 16)
+#define WMT_PIN_VDOUT17		WMT_PIN(1, 17)
+#define WMT_PIN_VDOUT18		WMT_PIN(1, 18)
+#define WMT_PIN_VDOUT19		WMT_PIN(1, 19)
+#define WMT_PIN_VDOUT20		WMT_PIN(1, 20)
+#define WMT_PIN_VDOUT21		WMT_PIN(1, 21)
+#define WMT_PIN_VDOUT22		WMT_PIN(1, 22)
+#define WMT_PIN_VDOUT23		WMT_PIN(1, 23)
+#define WMT_PIN_VDIN0		WMT_PIN(2, 0)
+#define WMT_PIN_VDIN1		WMT_PIN(2, 1)
+#define WMT_PIN_VDIN2		WMT_PIN(2, 2)
+#define WMT_PIN_VDIN3		WMT_PIN(2, 3)
+#define WMT_PIN_VDIN4		WMT_PIN(2, 4)
+#define WMT_PIN_VDIN5		WMT_PIN(2, 5)
+#define WMT_PIN_VDIN6		WMT_PIN(2, 6)
+#define WMT_PIN_VDIN7		WMT_PIN(2, 7)
+#define WMT_PIN_SPI0_MOSI	WMT_PIN(2, 24)
+#define WMT_PIN_SPI0_MISO	WMT_PIN(2, 25)
+#define WMT_PIN_SPI0_SS		WMT_PIN(2, 26)
+#define WMT_PIN_SPI0_CLK	WMT_PIN(2, 27)
+#define WMT_PIN_SPI0_SSB	WMT_PIN(2, 28)
+#define WMT_PIN_SD0CLK		WMT_PIN(3, 17)
+#define WMT_PIN_SD0CMD		WMT_PIN(3, 18)
+#define WMT_PIN_SD0WP		WMT_PIN(3, 19)
+#define WMT_PIN_SD0DATA0	WMT_PIN(3, 20)
+#define WMT_PIN_SD0DATA1	WMT_PIN(3, 21)
+#define WMT_PIN_SD0DATA2	WMT_PIN(3, 22)
+#define WMT_PIN_SD0DATA3	WMT_PIN(3, 23)
+#define WMT_PIN_SD1DATA0	WMT_PIN(3, 24)
+#define WMT_PIN_SD1DATA1	WMT_PIN(3, 25)
+#define WMT_PIN_SD1DATA2	WMT_PIN(3, 26)
+#define WMT_PIN_SD1DATA3	WMT_PIN(3, 27)
+#define WMT_PIN_SD1DATA4	WMT_PIN(3, 28)
+#define WMT_PIN_SD1DATA5	WMT_PIN(3, 29)
+#define WMT_PIN_SD1DATA6	WMT_PIN(3, 30)
+#define WMT_PIN_SD1DATA7	WMT_PIN(3, 31)
+#define WMT_PIN_I2C0_SCL	WMT_PIN(5, 8)
+#define WMT_PIN_I2C0_SDA	WMT_PIN(5, 9)
+#define WMT_PIN_I2C1_SCL	WMT_PIN(5, 10)
+#define WMT_PIN_I2C1_SDA	WMT_PIN(5, 11)
+#define WMT_PIN_I2C2_SCL	WMT_PIN(5, 12)
+#define WMT_PIN_I2C2_SDA	WMT_PIN(5, 13)
+#define WMT_PIN_UART0_RTS	WMT_PIN(5, 16)
+#define WMT_PIN_UART0_TXD	WMT_PIN(5, 17)
+#define WMT_PIN_UART0_CTS	WMT_PIN(5, 18)
+#define WMT_PIN_UART0_RXD	WMT_PIN(5, 19)
+#define WMT_PIN_UART1_RTS	WMT_PIN(5, 20)
+#define WMT_PIN_UART1_TXD	WMT_PIN(5, 21)
+#define WMT_PIN_UART1_CTS	WMT_PIN(5, 22)
+#define WMT_PIN_UART1_RXD	WMT_PIN(5, 23)
+#define WMT_PIN_UART2_RTS	WMT_PIN(5, 24)
+#define WMT_PIN_UART2_TXD	WMT_PIN(5, 25)
+#define WMT_PIN_UART2_CTS	WMT_PIN(5, 26)
+#define WMT_PIN_UART2_RXD	WMT_PIN(5, 27)
+#define WMT_PIN_SD2WP		WMT_PIN(6, 3)
+#define WMT_PIN_SD2CMD		WMT_PIN(6, 6)
+#define WMT_PIN_SD2CLK		WMT_PIN(6, 7)
+#define WMT_PIN_SD2PWR		WMT_PIN(6, 9)
+#define WMT_PIN_SD1CLK		WMT_PIN(7, 0)
+#define WMT_PIN_SD1CMD		WMT_PIN(7, 1)
+#define WMT_PIN_SD1PWR		WMT_PIN(7, 10)
+#define WMT_PIN_SD1WP		WMT_PIN(7, 11)
+#define WMT_PIN_SD1CD		WMT_PIN(7, 12)
+#define WMT_PIN_PWMOUT1		WMT_PIN(7, 26)
+#define WMT_PIN_PWMOUT0		WMT_PIN(7, 27)
+
+static const struct pinctrl_pin_desc wm8850_pins[] = {
+	PINCTRL_PIN(WMT_PIN_EXTGPIO0, "extgpio0"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO1, "extgpio1"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO2, "extgpio2"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO3, "extgpio3"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO4, "extgpio4"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO5, "extgpio5"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO6, "extgpio6"),
+	PINCTRL_PIN(WMT_PIN_EXTGPIO7, "extgpio7"),
+	PINCTRL_PIN(WMT_PIN_WAKEUP0, "wakeup0"),
+	PINCTRL_PIN(WMT_PIN_WAKEUP1, "wakeup1"),
+	PINCTRL_PIN(WMT_PIN_WAKEUP2, "wakeup2"),
+	PINCTRL_PIN(WMT_PIN_WAKEUP3, "wakeup3"),
+	PINCTRL_PIN(WMT_PIN_SUSGPIO0, "susgpio0"),
+	PINCTRL_PIN(WMT_PIN_SUSGPIO1, "susgpio1"),
+	PINCTRL_PIN(WMT_PIN_SD0CD, "sd0_cd"),
+	PINCTRL_PIN(WMT_PIN_VDOUT0, "vdout0"),
+	PINCTRL_PIN(WMT_PIN_VDOUT1, "vdout1"),
+	PINCTRL_PIN(WMT_PIN_VDOUT2, "vdout2"),
+	PINCTRL_PIN(WMT_PIN_VDOUT3, "vdout3"),
+	PINCTRL_PIN(WMT_PIN_VDOUT4, "vdout4"),
+	PINCTRL_PIN(WMT_PIN_VDOUT5, "vdout5"),
+	PINCTRL_PIN(WMT_PIN_VDOUT6, "vdout6"),
+	PINCTRL_PIN(WMT_PIN_VDOUT7, "vdout7"),
+	PINCTRL_PIN(WMT_PIN_VDOUT8, "vdout8"),
+	PINCTRL_PIN(WMT_PIN_VDOUT9, "vdout9"),
+	PINCTRL_PIN(WMT_PIN_VDOUT10, "vdout10"),
+	PINCTRL_PIN(WMT_PIN_VDOUT11, "vdout11"),
+	PINCTRL_PIN(WMT_PIN_VDOUT12, "vdout12"),
+	PINCTRL_PIN(WMT_PIN_VDOUT13, "vdout13"),
+	PINCTRL_PIN(WMT_PIN_VDOUT14, "vdout14"),
+	PINCTRL_PIN(WMT_PIN_VDOUT15, "vdout15"),
+	PINCTRL_PIN(WMT_PIN_VDOUT16, "vdout16"),
+	PINCTRL_PIN(WMT_PIN_VDOUT17, "vdout17"),
+	PINCTRL_PIN(WMT_PIN_VDOUT18, "vdout18"),
+	PINCTRL_PIN(WMT_PIN_VDOUT19, "vdout19"),
+	PINCTRL_PIN(WMT_PIN_VDOUT20, "vdout20"),
+	PINCTRL_PIN(WMT_PIN_VDOUT21, "vdout21"),
+	PINCTRL_PIN(WMT_PIN_VDOUT22, "vdout22"),
+	PINCTRL_PIN(WMT_PIN_VDOUT23, "vdout23"),
+	PINCTRL_PIN(WMT_PIN_VDIN0, "vdin0"),
+	PINCTRL_PIN(WMT_PIN_VDIN1, "vdin1"),
+	PINCTRL_PIN(WMT_PIN_VDIN2, "vdin2"),
+	PINCTRL_PIN(WMT_PIN_VDIN3, "vdin3"),
+	PINCTRL_PIN(WMT_PIN_VDIN4, "vdin4"),
+	PINCTRL_PIN(WMT_PIN_VDIN5, "vdin5"),
+	PINCTRL_PIN(WMT_PIN_VDIN6, "vdin6"),
+	PINCTRL_PIN(WMT_PIN_VDIN7, "vdin7"),
+	PINCTRL_PIN(WMT_PIN_SPI0_MOSI, "spi0_mosi"),
+	PINCTRL_PIN(WMT_PIN_SPI0_MISO, "spi0_miso"),
+	PINCTRL_PIN(WMT_PIN_SPI0_SS, "spi0_ss"),
+	PINCTRL_PIN(WMT_PIN_SPI0_CLK, "spi0_clk"),
+	PINCTRL_PIN(WMT_PIN_SPI0_SSB, "spi0_ssb"),
+	PINCTRL_PIN(WMT_PIN_SD0CLK, "sd0_clk"),
+	PINCTRL_PIN(WMT_PIN_SD0CMD, "sd0_cmd"),
+	PINCTRL_PIN(WMT_PIN_SD0WP, "sd0_wp"),
+	PINCTRL_PIN(WMT_PIN_SD0DATA0, "sd0_data0"),
+	PINCTRL_PIN(WMT_PIN_SD0DATA1, "sd0_data1"),
+	PINCTRL_PIN(WMT_PIN_SD0DATA2, "sd0_data2"),
+	PINCTRL_PIN(WMT_PIN_SD0DATA3, "sd0_data3"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA0, "sd1_data0"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA1, "sd1_data1"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA2, "sd1_data2"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA3, "sd1_data3"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA4, "sd1_data4"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA5, "sd1_data5"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA6, "sd1_data6"),
+	PINCTRL_PIN(WMT_PIN_SD1DATA7, "sd1_data7"),
+	PINCTRL_PIN(WMT_PIN_I2C0_SCL, "i2c0_scl"),
+	PINCTRL_PIN(WMT_PIN_I2C0_SDA, "i2c0_sda"),
+	PINCTRL_PIN(WMT_PIN_I2C1_SCL, "i2c1_scl"),
+	PINCTRL_PIN(WMT_PIN_I2C1_SDA, "i2c1_sda"),
+	PINCTRL_PIN(WMT_PIN_I2C2_SCL, "i2c2_scl"),
+	PINCTRL_PIN(WMT_PIN_I2C2_SDA, "i2c2_sda"),
+	PINCTRL_PIN(WMT_PIN_UART0_RTS, "uart0_rts"),
+	PINCTRL_PIN(WMT_PIN_UART0_TXD, "uart0_txd"),
+	PINCTRL_PIN(WMT_PIN_UART0_CTS, "uart0_cts"),
+	PINCTRL_PIN(WMT_PIN_UART0_RXD, "uart0_rxd"),
+	PINCTRL_PIN(WMT_PIN_UART1_RTS, "uart1_rts"),
+	PINCTRL_PIN(WMT_PIN_UART1_TXD, "uart1_txd"),
+	PINCTRL_PIN(WMT_PIN_UART1_CTS, "uart1_cts"),
+	PINCTRL_PIN(WMT_PIN_UART1_RXD, "uart1_rxd"),
+	PINCTRL_PIN(WMT_PIN_UART2_RTS, "uart2_rts"),
+	PINCTRL_PIN(WMT_PIN_UART2_TXD, "uart2_txd"),
+	PINCTRL_PIN(WMT_PIN_UART2_CTS, "uart2_cts"),
+	PINCTRL_PIN(WMT_PIN_UART2_RXD, "uart2_rxd"),
+	PINCTRL_PIN(WMT_PIN_SD2WP, "sd2_wp"),
+	PINCTRL_PIN(WMT_PIN_SD2CMD, "sd2_cmd"),
+	PINCTRL_PIN(WMT_PIN_SD2CLK, "sd2_clk"),
+	PINCTRL_PIN(WMT_PIN_SD2PWR, "sd2_pwr"),
+	PINCTRL_PIN(WMT_PIN_SD1CLK, "sd1_clk"),
+	PINCTRL_PIN(WMT_PIN_SD1CMD, "sd1_cmd"),
+	PINCTRL_PIN(WMT_PIN_SD1PWR, "sd1_pwr"),
+	PINCTRL_PIN(WMT_PIN_SD1WP, "sd1_wp"),
+	PINCTRL_PIN(WMT_PIN_SD1CD, "sd1_cd"),
+	PINCTRL_PIN(WMT_PIN_PWMOUT1, "pwmout1"),
+	PINCTRL_PIN(WMT_PIN_PWMOUT0, "pwmout0"),
+};
+
+/* Order of these names must match the above list */
+static const char * const wm8850_groups[] = {
+	"extgpio0",
+	"extgpio1",
+	"extgpio2",
+	"extgpio3",
+	"extgpio4",
+	"extgpio5",
+	"extgpio6",
+	"extgpio7",
+	"wakeup0",
+	"wakeup1",
+	"wakeup2",
+	"wakeup3",
+	"susgpio0",
+	"susgpio1",
+	"sd0_cd",
+	"vdout0",
+	"vdout1",
+	"vdout2",
+	"vdout3",
+	"vdout4",
+	"vdout5",
+	"vdout6",
+	"vdout7",
+	"vdout8",
+	"vdout9",
+	"vdout10",
+	"vdout11",
+	"vdout12",
+	"vdout13",
+	"vdout14",
+	"vdout15",
+	"vdout16",
+	"vdout17",
+	"vdout18",
+	"vdout19",
+	"vdout20",
+	"vdout21",
+	"vdout22",
+	"vdout23",
+	"vdin0",
+	"vdin1",
+	"vdin2",
+	"vdin3",
+	"vdin4",
+	"vdin5",
+	"vdin6",
+	"vdin7",
+	"spi0_mosi",
+	"spi0_miso",
+	"spi0_ss",
+	"spi0_clk",
+	"spi0_ssb",
+	"sd0_clk",
+	"sd0_cmd",
+	"sd0_wp",
+	"sd0_data0",
+	"sd0_data1",
+	"sd0_data2",
+	"sd0_data3",
+	"sd1_data0",
+	"sd1_data1",
+	"sd1_data2",
+	"sd1_data3",
+	"sd1_data4",
+	"sd1_data5",
+	"sd1_data6",
+	"sd1_data7",
+	"i2c0_scl",
+	"i2c0_sda",
+	"i2c1_scl",
+	"i2c1_sda",
+	"i2c2_scl",
+	"i2c2_sda",
+	"uart0_rts",
+	"uart0_txd",
+	"uart0_cts",
+	"uart0_rxd",
+	"uart1_rts",
+	"uart1_txd",
+	"uart1_cts",
+	"uart1_rxd",
+	"uart2_rts",
+	"uart2_txd",
+	"uart2_cts",
+	"uart2_rxd",
+	"sd2_wp",
+	"sd2_cmd",
+	"sd2_clk",
+	"sd2_pwr",
+	"sd1_clk",
+	"sd1_cmd",
+	"sd1_pwr",
+	"sd1_wp",
+	"sd1_cd",
+	"pwmout1",
+	"pwmout0",
+};
+
+static int wm8850_pinctrl_probe(struct platform_device *pdev)
+{
+	struct wmt_pinctrl_data *data;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		dev_err(&pdev->dev, "failed to allocate data\n");
+		return -ENOMEM;
+	}
+
+	data->banks = wm8850_banks;
+	data->nbanks = ARRAY_SIZE(wm8850_banks);
+	data->pins = wm8850_pins;
+	data->npins = ARRAY_SIZE(wm8850_pins);
+	data->groups = wm8850_groups;
+	data->ngroups = ARRAY_SIZE(wm8850_groups);
+
+	return wmt_pinctrl_probe(pdev, data);
+}
+
+static int wm8850_pinctrl_remove(struct platform_device *pdev)
+{
+	return wmt_pinctrl_remove(pdev);
+}
+
+static struct of_device_id wmt_pinctrl_of_match[] = {
+	{ .compatible = "wm,wm8850-pinctrl" },
+	{ /* sentinel */ },
+};
+
+static struct platform_driver wmt_pinctrl_driver = {
+	.probe	= wm8850_pinctrl_probe,
+	.remove	= wm8850_pinctrl_remove,
+	.driver = {
+		.name	= "pinctrl-wm8850",
+		.owner	= THIS_MODULE,
+		.of_match_table	= wmt_pinctrl_of_match,
+	},
+};
+
+module_platform_driver(wmt_pinctrl_driver);
+
+MODULE_AUTHOR("Tony Prisk <linux@prisktech.co.nz>");
+MODULE_DESCRIPTION("Wondermedia WM8850 Pincontrol driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, wmt_pinctrl_of_match);
diff --git a/drivers/pinctrl/vt8500/pinctrl-wmt.c b/drivers/pinctrl/vt8500/pinctrl-wmt.c
new file mode 100644
index 000000000000..ab63104e8dc9
--- /dev/null
+++ b/drivers/pinctrl/vt8500/pinctrl-wmt.c
@@ -0,0 +1,632 @@
+/*
+ * Pinctrl driver for the Wondermedia SoC's
+ *
+ * Copyright (c) 2013 Tony Prisk <linux@prisktech.co.nz>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/pinctrl/machine.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinconf-generic.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include "pinctrl-wmt.h"
+
+static inline void wmt_setbits(struct wmt_pinctrl_data *data, u32 reg,
+				 u32 mask)
+{
+	u32 val;
+
+	val = readl_relaxed(data->base + reg);
+	val |= mask;
+	writel_relaxed(val, data->base + reg);
+}
+
+static inline void wmt_clearbits(struct wmt_pinctrl_data *data, u32 reg,
+				   u32 mask)
+{
+	u32 val;
+
+	val = readl_relaxed(data->base + reg);
+	val &= ~mask;
+	writel_relaxed(val, data->base + reg);
+}
+
+enum wmt_func_sel {
+	WMT_FSEL_GPIO_IN = 0,
+	WMT_FSEL_GPIO_OUT = 1,
+	WMT_FSEL_ALT = 2,
+	WMT_FSEL_COUNT = 3,
+};
+
+static const char * const wmt_functions[WMT_FSEL_COUNT] = {
+	[WMT_FSEL_GPIO_IN] = "gpio_in",
+	[WMT_FSEL_GPIO_OUT] = "gpio_out",
+	[WMT_FSEL_ALT] = "alt",
+};
+
+static int wmt_pmx_get_functions_count(struct pinctrl_dev *pctldev)
+{
+	return WMT_FSEL_COUNT;
+}
+
+static const char *wmt_pmx_get_function_name(struct pinctrl_dev *pctldev,
+					     unsigned selector)
+{
+	return wmt_functions[selector];
+}
+
+static int wmt_pmx_get_function_groups(struct pinctrl_dev *pctldev,
+				       unsigned selector,
+				       const char * const **groups,
+				       unsigned * const num_groups)
+{
+	struct wmt_pinctrl_data *data = pinctrl_dev_get_drvdata(pctldev);
+
+	/* every pin does every function */
+	*groups = data->groups;
+	*num_groups = data->ngroups;
+
+	return 0;
+}
+
+static int wmt_set_pinmux(struct wmt_pinctrl_data *data, unsigned func,
+			  unsigned pin)
+{
+	u32 bank = WMT_BANK_FROM_PIN(pin);
+	u32 bit = WMT_BIT_FROM_PIN(pin);
+	u32 reg_en = data->banks[bank].reg_en;
+	u32 reg_dir = data->banks[bank].reg_dir;
+
+	if (reg_dir == NO_REG) {
+		dev_err(data->dev, "pin:%d no direction register defined\n",
+			pin);
+		return -EINVAL;
+	}
+
+	/*
+	 * If reg_en == NO_REG, we assume it is a dedicated GPIO and cannot be
+	 * disabled (as on VT8500) and that no alternate function is available.
+	 */
+	switch (func) {
+	case WMT_FSEL_GPIO_IN:
+		if (reg_en != NO_REG)
+			wmt_setbits(data, reg_en, BIT(bit));
+		wmt_clearbits(data, reg_dir, BIT(bit));
+		break;
+	case WMT_FSEL_GPIO_OUT:
+		if (reg_en != NO_REG)
+			wmt_setbits(data, reg_en, BIT(bit));
+		wmt_setbits(data, reg_dir, BIT(bit));
+		break;
+	case WMT_FSEL_ALT:
+		if (reg_en == NO_REG) {
+			dev_err(data->dev, "pin:%d no alt function available\n",
+				pin);
+			return -EINVAL;
+		}
+		wmt_clearbits(data, reg_en, BIT(bit));
+	}
+
+	return 0;
+}
+
+static int wmt_pmx_enable(struct pinctrl_dev *pctldev,
+			  unsigned func_selector,
+			  unsigned group_selector)
+{
+	struct wmt_pinctrl_data *data = pinctrl_dev_get_drvdata(pctldev);
+	u32 pinnum = data->pins[group_selector].number;
+
+	return wmt_set_pinmux(data, func_selector, pinnum);
+}
+
+static void wmt_pmx_disable(struct pinctrl_dev *pctldev,
+			    unsigned func_selector,
+			    unsigned group_selector)
+{
+	struct wmt_pinctrl_data *data = pinctrl_dev_get_drvdata(pctldev);
+	u32 pinnum = data->pins[group_selector].number;
+
+	/* disable by setting GPIO_IN */
+	wmt_set_pinmux(data, WMT_FSEL_GPIO_IN, pinnum);
+}
+
+static void wmt_pmx_gpio_disable_free(struct pinctrl_dev *pctldev,
+				      struct pinctrl_gpio_range *range,
+				      unsigned offset)
+{
+	struct wmt_pinctrl_data *data = pinctrl_dev_get_drvdata(pctldev);
+
+	/* disable by setting GPIO_IN */
+	wmt_set_pinmux(data, WMT_FSEL_GPIO_IN, offset);
+}
+
+static int wmt_pmx_gpio_set_direction(struct pinctrl_dev *pctldev,
+				      struct pinctrl_gpio_range *range,
+				      unsigned offset,
+				      bool input)
+{
+	struct wmt_pinctrl_data *data = pinctrl_dev_get_drvdata(pctldev);
+
+	wmt_set_pinmux(data, (input ? WMT_FSEL_GPIO_IN : WMT_FSEL_GPIO_OUT),
+		       offset);
+
+	return 0;
+}
+
+static struct pinmux_ops wmt_pinmux_ops = {
+	.get_functions_count = wmt_pmx_get_functions_count,
+	.get_function_name = wmt_pmx_get_function_name,
+	.get_function_groups = wmt_pmx_get_function_groups,
+	.enable = wmt_pmx_enable,
+	.disable = wmt_pmx_disable,
+	.gpio_disable_free = wmt_pmx_gpio_disable_free,
+	.gpio_set_direction = wmt_pmx_gpio_set_direction,
+};
+
+static int wmt_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	struct wmt_pinctrl_data *data = pinctrl_dev_get_drvdata(pctldev);
+
+	return data->ngroups;
+}
+
+static const char *wmt_get_group_name(struct pinctrl_dev *pctldev,
+				      unsigned selector)
+{
+	struct wmt_pinctrl_data *data = pinctrl_dev_get_drvdata(pctldev);
+
+	return data->groups[selector];
+}
+
+static int wmt_get_group_pins(struct pinctrl_dev *pctldev,
+			      unsigned selector,
+			      const unsigned **pins,
+			      unsigned *num_pins)
+{
+	struct wmt_pinctrl_data *data = pinctrl_dev_get_drvdata(pctldev);
+
+	*pins = &data->pins[selector].number;
+	*num_pins = 1;
+
+	return 0;
+}
+
+static int wmt_pctl_find_group_by_pin(struct wmt_pinctrl_data *data, u32 pin)
+{
+	int i;
+
+	for (i = 0; i < data->npins; i++) {
+		if (data->pins[i].number == pin)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static int wmt_pctl_dt_node_to_map_func(struct wmt_pinctrl_data *data,
+					struct device_node *np,
+					u32 pin, u32 fnum,
+					struct pinctrl_map **maps)
+{
+	int group;
+	struct pinctrl_map *map = *maps;
+
+	if (fnum >= ARRAY_SIZE(wmt_functions)) {
+		dev_err(data->dev, "invalid wm,function %d\n", fnum);
+		return -EINVAL;
+	}
+
+	group = wmt_pctl_find_group_by_pin(data, pin);
+	if (group < 0) {
+		dev_err(data->dev, "unable to match pin %d to group\n", pin);
+		return group;
+	}
+
+	map->type = PIN_MAP_TYPE_MUX_GROUP;
+	map->data.mux.group = data->groups[group];
+	map->data.mux.function = wmt_functions[fnum];
+	(*maps)++;
+
+	return 0;
+}
+
+static int wmt_pctl_dt_node_to_map_pull(struct wmt_pinctrl_data *data,
+					struct device_node *np,
+					u32 pin, u32 pull,
+					struct pinctrl_map **maps)
+{
+	int group;
+	unsigned long *configs;
+	struct pinctrl_map *map = *maps;
+
+	if (pull > 2) {
+		dev_err(data->dev, "invalid wm,pull %d\n", pull);
+		return -EINVAL;
+	}
+
+	group = wmt_pctl_find_group_by_pin(data, pin);
+	if (group < 0) {
+		dev_err(data->dev, "unable to match pin %d to group\n", pin);
+		return group;
+	}
+
+	configs = kzalloc(sizeof(*configs), GFP_KERNEL);
+	if (!configs)
+		return -ENOMEM;
+
+	configs[0] = pull;
+
+	map->type = PIN_MAP_TYPE_CONFIGS_PIN;
+	map->data.configs.group_or_pin = data->groups[group];
+	map->data.configs.configs = configs;
+	map->data.configs.num_configs = 1;
+	(*maps)++;
+
+	return 0;
+}
+
+static void wmt_pctl_dt_free_map(struct pinctrl_dev *pctldev,
+				 struct pinctrl_map *maps,
+				 unsigned num_maps)
+{
+	int i;
+
+	for (i = 0; i < num_maps; i++)
+		if (maps[i].type == PIN_MAP_TYPE_CONFIGS_PIN)
+			kfree(maps[i].data.configs.configs);
+
+	kfree(maps);
+}
+
+static int wmt_pctl_dt_node_to_map(struct pinctrl_dev *pctldev,
+				   struct device_node *np,
+				   struct pinctrl_map **map,
+				   unsigned *num_maps)
+{
+	struct pinctrl_map *maps, *cur_map;
+	struct property *pins, *funcs, *pulls;
+	u32 pin, func, pull;
+	int num_pins, num_funcs, num_pulls, maps_per_pin;
+	int i, err;
+	struct wmt_pinctrl_data *data = pinctrl_dev_get_drvdata(pctldev);
+
+	pins = of_find_property(np, "wm,pins", NULL);
+	if (!pins) {
+		dev_err(data->dev, "missing wmt,pins property\n");
+		return -EINVAL;
+	}
+
+	funcs = of_find_property(np, "wm,function", NULL);
+	pulls = of_find_property(np, "wm,pull", NULL);
+
+	if (!funcs && !pulls) {
+		dev_err(data->dev, "neither wm,function nor wm,pull specified\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * The following lines calculate how many values are defined for each
+	 * of the properties.
+	 */
+	num_pins = pins->length / sizeof(u32);
+	num_funcs = funcs ? (funcs->length / sizeof(u32)) : 0;
+	num_pulls = pulls ? (pulls->length / sizeof(u32)) : 0;
+
+	if (num_funcs > 1 && num_funcs != num_pins) {
+		dev_err(data->dev, "wm,function must have 1 or %d entries\n",
+			num_pins);
+		return -EINVAL;
+	}
+
+	if (num_pulls > 1 && num_pulls != num_pins) {
+		dev_err(data->dev, "wm,pull must have 1 or %d entries\n",
+			num_pins);
+		return -EINVAL;
+	}
+
+	maps_per_pin = 0;
+	if (num_funcs)
+		maps_per_pin++;
+	if (num_pulls)
+		maps_per_pin++;
+
+	cur_map = maps = kzalloc(num_pins * maps_per_pin * sizeof(*maps),
+				 GFP_KERNEL);
+	if (!maps)
+		return -ENOMEM;
+
+	for (i = 0; i < num_pins; i++) {
+		err = of_property_read_u32_index(np, "wm,pins", i, &pin);
+		if (err)
+			goto fail;
+
+		if (pin >= (data->nbanks * 32)) {
+			dev_err(data->dev, "invalid wm,pins value\n");
+			err = -EINVAL;
+			goto fail;
+		}
+
+		if (num_funcs) {
+			err = of_property_read_u32_index(np, "wm,function",
+						(num_funcs > 1 ? i : 0), &func);
+			if (err)
+				goto fail;
+
+			err = wmt_pctl_dt_node_to_map_func(data, np, pin, func,
+							   &cur_map);
+			if (err)
+				goto fail;
+		}
+
+		if (num_pulls) {
+			err = of_property_read_u32_index(np, "wm,pull",
+						(num_pulls > 1 ? i : 0), &pull);
+			if (err)
+				goto fail;
+
+			err = wmt_pctl_dt_node_to_map_pull(data, np, pin, pull,
+							   &cur_map);
+			if (err)
+				goto fail;
+		}
+	}
+	*map = maps;
+	*num_maps = num_pins * maps_per_pin;
+	return 0;
+
+/*
+ * The fail path removes any maps that have been allocated. The fail path is
+ * only called from code after maps has been kzalloc'd. It is also safe to
+ * pass 'num_pins * maps_per_pin' as the map count even though we probably
+ * failed before all the mappings were read as all maps are allocated at once,
+ * and configs are only allocated for .type = PIN_MAP_TYPE_CONFIGS_PIN - there
+ * is no failpath where a config can be allocated without .type being set.
+ */
+fail:
+	wmt_pctl_dt_free_map(pctldev, maps, num_pins * maps_per_pin);
+	return err;
+}
+
+static struct pinctrl_ops wmt_pctl_ops = {
+	.get_groups_count = wmt_get_groups_count,
+	.get_group_name	= wmt_get_group_name,
+	.get_group_pins	= wmt_get_group_pins,
+	.dt_node_to_map = wmt_pctl_dt_node_to_map,
+	.dt_free_map = wmt_pctl_dt_free_map,
+};
+
+static int wmt_pinconf_get(struct pinctrl_dev *pctldev, unsigned pin,
+			   unsigned long *config)
+{
+	return -ENOTSUPP;
+}
+
+static int wmt_pinconf_set(struct pinctrl_dev *pctldev, unsigned pin,
+			   unsigned long config)
+{
+	struct wmt_pinctrl_data *data = pinctrl_dev_get_drvdata(pctldev);
+	enum pin_config_param param = pinconf_to_config_param(config);
+	u16 arg = pinconf_to_config_argument(config);
+	u32 bank = WMT_BANK_FROM_PIN(pin);
+	u32 bit = WMT_BIT_FROM_PIN(pin);
+	u32 reg_pull_en = data->banks[bank].reg_pull_en;
+	u32 reg_pull_cfg = data->banks[bank].reg_pull_cfg;
+
+	if ((reg_pull_en == NO_REG) || (reg_pull_cfg == NO_REG)) {
+		dev_err(data->dev, "bias functions not supported on pin %d\n",
+			pin);
+		return -EINVAL;
+	}
+
+	if ((param == PIN_CONFIG_BIAS_PULL_DOWN) ||
+	    (param == PIN_CONFIG_BIAS_PULL_UP)) {
+		if (arg == 0)
+			param = PIN_CONFIG_BIAS_DISABLE;
+	}
+
+	switch (param) {
+	case PIN_CONFIG_BIAS_DISABLE:
+		wmt_clearbits(data, reg_pull_en, BIT(bit));
+		break;
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		wmt_clearbits(data, reg_pull_cfg, BIT(bit));
+		wmt_setbits(data, reg_pull_en, BIT(bit));
+		break;
+	case PIN_CONFIG_BIAS_PULL_UP:
+		wmt_setbits(data, reg_pull_cfg, BIT(bit));
+		wmt_setbits(data, reg_pull_en, BIT(bit));
+		break;
+	default:
+		dev_err(data->dev, "unknown pinconf param\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static struct pinconf_ops wmt_pinconf_ops = {
+	.pin_config_get = wmt_pinconf_get,
+	.pin_config_set = wmt_pinconf_set,
+};
+
+static struct pinctrl_desc wmt_desc = {
+	.owner = THIS_MODULE,
+	.name = "pinctrl-wmt",
+	.pctlops = &wmt_pctl_ops,
+	.pmxops = &wmt_pinmux_ops,
+	.confops = &wmt_pinconf_ops,
+};
+
+static int wmt_gpio_request(struct gpio_chip *chip, unsigned offset)
+{
+	return pinctrl_request_gpio(chip->base + offset);
+}
+
+static void wmt_gpio_free(struct gpio_chip *chip, unsigned offset)
+{
+	pinctrl_free_gpio(chip->base + offset);
+}
+
+static int wmt_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
+{
+	struct wmt_pinctrl_data *data = dev_get_drvdata(chip->dev);
+	u32 bank = WMT_BANK_FROM_PIN(offset);
+	u32 bit = WMT_BIT_FROM_PIN(offset);
+	u32 reg_dir = data->banks[bank].reg_dir;
+	u32 val;
+
+	val = readl_relaxed(data->base + reg_dir);
+	if (val & BIT(bit))
+		return GPIOF_DIR_OUT;
+	else
+		return GPIOF_DIR_IN;
+}
+
+static int wmt_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
+{
+	return pinctrl_gpio_direction_input(chip->base + offset);
+}
+
+static int wmt_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
+				     int value)
+{
+	return pinctrl_gpio_direction_output(chip->base + offset);
+}
+
+static int wmt_gpio_get_value(struct gpio_chip *chip, unsigned offset)
+{
+	struct wmt_pinctrl_data *data = dev_get_drvdata(chip->dev);
+	u32 bank = WMT_BANK_FROM_PIN(offset);
+	u32 bit = WMT_BIT_FROM_PIN(offset);
+	u32 reg_data_in = data->banks[bank].reg_data_in;
+
+	if (reg_data_in == NO_REG) {
+		dev_err(data->dev, "no data in register defined\n");
+		return -EINVAL;
+	}
+
+	return !!(readl_relaxed(data->base + reg_data_in) & BIT(bit));
+}
+
+static void wmt_gpio_set_value(struct gpio_chip *chip, unsigned offset,
+			       int val)
+{
+	struct wmt_pinctrl_data *data = dev_get_drvdata(chip->dev);
+	u32 bank = WMT_BANK_FROM_PIN(offset);
+	u32 bit = WMT_BIT_FROM_PIN(offset);
+	u32 reg_data_out = data->banks[bank].reg_data_out;
+
+	if (reg_data_out == NO_REG) {
+		dev_err(data->dev, "no data out register defined\n");
+		return;
+	}
+
+	if (val)
+		wmt_setbits(data, reg_data_out, BIT(bit));
+	else
+		wmt_clearbits(data, reg_data_out, BIT(bit));
+}
+
+static struct gpio_chip wmt_gpio_chip = {
+	.label = "gpio-wmt",
+	.owner = THIS_MODULE,
+	.request = wmt_gpio_request,
+	.free = wmt_gpio_free,
+	.get_direction = wmt_gpio_get_direction,
+	.direction_input = wmt_gpio_direction_input,
+	.direction_output = wmt_gpio_direction_output,
+	.get = wmt_gpio_get_value,
+	.set = wmt_gpio_set_value,
+	.can_sleep = 0,
+};
+
+int wmt_pinctrl_probe(struct platform_device *pdev,
+		      struct wmt_pinctrl_data *data)
+{
+	int err;
+	struct resource *res;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	data->base = devm_request_and_ioremap(&pdev->dev, res);
+	if (!data->base) {
+		dev_err(&pdev->dev, "failed to map memory resource\n");
+		return -EBUSY;
+	}
+
+	wmt_desc.pins = data->pins;
+	wmt_desc.npins = data->npins;
+
+	data->gpio_chip = wmt_gpio_chip;
+	data->gpio_chip.dev = &pdev->dev;
+	data->gpio_chip.of_node = pdev->dev.of_node;
+	data->gpio_chip.ngpio = data->nbanks * 32;
+
+	platform_set_drvdata(pdev, data);
+
+	data->dev = &pdev->dev;
+
+	data->pctl_dev = pinctrl_register(&wmt_desc, &pdev->dev, data);
+	if (!data->pctl_dev) {
+		dev_err(&pdev->dev, "Failed to register pinctrl\n");
+		return -EINVAL;
+	}
+
+	err = gpiochip_add(&data->gpio_chip);
+	if (err) {
+		dev_err(&pdev->dev, "could not add GPIO chip\n");
+		goto fail_gpio;
+	}
+
+	err = gpiochip_add_pin_range(&data->gpio_chip, dev_name(data->dev),
+				     0, 0, data->nbanks * 32);
+	if (err)
+		goto fail_range;
+
+	dev_info(&pdev->dev, "Pin controller initialized\n");
+
+	return 0;
+
+fail_range:
+	err = gpiochip_remove(&data->gpio_chip);
+	if (err)
+		dev_err(&pdev->dev, "failed to remove gpio chip\n");
+fail_gpio:
+	pinctrl_unregister(data->pctl_dev);
+	return err;
+}
+
+int wmt_pinctrl_remove(struct platform_device *pdev)
+{
+	struct wmt_pinctrl_data *data = platform_get_drvdata(pdev);
+	int err;
+
+	err = gpiochip_remove(&data->gpio_chip);
+	if (err)
+		dev_err(&pdev->dev, "failed to remove gpio chip\n");
+
+	pinctrl_unregister(data->pctl_dev);
+
+	return 0;
+}
diff --git a/drivers/pinctrl/vt8500/pinctrl-wmt.h b/drivers/pinctrl/vt8500/pinctrl-wmt.h
new file mode 100644
index 000000000000..41f5f2deb5d6
--- /dev/null
+++ b/drivers/pinctrl/vt8500/pinctrl-wmt.h
@@ -0,0 +1,79 @@
+/*
+ * Pinctrl driver for the Wondermedia SoC's
+ *
+ * Copyright (c) 2013 Tony Prisk <linux@prisktech.co.nz>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ */
+
+#include <linux/gpio.h>
+
+/* VT8500 has no enable register in the extgpio bank. */
+#define NO_REG	0xFFFF
+
+#define WMT_PINCTRL_BANK(__en, __dir, __dout, __din, __pen, __pcfg)	\
+{									\
+	.reg_en		= __en,						\
+	.reg_dir	= __dir,					\
+	.reg_data_out	= __dout,					\
+	.reg_data_in	= __din,					\
+	.reg_pull_en	= __pen,					\
+	.reg_pull_cfg	= __pcfg,					\
+}
+
+/* Encode/decode the bank/bit pairs into a pin value */
+#define WMT_PIN(__bank, __offset)	((__bank << 5) | __offset)
+#define WMT_BANK_FROM_PIN(__pin)	(__pin >> 5)
+#define WMT_BIT_FROM_PIN(__pin)		(__pin & 0x1f)
+
+#define WMT_GROUP(__name, __data)		\
+{						\
+	.name = __name,				\
+	.pins = __data,				\
+	.npins = ARRAY_SIZE(__data),		\
+}
+
+struct wmt_pinctrl_bank_registers {
+	u32	reg_en;
+	u32	reg_dir;
+	u32	reg_data_out;
+	u32	reg_data_in;
+
+	u32	reg_pull_en;
+	u32	reg_pull_cfg;
+};
+
+struct wmt_pinctrl_group {
+	const char *name;
+	const unsigned int *pins;
+	const unsigned npins;
+};
+
+struct wmt_pinctrl_data {
+	struct device *dev;
+	struct pinctrl_dev *pctl_dev;
+
+	/* must be initialized before calling wmt_pinctrl_probe */
+	void __iomem *base;
+	const struct wmt_pinctrl_bank_registers *banks;
+	const struct pinctrl_pin_desc *pins;
+	const char * const *groups;
+
+	u32 nbanks;
+	u32 npins;
+	u32 ngroups;
+
+	struct gpio_chip gpio_chip;
+	struct pinctrl_gpio_range gpio_range;
+};
+
+int wmt_pinctrl_probe(struct platform_device *pdev,
+		      struct wmt_pinctrl_data *data);
+int wmt_pinctrl_remove(struct platform_device *pdev);
diff --git a/drivers/platform/x86/sony-laptop.c b/drivers/platform/x86/sony-laptop.c
index 14d4dced1def..d544e3aaf761 100644
--- a/drivers/platform/x86/sony-laptop.c
+++ b/drivers/platform/x86/sony-laptop.c
@@ -4121,7 +4121,7 @@ static int sony_pic_enable(struct acpi_device *device,
 		resource->res3.data.irq.sharable = ACPI_SHARED;
 
 		resource->res4.type = ACPI_RESOURCE_TYPE_END_TAG;
-
+		resource->res4.length = sizeof(struct acpi_resource);
 	}
 	/* setup Type 2/3 resources */
 	else {
@@ -4140,6 +4140,7 @@ static int sony_pic_enable(struct acpi_device *device,
 		resource->res2.data.irq.sharable = ACPI_SHARED;
 
 		resource->res3.type = ACPI_RESOURCE_TYPE_END_TAG;
+		resource->res3.length = sizeof(struct acpi_resource);
 	}
 
 	/* Attempt to set the resource */
diff --git a/drivers/platform/x86/thinkpad_acpi.c b/drivers/platform/x86/thinkpad_acpi.c
index edec135b1685..54d31c0a9840 100644
--- a/drivers/platform/x86/thinkpad_acpi.c
+++ b/drivers/platform/x86/thinkpad_acpi.c
@@ -844,14 +844,14 @@ static int dispatch_proc_show(struct seq_file *m, void *v)
 
 static int dispatch_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, dispatch_proc_show, PDE(inode)->data);
+	return single_open(file, dispatch_proc_show, PDE_DATA(inode));
 }
 
 static ssize_t dispatch_proc_write(struct file *file,
 			const char __user *userbuf,
 			size_t count, loff_t *pos)
 {
-	struct ibm_struct *ibm = PDE(file_inode(file))->data;
+	struct ibm_struct *ibm = PDE_DATA(file_inode(file));
 	char *kernbuf;
 	int ret;
 
diff --git a/drivers/platform/x86/toshiba_acpi.c b/drivers/platform/x86/toshiba_acpi.c
index 242abac62d8b..eb3467ea6d86 100644
--- a/drivers/platform/x86/toshiba_acpi.c
+++ b/drivers/platform/x86/toshiba_acpi.c
@@ -553,7 +553,7 @@ static int lcd_proc_show(struct seq_file *m, void *v)
 
 static int lcd_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, lcd_proc_show, PDE(inode)->data);
+	return single_open(file, lcd_proc_show, PDE_DATA(inode));
 }
 
 static int set_lcd_brightness(struct toshiba_acpi_dev *dev, int value)
@@ -583,7 +583,7 @@ static int set_lcd_status(struct backlight_device *bd)
 static ssize_t lcd_proc_write(struct file *file, const char __user *buf,
 			      size_t count, loff_t *pos)
 {
-	struct toshiba_acpi_dev *dev = PDE(file_inode(file))->data;
+	struct toshiba_acpi_dev *dev = PDE_DATA(file_inode(file));
 	char cmd[42];
 	size_t len;
 	int value;
@@ -644,13 +644,13 @@ static int video_proc_show(struct seq_file *m, void *v)
 
 static int video_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, video_proc_show, PDE(inode)->data);
+	return single_open(file, video_proc_show, PDE_DATA(inode));
 }
 
 static ssize_t video_proc_write(struct file *file, const char __user *buf,
 				size_t count, loff_t *pos)
 {
-	struct toshiba_acpi_dev *dev = PDE(file_inode(file))->data;
+	struct toshiba_acpi_dev *dev = PDE_DATA(file_inode(file));
 	char *cmd, *buffer;
 	int ret;
 	int value;
@@ -744,13 +744,13 @@ static int fan_proc_show(struct seq_file *m, void *v)
 
 static int fan_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, fan_proc_show, PDE(inode)->data);
+	return single_open(file, fan_proc_show, PDE_DATA(inode));
 }
 
 static ssize_t fan_proc_write(struct file *file, const char __user *buf,
 			      size_t count, loff_t *pos)
 {
-	struct toshiba_acpi_dev *dev = PDE(file_inode(file))->data;
+	struct toshiba_acpi_dev *dev = PDE_DATA(file_inode(file));
 	char cmd[42];
 	size_t len;
 	int value;
@@ -816,13 +816,13 @@ static int keys_proc_show(struct seq_file *m, void *v)
 
 static int keys_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, keys_proc_show, PDE(inode)->data);
+	return single_open(file, keys_proc_show, PDE_DATA(inode));
 }
 
 static ssize_t keys_proc_write(struct file *file, const char __user *buf,
 			       size_t count, loff_t *pos)
 {
-	struct toshiba_acpi_dev *dev = PDE(file_inode(file))->data;
+	struct toshiba_acpi_dev *dev = PDE_DATA(file_inode(file));
 	char cmd[42];
 	size_t len;
 	int value;
@@ -859,7 +859,7 @@ static int version_proc_show(struct seq_file *m, void *v)
 
 static int version_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, version_proc_show, PDE(inode)->data);
+	return single_open(file, version_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations version_proc_fops = {
diff --git a/drivers/pnp/isapnp/core.c b/drivers/pnp/isapnp/core.c
index 918d5f044865..cf88f9b62445 100644
--- a/drivers/pnp/isapnp/core.c
+++ b/drivers/pnp/isapnp/core.c
@@ -379,10 +379,6 @@ static int __init isapnp_read_tag(unsigned char *type, unsigned short *size)
 		*type = (tag >> 3) & 0x0f;
 		*size = tag & 0x07;
 	}
-#if 0
-	printk(KERN_DEBUG "tag = 0x%x, type = 0x%x, size = %i\n", tag, *type,
-	       *size);
-#endif
 	if (*type == 0xff && *size == 0xffff)	/* probably invalid data */
 		return -1;
 	return 0;
@@ -813,13 +809,6 @@ static int __init isapnp_build_device_list(void)
 		if (!card)
 			continue;
 
-#if 0
-		dev_info(&card->dev,
-		       "vendor: %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
-		       header[0], header[1], header[2], header[3], header[4],
-		       header[5], header[6], header[7], header[8]);
-		dev_info(&card->dev, "checksum = %#x\n", checksum);
-#endif
 		INIT_LIST_HEAD(&card->devices);
 		card->serial =
 		    (header[7] << 24) | (header[6] << 16) | (header[5] << 8) |
diff --git a/drivers/pnp/isapnp/proc.c b/drivers/pnp/isapnp/proc.c
index 65f735ac6b3b..2365ef37ae24 100644
--- a/drivers/pnp/isapnp/proc.c
+++ b/drivers/pnp/isapnp/proc.c
@@ -55,9 +55,7 @@ static loff_t isapnp_proc_bus_lseek(struct file *file, loff_t off, int whence)
 static ssize_t isapnp_proc_bus_read(struct file *file, char __user * buf,
 				    size_t nbytes, loff_t * ppos)
 {
-	struct inode *ino = file_inode(file);
-	struct proc_dir_entry *dp = PDE(ino);
-	struct pnp_dev *dev = dp->data;
+	struct pnp_dev *dev = PDE_DATA(file_inode(file));
 	int pos = *ppos;
 	int cnt, size = 256;
 
@@ -107,7 +105,7 @@ static int isapnp_proc_attach_device(struct pnp_dev *dev)
 			&isapnp_proc_bus_file_operations, dev);
 	if (!e)
 		return -ENOMEM;
-	e->size = 256;
+	proc_set_size(e, 256);
 	return 0;
 }
 
diff --git a/drivers/pnp/pnpacpi/rsparser.c b/drivers/pnp/pnpacpi/rsparser.c
index b8f4ea7b27fc..9847ab163829 100644
--- a/drivers/pnp/pnpacpi/rsparser.c
+++ b/drivers/pnp/pnpacpi/rsparser.c
@@ -634,6 +634,7 @@ int pnpacpi_build_resource_template(struct pnp_dev *dev,
 	}
 	/* resource will pointer the end resource now */
 	resource->type = ACPI_RESOURCE_TYPE_END_TAG;
+	resource->length = sizeof(struct acpi_resource);
 
 	return 0;
 }
diff --git a/drivers/pnp/pnpbios/core.c b/drivers/pnp/pnpbios/core.c
index 5d66e5585f97..9b86a01af631 100644
--- a/drivers/pnp/pnpbios/core.c
+++ b/drivers/pnp/pnpbios/core.c
@@ -513,10 +513,6 @@ static int __init pnpbios_init(void)
 {
 	int ret;
 
-#if defined(CONFIG_PPC)
-	if (check_legacy_ioport(PNPBIOS_BASE))
-		return -ENODEV;
-#endif
 	if (pnpbios_disabled || dmi_check_system(pnpbios_dmi_table) ||
 	    paravirt_enabled()) {
 		printk(KERN_INFO "PnPBIOS: Disabled\n");
@@ -570,10 +566,7 @@ fs_initcall(pnpbios_init);
 static int __init pnpbios_thread_init(void)
 {
 	struct task_struct *task;
-#if defined(CONFIG_PPC)
-	if (check_legacy_ioport(PNPBIOS_BASE))
-		return 0;
-#endif
+
 	if (pnpbios_disabled)
 		return 0;
 
diff --git a/drivers/pnp/pnpbios/proc.c b/drivers/pnp/pnpbios/proc.c
index 63ddb0173456..c212db0fc65d 100644
--- a/drivers/pnp/pnpbios/proc.c
+++ b/drivers/pnp/pnpbios/proc.c
@@ -185,10 +185,9 @@ static int pnp_devices_proc_show(struct seq_file *m, void *v)
 
 		if (pnp_bios_get_dev_node(&nodenum, PNPMODE_DYNAMIC, node))
 			break;
-		seq_printf(m, "%02x\t%08x\t%02x:%02x:%02x\t%04x\n",
+		seq_printf(m, "%02x\t%08x\t%3phC\t%04x\n",
 			     node->handle, node->eisa_id,
-			     node->type_code[0], node->type_code[1],
-			     node->type_code[2], node->flags);
+			     node->type_code, node->flags);
 		if (nodenum <= thisnodenum) {
 			printk(KERN_ERR
 			       "%s Node number 0x%x is out of sequence following node 0x%x. Aborting.\n",
@@ -238,13 +237,13 @@ static int pnpbios_proc_show(struct seq_file *m, void *v)
 
 static int pnpbios_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, pnpbios_proc_show, PDE(inode)->data);
+	return single_open(file, pnpbios_proc_show, PDE_DATA(inode));
 }
 
 static ssize_t pnpbios_proc_write(struct file *file, const char __user *buf,
 				  size_t count, loff_t *pos)
 {
-	void *data = PDE(file_inode(file))->data;
+	void *data = PDE_DATA(file_inode(file));
 	struct pnp_bios_node *node;
 	int boot = (long)data >> 8;
 	u8 nodenum = (long)data;
diff --git a/drivers/power/88pm860x_charger.c b/drivers/power/88pm860x_charger.c
index 4b37a5af8deb..36fb4b5a4b0d 100644
--- a/drivers/power/88pm860x_charger.c
+++ b/drivers/power/88pm860x_charger.c
@@ -714,7 +714,6 @@ out_irq:
 	while (--i >= 0)
 		free_irq(info->irq[i], info);
 out:
-	kfree(info);
 	return ret;
 }
 
@@ -728,7 +727,6 @@ static int pm860x_charger_remove(struct platform_device *pdev)
 	free_irq(info->irq[0], info);
 	for (i = 0; i < info->irq_nums; i++)
 		free_irq(info->irq[i], info);
-	kfree(info);
 	return 0;
 }
 
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index ffe02fb7cbc7..0d0b5d7d19d0 100644
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -340,6 +340,13 @@ config CHARGER_SMB347
 	  Say Y to include support for Summit Microelectronics SMB347
 	  Battery Charger.
 
+config CHARGER_TPS65090
+	tristate "TPS65090 battery charger driver"
+	depends on MFD_TPS65090
+	help
+	 Say Y here to enable support for battery charging with TPS65090
+	 PMIC chips.
+
 config AB8500_BM
 	bool "AB8500 Battery Management Driver"
 	depends on AB8500_CORE && AB8500_GPADC
@@ -353,13 +360,6 @@ config BATTERY_GOLDFISH
 	  Say Y to enable support for the battery and AC power in the
 	  Goldfish emulator.
 
-config CHARGER_PM2301
-	bool "PM2301 Battery Charger Driver"
-	depends on AB8500_BM
-	help
-	  Say Y to include support for PM2301 charger driver.
-	  Depends on AB8500 battery management core.
-
 source "drivers/power/reset/Kconfig"
 
 endif # POWER_SUPPLY
diff --git a/drivers/power/Makefile b/drivers/power/Makefile
index 3f66436af45c..653bf6ceff30 100644
--- a/drivers/power/Makefile
+++ b/drivers/power/Makefile
@@ -39,7 +39,7 @@ obj-$(CONFIG_CHARGER_PCF50633)	+= pcf50633-charger.o
 obj-$(CONFIG_BATTERY_JZ4740)	+= jz4740-battery.o
 obj-$(CONFIG_BATTERY_INTEL_MID)	+= intel_mid_battery.o
 obj-$(CONFIG_BATTERY_RX51)	+= rx51_battery.o
-obj-$(CONFIG_AB8500_BM)		+= ab8500_bmdata.o ab8500_charger.o ab8500_fg.o ab8500_btemp.o abx500_chargalg.o
+obj-$(CONFIG_AB8500_BM)		+= ab8500_bmdata.o ab8500_charger.o ab8500_fg.o ab8500_btemp.o abx500_chargalg.o pm2301_charger.o
 obj-$(CONFIG_CHARGER_ISP1704)	+= isp1704_charger.o
 obj-$(CONFIG_CHARGER_MAX8903)	+= max8903_charger.o
 obj-$(CONFIG_CHARGER_TWL4030)	+= twl4030_charger.o
@@ -47,10 +47,10 @@ obj-$(CONFIG_CHARGER_LP8727)	+= lp8727_charger.o
 obj-$(CONFIG_CHARGER_LP8788)	+= lp8788-charger.o
 obj-$(CONFIG_CHARGER_GPIO)	+= gpio-charger.o
 obj-$(CONFIG_CHARGER_MANAGER)	+= charger-manager.o
-obj-$(CONFIG_CHARGER_PM2301)	+= pm2301_charger.o
 obj-$(CONFIG_CHARGER_MAX8997)	+= max8997_charger.o
 obj-$(CONFIG_CHARGER_MAX8998)	+= max8998_charger.o
 obj-$(CONFIG_CHARGER_BQ2415X)	+= bq2415x_charger.o
 obj-$(CONFIG_POWER_AVS)		+= avs/
 obj-$(CONFIG_CHARGER_SMB347)	+= smb347-charger.o
+obj-$(CONFIG_CHARGER_TPS65090)	+= tps65090-charger.o
 obj-$(CONFIG_POWER_RESET)	+= reset/
diff --git a/drivers/power/ab8500_bmdata.c b/drivers/power/ab8500_bmdata.c
index 7a96c0650fbb..d29864533093 100644
--- a/drivers/power/ab8500_bmdata.c
+++ b/drivers/power/ab8500_bmdata.c
@@ -11,7 +11,7 @@
  * Note that the res_to_temp table must be strictly sorted by falling resistance
  * values to work.
  */
-static struct abx500_res_to_temp temp_tbl_A_thermistor[] = {
+const struct abx500_res_to_temp ab8500_temp_tbl_a_thermistor[] = {
 	{-5, 53407},
 	{ 0, 48594},
 	{ 5, 43804},
@@ -28,8 +28,12 @@ static struct abx500_res_to_temp temp_tbl_A_thermistor[] = {
 	{60, 13437},
 	{65, 12500},
 };
+EXPORT_SYMBOL(ab8500_temp_tbl_a_thermistor);
 
-static struct abx500_res_to_temp temp_tbl_B_thermistor[] = {
+const int ab8500_temp_tbl_a_size = ARRAY_SIZE(ab8500_temp_tbl_a_thermistor);
+EXPORT_SYMBOL(ab8500_temp_tbl_a_size);
+
+const struct abx500_res_to_temp ab8500_temp_tbl_b_thermistor[] = {
 	{-5, 200000},
 	{ 0, 159024},
 	{ 5, 151921},
@@ -46,8 +50,12 @@ static struct abx500_res_to_temp temp_tbl_B_thermistor[] = {
 	{60,  85461},
 	{65,  82869},
 };
+EXPORT_SYMBOL(ab8500_temp_tbl_b_thermistor);
+
+const int ab8500_temp_tbl_b_size = ARRAY_SIZE(ab8500_temp_tbl_b_thermistor);
+EXPORT_SYMBOL(ab8500_temp_tbl_b_size);
 
-static struct abx500_v_to_cap cap_tbl_A_thermistor[] = {
+static const struct abx500_v_to_cap cap_tbl_a_thermistor[] = {
 	{4171,	100},
 	{4114,	 95},
 	{4009,	 83},
@@ -70,7 +78,7 @@ static struct abx500_v_to_cap cap_tbl_A_thermistor[] = {
 	{3247,	  0},
 };
 
-static struct abx500_v_to_cap cap_tbl_B_thermistor[] = {
+static const struct abx500_v_to_cap cap_tbl_b_thermistor[] = {
 	{4161,	100},
 	{4124,	 98},
 	{4044,	 90},
@@ -93,7 +101,7 @@ static struct abx500_v_to_cap cap_tbl_B_thermistor[] = {
 	{3250,	  0},
 };
 
-static struct abx500_v_to_cap cap_tbl[] = {
+static const struct abx500_v_to_cap cap_tbl[] = {
 	{4186,	100},
 	{4163,	 99},
 	{4114,	 95},
@@ -124,7 +132,7 @@ static struct abx500_v_to_cap cap_tbl[] = {
  * Note that the res_to_temp table must be strictly sorted by falling
  * resistance values to work.
  */
-static struct abx500_res_to_temp temp_tbl[] = {
+static const struct abx500_res_to_temp temp_tbl[] = {
 	{-5, 214834},
 	{ 0, 162943},
 	{ 5, 124820},
@@ -146,7 +154,7 @@ static struct abx500_res_to_temp temp_tbl[] = {
  * Note that the batres_vs_temp table must be strictly sorted by falling
  * temperature values to work.
  */
-static struct batres_vs_temp temp_to_batres_tbl_thermistor[] = {
+static const struct batres_vs_temp temp_to_batres_tbl_thermistor[] = {
 	{ 40, 120},
 	{ 30, 135},
 	{ 20, 165},
@@ -160,7 +168,7 @@ static struct batres_vs_temp temp_to_batres_tbl_thermistor[] = {
  * Note that the batres_vs_temp table must be strictly sorted by falling
  * temperature values to work.
  */
-static struct batres_vs_temp temp_to_batres_tbl_ext_thermistor[] = {
+static const struct batres_vs_temp temp_to_batres_tbl_ext_thermistor[] = {
 	{ 60, 300},
 	{ 30, 300},
 	{ 20, 300},
@@ -171,7 +179,7 @@ static struct batres_vs_temp temp_to_batres_tbl_ext_thermistor[] = {
 };
 
 /* battery resistance table for LI ION 9100 battery */
-static struct batres_vs_temp temp_to_batres_tbl_9100[] = {
+static const struct batres_vs_temp temp_to_batres_tbl_9100[] = {
 	{ 60, 180},
 	{ 30, 180},
 	{ 20, 180},
@@ -230,10 +238,10 @@ static struct abx500_battery_type bat_type_thermistor[] = {
 		.maint_b_chg_timer_h = 200,
 		.low_high_cur_lvl = 300,
 		.low_high_vol_lvl = 4000,
-		.n_temp_tbl_elements = ARRAY_SIZE(temp_tbl_A_thermistor),
-		.r_to_t_tbl = temp_tbl_A_thermistor,
-		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl_A_thermistor),
-		.v_to_cap_tbl = cap_tbl_A_thermistor,
+		.n_temp_tbl_elements = ARRAY_SIZE(ab8500_temp_tbl_a_thermistor),
+		.r_to_t_tbl = ab8500_temp_tbl_a_thermistor,
+		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl_a_thermistor),
+		.v_to_cap_tbl = cap_tbl_a_thermistor,
 		.n_batres_tbl_elements = ARRAY_SIZE(temp_to_batres_tbl_thermistor),
 		.batres_tbl = temp_to_batres_tbl_thermistor,
 
@@ -258,10 +266,10 @@ static struct abx500_battery_type bat_type_thermistor[] = {
 		.maint_b_chg_timer_h = 200,
 		.low_high_cur_lvl = 300,
 		.low_high_vol_lvl = 4000,
-		.n_temp_tbl_elements = ARRAY_SIZE(temp_tbl_B_thermistor),
-		.r_to_t_tbl = temp_tbl_B_thermistor,
-		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl_B_thermistor),
-		.v_to_cap_tbl = cap_tbl_B_thermistor,
+		.n_temp_tbl_elements = ARRAY_SIZE(ab8500_temp_tbl_b_thermistor),
+		.r_to_t_tbl = ab8500_temp_tbl_b_thermistor,
+		.n_v_cap_tbl_elements = ARRAY_SIZE(cap_tbl_b_thermistor),
+		.v_to_cap_tbl = cap_tbl_b_thermistor,
 		.n_batres_tbl_elements = ARRAY_SIZE(temp_to_batres_tbl_thermistor),
 		.batres_tbl = temp_to_batres_tbl_thermistor,
 	},
@@ -407,15 +415,27 @@ static const struct abx500_fg_parameters fg = {
 	.battok_raising_th_sel1 = 2860,
 	.maint_thres = 95,
 	.user_cap_limit = 15,
+	.pcut_enable = 1,
+	.pcut_max_time = 127,
+	.pcut_flag_time = 112,
+	.pcut_max_restart = 15,
+	.pcut_debounce_time = 2,
 };
 
-static const struct abx500_maxim_parameters maxi_params = {
+static const struct abx500_maxim_parameters ab8500_maxi_params = {
 	.ena_maxi = true,
 	.chg_curr = 910,
 	.wait_cycles = 10,
 	.charger_curr_step = 100,
 };
 
+static const struct abx500_maxim_parameters abx540_maxi_params = {
+        .ena_maxi = true,
+        .chg_curr = 3000,
+        .wait_cycles = 10,
+        .charger_curr_step = 200,
+};
+
 static const struct abx500_bm_charger_parameters chg = {
 	.usb_volt_max		= 5500,
 	.usb_curr_max		= 1500,
@@ -423,6 +443,46 @@ static const struct abx500_bm_charger_parameters chg = {
 	.ac_curr_max		= 1500,
 };
 
+/*
+ * This array maps the raw hex value to charger output current used by the
+ * AB8500 values
+ */
+static int ab8500_charge_output_curr_map[] = {
+        100,    200,    300,    400,    500,    600,    700,    800,
+        900,    1000,   1100,   1200,   1300,   1400,   1500,   1500,
+};
+
+static int ab8540_charge_output_curr_map[] = {
+        0,      0,      0,      75,     100,    125,    150,    175,
+        200,    225,    250,    275,    300,    325,    350,    375,
+        400,    425,    450,    475,    500,    525,    550,    575,
+        600,    625,    650,    675,    700,    725,    750,    775,
+        800,    825,    850,    875,    900,    925,    950,    975,
+        1000,   1025,   1050,   1075,   1100,   1125,   1150,   1175,
+        1200,   1225,   1250,   1275,   1300,   1325,   1350,   1375,
+        1400,   1425,   1450,   1500,   1600,   1700,   1900,   2000,
+};
+
+/*
+ * This array maps the raw hex value to charger input current used by the
+ * AB8500 values
+ */
+static int ab8500_charge_input_curr_map[] = {
+        50,     98,     193,    290,    380,    450,    500,    600,
+        700,    800,    900,    1000,   1100,   1300,   1400,   1500,
+};
+
+static int ab8540_charge_input_curr_map[] = {
+        25,     50,     75,     100,    125,    150,    175,    200,
+        225,    250,    275,    300,    325,    350,    375,    400,
+        425,    450,    475,    500,    525,    550,    575,    600,
+        625,    650,    675,    700,    725,    750,    775,    800,
+        825,    850,    875,    900,    925,    950,    975,    1000,
+        1025,   1050,   1075,   1100,   1125,   1150,   1175,   1200,
+        1225,   1250,   1275,   1300,   1325,   1350,   1375,   1400,
+        1425,   1450,   1475,   1500,   1500,   1500,   1500,   1500,
+};
+
 struct abx500_bm_data ab8500_bm_data = {
 	.temp_under             = 3,
 	.temp_low               = 8,
@@ -442,22 +502,60 @@ struct abx500_bm_data ab8500_bm_data = {
 	.fg_res                 = 100,
 	.cap_levels             = &cap_levels,
 	.bat_type               = bat_type_thermistor,
-	.n_btypes               = 3,
+	.n_btypes               = ARRAY_SIZE(bat_type_thermistor),
 	.batt_id                = 0,
 	.interval_charging      = 5,
 	.interval_not_charging  = 120,
 	.temp_hysteresis        = 3,
 	.gnd_lift_resistance    = 34,
-	.maxi                   = &maxi_params,
+	.chg_output_curr        = ab8500_charge_output_curr_map,
+	.n_chg_out_curr         = ARRAY_SIZE(ab8500_charge_output_curr_map),
+	.maxi                   = &ab8500_maxi_params,
 	.chg_params             = &chg,
 	.fg_params              = &fg,
+        .chg_input_curr         = ab8500_charge_input_curr_map,
+        .n_chg_in_curr          = ARRAY_SIZE(ab8500_charge_input_curr_map),
+};
+
+struct abx500_bm_data ab8540_bm_data = {
+        .temp_under             = 3,
+        .temp_low               = 8,
+        .temp_high              = 43,
+        .temp_over              = 48,
+        .main_safety_tmr_h      = 4,
+        .temp_interval_chg      = 20,
+        .temp_interval_nochg    = 120,
+        .usb_safety_tmr_h       = 4,
+        .bkup_bat_v             = BUP_VCH_SEL_2P6V,
+        .bkup_bat_i             = BUP_ICH_SEL_150UA,
+        .no_maintenance         = false,
+        .capacity_scaling       = false,
+        .adc_therm              = ABx500_ADC_THERM_BATCTRL,
+        .chg_unknown_bat        = false,
+        .enable_overshoot       = false,
+        .fg_res                 = 100,
+        .cap_levels             = &cap_levels,
+        .bat_type               = bat_type_thermistor,
+        .n_btypes               = ARRAY_SIZE(bat_type_thermistor),
+        .batt_id                = 0,
+        .interval_charging      = 5,
+        .interval_not_charging  = 120,
+        .temp_hysteresis        = 3,
+        .gnd_lift_resistance    = 0,
+        .maxi                   = &abx540_maxi_params,
+        .chg_params             = &chg,
+        .fg_params              = &fg,
+        .chg_output_curr        = ab8540_charge_output_curr_map,
+        .n_chg_out_curr         = ARRAY_SIZE(ab8540_charge_output_curr_map),
+        .chg_input_curr         = ab8540_charge_input_curr_map,
+        .n_chg_in_curr          = ARRAY_SIZE(ab8540_charge_input_curr_map),
 };
 
 int ab8500_bm_of_probe(struct device *dev,
 		       struct device_node *np,
 		       struct abx500_bm_data *bm)
 {
-	struct batres_vs_temp *tmp_batres_tbl;
+	const struct batres_vs_temp *tmp_batres_tbl;
 	struct device_node *battery_node;
 	const char *btech;
 	int i;
diff --git a/drivers/power/ab8500_btemp.c b/drivers/power/ab8500_btemp.c
index 07689064996e..d412d34bf3df 100644
--- a/drivers/power/ab8500_btemp.c
+++ b/drivers/power/ab8500_btemp.c
@@ -42,6 +42,9 @@
 #define BTEMP_BATCTRL_CURR_SRC_16UA	16
 #define BTEMP_BATCTRL_CURR_SRC_18UA	18
 
+#define BTEMP_BATCTRL_CURR_SRC_60UA	60
+#define BTEMP_BATCTRL_CURR_SRC_120UA	120
+
 #define to_ab8500_btemp_device_info(x) container_of((x), \
 	struct ab8500_btemp, btemp_psy);
 
@@ -76,8 +79,8 @@ struct ab8500_btemp_ranges {
  * @dev:		Pointer to the structure device
  * @node:		List of AB8500 BTEMPs, hence prepared for reentrance
  * @curr_source:	What current source we use, in uA
- * @bat_temp:		Battery temperature in degree Celcius
- * @prev_bat_temp	Last dispatched battery temperature
+ * @bat_temp:		Dispatched battery temperature in degree Celcius
+ * @prev_bat_temp	Last measured battery temperature in degree Celcius
  * @parent:		Pointer to the struct ab8500
  * @gpadc:		Pointer to the struct gpadc
  * @fg:			Pointer to the struct fg
@@ -128,6 +131,7 @@ struct ab8500_btemp *ab8500_btemp_get(void)
 
 	return btemp;
 }
+EXPORT_SYMBOL(ab8500_btemp_get);
 
 /**
  * ab8500_btemp_batctrl_volt_to_res() - convert batctrl voltage to resistance
@@ -155,7 +159,7 @@ static int ab8500_btemp_batctrl_volt_to_res(struct ab8500_btemp *di,
 	if (di->bm->adc_therm == ABx500_ADC_THERM_BATCTRL) {
 		/*
 		 * If the battery has internal NTC, we use the current
-		 * source to calculate the resistance, 7uA or 20uA
+		 * source to calculate the resistance.
 		 */
 		rbs = (v_batctrl * 1000
 		       - di->bm->gnd_lift_resistance * inst_curr)
@@ -216,7 +220,12 @@ static int ab8500_btemp_curr_source_enable(struct ab8500_btemp *di,
 	/* Only do this for batteries with internal NTC */
 	if (di->bm->adc_therm == ABx500_ADC_THERM_BATCTRL && enable) {
 
-		if (is_ab9540(di->parent) || is_ab8505(di->parent)) {
+		if (is_ab8540(di->parent)) {
+			if (di->curr_source == BTEMP_BATCTRL_CURR_SRC_60UA)
+				curr = BAT_CTRL_60U_ENA;
+			else
+				curr = BAT_CTRL_120U_ENA;
+		} else if (is_ab9540(di->parent) || is_ab8505(di->parent)) {
 			if (di->curr_source == BTEMP_BATCTRL_CURR_SRC_16UA)
 				curr = BAT_CTRL_16U_ENA;
 			else
@@ -257,7 +266,14 @@ static int ab8500_btemp_curr_source_enable(struct ab8500_btemp *di,
 	} else if (di->bm->adc_therm == ABx500_ADC_THERM_BATCTRL && !enable) {
 		dev_dbg(di->dev, "Disable BATCTRL curr source\n");
 
-		if (is_ab9540(di->parent) || is_ab8505(di->parent)) {
+		if (is_ab8540(di->parent)) {
+			/* Write 0 to the curr bits */
+			ret = abx500_mask_and_set_register_interruptible(
+				di->dev,
+				AB8500_CHARGER, AB8500_BAT_CTRL_CURRENT_SOURCE,
+				BAT_CTRL_60U_ENA | BAT_CTRL_120U_ENA,
+				~(BAT_CTRL_60U_ENA | BAT_CTRL_120U_ENA));
+		} else if (is_ab9540(di->parent) || is_ab8505(di->parent)) {
 			/* Write 0 to the curr bits */
 			ret = abx500_mask_and_set_register_interruptible(
 				di->dev,
@@ -314,7 +330,13 @@ static int ab8500_btemp_curr_source_enable(struct ab8500_btemp *di,
 	 * if we got an error above
 	 */
 disable_curr_source:
-	if (is_ab9540(di->parent) || is_ab8505(di->parent)) {
+	if (is_ab8540(di->parent)) {
+		/* Write 0 to the curr bits */
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB8500_CHARGER, AB8500_BAT_CTRL_CURRENT_SOURCE,
+			BAT_CTRL_60U_ENA | BAT_CTRL_120U_ENA,
+			~(BAT_CTRL_60U_ENA | BAT_CTRL_120U_ENA));
+	} else if (is_ab9540(di->parent) || is_ab8505(di->parent)) {
 		/* Write 0 to the curr bits */
 		ret = abx500_mask_and_set_register_interruptible(di->dev,
 			AB8500_CHARGER, AB8500_BAT_CTRL_CURRENT_SOURCE,
@@ -541,7 +563,9 @@ static int ab8500_btemp_id(struct ab8500_btemp *di)
 {
 	int res;
 	u8 i;
-	if (is_ab9540(di->parent) || is_ab8505(di->parent))
+	if (is_ab8540(di->parent))
+		di->curr_source = BTEMP_BATCTRL_CURR_SRC_60UA;
+	else if (is_ab9540(di->parent) || is_ab8505(di->parent))
 		di->curr_source = BTEMP_BATCTRL_CURR_SRC_16UA;
 	else
 		di->curr_source = BTEMP_BATCTRL_CURR_SRC_7UA;
@@ -579,12 +603,17 @@ static int ab8500_btemp_id(struct ab8500_btemp *di)
 
 	/*
 	 * We only have to change current source if the
-	 * detected type is Type 1, else we use the 7uA source
+	 * detected type is Type 1.
 	 */
 	if (di->bm->adc_therm == ABx500_ADC_THERM_BATCTRL &&
-			di->bm->batt_id == 1) {
-		if (is_ab9540(di->parent) || is_ab8505(di->parent)) {
-			dev_dbg(di->dev, "Set BATCTRL current source to 16uA\n");
+	    di->bm->batt_id == 1) {
+		if (is_ab8540(di->parent)) {
+			dev_dbg(di->dev,
+				"Set BATCTRL current source to 60uA\n");
+			di->curr_source = BTEMP_BATCTRL_CURR_SRC_60UA;
+		} else if (is_ab9540(di->parent) || is_ab8505(di->parent)) {
+			dev_dbg(di->dev,
+				"Set BATCTRL current source to 16uA\n");
 			di->curr_source = BTEMP_BATCTRL_CURR_SRC_16UA;
 		} else {
 			dev_dbg(di->dev, "Set BATCTRL current source to 20uA\n");
@@ -604,22 +633,37 @@ static int ab8500_btemp_id(struct ab8500_btemp *di)
 static void ab8500_btemp_periodic_work(struct work_struct *work)
 {
 	int interval;
+	int bat_temp;
 	struct ab8500_btemp *di = container_of(work,
 		struct ab8500_btemp, btemp_periodic_work.work);
 
 	if (!di->initialized) {
-		di->initialized = true;
 		/* Identify the battery */
 		if (ab8500_btemp_id(di) < 0)
 			dev_warn(di->dev, "failed to identify the battery\n");
 	}
 
-	di->bat_temp = ab8500_btemp_measure_temp(di);
-
-	if (di->bat_temp != di->prev_bat_temp) {
-		di->prev_bat_temp = di->bat_temp;
+	bat_temp = ab8500_btemp_measure_temp(di);
+	/*
+	 * Filter battery temperature.
+	 * Allow direct updates on temperature only if two samples result in
+	 * same temperature. Else only allow 1 degree change from previous
+	 * reported value in the direction of the new measurement.
+	 */
+	if ((bat_temp == di->prev_bat_temp) || !di->initialized) {
+		if ((di->bat_temp != di->prev_bat_temp) || !di->initialized) {
+			di->initialized = true;
+			di->bat_temp = bat_temp;
+			power_supply_changed(&di->btemp_psy);
+		}
+	} else if (bat_temp < di->prev_bat_temp) {
+		di->bat_temp--;
+		power_supply_changed(&di->btemp_psy);
+	} else if (bat_temp > di->prev_bat_temp) {
+		di->bat_temp++;
 		power_supply_changed(&di->btemp_psy);
 	}
+	di->prev_bat_temp = bat_temp;
 
 	if (di->events.ac_conn || di->events.usb_conn)
 		interval = di->bm->temp_interval_chg;
@@ -772,7 +816,7 @@ static void ab8500_btemp_periodic(struct ab8500_btemp *di,
  *
  * Returns battery temperature
  */
-static int ab8500_btemp_get_temp(struct ab8500_btemp *di)
+int ab8500_btemp_get_temp(struct ab8500_btemp *di)
 {
 	int temp = 0;
 
@@ -808,6 +852,7 @@ static int ab8500_btemp_get_temp(struct ab8500_btemp *di)
 	}
 	return temp;
 }
+EXPORT_SYMBOL(ab8500_btemp_get_temp);
 
 /**
  * ab8500_btemp_get_batctrl_temp() - get the temperature
@@ -819,6 +864,7 @@ int ab8500_btemp_get_batctrl_temp(struct ab8500_btemp *btemp)
 {
 	return btemp->bat_temp * 1000;
 }
+EXPORT_SYMBOL(ab8500_btemp_get_batctrl_temp);
 
 /**
  * ab8500_btemp_get_property() - get the btemp properties
diff --git a/drivers/power/ab8500_charger.c b/drivers/power/ab8500_charger.c
index 24b30b7ea5ca..a558318b169c 100644
--- a/drivers/power/ab8500_charger.c
+++ b/drivers/power/ab8500_charger.c
@@ -15,6 +15,7 @@
 #include <linux/device.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
+#include <linux/notifier.h>
 #include <linux/slab.h>
 #include <linux/platform_device.h>
 #include <linux/power_supply.h>
@@ -52,10 +53,15 @@
 #define VBUS_DET_DBNC100		0x02
 #define VBUS_DET_DBNC1			0x01
 #define OTP_ENABLE_WD			0x01
+#define DROP_COUNT_RESET		0x01
+#define USB_CH_DET			0x01
 
 #define MAIN_CH_INPUT_CURR_SHIFT	4
 #define VBUS_IN_CURR_LIM_SHIFT		4
+#define AB8540_VBUS_IN_CURR_LIM_SHIFT	2
 #define AUTO_VBUS_IN_CURR_LIM_SHIFT	4
+#define AB8540_AUTO_VBUS_IN_CURR_MASK	0x3F
+#define VBUS_IN_CURR_LIM_RETRY_SET_TIME	30 /* seconds */
 
 #define LED_INDICATOR_PWM_ENA		0x01
 #define LED_INDICATOR_PWM_DIS		0x00
@@ -77,7 +83,9 @@
 
 /* UsbLineStatus register bit masks */
 #define AB8500_USB_LINK_STATUS		0x78
+#define AB8505_USB_LINK_STATUS		0xF8
 #define AB8500_STD_HOST_SUSP		0x18
+#define USB_LINK_STATUS_SHIFT		3
 
 /* Watchdog timeout constant */
 #define WD_TIMER			0x30 /* 4min */
@@ -96,6 +104,10 @@
 #define AB8500_SW_CONTROL_FALLBACK	0x03
 /* Wait for enumeration before charing in us */
 #define WAIT_ACA_RID_ENUMERATION	(5 * 1000)
+/*External charger control*/
+#define AB8500_SYS_CHARGER_CONTROL_REG		0x52
+#define EXTERNAL_CHARGER_DISABLE_REG_VAL	0x03
+#define EXTERNAL_CHARGER_ENABLE_REG_VAL		0x07
 
 /* UsbLineStatus register - usb types */
 enum ab8500_charger_link_status {
@@ -196,10 +208,15 @@ struct ab8500_charger_usb_state {
 	spinlock_t usb_lock;
 };
 
+struct ab8500_charger_max_usb_in_curr {
+	int usb_type_max;
+	int set_max;
+	int calculated_max;
+};
+
 /**
  * struct ab8500_charger - ab8500 Charger device information
  * @dev:		Pointer to the structure device
- * @max_usb_in_curr:	Max USB charger input current
  * @vbus_detected:	VBUS detected
  * @vbus_detected_start:
  *			VBUS detected during startup
@@ -214,7 +231,6 @@ struct ab8500_charger_usb_state {
  * @autopower		Indicate if we should have automatic pwron after pwrloss
  * @autopower_cfg	platform specific power config support for "pwron after pwrloss"
  * @invalid_charger_detect_state State when forcing AB to use invalid charger
- * @is_usb_host:	Indicate if last detected USB type is host
  * @is_aca_rid:		Incicate if accessory is ACA type
  * @current_stepping_sessions:
  *			Counter for current stepping sessions
@@ -223,6 +239,7 @@ struct ab8500_charger_usb_state {
  * @bm:           	Platform specific battery management information
  * @flags:		Structure for information about events triggered
  * @usb_state:		Structure for usb stack information
+ * @max_usb_in_curr:	Max USB charger input current
  * @ac_chg:		AC charger power supply
  * @usb_chg:		USB charger power supply
  * @ac:			Structure that holds the AC charger properties
@@ -254,7 +271,6 @@ struct ab8500_charger_usb_state {
  */
 struct ab8500_charger {
 	struct device *dev;
-	int max_usb_in_curr;
 	bool vbus_detected;
 	bool vbus_detected_start;
 	bool ac_conn;
@@ -266,7 +282,6 @@ struct ab8500_charger {
 	bool autopower;
 	bool autopower_cfg;
 	int invalid_charger_detect_state;
-	bool is_usb_host;
 	int is_aca_rid;
 	atomic_t current_stepping_sessions;
 	struct ab8500 *parent;
@@ -274,6 +289,7 @@ struct ab8500_charger {
 	struct abx500_bm_data *bm;
 	struct ab8500_charger_event_flags flags;
 	struct ab8500_charger_usb_state usb_state;
+	struct ab8500_charger_max_usb_in_curr max_usb_in_curr;
 	struct ux500_charger ac_chg;
 	struct ux500_charger usb_chg;
 	struct ab8500_charger_info ac;
@@ -415,13 +431,18 @@ static void ab8500_charger_set_usb_connected(struct ab8500_charger *di,
 	if (connected != di->usb.charger_connected) {
 		dev_dbg(di->dev, "USB connected:%i\n", connected);
 		di->usb.charger_connected = connected;
+
+		if (!connected)
+			di->flags.vbus_drop_end = false;
+
 		sysfs_notify(&di->usb_chg.psy.dev->kobj, NULL, "present");
 
 		if (connected) {
 			mutex_lock(&di->charger_attached_mutex);
 			mutex_unlock(&di->charger_attached_mutex);
 
-			queue_delayed_work(di->charger_wq,
+			if (is_ab8500(di->parent))
+				queue_delayed_work(di->charger_wq,
 					   &di->usb_charger_attached_work,
 					   HZ);
 		} else {
@@ -668,23 +689,19 @@ static int ab8500_charger_max_usb_curr(struct ab8500_charger *di,
 	case USB_STAT_STD_HOST_C_S:
 		dev_dbg(di->dev, "USB Type - Standard host is "
 			"detected through USB driver\n");
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P5;
-		di->is_usb_host = true;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P5;
 		di->is_aca_rid = 0;
 		break;
 	case USB_STAT_HOST_CHG_HS_CHIRP:
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P5;
-		di->is_usb_host = true;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P5;
 		di->is_aca_rid = 0;
 		break;
 	case USB_STAT_HOST_CHG_HS:
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P5;
-		di->is_usb_host = true;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P5;
 		di->is_aca_rid = 0;
 		break;
 	case USB_STAT_ACA_RID_C_HS:
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P9;
-		di->is_usb_host = false;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P9;
 		di->is_aca_rid = 0;
 		break;
 	case USB_STAT_ACA_RID_A:
@@ -693,8 +710,7 @@ static int ab8500_charger_max_usb_curr(struct ab8500_charger *di,
 		 * can consume (900mA). Closest level is 500mA
 		 */
 		dev_dbg(di->dev, "USB_STAT_ACA_RID_A detected\n");
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P5;
-		di->is_usb_host = false;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P5;
 		di->is_aca_rid = 1;
 		break;
 	case USB_STAT_ACA_RID_B:
@@ -702,38 +718,35 @@ static int ab8500_charger_max_usb_curr(struct ab8500_charger *di,
 		 * Dedicated charger level minus 120mA (20mA for ACA and
 		 * 100mA for potential accessory). Closest level is 1300mA
 		 */
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_1P3;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_1P3;
 		dev_dbg(di->dev, "USB Type - 0x%02x MaxCurr: %d", link_status,
-				di->max_usb_in_curr);
-		di->is_usb_host = false;
+				di->max_usb_in_curr.usb_type_max);
 		di->is_aca_rid = 1;
 		break;
 	case USB_STAT_HOST_CHG_NM:
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P5;
-		di->is_usb_host = true;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P5;
 		di->is_aca_rid = 0;
 		break;
 	case USB_STAT_DEDICATED_CHG:
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_1P5;
-		di->is_usb_host = false;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_1P5;
 		di->is_aca_rid = 0;
 		break;
 	case USB_STAT_ACA_RID_C_HS_CHIRP:
 	case USB_STAT_ACA_RID_C_NM:
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_1P5;
-		di->is_usb_host = false;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_1P5;
 		di->is_aca_rid = 1;
 		break;
 	case USB_STAT_NOT_CONFIGURED:
 		if (di->vbus_detected) {
 			di->usb_device_is_unrecognised = true;
 			dev_dbg(di->dev, "USB Type - Legacy charger.\n");
-			di->max_usb_in_curr = USB_CH_IP_CUR_LVL_1P5;
+			di->max_usb_in_curr.usb_type_max =
+						USB_CH_IP_CUR_LVL_1P5;
 			break;
 		}
 	case USB_STAT_HM_IDGND:
 		dev_err(di->dev, "USB Type - Charging not allowed\n");
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P05;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P05;
 		ret = -ENXIO;
 		break;
 	case USB_STAT_RESERVED:
@@ -743,12 +756,13 @@ static int ab8500_charger_max_usb_curr(struct ab8500_charger *di,
 						"VBUS has collapsed\n");
 			ret = -ENXIO;
 			break;
-		}
-		if (is_ab9540(di->parent) || is_ab8505(di->parent)) {
+		} else {
 			dev_dbg(di->dev, "USB Type - Charging not allowed\n");
-			di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P05;
+			di->max_usb_in_curr.usb_type_max =
+						USB_CH_IP_CUR_LVL_0P05;
 			dev_dbg(di->dev, "USB Type - 0x%02x MaxCurr: %d",
-					link_status, di->max_usb_in_curr);
+				link_status,
+				di->max_usb_in_curr.usb_type_max);
 			ret = -ENXIO;
 			break;
 		}
@@ -757,23 +771,24 @@ static int ab8500_charger_max_usb_curr(struct ab8500_charger *di,
 	case USB_STAT_CARKIT_2:
 	case USB_STAT_ACA_DOCK_CHARGER:
 	case USB_STAT_CHARGER_LINE_1:
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P5;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P5;
 		dev_dbg(di->dev, "USB Type - 0x%02x MaxCurr: %d", link_status,
-				di->max_usb_in_curr);
+				di->max_usb_in_curr.usb_type_max);
 	case USB_STAT_NOT_VALID_LINK:
 		dev_err(di->dev, "USB Type invalid - try charging anyway\n");
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P5;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P5;
 		break;
 
 	default:
 		dev_err(di->dev, "USB Type - Unknown\n");
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P05;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P05;
 		ret = -ENXIO;
 		break;
 	};
 
+	di->max_usb_in_curr.set_max = di->max_usb_in_curr.usb_type_max;
 	dev_dbg(di->dev, "USB Type - 0x%02x MaxCurr: %d",
-		link_status, di->max_usb_in_curr);
+		link_status, di->max_usb_in_curr.set_max);
 
 	return ret;
 }
@@ -796,21 +811,22 @@ static int ab8500_charger_read_usb_type(struct ab8500_charger *di)
 		dev_err(di->dev, "%s ab8500 read failed\n", __func__);
 		return ret;
 	}
-	if (is_ab8500(di->parent)) {
+	if (is_ab8500(di->parent))
 		ret = abx500_get_register_interruptible(di->dev, AB8500_USB,
-				AB8500_USB_LINE_STAT_REG, &val);
-	} else {
-		if (is_ab9540(di->parent) || is_ab8505(di->parent))
-			ret = abx500_get_register_interruptible(di->dev,
-				AB8500_USB, AB8500_USB_LINK1_STAT_REG, &val);
-	}
+			AB8500_USB_LINE_STAT_REG, &val);
+	else
+		ret = abx500_get_register_interruptible(di->dev,
+			AB8500_USB, AB8500_USB_LINK1_STAT_REG, &val);
 	if (ret < 0) {
 		dev_err(di->dev, "%s ab8500 read failed\n", __func__);
 		return ret;
 	}
 
 	/* get the USB type */
-	val = (val & AB8500_USB_LINK_STATUS) >> 3;
+	if (is_ab8500(di->parent))
+		val = (val & AB8500_USB_LINK_STATUS) >> USB_LINK_STATUS_SHIFT;
+	else
+		val = (val & AB8505_USB_LINK_STATUS) >> USB_LINK_STATUS_SHIFT;
 	ret = ab8500_charger_max_usb_curr(di,
 		(enum ab8500_charger_link_status) val);
 
@@ -865,7 +881,12 @@ static int ab8500_charger_detect_usb_type(struct ab8500_charger *di)
 		 */
 
 		/* get the USB type */
-		val = (val & AB8500_USB_LINK_STATUS) >> 3;
+		if (is_ab8500(di->parent))
+			val = (val & AB8500_USB_LINK_STATUS) >>
+							USB_LINK_STATUS_SHIFT;
+		else
+			val = (val & AB8505_USB_LINK_STATUS) >>
+							USB_LINK_STATUS_SHIFT;
 		if (val)
 			break;
 	}
@@ -960,51 +981,6 @@ static int ab8500_charger_voltage_map[] = {
 	4600 ,
 };
 
-/*
- * This array maps the raw hex value to charger current used by the AB8500
- * Values taken from the UM0836
- */
-static int ab8500_charger_current_map[] = {
-	100 ,
-	200 ,
-	300 ,
-	400 ,
-	500 ,
-	600 ,
-	700 ,
-	800 ,
-	900 ,
-	1000 ,
-	1100 ,
-	1200 ,
-	1300 ,
-	1400 ,
-	1500 ,
-};
-
-/*
- * This array maps the raw hex value to VBUS input current used by the AB8500
- * Values taken from the UM0836
- */
-static int ab8500_charger_vbus_in_curr_map[] = {
-	USB_CH_IP_CUR_LVL_0P05,
-	USB_CH_IP_CUR_LVL_0P09,
-	USB_CH_IP_CUR_LVL_0P19,
-	USB_CH_IP_CUR_LVL_0P29,
-	USB_CH_IP_CUR_LVL_0P38,
-	USB_CH_IP_CUR_LVL_0P45,
-	USB_CH_IP_CUR_LVL_0P5,
-	USB_CH_IP_CUR_LVL_0P6,
-	USB_CH_IP_CUR_LVL_0P7,
-	USB_CH_IP_CUR_LVL_0P8,
-	USB_CH_IP_CUR_LVL_0P9,
-	USB_CH_IP_CUR_LVL_1P0,
-	USB_CH_IP_CUR_LVL_1P1,
-	USB_CH_IP_CUR_LVL_1P3,
-	USB_CH_IP_CUR_LVL_1P4,
-	USB_CH_IP_CUR_LVL_1P5,
-};
-
 static int ab8500_voltage_to_regval(int voltage)
 {
 	int i;
@@ -1026,41 +1002,41 @@ static int ab8500_voltage_to_regval(int voltage)
 		return -1;
 }
 
-static int ab8500_current_to_regval(int curr)
+static int ab8500_current_to_regval(struct ab8500_charger *di, int curr)
 {
 	int i;
 
-	if (curr < ab8500_charger_current_map[0])
+	if (curr < di->bm->chg_output_curr[0])
 		return 0;
 
-	for (i = 0; i < ARRAY_SIZE(ab8500_charger_current_map); i++) {
-		if (curr < ab8500_charger_current_map[i])
+	for (i = 0; i < di->bm->n_chg_out_curr; i++) {
+		if (curr < di->bm->chg_output_curr[i])
 			return i - 1;
 	}
 
 	/* If not last element, return error */
-	i = ARRAY_SIZE(ab8500_charger_current_map) - 1;
-	if (curr == ab8500_charger_current_map[i])
+	i = di->bm->n_chg_out_curr - 1;
+	if (curr == di->bm->chg_output_curr[i])
 		return i;
 	else
 		return -1;
 }
 
-static int ab8500_vbus_in_curr_to_regval(int curr)
+static int ab8500_vbus_in_curr_to_regval(struct ab8500_charger *di, int curr)
 {
 	int i;
 
-	if (curr < ab8500_charger_vbus_in_curr_map[0])
+	if (curr < di->bm->chg_input_curr[0])
 		return 0;
 
-	for (i = 0; i < ARRAY_SIZE(ab8500_charger_vbus_in_curr_map); i++) {
-		if (curr < ab8500_charger_vbus_in_curr_map[i])
+	for (i = 0; i < di->bm->n_chg_in_curr; i++) {
+		if (curr < di->bm->chg_input_curr[i])
 			return i - 1;
 	}
 
 	/* If not last element, return error */
-	i = ARRAY_SIZE(ab8500_charger_vbus_in_curr_map) - 1;
-	if (curr == ab8500_charger_vbus_in_curr_map[i])
+	i = di->bm->n_chg_in_curr - 1;
+	if (curr == di->bm->chg_input_curr[i])
 		return i;
 	else
 		return -1;
@@ -1077,28 +1053,48 @@ static int ab8500_vbus_in_curr_to_regval(int curr)
  */
 static int ab8500_charger_get_usb_cur(struct ab8500_charger *di)
 {
+	int ret = 0;
 	switch (di->usb_state.usb_current) {
 	case 100:
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P09;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P09;
 		break;
 	case 200:
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P19;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P19;
 		break;
 	case 300:
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P29;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P29;
 		break;
 	case 400:
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P38;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P38;
 		break;
 	case 500:
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P5;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P5;
 		break;
 	default:
-		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P05;
-		return -1;
+		di->max_usb_in_curr.usb_type_max = USB_CH_IP_CUR_LVL_0P05;
+		ret = -EPERM;
 		break;
 	};
-	return 0;
+	di->max_usb_in_curr.set_max = di->max_usb_in_curr.usb_type_max;
+	return ret;
+}
+
+/**
+ * ab8500_charger_check_continue_stepping() - Check to allow stepping
+ * @di:		pointer to the ab8500_charger structure
+ * @reg:	select what charger register to check
+ *
+ * Check if current stepping should be allowed to continue.
+ * Checks if charger source has not collapsed. If it has, further stepping
+ * is not allowed.
+ */
+static bool ab8500_charger_check_continue_stepping(struct ab8500_charger *di,
+						   int reg)
+{
+	if (reg == AB8500_USBCH_IPT_CRNTLVL_REG)
+		return !di->flags.vbus_drop_end;
+	else
+		return true;
 }
 
 /**
@@ -1118,7 +1114,7 @@ static int ab8500_charger_set_current(struct ab8500_charger *di,
 	int ich, int reg)
 {
 	int ret = 0;
-	int auto_curr_index, curr_index, prev_curr_index, shift_value, i;
+	int curr_index, prev_curr_index, shift_value, i;
 	u8 reg_value;
 	u32 step_udelay;
 	bool no_stepping = false;
@@ -1136,39 +1132,27 @@ static int ab8500_charger_set_current(struct ab8500_charger *di,
 	case AB8500_MCH_IPT_CURLVL_REG:
 		shift_value = MAIN_CH_INPUT_CURR_SHIFT;
 		prev_curr_index = (reg_value >> shift_value);
-		curr_index = ab8500_current_to_regval(ich);
+		curr_index = ab8500_current_to_regval(di, ich);
 		step_udelay = STEP_UDELAY;
 		if (!di->ac.charger_connected)
 			no_stepping = true;
 		break;
 	case AB8500_USBCH_IPT_CRNTLVL_REG:
-		shift_value = VBUS_IN_CURR_LIM_SHIFT;
+		if (is_ab8540(di->parent))
+			shift_value = AB8540_VBUS_IN_CURR_LIM_SHIFT;
+		else
+			shift_value = VBUS_IN_CURR_LIM_SHIFT;
 		prev_curr_index = (reg_value >> shift_value);
-		curr_index = ab8500_vbus_in_curr_to_regval(ich);
+		curr_index = ab8500_vbus_in_curr_to_regval(di, ich);
 		step_udelay = STEP_UDELAY * 100;
 
-		ret = abx500_get_register_interruptible(di->dev, AB8500_CHARGER,
-					AB8500_CH_USBCH_STAT2_REG, &reg_value);
-		if (ret < 0) {
-			dev_err(di->dev, "%s read failed\n", __func__);
-			goto exit_set_current;
-		}
-		auto_curr_index =
-			reg_value >> AUTO_VBUS_IN_CURR_LIM_SHIFT;
-
-		dev_dbg(di->dev, "%s Auto VBUS curr is %d mA\n",
-			__func__,
-			ab8500_charger_vbus_in_curr_map[auto_curr_index]);
-
-		prev_curr_index = min(prev_curr_index, auto_curr_index);
-
 		if (!di->usb.charger_connected)
 			no_stepping = true;
 		break;
 	case AB8500_CH_OPT_CRNTLVL_REG:
 		shift_value = 0;
 		prev_curr_index = (reg_value >> shift_value);
-		curr_index = ab8500_current_to_regval(ich);
+		curr_index = ab8500_current_to_regval(di, ich);
 		step_udelay = STEP_UDELAY;
 		if (curr_index && (curr_index - prev_curr_index) > 1)
 			step_udelay *= 100;
@@ -1219,7 +1203,8 @@ static int ab8500_charger_set_current(struct ab8500_charger *di,
 				usleep_range(step_udelay, step_udelay * 2);
 		}
 	} else {
-		for (i = prev_curr_index + 1; i <= curr_index; i++) {
+		bool allow = true;
+		for (i = prev_curr_index + 1; i <= curr_index && allow; i++) {
 			dev_dbg(di->dev, "curr change_2 to: %x for 0x%02x\n",
 				(u8)i << shift_value, reg);
 			ret = abx500_set_register_interruptible(di->dev,
@@ -1230,6 +1215,8 @@ static int ab8500_charger_set_current(struct ab8500_charger *di,
 			}
 			if (i != curr_index)
 				usleep_range(step_udelay, step_udelay * 2);
+
+			allow = ab8500_charger_check_continue_stepping(di, reg);
 		}
 	}
 
@@ -1255,6 +1242,11 @@ static int ab8500_charger_set_vbus_in_curr(struct ab8500_charger *di,
 
 	/* We should always use to lowest current limit */
 	min_value = min(di->bm->chg_params->usb_curr_max, ich_in);
+	if (di->max_usb_in_curr.set_max > 0)
+		min_value = min(di->max_usb_in_curr.set_max, min_value);
+
+	if (di->usb_state.usb_current >= 0)
+		min_value = min(di->usb_state.usb_current, min_value);
 
 	switch (min_value) {
 	case 100:
@@ -1400,8 +1392,8 @@ static int ab8500_charger_ac_en(struct ux500_charger *charger,
 
 		/* Check if the requested voltage or current is valid */
 		volt_index = ab8500_voltage_to_regval(vset);
-		curr_index = ab8500_current_to_regval(iset);
-		input_curr_index = ab8500_current_to_regval(
+		curr_index = ab8500_current_to_regval(di, iset);
+		input_curr_index = ab8500_current_to_regval(di,
 			di->bm->chg_params->ac_curr_max);
 		if (volt_index < 0 || curr_index < 0 || input_curr_index < 0) {
 			dev_err(di->dev,
@@ -1572,7 +1564,7 @@ static int ab8500_charger_usb_en(struct ux500_charger *charger,
 
 		/* Check if the requested voltage or current is valid */
 		volt_index = ab8500_voltage_to_regval(vset);
-		curr_index = ab8500_current_to_regval(ich_out);
+		curr_index = ab8500_current_to_regval(di, ich_out);
 		if (volt_index < 0 || curr_index < 0) {
 			dev_err(di->dev,
 				"Charger voltage or current too high, "
@@ -1609,7 +1601,8 @@ static int ab8500_charger_usb_en(struct ux500_charger *charger,
 		di->usb.charger_online = 1;
 
 		/* USBChInputCurr: current that can be drawn from the usb */
-		ret = ab8500_charger_set_vbus_in_curr(di, di->max_usb_in_curr);
+		ret = ab8500_charger_set_vbus_in_curr(di,
+					di->max_usb_in_curr.usb_type_max);
 		if (ret) {
 			dev_err(di->dev, "setting USBChInputCurr failed\n");
 			return ret;
@@ -1668,8 +1661,7 @@ static int ab8500_charger_usb_en(struct ux500_charger *charger,
 		dev_dbg(di->dev, "%s Disabled USB charging\n", __func__);
 
 		/* Cancel any pending Vbat check work */
-		if (delayed_work_pending(&di->check_vbat_work))
-			cancel_delayed_work(&di->check_vbat_work);
+		cancel_delayed_work(&di->check_vbat_work);
 
 	}
 	ab8500_power_supply_changed(di, &di->usb_chg.psy);
@@ -1677,6 +1669,128 @@ static int ab8500_charger_usb_en(struct ux500_charger *charger,
 	return ret;
 }
 
+static int ab8500_external_charger_prepare(struct notifier_block *charger_nb,
+				unsigned long event, void *data)
+{
+	int ret;
+	struct device *dev = data;
+	/*Toggle External charger control pin*/
+	ret = abx500_set_register_interruptible(dev, AB8500_SYS_CTRL1_BLOCK,
+				  AB8500_SYS_CHARGER_CONTROL_REG,
+				  EXTERNAL_CHARGER_DISABLE_REG_VAL);
+	if (ret < 0) {
+		dev_err(dev, "write reg failed %d\n", ret);
+		goto out;
+	}
+	ret = abx500_set_register_interruptible(dev, AB8500_SYS_CTRL1_BLOCK,
+				  AB8500_SYS_CHARGER_CONTROL_REG,
+				  EXTERNAL_CHARGER_ENABLE_REG_VAL);
+	if (ret < 0)
+		dev_err(dev, "Write reg failed %d\n", ret);
+
+out:
+	return ret;
+}
+
+/**
+ * ab8500_charger_usb_check_enable() - enable usb charging
+ * @charger:	pointer to the ux500_charger structure
+ * @vset:	charging voltage
+ * @iset:	charger output current
+ *
+ * Check if the VBUS charger has been disconnected and reconnected without
+ * AB8500 rising an interrupt. Returns 0 on success.
+ */
+static int ab8500_charger_usb_check_enable(struct ux500_charger *charger,
+	int vset, int iset)
+{
+	u8 usbch_ctrl1 = 0;
+	int ret = 0;
+
+	struct ab8500_charger *di = to_ab8500_charger_usb_device_info(charger);
+
+	if (!di->usb.charger_connected)
+		return ret;
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_CHARGER,
+				AB8500_USBCH_CTRL1_REG, &usbch_ctrl1);
+	if (ret < 0) {
+		dev_err(di->dev, "ab8500 read failed %d\n", __LINE__);
+		return ret;
+	}
+	dev_dbg(di->dev, "USB charger ctrl: 0x%02x\n", usbch_ctrl1);
+
+	if (!(usbch_ctrl1 & USB_CH_ENA)) {
+		dev_info(di->dev, "Charging has been disabled abnormally and will be re-enabled\n");
+
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+					AB8500_CHARGER, AB8500_CHARGER_CTRL,
+					DROP_COUNT_RESET, DROP_COUNT_RESET);
+		if (ret < 0) {
+			dev_err(di->dev, "ab8500 write failed %d\n", __LINE__);
+			return ret;
+		}
+
+		ret = ab8500_charger_usb_en(&di->usb_chg, true, vset, iset);
+		if (ret < 0) {
+			dev_err(di->dev, "Failed to enable VBUS charger %d\n",
+					__LINE__);
+			return ret;
+		}
+	}
+	return ret;
+}
+
+/**
+ * ab8500_charger_ac_check_enable() - enable usb charging
+ * @charger:	pointer to the ux500_charger structure
+ * @vset:	charging voltage
+ * @iset:	charger output current
+ *
+ * Check if the AC charger has been disconnected and reconnected without
+ * AB8500 rising an interrupt. Returns 0 on success.
+ */
+static int ab8500_charger_ac_check_enable(struct ux500_charger *charger,
+	int vset, int iset)
+{
+	u8 mainch_ctrl1 = 0;
+	int ret = 0;
+
+	struct ab8500_charger *di = to_ab8500_charger_ac_device_info(charger);
+
+	if (!di->ac.charger_connected)
+		return ret;
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_CHARGER,
+				AB8500_MCH_CTRL1, &mainch_ctrl1);
+	if (ret < 0) {
+		dev_err(di->dev, "ab8500 read failed %d\n", __LINE__);
+		return ret;
+	}
+	dev_dbg(di->dev, "AC charger ctrl: 0x%02x\n", mainch_ctrl1);
+
+	if (!(mainch_ctrl1 & MAIN_CH_ENA)) {
+		dev_info(di->dev, "Charging has been disabled abnormally and will be re-enabled\n");
+
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+					AB8500_CHARGER, AB8500_CHARGER_CTRL,
+					DROP_COUNT_RESET, DROP_COUNT_RESET);
+
+		if (ret < 0) {
+			dev_err(di->dev, "ab8500 write failed %d\n", __LINE__);
+			return ret;
+		}
+
+		ret = ab8500_charger_ac_en(&di->usb_chg, true, vset, iset);
+		if (ret < 0) {
+			dev_err(di->dev, "failed to enable AC charger %d\n",
+				__LINE__);
+			return ret;
+		}
+	}
+	return ret;
+}
+
 /**
  * ab8500_charger_watchdog_kick() - kick charger watchdog
  * @di:		pointer to the ab8500_charger structure
@@ -1734,8 +1848,68 @@ static int ab8500_charger_update_charger_current(struct ux500_charger *charger,
 
 	/* Reset the main and usb drop input current measurement counter */
 	ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
-				AB8500_CHARGER_CTRL,
-				0x1);
+				AB8500_CHARGER_CTRL, DROP_COUNT_RESET);
+	if (ret) {
+		dev_err(di->dev, "%s write failed\n", __func__);
+		return ret;
+	}
+
+	return ret;
+}
+
+/**
+ * ab8540_charger_power_path_enable() - enable usb power path mode
+ * @charger:	pointer to the ux500_charger structure
+ * @enable:	enable/disable flag
+ *
+ * Enable or disable the power path for usb mode
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab8540_charger_power_path_enable(struct ux500_charger *charger,
+		bool enable)
+{
+	int ret;
+	struct ab8500_charger *di;
+
+	if (charger->psy.type == POWER_SUPPLY_TYPE_USB)
+		di = to_ab8500_charger_usb_device_info(charger);
+	else
+		return -ENXIO;
+
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+				AB8500_CHARGER, AB8540_USB_PP_MODE_REG,
+				BUS_POWER_PATH_MODE_ENA, enable);
+	if (ret) {
+		dev_err(di->dev, "%s write failed\n", __func__);
+		return ret;
+	}
+
+	return ret;
+}
+
+
+/**
+ * ab8540_charger_usb_pre_chg_enable() - enable usb pre change
+ * @charger:	pointer to the ux500_charger structure
+ * @enable:	enable/disable flag
+ *
+ * Enable or disable the pre-chage for usb mode
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab8540_charger_usb_pre_chg_enable(struct ux500_charger *charger,
+		bool enable)
+{
+	int ret;
+	struct ab8500_charger *di;
+
+	if (charger->psy.type == POWER_SUPPLY_TYPE_USB)
+		di = to_ab8500_charger_usb_device_info(charger);
+	else
+		return -ENXIO;
+
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+				AB8500_CHARGER, AB8540_USB_PP_CHR_REG,
+				BUS_POWER_PATH_PRECHG_ENA, enable);
 	if (ret) {
 		dev_err(di->dev, "%s write failed\n", __func__);
 		return ret;
@@ -1823,9 +1997,10 @@ static void ab8500_charger_check_vbat_work(struct work_struct *work)
 		di->vbat > VBAT_TRESH_IP_CUR_RED))) {
 
 		dev_dbg(di->dev, "Vbat did cross threshold, curr: %d, new: %d,"
-			" old: %d\n", di->max_usb_in_curr, di->vbat,
-			di->old_vbat);
-		ab8500_charger_set_vbus_in_curr(di, di->max_usb_in_curr);
+			" old: %d\n", di->max_usb_in_curr.usb_type_max,
+			di->vbat, di->old_vbat);
+		ab8500_charger_set_vbus_in_curr(di,
+					di->max_usb_in_curr.usb_type_max);
 		power_supply_changed(&di->usb_chg.psy);
 	}
 
@@ -2105,7 +2280,8 @@ static void ab8500_charger_usb_link_attach_work(struct work_struct *work)
 
 	/* Update maximum input current if USB enumeration is not detected */
 	if (!di->usb.charger_online) {
-		ret = ab8500_charger_set_vbus_in_curr(di, di->max_usb_in_curr);
+		ret = ab8500_charger_set_vbus_in_curr(di,
+					di->max_usb_in_curr.usb_type_max);
 		if (ret)
 			return;
 	}
@@ -2125,6 +2301,7 @@ static void ab8500_charger_usb_link_status_work(struct work_struct *work)
 	int detected_chargers;
 	int ret;
 	u8 val;
+	u8 link_status;
 
 	struct ab8500_charger *di = container_of(work,
 		struct ab8500_charger, usb_link_status_work);
@@ -2144,38 +2321,61 @@ static void ab8500_charger_usb_link_status_work(struct work_struct *work)
 	 * to start the charging process. but by jumping
 	 * thru a few hoops it can be forced to start.
 	 */
-	ret = abx500_get_register_interruptible(di->dev, AB8500_USB,
-			AB8500_USB_LINE_STAT_REG, &val);
+	if (is_ab8500(di->parent))
+		ret = abx500_get_register_interruptible(di->dev, AB8500_USB,
+					AB8500_USB_LINE_STAT_REG, &val);
+	else
+		ret = abx500_get_register_interruptible(di->dev, AB8500_USB,
+					AB8500_USB_LINK1_STAT_REG, &val);
+
 	if (ret >= 0)
 		dev_dbg(di->dev, "UsbLineStatus register = 0x%02x\n", val);
 	else
 		dev_dbg(di->dev, "Error reading USB link status\n");
 
+	if (is_ab8500(di->parent))
+		link_status = AB8500_USB_LINK_STATUS;
+	else
+		link_status = AB8505_USB_LINK_STATUS;
+
 	if (detected_chargers & USB_PW_CONN) {
-		if (((val & AB8500_USB_LINK_STATUS) >> 3) == USB_STAT_NOT_VALID_LINK &&
+		if (((val & link_status) >> USB_LINK_STATUS_SHIFT) ==
+				USB_STAT_NOT_VALID_LINK &&
 				di->invalid_charger_detect_state == 0) {
-			dev_dbg(di->dev, "Invalid charger detected, state= 0\n");
+			dev_dbg(di->dev,
+					"Invalid charger detected, state= 0\n");
 			/*Enable charger*/
 			abx500_mask_and_set_register_interruptible(di->dev,
-					AB8500_CHARGER, AB8500_USBCH_CTRL1_REG, 0x01, 0x01);
+					AB8500_CHARGER, AB8500_USBCH_CTRL1_REG,
+					USB_CH_ENA, USB_CH_ENA);
 			/*Enable charger detection*/
-			abx500_mask_and_set_register_interruptible(di->dev, AB8500_USB,
-					AB8500_MCH_IPT_CURLVL_REG, 0x01, 0x01);
+			abx500_mask_and_set_register_interruptible(di->dev,
+					AB8500_USB, AB8500_USB_LINE_CTRL2_REG,
+					USB_CH_DET, USB_CH_DET);
 			di->invalid_charger_detect_state = 1;
 			/*exit and wait for new link status interrupt.*/
 			return;
 
 		}
 		if (di->invalid_charger_detect_state == 1) {
-			dev_dbg(di->dev, "Invalid charger detected, state= 1\n");
+			dev_dbg(di->dev,
+					"Invalid charger detected, state= 1\n");
 			/*Stop charger detection*/
-			abx500_mask_and_set_register_interruptible(di->dev, AB8500_USB,
-					AB8500_MCH_IPT_CURLVL_REG, 0x01, 0x00);
+			abx500_mask_and_set_register_interruptible(di->dev,
+					AB8500_USB, AB8500_USB_LINE_CTRL2_REG,
+					USB_CH_DET, 0x00);
 			/*Check link status*/
-			ret = abx500_get_register_interruptible(di->dev, AB8500_USB,
-					AB8500_USB_LINE_STAT_REG, &val);
+			if (is_ab8500(di->parent))
+				ret = abx500_get_register_interruptible(di->dev,
+					AB8500_USB, AB8500_USB_LINE_STAT_REG,
+					&val);
+			else
+				ret = abx500_get_register_interruptible(di->dev,
+					AB8500_USB, AB8500_USB_LINK1_STAT_REG,
+					&val);
+
 			dev_dbg(di->dev, "USB link status= 0x%02x\n",
-					(val & AB8500_USB_LINK_STATUS) >> 3);
+				(val & link_status) >> USB_LINK_STATUS_SHIFT);
 			di->invalid_charger_detect_state = 2;
 		}
 	} else {
@@ -2273,7 +2473,7 @@ static void ab8500_charger_usb_state_changed_work(struct work_struct *work)
 		if (!ab8500_charger_get_usb_cur(di)) {
 			/* Update maximum input current */
 			ret = ab8500_charger_set_vbus_in_curr(di,
-					di->max_usb_in_curr);
+					di->max_usb_in_curr.usb_type_max);
 			if (ret)
 				return;
 
@@ -2422,7 +2622,9 @@ static irqreturn_t ab8500_charger_mainchplugdet_handler(int irq, void *_di)
 
 	mutex_lock(&di->charger_attached_mutex);
 	mutex_unlock(&di->charger_attached_mutex);
-	queue_delayed_work(di->charger_wq,
+
+	if (is_ab8500(di->parent))
+		queue_delayed_work(di->charger_wq,
 			   &di->ac_charger_attached_work,
 			   HZ);
 	return IRQ_HANDLED;
@@ -2491,6 +2693,8 @@ static void ab8500_charger_vbus_drop_end_work(struct work_struct *work)
 {
 	struct ab8500_charger *di = container_of(work,
 		struct ab8500_charger, vbus_drop_end_work.work);
+	int ret, curr;
+	u8 reg_value;
 
 	di->flags.vbus_drop_end = false;
 
@@ -2498,8 +2702,45 @@ static void ab8500_charger_vbus_drop_end_work(struct work_struct *work)
 	abx500_set_register_interruptible(di->dev,
 				  AB8500_CHARGER, AB8500_CHARGER_CTRL, 0x01);
 
+	if (is_ab8540(di->parent))
+		ret = abx500_get_register_interruptible(di->dev, AB8500_CHARGER,
+				AB8540_CH_USBCH_STAT3_REG, &reg_value);
+	else
+		ret = abx500_get_register_interruptible(di->dev, AB8500_CHARGER,
+				AB8500_CH_USBCH_STAT2_REG, &reg_value);
+	if (ret < 0) {
+		dev_err(di->dev, "%s read failed\n", __func__);
+		return;
+	}
+
+	if (is_ab8540(di->parent))
+		curr = di->bm->chg_input_curr[
+			reg_value & AB8540_AUTO_VBUS_IN_CURR_MASK];
+	else
+		curr = di->bm->chg_input_curr[
+			reg_value >> AUTO_VBUS_IN_CURR_LIM_SHIFT];
+
+	if (di->max_usb_in_curr.calculated_max != curr) {
+		/* USB source is collapsing */
+		di->max_usb_in_curr.calculated_max = curr;
+		dev_dbg(di->dev,
+			 "VBUS input current limiting to %d mA\n",
+			 di->max_usb_in_curr.calculated_max);
+	} else {
+		/*
+		 * USB source can not give more than this amount.
+		 * Taking more will collapse the source.
+		 */
+		di->max_usb_in_curr.set_max =
+			di->max_usb_in_curr.calculated_max;
+		dev_dbg(di->dev,
+			 "VBUS input current limited to %d mA\n",
+			 di->max_usb_in_curr.set_max);
+	}
+
 	if (di->usb.charger_connected)
-		ab8500_charger_set_vbus_in_curr(di, di->max_usb_in_curr);
+		ab8500_charger_set_vbus_in_curr(di,
+					di->max_usb_in_curr.usb_type_max);
 }
 
 /**
@@ -2654,8 +2895,13 @@ static irqreturn_t ab8500_charger_vbuschdropend_handler(int irq, void *_di)
 
 	dev_dbg(di->dev, "VBUS charger drop ended\n");
 	di->flags.vbus_drop_end = true;
+
+	/*
+	 * VBUS might have dropped due to bad connection.
+	 * Schedule a new input limit set to the value SW requests.
+	 */
 	queue_delayed_work(di->charger_wq, &di->vbus_drop_end_work,
-			   round_jiffies(30 * HZ));
+			   round_jiffies(VBUS_IN_CURR_LIM_RETRY_SET_TIME * HZ));
 
 	return IRQ_HANDLED;
 }
@@ -2836,6 +3082,7 @@ static int ab8500_charger_usb_get_property(struct power_supply *psy,
 static int ab8500_charger_init_hw_registers(struct ab8500_charger *di)
 {
 	int ret = 0;
+	u8 bup_vch_range = 0, vbup33_vrtcn = 0;
 
 	/* Setup maximum charger current and voltage for ABB cut2.0 */
 	if (!is_ab8500_1p1_or_earlier(di->parent)) {
@@ -2848,9 +3095,14 @@ static int ab8500_charger_init_hw_registers(struct ab8500_charger *di)
 			goto out;
 		}
 
-		ret = abx500_set_register_interruptible(di->dev,
-			AB8500_CHARGER,
-			AB8500_CH_OPT_CRNTLVL_MAX_REG, CH_OP_CUR_LVL_1P6);
+		if (is_ab8540(di->parent))
+			ret = abx500_set_register_interruptible(di->dev,
+				AB8500_CHARGER, AB8500_CH_OPT_CRNTLVL_MAX_REG,
+				CH_OP_CUR_LVL_2P);
+		else
+			ret = abx500_set_register_interruptible(di->dev,
+				AB8500_CHARGER, AB8500_CH_OPT_CRNTLVL_MAX_REG,
+				CH_OP_CUR_LVL_1P6);
 		if (ret) {
 			dev_err(di->dev,
 				"failed to set CH_OPT_CRNTLVL_MAX_REG\n");
@@ -2858,7 +3110,8 @@ static int ab8500_charger_init_hw_registers(struct ab8500_charger *di)
 		}
 	}
 
-	if (is_ab9540_2p0(di->parent) || is_ab8505_2p0(di->parent))
+	if (is_ab9540_2p0(di->parent) || is_ab9540_3p0(di->parent)
+	 || is_ab8505_2p0(di->parent) || is_ab8540(di->parent))
 		ret = abx500_mask_and_set_register_interruptible(di->dev,
 			AB8500_CHARGER,
 			AB8500_USBCH_CTRL2_REG,
@@ -2930,14 +3183,6 @@ static int ab8500_charger_init_hw_registers(struct ab8500_charger *di)
 		goto out;
 	}
 
-	/* Set charger watchdog timeout */
-	ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
-		AB8500_CH_WD_TIMER_REG, WD_TIMER);
-	if (ret) {
-		dev_err(di->dev, "failed to set charger watchdog timeout\n");
-		goto out;
-	}
-
 	ret = ab8500_charger_led_en(di, false);
 	if (ret < 0) {
 		dev_err(di->dev, "failed to disable LED\n");
@@ -2945,15 +3190,30 @@ static int ab8500_charger_init_hw_registers(struct ab8500_charger *di)
 	}
 
 	/* Backup battery voltage and current */
+	if (di->bm->bkup_bat_v > BUP_VCH_SEL_3P1V)
+		bup_vch_range = BUP_VCH_RANGE;
+	if (di->bm->bkup_bat_v == BUP_VCH_SEL_3P3V)
+		vbup33_vrtcn = VBUP33_VRTCN;
+
 	ret = abx500_set_register_interruptible(di->dev,
 		AB8500_RTC,
 		AB8500_RTC_BACKUP_CHG_REG,
-		di->bm->bkup_bat_v |
-		di->bm->bkup_bat_i);
+		(di->bm->bkup_bat_v & 0x3) | di->bm->bkup_bat_i);
 	if (ret) {
 		dev_err(di->dev, "failed to setup backup battery charging\n");
 		goto out;
 	}
+	if (is_ab8540(di->parent)) {
+		ret = abx500_set_register_interruptible(di->dev,
+			AB8500_RTC,
+			AB8500_RTC_CTRL1_REG,
+			bup_vch_range | vbup33_vrtcn);
+		if (ret) {
+			dev_err(di->dev,
+				"failed to setup backup battery charging\n");
+			goto out;
+		}
+	}
 
 	/* Enable backup battery charging */
 	abx500_mask_and_set_register_interruptible(di->dev,
@@ -2962,6 +3222,25 @@ static int ab8500_charger_init_hw_registers(struct ab8500_charger *di)
 	if (ret < 0)
 		dev_err(di->dev, "%s mask and set failed\n", __func__);
 
+	if (is_ab8540(di->parent)) {
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB8500_CHARGER, AB8540_USB_PP_MODE_REG,
+			BUS_VSYS_VOL_SELECT_MASK, BUS_VSYS_VOL_SELECT_3P6V);
+		if (ret) {
+			dev_err(di->dev,
+				"failed to setup usb power path vsys voltage\n");
+			goto out;
+		}
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB8500_CHARGER, AB8540_USB_PP_CHR_REG,
+			BUS_PP_PRECHG_CURRENT_MASK, 0);
+		if (ret) {
+			dev_err(di->dev,
+				"failed to setup usb power path prechage current\n");
+			goto out;
+		}
+	}
+
 out:
 	return ret;
 }
@@ -3055,11 +3334,8 @@ static int ab8500_charger_resume(struct platform_device *pdev)
 			dev_err(di->dev, "Failed to kick WD!\n");
 
 		/* If not already pending start a new timer */
-		if (!delayed_work_pending(
-			&di->kick_wd_work)) {
-			queue_delayed_work(di->charger_wq, &di->kick_wd_work,
-				round_jiffies(WD_KICK_INTERVAL));
-		}
+		queue_delayed_work(di->charger_wq, &di->kick_wd_work,
+				   round_jiffies(WD_KICK_INTERVAL));
 	}
 
 	/* If we still have a HW failure, schedule a new check */
@@ -3079,12 +3355,9 @@ static int ab8500_charger_suspend(struct platform_device *pdev,
 {
 	struct ab8500_charger *di = platform_get_drvdata(pdev);
 
-	/* Cancel any pending HW failure check */
-	if (delayed_work_pending(&di->check_hw_failure_work))
-		cancel_delayed_work(&di->check_hw_failure_work);
-
-	if (delayed_work_pending(&di->vbus_drop_end_work))
-		cancel_delayed_work(&di->vbus_drop_end_work);
+	/* Cancel any pending jobs */
+	cancel_delayed_work(&di->check_hw_failure_work);
+	cancel_delayed_work(&di->vbus_drop_end_work);
 
 	flush_delayed_work(&di->attach_work);
 	flush_delayed_work(&di->usb_charger_attached_work);
@@ -3107,6 +3380,10 @@ static int ab8500_charger_suspend(struct platform_device *pdev,
 #define ab8500_charger_resume       NULL
 #endif
 
+static struct notifier_block charger_nb = {
+	.notifier_call = ab8500_external_charger_prepare,
+};
+
 static int ab8500_charger_remove(struct platform_device *pdev)
 {
 	struct ab8500_charger *di = platform_get_drvdata(pdev);
@@ -3136,13 +3413,18 @@ static int ab8500_charger_remove(struct platform_device *pdev)
 	/* Delete the work queue */
 	destroy_workqueue(di->charger_wq);
 
+	/* Unregister external charger enable notifier */
+	if (!di->ac_chg.enabled)
+		blocking_notifier_chain_unregister(
+			&charger_notifier_list, &charger_nb);
+
 	flush_scheduled_work();
-	if(di->usb_chg.enabled)
+	if (di->usb_chg.enabled)
 		power_supply_unregister(&di->usb_chg.psy);
-#if !defined(CONFIG_CHARGER_PM2301)
-	if(di->ac_chg.enabled)
+
+	if (di->ac_chg.enabled && !di->ac_chg.external)
 		power_supply_unregister(&di->ac_chg.psy);
-#endif
+
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
@@ -3206,16 +3488,22 @@ static int ab8500_charger_probe(struct platform_device *pdev)
 	di->ac_chg.psy.num_supplicants = ARRAY_SIZE(supply_interface),
 	/* ux500_charger sub-class */
 	di->ac_chg.ops.enable = &ab8500_charger_ac_en;
+	di->ac_chg.ops.check_enable = &ab8500_charger_ac_check_enable;
 	di->ac_chg.ops.kick_wd = &ab8500_charger_watchdog_kick;
 	di->ac_chg.ops.update_curr = &ab8500_charger_update_charger_current;
 	di->ac_chg.max_out_volt = ab8500_charger_voltage_map[
 		ARRAY_SIZE(ab8500_charger_voltage_map) - 1];
-	di->ac_chg.max_out_curr = ab8500_charger_current_map[
-		ARRAY_SIZE(ab8500_charger_current_map) - 1];
+	di->ac_chg.max_out_curr =
+		di->bm->chg_output_curr[di->bm->n_chg_out_curr - 1];
 	di->ac_chg.wdt_refresh = CHG_WD_INTERVAL;
 	di->ac_chg.enabled = di->bm->ac_enabled;
 	di->ac_chg.external = false;
 
+	/*notifier for external charger enabling*/
+	if (!di->ac_chg.enabled)
+		blocking_notifier_chain_register(
+			&charger_notifier_list, &charger_nb);
+
 	/* USB supply */
 	/* power_supply base class */
 	di->usb_chg.psy.name = "ab8500_usb";
@@ -3227,15 +3515,20 @@ static int ab8500_charger_probe(struct platform_device *pdev)
 	di->usb_chg.psy.num_supplicants = ARRAY_SIZE(supply_interface),
 	/* ux500_charger sub-class */
 	di->usb_chg.ops.enable = &ab8500_charger_usb_en;
+	di->usb_chg.ops.check_enable = &ab8500_charger_usb_check_enable;
 	di->usb_chg.ops.kick_wd = &ab8500_charger_watchdog_kick;
 	di->usb_chg.ops.update_curr = &ab8500_charger_update_charger_current;
+	di->usb_chg.ops.pp_enable = &ab8540_charger_power_path_enable;
+	di->usb_chg.ops.pre_chg_enable = &ab8540_charger_usb_pre_chg_enable;
 	di->usb_chg.max_out_volt = ab8500_charger_voltage_map[
 		ARRAY_SIZE(ab8500_charger_voltage_map) - 1];
-	di->usb_chg.max_out_curr = ab8500_charger_current_map[
-		ARRAY_SIZE(ab8500_charger_current_map) - 1];
+	di->usb_chg.max_out_curr =
+		di->bm->chg_output_curr[di->bm->n_chg_out_curr - 1];
 	di->usb_chg.wdt_refresh = CHG_WD_INTERVAL;
 	di->usb_chg.enabled = di->bm->usb_enabled;
 	di->usb_chg.external = false;
+	di->usb_chg.power_path = di->bm->usb_power_path;
+	di->usb_state.usb_current = -1;
 
 	/* Create a work queue for the charger */
 	di->charger_wq =
@@ -3316,7 +3609,7 @@ static int ab8500_charger_probe(struct platform_device *pdev)
 	}
 
 	/* Register AC charger class */
-	if(di->ac_chg.enabled) {
+	if (di->ac_chg.enabled) {
 		ret = power_supply_register(di->dev, &di->ac_chg.psy);
 		if (ret) {
 			dev_err(di->dev, "failed to register AC charger\n");
@@ -3325,7 +3618,7 @@ static int ab8500_charger_probe(struct platform_device *pdev)
 	}
 
 	/* Register USB charger class */
-	if(di->usb_chg.enabled) {
+	if (di->usb_chg.enabled) {
 		ret = power_supply_register(di->dev, &di->usb_chg.psy);
 		if (ret) {
 			dev_err(di->dev, "failed to register USB charger\n");
@@ -3385,14 +3678,16 @@ static int ab8500_charger_probe(struct platform_device *pdev)
 	ch_stat = ab8500_charger_detect_chargers(di, false);
 
 	if ((ch_stat & AC_PW_CONN) == AC_PW_CONN) {
-		queue_delayed_work(di->charger_wq,
-				   &di->ac_charger_attached_work,
-				   HZ);
+		if (is_ab8500(di->parent))
+			queue_delayed_work(di->charger_wq,
+					   &di->ac_charger_attached_work,
+					   HZ);
 	}
 	if ((ch_stat & USB_PW_CONN) == USB_PW_CONN) {
-		queue_delayed_work(di->charger_wq,
-				   &di->usb_charger_attached_work,
-				   HZ);
+		if (is_ab8500(di->parent))
+			queue_delayed_work(di->charger_wq,
+					   &di->usb_charger_attached_work,
+					   HZ);
 	}
 
 	mutex_unlock(&di->charger_attached_mutex);
@@ -3410,10 +3705,10 @@ free_irq:
 put_usb_phy:
 	usb_put_phy(di->usb_phy);
 free_usb:
-	if(di->usb_chg.enabled)
+	if (di->usb_chg.enabled)
 		power_supply_unregister(&di->usb_chg.psy);
 free_ac:
-	if(di->ac_chg.enabled)
+	if (di->ac_chg.enabled)
 		power_supply_unregister(&di->ac_chg.psy);
 free_charger_wq:
 	destroy_workqueue(di->charger_wq);
diff --git a/drivers/power/ab8500_fg.c b/drivers/power/ab8500_fg.c
index 25dae4c4b0ef..c5391f5c372d 100644
--- a/drivers/power/ab8500_fg.c
+++ b/drivers/power/ab8500_fg.c
@@ -36,7 +36,7 @@
 
 #define MILLI_TO_MICRO			1000
 #define FG_LSB_IN_MA			1627
-#define QLSB_NANO_AMP_HOURS_X10		1129
+#define QLSB_NANO_AMP_HOURS_X10		1071
 #define INS_CURR_TIMEOUT		(3 * HZ)
 
 #define SEC_TO_SAMPLE(S)		(S * 4)
@@ -672,11 +672,11 @@ int ab8500_fg_inst_curr_finalize(struct ab8500_fg *di, int *res)
 	/*
 	 * Convert to unit value in mA
 	 * Full scale input voltage is
-	 * 66.660mV => LSB = 66.660mV/(4096*res) = 1.627mA
+	 * 63.160mV => LSB = 63.160mV/(4096*res) = 1.542mA
 	 * Given a 250ms conversion cycle time the LSB corresponds
-	 * to 112.9 nAh. Convert to current by dividing by the conversion
+	 * to 107.1 nAh. Convert to current by dividing by the conversion
 	 * time in hours (250ms = 1 / (3600 * 4)h)
-	 * 112.9nAh assumes 10mOhm, but fg_res is in 0.1mOhm
+	 * 107.1nAh assumes 10mOhm, but fg_res is in 0.1mOhm
 	 */
 	val = (val * QLSB_NANO_AMP_HOURS_X10 * 36 * 4) /
 		(1000 * di->bm->fg_res);
@@ -863,7 +863,7 @@ static int ab8500_fg_bat_voltage(struct ab8500_fg *di)
 static int ab8500_fg_volt_to_capacity(struct ab8500_fg *di, int voltage)
 {
 	int i, tbl_size;
-	struct abx500_v_to_cap *tbl;
+	const struct abx500_v_to_cap *tbl;
 	int cap = 0;
 
 	tbl = di->bm->bat_type[di->bm->batt_id].v_to_cap_tbl,
@@ -915,7 +915,7 @@ static int ab8500_fg_uncomp_volt_to_capacity(struct ab8500_fg *di)
 static int ab8500_fg_battery_resistance(struct ab8500_fg *di)
 {
 	int i, tbl_size;
-	struct batres_vs_temp *tbl;
+	const struct batres_vs_temp *tbl;
 	int resist = 0;
 
 	tbl = di->bm->bat_type[di->bm->batt_id].batres_tbl;
@@ -1354,9 +1354,6 @@ static void ab8500_fg_check_capacity_limits(struct ab8500_fg *di, bool init)
 			 * algorithm says.
 			 */
 			di->bat_cap.prev_percent = 1;
-			di->bat_cap.permille = 1;
-			di->bat_cap.prev_mah = 1;
-			di->bat_cap.mah = 1;
 			percent = 1;
 
 			changed = true;
@@ -1683,7 +1680,6 @@ static void ab8500_fg_algorithm_discharging(struct ab8500_fg *di)
 		break;
 
 	case AB8500_FG_DISCHARGE_WAKEUP:
-		ab8500_fg_coulomb_counter(di, true);
 		ab8500_fg_calc_cap_discharge_voltage(di, true);
 
 		di->fg_samples = SEC_TO_SAMPLE(
@@ -1768,9 +1764,10 @@ static void ab8500_fg_algorithm(struct ab8500_fg *di)
 			ab8500_fg_algorithm_discharging(di);
 	}
 
-	dev_dbg(di->dev, "[FG_DATA] %d %d %d %d %d %d %d %d %d "
+	dev_dbg(di->dev, "[FG_DATA] %d %d %d %d %d %d %d %d %d %d "
 		"%d %d %d %d %d %d %d\n",
 		di->bat_cap.max_mah_design,
+		di->bat_cap.max_mah,
 		di->bat_cap.mah,
 		di->bat_cap.permille,
 		di->bat_cap.level,
@@ -1982,7 +1979,7 @@ static void ab8500_fg_instant_work(struct work_struct *work)
 }
 
 /**
- * ab8500_fg_cc_data_end_handler() - isr to get battery avg current.
+ * ab8500_fg_cc_data_end_handler() - end of data conversion isr.
  * @irq:       interrupt number
  * @_di:       pointer to the ab8500_fg structure
  *
@@ -2002,7 +1999,7 @@ static irqreturn_t ab8500_fg_cc_data_end_handler(int irq, void *_di)
 }
 
 /**
- * ab8500_fg_cc_convend_handler() - isr to get battery avg current.
+ * ab8500_fg_cc_int_calib_handler () - end of calibration isr.
  * @irq:       interrupt number
  * @_di:       pointer to the ab8500_fg structure
  *
@@ -2153,9 +2150,7 @@ static int ab8500_fg_get_property(struct power_supply *psy,
 			val->intval = di->bat_cap.prev_mah;
 		break;
 	case POWER_SUPPLY_PROP_CAPACITY:
-		if (di->bm->capacity_scaling)
-			val->intval = di->bat_cap.cap_scale.scaled_cap;
-		else if (di->flags.batt_unknown && !di->bm->chg_unknown_bat &&
+		if (di->flags.batt_unknown && !di->bm->chg_unknown_bat &&
 				di->flags.batt_id_received)
 			val->intval = 100;
 		else
@@ -2344,6 +2339,50 @@ static int ab8500_fg_init_hw_registers(struct ab8500_fg *di)
 		dev_err(di->dev, "BattOk init write failed.\n");
 		goto out;
 	}
+
+	if (((is_ab8505(di->parent) || is_ab9540(di->parent)) &&
+			abx500_get_chip_id(di->dev) >= AB8500_CUT2P0)
+			|| is_ab8540(di->parent)) {
+		ret = abx500_set_register_interruptible(di->dev, AB8500_RTC,
+			AB8505_RTC_PCUT_MAX_TIME_REG, di->bm->fg_params->pcut_max_time);
+
+		if (ret) {
+			dev_err(di->dev, "%s write failed AB8505_RTC_PCUT_MAX_TIME_REG\n", __func__);
+			goto out;
+		};
+
+		ret = abx500_set_register_interruptible(di->dev, AB8500_RTC,
+			AB8505_RTC_PCUT_FLAG_TIME_REG, di->bm->fg_params->pcut_flag_time);
+
+		if (ret) {
+			dev_err(di->dev, "%s write failed AB8505_RTC_PCUT_FLAG_TIME_REG\n", __func__);
+			goto out;
+		};
+
+		ret = abx500_set_register_interruptible(di->dev, AB8500_RTC,
+			AB8505_RTC_PCUT_RESTART_REG, di->bm->fg_params->pcut_max_restart);
+
+		if (ret) {
+			dev_err(di->dev, "%s write failed AB8505_RTC_PCUT_RESTART_REG\n", __func__);
+			goto out;
+		};
+
+		ret = abx500_set_register_interruptible(di->dev, AB8500_RTC,
+			AB8505_RTC_PCUT_DEBOUNCE_REG, di->bm->fg_params->pcut_debounce_time);
+
+		if (ret) {
+			dev_err(di->dev, "%s write failed AB8505_RTC_PCUT_DEBOUNCE_REG\n", __func__);
+			goto out;
+		};
+
+		ret = abx500_set_register_interruptible(di->dev, AB8500_RTC,
+			AB8505_RTC_PCUT_CTL_STATUS_REG, di->bm->fg_params->pcut_enable);
+
+		if (ret) {
+			dev_err(di->dev, "%s write failed AB8505_RTC_PCUT_CTL_STATUS_REG\n", __func__);
+			goto out;
+		};
+	}
 out:
 	return ret;
 }
@@ -2546,6 +2585,428 @@ static int ab8500_fg_sysfs_init(struct ab8500_fg *di)
 
 	return ret;
 }
+
+static ssize_t ab8505_powercut_flagtime_read(struct device *dev,
+			     struct device_attribute *attr,
+			     char *buf)
+{
+	int ret;
+	u8 reg_value;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
+		AB8505_RTC_PCUT_FLAG_TIME_REG, &reg_value);
+
+	if (ret < 0) {
+		dev_err(dev, "Failed to read AB8505_RTC_PCUT_FLAG_TIME_REG\n");
+		goto fail;
+	}
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", (reg_value & 0x7F));
+
+fail:
+	return ret;
+}
+
+static ssize_t ab8505_powercut_flagtime_write(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t count)
+{
+	int ret;
+	long unsigned reg_value;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	reg_value = simple_strtoul(buf, NULL, 10);
+
+	if (reg_value > 0x7F) {
+		dev_err(dev, "Incorrect parameter, echo 0 (1.98s) - 127 (15.625ms) for flagtime\n");
+		goto fail;
+	}
+
+	ret = abx500_set_register_interruptible(di->dev, AB8500_RTC,
+		AB8505_RTC_PCUT_FLAG_TIME_REG, (u8)reg_value);
+
+	if (ret < 0)
+		dev_err(dev, "Failed to set AB8505_RTC_PCUT_FLAG_TIME_REG\n");
+
+fail:
+	return count;
+}
+
+static ssize_t ab8505_powercut_maxtime_read(struct device *dev,
+			     struct device_attribute *attr,
+			     char *buf)
+{
+	int ret;
+	u8 reg_value;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
+		AB8505_RTC_PCUT_MAX_TIME_REG, &reg_value);
+
+	if (ret < 0) {
+		dev_err(dev, "Failed to read AB8505_RTC_PCUT_MAX_TIME_REG\n");
+		goto fail;
+	}
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", (reg_value & 0x7F));
+
+fail:
+	return ret;
+
+}
+
+static ssize_t ab8505_powercut_maxtime_write(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t count)
+{
+	int ret;
+	int reg_value;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	reg_value = simple_strtoul(buf, NULL, 10);
+	if (reg_value > 0x7F) {
+		dev_err(dev, "Incorrect parameter, echo 0 (0.0s) - 127 (1.98s) for maxtime\n");
+		goto fail;
+	}
+
+	ret = abx500_set_register_interruptible(di->dev, AB8500_RTC,
+		AB8505_RTC_PCUT_MAX_TIME_REG, (u8)reg_value);
+
+	if (ret < 0)
+		dev_err(dev, "Failed to set AB8505_RTC_PCUT_MAX_TIME_REG\n");
+
+fail:
+	return count;
+}
+
+static ssize_t ab8505_powercut_restart_read(struct device *dev,
+			     struct device_attribute *attr,
+			     char *buf)
+{
+	int ret;
+	u8 reg_value;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
+		AB8505_RTC_PCUT_RESTART_REG, &reg_value);
+
+	if (ret < 0) {
+		dev_err(dev, "Failed to read AB8505_RTC_PCUT_RESTART_REG\n");
+		goto fail;
+	}
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", (reg_value & 0xF));
+
+fail:
+	return ret;
+}
+
+static ssize_t ab8505_powercut_restart_write(struct device *dev,
+					     struct device_attribute *attr,
+					     const char *buf, size_t count)
+{
+	int ret;
+	int reg_value;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	reg_value = simple_strtoul(buf, NULL, 10);
+	if (reg_value > 0xF) {
+		dev_err(dev, "Incorrect parameter, echo 0 - 15 for number of restart\n");
+		goto fail;
+	}
+
+	ret = abx500_set_register_interruptible(di->dev, AB8500_RTC,
+						AB8505_RTC_PCUT_RESTART_REG, (u8)reg_value);
+
+	if (ret < 0)
+		dev_err(dev, "Failed to set AB8505_RTC_PCUT_RESTART_REG\n");
+
+fail:
+	return count;
+
+}
+
+static ssize_t ab8505_powercut_timer_read(struct device *dev,
+					  struct device_attribute *attr,
+					  char *buf)
+{
+	int ret;
+	u8 reg_value;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
+						AB8505_RTC_PCUT_TIME_REG, &reg_value);
+
+	if (ret < 0) {
+		dev_err(dev, "Failed to read AB8505_RTC_PCUT_TIME_REG\n");
+		goto fail;
+	}
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", (reg_value & 0x7F));
+
+fail:
+	return ret;
+}
+
+static ssize_t ab8505_powercut_restart_counter_read(struct device *dev,
+						    struct device_attribute *attr,
+						    char *buf)
+{
+	int ret;
+	u8 reg_value;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
+						AB8505_RTC_PCUT_RESTART_REG, &reg_value);
+
+	if (ret < 0) {
+		dev_err(dev, "Failed to read AB8505_RTC_PCUT_RESTART_REG\n");
+		goto fail;
+	}
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", (reg_value & 0xF0) >> 4);
+
+fail:
+	return ret;
+}
+
+static ssize_t ab8505_powercut_read(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	int ret;
+	u8 reg_value;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
+						AB8505_RTC_PCUT_CTL_STATUS_REG, &reg_value);
+
+	if (ret < 0)
+		goto fail;
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", (reg_value & 0x1));
+
+fail:
+	return ret;
+}
+
+static ssize_t ab8505_powercut_write(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	int ret;
+	int reg_value;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	reg_value = simple_strtoul(buf, NULL, 10);
+	if (reg_value > 0x1) {
+		dev_err(dev, "Incorrect parameter, echo 0/1 to disable/enable Pcut feature\n");
+		goto fail;
+	}
+
+	ret = abx500_set_register_interruptible(di->dev, AB8500_RTC,
+						AB8505_RTC_PCUT_CTL_STATUS_REG, (u8)reg_value);
+
+	if (ret < 0)
+		dev_err(dev, "Failed to set AB8505_RTC_PCUT_CTL_STATUS_REG\n");
+
+fail:
+	return count;
+}
+
+static ssize_t ab8505_powercut_flag_read(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+
+	int ret;
+	u8 reg_value;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
+						AB8505_RTC_PCUT_CTL_STATUS_REG,  &reg_value);
+
+	if (ret < 0) {
+		dev_err(dev, "Failed to read AB8505_RTC_PCUT_CTL_STATUS_REG\n");
+		goto fail;
+	}
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", ((reg_value & 0x10) >> 4));
+
+fail:
+	return ret;
+}
+
+static ssize_t ab8505_powercut_debounce_read(struct device *dev,
+					     struct device_attribute *attr,
+					     char *buf)
+{
+	int ret;
+	u8 reg_value;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
+						AB8505_RTC_PCUT_DEBOUNCE_REG,  &reg_value);
+
+	if (ret < 0) {
+		dev_err(dev, "Failed to read AB8505_RTC_PCUT_DEBOUNCE_REG\n");
+		goto fail;
+	}
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", (reg_value & 0x7));
+
+fail:
+	return ret;
+}
+
+static ssize_t ab8505_powercut_debounce_write(struct device *dev,
+					      struct device_attribute *attr,
+					      const char *buf, size_t count)
+{
+	int ret;
+	int reg_value;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	reg_value = simple_strtoul(buf, NULL, 10);
+	if (reg_value > 0x7) {
+		dev_err(dev, "Incorrect parameter, echo 0 to 7 for debounce setting\n");
+		goto fail;
+	}
+
+	ret = abx500_set_register_interruptible(di->dev, AB8500_RTC,
+						AB8505_RTC_PCUT_DEBOUNCE_REG, (u8)reg_value);
+
+	if (ret < 0)
+		dev_err(dev, "Failed to set AB8505_RTC_PCUT_DEBOUNCE_REG\n");
+
+fail:
+	return count;
+}
+
+static ssize_t ab8505_powercut_enable_status_read(struct device *dev,
+						  struct device_attribute *attr,
+						  char *buf)
+{
+	int ret;
+	u8 reg_value;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_RTC,
+						AB8505_RTC_PCUT_CTL_STATUS_REG, &reg_value);
+
+	if (ret < 0) {
+		dev_err(dev, "Failed to read AB8505_RTC_PCUT_CTL_STATUS_REG\n");
+		goto fail;
+	}
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", ((reg_value & 0x20) >> 5));
+
+fail:
+	return ret;
+}
+
+static struct device_attribute ab8505_fg_sysfs_psy_attrs[] = {
+	__ATTR(powercut_flagtime, (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8505_powercut_flagtime_read, ab8505_powercut_flagtime_write),
+	__ATTR(powercut_maxtime, (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8505_powercut_maxtime_read, ab8505_powercut_maxtime_write),
+	__ATTR(powercut_restart_max, (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8505_powercut_restart_read, ab8505_powercut_restart_write),
+	__ATTR(powercut_timer, S_IRUGO, ab8505_powercut_timer_read, NULL),
+	__ATTR(powercut_restart_counter, S_IRUGO,
+		ab8505_powercut_restart_counter_read, NULL),
+	__ATTR(powercut_enable, (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8505_powercut_read, ab8505_powercut_write),
+	__ATTR(powercut_flag, S_IRUGO, ab8505_powercut_flag_read, NULL),
+	__ATTR(powercut_debounce_time, (S_IRUGO | S_IWUSR | S_IWGRP),
+		ab8505_powercut_debounce_read, ab8505_powercut_debounce_write),
+	__ATTR(powercut_enable_status, S_IRUGO,
+		ab8505_powercut_enable_status_read, NULL),
+};
+
+static int ab8500_fg_sysfs_psy_create_attrs(struct device *dev)
+{
+	unsigned int i, j;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	if (((is_ab8505(di->parent) || is_ab9540(di->parent)) &&
+	     abx500_get_chip_id(dev->parent) >= AB8500_CUT2P0)
+	    || is_ab8540(di->parent)) {
+		for (j = 0; j < ARRAY_SIZE(ab8505_fg_sysfs_psy_attrs); j++)
+			if (device_create_file(dev, &ab8505_fg_sysfs_psy_attrs[j]))
+				goto sysfs_psy_create_attrs_failed_ab8505;
+	}
+	return 0;
+sysfs_psy_create_attrs_failed_ab8505:
+	dev_err(dev, "Failed creating sysfs psy attrs for ab8505.\n");
+	while (j--)
+		device_remove_file(dev, &ab8505_fg_sysfs_psy_attrs[i]);
+
+	return -EIO;
+}
+
+static void ab8500_fg_sysfs_psy_remove_attrs(struct device *dev)
+{
+	unsigned int i;
+	struct power_supply *psy = dev_get_drvdata(dev);
+	struct ab8500_fg *di;
+
+	di = to_ab8500_fg_device_info(psy);
+
+	if (((is_ab8505(di->parent) || is_ab9540(di->parent)) &&
+	     abx500_get_chip_id(dev->parent) >= AB8500_CUT2P0)
+	    || is_ab8540(di->parent)) {
+		for (i = 0; i < ARRAY_SIZE(ab8505_fg_sysfs_psy_attrs); i++)
+			(void)device_remove_file(dev, &ab8505_fg_sysfs_psy_attrs[i]);
+	}
+}
+
 /* Exposure to the sysfs interface <<END>> */
 
 #if defined(CONFIG_PM)
@@ -2607,6 +3068,7 @@ static int ab8500_fg_remove(struct platform_device *pdev)
 	ab8500_fg_sysfs_exit(di);
 
 	flush_scheduled_work();
+	ab8500_fg_sysfs_psy_remove_attrs(di->fg_psy.dev);
 	power_supply_unregister(&di->fg_psy);
 	platform_set_drvdata(pdev, NULL);
 	return ret;
@@ -2772,6 +3234,13 @@ static int ab8500_fg_probe(struct platform_device *pdev)
 		goto free_irq;
 	}
 
+	ret = ab8500_fg_sysfs_psy_create_attrs(di->fg_psy.dev);
+	if (ret) {
+		dev_err(di->dev, "failed to create FG psy\n");
+		ab8500_fg_sysfs_exit(di);
+		goto free_irq;
+	}
+
 	/* Calibrate the fg first time */
 	di->flags.calibrate = true;
 	di->calib_state = AB8500_FG_CALIB_INIT;
diff --git a/drivers/power/abx500_chargalg.c b/drivers/power/abx500_chargalg.c
index f043c0851a76..9863e423602c 100644
--- a/drivers/power/abx500_chargalg.c
+++ b/drivers/power/abx500_chargalg.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (C) ST-Ericsson SA 2012
+ * Copyright (c) 2012 Sony Mobile Communications AB
  *
  * Charging algorithm driver for abx500 variants
  *
@@ -8,11 +9,13 @@
  *	Johan Palsson <johan.palsson@stericsson.com>
  *	Karl Komierowski <karl.komierowski@stericsson.com>
  *	Arun R Murthy <arun.murthy@stericsson.com>
+ *	Author: Imre Sunyi <imre.sunyi@sonymobile.com>
  */
 
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/device.h>
+#include <linux/hrtimer.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/slab.h>
@@ -24,8 +27,10 @@
 #include <linux/of.h>
 #include <linux/mfd/core.h>
 #include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab8500.h>
 #include <linux/mfd/abx500/ux500_chargalg.h>
 #include <linux/mfd/abx500/ab8500-bm.h>
+#include <linux/notifier.h>
 
 /* Watchdog kick interval */
 #define CHG_WD_INTERVAL			(6 * HZ)
@@ -33,6 +38,18 @@
 /* End-of-charge criteria counter */
 #define EOC_COND_CNT			10
 
+/* One hour expressed in seconds */
+#define ONE_HOUR_IN_SECONDS            3600
+
+/* Five minutes expressed in seconds */
+#define FIVE_MINUTES_IN_SECONDS        300
+
+/* Plus margin for the low battery threshold */
+#define BAT_PLUS_MARGIN                (100)
+
+#define CHARGALG_CURR_STEP_LOW		0
+#define CHARGALG_CURR_STEP_HIGH	100
+
 #define to_abx500_chargalg_device_info(x) container_of((x), \
 	struct abx500_chargalg, chargalg_psy);
 
@@ -66,6 +83,11 @@ struct abx500_chargalg_suspension_status {
 	bool usb_suspended;
 };
 
+struct abx500_chargalg_current_step_status {
+	bool curr_step_change;
+	int curr_step;
+};
+
 struct abx500_chargalg_battery_data {
 	int temp;
 	int volt;
@@ -82,6 +104,7 @@ enum abx500_chargalg_states {
 	STATE_HW_TEMP_PROTECT_INIT,
 	STATE_HW_TEMP_PROTECT,
 	STATE_NORMAL_INIT,
+	STATE_USB_PP_PRE_CHARGE,
 	STATE_NORMAL,
 	STATE_WAIT_FOR_RECHARGE_INIT,
 	STATE_WAIT_FOR_RECHARGE,
@@ -113,6 +136,7 @@ static const char *states[] = {
 	"HW_TEMP_PROTECT_INIT",
 	"HW_TEMP_PROTECT",
 	"NORMAL_INIT",
+	"USB_PP_PRE_CHARGE",
 	"NORMAL",
 	"WAIT_FOR_RECHARGE_INIT",
 	"WAIT_FOR_RECHARGE",
@@ -204,6 +228,8 @@ enum maxim_ret {
  * @batt_data:		data of the battery
  * @susp_status:	current charger suspension status
  * @bm:           	Platform specific battery management information
+ * @curr_status:	Current step status for over-current protection
+ * @parent:		pointer to the struct abx500
  * @chargalg_psy:	structure that holds the battery properties exposed by
  *			the charging algorithm
  * @events:		structure for information about events triggered
@@ -227,6 +253,8 @@ struct abx500_chargalg {
 	struct abx500_chargalg_charger_info chg_info;
 	struct abx500_chargalg_battery_data batt_data;
 	struct abx500_chargalg_suspension_status susp_status;
+	struct ab8500 *parent;
+	struct abx500_chargalg_current_step_status curr_status;
 	struct abx500_bm_data *bm;
 	struct power_supply chargalg_psy;
 	struct ux500_charger *ac_chg;
@@ -236,51 +264,69 @@ struct abx500_chargalg {
 	struct delayed_work chargalg_periodic_work;
 	struct delayed_work chargalg_wd_work;
 	struct work_struct chargalg_work;
-	struct timer_list safety_timer;
-	struct timer_list maintenance_timer;
+	struct hrtimer safety_timer;
+	struct hrtimer maintenance_timer;
 	struct kobject chargalg_kobject;
 };
 
+/*External charger prepare notifier*/
+BLOCKING_NOTIFIER_HEAD(charger_notifier_list);
+
 /* Main battery properties */
 static enum power_supply_property abx500_chargalg_props[] = {
 	POWER_SUPPLY_PROP_STATUS,
 	POWER_SUPPLY_PROP_HEALTH,
 };
 
+struct abx500_chargalg_sysfs_entry {
+	struct attribute attr;
+	ssize_t (*show)(struct abx500_chargalg *, char *);
+	ssize_t (*store)(struct abx500_chargalg *, const char *, size_t);
+};
+
 /**
  * abx500_chargalg_safety_timer_expired() - Expiration of the safety timer
- * @data:	pointer to the abx500_chargalg structure
+ * @timer:     pointer to the hrtimer structure
  *
  * This function gets called when the safety timer for the charger
  * expires
  */
-static void abx500_chargalg_safety_timer_expired(unsigned long data)
+static enum hrtimer_restart
+abx500_chargalg_safety_timer_expired(struct hrtimer *timer)
 {
-	struct abx500_chargalg *di = (struct abx500_chargalg *) data;
+	struct abx500_chargalg *di = container_of(timer, struct abx500_chargalg,
+						  safety_timer);
 	dev_err(di->dev, "Safety timer expired\n");
 	di->events.safety_timer_expired = true;
 
 	/* Trigger execution of the algorithm instantly */
 	queue_work(di->chargalg_wq, &di->chargalg_work);
+
+	return HRTIMER_NORESTART;
 }
 
 /**
  * abx500_chargalg_maintenance_timer_expired() - Expiration of
  * the maintenance timer
- * @i:		pointer to the abx500_chargalg structure
+ * @timer:     pointer to the timer structure
  *
  * This function gets called when the maintenence timer
  * expires
  */
-static void abx500_chargalg_maintenance_timer_expired(unsigned long data)
+static enum hrtimer_restart
+abx500_chargalg_maintenance_timer_expired(struct hrtimer *timer)
 {
 
-	struct abx500_chargalg *di = (struct abx500_chargalg *) data;
+	struct abx500_chargalg *di = container_of(timer, struct abx500_chargalg,
+						  maintenance_timer);
+
 	dev_dbg(di->dev, "Maintenance timer expired\n");
 	di->events.maintenance_timer_expired = true;
 
 	/* Trigger execution of the algorithm instantly */
 	queue_work(di->chargalg_wq, &di->chargalg_work);
+
+	return HRTIMER_NORESTART;
 }
 
 /**
@@ -303,6 +349,30 @@ static void abx500_chargalg_state_to(struct abx500_chargalg *di,
 	di->charge_state = state;
 }
 
+static int abx500_chargalg_check_charger_enable(struct abx500_chargalg *di)
+{
+	switch (di->charge_state) {
+	case STATE_NORMAL:
+	case STATE_MAINTENANCE_A:
+	case STATE_MAINTENANCE_B:
+		break;
+	default:
+		return 0;
+	}
+
+	if (di->chg_info.charger_type & USB_CHG) {
+		return di->usb_chg->ops.check_enable(di->usb_chg,
+                         di->bm->bat_type[di->bm->batt_id].normal_vol_lvl,
+                         di->bm->bat_type[di->bm->batt_id].normal_cur_lvl);
+	} else if ((di->chg_info.charger_type & AC_CHG) &&
+		   !(di->ac_chg->external)) {
+		return di->ac_chg->ops.check_enable(di->ac_chg,
+                         di->bm->bat_type[di->bm->batt_id].normal_vol_lvl,
+                         di->bm->bat_type[di->bm->batt_id].normal_cur_lvl);
+	}
+	return 0;
+}
+
 /**
  * abx500_chargalg_check_charger_connection() - Check charger connection change
  * @di:		pointer to the abx500_chargalg structure
@@ -348,6 +418,22 @@ static int abx500_chargalg_check_charger_connection(struct abx500_chargalg *di)
 }
 
 /**
+ * abx500_chargalg_check_current_step_status() - Check charging current
+ * step status.
+ * @di:		pointer to the abx500_chargalg structure
+ *
+ * This function will check if there is a change in the charging current step
+ * and change charge state accordingly.
+ */
+static void abx500_chargalg_check_current_step_status
+	(struct abx500_chargalg *di)
+{
+	if (di->curr_status.curr_step_change)
+		abx500_chargalg_state_to(di, STATE_NORMAL_INIT);
+	di->curr_status.curr_step_change = false;
+}
+
+/**
  * abx500_chargalg_start_safety_timer() - Start charging safety timer
  * @di:		pointer to the abx500_chargalg structure
  *
@@ -356,19 +442,16 @@ static int abx500_chargalg_check_charger_connection(struct abx500_chargalg *di)
  */
 static void abx500_chargalg_start_safety_timer(struct abx500_chargalg *di)
 {
-	unsigned long timer_expiration = 0;
+	/* Charger-dependent expiration time in hours*/
+	int timer_expiration = 0;
 
 	switch (di->chg_info.charger_type) {
 	case AC_CHG:
-		timer_expiration =
-		round_jiffies(jiffies +
-			(di->bm->main_safety_tmr_h * 3600 * HZ));
+		timer_expiration = di->bm->main_safety_tmr_h;
 		break;
 
 	case USB_CHG:
-		timer_expiration =
-		round_jiffies(jiffies +
-			(di->bm->usb_safety_tmr_h * 3600 * HZ));
+		timer_expiration = di->bm->usb_safety_tmr_h;
 		break;
 
 	default:
@@ -377,11 +460,10 @@ static void abx500_chargalg_start_safety_timer(struct abx500_chargalg *di)
 	}
 
 	di->events.safety_timer_expired = false;
-	di->safety_timer.expires = timer_expiration;
-	if (!timer_pending(&di->safety_timer))
-		add_timer(&di->safety_timer);
-	else
-		mod_timer(&di->safety_timer, timer_expiration);
+	hrtimer_set_expires_range(&di->safety_timer,
+		ktime_set(timer_expiration * ONE_HOUR_IN_SECONDS, 0),
+		ktime_set(FIVE_MINUTES_IN_SECONDS, 0));
+	hrtimer_start_expires(&di->safety_timer, HRTIMER_MODE_REL);
 }
 
 /**
@@ -392,8 +474,8 @@ static void abx500_chargalg_start_safety_timer(struct abx500_chargalg *di)
  */
 static void abx500_chargalg_stop_safety_timer(struct abx500_chargalg *di)
 {
-	di->events.safety_timer_expired = false;
-	del_timer(&di->safety_timer);
+	if (hrtimer_try_to_cancel(&di->safety_timer) >= 0)
+		di->events.safety_timer_expired = false;
 }
 
 /**
@@ -408,17 +490,11 @@ static void abx500_chargalg_stop_safety_timer(struct abx500_chargalg *di)
 static void abx500_chargalg_start_maintenance_timer(struct abx500_chargalg *di,
 	int duration)
 {
-	unsigned long timer_expiration;
-
-	/* Convert from hours to jiffies */
-	timer_expiration = round_jiffies(jiffies + (duration * 3600 * HZ));
-
+	hrtimer_set_expires_range(&di->maintenance_timer,
+		ktime_set(duration * ONE_HOUR_IN_SECONDS, 0),
+		ktime_set(FIVE_MINUTES_IN_SECONDS, 0));
 	di->events.maintenance_timer_expired = false;
-	di->maintenance_timer.expires = timer_expiration;
-	if (!timer_pending(&di->maintenance_timer))
-		add_timer(&di->maintenance_timer);
-	else
-		mod_timer(&di->maintenance_timer, timer_expiration);
+	hrtimer_start_expires(&di->maintenance_timer, HRTIMER_MODE_REL);
 }
 
 /**
@@ -430,8 +506,8 @@ static void abx500_chargalg_start_maintenance_timer(struct abx500_chargalg *di,
  */
 static void abx500_chargalg_stop_maintenance_timer(struct abx500_chargalg *di)
 {
-	di->events.maintenance_timer_expired = false;
-	del_timer(&di->maintenance_timer);
+	if (hrtimer_try_to_cancel(&di->maintenance_timer) >= 0)
+		di->events.maintenance_timer_expired = false;
 }
 
 /**
@@ -477,6 +553,8 @@ static int abx500_chargalg_kick_watchdog(struct abx500_chargalg *di)
 static int abx500_chargalg_ac_en(struct abx500_chargalg *di, int enable,
 	int vset, int iset)
 {
+	static int abx500_chargalg_ex_ac_enable_toggle;
+
 	if (!di->ac_chg || !di->ac_chg->ops.enable)
 		return -ENXIO;
 
@@ -489,6 +567,14 @@ static int abx500_chargalg_ac_en(struct abx500_chargalg *di, int enable,
 	di->chg_info.ac_iset = iset;
 	di->chg_info.ac_vset = vset;
 
+	/* Enable external charger */
+	if (enable && di->ac_chg->external &&
+	    !abx500_chargalg_ex_ac_enable_toggle) {
+		blocking_notifier_call_chain(&charger_notifier_list,
+					     0, di->dev);
+		abx500_chargalg_ex_ac_enable_toggle++;
+	}
+
 	return di->ac_chg->ops.enable(di->ac_chg, enable, vset, iset);
 }
 
@@ -520,6 +606,37 @@ static int abx500_chargalg_usb_en(struct abx500_chargalg *di, int enable,
 	return di->usb_chg->ops.enable(di->usb_chg, enable, vset, iset);
 }
 
+ /**
+ * ab8540_chargalg_usb_pp_en() - Enable/ disable USB power path
+ * @di:                pointer to the abx500_chargalg structure
+ * @enable:    power path enable/disable
+ *
+ * The USB power path will be enable/ disable
+ */
+static int ab8540_chargalg_usb_pp_en(struct abx500_chargalg *di, bool enable)
+{
+	if (!di->usb_chg || !di->usb_chg->ops.pp_enable)
+		return -ENXIO;
+
+	return di->usb_chg->ops.pp_enable(di->usb_chg, enable);
+}
+
+/**
+ * ab8540_chargalg_usb_pre_chg_en() - Enable/ disable USB pre-charge
+ * @di:                pointer to the abx500_chargalg structure
+ * @enable:    USB pre-charge enable/disable
+ *
+ * The USB USB pre-charge will be enable/ disable
+ */
+static int ab8540_chargalg_usb_pre_chg_en(struct abx500_chargalg *di,
+					  bool enable)
+{
+	if (!di->usb_chg || !di->usb_chg->ops.pre_chg_enable)
+		return -ENXIO;
+
+	return di->usb_chg->ops.pre_chg_enable(di->usb_chg, enable);
+}
+
 /**
  * abx500_chargalg_update_chg_curr() - Update charger current
  * @di:		pointer to the abx500_chargalg structure
@@ -613,8 +730,6 @@ static void abx500_chargalg_hold_charging(struct abx500_chargalg *di)
 static void abx500_chargalg_start_charging(struct abx500_chargalg *di,
 	int vset, int iset)
 {
-	bool start_chargalg_wd = true;
-
 	switch (di->chg_info.charger_type) {
 	case AC_CHG:
 		dev_dbg(di->dev,
@@ -632,12 +747,8 @@ static void abx500_chargalg_start_charging(struct abx500_chargalg *di,
 
 	default:
 		dev_err(di->dev, "Unknown charger to charge from\n");
-		start_chargalg_wd = false;
 		break;
 	}
-
-	if (start_chargalg_wd && !delayed_work_pending(&di->chargalg_wd_work))
-		queue_delayed_work(di->chargalg_wq, &di->chargalg_wd_work, 0);
 }
 
 /**
@@ -725,6 +836,9 @@ static void abx500_chargalg_end_of_charge(struct abx500_chargalg *di)
 		di->batt_data.avg_curr > 0) {
 		if (++di->eoc_cnt >= EOC_COND_CNT) {
 			di->eoc_cnt = 0;
+			if ((di->chg_info.charger_type & USB_CHG) &&
+			   (di->usb_chg->power_path))
+				ab8540_chargalg_usb_pp_en(di, true);
 			di->charge_status = POWER_SUPPLY_STATUS_FULL;
 			di->maintenance_chg = true;
 			dev_dbg(di->dev, "EOC reached!\n");
@@ -1217,6 +1331,8 @@ static void abx500_chargalg_external_power_changed(struct power_supply *psy)
 static void abx500_chargalg_algorithm(struct abx500_chargalg *di)
 {
 	int charger_status;
+	int ret;
+	int curr_step_lvl;
 
 	/* Collect data from all power_supply class devices */
 	class_for_each_device(power_supply_class, NULL,
@@ -1227,6 +1343,15 @@ static void abx500_chargalg_algorithm(struct abx500_chargalg *di)
 	abx500_chargalg_check_charger_voltage(di);
 
 	charger_status = abx500_chargalg_check_charger_connection(di);
+	abx500_chargalg_check_current_step_status(di);
+
+	if (is_ab8500(di->parent)) {
+		ret = abx500_chargalg_check_charger_enable(di);
+		if (ret < 0)
+			dev_err(di->dev, "Checking charger is enabled error"
+					": Returned Value %d\n", ret);
+	}
+
 	/*
 	 * First check if we have a charger connected.
 	 * Also we don't allow charging of unknown batteries if configured
@@ -1416,9 +1541,34 @@ static void abx500_chargalg_algorithm(struct abx500_chargalg *di)
 		break;
 
 	case STATE_NORMAL_INIT:
-		abx500_chargalg_start_charging(di,
-			di->bm->bat_type[di->bm->batt_id].normal_vol_lvl,
-			di->bm->bat_type[di->bm->batt_id].normal_cur_lvl);
+		if ((di->chg_info.charger_type & USB_CHG) &&
+				di->usb_chg->power_path) {
+			if (di->batt_data.volt >
+			    (di->bm->fg_params->lowbat_threshold +
+			     BAT_PLUS_MARGIN)) {
+				ab8540_chargalg_usb_pre_chg_en(di, false);
+				ab8540_chargalg_usb_pp_en(di, false);
+			} else {
+				ab8540_chargalg_usb_pp_en(di, true);
+				ab8540_chargalg_usb_pre_chg_en(di, true);
+				abx500_chargalg_state_to(di,
+					STATE_USB_PP_PRE_CHARGE);
+				break;
+			}
+		}
+
+		if (di->curr_status.curr_step == CHARGALG_CURR_STEP_LOW)
+			abx500_chargalg_stop_charging(di);
+		else {
+			curr_step_lvl = di->bm->bat_type[
+				di->bm->batt_id].normal_cur_lvl
+				* di->curr_status.curr_step
+				/ CHARGALG_CURR_STEP_HIGH;
+			abx500_chargalg_start_charging(di,
+				di->bm->bat_type[di->bm->batt_id]
+				.normal_vol_lvl, curr_step_lvl);
+		}
+
 		abx500_chargalg_state_to(di, STATE_NORMAL);
 		abx500_chargalg_start_safety_timer(di);
 		abx500_chargalg_stop_maintenance_timer(di);
@@ -1430,6 +1580,13 @@ static void abx500_chargalg_algorithm(struct abx500_chargalg *di)
 
 		break;
 
+	case STATE_USB_PP_PRE_CHARGE:
+		if (di->batt_data.volt >
+			(di->bm->fg_params->lowbat_threshold +
+			BAT_PLUS_MARGIN))
+			abx500_chargalg_state_to(di, STATE_NORMAL_INIT);
+		break;
+
 	case STATE_NORMAL:
 		handle_maxim_chg_curr(di);
 		if (di->charge_status == POWER_SUPPLY_STATUS_FULL &&
@@ -1653,99 +1810,134 @@ static int abx500_chargalg_get_property(struct power_supply *psy,
 
 /* Exposure to the sysfs interface */
 
-/**
- * abx500_chargalg_sysfs_show() - sysfs show operations
- * @kobj:      pointer to the struct kobject
- * @attr:      pointer to the struct attribute
- * @buf:       buffer that holds the parameter to send to userspace
- *
- * Returns a buffer to be displayed in user space
- */
-static ssize_t abx500_chargalg_sysfs_show(struct kobject *kobj,
-					  struct attribute *attr, char *buf)
+static ssize_t abx500_chargalg_curr_step_show(struct abx500_chargalg *di,
+					      char *buf)
 {
-	struct abx500_chargalg *di = container_of(kobj,
-               struct abx500_chargalg, chargalg_kobject);
+	return sprintf(buf, "%d\n", di->curr_status.curr_step);
+}
+
+static ssize_t abx500_chargalg_curr_step_store(struct abx500_chargalg *di,
+					       const char *buf, size_t length)
+{
+	long int param;
+	int ret;
+
+	ret = kstrtol(buf, 10, &param);
+	if (ret < 0)
+		return ret;
+
+	di->curr_status.curr_step = param;
+	if (di->curr_status.curr_step >= CHARGALG_CURR_STEP_LOW &&
+		di->curr_status.curr_step <= CHARGALG_CURR_STEP_HIGH) {
+		di->curr_status.curr_step_change = true;
+		queue_work(di->chargalg_wq, &di->chargalg_work);
+	} else
+		dev_info(di->dev, "Wrong current step\n"
+			"Enter 0. Disable AC/USB Charging\n"
+			"1--100. Set AC/USB charging current step\n"
+			"100. Enable AC/USB Charging\n");
+
+	return strlen(buf);
+}
+
 
+static ssize_t abx500_chargalg_en_show(struct abx500_chargalg *di,
+				       char *buf)
+{
 	return sprintf(buf, "%d\n",
 		       di->susp_status.ac_suspended &&
 		       di->susp_status.usb_suspended);
 }
 
-/**
- * abx500_chargalg_sysfs_charger() - sysfs store operations
- * @kobj:      pointer to the struct kobject
- * @attr:      pointer to the struct attribute
- * @buf:       buffer that holds the parameter passed from userspace
- * @length:    length of the parameter passed
- *
- * Returns length of the buffer(input taken from user space) on success
- * else error code on failure
- * The operation to be performed on passing the parameters from the user space.
- */
-static ssize_t abx500_chargalg_sysfs_charger(struct kobject *kobj,
-	struct attribute *attr, const char *buf, size_t length)
+static ssize_t abx500_chargalg_en_store(struct abx500_chargalg *di,
+	const char *buf, size_t length)
 {
-	struct abx500_chargalg *di = container_of(kobj,
-		struct abx500_chargalg, chargalg_kobject);
 	long int param;
 	int ac_usb;
 	int ret;
-	char entry = *attr->name;
 
-	switch (entry) {
-	case 'c':
-		ret = strict_strtol(buf, 10, &param);
-		if (ret < 0)
-			return ret;
-
-		ac_usb = param;
-		switch (ac_usb) {
-		case 0:
-			/* Disable charging */
-			di->susp_status.ac_suspended = true;
-			di->susp_status.usb_suspended = true;
-			di->susp_status.suspended_change = true;
-			/* Trigger a state change */
-			queue_work(di->chargalg_wq,
-				&di->chargalg_work);
-			break;
-		case 1:
-			/* Enable AC Charging */
-			di->susp_status.ac_suspended = false;
-			di->susp_status.suspended_change = true;
-			/* Trigger a state change */
-			queue_work(di->chargalg_wq,
-				&di->chargalg_work);
-			break;
-		case 2:
-			/* Enable USB charging */
-			di->susp_status.usb_suspended = false;
-			di->susp_status.suspended_change = true;
-			/* Trigger a state change */
-			queue_work(di->chargalg_wq,
-				&di->chargalg_work);
-			break;
-		default:
-			dev_info(di->dev, "Wrong input\n"
-				"Enter 0. Disable AC/USB Charging\n"
-				"1. Enable AC charging\n"
-				"2. Enable USB Charging\n");
-		};
+	ret = kstrtol(buf, 10, &param);
+	if (ret < 0)
+		return ret;
+
+	ac_usb = param;
+	switch (ac_usb) {
+	case 0:
+		/* Disable charging */
+		di->susp_status.ac_suspended = true;
+		di->susp_status.usb_suspended = true;
+		di->susp_status.suspended_change = true;
+		/* Trigger a state change */
+		queue_work(di->chargalg_wq,
+			&di->chargalg_work);
+		break;
+	case 1:
+		/* Enable AC Charging */
+		di->susp_status.ac_suspended = false;
+		di->susp_status.suspended_change = true;
+		/* Trigger a state change */
+		queue_work(di->chargalg_wq,
+			&di->chargalg_work);
+		break;
+	case 2:
+		/* Enable USB charging */
+		di->susp_status.usb_suspended = false;
+		di->susp_status.suspended_change = true;
+		/* Trigger a state change */
+		queue_work(di->chargalg_wq,
+			&di->chargalg_work);
 		break;
+	default:
+		dev_info(di->dev, "Wrong input\n"
+			"Enter 0. Disable AC/USB Charging\n"
+			"1. Enable AC charging\n"
+			"2. Enable USB Charging\n");
 	};
 	return strlen(buf);
 }
 
-static struct attribute abx500_chargalg_en_charger = \
+static struct abx500_chargalg_sysfs_entry abx500_chargalg_en_charger =
+	__ATTR(chargalg, 0644, abx500_chargalg_en_show,
+				abx500_chargalg_en_store);
+
+static struct abx500_chargalg_sysfs_entry abx500_chargalg_curr_step =
+	__ATTR(chargalg_curr_step, 0644, abx500_chargalg_curr_step_show,
+					abx500_chargalg_curr_step_store);
+
+static ssize_t abx500_chargalg_sysfs_show(struct kobject *kobj,
+	struct attribute *attr, char *buf)
 {
-	.name = "chargalg",
-	.mode = S_IRUGO | S_IWUSR,
-};
+	struct abx500_chargalg_sysfs_entry *entry = container_of(attr,
+		struct abx500_chargalg_sysfs_entry, attr);
+
+	struct abx500_chargalg *di = container_of(kobj,
+		struct abx500_chargalg, chargalg_kobject);
+
+	if (!entry->show)
+		return -EIO;
+
+	return entry->show(di, buf);
+}
+
+static ssize_t abx500_chargalg_sysfs_charger(struct kobject *kobj,
+	struct attribute *attr, const char *buf, size_t length)
+{
+	struct abx500_chargalg_sysfs_entry *entry = container_of(attr,
+		struct abx500_chargalg_sysfs_entry, attr);
+
+	struct abx500_chargalg *di = container_of(kobj,
+		struct abx500_chargalg, chargalg_kobject);
+
+	if (!entry->store)
+		return -EIO;
+
+	return entry->store(di, buf, length);
+}
 
 static struct attribute *abx500_chargalg_chg[] = {
-	&abx500_chargalg_en_charger,
-	NULL
+	&abx500_chargalg_en_charger.attr,
+	&abx500_chargalg_curr_step.attr,
+	NULL,
 };
 
 static const struct sysfs_ops abx500_chargalg_sysfs_ops = {
@@ -1832,10 +2024,16 @@ static int abx500_chargalg_remove(struct platform_device *pdev)
 	/* sysfs interface to enable/disbale charging from user space */
 	abx500_chargalg_sysfs_exit(di);
 
+	hrtimer_cancel(&di->safety_timer);
+	hrtimer_cancel(&di->maintenance_timer);
+
+	cancel_delayed_work_sync(&di->chargalg_periodic_work);
+	cancel_delayed_work_sync(&di->chargalg_wd_work);
+	cancel_work_sync(&di->chargalg_work);
+
 	/* Delete the work queue */
 	destroy_workqueue(di->chargalg_wq);
 
-	flush_scheduled_work();
 	power_supply_unregister(&di->chargalg_psy);
 	platform_set_drvdata(pdev, NULL);
 
@@ -1873,8 +2071,9 @@ static int abx500_chargalg_probe(struct platform_device *pdev)
 		}
 	}
 
-	/* get device struct */
+	/* get device struct and parent */
 	di->dev = &pdev->dev;
+	di->parent = dev_get_drvdata(pdev->dev.parent);
 
 	/* chargalg supply */
 	di->chargalg_psy.name = "abx500_chargalg";
@@ -1888,15 +2087,13 @@ static int abx500_chargalg_probe(struct platform_device *pdev)
 		abx500_chargalg_external_power_changed;
 
 	/* Initilialize safety timer */
-	init_timer(&di->safety_timer);
+	hrtimer_init(&di->safety_timer, CLOCK_REALTIME, HRTIMER_MODE_ABS);
 	di->safety_timer.function = abx500_chargalg_safety_timer_expired;
-	di->safety_timer.data = (unsigned long) di;
 
 	/* Initilialize maintenance timer */
-	init_timer(&di->maintenance_timer);
+	hrtimer_init(&di->maintenance_timer, CLOCK_REALTIME, HRTIMER_MODE_ABS);
 	di->maintenance_timer.function =
 		abx500_chargalg_maintenance_timer_expired;
-	di->maintenance_timer.data = (unsigned long) di;
 
 	/* Create a work queue for the chargalg */
 	di->chargalg_wq =
@@ -1933,6 +2130,7 @@ static int abx500_chargalg_probe(struct platform_device *pdev)
 		dev_err(di->dev, "failed to create sysfs entry\n");
 		goto free_psy;
 	}
+	di->curr_status.curr_step = CHARGALG_CURR_STEP_HIGH;
 
 	/* Run the charging algorithm */
 	queue_delayed_work(di->chargalg_wq, &di->chargalg_periodic_work, 0);
@@ -1964,18 +2162,7 @@ static struct platform_driver abx500_chargalg_driver = {
 	},
 };
 
-static int __init abx500_chargalg_init(void)
-{
-	return platform_driver_register(&abx500_chargalg_driver);
-}
-
-static void __exit abx500_chargalg_exit(void)
-{
-	platform_driver_unregister(&abx500_chargalg_driver);
-}
-
-module_init(abx500_chargalg_init);
-module_exit(abx500_chargalg_exit);
+module_platform_driver(abx500_chargalg_driver);
 
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Johan Palsson, Karl Komierowski");
diff --git a/drivers/power/charger-manager.c b/drivers/power/charger-manager.c
index 8acc3f8d303c..fefc39fe42be 100644
--- a/drivers/power/charger-manager.c
+++ b/drivers/power/charger-manager.c
@@ -1485,13 +1485,12 @@ static int charger_manager_probe(struct platform_device *pdev)
 
 	/* Basic Values. Unspecified are Null or 0 */
 	cm->dev = &pdev->dev;
-	cm->desc = kzalloc(sizeof(struct charger_desc), GFP_KERNEL);
+	cm->desc = kmemdup(desc, sizeof(struct charger_desc), GFP_KERNEL);
 	if (!cm->desc) {
 		dev_err(&pdev->dev, "Cannot allocate memory.\n");
 		ret = -ENOMEM;
 		goto err_alloc_desc;
 	}
-	memcpy(cm->desc, desc, sizeof(struct charger_desc));
 	cm->last_temp_mC = INT_MIN; /* denotes "unmeasured, yet" */
 
 	/*
diff --git a/drivers/power/da9030_battery.c b/drivers/power/da9030_battery.c
index e8c5a391a498..ae6c41835ee6 100644
--- a/drivers/power/da9030_battery.c
+++ b/drivers/power/da9030_battery.c
@@ -505,7 +505,7 @@ static int da9030_battery_probe(struct platform_device *pdev)
 	    pdata->charge_millivolt > 4350)
 		return -EINVAL;
 
-	charger = kzalloc(sizeof(*charger), GFP_KERNEL);
+	charger = devm_kzalloc(&pdev->dev, sizeof(*charger), GFP_KERNEL);
 	if (charger == NULL)
 		return -ENOMEM;
 
@@ -557,8 +557,6 @@ err_notifier:
 	cancel_delayed_work(&charger->work);
 
 err_charger_init:
-	kfree(charger);
-
 	return ret;
 }
 
@@ -575,8 +573,6 @@ static int da9030_battery_remove(struct platform_device *dev)
 	da9030_set_charge(charger, 0);
 	power_supply_unregister(&charger->psy);
 
-	kfree(charger);
-
 	return 0;
 }
 
diff --git a/drivers/power/da9052-battery.c b/drivers/power/da9052-battery.c
index 08193feb3b08..f8f4c0f7c17d 100644
--- a/drivers/power/da9052-battery.c
+++ b/drivers/power/da9052-battery.c
@@ -594,7 +594,8 @@ static s32 da9052_bat_probe(struct platform_device *pdev)
 	int ret;
 	int i;
 
-	bat = kzalloc(sizeof(struct da9052_battery), GFP_KERNEL);
+	bat = devm_kzalloc(&pdev->dev, sizeof(struct da9052_battery),
+				GFP_KERNEL);
 	if (!bat)
 		return -ENOMEM;
 
@@ -635,7 +636,6 @@ err:
 	while (--i >= 0)
 		da9052_free_irq(bat->da9052, da9052_bat_irq_bits[i], bat);
 
-	kfree(bat);
 	return ret;
 }
 static int da9052_bat_remove(struct platform_device *pdev)
@@ -647,7 +647,6 @@ static int da9052_bat_remove(struct platform_device *pdev)
 		da9052_free_irq(bat->da9052, da9052_bat_irq_bits[i], bat);
 
 	power_supply_unregister(&bat->psy);
-	kfree(bat);
 
 	return 0;
 }
diff --git a/drivers/power/ds2760_battery.c b/drivers/power/ds2760_battery.c
index 704e652072be..85b4e6eca0b1 100644
--- a/drivers/power/ds2760_battery.c
+++ b/drivers/power/ds2760_battery.c
@@ -512,7 +512,7 @@ static int ds2760_battery_probe(struct platform_device *pdev)
 	int retval = 0;
 	struct ds2760_device_info *di;
 
-	di = kzalloc(sizeof(*di), GFP_KERNEL);
+	di = devm_kzalloc(&pdev->dev, sizeof(*di), GFP_KERNEL);
 	if (!di) {
 		retval = -ENOMEM;
 		goto di_alloc_failed;
@@ -576,7 +576,6 @@ static int ds2760_battery_probe(struct platform_device *pdev)
 workqueue_failed:
 	power_supply_unregister(&di->bat);
 batt_failed:
-	kfree(di);
 di_alloc_failed:
 success:
 	return retval;
@@ -590,7 +589,6 @@ static int ds2760_battery_remove(struct platform_device *pdev)
 	cancel_delayed_work_sync(&di->set_charged_work);
 	destroy_workqueue(di->monitor_wqueue);
 	power_supply_unregister(&di->bat);
-	kfree(di);
 
 	return 0;
 }
diff --git a/drivers/power/ds2780_battery.c b/drivers/power/ds2780_battery.c
index 8b6c4539e7f4..9f418fa879e5 100644
--- a/drivers/power/ds2780_battery.c
+++ b/drivers/power/ds2780_battery.c
@@ -760,7 +760,7 @@ static int ds2780_battery_probe(struct platform_device *pdev)
 	int ret = 0;
 	struct ds2780_device_info *dev_info;
 
-	dev_info = kzalloc(sizeof(*dev_info), GFP_KERNEL);
+	dev_info = devm_kzalloc(&pdev->dev, sizeof(*dev_info), GFP_KERNEL);
 	if (!dev_info) {
 		ret = -ENOMEM;
 		goto fail;
@@ -779,7 +779,7 @@ static int ds2780_battery_probe(struct platform_device *pdev)
 	ret = power_supply_register(&pdev->dev, &dev_info->bat);
 	if (ret) {
 		dev_err(dev_info->dev, "failed to register battery\n");
-		goto fail_free_info;
+		goto fail;
 	}
 
 	ret = sysfs_create_group(&dev_info->bat.dev->kobj, &ds2780_attr_group);
@@ -813,8 +813,6 @@ fail_remove_group:
 	sysfs_remove_group(&dev_info->bat.dev->kobj, &ds2780_attr_group);
 fail_unregister:
 	power_supply_unregister(&dev_info->bat);
-fail_free_info:
-	kfree(dev_info);
 fail:
 	return ret;
 }
@@ -828,7 +826,6 @@ static int ds2780_battery_remove(struct platform_device *pdev)
 
 	power_supply_unregister(&dev_info->bat);
 
-	kfree(dev_info);
 	return 0;
 }
 
diff --git a/drivers/power/ds2782_battery.c b/drivers/power/ds2782_battery.c
index c09e7726c96c..563174891c90 100644
--- a/drivers/power/ds2782_battery.c
+++ b/drivers/power/ds2782_battery.c
@@ -332,32 +332,32 @@ static int ds278x_battery_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 
-static int ds278x_suspend(struct i2c_client *client,
-		pm_message_t state)
+static int ds278x_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct ds278x_info *info = i2c_get_clientdata(client);
 
 	cancel_delayed_work(&info->bat_work);
 	return 0;
 }
 
-static int ds278x_resume(struct i2c_client *client)
+static int ds278x_resume(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct ds278x_info *info = i2c_get_clientdata(client);
 
 	schedule_delayed_work(&info->bat_work, DS278x_DELAY);
 	return 0;
 }
 
-#else
-
-#define ds278x_suspend NULL
-#define ds278x_resume NULL
-
-#endif /* CONFIG_PM */
+static SIMPLE_DEV_PM_OPS(ds278x_battery_pm_ops, ds278x_suspend, ds278x_resume);
+#define DS278X_BATTERY_PM_OPS (&ds278x_battery_pm_ops)
 
+#else
+#define DS278X_BATTERY_PM_OPS NULL
+#endif /* CONFIG_PM_SLEEP */
 
 enum ds278x_num_id {
 	DS2782 = 0,
@@ -460,11 +460,10 @@ MODULE_DEVICE_TABLE(i2c, ds278x_id);
 static struct i2c_driver ds278x_battery_driver = {
 	.driver 	= {
 		.name	= "ds2782-battery",
+		.pm	= DS278X_BATTERY_PM_OPS,
 	},
 	.probe		= ds278x_battery_probe,
 	.remove		= ds278x_battery_remove,
-	.suspend	= ds278x_suspend,
-	.resume		= ds278x_resume,
 	.id_table	= ds278x_id,
 };
 module_i2c_driver(ds278x_battery_driver);
diff --git a/drivers/power/goldfish_battery.c b/drivers/power/goldfish_battery.c
index c10f460f986f..29eba88a2963 100644
--- a/drivers/power/goldfish_battery.c
+++ b/drivers/power/goldfish_battery.c
@@ -178,7 +178,7 @@ static int goldfish_battery_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	data->reg_base = devm_ioremap(&pdev->dev, r->start, r->end - r->start + 1);
+	data->reg_base = devm_ioremap(&pdev->dev, r->start, resource_size(r));
 	if (data->reg_base == NULL) {
 		dev_err(&pdev->dev, "unable to remap MMIO\n");
 		return -ENOMEM;
diff --git a/drivers/power/gpio-charger.c b/drivers/power/gpio-charger.c
index e3e40a9f3af2..e9883eeeee76 100644
--- a/drivers/power/gpio-charger.c
+++ b/drivers/power/gpio-charger.c
@@ -86,7 +86,8 @@ static int gpio_charger_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	gpio_charger = kzalloc(sizeof(*gpio_charger), GFP_KERNEL);
+	gpio_charger = devm_kzalloc(&pdev->dev, sizeof(*gpio_charger),
+					GFP_KERNEL);
 	if (!gpio_charger) {
 		dev_err(&pdev->dev, "Failed to alloc driver structure\n");
 		return -ENOMEM;
@@ -140,7 +141,6 @@ static int gpio_charger_probe(struct platform_device *pdev)
 err_gpio_free:
 	gpio_free(pdata->gpio);
 err_free:
-	kfree(gpio_charger);
 	return ret;
 }
 
@@ -156,7 +156,6 @@ static int gpio_charger_remove(struct platform_device *pdev)
 	gpio_free(gpio_charger->pdata->gpio);
 
 	platform_set_drvdata(pdev, NULL);
-	kfree(gpio_charger);
 
 	return 0;
 }
diff --git a/drivers/power/isp1704_charger.c b/drivers/power/isp1704_charger.c
index 176ad59d99f5..fc04d191579b 100644
--- a/drivers/power/isp1704_charger.c
+++ b/drivers/power/isp1704_charger.c
@@ -411,7 +411,7 @@ static int isp1704_charger_probe(struct platform_device *pdev)
 	struct isp1704_charger	*isp;
 	int			ret = -ENODEV;
 
-	isp = kzalloc(sizeof *isp, GFP_KERNEL);
+	isp = devm_kzalloc(&pdev->dev, sizeof(*isp), GFP_KERNEL);
 	if (!isp)
 		return -ENOMEM;
 
@@ -477,8 +477,6 @@ fail1:
 	isp1704_charger_set_power(isp, 0);
 	usb_put_phy(isp->phy);
 fail0:
-	kfree(isp);
-
 	dev_err(&pdev->dev, "failed to register isp1704 with error %d\n", ret);
 
 	return ret;
@@ -492,7 +490,6 @@ static int isp1704_charger_remove(struct platform_device *pdev)
 	power_supply_unregister(&isp->psy);
 	usb_put_phy(isp->phy);
 	isp1704_charger_set_power(isp, 0);
-	kfree(isp);
 
 	return 0;
 }
diff --git a/drivers/power/lp8788-charger.c b/drivers/power/lp8788-charger.c
index 6d1f452810b8..ed49b50b220b 100644
--- a/drivers/power/lp8788-charger.c
+++ b/drivers/power/lp8788-charger.c
@@ -49,7 +49,6 @@
 #define LP8788_CHG_START		0x11
 #define LP8788_CHG_END			0x1C
 
-#define LP8788_BUF_SIZE			40
 #define LP8788_ISEL_MAX			23
 #define LP8788_ISEL_STEP		50
 #define LP8788_VTERM_MIN		4100
@@ -633,7 +632,7 @@ static ssize_t lp8788_show_charger_status(struct device *dev,
 	lp8788_read_byte(pchg->lp, LP8788_CHG_STATUS, &data);
 	state = (data & LP8788_CHG_STATE_M) >> LP8788_CHG_STATE_S;
 
-	return scnprintf(buf, LP8788_BUF_SIZE, "%s\n", desc[state]);
+	return scnprintf(buf, PAGE_SIZE, "%s\n", desc[state]);
 }
 
 static ssize_t lp8788_show_eoc_time(struct device *dev,
@@ -647,7 +646,7 @@ static ssize_t lp8788_show_eoc_time(struct device *dev,
 	lp8788_read_byte(pchg->lp, LP8788_CHG_EOC, &val);
 	val = (val & LP8788_CHG_EOC_TIME_M) >> LP8788_CHG_EOC_TIME_S;
 
-	return scnprintf(buf, LP8788_BUF_SIZE, "End Of Charge Time: %s\n",
+	return scnprintf(buf, PAGE_SIZE, "End Of Charge Time: %s\n",
 			stime[val]);
 }
 
@@ -667,8 +666,7 @@ static ssize_t lp8788_show_eoc_level(struct device *dev,
 	val = (val & LP8788_CHG_EOC_LEVEL_M) >> LP8788_CHG_EOC_LEVEL_S;
 	level = mode ? abs_level[val] : relative_level[val];
 
-	return scnprintf(buf, LP8788_BUF_SIZE, "End Of Charge Level: %s\n",
-			level);
+	return scnprintf(buf, PAGE_SIZE, "End Of Charge Level: %s\n", level);
 }
 
 static DEVICE_ATTR(charger_status, S_IRUSR, lp8788_show_charger_status, NULL);
diff --git a/drivers/power/max17040_battery.c b/drivers/power/max17040_battery.c
index 74a0bd9bc162..c7ff6d67f158 100644
--- a/drivers/power/max17040_battery.c
+++ b/drivers/power/max17040_battery.c
@@ -246,31 +246,34 @@ static int max17040_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 
-static int max17040_suspend(struct i2c_client *client,
-		pm_message_t state)
+static int max17040_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct max17040_chip *chip = i2c_get_clientdata(client);
 
 	cancel_delayed_work(&chip->work);
 	return 0;
 }
 
-static int max17040_resume(struct i2c_client *client)
+static int max17040_resume(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct max17040_chip *chip = i2c_get_clientdata(client);
 
 	schedule_delayed_work(&chip->work, MAX17040_DELAY);
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(max17040_pm_ops, max17040_suspend, max17040_resume);
+#define MAX17040_PM_OPS (&max17040_pm_ops)
+
 #else
 
-#define max17040_suspend NULL
-#define max17040_resume NULL
+#define MAX17040_PM_OPS NULL
 
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 static const struct i2c_device_id max17040_id[] = {
 	{ "max17040", 0 },
@@ -281,11 +284,10 @@ MODULE_DEVICE_TABLE(i2c, max17040_id);
 static struct i2c_driver max17040_i2c_driver = {
 	.driver	= {
 		.name	= "max17040",
+		.pm	= MAX17040_PM_OPS,
 	},
 	.probe		= max17040_probe,
 	.remove		= max17040_remove,
-	.suspend	= max17040_suspend,
-	.resume		= max17040_resume,
 	.id_table	= max17040_id,
 };
 module_i2c_driver(max17040_i2c_driver);
diff --git a/drivers/power/max8903_charger.c b/drivers/power/max8903_charger.c
index 14e2b96d93b0..08f0d7909b6b 100644
--- a/drivers/power/max8903_charger.c
+++ b/drivers/power/max8903_charger.c
@@ -189,7 +189,7 @@ static int max8903_probe(struct platform_device *pdev)
 	int ta_in = 0;
 	int usb_in = 0;
 
-	data = kzalloc(sizeof(struct max8903_data), GFP_KERNEL);
+	data = devm_kzalloc(dev, sizeof(struct max8903_data), GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(dev, "Cannot allocate memory.\n");
 		return -ENOMEM;
@@ -341,7 +341,6 @@ err_dc_irq:
 err_psy:
 	power_supply_unregister(&data->psy);
 err:
-	kfree(data);
 	return ret;
 }
 
@@ -359,7 +358,6 @@ static int max8903_remove(struct platform_device *pdev)
 		if (pdata->dc_valid)
 			free_irq(gpio_to_irq(pdata->dok), data);
 		power_supply_unregister(&data->psy);
-		kfree(data);
 	}
 
 	return 0;
diff --git a/drivers/power/max8925_power.c b/drivers/power/max8925_power.c
index 665cdc76c265..0ee1e14f76e9 100644
--- a/drivers/power/max8925_power.c
+++ b/drivers/power/max8925_power.c
@@ -489,7 +489,8 @@ static int max8925_power_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	info = kzalloc(sizeof(struct max8925_power_info), GFP_KERNEL);
+	info = devm_kzalloc(&pdev->dev, sizeof(struct max8925_power_info),
+				GFP_KERNEL);
 	if (!info)
 		return -ENOMEM;
 	info->chip = chip;
@@ -546,7 +547,6 @@ out_battery:
 out_usb:
 	power_supply_unregister(&info->ac);
 out:
-	kfree(info);
 	return ret;
 }
 
@@ -559,7 +559,6 @@ static int max8925_power_remove(struct platform_device *pdev)
 		power_supply_unregister(&info->usb);
 		power_supply_unregister(&info->battery);
 		max8925_deinit_charger(info);
-		kfree(info);
 	}
 	return 0;
 }
diff --git a/drivers/power/max8997_charger.c b/drivers/power/max8997_charger.c
index e757885b620c..4bdedfed936d 100644
--- a/drivers/power/max8997_charger.c
+++ b/drivers/power/max8997_charger.c
@@ -138,7 +138,8 @@ static int max8997_battery_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	charger = kzalloc(sizeof(struct charger_data), GFP_KERNEL);
+	charger = devm_kzalloc(&pdev->dev, sizeof(struct charger_data),
+				GFP_KERNEL);
 	if (charger == NULL) {
 		dev_err(&pdev->dev, "Cannot allocate memory.\n");
 		return -ENOMEM;
@@ -158,13 +159,10 @@ static int max8997_battery_probe(struct platform_device *pdev)
 	ret = power_supply_register(&pdev->dev, &charger->battery);
 	if (ret) {
 		dev_err(&pdev->dev, "failed: power supply register\n");
-		goto err;
+		return ret;
 	}
 
 	return 0;
-err:
-	kfree(charger);
-	return ret;
 }
 
 static int max8997_battery_remove(struct platform_device *pdev)
@@ -172,7 +170,6 @@ static int max8997_battery_remove(struct platform_device *pdev)
 	struct charger_data *charger = platform_get_drvdata(pdev);
 
 	power_supply_unregister(&charger->battery);
-	kfree(charger);
 	return 0;
 }
 
diff --git a/drivers/power/max8998_charger.c b/drivers/power/max8998_charger.c
index bf677e3daec9..5017470c2fc9 100644
--- a/drivers/power/max8998_charger.c
+++ b/drivers/power/max8998_charger.c
@@ -88,7 +88,8 @@ static int max8998_battery_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	max8998 = kzalloc(sizeof(struct max8998_battery_data), GFP_KERNEL);
+	max8998 = devm_kzalloc(&pdev->dev, sizeof(struct max8998_battery_data),
+				GFP_KERNEL);
 	if (!max8998)
 		return -ENOMEM;
 
@@ -174,7 +175,6 @@ static int max8998_battery_probe(struct platform_device *pdev)
 
 	return 0;
 err:
-	kfree(max8998);
 	return ret;
 }
 
@@ -183,7 +183,6 @@ static int max8998_battery_remove(struct platform_device *pdev)
 	struct max8998_battery_data *max8998 = platform_get_drvdata(pdev);
 
 	power_supply_unregister(&max8998->battery);
-	kfree(max8998);
 
 	return 0;
 }
diff --git a/drivers/power/pcf50633-charger.c b/drivers/power/pcf50633-charger.c
index c2122a7ad065..17fd77f24b2a 100644
--- a/drivers/power/pcf50633-charger.c
+++ b/drivers/power/pcf50633-charger.c
@@ -373,7 +373,7 @@ static int pcf50633_mbc_probe(struct platform_device *pdev)
 	int i;
 	u8 mbcs1;
 
-	mbc = kzalloc(sizeof(*mbc), GFP_KERNEL);
+	mbc = devm_kzalloc(&pdev->dev, sizeof(*mbc), GFP_KERNEL);
 	if (!mbc)
 		return -ENOMEM;
 
@@ -413,7 +413,6 @@ static int pcf50633_mbc_probe(struct platform_device *pdev)
 	ret = power_supply_register(&pdev->dev, &mbc->adapter);
 	if (ret) {
 		dev_err(mbc->pcf->dev, "failed to register adapter\n");
-		kfree(mbc);
 		return ret;
 	}
 
@@ -421,7 +420,6 @@ static int pcf50633_mbc_probe(struct platform_device *pdev)
 	if (ret) {
 		dev_err(mbc->pcf->dev, "failed to register usb\n");
 		power_supply_unregister(&mbc->adapter);
-		kfree(mbc);
 		return ret;
 	}
 
@@ -430,7 +428,6 @@ static int pcf50633_mbc_probe(struct platform_device *pdev)
 		dev_err(mbc->pcf->dev, "failed to register ac\n");
 		power_supply_unregister(&mbc->adapter);
 		power_supply_unregister(&mbc->usb);
-		kfree(mbc);
 		return ret;
 	}
 
@@ -461,8 +458,6 @@ static int pcf50633_mbc_remove(struct platform_device *pdev)
 	power_supply_unregister(&mbc->adapter);
 	power_supply_unregister(&mbc->ac);
 
-	kfree(mbc);
-
 	return 0;
 }
 
diff --git a/drivers/power/pm2301_charger.c b/drivers/power/pm2301_charger.c
index ee346d443571..a44175139bbf 100644
--- a/drivers/power/pm2301_charger.c
+++ b/drivers/power/pm2301_charger.c
@@ -16,24 +16,25 @@
 #include <linux/slab.h>
 #include <linux/platform_device.h>
 #include <linux/power_supply.h>
-#include <linux/completion.h>
 #include <linux/regulator/consumer.h>
 #include <linux/err.h>
 #include <linux/i2c.h>
 #include <linux/workqueue.h>
-#include <linux/kobject.h>
-#include <linux/mfd/abx500.h>
 #include <linux/mfd/abx500/ab8500.h>
 #include <linux/mfd/abx500/ab8500-bm.h>
-#include <linux/mfd/abx500/ab8500-gpadc.h>
 #include <linux/mfd/abx500/ux500_chargalg.h>
 #include <linux/pm2301_charger.h>
 #include <linux/gpio.h>
+#include <linux/pm_runtime.h>
+#include <linux/pm.h>
 
 #include "pm2301_charger.h"
 
 #define to_pm2xxx_charger_ac_device_info(x) container_of((x), \
 		struct pm2xxx_charger, ac_chg)
+#define SLEEP_MIN		50
+#define SLEEP_MAX		100
+#define PM2XXX_AUTOSUSPEND_DELAY 500
 
 static int pm2xxx_interrupt_registers[] = {
 	PM2XXX_REG_INT1,
@@ -113,33 +114,24 @@ static const struct i2c_device_id pm2xxx_ident[] = {
 
 static void set_lpn_pin(struct pm2xxx_charger *pm2)
 {
-	if (pm2->ac.charger_connected)
-		return;
-	gpio_set_value(pm2->lpn_pin, 1);
-
-	return;
+	if (!pm2->ac.charger_connected && gpio_is_valid(pm2->lpn_pin)) {
+		gpio_set_value(pm2->lpn_pin, 1);
+		usleep_range(SLEEP_MIN, SLEEP_MAX);
+	}
 }
 
 static void clear_lpn_pin(struct pm2xxx_charger *pm2)
 {
-	if (pm2->ac.charger_connected)
-		return;
-	gpio_set_value(pm2->lpn_pin, 0);
-
-	return;
+	if (!pm2->ac.charger_connected && gpio_is_valid(pm2->lpn_pin))
+		gpio_set_value(pm2->lpn_pin, 0);
 }
 
 static int pm2xxx_reg_read(struct pm2xxx_charger *pm2, int reg, u8 *val)
 {
 	int ret;
-	/*
-	 * When AC adaptor is unplugged, the host
-	 * must put LPN high to be able to
-	 * communicate by I2C with PM2301
-	 * and receive I2C "acknowledge" from PM2301.
-	 */
-	mutex_lock(&pm2->lock);
-	set_lpn_pin(pm2);
+
+	/* wake up the device */
+	pm_runtime_get_sync(pm2->dev);
 
 	ret = i2c_smbus_read_i2c_block_data(pm2->config.pm2xxx_i2c, reg,
 				1, val);
@@ -147,8 +139,8 @@ static int pm2xxx_reg_read(struct pm2xxx_charger *pm2, int reg, u8 *val)
 		dev_err(pm2->dev, "Error reading register at 0x%x\n", reg);
 	else
 		ret = 0;
-	clear_lpn_pin(pm2);
-	mutex_unlock(&pm2->lock);
+
+	pm_runtime_put_sync(pm2->dev);
 
 	return ret;
 }
@@ -156,14 +148,9 @@ static int pm2xxx_reg_read(struct pm2xxx_charger *pm2, int reg, u8 *val)
 static int pm2xxx_reg_write(struct pm2xxx_charger *pm2, int reg, u8 val)
 {
 	int ret;
-	/*
-	 * When AC adaptor is unplugged, the host
-	 * must put LPN high to be able to
-	 * communicate by I2C with PM2301
-	 * and receive I2C "acknowledge" from PM2301.
-	 */
-	mutex_lock(&pm2->lock);
-	set_lpn_pin(pm2);
+
+	/* wake up the device */
+	pm_runtime_get_sync(pm2->dev);
 
 	ret = i2c_smbus_write_i2c_block_data(pm2->config.pm2xxx_i2c, reg,
 				1, &val);
@@ -171,8 +158,8 @@ static int pm2xxx_reg_write(struct pm2xxx_charger *pm2, int reg, u8 val)
 		dev_err(pm2->dev, "Error writing register at 0x%x\n", reg);
 	else
 		ret = 0;
-	clear_lpn_pin(pm2);
-	mutex_unlock(&pm2->lock);
+
+	pm_runtime_put_sync(pm2->dev);
 
 	return ret;
 }
@@ -192,11 +179,22 @@ static int pm2xxx_charging_disable_mngt(struct pm2xxx_charger *pm2)
 {
 	int ret;
 
+	/* Disable SW EOC ctrl */
+	ret = pm2xxx_reg_write(pm2, PM2XXX_SW_CTRL_REG, PM2XXX_SWCTRL_HW);
+	if (ret < 0) {
+		dev_err(pm2->dev, "%s pm2xxx write failed\n", __func__);
+		return ret;
+	}
+
 	/* Disable charging */
 	ret = pm2xxx_reg_write(pm2, PM2XXX_BATT_CTRL_REG2,
 			(PM2XXX_CH_AUTO_RESUME_DIS | PM2XXX_CHARGER_DIS));
+	if (ret < 0) {
+		dev_err(pm2->dev, "%s pm2xxx write failed\n", __func__);
+		return ret;
+	}
 
-	return ret;
+	return 0;
 }
 
 static int pm2xxx_charger_batt_therm_mngt(struct pm2xxx_charger *pm2, int val)
@@ -216,21 +214,14 @@ int pm2xxx_charger_die_therm_mngt(struct pm2xxx_charger *pm2, int val)
 
 static int pm2xxx_charger_ovv_mngt(struct pm2xxx_charger *pm2, int val)
 {
-	int ret = 0;
+	dev_err(pm2->dev, "Overvoltage detected\n");
+	pm2->flags.ovv = true;
+	power_supply_changed(&pm2->ac_chg.psy);
 
-	pm2->failure_input_ovv++;
-	if (pm2->failure_input_ovv < 4) {
-		ret = pm2xxx_charging_enable_mngt(pm2);
-		goto out;
-	} else {
-		pm2->failure_input_ovv = 0;
-		dev_err(pm2->dev, "Overvoltage detected\n");
-		pm2->flags.ovv = true;
-		power_supply_changed(&pm2->ac_chg.psy);
-	}
+	/* Schedule a new HW failure check */
+	queue_delayed_work(pm2->charger_wq, &pm2->check_hw_failure_work, 0);
 
-out:
-	return ret;
+	return 0;
 }
 
 static int pm2xxx_charger_wd_exp_mngt(struct pm2xxx_charger *pm2, int val)
@@ -245,13 +236,29 @@ static int pm2xxx_charger_wd_exp_mngt(struct pm2xxx_charger *pm2, int val)
 
 static int pm2xxx_charger_vbat_lsig_mngt(struct pm2xxx_charger *pm2, int val)
 {
+	int ret;
+
 	switch (val) {
 	case PM2XXX_INT1_ITVBATLOWR:
 		dev_dbg(pm2->dev, "VBAT grows above VBAT_LOW level\n");
+		/* Enable SW EOC ctrl */
+		ret = pm2xxx_reg_write(pm2, PM2XXX_SW_CTRL_REG,
+							PM2XXX_SWCTRL_SW);
+		if (ret < 0) {
+			dev_err(pm2->dev, "%s pm2xxx write failed\n", __func__);
+			return ret;
+		}
 		break;
 
 	case PM2XXX_INT1_ITVBATLOWF:
 		dev_dbg(pm2->dev, "VBAT drops below VBAT_LOW level\n");
+		/* Disable SW EOC ctrl */
+		ret = pm2xxx_reg_write(pm2, PM2XXX_SW_CTRL_REG,
+							PM2XXX_SWCTRL_HW);
+		if (ret < 0) {
+			dev_err(pm2->dev, "%s pm2xxx write failed\n", __func__);
+			return ret;
+		}
 		break;
 
 	default:
@@ -322,16 +329,27 @@ static int pm2_int_reg0(void *pm2_data, int val)
 	struct pm2xxx_charger *pm2 = pm2_data;
 	int ret = 0;
 
-	if (val & (PM2XXX_INT1_ITVBATLOWR | PM2XXX_INT1_ITVBATLOWF)) {
-		ret = pm2xxx_charger_vbat_lsig_mngt(pm2, val &
-			(PM2XXX_INT1_ITVBATLOWR | PM2XXX_INT1_ITVBATLOWF));
+	if (val & PM2XXX_INT1_ITVBATLOWR) {
+		ret = pm2xxx_charger_vbat_lsig_mngt(pm2,
+						PM2XXX_INT1_ITVBATLOWR);
+		if (ret < 0)
+			goto out;
+	}
+
+	if (val & PM2XXX_INT1_ITVBATLOWF) {
+		ret = pm2xxx_charger_vbat_lsig_mngt(pm2,
+						PM2XXX_INT1_ITVBATLOWF);
+		if (ret < 0)
+			goto out;
 	}
 
 	if (val & PM2XXX_INT1_ITVBATDISCONNECT) {
 		ret = pm2xxx_charger_bat_disc_mngt(pm2,
 				PM2XXX_INT1_ITVBATDISCONNECT);
+		if (ret < 0)
+			goto out;
 	}
-
+out:
 	return ret;
 }
 
@@ -447,7 +465,6 @@ static int pm2_int_reg5(void *pm2_data, int val)
 	struct pm2xxx_charger *pm2 = pm2_data;
 	int ret = 0;
 
-
 	if (val & (PM2XXX_INT6_ITVPWR2DROP | PM2XXX_INT6_ITVPWR1DROP)) {
 		dev_dbg(pm2->dev, "VMPWR drop to VBAT level\n");
 	}
@@ -468,14 +485,22 @@ static irqreturn_t  pm2xxx_irq_int(int irq, void *data)
 	struct pm2xxx_interrupts *interrupt = pm2->pm2_int;
 	int i;
 
-	for (i = 0; i < PM2XXX_NUM_INT_REG; i++) {
-		 pm2xxx_reg_read(pm2,
+	/* wake up the device */
+	pm_runtime_get_sync(pm2->dev);
+
+	do {
+		for (i = 0; i < PM2XXX_NUM_INT_REG; i++) {
+			pm2xxx_reg_read(pm2,
 				pm2xxx_interrupt_registers[i],
 				&(interrupt->reg[i]));
 
-		if (interrupt->reg[i] > 0)
-			interrupt->handler[i](pm2, interrupt->reg[i]);
-	}
+			if (interrupt->reg[i] > 0)
+				interrupt->handler[i](pm2, interrupt->reg[i]);
+		}
+	} while (gpio_get_value(pm2->pdata->gpio_irq_number) == 0);
+
+	pm_runtime_mark_last_busy(pm2->dev);
+	pm_runtime_put_autosuspend(pm2->dev);
 
 	return IRQ_HANDLED;
 }
@@ -592,6 +617,8 @@ static int pm2xxx_charger_ac_get_property(struct power_supply *psy,
 			val->intval = POWER_SUPPLY_HEALTH_DEAD;
 		else if (pm2->flags.main_thermal_prot)
 			val->intval = POWER_SUPPLY_HEALTH_OVERHEAT;
+		else if (pm2->flags.ovv)
+			val->intval = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
 		else
 			val->intval = POWER_SUPPLY_HEALTH_GOOD;
 		break;
@@ -674,10 +701,6 @@ static int pm2xxx_charging_init(struct pm2xxx_charger *pm2)
 	ret = pm2xxx_reg_write(pm2, PM2XXX_BATT_LOW_LEV_COMP_REG,
 		PM2XXX_VBAT_LOW_MONITORING_ENA);
 
-	/* Disable LED */
-	ret = pm2xxx_reg_write(pm2, PM2XXX_LED_CTRL_REG,
-		PM2XXX_LED_SELECT_DIS);
-
 	return ret;
 }
 
@@ -822,10 +845,54 @@ static void pm2xxx_charger_ac_work(struct work_struct *work)
 	sysfs_notify(&pm2->ac_chg.psy.dev->kobj, NULL, "present");
 };
 
+static void pm2xxx_charger_check_hw_failure_work(struct work_struct *work)
+{
+	u8 reg_value;
+
+	struct pm2xxx_charger *pm2 = container_of(work,
+		struct pm2xxx_charger, check_hw_failure_work.work);
+
+	if (pm2->flags.ovv) {
+		pm2xxx_reg_read(pm2, PM2XXX_SRCE_REG_INT4, &reg_value);
+
+		if (!(reg_value & (PM2XXX_INT4_S_ITVPWR1OVV |
+					PM2XXX_INT4_S_ITVPWR2OVV))) {
+			pm2->flags.ovv = false;
+			power_supply_changed(&pm2->ac_chg.psy);
+		}
+	}
+
+	/* If we still have a failure, schedule a new check */
+	if (pm2->flags.ovv) {
+		queue_delayed_work(pm2->charger_wq,
+			&pm2->check_hw_failure_work, round_jiffies(HZ));
+	}
+}
+
 static void pm2xxx_charger_check_main_thermal_prot_work(
 	struct work_struct *work)
 {
-};
+	int ret;
+	u8 val;
+
+	struct pm2xxx_charger *pm2 = container_of(work, struct pm2xxx_charger,
+					check_main_thermal_prot_work);
+
+	/* Check if die temp warning is still active */
+	ret = pm2xxx_reg_read(pm2, PM2XXX_SRCE_REG_INT5, &val);
+	if (ret < 0) {
+		dev_err(pm2->dev, "%s pm2xxx read failed\n", __func__);
+		return;
+	}
+	if (val & (PM2XXX_INT5_S_ITTHERMALWARNINGRISE
+			| PM2XXX_INT5_S_ITTHERMALSHUTDOWNRISE))
+		pm2->flags.main_thermal_prot = true;
+	else if (val & (PM2XXX_INT5_S_ITTHERMALWARNINGFALL
+				| PM2XXX_INT5_S_ITTHERMALSHUTDOWNFALL))
+		pm2->flags.main_thermal_prot = false;
+
+	power_supply_changed(&pm2->ac_chg.psy);
+}
 
 static struct pm2xxx_interrupts pm2xxx_int = {
 	.handler[0] = pm2_int_reg0,
@@ -840,24 +907,105 @@ static struct pm2xxx_irq pm2xxx_charger_irq[] = {
 	{"PM2XXX_IRQ_INT", pm2xxx_irq_int},
 };
 
-static int pm2xxx_wall_charger_resume(struct i2c_client *i2c_client)
+#ifdef CONFIG_PM
+
+#ifdef CONFIG_PM_SLEEP
+
+static int pm2xxx_wall_charger_resume(struct device *dev)
 {
+	struct i2c_client *i2c_client = to_i2c_client(dev);
+	struct pm2xxx_charger *pm2;
+
+	pm2 =  (struct pm2xxx_charger *)i2c_get_clientdata(i2c_client);
+	set_lpn_pin(pm2);
+
+	/* If we still have a HW failure, schedule a new check */
+	if (pm2->flags.ovv)
+		queue_delayed_work(pm2->charger_wq,
+				&pm2->check_hw_failure_work, 0);
+
 	return 0;
 }
 
-static int pm2xxx_wall_charger_suspend(struct i2c_client *i2c_client,
-	pm_message_t state)
+static int pm2xxx_wall_charger_suspend(struct device *dev)
 {
+	struct i2c_client *i2c_client = to_i2c_client(dev);
+	struct pm2xxx_charger *pm2;
+
+	pm2 =  (struct pm2xxx_charger *)i2c_get_clientdata(i2c_client);
+	clear_lpn_pin(pm2);
+
+	/* Cancel any pending HW failure check */
+	if (delayed_work_pending(&pm2->check_hw_failure_work))
+		cancel_delayed_work(&pm2->check_hw_failure_work);
+
+	flush_work(&pm2->ac_work);
+	flush_work(&pm2->check_main_thermal_prot_work);
+
 	return 0;
 }
 
-static int __devinit pm2xxx_wall_charger_probe(struct i2c_client *i2c_client,
+#endif
+
+#ifdef CONFIG_PM_RUNTIME
+
+static int  pm2xxx_runtime_suspend(struct device *dev)
+{
+	struct i2c_client *pm2xxx_i2c_client = to_i2c_client(dev);
+	struct pm2xxx_charger *pm2;
+	int ret = 0;
+
+	pm2 = (struct pm2xxx_charger *)i2c_get_clientdata(pm2xxx_i2c_client);
+	if (!pm2) {
+		dev_err(pm2->dev, "no pm2xxx_charger data supplied\n");
+		ret = -EINVAL;
+		return ret;
+	}
+
+	clear_lpn_pin(pm2);
+
+	return ret;
+}
+
+static int  pm2xxx_runtime_resume(struct device *dev)
+{
+	struct i2c_client *pm2xxx_i2c_client = to_i2c_client(dev);
+	struct pm2xxx_charger *pm2;
+	int ret = 0;
+
+	pm2 = (struct pm2xxx_charger *)i2c_get_clientdata(pm2xxx_i2c_client);
+	if (!pm2) {
+		dev_err(pm2->dev, "no pm2xxx_charger data supplied\n");
+		ret = -EINVAL;
+		return ret;
+	}
+
+	if (gpio_is_valid(pm2->lpn_pin) && gpio_get_value(pm2->lpn_pin) == 0)
+		set_lpn_pin(pm2);
+
+	return ret;
+}
+
+#endif
+
+static const struct dev_pm_ops pm2xxx_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm2xxx_wall_charger_suspend,
+		pm2xxx_wall_charger_resume)
+	SET_RUNTIME_PM_OPS(pm2xxx_runtime_suspend, pm2xxx_runtime_resume, NULL)
+};
+#define  PM2XXX_PM_OPS (&pm2xxx_pm_ops)
+#else
+#define  PM2XXX_PM_OPS  NULL
+#endif
+
+static int pm2xxx_wall_charger_probe(struct i2c_client *i2c_client,
 		const struct i2c_device_id *id)
 {
 	struct pm2xxx_platform_data *pl_data = i2c_client->dev.platform_data;
 	struct pm2xxx_charger *pm2;
 	int ret = 0;
 	u8 val;
+	int i;
 
 	pm2 = kzalloc(sizeof(struct pm2xxx_charger), GFP_KERNEL);
 	if (!pm2) {
@@ -867,7 +1015,6 @@ static int __devinit pm2xxx_wall_charger_probe(struct i2c_client *i2c_client,
 
 	/* get parent data */
 	pm2->dev = &i2c_client->dev;
-	pm2->gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
 
 	pm2->pm2_int = &pm2xxx_int;
 
@@ -889,14 +1036,6 @@ static int __devinit pm2xxx_wall_charger_probe(struct i2c_client *i2c_client,
 
 	pm2->bat = pl_data->battery;
 
-	/*get lpn GPIO from platform data*/
-	if (!pm2->pdata->lpn_gpio) {
-		dev_err(pm2->dev, "no lpn gpio data supplied\n");
-		ret = -EINVAL;
-		goto free_device_info;
-	}
-	pm2->lpn_pin = pm2->pdata->lpn_gpio;
-
 	if (!i2c_check_functionality(i2c_client->adapter,
 			I2C_FUNC_SMBUS_BYTE_DATA |
 			I2C_FUNC_SMBUS_READ_WORD_DATA)) {
@@ -945,6 +1084,10 @@ static int __devinit pm2xxx_wall_charger_probe(struct i2c_client *i2c_client,
 	INIT_WORK(&pm2->check_main_thermal_prot_work,
 		pm2xxx_charger_check_main_thermal_prot_work);
 
+	/* Init work for HW failure check */
+	INIT_DEFERRABLE_WORK(&pm2->check_hw_failure_work,
+		pm2xxx_charger_check_hw_failure_work);
+
 	/*
 	 * VDD ADC supply needs to be enabled from this driver when there
 	 * is a charger connected to avoid erroneous BTEMP_HIGH/LOW
@@ -965,40 +1108,72 @@ static int __devinit pm2xxx_wall_charger_probe(struct i2c_client *i2c_client,
 	}
 
 	/* Register interrupts */
-	ret = request_threaded_irq(pm2->pdata->irq_number, NULL,
+	ret = request_threaded_irq(gpio_to_irq(pm2->pdata->gpio_irq_number),
+				NULL,
 				pm2xxx_charger_irq[0].isr,
 				pm2->pdata->irq_type,
 				pm2xxx_charger_irq[0].name, pm2);
 
 	if (ret != 0) {
 		dev_err(pm2->dev, "failed to request %s IRQ %d: %d\n",
-		pm2xxx_charger_irq[0].name, pm2->pdata->irq_number, ret);
+		pm2xxx_charger_irq[0].name,
+			gpio_to_irq(pm2->pdata->gpio_irq_number), ret);
 		goto unregister_pm2xxx_charger;
 	}
 
-	/*Initialize lock*/
-	mutex_init(&pm2->lock);
+	ret = pm_runtime_set_active(pm2->dev);
+	if (ret)
+		dev_err(pm2->dev, "set active Error\n");
 
-	/*
-	 * Charger detection mechanism requires pulling up the LPN pin
-	 * while i2c communication if Charger is not connected
-	 * LPN pin of PM2301 is GPIO60 of AB9540
-	 */
-	ret = gpio_request(pm2->lpn_pin, "pm2301_lpm_gpio");
-	if (ret < 0) {
-		dev_err(pm2->dev, "pm2301_lpm_gpio request failed\n");
-		goto unregister_pm2xxx_charger;
+	pm_runtime_enable(pm2->dev);
+	pm_runtime_set_autosuspend_delay(pm2->dev, PM2XXX_AUTOSUSPEND_DELAY);
+	pm_runtime_use_autosuspend(pm2->dev);
+	pm_runtime_resume(pm2->dev);
+
+	/* pm interrupt can wake up system */
+	ret = enable_irq_wake(gpio_to_irq(pm2->pdata->gpio_irq_number));
+	if (ret) {
+		dev_err(pm2->dev, "failed to set irq wake\n");
+		goto unregister_pm2xxx_interrupt;
 	}
-	ret = gpio_direction_output(pm2->lpn_pin, 0);
-	if (ret < 0) {
-		dev_err(pm2->dev, "pm2301_lpm_gpio direction failed\n");
-		goto free_gpio;
+
+	mutex_init(&pm2->lock);
+
+	if (gpio_is_valid(pm2->pdata->lpn_gpio)) {
+		/* get lpn GPIO from platform data */
+		pm2->lpn_pin = pm2->pdata->lpn_gpio;
+
+		/*
+		 * Charger detection mechanism requires pulling up the LPN pin
+		 * while i2c communication if Charger is not connected
+		 * LPN pin of PM2301 is GPIO60 of AB9540
+		 */
+		ret = gpio_request(pm2->lpn_pin, "pm2301_lpm_gpio");
+
+		if (ret < 0) {
+			dev_err(pm2->dev, "pm2301_lpm_gpio request failed\n");
+			goto disable_pm2_irq_wake;
+		}
+		ret = gpio_direction_output(pm2->lpn_pin, 0);
+		if (ret < 0) {
+			dev_err(pm2->dev, "pm2301_lpm_gpio direction failed\n");
+			goto free_gpio;
+		}
+		set_lpn_pin(pm2);
 	}
 
+	/* read  interrupt registers */
+	for (i = 0; i < PM2XXX_NUM_INT_REG; i++)
+		pm2xxx_reg_read(pm2,
+			pm2xxx_interrupt_registers[i],
+			&val);
+
 	ret = pm2xxx_charger_detection(pm2, &val);
 
 	if ((ret == 0) && val) {
 		pm2->ac.charger_connected = 1;
+		ab8500_override_turn_on_stat(~AB8500_POW_KEY_1_ON,
+					     AB8500_MAIN_CH_DET);
 		pm2->ac_conn = true;
 		power_supply_changed(&pm2->ac_chg.psy);
 		sysfs_notify(&pm2->ac_chg.psy.dev->kobj, NULL, "present");
@@ -1007,7 +1182,13 @@ static int __devinit pm2xxx_wall_charger_probe(struct i2c_client *i2c_client,
 	return 0;
 
 free_gpio:
-	gpio_free(pm2->lpn_pin);
+	if (gpio_is_valid(pm2->lpn_pin))
+		gpio_free(pm2->lpn_pin);
+disable_pm2_irq_wake:
+	disable_irq_wake(gpio_to_irq(pm2->pdata->gpio_irq_number));
+unregister_pm2xxx_interrupt:
+	/* disable interrupt */
+	free_irq(gpio_to_irq(pm2->pdata->gpio_irq_number), pm2);
 unregister_pm2xxx_charger:
 	/* unregister power supply */
 	power_supply_unregister(&pm2->ac_chg.psy);
@@ -1018,18 +1199,24 @@ free_charger_wq:
 	destroy_workqueue(pm2->charger_wq);
 free_device_info:
 	kfree(pm2);
+
 	return ret;
 }
 
-static int __devexit pm2xxx_wall_charger_remove(struct i2c_client *i2c_client)
+static int pm2xxx_wall_charger_remove(struct i2c_client *i2c_client)
 {
 	struct pm2xxx_charger *pm2 = i2c_get_clientdata(i2c_client);
 
+	/* Disable pm_runtime */
+	pm_runtime_disable(pm2->dev);
 	/* Disable AC charging */
 	pm2xxx_charger_ac_en(&pm2->ac_chg, false, 0, 0);
 
+	/* Disable wake by pm interrupt */
+	disable_irq_wake(gpio_to_irq(pm2->pdata->gpio_irq_number));
+
 	/* Disable interrupts */
-	free_irq(pm2->pdata->irq_number, pm2);
+	free_irq(gpio_to_irq(pm2->pdata->gpio_irq_number), pm2);
 
 	/* Delete the work queue */
 	destroy_workqueue(pm2->charger_wq);
@@ -1041,8 +1228,8 @@ static int __devexit pm2xxx_wall_charger_remove(struct i2c_client *i2c_client)
 
 	power_supply_unregister(&pm2->ac_chg.psy);
 
-	/*Free GPIO60*/
-	gpio_free(pm2->lpn_pin);
+	if (gpio_is_valid(pm2->lpn_pin))
+		gpio_free(pm2->lpn_pin);
 
 	kfree(pm2);
 
@@ -1058,12 +1245,11 @@ MODULE_DEVICE_TABLE(i2c, pm2xxx_id);
 
 static struct i2c_driver pm2xxx_charger_driver = {
 	.probe = pm2xxx_wall_charger_probe,
-	.remove = __devexit_p(pm2xxx_wall_charger_remove),
-	.suspend = pm2xxx_wall_charger_suspend,
-	.resume = pm2xxx_wall_charger_resume,
+	.remove = pm2xxx_wall_charger_remove,
 	.driver = {
 		.name = "pm2xxx-wall_charger",
 		.owner = THIS_MODULE,
+		.pm = PM2XXX_PM_OPS,
 	},
 	.id_table = pm2xxx_id,
 };
@@ -1078,11 +1264,10 @@ static void __exit pm2xxx_charger_exit(void)
 	i2c_del_driver(&pm2xxx_charger_driver);
 }
 
-subsys_initcall_sync(pm2xxx_charger_init);
+device_initcall_sync(pm2xxx_charger_init);
 module_exit(pm2xxx_charger_exit);
 
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Rajkumar kasirajan, Olivier Launay");
 MODULE_ALIAS("platform:pm2xxx-charger");
 MODULE_DESCRIPTION("PM2xxx charger management driver");
-
diff --git a/drivers/power/pm2301_charger.h b/drivers/power/pm2301_charger.h
index e6319cdbc94f..8ce3cc0195df 100644
--- a/drivers/power/pm2301_charger.h
+++ b/drivers/power/pm2301_charger.h
@@ -9,27 +9,6 @@
 #ifndef PM2301_CHARGER_H
 #define PM2301_CHARGER_H
 
-#define MAIN_WDOG_ENA			0x01
-#define MAIN_WDOG_KICK			0x02
-#define MAIN_WDOG_DIS			0x00
-#define CHARG_WD_KICK			0x01
-#define MAIN_CH_ENA			0x01
-#define MAIN_CH_NO_OVERSHOOT_ENA_N	0x02
-#define MAIN_CH_DET			0x01
-#define MAIN_CH_CV_ON			0x04
-#define OTP_ENABLE_WD			0x01
-
-#define MAIN_CH_INPUT_CURR_SHIFT	4
-
-#define LED_INDICATOR_PWM_ENA		0x01
-#define LED_INDICATOR_PWM_DIS		0x00
-#define LED_IND_CUR_5MA			0x04
-#define LED_INDICATOR_PWM_DUTY_252_256	0xBF
-
-/* HW failure constants */
-#define MAIN_CH_TH_PROT			0x02
-#define MAIN_CH_NOK			0x01
-
 /* Watchdog timeout constant */
 #define WD_TIMER			0x30 /* 4min */
 #define WD_KICK_INTERVAL		(30 * HZ)
@@ -495,7 +474,6 @@ struct pm2xxx_charger {
 	int failure_input_ovv;
 	unsigned int lpn_pin;
 	struct pm2xxx_interrupts *pm2_int;
-	struct ab8500_gpadc *gpadc;
 	struct regulator *regu;
 	struct pm2xxx_bm_data *bat;
 	struct mutex lock;
@@ -506,6 +484,7 @@ struct pm2xxx_charger {
 	struct delayed_work check_vbat_work;
 	struct work_struct ac_work;
 	struct work_struct check_main_thermal_prot_work;
+	struct delayed_work check_hw_failure_work;
 	struct ux500_charger ac_chg;
 	struct pm2xxx_charger_event_flags flags;
 };
diff --git a/drivers/power/power_supply_core.c b/drivers/power/power_supply_core.c
index 5deac432e2ae..1c517c34e4be 100644
--- a/drivers/power/power_supply_core.c
+++ b/drivers/power/power_supply_core.c
@@ -26,17 +26,42 @@ EXPORT_SYMBOL_GPL(power_supply_class);
 
 static struct device_type power_supply_dev_type;
 
+static bool __power_supply_is_supplied_by(struct power_supply *supplier,
+					 struct power_supply *supply)
+{
+	int i;
+
+	if (!supply->supplied_from && !supplier->supplied_to)
+		return false;
+
+	/* Support both supplied_to and supplied_from modes */
+	if (supply->supplied_from) {
+		if (!supplier->name)
+			return false;
+		for (i = 0; i < supply->num_supplies; i++)
+			if (!strcmp(supplier->name, supply->supplied_from[i]))
+				return true;
+	} else {
+		if (!supply->name)
+			return false;
+		for (i = 0; i < supplier->num_supplicants; i++)
+			if (!strcmp(supplier->supplied_to[i], supply->name))
+				return true;
+	}
+
+	return false;
+}
+
 static int __power_supply_changed_work(struct device *dev, void *data)
 {
 	struct power_supply *psy = (struct power_supply *)data;
 	struct power_supply *pst = dev_get_drvdata(dev);
-	int i;
 
-	for (i = 0; i < psy->num_supplicants; i++)
-		if (!strcmp(psy->supplied_to[i], pst->name)) {
-			if (pst->external_power_changed)
-				pst->external_power_changed(pst);
-		}
+	if (__power_supply_is_supplied_by(psy, pst)) {
+		if (pst->external_power_changed)
+			pst->external_power_changed(pst);
+	}
+
 	return 0;
 }
 
@@ -63,22 +88,151 @@ void power_supply_changed(struct power_supply *psy)
 }
 EXPORT_SYMBOL_GPL(power_supply_changed);
 
+#ifdef CONFIG_OF
+#include <linux/of.h>
+
+static int __power_supply_populate_supplied_from(struct device *dev,
+						 void *data)
+{
+	struct power_supply *psy = (struct power_supply *)data;
+	struct power_supply *epsy = dev_get_drvdata(dev);
+	struct device_node *np;
+	int i = 0;
+
+	do {
+		np = of_parse_phandle(psy->of_node, "power-supplies", i++);
+		if (!np)
+			continue;
+
+		if (np == epsy->of_node) {
+			dev_info(psy->dev, "%s: Found supply : %s\n",
+				psy->name, epsy->name);
+			psy->supplied_from[i-1] = (char *)epsy->name;
+			psy->num_supplies++;
+			break;
+		}
+	} while (np);
+
+	return 0;
+}
+
+static int power_supply_populate_supplied_from(struct power_supply *psy)
+{
+	int error;
+
+	error = class_for_each_device(power_supply_class, NULL, psy,
+				      __power_supply_populate_supplied_from);
+
+	dev_dbg(psy->dev, "%s %d\n", __func__, error);
+
+	return error;
+}
+
+static int  __power_supply_find_supply_from_node(struct device *dev,
+						 void *data)
+{
+	struct device_node *np = (struct device_node *)data;
+	struct power_supply *epsy = dev_get_drvdata(dev);
+
+	/* return error breaks out of class_for_each_device loop */
+	if (epsy->of_node == np)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int power_supply_find_supply_from_node(struct device_node *supply_node)
+{
+	int error;
+	struct device *dev;
+	struct class_dev_iter iter;
+
+	/*
+	 * Use iterator to see if any other device is registered.
+	 * This is required since class_for_each_device returns 0
+	 * if there are no devices registered.
+	 */
+	class_dev_iter_init(&iter, power_supply_class, NULL, NULL);
+	dev = class_dev_iter_next(&iter);
+
+	if (!dev)
+		return -EPROBE_DEFER;
+
+	/*
+	 * We have to treat the return value as inverted, because if
+	 * we return error on not found, then it won't continue looking.
+	 * So we trick it by returning error on success to stop looking
+	 * once the matching device is found.
+	 */
+	error = class_for_each_device(power_supply_class, NULL, supply_node,
+				       __power_supply_find_supply_from_node);
+
+	return error ? 0 : -EPROBE_DEFER;
+}
+
+static int power_supply_check_supplies(struct power_supply *psy)
+{
+	struct device_node *np;
+	int cnt = 0;
+
+	/* If there is already a list honor it */
+	if (psy->supplied_from && psy->num_supplies > 0)
+		return 0;
+
+	/* No device node found, nothing to do */
+	if (!psy->of_node)
+		return 0;
+
+	do {
+		int ret;
+
+		np = of_parse_phandle(psy->of_node, "power-supplies", cnt++);
+		if (!np)
+			continue;
+
+		ret = power_supply_find_supply_from_node(np);
+		if (ret) {
+			dev_dbg(psy->dev, "Failed to find supply, defer!\n");
+			return -EPROBE_DEFER;
+		}
+	} while (np);
+
+	/* All supplies found, allocate char ** array for filling */
+	psy->supplied_from = devm_kzalloc(psy->dev, sizeof(psy->supplied_from),
+					  GFP_KERNEL);
+	if (!psy->supplied_from) {
+		dev_err(psy->dev, "Couldn't allocate memory for supply list\n");
+		return -ENOMEM;
+	}
+
+	*psy->supplied_from = devm_kzalloc(psy->dev, sizeof(char *) * cnt,
+					   GFP_KERNEL);
+	if (!*psy->supplied_from) {
+		dev_err(psy->dev, "Couldn't allocate memory for supply list\n");
+		return -ENOMEM;
+	}
+
+	return power_supply_populate_supplied_from(psy);
+}
+#else
+static inline int power_supply_check_supplies(struct power_supply *psy)
+{
+	return 0;
+}
+#endif
+
 static int __power_supply_am_i_supplied(struct device *dev, void *data)
 {
 	union power_supply_propval ret = {0,};
 	struct power_supply *psy = (struct power_supply *)data;
 	struct power_supply *epsy = dev_get_drvdata(dev);
-	int i;
 
-	for (i = 0; i < epsy->num_supplicants; i++) {
-		if (!strcmp(epsy->supplied_to[i], psy->name)) {
-			if (epsy->get_property(epsy,
-				  POWER_SUPPLY_PROP_ONLINE, &ret))
-				continue;
+	if (__power_supply_is_supplied_by(epsy, psy))
+		if (!epsy->get_property(epsy, POWER_SUPPLY_PROP_ONLINE, &ret)) {
 			if (ret.intval)
 				return ret.intval;
 		}
-	}
+
 	return 0;
 }
 
@@ -336,6 +490,12 @@ int power_supply_register(struct device *parent, struct power_supply *psy)
 
 	INIT_WORK(&psy->changed_work, power_supply_changed_work);
 
+	rc = power_supply_check_supplies(psy);
+	if (rc) {
+		dev_info(dev, "Not all required supplies found, defer probe\n");
+		goto check_supplies_failed;
+	}
+
 	rc = kobject_set_name(&dev->kobj, "%s", psy->name);
 	if (rc)
 		goto kobject_set_name_failed;
@@ -368,6 +528,7 @@ register_thermal_failed:
 	device_del(dev);
 kobject_set_name_failed:
 device_add_failed:
+check_supplies_failed:
 	put_device(dev);
 success:
 	return rc;
diff --git a/drivers/power/reset/Kconfig b/drivers/power/reset/Kconfig
index 1ae65b822864..349e9ae8090a 100644
--- a/drivers/power/reset/Kconfig
+++ b/drivers/power/reset/Kconfig
@@ -30,3 +30,10 @@ config POWER_RESET_RESTART
 	  Some boards don't actually have the ability to power off.
 	  Instead they restart, and u-boot holds the SoC until the
 	  user presses a key. u-boot then boots into Linux.
+
+config POWER_RESET_VEXPRESS
+	bool
+	depends on POWER_RESET
+	help
+	  Power off and reset support for the ARM Ltd. Versatile
+	  Express boards.
diff --git a/drivers/power/reset/Makefile b/drivers/power/reset/Makefile
index 0f317f50c56f..372807fd83f7 100644
--- a/drivers/power/reset/Makefile
+++ b/drivers/power/reset/Makefile
@@ -1,3 +1,4 @@
 obj-$(CONFIG_POWER_RESET_GPIO) += gpio-poweroff.o
 obj-$(CONFIG_POWER_RESET_QNAP) += qnap-poweroff.o
-obj-$(CONFIG_POWER_RESET_RESTART) += restart-poweroff.o
\ No newline at end of file
+obj-$(CONFIG_POWER_RESET_RESTART) += restart-poweroff.o
+obj-$(CONFIG_POWER_RESET_VEXPRESS) += vexpress-poweroff.o
diff --git a/drivers/power/reset/vexpress-poweroff.c b/drivers/power/reset/vexpress-poweroff.c
new file mode 100644
index 000000000000..469e6962b2cf
--- /dev/null
+++ b/drivers/power/reset/vexpress-poweroff.c
@@ -0,0 +1,146 @@
+/*
+ * 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.
+ *
+ * Copyright (C) 2012 ARM Limited
+ */
+
+#include <linux/jiffies.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/stat.h>
+#include <linux/vexpress.h>
+
+#include <asm/system_misc.h>
+
+static void vexpress_reset_do(struct device *dev, const char *what)
+{
+	int err = -ENOENT;
+	struct vexpress_config_func *func =
+			vexpress_config_func_get_by_dev(dev);
+
+	if (func) {
+		unsigned long timeout;
+
+		err = vexpress_config_write(func, 0, 0);
+
+		timeout = jiffies + HZ;
+		while (time_before(jiffies, timeout))
+			cpu_relax();
+	}
+
+	dev_emerg(dev, "Unable to %s (%d)\n", what, err);
+}
+
+static struct device *vexpress_power_off_device;
+
+static void vexpress_power_off(void)
+{
+	vexpress_reset_do(vexpress_power_off_device, "power off");
+}
+
+static struct device *vexpress_restart_device;
+
+static void vexpress_restart(char str, const char *cmd)
+{
+	vexpress_reset_do(vexpress_restart_device, "restart");
+}
+
+static ssize_t vexpress_reset_active_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%d\n", vexpress_restart_device == dev);
+}
+
+static ssize_t vexpress_reset_active_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	long value;
+	int err = kstrtol(buf, 0, &value);
+
+	if (!err && value)
+		vexpress_restart_device = dev;
+
+	return err ? err : count;
+}
+
+DEVICE_ATTR(active, S_IRUGO | S_IWUSR, vexpress_reset_active_show,
+		vexpress_reset_active_store);
+
+
+enum vexpress_reset_func { FUNC_RESET, FUNC_SHUTDOWN, FUNC_REBOOT };
+
+static struct of_device_id vexpress_reset_of_match[] = {
+	{
+		.compatible = "arm,vexpress-reset",
+		.data = (void *)FUNC_RESET,
+	}, {
+		.compatible = "arm,vexpress-shutdown",
+		.data = (void *)FUNC_SHUTDOWN
+	}, {
+		.compatible = "arm,vexpress-reboot",
+		.data = (void *)FUNC_REBOOT
+	},
+	{}
+};
+
+static int vexpress_reset_probe(struct platform_device *pdev)
+{
+	enum vexpress_reset_func func;
+	const struct of_device_id *match =
+			of_match_device(vexpress_reset_of_match, &pdev->dev);
+
+	if (match)
+		func = (enum vexpress_reset_func)match->data;
+	else
+		func = pdev->id_entry->driver_data;
+
+	switch (func) {
+	case FUNC_SHUTDOWN:
+		vexpress_power_off_device = &pdev->dev;
+		pm_power_off = vexpress_power_off;
+		break;
+	case FUNC_RESET:
+		if (!vexpress_restart_device)
+			vexpress_restart_device = &pdev->dev;
+		arm_pm_restart = vexpress_restart;
+		device_create_file(&pdev->dev, &dev_attr_active);
+		break;
+	case FUNC_REBOOT:
+		vexpress_restart_device = &pdev->dev;
+		arm_pm_restart = vexpress_restart;
+		device_create_file(&pdev->dev, &dev_attr_active);
+		break;
+	};
+
+	return 0;
+}
+
+static const struct platform_device_id vexpress_reset_id_table[] = {
+	{ .name = "vexpress-reset", .driver_data = FUNC_RESET, },
+	{ .name = "vexpress-shutdown", .driver_data = FUNC_SHUTDOWN, },
+	{ .name = "vexpress-reboot", .driver_data = FUNC_REBOOT, },
+	{}
+};
+
+static struct platform_driver vexpress_reset_driver = {
+	.probe = vexpress_reset_probe,
+	.driver = {
+		.name = "vexpress-reset",
+		.of_match_table = vexpress_reset_of_match,
+	},
+	.id_table = vexpress_reset_id_table,
+};
+
+static int __init vexpress_reset_init(void)
+{
+	return platform_driver_register(&vexpress_reset_driver);
+}
+device_initcall(vexpress_reset_init);
diff --git a/drivers/power/rx51_battery.c b/drivers/power/rx51_battery.c
index 8208888b844e..cbde1d6d3228 100644
--- a/drivers/power/rx51_battery.c
+++ b/drivers/power/rx51_battery.c
@@ -42,6 +42,7 @@ static int rx51_battery_read_adc(int channel)
 	req.method = TWL4030_MADC_SW1;
 	req.func_cb = NULL;
 	req.type = TWL4030_MADC_WAIT;
+	req.raw = true;
 
 	if (twl4030_madc_conversion(&req) <= 0)
 		return -ENODATA;
@@ -119,7 +120,7 @@ static int rx51_battery_read_temperature(struct rx51_device_info *di)
 
 	/* First check for temperature in first direct table */
 	if (raw < ARRAY_SIZE(rx51_temp_table1))
-		return rx51_temp_table1[raw] * 100;
+		return rx51_temp_table1[raw] * 10;
 
 	/* Binary search RAW value in second inverse table */
 	while (max - min > 1) {
@@ -132,7 +133,7 @@ static int rx51_battery_read_temperature(struct rx51_device_info *di)
 			break;
 	}
 
-	return (rx51_temp_table2_first - min) * 100;
+	return (rx51_temp_table2_first - min) * 10;
 }
 
 /*
@@ -202,7 +203,7 @@ static int rx51_battery_probe(struct platform_device *pdev)
 	struct rx51_device_info *di;
 	int ret;
 
-	di = kzalloc(sizeof(*di), GFP_KERNEL);
+	di = devm_kzalloc(&pdev->dev, sizeof(*di), GFP_KERNEL);
 	if (!di)
 		return -ENOMEM;
 
@@ -217,7 +218,6 @@ static int rx51_battery_probe(struct platform_device *pdev)
 	ret = power_supply_register(di->dev, &di->bat);
 	if (ret) {
 		platform_set_drvdata(pdev, NULL);
-		kfree(di);
 		return ret;
 	}
 
@@ -230,7 +230,6 @@ static int rx51_battery_remove(struct platform_device *pdev)
 
 	power_supply_unregister(&di->bat);
 	platform_set_drvdata(pdev, NULL);
-	kfree(di);
 
 	return 0;
 }
diff --git a/drivers/power/s3c_adc_battery.c b/drivers/power/s3c_adc_battery.c
index d2ca989dcbdc..5948ce058bdd 100644
--- a/drivers/power/s3c_adc_battery.c
+++ b/drivers/power/s3c_adc_battery.c
@@ -145,14 +145,17 @@ static int s3c_adc_bat_get_property(struct power_supply *psy,
 
 	int new_level;
 	int full_volt;
-	const struct s3c_adc_bat_thresh *lut = bat->pdata->lut_noac;
-	unsigned int lut_size = bat->pdata->lut_noac_cnt;
+	const struct s3c_adc_bat_thresh *lut;
+	unsigned int lut_size;
 
 	if (!bat) {
 		dev_err(psy->dev, "no battery infos ?!\n");
 		return -EINVAL;
 	}
 
+	lut = bat->pdata->lut_noac;
+	lut_size = bat->pdata->lut_noac_cnt;
+
 	if (bat->volt_value < 0 || bat->cur_value < 0 ||
 		jiffies_to_msecs(jiffies - bat->timestamp) >
 			BAT_POLL_INTERVAL) {
diff --git a/drivers/power/sbs-battery.c b/drivers/power/sbs-battery.c
index 3960f0b2afe9..c8c78a74e75a 100644
--- a/drivers/power/sbs-battery.c
+++ b/drivers/power/sbs-battery.c
@@ -27,6 +27,7 @@
 #include <linux/slab.h>
 #include <linux/interrupt.h>
 #include <linux/gpio.h>
+#include <linux/of.h>
 
 #include <linux/power/sbs-battery.h>
 
@@ -667,7 +668,6 @@ of_out:
 	return pdata;
 }
 #else
-#define sbs_dt_ids NULL
 static struct sbs_platform_data *sbs_of_populate_pdata(
 	struct i2c_client *client)
 {
@@ -820,10 +820,11 @@ static int sbs_remove(struct i2c_client *client)
 	return 0;
 }
 
-#if defined CONFIG_PM
-static int sbs_suspend(struct i2c_client *client,
-	pm_message_t state)
+#if defined CONFIG_PM_SLEEP
+
+static int sbs_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct sbs_info *chip = i2c_get_clientdata(client);
 	s32 ret;
 
@@ -838,11 +839,13 @@ static int sbs_suspend(struct i2c_client *client,
 
 	return 0;
 }
+
+static SIMPLE_DEV_PM_OPS(sbs_pm_ops, sbs_suspend, NULL);
+#define SBS_PM_OPS (&sbs_pm_ops)
+
 #else
-#define sbs_suspend		NULL
+#define SBS_PM_OPS NULL
 #endif
-/* any smbus transaction will wake up sbs */
-#define sbs_resume		NULL
 
 static const struct i2c_device_id sbs_id[] = {
 	{ "bq20z75", 0 },
@@ -854,12 +857,11 @@ MODULE_DEVICE_TABLE(i2c, sbs_id);
 static struct i2c_driver sbs_battery_driver = {
 	.probe		= sbs_probe,
 	.remove		= sbs_remove,
-	.suspend	= sbs_suspend,
-	.resume		= sbs_resume,
 	.id_table	= sbs_id,
 	.driver = {
 		.name	= "sbs-battery",
-		.of_match_table = sbs_dt_ids,
+		.of_match_table = of_match_ptr(sbs_dt_ids),
+		.pm	= SBS_PM_OPS,
 	},
 };
 module_i2c_driver(sbs_battery_driver);
diff --git a/drivers/power/test_power.c b/drivers/power/test_power.c
index b99a452a4fda..0152f35dca5c 100644
--- a/drivers/power/test_power.c
+++ b/drivers/power/test_power.c
@@ -30,6 +30,8 @@ static int battery_technology		= POWER_SUPPLY_TECHNOLOGY_LION;
 static int battery_capacity		= 50;
 static int battery_voltage		= 3300;
 
+static bool module_initialized;
+
 static int test_power_get_ac_property(struct power_supply *psy,
 				      enum power_supply_property psp,
 				      union power_supply_propval *val)
@@ -185,6 +187,7 @@ static int __init test_power_init(void)
 		}
 	}
 
+	module_initialized = true;
 	return 0;
 failed:
 	while (--i >= 0)
@@ -209,6 +212,8 @@ static void __exit test_power_exit(void)
 
 	for (i = 0; i < ARRAY_SIZE(test_power_supplies); i++)
 		power_supply_unregister(&test_power_supplies[i]);
+
+	module_initialized = false;
 }
 module_exit(test_power_exit);
 
@@ -221,8 +226,8 @@ struct battery_property_map {
 };
 
 static struct battery_property_map map_ac_online[] = {
-	{ 0,  "on"  },
-	{ 1,  "off" },
+	{ 0,  "off"  },
+	{ 1,  "on" },
 	{ -1, NULL  },
 };
 
@@ -295,10 +300,16 @@ static const char *map_get_key(struct battery_property_map *map, int value,
 	return def_key;
 }
 
+static inline void signal_power_supply_changed(struct power_supply *psy)
+{
+	if (module_initialized)
+		power_supply_changed(psy);
+}
+
 static int param_set_ac_online(const char *key, const struct kernel_param *kp)
 {
 	ac_online = map_get_value(map_ac_online, key, ac_online);
-	power_supply_changed(&test_power_supplies[0]);
+	signal_power_supply_changed(&test_power_supplies[0]);
 	return 0;
 }
 
@@ -311,7 +322,7 @@ static int param_get_ac_online(char *buffer, const struct kernel_param *kp)
 static int param_set_usb_online(const char *key, const struct kernel_param *kp)
 {
 	usb_online = map_get_value(map_ac_online, key, usb_online);
-	power_supply_changed(&test_power_supplies[2]);
+	signal_power_supply_changed(&test_power_supplies[2]);
 	return 0;
 }
 
@@ -325,7 +336,7 @@ static int param_set_battery_status(const char *key,
 					const struct kernel_param *kp)
 {
 	battery_status = map_get_value(map_status, key, battery_status);
-	power_supply_changed(&test_power_supplies[1]);
+	signal_power_supply_changed(&test_power_supplies[1]);
 	return 0;
 }
 
@@ -339,7 +350,7 @@ static int param_set_battery_health(const char *key,
 					const struct kernel_param *kp)
 {
 	battery_health = map_get_value(map_health, key, battery_health);
-	power_supply_changed(&test_power_supplies[1]);
+	signal_power_supply_changed(&test_power_supplies[1]);
 	return 0;
 }
 
@@ -353,7 +364,7 @@ static int param_set_battery_present(const char *key,
 					const struct kernel_param *kp)
 {
 	battery_present = map_get_value(map_present, key, battery_present);
-	power_supply_changed(&test_power_supplies[0]);
+	signal_power_supply_changed(&test_power_supplies[0]);
 	return 0;
 }
 
@@ -369,7 +380,7 @@ static int param_set_battery_technology(const char *key,
 {
 	battery_technology = map_get_value(map_technology, key,
 						battery_technology);
-	power_supply_changed(&test_power_supplies[1]);
+	signal_power_supply_changed(&test_power_supplies[1]);
 	return 0;
 }
 
@@ -390,7 +401,7 @@ static int param_set_battery_capacity(const char *key,
 		return -EINVAL;
 
 	battery_capacity = tmp;
-	power_supply_changed(&test_power_supplies[1]);
+	signal_power_supply_changed(&test_power_supplies[1]);
 	return 0;
 }
 
@@ -405,7 +416,7 @@ static int param_set_battery_voltage(const char *key,
 		return -EINVAL;
 
 	battery_voltage = tmp;
-	power_supply_changed(&test_power_supplies[1]);
+	signal_power_supply_changed(&test_power_supplies[1]);
 	return 0;
 }
 
diff --git a/drivers/power/tps65090-charger.c b/drivers/power/tps65090-charger.c
new file mode 100644
index 000000000000..9fbca310a2ad
--- /dev/null
+++ b/drivers/power/tps65090-charger.c
@@ -0,0 +1,320 @@
+/*
+ * Battery charger driver for TI's tps65090
+ *
+ * Copyright (c) 2013, NVIDIA CORPORATION.  All rights reserved.
+
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+
+ * This program is distributed in the hope 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/err.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/mfd/tps65090.h>
+
+#define TPS65090_REG_INTR_STS	0x00
+#define TPS65090_REG_CG_CTRL0	0x04
+#define TPS65090_REG_CG_CTRL1	0x05
+#define TPS65090_REG_CG_CTRL2	0x06
+#define TPS65090_REG_CG_CTRL3	0x07
+#define TPS65090_REG_CG_CTRL4	0x08
+#define TPS65090_REG_CG_CTRL5	0x09
+#define TPS65090_REG_CG_STATUS1	0x0a
+#define TPS65090_REG_CG_STATUS2	0x0b
+
+#define TPS65090_CHARGER_ENABLE	BIT(0)
+#define TPS65090_VACG		BIT(1)
+#define TPS65090_NOITERM	BIT(5)
+
+struct tps65090_charger {
+	struct	device	*dev;
+	int	ac_online;
+	int	prev_ac_online;
+	int	irq;
+	struct power_supply	ac;
+	struct tps65090_platform_data *pdata;
+};
+
+static enum power_supply_property tps65090_ac_props[] = {
+	POWER_SUPPLY_PROP_ONLINE,
+};
+
+static int tps65090_low_chrg_current(struct tps65090_charger *charger)
+{
+	int ret;
+
+	ret = tps65090_write(charger->dev->parent, TPS65090_REG_CG_CTRL5,
+			TPS65090_NOITERM);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): error reading in register 0x%x\n",
+			__func__, TPS65090_REG_CG_CTRL5);
+		return ret;
+	}
+	return 0;
+}
+
+static int tps65090_enable_charging(struct tps65090_charger *charger,
+	uint8_t enable)
+{
+	int ret;
+	uint8_t ctrl0 = 0;
+
+	ret = tps65090_read(charger->dev->parent, TPS65090_REG_CG_CTRL0,
+			    &ctrl0);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): error reading in register 0x%x\n",
+				__func__, TPS65090_REG_CG_CTRL0);
+		return ret;
+	}
+
+	ret = tps65090_write(charger->dev->parent, TPS65090_REG_CG_CTRL0,
+				(ctrl0 | TPS65090_CHARGER_ENABLE));
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): error reading in register 0x%x\n",
+				__func__, TPS65090_REG_CG_CTRL0);
+		return ret;
+	}
+	return 0;
+}
+
+static int tps65090_config_charger(struct tps65090_charger *charger)
+{
+	int ret;
+
+	if (charger->pdata->enable_low_current_chrg) {
+		ret = tps65090_low_chrg_current(charger);
+		if (ret < 0) {
+			dev_err(charger->dev,
+				"error configuring low charge current\n");
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int tps65090_ac_get_property(struct power_supply *psy,
+			enum power_supply_property psp,
+			union power_supply_propval *val)
+{
+	struct tps65090_charger *charger = container_of(psy,
+					struct tps65090_charger, ac);
+
+	if (psp == POWER_SUPPLY_PROP_ONLINE) {
+		val->intval = charger->ac_online;
+		charger->prev_ac_online = charger->ac_online;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static irqreturn_t tps65090_charger_isr(int irq, void *dev_id)
+{
+	struct tps65090_charger *charger = dev_id;
+	int ret;
+	uint8_t status1 = 0;
+	uint8_t intrsts = 0;
+
+	ret = tps65090_read(charger->dev->parent, TPS65090_REG_CG_STATUS1,
+			    &status1);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): Error in reading reg 0x%x\n",
+				__func__, TPS65090_REG_CG_STATUS1);
+		return IRQ_HANDLED;
+	}
+	msleep(75);
+	ret = tps65090_read(charger->dev->parent, TPS65090_REG_INTR_STS,
+			    &intrsts);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): Error in reading reg 0x%x\n",
+				__func__, TPS65090_REG_INTR_STS);
+		return IRQ_HANDLED;
+	}
+
+	if (intrsts & TPS65090_VACG) {
+		ret = tps65090_enable_charging(charger, 1);
+		if (ret < 0)
+			return IRQ_HANDLED;
+		charger->ac_online = 1;
+	} else {
+		charger->ac_online = 0;
+	}
+
+	if (charger->prev_ac_online != charger->ac_online)
+		power_supply_changed(&charger->ac);
+
+	return IRQ_HANDLED;
+}
+
+#if defined(CONFIG_OF)
+
+#include <linux/of_device.h>
+
+static struct tps65090_platform_data *
+		tps65090_parse_dt_charger_data(struct platform_device *pdev)
+{
+	struct tps65090_platform_data *pdata;
+	struct device_node *np = pdev->dev.of_node;
+	unsigned int prop;
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata) {
+		dev_err(&pdev->dev, "Memory alloc for tps65090_pdata failed\n");
+		return NULL;
+	}
+
+	prop = of_property_read_bool(np, "ti,enable-low-current-chrg");
+	pdata->enable_low_current_chrg = prop;
+
+	pdata->irq_base = -1;
+
+	return pdata;
+
+}
+#else
+static struct tps65090_platform_data *
+		tps65090_parse_dt_charger_data(struct platform_device *pdev)
+{
+	return NULL;
+}
+#endif
+
+static int tps65090_charger_probe(struct platform_device *pdev)
+{
+	struct tps65090_charger *cdata;
+	struct tps65090_platform_data *pdata;
+	uint8_t status1 = 0;
+	int ret;
+	int irq;
+
+	pdata = dev_get_platdata(pdev->dev.parent);
+
+	if (!pdata && pdev->dev.of_node)
+		pdata = tps65090_parse_dt_charger_data(pdev);
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "%s():no platform data available\n",
+				__func__);
+		return -ENODEV;
+	}
+
+	cdata = devm_kzalloc(&pdev->dev, sizeof(*cdata), GFP_KERNEL);
+	if (!cdata) {
+		dev_err(&pdev->dev, "failed to allocate memory status\n");
+		return -ENOMEM;
+	}
+
+	dev_set_drvdata(&pdev->dev, cdata);
+
+	cdata->dev			= &pdev->dev;
+	cdata->pdata			= pdata;
+
+	cdata->ac.name			= "tps65090-ac";
+	cdata->ac.type			= POWER_SUPPLY_TYPE_MAINS;
+	cdata->ac.get_property		= tps65090_ac_get_property;
+	cdata->ac.properties		= tps65090_ac_props;
+	cdata->ac.num_properties	= ARRAY_SIZE(tps65090_ac_props);
+	cdata->ac.supplied_to		= pdata->supplied_to;
+	cdata->ac.num_supplicants	= pdata->num_supplicants;
+
+	ret = power_supply_register(&pdev->dev, &cdata->ac);
+	if (ret) {
+		dev_err(&pdev->dev, "failed: power supply register\n");
+		return ret;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq <= 0) {
+		dev_warn(&pdev->dev, "Unable to get charger irq = %d\n", irq);
+		ret = irq;
+		goto fail_unregister_supply;
+	}
+
+	cdata->irq = irq;
+
+	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
+		tps65090_charger_isr, 0, "tps65090-charger", cdata);
+	if (ret) {
+		dev_err(cdata->dev, "Unable to register irq %d err %d\n", irq,
+			ret);
+		goto fail_free_irq;
+	}
+
+	ret = tps65090_config_charger(cdata);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "charger config failed, err %d\n", ret);
+		goto fail_free_irq;
+	}
+
+	/* Check for charger presence */
+	ret = tps65090_read(cdata->dev->parent, TPS65090_REG_CG_STATUS1,
+			&status1);
+	if (ret < 0) {
+		dev_err(cdata->dev, "%s(): Error in reading reg 0x%x", __func__,
+			TPS65090_REG_CG_STATUS1);
+		goto fail_free_irq;
+	}
+
+	if (status1 != 0) {
+		ret = tps65090_enable_charging(cdata, 1);
+		if (ret < 0) {
+			dev_err(cdata->dev, "error enabling charger\n");
+			goto fail_free_irq;
+		}
+		cdata->ac_online = 1;
+		power_supply_changed(&cdata->ac);
+	}
+
+	return 0;
+
+fail_free_irq:
+	devm_free_irq(cdata->dev, irq, cdata);
+fail_unregister_supply:
+	power_supply_unregister(&cdata->ac);
+
+	return ret;
+}
+
+static int tps65090_charger_remove(struct platform_device *pdev)
+{
+	struct tps65090_charger *cdata = dev_get_drvdata(&pdev->dev);
+
+	devm_free_irq(cdata->dev, cdata->irq, cdata);
+	power_supply_unregister(&cdata->ac);
+
+	return 0;
+}
+
+static struct of_device_id of_tps65090_charger_match[] = {
+	{ .compatible = "ti,tps65090-charger", },
+	{ /* end */ }
+};
+
+static struct platform_driver tps65090_charger_driver = {
+	.driver	= {
+		.name	= "tps65090-charger",
+		.of_match_table = of_tps65090_charger_match,
+		.owner	= THIS_MODULE,
+	},
+	.probe	= tps65090_charger_probe,
+	.remove = tps65090_charger_remove,
+};
+module_platform_driver(tps65090_charger_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Syed Rafiuddin <srafiuddin@nvidia.com>");
+MODULE_DESCRIPTION("tps65090 battery charger driver");
diff --git a/drivers/power/twl4030_charger.c b/drivers/power/twl4030_charger.c
index a69d0d11b540..bed458172dd2 100644
--- a/drivers/power/twl4030_charger.c
+++ b/drivers/power/twl4030_charger.c
@@ -636,17 +636,7 @@ static struct platform_driver twl4030_bci_driver = {
 	.remove	= __exit_p(twl4030_bci_remove),
 };
 
-static int __init twl4030_bci_init(void)
-{
-	return platform_driver_probe(&twl4030_bci_driver, twl4030_bci_probe);
-}
-module_init(twl4030_bci_init);
-
-static void __exit twl4030_bci_exit(void)
-{
-	platform_driver_unregister(&twl4030_bci_driver);
-}
-module_exit(twl4030_bci_exit);
+module_platform_driver_probe(twl4030_bci_driver, twl4030_bci_probe);
 
 MODULE_AUTHOR("Gražvydas Ignotas");
 MODULE_DESCRIPTION("TWL4030 Battery Charger Interface driver");
diff --git a/drivers/power/wm831x_backup.c b/drivers/power/wm831x_backup.c
index d9cc169f1424..58cbb009b74f 100644
--- a/drivers/power/wm831x_backup.c
+++ b/drivers/power/wm831x_backup.c
@@ -169,7 +169,8 @@ static int wm831x_backup_probe(struct platform_device *pdev)
 	struct power_supply *backup;
 	int ret;
 
-	devdata = kzalloc(sizeof(struct wm831x_backup), GFP_KERNEL);
+	devdata = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_backup),
+				GFP_KERNEL);
 	if (devdata == NULL)
 		return -ENOMEM;
 
@@ -197,14 +198,8 @@ static int wm831x_backup_probe(struct platform_device *pdev)
 	backup->num_properties = ARRAY_SIZE(wm831x_backup_props);
 	backup->get_property = wm831x_backup_get_prop;
 	ret = power_supply_register(&pdev->dev, backup);
-	if (ret)
-		goto err_kmalloc;
 
 	return ret;
-
-err_kmalloc:
-	kfree(devdata);
-	return ret;
 }
 
 static int wm831x_backup_remove(struct platform_device *pdev)
@@ -213,7 +208,6 @@ static int wm831x_backup_remove(struct platform_device *pdev)
 
 	power_supply_unregister(&devdata->backup);
 	kfree(devdata->backup.name);
-	kfree(devdata);
 
 	return 0;
 }
diff --git a/drivers/pps/Kconfig b/drivers/pps/Kconfig
index 982d16b5a846..7512e98e9311 100644
--- a/drivers/pps/Kconfig
+++ b/drivers/pps/Kconfig
@@ -20,10 +20,10 @@ config PPS
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called pps_core.ko.
+if PPS
 
 config PPS_DEBUG
 	bool "PPS debugging messages"
-	depends on PPS
 	help
 	  Say Y here if you want the PPS support to produce a bunch of debug
 	  messages to the system log.  Select this if you are having a
@@ -31,13 +31,15 @@ config PPS_DEBUG
 
 config NTP_PPS
 	bool "PPS kernel consumer support"
-	depends on PPS && !NO_HZ
+	depends on !NO_HZ
 	help
 	  This option adds support for direct in-kernel time
 	  synchronization using an external PPS signal.
 
 	  It doesn't work on tickless systems at the moment.
 
+endif
+
 source drivers/pps/clients/Kconfig
 
 source drivers/pps/generators/Kconfig
diff --git a/drivers/pps/clients/pps_parport.c b/drivers/pps/clients/pps_parport.c
index e1b4705ae3ec..38a8bbe74810 100644
--- a/drivers/pps/clients/pps_parport.c
+++ b/drivers/pps/clients/pps_parport.c
@@ -32,6 +32,7 @@
 #include <linux/init.h>
 #include <linux/irqnr.h>
 #include <linux/time.h>
+#include <linux/slab.h>
 #include <linux/parport.h>
 #include <linux/pps_kernel.h>
 
diff --git a/drivers/pps/kc.c b/drivers/pps/kc.c
index 079e930b1938..e219db1f1c84 100644
--- a/drivers/pps/kc.c
+++ b/drivers/pps/kc.c
@@ -34,10 +34,10 @@
  */
 
 /* state variables to bind kernel consumer */
-DEFINE_SPINLOCK(pps_kc_hardpps_lock);
+static DEFINE_SPINLOCK(pps_kc_hardpps_lock);
 /* PPS API (RFC 2783): current source and mode for kernel consumer */
-struct pps_device *pps_kc_hardpps_dev;	/* unique pointer to device */
-int pps_kc_hardpps_mode;		/* mode bits for kernel consumer */
+static struct pps_device *pps_kc_hardpps_dev;	/* unique pointer to device */
+static int pps_kc_hardpps_mode;		/* mode bits for kernel consumer */
 
 /* pps_kc_bind - control PPS kernel consumer binding
  * @pps: the PPS source
diff --git a/drivers/ptp/ptp_clock.c b/drivers/ptp/ptp_clock.c
index 79f4bce061bd..4a8c388364ca 100644
--- a/drivers/ptp/ptp_clock.c
+++ b/drivers/ptp/ptp_clock.c
@@ -17,7 +17,7 @@
  *  along with this program; if not, write to the Free Software
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
-#include <linux/bitops.h>
+#include <linux/idr.h>
 #include <linux/device.h>
 #include <linux/err.h>
 #include <linux/init.h>
@@ -32,7 +32,6 @@
 #include "ptp_private.h"
 
 #define PTP_MAX_ALARMS 4
-#define PTP_MAX_CLOCKS 8
 #define PTP_PPS_DEFAULTS (PPS_CAPTUREASSERT | PPS_OFFSETASSERT)
 #define PTP_PPS_EVENT PPS_CAPTUREASSERT
 #define PTP_PPS_MODE (PTP_PPS_DEFAULTS | PPS_CANWAIT | PPS_TSFMT_TSPEC)
@@ -42,8 +41,7 @@
 static dev_t ptp_devt;
 static struct class *ptp_class;
 
-static DECLARE_BITMAP(ptp_clocks_map, PTP_MAX_CLOCKS);
-static DEFINE_MUTEX(ptp_clocks_mutex); /* protects 'ptp_clocks_map' */
+static DEFINE_IDA(ptp_clocks_map);
 
 /* time stamp event queue operations */
 
@@ -171,12 +169,7 @@ static void delete_ptp_clock(struct posix_clock *pc)
 	struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
 
 	mutex_destroy(&ptp->tsevq_mux);
-
-	/* Remove the clock from the bit map. */
-	mutex_lock(&ptp_clocks_mutex);
-	clear_bit(ptp->index, ptp_clocks_map);
-	mutex_unlock(&ptp_clocks_mutex);
-
+	ida_simple_remove(&ptp_clocks_map, ptp->index);
 	kfree(ptp);
 }
 
@@ -191,21 +184,18 @@ struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info,
 	if (info->n_alarm > PTP_MAX_ALARMS)
 		return ERR_PTR(-EINVAL);
 
-	/* Find a free clock slot and reserve it. */
-	err = -EBUSY;
-	mutex_lock(&ptp_clocks_mutex);
-	index = find_first_zero_bit(ptp_clocks_map, PTP_MAX_CLOCKS);
-	if (index < PTP_MAX_CLOCKS)
-		set_bit(index, ptp_clocks_map);
-	else
-		goto no_slot;
-
 	/* Initialize a clock structure. */
 	err = -ENOMEM;
 	ptp = kzalloc(sizeof(struct ptp_clock), GFP_KERNEL);
 	if (ptp == NULL)
 		goto no_memory;
 
+	index = ida_simple_get(&ptp_clocks_map, 0, MINORMASK + 1, GFP_KERNEL);
+	if (index < 0) {
+		err = index;
+		goto no_slot;
+	}
+
 	ptp->clock.ops = ptp_clock_ops;
 	ptp->clock.release = delete_ptp_clock;
 	ptp->info = info;
@@ -248,7 +238,6 @@ struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info,
 		goto no_clock;
 	}
 
-	mutex_unlock(&ptp_clocks_mutex);
 	return ptp;
 
 no_clock:
@@ -260,11 +249,9 @@ no_sysfs:
 	device_destroy(ptp_class, ptp->devid);
 no_device:
 	mutex_destroy(&ptp->tsevq_mux);
+no_slot:
 	kfree(ptp);
 no_memory:
-	clear_bit(index, ptp_clocks_map);
-no_slot:
-	mutex_unlock(&ptp_clocks_mutex);
 	return ERR_PTR(err);
 }
 EXPORT_SYMBOL(ptp_clock_register);
@@ -323,7 +310,8 @@ EXPORT_SYMBOL(ptp_clock_index);
 static void __exit ptp_exit(void)
 {
 	class_destroy(ptp_class);
-	unregister_chrdev_region(ptp_devt, PTP_MAX_CLOCKS);
+	unregister_chrdev_region(ptp_devt, MINORMASK + 1);
+	ida_destroy(&ptp_clocks_map);
 }
 
 static int __init ptp_init(void)
@@ -336,7 +324,7 @@ static int __init ptp_init(void)
 		return PTR_ERR(ptp_class);
 	}
 
-	err = alloc_chrdev_region(&ptp_devt, 0, PTP_MAX_CLOCKS, "ptp");
+	err = alloc_chrdev_region(&ptp_devt, 0, MINORMASK + 1, "ptp");
 	if (err < 0) {
 		pr_err("ptp: failed to allocate device region\n");
 		goto no_region;
diff --git a/drivers/ptp/ptp_pch.c b/drivers/ptp/ptp_pch.c
index 1367655eee39..bea94510ad2d 100644
--- a/drivers/ptp/ptp_pch.c
+++ b/drivers/ptp/ptp_pch.c
@@ -118,7 +118,7 @@ struct pch_ts_regs {
  * struct pch_dev - Driver private data
  */
 struct pch_dev {
-	struct pch_ts_regs *regs;
+	struct pch_ts_regs __iomem *regs;
 	struct ptp_clock *ptp_clock;
 	struct ptp_clock_info caps;
 	int exts0_enabled;
@@ -154,7 +154,7 @@ static inline void pch_eth_enable_set(struct pch_dev *chip)
 	iowrite32(val, (&chip->regs->ts_sel));
 }
 
-static u64 pch_systime_read(struct pch_ts_regs *regs)
+static u64 pch_systime_read(struct pch_ts_regs __iomem *regs)
 {
 	u64 ns;
 	u32 lo, hi;
@@ -169,7 +169,7 @@ static u64 pch_systime_read(struct pch_ts_regs *regs)
 	return ns;
 }
 
-static void pch_systime_write(struct pch_ts_regs *regs, u64 ns)
+static void pch_systime_write(struct pch_ts_regs __iomem *regs, u64 ns)
 {
 	u32 hi, lo;
 
@@ -315,7 +315,7 @@ int pch_set_station_address(u8 *addr, struct pci_dev *pdev)
 	struct pch_dev *chip = pci_get_drvdata(pdev);
 
 	/* Verify the parameter */
-	if ((chip->regs == 0) || addr == (u8 *)NULL) {
+	if ((chip->regs == NULL) || addr == (u8 *)NULL) {
 		dev_err(&pdev->dev,
 			"invalid params returning PCH_INVALIDPARAM\n");
 		return PCH_INVALIDPARAM;
@@ -361,7 +361,7 @@ EXPORT_SYMBOL(pch_set_station_address);
 static irqreturn_t isr(int irq, void *priv)
 {
 	struct pch_dev *pch_dev = priv;
-	struct pch_ts_regs *regs = pch_dev->regs;
+	struct pch_ts_regs __iomem *regs = pch_dev->regs;
 	struct ptp_clock_event event;
 	u32 ack = 0, lo, hi, val;
 
@@ -415,7 +415,7 @@ static int ptp_pch_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
 	u32 diff, addend;
 	int neg_adj = 0;
 	struct pch_dev *pch_dev = container_of(ptp, struct pch_dev, caps);
-	struct pch_ts_regs *regs = pch_dev->regs;
+	struct pch_ts_regs __iomem *regs = pch_dev->regs;
 
 	if (ppb < 0) {
 		neg_adj = 1;
@@ -438,7 +438,7 @@ static int ptp_pch_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	s64 now;
 	unsigned long flags;
 	struct pch_dev *pch_dev = container_of(ptp, struct pch_dev, caps);
-	struct pch_ts_regs *regs = pch_dev->regs;
+	struct pch_ts_regs __iomem *regs = pch_dev->regs;
 
 	spin_lock_irqsave(&pch_dev->register_lock, flags);
 	now = pch_systime_read(regs);
@@ -455,7 +455,7 @@ static int ptp_pch_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
 	u32 remainder;
 	unsigned long flags;
 	struct pch_dev *pch_dev = container_of(ptp, struct pch_dev, caps);
-	struct pch_ts_regs *regs = pch_dev->regs;
+	struct pch_ts_regs __iomem *regs = pch_dev->regs;
 
 	spin_lock_irqsave(&pch_dev->register_lock, flags);
 	ns = pch_systime_read(regs);
@@ -472,7 +472,7 @@ static int ptp_pch_settime(struct ptp_clock_info *ptp,
 	u64 ns;
 	unsigned long flags;
 	struct pch_dev *pch_dev = container_of(ptp, struct pch_dev, caps);
-	struct pch_ts_regs *regs = pch_dev->regs;
+	struct pch_ts_regs __iomem *regs = pch_dev->regs;
 
 	ns = ts->tv_sec * 1000000000ULL;
 	ns += ts->tv_nsec;
@@ -567,9 +567,9 @@ static void pch_remove(struct pci_dev *pdev)
 		free_irq(pdev->irq, chip);
 
 	/* unmap the virtual IO memory space */
-	if (chip->regs != 0) {
+	if (chip->regs != NULL) {
 		iounmap(chip->regs);
-		chip->regs = 0;
+		chip->regs = NULL;
 	}
 	/* release the reserved IO memory space */
 	if (chip->mem_base != 0) {
@@ -670,7 +670,7 @@ pch_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 err_req_irq:
 	ptp_clock_unregister(chip->ptp_clock);
 	iounmap(chip->regs);
-	chip->regs = 0;
+	chip->regs = NULL;
 
 err_ioremap:
 	release_mem_region(chip->mem_base, chip->mem_size);
@@ -723,9 +723,10 @@ static s32 __init ptp_pch_init(void)
 module_init(ptp_pch_init);
 module_exit(ptp_pch_exit);
 
-module_param_string(station, pch_param.station, sizeof pch_param.station, 0444);
+module_param_string(station,
+		    pch_param.station, sizeof(pch_param.station), 0444);
 MODULE_PARM_DESC(station,
-	 "IEEE 1588 station address to use - column separated hex values");
+	 "IEEE 1588 station address to use - colon separated hex values");
 
 MODULE_AUTHOR("LAPIS SEMICONDUCTOR, <tshimizu818@gmail.com>");
 MODULE_DESCRIPTION("PTP clock using the EG20T timer");
diff --git a/drivers/pwm/Kconfig b/drivers/pwm/Kconfig
index 0e0bfa035083..115b64453493 100644
--- a/drivers/pwm/Kconfig
+++ b/drivers/pwm/Kconfig
@@ -147,8 +147,7 @@ config PWM_TEGRA
 
 config  PWM_TIECAP
 	tristate "ECAP PWM support"
-	depends on SOC_AM33XX
-	select PWM_TIPWMSS
+	depends on SOC_AM33XX || ARCH_DAVINCI_DA8XX
 	help
 	  PWM driver support for the ECAP APWM controller found on AM33XX
 	  TI SOC
@@ -158,8 +157,7 @@ config  PWM_TIECAP
 
 config  PWM_TIEHRPWM
 	tristate "EHRPWM PWM support"
-	depends on SOC_AM33XX
-	select PWM_TIPWMSS
+	depends on SOC_AM33XX || ARCH_DAVINCI_DA8XX
 	help
 	  PWM driver support for the EHRPWM controller found on AM33XX
 	  TI SOC
@@ -169,7 +167,7 @@ config  PWM_TIEHRPWM
 
 config  PWM_TIPWMSS
 	bool
-	depends on SOC_AM33XX && (PWM_TIEHRPWM || PWM_TIECAP)
+	default y if SOC_AM33XX && (PWM_TIECAP || PWM_TIEHRPWM)
 	help
 	  PWM Subsystem driver support for AM33xx SOC.
 
diff --git a/drivers/pwm/pwm-ab8500.c b/drivers/pwm/pwm-ab8500.c
index 4248d0418273..1d07a6f99375 100644
--- a/drivers/pwm/pwm-ab8500.c
+++ b/drivers/pwm/pwm-ab8500.c
@@ -66,7 +66,7 @@ static int ab8500_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 				AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
 				1 << (chip->base - 1), ENABLE_PWM);
 	if (ret < 0)
-		dev_err(chip->dev, "%s: Failed to disable PWM, Error %d\n",
+		dev_err(chip->dev, "%s: Failed to enable PWM, Error %d\n",
 							pwm->label, ret);
 	return ret;
 }
@@ -88,6 +88,7 @@ static const struct pwm_ops ab8500_pwm_ops = {
 	.config = ab8500_pwm_config,
 	.enable = ab8500_pwm_enable,
 	.disable = ab8500_pwm_disable,
+	.owner = THIS_MODULE,
 };
 
 static int ab8500_pwm_probe(struct platform_device *pdev)
@@ -99,7 +100,7 @@ static int ab8500_pwm_probe(struct platform_device *pdev)
 	 * Nothing to be done in probe, this is required to get the
 	 * device which is required for ab8500 read and write
 	 */
-	ab8500 = kzalloc(sizeof(*ab8500), GFP_KERNEL);
+	ab8500 = devm_kzalloc(&pdev->dev, sizeof(*ab8500), GFP_KERNEL);
 	if (ab8500 == NULL) {
 		dev_err(&pdev->dev, "failed to allocate memory\n");
 		return -ENOMEM;
@@ -111,10 +112,8 @@ static int ab8500_pwm_probe(struct platform_device *pdev)
 	ab8500->chip.npwm = 1;
 
 	err = pwmchip_add(&ab8500->chip);
-	if (err < 0) {
-		kfree(ab8500);
+	if (err < 0)
 		return err;
-	}
 
 	dev_dbg(&pdev->dev, "pwm probe successful\n");
 	platform_set_drvdata(pdev, ab8500);
@@ -132,7 +131,6 @@ static int ab8500_pwm_remove(struct platform_device *pdev)
 		return err;
 
 	dev_dbg(&pdev->dev, "pwm driver removed\n");
-	kfree(ab8500);
 
 	return 0;
 }
diff --git a/drivers/pwm/pwm-atmel-tcb.c b/drivers/pwm/pwm-atmel-tcb.c
index 16cb53092857..0a7b6582edb1 100644
--- a/drivers/pwm/pwm-atmel-tcb.c
+++ b/drivers/pwm/pwm-atmel-tcb.c
@@ -358,6 +358,7 @@ static const struct pwm_ops atmel_tcb_pwm_ops = {
 	.set_polarity = atmel_tcb_pwm_set_polarity,
 	.enable = atmel_tcb_pwm_enable,
 	.disable = atmel_tcb_pwm_disable,
+	.owner = THIS_MODULE,
 };
 
 static int atmel_tcb_pwm_probe(struct platform_device *pdev)
diff --git a/drivers/pwm/pwm-imx.c b/drivers/pwm/pwm-imx.c
index 3f5677b7690e..ec287989eafc 100644
--- a/drivers/pwm/pwm-imx.c
+++ b/drivers/pwm/pwm-imx.c
@@ -43,7 +43,6 @@ struct imx_chip {
 	struct clk	*clk_per;
 	struct clk	*clk_ipg;
 
-	int		enabled;
 	void __iomem	*mmio_base;
 
 	struct pwm_chip	chip;
@@ -135,7 +134,7 @@ static int imx_pwm_config_v2(struct pwm_chip *chip,
 		MX3_PWMCR_DOZEEN | MX3_PWMCR_WAITEN |
 		MX3_PWMCR_DBGEN | MX3_PWMCR_CLKSRC_IPG_HIGH;
 
-	if (imx->enabled)
+	if (test_bit(PWMF_ENABLED, &pwm->flags))
 		cr |= MX3_PWMCR_EN;
 
 	writel(cr, imx->mmio_base + MX3_PWMCR);
@@ -186,8 +185,6 @@ static int imx_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 
 	imx->set_enable(chip, true);
 
-	imx->enabled = 1;
-
 	return 0;
 }
 
@@ -198,7 +195,6 @@ static void imx_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 	imx->set_enable(chip, false);
 
 	clk_disable_unprepare(imx->clk_per);
-	imx->enabled = 0;
 }
 
 static struct pwm_ops imx_pwm_ops = {
diff --git a/drivers/pwm/pwm-lpc32xx.c b/drivers/pwm/pwm-lpc32xx.c
index b3f0d0dfd748..8272883c0d05 100644
--- a/drivers/pwm/pwm-lpc32xx.c
+++ b/drivers/pwm/pwm-lpc32xx.c
@@ -37,6 +37,7 @@ static int lpc32xx_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 	struct lpc32xx_pwm_chip *lpc32xx = to_lpc32xx_pwm_chip(chip);
 	unsigned long long c;
 	int period_cycles, duty_cycles;
+	u32 val;
 
 	c = clk_get_rate(lpc32xx->clk) / 256;
 	c = c * period_ns;
@@ -68,8 +69,10 @@ static int lpc32xx_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 		c = 255;
 	duty_cycles = 256 - c;
 
-	writel(PWM_ENABLE | PWM_RELOADV(period_cycles) | PWM_DUTY(duty_cycles),
-		lpc32xx->base + (pwm->hwpwm << 2));
+	val = readl(lpc32xx->base + (pwm->hwpwm << 2));
+	val &= ~0xFFFF;
+	val |= PWM_RELOADV(period_cycles) | PWM_DUTY(duty_cycles);
+	writel(val, lpc32xx->base + (pwm->hwpwm << 2));
 
 	return 0;
 }
@@ -77,15 +80,29 @@ static int lpc32xx_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 static int lpc32xx_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	struct lpc32xx_pwm_chip *lpc32xx = to_lpc32xx_pwm_chip(chip);
+	u32 val;
+	int ret;
+
+	ret = clk_enable(lpc32xx->clk);
+	if (ret)
+		return ret;
 
-	return clk_enable(lpc32xx->clk);
+	val = readl(lpc32xx->base + (pwm->hwpwm << 2));
+	val |= PWM_ENABLE;
+	writel(val, lpc32xx->base + (pwm->hwpwm << 2));
+
+	return 0;
 }
 
 static void lpc32xx_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	struct lpc32xx_pwm_chip *lpc32xx = to_lpc32xx_pwm_chip(chip);
+	u32 val;
+
+	val = readl(lpc32xx->base + (pwm->hwpwm << 2));
+	val &= ~PWM_ENABLE;
+	writel(val, lpc32xx->base + (pwm->hwpwm << 2));
 
-	writel(0, lpc32xx->base + (pwm->hwpwm << 2));
 	clk_disable(lpc32xx->clk);
 }
 
@@ -145,7 +162,7 @@ static int lpc32xx_pwm_remove(struct platform_device *pdev)
 	return pwmchip_remove(&lpc32xx->chip);
 }
 
-static struct of_device_id lpc32xx_pwm_dt_ids[] = {
+static const struct of_device_id lpc32xx_pwm_dt_ids[] = {
 	{ .compatible = "nxp,lpc3220-pwm", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/pwm/pwm-mxs.c b/drivers/pwm/pwm-mxs.c
index a53d3094b75a..3febdddf71f9 100644
--- a/drivers/pwm/pwm-mxs.c
+++ b/drivers/pwm/pwm-mxs.c
@@ -38,7 +38,6 @@
 
 struct mxs_pwm_chip {
 	struct pwm_chip chip;
-	struct device *dev;
 	struct clk *clk;
 	void __iomem *base;
 };
@@ -166,7 +165,6 @@ static int mxs_pwm_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	mxs->dev = &pdev->dev;
 	platform_set_drvdata(pdev, mxs);
 
 	stmp_reset_block(mxs->base);
@@ -181,7 +179,7 @@ static int mxs_pwm_remove(struct platform_device *pdev)
 	return pwmchip_remove(&mxs->chip);
 }
 
-static struct of_device_id mxs_pwm_dt_ids[] = {
+static const struct of_device_id mxs_pwm_dt_ids[] = {
 	{ .compatible = "fsl,imx23-pwm", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/pwm/pwm-puv3.c b/drivers/pwm/pwm-puv3.c
index db964e6ecf5c..d1eb499fb15d 100644
--- a/drivers/pwm/pwm-puv3.c
+++ b/drivers/pwm/pwm-puv3.c
@@ -27,7 +27,6 @@ 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)
diff --git a/drivers/pwm/pwm-pxa.c b/drivers/pwm/pwm-pxa.c
index 20370e61de5a..dee6ab552a0a 100644
--- a/drivers/pwm/pwm-pxa.c
+++ b/drivers/pwm/pwm-pxa.c
@@ -23,14 +23,13 @@
 #include <asm/div64.h>
 
 #define HAS_SECONDARY_PWM	0x10
-#define PWM_ID_BASE(d)		((d) & 0xf)
 
 static const struct platform_device_id pwm_id_table[] = {
 	/*   PWM    has_secondary_pwm? */
 	{ "pxa25x-pwm", 0 },
-	{ "pxa27x-pwm", 0 | HAS_SECONDARY_PWM },
-	{ "pxa168-pwm", 1 },
-	{ "pxa910-pwm", 1 },
+	{ "pxa27x-pwm", HAS_SECONDARY_PWM },
+	{ "pxa168-pwm", 0 },
+	{ "pxa910-pwm", 0 },
 	{ },
 };
 MODULE_DEVICE_TABLE(platform, pwm_id_table);
@@ -48,7 +47,6 @@ struct pxa_pwm_chip {
 	struct device	*dev;
 
 	struct clk	*clk;
-	int		clk_enabled;
 	void __iomem	*mmio_base;
 };
 
@@ -108,24 +106,15 @@ static int pxa_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 static int pxa_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	struct pxa_pwm_chip *pc = to_pxa_pwm_chip(chip);
-	int rc = 0;
 
-	if (!pc->clk_enabled) {
-		rc = clk_prepare_enable(pc->clk);
-		if (!rc)
-			pc->clk_enabled++;
-	}
-	return rc;
+	return clk_prepare_enable(pc->clk);
 }
 
 static void pxa_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	struct pxa_pwm_chip *pc = to_pxa_pwm_chip(chip);
 
-	if (pc->clk_enabled) {
-		clk_disable_unprepare(pc->clk);
-		pc->clk_enabled--;
-	}
+	clk_disable_unprepare(pc->clk);
 }
 
 static struct pwm_ops pxa_pwm_ops = {
@@ -152,8 +141,6 @@ static int pwm_probe(struct platform_device *pdev)
 	if (IS_ERR(pwm->clk))
 		return PTR_ERR(pwm->clk);
 
-	pwm->clk_enabled = 0;
-
 	pwm->chip.dev = &pdev->dev;
 	pwm->chip.ops = &pxa_pwm_ops;
 	pwm->chip.base = -1;
diff --git a/drivers/pwm/pwm-samsung.c b/drivers/pwm/pwm-samsung.c
index 5207e6cd8648..a0ece50d70bb 100644
--- a/drivers/pwm/pwm-samsung.c
+++ b/drivers/pwm/pwm-samsung.c
@@ -289,10 +289,10 @@ static int s3c_pwm_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int s3c_pwm_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int s3c_pwm_suspend(struct device *dev)
 {
-	struct s3c_chip *s3c = platform_get_drvdata(pdev);
+	struct s3c_chip *s3c = dev_get_drvdata(dev);
 
 	/* No one preserve these values during suspend so reset them
 	 * Otherwise driver leaves PWM unconfigured if same values
@@ -304,9 +304,9 @@ static int s3c_pwm_suspend(struct platform_device *pdev, pm_message_t state)
 	return 0;
 }
 
-static int s3c_pwm_resume(struct platform_device *pdev)
+static int s3c_pwm_resume(struct device *dev)
 {
-	struct s3c_chip *s3c = platform_get_drvdata(pdev);
+	struct s3c_chip *s3c = dev_get_drvdata(dev);
 	unsigned long tcon;
 
 	/* Restore invertion */
@@ -316,21 +316,19 @@ static int s3c_pwm_resume(struct platform_device *pdev)
 
 	return 0;
 }
-
-#else
-#define s3c_pwm_suspend NULL
-#define s3c_pwm_resume NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(s3c_pwm_pm_ops, s3c_pwm_suspend,
+			s3c_pwm_resume);
+
 static struct platform_driver s3c_pwm_driver = {
 	.driver		= {
 		.name	= "s3c24xx-pwm",
 		.owner	= THIS_MODULE,
+		.pm	= &s3c_pwm_pm_ops,
 	},
 	.probe		= s3c_pwm_probe,
 	.remove		= s3c_pwm_remove,
-	.suspend	= s3c_pwm_suspend,
-	.resume		= s3c_pwm_resume,
 };
 
 static int __init pwm_init(void)
diff --git a/drivers/pwm/pwm-spear.c b/drivers/pwm/pwm-spear.c
index 69a2d9eb34db..6d99e2cbdc73 100644
--- a/drivers/pwm/pwm-spear.c
+++ b/drivers/pwm/pwm-spear.c
@@ -49,13 +49,11 @@
  * @mmio_base: base address of pwm chip
  * @clk: pointer to clk structure of pwm chip
  * @chip: linux pwm chip representation
- * @dev: pointer to device structure of pwm chip
  */
 struct spear_pwm_chip {
 	void __iomem *mmio_base;
 	struct clk *clk;
 	struct pwm_chip chip;
-	struct device *dev;
 };
 
 static inline struct spear_pwm_chip *to_spear_pwm_chip(struct pwm_chip *chip)
@@ -143,7 +141,7 @@ static int spear_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 	u32 val;
 
 	rc = clk_enable(pc->clk);
-	if (!rc)
+	if (rc)
 		return rc;
 
 	val = spear_pwm_readl(pc, pwm->hwpwm, PWMCR);
@@ -200,7 +198,6 @@ static int spear_pwm_probe(struct platform_device *pdev)
 	if (IS_ERR(pc->clk))
 		return PTR_ERR(pc->clk);
 
-	pc->dev = &pdev->dev;
 	platform_set_drvdata(pdev, pc);
 
 	pc->chip.dev = &pdev->dev;
@@ -209,12 +206,12 @@ static int spear_pwm_probe(struct platform_device *pdev)
 	pc->chip.npwm = NUM_PWM;
 
 	ret = clk_prepare(pc->clk);
-	if (!ret)
+	if (ret)
 		return ret;
 
 	if (of_device_is_compatible(np, "st,spear1340-pwm")) {
 		ret = clk_enable(pc->clk);
-		if (!ret) {
+		if (ret) {
 			clk_unprepare(pc->clk);
 			return ret;
 		}
@@ -251,7 +248,7 @@ static int spear_pwm_remove(struct platform_device *pdev)
 	return pwmchip_remove(&pc->chip);
 }
 
-static struct of_device_id spear_pwm_of_match[] = {
+static const struct of_device_id spear_pwm_of_match[] = {
 	{ .compatible = "st,spear320-pwm" },
 	{ .compatible = "st,spear1340-pwm" },
 	{ }
diff --git a/drivers/pwm/pwm-tegra.c b/drivers/pwm/pwm-tegra.c
index af3ab48cb7ef..3d75f4a88f98 100644
--- a/drivers/pwm/pwm-tegra.c
+++ b/drivers/pwm/pwm-tegra.c
@@ -233,7 +233,7 @@ static int tegra_pwm_remove(struct platform_device *pdev)
 	return pwmchip_remove(&pc->chip);
 }
 
-static struct of_device_id tegra_pwm_of_match[] = {
+static const struct of_device_id tegra_pwm_of_match[] = {
 	{ .compatible = "nvidia,tegra20-pwm" },
 	{ .compatible = "nvidia,tegra30-pwm" },
 	{ }
diff --git a/drivers/pwm/pwm-tiecap.c b/drivers/pwm/pwm-tiecap.c
index 22e96e2bffd3..0d65fb2e02c7 100644
--- a/drivers/pwm/pwm-tiecap.c
+++ b/drivers/pwm/pwm-tiecap.c
@@ -295,7 +295,7 @@ static int ecap_pwm_remove(struct platform_device *pdev)
 	return pwmchip_remove(&pc->chip);
 }
 
-void ecap_pwm_save_context(struct ecap_pwm_chip *pc)
+static void ecap_pwm_save_context(struct ecap_pwm_chip *pc)
 {
 	pm_runtime_get_sync(pc->chip.dev);
 	pc->ctx.ecctl2 = readw(pc->mmio_base + ECCTL2);
@@ -304,13 +304,14 @@ void ecap_pwm_save_context(struct ecap_pwm_chip *pc)
 	pm_runtime_put_sync(pc->chip.dev);
 }
 
-void ecap_pwm_restore_context(struct ecap_pwm_chip *pc)
+static void ecap_pwm_restore_context(struct ecap_pwm_chip *pc)
 {
 	writel(pc->ctx.cap3, pc->mmio_base + CAP3);
 	writel(pc->ctx.cap4, pc->mmio_base + CAP4);
 	writew(pc->ctx.ecctl2, pc->mmio_base + ECCTL2);
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int ecap_pwm_suspend(struct device *dev)
 {
 	struct ecap_pwm_chip *pc = dev_get_drvdata(dev);
@@ -337,6 +338,7 @@ static int ecap_pwm_resume(struct device *dev)
 	ecap_pwm_restore_context(pc);
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(ecap_pwm_pm_ops, ecap_pwm_suspend, ecap_pwm_resume);
 
diff --git a/drivers/pwm/pwm-tiehrpwm.c b/drivers/pwm/pwm-tiehrpwm.c
index 8b4c86fa99c8..6a217596942f 100644
--- a/drivers/pwm/pwm-tiehrpwm.c
+++ b/drivers/pwm/pwm-tiehrpwm.c
@@ -533,7 +533,7 @@ static int ehrpwm_pwm_remove(struct platform_device *pdev)
 	return pwmchip_remove(&pc->chip);
 }
 
-void ehrpwm_pwm_save_context(struct ehrpwm_pwm_chip *pc)
+static void ehrpwm_pwm_save_context(struct ehrpwm_pwm_chip *pc)
 {
 	pm_runtime_get_sync(pc->chip.dev);
 	pc->ctx.tbctl = ehrpwm_read(pc->mmio_base, TBCTL);
@@ -547,7 +547,7 @@ void ehrpwm_pwm_save_context(struct ehrpwm_pwm_chip *pc)
 	pm_runtime_put_sync(pc->chip.dev);
 }
 
-void ehrpwm_pwm_restore_context(struct ehrpwm_pwm_chip *pc)
+static void ehrpwm_pwm_restore_context(struct ehrpwm_pwm_chip *pc)
 {
 	ehrpwm_write(pc->mmio_base, TBPRD, pc->ctx.tbprd);
 	ehrpwm_write(pc->mmio_base, CMPA, pc->ctx.cmpa);
@@ -559,6 +559,7 @@ void ehrpwm_pwm_restore_context(struct ehrpwm_pwm_chip *pc)
 	ehrpwm_write(pc->mmio_base, TBCTL, pc->ctx.tbctl);
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int ehrpwm_pwm_suspend(struct device *dev)
 {
 	struct ehrpwm_pwm_chip *pc = dev_get_drvdata(dev);
@@ -594,6 +595,7 @@ static int ehrpwm_pwm_resume(struct device *dev)
 	ehrpwm_pwm_restore_context(pc);
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(ehrpwm_pwm_pm_ops, ehrpwm_pwm_suspend,
 		ehrpwm_pwm_resume);
diff --git a/drivers/pwm/pwm-tipwmss.c b/drivers/pwm/pwm-tipwmss.c
index 17cbc59660ec..c9c3d3a1e0eb 100644
--- a/drivers/pwm/pwm-tipwmss.c
+++ b/drivers/pwm/pwm-tipwmss.c
@@ -101,6 +101,7 @@ static int pwmss_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int pwmss_suspend(struct device *dev)
 {
 	struct pwmss_info *info = dev_get_drvdata(dev);
@@ -118,6 +119,7 @@ static int pwmss_resume(struct device *dev)
 	writew(info->pwmss_clkconfig, info->mmio_base + PWMSS_CLKCONFIG);
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(pwmss_pm_ops, pwmss_suspend, pwmss_resume);
 
diff --git a/drivers/pwm/pwm-tipwmss.h b/drivers/pwm/pwm-tipwmss.h
index 11f76a1e266b..10ad8040408b 100644
--- a/drivers/pwm/pwm-tipwmss.h
+++ b/drivers/pwm/pwm-tipwmss.h
@@ -18,7 +18,6 @@
 #ifndef __TIPWMSS_H
 #define __TIPWMSS_H
 
-#ifdef CONFIG_PWM_TIPWMSS
 /* PWM substem clock gating */
 #define PWMSS_ECAPCLK_EN	BIT(0)
 #define PWMSS_ECAPCLK_STOP_REQ	BIT(1)
@@ -28,6 +27,7 @@
 #define PWMSS_ECAPCLK_EN_ACK	BIT(0)
 #define PWMSS_EPWMCLK_EN_ACK	BIT(8)
 
+#ifdef CONFIG_PWM_TIPWMSS
 extern u16 pwmss_submodule_state_change(struct device *dev, int set);
 #else
 static inline u16 pwmss_submodule_state_change(struct device *dev, int set)
diff --git a/drivers/pwm/pwm-twl-led.c b/drivers/pwm/pwm-twl-led.c
index 83e25d45d640..29d1bba4804e 100644
--- a/drivers/pwm/pwm-twl-led.c
+++ b/drivers/pwm/pwm-twl-led.c
@@ -271,6 +271,7 @@ static const struct pwm_ops twl4030_pwmled_ops = {
 	.enable = twl4030_pwmled_enable,
 	.disable = twl4030_pwmled_disable,
 	.config = twl4030_pwmled_config,
+	.owner = THIS_MODULE,
 };
 
 static const struct pwm_ops twl6030_pwmled_ops = {
@@ -279,6 +280,7 @@ static const struct pwm_ops twl6030_pwmled_ops = {
 	.config = twl6030_pwmled_config,
 	.request = twl6030_pwmled_request,
 	.free = twl6030_pwmled_free,
+	.owner = THIS_MODULE,
 };
 
 static int twl_pwmled_probe(struct platform_device *pdev)
@@ -321,7 +323,7 @@ static int twl_pwmled_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id twl_pwmled_of_match[] = {
+static const struct of_device_id twl_pwmled_of_match[] = {
 	{ .compatible = "ti,twl4030-pwmled" },
 	{ .compatible = "ti,twl6030-pwmled" },
 	{ },
diff --git a/drivers/pwm/pwm-twl.c b/drivers/pwm/pwm-twl.c
index bf3fda294223..eef910580eae 100644
--- a/drivers/pwm/pwm-twl.c
+++ b/drivers/pwm/pwm-twl.c
@@ -248,7 +248,7 @@ static int twl6030_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 	twl->twl6030_toggle3 = val;
 out:
 	mutex_unlock(&twl->mutex);
-	return 0;
+	return ret;
 }
 
 static void twl6030_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
@@ -287,12 +287,14 @@ static const struct pwm_ops twl4030_pwm_ops = {
 	.disable = twl4030_pwm_disable,
 	.request = twl4030_pwm_request,
 	.free = twl4030_pwm_free,
+	.owner = THIS_MODULE,
 };
 
 static const struct pwm_ops twl6030_pwm_ops = {
 	.config = twl_pwm_config,
 	.enable = twl6030_pwm_enable,
 	.disable = twl6030_pwm_disable,
+	.owner = THIS_MODULE,
 };
 
 static int twl_pwm_probe(struct platform_device *pdev)
@@ -333,7 +335,7 @@ static int twl_pwm_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id twl_pwm_of_match[] = {
+static const struct of_device_id twl_pwm_of_match[] = {
 	{ .compatible = "ti,twl4030-pwm" },
 	{ .compatible = "ti,twl6030-pwm" },
 	{ },
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index a5d97eaee99e..8bb26446037e 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -66,7 +66,7 @@ config REGULATOR_USERSPACE_CONSUMER
 
 config REGULATOR_GPIO
 	tristate "GPIO regulator support"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  This driver provides support for regulators that can be
 	  controlled via gpios.
diff --git a/drivers/remoteproc/Kconfig b/drivers/remoteproc/Kconfig
index c6d77e20622c..d4d377c40ec9 100644
--- a/drivers/remoteproc/Kconfig
+++ b/drivers/remoteproc/Kconfig
@@ -4,13 +4,15 @@ menu "Remoteproc drivers"
 config REMOTEPROC
 	tristate
 	depends on HAS_DMA
+	select CRC32
 	select FW_LOADER
 	select VIRTIO
+	select VIRTUALIZATION
 
 config OMAP_REMOTEPROC
 	tristate "OMAP remoteproc support"
 	depends on HAS_DMA
-	depends on ARCH_OMAP4
+	depends on ARCH_OMAP4 || SOC_OMAP5
 	depends on OMAP_IOMMU
 	depends on OMAP_MBOX_FWK
 	select REMOTEPROC
@@ -38,4 +40,27 @@ config STE_MODEM_RPROC
 	  This can be either built-in or a loadable module.
 	  If unsure say N.
 
+config DA8XX_REMOTEPROC
+	tristate "DA8xx/OMAP-L13x remoteproc support"
+	depends on ARCH_DAVINCI_DA8XX
+	select CMA
+	select REMOTEPROC
+	select RPMSG
+	help
+	  Say y here to support DA8xx/OMAP-L13x remote processors via the
+	  remote processor framework.
+
+	  You want to say y here in order to enable AMP
+	  use-cases to run on your platform (multimedia codecs are
+	  offloaded to remote DSP processors using this framework).
+
+	  This module controls the name of the firmware file that gets
+	  loaded on the DSP.  This file must reside in the /lib/firmware
+	  directory.  It can be specified via the module parameter
+	  da8xx_fw_name=<filename>, and if not specified will default to
+	  "rproc-dsp-fw".
+
+	  It's safe to say n here if you're not interested in multimedia
+	  offloading.
+
 endmenu
diff --git a/drivers/remoteproc/Makefile b/drivers/remoteproc/Makefile
index 391b65181c05..ac2ff75686d2 100644
--- a/drivers/remoteproc/Makefile
+++ b/drivers/remoteproc/Makefile
@@ -9,3 +9,4 @@ remoteproc-y				+= remoteproc_virtio.o
 remoteproc-y				+= remoteproc_elf_loader.o
 obj-$(CONFIG_OMAP_REMOTEPROC)		+= omap_remoteproc.o
 obj-$(CONFIG_STE_MODEM_RPROC)	 	+= ste_modem_rproc.o
+obj-$(CONFIG_DA8XX_REMOTEPROC)		+= da8xx_remoteproc.o
diff --git a/drivers/remoteproc/da8xx_remoteproc.c b/drivers/remoteproc/da8xx_remoteproc.c
new file mode 100644
index 000000000000..9b2e60afa1a6
--- /dev/null
+++ b/drivers/remoteproc/da8xx_remoteproc.c
@@ -0,0 +1,324 @@
+/*
+ * Remote processor machine-specific module for DA8XX
+ *
+ * Copyright (C) 2013 Texas Instruments, Inc.
+ *
+ * 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/bitops.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/remoteproc.h>
+
+#include <mach/clock.h>   /* for davinci_clk_reset_assert/deassert() */
+
+#include "remoteproc_internal.h"
+
+static char *da8xx_fw_name;
+module_param(da8xx_fw_name, charp, S_IRUGO);
+MODULE_PARM_DESC(da8xx_fw_name,
+		 "\n\t\tName of DSP firmware file in /lib/firmware"
+		 " (if not specified defaults to 'rproc-dsp-fw')");
+
+/*
+ * OMAP-L138 Technical References:
+ * http://www.ti.com/product/omap-l138
+ */
+#define SYSCFG_CHIPSIG0 BIT(0)
+#define SYSCFG_CHIPSIG1 BIT(1)
+#define SYSCFG_CHIPSIG2 BIT(2)
+#define SYSCFG_CHIPSIG3 BIT(3)
+#define SYSCFG_CHIPSIG4 BIT(4)
+
+/**
+ * struct da8xx_rproc - da8xx remote processor instance state
+ * @rproc: rproc handle
+ * @dsp_clk: placeholder for platform's DSP clk
+ * @ack_fxn: chip-specific ack function for ack'ing irq
+ * @irq_data: ack_fxn function parameter
+ * @chipsig: virt ptr to DSP interrupt registers (CHIPSIG & CHIPSIG_CLR)
+ * @bootreg: virt ptr to DSP boot address register (HOST1CFG)
+ * @irq: irq # used by this instance
+ */
+struct da8xx_rproc {
+	struct rproc *rproc;
+	struct clk *dsp_clk;
+	void (*ack_fxn)(struct irq_data *data);
+	struct irq_data *irq_data;
+	void __iomem *chipsig;
+	void __iomem *bootreg;
+	int irq;
+};
+
+/**
+ * handle_event() - inbound virtqueue message workqueue function
+ *
+ * This function is registered as a kernel thread and is scheduled by the
+ * kernel handler.
+ */
+static irqreturn_t handle_event(int irq, void *p)
+{
+	struct rproc *rproc = (struct rproc *)p;
+
+	/* Process incoming buffers on all our vrings */
+	rproc_vq_interrupt(rproc, 0);
+	rproc_vq_interrupt(rproc, 1);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * da8xx_rproc_callback() - inbound virtqueue message handler
+ *
+ * This handler is invoked directly by the kernel whenever the remote
+ * core (DSP) has modified the state of a virtqueue.  There is no
+ * "payload" message indicating the virtqueue index as is the case with
+ * mailbox-based implementations on OMAP4.  As such, this handler "polls"
+ * each known virtqueue index for every invocation.
+ */
+static irqreturn_t da8xx_rproc_callback(int irq, void *p)
+{
+	struct rproc *rproc = (struct rproc *)p;
+	struct da8xx_rproc *drproc = (struct da8xx_rproc *)rproc->priv;
+	u32 chipsig;
+
+	chipsig = readl(drproc->chipsig);
+	if (chipsig & SYSCFG_CHIPSIG0) {
+		/* Clear interrupt level source */
+		writel(SYSCFG_CHIPSIG0, drproc->chipsig + 4);
+
+		/*
+		 * ACK intr to AINTC.
+		 *
+		 * It has already been ack'ed by the kernel before calling
+		 * this function, but since the ARM<->DSP interrupts in the
+		 * CHIPSIG register are "level" instead of "pulse" variety,
+		 * we need to ack it after taking down the level else we'll
+		 * be called again immediately after returning.
+		 */
+		drproc->ack_fxn(drproc->irq_data);
+
+		return IRQ_WAKE_THREAD;
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int da8xx_rproc_start(struct rproc *rproc)
+{
+	struct device *dev = rproc->dev.parent;
+	struct da8xx_rproc *drproc = (struct da8xx_rproc *)rproc->priv;
+	struct clk *dsp_clk = drproc->dsp_clk;
+
+	/* hw requires the start (boot) address be on 1KB boundary */
+	if (rproc->bootaddr & 0x3ff) {
+		dev_err(dev, "invalid boot address: must be aligned to 1KB\n");
+
+		return -EINVAL;
+	}
+
+	writel(rproc->bootaddr, drproc->bootreg);
+
+	clk_enable(dsp_clk);
+	davinci_clk_reset_deassert(dsp_clk);
+
+	return 0;
+}
+
+static int da8xx_rproc_stop(struct rproc *rproc)
+{
+	struct da8xx_rproc *drproc = rproc->priv;
+
+	clk_disable(drproc->dsp_clk);
+
+	return 0;
+}
+
+/* kick a virtqueue */
+static void da8xx_rproc_kick(struct rproc *rproc, int vqid)
+{
+	struct da8xx_rproc *drproc = (struct da8xx_rproc *)rproc->priv;
+
+	/* Interupt remote proc */
+	writel(SYSCFG_CHIPSIG2, drproc->chipsig);
+}
+
+static struct rproc_ops da8xx_rproc_ops = {
+	.start = da8xx_rproc_start,
+	.stop = da8xx_rproc_stop,
+	.kick = da8xx_rproc_kick,
+};
+
+static int reset_assert(struct device *dev)
+{
+	struct clk *dsp_clk;
+
+	dsp_clk = clk_get(dev, NULL);
+	if (IS_ERR(dsp_clk)) {
+		dev_err(dev, "clk_get error: %ld\n", PTR_ERR(dsp_clk));
+		return PTR_RET(dsp_clk);
+	}
+
+	davinci_clk_reset_assert(dsp_clk);
+	clk_put(dsp_clk);
+
+	return 0;
+}
+
+static int da8xx_rproc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct da8xx_rproc *drproc;
+	struct rproc *rproc;
+	struct irq_data *irq_data;
+	struct resource *bootreg_res;
+	struct resource *chipsig_res;
+	struct clk *dsp_clk;
+	void __iomem *chipsig;
+	void __iomem *bootreg;
+	int irq;
+	int ret;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(dev, "platform_get_irq(pdev, 0) error: %d\n", irq);
+		return irq;
+	}
+
+	irq_data = irq_get_irq_data(irq);
+	if (!irq_data) {
+		dev_err(dev, "irq_get_irq_data(%d): NULL\n", irq);
+		return -EINVAL;
+	}
+
+	bootreg_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!bootreg_res) {
+		dev_err(dev,
+			"platform_get_resource(IORESOURCE_MEM, 0): NULL\n");
+		return -EADDRNOTAVAIL;
+	}
+
+	chipsig_res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!chipsig_res) {
+		dev_err(dev,
+			"platform_get_resource(IORESOURCE_MEM, 1): NULL\n");
+		return -EADDRNOTAVAIL;
+	}
+
+	bootreg = devm_ioremap_resource(dev, bootreg_res);
+	if (IS_ERR(bootreg))
+		return PTR_ERR(bootreg);
+
+	chipsig = devm_ioremap_resource(dev, chipsig_res);
+	if (IS_ERR(chipsig))
+		return PTR_ERR(chipsig);
+
+	dsp_clk = devm_clk_get(dev, NULL);
+	if (IS_ERR(dsp_clk)) {
+		dev_err(dev, "clk_get error: %ld\n", PTR_ERR(dsp_clk));
+
+		return PTR_ERR(dsp_clk);
+	}
+
+	rproc = rproc_alloc(dev, "dsp", &da8xx_rproc_ops, da8xx_fw_name,
+		sizeof(*drproc));
+	if (!rproc)
+		return -ENOMEM;
+
+	drproc = rproc->priv;
+	drproc->rproc = rproc;
+
+	platform_set_drvdata(pdev, rproc);
+
+	/* everything the ISR needs is now setup, so hook it up */
+	ret = devm_request_threaded_irq(dev, irq, da8xx_rproc_callback,
+					handle_event, 0, "da8xx-remoteproc",
+					rproc);
+	if (ret) {
+		dev_err(dev, "devm_request_threaded_irq error: %d\n", ret);
+		goto free_rproc;
+	}
+
+	/*
+	 * rproc_add() can end up enabling the DSP's clk with the DSP
+	 * *not* in reset, but da8xx_rproc_start() needs the DSP to be
+	 * held in reset at the time it is called.
+	 */
+	ret = reset_assert(dev);
+	if (ret)
+		goto free_rproc;
+
+	drproc->chipsig = chipsig;
+	drproc->bootreg = bootreg;
+	drproc->ack_fxn = irq_data->chip->irq_ack;
+	drproc->irq_data = irq_data;
+	drproc->irq = irq;
+	drproc->dsp_clk = dsp_clk;
+
+	ret = rproc_add(rproc);
+	if (ret) {
+		dev_err(dev, "rproc_add failed: %d\n", ret);
+		goto free_rproc;
+	}
+
+	return 0;
+
+free_rproc:
+	rproc_put(rproc);
+
+	return ret;
+}
+
+static int da8xx_rproc_remove(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct rproc *rproc = platform_get_drvdata(pdev);
+	struct da8xx_rproc *drproc = (struct da8xx_rproc *)rproc->priv;
+
+	/*
+	 * It's important to place the DSP in reset before going away,
+	 * since a subsequent insmod of this module may enable the DSP's
+	 * clock before its program/boot-address has been loaded and
+	 * before this module's probe has had a chance to reset the DSP.
+	 * Without the reset, the DSP can lockup permanently when it
+	 * begins executing garbage.
+	 */
+	reset_assert(dev);
+
+	/*
+	 * The devm subsystem might end up releasing things before
+	 * freeing the irq, thus allowing an interrupt to sneak in while
+	 * the device is being removed.  This should prevent that.
+	 */
+	disable_irq(drproc->irq);
+
+	devm_clk_put(dev, drproc->dsp_clk);
+
+	rproc_del(rproc);
+	rproc_put(rproc);
+
+	return 0;
+}
+
+static struct platform_driver da8xx_rproc_driver = {
+	.probe = da8xx_rproc_probe,
+	.remove = da8xx_rproc_remove,
+	.driver = {
+		.name = "davinci-rproc",
+		.owner = THIS_MODULE,
+	},
+};
+
+module_platform_driver(da8xx_rproc_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("DA8XX Remote Processor control driver");
diff --git a/drivers/remoteproc/remoteproc_core.c b/drivers/remoteproc/remoteproc_core.c
index 814af5ab8a72..022dc635d01e 100644
--- a/drivers/remoteproc/remoteproc_core.c
+++ b/drivers/remoteproc/remoteproc_core.c
@@ -37,6 +37,7 @@
 #include <linux/iommu.h>
 #include <linux/idr.h>
 #include <linux/elf.h>
+#include <linux/crc32.h>
 #include <linux/virtio_ids.h>
 #include <linux/virtio_ring.h>
 #include <asm/byteorder.h>
@@ -45,7 +46,8 @@
 
 typedef int (*rproc_handle_resources_t)(struct rproc *rproc,
 				struct resource_table *table, int len);
-typedef int (*rproc_handle_resource_t)(struct rproc *rproc, void *, int avail);
+typedef int (*rproc_handle_resource_t)(struct rproc *rproc,
+				 void *, int offset, int avail);
 
 /* Unique indices for remoteproc devices */
 static DEFINE_IDA(rproc_dev_index);
@@ -192,6 +194,7 @@ int rproc_alloc_vring(struct rproc_vdev *rvdev, int i)
 	struct rproc *rproc = rvdev->rproc;
 	struct device *dev = &rproc->dev;
 	struct rproc_vring *rvring = &rvdev->vring[i];
+	struct fw_rsc_vdev *rsc;
 	dma_addr_t dma;
 	void *va;
 	int ret, size, notifyid;
@@ -202,7 +205,6 @@ int rproc_alloc_vring(struct rproc_vdev *rvdev, int i)
 	/*
 	 * Allocate non-cacheable memory for the vring. In the future
 	 * this call will also configure the IOMMU for us
-	 * TODO: let the rproc know the da of this vring
 	 */
 	va = dma_alloc_coherent(dev->parent, size, &dma, GFP_KERNEL);
 	if (!va) {
@@ -213,7 +215,6 @@ int rproc_alloc_vring(struct rproc_vdev *rvdev, int i)
 	/*
 	 * Assign an rproc-wide unique index for this vring
 	 * TODO: assign a notifyid for rvdev updates as well
-	 * TODO: let the rproc know the notifyid of this vring
 	 * TODO: support predefined notifyids (via resource table)
 	 */
 	ret = idr_alloc(&rproc->notifyids, rvring, 0, 0, GFP_KERNEL);
@@ -224,9 +225,6 @@ int rproc_alloc_vring(struct rproc_vdev *rvdev, int i)
 	}
 	notifyid = ret;
 
-	/* Store largest notifyid */
-	rproc->max_notifyid = max(rproc->max_notifyid, notifyid);
-
 	dev_dbg(dev, "vring%d: va %p dma %llx size %x idr %d\n", i, va,
 				(unsigned long long)dma, size, notifyid);
 
@@ -234,6 +232,15 @@ int rproc_alloc_vring(struct rproc_vdev *rvdev, int i)
 	rvring->dma = dma;
 	rvring->notifyid = notifyid;
 
+	/*
+	 * Let the rproc know the notifyid and da of this vring.
+	 * Not all platforms use dma_alloc_coherent to automatically
+	 * set up the iommu. In this case the device address (da) will
+	 * hold the physical address and not the device address.
+	 */
+	rsc = (void *)rproc->table_ptr + rvdev->rsc_offset;
+	rsc->vring[i].da = dma;
+	rsc->vring[i].notifyid = notifyid;
 	return 0;
 }
 
@@ -268,25 +275,20 @@ rproc_parse_vring(struct rproc_vdev *rvdev, struct fw_rsc_vdev *rsc, int i)
 	return 0;
 }
 
-static int rproc_max_notifyid(int id, void *p, void *data)
-{
-	int *maxid = data;
-	*maxid = max(*maxid, id);
-	return 0;
-}
-
 void rproc_free_vring(struct rproc_vring *rvring)
 {
 	int size = PAGE_ALIGN(vring_size(rvring->len, rvring->align));
 	struct rproc *rproc = rvring->rvdev->rproc;
-	int maxid = 0;
+	int idx = rvring->rvdev->vring - rvring;
+	struct fw_rsc_vdev *rsc;
 
 	dma_free_coherent(rproc->dev.parent, size, rvring->va, rvring->dma);
 	idr_remove(&rproc->notifyids, rvring->notifyid);
 
-	/* Find the largest remaining notifyid */
-	idr_for_each(&rproc->notifyids, rproc_max_notifyid, &maxid);
-	rproc->max_notifyid = maxid;
+	/* reset resource entry info */
+	rsc = (void *)rproc->table_ptr + rvring->rvdev->rsc_offset;
+	rsc->vring[idx].da = 0;
+	rsc->vring[idx].notifyid = -1;
 }
 
 /**
@@ -317,7 +319,7 @@ void rproc_free_vring(struct rproc_vring *rvring)
  * Returns 0 on success, or an appropriate error code otherwise
  */
 static int rproc_handle_vdev(struct rproc *rproc, struct fw_rsc_vdev *rsc,
-								int avail)
+							int offset, int avail)
 {
 	struct device *dev = &rproc->dev;
 	struct rproc_vdev *rvdev;
@@ -358,8 +360,8 @@ static int rproc_handle_vdev(struct rproc *rproc, struct fw_rsc_vdev *rsc,
 			goto free_rvdev;
 	}
 
-	/* remember the device features */
-	rvdev->dfeatures = rsc->dfeatures;
+	/* remember the resource offset*/
+	rvdev->rsc_offset = offset;
 
 	list_add_tail(&rvdev->node, &rproc->rvdevs);
 
@@ -394,7 +396,7 @@ free_rvdev:
  * Returns 0 on success, or an appropriate error code otherwise
  */
 static int rproc_handle_trace(struct rproc *rproc, struct fw_rsc_trace *rsc,
-								int avail)
+							int offset, int avail)
 {
 	struct rproc_mem_entry *trace;
 	struct device *dev = &rproc->dev;
@@ -476,7 +478,7 @@ static int rproc_handle_trace(struct rproc *rproc, struct fw_rsc_trace *rsc,
  * are outside those ranges.
  */
 static int rproc_handle_devmem(struct rproc *rproc, struct fw_rsc_devmem *rsc,
-								int avail)
+							int offset, int avail)
 {
 	struct rproc_mem_entry *mapping;
 	struct device *dev = &rproc->dev;
@@ -549,7 +551,9 @@ out:
  * pressure is important; it may have a substantial impact on performance.
  */
 static int rproc_handle_carveout(struct rproc *rproc,
-				struct fw_rsc_carveout *rsc, int avail)
+						struct fw_rsc_carveout *rsc,
+						int offset, int avail)
+
 {
 	struct rproc_mem_entry *carveout, *mapping;
 	struct device *dev = &rproc->dev;
@@ -671,28 +675,45 @@ free_carv:
 	return ret;
 }
 
+static int rproc_count_vrings(struct rproc *rproc, struct fw_rsc_vdev *rsc,
+			      int offset, int avail)
+{
+	/* Summarize the number of notification IDs */
+	rproc->max_notifyid += rsc->num_of_vrings;
+
+	return 0;
+}
+
 /*
  * A lookup table for resource handlers. The indices are defined in
  * enum fw_resource_type.
  */
-static rproc_handle_resource_t rproc_handle_rsc[] = {
+static rproc_handle_resource_t rproc_loading_handlers[RSC_LAST] = {
 	[RSC_CARVEOUT] = (rproc_handle_resource_t)rproc_handle_carveout,
 	[RSC_DEVMEM] = (rproc_handle_resource_t)rproc_handle_devmem,
 	[RSC_TRACE] = (rproc_handle_resource_t)rproc_handle_trace,
 	[RSC_VDEV] = NULL, /* VDEVs were handled upon registrarion */
 };
 
+static rproc_handle_resource_t rproc_vdev_handler[RSC_LAST] = {
+	[RSC_VDEV] = (rproc_handle_resource_t)rproc_handle_vdev,
+};
+
+static rproc_handle_resource_t rproc_count_vrings_handler[RSC_LAST] = {
+	[RSC_VDEV] = (rproc_handle_resource_t)rproc_count_vrings,
+};
+
 /* handle firmware resource entries before booting the remote processor */
-static int
-rproc_handle_boot_rsc(struct rproc *rproc, struct resource_table *table, int len)
+static int rproc_handle_resources(struct rproc *rproc, int len,
+				  rproc_handle_resource_t handlers[RSC_LAST])
 {
 	struct device *dev = &rproc->dev;
 	rproc_handle_resource_t handler;
 	int ret = 0, i;
 
-	for (i = 0; i < table->num; i++) {
-		int offset = table->offset[i];
-		struct fw_rsc_hdr *hdr = (void *)table + offset;
+	for (i = 0; i < rproc->table_ptr->num; i++) {
+		int offset = rproc->table_ptr->offset[i];
+		struct fw_rsc_hdr *hdr = (void *)rproc->table_ptr + offset;
 		int avail = len - offset - sizeof(*hdr);
 		void *rsc = (void *)hdr + sizeof(*hdr);
 
@@ -709,45 +730,11 @@ rproc_handle_boot_rsc(struct rproc *rproc, struct resource_table *table, int len
 			continue;
 		}
 
-		handler = rproc_handle_rsc[hdr->type];
+		handler = handlers[hdr->type];
 		if (!handler)
 			continue;
 
-		ret = handler(rproc, rsc, avail);
-		if (ret)
-			break;
-	}
-
-	return ret;
-}
-
-/* handle firmware resource entries while registering the remote processor */
-static int
-rproc_handle_virtio_rsc(struct rproc *rproc, struct resource_table *table, int len)
-{
-	struct device *dev = &rproc->dev;
-	int ret = 0, i;
-
-	for (i = 0; i < table->num; i++) {
-		int offset = table->offset[i];
-		struct fw_rsc_hdr *hdr = (void *)table + offset;
-		int avail = len - offset - sizeof(*hdr);
-		struct fw_rsc_vdev *vrsc;
-
-		/* make sure table isn't truncated */
-		if (avail < 0) {
-			dev_err(dev, "rsc table is truncated\n");
-			return -EINVAL;
-		}
-
-		dev_dbg(dev, "%s: rsc type %d\n", __func__, hdr->type);
-
-		if (hdr->type != RSC_VDEV)
-			continue;
-
-		vrsc = (struct fw_rsc_vdev *)hdr->data;
-
-		ret = rproc_handle_vdev(rproc, vrsc, avail);
+		ret = handler(rproc, rsc, offset + sizeof(*hdr), avail);
 		if (ret)
 			break;
 	}
@@ -805,9 +792,12 @@ static int rproc_fw_boot(struct rproc *rproc, const struct firmware *fw)
 {
 	struct device *dev = &rproc->dev;
 	const char *name = rproc->firmware;
-	struct resource_table *table;
+	struct resource_table *table, *loaded_table;
 	int ret, tablesz;
 
+	if (!rproc->table_ptr)
+		return -ENOMEM;
+
 	ret = rproc_fw_sanity_check(rproc, fw);
 	if (ret)
 		return ret;
@@ -833,8 +823,15 @@ static int rproc_fw_boot(struct rproc *rproc, const struct firmware *fw)
 		goto clean_up;
 	}
 
+	/* Verify that resource table in loaded fw is unchanged */
+	if (rproc->table_csum != crc32(0, table, tablesz)) {
+		dev_err(dev, "resource checksum failed, fw changed?\n");
+		ret = -EINVAL;
+		goto clean_up;
+	}
+
 	/* handle fw resources which are required to boot rproc */
-	ret = rproc_handle_boot_rsc(rproc, table, tablesz);
+	ret = rproc_handle_resources(rproc, tablesz, rproc_loading_handlers);
 	if (ret) {
 		dev_err(dev, "Failed to process resources: %d\n", ret);
 		goto clean_up;
@@ -847,6 +844,19 @@ static int rproc_fw_boot(struct rproc *rproc, const struct firmware *fw)
 		goto clean_up;
 	}
 
+	/*
+	 * The starting device has been given the rproc->cached_table as the
+	 * resource table. The address of the vring along with the other
+	 * allocated resources (carveouts etc) is stored in cached_table.
+	 * In order to pass this information to the remote device we must
+	 * copy this information to device memory.
+	 */
+	loaded_table = rproc_find_loaded_rsc_table(rproc, fw);
+	if (!loaded_table)
+		goto clean_up;
+
+	memcpy(loaded_table, rproc->cached_table, tablesz);
+
 	/* power up the remote processor */
 	ret = rproc->ops->start(rproc);
 	if (ret) {
@@ -854,6 +864,13 @@ static int rproc_fw_boot(struct rproc *rproc, const struct firmware *fw)
 		goto clean_up;
 	}
 
+	/*
+	 * Update table_ptr so that all subsequent vring allocations and
+	 * virtio fields manipulation update the actual loaded resource table
+	 * in device memory.
+	 */
+	rproc->table_ptr = loaded_table;
+
 	rproc->state = RPROC_RUNNING;
 
 	dev_info(dev, "remote processor %s is now up\n", rproc->name);
@@ -888,11 +905,30 @@ static void rproc_fw_config_virtio(const struct firmware *fw, void *context)
 	if (!table)
 		goto out;
 
-	/* look for virtio devices and register them */
-	ret = rproc_handle_virtio_rsc(rproc, table, tablesz);
+	rproc->table_csum = crc32(0, table, tablesz);
+
+	/*
+	 * Create a copy of the resource table. When a virtio device starts
+	 * and calls vring_new_virtqueue() the address of the allocated vring
+	 * will be stored in the cached_table. Before the device is started,
+	 * cached_table will be copied into devic memory.
+	 */
+	rproc->cached_table = kmalloc(tablesz, GFP_KERNEL);
+	if (!rproc->cached_table)
+		goto out;
+
+	memcpy(rproc->cached_table, table, tablesz);
+	rproc->table_ptr = rproc->cached_table;
+
+	/* count the number of notify-ids */
+	rproc->max_notifyid = -1;
+	ret = rproc_handle_resources(rproc, tablesz, rproc_count_vrings_handler);
 	if (ret)
 		goto out;
 
+	/* look for virtio devices and register them */
+	ret = rproc_handle_resources(rproc, tablesz, rproc_vdev_handler);
+
 out:
 	release_firmware(fw);
 	/* allow rproc_del() contexts, if any, to proceed */
@@ -950,6 +986,9 @@ int rproc_trigger_recovery(struct rproc *rproc)
 	/* wait until there is no more rproc users */
 	wait_for_completion(&rproc->crash_comp);
 
+	/* Free the copy of the resource table */
+	kfree(rproc->cached_table);
+
 	return rproc_add_virtio_devices(rproc);
 }
 
@@ -1105,6 +1144,9 @@ void rproc_shutdown(struct rproc *rproc)
 
 	rproc_disable_iommu(rproc);
 
+	/* Give the next start a clean resource table */
+	rproc->table_ptr = rproc->cached_table;
+
 	/* if in crash state, unlock crash handler */
 	if (rproc->state == RPROC_CRASHED)
 		complete_all(&rproc->crash_comp);
@@ -1196,11 +1238,11 @@ static struct device_type rproc_type = {
  * @dev: the underlying device
  * @name: name of this remote processor
  * @ops: platform-specific handlers (mainly start/stop)
- * @firmware: name of firmware file to load
+ * @firmware: name of firmware file to load, can be NULL
  * @len: length of private data needed by the rproc driver (in bytes)
  *
  * Allocates a new remote processor handle, but does not register
- * it yet.
+ * it yet. if @firmware is NULL, a default name is used.
  *
  * This function should be used by rproc implementations during initialization
  * of the remote processor.
@@ -1219,19 +1261,39 @@ struct rproc *rproc_alloc(struct device *dev, const char *name,
 				const char *firmware, int len)
 {
 	struct rproc *rproc;
+	char *p, *template = "rproc-%s-fw";
+	int name_len = 0;
 
 	if (!dev || !name || !ops)
 		return NULL;
 
-	rproc = kzalloc(sizeof(struct rproc) + len, GFP_KERNEL);
+	if (!firmware)
+		/*
+		 * Make room for default firmware name (minus %s plus '\0').
+		 * If the caller didn't pass in a firmware name then
+		 * construct a default name.  We're already glomming 'len'
+		 * bytes onto the end of the struct rproc allocation, so do
+		 * a few more for the default firmware name (but only if
+		 * the caller doesn't pass one).
+		 */
+		name_len = strlen(name) + strlen(template) - 2 + 1;
+
+	rproc = kzalloc(sizeof(struct rproc) + len + name_len, GFP_KERNEL);
 	if (!rproc) {
 		dev_err(dev, "%s: kzalloc failed\n", __func__);
 		return NULL;
 	}
 
+	if (!firmware) {
+		p = (char *)rproc + sizeof(struct rproc) + len;
+		snprintf(p, name_len, template, name);
+	} else {
+		p = (char *)firmware;
+	}
+
+	rproc->firmware = p;
 	rproc->name = name;
 	rproc->ops = ops;
-	rproc->firmware = firmware;
 	rproc->priv = &rproc[1];
 
 	device_initialize(&rproc->dev);
@@ -1315,6 +1377,9 @@ int rproc_del(struct rproc *rproc)
 	list_for_each_entry_safe(rvdev, tmp, &rproc->rvdevs, node)
 		rproc_remove_virtio_dev(rvdev);
 
+	/* Free the copy of the resource table */
+	kfree(rproc->cached_table);
+
 	device_del(&rproc->dev);
 
 	return 0;
diff --git a/drivers/remoteproc/remoteproc_elf_loader.c b/drivers/remoteproc/remoteproc_elf_loader.c
index 0d36f94ab51d..ce283a5b42a1 100644
--- a/drivers/remoteproc/remoteproc_elf_loader.c
+++ b/drivers/remoteproc/remoteproc_elf_loader.c
@@ -208,41 +208,22 @@ rproc_elf_load_segments(struct rproc *rproc, const struct firmware *fw)
 	return ret;
 }
 
-/**
- * rproc_elf_find_rsc_table() - find the resource table
- * @rproc: the rproc handle
- * @fw: the ELF firmware image
- * @tablesz: place holder for providing back the table size
- *
- * This function finds the resource table inside the remote processor's
- * firmware. It is used both upon the registration of @rproc (in order
- * to look for and register the supported virito devices), and when the
- * @rproc is booted.
- *
- * Returns the pointer to the resource table if it is found, and write its
- * size into @tablesz. If a valid table isn't found, NULL is returned
- * (and @tablesz isn't set).
- */
-static struct resource_table *
-rproc_elf_find_rsc_table(struct rproc *rproc, const struct firmware *fw,
-							int *tablesz)
+static struct elf32_shdr *
+find_table(struct device *dev, struct elf32_hdr *ehdr, size_t fw_size)
 {
-	struct elf32_hdr *ehdr;
 	struct elf32_shdr *shdr;
+	int i;
 	const char *name_table;
-	struct device *dev = &rproc->dev;
 	struct resource_table *table = NULL;
-	int i;
-	const u8 *elf_data = fw->data;
+	const u8 *elf_data = (void *)ehdr;
 
-	ehdr = (struct elf32_hdr *)elf_data;
+	/* look for the resource table and handle it */
 	shdr = (struct elf32_shdr *)(elf_data + ehdr->e_shoff);
 	name_table = elf_data + shdr[ehdr->e_shstrndx].sh_offset;
 
-	/* look for the resource table and handle it */
 	for (i = 0; i < ehdr->e_shnum; i++, shdr++) {
-		int size = shdr->sh_size;
-		int offset = shdr->sh_offset;
+		u32 size = shdr->sh_size;
+		u32 offset = shdr->sh_offset;
 
 		if (strcmp(name_table + shdr->sh_name, ".resource_table"))
 			continue;
@@ -250,7 +231,7 @@ rproc_elf_find_rsc_table(struct rproc *rproc, const struct firmware *fw,
 		table = (struct resource_table *)(elf_data + offset);
 
 		/* make sure we have the entire table */
-		if (offset + size > fw->size) {
+		if (offset + size > fw_size || offset + size < size) {
 			dev_err(dev, "resource table truncated\n");
 			return NULL;
 		}
@@ -280,16 +261,77 @@ rproc_elf_find_rsc_table(struct rproc *rproc, const struct firmware *fw,
 			return NULL;
 		}
 
-		*tablesz = shdr->sh_size;
-		break;
+		return shdr;
 	}
 
+	return NULL;
+}
+
+/**
+ * rproc_elf_find_rsc_table() - find the resource table
+ * @rproc: the rproc handle
+ * @fw: the ELF firmware image
+ * @tablesz: place holder for providing back the table size
+ *
+ * This function finds the resource table inside the remote processor's
+ * firmware. It is used both upon the registration of @rproc (in order
+ * to look for and register the supported virito devices), and when the
+ * @rproc is booted.
+ *
+ * Returns the pointer to the resource table if it is found, and write its
+ * size into @tablesz. If a valid table isn't found, NULL is returned
+ * (and @tablesz isn't set).
+ */
+static struct resource_table *
+rproc_elf_find_rsc_table(struct rproc *rproc, const struct firmware *fw,
+			 int *tablesz)
+{
+	struct elf32_hdr *ehdr;
+	struct elf32_shdr *shdr;
+	struct device *dev = &rproc->dev;
+	struct resource_table *table = NULL;
+	const u8 *elf_data = fw->data;
+
+	ehdr = (struct elf32_hdr *)elf_data;
+
+	shdr = find_table(dev, ehdr, fw->size);
+	if (!shdr)
+		return NULL;
+
+	table = (struct resource_table *)(elf_data + shdr->sh_offset);
+	*tablesz = shdr->sh_size;
+
 	return table;
 }
 
+/**
+ * rproc_elf_find_loaded_rsc_table() - find the loaded resource table
+ * @rproc: the rproc handle
+ * @fw: the ELF firmware image
+ *
+ * This function finds the location of the loaded resource table. Don't
+ * call this function if the table wasn't loaded yet - it's a bug if you do.
+ *
+ * Returns the pointer to the resource table if it is found or NULL otherwise.
+ * If the table wasn't loaded yet the result is unspecified.
+ */
+static struct resource_table *
+rproc_elf_find_loaded_rsc_table(struct rproc *rproc, const struct firmware *fw)
+{
+	struct elf32_hdr *ehdr = (struct elf32_hdr *)fw->data;
+	struct elf32_shdr *shdr;
+
+	shdr = find_table(&rproc->dev, ehdr, fw->size);
+	if (!shdr)
+		return NULL;
+
+	return rproc_da_to_va(rproc, shdr->sh_addr, shdr->sh_size);
+}
+
 const struct rproc_fw_ops rproc_elf_fw_ops = {
 	.load = rproc_elf_load_segments,
 	.find_rsc_table = rproc_elf_find_rsc_table,
+	.find_loaded_rsc_table = rproc_elf_find_loaded_rsc_table,
 	.sanity_check = rproc_elf_sanity_check,
 	.get_boot_addr = rproc_elf_get_boot_addr
 };
diff --git a/drivers/remoteproc/remoteproc_internal.h b/drivers/remoteproc/remoteproc_internal.h
index 7bb66482d061..157e762c1571 100644
--- a/drivers/remoteproc/remoteproc_internal.h
+++ b/drivers/remoteproc/remoteproc_internal.h
@@ -27,7 +27,8 @@ struct rproc;
 
 /**
  * struct rproc_fw_ops - firmware format specific operations.
- * @find_rsc_table:	finds the resource table inside the firmware image
+ * @find_rsc_table:	find the resource table inside the firmware image
+ * @find_loaded_rsc_table: find the loaded resouce table
  * @load:		load firmeware to memory, where the remote processor
  *			expects to find it
  * @sanity_check:	sanity check the fw image
@@ -37,6 +38,8 @@ struct rproc_fw_ops {
 	struct resource_table *(*find_rsc_table) (struct rproc *rproc,
 						const struct firmware *fw,
 						int *tablesz);
+	struct resource_table *(*find_loaded_rsc_table)(struct rproc *rproc,
+						const struct firmware *fw);
 	int (*load)(struct rproc *rproc, const struct firmware *fw);
 	int (*sanity_check)(struct rproc *rproc, const struct firmware *fw);
 	u32 (*get_boot_addr)(struct rproc *rproc, const struct firmware *fw);
@@ -102,6 +105,16 @@ struct resource_table *rproc_find_rsc_table(struct rproc *rproc,
 	return NULL;
 }
 
+static inline
+struct resource_table *rproc_find_loaded_rsc_table(struct rproc *rproc,
+                                const struct firmware *fw)
+{
+	if (rproc->fw_ops->find_loaded_rsc_table)
+		return rproc->fw_ops->find_loaded_rsc_table(rproc, fw);
+
+        return NULL;
+}
+
 extern const struct rproc_fw_ops rproc_elf_fw_ops;
 
 #endif /* REMOTEPROC_INTERNAL_H */
diff --git a/drivers/remoteproc/remoteproc_virtio.c b/drivers/remoteproc/remoteproc_virtio.c
index afed9b7731c4..b09c75c21b60 100644
--- a/drivers/remoteproc/remoteproc_virtio.c
+++ b/drivers/remoteproc/remoteproc_virtio.c
@@ -173,25 +173,35 @@ error:
 	return ret;
 }
 
-/*
- * We don't support yet real virtio status semantics.
- *
- * The plan is to provide this via the VDEV resource entry
- * which is part of the firmware: this way the remote processor
- * will be able to access the status values as set by us.
- */
 static u8 rproc_virtio_get_status(struct virtio_device *vdev)
 {
-	return 0;
+	struct rproc_vdev *rvdev = vdev_to_rvdev(vdev);
+	struct fw_rsc_vdev *rsc;
+
+	rsc = (void *)rvdev->rproc->table_ptr + rvdev->rsc_offset;
+
+	return rsc->status;
 }
 
 static void rproc_virtio_set_status(struct virtio_device *vdev, u8 status)
 {
+	struct rproc_vdev *rvdev = vdev_to_rvdev(vdev);
+	struct fw_rsc_vdev *rsc;
+
+	rsc = (void *)rvdev->rproc->table_ptr + rvdev->rsc_offset;
+
+	rsc->status = status;
 	dev_dbg(&vdev->dev, "status: %d\n", status);
 }
 
 static void rproc_virtio_reset(struct virtio_device *vdev)
 {
+	struct rproc_vdev *rvdev = vdev_to_rvdev(vdev);
+	struct fw_rsc_vdev *rsc;
+
+	rsc = (void *)rvdev->rproc->table_ptr + rvdev->rsc_offset;
+
+	rsc->status = 0;
 	dev_dbg(&vdev->dev, "reset !\n");
 }
 
@@ -199,13 +209,19 @@ static void rproc_virtio_reset(struct virtio_device *vdev)
 static u32 rproc_virtio_get_features(struct virtio_device *vdev)
 {
 	struct rproc_vdev *rvdev = vdev_to_rvdev(vdev);
+	struct fw_rsc_vdev *rsc;
+
+	rsc = (void *)rvdev->rproc->table_ptr + rvdev->rsc_offset;
 
-	return rvdev->dfeatures;
+	return rsc->dfeatures;
 }
 
 static void rproc_virtio_finalize_features(struct virtio_device *vdev)
 {
 	struct rproc_vdev *rvdev = vdev_to_rvdev(vdev);
+	struct fw_rsc_vdev *rsc;
+
+	rsc = (void *)rvdev->rproc->table_ptr + rvdev->rsc_offset;
 
 	/* Give virtio_ring a chance to accept features */
 	vring_transport_features(vdev);
@@ -213,13 +229,44 @@ static void rproc_virtio_finalize_features(struct virtio_device *vdev)
 	/*
 	 * Remember the finalized features of our vdev, and provide it
 	 * to the remote processor once it is powered on.
-	 *
-	 * Similarly to the status field, we don't expose yet the negotiated
-	 * features to the remote processors at this point. This will be
-	 * fixed as part of a small resource table overhaul and then an
-	 * extension of the virtio resource entries.
 	 */
-	rvdev->gfeatures = vdev->features[0];
+	rsc->gfeatures = vdev->features[0];
+}
+
+static void rproc_virtio_get(struct virtio_device *vdev, unsigned offset,
+							void *buf, unsigned len)
+{
+	struct rproc_vdev *rvdev = vdev_to_rvdev(vdev);
+	struct fw_rsc_vdev *rsc;
+	void *cfg;
+
+	rsc = (void *)rvdev->rproc->table_ptr + rvdev->rsc_offset;
+	cfg = &rsc->vring[rsc->num_of_vrings];
+
+	if (offset + len > rsc->config_len || offset + len < len) {
+		dev_err(&vdev->dev, "rproc_virtio_get: access out of bounds\n");
+		return;
+	}
+
+	memcpy(buf, cfg + offset, len);
+}
+
+static void rproc_virtio_set(struct virtio_device *vdev, unsigned offset,
+		      const void *buf, unsigned len)
+{
+	struct rproc_vdev *rvdev = vdev_to_rvdev(vdev);
+	struct fw_rsc_vdev *rsc;
+	void *cfg;
+
+	rsc = (void *)rvdev->rproc->table_ptr + rvdev->rsc_offset;
+	cfg = &rsc->vring[rsc->num_of_vrings];
+
+	if (offset + len > rsc->config_len || offset + len < len) {
+		dev_err(&vdev->dev, "rproc_virtio_set: access out of bounds\n");
+		return;
+	}
+
+	memcpy(cfg + offset, buf, len);
 }
 
 static const struct virtio_config_ops rproc_virtio_config_ops = {
@@ -230,6 +277,8 @@ static const struct virtio_config_ops rproc_virtio_config_ops = {
 	.reset		= rproc_virtio_reset,
 	.set_status	= rproc_virtio_set_status,
 	.get_status	= rproc_virtio_get_status,
+	.get		= rproc_virtio_get,
+	.set		= rproc_virtio_set,
 };
 
 /*
diff --git a/drivers/remoteproc/ste_modem_rproc.c b/drivers/remoteproc/ste_modem_rproc.c
index fb95c4220052..1ec39a4c0b3e 100644
--- a/drivers/remoteproc/ste_modem_rproc.c
+++ b/drivers/remoteproc/ste_modem_rproc.c
@@ -64,26 +64,18 @@ static int sproc_load_segments(struct rproc *rproc, const struct firmware *fw)
 }
 
 /* Find the entry for resource table in the Table of Content */
-static struct ste_toc_entry *sproc_find_rsc_entry(const struct firmware *fw)
+static const struct ste_toc_entry *sproc_find_rsc_entry(const void *data)
 {
 	int i;
-	struct ste_toc *toc;
-
-	if (!fw)
-		return NULL;
-
-	toc = (void *)fw->data;
+	const struct ste_toc *toc;
+	toc = data;
 
 	/* Search the table for the resource table */
 	for (i = 0; i < SPROC_MAX_TOC_ENTRIES &&
 		    toc->table[i].start != 0xffffffff; i++) {
-
 		if (!strncmp(toc->table[i].name, SPROC_RESOURCE_NAME,
-			     sizeof(toc->table[i].name))) {
-			if (toc->table[i].start > fw->size)
-				return NULL;
+			     sizeof(toc->table[i].name)))
 			return &toc->table[i];
-		}
 	}
 
 	return NULL;
@@ -96,9 +88,12 @@ sproc_find_rsc_table(struct rproc *rproc, const struct firmware *fw,
 {
 	struct sproc *sproc = rproc->priv;
 	struct resource_table *table;
-	struct ste_toc_entry *entry;
+	const struct ste_toc_entry *entry;
 
-	entry = sproc_find_rsc_entry(fw);
+	if (!fw)
+		return NULL;
+
+	entry = sproc_find_rsc_entry(fw->data);
 	if (!entry) {
 		sproc_err(sproc, "resource table not found in fw\n");
 		return NULL;
@@ -149,10 +144,30 @@ sproc_find_rsc_table(struct rproc *rproc, const struct firmware *fw,
 	return table;
 }
 
+/* Find the resource table inside the remote processor's firmware. */
+static struct resource_table *
+sproc_find_loaded_rsc_table(struct rproc *rproc, const struct firmware *fw)
+{
+	struct sproc *sproc = rproc->priv;
+	const struct ste_toc_entry *entry;
+
+	if (!fw || !sproc->fw_addr)
+		return NULL;
+
+	entry = sproc_find_rsc_entry(sproc->fw_addr);
+	if (!entry) {
+		sproc_err(sproc, "resource table not found in fw\n");
+		return NULL;
+	}
+
+	return sproc->fw_addr + entry->start;
+}
+
 /* STE modem firmware handler operations */
 const struct rproc_fw_ops sproc_fw_ops = {
 	.load = sproc_load_segments,
 	.find_rsc_table = sproc_find_rsc_table,
+	.find_loaded_rsc_table = sproc_find_loaded_rsc_table,
 };
 
 /* Kick the modem with specified notification id */
@@ -198,7 +213,7 @@ static int sproc_start(struct rproc *rproc)
 	}
 
 	/* Subscribe to notifications */
-	for (i = 0; i < rproc->max_notifyid; i++) {
+	for (i = 0; i <= rproc->max_notifyid; i++) {
 		err = sproc->mdev->ops.kick_subscribe(sproc->mdev, i);
 		if (err) {
 			sproc_err(sproc,
diff --git a/drivers/reset/Kconfig b/drivers/reset/Kconfig
new file mode 100644
index 000000000000..c9d04f797862
--- /dev/null
+++ b/drivers/reset/Kconfig
@@ -0,0 +1,13 @@
+config ARCH_HAS_RESET_CONTROLLER
+	bool
+
+menuconfig RESET_CONTROLLER
+	bool "Reset Controller Support"
+	default y if ARCH_HAS_RESET_CONTROLLER
+	help
+	  Generic Reset Controller support.
+
+	  This framework is designed to abstract reset handling of devices
+	  via GPIOs or SoC-internal reset controller modules.
+
+	  If unsure, say no.
diff --git a/drivers/reset/Makefile b/drivers/reset/Makefile
new file mode 100644
index 000000000000..1e2d83f2b995
--- /dev/null
+++ b/drivers/reset/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_RESET_CONTROLLER) += core.o
diff --git a/drivers/reset/core.c b/drivers/reset/core.c
new file mode 100644
index 000000000000..d1b6089a0ef8
--- /dev/null
+++ b/drivers/reset/core.c
@@ -0,0 +1,297 @@
+/*
+ * Reset Controller framework
+ *
+ * Copyright 2013 Philipp Zabel, Pengutronix
+ *
+ * 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/device.h>
+#include <linux/err.h>
+#include <linux/export.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/reset.h>
+#include <linux/reset-controller.h>
+#include <linux/slab.h>
+
+static DEFINE_MUTEX(reset_controller_list_mutex);
+static LIST_HEAD(reset_controller_list);
+
+/**
+ * struct reset_control - a reset control
+ * @rcdev: a pointer to the reset controller device
+ *         this reset control belongs to
+ * @id: ID of the reset controller in the reset
+ *      controller device
+ */
+struct reset_control {
+	struct reset_controller_dev *rcdev;
+	struct device *dev;
+	unsigned int id;
+};
+
+/**
+ * of_reset_simple_xlate - translate reset_spec to the reset line number
+ * @rcdev: a pointer to the reset controller device
+ * @reset_spec: reset line specifier as found in the device tree
+ * @flags: a flags pointer to fill in (optional)
+ *
+ * This simple translation function should be used for reset controllers
+ * with 1:1 mapping, where reset lines can be indexed by number without gaps.
+ */
+int of_reset_simple_xlate(struct reset_controller_dev *rcdev,
+			  const struct of_phandle_args *reset_spec)
+{
+	if (WARN_ON(reset_spec->args_count != rcdev->of_reset_n_cells))
+		return -EINVAL;
+
+	if (reset_spec->args[0] >= rcdev->nr_resets)
+		return -EINVAL;
+
+	return reset_spec->args[0];
+}
+EXPORT_SYMBOL_GPL(of_reset_simple_xlate);
+
+/**
+ * reset_controller_register - register a reset controller device
+ * @rcdev: a pointer to the initialized reset controller device
+ */
+int reset_controller_register(struct reset_controller_dev *rcdev)
+{
+	if (!rcdev->of_xlate) {
+		rcdev->of_reset_n_cells = 1;
+		rcdev->of_xlate = of_reset_simple_xlate;
+	}
+
+	mutex_lock(&reset_controller_list_mutex);
+	list_add(&rcdev->list, &reset_controller_list);
+	mutex_unlock(&reset_controller_list_mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(reset_controller_register);
+
+/**
+ * reset_controller_unregister - unregister a reset controller device
+ * @rcdev: a pointer to the reset controller device
+ */
+void reset_controller_unregister(struct reset_controller_dev *rcdev)
+{
+	mutex_lock(&reset_controller_list_mutex);
+	list_del(&rcdev->list);
+	mutex_unlock(&reset_controller_list_mutex);
+}
+EXPORT_SYMBOL_GPL(reset_controller_unregister);
+
+/**
+ * reset_control_reset - reset the controlled device
+ * @rstc: reset controller
+ */
+int reset_control_reset(struct reset_control *rstc)
+{
+	if (rstc->rcdev->ops->reset)
+		return rstc->rcdev->ops->reset(rstc->rcdev, rstc->id);
+
+	return -ENOSYS;
+}
+EXPORT_SYMBOL_GPL(reset_control_reset);
+
+/**
+ * reset_control_assert - asserts the reset line
+ * @rstc: reset controller
+ */
+int reset_control_assert(struct reset_control *rstc)
+{
+	if (rstc->rcdev->ops->assert)
+		return rstc->rcdev->ops->assert(rstc->rcdev, rstc->id);
+
+	return -ENOSYS;
+}
+EXPORT_SYMBOL_GPL(reset_control_assert);
+
+/**
+ * reset_control_deassert - deasserts the reset line
+ * @rstc: reset controller
+ */
+int reset_control_deassert(struct reset_control *rstc)
+{
+	if (rstc->rcdev->ops->deassert)
+		return rstc->rcdev->ops->deassert(rstc->rcdev, rstc->id);
+
+	return -ENOSYS;
+}
+EXPORT_SYMBOL_GPL(reset_control_deassert);
+
+/**
+ * reset_control_get - Lookup and obtain a reference to a reset controller.
+ * @dev: device to be reset by the controller
+ * @id: reset line name
+ *
+ * Returns a struct reset_control or IS_ERR() condition containing errno.
+ *
+ * Use of id names is optional.
+ */
+struct reset_control *reset_control_get(struct device *dev, const char *id)
+{
+	struct reset_control *rstc = ERR_PTR(-EPROBE_DEFER);
+	struct reset_controller_dev *r, *rcdev;
+	struct of_phandle_args args;
+	int index = 0;
+	int rstc_id;
+	int ret;
+
+	if (!dev)
+		return ERR_PTR(-EINVAL);
+
+	if (id)
+		index = of_property_match_string(dev->of_node,
+						 "reset-names", id);
+	ret = of_parse_phandle_with_args(dev->of_node, "resets", "#reset-cells",
+					 index, &args);
+	if (ret)
+		return ERR_PTR(ret);
+
+	mutex_lock(&reset_controller_list_mutex);
+	rcdev = NULL;
+	list_for_each_entry(r, &reset_controller_list, list) {
+		if (args.np == r->of_node) {
+			rcdev = r;
+			break;
+		}
+	}
+	of_node_put(args.np);
+
+	if (!rcdev) {
+		mutex_unlock(&reset_controller_list_mutex);
+		return ERR_PTR(-ENODEV);
+	}
+
+	rstc_id = rcdev->of_xlate(rcdev, &args);
+	if (rstc_id < 0) {
+		mutex_unlock(&reset_controller_list_mutex);
+		return ERR_PTR(rstc_id);
+	}
+
+	try_module_get(rcdev->owner);
+	mutex_unlock(&reset_controller_list_mutex);
+
+	rstc = kzalloc(sizeof(*rstc), GFP_KERNEL);
+	if (!rstc) {
+		module_put(rcdev->owner);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	rstc->dev = dev;
+	rstc->rcdev = rcdev;
+	rstc->id = rstc_id;
+
+	return rstc;
+}
+EXPORT_SYMBOL_GPL(reset_control_get);
+
+/**
+ * reset_control_put - free the reset controller
+ * @rstc: reset controller
+ */
+
+void reset_control_put(struct reset_control *rstc)
+{
+	if (IS_ERR(rstc))
+		return;
+
+	module_put(rstc->rcdev->owner);
+	kfree(rstc);
+}
+EXPORT_SYMBOL_GPL(reset_control_put);
+
+static void devm_reset_control_release(struct device *dev, void *res)
+{
+	reset_control_put(*(struct reset_control **)res);
+}
+
+/**
+ * devm_reset_control_get - resource managed reset_control_get()
+ * @dev: device to be reset by the controller
+ * @id: reset line name
+ *
+ * Managed reset_control_get(). For reset controllers returned from this
+ * function, reset_control_put() is called automatically on driver detach.
+ * See reset_control_get() for more information.
+ */
+struct reset_control *devm_reset_control_get(struct device *dev, const char *id)
+{
+	struct reset_control **ptr, *rstc;
+
+	ptr = devres_alloc(devm_reset_control_release, sizeof(*ptr),
+			   GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	rstc = reset_control_get(dev, id);
+	if (!IS_ERR(rstc)) {
+		*ptr = rstc;
+		devres_add(dev, ptr);
+	} else {
+		devres_free(ptr);
+	}
+
+	return rstc;
+}
+EXPORT_SYMBOL_GPL(devm_reset_control_get);
+
+static int devm_reset_control_match(struct device *dev, void *res, void *data)
+{
+	struct reset_control **rstc = res;
+	if (WARN_ON(!rstc || !*rstc))
+		return 0;
+	return *rstc == data;
+}
+
+/**
+ * devm_reset_control_put - resource managed reset_control_put()
+ * @rstc: reset controller to free
+ *
+ * Deallocate a reset control allocated withd devm_reset_control_get().
+ * This function will not need to be called normally, as devres will take
+ * care of freeing the resource.
+ */
+void devm_reset_control_put(struct reset_control *rstc)
+{
+	int ret;
+
+	ret = devres_release(rstc->dev, devm_reset_control_release,
+			     devm_reset_control_match, rstc);
+	if (ret)
+		WARN_ON(ret);
+}
+EXPORT_SYMBOL_GPL(devm_reset_control_put);
+
+/**
+ * device_reset - find reset controller associated with the device
+ *                and perform reset
+ * @dev: device to be reset by the controller
+ *
+ * Convenience wrapper for reset_control_get() and reset_control_reset().
+ * This is useful for the common case of devices with single, dedicated reset
+ * lines.
+ */
+int device_reset(struct device *dev)
+{
+	struct reset_control *rstc;
+	int ret;
+
+	rstc = reset_control_get(dev, NULL);
+	if (IS_ERR(rstc))
+		return PTR_ERR(rstc);
+
+	ret = reset_control_reset(rstc);
+
+	reset_control_put(rstc);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(device_reset);
diff --git a/drivers/rpmsg/Kconfig b/drivers/rpmsg/Kconfig
index f6e0ea6ffda5..69a219387582 100644
--- a/drivers/rpmsg/Kconfig
+++ b/drivers/rpmsg/Kconfig
@@ -4,5 +4,6 @@ menu "Rpmsg drivers"
 config RPMSG
 	tristate
 	select VIRTIO
+	select VIRTUALIZATION
 
 endmenu
diff --git a/drivers/rpmsg/virtio_rpmsg_bus.c b/drivers/rpmsg/virtio_rpmsg_bus.c
index 7861f1119b7d..b6135d4d54eb 100644
--- a/drivers/rpmsg/virtio_rpmsg_bus.c
+++ b/drivers/rpmsg/virtio_rpmsg_bus.c
@@ -757,14 +757,14 @@ int rpmsg_send_offchannel_raw(struct rpmsg_channel *rpdev, u32 src, u32 dst,
 	mutex_lock(&vrp->tx_lock);
 
 	/* add message to the remote processor's virtqueue */
-	err = virtqueue_add_buf(vrp->svq, &sg, 1, 0, msg, GFP_KERNEL);
+	err = virtqueue_add_outbuf(vrp->svq, &sg, 1, msg, GFP_KERNEL);
 	if (err) {
 		/*
 		 * need to reclaim the buffer here, otherwise it's lost
 		 * (memory won't leak, but rpmsg won't use it again for TX).
 		 * this will wait for a buffer management overhaul.
 		 */
-		dev_err(dev, "virtqueue_add_buf failed: %d\n", err);
+		dev_err(dev, "virtqueue_add_outbuf failed: %d\n", err);
 		goto out;
 	}
 
@@ -776,23 +776,13 @@ out:
 }
 EXPORT_SYMBOL(rpmsg_send_offchannel_raw);
 
-/* called when an rx buffer is used, and it's time to digest a message */
-static void rpmsg_recv_done(struct virtqueue *rvq)
+static int rpmsg_recv_single(struct virtproc_info *vrp, struct device *dev,
+			     struct rpmsg_hdr *msg, unsigned int len)
 {
-	struct rpmsg_hdr *msg;
-	unsigned int len;
 	struct rpmsg_endpoint *ept;
 	struct scatterlist sg;
-	struct virtproc_info *vrp = rvq->vdev->priv;
-	struct device *dev = &rvq->vdev->dev;
 	int err;
 
-	msg = virtqueue_get_buf(rvq, &len);
-	if (!msg) {
-		dev_err(dev, "uhm, incoming signal, but no used buffer ?\n");
-		return;
-	}
-
 	dev_dbg(dev, "From: 0x%x, To: 0x%x, Len: %d, Flags: %d, Reserved: %d\n",
 					msg->src, msg->dst, msg->len,
 					msg->flags, msg->reserved);
@@ -806,7 +796,7 @@ static void rpmsg_recv_done(struct virtqueue *rvq)
 	if (len > RPMSG_BUF_SIZE ||
 		msg->len > (len - sizeof(struct rpmsg_hdr))) {
 		dev_warn(dev, "inbound msg too big: (%d, %d)\n", len, msg->len);
-		return;
+		return -EINVAL;
 	}
 
 	/* use the dst addr to fetch the callback of the appropriate user */
@@ -839,14 +829,45 @@ static void rpmsg_recv_done(struct virtqueue *rvq)
 	sg_init_one(&sg, msg, RPMSG_BUF_SIZE);
 
 	/* add the buffer back to the remote processor's virtqueue */
-	err = virtqueue_add_buf(vrp->rvq, &sg, 0, 1, msg, GFP_KERNEL);
+	err = virtqueue_add_inbuf(vrp->rvq, &sg, 1, msg, GFP_KERNEL);
 	if (err < 0) {
 		dev_err(dev, "failed to add a virtqueue buffer: %d\n", err);
+		return err;
+	}
+
+	return 0;
+}
+
+/* called when an rx buffer is used, and it's time to digest a message */
+static void rpmsg_recv_done(struct virtqueue *rvq)
+{
+	struct virtproc_info *vrp = rvq->vdev->priv;
+	struct device *dev = &rvq->vdev->dev;
+	struct rpmsg_hdr *msg;
+	unsigned int len, msgs_received = 0;
+	int err;
+
+	msg = virtqueue_get_buf(rvq, &len);
+	if (!msg) {
+		dev_err(dev, "uhm, incoming signal, but no used buffer ?\n");
 		return;
 	}
 
+	while (msg) {
+		err = rpmsg_recv_single(vrp, dev, msg, len);
+		if (err)
+			break;
+
+		msgs_received++;
+
+		msg = virtqueue_get_buf(rvq, &len);
+	};
+
+	dev_dbg(dev, "Received %u messages\n", msgs_received);
+
 	/* tell the remote processor we added another available rx buffer */
-	virtqueue_kick(vrp->rvq);
+	if (msgs_received)
+		virtqueue_kick(vrp->rvq);
 }
 
 /*
@@ -972,7 +993,7 @@ static int rpmsg_probe(struct virtio_device *vdev)
 
 		sg_init_one(&sg, cpu_addr, RPMSG_BUF_SIZE);
 
-		err = virtqueue_add_buf(vrp->rvq, &sg, 0, 1, cpu_addr,
+		err = virtqueue_add_inbuf(vrp->rvq, &sg, 1, cpu_addr,
 								GFP_KERNEL);
 		WARN_ON(err); /* sanity check; this can't really happen */
 	}
diff --git a/drivers/rtc/rtc-proc.c b/drivers/rtc/rtc-proc.c
index e96236ac2e78..ffa69e1c9245 100644
--- a/drivers/rtc/rtc-proc.c
+++ b/drivers/rtc/rtc-proc.c
@@ -110,7 +110,7 @@ static int rtc_proc_show(struct seq_file *seq, void *offset)
 static int rtc_proc_open(struct inode *inode, struct file *file)
 {
 	int ret;
-	struct rtc_device *rtc = PDE(inode)->data;
+	struct rtc_device *rtc = PDE_DATA(inode);
 
 	if (!try_module_get(THIS_MODULE))
 		return -ENODEV;
diff --git a/drivers/rtc/rtc-rs5c372.c b/drivers/rtc/rtc-rs5c372.c
index 224d634322b4..ccf54f06396b 100644
--- a/drivers/rtc/rtc-rs5c372.c
+++ b/drivers/rtc/rtc-rs5c372.c
@@ -68,6 +68,7 @@
 enum rtc_type {
 	rtc_undef = 0,
 	rtc_r2025sd,
+	rtc_r2221tl,
 	rtc_rs5c372a,
 	rtc_rs5c372b,
 	rtc_rv5c386,
@@ -76,6 +77,7 @@ enum rtc_type {
 
 static const struct i2c_device_id rs5c372_id[] = {
 	{ "r2025sd", rtc_r2025sd },
+	{ "r2221tl", rtc_r2221tl },
 	{ "rs5c372a", rtc_rs5c372a },
 	{ "rs5c372b", rtc_rs5c372b },
 	{ "rv5c386", rtc_rv5c386 },
@@ -529,6 +531,7 @@ static int rs5c_oscillator_setup(struct rs5c372 *rs5c372)
 		rs5c372->time24 = 1;
 		break;
 	case rtc_r2025sd:
+	case rtc_r2221tl:
 	case rtc_rv5c386:
 	case rtc_rv5c387a:
 		buf[0] |= RV5C387_CTRL1_24;
@@ -609,6 +612,7 @@ static int rs5c372_probe(struct i2c_client *client,
 			rs5c372->time24 = 1;
 		break;
 	case rtc_r2025sd:
+	case rtc_r2221tl:
 	case rtc_rv5c386:
 	case rtc_rv5c387a:
 		if (rs5c372->regs[RS5C_REG_CTRL1] & RV5C387_CTRL1_24)
@@ -640,6 +644,7 @@ static int rs5c372_probe(struct i2c_client *client,
 	dev_info(&client->dev, "%s found, %s, driver version " DRV_VERSION "\n",
 			({ char *s; switch (rs5c372->type) {
 			case rtc_r2025sd:	s = "r2025sd"; break;
+			case rtc_r2221tl:	s = "r2221tl"; break;
 			case rtc_rs5c372a:	s = "rs5c372a"; break;
 			case rtc_rs5c372b:	s = "rs5c372b"; break;
 			case rtc_rv5c386:	s = "rv5c386"; break;
diff --git a/drivers/rtc/rtc-s3c.c b/drivers/rtc/rtc-s3c.c
index 8e96c00936be..14040b22888d 100644
--- a/drivers/rtc/rtc-s3c.c
+++ b/drivers/rtc/rtc-s3c.c
@@ -29,9 +29,8 @@
 #include <linux/uaccess.h>
 #include <linux/io.h>
 
-#include <mach/hardware.h>
 #include <asm/irq.h>
-#include <plat/regs-rtc.h>
+#include "rtc-s3c.h"
 
 enum s3c_cpu_type {
 	TYPE_S3C2410,
diff --git a/drivers/rtc/rtc-s3c.h b/drivers/rtc/rtc-s3c.h
new file mode 100644
index 000000000000..004b61a8343f
--- /dev/null
+++ b/drivers/rtc/rtc-s3c.h
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2003 Simtec Electronics <linux@simtec.co.uk>
+ *		      http://www.simtec.co.uk/products/SWLINUX/
+ *
+ * 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.
+ *
+ * S3C2410 Internal RTC register definition
+*/
+
+#ifndef __ASM_ARCH_REGS_RTC_H
+#define __ASM_ARCH_REGS_RTC_H __FILE__
+
+#define S3C2410_RTCREG(x) (x)
+#define S3C2410_INTP		S3C2410_RTCREG(0x30)
+#define S3C2410_INTP_ALM	(1 << 1)
+#define S3C2410_INTP_TIC	(1 << 0)
+
+#define S3C2410_RTCCON		S3C2410_RTCREG(0x40)
+#define S3C2410_RTCCON_RTCEN	(1 << 0)
+#define S3C2410_RTCCON_CNTSEL	(1 << 2)
+#define S3C2410_RTCCON_CLKRST	(1 << 3)
+#define S3C2443_RTCCON_TICSEL	(1 << 4)
+#define S3C64XX_RTCCON_TICEN	(1 << 8)
+
+#define S3C2410_TICNT		S3C2410_RTCREG(0x44)
+#define S3C2410_TICNT_ENABLE	(1 << 7)
+
+/* S3C2443: tick count is 15 bit wide
+ * TICNT[6:0] contains upper 7 bits
+ * TICNT1[7:0] contains lower 8 bits
+ */
+#define S3C2443_TICNT_PART(x)	((x & 0x7f00) >> 8)
+#define S3C2443_TICNT1		S3C2410_RTCREG(0x4C)
+#define S3C2443_TICNT1_PART(x)	(x & 0xff)
+
+/* S3C2416: tick count is 32 bit wide
+ * TICNT[6:0] contains bits [14:8]
+ * TICNT1[7:0] contains lower 8 bits
+ * TICNT2[16:0] contains upper 17 bits
+ */
+#define S3C2416_TICNT2		S3C2410_RTCREG(0x48)
+#define S3C2416_TICNT2_PART(x)	((x & 0xffff8000) >> 15)
+
+#define S3C2410_RTCALM		S3C2410_RTCREG(0x50)
+#define S3C2410_RTCALM_ALMEN	(1 << 6)
+#define S3C2410_RTCALM_YEAREN	(1 << 5)
+#define S3C2410_RTCALM_MONEN	(1 << 4)
+#define S3C2410_RTCALM_DAYEN	(1 << 3)
+#define S3C2410_RTCALM_HOUREN	(1 << 2)
+#define S3C2410_RTCALM_MINEN	(1 << 1)
+#define S3C2410_RTCALM_SECEN	(1 << 0)
+
+#define S3C2410_ALMSEC		S3C2410_RTCREG(0x54)
+#define S3C2410_ALMMIN		S3C2410_RTCREG(0x58)
+#define S3C2410_ALMHOUR		S3C2410_RTCREG(0x5c)
+
+#define S3C2410_ALMDATE		S3C2410_RTCREG(0x60)
+#define S3C2410_ALMMON		S3C2410_RTCREG(0x64)
+#define S3C2410_ALMYEAR		S3C2410_RTCREG(0x68)
+
+#define S3C2410_RTCSEC		S3C2410_RTCREG(0x70)
+#define S3C2410_RTCMIN		S3C2410_RTCREG(0x74)
+#define S3C2410_RTCHOUR		S3C2410_RTCREG(0x78)
+#define S3C2410_RTCDATE		S3C2410_RTCREG(0x7c)
+#define S3C2410_RTCMON		S3C2410_RTCREG(0x84)
+#define S3C2410_RTCYEAR		S3C2410_RTCREG(0x88)
+
+#endif /* __ASM_ARCH_REGS_RTC_H */
diff --git a/drivers/rtc/rtc-stmp3xxx.c b/drivers/rtc/rtc-stmp3xxx.c
index a9cd26a26dc0..483ce086990b 100644
--- a/drivers/rtc/rtc-stmp3xxx.c
+++ b/drivers/rtc/rtc-stmp3xxx.c
@@ -30,8 +30,6 @@
 #include <linux/stmp_device.h>
 #include <linux/stmp3xxx_rtc_wdt.h>
 
-#include <mach/common.h>
-
 #define STMP3XXX_RTC_CTRL			0x0
 #define STMP3XXX_RTC_CTRL_SET			0x4
 #define STMP3XXX_RTC_CTRL_CLR			0x8
@@ -264,7 +262,7 @@ static int stmp3xxx_rtc_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, rtc_data);
 
-	mxs_reset_block(rtc_data->io);
+	stmp_reset_block(rtc_data->io);
 	writel(STMP3XXX_RTC_PERSISTENT0_ALARM_EN |
 			STMP3XXX_RTC_PERSISTENT0_ALARM_WAKE_EN |
 			STMP3XXX_RTC_PERSISTENT0_ALARM_WAKE,
@@ -307,7 +305,7 @@ static int stmp3xxx_rtc_resume(struct device *dev)
 {
 	struct stmp3xxx_rtc_data *rtc_data = dev_get_drvdata(dev);
 
-	mxs_reset_block(rtc_data->io);
+	stmp_reset_block(rtc_data->io);
 	writel(STMP3XXX_RTC_PERSISTENT0_ALARM_EN |
 			STMP3XXX_RTC_PERSISTENT0_ALARM_WAKE_EN |
 			STMP3XXX_RTC_PERSISTENT0_ALARM_WAKE,
diff --git a/drivers/rtc/rtc-tile.c b/drivers/rtc/rtc-tile.c
index 249b6531f119..fc3dee95f166 100644
--- a/drivers/rtc/rtc-tile.c
+++ b/drivers/rtc/rtc-tile.c
@@ -146,6 +146,7 @@ exit_driver_unregister:
  */
 static void __exit tile_rtc_driver_exit(void)
 {
+	platform_device_unregister(tile_rtc_platform_device);
 	platform_driver_unregister(&tile_rtc_platform_driver);
 }
 
diff --git a/drivers/s390/block/dasd.c b/drivers/s390/block/dasd.c
index 82758cbb220b..4361d9772c42 100644
--- a/drivers/s390/block/dasd.c
+++ b/drivers/s390/block/dasd.c
@@ -2997,18 +2997,14 @@ unlock:
 	return rc;
 }
 
-static int dasd_release(struct gendisk *disk, fmode_t mode)
+static void dasd_release(struct gendisk *disk, fmode_t mode)
 {
-	struct dasd_device *base;
-
-	base = dasd_device_from_gendisk(disk);
-	if (!base)
-		return -ENODEV;
-
-	atomic_dec(&base->block->open_count);
-	module_put(base->discipline->owner);
-	dasd_put_device(base);
-	return 0;
+	struct dasd_device *base = dasd_device_from_gendisk(disk);
+	if (base) {
+		atomic_dec(&base->block->open_count);
+		module_put(base->discipline->owner);
+		dasd_put_device(base);
+	}
 }
 
 /*
diff --git a/drivers/s390/block/dcssblk.c b/drivers/s390/block/dcssblk.c
index b6ad0de07930..6eca019bcf30 100644
--- a/drivers/s390/block/dcssblk.c
+++ b/drivers/s390/block/dcssblk.c
@@ -26,7 +26,7 @@
 #define DCSS_BUS_ID_SIZE 20
 
 static int dcssblk_open(struct block_device *bdev, fmode_t mode);
-static int dcssblk_release(struct gendisk *disk, fmode_t mode);
+static void dcssblk_release(struct gendisk *disk, fmode_t mode);
 static void dcssblk_make_request(struct request_queue *q, struct bio *bio);
 static int dcssblk_direct_access(struct block_device *bdev, sector_t secnum,
 				 void **kaddr, unsigned long *pfn);
@@ -781,16 +781,15 @@ out:
 	return rc;
 }
 
-static int
+static void
 dcssblk_release(struct gendisk *disk, fmode_t mode)
 {
 	struct dcssblk_dev_info *dev_info = disk->private_data;
 	struct segment_info *entry;
-	int rc;
 
 	if (!dev_info) {
-		rc = -ENODEV;
-		goto out;
+		WARN_ON(1);
+		return;
 	}
 	down_write(&dcssblk_devices_sem);
 	if (atomic_dec_and_test(&dev_info->use_count)
@@ -803,9 +802,6 @@ dcssblk_release(struct gendisk *disk, fmode_t mode)
 		dev_info->save_pending = 0;
 	}
 	up_write(&dcssblk_devices_sem);
-	rc = 0;
-out:
-	return rc;
 }
 
 static void
@@ -826,8 +822,7 @@ dcssblk_make_request(struct request_queue *q, struct bio *bio)
 	if ((bio->bi_sector & 7) != 0 || (bio->bi_size & 4095) != 0)
 		/* Request is not page-aligned. */
 		goto fail;
-	if (((bio->bi_size >> 9) + bio->bi_sector)
-			> get_capacity(bio->bi_bdev->bd_disk)) {
+	if (bio_end_sector(bio) > get_capacity(bio->bi_bdev->bd_disk)) {
 		/* Request beyond end of DCSS segment. */
 		goto fail;
 	}
diff --git a/drivers/s390/block/scm_blk.c b/drivers/s390/block/scm_blk.c
index b303cab76a7f..5d73e6e49af6 100644
--- a/drivers/s390/block/scm_blk.c
+++ b/drivers/s390/block/scm_blk.c
@@ -123,10 +123,9 @@ static int scm_open(struct block_device *blkdev, fmode_t mode)
 	return scm_get_ref();
 }
 
-static int scm_release(struct gendisk *gendisk, fmode_t mode)
+static void scm_release(struct gendisk *gendisk, fmode_t mode)
 {
 	scm_put_ref();
-	return 0;
 }
 
 static const struct block_device_operations scm_blk_devops = {
diff --git a/drivers/s390/char/sclp_cmd.c b/drivers/s390/char/sclp_cmd.c
index 178836ec252b..bf07c3a188d4 100644
--- a/drivers/s390/char/sclp_cmd.c
+++ b/drivers/s390/char/sclp_cmd.c
@@ -345,7 +345,6 @@ struct memory_increment {
 	struct list_head list;
 	u16 rn;
 	int standby;
-	int usecount;
 };
 
 struct assign_storage_sccb {
@@ -463,21 +462,10 @@ static int sclp_mem_change_state(unsigned long start, unsigned long size,
 			break;
 		if (start > istart + rzm - 1)
 			continue;
-		if (online) {
-			if (incr->usecount++)
-				continue;
-			/*
-			 * Don't break the loop if one assign fails. Loop may
-			 * be walked again on CANCEL and we can't save
-			 * information if state changed before or not.
-			 * So continue and increase usecount for all increments.
-			 */
+		if (online)
 			rc |= sclp_assign_storage(incr->rn);
-		} else {
-			if (--incr->usecount)
-				continue;
+		else
 			sclp_unassign_storage(incr->rn);
-		}
 	}
 	return rc ? -EIO : 0;
 }
@@ -561,8 +549,6 @@ static void __init sclp_add_standby_memory(void)
 	add_memory_merged(0);
 }
 
-#define MEM_SCT_SIZE (1UL << SECTION_SIZE_BITS)
-
 static void __init insert_increment(u16 rn, int standby, int assigned)
 {
 	struct memory_increment *incr, *new_incr;
@@ -574,8 +560,6 @@ static void __init insert_increment(u16 rn, int standby, int assigned)
 		return;
 	new_incr->rn = rn;
 	new_incr->standby = standby;
-	if (!standby)
-		new_incr->usecount = rzm > MEM_SCT_SIZE ? rzm/MEM_SCT_SIZE : 1;
 	last_rn = 0;
 	prev = &sclp_mem_list;
 	list_for_each_entry(incr, &sclp_mem_list, list) {
diff --git a/drivers/s390/char/zcore.c b/drivers/s390/char/zcore.c
index 22820610022c..9e5e14686e75 100644
--- a/drivers/s390/char/zcore.c
+++ b/drivers/s390/char/zcore.c
@@ -426,7 +426,7 @@ static int zcore_memmap_open(struct inode *inode, struct file *filp)
 			      GFP_KERNEL);
 	if (!chunk_array)
 		return -ENOMEM;
-	detect_memory_layout(chunk_array);
+	detect_memory_layout(chunk_array, 0);
 	buf = kzalloc(MEMORY_CHUNKS * CHUNK_INFO_SIZE, GFP_KERNEL);
 	if (!buf) {
 		kfree(chunk_array);
@@ -557,7 +557,7 @@ static void __init set_lc_mask(struct save_area *map)
 /*
  * Initialize dump globals for a given architecture
  */
-static int __init sys_info_init(enum arch_id arch)
+static int __init sys_info_init(enum arch_id arch, unsigned long mem_end)
 {
 	int rc;
 
@@ -579,7 +579,7 @@ static int __init sys_info_init(enum arch_id arch)
 	rc = init_cpu_info(arch);
 	if (rc)
 		return rc;
-	sys_info.mem_size = real_memory_size;
+	sys_info.mem_size = mem_end;
 
 	return 0;
 }
@@ -601,7 +601,7 @@ static int __init check_sdias(void)
 	return 0;
 }
 
-static int __init get_mem_size(unsigned long *mem)
+static int __init get_mem_info(unsigned long *mem, unsigned long *end)
 {
 	int i;
 	struct mem_chunk *chunk_array;
@@ -610,33 +610,31 @@ static int __init get_mem_size(unsigned long *mem)
 			      GFP_KERNEL);
 	if (!chunk_array)
 		return -ENOMEM;
-	detect_memory_layout(chunk_array);
+	detect_memory_layout(chunk_array, 0);
 	for (i = 0; i < MEMORY_CHUNKS; i++) {
 		if (chunk_array[i].size == 0)
 			break;
 		*mem += chunk_array[i].size;
+		*end = max(*end, chunk_array[i].addr + chunk_array[i].size);
 	}
 	kfree(chunk_array);
 	return 0;
 }
 
-static int __init zcore_header_init(int arch, struct zcore_header *hdr)
+static void __init zcore_header_init(int arch, struct zcore_header *hdr,
+				     unsigned long mem_size)
 {
-	int rc, i;
-	unsigned long memory = 0;
 	u32 prefix;
+	int i;
 
 	if (arch == ARCH_S390X)
 		hdr->arch_id = DUMP_ARCH_S390X;
 	else
 		hdr->arch_id = DUMP_ARCH_S390;
-	rc = get_mem_size(&memory);
-	if (rc)
-		return rc;
-	hdr->mem_size = memory;
-	hdr->rmem_size = memory;
+	hdr->mem_size = mem_size;
+	hdr->rmem_size = mem_size;
 	hdr->mem_end = sys_info.mem_size;
-	hdr->num_pages = memory / PAGE_SIZE;
+	hdr->num_pages = mem_size / PAGE_SIZE;
 	hdr->tod = get_tod_clock();
 	get_cpu_id(&hdr->cpu_id);
 	for (i = 0; zfcpdump_save_areas[i]; i++) {
@@ -647,7 +645,6 @@ static int __init zcore_header_init(int arch, struct zcore_header *hdr)
 		hdr->lc_vec[hdr->cpu_cnt] = prefix;
 		hdr->cpu_cnt++;
 	}
-	return 0;
 }
 
 /*
@@ -682,9 +679,11 @@ static int __init zcore_reipl_init(void)
 
 static int __init zcore_init(void)
 {
+	unsigned long mem_size, mem_end;
 	unsigned char arch;
 	int rc;
 
+	mem_size = mem_end = 0;
 	if (ipl_info.type != IPL_TYPE_FCP_DUMP)
 		return -ENODATA;
 	if (OLDMEM_BASE)
@@ -727,13 +726,14 @@ static int __init zcore_init(void)
 	}
 #endif /* CONFIG_64BIT */
 
-	rc = sys_info_init(arch);
+	rc = get_mem_info(&mem_size, &mem_end);
 	if (rc)
 		goto fail;
 
-	rc = zcore_header_init(arch, &zcore_header);
+	rc = sys_info_init(arch, mem_end);
 	if (rc)
 		goto fail;
+	zcore_header_init(arch, &zcore_header, mem_size);
 
 	rc = zcore_reipl_init();
 	if (rc)
diff --git a/drivers/s390/cio/blacklist.c b/drivers/s390/cio/blacklist.c
index 2d2a966a3b39..a9fe3de2dec1 100644
--- a/drivers/s390/cio/blacklist.c
+++ b/drivers/s390/cio/blacklist.c
@@ -1,7 +1,7 @@
 /*
  *   S/390 common I/O routines -- blacklisting of specific devices
  *
- *    Copyright IBM Corp. 1999, 2002
+ *    Copyright IBM Corp. 1999, 2013
  *    Author(s): Ingo Adlung (adlung@de.ibm.com)
  *		 Cornelia Huck (cornelia.huck@de.ibm.com)
  *		 Arnd Bergmann (arndb@de.ibm.com)
@@ -17,8 +17,9 @@
 #include <linux/ctype.h>
 #include <linux/device.h>
 
-#include <asm/cio.h>
 #include <asm/uaccess.h>
+#include <asm/cio.h>
+#include <asm/ipl.h>
 
 #include "blacklist.h"
 #include "cio.h"
@@ -172,6 +173,29 @@ static int blacklist_parse_parameters(char *str, range_action action,
 			to_cssid = __MAX_CSSID;
 			to_ssid = __MAX_SSID;
 			to = __MAX_SUBCHANNEL;
+		} else if (strcmp(parm, "ipldev") == 0) {
+			if (ipl_info.type == IPL_TYPE_CCW) {
+				from_cssid = 0;
+				from_ssid = ipl_info.data.ccw.dev_id.ssid;
+				from = ipl_info.data.ccw.dev_id.devno;
+			} else if (ipl_info.type == IPL_TYPE_FCP ||
+				   ipl_info.type == IPL_TYPE_FCP_DUMP) {
+				from_cssid = 0;
+				from_ssid = ipl_info.data.fcp.dev_id.ssid;
+				from = ipl_info.data.fcp.dev_id.devno;
+			} else {
+				continue;
+			}
+			to_cssid = from_cssid;
+			to_ssid = from_ssid;
+			to = from;
+		} else if (strcmp(parm, "condev") == 0) {
+			if (console_devno == -1)
+				continue;
+
+			from_cssid = to_cssid = 0;
+			from_ssid = to_ssid = 0;
+			from = to = console_devno;
 		} else {
 			rc = parse_busid(strsep(&parm, "-"), &from_cssid,
 					 &from_ssid, &from, msgtrigger);
diff --git a/drivers/s390/cio/qdio_debug.c b/drivers/s390/cio/qdio_debug.c
index ccaae9d63d27..4221b02085ad 100644
--- a/drivers/s390/cio/qdio_debug.c
+++ b/drivers/s390/cio/qdio_debug.c
@@ -224,7 +224,7 @@ static int qperf_seq_open(struct inode *inode, struct file *filp)
 			   file_inode(filp)->i_private);
 }
 
-static struct file_operations debugfs_perf_fops = {
+static const struct file_operations debugfs_perf_fops = {
 	.owner	 = THIS_MODULE,
 	.open	 = qperf_seq_open,
 	.read	 = seq_read,
diff --git a/drivers/s390/crypto/ap_bus.c b/drivers/s390/crypto/ap_bus.c
index b8b340ac5332..9de41aa14896 100644
--- a/drivers/s390/crypto/ap_bus.c
+++ b/drivers/s390/crypto/ap_bus.c
@@ -954,15 +954,11 @@ EXPORT_SYMBOL(ap_driver_unregister);
 
 void ap_bus_force_rescan(void)
 {
-	/* Delete the AP bus rescan timer. */
-	del_timer(&ap_config_timer);
-
-	/* processing a synchonuous bus rescan */
-	ap_scan_bus(NULL);
-
-	/* Setup the AP bus rescan timer again. */
-	ap_config_timer.expires = jiffies + ap_config_time * HZ;
-	add_timer(&ap_config_timer);
+	/* reconfigure the AP bus rescan timer. */
+	mod_timer(&ap_config_timer, jiffies + ap_config_time * HZ);
+	/* processing a asynchronous bus rescan */
+	queue_work(ap_work_queue, &ap_config_work);
+	flush_work(&ap_config_work);
 }
 EXPORT_SYMBOL(ap_bus_force_rescan);
 
@@ -1305,8 +1301,9 @@ static void ap_scan_bus(struct work_struct *unused)
 	int rc, i;
 
 	ap_query_configuration();
-	if (ap_select_domain() != 0)
+	if (ap_select_domain() != 0) {
 		return;
+	}
 	for (i = 0; i < AP_DEVICES; i++) {
 		qid = AP_MKQID(i, ap_domain_index);
 		dev = bus_find_device(&ap_bus_type, NULL,
diff --git a/drivers/s390/kvm/kvm_virtio.c b/drivers/s390/kvm/kvm_virtio.c
index 6711e65764b5..2ea6165366b6 100644
--- a/drivers/s390/kvm/kvm_virtio.c
+++ b/drivers/s390/kvm/kvm_virtio.c
@@ -443,29 +443,30 @@ static int __init test_devices_support(unsigned long addr)
 }
 /*
  * Init function for virtio
- * devices are in a single page above top of "normal" mem
+ * devices are in a single page above top of "normal" + standby mem
  */
 static int __init kvm_devices_init(void)
 {
 	int rc;
+	unsigned long total_memory_size = sclp_get_rzm() * sclp_get_rnmax();
 
 	if (!MACHINE_IS_KVM)
 		return -ENODEV;
 
-	if (test_devices_support(real_memory_size) < 0)
+	if (test_devices_support(total_memory_size) < 0)
 		return -ENODEV;
 
-	rc = vmem_add_mapping(real_memory_size, PAGE_SIZE);
+	rc = vmem_add_mapping(total_memory_size, PAGE_SIZE);
 	if (rc)
 		return rc;
 
-	kvm_devices = (void *) real_memory_size;
+	kvm_devices = (void *) total_memory_size;
 
 	kvm_root = root_device_register("kvm_s390");
 	if (IS_ERR(kvm_root)) {
 		rc = PTR_ERR(kvm_root);
 		printk(KERN_ERR "Could not register kvm_s390 root device");
-		vmem_remove_mapping(real_memory_size, PAGE_SIZE);
+		vmem_remove_mapping(total_memory_size, PAGE_SIZE);
 		return rc;
 	}
 
diff --git a/drivers/s390/kvm/virtio_ccw.c b/drivers/s390/kvm/virtio_ccw.c
index 2029b6caa595..779dc5136291 100644
--- a/drivers/s390/kvm/virtio_ccw.c
+++ b/drivers/s390/kvm/virtio_ccw.c
@@ -31,6 +31,7 @@
 #include <asm/irq.h>
 #include <asm/cio.h>
 #include <asm/ccwdev.h>
+#include <asm/virtio-ccw.h>
 
 /*
  * virtio related functions
@@ -77,12 +78,9 @@ struct virtio_ccw_vq_info {
 	void *queue;
 	struct vq_info_block *info_block;
 	struct list_head node;
+	long cookie;
 };
 
-#define KVM_VIRTIO_CCW_RING_ALIGN 4096
-
-#define KVM_S390_VIRTIO_CCW_NOTIFY 3
-
 #define CCW_CMD_SET_VQ 0x13
 #define CCW_CMD_VDEV_RESET 0x33
 #define CCW_CMD_SET_IND 0x43
@@ -135,8 +133,11 @@ static int ccw_io_helper(struct virtio_ccw_device *vcdev,
 	do {
 		spin_lock_irqsave(get_ccwdev_lock(vcdev->cdev), flags);
 		ret = ccw_device_start(vcdev->cdev, ccw, intparm, 0, 0);
-		if (!ret)
+		if (!ret) {
+			if (!vcdev->curr_io)
+				vcdev->err = 0;
 			vcdev->curr_io |= flag;
+		}
 		spin_unlock_irqrestore(get_ccwdev_lock(vcdev->cdev), flags);
 		cpu_relax();
 	} while (ret == -EBUSY);
@@ -145,15 +146,18 @@ static int ccw_io_helper(struct virtio_ccw_device *vcdev,
 }
 
 static inline long do_kvm_notify(struct subchannel_id schid,
-				 unsigned long queue_index)
+				 unsigned long queue_index,
+				 long cookie)
 {
 	register unsigned long __nr asm("1") = KVM_S390_VIRTIO_CCW_NOTIFY;
 	register struct subchannel_id __schid asm("2") = schid;
 	register unsigned long __index asm("3") = queue_index;
 	register long __rc asm("2");
+	register long __cookie asm("4") = cookie;
 
 	asm volatile ("diag 2,4,0x500\n"
-		      : "=d" (__rc) : "d" (__nr), "d" (__schid), "d" (__index)
+		      : "=d" (__rc) : "d" (__nr), "d" (__schid), "d" (__index),
+		      "d"(__cookie)
 		      : "memory", "cc");
 	return __rc;
 }
@@ -166,7 +170,7 @@ static void virtio_ccw_kvm_notify(struct virtqueue *vq)
 
 	vcdev = to_vc_device(info->vq->vdev);
 	ccw_device_get_schid(vcdev->cdev, &schid);
-	do_kvm_notify(schid, virtqueue_get_queue_index(vq));
+	info->cookie = do_kvm_notify(schid, vq->index, info->cookie);
 }
 
 static int virtio_ccw_read_vq_conf(struct virtio_ccw_device *vcdev,
@@ -188,7 +192,7 @@ static void virtio_ccw_del_vq(struct virtqueue *vq, struct ccw1 *ccw)
 	unsigned long flags;
 	unsigned long size;
 	int ret;
-	unsigned int index = virtqueue_get_queue_index(vq);
+	unsigned int index = vq->index;
 
 	/* Remove from our list. */
 	spin_lock_irqsave(&vcdev->lock, flags);
@@ -610,7 +614,7 @@ static struct virtqueue *virtio_ccw_vq_by_ind(struct virtio_ccw_device *vcdev,
 	vq = NULL;
 	spin_lock_irqsave(&vcdev->lock, flags);
 	list_for_each_entry(info, &vcdev->virtqueues, node) {
-		if (virtqueue_get_queue_index(info->vq) == index) {
+		if (info->vq->index == index) {
 			vq = info->vq;
 			break;
 		}
diff --git a/drivers/s390/net/qeth_core.h b/drivers/s390/net/qeth_core.h
index 6ccb7457746b..c4f392d5db4c 100644
--- a/drivers/s390/net/qeth_core.h
+++ b/drivers/s390/net/qeth_core.h
@@ -918,7 +918,7 @@ int qeth_send_control_data(struct qeth_card *, int, struct qeth_cmd_buffer *,
 	int (*reply_cb)(struct qeth_card *, struct qeth_reply*, unsigned long),
 	void *reply_param);
 int qeth_get_priority_queue(struct qeth_card *, struct sk_buff *, int, int);
-int qeth_get_elements_no(struct qeth_card *, void *, struct sk_buff *, int);
+int qeth_get_elements_no(struct qeth_card *, struct sk_buff *, int);
 int qeth_get_elements_for_frags(struct sk_buff *);
 int qeth_do_send_packet_fast(struct qeth_card *, struct qeth_qdio_out_q *,
 			struct sk_buff *, struct qeth_hdr *, int, int, int);
@@ -932,7 +932,7 @@ void qeth_core_get_drvinfo(struct net_device *, struct ethtool_drvinfo *);
 void qeth_dbf_longtext(debug_info_t *id, int level, char *text, ...);
 int qeth_core_ethtool_get_settings(struct net_device *, struct ethtool_cmd *);
 int qeth_set_access_ctrl_online(struct qeth_card *card, int fallback);
-int qeth_hdr_chk_and_bounce(struct sk_buff *, int);
+int qeth_hdr_chk_and_bounce(struct sk_buff *, struct qeth_hdr **, int);
 int qeth_configure_cq(struct qeth_card *, enum qeth_cq);
 int qeth_hw_trap(struct qeth_card *, enum qeth_diags_trap_action);
 int qeth_query_ipassists(struct qeth_card *, enum qeth_prot_versions prot);
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index 451f92020599..6cd0fc1b203a 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -335,7 +335,7 @@ static inline int qeth_alloc_cq(struct qeth_card *card)
 
 		card->qdio.no_in_queues = 2;
 
-		card->qdio.out_bufstates = (struct qdio_outbuf_state *)
+		card->qdio.out_bufstates =
 			kzalloc(card->qdio.no_out_queues *
 				QDIO_MAX_BUFFERS_PER_Q *
 				sizeof(struct qdio_outbuf_state), GFP_KERNEL);
@@ -3717,7 +3717,7 @@ int qeth_get_elements_for_frags(struct sk_buff *skb)
 }
 EXPORT_SYMBOL_GPL(qeth_get_elements_for_frags);
 
-int qeth_get_elements_no(struct qeth_card *card, void *hdr,
+int qeth_get_elements_no(struct qeth_card *card,
 		     struct sk_buff *skb, int elems)
 {
 	int dlen = skb->len - skb->data_len;
@@ -3736,7 +3736,7 @@ int qeth_get_elements_no(struct qeth_card *card, void *hdr,
 }
 EXPORT_SYMBOL_GPL(qeth_get_elements_no);
 
-int qeth_hdr_chk_and_bounce(struct sk_buff *skb, int len)
+int qeth_hdr_chk_and_bounce(struct sk_buff *skb, struct qeth_hdr **hdr, int len)
 {
 	int hroom, inpage, rest;
 
@@ -3749,6 +3749,8 @@ int qeth_hdr_chk_and_bounce(struct sk_buff *skb, int len)
 			return 1;
 		memmove(skb->data - rest, skb->data, skb->len - skb->data_len);
 		skb->data -= rest;
+		skb->tail -= rest;
+		*hdr = (struct qeth_hdr *)skb->data;
 		QETH_DBF_MESSAGE(2, "skb bounce len: %d rest: %d\n", len, rest);
 	}
 	return 0;
diff --git a/drivers/s390/net/qeth_l2_main.c b/drivers/s390/net/qeth_l2_main.c
index 155b101bd730..ec8ccdae7aba 100644
--- a/drivers/s390/net/qeth_l2_main.c
+++ b/drivers/s390/net/qeth_l2_main.c
@@ -302,7 +302,8 @@ static void qeth_l2_process_vlans(struct qeth_card *card)
 	spin_unlock_bh(&card->vlanlock);
 }
 
-static int qeth_l2_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
+static int qeth_l2_vlan_rx_add_vid(struct net_device *dev,
+				   __be16 proto, u16 vid)
 {
 	struct qeth_card *card = dev->ml_priv;
 	struct qeth_vlan_vid *id;
@@ -331,7 +332,8 @@ static int qeth_l2_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
 	return 0;
 }
 
-static int qeth_l2_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
+static int qeth_l2_vlan_rx_kill_vid(struct net_device *dev,
+				    __be16 proto, u16 vid)
 {
 	struct qeth_vlan_vid *id, *tmpid = NULL;
 	struct qeth_card *card = dev->ml_priv;
@@ -771,8 +773,7 @@ static int qeth_l2_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		}
 	}
 
-	elements = qeth_get_elements_no(card, (void *)hdr, new_skb,
-						elements_needed);
+	elements = qeth_get_elements_no(card, new_skb, elements_needed);
 	if (!elements) {
 		if (data_offset >= 0)
 			kmem_cache_free(qeth_core_header_cache, hdr);
@@ -780,7 +781,7 @@ static int qeth_l2_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	}
 
 	if (card->info.type != QETH_CARD_TYPE_IQD) {
-		if (qeth_hdr_chk_and_bounce(new_skb,
+		if (qeth_hdr_chk_and_bounce(new_skb, &hdr,
 		    sizeof(struct qeth_hdr_layer2)))
 			goto tx_drop;
 		rc = qeth_do_send_packet(card, queue, new_skb, hdr,
@@ -959,7 +960,7 @@ static int qeth_l2_setup_netdev(struct qeth_card *card)
 		SET_ETHTOOL_OPS(card->dev, &qeth_l2_ethtool_ops);
 	else
 		SET_ETHTOOL_OPS(card->dev, &qeth_l2_osn_ops);
-	card->dev->features |= NETIF_F_HW_VLAN_FILTER;
+	card->dev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
 	card->info.broadcast_capable = 1;
 	qeth_l2_request_initial_mac(card);
 	SET_NETDEV_DEV(card->dev, &card->gdev->dev);
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index 1f7edf1b26c3..c1b0b2761f8d 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -1659,7 +1659,8 @@ static void qeth_l3_add_vlan_mc(struct qeth_card *card)
 	for_each_set_bit(vid, card->active_vlans, VLAN_N_VID) {
 		struct net_device *netdev;
 
-		netdev = __vlan_find_dev_deep(card->dev, vid);
+		netdev = __vlan_find_dev_deep(card->dev, htons(ETH_P_8021Q),
+					      vid);
 		if (netdev == NULL ||
 		    !(netdev->flags & IFF_UP))
 			continue;
@@ -1720,7 +1721,8 @@ static void qeth_l3_add_vlan_mc6(struct qeth_card *card)
 	for_each_set_bit(vid, card->active_vlans, VLAN_N_VID) {
 		struct net_device *netdev;
 
-		netdev = __vlan_find_dev_deep(card->dev, vid);
+		netdev = __vlan_find_dev_deep(card->dev, htons(ETH_P_8021Q),
+					      vid);
 		if (netdev == NULL ||
 		    !(netdev->flags & IFF_UP))
 			continue;
@@ -1764,7 +1766,7 @@ static void qeth_l3_free_vlan_addresses4(struct qeth_card *card,
 
 	QETH_CARD_TEXT(card, 4, "frvaddr4");
 
-	netdev = __vlan_find_dev_deep(card->dev, vid);
+	netdev = __vlan_find_dev_deep(card->dev, htons(ETH_P_8021Q), vid);
 	if (!netdev)
 		return;
 	in_dev = in_dev_get(netdev);
@@ -1794,7 +1796,7 @@ static void qeth_l3_free_vlan_addresses6(struct qeth_card *card,
 
 	QETH_CARD_TEXT(card, 4, "frvaddr6");
 
-	netdev = __vlan_find_dev_deep(card->dev, vid);
+	netdev = __vlan_find_dev_deep(card->dev, htons(ETH_P_8021Q), vid);
 	if (!netdev)
 		return;
 	in6_dev = in6_dev_get(netdev);
@@ -1824,7 +1826,8 @@ static void qeth_l3_free_vlan_addresses(struct qeth_card *card,
 	rcu_read_unlock();
 }
 
-static int qeth_l3_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
+static int qeth_l3_vlan_rx_add_vid(struct net_device *dev,
+				   __be16 proto, u16 vid)
 {
 	struct qeth_card *card = dev->ml_priv;
 
@@ -1832,7 +1835,8 @@ static int qeth_l3_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
 	return 0;
 }
 
-static int qeth_l3_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
+static int qeth_l3_vlan_rx_kill_vid(struct net_device *dev,
+				    __be16 proto, u16 vid)
 {
 	struct qeth_card *card = dev->ml_priv;
 	unsigned long flags;
@@ -1975,7 +1979,8 @@ static int qeth_l3_process_inbound_buffer(struct qeth_card *card,
 						      &vlan_tag);
 				len = skb->len;
 				if (is_vlan && !card->options.sniffer)
-					__vlan_hwaccel_put_tag(skb, vlan_tag);
+					__vlan_hwaccel_put_tag(skb,
+						htons(ETH_P_8021Q), vlan_tag);
 				napi_gro_receive(&card->napi, skb);
 			}
 			break;
@@ -2084,7 +2089,8 @@ static int qeth_l3_verify_vlan_dev(struct net_device *dev,
 		struct net_device *netdev;
 
 		rcu_read_lock();
-		netdev = __vlan_find_dev_deep(card->dev, vid);
+		netdev = __vlan_find_dev_deep(card->dev, htons(ETH_P_8021Q),
+					      vid);
 		rcu_read_unlock();
 		if (netdev == dev) {
 			rc = QETH_VLAN_CARD;
@@ -3031,8 +3037,7 @@ static int qeth_l3_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 			qeth_l3_hdr_csum(card, hdr, new_skb);
 	}
 
-	elems = qeth_get_elements_no(card, (void *)hdr, new_skb,
-						 elements_needed);
+	elems = qeth_get_elements_no(card, new_skb, elements_needed);
 	if (!elems) {
 		if (data_offset >= 0)
 			kmem_cache_free(qeth_core_header_cache, hdr);
@@ -3050,7 +3055,7 @@ static int qeth_l3_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		else
 			len = sizeof(struct qeth_hdr_layer3);
 
-		if (qeth_hdr_chk_and_bounce(new_skb, len))
+		if (qeth_hdr_chk_and_bounce(new_skb, &hdr, len))
 			goto tx_drop;
 		rc = qeth_do_send_packet(card, queue, new_skb, hdr,
 					 elements_needed);
@@ -3294,9 +3299,9 @@ static int qeth_l3_setup_netdev(struct qeth_card *card)
 	card->dev->watchdog_timeo = QETH_TX_TIMEOUT;
 	card->dev->mtu = card->info.initial_mtu;
 	SET_ETHTOOL_OPS(card->dev, &qeth_l3_ethtool_ops);
-	card->dev->features |=	NETIF_F_HW_VLAN_TX |
-				NETIF_F_HW_VLAN_RX |
-				NETIF_F_HW_VLAN_FILTER;
+	card->dev->features |=	NETIF_F_HW_VLAN_CTAG_TX |
+				NETIF_F_HW_VLAN_CTAG_RX |
+				NETIF_F_HW_VLAN_CTAG_FILTER;
 	card->dev->priv_flags &= ~IFF_XMIT_DST_RELEASE;
 	card->dev->gso_max_size = 15 * PAGE_SIZE;
 
diff --git a/drivers/scsi/BusLogic.c b/drivers/scsi/BusLogic.c
index d7ca247efa35..344d87599cd2 100644
--- a/drivers/scsi/BusLogic.c
+++ b/drivers/scsi/BusLogic.c
@@ -3201,26 +3201,30 @@ static int BusLogic_BIOSDiskParameters(struct scsi_device *sdev, struct block_de
   BugLogic_ProcDirectoryInfo implements /proc/scsi/BusLogic/<N>.
 */
 
-static int BusLogic_ProcDirectoryInfo(struct Scsi_Host *shost, char *ProcBuffer, char **StartPointer, off_t Offset, int BytesAvailable, int WriteFlag)
+static int BusLogic_write_info(struct Scsi_Host *shost, char *ProcBuffer, int BytesAvailable)
 {
 	struct BusLogic_HostAdapter *HostAdapter = (struct BusLogic_HostAdapter *) shost->hostdata;
 	struct BusLogic_TargetStatistics *TargetStatistics;
-	int TargetID, Length;
-	char *Buffer;
 
 	TargetStatistics = HostAdapter->TargetStatistics;
-	if (WriteFlag) {
-		HostAdapter->ExternalHostAdapterResets = 0;
-		HostAdapter->HostAdapterInternalErrors = 0;
-		memset(TargetStatistics, 0, BusLogic_MaxTargetDevices * sizeof(struct BusLogic_TargetStatistics));
-		return 0;
-	}
-	Buffer = HostAdapter->MessageBuffer;
-	Length = HostAdapter->MessageBufferLength;
-	Length += sprintf(&Buffer[Length], "\n\
+	HostAdapter->ExternalHostAdapterResets = 0;
+	HostAdapter->HostAdapterInternalErrors = 0;
+	memset(TargetStatistics, 0, BusLogic_MaxTargetDevices * sizeof(struct BusLogic_TargetStatistics));
+	return 0;
+}
+
+static int BusLogic_show_info(struct seq_file *m, struct Scsi_Host *shost)
+{
+	struct BusLogic_HostAdapter *HostAdapter = (struct BusLogic_HostAdapter *) shost->hostdata;
+	struct BusLogic_TargetStatistics *TargetStatistics;
+	int TargetID;
+
+	TargetStatistics = HostAdapter->TargetStatistics;
+	seq_write(m, HostAdapter->MessageBuffer, HostAdapter->MessageBufferLength);
+	seq_printf(m, "\n\
 Current Driver Queue Depth:	%d\n\
 Currently Allocated CCBs:	%d\n", HostAdapter->DriverQueueDepth, HostAdapter->AllocatedCCBs);
-	Length += sprintf(&Buffer[Length], "\n\n\
+	seq_printf(m, "\n\n\
 			   DATA TRANSFER STATISTICS\n\
 \n\
 Target	Tagged Queuing	Queue Depth  Active  Attempted	Completed\n\
@@ -3229,66 +3233,62 @@ Target	Tagged Queuing	Queue Depth  Active  Attempted	Completed\n\
 		struct BusLogic_TargetFlags *TargetFlags = &HostAdapter->TargetFlags[TargetID];
 		if (!TargetFlags->TargetExists)
 			continue;
-		Length += sprintf(&Buffer[Length], "  %2d	%s", TargetID, (TargetFlags->TaggedQueuingSupported ? (TargetFlags->TaggedQueuingActive ? "    Active" : (HostAdapter->TaggedQueuingPermitted & (1 << TargetID)
+		seq_printf(m, "  %2d	%s", TargetID, (TargetFlags->TaggedQueuingSupported ? (TargetFlags->TaggedQueuingActive ? "    Active" : (HostAdapter->TaggedQueuingPermitted & (1 << TargetID)
 																				    ? "  Permitted" : "   Disabled"))
 									  : "Not Supported"));
-		Length += sprintf(&Buffer[Length],
+		seq_printf(m,
 				  "	    %3d       %3u    %9u	%9u\n", HostAdapter->QueueDepth[TargetID], HostAdapter->ActiveCommands[TargetID], TargetStatistics[TargetID].CommandsAttempted, TargetStatistics[TargetID].CommandsCompleted);
 	}
-	Length += sprintf(&Buffer[Length], "\n\
+	seq_printf(m, "\n\
 Target  Read Commands  Write Commands   Total Bytes Read    Total Bytes Written\n\
 ======  =============  ==============  ===================  ===================\n");
 	for (TargetID = 0; TargetID < HostAdapter->MaxTargetDevices; TargetID++) {
 		struct BusLogic_TargetFlags *TargetFlags = &HostAdapter->TargetFlags[TargetID];
 		if (!TargetFlags->TargetExists)
 			continue;
-		Length += sprintf(&Buffer[Length], "  %2d	  %9u	 %9u", TargetID, TargetStatistics[TargetID].ReadCommands, TargetStatistics[TargetID].WriteCommands);
+		seq_printf(m, "  %2d	  %9u	 %9u", TargetID, TargetStatistics[TargetID].ReadCommands, TargetStatistics[TargetID].WriteCommands);
 		if (TargetStatistics[TargetID].TotalBytesRead.Billions > 0)
-			Length += sprintf(&Buffer[Length], "     %9u%09u", TargetStatistics[TargetID].TotalBytesRead.Billions, TargetStatistics[TargetID].TotalBytesRead.Units);
+			seq_printf(m, "     %9u%09u", TargetStatistics[TargetID].TotalBytesRead.Billions, TargetStatistics[TargetID].TotalBytesRead.Units);
 		else
-			Length += sprintf(&Buffer[Length], "		%9u", TargetStatistics[TargetID].TotalBytesRead.Units);
+			seq_printf(m, "		%9u", TargetStatistics[TargetID].TotalBytesRead.Units);
 		if (TargetStatistics[TargetID].TotalBytesWritten.Billions > 0)
-			Length += sprintf(&Buffer[Length], "   %9u%09u\n", TargetStatistics[TargetID].TotalBytesWritten.Billions, TargetStatistics[TargetID].TotalBytesWritten.Units);
+			seq_printf(m, "   %9u%09u\n", TargetStatistics[TargetID].TotalBytesWritten.Billions, TargetStatistics[TargetID].TotalBytesWritten.Units);
 		else
-			Length += sprintf(&Buffer[Length], "	     %9u\n", TargetStatistics[TargetID].TotalBytesWritten.Units);
+			seq_printf(m, "	     %9u\n", TargetStatistics[TargetID].TotalBytesWritten.Units);
 	}
-	Length += sprintf(&Buffer[Length], "\n\
+	seq_printf(m, "\n\
 Target  Command    0-1KB      1-2KB      2-4KB      4-8KB     8-16KB\n\
 ======  =======  =========  =========  =========  =========  =========\n");
 	for (TargetID = 0; TargetID < HostAdapter->MaxTargetDevices; TargetID++) {
 		struct BusLogic_TargetFlags *TargetFlags = &HostAdapter->TargetFlags[TargetID];
 		if (!TargetFlags->TargetExists)
 			continue;
-		Length +=
-		    sprintf(&Buffer[Length],
+		seq_printf(m,
 			    "  %2d	 Read	 %9u  %9u  %9u  %9u  %9u\n", TargetID,
 			    TargetStatistics[TargetID].ReadCommandSizeBuckets[0],
 			    TargetStatistics[TargetID].ReadCommandSizeBuckets[1], TargetStatistics[TargetID].ReadCommandSizeBuckets[2], TargetStatistics[TargetID].ReadCommandSizeBuckets[3], TargetStatistics[TargetID].ReadCommandSizeBuckets[4]);
-		Length +=
-		    sprintf(&Buffer[Length],
+		seq_printf(m,
 			    "  %2d	 Write	 %9u  %9u  %9u  %9u  %9u\n", TargetID,
 			    TargetStatistics[TargetID].WriteCommandSizeBuckets[0],
 			    TargetStatistics[TargetID].WriteCommandSizeBuckets[1], TargetStatistics[TargetID].WriteCommandSizeBuckets[2], TargetStatistics[TargetID].WriteCommandSizeBuckets[3], TargetStatistics[TargetID].WriteCommandSizeBuckets[4]);
 	}
-	Length += sprintf(&Buffer[Length], "\n\
+	seq_printf(m, "\n\
 Target  Command   16-32KB    32-64KB   64-128KB   128-256KB   256KB+\n\
 ======  =======  =========  =========  =========  =========  =========\n");
 	for (TargetID = 0; TargetID < HostAdapter->MaxTargetDevices; TargetID++) {
 		struct BusLogic_TargetFlags *TargetFlags = &HostAdapter->TargetFlags[TargetID];
 		if (!TargetFlags->TargetExists)
 			continue;
-		Length +=
-		    sprintf(&Buffer[Length],
+		seq_printf(m,
 			    "  %2d	 Read	 %9u  %9u  %9u  %9u  %9u\n", TargetID,
 			    TargetStatistics[TargetID].ReadCommandSizeBuckets[5],
 			    TargetStatistics[TargetID].ReadCommandSizeBuckets[6], TargetStatistics[TargetID].ReadCommandSizeBuckets[7], TargetStatistics[TargetID].ReadCommandSizeBuckets[8], TargetStatistics[TargetID].ReadCommandSizeBuckets[9]);
-		Length +=
-		    sprintf(&Buffer[Length],
+		seq_printf(m,
 			    "  %2d	 Write	 %9u  %9u  %9u  %9u  %9u\n", TargetID,
 			    TargetStatistics[TargetID].WriteCommandSizeBuckets[5],
 			    TargetStatistics[TargetID].WriteCommandSizeBuckets[6], TargetStatistics[TargetID].WriteCommandSizeBuckets[7], TargetStatistics[TargetID].WriteCommandSizeBuckets[8], TargetStatistics[TargetID].WriteCommandSizeBuckets[9]);
 	}
-	Length += sprintf(&Buffer[Length], "\n\n\
+	seq_printf(m, "\n\n\
 			   ERROR RECOVERY STATISTICS\n\
 \n\
 	  Command Aborts      Bus Device Resets	  Host Adapter Resets\n\
@@ -3299,20 +3299,12 @@ Target	Requested Completed  Requested Completed  Requested Completed\n\
 		struct BusLogic_TargetFlags *TargetFlags = &HostAdapter->TargetFlags[TargetID];
 		if (!TargetFlags->TargetExists)
 			continue;
-		Length += sprintf(&Buffer[Length], "\
+		seq_printf(m, "\
   %2d	 %5d %5d %5d    %5d %5d %5d	   %5d %5d %5d\n", TargetID, TargetStatistics[TargetID].CommandAbortsRequested, TargetStatistics[TargetID].CommandAbortsAttempted, TargetStatistics[TargetID].CommandAbortsCompleted, TargetStatistics[TargetID].BusDeviceResetsRequested, TargetStatistics[TargetID].BusDeviceResetsAttempted, TargetStatistics[TargetID].BusDeviceResetsCompleted, TargetStatistics[TargetID].HostAdapterResetsRequested, TargetStatistics[TargetID].HostAdapterResetsAttempted, TargetStatistics[TargetID].HostAdapterResetsCompleted);
 	}
-	Length += sprintf(&Buffer[Length], "\nExternal Host Adapter Resets: %d\n", HostAdapter->ExternalHostAdapterResets);
-	Length += sprintf(&Buffer[Length], "Host Adapter Internal Errors: %d\n", HostAdapter->HostAdapterInternalErrors);
-	if (Length >= BusLogic_MessageBufferSize)
-		BusLogic_Error("Message Buffer length %d exceeds size %d\n", HostAdapter, Length, BusLogic_MessageBufferSize);
-	if ((Length -= Offset) <= 0)
-		return 0;
-	if (Length >= BytesAvailable)
-		Length = BytesAvailable;
-	memcpy(ProcBuffer, HostAdapter->MessageBuffer + Offset, Length);
-	*StartPointer = ProcBuffer;
-	return Length;
+	seq_printf(m, "\nExternal Host Adapter Resets: %d\n", HostAdapter->ExternalHostAdapterResets);
+	seq_printf(m, "Host Adapter Internal Errors: %d\n", HostAdapter->HostAdapterInternalErrors);
+	return 0;
 }
 
 
@@ -3566,7 +3558,8 @@ static int __init BusLogic_ParseDriverOptions(char *OptionsString)
 static struct scsi_host_template Bus_Logic_template = {
 	.module = THIS_MODULE,
 	.proc_name = "BusLogic",
-	.proc_info = BusLogic_ProcDirectoryInfo,
+	.write_info = BusLogic_write_info,
+	.show_info = BusLogic_show_info,
 	.name = "BusLogic",
 	.info = BusLogic_DriverInfo,
 	.queuecommand = BusLogic_QueueCommand,
diff --git a/drivers/scsi/BusLogic.h b/drivers/scsi/BusLogic.h
index 649fcb31f26d..6c6c13c3be1b 100644
--- a/drivers/scsi/BusLogic.h
+++ b/drivers/scsi/BusLogic.h
@@ -1321,7 +1321,6 @@ static inline void BusLogic_IncrementSizeBucket(BusLogic_CommandSizeBuckets_T Co
 static const char *BusLogic_DriverInfo(struct Scsi_Host *);
 static int BusLogic_QueueCommand(struct Scsi_Host *h, struct scsi_cmnd *);
 static int BusLogic_BIOSDiskParameters(struct scsi_device *, struct block_device *, sector_t, int *);
-static int BusLogic_ProcDirectoryInfo(struct Scsi_Host *, char *, char **, off_t, int, int);
 static int BusLogic_SlaveConfigure(struct scsi_device *);
 static void BusLogic_QueueCompletedCCB(struct BusLogic_CCB *);
 static irqreturn_t BusLogic_InterruptHandler(int, void *);
diff --git a/drivers/scsi/NCR5380.c b/drivers/scsi/NCR5380.c
index 450353e04dde..1e9d6ad9302b 100644
--- a/drivers/scsi/NCR5380.c
+++ b/drivers/scsi/NCR5380.c
@@ -695,33 +695,35 @@ static void NCR5380_print_status(struct Scsi_Host *instance)
  * Return the number of bytes read from or written
  */
 
+static int __maybe_unused NCR5380_write_info(struct Scsi_Host *instance,
+	char *buffer, int length)
+{
+#ifdef DTC_PUBLIC_RELEASE
+	dtc_wmaxi = dtc_maxi = 0;
+#endif
+#ifdef PAS16_PUBLIC_RELEASE
+	pas_wmaxi = pas_maxi = 0;
+#endif
+	return (-ENOSYS);	/* Currently this is a no-op */
+}
+
 #undef SPRINTF
-#define SPRINTF(args...) do { if(pos < buffer + length-80) pos += sprintf(pos, ## args); } while(0)
+#define SPRINTF(args...) seq_printf(m, ## args)
 static
-char *lprint_Scsi_Cmnd(Scsi_Cmnd * cmd, char *pos, char *buffer, int length);
+void lprint_Scsi_Cmnd(Scsi_Cmnd * cmd, struct seq_file *m);
 static
-char *lprint_command(unsigned char *cmd, char *pos, char *buffer, int len);
+void lprint_command(unsigned char *cmd, struct seq_file *m);
 static
-char *lprint_opcode(int opcode, char *pos, char *buffer, int length);
+void lprint_opcode(int opcode, struct seq_file *m);
 
-static int __maybe_unused NCR5380_proc_info(struct Scsi_Host *instance,
-	char *buffer, char **start, off_t offset, int length, int inout)
+static int __maybe_unused NCR5380_show_info(struct seq_file *m,
+	struct Scsi_Host *instance)
 {
-	char *pos = buffer;
 	struct NCR5380_hostdata *hostdata;
 	Scsi_Cmnd *ptr;
 
 	hostdata = (struct NCR5380_hostdata *) instance->hostdata;
 
-	if (inout) {		/* Has data been written to the file ? */
-#ifdef DTC_PUBLIC_RELEASE
-		dtc_wmaxi = dtc_maxi = 0;
-#endif
-#ifdef PAS16_PUBLIC_RELEASE
-		pas_wmaxi = pas_maxi = 0;
-#endif
-		return (-ENOSYS);	/* Currently this is a no-op */
-	}
 	SPRINTF("NCR5380 core release=%d.   ", NCR5380_PUBLIC_RELEASE);
 	if (((struct NCR5380_hostdata *) instance->hostdata)->flags & FLAG_NCR53C400)
 		SPRINTF("ncr53c400 release=%d.  ", NCR53C400_PUBLIC_RELEASE);
@@ -755,46 +757,37 @@ static int __maybe_unused NCR5380_proc_info(struct Scsi_Host *instance,
 	if (!hostdata->connected)
 		SPRINTF("scsi%d: no currently connected command\n", instance->host_no);
 	else
-		pos = lprint_Scsi_Cmnd((Scsi_Cmnd *) hostdata->connected, pos, buffer, length);
+		lprint_Scsi_Cmnd((Scsi_Cmnd *) hostdata->connected, m);
 	SPRINTF("scsi%d: issue_queue\n", instance->host_no);
 	for (ptr = (Scsi_Cmnd *) hostdata->issue_queue; ptr; ptr = (Scsi_Cmnd *) ptr->host_scribble)
-		pos = lprint_Scsi_Cmnd(ptr, pos, buffer, length);
+		lprint_Scsi_Cmnd(ptr, m);
 
 	SPRINTF("scsi%d: disconnected_queue\n", instance->host_no);
 	for (ptr = (Scsi_Cmnd *) hostdata->disconnected_queue; ptr; ptr = (Scsi_Cmnd *) ptr->host_scribble)
-		pos = lprint_Scsi_Cmnd(ptr, pos, buffer, length);
+		lprint_Scsi_Cmnd(ptr, m);
 	spin_unlock_irq(instance->host_lock);
-	
-	*start = buffer;
-	if (pos - buffer < offset)
-		return 0;
-	else if (pos - buffer - offset < length)
-		return pos - buffer - offset;
-	return length;
+	return 0;
 }
 
-static char *lprint_Scsi_Cmnd(Scsi_Cmnd * cmd, char *pos, char *buffer, int length)
+static void lprint_Scsi_Cmnd(Scsi_Cmnd * cmd, struct seq_file *m)
 {
 	SPRINTF("scsi%d : destination target %d, lun %d\n", cmd->device->host->host_no, cmd->device->id, cmd->device->lun);
 	SPRINTF("        command = ");
-	pos = lprint_command(cmd->cmnd, pos, buffer, length);
-	return (pos);
+	lprint_command(cmd->cmnd, m);
 }
 
-static char *lprint_command(unsigned char *command, char *pos, char *buffer, int length)
+static void lprint_command(unsigned char *command, struct seq_file *m)
 {
 	int i, s;
-	pos = lprint_opcode(command[0], pos, buffer, length);
+	lprint_opcode(command[0], m);
 	for (i = 1, s = COMMAND_SIZE(command[0]); i < s; ++i)
 		SPRINTF("%02x ", command[i]);
 	SPRINTF("\n");
-	return (pos);
 }
 
-static char *lprint_opcode(int opcode, char *pos, char *buffer, int length)
+static void lprint_opcode(int opcode, struct seq_file *m)
 {
 	SPRINTF("%2d (0x%02x)", opcode, opcode);
-	return (pos);
 }
 
 
diff --git a/drivers/scsi/NCR5380.h b/drivers/scsi/NCR5380.h
index fd40a32b1f6f..14964d0a0e9d 100644
--- a/drivers/scsi/NCR5380.h
+++ b/drivers/scsi/NCR5380.h
@@ -314,8 +314,10 @@ static void NCR5380_print(struct Scsi_Host *instance);
 static int NCR5380_abort(Scsi_Cmnd * cmd);
 static int NCR5380_bus_reset(Scsi_Cmnd * cmd);
 static int NCR5380_queue_command(struct Scsi_Host *, struct scsi_cmnd *);
-static int __maybe_unused NCR5380_proc_info(struct Scsi_Host *instance,
-	char *buffer, char **start, off_t offset, int length, int inout);
+static int __maybe_unused NCR5380_show_info(struct seq_file *,
+	struct Scsi_Host *);
+static int __maybe_unused NCR5380_write_info(struct Scsi_Host *instance,
+	char *buffer, int length);
 
 static void NCR5380_reselect(struct Scsi_Host *instance);
 static int NCR5380_select(struct Scsi_Host *instance, Scsi_Cmnd * cmd, int tag);
diff --git a/drivers/scsi/a2091.c b/drivers/scsi/a2091.c
index 3e09aa21c1ca..30fa38a0ad39 100644
--- a/drivers/scsi/a2091.c
+++ b/drivers/scsi/a2091.c
@@ -166,7 +166,8 @@ static int a2091_bus_reset(struct scsi_cmnd *cmd)
 static struct scsi_host_template a2091_scsi_template = {
 	.module			= THIS_MODULE,
 	.name			= "Commodore A2091/A590 SCSI",
-	.proc_info		= wd33c93_proc_info,
+	.show_info		= wd33c93_show_info,
+	.write_info		= wd33c93_write_info,
 	.proc_name		= "A2901",
 	.queuecommand		= wd33c93_queuecommand,
 	.eh_abort_handler	= wd33c93_abort,
diff --git a/drivers/scsi/a3000.c b/drivers/scsi/a3000.c
index e29fe0e708f8..c487916a9d45 100644
--- a/drivers/scsi/a3000.c
+++ b/drivers/scsi/a3000.c
@@ -181,7 +181,8 @@ static int a3000_bus_reset(struct scsi_cmnd *cmd)
 static struct scsi_host_template amiga_a3000_scsi_template = {
 	.module			= THIS_MODULE,
 	.name			= "Amiga 3000 built-in SCSI",
-	.proc_info		= wd33c93_proc_info,
+	.show_info		= wd33c93_show_info,
+	.write_info		= wd33c93_write_info,
 	.proc_name		= "A3000",
 	.queuecommand		= wd33c93_queuecommand,
 	.eh_abort_handler	= wd33c93_abort,
diff --git a/drivers/scsi/aacraid/aacraid.h b/drivers/scsi/aacraid/aacraid.h
index a6f7190c09a4..9323d058706b 100644
--- a/drivers/scsi/aacraid/aacraid.h
+++ b/drivers/scsi/aacraid/aacraid.h
@@ -12,7 +12,7 @@
  *----------------------------------------------------------------------------*/
 
 #ifndef AAC_DRIVER_BUILD
-# define AAC_DRIVER_BUILD 30000
+# define AAC_DRIVER_BUILD 30200
 # define AAC_DRIVER_BRANCH "-ms"
 #endif
 #define MAXIMUM_NUM_CONTAINERS	32
@@ -1918,6 +1918,10 @@ extern struct aac_common aac_config;
 #define	MONITOR_PANIC			0x00000020
 #define	KERNEL_UP_AND_RUNNING		0x00000080
 #define	KERNEL_PANIC			0x00000100
+#define	FLASH_UPD_PENDING		0x00002000
+#define	FLASH_UPD_SUCCESS		0x00004000
+#define	FLASH_UPD_FAILED		0x00008000
+#define	FWUPD_TIMEOUT			(5 * 60)
 
 /*
  *	Doorbell bit defines
diff --git a/drivers/scsi/aacraid/comminit.c b/drivers/scsi/aacraid/comminit.c
index 3f759957f4b4..177b094c7792 100644
--- a/drivers/scsi/aacraid/comminit.c
+++ b/drivers/scsi/aacraid/comminit.c
@@ -214,7 +214,7 @@ int aac_send_shutdown(struct aac_dev * dev)
 	cmd = (struct aac_close *) fib_data(fibctx);
 
 	cmd->command = cpu_to_le32(VM_CloseAll);
-	cmd->cid = cpu_to_le32(0xffffffff);
+	cmd->cid = cpu_to_le32(0xfffffffe);
 
 	status = aac_fib_send(ContainerCommand,
 			  fibctx,
diff --git a/drivers/scsi/aacraid/src.c b/drivers/scsi/aacraid/src.c
index e2e349204e7d..0f56d8d7524f 100644
--- a/drivers/scsi/aacraid/src.c
+++ b/drivers/scsi/aacraid/src.c
@@ -703,6 +703,28 @@ int aac_srcv_init(struct aac_dev *dev)
 		!aac_src_restart_adapter(dev, 0))
 		++restart;
 	/*
+	 *	Check to see if flash update is running.
+	 *	Wait for the adapter to be up and running. Wait up to 5 minutes
+	 */
+	status = src_readl(dev, MUnit.OMR);
+	if (status & FLASH_UPD_PENDING) {
+		start = jiffies;
+		do {
+			status = src_readl(dev, MUnit.OMR);
+			if (time_after(jiffies, start+HZ*FWUPD_TIMEOUT)) {
+				printk(KERN_ERR "%s%d: adapter flash update failed.\n",
+					dev->name, instance);
+				goto error_iounmap;
+			}
+		} while (!(status & FLASH_UPD_SUCCESS) &&
+			 !(status & FLASH_UPD_FAILED));
+		/* Delay 10 seconds.
+		 * Because right now FW is doing a soft reset,
+		 * do not read scratch pad register at this time
+		 */
+		ssleep(10);
+	}
+	/*
 	 *	Check to see if the board panic'd while booting.
 	 */
 	status = src_readl(dev, MUnit.OMR);
@@ -730,7 +752,9 @@ int aac_srcv_init(struct aac_dev *dev)
 	/*
 	 *	Wait for the adapter to be up and running. Wait up to 3 minutes
 	 */
-	while (!((status = src_readl(dev, MUnit.OMR)) & KERNEL_UP_AND_RUNNING)) {
+	while (!((status = src_readl(dev, MUnit.OMR)) &
+		KERNEL_UP_AND_RUNNING) ||
+		status == 0xffffffff) {
 		if ((restart &&
 		  (status & (KERNEL_PANIC|SELF_TEST_FAILED|MONITOR_PANIC))) ||
 		  time_after(jiffies, start+HZ*startup_timeout)) {
diff --git a/drivers/scsi/advansys.c b/drivers/scsi/advansys.c
index dcfaee66a8b9..c67e401954c5 100644
--- a/drivers/scsi/advansys.c
+++ b/drivers/scsi/advansys.c
@@ -2178,22 +2178,6 @@ do { \
 
 #define ASC_INFO_SIZE           128	/* advansys_info() line size */
 
-#ifdef CONFIG_PROC_FS
-/* /proc/scsi/advansys/[0...] related definitions */
-#define ASC_PRTBUF_SIZE         2048
-#define ASC_PRTLINE_SIZE        160
-
-#define ASC_PRT_NEXT() \
-    if (cp) { \
-        totlen += len; \
-        leftlen -= len; \
-        if (leftlen == 0) { \
-            return totlen; \
-        } \
-        cp += len; \
-    }
-#endif /* CONFIG_PROC_FS */
-
 /* Asc Library return codes */
 #define ASC_TRUE        1
 #define ASC_FALSE       0
@@ -2384,7 +2368,6 @@ struct asc_board {
 	} eep_config;
 	ulong last_reset;	/* Saved last reset time */
 	/* /proc/scsi/advansys/[0...] */
-	char *prtbuf;		/* /proc print buffer */
 #ifdef ADVANSYS_STATS
 	struct asc_stats asc_stats;	/* Board statistics */
 #endif				/* ADVANSYS_STATS */
@@ -2875,64 +2858,21 @@ static const char *advansys_info(struct Scsi_Host *shost)
 }
 
 #ifdef CONFIG_PROC_FS
-/*
- * asc_prt_line()
- *
- * If 'cp' is NULL print to the console, otherwise print to a buffer.
- *
- * Return 0 if printing to the console, otherwise return the number of
- * bytes written to the buffer.
- *
- * Note: If any single line is greater than ASC_PRTLINE_SIZE bytes the stack
- * will be corrupted. 's[]' is defined to be ASC_PRTLINE_SIZE bytes.
- */
-static int asc_prt_line(char *buf, int buflen, char *fmt, ...)
-{
-	va_list args;
-	int ret;
-	char s[ASC_PRTLINE_SIZE];
-
-	va_start(args, fmt);
-	ret = vsprintf(s, fmt, args);
-	BUG_ON(ret >= ASC_PRTLINE_SIZE);
-	if (buf == NULL) {
-		(void)printk(s);
-		ret = 0;
-	} else {
-		ret = min(buflen, ret);
-		memcpy(buf, s, ret);
-	}
-	va_end(args);
-	return ret;
-}
 
 /*
  * asc_prt_board_devices()
  *
  * Print driver information for devices attached to the board.
- *
- * Note: no single line should be greater than ASC_PRTLINE_SIZE,
- * cf. asc_prt_line().
- *
- * Return the number of characters copied into 'cp'. No more than
- * 'cplen' characters will be copied to 'cp'.
  */
-static int asc_prt_board_devices(struct Scsi_Host *shost, char *cp, int cplen)
+static void asc_prt_board_devices(struct seq_file *m, struct Scsi_Host *shost)
 {
 	struct asc_board *boardp = shost_priv(shost);
-	int leftlen;
-	int totlen;
-	int len;
 	int chip_scsi_id;
 	int i;
 
-	leftlen = cplen;
-	totlen = len = 0;
-
-	len = asc_prt_line(cp, leftlen,
-			   "\nDevice Information for AdvanSys SCSI Host %d:\n",
-			   shost->host_no);
-	ASC_PRT_NEXT();
+	seq_printf(m,
+		   "\nDevice Information for AdvanSys SCSI Host %d:\n",
+		   shost->host_no);
 
 	if (ASC_NARROW_BOARD(boardp)) {
 		chip_scsi_id = boardp->dvc_cfg.asc_dvc_cfg.chip_scsi_id;
@@ -2940,60 +2880,42 @@ static int asc_prt_board_devices(struct Scsi_Host *shost, char *cp, int cplen)
 		chip_scsi_id = boardp->dvc_var.adv_dvc_var.chip_scsi_id;
 	}
 
-	len = asc_prt_line(cp, leftlen, "Target IDs Detected:");
-	ASC_PRT_NEXT();
+	seq_printf(m, "Target IDs Detected:");
 	for (i = 0; i <= ADV_MAX_TID; i++) {
-		if (boardp->init_tidmask & ADV_TID_TO_TIDMASK(i)) {
-			len = asc_prt_line(cp, leftlen, " %X,", i);
-			ASC_PRT_NEXT();
-		}
+		if (boardp->init_tidmask & ADV_TID_TO_TIDMASK(i))
+			seq_printf(m, " %X,", i);
 	}
-	len = asc_prt_line(cp, leftlen, " (%X=Host Adapter)\n", chip_scsi_id);
-	ASC_PRT_NEXT();
-
-	return totlen;
+	seq_printf(m, " (%X=Host Adapter)\n", chip_scsi_id);
 }
 
 /*
  * Display Wide Board BIOS Information.
  */
-static int asc_prt_adv_bios(struct Scsi_Host *shost, char *cp, int cplen)
+static void asc_prt_adv_bios(struct seq_file *m, struct Scsi_Host *shost)
 {
 	struct asc_board *boardp = shost_priv(shost);
-	int leftlen;
-	int totlen;
-	int len;
 	ushort major, minor, letter;
 
-	leftlen = cplen;
-	totlen = len = 0;
-
-	len = asc_prt_line(cp, leftlen, "\nROM BIOS Version: ");
-	ASC_PRT_NEXT();
+	seq_printf(m, "\nROM BIOS Version: ");
 
 	/*
 	 * If the BIOS saved a valid signature, then fill in
 	 * the BIOS code segment base address.
 	 */
 	if (boardp->bios_signature != 0x55AA) {
-		len = asc_prt_line(cp, leftlen, "Disabled or Pre-3.1\n");
-		ASC_PRT_NEXT();
-		len = asc_prt_line(cp, leftlen,
-				   "BIOS either disabled or Pre-3.1. If it is pre-3.1, then a newer version\n");
-		ASC_PRT_NEXT();
-		len = asc_prt_line(cp, leftlen,
-				   "can be found at the ConnectCom FTP site: ftp://ftp.connectcom.net/pub\n");
-		ASC_PRT_NEXT();
+		seq_printf(m, "Disabled or Pre-3.1\n");
+		seq_printf(m,
+			  "BIOS either disabled or Pre-3.1. If it is pre-3.1, then a newer version\n");
+		seq_printf(m,
+			  "can be found at the ConnectCom FTP site: ftp://ftp.connectcom.net/pub\n");
 	} else {
 		major = (boardp->bios_version >> 12) & 0xF;
 		minor = (boardp->bios_version >> 8) & 0xF;
 		letter = (boardp->bios_version & 0xFF);
 
-		len = asc_prt_line(cp, leftlen, "%d.%d%c\n",
+		seq_printf(m, "%d.%d%c\n",
 				   major, minor,
 				   letter >= 26 ? '?' : letter + 'A');
-		ASC_PRT_NEXT();
-
 		/*
 		 * Current available ROM BIOS release is 3.1I for UW
 		 * and 3.2I for U2W. This code doesn't differentiate
@@ -3001,16 +2923,12 @@ static int asc_prt_adv_bios(struct Scsi_Host *shost, char *cp, int cplen)
 		 */
 		if (major < 3 || (major <= 3 && minor < 1) ||
 		    (major <= 3 && minor <= 1 && letter < ('I' - 'A'))) {
-			len = asc_prt_line(cp, leftlen,
-					   "Newer version of ROM BIOS is available at the ConnectCom FTP site:\n");
-			ASC_PRT_NEXT();
-			len = asc_prt_line(cp, leftlen,
-					   "ftp://ftp.connectcom.net/pub\n");
-			ASC_PRT_NEXT();
+			seq_printf(m,
+				   "Newer version of ROM BIOS is available at the ConnectCom FTP site:\n");
+			seq_printf(m,
+				   "ftp://ftp.connectcom.net/pub\n");
 		}
 	}
-
-	return totlen;
 }
 
 /*
@@ -3115,20 +3033,11 @@ static int asc_get_eeprom_string(ushort *serialnum, uchar *cp)
  * asc_prt_asc_board_eeprom()
  *
  * Print board EEPROM configuration.
- *
- * Note: no single line should be greater than ASC_PRTLINE_SIZE,
- * cf. asc_prt_line().
- *
- * Return the number of characters copied into 'cp'. No more than
- * 'cplen' characters will be copied to 'cp'.
  */
-static int asc_prt_asc_board_eeprom(struct Scsi_Host *shost, char *cp, int cplen)
+static void asc_prt_asc_board_eeprom(struct seq_file *m, struct Scsi_Host *shost)
 {
 	struct asc_board *boardp = shost_priv(shost);
 	ASC_DVC_VAR *asc_dvc_varp;
-	int leftlen;
-	int totlen;
-	int len;
 	ASCEEP_CONFIG *ep;
 	int i;
 #ifdef CONFIG_ISA
@@ -3139,129 +3048,75 @@ static int asc_prt_asc_board_eeprom(struct Scsi_Host *shost, char *cp, int cplen
 	asc_dvc_varp = &boardp->dvc_var.asc_dvc_var;
 	ep = &boardp->eep_config.asc_eep;
 
-	leftlen = cplen;
-	totlen = len = 0;
-
-	len = asc_prt_line(cp, leftlen,
-			   "\nEEPROM Settings for AdvanSys SCSI Host %d:\n",
-			   shost->host_no);
-	ASC_PRT_NEXT();
+	seq_printf(m,
+		   "\nEEPROM Settings for AdvanSys SCSI Host %d:\n",
+		   shost->host_no);
 
 	if (asc_get_eeprom_string((ushort *)&ep->adapter_info[0], serialstr)
-	    == ASC_TRUE) {
-		len =
-		    asc_prt_line(cp, leftlen, " Serial Number: %s\n",
-				 serialstr);
-		ASC_PRT_NEXT();
-	} else {
-		if (ep->adapter_info[5] == 0xBB) {
-			len = asc_prt_line(cp, leftlen,
-					   " Default Settings Used for EEPROM-less Adapter.\n");
-			ASC_PRT_NEXT();
-		} else {
-			len = asc_prt_line(cp, leftlen,
-					   " Serial Number Signature Not Present.\n");
-			ASC_PRT_NEXT();
-		}
-	}
-
-	len = asc_prt_line(cp, leftlen,
-			   " Host SCSI ID: %u, Host Queue Size: %u, Device Queue Size: %u\n",
-			   ASC_EEP_GET_CHIP_ID(ep), ep->max_total_qng,
-			   ep->max_tag_qng);
-	ASC_PRT_NEXT();
-
-	len = asc_prt_line(cp, leftlen,
-			   " cntl 0x%x, no_scam 0x%x\n", ep->cntl, ep->no_scam);
-	ASC_PRT_NEXT();
-
-	len = asc_prt_line(cp, leftlen, " Target ID:           ");
-	ASC_PRT_NEXT();
-	for (i = 0; i <= ASC_MAX_TID; i++) {
-		len = asc_prt_line(cp, leftlen, " %d", i);
-		ASC_PRT_NEXT();
-	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
-
-	len = asc_prt_line(cp, leftlen, " Disconnects:         ");
-	ASC_PRT_NEXT();
-	for (i = 0; i <= ASC_MAX_TID; i++) {
-		len = asc_prt_line(cp, leftlen, " %c",
-				   (ep->
-				    disc_enable & ADV_TID_TO_TIDMASK(i)) ? 'Y' :
-				   'N');
-		ASC_PRT_NEXT();
-	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
-
-	len = asc_prt_line(cp, leftlen, " Command Queuing:     ");
-	ASC_PRT_NEXT();
-	for (i = 0; i <= ASC_MAX_TID; i++) {
-		len = asc_prt_line(cp, leftlen, " %c",
-				   (ep->
-				    use_cmd_qng & ADV_TID_TO_TIDMASK(i)) ? 'Y' :
-				   'N');
-		ASC_PRT_NEXT();
-	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
-
-	len = asc_prt_line(cp, leftlen, " Start Motor:         ");
-	ASC_PRT_NEXT();
-	for (i = 0; i <= ASC_MAX_TID; i++) {
-		len = asc_prt_line(cp, leftlen, " %c",
-				   (ep->
-				    start_motor & ADV_TID_TO_TIDMASK(i)) ? 'Y' :
-				   'N');
-		ASC_PRT_NEXT();
-	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
-
-	len = asc_prt_line(cp, leftlen, " Synchronous Transfer:");
-	ASC_PRT_NEXT();
-	for (i = 0; i <= ASC_MAX_TID; i++) {
-		len = asc_prt_line(cp, leftlen, " %c",
-				   (ep->
-				    init_sdtr & ADV_TID_TO_TIDMASK(i)) ? 'Y' :
-				   'N');
-		ASC_PRT_NEXT();
-	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	    == ASC_TRUE)
+		seq_printf(m, " Serial Number: %s\n", serialstr);
+	else if (ep->adapter_info[5] == 0xBB)
+		seq_printf(m,
+			   " Default Settings Used for EEPROM-less Adapter.\n");
+	else
+		seq_printf(m,
+			   " Serial Number Signature Not Present.\n");
+
+	seq_printf(m,
+		   " Host SCSI ID: %u, Host Queue Size: %u, Device Queue Size: %u\n",
+		   ASC_EEP_GET_CHIP_ID(ep), ep->max_total_qng,
+		   ep->max_tag_qng);
+
+	seq_printf(m,
+		   " cntl 0x%x, no_scam 0x%x\n", ep->cntl, ep->no_scam);
+
+	seq_printf(m, " Target ID:           ");
+	for (i = 0; i <= ASC_MAX_TID; i++)
+		seq_printf(m, " %d", i);
+	seq_printf(m, "\n");
+
+	seq_printf(m, " Disconnects:         ");
+	for (i = 0; i <= ASC_MAX_TID; i++)
+		seq_printf(m, " %c",
+			   (ep->disc_enable & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
+	seq_printf(m, "\n");
+
+	seq_printf(m, " Command Queuing:     ");
+	for (i = 0; i <= ASC_MAX_TID; i++)
+		seq_printf(m, " %c",
+			   (ep->use_cmd_qng & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
+	seq_printf(m, "\n");
+
+	seq_printf(m, " Start Motor:         ");
+	for (i = 0; i <= ASC_MAX_TID; i++)
+		seq_printf(m, " %c",
+			   (ep->start_motor & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
+	seq_printf(m, "\n");
+
+	seq_printf(m, " Synchronous Transfer:");
+	for (i = 0; i <= ASC_MAX_TID; i++)
+		seq_printf(m, " %c",
+			   (ep->init_sdtr & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
+	seq_printf(m, "\n");
 
 #ifdef CONFIG_ISA
 	if (asc_dvc_varp->bus_type & ASC_IS_ISA) {
-		len = asc_prt_line(cp, leftlen,
-				   " Host ISA DMA speed:   %d MB/S\n",
-				   isa_dma_speed[ASC_EEP_GET_DMA_SPD(ep)]);
-		ASC_PRT_NEXT();
+		seq_printf(m,
+			   " Host ISA DMA speed:   %d MB/S\n",
+			   isa_dma_speed[ASC_EEP_GET_DMA_SPD(ep)]);
 	}
 #endif /* CONFIG_ISA */
-
-	return totlen;
 }
 
 /*
  * asc_prt_adv_board_eeprom()
  *
  * Print board EEPROM configuration.
- *
- * Note: no single line should be greater than ASC_PRTLINE_SIZE,
- * cf. asc_prt_line().
- *
- * Return the number of characters copied into 'cp'. No more than
- * 'cplen' characters will be copied to 'cp'.
  */
-static int asc_prt_adv_board_eeprom(struct Scsi_Host *shost, char *cp, int cplen)
+static void asc_prt_adv_board_eeprom(struct seq_file *m, struct Scsi_Host *shost)
 {
 	struct asc_board *boardp = shost_priv(shost);
 	ADV_DVC_VAR *adv_dvc_varp;
-	int leftlen;
-	int totlen;
-	int len;
 	int i;
 	char *termstr;
 	uchar serialstr[13];
@@ -3281,13 +3136,9 @@ static int asc_prt_adv_board_eeprom(struct Scsi_Host *shost, char *cp, int cplen
 		ep_38C1600 = &boardp->eep_config.adv_38C1600_eep;
 	}
 
-	leftlen = cplen;
-	totlen = len = 0;
-
-	len = asc_prt_line(cp, leftlen,
-			   "\nEEPROM Settings for AdvanSys SCSI Host %d:\n",
-			   shost->host_no);
-	ASC_PRT_NEXT();
+	seq_printf(m,
+		   "\nEEPROM Settings for AdvanSys SCSI Host %d:\n",
+		   shost->host_no);
 
 	if (adv_dvc_varp->chip_type == ADV_CHIP_ASC3550) {
 		wordp = &ep_3550->serial_number_word1;
@@ -3297,38 +3148,28 @@ static int asc_prt_adv_board_eeprom(struct Scsi_Host *shost, char *cp, int cplen
 		wordp = &ep_38C1600->serial_number_word1;
 	}
 
-	if (asc_get_eeprom_string(wordp, serialstr) == ASC_TRUE) {
-		len =
-		    asc_prt_line(cp, leftlen, " Serial Number: %s\n",
-				 serialstr);
-		ASC_PRT_NEXT();
-	} else {
-		len = asc_prt_line(cp, leftlen,
-				   " Serial Number Signature Not Present.\n");
-		ASC_PRT_NEXT();
-	}
+	if (asc_get_eeprom_string(wordp, serialstr) == ASC_TRUE)
+		seq_printf(m, " Serial Number: %s\n", serialstr);
+	else
+		seq_printf(m, " Serial Number Signature Not Present.\n");
 
-	if (adv_dvc_varp->chip_type == ADV_CHIP_ASC3550) {
-		len = asc_prt_line(cp, leftlen,
-				   " Host SCSI ID: %u, Host Queue Size: %u, Device Queue Size: %u\n",
-				   ep_3550->adapter_scsi_id,
-				   ep_3550->max_host_qng, ep_3550->max_dvc_qng);
-		ASC_PRT_NEXT();
-	} else if (adv_dvc_varp->chip_type == ADV_CHIP_ASC38C0800) {
-		len = asc_prt_line(cp, leftlen,
-				   " Host SCSI ID: %u, Host Queue Size: %u, Device Queue Size: %u\n",
-				   ep_38C0800->adapter_scsi_id,
-				   ep_38C0800->max_host_qng,
-				   ep_38C0800->max_dvc_qng);
-		ASC_PRT_NEXT();
-	} else {
-		len = asc_prt_line(cp, leftlen,
-				   " Host SCSI ID: %u, Host Queue Size: %u, Device Queue Size: %u\n",
-				   ep_38C1600->adapter_scsi_id,
-				   ep_38C1600->max_host_qng,
-				   ep_38C1600->max_dvc_qng);
-		ASC_PRT_NEXT();
-	}
+	if (adv_dvc_varp->chip_type == ADV_CHIP_ASC3550)
+		seq_printf(m,
+			   " Host SCSI ID: %u, Host Queue Size: %u, Device Queue Size: %u\n",
+			   ep_3550->adapter_scsi_id,
+			   ep_3550->max_host_qng, ep_3550->max_dvc_qng);
+	else if (adv_dvc_varp->chip_type == ADV_CHIP_ASC38C0800)
+		seq_printf(m,
+			   " Host SCSI ID: %u, Host Queue Size: %u, Device Queue Size: %u\n",
+			   ep_38C0800->adapter_scsi_id,
+			   ep_38C0800->max_host_qng,
+			   ep_38C0800->max_dvc_qng);
+	else
+		seq_printf(m,
+			   " Host SCSI ID: %u, Host Queue Size: %u, Device Queue Size: %u\n",
+			   ep_38C1600->adapter_scsi_id,
+			   ep_38C1600->max_host_qng,
+			   ep_38C1600->max_dvc_qng);
 	if (adv_dvc_varp->chip_type == ADV_CHIP_ASC3550) {
 		word = ep_3550->termination;
 	} else if (adv_dvc_varp->chip_type == ADV_CHIP_ASC38C0800) {
@@ -3352,34 +3193,26 @@ static int asc_prt_adv_board_eeprom(struct Scsi_Host *shost, char *cp, int cplen
 		break;
 	}
 
-	if (adv_dvc_varp->chip_type == ADV_CHIP_ASC3550) {
-		len = asc_prt_line(cp, leftlen,
-				   " termination: %u (%s), bios_ctrl: 0x%x\n",
-				   ep_3550->termination, termstr,
-				   ep_3550->bios_ctrl);
-		ASC_PRT_NEXT();
-	} else if (adv_dvc_varp->chip_type == ADV_CHIP_ASC38C0800) {
-		len = asc_prt_line(cp, leftlen,
-				   " termination: %u (%s), bios_ctrl: 0x%x\n",
-				   ep_38C0800->termination_lvd, termstr,
-				   ep_38C0800->bios_ctrl);
-		ASC_PRT_NEXT();
-	} else {
-		len = asc_prt_line(cp, leftlen,
-				   " termination: %u (%s), bios_ctrl: 0x%x\n",
-				   ep_38C1600->termination_lvd, termstr,
-				   ep_38C1600->bios_ctrl);
-		ASC_PRT_NEXT();
-	}
+	if (adv_dvc_varp->chip_type == ADV_CHIP_ASC3550)
+		seq_printf(m,
+			   " termination: %u (%s), bios_ctrl: 0x%x\n",
+			   ep_3550->termination, termstr,
+			   ep_3550->bios_ctrl);
+	else if (adv_dvc_varp->chip_type == ADV_CHIP_ASC38C0800)
+		seq_printf(m,
+			   " termination: %u (%s), bios_ctrl: 0x%x\n",
+			   ep_38C0800->termination_lvd, termstr,
+			   ep_38C0800->bios_ctrl);
+	else
+		seq_printf(m,
+			   " termination: %u (%s), bios_ctrl: 0x%x\n",
+			   ep_38C1600->termination_lvd, termstr,
+			   ep_38C1600->bios_ctrl);
 
-	len = asc_prt_line(cp, leftlen, " Target ID:           ");
-	ASC_PRT_NEXT();
-	for (i = 0; i <= ADV_MAX_TID; i++) {
-		len = asc_prt_line(cp, leftlen, " %X", i);
-		ASC_PRT_NEXT();
-	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Target ID:           ");
+	for (i = 0; i <= ADV_MAX_TID; i++)
+		seq_printf(m, " %X", i);
+	seq_printf(m, "\n");
 
 	if (adv_dvc_varp->chip_type == ADV_CHIP_ASC3550) {
 		word = ep_3550->disc_enable;
@@ -3388,15 +3221,11 @@ static int asc_prt_adv_board_eeprom(struct Scsi_Host *shost, char *cp, int cplen
 	} else {
 		word = ep_38C1600->disc_enable;
 	}
-	len = asc_prt_line(cp, leftlen, " Disconnects:         ");
-	ASC_PRT_NEXT();
-	for (i = 0; i <= ADV_MAX_TID; i++) {
-		len = asc_prt_line(cp, leftlen, " %c",
-				   (word & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
-		ASC_PRT_NEXT();
-	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Disconnects:         ");
+	for (i = 0; i <= ADV_MAX_TID; i++)
+		seq_printf(m, " %c",
+			   (word & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
+	seq_printf(m, "\n");
 
 	if (adv_dvc_varp->chip_type == ADV_CHIP_ASC3550) {
 		word = ep_3550->tagqng_able;
@@ -3405,15 +3234,11 @@ static int asc_prt_adv_board_eeprom(struct Scsi_Host *shost, char *cp, int cplen
 	} else {
 		word = ep_38C1600->tagqng_able;
 	}
-	len = asc_prt_line(cp, leftlen, " Command Queuing:     ");
-	ASC_PRT_NEXT();
-	for (i = 0; i <= ADV_MAX_TID; i++) {
-		len = asc_prt_line(cp, leftlen, " %c",
-				   (word & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
-		ASC_PRT_NEXT();
-	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Command Queuing:     ");
+	for (i = 0; i <= ADV_MAX_TID; i++)
+		seq_printf(m, " %c",
+			   (word & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
+	seq_printf(m, "\n");
 
 	if (adv_dvc_varp->chip_type == ADV_CHIP_ASC3550) {
 		word = ep_3550->start_motor;
@@ -3422,42 +3247,28 @@ static int asc_prt_adv_board_eeprom(struct Scsi_Host *shost, char *cp, int cplen
 	} else {
 		word = ep_38C1600->start_motor;
 	}
-	len = asc_prt_line(cp, leftlen, " Start Motor:         ");
-	ASC_PRT_NEXT();
-	for (i = 0; i <= ADV_MAX_TID; i++) {
-		len = asc_prt_line(cp, leftlen, " %c",
-				   (word & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
-		ASC_PRT_NEXT();
-	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Start Motor:         ");
+	for (i = 0; i <= ADV_MAX_TID; i++)
+		seq_printf(m, " %c",
+			   (word & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
+	seq_printf(m, "\n");
 
 	if (adv_dvc_varp->chip_type == ADV_CHIP_ASC3550) {
-		len = asc_prt_line(cp, leftlen, " Synchronous Transfer:");
-		ASC_PRT_NEXT();
-		for (i = 0; i <= ADV_MAX_TID; i++) {
-			len = asc_prt_line(cp, leftlen, " %c",
-					   (ep_3550->
-					    sdtr_able & ADV_TID_TO_TIDMASK(i)) ?
-					   'Y' : 'N');
-			ASC_PRT_NEXT();
-		}
-		len = asc_prt_line(cp, leftlen, "\n");
-		ASC_PRT_NEXT();
+		seq_printf(m, " Synchronous Transfer:");
+		for (i = 0; i <= ADV_MAX_TID; i++)
+			seq_printf(m, " %c",
+				   (ep_3550->sdtr_able & ADV_TID_TO_TIDMASK(i)) ?
+				   'Y' : 'N');
+		seq_printf(m, "\n");
 	}
 
 	if (adv_dvc_varp->chip_type == ADV_CHIP_ASC3550) {
-		len = asc_prt_line(cp, leftlen, " Ultra Transfer:      ");
-		ASC_PRT_NEXT();
-		for (i = 0; i <= ADV_MAX_TID; i++) {
-			len = asc_prt_line(cp, leftlen, " %c",
-					   (ep_3550->
-					    ultra_able & ADV_TID_TO_TIDMASK(i))
-					   ? 'Y' : 'N');
-			ASC_PRT_NEXT();
-		}
-		len = asc_prt_line(cp, leftlen, "\n");
-		ASC_PRT_NEXT();
+		seq_printf(m, " Ultra Transfer:      ");
+		for (i = 0; i <= ADV_MAX_TID; i++)
+			seq_printf(m, " %c",
+				   (ep_3550->ultra_able & ADV_TID_TO_TIDMASK(i))
+				   ? 'Y' : 'N');
+		seq_printf(m, "\n");
 	}
 
 	if (adv_dvc_varp->chip_type == ADV_CHIP_ASC3550) {
@@ -3467,21 +3278,16 @@ static int asc_prt_adv_board_eeprom(struct Scsi_Host *shost, char *cp, int cplen
 	} else {
 		word = ep_38C1600->wdtr_able;
 	}
-	len = asc_prt_line(cp, leftlen, " Wide Transfer:       ");
-	ASC_PRT_NEXT();
-	for (i = 0; i <= ADV_MAX_TID; i++) {
-		len = asc_prt_line(cp, leftlen, " %c",
-				   (word & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
-		ASC_PRT_NEXT();
-	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Wide Transfer:       ");
+	for (i = 0; i <= ADV_MAX_TID; i++)
+		seq_printf(m, " %c",
+			   (word & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
+	seq_printf(m, "\n");
 
 	if (adv_dvc_varp->chip_type == ADV_CHIP_ASC38C0800 ||
 	    adv_dvc_varp->chip_type == ADV_CHIP_ASC38C1600) {
-		len = asc_prt_line(cp, leftlen,
-				   " Synchronous Transfer Speed (Mhz):\n  ");
-		ASC_PRT_NEXT();
+		seq_printf(m,
+			   " Synchronous Transfer Speed (Mhz):\n  ");
 		for (i = 0; i <= ADV_MAX_TID; i++) {
 			char *speed_str;
 
@@ -3517,99 +3323,64 @@ static int asc_prt_adv_board_eeprom(struct Scsi_Host *shost, char *cp, int cplen
 				speed_str = "Unk";
 				break;
 			}
-			len = asc_prt_line(cp, leftlen, "%X:%s ", i, speed_str);
-			ASC_PRT_NEXT();
-			if (i == 7) {
-				len = asc_prt_line(cp, leftlen, "\n  ");
-				ASC_PRT_NEXT();
-			}
+			seq_printf(m, "%X:%s ", i, speed_str);
+			if (i == 7)
+				seq_printf(m, "\n  ");
 			sdtr_speed >>= 4;
 		}
-		len = asc_prt_line(cp, leftlen, "\n");
-		ASC_PRT_NEXT();
+		seq_printf(m, "\n");
 	}
-
-	return totlen;
 }
 
 /*
  * asc_prt_driver_conf()
- *
- * Note: no single line should be greater than ASC_PRTLINE_SIZE,
- * cf. asc_prt_line().
- *
- * Return the number of characters copied into 'cp'. No more than
- * 'cplen' characters will be copied to 'cp'.
  */
-static int asc_prt_driver_conf(struct Scsi_Host *shost, char *cp, int cplen)
+static void asc_prt_driver_conf(struct seq_file *m, struct Scsi_Host *shost)
 {
 	struct asc_board *boardp = shost_priv(shost);
-	int leftlen;
-	int totlen;
-	int len;
 	int chip_scsi_id;
 
-	leftlen = cplen;
-	totlen = len = 0;
+	seq_printf(m,
+		"\nLinux Driver Configuration and Information for AdvanSys SCSI Host %d:\n",
+		shost->host_no);
 
-	len = asc_prt_line(cp, leftlen,
-			   "\nLinux Driver Configuration and Information for AdvanSys SCSI Host %d:\n",
-			   shost->host_no);
-	ASC_PRT_NEXT();
+	seq_printf(m,
+		   " host_busy %u, last_reset %lu, max_id %u, max_lun %u, max_channel %u\n",
+		   shost->host_busy, shost->last_reset, shost->max_id,
+		   shost->max_lun, shost->max_channel);
 
-	len = asc_prt_line(cp, leftlen,
-			   " host_busy %u, last_reset %u, max_id %u, max_lun %u, max_channel %u\n",
-			   shost->host_busy, shost->last_reset, shost->max_id,
-			   shost->max_lun, shost->max_channel);
-	ASC_PRT_NEXT();
+	seq_printf(m,
+		   " unique_id %d, can_queue %d, this_id %d, sg_tablesize %u, cmd_per_lun %u\n",
+		   shost->unique_id, shost->can_queue, shost->this_id,
+		   shost->sg_tablesize, shost->cmd_per_lun);
 
-	len = asc_prt_line(cp, leftlen,
-			   " unique_id %d, can_queue %d, this_id %d, sg_tablesize %u, cmd_per_lun %u\n",
-			   shost->unique_id, shost->can_queue, shost->this_id,
-			   shost->sg_tablesize, shost->cmd_per_lun);
-	ASC_PRT_NEXT();
+	seq_printf(m,
+		   " unchecked_isa_dma %d, use_clustering %d\n",
+		   shost->unchecked_isa_dma, shost->use_clustering);
 
-	len = asc_prt_line(cp, leftlen,
-			   " unchecked_isa_dma %d, use_clustering %d\n",
-			   shost->unchecked_isa_dma, shost->use_clustering);
-	ASC_PRT_NEXT();
+	seq_printf(m,
+		   " flags 0x%x, last_reset 0x%lx, jiffies 0x%lx, asc_n_io_port 0x%x\n",
+		   boardp->flags, boardp->last_reset, jiffies,
+		   boardp->asc_n_io_port);
 
-	len = asc_prt_line(cp, leftlen,
-			   " flags 0x%x, last_reset 0x%x, jiffies 0x%x, asc_n_io_port 0x%x\n",
-			   boardp->flags, boardp->last_reset, jiffies,
-			   boardp->asc_n_io_port);
-	ASC_PRT_NEXT();
-
-	len = asc_prt_line(cp, leftlen, " io_port 0x%x\n", shost->io_port);
-	ASC_PRT_NEXT();
+	seq_printf(m, " io_port 0x%lx\n", shost->io_port);
 
 	if (ASC_NARROW_BOARD(boardp)) {
 		chip_scsi_id = boardp->dvc_cfg.asc_dvc_cfg.chip_scsi_id;
 	} else {
 		chip_scsi_id = boardp->dvc_var.adv_dvc_var.chip_scsi_id;
 	}
-
-	return totlen;
 }
 
 /*
  * asc_prt_asc_board_info()
  *
  * Print dynamic board configuration information.
- *
- * Note: no single line should be greater than ASC_PRTLINE_SIZE,
- * cf. asc_prt_line().
- *
- * Return the number of characters copied into 'cp'. No more than
- * 'cplen' characters will be copied to 'cp'.
  */
-static int asc_prt_asc_board_info(struct Scsi_Host *shost, char *cp, int cplen)
+static void asc_prt_asc_board_info(struct seq_file *m, struct Scsi_Host *shost)
 {
 	struct asc_board *boardp = shost_priv(shost);
 	int chip_scsi_id;
-	int leftlen;
-	int totlen;
-	int len;
 	ASC_DVC_VAR *v;
 	ASC_DVC_CFG *c;
 	int i;
@@ -3619,105 +3390,79 @@ static int asc_prt_asc_board_info(struct Scsi_Host *shost, char *cp, int cplen)
 	c = &boardp->dvc_cfg.asc_dvc_cfg;
 	chip_scsi_id = c->chip_scsi_id;
 
-	leftlen = cplen;
-	totlen = len = 0;
+	seq_printf(m,
+		   "\nAsc Library Configuration and Statistics for AdvanSys SCSI Host %d:\n",
+		   shost->host_no);
 
-	len = asc_prt_line(cp, leftlen,
-			   "\nAsc Library Configuration and Statistics for AdvanSys SCSI Host %d:\n",
-			   shost->host_no);
-	ASC_PRT_NEXT();
-
-	len = asc_prt_line(cp, leftlen, " chip_version %u, mcode_date 0x%x, "
-			   "mcode_version 0x%x, err_code %u\n",
-			   c->chip_version, c->mcode_date, c->mcode_version,
-			   v->err_code);
-	ASC_PRT_NEXT();
+	seq_printf(m, " chip_version %u, mcode_date 0x%x, "
+		   "mcode_version 0x%x, err_code %u\n",
+		   c->chip_version, c->mcode_date, c->mcode_version,
+		   v->err_code);
 
 	/* Current number of commands waiting for the host. */
-	len = asc_prt_line(cp, leftlen,
-			   " Total Command Pending: %d\n", v->cur_total_qng);
-	ASC_PRT_NEXT();
+	seq_printf(m,
+		   " Total Command Pending: %d\n", v->cur_total_qng);
 
-	len = asc_prt_line(cp, leftlen, " Command Queuing:");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Command Queuing:");
 	for (i = 0; i <= ASC_MAX_TID; i++) {
 		if ((chip_scsi_id == i) ||
 		    ((boardp->init_tidmask & ADV_TID_TO_TIDMASK(i)) == 0)) {
 			continue;
 		}
-		len = asc_prt_line(cp, leftlen, " %X:%c",
-				   i,
-				   (v->
-				    use_tagged_qng & ADV_TID_TO_TIDMASK(i)) ?
-				   'Y' : 'N');
-		ASC_PRT_NEXT();
+		seq_printf(m, " %X:%c",
+			   i,
+			   (v->use_tagged_qng & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
 	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, "\n");
 
 	/* Current number of commands waiting for a device. */
-	len = asc_prt_line(cp, leftlen, " Command Queue Pending:");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Command Queue Pending:");
 	for (i = 0; i <= ASC_MAX_TID; i++) {
 		if ((chip_scsi_id == i) ||
 		    ((boardp->init_tidmask & ADV_TID_TO_TIDMASK(i)) == 0)) {
 			continue;
 		}
-		len = asc_prt_line(cp, leftlen, " %X:%u", i, v->cur_dvc_qng[i]);
-		ASC_PRT_NEXT();
+		seq_printf(m, " %X:%u", i, v->cur_dvc_qng[i]);
 	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, "\n");
 
 	/* Current limit on number of commands that can be sent to a device. */
-	len = asc_prt_line(cp, leftlen, " Command Queue Limit:");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Command Queue Limit:");
 	for (i = 0; i <= ASC_MAX_TID; i++) {
 		if ((chip_scsi_id == i) ||
 		    ((boardp->init_tidmask & ADV_TID_TO_TIDMASK(i)) == 0)) {
 			continue;
 		}
-		len = asc_prt_line(cp, leftlen, " %X:%u", i, v->max_dvc_qng[i]);
-		ASC_PRT_NEXT();
+		seq_printf(m, " %X:%u", i, v->max_dvc_qng[i]);
 	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, "\n");
 
 	/* Indicate whether the device has returned queue full status. */
-	len = asc_prt_line(cp, leftlen, " Command Queue Full:");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Command Queue Full:");
 	for (i = 0; i <= ASC_MAX_TID; i++) {
 		if ((chip_scsi_id == i) ||
 		    ((boardp->init_tidmask & ADV_TID_TO_TIDMASK(i)) == 0)) {
 			continue;
 		}
-		if (boardp->queue_full & ADV_TID_TO_TIDMASK(i)) {
-			len = asc_prt_line(cp, leftlen, " %X:Y-%d",
-					   i, boardp->queue_full_cnt[i]);
-		} else {
-			len = asc_prt_line(cp, leftlen, " %X:N", i);
-		}
-		ASC_PRT_NEXT();
+		if (boardp->queue_full & ADV_TID_TO_TIDMASK(i))
+			seq_printf(m, " %X:Y-%d",
+				   i, boardp->queue_full_cnt[i]);
+		else
+			seq_printf(m, " %X:N", i);
 	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, "\n");
 
-	len = asc_prt_line(cp, leftlen, " Synchronous Transfer:");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Synchronous Transfer:");
 	for (i = 0; i <= ASC_MAX_TID; i++) {
 		if ((chip_scsi_id == i) ||
 		    ((boardp->init_tidmask & ADV_TID_TO_TIDMASK(i)) == 0)) {
 			continue;
 		}
-		len = asc_prt_line(cp, leftlen, " %X:%c",
-				   i,
-				   (v->
-				    sdtr_done & ADV_TID_TO_TIDMASK(i)) ? 'Y' :
-				   'N');
-		ASC_PRT_NEXT();
+		seq_printf(m, " %X:%c",
+			   i,
+			   (v->sdtr_done & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
 	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, "\n");
 
 	for (i = 0; i <= ASC_MAX_TID; i++) {
 		uchar syn_period_ix;
@@ -3728,69 +3473,48 @@ static int asc_prt_asc_board_info(struct Scsi_Host *shost, char *cp, int cplen)
 			continue;
 		}
 
-		len = asc_prt_line(cp, leftlen, "  %X:", i);
-		ASC_PRT_NEXT();
+		seq_printf(m, "  %X:", i);
 
 		if ((boardp->sdtr_data[i] & ASC_SYN_MAX_OFFSET) == 0) {
-			len = asc_prt_line(cp, leftlen, " Asynchronous");
-			ASC_PRT_NEXT();
+			seq_printf(m, " Asynchronous");
 		} else {
 			syn_period_ix =
 			    (boardp->sdtr_data[i] >> 4) & (v->max_sdtr_index -
 							   1);
 
-			len = asc_prt_line(cp, leftlen,
-					   " Transfer Period Factor: %d (%d.%d Mhz),",
-					   v->sdtr_period_tbl[syn_period_ix],
-					   250 /
-					   v->sdtr_period_tbl[syn_period_ix],
-					   ASC_TENTHS(250,
-						      v->
-						      sdtr_period_tbl
-						      [syn_period_ix]));
-			ASC_PRT_NEXT();
+			seq_printf(m,
+				   " Transfer Period Factor: %d (%d.%d Mhz),",
+				   v->sdtr_period_tbl[syn_period_ix],
+				   250 / v->sdtr_period_tbl[syn_period_ix],
+				   ASC_TENTHS(250,
+					      v->sdtr_period_tbl[syn_period_ix]));
 
-			len = asc_prt_line(cp, leftlen, " REQ/ACK Offset: %d",
-					   boardp->
-					   sdtr_data[i] & ASC_SYN_MAX_OFFSET);
-			ASC_PRT_NEXT();
+			seq_printf(m, " REQ/ACK Offset: %d",
+				   boardp->sdtr_data[i] & ASC_SYN_MAX_OFFSET);
 		}
 
 		if ((v->sdtr_done & ADV_TID_TO_TIDMASK(i)) == 0) {
-			len = asc_prt_line(cp, leftlen, "*\n");
+			seq_printf(m, "*\n");
 			renegotiate = 1;
 		} else {
-			len = asc_prt_line(cp, leftlen, "\n");
+			seq_printf(m, "\n");
 		}
-		ASC_PRT_NEXT();
 	}
 
 	if (renegotiate) {
-		len = asc_prt_line(cp, leftlen,
-				   " * = Re-negotiation pending before next command.\n");
-		ASC_PRT_NEXT();
+		seq_printf(m,
+			   " * = Re-negotiation pending before next command.\n");
 	}
-
-	return totlen;
 }
 
 /*
  * asc_prt_adv_board_info()
  *
  * Print dynamic board configuration information.
- *
- * Note: no single line should be greater than ASC_PRTLINE_SIZE,
- * cf. asc_prt_line().
- *
- * Return the number of characters copied into 'cp'. No more than
- * 'cplen' characters will be copied to 'cp'.
  */
-static int asc_prt_adv_board_info(struct Scsi_Host *shost, char *cp, int cplen)
+static void asc_prt_adv_board_info(struct seq_file *m, struct Scsi_Host *shost)
 {
 	struct asc_board *boardp = shost_priv(shost);
-	int leftlen;
-	int totlen;
-	int len;
 	int i;
 	ADV_DVC_VAR *v;
 	ADV_DVC_CFG *c;
@@ -3809,47 +3533,35 @@ static int asc_prt_adv_board_info(struct Scsi_Host *shost, char *cp, int cplen)
 	iop_base = v->iop_base;
 	chip_scsi_id = v->chip_scsi_id;
 
-	leftlen = cplen;
-	totlen = len = 0;
-
-	len = asc_prt_line(cp, leftlen,
-			   "\nAdv Library Configuration and Statistics for AdvanSys SCSI Host %d:\n",
-			   shost->host_no);
-	ASC_PRT_NEXT();
+	seq_printf(m,
+		   "\nAdv Library Configuration and Statistics for AdvanSys SCSI Host %d:\n",
+		   shost->host_no);
 
-	len = asc_prt_line(cp, leftlen,
-			   " iop_base 0x%lx, cable_detect: %X, err_code %u\n",
-			   v->iop_base,
-			   AdvReadWordRegister(iop_base,
-					       IOPW_SCSI_CFG1) & CABLE_DETECT,
-			   v->err_code);
-	ASC_PRT_NEXT();
+	seq_printf(m,
+		   " iop_base 0x%lx, cable_detect: %X, err_code %u\n",
+		   (unsigned long)v->iop_base,
+		   AdvReadWordRegister(iop_base,IOPW_SCSI_CFG1) & CABLE_DETECT,
+		   v->err_code);
 
-	len = asc_prt_line(cp, leftlen, " chip_version %u, mcode_date 0x%x, "
-			   "mcode_version 0x%x\n", c->chip_version,
-			   c->mcode_date, c->mcode_version);
-	ASC_PRT_NEXT();
+	seq_printf(m, " chip_version %u, mcode_date 0x%x, "
+		   "mcode_version 0x%x\n", c->chip_version,
+		   c->mcode_date, c->mcode_version);
 
 	AdvReadWordLram(iop_base, ASC_MC_TAGQNG_ABLE, tagqng_able);
-	len = asc_prt_line(cp, leftlen, " Queuing Enabled:");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Queuing Enabled:");
 	for (i = 0; i <= ADV_MAX_TID; i++) {
 		if ((chip_scsi_id == i) ||
 		    ((boardp->init_tidmask & ADV_TID_TO_TIDMASK(i)) == 0)) {
 			continue;
 		}
 
-		len = asc_prt_line(cp, leftlen, " %X:%c",
-				   i,
-				   (tagqng_able & ADV_TID_TO_TIDMASK(i)) ? 'Y' :
-				   'N');
-		ASC_PRT_NEXT();
+		seq_printf(m, " %X:%c",
+			   i,
+			   (tagqng_able & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
 	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, "\n");
 
-	len = asc_prt_line(cp, leftlen, " Queue Limit:");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Queue Limit:");
 	for (i = 0; i <= ADV_MAX_TID; i++) {
 		if ((chip_scsi_id == i) ||
 		    ((boardp->init_tidmask & ADV_TID_TO_TIDMASK(i)) == 0)) {
@@ -3859,14 +3571,11 @@ static int asc_prt_adv_board_info(struct Scsi_Host *shost, char *cp, int cplen)
 		AdvReadByteLram(iop_base, ASC_MC_NUMBER_OF_MAX_CMD + i,
 				lrambyte);
 
-		len = asc_prt_line(cp, leftlen, " %X:%d", i, lrambyte);
-		ASC_PRT_NEXT();
+		seq_printf(m, " %X:%d", i, lrambyte);
 	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, "\n");
 
-	len = asc_prt_line(cp, leftlen, " Command Pending:");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Command Pending:");
 	for (i = 0; i <= ADV_MAX_TID; i++) {
 		if ((chip_scsi_id == i) ||
 		    ((boardp->init_tidmask & ADV_TID_TO_TIDMASK(i)) == 0)) {
@@ -3876,33 +3585,26 @@ static int asc_prt_adv_board_info(struct Scsi_Host *shost, char *cp, int cplen)
 		AdvReadByteLram(iop_base, ASC_MC_NUMBER_OF_QUEUED_CMD + i,
 				lrambyte);
 
-		len = asc_prt_line(cp, leftlen, " %X:%d", i, lrambyte);
-		ASC_PRT_NEXT();
+		seq_printf(m, " %X:%d", i, lrambyte);
 	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, "\n");
 
 	AdvReadWordLram(iop_base, ASC_MC_WDTR_ABLE, wdtr_able);
-	len = asc_prt_line(cp, leftlen, " Wide Enabled:");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Wide Enabled:");
 	for (i = 0; i <= ADV_MAX_TID; i++) {
 		if ((chip_scsi_id == i) ||
 		    ((boardp->init_tidmask & ADV_TID_TO_TIDMASK(i)) == 0)) {
 			continue;
 		}
 
-		len = asc_prt_line(cp, leftlen, " %X:%c",
-				   i,
-				   (wdtr_able & ADV_TID_TO_TIDMASK(i)) ? 'Y' :
-				   'N');
-		ASC_PRT_NEXT();
+		seq_printf(m, " %X:%c",
+			   i,
+			   (wdtr_able & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
 	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, "\n");
 
 	AdvReadWordLram(iop_base, ASC_MC_WDTR_DONE, wdtr_done);
-	len = asc_prt_line(cp, leftlen, " Transfer Bit Width:");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Transfer Bit Width:");
 	for (i = 0; i <= ADV_MAX_TID; i++) {
 		if ((chip_scsi_id == i) ||
 		    ((boardp->init_tidmask & ADV_TID_TO_TIDMASK(i)) == 0)) {
@@ -3913,37 +3615,30 @@ static int asc_prt_adv_board_info(struct Scsi_Host *shost, char *cp, int cplen)
 				ASC_MC_DEVICE_HSHK_CFG_TABLE + (2 * i),
 				lramword);
 
-		len = asc_prt_line(cp, leftlen, " %X:%d",
-				   i, (lramword & 0x8000) ? 16 : 8);
-		ASC_PRT_NEXT();
+		seq_printf(m, " %X:%d",
+			   i, (lramword & 0x8000) ? 16 : 8);
 
 		if ((wdtr_able & ADV_TID_TO_TIDMASK(i)) &&
 		    (wdtr_done & ADV_TID_TO_TIDMASK(i)) == 0) {
-			len = asc_prt_line(cp, leftlen, "*");
-			ASC_PRT_NEXT();
+			seq_printf(m, "*");
 			renegotiate = 1;
 		}
 	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, "\n");
 
 	AdvReadWordLram(iop_base, ASC_MC_SDTR_ABLE, sdtr_able);
-	len = asc_prt_line(cp, leftlen, " Synchronous Enabled:");
-	ASC_PRT_NEXT();
+	seq_printf(m, " Synchronous Enabled:");
 	for (i = 0; i <= ADV_MAX_TID; i++) {
 		if ((chip_scsi_id == i) ||
 		    ((boardp->init_tidmask & ADV_TID_TO_TIDMASK(i)) == 0)) {
 			continue;
 		}
 
-		len = asc_prt_line(cp, leftlen, " %X:%c",
-				   i,
-				   (sdtr_able & ADV_TID_TO_TIDMASK(i)) ? 'Y' :
-				   'N');
-		ASC_PRT_NEXT();
+		seq_printf(m, " %X:%c",
+			   i,
+			   (sdtr_able & ADV_TID_TO_TIDMASK(i)) ? 'Y' : 'N');
 	}
-	len = asc_prt_line(cp, leftlen, "\n");
-	ASC_PRT_NEXT();
+	seq_printf(m, "\n");
 
 	AdvReadWordLram(iop_base, ASC_MC_SDTR_DONE, sdtr_done);
 	for (i = 0; i <= ADV_MAX_TID; i++) {
@@ -3959,358 +3654,170 @@ static int asc_prt_adv_board_info(struct Scsi_Host *shost, char *cp, int cplen)
 			continue;
 		}
 
-		len = asc_prt_line(cp, leftlen, "  %X:", i);
-		ASC_PRT_NEXT();
+		seq_printf(m, "  %X:", i);
 
 		if ((lramword & 0x1F) == 0) {	/* Check for REQ/ACK Offset 0. */
-			len = asc_prt_line(cp, leftlen, " Asynchronous");
-			ASC_PRT_NEXT();
+			seq_printf(m, " Asynchronous");
 		} else {
-			len =
-			    asc_prt_line(cp, leftlen,
-					 " Transfer Period Factor: ");
-			ASC_PRT_NEXT();
+			seq_printf(m, " Transfer Period Factor: ");
 
 			if ((lramword & 0x1F00) == 0x1100) {	/* 80 Mhz */
-				len =
-				    asc_prt_line(cp, leftlen, "9 (80.0 Mhz),");
-				ASC_PRT_NEXT();
+				seq_printf(m, "9 (80.0 Mhz),");
 			} else if ((lramword & 0x1F00) == 0x1000) {	/* 40 Mhz */
-				len =
-				    asc_prt_line(cp, leftlen, "10 (40.0 Mhz),");
-				ASC_PRT_NEXT();
+				seq_printf(m, "10 (40.0 Mhz),");
 			} else {	/* 20 Mhz or below. */
 
 				period = (((lramword >> 8) * 25) + 50) / 4;
 
 				if (period == 0) {	/* Should never happen. */
-					len =
-					    asc_prt_line(cp, leftlen,
-							 "%d (? Mhz), ");
-					ASC_PRT_NEXT();
+					seq_printf(m, "%d (? Mhz), ", period);
 				} else {
-					len = asc_prt_line(cp, leftlen,
-							   "%d (%d.%d Mhz),",
-							   period, 250 / period,
-							   ASC_TENTHS(250,
-								      period));
-					ASC_PRT_NEXT();
+					seq_printf(m,
+						   "%d (%d.%d Mhz),",
+						   period, 250 / period,
+						   ASC_TENTHS(250, period));
 				}
 			}
 
-			len = asc_prt_line(cp, leftlen, " REQ/ACK Offset: %d",
-					   lramword & 0x1F);
-			ASC_PRT_NEXT();
+			seq_printf(m, " REQ/ACK Offset: %d",
+				   lramword & 0x1F);
 		}
 
 		if ((sdtr_done & ADV_TID_TO_TIDMASK(i)) == 0) {
-			len = asc_prt_line(cp, leftlen, "*\n");
+			seq_printf(m, "*\n");
 			renegotiate = 1;
 		} else {
-			len = asc_prt_line(cp, leftlen, "\n");
+			seq_printf(m, "\n");
 		}
-		ASC_PRT_NEXT();
 	}
 
 	if (renegotiate) {
-		len = asc_prt_line(cp, leftlen,
-				   " * = Re-negotiation pending before next command.\n");
-		ASC_PRT_NEXT();
+		seq_printf(m,
+			   " * = Re-negotiation pending before next command.\n");
 	}
-
-	return totlen;
-}
-
-/*
- * asc_proc_copy()
- *
- * Copy proc information to a read buffer taking into account the current
- * read offset in the file and the remaining space in the read buffer.
- */
-static int
-asc_proc_copy(off_t advoffset, off_t offset, char *curbuf, int leftlen,
-	      char *cp, int cplen)
-{
-	int cnt = 0;
-
-	ASC_DBG(2, "offset %d, advoffset %d, cplen %d\n",
-		 (unsigned)offset, (unsigned)advoffset, cplen);
-	if (offset <= advoffset) {
-		/* Read offset below current offset, copy everything. */
-		cnt = min(cplen, leftlen);
-		ASC_DBG(2, "curbuf 0x%lx, cp 0x%lx, cnt %d\n",
-			 (ulong)curbuf, (ulong)cp, cnt);
-		memcpy(curbuf, cp, cnt);
-	} else if (offset < advoffset + cplen) {
-		/* Read offset within current range, partial copy. */
-		cnt = (advoffset + cplen) - offset;
-		cp = (cp + cplen) - cnt;
-		cnt = min(cnt, leftlen);
-		ASC_DBG(2, "curbuf 0x%lx, cp 0x%lx, cnt %d\n",
-			 (ulong)curbuf, (ulong)cp, cnt);
-		memcpy(curbuf, cp, cnt);
-	}
-	return cnt;
 }
 
 #ifdef ADVANSYS_STATS
 /*
  * asc_prt_board_stats()
- *
- * Note: no single line should be greater than ASC_PRTLINE_SIZE,
- * cf. asc_prt_line().
- *
- * Return the number of characters copied into 'cp'. No more than
- * 'cplen' characters will be copied to 'cp'.
  */
-static int asc_prt_board_stats(struct Scsi_Host *shost, char *cp, int cplen)
+static void asc_prt_board_stats(struct seq_file *m, struct Scsi_Host *shost)
 {
 	struct asc_board *boardp = shost_priv(shost);
 	struct asc_stats *s = &boardp->asc_stats;
 
-	int leftlen = cplen;
-	int len, totlen = 0;
+	seq_printf(m,
+		   "\nLinux Driver Statistics for AdvanSys SCSI Host %d:\n",
+		   shost->host_no);
 
-	len = asc_prt_line(cp, leftlen,
-			   "\nLinux Driver Statistics for AdvanSys SCSI Host %d:\n",
-			   shost->host_no);
-	ASC_PRT_NEXT();
+	seq_printf(m,
+		   " queuecommand %u, reset %u, biosparam %u, interrupt %u\n",
+		   s->queuecommand, s->reset, s->biosparam,
+		   s->interrupt);
 
-	len = asc_prt_line(cp, leftlen,
-			   " queuecommand %lu, reset %lu, biosparam %lu, interrupt %lu\n",
-			   s->queuecommand, s->reset, s->biosparam,
-			   s->interrupt);
-	ASC_PRT_NEXT();
+	seq_printf(m,
+		   " callback %u, done %u, build_error %u, build_noreq %u, build_nosg %u\n",
+		   s->callback, s->done, s->build_error,
+		   s->adv_build_noreq, s->adv_build_nosg);
 
-	len = asc_prt_line(cp, leftlen,
-			   " callback %lu, done %lu, build_error %lu, build_noreq %lu, build_nosg %lu\n",
-			   s->callback, s->done, s->build_error,
-			   s->adv_build_noreq, s->adv_build_nosg);
-	ASC_PRT_NEXT();
-
-	len = asc_prt_line(cp, leftlen,
-			   " exe_noerror %lu, exe_busy %lu, exe_error %lu, exe_unknown %lu\n",
-			   s->exe_noerror, s->exe_busy, s->exe_error,
-			   s->exe_unknown);
-	ASC_PRT_NEXT();
+	seq_printf(m,
+		   " exe_noerror %u, exe_busy %u, exe_error %u, exe_unknown %u\n",
+		   s->exe_noerror, s->exe_busy, s->exe_error,
+		   s->exe_unknown);
 
 	/*
 	 * Display data transfer statistics.
 	 */
 	if (s->xfer_cnt > 0) {
-		len = asc_prt_line(cp, leftlen, " xfer_cnt %lu, xfer_elem %lu, ",
-				   s->xfer_cnt, s->xfer_elem);
-		ASC_PRT_NEXT();
+		seq_printf(m, " xfer_cnt %u, xfer_elem %u, ",
+			   s->xfer_cnt, s->xfer_elem);
 
-		len = asc_prt_line(cp, leftlen, "xfer_bytes %lu.%01lu kb\n",
-				   s->xfer_sect / 2, ASC_TENTHS(s->xfer_sect, 2));
-		ASC_PRT_NEXT();
+		seq_printf(m, "xfer_bytes %u.%01u kb\n",
+			   s->xfer_sect / 2, ASC_TENTHS(s->xfer_sect, 2));
 
 		/* Scatter gather transfer statistics */
-		len = asc_prt_line(cp, leftlen, " avg_num_elem %lu.%01lu, ",
-				   s->xfer_elem / s->xfer_cnt,
-				   ASC_TENTHS(s->xfer_elem, s->xfer_cnt));
-		ASC_PRT_NEXT();
+		seq_printf(m, " avg_num_elem %u.%01u, ",
+			   s->xfer_elem / s->xfer_cnt,
+			   ASC_TENTHS(s->xfer_elem, s->xfer_cnt));
 
-		len = asc_prt_line(cp, leftlen, "avg_elem_size %lu.%01lu kb, ",
-				   (s->xfer_sect / 2) / s->xfer_elem,
-				   ASC_TENTHS((s->xfer_sect / 2), s->xfer_elem));
-		ASC_PRT_NEXT();
+		seq_printf(m, "avg_elem_size %u.%01u kb, ",
+			   (s->xfer_sect / 2) / s->xfer_elem,
+			   ASC_TENTHS((s->xfer_sect / 2), s->xfer_elem));
 
-		len = asc_prt_line(cp, leftlen, "avg_xfer_size %lu.%01lu kb\n",
-				   (s->xfer_sect / 2) / s->xfer_cnt,
-				   ASC_TENTHS((s->xfer_sect / 2), s->xfer_cnt));
-		ASC_PRT_NEXT();
+		seq_printf(m, "avg_xfer_size %u.%01u kb\n",
+			   (s->xfer_sect / 2) / s->xfer_cnt,
+			   ASC_TENTHS((s->xfer_sect / 2), s->xfer_cnt));
 	}
-
-	return totlen;
 }
 #endif /* ADVANSYS_STATS */
 
 /*
- * advansys_proc_info() - /proc/scsi/advansys/{0,1,2,3,...}
+ * advansys_show_info() - /proc/scsi/advansys/{0,1,2,3,...}
  *
- * *buffer: I/O buffer
- * **start: if inout == FALSE pointer into buffer where user read should start
- * offset: current offset into a /proc/scsi/advansys/[0...] file
- * length: length of buffer
- * hostno: Scsi_Host host_no
- * inout: TRUE - user is writing; FALSE - user is reading
+ * m: seq_file to print into
+ * shost: Scsi_Host
  *
  * Return the number of bytes read from or written to a
  * /proc/scsi/advansys/[0...] file.
- *
- * Note: This function uses the per board buffer 'prtbuf' which is
- * allocated when the board is initialized in advansys_detect(). The
- * buffer is ASC_PRTBUF_SIZE bytes. The function asc_proc_copy() is
- * used to write to the buffer. The way asc_proc_copy() is written
- * if 'prtbuf' is too small it will not be overwritten. Instead the
- * user just won't get all the available statistics.
  */
 static int
-advansys_proc_info(struct Scsi_Host *shost, char *buffer, char **start,
-		   off_t offset, int length, int inout)
+advansys_show_info(struct seq_file *m, struct Scsi_Host *shost)
 {
 	struct asc_board *boardp = shost_priv(shost);
-	char *cp;
-	int cplen;
-	int cnt;
-	int totcnt;
-	int leftlen;
-	char *curbuf;
-	off_t advoffset;
 
 	ASC_DBG(1, "begin\n");
 
 	/*
-	 * User write not supported.
-	 */
-	if (inout == TRUE)
-		return -ENOSYS;
-
-	/*
 	 * User read of /proc/scsi/advansys/[0...] file.
 	 */
 
-	/* Copy read data starting at the beginning of the buffer. */
-	*start = buffer;
-	curbuf = buffer;
-	advoffset = 0;
-	totcnt = 0;
-	leftlen = length;
-
 	/*
 	 * Get board configuration information.
 	 *
 	 * advansys_info() returns the board string from its own static buffer.
 	 */
-	cp = (char *)advansys_info(shost);
-	strcat(cp, "\n");
-	cplen = strlen(cp);
 	/* Copy board information. */
-	cnt = asc_proc_copy(advoffset, offset, curbuf, leftlen, cp, cplen);
-	totcnt += cnt;
-	leftlen -= cnt;
-	if (leftlen == 0) {
-		ASC_DBG(1, "totcnt %d\n", totcnt);
-		return totcnt;
-	}
-	advoffset += cplen;
-	curbuf += cnt;
-
+	seq_printf(m, "%s\n", (char *)advansys_info(shost));
 	/*
 	 * Display Wide Board BIOS Information.
 	 */
-	if (!ASC_NARROW_BOARD(boardp)) {
-		cp = boardp->prtbuf;
-		cplen = asc_prt_adv_bios(shost, cp, ASC_PRTBUF_SIZE);
-		BUG_ON(cplen >= ASC_PRTBUF_SIZE);
-		cnt = asc_proc_copy(advoffset, offset, curbuf, leftlen, cp,
-				  cplen);
-		totcnt += cnt;
-		leftlen -= cnt;
-		if (leftlen == 0) {
-			ASC_DBG(1, "totcnt %d\n", totcnt);
-			return totcnt;
-		}
-		advoffset += cplen;
-		curbuf += cnt;
-	}
+	if (!ASC_NARROW_BOARD(boardp))
+		asc_prt_adv_bios(m, shost);
 
 	/*
 	 * Display driver information for each device attached to the board.
 	 */
-	cp = boardp->prtbuf;
-	cplen = asc_prt_board_devices(shost, cp, ASC_PRTBUF_SIZE);
-	BUG_ON(cplen >= ASC_PRTBUF_SIZE);
-	cnt = asc_proc_copy(advoffset, offset, curbuf, leftlen, cp, cplen);
-	totcnt += cnt;
-	leftlen -= cnt;
-	if (leftlen == 0) {
-		ASC_DBG(1, "totcnt %d\n", totcnt);
-		return totcnt;
-	}
-	advoffset += cplen;
-	curbuf += cnt;
+	asc_prt_board_devices(m, shost);
 
 	/*
 	 * Display EEPROM configuration for the board.
 	 */
-	cp = boardp->prtbuf;
-	if (ASC_NARROW_BOARD(boardp)) {
-		cplen = asc_prt_asc_board_eeprom(shost, cp, ASC_PRTBUF_SIZE);
-	} else {
-		cplen = asc_prt_adv_board_eeprom(shost, cp, ASC_PRTBUF_SIZE);
-	}
-	BUG_ON(cplen >= ASC_PRTBUF_SIZE);
-	cnt = asc_proc_copy(advoffset, offset, curbuf, leftlen, cp, cplen);
-	totcnt += cnt;
-	leftlen -= cnt;
-	if (leftlen == 0) {
-		ASC_DBG(1, "totcnt %d\n", totcnt);
-		return totcnt;
-	}
-	advoffset += cplen;
-	curbuf += cnt;
+	if (ASC_NARROW_BOARD(boardp))
+		asc_prt_asc_board_eeprom(m, shost);
+	else
+		asc_prt_adv_board_eeprom(m, shost);
 
 	/*
 	 * Display driver configuration and information for the board.
 	 */
-	cp = boardp->prtbuf;
-	cplen = asc_prt_driver_conf(shost, cp, ASC_PRTBUF_SIZE);
-	BUG_ON(cplen >= ASC_PRTBUF_SIZE);
-	cnt = asc_proc_copy(advoffset, offset, curbuf, leftlen, cp, cplen);
-	totcnt += cnt;
-	leftlen -= cnt;
-	if (leftlen == 0) {
-		ASC_DBG(1, "totcnt %d\n", totcnt);
-		return totcnt;
-	}
-	advoffset += cplen;
-	curbuf += cnt;
+	asc_prt_driver_conf(m, shost);
 
 #ifdef ADVANSYS_STATS
 	/*
 	 * Display driver statistics for the board.
 	 */
-	cp = boardp->prtbuf;
-	cplen = asc_prt_board_stats(shost, cp, ASC_PRTBUF_SIZE);
-	BUG_ON(cplen >= ASC_PRTBUF_SIZE);
-	cnt = asc_proc_copy(advoffset, offset, curbuf, leftlen, cp, cplen);
-	totcnt += cnt;
-	leftlen -= cnt;
-	if (leftlen == 0) {
-		ASC_DBG(1, "totcnt %d\n", totcnt);
-		return totcnt;
-	}
-	advoffset += cplen;
-	curbuf += cnt;
+	asc_prt_board_stats(m, shost);
 #endif /* ADVANSYS_STATS */
 
 	/*
 	 * Display Asc Library dynamic configuration information
 	 * for the board.
 	 */
-	cp = boardp->prtbuf;
-	if (ASC_NARROW_BOARD(boardp)) {
-		cplen = asc_prt_asc_board_info(shost, cp, ASC_PRTBUF_SIZE);
-	} else {
-		cplen = asc_prt_adv_board_info(shost, cp, ASC_PRTBUF_SIZE);
-	}
-	BUG_ON(cplen >= ASC_PRTBUF_SIZE);
-	cnt = asc_proc_copy(advoffset, offset, curbuf, leftlen, cp, cplen);
-	totcnt += cnt;
-	leftlen -= cnt;
-	if (leftlen == 0) {
-		ASC_DBG(1, "totcnt %d\n", totcnt);
-		return totcnt;
-	}
-	advoffset += cplen;
-	curbuf += cnt;
-
-	ASC_DBG(1, "totcnt %d\n", totcnt);
-
-	return totcnt;
+	if (ASC_NARROW_BOARD(boardp))
+		asc_prt_asc_board_info(m, shost);
+	else
+		asc_prt_adv_board_info(m, shost);
+	return 0;
 }
 #endif /* CONFIG_PROC_FS */
 
@@ -11743,7 +11250,7 @@ static int AdvInitGetConfig(struct pci_dev *pdev, struct Scsi_Host *shost)
 static struct scsi_host_template advansys_template = {
 	.proc_name = DRV_NAME,
 #ifdef CONFIG_PROC_FS
-	.proc_info = advansys_proc_info,
+	.show_info = advansys_show_info,
 #endif
 	.name = DRV_NAME,
 	.info = advansys_info,
@@ -11939,20 +11446,6 @@ static int advansys_board_found(struct Scsi_Host *shost, unsigned int iop,
 #endif /* CONFIG_PCI */
 	}
 
-#ifdef CONFIG_PROC_FS
-	/*
-	 * Allocate buffer for printing information from
-	 * /proc/scsi/advansys/[0...].
-	 */
-	boardp->prtbuf = kmalloc(ASC_PRTBUF_SIZE, GFP_KERNEL);
-	if (!boardp->prtbuf) {
-		shost_printk(KERN_ERR, shost, "kmalloc(%d) returned NULL\n",
-				ASC_PRTBUF_SIZE);
-		ret = -ENOMEM;
-		goto err_unmap;
-	}
-#endif /* CONFIG_PROC_FS */
-
 	if (ASC_NARROW_BOARD(boardp)) {
 		/*
 		 * Set the board bus type and PCI IRQ before
@@ -12010,7 +11503,7 @@ static int advansys_board_found(struct Scsi_Host *shost, unsigned int iop,
 	}
 
 	if (ret)
-		goto err_free_proc;
+		goto err_unmap;
 
 	/*
 	 * Save the EEPROM configuration so that it can be displayed
@@ -12055,7 +11548,7 @@ static int advansys_board_found(struct Scsi_Host *shost, unsigned int iop,
 		ASC_DBG(2, "AscInitSetConfig()\n");
 		ret = AscInitSetConfig(pdev, shost) ? -ENODEV : 0;
 		if (ret)
-			goto err_free_proc;
+			goto err_unmap;
 	} else {
 		ADVEEP_3550_CONFIG *ep_3550;
 		ADVEEP_38C0800_CONFIG *ep_38C0800;
@@ -12290,7 +11783,7 @@ static int advansys_board_found(struct Scsi_Host *shost, unsigned int iop,
 				shost_printk(KERN_ERR, shost, "request_dma() "
 						"%d failed %d\n",
 						shost->dma_channel, ret);
-				goto err_free_proc;
+				goto err_unmap;
 			}
 			AscEnableIsaDma(shost->dma_channel);
 		}
@@ -12371,8 +11864,6 @@ static int advansys_board_found(struct Scsi_Host *shost, unsigned int iop,
 	if (shost->dma_channel != NO_ISA_DMA)
 		free_dma(shost->dma_channel);
 #endif
- err_free_proc:
-	kfree(boardp->prtbuf);
  err_unmap:
 	if (boardp->ioremap_addr)
 		iounmap(boardp->ioremap_addr);
@@ -12406,7 +11897,6 @@ static int advansys_release(struct Scsi_Host *shost)
 		iounmap(board->ioremap_addr);
 		advansys_wide_free_mem(board);
 	}
-	kfree(board->prtbuf);
 	scsi_host_put(shost);
 	ASC_DBG(1, "end\n");
 	return 0;
diff --git a/drivers/scsi/aha152x.c b/drivers/scsi/aha152x.c
index a284be17699f..3f7b6fee0a74 100644
--- a/drivers/scsi/aha152x.c
+++ b/drivers/scsi/aha152x.c
@@ -2977,11 +2977,10 @@ static void show_queues(struct Scsi_Host *shpnt)
 }
 
 #undef SPRINTF
-#define SPRINTF(args...) pos += sprintf(pos, ## args)
+#define SPRINTF(args...) seq_printf(m, ##args)
 
-static int get_command(char *pos, Scsi_Cmnd * ptr)
+static void get_command(struct seq_file *m, Scsi_Cmnd * ptr)
 {
-	char *start = pos;
 	int i;
 
 	SPRINTF("%p: target=%d; lun=%d; cmnd=( ",
@@ -3011,13 +3010,10 @@ static int get_command(char *pos, Scsi_Cmnd * ptr)
 	if (ptr->SCp.phase & syncneg)
 		SPRINTF("syncneg|");
 	SPRINTF("; next=0x%p\n", SCNEXT(ptr));
-
-	return (pos - start);
 }
 
-static int get_ports(struct Scsi_Host *shpnt, char *pos)
+static void get_ports(struct seq_file *m, struct Scsi_Host *shpnt)
 {
-	char *start = pos;
 	int s;
 
 	SPRINTF("\n%s: %s(%s) ", CURRENT_SC ? "on bus" : "waiting", states[STATE].name, states[PREVSTATE].name);
@@ -3273,11 +3269,9 @@ static int get_ports(struct Scsi_Host *shpnt, char *pos)
 	if (s & ENREQINIT)
 		SPRINTF("ENREQINIT ");
 	SPRINTF(")\n");
-
-	return (pos - start);
 }
 
-static int aha152x_set_info(char *buffer, int length, struct Scsi_Host *shpnt)
+static int aha152x_set_info(struct Scsi_Host *shpnt, char *buffer, int length)
 {
 	if(!shpnt || !buffer || length<8 || strncmp("aha152x ", buffer, 8)!=0)
 		return -EINVAL;
@@ -3320,26 +3314,11 @@ static int aha152x_set_info(char *buffer, int length, struct Scsi_Host *shpnt)
 	return length;
 }
 
-#undef SPRINTF
-#define SPRINTF(args...) \
-	do { if(pos < buffer + length) pos += sprintf(pos, ## args); } while(0)
-
-static int aha152x_proc_info(struct Scsi_Host *shpnt, char *buffer, char **start,
-		      off_t offset, int length, int inout)
+static int aha152x_show_info(struct seq_file *m, struct Scsi_Host *shpnt)
 {
 	int i;
-	char *pos = buffer;
 	Scsi_Cmnd *ptr;
 	unsigned long flags;
-	int thislength;
-
-	DPRINTK(debug_procinfo, 
-	       KERN_DEBUG "aha152x_proc_info: buffer=%p offset=%ld length=%d hostno=%d inout=%d\n",
-	       buffer, offset, length, shpnt->host_no, inout);
-
-
-	if (inout)
-		return aha152x_set_info(buffer, length, shpnt);
 
 	SPRINTF(AHA152X_REVID "\n");
 
@@ -3392,25 +3371,25 @@ static int aha152x_proc_info(struct Scsi_Host *shpnt, char *buffer, char **start
 	if (ISSUE_SC) {
 		SPRINTF("not yet issued commands:\n");
 		for (ptr = ISSUE_SC; ptr; ptr = SCNEXT(ptr))
-			pos += get_command(pos, ptr);
+			get_command(m, ptr);
 	} else
 		SPRINTF("no not yet issued commands\n");
 	DO_UNLOCK(flags);
 
 	if (CURRENT_SC) {
 		SPRINTF("current command:\n");
-		pos += get_command(pos, CURRENT_SC);
+		get_command(m, CURRENT_SC);
 	} else
 		SPRINTF("no current command\n");
 
 	if (DISCONNECTED_SC) {
 		SPRINTF("disconnected commands:\n");
 		for (ptr = DISCONNECTED_SC; ptr; ptr = SCNEXT(ptr))
-			pos += get_command(pos, ptr);
+			get_command(m, ptr);
 	} else
 		SPRINTF("no disconnected commands\n");
 
-	pos += get_ports(shpnt, pos);
+	get_ports(m, shpnt);
 
 #if defined(AHA152X_STAT)
 	SPRINTF("statistics:\n"
@@ -3440,24 +3419,7 @@ static int aha152x_proc_info(struct Scsi_Host *shpnt, char *buffer, char **start
 			HOSTDATA(shpnt)->time[i]);
 	}
 #endif
-
-	DPRINTK(debug_procinfo, KERN_DEBUG "aha152x_proc_info: pos=%p\n", pos);
-
-	thislength = pos - (buffer + offset);
-	DPRINTK(debug_procinfo, KERN_DEBUG "aha152x_proc_info: length=%d thislength=%d\n", length, thislength);
-
-	if(thislength<0) {
-		DPRINTK(debug_procinfo, KERN_DEBUG "aha152x_proc_info: output too short\n");
-		*start = NULL;
-		return 0;
-	}
-
-	thislength = thislength<length ? thislength : length;
-
-	DPRINTK(debug_procinfo, KERN_DEBUG "aha152x_proc_info: return %d\n", thislength);
-
-	*start = buffer + offset;
-	return thislength < length ? thislength : length;
+	return 0;
 }
 
 static int aha152x_adjust_queue(struct scsi_device *device)
@@ -3470,7 +3432,8 @@ static struct scsi_host_template aha152x_driver_template = {
 	.module				= THIS_MODULE,
 	.name				= AHA152X_REVID,
 	.proc_name			= "aha152x",
-	.proc_info			= aha152x_proc_info,
+	.show_info			= aha152x_show_info,
+	.write_info			= aha152x_set_info,
 	.queuecommand			= aha152x_queue,
 	.eh_abort_handler		= aha152x_abort,
 	.eh_device_reset_handler	= aha152x_device_reset,
diff --git a/drivers/scsi/aha1740.c b/drivers/scsi/aha1740.c
index df775e6ba579..5f3101797c93 100644
--- a/drivers/scsi/aha1740.c
+++ b/drivers/scsi/aha1740.c
@@ -106,33 +106,14 @@ static inline dma_addr_t ecb_cpu_to_dma (struct Scsi_Host *host, void *cpu)
 	return hdata->ecb_dma_addr + offset;
 }
 
-static int aha1740_proc_info(struct Scsi_Host *shpnt, char *buffer,
-			     char **start, off_t offset,
-			     int length, int inout)
+static int aha1740_show_info(struct seq_file *m, struct Scsi_Host *shpnt)
 {
-	int len;
-	struct aha1740_hostdata *host;
-
-	if (inout)
-		return-ENOSYS;
-
-	host = HOSTDATA(shpnt);
-
-	len = sprintf(buffer, "aha174x at IO:%lx, IRQ %d, SLOT %d.\n"
+	struct aha1740_hostdata *host = HOSTDATA(shpnt);
+	seq_printf(m, "aha174x at IO:%lx, IRQ %d, SLOT %d.\n"
 		      "Extended translation %sabled.\n",
 		      shpnt->io_port, shpnt->irq, host->edev->slot,
 		      host->translation ? "en" : "dis");
-
-	if (offset > len) {
-		*start = buffer;
-		return 0;
-	}
-
-	*start = buffer + offset;
-	len -= offset;
-	if (len > length)
-		len = length;
-	return len;
+	return 0;
 }
 
 static int aha1740_makecode(unchar *sense, unchar *status)
@@ -556,7 +537,7 @@ static int aha1740_eh_abort_handler (Scsi_Cmnd *dummy)
 static struct scsi_host_template aha1740_template = {
 	.module           = THIS_MODULE,
 	.proc_name        = "aha1740",
-	.proc_info        = aha1740_proc_info,
+	.show_info        = aha1740_show_info,
 	.name             = "Adaptec 174x (EISA)",
 	.queuecommand     = aha1740_queuecommand,
 	.bios_param       = aha1740_biosparam,
diff --git a/drivers/scsi/aic7xxx/aic79xx_osm.c b/drivers/scsi/aic7xxx/aic79xx_osm.c
index 9328121804bb..69d5c43a65e5 100644
--- a/drivers/scsi/aic7xxx/aic79xx_osm.c
+++ b/drivers/scsi/aic7xxx/aic79xx_osm.c
@@ -906,7 +906,8 @@ struct scsi_host_template aic79xx_driver_template = {
 	.module			= THIS_MODULE,
 	.name			= "aic79xx",
 	.proc_name		= "aic79xx",
-	.proc_info		= ahd_linux_proc_info,
+	.show_info		= ahd_linux_show_info,
+	.write_info	 	= ahd_proc_write_seeprom,
 	.info			= ahd_linux_info,
 	.queuecommand		= ahd_linux_queue,
 	.eh_abort_handler	= ahd_linux_abort,
@@ -1702,19 +1703,13 @@ ahd_send_async(struct ahd_softc *ahd, char channel,
 	switch (code) {
 	case AC_TRANSFER_NEG:
 	{
-		char	buf[80];
 		struct  scsi_target *starget;
-		struct	info_str info;
 		struct	ahd_initiator_tinfo *tinfo;
 		struct	ahd_tmode_tstate *tstate;
 		unsigned int target_ppr_options;
 
 		BUG_ON(target == CAM_TARGET_WILDCARD);
 
-		info.buffer = buf;
-		info.length = sizeof(buf);
-		info.offset = 0;
-		info.pos = 0;
 		tinfo = ahd_fetch_transinfo(ahd, channel, ahd->our_id,
 					    target, &tstate);
 
diff --git a/drivers/scsi/aic7xxx/aic79xx_osm.h b/drivers/scsi/aic7xxx/aic79xx_osm.h
index 28e43498cdff..c58fa33c6592 100644
--- a/drivers/scsi/aic7xxx/aic79xx_osm.h
+++ b/drivers/scsi/aic7xxx/aic79xx_osm.h
@@ -379,14 +379,6 @@ void ahd_insb(struct ahd_softc * ahd, long port,
 int		ahd_linux_register_host(struct ahd_softc *,
 					struct scsi_host_template *);
 
-/*************************** Pretty Printing **********************************/
-struct info_str {
-	char *buffer;
-	int length;
-	off_t offset;
-	int pos;
-};
-
 /******************************** Locking *************************************/
 static inline void
 ahd_lockinit(struct ahd_softc *ahd)
@@ -513,8 +505,8 @@ ahd_flush_device_writes(struct ahd_softc *ahd)
 }
 
 /**************************** Proc FS Support *********************************/
-int	ahd_linux_proc_info(struct Scsi_Host *, char *, char **,
-			    off_t, int, int);
+int	ahd_proc_write_seeprom(struct Scsi_Host *, char *, int);
+int	ahd_linux_show_info(struct seq_file *,struct Scsi_Host *);
 
 /*********************** Transaction Access Wrappers **************************/
 static inline void ahd_cmd_set_transaction_status(struct scsi_cmnd *, uint32_t);
diff --git a/drivers/scsi/aic7xxx/aic79xx_proc.c b/drivers/scsi/aic7xxx/aic79xx_proc.c
index 59c85d5a153a..e9778b4f7e32 100644
--- a/drivers/scsi/aic7xxx/aic79xx_proc.c
+++ b/drivers/scsi/aic7xxx/aic79xx_proc.c
@@ -42,16 +42,12 @@
 #include "aic79xx_osm.h"
 #include "aic79xx_inline.h"
 
-static void	copy_mem_info(struct info_str *info, char *data, int len);
-static int	copy_info(struct info_str *info, char *fmt, ...);
 static void	ahd_dump_target_state(struct ahd_softc *ahd,
-				      struct info_str *info,
+				      struct seq_file *m,
 				      u_int our_id, char channel,
 				      u_int target_id);
-static void	ahd_dump_device_state(struct info_str *info,
+static void	ahd_dump_device_state(struct seq_file *m,
 				      struct scsi_device *sdev);
-static int	ahd_proc_write_seeprom(struct ahd_softc *ahd,
-				       char *buffer, int length);
 
 /*
  * Table of syncrates that don't follow the "divisible by 4"
@@ -93,58 +89,15 @@ ahd_calc_syncsrate(u_int period_factor)
 	return (10000000 / (period_factor * 4 * 10));
 }
 
-
-static void
-copy_mem_info(struct info_str *info, char *data, int len)
-{
-	if (info->pos + len > info->offset + info->length)
-		len = info->offset + info->length - info->pos;
-
-	if (info->pos + len < info->offset) {
-		info->pos += len;
-		return;
-	}
-
-	if (info->pos < info->offset) {
-		off_t partial;
-
-		partial = info->offset - info->pos;
-		data += partial;
-		info->pos += partial;
-		len  -= partial;
-	}
-
-	if (len > 0) {
-		memcpy(info->buffer, data, len);
-		info->pos += len;
-		info->buffer += len;
-	}
-}
-
-static int
-copy_info(struct info_str *info, char *fmt, ...)
-{
-	va_list args;
-	char buf[256];
-	int len;
-
-	va_start(args, fmt);
-	len = vsprintf(buf, fmt, args);
-	va_end(args);
-
-	copy_mem_info(info, buf, len);
-	return (len);
-}
-
 static void
-ahd_format_transinfo(struct info_str *info, struct ahd_transinfo *tinfo)
+ahd_format_transinfo(struct seq_file *m, struct ahd_transinfo *tinfo)
 {
 	u_int speed;
 	u_int freq;
 	u_int mb;
 
 	if (tinfo->period == AHD_PERIOD_UNKNOWN) {
-		copy_info(info, "Renegotiation Pending\n");
+		seq_printf(m, "Renegotiation Pending\n");
 		return;
 	}
         speed = 3300;
@@ -156,34 +109,34 @@ ahd_format_transinfo(struct info_str *info, struct ahd_transinfo *tinfo)
 	speed *= (0x01 << tinfo->width);
         mb = speed / 1000;
         if (mb > 0)
-		copy_info(info, "%d.%03dMB/s transfers", mb, speed % 1000);
+		seq_printf(m, "%d.%03dMB/s transfers", mb, speed % 1000);
         else
-		copy_info(info, "%dKB/s transfers", speed);
+		seq_printf(m, "%dKB/s transfers", speed);
 
 	if (freq != 0) {
 		int	printed_options;
 
 		printed_options = 0;
-		copy_info(info, " (%d.%03dMHz", freq / 1000, freq % 1000);
+		seq_printf(m, " (%d.%03dMHz", freq / 1000, freq % 1000);
 		if ((tinfo->ppr_options & MSG_EXT_PPR_RD_STRM) != 0) {
-			copy_info(info, " RDSTRM");
+			seq_printf(m, " RDSTRM");
 			printed_options++;
 		}
 		if ((tinfo->ppr_options & MSG_EXT_PPR_DT_REQ) != 0) {
-			copy_info(info, "%s", printed_options ? "|DT" : " DT");
+			seq_printf(m, "%s", printed_options ? "|DT" : " DT");
 			printed_options++;
 		}
 		if ((tinfo->ppr_options & MSG_EXT_PPR_IU_REQ) != 0) {
-			copy_info(info, "%s", printed_options ? "|IU" : " IU");
+			seq_printf(m, "%s", printed_options ? "|IU" : " IU");
 			printed_options++;
 		}
 		if ((tinfo->ppr_options & MSG_EXT_PPR_RTI) != 0) {
-			copy_info(info, "%s",
+			seq_printf(m, "%s",
 				  printed_options ? "|RTI" : " RTI");
 			printed_options++;
 		}
 		if ((tinfo->ppr_options & MSG_EXT_PPR_QAS_REQ) != 0) {
-			copy_info(info, "%s",
+			seq_printf(m, "%s",
 				  printed_options ? "|QAS" : " QAS");
 			printed_options++;
 		}
@@ -191,19 +144,19 @@ ahd_format_transinfo(struct info_str *info, struct ahd_transinfo *tinfo)
 
 	if (tinfo->width > 0) {
 		if (freq != 0) {
-			copy_info(info, ", ");
+			seq_printf(m, ", ");
 		} else {
-			copy_info(info, " (");
+			seq_printf(m, " (");
 		}
-		copy_info(info, "%dbit)", 8 * (0x01 << tinfo->width));
+		seq_printf(m, "%dbit)", 8 * (0x01 << tinfo->width));
 	} else if (freq != 0) {
-		copy_info(info, ")");
+		seq_printf(m, ")");
 	}
-	copy_info(info, "\n");
+	seq_printf(m, "\n");
 }
 
 static void
-ahd_dump_target_state(struct ahd_softc *ahd, struct info_str *info,
+ahd_dump_target_state(struct ahd_softc *ahd, struct seq_file *m,
 		      u_int our_id, char channel, u_int target_id)
 {
 	struct  scsi_target *starget;
@@ -213,17 +166,17 @@ ahd_dump_target_state(struct ahd_softc *ahd, struct info_str *info,
 
 	tinfo = ahd_fetch_transinfo(ahd, channel, our_id,
 				    target_id, &tstate);
-	copy_info(info, "Target %d Negotiation Settings\n", target_id);
-	copy_info(info, "\tUser: ");
-	ahd_format_transinfo(info, &tinfo->user);
+	seq_printf(m, "Target %d Negotiation Settings\n", target_id);
+	seq_printf(m, "\tUser: ");
+	ahd_format_transinfo(m, &tinfo->user);
 	starget = ahd->platform_data->starget[target_id];
 	if (starget == NULL)
 		return;
 
-	copy_info(info, "\tGoal: ");
-	ahd_format_transinfo(info, &tinfo->goal);
-	copy_info(info, "\tCurr: ");
-	ahd_format_transinfo(info, &tinfo->curr);
+	seq_printf(m, "\tGoal: ");
+	ahd_format_transinfo(m, &tinfo->goal);
+	seq_printf(m, "\tCurr: ");
+	ahd_format_transinfo(m, &tinfo->curr);
 
 	for (lun = 0; lun < AHD_NUM_LUNS; lun++) {
 		struct scsi_device *dev;
@@ -233,29 +186,30 @@ ahd_dump_target_state(struct ahd_softc *ahd, struct info_str *info,
 		if (dev == NULL)
 			continue;
 
-		ahd_dump_device_state(info, dev);
+		ahd_dump_device_state(m, dev);
 	}
 }
 
 static void
-ahd_dump_device_state(struct info_str *info, struct scsi_device *sdev)
+ahd_dump_device_state(struct seq_file *m, struct scsi_device *sdev)
 {
 	struct ahd_linux_device *dev = scsi_transport_device_data(sdev);
 
-	copy_info(info, "\tChannel %c Target %d Lun %d Settings\n",
+	seq_printf(m, "\tChannel %c Target %d Lun %d Settings\n",
 		  sdev->sdev_target->channel + 'A',
 		  sdev->sdev_target->id, sdev->lun);
 
-	copy_info(info, "\t\tCommands Queued %ld\n", dev->commands_issued);
-	copy_info(info, "\t\tCommands Active %d\n", dev->active);
-	copy_info(info, "\t\tCommand Openings %d\n", dev->openings);
-	copy_info(info, "\t\tMax Tagged Openings %d\n", dev->maxtags);
-	copy_info(info, "\t\tDevice Queue Frozen Count %d\n", dev->qfrozen);
+	seq_printf(m, "\t\tCommands Queued %ld\n", dev->commands_issued);
+	seq_printf(m, "\t\tCommands Active %d\n", dev->active);
+	seq_printf(m, "\t\tCommand Openings %d\n", dev->openings);
+	seq_printf(m, "\t\tMax Tagged Openings %d\n", dev->maxtags);
+	seq_printf(m, "\t\tDevice Queue Frozen Count %d\n", dev->qfrozen);
 }
 
-static int
-ahd_proc_write_seeprom(struct ahd_softc *ahd, char *buffer, int length)
+int
+ahd_proc_write_seeprom(struct Scsi_Host *shost, char *buffer, int length)
 {
+	struct	ahd_softc *ahd = *(struct ahd_softc **)shost->hostdata;
 	ahd_mode_state saved_modes;
 	int have_seeprom;
 	u_long s;
@@ -319,64 +273,45 @@ done:
  * Return information to handle /proc support for the driver.
  */
 int
-ahd_linux_proc_info(struct Scsi_Host *shost, char *buffer, char **start,
-		    off_t offset, int length, int inout)
+ahd_linux_show_info(struct seq_file *m, struct Scsi_Host *shost)
 {
 	struct	ahd_softc *ahd = *(struct ahd_softc **)shost->hostdata;
-	struct	info_str info;
 	char	ahd_info[256];
 	u_int	max_targ;
 	u_int	i;
-	int	retval;
 
-	 /* Has data been written to the file? */ 
-	if (inout == TRUE) {
-		retval = ahd_proc_write_seeprom(ahd, buffer, length);
-		goto done;
-	}
-
-	if (start)
-		*start = buffer;
-
-	info.buffer	= buffer;
-	info.length	= length;
-	info.offset	= offset;
-	info.pos	= 0;
-
-	copy_info(&info, "Adaptec AIC79xx driver version: %s\n",
+	seq_printf(m, "Adaptec AIC79xx driver version: %s\n",
 		  AIC79XX_DRIVER_VERSION);
-	copy_info(&info, "%s\n", ahd->description);
+	seq_printf(m, "%s\n", ahd->description);
 	ahd_controller_info(ahd, ahd_info);
-	copy_info(&info, "%s\n", ahd_info);
-	copy_info(&info, "Allocated SCBs: %d, SG List Length: %d\n\n",
+	seq_printf(m, "%s\n", ahd_info);
+	seq_printf(m, "Allocated SCBs: %d, SG List Length: %d\n\n",
 		  ahd->scb_data.numscbs, AHD_NSEG);
 
 	max_targ = 16;
 
 	if (ahd->seep_config == NULL)
-		copy_info(&info, "No Serial EEPROM\n");
+		seq_printf(m, "No Serial EEPROM\n");
 	else {
-		copy_info(&info, "Serial EEPROM:\n");
+		seq_printf(m, "Serial EEPROM:\n");
 		for (i = 0; i < sizeof(*ahd->seep_config)/2; i++) {
 			if (((i % 8) == 0) && (i != 0)) {
-				copy_info(&info, "\n");
+				seq_printf(m, "\n");
 			}
-			copy_info(&info, "0x%.4x ",
+			seq_printf(m, "0x%.4x ",
 				  ((uint16_t*)ahd->seep_config)[i]);
 		}
-		copy_info(&info, "\n");
+		seq_printf(m, "\n");
 	}
-	copy_info(&info, "\n");
+	seq_printf(m, "\n");
 
 	if ((ahd->features & AHD_WIDE) == 0)
 		max_targ = 8;
 
 	for (i = 0; i < max_targ; i++) {
 
-		ahd_dump_target_state(ahd, &info, ahd->our_id, 'A',
+		ahd_dump_target_state(ahd, m, ahd->our_id, 'A',
 				      /*target_id*/i);
 	}
-	retval = info.pos > info.offset ? info.pos - info.offset : 0;
-done:
-	return (retval);
+	return 0;
 }
diff --git a/drivers/scsi/aic7xxx/aic7xxx_osm.c b/drivers/scsi/aic7xxx/aic7xxx_osm.c
index 5a477cdc780d..c0c62583b542 100644
--- a/drivers/scsi/aic7xxx/aic7xxx_osm.c
+++ b/drivers/scsi/aic7xxx/aic7xxx_osm.c
@@ -803,7 +803,8 @@ struct scsi_host_template aic7xxx_driver_template = {
 	.module			= THIS_MODULE,
 	.name			= "aic7xxx",
 	.proc_name		= "aic7xxx",
-	.proc_info		= ahc_linux_proc_info,
+	.show_info		= ahc_linux_show_info,
+	.write_info		= ahc_proc_write_seeprom,
 	.info			= ahc_linux_info,
 	.queuecommand		= ahc_linux_queue,
 	.eh_abort_handler	= ahc_linux_abort,
@@ -1631,10 +1632,8 @@ ahc_send_async(struct ahc_softc *ahc, char channel,
 	switch (code) {
 	case AC_TRANSFER_NEG:
 	{
-		char	buf[80];
 		struct	scsi_target *starget;
 		struct	ahc_linux_target *targ;
-		struct	info_str info;
 		struct	ahc_initiator_tinfo *tinfo;
 		struct	ahc_tmode_tstate *tstate;
 		int	target_offset;
@@ -1642,10 +1641,6 @@ ahc_send_async(struct ahc_softc *ahc, char channel,
 
 		BUG_ON(target == CAM_TARGET_WILDCARD);
 
-		info.buffer = buf;
-		info.length = sizeof(buf);
-		info.offset = 0;
-		info.pos = 0;
 		tinfo = ahc_fetch_transinfo(ahc, channel,
 						channel == 'A' ? ahc->our_id
 							       : ahc->our_id_b,
diff --git a/drivers/scsi/aic7xxx/aic7xxx_osm.h b/drivers/scsi/aic7xxx/aic7xxx_osm.h
index bca0fb83f553..bc4cca92ff04 100644
--- a/drivers/scsi/aic7xxx/aic7xxx_osm.h
+++ b/drivers/scsi/aic7xxx/aic7xxx_osm.h
@@ -383,14 +383,6 @@ void ahc_insb(struct ahc_softc * ahc, long port,
 int		ahc_linux_register_host(struct ahc_softc *,
 					struct scsi_host_template *);
 
-/*************************** Pretty Printing **********************************/
-struct info_str {
-	char *buffer;
-	int length;
-	off_t offset;
-	int pos;
-};
-
 /******************************** Locking *************************************/
 /* Lock protecting internal data structures */
 
@@ -523,8 +515,8 @@ ahc_flush_device_writes(struct ahc_softc *ahc)
 }
 
 /**************************** Proc FS Support *********************************/
-int	ahc_linux_proc_info(struct Scsi_Host *, char *, char **,
-			    off_t, int, int);
+int	ahc_proc_write_seeprom(struct Scsi_Host *, char *, int);
+int	ahc_linux_show_info(struct seq_file *, struct Scsi_Host *);
 
 /*************************** Domain Validation ********************************/
 /*********************** Transaction Access Wrappers *************************/
diff --git a/drivers/scsi/aic7xxx/aic7xxx_proc.c b/drivers/scsi/aic7xxx/aic7xxx_proc.c
index f2525f8ed1c7..383a3d11652d 100644
--- a/drivers/scsi/aic7xxx/aic7xxx_proc.c
+++ b/drivers/scsi/aic7xxx/aic7xxx_proc.c
@@ -43,16 +43,12 @@
 #include "aic7xxx_inline.h"
 #include "aic7xxx_93cx6.h"
 
-static void	copy_mem_info(struct info_str *info, char *data, int len);
-static int	copy_info(struct info_str *info, char *fmt, ...);
 static void	ahc_dump_target_state(struct ahc_softc *ahc,
-				      struct info_str *info,
+				      struct seq_file *m,
 				      u_int our_id, char channel,
 				      u_int target_id, u_int target_offset);
-static void	ahc_dump_device_state(struct info_str *info,
+static void	ahc_dump_device_state(struct seq_file *m,
 				      struct scsi_device *dev);
-static int	ahc_proc_write_seeprom(struct ahc_softc *ahc,
-				       char *buffer, int length);
 
 /*
  * Table of syncrates that don't follow the "divisible by 4"
@@ -94,51 +90,8 @@ ahc_calc_syncsrate(u_int period_factor)
 	return (10000000 / (period_factor * 4 * 10));
 }
 
-
-static void
-copy_mem_info(struct info_str *info, char *data, int len)
-{
-	if (info->pos + len > info->offset + info->length)
-		len = info->offset + info->length - info->pos;
-
-	if (info->pos + len < info->offset) {
-		info->pos += len;
-		return;
-	}
-
-	if (info->pos < info->offset) {
-		off_t partial;
-
-		partial = info->offset - info->pos;
-		data += partial;
-		info->pos += partial;
-		len  -= partial;
-	}
-
-	if (len > 0) {
-		memcpy(info->buffer, data, len);
-		info->pos += len;
-		info->buffer += len;
-	}
-}
-
-static int
-copy_info(struct info_str *info, char *fmt, ...)
-{
-	va_list args;
-	char buf[256];
-	int len;
-
-	va_start(args, fmt);
-	len = vsprintf(buf, fmt, args);
-	va_end(args);
-
-	copy_mem_info(info, buf, len);
-	return (len);
-}
-
 static void
-ahc_format_transinfo(struct info_str *info, struct ahc_transinfo *tinfo)
+ahc_format_transinfo(struct seq_file *m, struct ahc_transinfo *tinfo)
 {
 	u_int speed;
 	u_int freq;
@@ -153,12 +106,12 @@ ahc_format_transinfo(struct info_str *info, struct ahc_transinfo *tinfo)
 	speed *= (0x01 << tinfo->width);
         mb = speed / 1000;
         if (mb > 0)
-		copy_info(info, "%d.%03dMB/s transfers", mb, speed % 1000);
+		seq_printf(m, "%d.%03dMB/s transfers", mb, speed % 1000);
         else
-		copy_info(info, "%dKB/s transfers", speed);
+		seq_printf(m, "%dKB/s transfers", speed);
 
 	if (freq != 0) {
-		copy_info(info, " (%d.%03dMHz%s, offset %d",
+		seq_printf(m, " (%d.%03dMHz%s, offset %d",
 			 freq / 1000, freq % 1000,
 			 (tinfo->ppr_options & MSG_EXT_PPR_DT_REQ) != 0
 			 ? " DT" : "", tinfo->offset);
@@ -166,19 +119,19 @@ ahc_format_transinfo(struct info_str *info, struct ahc_transinfo *tinfo)
 
 	if (tinfo->width > 0) {
 		if (freq != 0) {
-			copy_info(info, ", ");
+			seq_printf(m, ", ");
 		} else {
-			copy_info(info, " (");
+			seq_printf(m, " (");
 		}
-		copy_info(info, "%dbit)", 8 * (0x01 << tinfo->width));
+		seq_printf(m, "%dbit)", 8 * (0x01 << tinfo->width));
 	} else if (freq != 0) {
-		copy_info(info, ")");
+		seq_printf(m, ")");
 	}
-	copy_info(info, "\n");
+	seq_printf(m, "\n");
 }
 
 static void
-ahc_dump_target_state(struct ahc_softc *ahc, struct info_str *info,
+ahc_dump_target_state(struct ahc_softc *ahc, struct seq_file *m,
 		      u_int our_id, char channel, u_int target_id,
 		      u_int target_offset)
 {
@@ -190,18 +143,18 @@ ahc_dump_target_state(struct ahc_softc *ahc, struct info_str *info,
 	tinfo = ahc_fetch_transinfo(ahc, channel, our_id,
 				    target_id, &tstate);
 	if ((ahc->features & AHC_TWIN) != 0)
-		copy_info(info, "Channel %c ", channel);
-	copy_info(info, "Target %d Negotiation Settings\n", target_id);
-	copy_info(info, "\tUser: ");
-	ahc_format_transinfo(info, &tinfo->user);
+		seq_printf(m, "Channel %c ", channel);
+	seq_printf(m, "Target %d Negotiation Settings\n", target_id);
+	seq_printf(m, "\tUser: ");
+	ahc_format_transinfo(m, &tinfo->user);
 	starget = ahc->platform_data->starget[target_offset];
 	if (!starget)
 		return;
 
-	copy_info(info, "\tGoal: ");
-	ahc_format_transinfo(info, &tinfo->goal);
-	copy_info(info, "\tCurr: ");
-	ahc_format_transinfo(info, &tinfo->curr);
+	seq_printf(m, "\tGoal: ");
+	ahc_format_transinfo(m, &tinfo->goal);
+	seq_printf(m, "\tCurr: ");
+	ahc_format_transinfo(m, &tinfo->curr);
 
 	for (lun = 0; lun < AHC_NUM_LUNS; lun++) {
 		struct scsi_device *sdev;
@@ -211,29 +164,30 @@ ahc_dump_target_state(struct ahc_softc *ahc, struct info_str *info,
 		if (sdev == NULL)
 			continue;
 
-		ahc_dump_device_state(info, sdev);
+		ahc_dump_device_state(m, sdev);
 	}
 }
 
 static void
-ahc_dump_device_state(struct info_str *info, struct scsi_device *sdev)
+ahc_dump_device_state(struct seq_file *m, struct scsi_device *sdev)
 {
 	struct ahc_linux_device *dev = scsi_transport_device_data(sdev);
 
-	copy_info(info, "\tChannel %c Target %d Lun %d Settings\n",
+	seq_printf(m, "\tChannel %c Target %d Lun %d Settings\n",
 		  sdev->sdev_target->channel + 'A',
 		  sdev->sdev_target->id, sdev->lun);
 
-	copy_info(info, "\t\tCommands Queued %ld\n", dev->commands_issued);
-	copy_info(info, "\t\tCommands Active %d\n", dev->active);
-	copy_info(info, "\t\tCommand Openings %d\n", dev->openings);
-	copy_info(info, "\t\tMax Tagged Openings %d\n", dev->maxtags);
-	copy_info(info, "\t\tDevice Queue Frozen Count %d\n", dev->qfrozen);
+	seq_printf(m, "\t\tCommands Queued %ld\n", dev->commands_issued);
+	seq_printf(m, "\t\tCommands Active %d\n", dev->active);
+	seq_printf(m, "\t\tCommand Openings %d\n", dev->openings);
+	seq_printf(m, "\t\tMax Tagged Openings %d\n", dev->maxtags);
+	seq_printf(m, "\t\tDevice Queue Frozen Count %d\n", dev->qfrozen);
 }
 
-static int
-ahc_proc_write_seeprom(struct ahc_softc *ahc, char *buffer, int length)
+int
+ahc_proc_write_seeprom(struct Scsi_Host *shost, char *buffer, int length)
 {
+	struct	ahc_softc *ahc = *(struct ahc_softc **)shost->hostdata;
 	struct seeprom_descriptor sd;
 	int have_seeprom;
 	u_long s;
@@ -332,53 +286,36 @@ done:
  * Return information to handle /proc support for the driver.
  */
 int
-ahc_linux_proc_info(struct Scsi_Host *shost, char *buffer, char **start,
-		    off_t offset, int length, int inout)
+ahc_linux_show_info(struct seq_file *m, struct Scsi_Host *shost)
 {
 	struct	ahc_softc *ahc = *(struct ahc_softc **)shost->hostdata;
-	struct	info_str info;
 	char	ahc_info[256];
 	u_int	max_targ;
 	u_int	i;
-	int	retval;
 
-	 /* Has data been written to the file? */ 
-	if (inout == TRUE) {
-		retval = ahc_proc_write_seeprom(ahc, buffer, length);
-		goto done;
-	}
-
-	if (start)
-		*start = buffer;
-
-	info.buffer	= buffer;
-	info.length	= length;
-	info.offset	= offset;
-	info.pos	= 0;
-
-	copy_info(&info, "Adaptec AIC7xxx driver version: %s\n",
+	seq_printf(m, "Adaptec AIC7xxx driver version: %s\n",
 		  AIC7XXX_DRIVER_VERSION);
-	copy_info(&info, "%s\n", ahc->description);
+	seq_printf(m, "%s\n", ahc->description);
 	ahc_controller_info(ahc, ahc_info);
-	copy_info(&info, "%s\n", ahc_info);
-	copy_info(&info, "Allocated SCBs: %d, SG List Length: %d\n\n",
+	seq_printf(m, "%s\n", ahc_info);
+	seq_printf(m, "Allocated SCBs: %d, SG List Length: %d\n\n",
 		  ahc->scb_data->numscbs, AHC_NSEG);
 
 
 	if (ahc->seep_config == NULL)
-		copy_info(&info, "No Serial EEPROM\n");
+		seq_printf(m, "No Serial EEPROM\n");
 	else {
-		copy_info(&info, "Serial EEPROM:\n");
+		seq_printf(m, "Serial EEPROM:\n");
 		for (i = 0; i < sizeof(*ahc->seep_config)/2; i++) {
 			if (((i % 8) == 0) && (i != 0)) {
-				copy_info(&info, "\n");
+				seq_printf(m, "\n");
 			}
-			copy_info(&info, "0x%.4x ",
+			seq_printf(m, "0x%.4x ",
 				  ((uint16_t*)ahc->seep_config)[i]);
 		}
-		copy_info(&info, "\n");
+		seq_printf(m, "\n");
 	}
-	copy_info(&info, "\n");
+	seq_printf(m, "\n");
 
 	max_targ = 16;
 	if ((ahc->features & (AHC_WIDE|AHC_TWIN)) == 0)
@@ -398,10 +335,8 @@ ahc_linux_proc_info(struct Scsi_Host *shost, char *buffer, char **start,
 			target_id = i % 8;
 		}
 
-		ahc_dump_target_state(ahc, &info, our_id,
+		ahc_dump_target_state(ahc, m, our_id,
 				      channel, target_id, i);
 	}
-	retval = info.pos > info.offset ? info.pos - info.offset : 0;
-done:
-	return (retval);
+	return 0;
 }
diff --git a/drivers/scsi/aic7xxx_old.c b/drivers/scsi/aic7xxx_old.c
index 5b212f0df898..33ec9c643400 100644
--- a/drivers/scsi/aic7xxx_old.c
+++ b/drivers/scsi/aic7xxx_old.c
@@ -11108,7 +11108,7 @@ MODULE_VERSION(AIC7XXX_H_VERSION);
 
 
 static struct scsi_host_template driver_template = {
-	.proc_info		= aic7xxx_proc_info,
+	.show_info		= aic7xxx_show_info,
 	.detect			= aic7xxx_detect,
 	.release		= aic7xxx_release,
 	.info			= aic7xxx_info,	
diff --git a/drivers/scsi/aic7xxx_old/aic7xxx_proc.c b/drivers/scsi/aic7xxx_old/aic7xxx_proc.c
index b07e4f04fd00..976f45ccf2cf 100644
--- a/drivers/scsi/aic7xxx_old/aic7xxx_proc.c
+++ b/drivers/scsi/aic7xxx_old/aic7xxx_proc.c
@@ -30,62 +30,23 @@
  *-M*************************************************************************/
 
 
-#define	BLS	(&aic7xxx_buffer[size])
 #define HDRB \
 "               0 - 4K   4 - 16K   16 - 64K  64 - 256K  256K - 1M        1M+"
 
-#ifdef PROC_DEBUG
-extern int vsprintf(char *, const char *, va_list);
-
-static void
-proc_debug(const char *fmt, ...)
-{
-  va_list ap;
-  char buf[256];
-
-  va_start(ap, fmt);
-  vsprintf(buf, fmt, ap);
-  printk(buf);
-  va_end(ap);
-}
-#else /* PROC_DEBUG */
-#  define proc_debug(fmt, args...)
-#endif /* PROC_DEBUG */
-
-static int aic7xxx_buffer_size = 0;
-static char *aic7xxx_buffer = NULL;
-
 
 /*+F*************************************************************************
  * Function:
- *   aic7xxx_set_info
- *
- * Description:
- *   Set parameters for the driver from the /proc filesystem.
- *-F*************************************************************************/
-static int
-aic7xxx_set_info(char *buffer, int length, struct Scsi_Host *HBAptr)
-{
-  proc_debug("aic7xxx_set_info(): %s\n", buffer);
-  return (-ENOSYS);  /* Currently this is a no-op */
-}
-
-
-/*+F*************************************************************************
- * Function:
- *   aic7xxx_proc_info
+ *   aic7xxx_show_info
  *
  * Description:
  *   Return information to handle /proc support for the driver.
  *-F*************************************************************************/
 int
-aic7xxx_proc_info ( struct Scsi_Host *HBAptr, char *buffer, char **start, off_t offset, int length, 
-                    int inout)
+aic7xxx_show_info(struct seq_file *m, struct Scsi_Host *HBAptr)
 {
   struct aic7xxx_host *p;
   struct aic_dev_data *aic_dev;
   struct scsi_device *sdptr;
-  int    size = 0;
   unsigned char i;
   unsigned char tindex;
 
@@ -94,66 +55,21 @@ aic7xxx_proc_info ( struct Scsi_Host *HBAptr, char *buffer, char **start, off_t
 
   if (!p)
   {
-    size += sprintf(buffer, "Can't find adapter for host number %d\n", HBAptr->host_no);
-    if (size > length)
-    {
-      return (size);
-    }
-    else
-    {
-      return (length);
-    }
-  }
-
-  if (inout == TRUE) /* Has data been written to the file? */ 
-  {
-    return (aic7xxx_set_info(buffer, length, HBAptr));
+    seq_printf(m, "Can't find adapter for host number %d\n", HBAptr->host_no);
+    return 0;
   }
 
   p = (struct aic7xxx_host *) HBAptr->hostdata;
 
-  /*
-   * It takes roughly 1K of space to hold all relevant card info, not
-   * counting any proc stats, so we start out with a 1.5k buffer size and
-   * if proc_stats is defined, then we sweep the stats structure to see
-   * how many drives we will be printing out for and add 384 bytes per
-   * device with active stats.
-   *
-   * Hmmmm...that 1.5k seems to keep growing as items get added so they
-   * can be easily viewed for debugging purposes.  So, we bumped that
-   * 1.5k to 4k so we can quit having to bump it all the time.
-   */
-
-  size = 4096;
-  list_for_each_entry(aic_dev, &p->aic_devs, list)
-    size += 512;
-  if (aic7xxx_buffer_size != size)
-  {
-    if (aic7xxx_buffer != NULL) 
-    {
-      kfree(aic7xxx_buffer);
-      aic7xxx_buffer_size = 0;
-    }
-    aic7xxx_buffer = kmalloc(size, GFP_KERNEL);
-  }
-  if (aic7xxx_buffer == NULL)
-  {
-    size = sprintf(buffer, "AIC7xxx - kmalloc error at line %d\n",
-        __LINE__);
-    return size;
-  }
-  aic7xxx_buffer_size = size;
-
-  size = 0;
-  size += sprintf(BLS, "Adaptec AIC7xxx driver version: ");
-  size += sprintf(BLS, "%s/", AIC7XXX_C_VERSION);
-  size += sprintf(BLS, "%s", AIC7XXX_H_VERSION);
-  size += sprintf(BLS, "\n");
-  size += sprintf(BLS, "Adapter Configuration:\n");
-  size += sprintf(BLS, "           SCSI Adapter: %s\n",
+  seq_printf(m, "Adaptec AIC7xxx driver version: ");
+  seq_printf(m, "%s/", AIC7XXX_C_VERSION);
+  seq_printf(m, "%s", AIC7XXX_H_VERSION);
+  seq_printf(m, "\n");
+  seq_printf(m, "Adapter Configuration:\n");
+  seq_printf(m, "           SCSI Adapter: %s\n",
       board_names[p->board_name_index]);
   if (p->flags & AHC_TWIN)
-    size += sprintf(BLS, "                         Twin Channel Controller ");
+    seq_printf(m, "                         Twin Channel Controller ");
   else
   {
     char *channel = "";
@@ -184,86 +100,86 @@ aic7xxx_proc_info ( struct Scsi_Host *HBAptr, char *buffer, char **start, off_t
       ultra = "Ultra-2 LVD/SE ";
     else if (p->features & AHC_ULTRA)
       ultra = "Ultra ";
-    size += sprintf(BLS, "                           %s%sController%s ",
+    seq_printf(m, "                           %s%sController%s ",
       ultra, wide, channel);
   }
   switch(p->chip & ~AHC_CHIPID_MASK)
   {
     case AHC_VL:
-      size += sprintf(BLS, "at VLB slot %d\n", p->pci_device_fn);
+      seq_printf(m, "at VLB slot %d\n", p->pci_device_fn);
       break;
     case AHC_EISA:
-      size += sprintf(BLS, "at EISA slot %d\n", p->pci_device_fn);
+      seq_printf(m, "at EISA slot %d\n", p->pci_device_fn);
       break;
     default:
-      size += sprintf(BLS, "at PCI %d/%d/%d\n", p->pci_bus,
+      seq_printf(m, "at PCI %d/%d/%d\n", p->pci_bus,
         PCI_SLOT(p->pci_device_fn), PCI_FUNC(p->pci_device_fn));
       break;
   }
   if( !(p->maddr) )
   {
-    size += sprintf(BLS, "    Programmed I/O Base: %lx\n", p->base);
+    seq_printf(m, "    Programmed I/O Base: %lx\n", p->base);
   }
   else
   {
-    size += sprintf(BLS, "    PCI MMAPed I/O Base: 0x%lx\n", p->mbase);
+    seq_printf(m, "    PCI MMAPed I/O Base: 0x%lx\n", p->mbase);
   }
   if( (p->chip & (AHC_VL | AHC_EISA)) )
   {
-    size += sprintf(BLS, "    BIOS Memory Address: 0x%08x\n", p->bios_address);
+    seq_printf(m, "    BIOS Memory Address: 0x%08x\n", p->bios_address);
   }
-  size += sprintf(BLS, " Adapter SEEPROM Config: %s\n",
+  seq_printf(m, " Adapter SEEPROM Config: %s\n",
           (p->flags & AHC_SEEPROM_FOUND) ? "SEEPROM found and used." :
          ((p->flags & AHC_USEDEFAULTS) ? "SEEPROM not found, using defaults." :
            "SEEPROM not found, using leftover BIOS values.") );
-  size += sprintf(BLS, "      Adaptec SCSI BIOS: %s\n",
+  seq_printf(m, "      Adaptec SCSI BIOS: %s\n",
           (p->flags & AHC_BIOS_ENABLED) ? "Enabled" : "Disabled");
-  size += sprintf(BLS, "                    IRQ: %d\n", HBAptr->irq);
-  size += sprintf(BLS, "                   SCBs: Active %d, Max Active %d,\n",
+  seq_printf(m, "                    IRQ: %d\n", HBAptr->irq);
+  seq_printf(m, "                   SCBs: Active %d, Max Active %d,\n",
             p->activescbs, p->max_activescbs);
-  size += sprintf(BLS, "                         Allocated %d, HW %d, "
+  seq_printf(m, "                         Allocated %d, HW %d, "
             "Page %d\n", p->scb_data->numscbs, p->scb_data->maxhscbs,
             p->scb_data->maxscbs);
   if (p->flags & AHC_EXTERNAL_SRAM)
-    size += sprintf(BLS, "                         Using External SCB SRAM\n");
-  size += sprintf(BLS, "             Interrupts: %ld", p->isr_count);
+    seq_printf(m, "                         Using External SCB SRAM\n");
+  seq_printf(m, "             Interrupts: %ld", p->isr_count);
   if (p->chip & AHC_EISA)
   {
-    size += sprintf(BLS, " %s\n",
+    seq_printf(m, " %s\n",
         (p->pause & IRQMS) ? "(Level Sensitive)" : "(Edge Triggered)");
   }
   else
   {
-    size += sprintf(BLS, "\n");
+    seq_printf(m, "\n");
   }
-  size += sprintf(BLS, "      BIOS Control Word: 0x%04x\n",
+  seq_printf(m, "      BIOS Control Word: 0x%04x\n",
             p->bios_control);
-  size += sprintf(BLS, "   Adapter Control Word: 0x%04x\n",
+  seq_printf(m, "   Adapter Control Word: 0x%04x\n",
             p->adapter_control);
-  size += sprintf(BLS, "   Extended Translation: %sabled\n",
+  seq_printf(m, "   Extended Translation: %sabled\n",
       (p->flags & AHC_EXTEND_TRANS_A) ? "En" : "Dis");
-  size += sprintf(BLS, "Disconnect Enable Flags: 0x%04x\n", p->discenable);
+  seq_printf(m, "Disconnect Enable Flags: 0x%04x\n", p->discenable);
   if (p->features & (AHC_ULTRA | AHC_ULTRA2))
   {
-    size += sprintf(BLS, "     Ultra Enable Flags: 0x%04x\n", p->ultraenb);
+    seq_printf(m, "     Ultra Enable Flags: 0x%04x\n", p->ultraenb);
   }
-  size += sprintf(BLS, "Default Tag Queue Depth: %d\n", aic7xxx_default_queue_depth);
-  size += sprintf(BLS, "    Tagged Queue By Device array for aic7xxx host "
+  seq_printf(m, "Default Tag Queue Depth: %d\n", aic7xxx_default_queue_depth);
+  seq_printf(m, "    Tagged Queue By Device array for aic7xxx host "
                        "instance %d:\n", p->instance);
-  size += sprintf(BLS, "      {");
+  seq_printf(m, "      {");
   for(i=0; i < (MAX_TARGETS - 1); i++)
-    size += sprintf(BLS, "%d,",aic7xxx_tag_info[p->instance].tag_commands[i]);
-  size += sprintf(BLS, "%d}\n",aic7xxx_tag_info[p->instance].tag_commands[i]);
+    seq_printf(m, "%d,",aic7xxx_tag_info[p->instance].tag_commands[i]);
+  seq_printf(m, "%d}\n",aic7xxx_tag_info[p->instance].tag_commands[i]);
 
-  size += sprintf(BLS, "\n");
-  size += sprintf(BLS, "Statistics:\n\n");
+  seq_printf(m, "\n");
+  seq_printf(m, "Statistics:\n\n");
   list_for_each_entry(aic_dev, &p->aic_devs, list)
   {
     sdptr = aic_dev->SDptr;
     tindex = sdptr->channel << 3 | sdptr->id;
-    size += sprintf(BLS, "(scsi%d:%d:%d:%d)\n",
+    seq_printf(m, "(scsi%d:%d:%d:%d)\n",
         p->host_no, sdptr->channel, sdptr->id, sdptr->lun);
-    size += sprintf(BLS, "  Device using %s/%s",
+    seq_printf(m, "  Device using %s/%s",
           (aic_dev->cur.width == MSG_EXT_WDTR_BUS_16_BIT) ?
           "Wide" : "Narrow",
           (aic_dev->cur.offset != 0) ?
@@ -279,78 +195,59 @@ aic7xxx_proc_info ( struct Scsi_Host *HBAptr, char *buffer, char **start, off_t
       sync_rate = aic7xxx_find_syncrate(p, &period, 0, &options);
       if (sync_rate != NULL)
       {
-        size += sprintf(BLS, "%s MByte/sec, offset %d\n",
+        seq_printf(m, "%s MByte/sec, offset %d\n",
                         sync_rate->rate[rate],
                         aic_dev->cur.offset );
       }
       else
       {
-        size += sprintf(BLS, "3.3 MByte/sec, offset %d\n",
+        seq_printf(m, "3.3 MByte/sec, offset %d\n",
                         aic_dev->cur.offset );
       }
     }
-    size += sprintf(BLS, "  Transinfo settings: ");
-    size += sprintf(BLS, "current(%d/%d/%d/%d), ",
+    seq_printf(m, "  Transinfo settings: ");
+    seq_printf(m, "current(%d/%d/%d/%d), ",
                     aic_dev->cur.period,
                     aic_dev->cur.offset,
                     aic_dev->cur.width,
                     aic_dev->cur.options);
-    size += sprintf(BLS, "goal(%d/%d/%d/%d), ",
+    seq_printf(m, "goal(%d/%d/%d/%d), ",
                     aic_dev->goal.period,
                     aic_dev->goal.offset,
                     aic_dev->goal.width,
                     aic_dev->goal.options);
-    size += sprintf(BLS, "user(%d/%d/%d/%d)\n",
+    seq_printf(m, "user(%d/%d/%d/%d)\n",
                     p->user[tindex].period,
                     p->user[tindex].offset,
                     p->user[tindex].width,
                     p->user[tindex].options);
     if(sdptr->simple_tags)
     {
-      size += sprintf(BLS, "  Tagged Command Queueing Enabled, Ordered Tags %s, Depth %d/%d\n", sdptr->ordered_tags ? "Enabled" : "Disabled", sdptr->queue_depth, aic_dev->max_q_depth);
+      seq_printf(m, "  Tagged Command Queueing Enabled, Ordered Tags %s, Depth %d/%d\n", sdptr->ordered_tags ? "Enabled" : "Disabled", sdptr->queue_depth, aic_dev->max_q_depth);
     }
     if(aic_dev->barrier_total)
-      size += sprintf(BLS, "  Total transfers %ld:\n    (%ld/%ld/%ld/%ld reads/writes/REQ_BARRIER/Ordered Tags)\n",
+      seq_printf(m, "  Total transfers %ld:\n    (%ld/%ld/%ld/%ld reads/writes/REQ_BARRIER/Ordered Tags)\n",
         aic_dev->r_total+aic_dev->w_total, aic_dev->r_total, aic_dev->w_total,
         aic_dev->barrier_total, aic_dev->ordered_total);
     else
-      size += sprintf(BLS, "  Total transfers %ld:\n    (%ld/%ld reads/writes)\n",
+      seq_printf(m, "  Total transfers %ld:\n    (%ld/%ld reads/writes)\n",
         aic_dev->r_total+aic_dev->w_total, aic_dev->r_total, aic_dev->w_total);
-    size += sprintf(BLS, "%s\n", HDRB);
-    size += sprintf(BLS, "   Reads:");
+    seq_printf(m, "%s\n", HDRB);
+    seq_printf(m, "   Reads:");
     for (i = 0; i < ARRAY_SIZE(aic_dev->r_bins); i++)
     {
-      size += sprintf(BLS, " %10ld", aic_dev->r_bins[i]);
+      seq_printf(m, " %10ld", aic_dev->r_bins[i]);
     }
-    size += sprintf(BLS, "\n");
-    size += sprintf(BLS, "  Writes:");
+    seq_printf(m, "\n");
+    seq_printf(m, "  Writes:");
     for (i = 0; i < ARRAY_SIZE(aic_dev->w_bins); i++)
     {
-      size += sprintf(BLS, " %10ld", aic_dev->w_bins[i]);
+      seq_printf(m, " %10ld", aic_dev->w_bins[i]);
     }
-    size += sprintf(BLS, "\n");
-    size += sprintf(BLS, "\n\n");
-  }
-  if (size >= aic7xxx_buffer_size)
-  {
-    printk(KERN_WARNING "aic7xxx: Overflow in aic7xxx_proc.c\n");
-  }
-
-  if (offset > size - 1)
-  {
-    kfree(aic7xxx_buffer);
-    aic7xxx_buffer = NULL;
-    aic7xxx_buffer_size = length = 0;
-    *start = NULL;
+    seq_printf(m, "\n");
+    seq_printf(m, "\n\n");
   }
-  else
-  {
-    *start = buffer;
-    length = min_t(int, length, size - offset);
-    memcpy(buffer, &aic7xxx_buffer[offset], length);
-  }
-
-  return (length);
+  return 0;
 }
 
 /*
diff --git a/drivers/scsi/arm/acornscsi.c b/drivers/scsi/arm/acornscsi.c
index 3e1172adb37b..09ba1869d366 100644
--- a/drivers/scsi/arm/acornscsi.c
+++ b/drivers/scsi/arm/acornscsi.c
@@ -2836,20 +2836,15 @@ char *acornscsi_info(struct Scsi_Host *host)
     return string;
 }
 
-int acornscsi_proc_info(struct Scsi_Host *instance, char *buffer, char **start, off_t offset,
-			int length, int inout)
+static int acornscsi_show_info(struct seq_file *m, struct Scsi_Host *instance)
 {
-    int pos, begin = 0, devidx;
+    int devidx;
     struct scsi_device *scd;
     AS_Host *host;
-    char *p = buffer;
-
-    if (inout == 1)
-	return -EINVAL;
 
     host  = (AS_Host *)instance->hostdata;
     
-    p += sprintf(p, "AcornSCSI driver v%d.%d.%d"
+    seq_printf(m, "AcornSCSI driver v%d.%d.%d"
 #ifdef CONFIG_SCSI_ACORNSCSI_SYNC
     " SYNC"
 #endif
@@ -2864,14 +2859,14 @@ int acornscsi_proc_info(struct Scsi_Host *instance, char *buffer, char **start,
 #endif
 		"\n\n", VER_MAJOR, VER_MINOR, VER_PATCH);
 
-    p += sprintf(p,	"SBIC: WD33C93A  Address: %p    IRQ : %d\n",
+    seq_printf(m,	"SBIC: WD33C93A  Address: %p    IRQ : %d\n",
 			host->base + SBIC_REGIDX, host->scsi.irq);
 #ifdef USE_DMAC
-    p += sprintf(p,	"DMAC: uPC71071  Address: %p  IRQ : %d\n\n",
+    seq_printf(m,	"DMAC: uPC71071  Address: %p  IRQ : %d\n\n",
 			host->base + DMAC_OFFSET, host->scsi.irq);
 #endif
 
-    p += sprintf(p,	"Statistics:\n"
+    seq_printf(m,	"Statistics:\n"
 			"Queued commands: %-10u    Issued commands: %-10u\n"
 			"Done commands  : %-10u    Reads          : %-10u\n"
 			"Writes         : %-10u    Others         : %-10u\n"
@@ -2886,7 +2881,7 @@ int acornscsi_proc_info(struct Scsi_Host *instance, char *buffer, char **start,
     for (devidx = 0; devidx < 9; devidx ++) {
 	unsigned int statptr, prev;
 
-	p += sprintf(p, "\n%c:", devidx == 8 ? 'H' : ('0' + devidx));
+	seq_printf(m, "\n%c:", devidx == 8 ? 'H' : ('0' + devidx));
 	statptr = host->status_ptr[devidx] - 10;
 
 	if ((signed int)statptr < 0)
@@ -2896,7 +2891,7 @@ int acornscsi_proc_info(struct Scsi_Host *instance, char *buffer, char **start,
 
 	for (; statptr != host->status_ptr[devidx]; statptr = (statptr + 1) & (STATUS_BUFFER_SIZE - 1)) {
 	    if (host->status[devidx][statptr].when) {
-		p += sprintf(p, "%c%02X:%02X+%2ld",
+		seq_printf(m, "%c%02X:%02X+%2ld",
 			host->status[devidx][statptr].irq ? '-' : ' ',
 			host->status[devidx][statptr].ph,
 			host->status[devidx][statptr].ssr,
@@ -2907,51 +2902,32 @@ int acornscsi_proc_info(struct Scsi_Host *instance, char *buffer, char **start,
 	}
     }
 
-    p += sprintf(p, "\nAttached devices:\n");
+    seq_printf(m, "\nAttached devices:\n");
 
     shost_for_each_device(scd, instance) {
-	p += sprintf(p, "Device/Lun TaggedQ      Sync\n");
-	p += sprintf(p, "     %d/%d   ", scd->id, scd->lun);
+	seq_printf(m, "Device/Lun TaggedQ      Sync\n");
+	seq_printf(m, "     %d/%d   ", scd->id, scd->lun);
 	if (scd->tagged_supported)
-		p += sprintf(p, "%3sabled(%3d) ",
+		seq_printf(m, "%3sabled(%3d) ",
 			     scd->simple_tags ? "en" : "dis",
 			     scd->current_tag);
 	else
-		p += sprintf(p, "unsupported  ");
+		seq_printf(m, "unsupported  ");
 
 	if (host->device[scd->id].sync_xfer & 15)
-		p += sprintf(p, "offset %d, %d ns\n",
+		seq_printf(m, "offset %d, %d ns\n",
 			     host->device[scd->id].sync_xfer & 15,
 			     acornscsi_getperiod(host->device[scd->id].sync_xfer));
 	else
-		p += sprintf(p, "async\n");
+		seq_printf(m, "async\n");
 
-	pos = p - buffer;
-	if (pos + begin < offset) {
-	    begin += pos;
-	    p = buffer;
-	}
-	pos = p - buffer;
-	if (pos + begin > offset + length) {
-	    scsi_device_put(scd);
-	    break;
-	}
     }
-
-    pos = p - buffer;
-
-    *start = buffer + (offset - begin);
-    pos -= offset - begin;
-
-    if (pos > length)
-	pos = length;
-
-    return pos;
+    return 0;
 }
 
 static struct scsi_host_template acornscsi_template = {
 	.module			= THIS_MODULE,
-	.proc_info		= acornscsi_proc_info,
+	.show_info		= acornscsi_show_info,
 	.name			= "AcornSCSI",
 	.info			= acornscsi_info,
 	.queuecommand		= acornscsi_queuecmd,
diff --git a/drivers/scsi/arm/arxescsi.c b/drivers/scsi/arm/arxescsi.c
index 9274510294ac..32d23212de48 100644
--- a/drivers/scsi/arm/arxescsi.c
+++ b/drivers/scsi/arm/arxescsi.c
@@ -220,47 +220,21 @@ static const char *arxescsi_info(struct Scsi_Host *host)
 	return string;
 }
 
-/*
- * Function: int arxescsi_proc_info(char *buffer, char **start, off_t offset,
- *					 int length, int host_no, int inout)
- * Purpose : Return information about the driver to a user process accessing
- *	     the /proc filesystem.
- * Params  : buffer - a buffer to write information to
- *	     start  - a pointer into this buffer set by this routine to the start
- *		      of the required information.
- *	     offset - offset into information that we have read up to.
- *	     length - length of buffer
- *	     host_no - host number to return information for
- *	     inout  - 0 for reading, 1 for writing.
- * Returns : length of data written to buffer.
- */
 static int
-arxescsi_proc_info(struct Scsi_Host *host, char *buffer, char **start, off_t offset, int length,
-		   int inout)
+arxescsi_show_info(struct seq_file *m, struct Scsi_Host *host)
 {
 	struct arxescsi_info *info;
-	char *p = buffer;
-	int pos;
-
 	info = (struct arxescsi_info *)host->hostdata;
-	if (inout == 1)
-		return -EINVAL;
-
-	p += sprintf(p, "ARXE 16-bit SCSI driver v%s\n", VERSION);
-	p += fas216_print_host(&info->info, p);
-	p += fas216_print_stats(&info->info, p);
-	p += fas216_print_devices(&info->info, p);
-
-	*start = buffer + offset;
-	pos = p - buffer - offset;
-	if (pos > length)
-		pos = length;
 
-	return pos;
+	seq_printf(m, "ARXE 16-bit SCSI driver v%s\n", VERSION);
+	fas216_print_host(&info->info, m);
+	fas216_print_stats(&info->info, m);
+	fas216_print_devices(&info->info, m);
+	return 0;
 }
 
 static struct scsi_host_template arxescsi_template = {
-	.proc_info			= arxescsi_proc_info,
+	.show_info			= arxescsi_show_info,
 	.name				= "ARXE SCSI card",
 	.info				= arxescsi_info,
 	.queuecommand			= fas216_noqueue_command,
diff --git a/drivers/scsi/arm/cumana_1.c b/drivers/scsi/arm/cumana_1.c
index c93938b246d5..b679778376c5 100644
--- a/drivers/scsi/arm/cumana_1.c
+++ b/drivers/scsi/arm/cumana_1.c
@@ -30,7 +30,6 @@
 #define NCR5380_write(reg, value)	cumanascsi_write(_instance, reg, value)
 #define NCR5380_intr			cumanascsi_intr
 #define NCR5380_queue_command		cumanascsi_queue_command
-#define NCR5380_proc_info		cumanascsi_proc_info
 
 #define NCR5380_implementation_fields	\
 	unsigned ctrl;			\
diff --git a/drivers/scsi/arm/cumana_2.c b/drivers/scsi/arm/cumana_2.c
index e3bae93c3c22..58915f29055b 100644
--- a/drivers/scsi/arm/cumana_2.c
+++ b/drivers/scsi/arm/cumana_2.c
@@ -337,50 +337,25 @@ cumanascsi_2_set_proc_info(struct Scsi_Host *host, char *buffer, int length)
 	return ret;
 }
 
-/* Prototype: int cumanascsi_2_proc_info(char *buffer, char **start, off_t offset,
- *					 int length, int host_no, int inout)
- * Purpose  : Return information about the driver to a user process accessing
- *	      the /proc filesystem.
- * Params   : buffer - a buffer to write information to
- *	      start  - a pointer into this buffer set by this routine to the start
- *		       of the required information.
- *	      offset - offset into information that we have read up to.
- *	      length - length of buffer
- *	      host_no - host number to return information for
- *	      inout  - 0 for reading, 1 for writing.
- * Returns  : length of data written to buffer.
- */
-int cumanascsi_2_proc_info (struct Scsi_Host *host, char *buffer, char **start, off_t offset,
-			    int length, int inout)
+static int cumanascsi_2_show_info(struct seq_file *m, struct Scsi_Host *host)
 {
 	struct cumanascsi2_info *info;
-	char *p = buffer;
-	int pos;
-
-	if (inout == 1)
-		return cumanascsi_2_set_proc_info(host, buffer, length);
-
 	info = (struct cumanascsi2_info *)host->hostdata;
 
-	p += sprintf(p, "Cumana SCSI II driver v%s\n", VERSION);
-	p += fas216_print_host(&info->info, p);
-	p += sprintf(p, "Term    : o%s\n",
+	seq_printf(m, "Cumana SCSI II driver v%s\n", VERSION);
+	fas216_print_host(&info->info, m);
+	seq_printf(m, "Term    : o%s\n",
 			info->terms ? "n" : "ff");
 
-	p += fas216_print_stats(&info->info, p);
-	p += fas216_print_devices(&info->info, p);
-
-	*start = buffer + offset;
-	pos = p - buffer - offset;
-	if (pos > length)
-		pos = length;
-
-	return pos;
+	fas216_print_stats(&info->info, m);
+	fas216_print_devices(&info->info, m);
+	return 0;
 }
 
 static struct scsi_host_template cumanascsi2_template = {
 	.module				= THIS_MODULE,
-	.proc_info			= cumanascsi_2_proc_info,
+	.show_info			= cumanascsi_2_show_info,
+	.write_info			= cumanascsi_2_set_proc_info,
 	.name				= "Cumana SCSI II",
 	.info				= cumanascsi_2_info,
 	.queuecommand			= fas216_queue_command,
diff --git a/drivers/scsi/arm/eesox.c b/drivers/scsi/arm/eesox.c
index 8e36908415ec..5bf3c0d134b4 100644
--- a/drivers/scsi/arm/eesox.c
+++ b/drivers/scsi/arm/eesox.c
@@ -422,45 +422,20 @@ eesoxscsi_set_proc_info(struct Scsi_Host *host, char *buffer, int length)
 	return ret;
 }
 
-/* Prototype: int eesoxscsi_proc_info(char *buffer, char **start, off_t offset,
- *				      int length, int host_no, int inout)
- * Purpose  : Return information about the driver to a user process accessing
- *	      the /proc filesystem.
- * Params   : buffer - a buffer to write information to
- *	      start  - a pointer into this buffer set by this routine to the start
- *		       of the required information.
- *	      offset - offset into information that we have read up to.
- *	      length - length of buffer
- *	      host_no - host number to return information for
- *	      inout  - 0 for reading, 1 for writing.
- * Returns  : length of data written to buffer.
- */
-int eesoxscsi_proc_info(struct Scsi_Host *host, char *buffer, char **start, off_t offset,
-			    int length, int inout)
+static int eesoxscsi_show_info(struct seq_file *m, struct Scsi_Host *host)
 {
 	struct eesoxscsi_info *info;
-	char *p = buffer;
-	int pos;
-
-	if (inout == 1)
-		return eesoxscsi_set_proc_info(host, buffer, length);
 
 	info = (struct eesoxscsi_info *)host->hostdata;
 
-	p += sprintf(p, "EESOX SCSI driver v%s\n", VERSION);
-	p += fas216_print_host(&info->info, p);
-	p += sprintf(p, "Term    : o%s\n",
+	seq_printf(m, "EESOX SCSI driver v%s\n", VERSION);
+	fas216_print_host(&info->info, m);
+	seq_printf(m, "Term    : o%s\n",
 			info->control & EESOX_TERM_ENABLE ? "n" : "ff");
 
-	p += fas216_print_stats(&info->info, p);
-	p += fas216_print_devices(&info->info, p);
-
-	*start = buffer + offset;
-	pos = p - buffer - offset;
-	if (pos > length)
-		pos = length;
-
-	return pos;
+	fas216_print_stats(&info->info, m);
+	fas216_print_devices(&info->info, m);
+	return 0;
 }
 
 static ssize_t eesoxscsi_show_term(struct device *dev, struct device_attribute *attr, char *buf)
@@ -498,7 +473,8 @@ static DEVICE_ATTR(bus_term, S_IRUGO | S_IWUSR,
 
 static struct scsi_host_template eesox_template = {
 	.module				= THIS_MODULE,
-	.proc_info			= eesoxscsi_proc_info,
+	.show_info			= eesoxscsi_show_info,
+	.write_info			= eesoxscsi_set_proc_info,
 	.name				= "EESOX SCSI",
 	.info				= eesoxscsi_info,
 	.queuecommand			= fas216_queue_command,
diff --git a/drivers/scsi/arm/fas216.c b/drivers/scsi/arm/fas216.c
index 737554c37d9e..b46a6f6c0eb3 100644
--- a/drivers/scsi/arm/fas216.c
+++ b/drivers/scsi/arm/fas216.c
@@ -2958,9 +2958,9 @@ void fas216_release(struct Scsi_Host *host)
 	queue_free(&info->queues.issue);
 }
 
-int fas216_print_host(FAS216_Info *info, char *buffer)
+void fas216_print_host(FAS216_Info *info, struct seq_file *m)
 {
-	return sprintf(buffer,
+	seq_printf(m,
 			"\n"
 			"Chip    : %s\n"
 			" Address: 0x%p\n"
@@ -2970,11 +2970,9 @@ int fas216_print_host(FAS216_Info *info, char *buffer)
 			info->scsi.irq, info->scsi.dma);
 }
 
-int fas216_print_stats(FAS216_Info *info, char *buffer)
+void fas216_print_stats(FAS216_Info *info, struct seq_file *m)
 {
-	char *p = buffer;
-
-	p += sprintf(p, "\n"
+	seq_printf(m, "\n"
 			"Command Statistics:\n"
 			" Queued     : %u\n"
 			" Issued     : %u\n"
@@ -2991,38 +2989,33 @@ int fas216_print_stats(FAS216_Info *info, char *buffer)
 			info->stats.writes,	 info->stats.miscs,
 			info->stats.disconnects, info->stats.aborts,
 			info->stats.bus_resets,	 info->stats.host_resets);
-
-	return p - buffer;
 }
 
-int fas216_print_devices(FAS216_Info *info, char *buffer)
+void fas216_print_devices(FAS216_Info *info, struct seq_file *m)
 {
 	struct fas216_device *dev;
 	struct scsi_device *scd;
-	char *p = buffer;
 
-	p += sprintf(p, "Device/Lun TaggedQ       Parity   Sync\n");
+	seq_printf(m, "Device/Lun TaggedQ       Parity   Sync\n");
 
 	shost_for_each_device(scd, info->host) {
 		dev = &info->device[scd->id];
-		p += sprintf(p, "     %d/%d   ", scd->id, scd->lun);
+		seq_printf(m, "     %d/%d   ", scd->id, scd->lun);
 		if (scd->tagged_supported)
-			p += sprintf(p, "%3sabled(%3d) ",
+			seq_printf(m, "%3sabled(%3d) ",
 				     scd->simple_tags ? "en" : "dis",
 				     scd->current_tag);
 		else
-			p += sprintf(p, "unsupported   ");
+			seq_printf(m, "unsupported   ");
 
-		p += sprintf(p, "%3sabled ", dev->parity_enabled ? "en" : "dis");
+		seq_printf(m, "%3sabled ", dev->parity_enabled ? "en" : "dis");
 
 		if (dev->sof)
-			p += sprintf(p, "offset %d, %d ns\n",
+			seq_printf(m, "offset %d, %d ns\n",
 				     dev->sof, dev->period * 4);
 		else
-			p += sprintf(p, "async\n");
+			seq_printf(m, "async\n");
 	}
-
-	return p - buffer;
 }
 
 EXPORT_SYMBOL(fas216_init);
diff --git a/drivers/scsi/arm/fas216.h b/drivers/scsi/arm/fas216.h
index df2e1b3ddfe2..c57c16ef8193 100644
--- a/drivers/scsi/arm/fas216.h
+++ b/drivers/scsi/arm/fas216.h
@@ -358,9 +358,9 @@ extern void fas216_remove (struct Scsi_Host *instance);
  */
 extern void fas216_release (struct Scsi_Host *instance);
 
-extern int fas216_print_host(FAS216_Info *info, char *buffer);
-extern int fas216_print_stats(FAS216_Info *info, char *buffer);
-extern int fas216_print_devices(FAS216_Info *info, char *buffer);
+extern void fas216_print_host(FAS216_Info *info, struct seq_file *m);
+extern void fas216_print_stats(FAS216_Info *info, struct seq_file *m);
+extern void fas216_print_devices(FAS216_Info *info, struct seq_file *m);
 
 /* Function: int fas216_eh_abort(struct scsi_cmnd *SCpnt)
  * Purpose : abort this command
diff --git a/drivers/scsi/arm/oak.c b/drivers/scsi/arm/oak.c
index 48facdc18002..4266eef8aca1 100644
--- a/drivers/scsi/arm/oak.c
+++ b/drivers/scsi/arm/oak.c
@@ -31,7 +31,8 @@
 #define NCR5380_write(reg, value)	writeb(value, _base + ((reg) << 2))
 #define NCR5380_intr			oakscsi_intr
 #define NCR5380_queue_command		oakscsi_queue_command
-#define NCR5380_proc_info		oakscsi_proc_info
+#define NCR5380_show_info		oakscsi_show_info
+#define NCR5380_write_info		oakscsi_write_info
 
 #define NCR5380_implementation_fields	\
 	void __iomem *base
@@ -115,7 +116,8 @@ printk("reading %p len %d\n", addr, len);
 
 static struct scsi_host_template oakscsi_template = {
 	.module			= THIS_MODULE,
-	.proc_info		= oakscsi_proc_info,
+	.show_info		= oakscsi_show_info,
+	.write_info		= oakscsi_write_info,
 	.name			= "Oak 16-bit SCSI",
 	.info			= oakscsi_info,
 	.queuecommand		= oakscsi_queue_command,
diff --git a/drivers/scsi/arm/powertec.c b/drivers/scsi/arm/powertec.c
index 246600b93555..abc9593615e9 100644
--- a/drivers/scsi/arm/powertec.c
+++ b/drivers/scsi/arm/powertec.c
@@ -237,32 +237,20 @@ powertecscsi_set_proc_info(struct Scsi_Host *host, char *buffer, int length)
  *	      inout   - 0 for reading, 1 for writing.
  * Returns  : length of data written to buffer.
  */
-int powertecscsi_proc_info(struct Scsi_Host *host, char *buffer, char **start, off_t offset,
-			    int length, int inout)
+static int powertecscsi_show_info(struct seq_file *m, struct Scsi_Host *host)
 {
 	struct powertec_info *info;
-	char *p = buffer;
-	int pos;
-
-	if (inout == 1)
-		return powertecscsi_set_proc_info(host, buffer, length);
 
 	info = (struct powertec_info *)host->hostdata;
 
-	p += sprintf(p, "PowerTec SCSI driver v%s\n", VERSION);
-	p += fas216_print_host(&info->info, p);
-	p += sprintf(p, "Term    : o%s\n",
+	seq_printf(m, "PowerTec SCSI driver v%s\n", VERSION);
+	fas216_print_host(&info->info, m);
+	seq_printf(m, "Term    : o%s\n",
 			info->term_ctl ? "n" : "ff");
 
-	p += fas216_print_stats(&info->info, p);
-	p += fas216_print_devices(&info->info, p);
-
-	*start = buffer + offset;
-	pos = p - buffer - offset;
-	if (pos > length)
-		pos = length;
-
-	return pos;
+	fas216_print_stats(&info->info, m);
+	fas216_print_devices(&info->info, m);
+	return 0;
 }
 
 static ssize_t powertecscsi_show_term(struct device *dev, struct device_attribute *attr, char *buf)
@@ -291,7 +279,8 @@ static DEVICE_ATTR(bus_term, S_IRUGO | S_IWUSR,
 
 static struct scsi_host_template powertecscsi_template = {
 	.module				= THIS_MODULE,
-	.proc_info			= powertecscsi_proc_info,
+	.show_info			= powertecscsi_show_info,
+	.write_info			= powertecscsi_set_proc_info,
 	.name				= "PowerTec SCSI",
 	.info				= powertecscsi_info,
 	.queuecommand			= fas216_queue_command,
diff --git a/drivers/scsi/atari_NCR5380.c b/drivers/scsi/atari_NCR5380.c
index 2db79b469d9e..0f3cdbc80ba6 100644
--- a/drivers/scsi/atari_NCR5380.c
+++ b/drivers/scsi/atari_NCR5380.c
@@ -719,119 +719,94 @@ static void __init NCR5380_print_options(struct Scsi_Host *instance)
  * Inputs : instance, pointer to this instance.
  */
 
-static void NCR5380_print_status(struct Scsi_Host *instance)
+static void lprint_Scsi_Cmnd(Scsi_Cmnd *cmd)
 {
-	char *pr_bfr;
-	char *start;
-	int len;
-
-	NCR_PRINT(NDEBUG_ANY);
-	NCR_PRINT_PHASE(NDEBUG_ANY);
-
-	pr_bfr = (char *)__get_free_page(GFP_ATOMIC);
-	if (!pr_bfr) {
-		printk("NCR5380_print_status: no memory for print buffer\n");
-		return;
-	}
-	len = NCR5380_proc_info(instance, pr_bfr, &start, 0, PAGE_SIZE, 0);
-	pr_bfr[len] = 0;
-	printk("\n%s\n", pr_bfr);
-	free_page((unsigned long)pr_bfr);
+	int i, s;
+	unsigned char *command;
+	printk("scsi%d: destination target %d, lun %d\n",
+		H_NO(cmd), cmd->device->id, cmd->device->lun);
+	printk(KERN_CONT "        command = ");
+	command = cmd->cmnd;
+	printk(KERN_CONT "%2d (0x%02x)", command[0], command[0]);
+	for (i = 1, s = COMMAND_SIZE(command[0]); i < s; ++i)
+		printk(KERN_CONT " %02x", command[i]);
+	printk("\n");
 }
 
-
-/******************************************/
-/*
- * /proc/scsi/[dtc pas16 t128 generic]/[0-ASC_NUM_BOARD_SUPPORTED]
- *
- * *buffer: I/O buffer
- * **start: if inout == FALSE pointer into buffer where user read should start
- * offset: current offset
- * length: length of buffer
- * hostno: Scsi_Host host_no
- * inout: TRUE - user is writing; FALSE - user is reading
- *
- * Return the number of bytes read from or written
-*/
-
-#undef SPRINTF
-#define SPRINTF(fmt,args...)							\
-	do {									\
-		if (pos + strlen(fmt) + 20 /* slop */ < buffer + length)	\
-			pos += sprintf(pos, fmt , ## args);			\
-	} while(0)
-static char *lprint_Scsi_Cmnd(Scsi_Cmnd *cmd, char *pos, char *buffer, int length);
-
-static int NCR5380_proc_info(struct Scsi_Host *instance, char *buffer,
-			     char **start, off_t offset, int length, int inout)
+static void NCR5380_print_status(struct Scsi_Host *instance)
 {
-	char *pos = buffer;
 	struct NCR5380_hostdata *hostdata;
 	Scsi_Cmnd *ptr;
 	unsigned long flags;
-	off_t begin = 0;
-#define check_offset()					\
-	do {						\
-		if (pos - buffer < offset - begin) {	\
-			begin += pos - buffer;		\
-			pos = buffer;			\
-		}					\
-	} while (0)
+
+	NCR_PRINT(NDEBUG_ANY);
+	NCR_PRINT_PHASE(NDEBUG_ANY);
 
 	hostdata = (struct NCR5380_hostdata *)instance->hostdata;
 
-	if (inout)			/* Has data been written to the file ? */
-		return -ENOSYS;		/* Currently this is a no-op */
-	SPRINTF("NCR5380 core release=%d.\n", NCR5380_PUBLIC_RELEASE);
-	check_offset();
+	printk("\nNCR5380 core release=%d.\n", NCR5380_PUBLIC_RELEASE);
 	local_irq_save(flags);
-	SPRINTF("NCR5380: coroutine is%s running.\n",
+	printk("NCR5380: coroutine is%s running.\n",
 		main_running ? "" : "n't");
-	check_offset();
 	if (!hostdata->connected)
-		SPRINTF("scsi%d: no currently connected command\n", HOSTNO);
+		printk("scsi%d: no currently connected command\n", HOSTNO);
 	else
-		pos = lprint_Scsi_Cmnd((Scsi_Cmnd *) hostdata->connected,
-				       pos, buffer, length);
-	SPRINTF("scsi%d: issue_queue\n", HOSTNO);
-	check_offset();
-	for (ptr = (Scsi_Cmnd *)hostdata->issue_queue; ptr; ptr = NEXT(ptr)) {
-		pos = lprint_Scsi_Cmnd(ptr, pos, buffer, length);
-		check_offset();
-	}
+		lprint_Scsi_Cmnd((Scsi_Cmnd *) hostdata->connected);
+	printk("scsi%d: issue_queue\n", HOSTNO);
+	for (ptr = (Scsi_Cmnd *)hostdata->issue_queue; ptr; ptr = NEXT(ptr))
+		lprint_Scsi_Cmnd(ptr);
 
-	SPRINTF("scsi%d: disconnected_queue\n", HOSTNO);
-	check_offset();
+	printk("scsi%d: disconnected_queue\n", HOSTNO);
 	for (ptr = (Scsi_Cmnd *) hostdata->disconnected_queue; ptr;
-	     ptr = NEXT(ptr)) {
-		pos = lprint_Scsi_Cmnd(ptr, pos, buffer, length);
-		check_offset();
-	}
+	     ptr = NEXT(ptr))
+		lprint_Scsi_Cmnd(ptr);
 
 	local_irq_restore(flags);
-	*start = buffer + (offset - begin);
-	if (pos - buffer < offset - begin)
-		return 0;
-	else if (pos - buffer - (offset - begin) < length)
-		return pos - buffer - (offset - begin);
-	return length;
+	printk("\n");
 }
 
-static char *lprint_Scsi_Cmnd(Scsi_Cmnd *cmd, char *pos, char *buffer, int length)
+static void show_Scsi_Cmnd(Scsi_Cmnd *cmd, struct seq_file *m)
 {
 	int i, s;
 	unsigned char *command;
-	SPRINTF("scsi%d: destination target %d, lun %d\n",
+	seq_printf(m, "scsi%d: destination target %d, lun %d\n",
 		H_NO(cmd), cmd->device->id, cmd->device->lun);
-	SPRINTF("        command = ");
+	seq_printf(m, "        command = ");
 	command = cmd->cmnd;
-	SPRINTF("%2d (0x%02x)", command[0], command[0]);
+	seq_printf(m, "%2d (0x%02x)", command[0], command[0]);
 	for (i = 1, s = COMMAND_SIZE(command[0]); i < s; ++i)
-		SPRINTF(" %02x", command[i]);
-	SPRINTF("\n");
-	return pos;
+		seq_printf(m, " %02x", command[i]);
+	seq_printf(m, "\n");
 }
 
+static int NCR5380_show_info(struct seq_file *m, struct Scsi_Host *instance)
+{
+	struct NCR5380_hostdata *hostdata;
+	Scsi_Cmnd *ptr;
+	unsigned long flags;
+
+	hostdata = (struct NCR5380_hostdata *)instance->hostdata;
+
+	seq_printf(m, "NCR5380 core release=%d.\n", NCR5380_PUBLIC_RELEASE);
+	local_irq_save(flags);
+	seq_printf(m, "NCR5380: coroutine is%s running.\n",
+		main_running ? "" : "n't");
+	if (!hostdata->connected)
+		seq_printf(m, "scsi%d: no currently connected command\n", HOSTNO);
+	else
+		show_Scsi_Cmnd((Scsi_Cmnd *) hostdata->connected, m);
+	seq_printf(m, "scsi%d: issue_queue\n", HOSTNO);
+	for (ptr = (Scsi_Cmnd *)hostdata->issue_queue; ptr; ptr = NEXT(ptr))
+		show_Scsi_Cmnd(ptr, m);
+
+	seq_printf(m, "scsi%d: disconnected_queue\n", HOSTNO);
+	for (ptr = (Scsi_Cmnd *) hostdata->disconnected_queue; ptr;
+	     ptr = NEXT(ptr))
+		show_Scsi_Cmnd(ptr, m);
+
+	local_irq_restore(flags);
+	return 0;
+}
 
 /*
  * Function : void NCR5380_init (struct Scsi_Host *instance)
diff --git a/drivers/scsi/atari_scsi.c b/drivers/scsi/atari_scsi.c
index df740cbbaef4..a3e6c8a3ff0f 100644
--- a/drivers/scsi/atari_scsi.c
+++ b/drivers/scsi/atari_scsi.c
@@ -1100,7 +1100,7 @@ static void atari_scsi_falcon_reg_write(unsigned char reg, unsigned char value)
 #include "atari_NCR5380.c"
 
 static struct scsi_host_template driver_template = {
-	.proc_info		= atari_scsi_proc_info,
+	.show_info		= atari_scsi_show_info,
 	.name			= "Atari native SCSI",
 	.detect			= atari_scsi_detect,
 	.release		= atari_scsi_release,
diff --git a/drivers/scsi/atari_scsi.h b/drivers/scsi/atari_scsi.h
index bd52df78b209..11c624bb122d 100644
--- a/drivers/scsi/atari_scsi.h
+++ b/drivers/scsi/atari_scsi.h
@@ -47,7 +47,7 @@
 #define NCR5380_intr atari_scsi_intr
 #define NCR5380_queue_command atari_scsi_queue_command
 #define NCR5380_abort atari_scsi_abort
-#define NCR5380_proc_info atari_scsi_proc_info
+#define NCR5380_show_info atari_scsi_show_info
 #define NCR5380_dma_read_setup(inst,d,c) atari_scsi_dma_setup (inst, d, c, 0)
 #define NCR5380_dma_write_setup(inst,d,c) atari_scsi_dma_setup (inst, d, c, 1)
 #define NCR5380_dma_residual(inst) atari_scsi_dma_residual( inst )
diff --git a/drivers/scsi/atp870u.c b/drivers/scsi/atp870u.c
index cfc73041f102..15a629d8ed08 100644
--- a/drivers/scsi/atp870u.c
+++ b/drivers/scsi/atp870u.c
@@ -3099,38 +3099,14 @@ static const char *atp870u_info(struct Scsi_Host *notused)
 	return buffer;
 }
 
-#define BLS buffer + len + size
-static int atp870u_proc_info(struct Scsi_Host *HBAptr, char *buffer, 
-			     char **start, off_t offset, int length, int inout)
+static int atp870u_show_info(struct seq_file *m, struct Scsi_Host *HBAptr)
 {
-	static u8 buff[512];
-	int size = 0;
-	int len = 0;
-	off_t begin = 0;
-	off_t pos = 0;
-	
-	if (inout) 	
-		return -EINVAL;
-	if (offset == 0)
-		memset(buff, 0, sizeof(buff));
-	size += sprintf(BLS, "ACARD AEC-671X Driver Version: 2.6+ac\n");
-	len += size;
-	pos = begin + len;
-	size = 0;
-
-	size += sprintf(BLS, "\n");
-	size += sprintf(BLS, "Adapter Configuration:\n");
-	size += sprintf(BLS, "               Base IO: %#.4lx\n", HBAptr->io_port);
-	size += sprintf(BLS, "                   IRQ: %d\n", HBAptr->irq);
-	len += size;
-	pos = begin + len;
-	
-	*start = buffer + (offset - begin);	/* Start of wanted data */
-	len -= (offset - begin);	/* Start slop */
-	if (len > length) {
-		len = length;	/* Ending slop */
-	}
-	return (len);
+	seq_printf(m, "ACARD AEC-671X Driver Version: 2.6+ac\n");
+	seq_printf(m, "\n");
+	seq_printf(m, "Adapter Configuration:\n");
+	seq_printf(m, "               Base IO: %#.4lx\n", HBAptr->io_port);
+	seq_printf(m, "                   IRQ: %d\n", HBAptr->irq);
+	return 0;
 }
 
 
@@ -3177,7 +3153,7 @@ static struct scsi_host_template atp870u_template = {
      .module			= THIS_MODULE,
      .name              	= "atp870u"		/* name */,
      .proc_name			= "atp870u",
-     .proc_info			= atp870u_proc_info,
+     .show_info			= atp870u_show_info,
      .info              	= atp870u_info		/* info */,
      .queuecommand      	= atp870u_queuecommand	/* queuecommand */,
      .eh_abort_handler  	= atp870u_abort		/* abort */,
diff --git a/drivers/scsi/bnx2fc/bnx2fc.h b/drivers/scsi/bnx2fc/bnx2fc.h
index 50fcd018d14b..11596b2c4702 100644
--- a/drivers/scsi/bnx2fc/bnx2fc.h
+++ b/drivers/scsi/bnx2fc/bnx2fc.h
@@ -88,9 +88,6 @@
 
 #define BNX2FC_MAX_NPIV		256
 
-#define BNX2FC_MAX_OUTSTANDING_CMNDS	2048
-#define BNX2FC_CAN_QUEUE		BNX2FC_MAX_OUTSTANDING_CMNDS
-#define BNX2FC_ELSTM_XIDS		BNX2FC_CAN_QUEUE
 #define BNX2FC_MIN_PAYLOAD		256
 #define BNX2FC_MAX_PAYLOAD		2048
 #define BNX2FC_MFS			\
@@ -108,11 +105,8 @@
 #define BNX2FC_CONFQ_WQE_SIZE		(sizeof(struct fcoe_confqe))
 #define BNX2FC_5771X_DB_PAGE_SIZE	128
 
-#define BNX2FC_MAX_TASKS		\
-			     (BNX2FC_MAX_OUTSTANDING_CMNDS + BNX2FC_ELSTM_XIDS)
 #define BNX2FC_TASK_SIZE		128
 #define	BNX2FC_TASKS_PER_PAGE		(PAGE_SIZE/BNX2FC_TASK_SIZE)
-#define BNX2FC_TASK_CTX_ARR_SZ		(BNX2FC_MAX_TASKS/BNX2FC_TASKS_PER_PAGE)
 
 #define BNX2FC_MAX_ROWS_IN_HASH_TBL	8
 #define BNX2FC_HASH_TBL_CHUNK_SIZE	(16 * 1024)
@@ -125,12 +119,9 @@
 #define BNX2FC_WRITE			(1 << 0)
 
 #define BNX2FC_MIN_XID			0
-#define BNX2FC_MAX_XID			\
-			(BNX2FC_MAX_OUTSTANDING_CMNDS + BNX2FC_ELSTM_XIDS - 1)
 #define FCOE_MAX_NUM_XIDS		0x2000
-#define FCOE_MIN_XID			(BNX2FC_MAX_XID + 1)
-#define FCOE_MAX_XID			(FCOE_MIN_XID + FCOE_MAX_NUM_XIDS - 1)
-#define FCOE_XIDS_PER_CPU		(FCOE_MIN_XID + (512 * nr_cpu_ids) - 1)
+#define FCOE_MAX_XID_OFFSET		(FCOE_MAX_NUM_XIDS - 1)
+#define FCOE_XIDS_PER_CPU_OFFSET	((512 * nr_cpu_ids) - 1)
 #define BNX2FC_MAX_LUN			0xFFFF
 #define BNX2FC_MAX_FCP_TGT		256
 #define BNX2FC_MAX_CMD_LEN		16
@@ -206,6 +197,13 @@ struct bnx2fc_hba {
 		#define BNX2FC_FLAG_FW_INIT_DONE	0
 		#define BNX2FC_FLAG_DESTROY_CMPL	1
 	u32 next_conn_id;
+
+	/* xid resources */
+	u16 max_xid;
+	u32 max_tasks;
+	u32 max_outstanding_cmds;
+	u32 elstm_xids;
+
 	struct fcoe_task_ctx_entry **task_ctx;
 	dma_addr_t *task_ctx_dma;
 	struct regpair *task_ctx_bd_tbl;
@@ -504,8 +502,7 @@ int bnx2fc_setup_task_ctx(struct bnx2fc_hba *hba);
 void bnx2fc_free_task_ctx(struct bnx2fc_hba *hba);
 int bnx2fc_setup_fw_resc(struct bnx2fc_hba *hba);
 void bnx2fc_free_fw_resc(struct bnx2fc_hba *hba);
-struct bnx2fc_cmd_mgr *bnx2fc_cmd_mgr_alloc(struct bnx2fc_hba *hba,
-						u16 min_xid, u16 max_xid);
+struct bnx2fc_cmd_mgr *bnx2fc_cmd_mgr_alloc(struct bnx2fc_hba *hba);
 void bnx2fc_cmd_mgr_free(struct bnx2fc_cmd_mgr *cmgr);
 void bnx2fc_get_link_state(struct bnx2fc_hba *hba);
 char *bnx2fc_get_next_rqe(struct bnx2fc_rport *tgt, u8 num_items);
diff --git a/drivers/scsi/bnx2fc/bnx2fc_fcoe.c b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
index 90bc7bd00966..7dffec1e5715 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
+++ b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
@@ -71,7 +71,7 @@ static void bnx2fc_recv_frame(struct sk_buff *skb);
 static void bnx2fc_start_disc(struct bnx2fc_interface *interface);
 static int bnx2fc_shost_config(struct fc_lport *lport, struct device *dev);
 static int bnx2fc_lport_config(struct fc_lport *lport);
-static int bnx2fc_em_config(struct fc_lport *lport);
+static int bnx2fc_em_config(struct fc_lport *lport, struct bnx2fc_hba *hba);
 static int bnx2fc_bind_adapter_devices(struct bnx2fc_hba *hba);
 static void bnx2fc_unbind_adapter_devices(struct bnx2fc_hba *hba);
 static int bnx2fc_bind_pcidev(struct bnx2fc_hba *hba);
@@ -944,16 +944,17 @@ static int bnx2fc_libfc_config(struct fc_lport *lport)
 	return 0;
 }
 
-static int bnx2fc_em_config(struct fc_lport *lport)
+static int bnx2fc_em_config(struct fc_lport *lport, struct bnx2fc_hba *hba)
 {
-	int max_xid;
+	int fcoe_min_xid, fcoe_max_xid;
 
+	fcoe_min_xid = hba->max_xid + 1;
 	if (nr_cpu_ids <= 2)
-		max_xid = FCOE_XIDS_PER_CPU;
+		fcoe_max_xid = hba->max_xid + FCOE_XIDS_PER_CPU_OFFSET;
 	else
-		max_xid = FCOE_MAX_XID;
-	if (!fc_exch_mgr_alloc(lport, FC_CLASS_3, FCOE_MIN_XID,
-				max_xid, NULL)) {
+		fcoe_max_xid = hba->max_xid + FCOE_MAX_XID_OFFSET;
+	if (!fc_exch_mgr_alloc(lport, FC_CLASS_3, fcoe_min_xid,
+			       fcoe_max_xid, NULL)) {
 		printk(KERN_ERR PFX "em_config:fc_exch_mgr_alloc failed\n");
 		return -ENOMEM;
 	}
@@ -1300,6 +1301,12 @@ static struct bnx2fc_hba *bnx2fc_hba_create(struct cnic_dev *cnic)
 	mutex_init(&hba->hba_mutex);
 
 	hba->cnic = cnic;
+
+	hba->max_tasks = cnic->max_fcoe_exchanges;
+	hba->elstm_xids = (hba->max_tasks / 2);
+	hba->max_outstanding_cmds = hba->elstm_xids;
+	hba->max_xid = (hba->max_tasks - 1);
+
 	rc = bnx2fc_bind_pcidev(hba);
 	if (rc) {
 		printk(KERN_ERR PFX "create_adapter:  bind error\n");
@@ -1318,8 +1325,7 @@ static struct bnx2fc_hba *bnx2fc_hba_create(struct cnic_dev *cnic)
 
 	hba->num_ofld_sess = 0;
 
-	hba->cmd_mgr = bnx2fc_cmd_mgr_alloc(hba, BNX2FC_MIN_XID,
-						BNX2FC_MAX_XID);
+	hba->cmd_mgr = bnx2fc_cmd_mgr_alloc(hba);
 	if (!hba->cmd_mgr) {
 		printk(KERN_ERR PFX "em_config:bnx2fc_cmd_mgr_alloc failed\n");
 		goto cmgr_err;
@@ -1330,13 +1336,13 @@ static struct bnx2fc_hba *bnx2fc_hba_create(struct cnic_dev *cnic)
 					FCOE_IOS_PER_CONNECTION_SHIFT;
 	fcoe_cap->capability1 |= BNX2FC_NUM_MAX_SESS <<
 					FCOE_LOGINS_PER_PORT_SHIFT;
-	fcoe_cap->capability2 = BNX2FC_MAX_OUTSTANDING_CMNDS <<
+	fcoe_cap->capability2 = hba->max_outstanding_cmds <<
 					FCOE_NUMBER_OF_EXCHANGES_SHIFT;
 	fcoe_cap->capability2 |= BNX2FC_MAX_NPIV <<
 					FCOE_NPIV_WWN_PER_PORT_SHIFT;
 	fcoe_cap->capability3 = BNX2FC_NUM_MAX_SESS <<
 					FCOE_TARGETS_SUPPORTED_SHIFT;
-	fcoe_cap->capability3 |= BNX2FC_MAX_OUTSTANDING_CMNDS <<
+	fcoe_cap->capability3 |= hba->max_outstanding_cmds <<
 					FCOE_OUTSTANDING_COMMANDS_SHIFT;
 	fcoe_cap->capability4 = FCOE_CAPABILITY4_STATEFUL;
 
@@ -1416,7 +1422,7 @@ static struct fc_lport *bnx2fc_if_create(struct bnx2fc_interface *interface,
 	struct Scsi_Host	*shost;
 	struct fc_vport		*vport = dev_to_vport(parent);
 	struct bnx2fc_lport	*blport;
-	struct bnx2fc_hba	*hba;
+	struct bnx2fc_hba	*hba = interface->hba;
 	int			rc = 0;
 
 	blport = kzalloc(sizeof(struct bnx2fc_lport), GFP_KERNEL);
@@ -1426,6 +1432,7 @@ static struct fc_lport *bnx2fc_if_create(struct bnx2fc_interface *interface,
 	}
 
 	/* Allocate Scsi_Host structure */
+	bnx2fc_shost_template.can_queue = hba->max_outstanding_cmds;
 	if (!npiv)
 		lport = libfc_host_alloc(&bnx2fc_shost_template, sizeof(*port));
 	else
@@ -1477,7 +1484,7 @@ static struct fc_lport *bnx2fc_if_create(struct bnx2fc_interface *interface,
 
 	/* Allocate exchange manager */
 	if (!npiv)
-		rc = bnx2fc_em_config(lport);
+		rc = bnx2fc_em_config(lport, hba);
 	else {
 		shost = vport_to_shost(vport);
 		n_port = shost_priv(shost);
@@ -1491,7 +1498,6 @@ static struct fc_lport *bnx2fc_if_create(struct bnx2fc_interface *interface,
 
 	bnx2fc_interface_get(interface);
 
-	hba = interface->hba;
 	spin_lock_bh(&hba->hba_lock);
 	blport->lport = lport;
 	list_add_tail(&blport->list, &hba->vports);
@@ -2706,7 +2712,6 @@ static struct scsi_host_template bnx2fc_shost_template = {
 	.change_queue_type	= fc_change_queue_type,
 	.this_id		= -1,
 	.cmd_per_lun		= 3,
-	.can_queue		= BNX2FC_CAN_QUEUE,
 	.use_clustering		= ENABLE_CLUSTERING,
 	.sg_tablesize		= BNX2FC_MAX_BDS_PER_CMD,
 	.max_sectors		= 1024,
diff --git a/drivers/scsi/bnx2fc/bnx2fc_hwi.c b/drivers/scsi/bnx2fc/bnx2fc_hwi.c
index 85ea98a80f40..50510ffe1bf5 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_hwi.c
+++ b/drivers/scsi/bnx2fc/bnx2fc_hwi.c
@@ -77,7 +77,7 @@ int bnx2fc_send_fw_fcoe_init_msg(struct bnx2fc_hba *hba)
 	fcoe_init1.hdr.flags = (FCOE_KWQE_LAYER_CODE <<
 					FCOE_KWQE_HEADER_LAYER_CODE_SHIFT);
 
-	fcoe_init1.num_tasks = BNX2FC_MAX_TASKS;
+	fcoe_init1.num_tasks = hba->max_tasks;
 	fcoe_init1.sq_num_wqes = BNX2FC_SQ_WQES_MAX;
 	fcoe_init1.rq_num_wqes = BNX2FC_RQ_WQES_MAX;
 	fcoe_init1.rq_buffer_log_size = BNX2FC_RQ_BUF_LOG_SZ;
@@ -697,7 +697,7 @@ static void bnx2fc_process_unsol_compl(struct bnx2fc_rport *tgt, u16 wqe)
 			err_entry->data.tx_buf_off, err_entry->data.rx_buf_off);
 
 
-		if (xid > BNX2FC_MAX_XID) {
+		if (xid > hba->max_xid) {
 			BNX2FC_TGT_DBG(tgt, "xid(0x%x) out of FW range\n",
 				   xid);
 			goto ret_err_rqe;
@@ -815,7 +815,7 @@ ret_err_rqe:
 		BNX2FC_TGT_DBG(tgt, "buf_offsets - tx = 0x%x, rx = 0x%x",
 			err_entry->data.tx_buf_off, err_entry->data.rx_buf_off);
 
-		if (xid > BNX2FC_MAX_XID) {
+		if (xid > hba->max_xid) {
 			BNX2FC_TGT_DBG(tgt, "xid(0x%x) out of FW range\n", xid);
 			goto ret_warn_rqe;
 		}
@@ -880,7 +880,7 @@ void bnx2fc_process_cq_compl(struct bnx2fc_rport *tgt, u16 wqe)
 
 	spin_lock_bh(&tgt->tgt_lock);
 	xid = wqe & FCOE_PEND_WQ_CQE_TASK_ID;
-	if (xid >= BNX2FC_MAX_TASKS) {
+	if (xid >= hba->max_tasks) {
 		printk(KERN_ERR PFX "ERROR:xid out of range\n");
 		spin_unlock_bh(&tgt->tgt_lock);
 		return;
@@ -1842,6 +1842,7 @@ int bnx2fc_setup_task_ctx(struct bnx2fc_hba *hba)
 	int rc = 0;
 	struct regpair *task_ctx_bdt;
 	dma_addr_t addr;
+	int task_ctx_arr_sz;
 	int i;
 
 	/*
@@ -1865,7 +1866,8 @@ int bnx2fc_setup_task_ctx(struct bnx2fc_hba *hba)
 	 * Allocate task_ctx which is an array of pointers pointing to
 	 * a page containing 32 task contexts
 	 */
-	hba->task_ctx = kzalloc((BNX2FC_TASK_CTX_ARR_SZ * sizeof(void *)),
+	task_ctx_arr_sz = (hba->max_tasks / BNX2FC_TASKS_PER_PAGE);
+	hba->task_ctx = kzalloc((task_ctx_arr_sz * sizeof(void *)),
 				 GFP_KERNEL);
 	if (!hba->task_ctx) {
 		printk(KERN_ERR PFX "unable to allocate task context array\n");
@@ -1876,7 +1878,7 @@ int bnx2fc_setup_task_ctx(struct bnx2fc_hba *hba)
 	/*
 	 * Allocate task_ctx_dma which is an array of dma addresses
 	 */
-	hba->task_ctx_dma = kmalloc((BNX2FC_TASK_CTX_ARR_SZ *
+	hba->task_ctx_dma = kmalloc((task_ctx_arr_sz *
 					sizeof(dma_addr_t)), GFP_KERNEL);
 	if (!hba->task_ctx_dma) {
 		printk(KERN_ERR PFX "unable to alloc context mapping array\n");
@@ -1885,7 +1887,7 @@ int bnx2fc_setup_task_ctx(struct bnx2fc_hba *hba)
 	}
 
 	task_ctx_bdt = (struct regpair *)hba->task_ctx_bd_tbl;
-	for (i = 0; i < BNX2FC_TASK_CTX_ARR_SZ; i++) {
+	for (i = 0; i < task_ctx_arr_sz; i++) {
 
 		hba->task_ctx[i] = dma_alloc_coherent(&hba->pcidev->dev,
 						      PAGE_SIZE,
@@ -1905,7 +1907,7 @@ int bnx2fc_setup_task_ctx(struct bnx2fc_hba *hba)
 	return 0;
 
 out3:
-	for (i = 0; i < BNX2FC_TASK_CTX_ARR_SZ; i++) {
+	for (i = 0; i < task_ctx_arr_sz; i++) {
 		if (hba->task_ctx[i]) {
 
 			dma_free_coherent(&hba->pcidev->dev, PAGE_SIZE,
@@ -1929,6 +1931,7 @@ out:
 
 void bnx2fc_free_task_ctx(struct bnx2fc_hba *hba)
 {
+	int task_ctx_arr_sz;
 	int i;
 
 	if (hba->task_ctx_bd_tbl) {
@@ -1938,8 +1941,9 @@ void bnx2fc_free_task_ctx(struct bnx2fc_hba *hba)
 		hba->task_ctx_bd_tbl = NULL;
 	}
 
+	task_ctx_arr_sz = (hba->max_tasks / BNX2FC_TASKS_PER_PAGE);
 	if (hba->task_ctx) {
-		for (i = 0; i < BNX2FC_TASK_CTX_ARR_SZ; i++) {
+		for (i = 0; i < task_ctx_arr_sz; i++) {
 			if (hba->task_ctx[i]) {
 				dma_free_coherent(&hba->pcidev->dev, PAGE_SIZE,
 						    hba->task_ctx[i],
diff --git a/drivers/scsi/bnx2fc/bnx2fc_io.c b/drivers/scsi/bnx2fc/bnx2fc_io.c
index 60798e829de6..723a9a8ba5ee 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_io.c
+++ b/drivers/scsi/bnx2fc/bnx2fc_io.c
@@ -239,8 +239,7 @@ static void bnx2fc_scsi_done(struct bnx2fc_cmd *io_req, int err_code)
 	sc_cmd->scsi_done(sc_cmd);
 }
 
-struct bnx2fc_cmd_mgr *bnx2fc_cmd_mgr_alloc(struct bnx2fc_hba *hba,
-						u16 min_xid, u16 max_xid)
+struct bnx2fc_cmd_mgr *bnx2fc_cmd_mgr_alloc(struct bnx2fc_hba *hba)
 {
 	struct bnx2fc_cmd_mgr *cmgr;
 	struct io_bdt *bdt_info;
@@ -252,6 +251,8 @@ struct bnx2fc_cmd_mgr *bnx2fc_cmd_mgr_alloc(struct bnx2fc_hba *hba,
 	int num_ios, num_pri_ios;
 	size_t bd_tbl_sz;
 	int arr_sz = num_possible_cpus() + 1;
+	u16 min_xid = BNX2FC_MIN_XID;
+	u16 max_xid = hba->max_xid;
 
 	if (max_xid <= min_xid || max_xid == FC_XID_UNKNOWN) {
 		printk(KERN_ERR PFX "cmd_mgr_alloc: Invalid min_xid 0x%x \
@@ -298,7 +299,7 @@ struct bnx2fc_cmd_mgr *bnx2fc_cmd_mgr_alloc(struct bnx2fc_hba *hba,
 	 * of slow path requests.
 	 */
 	xid = BNX2FC_MIN_XID;
-	num_pri_ios = num_ios - BNX2FC_ELSTM_XIDS;
+	num_pri_ios = num_ios - hba->elstm_xids;
 	for (i = 0; i < num_ios; i++) {
 		io_req = kzalloc(sizeof(*io_req), GFP_KERNEL);
 
@@ -367,7 +368,7 @@ void bnx2fc_cmd_mgr_free(struct bnx2fc_cmd_mgr *cmgr)
 	struct bnx2fc_hba *hba = cmgr->hba;
 	size_t bd_tbl_sz;
 	u16 min_xid = BNX2FC_MIN_XID;
-	u16 max_xid = BNX2FC_MAX_XID;
+	u16 max_xid = hba->max_xid;
 	int num_ios;
 	int i;
 
diff --git a/drivers/scsi/csiostor/Makefile b/drivers/scsi/csiostor/Makefile
index b581966c88f9..913b9a92fb06 100644
--- a/drivers/scsi/csiostor/Makefile
+++ b/drivers/scsi/csiostor/Makefile
@@ -8,4 +8,5 @@ ccflags-y += -I$(srctree)/drivers/net/ethernet/chelsio/cxgb4
 obj-$(CONFIG_SCSI_CHELSIO_FCOE) += csiostor.o
 
 csiostor-objs := csio_attr.o csio_init.o csio_lnode.o csio_scsi.o \
-		csio_hw.o csio_isr.o csio_mb.o csio_rnode.o csio_wr.o
+		csio_hw.o csio_hw_t4.o csio_hw_t5.o csio_isr.o \
+		csio_mb.o csio_rnode.o csio_wr.o
diff --git a/drivers/scsi/csiostor/csio_hw.c b/drivers/scsi/csiostor/csio_hw.c
index bdd78fb4fc70..193605519361 100644
--- a/drivers/scsi/csiostor/csio_hw.c
+++ b/drivers/scsi/csiostor/csio_hw.c
@@ -61,7 +61,7 @@ int csio_msi = 2;
 static int dev_num;
 
 /* FCoE Adapter types & its description */
-static const struct csio_adap_desc csio_fcoe_adapters[] = {
+static const struct csio_adap_desc csio_t4_fcoe_adapters[] = {
 	{"T440-Dbg 10G", "Chelsio T440-Dbg 10G [FCoE]"},
 	{"T420-CR 10G", "Chelsio T420-CR 10G [FCoE]"},
 	{"T422-CR 10G/1G", "Chelsio T422-CR 10G/1G [FCoE]"},
@@ -77,7 +77,38 @@ static const struct csio_adap_desc csio_fcoe_adapters[] = {
 	{"B404-BT 1G", "Chelsio B404-BT 1G [FCoE]"},
 	{"T480-CR 10G", "Chelsio T480-CR 10G [FCoE]"},
 	{"T440-LP-CR 10G", "Chelsio T440-LP-CR 10G [FCoE]"},
-	{"T4 FPGA", "Chelsio T4 FPGA [FCoE]"}
+	{"AMSTERDAM 10G", "Chelsio AMSTERDAM 10G [FCoE]"},
+	{"HUAWEI T480 10G", "Chelsio HUAWEI T480 10G [FCoE]"},
+	{"HUAWEI T440 10G", "Chelsio HUAWEI T440 10G [FCoE]"},
+	{"HUAWEI STG 10G", "Chelsio HUAWEI STG 10G [FCoE]"},
+	{"ACROMAG XAUI 10G", "Chelsio ACROMAG XAUI 10G [FCoE]"},
+	{"ACROMAG SFP+ 10G", "Chelsio ACROMAG SFP+ 10G [FCoE]"},
+	{"QUANTA SFP+ 10G", "Chelsio QUANTA SFP+ 10G [FCoE]"},
+	{"HUAWEI 10Gbase-T", "Chelsio HUAWEI 10Gbase-T [FCoE]"},
+	{"HUAWEI T4TOE 10G", "Chelsio HUAWEI T4TOE 10G [FCoE]"}
+};
+
+static const struct csio_adap_desc csio_t5_fcoe_adapters[] = {
+	{"T580-Dbg 10G", "Chelsio T580-Dbg 10G [FCoE]"},
+	{"T520-CR 10G", "Chelsio T520-CR 10G [FCoE]"},
+	{"T522-CR 10G/1G", "Chelsio T452-CR 10G/1G [FCoE]"},
+	{"T540-CR 10G", "Chelsio T540-CR 10G [FCoE]"},
+	{"T520-BCH 10G", "Chelsio T520-BCH 10G [FCoE]"},
+	{"T540-BCH 10G", "Chelsio T540-BCH 10G [FCoE]"},
+	{"T540-CH 10G", "Chelsio T540-CH 10G [FCoE]"},
+	{"T520-SO 10G", "Chelsio T520-SO 10G [FCoE]"},
+	{"T520-CX4 10G", "Chelsio T520-CX4 10G [FCoE]"},
+	{"T520-BT 10G", "Chelsio T520-BT 10G [FCoE]"},
+	{"T504-BT 1G", "Chelsio T504-BT 1G [FCoE]"},
+	{"B520-SR 10G", "Chelsio B520-SR 10G [FCoE]"},
+	{"B504-BT 1G", "Chelsio B504-BT 1G [FCoE]"},
+	{"T580-CR 10G", "Chelsio T580-CR 10G [FCoE]"},
+	{"T540-LP-CR 10G", "Chelsio T540-LP-CR 10G [FCoE]"},
+	{"AMSTERDAM 10G", "Chelsio AMSTERDAM 10G [FCoE]"},
+	{"T580-LP-CR 40G", "Chelsio T580-LP-CR 40G [FCoE]"},
+	{"T520-LL-CR 10G", "Chelsio T520-LL-CR 10G [FCoE]"},
+	{"T560-CR 40G", "Chelsio T560-CR 40G [FCoE]"},
+	{"T580-CR 40G", "Chelsio T580-CR 40G [FCoE]"}
 };
 
 static void csio_mgmtm_cleanup(struct csio_mgmtm *);
@@ -124,7 +155,7 @@ int csio_is_hw_removing(struct csio_hw *hw)
  *	at the time it indicated completion is stored there.  Returns 0 if the
  *	operation completes and	-EAGAIN	otherwise.
  */
-static int
+int
 csio_hw_wait_op_done_val(struct csio_hw *hw, int reg, uint32_t mask,
 			 int polarity, int attempts, int delay, uint32_t *valp)
 {
@@ -145,6 +176,24 @@ csio_hw_wait_op_done_val(struct csio_hw *hw, int reg, uint32_t mask,
 	}
 }
 
+/*
+ *	csio_hw_tp_wr_bits_indirect - set/clear bits in an indirect TP register
+ *	@hw: the adapter
+ *	@addr: the indirect TP register address
+ *	@mask: specifies the field within the register to modify
+ *	@val: new value for the field
+ *
+ *	Sets a field of an indirect TP register to the given value.
+ */
+void
+csio_hw_tp_wr_bits_indirect(struct csio_hw *hw, unsigned int addr,
+			unsigned int mask, unsigned int val)
+{
+	csio_wr_reg32(hw, addr, TP_PIO_ADDR);
+	val |= csio_rd_reg32(hw, TP_PIO_DATA) & ~mask;
+	csio_wr_reg32(hw, val, TP_PIO_DATA);
+}
+
 void
 csio_set_reg_field(struct csio_hw *hw, uint32_t reg, uint32_t mask,
 		   uint32_t value)
@@ -157,242 +206,22 @@ csio_set_reg_field(struct csio_hw *hw, uint32_t reg, uint32_t mask,
 
 }
 
-/*
- *	csio_hw_mc_read - read from MC through backdoor accesses
- *	@hw: the hw module
- *	@addr: address of first byte requested
- *	@data: 64 bytes of data containing the requested address
- *	@ecc: where to store the corresponding 64-bit ECC word
- *
- *	Read 64 bytes of data from MC starting at a 64-byte-aligned address
- *	that covers the requested address @addr.  If @parity is not %NULL it
- *	is assigned the 64-bit ECC word for the read data.
- */
-int
-csio_hw_mc_read(struct csio_hw *hw, uint32_t addr, __be32 *data,
-		uint64_t *ecc)
-{
-	int i;
-
-	if (csio_rd_reg32(hw, MC_BIST_CMD) & START_BIST)
-		return -EBUSY;
-	csio_wr_reg32(hw, addr & ~0x3fU, MC_BIST_CMD_ADDR);
-	csio_wr_reg32(hw, 64, MC_BIST_CMD_LEN);
-	csio_wr_reg32(hw, 0xc, MC_BIST_DATA_PATTERN);
-	csio_wr_reg32(hw, BIST_OPCODE(1) | START_BIST |  BIST_CMD_GAP(1),
-		      MC_BIST_CMD);
-	i = csio_hw_wait_op_done_val(hw, MC_BIST_CMD, START_BIST,
-		 0, 10, 1, NULL);
-	if (i)
-		return i;
-
-#define MC_DATA(i) MC_BIST_STATUS_REG(MC_BIST_STATUS_RDATA, i)
-
-	for (i = 15; i >= 0; i--)
-		*data++ = htonl(csio_rd_reg32(hw, MC_DATA(i)));
-	if (ecc)
-		*ecc = csio_rd_reg64(hw, MC_DATA(16));
-#undef MC_DATA
-	return 0;
-}
-
-/*
- *	csio_hw_edc_read - read from EDC through backdoor accesses
- *	@hw: the hw module
- *	@idx: which EDC to access
- *	@addr: address of first byte requested
- *	@data: 64 bytes of data containing the requested address
- *	@ecc: where to store the corresponding 64-bit ECC word
- *
- *	Read 64 bytes of data from EDC starting at a 64-byte-aligned address
- *	that covers the requested address @addr.  If @parity is not %NULL it
- *	is assigned the 64-bit ECC word for the read data.
- */
-int
-csio_hw_edc_read(struct csio_hw *hw, int idx, uint32_t addr, __be32 *data,
-		uint64_t *ecc)
-{
-	int i;
-
-	idx *= EDC_STRIDE;
-	if (csio_rd_reg32(hw, EDC_BIST_CMD + idx) & START_BIST)
-		return -EBUSY;
-	csio_wr_reg32(hw, addr & ~0x3fU, EDC_BIST_CMD_ADDR + idx);
-	csio_wr_reg32(hw, 64, EDC_BIST_CMD_LEN + idx);
-	csio_wr_reg32(hw, 0xc, EDC_BIST_DATA_PATTERN + idx);
-	csio_wr_reg32(hw, BIST_OPCODE(1) | BIST_CMD_GAP(1) | START_BIST,
-		     EDC_BIST_CMD + idx);
-	i = csio_hw_wait_op_done_val(hw, EDC_BIST_CMD + idx, START_BIST,
-		 0, 10, 1, NULL);
-	if (i)
-		return i;
-
-#define EDC_DATA(i) (EDC_BIST_STATUS_REG(EDC_BIST_STATUS_RDATA, i) + idx)
-
-	for (i = 15; i >= 0; i--)
-		*data++ = htonl(csio_rd_reg32(hw, EDC_DATA(i)));
-	if (ecc)
-		*ecc = csio_rd_reg64(hw, EDC_DATA(16));
-#undef EDC_DATA
-	return 0;
-}
-
-/*
- *      csio_mem_win_rw - read/write memory through PCIE memory window
- *      @hw: the adapter
- *      @addr: address of first byte requested
- *      @data: MEMWIN0_APERTURE bytes of data containing the requested address
- *      @dir: direction of transfer 1 => read, 0 => write
- *
- *      Read/write MEMWIN0_APERTURE bytes of data from MC starting at a
- *      MEMWIN0_APERTURE-byte-aligned address that covers the requested
- *      address @addr.
- */
-static int
-csio_mem_win_rw(struct csio_hw *hw, u32 addr, u32 *data, int dir)
-{
-	int i;
-
-	/*
-	 * Setup offset into PCIE memory window.  Address must be a
-	 * MEMWIN0_APERTURE-byte-aligned address.  (Read back MA register to
-	 * ensure that changes propagate before we attempt to use the new
-	 * values.)
-	 */
-	csio_wr_reg32(hw, addr & ~(MEMWIN0_APERTURE - 1),
-			PCIE_MEM_ACCESS_OFFSET);
-	csio_rd_reg32(hw, PCIE_MEM_ACCESS_OFFSET);
-
-	/* Collecting data 4 bytes at a time upto MEMWIN0_APERTURE */
-	for (i = 0; i < MEMWIN0_APERTURE; i = i + sizeof(__be32)) {
-		if (dir)
-			*data++ = csio_rd_reg32(hw, (MEMWIN0_BASE + i));
-		else
-			csio_wr_reg32(hw, *data++, (MEMWIN0_BASE + i));
-	}
-
-	return 0;
-}
-
-/*
- *      csio_memory_rw - read/write EDC 0, EDC 1 or MC via PCIE memory window
- *      @hw: the csio_hw
- *      @mtype: memory type: MEM_EDC0, MEM_EDC1 or MEM_MC
- *      @addr: address within indicated memory type
- *      @len: amount of memory to transfer
- *      @buf: host memory buffer
- *      @dir: direction of transfer 1 => read, 0 => write
- *
- *      Reads/writes an [almost] arbitrary memory region in the firmware: the
- *      firmware memory address, length and host buffer must be aligned on
- *      32-bit boudaries.  The memory is transferred as a raw byte sequence
- *      from/to the firmware's memory.  If this memory contains data
- *      structures which contain multi-byte integers, it's the callers
- *      responsibility to perform appropriate byte order conversions.
- */
-static int
-csio_memory_rw(struct csio_hw *hw, int mtype, u32 addr, u32 len,
-		uint32_t *buf, int dir)
-{
-	uint32_t pos, start, end, offset, memoffset;
-	int ret;
-	uint32_t *data;
-
-	/*
-	 * Argument sanity checks ...
-	 */
-	if ((addr & 0x3) || (len & 0x3))
-		return -EINVAL;
-
-	data = kzalloc(MEMWIN0_APERTURE, GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
-
-	/* Offset into the region of memory which is being accessed
-	 * MEM_EDC0 = 0
-	 * MEM_EDC1 = 1
-	 * MEM_MC   = 2
-	 */
-	memoffset = (mtype * (5 * 1024 * 1024));
-
-	/* Determine the PCIE_MEM_ACCESS_OFFSET */
-	addr = addr + memoffset;
-
-	/*
-	 * The underlaying EDC/MC read routines read MEMWIN0_APERTURE bytes
-	 * at a time so we need to round down the start and round up the end.
-	 * We'll start copying out of the first line at (addr - start) a word
-	 * at a time.
-	 */
-	start = addr & ~(MEMWIN0_APERTURE-1);
-	end = (addr + len + MEMWIN0_APERTURE-1) & ~(MEMWIN0_APERTURE-1);
-	offset = (addr - start)/sizeof(__be32);
-
-	for (pos = start; pos < end; pos += MEMWIN0_APERTURE, offset = 0) {
-		/*
-		 * If we're writing, copy the data from the caller's memory
-		 * buffer
-		 */
-		if (!dir) {
-			/*
-			 * If we're doing a partial write, then we need to do
-			 * a read-modify-write ...
-			 */
-			if (offset || len < MEMWIN0_APERTURE) {
-				ret = csio_mem_win_rw(hw, pos, data, 1);
-				if (ret) {
-					kfree(data);
-					return ret;
-				}
-			}
-			while (offset < (MEMWIN0_APERTURE/sizeof(__be32)) &&
-								len > 0) {
-				data[offset++] = *buf++;
-				len -= sizeof(__be32);
-			}
-		}
-
-		/*
-		 * Transfer a block of memory and bail if there's an error.
-		 */
-		ret = csio_mem_win_rw(hw, pos, data, dir);
-		if (ret) {
-			kfree(data);
-			return ret;
-		}
-
-		/*
-		 * If we're reading, copy the data into the caller's memory
-		 * buffer.
-		 */
-		if (dir)
-			while (offset < (MEMWIN0_APERTURE/sizeof(__be32)) &&
-								len > 0) {
-				*buf++ = data[offset++];
-				len -= sizeof(__be32);
-			}
-	}
-
-	kfree(data);
-
-	return 0;
-}
-
 static int
 csio_memory_write(struct csio_hw *hw, int mtype, u32 addr, u32 len, u32 *buf)
 {
-	return csio_memory_rw(hw, mtype, addr, len, buf, 0);
+	return hw->chip_ops->chip_memory_rw(hw, MEMWIN_CSIOSTOR, mtype,
+					    addr, len, buf, 0);
 }
 
 /*
  * EEPROM reads take a few tens of us while writes can take a bit over 5 ms.
  */
-#define EEPROM_MAX_RD_POLL 40
-#define EEPROM_MAX_WR_POLL 6
-#define EEPROM_STAT_ADDR   0x7bfc
-#define VPD_BASE           0x400
-#define VPD_BASE_OLD	   0
-#define VPD_LEN            512
+#define EEPROM_MAX_RD_POLL	40
+#define EEPROM_MAX_WR_POLL	6
+#define EEPROM_STAT_ADDR	0x7bfc
+#define VPD_BASE		0x400
+#define VPD_BASE_OLD		0
+#define VPD_LEN			1024
 #define VPD_INFO_FLD_HDR_SIZE	3
 
 /*
@@ -817,23 +646,6 @@ out:
 	return 0;
 }
 
-/*
- *	csio_hw_flash_cfg_addr - return the address of the flash
- *				configuration file
- *	@hw: the HW module
- *
- *	Return the address within the flash where the Firmware Configuration
- *	File is stored.
- */
-static unsigned int
-csio_hw_flash_cfg_addr(struct csio_hw *hw)
-{
-	if (hw->params.sf_size == 0x100000)
-		return FPGA_FLASH_CFG_OFFSET;
-	else
-		return FLASH_CFG_OFFSET;
-}
-
 static void
 csio_hw_print_fw_version(struct csio_hw *hw, char *str)
 {
@@ -898,13 +710,13 @@ csio_hw_check_fw_version(struct csio_hw *hw)
 	minor = FW_HDR_FW_VER_MINOR_GET(hw->fwrev);
 	micro = FW_HDR_FW_VER_MICRO_GET(hw->fwrev);
 
-	if (major != FW_VERSION_MAJOR) {            /* major mismatch - fail */
+	if (major != FW_VERSION_MAJOR(hw)) {	/* major mismatch - fail */
 		csio_err(hw, "card FW has major version %u, driver wants %u\n",
-			 major, FW_VERSION_MAJOR);
+			 major, FW_VERSION_MAJOR(hw));
 		return -EINVAL;
 	}
 
-	if (minor == FW_VERSION_MINOR && micro == FW_VERSION_MICRO)
+	if (minor == FW_VERSION_MINOR(hw) && micro == FW_VERSION_MICRO(hw))
 		return 0;        /* perfect match */
 
 	/* Minor/micro version mismatch */
@@ -1044,7 +856,7 @@ static void
 csio_set_pcie_completion_timeout(struct csio_hw *hw, u8 range)
 {
 	uint16_t val;
-	uint32_t pcie_cap;
+	int pcie_cap;
 
 	if (!csio_pci_capability(hw->pdev, PCI_CAP_ID_EXP, &pcie_cap)) {
 		pci_read_config_word(hw->pdev,
@@ -1056,84 +868,6 @@ csio_set_pcie_completion_timeout(struct csio_hw *hw, u8 range)
 	}
 }
 
-
-/*
- * Return the specified PCI-E Configuration Space register from our Physical
- * Function.  We try first via a Firmware LDST Command since we prefer to let
- * the firmware own all of these registers, but if that fails we go for it
- * directly ourselves.
- */
-static uint32_t
-csio_read_pcie_cfg4(struct csio_hw *hw, int reg)
-{
-	u32 val = 0;
-	struct csio_mb *mbp;
-	int rv;
-	struct fw_ldst_cmd *ldst_cmd;
-
-	mbp = mempool_alloc(hw->mb_mempool, GFP_ATOMIC);
-	if (!mbp) {
-		CSIO_INC_STATS(hw, n_err_nomem);
-		pci_read_config_dword(hw->pdev, reg, &val);
-		return val;
-	}
-
-	csio_mb_ldst(hw, mbp, CSIO_MB_DEFAULT_TMO, reg);
-
-	rv = csio_mb_issue(hw, mbp);
-
-	/*
-	 * If the LDST Command suucceeded, exctract the returned register
-	 * value.  Otherwise read it directly ourself.
-	 */
-	if (rv == 0) {
-		ldst_cmd = (struct fw_ldst_cmd *)(mbp->mb);
-		val = ntohl(ldst_cmd->u.pcie.data[0]);
-	} else
-		pci_read_config_dword(hw->pdev, reg, &val);
-
-	mempool_free(mbp, hw->mb_mempool);
-
-	return val;
-} /* csio_read_pcie_cfg4 */
-
-static int
-csio_hw_set_mem_win(struct csio_hw *hw)
-{
-	u32 bar0;
-
-	/*
-	 * Truncation intentional: we only read the bottom 32-bits of the
-	 * 64-bit BAR0/BAR1 ...  We use the hardware backdoor mechanism to
-	 * read BAR0 instead of using pci_resource_start() because we could be
-	 * operating from within a Virtual Machine which is trapping our
-	 * accesses to our Configuration Space and we need to set up the PCI-E
-	 * Memory Window decoders with the actual addresses which will be
-	 * coming across the PCI-E link.
-	 */
-	bar0 = csio_read_pcie_cfg4(hw, PCI_BASE_ADDRESS_0);
-	bar0 &= PCI_BASE_ADDRESS_MEM_MASK;
-
-	/*
-	 * Set up memory window for accessing adapter memory ranges.  (Read
-	 * back MA register to ensure that changes propagate before we attempt
-	 * to use the new values.)
-	 */
-	csio_wr_reg32(hw, (bar0 + MEMWIN0_BASE) | BIR(0) |
-		WINDOW(ilog2(MEMWIN0_APERTURE) - 10),
-		PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN, 0));
-	csio_wr_reg32(hw, (bar0 + MEMWIN1_BASE) | BIR(0) |
-		WINDOW(ilog2(MEMWIN1_APERTURE) - 10),
-		PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN, 1));
-	csio_wr_reg32(hw, (bar0 + MEMWIN2_BASE) | BIR(0) |
-		WINDOW(ilog2(MEMWIN2_APERTURE) - 10),
-		PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN, 2));
-	csio_rd_reg32(hw, PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN, 2));
-	return 0;
-} /* csio_hw_set_mem_win */
-
-
-
 /*****************************************************************************/
 /* HW State machine assists                                                  */
 /*****************************************************************************/
@@ -1234,7 +968,9 @@ retry:
 		for (;;) {
 			uint32_t pcie_fw;
 
+			spin_unlock_irq(&hw->lock);
 			msleep(50);
+			spin_lock_irq(&hw->lock);
 			waiting -= 50;
 
 			/*
@@ -2121,9 +1857,9 @@ csio_hw_flash_config(struct csio_hw *hw, u32 *fw_cfg_param, char *path)
 	uint32_t *cfg_data;
 	int value_to_add = 0;
 
-	if (request_firmware(&cf, CSIO_CF_FNAME, dev) < 0) {
-		csio_err(hw, "could not find config file " CSIO_CF_FNAME
-			 ",err: %d\n", ret);
+	if (request_firmware(&cf, CSIO_CF_FNAME(hw), dev) < 0) {
+		csio_err(hw, "could not find config file %s, err: %d\n",
+			 CSIO_CF_FNAME(hw), ret);
 		return -ENOENT;
 	}
 
@@ -2147,9 +1883,24 @@ csio_hw_flash_config(struct csio_hw *hw, u32 *fw_cfg_param, char *path)
 
 	ret = csio_memory_write(hw, mtype, maddr,
 				cf->size + value_to_add, cfg_data);
+
+	if ((ret == 0) && (value_to_add != 0)) {
+		union {
+			u32 word;
+			char buf[4];
+		} last;
+		size_t size = cf->size & ~0x3;
+		int i;
+
+		last.word = cfg_data[size >> 2];
+		for (i = value_to_add; i < 4; i++)
+			last.buf[i] = 0;
+		ret = csio_memory_write(hw, mtype, maddr + size, 4, &last.word);
+	}
 	if (ret == 0) {
-		csio_info(hw, "config file upgraded to " CSIO_CF_FNAME "\n");
-		strncpy(path, "/lib/firmware/" CSIO_CF_FNAME, 64);
+		csio_info(hw, "config file upgraded to %s\n",
+			  CSIO_CF_FNAME(hw));
+		snprintf(path, 64, "%s%s", "/lib/firmware/", CSIO_CF_FNAME(hw));
 	}
 
 leave:
@@ -2179,7 +1930,7 @@ csio_hw_use_fwconfig(struct csio_hw *hw, int reset, u32 *fw_cfg_param)
 {
 	unsigned int mtype, maddr;
 	int rv;
-	uint32_t finiver, finicsum, cfcsum;
+	uint32_t finiver = 0, finicsum = 0, cfcsum = 0;
 	int using_flash;
 	char path[64];
 
@@ -2207,7 +1958,7 @@ csio_hw_use_fwconfig(struct csio_hw *hw, int reset, u32 *fw_cfg_param)
 			 * config file from flash.
 			 */
 			mtype = FW_MEMTYPE_CF_FLASH;
-			maddr = csio_hw_flash_cfg_addr(hw);
+			maddr = hw->chip_ops->chip_flash_cfg_addr(hw);
 			using_flash = 1;
 		} else {
 			/*
@@ -2346,30 +2097,32 @@ csio_hw_flash_fw(struct csio_hw *hw)
 	struct pci_dev *pci_dev = hw->pdev;
 	struct device *dev = &pci_dev->dev ;
 
-	if (request_firmware(&fw, CSIO_FW_FNAME, dev) < 0) {
-		csio_err(hw, "could not find firmware image " CSIO_FW_FNAME
-		",err: %d\n", ret);
+	if (request_firmware(&fw, CSIO_FW_FNAME(hw), dev) < 0) {
+		csio_err(hw, "could not find firmware image %s, err: %d\n",
+			 CSIO_FW_FNAME(hw), ret);
 		return -EINVAL;
 	}
 
 	hdr = (const struct fw_hdr *)fw->data;
 	fw_ver = ntohl(hdr->fw_ver);
-	if (FW_HDR_FW_VER_MAJOR_GET(fw_ver) != FW_VERSION_MAJOR)
+	if (FW_HDR_FW_VER_MAJOR_GET(fw_ver) != FW_VERSION_MAJOR(hw))
 		return -EINVAL;      /* wrong major version, won't do */
 
 	/*
 	 * If the flash FW is unusable or we found something newer, load it.
 	 */
-	if (FW_HDR_FW_VER_MAJOR_GET(hw->fwrev) != FW_VERSION_MAJOR ||
+	if (FW_HDR_FW_VER_MAJOR_GET(hw->fwrev) != FW_VERSION_MAJOR(hw) ||
 	    fw_ver > hw->fwrev) {
 		ret = csio_hw_fw_upgrade(hw, hw->pfn, fw->data, fw->size,
 				    /*force=*/false);
 		if (!ret)
-			csio_info(hw, "firmware upgraded to version %pI4 from "
-				  CSIO_FW_FNAME "\n", &hdr->fw_ver);
+			csio_info(hw,
+				  "firmware upgraded to version %pI4 from %s\n",
+				  &hdr->fw_ver, CSIO_FW_FNAME(hw));
 		else
 			csio_err(hw, "firmware upgrade failed! err=%d\n", ret);
-	}
+	} else
+		ret = -EINVAL;
 
 	release_firmware(fw);
 
@@ -2410,7 +2163,7 @@ csio_hw_configure(struct csio_hw *hw)
 	/* Set pci completion timeout value to 4 seconds. */
 	csio_set_pcie_completion_timeout(hw, 0xd);
 
-	csio_hw_set_mem_win(hw);
+	hw->chip_ops->chip_set_mem_win(hw, MEMWIN_CSIOSTOR);
 
 	rv = csio_hw_get_fw_version(hw, &hw->fwrev);
 	if (rv != 0)
@@ -2478,6 +2231,8 @@ csio_hw_configure(struct csio_hw *hw)
 	} else {
 		if (hw->fw_state == CSIO_DEV_STATE_INIT) {
 
+			hw->flags |= CSIO_HWF_USING_SOFT_PARAMS;
+
 			/* device parameters */
 			rv = csio_get_device_params(hw);
 			if (rv != 0)
@@ -2651,7 +2406,7 @@ csio_hw_intr_disable(struct csio_hw *hw)
 
 }
 
-static void
+void
 csio_hw_fatal_err(struct csio_hw *hw)
 {
 	csio_set_reg_field(hw, SGE_CONTROL, GLOBALENABLE, 0);
@@ -2990,14 +2745,6 @@ csio_hws_pcierr(struct csio_hw *hw, enum csio_hw_ev evt)
 /* END: HW SM                                                                */
 /*****************************************************************************/
 
-/* Slow path handlers */
-struct intr_info {
-	unsigned int mask;       /* bits to check in interrupt status */
-	const char *msg;         /* message to print or NULL */
-	short stat_idx;          /* stat counter to increment or -1 */
-	unsigned short fatal;    /* whether the condition reported is fatal */
-};
-
 /*
  *	csio_handle_intr_status - table driven interrupt handler
  *	@hw: HW instance
@@ -3011,7 +2758,7 @@ struct intr_info {
  *	by an entry specifying mask 0.  Returns the number of fatal interrupt
  *	conditions.
  */
-static int
+int
 csio_handle_intr_status(struct csio_hw *hw, unsigned int reg,
 				 const struct intr_info *acts)
 {
@@ -3038,80 +2785,6 @@ csio_handle_intr_status(struct csio_hw *hw, unsigned int reg,
 }
 
 /*
- * Interrupt handler for the PCIE module.
- */
-static void
-csio_pcie_intr_handler(struct csio_hw *hw)
-{
-	static struct intr_info sysbus_intr_info[] = {
-		{ RNPP, "RXNP array parity error", -1, 1 },
-		{ RPCP, "RXPC array parity error", -1, 1 },
-		{ RCIP, "RXCIF array parity error", -1, 1 },
-		{ RCCP, "Rx completions control array parity error", -1, 1 },
-		{ RFTP, "RXFT array parity error", -1, 1 },
-		{ 0, NULL, 0, 0 }
-	};
-	static struct intr_info pcie_port_intr_info[] = {
-		{ TPCP, "TXPC array parity error", -1, 1 },
-		{ TNPP, "TXNP array parity error", -1, 1 },
-		{ TFTP, "TXFT array parity error", -1, 1 },
-		{ TCAP, "TXCA array parity error", -1, 1 },
-		{ TCIP, "TXCIF array parity error", -1, 1 },
-		{ RCAP, "RXCA array parity error", -1, 1 },
-		{ OTDD, "outbound request TLP discarded", -1, 1 },
-		{ RDPE, "Rx data parity error", -1, 1 },
-		{ TDUE, "Tx uncorrectable data error", -1, 1 },
-		{ 0, NULL, 0, 0 }
-	};
-	static struct intr_info pcie_intr_info[] = {
-		{ MSIADDRLPERR, "MSI AddrL parity error", -1, 1 },
-		{ MSIADDRHPERR, "MSI AddrH parity error", -1, 1 },
-		{ MSIDATAPERR, "MSI data parity error", -1, 1 },
-		{ MSIXADDRLPERR, "MSI-X AddrL parity error", -1, 1 },
-		{ MSIXADDRHPERR, "MSI-X AddrH parity error", -1, 1 },
-		{ MSIXDATAPERR, "MSI-X data parity error", -1, 1 },
-		{ MSIXDIPERR, "MSI-X DI parity error", -1, 1 },
-		{ PIOCPLPERR, "PCI PIO completion FIFO parity error", -1, 1 },
-		{ PIOREQPERR, "PCI PIO request FIFO parity error", -1, 1 },
-		{ TARTAGPERR, "PCI PCI target tag FIFO parity error", -1, 1 },
-		{ CCNTPERR, "PCI CMD channel count parity error", -1, 1 },
-		{ CREQPERR, "PCI CMD channel request parity error", -1, 1 },
-		{ CRSPPERR, "PCI CMD channel response parity error", -1, 1 },
-		{ DCNTPERR, "PCI DMA channel count parity error", -1, 1 },
-		{ DREQPERR, "PCI DMA channel request parity error", -1, 1 },
-		{ DRSPPERR, "PCI DMA channel response parity error", -1, 1 },
-		{ HCNTPERR, "PCI HMA channel count parity error", -1, 1 },
-		{ HREQPERR, "PCI HMA channel request parity error", -1, 1 },
-		{ HRSPPERR, "PCI HMA channel response parity error", -1, 1 },
-		{ CFGSNPPERR, "PCI config snoop FIFO parity error", -1, 1 },
-		{ FIDPERR, "PCI FID parity error", -1, 1 },
-		{ INTXCLRPERR, "PCI INTx clear parity error", -1, 1 },
-		{ MATAGPERR, "PCI MA tag parity error", -1, 1 },
-		{ PIOTAGPERR, "PCI PIO tag parity error", -1, 1 },
-		{ RXCPLPERR, "PCI Rx completion parity error", -1, 1 },
-		{ RXWRPERR, "PCI Rx write parity error", -1, 1 },
-		{ RPLPERR, "PCI replay buffer parity error", -1, 1 },
-		{ PCIESINT, "PCI core secondary fault", -1, 1 },
-		{ PCIEPINT, "PCI core primary fault", -1, 1 },
-		{ UNXSPLCPLERR, "PCI unexpected split completion error", -1,
-		  0 },
-		{ 0, NULL, 0, 0 }
-	};
-
-	int fat;
-
-	fat = csio_handle_intr_status(hw,
-				    PCIE_CORE_UTL_SYSTEM_BUS_AGENT_STATUS,
-				    sysbus_intr_info) +
-	      csio_handle_intr_status(hw,
-				    PCIE_CORE_UTL_PCI_EXPRESS_PORT_STATUS,
-				    pcie_port_intr_info) +
-	      csio_handle_intr_status(hw, PCIE_INT_CAUSE, pcie_intr_info);
-	if (fat)
-		csio_hw_fatal_err(hw);
-}
-
-/*
  * TP interrupt handler.
  */
 static void csio_tp_intr_handler(struct csio_hw *hw)
@@ -3517,7 +3190,7 @@ static void csio_ncsi_intr_handler(struct csio_hw *hw)
  */
 static void csio_xgmac_intr_handler(struct csio_hw *hw, int port)
 {
-	uint32_t v = csio_rd_reg32(hw, PORT_REG(port, XGMAC_PORT_INT_CAUSE));
+	uint32_t v = csio_rd_reg32(hw, CSIO_MAC_INT_CAUSE_REG(hw, port));
 
 	v &= TXFIFO_PRTY_ERR | RXFIFO_PRTY_ERR;
 	if (!v)
@@ -3527,7 +3200,7 @@ static void csio_xgmac_intr_handler(struct csio_hw *hw, int port)
 		csio_fatal(hw, "XGMAC %d Tx FIFO parity error\n", port);
 	if (v & RXFIFO_PRTY_ERR)
 		csio_fatal(hw, "XGMAC %d Rx FIFO parity error\n", port);
-	csio_wr_reg32(hw, v, PORT_REG(port, XGMAC_PORT_INT_CAUSE));
+	csio_wr_reg32(hw, v, CSIO_MAC_INT_CAUSE_REG(hw, port));
 	csio_hw_fatal_err(hw);
 }
 
@@ -3596,7 +3269,7 @@ csio_hw_slow_intr_handler(struct csio_hw *hw)
 		csio_xgmac_intr_handler(hw, 3);
 
 	if (cause & PCIE)
-		csio_pcie_intr_handler(hw);
+		hw->chip_ops->chip_pcie_intr_handler(hw);
 
 	if (cause & MC)
 		csio_mem_intr_handler(hw, MEM_MC);
@@ -3892,7 +3565,6 @@ csio_process_fwevtq_entry(struct csio_hw *hw, void *wr, uint32_t len,
 			  struct csio_fl_dma_buf *flb, void *priv)
 {
 	__u8 op;
-	__be64 *data;
 	void *msg = NULL;
 	uint32_t msg_len = 0;
 	bool msg_sg = 0;
@@ -3908,8 +3580,6 @@ csio_process_fwevtq_entry(struct csio_hw *hw, void *wr, uint32_t len,
 		msg = (void *) flb;
 		msg_len = flb->totlen;
 		msg_sg = 1;
-
-		data = (__be64 *) msg;
 	} else if (op == CPL_FW6_MSG || op == CPL_FW4_MSG) {
 
 		CSIO_INC_STATS(hw, n_cpl_fw6_msg);
@@ -3917,8 +3587,6 @@ csio_process_fwevtq_entry(struct csio_hw *hw, void *wr, uint32_t len,
 		msg = (void *)((uintptr_t)wr + sizeof(__be64));
 		msg_len = (op == CPL_FW6_MSG) ? sizeof(struct cpl_fw6_msg) :
 			   sizeof(struct cpl_fw4_msg);
-
-		data = (__be64 *) msg;
 	} else {
 		csio_warn(hw, "unexpected CPL %#x on FW event queue\n", op);
 		CSIO_INC_STATS(hw, n_cpl_unexp);
@@ -4262,6 +3930,7 @@ csio_hw_get_device_id(struct csio_hw *hw)
 			     &hw->params.pci.device_id);
 
 	csio_dev_id_cached(hw);
+	hw->chip_id = (hw->params.pci.device_id & CSIO_HW_CHIP_MASK);
 
 } /* csio_hw_get_device_id */
 
@@ -4280,19 +3949,21 @@ csio_hw_set_description(struct csio_hw *hw, uint16_t ven_id, uint16_t dev_id)
 		prot_type = (dev_id & CSIO_ASIC_DEVID_PROTO_MASK);
 		adap_type = (dev_id & CSIO_ASIC_DEVID_TYPE_MASK);
 
-		if (prot_type == CSIO_FPGA) {
+		if (prot_type == CSIO_T4_FCOE_ASIC) {
+			memcpy(hw->hw_ver,
+			       csio_t4_fcoe_adapters[adap_type].model_no, 16);
 			memcpy(hw->model_desc,
-				csio_fcoe_adapters[13].description, 32);
-		} else if (prot_type == CSIO_T4_FCOE_ASIC) {
+			       csio_t4_fcoe_adapters[adap_type].description,
+			       32);
+		} else if (prot_type == CSIO_T5_FCOE_ASIC) {
 			memcpy(hw->hw_ver,
-			       csio_fcoe_adapters[adap_type].model_no, 16);
+			       csio_t5_fcoe_adapters[adap_type].model_no, 16);
 			memcpy(hw->model_desc,
-				csio_fcoe_adapters[adap_type].description, 32);
+			       csio_t5_fcoe_adapters[adap_type].description,
+			       32);
 		} else {
 			char tempName[32] = "Chelsio FCoE Controller";
 			memcpy(hw->model_desc, tempName, 32);
-
-			CSIO_DB_ASSERT(0);
 		}
 	}
 } /* csio_hw_set_description */
@@ -4321,6 +3992,9 @@ csio_hw_init(struct csio_hw *hw)
 
 	strcpy(hw->name, CSIO_HW_NAME);
 
+	/* Initialize the HW chip ops with T4/T5 specific ops */
+	hw->chip_ops = csio_is_t4(hw->chip_id) ? &t4_ops : &t5_ops;
+
 	/* Set the model & its description */
 
 	ven_id = hw->params.pci.vendor_id;
diff --git a/drivers/scsi/csiostor/csio_hw.h b/drivers/scsi/csiostor/csio_hw.h
index 9edcca4c71af..489fc095cb03 100644
--- a/drivers/scsi/csiostor/csio_hw.h
+++ b/drivers/scsi/csiostor/csio_hw.h
@@ -48,6 +48,7 @@
 #include <scsi/scsi_device.h>
 #include <scsi/scsi_transport_fc.h>
 
+#include "csio_hw_chip.h"
 #include "csio_wr.h"
 #include "csio_mb.h"
 #include "csio_scsi.h"
@@ -60,13 +61,6 @@
  */
 #define	FW_HOSTERROR			255
 
-#define CSIO_FW_FNAME		"cxgb4/t4fw.bin"
-#define CSIO_CF_FNAME		"cxgb4/t4-config.txt"
-
-#define FW_VERSION_MAJOR	1
-#define FW_VERSION_MINOR	2
-#define FW_VERSION_MICRO	8
-
 #define CSIO_HW_NAME		"Chelsio FCoE Adapter"
 #define CSIO_MAX_PFN		8
 #define CSIO_MAX_PPORTS		4
@@ -123,8 +117,6 @@ extern int csio_msi;
 #define CSIO_VENDOR_ID				0x1425
 #define CSIO_ASIC_DEVID_PROTO_MASK		0xFF00
 #define CSIO_ASIC_DEVID_TYPE_MASK		0x00FF
-#define CSIO_FPGA				0xA000
-#define CSIO_T4_FCOE_ASIC			0x4600
 
 #define CSIO_GLBL_INTR_MASK		(CIM | MPS | PL | PCIE | MC | EDC0 | \
 					 EDC1 | LE | TP | MA | PM_TX | PM_RX | \
@@ -207,17 +199,6 @@ enum {
 	SF_SIZE = SF_SEC_SIZE * 16,   /* serial flash size */
 };
 
-enum { MEM_EDC0, MEM_EDC1, MEM_MC };
-
-enum {
-	MEMWIN0_APERTURE = 2048,
-	MEMWIN0_BASE     = 0x1b800,
-	MEMWIN1_APERTURE = 32768,
-	MEMWIN1_BASE     = 0x28000,
-	MEMWIN2_APERTURE = 65536,
-	MEMWIN2_BASE     = 0x30000,
-};
-
 /* serial flash and firmware constants */
 enum {
 	SF_ATTEMPTS = 10,             /* max retries for SF operations */
@@ -239,9 +220,6 @@ enum {
 	FLASH_CFG_MAX_SIZE    = 0x10000 , /* max size of the flash config file*/
 	FLASH_CFG_OFFSET      = 0x1f0000,
 	FLASH_CFG_START_SEC   = FLASH_CFG_OFFSET / SF_SEC_SIZE,
-	FPGA_FLASH_CFG_OFFSET = 0xf0000 , /* if FPGA mode, then cfg file is
-					   * at 1MB - 64KB */
-	FPGA_FLASH_CFG_START_SEC  = FPGA_FLASH_CFG_OFFSET / SF_SEC_SIZE,
 };
 
 /*
@@ -259,6 +237,8 @@ enum {
 	FLASH_FW_START = FLASH_START(FLASH_FW_START_SEC),
 	FLASH_FW_MAX_SIZE = FLASH_MAX_SIZE(FLASH_FW_NSECS),
 
+	/* Location of Firmware Configuration File in FLASH. */
+	FLASH_CFG_START = FLASH_START(FLASH_CFG_START_SEC),
 };
 
 #undef FLASH_START
@@ -310,7 +290,7 @@ struct csio_adap_desc {
 struct pci_params {
 	uint16_t   vendor_id;
 	uint16_t   device_id;
-	uint32_t   vpd_cap_addr;
+	int        vpd_cap_addr;
 	uint16_t   speed;
 	uint8_t    width;
 };
@@ -513,6 +493,7 @@ struct csio_hw {
 	uint32_t		fwrev;
 	uint32_t		tp_vers;
 	char			chip_ver;
+	uint16_t		chip_id;		/* Tells T4/T5 chip */
 	uint32_t		cfg_finiver;
 	uint32_t		cfg_finicsum;
 	uint32_t		cfg_cfcsum;
@@ -556,6 +537,9 @@ struct csio_hw {
 							 */
 
 	struct csio_fcoe_res_info  fres_info;		/* Fcoe resource info */
+	struct csio_hw_chip_ops	*chip_ops;		/* T4/T5 Chip specific
+							 * Operations
+							 */
 
 	/* MSIX vectors */
 	struct csio_msix_entries msix_entries[CSIO_MAX_MSIX_VECS];
@@ -636,9 +620,16 @@ csio_us_to_core_ticks(struct csio_hw *hw, uint32_t us)
 #define csio_dbg(__hw, __fmt, ...)
 #endif
 
+int csio_hw_wait_op_done_val(struct csio_hw *, int, uint32_t, int,
+			     int, int, uint32_t *);
+void csio_hw_tp_wr_bits_indirect(struct csio_hw *, unsigned int,
+				 unsigned int, unsigned int);
 int csio_mgmt_req_lookup(struct csio_mgmtm *, struct csio_ioreq *);
 void csio_hw_intr_disable(struct csio_hw *);
-int csio_hw_slow_intr_handler(struct csio_hw *hw);
+int csio_hw_slow_intr_handler(struct csio_hw *);
+int csio_handle_intr_status(struct csio_hw *, unsigned int,
+			    const struct intr_info *);
+
 int csio_hw_start(struct csio_hw *);
 int csio_hw_stop(struct csio_hw *);
 int csio_hw_reset(struct csio_hw *);
@@ -647,19 +638,17 @@ int csio_is_hw_removing(struct csio_hw *);
 
 int csio_fwevtq_handler(struct csio_hw *);
 void csio_evtq_worker(struct work_struct *);
-int csio_enqueue_evt(struct csio_hw *hw, enum csio_evt type,
-				void *evt_msg, uint16_t len);
+int csio_enqueue_evt(struct csio_hw *, enum csio_evt, void *, uint16_t);
 void csio_evtq_flush(struct csio_hw *hw);
 
 int csio_request_irqs(struct csio_hw *);
 void csio_intr_enable(struct csio_hw *);
 void csio_intr_disable(struct csio_hw *, bool);
+void csio_hw_fatal_err(struct csio_hw *);
 
 struct csio_lnode *csio_lnode_alloc(struct csio_hw *);
 int csio_config_queues(struct csio_hw *);
 
-int csio_hw_mc_read(struct csio_hw *, uint32_t, __be32 *, uint64_t *);
-int csio_hw_edc_read(struct csio_hw *, int, uint32_t, __be32 *, uint64_t *);
 int csio_hw_init(struct csio_hw *);
 void csio_hw_exit(struct csio_hw *);
 #endif /* ifndef __CSIO_HW_H__ */
diff --git a/drivers/scsi/csiostor/csio_hw_chip.h b/drivers/scsi/csiostor/csio_hw_chip.h
new file mode 100644
index 000000000000..bca0de61ae80
--- /dev/null
+++ b/drivers/scsi/csiostor/csio_hw_chip.h
@@ -0,0 +1,175 @@
+/*
+ * This file is part of the Chelsio FCoE driver for Linux.
+ *
+ * Copyright (c) 2008-2013 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef __CSIO_HW_CHIP_H__
+#define __CSIO_HW_CHIP_H__
+
+#include "csio_defs.h"
+
+/* FCoE device IDs for T4 */
+#define CSIO_DEVID_T440DBG_FCOE			0x4600
+#define CSIO_DEVID_T420CR_FCOE			0x4601
+#define CSIO_DEVID_T422CR_FCOE			0x4602
+#define CSIO_DEVID_T440CR_FCOE			0x4603
+#define CSIO_DEVID_T420BCH_FCOE			0x4604
+#define CSIO_DEVID_T440BCH_FCOE			0x4605
+#define CSIO_DEVID_T440CH_FCOE			0x4606
+#define CSIO_DEVID_T420SO_FCOE			0x4607
+#define CSIO_DEVID_T420CX_FCOE			0x4608
+#define CSIO_DEVID_T420BT_FCOE			0x4609
+#define CSIO_DEVID_T404BT_FCOE			0x460A
+#define CSIO_DEVID_B420_FCOE			0x460B
+#define CSIO_DEVID_B404_FCOE			0x460C
+#define CSIO_DEVID_T480CR_FCOE			0x460D
+#define CSIO_DEVID_T440LPCR_FCOE		0x460E
+#define CSIO_DEVID_AMSTERDAM_T4_FCOE		0x460F
+#define CSIO_DEVID_HUAWEI_T480_FCOE		0x4680
+#define CSIO_DEVID_HUAWEI_T440_FCOE		0x4681
+#define CSIO_DEVID_HUAWEI_STG310_FCOE		0x4682
+#define CSIO_DEVID_ACROMAG_XMC_XAUI		0x4683
+#define CSIO_DEVID_ACROMAG_XMC_SFP_FCOE		0x4684
+#define CSIO_DEVID_QUANTA_MEZZ_SFP_FCOE		0x4685
+#define CSIO_DEVID_HUAWEI_10GT_FCOE		0x4686
+#define CSIO_DEVID_HUAWEI_T440_TOE_FCOE		0x4687
+
+/* FCoE device IDs for T5 */
+#define CSIO_DEVID_T580DBG_FCOE			0x5600
+#define CSIO_DEVID_T520CR_FCOE			0x5601
+#define CSIO_DEVID_T522CR_FCOE			0x5602
+#define CSIO_DEVID_T540CR_FCOE			0x5603
+#define CSIO_DEVID_T520BCH_FCOE			0x5604
+#define CSIO_DEVID_T540BCH_FCOE			0x5605
+#define CSIO_DEVID_T540CH_FCOE			0x5606
+#define CSIO_DEVID_T520SO_FCOE			0x5607
+#define CSIO_DEVID_T520CX_FCOE			0x5608
+#define CSIO_DEVID_T520BT_FCOE			0x5609
+#define CSIO_DEVID_T504BT_FCOE			0x560A
+#define CSIO_DEVID_B520_FCOE			0x560B
+#define CSIO_DEVID_B504_FCOE			0x560C
+#define CSIO_DEVID_T580CR2_FCOE			0x560D
+#define CSIO_DEVID_T540LPCR_FCOE		0x560E
+#define CSIO_DEVID_AMSTERDAM_T5_FCOE		0x560F
+#define CSIO_DEVID_T580LPCR_FCOE		0x5610
+#define CSIO_DEVID_T520LLCR_FCOE		0x5611
+#define CSIO_DEVID_T560CR_FCOE			0x5612
+#define CSIO_DEVID_T580CR_FCOE			0x5613
+
+/* Define MACRO values */
+#define CSIO_HW_T4				0x4000
+#define CSIO_T4_FCOE_ASIC			0x4600
+#define CSIO_HW_T5				0x5000
+#define CSIO_T5_FCOE_ASIC			0x5600
+#define CSIO_HW_CHIP_MASK			0xF000
+#define T4_REGMAP_SIZE				(160 * 1024)
+#define T5_REGMAP_SIZE				(332 * 1024)
+#define FW_FNAME_T4				"cxgb4/t4fw.bin"
+#define FW_FNAME_T5				"cxgb4/t5fw.bin"
+#define FW_CFG_NAME_T4				"cxgb4/t4-config.txt"
+#define FW_CFG_NAME_T5				"cxgb4/t5-config.txt"
+
+/* Define static functions */
+static inline int csio_is_t4(uint16_t chip)
+{
+	return (chip == CSIO_HW_T4);
+}
+
+static inline int csio_is_t5(uint16_t chip)
+{
+	return (chip == CSIO_HW_T5);
+}
+
+/* Define MACRO DEFINITIONS */
+#define CSIO_DEVICE(devid, idx)						\
+	{ PCI_VENDOR_ID_CHELSIO, (devid), PCI_ANY_ID, PCI_ANY_ID, 0, 0, (idx) }
+
+#define CSIO_HW_PIDX(hw, index)						\
+	(csio_is_t4(hw->chip_id) ? (PIDX(index)) :			\
+					(PIDX_T5(index) | DBTYPE(1U)))
+
+#define CSIO_HW_LP_INT_THRESH(hw, val)					\
+	(csio_is_t4(hw->chip_id) ? (LP_INT_THRESH(val)) :		\
+					(V_LP_INT_THRESH_T5(val)))
+
+#define CSIO_HW_M_LP_INT_THRESH(hw)					\
+	(csio_is_t4(hw->chip_id) ? (LP_INT_THRESH_MASK) : (M_LP_INT_THRESH_T5))
+
+#define CSIO_MAC_INT_CAUSE_REG(hw, port)				\
+	(csio_is_t4(hw->chip_id) ? (PORT_REG(port, XGMAC_PORT_INT_CAUSE)) : \
+				(T5_PORT_REG(port, MAC_PORT_INT_CAUSE)))
+
+#define FW_VERSION_MAJOR(hw) (csio_is_t4(hw->chip_id) ? 1 : 0)
+#define FW_VERSION_MINOR(hw) (csio_is_t4(hw->chip_id) ? 2 : 0)
+#define FW_VERSION_MICRO(hw) (csio_is_t4(hw->chip_id) ? 8 : 0)
+
+#define CSIO_FW_FNAME(hw)						\
+	(csio_is_t4(hw->chip_id) ? FW_FNAME_T4 : FW_FNAME_T5)
+
+#define CSIO_CF_FNAME(hw)						\
+	(csio_is_t4(hw->chip_id) ? FW_CFG_NAME_T4 : FW_CFG_NAME_T5)
+
+/* Declare ENUMS */
+enum { MEM_EDC0, MEM_EDC1, MEM_MC, MEM_MC0 = MEM_MC, MEM_MC1 };
+
+enum {
+	MEMWIN_APERTURE = 2048,
+	MEMWIN_BASE     = 0x1b800,
+	MEMWIN_CSIOSTOR = 6,		/* PCI-e Memory Window access */
+};
+
+/* Slow path handlers */
+struct intr_info {
+	unsigned int mask;       /* bits to check in interrupt status */
+	const char *msg;         /* message to print or NULL */
+	short stat_idx;          /* stat counter to increment or -1 */
+	unsigned short fatal;    /* whether the condition reported is fatal */
+};
+
+/* T4/T5 Chip specific ops */
+struct csio_hw;
+struct csio_hw_chip_ops {
+	int (*chip_set_mem_win)(struct csio_hw *, uint32_t);
+	void (*chip_pcie_intr_handler)(struct csio_hw *);
+	uint32_t (*chip_flash_cfg_addr)(struct csio_hw *);
+	int (*chip_mc_read)(struct csio_hw *, int, uint32_t,
+					__be32 *, uint64_t *);
+	int (*chip_edc_read)(struct csio_hw *, int, uint32_t,
+					__be32 *, uint64_t *);
+	int (*chip_memory_rw)(struct csio_hw *, u32, int, u32,
+					u32, uint32_t *, int);
+	void (*chip_dfs_create_ext_mem)(struct csio_hw *);
+};
+
+extern struct csio_hw_chip_ops t4_ops;
+extern struct csio_hw_chip_ops t5_ops;
+
+#endif /* #ifndef __CSIO_HW_CHIP_H__ */
diff --git a/drivers/scsi/csiostor/csio_hw_t4.c b/drivers/scsi/csiostor/csio_hw_t4.c
new file mode 100644
index 000000000000..89ecbac5478f
--- /dev/null
+++ b/drivers/scsi/csiostor/csio_hw_t4.c
@@ -0,0 +1,403 @@
+/*
+ * This file is part of the Chelsio FCoE driver for Linux.
+ *
+ * Copyright (c) 2008-2013 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "csio_hw.h"
+#include "csio_init.h"
+
+/*
+ * Return the specified PCI-E Configuration Space register from our Physical
+ * Function.  We try first via a Firmware LDST Command since we prefer to let
+ * the firmware own all of these registers, but if that fails we go for it
+ * directly ourselves.
+ */
+static uint32_t
+csio_t4_read_pcie_cfg4(struct csio_hw *hw, int reg)
+{
+	u32 val = 0;
+	struct csio_mb *mbp;
+	int rv;
+	struct fw_ldst_cmd *ldst_cmd;
+
+	mbp = mempool_alloc(hw->mb_mempool, GFP_ATOMIC);
+	if (!mbp) {
+		CSIO_INC_STATS(hw, n_err_nomem);
+		pci_read_config_dword(hw->pdev, reg, &val);
+		return val;
+	}
+
+	csio_mb_ldst(hw, mbp, CSIO_MB_DEFAULT_TMO, reg);
+	rv = csio_mb_issue(hw, mbp);
+
+	/*
+	 * If the LDST Command suucceeded, exctract the returned register
+	 * value.  Otherwise read it directly ourself.
+	 */
+	if (rv == 0) {
+		ldst_cmd = (struct fw_ldst_cmd *)(mbp->mb);
+		val = ntohl(ldst_cmd->u.pcie.data[0]);
+	} else
+		pci_read_config_dword(hw->pdev, reg, &val);
+
+	mempool_free(mbp, hw->mb_mempool);
+
+	return val;
+}
+
+static int
+csio_t4_set_mem_win(struct csio_hw *hw, uint32_t win)
+{
+	u32 bar0;
+	u32 mem_win_base;
+
+	/*
+	 * Truncation intentional: we only read the bottom 32-bits of the
+	 * 64-bit BAR0/BAR1 ...  We use the hardware backdoor mechanism to
+	 * read BAR0 instead of using pci_resource_start() because we could be
+	 * operating from within a Virtual Machine which is trapping our
+	 * accesses to our Configuration Space and we need to set up the PCI-E
+	 * Memory Window decoders with the actual addresses which will be
+	 * coming across the PCI-E link.
+	 */
+	bar0 = csio_t4_read_pcie_cfg4(hw, PCI_BASE_ADDRESS_0);
+	bar0 &= PCI_BASE_ADDRESS_MEM_MASK;
+
+	mem_win_base = bar0 + MEMWIN_BASE;
+
+	/*
+	 * Set up memory window for accessing adapter memory ranges.  (Read
+	 * back MA register to ensure that changes propagate before we attempt
+	 * to use the new values.)
+	 */
+	csio_wr_reg32(hw, mem_win_base | BIR(0) |
+			  WINDOW(ilog2(MEMWIN_APERTURE) - 10),
+			  PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN, win));
+	csio_rd_reg32(hw,
+		      PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN, win));
+	return 0;
+}
+
+/*
+ * Interrupt handler for the PCIE module.
+ */
+static void
+csio_t4_pcie_intr_handler(struct csio_hw *hw)
+{
+	static struct intr_info sysbus_intr_info[] = {
+		{ RNPP, "RXNP array parity error", -1, 1 },
+		{ RPCP, "RXPC array parity error", -1, 1 },
+		{ RCIP, "RXCIF array parity error", -1, 1 },
+		{ RCCP, "Rx completions control array parity error", -1, 1 },
+		{ RFTP, "RXFT array parity error", -1, 1 },
+		{ 0, NULL, 0, 0 }
+	};
+	static struct intr_info pcie_port_intr_info[] = {
+		{ TPCP, "TXPC array parity error", -1, 1 },
+		{ TNPP, "TXNP array parity error", -1, 1 },
+		{ TFTP, "TXFT array parity error", -1, 1 },
+		{ TCAP, "TXCA array parity error", -1, 1 },
+		{ TCIP, "TXCIF array parity error", -1, 1 },
+		{ RCAP, "RXCA array parity error", -1, 1 },
+		{ OTDD, "outbound request TLP discarded", -1, 1 },
+		{ RDPE, "Rx data parity error", -1, 1 },
+		{ TDUE, "Tx uncorrectable data error", -1, 1 },
+		{ 0, NULL, 0, 0 }
+	};
+
+	static struct intr_info pcie_intr_info[] = {
+		{ MSIADDRLPERR, "MSI AddrL parity error", -1, 1 },
+		{ MSIADDRHPERR, "MSI AddrH parity error", -1, 1 },
+		{ MSIDATAPERR, "MSI data parity error", -1, 1 },
+		{ MSIXADDRLPERR, "MSI-X AddrL parity error", -1, 1 },
+		{ MSIXADDRHPERR, "MSI-X AddrH parity error", -1, 1 },
+		{ MSIXDATAPERR, "MSI-X data parity error", -1, 1 },
+		{ MSIXDIPERR, "MSI-X DI parity error", -1, 1 },
+		{ PIOCPLPERR, "PCI PIO completion FIFO parity error", -1, 1 },
+		{ PIOREQPERR, "PCI PIO request FIFO parity error", -1, 1 },
+		{ TARTAGPERR, "PCI PCI target tag FIFO parity error", -1, 1 },
+		{ CCNTPERR, "PCI CMD channel count parity error", -1, 1 },
+		{ CREQPERR, "PCI CMD channel request parity error", -1, 1 },
+		{ CRSPPERR, "PCI CMD channel response parity error", -1, 1 },
+		{ DCNTPERR, "PCI DMA channel count parity error", -1, 1 },
+		{ DREQPERR, "PCI DMA channel request parity error", -1, 1 },
+		{ DRSPPERR, "PCI DMA channel response parity error", -1, 1 },
+		{ HCNTPERR, "PCI HMA channel count parity error", -1, 1 },
+		{ HREQPERR, "PCI HMA channel request parity error", -1, 1 },
+		{ HRSPPERR, "PCI HMA channel response parity error", -1, 1 },
+		{ CFGSNPPERR, "PCI config snoop FIFO parity error", -1, 1 },
+		{ FIDPERR, "PCI FID parity error", -1, 1 },
+		{ INTXCLRPERR, "PCI INTx clear parity error", -1, 1 },
+		{ MATAGPERR, "PCI MA tag parity error", -1, 1 },
+		{ PIOTAGPERR, "PCI PIO tag parity error", -1, 1 },
+		{ RXCPLPERR, "PCI Rx completion parity error", -1, 1 },
+		{ RXWRPERR, "PCI Rx write parity error", -1, 1 },
+		{ RPLPERR, "PCI replay buffer parity error", -1, 1 },
+		{ PCIESINT, "PCI core secondary fault", -1, 1 },
+		{ PCIEPINT, "PCI core primary fault", -1, 1 },
+		{ UNXSPLCPLERR, "PCI unexpected split completion error", -1,
+		  0 },
+		{ 0, NULL, 0, 0 }
+	};
+
+	int fat;
+	fat = csio_handle_intr_status(hw,
+				      PCIE_CORE_UTL_SYSTEM_BUS_AGENT_STATUS,
+				      sysbus_intr_info) +
+	      csio_handle_intr_status(hw,
+				      PCIE_CORE_UTL_PCI_EXPRESS_PORT_STATUS,
+				      pcie_port_intr_info) +
+	      csio_handle_intr_status(hw, PCIE_INT_CAUSE, pcie_intr_info);
+	if (fat)
+		csio_hw_fatal_err(hw);
+}
+
+/*
+ * csio_t4_flash_cfg_addr - return the address of the flash configuration file
+ * @hw: the HW module
+ *
+ * Return the address within the flash where the Firmware Configuration
+ * File is stored.
+ */
+static unsigned int
+csio_t4_flash_cfg_addr(struct csio_hw *hw)
+{
+	return FLASH_CFG_OFFSET;
+}
+
+/*
+ *      csio_t4_mc_read - read from MC through backdoor accesses
+ *      @hw: the hw module
+ *      @idx: not used for T4 adapter
+ *      @addr: address of first byte requested
+ *      @data: 64 bytes of data containing the requested address
+ *      @ecc: where to store the corresponding 64-bit ECC word
+ *
+ *      Read 64 bytes of data from MC starting at a 64-byte-aligned address
+ *      that covers the requested address @addr.  If @parity is not %NULL it
+ *      is assigned the 64-bit ECC word for the read data.
+ */
+static int
+csio_t4_mc_read(struct csio_hw *hw, int idx, uint32_t addr, __be32 *data,
+		uint64_t *ecc)
+{
+	int i;
+
+	if (csio_rd_reg32(hw, MC_BIST_CMD) & START_BIST)
+		return -EBUSY;
+	csio_wr_reg32(hw, addr & ~0x3fU, MC_BIST_CMD_ADDR);
+	csio_wr_reg32(hw, 64, MC_BIST_CMD_LEN);
+	csio_wr_reg32(hw, 0xc, MC_BIST_DATA_PATTERN);
+	csio_wr_reg32(hw, BIST_OPCODE(1) | START_BIST | BIST_CMD_GAP(1),
+		      MC_BIST_CMD);
+	i = csio_hw_wait_op_done_val(hw, MC_BIST_CMD, START_BIST,
+				     0, 10, 1, NULL);
+	if (i)
+		return i;
+
+#define MC_DATA(i) MC_BIST_STATUS_REG(MC_BIST_STATUS_RDATA, i)
+
+	for (i = 15; i >= 0; i--)
+		*data++ = htonl(csio_rd_reg32(hw, MC_DATA(i)));
+	if (ecc)
+		*ecc = csio_rd_reg64(hw, MC_DATA(16));
+#undef MC_DATA
+	return 0;
+}
+
+/*
+ *      csio_t4_edc_read - read from EDC through backdoor accesses
+ *      @hw: the hw module
+ *      @idx: which EDC to access
+ *      @addr: address of first byte requested
+ *      @data: 64 bytes of data containing the requested address
+ *      @ecc: where to store the corresponding 64-bit ECC word
+ *
+ *      Read 64 bytes of data from EDC starting at a 64-byte-aligned address
+ *      that covers the requested address @addr.  If @parity is not %NULL it
+ *      is assigned the 64-bit ECC word for the read data.
+ */
+static int
+csio_t4_edc_read(struct csio_hw *hw, int idx, uint32_t addr, __be32 *data,
+		uint64_t *ecc)
+{
+	int i;
+
+	idx *= EDC_STRIDE;
+	if (csio_rd_reg32(hw, EDC_BIST_CMD + idx) & START_BIST)
+		return -EBUSY;
+	csio_wr_reg32(hw, addr & ~0x3fU, EDC_BIST_CMD_ADDR + idx);
+	csio_wr_reg32(hw, 64, EDC_BIST_CMD_LEN + idx);
+	csio_wr_reg32(hw, 0xc, EDC_BIST_DATA_PATTERN + idx);
+	csio_wr_reg32(hw, BIST_OPCODE(1) | BIST_CMD_GAP(1) | START_BIST,
+		      EDC_BIST_CMD + idx);
+	i = csio_hw_wait_op_done_val(hw, EDC_BIST_CMD + idx, START_BIST,
+				     0, 10, 1, NULL);
+	if (i)
+		return i;
+
+#define EDC_DATA(i) (EDC_BIST_STATUS_REG(EDC_BIST_STATUS_RDATA, i) + idx)
+
+	for (i = 15; i >= 0; i--)
+		*data++ = htonl(csio_rd_reg32(hw, EDC_DATA(i)));
+	if (ecc)
+		*ecc = csio_rd_reg64(hw, EDC_DATA(16));
+#undef EDC_DATA
+	return 0;
+}
+
+/*
+ * csio_t4_memory_rw - read/write EDC 0, EDC 1 or MC via PCIE memory window
+ * @hw: the csio_hw
+ * @win: PCI-E memory Window to use
+ * @mtype: memory type: MEM_EDC0, MEM_EDC1, MEM_MC0 (or MEM_MC) or MEM_MC1
+ * @addr: address within indicated memory type
+ * @len: amount of memory to transfer
+ * @buf: host memory buffer
+ * @dir: direction of transfer 1 => read, 0 => write
+ *
+ * Reads/writes an [almost] arbitrary memory region in the firmware: the
+ * firmware memory address, length and host buffer must be aligned on
+ * 32-bit boudaries.  The memory is transferred as a raw byte sequence
+ * from/to the firmware's memory.  If this memory contains data
+ * structures which contain multi-byte integers, it's the callers
+ * responsibility to perform appropriate byte order conversions.
+ */
+static int
+csio_t4_memory_rw(struct csio_hw *hw, u32 win, int mtype, u32 addr,
+		u32 len, uint32_t *buf, int dir)
+{
+	u32 pos, start, offset, memoffset, bar0;
+	u32 edc_size, mc_size, mem_reg, mem_aperture, mem_base;
+
+	/*
+	 * Argument sanity checks ...
+	 */
+	if ((addr & 0x3) || (len & 0x3))
+		return -EINVAL;
+
+	/* Offset into the region of memory which is being accessed
+	 * MEM_EDC0 = 0
+	 * MEM_EDC1 = 1
+	 * MEM_MC   = 2 -- T4
+	 */
+	edc_size  = EDRAM_SIZE_GET(csio_rd_reg32(hw, MA_EDRAM0_BAR));
+	if (mtype != MEM_MC1)
+		memoffset = (mtype * (edc_size * 1024 * 1024));
+	else {
+		mc_size = EXT_MEM_SIZE_GET(csio_rd_reg32(hw,
+							 MA_EXT_MEMORY_BAR));
+		memoffset = (MEM_MC0 * edc_size + mc_size) * 1024 * 1024;
+	}
+
+	/* Determine the PCIE_MEM_ACCESS_OFFSET */
+	addr = addr + memoffset;
+
+	/*
+	 * Each PCI-E Memory Window is programmed with a window size -- or
+	 * "aperture" -- which controls the granularity of its mapping onto
+	 * adapter memory.  We need to grab that aperture in order to know
+	 * how to use the specified window.  The window is also programmed
+	 * with the base address of the Memory Window in BAR0's address
+	 * space.  For T4 this is an absolute PCI-E Bus Address.  For T5
+	 * the address is relative to BAR0.
+	 */
+	mem_reg = csio_rd_reg32(hw,
+			PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN, win));
+	mem_aperture = 1 << (WINDOW(mem_reg) + 10);
+	mem_base = GET_PCIEOFST(mem_reg) << 10;
+
+	bar0 = csio_t4_read_pcie_cfg4(hw, PCI_BASE_ADDRESS_0);
+	bar0 &= PCI_BASE_ADDRESS_MEM_MASK;
+	mem_base -= bar0;
+
+	start = addr & ~(mem_aperture-1);
+	offset = addr - start;
+
+	csio_dbg(hw, "csio_t4_memory_rw: mem_reg: 0x%x, mem_aperture: 0x%x\n",
+		 mem_reg, mem_aperture);
+	csio_dbg(hw, "csio_t4_memory_rw: mem_base: 0x%x, mem_offset: 0x%x\n",
+		 mem_base, memoffset);
+	csio_dbg(hw, "csio_t4_memory_rw: bar0: 0x%x, start:0x%x, offset:0x%x\n",
+		 bar0, start, offset);
+	csio_dbg(hw, "csio_t4_memory_rw: mtype: %d, addr: 0x%x, len: %d\n",
+		 mtype, addr, len);
+
+	for (pos = start; len > 0; pos += mem_aperture, offset = 0) {
+		/*
+		 * Move PCI-E Memory Window to our current transfer
+		 * position.  Read it back to ensure that changes propagate
+		 * before we attempt to use the new value.
+		 */
+		csio_wr_reg32(hw, pos,
+			PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_OFFSET, win));
+		csio_rd_reg32(hw,
+			PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_OFFSET, win));
+
+		while (offset < mem_aperture && len > 0) {
+			if (dir)
+				*buf++ = csio_rd_reg32(hw, mem_base + offset);
+			else
+				csio_wr_reg32(hw, *buf++, mem_base + offset);
+
+			offset += sizeof(__be32);
+			len -= sizeof(__be32);
+		}
+	}
+	return 0;
+}
+
+/*
+ * csio_t4_dfs_create_ext_mem - setup debugfs for MC to read the values
+ * @hw: the csio_hw
+ *
+ * This function creates files in the debugfs with external memory region MC.
+ */
+static void
+csio_t4_dfs_create_ext_mem(struct csio_hw *hw)
+{
+	u32 size;
+	int i = csio_rd_reg32(hw, MA_TARGET_MEM_ENABLE);
+	if (i & EXT_MEM_ENABLE) {
+		size = csio_rd_reg32(hw, MA_EXT_MEMORY_BAR);
+		csio_add_debugfs_mem(hw, "mc", MEM_MC,
+				     EXT_MEM_SIZE_GET(size));
+	}
+}
+
+/* T4 adapter specific function */
+struct csio_hw_chip_ops t4_ops = {
+	.chip_set_mem_win		= csio_t4_set_mem_win,
+	.chip_pcie_intr_handler		= csio_t4_pcie_intr_handler,
+	.chip_flash_cfg_addr		= csio_t4_flash_cfg_addr,
+	.chip_mc_read			= csio_t4_mc_read,
+	.chip_edc_read			= csio_t4_edc_read,
+	.chip_memory_rw			= csio_t4_memory_rw,
+	.chip_dfs_create_ext_mem	= csio_t4_dfs_create_ext_mem,
+};
diff --git a/drivers/scsi/csiostor/csio_hw_t5.c b/drivers/scsi/csiostor/csio_hw_t5.c
new file mode 100644
index 000000000000..27745c170c24
--- /dev/null
+++ b/drivers/scsi/csiostor/csio_hw_t5.c
@@ -0,0 +1,397 @@
+/*
+ * This file is part of the Chelsio FCoE driver for Linux.
+ *
+ * Copyright (c) 2008-2013 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "csio_hw.h"
+#include "csio_init.h"
+
+static int
+csio_t5_set_mem_win(struct csio_hw *hw, uint32_t win)
+{
+	u32 mem_win_base;
+	/*
+	 * Truncation intentional: we only read the bottom 32-bits of the
+	 * 64-bit BAR0/BAR1 ...  We use the hardware backdoor mechanism to
+	 * read BAR0 instead of using pci_resource_start() because we could be
+	 * operating from within a Virtual Machine which is trapping our
+	 * accesses to our Configuration Space and we need to set up the PCI-E
+	 * Memory Window decoders with the actual addresses which will be
+	 * coming across the PCI-E link.
+	 */
+
+	/* For T5, only relative offset inside the PCIe BAR is passed */
+	mem_win_base = MEMWIN_BASE;
+
+	/*
+	 * Set up memory window for accessing adapter memory ranges.  (Read
+	 * back MA register to ensure that changes propagate before we attempt
+	 * to use the new values.)
+	 */
+	csio_wr_reg32(hw, mem_win_base | BIR(0) |
+			  WINDOW(ilog2(MEMWIN_APERTURE) - 10),
+			  PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN, win));
+	csio_rd_reg32(hw,
+		      PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN, win));
+
+	return 0;
+}
+
+/*
+ * Interrupt handler for the PCIE module.
+ */
+static void
+csio_t5_pcie_intr_handler(struct csio_hw *hw)
+{
+	static struct intr_info sysbus_intr_info[] = {
+		{ RNPP, "RXNP array parity error", -1, 1 },
+		{ RPCP, "RXPC array parity error", -1, 1 },
+		{ RCIP, "RXCIF array parity error", -1, 1 },
+		{ RCCP, "Rx completions control array parity error", -1, 1 },
+		{ RFTP, "RXFT array parity error", -1, 1 },
+		{ 0, NULL, 0, 0 }
+	};
+	static struct intr_info pcie_port_intr_info[] = {
+		{ TPCP, "TXPC array parity error", -1, 1 },
+		{ TNPP, "TXNP array parity error", -1, 1 },
+		{ TFTP, "TXFT array parity error", -1, 1 },
+		{ TCAP, "TXCA array parity error", -1, 1 },
+		{ TCIP, "TXCIF array parity error", -1, 1 },
+		{ RCAP, "RXCA array parity error", -1, 1 },
+		{ OTDD, "outbound request TLP discarded", -1, 1 },
+		{ RDPE, "Rx data parity error", -1, 1 },
+		{ TDUE, "Tx uncorrectable data error", -1, 1 },
+		{ 0, NULL, 0, 0 }
+	};
+
+	static struct intr_info pcie_intr_info[] = {
+		{ MSTGRPPERR, "Master Response Read Queue parity error",
+		-1, 1 },
+		{ MSTTIMEOUTPERR, "Master Timeout FIFO parity error", -1, 1 },
+		{ MSIXSTIPERR, "MSI-X STI SRAM parity error", -1, 1 },
+		{ MSIXADDRLPERR, "MSI-X AddrL parity error", -1, 1 },
+		{ MSIXADDRHPERR, "MSI-X AddrH parity error", -1, 1 },
+		{ MSIXDATAPERR, "MSI-X data parity error", -1, 1 },
+		{ MSIXDIPERR, "MSI-X DI parity error", -1, 1 },
+		{ PIOCPLGRPPERR, "PCI PIO completion Group FIFO parity error",
+		-1, 1 },
+		{ PIOREQGRPPERR, "PCI PIO request Group FIFO parity error",
+		-1, 1 },
+		{ TARTAGPERR, "PCI PCI target tag FIFO parity error", -1, 1 },
+		{ MSTTAGQPERR, "PCI master tag queue parity error", -1, 1 },
+		{ CREQPERR, "PCI CMD channel request parity error", -1, 1 },
+		{ CRSPPERR, "PCI CMD channel response parity error", -1, 1 },
+		{ DREQWRPERR, "PCI DMA channel write request parity error",
+		-1, 1 },
+		{ DREQPERR, "PCI DMA channel request parity error", -1, 1 },
+		{ DRSPPERR, "PCI DMA channel response parity error", -1, 1 },
+		{ HREQWRPERR, "PCI HMA channel count parity error", -1, 1 },
+		{ HREQPERR, "PCI HMA channel request parity error", -1, 1 },
+		{ HRSPPERR, "PCI HMA channel response parity error", -1, 1 },
+		{ CFGSNPPERR, "PCI config snoop FIFO parity error", -1, 1 },
+		{ FIDPERR, "PCI FID parity error", -1, 1 },
+		{ VFIDPERR, "PCI INTx clear parity error", -1, 1 },
+		{ MAGRPPERR, "PCI MA group FIFO parity error", -1, 1 },
+		{ PIOTAGPERR, "PCI PIO tag parity error", -1, 1 },
+		{ IPRXHDRGRPPERR, "PCI IP Rx header group parity error",
+		-1, 1 },
+		{ IPRXDATAGRPPERR, "PCI IP Rx data group parity error",
+		-1, 1 },
+		{ RPLPERR, "PCI IP replay buffer parity error", -1, 1 },
+		{ IPSOTPERR, "PCI IP SOT buffer parity error", -1, 1 },
+		{ TRGT1GRPPERR, "PCI TRGT1 group FIFOs parity error", -1, 1 },
+		{ READRSPERR, "Outbound read error", -1, 0 },
+		{ 0, NULL, 0, 0 }
+	};
+
+	int fat;
+	fat = csio_handle_intr_status(hw,
+				      PCIE_CORE_UTL_SYSTEM_BUS_AGENT_STATUS,
+				      sysbus_intr_info) +
+	      csio_handle_intr_status(hw,
+				      PCIE_CORE_UTL_PCI_EXPRESS_PORT_STATUS,
+				      pcie_port_intr_info) +
+	      csio_handle_intr_status(hw, PCIE_INT_CAUSE, pcie_intr_info);
+	if (fat)
+		csio_hw_fatal_err(hw);
+}
+
+/*
+ * csio_t5_flash_cfg_addr - return the address of the flash configuration file
+ * @hw: the HW module
+ *
+ * Return the address within the flash where the Firmware Configuration
+ * File is stored.
+ */
+static unsigned int
+csio_t5_flash_cfg_addr(struct csio_hw *hw)
+{
+	return FLASH_CFG_START;
+}
+
+/*
+ *      csio_t5_mc_read - read from MC through backdoor accesses
+ *      @hw: the hw module
+ *      @idx: index to the register
+ *      @addr: address of first byte requested
+ *      @data: 64 bytes of data containing the requested address
+ *      @ecc: where to store the corresponding 64-bit ECC word
+ *
+ *      Read 64 bytes of data from MC starting at a 64-byte-aligned address
+ *      that covers the requested address @addr.  If @parity is not %NULL it
+ *      is assigned the 64-bit ECC word for the read data.
+ */
+static int
+csio_t5_mc_read(struct csio_hw *hw, int idx, uint32_t addr, __be32 *data,
+		uint64_t *ecc)
+{
+	int i;
+	uint32_t mc_bist_cmd_reg, mc_bist_cmd_addr_reg, mc_bist_cmd_len_reg;
+	uint32_t mc_bist_status_rdata_reg, mc_bist_data_pattern_reg;
+
+	mc_bist_cmd_reg = MC_REG(MC_P_BIST_CMD, idx);
+	mc_bist_cmd_addr_reg = MC_REG(MC_P_BIST_CMD_ADDR, idx);
+	mc_bist_cmd_len_reg = MC_REG(MC_P_BIST_CMD_LEN, idx);
+	mc_bist_status_rdata_reg = MC_REG(MC_P_BIST_STATUS_RDATA, idx);
+	mc_bist_data_pattern_reg = MC_REG(MC_P_BIST_DATA_PATTERN, idx);
+
+	if (csio_rd_reg32(hw, mc_bist_cmd_reg) & START_BIST)
+		return -EBUSY;
+	csio_wr_reg32(hw, addr & ~0x3fU, mc_bist_cmd_addr_reg);
+	csio_wr_reg32(hw, 64, mc_bist_cmd_len_reg);
+	csio_wr_reg32(hw, 0xc, mc_bist_data_pattern_reg);
+	csio_wr_reg32(hw, BIST_OPCODE(1) | START_BIST |  BIST_CMD_GAP(1),
+		      mc_bist_cmd_reg);
+	i = csio_hw_wait_op_done_val(hw, mc_bist_cmd_reg, START_BIST,
+				     0, 10, 1, NULL);
+	if (i)
+		return i;
+
+#define MC_DATA(i) MC_BIST_STATUS_REG(MC_BIST_STATUS_RDATA, i)
+
+	for (i = 15; i >= 0; i--)
+		*data++ = htonl(csio_rd_reg32(hw, MC_DATA(i)));
+	if (ecc)
+		*ecc = csio_rd_reg64(hw, MC_DATA(16));
+#undef MC_DATA
+	return 0;
+}
+
+/*
+ *      csio_t5_edc_read - read from EDC through backdoor accesses
+ *      @hw: the hw module
+ *      @idx: which EDC to access
+ *      @addr: address of first byte requested
+ *      @data: 64 bytes of data containing the requested address
+ *      @ecc: where to store the corresponding 64-bit ECC word
+ *
+ *      Read 64 bytes of data from EDC starting at a 64-byte-aligned address
+ *      that covers the requested address @addr.  If @parity is not %NULL it
+ *      is assigned the 64-bit ECC word for the read data.
+ */
+static int
+csio_t5_edc_read(struct csio_hw *hw, int idx, uint32_t addr, __be32 *data,
+		uint64_t *ecc)
+{
+	int i;
+	uint32_t edc_bist_cmd_reg, edc_bist_cmd_addr_reg, edc_bist_cmd_len_reg;
+	uint32_t edc_bist_cmd_data_pattern, edc_bist_status_rdata_reg;
+
+/*
+ * These macro are missing in t4_regs.h file.
+ */
+#define EDC_STRIDE_T5 (EDC_T51_BASE_ADDR - EDC_T50_BASE_ADDR)
+#define EDC_REG_T5(reg, idx) (reg + EDC_STRIDE_T5 * idx)
+
+	edc_bist_cmd_reg = EDC_REG_T5(EDC_H_BIST_CMD, idx);
+	edc_bist_cmd_addr_reg = EDC_REG_T5(EDC_H_BIST_CMD_ADDR, idx);
+	edc_bist_cmd_len_reg = EDC_REG_T5(EDC_H_BIST_CMD_LEN, idx);
+	edc_bist_cmd_data_pattern = EDC_REG_T5(EDC_H_BIST_DATA_PATTERN, idx);
+	edc_bist_status_rdata_reg = EDC_REG_T5(EDC_H_BIST_STATUS_RDATA, idx);
+#undef EDC_REG_T5
+#undef EDC_STRIDE_T5
+
+	if (csio_rd_reg32(hw, edc_bist_cmd_reg) & START_BIST)
+		return -EBUSY;
+	csio_wr_reg32(hw, addr & ~0x3fU, edc_bist_cmd_addr_reg);
+	csio_wr_reg32(hw, 64, edc_bist_cmd_len_reg);
+	csio_wr_reg32(hw, 0xc, edc_bist_cmd_data_pattern);
+	csio_wr_reg32(hw, BIST_OPCODE(1) | START_BIST |  BIST_CMD_GAP(1),
+		      edc_bist_cmd_reg);
+	i = csio_hw_wait_op_done_val(hw, edc_bist_cmd_reg, START_BIST,
+				     0, 10, 1, NULL);
+	if (i)
+		return i;
+
+#define EDC_DATA(i) (EDC_BIST_STATUS_REG(EDC_BIST_STATUS_RDATA, i) + idx)
+
+	for (i = 15; i >= 0; i--)
+		*data++ = htonl(csio_rd_reg32(hw, EDC_DATA(i)));
+	if (ecc)
+		*ecc = csio_rd_reg64(hw, EDC_DATA(16));
+#undef EDC_DATA
+	return 0;
+}
+
+/*
+ * csio_t5_memory_rw - read/write EDC 0, EDC 1 or MC via PCIE memory window
+ * @hw: the csio_hw
+ * @win: PCI-E memory Window to use
+ * @mtype: memory type: MEM_EDC0, MEM_EDC1, MEM_MC0 (or MEM_MC) or MEM_MC1
+ * @addr: address within indicated memory type
+ * @len: amount of memory to transfer
+ * @buf: host memory buffer
+ * @dir: direction of transfer 1 => read, 0 => write
+ *
+ * Reads/writes an [almost] arbitrary memory region in the firmware: the
+ * firmware memory address, length and host buffer must be aligned on
+ * 32-bit boudaries.  The memory is transferred as a raw byte sequence
+ * from/to the firmware's memory.  If this memory contains data
+ * structures which contain multi-byte integers, it's the callers
+ * responsibility to perform appropriate byte order conversions.
+ */
+static int
+csio_t5_memory_rw(struct csio_hw *hw, u32 win, int mtype, u32 addr,
+		u32 len, uint32_t *buf, int dir)
+{
+	u32 pos, start, offset, memoffset;
+	u32 edc_size, mc_size, win_pf, mem_reg, mem_aperture, mem_base;
+
+	/*
+	 * Argument sanity checks ...
+	 */
+	if ((addr & 0x3) || (len & 0x3))
+		return -EINVAL;
+
+	/* Offset into the region of memory which is being accessed
+	 * MEM_EDC0 = 0
+	 * MEM_EDC1 = 1
+	 * MEM_MC   = 2 -- T4
+	 * MEM_MC0  = 2 -- For T5
+	 * MEM_MC1  = 3 -- For T5
+	 */
+	edc_size  = EDRAM_SIZE_GET(csio_rd_reg32(hw, MA_EDRAM0_BAR));
+	if (mtype != MEM_MC1)
+		memoffset = (mtype * (edc_size * 1024 * 1024));
+	else {
+		mc_size = EXT_MEM_SIZE_GET(csio_rd_reg32(hw,
+							 MA_EXT_MEMORY_BAR));
+		memoffset = (MEM_MC0 * edc_size + mc_size) * 1024 * 1024;
+	}
+
+	/* Determine the PCIE_MEM_ACCESS_OFFSET */
+	addr = addr + memoffset;
+
+	/*
+	 * Each PCI-E Memory Window is programmed with a window size -- or
+	 * "aperture" -- which controls the granularity of its mapping onto
+	 * adapter memory.  We need to grab that aperture in order to know
+	 * how to use the specified window.  The window is also programmed
+	 * with the base address of the Memory Window in BAR0's address
+	 * space.  For T4 this is an absolute PCI-E Bus Address.  For T5
+	 * the address is relative to BAR0.
+	 */
+	mem_reg = csio_rd_reg32(hw,
+			PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN, win));
+	mem_aperture = 1 << (WINDOW(mem_reg) + 10);
+	mem_base = GET_PCIEOFST(mem_reg) << 10;
+
+	start = addr & ~(mem_aperture-1);
+	offset = addr - start;
+	win_pf = V_PFNUM(hw->pfn);
+
+	csio_dbg(hw, "csio_t5_memory_rw: mem_reg: 0x%x, mem_aperture: 0x%x\n",
+		 mem_reg, mem_aperture);
+	csio_dbg(hw, "csio_t5_memory_rw: mem_base: 0x%x, mem_offset: 0x%x\n",
+		 mem_base, memoffset);
+	csio_dbg(hw, "csio_t5_memory_rw: start:0x%x, offset:0x%x, win_pf:%d\n",
+		 start, offset, win_pf);
+	csio_dbg(hw, "csio_t5_memory_rw: mtype: %d, addr: 0x%x, len: %d\n",
+		 mtype, addr, len);
+
+	for (pos = start; len > 0; pos += mem_aperture, offset = 0) {
+		/*
+		 * Move PCI-E Memory Window to our current transfer
+		 * position.  Read it back to ensure that changes propagate
+		 * before we attempt to use the new value.
+		 */
+		csio_wr_reg32(hw, pos | win_pf,
+			PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_OFFSET, win));
+		csio_rd_reg32(hw,
+			PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_OFFSET, win));
+
+		while (offset < mem_aperture && len > 0) {
+			if (dir)
+				*buf++ = csio_rd_reg32(hw, mem_base + offset);
+			else
+				csio_wr_reg32(hw, *buf++, mem_base + offset);
+
+			offset += sizeof(__be32);
+			len -= sizeof(__be32);
+		}
+	}
+	return 0;
+}
+
+/*
+ * csio_t5_dfs_create_ext_mem - setup debugfs for MC0 or MC1 to read the values
+ * @hw: the csio_hw
+ *
+ * This function creates files in the debugfs with external memory region
+ * MC0 & MC1.
+ */
+static void
+csio_t5_dfs_create_ext_mem(struct csio_hw *hw)
+{
+	u32 size;
+	int i = csio_rd_reg32(hw, MA_TARGET_MEM_ENABLE);
+	if (i & EXT_MEM_ENABLE) {
+		size = csio_rd_reg32(hw, MA_EXT_MEMORY_BAR);
+		csio_add_debugfs_mem(hw, "mc0", MEM_MC0,
+				     EXT_MEM_SIZE_GET(size));
+	}
+	if (i & EXT_MEM1_ENABLE) {
+		size = csio_rd_reg32(hw, MA_EXT_MEMORY1_BAR);
+		csio_add_debugfs_mem(hw, "mc1", MEM_MC1,
+				     EXT_MEM_SIZE_GET(size));
+	}
+}
+
+/* T5 adapter specific function */
+struct csio_hw_chip_ops t5_ops = {
+	.chip_set_mem_win		= csio_t5_set_mem_win,
+	.chip_pcie_intr_handler		= csio_t5_pcie_intr_handler,
+	.chip_flash_cfg_addr		= csio_t5_flash_cfg_addr,
+	.chip_mc_read			= csio_t5_mc_read,
+	.chip_edc_read			= csio_t5_edc_read,
+	.chip_memory_rw			= csio_t5_memory_rw,
+	.chip_dfs_create_ext_mem	= csio_t5_dfs_create_ext_mem,
+};
diff --git a/drivers/scsi/csiostor/csio_init.c b/drivers/scsi/csiostor/csio_init.c
index 0604b5ff3638..00346fe939d5 100644
--- a/drivers/scsi/csiostor/csio_init.c
+++ b/drivers/scsi/csiostor/csio_init.c
@@ -81,9 +81,11 @@ csio_mem_read(struct file *file, char __user *buf, size_t count, loff_t *ppos)
 		__be32 data[16];
 
 		if (mem == MEM_MC)
-			ret = csio_hw_mc_read(hw, pos, data, NULL);
+			ret = hw->chip_ops->chip_mc_read(hw, 0, pos,
+							 data, NULL);
 		else
-			ret = csio_hw_edc_read(hw, mem, pos, data, NULL);
+			ret = hw->chip_ops->chip_edc_read(hw, mem, pos,
+							  data, NULL);
 		if (ret)
 			return ret;
 
@@ -108,7 +110,7 @@ static const struct file_operations csio_mem_debugfs_fops = {
 	.llseek  = default_llseek,
 };
 
-static void csio_add_debugfs_mem(struct csio_hw *hw, const char *name,
+void csio_add_debugfs_mem(struct csio_hw *hw, const char *name,
 				 unsigned int idx, unsigned int size_mb)
 {
 	struct dentry *de;
@@ -131,9 +133,8 @@ static int csio_setup_debugfs(struct csio_hw *hw)
 		csio_add_debugfs_mem(hw, "edc0", MEM_EDC0, 5);
 	if (i & EDRAM1_ENABLE)
 		csio_add_debugfs_mem(hw, "edc1", MEM_EDC1, 5);
-	if (i & EXT_MEM_ENABLE)
-		csio_add_debugfs_mem(hw, "mc", MEM_MC,
-		      EXT_MEM_SIZE_GET(csio_rd_reg32(hw, MA_EXT_MEMORY_BAR)));
+
+	hw->chip_ops->chip_dfs_create_ext_mem(hw);
 	return 0;
 }
 
@@ -1169,7 +1170,7 @@ static struct pci_error_handlers csio_err_handler = {
 };
 
 static DEFINE_PCI_DEVICE_TABLE(csio_pci_tbl) = {
-	CSIO_DEVICE(CSIO_DEVID_T440DBG_FCOE, 0),	/* T440DBG FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T440DBG_FCOE, 0),        /* T4 DEBUG FCOE */
 	CSIO_DEVICE(CSIO_DEVID_T420CR_FCOE, 0),		/* T420CR FCOE */
 	CSIO_DEVICE(CSIO_DEVID_T422CR_FCOE, 0),		/* T422CR FCOE */
 	CSIO_DEVICE(CSIO_DEVID_T440CR_FCOE, 0),		/* T440CR FCOE */
@@ -1184,8 +1185,34 @@ static DEFINE_PCI_DEVICE_TABLE(csio_pci_tbl) = {
 	CSIO_DEVICE(CSIO_DEVID_B404_FCOE, 0),		/* B404 FCOE */
 	CSIO_DEVICE(CSIO_DEVID_T480CR_FCOE, 0),		/* T480 CR FCOE */
 	CSIO_DEVICE(CSIO_DEVID_T440LPCR_FCOE, 0),	/* T440 LP-CR FCOE */
-	CSIO_DEVICE(CSIO_DEVID_PE10K, 0),		/* PE10K FCOE */
-	CSIO_DEVICE(CSIO_DEVID_PE10K_PF1, 0),	/* PE10K FCOE on PF1 */
+	CSIO_DEVICE(CSIO_DEVID_AMSTERDAM_T4_FCOE, 0),   /* AMSTERDAM T4 FCOE */
+	CSIO_DEVICE(CSIO_DEVID_HUAWEI_T480_FCOE, 0),    /* HUAWEI T480 FCOE */
+	CSIO_DEVICE(CSIO_DEVID_HUAWEI_T440_FCOE, 0),    /* HUAWEI T440 FCOE */
+	CSIO_DEVICE(CSIO_DEVID_HUAWEI_STG310_FCOE, 0),  /* HUAWEI STG FCOE */
+	CSIO_DEVICE(CSIO_DEVID_ACROMAG_XMC_XAUI, 0),    /* ACROMAG XAUI FCOE */
+	CSIO_DEVICE(CSIO_DEVID_QUANTA_MEZZ_SFP_FCOE, 0),/* QUANTA MEZZ FCOE */
+	CSIO_DEVICE(CSIO_DEVID_HUAWEI_10GT_FCOE, 0),    /* HUAWEI 10GT FCOE */
+	CSIO_DEVICE(CSIO_DEVID_HUAWEI_T440_TOE_FCOE, 0),/* HUAWEI T4 TOE FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T580DBG_FCOE, 0),        /* T5 DEBUG FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T520CR_FCOE, 0),         /* T520CR FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T522CR_FCOE, 0),         /* T522CR FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T540CR_FCOE, 0),         /* T540CR FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T520BCH_FCOE, 0),        /* T520BCH FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T540BCH_FCOE, 0),        /* T540BCH FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T540CH_FCOE, 0),         /* T540CH FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T520SO_FCOE, 0),         /* T520SO FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T520CX_FCOE, 0),         /* T520CX FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T520BT_FCOE, 0),         /* T520BT FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T504BT_FCOE, 0),         /* T504BT FCOE */
+	CSIO_DEVICE(CSIO_DEVID_B520_FCOE, 0),           /* B520 FCOE */
+	CSIO_DEVICE(CSIO_DEVID_B504_FCOE, 0),           /* B504 FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T580CR2_FCOE, 0),	/* T580 CR FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T540LPCR_FCOE, 0),       /* T540 LP-CR FCOE */
+	CSIO_DEVICE(CSIO_DEVID_AMSTERDAM_T5_FCOE, 0),   /* AMSTERDAM T5 FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T580LPCR_FCOE, 0),       /* T580 LP-CR FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T520LLCR_FCOE, 0),       /* T520 LL-CR FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T560CR_FCOE, 0),         /* T560 CR FCOE */
+	CSIO_DEVICE(CSIO_DEVID_T580CR_FCOE, 0),         /* T580 CR FCOE */
 	{ 0, 0, 0, 0, 0, 0, 0 }
 };
 
@@ -1259,4 +1286,5 @@ MODULE_DESCRIPTION(CSIO_DRV_DESC);
 MODULE_LICENSE(CSIO_DRV_LICENSE);
 MODULE_DEVICE_TABLE(pci, csio_pci_tbl);
 MODULE_VERSION(CSIO_DRV_VERSION);
-MODULE_FIRMWARE(CSIO_FW_FNAME);
+MODULE_FIRMWARE(FW_FNAME_T4);
+MODULE_FIRMWARE(FW_FNAME_T5);
diff --git a/drivers/scsi/csiostor/csio_init.h b/drivers/scsi/csiostor/csio_init.h
index 0838fd7ec9c7..5cc5d317a442 100644
--- a/drivers/scsi/csiostor/csio_init.h
+++ b/drivers/scsi/csiostor/csio_init.h
@@ -52,31 +52,6 @@
 #define CSIO_DRV_DESC			"Chelsio FCoE driver"
 #define CSIO_DRV_VERSION		"1.0.0"
 
-#define CSIO_DEVICE(devid, idx)					\
-{ PCI_VENDOR_ID_CHELSIO, (devid), PCI_ANY_ID, PCI_ANY_ID, 0, 0, (idx) }
-
-#define CSIO_IS_T4_FPGA(_dev)		(((_dev) == CSIO_DEVID_PE10K) ||\
-					 ((_dev) == CSIO_DEVID_PE10K_PF1))
-
-/* FCoE device IDs */
-#define CSIO_DEVID_PE10K		0xA000
-#define CSIO_DEVID_PE10K_PF1		0xA001
-#define CSIO_DEVID_T440DBG_FCOE		0x4600
-#define CSIO_DEVID_T420CR_FCOE		0x4601
-#define CSIO_DEVID_T422CR_FCOE		0x4602
-#define CSIO_DEVID_T440CR_FCOE		0x4603
-#define CSIO_DEVID_T420BCH_FCOE		0x4604
-#define CSIO_DEVID_T440BCH_FCOE		0x4605
-#define CSIO_DEVID_T440CH_FCOE		0x4606
-#define CSIO_DEVID_T420SO_FCOE		0x4607
-#define CSIO_DEVID_T420CX_FCOE		0x4608
-#define CSIO_DEVID_T420BT_FCOE		0x4609
-#define CSIO_DEVID_T404BT_FCOE		0x460A
-#define CSIO_DEVID_B420_FCOE		0x460B
-#define CSIO_DEVID_B404_FCOE		0x460C
-#define CSIO_DEVID_T480CR_FCOE		0x460D
-#define CSIO_DEVID_T440LPCR_FCOE	0x460E
-
 extern struct fc_function_template csio_fc_transport_funcs;
 extern struct fc_function_template csio_fc_transport_vport_funcs;
 
@@ -100,6 +75,10 @@ struct csio_lnode *csio_shost_init(struct csio_hw *, struct device *, bool,
 void csio_shost_exit(struct csio_lnode *);
 void csio_lnodes_exit(struct csio_hw *, bool);
 
+/* DebugFS helper routines */
+void csio_add_debugfs_mem(struct csio_hw *, const char *,
+		unsigned int, unsigned int);
+
 static inline struct Scsi_Host *
 csio_ln_to_shost(struct csio_lnode *ln)
 {
diff --git a/drivers/scsi/csiostor/csio_lnode.h b/drivers/scsi/csiostor/csio_lnode.h
index 8d84988ab06d..0f9c04175b11 100644
--- a/drivers/scsi/csiostor/csio_lnode.h
+++ b/drivers/scsi/csiostor/csio_lnode.h
@@ -114,7 +114,7 @@ struct csio_lnode_stats {
 	uint32_t	n_rnode_match;  /* matched rnode */
 	uint32_t	n_dev_loss_tmo; /* Device loss timeout */
 	uint32_t	n_fdmi_err;	/* fdmi err */
-	uint32_t	n_evt_fw[RSCN_DEV_LOST];	/* fw events */
+	uint32_t	n_evt_fw[PROTO_ERR_IMPL_LOGO];	/* fw events */
 	enum csio_ln_ev	n_evt_sm[CSIO_LNE_MAX_EVENT];	/* State m/c events */
 	uint32_t	n_rnode_alloc;	/* rnode allocated */
 	uint32_t	n_rnode_free;	/* rnode freed */
diff --git a/drivers/scsi/csiostor/csio_rnode.c b/drivers/scsi/csiostor/csio_rnode.c
index 51c6a388de2b..e9c3b045f587 100644
--- a/drivers/scsi/csiostor/csio_rnode.c
+++ b/drivers/scsi/csiostor/csio_rnode.c
@@ -302,7 +302,7 @@ csio_confirm_rnode(struct csio_lnode *ln, uint32_t rdev_flowid,
 {
 	uint8_t rport_type;
 	struct csio_rnode *rn, *match_rn;
-	uint32_t vnp_flowid;
+	uint32_t vnp_flowid = 0;
 	__be32 *port_id;
 
 	port_id = (__be32 *)&rdevp->r_id[0];
@@ -350,6 +350,14 @@ csio_confirm_rnode(struct csio_lnode *ln, uint32_t rdev_flowid,
 			 * Else, go ahead and alloc a new rnode.
 			 */
 			if (!memcmp(csio_rn_wwpn(match_rn), rdevp->wwpn, 8)) {
+				if (rn == match_rn)
+					goto found_rnode;
+				csio_ln_dbg(ln,
+					    "nport_id:x%x and wwpn:%llx"
+					    " match for ssni:x%x\n",
+					    rn->nport_id,
+					    wwn_to_u64(rdevp->wwpn),
+					    rdev_flowid);
 				if (csio_is_rnode_ready(rn)) {
 					csio_ln_warn(ln,
 						     "rnode is already"
diff --git a/drivers/scsi/csiostor/csio_rnode.h b/drivers/scsi/csiostor/csio_rnode.h
index a3b434c801da..65940096a80d 100644
--- a/drivers/scsi/csiostor/csio_rnode.h
+++ b/drivers/scsi/csiostor/csio_rnode.h
@@ -63,7 +63,7 @@ struct csio_rnode_stats {
 	uint32_t	n_err_nomem;	/* error nomem */
 	uint32_t	n_evt_unexp;	/* unexpected event */
 	uint32_t	n_evt_drop;	/* unexpected event */
-	uint32_t	n_evt_fw[RSCN_DEV_LOST];	/* fw events */
+	uint32_t	n_evt_fw[PROTO_ERR_IMPL_LOGO];	/* fw events */
 	enum csio_rn_ev	n_evt_sm[CSIO_RNFE_MAX_EVENT];	/* State m/c events */
 	uint32_t	n_lun_rst;	/* Number of resets of
 					 * of LUNs under this
diff --git a/drivers/scsi/csiostor/csio_wr.c b/drivers/scsi/csiostor/csio_wr.c
index c32df1bdaa97..4255ce264abf 100644
--- a/drivers/scsi/csiostor/csio_wr.c
+++ b/drivers/scsi/csiostor/csio_wr.c
@@ -85,8 +85,8 @@ csio_wr_ring_fldb(struct csio_hw *hw, struct csio_q *flq)
 	 */
 	if (flq->inc_idx >= 8) {
 		csio_wr_reg32(hw, DBPRIO(1) | QID(flq->un.fl.flid) |
-			      PIDX(flq->inc_idx / 8),
-			      MYPF_REG(SGE_PF_KDOORBELL));
+				  CSIO_HW_PIDX(hw, flq->inc_idx / 8),
+				  MYPF_REG(SGE_PF_KDOORBELL));
 		flq->inc_idx &= 7;
 	}
 }
@@ -989,7 +989,8 @@ csio_wr_issue(struct csio_hw *hw, int qidx, bool prio)
 	wmb();
 	/* Ring SGE Doorbell writing q->pidx into it */
 	csio_wr_reg32(hw, DBPRIO(prio) | QID(q->un.eq.physeqid) |
-		      PIDX(q->inc_idx), MYPF_REG(SGE_PF_KDOORBELL));
+			  CSIO_HW_PIDX(hw, q->inc_idx),
+			  MYPF_REG(SGE_PF_KDOORBELL));
 	q->inc_idx = 0;
 
 	return 0;
@@ -1331,20 +1332,30 @@ csio_wr_fixup_host_params(struct csio_hw *hw)
 
 	/* FL BUFFER SIZE#0 is Page size i,e already aligned to cache line */
 	csio_wr_reg32(hw, PAGE_SIZE, SGE_FL_BUFFER_SIZE0);
-	csio_wr_reg32(hw,
-		      (csio_rd_reg32(hw, SGE_FL_BUFFER_SIZE2) +
-		      sge->csio_fl_align - 1) & ~(sge->csio_fl_align - 1),
-		      SGE_FL_BUFFER_SIZE2);
-	csio_wr_reg32(hw,
-		      (csio_rd_reg32(hw, SGE_FL_BUFFER_SIZE3) +
-		      sge->csio_fl_align - 1) & ~(sge->csio_fl_align - 1),
-		      SGE_FL_BUFFER_SIZE3);
+
+	/*
+	 * If using hard params, the following will get set correctly
+	 * in csio_wr_set_sge().
+	 */
+	if (hw->flags & CSIO_HWF_USING_SOFT_PARAMS) {
+		csio_wr_reg32(hw,
+			(csio_rd_reg32(hw, SGE_FL_BUFFER_SIZE2) +
+			sge->csio_fl_align - 1) & ~(sge->csio_fl_align - 1),
+			SGE_FL_BUFFER_SIZE2);
+		csio_wr_reg32(hw,
+			(csio_rd_reg32(hw, SGE_FL_BUFFER_SIZE3) +
+			sge->csio_fl_align - 1) & ~(sge->csio_fl_align - 1),
+			SGE_FL_BUFFER_SIZE3);
+	}
 
 	csio_wr_reg32(hw, HPZ0(PAGE_SHIFT - 12), ULP_RX_TDDP_PSZ);
 
 	/* default value of rx_dma_offset of the NIC driver */
 	csio_set_reg_field(hw, SGE_CONTROL, PKTSHIFT_MASK,
 			   PKTSHIFT(CSIO_SGE_RX_DMA_OFFSET));
+
+	csio_hw_tp_wr_bits_indirect(hw, TP_INGRESS_CONFIG,
+				    CSUM_HAS_PSEUDO_HDR, 0);
 }
 
 static void
@@ -1460,18 +1471,21 @@ csio_wr_set_sge(struct csio_hw *hw)
 	 * and generate an interrupt when this occurs so we can recover.
 	 */
 	csio_set_reg_field(hw, SGE_DBFIFO_STATUS,
-			   HP_INT_THRESH(HP_INT_THRESH_MASK) |
-			   LP_INT_THRESH(LP_INT_THRESH_MASK),
-			   HP_INT_THRESH(CSIO_SGE_DBFIFO_INT_THRESH) |
-			   LP_INT_THRESH(CSIO_SGE_DBFIFO_INT_THRESH));
+		   HP_INT_THRESH(HP_INT_THRESH_MASK) |
+		   CSIO_HW_LP_INT_THRESH(hw, CSIO_HW_M_LP_INT_THRESH(hw)),
+		   HP_INT_THRESH(CSIO_SGE_DBFIFO_INT_THRESH) |
+		   CSIO_HW_LP_INT_THRESH(hw, CSIO_SGE_DBFIFO_INT_THRESH));
+
 	csio_set_reg_field(hw, SGE_DOORBELL_CONTROL, ENABLE_DROP,
 			   ENABLE_DROP);
 
 	/* SGE_FL_BUFFER_SIZE0 is set up by csio_wr_fixup_host_params(). */
 
 	CSIO_SET_FLBUF_SIZE(hw, 1, CSIO_SGE_FLBUF_SIZE1);
-	CSIO_SET_FLBUF_SIZE(hw, 2, CSIO_SGE_FLBUF_SIZE2);
-	CSIO_SET_FLBUF_SIZE(hw, 3, CSIO_SGE_FLBUF_SIZE3);
+	csio_wr_reg32(hw, (CSIO_SGE_FLBUF_SIZE2 + sge->csio_fl_align - 1)
+		      & ~(sge->csio_fl_align - 1), SGE_FL_BUFFER_SIZE2);
+	csio_wr_reg32(hw, (CSIO_SGE_FLBUF_SIZE3 + sge->csio_fl_align - 1)
+		      & ~(sge->csio_fl_align - 1), SGE_FL_BUFFER_SIZE3);
 	CSIO_SET_FLBUF_SIZE(hw, 4, CSIO_SGE_FLBUF_SIZE4);
 	CSIO_SET_FLBUF_SIZE(hw, 5, CSIO_SGE_FLBUF_SIZE5);
 	CSIO_SET_FLBUF_SIZE(hw, 6, CSIO_SGE_FLBUF_SIZE6);
@@ -1522,22 +1536,24 @@ void
 csio_wr_sge_init(struct csio_hw *hw)
 {
 	/*
-	 * If we are master:
+	 * If we are master and chip is not initialized:
 	 *    - If we plan to use the config file, we need to fixup some
 	 *      host specific registers, and read the rest of the SGE
 	 *      configuration.
 	 *    - If we dont plan to use the config file, we need to initialize
 	 *      SGE entirely, including fixing the host specific registers.
+	 * If we are master and chip is initialized, just read and work off of
+	 *	the already initialized SGE values.
 	 * If we arent the master, we are only allowed to read and work off of
 	 *      the already initialized SGE values.
 	 *
 	 * Therefore, before calling this function, we assume that the master-
-	 * ship of the card, and whether to use config file or not, have
-	 * already been decided. In other words, CSIO_HWF_USING_SOFT_PARAMS and
-	 * CSIO_HWF_MASTER should be set/unset.
+	 * ship of the card, state and whether to use config file or not, have
+	 * already been decided.
 	 */
 	if (csio_is_hw_master(hw)) {
-		csio_wr_fixup_host_params(hw);
+		if (hw->fw_state != CSIO_DEV_STATE_INIT)
+			csio_wr_fixup_host_params(hw);
 
 		if (hw->flags & CSIO_HWF_USING_SOFT_PARAMS)
 			csio_wr_get_sge(hw);
diff --git a/drivers/scsi/dc395x.c b/drivers/scsi/dc395x.c
index fed486bfd3f4..694e13c45dfd 100644
--- a/drivers/scsi/dc395x.c
+++ b/drivers/scsi/dc395x.c
@@ -4616,26 +4616,21 @@ static void adapter_uninit(struct AdapterCtlBlk *acb)
 
 
 #undef SPRINTF
-#define SPRINTF(args...) pos += sprintf(pos, args)
+#define SPRINTF(args...) seq_printf(m,##args)
 
 #undef YESNO
 #define YESNO(YN) \
  if (YN) SPRINTF(" Yes ");\
  else SPRINTF(" No  ")
 
-static int dc395x_proc_info(struct Scsi_Host *host, char *buffer,
-		char **start, off_t offset, int length, int inout)
+static int dc395x_show_info(struct seq_file *m, struct Scsi_Host *host)
 {
 	struct AdapterCtlBlk *acb = (struct AdapterCtlBlk *)host->hostdata;
 	int spd, spd1;
-	char *pos = buffer;
 	struct DeviceCtlBlk *dcb;
 	unsigned long flags;
 	int dev;
 
-	if (inout)		/* Has data been written to the file ? */
-		return -EPERM;
-
 	SPRINTF(DC395X_BANNER " PCI SCSI Host Adapter\n");
 	SPRINTF(" Driver Version " DC395X_VERSION "\n");
 
@@ -4735,22 +4730,15 @@ static int dc395x_proc_info(struct Scsi_Host *host, char *buffer,
 		SPRINTF("END\n");
 	}
 
-	*start = buffer + offset;
 	DC395x_UNLOCK_IO(acb->scsi_host, flags);
-
-	if (pos - buffer < offset)
-		return 0;
-	else if (pos - buffer - offset < length)
-		return pos - buffer - offset;
-	else
-		return length;
+	return 0;
 }
 
 
 static struct scsi_host_template dc395x_driver_template = {
 	.module                 = THIS_MODULE,
 	.proc_name              = DC395X_NAME,
-	.proc_info              = dc395x_proc_info,
+	.show_info              = dc395x_show_info,
 	.name                   = DC395X_BANNER " " DC395X_VERSION,
 	.queuecommand           = dc395x_queue_command,
 	.bios_param             = dc395x_bios_param,
diff --git a/drivers/scsi/device_handler/scsi_dh_alua.c b/drivers/scsi/device_handler/scsi_dh_alua.c
index 6f4d8e6f32f1..68adb8955d2d 100644
--- a/drivers/scsi/device_handler/scsi_dh_alua.c
+++ b/drivers/scsi/device_handler/scsi_dh_alua.c
@@ -232,13 +232,13 @@ static void stpg_endio(struct request *req, int error)
 	struct scsi_sense_hdr sense_hdr;
 	unsigned err = SCSI_DH_OK;
 
-	if (error || host_byte(req->errors) != DID_OK ||
-			msg_byte(req->errors) != COMMAND_COMPLETE) {
+	if (host_byte(req->errors) != DID_OK ||
+	    msg_byte(req->errors) != COMMAND_COMPLETE) {
 		err = SCSI_DH_IO;
 		goto done;
 	}
 
-	if (h->senselen > 0) {
+	if (req->sense_len > 0) {
 		err = scsi_normalize_sense(h->sense, SCSI_SENSE_BUFFERSIZE,
 					   &sense_hdr);
 		if (!err) {
@@ -255,7 +255,9 @@ static void stpg_endio(struct request *req, int error)
 			    ALUA_DH_NAME, sense_hdr.sense_key,
 			    sense_hdr.asc, sense_hdr.ascq);
 		err = SCSI_DH_IO;
-	}
+	} else if (error)
+		err = SCSI_DH_IO;
+
 	if (err == SCSI_DH_OK) {
 		h->state = TPGS_STATE_OPTIMIZED;
 		sdev_printk(KERN_INFO, h->sdev,
@@ -710,6 +712,10 @@ static int alua_set_params(struct scsi_device *sdev, const char *params)
 	return result;
 }
 
+static uint optimize_stpg;
+module_param(optimize_stpg, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(optimize_stpg, "Allow use of a non-optimized path, rather than sending a STPG, when implicit TPGS is supported (0=No,1=Yes). Default is 0.");
+
 /*
  * alua_activate - activate a path
  * @sdev: device on the path to be activated
@@ -731,6 +737,9 @@ static int alua_activate(struct scsi_device *sdev,
 	if (err != SCSI_DH_OK)
 		goto out;
 
+	if (optimize_stpg)
+		h->flags |= ALUA_OPTIMIZE_STPG;
+
 	if (h->tpgs & TPGS_MODE_EXPLICIT) {
 		switch (h->state) {
 		case TPGS_STATE_NONOPTIMIZED:
diff --git a/drivers/scsi/dpt_i2o.c b/drivers/scsi/dpt_i2o.c
index b6e2700ec1c6..19e1b422260a 100644
--- a/drivers/scsi/dpt_i2o.c
+++ b/drivers/scsi/dpt_i2o.c
@@ -553,36 +553,14 @@ static const char *adpt_info(struct Scsi_Host *host)
 	return (char *) (pHba->detail);
 }
 
-static int adpt_proc_info(struct Scsi_Host *host, char *buffer, char **start, off_t offset,
-		  int length, int inout)
+static int adpt_show_info(struct seq_file *m, struct Scsi_Host *host)
 {
 	struct adpt_device* d;
 	int id;
 	int chan;
-	int len = 0;
-	int begin = 0;
-	int pos = 0;
 	adpt_hba* pHba;
 	int unit;
 
-	*start = buffer;
-	if (inout == TRUE) {
-		/*
-		 * The user has done a write and wants us to take the
-		 * data in the buffer and do something with it.
-		 * proc_scsiwrite calls us with inout = 1
-		 *
-		 * Read data from buffer (writing to us) - NOT SUPPORTED
-		 */
-		return -EINVAL;
-	}
-
-	/*
-	 * inout = 0 means the user has done a read and wants information
-	 * returned, so we write information about the cards into the buffer
-	 * proc_scsiread() calls us with inout = 0
-	 */
-
 	// Find HBA (host bus adapter) we are looking for
 	mutex_lock(&adpt_configuration_lock);
 	for (pHba = hba_chain; pHba; pHba = pHba->next) {
@@ -596,86 +574,30 @@ static int adpt_proc_info(struct Scsi_Host *host, char *buffer, char **start, of
 	}
 	host = pHba->host;
 
-	len  = sprintf(buffer    , "Adaptec I2O RAID Driver Version: %s\n\n", DPT_I2O_VERSION);
-	len += sprintf(buffer+len, "%s\n", pHba->detail);
-	len += sprintf(buffer+len, "SCSI Host=scsi%d  Control Node=/dev/%s  irq=%d\n", 
+	seq_printf(m, "Adaptec I2O RAID Driver Version: %s\n\n", DPT_I2O_VERSION);
+	seq_printf(m, "%s\n", pHba->detail);
+	seq_printf(m, "SCSI Host=scsi%d  Control Node=/dev/%s  irq=%d\n", 
 			pHba->host->host_no, pHba->name, host->irq);
-	len += sprintf(buffer+len, "\tpost fifo size  = %d\n\treply fifo size = %d\n\tsg table size   = %d\n\n",
+	seq_printf(m, "\tpost fifo size  = %d\n\treply fifo size = %d\n\tsg table size   = %d\n\n",
 			host->can_queue, (int) pHba->reply_fifo_size , host->sg_tablesize);
 
-	pos = begin + len;
-
-	/* CHECKPOINT */
-	if(pos > offset + length) {
-		goto stop_output;
-	}
-	if(pos <= offset) {
-		/*
-		 * If we haven't even written to where we last left
-		 * off (the last time we were called), reset the 
-		 * beginning pointer.
-		 */
-		len = 0;
-		begin = pos;
-	}
-	len +=  sprintf(buffer+len, "Devices:\n");
+	seq_printf(m, "Devices:\n");
 	for(chan = 0; chan < MAX_CHANNEL; chan++) {
 		for(id = 0; id < MAX_ID; id++) {
 			d = pHba->channel[chan].device[id];
-			while(d){
-				len += sprintf(buffer+len,"\t%-24.24s", d->pScsi_dev->vendor);
-				len += sprintf(buffer+len," Rev: %-8.8s\n", d->pScsi_dev->rev);
-				pos = begin + len;
-
-
-				/* CHECKPOINT */
-				if(pos > offset + length) {
-					goto stop_output;
-				}
-				if(pos <= offset) {
-					len = 0;
-					begin = pos;
-				}
+			while(d) {
+				seq_printf(m,"\t%-24.24s", d->pScsi_dev->vendor);
+				seq_printf(m," Rev: %-8.8s\n", d->pScsi_dev->rev);
 
 				unit = d->pI2o_dev->lct_data.tid;
-				len += sprintf(buffer+len, "\tTID=%d, (Channel=%d, Target=%d, Lun=%d)  (%s)\n\n",
+				seq_printf(m, "\tTID=%d, (Channel=%d, Target=%d, Lun=%d)  (%s)\n\n",
 					       unit, (int)d->scsi_channel, (int)d->scsi_id, (int)d->scsi_lun,
 					       scsi_device_online(d->pScsi_dev)? "online":"offline"); 
-				pos = begin + len;
-
-				/* CHECKPOINT */
-				if(pos > offset + length) {
-					goto stop_output;
-				}
-				if(pos <= offset) {
-					len = 0;
-					begin = pos;
-				}
-
 				d = d->next_lun;
 			}
 		}
 	}
-
-	/*
-	 * begin is where we last checked our position with regards to offset
-	 * begin is always less than offset.  len is relative to begin.  It
-	 * is the number of bytes written past begin
-	 *
-	 */
-stop_output:
-	/* stop the output and calculate the correct length */
-	*(buffer + len) = '\0';
-
-	*start = buffer + (offset - begin);	/* Start of wanted data */
-	len -= (offset - begin);
-	if(len > length) {
-		len = length;
-	} else if(len < 0){
-		len = 0;
-		**start = '\0';
-	}
-	return len;
+	return 0;
 }
 
 /*
@@ -3639,7 +3561,7 @@ static struct scsi_host_template driver_template = {
 	.module			= THIS_MODULE,
 	.name			= "dpt_i2o",
 	.proc_name		= "dpt_i2o",
-	.proc_info		= adpt_proc_info,
+	.show_info		= adpt_show_info,
 	.info			= adpt_info,
 	.queuecommand		= adpt_queue,
 	.eh_abort_handler	= adpt_abort,
diff --git a/drivers/scsi/dtc.c b/drivers/scsi/dtc.c
index 4b11bb04f5c4..d01f01604140 100644
--- a/drivers/scsi/dtc.c
+++ b/drivers/scsi/dtc.c
@@ -216,7 +216,8 @@ static int __init dtc_detect(struct scsi_host_template * tpnt)
 	int sig, count;
 
 	tpnt->proc_name = "dtc3x80";
-	tpnt->proc_info = &dtc_proc_info;
+	tpnt->show_info = dtc_show_info;
+	tpnt->write_info = dtc_write_info;
 
 	for (count = 0; current_override < NO_OVERRIDES; ++current_override) {
 		addr = 0;
diff --git a/drivers/scsi/dtc.h b/drivers/scsi/dtc.h
index cdc621204b66..92d7cfc3f4fc 100644
--- a/drivers/scsi/dtc.h
+++ b/drivers/scsi/dtc.h
@@ -88,7 +88,8 @@ static int dtc_bus_reset(Scsi_Cmnd *);
 #define NCR5380_queue_command		dtc_queue_command
 #define NCR5380_abort			dtc_abort
 #define NCR5380_bus_reset		dtc_bus_reset
-#define NCR5380_proc_info		dtc_proc_info 
+#define NCR5380_show_info		dtc_show_info 
+#define NCR5380_write_info		dtc_write_info 
 
 /* 15 12 11 10
    1001 1100 0000 0000 */
diff --git a/drivers/scsi/eata_pio.c b/drivers/scsi/eata_pio.c
index d5f8362335d3..356def44ce58 100644
--- a/drivers/scsi/eata_pio.c
+++ b/drivers/scsi/eata_pio.c
@@ -92,58 +92,22 @@ static unsigned long queue_counter;
 
 static struct scsi_host_template driver_template;
 
-/*
- * eata_proc_info
- * inout : decides on the direction of the dataflow and the meaning of the 
- *         variables
- * buffer: If inout==FALSE data is being written to it else read from it
- * *start: If inout==FALSE start of the valid data in the buffer
- * offset: If inout==FALSE offset from the beginning of the imaginary file 
- *         from which we start writing into the buffer
- * length: If inout==FALSE max number of bytes to be written into the buffer 
- *         else number of bytes in the buffer
- */
-static int eata_pio_proc_info(struct Scsi_Host *shost, char *buffer, char **start, off_t offset,
-			      int length, int rw)
+static int eata_pio_show_info(struct seq_file *m, struct Scsi_Host *shost)
 {
-	int len = 0;
-	off_t begin = 0, pos = 0;
-
-	if (rw)
-		return -ENOSYS;
-
-	len += sprintf(buffer+len, "EATA (Extended Attachment) PIO driver version: "
+	seq_printf(m, "EATA (Extended Attachment) PIO driver version: "
 		   "%d.%d%s\n",VER_MAJOR, VER_MINOR, VER_SUB);
-	len += sprintf(buffer + len, "queued commands:     %10ld\n"
+	seq_printf(m, "queued commands:     %10ld\n"
 		   "processed interrupts:%10ld\n", queue_counter, int_counter);
-	len += sprintf(buffer + len, "\nscsi%-2d: HBA %.10s\n",
+	seq_printf(m, "\nscsi%-2d: HBA %.10s\n",
 		   shost->host_no, SD(shost)->name);
-	len += sprintf(buffer + len, "Firmware revision: v%s\n",
+	seq_printf(m, "Firmware revision: v%s\n",
 		   SD(shost)->revision);
-	len += sprintf(buffer + len, "IO: PIO\n");
-	len += sprintf(buffer + len, "Base IO : %#.4x\n", (u32) shost->base);
-	len += sprintf(buffer + len, "Host Bus: %s\n",
+	seq_printf(m, "IO: PIO\n");
+	seq_printf(m, "Base IO : %#.4x\n", (u32) shost->base);
+	seq_printf(m, "Host Bus: %s\n",
 		   (SD(shost)->bustype == 'P')?"PCI ":
 		   (SD(shost)->bustype == 'E')?"EISA":"ISA ");
-    
-	pos = begin + len;
-    
-	if (pos < offset) {
-		len = 0;
-		begin = pos;
-	}
-	if (pos > offset + length)
-		goto stop_output;
-    
-stop_output:
-	DBG(DBG_PROC, printk("2pos: %ld offset: %ld len: %d\n", pos, offset, len));
-	*start = buffer + (offset - begin);   /* Start of wanted data */
-	len -= (offset - begin);            /* Start slop */
-	if (len > length)
-		len = length;               /* Ending slop */
-	DBG(DBG_PROC, printk("3pos: %ld offset: %ld len: %d\n", pos, offset, len));
-    
-	return len;
+	return 0;
 }
 
 static int eata_pio_release(struct Scsi_Host *sh)
@@ -985,7 +949,7 @@ static int eata_pio_detect(struct scsi_host_template *tpnt)
 static struct scsi_host_template driver_template = {
 	.proc_name		= "eata_pio",
 	.name              	= "EATA (Extended Attachment) PIO driver",
-	.proc_info         	= eata_pio_proc_info,
+	.show_info         	= eata_pio_show_info,
 	.detect            	= eata_pio_detect,
 	.release           	= eata_pio_release,
 	.queuecommand      	= eata_pio_queue,
diff --git a/drivers/scsi/fcoe/fcoe.c b/drivers/scsi/fcoe/fcoe.c
index 9bfdc9a3f897..292b24f9bf93 100644
--- a/drivers/scsi/fcoe/fcoe.c
+++ b/drivers/scsi/fcoe/fcoe.c
@@ -1655,7 +1655,7 @@ static int fcoe_xmit(struct fc_lport *lport, struct fc_frame *fp)
 	skb->priority = fcoe->priority;
 
 	if (fcoe->netdev->priv_flags & IFF_802_1Q_VLAN &&
-	    fcoe->realdev->features & NETIF_F_HW_VLAN_TX) {
+	    fcoe->realdev->features & NETIF_F_HW_VLAN_CTAG_TX) {
 		skb->vlan_tci = VLAN_TAG_PRESENT |
 				vlan_dev_vlan_id(fcoe->netdev);
 		skb->dev = fcoe->realdev;
diff --git a/drivers/scsi/g_NCR5380.c b/drivers/scsi/g_NCR5380.c
index 5041f925c191..5cec6c60ca22 100644
--- a/drivers/scsi/g_NCR5380.c
+++ b/drivers/scsi/g_NCR5380.c
@@ -745,42 +745,36 @@ static inline int NCR5380_pwrite(struct Scsi_Host *instance, unsigned char *src,
  
 #include "NCR5380.c"
 
-#define PRINTP(x) len += sprintf(buffer+len, x)
+#define PRINTP(x) seq_printf(m, x)
 #define ANDP ,
 
-static int sprint_opcode(char *buffer, int len, int opcode)
+static void sprint_opcode(struct seq_file *m, int opcode)
 {
-	int start = len;
 	PRINTP("0x%02x " ANDP opcode);
-	return len - start;
 }
 
-static int sprint_command(char *buffer, int len, unsigned char *command)
+static void sprint_command(struct seq_file *m, unsigned char *command)
 {
-	int i, s, start = len;
-	len += sprint_opcode(buffer, len, command[0]);
+	int i, s;
+	sprint_opcode(m, command[0]);
 	for (i = 1, s = COMMAND_SIZE(command[0]); i < s; ++i)
 		PRINTP("%02x " ANDP command[i]);
 	PRINTP("\n");
-	return len - start;
 }
 
 /**
  *	sprintf_Scsi_Cmnd	-	print a scsi command
- *	@buffer: buffr to print into
- *	@len: buffer length
+ *	@m: seq_fil to print into
  *	@cmd: SCSI command block
  *	
  *	Print out the target and command data in hex
  */
 
-static int sprint_Scsi_Cmnd(char *buffer, int len, Scsi_Cmnd * cmd)
+static void sprint_Scsi_Cmnd(struct seq_file *m, Scsi_Cmnd * cmd)
 {
-	int start = len;
 	PRINTP("host number %d destination target %d, lun %d\n" ANDP cmd->device->host->host_no ANDP cmd->device->id ANDP cmd->device->lun);
 	PRINTP("        command = ");
-	len += sprint_command(buffer, len, cmd->cmnd);
-	return len - start;
+	sprint_command(m, cmd->cmnd);
 }
 
 /**
@@ -800,9 +794,8 @@ static int sprint_Scsi_Cmnd(char *buffer, int len, Scsi_Cmnd * cmd)
  *	Locks: global cli/lock for queue walk
  */
  
-static int generic_NCR5380_proc_info(struct Scsi_Host *scsi_ptr, char *buffer, char **start, off_t offset, int length, int inout)
+static int generic_NCR5380_show_info(struct seq_file *m, struct Scsi_Host *scsi_ptr)
 {
-	int len = 0;
 	NCR5380_local_declare();
 	unsigned long flags;
 	unsigned char status;
@@ -853,16 +846,16 @@ static int generic_NCR5380_proc_info(struct Scsi_Host *scsi_ptr, char *buffer, c
 		PRINTP("  T:%d %s " ANDP dev->id ANDP scsi_device_type(dev->type));
 		for (i = 0; i < 8; i++)
 			if (dev->vendor[i] >= 0x20)
-				*(buffer + (len++)) = dev->vendor[i];
-		*(buffer + (len++)) = ' ';
+				seq_putc(m, dev->vendor[i]);
+		seq_putc(m, ' ');
 		for (i = 0; i < 16; i++)
 			if (dev->model[i] >= 0x20)
-				*(buffer + (len++)) = dev->model[i];
-		*(buffer + (len++)) = ' ';
+				seq_putc(m, dev->model[i]);
+		seq_putc(m, ' ');
 		for (i = 0; i < 4; i++)
 			if (dev->rev[i] >= 0x20)
-				*(buffer + (len++)) = dev->rev[i];
-		*(buffer + (len++)) = ' ';
+				seq_putc(m, dev->rev[i]);
+		seq_putc(m, ' ');
 
 		PRINTP("\n%10ld kb read    in %5ld secs" ANDP br / 1024 ANDP tr);
 		if (tr)
@@ -886,32 +879,28 @@ static int generic_NCR5380_proc_info(struct Scsi_Host *scsi_ptr, char *buffer, c
 	if (!hostdata->connected) {
 		PRINTP("No currently connected command\n");
 	} else {
-		len += sprint_Scsi_Cmnd(buffer, len, (Scsi_Cmnd *) hostdata->connected);
+		sprint_Scsi_Cmnd(m, (Scsi_Cmnd *) hostdata->connected);
 	}
 
 	PRINTP("issue_queue\n");
 
 	for (ptr = (Scsi_Cmnd *) hostdata->issue_queue; ptr; ptr = (Scsi_Cmnd *) ptr->host_scribble)
-		len += sprint_Scsi_Cmnd(buffer, len, ptr);
+		sprint_Scsi_Cmnd(m, ptr);
 
 	PRINTP("disconnected_queue\n");
 
 	for (ptr = (Scsi_Cmnd *) hostdata->disconnected_queue; ptr; ptr = (Scsi_Cmnd *) ptr->host_scribble)
-		len += sprint_Scsi_Cmnd(buffer, len, ptr);
+		sprint_Scsi_Cmnd(m, ptr);
 
-	*start = buffer + offset;
-	len -= offset;
-	if (len > length)
-		len = length;
 	spin_unlock_irqrestore(scsi_ptr->host_lock, flags);
-	return len;
+	return 0;
 }
 
 #undef PRINTP
 #undef ANDP
 
 static struct scsi_host_template driver_template = {
-	.proc_info      	= generic_NCR5380_proc_info,
+	.show_info      	= generic_NCR5380_show_info,
 	.name           	= "Generic NCR5380/NCR53C400 Scsi Driver",
 	.detect         	= generic_NCR5380_detect,
 	.release        	= generic_NCR5380_release_resources,
diff --git a/drivers/scsi/gdth.c b/drivers/scsi/gdth.c
index 59bceac51a4c..6d55b4e7e792 100644
--- a/drivers/scsi/gdth.c
+++ b/drivers/scsi/gdth.c
@@ -4676,7 +4676,8 @@ static struct scsi_host_template gdth_template = {
         .eh_bus_reset_handler   = gdth_eh_bus_reset,
         .slave_configure        = gdth_slave_configure,
         .bios_param             = gdth_bios_param,
-        .proc_info              = gdth_proc_info,
+        .show_info              = gdth_show_info,
+        .write_info             = gdth_set_info,
 	.eh_timed_out		= gdth_timed_out,
         .proc_name              = "gdth",
         .can_queue              = GDTH_MAXCMDS,
diff --git a/drivers/scsi/gdth.h b/drivers/scsi/gdth.h
index fbf6f0f4b0dd..3fd8b83ffbf9 100644
--- a/drivers/scsi/gdth.h
+++ b/drivers/scsi/gdth.h
@@ -1007,6 +1007,7 @@ typedef struct {
 
 /* function prototyping */
 
-int gdth_proc_info(struct Scsi_Host *, char *,char **,off_t,int,int);
+int gdth_show_info(struct seq_file *, struct Scsi_Host *);
+int gdth_set_info(struct Scsi_Host *, char *, int);
 
 #endif
diff --git a/drivers/scsi/gdth_proc.c b/drivers/scsi/gdth_proc.c
index 652754319a4b..9fb632684863 100644
--- a/drivers/scsi/gdth_proc.c
+++ b/drivers/scsi/gdth_proc.c
@@ -5,23 +5,9 @@
 #include <linux/completion.h>
 #include <linux/slab.h>
 
-int gdth_proc_info(struct Scsi_Host *host, char *buffer,char **start,off_t offset,int length,   
-                   int inout)
+int gdth_set_info(struct Scsi_Host *host, char *buffer, int length)
 {
     gdth_ha_str *ha = shost_priv(host);
-
-    TRACE2(("gdth_proc_info() length %d offs %d inout %d\n",
-            length,(int)offset,inout));
-
-    if (inout)
-        return(gdth_set_info(buffer,length,host,ha));
-    else
-        return(gdth_get_info(buffer,start,offset,length,host,ha));
-}
-
-static int gdth_set_info(char *buffer,int length,struct Scsi_Host *host,
-                         gdth_ha_str *ha)
-{
     int ret_val = -EINVAL;
 
     TRACE2(("gdth_set_info() ha %d\n",ha->hanum,));
@@ -149,12 +135,10 @@ static int gdth_set_asc_info(struct Scsi_Host *host, char *buffer,
     return(-EINVAL);
 }
 
-static int gdth_get_info(char *buffer,char **start,off_t offset,int length,
-                         struct Scsi_Host *host, gdth_ha_str *ha)
+int gdth_show_info(struct seq_file *m, struct Scsi_Host *host)
 {
-    int size = 0,len = 0;
+    gdth_ha_str *ha = shost_priv(host);
     int hlen;
-    off_t begin = 0,pos = 0;
     int id, i, j, k, sec, flag;
     int no_mdrv = 0, drv_no, is_mirr;
     u32 cnt;
@@ -189,8 +173,7 @@ static int gdth_get_info(char *buffer,char **start,off_t offset,int length,
     /* request is i.e. "cat /proc/scsi/gdth/0" */ 
     /* format: %-15s\t%-10s\t%-15s\t%s */
     /* driver parameters */
-    size = sprintf(buffer+len,"Driver Parameters:\n");
-    len += size;  pos = begin + len;
+    seq_printf(m, "Driver Parameters:\n");
     if (reserve_list[0] == 0xff)
         strcpy(hrec, "--");
     else {
@@ -201,69 +184,50 @@ static int gdth_get_info(char *buffer,char **start,off_t offset,int length,
             hlen += snprintf(hrec + hlen , 161 - hlen, ",%d", reserve_list[i]);
         }
     }
-    size = sprintf(buffer+len,
+    seq_printf(m,
                    " reserve_mode: \t%d         \treserve_list:  \t%s\n",
                    reserve_mode, hrec);
-    len += size;  pos = begin + len;
-    size = sprintf(buffer+len,
+    seq_printf(m,
                    " max_ids:      \t%-3d       \thdr_channel:   \t%d\n",
                    max_ids, hdr_channel);
-    len += size;  pos = begin + len;
 
     /* controller information */
-    size = sprintf(buffer+len,"\nDisk Array Controller Information:\n");
-    len += size;  pos = begin + len;
-    strcpy(hrec, ha->binfo.type_string);
-    size = sprintf(buffer+len,
+    seq_printf(m,"\nDisk Array Controller Information:\n");
+    seq_printf(m,
                    " Number:       \t%d         \tName:          \t%s\n",
-                   ha->hanum, hrec);
-    len += size;  pos = begin + len;
+                   ha->hanum, ha->binfo.type_string);
 
+    seq_printf(m,
+                   " Driver Ver.:  \t%-10s\tFirmware Ver.: \t",
+                   GDTH_VERSION_STR);
     if (ha->more_proc)
-        sprintf(hrec, "%d.%02d.%02d-%c%03X", 
+        seq_printf(m, "%d.%02d.%02d-%c%03X\n", 
                 (u8)(ha->binfo.upd_fw_ver>>24),
                 (u8)(ha->binfo.upd_fw_ver>>16),
                 (u8)(ha->binfo.upd_fw_ver),
                 ha->bfeat.raid ? 'R':'N',
                 ha->binfo.upd_revision);
     else
-        sprintf(hrec, "%d.%02d", (u8)(ha->cpar.version>>8),
+        seq_printf(m, "%d.%02d\n", (u8)(ha->cpar.version>>8),
                 (u8)(ha->cpar.version));
-
-    size = sprintf(buffer+len,
-                   " Driver Ver.:  \t%-10s\tFirmware Ver.: \t%s\n",
-                   GDTH_VERSION_STR, hrec);
-    len += size;  pos = begin + len;
  
-    if (ha->more_proc) {
+    if (ha->more_proc)
         /* more information: 1. about controller */
-        size = sprintf(buffer+len,
+        seq_printf(m,
                        " Serial No.:   \t0x%8X\tCache RAM size:\t%d KB\n",
                        ha->binfo.ser_no, ha->binfo.memsize / 1024);
-        len += size;  pos = begin + len;
-    }
 
 #ifdef GDTH_DMA_STATISTICS
     /* controller statistics */
-    size = sprintf(buffer+len,"\nController Statistics:\n");
-    len += size;  pos = begin + len;
-    size = sprintf(buffer+len,
+    seq_printf(m,"\nController Statistics:\n");
+    seq_printf(m,
                    " 32-bit DMA buffer:\t%lu\t64-bit DMA buffer:\t%lu\n",
                    ha->dma32_cnt, ha->dma64_cnt);
-    len += size;  pos = begin + len;
 #endif
 
-    if (pos < offset) {
-        len = 0;
-        begin = pos;
-    }
-    if (pos > offset + length)
-        goto stop_output;
-
     if (ha->more_proc) {
         /* more information: 2. about physical devices */
-        size = sprintf(buffer+len,"\nPhysical Devices:");
-        len += size;  pos = begin + len;
+        seq_printf(m, "\nPhysical Devices:");
         flag = FALSE;
             
         buf = gdth_ioctl_alloc(ha, GDTH_SCRATCH, FALSE, &paddr);
@@ -309,21 +273,19 @@ static int gdth_get_info(char *buffer,char **start,off_t offset,int length,
                     strncpy(hrec+8,pdi->product,16);
                     strncpy(hrec+24,pdi->revision,4);
                     hrec[28] = 0;
-                    size = sprintf(buffer+len,
+                    seq_printf(m,
                                    "\n Chn/ID/LUN:   \t%c/%02d/%d    \tName:          \t%s\n",
                                    'A'+i,pdi->target_id,pdi->lun,hrec);
-                    len += size;  pos = begin + len;
                     flag = TRUE;
                     pdi->no_ldrive &= 0xffff;
                     if (pdi->no_ldrive == 0xffff)
                         strcpy(hrec,"--");
                     else
                         sprintf(hrec,"%d",pdi->no_ldrive);
-                    size = sprintf(buffer+len,
+                    seq_printf(m,
                                    " Capacity [MB]:\t%-6d    \tTo Log. Drive: \t%s\n",
                                    pdi->blkcnt/(1024*1024/pdi->blksize),
                                    hrec);
-                    len += size;  pos = begin + len;
                 } else {
                     pdi->devtype = 0xff;
                 }
@@ -333,11 +295,10 @@ static int gdth_get_info(char *buffer,char **start,off_t offset,int length,
                     for (k = 0; k < pds->count; ++k) {
                         if (pds->list[k].tid == pdi->target_id &&
                             pds->list[k].lun == pdi->lun) {
-                            size = sprintf(buffer+len,
+                            seq_printf(m,
                                            " Retries:      \t%-6d    \tReassigns:     \t%d\n",
                                            pds->list[k].retries,
                                            pds->list[k].reassigns);
-                            len += size;  pos = begin + len;
                             break;
                         }
                     }
@@ -355,32 +316,20 @@ static int gdth_get_info(char *buffer,char **start,off_t offset,int length,
                     pdef->sddc_type = 0x08;
 
                     if (gdth_execute(host, gdtcmd, cmnd, 30, NULL) == S_OK) {
-                        size = sprintf(buffer+len,
+                        seq_printf(m,
                                        " Grown Defects:\t%d\n",
                                        pdef->sddc_cnt);
-                        len += size;  pos = begin + len;
                     }
                 }
-                if (pos < offset) {
-                    len = 0;
-                    begin = pos;
-                }
-		if (pos > offset + length) {
-		    gdth_ioctl_free(ha, GDTH_SCRATCH, buf, paddr);
-                    goto stop_output;
-		}
             }
         }
         gdth_ioctl_free(ha, GDTH_SCRATCH, buf, paddr);
 
-        if (!flag) {
-            size = sprintf(buffer+len, "\n --\n");
-            len += size;  pos = begin + len;
-        }
+        if (!flag)
+            seq_printf(m, "\n --\n");
 
         /* 3. about logical drives */
-        size = sprintf(buffer+len,"\nLogical Drives:");
-        len += size;  pos = begin + len;
+        seq_printf(m,"\nLogical Drives:");
         flag = FALSE;
 
         buf = gdth_ioctl_alloc(ha, GDTH_SCRATCH, FALSE, &paddr);
@@ -418,10 +367,9 @@ static int gdth_get_info(char *buffer,char **start,off_t offset,int length,
                 }
                     
                 if (drv_no == i) {
-                    size = sprintf(buffer+len,
+                    seq_printf(m,
                                    "\n Number:       \t%-2d        \tStatus:        \t%s\n",
                                    drv_no, hrec);
-                    len += size;  pos = begin + len;
                     flag = TRUE;
                     no_mdrv = pcdi->cd_ldcnt;
                     if (no_mdrv > 1 || pcdi->ld_slave != -1) {
@@ -436,61 +384,37 @@ static int gdth_get_info(char *buffer,char **start,off_t offset,int length,
                     } else {
                         strcpy(hrec, "???");
                     }
-                    size = sprintf(buffer+len,
+                    seq_printf(m,
                                    " Capacity [MB]:\t%-6d    \tType:          \t%s\n",
                                    pcdi->ld_blkcnt/(1024*1024/pcdi->ld_blksize),
                                    hrec);
-                    len += size;  pos = begin + len;
                 } else {
-                    size = sprintf(buffer+len,
+                    seq_printf(m,
                                    " Slave Number: \t%-2d        \tStatus:        \t%s\n",
                                    drv_no & 0x7fff, hrec);
-                    len += size;  pos = begin + len;
                 }
                 drv_no = pcdi->ld_slave;
-                if (pos < offset) {
-                    len = 0;
-                    begin = pos;
-                }
-		if (pos > offset + length) {
-		    gdth_ioctl_free(ha, GDTH_SCRATCH, buf, paddr);
-                    goto stop_output;
-		}
             } while (drv_no != -1);
              
-            if (is_mirr) {
-                size = sprintf(buffer+len,
+            if (is_mirr)
+                seq_printf(m,
                                " Missing Drv.: \t%-2d        \tInvalid Drv.:  \t%d\n",
                                no_mdrv - j - k, k);
-                len += size;  pos = begin + len;
-            }
-              
+
             if (!ha->hdr[i].is_arraydrv)
                 strcpy(hrec, "--");
             else
                 sprintf(hrec, "%d", ha->hdr[i].master_no);
-            size = sprintf(buffer+len,
+            seq_printf(m,
                            " To Array Drv.:\t%s\n", hrec);
-            len += size;  pos = begin + len;
-            if (pos < offset) {
-                len = 0;
-                begin = pos;
-            }
-	    if (pos > offset + length) {
-		gdth_ioctl_free(ha, GDTH_SCRATCH, buf, paddr);
-                goto stop_output;
-	    }
         }       
         gdth_ioctl_free(ha, GDTH_SCRATCH, buf, paddr);
         
-        if (!flag) {
-            size = sprintf(buffer+len, "\n --\n");
-            len += size;  pos = begin + len;
-        }   
+        if (!flag)
+            seq_printf(m, "\n --\n");
 
         /* 4. about array drives */
-        size = sprintf(buffer+len,"\nArray Drives:");
-        len += size;  pos = begin + len;
+        seq_printf(m,"\nArray Drives:");
         flag = FALSE;
 
         buf = gdth_ioctl_alloc(ha, GDTH_SCRATCH, FALSE, &paddr);
@@ -525,10 +449,9 @@ static int gdth_get_info(char *buffer,char **start,off_t offset,int length,
                     strcat(hrec, "/expand");
                 else if (pai->ai_ext_state & 0x1)
                     strcat(hrec, "/patch");
-                size = sprintf(buffer+len,
+                seq_printf(m,
                                "\n Number:       \t%-2d        \tStatus:        \t%s\n",
                                i,hrec);
-                len += size;  pos = begin + len;
                 flag = TRUE;
 
                 if (pai->ai_type == 0)
@@ -539,31 +462,19 @@ static int gdth_get_info(char *buffer,char **start,off_t offset,int length,
                     strcpy(hrec, "RAID-5");
                 else 
                     strcpy(hrec, "RAID-10");
-                size = sprintf(buffer+len,
+                seq_printf(m,
                                " Capacity [MB]:\t%-6d    \tType:          \t%s\n",
                                pai->ai_size/(1024*1024/pai->ai_secsize),
                                hrec);
-                len += size;  pos = begin + len;
-                if (pos < offset) {
-                    len = 0;
-                    begin = pos;
-                }
-		if (pos > offset + length) {
-		    gdth_ioctl_free(ha, GDTH_SCRATCH, buf, paddr);
-                    goto stop_output;
-		}
             }
         }
         gdth_ioctl_free(ha, GDTH_SCRATCH, buf, paddr);
         
-        if (!flag) {
-            size = sprintf(buffer+len, "\n --\n");
-            len += size;  pos = begin + len;
-        }
+        if (!flag)
+            seq_printf(m, "\n --\n");
 
         /* 5. about host drives */
-        size = sprintf(buffer+len,"\nHost Drives:");
-        len += size;  pos = begin + len;
+        seq_printf(m,"\nHost Drives:");
         flag = FALSE;
 
         buf = gdth_ioctl_alloc(ha, sizeof(gdth_hget_str), FALSE, &paddr);
@@ -605,33 +516,22 @@ static int gdth_get_info(char *buffer,char **start,off_t offset,int length,
             if (!(ha->hdr[i].present))
                 continue;
               
-            size = sprintf(buffer+len,
+            seq_printf(m,
                            "\n Number:       \t%-2d        \tArr/Log. Drive:\t%d\n",
                            i, ha->hdr[i].ldr_no);
-            len += size;  pos = begin + len;
             flag = TRUE;
 
-            size = sprintf(buffer+len,
+            seq_printf(m,
                            " Capacity [MB]:\t%-6d    \tStart Sector:  \t%d\n",
                            (u32)(ha->hdr[i].size/2048), ha->hdr[i].start_sec);
-            len += size;  pos = begin + len;
-            if (pos < offset) {
-                len = 0;
-                begin = pos;
-            }
-            if (pos > offset + length)
-                goto stop_output;
         }
         
-        if (!flag) {
-            size = sprintf(buffer+len, "\n --\n");
-            len += size;  pos = begin + len;
-        }
+        if (!flag)
+            seq_printf(m, "\n --\n");
     }
 
     /* controller events */
-    size = sprintf(buffer+len,"\nController Events:\n");
-    len += size;  pos = begin + len;
+    seq_printf(m,"\nController Events:\n");
 
     for (id = -1;;) {
         id = gdth_read_event(ha, id, estr);
@@ -643,29 +543,14 @@ static int gdth_get_info(char *buffer,char **start,off_t offset,int length,
             do_gettimeofday(&tv);
             sec = (int)(tv.tv_sec - estr->first_stamp);
             if (sec < 0) sec = 0;
-            size = sprintf(buffer+len," date- %02d:%02d:%02d\t%s\n",
+            seq_printf(m," date- %02d:%02d:%02d\t%s\n",
                            sec/3600, sec%3600/60, sec%60, hrec);
-            len += size;  pos = begin + len;
-            if (pos < offset) {
-                len = 0;
-                begin = pos;
-            }
-            if (pos > offset + length)
-                goto stop_output;
         }
         if (id == -1)
             break;
     }
-
 stop_output:
-    *start = buffer +(offset-begin);
-    len -= (offset-begin);
-    if (len > length)
-        len = length;
-    TRACE2(("get_info() len %d pos %d begin %d offset %d length %d size %d\n",
-            len,(int)pos,(int)begin,(int)offset,length,size));
-    rc = len;
-
+    rc = 0;
 free_fail:
     kfree(gdtcmd);
     kfree(estr);
diff --git a/drivers/scsi/gdth_proc.h b/drivers/scsi/gdth_proc.h
index dab15f59f2cc..aaa618198972 100644
--- a/drivers/scsi/gdth_proc.h
+++ b/drivers/scsi/gdth_proc.h
@@ -8,11 +8,6 @@
 int gdth_execute(struct Scsi_Host *shost, gdth_cmd_str *gdtcmd, char *cmnd,
                  int timeout, u32 *info);
 
-static int gdth_set_info(char *buffer,int length,struct Scsi_Host *host,
-                         gdth_ha_str *ha);
-static int gdth_get_info(char *buffer,char **start,off_t offset,int length,
-                         struct Scsi_Host *host, gdth_ha_str *ha);
-
 static int gdth_set_asc_info(struct Scsi_Host *host, char *buffer,
                              int length, gdth_ha_str *ha);
 
diff --git a/drivers/scsi/gvp11.c b/drivers/scsi/gvp11.c
index dbe4cc6b9f8b..2203ac281103 100644
--- a/drivers/scsi/gvp11.c
+++ b/drivers/scsi/gvp11.c
@@ -191,7 +191,8 @@ static int gvp11_bus_reset(struct scsi_cmnd *cmd)
 static struct scsi_host_template gvp11_scsi_template = {
 	.module			= THIS_MODULE,
 	.name			= "GVP Series II SCSI",
-	.proc_info		= wd33c93_proc_info,
+	.show_info		= wd33c93_show_info,
+	.write_info		= wd33c93_write_info,
 	.proc_name		= "GVP11",
 	.queuecommand		= wd33c93_queuecommand,
 	.eh_abort_handler	= wd33c93_abort,
diff --git a/drivers/scsi/imm.c b/drivers/scsi/imm.c
index 26cd9d1d7571..89a8266560d0 100644
--- a/drivers/scsi/imm.c
+++ b/drivers/scsi/imm.c
@@ -121,45 +121,26 @@ static inline void imm_pb_release(imm_struct *dev)
  * testing...
  * Also gives a method to use a script to obtain optimum timings (TODO)
  */
-static inline int imm_proc_write(imm_struct *dev, char *buffer, int length)
+static int imm_write_info(struct Scsi_Host *host, char *buffer, int length)
 {
-	unsigned long x;
+	imm_struct *dev = imm_dev(host);
 
 	if ((length > 5) && (strncmp(buffer, "mode=", 5) == 0)) {
-		x = simple_strtoul(buffer + 5, NULL, 0);
-		dev->mode = x;
+		dev->mode = simple_strtoul(buffer + 5, NULL, 0);
 		return length;
 	}
 	printk("imm /proc: invalid variable\n");
-	return (-EINVAL);
+	return -EINVAL;
 }
 
-static int imm_proc_info(struct Scsi_Host *host, char *buffer, char **start,
-			off_t offset, int length, int inout)
+static int imm_show_info(struct seq_file *m, struct Scsi_Host *host)
 {
 	imm_struct *dev = imm_dev(host);
-	int len = 0;
-
-	if (inout)
-		return imm_proc_write(dev, buffer, length);
-
-	len += sprintf(buffer + len, "Version : %s\n", IMM_VERSION);
-	len +=
-	    sprintf(buffer + len, "Parport : %s\n",
-		    dev->dev->port->name);
-	len +=
-	    sprintf(buffer + len, "Mode    : %s\n",
-		    IMM_MODE_STRING[dev->mode]);
 
-	/* Request for beyond end of buffer */
-	if (offset > len)
-		return 0;
-
-	*start = buffer + offset;
-	len -= offset;
-	if (len > length)
-		len = length;
-	return len;
+	seq_printf(m, "Version : %s\n", IMM_VERSION);
+	seq_printf(m, "Parport : %s\n", dev->dev->port->name);
+	seq_printf(m, "Mode    : %s\n", IMM_MODE_STRING[dev->mode]);
+	return 0;
 }
 
 #if IMM_DEBUG > 0
@@ -1118,7 +1099,8 @@ static int imm_adjust_queue(struct scsi_device *device)
 static struct scsi_host_template imm_template = {
 	.module			= THIS_MODULE,
 	.proc_name		= "imm",
-	.proc_info		= imm_proc_info,
+	.show_info		= imm_show_info,
+	.write_info		= imm_write_info,
 	.name			= "Iomega VPI2 (imm) interface",
 	.queuecommand		= imm_queuecommand,
 	.eh_abort_handler	= imm_abort,
diff --git a/drivers/scsi/in2000.c b/drivers/scsi/in2000.c
index deb5b6d8398e..bf028218ac36 100644
--- a/drivers/scsi/in2000.c
+++ b/drivers/scsi/in2000.c
@@ -2166,152 +2166,117 @@ static int in2000_biosparam(struct scsi_device *sdev, struct block_device *bdev,
 }
 
 
-static int in2000_proc_info(struct Scsi_Host *instance, char *buf, char **start, off_t off, int len, int in)
+static int in2000_write_info(struct Scsi_Host *instance, char *buf, int len)
 {
 
 #ifdef PROC_INTERFACE
 
 	char *bp;
-	char tbuf[128];
-	unsigned long flags;
 	struct IN2000_hostdata *hd;
-	Scsi_Cmnd *cmd;
 	int x, i;
-	static int stop = 0;
 
 	hd = (struct IN2000_hostdata *) instance->hostdata;
 
-/* If 'in' is TRUE we need to _read_ the proc file. We accept the following
- * keywords (same format as command-line, but only ONE per read):
- *    debug
- *    disconnect
- *    period
- *    resync
- *    proc
- */
-
-	if (in) {
-		buf[len] = '\0';
-		bp = buf;
-		if (!strncmp(bp, "debug:", 6)) {
-			bp += 6;
-			hd->args = simple_strtoul(bp, NULL, 0) & DB_MASK;
-		} else if (!strncmp(bp, "disconnect:", 11)) {
-			bp += 11;
-			x = simple_strtoul(bp, NULL, 0);
-			if (x < DIS_NEVER || x > DIS_ALWAYS)
-				x = DIS_ADAPTIVE;
-			hd->disconnect = x;
-		} else if (!strncmp(bp, "period:", 7)) {
-			bp += 7;
-			x = simple_strtoul(bp, NULL, 0);
-			hd->default_sx_per = sx_table[round_period((unsigned int) x)].period_ns;
-		} else if (!strncmp(bp, "resync:", 7)) {
-			bp += 7;
-			x = simple_strtoul(bp, NULL, 0);
-			for (i = 0; i < 7; i++)
-				if (x & (1 << i))
-					hd->sync_stat[i] = SS_UNSET;
-		} else if (!strncmp(bp, "proc:", 5)) {
-			bp += 5;
-			hd->proc = simple_strtoul(bp, NULL, 0);
-		} else if (!strncmp(bp, "level2:", 7)) {
-			bp += 7;
-			hd->level2 = simple_strtoul(bp, NULL, 0);
-		}
-		return len;
+	buf[len] = '\0';
+	bp = buf;
+	if (!strncmp(bp, "debug:", 6)) {
+		bp += 6;
+		hd->args = simple_strtoul(bp, NULL, 0) & DB_MASK;
+	} else if (!strncmp(bp, "disconnect:", 11)) {
+		bp += 11;
+		x = simple_strtoul(bp, NULL, 0);
+		if (x < DIS_NEVER || x > DIS_ALWAYS)
+			x = DIS_ADAPTIVE;
+		hd->disconnect = x;
+	} else if (!strncmp(bp, "period:", 7)) {
+		bp += 7;
+		x = simple_strtoul(bp, NULL, 0);
+		hd->default_sx_per = sx_table[round_period((unsigned int) x)].period_ns;
+	} else if (!strncmp(bp, "resync:", 7)) {
+		bp += 7;
+		x = simple_strtoul(bp, NULL, 0);
+		for (i = 0; i < 7; i++)
+			if (x & (1 << i))
+				hd->sync_stat[i] = SS_UNSET;
+	} else if (!strncmp(bp, "proc:", 5)) {
+		bp += 5;
+		hd->proc = simple_strtoul(bp, NULL, 0);
+	} else if (!strncmp(bp, "level2:", 7)) {
+		bp += 7;
+		hd->level2 = simple_strtoul(bp, NULL, 0);
 	}
+#endif
+	return len;
+}
+
+static int in2000_show_info(struct seq_file *m, struct Scsi_Host *instance)
+{
+
+#ifdef PROC_INTERFACE
+	unsigned long flags;
+	struct IN2000_hostdata *hd;
+	Scsi_Cmnd *cmd;
+	int x;
+
+	hd = (struct IN2000_hostdata *) instance->hostdata;
 
 	spin_lock_irqsave(instance->host_lock, flags);
-	bp = buf;
-	*bp = '\0';
-	if (hd->proc & PR_VERSION) {
-		sprintf(tbuf, "\nVersion %s - %s.", IN2000_VERSION, IN2000_DATE);
-		strcat(bp, tbuf);
-	}
+	if (hd->proc & PR_VERSION)
+		seq_printf(m, "\nVersion %s - %s.", IN2000_VERSION, IN2000_DATE);
+
 	if (hd->proc & PR_INFO) {
-		sprintf(tbuf, "\ndip_switch=%02x: irq=%d io=%02x floppy=%s sync/DOS5=%s", (hd->dip_switch & 0x7f), instance->irq, hd->io_base, (hd->dip_switch & 0x40) ? "Yes" : "No", (hd->dip_switch & 0x20) ? "Yes" : "No");
-		strcat(bp, tbuf);
-		strcat(bp, "\nsync_xfer[] =       ");
-		for (x = 0; x < 7; x++) {
-			sprintf(tbuf, "\t%02x", hd->sync_xfer[x]);
-			strcat(bp, tbuf);
-		}
-		strcat(bp, "\nsync_stat[] =       ");
-		for (x = 0; x < 7; x++) {
-			sprintf(tbuf, "\t%02x", hd->sync_stat[x]);
-			strcat(bp, tbuf);
-		}
+		seq_printf(m, "\ndip_switch=%02x: irq=%d io=%02x floppy=%s sync/DOS5=%s", (hd->dip_switch & 0x7f), instance->irq, hd->io_base, (hd->dip_switch & 0x40) ? "Yes" : "No", (hd->dip_switch & 0x20) ? "Yes" : "No");
+		seq_printf(m, "\nsync_xfer[] =       ");
+		for (x = 0; x < 7; x++)
+			seq_printf(m, "\t%02x", hd->sync_xfer[x]);
+		seq_printf(m, "\nsync_stat[] =       ");
+		for (x = 0; x < 7; x++)
+			seq_printf(m, "\t%02x", hd->sync_stat[x]);
 	}
 #ifdef PROC_STATISTICS
 	if (hd->proc & PR_STATISTICS) {
-		strcat(bp, "\ncommands issued:    ");
-		for (x = 0; x < 7; x++) {
-			sprintf(tbuf, "\t%ld", hd->cmd_cnt[x]);
-			strcat(bp, tbuf);
-		}
-		strcat(bp, "\ndisconnects allowed:");
-		for (x = 0; x < 7; x++) {
-			sprintf(tbuf, "\t%ld", hd->disc_allowed_cnt[x]);
-			strcat(bp, tbuf);
-		}
-		strcat(bp, "\ndisconnects done:   ");
-		for (x = 0; x < 7; x++) {
-			sprintf(tbuf, "\t%ld", hd->disc_done_cnt[x]);
-			strcat(bp, tbuf);
-		}
-		sprintf(tbuf, "\ninterrupts:      \t%ld", hd->int_cnt);
-		strcat(bp, tbuf);
+		seq_printf(m, "\ncommands issued:    ");
+		for (x = 0; x < 7; x++)
+			seq_printf(m, "\t%ld", hd->cmd_cnt[x]);
+		seq_printf(m, "\ndisconnects allowed:");
+		for (x = 0; x < 7; x++)
+			seq_printf(m, "\t%ld", hd->disc_allowed_cnt[x]);
+		seq_printf(m, "\ndisconnects done:   ");
+		for (x = 0; x < 7; x++)
+			seq_printf(m, "\t%ld", hd->disc_done_cnt[x]);
+		seq_printf(m, "\ninterrupts:      \t%ld", hd->int_cnt);
 	}
 #endif
 	if (hd->proc & PR_CONNECTED) {
-		strcat(bp, "\nconnected:     ");
+		seq_printf(m, "\nconnected:     ");
 		if (hd->connected) {
 			cmd = (Scsi_Cmnd *) hd->connected;
-			sprintf(tbuf, " %d:%d(%02x)", cmd->device->id, cmd->device->lun, cmd->cmnd[0]);
-			strcat(bp, tbuf);
+			seq_printf(m, " %d:%d(%02x)", cmd->device->id, cmd->device->lun, cmd->cmnd[0]);
 		}
 	}
 	if (hd->proc & PR_INPUTQ) {
-		strcat(bp, "\ninput_Q:       ");
+		seq_printf(m, "\ninput_Q:       ");
 		cmd = (Scsi_Cmnd *) hd->input_Q;
 		while (cmd) {
-			sprintf(tbuf, " %d:%d(%02x)", cmd->device->id, cmd->device->lun, cmd->cmnd[0]);
-			strcat(bp, tbuf);
+			seq_printf(m, " %d:%d(%02x)", cmd->device->id, cmd->device->lun, cmd->cmnd[0]);
 			cmd = (Scsi_Cmnd *) cmd->host_scribble;
 		}
 	}
 	if (hd->proc & PR_DISCQ) {
-		strcat(bp, "\ndisconnected_Q:");
+		seq_printf(m, "\ndisconnected_Q:");
 		cmd = (Scsi_Cmnd *) hd->disconnected_Q;
 		while (cmd) {
-			sprintf(tbuf, " %d:%d(%02x)", cmd->device->id, cmd->device->lun, cmd->cmnd[0]);
-			strcat(bp, tbuf);
+			seq_printf(m, " %d:%d(%02x)", cmd->device->id, cmd->device->lun, cmd->cmnd[0]);
 			cmd = (Scsi_Cmnd *) cmd->host_scribble;
 		}
 	}
 	if (hd->proc & PR_TEST) {
 		;		/* insert your own custom function here */
 	}
-	strcat(bp, "\n");
+	seq_printf(m, "\n");
 	spin_unlock_irqrestore(instance->host_lock, flags);
-	*start = buf;
-	if (stop) {
-		stop = 0;
-		return 0;	/* return 0 to signal end-of-file */
-	}
-	if (off > 0x40000)	/* ALWAYS stop after 256k bytes have been read */
-		stop = 1;
-	if (hd->proc & PR_STOP)	/* stop every other time */
-		stop = 1;
-	return strlen(bp);
-
-#else				/* PROC_INTERFACE */
-
-	return 0;
-
 #endif				/* PROC_INTERFACE */
-
+	return 0;
 }
 
 MODULE_LICENSE("GPL");
@@ -2319,7 +2284,8 @@ MODULE_LICENSE("GPL");
 
 static struct scsi_host_template driver_template = {
 	.proc_name       		= "in2000",
-	.proc_info       		= in2000_proc_info,
+	.write_info       		= in2000_write_info,
+	.show_info       		= in2000_show_info,
 	.name            		= "Always IN2000",
 	.detect          		= in2000_detect, 
 	.release			= in2000_release,
diff --git a/drivers/scsi/ips.c b/drivers/scsi/ips.c
index 9aa86a315a08..8d5ea8a1e5a6 100644
--- a/drivers/scsi/ips.c
+++ b/drivers/scsi/ips.c
@@ -326,10 +326,9 @@ static void ips_scmd_buf_write(struct scsi_cmnd * scmd, void *data,
 static void ips_scmd_buf_read(struct scsi_cmnd * scmd, void *data,
 			      unsigned int count);
 
-static int ips_proc_info(struct Scsi_Host *, char *, char **, off_t, int, int);
-static int ips_host_info(ips_ha_t *, char *, off_t, int);
-static void copy_mem_info(IPS_INFOSTR *, char *, int);
-static int copy_info(IPS_INFOSTR *, char *, ...);
+static int ips_write_info(struct Scsi_Host *, char *, int);
+static int ips_show_info(struct seq_file *, struct Scsi_Host *);
+static int ips_host_info(ips_ha_t *, struct seq_file *);
 static int ips_abort_init(ips_ha_t * ha, int index);
 static int ips_init_phase2(int index);
 
@@ -367,7 +366,8 @@ static struct scsi_host_template ips_driver_template = {
 	.eh_abort_handler	= ips_eh_abort,
 	.eh_host_reset_handler	= ips_eh_reset,
 	.proc_name		= "ips",
-	.proc_info		= ips_proc_info,
+	.show_info		= ips_show_info,
+	.write_info		= ips_write_info,
 	.slave_configure	= ips_slave_configure,
 	.bios_param		= ips_biosparam,
 	.this_id		= -1,
@@ -1433,25 +1433,12 @@ ips_info(struct Scsi_Host *SH)
 	return (bp);
 }
 
-/****************************************************************************/
-/*                                                                          */
-/* Routine Name: ips_proc_info                                              */
-/*                                                                          */
-/* Routine Description:                                                     */
-/*                                                                          */
-/*   The passthru interface for the driver                                  */
-/*                                                                          */
-/****************************************************************************/
 static int
-ips_proc_info(struct Scsi_Host *host, char *buffer, char **start, off_t offset,
-	      int length, int func)
+ips_write_info(struct Scsi_Host *host, char *buffer, int length)
 {
 	int i;
-	int ret;
 	ips_ha_t *ha = NULL;
 
-	METHOD_TRACE("ips_proc_info", 1);
-
 	/* Find our host structure */
 	for (i = 0; i < ips_next_controller; i++) {
 		if (ips_sh[i]) {
@@ -1465,18 +1452,29 @@ ips_proc_info(struct Scsi_Host *host, char *buffer, char **start, off_t offset,
 	if (!ha)
 		return (-EINVAL);
 
-	if (func) {
-		/* write */
-		return (0);
-	} else {
-		/* read */
-		if (start)
-			*start = buffer;
+	return 0;
+}
 
-		ret = ips_host_info(ha, buffer, offset, length);
+static int
+ips_show_info(struct seq_file *m, struct Scsi_Host *host)
+{
+	int i;
+	ips_ha_t *ha = NULL;
 
-		return (ret);
+	/* Find our host structure */
+	for (i = 0; i < ips_next_controller; i++) {
+		if (ips_sh[i]) {
+			if (ips_sh[i] == host) {
+				ha = (ips_ha_t *) ips_sh[i]->hostdata;
+				break;
+			}
+		}
 	}
+
+	if (!ha)
+		return (-EINVAL);
+
+	return ips_host_info(ha, m);
 }
 
 /*--------------------------------------------------------------------------*/
@@ -2035,183 +2033,113 @@ ips_cleanup_passthru(ips_ha_t * ha, ips_scb_t * scb)
 /*                                                                          */
 /****************************************************************************/
 static int
-ips_host_info(ips_ha_t * ha, char *ptr, off_t offset, int len)
+ips_host_info(ips_ha_t *ha, struct seq_file *m)
 {
-	IPS_INFOSTR info;
-
 	METHOD_TRACE("ips_host_info", 1);
 
-	info.buffer = ptr;
-	info.length = len;
-	info.offset = offset;
-	info.pos = 0;
-	info.localpos = 0;
-
-	copy_info(&info, "\nIBM ServeRAID General Information:\n\n");
+	seq_printf(m, "\nIBM ServeRAID General Information:\n\n");
 
 	if ((le32_to_cpu(ha->nvram->signature) == IPS_NVRAM_P5_SIG) &&
 	    (le16_to_cpu(ha->nvram->adapter_type) != 0))
-		copy_info(&info, "\tController Type                   : %s\n",
+		seq_printf(m, "\tController Type                   : %s\n",
 			  ips_adapter_name[ha->ad_type - 1]);
 	else
-		copy_info(&info,
+		seq_printf(m,
 			  "\tController Type                   : Unknown\n");
 
 	if (ha->io_addr)
-		copy_info(&info,
-			  "\tIO region                         : 0x%lx (%d bytes)\n",
+		seq_printf(m,
+			  "\tIO region                         : 0x%x (%d bytes)\n",
 			  ha->io_addr, ha->io_len);
 
 	if (ha->mem_addr) {
-		copy_info(&info,
-			  "\tMemory region                     : 0x%lx (%d bytes)\n",
+		seq_printf(m,
+			  "\tMemory region                     : 0x%x (%d bytes)\n",
 			  ha->mem_addr, ha->mem_len);
-		copy_info(&info,
+		seq_printf(m,
 			  "\tShared memory address             : 0x%lx\n",
-			  ha->mem_ptr);
+			  (unsigned long)ha->mem_ptr);
 	}
 
-	copy_info(&info, "\tIRQ number                        : %d\n", ha->pcidev->irq);
+	seq_printf(m, "\tIRQ number                        : %d\n", ha->pcidev->irq);
 
     /* For the Next 3 lines Check for Binary 0 at the end and don't include it if it's there. */
     /* That keeps everything happy for "text" operations on the proc file.                    */
 
 	if (le32_to_cpu(ha->nvram->signature) == IPS_NVRAM_P5_SIG) {
 	if (ha->nvram->bios_low[3] == 0) {
-            copy_info(&info,
-			          "\tBIOS Version                      : %c%c%c%c%c%c%c\n",
-			          ha->nvram->bios_high[0], ha->nvram->bios_high[1],
-			          ha->nvram->bios_high[2], ha->nvram->bios_high[3],
-			          ha->nvram->bios_low[0], ha->nvram->bios_low[1],
-			          ha->nvram->bios_low[2]);
+		seq_printf(m,
+			  "\tBIOS Version                      : %c%c%c%c%c%c%c\n",
+			  ha->nvram->bios_high[0], ha->nvram->bios_high[1],
+			  ha->nvram->bios_high[2], ha->nvram->bios_high[3],
+			  ha->nvram->bios_low[0], ha->nvram->bios_low[1],
+			  ha->nvram->bios_low[2]);
 
         } else {
-		    copy_info(&info,
-			          "\tBIOS Version                      : %c%c%c%c%c%c%c%c\n",
-			          ha->nvram->bios_high[0], ha->nvram->bios_high[1],
-			          ha->nvram->bios_high[2], ha->nvram->bios_high[3],
-			          ha->nvram->bios_low[0], ha->nvram->bios_low[1],
-			          ha->nvram->bios_low[2], ha->nvram->bios_low[3]);
+		seq_printf(m,
+			  "\tBIOS Version                      : %c%c%c%c%c%c%c%c\n",
+			  ha->nvram->bios_high[0], ha->nvram->bios_high[1],
+			  ha->nvram->bios_high[2], ha->nvram->bios_high[3],
+			  ha->nvram->bios_low[0], ha->nvram->bios_low[1],
+			  ha->nvram->bios_low[2], ha->nvram->bios_low[3]);
         }
 
     }
 
     if (ha->enq->CodeBlkVersion[7] == 0) {
-        copy_info(&info,
-		          "\tFirmware Version                  : %c%c%c%c%c%c%c\n",
-		          ha->enq->CodeBlkVersion[0], ha->enq->CodeBlkVersion[1],
-		          ha->enq->CodeBlkVersion[2], ha->enq->CodeBlkVersion[3],
-		          ha->enq->CodeBlkVersion[4], ha->enq->CodeBlkVersion[5],
-		          ha->enq->CodeBlkVersion[6]);
+        seq_printf(m,
+		  "\tFirmware Version                  : %c%c%c%c%c%c%c\n",
+		  ha->enq->CodeBlkVersion[0], ha->enq->CodeBlkVersion[1],
+		  ha->enq->CodeBlkVersion[2], ha->enq->CodeBlkVersion[3],
+		  ha->enq->CodeBlkVersion[4], ha->enq->CodeBlkVersion[5],
+		  ha->enq->CodeBlkVersion[6]);
     } else {
-        copy_info(&info,
-		          "\tFirmware Version                  : %c%c%c%c%c%c%c%c\n",
-		          ha->enq->CodeBlkVersion[0], ha->enq->CodeBlkVersion[1],
-		          ha->enq->CodeBlkVersion[2], ha->enq->CodeBlkVersion[3],
-		          ha->enq->CodeBlkVersion[4], ha->enq->CodeBlkVersion[5],
-		          ha->enq->CodeBlkVersion[6], ha->enq->CodeBlkVersion[7]);
+	seq_printf(m,
+		  "\tFirmware Version                  : %c%c%c%c%c%c%c%c\n",
+		  ha->enq->CodeBlkVersion[0], ha->enq->CodeBlkVersion[1],
+		  ha->enq->CodeBlkVersion[2], ha->enq->CodeBlkVersion[3],
+		  ha->enq->CodeBlkVersion[4], ha->enq->CodeBlkVersion[5],
+		  ha->enq->CodeBlkVersion[6], ha->enq->CodeBlkVersion[7]);
     }
 
     if (ha->enq->BootBlkVersion[7] == 0) {
-        copy_info(&info,
-		          "\tBoot Block Version                : %c%c%c%c%c%c%c\n",
-		          ha->enq->BootBlkVersion[0], ha->enq->BootBlkVersion[1],
-		          ha->enq->BootBlkVersion[2], ha->enq->BootBlkVersion[3],
-		          ha->enq->BootBlkVersion[4], ha->enq->BootBlkVersion[5],
-		          ha->enq->BootBlkVersion[6]);
+        seq_printf(m,
+		  "\tBoot Block Version                : %c%c%c%c%c%c%c\n",
+		  ha->enq->BootBlkVersion[0], ha->enq->BootBlkVersion[1],
+		  ha->enq->BootBlkVersion[2], ha->enq->BootBlkVersion[3],
+		  ha->enq->BootBlkVersion[4], ha->enq->BootBlkVersion[5],
+		  ha->enq->BootBlkVersion[6]);
     } else {
-        copy_info(&info,
-		          "\tBoot Block Version                : %c%c%c%c%c%c%c%c\n",
-		          ha->enq->BootBlkVersion[0], ha->enq->BootBlkVersion[1],
-		          ha->enq->BootBlkVersion[2], ha->enq->BootBlkVersion[3],
-		          ha->enq->BootBlkVersion[4], ha->enq->BootBlkVersion[5],
-		          ha->enq->BootBlkVersion[6], ha->enq->BootBlkVersion[7]);
+        seq_printf(m,
+		  "\tBoot Block Version                : %c%c%c%c%c%c%c%c\n",
+		  ha->enq->BootBlkVersion[0], ha->enq->BootBlkVersion[1],
+		  ha->enq->BootBlkVersion[2], ha->enq->BootBlkVersion[3],
+		  ha->enq->BootBlkVersion[4], ha->enq->BootBlkVersion[5],
+		  ha->enq->BootBlkVersion[6], ha->enq->BootBlkVersion[7]);
     }
 
-	copy_info(&info, "\tDriver Version                    : %s%s\n",
+	seq_printf(m, "\tDriver Version                    : %s%s\n",
 		  IPS_VERSION_HIGH, IPS_VERSION_LOW);
 
-	copy_info(&info, "\tDriver Build                      : %d\n",
+	seq_printf(m, "\tDriver Build                      : %d\n",
 		  IPS_BUILD_IDENT);
 
-	copy_info(&info, "\tMax Physical Devices              : %d\n",
+	seq_printf(m, "\tMax Physical Devices              : %d\n",
 		  ha->enq->ucMaxPhysicalDevices);
-	copy_info(&info, "\tMax Active Commands               : %d\n",
+	seq_printf(m, "\tMax Active Commands               : %d\n",
 		  ha->max_cmds);
-	copy_info(&info, "\tCurrent Queued Commands           : %d\n",
+	seq_printf(m, "\tCurrent Queued Commands           : %d\n",
 		  ha->scb_waitlist.count);
-	copy_info(&info, "\tCurrent Active Commands           : %d\n",
+	seq_printf(m, "\tCurrent Active Commands           : %d\n",
 		  ha->scb_activelist.count - ha->num_ioctl);
-	copy_info(&info, "\tCurrent Queued PT Commands        : %d\n",
+	seq_printf(m, "\tCurrent Queued PT Commands        : %d\n",
 		  ha->copp_waitlist.count);
-	copy_info(&info, "\tCurrent Active PT Commands        : %d\n",
+	seq_printf(m, "\tCurrent Active PT Commands        : %d\n",
 		  ha->num_ioctl);
 
-	copy_info(&info, "\n");
-
-	return (info.localpos);
-}
-
-/****************************************************************************/
-/*                                                                          */
-/* Routine Name: copy_mem_info                                              */
-/*                                                                          */
-/* Routine Description:                                                     */
-/*                                                                          */
-/*   Copy data into an IPS_INFOSTR structure                                */
-/*                                                                          */
-/****************************************************************************/
-static void
-copy_mem_info(IPS_INFOSTR * info, char *data, int len)
-{
-	METHOD_TRACE("copy_mem_info", 1);
-
-	if (info->pos + len < info->offset) {
-		info->pos += len;
-		return;
-	}
-
-	if (info->pos < info->offset) {
-		data += (info->offset - info->pos);
-		len -= (info->offset - info->pos);
-		info->pos += (info->offset - info->pos);
-	}
-
-	if (info->localpos + len > info->length)
-		len = info->length - info->localpos;
+	seq_printf(m, "\n");
 
-	if (len > 0) {
-		memcpy(info->buffer + info->localpos, data, len);
-		info->pos += len;
-		info->localpos += len;
-	}
-}
-
-/****************************************************************************/
-/*                                                                          */
-/* Routine Name: copy_info                                                  */
-/*                                                                          */
-/* Routine Description:                                                     */
-/*                                                                          */
-/*   printf style wrapper for an info structure                             */
-/*                                                                          */
-/****************************************************************************/
-static int
-copy_info(IPS_INFOSTR * info, char *fmt, ...)
-{
-	va_list args;
-	char buf[128];
-	int len;
-
-	METHOD_TRACE("copy_info", 1);
-
-	va_start(args, fmt);
-	len = vsprintf(buf, fmt, args);
-	va_end(args);
-
-	copy_mem_info(info, buf, len);
-
-	return (len);
+	return 0;
 }
 
 /****************************************************************************/
diff --git a/drivers/scsi/ips.h b/drivers/scsi/ips.h
index f2df0593332b..45b9566b928e 100644
--- a/drivers/scsi/ips.h
+++ b/drivers/scsi/ips.h
@@ -416,7 +416,6 @@
    /*
     * Scsi_Host Template
     */
-   static int ips_proc_info(struct Scsi_Host *, char *, char **, off_t, int, int);
    static int ips_biosparam(struct scsi_device *sdev, struct block_device *bdev,
 		sector_t capacity, int geom[]);
    static int ips_slave_configure(struct scsi_device *SDptr);
@@ -959,14 +958,6 @@ typedef union {
    IPS_ENH_SG_LIST  *enh_list;
 } IPS_SG_LIST;
 
-typedef struct _IPS_INFOSTR {
-   char *buffer;
-   int   length;
-   int   offset;
-   int   pos;
-   int   localpos;
-} IPS_INFOSTR;
-
 typedef struct {
    char *option_name;
    int  *option_flag;
diff --git a/drivers/scsi/isci/init.c b/drivers/scsi/isci/init.c
index 2839baa82a5a..d25d0d859f05 100644
--- a/drivers/scsi/isci/init.c
+++ b/drivers/scsi/isci/init.c
@@ -721,7 +721,7 @@ static void isci_pci_remove(struct pci_dev *pdev)
 	}
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int isci_suspend(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
@@ -770,18 +770,16 @@ static int isci_resume(struct device *dev)
 
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(isci_pm_ops, isci_suspend, isci_resume);
-#endif
 
 static struct pci_driver isci_pci_driver = {
 	.name		= DRV_NAME,
 	.id_table	= isci_id_table,
 	.probe		= isci_pci_probe,
 	.remove		= isci_pci_remove,
-#ifdef CONFIG_PM
 	.driver.pm      = &isci_pm_ops,
-#endif
 };
 
 static __init int isci_init(void)
diff --git a/drivers/scsi/iscsi_tcp.c b/drivers/scsi/iscsi_tcp.c
index 1b91ca0dc1e3..9e2588a6881c 100644
--- a/drivers/scsi/iscsi_tcp.c
+++ b/drivers/scsi/iscsi_tcp.c
@@ -370,17 +370,24 @@ static inline int iscsi_sw_tcp_xmit_qlen(struct iscsi_conn *conn)
 static int iscsi_sw_tcp_pdu_xmit(struct iscsi_task *task)
 {
 	struct iscsi_conn *conn = task->conn;
-	int rc;
+	unsigned long pflags = current->flags;
+	int rc = 0;
+
+	current->flags |= PF_MEMALLOC;
 
 	while (iscsi_sw_tcp_xmit_qlen(conn)) {
 		rc = iscsi_sw_tcp_xmit(conn);
-		if (rc == 0)
-			return -EAGAIN;
+		if (rc == 0) {
+			rc = -EAGAIN;
+			break;
+		}
 		if (rc < 0)
-			return rc;
+			break;
+		rc = 0;
 	}
 
-	return 0;
+	tsk_restore_flags(current, pflags, PF_MEMALLOC);
+	return rc;
 }
 
 /*
@@ -665,6 +672,7 @@ iscsi_sw_tcp_conn_bind(struct iscsi_cls_session *cls_session,
 	sk->sk_reuse = SK_CAN_REUSE;
 	sk->sk_sndtimeo = 15 * HZ; /* FIXME: make it configurable */
 	sk->sk_allocation = GFP_ATOMIC;
+	sk_set_memalloc(sk);
 
 	iscsi_sw_tcp_conn_set_callbacks(conn);
 	tcp_sw_conn->sendpage = tcp_sw_conn->sock->ops->sendpage;
diff --git a/drivers/scsi/libiscsi.c b/drivers/scsi/libiscsi.c
index 82c3fd4bc938..5de946984500 100644
--- a/drivers/scsi/libiscsi.c
+++ b/drivers/scsi/libiscsi.c
@@ -507,7 +507,6 @@ static void iscsi_free_task(struct iscsi_task *task)
 	kfifo_in(&session->cmdpool.queue, (void*)&task, sizeof(void*));
 
 	if (sc) {
-		task->sc = NULL;
 		/* SCSI eh reuses commands to verify us */
 		sc->SCp.ptr = NULL;
 		/*
@@ -3142,7 +3141,7 @@ int iscsi_conn_bind(struct iscsi_cls_session *cls_session,
 }
 EXPORT_SYMBOL_GPL(iscsi_conn_bind);
 
-static int iscsi_switch_str_param(char **param, char *new_val_buf)
+int iscsi_switch_str_param(char **param, char *new_val_buf)
 {
 	char *new_val;
 
@@ -3159,6 +3158,7 @@ static int iscsi_switch_str_param(char **param, char *new_val_buf)
 	*param = new_val;
 	return 0;
 }
+EXPORT_SYMBOL_GPL(iscsi_switch_str_param);
 
 int iscsi_set_param(struct iscsi_cls_conn *cls_conn,
 		    enum iscsi_param param, char *buf, int buflen)
diff --git a/drivers/scsi/libsas/sas_expander.c b/drivers/scsi/libsas/sas_expander.c
index 55cbd0180159..f42b0e15410f 100644
--- a/drivers/scsi/libsas/sas_expander.c
+++ b/drivers/scsi/libsas/sas_expander.c
@@ -2163,10 +2163,10 @@ int sas_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	}
 
 	/* do we need to support multiple segments? */
-	if (req->bio->bi_vcnt > 1 || rsp->bio->bi_vcnt > 1) {
+	if (bio_segments(req->bio) > 1 || bio_segments(rsp->bio) > 1) {
 		printk("%s: multiple segments req %u %u, rsp %u %u\n",
-		       __func__, req->bio->bi_vcnt, blk_rq_bytes(req),
-		       rsp->bio->bi_vcnt, blk_rq_bytes(rsp));
+		       __func__, bio_segments(req->bio), blk_rq_bytes(req),
+		       bio_segments(rsp->bio), blk_rq_bytes(rsp));
 		return -EINVAL;
 	}
 
diff --git a/drivers/scsi/lpfc/lpfc_attr.c b/drivers/scsi/lpfc/lpfc_attr.c
index a364cae9e984..9290713af253 100644
--- a/drivers/scsi/lpfc/lpfc_attr.c
+++ b/drivers/scsi/lpfc/lpfc_attr.c
@@ -692,7 +692,7 @@ lpfc_do_offline(struct lpfc_hba *phba, uint32_t type)
 	 */
 	for (i = 0; i < psli->num_rings; i++) {
 		pring = &psli->ring[i];
-		while (pring->txcmplq_cnt) {
+		while (!list_empty(&pring->txcmplq)) {
 			msleep(10);
 			if (cnt++ > 500) {  /* 5 secs */
 				lpfc_printf_log(phba,
@@ -2302,11 +2302,17 @@ static DEVICE_ATTR(lpfc_enable_npiv, S_IRUGO, lpfc_enable_npiv_show, NULL);
 LPFC_ATTR_R(fcf_failover_policy, 1, 1, 2,
 	"FCF Fast failover=1 Priority failover=2");
 
-int lpfc_enable_rrq;
+int lpfc_enable_rrq = 2;
 module_param(lpfc_enable_rrq, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_enable_rrq, "Enable RRQ functionality");
 lpfc_param_show(enable_rrq);
-lpfc_param_init(enable_rrq, 0, 0, 1);
+/*
+# lpfc_enable_rrq: Track XRI/OXID reuse after IO failures
+#	0x0 = disabled, XRI/OXID use not tracked.
+#	0x1 = XRI/OXID reuse is timed with ratov, RRQ sent.
+#	0x2 = XRI/OXID reuse is timed with ratov, No RRQ sent.
+*/
+lpfc_param_init(enable_rrq, 2, 0, 2);
 static DEVICE_ATTR(lpfc_enable_rrq, S_IRUGO, lpfc_enable_rrq_show, NULL);
 
 /*
diff --git a/drivers/scsi/lpfc/lpfc_bsg.c b/drivers/scsi/lpfc/lpfc_bsg.c
index f5d106456f1d..888666892004 100644
--- a/drivers/scsi/lpfc/lpfc_bsg.c
+++ b/drivers/scsi/lpfc/lpfc_bsg.c
@@ -64,18 +64,14 @@ struct lpfc_bsg_event {
 	struct list_head events_to_get;
 	struct list_head events_to_see;
 
-	/* job waiting for this event to finish */
-	struct fc_bsg_job *set_job;
+	/* driver data associated with the job */
+	void *dd_data;
 };
 
 struct lpfc_bsg_iocb {
 	struct lpfc_iocbq *cmdiocbq;
-	struct lpfc_iocbq *rspiocbq;
-	struct lpfc_dmabuf *bmp;
+	struct lpfc_dmabuf *rmp;
 	struct lpfc_nodelist *ndlp;
-
-	/* job waiting for this iocb to finish */
-	struct fc_bsg_job *set_job;
 };
 
 struct lpfc_bsg_mbox {
@@ -86,20 +82,13 @@ struct lpfc_bsg_mbox {
 	uint32_t mbOffset; /* from app */
 	uint32_t inExtWLen; /* from app */
 	uint32_t outExtWLen; /* from app */
-
-	/* job waiting for this mbox command to finish */
-	struct fc_bsg_job *set_job;
 };
 
 #define MENLO_DID 0x0000FC0E
 
 struct lpfc_bsg_menlo {
 	struct lpfc_iocbq *cmdiocbq;
-	struct lpfc_iocbq *rspiocbq;
-	struct lpfc_dmabuf *bmp;
-
-	/* job waiting for this iocb to finish */
-	struct fc_bsg_job *set_job;
+	struct lpfc_dmabuf *rmp;
 };
 
 #define TYPE_EVT 	1
@@ -108,6 +97,7 @@ struct lpfc_bsg_menlo {
 #define TYPE_MENLO	4
 struct bsg_job_data {
 	uint32_t type;
+	struct fc_bsg_job *set_job; /* job waiting for this iocb to finish */
 	union {
 		struct lpfc_bsg_event *evt;
 		struct lpfc_bsg_iocb iocb;
@@ -141,6 +131,138 @@ struct lpfc_dmabufext {
 	uint32_t flag;
 };
 
+static void
+lpfc_free_bsg_buffers(struct lpfc_hba *phba, struct lpfc_dmabuf *mlist)
+{
+	struct lpfc_dmabuf *mlast, *next_mlast;
+
+	if (mlist) {
+		list_for_each_entry_safe(mlast, next_mlast, &mlist->list,
+					 list) {
+			lpfc_mbuf_free(phba, mlast->virt, mlast->phys);
+			list_del(&mlast->list);
+			kfree(mlast);
+		}
+		lpfc_mbuf_free(phba, mlist->virt, mlist->phys);
+		kfree(mlist);
+	}
+	return;
+}
+
+static struct lpfc_dmabuf *
+lpfc_alloc_bsg_buffers(struct lpfc_hba *phba, unsigned int size,
+		       int outbound_buffers, struct ulp_bde64 *bpl,
+		       int *bpl_entries)
+{
+	struct lpfc_dmabuf *mlist = NULL;
+	struct lpfc_dmabuf *mp;
+	unsigned int bytes_left = size;
+
+	/* Verify we can support the size specified */
+	if (!size || (size > (*bpl_entries * LPFC_BPL_SIZE)))
+		return NULL;
+
+	/* Determine the number of dma buffers to allocate */
+	*bpl_entries = (size % LPFC_BPL_SIZE ? size/LPFC_BPL_SIZE + 1 :
+			size/LPFC_BPL_SIZE);
+
+	/* Allocate dma buffer and place in BPL passed */
+	while (bytes_left) {
+		/* Allocate dma buffer  */
+		mp = kmalloc(sizeof(struct lpfc_dmabuf), GFP_KERNEL);
+		if (!mp) {
+			if (mlist)
+				lpfc_free_bsg_buffers(phba, mlist);
+			return NULL;
+		}
+
+		INIT_LIST_HEAD(&mp->list);
+		mp->virt = lpfc_mbuf_alloc(phba, MEM_PRI, &(mp->phys));
+
+		if (!mp->virt) {
+			kfree(mp);
+			if (mlist)
+				lpfc_free_bsg_buffers(phba, mlist);
+			return NULL;
+		}
+
+		/* Queue it to a linked list */
+		if (!mlist)
+			mlist = mp;
+		else
+			list_add_tail(&mp->list, &mlist->list);
+
+		/* Add buffer to buffer pointer list */
+		if (outbound_buffers)
+			bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64;
+		else
+			bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64I;
+		bpl->addrLow = le32_to_cpu(putPaddrLow(mp->phys));
+		bpl->addrHigh = le32_to_cpu(putPaddrHigh(mp->phys));
+		bpl->tus.f.bdeSize = (uint16_t)
+			(bytes_left >= LPFC_BPL_SIZE ? LPFC_BPL_SIZE :
+			 bytes_left);
+		bytes_left -= bpl->tus.f.bdeSize;
+		bpl->tus.w = le32_to_cpu(bpl->tus.w);
+		bpl++;
+	}
+	return mlist;
+}
+
+static unsigned int
+lpfc_bsg_copy_data(struct lpfc_dmabuf *dma_buffers,
+		   struct fc_bsg_buffer *bsg_buffers,
+		   unsigned int bytes_to_transfer, int to_buffers)
+{
+
+	struct lpfc_dmabuf *mp;
+	unsigned int transfer_bytes, bytes_copied = 0;
+	unsigned int sg_offset, dma_offset;
+	unsigned char *dma_address, *sg_address;
+	struct scatterlist *sgel;
+	LIST_HEAD(temp_list);
+
+
+	list_splice_init(&dma_buffers->list, &temp_list);
+	list_add(&dma_buffers->list, &temp_list);
+	sg_offset = 0;
+	sgel = bsg_buffers->sg_list;
+	list_for_each_entry(mp, &temp_list, list) {
+		dma_offset = 0;
+		while (bytes_to_transfer && sgel &&
+		       (dma_offset < LPFC_BPL_SIZE)) {
+			dma_address = mp->virt + dma_offset;
+			if (sg_offset) {
+				/* Continue previous partial transfer of sg */
+				sg_address = sg_virt(sgel) + sg_offset;
+				transfer_bytes = sgel->length - sg_offset;
+			} else {
+				sg_address = sg_virt(sgel);
+				transfer_bytes = sgel->length;
+			}
+			if (bytes_to_transfer < transfer_bytes)
+				transfer_bytes = bytes_to_transfer;
+			if (transfer_bytes > (LPFC_BPL_SIZE - dma_offset))
+				transfer_bytes = LPFC_BPL_SIZE - dma_offset;
+			if (to_buffers)
+				memcpy(dma_address, sg_address, transfer_bytes);
+			else
+				memcpy(sg_address, dma_address, transfer_bytes);
+			dma_offset += transfer_bytes;
+			sg_offset += transfer_bytes;
+			bytes_to_transfer -= transfer_bytes;
+			bytes_copied += transfer_bytes;
+			if (sg_offset >= sgel->length) {
+				sg_offset = 0;
+				sgel = sg_next(sgel);
+			}
+		}
+	}
+	list_del_init(&dma_buffers->list);
+	list_splice(&temp_list, &dma_buffers->list);
+	return bytes_copied;
+}
+
 /**
  * lpfc_bsg_send_mgmt_cmd_cmp - lpfc_bsg_send_mgmt_cmd's completion handler
  * @phba: Pointer to HBA context object.
@@ -166,62 +288,72 @@ lpfc_bsg_send_mgmt_cmd_cmp(struct lpfc_hba *phba,
 	struct bsg_job_data *dd_data;
 	struct fc_bsg_job *job;
 	IOCB_t *rsp;
-	struct lpfc_dmabuf *bmp;
+	struct lpfc_dmabuf *bmp, *cmp, *rmp;
 	struct lpfc_nodelist *ndlp;
 	struct lpfc_bsg_iocb *iocb;
 	unsigned long flags;
+	unsigned int rsp_size;
 	int rc = 0;
 
+	dd_data = cmdiocbq->context1;
+
+	/* Determine if job has been aborted */
 	spin_lock_irqsave(&phba->ct_ev_lock, flags);
-	dd_data = cmdiocbq->context2;
-	if (!dd_data) {
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		lpfc_sli_release_iocbq(phba, cmdiocbq);
-		return;
+	job = dd_data->set_job;
+	if (job) {
+		/* Prevent timeout handling from trying to abort job */
+		job->dd_data = NULL;
 	}
+	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
 
 	iocb = &dd_data->context_un.iocb;
-	job = iocb->set_job;
-	job->dd_data = NULL; /* so timeout handler does not reply */
-
-	bmp = iocb->bmp;
+	ndlp = iocb->ndlp;
+	rmp = iocb->rmp;
+	cmp = cmdiocbq->context2;
+	bmp = cmdiocbq->context3;
 	rsp = &rspiocbq->iocb;
-	ndlp = cmdiocbq->context1;
 
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-		     job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	pci_unmap_sg(phba->pcidev, job->reply_payload.sg_list,
-		     job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+	/* Copy the completed data or set the error status */
 
-	if (rsp->ulpStatus) {
-		if (rsp->ulpStatus == IOSTAT_LOCAL_REJECT) {
-			switch (rsp->un.ulpWord[4] & IOERR_PARAM_MASK) {
-			case IOERR_SEQUENCE_TIMEOUT:
-				rc = -ETIMEDOUT;
-				break;
-			case IOERR_INVALID_RPI:
-				rc = -EFAULT;
-				break;
-			default:
+	if (job) {
+		if (rsp->ulpStatus) {
+			if (rsp->ulpStatus == IOSTAT_LOCAL_REJECT) {
+				switch (rsp->un.ulpWord[4] & IOERR_PARAM_MASK) {
+				case IOERR_SEQUENCE_TIMEOUT:
+					rc = -ETIMEDOUT;
+					break;
+				case IOERR_INVALID_RPI:
+					rc = -EFAULT;
+					break;
+				default:
+					rc = -EACCES;
+					break;
+				}
+			} else {
 				rc = -EACCES;
-				break;
 			}
-		} else
-			rc = -EACCES;
-	} else
-		job->reply->reply_payload_rcv_len =
-			rsp->un.genreq64.bdl.bdeSize;
+		} else {
+			rsp_size = rsp->un.genreq64.bdl.bdeSize;
+			job->reply->reply_payload_rcv_len =
+				lpfc_bsg_copy_data(rmp, &job->reply_payload,
+						   rsp_size, 0);
+		}
+	}
 
+	lpfc_free_bsg_buffers(phba, cmp);
+	lpfc_free_bsg_buffers(phba, rmp);
 	lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
+	kfree(bmp);
 	lpfc_sli_release_iocbq(phba, cmdiocbq);
 	lpfc_nlp_put(ndlp);
-	kfree(bmp);
 	kfree(dd_data);
-	/* make error code available to userspace */
-	job->reply->result = rc;
-	/* complete the job back to userspace */
-	job->job_done(job);
-	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+
+	/* Complete the job if the job is still active */
+
+	if (job) {
+		job->reply->result = rc;
+		job->job_done(job);
+	}
 	return;
 }
 
@@ -240,12 +372,9 @@ lpfc_bsg_send_mgmt_cmd(struct fc_bsg_job *job)
 	uint32_t timeout;
 	struct lpfc_iocbq *cmdiocbq = NULL;
 	IOCB_t *cmd;
-	struct lpfc_dmabuf *bmp = NULL;
+	struct lpfc_dmabuf *bmp = NULL, *cmp = NULL, *rmp = NULL;
 	int request_nseg;
 	int reply_nseg;
-	struct scatterlist *sgel = NULL;
-	int numbde;
-	dma_addr_t busaddr;
 	struct bsg_job_data *dd_data;
 	uint32_t creg_val;
 	int rc = 0;
@@ -268,54 +397,50 @@ lpfc_bsg_send_mgmt_cmd(struct fc_bsg_job *job)
 		goto no_ndlp;
 	}
 
-	bmp = kmalloc(sizeof(struct lpfc_dmabuf), GFP_KERNEL);
-	if (!bmp) {
-		rc = -ENOMEM;
-		goto free_ndlp;
-	}
-
 	if (ndlp->nlp_flag & NLP_ELS_SND_MASK) {
 		rc = -ENODEV;
-		goto free_bmp;
+		goto free_ndlp;
 	}
 
 	cmdiocbq = lpfc_sli_get_iocbq(phba);
 	if (!cmdiocbq) {
 		rc = -ENOMEM;
-		goto free_bmp;
+		goto free_ndlp;
 	}
 
 	cmd = &cmdiocbq->iocb;
+
+	bmp = kmalloc(sizeof(struct lpfc_dmabuf), GFP_KERNEL);
+	if (!bmp) {
+		rc = -ENOMEM;
+		goto free_cmdiocbq;
+	}
 	bmp->virt = lpfc_mbuf_alloc(phba, 0, &bmp->phys);
 	if (!bmp->virt) {
 		rc = -ENOMEM;
-		goto free_cmdiocbq;
+		goto free_bmp;
 	}
 
 	INIT_LIST_HEAD(&bmp->list);
+
 	bpl = (struct ulp_bde64 *) bmp->virt;
-	request_nseg = pci_map_sg(phba->pcidev, job->request_payload.sg_list,
-				  job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	for_each_sg(job->request_payload.sg_list, sgel, request_nseg, numbde) {
-		busaddr = sg_dma_address(sgel);
-		bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64;
-		bpl->tus.f.bdeSize = sg_dma_len(sgel);
-		bpl->tus.w = cpu_to_le32(bpl->tus.w);
-		bpl->addrLow = cpu_to_le32(putPaddrLow(busaddr));
-		bpl->addrHigh = cpu_to_le32(putPaddrHigh(busaddr));
-		bpl++;
+	request_nseg = LPFC_BPL_SIZE/sizeof(struct ulp_bde64);
+	cmp = lpfc_alloc_bsg_buffers(phba, job->request_payload.payload_len,
+				     1, bpl, &request_nseg);
+	if (!cmp) {
+		rc = -ENOMEM;
+		goto free_bmp;
 	}
+	lpfc_bsg_copy_data(cmp, &job->request_payload,
+			   job->request_payload.payload_len, 1);
 
-	reply_nseg = pci_map_sg(phba->pcidev, job->reply_payload.sg_list,
-				job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
-	for_each_sg(job->reply_payload.sg_list, sgel, reply_nseg, numbde) {
-		busaddr = sg_dma_address(sgel);
-		bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64I;
-		bpl->tus.f.bdeSize = sg_dma_len(sgel);
-		bpl->tus.w = cpu_to_le32(bpl->tus.w);
-		bpl->addrLow = cpu_to_le32(putPaddrLow(busaddr));
-		bpl->addrHigh = cpu_to_le32(putPaddrHigh(busaddr));
-		bpl++;
+	bpl += request_nseg;
+	reply_nseg = LPFC_BPL_SIZE/sizeof(struct ulp_bde64) - request_nseg;
+	rmp = lpfc_alloc_bsg_buffers(phba, job->reply_payload.payload_len, 0,
+				     bpl, &reply_nseg);
+	if (!rmp) {
+		rc = -ENOMEM;
+		goto free_cmp;
 	}
 
 	cmd->un.genreq64.bdl.ulpIoTag32 = 0;
@@ -343,17 +468,20 @@ lpfc_bsg_send_mgmt_cmd(struct fc_bsg_job *job)
 	cmd->ulpTimeout = timeout;
 
 	cmdiocbq->iocb_cmpl = lpfc_bsg_send_mgmt_cmd_cmp;
-	cmdiocbq->context1 = ndlp;
-	cmdiocbq->context2 = dd_data;
+	cmdiocbq->context1 = dd_data;
+	cmdiocbq->context2 = cmp;
+	cmdiocbq->context3 = bmp;
 	dd_data->type = TYPE_IOCB;
+	dd_data->set_job = job;
 	dd_data->context_un.iocb.cmdiocbq = cmdiocbq;
-	dd_data->context_un.iocb.set_job = job;
-	dd_data->context_un.iocb.bmp = bmp;
+	dd_data->context_un.iocb.ndlp = ndlp;
+	dd_data->context_un.iocb.rmp = rmp;
+	job->dd_data = dd_data;
 
 	if (phba->cfg_poll & DISABLE_FCP_RING_INT) {
 		if (lpfc_readl(phba->HCregaddr, &creg_val)) {
 			rc = -EIO ;
-			goto free_cmdiocbq;
+			goto free_rmp;
 		}
 		creg_val |= (HC_R0INT_ENA << LPFC_FCP_RING);
 		writel(creg_val, phba->HCregaddr);
@@ -368,19 +496,18 @@ lpfc_bsg_send_mgmt_cmd(struct fc_bsg_job *job)
 	else
 		rc = -EIO;
 
-
 	/* iocb failed so cleanup */
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-		     job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	pci_unmap_sg(phba->pcidev, job->reply_payload.sg_list,
-		     job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
 
-	lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
-
-free_cmdiocbq:
-	lpfc_sli_release_iocbq(phba, cmdiocbq);
+free_rmp:
+	lpfc_free_bsg_buffers(phba, rmp);
+free_cmp:
+	lpfc_free_bsg_buffers(phba, cmp);
 free_bmp:
+	if (bmp->virt)
+		lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
 	kfree(bmp);
+free_cmdiocbq:
+	lpfc_sli_release_iocbq(phba, cmdiocbq);
 free_ndlp:
 	lpfc_nlp_put(ndlp);
 no_ndlp:
@@ -418,67 +545,68 @@ lpfc_bsg_rport_els_cmp(struct lpfc_hba *phba,
 	struct fc_bsg_job *job;
 	IOCB_t *rsp;
 	struct lpfc_nodelist *ndlp;
-	struct lpfc_dmabuf *pbuflist = NULL;
+	struct lpfc_dmabuf *pcmd = NULL, *prsp = NULL;
 	struct fc_bsg_ctels_reply *els_reply;
 	uint8_t *rjt_data;
 	unsigned long flags;
+	unsigned int rsp_size;
 	int rc = 0;
 
-	spin_lock_irqsave(&phba->ct_ev_lock, flags);
 	dd_data = cmdiocbq->context1;
-	/* normal completion and timeout crossed paths, already done */
-	if (!dd_data) {
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		return;
-	}
+	ndlp = dd_data->context_un.iocb.ndlp;
+	cmdiocbq->context1 = ndlp;
 
-	cmdiocbq->iocb_flag |= LPFC_IO_WAKE;
-	if (cmdiocbq->context2 && rspiocbq)
-		memcpy(&((struct lpfc_iocbq *)cmdiocbq->context2)->iocb,
-		       &rspiocbq->iocb, sizeof(IOCB_t));
+	/* Determine if job has been aborted */
+	spin_lock_irqsave(&phba->ct_ev_lock, flags);
+	job = dd_data->set_job;
+	if (job) {
+		/* Prevent timeout handling from trying to abort job  */
+		job->dd_data = NULL;
+	}
+	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
 
-	job = dd_data->context_un.iocb.set_job;
-	cmdiocbq = dd_data->context_un.iocb.cmdiocbq;
-	rspiocbq = dd_data->context_un.iocb.rspiocbq;
 	rsp = &rspiocbq->iocb;
-	ndlp = dd_data->context_un.iocb.ndlp;
+	pcmd = (struct lpfc_dmabuf *)cmdiocbq->context2;
+	prsp = (struct lpfc_dmabuf *)pcmd->list.next;
 
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-		     job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	pci_unmap_sg(phba->pcidev, job->reply_payload.sg_list,
-		     job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+	/* Copy the completed job data or determine the job status if job is
+	 * still active
+	 */
 
-	if (job->reply->result == -EAGAIN)
-		rc = -EAGAIN;
-	else if (rsp->ulpStatus == IOSTAT_SUCCESS)
-		job->reply->reply_payload_rcv_len =
-			rsp->un.elsreq64.bdl.bdeSize;
-	else if (rsp->ulpStatus == IOSTAT_LS_RJT) {
-		job->reply->reply_payload_rcv_len =
-			sizeof(struct fc_bsg_ctels_reply);
-		/* LS_RJT data returned in word 4 */
-		rjt_data = (uint8_t *)&rsp->un.ulpWord[4];
-		els_reply = &job->reply->reply_data.ctels_reply;
-		els_reply->status = FC_CTELS_STATUS_REJECT;
-		els_reply->rjt_data.action = rjt_data[3];
-		els_reply->rjt_data.reason_code = rjt_data[2];
-		els_reply->rjt_data.reason_explanation = rjt_data[1];
-		els_reply->rjt_data.vendor_unique = rjt_data[0];
-	} else
-		rc = -EIO;
+	if (job) {
+		if (rsp->ulpStatus == IOSTAT_SUCCESS) {
+			rsp_size = rsp->un.elsreq64.bdl.bdeSize;
+			job->reply->reply_payload_rcv_len =
+				sg_copy_from_buffer(job->reply_payload.sg_list,
+						    job->reply_payload.sg_cnt,
+						    prsp->virt,
+						    rsp_size);
+		} else if (rsp->ulpStatus == IOSTAT_LS_RJT) {
+			job->reply->reply_payload_rcv_len =
+				sizeof(struct fc_bsg_ctels_reply);
+			/* LS_RJT data returned in word 4 */
+			rjt_data = (uint8_t *)&rsp->un.ulpWord[4];
+			els_reply = &job->reply->reply_data.ctels_reply;
+			els_reply->status = FC_CTELS_STATUS_REJECT;
+			els_reply->rjt_data.action = rjt_data[3];
+			els_reply->rjt_data.reason_code = rjt_data[2];
+			els_reply->rjt_data.reason_explanation = rjt_data[1];
+			els_reply->rjt_data.vendor_unique = rjt_data[0];
+		} else {
+			rc = -EIO;
+		}
+	}
 
-	pbuflist = (struct lpfc_dmabuf *) cmdiocbq->context3;
-	lpfc_mbuf_free(phba, pbuflist->virt, pbuflist->phys);
-	lpfc_sli_release_iocbq(phba, rspiocbq);
-	lpfc_sli_release_iocbq(phba, cmdiocbq);
 	lpfc_nlp_put(ndlp);
+	lpfc_els_free_iocb(phba, cmdiocbq);
 	kfree(dd_data);
-	/* make error code available to userspace */
-	job->reply->result = rc;
-	job->dd_data = NULL;
-	/* complete the job back to userspace */
-	job->job_done(job);
-	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+
+	/* Complete the job if the job is still active */
+
+	if (job) {
+		job->reply->result = rc;
+		job->job_done(job);
+	}
 	return;
 }
 
@@ -496,19 +624,8 @@ lpfc_bsg_rport_els(struct fc_bsg_job *job)
 	uint32_t elscmd;
 	uint32_t cmdsize;
 	uint32_t rspsize;
-	struct lpfc_iocbq *rspiocbq;
 	struct lpfc_iocbq *cmdiocbq;
-	IOCB_t *rsp;
 	uint16_t rpi = 0;
-	struct lpfc_dmabuf *pcmd;
-	struct lpfc_dmabuf *prsp;
-	struct lpfc_dmabuf *pbuflist = NULL;
-	struct ulp_bde64 *bpl;
-	int request_nseg;
-	int reply_nseg;
-	struct scatterlist *sgel = NULL;
-	int numbde;
-	dma_addr_t busaddr;
 	struct bsg_job_data *dd_data;
 	uint32_t creg_val;
 	int rc = 0;
@@ -516,6 +633,15 @@ lpfc_bsg_rport_els(struct fc_bsg_job *job)
 	/* in case no data is transferred */
 	job->reply->reply_payload_rcv_len = 0;
 
+	/* verify the els command is not greater than the
+	 * maximum ELS transfer size.
+	 */
+
+	if (job->request_payload.payload_len > FCELSSIZE) {
+		rc = -EINVAL;
+		goto no_dd_data;
+	}
+
 	/* allocate our bsg tracking structure */
 	dd_data = kmalloc(sizeof(struct bsg_job_data), GFP_KERNEL);
 	if (!dd_data) {
@@ -525,88 +651,51 @@ lpfc_bsg_rport_els(struct fc_bsg_job *job)
 		goto no_dd_data;
 	}
 
-	if (!lpfc_nlp_get(ndlp)) {
-		rc = -ENODEV;
-		goto free_dd_data;
-	}
-
 	elscmd = job->request->rqst_data.r_els.els_code;
 	cmdsize = job->request_payload.payload_len;
 	rspsize = job->reply_payload.payload_len;
-	rspiocbq = lpfc_sli_get_iocbq(phba);
-	if (!rspiocbq) {
-		lpfc_nlp_put(ndlp);
-		rc = -ENOMEM;
+
+	if (!lpfc_nlp_get(ndlp)) {
+		rc = -ENODEV;
 		goto free_dd_data;
 	}
 
-	rsp = &rspiocbq->iocb;
-	rpi = ndlp->nlp_rpi;
+	/* We will use the allocated dma buffers by prep els iocb for command
+	 * and response to ensure if the job times out and the request is freed,
+	 * we won't be dma into memory that is no longer allocated to for the
+	 * request.
+	 */
 
 	cmdiocbq = lpfc_prep_els_iocb(vport, 1, cmdsize, 0, ndlp,
 				      ndlp->nlp_DID, elscmd);
 	if (!cmdiocbq) {
 		rc = -EIO;
-		goto free_rspiocbq;
+		goto release_ndlp;
 	}
 
-	/* prep els iocb set context1 to the ndlp, context2 to the command
-	 * dmabuf, context3 holds the data dmabuf
-	 */
-	pcmd = (struct lpfc_dmabuf *) cmdiocbq->context2;
-	prsp = (struct lpfc_dmabuf *) pcmd->list.next;
-	lpfc_mbuf_free(phba, pcmd->virt, pcmd->phys);
-	kfree(pcmd);
-	lpfc_mbuf_free(phba, prsp->virt, prsp->phys);
-	kfree(prsp);
-	cmdiocbq->context2 = NULL;
-
-	pbuflist = (struct lpfc_dmabuf *) cmdiocbq->context3;
-	bpl = (struct ulp_bde64 *) pbuflist->virt;
-
-	request_nseg = pci_map_sg(phba->pcidev, job->request_payload.sg_list,
-				  job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	for_each_sg(job->request_payload.sg_list, sgel, request_nseg, numbde) {
-		busaddr = sg_dma_address(sgel);
-		bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64;
-		bpl->tus.f.bdeSize = sg_dma_len(sgel);
-		bpl->tus.w = cpu_to_le32(bpl->tus.w);
-		bpl->addrLow = cpu_to_le32(putPaddrLow(busaddr));
-		bpl->addrHigh = cpu_to_le32(putPaddrHigh(busaddr));
-		bpl++;
-	}
+	rpi = ndlp->nlp_rpi;
+
+	/* Transfer the request payload to allocated command dma buffer */
+
+	sg_copy_to_buffer(job->request_payload.sg_list,
+			  job->request_payload.sg_cnt,
+			  ((struct lpfc_dmabuf *)cmdiocbq->context2)->virt,
+			  cmdsize);
 
-	reply_nseg = pci_map_sg(phba->pcidev, job->reply_payload.sg_list,
-				job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
-	for_each_sg(job->reply_payload.sg_list, sgel, reply_nseg, numbde) {
-		busaddr = sg_dma_address(sgel);
-		bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64I;
-		bpl->tus.f.bdeSize = sg_dma_len(sgel);
-		bpl->tus.w = cpu_to_le32(bpl->tus.w);
-		bpl->addrLow = cpu_to_le32(putPaddrLow(busaddr));
-		bpl->addrHigh = cpu_to_le32(putPaddrHigh(busaddr));
-		bpl++;
-	}
-	cmdiocbq->iocb.un.elsreq64.bdl.bdeSize =
-		(request_nseg + reply_nseg) * sizeof(struct ulp_bde64);
 	if (phba->sli_rev == LPFC_SLI_REV4)
 		cmdiocbq->iocb.ulpContext = phba->sli4_hba.rpi_ids[rpi];
 	else
 		cmdiocbq->iocb.ulpContext = rpi;
 	cmdiocbq->iocb_flag |= LPFC_IO_LIBDFC;
-	cmdiocbq->context1 = NULL;
-	cmdiocbq->context2 = NULL;
-
-	cmdiocbq->iocb_cmpl = lpfc_bsg_rport_els_cmp;
 	cmdiocbq->context1 = dd_data;
 	cmdiocbq->context_un.ndlp = ndlp;
-	cmdiocbq->context2 = rspiocbq;
+	cmdiocbq->iocb_cmpl = lpfc_bsg_rport_els_cmp;
 	dd_data->type = TYPE_IOCB;
+	dd_data->set_job = job;
 	dd_data->context_un.iocb.cmdiocbq = cmdiocbq;
-	dd_data->context_un.iocb.rspiocbq = rspiocbq;
-	dd_data->context_un.iocb.set_job = job;
-	dd_data->context_un.iocb.bmp = NULL;
 	dd_data->context_un.iocb.ndlp = ndlp;
+	dd_data->context_un.iocb.rmp = NULL;
+	job->dd_data = dd_data;
 
 	if (phba->cfg_poll & DISABLE_FCP_RING_INT) {
 		if (lpfc_readl(phba->HCregaddr, &creg_val)) {
@@ -617,8 +706,9 @@ lpfc_bsg_rport_els(struct fc_bsg_job *job)
 		writel(creg_val, phba->HCregaddr);
 		readl(phba->HCregaddr); /* flush */
 	}
+
 	rc = lpfc_sli_issue_iocb(phba, LPFC_ELS_RING, cmdiocbq, 0);
-	lpfc_nlp_put(ndlp);
+
 	if (rc == IOCB_SUCCESS)
 		return 0; /* done for now */
 	else if (rc == IOCB_BUSY)
@@ -627,17 +717,12 @@ lpfc_bsg_rport_els(struct fc_bsg_job *job)
 		rc = -EIO;
 
 linkdown_err:
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-		     job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	pci_unmap_sg(phba->pcidev, job->reply_payload.sg_list,
-		     job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
 
-	lpfc_mbuf_free(phba, pbuflist->virt, pbuflist->phys);
-
-	lpfc_sli_release_iocbq(phba, cmdiocbq);
+	cmdiocbq->context1 = ndlp;
+	lpfc_els_free_iocb(phba, cmdiocbq);
 
-free_rspiocbq:
-	lpfc_sli_release_iocbq(phba, rspiocbq);
+release_ndlp:
+	lpfc_nlp_put(ndlp);
 
 free_dd_data:
 	kfree(dd_data);
@@ -680,6 +765,7 @@ lpfc_bsg_event_free(struct kref *kref)
 		kfree(ed);
 	}
 
+	kfree(evt->dd_data);
 	kfree(evt);
 }
 
@@ -723,6 +809,7 @@ lpfc_bsg_event_new(uint32_t ev_mask, int ev_reg_id, uint32_t ev_req_id)
 	evt->req_id = ev_req_id;
 	evt->reg_id = ev_reg_id;
 	evt->wait_time_stamp = jiffies;
+	evt->dd_data = NULL;
 	init_waitqueue_head(&evt->wq);
 	kref_init(&evt->kref);
 	return evt;
@@ -790,6 +877,7 @@ lpfc_bsg_ct_unsol_event(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 	struct lpfc_hbq_entry *hbqe;
 	struct lpfc_sli_ct_request *ct_req;
 	struct fc_bsg_job *job = NULL;
+	struct bsg_job_data *dd_data = NULL;
 	unsigned long flags;
 	int size = 0;
 
@@ -986,10 +1074,11 @@ lpfc_bsg_ct_unsol_event(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 		}
 
 		list_move(evt->events_to_see.prev, &evt->events_to_get);
-		lpfc_bsg_event_unref(evt);
 
-		job = evt->set_job;
-		evt->set_job = NULL;
+		dd_data = (struct bsg_job_data *)evt->dd_data;
+		job = dd_data->set_job;
+		dd_data->set_job = NULL;
+		lpfc_bsg_event_unref(evt);
 		if (job) {
 			job->reply->reply_payload_rcv_len = size;
 			/* make error code available to userspace */
@@ -1078,14 +1167,6 @@ lpfc_bsg_hba_set_event(struct fc_bsg_job *job)
 		goto job_error;
 	}
 
-	dd_data = kmalloc(sizeof(struct bsg_job_data), GFP_KERNEL);
-	if (dd_data == NULL) {
-		lpfc_printf_log(phba, KERN_WARNING, LOG_LIBDFC,
-				"2734 Failed allocation of dd_data\n");
-		rc = -ENOMEM;
-		goto job_error;
-	}
-
 	event_req = (struct set_ct_event *)
 		job->request->rqst_data.h_vendor.vendor_cmd;
 	ev_mask = ((uint32_t)(unsigned long)event_req->type_mask &
@@ -1095,6 +1176,7 @@ lpfc_bsg_hba_set_event(struct fc_bsg_job *job)
 		if (evt->reg_id == event_req->ev_reg_id) {
 			lpfc_bsg_event_ref(evt);
 			evt->wait_time_stamp = jiffies;
+			dd_data = (struct bsg_job_data *)evt->dd_data;
 			break;
 		}
 	}
@@ -1102,6 +1184,13 @@ lpfc_bsg_hba_set_event(struct fc_bsg_job *job)
 
 	if (&evt->node == &phba->ct_ev_waiters) {
 		/* no event waiting struct yet - first call */
+		dd_data = kmalloc(sizeof(struct bsg_job_data), GFP_KERNEL);
+		if (dd_data == NULL) {
+			lpfc_printf_log(phba, KERN_WARNING, LOG_LIBDFC,
+					"2734 Failed allocation of dd_data\n");
+			rc = -ENOMEM;
+			goto job_error;
+		}
 		evt = lpfc_bsg_event_new(ev_mask, event_req->ev_reg_id,
 					event_req->ev_req_id);
 		if (!evt) {
@@ -1111,7 +1200,10 @@ lpfc_bsg_hba_set_event(struct fc_bsg_job *job)
 			rc = -ENOMEM;
 			goto job_error;
 		}
-
+		dd_data->type = TYPE_EVT;
+		dd_data->set_job = NULL;
+		dd_data->context_un.evt = evt;
+		evt->dd_data = (void *)dd_data;
 		spin_lock_irqsave(&phba->ct_ev_lock, flags);
 		list_add(&evt->node, &phba->ct_ev_waiters);
 		lpfc_bsg_event_ref(evt);
@@ -1121,9 +1213,7 @@ lpfc_bsg_hba_set_event(struct fc_bsg_job *job)
 
 	spin_lock_irqsave(&phba->ct_ev_lock, flags);
 	evt->waiting = 1;
-	dd_data->type = TYPE_EVT;
-	dd_data->context_un.evt = evt;
-	evt->set_job = job; /* for unsolicited command */
+	dd_data->set_job = job; /* for unsolicited command */
 	job->dd_data = dd_data; /* for fc transport timeout callback*/
 	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
 	return 0; /* call job done later */
@@ -1252,57 +1342,64 @@ lpfc_issue_ct_rsp_cmp(struct lpfc_hba *phba,
 	struct bsg_job_data *dd_data;
 	struct fc_bsg_job *job;
 	IOCB_t *rsp;
-	struct lpfc_dmabuf *bmp;
+	struct lpfc_dmabuf *bmp, *cmp;
 	struct lpfc_nodelist *ndlp;
 	unsigned long flags;
 	int rc = 0;
 
+	dd_data = cmdiocbq->context1;
+
+	/* Determine if job has been aborted */
 	spin_lock_irqsave(&phba->ct_ev_lock, flags);
-	dd_data = cmdiocbq->context2;
-	/* normal completion and timeout crossed paths, already done */
-	if (!dd_data) {
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		return;
+	job = dd_data->set_job;
+	if (job) {
+		/* Prevent timeout handling from trying to abort job  */
+		job->dd_data = NULL;
 	}
+	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
 
-	job = dd_data->context_un.iocb.set_job;
-	bmp = dd_data->context_un.iocb.bmp;
-	rsp = &rspiocbq->iocb;
 	ndlp = dd_data->context_un.iocb.ndlp;
+	cmp = cmdiocbq->context2;
+	bmp = cmdiocbq->context3;
+	rsp = &rspiocbq->iocb;
 
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-		     job->request_payload.sg_cnt, DMA_TO_DEVICE);
+	/* Copy the completed job data or set the error status */
 
-	if (rsp->ulpStatus) {
-		if (rsp->ulpStatus == IOSTAT_LOCAL_REJECT) {
-			switch (rsp->un.ulpWord[4] & IOERR_PARAM_MASK) {
-			case IOERR_SEQUENCE_TIMEOUT:
-				rc = -ETIMEDOUT;
-				break;
-			case IOERR_INVALID_RPI:
-				rc = -EFAULT;
-				break;
-			default:
+	if (job) {
+		if (rsp->ulpStatus) {
+			if (rsp->ulpStatus == IOSTAT_LOCAL_REJECT) {
+				switch (rsp->un.ulpWord[4] & IOERR_PARAM_MASK) {
+				case IOERR_SEQUENCE_TIMEOUT:
+					rc = -ETIMEDOUT;
+					break;
+				case IOERR_INVALID_RPI:
+					rc = -EFAULT;
+					break;
+				default:
+					rc = -EACCES;
+					break;
+				}
+			} else {
 				rc = -EACCES;
-				break;
 			}
-		} else
-			rc = -EACCES;
-	} else
-		job->reply->reply_payload_rcv_len =
-			rsp->un.genreq64.bdl.bdeSize;
+		} else {
+			job->reply->reply_payload_rcv_len = 0;
+		}
+	}
 
+	lpfc_free_bsg_buffers(phba, cmp);
 	lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
+	kfree(bmp);
 	lpfc_sli_release_iocbq(phba, cmdiocbq);
 	lpfc_nlp_put(ndlp);
-	kfree(bmp);
 	kfree(dd_data);
-	/* make error code available to userspace */
-	job->reply->result = rc;
-	job->dd_data = NULL;
-	/* complete the job back to userspace */
-	job->job_done(job);
-	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+
+	/* Complete the job if the job is still active */
+
+	if (job) {
+		job->reply->result = rc;
+		job->job_done(job);
+	}
 	return;
 }
 
@@ -1316,7 +1413,8 @@ lpfc_issue_ct_rsp_cmp(struct lpfc_hba *phba,
  **/
 static int
 lpfc_issue_ct_rsp(struct lpfc_hba *phba, struct fc_bsg_job *job, uint32_t tag,
-		  struct lpfc_dmabuf *bmp, int num_entry)
+		  struct lpfc_dmabuf *cmp, struct lpfc_dmabuf *bmp,
+		  int num_entry)
 {
 	IOCB_t *icmd;
 	struct lpfc_iocbq *ctiocb = NULL;
@@ -1377,7 +1475,7 @@ lpfc_issue_ct_rsp(struct lpfc_hba *phba, struct fc_bsg_job *job, uint32_t tag,
 
 		/* Check if the ndlp is active */
 		if (!ndlp || !NLP_CHK_NODE_ACT(ndlp)) {
-			rc = -IOCB_ERROR;
+			rc = IOCB_ERROR;
 			goto issue_ct_rsp_exit;
 		}
 
@@ -1385,7 +1483,7 @@ lpfc_issue_ct_rsp(struct lpfc_hba *phba, struct fc_bsg_job *job, uint32_t tag,
 		 * we respond
 		 */
 		if (!lpfc_nlp_get(ndlp)) {
-			rc = -IOCB_ERROR;
+			rc = IOCB_ERROR;
 			goto issue_ct_rsp_exit;
 		}
 
@@ -1407,17 +1505,17 @@ lpfc_issue_ct_rsp(struct lpfc_hba *phba, struct fc_bsg_job *job, uint32_t tag,
 	ctiocb->iocb_cmpl = NULL;
 	ctiocb->iocb_flag |= LPFC_IO_LIBDFC;
 	ctiocb->vport = phba->pport;
+	ctiocb->context1 = dd_data;
+	ctiocb->context2 = cmp;
 	ctiocb->context3 = bmp;
-
 	ctiocb->iocb_cmpl = lpfc_issue_ct_rsp_cmp;
-	ctiocb->context2 = dd_data;
-	ctiocb->context1 = ndlp;
+
 	dd_data->type = TYPE_IOCB;
+	dd_data->set_job = job;
 	dd_data->context_un.iocb.cmdiocbq = ctiocb;
-	dd_data->context_un.iocb.rspiocbq = NULL;
-	dd_data->context_un.iocb.set_job = job;
-	dd_data->context_un.iocb.bmp = bmp;
 	dd_data->context_un.iocb.ndlp = ndlp;
+	dd_data->context_un.iocb.rmp = NULL;
+	job->dd_data = dd_data;
 
 	if (phba->cfg_poll & DISABLE_FCP_RING_INT) {
 		if (lpfc_readl(phba->HCregaddr, &creg_val)) {
@@ -1454,11 +1552,8 @@ lpfc_bsg_send_mgmt_rsp(struct fc_bsg_job *job)
 	struct send_mgmt_resp *mgmt_resp = (struct send_mgmt_resp *)
 		job->request->rqst_data.h_vendor.vendor_cmd;
 	struct ulp_bde64 *bpl;
-	struct lpfc_dmabuf *bmp = NULL;
-	struct scatterlist *sgel = NULL;
-	int request_nseg;
-	int numbde;
-	dma_addr_t busaddr;
+	struct lpfc_dmabuf *bmp = NULL, *cmp = NULL;
+	int bpl_entries;
 	uint32_t tag = mgmt_resp->tag;
 	unsigned long reqbfrcnt =
 			(unsigned long)job->request_payload.payload_len;
@@ -1486,30 +1581,28 @@ lpfc_bsg_send_mgmt_rsp(struct fc_bsg_job *job)
 
 	INIT_LIST_HEAD(&bmp->list);
 	bpl = (struct ulp_bde64 *) bmp->virt;
-	request_nseg = pci_map_sg(phba->pcidev, job->request_payload.sg_list,
-				  job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	for_each_sg(job->request_payload.sg_list, sgel, request_nseg, numbde) {
-		busaddr = sg_dma_address(sgel);
-		bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64;
-		bpl->tus.f.bdeSize = sg_dma_len(sgel);
-		bpl->tus.w = cpu_to_le32(bpl->tus.w);
-		bpl->addrLow = cpu_to_le32(putPaddrLow(busaddr));
-		bpl->addrHigh = cpu_to_le32(putPaddrHigh(busaddr));
-		bpl++;
+	bpl_entries = (LPFC_BPL_SIZE/sizeof(struct ulp_bde64));
+	cmp = lpfc_alloc_bsg_buffers(phba, job->request_payload.payload_len,
+				     1, bpl, &bpl_entries);
+	if (!cmp) {
+		rc = -ENOMEM;
+		goto send_mgmt_rsp_free_bmp;
 	}
+	lpfc_bsg_copy_data(cmp, &job->request_payload,
+			   job->request_payload.payload_len, 1);
 
-	rc = lpfc_issue_ct_rsp(phba, job, tag, bmp, request_nseg);
+	rc = lpfc_issue_ct_rsp(phba, job, tag, cmp, bmp, bpl_entries);
 
 	if (rc == IOCB_SUCCESS)
 		return 0; /* done for now */
 
-	/* TBD need to handle a timeout */
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-			  job->request_payload.sg_cnt, DMA_TO_DEVICE);
 	rc = -EACCES;
-	lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
+
+	lpfc_free_bsg_buffers(phba, cmp);
 
 send_mgmt_rsp_free_bmp:
+	if (bmp->virt)
+		lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
 	kfree(bmp);
 send_mgmt_rsp_exit:
 	/* make error code available to userspace */
@@ -1559,7 +1652,7 @@ lpfc_bsg_diag_mode_enter(struct lpfc_hba *phba)
 		scsi_block_requests(shost);
 	}
 
-	while (pring->txcmplq_cnt) {
+	while (!list_empty(&pring->txcmplq)) {
 		if (i++ > 500)  /* wait up to 5 seconds */
 			break;
 		msleep(10);
@@ -3193,13 +3286,7 @@ lpfc_bsg_issue_mbox_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 	unsigned long flags;
 	uint8_t *pmb, *pmb_buf;
 
-	spin_lock_irqsave(&phba->ct_ev_lock, flags);
 	dd_data = pmboxq->context1;
-	/* job already timed out? */
-	if (!dd_data) {
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		return;
-	}
 
 	/*
 	 * The outgoing buffer is readily referred from the dma buffer,
@@ -3209,29 +3296,33 @@ lpfc_bsg_issue_mbox_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 	pmb_buf = (uint8_t *)dd_data->context_un.mbox.mb;
 	memcpy(pmb_buf, pmb, sizeof(MAILBOX_t));
 
-	job = dd_data->context_un.mbox.set_job;
+	/* Determine if job has been aborted */
+
+	spin_lock_irqsave(&phba->ct_ev_lock, flags);
+	job = dd_data->set_job;
+	if (job) {
+		/* Prevent timeout handling from trying to abort job  */
+		job->dd_data = NULL;
+	}
+	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+
+	/* Copy the mailbox data to the job if it is still active */
+
 	if (job) {
 		size = job->reply_payload.payload_len;
 		job->reply->reply_payload_rcv_len =
 			sg_copy_from_buffer(job->reply_payload.sg_list,
 					    job->reply_payload.sg_cnt,
 					    pmb_buf, size);
-		/* need to hold the lock until we set job->dd_data to NULL
-		 * to hold off the timeout handler returning to the mid-layer
-		 * while we are still processing the job.
-		 */
-		job->dd_data = NULL;
-		dd_data->context_un.mbox.set_job = NULL;
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-	} else {
-		dd_data->context_un.mbox.set_job = NULL;
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
 	}
 
+	dd_data->set_job = NULL;
 	mempool_free(dd_data->context_un.mbox.pmboxq, phba->mbox_mem_pool);
 	lpfc_bsg_dma_page_free(phba, dd_data->context_un.mbox.dmabuffers);
 	kfree(dd_data);
 
+	/* Complete the job if the job is still active */
+
 	if (job) {
 		job->reply->result = 0;
 		job->job_done(job);
@@ -3377,19 +3468,22 @@ lpfc_bsg_issue_mbox_ext_handle_job(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 	struct lpfc_sli_config_mbox *sli_cfg_mbx;
 	uint8_t *pmbx;
 
-	spin_lock_irqsave(&phba->ct_ev_lock, flags);
 	dd_data = pmboxq->context1;
-	/* has the job already timed out? */
-	if (!dd_data) {
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		job = NULL;
-		goto job_done_out;
+
+	/* Determine if job has been aborted */
+	spin_lock_irqsave(&phba->ct_ev_lock, flags);
+	job = dd_data->set_job;
+	if (job) {
+		/* Prevent timeout handling from trying to abort job  */
+		job->dd_data = NULL;
 	}
+	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
 
 	/*
 	 * The outgoing buffer is readily referred from the dma buffer,
 	 * just need to get header part from mailboxq structure.
 	 */
+
 	pmb = (uint8_t *)&pmboxq->u.mb;
 	pmb_buf = (uint8_t *)dd_data->context_un.mbox.mb;
 	/* Copy the byte swapped response mailbox back to the user */
@@ -3406,21 +3500,18 @@ lpfc_bsg_issue_mbox_ext_handle_job(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 			sli_cfg_mbx->un.sli_config_emb0_subsys.mse[0].buf_len);
 	}
 
-	job = dd_data->context_un.mbox.set_job;
+	/* Complete the job if the job is still active */
+
 	if (job) {
 		size = job->reply_payload.payload_len;
 		job->reply->reply_payload_rcv_len =
 			sg_copy_from_buffer(job->reply_payload.sg_list,
 					    job->reply_payload.sg_cnt,
 					    pmb_buf, size);
+
 		/* result for successful */
 		job->reply->result = 0;
-		job->dd_data = NULL;
-		/* need to hold the lock util we set job->dd_data to NULL
-		 * to hold off the timeout handler from midlayer to take
-		 * any action.
-		 */
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+
 		lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
 				"2937 SLI_CONFIG ext-buffer maibox command "
 				"(x%x/x%x) complete bsg job done, bsize:%d\n",
@@ -3431,20 +3522,18 @@ lpfc_bsg_issue_mbox_ext_handle_job(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 					phba->mbox_ext_buf_ctx.mboxType,
 					dma_ebuf, sta_pos_addr,
 					phba->mbox_ext_buf_ctx.mbx_dmabuf, 0);
-	} else
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-
-job_done_out:
-	if (!job)
+	} else {
 		lpfc_printf_log(phba, KERN_ERR, LOG_LIBDFC,
 				"2938 SLI_CONFIG ext-buffer maibox "
 				"command (x%x/x%x) failure, rc:x%x\n",
 				phba->mbox_ext_buf_ctx.nembType,
 				phba->mbox_ext_buf_ctx.mboxType, rc);
+	}
+
+
 	/* state change */
 	phba->mbox_ext_buf_ctx.state = LPFC_BSG_MBOX_DONE;
 	kfree(dd_data);
-
 	return job;
 }
 
@@ -3461,8 +3550,10 @@ lpfc_bsg_issue_read_mbox_ext_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 {
 	struct fc_bsg_job *job;
 
+	job = lpfc_bsg_issue_mbox_ext_handle_job(phba, pmboxq);
+
 	/* handle the BSG job with mailbox command */
-	if (phba->mbox_ext_buf_ctx.state == LPFC_BSG_MBOX_ABTS)
+	if (!job)
 		pmboxq->u.mb.mbxStatus = MBXERR_ERROR;
 
 	lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
@@ -3470,15 +3561,13 @@ lpfc_bsg_issue_read_mbox_ext_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 			"complete, ctxState:x%x, mbxStatus:x%x\n",
 			phba->mbox_ext_buf_ctx.state, pmboxq->u.mb.mbxStatus);
 
-	job = lpfc_bsg_issue_mbox_ext_handle_job(phba, pmboxq);
-
 	if (pmboxq->u.mb.mbxStatus || phba->mbox_ext_buf_ctx.numBuf == 1)
 		lpfc_bsg_mbox_ext_session_reset(phba);
 
 	/* free base driver mailbox structure memory */
 	mempool_free(pmboxq, phba->mbox_mem_pool);
 
-	/* complete the bsg job if we have it */
+	/* if the job is still active, call job done */
 	if (job)
 		job->job_done(job);
 
@@ -3498,8 +3587,10 @@ lpfc_bsg_issue_write_mbox_ext_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 {
 	struct fc_bsg_job *job;
 
+	job = lpfc_bsg_issue_mbox_ext_handle_job(phba, pmboxq);
+
 	/* handle the BSG job with the mailbox command */
-	if (phba->mbox_ext_buf_ctx.state == LPFC_BSG_MBOX_ABTS)
+	if (!job)
 		pmboxq->u.mb.mbxStatus = MBXERR_ERROR;
 
 	lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
@@ -3507,13 +3598,11 @@ lpfc_bsg_issue_write_mbox_ext_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 			"complete, ctxState:x%x, mbxStatus:x%x\n",
 			phba->mbox_ext_buf_ctx.state, pmboxq->u.mb.mbxStatus);
 
-	job = lpfc_bsg_issue_mbox_ext_handle_job(phba, pmboxq);
-
 	/* free all memory, including dma buffers */
 	mempool_free(pmboxq, phba->mbox_mem_pool);
 	lpfc_bsg_mbox_ext_session_reset(phba);
 
-	/* complete the bsg job if we have it */
+	/* if the job is still active, call job done */
 	if (job)
 		job->job_done(job);
 
@@ -3759,9 +3848,9 @@ lpfc_bsg_sli_cfg_read_cmd_ext(struct lpfc_hba *phba, struct fc_bsg_job *job,
 	/* context fields to callback function */
 	pmboxq->context1 = dd_data;
 	dd_data->type = TYPE_MBOX;
+	dd_data->set_job = job;
 	dd_data->context_un.mbox.pmboxq = pmboxq;
 	dd_data->context_un.mbox.mb = (MAILBOX_t *)pmbx;
-	dd_data->context_un.mbox.set_job = job;
 	job->dd_data = dd_data;
 
 	/* state change */
@@ -3928,14 +4017,14 @@ lpfc_bsg_sli_cfg_write_cmd_ext(struct lpfc_hba *phba, struct fc_bsg_job *job,
 		/* context fields to callback function */
 		pmboxq->context1 = dd_data;
 		dd_data->type = TYPE_MBOX;
+		dd_data->set_job = job;
 		dd_data->context_un.mbox.pmboxq = pmboxq;
 		dd_data->context_un.mbox.mb = (MAILBOX_t *)mbx;
-		dd_data->context_un.mbox.set_job = job;
 		job->dd_data = dd_data;
 
 		/* state change */
-		phba->mbox_ext_buf_ctx.state = LPFC_BSG_MBOX_PORT;
 
+		phba->mbox_ext_buf_ctx.state = LPFC_BSG_MBOX_PORT;
 		rc = lpfc_sli_issue_mbox(phba, pmboxq, MBX_NOWAIT);
 		if ((rc == MBX_SUCCESS) || (rc == MBX_BUSY)) {
 			lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
@@ -3951,6 +4040,7 @@ lpfc_bsg_sli_cfg_write_cmd_ext(struct lpfc_hba *phba, struct fc_bsg_job *job,
 	}
 
 	/* wait for additoinal external buffers */
+
 	job->reply->result = 0;
 	job->job_done(job);
 	return SLI_CONFIG_HANDLED;
@@ -4268,9 +4358,9 @@ lpfc_bsg_write_ebuf_set(struct lpfc_hba *phba, struct fc_bsg_job *job,
 		/* context fields to callback function */
 		pmboxq->context1 = dd_data;
 		dd_data->type = TYPE_MBOX;
+		dd_data->set_job = job;
 		dd_data->context_un.mbox.pmboxq = pmboxq;
 		dd_data->context_un.mbox.mb = (MAILBOX_t *)pbuf;
-		dd_data->context_un.mbox.set_job = job;
 		job->dd_data = dd_data;
 
 		/* state change */
@@ -4455,7 +4545,6 @@ lpfc_bsg_issue_mbox(struct lpfc_hba *phba, struct fc_bsg_job *job,
 	uint8_t *from;
 	uint32_t size;
 
-
 	/* in case no data is transferred */
 	job->reply->reply_payload_rcv_len = 0;
 
@@ -4681,9 +4770,9 @@ lpfc_bsg_issue_mbox(struct lpfc_hba *phba, struct fc_bsg_job *job,
 	/* setup context field to pass wait_queue pointer to wake function */
 	pmboxq->context1 = dd_data;
 	dd_data->type = TYPE_MBOX;
+	dd_data->set_job = job;
 	dd_data->context_un.mbox.pmboxq = pmboxq;
 	dd_data->context_un.mbox.mb = (MAILBOX_t *)pmbx;
-	dd_data->context_un.mbox.set_job = job;
 	dd_data->context_un.mbox.ext = ext;
 	dd_data->context_un.mbox.mbOffset = mbox_req->mbOffset;
 	dd_data->context_un.mbox.inExtWLen = mbox_req->inExtWLen;
@@ -4797,75 +4886,79 @@ lpfc_bsg_menlo_cmd_cmp(struct lpfc_hba *phba,
 	struct bsg_job_data *dd_data;
 	struct fc_bsg_job *job;
 	IOCB_t *rsp;
-	struct lpfc_dmabuf *bmp;
+	struct lpfc_dmabuf *bmp, *cmp, *rmp;
 	struct lpfc_bsg_menlo *menlo;
 	unsigned long flags;
 	struct menlo_response *menlo_resp;
+	unsigned int rsp_size;
 	int rc = 0;
 
-	spin_lock_irqsave(&phba->ct_ev_lock, flags);
 	dd_data = cmdiocbq->context1;
-	if (!dd_data) {
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		return;
-	}
-
+	cmp = cmdiocbq->context2;
+	bmp = cmdiocbq->context3;
 	menlo = &dd_data->context_un.menlo;
-	job = menlo->set_job;
-	job->dd_data = NULL; /* so timeout handler does not reply */
-
-	spin_lock(&phba->hbalock);
-	cmdiocbq->iocb_flag |= LPFC_IO_WAKE;
-	if (cmdiocbq->context2 && rspiocbq)
-		memcpy(&((struct lpfc_iocbq *)cmdiocbq->context2)->iocb,
-		       &rspiocbq->iocb, sizeof(IOCB_t));
-	spin_unlock(&phba->hbalock);
-
-	bmp = menlo->bmp;
-	rspiocbq = menlo->rspiocbq;
+	rmp = menlo->rmp;
 	rsp = &rspiocbq->iocb;
 
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-		     job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	pci_unmap_sg(phba->pcidev, job->reply_payload.sg_list,
-		     job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+	/* Determine if job has been aborted */
+	spin_lock_irqsave(&phba->ct_ev_lock, flags);
+	job = dd_data->set_job;
+	if (job) {
+		/* Prevent timeout handling from trying to abort job  */
+		job->dd_data = NULL;
+	}
+	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+
+	/* Copy the job data or set the failing status for the job */
 
-	/* always return the xri, this would be used in the case
-	 * of a menlo download to allow the data to be sent as a continuation
-	 * of the exchange.
-	 */
-	menlo_resp = (struct menlo_response *)
-		job->reply->reply_data.vendor_reply.vendor_rsp;
-	menlo_resp->xri = rsp->ulpContext;
-	if (rsp->ulpStatus) {
-		if (rsp->ulpStatus == IOSTAT_LOCAL_REJECT) {
-			switch (rsp->un.ulpWord[4] & IOERR_PARAM_MASK) {
-			case IOERR_SEQUENCE_TIMEOUT:
-				rc = -ETIMEDOUT;
-				break;
-			case IOERR_INVALID_RPI:
-				rc = -EFAULT;
-				break;
-			default:
+	if (job) {
+		/* always return the xri, this would be used in the case
+		 * of a menlo download to allow the data to be sent as a
+		 * continuation of the exchange.
+		 */
+
+		menlo_resp = (struct menlo_response *)
+			job->reply->reply_data.vendor_reply.vendor_rsp;
+		menlo_resp->xri = rsp->ulpContext;
+		if (rsp->ulpStatus) {
+			if (rsp->ulpStatus == IOSTAT_LOCAL_REJECT) {
+				switch (rsp->un.ulpWord[4] & IOERR_PARAM_MASK) {
+				case IOERR_SEQUENCE_TIMEOUT:
+					rc = -ETIMEDOUT;
+					break;
+				case IOERR_INVALID_RPI:
+					rc = -EFAULT;
+					break;
+				default:
+					rc = -EACCES;
+					break;
+				}
+			} else {
 				rc = -EACCES;
-				break;
 			}
-		} else
-			rc = -EACCES;
-	} else
-		job->reply->reply_payload_rcv_len =
-			rsp->un.genreq64.bdl.bdeSize;
+		} else {
+			rsp_size = rsp->un.genreq64.bdl.bdeSize;
+			job->reply->reply_payload_rcv_len =
+				lpfc_bsg_copy_data(rmp, &job->reply_payload,
+						   rsp_size, 0);
+		}
+
+	}
 
-	lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
-	lpfc_sli_release_iocbq(phba, rspiocbq);
 	lpfc_sli_release_iocbq(phba, cmdiocbq);
+	lpfc_free_bsg_buffers(phba, cmp);
+	lpfc_free_bsg_buffers(phba, rmp);
+	lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
 	kfree(bmp);
 	kfree(dd_data);
-	/* make error code available to userspace */
-	job->reply->result = rc;
-	/* complete the job back to userspace */
-	job->job_done(job);
-	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+
+	/* Complete the job if active */
+
+	if (job) {
+		job->reply->result = rc;
+		job->job_done(job);
+	}
+
 	return;
 }
 
@@ -4883,17 +4976,14 @@ lpfc_menlo_cmd(struct fc_bsg_job *job)
 {
 	struct lpfc_vport *vport = (struct lpfc_vport *)job->shost->hostdata;
 	struct lpfc_hba *phba = vport->phba;
-	struct lpfc_iocbq *cmdiocbq, *rspiocbq;
-	IOCB_t *cmd, *rsp;
+	struct lpfc_iocbq *cmdiocbq;
+	IOCB_t *cmd;
 	int rc = 0;
 	struct menlo_command *menlo_cmd;
 	struct menlo_response *menlo_resp;
-	struct lpfc_dmabuf *bmp = NULL;
+	struct lpfc_dmabuf *bmp = NULL, *cmp = NULL, *rmp = NULL;
 	int request_nseg;
 	int reply_nseg;
-	struct scatterlist *sgel = NULL;
-	int numbde;
-	dma_addr_t busaddr;
 	struct bsg_job_data *dd_data;
 	struct ulp_bde64 *bpl = NULL;
 
@@ -4948,50 +5038,38 @@ lpfc_menlo_cmd(struct fc_bsg_job *job)
 		goto free_dd;
 	}
 
-	cmdiocbq = lpfc_sli_get_iocbq(phba);
-	if (!cmdiocbq) {
+	bmp->virt = lpfc_mbuf_alloc(phba, 0, &bmp->phys);
+	if (!bmp->virt) {
 		rc = -ENOMEM;
 		goto free_bmp;
 	}
 
-	rspiocbq = lpfc_sli_get_iocbq(phba);
-	if (!rspiocbq) {
-		rc = -ENOMEM;
-		goto free_cmdiocbq;
-	}
-
-	rsp = &rspiocbq->iocb;
+	INIT_LIST_HEAD(&bmp->list);
 
-	bmp->virt = lpfc_mbuf_alloc(phba, 0, &bmp->phys);
-	if (!bmp->virt) {
+	bpl = (struct ulp_bde64 *)bmp->virt;
+	request_nseg = LPFC_BPL_SIZE/sizeof(struct ulp_bde64);
+	cmp = lpfc_alloc_bsg_buffers(phba, job->request_payload.payload_len,
+				     1, bpl, &request_nseg);
+	if (!cmp) {
 		rc = -ENOMEM;
-		goto free_rspiocbq;
+		goto free_bmp;
 	}
+	lpfc_bsg_copy_data(cmp, &job->request_payload,
+			   job->request_payload.payload_len, 1);
 
-	INIT_LIST_HEAD(&bmp->list);
-	bpl = (struct ulp_bde64 *) bmp->virt;
-	request_nseg = pci_map_sg(phba->pcidev, job->request_payload.sg_list,
-				  job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	for_each_sg(job->request_payload.sg_list, sgel, request_nseg, numbde) {
-		busaddr = sg_dma_address(sgel);
-		bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64;
-		bpl->tus.f.bdeSize = sg_dma_len(sgel);
-		bpl->tus.w = cpu_to_le32(bpl->tus.w);
-		bpl->addrLow = cpu_to_le32(putPaddrLow(busaddr));
-		bpl->addrHigh = cpu_to_le32(putPaddrHigh(busaddr));
-		bpl++;
+	bpl += request_nseg;
+	reply_nseg = LPFC_BPL_SIZE/sizeof(struct ulp_bde64) - request_nseg;
+	rmp = lpfc_alloc_bsg_buffers(phba, job->reply_payload.payload_len, 0,
+				     bpl, &reply_nseg);
+	if (!rmp) {
+		rc = -ENOMEM;
+		goto free_cmp;
 	}
 
-	reply_nseg = pci_map_sg(phba->pcidev, job->reply_payload.sg_list,
-				job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
-	for_each_sg(job->reply_payload.sg_list, sgel, reply_nseg, numbde) {
-		busaddr = sg_dma_address(sgel);
-		bpl->tus.f.bdeFlags = BUFF_TYPE_BDE_64I;
-		bpl->tus.f.bdeSize = sg_dma_len(sgel);
-		bpl->tus.w = cpu_to_le32(bpl->tus.w);
-		bpl->addrLow = cpu_to_le32(putPaddrLow(busaddr));
-		bpl->addrHigh = cpu_to_le32(putPaddrHigh(busaddr));
-		bpl++;
+	cmdiocbq = lpfc_sli_get_iocbq(phba);
+	if (!cmdiocbq) {
+		rc = -ENOMEM;
+		goto free_rmp;
 	}
 
 	cmd = &cmdiocbq->iocb;
@@ -5013,11 +5091,10 @@ lpfc_menlo_cmd(struct fc_bsg_job *job)
 	cmdiocbq->vport = phba->pport;
 	/* We want the firmware to timeout before we do */
 	cmd->ulpTimeout = MENLO_TIMEOUT - 5;
-	cmdiocbq->context3 = bmp;
-	cmdiocbq->context2 = rspiocbq;
 	cmdiocbq->iocb_cmpl = lpfc_bsg_menlo_cmd_cmp;
 	cmdiocbq->context1 = dd_data;
-	cmdiocbq->context2 = rspiocbq;
+	cmdiocbq->context2 = cmp;
+	cmdiocbq->context3 = bmp;
 	if (menlo_cmd->cmd == LPFC_BSG_VENDOR_MENLO_CMD) {
 		cmd->ulpCommand = CMD_GEN_REQUEST64_CR;
 		cmd->ulpPU = MENLO_PU; /* 3 */
@@ -5031,29 +5108,25 @@ lpfc_menlo_cmd(struct fc_bsg_job *job)
 	}
 
 	dd_data->type = TYPE_MENLO;
+	dd_data->set_job = job;
 	dd_data->context_un.menlo.cmdiocbq = cmdiocbq;
-	dd_data->context_un.menlo.rspiocbq = rspiocbq;
-	dd_data->context_un.menlo.set_job = job;
-	dd_data->context_un.menlo.bmp = bmp;
+	dd_data->context_un.menlo.rmp = rmp;
+	job->dd_data = dd_data;
 
 	rc = lpfc_sli_issue_iocb(phba, LPFC_ELS_RING, cmdiocbq,
 		MENLO_TIMEOUT - 5);
 	if (rc == IOCB_SUCCESS)
 		return 0; /* done for now */
 
-	/* iocb failed so cleanup */
-	pci_unmap_sg(phba->pcidev, job->request_payload.sg_list,
-		     job->request_payload.sg_cnt, DMA_TO_DEVICE);
-	pci_unmap_sg(phba->pcidev, job->reply_payload.sg_list,
-		     job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
-
-	lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
-
-free_rspiocbq:
-	lpfc_sli_release_iocbq(phba, rspiocbq);
-free_cmdiocbq:
 	lpfc_sli_release_iocbq(phba, cmdiocbq);
+
+free_rmp:
+	lpfc_free_bsg_buffers(phba, rmp);
+free_cmp:
+	lpfc_free_bsg_buffers(phba, cmp);
 free_bmp:
+	if (bmp->virt)
+		lpfc_mbuf_free(phba, bmp->virt, bmp->phys);
 	kfree(bmp);
 free_dd:
 	kfree(dd_data);
@@ -5162,70 +5235,94 @@ lpfc_bsg_timeout(struct fc_bsg_job *job)
 	struct lpfc_vport *vport = (struct lpfc_vport *)job->shost->hostdata;
 	struct lpfc_hba *phba = vport->phba;
 	struct lpfc_iocbq *cmdiocb;
-	struct lpfc_bsg_event *evt;
-	struct lpfc_bsg_iocb *iocb;
-	struct lpfc_bsg_mbox *mbox;
-	struct lpfc_bsg_menlo *menlo;
 	struct lpfc_sli_ring *pring = &phba->sli.ring[LPFC_ELS_RING];
 	struct bsg_job_data *dd_data;
 	unsigned long flags;
+	int rc = 0;
+	LIST_HEAD(completions);
+	struct lpfc_iocbq *check_iocb, *next_iocb;
+
+	/* if job's driver data is NULL, the command completed or is in the
+	 * the process of completing.  In this case, return status to request
+	 * so the timeout is retried.  This avoids double completion issues
+	 * and the request will be pulled off the timer queue when the
+	 * command's completion handler executes.  Otherwise, prevent the
+	 * command's completion handler from executing the job done callback
+	 * and continue processing to abort the outstanding the command.
+	 */
 
 	spin_lock_irqsave(&phba->ct_ev_lock, flags);
 	dd_data = (struct bsg_job_data *)job->dd_data;
-	/* timeout and completion crossed paths if no dd_data */
-	if (!dd_data) {
+	if (dd_data) {
+		dd_data->set_job = NULL;
+		job->dd_data = NULL;
+	} else {
 		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		return 0;
+		return -EAGAIN;
 	}
 
 	switch (dd_data->type) {
 	case TYPE_IOCB:
-		iocb = &dd_data->context_un.iocb;
-		cmdiocb = iocb->cmdiocbq;
-		/* hint to completion handler that the job timed out */
-		job->reply->result = -EAGAIN;
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		/* this will call our completion handler */
+		/* Check to see if IOCB was issued to the port or not. If not,
+		 * remove it from the txq queue and call cancel iocbs.
+		 * Otherwise, call abort iotag
+		 */
+
+		cmdiocb = dd_data->context_un.iocb.cmdiocbq;
 		spin_lock_irq(&phba->hbalock);
-		lpfc_sli_issue_abort_iotag(phba, pring, cmdiocb);
+		list_for_each_entry_safe(check_iocb, next_iocb, &pring->txq,
+					 list) {
+			if (check_iocb == cmdiocb) {
+				list_move_tail(&check_iocb->list, &completions);
+				break;
+			}
+		}
+		if (list_empty(&completions))
+			lpfc_sli_issue_abort_iotag(phba, pring, cmdiocb);
 		spin_unlock_irq(&phba->hbalock);
+		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+		if (!list_empty(&completions)) {
+			lpfc_sli_cancel_iocbs(phba, &completions,
+					      IOSTAT_LOCAL_REJECT,
+					      IOERR_SLI_ABORTED);
+		}
 		break;
+
 	case TYPE_EVT:
-		evt = dd_data->context_un.evt;
-		/* this event has no job anymore */
-		evt->set_job = NULL;
-		job->dd_data = NULL;
-		job->reply->reply_payload_rcv_len = 0;
-		/* Return -EAGAIN which is our way of signallying the
-		 * app to retry.
-		 */
-		job->reply->result = -EAGAIN;
 		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		job->job_done(job);
 		break;
+
 	case TYPE_MBOX:
-		mbox = &dd_data->context_un.mbox;
-		/* this mbox has no job anymore */
-		mbox->set_job = NULL;
-		job->dd_data = NULL;
-		job->reply->reply_payload_rcv_len = 0;
-		job->reply->result = -EAGAIN;
-		/* the mbox completion handler can now be run */
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		job->job_done(job);
+		/* Update the ext buf ctx state if needed */
+
 		if (phba->mbox_ext_buf_ctx.state == LPFC_BSG_MBOX_PORT)
 			phba->mbox_ext_buf_ctx.state = LPFC_BSG_MBOX_ABTS;
+		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
 		break;
 	case TYPE_MENLO:
-		menlo = &dd_data->context_un.menlo;
-		cmdiocb = menlo->cmdiocbq;
-		/* hint to completion handler that the job timed out */
-		job->reply->result = -EAGAIN;
-		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
-		/* this will call our completion handler */
+		/* Check to see if IOCB was issued to the port or not. If not,
+		 * remove it from the txq queue and call cancel iocbs.
+		 * Otherwise, call abort iotag.
+		 */
+
+		cmdiocb = dd_data->context_un.menlo.cmdiocbq;
 		spin_lock_irq(&phba->hbalock);
-		lpfc_sli_issue_abort_iotag(phba, pring, cmdiocb);
+		list_for_each_entry_safe(check_iocb, next_iocb, &pring->txq,
+					 list) {
+			if (check_iocb == cmdiocb) {
+				list_move_tail(&check_iocb->list, &completions);
+				break;
+			}
+		}
+		if (list_empty(&completions))
+			lpfc_sli_issue_abort_iotag(phba, pring, cmdiocb);
 		spin_unlock_irq(&phba->hbalock);
+		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
+		if (!list_empty(&completions)) {
+			lpfc_sli_cancel_iocbs(phba, &completions,
+					      IOSTAT_LOCAL_REJECT,
+					      IOERR_SLI_ABORTED);
+		}
 		break;
 	default:
 		spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
@@ -5236,5 +5333,5 @@ lpfc_bsg_timeout(struct fc_bsg_job *job)
 	 * otherwise an error message will be displayed on the console
 	 * so always return success (zero)
 	 */
-	return 0;
+	return rc;
 }
diff --git a/drivers/scsi/lpfc/lpfc_crtn.h b/drivers/scsi/lpfc/lpfc_crtn.h
index 76ca65dae781..7631893ae005 100644
--- a/drivers/scsi/lpfc/lpfc_crtn.h
+++ b/drivers/scsi/lpfc/lpfc_crtn.h
@@ -106,6 +106,7 @@ void lpfc_cleanup_discovery_resources(struct lpfc_vport *);
 void lpfc_cleanup(struct lpfc_vport *);
 void lpfc_disc_timeout(unsigned long);
 
+int lpfc_unregister_fcf_prep(struct lpfc_hba *);
 struct lpfc_nodelist *__lpfc_findnode_rpi(struct lpfc_vport *, uint16_t);
 struct lpfc_nodelist *lpfc_findnode_rpi(struct lpfc_vport *, uint16_t);
 void lpfc_worker_wake_up(struct lpfc_hba *);
diff --git a/drivers/scsi/lpfc/lpfc_els.c b/drivers/scsi/lpfc/lpfc_els.c
index 08d156a9094f..bbed8471bf0b 100644
--- a/drivers/scsi/lpfc/lpfc_els.c
+++ b/drivers/scsi/lpfc/lpfc_els.c
@@ -484,6 +484,7 @@ lpfc_issue_reg_vfi(struct lpfc_vport *vport)
 	vport->port_state = LPFC_FABRIC_CFG_LINK;
 	memcpy(dmabuf->virt, &phba->fc_fabparam, sizeof(vport->fc_sparam));
 	lpfc_reg_vfi(mboxq, vport, dmabuf->phys);
+
 	mboxq->mbox_cmpl = lpfc_mbx_cmpl_reg_vfi;
 	mboxq->vport = vport;
 	mboxq->context1 = dmabuf;
@@ -700,6 +701,20 @@ lpfc_cmpl_els_flogi_fabric(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 		}
 	}
 
+	/*
+	 * For FC we need to do some special processing because of the SLI
+	 * Port's default settings of the Common Service Parameters.
+	 */
+	if (phba->sli4_hba.lnk_info.lnk_tp == LPFC_LNK_TYPE_FC) {
+		/* If physical FC port changed, unreg VFI and ALL VPIs / RPIs */
+		if ((phba->sli_rev == LPFC_SLI_REV4) && fabric_param_changed)
+			lpfc_unregister_fcf_prep(phba);
+
+		/* This should just update the VFI CSPs*/
+		if (vport->fc_flag & FC_VFI_REGISTERED)
+			lpfc_issue_reg_vfi(vport);
+	}
+
 	if (fabric_param_changed &&
 		!(vport->fc_flag & FC_VPORT_NEEDS_REG_VPI)) {
 
@@ -6225,7 +6240,7 @@ lpfc_els_timeout_handler(struct lpfc_vport *vport)
 		spin_unlock_irq(&phba->hbalock);
 	}
 
-	if (phba->sli.ring[LPFC_ELS_RING].txcmplq_cnt)
+	if (!list_empty(&phba->sli.ring[LPFC_ELS_RING].txcmplq))
 		mod_timer(&vport->els_tmofunc, jiffies + HZ * timeout);
 }
 
@@ -6279,7 +6294,6 @@ lpfc_els_flush_cmd(struct lpfc_vport *vport)
 			continue;
 
 		list_move_tail(&piocb->list, &completions);
-		pring->txq_cnt--;
 	}
 
 	list_for_each_entry_safe(piocb, tmp_iocb, &pring->txcmplq, list) {
@@ -6339,7 +6353,6 @@ lpfc_els_flush_all_cmd(struct lpfc_hba  *phba)
 		    cmd->ulpCommand == CMD_ABORT_XRI_CN)
 			continue;
 		list_move_tail(&piocb->list, &completions);
-		pring->txq_cnt--;
 	}
 	list_for_each_entry_safe(piocb, tmp_iocb, &pring->txcmplq, list) {
 		if (piocb->iocb_flag & LPFC_IO_LIBDFC)
@@ -8065,7 +8078,7 @@ lpfc_sli4_els_xri_aborted(struct lpfc_hba *phba,
 				rxid, 1);
 
 			/* Check if TXQ queue needs to be serviced */
-			if (pring->txq_cnt)
+			if (!(list_empty(&pring->txq)))
 				lpfc_worker_wake_up(phba);
 			return;
 		}
diff --git a/drivers/scsi/lpfc/lpfc_hbadisc.c b/drivers/scsi/lpfc/lpfc_hbadisc.c
index bfda18467ee6..326e05a65a73 100644
--- a/drivers/scsi/lpfc/lpfc_hbadisc.c
+++ b/drivers/scsi/lpfc/lpfc_hbadisc.c
@@ -691,12 +691,15 @@ lpfc_work_done(struct lpfc_hba *phba)
 			/* Set the lpfc data pending flag */
 			set_bit(LPFC_DATA_READY, &phba->data_flags);
 		} else {
-			pring->flag &= ~LPFC_DEFERRED_RING_EVENT;
-			lpfc_sli_handle_slow_ring_event(phba, pring,
-							(status &
-							 HA_RXMASK));
+			if (phba->link_state >= LPFC_LINK_UP) {
+				pring->flag &= ~LPFC_DEFERRED_RING_EVENT;
+				lpfc_sli_handle_slow_ring_event(phba, pring,
+								(status &
+								HA_RXMASK));
+			}
 		}
-		if ((phba->sli_rev == LPFC_SLI_REV4) && pring->txq_cnt)
+		if ((phba->sli_rev == LPFC_SLI_REV4) &
+				 (!list_empty(&pring->txq)))
 			lpfc_drain_txq(phba);
 		/*
 		 * Turn on Ring interrupts
@@ -1792,6 +1795,8 @@ lpfc_sli4_fcf_rec_mbox_parse(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq,
 	virt_addr = mboxq->sge_array->addr[0];
 
 	shdr = (union lpfc_sli4_cfg_shdr *)virt_addr;
+	lpfc_sli_pcimem_bcopy(shdr, shdr,
+			      sizeof(union lpfc_sli4_cfg_shdr));
 	shdr_status = bf_get(lpfc_mbox_hdr_status, &shdr->response);
 	shdr_add_status = bf_get(lpfc_mbox_hdr_add_status, &shdr->response);
 	if (shdr_status || shdr_add_status) {
@@ -2888,6 +2893,11 @@ lpfc_mbx_cmpl_reg_vfi(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 		lpfc_vport_set_state(vport, FC_VPORT_FAILED);
 		goto out_free_mem;
 	}
+
+	/* If the VFI is already registered, there is nothing else to do */
+	if (vport->fc_flag & FC_VFI_REGISTERED)
+		goto out_free_mem;
+
 	/* The VPI is implicitly registered when the VFI is registered */
 	spin_lock_irq(shost->host_lock);
 	vport->vpi_state |= LPFC_VPI_REGISTERED;
@@ -2980,6 +2990,7 @@ lpfc_mbx_process_link_up(struct lpfc_hba *phba, struct lpfc_mbx_read_top *la)
 	struct lpfc_dmabuf *mp;
 	int rc;
 	struct fcf_record *fcf_record;
+	uint32_t fc_flags = 0;
 
 	spin_lock_irq(&phba->hbalock);
 	switch (bf_get(lpfc_mbx_read_top_link_spd, la)) {
@@ -3011,11 +3022,8 @@ lpfc_mbx_process_link_up(struct lpfc_hba *phba, struct lpfc_mbx_read_top *la)
 				"1309 Link Up Event npiv not supported in loop "
 				"topology\n");
 				/* Get Loop Map information */
-		if (bf_get(lpfc_mbx_read_top_il, la)) {
-			spin_lock(shost->host_lock);
-			vport->fc_flag |= FC_LBIT;
-			spin_unlock(shost->host_lock);
-		}
+		if (bf_get(lpfc_mbx_read_top_il, la))
+			fc_flags |= FC_LBIT;
 
 		vport->fc_myDID = bf_get(lpfc_mbx_read_top_alpa_granted, la);
 		i = la->lilpBde64.tus.f.bdeSize;
@@ -3064,12 +3072,16 @@ lpfc_mbx_process_link_up(struct lpfc_hba *phba, struct lpfc_mbx_read_top *la)
 				phba->sli3_options |= LPFC_SLI3_NPIV_ENABLED;
 		}
 		vport->fc_myDID = phba->fc_pref_DID;
-		spin_lock(shost->host_lock);
-		vport->fc_flag |= FC_LBIT;
-		spin_unlock(shost->host_lock);
+		fc_flags |= FC_LBIT;
 	}
 	spin_unlock_irq(&phba->hbalock);
 
+	if (fc_flags) {
+		spin_lock_irq(shost->host_lock);
+		vport->fc_flag |= fc_flags;
+		spin_unlock_irq(shost->host_lock);
+	}
+
 	lpfc_linkup(phba);
 	sparam_mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
 	if (!sparam_mbox)
@@ -3237,8 +3249,7 @@ lpfc_mbx_cmpl_read_topology(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 		vport->fc_flag &= ~FC_BYPASSED_MODE;
 	spin_unlock_irq(shost->host_lock);
 
-	if ((phba->fc_eventTag  < la->eventTag) ||
-	    (phba->fc_eventTag == la->eventTag)) {
+	if (phba->fc_eventTag <= la->eventTag) {
 		phba->fc_stat.LinkMultiEvent++;
 		if (bf_get(lpfc_mbx_read_top_att_type, la) == LPFC_ATT_LINK_UP)
 			if (phba->fc_eventTag != 0)
@@ -3246,16 +3257,18 @@ lpfc_mbx_cmpl_read_topology(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 	}
 
 	phba->fc_eventTag = la->eventTag;
-	spin_lock_irq(&phba->hbalock);
-	if (bf_get(lpfc_mbx_read_top_mm, la))
-		phba->sli.sli_flag |= LPFC_MENLO_MAINT;
-	else
-		phba->sli.sli_flag &= ~LPFC_MENLO_MAINT;
-	spin_unlock_irq(&phba->hbalock);
+	if (phba->sli_rev < LPFC_SLI_REV4) {
+		spin_lock_irq(&phba->hbalock);
+		if (bf_get(lpfc_mbx_read_top_mm, la))
+			phba->sli.sli_flag |= LPFC_MENLO_MAINT;
+		else
+			phba->sli.sli_flag &= ~LPFC_MENLO_MAINT;
+		spin_unlock_irq(&phba->hbalock);
+	}
 
 	phba->link_events++;
 	if ((bf_get(lpfc_mbx_read_top_att_type, la) == LPFC_ATT_LINK_UP) &&
-	    (!bf_get(lpfc_mbx_read_top_mm, la))) {
+	    !(phba->sli.sli_flag & LPFC_MENLO_MAINT)) {
 		phba->fc_stat.LinkUp++;
 		if (phba->link_flag & LS_LOOPBACK_MODE) {
 			lpfc_printf_log(phba, KERN_ERR, LOG_LINK_EVENT,
@@ -3300,8 +3313,8 @@ lpfc_mbx_cmpl_read_topology(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 				bf_get(lpfc_mbx_read_top_fa, la));
 		lpfc_mbx_issue_link_down(phba);
 	}
-	if ((bf_get(lpfc_mbx_read_top_mm, la)) &&
-	    (bf_get(lpfc_mbx_read_top_att_type, la) == LPFC_ATT_LINK_UP)) {
+	if ((phba->sli.sli_flag & LPFC_MENLO_MAINT) &&
+	    ((bf_get(lpfc_mbx_read_top_att_type, la) == LPFC_ATT_LINK_UP))) {
 		if (phba->link_state != LPFC_LINK_DOWN) {
 			phba->fc_stat.LinkDown++;
 			lpfc_printf_log(phba, KERN_ERR, LOG_LINK_EVENT,
@@ -3329,8 +3342,9 @@ lpfc_mbx_cmpl_read_topology(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 		}
 	}
 
-	if (bf_get(lpfc_mbx_read_top_fa, la)) {
-		if (bf_get(lpfc_mbx_read_top_mm, la))
+	if ((phba->sli_rev < LPFC_SLI_REV4) &&
+	    bf_get(lpfc_mbx_read_top_fa, la)) {
+		if (phba->sli.sli_flag & LPFC_MENLO_MAINT)
 			lpfc_issue_clear_la(phba, vport);
 		lpfc_printf_log(phba, KERN_INFO, LOG_LINK_EVENT,
 				"1311 fa %d\n",
@@ -4354,7 +4368,6 @@ lpfc_no_rpi(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp)
 					   with an error */
 					list_move_tail(&iocb->list,
 						       &completions);
-					pring->txq_cnt--;
 				}
 			}
 			spin_unlock_irq(&phba->hbalock);
@@ -5055,7 +5068,6 @@ lpfc_free_tx(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp)
 		    (icmd->ulpCommand == CMD_XMIT_ELS_RSP64_CX)) {
 
 			list_move_tail(&iocb->list, &completions);
-			pring->txq_cnt--;
 		}
 	}
 
diff --git a/drivers/scsi/lpfc/lpfc_hw4.h b/drivers/scsi/lpfc/lpfc_hw4.h
index 6e93b886cd4d..1dd2f6f0a127 100644
--- a/drivers/scsi/lpfc/lpfc_hw4.h
+++ b/drivers/scsi/lpfc/lpfc_hw4.h
@@ -1958,6 +1958,9 @@ struct lpfc_mbx_init_vfi {
 
 struct lpfc_mbx_reg_vfi {
 	uint32_t word1;
+#define lpfc_reg_vfi_upd_SHIFT		29
+#define lpfc_reg_vfi_upd_MASK		0x00000001
+#define lpfc_reg_vfi_upd_WORD		word1
 #define lpfc_reg_vfi_vp_SHIFT		28
 #define lpfc_reg_vfi_vp_MASK		0x00000001
 #define lpfc_reg_vfi_vp_WORD		word1
diff --git a/drivers/scsi/lpfc/lpfc_init.c b/drivers/scsi/lpfc/lpfc_init.c
index 314b4f61b9e3..90b8b0515e23 100644
--- a/drivers/scsi/lpfc/lpfc_init.c
+++ b/drivers/scsi/lpfc/lpfc_init.c
@@ -839,7 +839,6 @@ lpfc_hba_down_post_s3(struct lpfc_hba *phba)
 		 * way, nothing should be on txcmplq as it will NEVER complete.
 		 */
 		list_splice_init(&pring->txcmplq, &completions);
-		pring->txcmplq_cnt = 0;
 		spin_unlock_irq(&phba->hbalock);
 
 		/* Cancel all the IOCBs from the completions list */
@@ -2915,9 +2914,9 @@ lpfc_sli4_xri_sgl_update(struct lpfc_hba *phba)
 			sglq_entry->state = SGL_FREED;
 			list_add_tail(&sglq_entry->list, &els_sgl_list);
 		}
-		spin_lock(&phba->hbalock);
+		spin_lock_irq(&phba->hbalock);
 		list_splice_init(&els_sgl_list, &phba->sli4_hba.lpfc_sgl_list);
-		spin_unlock(&phba->hbalock);
+		spin_unlock_irq(&phba->hbalock);
 	} else if (els_xri_cnt < phba->sli4_hba.els_xri_cnt) {
 		/* els xri-sgl shrinked */
 		xri_cnt = phba->sli4_hba.els_xri_cnt - els_xri_cnt;
@@ -3015,9 +3014,9 @@ lpfc_sli4_xri_sgl_update(struct lpfc_hba *phba)
 		psb->cur_iocbq.sli4_lxritag = lxri;
 		psb->cur_iocbq.sli4_xritag = phba->sli4_hba.xri_ids[lxri];
 	}
-	spin_lock(&phba->scsi_buf_list_lock);
+	spin_lock_irq(&phba->scsi_buf_list_lock);
 	list_splice_init(&scsi_sgl_list, &phba->lpfc_scsi_buf_list);
-	spin_unlock(&phba->scsi_buf_list_lock);
+	spin_unlock_irq(&phba->scsi_buf_list_lock);
 
 	return 0;
 
@@ -4004,6 +4003,52 @@ lpfc_sli4_perform_all_vport_cvl(struct lpfc_hba *phba)
 }
 
 /**
+ * lpfc_sli4_perform_inuse_fcf_recovery - Perform inuse fcf recovery
+ * @vport: pointer to lpfc hba data structure.
+ *
+ * This routine is to perform FCF recovery when the in-use FCF either dead or
+ * got modified.
+ **/
+static void
+lpfc_sli4_perform_inuse_fcf_recovery(struct lpfc_hba *phba,
+				     struct lpfc_acqe_fip *acqe_fip)
+{
+	int rc;
+
+	spin_lock_irq(&phba->hbalock);
+	/* Mark the fast failover process in progress */
+	phba->fcf.fcf_flag |= FCF_DEAD_DISC;
+	spin_unlock_irq(&phba->hbalock);
+
+	lpfc_printf_log(phba, KERN_INFO, LOG_FIP | LOG_DISCOVERY,
+			"2771 Start FCF fast failover process due to in-use "
+			"FCF DEAD/MODIFIED event: evt_tag:x%x, index:x%x\n",
+			acqe_fip->event_tag, acqe_fip->index);
+	rc = lpfc_sli4_redisc_fcf_table(phba);
+	if (rc) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_FIP | LOG_DISCOVERY,
+				"2772 Issue FCF rediscover mabilbox command "
+				"failed, fail through to FCF dead event\n");
+		spin_lock_irq(&phba->hbalock);
+		phba->fcf.fcf_flag &= ~FCF_DEAD_DISC;
+		spin_unlock_irq(&phba->hbalock);
+		/*
+		 * Last resort will fail over by treating this as a link
+		 * down to FCF registration.
+		 */
+		lpfc_sli4_fcf_dead_failthrough(phba);
+	} else {
+		/* Reset FCF roundrobin bmask for new discovery */
+		lpfc_sli4_clear_fcf_rr_bmask(phba);
+		/*
+		 * Handling fast FCF failover to a DEAD FCF event is
+		 * considered equalivant to receiving CVL to all vports.
+		 */
+		lpfc_sli4_perform_all_vport_cvl(phba);
+	}
+}
+
+/**
  * lpfc_sli4_async_fip_evt - Process the asynchronous FCoE FIP event
  * @phba: pointer to lpfc hba data structure.
  * @acqe_link: pointer to the async fcoe completion queue entry.
@@ -4068,9 +4113,22 @@ lpfc_sli4_async_fip_evt(struct lpfc_hba *phba,
 			break;
 		}
 
-		/* If the FCF has been in discovered state, do nothing. */
-		if (phba->fcf.fcf_flag & FCF_SCAN_DONE) {
+		/* If FCF has been in discovered state, perform rediscovery
+		 * only if the FCF with the same index of the in-use FCF got
+		 * modified during normal operation. Otherwise, do nothing.
+		 */
+		if (phba->pport->port_state > LPFC_FLOGI) {
 			spin_unlock_irq(&phba->hbalock);
+			if (phba->fcf.current_rec.fcf_indx ==
+			    acqe_fip->index) {
+				lpfc_printf_log(phba, KERN_ERR, LOG_FIP,
+						"3300 In-use FCF (%d) "
+						"modified, perform FCF "
+						"rediscovery\n",
+						acqe_fip->index);
+				lpfc_sli4_perform_inuse_fcf_recovery(phba,
+								     acqe_fip);
+			}
 			break;
 		}
 		spin_unlock_irq(&phba->hbalock);
@@ -4123,39 +4181,7 @@ lpfc_sli4_async_fip_evt(struct lpfc_hba *phba,
 		 * is no longer valid as we are not in the middle of FCF
 		 * failover process already.
 		 */
-		spin_lock_irq(&phba->hbalock);
-		/* Mark the fast failover process in progress */
-		phba->fcf.fcf_flag |= FCF_DEAD_DISC;
-		spin_unlock_irq(&phba->hbalock);
-
-		lpfc_printf_log(phba, KERN_INFO, LOG_FIP | LOG_DISCOVERY,
-				"2771 Start FCF fast failover process due to "
-				"FCF DEAD event: evt_tag:x%x, fcf_index:x%x "
-				"\n", acqe_fip->event_tag, acqe_fip->index);
-		rc = lpfc_sli4_redisc_fcf_table(phba);
-		if (rc) {
-			lpfc_printf_log(phba, KERN_ERR, LOG_FIP |
-					LOG_DISCOVERY,
-					"2772 Issue FCF rediscover mabilbox "
-					"command failed, fail through to FCF "
-					"dead event\n");
-			spin_lock_irq(&phba->hbalock);
-			phba->fcf.fcf_flag &= ~FCF_DEAD_DISC;
-			spin_unlock_irq(&phba->hbalock);
-			/*
-			 * Last resort will fail over by treating this
-			 * as a link down to FCF registration.
-			 */
-			lpfc_sli4_fcf_dead_failthrough(phba);
-		} else {
-			/* Reset FCF roundrobin bmask for new discovery */
-			lpfc_sli4_clear_fcf_rr_bmask(phba);
-			/*
-			 * Handling fast FCF failover to a DEAD FCF event is
-			 * considered equalivant to receiving CVL to all vports.
-			 */
-			lpfc_sli4_perform_all_vport_cvl(phba);
-		}
+		lpfc_sli4_perform_inuse_fcf_recovery(phba, acqe_fip);
 		break;
 	case LPFC_FIP_EVENT_TYPE_CVL:
 		phba->fcoe_cvl_eventtag = acqe_fip->event_tag;
@@ -10368,36 +10394,6 @@ lpfc_io_resume(struct pci_dev *pdev)
 	return;
 }
 
-/**
- * lpfc_mgmt_open - method called when 'lpfcmgmt' is opened from userspace
- * @inode: pointer to the inode representing the lpfcmgmt device
- * @filep: pointer to the file representing the open lpfcmgmt device
- *
- * This routine puts a reference count on the lpfc module whenever the
- * character device is opened
- **/
-static int
-lpfc_mgmt_open(struct inode *inode, struct file *filep)
-{
-	try_module_get(THIS_MODULE);
-	return 0;
-}
-
-/**
- * lpfc_mgmt_release - method called when 'lpfcmgmt' is closed in userspace
- * @inode: pointer to the inode representing the lpfcmgmt device
- * @filep: pointer to the file representing the open lpfcmgmt device
- *
- * This routine removes a reference count from the lpfc module when the
- * character device is closed
- **/
-static int
-lpfc_mgmt_release(struct inode *inode, struct file *filep)
-{
-	module_put(THIS_MODULE);
-	return 0;
-}
-
 static struct pci_device_id lpfc_id_table[] = {
 	{PCI_VENDOR_ID_EMULEX, PCI_DEVICE_ID_VIPER,
 		PCI_ANY_ID, PCI_ANY_ID, },
@@ -10515,8 +10511,7 @@ static struct pci_driver lpfc_driver = {
 };
 
 static const struct file_operations lpfc_mgmt_fop = {
-	.open = lpfc_mgmt_open,
-	.release = lpfc_mgmt_release,
+	.owner = THIS_MODULE,
 };
 
 static struct miscdevice lpfc_mgmt_dev = {
diff --git a/drivers/scsi/lpfc/lpfc_mbox.c b/drivers/scsi/lpfc/lpfc_mbox.c
index efc9cd9def8b..a7a9fa468308 100644
--- a/drivers/scsi/lpfc/lpfc_mbox.c
+++ b/drivers/scsi/lpfc/lpfc_mbox.c
@@ -2126,32 +2126,40 @@ void
 lpfc_reg_vfi(struct lpfcMboxq *mbox, struct lpfc_vport *vport, dma_addr_t phys)
 {
 	struct lpfc_mbx_reg_vfi *reg_vfi;
+	struct lpfc_hba *phba = vport->phba;
 
 	memset(mbox, 0, sizeof(*mbox));
 	reg_vfi = &mbox->u.mqe.un.reg_vfi;
 	bf_set(lpfc_mqe_command, &mbox->u.mqe, MBX_REG_VFI);
 	bf_set(lpfc_reg_vfi_vp, reg_vfi, 1);
 	bf_set(lpfc_reg_vfi_vfi, reg_vfi,
-	       vport->phba->sli4_hba.vfi_ids[vport->vfi]);
-	bf_set(lpfc_reg_vfi_fcfi, reg_vfi, vport->phba->fcf.fcfi);
-	bf_set(lpfc_reg_vfi_vpi, reg_vfi, vport->phba->vpi_ids[vport->vpi]);
+	       phba->sli4_hba.vfi_ids[vport->vfi]);
+	bf_set(lpfc_reg_vfi_fcfi, reg_vfi, phba->fcf.fcfi);
+	bf_set(lpfc_reg_vfi_vpi, reg_vfi, phba->vpi_ids[vport->vpi]);
 	memcpy(reg_vfi->wwn, &vport->fc_portname, sizeof(struct lpfc_name));
 	reg_vfi->wwn[0] = cpu_to_le32(reg_vfi->wwn[0]);
 	reg_vfi->wwn[1] = cpu_to_le32(reg_vfi->wwn[1]);
-	reg_vfi->e_d_tov = vport->phba->fc_edtov;
-	reg_vfi->r_a_tov = vport->phba->fc_ratov;
+	reg_vfi->e_d_tov = phba->fc_edtov;
+	reg_vfi->r_a_tov = phba->fc_ratov;
 	reg_vfi->bde.addrHigh = putPaddrHigh(phys);
 	reg_vfi->bde.addrLow = putPaddrLow(phys);
 	reg_vfi->bde.tus.f.bdeSize = sizeof(vport->fc_sparam);
 	reg_vfi->bde.tus.f.bdeFlags = BUFF_TYPE_BDE_64;
 	bf_set(lpfc_reg_vfi_nport_id, reg_vfi, vport->fc_myDID);
+
+	/* Only FC supports upd bit */
+	if ((phba->sli4_hba.lnk_info.lnk_tp == LPFC_LNK_TYPE_FC) &&
+	    (vport->fc_flag & FC_VFI_REGISTERED)) {
+		bf_set(lpfc_reg_vfi_vp, reg_vfi, 0);
+		bf_set(lpfc_reg_vfi_upd, reg_vfi, 1);
+	}
 	lpfc_printf_vlog(vport, KERN_INFO, LOG_MBOX,
 			"3134 Register VFI, mydid:x%x, fcfi:%d, "
 			" vfi:%d, vpi:%d, fc_pname:%x%x\n",
 			vport->fc_myDID,
-			vport->phba->fcf.fcfi,
-			vport->phba->sli4_hba.vfi_ids[vport->vfi],
-			vport->phba->vpi_ids[vport->vpi],
+			phba->fcf.fcfi,
+			phba->sli4_hba.vfi_ids[vport->vfi],
+			phba->vpi_ids[vport->vpi],
 			reg_vfi->wwn[0], reg_vfi->wwn[1]);
 }
 
diff --git a/drivers/scsi/lpfc/lpfc_nportdisc.c b/drivers/scsi/lpfc/lpfc_nportdisc.c
index 46128c679202..82f4d3542289 100644
--- a/drivers/scsi/lpfc/lpfc_nportdisc.c
+++ b/drivers/scsi/lpfc/lpfc_nportdisc.c
@@ -226,7 +226,6 @@ lpfc_els_abort(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp)
 		if (lpfc_check_sli_ndlp(phba, pring, iocb, ndlp)) {
 			/* It matches, so deque and call compl with anp error */
 			list_move_tail(&iocb->list, &completions);
-			pring->txq_cnt--;
 		}
 	}
 
diff --git a/drivers/scsi/lpfc/lpfc_scsi.c b/drivers/scsi/lpfc/lpfc_scsi.c
index 98af07c6e300..74b8710e1e90 100644
--- a/drivers/scsi/lpfc/lpfc_scsi.c
+++ b/drivers/scsi/lpfc/lpfc_scsi.c
@@ -732,7 +732,7 @@ lpfc_sli4_fcp_xri_aborted(struct lpfc_hba *phba,
 		psb = container_of(iocbq, struct lpfc_scsi_buf, cur_iocbq);
 		psb->exch_busy = 0;
 		spin_unlock_irqrestore(&phba->hbalock, iflag);
-		if (pring->txq_cnt)
+		if (!list_empty(&pring->txq))
 			lpfc_worker_wake_up(phba);
 		return;
 
@@ -885,9 +885,9 @@ lpfc_sli4_repost_scsi_sgl_list(struct lpfc_hba *phba)
 	int num_posted, rc = 0;
 
 	/* get all SCSI buffers need to repost to a local list */
-	spin_lock(&phba->scsi_buf_list_lock);
+	spin_lock_irq(&phba->scsi_buf_list_lock);
 	list_splice_init(&phba->lpfc_scsi_buf_list, &post_sblist);
-	spin_unlock(&phba->scsi_buf_list_lock);
+	spin_unlock_irq(&phba->scsi_buf_list_lock);
 
 	/* post the list of scsi buffer sgls to port if available */
 	if (!list_empty(&post_sblist)) {
@@ -4246,7 +4246,7 @@ static __inline__ void lpfc_poll_rearm_timer(struct lpfc_hba * phba)
 	unsigned long  poll_tmo_expires =
 		(jiffies + msecs_to_jiffies(phba->cfg_poll_tmo));
 
-	if (phba->sli.ring[LPFC_FCP_RING].txcmplq_cnt)
+	if (!list_empty(&phba->sli.ring[LPFC_FCP_RING].txcmplq))
 		mod_timer(&phba->fcp_poll_timer,
 			  poll_tmo_expires);
 }
diff --git a/drivers/scsi/lpfc/lpfc_sli.c b/drivers/scsi/lpfc/lpfc_sli.c
index d43faf34c1e2..35dd17eb0f27 100644
--- a/drivers/scsi/lpfc/lpfc_sli.c
+++ b/drivers/scsi/lpfc/lpfc_sli.c
@@ -873,14 +873,16 @@ lpfc_set_rrq_active(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp,
 				xritag, rxid, ndlp->nlp_DID, send_rrq);
 		return -EINVAL;
 	}
-	rrq->send_rrq = send_rrq;
+	if (phba->cfg_enable_rrq == 1)
+		rrq->send_rrq = send_rrq;
+	else
+		rrq->send_rrq = 0;
 	rrq->xritag = xritag;
 	rrq->rrq_stop_time = jiffies + HZ * (phba->fc_ratov + 1);
 	rrq->ndlp = ndlp;
 	rrq->nlp_DID = ndlp->nlp_DID;
 	rrq->vport = ndlp->vport;
 	rrq->rxid = rxid;
-	rrq->send_rrq = send_rrq;
 	spin_lock_irqsave(&phba->hbalock, iflags);
 	empty = list_empty(&phba->active_rrq_list);
 	list_add_tail(&rrq->list, &phba->active_rrq_list);
@@ -1009,6 +1011,18 @@ __lpfc_sli_release_iocbq_s4(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq)
 	else
 		sglq = __lpfc_clear_active_sglq(phba, iocbq->sli4_lxritag);
 
+	/*
+	** This should have been removed from the txcmplq before calling
+	** iocbq_release. The normal completion
+	** path should have already done the list_del_init.
+	*/
+	if (unlikely(!list_empty(&iocbq->list))) {
+		if (iocbq->iocb_flag & LPFC_IO_ON_TXCMPLQ)
+			iocbq->iocb_flag &= ~LPFC_IO_ON_TXCMPLQ;
+		list_del_init(&iocbq->list);
+	}
+
+
 	if (sglq)  {
 		if ((iocbq->iocb_flag & LPFC_EXCHANGE_BUSY) &&
 			(sglq->state != SGL_XRI_ABORTED)) {
@@ -1025,7 +1039,7 @@ __lpfc_sli_release_iocbq_s4(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq)
 				&phba->sli4_hba.lpfc_sgl_list);
 
 			/* Check if TXQ queue needs to be serviced */
-			if (pring->txq_cnt)
+			if (!list_empty(&pring->txq))
 				lpfc_worker_wake_up(phba);
 		}
 	}
@@ -1057,6 +1071,14 @@ __lpfc_sli_release_iocbq_s3(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq)
 	size_t start_clean = offsetof(struct lpfc_iocbq, iocb);
 
 	/*
+	** This should have been removed from the txcmplq before calling
+	** iocbq_release. The normal completion
+	** path should have already done the list_del_init.
+	*/
+	if (unlikely(!list_empty(&iocbq->list)))
+		list_del_init(&iocbq->list);
+
+	/*
 	 * Clean all volatile data fields, preserve iotag and node struct.
 	 */
 	memset((char*)iocbq + start_clean, 0, sizeof(*iocbq) - start_clean);
@@ -1122,7 +1144,6 @@ lpfc_sli_cancel_iocbs(struct lpfc_hba *phba, struct list_head *iocblist,
 
 	while (!list_empty(iocblist)) {
 		list_remove_head(iocblist, piocb, struct lpfc_iocbq, list);
-
 		if (!piocb->iocb_cmpl)
 			lpfc_sli_release_iocbq(phba, piocb);
 		else {
@@ -1310,9 +1331,6 @@ lpfc_sli_ringtxcmpl_put(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 {
 	list_add_tail(&piocb->list, &pring->txcmplq);
 	piocb->iocb_flag |= LPFC_IO_ON_TXCMPLQ;
-	pring->txcmplq_cnt++;
-	if (pring->txcmplq_cnt > pring->txcmplq_max)
-		pring->txcmplq_max = pring->txcmplq_cnt;
 
 	if ((unlikely(pring->ringno == LPFC_ELS_RING)) &&
 	   (piocb->iocb.ulpCommand != CMD_ABORT_XRI_CN) &&
@@ -1344,8 +1362,6 @@ lpfc_sli_ringtx_get(struct lpfc_hba *phba, struct lpfc_sli_ring *pring)
 	struct lpfc_iocbq *cmd_iocb;
 
 	list_remove_head((&pring->txq), cmd_iocb, struct lpfc_iocbq, list);
-	if (cmd_iocb != NULL)
-		pring->txq_cnt--;
 	return cmd_iocb;
 }
 
@@ -1614,8 +1630,9 @@ lpfc_sli_resume_iocb(struct lpfc_hba *phba, struct lpfc_sli_ring *pring)
 	 *  (c) link attention events can be processed (fcp ring only)
 	 *  (d) IOCB processing is not blocked by the outstanding mbox command.
 	 */
-	if (pring->txq_cnt &&
-	    lpfc_is_link_up(phba) &&
+
+	if (lpfc_is_link_up(phba) &&
+	    (!list_empty(&pring->txq)) &&
 	    (pring->ringno != phba->sli.fcp_ring ||
 	     phba->sli.sli_flag & LPFC_PROCESS_LA)) {
 
@@ -2612,7 +2629,6 @@ lpfc_sli_iocbq_lookup(struct lpfc_hba *phba,
 		cmd_iocb = phba->sli.iocbq_lookup[iotag];
 		list_del_init(&cmd_iocb->list);
 		if (cmd_iocb->iocb_flag & LPFC_IO_ON_TXCMPLQ) {
-			pring->txcmplq_cnt--;
 			cmd_iocb->iocb_flag &= ~LPFC_IO_ON_TXCMPLQ;
 		}
 		return cmd_iocb;
@@ -2650,7 +2666,6 @@ lpfc_sli_iocbq_lookup_by_tag(struct lpfc_hba *phba,
 			/* remove from txcmpl queue list */
 			list_del_init(&cmd_iocb->list);
 			cmd_iocb->iocb_flag &= ~LPFC_IO_ON_TXCMPLQ;
-			pring->txcmplq_cnt--;
 			return cmd_iocb;
 		}
 	}
@@ -3499,7 +3514,6 @@ lpfc_sli_abort_iocb_ring(struct lpfc_hba *phba, struct lpfc_sli_ring *pring)
 	 */
 	spin_lock_irq(&phba->hbalock);
 	list_splice_init(&pring->txq, &completions);
-	pring->txq_cnt = 0;
 
 	/* Next issue ABTS for everything on the txcmplq */
 	list_for_each_entry_safe(iocb, next_iocb, &pring->txcmplq, list)
@@ -3536,11 +3550,9 @@ lpfc_sli_flush_fcp_rings(struct lpfc_hba *phba)
 	spin_lock_irq(&phba->hbalock);
 	/* Retrieve everything on txq */
 	list_splice_init(&pring->txq, &txq);
-	pring->txq_cnt = 0;
 
 	/* Retrieve everything on the txcmplq */
 	list_splice_init(&pring->txcmplq, &txcmplq);
-	pring->txcmplq_cnt = 0;
 
 	/* Indicate the I/O queues are flushed */
 	phba->hba_flag |= HBA_FCP_IOQ_FLUSH;
@@ -5988,9 +6000,9 @@ lpfc_sli4_repost_els_sgl_list(struct lpfc_hba *phba)
 	LIST_HEAD(post_sgl_list);
 	LIST_HEAD(free_sgl_list);
 
-	spin_lock(&phba->hbalock);
+	spin_lock_irq(&phba->hbalock);
 	list_splice_init(&phba->sli4_hba.lpfc_sgl_list, &allc_sgl_list);
-	spin_unlock(&phba->hbalock);
+	spin_unlock_irq(&phba->hbalock);
 
 	list_for_each_entry_safe(sglq_entry, sglq_entry_next,
 				 &allc_sgl_list, list) {
@@ -6091,10 +6103,10 @@ lpfc_sli4_repost_els_sgl_list(struct lpfc_hba *phba)
 
 	/* push els sgls posted to the availble list */
 	if (!list_empty(&post_sgl_list)) {
-		spin_lock(&phba->hbalock);
+		spin_lock_irq(&phba->hbalock);
 		list_splice_init(&post_sgl_list,
 				 &phba->sli4_hba.lpfc_sgl_list);
-		spin_unlock(&phba->hbalock);
+		spin_unlock_irq(&phba->hbalock);
 	} else {
 		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 				"3161 Failure to post els sgl to port.\n");
@@ -7615,7 +7627,6 @@ __lpfc_sli_ringtx_put(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 {
 	/* Insert the caller's iocb in the txq tail for later processing. */
 	list_add_tail(&piocb->list, &pring->txq);
-	pring->txq_cnt++;
 }
 
 /**
@@ -8387,7 +8398,7 @@ __lpfc_sli_issue_iocb_s4(struct lpfc_hba *phba, uint32_t ring_number,
 		    piocb->iocb.ulpCommand == CMD_CLOSE_XRI_CN)
 			sglq = NULL;
 		else {
-			if (pring->txq_cnt) {
+			if (!list_empty(&pring->txq)) {
 				if (!(flag & SLI_IOCB_RET_IOCB)) {
 					__lpfc_sli_ringtx_put(phba,
 						pring, piocb);
@@ -9055,7 +9066,6 @@ lpfc_sli_host_down(struct lpfc_vport *vport)
 			if (iocb->vport != vport)
 				continue;
 			list_move_tail(&iocb->list, &completions);
-			pring->txq_cnt--;
 		}
 
 		/* Next issue ABTS for everything on the txcmplq */
@@ -9124,8 +9134,6 @@ lpfc_sli_hba_down(struct lpfc_hba *phba)
 		 * given to the FW yet.
 		 */
 		list_splice_init(&pring->txq, &completions);
-		pring->txq_cnt = 0;
-
 	}
 	spin_unlock_irqrestore(&phba->hbalock, flags);
 
@@ -9966,6 +9974,9 @@ lpfc_sli_issue_iocb_wait(struct lpfc_hba *phba,
 	long timeleft, timeout_req = 0;
 	int retval = IOCB_SUCCESS;
 	uint32_t creg_val;
+	struct lpfc_iocbq *iocb;
+	int txq_cnt = 0;
+	int txcmplq_cnt = 0;
 	struct lpfc_sli_ring *pring = &phba->sli.ring[LPFC_ELS_RING];
 	/*
 	 * If the caller has provided a response iocbq buffer, then context2
@@ -10013,9 +10024,17 @@ lpfc_sli_issue_iocb_wait(struct lpfc_hba *phba,
 			retval = IOCB_TIMEDOUT;
 		}
 	} else if (retval == IOCB_BUSY) {
-		lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
-			"2818 Max IOCBs %d txq cnt %d txcmplq cnt %d\n",
-			phba->iocb_cnt, pring->txq_cnt, pring->txcmplq_cnt);
+		if (phba->cfg_log_verbose & LOG_SLI) {
+			list_for_each_entry(iocb, &pring->txq, list) {
+				txq_cnt++;
+			}
+			list_for_each_entry(iocb, &pring->txcmplq, list) {
+				txcmplq_cnt++;
+			}
+			lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
+				"2818 Max IOCBs %d txq cnt %d txcmplq cnt %d\n",
+				phba->iocb_cnt, txq_cnt, txcmplq_cnt);
+		}
 		return retval;
 	} else {
 		lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
@@ -11298,16 +11317,25 @@ lpfc_sli4_sp_handle_els_wcqe(struct lpfc_hba *phba, struct lpfc_queue *cq,
 	struct lpfc_iocbq *irspiocbq;
 	unsigned long iflags;
 	struct lpfc_sli_ring *pring = cq->pring;
+	int txq_cnt = 0;
+	int txcmplq_cnt = 0;
+	int fcp_txcmplq_cnt = 0;
 
 	/* Get an irspiocbq for later ELS response processing use */
 	irspiocbq = lpfc_sli_get_iocbq(phba);
 	if (!irspiocbq) {
+		if (!list_empty(&pring->txq))
+			txq_cnt++;
+		if (!list_empty(&pring->txcmplq))
+			txcmplq_cnt++;
+		if (!list_empty(&phba->sli.ring[LPFC_FCP_RING].txcmplq))
+			fcp_txcmplq_cnt++;
 		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 			"0387 NO IOCBQ data: txq_cnt=%d iocb_cnt=%d "
 			"fcp_txcmplq_cnt=%d, els_txcmplq_cnt=%d\n",
-			pring->txq_cnt, phba->iocb_cnt,
-			phba->sli.ring[LPFC_FCP_RING].txcmplq_cnt,
-			phba->sli.ring[LPFC_ELS_RING].txcmplq_cnt);
+			txq_cnt, phba->iocb_cnt,
+			fcp_txcmplq_cnt,
+			txcmplq_cnt);
 		return false;
 	}
 
@@ -15482,11 +15510,18 @@ lpfc_check_next_fcf_pri_level(struct lpfc_hba *phba)
 			LPFC_SLI4_FCF_TBL_INDX_MAX);
 	lpfc_printf_log(phba, KERN_INFO, LOG_FIP,
 			"3060 Last IDX %d\n", last_index);
-	if (list_empty(&phba->fcf.fcf_pri_list)) {
+
+	/* Verify the priority list has 2 or more entries */
+	spin_lock_irq(&phba->hbalock);
+	if (list_empty(&phba->fcf.fcf_pri_list) ||
+	    list_is_singular(&phba->fcf.fcf_pri_list)) {
+		spin_unlock_irq(&phba->hbalock);
 		lpfc_printf_log(phba, KERN_ERR, LOG_FIP,
 			"3061 Last IDX %d\n", last_index);
 		return 0; /* Empty rr list */
 	}
+	spin_unlock_irq(&phba->hbalock);
+
 	next_fcf_pri = 0;
 	/*
 	 * Clear the rr_bmask and set all of the bits that are at this
@@ -16245,14 +16280,19 @@ lpfc_drain_txq(struct lpfc_hba *phba)
 	char *fail_msg = NULL;
 	struct lpfc_sglq *sglq;
 	union lpfc_wqe wqe;
+	int txq_cnt = 0;
 
 	spin_lock_irqsave(&phba->hbalock, iflags);
-	if (pring->txq_cnt > pring->txq_max)
-		pring->txq_max = pring->txq_cnt;
+	list_for_each_entry(piocbq, &pring->txq, list) {
+		txq_cnt++;
+	}
+
+	if (txq_cnt > pring->txq_max)
+		pring->txq_max = txq_cnt;
 
 	spin_unlock_irqrestore(&phba->hbalock, iflags);
 
-	while (pring->txq_cnt) {
+	while (!list_empty(&pring->txq)) {
 		spin_lock_irqsave(&phba->hbalock, iflags);
 
 		piocbq = lpfc_sli_ringtx_get(phba, pring);
@@ -16260,7 +16300,7 @@ lpfc_drain_txq(struct lpfc_hba *phba)
 			spin_unlock_irqrestore(&phba->hbalock, iflags);
 			lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 				"2823 txq empty and txq_cnt is %d\n ",
-				pring->txq_cnt);
+				txq_cnt);
 			break;
 		}
 		sglq = __lpfc_sli_get_sglq(phba, piocbq);
@@ -16269,6 +16309,7 @@ lpfc_drain_txq(struct lpfc_hba *phba)
 			spin_unlock_irqrestore(&phba->hbalock, iflags);
 			break;
 		}
+		txq_cnt--;
 
 		/* The xri and iocb resources secured,
 		 * attempt to issue request
@@ -16300,5 +16341,5 @@ lpfc_drain_txq(struct lpfc_hba *phba)
 	lpfc_sli_cancel_iocbs(phba, &completions, IOSTAT_LOCAL_REJECT,
 				IOERR_SLI_ABORTED);
 
-	return pring->txq_cnt;
+	return txq_cnt;
 }
diff --git a/drivers/scsi/lpfc/lpfc_version.h b/drivers/scsi/lpfc/lpfc_version.h
index f3b7795a296b..664cd04f7cd8 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.37"
+#define LPFC_DRIVER_VERSION "8.3.38"
 #define LPFC_DRIVER_NAME		"lpfc"
 
 /* Used for SLI 2/3 */
diff --git a/drivers/scsi/mac_scsi.c b/drivers/scsi/mac_scsi.c
index 24828b54773a..858075723c87 100644
--- a/drivers/scsi/mac_scsi.c
+++ b/drivers/scsi/mac_scsi.c
@@ -561,7 +561,8 @@ static int macscsi_pwrite (struct Scsi_Host *instance,
 
 static struct scsi_host_template driver_template = {
 	.proc_name			= "Mac5380",
-	.proc_info			= macscsi_proc_info,
+	.show_info			= macscsi_show_info,
+	.write_info			= macscsi_write_info,
 	.name				= "Macintosh NCR5380 SCSI",
 	.detect				= macscsi_detect,
 	.release			= macscsi_release,
diff --git a/drivers/scsi/mac_scsi.h b/drivers/scsi/mac_scsi.h
index d26e331c6c12..7dc62fce1c4c 100644
--- a/drivers/scsi/mac_scsi.h
+++ b/drivers/scsi/mac_scsi.h
@@ -72,7 +72,8 @@
 #define NCR5380_queue_command macscsi_queue_command
 #define NCR5380_abort macscsi_abort
 #define NCR5380_bus_reset macscsi_bus_reset
-#define NCR5380_proc_info macscsi_proc_info
+#define NCR5380_show_info macscsi_show_info
+#define NCR5380_write_info macscsi_write_info
 
 #define BOARD_NORMAL	0
 #define BOARD_NCR53C400	1
diff --git a/drivers/scsi/megaraid.c b/drivers/scsi/megaraid.c
index 9504ec0ec682..846f475f62c1 100644
--- a/drivers/scsi/megaraid.c
+++ b/drivers/scsi/megaraid.c
@@ -39,6 +39,7 @@
 #include <linux/completion.h>
 #include <linux/delay.h>
 #include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 #include <linux/reboot.h>
 #include <linux/module.h>
 #include <linux/list.h>
@@ -2069,385 +2070,201 @@ mega_free_inquiry(void *inquiry, dma_addr_t dma_handle, struct pci_dev *pdev)
 #ifdef CONFIG_PROC_FS
 /* Following code handles /proc fs  */
 
-#define CREATE_READ_PROC(string, func)	create_proc_read_entry(string,	\
-					S_IRUSR | S_IFREG,		\
-					controller_proc_dir_entry,	\
-					func, adapter)
-
-/**
- * mega_create_proc_entry()
- * @index - index in soft state array
- * @parent - parent node for this /proc entry
- *
- * Creates /proc entries for our controllers.
- */
-static void
-mega_create_proc_entry(int index, struct proc_dir_entry *parent)
-{
-	struct proc_dir_entry	*controller_proc_dir_entry = NULL;
-	u8		string[64] = { 0 };
-	adapter_t	*adapter = hba_soft_state[index];
-
-	sprintf(string, "hba%d", adapter->host->host_no);
-
-	controller_proc_dir_entry =
-		adapter->controller_proc_dir_entry = proc_mkdir(string, parent);
-
-	if(!controller_proc_dir_entry) {
-		printk(KERN_WARNING "\nmegaraid: proc_mkdir failed\n");
-		return;
-	}
-	adapter->proc_read = CREATE_READ_PROC("config", proc_read_config);
-	adapter->proc_stat = CREATE_READ_PROC("stat", proc_read_stat);
-	adapter->proc_mbox = CREATE_READ_PROC("mailbox", proc_read_mbox);
-#if MEGA_HAVE_ENH_PROC
-	adapter->proc_rr = CREATE_READ_PROC("rebuild-rate", proc_rebuild_rate);
-	adapter->proc_battery = CREATE_READ_PROC("battery-status",
-			proc_battery);
-
-	/*
-	 * Display each physical drive on its channel
-	 */
-	adapter->proc_pdrvstat[0] = CREATE_READ_PROC("diskdrives-ch0",
-					proc_pdrv_ch0);
-	adapter->proc_pdrvstat[1] = CREATE_READ_PROC("diskdrives-ch1",
-					proc_pdrv_ch1);
-	adapter->proc_pdrvstat[2] = CREATE_READ_PROC("diskdrives-ch2",
-					proc_pdrv_ch2);
-	adapter->proc_pdrvstat[3] = CREATE_READ_PROC("diskdrives-ch3",
-					proc_pdrv_ch3);
-
-	/*
-	 * Display a set of up to 10 logical drive through each of following
-	 * /proc entries
-	 */
-	adapter->proc_rdrvstat[0] = CREATE_READ_PROC("raiddrives-0-9",
-					proc_rdrv_10);
-	adapter->proc_rdrvstat[1] = CREATE_READ_PROC("raiddrives-10-19",
-					proc_rdrv_20);
-	adapter->proc_rdrvstat[2] = CREATE_READ_PROC("raiddrives-20-29",
-					proc_rdrv_30);
-	adapter->proc_rdrvstat[3] = CREATE_READ_PROC("raiddrives-30-39",
-					proc_rdrv_40);
-#endif
-}
-
-
 /**
- * proc_read_config()
- * @page - buffer to write the data in
- * @start - where the actual data has been written in page
- * @offset - same meaning as the read system call
- * @count - same meaning as the read system call
- * @eof - set if no more data needs to be returned
- * @data - pointer to our soft state
+ * proc_show_config()
+ * @m - Synthetic file construction data
+ * @v - File iterator
  *
  * Display configuration information about the controller.
  */
 static int
-proc_read_config(char *page, char **start, off_t offset, int count, int *eof,
-		void *data)
+proc_show_config(struct seq_file *m, void *v)
 {
 
-	adapter_t *adapter = (adapter_t *)data;
-	int len = 0;
-
-	len += sprintf(page+len, "%s", MEGARAID_VERSION);
+	adapter_t *adapter = m->private;
 
+	seq_puts(m, MEGARAID_VERSION);
 	if(adapter->product_info.product_name[0])
-		len += sprintf(page+len, "%s\n",
-				adapter->product_info.product_name);
-
-	len += sprintf(page+len, "Controller Type: ");
+		seq_printf(m, "%s\n", adapter->product_info.product_name);
 
-	if( adapter->flag & BOARD_MEMMAP ) {
-		len += sprintf(page+len,
-			"438/466/467/471/493/518/520/531/532\n");
-	}
-	else {
-		len += sprintf(page+len,
-			"418/428/434\n");
-	}
+	seq_puts(m, "Controller Type: ");
 
-	if(adapter->flag & BOARD_40LD) {
-		len += sprintf(page+len,
-				"Controller Supports 40 Logical Drives\n");
-	}
+	if( adapter->flag & BOARD_MEMMAP )
+		seq_puts(m, "438/466/467/471/493/518/520/531/532\n");
+	else
+		seq_puts(m, "418/428/434\n");
 
-	if(adapter->flag & BOARD_64BIT) {
-		len += sprintf(page+len,
-		"Controller capable of 64-bit memory addressing\n");
-	}
-	if( adapter->has_64bit_addr ) {
-		len += sprintf(page+len,
-			"Controller using 64-bit memory addressing\n");
-	}
-	else {
-		len += sprintf(page+len,
-			"Controller is not using 64-bit memory addressing\n");
-	}
+	if(adapter->flag & BOARD_40LD)
+		seq_puts(m, "Controller Supports 40 Logical Drives\n");
 
-	len += sprintf(page+len, "Base = %08lx, Irq = %d, ", adapter->base,
-			adapter->host->irq);
-
-	len += sprintf(page+len, "Logical Drives = %d, Channels = %d\n",
-			adapter->numldrv, adapter->product_info.nchannels);
-
-	len += sprintf(page+len, "Version =%s:%s, DRAM = %dMb\n",
-			adapter->fw_version, adapter->bios_version,
-			adapter->product_info.dram_size);
-
-	len += sprintf(page+len,
-		"Controller Queue Depth = %d, Driver Queue Depth = %d\n",
-		adapter->product_info.max_commands, adapter->max_cmds);
-
-	len += sprintf(page+len, "support_ext_cdb    = %d\n",
-			adapter->support_ext_cdb);
-	len += sprintf(page+len, "support_random_del = %d\n",
-			adapter->support_random_del);
-	len += sprintf(page+len, "boot_ldrv_enabled  = %d\n",
-			adapter->boot_ldrv_enabled);
-	len += sprintf(page+len, "boot_ldrv          = %d\n",
-			adapter->boot_ldrv);
-	len += sprintf(page+len, "boot_pdrv_enabled  = %d\n",
-			adapter->boot_pdrv_enabled);
-	len += sprintf(page+len, "boot_pdrv_ch       = %d\n",
-			adapter->boot_pdrv_ch);
-	len += sprintf(page+len, "boot_pdrv_tgt      = %d\n",
-			adapter->boot_pdrv_tgt);
-	len += sprintf(page+len, "quiescent          = %d\n",
-			atomic_read(&adapter->quiescent));
-	len += sprintf(page+len, "has_cluster        = %d\n",
-			adapter->has_cluster);
-
-	len += sprintf(page+len, "\nModule Parameters:\n");
-	len += sprintf(page+len, "max_cmd_per_lun    = %d\n",
-			max_cmd_per_lun);
-	len += sprintf(page+len, "max_sectors_per_io = %d\n",
-			max_sectors_per_io);
-
-	*eof = 1;
-
-	return len;
+	if(adapter->flag & BOARD_64BIT)
+		seq_puts(m, "Controller capable of 64-bit memory addressing\n");
+	if( adapter->has_64bit_addr )
+		seq_puts(m, "Controller using 64-bit memory addressing\n");
+	else
+		seq_puts(m, "Controller is not using 64-bit memory addressing\n");
+
+	seq_printf(m, "Base = %08lx, Irq = %d, ",
+		   adapter->base, adapter->host->irq);
+
+	seq_printf(m, "Logical Drives = %d, Channels = %d\n",
+		   adapter->numldrv, adapter->product_info.nchannels);
+
+	seq_printf(m, "Version =%s:%s, DRAM = %dMb\n",
+		   adapter->fw_version, adapter->bios_version,
+		   adapter->product_info.dram_size);
+
+	seq_printf(m, "Controller Queue Depth = %d, Driver Queue Depth = %d\n",
+		   adapter->product_info.max_commands, adapter->max_cmds);
+
+	seq_printf(m, "support_ext_cdb    = %d\n", adapter->support_ext_cdb);
+	seq_printf(m, "support_random_del = %d\n", adapter->support_random_del);
+	seq_printf(m, "boot_ldrv_enabled  = %d\n", adapter->boot_ldrv_enabled);
+	seq_printf(m, "boot_ldrv          = %d\n", adapter->boot_ldrv);
+	seq_printf(m, "boot_pdrv_enabled  = %d\n", adapter->boot_pdrv_enabled);
+	seq_printf(m, "boot_pdrv_ch       = %d\n", adapter->boot_pdrv_ch);
+	seq_printf(m, "boot_pdrv_tgt      = %d\n", adapter->boot_pdrv_tgt);
+	seq_printf(m, "quiescent          = %d\n",
+		   atomic_read(&adapter->quiescent));
+	seq_printf(m, "has_cluster        = %d\n", adapter->has_cluster);
+
+	seq_puts(m, "\nModule Parameters:\n");
+	seq_printf(m, "max_cmd_per_lun    = %d\n", max_cmd_per_lun);
+	seq_printf(m, "max_sectors_per_io = %d\n", max_sectors_per_io);
+	return 0;
 }
 
-
-
 /**
- * proc_read_stat()
- * @page - buffer to write the data in
- * @start - where the actual data has been written in page
- * @offset - same meaning as the read system call
- * @count - same meaning as the read system call
- * @eof - set if no more data needs to be returned
- * @data - pointer to our soft state
+ * proc_show_stat()
+ * @m - Synthetic file construction data
+ * @v - File iterator
  *
- * Diaplay statistical information about the I/O activity.
+ * Display statistical information about the I/O activity.
  */
 static int
-proc_read_stat(char *page, char **start, off_t offset, int count, int *eof,
-		void *data)
+proc_show_stat(struct seq_file *m, void *v)
 {
-	adapter_t	*adapter;
-	int	len;
+	adapter_t *adapter = m->private;
+#if MEGA_HAVE_STATS
 	int	i;
+#endif
 
-	i = 0;	/* avoid compilation warnings */
-	len = 0;
-	adapter = (adapter_t *)data;
-
-	len = sprintf(page, "Statistical Information for this controller\n");
-	len += sprintf(page+len, "pend_cmds = %d\n",
-			atomic_read(&adapter->pend_cmds));
+	seq_puts(m, "Statistical Information for this controller\n");
+	seq_printf(m, "pend_cmds = %d\n", atomic_read(&adapter->pend_cmds));
 #if MEGA_HAVE_STATS
 	for(i = 0; i < adapter->numldrv; i++) {
-		len += sprintf(page+len, "Logical Drive %d:\n", i);
-
-		len += sprintf(page+len,
-			"\tReads Issued = %lu, Writes Issued = %lu\n",
-			adapter->nreads[i], adapter->nwrites[i]);
-
-		len += sprintf(page+len,
-			"\tSectors Read = %lu, Sectors Written = %lu\n",
-			adapter->nreadblocks[i], adapter->nwriteblocks[i]);
-
-		len += sprintf(page+len,
-			"\tRead errors = %lu, Write errors = %lu\n\n",
-			adapter->rd_errors[i], adapter->wr_errors[i]);
+		seq_printf(m, "Logical Drive %d:\n", i);
+		seq_printf(m, "\tReads Issued = %lu, Writes Issued = %lu\n",
+			   adapter->nreads[i], adapter->nwrites[i]);
+		seq_printf(m, "\tSectors Read = %lu, Sectors Written = %lu\n",
+			   adapter->nreadblocks[i], adapter->nwriteblocks[i]);
+		seq_printf(m, "\tRead errors = %lu, Write errors = %lu\n\n",
+			   adapter->rd_errors[i], adapter->wr_errors[i]);
 	}
 #else
-	len += sprintf(page+len,
-			"IO and error counters not compiled in driver.\n");
+	seq_puts(m, "IO and error counters not compiled in driver.\n");
 #endif
-
-	*eof = 1;
-
-	return len;
+	return 0;
 }
 
 
 /**
- * proc_read_mbox()
- * @page - buffer to write the data in
- * @start - where the actual data has been written in page
- * @offset - same meaning as the read system call
- * @count - same meaning as the read system call
- * @eof - set if no more data needs to be returned
- * @data - pointer to our soft state
+ * proc_show_mbox()
+ * @m - Synthetic file construction data
+ * @v - File iterator
  *
  * Display mailbox information for the last command issued. This information
  * is good for debugging.
  */
 static int
-proc_read_mbox(char *page, char **start, off_t offset, int count, int *eof,
-		void *data)
+proc_show_mbox(struct seq_file *m, void *v)
 {
-
-	adapter_t	*adapter = (adapter_t *)data;
+	adapter_t	*adapter = m->private;
 	volatile mbox_t	*mbox = adapter->mbox;
-	int	len = 0;
-
-	len = sprintf(page, "Contents of Mail Box Structure\n");
-	len += sprintf(page+len, "  Fw Command   = 0x%02x\n", 
-			mbox->m_out.cmd);
-	len += sprintf(page+len, "  Cmd Sequence = 0x%02x\n", 
-			mbox->m_out.cmdid);
-	len += sprintf(page+len, "  No of Sectors= %04d\n", 
-			mbox->m_out.numsectors);
-	len += sprintf(page+len, "  LBA          = 0x%02x\n", 
-			mbox->m_out.lba);
-	len += sprintf(page+len, "  DTA          = 0x%08x\n", 
-			mbox->m_out.xferaddr);
-	len += sprintf(page+len, "  Logical Drive= 0x%02x\n", 
-			mbox->m_out.logdrv);
-	len += sprintf(page+len, "  No of SG Elmt= 0x%02x\n",
-			mbox->m_out.numsgelements);
-	len += sprintf(page+len, "  Busy         = %01x\n", 
-			mbox->m_in.busy);
-	len += sprintf(page+len, "  Status       = 0x%02x\n", 
-			mbox->m_in.status);
-
-	*eof = 1;
-
-	return len;
+
+	seq_puts(m, "Contents of Mail Box Structure\n");
+	seq_printf(m, "  Fw Command   = 0x%02x\n", mbox->m_out.cmd);
+	seq_printf(m, "  Cmd Sequence = 0x%02x\n", mbox->m_out.cmdid);
+	seq_printf(m, "  No of Sectors= %04d\n", mbox->m_out.numsectors);
+	seq_printf(m, "  LBA          = 0x%02x\n", mbox->m_out.lba);
+	seq_printf(m, "  DTA          = 0x%08x\n", mbox->m_out.xferaddr);
+	seq_printf(m, "  Logical Drive= 0x%02x\n", mbox->m_out.logdrv);
+	seq_printf(m, "  No of SG Elmt= 0x%02x\n", mbox->m_out.numsgelements);
+	seq_printf(m, "  Busy         = %01x\n", mbox->m_in.busy);
+	seq_printf(m, "  Status       = 0x%02x\n", mbox->m_in.status);
+	return 0;
 }
 
 
 /**
- * proc_rebuild_rate()
- * @page - buffer to write the data in
- * @start - where the actual data has been written in page
- * @offset - same meaning as the read system call
- * @count - same meaning as the read system call
- * @eof - set if no more data needs to be returned
- * @data - pointer to our soft state
+ * proc_show_rebuild_rate()
+ * @m - Synthetic file construction data
+ * @v - File iterator
  *
  * Display current rebuild rate
  */
 static int
-proc_rebuild_rate(char *page, char **start, off_t offset, int count, int *eof,
-		void *data)
+proc_show_rebuild_rate(struct seq_file *m, void *v)
 {
-	adapter_t	*adapter = (adapter_t *)data;
+	adapter_t	*adapter = m->private;
 	dma_addr_t	dma_handle;
 	caddr_t		inquiry;
 	struct pci_dev	*pdev;
-	int	len = 0;
 
-	if( make_local_pdev(adapter, &pdev) != 0 ) {
-		*eof = 1;
-		return len;
-	}
+	if( make_local_pdev(adapter, &pdev) != 0 )
+		return 0;
 
-	if( (inquiry = mega_allocate_inquiry(&dma_handle, pdev)) == NULL ) {
-		free_local_pdev(pdev);
-		*eof = 1;
-		return len;
-	}
+	if( (inquiry = mega_allocate_inquiry(&dma_handle, pdev)) == NULL )
+		goto free_pdev;
 
 	if( mega_adapinq(adapter, dma_handle) != 0 ) {
-
-		len = sprintf(page, "Adapter inquiry failed.\n");
-
+		seq_puts(m, "Adapter inquiry failed.\n");
 		printk(KERN_WARNING "megaraid: inquiry failed.\n");
-
-		mega_free_inquiry(inquiry, dma_handle, pdev);
-
-		free_local_pdev(pdev);
-
-		*eof = 1;
-
-		return len;
+		goto free_inquiry;
 	}
 
-	if( adapter->flag & BOARD_40LD ) {
-		len = sprintf(page, "Rebuild Rate: [%d%%]\n",
-			((mega_inquiry3 *)inquiry)->rebuild_rate);
-	}
-	else {
-		len = sprintf(page, "Rebuild Rate: [%d%%]\n",
+	if( adapter->flag & BOARD_40LD )
+		seq_printf(m, "Rebuild Rate: [%d%%]\n",
+			   ((mega_inquiry3 *)inquiry)->rebuild_rate);
+	else
+		seq_printf(m, "Rebuild Rate: [%d%%]\n",
 			((mraid_ext_inquiry *)
-			inquiry)->raid_inq.adapter_info.rebuild_rate);
-	}
-
+			 inquiry)->raid_inq.adapter_info.rebuild_rate);
 
+free_inquiry:
 	mega_free_inquiry(inquiry, dma_handle, pdev);
-
+free_pdev:
 	free_local_pdev(pdev);
-
-	*eof = 1;
-
-	return len;
+	return 0;
 }
 
 
 /**
- * proc_battery()
- * @page - buffer to write the data in
- * @start - where the actual data has been written in page
- * @offset - same meaning as the read system call
- * @count - same meaning as the read system call
- * @eof - set if no more data needs to be returned
- * @data - pointer to our soft state
+ * proc_show_battery()
+ * @m - Synthetic file construction data
+ * @v - File iterator
  *
  * Display information about the battery module on the controller.
  */
 static int
-proc_battery(char *page, char **start, off_t offset, int count, int *eof,
-		void *data)
+proc_show_battery(struct seq_file *m, void *v)
 {
-	adapter_t	*adapter = (adapter_t *)data;
+	adapter_t	*adapter = m->private;
 	dma_addr_t	dma_handle;
 	caddr_t		inquiry;
 	struct pci_dev	*pdev;
-	u8	battery_status = 0;
-	char	str[256];
-	int	len = 0;
+	u8	battery_status;
 
-	if( make_local_pdev(adapter, &pdev) != 0 ) {
-		*eof = 1;
-		return len;
-	}
+	if( make_local_pdev(adapter, &pdev) != 0 )
+		return 0;
 
-	if( (inquiry = mega_allocate_inquiry(&dma_handle, pdev)) == NULL ) {
-		free_local_pdev(pdev);
-		*eof = 1;
-		return len;
-	}
+	if( (inquiry = mega_allocate_inquiry(&dma_handle, pdev)) == NULL )
+		goto free_pdev;
 
 	if( mega_adapinq(adapter, dma_handle) != 0 ) {
-
-		len = sprintf(page, "Adapter inquiry failed.\n");
-
+		seq_printf(m, "Adapter inquiry failed.\n");
 		printk(KERN_WARNING "megaraid: inquiry failed.\n");
-
-		mega_free_inquiry(inquiry, dma_handle, pdev);
-
-		free_local_pdev(pdev);
-
-		*eof = 1;
-
-		return len;
+		goto free_inquiry;
 	}
 
 	if( adapter->flag & BOARD_40LD ) {
@@ -2461,146 +2278,80 @@ proc_battery(char *page, char **start, off_t offset, int count, int *eof,
 	/*
 	 * Decode the battery status
 	 */
-	sprintf(str, "Battery Status:[%d]", battery_status);
+	seq_printf(m, "Battery Status:[%d]", battery_status);
 
 	if(battery_status == MEGA_BATT_CHARGE_DONE)
-		strcat(str, " Charge Done");
+		seq_puts(m, " Charge Done");
 
 	if(battery_status & MEGA_BATT_MODULE_MISSING)
-		strcat(str, " Module Missing");
+		seq_puts(m, " Module Missing");
 	
 	if(battery_status & MEGA_BATT_LOW_VOLTAGE)
-		strcat(str, " Low Voltage");
+		seq_puts(m, " Low Voltage");
 	
 	if(battery_status & MEGA_BATT_TEMP_HIGH)
-		strcat(str, " Temperature High");
+		seq_puts(m, " Temperature High");
 	
 	if(battery_status & MEGA_BATT_PACK_MISSING)
-		strcat(str, " Pack Missing");
+		seq_puts(m, " Pack Missing");
 	
 	if(battery_status & MEGA_BATT_CHARGE_INPROG)
-		strcat(str, " Charge In-progress");
+		seq_puts(m, " Charge In-progress");
 	
 	if(battery_status & MEGA_BATT_CHARGE_FAIL)
-		strcat(str, " Charge Fail");
+		seq_puts(m, " Charge Fail");
 	
 	if(battery_status & MEGA_BATT_CYCLES_EXCEEDED)
-		strcat(str, " Cycles Exceeded");
-
-	len = sprintf(page, "%s\n", str);
+		seq_puts(m, " Cycles Exceeded");
 
+	seq_putc(m, '\n');
 
+free_inquiry:
 	mega_free_inquiry(inquiry, dma_handle, pdev);
-
+free_pdev:
 	free_local_pdev(pdev);
-
-	*eof = 1;
-
-	return len;
-}
-
-
-/**
- * proc_pdrv_ch0()
- * @page - buffer to write the data in
- * @start - where the actual data has been written in page
- * @offset - same meaning as the read system call
- * @count - same meaning as the read system call
- * @eof - set if no more data needs to be returned
- * @data - pointer to our soft state
- *
- * Display information about the physical drives on physical channel 0.
- */
-static int
-proc_pdrv_ch0(char *page, char **start, off_t offset, int count, int *eof,
-		void *data)
-{
-	adapter_t *adapter = (adapter_t *)data;
-
-	*eof = 1;
-
-	return (proc_pdrv(adapter, page, 0));
-}
-
-
-/**
- * proc_pdrv_ch1()
- * @page - buffer to write the data in
- * @start - where the actual data has been written in page
- * @offset - same meaning as the read system call
- * @count - same meaning as the read system call
- * @eof - set if no more data needs to be returned
- * @data - pointer to our soft state
- *
- * Display information about the physical drives on physical channel 1.
- */
-static int
-proc_pdrv_ch1(char *page, char **start, off_t offset, int count, int *eof,
-		void *data)
-{
-	adapter_t *adapter = (adapter_t *)data;
-
-	*eof = 1;
-
-	return (proc_pdrv(adapter, page, 1));
+	return 0;
 }
 
 
-/**
- * proc_pdrv_ch2()
- * @page - buffer to write the data in
- * @start - where the actual data has been written in page
- * @offset - same meaning as the read system call
- * @count - same meaning as the read system call
- * @eof - set if no more data needs to be returned
- * @data - pointer to our soft state
- *
- * Display information about the physical drives on physical channel 2.
+/*
+ * Display scsi inquiry
  */
-static int
-proc_pdrv_ch2(char *page, char **start, off_t offset, int count, int *eof,
-		void *data)
+static void
+mega_print_inquiry(struct seq_file *m, char *scsi_inq)
 {
-	adapter_t *adapter = (adapter_t *)data;
-
-	*eof = 1;
-
-	return (proc_pdrv(adapter, page, 2));
-}
+	int	i;
 
+	seq_puts(m, "  Vendor: ");
+	seq_write(m, scsi_inq + 8, 8);
+	seq_puts(m, "  Model: ");
+	seq_write(m, scsi_inq + 16, 16);
+	seq_puts(m, "  Rev: ");
+	seq_write(m, scsi_inq + 32, 4);
+	seq_putc(m, '\n');
 
-/**
- * proc_pdrv_ch3()
- * @page - buffer to write the data in
- * @start - where the actual data has been written in page
- * @offset - same meaning as the read system call
- * @count - same meaning as the read system call
- * @eof - set if no more data needs to be returned
- * @data - pointer to our soft state
- *
- * Display information about the physical drives on physical channel 3.
- */
-static int
-proc_pdrv_ch3(char *page, char **start, off_t offset, int count, int *eof,
-		void *data)
-{
-	adapter_t *adapter = (adapter_t *)data;
+	i = scsi_inq[0] & 0x1f;
+	seq_printf(m, "  Type:   %s ", scsi_device_type(i));
 
-	*eof = 1;
+	seq_printf(m, "                 ANSI SCSI revision: %02x",
+		   scsi_inq[2] & 0x07);
 
-	return (proc_pdrv(adapter, page, 3));
+	if( (scsi_inq[2] & 0x07) == 1 && (scsi_inq[3] & 0x0f) == 1 )
+		seq_puts(m, " CCS\n");
+	else
+		seq_putc(m, '\n');
 }
 
-
 /**
- * proc_pdrv()
+ * proc_show_pdrv()
+ * @m - Synthetic file construction data
  * @page - buffer to write the data in
  * @adapter - pointer to our soft state
  *
  * Display information about the physical drives.
  */
 static int
-proc_pdrv(adapter_t *adapter, char *page, int channel)
+proc_show_pdrv(struct seq_file *m, adapter_t *adapter, int channel)
 {
 	dma_addr_t	dma_handle;
 	char		*scsi_inq;
@@ -2611,32 +2362,24 @@ proc_pdrv(adapter_t *adapter, char *page, int channel)
 	u8	state;
 	int	tgt;
 	int	max_channels;
-	int	len = 0;
-	char	str[80];
 	int	i;
 
-	if( make_local_pdev(adapter, &pdev) != 0 ) {
-		return len;
-	}
+	if( make_local_pdev(adapter, &pdev) != 0 )
+		return 0;
 
-	if( (inquiry = mega_allocate_inquiry(&dma_handle, pdev)) == NULL ) {
+	if( (inquiry = mega_allocate_inquiry(&dma_handle, pdev)) == NULL )
 		goto free_pdev;
-	}
 
 	if( mega_adapinq(adapter, dma_handle) != 0 ) {
-		len = sprintf(page, "Adapter inquiry failed.\n");
-
+		seq_puts(m, "Adapter inquiry failed.\n");
 		printk(KERN_WARNING "megaraid: inquiry failed.\n");
-
 		goto free_inquiry;
 	}
 
 
 	scsi_inq = pci_alloc_consistent(pdev, 256, &scsi_inq_dma_handle);
-
 	if( scsi_inq == NULL ) {
-		len = sprintf(page, "memory not available for scsi inq.\n");
-
+		seq_puts(m, "memory not available for scsi inq.\n");
 		goto free_inquiry;
 	}
 
@@ -2659,39 +2402,31 @@ proc_pdrv(adapter_t *adapter, char *page, int channel)
 		i = channel*16 + tgt;
 
 		state = *(pdrv_state + i);
-
 		switch( state & 0x0F ) {
-
 		case PDRV_ONLINE:
-			sprintf(str,
-			"Channel:%2d Id:%2d State: Online",
-				channel, tgt);
+			seq_printf(m, "Channel:%2d Id:%2d State: Online",
+				   channel, tgt);
 			break;
 
 		case PDRV_FAILED:
-			sprintf(str,
-			"Channel:%2d Id:%2d State: Failed",
-				channel, tgt);
+			seq_printf(m, "Channel:%2d Id:%2d State: Failed",
+				   channel, tgt);
 			break;
 
 		case PDRV_RBLD:
-			sprintf(str,
-			"Channel:%2d Id:%2d State: Rebuild",
-				channel, tgt);
+			seq_printf(m, "Channel:%2d Id:%2d State: Rebuild",
+				   channel, tgt);
 			break;
 
 		case PDRV_HOTSPARE:
-			sprintf(str,
-			"Channel:%2d Id:%2d State: Hot spare",
-				channel, tgt);
+			seq_printf(m, "Channel:%2d Id:%2d State: Hot spare",
+				   channel, tgt);
 			break;
 
 		default:
-			sprintf(str,
-			"Channel:%2d Id:%2d State: Un-configured",
-				channel, tgt);
+			seq_printf(m, "Channel:%2d Id:%2d State: Un-configured",
+				   channel, tgt);
 			break;
-
 		}
 
 		/*
@@ -2710,11 +2445,8 @@ proc_pdrv(adapter_t *adapter, char *page, int channel)
 		 * Check for overflow. We print less than 240
 		 * characters for inquiry
 		 */
-		if( (len + 240) >= PAGE_SIZE ) break;
-
-		len += sprintf(page+len, "%s.\n", str);
-
-		len += mega_print_inquiry(page+len, scsi_inq);
+		seq_puts(m, ".\n");
+		mega_print_inquiry(m, scsi_inq);
 	}
 
 free_pci:
@@ -2723,150 +2455,68 @@ free_inquiry:
 	mega_free_inquiry(inquiry, dma_handle, pdev);
 free_pdev:
 	free_local_pdev(pdev);
-
-	return len;
-}
-
-
-/*
- * Display scsi inquiry
- */
-static int
-mega_print_inquiry(char *page, char *scsi_inq)
-{
-	int	len = 0;
-	int	i;
-
-	len = sprintf(page, "  Vendor: ");
-	for( i = 8; i < 16; i++ ) {
-		len += sprintf(page+len, "%c", scsi_inq[i]);
-	}
-
-	len += sprintf(page+len, "  Model: ");
-
-	for( i = 16; i < 32; i++ ) {
-		len += sprintf(page+len, "%c", scsi_inq[i]);
-	}
-
-	len += sprintf(page+len, "  Rev: ");
-
-	for( i = 32; i < 36; i++ ) {
-		len += sprintf(page+len, "%c", scsi_inq[i]);
-	}
-
-	len += sprintf(page+len, "\n");
-
-	i = scsi_inq[0] & 0x1f;
-
-	len += sprintf(page+len, "  Type:   %s ", scsi_device_type(i));
-
-	len += sprintf(page+len,
-	"                 ANSI SCSI revision: %02x", scsi_inq[2] & 0x07);
-
-	if( (scsi_inq[2] & 0x07) == 1 && (scsi_inq[3] & 0x0f) == 1 )
-		len += sprintf(page+len, " CCS\n");
-	else
-		len += sprintf(page+len, "\n");
-
-	return len;
+	return 0;
 }
 
-
 /**
- * proc_rdrv_10()
- * @page - buffer to write the data in
- * @start - where the actual data has been written in page
- * @offset - same meaning as the read system call
- * @count - same meaning as the read system call
- * @eof - set if no more data needs to be returned
- * @data - pointer to our soft state
+ * proc_show_pdrv_ch0()
+ * @m - Synthetic file construction data
+ * @v - File iterator
  *
- * Display real time information about the logical drives 0 through 9.
+ * Display information about the physical drives on physical channel 0.
  */
 static int
-proc_rdrv_10(char *page, char **start, off_t offset, int count, int *eof,
-		void *data)
+proc_show_pdrv_ch0(struct seq_file *m, void *v)
 {
-	adapter_t *adapter = (adapter_t *)data;
-
-	*eof = 1;
-
-	return (proc_rdrv(adapter, page, 0, 9));
+	return proc_show_pdrv(m, m->private, 0);
 }
 
 
 /**
- * proc_rdrv_20()
- * @page - buffer to write the data in
- * @start - where the actual data has been written in page
- * @offset - same meaning as the read system call
- * @count - same meaning as the read system call
- * @eof - set if no more data needs to be returned
- * @data - pointer to our soft state
+ * proc_show_pdrv_ch1()
+ * @m - Synthetic file construction data
+ * @v - File iterator
  *
- * Display real time information about the logical drives 0 through 9.
+ * Display information about the physical drives on physical channel 1.
  */
 static int
-proc_rdrv_20(char *page, char **start, off_t offset, int count, int *eof,
-		void *data)
+proc_show_pdrv_ch1(struct seq_file *m, void *v)
 {
-	adapter_t *adapter = (adapter_t *)data;
-
-	*eof = 1;
-
-	return (proc_rdrv(adapter, page, 10, 19));
+	return proc_show_pdrv(m, m->private, 1);
 }
 
 
 /**
- * proc_rdrv_30()
- * @page - buffer to write the data in
- * @start - where the actual data has been written in page
- * @offset - same meaning as the read system call
- * @count - same meaning as the read system call
- * @eof - set if no more data needs to be returned
- * @data - pointer to our soft state
+ * proc_show_pdrv_ch2()
+ * @m - Synthetic file construction data
+ * @v - File iterator
  *
- * Display real time information about the logical drives 0 through 9.
+ * Display information about the physical drives on physical channel 2.
  */
 static int
-proc_rdrv_30(char *page, char **start, off_t offset, int count, int *eof,
-		void *data)
+proc_show_pdrv_ch2(struct seq_file *m, void *v)
 {
-	adapter_t *adapter = (adapter_t *)data;
-
-	*eof = 1;
-
-	return (proc_rdrv(adapter, page, 20, 29));
+	return proc_show_pdrv(m, m->private, 2);
 }
 
 
 /**
- * proc_rdrv_40()
- * @page - buffer to write the data in
- * @start - where the actual data has been written in page
- * @offset - same meaning as the read system call
- * @count - same meaning as the read system call
- * @eof - set if no more data needs to be returned
- * @data - pointer to our soft state
+ * proc_show_pdrv_ch3()
+ * @m - Synthetic file construction data
+ * @v - File iterator
  *
- * Display real time information about the logical drives 0 through 9.
+ * Display information about the physical drives on physical channel 3.
  */
 static int
-proc_rdrv_40(char *page, char **start, off_t offset, int count, int *eof,
-		void *data)
+proc_show_pdrv_ch3(struct seq_file *m, void *v)
 {
-	adapter_t *adapter = (adapter_t *)data;
-
-	*eof = 1;
-
-	return (proc_rdrv(adapter, page, 30, 39));
+	return proc_show_pdrv(m, m->private, 3);
 }
 
 
 /**
- * proc_rdrv()
- * @page - buffer to write the data in
+ * proc_show_rdrv()
+ * @m - Synthetic file construction data
  * @adapter - pointer to our soft state
  * @start - starting logical drive to display
  * @end - ending logical drive to display
@@ -2875,7 +2525,7 @@ proc_rdrv_40(char *page, char **start, off_t offset, int count, int *eof,
  * /proc/scsi/scsi interface
  */
 static int
-proc_rdrv(adapter_t *adapter, char *page, int start, int end )
+proc_show_rdrv(struct seq_file *m, adapter_t *adapter, int start, int end )
 {
 	dma_addr_t	dma_handle;
 	logdrv_param	*lparam;
@@ -2887,29 +2537,18 @@ proc_rdrv(adapter_t *adapter, char *page, int start, int end )
 	u8	*rdrv_state;
 	int	num_ldrv;
 	u32	array_sz;
-	int	len = 0;
 	int	i;
 
-	if( make_local_pdev(adapter, &pdev) != 0 ) {
-		return len;
-	}
+	if( make_local_pdev(adapter, &pdev) != 0 )
+		return 0;
 
-	if( (inquiry = mega_allocate_inquiry(&dma_handle, pdev)) == NULL ) {
-		free_local_pdev(pdev);
-		return len;
-	}
+	if( (inquiry = mega_allocate_inquiry(&dma_handle, pdev)) == NULL )
+		goto free_pdev;
 
 	if( mega_adapinq(adapter, dma_handle) != 0 ) {
-
-		len = sprintf(page, "Adapter inquiry failed.\n");
-
+		seq_puts(m, "Adapter inquiry failed.\n");
 		printk(KERN_WARNING "megaraid: inquiry failed.\n");
-
-		mega_free_inquiry(inquiry, dma_handle, pdev);
-
-		free_local_pdev(pdev);
-
-		return len;
+		goto free_inquiry;
 	}
 
 	memset(&mc, 0, sizeof(megacmd_t));
@@ -2935,13 +2574,8 @@ proc_rdrv(adapter_t *adapter, char *page, int start, int end )
 			&disk_array_dma_handle);
 
 	if( disk_array == NULL ) {
-		len = sprintf(page, "memory not available.\n");
-
-		mega_free_inquiry(inquiry, dma_handle, pdev);
-
-		free_local_pdev(pdev);
-
-		return len;
+		seq_puts(m, "memory not available.\n");
+		goto free_inquiry;
 	}
 
 	mc.xferaddr = (u32)disk_array_dma_handle;
@@ -2951,17 +2585,8 @@ proc_rdrv(adapter_t *adapter, char *page, int start, int end )
 		mc.opcode = OP_DCMD_READ_CONFIG;
 
 		if( mega_internal_command(adapter, &mc, NULL) ) {
-
-			len = sprintf(page, "40LD read config failed.\n");
-
-			mega_free_inquiry(inquiry, dma_handle, pdev);
-
-			pci_free_consistent(pdev, array_sz, disk_array,
-					disk_array_dma_handle);
-
-			free_local_pdev(pdev);
-
-			return len;
+			seq_puts(m, "40LD read config failed.\n");
+			goto free_pci;
 		}
 
 	}
@@ -2969,24 +2594,10 @@ proc_rdrv(adapter_t *adapter, char *page, int start, int end )
 		mc.cmd = NEW_READ_CONFIG_8LD;
 
 		if( mega_internal_command(adapter, &mc, NULL) ) {
-
 			mc.cmd = READ_CONFIG_8LD;
-
-			if( mega_internal_command(adapter, &mc,
-						NULL) ){
-
-				len = sprintf(page,
-					"8LD read config failed.\n");
-
-				mega_free_inquiry(inquiry, dma_handle, pdev);
-
-				pci_free_consistent(pdev, array_sz,
-						disk_array,
-						disk_array_dma_handle);
-
-				free_local_pdev(pdev);
-
-				return len;
+			if( mega_internal_command(adapter, &mc, NULL) ) {
+				seq_puts(m, "8LD read config failed.\n");
+				goto free_pci;
 			}
 		}
 	}
@@ -3006,29 +2617,23 @@ proc_rdrv(adapter_t *adapter, char *page, int start, int end )
 		 * Check for overflow. We print less than 240 characters for
 		 * information about each logical drive.
 		 */
-		if( (len + 240) >= PAGE_SIZE ) break;
-
-		len += sprintf(page+len, "Logical drive:%2d:, ", i);
+		seq_printf(m, "Logical drive:%2d:, ", i);
 
 		switch( rdrv_state[i] & 0x0F ) {
 		case RDRV_OFFLINE:
-			len += sprintf(page+len, "state: offline");
+			seq_puts(m, "state: offline");
 			break;
-
 		case RDRV_DEGRADED:
-			len += sprintf(page+len, "state: degraded");
+			seq_puts(m, "state: degraded");
 			break;
-
 		case RDRV_OPTIMAL:
-			len += sprintf(page+len, "state: optimal");
+			seq_puts(m, "state: optimal");
 			break;
-
 		case RDRV_DELETED:
-			len += sprintf(page+len, "state: deleted");
+			seq_puts(m, "state: deleted");
 			break;
-
 		default:
-			len += sprintf(page+len, "state: unknown");
+			seq_puts(m, "state: unknown");
 			break;
 		}
 
@@ -3036,84 +2641,203 @@ proc_rdrv(adapter_t *adapter, char *page, int start, int end )
 		 * Check if check consistency or initialization is going on
 		 * for this logical drive.
 		 */
-		if( (rdrv_state[i] & 0xF0) == 0x20 ) {
-			len += sprintf(page+len,
-					", check-consistency in progress");
-		}
-		else if( (rdrv_state[i] & 0xF0) == 0x10 ) {
-			len += sprintf(page+len,
-					", initialization in progress");
-		}
+		if( (rdrv_state[i] & 0xF0) == 0x20 )
+			seq_puts(m, ", check-consistency in progress");
+		else if( (rdrv_state[i] & 0xF0) == 0x10 )
+			seq_puts(m, ", initialization in progress");
 		
-		len += sprintf(page+len, "\n");
-
-		len += sprintf(page+len, "Span depth:%3d, ",
-				lparam->span_depth);
-
-		len += sprintf(page+len, "RAID level:%3d, ",
-				lparam->level);
-
-		len += sprintf(page+len, "Stripe size:%3d, ",
-				lparam->stripe_sz ? lparam->stripe_sz/2: 128);
-
-		len += sprintf(page+len, "Row size:%3d\n",
-				lparam->row_size);
+		seq_putc(m, '\n');
 
+		seq_printf(m, "Span depth:%3d, ", lparam->span_depth);
+		seq_printf(m, "RAID level:%3d, ", lparam->level);
+		seq_printf(m, "Stripe size:%3d, ",
+			   lparam->stripe_sz ? lparam->stripe_sz/2: 128);
+		seq_printf(m, "Row size:%3d\n", lparam->row_size);
 
-		len += sprintf(page+len, "Read Policy: ");
-
+		seq_puts(m, "Read Policy: ");
 		switch(lparam->read_ahead) {
-
 		case NO_READ_AHEAD:
-			len += sprintf(page+len, "No read ahead, ");
+			seq_puts(m, "No read ahead, ");
 			break;
-
 		case READ_AHEAD:
-			len += sprintf(page+len, "Read ahead, ");
+			seq_puts(m, "Read ahead, ");
 			break;
-
 		case ADAP_READ_AHEAD:
-			len += sprintf(page+len, "Adaptive, ");
+			seq_puts(m, "Adaptive, ");
 			break;
 
 		}
 
-		len += sprintf(page+len, "Write Policy: ");
-
+		seq_puts(m, "Write Policy: ");
 		switch(lparam->write_mode) {
-
 		case WRMODE_WRITE_THRU:
-			len += sprintf(page+len, "Write thru, ");
+			seq_puts(m, "Write thru, ");
 			break;
-
 		case WRMODE_WRITE_BACK:
-			len += sprintf(page+len, "Write back, ");
+			seq_puts(m, "Write back, ");
 			break;
 		}
 
-		len += sprintf(page+len, "Cache Policy: ");
-
+		seq_puts(m, "Cache Policy: ");
 		switch(lparam->direct_io) {
-
 		case CACHED_IO:
-			len += sprintf(page+len, "Cached IO\n\n");
+			seq_puts(m, "Cached IO\n\n");
 			break;
-
 		case DIRECT_IO:
-			len += sprintf(page+len, "Direct IO\n\n");
+			seq_puts(m, "Direct IO\n\n");
 			break;
 		}
 	}
 
-	mega_free_inquiry(inquiry, dma_handle, pdev);
-
+free_pci:
 	pci_free_consistent(pdev, array_sz, disk_array,
 			disk_array_dma_handle);
-
+free_inquiry:
+	mega_free_inquiry(inquiry, dma_handle, pdev);
+free_pdev:
 	free_local_pdev(pdev);
+	return 0;
+}
+
+/**
+ * proc_show_rdrv_10()
+ * @m - Synthetic file construction data
+ * @v - File iterator
+ *
+ * Display real time information about the logical drives 0 through 9.
+ */
+static int
+proc_show_rdrv_10(struct seq_file *m, void *v)
+{
+	return proc_show_rdrv(m, m->private, 0, 9);
+}
+
+
+/**
+ * proc_show_rdrv_20()
+ * @m - Synthetic file construction data
+ * @v - File iterator
+ *
+ * Display real time information about the logical drives 0 through 9.
+ */
+static int
+proc_show_rdrv_20(struct seq_file *m, void *v)
+{
+	return proc_show_rdrv(m, m->private, 10, 19);
+}
+
+
+/**
+ * proc_show_rdrv_30()
+ * @m - Synthetic file construction data
+ * @v - File iterator
+ *
+ * Display real time information about the logical drives 0 through 9.
+ */
+static int
+proc_show_rdrv_30(struct seq_file *m, void *v)
+{
+	return proc_show_rdrv(m, m->private, 20, 29);
+}
+
+
+/**
+ * proc_show_rdrv_40()
+ * @m - Synthetic file construction data
+ * @v - File iterator
+ *
+ * Display real time information about the logical drives 0 through 9.
+ */
+static int
+proc_show_rdrv_40(struct seq_file *m, void *v)
+{
+	return proc_show_rdrv(m, m->private, 30, 39);
+}
+
+
+/*
+ * seq_file wrappers for procfile show routines.
+ */
+static int mega_proc_open(struct inode *inode, struct file *file)
+{
+	adapter_t *adapter = proc_get_parent_data(inode);
+	int (*show)(struct seq_file *, void *) = PDE_DATA(inode);
+
+	return single_open(file, show, adapter);
+}
+
+static const struct file_operations mega_proc_fops = {
+	.open		= mega_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+/*
+ * Table of proc files we need to create.
+ */
+struct mega_proc_file {
+	const char *name;
+	unsigned short ptr_offset;
+	int (*show) (struct seq_file *m, void *v);
+};
+
+static const struct mega_proc_file mega_proc_files[] = {
+	{ "config",	      offsetof(adapter_t, proc_read), proc_show_config },
+	{ "stat",	      offsetof(adapter_t, proc_stat), proc_show_stat },
+	{ "mailbox",	      offsetof(adapter_t, proc_mbox), proc_show_mbox },
+#if MEGA_HAVE_ENH_PROC
+	{ "rebuild-rate",     offsetof(adapter_t, proc_rr), proc_show_rebuild_rate },
+	{ "battery-status",   offsetof(adapter_t, proc_battery), proc_show_battery },
+	{ "diskdrives-ch0",   offsetof(adapter_t, proc_pdrvstat[0]), proc_show_pdrv_ch0 },
+	{ "diskdrives-ch1",   offsetof(adapter_t, proc_pdrvstat[1]), proc_show_pdrv_ch1 },
+	{ "diskdrives-ch2",   offsetof(adapter_t, proc_pdrvstat[2]), proc_show_pdrv_ch2 },
+	{ "diskdrives-ch3",   offsetof(adapter_t, proc_pdrvstat[3]), proc_show_pdrv_ch3 },
+	{ "raiddrives-0-9",   offsetof(adapter_t, proc_rdrvstat[0]), proc_show_rdrv_10 },
+	{ "raiddrives-10-19", offsetof(adapter_t, proc_rdrvstat[1]), proc_show_rdrv_20 },
+	{ "raiddrives-20-29", offsetof(adapter_t, proc_rdrvstat[2]), proc_show_rdrv_30 },
+	{ "raiddrives-30-39", offsetof(adapter_t, proc_rdrvstat[3]), proc_show_rdrv_40 },
+#endif
+	{ NULL }
+};
 
-	return len;
+/**
+ * mega_create_proc_entry()
+ * @index - index in soft state array
+ * @parent - parent node for this /proc entry
+ *
+ * Creates /proc entries for our controllers.
+ */
+static void
+mega_create_proc_entry(int index, struct proc_dir_entry *parent)
+{
+	const struct mega_proc_file *f;
+	adapter_t	*adapter = hba_soft_state[index];
+	struct proc_dir_entry	*dir, *de, **ppde;
+	u8		string[16];
+
+	sprintf(string, "hba%d", adapter->host->host_no);
+
+	dir = adapter->controller_proc_dir_entry =
+		proc_mkdir_data(string, 0, parent, adapter);
+	if(!dir) {
+		printk(KERN_WARNING "\nmegaraid: proc_mkdir failed\n");
+		return;
+	}
+
+	for (f = mega_proc_files; f->name; f++) {
+		de = proc_create_data(f->name, S_IRUSR, dir, &mega_proc_fops,
+				      f->show);
+		if (!de) {
+			printk(KERN_WARNING "\nmegaraid: proc_create failed\n");
+			return;
+		}
+
+		ppde = (void *)adapter + f->ptr_offset;
+		*ppde = de;
+	}
 }
+
 #else
 static inline void mega_create_proc_entry(int index, struct proc_dir_entry *parent)
 {
diff --git a/drivers/scsi/megaraid.h b/drivers/scsi/megaraid.h
index 4fb2adf6b80d..4d0ce4e78dfd 100644
--- a/drivers/scsi/megaraid.h
+++ b/drivers/scsi/megaraid.h
@@ -987,24 +987,7 @@ static int mega_init_scb (adapter_t *);
 static int mega_is_bios_enabled (adapter_t *);
 
 #ifdef CONFIG_PROC_FS
-static int mega_print_inquiry(char *, char *);
 static void mega_create_proc_entry(int, struct proc_dir_entry *);
-static int proc_read_config(char *, char **, off_t, int, int *, void *);
-static int proc_read_stat(char *, char **, off_t, int, int *, void *);
-static int proc_read_mbox(char *, char **, off_t, int, int *, void *);
-static int proc_rebuild_rate(char *, char **, off_t, int, int *, void *);
-static int proc_battery(char *, char **, off_t, int, int *, void *);
-static int proc_pdrv_ch0(char *, char **, off_t, int, int *, void *);
-static int proc_pdrv_ch1(char *, char **, off_t, int, int *, void *);
-static int proc_pdrv_ch2(char *, char **, off_t, int, int *, void *);
-static int proc_pdrv_ch3(char *, char **, off_t, int, int *, void *);
-static int proc_pdrv(adapter_t *, char *, int);
-static int proc_rdrv_10(char *, char **, off_t, int, int *, void *);
-static int proc_rdrv_20(char *, char **, off_t, int, int *, void *);
-static int proc_rdrv_30(char *, char **, off_t, int, int *, void *);
-static int proc_rdrv_40(char *, char **, off_t, int, int *, void *);
-static int proc_rdrv(adapter_t *, char *, int, int);
-
 static int mega_adapinq(adapter_t *, dma_addr_t);
 static int mega_internal_dev_inquiry(adapter_t *, u8, u8, dma_addr_t);
 #endif
diff --git a/drivers/scsi/mpt2sas/mpt2sas_ctl.c b/drivers/scsi/mpt2sas/mpt2sas_ctl.c
index 08685c4cf231..eec052c2670a 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_ctl.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_ctl.c
@@ -505,19 +505,6 @@ _ctl_fasync(int fd, struct file *filep, int mode)
 }
 
 /**
- * _ctl_release -
- * @inode -
- * @filep -
- *
- * Called when application releases the fasyn callback handler.
- */
-static int
-_ctl_release(struct inode *inode, struct file *filep)
-{
-	return fasync_helper(-1, filep, 0, &async_queue);
-}
-
-/**
  * _ctl_poll -
  * @file -
  * @wait -
@@ -3027,7 +3014,6 @@ struct device_attribute *mpt2sas_dev_attrs[] = {
 static const struct file_operations ctl_fops = {
 	.owner = THIS_MODULE,
 	.unlocked_ioctl = _ctl_ioctl,
-	.release = _ctl_release,
 	.poll = _ctl_poll,
 	.fasync = _ctl_fasync,
 #ifdef CONFIG_COMPAT
diff --git a/drivers/scsi/mpt2sas/mpt2sas_transport.c b/drivers/scsi/mpt2sas/mpt2sas_transport.c
index 8c2ffbe6af0f..193e7ae90c3b 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_transport.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_transport.c
@@ -1939,7 +1939,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	ioc->transport_cmds.status = MPT2_CMD_PENDING;
 
 	/* Check if the request is split across multiple segments */
-	if (req->bio->bi_vcnt > 1) {
+	if (bio_segments(req->bio) > 1) {
 		u32 offset = 0;
 
 		/* Allocate memory and copy the request */
@@ -1971,7 +1971,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 
 	/* Check if the response needs to be populated across
 	 * multiple segments */
-	if (rsp->bio->bi_vcnt > 1) {
+	if (bio_segments(rsp->bio) > 1) {
 		pci_addr_in = pci_alloc_consistent(ioc->pdev, blk_rq_bytes(rsp),
 		    &pci_dma_in);
 		if (!pci_addr_in) {
@@ -2038,7 +2038,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	sgl_flags = (MPI2_SGE_FLAGS_SIMPLE_ELEMENT |
 	    MPI2_SGE_FLAGS_END_OF_BUFFER | MPI2_SGE_FLAGS_HOST_TO_IOC);
 	sgl_flags = sgl_flags << MPI2_SGE_FLAGS_SHIFT;
-	if (req->bio->bi_vcnt > 1) {
+	if (bio_segments(req->bio) > 1) {
 		ioc->base_add_sg_single(psge, sgl_flags |
 		    (blk_rq_bytes(req) - 4), pci_dma_out);
 	} else {
@@ -2054,7 +2054,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	    MPI2_SGE_FLAGS_LAST_ELEMENT | MPI2_SGE_FLAGS_END_OF_BUFFER |
 	    MPI2_SGE_FLAGS_END_OF_LIST);
 	sgl_flags = sgl_flags << MPI2_SGE_FLAGS_SHIFT;
-	if (rsp->bio->bi_vcnt > 1) {
+	if (bio_segments(rsp->bio) > 1) {
 		ioc->base_add_sg_single(psge, sgl_flags |
 		    (blk_rq_bytes(rsp) + 4), pci_dma_in);
 	} else {
@@ -2099,7 +2099,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 		    le16_to_cpu(mpi_reply->ResponseDataLength);
 		/* check if the resp needs to be copied from the allocated
 		 * pci mem */
-		if (rsp->bio->bi_vcnt > 1) {
+		if (bio_segments(rsp->bio) > 1) {
 			u32 offset = 0;
 			u32 bytes_to_copy =
 			    le16_to_cpu(mpi_reply->ResponseDataLength);
diff --git a/drivers/scsi/mpt3sas/mpt3sas_ctl.c b/drivers/scsi/mpt3sas/mpt3sas_ctl.c
index 054d5231c974..0b402b6f2d26 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_ctl.c
+++ b/drivers/scsi/mpt3sas/mpt3sas_ctl.c
@@ -503,19 +503,6 @@ _ctl_fasync(int fd, struct file *filep, int mode)
 }
 
 /**
- * _ctl_release -
- * @inode -
- * @filep -
- *
- * Called when application releases the fasyn callback handler.
- */
-static int
-_ctl_release(struct inode *inode, struct file *filep)
-{
-	return fasync_helper(-1, filep, 0, &async_queue);
-}
-
-/**
  * _ctl_poll -
  * @file -
  * @wait -
@@ -3233,7 +3220,6 @@ struct device_attribute *mpt3sas_dev_attrs[] = {
 static const struct file_operations ctl_fops = {
 	.owner = THIS_MODULE,
 	.unlocked_ioctl = _ctl_ioctl,
-	.release = _ctl_release,
 	.poll = _ctl_poll,
 	.fasync = _ctl_fasync,
 #ifdef CONFIG_COMPAT
diff --git a/drivers/scsi/mvme147.c b/drivers/scsi/mvme147.c
index c29d0dbb9660..e7f6661a8862 100644
--- a/drivers/scsi/mvme147.c
+++ b/drivers/scsi/mvme147.c
@@ -76,7 +76,8 @@ int mvme147_detect(struct scsi_host_template *tpnt)
 	called++;
 
 	tpnt->proc_name = "MVME147";
-	tpnt->proc_info = &wd33c93_proc_info;
+	tpnt->show_info = wd33c93_show_info,
+	tpnt->write_info = wd33c93_write_info,
 
 	instance = scsi_register(tpnt, sizeof(struct WD33C93_hostdata));
 	if (!instance)
diff --git a/drivers/scsi/nsp32.c b/drivers/scsi/nsp32.c
index 1cc0c1c69c88..1e3879dcbdcc 100644
--- a/drivers/scsi/nsp32.c
+++ b/drivers/scsi/nsp32.c
@@ -192,7 +192,7 @@ static int  __init init_nsp32  (void);
 static void __exit exit_nsp32  (void);
 
 /* struct struct scsi_host_template */
-static int         nsp32_proc_info   (struct Scsi_Host *, char *, char **, off_t, int, int);
+static int         nsp32_show_info   (struct seq_file *, struct Scsi_Host *);
 
 static int         nsp32_detect      (struct pci_dev *pdev);
 static int         nsp32_queuecommand(struct Scsi_Host *, struct scsi_cmnd *);
@@ -268,7 +268,7 @@ static void nsp32_dmessage(const char *, int, int,    char *, ...);
 static struct scsi_host_template nsp32_template = {
 	.proc_name			= "nsp32",
 	.name				= "Workbit NinjaSCSI-32Bi/UDE",
-	.proc_info			= nsp32_proc_info,
+	.show_info			= nsp32_show_info,
 	.info				= nsp32_info,
 	.queuecommand			= nsp32_queuecommand,
 	.can_queue			= 1,
@@ -1442,19 +1442,10 @@ static irqreturn_t do_nsp32_isr(int irq, void *dev_id)
 }
 
 #undef SPRINTF
-#define SPRINTF(args...) \
-	do { \
-		if(length > (pos - buffer)) { \
-			pos += snprintf(pos, length - (pos - buffer) + 1, ## args); \
-			nsp32_dbg(NSP32_DEBUG_PROC, "buffer=0x%p pos=0x%p length=%d %d\n", buffer, pos, length,  length - (pos - buffer));\
-		} \
-	} while(0)
-
-static int nsp32_proc_info(struct Scsi_Host *host, char *buffer, char **start,
-			   off_t offset, int length, int inout)
+#define SPRINTF(args...) seq_printf(m, ##args)
+
+static int nsp32_show_info(struct seq_file *m, struct Scsi_Host *host)
 {
-	char             *pos = buffer;
-	int               thislength;
 	unsigned long     flags;
 	nsp32_hw_data    *data;
 	int               hostno;
@@ -1463,11 +1454,6 @@ static int nsp32_proc_info(struct Scsi_Host *host, char *buffer, char **start,
 	int               id, speed;
 	long              model;
 
-	/* Write is not supported, just return. */
-	if (inout == TRUE) {
-		return -EINVAL;
-	}
-
 	hostno = host->host_no;
 	data = (nsp32_hw_data *)host->hostdata;
 	base = host->io_port;
@@ -1527,20 +1513,7 @@ static int nsp32_proc_info(struct Scsi_Host *host, char *buffer, char **start,
 		}
 		SPRINTF("\n");
 	}
-
-
-	thislength = pos - (buffer + offset);
-
-	if(thislength < 0) {
-		*start = NULL;
-                return 0;
-        }
-
-
-	thislength = min(thislength, length);
-	*start = buffer + offset;
-
-	return thislength;
+	return 0;
 }
 #undef SPRINTF
 
diff --git a/drivers/scsi/pas16.c b/drivers/scsi/pas16.c
index 2f72c9807b12..62f1a6031765 100644
--- a/drivers/scsi/pas16.c
+++ b/drivers/scsi/pas16.c
@@ -388,7 +388,8 @@ int __init pas16_detect(struct scsi_host_template * tpnt)
     int  count;
 
     tpnt->proc_name = "pas16";
-    tpnt->proc_info = &pas16_proc_info;
+    tpnt->show_info = pas16_show_info;
+    tpnt->write_info = pas16_write_info;
 
     if (pas16_addr != 0) {
 	overrides[0].io_port = pas16_addr;
diff --git a/drivers/scsi/pas16.h b/drivers/scsi/pas16.h
index a04281cace2e..3721342835e9 100644
--- a/drivers/scsi/pas16.h
+++ b/drivers/scsi/pas16.h
@@ -163,7 +163,8 @@ static int pas16_bus_reset(Scsi_Cmnd *);
 #define NCR5380_queue_command pas16_queue_command
 #define NCR5380_abort pas16_abort
 #define NCR5380_bus_reset pas16_bus_reset
-#define NCR5380_proc_info pas16_proc_info
+#define NCR5380_show_info pas16_show_info
+#define NCR5380_write_info pas16_write_info
 
 /* 15 14 12 10 7 5 3 
    1101 0100 1010 1000 */
diff --git a/drivers/scsi/pcmcia/nsp_cs.c b/drivers/scsi/pcmcia/nsp_cs.c
index b61a753eb896..987fbb1b244e 100644
--- a/drivers/scsi/pcmcia/nsp_cs.c
+++ b/drivers/scsi/pcmcia/nsp_cs.c
@@ -76,7 +76,7 @@ MODULE_PARM_DESC(free_ports, "Release IO ports after configuration? (default: 0
 
 static struct scsi_host_template nsp_driver_template = {
 	.proc_name	         = "nsp_cs",
-	.proc_info		 = nsp_proc_info,
+	.show_info		 = nsp_show_info,
 	.name			 = "WorkBit NinjaSCSI-3/32Bi(16bit)",
 	.info			 = nsp_info,
 	.queuecommand		 = nsp_queuecommand,
@@ -1365,33 +1365,19 @@ static const char *nsp_info(struct Scsi_Host *shpnt)
 }
 
 #undef SPRINTF
-#define SPRINTF(args...) \
-        do { \
-		if(length > (pos - buffer)) { \
-			pos += snprintf(pos, length - (pos - buffer) + 1, ## args); \
-			nsp_dbg(NSP_DEBUG_PROC, "buffer=0x%p pos=0x%p length=%d %d\n", buffer, pos, length,  length - (pos - buffer));\
-		} \
-	} while(0)
-
-static int nsp_proc_info(struct Scsi_Host *host, char *buffer, char **start,
-			 off_t offset, int length, int inout)
+#define SPRINTF(args...) seq_printf(m, ##args)
+
+static int nsp_show_info(struct seq_file *m, struct Scsi_Host *host)
 {
 	int id;
-	char *pos = buffer;
-	int thislength;
 	int speed;
 	unsigned long flags;
 	nsp_hw_data *data;
 	int hostno;
 
-	if (inout) {
-		return -EINVAL;
-	}
-
 	hostno = host->host_no;
 	data = (nsp_hw_data *)host->hostdata;
 
-
 	SPRINTF("NinjaSCSI status\n\n");
 	SPRINTF("Driver version:        $Revision: 1.23 $\n");
 	SPRINTF("SCSI host No.:         %d\n",          hostno);
@@ -1458,19 +1444,7 @@ static int nsp_proc_info(struct Scsi_Host *host, char *buffer, char **start,
 		}
 		SPRINTF("\n");
 	}
-
-	thislength = pos - (buffer + offset);
-
-	if(thislength < 0) {
-		*start = NULL;
-                return 0;
-        }
-
-
-	thislength = min(thislength, length);
-	*start = buffer + offset;
-
-	return thislength;
+	return 0;
 }
 #undef SPRINTF
 
diff --git a/drivers/scsi/pcmcia/nsp_cs.h b/drivers/scsi/pcmcia/nsp_cs.h
index 7fc9a9d0a448..afd64f0adc4b 100644
--- a/drivers/scsi/pcmcia/nsp_cs.h
+++ b/drivers/scsi/pcmcia/nsp_cs.h
@@ -292,13 +292,8 @@ static int        nsp_cs_config (struct pcmcia_device *link);
 /* Linux SCSI subsystem specific functions */
 static struct Scsi_Host *nsp_detect     (struct scsi_host_template *sht);
 static const  char      *nsp_info       (struct Scsi_Host *shpnt);
-static        int        nsp_proc_info  (
-	                                 struct Scsi_Host *host,
-					 char   *buffer,
-					 char  **start,
-					 off_t   offset,
-					 int     length,
-					 int     inout);
+static        int        nsp_show_info  (struct seq_file *m,
+	                                 struct Scsi_Host *host);
 static int nsp_queuecommand(struct Scsi_Host *h, struct scsi_cmnd *SCpnt);
 
 /* Error handler */
diff --git a/drivers/scsi/pmcraid.c b/drivers/scsi/pmcraid.c
index b46f5e906837..8e1b73775065 100644
--- a/drivers/scsi/pmcraid.c
+++ b/drivers/scsi/pmcraid.c
@@ -3599,19 +3599,6 @@ static int pmcraid_chr_open(struct inode *inode, struct file *filep)
 }
 
 /**
- * pmcraid_release - char node "release" entry point
- */
-static int pmcraid_chr_release(struct inode *inode, struct file *filep)
-{
-	struct pmcraid_instance *pinstance = filep->private_data;
-
-	filep->private_data = NULL;
-	fasync_helper(-1, filep, 0, &pinstance->aen_queue);
-
-	return 0;
-}
-
-/**
  * pmcraid_fasync - Async notifier registration from applications
  *
  * This function adds the calling process to a driver global queue. When an
@@ -4167,7 +4154,6 @@ static long pmcraid_chr_ioctl(
 static const struct file_operations pmcraid_fops = {
 	.owner = THIS_MODULE,
 	.open = pmcraid_chr_open,
-	.release = pmcraid_chr_release,
 	.fasync = pmcraid_chr_fasync,
 	.unlocked_ioctl = pmcraid_chr_ioctl,
 #ifdef CONFIG_COMPAT
diff --git a/drivers/scsi/ppa.c b/drivers/scsi/ppa.c
index d164c9639361..1db8b26063b4 100644
--- a/drivers/scsi/ppa.c
+++ b/drivers/scsi/ppa.c
@@ -118,8 +118,9 @@ static inline void ppa_pb_release(ppa_struct *dev)
  * Also gives a method to use a script to obtain optimum timings (TODO)
  */
 
-static inline int ppa_proc_write(ppa_struct *dev, char *buffer, int length)
+static inline int ppa_write_info(struct Scsi_Host *host, char *buffer, int length)
 {
+	ppa_struct *dev = ppa_dev(host);
 	unsigned long x;
 
 	if ((length > 5) && (strncmp(buffer, "mode=", 5) == 0)) {
@@ -137,35 +138,17 @@ static inline int ppa_proc_write(ppa_struct *dev, char *buffer, int length)
 	return -EINVAL;
 }
 
-static int ppa_proc_info(struct Scsi_Host *host, char *buffer, char **start, off_t offset, int length, int inout)
+static int ppa_show_info(struct seq_file *m, struct Scsi_Host *host)
 {
-	int len = 0;
 	ppa_struct *dev = ppa_dev(host);
 
-	if (inout)
-		return ppa_proc_write(dev, buffer, length);
-
-	len += sprintf(buffer + len, "Version : %s\n", PPA_VERSION);
-	len +=
-	    sprintf(buffer + len, "Parport : %s\n",
-		    dev->dev->port->name);
-	len +=
-	    sprintf(buffer + len, "Mode    : %s\n",
-		    PPA_MODE_STRING[dev->mode]);
+	seq_printf(m, "Version : %s\n", PPA_VERSION);
+	seq_printf(m, "Parport : %s\n", dev->dev->port->name);
+	seq_printf(m, "Mode    : %s\n", PPA_MODE_STRING[dev->mode]);
 #if PPA_DEBUG > 0
-	len +=
-	    sprintf(buffer + len, "recon_tmo : %lu\n", dev->recon_tmo);
+	seq_printf(m, "recon_tmo : %lu\n", dev->recon_tmo);
 #endif
-
-	/* Request for beyond end of buffer */
-	if (offset > length)
-		return 0;
-
-	*start = buffer + offset;
-	len -= offset;
-	if (len > length)
-		len = length;
-	return len;
+	return 0;
 }
 
 static int device_check(ppa_struct *dev);
@@ -981,7 +964,8 @@ static int ppa_adjust_queue(struct scsi_device *device)
 static struct scsi_host_template ppa_template = {
 	.module			= THIS_MODULE,
 	.proc_name		= "ppa",
-	.proc_info		= ppa_proc_info,
+	.show_info		= ppa_show_info,
+	.write_info		= ppa_write_info,
 	.name			= "Iomega VPI0 (ppa) interface",
 	.queuecommand		= ppa_queuecommand,
 	.eh_abort_handler	= ppa_abort,
diff --git a/drivers/scsi/qla2xxx/Makefile b/drivers/scsi/qla2xxx/Makefile
index dce7d788cdc9..c37b244cf8ae 100644
--- a/drivers/scsi/qla2xxx/Makefile
+++ b/drivers/scsi/qla2xxx/Makefile
@@ -1,6 +1,6 @@
 qla2xxx-y := qla_os.o qla_init.o qla_mbx.o qla_iocb.o qla_isr.o qla_gs.o \
 		qla_dbg.o qla_sup.o qla_attr.o qla_mid.o qla_dfs.o qla_bsg.o \
-        qla_nx.o qla_target.o
+        qla_nx.o qla_mr.o qla_target.o
 
 obj-$(CONFIG_SCSI_QLA_FC) += qla2xxx.o
 obj-$(CONFIG_TCM_QLA2XXX) += tcm_qla2xxx.o
diff --git a/drivers/scsi/qla2xxx/qla_attr.c b/drivers/scsi/qla2xxx/qla_attr.c
index b3db9dcc2619..bf60c631abb5 100644
--- a/drivers/scsi/qla2xxx/qla_attr.c
+++ b/drivers/scsi/qla2xxx/qla_attr.c
@@ -888,7 +888,10 @@ qla2x00_serial_num_show(struct device *dev, struct device_attribute *attr,
 	struct qla_hw_data *ha = vha->hw;
 	uint32_t sn;
 
-	if (IS_FWI2_CAPABLE(ha)) {
+	if (IS_QLAFX00(vha->hw)) {
+		return snprintf(buf, PAGE_SIZE, "%s\n",
+		    vha->hw->mr.serial_num);
+	} else if (IS_FWI2_CAPABLE(ha)) {
 		qla2xxx_get_vpd_field(vha, "SN", buf, PAGE_SIZE);
 		return snprintf(buf, PAGE_SIZE, "%s\n", buf);
 	}
@@ -912,6 +915,11 @@ qla2x00_isp_id_show(struct device *dev, struct device_attribute *attr,
 {
 	scsi_qla_host_t *vha = shost_priv(class_to_shost(dev));
 	struct qla_hw_data *ha = vha->hw;
+
+	if (IS_QLAFX00(vha->hw))
+		return snprintf(buf, PAGE_SIZE, "%s\n",
+		    vha->hw->mr.hw_version);
+
 	return snprintf(buf, PAGE_SIZE, "%04x %04x %04x %04x\n",
 	    ha->product_id[0], ha->product_id[1], ha->product_id[2],
 	    ha->product_id[3]);
@@ -922,6 +930,11 @@ qla2x00_model_name_show(struct device *dev, struct device_attribute *attr,
 			char *buf)
 {
 	scsi_qla_host_t *vha = shost_priv(class_to_shost(dev));
+
+	if (IS_QLAFX00(vha->hw))
+		return snprintf(buf, PAGE_SIZE, "%s\n",
+		    vha->hw->mr.product_name);
+
 	return snprintf(buf, PAGE_SIZE, "%s\n", vha->hw->model_number);
 }
 
@@ -1304,6 +1317,12 @@ qla2x00_fw_state_show(struct device *dev, struct device_attribute *attr,
 	scsi_qla_host_t *vha = shost_priv(class_to_shost(dev));
 	int rval = QLA_FUNCTION_FAILED;
 	uint16_t state[5];
+	uint32_t pstate;
+
+	if (IS_QLAFX00(vha->hw)) {
+		pstate = qlafx00_fw_state_show(dev, attr, buf);
+		return snprintf(buf, PAGE_SIZE, "0x%x\n", pstate);
+	}
 
 	if (qla2x00_reset_active(vha))
 		ql_log(ql_log_warn, vha, 0x707c,
@@ -1454,6 +1473,11 @@ qla2x00_get_host_speed(struct Scsi_Host *shost)
 					(shost_priv(shost)))->hw;
 	u32 speed = FC_PORTSPEED_UNKNOWN;
 
+	if (IS_QLAFX00(ha)) {
+		qlafx00_get_host_speed(shost);
+		return;
+	}
+
 	switch (ha->link_data_rate) {
 	case PORT_SPEED_1GB:
 		speed = FC_PORTSPEED_1GBIT;
@@ -1637,6 +1661,9 @@ qla2x00_issue_lip(struct Scsi_Host *shost)
 {
 	scsi_qla_host_t *vha = shost_priv(shost);
 
+	if (IS_QLAFX00(vha->hw))
+		return 0;
+
 	qla2x00_loop_reset(vha);
 	return 0;
 }
@@ -1655,6 +1682,9 @@ qla2x00_get_fc_host_stats(struct Scsi_Host *shost)
 	pfc_host_stat = &vha->fc_host_stat;
 	memset(pfc_host_stat, -1, sizeof(struct fc_host_statistics));
 
+	if (IS_QLAFX00(vha->hw))
+		goto done;
+
 	if (test_bit(UNLOADING, &vha->dpc_flags))
 		goto done;
 
@@ -2087,6 +2117,9 @@ qla2x00_init_host_attr(scsi_qla_host_t *vha)
 		    FC_PORTSPEED_1GBIT;
 	else if (IS_QLA23XX(ha))
 		speed = FC_PORTSPEED_2GBIT | FC_PORTSPEED_1GBIT;
+	else if (IS_QLAFX00(ha))
+		speed = FC_PORTSPEED_8GBIT | FC_PORTSPEED_4GBIT |
+		    FC_PORTSPEED_2GBIT | FC_PORTSPEED_1GBIT;
 	else
 		speed = FC_PORTSPEED_1GBIT;
 	fc_host_supported_speeds(vha->host) = speed;
diff --git a/drivers/scsi/qla2xxx/qla_bsg.c b/drivers/scsi/qla2xxx/qla_bsg.c
index ad54099cb805..39719f892488 100644
--- a/drivers/scsi/qla2xxx/qla_bsg.c
+++ b/drivers/scsi/qla2xxx/qla_bsg.c
@@ -30,14 +30,31 @@ qla2x00_bsg_sp_free(void *data, void *ptr)
 	struct scsi_qla_host *vha = sp->fcport->vha;
 	struct fc_bsg_job *bsg_job = sp->u.bsg_job;
 	struct qla_hw_data *ha = vha->hw;
+	struct qla_mt_iocb_rqst_fx00 *piocb_rqst;
 
-	dma_unmap_sg(&ha->pdev->dev, bsg_job->request_payload.sg_list,
-	    bsg_job->request_payload.sg_cnt, DMA_TO_DEVICE);
+	if (sp->type == SRB_FXIOCB_BCMD) {
+		piocb_rqst = (struct qla_mt_iocb_rqst_fx00 *)
+		    &bsg_job->request->rqst_data.h_vendor.vendor_cmd[1];
 
-	dma_unmap_sg(&ha->pdev->dev, bsg_job->reply_payload.sg_list,
-	    bsg_job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+		if (piocb_rqst->flags & SRB_FXDISC_REQ_DMA_VALID)
+			dma_unmap_sg(&ha->pdev->dev,
+			    bsg_job->request_payload.sg_list,
+			    bsg_job->request_payload.sg_cnt, DMA_TO_DEVICE);
+
+		if (piocb_rqst->flags & SRB_FXDISC_RESP_DMA_VALID)
+			dma_unmap_sg(&ha->pdev->dev,
+			    bsg_job->reply_payload.sg_list,
+			    bsg_job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+	} else {
+		dma_unmap_sg(&ha->pdev->dev, bsg_job->request_payload.sg_list,
+		    bsg_job->request_payload.sg_cnt, DMA_TO_DEVICE);
+
+		dma_unmap_sg(&ha->pdev->dev, bsg_job->reply_payload.sg_list,
+		    bsg_job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+	}
 
 	if (sp->type == SRB_CT_CMD ||
+	    sp->type == SRB_FXIOCB_BCMD ||
 	    sp->type == SRB_ELS_CMD_HST)
 		kfree(sp->fcport);
 	qla2x00_rel_sp(vha, sp);
@@ -751,6 +768,8 @@ qla2x00_process_loopback(struct fc_bsg_job *bsg_job)
 	elreq.transfer_size = req_data_len;
 
 	elreq.options = bsg_job->request->rqst_data.h_vendor.vendor_cmd[1];
+	elreq.iteration_count =
+	    bsg_job->request->rqst_data.h_vendor.vendor_cmd[2];
 
 	if (atomic_read(&vha->loop_state) == LOOP_READY &&
 	    (ha->current_topology == ISP_CFG_F ||
@@ -1883,6 +1902,128 @@ done:
 }
 
 static int
+qlafx00_mgmt_cmd(struct fc_bsg_job *bsg_job)
+{
+	struct Scsi_Host *host = bsg_job->shost;
+	scsi_qla_host_t *vha = shost_priv(host);
+	struct qla_hw_data *ha = vha->hw;
+	int rval = (DRIVER_ERROR << 16);
+	struct qla_mt_iocb_rqst_fx00 *piocb_rqst;
+	srb_t *sp;
+	int req_sg_cnt = 0, rsp_sg_cnt = 0;
+	struct fc_port *fcport;
+	char  *type = "FC_BSG_HST_FX_MGMT";
+
+	/* Copy the IOCB specific information */
+	piocb_rqst = (struct qla_mt_iocb_rqst_fx00 *)
+	    &bsg_job->request->rqst_data.h_vendor.vendor_cmd[1];
+
+	/* Dump the vendor information */
+	ql_dump_buffer(ql_dbg_user + ql_dbg_verbose , vha, 0x70cf,
+	    (uint8_t *)piocb_rqst, sizeof(struct qla_mt_iocb_rqst_fx00));
+
+	if (!vha->flags.online) {
+		ql_log(ql_log_warn, vha, 0x70d0,
+		    "Host is not online.\n");
+		rval = -EIO;
+		goto done;
+	}
+
+	if (piocb_rqst->flags & SRB_FXDISC_REQ_DMA_VALID) {
+		req_sg_cnt = dma_map_sg(&ha->pdev->dev,
+		    bsg_job->request_payload.sg_list,
+		    bsg_job->request_payload.sg_cnt, DMA_TO_DEVICE);
+		if (!req_sg_cnt) {
+			ql_log(ql_log_warn, vha, 0x70c7,
+			    "dma_map_sg return %d for request\n", req_sg_cnt);
+			rval = -ENOMEM;
+			goto done;
+		}
+	}
+
+	if (piocb_rqst->flags & SRB_FXDISC_RESP_DMA_VALID) {
+		rsp_sg_cnt = dma_map_sg(&ha->pdev->dev,
+		    bsg_job->reply_payload.sg_list,
+		    bsg_job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+		if (!rsp_sg_cnt) {
+			ql_log(ql_log_warn, vha, 0x70c8,
+			    "dma_map_sg return %d for reply\n", rsp_sg_cnt);
+			rval = -ENOMEM;
+			goto done_unmap_req_sg;
+		}
+	}
+
+	ql_dbg(ql_dbg_user, vha, 0x70c9,
+	    "request_sg_cnt: %x dma_request_sg_cnt: %x reply_sg_cnt:%x "
+	    "dma_reply_sg_cnt: %x\n", bsg_job->request_payload.sg_cnt,
+	    req_sg_cnt, bsg_job->reply_payload.sg_cnt, rsp_sg_cnt);
+
+	/* Allocate a dummy fcport structure, since functions preparing the
+	 * IOCB and mailbox command retrieves port specific information
+	 * from fcport structure. For Host based ELS commands there will be
+	 * no fcport structure allocated
+	 */
+	fcport = qla2x00_alloc_fcport(vha, GFP_KERNEL);
+	if (!fcport) {
+		ql_log(ql_log_warn, vha, 0x70ca,
+		    "Failed to allocate fcport.\n");
+		rval = -ENOMEM;
+		goto done_unmap_rsp_sg;
+	}
+
+	/* Alloc SRB structure */
+	sp = qla2x00_get_sp(vha, fcport, GFP_KERNEL);
+	if (!sp) {
+		ql_log(ql_log_warn, vha, 0x70cb,
+		    "qla2x00_get_sp failed.\n");
+		rval = -ENOMEM;
+		goto done_free_fcport;
+	}
+
+	/* Initialize all required  fields of fcport */
+	fcport->vha = vha;
+	fcport->loop_id = piocb_rqst->dataword;
+
+	sp->type = SRB_FXIOCB_BCMD;
+	sp->name = "bsg_fx_mgmt";
+	sp->iocbs = qla24xx_calc_ct_iocbs(req_sg_cnt + rsp_sg_cnt);
+	sp->u.bsg_job = bsg_job;
+	sp->free = qla2x00_bsg_sp_free;
+	sp->done = qla2x00_bsg_job_done;
+
+	ql_dbg(ql_dbg_user, vha, 0x70cc,
+	    "bsg rqst type: %s fx_mgmt_type: %x id=%x\n",
+	    type, piocb_rqst->func_type, fcport->loop_id);
+
+	rval = qla2x00_start_sp(sp);
+	if (rval != QLA_SUCCESS) {
+		ql_log(ql_log_warn, vha, 0x70cd,
+		    "qla2x00_start_sp failed=%d.\n", rval);
+		mempool_free(sp, ha->srb_mempool);
+		rval = -EIO;
+		goto done_free_fcport;
+	}
+	return rval;
+
+done_free_fcport:
+	kfree(fcport);
+
+done_unmap_rsp_sg:
+	if (piocb_rqst->flags & SRB_FXDISC_RESP_DMA_VALID)
+		dma_unmap_sg(&ha->pdev->dev,
+		    bsg_job->reply_payload.sg_list,
+		    bsg_job->reply_payload.sg_cnt, DMA_FROM_DEVICE);
+done_unmap_req_sg:
+	if (piocb_rqst->flags & SRB_FXDISC_REQ_DMA_VALID)
+		dma_unmap_sg(&ha->pdev->dev,
+		    bsg_job->request_payload.sg_list,
+		    bsg_job->request_payload.sg_cnt, DMA_TO_DEVICE);
+
+done:
+	return rval;
+}
+
+static int
 qla2x00_process_vendor_specific(struct fc_bsg_job *bsg_job)
 {
 	switch (bsg_job->request->rqst_data.h_vendor.vendor_cmd[0]) {
@@ -1928,6 +2069,8 @@ qla2x00_process_vendor_specific(struct fc_bsg_job *bsg_job)
 	case QL_VND_DIAG_IO_CMD:
 		return qla24xx_process_bidir_cmd(bsg_job);
 
+	case QL_VND_FX00_MGMT_CMD:
+		return qlafx00_mgmt_cmd(bsg_job);
 	default:
 		return -ENOSYS;
 	}
@@ -2007,7 +2150,8 @@ qla24xx_bsg_timeout(struct fc_bsg_job *bsg_job)
 			sp = req->outstanding_cmds[cnt];
 			if (sp) {
 				if (((sp->type == SRB_CT_CMD) ||
-					(sp->type == SRB_ELS_CMD_HST))
+					(sp->type == SRB_ELS_CMD_HST) ||
+					(sp->type == SRB_FXIOCB_BCMD))
 					&& (sp->u.bsg_job == bsg_job)) {
 					spin_unlock_irqrestore(&ha->hardware_lock, flags);
 					if (ha->isp_ops->abort_command(sp)) {
diff --git a/drivers/scsi/qla2xxx/qla_bsg.h b/drivers/scsi/qla2xxx/qla_bsg.h
index e9f6b9bbf29a..04f770332c2b 100644
--- a/drivers/scsi/qla2xxx/qla_bsg.h
+++ b/drivers/scsi/qla2xxx/qla_bsg.h
@@ -22,6 +22,7 @@
 #define QL_VND_DIAG_IO_CMD	0x0A
 #define QL_VND_WRITE_I2C	0x10
 #define QL_VND_READ_I2C		0x11
+#define QL_VND_FX00_MGMT_CMD	0x12
 
 /* BSG Vendor specific subcode returns */
 #define EXT_STATUS_OK			0
diff --git a/drivers/scsi/qla2xxx/qla_dbg.c b/drivers/scsi/qla2xxx/qla_dbg.c
index fbc305f1c87c..cfa2a20dee97 100644
--- a/drivers/scsi/qla2xxx/qla_dbg.c
+++ b/drivers/scsi/qla2xxx/qla_dbg.c
@@ -11,28 +11,31 @@
  * ----------------------------------------------------------------------
  * |             Level            |   Last Value Used  |     Holes	|
  * ----------------------------------------------------------------------
- * | Module Init and Probe        |       0x0126       | 0x4b,0xba,0xfa |
- * | Mailbox commands             |       0x115b       | 0x111a-0x111b  |
- * |                              |                    | 0x112c-0x112e  |
- * |                              |                    | 0x113a         |
+ * | Module Init and Probe        |       0x014f       | 0x4b,0xba,0xfa |
+ * | Mailbox commands             |       0x1179       | 0x111a-0x111b  |
  * |                              |                    | 0x1155-0x1158  |
- * | Device Discovery             |       0x2087       | 0x2020-0x2022, |
+ * | Device Discovery             |       0x2095       | 0x2020-0x2022, |
  * |                              |                    | 0x2016         |
- * | Queue Command and IO tracing |       0x3031       | 0x3006-0x300b  |
+ * | Queue Command and IO tracing |       0x3058       | 0x3006-0x300b  |
  * |                              |                    | 0x3027-0x3028  |
- * |                              |                    | 0x302d-0x302e  |
- * | DPC Thread                   |       0x401d       | 0x4002,0x4013  |
- * | Async Events                 |       0x5071       | 0x502b-0x502f  |
+ * |                              |                    | 0x303d-0x3041  |
+ * |                              |                    | 0x302d,0x3033  |
+ * |                              |                    | 0x3036,0x3038  |
+ * |                              |                    | 0x303a		|
+ * | DPC Thread                   |       0x4022       | 0x4002,0x4013  |
+ * | Async Events                 |       0x5081       | 0x502b-0x502f  |
  * |                              |                    | 0x5047,0x5052  |
+ * |                              |                    | 0x5040,0x5075  |
  * | Timer Routines               |       0x6011       |                |
- * | User Space Interactions      |       0x70c4       | 0x7018,0x702e, |
+ * | User Space Interactions      |       0x70dd       | 0x7018,0x702e, |
  * |                              |                    | 0x7020,0x7024, |
  * |                              |                    | 0x7039,0x7045, |
  * |                              |                    | 0x7073-0x7075, |
- * |                              |                    | 0x708c,        |
+ * |                              |                    | 0x707b,0x708c, |
  * |                              |                    | 0x70a5,0x70a6, |
  * |                              |                    | 0x70a8,0x70ab, |
- * |                              |                    | 0x70ad-0x70ae  |
+ * |                              |                    | 0x70ad-0x70ae, |
+ * |                              |                    | 0x70d1-0x70da  |
  * | Task Management              |       0x803c       | 0x8025-0x8026  |
  * |                              |                    | 0x800b,0x8039  |
  * | AER/EEH                      |       0x9011       |		|
diff --git a/drivers/scsi/qla2xxx/qla_def.h b/drivers/scsi/qla2xxx/qla_def.h
index 65c5ff75936b..c32efc753229 100644
--- a/drivers/scsi/qla2xxx/qla_def.h
+++ b/drivers/scsi/qla2xxx/qla_def.h
@@ -245,7 +245,6 @@
 
 #define MAX_CMDSZ	16		/* SCSI maximum CDB size. */
 #include "qla_fw.h"
-
 /*
  * Timeout timer counts in seconds
  */
@@ -265,6 +264,7 @@
 #define RESPONSE_ENTRY_CNT_2300		512	/* Number of response entries.*/
 #define RESPONSE_ENTRY_CNT_MQ		128	/* Number of response entries.*/
 #define ATIO_ENTRY_CNT_24XX		4096	/* Number of ATIO entries. */
+#define RESPONSE_ENTRY_CNT_FX00		256     /* Number of response entries.*/
 
 struct req_que;
 
@@ -284,6 +284,7 @@ struct sd_dif_tuple {
 struct srb_cmd {
 	struct scsi_cmnd *cmd;		/* Linux SCSI command pkt */
 	uint32_t request_sense_length;
+	uint32_t fw_sense_length;
 	uint8_t *request_sense_ptr;
 	void *ctx;
 };
@@ -321,7 +322,39 @@ struct srb_iocb {
 			uint32_t flags;
 			uint32_t lun;
 			uint32_t data;
+			struct completion comp;
+			uint32_t comp_status;
 		} tmf;
+		struct {
+#define SRB_FXDISC_REQ_DMA_VALID	BIT_0
+#define SRB_FXDISC_RESP_DMA_VALID	BIT_1
+#define SRB_FXDISC_REQ_DWRD_VALID	BIT_2
+#define SRB_FXDISC_RSP_DWRD_VALID	BIT_3
+#define FXDISC_TIMEOUT 20
+			uint8_t flags;
+			uint32_t req_len;
+			uint32_t rsp_len;
+			void *req_addr;
+			void *rsp_addr;
+			dma_addr_t req_dma_handle;
+			dma_addr_t rsp_dma_handle;
+			uint32_t adapter_id;
+			uint32_t adapter_id_hi;
+			uint32_t req_func_type;
+			uint32_t req_data;
+			uint32_t req_data_extra;
+			uint32_t result;
+			uint32_t seq_number;
+			uint32_t fw_flags;
+			struct completion fxiocb_comp;
+			uint32_t reserved_0;
+			uint8_t reserved_1;
+		} fxiocb;
+		struct {
+			uint32_t cmd_hndl;
+			uint32_t comp_status;
+			struct completion comp;
+		} abt;
 	} u;
 
 	struct timer_list timer;
@@ -338,6 +371,10 @@ struct srb_iocb {
 #define SRB_TM_CMD	7
 #define SRB_SCSI_CMD	8
 #define SRB_BIDI_CMD	9
+#define SRB_FXIOCB_DCMD	10
+#define SRB_FXIOCB_BCMD	11
+#define SRB_ABT_CMD	12
+
 
 typedef struct srb {
 	atomic_t ref_count;
@@ -368,6 +405,10 @@ typedef struct srb {
 	(sp->u.scmd.request_sense_ptr)
 #define SET_CMD_SENSE_PTR(sp, ptr) \
 	(sp->u.scmd.request_sense_ptr = ptr)
+#define GET_FW_SENSE_LEN(sp) \
+	(sp->u.scmd.fw_sense_length)
+#define SET_FW_SENSE_LEN(sp, len) \
+	(sp->u.scmd.fw_sense_length = len)
 
 struct msg_echo_lb {
 	dma_addr_t send_dma;
@@ -376,6 +417,7 @@ struct msg_echo_lb {
 	uint16_t rsp_sg_cnt;
 	uint16_t options;
 	uint32_t transfer_size;
+	uint32_t iteration_count;
 };
 
 /*
@@ -542,11 +584,74 @@ struct device_reg_25xxmq {
 	uint32_t atio_q_out;
 };
 
+
+struct device_reg_fx00 {
+	uint32_t mailbox0;		/* 00 */
+	uint32_t mailbox1;		/* 04 */
+	uint32_t mailbox2;		/* 08 */
+	uint32_t mailbox3;		/* 0C */
+	uint32_t mailbox4;		/* 10 */
+	uint32_t mailbox5;		/* 14 */
+	uint32_t mailbox6;		/* 18 */
+	uint32_t mailbox7;		/* 1C */
+	uint32_t mailbox8;		/* 20 */
+	uint32_t mailbox9;		/* 24 */
+	uint32_t mailbox10;		/* 28 */
+	uint32_t mailbox11;
+	uint32_t mailbox12;
+	uint32_t mailbox13;
+	uint32_t mailbox14;
+	uint32_t mailbox15;
+	uint32_t mailbox16;
+	uint32_t mailbox17;
+	uint32_t mailbox18;
+	uint32_t mailbox19;
+	uint32_t mailbox20;
+	uint32_t mailbox21;
+	uint32_t mailbox22;
+	uint32_t mailbox23;
+	uint32_t mailbox24;
+	uint32_t mailbox25;
+	uint32_t mailbox26;
+	uint32_t mailbox27;
+	uint32_t mailbox28;
+	uint32_t mailbox29;
+	uint32_t mailbox30;
+	uint32_t mailbox31;
+	uint32_t aenmailbox0;
+	uint32_t aenmailbox1;
+	uint32_t aenmailbox2;
+	uint32_t aenmailbox3;
+	uint32_t aenmailbox4;
+	uint32_t aenmailbox5;
+	uint32_t aenmailbox6;
+	uint32_t aenmailbox7;
+	/* Request Queue. */
+	uint32_t req_q_in;		/* A0 - Request Queue In-Pointer */
+	uint32_t req_q_out;		/* A4 - Request Queue Out-Pointer */
+	/* Response Queue. */
+	uint32_t rsp_q_in;		/* A8 - Response Queue In-Pointer */
+	uint32_t rsp_q_out;		/* AC - Response Queue Out-Pointer */
+	/* Init values shadowed on FW Up Event */
+	uint32_t initval0;		/* B0 */
+	uint32_t initval1;		/* B4 */
+	uint32_t initval2;		/* B8 */
+	uint32_t initval3;		/* BC */
+	uint32_t initval4;		/* C0 */
+	uint32_t initval5;		/* C4 */
+	uint32_t initval6;		/* C8 */
+	uint32_t initval7;		/* CC */
+	uint32_t fwheartbeat;		/* D0 */
+};
+
+
+
 typedef union {
 		struct device_reg_2xxx isp;
 		struct device_reg_24xx isp24;
 		struct device_reg_25xxmq isp25mq;
 		struct device_reg_82xx isp82;
+		struct device_reg_fx00 ispfx00;
 } device_reg_t;
 
 #define ISP_REQ_Q_IN(ha, reg) \
@@ -602,6 +707,20 @@ typedef struct {
 #define IOCTL_CMD	BIT_2
 } mbx_cmd_t;
 
+struct mbx_cmd_32 {
+	uint32_t	out_mb;		/* outbound from driver */
+	uint32_t	in_mb;			/* Incoming from RISC */
+	uint32_t	mb[MAILBOX_REGISTER_COUNT];
+	long		buf_size;
+	void		*bufp;
+	uint32_t	tov;
+	uint8_t		flags;
+#define MBX_DMA_IN	BIT_0
+#define	MBX_DMA_OUT	BIT_1
+#define IOCTL_CMD	BIT_2
+};
+
+
 #define	MBX_TOV_SECONDS	30
 
 /*
@@ -677,6 +796,15 @@ typedef struct {
 #define MBA_BYPASS_NOTIFICATION	0x8043	/* Auto bypass notification. */
 #define MBA_DISCARD_RND_FRAME	0x8048	/* discard RND frame due to error. */
 #define MBA_REJECTED_FCP_CMD	0x8049	/* rejected FCP_CMD. */
+#define MBA_FW_NOT_STARTED	0x8050	/* Firmware not started */
+#define MBA_FW_STARTING		0x8051	/* Firmware starting */
+#define MBA_FW_RESTART_CMPLT	0x8060	/* Firmware restart complete */
+#define MBA_INIT_REQUIRED	0x8061	/* Initialization required */
+#define MBA_SHUTDOWN_REQUESTED	0x8062	/* Shutdown Requested */
+#define MBA_FW_INIT_FAILURE	0x8401	/* Firmware initialization failure */
+#define MBA_MIRROR_LUN_CHANGE	0x8402	/* Mirror LUN State Change
+					   Notification */
+#define MBA_FW_POLL_STATE	0x8600  /* Firmware in poll diagnostic state */
 
 /* 83XX FCoE specific */
 #define MBA_IDC_AEN		0x8200  /* FCoE: NIC Core state change AEN */
@@ -798,6 +926,12 @@ typedef struct {
 #define MBC_LUN_RESET			0x7E	/* Send LUN reset */
 
 /*
+ * all the Mt. Rainier mailbox command codes that clash with FC/FCoE ones
+ * should be defined with MBC_MR_*
+ */
+#define MBC_MR_DRV_SHUTDOWN		0x6A
+
+/*
  * ISP24xx mailbox commands
  */
 #define MBC_SERDES_PARAMS		0x10	/* Serdes Tx Parameters. */
@@ -1058,6 +1192,30 @@ typedef struct {
 	uint8_t  reserved_3[26];
 } init_cb_t;
 
+
+struct init_cb_fx {
+	uint16_t	version;
+	uint16_t	reserved_1[13];
+	uint16_t	request_q_outpointer;
+	uint16_t	response_q_inpointer;
+	uint16_t	reserved_2[2];
+	uint16_t	response_q_length;
+	uint16_t	request_q_length;
+	uint16_t	reserved_3[2];
+	uint32_t	request_q_address[2];
+	uint32_t	response_q_address[2];
+	uint16_t	reserved_4[4];
+	uint8_t		response_q_msivec;
+	uint8_t		reserved_5[19];
+	uint16_t	interrupt_delay_timer;
+	uint16_t	reserved_6;
+	uint32_t	fwoptions1;
+	uint32_t	fwoptions2;
+	uint32_t	fwoptions3;
+	uint8_t		reserved_7[24];
+};
+
+
 /*
  * Get Link Status mailbox command return buffer.
  */
@@ -1831,6 +1989,9 @@ typedef struct fc_port {
 	uint16_t loop_id;
 	uint16_t old_loop_id;
 
+	uint16_t tgt_id;
+	uint16_t old_tgt_id;
+
 	uint8_t fcp_prio;
 
 	uint8_t fabric_port_name[WWN_SIZE];
@@ -1848,8 +2009,15 @@ typedef struct fc_port {
 
 	uint8_t fc4_type;
 	uint8_t scan_state;
+
+	unsigned long last_queue_full;
+	unsigned long last_ramp_up;
+
+	uint16_t port_id;
 } fc_port_t;
 
+#include "qla_mr.h"
+
 /*
  * Fibre channel port/lun states.
  */
@@ -2391,6 +2559,7 @@ struct isp_operations {
 	int (*start_scsi) (srb_t *);
 	int (*abort_isp) (struct scsi_qla_host *);
 	int (*iospace_config)(struct qla_hw_data*);
+	int (*initialize_adapter)(struct scsi_qla_host *);
 };
 
 /* MSI-X Support *************************************************************/
@@ -2429,6 +2598,7 @@ enum qla_work_type {
 	QLA_EVT_ASYNC_ADISC,
 	QLA_EVT_ASYNC_ADISC_DONE,
 	QLA_EVT_UEVENT,
+	QLA_EVT_AENFX,
 };
 
 
@@ -2456,7 +2626,15 @@ struct qla_work_evt {
 			u32 code;
 #define QLA_UEVENT_CODE_FW_DUMP	0
 		} uevent;
-	} u;
+		struct {
+			uint32_t        evtcode;
+			uint32_t        mbx[8];
+			uint32_t        count;
+		} aenfx;
+		struct {
+			srb_t *sp;
+		} iosb;
+	 } u;
 };
 
 struct qla_chip_state_84xx {
@@ -2520,6 +2698,11 @@ struct rsp_que {
 	struct req_que *req;
 	srb_t *status_srb; /* status continuation entry */
 	struct work_struct q_work;
+
+	dma_addr_t  dma_fx00;
+	response_t *ring_fx00;
+	uint16_t  length_fx00;
+	uint8_t rsp_pkt[REQUEST_ENTRY_SIZE];
 };
 
 /* Request queue data structure */
@@ -2544,6 +2727,11 @@ struct req_que {
 	uint16_t num_outstanding_cmds;
 #define	MAX_Q_DEPTH		32
 	int max_q_depth;
+
+	dma_addr_t  dma_fx00;
+	request_t *ring_fx00;
+	uint16_t  length_fx00;
+	uint8_t req_pkt[REQUEST_ENTRY_SIZE];
 };
 
 /* Place holder for FW buffer parameters */
@@ -2633,7 +2821,10 @@ struct qla_hw_data {
 		uint32_t	isp82xx_no_md_cap:1;
 		uint32_t	host_shutting_down:1;
 		uint32_t	idc_compl_status:1;
-		/* 32 bits */
+
+		uint32_t        mr_reset_hdlr_active:1;
+		uint32_t        mr_intr_valid:1;
+		/* 34 bits */
 	} flags;
 
 	/* This spinlock is used to protect "io transactions", you must
@@ -2650,7 +2841,21 @@ struct qla_hw_data {
 	resource_size_t pio_address;
 
 #define MIN_IOBASE_LEN          0x100
-/* Multi queue data structs */
+	dma_addr_t		bar0_hdl;
+
+	void __iomem *cregbase;
+	dma_addr_t		bar2_hdl;
+#define BAR0_LEN_FX00			(1024 * 1024)
+#define BAR2_LEN_FX00			(128 * 1024)
+
+	uint32_t		rqstq_intr_code;
+	uint32_t		mbx_intr_code;
+	uint32_t		req_que_len;
+	uint32_t		rsp_que_len;
+	uint32_t		req_que_off;
+	uint32_t		rsp_que_off;
+
+	/* Multi queue data structs */
 	device_reg_t __iomem *mqiobase;
 	device_reg_t __iomem *msixbase;
 	uint16_t        msix_count;
@@ -2729,7 +2934,8 @@ struct qla_hw_data {
 #define DT_ISP8021			BIT_14
 #define DT_ISP2031			BIT_15
 #define DT_ISP8031			BIT_16
-#define DT_ISP_LAST			(DT_ISP8031 << 1)
+#define DT_ISPFX00			BIT_17
+#define DT_ISP_LAST			(DT_ISPFX00 << 1)
 
 #define DT_T10_PI                       BIT_25
 #define DT_IIDMA                        BIT_26
@@ -2757,6 +2963,7 @@ struct qla_hw_data {
 #define IS_QLA82XX(ha)	(DT_MASK(ha) & DT_ISP8021)
 #define IS_QLA2031(ha)	(DT_MASK(ha) & DT_ISP2031)
 #define IS_QLA8031(ha)	(DT_MASK(ha) & DT_ISP8031)
+#define IS_QLAFX00(ha)	(DT_MASK(ha) & DT_ISPFX00)
 
 #define IS_QLA23XX(ha)  (IS_QLA2300(ha) || IS_QLA2312(ha) || IS_QLA2322(ha) || \
 			IS_QLA6312(ha) || IS_QLA6322(ha))
@@ -2821,6 +3028,7 @@ struct qla_hw_data {
 	uint16_t	r_a_tov;
 	int		port_down_retry_count;
 	uint8_t		mbx_count;
+	uint8_t		aen_mbx_count;
 
 	uint32_t	login_retry_count;
 	/* SNS command interfaces. */
@@ -2868,9 +3076,13 @@ struct qla_hw_data {
 	void		*swl;
 
 	/* These are used by mailbox operations. */
-	volatile uint16_t mailbox_out[MAILBOX_REGISTER_COUNT];
+	uint16_t mailbox_out[MAILBOX_REGISTER_COUNT];
+	uint32_t mailbox_out32[MAILBOX_REGISTER_COUNT];
+	uint32_t aenmb[AEN_MAILBOX_REGISTER_COUNT_FX00];
 
 	mbx_cmd_t	*mcp;
+	struct mbx_cmd_32	*mcp32;
+
 	unsigned long	mbx_cmd_flags;
 #define MBX_INTERRUPT		1
 #define MBX_INTR_WAIT		2
@@ -3014,6 +3226,7 @@ struct qla_hw_data {
 	int             cur_vport_count;
 
 	struct qla_chip_state_84xx *cs84xx;
+	struct qla_statistics qla_stats;
 	struct isp_operations *isp_ops;
 	struct workqueue_struct *wq;
 	struct qlfc_fw fw_buf;
@@ -3080,6 +3293,8 @@ struct qla_hw_data {
 	unsigned long   host_last_rampup_time;
 	int             cfg_lun_q_depth;
 
+	struct mr_data_fx00 mr;
+
 	struct qlt_hw_data tgt;
 	uint16_t	thermal_support;
 #define THERMAL_SUPPORT_I2C BIT_0
@@ -3109,6 +3324,8 @@ typedef struct scsi_qla_host {
 		uint32_t	process_response_queue	:1;
 		uint32_t	difdix_supported:1;
 		uint32_t	delete_progress:1;
+
+		uint32_t	fw_tgt_reported:1;
 	} flags;
 
 	atomic_t	loop_state;
@@ -3144,6 +3361,9 @@ typedef struct scsi_qla_host {
 #define SCR_PENDING		21	/* SCR in target mode */
 #define HOST_RAMP_DOWN_QUEUE_DEPTH     22
 #define HOST_RAMP_UP_QUEUE_DEPTH       23
+#define PORT_UPDATE_NEEDED	24
+#define FX00_RESET_RECOVERY	25
+#define FX00_TARGET_SCAN	26
 
 	uint32_t	device_flags;
 #define SWITCH_FOUND		BIT_0
@@ -3234,6 +3454,10 @@ struct qla_tgt_vp_map {
 	 test_bit(LOOP_RESYNC_NEEDED, &ha->dpc_flags) || \
 	 atomic_read(&ha->loop_state) == LOOP_DOWN)
 
+#define STATE_TRANSITION(ha) \
+		(test_bit(ISP_ABORT_NEEDED, &ha->dpc_flags) || \
+			 test_bit(LOOP_RESYNC_NEEDED, &ha->dpc_flags))
+
 #define QLA_VHA_MARK_BUSY(__vha, __bail) do {		     \
 	atomic_inc(&__vha->vref_count);			     \
 	mb();						     \
diff --git a/drivers/scsi/qla2xxx/qla_gbl.h b/drivers/scsi/qla2xxx/qla_gbl.h
index b310fa97b545..026bfde33e67 100644
--- a/drivers/scsi/qla2xxx/qla_gbl.h
+++ b/drivers/scsi/qla2xxx/qla_gbl.h
@@ -86,6 +86,7 @@ extern int qla2xxx_mctp_dump(scsi_qla_host_t *);
 
 extern int
 qla2x00_alloc_outstanding_cmds(struct qla_hw_data *, struct req_que *);
+extern int qla2x00_init_rings(scsi_qla_host_t *);
 
 /*
  * Global Data in qla_os.c source file.
@@ -134,7 +135,6 @@ extern int qla2x00_post_async_adisc_work(struct scsi_qla_host *, fc_port_t *,
     uint16_t *);
 extern int qla2x00_post_async_adisc_done_work(struct scsi_qla_host *,
     fc_port_t *, uint16_t *);
-extern int qla2x00_post_uevent_work(struct scsi_qla_host *, u32);
 
 extern int qla81xx_restart_mpi_firmware(scsi_qla_host_t *);
 
@@ -158,6 +158,7 @@ extern int qla83xx_set_drv_presence(scsi_qla_host_t *vha);
 extern int __qla83xx_set_drv_presence(scsi_qla_host_t *vha);
 extern int qla83xx_clear_drv_presence(scsi_qla_host_t *vha);
 extern int __qla83xx_clear_drv_presence(scsi_qla_host_t *vha);
+extern int qla2x00_post_uevent_work(struct scsi_qla_host *, u32);
 
 /*
  * Global Functions in qla_mid.c source file.
@@ -211,8 +212,6 @@ extern int qla24xx_start_scsi(srb_t *sp);
 int qla2x00_marker(struct scsi_qla_host *, struct req_que *, struct rsp_que *,
 						uint16_t, uint16_t, uint8_t);
 extern int qla2x00_start_sp(srb_t *);
-extern uint16_t qla24xx_calc_iocbs(scsi_qla_host_t *, uint16_t);
-extern void qla24xx_build_scsi_iocbs(srb_t *, struct cmd_type_7 *, uint16_t);
 extern int qla24xx_dif_start_scsi(srb_t *);
 extern int qla2x00_start_bidir(srb_t *, struct scsi_qla_host *, uint32_t);
 extern unsigned long qla2x00_get_async_timeout(struct scsi_qla_host *);
@@ -424,6 +423,12 @@ extern void qla2x00_free_irqs(scsi_qla_host_t *);
 
 extern int qla2x00_get_data_rate(scsi_qla_host_t *);
 extern const char *qla2x00_get_link_speed_str(struct qla_hw_data *, uint16_t);
+extern srb_t *
+qla2x00_get_sp_from_handle(scsi_qla_host_t *, const char *, struct req_que *,
+	void *);
+extern void
+qla2x00_process_completed_request(struct scsi_qla_host *, struct req_que *,
+	uint32_t);
 
 /*
  * Global Function Prototypes in qla_sup.c source file.
@@ -561,6 +566,42 @@ extern void qla25xx_wrt_req_reg(struct qla_hw_data *, uint16_t, uint16_t);
 extern void qla25xx_wrt_rsp_reg(struct qla_hw_data *, uint16_t, uint16_t);
 extern void qla24xx_wrt_rsp_reg(struct qla_hw_data *, uint16_t, uint16_t);
 
+/* qlafx00 related functions */
+extern int qlafx00_pci_config(struct scsi_qla_host *);
+extern int qlafx00_initialize_adapter(struct scsi_qla_host *);
+extern void qlafx00_soft_reset(scsi_qla_host_t *);
+extern int qlafx00_chip_diag(scsi_qla_host_t *);
+extern void qlafx00_config_rings(struct scsi_qla_host *);
+extern char *qlafx00_pci_info_str(struct scsi_qla_host *, char *);
+extern char *qlafx00_fw_version_str(struct scsi_qla_host *, char *);
+extern irqreturn_t qlafx00_intr_handler(int, void *);
+extern void qlafx00_enable_intrs(struct qla_hw_data *);
+extern void qlafx00_disable_intrs(struct qla_hw_data *);
+extern int qlafx00_abort_command(srb_t *);
+extern int qlafx00_abort_target(fc_port_t *, unsigned int, int);
+extern int qlafx00_lun_reset(fc_port_t *, unsigned int, int);
+extern int qlafx00_start_scsi(srb_t *);
+extern int qlafx00_abort_isp(scsi_qla_host_t *);
+extern int qlafx00_iospace_config(struct qla_hw_data *);
+extern int qlafx00_init_firmware(scsi_qla_host_t *, uint16_t);
+extern int qlafx00_fw_ready(scsi_qla_host_t *);
+extern int qlafx00_configure_devices(scsi_qla_host_t *);
+extern int qlafx00_reset_initialize(scsi_qla_host_t *);
+extern int qlafx00_fx_disc(scsi_qla_host_t *, fc_port_t *, uint8_t);
+extern int qlafx00_process_aen(struct scsi_qla_host *, struct qla_work_evt *);
+extern int qlafx00_post_aenfx_work(struct scsi_qla_host *,  uint32_t,
+				   uint32_t *, int);
+extern uint32_t qlafx00_fw_state_show(struct device *,
+				      struct device_attribute *, char *);
+extern void qlafx00_get_host_speed(struct Scsi_Host *);
+extern void qlafx00_init_response_q_entries(struct rsp_que *);
+
+extern void qlafx00_tm_iocb(srb_t *, struct tsk_mgmt_entry_fx00 *);
+extern void qlafx00_abort_iocb(srb_t *, struct abort_iocb_entry_fx00 *);
+extern void qlafx00_fxdisc_iocb(srb_t *, struct fxdisc_entry_fx00 *);
+extern void qlafx00_timer_routine(scsi_qla_host_t *);
+extern int qlafx00_rescan_isp(scsi_qla_host_t *);
+
 /* qla82xx related functions */
 
 /* PCI related functions */
diff --git a/drivers/scsi/qla2xxx/qla_gs.c b/drivers/scsi/qla2xxx/qla_gs.c
index 9b455250c101..d0ea8b921177 100644
--- a/drivers/scsi/qla2xxx/qla_gs.c
+++ b/drivers/scsi/qla2xxx/qla_gs.c
@@ -639,9 +639,14 @@ void
 qla2x00_get_sym_node_name(scsi_qla_host_t *vha, uint8_t *snn)
 {
 	struct qla_hw_data *ha = vha->hw;
-	sprintf(snn, "%s FW:v%d.%02d.%02d DVR:v%s",ha->model_number,
-	    ha->fw_major_version, ha->fw_minor_version,
-	    ha->fw_subminor_version, qla2x00_version_str);
+
+	if (IS_QLAFX00(ha))
+		sprintf(snn, "%s FW:v%s DVR:v%s", ha->model_number,
+		    ha->mr.fw_version, qla2x00_version_str);
+	else
+		sprintf(snn, "%s FW:v%d.%02d.%02d DVR:v%s", ha->model_number,
+		    ha->fw_major_version, ha->fw_minor_version,
+		    ha->fw_subminor_version, qla2x00_version_str);
 }
 
 /**
@@ -923,7 +928,7 @@ qla2x00_sns_gpn_id(scsi_qla_host_t *vha, sw_info_t *list)
 		    sns_cmd->p.gpn_data[9] != 0x02) {
 			ql_dbg(ql_dbg_disc + ql_dbg_buffer, vha, 0x207e,
 			    "GPN_ID failed, rejected request, gpn_rsp:\n");
-			ql_dump_buffer(ql_dbg_disc, vha, 0x207f,
+			ql_dump_buffer(ql_dbg_disc + ql_dbg_buffer, vha, 0x207f,
 			    sns_cmd->p.gpn_data, 16);
 			rval = QLA_FUNCTION_FAILED;
 		} else {
@@ -1718,7 +1723,8 @@ qla2x00_fdmi_register(scsi_qla_host_t *vha)
 	int rval;
        struct qla_hw_data *ha = vha->hw;
 
-	if (IS_QLA2100(ha) || IS_QLA2200(ha))
+	if (IS_QLA2100(ha) || IS_QLA2200(ha) ||
+	    IS_QLAFX00(ha))
 		return QLA_FUNCTION_FAILED;
 
 	rval = qla2x00_mgmt_svr_login(vha);
diff --git a/drivers/scsi/qla2xxx/qla_init.c b/drivers/scsi/qla2xxx/qla_init.c
index b59203393cb2..3565dfd8f370 100644
--- a/drivers/scsi/qla2xxx/qla_init.c
+++ b/drivers/scsi/qla2xxx/qla_init.c
@@ -25,7 +25,6 @@
 */
 static int qla2x00_isp_firmware(scsi_qla_host_t *);
 static int qla2x00_setup_chip(scsi_qla_host_t *);
-static int qla2x00_init_rings(scsi_qla_host_t *);
 static int qla2x00_fw_ready(scsi_qla_host_t *);
 static int qla2x00_configure_hba(scsi_qla_host_t *);
 static int qla2x00_configure_loop(scsi_qla_host_t *);
@@ -83,7 +82,9 @@ qla2x00_get_async_timeout(struct scsi_qla_host *vha)
 
 	/* Firmware should use switch negotiated r_a_tov for timeout. */
 	tmo = ha->r_a_tov / 10 * 2;
-	if (!IS_FWI2_CAPABLE(ha)) {
+	if (IS_QLAFX00(ha)) {
+		tmo = FX00_DEF_RATOV * 2;
+	} else if (!IS_FWI2_CAPABLE(ha)) {
 		/*
 		 * Except for earlier ISPs where the timeout is seeded from the
 		 * initialization control block.
@@ -1977,7 +1978,7 @@ qla24xx_config_rings(struct scsi_qla_host *vha)
  *
  * Returns 0 on success.
  */
-static int
+int
 qla2x00_init_rings(scsi_qla_host_t *vha)
 {
 	int	rval;
@@ -2012,7 +2013,10 @@ qla2x00_init_rings(scsi_qla_host_t *vha)
 		if (!rsp)
 			continue;
 		/* Initialize response queue entries */
-		qla2x00_init_response_q_entries(rsp);
+		if (IS_QLAFX00(ha))
+			qlafx00_init_response_q_entries(rsp);
+		else
+			qla2x00_init_response_q_entries(rsp);
 	}
 
 	ha->tgt.atio_ring_ptr = ha->tgt.atio_ring;
@@ -2024,11 +2028,16 @@ qla2x00_init_rings(scsi_qla_host_t *vha)
 
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
+	ql_dbg(ql_dbg_init, vha, 0x00d1, "Issue init firmware.\n");
+
+	if (IS_QLAFX00(ha)) {
+		rval = qlafx00_init_firmware(vha, ha->init_cb_size);
+		goto next_check;
+	}
+
 	/* Update any ISP specific firmware options before initialization. */
 	ha->isp_ops->update_fw_options(vha);
 
-	ql_dbg(ql_dbg_init, vha, 0x00d1, "Issue init firmware.\n");
-
 	if (ha->flags.npiv_supported) {
 		if (ha->operating_mode == LOOP && !IS_CNA_CAPABLE(ha))
 			ha->max_npiv_vports = MIN_MULTI_ID_FABRIC - 1;
@@ -2042,6 +2051,7 @@ qla2x00_init_rings(scsi_qla_host_t *vha)
 	}
 
 	rval = qla2x00_init_firmware(vha, ha->init_cb_size);
+next_check:
 	if (rval) {
 		ql_log(ql_log_fatal, vha, 0x00d2,
 		    "Init Firmware **** FAILED ****.\n");
@@ -2069,6 +2079,9 @@ qla2x00_fw_ready(scsi_qla_host_t *vha)
 	uint16_t	state[5];
 	struct qla_hw_data *ha = vha->hw;
 
+	if (IS_QLAFX00(vha->hw))
+		return qlafx00_fw_ready(vha);
+
 	rval = QLA_SUCCESS;
 
 	/* 20 seconds for loop down. */
@@ -3134,6 +3147,12 @@ void
 qla2x00_update_fcport(scsi_qla_host_t *vha, fc_port_t *fcport)
 {
 	fcport->vha = vha;
+
+	if (IS_QLAFX00(vha->hw)) {
+		qla2x00_set_fcport_state(fcport, FCS_ONLINE);
+		qla2x00_reg_remote_port(vha, fcport);
+		return;
+	}
 	fcport->login_retry = 0;
 	fcport->flags &= ~(FCF_LOGIN_NEEDED | FCF_ASYNC_SENT);
 
@@ -3894,15 +3913,24 @@ qla2x00_loop_resync(scsi_qla_host_t *vha)
 			/* Wait at most MAX_TARGET RSCNs for a stable link. */
 			wait_time = 256;
 			do {
-				/* Issue a marker after FW becomes ready. */
-				qla2x00_marker(vha, req, rsp, 0, 0,
-					MK_SYNC_ALL);
-				vha->marker_needed = 0;
+				if (!IS_QLAFX00(vha->hw)) {
+					/*
+					 * Issue a marker after FW becomes
+					 * ready.
+					 */
+					qla2x00_marker(vha, req, rsp, 0, 0,
+						MK_SYNC_ALL);
+					vha->marker_needed = 0;
+				}
 
 				/* Remap devices on Loop. */
 				clear_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
 
-				qla2x00_configure_loop(vha);
+				if (IS_QLAFX00(vha->hw))
+					qlafx00_configure_devices(vha);
+				else
+					qla2x00_configure_loop(vha);
+
 				wait_time--;
 			} while (!atomic_read(&vha->loop_down_timer) &&
 				!(test_bit(ISP_ABORT_NEEDED, &vha->dpc_flags))
@@ -3968,9 +3996,7 @@ qla2x00_update_fcports(scsi_qla_host_t *base_vha)
 			if (fcport->drport &&
 			    atomic_read(&fcport->state) != FCS_UNCONFIGURED) {
 				spin_unlock_irqrestore(&ha->vport_slock, flags);
-
 				qla2x00_rport_del(fcport);
-
 				spin_lock_irqsave(&ha->vport_slock, flags);
 			}
 		}
diff --git a/drivers/scsi/qla2xxx/qla_inline.h b/drivers/scsi/qla2xxx/qla_inline.h
index 68e2c4afc134..98ab921070d2 100644
--- a/drivers/scsi/qla2xxx/qla_inline.h
+++ b/drivers/scsi/qla2xxx/qla_inline.h
@@ -5,6 +5,28 @@
  * See LICENSE.qla2xxx for copyright and licensing details.
  */
 
+/**
+ * qla24xx_calc_iocbs() - Determine number of Command Type 3 and
+ * Continuation Type 1 IOCBs to allocate.
+ *
+ * @dsds: number of data segment decriptors needed
+ *
+ * Returns the number of IOCB entries needed to store @dsds.
+ */
+static inline uint16_t
+qla24xx_calc_iocbs(scsi_qla_host_t *vha, uint16_t dsds)
+{
+	uint16_t iocbs;
+
+	iocbs = 1;
+	if (dsds > 1) {
+		iocbs += (dsds - 1) / 5;
+		if ((dsds - 1) % 5)
+			iocbs++;
+	}
+	return iocbs;
+}
+
 /*
  * qla2x00_debounce_register
  *      Debounce register.
@@ -58,6 +80,17 @@ host_to_fcp_swap(uint8_t *fcp, uint32_t bsize)
 }
 
 static inline void
+host_to_adap(uint8_t *src, uint8_t *dst, uint32_t bsize)
+{
+	uint32_t *isrc = (uint32_t *) src;
+	uint32_t *odest = (uint32_t *) dst;
+	uint32_t iter = bsize >> 2;
+
+	for (; iter ; iter--)
+		*odest++ = cpu_to_le32(*isrc++);
+}
+
+static inline void
 qla2x00_set_reserved_loop_ids(struct qla_hw_data *ha)
 {
 	int i;
@@ -213,12 +246,18 @@ qla2x00_init_timer(srb_t *sp, unsigned long tmo)
 	sp->u.iocb_cmd.timer.function = qla2x00_sp_timeout;
 	add_timer(&sp->u.iocb_cmd.timer);
 	sp->free = qla2x00_sp_free;
+	if ((IS_QLAFX00(sp->fcport->vha->hw)) &&
+	    (sp->type == SRB_FXIOCB_DCMD))
+		init_completion(&sp->u.iocb_cmd.u.fxiocb.fxiocb_comp);
 }
 
 static inline int
 qla2x00_gid_list_size(struct qla_hw_data *ha)
 {
-	return sizeof(struct gid_list_info) * ha->max_fibre_devices;
+	if (IS_QLAFX00(ha))
+		return sizeof(uint32_t) * 32;
+	else
+		return sizeof(struct gid_list_info) * ha->max_fibre_devices;
 }
 
 static inline void
diff --git a/drivers/scsi/qla2xxx/qla_iocb.c b/drivers/scsi/qla2xxx/qla_iocb.c
index d2630317cce8..15e4080b347c 100644
--- a/drivers/scsi/qla2xxx/qla_iocb.c
+++ b/drivers/scsi/qla2xxx/qla_iocb.c
@@ -135,7 +135,8 @@ qla2x00_prep_cont_type1_iocb(scsi_qla_host_t *vha, struct req_que *req)
 	cont_pkt = (cont_a64_entry_t *)req->ring_ptr;
 
 	/* Load packet defaults. */
-	*((uint32_t *)(&cont_pkt->entry_type)) =
+	*((uint32_t *)(&cont_pkt->entry_type)) = IS_QLAFX00(vha->hw) ?
+	    __constant_cpu_to_le32(CONTINUE_A64_TYPE_FX00) :
 	    __constant_cpu_to_le32(CONTINUE_A64_TYPE);
 
 	return (cont_pkt);
@@ -486,6 +487,10 @@ qla2x00_start_iocbs(struct scsi_qla_host *vha, struct req_que *req)
 		if (ha->mqenable || IS_QLA83XX(ha)) {
 			WRT_REG_DWORD(req->req_q_in, req->ring_index);
 			RD_REG_DWORD_RELAXED(&ha->iobase->isp24.hccr);
+		} else if (IS_QLAFX00(ha)) {
+			WRT_REG_DWORD(&reg->ispfx00.req_q_in, req->ring_index);
+			RD_REG_DWORD_RELAXED(&reg->ispfx00.req_q_in);
+			QLAFX00_SET_HST_INTR(ha, ha->rqstq_intr_code);
 		} else if (IS_FWI2_CAPABLE(ha)) {
 			WRT_REG_DWORD(&reg->isp24.req_q_in, req->ring_index);
 			RD_REG_DWORD_RELAXED(&reg->isp24.req_q_in);
@@ -514,11 +519,12 @@ __qla2x00_marker(struct scsi_qla_host *vha, struct req_que *req,
 			uint16_t lun, uint8_t type)
 {
 	mrk_entry_t *mrk;
-	struct mrk_entry_24xx *mrk24;
+	struct mrk_entry_24xx *mrk24 = NULL;
+	struct mrk_entry_fx00 *mrkfx = NULL;
+
 	struct qla_hw_data *ha = vha->hw;
 	scsi_qla_host_t *base_vha = pci_get_drvdata(ha->pdev);
 
-	mrk24 = NULL;
 	req = ha->req_q_map[0];
 	mrk = (mrk_entry_t *)qla2x00_alloc_iocbs(vha, NULL);
 	if (mrk == NULL) {
@@ -531,7 +537,15 @@ __qla2x00_marker(struct scsi_qla_host *vha, struct req_que *req,
 	mrk->entry_type = MARKER_TYPE;
 	mrk->modifier = type;
 	if (type != MK_SYNC_ALL) {
-		if (IS_FWI2_CAPABLE(ha)) {
+		if (IS_QLAFX00(ha)) {
+			mrkfx = (struct mrk_entry_fx00 *) mrk;
+			mrkfx->handle = MAKE_HANDLE(req->id, mrkfx->handle);
+			mrkfx->handle_hi = 0;
+			mrkfx->tgt_id = cpu_to_le16(loop_id);
+			mrkfx->lun[1] = LSB(lun);
+			mrkfx->lun[2] = MSB(lun);
+			host_to_fcp_swap(mrkfx->lun, sizeof(mrkfx->lun));
+		} else if (IS_FWI2_CAPABLE(ha)) {
 			mrk24 = (struct mrk_entry_24xx *) mrk;
 			mrk24->nport_handle = cpu_to_le16(loop_id);
 			mrk24->lun[1] = LSB(lun);
@@ -589,28 +603,6 @@ int qla2x00_issue_marker(scsi_qla_host_t *vha, int ha_locked)
 	return QLA_SUCCESS;
 }
 
-/**
- * qla24xx_calc_iocbs() - Determine number of Command Type 3 and
- * Continuation Type 1 IOCBs to allocate.
- *
- * @dsds: number of data segment decriptors needed
- *
- * Returns the number of IOCB entries needed to store @dsds.
- */
-inline uint16_t
-qla24xx_calc_iocbs(scsi_qla_host_t *vha, uint16_t dsds)
-{
-	uint16_t iocbs;
-
-	iocbs = 1;
-	if (dsds > 1) {
-		iocbs += (dsds - 1) / 5;
-		if ((dsds - 1) % 5)
-			iocbs++;
-	}
-	return iocbs;
-}
-
 static inline int
 qla24xx_build_scsi_type_6_iocbs(srb_t *sp, struct cmd_type_6 *cmd_pkt,
 	uint16_t tot_dsds)
@@ -1583,7 +1575,6 @@ queuing_error:
 	return QLA_FUNCTION_FAILED;
 }
 
-
 /**
  * qla24xx_dif_start_scsi() - Send a SCSI command to the ISP
  * @sp: command to send to the ISP
@@ -1852,6 +1843,8 @@ skip_cmd_array:
 			cnt = RD_REG_DWORD(&reg->isp82.req_q_out);
 		else if (IS_FWI2_CAPABLE(ha))
 			cnt = RD_REG_DWORD(&reg->isp24.req_q_out);
+		else if (IS_QLAFX00(ha))
+			cnt = RD_REG_DWORD(&reg->ispfx00.req_q_out);
 		else
 			cnt = qla2x00_debounce_register(
 			    ISP_REQ_Q_OUT(ha, &reg->isp));
@@ -1869,8 +1862,13 @@ skip_cmd_array:
 	req->cnt -= req_cnt;
 	pkt = req->ring_ptr;
 	memset(pkt, 0, REQUEST_ENTRY_SIZE);
-	pkt->entry_count = req_cnt;
-	pkt->handle = handle;
+	if (IS_QLAFX00(ha)) {
+		WRT_REG_BYTE(&pkt->entry_count, req_cnt);
+		WRT_REG_WORD(&pkt->handle, handle);
+	} else {
+		pkt->entry_count = req_cnt;
+		pkt->handle = handle;
+	}
 
 queuing_error:
 	return pkt;
@@ -2625,7 +2623,16 @@ qla2x00_start_sp(srb_t *sp)
 		    qla2x00_adisc_iocb(sp, pkt);
 		break;
 	case SRB_TM_CMD:
-		qla24xx_tm_iocb(sp, pkt);
+		IS_QLAFX00(ha) ?
+		    qlafx00_tm_iocb(sp, pkt) :
+		    qla24xx_tm_iocb(sp, pkt);
+		break;
+	case SRB_FXIOCB_DCMD:
+	case SRB_FXIOCB_BCMD:
+		qlafx00_fxdisc_iocb(sp, pkt);
+		break;
+	case SRB_ABT_CMD:
+		qlafx00_abort_iocb(sp, pkt);
 		break;
 	default:
 		break;
diff --git a/drivers/scsi/qla2xxx/qla_isr.c b/drivers/scsi/qla2xxx/qla_isr.c
index e9dbd74c20d3..259d9205d876 100644
--- a/drivers/scsi/qla2xxx/qla_isr.c
+++ b/drivers/scsi/qla2xxx/qla_isr.c
@@ -13,11 +13,7 @@
 #include <scsi/scsi_bsg_fc.h>
 #include <scsi/scsi_eh.h>
 
-#include "qla_target.h"
-
 static void qla2x00_mbx_completion(scsi_qla_host_t *, uint16_t);
-static void qla2x00_process_completed_request(struct scsi_qla_host *,
-	struct req_que *, uint32_t);
 static void qla2x00_status_entry(scsi_qla_host_t *, struct rsp_que *, void *);
 static void qla2x00_status_cont_entry(struct rsp_que *, sts_cont_entry_t *);
 static void qla2x00_error_entry(scsi_qla_host_t *, struct rsp_que *,
@@ -1065,9 +1061,9 @@ skip_rio:
  * @ha: SCSI driver HA context
  * @index: SRB index
  */
-static void
+void
 qla2x00_process_completed_request(struct scsi_qla_host *vha,
-				struct req_que *req, uint32_t index)
+				  struct req_que *req, uint32_t index)
 {
 	srb_t *sp;
 	struct qla_hw_data *ha = vha->hw;
@@ -1101,7 +1097,7 @@ qla2x00_process_completed_request(struct scsi_qla_host *vha,
 	}
 }
 
-static srb_t *
+srb_t *
 qla2x00_get_sp_from_handle(scsi_qla_host_t *vha, const char *func,
     struct req_que *req, void *iocb)
 {
@@ -1994,7 +1990,7 @@ qla2x00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
 		return;
 	}
 
-  	lscsi_status = scsi_status & STATUS_MASK;
+	lscsi_status = scsi_status & STATUS_MASK;
 
 	fcport = sp->fcport;
 
@@ -2939,7 +2935,7 @@ qla2x00_request_irqs(struct qla_hw_data *ha, struct rsp_que *rsp)
 
 	/* If possible, enable MSI-X. */
 	if (!IS_QLA2432(ha) && !IS_QLA2532(ha) && !IS_QLA8432(ha) &&
-		!IS_CNA_CAPABLE(ha) && !IS_QLA2031(ha))
+		!IS_CNA_CAPABLE(ha) && !IS_QLA2031(ha) && !IS_QLAFX00(ha))
 		goto skip_msi;
 
 	if (ha->pdev->subsystem_vendor == PCI_VENDOR_ID_HP &&
@@ -2972,7 +2968,7 @@ qla2x00_request_irqs(struct qla_hw_data *ha, struct rsp_que *rsp)
 skip_msix:
 
 	if (!IS_QLA24XX(ha) && !IS_QLA2532(ha) && !IS_QLA8432(ha) &&
-	    !IS_QLA8001(ha) && !IS_QLA82XX(ha))
+	    !IS_QLA8001(ha) && !IS_QLA82XX(ha) && !IS_QLAFX00(ha))
 		goto skip_msi;
 
 	ret = pci_enable_msi(ha->pdev);
@@ -2998,9 +2994,11 @@ skip_msi:
 		    "Failed to reserve interrupt %d already in use.\n",
 		    ha->pdev->irq);
 		goto fail;
-	} else if (!ha->flags.msi_enabled)
+	} else if (!ha->flags.msi_enabled) {
 		ql_dbg(ql_dbg_init, vha, 0x0125,
 		    "INTa mode: Enabled.\n");
+		ha->flags.mr_intr_valid = 1;
+	}
 
 clear_risc_ints:
 
diff --git a/drivers/scsi/qla2xxx/qla_mbx.c b/drivers/scsi/qla2xxx/qla_mbx.c
index 43345af56431..9e5d89db7272 100644
--- a/drivers/scsi/qla2xxx/qla_mbx.c
+++ b/drivers/scsi/qla2xxx/qla_mbx.c
@@ -4113,7 +4113,6 @@ qla2x00_loopback_test(scsi_qla_host_t *vha, struct msg_echo_lb *mreq,
 	int rval;
 	mbx_cmd_t mc;
 	mbx_cmd_t *mcp = &mc;
-	uint32_t iter_cnt = 0x1;
 
 	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x10f7,
 	    "Entered %s.\n", __func__);
@@ -4139,8 +4138,8 @@ qla2x00_loopback_test(scsi_qla_host_t *vha, struct msg_echo_lb *mreq,
 	mcp->mb[7] = MSW(MSD(mreq->rcv_dma));
 
 	/* Iteration count */
-	mcp->mb[18] = LSW(iter_cnt);
-	mcp->mb[19] = MSW(iter_cnt);
+	mcp->mb[18] = LSW(mreq->iteration_count);
+	mcp->mb[19] = MSW(mreq->iteration_count);
 
 	mcp->out_mb = MBX_21|MBX_20|MBX_19|MBX_18|MBX_17|MBX_16|MBX_15|
 	    MBX_14|MBX_13|MBX_12|MBX_11|MBX_10|MBX_7|MBX_6|MBX_1|MBX_0;
@@ -4518,7 +4517,8 @@ qla2x00_get_thermal_temp(scsi_qla_host_t *vha, uint16_t *temp)
 			goto done;
 
 		ql_log(ql_log_warn, vha, 0x10c9,
-		    "Thermal not supported by I2C.\n");
+		    "Thermal not supported through I2C bus, trying alternate "
+		    "method (ISP access).\n");
 		ha->thermal_support &= ~THERMAL_SUPPORT_I2C;
 	}
 
@@ -4528,7 +4528,7 @@ qla2x00_get_thermal_temp(scsi_qla_host_t *vha, uint16_t *temp)
 			goto done;
 
 		ql_log(ql_log_warn, vha, 0x1019,
-		    "Thermal not supported by ISP.\n");
+		    "Thermal not supported through ISP.\n");
 		ha->thermal_support &= ~THERMAL_SUPPORT_ISP;
 	}
 
diff --git a/drivers/scsi/qla2xxx/qla_mr.c b/drivers/scsi/qla2xxx/qla_mr.c
new file mode 100644
index 000000000000..729b74389f83
--- /dev/null
+++ b/drivers/scsi/qla2xxx/qla_mr.c
@@ -0,0 +1,3476 @@
+/*
+ * QLogic Fibre Channel HBA Driver
+ * Copyright (c)  2003-2013 QLogic Corporation
+ *
+ * See LICENSE.qla2xxx for copyright and licensing details.
+ */
+#include "qla_def.h"
+#include <linux/delay.h>
+#include <linux/pci.h>
+#include <linux/ratelimit.h>
+#include <linux/vmalloc.h>
+#include <scsi/scsi_tcq.h>
+#include <linux/utsname.h>
+
+
+/* QLAFX00 specific Mailbox implementation functions */
+
+/*
+ * qlafx00_mailbox_command
+ *	Issue mailbox command and waits for completion.
+ *
+ * Input:
+ *	ha = adapter block pointer.
+ *	mcp = driver internal mbx struct pointer.
+ *
+ * Output:
+ *	mb[MAX_MAILBOX_REGISTER_COUNT] = returned mailbox data.
+ *
+ * Returns:
+ *	0 : QLA_SUCCESS = cmd performed success
+ *	1 : QLA_FUNCTION_FAILED   (error encountered)
+ *	6 : QLA_FUNCTION_TIMEOUT (timeout condition encountered)
+ *
+ * Context:
+ *	Kernel context.
+ */
+static int
+qlafx00_mailbox_command(scsi_qla_host_t *vha, struct mbx_cmd_32 *mcp)
+
+{
+	int		rval;
+	unsigned long    flags = 0;
+	device_reg_t __iomem *reg;
+	uint8_t		abort_active;
+	uint8_t		io_lock_on;
+	uint16_t	command = 0;
+	uint32_t	*iptr;
+	uint32_t __iomem *optr;
+	uint32_t	cnt;
+	uint32_t	mboxes;
+	unsigned long	wait_time;
+	struct qla_hw_data *ha = vha->hw;
+	scsi_qla_host_t *base_vha = pci_get_drvdata(ha->pdev);
+
+	if (ha->pdev->error_state > pci_channel_io_frozen) {
+		ql_log(ql_log_warn, vha, 0x115c,
+		    "error_state is greater than pci_channel_io_frozen, "
+		    "exiting.\n");
+		return QLA_FUNCTION_TIMEOUT;
+	}
+
+	if (vha->device_flags & DFLG_DEV_FAILED) {
+		ql_log(ql_log_warn, vha, 0x115f,
+		    "Device in failed state, exiting.\n");
+		return QLA_FUNCTION_TIMEOUT;
+	}
+
+	reg = ha->iobase;
+	io_lock_on = base_vha->flags.init_done;
+
+	rval = QLA_SUCCESS;
+	abort_active = test_bit(ABORT_ISP_ACTIVE, &base_vha->dpc_flags);
+
+	if (ha->flags.pci_channel_io_perm_failure) {
+		ql_log(ql_log_warn, vha, 0x1175,
+		    "Perm failure on EEH timeout MBX, exiting.\n");
+		return QLA_FUNCTION_TIMEOUT;
+	}
+
+	if (ha->flags.isp82xx_fw_hung) {
+		/* Setting Link-Down error */
+		mcp->mb[0] = MBS_LINK_DOWN_ERROR;
+		ql_log(ql_log_warn, vha, 0x1176,
+		    "FW hung = %d.\n", ha->flags.isp82xx_fw_hung);
+		rval = QLA_FUNCTION_FAILED;
+		goto premature_exit;
+	}
+
+	/*
+	 * Wait for active mailbox commands to finish by waiting at most tov
+	 * seconds. This is to serialize actual issuing of mailbox cmds during
+	 * non ISP abort time.
+	 */
+	if (!wait_for_completion_timeout(&ha->mbx_cmd_comp, mcp->tov * HZ)) {
+		/* Timeout occurred. Return error. */
+		ql_log(ql_log_warn, vha, 0x1177,
+		    "Cmd access timeout, cmd=0x%x, Exiting.\n",
+		    mcp->mb[0]);
+		return QLA_FUNCTION_TIMEOUT;
+	}
+
+	ha->flags.mbox_busy = 1;
+	/* Save mailbox command for debug */
+	ha->mcp32 = mcp;
+
+	ql_dbg(ql_dbg_mbx, vha, 0x1178,
+	    "Prepare to issue mbox cmd=0x%x.\n", mcp->mb[0]);
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+
+	/* Load mailbox registers. */
+	optr = (uint32_t __iomem *)&reg->ispfx00.mailbox0;
+
+	iptr = mcp->mb;
+	command = mcp->mb[0];
+	mboxes = mcp->out_mb;
+
+	for (cnt = 0; cnt < ha->mbx_count; cnt++) {
+		if (mboxes & BIT_0)
+			WRT_REG_DWORD(optr, *iptr);
+
+		mboxes >>= 1;
+		optr++;
+		iptr++;
+	}
+
+	/* Issue set host interrupt command to send cmd out. */
+	ha->flags.mbox_int = 0;
+	clear_bit(MBX_INTERRUPT, &ha->mbx_cmd_flags);
+
+	ql_dump_buffer(ql_dbg_mbx + ql_dbg_buffer, vha, 0x1172,
+	    (uint8_t *)mcp->mb, 16);
+	ql_dump_buffer(ql_dbg_mbx + ql_dbg_buffer, vha, 0x1173,
+	    ((uint8_t *)mcp->mb + 0x10), 16);
+	ql_dump_buffer(ql_dbg_mbx + ql_dbg_buffer, vha, 0x1174,
+	    ((uint8_t *)mcp->mb + 0x20), 8);
+
+	/* Unlock mbx registers and wait for interrupt */
+	ql_dbg(ql_dbg_mbx, vha, 0x1179,
+	    "Going to unlock irq & waiting for interrupts. "
+	    "jiffies=%lx.\n", jiffies);
+
+	/* Wait for mbx cmd completion until timeout */
+	if ((!abort_active && io_lock_on) || IS_NOPOLLING_TYPE(ha)) {
+		set_bit(MBX_INTR_WAIT, &ha->mbx_cmd_flags);
+
+		QLAFX00_SET_HST_INTR(ha, ha->mbx_intr_code);
+		spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
+		wait_for_completion_timeout(&ha->mbx_intr_comp, mcp->tov * HZ);
+
+		clear_bit(MBX_INTR_WAIT, &ha->mbx_cmd_flags);
+
+	} else {
+		ql_dbg(ql_dbg_mbx, vha, 0x112c,
+		    "Cmd=%x Polling Mode.\n", command);
+
+		QLAFX00_SET_HST_INTR(ha, ha->mbx_intr_code);
+		spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
+		wait_time = jiffies + mcp->tov * HZ; /* wait at most tov secs */
+		while (!ha->flags.mbox_int) {
+			if (time_after(jiffies, wait_time))
+				break;
+
+			/* Check for pending interrupts. */
+			qla2x00_poll(ha->rsp_q_map[0]);
+
+			if (!ha->flags.mbox_int &&
+			    !(IS_QLA2200(ha) &&
+			    command == MBC_LOAD_RISC_RAM_EXTENDED))
+				usleep_range(10000, 11000);
+		} /* while */
+		ql_dbg(ql_dbg_mbx, vha, 0x112d,
+		    "Waited %d sec.\n",
+		    (uint)((jiffies - (wait_time - (mcp->tov * HZ)))/HZ));
+	}
+
+	/* Check whether we timed out */
+	if (ha->flags.mbox_int) {
+		uint32_t *iptr2;
+
+		ql_dbg(ql_dbg_mbx, vha, 0x112e,
+		    "Cmd=%x completed.\n", command);
+
+		/* Got interrupt. Clear the flag. */
+		ha->flags.mbox_int = 0;
+		clear_bit(MBX_INTERRUPT, &ha->mbx_cmd_flags);
+
+		if (ha->mailbox_out32[0] != MBS_COMMAND_COMPLETE)
+			rval = QLA_FUNCTION_FAILED;
+
+		/* Load return mailbox registers. */
+		iptr2 = mcp->mb;
+		iptr = (uint32_t *)&ha->mailbox_out32[0];
+		mboxes = mcp->in_mb;
+		for (cnt = 0; cnt < ha->mbx_count; cnt++) {
+			if (mboxes & BIT_0)
+				*iptr2 = *iptr;
+
+			mboxes >>= 1;
+			iptr2++;
+			iptr++;
+		}
+	} else {
+
+		rval = QLA_FUNCTION_TIMEOUT;
+	}
+
+	ha->flags.mbox_busy = 0;
+
+	/* Clean up */
+	ha->mcp32 = NULL;
+
+	if ((abort_active || !io_lock_on) && !IS_NOPOLLING_TYPE(ha)) {
+		ql_dbg(ql_dbg_mbx, vha, 0x113a,
+		    "checking for additional resp interrupt.\n");
+
+		/* polling mode for non isp_abort commands. */
+		qla2x00_poll(ha->rsp_q_map[0]);
+	}
+
+	if (rval == QLA_FUNCTION_TIMEOUT &&
+	    mcp->mb[0] != MBC_GEN_SYSTEM_ERROR) {
+		if (!io_lock_on || (mcp->flags & IOCTL_CMD) ||
+		    ha->flags.eeh_busy) {
+			/* not in dpc. schedule it for dpc to take over. */
+			ql_dbg(ql_dbg_mbx, vha, 0x115d,
+			    "Timeout, schedule isp_abort_needed.\n");
+
+			if (!test_bit(ISP_ABORT_NEEDED, &vha->dpc_flags) &&
+			    !test_bit(ABORT_ISP_ACTIVE, &vha->dpc_flags) &&
+			    !test_bit(ISP_ABORT_RETRY, &vha->dpc_flags)) {
+
+				ql_log(ql_log_info, base_vha, 0x115e,
+				    "Mailbox cmd timeout occurred, cmd=0x%x, "
+				    "mb[0]=0x%x, eeh_busy=0x%x. Scheduling ISP "
+				    "abort.\n", command, mcp->mb[0],
+				    ha->flags.eeh_busy);
+				set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+				qla2xxx_wake_dpc(vha);
+			}
+		} else if (!abort_active) {
+			/* call abort directly since we are in the DPC thread */
+			ql_dbg(ql_dbg_mbx, vha, 0x1160,
+			    "Timeout, calling abort_isp.\n");
+
+			if (!test_bit(ISP_ABORT_NEEDED, &vha->dpc_flags) &&
+			    !test_bit(ABORT_ISP_ACTIVE, &vha->dpc_flags) &&
+			    !test_bit(ISP_ABORT_RETRY, &vha->dpc_flags)) {
+
+				ql_log(ql_log_info, base_vha, 0x1161,
+				    "Mailbox cmd timeout occurred, cmd=0x%x, "
+				    "mb[0]=0x%x. Scheduling ISP abort ",
+				    command, mcp->mb[0]);
+
+				set_bit(ABORT_ISP_ACTIVE, &vha->dpc_flags);
+				clear_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+				if (ha->isp_ops->abort_isp(vha)) {
+					/* Failed. retry later. */
+					set_bit(ISP_ABORT_NEEDED,
+					    &vha->dpc_flags);
+				}
+				clear_bit(ABORT_ISP_ACTIVE, &vha->dpc_flags);
+				ql_dbg(ql_dbg_mbx, vha, 0x1162,
+				    "Finished abort_isp.\n");
+			}
+		}
+	}
+
+premature_exit:
+	/* Allow next mbx cmd to come in. */
+	complete(&ha->mbx_cmd_comp);
+
+	if (rval) {
+		ql_log(ql_log_warn, base_vha, 0x1163,
+		    "**** Failed mbx[0]=%x, mb[1]=%x, mb[2]=%x, "
+		    "mb[3]=%x, cmd=%x ****.\n",
+		    mcp->mb[0], mcp->mb[1], mcp->mb[2], mcp->mb[3], command);
+	} else {
+		ql_dbg(ql_dbg_mbx, base_vha, 0x1164, "Done %s.\n", __func__);
+	}
+
+	return rval;
+}
+
+/*
+ * qlafx00_driver_shutdown
+ *	Indicate a driver shutdown to firmware.
+ *
+ * Input:
+ *	ha = adapter block pointer.
+ *
+ * Returns:
+ *	local function return status code.
+ *
+ * Context:
+ *	Kernel context.
+ */
+static int
+qlafx00_driver_shutdown(scsi_qla_host_t *vha, int tmo)
+{
+	int rval;
+	struct mbx_cmd_32 mc;
+	struct mbx_cmd_32 *mcp = &mc;
+
+	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1166,
+	    "Entered %s.\n", __func__);
+
+	mcp->mb[0] = MBC_MR_DRV_SHUTDOWN;
+	mcp->out_mb = MBX_0;
+	mcp->in_mb = MBX_0;
+	if (tmo)
+		mcp->tov = tmo;
+	else
+		mcp->tov = MBX_TOV_SECONDS;
+	mcp->flags = 0;
+	rval = qlafx00_mailbox_command(vha, mcp);
+
+	if (rval != QLA_SUCCESS) {
+		ql_dbg(ql_dbg_mbx, vha, 0x1167,
+		    "Failed=%x.\n", rval);
+	} else {
+		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1168,
+		    "Done %s.\n", __func__);
+	}
+
+	return rval;
+}
+
+/*
+ * qlafx00_get_firmware_state
+ *	Get adapter firmware state.
+ *
+ * Input:
+ *	ha = adapter block pointer.
+ *	TARGET_QUEUE_LOCK must be released.
+ *	ADAPTER_STATE_LOCK must be released.
+ *
+ * Returns:
+ *	qla7xxx local function return status code.
+ *
+ * Context:
+ *	Kernel context.
+ */
+static int
+qlafx00_get_firmware_state(scsi_qla_host_t *vha, uint32_t *states)
+{
+	int rval;
+	struct mbx_cmd_32 mc;
+	struct mbx_cmd_32 *mcp = &mc;
+
+	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1169,
+	    "Entered %s.\n", __func__);
+
+	mcp->mb[0] = MBC_GET_FIRMWARE_STATE;
+	mcp->out_mb = MBX_0;
+	mcp->in_mb = MBX_1|MBX_0;
+	mcp->tov = MBX_TOV_SECONDS;
+	mcp->flags = 0;
+	rval = qlafx00_mailbox_command(vha, mcp);
+
+	/* Return firmware states. */
+	states[0] = mcp->mb[1];
+
+	if (rval != QLA_SUCCESS) {
+		ql_dbg(ql_dbg_mbx, vha, 0x116a,
+		    "Failed=%x mb[0]=%x.\n", rval, mcp->mb[0]);
+	} else {
+		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x116b,
+		    "Done %s.\n", __func__);
+	}
+	return rval;
+}
+
+/*
+ * qlafx00_init_firmware
+ *	Initialize adapter firmware.
+ *
+ * Input:
+ *	ha = adapter block pointer.
+ *	dptr = Initialization control block pointer.
+ *	size = size of initialization control block.
+ *	TARGET_QUEUE_LOCK must be released.
+ *	ADAPTER_STATE_LOCK must be released.
+ *
+ * Returns:
+ *	qlafx00 local function return status code.
+ *
+ * Context:
+ *	Kernel context.
+ */
+int
+qlafx00_init_firmware(scsi_qla_host_t *vha, uint16_t size)
+{
+	int rval;
+	struct mbx_cmd_32 mc;
+	struct mbx_cmd_32 *mcp = &mc;
+	struct qla_hw_data *ha = vha->hw;
+
+	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x116c,
+	    "Entered %s.\n", __func__);
+
+	mcp->mb[0] = MBC_INITIALIZE_FIRMWARE;
+
+	mcp->mb[1] = 0;
+	mcp->mb[2] = MSD(ha->init_cb_dma);
+	mcp->mb[3] = LSD(ha->init_cb_dma);
+
+	mcp->out_mb = MBX_3|MBX_2|MBX_1|MBX_0;
+	mcp->in_mb = MBX_0;
+	mcp->buf_size = size;
+	mcp->flags = MBX_DMA_OUT;
+	mcp->tov = MBX_TOV_SECONDS;
+	rval = qlafx00_mailbox_command(vha, mcp);
+
+	if (rval != QLA_SUCCESS) {
+		ql_dbg(ql_dbg_mbx, vha, 0x116d,
+		    "Failed=%x mb[0]=%x.\n", rval, mcp->mb[0]);
+	} else {
+		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x116e,
+		    "Done %s.\n", __func__);
+	}
+	return rval;
+}
+
+/*
+ * qlafx00_mbx_reg_test
+ */
+static int
+qlafx00_mbx_reg_test(scsi_qla_host_t *vha)
+{
+	int rval;
+	struct mbx_cmd_32 mc;
+	struct mbx_cmd_32 *mcp = &mc;
+
+	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x116f,
+	    "Entered %s.\n", __func__);
+
+
+	mcp->mb[0] = MBC_MAILBOX_REGISTER_TEST;
+	mcp->mb[1] = 0xAAAA;
+	mcp->mb[2] = 0x5555;
+	mcp->mb[3] = 0xAA55;
+	mcp->mb[4] = 0x55AA;
+	mcp->mb[5] = 0xA5A5;
+	mcp->mb[6] = 0x5A5A;
+	mcp->mb[7] = 0x2525;
+	mcp->mb[8] = 0xBBBB;
+	mcp->mb[9] = 0x6666;
+	mcp->mb[10] = 0xBB66;
+	mcp->mb[11] = 0x66BB;
+	mcp->mb[12] = 0xB6B6;
+	mcp->mb[13] = 0x6B6B;
+	mcp->mb[14] = 0x3636;
+	mcp->mb[15] = 0xCCCC;
+
+
+	mcp->out_mb = MBX_15|MBX_14|MBX_13|MBX_12|MBX_11|MBX_10|MBX_9|MBX_8|
+			MBX_7|MBX_6|MBX_5|MBX_4|MBX_3|MBX_2|MBX_1|MBX_0;
+	mcp->in_mb = MBX_15|MBX_14|MBX_13|MBX_12|MBX_11|MBX_10|MBX_9|MBX_8|
+			MBX_7|MBX_6|MBX_5|MBX_4|MBX_3|MBX_2|MBX_1|MBX_0;
+	mcp->buf_size = 0;
+	mcp->flags = MBX_DMA_OUT;
+	mcp->tov = MBX_TOV_SECONDS;
+	rval = qlafx00_mailbox_command(vha, mcp);
+	if (rval == QLA_SUCCESS) {
+		if (mcp->mb[17] != 0xAAAA || mcp->mb[18] != 0x5555 ||
+		    mcp->mb[19] != 0xAA55 || mcp->mb[20] != 0x55AA)
+			rval = QLA_FUNCTION_FAILED;
+		if (mcp->mb[21] != 0xA5A5 || mcp->mb[22] != 0x5A5A ||
+		    mcp->mb[23] != 0x2525 || mcp->mb[24] != 0xBBBB)
+			rval = QLA_FUNCTION_FAILED;
+		if (mcp->mb[25] != 0x6666 || mcp->mb[26] != 0xBB66 ||
+		    mcp->mb[27] != 0x66BB || mcp->mb[28] != 0xB6B6)
+			rval = QLA_FUNCTION_FAILED;
+		if (mcp->mb[29] != 0x6B6B || mcp->mb[30] != 0x3636 ||
+		    mcp->mb[31] != 0xCCCC)
+			rval = QLA_FUNCTION_FAILED;
+	}
+
+	if (rval != QLA_SUCCESS) {
+		ql_dbg(ql_dbg_mbx, vha, 0x1170,
+		    "Failed=%x mb[0]=%x.\n", rval, mcp->mb[0]);
+	} else {
+		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1171,
+		    "Done %s.\n", __func__);
+	}
+	return rval;
+}
+
+/**
+ * qlafx00_pci_config() - Setup ISPFx00 PCI configuration registers.
+ * @ha: HA context
+ *
+ * Returns 0 on success.
+ */
+int
+qlafx00_pci_config(scsi_qla_host_t *vha)
+{
+	uint16_t w;
+	struct qla_hw_data *ha = vha->hw;
+
+	pci_set_master(ha->pdev);
+	pci_try_set_mwi(ha->pdev);
+
+	pci_read_config_word(ha->pdev, PCI_COMMAND, &w);
+	w |= (PCI_COMMAND_PARITY | PCI_COMMAND_SERR);
+	w &= ~PCI_COMMAND_INTX_DISABLE;
+	pci_write_config_word(ha->pdev, PCI_COMMAND, w);
+
+	/* PCIe -- adjust Maximum Read Request Size (2048). */
+	if (pci_find_capability(ha->pdev, PCI_CAP_ID_EXP))
+		pcie_set_readrq(ha->pdev, 2048);
+
+	ha->chip_revision = ha->pdev->revision;
+
+	return QLA_SUCCESS;
+}
+
+/**
+ * qlafx00_warm_reset() - Perform warm reset of iSA(CPUs being reset on SOC).
+ * @ha: HA context
+ *
+  */
+static inline void
+qlafx00_soc_cpu_reset(scsi_qla_host_t *vha)
+{
+	unsigned long flags = 0;
+	struct qla_hw_data *ha = vha->hw;
+	int i, core;
+	uint32_t cnt;
+
+	/* Set all 4 cores in reset */
+	for (i = 0; i < 4; i++) {
+		QLAFX00_SET_HBA_SOC_REG(ha,
+		    (SOC_SW_RST_CONTROL_REG_CORE0 + 8*i), (0xF01));
+	}
+
+	/* Set all 4 core Clock gating control */
+	for (i = 0; i < 4; i++) {
+		QLAFX00_SET_HBA_SOC_REG(ha,
+		    (SOC_SW_RST_CONTROL_REG_CORE0 + 4 + 8*i), (0x01010101));
+	}
+
+	/* Reset all units in Fabric */
+	QLAFX00_SET_HBA_SOC_REG(ha, SOC_FABRIC_RST_CONTROL_REG, (0x11F0101));
+
+	/* Reset all interrupt control registers */
+	for (i = 0; i < 115; i++) {
+		QLAFX00_SET_HBA_SOC_REG(ha,
+		    (SOC_INTERRUPT_SOURCE_I_CONTROL_REG + 4*i), (0x0));
+	}
+
+	/* Reset Timers control registers. per core */
+	for (core = 0; core < 4; core++)
+		for (i = 0; i < 8; i++)
+			QLAFX00_SET_HBA_SOC_REG(ha,
+			    (SOC_CORE_TIMER_REG + 0x100*core + 4*i), (0x0));
+
+	/* Reset per core IRQ ack register */
+	for (core = 0; core < 4; core++)
+		QLAFX00_SET_HBA_SOC_REG(ha,
+		    (SOC_IRQ_ACK_REG + 0x100*core), (0x3FF));
+
+	/* Set Fabric control and config to defaults */
+	QLAFX00_SET_HBA_SOC_REG(ha, SOC_FABRIC_CONTROL_REG, (0x2));
+	QLAFX00_SET_HBA_SOC_REG(ha, SOC_FABRIC_CONFIG_REG, (0x3));
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+
+	/* Kick in Fabric units */
+	QLAFX00_SET_HBA_SOC_REG(ha, SOC_FABRIC_RST_CONTROL_REG, (0x0));
+
+	/* Kick in Core0 to start boot process */
+	QLAFX00_SET_HBA_SOC_REG(ha, SOC_SW_RST_CONTROL_REG_CORE0, (0xF00));
+
+	/* Wait 10secs for soft-reset to complete. */
+	for (cnt = 10; cnt; cnt--) {
+		msleep(1000);
+		barrier();
+	}
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+}
+
+/**
+ * qlafx00_soft_reset() - Soft Reset ISPFx00.
+ * @ha: HA context
+ *
+ * Returns 0 on success.
+ */
+void
+qlafx00_soft_reset(scsi_qla_host_t *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+
+	if (unlikely(pci_channel_offline(ha->pdev) &&
+	    ha->flags.pci_channel_io_perm_failure))
+		return;
+
+	ha->isp_ops->disable_intrs(ha);
+	qlafx00_soc_cpu_reset(vha);
+	ha->isp_ops->enable_intrs(ha);
+}
+
+/**
+ * qlafx00_chip_diag() - Test ISPFx00 for proper operation.
+ * @ha: HA context
+ *
+ * Returns 0 on success.
+ */
+int
+qlafx00_chip_diag(scsi_qla_host_t *vha)
+{
+	int rval = 0;
+	struct qla_hw_data *ha = vha->hw;
+	struct req_que *req = ha->req_q_map[0];
+
+	ha->fw_transfer_size = REQUEST_ENTRY_SIZE * req->length;
+
+	rval = qlafx00_mbx_reg_test(vha);
+	if (rval) {
+		ql_log(ql_log_warn, vha, 0x1165,
+		    "Failed mailbox send register test\n");
+	} else {
+		/* Flag a successful rval */
+		rval = QLA_SUCCESS;
+	}
+	return rval;
+}
+
+void
+qlafx00_config_rings(struct scsi_qla_host *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+	struct device_reg_fx00 __iomem *reg = &ha->iobase->ispfx00;
+	struct init_cb_fx *icb;
+	struct req_que *req = ha->req_q_map[0];
+	struct rsp_que *rsp = ha->rsp_q_map[0];
+
+	/* Setup ring parameters in initialization control block. */
+	icb = (struct init_cb_fx *)ha->init_cb;
+	icb->request_q_outpointer = __constant_cpu_to_le16(0);
+	icb->response_q_inpointer = __constant_cpu_to_le16(0);
+	icb->request_q_length = cpu_to_le16(req->length);
+	icb->response_q_length = cpu_to_le16(rsp->length);
+	icb->request_q_address[0] = cpu_to_le32(LSD(req->dma));
+	icb->request_q_address[1] = cpu_to_le32(MSD(req->dma));
+	icb->response_q_address[0] = cpu_to_le32(LSD(rsp->dma));
+	icb->response_q_address[1] = cpu_to_le32(MSD(rsp->dma));
+
+	WRT_REG_DWORD(&reg->req_q_in, 0);
+	WRT_REG_DWORD(&reg->req_q_out, 0);
+
+	WRT_REG_DWORD(&reg->rsp_q_in, 0);
+	WRT_REG_DWORD(&reg->rsp_q_out, 0);
+
+	/* PCI posting */
+	RD_REG_DWORD(&reg->rsp_q_out);
+}
+
+char *
+qlafx00_pci_info_str(struct scsi_qla_host *vha, char *str)
+{
+	struct qla_hw_data *ha = vha->hw;
+	int pcie_reg;
+
+	pcie_reg = pci_find_capability(ha->pdev, PCI_CAP_ID_EXP);
+	if (pcie_reg) {
+		strcpy(str, "PCIe iSA");
+		return str;
+	}
+	return str;
+}
+
+char *
+qlafx00_fw_version_str(struct scsi_qla_host *vha, char *str)
+{
+	struct qla_hw_data *ha = vha->hw;
+
+	sprintf(str, "%s", ha->mr.fw_version);
+	return str;
+}
+
+void
+qlafx00_enable_intrs(struct qla_hw_data *ha)
+{
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+	ha->interrupts_on = 1;
+	QLAFX00_ENABLE_ICNTRL_REG(ha);
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+}
+
+void
+qlafx00_disable_intrs(struct qla_hw_data *ha)
+{
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+	ha->interrupts_on = 0;
+	QLAFX00_DISABLE_ICNTRL_REG(ha);
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+}
+
+static void
+qlafx00_tmf_iocb_timeout(void *data)
+{
+	srb_t *sp = (srb_t *)data;
+	struct srb_iocb *tmf = &sp->u.iocb_cmd;
+
+	tmf->u.tmf.comp_status = CS_TIMEOUT;
+	complete(&tmf->u.tmf.comp);
+}
+
+static void
+qlafx00_tmf_sp_done(void *data, void *ptr, int res)
+{
+	srb_t *sp = (srb_t *)ptr;
+	struct srb_iocb *tmf = &sp->u.iocb_cmd;
+
+	complete(&tmf->u.tmf.comp);
+}
+
+static int
+qlafx00_async_tm_cmd(fc_port_t *fcport, uint32_t flags,
+		     uint32_t lun, uint32_t tag)
+{
+	scsi_qla_host_t *vha = fcport->vha;
+	struct srb_iocb *tm_iocb;
+	srb_t *sp;
+	int rval = QLA_FUNCTION_FAILED;
+
+	sp = qla2x00_get_sp(vha, fcport, GFP_KERNEL);
+	if (!sp)
+		goto done;
+
+	tm_iocb = &sp->u.iocb_cmd;
+	sp->type = SRB_TM_CMD;
+	sp->name = "tmf";
+	qla2x00_init_timer(sp, qla2x00_get_async_timeout(vha));
+	tm_iocb->u.tmf.flags = flags;
+	tm_iocb->u.tmf.lun = lun;
+	tm_iocb->u.tmf.data = tag;
+	sp->done = qlafx00_tmf_sp_done;
+	tm_iocb->timeout = qlafx00_tmf_iocb_timeout;
+	init_completion(&tm_iocb->u.tmf.comp);
+
+	rval = qla2x00_start_sp(sp);
+	if (rval != QLA_SUCCESS)
+		goto done_free_sp;
+
+	ql_dbg(ql_dbg_async, vha, 0x507b,
+	    "Task management command issued target_id=%x\n",
+	    fcport->tgt_id);
+
+	wait_for_completion(&tm_iocb->u.tmf.comp);
+
+	rval = tm_iocb->u.tmf.comp_status == CS_COMPLETE ?
+	    QLA_SUCCESS : QLA_FUNCTION_FAILED;
+
+done_free_sp:
+	sp->free(vha, sp);
+done:
+	return rval;
+}
+
+int
+qlafx00_abort_target(fc_port_t *fcport, unsigned int l, int tag)
+{
+	return qlafx00_async_tm_cmd(fcport, TCF_TARGET_RESET, l, tag);
+}
+
+int
+qlafx00_lun_reset(fc_port_t *fcport, unsigned int l, int tag)
+{
+	return qlafx00_async_tm_cmd(fcport, TCF_LUN_RESET, l, tag);
+}
+
+int
+qlafx00_iospace_config(struct qla_hw_data *ha)
+{
+	if (pci_request_selected_regions(ha->pdev, ha->bars,
+	    QLA2XXX_DRIVER_NAME)) {
+		ql_log_pci(ql_log_fatal, ha->pdev, 0x014e,
+		    "Failed to reserve PIO/MMIO regions (%s), aborting.\n",
+		    pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+
+	/* Use MMIO operations for all accesses. */
+	if (!(pci_resource_flags(ha->pdev, 0) & IORESOURCE_MEM)) {
+		ql_log_pci(ql_log_warn, ha->pdev, 0x014f,
+		    "Invalid pci I/O region size (%s).\n",
+		    pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+	if (pci_resource_len(ha->pdev, 0) < BAR0_LEN_FX00) {
+		ql_log_pci(ql_log_warn, ha->pdev, 0x0127,
+		    "Invalid PCI mem BAR0 region size (%s), aborting\n",
+			pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+
+	ha->cregbase =
+	    ioremap_nocache(pci_resource_start(ha->pdev, 0), BAR0_LEN_FX00);
+	if (!ha->cregbase) {
+		ql_log_pci(ql_log_fatal, ha->pdev, 0x0128,
+		    "cannot remap MMIO (%s), aborting\n", pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+
+	if (!(pci_resource_flags(ha->pdev, 2) & IORESOURCE_MEM)) {
+		ql_log_pci(ql_log_warn, ha->pdev, 0x0129,
+		    "region #2 not an MMIO resource (%s), aborting\n",
+		    pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+	if (pci_resource_len(ha->pdev, 2) < BAR2_LEN_FX00) {
+		ql_log_pci(ql_log_warn, ha->pdev, 0x012a,
+		    "Invalid PCI mem BAR2 region size (%s), aborting\n",
+			pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+
+	ha->iobase =
+	    ioremap_nocache(pci_resource_start(ha->pdev, 2), BAR2_LEN_FX00);
+	if (!ha->iobase) {
+		ql_log_pci(ql_log_fatal, ha->pdev, 0x012b,
+		    "cannot remap MMIO (%s), aborting\n", pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+
+	/* Determine queue resources */
+	ha->max_req_queues = ha->max_rsp_queues = 1;
+
+	ql_log_pci(ql_log_info, ha->pdev, 0x012c,
+	    "Bars 0x%x, iobase0 0x%p, iobase2 0x%p\n",
+	    ha->bars, ha->cregbase, ha->iobase);
+
+	return 0;
+
+iospace_error_exit:
+	return -ENOMEM;
+}
+
+static void
+qlafx00_save_queue_ptrs(struct scsi_qla_host *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+	struct req_que *req = ha->req_q_map[0];
+	struct rsp_que *rsp = ha->rsp_q_map[0];
+
+	req->length_fx00 = req->length;
+	req->ring_fx00 = req->ring;
+	req->dma_fx00 = req->dma;
+
+	rsp->length_fx00 = rsp->length;
+	rsp->ring_fx00 = rsp->ring;
+	rsp->dma_fx00 = rsp->dma;
+
+	ql_dbg(ql_dbg_init, vha, 0x012d,
+	    "req: %p, ring_fx00: %p, length_fx00: 0x%x,"
+	    "req->dma_fx00: 0x%llx\n", req, req->ring_fx00,
+	    req->length_fx00, (u64)req->dma_fx00);
+
+	ql_dbg(ql_dbg_init, vha, 0x012e,
+	    "rsp: %p, ring_fx00: %p, length_fx00: 0x%x,"
+	    "rsp->dma_fx00: 0x%llx\n", rsp, rsp->ring_fx00,
+	    rsp->length_fx00, (u64)rsp->dma_fx00);
+}
+
+static int
+qlafx00_config_queues(struct scsi_qla_host *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+	struct req_que *req = ha->req_q_map[0];
+	struct rsp_que *rsp = ha->rsp_q_map[0];
+	dma_addr_t bar2_hdl = pci_resource_start(ha->pdev, 2);
+
+	req->length = ha->req_que_len;
+	req->ring = (void *)ha->iobase + ha->req_que_off;
+	req->dma = bar2_hdl + ha->req_que_off;
+	if ((!req->ring) || (req->length == 0)) {
+		ql_log_pci(ql_log_info, ha->pdev, 0x012f,
+		    "Unable to allocate memory for req_ring\n");
+		return QLA_FUNCTION_FAILED;
+	}
+
+	ql_dbg(ql_dbg_init, vha, 0x0130,
+	    "req: %p req_ring pointer %p req len 0x%x "
+	    "req off 0x%x\n, req->dma: 0x%llx",
+	    req, req->ring, req->length,
+	    ha->req_que_off, (u64)req->dma);
+
+	rsp->length = ha->rsp_que_len;
+	rsp->ring = (void *)ha->iobase + ha->rsp_que_off;
+	rsp->dma = bar2_hdl + ha->rsp_que_off;
+	if ((!rsp->ring) || (rsp->length == 0)) {
+		ql_log_pci(ql_log_info, ha->pdev, 0x0131,
+		    "Unable to allocate memory for rsp_ring\n");
+		return QLA_FUNCTION_FAILED;
+	}
+
+	ql_dbg(ql_dbg_init, vha, 0x0132,
+	    "rsp: %p rsp_ring pointer %p rsp len 0x%x "
+	    "rsp off 0x%x, rsp->dma: 0x%llx\n",
+	    rsp, rsp->ring, rsp->length,
+	    ha->rsp_que_off, (u64)rsp->dma);
+
+	return QLA_SUCCESS;
+}
+
+static int
+qlafx00_init_fw_ready(scsi_qla_host_t *vha)
+{
+	int rval = 0;
+	unsigned long wtime;
+	uint16_t wait_time;	/* Wait time */
+	struct qla_hw_data *ha = vha->hw;
+	struct device_reg_fx00 __iomem *reg = &ha->iobase->ispfx00;
+	uint32_t aenmbx, aenmbx7 = 0;
+	uint32_t state[5];
+	bool done = false;
+
+	/* 30 seconds wait - Adjust if required */
+	wait_time = 30;
+
+	/* wait time before firmware ready */
+	wtime = jiffies + (wait_time * HZ);
+	do {
+		aenmbx = RD_REG_DWORD(&reg->aenmailbox0);
+		barrier();
+		ql_dbg(ql_dbg_mbx, vha, 0x0133,
+		    "aenmbx: 0x%x\n", aenmbx);
+
+		switch (aenmbx) {
+		case MBA_FW_NOT_STARTED:
+		case MBA_FW_STARTING:
+			break;
+
+		case MBA_SYSTEM_ERR:
+		case MBA_REQ_TRANSFER_ERR:
+		case MBA_RSP_TRANSFER_ERR:
+		case MBA_FW_INIT_FAILURE:
+			qlafx00_soft_reset(vha);
+			break;
+
+		case MBA_FW_RESTART_CMPLT:
+			/* Set the mbx and rqstq intr code */
+			aenmbx7 = RD_REG_DWORD(&reg->aenmailbox7);
+			ha->mbx_intr_code = MSW(aenmbx7);
+			ha->rqstq_intr_code = LSW(aenmbx7);
+			ha->req_que_off = RD_REG_DWORD(&reg->aenmailbox1);
+			ha->rsp_que_off = RD_REG_DWORD(&reg->aenmailbox3);
+			ha->req_que_len = RD_REG_DWORD(&reg->aenmailbox5);
+			ha->rsp_que_len = RD_REG_DWORD(&reg->aenmailbox6);
+			WRT_REG_DWORD(&reg->aenmailbox0, 0);
+			RD_REG_DWORD_RELAXED(&reg->aenmailbox0);
+			ql_dbg(ql_dbg_init, vha, 0x0134,
+			    "f/w returned mbx_intr_code: 0x%x, "
+			    "rqstq_intr_code: 0x%x\n",
+			    ha->mbx_intr_code, ha->rqstq_intr_code);
+			QLAFX00_CLR_INTR_REG(ha, QLAFX00_HST_INT_STS_BITS);
+			rval = QLA_SUCCESS;
+			done = true;
+			break;
+
+		default:
+			/* If fw is apparently not ready. In order to continue,
+			 * we might need to issue Mbox cmd, but the problem is
+			 * that the DoorBell vector values that come with the
+			 * 8060 AEN are most likely gone by now (and thus no
+			 * bell would be rung on the fw side when mbox cmd is
+			 * issued). We have to therefore grab the 8060 AEN
+			 * shadow regs (filled in by FW when the last 8060
+			 * AEN was being posted).
+			 * Do the following to determine what is needed in
+			 * order to get the FW ready:
+			 * 1. reload the 8060 AEN values from the shadow regs
+			 * 2. clear int status to get rid of possible pending
+			 *    interrupts
+			 * 3. issue Get FW State Mbox cmd to determine fw state
+			 * Set the mbx and rqstq intr code from Shadow Regs
+			 */
+			aenmbx7 = RD_REG_DWORD(&reg->initval7);
+			ha->mbx_intr_code = MSW(aenmbx7);
+			ha->rqstq_intr_code = LSW(aenmbx7);
+			ha->req_que_off = RD_REG_DWORD(&reg->initval1);
+			ha->rsp_que_off = RD_REG_DWORD(&reg->initval3);
+			ha->req_que_len = RD_REG_DWORD(&reg->initval5);
+			ha->rsp_que_len = RD_REG_DWORD(&reg->initval6);
+			ql_dbg(ql_dbg_init, vha, 0x0135,
+			    "f/w returned mbx_intr_code: 0x%x, "
+			    "rqstq_intr_code: 0x%x\n",
+			    ha->mbx_intr_code, ha->rqstq_intr_code);
+			QLAFX00_CLR_INTR_REG(ha, QLAFX00_HST_INT_STS_BITS);
+
+			/* Get the FW state */
+			rval = qlafx00_get_firmware_state(vha, state);
+			if (rval != QLA_SUCCESS) {
+				/* Retry if timer has not expired */
+				break;
+			}
+
+			if (state[0] == FSTATE_FX00_CONFIG_WAIT) {
+				/* Firmware is waiting to be
+				 * initialized by driver
+				 */
+				rval = QLA_SUCCESS;
+				done = true;
+				break;
+			}
+
+			/* Issue driver shutdown and wait until f/w recovers.
+			 * Driver should continue to poll until 8060 AEN is
+			 * received indicating firmware recovery.
+			 */
+			ql_dbg(ql_dbg_init, vha, 0x0136,
+			    "Sending Driver shutdown fw_state 0x%x\n",
+			    state[0]);
+
+			rval = qlafx00_driver_shutdown(vha, 10);
+			if (rval != QLA_SUCCESS) {
+				rval = QLA_FUNCTION_FAILED;
+				break;
+			}
+			msleep(500);
+
+			wtime = jiffies + (wait_time * HZ);
+			break;
+		}
+
+		if (!done) {
+			if (time_after_eq(jiffies, wtime)) {
+				ql_dbg(ql_dbg_init, vha, 0x0137,
+				    "Init f/w failed: aen[7]: 0x%x\n",
+				    RD_REG_DWORD(&reg->aenmailbox7));
+				rval = QLA_FUNCTION_FAILED;
+				done = true;
+				break;
+			}
+			/* Delay for a while */
+			msleep(500);
+		}
+	} while (!done);
+
+	if (rval)
+		ql_dbg(ql_dbg_init, vha, 0x0138,
+		    "%s **** FAILED ****.\n", __func__);
+	else
+		ql_dbg(ql_dbg_init, vha, 0x0139,
+		    "%s **** SUCCESS ****.\n", __func__);
+
+	return rval;
+}
+
+/*
+ * qlafx00_fw_ready() - Waits for firmware ready.
+ * @ha: HA context
+ *
+ * Returns 0 on success.
+ */
+int
+qlafx00_fw_ready(scsi_qla_host_t *vha)
+{
+	int		rval;
+	unsigned long	wtime;
+	uint16_t	wait_time;	/* Wait time if loop is coming ready */
+	uint32_t	state[5];
+
+	rval = QLA_SUCCESS;
+
+	wait_time = 10;
+
+	/* wait time before firmware ready */
+	wtime = jiffies + (wait_time * HZ);
+
+	/* Wait for ISP to finish init */
+	if (!vha->flags.init_done)
+		ql_dbg(ql_dbg_init, vha, 0x013a,
+		    "Waiting for init to complete...\n");
+
+	do {
+		rval = qlafx00_get_firmware_state(vha, state);
+
+		if (rval == QLA_SUCCESS) {
+			if (state[0] == FSTATE_FX00_INITIALIZED) {
+				ql_dbg(ql_dbg_init, vha, 0x013b,
+				    "fw_state=%x\n", state[0]);
+				rval = QLA_SUCCESS;
+					break;
+			}
+		}
+		rval = QLA_FUNCTION_FAILED;
+
+		if (time_after_eq(jiffies, wtime))
+			break;
+
+		/* Delay for a while */
+		msleep(500);
+
+		ql_dbg(ql_dbg_init, vha, 0x013c,
+		    "fw_state=%x curr time=%lx.\n", state[0], jiffies);
+	} while (1);
+
+
+	if (rval)
+		ql_dbg(ql_dbg_init, vha, 0x013d,
+		    "Firmware ready **** FAILED ****.\n");
+	else
+		ql_dbg(ql_dbg_init, vha, 0x013e,
+		    "Firmware ready **** SUCCESS ****.\n");
+
+	return rval;
+}
+
+static int
+qlafx00_find_all_targets(scsi_qla_host_t *vha,
+	struct list_head *new_fcports)
+{
+	int		rval;
+	uint16_t	tgt_id;
+	fc_port_t	*fcport, *new_fcport;
+	int		found;
+	struct qla_hw_data *ha = vha->hw;
+
+	rval = QLA_SUCCESS;
+
+	if (!test_bit(LOOP_RESYNC_ACTIVE, &vha->dpc_flags))
+		return QLA_FUNCTION_FAILED;
+
+	if ((atomic_read(&vha->loop_down_timer) ||
+	     STATE_TRANSITION(vha))) {
+		atomic_set(&vha->loop_down_timer, 0);
+		set_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
+		return QLA_FUNCTION_FAILED;
+	}
+
+	ql_dbg(ql_dbg_disc + ql_dbg_init, vha, 0x2088,
+	    "Listing Target bit map...\n");
+	ql_dump_buffer(ql_dbg_disc + ql_dbg_init, vha,
+	    0x2089, (uint8_t *)ha->gid_list, 32);
+
+	/* Allocate temporary rmtport for any new rmtports discovered. */
+	new_fcport = qla2x00_alloc_fcport(vha, GFP_KERNEL);
+	if (new_fcport == NULL)
+		return QLA_MEMORY_ALLOC_FAILED;
+
+	for_each_set_bit(tgt_id, (void *)ha->gid_list,
+	    QLAFX00_TGT_NODE_LIST_SIZE) {
+
+		/* Send get target node info */
+		new_fcport->tgt_id = tgt_id;
+		rval = qlafx00_fx_disc(vha, new_fcport,
+		    FXDISC_GET_TGT_NODE_INFO);
+		if (rval != QLA_SUCCESS) {
+			ql_log(ql_log_warn, vha, 0x208a,
+			    "Target info scan failed -- assuming zero-entry "
+			    "result...\n");
+			continue;
+		}
+
+		/* Locate matching device in database. */
+		found = 0;
+		list_for_each_entry(fcport, &vha->vp_fcports, list) {
+			if (memcmp(new_fcport->port_name,
+			    fcport->port_name, WWN_SIZE))
+				continue;
+
+			found++;
+
+			/*
+			 * If tgt_id is same and state FCS_ONLINE, nothing
+			 * changed.
+			 */
+			if (fcport->tgt_id == new_fcport->tgt_id &&
+			    atomic_read(&fcport->state) == FCS_ONLINE)
+				break;
+
+			/*
+			 * Tgt ID changed or device was marked to be updated.
+			 */
+			ql_dbg(ql_dbg_disc + ql_dbg_init, vha, 0x208b,
+			    "TGT-ID Change(%s): Present tgt id: "
+			    "0x%x state: 0x%x "
+			    "wwnn = %llx wwpn = %llx.\n",
+			    __func__, fcport->tgt_id,
+			    atomic_read(&fcport->state),
+			    (unsigned long long)wwn_to_u64(fcport->node_name),
+			    (unsigned long long)wwn_to_u64(fcport->port_name));
+
+			ql_log(ql_log_info, vha, 0x208c,
+			    "TGT-ID Announce(%s): Discovered tgt "
+			    "id 0x%x wwnn = %llx "
+			    "wwpn = %llx.\n", __func__, new_fcport->tgt_id,
+			    (unsigned long long)
+			    wwn_to_u64(new_fcport->node_name),
+			    (unsigned long long)
+			    wwn_to_u64(new_fcport->port_name));
+
+			if (atomic_read(&fcport->state) != FCS_ONLINE) {
+				fcport->old_tgt_id = fcport->tgt_id;
+				fcport->tgt_id = new_fcport->tgt_id;
+				ql_log(ql_log_info, vha, 0x208d,
+				   "TGT-ID: New fcport Added: %p\n", fcport);
+				qla2x00_update_fcport(vha, fcport);
+			} else {
+				ql_log(ql_log_info, vha, 0x208e,
+				    " Existing TGT-ID %x did not get "
+				    " offline event from firmware.\n",
+				    fcport->old_tgt_id);
+				qla2x00_mark_device_lost(vha, fcport, 0, 0);
+				set_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
+				kfree(new_fcport);
+				return rval;
+			}
+			break;
+		}
+
+		if (found)
+			continue;
+
+		/* If device was not in our fcports list, then add it. */
+		list_add_tail(&new_fcport->list, new_fcports);
+
+		/* Allocate a new replacement fcport. */
+		new_fcport = qla2x00_alloc_fcport(vha, GFP_KERNEL);
+		if (new_fcport == NULL)
+			return QLA_MEMORY_ALLOC_FAILED;
+	}
+
+	kfree(new_fcport);
+	return rval;
+}
+
+/*
+ * qlafx00_configure_all_targets
+ *      Setup target devices with node ID's.
+ *
+ * Input:
+ *      ha = adapter block pointer.
+ *
+ * Returns:
+ *      0 = success.
+ *      BIT_0 = error
+ */
+static int
+qlafx00_configure_all_targets(scsi_qla_host_t *vha)
+{
+	int rval;
+	fc_port_t *fcport, *rmptemp;
+	LIST_HEAD(new_fcports);
+
+	rval = qlafx00_fx_disc(vha, &vha->hw->mr.fcport,
+	    FXDISC_GET_TGT_NODE_LIST);
+	if (rval != QLA_SUCCESS) {
+		set_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
+		return rval;
+	}
+
+	rval = qlafx00_find_all_targets(vha, &new_fcports);
+	if (rval != QLA_SUCCESS) {
+		set_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
+		return rval;
+	}
+
+	/*
+	 * Delete all previous devices marked lost.
+	 */
+	list_for_each_entry(fcport, &vha->vp_fcports, list) {
+		if (test_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags))
+			break;
+
+		if (atomic_read(&fcport->state) == FCS_DEVICE_LOST) {
+			if (fcport->port_type != FCT_INITIATOR)
+				qla2x00_mark_device_lost(vha, fcport, 0, 0);
+		}
+	}
+
+	/*
+	 * Add the new devices to our devices list.
+	 */
+	list_for_each_entry_safe(fcport, rmptemp, &new_fcports, list) {
+		if (test_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags))
+			break;
+
+		qla2x00_update_fcport(vha, fcport);
+		list_move_tail(&fcport->list, &vha->vp_fcports);
+		ql_log(ql_log_info, vha, 0x208f,
+		    "Attach new target id 0x%x wwnn = %llx "
+		    "wwpn = %llx.\n",
+		    fcport->tgt_id,
+		    (unsigned long long)wwn_to_u64(fcport->node_name),
+		    (unsigned long long)wwn_to_u64(fcport->port_name));
+	}
+
+	/* Free all new device structures not processed. */
+	list_for_each_entry_safe(fcport, rmptemp, &new_fcports, list) {
+		list_del(&fcport->list);
+		kfree(fcport);
+	}
+
+	return rval;
+}
+
+/*
+ * qlafx00_configure_devices
+ *      Updates Fibre Channel Device Database with what is actually on loop.
+ *
+ * Input:
+ *      ha                = adapter block pointer.
+ *
+ * Returns:
+ *      0 = success.
+ *      1 = error.
+ *      2 = database was full and device was not configured.
+ */
+int
+qlafx00_configure_devices(scsi_qla_host_t *vha)
+{
+	int  rval;
+	unsigned long flags, save_flags;
+	rval = QLA_SUCCESS;
+
+	save_flags = flags = vha->dpc_flags;
+
+	ql_dbg(ql_dbg_disc, vha, 0x2090,
+	    "Configure devices -- dpc flags =0x%lx\n", flags);
+
+	rval = qlafx00_configure_all_targets(vha);
+
+	if (rval == QLA_SUCCESS) {
+		if (test_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags)) {
+			rval = QLA_FUNCTION_FAILED;
+		} else {
+			atomic_set(&vha->loop_state, LOOP_READY);
+			ql_log(ql_log_info, vha, 0x2091,
+			    "Device Ready\n");
+		}
+	}
+
+	if (rval) {
+		ql_dbg(ql_dbg_disc, vha, 0x2092,
+		    "%s *** FAILED ***.\n", __func__);
+	} else {
+		ql_dbg(ql_dbg_disc, vha, 0x2093,
+		    "%s: exiting normally.\n", __func__);
+	}
+	return rval;
+}
+
+static void
+qlafx00_abort_isp_cleanup(scsi_qla_host_t *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+	fc_port_t *fcport;
+
+	vha->flags.online = 0;
+	ha->flags.chip_reset_done = 0;
+	ha->mr.fw_hbt_en = 0;
+	clear_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+	vha->qla_stats.total_isp_aborts++;
+
+	ql_log(ql_log_info, vha, 0x013f,
+	    "Performing ISP error recovery - ha = %p.\n", ha);
+
+	ha->isp_ops->reset_chip(vha);
+
+	if (atomic_read(&vha->loop_state) != LOOP_DOWN) {
+		atomic_set(&vha->loop_state, LOOP_DOWN);
+		atomic_set(&vha->loop_down_timer,
+		    QLAFX00_LOOP_DOWN_TIME);
+	} else {
+		if (!atomic_read(&vha->loop_down_timer))
+			atomic_set(&vha->loop_down_timer,
+			    QLAFX00_LOOP_DOWN_TIME);
+	}
+
+	/* Clear all async request states across all VPs. */
+	list_for_each_entry(fcport, &vha->vp_fcports, list) {
+		fcport->flags = 0;
+		if (atomic_read(&fcport->state) == FCS_ONLINE)
+			qla2x00_set_fcport_state(fcport, FCS_DEVICE_LOST);
+	}
+
+	if (!ha->flags.eeh_busy) {
+		/* Requeue all commands in outstanding command list. */
+		qla2x00_abort_all_cmds(vha, DID_RESET << 16);
+	}
+
+	qla2x00_free_irqs(vha);
+	set_bit(FX00_RESET_RECOVERY, &vha->dpc_flags);
+
+	/* Clear the Interrupts */
+	QLAFX00_CLR_INTR_REG(ha, QLAFX00_HST_INT_STS_BITS);
+
+	ql_log(ql_log_info, vha, 0x0140,
+	    "%s Done done - ha=%p.\n", __func__, ha);
+}
+
+/**
+ * qlafx00_init_response_q_entries() - Initializes response queue entries.
+ * @ha: HA context
+ *
+ * Beginning of request ring has initialization control block already built
+ * by nvram config routine.
+ *
+ * Returns 0 on success.
+ */
+void
+qlafx00_init_response_q_entries(struct rsp_que *rsp)
+{
+	uint16_t cnt;
+	response_t *pkt;
+
+	rsp->ring_ptr = rsp->ring;
+	rsp->ring_index    = 0;
+	rsp->status_srb = NULL;
+	pkt = rsp->ring_ptr;
+	for (cnt = 0; cnt < rsp->length; cnt++) {
+		pkt->signature = RESPONSE_PROCESSED;
+		WRT_REG_DWORD(&pkt->signature, RESPONSE_PROCESSED);
+		pkt++;
+	}
+}
+
+int
+qlafx00_rescan_isp(scsi_qla_host_t *vha)
+{
+	uint32_t status = QLA_FUNCTION_FAILED;
+	struct qla_hw_data *ha = vha->hw;
+	struct device_reg_fx00 __iomem *reg = &ha->iobase->ispfx00;
+	uint32_t aenmbx7;
+
+	qla2x00_request_irqs(ha, ha->rsp_q_map[0]);
+
+	aenmbx7 = RD_REG_DWORD(&reg->aenmailbox7);
+	ha->mbx_intr_code = MSW(aenmbx7);
+	ha->rqstq_intr_code = LSW(aenmbx7);
+	ha->req_que_off = RD_REG_DWORD(&reg->aenmailbox1);
+	ha->rsp_que_off = RD_REG_DWORD(&reg->aenmailbox3);
+	ha->req_que_len = RD_REG_DWORD(&reg->aenmailbox5);
+	ha->rsp_que_len = RD_REG_DWORD(&reg->aenmailbox6);
+
+	ql_dbg(ql_dbg_disc, vha, 0x2094,
+	    "fw returned mbx_intr_code: 0x%x, rqstq_intr_code: 0x%x "
+	    " Req que offset 0x%x Rsp que offset 0x%x\n",
+	    ha->mbx_intr_code, ha->rqstq_intr_code,
+	    ha->req_que_off, ha->rsp_que_len);
+
+	/* Clear the Interrupts */
+	QLAFX00_CLR_INTR_REG(ha, QLAFX00_HST_INT_STS_BITS);
+
+	status = qla2x00_init_rings(vha);
+	if (!status) {
+		vha->flags.online = 1;
+
+		/* if no cable then assume it's good */
+		if ((vha->device_flags & DFLG_NO_CABLE))
+			status = 0;
+		/* Register system information */
+		if (qlafx00_fx_disc(vha,
+		    &vha->hw->mr.fcport, FXDISC_REG_HOST_INFO))
+			ql_dbg(ql_dbg_disc, vha, 0x2095,
+			    "failed to register host info\n");
+	}
+	scsi_unblock_requests(vha->host);
+	return status;
+}
+
+void
+qlafx00_timer_routine(scsi_qla_host_t *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+	uint32_t fw_heart_beat;
+	uint32_t aenmbx0;
+	struct device_reg_fx00 __iomem *reg = &ha->iobase->ispfx00;
+
+	/* Check firmware health */
+	if (ha->mr.fw_hbt_cnt)
+		ha->mr.fw_hbt_cnt--;
+	else {
+		if ((!ha->flags.mr_reset_hdlr_active) &&
+		    (!test_bit(UNLOADING, &vha->dpc_flags)) &&
+		    (!test_bit(ABORT_ISP_ACTIVE, &vha->dpc_flags)) &&
+		    (ha->mr.fw_hbt_en)) {
+			fw_heart_beat = RD_REG_DWORD(&reg->fwheartbeat);
+			if (fw_heart_beat != ha->mr.old_fw_hbt_cnt) {
+				ha->mr.old_fw_hbt_cnt = fw_heart_beat;
+				ha->mr.fw_hbt_miss_cnt = 0;
+			} else {
+				ha->mr.fw_hbt_miss_cnt++;
+				if (ha->mr.fw_hbt_miss_cnt ==
+				    QLAFX00_HEARTBEAT_MISS_CNT) {
+					set_bit(ISP_ABORT_NEEDED,
+					    &vha->dpc_flags);
+					qla2xxx_wake_dpc(vha);
+					ha->mr.fw_hbt_miss_cnt = 0;
+				}
+			}
+		}
+		ha->mr.fw_hbt_cnt = QLAFX00_HEARTBEAT_INTERVAL;
+	}
+
+	if (test_bit(FX00_RESET_RECOVERY, &vha->dpc_flags)) {
+		/* Reset recovery to be performed in timer routine */
+		aenmbx0 = RD_REG_DWORD(&reg->aenmailbox0);
+		if (ha->mr.fw_reset_timer_exp) {
+			set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+			qla2xxx_wake_dpc(vha);
+			ha->mr.fw_reset_timer_exp = 0;
+		} else if (aenmbx0 == MBA_FW_RESTART_CMPLT) {
+			/* Wake up DPC to rescan the targets */
+			set_bit(FX00_TARGET_SCAN, &vha->dpc_flags);
+			clear_bit(FX00_RESET_RECOVERY, &vha->dpc_flags);
+			qla2xxx_wake_dpc(vha);
+			ha->mr.fw_reset_timer_tick = QLAFX00_RESET_INTERVAL;
+		} else if ((aenmbx0 == MBA_FW_STARTING) &&
+		    (!ha->mr.fw_hbt_en)) {
+			ha->mr.fw_hbt_en = 1;
+		} else if (!ha->mr.fw_reset_timer_tick) {
+			if (aenmbx0 == ha->mr.old_aenmbx0_state)
+				ha->mr.fw_reset_timer_exp = 1;
+			ha->mr.fw_reset_timer_tick = QLAFX00_RESET_INTERVAL;
+		} else if (aenmbx0 == 0xFFFFFFFF) {
+			uint32_t data0, data1;
+
+			data0 = QLAFX00_RD_REG(ha,
+			    QLAFX00_BAR1_BASE_ADDR_REG);
+			data1 = QLAFX00_RD_REG(ha,
+			    QLAFX00_PEX0_WIN0_BASE_ADDR_REG);
+
+			data0 &= 0xffff0000;
+			data1 &= 0x0000ffff;
+
+			QLAFX00_WR_REG(ha,
+			    QLAFX00_PEX0_WIN0_BASE_ADDR_REG,
+			    (data0 | data1));
+		} else if ((aenmbx0 & 0xFF00) == MBA_FW_POLL_STATE) {
+			ha->mr.fw_reset_timer_tick =
+			    QLAFX00_MAX_RESET_INTERVAL;
+		}
+		ha->mr.old_aenmbx0_state = aenmbx0;
+		ha->mr.fw_reset_timer_tick--;
+	}
+}
+
+/*
+ *  qlfx00a_reset_initialize
+ *      Re-initialize after a iSA device reset.
+ *
+ * Input:
+ *      ha  = adapter block pointer.
+ *
+ * Returns:
+ *      0 = success
+ */
+int
+qlafx00_reset_initialize(scsi_qla_host_t *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+
+	if (vha->device_flags & DFLG_DEV_FAILED) {
+		ql_dbg(ql_dbg_init, vha, 0x0142,
+		    "Device in failed state\n");
+		return QLA_SUCCESS;
+	}
+
+	ha->flags.mr_reset_hdlr_active = 1;
+
+	if (vha->flags.online) {
+		scsi_block_requests(vha->host);
+		qlafx00_abort_isp_cleanup(vha);
+	}
+
+	ql_log(ql_log_info, vha, 0x0143,
+	    "(%s): succeeded.\n", __func__);
+	ha->flags.mr_reset_hdlr_active = 0;
+	return QLA_SUCCESS;
+}
+
+/*
+ *  qlafx00_abort_isp
+ *      Resets ISP and aborts all outstanding commands.
+ *
+ * Input:
+ *      ha  = adapter block pointer.
+ *
+ * Returns:
+ *      0 = success
+ */
+int
+qlafx00_abort_isp(scsi_qla_host_t *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+
+	if (vha->flags.online) {
+		if (unlikely(pci_channel_offline(ha->pdev) &&
+		    ha->flags.pci_channel_io_perm_failure)) {
+			clear_bit(ISP_ABORT_RETRY, &vha->dpc_flags);
+			return QLA_SUCCESS;
+		}
+
+		scsi_block_requests(vha->host);
+		qlafx00_abort_isp_cleanup(vha);
+	}
+
+	ql_log(ql_log_info, vha, 0x0145,
+	    "(%s): succeeded.\n", __func__);
+
+	return QLA_SUCCESS;
+}
+
+static inline fc_port_t*
+qlafx00_get_fcport(struct scsi_qla_host *vha, int tgt_id)
+{
+	fc_port_t	*fcport;
+
+	/* Check for matching device in remote port list. */
+	fcport = NULL;
+	list_for_each_entry(fcport, &vha->vp_fcports, list) {
+		if (fcport->tgt_id == tgt_id) {
+			ql_dbg(ql_dbg_async, vha, 0x5072,
+			    "Matching fcport(%p) found with TGT-ID: 0x%x "
+			    "and Remote TGT_ID: 0x%x\n",
+			    fcport, fcport->tgt_id, tgt_id);
+			break;
+		}
+	}
+	return fcport;
+}
+
+static void
+qlafx00_tgt_detach(struct scsi_qla_host *vha, int tgt_id)
+{
+	fc_port_t	*fcport;
+
+	ql_log(ql_log_info, vha, 0x5073,
+	    "Detach TGT-ID: 0x%x\n", tgt_id);
+
+	fcport = qlafx00_get_fcport(vha, tgt_id);
+	if (!fcport)
+		return;
+
+	qla2x00_mark_device_lost(vha, fcport, 0, 0);
+
+	return;
+}
+
+int
+qlafx00_process_aen(struct scsi_qla_host *vha, struct qla_work_evt *evt)
+{
+	int rval = 0;
+	uint32_t aen_code, aen_data;
+
+	aen_code = FCH_EVT_VENDOR_UNIQUE;
+	aen_data = evt->u.aenfx.evtcode;
+
+	switch (evt->u.aenfx.evtcode) {
+	case QLAFX00_MBA_PORT_UPDATE:		/* Port database update */
+		if (evt->u.aenfx.mbx[1] == 0) {
+			if (evt->u.aenfx.mbx[2] == 1) {
+				if (!vha->flags.fw_tgt_reported)
+					vha->flags.fw_tgt_reported = 1;
+				atomic_set(&vha->loop_down_timer, 0);
+				atomic_set(&vha->loop_state, LOOP_UP);
+				set_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
+				qla2xxx_wake_dpc(vha);
+			} else if (evt->u.aenfx.mbx[2] == 2) {
+				qlafx00_tgt_detach(vha, evt->u.aenfx.mbx[3]);
+			}
+		} else if (evt->u.aenfx.mbx[1] == 0xffff) {
+			if (evt->u.aenfx.mbx[2] == 1) {
+				if (!vha->flags.fw_tgt_reported)
+					vha->flags.fw_tgt_reported = 1;
+				set_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
+			} else if (evt->u.aenfx.mbx[2] == 2) {
+				vha->device_flags |= DFLG_NO_CABLE;
+				qla2x00_mark_all_devices_lost(vha, 1);
+			}
+		}
+		break;
+	case QLAFX00_MBA_LINK_UP:
+		aen_code = FCH_EVT_LINKUP;
+		aen_data = 0;
+		break;
+	case QLAFX00_MBA_LINK_DOWN:
+		aen_code = FCH_EVT_LINKDOWN;
+		aen_data = 0;
+		break;
+	}
+
+	fc_host_post_event(vha->host, fc_get_event_number(),
+	    aen_code, aen_data);
+
+	return rval;
+}
+
+static void
+qlafx00_update_host_attr(scsi_qla_host_t *vha, struct port_info_data *pinfo)
+{
+	u64 port_name = 0, node_name = 0;
+
+	port_name = (unsigned long long)wwn_to_u64(pinfo->port_name);
+	node_name = (unsigned long long)wwn_to_u64(pinfo->node_name);
+
+	fc_host_node_name(vha->host) = node_name;
+	fc_host_port_name(vha->host) = port_name;
+	if (!pinfo->port_type)
+		vha->hw->current_topology = ISP_CFG_F;
+	if (pinfo->link_status == QLAFX00_LINK_STATUS_UP)
+		atomic_set(&vha->loop_state, LOOP_READY);
+	else if (pinfo->link_status == QLAFX00_LINK_STATUS_DOWN)
+		atomic_set(&vha->loop_state, LOOP_DOWN);
+	vha->hw->link_data_rate = (uint16_t)pinfo->link_config;
+}
+
+static void
+qla2x00_fxdisc_iocb_timeout(void *data)
+{
+	srb_t *sp = (srb_t *)data;
+	struct srb_iocb *lio = &sp->u.iocb_cmd;
+
+	complete(&lio->u.fxiocb.fxiocb_comp);
+}
+
+static void
+qla2x00_fxdisc_sp_done(void *data, void *ptr, int res)
+{
+	srb_t *sp = (srb_t *)ptr;
+	struct srb_iocb *lio = &sp->u.iocb_cmd;
+
+	complete(&lio->u.fxiocb.fxiocb_comp);
+}
+
+int
+qlafx00_fx_disc(scsi_qla_host_t *vha, fc_port_t *fcport, uint8_t fx_type)
+{
+	srb_t *sp;
+	struct srb_iocb *fdisc;
+	int rval = QLA_FUNCTION_FAILED;
+	struct qla_hw_data *ha = vha->hw;
+	struct host_system_info *phost_info;
+	struct register_host_info *preg_hsi;
+	struct new_utsname *p_sysid = NULL;
+	struct timeval tv;
+
+	sp = qla2x00_get_sp(vha, fcport, GFP_KERNEL);
+	if (!sp)
+		goto done;
+
+	fdisc = &sp->u.iocb_cmd;
+	switch (fx_type) {
+	case FXDISC_GET_CONFIG_INFO:
+	fdisc->u.fxiocb.flags =
+		    SRB_FXDISC_RESP_DMA_VALID;
+		fdisc->u.fxiocb.rsp_len = sizeof(struct config_info_data);
+		break;
+	case FXDISC_GET_PORT_INFO:
+		fdisc->u.fxiocb.flags =
+		    SRB_FXDISC_RESP_DMA_VALID | SRB_FXDISC_REQ_DWRD_VALID;
+		fdisc->u.fxiocb.rsp_len = QLAFX00_PORT_DATA_INFO;
+		fdisc->u.fxiocb.req_data = fcport->port_id;
+		break;
+	case FXDISC_GET_TGT_NODE_INFO:
+		fdisc->u.fxiocb.flags =
+		    SRB_FXDISC_RESP_DMA_VALID | SRB_FXDISC_REQ_DWRD_VALID;
+		fdisc->u.fxiocb.rsp_len = QLAFX00_TGT_NODE_INFO;
+		fdisc->u.fxiocb.req_data = fcport->tgt_id;
+		break;
+	case FXDISC_GET_TGT_NODE_LIST:
+		fdisc->u.fxiocb.flags =
+		    SRB_FXDISC_RESP_DMA_VALID | SRB_FXDISC_REQ_DWRD_VALID;
+		fdisc->u.fxiocb.rsp_len = QLAFX00_TGT_NODE_LIST_SIZE;
+		break;
+	case FXDISC_REG_HOST_INFO:
+		fdisc->u.fxiocb.flags = SRB_FXDISC_REQ_DMA_VALID;
+		fdisc->u.fxiocb.req_len = sizeof(struct register_host_info);
+		p_sysid = utsname();
+		if (!p_sysid) {
+			ql_log(ql_log_warn, vha, 0x303c,
+			    "Not able to get the system informtion\n");
+			goto done_free_sp;
+		}
+		break;
+	default:
+		break;
+	}
+
+	if (fdisc->u.fxiocb.flags & SRB_FXDISC_REQ_DMA_VALID) {
+		fdisc->u.fxiocb.req_addr = dma_alloc_coherent(&ha->pdev->dev,
+		    fdisc->u.fxiocb.req_len,
+		    &fdisc->u.fxiocb.req_dma_handle, GFP_KERNEL);
+		if (!fdisc->u.fxiocb.req_addr)
+			goto done_free_sp;
+
+		if (fx_type == FXDISC_REG_HOST_INFO) {
+			preg_hsi = (struct register_host_info *)
+				fdisc->u.fxiocb.req_addr;
+			phost_info = &preg_hsi->hsi;
+			memset(preg_hsi, 0, sizeof(struct register_host_info));
+			phost_info->os_type = OS_TYPE_LINUX;
+			strncpy(phost_info->sysname,
+			    p_sysid->sysname, SYSNAME_LENGTH);
+			strncpy(phost_info->nodename,
+			    p_sysid->nodename, NODENAME_LENGTH);
+			strncpy(phost_info->release,
+			    p_sysid->release, RELEASE_LENGTH);
+			strncpy(phost_info->version,
+			    p_sysid->version, VERSION_LENGTH);
+			strncpy(phost_info->machine,
+			    p_sysid->machine, MACHINE_LENGTH);
+			strncpy(phost_info->domainname,
+			    p_sysid->domainname, DOMNAME_LENGTH);
+			strncpy(phost_info->hostdriver,
+			    QLA2XXX_VERSION, VERSION_LENGTH);
+			do_gettimeofday(&tv);
+			preg_hsi->utc = (uint64_t)tv.tv_sec;
+			ql_dbg(ql_dbg_init, vha, 0x0149,
+			    "ISP%04X: Host registration with firmware\n",
+			    ha->pdev->device);
+			ql_dbg(ql_dbg_init, vha, 0x014a,
+			    "os_type = '%d', sysname = '%s', nodname = '%s'\n",
+			    phost_info->os_type,
+			    phost_info->sysname,
+			    phost_info->nodename);
+			ql_dbg(ql_dbg_init, vha, 0x014b,
+			    "release = '%s', version = '%s'\n",
+			    phost_info->release,
+			    phost_info->version);
+			ql_dbg(ql_dbg_init, vha, 0x014c,
+			    "machine = '%s' "
+			    "domainname = '%s', hostdriver = '%s'\n",
+			    phost_info->machine,
+			    phost_info->domainname,
+			    phost_info->hostdriver);
+			ql_dump_buffer(ql_dbg_init + ql_dbg_disc, vha, 0x014d,
+			    (uint8_t *)phost_info,
+			    sizeof(struct host_system_info));
+		}
+	}
+
+	if (fdisc->u.fxiocb.flags & SRB_FXDISC_RESP_DMA_VALID) {
+		fdisc->u.fxiocb.rsp_addr = dma_alloc_coherent(&ha->pdev->dev,
+		    fdisc->u.fxiocb.rsp_len,
+		    &fdisc->u.fxiocb.rsp_dma_handle, GFP_KERNEL);
+		if (!fdisc->u.fxiocb.rsp_addr)
+			goto done_unmap_req;
+	}
+
+	sp->type = SRB_FXIOCB_DCMD;
+	sp->name = "fxdisc";
+	qla2x00_init_timer(sp, FXDISC_TIMEOUT);
+	fdisc->timeout = qla2x00_fxdisc_iocb_timeout;
+	fdisc->u.fxiocb.req_func_type = fx_type;
+	sp->done = qla2x00_fxdisc_sp_done;
+
+	rval = qla2x00_start_sp(sp);
+	if (rval != QLA_SUCCESS)
+		goto done_unmap_dma;
+
+	wait_for_completion(&fdisc->u.fxiocb.fxiocb_comp);
+
+	if (fx_type == FXDISC_GET_CONFIG_INFO) {
+		struct config_info_data *pinfo =
+		    (struct config_info_data *) fdisc->u.fxiocb.rsp_addr;
+		memcpy(&vha->hw->mr.product_name, pinfo->product_name,
+		    sizeof(vha->hw->mr.product_name));
+		memcpy(&vha->hw->mr.symbolic_name, pinfo->symbolic_name,
+		    sizeof(vha->hw->mr.symbolic_name));
+		memcpy(&vha->hw->mr.serial_num, pinfo->serial_num,
+		    sizeof(vha->hw->mr.serial_num));
+		memcpy(&vha->hw->mr.hw_version, pinfo->hw_version,
+		    sizeof(vha->hw->mr.hw_version));
+		memcpy(&vha->hw->mr.fw_version, pinfo->fw_version,
+		    sizeof(vha->hw->mr.fw_version));
+		strim(vha->hw->mr.fw_version);
+		memcpy(&vha->hw->mr.uboot_version, pinfo->uboot_version,
+		    sizeof(vha->hw->mr.uboot_version));
+		memcpy(&vha->hw->mr.fru_serial_num, pinfo->fru_serial_num,
+		    sizeof(vha->hw->mr.fru_serial_num));
+	} else if (fx_type == FXDISC_GET_PORT_INFO) {
+		struct port_info_data *pinfo =
+		    (struct port_info_data *) fdisc->u.fxiocb.rsp_addr;
+		memcpy(vha->node_name, pinfo->node_name, WWN_SIZE);
+		memcpy(vha->port_name, pinfo->port_name, WWN_SIZE);
+		vha->d_id.b.domain = pinfo->port_id[0];
+		vha->d_id.b.area = pinfo->port_id[1];
+		vha->d_id.b.al_pa = pinfo->port_id[2];
+		qlafx00_update_host_attr(vha, pinfo);
+		ql_dump_buffer(ql_dbg_init + ql_dbg_buffer, vha, 0x0141,
+		    (uint8_t *)pinfo, 16);
+	} else if (fx_type == FXDISC_GET_TGT_NODE_INFO) {
+		struct qlafx00_tgt_node_info *pinfo =
+		    (struct qlafx00_tgt_node_info *) fdisc->u.fxiocb.rsp_addr;
+		memcpy(fcport->node_name, pinfo->tgt_node_wwnn, WWN_SIZE);
+		memcpy(fcport->port_name, pinfo->tgt_node_wwpn, WWN_SIZE);
+		fcport->port_type = FCT_TARGET;
+		ql_dump_buffer(ql_dbg_init + ql_dbg_buffer, vha, 0x0144,
+		    (uint8_t *)pinfo, 16);
+	} else if (fx_type == FXDISC_GET_TGT_NODE_LIST) {
+		struct qlafx00_tgt_node_info *pinfo =
+		    (struct qlafx00_tgt_node_info *) fdisc->u.fxiocb.rsp_addr;
+		ql_dump_buffer(ql_dbg_init + ql_dbg_buffer, vha, 0x0146,
+		    (uint8_t *)pinfo, 16);
+		memcpy(vha->hw->gid_list, pinfo, QLAFX00_TGT_NODE_LIST_SIZE);
+	}
+	rval = fdisc->u.fxiocb.result;
+
+done_unmap_dma:
+	if (fdisc->u.fxiocb.rsp_addr)
+		dma_free_coherent(&ha->pdev->dev, fdisc->u.fxiocb.rsp_len,
+		    fdisc->u.fxiocb.rsp_addr, fdisc->u.fxiocb.rsp_dma_handle);
+
+done_unmap_req:
+	if (fdisc->u.fxiocb.req_addr)
+		dma_free_coherent(&ha->pdev->dev, fdisc->u.fxiocb.req_len,
+		    fdisc->u.fxiocb.req_addr, fdisc->u.fxiocb.req_dma_handle);
+done_free_sp:
+	sp->free(vha, sp);
+done:
+	return rval;
+}
+
+static void
+qlafx00_abort_iocb_timeout(void *data)
+{
+	srb_t *sp = (srb_t *)data;
+	struct srb_iocb *abt = &sp->u.iocb_cmd;
+
+	abt->u.abt.comp_status = CS_TIMEOUT;
+	complete(&abt->u.abt.comp);
+}
+
+static void
+qlafx00_abort_sp_done(void *data, void *ptr, int res)
+{
+	srb_t *sp = (srb_t *)ptr;
+	struct srb_iocb *abt = &sp->u.iocb_cmd;
+
+	complete(&abt->u.abt.comp);
+}
+
+static int
+qlafx00_async_abt_cmd(srb_t *cmd_sp)
+{
+	scsi_qla_host_t *vha = cmd_sp->fcport->vha;
+	fc_port_t *fcport = cmd_sp->fcport;
+	struct srb_iocb *abt_iocb;
+	srb_t *sp;
+	int rval = QLA_FUNCTION_FAILED;
+
+	sp = qla2x00_get_sp(vha, fcport, GFP_KERNEL);
+	if (!sp)
+		goto done;
+
+	abt_iocb = &sp->u.iocb_cmd;
+	sp->type = SRB_ABT_CMD;
+	sp->name = "abort";
+	qla2x00_init_timer(sp, FXDISC_TIMEOUT);
+	abt_iocb->u.abt.cmd_hndl = cmd_sp->handle;
+	sp->done = qlafx00_abort_sp_done;
+	abt_iocb->timeout = qlafx00_abort_iocb_timeout;
+	init_completion(&abt_iocb->u.abt.comp);
+
+	rval = qla2x00_start_sp(sp);
+	if (rval != QLA_SUCCESS)
+		goto done_free_sp;
+
+	ql_dbg(ql_dbg_async, vha, 0x507c,
+	    "Abort command issued - hdl=%x, target_id=%x\n",
+	    cmd_sp->handle, fcport->tgt_id);
+
+	wait_for_completion(&abt_iocb->u.abt.comp);
+
+	rval = abt_iocb->u.abt.comp_status == CS_COMPLETE ?
+	    QLA_SUCCESS : QLA_FUNCTION_FAILED;
+
+done_free_sp:
+	sp->free(vha, sp);
+done:
+	return rval;
+}
+
+int
+qlafx00_abort_command(srb_t *sp)
+{
+	unsigned long   flags = 0;
+
+	uint32_t	handle;
+	fc_port_t	*fcport = sp->fcport;
+	struct scsi_qla_host *vha = fcport->vha;
+	struct qla_hw_data *ha = vha->hw;
+	struct req_que *req = vha->req;
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+	for (handle = 1; handle < DEFAULT_OUTSTANDING_COMMANDS; handle++) {
+		if (req->outstanding_cmds[handle] == sp)
+			break;
+	}
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	if (handle == DEFAULT_OUTSTANDING_COMMANDS) {
+		/* Command not found. */
+		return QLA_FUNCTION_FAILED;
+	}
+	return qlafx00_async_abt_cmd(sp);
+}
+
+/*
+ * qlafx00_initialize_adapter
+ *      Initialize board.
+ *
+ * Input:
+ *      ha = adapter block pointer.
+ *
+ * Returns:
+ *      0 = success
+ */
+int
+qlafx00_initialize_adapter(scsi_qla_host_t *vha)
+{
+	int	rval;
+	struct qla_hw_data *ha = vha->hw;
+
+	/* Clear adapter flags. */
+	vha->flags.online = 0;
+	ha->flags.chip_reset_done = 0;
+	vha->flags.reset_active = 0;
+	ha->flags.pci_channel_io_perm_failure = 0;
+	ha->flags.eeh_busy = 0;
+	ha->thermal_support = 0;
+	atomic_set(&vha->loop_down_timer, LOOP_DOWN_TIME);
+	atomic_set(&vha->loop_state, LOOP_DOWN);
+	vha->device_flags = DFLG_NO_CABLE;
+	vha->dpc_flags = 0;
+	vha->flags.management_server_logged_in = 0;
+	vha->marker_needed = 0;
+	ha->isp_abort_cnt = 0;
+	ha->beacon_blink_led = 0;
+
+	set_bit(0, ha->req_qid_map);
+	set_bit(0, ha->rsp_qid_map);
+
+	ql_dbg(ql_dbg_init, vha, 0x0147,
+	    "Configuring PCI space...\n");
+
+	rval = ha->isp_ops->pci_config(vha);
+	if (rval) {
+		ql_log(ql_log_warn, vha, 0x0148,
+		    "Unable to configure PCI space.\n");
+		return rval;
+	}
+
+	rval = qlafx00_init_fw_ready(vha);
+	if (rval != QLA_SUCCESS)
+		return rval;
+
+	qlafx00_save_queue_ptrs(vha);
+
+	rval = qlafx00_config_queues(vha);
+	if (rval != QLA_SUCCESS)
+		return rval;
+
+	/*
+	 * Allocate the array of outstanding commands
+	 * now that we know the firmware resources.
+	 */
+	rval = qla2x00_alloc_outstanding_cmds(ha, vha->req);
+	if (rval != QLA_SUCCESS)
+		return rval;
+
+	rval = qla2x00_init_rings(vha);
+	ha->flags.chip_reset_done = 1;
+
+	return rval;
+}
+
+uint32_t
+qlafx00_fw_state_show(struct device *dev, struct device_attribute *attr,
+		      char *buf)
+{
+	scsi_qla_host_t *vha = shost_priv(class_to_shost(dev));
+	int rval = QLA_FUNCTION_FAILED;
+	uint32_t state[1];
+
+	if (qla2x00_reset_active(vha))
+		ql_log(ql_log_warn, vha, 0x70ce,
+		    "ISP reset active.\n");
+	else if (!vha->hw->flags.eeh_busy) {
+		rval = qlafx00_get_firmware_state(vha, state);
+	}
+	if (rval != QLA_SUCCESS)
+		memset(state, -1, sizeof(state));
+
+	return state[0];
+}
+
+void
+qlafx00_get_host_speed(struct Scsi_Host *shost)
+{
+	struct qla_hw_data *ha = ((struct scsi_qla_host *)
+					(shost_priv(shost)))->hw;
+	u32 speed = FC_PORTSPEED_UNKNOWN;
+
+	switch (ha->link_data_rate) {
+	case QLAFX00_PORT_SPEED_2G:
+		speed = FC_PORTSPEED_2GBIT;
+		break;
+	case QLAFX00_PORT_SPEED_4G:
+		speed = FC_PORTSPEED_4GBIT;
+		break;
+	case QLAFX00_PORT_SPEED_8G:
+		speed = FC_PORTSPEED_8GBIT;
+		break;
+	case QLAFX00_PORT_SPEED_10G:
+		speed = FC_PORTSPEED_10GBIT;
+		break;
+	}
+	fc_host_speed(shost) = speed;
+}
+
+/** QLAFX00 specific ISR implementation functions */
+
+static inline void
+qlafx00_handle_sense(srb_t *sp, uint8_t *sense_data, uint32_t par_sense_len,
+		     uint32_t sense_len, struct rsp_que *rsp, int res)
+{
+	struct scsi_qla_host *vha = sp->fcport->vha;
+	struct scsi_cmnd *cp = GET_CMD_SP(sp);
+	uint32_t track_sense_len;
+
+	SET_FW_SENSE_LEN(sp, sense_len);
+
+	if (sense_len >= SCSI_SENSE_BUFFERSIZE)
+		sense_len = SCSI_SENSE_BUFFERSIZE;
+
+	SET_CMD_SENSE_LEN(sp, sense_len);
+	SET_CMD_SENSE_PTR(sp, cp->sense_buffer);
+	track_sense_len = sense_len;
+
+	if (sense_len > par_sense_len)
+		sense_len = par_sense_len;
+
+	memcpy(cp->sense_buffer, sense_data, sense_len);
+
+	SET_FW_SENSE_LEN(sp, GET_FW_SENSE_LEN(sp) - sense_len);
+
+	SET_CMD_SENSE_PTR(sp, cp->sense_buffer + sense_len);
+	track_sense_len -= sense_len;
+	SET_CMD_SENSE_LEN(sp, track_sense_len);
+
+	ql_dbg(ql_dbg_io, vha, 0x304d,
+	    "sense_len=0x%x par_sense_len=0x%x track_sense_len=0x%x.\n",
+	    sense_len, par_sense_len, track_sense_len);
+	if (GET_FW_SENSE_LEN(sp) > 0) {
+		rsp->status_srb = sp;
+		cp->result = res;
+	}
+
+	if (sense_len) {
+		ql_dbg(ql_dbg_io + ql_dbg_buffer, vha, 0x3039,
+		    "Check condition Sense data, nexus%ld:%d:%d cmd=%p.\n",
+		    sp->fcport->vha->host_no, cp->device->id, cp->device->lun,
+		    cp);
+		ql_dump_buffer(ql_dbg_io + ql_dbg_buffer, vha, 0x3049,
+		    cp->sense_buffer, sense_len);
+	}
+}
+
+static void
+qlafx00_tm_iocb_entry(scsi_qla_host_t *vha, struct req_que *req,
+		      struct tsk_mgmt_entry_fx00 *pkt, srb_t *sp,
+		      uint16_t sstatus, uint16_t cpstatus)
+{
+	struct srb_iocb *tmf;
+
+	tmf = &sp->u.iocb_cmd;
+	if (cpstatus != CS_COMPLETE ||
+	    (sstatus & SS_RESPONSE_INFO_LEN_VALID))
+		cpstatus = CS_INCOMPLETE;
+	tmf->u.tmf.comp_status = cpstatus;
+	sp->done(vha, sp, 0);
+}
+
+static void
+qlafx00_abort_iocb_entry(scsi_qla_host_t *vha, struct req_que *req,
+			 struct abort_iocb_entry_fx00 *pkt)
+{
+	const char func[] = "ABT_IOCB";
+	srb_t *sp;
+	struct srb_iocb *abt;
+
+	sp = qla2x00_get_sp_from_handle(vha, func, req, pkt);
+	if (!sp)
+		return;
+
+	abt = &sp->u.iocb_cmd;
+	abt->u.abt.comp_status = le32_to_cpu(pkt->tgt_id_sts);
+	sp->done(vha, sp, 0);
+}
+
+static void
+qlafx00_ioctl_iosb_entry(scsi_qla_host_t *vha, struct req_que *req,
+			 struct ioctl_iocb_entry_fx00 *pkt)
+{
+	const char func[] = "IOSB_IOCB";
+	srb_t *sp;
+	struct fc_bsg_job *bsg_job;
+	struct srb_iocb *iocb_job;
+	int res;
+	struct qla_mt_iocb_rsp_fx00 fstatus;
+	uint8_t	*fw_sts_ptr;
+
+	sp = qla2x00_get_sp_from_handle(vha, func, req, pkt);
+	if (!sp)
+		return;
+
+	if (sp->type == SRB_FXIOCB_DCMD) {
+		iocb_job = &sp->u.iocb_cmd;
+		iocb_job->u.fxiocb.seq_number = le32_to_cpu(pkt->seq_no);
+		iocb_job->u.fxiocb.fw_flags = le32_to_cpu(pkt->fw_iotcl_flags);
+		iocb_job->u.fxiocb.result = le32_to_cpu(pkt->status);
+		if (iocb_job->u.fxiocb.flags & SRB_FXDISC_RSP_DWRD_VALID)
+			iocb_job->u.fxiocb.req_data =
+			    le32_to_cpu(pkt->dataword_r);
+	} else {
+		bsg_job = sp->u.bsg_job;
+
+		memset(&fstatus, 0, sizeof(struct qla_mt_iocb_rsp_fx00));
+
+		fstatus.reserved_1 = pkt->reserved_0;
+		fstatus.func_type = pkt->comp_func_num;
+		fstatus.ioctl_flags = pkt->fw_iotcl_flags;
+		fstatus.ioctl_data = pkt->dataword_r;
+		fstatus.adapid = pkt->adapid;
+		fstatus.adapid_hi = pkt->adapid_hi;
+		fstatus.reserved_2 = pkt->reserved_1;
+		fstatus.res_count = pkt->residuallen;
+		fstatus.status = pkt->status;
+		fstatus.seq_number = pkt->seq_no;
+		memcpy(fstatus.reserved_3,
+		    pkt->reserved_2, 20 * sizeof(uint8_t));
+
+		fw_sts_ptr = ((uint8_t *)bsg_job->req->sense) +
+		    sizeof(struct fc_bsg_reply);
+
+		memcpy(fw_sts_ptr, (uint8_t *)&fstatus,
+		    sizeof(struct qla_mt_iocb_rsp_fx00));
+		bsg_job->reply_len = sizeof(struct fc_bsg_reply) +
+			sizeof(struct qla_mt_iocb_rsp_fx00) + sizeof(uint8_t);
+
+		ql_dump_buffer(ql_dbg_user + ql_dbg_verbose,
+		    sp->fcport->vha, 0x5080,
+		    (uint8_t *)pkt, sizeof(struct ioctl_iocb_entry_fx00));
+
+		ql_dump_buffer(ql_dbg_user + ql_dbg_verbose,
+		    sp->fcport->vha, 0x5074,
+		    (uint8_t *)fw_sts_ptr, sizeof(struct qla_mt_iocb_rsp_fx00));
+
+		res = bsg_job->reply->result = DID_OK << 16;
+		bsg_job->reply->reply_payload_rcv_len =
+		    bsg_job->reply_payload.payload_len;
+	}
+	sp->done(vha, sp, res);
+}
+
+/**
+ * qlafx00_status_entry() - Process a Status IOCB entry.
+ * @ha: SCSI driver HA context
+ * @pkt: Entry pointer
+ */
+static void
+qlafx00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
+{
+	srb_t		*sp;
+	fc_port_t	*fcport;
+	struct scsi_cmnd *cp;
+	struct sts_entry_fx00 *sts;
+	uint16_t	comp_status;
+	uint16_t	scsi_status;
+	uint16_t	ox_id;
+	uint8_t		lscsi_status;
+	int32_t		resid;
+	uint32_t	sense_len, par_sense_len, rsp_info_len, resid_len,
+	    fw_resid_len;
+	uint8_t		*rsp_info = NULL, *sense_data = NULL;
+	struct qla_hw_data *ha = vha->hw;
+	uint32_t hindex, handle;
+	uint16_t que;
+	struct req_que *req;
+	int logit = 1;
+	int res = 0;
+
+	sts = (struct sts_entry_fx00 *) pkt;
+
+	comp_status = le16_to_cpu(sts->comp_status);
+	scsi_status = le16_to_cpu(sts->scsi_status) & SS_MASK;
+	hindex = sts->handle;
+	handle = LSW(hindex);
+
+	que = MSW(hindex);
+	req = ha->req_q_map[que];
+
+	/* Validate handle. */
+	if (handle < req->num_outstanding_cmds)
+		sp = req->outstanding_cmds[handle];
+	else
+		sp = NULL;
+
+	if (sp == NULL) {
+		ql_dbg(ql_dbg_io, vha, 0x3034,
+		    "Invalid status handle (0x%x).\n", handle);
+
+		set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+		qla2xxx_wake_dpc(vha);
+		return;
+	}
+
+	if (sp->type == SRB_TM_CMD) {
+		req->outstanding_cmds[handle] = NULL;
+		qlafx00_tm_iocb_entry(vha, req, pkt, sp,
+		    scsi_status, comp_status);
+		return;
+	}
+
+	/* Fast path completion. */
+	if (comp_status == CS_COMPLETE && scsi_status == 0) {
+		qla2x00_do_host_ramp_up(vha);
+		qla2x00_process_completed_request(vha, req, handle);
+		return;
+	}
+
+	req->outstanding_cmds[handle] = NULL;
+	cp = GET_CMD_SP(sp);
+	if (cp == NULL) {
+		ql_dbg(ql_dbg_io, vha, 0x3048,
+		    "Command already returned (0x%x/%p).\n",
+		    handle, sp);
+
+		return;
+	}
+
+	lscsi_status = scsi_status & STATUS_MASK;
+
+	fcport = sp->fcport;
+
+	ox_id = 0;
+	sense_len = par_sense_len = rsp_info_len = resid_len =
+		fw_resid_len = 0;
+	if (scsi_status & SS_SENSE_LEN_VALID)
+		sense_len = le32_to_cpu(sts->sense_len);
+	if (scsi_status & (SS_RESIDUAL_UNDER | SS_RESIDUAL_OVER))
+		resid_len = le32_to_cpu(sts->residual_len);
+	if (comp_status == CS_DATA_UNDERRUN)
+		fw_resid_len = le32_to_cpu(sts->residual_len);
+	rsp_info = sense_data = sts->data;
+	par_sense_len = sizeof(sts->data);
+
+	/* Check for overrun. */
+	if (comp_status == CS_COMPLETE &&
+	    scsi_status & SS_RESIDUAL_OVER)
+		comp_status = CS_DATA_OVERRUN;
+
+	/*
+	 * Based on Host and scsi status generate status code for Linux
+	 */
+	switch (comp_status) {
+	case CS_COMPLETE:
+	case CS_QUEUE_FULL:
+		if (scsi_status == 0) {
+			res = DID_OK << 16;
+			break;
+		}
+		if (scsi_status & (SS_RESIDUAL_UNDER | SS_RESIDUAL_OVER)) {
+			resid = resid_len;
+			scsi_set_resid(cp, resid);
+
+			if (!lscsi_status &&
+			    ((unsigned)(scsi_bufflen(cp) - resid) <
+			     cp->underflow)) {
+				ql_dbg(ql_dbg_io, fcport->vha, 0x3050,
+				    "Mid-layer underflow "
+				    "detected (0x%x of 0x%x bytes).\n",
+				    resid, scsi_bufflen(cp));
+
+				res = DID_ERROR << 16;
+				break;
+			}
+		}
+		res = DID_OK << 16 | lscsi_status;
+
+		if (lscsi_status == SAM_STAT_TASK_SET_FULL) {
+			ql_dbg(ql_dbg_io, fcport->vha, 0x3051,
+			    "QUEUE FULL detected.\n");
+			break;
+		}
+		logit = 0;
+		if (lscsi_status != SS_CHECK_CONDITION)
+			break;
+
+		memset(cp->sense_buffer, 0, SCSI_SENSE_BUFFERSIZE);
+		if (!(scsi_status & SS_SENSE_LEN_VALID))
+			break;
+
+		qlafx00_handle_sense(sp, sense_data, par_sense_len, sense_len,
+		    rsp, res);
+		break;
+
+	case CS_DATA_UNDERRUN:
+		/* Use F/W calculated residual length. */
+		if (IS_FWI2_CAPABLE(ha) || IS_QLAFX00(ha))
+			resid = fw_resid_len;
+		else
+			resid = resid_len;
+		scsi_set_resid(cp, resid);
+		if (scsi_status & SS_RESIDUAL_UNDER) {
+			if ((IS_FWI2_CAPABLE(ha) || IS_QLAFX00(ha))
+			    && fw_resid_len != resid_len) {
+				ql_dbg(ql_dbg_io, fcport->vha, 0x3052,
+				    "Dropped frame(s) detected "
+				    "(0x%x of 0x%x bytes).\n",
+				    resid, scsi_bufflen(cp));
+
+				res = DID_ERROR << 16 | lscsi_status;
+				goto check_scsi_status;
+			}
+
+			if (!lscsi_status &&
+			    ((unsigned)(scsi_bufflen(cp) - resid) <
+			    cp->underflow)) {
+				ql_dbg(ql_dbg_io, fcport->vha, 0x3053,
+				    "Mid-layer underflow "
+				    "detected (0x%x of 0x%x bytes, "
+				    "cp->underflow: 0x%x).\n",
+				    resid, scsi_bufflen(cp), cp->underflow);
+
+				res = DID_ERROR << 16;
+				break;
+			}
+		} else if (lscsi_status != SAM_STAT_TASK_SET_FULL &&
+			    lscsi_status != SAM_STAT_BUSY) {
+			/*
+			 * scsi status of task set and busy are considered
+			 * to be task not completed.
+			 */
+
+			ql_dbg(ql_dbg_io, fcport->vha, 0x3054,
+			    "Dropped frame(s) detected (0x%x "
+			    "of 0x%x bytes).\n", resid,
+			    scsi_bufflen(cp));
+
+			res = DID_ERROR << 16 | lscsi_status;
+			goto check_scsi_status;
+		} else {
+			ql_dbg(ql_dbg_io, fcport->vha, 0x3055,
+			    "scsi_status: 0x%x, lscsi_status: 0x%x\n",
+			    scsi_status, lscsi_status);
+		}
+
+		res = DID_OK << 16 | lscsi_status;
+		logit = 0;
+
+check_scsi_status:
+		/*
+		 * Check to see if SCSI Status is non zero. If so report SCSI
+		 * Status.
+		 */
+		if (lscsi_status != 0) {
+			if (lscsi_status == SAM_STAT_TASK_SET_FULL) {
+				ql_dbg(ql_dbg_io, fcport->vha, 0x3056,
+				    "QUEUE FULL detected.\n");
+				logit = 1;
+				break;
+			}
+			if (lscsi_status != SS_CHECK_CONDITION)
+				break;
+
+			memset(cp->sense_buffer, 0, SCSI_SENSE_BUFFERSIZE);
+			if (!(scsi_status & SS_SENSE_LEN_VALID))
+				break;
+
+			qlafx00_handle_sense(sp, sense_data, par_sense_len,
+			    sense_len, rsp, res);
+		}
+		break;
+
+	case CS_PORT_LOGGED_OUT:
+	case CS_PORT_CONFIG_CHG:
+	case CS_PORT_BUSY:
+	case CS_INCOMPLETE:
+	case CS_PORT_UNAVAILABLE:
+	case CS_TIMEOUT:
+	case CS_RESET:
+
+		/*
+		 * We are going to have the fc class block the rport
+		 * while we try to recover so instruct the mid layer
+		 * to requeue until the class decides how to handle this.
+		 */
+		res = DID_TRANSPORT_DISRUPTED << 16;
+
+		ql_dbg(ql_dbg_io, fcport->vha, 0x3057,
+		    "Port down status: port-state=0x%x.\n",
+		    atomic_read(&fcport->state));
+
+		if (atomic_read(&fcport->state) == FCS_ONLINE)
+			qla2x00_mark_device_lost(fcport->vha, fcport, 1, 1);
+		break;
+
+	case CS_ABORTED:
+		res = DID_RESET << 16;
+		break;
+
+	default:
+		res = DID_ERROR << 16;
+		break;
+	}
+
+	if (logit)
+		ql_dbg(ql_dbg_io, fcport->vha, 0x3058,
+		    "FCP command status: 0x%x-0x%x (0x%x) "
+		    "nexus=%ld:%d:%d tgt_id: 0x%x lscsi_status: 0x%x"
+		    "cdb=%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x len=0x%x "
+		    "rsp_info=0x%x resid=0x%x fw_resid=0x%x "
+		    "sense_len=0x%x, par_sense_len=0x%x, rsp_info_len=0x%x\n",
+		    comp_status, scsi_status, res, vha->host_no,
+		    cp->device->id, cp->device->lun, fcport->tgt_id,
+		    lscsi_status, cp->cmnd[0], cp->cmnd[1], cp->cmnd[2],
+		    cp->cmnd[3], cp->cmnd[4], cp->cmnd[5], cp->cmnd[6],
+		    cp->cmnd[7], cp->cmnd[8], cp->cmnd[9], scsi_bufflen(cp),
+		    rsp_info_len, resid_len, fw_resid_len, sense_len,
+		    par_sense_len, rsp_info_len);
+
+	if (!res)
+		qla2x00_do_host_ramp_up(vha);
+
+	if (rsp->status_srb == NULL)
+		sp->done(ha, sp, res);
+}
+
+/**
+ * qlafx00_status_cont_entry() - Process a Status Continuations entry.
+ * @ha: SCSI driver HA context
+ * @pkt: Entry pointer
+ *
+ * Extended sense data.
+ */
+static void
+qlafx00_status_cont_entry(struct rsp_que *rsp, sts_cont_entry_t *pkt)
+{
+	uint8_t	sense_sz = 0;
+	struct qla_hw_data *ha = rsp->hw;
+	struct scsi_qla_host *vha = pci_get_drvdata(ha->pdev);
+	srb_t *sp = rsp->status_srb;
+	struct scsi_cmnd *cp;
+	uint32_t sense_len;
+	uint8_t *sense_ptr;
+
+	if (!sp) {
+		ql_dbg(ql_dbg_io, vha, 0x3037,
+		    "no SP, sp = %p\n", sp);
+		return;
+	}
+
+	if (!GET_FW_SENSE_LEN(sp)) {
+		ql_dbg(ql_dbg_io, vha, 0x304b,
+		    "no fw sense data, sp = %p\n", sp);
+		return;
+	}
+	cp = GET_CMD_SP(sp);
+	if (cp == NULL) {
+		ql_log(ql_log_warn, vha, 0x303b,
+		    "cmd is NULL: already returned to OS (sp=%p).\n", sp);
+
+		rsp->status_srb = NULL;
+		return;
+	}
+
+	if (!GET_CMD_SENSE_LEN(sp)) {
+		ql_dbg(ql_dbg_io, vha, 0x304c,
+		    "no sense data, sp = %p\n", sp);
+	} else {
+		sense_len = GET_CMD_SENSE_LEN(sp);
+		sense_ptr = GET_CMD_SENSE_PTR(sp);
+		ql_dbg(ql_dbg_io, vha, 0x304f,
+		    "sp=%p sense_len=0x%x sense_ptr=%p.\n",
+		    sp, sense_len, sense_ptr);
+
+		if (sense_len > sizeof(pkt->data))
+			sense_sz = sizeof(pkt->data);
+		else
+			sense_sz = sense_len;
+
+		/* Move sense data. */
+		ql_dump_buffer(ql_dbg_io + ql_dbg_buffer, vha, 0x304e,
+		    (uint8_t *)pkt, sizeof(sts_cont_entry_t));
+		memcpy(sense_ptr, pkt->data, sense_sz);
+		ql_dump_buffer(ql_dbg_io + ql_dbg_buffer, vha, 0x304a,
+		    sense_ptr, sense_sz);
+
+		sense_len -= sense_sz;
+		sense_ptr += sense_sz;
+
+		SET_CMD_SENSE_PTR(sp, sense_ptr);
+		SET_CMD_SENSE_LEN(sp, sense_len);
+	}
+	sense_len = GET_FW_SENSE_LEN(sp);
+	sense_len = (sense_len > sizeof(pkt->data)) ?
+	    (sense_len - sizeof(pkt->data)) : 0;
+	SET_FW_SENSE_LEN(sp, sense_len);
+
+	/* Place command on done queue. */
+	if (sense_len == 0) {
+		rsp->status_srb = NULL;
+		sp->done(ha, sp, cp->result);
+	}
+}
+
+/**
+ * qlafx00_multistatus_entry() - Process Multi response queue entries.
+ * @ha: SCSI driver HA context
+ */
+static void
+qlafx00_multistatus_entry(struct scsi_qla_host *vha,
+	struct rsp_que *rsp, void *pkt)
+{
+	srb_t		*sp;
+	struct multi_sts_entry_fx00 *stsmfx;
+	struct qla_hw_data *ha = vha->hw;
+	uint32_t handle, hindex, handle_count, i;
+	uint16_t que;
+	struct req_que *req;
+	uint32_t *handle_ptr;
+
+	stsmfx = (struct multi_sts_entry_fx00 *) pkt;
+
+	handle_count = stsmfx->handle_count;
+
+	if (handle_count > MAX_HANDLE_COUNT) {
+		ql_dbg(ql_dbg_io, vha, 0x3035,
+		    "Invalid handle count (0x%x).\n", handle_count);
+		set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+		qla2xxx_wake_dpc(vha);
+		return;
+	}
+
+	handle_ptr = (uint32_t *) &stsmfx->handles[0];
+
+	for (i = 0; i < handle_count; i++) {
+		hindex = le32_to_cpu(*handle_ptr);
+		handle = LSW(hindex);
+		que = MSW(hindex);
+		req = ha->req_q_map[que];
+
+		/* Validate handle. */
+		if (handle < req->num_outstanding_cmds)
+			sp = req->outstanding_cmds[handle];
+		else
+			sp = NULL;
+
+		if (sp == NULL) {
+			ql_dbg(ql_dbg_io, vha, 0x3044,
+			    "Invalid status handle (0x%x).\n", handle);
+			set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+			qla2xxx_wake_dpc(vha);
+			return;
+		}
+		qla2x00_process_completed_request(vha, req, handle);
+		handle_ptr++;
+	}
+}
+
+/**
+ * qlafx00_error_entry() - Process an error entry.
+ * @ha: SCSI driver HA context
+ * @pkt: Entry pointer
+ */
+static void
+qlafx00_error_entry(scsi_qla_host_t *vha, struct rsp_que *rsp,
+		    struct sts_entry_fx00 *pkt, uint8_t estatus, uint8_t etype)
+{
+	srb_t *sp;
+	struct qla_hw_data *ha = vha->hw;
+	const char func[] = "ERROR-IOCB";
+	uint16_t que = MSW(pkt->handle);
+	struct req_que *req = NULL;
+	int res = DID_ERROR << 16;
+
+	ql_dbg(ql_dbg_async, vha, 0x507f,
+	    "type of error status in response: 0x%x\n", estatus);
+
+	req = ha->req_q_map[que];
+
+	sp = qla2x00_get_sp_from_handle(vha, func, req, pkt);
+	if (sp) {
+		sp->done(ha, sp, res);
+		return;
+	}
+
+	set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+	qla2xxx_wake_dpc(vha);
+}
+
+/**
+ * qlafx00_process_response_queue() - Process response queue entries.
+ * @ha: SCSI driver HA context
+ */
+static void
+qlafx00_process_response_queue(struct scsi_qla_host *vha,
+	struct rsp_que *rsp)
+{
+	struct sts_entry_fx00 *pkt;
+	response_t *lptr;
+
+	if (!vha->flags.online)
+		return;
+
+	while (RD_REG_DWORD(&(rsp->ring_ptr->signature)) !=
+	    RESPONSE_PROCESSED) {
+		lptr = rsp->ring_ptr;
+		memcpy_fromio(rsp->rsp_pkt, lptr, sizeof(rsp->rsp_pkt));
+		pkt = (struct sts_entry_fx00 *)rsp->rsp_pkt;
+
+		rsp->ring_index++;
+		if (rsp->ring_index == rsp->length) {
+			rsp->ring_index = 0;
+			rsp->ring_ptr = rsp->ring;
+		} else {
+			rsp->ring_ptr++;
+		}
+
+		if (pkt->entry_status != 0 &&
+		    pkt->entry_type != IOCTL_IOSB_TYPE_FX00) {
+			qlafx00_error_entry(vha, rsp,
+			    (struct sts_entry_fx00 *)pkt, pkt->entry_status,
+			    pkt->entry_type);
+			goto next_iter;
+			continue;
+		}
+
+		switch (pkt->entry_type) {
+		case STATUS_TYPE_FX00:
+			qlafx00_status_entry(vha, rsp, pkt);
+			break;
+
+		case STATUS_CONT_TYPE_FX00:
+			qlafx00_status_cont_entry(rsp, (sts_cont_entry_t *)pkt);
+			break;
+
+		case MULTI_STATUS_TYPE_FX00:
+			qlafx00_multistatus_entry(vha, rsp, pkt);
+			break;
+
+		case ABORT_IOCB_TYPE_FX00:
+			qlafx00_abort_iocb_entry(vha, rsp->req,
+			   (struct abort_iocb_entry_fx00 *)pkt);
+			break;
+
+		case IOCTL_IOSB_TYPE_FX00:
+			qlafx00_ioctl_iosb_entry(vha, rsp->req,
+			    (struct ioctl_iocb_entry_fx00 *)pkt);
+			break;
+		default:
+			/* Type Not Supported. */
+			ql_dbg(ql_dbg_async, vha, 0x5081,
+			    "Received unknown response pkt type %x "
+			    "entry status=%x.\n",
+			    pkt->entry_type, pkt->entry_status);
+			break;
+		}
+next_iter:
+		WRT_REG_DWORD(&lptr->signature, RESPONSE_PROCESSED);
+		wmb();
+	}
+
+	/* Adjust ring index */
+	WRT_REG_DWORD(rsp->rsp_q_out, rsp->ring_index);
+}
+
+/**
+ * qlafx00_async_event() - Process aynchronous events.
+ * @ha: SCSI driver HA context
+ */
+static void
+qlafx00_async_event(scsi_qla_host_t *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+	struct device_reg_fx00 __iomem *reg;
+	int data_size = 1;
+
+	reg = &ha->iobase->ispfx00;
+	/* Setup to process RIO completion. */
+	switch (ha->aenmb[0]) {
+	case QLAFX00_MBA_SYSTEM_ERR:		/* System Error */
+		ql_log(ql_log_warn, vha, 0x5079,
+		    "ISP System Error - mbx1=%x\n", ha->aenmb[0]);
+		set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+		break;
+
+	case QLAFX00_MBA_SHUTDOWN_RQSTD:	/* Shutdown requested */
+		ql_dbg(ql_dbg_async, vha, 0x5076,
+		    "Asynchronous FW shutdown requested.\n");
+		set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+		qla2xxx_wake_dpc(vha);
+		break;
+
+	case QLAFX00_MBA_PORT_UPDATE:		/* Port database update */
+		ha->aenmb[1] = RD_REG_WORD(&reg->aenmailbox1);
+		ha->aenmb[2] = RD_REG_WORD(&reg->aenmailbox2);
+		ha->aenmb[3] = RD_REG_WORD(&reg->aenmailbox3);
+		ql_dbg(ql_dbg_async, vha, 0x5077,
+		    "Asynchronous port Update received "
+		    "aenmb[0]: %x, aenmb[1]: %x, aenmb[2]: %x, aenmb[3]: %x\n",
+		    ha->aenmb[0], ha->aenmb[1], ha->aenmb[2], ha->aenmb[3]);
+		data_size = 4;
+		break;
+	default:
+		ha->aenmb[1] = RD_REG_WORD(&reg->aenmailbox1);
+		ha->aenmb[2] = RD_REG_WORD(&reg->aenmailbox2);
+		ha->aenmb[3] = RD_REG_WORD(&reg->aenmailbox3);
+		ha->aenmb[4] = RD_REG_WORD(&reg->aenmailbox4);
+		ha->aenmb[5] = RD_REG_WORD(&reg->aenmailbox5);
+		ha->aenmb[6] = RD_REG_WORD(&reg->aenmailbox6);
+		ha->aenmb[7] = RD_REG_WORD(&reg->aenmailbox7);
+		ql_dbg(ql_dbg_async, vha, 0x5078,
+		    "AEN:%04x %04x %04x %04x :%04x %04x %04x %04x\n",
+		    ha->aenmb[0], ha->aenmb[1], ha->aenmb[2], ha->aenmb[3],
+		    ha->aenmb[4], ha->aenmb[5], ha->aenmb[6], ha->aenmb[7]);
+		break;
+	}
+	qlafx00_post_aenfx_work(vha, ha->aenmb[0],
+	    (uint32_t *)ha->aenmb, data_size);
+}
+
+/**
+ *
+ * qlafx00x_mbx_completion() - Process mailbox command completions.
+ * @ha: SCSI driver HA context
+ * @mb16: Mailbox16 register
+ */
+static void
+qlafx00_mbx_completion(scsi_qla_host_t *vha, uint32_t mb0)
+{
+	uint16_t	cnt;
+	uint16_t __iomem *wptr;
+	struct qla_hw_data *ha = vha->hw;
+	struct device_reg_fx00 __iomem *reg = &ha->iobase->ispfx00;
+
+	if (!ha->mcp32)
+		ql_dbg(ql_dbg_async, vha, 0x507e, "MBX pointer ERROR.\n");
+
+	/* Load return mailbox registers. */
+	ha->flags.mbox_int = 1;
+	ha->mailbox_out32[0] = mb0;
+	wptr = (uint16_t __iomem *)&reg->mailbox17;
+
+	for (cnt = 1; cnt < ha->mbx_count; cnt++) {
+		ha->mailbox_out32[cnt] = RD_REG_WORD(wptr);
+		wptr++;
+	}
+}
+
+/**
+ * qlafx00_intr_handler() - Process interrupts for the ISPFX00.
+ * @irq:
+ * @dev_id: SCSI driver HA context
+ *
+ * Called by system whenever the host adapter generates an interrupt.
+ *
+ * Returns handled flag.
+ */
+irqreturn_t
+qlafx00_intr_handler(int irq, void *dev_id)
+{
+	scsi_qla_host_t	*vha;
+	struct qla_hw_data *ha;
+	struct device_reg_fx00 __iomem *reg;
+	int		status;
+	unsigned long	iter;
+	uint32_t	stat;
+	uint32_t	mb[8];
+	struct rsp_que *rsp;
+	unsigned long	flags;
+	uint32_t clr_intr = 0;
+
+	rsp = (struct rsp_que *) dev_id;
+	if (!rsp) {
+		ql_log(ql_log_info, NULL, 0x507d,
+		    "%s: NULL response queue pointer.\n", __func__);
+		return IRQ_NONE;
+	}
+
+	ha = rsp->hw;
+	reg = &ha->iobase->ispfx00;
+	status = 0;
+
+	if (unlikely(pci_channel_offline(ha->pdev)))
+		return IRQ_HANDLED;
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+	vha = pci_get_drvdata(ha->pdev);
+	for (iter = 50; iter--; clr_intr = 0) {
+		stat = QLAFX00_RD_INTR_REG(ha);
+		if ((stat & QLAFX00_HST_INT_STS_BITS) == 0)
+			break;
+
+		switch (stat & QLAFX00_HST_INT_STS_BITS) {
+		case QLAFX00_INTR_MB_CMPLT:
+		case QLAFX00_INTR_MB_RSP_CMPLT:
+		case QLAFX00_INTR_MB_ASYNC_CMPLT:
+		case QLAFX00_INTR_ALL_CMPLT:
+			mb[0] = RD_REG_WORD(&reg->mailbox16);
+			qlafx00_mbx_completion(vha, mb[0]);
+			status |= MBX_INTERRUPT;
+			clr_intr |= QLAFX00_INTR_MB_CMPLT;
+			break;
+		case QLAFX00_INTR_ASYNC_CMPLT:
+		case QLAFX00_INTR_RSP_ASYNC_CMPLT:
+			ha->aenmb[0] = RD_REG_WORD(&reg->aenmailbox0);
+			qlafx00_async_event(vha);
+			clr_intr |= QLAFX00_INTR_ASYNC_CMPLT;
+			break;
+		case QLAFX00_INTR_RSP_CMPLT:
+			qlafx00_process_response_queue(vha, rsp);
+			clr_intr |= QLAFX00_INTR_RSP_CMPLT;
+			break;
+		default:
+			ql_dbg(ql_dbg_async, vha, 0x507a,
+			    "Unrecognized interrupt type (%d).\n", stat);
+			break;
+		}
+		QLAFX00_CLR_INTR_REG(ha, clr_intr);
+		QLAFX00_RD_INTR_REG(ha);
+	}
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
+	if (test_bit(MBX_INTR_WAIT, &ha->mbx_cmd_flags) &&
+	    (status & MBX_INTERRUPT) && ha->flags.mbox_int) {
+		set_bit(MBX_INTERRUPT, &ha->mbx_cmd_flags);
+		complete(&ha->mbx_intr_comp);
+	}
+	return IRQ_HANDLED;
+}
+
+/** QLAFX00 specific IOCB implementation functions */
+
+static inline cont_a64_entry_t *
+qlafx00_prep_cont_type1_iocb(struct req_que *req,
+			     cont_a64_entry_t *lcont_pkt)
+{
+	cont_a64_entry_t *cont_pkt;
+
+	/* Adjust ring index. */
+	req->ring_index++;
+	if (req->ring_index == req->length) {
+		req->ring_index = 0;
+		req->ring_ptr = req->ring;
+	} else {
+		req->ring_ptr++;
+	}
+
+	cont_pkt = (cont_a64_entry_t *)req->ring_ptr;
+
+	/* Load packet defaults. */
+	*((uint32_t *)(&lcont_pkt->entry_type)) =
+	    __constant_cpu_to_le32(CONTINUE_A64_TYPE_FX00);
+
+	return cont_pkt;
+}
+
+static inline void
+qlafx00_build_scsi_iocbs(srb_t *sp, struct cmd_type_7_fx00 *cmd_pkt,
+			 uint16_t tot_dsds, struct cmd_type_7_fx00 *lcmd_pkt)
+{
+	uint16_t	avail_dsds;
+	uint32_t	*cur_dsd;
+	scsi_qla_host_t	*vha;
+	struct scsi_cmnd *cmd;
+	struct scatterlist *sg;
+	int i, cont;
+	struct req_que *req;
+	cont_a64_entry_t lcont_pkt;
+	cont_a64_entry_t *cont_pkt;
+
+	vha = sp->fcport->vha;
+	req = vha->req;
+
+	cmd = GET_CMD_SP(sp);
+	cont = 0;
+	cont_pkt = NULL;
+
+	/* Update entry type to indicate Command Type 3 IOCB */
+	*((uint32_t *)(&lcmd_pkt->entry_type)) =
+	    __constant_cpu_to_le32(FX00_COMMAND_TYPE_7);
+
+	/* No data transfer */
+	if (!scsi_bufflen(cmd) || cmd->sc_data_direction == DMA_NONE) {
+		lcmd_pkt->byte_count = __constant_cpu_to_le32(0);
+		return;
+	}
+
+	/* Set transfer direction */
+	if (cmd->sc_data_direction == DMA_TO_DEVICE) {
+		lcmd_pkt->cntrl_flags =
+		    __constant_cpu_to_le16(TMF_WRITE_DATA);
+		vha->qla_stats.output_bytes += scsi_bufflen(cmd);
+	} else if (cmd->sc_data_direction == DMA_FROM_DEVICE) {
+		lcmd_pkt->cntrl_flags =
+		    __constant_cpu_to_le16(TMF_READ_DATA);
+		vha->qla_stats.input_bytes += scsi_bufflen(cmd);
+	}
+
+	/* One DSD is available in the Command Type 3 IOCB */
+	avail_dsds = 1;
+	cur_dsd = (uint32_t *)&lcmd_pkt->dseg_0_address;
+
+	/* Load data segments */
+	scsi_for_each_sg(cmd, sg, tot_dsds, i) {
+		dma_addr_t	sle_dma;
+
+		/* Allocate additional continuation packets? */
+		if (avail_dsds == 0) {
+			/*
+			 * Five DSDs are available in the Continuation
+			 * Type 1 IOCB.
+			 */
+			memset(&lcont_pkt, 0, REQUEST_ENTRY_SIZE);
+			cont_pkt =
+			    qlafx00_prep_cont_type1_iocb(req, &lcont_pkt);
+			cur_dsd = (uint32_t *)lcont_pkt.dseg_0_address;
+			avail_dsds = 5;
+			cont = 1;
+		}
+
+		sle_dma = sg_dma_address(sg);
+		*cur_dsd++ = cpu_to_le32(LSD(sle_dma));
+		*cur_dsd++ = cpu_to_le32(MSD(sle_dma));
+		*cur_dsd++ = cpu_to_le32(sg_dma_len(sg));
+		avail_dsds--;
+		if (avail_dsds == 0 && cont == 1) {
+			cont = 0;
+			memcpy_toio((void __iomem *)cont_pkt, &lcont_pkt,
+			    REQUEST_ENTRY_SIZE);
+		}
+
+	}
+	if (avail_dsds != 0 && cont == 1) {
+		memcpy_toio((void __iomem *)cont_pkt, &lcont_pkt,
+		    REQUEST_ENTRY_SIZE);
+	}
+}
+
+/**
+ * qlafx00_start_scsi() - Send a SCSI command to the ISP
+ * @sp: command to send to the ISP
+ *
+ * Returns non-zero if a failure occurred, else zero.
+ */
+int
+qlafx00_start_scsi(srb_t *sp)
+{
+	int		ret, nseg;
+	unsigned long   flags;
+	uint32_t        index;
+	uint32_t	handle;
+	uint16_t	cnt;
+	uint16_t	req_cnt;
+	uint16_t	tot_dsds;
+	struct req_que *req = NULL;
+	struct rsp_que *rsp = NULL;
+	struct scsi_cmnd *cmd = GET_CMD_SP(sp);
+	struct scsi_qla_host *vha = sp->fcport->vha;
+	struct qla_hw_data *ha = vha->hw;
+	struct cmd_type_7_fx00 *cmd_pkt;
+	struct cmd_type_7_fx00 lcmd_pkt;
+	struct scsi_lun llun;
+	char		tag[2];
+
+	/* Setup device pointers. */
+	ret = 0;
+
+	rsp = ha->rsp_q_map[0];
+	req = vha->req;
+
+	/* So we know we haven't pci_map'ed anything yet */
+	tot_dsds = 0;
+
+	/* Forcing marker needed for now */
+	vha->marker_needed = 0;
+
+	/* Send marker if required */
+	if (vha->marker_needed != 0) {
+		if (qla2x00_marker(vha, req, rsp, 0, 0, MK_SYNC_ALL) !=
+		    QLA_SUCCESS)
+			return QLA_FUNCTION_FAILED;
+		vha->marker_needed = 0;
+	}
+
+	/* Acquire ring specific lock */
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+
+	/* Check for room in outstanding command list. */
+	handle = req->current_outstanding_cmd;
+	for (index = 1; index < req->num_outstanding_cmds; index++) {
+		handle++;
+		if (handle == req->num_outstanding_cmds)
+			handle = 1;
+		if (!req->outstanding_cmds[handle])
+			break;
+	}
+	if (index == req->num_outstanding_cmds)
+		goto queuing_error;
+
+	/* Map the sg table so we have an accurate count of sg entries needed */
+	if (scsi_sg_count(cmd)) {
+		nseg = dma_map_sg(&ha->pdev->dev, scsi_sglist(cmd),
+		    scsi_sg_count(cmd), cmd->sc_data_direction);
+		if (unlikely(!nseg))
+			goto queuing_error;
+	} else
+		nseg = 0;
+
+	tot_dsds = nseg;
+	req_cnt = qla24xx_calc_iocbs(vha, tot_dsds);
+	if (req->cnt < (req_cnt + 2)) {
+		cnt = RD_REG_DWORD_RELAXED(req->req_q_out);
+
+		if (req->ring_index < cnt)
+			req->cnt = cnt - req->ring_index;
+		else
+			req->cnt = req->length -
+				(req->ring_index - cnt);
+		if (req->cnt < (req_cnt + 2))
+			goto queuing_error;
+	}
+
+	/* Build command packet. */
+	req->current_outstanding_cmd = handle;
+	req->outstanding_cmds[handle] = sp;
+	sp->handle = handle;
+	cmd->host_scribble = (unsigned char *)(unsigned long)handle;
+	req->cnt -= req_cnt;
+
+	cmd_pkt = (struct cmd_type_7_fx00 *)req->ring_ptr;
+
+	memset(&lcmd_pkt, 0, REQUEST_ENTRY_SIZE);
+
+	lcmd_pkt.handle = MAKE_HANDLE(req->id, sp->handle);
+	lcmd_pkt.handle_hi = 0;
+	lcmd_pkt.dseg_count = cpu_to_le16(tot_dsds);
+	lcmd_pkt.tgt_idx = cpu_to_le16(sp->fcport->tgt_id);
+
+	int_to_scsilun(cmd->device->lun, &llun);
+	host_to_adap((uint8_t *)&llun, (uint8_t *)&lcmd_pkt.lun,
+	    sizeof(lcmd_pkt.lun));
+
+	/* Update tagged queuing modifier -- default is TSK_SIMPLE (0). */
+	if (scsi_populate_tag_msg(cmd, tag)) {
+		switch (tag[0]) {
+		case HEAD_OF_QUEUE_TAG:
+			lcmd_pkt.task = TSK_HEAD_OF_QUEUE;
+			break;
+		case ORDERED_QUEUE_TAG:
+			lcmd_pkt.task = TSK_ORDERED;
+			break;
+		}
+	}
+
+	/* Load SCSI command packet. */
+	host_to_adap(cmd->cmnd, lcmd_pkt.fcp_cdb, sizeof(lcmd_pkt.fcp_cdb));
+	lcmd_pkt.byte_count = cpu_to_le32((uint32_t)scsi_bufflen(cmd));
+
+	/* Build IOCB segments */
+	qlafx00_build_scsi_iocbs(sp, cmd_pkt, tot_dsds, &lcmd_pkt);
+
+	/* Set total data segment count. */
+	lcmd_pkt.entry_count = (uint8_t)req_cnt;
+
+	/* Specify response queue number where completion should happen */
+	lcmd_pkt.entry_status = (uint8_t) rsp->id;
+
+	ql_dump_buffer(ql_dbg_io + ql_dbg_buffer, vha, 0x302e,
+	    (uint8_t *)cmd->cmnd, cmd->cmd_len);
+	ql_dump_buffer(ql_dbg_io + ql_dbg_buffer, vha, 0x3032,
+	    (uint8_t *)&lcmd_pkt, REQUEST_ENTRY_SIZE);
+
+	memcpy_toio((void __iomem *)cmd_pkt, &lcmd_pkt, REQUEST_ENTRY_SIZE);
+	wmb();
+
+	/* Adjust ring index. */
+	req->ring_index++;
+	if (req->ring_index == req->length) {
+		req->ring_index = 0;
+		req->ring_ptr = req->ring;
+	} else
+		req->ring_ptr++;
+
+	sp->flags |= SRB_DMA_VALID;
+
+	/* Set chip new ring index. */
+	WRT_REG_DWORD(req->req_q_in, req->ring_index);
+	QLAFX00_SET_HST_INTR(ha, ha->rqstq_intr_code);
+
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	return QLA_SUCCESS;
+
+queuing_error:
+	if (tot_dsds)
+		scsi_dma_unmap(cmd);
+
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
+	return QLA_FUNCTION_FAILED;
+}
+
+void
+qlafx00_tm_iocb(srb_t *sp, struct tsk_mgmt_entry_fx00 *ptm_iocb)
+{
+	struct srb_iocb *fxio = &sp->u.iocb_cmd;
+	scsi_qla_host_t *vha = sp->fcport->vha;
+	struct req_que *req = vha->req;
+	struct tsk_mgmt_entry_fx00 tm_iocb;
+	struct scsi_lun llun;
+
+	memset(&tm_iocb, 0, sizeof(struct tsk_mgmt_entry_fx00));
+	tm_iocb.entry_type = TSK_MGMT_IOCB_TYPE_FX00;
+	tm_iocb.entry_count = 1;
+	tm_iocb.handle = cpu_to_le32(MAKE_HANDLE(req->id, sp->handle));
+	tm_iocb.handle_hi = 0;
+	tm_iocb.timeout = cpu_to_le16(qla2x00_get_async_timeout(vha) + 2);
+	tm_iocb.tgt_id = cpu_to_le16(sp->fcport->tgt_id);
+	tm_iocb.control_flags = cpu_to_le32(fxio->u.tmf.flags);
+	if (tm_iocb.control_flags == TCF_LUN_RESET) {
+		int_to_scsilun(fxio->u.tmf.lun, &llun);
+		host_to_adap((uint8_t *)&llun, (uint8_t *)&tm_iocb.lun,
+		    sizeof(struct scsi_lun));
+	}
+
+	memcpy((void __iomem *)ptm_iocb, &tm_iocb,
+	    sizeof(struct tsk_mgmt_entry_fx00));
+	wmb();
+}
+
+void
+qlafx00_abort_iocb(srb_t *sp, struct abort_iocb_entry_fx00 *pabt_iocb)
+{
+	struct srb_iocb *fxio = &sp->u.iocb_cmd;
+	scsi_qla_host_t *vha = sp->fcport->vha;
+	struct req_que *req = vha->req;
+	struct abort_iocb_entry_fx00 abt_iocb;
+
+	memset(&abt_iocb, 0, sizeof(struct abort_iocb_entry_fx00));
+	abt_iocb.entry_type = ABORT_IOCB_TYPE_FX00;
+	abt_iocb.entry_count = 1;
+	abt_iocb.handle = cpu_to_le32(MAKE_HANDLE(req->id, sp->handle));
+	abt_iocb.abort_handle =
+	    cpu_to_le32(MAKE_HANDLE(req->id, fxio->u.abt.cmd_hndl));
+	abt_iocb.tgt_id_sts = cpu_to_le16(sp->fcport->tgt_id);
+	abt_iocb.req_que_no = cpu_to_le16(req->id);
+
+	memcpy((void __iomem *)pabt_iocb, &abt_iocb,
+	    sizeof(struct abort_iocb_entry_fx00));
+	wmb();
+}
+
+void
+qlafx00_fxdisc_iocb(srb_t *sp, struct fxdisc_entry_fx00 *pfxiocb)
+{
+	struct srb_iocb *fxio = &sp->u.iocb_cmd;
+	struct qla_mt_iocb_rqst_fx00 *piocb_rqst;
+	struct fc_bsg_job *bsg_job;
+	struct fxdisc_entry_fx00 fx_iocb;
+	uint8_t entry_cnt = 1;
+
+	memset(&fx_iocb, 0, sizeof(struct fxdisc_entry_fx00));
+	fx_iocb.entry_type = FX00_IOCB_TYPE;
+	fx_iocb.handle = cpu_to_le32(sp->handle);
+	fx_iocb.entry_count = entry_cnt;
+
+	if (sp->type == SRB_FXIOCB_DCMD) {
+		fx_iocb.func_num =
+		    cpu_to_le16(sp->u.iocb_cmd.u.fxiocb.req_func_type);
+		fx_iocb.adapid = cpu_to_le32(fxio->u.fxiocb.adapter_id);
+		fx_iocb.adapid_hi = cpu_to_le32(fxio->u.fxiocb.adapter_id_hi);
+		fx_iocb.reserved_0 = cpu_to_le32(fxio->u.fxiocb.reserved_0);
+		fx_iocb.reserved_1 = cpu_to_le32(fxio->u.fxiocb.reserved_1);
+		fx_iocb.dataword_extra =
+		    cpu_to_le32(fxio->u.fxiocb.req_data_extra);
+
+		if (fxio->u.fxiocb.flags & SRB_FXDISC_REQ_DMA_VALID) {
+			fx_iocb.req_dsdcnt = cpu_to_le16(1);
+			fx_iocb.req_xfrcnt =
+			    cpu_to_le16(fxio->u.fxiocb.req_len);
+			fx_iocb.dseg_rq_address[0] =
+			    cpu_to_le32(LSD(fxio->u.fxiocb.req_dma_handle));
+			fx_iocb.dseg_rq_address[1] =
+			    cpu_to_le32(MSD(fxio->u.fxiocb.req_dma_handle));
+			fx_iocb.dseg_rq_len =
+			    cpu_to_le32(fxio->u.fxiocb.req_len);
+		}
+
+		if (fxio->u.fxiocb.flags & SRB_FXDISC_RESP_DMA_VALID) {
+			fx_iocb.rsp_dsdcnt = cpu_to_le16(1);
+			fx_iocb.rsp_xfrcnt =
+			    cpu_to_le16(fxio->u.fxiocb.rsp_len);
+			fx_iocb.dseg_rsp_address[0] =
+			    cpu_to_le32(LSD(fxio->u.fxiocb.rsp_dma_handle));
+			fx_iocb.dseg_rsp_address[1] =
+			    cpu_to_le32(MSD(fxio->u.fxiocb.rsp_dma_handle));
+			fx_iocb.dseg_rsp_len =
+			    cpu_to_le32(fxio->u.fxiocb.rsp_len);
+		}
+
+		if (fxio->u.fxiocb.flags & SRB_FXDISC_REQ_DWRD_VALID) {
+			fx_iocb.dataword =
+			    cpu_to_le32(fxio->u.fxiocb.req_data);
+		}
+		fx_iocb.flags = fxio->u.fxiocb.flags;
+	} else {
+		struct scatterlist *sg;
+		bsg_job = sp->u.bsg_job;
+		piocb_rqst = (struct qla_mt_iocb_rqst_fx00 *)
+			&bsg_job->request->rqst_data.h_vendor.vendor_cmd[1];
+
+		fx_iocb.func_num = piocb_rqst->func_type;
+		fx_iocb.adapid = piocb_rqst->adapid;
+		fx_iocb.adapid_hi = piocb_rqst->adapid_hi;
+		fx_iocb.reserved_0 = piocb_rqst->reserved_0;
+		fx_iocb.reserved_1 = piocb_rqst->reserved_1;
+		fx_iocb.dataword_extra = piocb_rqst->dataword_extra;
+		fx_iocb.dataword = piocb_rqst->dataword;
+		fx_iocb.req_xfrcnt = cpu_to_le16(piocb_rqst->req_len);
+		fx_iocb.rsp_xfrcnt = cpu_to_le16(piocb_rqst->rsp_len);
+
+		if (piocb_rqst->flags & SRB_FXDISC_REQ_DMA_VALID) {
+			int avail_dsds, tot_dsds;
+			cont_a64_entry_t lcont_pkt;
+			cont_a64_entry_t *cont_pkt = NULL;
+			uint32_t *cur_dsd;
+			int index = 0, cont = 0;
+
+			fx_iocb.req_dsdcnt =
+			    cpu_to_le16(bsg_job->request_payload.sg_cnt);
+			tot_dsds =
+			    cpu_to_le32(bsg_job->request_payload.sg_cnt);
+			cur_dsd = (uint32_t *)&fx_iocb.dseg_rq_address[0];
+			avail_dsds = 1;
+			for_each_sg(bsg_job->request_payload.sg_list, sg,
+			    tot_dsds, index) {
+				dma_addr_t sle_dma;
+
+				/* Allocate additional continuation packets? */
+				if (avail_dsds == 0) {
+					/*
+					 * Five DSDs are available in the Cont.
+					 * Type 1 IOCB.
+					 */
+					memset(&lcont_pkt, 0,
+					    REQUEST_ENTRY_SIZE);
+					cont_pkt =
+					    qlafx00_prep_cont_type1_iocb(
+						sp->fcport->vha->req,
+						&lcont_pkt);
+					cur_dsd = (uint32_t *)
+					    lcont_pkt.dseg_0_address;
+					avail_dsds = 5;
+					cont = 1;
+					entry_cnt++;
+				}
+
+				sle_dma = sg_dma_address(sg);
+				*cur_dsd++   = cpu_to_le32(LSD(sle_dma));
+				*cur_dsd++   = cpu_to_le32(MSD(sle_dma));
+				*cur_dsd++   = cpu_to_le32(sg_dma_len(sg));
+				avail_dsds--;
+
+				if (avail_dsds == 0 && cont == 1) {
+					cont = 0;
+					memcpy_toio(
+					    (void __iomem *)cont_pkt,
+					    &lcont_pkt, REQUEST_ENTRY_SIZE);
+					ql_dump_buffer(
+					    ql_dbg_user + ql_dbg_verbose,
+					    sp->fcport->vha, 0x3042,
+					    (uint8_t *)&lcont_pkt,
+					     REQUEST_ENTRY_SIZE);
+				}
+			}
+			if (avail_dsds != 0 && cont == 1) {
+				memcpy_toio((void __iomem *)cont_pkt,
+				    &lcont_pkt, REQUEST_ENTRY_SIZE);
+				ql_dump_buffer(ql_dbg_user + ql_dbg_verbose,
+				    sp->fcport->vha, 0x3043,
+				    (uint8_t *)&lcont_pkt, REQUEST_ENTRY_SIZE);
+			}
+		}
+
+		if (piocb_rqst->flags & SRB_FXDISC_RESP_DMA_VALID) {
+			int avail_dsds, tot_dsds;
+			cont_a64_entry_t lcont_pkt;
+			cont_a64_entry_t *cont_pkt = NULL;
+			uint32_t *cur_dsd;
+			int index = 0, cont = 0;
+
+			fx_iocb.rsp_dsdcnt =
+			   cpu_to_le16(bsg_job->reply_payload.sg_cnt);
+			tot_dsds = cpu_to_le32(bsg_job->reply_payload.sg_cnt);
+			cur_dsd = (uint32_t *)&fx_iocb.dseg_rsp_address[0];
+			avail_dsds = 1;
+
+			for_each_sg(bsg_job->reply_payload.sg_list, sg,
+			    tot_dsds, index) {
+				dma_addr_t sle_dma;
+
+				/* Allocate additional continuation packets? */
+				if (avail_dsds == 0) {
+					/*
+					* Five DSDs are available in the Cont.
+					* Type 1 IOCB.
+					*/
+					memset(&lcont_pkt, 0,
+					    REQUEST_ENTRY_SIZE);
+					cont_pkt =
+					    qlafx00_prep_cont_type1_iocb(
+						sp->fcport->vha->req,
+						&lcont_pkt);
+					cur_dsd = (uint32_t *)
+					    lcont_pkt.dseg_0_address;
+					avail_dsds = 5;
+					cont = 1;
+					entry_cnt++;
+				}
+
+				sle_dma = sg_dma_address(sg);
+				*cur_dsd++   = cpu_to_le32(LSD(sle_dma));
+				*cur_dsd++   = cpu_to_le32(MSD(sle_dma));
+				*cur_dsd++   = cpu_to_le32(sg_dma_len(sg));
+				avail_dsds--;
+
+				if (avail_dsds == 0 && cont == 1) {
+					cont = 0;
+					memcpy_toio((void __iomem *)cont_pkt,
+					    &lcont_pkt,
+					    REQUEST_ENTRY_SIZE);
+					ql_dump_buffer(
+					    ql_dbg_user + ql_dbg_verbose,
+					    sp->fcport->vha, 0x3045,
+					    (uint8_t *)&lcont_pkt,
+					    REQUEST_ENTRY_SIZE);
+				}
+			}
+			if (avail_dsds != 0 && cont == 1) {
+				memcpy_toio((void __iomem *)cont_pkt,
+				    &lcont_pkt, REQUEST_ENTRY_SIZE);
+				ql_dump_buffer(ql_dbg_user + ql_dbg_verbose,
+				    sp->fcport->vha, 0x3046,
+				    (uint8_t *)&lcont_pkt, REQUEST_ENTRY_SIZE);
+			}
+		}
+
+		if (piocb_rqst->flags & SRB_FXDISC_REQ_DWRD_VALID)
+			fx_iocb.dataword = cpu_to_le32(piocb_rqst->dataword);
+		fx_iocb.flags = piocb_rqst->flags;
+		fx_iocb.entry_count = entry_cnt;
+	}
+
+	ql_dump_buffer(ql_dbg_user + ql_dbg_verbose,
+	    sp->fcport->vha, 0x3047,
+	    (uint8_t *)&fx_iocb, sizeof(struct fxdisc_entry_fx00));
+
+	memcpy((void __iomem *)pfxiocb, &fx_iocb,
+	    sizeof(struct fxdisc_entry_fx00));
+	wmb();
+}
diff --git a/drivers/scsi/qla2xxx/qla_mr.h b/drivers/scsi/qla2xxx/qla_mr.h
new file mode 100644
index 000000000000..cc327dc2fd10
--- /dev/null
+++ b/drivers/scsi/qla2xxx/qla_mr.h
@@ -0,0 +1,510 @@
+/*
+ * QLogic Fibre Channel HBA Driver
+ * Copyright (c)  2003-2013 QLogic Corporation
+ *
+ * See LICENSE.qla2xxx for copyright and licensing details.
+ */
+#ifndef __QLA_MR_H
+#define __QLA_MR_H
+
+/*
+ * The PCI VendorID and DeviceID for our board.
+ */
+#define PCI_DEVICE_ID_QLOGIC_ISPF001		0xF001
+
+/* FX00 specific definitions */
+
+#define FX00_COMMAND_TYPE_7	0x07	/* Command Type 7 entry for 7XXX */
+struct cmd_type_7_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t sys_define;		/* System defined. */
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+	uint32_t handle_hi;
+
+	uint16_t tgt_idx;		/* Target Idx. */
+	uint16_t timeout;		/* Command timeout. */
+
+	uint16_t dseg_count;		/* Data segment count. */
+	uint16_t scsi_rsp_dsd_len;
+
+	struct scsi_lun lun;		/* LUN (LE). */
+
+	uint8_t cntrl_flags;
+
+	uint8_t task_mgmt_flags;	/* Task management flags. */
+
+	uint8_t task;
+
+	uint8_t crn;
+
+	uint8_t fcp_cdb[MAX_CMDSZ];	/* SCSI command words. */
+	uint32_t byte_count;		/* Total byte count. */
+
+	uint32_t dseg_0_address[2];	/* Data segment 0 address. */
+	uint32_t dseg_0_len;		/* Data segment 0 length. */
+};
+
+/*
+ * ISP queue - marker entry structure definition.
+ */
+struct mrk_entry_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t handle_count;		/* Handle count. */
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+	uint32_t handle_hi;		/* System handle. */
+
+	uint16_t tgt_id;		/* Target ID. */
+
+	uint8_t modifier;		/* Modifier (7-0). */
+	uint8_t reserved_1;
+
+	uint8_t reserved_2[5];
+
+	uint8_t lun[8];			/* FCP LUN (BE). */
+	uint8_t reserved_3[36];
+};
+
+
+#define	STATUS_TYPE_FX00	0x01		/* Status entry. */
+struct sts_entry_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t sys_define;		/* System defined. */
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+	uint32_t handle_hi;		/* System handle. */
+
+	uint16_t comp_status;		/* Completion status. */
+	uint16_t reserved_0;		/* OX_ID used by the firmware. */
+
+	uint32_t residual_len;		/* FW calc residual transfer length. */
+
+	uint16_t reserved_1;
+	uint16_t state_flags;		/* State flags. */
+
+	uint16_t reserved_2;
+	uint16_t scsi_status;		/* SCSI status. */
+
+	uint32_t sense_len;		/* FCP SENSE length. */
+	uint8_t data[32];		/* FCP response/sense information. */
+};
+
+
+#define MAX_HANDLE_COUNT	15
+#define MULTI_STATUS_TYPE_FX00	0x0D
+
+struct multi_sts_entry_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t sys_define;		/* System defined. */
+	uint8_t handle_count;
+	uint8_t entry_status;
+
+	uint32_t handles[MAX_HANDLE_COUNT];
+};
+
+#define TSK_MGMT_IOCB_TYPE_FX00		0x05
+struct tsk_mgmt_entry_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t sys_define;
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+
+	uint32_t handle_hi;		/* System handle. */
+
+	uint16_t tgt_id;		/* Target Idx. */
+
+	uint16_t reserved_1;
+
+	uint16_t delay;			/* Activity delay in seconds. */
+
+	uint16_t timeout;		/* Command timeout. */
+
+	struct scsi_lun lun;		/* LUN (LE). */
+
+	uint32_t control_flags;		/* Control Flags. */
+
+	uint8_t reserved_2[32];
+};
+
+
+#define	ABORT_IOCB_TYPE_FX00	0x08		/* Abort IOCB status. */
+struct abort_iocb_entry_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t sys_define;		/* System defined. */
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+	uint32_t handle_hi;		/* System handle. */
+
+	uint16_t tgt_id_sts;		/* Completion status. */
+	uint16_t options;
+
+	uint32_t abort_handle;		/* System handle. */
+	uint32_t abort_handle_hi;	/* System handle. */
+
+	uint16_t req_que_no;
+	uint8_t reserved_1[38];
+};
+
+#define IOCTL_IOSB_TYPE_FX00	0x0C
+struct ioctl_iocb_entry_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t sys_define;		/* System defined. */
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+	uint32_t reserved_0;		/* System handle. */
+
+	uint16_t comp_func_num;
+	uint16_t fw_iotcl_flags;
+
+	uint32_t dataword_r;		/* Data word returned */
+	uint32_t adapid;		/* Adapter ID */
+	uint32_t adapid_hi;		/* Adapter ID high */
+	uint32_t reserved_1;
+
+	uint32_t seq_no;
+	uint8_t reserved_2[20];
+	uint32_t residuallen;
+	uint32_t status;
+};
+
+#define STATUS_CONT_TYPE_FX00 0x04
+
+#define FX00_IOCB_TYPE		0x0B
+struct fxdisc_entry_fx00 {
+	uint8_t entry_type;		/* Entry type. */
+	uint8_t entry_count;		/* Entry count. */
+	uint8_t sys_define;		/* System Defined. */
+	uint8_t entry_status;		/* Entry Status. */
+
+	uint32_t handle;		/* System handle. */
+	uint32_t reserved_0;		/* System handle. */
+
+	uint16_t func_num;
+	uint16_t req_xfrcnt;
+	uint16_t req_dsdcnt;
+	uint16_t rsp_xfrcnt;
+	uint16_t rsp_dsdcnt;
+	uint8_t flags;
+	uint8_t reserved_1;
+
+	uint32_t dseg_rq_address[2];	/* Data segment 0 address. */
+	uint32_t dseg_rq_len;		/* Data segment 0 length. */
+	uint32_t dseg_rsp_address[2];	/* Data segment 1 address. */
+	uint32_t dseg_rsp_len;		/* Data segment 1 length. */
+
+	uint32_t dataword;
+	uint32_t adapid;
+	uint32_t adapid_hi;
+	uint32_t dataword_extra;
+};
+
+struct qlafx00_tgt_node_info {
+	uint8_t tgt_node_wwpn[WWN_SIZE];
+	uint8_t tgt_node_wwnn[WWN_SIZE];
+	uint32_t tgt_node_state;
+	uint8_t reserved[128];
+	uint32_t reserved_1[8];
+	uint64_t reserved_2[4];
+} __packed;
+
+#define QLAFX00_TGT_NODE_INFO sizeof(struct qlafx00_tgt_node_info)
+
+#define QLAFX00_LINK_STATUS_DOWN	0x10
+#define QLAFX00_LINK_STATUS_UP		0x11
+
+#define QLAFX00_PORT_SPEED_2G	0x2
+#define QLAFX00_PORT_SPEED_4G	0x4
+#define QLAFX00_PORT_SPEED_8G	0x8
+#define QLAFX00_PORT_SPEED_10G	0xa
+struct port_info_data {
+	uint8_t         port_state;
+	uint8_t         port_type;
+	uint16_t        port_identifier;
+	uint32_t        up_port_state;
+	uint8_t         fw_ver_num[32];
+	uint8_t         portal_attrib;
+	uint16_t        host_option;
+	uint8_t         reset_delay;
+	uint8_t         pdwn_retry_cnt;
+	uint16_t        max_luns2tgt;
+	uint8_t         risc_ver;
+	uint8_t         pconn_option;
+	uint16_t        risc_option;
+	uint16_t        max_frame_len;
+	uint16_t        max_iocb_alloc;
+	uint16_t        exec_throttle;
+	uint8_t         retry_cnt;
+	uint8_t         retry_delay;
+	uint8_t         port_name[8];
+	uint8_t         port_id[3];
+	uint8_t         link_status;
+	uint8_t         plink_rate;
+	uint32_t        link_config;
+	uint16_t        adap_haddr;
+	uint8_t         tgt_disc;
+	uint8_t         log_tout;
+	uint8_t         node_name[8];
+	uint16_t        erisc_opt1;
+	uint8_t         resp_acc_tmr;
+	uint8_t         intr_del_tmr;
+	uint8_t         erisc_opt2;
+	uint8_t         alt_port_name[8];
+	uint8_t         alt_node_name[8];
+	uint8_t         link_down_tout;
+	uint8_t         conn_type;
+	uint8_t         fc_fw_mode;
+	uint32_t        uiReserved[48];
+} __packed;
+
+/* OS Type Designations */
+#define OS_TYPE_UNKNOWN             0
+#define OS_TYPE_LINUX               2
+
+/* Linux Info */
+#define SYSNAME_LENGTH              128
+#define NODENAME_LENGTH             64
+#define RELEASE_LENGTH              64
+#define VERSION_LENGTH              64
+#define MACHINE_LENGTH              64
+#define DOMNAME_LENGTH              64
+
+struct host_system_info {
+	uint32_t os_type;
+	char    sysname[SYSNAME_LENGTH];
+	char    nodename[NODENAME_LENGTH];
+	char    release[RELEASE_LENGTH];
+	char    version[VERSION_LENGTH];
+	char    machine[MACHINE_LENGTH];
+	char    domainname[DOMNAME_LENGTH];
+	char    hostdriver[VERSION_LENGTH];
+	uint32_t reserved[64];
+} __packed;
+
+struct register_host_info {
+	struct host_system_info     hsi;	/* host system info */
+	uint64_t        utc;			/* UTC (system time) */
+	uint32_t        reserved[64];		/* future additions */
+} __packed;
+
+
+#define QLAFX00_PORT_DATA_INFO (sizeof(struct port_info_data))
+#define QLAFX00_TGT_NODE_LIST_SIZE (sizeof(uint32_t) * 32)
+
+struct config_info_data {
+	uint8_t		product_name[256];
+	uint8_t		symbolic_name[64];
+	uint8_t		serial_num[32];
+	uint8_t		hw_version[16];
+	uint8_t		fw_version[16];
+	uint8_t		uboot_version[16];
+	uint8_t		fru_serial_num[32];
+
+	uint8_t		fc_port_count;
+	uint8_t		iscsi_port_count;
+	uint8_t		reserved1[2];
+
+	uint8_t		mode;
+	uint8_t		log_level;
+	uint8_t		reserved2[2];
+
+	uint32_t	log_size;
+
+	uint8_t		tgt_pres_mode;
+	uint8_t		iqn_flags;
+	uint8_t		lun_mapping;
+
+	uint64_t	adapter_id;
+
+	uint32_t	cluster_key_len;
+	uint8_t		cluster_key[10];
+
+	uint64_t	cluster_master_id;
+	uint64_t	cluster_slave_id;
+	uint8_t		cluster_flags;
+} __packed;
+
+#define FXDISC_GET_CONFIG_INFO		0x01
+#define FXDISC_GET_PORT_INFO		0x02
+#define FXDISC_GET_TGT_NODE_INFO	0x80
+#define FXDISC_GET_TGT_NODE_LIST	0x81
+#define FXDISC_REG_HOST_INFO		0x99
+
+#define QLAFX00_HBA_ICNTRL_REG		0x21B08
+#define QLAFX00_ICR_ENB_MASK            0x80000000
+#define QLAFX00_ICR_DIS_MASK            0x7fffffff
+#define QLAFX00_HST_RST_REG		0x18264
+#define QLAFX00_HST_TO_HBA_REG		0x20A04
+#define QLAFX00_HBA_TO_HOST_REG		0x21B70
+#define QLAFX00_HST_INT_STS_BITS	0x7
+#define QLAFX00_BAR1_BASE_ADDR_REG	0x40018
+#define QLAFX00_PEX0_WIN0_BASE_ADDR_REG	0x41824
+
+#define QLAFX00_INTR_MB_CMPLT		0x1
+#define QLAFX00_INTR_RSP_CMPLT		0x2
+#define QLAFX00_INTR_MB_RSP_CMPLT	0x3
+#define QLAFX00_INTR_ASYNC_CMPLT	0x4
+#define QLAFX00_INTR_MB_ASYNC_CMPLT	0x5
+#define QLAFX00_INTR_RSP_ASYNC_CMPLT	0x6
+#define QLAFX00_INTR_ALL_CMPLT		0x7
+
+#define QLAFX00_MBA_SYSTEM_ERR		0x8002
+#define QLAFX00_MBA_LINK_UP		0x8011
+#define QLAFX00_MBA_LINK_DOWN		0x8012
+#define QLAFX00_MBA_PORT_UPDATE		0x8014
+#define QLAFX00_MBA_SHUTDOWN_RQSTD	0x8062
+
+#define SOC_SW_RST_CONTROL_REG_CORE0     0x0020800
+#define SOC_FABRIC_RST_CONTROL_REG       0x0020840
+#define SOC_FABRIC_CONTROL_REG           0x0020200
+#define SOC_FABRIC_CONFIG_REG            0x0020204
+
+#define SOC_INTERRUPT_SOURCE_I_CONTROL_REG     0x0020B00
+#define SOC_CORE_TIMER_REG                     0x0021850
+#define SOC_IRQ_ACK_REG                        0x00218b4
+
+#define CONTINUE_A64_TYPE_FX00	0x03	/* Continuation entry. */
+
+#define QLAFX00_SET_HST_INTR(ha, value) \
+	WRT_REG_DWORD((ha)->cregbase + QLAFX00_HST_TO_HBA_REG, \
+	value)
+
+#define QLAFX00_CLR_HST_INTR(ha, value) \
+	WRT_REG_DWORD((ha)->cregbase + QLAFX00_HBA_TO_HOST_REG, \
+	~value)
+
+#define QLAFX00_RD_INTR_REG(ha) \
+	RD_REG_DWORD((ha)->cregbase + QLAFX00_HBA_TO_HOST_REG)
+
+#define QLAFX00_CLR_INTR_REG(ha, value) \
+	WRT_REG_DWORD((ha)->cregbase + QLAFX00_HBA_TO_HOST_REG, \
+	~value)
+
+#define QLAFX00_SET_HBA_SOC_REG(ha, off, val)\
+	WRT_REG_DWORD((ha)->cregbase + off, val)
+
+#define QLAFX00_GET_HBA_SOC_REG(ha, off)\
+	RD_REG_DWORD((ha)->cregbase + off)
+
+#define QLAFX00_HBA_RST_REG(ha, val)\
+	WRT_REG_DWORD((ha)->cregbase + QLAFX00_HST_RST_REG, val)
+
+#define QLAFX00_RD_ICNTRL_REG(ha) \
+	RD_REG_DWORD((ha)->cregbase + QLAFX00_HBA_ICNTRL_REG)
+
+#define QLAFX00_ENABLE_ICNTRL_REG(ha) \
+	WRT_REG_DWORD((ha)->cregbase + QLAFX00_HBA_ICNTRL_REG, \
+	(QLAFX00_GET_HBA_SOC_REG(ha, QLAFX00_HBA_ICNTRL_REG) | \
+	 QLAFX00_ICR_ENB_MASK))
+
+#define QLAFX00_DISABLE_ICNTRL_REG(ha) \
+	WRT_REG_DWORD((ha)->cregbase + QLAFX00_HBA_ICNTRL_REG, \
+	(QLAFX00_GET_HBA_SOC_REG(ha, QLAFX00_HBA_ICNTRL_REG) & \
+	 QLAFX00_ICR_DIS_MASK))
+
+#define QLAFX00_RD_REG(ha, off) \
+	RD_REG_DWORD((ha)->cregbase + off)
+
+#define QLAFX00_WR_REG(ha, off, val) \
+	WRT_REG_DWORD((ha)->cregbase + off, val)
+
+struct qla_mt_iocb_rqst_fx00 {
+	uint32_t reserved_0;
+
+	uint16_t func_type;
+	uint8_t flags;
+	uint8_t reserved_1;
+
+	uint32_t dataword;
+
+	uint32_t adapid;
+	uint32_t adapid_hi;
+
+	uint32_t dataword_extra;
+
+	uint32_t req_len;
+
+	uint32_t rsp_len;
+};
+
+struct qla_mt_iocb_rsp_fx00 {
+	uint32_t reserved_1;
+
+	uint16_t func_type;
+	uint16_t ioctl_flags;
+
+	uint32_t ioctl_data;
+
+	uint32_t adapid;
+	uint32_t adapid_hi;
+
+	uint32_t reserved_2;
+	uint32_t seq_number;
+
+	uint8_t reserved_3[20];
+
+	int32_t res_count;
+
+	uint32_t status;
+};
+
+
+#define MAILBOX_REGISTER_COUNT_FX00	16
+#define AEN_MAILBOX_REGISTER_COUNT_FX00	8
+#define MAX_FIBRE_DEVICES_FX00	512
+#define MAX_LUNS_FX00		0x1024
+#define MAX_TARGETS_FX00	MAX_ISA_DEVICES
+#define REQUEST_ENTRY_CNT_FX00		512	/* Number of request entries. */
+#define RESPONSE_ENTRY_CNT_FX00		256	/* Number of response entries.*/
+
+/*
+ * Firmware state codes for QLAFX00 adapters
+ */
+#define FSTATE_FX00_CONFIG_WAIT     0x0000	/* Waiting for driver to issue
+						 * Initialize FW Mbox cmd
+						 */
+#define FSTATE_FX00_INITIALIZED     0x1000	/* FW has been initialized by
+						 * the driver
+						 */
+
+#define FX00_DEF_RATOV	10
+
+struct mr_data_fx00 {
+	uint8_t	product_name[256];
+	uint8_t	symbolic_name[64];
+	uint8_t	serial_num[32];
+	uint8_t	hw_version[16];
+	uint8_t	fw_version[16];
+	uint8_t	uboot_version[16];
+	uint8_t	fru_serial_num[32];
+	fc_port_t       fcport;		/* fcport used for requests
+					 * that are not linked
+					 * to a particular target
+					 */
+	uint8_t fw_hbt_en;
+	uint8_t fw_hbt_cnt;
+	uint8_t fw_hbt_miss_cnt;
+	uint32_t old_fw_hbt_cnt;
+	uint16_t fw_reset_timer_tick;
+	uint8_t fw_reset_timer_exp;
+	uint32_t old_aenmbx0_state;
+};
+
+#define QLAFX00_LOOP_DOWN_TIME		615     /* 600 */
+#define QLAFX00_HEARTBEAT_INTERVAL	6	/* number of seconds */
+#define QLAFX00_HEARTBEAT_MISS_CNT	3	/* number of miss */
+#define QLAFX00_RESET_INTERVAL		120	/* number of seconds */
+#define QLAFX00_MAX_RESET_INTERVAL	600	/* number of seconds */
+#endif
diff --git a/drivers/scsi/qla2xxx/qla_os.c b/drivers/scsi/qla2xxx/qla_os.c
index 2c6dd3dfe0f4..5307bf86d5e0 100644
--- a/drivers/scsi/qla2xxx/qla_os.c
+++ b/drivers/scsi/qla2xxx/qla_os.c
@@ -47,6 +47,7 @@ MODULE_PARM_DESC(ql2xenableclass2,
 		"Specify if Class 2 operations are supported from the very "
 		"beginning. Default is 0 - class 2 not supported.");
 
+
 int ql2xlogintimeout = 20;
 module_param(ql2xlogintimeout, int, S_IRUGO);
 MODULE_PARM_DESC(ql2xlogintimeout,
@@ -354,7 +355,12 @@ fail_req_map:
 
 static void qla2x00_free_req_que(struct qla_hw_data *ha, struct req_que *req)
 {
-	if (req && req->ring)
+	if (IS_QLAFX00(ha)) {
+		if (req && req->ring_fx00)
+			dma_free_coherent(&ha->pdev->dev,
+			    (req->length_fx00 + 1) * sizeof(request_t),
+			    req->ring_fx00, req->dma_fx00);
+	} else if (req && req->ring)
 		dma_free_coherent(&ha->pdev->dev,
 		(req->length + 1) * sizeof(request_t),
 		req->ring, req->dma);
@@ -368,11 +374,16 @@ static void qla2x00_free_req_que(struct qla_hw_data *ha, struct req_que *req)
 
 static void qla2x00_free_rsp_que(struct qla_hw_data *ha, struct rsp_que *rsp)
 {
-	if (rsp && rsp->ring)
+	if (IS_QLAFX00(ha)) {
+		if (rsp && rsp->ring)
+			dma_free_coherent(&ha->pdev->dev,
+			    (rsp->length_fx00 + 1) * sizeof(request_t),
+			    rsp->ring_fx00, rsp->dma_fx00);
+	} else if (rsp && rsp->ring) {
 		dma_free_coherent(&ha->pdev->dev,
 		(rsp->length + 1) * sizeof(response_t),
 		rsp->ring, rsp->dma);
-
+	}
 	kfree(rsp);
 	rsp = NULL;
 }
@@ -633,7 +644,7 @@ qla2x00_sp_free_dma(void *vha, void *ptr)
 	qla2x00_rel_sp(sp->fcport->vha, sp);
 }
 
-static void
+void
 qla2x00_sp_compl(void *data, void *ptr, int res)
 {
 	struct qla_hw_data *ha = (struct qla_hw_data *)data;
@@ -657,6 +668,9 @@ qla2x00_sp_compl(void *data, void *ptr, int res)
 	cmd->scsi_done(cmd);
 }
 
+/* If we are SP1 here, we need to still take and release the host_lock as SP1
+ * does not have the changes necessary to avoid taking host->host_lock.
+ */
 static int
 qla2xxx_queuecommand(struct Scsi_Host *host, struct scsi_cmnd *cmd)
 {
@@ -1304,6 +1318,9 @@ qla2x00_loop_reset(scsi_qla_host_t *vha)
 		}
 	}
 
+	if (IS_QLAFX00(ha))
+		return QLA_SUCCESS;
+
 	if (ha->flags.enable_lip_full_login && !IS_CNA_CAPABLE(ha)) {
 		atomic_set(&vha->loop_state, LOOP_DOWN);
 		atomic_set(&vha->loop_down_timer, LOOP_DOWN_TIME);
@@ -1858,6 +1875,7 @@ static struct isp_operations qla2100_isp_ops = {
 	.start_scsi		= qla2x00_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
 	.iospace_config     	= qla2x00_iospace_config,
+	.initialize_adapter	= qla2x00_initialize_adapter,
 };
 
 static struct isp_operations qla2300_isp_ops = {
@@ -1895,6 +1913,7 @@ static struct isp_operations qla2300_isp_ops = {
 	.start_scsi		= qla2x00_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
 	.iospace_config     	= qla2x00_iospace_config,
+	.initialize_adapter	= qla2x00_initialize_adapter,
 };
 
 static struct isp_operations qla24xx_isp_ops = {
@@ -1932,6 +1951,7 @@ static struct isp_operations qla24xx_isp_ops = {
 	.start_scsi		= qla24xx_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
 	.iospace_config     	= qla2x00_iospace_config,
+	.initialize_adapter	= qla2x00_initialize_adapter,
 };
 
 static struct isp_operations qla25xx_isp_ops = {
@@ -1969,6 +1989,7 @@ static struct isp_operations qla25xx_isp_ops = {
 	.start_scsi		= qla24xx_dif_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
 	.iospace_config     	= qla2x00_iospace_config,
+	.initialize_adapter	= qla2x00_initialize_adapter,
 };
 
 static struct isp_operations qla81xx_isp_ops = {
@@ -2006,6 +2027,7 @@ static struct isp_operations qla81xx_isp_ops = {
 	.start_scsi		= qla24xx_dif_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
 	.iospace_config     	= qla2x00_iospace_config,
+	.initialize_adapter	= qla2x00_initialize_adapter,
 };
 
 static struct isp_operations qla82xx_isp_ops = {
@@ -2043,6 +2065,7 @@ static struct isp_operations qla82xx_isp_ops = {
 	.start_scsi             = qla82xx_start_scsi,
 	.abort_isp		= qla82xx_abort_isp,
 	.iospace_config     	= qla82xx_iospace_config,
+	.initialize_adapter	= qla2x00_initialize_adapter,
 };
 
 static struct isp_operations qla83xx_isp_ops = {
@@ -2080,6 +2103,45 @@ static struct isp_operations qla83xx_isp_ops = {
 	.start_scsi		= qla24xx_dif_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
 	.iospace_config		= qla83xx_iospace_config,
+	.initialize_adapter	= qla2x00_initialize_adapter,
+};
+
+static struct isp_operations qlafx00_isp_ops = {
+	.pci_config		= qlafx00_pci_config,
+	.reset_chip		= qlafx00_soft_reset,
+	.chip_diag		= qlafx00_chip_diag,
+	.config_rings		= qlafx00_config_rings,
+	.reset_adapter		= qlafx00_soft_reset,
+	.nvram_config		= NULL,
+	.update_fw_options	= NULL,
+	.load_risc		= NULL,
+	.pci_info_str		= qlafx00_pci_info_str,
+	.fw_version_str		= qlafx00_fw_version_str,
+	.intr_handler		= qlafx00_intr_handler,
+	.enable_intrs		= qlafx00_enable_intrs,
+	.disable_intrs		= qlafx00_disable_intrs,
+	.abort_command		= qlafx00_abort_command,
+	.target_reset		= qlafx00_abort_target,
+	.lun_reset		= qlafx00_lun_reset,
+	.fabric_login		= NULL,
+	.fabric_logout		= NULL,
+	.calc_req_entries	= NULL,
+	.build_iocbs		= NULL,
+	.prep_ms_iocb		= qla24xx_prep_ms_iocb,
+	.prep_ms_fdmi_iocb	= qla24xx_prep_ms_fdmi_iocb,
+	.read_nvram		= qla24xx_read_nvram_data,
+	.write_nvram		= qla24xx_write_nvram_data,
+	.fw_dump		= NULL,
+	.beacon_on		= qla24xx_beacon_on,
+	.beacon_off		= qla24xx_beacon_off,
+	.beacon_blink		= NULL,
+	.read_optrom		= qla24xx_read_optrom_data,
+	.write_optrom		= qla24xx_write_optrom_data,
+	.get_flash_version	= qla24xx_get_flash_version,
+	.start_scsi		= qlafx00_start_scsi,
+	.abort_isp		= qlafx00_abort_isp,
+	.iospace_config		= qlafx00_iospace_config,
+	.initialize_adapter	= qlafx00_initialize_adapter,
 };
 
 static inline void
@@ -2192,6 +2254,9 @@ qla2x00_set_isp_flags(struct qla_hw_data *ha)
 		ha->device_type |= DT_T10_PI;
 		ha->fw_srisc_address = RISC_START_ADDRESS_2400;
 		break;
+	case PCI_DEVICE_ID_QLOGIC_ISPF001:
+		ha->device_type |= DT_ISPFX00;
+		break;
 	}
 
 	if (IS_QLA82XX(ha))
@@ -2265,7 +2330,8 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISP8001 ||
 	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISP8021 ||
 	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISP2031 ||
-	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISP8031) {
+	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISP8031 ||
+	    pdev->device == PCI_DEVICE_ID_QLOGIC_ISPF001) {
 		bars = pci_select_bars(pdev, IORESOURCE_MEM);
 		mem_only = 1;
 		ql_dbg_pci(ql_dbg_init, pdev, 0x0007,
@@ -2436,6 +2502,18 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 		ha->flash_data_off = FARX_ACCESS_FLASH_DATA_81XX;
 		ha->nvram_conf_off = ~0;
 		ha->nvram_data_off = ~0;
+	}  else if (IS_QLAFX00(ha)) {
+		ha->max_fibre_devices = MAX_FIBRE_DEVICES_FX00;
+		ha->mbx_count = MAILBOX_REGISTER_COUNT_FX00;
+		ha->aen_mbx_count = AEN_MAILBOX_REGISTER_COUNT_FX00;
+		req_length = REQUEST_ENTRY_CNT_FX00;
+		rsp_length = RESPONSE_ENTRY_CNT_FX00;
+		ha->init_cb_size = sizeof(struct init_cb_fx);
+		ha->isp_ops = &qlafx00_isp_ops;
+		ha->port_down_retry_count = 30; /* default value */
+		ha->mr.fw_hbt_cnt = QLAFX00_HEARTBEAT_INTERVAL;
+		ha->mr.fw_reset_timer_tick = QLAFX00_RESET_INTERVAL;
+		ha->mr.fw_hbt_en = 1;
 	}
 
 	ql_dbg_pci(ql_dbg_init, pdev, 0x001e,
@@ -2500,13 +2578,24 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	host = base_vha->host;
 	base_vha->req = req;
-	host->can_queue = req->length + 128;
+	if (IS_QLAFX00(ha))
+		host->can_queue = 1024;
+	else
+		host->can_queue = req->length + 128;
 	if (IS_QLA2XXX_MIDTYPE(ha))
 		base_vha->mgmt_svr_loop_id = 10 + base_vha->vp_idx;
 	else
 		base_vha->mgmt_svr_loop_id = MANAGEMENT_SERVER +
 						base_vha->vp_idx;
 
+	/* Setup fcport template structure. */
+	ha->mr.fcport.vha = base_vha;
+	ha->mr.fcport.port_type = FCT_UNKNOWN;
+	ha->mr.fcport.loop_id = FC_NO_LOOP_ID;
+	qla2x00_set_fcport_state(&ha->mr.fcport, FCS_UNCONFIGURED);
+	ha->mr.fcport.supported_classes = FC_COS_UNSPECIFIED;
+	ha->mr.fcport.scan_state = 1;
+
 	/* Set the SG table size based on ISP type */
 	if (!IS_FWI2_CAPABLE(ha)) {
 		if (IS_QLA2100(ha))
@@ -2562,6 +2651,13 @@ que_init:
 	rsp->req = req;
 	req->rsp = rsp;
 
+	if (IS_QLAFX00(ha)) {
+		ha->rsp_q_map[0] = rsp;
+		ha->req_q_map[0] = req;
+		set_bit(0, ha->req_qid_map);
+		set_bit(0, ha->rsp_qid_map);
+	}
+
 	/* FWI2-capable only. */
 	req->req_q_in = &ha->iobase->isp24.req_q_in;
 	req->req_q_out = &ha->iobase->isp24.req_q_out;
@@ -2574,6 +2670,13 @@ que_init:
 		rsp->rsp_q_out =  &ha->mqiobase->isp25mq.rsp_q_out;
 	}
 
+	if (IS_QLAFX00(ha)) {
+		req->req_q_in = &ha->iobase->ispfx00.req_q_in;
+		req->req_q_out = &ha->iobase->ispfx00.req_q_out;
+		rsp->rsp_q_in = &ha->iobase->ispfx00.rsp_q_in;
+		rsp->rsp_q_out = &ha->iobase->ispfx00.rsp_q_out;
+	}
+
 	if (IS_QLA82XX(ha)) {
 		req->req_q_out = &ha->iobase->isp82.req_q_out[0];
 		rsp->rsp_q_in = &ha->iobase->isp82.rsp_q_in[0];
@@ -2595,7 +2698,7 @@ que_init:
 	    "req->req_q_in=%p req->req_q_out=%p rsp->rsp_q_in=%p rsp->rsp_q_out=%p.\n",
 	    req->req_q_in, req->req_q_out, rsp->rsp_q_in, rsp->rsp_q_out);
 
-	if (qla2x00_initialize_adapter(base_vha)) {
+	if (ha->isp_ops->initialize_adapter(base_vha)) {
 		ql_log(ql_log_fatal, base_vha, 0x00d6,
 		    "Failed to initialize adapter - Adapter flags %x.\n",
 		    base_vha->device_flags);
@@ -2720,6 +2823,18 @@ skip_dpc:
 
 	qla2x00_alloc_sysfs_attr(base_vha);
 
+	if (IS_QLAFX00(ha)) {
+		ret = qlafx00_fx_disc(base_vha,
+			&base_vha->hw->mr.fcport, FXDISC_GET_CONFIG_INFO);
+
+		ret = qlafx00_fx_disc(base_vha,
+			&base_vha->hw->mr.fcport, FXDISC_GET_PORT_INFO);
+
+		/* Register system information */
+		ret =  qlafx00_fx_disc(base_vha,
+			&base_vha->hw->mr.fcport, FXDISC_REG_HOST_INFO);
+	}
+
 	qla2x00_init_host_attr(base_vha);
 
 	qla2x00_dfs_setup(base_vha);
@@ -2777,6 +2892,8 @@ iospace_config_failed:
 	} else {
 		if (ha->iobase)
 			iounmap(ha->iobase);
+		if (ha->cregbase)
+			iounmap(ha->cregbase);
 	}
 	pci_release_selected_regions(ha->pdev, ha->bars);
 	kfree(ha);
@@ -2960,6 +3077,9 @@ qla2x00_remove_one(struct pci_dev *pdev)
 		if (ha->iobase)
 			iounmap(ha->iobase);
 
+		if (ha->cregbase)
+			iounmap(ha->cregbase);
+
 		if (ha->mqiobase)
 			iounmap(ha->mqiobase);
 
@@ -3068,6 +3188,12 @@ qla2x00_schedule_rport_del(struct scsi_qla_host *vha, fc_port_t *fcport,
 void qla2x00_mark_device_lost(scsi_qla_host_t *vha, fc_port_t *fcport,
     int do_login, int defer)
 {
+	if (IS_QLAFX00(vha->hw)) {
+		qla2x00_set_fcport_state(fcport, FCS_DEVICE_LOST);
+		qla2x00_schedule_rport_del(vha, fcport, defer);
+		return;
+	}
+
 	if (atomic_read(&fcport->state) == FCS_ONLINE &&
 	    vha->vp_idx == fcport->vha->vp_idx) {
 		qla2x00_set_fcport_state(fcport, FCS_DEVICE_LOST);
@@ -3710,6 +3836,22 @@ qla2x00_uevent_emit(struct scsi_qla_host *vha, u32 code)
 	kobject_uevent_env(&vha->hw->pdev->dev.kobj, KOBJ_CHANGE, envp);
 }
 
+int
+qlafx00_post_aenfx_work(struct scsi_qla_host *vha,  uint32_t evtcode,
+			uint32_t *data, int cnt)
+{
+	struct qla_work_evt *e;
+
+	e = qla2x00_alloc_work(vha, QLA_EVT_AENFX);
+	if (!e)
+		return QLA_FUNCTION_FAILED;
+
+	e->u.aenfx.evtcode = evtcode;
+	e->u.aenfx.count = cnt;
+	memcpy(e->u.aenfx.mbx, data, sizeof(*data) * cnt);
+	return qla2x00_post_work(vha, e);
+}
+
 void
 qla2x00_do_work(struct scsi_qla_host *vha)
 {
@@ -3758,6 +3900,9 @@ qla2x00_do_work(struct scsi_qla_host *vha)
 		case QLA_EVT_UEVENT:
 			qla2x00_uevent_emit(vha, e->u.uevent.code);
 			break;
+		case QLA_EVT_AENFX:
+			qlafx00_process_aen(vha, e);
+			break;
 		}
 		if (e->flags & QLA_EVT_FLAG_FREE)
 			kfree(e);
@@ -4592,6 +4737,38 @@ qla2x00_do_dpc(void *data)
 				ql_dbg(ql_dbg_dpc, base_vha, 0x4006,
 				    "FCoE context reset end.\n");
 			}
+		} else if (IS_QLAFX00(ha)) {
+			if (test_and_clear_bit(ISP_UNRECOVERABLE,
+				&base_vha->dpc_flags)) {
+				ql_dbg(ql_dbg_dpc, base_vha, 0x4020,
+				    "Firmware Reset Recovery\n");
+				if (qlafx00_reset_initialize(base_vha)) {
+					/* Failed. Abort isp later. */
+					if (!test_bit(UNLOADING,
+					    &base_vha->dpc_flags))
+						set_bit(ISP_UNRECOVERABLE,
+						    &base_vha->dpc_flags);
+						ql_dbg(ql_dbg_dpc, base_vha,
+						    0x4021,
+						    "Reset Recovery Failed\n");
+				}
+			}
+
+			if (test_and_clear_bit(FX00_TARGET_SCAN,
+				&base_vha->dpc_flags)) {
+				ql_dbg(ql_dbg_dpc, base_vha, 0x4022,
+				    "ISPFx00 Target Scan scheduled\n");
+				if (qlafx00_rescan_isp(base_vha)) {
+					if (!test_bit(UNLOADING,
+					    &base_vha->dpc_flags))
+						set_bit(ISP_UNRECOVERABLE,
+						    &base_vha->dpc_flags);
+					ql_dbg(ql_dbg_dpc, base_vha, 0x401e,
+					    "ISPFx00 Target Scan Failed\n");
+				}
+				ql_dbg(ql_dbg_dpc, base_vha, 0x401f,
+				    "ISPFx00 Target Scan End\n");
+			}
 		}
 
 		if (test_and_clear_bit(ISP_ABORT_NEEDED,
@@ -4630,6 +4807,9 @@ qla2x00_do_dpc(void *data)
 			clear_bit(SCR_PENDING, &base_vha->dpc_flags);
 		}
 
+		if (IS_QLAFX00(ha))
+			goto loop_resync_check;
+
 		if (test_bit(ISP_QUIESCE_NEEDED, &base_vha->dpc_flags)) {
 			ql_dbg(ql_dbg_dpc, base_vha, 0x4009,
 			    "Quiescence mode scheduled.\n");
@@ -4654,7 +4834,7 @@ qla2x00_do_dpc(void *data)
 		}
 
 		if (test_and_clear_bit(RESET_MARKER_NEEDED,
-							&base_vha->dpc_flags) &&
+				&base_vha->dpc_flags) &&
 		    (!(test_and_set_bit(RESET_ACTIVE, &base_vha->dpc_flags)))) {
 
 			ql_dbg(ql_dbg_dpc, base_vha, 0x400b,
@@ -4677,9 +4857,9 @@ qla2x00_do_dpc(void *data)
 			ql_dbg(ql_dbg_dpc, base_vha, 0x400e,
 			    "Relogin end.\n");
 		}
-
+loop_resync_check:
 		if (test_and_clear_bit(LOOP_RESYNC_NEEDED,
-							&base_vha->dpc_flags)) {
+		    &base_vha->dpc_flags)) {
 
 			ql_dbg(ql_dbg_dpc, base_vha, 0x400f,
 			    "Loop resync scheduled.\n");
@@ -4697,6 +4877,9 @@ qla2x00_do_dpc(void *data)
 			    "Loop resync end.\n");
 		}
 
+		if (IS_QLAFX00(ha))
+			goto intr_on_check;
+
 		if (test_bit(NPIV_CONFIG_NEEDED, &base_vha->dpc_flags) &&
 		    atomic_read(&base_vha->loop_state) == LOOP_READY) {
 			clear_bit(NPIV_CONFIG_NEEDED, &base_vha->dpc_flags);
@@ -4714,7 +4897,7 @@ qla2x00_do_dpc(void *data)
 		if (test_and_clear_bit(HOST_RAMP_UP_QUEUE_DEPTH,
 		    &base_vha->dpc_flags))
 			qla2x00_host_ramp_up_queuedepth(base_vha);
-
+intr_on_check:
 		if (!ha->interrupts_on)
 			ha->isp_ops->enable_intrs(ha);
 
@@ -4722,7 +4905,8 @@ qla2x00_do_dpc(void *data)
 					&base_vha->dpc_flags))
 			ha->isp_ops->beacon_blink(base_vha);
 
-		qla2x00_do_dpc_all_vps(base_vha);
+		if (!IS_QLAFX00(ha))
+			qla2x00_do_dpc_all_vps(base_vha);
 
 		ha->dpc_active = 0;
 end_loop:
@@ -4818,6 +5002,9 @@ qla2x00_timer(scsi_qla_host_t *vha)
 		qla82xx_watchdog(vha);
 	}
 
+	if (!vha->vp_idx && IS_QLAFX00(ha))
+		qlafx00_timer_routine(vha);
+
 	/* Loop down handler. */
 	if (atomic_read(&vha->loop_down_timer) > 0 &&
 	    !(test_bit(ABORT_ISP_ACTIVE, &vha->dpc_flags)) &&
@@ -5335,6 +5522,7 @@ static struct pci_device_id qla2xxx_pci_tbl[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_QLOGIC, PCI_DEVICE_ID_QLOGIC_ISP8001) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_QLOGIC, PCI_DEVICE_ID_QLOGIC_ISP8021) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_QLOGIC, PCI_DEVICE_ID_QLOGIC_ISP8031) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_QLOGIC, PCI_DEVICE_ID_QLOGIC_ISPF001) },
 	{ 0 },
 };
 MODULE_DEVICE_TABLE(pci, qla2xxx_pci_tbl);
@@ -5351,7 +5539,7 @@ static struct pci_driver qla2xxx_pci_driver = {
 	.err_handler	= &qla2xxx_err_handler,
 };
 
-static struct file_operations apidev_fops = {
+static const struct file_operations apidev_fops = {
 	.owner = THIS_MODULE,
 	.llseek = noop_llseek,
 };
diff --git a/drivers/scsi/qla2xxx/qla_target.c b/drivers/scsi/qla2xxx/qla_target.c
index 61b5d8c2b5da..fcdc22306cab 100644
--- a/drivers/scsi/qla2xxx/qla_target.c
+++ b/drivers/scsi/qla2xxx/qla_target.c
@@ -2585,25 +2585,6 @@ static void qlt_do_ctio_completion(struct scsi_qla_host *vha, uint32_t handle,
 	ha->tgt.tgt_ops->free_cmd(cmd);
 }
 
-/* ha->hardware_lock supposed to be held on entry */
-/* called via callback from qla2xxx */
-void qlt_ctio_completion(struct scsi_qla_host *vha, uint32_t handle)
-{
-	struct qla_hw_data *ha = vha->hw;
-	struct qla_tgt *tgt = ha->tgt.qla_tgt;
-
-	if (likely(tgt == NULL)) {
-		ql_dbg(ql_dbg_tgt, vha, 0xe021,
-		    "CTIO, but target mode not enabled"
-		    " (ha %d %p handle %#x)", vha->vp_idx, ha, handle);
-		return;
-	}
-
-	tgt->irq_cmd_count++;
-	qlt_do_ctio_completion(vha, handle, CTIO_SUCCESS, NULL);
-	tgt->irq_cmd_count--;
-}
-
 static inline int qlt_get_fcp_task_attr(struct scsi_qla_host *vha,
 	uint8_t task_codes)
 {
diff --git a/drivers/scsi/qla2xxx/qla_target.h b/drivers/scsi/qla2xxx/qla_target.h
index ff9ccb9fd036..b33e411f28a0 100644
--- a/drivers/scsi/qla2xxx/qla_target.h
+++ b/drivers/scsi/qla2xxx/qla_target.h
@@ -980,7 +980,6 @@ extern int qlt_xmit_response(struct qla_tgt_cmd *, int, uint8_t);
 extern void qlt_xmit_tm_rsp(struct qla_tgt_mgmt_cmd *);
 extern void qlt_free_mcmd(struct qla_tgt_mgmt_cmd *);
 extern void qlt_free_cmd(struct qla_tgt_cmd *cmd);
-extern void qlt_ctio_completion(struct scsi_qla_host *, uint32_t);
 extern void qlt_async_event(uint16_t, struct scsi_qla_host *, uint16_t *);
 extern void qlt_enable_vha(struct scsi_qla_host *);
 extern void qlt_vport_create(struct scsi_qla_host *, struct qla_hw_data *);
diff --git a/drivers/scsi/qla2xxx/qla_version.h b/drivers/scsi/qla2xxx/qla_version.h
index ec54036d1e12..6c66d22eb1b1 100644
--- a/drivers/scsi/qla2xxx/qla_version.h
+++ b/drivers/scsi/qla2xxx/qla_version.h
@@ -7,9 +7,9 @@
 /*
  * Driver version
  */
-#define QLA2XXX_VERSION      "8.04.00.13-k"
+#define QLA2XXX_VERSION      "8.05.00.03-k"
 
 #define QLA_DRIVER_MAJOR_VER	8
-#define QLA_DRIVER_MINOR_VER	4
+#define QLA_DRIVER_MINOR_VER	5
 #define QLA_DRIVER_PATCH_VER	0
 #define QLA_DRIVER_BETA_VER	0
diff --git a/drivers/scsi/qla4xxx/ql4_83xx.c b/drivers/scsi/qla4xxx/ql4_83xx.c
index 5d8fe4f75650..d607eb8e24cb 100644
--- a/drivers/scsi/qla4xxx/ql4_83xx.c
+++ b/drivers/scsi/qla4xxx/ql4_83xx.c
@@ -1629,9 +1629,37 @@ static void __qla4_83xx_disable_pause(struct scsi_qla_host *ha)
 	ql4_printk(KERN_INFO, ha, "Disabled pause frames successfully.\n");
 }
 
+/**
+ * qla4_83xx_eport_init - Initialize EPort.
+ * @ha: Pointer to host adapter structure.
+ *
+ * If EPort hardware is in reset state before disabling pause, there would be
+ * serious hardware wedging issues. To prevent this perform eport init everytime
+ * before disabling pause frames.
+ **/
+static void qla4_83xx_eport_init(struct scsi_qla_host *ha)
+{
+	/* Clear the 8 registers */
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_REG, 0x0);
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_PORT0, 0x0);
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_PORT1, 0x0);
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_PORT2, 0x0);
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_PORT3, 0x0);
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_SRE_SHIM, 0x0);
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_EPG_SHIM, 0x0);
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_ETHER_PCS, 0x0);
+
+	/* Write any value to Reset Control register */
+	qla4_83xx_wr_reg_indirect(ha, QLA83XX_RESET_CONTROL, 0xFF);
+
+	ql4_printk(KERN_INFO, ha, "EPORT is out of reset.\n");
+}
+
 void qla4_83xx_disable_pause(struct scsi_qla_host *ha)
 {
 	ha->isp_ops->idc_lock(ha);
+	/* Before disabling pause frames, ensure that eport is not in reset */
+	qla4_83xx_eport_init(ha);
 	qla4_83xx_dump_pause_control_regs(ha);
 	__qla4_83xx_disable_pause(ha);
 	ha->isp_ops->idc_unlock(ha);
diff --git a/drivers/scsi/qla4xxx/ql4_83xx.h b/drivers/scsi/qla4xxx/ql4_83xx.h
index 6a00f903f2a6..fab237fa32cc 100644
--- a/drivers/scsi/qla4xxx/ql4_83xx.h
+++ b/drivers/scsi/qla4xxx/ql4_83xx.h
@@ -55,6 +55,16 @@
 #define QLA83XX_SET_PAUSE_VAL		0x0
 #define QLA83XX_SET_TC_MAX_CELL_VAL	0x03FF03FF
 
+#define QLA83XX_RESET_CONTROL		0x28084E50
+#define QLA83XX_RESET_REG		0x28084E60
+#define QLA83XX_RESET_PORT0		0x28084E70
+#define QLA83XX_RESET_PORT1		0x28084E80
+#define QLA83XX_RESET_PORT2		0x28084E90
+#define QLA83XX_RESET_PORT3		0x28084EA0
+#define QLA83XX_RESET_SRE_SHIM		0x28084EB0
+#define QLA83XX_RESET_EPG_SHIM		0x28084EC0
+#define QLA83XX_RESET_ETHER_PCS		0x28084ED0
+
 /* qla_83xx_reg_tbl registers */
 #define QLA83XX_PEG_HALT_STATUS1	0x34A8
 #define QLA83XX_PEG_HALT_STATUS2	0x34AC
diff --git a/drivers/scsi/qla4xxx/ql4_dbg.h b/drivers/scsi/qla4xxx/ql4_dbg.h
index 5b0afc18ef18..51c365bcf912 100644
--- a/drivers/scsi/qla4xxx/ql4_dbg.h
+++ b/drivers/scsi/qla4xxx/ql4_dbg.h
@@ -12,6 +12,7 @@
 /* #define QL_DEBUG_LEVEL_3  */		/* Output function tracing */
 /* #define QL_DEBUG_LEVEL_4  */
 /* #define QL_DEBUG_LEVEL_5  */
+/* #define QL_DEBUG_LEVEL_7  */
 /* #define QL_DEBUG_LEVEL_9  */
 
 #define QL_DEBUG_LEVEL_2	/* ALways enable error messagess */
@@ -48,6 +49,12 @@
 #define DEBUG5(x)	do {} while (0);
 #endif				/*  */
 
+#if defined(QL_DEBUG_LEVEL_7)
+#define DEBUG7(x)	do {x; } while (0)
+#else				/*  */
+#define DEBUG7(x)	do {} while (0)
+#endif				/*  */
+
 #if defined(QL_DEBUG_LEVEL_9)
 #define DEBUG9(x)	do {x;} while (0);
 #else				/*  */
diff --git a/drivers/scsi/qla4xxx/ql4_def.h b/drivers/scsi/qla4xxx/ql4_def.h
index 129f5dd02822..ddf16a86bbf5 100644
--- a/drivers/scsi/qla4xxx/ql4_def.h
+++ b/drivers/scsi/qla4xxx/ql4_def.h
@@ -159,6 +159,22 @@
 #define LSDW(x) ((u32)((u64)(x)))
 #define MSDW(x) ((u32)((((u64)(x)) >> 16) >> 16))
 
+#define DEV_DB_NON_PERSISTENT	0
+#define DEV_DB_PERSISTENT	1
+
+#define COPY_ISID(dst_isid, src_isid) {			\
+	int i, j;					\
+	for (i = 0, j = ISID_SIZE - 1; i < ISID_SIZE;)	\
+		dst_isid[i++] = src_isid[j--];		\
+}
+
+#define SET_BITVAL(o, n, v) {	\
+	if (o)			\
+		n |= v;		\
+	else			\
+		n &= ~v;	\
+}
+
 /*
  * Retry & Timeout Values
  */
@@ -363,6 +379,8 @@ struct ql82xx_hw_data {
 	uint32_t flt_iscsi_param;
 	uint32_t flt_region_chap;
 	uint32_t flt_chap_size;
+	uint32_t flt_region_ddb;
+	uint32_t flt_ddb_size;
 };
 
 struct qla4_8xxx_legacy_intr_set {
@@ -501,6 +519,7 @@ struct scsi_qla_host {
 #define AF_INIT_DONE			1 /* 0x00000002 */
 #define AF_MBOX_COMMAND			2 /* 0x00000004 */
 #define AF_MBOX_COMMAND_DONE		3 /* 0x00000008 */
+#define AF_ST_DISCOVERY_IN_PROGRESS	4 /* 0x00000010 */
 #define AF_INTERRUPTS_ON		6 /* 0x00000040 */
 #define AF_GET_CRASH_RECORD		7 /* 0x00000080 */
 #define AF_LINK_UP			8 /* 0x00000100 */
diff --git a/drivers/scsi/qla4xxx/ql4_fw.h b/drivers/scsi/qla4xxx/ql4_fw.h
index ad9d2e2d370f..c7b8892b5a83 100644
--- a/drivers/scsi/qla4xxx/ql4_fw.h
+++ b/drivers/scsi/qla4xxx/ql4_fw.h
@@ -288,6 +288,8 @@ union external_hw_config_reg {
 #define FA_GOLD_RISC_CODE_ADDR_82	0x80000
 #define FA_FLASH_ISCSI_CHAP		0x540000
 #define FA_FLASH_CHAP_SIZE		0xC0000
+#define FA_FLASH_ISCSI_DDB		0x420000
+#define FA_FLASH_DDB_SIZE		0x080000
 
 /* Flash Description Table */
 struct qla_fdt_layout {
@@ -348,6 +350,7 @@ struct qla_flt_header {
 #define FLT_REG_BOOT_CODE_82	0x78
 #define FLT_REG_ISCSI_PARAM	0x65
 #define FLT_REG_ISCSI_CHAP	0x63
+#define FLT_REG_ISCSI_DDB	0x6A
 
 struct qla_flt_region {
 	uint32_t code;
@@ -490,12 +493,16 @@ struct qla_flt_region {
 #define MBOX_ASTS_SUBNET_STATE_CHANGE		0x8027
 #define MBOX_ASTS_RESPONSE_QUEUE_FULL		0x8028
 #define MBOX_ASTS_IP_ADDR_STATE_CHANGED		0x8029
+#define MBOX_ASTS_IPV6_DEFAULT_ROUTER_CHANGED	0x802A
 #define MBOX_ASTS_IPV6_PREFIX_EXPIRED		0x802B
 #define MBOX_ASTS_IPV6_ND_PREFIX_IGNORED	0x802C
 #define MBOX_ASTS_IPV6_LCL_PREFIX_IGNORED	0x802D
 #define MBOX_ASTS_ICMPV6_ERROR_MSG_RCVD		0x802E
+#define MBOX_ASTS_INITIALIZATION_FAILED		0x8031
+#define MBOX_ASTS_SYSTEM_WARNING_EVENT		0x8036
 #define MBOX_ASTS_IDC_COMPLETE			0x8100
 #define MBOX_ASTS_IDC_REQUEST_NOTIFICATION	0x8101
+#define MBOX_ASTS_DCBX_CONF_CHANGE		0x8110
 #define MBOX_ASTS_TXSCVR_INSERTED		0x8130
 #define MBOX_ASTS_TXSCVR_REMOVED		0x8131
 
@@ -779,12 +786,41 @@ struct dev_db_entry {
 #define DDB_OPT_IPV6_NULL_LINK_LOCAL		0x800 /* post connection */
 #define DDB_OPT_IPV6_FW_DEFINED_LINK_LOCAL	0x800 /* pre connection */
 
+#define OPT_IS_FW_ASSIGNED_IPV6		11
+#define OPT_IPV6_DEVICE			8
+#define OPT_AUTO_SENDTGTS_DISABLE	6
+#define OPT_DISC_SESSION		4
+#define OPT_ENTRY_STATE			3
 	uint16_t exec_throttle;	/* 02-03 */
 	uint16_t exec_count;	/* 04-05 */
 	uint16_t res0;	/* 06-07 */
 	uint16_t iscsi_options;	/* 08-09 */
+#define ISCSIOPT_HEADER_DIGEST_EN		13
+#define ISCSIOPT_DATA_DIGEST_EN			12
+#define ISCSIOPT_IMMEDIATE_DATA_EN		11
+#define ISCSIOPT_INITIAL_R2T_EN			10
+#define ISCSIOPT_DATA_SEQ_IN_ORDER		9
+#define ISCSIOPT_DATA_PDU_IN_ORDER		8
+#define ISCSIOPT_CHAP_AUTH_EN			7
+#define ISCSIOPT_SNACK_REQ_EN			6
+#define ISCSIOPT_DISCOVERY_LOGOUT_EN		5
+#define ISCSIOPT_BIDI_CHAP_EN			4
+#define ISCSIOPT_DISCOVERY_AUTH_OPTIONAL	3
+#define ISCSIOPT_ERL1				1
+#define ISCSIOPT_ERL0				0
+
 	uint16_t tcp_options;	/* 0A-0B */
+#define TCPOPT_TIMESTAMP_STAT	6
+#define TCPOPT_NAGLE_DISABLE	5
+#define TCPOPT_WSF_DISABLE	4
+#define TCPOPT_TIMER_SCALE3	3
+#define TCPOPT_TIMER_SCALE2	2
+#define TCPOPT_TIMER_SCALE1	1
+#define TCPOPT_TIMESTAMP_EN	0
+
 	uint16_t ip_options;	/* 0C-0D */
+#define IPOPT_FRAGMENT_DISABLE	4
+
 	uint16_t iscsi_max_rcv_data_seg_len;	/* 0E-0F */
 #define BYTE_UNITS	512
 	uint32_t res1;	/* 10-13 */
@@ -816,6 +852,8 @@ struct dev_db_entry {
 					 * much RAM */
 	uint8_t link_local_ipv6_addr[0x10]; /* 1A0-1AF */
 	uint8_t res5[0x10];	/* 1B0-1BF */
+#define DDB_NO_LINK	0xFFFF
+#define DDB_ISNS	0xFFFD
 	uint16_t ddb_link;	/* 1C0-1C1 */
 	uint16_t chap_tbl_idx;	/* 1C2-1C3 */
 	uint16_t tgt_portal_grp; /* 1C4-1C5 */
diff --git a/drivers/scsi/qla4xxx/ql4_glbl.h b/drivers/scsi/qla4xxx/ql4_glbl.h
index 982293edf02c..4a428009f699 100644
--- a/drivers/scsi/qla4xxx/ql4_glbl.h
+++ b/drivers/scsi/qla4xxx/ql4_glbl.h
@@ -191,6 +191,9 @@ int qla4xxx_ping_iocb(struct scsi_qla_host *ha, uint32_t options,
 int qla4xxx_post_ping_evt_work(struct scsi_qla_host *ha,
 			       uint32_t status, uint32_t pid,
 			       uint32_t data_size, uint8_t *data);
+int qla4xxx_flashdb_by_index(struct scsi_qla_host *ha,
+			     struct dev_db_entry *fw_ddb_entry,
+			     dma_addr_t fw_ddb_entry_dma, uint16_t ddb_index);
 
 /* BSG Functions */
 int qla4xxx_bsg_request(struct bsg_job *bsg_job);
@@ -224,8 +227,6 @@ void qla4_83xx_interrupt_service_routine(struct scsi_qla_host *ha,
 int qla4_83xx_isp_reset(struct scsi_qla_host *ha);
 void qla4_83xx_queue_iocb(struct scsi_qla_host *ha);
 void qla4_83xx_complete_iocb(struct scsi_qla_host *ha);
-uint16_t qla4_83xx_rd_shdw_req_q_out(struct scsi_qla_host *ha);
-uint16_t qla4_83xx_rd_shdw_rsp_q_in(struct scsi_qla_host *ha);
 uint32_t qla4_83xx_rd_reg(struct scsi_qla_host *ha, ulong addr);
 void qla4_83xx_wr_reg(struct scsi_qla_host *ha, ulong addr, uint32_t val);
 int qla4_83xx_rd_reg_indirect(struct scsi_qla_host *ha, uint32_t addr,
@@ -261,6 +262,10 @@ int qla4_83xx_post_idc_ack(struct scsi_qla_host *ha);
 void qla4_83xx_disable_pause(struct scsi_qla_host *ha);
 void qla4_83xx_enable_mbox_intrs(struct scsi_qla_host *ha);
 int qla4_83xx_can_perform_reset(struct scsi_qla_host *ha);
+int qla4xxx_get_default_ddb(struct scsi_qla_host *ha, uint32_t options,
+			    dma_addr_t dma_addr);
+int qla4xxx_get_uni_chap_at_index(struct scsi_qla_host *ha, char *username,
+				  char *password, uint16_t chap_index);
 
 extern int ql4xextended_error_logging;
 extern int ql4xdontresethba;
diff --git a/drivers/scsi/qla4xxx/ql4_isr.c b/drivers/scsi/qla4xxx/ql4_isr.c
index 1b83dc283d2e..482287f4005f 100644
--- a/drivers/scsi/qla4xxx/ql4_isr.c
+++ b/drivers/scsi/qla4xxx/ql4_isr.c
@@ -396,7 +396,6 @@ static void qla4xxx_passthru_status_entry(struct scsi_qla_host *ha,
 
 	task_data = task->dd_data;
 	memcpy(&task_data->sts, sts_entry, sizeof(struct passthru_status));
-	ha->req_q_count += task_data->iocb_req_cnt;
 	ha->iocb_cnt -= task_data->iocb_req_cnt;
 	queue_work(ha->task_wq, &task_data->task_work);
 }
@@ -416,7 +415,6 @@ static struct mrb *qla4xxx_del_mrb_from_active_array(struct scsi_qla_host *ha,
 		return mrb;
 
 	/* update counters */
-	ha->req_q_count += mrb->iocb_cnt;
 	ha->iocb_cnt -= mrb->iocb_cnt;
 
 	return mrb;
@@ -877,6 +875,43 @@ static void qla4xxx_isr_decode_mailbox(struct scsi_qla_host * ha,
 			}
 			break;
 
+		case MBOX_ASTS_IPV6_DEFAULT_ROUTER_CHANGED:
+			DEBUG2(ql4_printk(KERN_INFO, ha,
+					  "scsi%ld: AEN %04x, mbox_sts[1]=%08x, mbox_sts[2]=%08x, mbox_sts[3]=%08x, mbox_sts[4]=%08x mbox_sts[5]=%08x\n",
+					  ha->host_no, mbox_sts[0], mbox_sts[1],
+					  mbox_sts[2], mbox_sts[3], mbox_sts[4],
+					  mbox_sts[5]));
+			DEBUG2(ql4_printk(KERN_INFO, ha,
+					  "scsi%ld: AEN %04x Received IPv6 default router changed notification\n",
+					  ha->host_no, mbox_sts[0]));
+			break;
+
+		case MBOX_ASTS_INITIALIZATION_FAILED:
+			DEBUG2(ql4_printk(KERN_INFO, ha,
+					  "scsi%ld: AEN %04x, mbox_sts[3]=%08x\n",
+					  ha->host_no, mbox_sts[0],
+					  mbox_sts[3]));
+			break;
+
+		case MBOX_ASTS_SYSTEM_WARNING_EVENT:
+			DEBUG2(ql4_printk(KERN_WARNING, ha,
+					  "scsi%ld: AEN %04x, mbox_sts[1]=%08x, mbox_sts[2]=%08x, mbox_sts[3]=%08x, mbox_sts[4]=%08x mbox_sts[5]=%08x\n",
+					  ha->host_no, mbox_sts[0], mbox_sts[1],
+					  mbox_sts[2], mbox_sts[3], mbox_sts[4],
+					  mbox_sts[5]));
+			break;
+
+		case MBOX_ASTS_DCBX_CONF_CHANGE:
+			DEBUG2(ql4_printk(KERN_INFO, ha,
+					  "scsi%ld: AEN %04x, mbox_sts[1]=%08x, mbox_sts[2]=%08x, mbox_sts[3]=%08x, mbox_sts[4]=%08x mbox_sts[5]=%08x\n",
+					  ha->host_no, mbox_sts[0], mbox_sts[1],
+					  mbox_sts[2], mbox_sts[3], mbox_sts[4],
+					  mbox_sts[5]));
+			DEBUG2(ql4_printk(KERN_INFO, ha,
+					  "scsi%ld: AEN %04x Received DCBX configuration changed notification\n",
+					  ha->host_no, mbox_sts[0]));
+			break;
+
 		default:
 			DEBUG2(printk(KERN_WARNING
 				      "scsi%ld: AEN %04x UNKNOWN\n",
@@ -1099,8 +1134,8 @@ irqreturn_t qla4_82xx_intr_handler(int irq, void *dev_id)
 
 	status = qla4_82xx_rd_32(ha, ISR_INT_STATE_REG);
 	if (!ISR_IS_LEGACY_INTR_TRIGGERED(status)) {
-		DEBUG2(ql4_printk(KERN_INFO, ha,
-		    "%s legacy Int not triggered\n", __func__));
+		DEBUG7(ql4_printk(KERN_INFO, ha,
+				  "%s legacy Int not triggered\n", __func__));
 		return IRQ_NONE;
 	}
 
@@ -1158,7 +1193,7 @@ irqreturn_t qla4_83xx_intr_handler(int irq, void *dev_id)
 
 	/* Legacy interrupt is valid if bit31 of leg_int_ptr is set */
 	if (!(leg_int_ptr & LEG_INT_PTR_B31)) {
-		DEBUG2(ql4_printk(KERN_ERR, ha,
+		DEBUG7(ql4_printk(KERN_ERR, ha,
 				  "%s: Legacy Interrupt Bit 31 not set, spurious interrupt!\n",
 				  __func__));
 		return IRQ_NONE;
@@ -1166,7 +1201,7 @@ irqreturn_t qla4_83xx_intr_handler(int irq, void *dev_id)
 
 	/* Validate the PCIE function ID set in leg_int_ptr bits [19..16] */
 	if ((leg_int_ptr & PF_BITS_MASK) != ha->pf_bit) {
-		DEBUG2(ql4_printk(KERN_ERR, ha,
+		DEBUG7(ql4_printk(KERN_ERR, ha,
 				  "%s: Incorrect function ID 0x%x in legacy interrupt register, ha->pf_bit = 0x%x\n",
 				  __func__, (leg_int_ptr & PF_BITS_MASK),
 				  ha->pf_bit));
diff --git a/drivers/scsi/qla4xxx/ql4_mbx.c b/drivers/scsi/qla4xxx/ql4_mbx.c
index 160d33697216..a501beab3ffe 100644
--- a/drivers/scsi/qla4xxx/ql4_mbx.c
+++ b/drivers/scsi/qla4xxx/ql4_mbx.c
@@ -1129,6 +1129,7 @@ int qla4xxx_reset_lun(struct scsi_qla_host * ha, struct ddb_entry * ddb_entry,
 {
 	uint32_t mbox_cmd[MBOX_REG_COUNT];
 	uint32_t mbox_sts[MBOX_REG_COUNT];
+	uint32_t scsi_lun[2];
 	int status = QLA_SUCCESS;
 
 	DEBUG2(printk("scsi%ld:%d:%d: lun reset issued\n", ha->host_no,
@@ -1140,10 +1141,16 @@ int qla4xxx_reset_lun(struct scsi_qla_host * ha, struct ddb_entry * ddb_entry,
 	 */
 	memset(&mbox_cmd, 0, sizeof(mbox_cmd));
 	memset(&mbox_sts, 0, sizeof(mbox_sts));
+	int_to_scsilun(lun, (struct scsi_lun *) scsi_lun);
 
 	mbox_cmd[0] = MBOX_CMD_LUN_RESET;
 	mbox_cmd[1] = ddb_entry->fw_ddb_index;
-	mbox_cmd[2] = lun << 8;
+	/* FW expects LUN bytes 0-3 in Incoming Mailbox 2
+	 * (LUN byte 0 is LSByte, byte 3 is MSByte) */
+	mbox_cmd[2] = cpu_to_le32(scsi_lun[0]);
+	/* FW expects LUN bytes 4-7 in Incoming Mailbox 3
+	 * (LUN byte 4 is LSByte, byte 7 is MSByte) */
+	mbox_cmd[3] = cpu_to_le32(scsi_lun[1]);
 	mbox_cmd[5] = 0x01;	/* Immediate Command Enable */
 
 	qla4xxx_mailbox_command(ha, MBOX_REG_COUNT, 1, &mbox_cmd[0], &mbox_sts[0]);
@@ -1281,8 +1288,8 @@ exit_about_fw:
 	return status;
 }
 
-static int qla4xxx_get_default_ddb(struct scsi_qla_host *ha, uint32_t options,
-				   dma_addr_t dma_addr)
+int qla4xxx_get_default_ddb(struct scsi_qla_host *ha, uint32_t options,
+			    dma_addr_t dma_addr)
 {
 	uint32_t mbox_cmd[MBOX_REG_COUNT];
 	uint32_t mbox_sts[MBOX_REG_COUNT];
@@ -1410,6 +1417,55 @@ exit_bootdb_failed:
 	return status;
 }
 
+int qla4xxx_flashdb_by_index(struct scsi_qla_host *ha,
+			     struct dev_db_entry *fw_ddb_entry,
+			     dma_addr_t fw_ddb_entry_dma, uint16_t ddb_index)
+{
+	uint32_t dev_db_start_offset;
+	uint32_t dev_db_end_offset;
+	int status = QLA_ERROR;
+
+	memset(fw_ddb_entry, 0, sizeof(*fw_ddb_entry));
+
+	if (is_qla40XX(ha)) {
+		dev_db_start_offset = FLASH_OFFSET_DB_INFO;
+		dev_db_end_offset = FLASH_OFFSET_DB_END;
+	} else {
+		dev_db_start_offset = FLASH_RAW_ACCESS_ADDR +
+				      (ha->hw.flt_region_ddb << 2);
+		/* flt_ddb_size is DDB table size for both ports
+		 * so divide it by 2 to calculate the offset for second port
+		 */
+		if (ha->port_num == 1)
+			dev_db_start_offset += (ha->hw.flt_ddb_size / 2);
+
+		dev_db_end_offset = dev_db_start_offset +
+				    (ha->hw.flt_ddb_size / 2);
+	}
+
+	dev_db_start_offset += (ddb_index * sizeof(*fw_ddb_entry));
+
+	if (dev_db_start_offset > dev_db_end_offset) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s:Invalid DDB index %d", __func__,
+				  ddb_index));
+		goto exit_fdb_failed;
+	}
+
+	if (qla4xxx_get_flash(ha, fw_ddb_entry_dma, dev_db_start_offset,
+			      sizeof(*fw_ddb_entry)) != QLA_SUCCESS) {
+		ql4_printk(KERN_ERR, ha, "scsi%ld: %s: Get Flash failed\n",
+			   ha->host_no, __func__);
+		goto exit_fdb_failed;
+	}
+
+	if (fw_ddb_entry->cookie == DDB_VALID_COOKIE)
+		status = QLA_SUCCESS;
+
+exit_fdb_failed:
+	return status;
+}
+
 int qla4xxx_get_chap(struct scsi_qla_host *ha, char *username, char *password,
 		     uint16_t idx)
 {
@@ -1503,6 +1559,62 @@ exit_set_chap:
 	return ret;
 }
 
+
+int qla4xxx_get_uni_chap_at_index(struct scsi_qla_host *ha, char *username,
+				  char *password, uint16_t chap_index)
+{
+	int rval = QLA_ERROR;
+	struct ql4_chap_table *chap_table = NULL;
+	int max_chap_entries;
+
+	if (!ha->chap_list) {
+		ql4_printk(KERN_ERR, ha, "Do not have CHAP table cache\n");
+		rval = QLA_ERROR;
+		goto exit_uni_chap;
+	}
+
+	if (!username || !password) {
+		ql4_printk(KERN_ERR, ha, "No memory for username & secret\n");
+		rval = QLA_ERROR;
+		goto exit_uni_chap;
+	}
+
+	if (is_qla80XX(ha))
+		max_chap_entries = (ha->hw.flt_chap_size / 2) /
+				   sizeof(struct ql4_chap_table);
+	else
+		max_chap_entries = MAX_CHAP_ENTRIES_40XX;
+
+	if (chap_index > max_chap_entries) {
+		ql4_printk(KERN_ERR, ha, "Invalid Chap index\n");
+		rval = QLA_ERROR;
+		goto exit_uni_chap;
+	}
+
+	mutex_lock(&ha->chap_sem);
+	chap_table = (struct ql4_chap_table *)ha->chap_list + chap_index;
+	if (chap_table->cookie != __constant_cpu_to_le16(CHAP_VALID_COOKIE)) {
+		rval = QLA_ERROR;
+		goto exit_unlock_uni_chap;
+	}
+
+	if (!(chap_table->flags & BIT_6)) {
+		ql4_printk(KERN_ERR, ha, "Unidirectional entry not set\n");
+		rval = QLA_ERROR;
+		goto exit_unlock_uni_chap;
+	}
+
+	strncpy(password, chap_table->secret, MAX_CHAP_SECRET_LEN);
+	strncpy(username, chap_table->name, MAX_CHAP_NAME_LEN);
+
+	rval = QLA_SUCCESS;
+
+exit_unlock_uni_chap:
+	mutex_unlock(&ha->chap_sem);
+exit_uni_chap:
+	return rval;
+}
+
 /**
  * qla4xxx_get_chap_index - Get chap index given username and secret
  * @ha: pointer to adapter structure
@@ -1524,7 +1636,7 @@ int qla4xxx_get_chap_index(struct scsi_qla_host *ha, char *username,
 	int max_chap_entries = 0;
 	struct ql4_chap_table *chap_table;
 
-	if (is_qla8022(ha))
+	if (is_qla80XX(ha))
 		max_chap_entries = (ha->hw.flt_chap_size / 2) /
 						sizeof(struct ql4_chap_table);
 	else
diff --git a/drivers/scsi/qla4xxx/ql4_nx.c b/drivers/scsi/qla4xxx/ql4_nx.c
index 9299400d3c9e..eaf00c162eb2 100644
--- a/drivers/scsi/qla4xxx/ql4_nx.c
+++ b/drivers/scsi/qla4xxx/ql4_nx.c
@@ -3154,6 +3154,10 @@ qla4_8xxx_get_flt_info(struct scsi_qla_host *ha, uint32_t flt_addr)
 			hw->flt_region_chap =  start;
 			hw->flt_chap_size =  le32_to_cpu(region->size);
 			break;
+		case FLT_REG_ISCSI_DDB:
+			hw->flt_region_ddb =  start;
+			hw->flt_ddb_size =  le32_to_cpu(region->size);
+			break;
 		}
 	}
 	goto done;
@@ -3166,14 +3170,19 @@ no_flash_data:
 	hw->flt_region_boot     = FA_BOOT_CODE_ADDR_82;
 	hw->flt_region_bootload = FA_BOOT_LOAD_ADDR_82;
 	hw->flt_region_fw       = FA_RISC_CODE_ADDR_82;
-	hw->flt_region_chap	= FA_FLASH_ISCSI_CHAP;
+	hw->flt_region_chap	= FA_FLASH_ISCSI_CHAP >> 2;
 	hw->flt_chap_size	= FA_FLASH_CHAP_SIZE;
+	hw->flt_region_ddb	= FA_FLASH_ISCSI_DDB >> 2;
+	hw->flt_ddb_size	= FA_FLASH_DDB_SIZE;
 
 done:
-	DEBUG2(ql4_printk(KERN_INFO, ha, "FLT[%s]: flt=0x%x fdt=0x%x "
-	    "boot=0x%x bootload=0x%x fw=0x%x\n", loc, hw->flt_region_flt,
-	    hw->flt_region_fdt,	hw->flt_region_boot, hw->flt_region_bootload,
-	    hw->flt_region_fw));
+	DEBUG2(ql4_printk(KERN_INFO, ha,
+			  "FLT[%s]: flt=0x%x fdt=0x%x boot=0x%x bootload=0x%x fw=0x%x chap=0x%x chap_size=0x%x ddb=0x%x  ddb_size=0x%x\n",
+			  loc, hw->flt_region_flt, hw->flt_region_fdt,
+			  hw->flt_region_boot, hw->flt_region_bootload,
+			  hw->flt_region_fw, hw->flt_region_chap,
+			  hw->flt_chap_size, hw->flt_region_ddb,
+			  hw->flt_ddb_size));
 }
 
 static void
diff --git a/drivers/scsi/qla4xxx/ql4_os.c b/drivers/scsi/qla4xxx/ql4_os.c
index 6142729167f4..a47f99957ba8 100644
--- a/drivers/scsi/qla4xxx/ql4_os.c
+++ b/drivers/scsi/qla4xxx/ql4_os.c
@@ -166,6 +166,26 @@ static int qla4xxx_host_reset(struct Scsi_Host *shost, int reset_type);
 static int qla4xxx_change_queue_depth(struct scsi_device *sdev, int qdepth,
 				      int reason);
 
+/*
+ * iSCSI Flash DDB sysfs entry points
+ */
+static int
+qla4xxx_sysfs_ddb_set_param(struct iscsi_bus_flash_session *fnode_sess,
+			    struct iscsi_bus_flash_conn *fnode_conn,
+			    void *data, int len);
+static int
+qla4xxx_sysfs_ddb_get_param(struct iscsi_bus_flash_session *fnode_sess,
+			    int param, char *buf);
+static int qla4xxx_sysfs_ddb_add(struct Scsi_Host *shost, const char *buf,
+				 int len);
+static int
+qla4xxx_sysfs_ddb_delete(struct iscsi_bus_flash_session *fnode_sess);
+static int qla4xxx_sysfs_ddb_login(struct iscsi_bus_flash_session *fnode_sess,
+				   struct iscsi_bus_flash_conn *fnode_conn);
+static int qla4xxx_sysfs_ddb_logout(struct iscsi_bus_flash_session *fnode_sess,
+				    struct iscsi_bus_flash_conn *fnode_conn);
+static int qla4xxx_sysfs_ddb_logout_sid(struct iscsi_cls_session *cls_sess);
+
 static struct qla4_8xxx_legacy_intr_set legacy_intr[] =
     QLA82XX_LEGACY_INTR_CONFIG;
 
@@ -232,6 +252,13 @@ static struct iscsi_transport qla4xxx_iscsi_transport = {
 	.send_ping		= qla4xxx_send_ping,
 	.get_chap		= qla4xxx_get_chap_list,
 	.delete_chap		= qla4xxx_delete_chap,
+	.get_flashnode_param	= qla4xxx_sysfs_ddb_get_param,
+	.set_flashnode_param	= qla4xxx_sysfs_ddb_set_param,
+	.new_flashnode		= qla4xxx_sysfs_ddb_add,
+	.del_flashnode		= qla4xxx_sysfs_ddb_delete,
+	.login_flashnode	= qla4xxx_sysfs_ddb_login,
+	.logout_flashnode	= qla4xxx_sysfs_ddb_logout,
+	.logout_flashnode_sid	= qla4xxx_sysfs_ddb_logout_sid,
 };
 
 static struct scsi_transport_template *qla4xxx_scsi_transport;
@@ -376,6 +403,68 @@ static umode_t qla4_attr_is_visible(int param_type, int param)
 		default:
 			return 0;
 		}
+	case ISCSI_FLASHNODE_PARAM:
+		switch (param) {
+		case ISCSI_FLASHNODE_IS_FW_ASSIGNED_IPV6:
+		case ISCSI_FLASHNODE_PORTAL_TYPE:
+		case ISCSI_FLASHNODE_AUTO_SND_TGT_DISABLE:
+		case ISCSI_FLASHNODE_DISCOVERY_SESS:
+		case ISCSI_FLASHNODE_ENTRY_EN:
+		case ISCSI_FLASHNODE_HDR_DGST_EN:
+		case ISCSI_FLASHNODE_DATA_DGST_EN:
+		case ISCSI_FLASHNODE_IMM_DATA_EN:
+		case ISCSI_FLASHNODE_INITIAL_R2T_EN:
+		case ISCSI_FLASHNODE_DATASEQ_INORDER:
+		case ISCSI_FLASHNODE_PDU_INORDER:
+		case ISCSI_FLASHNODE_CHAP_AUTH_EN:
+		case ISCSI_FLASHNODE_SNACK_REQ_EN:
+		case ISCSI_FLASHNODE_DISCOVERY_LOGOUT_EN:
+		case ISCSI_FLASHNODE_BIDI_CHAP_EN:
+		case ISCSI_FLASHNODE_DISCOVERY_AUTH_OPTIONAL:
+		case ISCSI_FLASHNODE_ERL:
+		case ISCSI_FLASHNODE_TCP_TIMESTAMP_STAT:
+		case ISCSI_FLASHNODE_TCP_NAGLE_DISABLE:
+		case ISCSI_FLASHNODE_TCP_WSF_DISABLE:
+		case ISCSI_FLASHNODE_TCP_TIMER_SCALE:
+		case ISCSI_FLASHNODE_TCP_TIMESTAMP_EN:
+		case ISCSI_FLASHNODE_IP_FRAG_DISABLE:
+		case ISCSI_FLASHNODE_MAX_RECV_DLENGTH:
+		case ISCSI_FLASHNODE_MAX_XMIT_DLENGTH:
+		case ISCSI_FLASHNODE_FIRST_BURST:
+		case ISCSI_FLASHNODE_DEF_TIME2WAIT:
+		case ISCSI_FLASHNODE_DEF_TIME2RETAIN:
+		case ISCSI_FLASHNODE_MAX_R2T:
+		case ISCSI_FLASHNODE_KEEPALIVE_TMO:
+		case ISCSI_FLASHNODE_ISID:
+		case ISCSI_FLASHNODE_TSID:
+		case ISCSI_FLASHNODE_PORT:
+		case ISCSI_FLASHNODE_MAX_BURST:
+		case ISCSI_FLASHNODE_DEF_TASKMGMT_TMO:
+		case ISCSI_FLASHNODE_IPADDR:
+		case ISCSI_FLASHNODE_ALIAS:
+		case ISCSI_FLASHNODE_REDIRECT_IPADDR:
+		case ISCSI_FLASHNODE_MAX_SEGMENT_SIZE:
+		case ISCSI_FLASHNODE_LOCAL_PORT:
+		case ISCSI_FLASHNODE_IPV4_TOS:
+		case ISCSI_FLASHNODE_IPV6_TC:
+		case ISCSI_FLASHNODE_IPV6_FLOW_LABEL:
+		case ISCSI_FLASHNODE_NAME:
+		case ISCSI_FLASHNODE_TPGT:
+		case ISCSI_FLASHNODE_LINK_LOCAL_IPV6:
+		case ISCSI_FLASHNODE_DISCOVERY_PARENT_IDX:
+		case ISCSI_FLASHNODE_DISCOVERY_PARENT_TYPE:
+		case ISCSI_FLASHNODE_TCP_XMIT_WSF:
+		case ISCSI_FLASHNODE_TCP_RECV_WSF:
+		case ISCSI_FLASHNODE_CHAP_OUT_IDX:
+		case ISCSI_FLASHNODE_USERNAME:
+		case ISCSI_FLASHNODE_PASSWORD:
+		case ISCSI_FLASHNODE_STATSN:
+		case ISCSI_FLASHNODE_EXP_STATSN:
+		case ISCSI_FLASHNODE_IS_BOOT_TGT:
+			return S_IRUGO;
+		default:
+			return 0;
+		}
 	}
 
 	return 0;
@@ -391,7 +480,7 @@ static int qla4xxx_get_chap_list(struct Scsi_Host *shost, uint16_t chap_tbl_idx,
 	int valid_chap_entries = 0;
 	int ret = 0, i;
 
-	if (is_qla8022(ha))
+	if (is_qla80XX(ha))
 		max_chap_entries = (ha->hw.flt_chap_size / 2) /
 					sizeof(struct ql4_chap_table);
 	else
@@ -495,7 +584,7 @@ static int qla4xxx_delete_chap(struct Scsi_Host *shost, uint16_t chap_tbl_idx)
 
 	memset(chap_table, 0, sizeof(struct ql4_chap_table));
 
-	if (is_qla8022(ha))
+	if (is_qla80XX(ha))
 		max_chap_entries = (ha->hw.flt_chap_size / 2) /
 				   sizeof(struct ql4_chap_table);
 	else
@@ -1922,6 +2011,252 @@ static int qla4xxx_task_xmit(struct iscsi_task *task)
 	return -ENOSYS;
 }
 
+static int qla4xxx_copy_from_fwddb_param(struct iscsi_bus_flash_session *sess,
+					 struct iscsi_bus_flash_conn *conn,
+					 struct dev_db_entry *fw_ddb_entry)
+{
+	unsigned long options = 0;
+	int rc = 0;
+
+	options = le16_to_cpu(fw_ddb_entry->options);
+	conn->is_fw_assigned_ipv6 = test_bit(OPT_IS_FW_ASSIGNED_IPV6, &options);
+	if (test_bit(OPT_IPV6_DEVICE, &options)) {
+		rc = iscsi_switch_str_param(&sess->portal_type,
+					    PORTAL_TYPE_IPV6);
+		if (rc)
+			goto exit_copy;
+	} else {
+		rc = iscsi_switch_str_param(&sess->portal_type,
+					    PORTAL_TYPE_IPV4);
+		if (rc)
+			goto exit_copy;
+	}
+
+	sess->auto_snd_tgt_disable = test_bit(OPT_AUTO_SENDTGTS_DISABLE,
+					      &options);
+	sess->discovery_sess = test_bit(OPT_DISC_SESSION, &options);
+	sess->entry_state = test_bit(OPT_ENTRY_STATE, &options);
+
+	options = le16_to_cpu(fw_ddb_entry->iscsi_options);
+	conn->hdrdgst_en = test_bit(ISCSIOPT_HEADER_DIGEST_EN, &options);
+	conn->datadgst_en = test_bit(ISCSIOPT_DATA_DIGEST_EN, &options);
+	sess->imm_data_en = test_bit(ISCSIOPT_IMMEDIATE_DATA_EN, &options);
+	sess->initial_r2t_en = test_bit(ISCSIOPT_INITIAL_R2T_EN, &options);
+	sess->dataseq_inorder_en = test_bit(ISCSIOPT_DATA_SEQ_IN_ORDER,
+					    &options);
+	sess->pdu_inorder_en = test_bit(ISCSIOPT_DATA_PDU_IN_ORDER, &options);
+	sess->chap_auth_en = test_bit(ISCSIOPT_CHAP_AUTH_EN, &options);
+	conn->snack_req_en = test_bit(ISCSIOPT_SNACK_REQ_EN, &options);
+	sess->discovery_logout_en = test_bit(ISCSIOPT_DISCOVERY_LOGOUT_EN,
+					     &options);
+	sess->bidi_chap_en = test_bit(ISCSIOPT_BIDI_CHAP_EN, &options);
+	sess->discovery_auth_optional =
+			test_bit(ISCSIOPT_DISCOVERY_AUTH_OPTIONAL, &options);
+	if (test_bit(ISCSIOPT_ERL1, &options))
+		sess->erl |= BIT_1;
+	if (test_bit(ISCSIOPT_ERL0, &options))
+		sess->erl |= BIT_0;
+
+	options = le16_to_cpu(fw_ddb_entry->tcp_options);
+	conn->tcp_timestamp_stat = test_bit(TCPOPT_TIMESTAMP_STAT, &options);
+	conn->tcp_nagle_disable = test_bit(TCPOPT_NAGLE_DISABLE, &options);
+	conn->tcp_wsf_disable = test_bit(TCPOPT_WSF_DISABLE, &options);
+	if (test_bit(TCPOPT_TIMER_SCALE3, &options))
+		conn->tcp_timer_scale |= BIT_3;
+	if (test_bit(TCPOPT_TIMER_SCALE2, &options))
+		conn->tcp_timer_scale |= BIT_2;
+	if (test_bit(TCPOPT_TIMER_SCALE1, &options))
+		conn->tcp_timer_scale |= BIT_1;
+
+	conn->tcp_timer_scale >>= 1;
+	conn->tcp_timestamp_en = test_bit(TCPOPT_TIMESTAMP_EN, &options);
+
+	options = le16_to_cpu(fw_ddb_entry->ip_options);
+	conn->fragment_disable = test_bit(IPOPT_FRAGMENT_DISABLE, &options);
+
+	conn->max_recv_dlength = BYTE_UNITS *
+			  le16_to_cpu(fw_ddb_entry->iscsi_max_rcv_data_seg_len);
+	conn->max_xmit_dlength = BYTE_UNITS *
+			  le16_to_cpu(fw_ddb_entry->iscsi_max_snd_data_seg_len);
+	sess->first_burst = BYTE_UNITS *
+			       le16_to_cpu(fw_ddb_entry->iscsi_first_burst_len);
+	sess->max_burst = BYTE_UNITS *
+				 le16_to_cpu(fw_ddb_entry->iscsi_max_burst_len);
+	sess->max_r2t = le16_to_cpu(fw_ddb_entry->iscsi_max_outsnd_r2t);
+	sess->time2wait = le16_to_cpu(fw_ddb_entry->iscsi_def_time2wait);
+	sess->time2retain = le16_to_cpu(fw_ddb_entry->iscsi_def_time2retain);
+	sess->tpgt = le32_to_cpu(fw_ddb_entry->tgt_portal_grp);
+	conn->max_segment_size = le16_to_cpu(fw_ddb_entry->mss);
+	conn->tcp_xmit_wsf = fw_ddb_entry->tcp_xmt_wsf;
+	conn->tcp_recv_wsf = fw_ddb_entry->tcp_rcv_wsf;
+	conn->ipv6_flow_label = le16_to_cpu(fw_ddb_entry->ipv6_flow_lbl);
+	conn->keepalive_timeout = le16_to_cpu(fw_ddb_entry->ka_timeout);
+	conn->local_port = le16_to_cpu(fw_ddb_entry->lcl_port);
+	conn->statsn = le32_to_cpu(fw_ddb_entry->stat_sn);
+	conn->exp_statsn = le32_to_cpu(fw_ddb_entry->exp_stat_sn);
+	sess->discovery_parent_idx = le16_to_cpu(fw_ddb_entry->ddb_link);
+	sess->discovery_parent_type = le16_to_cpu(fw_ddb_entry->ddb_link);
+	sess->chap_out_idx = le16_to_cpu(fw_ddb_entry->chap_tbl_idx);
+	sess->tsid = le16_to_cpu(fw_ddb_entry->tsid);
+
+	sess->default_taskmgmt_timeout =
+				le16_to_cpu(fw_ddb_entry->def_timeout);
+	conn->port = le16_to_cpu(fw_ddb_entry->port);
+
+	options = le16_to_cpu(fw_ddb_entry->options);
+	conn->ipaddress = kzalloc(IPv6_ADDR_LEN, GFP_KERNEL);
+	if (!conn->ipaddress) {
+		rc = -ENOMEM;
+		goto exit_copy;
+	}
+
+	conn->redirect_ipaddr = kzalloc(IPv6_ADDR_LEN, GFP_KERNEL);
+	if (!conn->redirect_ipaddr) {
+		rc = -ENOMEM;
+		goto exit_copy;
+	}
+
+	memcpy(conn->ipaddress, fw_ddb_entry->ip_addr, IPv6_ADDR_LEN);
+	memcpy(conn->redirect_ipaddr, fw_ddb_entry->tgt_addr, IPv6_ADDR_LEN);
+
+	if (test_bit(OPT_IPV6_DEVICE, &options)) {
+		conn->ipv6_traffic_class = fw_ddb_entry->ipv4_tos;
+
+		conn->link_local_ipv6_addr = kzalloc(IPv6_ADDR_LEN, GFP_KERNEL);
+		if (!conn->link_local_ipv6_addr) {
+			rc = -ENOMEM;
+			goto exit_copy;
+		}
+
+		memcpy(conn->link_local_ipv6_addr,
+		       fw_ddb_entry->link_local_ipv6_addr, IPv6_ADDR_LEN);
+	} else {
+		conn->ipv4_tos = fw_ddb_entry->ipv4_tos;
+	}
+
+	if (fw_ddb_entry->iscsi_name[0]) {
+		rc = iscsi_switch_str_param(&sess->targetname,
+					    (char *)fw_ddb_entry->iscsi_name);
+		if (rc)
+			goto exit_copy;
+	}
+
+	if (fw_ddb_entry->iscsi_alias[0]) {
+		rc = iscsi_switch_str_param(&sess->targetalias,
+					    (char *)fw_ddb_entry->iscsi_alias);
+		if (rc)
+			goto exit_copy;
+	}
+
+	COPY_ISID(sess->isid, fw_ddb_entry->isid);
+
+exit_copy:
+	return rc;
+}
+
+static int qla4xxx_copy_to_fwddb_param(struct iscsi_bus_flash_session *sess,
+				       struct iscsi_bus_flash_conn *conn,
+				       struct dev_db_entry *fw_ddb_entry)
+{
+	uint16_t options;
+	int rc = 0;
+
+	options = le16_to_cpu(fw_ddb_entry->options);
+	SET_BITVAL(conn->is_fw_assigned_ipv6,  options, BIT_11);
+	if (!strncmp(sess->portal_type, PORTAL_TYPE_IPV6, 4))
+		options |= BIT_8;
+	else
+		options &= ~BIT_8;
+
+	SET_BITVAL(sess->auto_snd_tgt_disable, options, BIT_6);
+	SET_BITVAL(sess->discovery_sess, options, BIT_4);
+	SET_BITVAL(sess->entry_state, options, BIT_3);
+	fw_ddb_entry->options = cpu_to_le16(options);
+
+	options = le16_to_cpu(fw_ddb_entry->iscsi_options);
+	SET_BITVAL(conn->hdrdgst_en, options, BIT_13);
+	SET_BITVAL(conn->datadgst_en, options, BIT_12);
+	SET_BITVAL(sess->imm_data_en, options, BIT_11);
+	SET_BITVAL(sess->initial_r2t_en, options, BIT_10);
+	SET_BITVAL(sess->dataseq_inorder_en, options, BIT_9);
+	SET_BITVAL(sess->pdu_inorder_en, options, BIT_8);
+	SET_BITVAL(sess->chap_auth_en, options, BIT_7);
+	SET_BITVAL(conn->snack_req_en, options, BIT_6);
+	SET_BITVAL(sess->discovery_logout_en, options, BIT_5);
+	SET_BITVAL(sess->bidi_chap_en, options, BIT_4);
+	SET_BITVAL(sess->discovery_auth_optional, options, BIT_3);
+	SET_BITVAL(sess->erl & BIT_1, options, BIT_1);
+	SET_BITVAL(sess->erl & BIT_0, options, BIT_0);
+	fw_ddb_entry->iscsi_options = cpu_to_le16(options);
+
+	options = le16_to_cpu(fw_ddb_entry->tcp_options);
+	SET_BITVAL(conn->tcp_timestamp_stat, options, BIT_6);
+	SET_BITVAL(conn->tcp_nagle_disable, options, BIT_5);
+	SET_BITVAL(conn->tcp_wsf_disable, options, BIT_4);
+	SET_BITVAL(conn->tcp_timer_scale & BIT_2, options, BIT_3);
+	SET_BITVAL(conn->tcp_timer_scale & BIT_1, options, BIT_2);
+	SET_BITVAL(conn->tcp_timer_scale & BIT_0, options, BIT_1);
+	SET_BITVAL(conn->tcp_timestamp_en, options, BIT_0);
+	fw_ddb_entry->tcp_options = cpu_to_le16(options);
+
+	options = le16_to_cpu(fw_ddb_entry->ip_options);
+	SET_BITVAL(conn->fragment_disable, options, BIT_4);
+	fw_ddb_entry->ip_options = cpu_to_le16(options);
+
+	fw_ddb_entry->iscsi_max_outsnd_r2t = cpu_to_le16(sess->max_r2t);
+	fw_ddb_entry->iscsi_max_rcv_data_seg_len =
+			       cpu_to_le16(conn->max_recv_dlength / BYTE_UNITS);
+	fw_ddb_entry->iscsi_max_snd_data_seg_len =
+			       cpu_to_le16(conn->max_xmit_dlength / BYTE_UNITS);
+	fw_ddb_entry->iscsi_first_burst_len =
+				cpu_to_le16(sess->first_burst / BYTE_UNITS);
+	fw_ddb_entry->iscsi_max_burst_len = cpu_to_le16(sess->max_burst /
+					    BYTE_UNITS);
+	fw_ddb_entry->iscsi_def_time2wait = cpu_to_le16(sess->time2wait);
+	fw_ddb_entry->iscsi_def_time2retain = cpu_to_le16(sess->time2retain);
+	fw_ddb_entry->tgt_portal_grp = cpu_to_le16(sess->tpgt);
+	fw_ddb_entry->mss = cpu_to_le16(conn->max_segment_size);
+	fw_ddb_entry->tcp_xmt_wsf = cpu_to_le16(conn->tcp_xmit_wsf);
+	fw_ddb_entry->tcp_rcv_wsf = cpu_to_le16(conn->tcp_recv_wsf);
+	fw_ddb_entry->ipv4_tos = conn->ipv4_tos;
+	fw_ddb_entry->ipv6_flow_lbl = cpu_to_le16(conn->ipv6_flow_label);
+	fw_ddb_entry->ka_timeout = cpu_to_le16(conn->keepalive_timeout);
+	fw_ddb_entry->lcl_port = cpu_to_le16(conn->local_port);
+	fw_ddb_entry->stat_sn = cpu_to_le16(conn->statsn);
+	fw_ddb_entry->exp_stat_sn = cpu_to_le16(conn->exp_statsn);
+	fw_ddb_entry->ddb_link = cpu_to_le16(sess->discovery_parent_type);
+	fw_ddb_entry->chap_tbl_idx = cpu_to_le16(sess->chap_out_idx);
+	fw_ddb_entry->tsid = cpu_to_le16(sess->tsid);
+	fw_ddb_entry->port = cpu_to_le16(conn->port);
+	fw_ddb_entry->def_timeout =
+				cpu_to_le16(sess->default_taskmgmt_timeout);
+
+	if (conn->ipaddress)
+		memcpy(fw_ddb_entry->ip_addr, conn->ipaddress,
+		       sizeof(fw_ddb_entry->ip_addr));
+
+	if (conn->redirect_ipaddr)
+		memcpy(fw_ddb_entry->tgt_addr, conn->redirect_ipaddr,
+		       sizeof(fw_ddb_entry->tgt_addr));
+
+	if (conn->link_local_ipv6_addr)
+		memcpy(fw_ddb_entry->link_local_ipv6_addr,
+		       conn->link_local_ipv6_addr,
+		       sizeof(fw_ddb_entry->link_local_ipv6_addr));
+
+	if (sess->targetname)
+		memcpy(fw_ddb_entry->iscsi_name, sess->targetname,
+		       sizeof(fw_ddb_entry->iscsi_name));
+
+	if (sess->targetalias)
+		memcpy(fw_ddb_entry->iscsi_alias, sess->targetalias,
+		       sizeof(fw_ddb_entry->iscsi_alias));
+
+	COPY_ISID(fw_ddb_entry->isid, sess->isid);
+
+	return rc;
+}
+
 static void qla4xxx_copy_fwddb_param(struct scsi_qla_host *ha,
 				     struct dev_db_entry *fw_ddb_entry,
 				     struct iscsi_cls_session *cls_sess,
@@ -2543,6 +2878,7 @@ static void qla4_8xxx_process_fw_error(struct scsi_qla_host *ha)
 void qla4_8xxx_watchdog(struct scsi_qla_host *ha)
 {
 	uint32_t dev_state;
+	uint32_t idc_ctrl;
 
 	/* don't poll if reset is going on */
 	if (!(test_bit(DPC_RESET_ACTIVE, &ha->dpc_flags) ||
@@ -2561,10 +2897,23 @@ void qla4_8xxx_watchdog(struct scsi_qla_host *ha)
 			qla4xxx_wake_dpc(ha);
 		} else if (dev_state == QLA8XXX_DEV_NEED_RESET &&
 			   !test_bit(DPC_RESET_HA, &ha->dpc_flags)) {
+
+			ql4_printk(KERN_INFO, ha, "%s: HW State: NEED RESET!\n",
+				   __func__);
+
+			if (is_qla8032(ha)) {
+				idc_ctrl = qla4_83xx_rd_reg(ha,
+							QLA83XX_IDC_DRV_CTRL);
+				if (!(idc_ctrl & GRACEFUL_RESET_BIT1)) {
+					ql4_printk(KERN_INFO, ha, "%s: Graceful reset bit is not set\n",
+						   __func__);
+					qla4xxx_mailbox_premature_completion(
+									    ha);
+				}
+			}
+
 			if (is_qla8032(ha) ||
 			    (is_qla8022(ha) && !ql4xdontresethba)) {
-				ql4_printk(KERN_INFO, ha, "%s: HW State: "
-				    "NEED RESET!\n", __func__);
 				set_bit(DPC_RESET_HA, &ha->dpc_flags);
 				qla4xxx_wake_dpc(ha);
 			}
@@ -3737,8 +4086,8 @@ static struct isp_operations qla4_83xx_isp_ops = {
 	.reset_firmware		= qla4_8xxx_stop_firmware,
 	.queue_iocb		= qla4_83xx_queue_iocb,
 	.complete_iocb		= qla4_83xx_complete_iocb,
-	.rd_shdw_req_q_out	= qla4_83xx_rd_shdw_req_q_out,
-	.rd_shdw_rsp_q_in	= qla4_83xx_rd_shdw_rsp_q_in,
+	.rd_shdw_req_q_out	= qla4xxx_rd_shdw_req_q_out,
+	.rd_shdw_rsp_q_in	= qla4xxx_rd_shdw_rsp_q_in,
 	.get_sys_info		= qla4_8xxx_get_sys_info,
 	.rd_reg_direct		= qla4_83xx_rd_reg,
 	.wr_reg_direct		= qla4_83xx_wr_reg,
@@ -3761,11 +4110,6 @@ uint16_t qla4_82xx_rd_shdw_req_q_out(struct scsi_qla_host *ha)
 	return (uint16_t)le32_to_cpu(readl(&ha->qla4_82xx_reg->req_q_out));
 }
 
-uint16_t qla4_83xx_rd_shdw_req_q_out(struct scsi_qla_host *ha)
-{
-	return (uint16_t)le32_to_cpu(readl(&ha->qla4_83xx_reg->req_q_out));
-}
-
 uint16_t qla4xxx_rd_shdw_rsp_q_in(struct scsi_qla_host *ha)
 {
 	return (uint16_t)le32_to_cpu(ha->shadow_regs->rsp_q_in);
@@ -3776,11 +4120,6 @@ uint16_t qla4_82xx_rd_shdw_rsp_q_in(struct scsi_qla_host *ha)
 	return (uint16_t)le32_to_cpu(readl(&ha->qla4_82xx_reg->rsp_q_in));
 }
 
-uint16_t qla4_83xx_rd_shdw_rsp_q_in(struct scsi_qla_host *ha)
-{
-	return (uint16_t)le32_to_cpu(readl(&ha->qla4_83xx_reg->rsp_q_in));
-}
-
 static ssize_t qla4xxx_show_boot_eth_info(void *data, int type, char *buf)
 {
 	struct scsi_qla_host *ha = data;
@@ -4005,7 +4344,7 @@ static int get_fw_boot_info(struct scsi_qla_host *ha, uint16_t ddb_index[])
 		if (val & BIT_7)
 			ddb_index[1] = (val & 0x7f);
 
-	} else if (is_qla8022(ha)) {
+	} else if (is_qla80XX(ha)) {
 		buf = dma_alloc_coherent(&ha->pdev->dev, size,
 					 &buf_dma, GFP_KERNEL);
 		if (!buf) {
@@ -4083,7 +4422,7 @@ static int qla4xxx_get_bidi_chap(struct scsi_qla_host *ha, char *username,
 	int max_chap_entries = 0;
 	struct ql4_chap_table *chap_table;
 
-	if (is_qla8022(ha))
+	if (is_qla80XX(ha))
 		max_chap_entries = (ha->hw.flt_chap_size / 2) /
 						sizeof(struct ql4_chap_table);
 	else
@@ -5058,6 +5397,1342 @@ exit_nt_list:
 		dma_pool_free(ha->fw_ddb_dma_pool, fw_ddb_entry, fw_ddb_dma);
 }
 
+static void qla4xxx_build_new_nt_list(struct scsi_qla_host *ha,
+				      struct list_head *list_nt)
+{
+	struct dev_db_entry *fw_ddb_entry;
+	dma_addr_t fw_ddb_dma;
+	int max_ddbs;
+	int fw_idx_size;
+	int ret;
+	uint32_t idx = 0, next_idx = 0;
+	uint32_t state = 0, conn_err = 0;
+	uint16_t conn_id = 0;
+	struct qla_ddb_index  *nt_ddb_idx;
+
+	fw_ddb_entry = dma_pool_alloc(ha->fw_ddb_dma_pool, GFP_KERNEL,
+				      &fw_ddb_dma);
+	if (fw_ddb_entry == NULL) {
+		DEBUG2(ql4_printk(KERN_ERR, ha, "Out of memory\n"));
+		goto exit_new_nt_list;
+	}
+	max_ddbs =  is_qla40XX(ha) ? MAX_DEV_DB_ENTRIES_40XX :
+				     MAX_DEV_DB_ENTRIES;
+	fw_idx_size = sizeof(struct qla_ddb_index);
+
+	for (idx = 0; idx < max_ddbs; idx = next_idx) {
+		ret = qla4xxx_get_fwddb_entry(ha, idx, fw_ddb_entry, fw_ddb_dma,
+					      NULL, &next_idx, &state,
+					      &conn_err, NULL, &conn_id);
+		if (ret == QLA_ERROR)
+			break;
+
+		/* Check if NT, then add it to list */
+		if (strlen((char *)fw_ddb_entry->iscsi_name) == 0)
+			goto continue_next_new_nt;
+
+		if (!(state == DDB_DS_NO_CONNECTION_ACTIVE))
+			goto continue_next_new_nt;
+
+		DEBUG2(ql4_printk(KERN_INFO, ha,
+				  "Adding  DDB to session = 0x%x\n", idx));
+
+		nt_ddb_idx = vmalloc(fw_idx_size);
+		if (!nt_ddb_idx)
+			break;
+
+		nt_ddb_idx->fw_ddb_idx = idx;
+
+		ret = qla4xxx_is_session_exists(ha, fw_ddb_entry);
+		if (ret == QLA_SUCCESS) {
+			/* free nt_ddb_idx and do not add to list_nt */
+			vfree(nt_ddb_idx);
+			goto continue_next_new_nt;
+		}
+
+		list_add_tail(&nt_ddb_idx->list, list_nt);
+
+		ret = qla4xxx_sess_conn_setup(ha, fw_ddb_entry, RESET_ADAPTER,
+					      idx);
+		if (ret == QLA_ERROR)
+			goto exit_new_nt_list;
+
+continue_next_new_nt:
+		if (next_idx == 0)
+			break;
+	}
+
+exit_new_nt_list:
+	if (fw_ddb_entry)
+		dma_pool_free(ha->fw_ddb_dma_pool, fw_ddb_entry, fw_ddb_dma);
+}
+
+/**
+ * qla4xxx_sysfs_ddb_is_non_persistent - check for non-persistence of ddb entry
+ * @dev: dev associated with the sysfs entry
+ * @data: pointer to flashnode session object
+ *
+ * Returns:
+ *	1: if flashnode entry is non-persistent
+ *	0: if flashnode entry is persistent
+ **/
+static int qla4xxx_sysfs_ddb_is_non_persistent(struct device *dev, void *data)
+{
+	struct iscsi_bus_flash_session *fnode_sess;
+
+	if (!iscsi_flashnode_bus_match(dev, NULL))
+		return 0;
+
+	fnode_sess = iscsi_dev_to_flash_session(dev);
+
+	return (fnode_sess->flash_state == DEV_DB_NON_PERSISTENT);
+}
+
+/**
+ * qla4xxx_sysfs_ddb_tgt_create - Create sysfs entry for target
+ * @ha: pointer to host
+ * @fw_ddb_entry: flash ddb data
+ * @idx: target index
+ * @user: if set then this call is made from userland else from kernel
+ *
+ * Returns:
+ * On sucess: QLA_SUCCESS
+ * On failure: QLA_ERROR
+ *
+ * This create separate sysfs entries for session and connection attributes of
+ * the given fw ddb entry.
+ * If this is invoked as a result of a userspace call then the entry is marked
+ * as nonpersistent using flash_state field.
+ **/
+int qla4xxx_sysfs_ddb_tgt_create(struct scsi_qla_host *ha,
+				 struct dev_db_entry *fw_ddb_entry,
+				 uint16_t *idx, int user)
+{
+	struct iscsi_bus_flash_session *fnode_sess = NULL;
+	struct iscsi_bus_flash_conn *fnode_conn = NULL;
+	int rc = QLA_ERROR;
+
+	fnode_sess = iscsi_create_flashnode_sess(ha->host, *idx,
+						 &qla4xxx_iscsi_transport, 0);
+	if (!fnode_sess) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Unable to create session sysfs entry for flashnode %d of host%lu\n",
+			   __func__, *idx, ha->host_no);
+		goto exit_tgt_create;
+	}
+
+	fnode_conn = iscsi_create_flashnode_conn(ha->host, fnode_sess,
+						 &qla4xxx_iscsi_transport, 0);
+	if (!fnode_conn) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Unable to create conn sysfs entry for flashnode %d of host%lu\n",
+			   __func__, *idx, ha->host_no);
+		goto free_sess;
+	}
+
+	if (user) {
+		fnode_sess->flash_state = DEV_DB_NON_PERSISTENT;
+	} else {
+		fnode_sess->flash_state = DEV_DB_PERSISTENT;
+
+		if (*idx == ha->pri_ddb_idx || *idx == ha->sec_ddb_idx)
+			fnode_sess->is_boot_target = 1;
+		else
+			fnode_sess->is_boot_target = 0;
+	}
+
+	rc = qla4xxx_copy_from_fwddb_param(fnode_sess, fnode_conn,
+					   fw_ddb_entry);
+
+	ql4_printk(KERN_INFO, ha, "%s: sysfs entry %s created\n",
+		   __func__, fnode_sess->dev.kobj.name);
+
+	ql4_printk(KERN_INFO, ha, "%s: sysfs entry %s created\n",
+		   __func__, fnode_conn->dev.kobj.name);
+
+	return QLA_SUCCESS;
+
+free_sess:
+	iscsi_destroy_flashnode_sess(fnode_sess);
+
+exit_tgt_create:
+	return QLA_ERROR;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_add - Add new ddb entry in flash
+ * @shost: pointer to host
+ * @buf: type of ddb entry (ipv4/ipv6)
+ * @len: length of buf
+ *
+ * This creates new ddb entry in the flash by finding first free index and
+ * storing default ddb there. And then create sysfs entry for the new ddb entry.
+ **/
+static int qla4xxx_sysfs_ddb_add(struct Scsi_Host *shost, const char *buf,
+				 int len)
+{
+	struct scsi_qla_host *ha = to_qla_host(shost);
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	dma_addr_t fw_ddb_entry_dma;
+	struct device *dev;
+	uint16_t idx = 0;
+	uint16_t max_ddbs = 0;
+	uint32_t options = 0;
+	uint32_t rval = QLA_ERROR;
+
+	if (strncasecmp(PORTAL_TYPE_IPV4, buf, 4) &&
+	    strncasecmp(PORTAL_TYPE_IPV6, buf, 4)) {
+		DEBUG2(ql4_printk(KERN_ERR, ha, "%s: Invalid portal type\n",
+				  __func__));
+		goto exit_ddb_add;
+	}
+
+	max_ddbs =  is_qla40XX(ha) ? MAX_PRST_DEV_DB_ENTRIES :
+				     MAX_DEV_DB_ENTRIES;
+
+	fw_ddb_entry = dma_alloc_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+					  &fw_ddb_entry_dma, GFP_KERNEL);
+	if (!fw_ddb_entry) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to allocate dma buffer\n",
+				  __func__));
+		goto exit_ddb_add;
+	}
+
+	dev = iscsi_find_flashnode_sess(ha->host, NULL,
+					qla4xxx_sysfs_ddb_is_non_persistent);
+	if (dev) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s: A non-persistent entry %s found\n",
+			   __func__, dev->kobj.name);
+		goto exit_ddb_add;
+	}
+
+	for (idx = 0; idx < max_ddbs; idx++) {
+		if (qla4xxx_flashdb_by_index(ha, fw_ddb_entry,
+					     fw_ddb_entry_dma, idx))
+			break;
+	}
+
+	if (idx == max_ddbs)
+		goto exit_ddb_add;
+
+	if (!strncasecmp("ipv6", buf, 4))
+		options |= IPV6_DEFAULT_DDB_ENTRY;
+
+	rval = qla4xxx_get_default_ddb(ha, options, fw_ddb_entry_dma);
+	if (rval == QLA_ERROR)
+		goto exit_ddb_add;
+
+	rval = qla4xxx_sysfs_ddb_tgt_create(ha, fw_ddb_entry, &idx, 1);
+
+exit_ddb_add:
+	if (fw_ddb_entry)
+		dma_free_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+				  fw_ddb_entry, fw_ddb_entry_dma);
+	if (rval == QLA_SUCCESS)
+		return idx;
+	else
+		return -EIO;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_apply - write the target ddb contents to Flash
+ * @fnode_sess: pointer to session attrs of flash ddb entry
+ * @fnode_conn: pointer to connection attrs of flash ddb entry
+ *
+ * This writes the contents of target ddb buffer to Flash with a valid cookie
+ * value in order to make the ddb entry persistent.
+ **/
+static int  qla4xxx_sysfs_ddb_apply(struct iscsi_bus_flash_session *fnode_sess,
+				    struct iscsi_bus_flash_conn *fnode_conn)
+{
+	struct Scsi_Host *shost = iscsi_flash_session_to_shost(fnode_sess);
+	struct scsi_qla_host *ha = to_qla_host(shost);
+	uint32_t dev_db_start_offset = FLASH_OFFSET_DB_INFO;
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	dma_addr_t fw_ddb_entry_dma;
+	uint32_t options = 0;
+	int rval = 0;
+
+	fw_ddb_entry = dma_alloc_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+					  &fw_ddb_entry_dma, GFP_KERNEL);
+	if (!fw_ddb_entry) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to allocate dma buffer\n",
+				  __func__));
+		rval = -ENOMEM;
+		goto exit_ddb_apply;
+	}
+
+	if (!strncasecmp(fnode_sess->portal_type, PORTAL_TYPE_IPV6, 4))
+		options |= IPV6_DEFAULT_DDB_ENTRY;
+
+	rval = qla4xxx_get_default_ddb(ha, options, fw_ddb_entry_dma);
+	if (rval == QLA_ERROR)
+		goto exit_ddb_apply;
+
+	dev_db_start_offset += (fnode_sess->target_id *
+				sizeof(*fw_ddb_entry));
+
+	qla4xxx_copy_to_fwddb_param(fnode_sess, fnode_conn, fw_ddb_entry);
+	fw_ddb_entry->cookie = DDB_VALID_COOKIE;
+
+	rval = qla4xxx_set_flash(ha, fw_ddb_entry_dma, dev_db_start_offset,
+				 sizeof(*fw_ddb_entry), FLASH_OPT_RMW_COMMIT);
+
+	if (rval == QLA_SUCCESS) {
+		fnode_sess->flash_state = DEV_DB_PERSISTENT;
+		ql4_printk(KERN_INFO, ha,
+			   "%s: flash node %u of host %lu written to flash\n",
+			   __func__, fnode_sess->target_id, ha->host_no);
+	} else {
+		rval = -EIO;
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Error while writing flash node %u of host %lu to flash\n",
+			   __func__, fnode_sess->target_id, ha->host_no);
+	}
+
+exit_ddb_apply:
+	if (fw_ddb_entry)
+		dma_free_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+				  fw_ddb_entry, fw_ddb_entry_dma);
+	return rval;
+}
+
+static ssize_t qla4xxx_sysfs_ddb_conn_open(struct scsi_qla_host *ha,
+					   struct dev_db_entry *fw_ddb_entry,
+					   uint16_t idx)
+{
+	struct dev_db_entry *ddb_entry = NULL;
+	dma_addr_t ddb_entry_dma;
+	unsigned long wtime;
+	uint32_t mbx_sts = 0;
+	uint32_t state = 0, conn_err = 0;
+	uint16_t tmo = 0;
+	int ret = 0;
+
+	ddb_entry = dma_alloc_coherent(&ha->pdev->dev, sizeof(*ddb_entry),
+				       &ddb_entry_dma, GFP_KERNEL);
+	if (!ddb_entry) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to allocate dma buffer\n",
+				  __func__));
+		return QLA_ERROR;
+	}
+
+	memcpy(ddb_entry, fw_ddb_entry, sizeof(*ddb_entry));
+
+	ret = qla4xxx_set_ddb_entry(ha, idx, ddb_entry_dma, &mbx_sts);
+	if (ret != QLA_SUCCESS) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to set ddb entry for index %d\n",
+				  __func__, idx));
+		goto exit_ddb_conn_open;
+	}
+
+	qla4xxx_conn_open(ha, idx);
+
+	/* To ensure that sendtargets is done, wait for at least 12 secs */
+	tmo = ((ha->def_timeout > LOGIN_TOV) &&
+	       (ha->def_timeout < LOGIN_TOV * 10) ?
+	       ha->def_timeout : LOGIN_TOV);
+
+	DEBUG2(ql4_printk(KERN_INFO, ha,
+			  "Default time to wait for login to ddb %d\n", tmo));
+
+	wtime = jiffies + (HZ * tmo);
+	do {
+		ret = qla4xxx_get_fwddb_entry(ha, idx, NULL, 0, NULL,
+					      NULL, &state, &conn_err, NULL,
+					      NULL);
+		if (ret == QLA_ERROR)
+			continue;
+
+		if (state == DDB_DS_NO_CONNECTION_ACTIVE ||
+		    state == DDB_DS_SESSION_FAILED)
+			break;
+
+		schedule_timeout_uninterruptible(HZ / 10);
+	} while (time_after(wtime, jiffies));
+
+exit_ddb_conn_open:
+	if (ddb_entry)
+		dma_free_coherent(&ha->pdev->dev, sizeof(*ddb_entry),
+				  ddb_entry, ddb_entry_dma);
+	return ret;
+}
+
+static int qla4xxx_ddb_login_st(struct scsi_qla_host *ha,
+				struct dev_db_entry *fw_ddb_entry)
+{
+	struct qla_ddb_index *ddb_idx, *ddb_idx_tmp;
+	struct list_head list_nt;
+	uint16_t ddb_index;
+	int ret = 0;
+
+	if (test_bit(AF_ST_DISCOVERY_IN_PROGRESS, &ha->flags)) {
+		ql4_printk(KERN_WARNING, ha,
+			   "%s: A discovery already in progress!\n", __func__);
+		return QLA_ERROR;
+	}
+
+	INIT_LIST_HEAD(&list_nt);
+
+	set_bit(AF_ST_DISCOVERY_IN_PROGRESS, &ha->flags);
+
+	ret = qla4xxx_get_ddb_index(ha, &ddb_index);
+	if (ret == QLA_ERROR)
+		goto exit_login_st_clr_bit;
+
+	ret = qla4xxx_sysfs_ddb_conn_open(ha, fw_ddb_entry, ddb_index);
+	if (ret == QLA_ERROR)
+		goto exit_login_st;
+
+	qla4xxx_build_new_nt_list(ha, &list_nt);
+
+	list_for_each_entry_safe(ddb_idx, ddb_idx_tmp, &list_nt, list) {
+		list_del_init(&ddb_idx->list);
+		qla4xxx_clear_ddb_entry(ha, ddb_idx->fw_ddb_idx);
+		vfree(ddb_idx);
+	}
+
+exit_login_st:
+	if (qla4xxx_clear_ddb_entry(ha, ddb_index) == QLA_ERROR) {
+		ql4_printk(KERN_ERR, ha,
+			   "Unable to clear DDB index = 0x%x\n", ddb_index);
+	}
+
+	clear_bit(ddb_index, ha->ddb_idx_map);
+
+exit_login_st_clr_bit:
+	clear_bit(AF_ST_DISCOVERY_IN_PROGRESS, &ha->flags);
+	return ret;
+}
+
+static int qla4xxx_ddb_login_nt(struct scsi_qla_host *ha,
+				struct dev_db_entry *fw_ddb_entry,
+				uint16_t idx)
+{
+	int ret = QLA_ERROR;
+
+	ret = qla4xxx_is_session_exists(ha, fw_ddb_entry);
+	if (ret != QLA_SUCCESS)
+		ret = qla4xxx_sess_conn_setup(ha, fw_ddb_entry, RESET_ADAPTER,
+					      idx);
+	else
+		ret = -EPERM;
+
+	return ret;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_login - Login to the specified target
+ * @fnode_sess: pointer to session attrs of flash ddb entry
+ * @fnode_conn: pointer to connection attrs of flash ddb entry
+ *
+ * This logs in to the specified target
+ **/
+static int qla4xxx_sysfs_ddb_login(struct iscsi_bus_flash_session *fnode_sess,
+				   struct iscsi_bus_flash_conn *fnode_conn)
+{
+	struct Scsi_Host *shost = iscsi_flash_session_to_shost(fnode_sess);
+	struct scsi_qla_host *ha = to_qla_host(shost);
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	dma_addr_t fw_ddb_entry_dma;
+	uint32_t options = 0;
+	int ret = 0;
+
+	if (fnode_sess->flash_state == DEV_DB_NON_PERSISTENT) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Target info is not persistent\n", __func__);
+		ret = -EIO;
+		goto exit_ddb_login;
+	}
+
+	fw_ddb_entry = dma_alloc_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+					  &fw_ddb_entry_dma, GFP_KERNEL);
+	if (!fw_ddb_entry) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to allocate dma buffer\n",
+				  __func__));
+		ret = -ENOMEM;
+		goto exit_ddb_login;
+	}
+
+	if (!strncasecmp(fnode_sess->portal_type, PORTAL_TYPE_IPV6, 4))
+		options |= IPV6_DEFAULT_DDB_ENTRY;
+
+	ret = qla4xxx_get_default_ddb(ha, options, fw_ddb_entry_dma);
+	if (ret == QLA_ERROR)
+		goto exit_ddb_login;
+
+	qla4xxx_copy_to_fwddb_param(fnode_sess, fnode_conn, fw_ddb_entry);
+	fw_ddb_entry->cookie = DDB_VALID_COOKIE;
+
+	if (strlen((char *)fw_ddb_entry->iscsi_name) == 0)
+		ret = qla4xxx_ddb_login_st(ha, fw_ddb_entry);
+	else
+		ret = qla4xxx_ddb_login_nt(ha, fw_ddb_entry,
+					   fnode_sess->target_id);
+
+	if (ret > 0)
+		ret = -EIO;
+
+exit_ddb_login:
+	if (fw_ddb_entry)
+		dma_free_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+				  fw_ddb_entry, fw_ddb_entry_dma);
+	return ret;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_logout_sid - Logout session for the specified target
+ * @cls_sess: pointer to session to be logged out
+ *
+ * This performs session log out from the specified target
+ **/
+static int qla4xxx_sysfs_ddb_logout_sid(struct iscsi_cls_session *cls_sess)
+{
+	struct iscsi_session *sess;
+	struct ddb_entry *ddb_entry = NULL;
+	struct scsi_qla_host *ha;
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	dma_addr_t fw_ddb_entry_dma;
+	unsigned long flags;
+	unsigned long wtime;
+	uint32_t ddb_state;
+	int options;
+	int ret = 0;
+
+	sess = cls_sess->dd_data;
+	ddb_entry = sess->dd_data;
+	ha = ddb_entry->ha;
+
+	if (ddb_entry->ddb_type != FLASH_DDB) {
+		ql4_printk(KERN_ERR, ha, "%s: Not a flash node session\n",
+			   __func__);
+		ret = -ENXIO;
+		goto exit_ddb_logout;
+	}
+
+	if (test_bit(DF_BOOT_TGT, &ddb_entry->flags)) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Logout from boot target entry is not permitted.\n",
+			   __func__);
+		ret = -EPERM;
+		goto exit_ddb_logout;
+	}
+
+	options = LOGOUT_OPTION_CLOSE_SESSION;
+	if (qla4xxx_session_logout_ddb(ha, ddb_entry, options) == QLA_ERROR) {
+		ql4_printk(KERN_ERR, ha, "%s: Logout failed\n", __func__);
+		ret = -EIO;
+		goto exit_ddb_logout;
+	}
+
+	fw_ddb_entry = dma_alloc_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+					  &fw_ddb_entry_dma, GFP_KERNEL);
+	if (!fw_ddb_entry) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Unable to allocate dma buffer\n", __func__);
+		ret = -ENOMEM;
+		goto exit_ddb_logout;
+	}
+
+	wtime = jiffies + (HZ * LOGOUT_TOV);
+	do {
+		ret = qla4xxx_get_fwddb_entry(ha, ddb_entry->fw_ddb_index,
+					      fw_ddb_entry, fw_ddb_entry_dma,
+					      NULL, NULL, &ddb_state, NULL,
+					      NULL, NULL);
+		if (ret == QLA_ERROR)
+			goto ddb_logout_clr_sess;
+
+		if ((ddb_state == DDB_DS_NO_CONNECTION_ACTIVE) ||
+		    (ddb_state == DDB_DS_SESSION_FAILED))
+			goto ddb_logout_clr_sess;
+
+		schedule_timeout_uninterruptible(HZ);
+	} while ((time_after(wtime, jiffies)));
+
+ddb_logout_clr_sess:
+	qla4xxx_clear_ddb_entry(ha, ddb_entry->fw_ddb_index);
+	/*
+	 * we have decremented the reference count of the driver
+	 * when we setup the session to have the driver unload
+	 * to be seamless without actually destroying the
+	 * session
+	 **/
+	try_module_get(qla4xxx_iscsi_transport.owner);
+	iscsi_destroy_endpoint(ddb_entry->conn->ep);
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+	qla4xxx_free_ddb(ha, ddb_entry);
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
+	iscsi_session_teardown(ddb_entry->sess);
+
+	ret = QLA_SUCCESS;
+
+exit_ddb_logout:
+	if (fw_ddb_entry)
+		dma_free_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry),
+				  fw_ddb_entry, fw_ddb_entry_dma);
+	return ret;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_logout - Logout from the specified target
+ * @fnode_sess: pointer to session attrs of flash ddb entry
+ * @fnode_conn: pointer to connection attrs of flash ddb entry
+ *
+ * This performs log out from the specified target
+ **/
+static int qla4xxx_sysfs_ddb_logout(struct iscsi_bus_flash_session *fnode_sess,
+				    struct iscsi_bus_flash_conn *fnode_conn)
+{
+	struct Scsi_Host *shost = iscsi_flash_session_to_shost(fnode_sess);
+	struct scsi_qla_host *ha = to_qla_host(shost);
+	struct ql4_tuple_ddb *flash_tddb = NULL;
+	struct ql4_tuple_ddb *tmp_tddb = NULL;
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	struct ddb_entry *ddb_entry = NULL;
+	dma_addr_t fw_ddb_dma;
+	uint32_t next_idx = 0;
+	uint32_t state = 0, conn_err = 0;
+	uint16_t conn_id = 0;
+	int idx, index;
+	int status, ret = 0;
+
+	fw_ddb_entry = dma_pool_alloc(ha->fw_ddb_dma_pool, GFP_KERNEL,
+				      &fw_ddb_dma);
+	if (fw_ddb_entry == NULL) {
+		ql4_printk(KERN_ERR, ha, "%s:Out of memory\n", __func__);
+		ret = -ENOMEM;
+		goto exit_ddb_logout;
+	}
+
+	flash_tddb = vzalloc(sizeof(*flash_tddb));
+	if (!flash_tddb) {
+		ql4_printk(KERN_WARNING, ha,
+			   "%s:Memory Allocation failed.\n", __func__);
+		ret = -ENOMEM;
+		goto exit_ddb_logout;
+	}
+
+	tmp_tddb = vzalloc(sizeof(*tmp_tddb));
+	if (!tmp_tddb) {
+		ql4_printk(KERN_WARNING, ha,
+			   "%s:Memory Allocation failed.\n", __func__);
+		ret = -ENOMEM;
+		goto exit_ddb_logout;
+	}
+
+	if (!fnode_sess->targetname) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s:Cannot logout from SendTarget entry\n",
+			   __func__);
+		ret = -EPERM;
+		goto exit_ddb_logout;
+	}
+
+	if (fnode_sess->is_boot_target) {
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Logout from boot target entry is not permitted.\n",
+			   __func__);
+		ret = -EPERM;
+		goto exit_ddb_logout;
+	}
+
+	strncpy(flash_tddb->iscsi_name, fnode_sess->targetname,
+		ISCSI_NAME_SIZE);
+
+	if (!strncmp(fnode_sess->portal_type, PORTAL_TYPE_IPV6, 4))
+		sprintf(flash_tddb->ip_addr, "%pI6", fnode_conn->ipaddress);
+	else
+		sprintf(flash_tddb->ip_addr, "%pI4", fnode_conn->ipaddress);
+
+	flash_tddb->tpgt = fnode_sess->tpgt;
+	flash_tddb->port = fnode_conn->port;
+
+	COPY_ISID(flash_tddb->isid, fnode_sess->isid);
+
+	for (idx = 0; idx < MAX_DDB_ENTRIES; idx++) {
+		ddb_entry = qla4xxx_lookup_ddb_by_fw_index(ha, idx);
+		if (ddb_entry == NULL)
+			continue;
+
+		if (ddb_entry->ddb_type != FLASH_DDB)
+			continue;
+
+		index = ddb_entry->sess->target_id;
+		status = qla4xxx_get_fwddb_entry(ha, index, fw_ddb_entry,
+						 fw_ddb_dma, NULL, &next_idx,
+						 &state, &conn_err, NULL,
+						 &conn_id);
+		if (status == QLA_ERROR) {
+			ret = -ENOMEM;
+			break;
+		}
+
+		qla4xxx_convert_param_ddb(fw_ddb_entry, tmp_tddb, NULL);
+
+		status = qla4xxx_compare_tuple_ddb(ha, flash_tddb, tmp_tddb,
+						   true);
+		if (status == QLA_SUCCESS) {
+			ret = qla4xxx_sysfs_ddb_logout_sid(ddb_entry->sess);
+			break;
+		}
+	}
+
+	if (idx == MAX_DDB_ENTRIES)
+		ret = -ESRCH;
+
+exit_ddb_logout:
+	if (flash_tddb)
+		vfree(flash_tddb);
+	if (tmp_tddb)
+		vfree(tmp_tddb);
+	if (fw_ddb_entry)
+		dma_pool_free(ha->fw_ddb_dma_pool, fw_ddb_entry, fw_ddb_dma);
+
+	return ret;
+}
+
+static int
+qla4xxx_sysfs_ddb_get_param(struct iscsi_bus_flash_session *fnode_sess,
+			    int param, char *buf)
+{
+	struct Scsi_Host *shost = iscsi_flash_session_to_shost(fnode_sess);
+	struct scsi_qla_host *ha = to_qla_host(shost);
+	struct iscsi_bus_flash_conn *fnode_conn;
+	struct ql4_chap_table chap_tbl;
+	struct device *dev;
+	int parent_type, parent_index = 0xffff;
+	int rc = 0;
+
+	dev = iscsi_find_flashnode_conn(fnode_sess, NULL,
+					iscsi_is_flashnode_conn_dev);
+	if (!dev)
+		return -EIO;
+
+	fnode_conn = iscsi_dev_to_flash_conn(dev);
+
+	switch (param) {
+	case ISCSI_FLASHNODE_IS_FW_ASSIGNED_IPV6:
+		rc = sprintf(buf, "%u\n", fnode_conn->is_fw_assigned_ipv6);
+		break;
+	case ISCSI_FLASHNODE_PORTAL_TYPE:
+		rc = sprintf(buf, "%s\n", fnode_sess->portal_type);
+		break;
+	case ISCSI_FLASHNODE_AUTO_SND_TGT_DISABLE:
+		rc = sprintf(buf, "%u\n", fnode_sess->auto_snd_tgt_disable);
+		break;
+	case ISCSI_FLASHNODE_DISCOVERY_SESS:
+		rc = sprintf(buf, "%u\n", fnode_sess->discovery_sess);
+		break;
+	case ISCSI_FLASHNODE_ENTRY_EN:
+		rc = sprintf(buf, "%u\n", fnode_sess->entry_state);
+		break;
+	case ISCSI_FLASHNODE_HDR_DGST_EN:
+		rc = sprintf(buf, "%u\n", fnode_conn->hdrdgst_en);
+		break;
+	case ISCSI_FLASHNODE_DATA_DGST_EN:
+		rc = sprintf(buf, "%u\n", fnode_conn->datadgst_en);
+		break;
+	case ISCSI_FLASHNODE_IMM_DATA_EN:
+		rc = sprintf(buf, "%u\n", fnode_sess->imm_data_en);
+		break;
+	case ISCSI_FLASHNODE_INITIAL_R2T_EN:
+		rc = sprintf(buf, "%u\n", fnode_sess->initial_r2t_en);
+		break;
+	case ISCSI_FLASHNODE_DATASEQ_INORDER:
+		rc = sprintf(buf, "%u\n", fnode_sess->dataseq_inorder_en);
+		break;
+	case ISCSI_FLASHNODE_PDU_INORDER:
+		rc = sprintf(buf, "%u\n", fnode_sess->pdu_inorder_en);
+		break;
+	case ISCSI_FLASHNODE_CHAP_AUTH_EN:
+		rc = sprintf(buf, "%u\n", fnode_sess->chap_auth_en);
+		break;
+	case ISCSI_FLASHNODE_SNACK_REQ_EN:
+		rc = sprintf(buf, "%u\n", fnode_conn->snack_req_en);
+		break;
+	case ISCSI_FLASHNODE_DISCOVERY_LOGOUT_EN:
+		rc = sprintf(buf, "%u\n", fnode_sess->discovery_logout_en);
+		break;
+	case ISCSI_FLASHNODE_BIDI_CHAP_EN:
+		rc = sprintf(buf, "%u\n", fnode_sess->bidi_chap_en);
+		break;
+	case ISCSI_FLASHNODE_DISCOVERY_AUTH_OPTIONAL:
+		rc = sprintf(buf, "%u\n", fnode_sess->discovery_auth_optional);
+		break;
+	case ISCSI_FLASHNODE_ERL:
+		rc = sprintf(buf, "%u\n", fnode_sess->erl);
+		break;
+	case ISCSI_FLASHNODE_TCP_TIMESTAMP_STAT:
+		rc = sprintf(buf, "%u\n", fnode_conn->tcp_timestamp_stat);
+		break;
+	case ISCSI_FLASHNODE_TCP_NAGLE_DISABLE:
+		rc = sprintf(buf, "%u\n", fnode_conn->tcp_nagle_disable);
+		break;
+	case ISCSI_FLASHNODE_TCP_WSF_DISABLE:
+		rc = sprintf(buf, "%u\n", fnode_conn->tcp_wsf_disable);
+		break;
+	case ISCSI_FLASHNODE_TCP_TIMER_SCALE:
+		rc = sprintf(buf, "%u\n", fnode_conn->tcp_timer_scale);
+		break;
+	case ISCSI_FLASHNODE_TCP_TIMESTAMP_EN:
+		rc = sprintf(buf, "%u\n", fnode_conn->tcp_timestamp_en);
+		break;
+	case ISCSI_FLASHNODE_IP_FRAG_DISABLE:
+		rc = sprintf(buf, "%u\n", fnode_conn->fragment_disable);
+		break;
+	case ISCSI_FLASHNODE_MAX_RECV_DLENGTH:
+		rc = sprintf(buf, "%u\n", fnode_conn->max_recv_dlength);
+		break;
+	case ISCSI_FLASHNODE_MAX_XMIT_DLENGTH:
+		rc = sprintf(buf, "%u\n", fnode_conn->max_xmit_dlength);
+		break;
+	case ISCSI_FLASHNODE_FIRST_BURST:
+		rc = sprintf(buf, "%u\n", fnode_sess->first_burst);
+		break;
+	case ISCSI_FLASHNODE_DEF_TIME2WAIT:
+		rc = sprintf(buf, "%u\n", fnode_sess->time2wait);
+		break;
+	case ISCSI_FLASHNODE_DEF_TIME2RETAIN:
+		rc = sprintf(buf, "%u\n", fnode_sess->time2retain);
+		break;
+	case ISCSI_FLASHNODE_MAX_R2T:
+		rc = sprintf(buf, "%u\n", fnode_sess->max_r2t);
+		break;
+	case ISCSI_FLASHNODE_KEEPALIVE_TMO:
+		rc = sprintf(buf, "%u\n", fnode_conn->keepalive_timeout);
+		break;
+	case ISCSI_FLASHNODE_ISID:
+		rc = sprintf(buf, "%02x%02x%02x%02x%02x%02x\n",
+			     fnode_sess->isid[0], fnode_sess->isid[1],
+			     fnode_sess->isid[2], fnode_sess->isid[3],
+			     fnode_sess->isid[4], fnode_sess->isid[5]);
+		break;
+	case ISCSI_FLASHNODE_TSID:
+		rc = sprintf(buf, "%u\n", fnode_sess->tsid);
+		break;
+	case ISCSI_FLASHNODE_PORT:
+		rc = sprintf(buf, "%d\n", fnode_conn->port);
+		break;
+	case ISCSI_FLASHNODE_MAX_BURST:
+		rc = sprintf(buf, "%u\n", fnode_sess->max_burst);
+		break;
+	case ISCSI_FLASHNODE_DEF_TASKMGMT_TMO:
+		rc = sprintf(buf, "%u\n",
+			     fnode_sess->default_taskmgmt_timeout);
+		break;
+	case ISCSI_FLASHNODE_IPADDR:
+		if (!strncmp(fnode_sess->portal_type, PORTAL_TYPE_IPV6, 4))
+			rc = sprintf(buf, "%pI6\n", fnode_conn->ipaddress);
+		else
+			rc = sprintf(buf, "%pI4\n", fnode_conn->ipaddress);
+		break;
+	case ISCSI_FLASHNODE_ALIAS:
+		if (fnode_sess->targetalias)
+			rc = sprintf(buf, "%s\n", fnode_sess->targetalias);
+		else
+			rc = sprintf(buf, "\n");
+		break;
+	case ISCSI_FLASHNODE_REDIRECT_IPADDR:
+		if (!strncmp(fnode_sess->portal_type, PORTAL_TYPE_IPV6, 4))
+			rc = sprintf(buf, "%pI6\n",
+				     fnode_conn->redirect_ipaddr);
+		else
+			rc = sprintf(buf, "%pI4\n",
+				     fnode_conn->redirect_ipaddr);
+		break;
+	case ISCSI_FLASHNODE_MAX_SEGMENT_SIZE:
+		rc = sprintf(buf, "%u\n", fnode_conn->max_segment_size);
+		break;
+	case ISCSI_FLASHNODE_LOCAL_PORT:
+		rc = sprintf(buf, "%u\n", fnode_conn->local_port);
+		break;
+	case ISCSI_FLASHNODE_IPV4_TOS:
+		rc = sprintf(buf, "%u\n", fnode_conn->ipv4_tos);
+		break;
+	case ISCSI_FLASHNODE_IPV6_TC:
+		if (!strncmp(fnode_sess->portal_type, PORTAL_TYPE_IPV6, 4))
+			rc = sprintf(buf, "%u\n",
+				     fnode_conn->ipv6_traffic_class);
+		else
+			rc = sprintf(buf, "\n");
+		break;
+	case ISCSI_FLASHNODE_IPV6_FLOW_LABEL:
+		rc = sprintf(buf, "%u\n", fnode_conn->ipv6_flow_label);
+		break;
+	case ISCSI_FLASHNODE_LINK_LOCAL_IPV6:
+		if (!strncmp(fnode_sess->portal_type, PORTAL_TYPE_IPV6, 4))
+			rc = sprintf(buf, "%pI6\n",
+				     fnode_conn->link_local_ipv6_addr);
+		else
+			rc = sprintf(buf, "\n");
+		break;
+	case ISCSI_FLASHNODE_DISCOVERY_PARENT_IDX:
+		if ((fnode_sess->discovery_parent_idx) >= 0  &&
+		    (fnode_sess->discovery_parent_idx < MAX_DDB_ENTRIES))
+			parent_index = fnode_sess->discovery_parent_idx;
+
+		rc = sprintf(buf, "%u\n", parent_index);
+		break;
+	case ISCSI_FLASHNODE_DISCOVERY_PARENT_TYPE:
+		if (fnode_sess->discovery_parent_type == DDB_ISNS)
+			parent_type = ISCSI_DISC_PARENT_ISNS;
+		else if (fnode_sess->discovery_parent_type == DDB_NO_LINK)
+			parent_type = ISCSI_DISC_PARENT_UNKNOWN;
+		else if (fnode_sess->discovery_parent_type >= 0  &&
+			 fnode_sess->discovery_parent_type < MAX_DDB_ENTRIES)
+			parent_type = ISCSI_DISC_PARENT_SENDTGT;
+		else
+			parent_type = ISCSI_DISC_PARENT_UNKNOWN;
+
+		rc = sprintf(buf, "%s\n",
+			     iscsi_get_discovery_parent_name(parent_type));
+		break;
+	case ISCSI_FLASHNODE_NAME:
+		if (fnode_sess->targetname)
+			rc = sprintf(buf, "%s\n", fnode_sess->targetname);
+		else
+			rc = sprintf(buf, "\n");
+		break;
+	case ISCSI_FLASHNODE_TPGT:
+		rc = sprintf(buf, "%u\n", fnode_sess->tpgt);
+		break;
+	case ISCSI_FLASHNODE_TCP_XMIT_WSF:
+		rc = sprintf(buf, "%u\n", fnode_conn->tcp_xmit_wsf);
+		break;
+	case ISCSI_FLASHNODE_TCP_RECV_WSF:
+		rc = sprintf(buf, "%u\n", fnode_conn->tcp_recv_wsf);
+		break;
+	case ISCSI_FLASHNODE_CHAP_OUT_IDX:
+		rc = sprintf(buf, "%u\n", fnode_sess->chap_out_idx);
+		break;
+	case ISCSI_FLASHNODE_USERNAME:
+		if (fnode_sess->chap_auth_en) {
+			qla4xxx_get_uni_chap_at_index(ha,
+						      chap_tbl.name,
+						      chap_tbl.secret,
+						      fnode_sess->chap_out_idx);
+			rc = sprintf(buf, "%s\n", chap_tbl.name);
+		} else {
+			rc = sprintf(buf, "\n");
+		}
+		break;
+	case ISCSI_FLASHNODE_PASSWORD:
+		if (fnode_sess->chap_auth_en) {
+			qla4xxx_get_uni_chap_at_index(ha,
+						      chap_tbl.name,
+						      chap_tbl.secret,
+						      fnode_sess->chap_out_idx);
+			rc = sprintf(buf, "%s\n", chap_tbl.secret);
+		} else {
+			rc = sprintf(buf, "\n");
+		}
+		break;
+	case ISCSI_FLASHNODE_STATSN:
+		rc = sprintf(buf, "%u\n", fnode_conn->statsn);
+		break;
+	case ISCSI_FLASHNODE_EXP_STATSN:
+		rc = sprintf(buf, "%u\n", fnode_conn->exp_statsn);
+		break;
+	case ISCSI_FLASHNODE_IS_BOOT_TGT:
+		rc = sprintf(buf, "%u\n", fnode_sess->is_boot_target);
+		break;
+	default:
+		rc = -ENOSYS;
+		break;
+	}
+	return rc;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_set_param - Set parameter for firmware DDB entry
+ * @fnode_sess: pointer to session attrs of flash ddb entry
+ * @fnode_conn: pointer to connection attrs of flash ddb entry
+ * @data: Parameters and their values to update
+ * @len: len of data
+ *
+ * This sets the parameter of flash ddb entry and writes them to flash
+ **/
+static int
+qla4xxx_sysfs_ddb_set_param(struct iscsi_bus_flash_session *fnode_sess,
+			    struct iscsi_bus_flash_conn *fnode_conn,
+			    void *data, int len)
+{
+	struct Scsi_Host *shost = iscsi_flash_session_to_shost(fnode_sess);
+	struct scsi_qla_host *ha = to_qla_host(shost);
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	struct iscsi_flashnode_param_info *fnode_param;
+	struct nlattr *attr;
+	int rc = QLA_ERROR;
+	uint32_t rem = len;
+
+	fw_ddb_entry = kzalloc(sizeof(*fw_ddb_entry), GFP_KERNEL);
+	if (!fw_ddb_entry) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to allocate ddb buffer\n",
+				  __func__));
+		return -ENOMEM;
+	}
+
+	nla_for_each_attr(attr, data, len, rem) {
+		fnode_param = nla_data(attr);
+
+		switch (fnode_param->param) {
+		case ISCSI_FLASHNODE_IS_FW_ASSIGNED_IPV6:
+			fnode_conn->is_fw_assigned_ipv6 = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_PORTAL_TYPE:
+			memcpy(fnode_sess->portal_type, fnode_param->value,
+			       strlen(fnode_sess->portal_type));
+			break;
+		case ISCSI_FLASHNODE_AUTO_SND_TGT_DISABLE:
+			fnode_sess->auto_snd_tgt_disable =
+							fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_DISCOVERY_SESS:
+			fnode_sess->discovery_sess = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_ENTRY_EN:
+			fnode_sess->entry_state = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_HDR_DGST_EN:
+			fnode_conn->hdrdgst_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_DATA_DGST_EN:
+			fnode_conn->datadgst_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_IMM_DATA_EN:
+			fnode_sess->imm_data_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_INITIAL_R2T_EN:
+			fnode_sess->initial_r2t_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_DATASEQ_INORDER:
+			fnode_sess->dataseq_inorder_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_PDU_INORDER:
+			fnode_sess->pdu_inorder_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_CHAP_AUTH_EN:
+			fnode_sess->chap_auth_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_SNACK_REQ_EN:
+			fnode_conn->snack_req_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_DISCOVERY_LOGOUT_EN:
+			fnode_sess->discovery_logout_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_BIDI_CHAP_EN:
+			fnode_sess->bidi_chap_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_DISCOVERY_AUTH_OPTIONAL:
+			fnode_sess->discovery_auth_optional =
+							fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_ERL:
+			fnode_sess->erl = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_TCP_TIMESTAMP_STAT:
+			fnode_conn->tcp_timestamp_stat = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_TCP_NAGLE_DISABLE:
+			fnode_conn->tcp_nagle_disable = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_TCP_WSF_DISABLE:
+			fnode_conn->tcp_wsf_disable = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_TCP_TIMER_SCALE:
+			fnode_conn->tcp_timer_scale = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_TCP_TIMESTAMP_EN:
+			fnode_conn->tcp_timestamp_en = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_IP_FRAG_DISABLE:
+			fnode_conn->fragment_disable = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_MAX_RECV_DLENGTH:
+			fnode_conn->max_recv_dlength =
+					*(unsigned *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_MAX_XMIT_DLENGTH:
+			fnode_conn->max_xmit_dlength =
+					*(unsigned *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_FIRST_BURST:
+			fnode_sess->first_burst =
+					*(unsigned *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_DEF_TIME2WAIT:
+			fnode_sess->time2wait = *(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_DEF_TIME2RETAIN:
+			fnode_sess->time2retain =
+						*(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_MAX_R2T:
+			fnode_sess->max_r2t =
+					*(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_KEEPALIVE_TMO:
+			fnode_conn->keepalive_timeout =
+				*(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_ISID:
+			memcpy(fnode_sess->isid, fnode_param->value,
+			       sizeof(fnode_sess->isid));
+			break;
+		case ISCSI_FLASHNODE_TSID:
+			fnode_sess->tsid = *(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_PORT:
+			fnode_conn->port = *(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_MAX_BURST:
+			fnode_sess->max_burst = *(unsigned *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_DEF_TASKMGMT_TMO:
+			fnode_sess->default_taskmgmt_timeout =
+						*(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_IPADDR:
+			memcpy(fnode_conn->ipaddress, fnode_param->value,
+			       IPv6_ADDR_LEN);
+			break;
+		case ISCSI_FLASHNODE_ALIAS:
+			rc = iscsi_switch_str_param(&fnode_sess->targetalias,
+						    (char *)fnode_param->value);
+			break;
+		case ISCSI_FLASHNODE_REDIRECT_IPADDR:
+			memcpy(fnode_conn->redirect_ipaddr, fnode_param->value,
+			       IPv6_ADDR_LEN);
+			break;
+		case ISCSI_FLASHNODE_MAX_SEGMENT_SIZE:
+			fnode_conn->max_segment_size =
+					*(unsigned *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_LOCAL_PORT:
+			fnode_conn->local_port =
+						*(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_IPV4_TOS:
+			fnode_conn->ipv4_tos = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_IPV6_TC:
+			fnode_conn->ipv6_traffic_class = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_IPV6_FLOW_LABEL:
+			fnode_conn->ipv6_flow_label = fnode_param->value[0];
+			break;
+		case ISCSI_FLASHNODE_NAME:
+			rc = iscsi_switch_str_param(&fnode_sess->targetname,
+						    (char *)fnode_param->value);
+			break;
+		case ISCSI_FLASHNODE_TPGT:
+			fnode_sess->tpgt = *(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_LINK_LOCAL_IPV6:
+			memcpy(fnode_conn->link_local_ipv6_addr,
+			       fnode_param->value, IPv6_ADDR_LEN);
+			break;
+		case ISCSI_FLASHNODE_DISCOVERY_PARENT_TYPE:
+			fnode_sess->discovery_parent_type =
+						*(uint16_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_TCP_XMIT_WSF:
+			fnode_conn->tcp_xmit_wsf =
+						*(uint8_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_TCP_RECV_WSF:
+			fnode_conn->tcp_recv_wsf =
+						*(uint8_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_STATSN:
+			fnode_conn->statsn = *(uint32_t *)fnode_param->value;
+			break;
+		case ISCSI_FLASHNODE_EXP_STATSN:
+			fnode_conn->exp_statsn =
+						*(uint32_t *)fnode_param->value;
+			break;
+		default:
+			ql4_printk(KERN_ERR, ha,
+				   "%s: No such sysfs attribute\n", __func__);
+			rc = -ENOSYS;
+			goto exit_set_param;
+		}
+	}
+
+	rc = qla4xxx_sysfs_ddb_apply(fnode_sess, fnode_conn);
+
+exit_set_param:
+	return rc;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_delete - Delete firmware DDB entry
+ * @fnode_sess: pointer to session attrs of flash ddb entry
+ *
+ * This invalidates the flash ddb entry at the given index
+ **/
+static int qla4xxx_sysfs_ddb_delete(struct iscsi_bus_flash_session *fnode_sess)
+{
+	struct Scsi_Host *shost = iscsi_flash_session_to_shost(fnode_sess);
+	struct scsi_qla_host *ha = to_qla_host(shost);
+	uint32_t dev_db_start_offset;
+	uint32_t dev_db_end_offset;
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	dma_addr_t fw_ddb_entry_dma;
+	uint16_t *ddb_cookie = NULL;
+	size_t ddb_size;
+	void *pddb = NULL;
+	int target_id;
+	int rc = 0;
+
+	if (!fnode_sess) {
+		rc = -EINVAL;
+		goto exit_ddb_del;
+	}
+
+	if (fnode_sess->is_boot_target) {
+		rc = -EPERM;
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Deletion of boot target entry is not permitted.\n",
+				  __func__));
+		goto exit_ddb_del;
+	}
+
+	if (fnode_sess->flash_state == DEV_DB_NON_PERSISTENT)
+		goto sysfs_ddb_del;
+
+	if (is_qla40XX(ha)) {
+		dev_db_start_offset = FLASH_OFFSET_DB_INFO;
+		dev_db_end_offset = FLASH_OFFSET_DB_END;
+		dev_db_start_offset += (fnode_sess->target_id *
+				       sizeof(*fw_ddb_entry));
+		ddb_size = sizeof(*fw_ddb_entry);
+	} else {
+		dev_db_start_offset = FLASH_RAW_ACCESS_ADDR +
+				      (ha->hw.flt_region_ddb << 2);
+		/* flt_ddb_size is DDB table size for both ports
+		 * so divide it by 2 to calculate the offset for second port
+		 */
+		if (ha->port_num == 1)
+			dev_db_start_offset += (ha->hw.flt_ddb_size / 2);
+
+		dev_db_end_offset = dev_db_start_offset +
+				    (ha->hw.flt_ddb_size / 2);
+
+		dev_db_start_offset += (fnode_sess->target_id *
+				       sizeof(*fw_ddb_entry));
+		dev_db_start_offset += (void *)&(fw_ddb_entry->cookie) -
+				       (void *)fw_ddb_entry;
+
+		ddb_size = sizeof(*ddb_cookie);
+	}
+
+	DEBUG2(ql4_printk(KERN_ERR, ha, "%s: start offset=%u, end offset=%u\n",
+			  __func__, dev_db_start_offset, dev_db_end_offset));
+
+	if (dev_db_start_offset > dev_db_end_offset) {
+		rc = -EIO;
+		DEBUG2(ql4_printk(KERN_ERR, ha, "%s:Invalid DDB index %u\n",
+				  __func__, fnode_sess->target_id));
+		goto exit_ddb_del;
+	}
+
+	pddb = dma_alloc_coherent(&ha->pdev->dev, ddb_size,
+				  &fw_ddb_entry_dma, GFP_KERNEL);
+	if (!pddb) {
+		rc = -ENOMEM;
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to allocate dma buffer\n",
+				  __func__));
+		goto exit_ddb_del;
+	}
+
+	if (is_qla40XX(ha)) {
+		fw_ddb_entry = pddb;
+		memset(fw_ddb_entry, 0, ddb_size);
+		ddb_cookie = &fw_ddb_entry->cookie;
+	} else {
+		ddb_cookie = pddb;
+	}
+
+	/* invalidate the cookie */
+	*ddb_cookie = 0xFFEE;
+	qla4xxx_set_flash(ha, fw_ddb_entry_dma, dev_db_start_offset,
+			  ddb_size, FLASH_OPT_RMW_COMMIT);
+
+sysfs_ddb_del:
+	target_id = fnode_sess->target_id;
+	iscsi_destroy_flashnode_sess(fnode_sess);
+	ql4_printk(KERN_INFO, ha,
+		   "%s: session and conn entries for flashnode %u of host %lu deleted\n",
+		   __func__, target_id, ha->host_no);
+exit_ddb_del:
+	if (pddb)
+		dma_free_coherent(&ha->pdev->dev, ddb_size, pddb,
+				  fw_ddb_entry_dma);
+	return rc;
+}
+
+/**
+ * qla4xxx_sysfs_ddb_export - Create sysfs entries for firmware DDBs
+ * @ha: pointer to adapter structure
+ *
+ * Export the firmware DDB for all send targets and normal targets to sysfs.
+ **/
+static int qla4xxx_sysfs_ddb_export(struct scsi_qla_host *ha)
+{
+	struct dev_db_entry *fw_ddb_entry = NULL;
+	dma_addr_t fw_ddb_entry_dma;
+	uint16_t max_ddbs;
+	uint16_t idx = 0;
+	int ret = QLA_SUCCESS;
+
+	fw_ddb_entry = dma_alloc_coherent(&ha->pdev->dev,
+					  sizeof(*fw_ddb_entry),
+					  &fw_ddb_entry_dma, GFP_KERNEL);
+	if (!fw_ddb_entry) {
+		DEBUG2(ql4_printk(KERN_ERR, ha,
+				  "%s: Unable to allocate dma buffer\n",
+				  __func__));
+		return -ENOMEM;
+	}
+
+	max_ddbs =  is_qla40XX(ha) ? MAX_PRST_DEV_DB_ENTRIES :
+				     MAX_DEV_DB_ENTRIES;
+
+	for (idx = 0; idx < max_ddbs; idx++) {
+		if (qla4xxx_flashdb_by_index(ha, fw_ddb_entry, fw_ddb_entry_dma,
+					     idx))
+			continue;
+
+		ret = qla4xxx_sysfs_ddb_tgt_create(ha, fw_ddb_entry, &idx, 0);
+		if (ret) {
+			ret = -EIO;
+			break;
+		}
+	}
+
+	dma_free_coherent(&ha->pdev->dev, sizeof(*fw_ddb_entry), fw_ddb_entry,
+			  fw_ddb_entry_dma);
+
+	return ret;
+}
+
+static void qla4xxx_sysfs_ddb_remove(struct scsi_qla_host *ha)
+{
+	iscsi_destroy_all_flashnode(ha->host);
+}
+
 /**
  * qla4xxx_build_ddb_list - Build ddb list and setup sessions
  * @ha: pointer to adapter structure
@@ -5341,8 +7016,11 @@ static int qla4xxx_probe_adapter(struct pci_dev *pdev,
 	status = qla4xxx_initialize_adapter(ha, INIT_ADAPTER);
 
 	/* Dont retry adapter initialization if IRQ allocation failed */
-	if (!test_bit(AF_IRQ_ATTACHED, &ha->flags))
+	if (is_qla80XX(ha) && !test_bit(AF_IRQ_ATTACHED, &ha->flags)) {
+		ql4_printk(KERN_WARNING, ha, "%s: Skipping retry of adapter initialization\n",
+			   __func__);
 		goto skip_retry_init;
+	}
 
 	while ((!test_bit(AF_ONLINE, &ha->flags)) &&
 	    init_retry_count++ < MAX_INIT_RETRIES) {
@@ -5445,6 +7123,10 @@ skip_retry_init:
 		ql4_printk(KERN_ERR, ha,
 			   "%s: No iSCSI boot target configured\n", __func__);
 
+	if (qla4xxx_sysfs_ddb_export(ha))
+		ql4_printk(KERN_ERR, ha,
+			   "%s: Error exporting ddb to sysfs\n", __func__);
+
 		/* Perform the build ddb list and login to each */
 	qla4xxx_build_ddb_list(ha, INIT_ADAPTER);
 	iscsi_host_for_each_session(ha->host, qla4xxx_login_flash_ddb);
@@ -5570,6 +7252,7 @@ static void qla4xxx_remove_adapter(struct pci_dev *pdev)
 	qla4xxx_destroy_fw_ddb_session(ha);
 	qla4_8xxx_free_sysfs_attr(ha);
 
+	qla4xxx_sysfs_ddb_remove(ha);
 	scsi_remove_host(ha->host);
 
 	qla4xxx_free_adapter(ha);
@@ -5669,7 +7352,6 @@ struct srb *qla4xxx_del_from_active_array(struct scsi_qla_host *ha,
 
 	/* update counters */
 	if (srb->flags & SRB_DMA_VALID) {
-		ha->req_q_count += srb->iocb_cnt;
 		ha->iocb_cnt -= srb->iocb_cnt;
 		if (srb->cmd)
 			srb->cmd->host_scribble =
@@ -6081,6 +7763,7 @@ static int qla4xxx_host_reset(struct Scsi_Host *shost, int reset_type)
 {
 	struct scsi_qla_host *ha = to_qla_host(shost);
 	int rval = QLA_SUCCESS;
+	uint32_t idc_ctrl;
 
 	if (ql4xdontresethba) {
 		DEBUG2(ql4_printk(KERN_INFO, ha, "%s: Don't Reset HBA\n",
@@ -6111,6 +7794,14 @@ static int qla4xxx_host_reset(struct Scsi_Host *shost, int reset_type)
 	}
 
 recover_adapter:
+	/* For ISP83XX set graceful reset bit in IDC_DRV_CTRL if
+	 * reset is issued by application */
+	if (is_qla8032(ha) && test_bit(DPC_RESET_HA, &ha->dpc_flags)) {
+		idc_ctrl = qla4_83xx_rd_reg(ha, QLA83XX_IDC_DRV_CTRL);
+		qla4_83xx_wr_reg(ha, QLA83XX_IDC_DRV_CTRL,
+				 (idc_ctrl | GRACEFUL_RESET_BIT1));
+	}
+
 	rval = qla4xxx_recover_adapter(ha);
 	if (rval != QLA_SUCCESS) {
 		DEBUG2(ql4_printk(KERN_INFO, ha, "%s: recover adapter fail\n",
diff --git a/drivers/scsi/qla4xxx/ql4_version.h b/drivers/scsi/qla4xxx/ql4_version.h
index 6775a45af315..83e0fec35d56 100644
--- a/drivers/scsi/qla4xxx/ql4_version.h
+++ b/drivers/scsi/qla4xxx/ql4_version.h
@@ -5,4 +5,4 @@
  * See LICENSE.qla4xxx for copyright and licensing details.
  */
 
-#define QLA4XXX_DRIVER_VERSION	"5.03.00-k4"
+#define QLA4XXX_DRIVER_VERSION	"5.03.00-k8"
diff --git a/drivers/scsi/scsi_debug.c b/drivers/scsi/scsi_debug.c
index 5cda11c07c68..5add6f4e7928 100644
--- a/drivers/scsi/scsi_debug.c
+++ b/drivers/scsi/scsi_debug.c
@@ -2823,31 +2823,27 @@ static const char * scsi_debug_info(struct Scsi_Host * shp)
 /* scsi_debug_proc_info
  * Used if the driver currently has no own support for /proc/scsi
  */
-static int scsi_debug_proc_info(struct Scsi_Host *host, char *buffer, char **start, off_t offset,
-				int length, int inout)
+static int scsi_debug_write_info(struct Scsi_Host *host, char *buffer, int length)
 {
-	int len, pos, begin;
-	int orig_length;
+	char arr[16];
+	int opts;
+	int minLen = length > 15 ? 15 : length;
 
-	orig_length = length;
-
-	if (inout == 1) {
-		char arr[16];
-		int minLen = length > 15 ? 15 : length;
+	if (!capable(CAP_SYS_ADMIN) || !capable(CAP_SYS_RAWIO))
+		return -EACCES;
+	memcpy(arr, buffer, minLen);
+	arr[minLen] = '\0';
+	if (1 != sscanf(arr, "%d", &opts))
+		return -EINVAL;
+	scsi_debug_opts = opts;
+	if (scsi_debug_every_nth != 0)
+		scsi_debug_cmnd_count = 0;
+	return length;
+}
 
-		if (!capable(CAP_SYS_ADMIN) || !capable(CAP_SYS_RAWIO))
-			return -EACCES;
-		memcpy(arr, buffer, minLen);
-		arr[minLen] = '\0';
-		if (1 != sscanf(arr, "%d", &pos))
-			return -EINVAL;
-		scsi_debug_opts = pos;
-		if (scsi_debug_every_nth != 0)
-                        scsi_debug_cmnd_count = 0;
-		return length;
-	}
-	begin = 0;
-	pos = len = sprintf(buffer, "scsi_debug adapter driver, version "
+static int scsi_debug_show_info(struct seq_file *m, struct Scsi_Host *host)
+{
+	seq_printf(m, "scsi_debug adapter driver, version "
 	    "%s [%s]\n"
 	    "num_tgts=%d, shared (ram) size=%d MB, opts=0x%x, "
 	    "every_nth=%d(curr:%d)\n"
@@ -2862,15 +2858,7 @@ static int scsi_debug_proc_info(struct Scsi_Host *host, char *buffer, char **sta
 	    scsi_debug_sector_size, sdebug_cylinders_per, sdebug_heads,
 	    sdebug_sectors_per, num_aborts, num_dev_resets, num_bus_resets,
 	    num_host_resets, dix_reads, dix_writes, dif_errors);
-	if (pos < offset) {
-		len = 0;
-		begin = pos;
-	}
-	*start = buffer + (offset - begin);	/* Start of wanted data */
-	len -= (offset - begin);
-	if (len > length)
-		len = length;
-	return len;
+	return 0;
 }
 
 static ssize_t sdebug_delay_show(struct device_driver * ddp, char * buf)
@@ -3957,7 +3945,8 @@ write:
 static DEF_SCSI_QCMD(scsi_debug_queuecommand)
 
 static struct scsi_host_template sdebug_driver_template = {
-	.proc_info =		scsi_debug_proc_info,
+	.show_info =		scsi_debug_show_info,
+	.write_info =		scsi_debug_write_info,
 	.proc_name =		sdebug_proc_name,
 	.name =			"SCSI DEBUG",
 	.info =			scsi_debug_info,
diff --git a/drivers/scsi/scsi_netlink.c b/drivers/scsi/scsi_netlink.c
index 65123a21b97e..fe30ea94ffe6 100644
--- a/drivers/scsi/scsi_netlink.c
+++ b/drivers/scsi/scsi_netlink.c
@@ -50,7 +50,7 @@ scsi_nl_rcv_msg(struct sk_buff *skb)
 	u32 rlen;
 	int err, tport;
 
-	while (skb->len >= NLMSG_SPACE(0)) {
+	while (skb->len >= NLMSG_HDRLEN) {
 		err = 0;
 
 		nlh = nlmsg_hdr(skb);
@@ -70,7 +70,7 @@ scsi_nl_rcv_msg(struct sk_buff *skb)
 			goto next_msg;
 		}
 
-		hdr = NLMSG_DATA(nlh);
+		hdr = nlmsg_data(nlh);
 		if ((hdr->version != SCSI_NL_VERSION) ||
 		    (hdr->magic != SCSI_NL_MAGIC)) {
 			err = -EPROTOTYPE;
diff --git a/drivers/scsi/scsi_proc.c b/drivers/scsi/scsi_proc.c
index ad747dc337da..db66357211ed 100644
--- a/drivers/scsi/scsi_proc.c
+++ b/drivers/scsi/scsi_proc.c
@@ -45,58 +45,50 @@ static struct proc_dir_entry *proc_scsi;
 /* Protect sht->present and sht->proc_dir */
 static DEFINE_MUTEX(global_host_template_mutex);
 
-/**
- * proc_scsi_read - handle read from /proc by calling host's proc_info() command
- * @buffer: passed to proc_info
- * @start: passed to proc_info
- * @offset: passed to proc_info
- * @length: passed to proc_info
- * @eof: returns whether length read was less than requested
- * @data: pointer to a &struct Scsi_Host
- */
-
-static int proc_scsi_read(char *buffer, char **start, off_t offset,
-			  int length, int *eof, void *data)
-{
-	struct Scsi_Host *shost = data;
-	int n;
-
-	n = shost->hostt->proc_info(shost, buffer, start, offset, length, 0);
-	*eof = (n < length);
-
-	return n;
-}
-
-/**
- * proc_scsi_write_proc - Handle write to /proc by calling host's proc_info()
- * @file: not used
- * @buf: source of data to write.
- * @count: number of bytes (at most PROC_BLOCK_SIZE) to write.
- * @data: pointer to &struct Scsi_Host
- */
-static int proc_scsi_write_proc(struct file *file, const char __user *buf,
-                           unsigned long count, void *data)
+static ssize_t proc_scsi_host_write(struct file *file, const char __user *buf,
+                           size_t count, loff_t *ppos)
 {
-	struct Scsi_Host *shost = data;
+	struct Scsi_Host *shost = PDE_DATA(file_inode(file));
 	ssize_t ret = -ENOMEM;
 	char *page;
-	char *start;
     
 	if (count > PROC_BLOCK_SIZE)
 		return -EOVERFLOW;
 
+	if (!shost->hostt->write_info)
+		return -EINVAL;
+
 	page = (char *)__get_free_page(GFP_KERNEL);
 	if (page) {
 		ret = -EFAULT;
 		if (copy_from_user(page, buf, count))
 			goto out;
-		ret = shost->hostt->proc_info(shost, page, &start, 0, count, 1);
+		ret = shost->hostt->write_info(shost, page, count);
 	}
 out:
 	free_page((unsigned long)page);
 	return ret;
 }
 
+static int proc_scsi_show(struct seq_file *m, void *v)
+{
+	struct Scsi_Host *shost = m->private;
+	return shost->hostt->show_info(m, shost);
+}
+
+static int proc_scsi_host_open(struct inode *inode, struct file *file)
+{
+	return single_open_size(file, proc_scsi_show, PDE_DATA(inode),
+				4 * PAGE_SIZE);
+}
+
+static const struct file_operations proc_scsi_fops = {
+	.open = proc_scsi_host_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.write = proc_scsi_host_write
+};
+
 /**
  * scsi_proc_hostdir_add - Create directory in /proc for a scsi host
  * @sht: owner of this directory
@@ -106,7 +98,7 @@ out:
 
 void scsi_proc_hostdir_add(struct scsi_host_template *sht)
 {
-	if (!sht->proc_info)
+	if (!sht->show_info)
 		return;
 
 	mutex_lock(&global_host_template_mutex);
@@ -125,7 +117,7 @@ void scsi_proc_hostdir_add(struct scsi_host_template *sht)
  */
 void scsi_proc_hostdir_rm(struct scsi_host_template *sht)
 {
-	if (!sht->proc_info)
+	if (!sht->show_info)
 		return;
 
 	mutex_lock(&global_host_template_mutex);
@@ -151,16 +143,12 @@ void scsi_proc_host_add(struct Scsi_Host *shost)
 		return;
 
 	sprintf(name,"%d", shost->host_no);
-	p = create_proc_read_entry(name, S_IFREG | S_IRUGO | S_IWUSR,
-			sht->proc_dir, proc_scsi_read, shost);
-	if (!p) {
+	p = proc_create_data(name, S_IRUGO | S_IWUSR,
+		sht->proc_dir, &proc_scsi_fops, shost);
+	if (!p)
 		printk(KERN_ERR "%s: Failed to register host %d in"
 		       "%s\n", __func__, shost->host_no,
 		       sht->proc_name);
-		return;
-	} 
-
-	p->write_proc = proc_scsi_write_proc;
 }
 
 /**
diff --git a/drivers/scsi/scsi_transport_fc.c b/drivers/scsi/scsi_transport_fc.c
index e894ca7b54c0..e106c276aa00 100644
--- a/drivers/scsi/scsi_transport_fc.c
+++ b/drivers/scsi/scsi_transport_fc.c
@@ -35,7 +35,6 @@
 #include <scsi/scsi_transport.h>
 #include <scsi/scsi_transport_fc.h>
 #include <scsi/scsi_cmnd.h>
-#include <linux/netlink.h>
 #include <net/netlink.h>
 #include <scsi/scsi_netlink_fc.h>
 #include <scsi/scsi_bsg_fc.h>
@@ -534,7 +533,7 @@ fc_host_post_event(struct Scsi_Host *shost, u32 event_number,
 	struct nlmsghdr	*nlh;
 	struct fc_nl_event *event;
 	const char *name;
-	u32 len, skblen;
+	u32 len;
 	int err;
 
 	if (!scsi_nl_sock) {
@@ -543,21 +542,19 @@ fc_host_post_event(struct Scsi_Host *shost, u32 event_number,
 	}
 
 	len = FC_NL_MSGALIGN(sizeof(*event));
-	skblen = NLMSG_SPACE(len);
 
-	skb = alloc_skb(skblen, GFP_KERNEL);
+	skb = nlmsg_new(len, GFP_KERNEL);
 	if (!skb) {
 		err = -ENOBUFS;
 		goto send_fail;
 	}
 
-	nlh = nlmsg_put(skb, 0, 0, SCSI_TRANSPORT_MSG,
-				skblen - sizeof(*nlh), 0);
+	nlh = nlmsg_put(skb, 0, 0, SCSI_TRANSPORT_MSG, len, 0);
 	if (!nlh) {
 		err = -ENOBUFS;
 		goto send_fail_skb;
 	}
-	event = NLMSG_DATA(nlh);
+	event = nlmsg_data(nlh);
 
 	INIT_SCSI_NL_HDR(&event->snlh, SCSI_NL_TRANSPORT_FC,
 				FC_NL_ASYNC_EVENT, len);
@@ -604,7 +601,7 @@ fc_host_post_vendor_event(struct Scsi_Host *shost, u32 event_number,
 	struct sk_buff *skb;
 	struct nlmsghdr	*nlh;
 	struct fc_nl_event *event;
-	u32 len, skblen;
+	u32 len;
 	int err;
 
 	if (!scsi_nl_sock) {
@@ -613,21 +610,19 @@ fc_host_post_vendor_event(struct Scsi_Host *shost, u32 event_number,
 	}
 
 	len = FC_NL_MSGALIGN(sizeof(*event) + data_len);
-	skblen = NLMSG_SPACE(len);
 
-	skb = alloc_skb(skblen, GFP_KERNEL);
+	skb = nlmsg_new(len, GFP_KERNEL);
 	if (!skb) {
 		err = -ENOBUFS;
 		goto send_vendor_fail;
 	}
 
-	nlh = nlmsg_put(skb, 0, 0, SCSI_TRANSPORT_MSG,
-				skblen - sizeof(*nlh), 0);
+	nlh = nlmsg_put(skb, 0, 0, SCSI_TRANSPORT_MSG, len, 0);
 	if (!nlh) {
 		err = -ENOBUFS;
 		goto send_vendor_fail_skb;
 	}
-	event = NLMSG_DATA(nlh);
+	event = nlmsg_data(nlh);
 
 	INIT_SCSI_NL_HDR(&event->snlh, SCSI_NL_TRANSPORT_FC,
 				FC_NL_ASYNC_EVENT, len);
diff --git a/drivers/scsi/scsi_transport_iscsi.c b/drivers/scsi/scsi_transport_iscsi.c
index 0a74b975efdf..47799a33d6ca 100644
--- a/drivers/scsi/scsi_transport_iscsi.c
+++ b/drivers/scsi/scsi_transport_iscsi.c
@@ -25,6 +25,7 @@
 #include <linux/slab.h>
 #include <linux/bsg-lib.h>
 #include <linux/idr.h>
+#include <linux/list.h>
 #include <net/tcp.h>
 #include <scsi/scsi.h>
 #include <scsi/scsi_host.h>
@@ -460,6 +461,689 @@ void iscsi_destroy_iface(struct iscsi_iface *iface)
 EXPORT_SYMBOL_GPL(iscsi_destroy_iface);
 
 /*
+ * Interface to display flash node params to sysfs
+ */
+
+#define ISCSI_FLASHNODE_ATTR(_prefix, _name, _mode, _show, _store)	\
+struct device_attribute dev_attr_##_prefix##_##_name =			\
+	__ATTR(_name, _mode, _show, _store)
+
+/* flash node session attrs show */
+#define iscsi_flashnode_sess_attr_show(type, name, param)		\
+static ssize_t								\
+show_##type##_##name(struct device *dev, struct device_attribute *attr,	\
+		     char *buf)						\
+{									\
+	struct iscsi_bus_flash_session *fnode_sess =			\
+					iscsi_dev_to_flash_session(dev);\
+	struct iscsi_transport *t = fnode_sess->transport;		\
+	return t->get_flashnode_param(fnode_sess, param, buf);		\
+}									\
+
+
+#define iscsi_flashnode_sess_attr(type, name, param)			\
+	iscsi_flashnode_sess_attr_show(type, name, param)		\
+static ISCSI_FLASHNODE_ATTR(type, name, S_IRUGO,			\
+			    show_##type##_##name, NULL);
+
+/* Flash node session attributes */
+
+iscsi_flashnode_sess_attr(fnode, auto_snd_tgt_disable,
+			  ISCSI_FLASHNODE_AUTO_SND_TGT_DISABLE);
+iscsi_flashnode_sess_attr(fnode, discovery_session,
+			  ISCSI_FLASHNODE_DISCOVERY_SESS);
+iscsi_flashnode_sess_attr(fnode, portal_type, ISCSI_FLASHNODE_PORTAL_TYPE);
+iscsi_flashnode_sess_attr(fnode, entry_enable, ISCSI_FLASHNODE_ENTRY_EN);
+iscsi_flashnode_sess_attr(fnode, immediate_data, ISCSI_FLASHNODE_IMM_DATA_EN);
+iscsi_flashnode_sess_attr(fnode, initial_r2t, ISCSI_FLASHNODE_INITIAL_R2T_EN);
+iscsi_flashnode_sess_attr(fnode, data_seq_in_order,
+			  ISCSI_FLASHNODE_DATASEQ_INORDER);
+iscsi_flashnode_sess_attr(fnode, data_pdu_in_order,
+			  ISCSI_FLASHNODE_PDU_INORDER);
+iscsi_flashnode_sess_attr(fnode, chap_auth, ISCSI_FLASHNODE_CHAP_AUTH_EN);
+iscsi_flashnode_sess_attr(fnode, discovery_logout,
+			  ISCSI_FLASHNODE_DISCOVERY_LOGOUT_EN);
+iscsi_flashnode_sess_attr(fnode, bidi_chap, ISCSI_FLASHNODE_BIDI_CHAP_EN);
+iscsi_flashnode_sess_attr(fnode, discovery_auth_optional,
+			  ISCSI_FLASHNODE_DISCOVERY_AUTH_OPTIONAL);
+iscsi_flashnode_sess_attr(fnode, erl, ISCSI_FLASHNODE_ERL);
+iscsi_flashnode_sess_attr(fnode, first_burst_len, ISCSI_FLASHNODE_FIRST_BURST);
+iscsi_flashnode_sess_attr(fnode, def_time2wait, ISCSI_FLASHNODE_DEF_TIME2WAIT);
+iscsi_flashnode_sess_attr(fnode, def_time2retain,
+			  ISCSI_FLASHNODE_DEF_TIME2RETAIN);
+iscsi_flashnode_sess_attr(fnode, max_outstanding_r2t, ISCSI_FLASHNODE_MAX_R2T);
+iscsi_flashnode_sess_attr(fnode, isid, ISCSI_FLASHNODE_ISID);
+iscsi_flashnode_sess_attr(fnode, tsid, ISCSI_FLASHNODE_TSID);
+iscsi_flashnode_sess_attr(fnode, max_burst_len, ISCSI_FLASHNODE_MAX_BURST);
+iscsi_flashnode_sess_attr(fnode, def_taskmgmt_tmo,
+			  ISCSI_FLASHNODE_DEF_TASKMGMT_TMO);
+iscsi_flashnode_sess_attr(fnode, targetalias, ISCSI_FLASHNODE_ALIAS);
+iscsi_flashnode_sess_attr(fnode, targetname, ISCSI_FLASHNODE_NAME);
+iscsi_flashnode_sess_attr(fnode, tpgt, ISCSI_FLASHNODE_TPGT);
+iscsi_flashnode_sess_attr(fnode, discovery_parent_idx,
+			  ISCSI_FLASHNODE_DISCOVERY_PARENT_IDX);
+iscsi_flashnode_sess_attr(fnode, discovery_parent_type,
+			  ISCSI_FLASHNODE_DISCOVERY_PARENT_TYPE);
+iscsi_flashnode_sess_attr(fnode, chap_in_idx, ISCSI_FLASHNODE_CHAP_IN_IDX);
+iscsi_flashnode_sess_attr(fnode, chap_out_idx, ISCSI_FLASHNODE_CHAP_OUT_IDX);
+iscsi_flashnode_sess_attr(fnode, username, ISCSI_FLASHNODE_USERNAME);
+iscsi_flashnode_sess_attr(fnode, username_in, ISCSI_FLASHNODE_USERNAME_IN);
+iscsi_flashnode_sess_attr(fnode, password, ISCSI_FLASHNODE_PASSWORD);
+iscsi_flashnode_sess_attr(fnode, password_in, ISCSI_FLASHNODE_PASSWORD_IN);
+iscsi_flashnode_sess_attr(fnode, is_boot_target, ISCSI_FLASHNODE_IS_BOOT_TGT);
+
+static struct attribute *iscsi_flashnode_sess_attrs[] = {
+	&dev_attr_fnode_auto_snd_tgt_disable.attr,
+	&dev_attr_fnode_discovery_session.attr,
+	&dev_attr_fnode_portal_type.attr,
+	&dev_attr_fnode_entry_enable.attr,
+	&dev_attr_fnode_immediate_data.attr,
+	&dev_attr_fnode_initial_r2t.attr,
+	&dev_attr_fnode_data_seq_in_order.attr,
+	&dev_attr_fnode_data_pdu_in_order.attr,
+	&dev_attr_fnode_chap_auth.attr,
+	&dev_attr_fnode_discovery_logout.attr,
+	&dev_attr_fnode_bidi_chap.attr,
+	&dev_attr_fnode_discovery_auth_optional.attr,
+	&dev_attr_fnode_erl.attr,
+	&dev_attr_fnode_first_burst_len.attr,
+	&dev_attr_fnode_def_time2wait.attr,
+	&dev_attr_fnode_def_time2retain.attr,
+	&dev_attr_fnode_max_outstanding_r2t.attr,
+	&dev_attr_fnode_isid.attr,
+	&dev_attr_fnode_tsid.attr,
+	&dev_attr_fnode_max_burst_len.attr,
+	&dev_attr_fnode_def_taskmgmt_tmo.attr,
+	&dev_attr_fnode_targetalias.attr,
+	&dev_attr_fnode_targetname.attr,
+	&dev_attr_fnode_tpgt.attr,
+	&dev_attr_fnode_discovery_parent_idx.attr,
+	&dev_attr_fnode_discovery_parent_type.attr,
+	&dev_attr_fnode_chap_in_idx.attr,
+	&dev_attr_fnode_chap_out_idx.attr,
+	&dev_attr_fnode_username.attr,
+	&dev_attr_fnode_username_in.attr,
+	&dev_attr_fnode_password.attr,
+	&dev_attr_fnode_password_in.attr,
+	&dev_attr_fnode_is_boot_target.attr,
+	NULL,
+};
+
+static umode_t iscsi_flashnode_sess_attr_is_visible(struct kobject *kobj,
+						    struct attribute *attr,
+						    int i)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct iscsi_bus_flash_session *fnode_sess =
+						iscsi_dev_to_flash_session(dev);
+	struct iscsi_transport *t = fnode_sess->transport;
+	int param;
+
+	if (attr == &dev_attr_fnode_auto_snd_tgt_disable.attr) {
+		param = ISCSI_FLASHNODE_AUTO_SND_TGT_DISABLE;
+	} else if (attr == &dev_attr_fnode_discovery_session.attr) {
+		param = ISCSI_FLASHNODE_DISCOVERY_SESS;
+	} else if (attr == &dev_attr_fnode_portal_type.attr) {
+		param = ISCSI_FLASHNODE_PORTAL_TYPE;
+	} else if (attr == &dev_attr_fnode_entry_enable.attr) {
+		param = ISCSI_FLASHNODE_ENTRY_EN;
+	} else if (attr == &dev_attr_fnode_immediate_data.attr) {
+		param = ISCSI_FLASHNODE_IMM_DATA_EN;
+	} else if (attr == &dev_attr_fnode_initial_r2t.attr) {
+		param = ISCSI_FLASHNODE_INITIAL_R2T_EN;
+	} else if (attr == &dev_attr_fnode_data_seq_in_order.attr) {
+		param = ISCSI_FLASHNODE_DATASEQ_INORDER;
+	} else if (attr == &dev_attr_fnode_data_pdu_in_order.attr) {
+		param = ISCSI_FLASHNODE_PDU_INORDER;
+	} else if (attr == &dev_attr_fnode_chap_auth.attr) {
+		param = ISCSI_FLASHNODE_CHAP_AUTH_EN;
+	} else if (attr == &dev_attr_fnode_discovery_logout.attr) {
+		param = ISCSI_FLASHNODE_DISCOVERY_LOGOUT_EN;
+	} else if (attr == &dev_attr_fnode_bidi_chap.attr) {
+		param = ISCSI_FLASHNODE_BIDI_CHAP_EN;
+	} else if (attr == &dev_attr_fnode_discovery_auth_optional.attr) {
+		param = ISCSI_FLASHNODE_DISCOVERY_AUTH_OPTIONAL;
+	} else if (attr == &dev_attr_fnode_erl.attr) {
+		param = ISCSI_FLASHNODE_ERL;
+	} else if (attr == &dev_attr_fnode_first_burst_len.attr) {
+		param = ISCSI_FLASHNODE_FIRST_BURST;
+	} else if (attr == &dev_attr_fnode_def_time2wait.attr) {
+		param = ISCSI_FLASHNODE_DEF_TIME2WAIT;
+	} else if (attr == &dev_attr_fnode_def_time2retain.attr) {
+		param = ISCSI_FLASHNODE_DEF_TIME2RETAIN;
+	} else if (attr == &dev_attr_fnode_max_outstanding_r2t.attr) {
+		param = ISCSI_FLASHNODE_MAX_R2T;
+	} else if (attr == &dev_attr_fnode_isid.attr) {
+		param = ISCSI_FLASHNODE_ISID;
+	} else if (attr == &dev_attr_fnode_tsid.attr) {
+		param = ISCSI_FLASHNODE_TSID;
+	} else if (attr == &dev_attr_fnode_max_burst_len.attr) {
+		param = ISCSI_FLASHNODE_MAX_BURST;
+	} else if (attr == &dev_attr_fnode_def_taskmgmt_tmo.attr) {
+		param = ISCSI_FLASHNODE_DEF_TASKMGMT_TMO;
+	} else if (attr == &dev_attr_fnode_targetalias.attr) {
+		param = ISCSI_FLASHNODE_ALIAS;
+	} else if (attr == &dev_attr_fnode_targetname.attr) {
+		param = ISCSI_FLASHNODE_NAME;
+	} else if (attr == &dev_attr_fnode_tpgt.attr) {
+		param = ISCSI_FLASHNODE_TPGT;
+	} else if (attr == &dev_attr_fnode_discovery_parent_idx.attr) {
+		param = ISCSI_FLASHNODE_DISCOVERY_PARENT_IDX;
+	} else if (attr == &dev_attr_fnode_discovery_parent_type.attr) {
+		param = ISCSI_FLASHNODE_DISCOVERY_PARENT_TYPE;
+	} else if (attr == &dev_attr_fnode_chap_in_idx.attr) {
+		param = ISCSI_FLASHNODE_CHAP_IN_IDX;
+	} else if (attr == &dev_attr_fnode_chap_out_idx.attr) {
+		param = ISCSI_FLASHNODE_CHAP_OUT_IDX;
+	} else if (attr == &dev_attr_fnode_username.attr) {
+		param = ISCSI_FLASHNODE_USERNAME;
+	} else if (attr == &dev_attr_fnode_username_in.attr) {
+		param = ISCSI_FLASHNODE_USERNAME_IN;
+	} else if (attr == &dev_attr_fnode_password.attr) {
+		param = ISCSI_FLASHNODE_PASSWORD;
+	} else if (attr == &dev_attr_fnode_password_in.attr) {
+		param = ISCSI_FLASHNODE_PASSWORD_IN;
+	} else if (attr == &dev_attr_fnode_is_boot_target.attr) {
+		param = ISCSI_FLASHNODE_IS_BOOT_TGT;
+	} else {
+		WARN_ONCE(1, "Invalid flashnode session attr");
+		return 0;
+	}
+
+	return t->attr_is_visible(ISCSI_FLASHNODE_PARAM, param);
+}
+
+static struct attribute_group iscsi_flashnode_sess_attr_group = {
+	.attrs = iscsi_flashnode_sess_attrs,
+	.is_visible = iscsi_flashnode_sess_attr_is_visible,
+};
+
+static const struct attribute_group *iscsi_flashnode_sess_attr_groups[] = {
+	&iscsi_flashnode_sess_attr_group,
+	NULL,
+};
+
+static void iscsi_flashnode_sess_release(struct device *dev)
+{
+	struct iscsi_bus_flash_session *fnode_sess =
+						iscsi_dev_to_flash_session(dev);
+
+	kfree(fnode_sess->targetname);
+	kfree(fnode_sess->targetalias);
+	kfree(fnode_sess->portal_type);
+	kfree(fnode_sess);
+}
+
+struct device_type iscsi_flashnode_sess_dev_type = {
+	.name = "iscsi_flashnode_sess_dev_type",
+	.groups = iscsi_flashnode_sess_attr_groups,
+	.release = iscsi_flashnode_sess_release,
+};
+
+/* flash node connection attrs show */
+#define iscsi_flashnode_conn_attr_show(type, name, param)		\
+static ssize_t								\
+show_##type##_##name(struct device *dev, struct device_attribute *attr,	\
+		     char *buf)						\
+{									\
+	struct iscsi_bus_flash_conn *fnode_conn = iscsi_dev_to_flash_conn(dev);\
+	struct iscsi_bus_flash_session *fnode_sess =			\
+				iscsi_flash_conn_to_flash_session(fnode_conn);\
+	struct iscsi_transport *t = fnode_conn->transport;		\
+	return t->get_flashnode_param(fnode_sess, param, buf);		\
+}									\
+
+
+#define iscsi_flashnode_conn_attr(type, name, param)			\
+	iscsi_flashnode_conn_attr_show(type, name, param)		\
+static ISCSI_FLASHNODE_ATTR(type, name, S_IRUGO,			\
+			    show_##type##_##name, NULL);
+
+/* Flash node connection attributes */
+
+iscsi_flashnode_conn_attr(fnode, is_fw_assigned_ipv6,
+			  ISCSI_FLASHNODE_IS_FW_ASSIGNED_IPV6);
+iscsi_flashnode_conn_attr(fnode, header_digest, ISCSI_FLASHNODE_HDR_DGST_EN);
+iscsi_flashnode_conn_attr(fnode, data_digest, ISCSI_FLASHNODE_DATA_DGST_EN);
+iscsi_flashnode_conn_attr(fnode, snack_req, ISCSI_FLASHNODE_SNACK_REQ_EN);
+iscsi_flashnode_conn_attr(fnode, tcp_timestamp_stat,
+			  ISCSI_FLASHNODE_TCP_TIMESTAMP_STAT);
+iscsi_flashnode_conn_attr(fnode, tcp_nagle_disable,
+			  ISCSI_FLASHNODE_TCP_NAGLE_DISABLE);
+iscsi_flashnode_conn_attr(fnode, tcp_wsf_disable,
+			  ISCSI_FLASHNODE_TCP_WSF_DISABLE);
+iscsi_flashnode_conn_attr(fnode, tcp_timer_scale,
+			  ISCSI_FLASHNODE_TCP_TIMER_SCALE);
+iscsi_flashnode_conn_attr(fnode, tcp_timestamp_enable,
+			  ISCSI_FLASHNODE_TCP_TIMESTAMP_EN);
+iscsi_flashnode_conn_attr(fnode, fragment_disable,
+			  ISCSI_FLASHNODE_IP_FRAG_DISABLE);
+iscsi_flashnode_conn_attr(fnode, keepalive_tmo, ISCSI_FLASHNODE_KEEPALIVE_TMO);
+iscsi_flashnode_conn_attr(fnode, port, ISCSI_FLASHNODE_PORT);
+iscsi_flashnode_conn_attr(fnode, ipaddress, ISCSI_FLASHNODE_IPADDR);
+iscsi_flashnode_conn_attr(fnode, max_recv_dlength,
+			  ISCSI_FLASHNODE_MAX_RECV_DLENGTH);
+iscsi_flashnode_conn_attr(fnode, max_xmit_dlength,
+			  ISCSI_FLASHNODE_MAX_XMIT_DLENGTH);
+iscsi_flashnode_conn_attr(fnode, local_port, ISCSI_FLASHNODE_LOCAL_PORT);
+iscsi_flashnode_conn_attr(fnode, ipv4_tos, ISCSI_FLASHNODE_IPV4_TOS);
+iscsi_flashnode_conn_attr(fnode, ipv6_traffic_class, ISCSI_FLASHNODE_IPV6_TC);
+iscsi_flashnode_conn_attr(fnode, ipv6_flow_label,
+			  ISCSI_FLASHNODE_IPV6_FLOW_LABEL);
+iscsi_flashnode_conn_attr(fnode, redirect_ipaddr,
+			  ISCSI_FLASHNODE_REDIRECT_IPADDR);
+iscsi_flashnode_conn_attr(fnode, max_segment_size,
+			  ISCSI_FLASHNODE_MAX_SEGMENT_SIZE);
+iscsi_flashnode_conn_attr(fnode, link_local_ipv6,
+			  ISCSI_FLASHNODE_LINK_LOCAL_IPV6);
+iscsi_flashnode_conn_attr(fnode, tcp_xmit_wsf, ISCSI_FLASHNODE_TCP_XMIT_WSF);
+iscsi_flashnode_conn_attr(fnode, tcp_recv_wsf, ISCSI_FLASHNODE_TCP_RECV_WSF);
+iscsi_flashnode_conn_attr(fnode, statsn, ISCSI_FLASHNODE_STATSN);
+iscsi_flashnode_conn_attr(fnode, exp_statsn, ISCSI_FLASHNODE_EXP_STATSN);
+
+static struct attribute *iscsi_flashnode_conn_attrs[] = {
+	&dev_attr_fnode_is_fw_assigned_ipv6.attr,
+	&dev_attr_fnode_header_digest.attr,
+	&dev_attr_fnode_data_digest.attr,
+	&dev_attr_fnode_snack_req.attr,
+	&dev_attr_fnode_tcp_timestamp_stat.attr,
+	&dev_attr_fnode_tcp_nagle_disable.attr,
+	&dev_attr_fnode_tcp_wsf_disable.attr,
+	&dev_attr_fnode_tcp_timer_scale.attr,
+	&dev_attr_fnode_tcp_timestamp_enable.attr,
+	&dev_attr_fnode_fragment_disable.attr,
+	&dev_attr_fnode_max_recv_dlength.attr,
+	&dev_attr_fnode_max_xmit_dlength.attr,
+	&dev_attr_fnode_keepalive_tmo.attr,
+	&dev_attr_fnode_port.attr,
+	&dev_attr_fnode_ipaddress.attr,
+	&dev_attr_fnode_redirect_ipaddr.attr,
+	&dev_attr_fnode_max_segment_size.attr,
+	&dev_attr_fnode_local_port.attr,
+	&dev_attr_fnode_ipv4_tos.attr,
+	&dev_attr_fnode_ipv6_traffic_class.attr,
+	&dev_attr_fnode_ipv6_flow_label.attr,
+	&dev_attr_fnode_link_local_ipv6.attr,
+	&dev_attr_fnode_tcp_xmit_wsf.attr,
+	&dev_attr_fnode_tcp_recv_wsf.attr,
+	&dev_attr_fnode_statsn.attr,
+	&dev_attr_fnode_exp_statsn.attr,
+	NULL,
+};
+
+static umode_t iscsi_flashnode_conn_attr_is_visible(struct kobject *kobj,
+						    struct attribute *attr,
+						    int i)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct iscsi_bus_flash_conn *fnode_conn = iscsi_dev_to_flash_conn(dev);
+	struct iscsi_transport *t = fnode_conn->transport;
+	int param;
+
+	if (attr == &dev_attr_fnode_is_fw_assigned_ipv6.attr) {
+		param = ISCSI_FLASHNODE_IS_FW_ASSIGNED_IPV6;
+	} else if (attr == &dev_attr_fnode_header_digest.attr) {
+		param = ISCSI_FLASHNODE_HDR_DGST_EN;
+	} else if (attr == &dev_attr_fnode_data_digest.attr) {
+		param = ISCSI_FLASHNODE_DATA_DGST_EN;
+	} else if (attr == &dev_attr_fnode_snack_req.attr) {
+		param = ISCSI_FLASHNODE_SNACK_REQ_EN;
+	} else if (attr == &dev_attr_fnode_tcp_timestamp_stat.attr) {
+		param = ISCSI_FLASHNODE_TCP_TIMESTAMP_STAT;
+	} else if (attr == &dev_attr_fnode_tcp_nagle_disable.attr) {
+		param = ISCSI_FLASHNODE_TCP_NAGLE_DISABLE;
+	} else if (attr == &dev_attr_fnode_tcp_wsf_disable.attr) {
+		param = ISCSI_FLASHNODE_TCP_WSF_DISABLE;
+	} else if (attr == &dev_attr_fnode_tcp_timer_scale.attr) {
+		param = ISCSI_FLASHNODE_TCP_TIMER_SCALE;
+	} else if (attr == &dev_attr_fnode_tcp_timestamp_enable.attr) {
+		param = ISCSI_FLASHNODE_TCP_TIMESTAMP_EN;
+	} else if (attr == &dev_attr_fnode_fragment_disable.attr) {
+		param = ISCSI_FLASHNODE_IP_FRAG_DISABLE;
+	} else if (attr == &dev_attr_fnode_max_recv_dlength.attr) {
+		param = ISCSI_FLASHNODE_MAX_RECV_DLENGTH;
+	} else if (attr == &dev_attr_fnode_max_xmit_dlength.attr) {
+		param = ISCSI_FLASHNODE_MAX_XMIT_DLENGTH;
+	} else if (attr == &dev_attr_fnode_keepalive_tmo.attr) {
+		param = ISCSI_FLASHNODE_KEEPALIVE_TMO;
+	} else if (attr == &dev_attr_fnode_port.attr) {
+		param = ISCSI_FLASHNODE_PORT;
+	} else if (attr == &dev_attr_fnode_ipaddress.attr) {
+		param = ISCSI_FLASHNODE_IPADDR;
+	} else if (attr == &dev_attr_fnode_redirect_ipaddr.attr) {
+		param = ISCSI_FLASHNODE_REDIRECT_IPADDR;
+	} else if (attr == &dev_attr_fnode_max_segment_size.attr) {
+		param = ISCSI_FLASHNODE_MAX_SEGMENT_SIZE;
+	} else if (attr == &dev_attr_fnode_local_port.attr) {
+		param = ISCSI_FLASHNODE_LOCAL_PORT;
+	} else if (attr == &dev_attr_fnode_ipv4_tos.attr) {
+		param = ISCSI_FLASHNODE_IPV4_TOS;
+	} else if (attr == &dev_attr_fnode_ipv6_traffic_class.attr) {
+		param = ISCSI_FLASHNODE_IPV6_TC;
+	} else if (attr == &dev_attr_fnode_ipv6_flow_label.attr) {
+		param = ISCSI_FLASHNODE_IPV6_FLOW_LABEL;
+	} else if (attr == &dev_attr_fnode_link_local_ipv6.attr) {
+		param = ISCSI_FLASHNODE_LINK_LOCAL_IPV6;
+	} else if (attr == &dev_attr_fnode_tcp_xmit_wsf.attr) {
+		param = ISCSI_FLASHNODE_TCP_XMIT_WSF;
+	} else if (attr == &dev_attr_fnode_tcp_recv_wsf.attr) {
+		param = ISCSI_FLASHNODE_TCP_RECV_WSF;
+	} else if (attr == &dev_attr_fnode_statsn.attr) {
+		param = ISCSI_FLASHNODE_STATSN;
+	} else if (attr == &dev_attr_fnode_exp_statsn.attr) {
+		param = ISCSI_FLASHNODE_EXP_STATSN;
+	} else {
+		WARN_ONCE(1, "Invalid flashnode connection attr");
+		return 0;
+	}
+
+	return t->attr_is_visible(ISCSI_FLASHNODE_PARAM, param);
+}
+
+static struct attribute_group iscsi_flashnode_conn_attr_group = {
+	.attrs = iscsi_flashnode_conn_attrs,
+	.is_visible = iscsi_flashnode_conn_attr_is_visible,
+};
+
+static const struct attribute_group *iscsi_flashnode_conn_attr_groups[] = {
+	&iscsi_flashnode_conn_attr_group,
+	NULL,
+};
+
+static void iscsi_flashnode_conn_release(struct device *dev)
+{
+	struct iscsi_bus_flash_conn *fnode_conn = iscsi_dev_to_flash_conn(dev);
+
+	kfree(fnode_conn->ipaddress);
+	kfree(fnode_conn->redirect_ipaddr);
+	kfree(fnode_conn->link_local_ipv6_addr);
+	kfree(fnode_conn);
+}
+
+struct device_type iscsi_flashnode_conn_dev_type = {
+	.name = "iscsi_flashnode_conn_dev_type",
+	.groups = iscsi_flashnode_conn_attr_groups,
+	.release = iscsi_flashnode_conn_release,
+};
+
+struct bus_type iscsi_flashnode_bus;
+
+int iscsi_flashnode_bus_match(struct device *dev,
+				     struct device_driver *drv)
+{
+	if (dev->bus == &iscsi_flashnode_bus)
+		return 1;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(iscsi_flashnode_bus_match);
+
+struct bus_type iscsi_flashnode_bus = {
+	.name = "iscsi_flashnode",
+	.match = &iscsi_flashnode_bus_match,
+};
+
+/**
+ * iscsi_create_flashnode_sess - Add flashnode session entry in sysfs
+ * @shost: pointer to host data
+ * @index: index of flashnode to add in sysfs
+ * @transport: pointer to transport data
+ * @dd_size: total size to allocate
+ *
+ * Adds a sysfs entry for the flashnode session attributes
+ *
+ * Returns:
+ *  pointer to allocated flashnode sess on sucess
+ *  %NULL on failure
+ */
+struct iscsi_bus_flash_session *
+iscsi_create_flashnode_sess(struct Scsi_Host *shost, int index,
+			    struct iscsi_transport *transport,
+			    int dd_size)
+{
+	struct iscsi_bus_flash_session *fnode_sess;
+	int err;
+
+	fnode_sess = kzalloc(sizeof(*fnode_sess) + dd_size, GFP_KERNEL);
+	if (!fnode_sess)
+		return NULL;
+
+	fnode_sess->transport = transport;
+	fnode_sess->target_id = index;
+	fnode_sess->dev.type = &iscsi_flashnode_sess_dev_type;
+	fnode_sess->dev.bus = &iscsi_flashnode_bus;
+	fnode_sess->dev.parent = &shost->shost_gendev;
+	dev_set_name(&fnode_sess->dev, "flashnode_sess-%u:%u",
+		     shost->host_no, index);
+
+	err = device_register(&fnode_sess->dev);
+	if (err)
+		goto free_fnode_sess;
+
+	if (dd_size)
+		fnode_sess->dd_data = &fnode_sess[1];
+
+	return fnode_sess;
+
+free_fnode_sess:
+	kfree(fnode_sess);
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(iscsi_create_flashnode_sess);
+
+/**
+ * iscsi_create_flashnode_conn - Add flashnode conn entry in sysfs
+ * @shost: pointer to host data
+ * @fnode_sess: pointer to the parent flashnode session entry
+ * @transport: pointer to transport data
+ * @dd_size: total size to allocate
+ *
+ * Adds a sysfs entry for the flashnode connection attributes
+ *
+ * Returns:
+ *  pointer to allocated flashnode conn on success
+ *  %NULL on failure
+ */
+struct iscsi_bus_flash_conn *
+iscsi_create_flashnode_conn(struct Scsi_Host *shost,
+			    struct iscsi_bus_flash_session *fnode_sess,
+			    struct iscsi_transport *transport,
+			    int dd_size)
+{
+	struct iscsi_bus_flash_conn *fnode_conn;
+	int err;
+
+	fnode_conn = kzalloc(sizeof(*fnode_conn) + dd_size, GFP_KERNEL);
+	if (!fnode_conn)
+		return NULL;
+
+	fnode_conn->transport = transport;
+	fnode_conn->dev.type = &iscsi_flashnode_conn_dev_type;
+	fnode_conn->dev.bus = &iscsi_flashnode_bus;
+	fnode_conn->dev.parent = &fnode_sess->dev;
+	dev_set_name(&fnode_conn->dev, "flashnode_conn-%u:%u:0",
+		     shost->host_no, fnode_sess->target_id);
+
+	err = device_register(&fnode_conn->dev);
+	if (err)
+		goto free_fnode_conn;
+
+	if (dd_size)
+		fnode_conn->dd_data = &fnode_conn[1];
+
+	return fnode_conn;
+
+free_fnode_conn:
+	kfree(fnode_conn);
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(iscsi_create_flashnode_conn);
+
+/**
+ * iscsi_is_flashnode_conn_dev - verify passed device is to be flashnode conn
+ * @dev: device to verify
+ * @data: pointer to data containing value to use for verification
+ *
+ * Verifies if the passed device is flashnode conn device
+ *
+ * Returns:
+ *  1 on success
+ *  0 on failure
+ */
+int iscsi_is_flashnode_conn_dev(struct device *dev, void *data)
+{
+	return dev->bus == &iscsi_flashnode_bus;
+}
+EXPORT_SYMBOL_GPL(iscsi_is_flashnode_conn_dev);
+
+static int iscsi_destroy_flashnode_conn(struct iscsi_bus_flash_conn *fnode_conn)
+{
+	device_unregister(&fnode_conn->dev);
+	return 0;
+}
+
+static int flashnode_match_index(struct device *dev, void *data)
+{
+	struct iscsi_bus_flash_session *fnode_sess = NULL;
+	int ret = 0;
+
+	if (!iscsi_flashnode_bus_match(dev, NULL))
+		goto exit_match_index;
+
+	fnode_sess = iscsi_dev_to_flash_session(dev);
+	ret = (fnode_sess->target_id == *((int *)data)) ? 1 : 0;
+
+exit_match_index:
+	return ret;
+}
+
+/**
+ * iscsi_get_flashnode_by_index -finds flashnode session entry by index
+ * @shost: pointer to host data
+ * @data: pointer to data containing value to use for comparison
+ * @fn: function pointer that does actual comparison
+ *
+ * Finds the flashnode session object for the passed index
+ *
+ * Returns:
+ *  pointer to found flashnode session object on success
+ *  %NULL on failure
+ */
+static struct iscsi_bus_flash_session *
+iscsi_get_flashnode_by_index(struct Scsi_Host *shost, void *data,
+			     int (*fn)(struct device *dev, void *data))
+{
+	struct iscsi_bus_flash_session *fnode_sess = NULL;
+	struct device *dev;
+
+	dev = device_find_child(&shost->shost_gendev, data, fn);
+	if (dev)
+		fnode_sess = iscsi_dev_to_flash_session(dev);
+
+	return fnode_sess;
+}
+
+/**
+ * iscsi_find_flashnode_sess - finds flashnode session entry
+ * @shost: pointer to host data
+ * @data: pointer to data containing value to use for comparison
+ * @fn: function pointer that does actual comparison
+ *
+ * Finds the flashnode session object comparing the data passed using logic
+ * defined in passed function pointer
+ *
+ * Returns:
+ *  pointer to found flashnode session device object on success
+ *  %NULL on failure
+ */
+struct device *
+iscsi_find_flashnode_sess(struct Scsi_Host *shost, void *data,
+			  int (*fn)(struct device *dev, void *data))
+{
+	struct device *dev;
+
+	dev = device_find_child(&shost->shost_gendev, data, fn);
+	return dev;
+}
+EXPORT_SYMBOL_GPL(iscsi_find_flashnode_sess);
+
+/**
+ * iscsi_find_flashnode_conn - finds flashnode connection entry
+ * @fnode_sess: pointer to parent flashnode session entry
+ * @data: pointer to data containing value to use for comparison
+ * @fn: function pointer that does actual comparison
+ *
+ * Finds the flashnode connection object comparing the data passed using logic
+ * defined in passed function pointer
+ *
+ * Returns:
+ *  pointer to found flashnode connection device object on success
+ *  %NULL on failure
+ */
+struct device *
+iscsi_find_flashnode_conn(struct iscsi_bus_flash_session *fnode_sess,
+			  void *data,
+			  int (*fn)(struct device *dev, void *data))
+{
+	struct device *dev;
+
+	dev = device_find_child(&fnode_sess->dev, data, fn);
+	return dev;
+}
+EXPORT_SYMBOL_GPL(iscsi_find_flashnode_conn);
+
+static int iscsi_iter_destroy_flashnode_conn_fn(struct device *dev, void *data)
+{
+	if (!iscsi_is_flashnode_conn_dev(dev, NULL))
+		return 0;
+
+	return iscsi_destroy_flashnode_conn(iscsi_dev_to_flash_conn(dev));
+}
+
+/**
+ * iscsi_destroy_flashnode_sess - destory flashnode session entry
+ * @fnode_sess: pointer to flashnode session entry to be destroyed
+ *
+ * Deletes the flashnode session entry and all children flashnode connection
+ * entries from sysfs
+ */
+void iscsi_destroy_flashnode_sess(struct iscsi_bus_flash_session *fnode_sess)
+{
+	int err;
+
+	err = device_for_each_child(&fnode_sess->dev, NULL,
+				    iscsi_iter_destroy_flashnode_conn_fn);
+	if (err)
+		pr_err("Could not delete all connections for %s. Error %d.\n",
+		       fnode_sess->dev.kobj.name, err);
+
+	device_unregister(&fnode_sess->dev);
+}
+EXPORT_SYMBOL_GPL(iscsi_destroy_flashnode_sess);
+
+static int iscsi_iter_destroy_flashnode_fn(struct device *dev, void *data)
+{
+	if (!iscsi_flashnode_bus_match(dev, NULL))
+		return 0;
+
+	iscsi_destroy_flashnode_sess(iscsi_dev_to_flash_session(dev));
+	return 0;
+}
+
+/**
+ * iscsi_destroy_all_flashnode - destory all flashnode session entries
+ * @shost: pointer to host data
+ *
+ * Destroys all the flashnode session entries and all corresponding children
+ * flashnode connection entries from sysfs
+ */
+void iscsi_destroy_all_flashnode(struct Scsi_Host *shost)
+{
+	device_for_each_child(&shost->shost_gendev, NULL,
+			      iscsi_iter_destroy_flashnode_fn);
+}
+EXPORT_SYMBOL_GPL(iscsi_destroy_all_flashnode);
+
+/*
  * BSG support
  */
 /**
@@ -1344,8 +2028,8 @@ int iscsi_recv_pdu(struct iscsi_cls_conn *conn, struct iscsi_hdr *hdr,
 	struct iscsi_uevent *ev;
 	char *pdu;
 	struct iscsi_internal *priv;
-	int len = NLMSG_SPACE(sizeof(*ev) + sizeof(struct iscsi_hdr) +
-			      data_size);
+	int len = nlmsg_total_size(sizeof(*ev) + sizeof(struct iscsi_hdr) +
+				   data_size);
 
 	priv = iscsi_if_transport_lookup(conn->transport);
 	if (!priv)
@@ -1360,7 +2044,7 @@ int iscsi_recv_pdu(struct iscsi_cls_conn *conn, struct iscsi_hdr *hdr,
 	}
 
 	nlh = __nlmsg_put(skb, 0, 0, 0, (len - sizeof(*nlh)), 0);
-	ev = NLMSG_DATA(nlh);
+	ev = nlmsg_data(nlh);
 	memset(ev, 0, sizeof(*ev));
 	ev->transport_handle = iscsi_handle(conn->transport);
 	ev->type = ISCSI_KEVENT_RECV_PDU;
@@ -1381,7 +2065,7 @@ int iscsi_offload_mesg(struct Scsi_Host *shost,
 	struct nlmsghdr	*nlh;
 	struct sk_buff *skb;
 	struct iscsi_uevent *ev;
-	int len = NLMSG_SPACE(sizeof(*ev) + data_size);
+	int len = nlmsg_total_size(sizeof(*ev) + data_size);
 
 	skb = alloc_skb(len, GFP_ATOMIC);
 	if (!skb) {
@@ -1390,7 +2074,7 @@ int iscsi_offload_mesg(struct Scsi_Host *shost,
 	}
 
 	nlh = __nlmsg_put(skb, 0, 0, 0, (len - sizeof(*nlh)), 0);
-	ev = NLMSG_DATA(nlh);
+	ev = nlmsg_data(nlh);
 	memset(ev, 0, sizeof(*ev));
 	ev->type = type;
 	ev->transport_handle = iscsi_handle(transport);
@@ -1415,7 +2099,7 @@ void iscsi_conn_error_event(struct iscsi_cls_conn *conn, enum iscsi_err error)
 	struct sk_buff	*skb;
 	struct iscsi_uevent *ev;
 	struct iscsi_internal *priv;
-	int len = NLMSG_SPACE(sizeof(*ev));
+	int len = nlmsg_total_size(sizeof(*ev));
 
 	priv = iscsi_if_transport_lookup(conn->transport);
 	if (!priv)
@@ -1429,7 +2113,7 @@ void iscsi_conn_error_event(struct iscsi_cls_conn *conn, enum iscsi_err error)
 	}
 
 	nlh = __nlmsg_put(skb, 0, 0, 0, (len - sizeof(*nlh)), 0);
-	ev = NLMSG_DATA(nlh);
+	ev = nlmsg_data(nlh);
 	ev->transport_handle = iscsi_handle(conn->transport);
 	ev->type = ISCSI_KEVENT_CONN_ERROR;
 	ev->r.connerror.error = error;
@@ -1450,7 +2134,7 @@ void iscsi_conn_login_event(struct iscsi_cls_conn *conn,
 	struct sk_buff  *skb;
 	struct iscsi_uevent *ev;
 	struct iscsi_internal *priv;
-	int len = NLMSG_SPACE(sizeof(*ev));
+	int len = nlmsg_total_size(sizeof(*ev));
 
 	priv = iscsi_if_transport_lookup(conn->transport);
 	if (!priv)
@@ -1464,7 +2148,7 @@ void iscsi_conn_login_event(struct iscsi_cls_conn *conn,
 	}
 
 	nlh = __nlmsg_put(skb, 0, 0, 0, (len - sizeof(*nlh)), 0);
-	ev = NLMSG_DATA(nlh);
+	ev = nlmsg_data(nlh);
 	ev->transport_handle = iscsi_handle(conn->transport);
 	ev->type = ISCSI_KEVENT_CONN_LOGIN_STATE;
 	ev->r.conn_login.state = state;
@@ -1484,7 +2168,7 @@ void iscsi_post_host_event(uint32_t host_no, struct iscsi_transport *transport,
 	struct nlmsghdr *nlh;
 	struct sk_buff *skb;
 	struct iscsi_uevent *ev;
-	int len = NLMSG_SPACE(sizeof(*ev) + data_size);
+	int len = nlmsg_total_size(sizeof(*ev) + data_size);
 
 	skb = alloc_skb(len, GFP_NOIO);
 	if (!skb) {
@@ -1494,7 +2178,7 @@ void iscsi_post_host_event(uint32_t host_no, struct iscsi_transport *transport,
 	}
 
 	nlh = __nlmsg_put(skb, 0, 0, 0, (len - sizeof(*nlh)), 0);
-	ev = NLMSG_DATA(nlh);
+	ev = nlmsg_data(nlh);
 	ev->transport_handle = iscsi_handle(transport);
 	ev->type = ISCSI_KEVENT_HOST_EVENT;
 	ev->r.host_event.host_no = host_no;
@@ -1515,7 +2199,7 @@ void iscsi_ping_comp_event(uint32_t host_no, struct iscsi_transport *transport,
 	struct nlmsghdr *nlh;
 	struct sk_buff *skb;
 	struct iscsi_uevent *ev;
-	int len = NLMSG_SPACE(sizeof(*ev) + data_size);
+	int len = nlmsg_total_size(sizeof(*ev) + data_size);
 
 	skb = alloc_skb(len, GFP_NOIO);
 	if (!skb) {
@@ -1524,7 +2208,7 @@ void iscsi_ping_comp_event(uint32_t host_no, struct iscsi_transport *transport,
 	}
 
 	nlh = __nlmsg_put(skb, 0, 0, 0, (len - sizeof(*nlh)), 0);
-	ev = NLMSG_DATA(nlh);
+	ev = nlmsg_data(nlh);
 	ev->transport_handle = iscsi_handle(transport);
 	ev->type = ISCSI_KEVENT_PING_COMP;
 	ev->r.ping_comp.host_no = host_no;
@@ -1543,7 +2227,7 @@ iscsi_if_send_reply(uint32_t group, int seq, int type, int done, int multi,
 {
 	struct sk_buff	*skb;
 	struct nlmsghdr	*nlh;
-	int len = NLMSG_SPACE(size);
+	int len = nlmsg_total_size(size);
 	int flags = multi ? NLM_F_MULTI : 0;
 	int t = done ? NLMSG_DONE : type;
 
@@ -1555,24 +2239,24 @@ iscsi_if_send_reply(uint32_t group, int seq, int type, int done, int multi,
 
 	nlh = __nlmsg_put(skb, 0, 0, t, (len - sizeof(*nlh)), 0);
 	nlh->nlmsg_flags = flags;
-	memcpy(NLMSG_DATA(nlh), payload, size);
+	memcpy(nlmsg_data(nlh), payload, size);
 	return iscsi_multicast_skb(skb, group, GFP_ATOMIC);
 }
 
 static int
 iscsi_if_get_stats(struct iscsi_transport *transport, struct nlmsghdr *nlh)
 {
-	struct iscsi_uevent *ev = NLMSG_DATA(nlh);
+	struct iscsi_uevent *ev = nlmsg_data(nlh);
 	struct iscsi_stats *stats;
 	struct sk_buff *skbstat;
 	struct iscsi_cls_conn *conn;
 	struct nlmsghdr	*nlhstat;
 	struct iscsi_uevent *evstat;
 	struct iscsi_internal *priv;
-	int len = NLMSG_SPACE(sizeof(*ev) +
-			      sizeof(struct iscsi_stats) +
-			      sizeof(struct iscsi_stats_custom) *
-			      ISCSI_STATS_CUSTOM_MAX);
+	int len = nlmsg_total_size(sizeof(*ev) +
+				   sizeof(struct iscsi_stats) +
+				   sizeof(struct iscsi_stats_custom) *
+				   ISCSI_STATS_CUSTOM_MAX);
 	int err = 0;
 
 	priv = iscsi_if_transport_lookup(transport);
@@ -1595,7 +2279,7 @@ iscsi_if_get_stats(struct iscsi_transport *transport, struct nlmsghdr *nlh)
 
 		nlhstat = __nlmsg_put(skbstat, 0, 0, 0,
 				      (len - sizeof(*nlhstat)), 0);
-		evstat = NLMSG_DATA(nlhstat);
+		evstat = nlmsg_data(nlhstat);
 		memset(evstat, 0, sizeof(*evstat));
 		evstat->transport_handle = iscsi_handle(conn->transport);
 		evstat->type = nlh->nlmsg_type;
@@ -1608,12 +2292,12 @@ iscsi_if_get_stats(struct iscsi_transport *transport, struct nlmsghdr *nlh)
 		memset(stats, 0, sizeof(*stats));
 
 		transport->get_stats(conn, stats);
-		actual_size = NLMSG_SPACE(sizeof(struct iscsi_uevent) +
-					  sizeof(struct iscsi_stats) +
-					  sizeof(struct iscsi_stats_custom) *
-					  stats->custom_length);
+		actual_size = nlmsg_total_size(sizeof(struct iscsi_uevent) +
+					       sizeof(struct iscsi_stats) +
+					       sizeof(struct iscsi_stats_custom) *
+					       stats->custom_length);
 		actual_size -= sizeof(*nlhstat);
-		actual_size = NLMSG_LENGTH(actual_size);
+		actual_size = nlmsg_msg_size(actual_size);
 		skb_trim(skbstat, NLMSG_ALIGN(actual_size));
 		nlhstat->nlmsg_len = actual_size;
 
@@ -1637,7 +2321,7 @@ int iscsi_session_event(struct iscsi_cls_session *session,
 	struct iscsi_uevent *ev;
 	struct sk_buff  *skb;
 	struct nlmsghdr *nlh;
-	int rc, len = NLMSG_SPACE(sizeof(*ev));
+	int rc, len = nlmsg_total_size(sizeof(*ev));
 
 	priv = iscsi_if_transport_lookup(session->transport);
 	if (!priv)
@@ -1653,7 +2337,7 @@ int iscsi_session_event(struct iscsi_cls_session *session,
 	}
 
 	nlh = __nlmsg_put(skb, 0, 0, 0, (len - sizeof(*nlh)), 0);
-	ev = NLMSG_DATA(nlh);
+	ev = nlmsg_data(nlh);
 	ev->transport_handle = iscsi_handle(session->transport);
 
 	ev->type = event;
@@ -2005,7 +2689,7 @@ iscsi_send_ping(struct iscsi_transport *transport, struct iscsi_uevent *ev)
 static int
 iscsi_get_chap(struct iscsi_transport *transport, struct nlmsghdr *nlh)
 {
-	struct iscsi_uevent *ev = NLMSG_DATA(nlh);
+	struct iscsi_uevent *ev = nlmsg_data(nlh);
 	struct Scsi_Host *shost = NULL;
 	struct iscsi_chap_rec *chap_rec;
 	struct iscsi_internal *priv;
@@ -2024,7 +2708,7 @@ iscsi_get_chap(struct iscsi_transport *transport, struct nlmsghdr *nlh)
 		return -EINVAL;
 
 	chap_buf_size = (ev->u.get_chap.num_entries * sizeof(*chap_rec));
-	len = NLMSG_SPACE(sizeof(*ev) + chap_buf_size);
+	len = nlmsg_total_size(sizeof(*ev) + chap_buf_size);
 
 	shost = scsi_host_lookup(ev->u.get_chap.host_no);
 	if (!shost) {
@@ -2045,7 +2729,7 @@ iscsi_get_chap(struct iscsi_transport *transport, struct nlmsghdr *nlh)
 
 		nlhchap = __nlmsg_put(skbchap, 0, 0, 0,
 				      (len - sizeof(*nlhchap)), 0);
-		evchap = NLMSG_DATA(nlhchap);
+		evchap = nlmsg_data(nlhchap);
 		memset(evchap, 0, sizeof(*evchap));
 		evchap->transport_handle = iscsi_handle(transport);
 		evchap->type = nlh->nlmsg_type;
@@ -2058,7 +2742,7 @@ iscsi_get_chap(struct iscsi_transport *transport, struct nlmsghdr *nlh)
 		err = transport->get_chap(shost, ev->u.get_chap.chap_tbl_idx,
 				    &evchap->u.get_chap.num_entries, buf);
 
-		actual_size = NLMSG_SPACE(sizeof(*ev) + chap_buf_size);
+		actual_size = nlmsg_total_size(sizeof(*ev) + chap_buf_size);
 		skb_trim(skbchap, NLMSG_ALIGN(actual_size));
 		nlhchap->nlmsg_len = actual_size;
 
@@ -2092,11 +2776,299 @@ static int iscsi_delete_chap(struct iscsi_transport *transport,
 	return err;
 }
 
+static const struct {
+	enum iscsi_discovery_parent_type value;
+	char				*name;
+} iscsi_discovery_parent_names[] = {
+	{ISCSI_DISC_PARENT_UNKNOWN,	"Unknown" },
+	{ISCSI_DISC_PARENT_SENDTGT,	"Sendtarget" },
+	{ISCSI_DISC_PARENT_ISNS,	"isns" },
+};
+
+char *iscsi_get_discovery_parent_name(int parent_type)
+{
+	int i;
+	char *state = "Unknown!";
+
+	for (i = 0; i < ARRAY_SIZE(iscsi_discovery_parent_names); i++) {
+		if (iscsi_discovery_parent_names[i].value & parent_type) {
+			state = iscsi_discovery_parent_names[i].name;
+			break;
+		}
+	}
+	return state;
+}
+EXPORT_SYMBOL_GPL(iscsi_get_discovery_parent_name);
+
+static int iscsi_set_flashnode_param(struct iscsi_transport *transport,
+				     struct iscsi_uevent *ev, uint32_t len)
+{
+	char *data = (char *)ev + sizeof(*ev);
+	struct Scsi_Host *shost;
+	struct iscsi_bus_flash_session *fnode_sess;
+	struct iscsi_bus_flash_conn *fnode_conn;
+	struct device *dev;
+	uint32_t *idx;
+	int err = 0;
+
+	if (!transport->set_flashnode_param) {
+		err = -ENOSYS;
+		goto exit_set_fnode;
+	}
+
+	shost = scsi_host_lookup(ev->u.set_flashnode.host_no);
+	if (!shost) {
+		pr_err("%s could not find host no %u\n",
+		       __func__, ev->u.set_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	idx = &ev->u.set_flashnode.flashnode_idx;
+	fnode_sess = iscsi_get_flashnode_by_index(shost, idx,
+						  flashnode_match_index);
+	if (!fnode_sess) {
+		pr_err("%s could not find flashnode %u for host no %u\n",
+		       __func__, *idx, ev->u.set_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	dev = iscsi_find_flashnode_conn(fnode_sess, NULL,
+					iscsi_is_flashnode_conn_dev);
+	if (!dev) {
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	fnode_conn = iscsi_dev_to_flash_conn(dev);
+	err = transport->set_flashnode_param(fnode_sess, fnode_conn, data, len);
+
+put_host:
+	scsi_host_put(shost);
+
+exit_set_fnode:
+	return err;
+}
+
+static int iscsi_new_flashnode(struct iscsi_transport *transport,
+			       struct iscsi_uevent *ev, uint32_t len)
+{
+	char *data = (char *)ev + sizeof(*ev);
+	struct Scsi_Host *shost;
+	int index;
+	int err = 0;
+
+	if (!transport->new_flashnode) {
+		err = -ENOSYS;
+		goto exit_new_fnode;
+	}
+
+	shost = scsi_host_lookup(ev->u.new_flashnode.host_no);
+	if (!shost) {
+		pr_err("%s could not find host no %u\n",
+		       __func__, ev->u.new_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	index = transport->new_flashnode(shost, data, len);
+
+	if (index >= 0)
+		ev->r.new_flashnode_ret.flashnode_idx = index;
+	else
+		err = -EIO;
+
+put_host:
+	scsi_host_put(shost);
+
+exit_new_fnode:
+	return err;
+}
+
+static int iscsi_del_flashnode(struct iscsi_transport *transport,
+			       struct iscsi_uevent *ev)
+{
+	struct Scsi_Host *shost;
+	struct iscsi_bus_flash_session *fnode_sess;
+	uint32_t *idx;
+	int err = 0;
+
+	if (!transport->del_flashnode) {
+		err = -ENOSYS;
+		goto exit_del_fnode;
+	}
+
+	shost = scsi_host_lookup(ev->u.del_flashnode.host_no);
+	if (!shost) {
+		pr_err("%s could not find host no %u\n",
+		       __func__, ev->u.del_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	idx = &ev->u.del_flashnode.flashnode_idx;
+	fnode_sess = iscsi_get_flashnode_by_index(shost, idx,
+						  flashnode_match_index);
+	if (!fnode_sess) {
+		pr_err("%s could not find flashnode %u for host no %u\n",
+		       __func__, *idx, ev->u.del_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	err = transport->del_flashnode(fnode_sess);
+
+put_host:
+	scsi_host_put(shost);
+
+exit_del_fnode:
+	return err;
+}
+
+static int iscsi_login_flashnode(struct iscsi_transport *transport,
+				 struct iscsi_uevent *ev)
+{
+	struct Scsi_Host *shost;
+	struct iscsi_bus_flash_session *fnode_sess;
+	struct iscsi_bus_flash_conn *fnode_conn;
+	struct device *dev;
+	uint32_t *idx;
+	int err = 0;
+
+	if (!transport->login_flashnode) {
+		err = -ENOSYS;
+		goto exit_login_fnode;
+	}
+
+	shost = scsi_host_lookup(ev->u.login_flashnode.host_no);
+	if (!shost) {
+		pr_err("%s could not find host no %u\n",
+		       __func__, ev->u.login_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	idx = &ev->u.login_flashnode.flashnode_idx;
+	fnode_sess = iscsi_get_flashnode_by_index(shost, idx,
+						  flashnode_match_index);
+	if (!fnode_sess) {
+		pr_err("%s could not find flashnode %u for host no %u\n",
+		       __func__, *idx, ev->u.login_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	dev = iscsi_find_flashnode_conn(fnode_sess, NULL,
+					iscsi_is_flashnode_conn_dev);
+	if (!dev) {
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	fnode_conn = iscsi_dev_to_flash_conn(dev);
+	err = transport->login_flashnode(fnode_sess, fnode_conn);
+
+put_host:
+	scsi_host_put(shost);
+
+exit_login_fnode:
+	return err;
+}
+
+static int iscsi_logout_flashnode(struct iscsi_transport *transport,
+				  struct iscsi_uevent *ev)
+{
+	struct Scsi_Host *shost;
+	struct iscsi_bus_flash_session *fnode_sess;
+	struct iscsi_bus_flash_conn *fnode_conn;
+	struct device *dev;
+	uint32_t *idx;
+	int err = 0;
+
+	if (!transport->logout_flashnode) {
+		err = -ENOSYS;
+		goto exit_logout_fnode;
+	}
+
+	shost = scsi_host_lookup(ev->u.logout_flashnode.host_no);
+	if (!shost) {
+		pr_err("%s could not find host no %u\n",
+		       __func__, ev->u.logout_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	idx = &ev->u.logout_flashnode.flashnode_idx;
+	fnode_sess = iscsi_get_flashnode_by_index(shost, idx,
+						  flashnode_match_index);
+	if (!fnode_sess) {
+		pr_err("%s could not find flashnode %u for host no %u\n",
+		       __func__, *idx, ev->u.logout_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	dev = iscsi_find_flashnode_conn(fnode_sess, NULL,
+					iscsi_is_flashnode_conn_dev);
+	if (!dev) {
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	fnode_conn = iscsi_dev_to_flash_conn(dev);
+
+	err = transport->logout_flashnode(fnode_sess, fnode_conn);
+
+put_host:
+	scsi_host_put(shost);
+
+exit_logout_fnode:
+	return err;
+}
+
+static int iscsi_logout_flashnode_sid(struct iscsi_transport *transport,
+				      struct iscsi_uevent *ev)
+{
+	struct Scsi_Host *shost;
+	struct iscsi_cls_session *session;
+	int err = 0;
+
+	if (!transport->logout_flashnode_sid) {
+		err = -ENOSYS;
+		goto exit_logout_sid;
+	}
+
+	shost = scsi_host_lookup(ev->u.logout_flashnode_sid.host_no);
+	if (!shost) {
+		pr_err("%s could not find host no %u\n",
+		       __func__, ev->u.logout_flashnode.host_no);
+		err = -ENODEV;
+		goto put_host;
+	}
+
+	session = iscsi_session_lookup(ev->u.logout_flashnode_sid.sid);
+	if (!session) {
+		pr_err("%s could not find session id %u\n",
+		       __func__, ev->u.logout_flashnode_sid.sid);
+		err = -EINVAL;
+		goto put_host;
+	}
+
+	err = transport->logout_flashnode_sid(session);
+
+put_host:
+	scsi_host_put(shost);
+
+exit_logout_sid:
+	return err;
+}
+
 static int
 iscsi_if_recv_msg(struct sk_buff *skb, struct nlmsghdr *nlh, uint32_t *group)
 {
 	int err = 0;
-	struct iscsi_uevent *ev = NLMSG_DATA(nlh);
+	struct iscsi_uevent *ev = nlmsg_data(nlh);
 	struct iscsi_transport *transport = NULL;
 	struct iscsi_internal *priv;
 	struct iscsi_cls_session *session;
@@ -2246,6 +3218,27 @@ iscsi_if_recv_msg(struct sk_buff *skb, struct nlmsghdr *nlh, uint32_t *group)
 	case ISCSI_UEVENT_DELETE_CHAP:
 		err = iscsi_delete_chap(transport, ev);
 		break;
+	case ISCSI_UEVENT_SET_FLASHNODE_PARAMS:
+		err = iscsi_set_flashnode_param(transport, ev,
+						nlmsg_attrlen(nlh,
+							      sizeof(*ev)));
+		break;
+	case ISCSI_UEVENT_NEW_FLASHNODE:
+		err = iscsi_new_flashnode(transport, ev,
+					  nlmsg_attrlen(nlh, sizeof(*ev)));
+		break;
+	case ISCSI_UEVENT_DEL_FLASHNODE:
+		err = iscsi_del_flashnode(transport, ev);
+		break;
+	case ISCSI_UEVENT_LOGIN_FLASHNODE:
+		err = iscsi_login_flashnode(transport, ev);
+		break;
+	case ISCSI_UEVENT_LOGOUT_FLASHNODE:
+		err = iscsi_logout_flashnode(transport, ev);
+		break;
+	case ISCSI_UEVENT_LOGOUT_FLASHNODE_SID:
+		err = iscsi_logout_flashnode_sid(transport, ev);
+		break;
 	default:
 		err = -ENOSYS;
 		break;
@@ -2263,7 +3256,7 @@ static void
 iscsi_if_rx(struct sk_buff *skb)
 {
 	mutex_lock(&rx_queue_mutex);
-	while (skb->len >= NLMSG_SPACE(0)) {
+	while (skb->len >= NLMSG_HDRLEN) {
 		int err;
 		uint32_t rlen;
 		struct nlmsghdr	*nlh;
@@ -2276,7 +3269,7 @@ iscsi_if_rx(struct sk_buff *skb)
 			break;
 		}
 
-		ev = NLMSG_DATA(nlh);
+		ev = nlmsg_data(nlh);
 		rlen = NLMSG_ALIGN(nlh->nlmsg_len);
 		if (rlen > skb->len)
 			rlen = skb->len;
@@ -2981,10 +3974,14 @@ static __init int iscsi_transport_init(void)
 	if (err)
 		goto unregister_conn_class;
 
+	err = bus_register(&iscsi_flashnode_bus);
+	if (err)
+		goto unregister_session_class;
+
 	nls = netlink_kernel_create(&init_net, NETLINK_ISCSI, &cfg);
 	if (!nls) {
 		err = -ENOBUFS;
-		goto unregister_session_class;
+		goto unregister_flashnode_bus;
 	}
 
 	iscsi_eh_timer_workq = create_singlethread_workqueue("iscsi_eh");
@@ -2995,6 +3992,8 @@ static __init int iscsi_transport_init(void)
 
 release_nls:
 	netlink_kernel_release(nls);
+unregister_flashnode_bus:
+	bus_unregister(&iscsi_flashnode_bus);
 unregister_session_class:
 	transport_class_unregister(&iscsi_session_class);
 unregister_conn_class:
@@ -3014,6 +4013,7 @@ static void __exit iscsi_transport_exit(void)
 {
 	destroy_workqueue(iscsi_eh_timer_workq);
 	netlink_kernel_release(nls);
+	bus_unregister(&iscsi_flashnode_bus);
 	transport_class_unregister(&iscsi_connection_class);
 	transport_class_unregister(&iscsi_session_class);
 	transport_class_unregister(&iscsi_host_class);
diff --git a/drivers/scsi/sd.c b/drivers/scsi/sd.c
index 7992635d405f..e6689776b4f6 100644
--- a/drivers/scsi/sd.c
+++ b/drivers/scsi/sd.c
@@ -1188,7 +1188,7 @@ error_autopm:
  *
  *	Locking: called with bdev->bd_mutex held.
  **/
-static int sd_release(struct gendisk *disk, fmode_t mode)
+static void sd_release(struct gendisk *disk, fmode_t mode)
 {
 	struct scsi_disk *sdkp = scsi_disk(disk);
 	struct scsi_device *sdev = sdkp->device;
@@ -1207,7 +1207,6 @@ static int sd_release(struct gendisk *disk, fmode_t mode)
 
 	scsi_autopm_put_device(sdev);
 	scsi_disk_put(sdkp);
-	return 0;
 }
 
 static int sd_getgeo(struct block_device *bdev, struct hd_geometry *geo)
diff --git a/drivers/scsi/sg.c b/drivers/scsi/sg.c
index 9f0c46547459..df5e961484e1 100644
--- a/drivers/scsi/sg.c
+++ b/drivers/scsi/sg.c
@@ -35,6 +35,7 @@ static int sg_version_num = 30534;	/* 2 digits for each component */
 #include <linux/sched.h>
 #include <linux/string.h>
 #include <linux/mm.h>
+#include <linux/aio.h>
 #include <linux/errno.h>
 #include <linux/mtio.h>
 #include <linux/ioctl.h>
diff --git a/drivers/scsi/sr.c b/drivers/scsi/sr.c
index f2884ee90710..119d67f9c47e 100644
--- a/drivers/scsi/sr.c
+++ b/drivers/scsi/sr.c
@@ -541,14 +541,13 @@ static int sr_block_open(struct block_device *bdev, fmode_t mode)
 	return ret;
 }
 
-static int sr_block_release(struct gendisk *disk, fmode_t mode)
+static void sr_block_release(struct gendisk *disk, fmode_t mode)
 {
 	struct scsi_cd *cd = scsi_cd(disk);
 	mutex_lock(&sr_mutex);
 	cdrom_release(&cd->cdi, mode);
 	scsi_cd_put(cd);
 	mutex_unlock(&sr_mutex);
-	return 0;
 }
 
 static int sr_block_ioctl(struct block_device *bdev, fmode_t mode, unsigned cmd,
diff --git a/drivers/scsi/sun3_NCR5380.c b/drivers/scsi/sun3_NCR5380.c
index 7e12a2e4e0a3..636bbe0ea84c 100644
--- a/drivers/scsi/sun3_NCR5380.c
+++ b/drivers/scsi/sun3_NCR5380.c
@@ -661,121 +661,94 @@ static void __init NCR5380_print_options (struct Scsi_Host *instance)
  * Inputs : instance, pointer to this instance.  
  */
 
-static void NCR5380_print_status (struct Scsi_Host *instance)
+static void lprint_Scsi_Cmnd(Scsi_Cmnd *cmd)
 {
-    char *pr_bfr;
-    char *start;
-    int len;
-
-    NCR_PRINT(NDEBUG_ANY);
-    NCR_PRINT_PHASE(NDEBUG_ANY);
-
-    pr_bfr = (char *) __get_free_page(GFP_ATOMIC);
-    if (!pr_bfr) {
-	printk("NCR5380_print_status: no memory for print buffer\n");
-	return;
-    }
-    len = NCR5380_proc_info(instance, pr_bfr, &start, 0, PAGE_SIZE, 0);
-    pr_bfr[len] = 0;
-    printk("\n%s\n", pr_bfr);
-    free_page((unsigned long) pr_bfr);
+	int i, s;
+	unsigned char *command;
+	printk("scsi%d: destination target %d, lun %d\n",
+		H_NO(cmd), cmd->device->id, cmd->device->lun);
+	printk(KERN_CONT "        command = ");
+	command = cmd->cmnd;
+	printk(KERN_CONT "%2d (0x%02x)", command[0], command[0]);
+	for (i = 1, s = COMMAND_SIZE(command[0]); i < s; ++i)
+		printk(KERN_CONT " %02x", command[i]);
+	printk("\n");
 }
 
-
-/******************************************/
-/*
- * /proc/scsi/[dtc pas16 t128 generic]/[0-ASC_NUM_BOARD_SUPPORTED]
- *
- * *buffer: I/O buffer
- * **start: if inout == FALSE pointer into buffer where user read should start
- * offset: current offset
- * length: length of buffer
- * hostno: Scsi_Host host_no
- * inout: TRUE - user is writing; FALSE - user is reading
- *
- * Return the number of bytes read from or written
-*/
-
-#undef SPRINTF
-#define SPRINTF(fmt,args...) \
-  do { if (pos + strlen(fmt) + 20 /* slop */ < buffer + length) \
-	 pos += sprintf(pos, fmt , ## args); } while(0)
-static
-char *lprint_Scsi_Cmnd(struct scsi_cmnd *cmd, char *pos, char *buffer,
-		       int length);
-
-static int NCR5380_proc_info(struct Scsi_Host *instance, char *buffer,
-			     char **start, off_t offset, int length, int inout)
+static void NCR5380_print_status(struct Scsi_Host *instance)
 {
-    char *pos = buffer;
-    struct NCR5380_hostdata *hostdata;
-    struct scsi_cmnd *ptr;
-    unsigned long flags;
-    off_t begin = 0;
-#define check_offset()				\
-    do {					\
-	if (pos - buffer < offset - begin) {	\
-	    begin += pos - buffer;		\
-	    pos = buffer;			\
-	}					\
-    } while (0)
-
-    hostdata = (struct NCR5380_hostdata *)instance->hostdata;
-
-    if (inout) { /* Has data been written to the file ? */
-	return(-ENOSYS);  /* Currently this is a no-op */
-    }
-    SPRINTF("NCR5380 core release=%d.\n", NCR5380_PUBLIC_RELEASE);
-    check_offset();
-    local_irq_save(flags);
-    SPRINTF("NCR5380: coroutine is%s running.\n", main_running ? "" : "n't");
-    check_offset();
-    if (!hostdata->connected)
-	SPRINTF("scsi%d: no currently connected command\n", HOSTNO);
-    else
-	pos = lprint_Scsi_Cmnd ((struct scsi_cmnd *) hostdata->connected,
-				pos, buffer, length);
-    SPRINTF("scsi%d: issue_queue\n", HOSTNO);
-    check_offset();
-    for (ptr = (struct scsi_cmnd *) hostdata->issue_queue; ptr; ptr = NEXT(ptr))
-    {
-	pos = lprint_Scsi_Cmnd (ptr, pos, buffer, length);
-	check_offset();
-    }
-
-    SPRINTF("scsi%d: disconnected_queue\n", HOSTNO);
-    check_offset();
-    for (ptr = (struct scsi_cmnd *) hostdata->disconnected_queue; ptr;
-	 ptr = NEXT(ptr)) {
-	pos = lprint_Scsi_Cmnd (ptr, pos, buffer, length);
-	check_offset();
-    }
+	struct NCR5380_hostdata *hostdata;
+	Scsi_Cmnd *ptr;
+	unsigned long flags;
+
+	NCR_PRINT(NDEBUG_ANY);
+	NCR_PRINT_PHASE(NDEBUG_ANY);
+
+	hostdata = (struct NCR5380_hostdata *)instance->hostdata;
+
+	printk("\nNCR5380 core release=%d.\n", NCR5380_PUBLIC_RELEASE);
+	local_irq_save(flags);
+	printk("NCR5380: coroutine is%s running.\n",
+		main_running ? "" : "n't");
+	if (!hostdata->connected)
+		printk("scsi%d: no currently connected command\n", HOSTNO);
+	else
+		lprint_Scsi_Cmnd((Scsi_Cmnd *) hostdata->connected);
+	printk("scsi%d: issue_queue\n", HOSTNO);
+	for (ptr = (Scsi_Cmnd *)hostdata->issue_queue; ptr; ptr = NEXT(ptr))
+		lprint_Scsi_Cmnd(ptr);
+
+	printk("scsi%d: disconnected_queue\n", HOSTNO);
+	for (ptr = (Scsi_Cmnd *) hostdata->disconnected_queue; ptr;
+	     ptr = NEXT(ptr))
+		lprint_Scsi_Cmnd(ptr);
 
-    local_irq_restore(flags);
-    *start = buffer + (offset - begin);
-    if (pos - buffer < offset - begin)
-	return 0;
-    else if (pos - buffer - (offset - begin) < length)
-	return pos - buffer - (offset - begin);
-    return length;
+	local_irq_restore(flags);
+	printk("\n");
 }
 
-static char *lprint_Scsi_Cmnd(struct scsi_cmnd *cmd, char *pos, char *buffer,
-			      int length)
+static void show_Scsi_Cmnd(Scsi_Cmnd *cmd, struct seq_file *m)
 {
-    int i, s;
-    unsigned char *command;
-    SPRINTF("scsi%d: destination target %d, lun %d\n",
-	    H_NO(cmd), cmd->device->id, cmd->device->lun);
-    SPRINTF("        command = ");
-    command = cmd->cmnd;
-    SPRINTF("%2d (0x%02x)", command[0], command[0]);
-    for (i = 1, s = COMMAND_SIZE(command[0]); i < s; ++i)
-	SPRINTF(" %02x", command[i]);
-    SPRINTF("\n");
-    return pos;
+	int i, s;
+	unsigned char *command;
+	seq_printf(m, "scsi%d: destination target %d, lun %d\n",
+		H_NO(cmd), cmd->device->id, cmd->device->lun);
+	seq_printf(m, "        command = ");
+	command = cmd->cmnd;
+	seq_printf(m, "%2d (0x%02x)", command[0], command[0]);
+	for (i = 1, s = COMMAND_SIZE(command[0]); i < s; ++i)
+		seq_printf(m, " %02x", command[i]);
+	seq_printf(m, "\n");
 }
 
+static int NCR5380_show_info(struct seq_file *m, struct Scsi_Host *instance)
+{
+	struct NCR5380_hostdata *hostdata;
+	Scsi_Cmnd *ptr;
+	unsigned long flags;
+
+	hostdata = (struct NCR5380_hostdata *)instance->hostdata;
+
+	seq_printf(m, "NCR5380 core release=%d.\n", NCR5380_PUBLIC_RELEASE);
+	local_irq_save(flags);
+	seq_printf(m, "NCR5380: coroutine is%s running.\n",
+		main_running ? "" : "n't");
+	if (!hostdata->connected)
+		seq_printf(m, "scsi%d: no currently connected command\n", HOSTNO);
+	else
+		show_Scsi_Cmnd((Scsi_Cmnd *) hostdata->connected, m);
+	seq_printf(m, "scsi%d: issue_queue\n", HOSTNO);
+	for (ptr = (Scsi_Cmnd *)hostdata->issue_queue; ptr; ptr = NEXT(ptr))
+		show_Scsi_Cmnd(ptr, m);
+
+	seq_printf(m, "scsi%d: disconnected_queue\n", HOSTNO);
+	for (ptr = (Scsi_Cmnd *) hostdata->disconnected_queue; ptr;
+	     ptr = NEXT(ptr))
+		show_Scsi_Cmnd(ptr, m);
+
+	local_irq_restore(flags);
+	return 0;
+}
 
 /* 
  * Function : void NCR5380_init (struct Scsi_Host *instance)
diff --git a/drivers/scsi/sun3_scsi.c b/drivers/scsi/sun3_scsi.c
index 6e25889db9d4..e2c009b033ce 100644
--- a/drivers/scsi/sun3_scsi.c
+++ b/drivers/scsi/sun3_scsi.c
@@ -626,6 +626,7 @@ static int sun3scsi_dma_finish(int write_flag)
 #include "sun3_NCR5380.c"
 
 static struct scsi_host_template driver_template = {
+	.show_info		= sun3scsi_show_info,
 	.name			= SUN3_SCSI_NAME,
 	.detect			= sun3scsi_detect,
 	.release		= sun3scsi_release,
diff --git a/drivers/scsi/sun3_scsi.h b/drivers/scsi/sun3_scsi.h
index bcefd8458e65..a8da9c710fea 100644
--- a/drivers/scsi/sun3_scsi.h
+++ b/drivers/scsi/sun3_scsi.h
@@ -100,7 +100,7 @@ static int sun3scsi_release (struct Scsi_Host *);
 #define NCR5380_queue_command sun3scsi_queue_command
 #define NCR5380_bus_reset sun3scsi_bus_reset
 #define NCR5380_abort sun3scsi_abort
-#define NCR5380_proc_info sun3scsi_proc_info
+#define NCR5380_show_info sun3scsi_show_info
 #define NCR5380_dma_xfer_len(i, cmd, phase) \
         sun3scsi_dma_xfer_len(cmd->SCp.this_residual,cmd,((phase) & SR_IO) ? 0 : 1)
 
diff --git a/drivers/scsi/sym53c8xx_2/sym_glue.c b/drivers/scsi/sym53c8xx_2/sym_glue.c
index 599568299fbe..bac55f7f69f9 100644
--- a/drivers/scsi/sym53c8xx_2/sym_glue.c
+++ b/drivers/scsi/sym53c8xx_2/sym_glue.c
@@ -1171,112 +1171,36 @@ printk("sym_user_command: data=%ld\n", uc->data);
 #endif	/* SYM_LINUX_USER_COMMAND_SUPPORT */
 
 
-#ifdef SYM_LINUX_USER_INFO_SUPPORT
-/*
- *  Informations through the proc file system.
- */
-struct info_str {
-	char *buffer;
-	int length;
-	int offset;
-	int pos;
-};
-
-static void copy_mem_info(struct info_str *info, char *data, int len)
-{
-	if (info->pos + len > info->length)
-		len = info->length - info->pos;
-
-	if (info->pos + len < info->offset) {
-		info->pos += len;
-		return;
-	}
-	if (info->pos < info->offset) {
-		data += (info->offset - info->pos);
-		len  -= (info->offset - info->pos);
-	}
-
-	if (len > 0) {
-		memcpy(info->buffer + info->pos, data, len);
-		info->pos += len;
-	}
-}
-
-static int copy_info(struct info_str *info, char *fmt, ...)
-{
-	va_list args;
-	char buf[81];
-	int len;
-
-	va_start(args, fmt);
-	len = vsprintf(buf, fmt, args);
-	va_end(args);
-
-	copy_mem_info(info, buf, len);
-	return len;
-}
-
 /*
  *  Copy formatted information into the input buffer.
  */
-static int sym_host_info(struct Scsi_Host *shost, char *ptr, off_t offset, int len)
+static int sym_show_info(struct seq_file *m, struct Scsi_Host *shost)
 {
+#ifdef SYM_LINUX_USER_INFO_SUPPORT
 	struct sym_data *sym_data = shost_priv(shost);
 	struct pci_dev *pdev = sym_data->pdev;
 	struct sym_hcb *np = sym_data->ncb;
-	struct info_str info;
-
-	info.buffer	= ptr;
-	info.length	= len;
-	info.offset	= offset;
-	info.pos	= 0;
 
-	copy_info(&info, "Chip " NAME53C "%s, device id 0x%x, "
-			 "revision id 0x%x\n", np->s.chip_name,
-			 pdev->device, pdev->revision);
-	copy_info(&info, "At PCI address %s, IRQ %u\n",
+	seq_printf(m, "Chip " NAME53C "%s, device id 0x%x, "
+		 "revision id 0x%x\n", np->s.chip_name,
+		 pdev->device, pdev->revision);
+	seq_printf(m, "At PCI address %s, IRQ %u\n",
 			 pci_name(pdev), pdev->irq);
-	copy_info(&info, "Min. period factor %d, %s SCSI BUS%s\n",
-			 (int) (np->minsync_dt ? np->minsync_dt : np->minsync),
-			 np->maxwide ? "Wide" : "Narrow",
-			 np->minsync_dt ? ", DT capable" : "");
+	seq_printf(m, "Min. period factor %d, %s SCSI BUS%s\n",
+		 (int) (np->minsync_dt ? np->minsync_dt : np->minsync),
+		 np->maxwide ? "Wide" : "Narrow",
+		 np->minsync_dt ? ", DT capable" : "");
 
-	copy_info(&info, "Max. started commands %d, "
-			 "max. commands per LUN %d\n",
-			 SYM_CONF_MAX_START, SYM_CONF_MAX_TAG);
+	seq_printf(m, "Max. started commands %d, "
+		 "max. commands per LUN %d\n",
+		 SYM_CONF_MAX_START, SYM_CONF_MAX_TAG);
 
-	return info.pos > info.offset? info.pos - info.offset : 0;
-}
-#endif /* SYM_LINUX_USER_INFO_SUPPORT */
-
-/*
- *  Entry point of the scsi proc fs of the driver.
- *  - func = 0 means read  (returns adapter infos)
- *  - func = 1 means write (not yet merget from sym53c8xx)
- */
-static int sym53c8xx_proc_info(struct Scsi_Host *shost, char *buffer,
-			char **start, off_t offset, int length, int func)
-{
-	int retv;
-
-	if (func) {
-#ifdef	SYM_LINUX_USER_COMMAND_SUPPORT
-		retv = sym_user_command(shost, buffer, length);
-#else
-		retv = -EINVAL;
-#endif
-	} else {
-		if (start)
-			*start = buffer;
-#ifdef SYM_LINUX_USER_INFO_SUPPORT
-		retv = sym_host_info(shost, buffer, offset, length);
+	return 0;
 #else
-		retv = -EINVAL;
-#endif
-	}
-
-	return retv;
+	return -EINVAL;
+#endif /* SYM_LINUX_USER_INFO_SUPPORT */
 }
+
 #endif /* SYM_LINUX_PROC_INFO_SUPPORT */
 
 /*
@@ -1742,7 +1666,10 @@ static struct scsi_host_template sym2_template = {
 	.use_clustering		= ENABLE_CLUSTERING,
 	.max_sectors		= 0xFFFF,
 #ifdef SYM_LINUX_PROC_INFO_SUPPORT
-	.proc_info		= sym53c8xx_proc_info,
+	.show_info		= sym_show_info,
+#ifdef	SYM_LINUX_USER_COMMAND_SUPPORT
+	.write_info		= sym_user_command,
+#endif
 	.proc_name		= NAME53C8XX,
 #endif
 };
diff --git a/drivers/scsi/t128.c b/drivers/scsi/t128.c
index d672d97fb84a..f1e4b4148c75 100644
--- a/drivers/scsi/t128.c
+++ b/drivers/scsi/t128.c
@@ -201,7 +201,8 @@ int __init t128_detect(struct scsi_host_template * tpnt){
     int sig, count;
 
     tpnt->proc_name = "t128";
-    tpnt->proc_info = &t128_proc_info;
+    tpnt->show_info = t128_show_info;
+    tpnt->write_info = t128_write_info;
 
     for (count = 0; current_override < NO_OVERRIDES; ++current_override) {
 	base = 0;
diff --git a/drivers/scsi/t128.h b/drivers/scsi/t128.h
index ada1115079c9..1df82c28e56d 100644
--- a/drivers/scsi/t128.h
+++ b/drivers/scsi/t128.h
@@ -140,7 +140,8 @@ static int t128_bus_reset(struct scsi_cmnd *);
 #define NCR5380_queue_command t128_queue_command
 #define NCR5380_abort t128_abort
 #define NCR5380_bus_reset t128_bus_reset
-#define NCR5380_proc_info t128_proc_info
+#define NCR5380_show_info t128_show_info
+#define NCR5380_write_info t128_write_info
 
 /* 15 14 12 10 7 5 3
    1101 0100 1010 1000 */
diff --git a/drivers/scsi/virtio_scsi.c b/drivers/scsi/virtio_scsi.c
index 3449a1f8c656..2168258fb2c3 100644
--- a/drivers/scsi/virtio_scsi.c
+++ b/drivers/scsi/virtio_scsi.c
@@ -13,6 +13,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/mempool.h>
@@ -20,12 +22,14 @@
 #include <linux/virtio_ids.h>
 #include <linux/virtio_config.h>
 #include <linux/virtio_scsi.h>
+#include <linux/cpu.h>
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_device.h>
 #include <scsi/scsi_cmnd.h>
 
 #define VIRTIO_SCSI_MEMPOOL_SZ 64
 #define VIRTIO_SCSI_EVENT_LEN 8
+#define VIRTIO_SCSI_VQ_BASE 2
 
 /* Command queue element */
 struct virtio_scsi_cmd {
@@ -57,27 +61,61 @@ struct virtio_scsi_vq {
 	struct virtqueue *vq;
 };
 
-/* Per-target queue state */
+/*
+ * Per-target queue state.
+ *
+ * This struct holds the data needed by the queue steering policy.  When a
+ * target is sent multiple requests, we need to drive them to the same queue so
+ * that FIFO processing order is kept.  However, if a target was idle, we can
+ * choose a queue arbitrarily.  In this case the queue is chosen according to
+ * the current VCPU, so the driver expects the number of request queues to be
+ * equal to the number of VCPUs.  This makes it easy and fast to select the
+ * queue, and also lets the driver optimize the IRQ affinity for the virtqueues
+ * (each virtqueue's affinity is set to the CPU that "owns" the queue).
+ *
+ * An interesting effect of this policy is that only writes to req_vq need to
+ * take the tgt_lock.  Read can be done outside the lock because:
+ *
+ * - writes of req_vq only occur when atomic_inc_return(&tgt->reqs) returns 1.
+ *   In that case, no other CPU is reading req_vq: even if they were in
+ *   virtscsi_queuecommand_multi, they would be spinning on tgt_lock.
+ *
+ * - reads of req_vq only occur when the target is not idle (reqs != 0).
+ *   A CPU that enters virtscsi_queuecommand_multi will not modify req_vq.
+ *
+ * Similarly, decrements of reqs are never concurrent with writes of req_vq.
+ * Thus they can happen outside the tgt_lock, provided of course we make reqs
+ * an atomic_t.
+ */
 struct virtio_scsi_target_state {
-	/* Protects sg.  Lock hierarchy is tgt_lock -> vq_lock.  */
+	/* This spinlock never held at the same time as vq_lock. */
 	spinlock_t tgt_lock;
 
-	/* For sglist construction when adding commands to the virtqueue.  */
-	struct scatterlist sg[];
+	/* Count of outstanding requests. */
+	atomic_t reqs;
+
+	/* Currently active virtqueue for requests sent to this target. */
+	struct virtio_scsi_vq *req_vq;
 };
 
 /* Driver instance state */
 struct virtio_scsi {
 	struct virtio_device *vdev;
 
-	struct virtio_scsi_vq ctrl_vq;
-	struct virtio_scsi_vq event_vq;
-	struct virtio_scsi_vq req_vq;
-
 	/* Get some buffers ready for event vq */
 	struct virtio_scsi_event_node event_list[VIRTIO_SCSI_EVENT_LEN];
 
-	struct virtio_scsi_target_state *tgt[];
+	u32 num_queues;
+
+	/* If the affinity hint is set for virtqueues */
+	bool affinity_hint_set;
+
+	/* CPU hotplug notifier */
+	struct notifier_block nb;
+
+	struct virtio_scsi_vq ctrl_vq;
+	struct virtio_scsi_vq event_vq;
+	struct virtio_scsi_vq req_vqs[];
 };
 
 static struct kmem_cache *virtscsi_cmd_cache;
@@ -107,11 +145,13 @@ static void virtscsi_compute_resid(struct scsi_cmnd *sc, u32 resid)
  *
  * Called with vq_lock held.
  */
-static void virtscsi_complete_cmd(void *buf)
+static void virtscsi_complete_cmd(struct virtio_scsi *vscsi, void *buf)
 {
 	struct virtio_scsi_cmd *cmd = buf;
 	struct scsi_cmnd *sc = cmd->sc;
 	struct virtio_scsi_cmd_resp *resp = &cmd->resp.cmd;
+	struct virtio_scsi_target_state *tgt =
+				scsi_target(sc->device)->hostdata;
 
 	dev_dbg(&sc->device->sdev_gendev,
 		"cmd %p response %u status %#02x sense_len %u\n",
@@ -166,32 +206,71 @@ static void virtscsi_complete_cmd(void *buf)
 
 	mempool_free(cmd, virtscsi_cmd_pool);
 	sc->scsi_done(sc);
+
+	atomic_dec(&tgt->reqs);
 }
 
-static void virtscsi_vq_done(struct virtqueue *vq, void (*fn)(void *buf))
+static void virtscsi_vq_done(struct virtio_scsi *vscsi,
+			     struct virtio_scsi_vq *virtscsi_vq,
+			     void (*fn)(struct virtio_scsi *vscsi, void *buf))
 {
 	void *buf;
 	unsigned int len;
+	unsigned long flags;
+	struct virtqueue *vq = virtscsi_vq->vq;
 
+	spin_lock_irqsave(&virtscsi_vq->vq_lock, flags);
 	do {
 		virtqueue_disable_cb(vq);
 		while ((buf = virtqueue_get_buf(vq, &len)) != NULL)
-			fn(buf);
+			fn(vscsi, buf);
 	} while (!virtqueue_enable_cb(vq));
+	spin_unlock_irqrestore(&virtscsi_vq->vq_lock, flags);
 }
 
 static void virtscsi_req_done(struct virtqueue *vq)
 {
 	struct Scsi_Host *sh = virtio_scsi_host(vq->vdev);
 	struct virtio_scsi *vscsi = shost_priv(sh);
-	unsigned long flags;
+	int index = vq->index - VIRTIO_SCSI_VQ_BASE;
+	struct virtio_scsi_vq *req_vq = &vscsi->req_vqs[index];
 
-	spin_lock_irqsave(&vscsi->req_vq.vq_lock, flags);
-	virtscsi_vq_done(vq, virtscsi_complete_cmd);
-	spin_unlock_irqrestore(&vscsi->req_vq.vq_lock, flags);
+	/*
+	 * Read req_vq before decrementing the reqs field in
+	 * virtscsi_complete_cmd.
+	 *
+	 * With barriers:
+	 *
+	 * 	CPU #0			virtscsi_queuecommand_multi (CPU #1)
+	 * 	------------------------------------------------------------
+	 * 	lock vq_lock
+	 * 	read req_vq
+	 * 	read reqs (reqs = 1)
+	 * 	write reqs (reqs = 0)
+	 * 				increment reqs (reqs = 1)
+	 * 				write req_vq
+	 *
+	 * Possible reordering without barriers:
+	 *
+	 * 	CPU #0			virtscsi_queuecommand_multi (CPU #1)
+	 * 	------------------------------------------------------------
+	 * 	lock vq_lock
+	 * 	read reqs (reqs = 1)
+	 * 	write reqs (reqs = 0)
+	 * 				increment reqs (reqs = 1)
+	 * 				write req_vq
+	 * 	read (wrong) req_vq
+	 *
+	 * We do not need a full smp_rmb, because req_vq is required to get
+	 * to tgt->reqs: tgt is &vscsi->tgt[sc->device->id], where sc is stored
+	 * in the virtqueue as the user token.
+	 */
+	smp_read_barrier_depends();
+
+	virtscsi_vq_done(vscsi, req_vq, virtscsi_complete_cmd);
 };
 
-static void virtscsi_complete_free(void *buf)
+static void virtscsi_complete_free(struct virtio_scsi *vscsi, void *buf)
 {
 	struct virtio_scsi_cmd *cmd = buf;
 
@@ -205,11 +284,8 @@ static void virtscsi_ctrl_done(struct virtqueue *vq)
 {
 	struct Scsi_Host *sh = virtio_scsi_host(vq->vdev);
 	struct virtio_scsi *vscsi = shost_priv(sh);
-	unsigned long flags;
 
-	spin_lock_irqsave(&vscsi->ctrl_vq.vq_lock, flags);
-	virtscsi_vq_done(vq, virtscsi_complete_free);
-	spin_unlock_irqrestore(&vscsi->ctrl_vq.vq_lock, flags);
+	virtscsi_vq_done(vscsi, &vscsi->ctrl_vq, virtscsi_complete_free);
 };
 
 static int virtscsi_kick_event(struct virtio_scsi *vscsi,
@@ -223,8 +299,8 @@ static int virtscsi_kick_event(struct virtio_scsi *vscsi,
 
 	spin_lock_irqsave(&vscsi->event_vq.vq_lock, flags);
 
-	err = virtqueue_add_buf(vscsi->event_vq.vq, &sg, 0, 1, event_node,
-				GFP_ATOMIC);
+	err = virtqueue_add_inbuf(vscsi->event_vq.vq, &sg, 1, event_node,
+				  GFP_ATOMIC);
 	if (!err)
 		virtqueue_kick(vscsi->event_vq.vq);
 
@@ -254,7 +330,7 @@ static void virtscsi_cancel_event_work(struct virtio_scsi *vscsi)
 }
 
 static void virtscsi_handle_transport_reset(struct virtio_scsi *vscsi,
-						struct virtio_scsi_event *event)
+					    struct virtio_scsi_event *event)
 {
 	struct scsi_device *sdev;
 	struct Scsi_Host *shost = virtio_scsi_host(vscsi->vdev);
@@ -332,7 +408,7 @@ static void virtscsi_handle_event(struct work_struct *work)
 	virtscsi_kick_event(vscsi, event_node);
 }
 
-static void virtscsi_complete_event(void *buf)
+static void virtscsi_complete_event(struct virtio_scsi *vscsi, void *buf)
 {
 	struct virtio_scsi_event_node *event_node = buf;
 
@@ -344,82 +420,65 @@ static void virtscsi_event_done(struct virtqueue *vq)
 {
 	struct Scsi_Host *sh = virtio_scsi_host(vq->vdev);
 	struct virtio_scsi *vscsi = shost_priv(sh);
-	unsigned long flags;
 
-	spin_lock_irqsave(&vscsi->event_vq.vq_lock, flags);
-	virtscsi_vq_done(vq, virtscsi_complete_event);
-	spin_unlock_irqrestore(&vscsi->event_vq.vq_lock, flags);
+	virtscsi_vq_done(vscsi, &vscsi->event_vq, virtscsi_complete_event);
 };
 
-static void virtscsi_map_sgl(struct scatterlist *sg, unsigned int *p_idx,
-			     struct scsi_data_buffer *sdb)
-{
-	struct sg_table *table = &sdb->table;
-	struct scatterlist *sg_elem;
-	unsigned int idx = *p_idx;
-	int i;
-
-	for_each_sg(table->sgl, sg_elem, table->nents, i)
-		sg[idx++] = *sg_elem;
-
-	*p_idx = idx;
-}
-
 /**
- * virtscsi_map_cmd - map a scsi_cmd to a virtqueue scatterlist
- * @vscsi	: virtio_scsi state
+ * virtscsi_add_cmd - add a virtio_scsi_cmd to a virtqueue
+ * @vq		: the struct virtqueue we're talking about
  * @cmd		: command structure
- * @out_num	: number of read-only elements
- * @in_num	: number of write-only elements
  * @req_size	: size of the request buffer
  * @resp_size	: size of the response buffer
- *
- * Called with tgt_lock held.
+ * @gfp	: flags to use for memory allocations
  */
-static void virtscsi_map_cmd(struct virtio_scsi_target_state *tgt,
-			     struct virtio_scsi_cmd *cmd,
-			     unsigned *out_num, unsigned *in_num,
-			     size_t req_size, size_t resp_size)
+static int virtscsi_add_cmd(struct virtqueue *vq,
+			    struct virtio_scsi_cmd *cmd,
+			    size_t req_size, size_t resp_size, gfp_t gfp)
 {
 	struct scsi_cmnd *sc = cmd->sc;
-	struct scatterlist *sg = tgt->sg;
-	unsigned int idx = 0;
+	struct scatterlist *sgs[4], req, resp;
+	struct sg_table *out, *in;
+	unsigned out_num = 0, in_num = 0;
+
+	out = in = NULL;
+
+	if (sc && sc->sc_data_direction != DMA_NONE) {
+		if (sc->sc_data_direction != DMA_FROM_DEVICE)
+			out = &scsi_out(sc)->table;
+		if (sc->sc_data_direction != DMA_TO_DEVICE)
+			in = &scsi_in(sc)->table;
+	}
 
 	/* Request header.  */
-	sg_set_buf(&sg[idx++], &cmd->req, req_size);
+	sg_init_one(&req, &cmd->req, req_size);
+	sgs[out_num++] = &req;
 
 	/* Data-out buffer.  */
-	if (sc && sc->sc_data_direction != DMA_FROM_DEVICE)
-		virtscsi_map_sgl(sg, &idx, scsi_out(sc));
-
-	*out_num = idx;
+	if (out)
+		sgs[out_num++] = out->sgl;
 
 	/* Response header.  */
-	sg_set_buf(&sg[idx++], &cmd->resp, resp_size);
+	sg_init_one(&resp, &cmd->resp, resp_size);
+	sgs[out_num + in_num++] = &resp;
 
 	/* Data-in buffer */
-	if (sc && sc->sc_data_direction != DMA_TO_DEVICE)
-		virtscsi_map_sgl(sg, &idx, scsi_in(sc));
+	if (in)
+		sgs[out_num + in_num++] = in->sgl;
 
-	*in_num = idx - *out_num;
+	return virtqueue_add_sgs(vq, sgs, out_num, in_num, cmd, gfp);
 }
 
-static int virtscsi_kick_cmd(struct virtio_scsi_target_state *tgt,
-			     struct virtio_scsi_vq *vq,
+static int virtscsi_kick_cmd(struct virtio_scsi_vq *vq,
 			     struct virtio_scsi_cmd *cmd,
 			     size_t req_size, size_t resp_size, gfp_t gfp)
 {
-	unsigned int out_num, in_num;
 	unsigned long flags;
 	int err;
 	bool needs_kick = false;
 
-	spin_lock_irqsave(&tgt->tgt_lock, flags);
-	virtscsi_map_cmd(tgt, cmd, &out_num, &in_num, req_size, resp_size);
-
-	spin_lock(&vq->vq_lock);
-	err = virtqueue_add_buf(vq->vq, tgt->sg, out_num, in_num, cmd, gfp);
-	spin_unlock(&tgt->tgt_lock);
+	spin_lock_irqsave(&vq->vq_lock, flags);
+	err = virtscsi_add_cmd(vq->vq, cmd, req_size, resp_size, gfp);
 	if (!err)
 		needs_kick = virtqueue_kick_prepare(vq->vq);
 
@@ -430,10 +489,10 @@ static int virtscsi_kick_cmd(struct virtio_scsi_target_state *tgt,
 	return err;
 }
 
-static int virtscsi_queuecommand(struct Scsi_Host *sh, struct scsi_cmnd *sc)
+static int virtscsi_queuecommand(struct virtio_scsi *vscsi,
+				 struct virtio_scsi_vq *req_vq,
+				 struct scsi_cmnd *sc)
 {
-	struct virtio_scsi *vscsi = shost_priv(sh);
-	struct virtio_scsi_target_state *tgt = vscsi->tgt[sc->device->id];
 	struct virtio_scsi_cmd *cmd;
 	int ret;
 
@@ -467,7 +526,7 @@ static int virtscsi_queuecommand(struct Scsi_Host *sh, struct scsi_cmnd *sc)
 	BUG_ON(sc->cmd_len > VIRTIO_SCSI_CDB_SIZE);
 	memcpy(cmd->req.cmd.cdb, sc->cmnd, sc->cmd_len);
 
-	if (virtscsi_kick_cmd(tgt, &vscsi->req_vq, cmd,
+	if (virtscsi_kick_cmd(req_vq, cmd,
 			      sizeof cmd->req.cmd, sizeof cmd->resp.cmd,
 			      GFP_ATOMIC) == 0)
 		ret = 0;
@@ -478,14 +537,62 @@ out:
 	return ret;
 }
 
+static int virtscsi_queuecommand_single(struct Scsi_Host *sh,
+					struct scsi_cmnd *sc)
+{
+	struct virtio_scsi *vscsi = shost_priv(sh);
+	struct virtio_scsi_target_state *tgt =
+				scsi_target(sc->device)->hostdata;
+
+	atomic_inc(&tgt->reqs);
+	return virtscsi_queuecommand(vscsi, &vscsi->req_vqs[0], sc);
+}
+
+static struct virtio_scsi_vq *virtscsi_pick_vq(struct virtio_scsi *vscsi,
+					       struct virtio_scsi_target_state *tgt)
+{
+	struct virtio_scsi_vq *vq;
+	unsigned long flags;
+	u32 queue_num;
+
+	spin_lock_irqsave(&tgt->tgt_lock, flags);
+
+	/*
+	 * The memory barrier after atomic_inc_return matches
+	 * the smp_read_barrier_depends() in virtscsi_req_done.
+	 */
+	if (atomic_inc_return(&tgt->reqs) > 1)
+		vq = ACCESS_ONCE(tgt->req_vq);
+	else {
+		queue_num = smp_processor_id();
+		while (unlikely(queue_num >= vscsi->num_queues))
+			queue_num -= vscsi->num_queues;
+
+		tgt->req_vq = vq = &vscsi->req_vqs[queue_num];
+	}
+
+	spin_unlock_irqrestore(&tgt->tgt_lock, flags);
+	return vq;
+}
+
+static int virtscsi_queuecommand_multi(struct Scsi_Host *sh,
+				       struct scsi_cmnd *sc)
+{
+	struct virtio_scsi *vscsi = shost_priv(sh);
+	struct virtio_scsi_target_state *tgt =
+				scsi_target(sc->device)->hostdata;
+	struct virtio_scsi_vq *req_vq = virtscsi_pick_vq(vscsi, tgt);
+
+	return virtscsi_queuecommand(vscsi, req_vq, sc);
+}
+
 static int virtscsi_tmf(struct virtio_scsi *vscsi, struct virtio_scsi_cmd *cmd)
 {
 	DECLARE_COMPLETION_ONSTACK(comp);
-	struct virtio_scsi_target_state *tgt = vscsi->tgt[cmd->sc->device->id];
 	int ret = FAILED;
 
 	cmd->comp = &comp;
-	if (virtscsi_kick_cmd(tgt, &vscsi->ctrl_vq, cmd,
+	if (virtscsi_kick_cmd(&vscsi->ctrl_vq, cmd,
 			      sizeof cmd->req.tmf, sizeof cmd->resp.tmf,
 			      GFP_NOIO) < 0)
 		goto out;
@@ -547,18 +654,57 @@ static int virtscsi_abort(struct scsi_cmnd *sc)
 	return virtscsi_tmf(vscsi, cmd);
 }
 
-static struct scsi_host_template virtscsi_host_template = {
+static int virtscsi_target_alloc(struct scsi_target *starget)
+{
+	struct virtio_scsi_target_state *tgt =
+				kmalloc(sizeof(*tgt), GFP_KERNEL);
+	if (!tgt)
+		return -ENOMEM;
+
+	spin_lock_init(&tgt->tgt_lock);
+	atomic_set(&tgt->reqs, 0);
+	tgt->req_vq = NULL;
+
+	starget->hostdata = tgt;
+	return 0;
+}
+
+static void virtscsi_target_destroy(struct scsi_target *starget)
+{
+	struct virtio_scsi_target_state *tgt = starget->hostdata;
+	kfree(tgt);
+}
+
+static struct scsi_host_template virtscsi_host_template_single = {
+	.module = THIS_MODULE,
+	.name = "Virtio SCSI HBA",
+	.proc_name = "virtio_scsi",
+	.this_id = -1,
+	.queuecommand = virtscsi_queuecommand_single,
+	.eh_abort_handler = virtscsi_abort,
+	.eh_device_reset_handler = virtscsi_device_reset,
+
+	.can_queue = 1024,
+	.dma_boundary = UINT_MAX,
+	.use_clustering = ENABLE_CLUSTERING,
+	.target_alloc = virtscsi_target_alloc,
+	.target_destroy = virtscsi_target_destroy,
+};
+
+static struct scsi_host_template virtscsi_host_template_multi = {
 	.module = THIS_MODULE,
 	.name = "Virtio SCSI HBA",
 	.proc_name = "virtio_scsi",
-	.queuecommand = virtscsi_queuecommand,
 	.this_id = -1,
+	.queuecommand = virtscsi_queuecommand_multi,
 	.eh_abort_handler = virtscsi_abort,
 	.eh_device_reset_handler = virtscsi_device_reset,
 
 	.can_queue = 1024,
 	.dma_boundary = UINT_MAX,
 	.use_clustering = ENABLE_CLUSTERING,
+	.target_alloc = virtscsi_target_alloc,
+	.target_destroy = virtscsi_target_destroy,
 };
 
 #define virtscsi_config_get(vdev, fld) \
@@ -578,29 +724,69 @@ static struct scsi_host_template virtscsi_host_template = {
 				  &__val, sizeof(__val)); \
 	})
 
-static void virtscsi_init_vq(struct virtio_scsi_vq *virtscsi_vq,
-			     struct virtqueue *vq)
+static void __virtscsi_set_affinity(struct virtio_scsi *vscsi, bool affinity)
 {
-	spin_lock_init(&virtscsi_vq->vq_lock);
-	virtscsi_vq->vq = vq;
+	int i;
+	int cpu;
+
+	/* In multiqueue mode, when the number of cpu is equal
+	 * to the number of request queues, we let the qeueues
+	 * to be private to one cpu by setting the affinity hint
+	 * to eliminate the contention.
+	 */
+	if ((vscsi->num_queues == 1 ||
+	     vscsi->num_queues != num_online_cpus()) && affinity) {
+		if (vscsi->affinity_hint_set)
+			affinity = false;
+		else
+			return;
+	}
+
+	if (affinity) {
+		i = 0;
+		for_each_online_cpu(cpu) {
+			virtqueue_set_affinity(vscsi->req_vqs[i].vq, cpu);
+			i++;
+		}
+
+		vscsi->affinity_hint_set = true;
+	} else {
+		for (i = 0; i < vscsi->num_queues - VIRTIO_SCSI_VQ_BASE; i++)
+			virtqueue_set_affinity(vscsi->req_vqs[i].vq, -1);
+
+		vscsi->affinity_hint_set = false;
+	}
 }
 
-static struct virtio_scsi_target_state *virtscsi_alloc_tgt(
-	struct virtio_device *vdev, int sg_elems)
+static void virtscsi_set_affinity(struct virtio_scsi *vscsi, bool affinity)
 {
-	struct virtio_scsi_target_state *tgt;
-	gfp_t gfp_mask = GFP_KERNEL;
-
-	/* We need extra sg elements at head and tail.  */
-	tgt = kmalloc(sizeof(*tgt) + sizeof(tgt->sg[0]) * (sg_elems + 2),
-		      gfp_mask);
+	get_online_cpus();
+	__virtscsi_set_affinity(vscsi, affinity);
+	put_online_cpus();
+}
 
-	if (!tgt)
-		return NULL;
+static int virtscsi_cpu_callback(struct notifier_block *nfb,
+				 unsigned long action, void *hcpu)
+{
+	struct virtio_scsi *vscsi = container_of(nfb, struct virtio_scsi, nb);
+	switch(action) {
+	case CPU_ONLINE:
+	case CPU_ONLINE_FROZEN:
+	case CPU_DEAD:
+	case CPU_DEAD_FROZEN:
+		__virtscsi_set_affinity(vscsi, true);
+		break;
+	default:
+		break;
+	}
+	return NOTIFY_OK;
+}
 
-	spin_lock_init(&tgt->tgt_lock);
-	sg_init_table(tgt->sg, sg_elems + 2);
-	return tgt;
+static void virtscsi_init_vq(struct virtio_scsi_vq *virtscsi_vq,
+			     struct virtqueue *vq)
+{
+	spin_lock_init(&virtscsi_vq->vq_lock);
+	virtscsi_vq->vq = vq;
 }
 
 static void virtscsi_scan(struct virtio_device *vdev)
@@ -614,46 +800,56 @@ static void virtscsi_remove_vqs(struct virtio_device *vdev)
 {
 	struct Scsi_Host *sh = virtio_scsi_host(vdev);
 	struct virtio_scsi *vscsi = shost_priv(sh);
-	u32 i, num_targets;
+
+	virtscsi_set_affinity(vscsi, false);
 
 	/* Stop all the virtqueues. */
 	vdev->config->reset(vdev);
 
-	num_targets = sh->max_id;
-	for (i = 0; i < num_targets; i++) {
-		kfree(vscsi->tgt[i]);
-		vscsi->tgt[i] = NULL;
-	}
-
 	vdev->config->del_vqs(vdev);
 }
 
 static int virtscsi_init(struct virtio_device *vdev,
-			 struct virtio_scsi *vscsi, int num_targets)
+			 struct virtio_scsi *vscsi)
 {
 	int err;
-	struct virtqueue *vqs[3];
-	u32 i, sg_elems;
+	u32 i;
+	u32 num_vqs;
+	vq_callback_t **callbacks;
+	const char **names;
+	struct virtqueue **vqs;
+
+	num_vqs = vscsi->num_queues + VIRTIO_SCSI_VQ_BASE;
+	vqs = kmalloc(num_vqs * sizeof(struct virtqueue *), GFP_KERNEL);
+	callbacks = kmalloc(num_vqs * sizeof(vq_callback_t *), GFP_KERNEL);
+	names = kmalloc(num_vqs * sizeof(char *), GFP_KERNEL);
+
+	if (!callbacks || !vqs || !names) {
+		err = -ENOMEM;
+		goto out;
+	}
 
-	vq_callback_t *callbacks[] = {
-		virtscsi_ctrl_done,
-		virtscsi_event_done,
-		virtscsi_req_done
-	};
-	const char *names[] = {
-		"control",
-		"event",
-		"request"
-	};
+	callbacks[0] = virtscsi_ctrl_done;
+	callbacks[1] = virtscsi_event_done;
+	names[0] = "control";
+	names[1] = "event";
+	for (i = VIRTIO_SCSI_VQ_BASE; i < num_vqs; i++) {
+		callbacks[i] = virtscsi_req_done;
+		names[i] = "request";
+	}
 
 	/* Discover virtqueues and write information to configuration.  */
-	err = vdev->config->find_vqs(vdev, 3, vqs, callbacks, names);
+	err = vdev->config->find_vqs(vdev, num_vqs, vqs, callbacks, names);
 	if (err)
-		return err;
+		goto out;
 
 	virtscsi_init_vq(&vscsi->ctrl_vq, vqs[0]);
 	virtscsi_init_vq(&vscsi->event_vq, vqs[1]);
-	virtscsi_init_vq(&vscsi->req_vq, vqs[2]);
+	for (i = VIRTIO_SCSI_VQ_BASE; i < num_vqs; i++)
+		virtscsi_init_vq(&vscsi->req_vqs[i - VIRTIO_SCSI_VQ_BASE],
+				 vqs[i]);
+
+	virtscsi_set_affinity(vscsi, true);
 
 	virtscsi_config_set(vdev, cdb_size, VIRTIO_SCSI_CDB_SIZE);
 	virtscsi_config_set(vdev, sense_size, VIRTIO_SCSI_SENSE_SIZE);
@@ -661,19 +857,12 @@ static int virtscsi_init(struct virtio_device *vdev,
 	if (virtio_has_feature(vdev, VIRTIO_SCSI_F_HOTPLUG))
 		virtscsi_kick_event_all(vscsi);
 
-	/* We need to know how many segments before we allocate.  */
-	sg_elems = virtscsi_config_get(vdev, seg_max) ?: 1;
-
-	for (i = 0; i < num_targets; i++) {
-		vscsi->tgt[i] = virtscsi_alloc_tgt(vdev, sg_elems);
-		if (!vscsi->tgt[i]) {
-			err = -ENOMEM;
-			goto out;
-		}
-	}
 	err = 0;
 
 out:
+	kfree(names);
+	kfree(callbacks);
+	kfree(vqs);
 	if (err)
 		virtscsi_remove_vqs(vdev);
 	return err;
@@ -686,13 +875,21 @@ static int virtscsi_probe(struct virtio_device *vdev)
 	int err;
 	u32 sg_elems, num_targets;
 	u32 cmd_per_lun;
+	u32 num_queues;
+	struct scsi_host_template *hostt;
+
+	/* We need to know how many queues before we allocate. */
+	num_queues = virtscsi_config_get(vdev, num_queues) ? : 1;
 
-	/* Allocate memory and link the structs together.  */
 	num_targets = virtscsi_config_get(vdev, max_target) + 1;
-	shost = scsi_host_alloc(&virtscsi_host_template,
-		sizeof(*vscsi)
-		+ num_targets * sizeof(struct virtio_scsi_target_state));
 
+	if (num_queues == 1)
+		hostt = &virtscsi_host_template_single;
+	else
+		hostt = &virtscsi_host_template_multi;
+
+	shost = scsi_host_alloc(hostt,
+		sizeof(*vscsi) + sizeof(vscsi->req_vqs[0]) * num_queues);
 	if (!shost)
 		return -ENOMEM;
 
@@ -700,12 +897,20 @@ static int virtscsi_probe(struct virtio_device *vdev)
 	shost->sg_tablesize = sg_elems;
 	vscsi = shost_priv(shost);
 	vscsi->vdev = vdev;
+	vscsi->num_queues = num_queues;
 	vdev->priv = shost;
 
-	err = virtscsi_init(vdev, vscsi, num_targets);
+	err = virtscsi_init(vdev, vscsi);
 	if (err)
 		goto virtscsi_init_failed;
 
+	vscsi->nb.notifier_call = &virtscsi_cpu_callback;
+	err = register_hotcpu_notifier(&vscsi->nb);
+	if (err) {
+		pr_err("registering cpu notifier failed\n");
+		goto scsi_add_host_failed;
+	}
+
 	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;
@@ -743,6 +948,8 @@ static void virtscsi_remove(struct virtio_device *vdev)
 
 	scsi_remove_host(shost);
 
+	unregister_hotcpu_notifier(&vscsi->nb);
+
 	virtscsi_remove_vqs(vdev);
 	scsi_host_put(shost);
 }
@@ -759,7 +966,7 @@ static int virtscsi_restore(struct virtio_device *vdev)
 	struct Scsi_Host *sh = virtio_scsi_host(vdev);
 	struct virtio_scsi *vscsi = shost_priv(sh);
 
-	return virtscsi_init(vdev, vscsi, sh->max_id);
+	return virtscsi_init(vdev, vscsi);
 }
 #endif
 
@@ -794,8 +1001,7 @@ static int __init init(void)
 
 	virtscsi_cmd_cache = KMEM_CACHE(virtio_scsi_cmd, 0);
 	if (!virtscsi_cmd_cache) {
-		printk(KERN_ERR "kmem_cache_create() for "
-				"virtscsi_cmd_cache failed\n");
+		pr_err("kmem_cache_create() for virtscsi_cmd_cache failed\n");
 		goto error;
 	}
 
@@ -804,8 +1010,7 @@ static int __init init(void)
 		mempool_create_slab_pool(VIRTIO_SCSI_MEMPOOL_SZ,
 					 virtscsi_cmd_cache);
 	if (!virtscsi_cmd_pool) {
-		printk(KERN_ERR "mempool_create() for"
-				"virtscsi_cmd_pool failed\n");
+		pr_err("mempool_create() for virtscsi_cmd_pool failed\n");
 		goto error;
 	}
 	ret = register_virtio_driver(&virtio_scsi_driver);
diff --git a/drivers/scsi/wd33c93.c b/drivers/scsi/wd33c93.c
index c0ee4ea28a19..41883a87931d 100644
--- a/drivers/scsi/wd33c93.c
+++ b/drivers/scsi/wd33c93.c
@@ -2054,22 +2054,16 @@ wd33c93_init(struct Scsi_Host *instance, const wd33c93_regs regs,
 	printk("           Version %s - %s\n", WD33C93_VERSION, WD33C93_DATE);
 }
 
-int
-wd33c93_proc_info(struct Scsi_Host *instance, char *buf, char **start, off_t off, int len, int in)
+int wd33c93_write_info(struct Scsi_Host *instance, char *buf, int len)
 {
-
 #ifdef PROC_INTERFACE
-
 	char *bp;
-	char tbuf[128];
 	struct WD33C93_hostdata *hd;
-	struct scsi_cmnd *cmd;
 	int x;
-	static int stop = 0;
 
 	hd = (struct WD33C93_hostdata *) instance->hostdata;
 
-/* If 'in' is TRUE we need to _read_ the proc file. We accept the following
+/* We accept the following
  * keywords (same format as command-line, but arguments are not optional):
  *    debug
  *    disconnect
@@ -2083,145 +2077,124 @@ wd33c93_proc_info(struct Scsi_Host *instance, char *buf, char **start, off_t off
  *    nosync
  */
 
-	if (in) {
-		buf[len] = '\0';
-		for (bp = buf; *bp; ) {
-			while (',' == *bp || ' ' == *bp)
-				++bp;
-		if (!strncmp(bp, "debug:", 6)) {
-				hd->args = simple_strtoul(bp+6, &bp, 0) & DB_MASK;
-		} else if (!strncmp(bp, "disconnect:", 11)) {
-				x = simple_strtoul(bp+11, &bp, 0);
-			if (x < DIS_NEVER || x > DIS_ALWAYS)
-				x = DIS_ADAPTIVE;
-			hd->disconnect = x;
-		} else if (!strncmp(bp, "period:", 7)) {
+	buf[len] = '\0';
+	for (bp = buf; *bp; ) {
+		while (',' == *bp || ' ' == *bp)
+			++bp;
+	if (!strncmp(bp, "debug:", 6)) {
+			hd->args = simple_strtoul(bp+6, &bp, 0) & DB_MASK;
+	} else if (!strncmp(bp, "disconnect:", 11)) {
+			x = simple_strtoul(bp+11, &bp, 0);
+		if (x < DIS_NEVER || x > DIS_ALWAYS)
+			x = DIS_ADAPTIVE;
+		hd->disconnect = x;
+	} else if (!strncmp(bp, "period:", 7)) {
+		x = simple_strtoul(bp+7, &bp, 0);
+		hd->default_sx_per =
+			hd->sx_table[round_period((unsigned int) x,
+						  hd->sx_table)].period_ns;
+	} else if (!strncmp(bp, "resync:", 7)) {
+			set_resync(hd, (int)simple_strtoul(bp+7, &bp, 0));
+	} else if (!strncmp(bp, "proc:", 5)) {
+			hd->proc = simple_strtoul(bp+5, &bp, 0);
+	} else if (!strncmp(bp, "nodma:", 6)) {
+			hd->no_dma = simple_strtoul(bp+6, &bp, 0);
+	} else if (!strncmp(bp, "level2:", 7)) {
+			hd->level2 = simple_strtoul(bp+7, &bp, 0);
+		} else if (!strncmp(bp, "burst:", 6)) {
+			hd->dma_mode =
+				simple_strtol(bp+6, &bp, 0) ? CTRL_BURST:CTRL_DMA;
+		} else if (!strncmp(bp, "fast:", 5)) {
+			x = !!simple_strtol(bp+5, &bp, 0);
+			if (x != hd->fast)
+				set_resync(hd, 0xff);
+			hd->fast = x;
+		} else if (!strncmp(bp, "nosync:", 7)) {
 			x = simple_strtoul(bp+7, &bp, 0);
-			hd->default_sx_per =
-				hd->sx_table[round_period((unsigned int) x,
-							  hd->sx_table)].period_ns;
-		} else if (!strncmp(bp, "resync:", 7)) {
-				set_resync(hd, (int)simple_strtoul(bp+7, &bp, 0));
-		} else if (!strncmp(bp, "proc:", 5)) {
-				hd->proc = simple_strtoul(bp+5, &bp, 0);
-		} else if (!strncmp(bp, "nodma:", 6)) {
-				hd->no_dma = simple_strtoul(bp+6, &bp, 0);
-		} else if (!strncmp(bp, "level2:", 7)) {
-				hd->level2 = simple_strtoul(bp+7, &bp, 0);
-			} else if (!strncmp(bp, "burst:", 6)) {
-				hd->dma_mode =
-					simple_strtol(bp+6, &bp, 0) ? CTRL_BURST:CTRL_DMA;
-			} else if (!strncmp(bp, "fast:", 5)) {
-				x = !!simple_strtol(bp+5, &bp, 0);
-				if (x != hd->fast)
-					set_resync(hd, 0xff);
-				hd->fast = x;
-			} else if (!strncmp(bp, "nosync:", 7)) {
-				x = simple_strtoul(bp+7, &bp, 0);
-				set_resync(hd, x ^ hd->no_sync);
-				hd->no_sync = x;
-			} else {
-				break; /* unknown keyword,syntax-error,... */
-			}
+			set_resync(hd, x ^ hd->no_sync);
+			hd->no_sync = x;
+		} else {
+			break; /* unknown keyword,syntax-error,... */
 		}
-		return len;
 	}
+	return len;
+#else
+	return 0;
+#endif
+}
+
+int
+wd33c93_show_info(struct seq_file *m, struct Scsi_Host *instance)
+{
+#ifdef PROC_INTERFACE
+	struct WD33C93_hostdata *hd;
+	struct scsi_cmnd *cmd;
+	int x;
+
+	hd = (struct WD33C93_hostdata *) instance->hostdata;
 
 	spin_lock_irq(&hd->lock);
-	bp = buf;
-	*bp = '\0';
-	if (hd->proc & PR_VERSION) {
-		sprintf(tbuf, "\nVersion %s - %s.",
+	if (hd->proc & PR_VERSION)
+		seq_printf(m, "\nVersion %s - %s.",
 			WD33C93_VERSION, WD33C93_DATE);
-		strcat(bp, tbuf);
-	}
+
 	if (hd->proc & PR_INFO) {
-		sprintf(tbuf, "\nclock_freq=%02x no_sync=%02x no_dma=%d"
+		seq_printf(m, "\nclock_freq=%02x no_sync=%02x no_dma=%d"
 			" dma_mode=%02x fast=%d",
 			hd->clock_freq, hd->no_sync, hd->no_dma, hd->dma_mode, hd->fast);
-		strcat(bp, tbuf);
-		strcat(bp, "\nsync_xfer[] =       ");
-		for (x = 0; x < 7; x++) {
-			sprintf(tbuf, "\t%02x", hd->sync_xfer[x]);
-			strcat(bp, tbuf);
-		}
-		strcat(bp, "\nsync_stat[] =       ");
-		for (x = 0; x < 7; x++) {
-			sprintf(tbuf, "\t%02x", hd->sync_stat[x]);
-			strcat(bp, tbuf);
-		}
+		seq_printf(m, "\nsync_xfer[] =       ");
+		for (x = 0; x < 7; x++)
+			seq_printf(m, "\t%02x", hd->sync_xfer[x]);
+		seq_printf(m, "\nsync_stat[] =       ");
+		for (x = 0; x < 7; x++)
+			seq_printf(m, "\t%02x", hd->sync_stat[x]);
 	}
 #ifdef PROC_STATISTICS
 	if (hd->proc & PR_STATISTICS) {
-		strcat(bp, "\ncommands issued:    ");
-		for (x = 0; x < 7; x++) {
-			sprintf(tbuf, "\t%ld", hd->cmd_cnt[x]);
-			strcat(bp, tbuf);
-		}
-		strcat(bp, "\ndisconnects allowed:");
-		for (x = 0; x < 7; x++) {
-			sprintf(tbuf, "\t%ld", hd->disc_allowed_cnt[x]);
-			strcat(bp, tbuf);
-		}
-		strcat(bp, "\ndisconnects done:   ");
-		for (x = 0; x < 7; x++) {
-			sprintf(tbuf, "\t%ld", hd->disc_done_cnt[x]);
-			strcat(bp, tbuf);
-		}
-		sprintf(tbuf,
+		seq_printf(m, "\ncommands issued:    ");
+		for (x = 0; x < 7; x++)
+			seq_printf(m, "\t%ld", hd->cmd_cnt[x]);
+		seq_printf(m, "\ndisconnects allowed:");
+		for (x = 0; x < 7; x++)
+			seq_printf(m, "\t%ld", hd->disc_allowed_cnt[x]);
+		seq_printf(m, "\ndisconnects done:   ");
+		for (x = 0; x < 7; x++)
+			seq_printf(m, "\t%ld", hd->disc_done_cnt[x]);
+		seq_printf(m,
 			"\ninterrupts: %ld, DATA_PHASE ints: %ld DMA, %ld PIO",
 			hd->int_cnt, hd->dma_cnt, hd->pio_cnt);
-		strcat(bp, tbuf);
 	}
 #endif
 	if (hd->proc & PR_CONNECTED) {
-		strcat(bp, "\nconnected:     ");
+		seq_printf(m, "\nconnected:     ");
 		if (hd->connected) {
 			cmd = (struct scsi_cmnd *) hd->connected;
-			sprintf(tbuf, " %d:%d(%02x)",
+			seq_printf(m, " %d:%d(%02x)",
 				cmd->device->id, cmd->device->lun, cmd->cmnd[0]);
-			strcat(bp, tbuf);
 		}
 	}
 	if (hd->proc & PR_INPUTQ) {
-		strcat(bp, "\ninput_Q:       ");
+		seq_printf(m, "\ninput_Q:       ");
 		cmd = (struct scsi_cmnd *) hd->input_Q;
 		while (cmd) {
-			sprintf(tbuf, " %d:%d(%02x)",
+			seq_printf(m, " %d:%d(%02x)",
 				cmd->device->id, cmd->device->lun, cmd->cmnd[0]);
-			strcat(bp, tbuf);
 			cmd = (struct scsi_cmnd *) cmd->host_scribble;
 		}
 	}
 	if (hd->proc & PR_DISCQ) {
-		strcat(bp, "\ndisconnected_Q:");
+		seq_printf(m, "\ndisconnected_Q:");
 		cmd = (struct scsi_cmnd *) hd->disconnected_Q;
 		while (cmd) {
-			sprintf(tbuf, " %d:%d(%02x)",
+			seq_printf(m, " %d:%d(%02x)",
 				cmd->device->id, cmd->device->lun, cmd->cmnd[0]);
-			strcat(bp, tbuf);
 			cmd = (struct scsi_cmnd *) cmd->host_scribble;
 		}
 	}
-	strcat(bp, "\n");
+	seq_printf(m, "\n");
 	spin_unlock_irq(&hd->lock);
-	*start = buf;
-	if (stop) {
-		stop = 0;
-		return 0;
-	}
-	if (off > 0x40000)	/* ALWAYS stop after 256k bytes have been read */
-		stop = 1;
-	if (hd->proc & PR_STOP)	/* stop every other time */
-		stop = 1;
-	return strlen(bp);
-
-#else				/* PROC_INTERFACE */
-
-	return 0;
-
 #endif				/* PROC_INTERFACE */
-
+	return 0;
 }
 
 EXPORT_SYMBOL(wd33c93_host_reset);
@@ -2229,4 +2202,5 @@ EXPORT_SYMBOL(wd33c93_init);
 EXPORT_SYMBOL(wd33c93_abort);
 EXPORT_SYMBOL(wd33c93_queuecommand);
 EXPORT_SYMBOL(wd33c93_intr);
-EXPORT_SYMBOL(wd33c93_proc_info);
+EXPORT_SYMBOL(wd33c93_show_info);
+EXPORT_SYMBOL(wd33c93_write_info);
diff --git a/drivers/scsi/wd33c93.h b/drivers/scsi/wd33c93.h
index 3b463d7304dc..08abe508e9ad 100644
--- a/drivers/scsi/wd33c93.h
+++ b/drivers/scsi/wd33c93.h
@@ -345,7 +345,8 @@ void wd33c93_init (struct Scsi_Host *instance, const wd33c93_regs regs,
 int wd33c93_abort (struct scsi_cmnd *cmd);
 int wd33c93_queuecommand (struct Scsi_Host *h, struct scsi_cmnd *cmd);
 void wd33c93_intr (struct Scsi_Host *instance);
-int wd33c93_proc_info(struct Scsi_Host *, char *, char **, off_t, int, int);
+int wd33c93_show_info(struct seq_file *, struct Scsi_Host *);
+int wd33c93_write_info(struct Scsi_Host *, char *, int);
 int wd33c93_host_reset (struct scsi_cmnd *);
 
 #endif /* WD33C93_H */
diff --git a/drivers/scsi/wd7000.c b/drivers/scsi/wd7000.c
index d89a5dfd3ade..f9a6e4b0affe 100644
--- a/drivers/scsi/wd7000.c
+++ b/drivers/scsi/wd7000.c
@@ -1296,9 +1296,9 @@ static void wd7000_revision(Adapter * host)
 
 
 #undef SPRINTF
-#define SPRINTF(args...) { if (pos < (buffer + length)) pos += sprintf (pos, ## args); }
+#define SPRINTF(args...) { seq_printf(m, ## args); }
 
-static int wd7000_set_info(char *buffer, int length, struct Scsi_Host *host)
+static int wd7000_set_info(struct Scsi_Host *host, char *buffer, int length)
 {
 	dprintk("Buffer = <%.*s>, length = %d\n", length, buffer, length);
 
@@ -1310,22 +1310,15 @@ static int wd7000_set_info(char *buffer, int length, struct Scsi_Host *host)
 }
 
 
-static int wd7000_proc_info(struct Scsi_Host *host, char *buffer, char **start, off_t offset, int length,  int inout)
+static int wd7000_show_info(struct seq_file *m, struct Scsi_Host *host)
 {
 	Adapter *adapter = (Adapter *)host->hostdata;
 	unsigned long flags;
-	char *pos = buffer;
 #ifdef WD7000_DEBUG
 	Mailbox *ogmbs, *icmbs;
 	short count;
 #endif
 
-	/*
-	 * Has data been written to the file ?
-	 */
-	if (inout)
-		return (wd7000_set_info(buffer, length, host));
-
 	spin_lock_irqsave(host->host_lock, flags);
 	SPRINTF("Host scsi%d: Western Digital WD-7000 (rev %d.%d)\n", host->host_no, adapter->rev1, adapter->rev2);
 	SPRINTF("  IO base:      0x%x\n", adapter->iobase);
@@ -1368,17 +1361,7 @@ static int wd7000_proc_info(struct Scsi_Host *host, char *buffer, char **start,
 
 	spin_unlock_irqrestore(host->host_lock, flags);
 
-	/*
-	 * Calculate start of next buffer, and return value.
-	 */
-	*start = buffer + offset;
-
-	if ((pos - buffer) < offset)
-		return (0);
-	else if ((pos - buffer - offset) < length)
-		return (pos - buffer - offset);
-	else
-		return (length);
+	return 0;
 }
 
 
@@ -1413,7 +1396,8 @@ static __init int wd7000_detect(struct scsi_host_template *tpnt)
 	for (i = 0; i < NUM_CONFIGS; biosptr[i++] = -1);
 
 	tpnt->proc_name = "wd7000";
-	tpnt->proc_info = &wd7000_proc_info;
+	tpnt->show_info = &wd7000_show_info;
+	tpnt->write_info = wd7000_set_info;
 
 	/*
 	 * Set up SCB free list, which is shared by all adapters
@@ -1658,7 +1642,8 @@ MODULE_LICENSE("GPL");
 
 static struct scsi_host_template driver_template = {
 	.proc_name		= "wd7000",
-	.proc_info		= wd7000_proc_info,
+	.show_info		= wd7000_show_info,
+	.write_info		= wd7000_set_info,
 	.name			= "Western Digital WD-7000",
 	.detect			= wd7000_detect,
 	.release		= wd7000_release,
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 141d8c10b764..92a9345d7a6b 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -62,7 +62,7 @@ config SPI_ALTERA
 
 config SPI_ATH79
 	tristate "Atheros AR71XX/AR724X/AR913X SPI controller driver"
-	depends on ATH79 && GENERIC_GPIO
+	depends on ATH79 && GPIOLIB
 	select SPI_BITBANG
 	help
 	  This enables support for the SPI controller present on the
@@ -175,7 +175,7 @@ config SPI_FALCON
 
 config SPI_GPIO
 	tristate "GPIO-based bitbanging SPI Master"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select SPI_BITBANG
 	help
 	  This simple GPIO bitbanging SPI master uses the arch-neutral GPIO
@@ -259,7 +259,7 @@ config SPI_FSL_ESPI
 
 config SPI_OC_TINY
 	tristate "OpenCores tiny SPI"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select SPI_BITBANG
 	help
 	  This is the driver for OpenCores tiny SPI master controller.
@@ -457,7 +457,7 @@ config SPI_TOPCLIFF_PCH
 
 config SPI_TXX9
 	tristate "Toshiba TXx9 SPI controller"
-	depends on GENERIC_GPIO && CPU_TX49XX
+	depends on GPIOLIB && CPU_TX49XX
 	help
 	  SPI driver for Toshiba TXx9 MIPS SoCs
 
diff --git a/drivers/spi/spi-davinci.c b/drivers/spi/spi-davinci.c
index 8234d2259722..2e8f24a1fb95 100644
--- a/drivers/spi/spi-davinci.c
+++ b/drivers/spi/spi-davinci.c
@@ -776,10 +776,10 @@ rx_dma_failed:
 #if defined(CONFIG_OF)
 static const struct of_device_id davinci_spi_of_match[] = {
 	{
-		.compatible = "ti,dm644x-spi",
+		.compatible = "ti,dm6441-spi",
 	},
 	{
-		.compatible = "ti,da8xx-spi",
+		.compatible = "ti,da830-spi",
 		.data = (void *)SPI_VERSION_2,
 	},
 	{ },
diff --git a/drivers/spi/spi-mxs.c b/drivers/spi/spi-mxs.c
index a1d5778e2bbb..84982768cd10 100644
--- a/drivers/spi/spi-mxs.c
+++ b/drivers/spi/spi-mxs.c
@@ -490,21 +490,6 @@ static int mxs_spi_transfer_one(struct spi_master *master,
 	return status;
 }
 
-static bool mxs_ssp_dma_filter(struct dma_chan *chan, void *param)
-{
-	struct mxs_ssp *ssp = param;
-
-	if (!mxs_dma_is_apbh(chan))
-		return false;
-
-	if (chan->chan_id != ssp->dma_channel)
-		return false;
-
-	chan->private = &ssp->dma_data;
-
-	return true;
-}
-
 static const struct of_device_id mxs_spi_dt_ids[] = {
 	{ .compatible = "fsl,imx23-spi", .data = (void *) IMX23_SSP, },
 	{ .compatible = "fsl,imx28-spi", .data = (void *) IMX28_SSP, },
@@ -520,13 +505,12 @@ static int mxs_spi_probe(struct platform_device *pdev)
 	struct spi_master *master;
 	struct mxs_spi *spi;
 	struct mxs_ssp *ssp;
-	struct resource *iores, *dmares;
+	struct resource *iores;
 	struct pinctrl *pinctrl;
 	struct clk *clk;
 	void __iomem *base;
-	int devid, dma_channel, clk_freq;
-	int ret = 0, irq_err, irq_dma;
-	dma_cap_mask_t mask;
+	int devid, clk_freq;
+	int ret = 0, irq_err;
 
 	/*
 	 * Default clock speed for the SPI core. 160MHz seems to
@@ -537,8 +521,7 @@ static int mxs_spi_probe(struct platform_device *pdev)
 
 	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	irq_err = platform_get_irq(pdev, 0);
-	irq_dma = platform_get_irq(pdev, 1);
-	if (!iores || irq_err < 0 || irq_dma < 0)
+	if (!iores || irq_err < 0)
 		return -EINVAL;
 
 	base = devm_ioremap_resource(&pdev->dev, iores);
@@ -553,32 +536,11 @@ static int mxs_spi_probe(struct platform_device *pdev)
 	if (IS_ERR(clk))
 		return PTR_ERR(clk);
 
-	if (np) {
-		devid = (enum mxs_ssp_id) of_id->data;
-		/*
-		 * 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(np, "fsl,ssp-dma-channel",
-					   &dma_channel);
-		if (ret) {
-			dev_err(&pdev->dev,
-				"Failed to get DMA channel\n");
-			return -EINVAL;
-		}
-
-		ret = of_property_read_u32(np, "clock-frequency",
-					   &clk_freq);
-		if (ret)
-			clk_freq = clk_freq_default;
-	} else {
-		dmares = platform_get_resource(pdev, IORESOURCE_DMA, 0);
-		if (!dmares)
-			return -EINVAL;
-		devid = pdev->id_entry->driver_data;
-		dma_channel = dmares->start;
+	devid = (enum mxs_ssp_id) of_id->data;
+	ret = of_property_read_u32(np, "clock-frequency",
+				   &clk_freq);
+	if (ret)
 		clk_freq = clk_freq_default;
-	}
 
 	master = spi_alloc_master(&pdev->dev, sizeof(*spi));
 	if (!master)
@@ -597,7 +559,6 @@ static int mxs_spi_probe(struct platform_device *pdev)
 	ssp->clk = clk;
 	ssp->base = base;
 	ssp->devid = devid;
-	ssp->dma_channel = dma_channel;
 
 	init_completion(&spi->c);
 
@@ -606,10 +567,7 @@ static int mxs_spi_probe(struct platform_device *pdev)
 	if (ret)
 		goto out_master_free;
 
-	dma_cap_zero(mask);
-	dma_cap_set(DMA_SLAVE, mask);
-	ssp->dma_data.chan_irq = irq_dma;
-	ssp->dmach = dma_request_channel(mask, mxs_ssp_dma_filter, ssp);
+	ssp->dmach = dma_request_slave_channel(&pdev->dev, "rx-tx");
 	if (!ssp->dmach) {
 		dev_err(ssp->dev, "Failed to request DMA\n");
 		ret = -ENODEV;
diff --git a/drivers/spi/spi-pl022.c b/drivers/spi/spi-pl022.c
index b0fe393c882c..371cc66f1a0e 100644
--- a/drivers/spi/spi-pl022.c
+++ b/drivers/spi/spi-pl022.c
@@ -1139,6 +1139,35 @@ err_no_rxchan:
 	return -ENODEV;
 }
 
+static int pl022_dma_autoprobe(struct pl022 *pl022)
+{
+	struct device *dev = &pl022->adev->dev;
+
+	/* automatically configure DMA channels from platform, normally using DT */
+	pl022->dma_rx_channel = dma_request_slave_channel(dev, "rx");
+	if (!pl022->dma_rx_channel)
+		goto err_no_rxchan;
+
+	pl022->dma_tx_channel = dma_request_slave_channel(dev, "tx");
+	if (!pl022->dma_tx_channel)
+		goto err_no_txchan;
+
+	pl022->dummypage = kmalloc(PAGE_SIZE, GFP_KERNEL);
+	if (!pl022->dummypage)
+		goto err_no_dummypage;
+
+	return 0;
+
+err_no_dummypage:
+	dma_release_channel(pl022->dma_tx_channel);
+	pl022->dma_tx_channel = NULL;
+err_no_txchan:
+	dma_release_channel(pl022->dma_rx_channel);
+	pl022->dma_rx_channel = NULL;
+err_no_rxchan:
+	return -ENODEV;
+}
+		
 static void terminate_dma(struct pl022 *pl022)
 {
 	struct dma_chan *rxchan = pl022->dma_rx_channel;
@@ -1167,6 +1196,11 @@ static inline int configure_dma(struct pl022 *pl022)
 	return -ENODEV;
 }
 
+static inline int pl022_dma_autoprobe(struct pl022 *pl022)
+{
+	return 0;
+}
+
 static inline int pl022_dma_probe(struct pl022 *pl022)
 {
 	return 0;
@@ -2226,8 +2260,13 @@ static int pl022_probe(struct amba_device *adev, const struct amba_id *id)
 		goto err_no_irq;
 	}
 
-	/* Get DMA channels */
-	if (platform_info->enable_dma) {
+	/* Get DMA channels, try autoconfiguration first */
+	status = pl022_dma_autoprobe(pl022);
+
+	/* If that failed, use channels from platform_info */
+	if (status == 0)
+		platform_info->enable_dma = 1;
+	else if (platform_info->enable_dma) {
 		status = pl022_dma_probe(pl022);
 		if (status != 0)
 			platform_info->enable_dma = 0;
diff --git a/drivers/ssb/driver_chipcommon.c b/drivers/ssb/driver_chipcommon.c
index 71098a7b5fed..7cb7d2c8fd86 100644
--- a/drivers/ssb/driver_chipcommon.c
+++ b/drivers/ssb/driver_chipcommon.c
@@ -354,7 +354,7 @@ void ssb_chipcommon_init(struct ssb_chipcommon *cc)
 
 	if (cc->dev->id.revision >= 11)
 		cc->status = chipco_read32(cc, SSB_CHIPCO_CHIPSTAT);
-	ssb_dprintk(KERN_INFO PFX "chipcommon status is 0x%x\n", cc->status);
+	ssb_dbg("chipcommon status is 0x%x\n", cc->status);
 
 	if (cc->dev->id.revision >= 20) {
 		chipco_write32(cc, SSB_CHIPCO_GPIOPULLUP, 0);
diff --git a/drivers/ssb/driver_chipcommon_pmu.c b/drivers/ssb/driver_chipcommon_pmu.c
index 7b0bce936762..1173a091b402 100644
--- a/drivers/ssb/driver_chipcommon_pmu.c
+++ b/drivers/ssb/driver_chipcommon_pmu.c
@@ -110,8 +110,8 @@ static void ssb_pmu0_pllinit_r0(struct ssb_chipcommon *cc,
 		return;
 	}
 
-	ssb_printk(KERN_INFO PFX "Programming PLL to %u.%03u MHz\n",
-		   (crystalfreq / 1000), (crystalfreq % 1000));
+	ssb_info("Programming PLL to %u.%03u MHz\n",
+		 crystalfreq / 1000, crystalfreq % 1000);
 
 	/* First turn the PLL off. */
 	switch (bus->chip_id) {
@@ -138,7 +138,7 @@ static void ssb_pmu0_pllinit_r0(struct ssb_chipcommon *cc,
 	}
 	tmp = chipco_read32(cc, SSB_CHIPCO_CLKCTLST);
 	if (tmp & SSB_CHIPCO_CLKCTLST_HAVEHT)
-		ssb_printk(KERN_EMERG PFX "Failed to turn the PLL off!\n");
+		ssb_emerg("Failed to turn the PLL off!\n");
 
 	/* Set PDIV in PLL control 0. */
 	pllctl = ssb_chipco_pll_read(cc, SSB_PMU0_PLLCTL0);
@@ -249,8 +249,8 @@ static void ssb_pmu1_pllinit_r0(struct ssb_chipcommon *cc,
 		return;
 	}
 
-	ssb_printk(KERN_INFO PFX "Programming PLL to %u.%03u MHz\n",
-		   (crystalfreq / 1000), (crystalfreq % 1000));
+	ssb_info("Programming PLL to %u.%03u MHz\n",
+		 crystalfreq / 1000, crystalfreq % 1000);
 
 	/* First turn the PLL off. */
 	switch (bus->chip_id) {
@@ -275,7 +275,7 @@ static void ssb_pmu1_pllinit_r0(struct ssb_chipcommon *cc,
 	}
 	tmp = chipco_read32(cc, SSB_CHIPCO_CLKCTLST);
 	if (tmp & SSB_CHIPCO_CLKCTLST_HAVEHT)
-		ssb_printk(KERN_EMERG PFX "Failed to turn the PLL off!\n");
+		ssb_emerg("Failed to turn the PLL off!\n");
 
 	/* Set p1div and p2div. */
 	pllctl = ssb_chipco_pll_read(cc, SSB_PMU1_PLLCTL0);
@@ -349,9 +349,8 @@ static void ssb_pmu_pll_init(struct ssb_chipcommon *cc)
 	case 43222:
 		break;
 	default:
-		ssb_printk(KERN_ERR PFX
-			   "ERROR: PLL init unknown for device %04X\n",
-			   bus->chip_id);
+		ssb_err("ERROR: PLL init unknown for device %04X\n",
+			bus->chip_id);
 	}
 }
 
@@ -472,9 +471,8 @@ static void ssb_pmu_resources_init(struct ssb_chipcommon *cc)
 		max_msk = 0xFFFFF;
 		break;
 	default:
-		ssb_printk(KERN_ERR PFX
-			   "ERROR: PMU resource config unknown for device %04X\n",
-			   bus->chip_id);
+		ssb_err("ERROR: PMU resource config unknown for device %04X\n",
+			bus->chip_id);
 	}
 
 	if (updown_tab) {
@@ -526,8 +524,8 @@ void ssb_pmu_init(struct ssb_chipcommon *cc)
 	pmucap = chipco_read32(cc, SSB_CHIPCO_PMU_CAP);
 	cc->pmu.rev = (pmucap & SSB_CHIPCO_PMU_CAP_REVISION);
 
-	ssb_dprintk(KERN_DEBUG PFX "Found rev %u PMU (capabilities 0x%08X)\n",
-		    cc->pmu.rev, pmucap);
+	ssb_dbg("Found rev %u PMU (capabilities 0x%08X)\n",
+		cc->pmu.rev, pmucap);
 
 	if (cc->pmu.rev == 1)
 		chipco_mask32(cc, SSB_CHIPCO_PMU_CTL,
@@ -638,9 +636,8 @@ u32 ssb_pmu_get_alp_clock(struct ssb_chipcommon *cc)
 	case 0x5354:
 		ssb_pmu_get_alp_clock_clk0(cc);
 	default:
-		ssb_printk(KERN_ERR PFX
-			   "ERROR: PMU alp clock unknown for device %04X\n",
-			   bus->chip_id);
+		ssb_err("ERROR: PMU alp clock unknown for device %04X\n",
+			bus->chip_id);
 		return 0;
 	}
 }
@@ -654,9 +651,8 @@ u32 ssb_pmu_get_cpu_clock(struct ssb_chipcommon *cc)
 		/* 5354 chip uses a non programmable PLL of frequency 240MHz */
 		return 240000000;
 	default:
-		ssb_printk(KERN_ERR PFX
-			   "ERROR: PMU cpu clock unknown for device %04X\n",
-			   bus->chip_id);
+		ssb_err("ERROR: PMU cpu clock unknown for device %04X\n",
+			bus->chip_id);
 		return 0;
 	}
 }
@@ -669,9 +665,8 @@ u32 ssb_pmu_get_controlclock(struct ssb_chipcommon *cc)
 	case 0x5354:
 		return 120000000;
 	default:
-		ssb_printk(KERN_ERR PFX
-			   "ERROR: PMU controlclock unknown for device %04X\n",
-			   bus->chip_id);
+		ssb_err("ERROR: PMU controlclock unknown for device %04X\n",
+			bus->chip_id);
 		return 0;
 	}
 }
@@ -692,8 +687,23 @@ void ssb_pmu_spuravoid_pllupdate(struct ssb_chipcommon *cc, int spuravoid)
 		pmu_ctl = SSB_CHIPCO_PMU_CTL_PLL_UPD;
 		break;
 	case 43222:
-		/* TODO: BCM43222 requires updating PLLs too */
-		return;
+		if (spuravoid == 1) {
+			ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL0, 0x11500008);
+			ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL1, 0x0C000C06);
+			ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL2, 0x0F600a08);
+			ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL3, 0x00000000);
+			ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL4, 0x2001E920);
+			ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL5, 0x88888815);
+		} else {
+			ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL0, 0x11100008);
+			ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL1, 0x0c000c06);
+			ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL2, 0x03000a08);
+			ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL3, 0x00000000);
+			ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL4, 0x200005c0);
+			ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL5, 0x88888855);
+		}
+		pmu_ctl = SSB_CHIPCO_PMU_CTL_PLL_UPD;
+		break;
 	default:
 		ssb_printk(KERN_ERR PFX
 			   "Unknown spuravoidance settings for chip 0x%04X, not changing PLL\n",
diff --git a/drivers/ssb/driver_mipscore.c b/drivers/ssb/driver_mipscore.c
index 33b37dac40bd..09077067b0c8 100644
--- a/drivers/ssb/driver_mipscore.c
+++ b/drivers/ssb/driver_mipscore.c
@@ -18,7 +18,7 @@
 
 #include "ssb_private.h"
 
-static const char *part_probes[] = { "bcm47xxpart", NULL };
+static const char * const part_probes[] = { "bcm47xxpart", NULL };
 
 static struct physmap_flash_data ssb_pflash_data = {
 	.part_probe_types	= part_probes,
@@ -167,21 +167,22 @@ static void set_irq(struct ssb_device *dev, unsigned int irq)
 		irqflag |= (ipsflag & ~ipsflag_irq_mask[irq]);
 		ssb_write32(mdev, SSB_IPSFLAG, irqflag);
 	}
-	ssb_dprintk(KERN_INFO PFX
-		    "set_irq: core 0x%04x, irq %d => %d\n",
-		    dev->id.coreid, oldirq+2, irq+2);
+	ssb_dbg("set_irq: core 0x%04x, irq %d => %d\n",
+		dev->id.coreid, oldirq+2, irq+2);
 }
 
 static void print_irq(struct ssb_device *dev, unsigned int irq)
 {
-	int i;
 	static const char *irq_name[] = {"2(S)", "3", "4", "5", "6", "D", "I"};
-	ssb_dprintk(KERN_INFO PFX
-		"core 0x%04x, irq :", dev->id.coreid);
-	for (i = 0; i <= 6; i++) {
-		ssb_dprintk(" %s%s", irq_name[i], i==irq?"*":" ");
-	}
-	ssb_dprintk("\n");
+	ssb_dbg("core 0x%04x, irq : %s%s %s%s %s%s %s%s %s%s %s%s %s%s\n",
+		dev->id.coreid,
+		irq_name[0], irq == 0 ? "*" : " ",
+		irq_name[1], irq == 1 ? "*" : " ",
+		irq_name[2], irq == 2 ? "*" : " ",
+		irq_name[3], irq == 3 ? "*" : " ",
+		irq_name[4], irq == 4 ? "*" : " ",
+		irq_name[5], irq == 5 ? "*" : " ",
+		irq_name[6], irq == 6 ? "*" : " ");
 }
 
 static void dump_irq(struct ssb_bus *bus)
@@ -286,7 +287,7 @@ void ssb_mipscore_init(struct ssb_mipscore *mcore)
 	if (!mcore->dev)
 		return; /* We don't have a MIPS core */
 
-	ssb_dprintk(KERN_INFO PFX "Initializing MIPS core...\n");
+	ssb_dbg("Initializing MIPS core...\n");
 
 	bus = mcore->dev->bus;
 	hz = ssb_clockspeed(bus);
@@ -334,7 +335,7 @@ void ssb_mipscore_init(struct ssb_mipscore *mcore)
 			break;
 		}
 	}
-	ssb_dprintk(KERN_INFO PFX "after irq reconfiguration\n");
+	ssb_dbg("after irq reconfiguration\n");
 	dump_irq(bus);
 
 	ssb_mips_serial_init(mcore);
diff --git a/drivers/ssb/driver_pcicore.c b/drivers/ssb/driver_pcicore.c
index 59801d23d7ec..d75b72ba2672 100644
--- a/drivers/ssb/driver_pcicore.c
+++ b/drivers/ssb/driver_pcicore.c
@@ -263,8 +263,7 @@ int ssb_pcicore_plat_dev_init(struct pci_dev *d)
 		return -ENODEV;
 	}
 
-	ssb_printk(KERN_INFO "PCI: Fixing up device %s\n",
-		   pci_name(d));
+	ssb_info("PCI: Fixing up device %s\n", pci_name(d));
 
 	/* Fix up interrupt lines */
 	d->irq = ssb_mips_irq(extpci_core->dev) + 2;
@@ -285,12 +284,12 @@ static void ssb_pcicore_fixup_pcibridge(struct pci_dev *dev)
 	if (dev->bus->number != 0 || PCI_SLOT(dev->devfn) != 0)
 		return;
 
-	ssb_printk(KERN_INFO "PCI: Fixing up bridge %s\n", pci_name(dev));
+	ssb_info("PCI: Fixing up bridge %s\n", pci_name(dev));
 
 	/* Enable PCI bridge bus mastering and memory space */
 	pci_set_master(dev);
 	if (pcibios_enable_device(dev, ~0) < 0) {
-		ssb_printk(KERN_ERR "PCI: SSB bridge enable failed\n");
+		ssb_err("PCI: SSB bridge enable failed\n");
 		return;
 	}
 
@@ -299,8 +298,8 @@ static void ssb_pcicore_fixup_pcibridge(struct pci_dev *dev)
 
 	/* Make sure our latency is high enough to handle the devices behind us */
 	lat = 168;
-	ssb_printk(KERN_INFO "PCI: Fixing latency timer of device %s to %u\n",
-		   pci_name(dev), lat);
+	ssb_info("PCI: Fixing latency timer of device %s to %u\n",
+		 pci_name(dev), lat);
 	pci_write_config_byte(dev, PCI_LATENCY_TIMER, lat);
 }
 DECLARE_PCI_FIXUP_EARLY(PCI_ANY_ID, PCI_ANY_ID, ssb_pcicore_fixup_pcibridge);
@@ -323,7 +322,7 @@ static void ssb_pcicore_init_hostmode(struct ssb_pcicore *pc)
 		return;
 	extpci_core = pc;
 
-	ssb_dprintk(KERN_INFO PFX "PCIcore in host mode found\n");
+	ssb_dbg("PCIcore in host mode found\n");
 	/* Reset devices on the external PCI bus */
 	val = SSB_PCICORE_CTL_RST_OE;
 	val |= SSB_PCICORE_CTL_CLK_OE;
@@ -338,7 +337,7 @@ static void ssb_pcicore_init_hostmode(struct ssb_pcicore *pc)
 	udelay(1); /* Assertion time demanded by the PCI standard */
 
 	if (pc->dev->bus->has_cardbus_slot) {
-		ssb_dprintk(KERN_INFO PFX "CardBus slot detected\n");
+		ssb_dbg("CardBus slot detected\n");
 		pc->cardbusmode = 1;
 		/* GPIO 1 resets the bridge */
 		ssb_gpio_out(pc->dev->bus, 1, 1);
diff --git a/drivers/ssb/embedded.c b/drivers/ssb/embedded.c
index bb18d76f9f2c..55e101115038 100644
--- a/drivers/ssb/embedded.c
+++ b/drivers/ssb/embedded.c
@@ -57,9 +57,8 @@ int ssb_watchdog_register(struct ssb_bus *bus)
 					     bus->busnumber, &wdt,
 					     sizeof(wdt));
 	if (IS_ERR(pdev)) {
-		ssb_dprintk(KERN_INFO PFX
-			    "can not register watchdog device, err: %li\n",
-			    PTR_ERR(pdev));
+		ssb_dbg("can not register watchdog device, err: %li\n",
+			PTR_ERR(pdev));
 		return PTR_ERR(pdev);
 	}
 
diff --git a/drivers/ssb/main.c b/drivers/ssb/main.c
index 3b645b8a261f..812775a4bfb6 100644
--- a/drivers/ssb/main.c
+++ b/drivers/ssb/main.c
@@ -275,8 +275,8 @@ int ssb_devices_thaw(struct ssb_freeze_context *ctx)
 
 		err = sdrv->probe(sdev, &sdev->id);
 		if (err) {
-			ssb_printk(KERN_ERR PFX "Failed to thaw device %s\n",
-				   dev_name(sdev->dev));
+			ssb_err("Failed to thaw device %s\n",
+				dev_name(sdev->dev));
 			result = err;
 		}
 		ssb_device_put(sdev);
@@ -447,10 +447,9 @@ void ssb_bus_unregister(struct ssb_bus *bus)
 
 	err = ssb_gpio_unregister(bus);
 	if (err == -EBUSY)
-		ssb_dprintk(KERN_ERR PFX "Some GPIOs are still in use.\n");
+		ssb_dbg("Some GPIOs are still in use\n");
 	else if (err)
-		ssb_dprintk(KERN_ERR PFX
-			    "Can not unregister GPIO driver: %i\n", err);
+		ssb_dbg("Can not unregister GPIO driver: %i\n", err);
 
 	ssb_buses_lock();
 	ssb_devices_unregister(bus);
@@ -497,8 +496,7 @@ static int ssb_devices_register(struct ssb_bus *bus)
 
 		devwrap = kzalloc(sizeof(*devwrap), GFP_KERNEL);
 		if (!devwrap) {
-			ssb_printk(KERN_ERR PFX
-				   "Could not allocate device\n");
+			ssb_err("Could not allocate device\n");
 			err = -ENOMEM;
 			goto error;
 		}
@@ -537,9 +535,7 @@ static int ssb_devices_register(struct ssb_bus *bus)
 		sdev->dev = dev;
 		err = device_register(dev);
 		if (err) {
-			ssb_printk(KERN_ERR PFX
-				   "Could not register %s\n",
-				   dev_name(dev));
+			ssb_err("Could not register %s\n", dev_name(dev));
 			/* Set dev to NULL to not unregister
 			 * dev on error unwinding. */
 			sdev->dev = NULL;
@@ -825,10 +821,9 @@ static int ssb_bus_register(struct ssb_bus *bus,
 	ssb_mipscore_init(&bus->mipscore);
 	err = ssb_gpio_init(bus);
 	if (err == -ENOTSUPP)
-		ssb_dprintk(KERN_DEBUG PFX "GPIO driver not activated\n");
+		ssb_dbg("GPIO driver not activated\n");
 	else if (err)
-		ssb_dprintk(KERN_ERR PFX
-			   "Error registering GPIO driver: %i\n", err);
+		ssb_dbg("Error registering GPIO driver: %i\n", err);
 	err = ssb_fetch_invariants(bus, get_invariants);
 	if (err) {
 		ssb_bus_may_powerdown(bus);
@@ -878,11 +873,11 @@ int ssb_bus_pcibus_register(struct ssb_bus *bus, struct pci_dev *host_pci)
 
 	err = ssb_bus_register(bus, ssb_pci_get_invariants, 0);
 	if (!err) {
-		ssb_printk(KERN_INFO PFX "Sonics Silicon Backplane found on "
-			   "PCI device %s\n", dev_name(&host_pci->dev));
+		ssb_info("Sonics Silicon Backplane found on PCI device %s\n",
+			 dev_name(&host_pci->dev));
 	} else {
-		ssb_printk(KERN_ERR PFX "Failed to register PCI version"
-			   " of SSB with error %d\n", err);
+		ssb_err("Failed to register PCI version of SSB with error %d\n",
+			err);
 	}
 
 	return err;
@@ -903,8 +898,8 @@ int ssb_bus_pcmciabus_register(struct ssb_bus *bus,
 
 	err = ssb_bus_register(bus, ssb_pcmcia_get_invariants, baseaddr);
 	if (!err) {
-		ssb_printk(KERN_INFO PFX "Sonics Silicon Backplane found on "
-			   "PCMCIA device %s\n", pcmcia_dev->devname);
+		ssb_info("Sonics Silicon Backplane found on PCMCIA device %s\n",
+			 pcmcia_dev->devname);
 	}
 
 	return err;
@@ -925,8 +920,8 @@ int ssb_bus_sdiobus_register(struct ssb_bus *bus, struct sdio_func *func,
 
 	err = ssb_bus_register(bus, ssb_sdio_get_invariants, ~0);
 	if (!err) {
-		ssb_printk(KERN_INFO PFX "Sonics Silicon Backplane found on "
-			   "SDIO device %s\n", sdio_func_id(func));
+		ssb_info("Sonics Silicon Backplane found on SDIO device %s\n",
+			 sdio_func_id(func));
 	}
 
 	return err;
@@ -944,8 +939,8 @@ int ssb_bus_ssbbus_register(struct ssb_bus *bus, unsigned long baseaddr,
 
 	err = ssb_bus_register(bus, get_invariants, baseaddr);
 	if (!err) {
-		ssb_printk(KERN_INFO PFX "Sonics Silicon Backplane found at "
-			   "address 0x%08lX\n", baseaddr);
+		ssb_info("Sonics Silicon Backplane found at address 0x%08lX\n",
+			 baseaddr);
 	}
 
 	return err;
@@ -1339,7 +1334,7 @@ out:
 #endif
 	return err;
 error:
-	ssb_printk(KERN_ERR PFX "Bus powerdown failed\n");
+	ssb_err("Bus powerdown failed\n");
 	goto out;
 }
 EXPORT_SYMBOL(ssb_bus_may_powerdown);
@@ -1362,7 +1357,7 @@ int ssb_bus_powerup(struct ssb_bus *bus, bool dynamic_pctl)
 
 	return 0;
 error:
-	ssb_printk(KERN_ERR PFX "Bus powerup failed\n");
+	ssb_err("Bus powerup failed\n");
 	return err;
 }
 EXPORT_SYMBOL(ssb_bus_powerup);
@@ -1470,15 +1465,13 @@ static int __init ssb_modinit(void)
 
 	err = b43_pci_ssb_bridge_init();
 	if (err) {
-		ssb_printk(KERN_ERR "Broadcom 43xx PCI-SSB-bridge "
-			   "initialization failed\n");
+		ssb_err("Broadcom 43xx PCI-SSB-bridge initialization failed\n");
 		/* don't fail SSB init because of this */
 		err = 0;
 	}
 	err = ssb_gige_init();
 	if (err) {
-		ssb_printk(KERN_ERR "SSB Broadcom Gigabit Ethernet "
-			   "driver initialization failed\n");
+		ssb_err("SSB Broadcom Gigabit Ethernet driver initialization failed\n");
 		/* don't fail SSB init because of this */
 		err = 0;
 	}
diff --git a/drivers/ssb/pci.c b/drivers/ssb/pci.c
index e9d94968f394..a8dc95ebf2d6 100644
--- a/drivers/ssb/pci.c
+++ b/drivers/ssb/pci.c
@@ -56,7 +56,7 @@ int ssb_pci_switch_coreidx(struct ssb_bus *bus, u8 coreidx)
 	}
 	return 0;
 error:
-	ssb_printk(KERN_ERR PFX "Failed to switch to core %u\n", coreidx);
+	ssb_err("Failed to switch to core %u\n", coreidx);
 	return -ENODEV;
 }
 
@@ -67,10 +67,9 @@ int ssb_pci_switch_core(struct ssb_bus *bus,
 	unsigned long flags;
 
 #if SSB_VERBOSE_PCICORESWITCH_DEBUG
-	ssb_printk(KERN_INFO PFX
-		   "Switching to %s core, index %d\n",
-		   ssb_core_name(dev->id.coreid),
-		   dev->core_index);
+	ssb_info("Switching to %s core, index %d\n",
+		 ssb_core_name(dev->id.coreid),
+		 dev->core_index);
 #endif
 
 	spin_lock_irqsave(&bus->bar_lock, flags);
@@ -231,6 +230,15 @@ static inline u8 ssb_crc8(u8 crc, u8 data)
 	return t[crc ^ data];
 }
 
+static void sprom_get_mac(char *mac, const u16 *in)
+{
+	int i;
+	for (i = 0; i < 3; i++) {
+		*mac++ = in[i] >> 8;
+		*mac++ = in[i];
+	}
+}
+
 static u8 ssb_sprom_crc(const u16 *sprom, u16 size)
 {
 	int word;
@@ -278,7 +286,7 @@ static int sprom_do_write(struct ssb_bus *bus, const u16 *sprom)
 	u32 spromctl;
 	u16 size = bus->sprom_size;
 
-	ssb_printk(KERN_NOTICE PFX "Writing SPROM. Do NOT turn off the power! Please stand by...\n");
+	ssb_notice("Writing SPROM. Do NOT turn off the power! Please stand by...\n");
 	err = pci_read_config_dword(pdev, SSB_SPROMCTL, &spromctl);
 	if (err)
 		goto err_ctlreg;
@@ -286,17 +294,17 @@ static int sprom_do_write(struct ssb_bus *bus, const u16 *sprom)
 	err = pci_write_config_dword(pdev, SSB_SPROMCTL, spromctl);
 	if (err)
 		goto err_ctlreg;
-	ssb_printk(KERN_NOTICE PFX "[ 0%%");
+	ssb_notice("[ 0%%");
 	msleep(500);
 	for (i = 0; i < size; i++) {
 		if (i == size / 4)
-			ssb_printk("25%%");
+			ssb_cont("25%%");
 		else if (i == size / 2)
-			ssb_printk("50%%");
+			ssb_cont("50%%");
 		else if (i == (size * 3) / 4)
-			ssb_printk("75%%");
+			ssb_cont("75%%");
 		else if (i % 2)
-			ssb_printk(".");
+			ssb_cont(".");
 		writew(sprom[i], bus->mmio + bus->sprom_offset + (i * 2));
 		mmiowb();
 		msleep(20);
@@ -309,12 +317,12 @@ static int sprom_do_write(struct ssb_bus *bus, const u16 *sprom)
 	if (err)
 		goto err_ctlreg;
 	msleep(500);
-	ssb_printk("100%% ]\n");
-	ssb_printk(KERN_NOTICE PFX "SPROM written.\n");
+	ssb_cont("100%% ]\n");
+	ssb_notice("SPROM written\n");
 
 	return 0;
 err_ctlreg:
-	ssb_printk(KERN_ERR PFX "Could not access SPROM control register.\n");
+	ssb_err("Could not access SPROM control register.\n");
 	return err;
 }
 
@@ -339,10 +347,23 @@ static s8 r123_extract_antgain(u8 sprom_revision, const u16 *in,
 	return (s8)gain;
 }
 
+static void sprom_extract_r23(struct ssb_sprom *out, const u16 *in)
+{
+	SPEX(boardflags_hi, SSB_SPROM2_BFLHI, 0xFFFF, 0);
+	SPEX(opo, SSB_SPROM2_OPO, SSB_SPROM2_OPO_VALUE, 0);
+	SPEX(pa1lob0, SSB_SPROM2_PA1LOB0, 0xFFFF, 0);
+	SPEX(pa1lob1, SSB_SPROM2_PA1LOB1, 0xFFFF, 0);
+	SPEX(pa1lob2, SSB_SPROM2_PA1LOB2, 0xFFFF, 0);
+	SPEX(pa1hib0, SSB_SPROM2_PA1HIB0, 0xFFFF, 0);
+	SPEX(pa1hib1, SSB_SPROM2_PA1HIB1, 0xFFFF, 0);
+	SPEX(pa1hib2, SSB_SPROM2_PA1HIB2, 0xFFFF, 0);
+	SPEX(maxpwr_ah, SSB_SPROM2_MAXP_A, SSB_SPROM2_MAXP_A_HI, 0);
+	SPEX(maxpwr_al, SSB_SPROM2_MAXP_A, SSB_SPROM2_MAXP_A_LO,
+	     SSB_SPROM2_MAXP_A_LO_SHIFT);
+}
+
 static void sprom_extract_r123(struct ssb_sprom *out, const u16 *in)
 {
-	int i;
-	u16 v;
 	u16 loc[3];
 
 	if (out->revision == 3)			/* rev 3 moved MAC */
@@ -352,19 +373,10 @@ static void sprom_extract_r123(struct ssb_sprom *out, const u16 *in)
 		loc[1] = SSB_SPROM1_ET0MAC;
 		loc[2] = SSB_SPROM1_ET1MAC;
 	}
-	for (i = 0; i < 3; i++) {
-		v = in[SPOFF(loc[0]) + i];
-		*(((__be16 *)out->il0mac) + i) = cpu_to_be16(v);
-	}
+	sprom_get_mac(out->il0mac, &in[SPOFF(loc[0])]);
 	if (out->revision < 3) { 	/* only rev 1-2 have et0, et1 */
-		for (i = 0; i < 3; i++) {
-			v = in[SPOFF(loc[1]) + i];
-			*(((__be16 *)out->et0mac) + i) = cpu_to_be16(v);
-		}
-		for (i = 0; i < 3; i++) {
-			v = in[SPOFF(loc[2]) + i];
-			*(((__be16 *)out->et1mac) + i) = cpu_to_be16(v);
-		}
+		sprom_get_mac(out->et0mac, &in[SPOFF(loc[1])]);
+		sprom_get_mac(out->et1mac, &in[SPOFF(loc[2])]);
 	}
 	SPEX(et0phyaddr, SSB_SPROM1_ETHPHY, SSB_SPROM1_ETHPHY_ET0A, 0);
 	SPEX(et1phyaddr, SSB_SPROM1_ETHPHY, SSB_SPROM1_ETHPHY_ET1A,
@@ -372,6 +384,7 @@ static void sprom_extract_r123(struct ssb_sprom *out, const u16 *in)
 	SPEX(et0mdcport, SSB_SPROM1_ETHPHY, SSB_SPROM1_ETHPHY_ET0M, 14);
 	SPEX(et1mdcport, SSB_SPROM1_ETHPHY, SSB_SPROM1_ETHPHY_ET1M, 15);
 	SPEX(board_rev, SSB_SPROM1_BINF, SSB_SPROM1_BINF_BREV, 0);
+	SPEX(board_type, SSB_SPROM1_SPID, 0xFFFF, 0);
 	if (out->revision == 1)
 		SPEX(country_code, SSB_SPROM1_BINF, SSB_SPROM1_BINF_CCODE,
 		     SSB_SPROM1_BINF_CCODE_SHIFT);
@@ -398,8 +411,7 @@ static void sprom_extract_r123(struct ssb_sprom *out, const u16 *in)
 	     SSB_SPROM1_ITSSI_A_SHIFT);
 	SPEX(itssi_bg, SSB_SPROM1_ITSSI, SSB_SPROM1_ITSSI_BG, 0);
 	SPEX(boardflags_lo, SSB_SPROM1_BFLLO, 0xFFFF, 0);
-	if (out->revision >= 2)
-		SPEX(boardflags_hi, SSB_SPROM2_BFLHI, 0xFFFF, 0);
+
 	SPEX(alpha2[0], SSB_SPROM1_CCODE, 0xff00, 8);
 	SPEX(alpha2[1], SSB_SPROM1_CCODE, 0x00ff, 0);
 
@@ -410,6 +422,8 @@ static void sprom_extract_r123(struct ssb_sprom *out, const u16 *in)
 	out->antenna_gain.a1 = r123_extract_antgain(out->revision, in,
 						    SSB_SPROM1_AGAIN_A,
 						    SSB_SPROM1_AGAIN_A_SHIFT);
+	if (out->revision >= 2)
+		sprom_extract_r23(out, in);
 }
 
 /* Revs 4 5 and 8 have partially shared layout */
@@ -454,23 +468,20 @@ static void sprom_extract_r458(struct ssb_sprom *out, const u16 *in)
 
 static void sprom_extract_r45(struct ssb_sprom *out, const u16 *in)
 {
-	int i;
-	u16 v;
 	u16 il0mac_offset;
 
 	if (out->revision == 4)
 		il0mac_offset = SSB_SPROM4_IL0MAC;
 	else
 		il0mac_offset = SSB_SPROM5_IL0MAC;
-	/* extract the MAC address */
-	for (i = 0; i < 3; i++) {
-		v = in[SPOFF(il0mac_offset) + i];
-		*(((__be16 *)out->il0mac) + i) = cpu_to_be16(v);
-	}
+
+	sprom_get_mac(out->il0mac, &in[SPOFF(il0mac_offset)]);
+
 	SPEX(et0phyaddr, SSB_SPROM4_ETHPHY, SSB_SPROM4_ETHPHY_ET0A, 0);
 	SPEX(et1phyaddr, SSB_SPROM4_ETHPHY, SSB_SPROM4_ETHPHY_ET1A,
 	     SSB_SPROM4_ETHPHY_ET1A_SHIFT);
 	SPEX(board_rev, SSB_SPROM4_BOARDREV, 0xFFFF, 0);
+	SPEX(board_type, SSB_SPROM1_SPID, 0xFFFF, 0);
 	if (out->revision == 4) {
 		SPEX(alpha2[0], SSB_SPROM4_CCODE, 0xff00, 8);
 		SPEX(alpha2[1], SSB_SPROM4_CCODE, 0x00ff, 0);
@@ -530,7 +541,7 @@ static void sprom_extract_r45(struct ssb_sprom *out, const u16 *in)
 static void sprom_extract_r8(struct ssb_sprom *out, const u16 *in)
 {
 	int i;
-	u16 v, o;
+	u16 o;
 	u16 pwr_info_offset[] = {
 		SSB_SROM8_PWR_INFO_CORE0, SSB_SROM8_PWR_INFO_CORE1,
 		SSB_SROM8_PWR_INFO_CORE2, SSB_SROM8_PWR_INFO_CORE3
@@ -539,11 +550,10 @@ static void sprom_extract_r8(struct ssb_sprom *out, const u16 *in)
 			ARRAY_SIZE(out->core_pwr_info));
 
 	/* extract the MAC address */
-	for (i = 0; i < 3; i++) {
-		v = in[SPOFF(SSB_SPROM8_IL0MAC) + i];
-		*(((__be16 *)out->il0mac) + i) = cpu_to_be16(v);
-	}
+	sprom_get_mac(out->il0mac, &in[SPOFF(SSB_SPROM8_IL0MAC)]);
+
 	SPEX(board_rev, SSB_SPROM8_BOARDREV, 0xFFFF, 0);
+	SPEX(board_type, SSB_SPROM1_SPID, 0xFFFF, 0);
 	SPEX(alpha2[0], SSB_SPROM8_CCODE, 0xff00, 8);
 	SPEX(alpha2[1], SSB_SPROM8_CCODE, 0x00ff, 0);
 	SPEX(boardflags_lo, SSB_SPROM8_BFLLO, 0xFFFF, 0);
@@ -743,7 +753,7 @@ static int sprom_extract(struct ssb_bus *bus, struct ssb_sprom *out,
 	memset(out, 0, sizeof(*out));
 
 	out->revision = in[size - 1] & 0x00FF;
-	ssb_dprintk(KERN_DEBUG PFX "SPROM revision %d detected.\n", out->revision);
+	ssb_dbg("SPROM revision %d detected\n", out->revision);
 	memset(out->et0mac, 0xFF, 6);		/* preset et0 and et1 mac */
 	memset(out->et1mac, 0xFF, 6);
 
@@ -752,7 +762,7 @@ static int sprom_extract(struct ssb_bus *bus, struct ssb_sprom *out,
 		 * number stored in the SPROM.
 		 * Always extract r1. */
 		out->revision = 1;
-		ssb_dprintk(KERN_DEBUG PFX "SPROM treated as revision %d\n", out->revision);
+		ssb_dbg("SPROM treated as revision %d\n", out->revision);
 	}
 
 	switch (out->revision) {
@@ -769,9 +779,8 @@ static int sprom_extract(struct ssb_bus *bus, struct ssb_sprom *out,
 		sprom_extract_r8(out, in);
 		break;
 	default:
-		ssb_printk(KERN_WARNING PFX "Unsupported SPROM"
-			   " revision %d detected. Will extract"
-			   " v1\n", out->revision);
+		ssb_warn("Unsupported SPROM revision %d detected. Will extract v1\n",
+			 out->revision);
 		out->revision = 1;
 		sprom_extract_r123(out, in);
 	}
@@ -791,7 +800,7 @@ static int ssb_pci_sprom_get(struct ssb_bus *bus,
 	u16 *buf;
 
 	if (!ssb_is_sprom_available(bus)) {
-		ssb_printk(KERN_ERR PFX "No SPROM available!\n");
+		ssb_err("No SPROM available!\n");
 		return -ENODEV;
 	}
 	if (bus->chipco.dev) {	/* can be unavailable! */
@@ -810,7 +819,7 @@ static int ssb_pci_sprom_get(struct ssb_bus *bus,
 	} else {
 		bus->sprom_offset = SSB_SPROM_BASE1;
 	}
-	ssb_dprintk(KERN_INFO PFX "SPROM offset is 0x%x\n", bus->sprom_offset);
+	ssb_dbg("SPROM offset is 0x%x\n", bus->sprom_offset);
 
 	buf = kcalloc(SSB_SPROMSIZE_WORDS_R123, sizeof(u16), GFP_KERNEL);
 	if (!buf)
@@ -835,18 +844,15 @@ static int ssb_pci_sprom_get(struct ssb_bus *bus,
 			 * available for this device in some other storage */
 			err = ssb_fill_sprom_with_fallback(bus, sprom);
 			if (err) {
-				ssb_printk(KERN_WARNING PFX "WARNING: Using"
-					   " fallback SPROM failed (err %d)\n",
-					   err);
+				ssb_warn("WARNING: Using fallback SPROM failed (err %d)\n",
+					 err);
 			} else {
-				ssb_dprintk(KERN_DEBUG PFX "Using SPROM"
-					    " revision %d provided by"
-					    " platform.\n", sprom->revision);
+				ssb_dbg("Using SPROM revision %d provided by platform\n",
+					sprom->revision);
 				err = 0;
 				goto out_free;
 			}
-			ssb_printk(KERN_WARNING PFX "WARNING: Invalid"
-				   " SPROM CRC (corrupt SPROM)\n");
+			ssb_warn("WARNING: Invalid SPROM CRC (corrupt SPROM)\n");
 		}
 	}
 	err = sprom_extract(bus, sprom, buf, bus->sprom_size);
diff --git a/drivers/ssb/pcmcia.c b/drivers/ssb/pcmcia.c
index fbafed5b729b..b413e0187087 100644
--- a/drivers/ssb/pcmcia.c
+++ b/drivers/ssb/pcmcia.c
@@ -143,7 +143,7 @@ int ssb_pcmcia_switch_coreidx(struct ssb_bus *bus,
 
 	return 0;
 error:
-	ssb_printk(KERN_ERR PFX "Failed to switch to core %u\n", coreidx);
+	ssb_err("Failed to switch to core %u\n", coreidx);
 	return err;
 }
 
@@ -153,10 +153,9 @@ int ssb_pcmcia_switch_core(struct ssb_bus *bus,
 	int err;
 
 #if SSB_VERBOSE_PCMCIACORESWITCH_DEBUG
-	ssb_printk(KERN_INFO PFX
-		   "Switching to %s core, index %d\n",
-		   ssb_core_name(dev->id.coreid),
-		   dev->core_index);
+	ssb_info("Switching to %s core, index %d\n",
+		 ssb_core_name(dev->id.coreid),
+		 dev->core_index);
 #endif
 
 	err = ssb_pcmcia_switch_coreidx(bus, dev->core_index);
@@ -192,7 +191,7 @@ int ssb_pcmcia_switch_segment(struct ssb_bus *bus, u8 seg)
 
 	return 0;
 error:
-	ssb_printk(KERN_ERR PFX "Failed to switch pcmcia segment\n");
+	ssb_err("Failed to switch pcmcia segment\n");
 	return err;
 }
 
@@ -549,44 +548,39 @@ static int ssb_pcmcia_sprom_write_all(struct ssb_bus *bus, const u16 *sprom)
 	bool failed = 0;
 	size_t size = SSB_PCMCIA_SPROM_SIZE;
 
-	ssb_printk(KERN_NOTICE PFX
-		   "Writing SPROM. Do NOT turn off the power! "
-		   "Please stand by...\n");
+	ssb_notice("Writing SPROM. Do NOT turn off the power! Please stand by...\n");
 	err = ssb_pcmcia_sprom_command(bus, SSB_PCMCIA_SPROMCTL_WRITEEN);
 	if (err) {
-		ssb_printk(KERN_NOTICE PFX
-			   "Could not enable SPROM write access.\n");
+		ssb_notice("Could not enable SPROM write access\n");
 		return -EBUSY;
 	}
-	ssb_printk(KERN_NOTICE PFX "[ 0%%");
+	ssb_notice("[ 0%%");
 	msleep(500);
 	for (i = 0; i < size; i++) {
 		if (i == size / 4)
-			ssb_printk("25%%");
+			ssb_cont("25%%");
 		else if (i == size / 2)
-			ssb_printk("50%%");
+			ssb_cont("50%%");
 		else if (i == (size * 3) / 4)
-			ssb_printk("75%%");
+			ssb_cont("75%%");
 		else if (i % 2)
-			ssb_printk(".");
+			ssb_cont(".");
 		err = ssb_pcmcia_sprom_write(bus, i, sprom[i]);
 		if (err) {
-			ssb_printk(KERN_NOTICE PFX
-				   "Failed to write to SPROM.\n");
+			ssb_notice("Failed to write to SPROM\n");
 			failed = 1;
 			break;
 		}
 	}
 	err = ssb_pcmcia_sprom_command(bus, SSB_PCMCIA_SPROMCTL_WRITEDIS);
 	if (err) {
-		ssb_printk(KERN_NOTICE PFX
-			   "Could not disable SPROM write access.\n");
+		ssb_notice("Could not disable SPROM write access\n");
 		failed = 1;
 	}
 	msleep(500);
 	if (!failed) {
-		ssb_printk("100%% ]\n");
-		ssb_printk(KERN_NOTICE PFX "SPROM written.\n");
+		ssb_cont("100%% ]\n");
+		ssb_notice("SPROM written\n");
 	}
 
 	return failed ? -EBUSY : 0;
@@ -700,7 +694,7 @@ static int ssb_pcmcia_do_get_invariants(struct pcmcia_device *p_dev,
 	return -ENOSPC; /* continue with next entry */
 
 error:
-	ssb_printk(KERN_ERR PFX
+	ssb_err(
 		   "PCMCIA: Failed to fetch device invariants: %s\n",
 		   error_description);
 	return -ENODEV;
@@ -722,7 +716,7 @@ int ssb_pcmcia_get_invariants(struct ssb_bus *bus,
 	res = pcmcia_loop_tuple(bus->host_pcmcia, CISTPL_FUNCE,
 				ssb_pcmcia_get_mac, sprom);
 	if (res != 0) {
-		ssb_printk(KERN_ERR PFX
+		ssb_err(
 			"PCMCIA: Failed to fetch MAC address\n");
 		return -ENODEV;
 	}
@@ -733,7 +727,7 @@ int ssb_pcmcia_get_invariants(struct ssb_bus *bus,
 	if ((res == 0) || (res == -ENOSPC))
 		return 0;
 
-	ssb_printk(KERN_ERR PFX
+	ssb_err(
 			"PCMCIA: Failed to fetch device invariants\n");
 	return -ENODEV;
 }
@@ -843,6 +837,6 @@ int ssb_pcmcia_init(struct ssb_bus *bus)
 
 	return 0;
 error:
-	ssb_printk(KERN_ERR PFX "Failed to initialize PCMCIA host device\n");
+	ssb_err("Failed to initialize PCMCIA host device\n");
 	return err;
 }
diff --git a/drivers/ssb/scan.c b/drivers/ssb/scan.c
index ab4627cf1114..b9429df583eb 100644
--- a/drivers/ssb/scan.c
+++ b/drivers/ssb/scan.c
@@ -125,8 +125,7 @@ static u16 pcidev_to_chipid(struct pci_dev *pci_dev)
 		chipid_fallback = 0x4401;
 		break;
 	default:
-		ssb_printk(KERN_ERR PFX
-			   "PCI-ID not in fallback list\n");
+		ssb_err("PCI-ID not in fallback list\n");
 	}
 
 	return chipid_fallback;
@@ -152,8 +151,7 @@ static u8 chipid_to_nrcores(u16 chipid)
 	case 0x4704:
 		return 9;
 	default:
-		ssb_printk(KERN_ERR PFX
-			   "CHIPID not in nrcores fallback list\n");
+		ssb_err("CHIPID not in nrcores fallback list\n");
 	}
 
 	return 1;
@@ -320,15 +318,13 @@ int ssb_bus_scan(struct ssb_bus *bus,
 			bus->chip_package = 0;
 		}
 	}
-	ssb_printk(KERN_INFO PFX "Found chip with id 0x%04X, rev 0x%02X and "
-		   "package 0x%02X\n", bus->chip_id, bus->chip_rev,
-		   bus->chip_package);
+	ssb_info("Found chip with id 0x%04X, rev 0x%02X and package 0x%02X\n",
+		 bus->chip_id, bus->chip_rev, bus->chip_package);
 	if (!bus->nr_devices)
 		bus->nr_devices = chipid_to_nrcores(bus->chip_id);
 	if (bus->nr_devices > ARRAY_SIZE(bus->devices)) {
-		ssb_printk(KERN_ERR PFX
-			   "More than %d ssb cores found (%d)\n",
-			   SSB_MAX_NR_CORES, bus->nr_devices);
+		ssb_err("More than %d ssb cores found (%d)\n",
+			SSB_MAX_NR_CORES, bus->nr_devices);
 		goto err_unmap;
 	}
 	if (bus->bustype == SSB_BUSTYPE_SSB) {
@@ -370,8 +366,7 @@ int ssb_bus_scan(struct ssb_bus *bus,
 			nr_80211_cores++;
 			if (nr_80211_cores > 1) {
 				if (!we_support_multiple_80211_cores(bus)) {
-					ssb_dprintk(KERN_INFO PFX "Ignoring additional "
-						    "802.11 core\n");
+					ssb_dbg("Ignoring additional 802.11 core\n");
 					continue;
 				}
 			}
@@ -379,8 +374,7 @@ int ssb_bus_scan(struct ssb_bus *bus,
 		case SSB_DEV_EXTIF:
 #ifdef CONFIG_SSB_DRIVER_EXTIF
 			if (bus->extif.dev) {
-				ssb_printk(KERN_WARNING PFX
-					   "WARNING: Multiple EXTIFs found\n");
+				ssb_warn("WARNING: Multiple EXTIFs found\n");
 				break;
 			}
 			bus->extif.dev = dev;
@@ -388,8 +382,7 @@ int ssb_bus_scan(struct ssb_bus *bus,
 			break;
 		case SSB_DEV_CHIPCOMMON:
 			if (bus->chipco.dev) {
-				ssb_printk(KERN_WARNING PFX
-					   "WARNING: Multiple ChipCommon found\n");
+				ssb_warn("WARNING: Multiple ChipCommon found\n");
 				break;
 			}
 			bus->chipco.dev = dev;
@@ -398,8 +391,7 @@ int ssb_bus_scan(struct ssb_bus *bus,
 		case SSB_DEV_MIPS_3302:
 #ifdef CONFIG_SSB_DRIVER_MIPS
 			if (bus->mipscore.dev) {
-				ssb_printk(KERN_WARNING PFX
-					   "WARNING: Multiple MIPS cores found\n");
+				ssb_warn("WARNING: Multiple MIPS cores found\n");
 				break;
 			}
 			bus->mipscore.dev = dev;
@@ -420,8 +412,7 @@ int ssb_bus_scan(struct ssb_bus *bus,
 				}
 			}
 			if (bus->pcicore.dev) {
-				ssb_printk(KERN_WARNING PFX
-					   "WARNING: Multiple PCI(E) cores found\n");
+				ssb_warn("WARNING: Multiple PCI(E) cores found\n");
 				break;
 			}
 			bus->pcicore.dev = dev;
diff --git a/drivers/ssb/sprom.c b/drivers/ssb/sprom.c
index 80d366fcf8d3..a3b23644b0fb 100644
--- a/drivers/ssb/sprom.c
+++ b/drivers/ssb/sprom.c
@@ -127,13 +127,13 @@ ssize_t ssb_attr_sprom_store(struct ssb_bus *bus,
 		goto out_kfree;
 	err = ssb_devices_freeze(bus, &freeze);
 	if (err) {
-		ssb_printk(KERN_ERR PFX "SPROM write: Could not freeze all devices\n");
+		ssb_err("SPROM write: Could not freeze all devices\n");
 		goto out_unlock;
 	}
 	res = sprom_write(bus, sprom);
 	err = ssb_devices_thaw(&freeze);
 	if (err)
-		ssb_printk(KERN_ERR PFX "SPROM write: Could not thaw all devices\n");
+		ssb_err("SPROM write: Could not thaw all devices\n");
 out_unlock:
 	mutex_unlock(&bus->sprom_mutex);
 out_kfree:
diff --git a/drivers/ssb/ssb_private.h b/drivers/ssb/ssb_private.h
index 466171b77f68..4671f17f09af 100644
--- a/drivers/ssb/ssb_private.h
+++ b/drivers/ssb/ssb_private.h
@@ -9,16 +9,27 @@
 #define PFX	"ssb: "
 
 #ifdef CONFIG_SSB_SILENT
-# define ssb_printk(fmt, x...)	do { /* nothing */ } while (0)
+# define ssb_printk(fmt, ...)					\
+	do { if (0) printk(fmt, ##__VA_ARGS__); } while (0)
 #else
-# define ssb_printk		printk
+# define ssb_printk(fmt, ...)					\
+	printk(fmt, ##__VA_ARGS__)
 #endif /* CONFIG_SSB_SILENT */
 
+#define ssb_emerg(fmt, ...)	ssb_printk(KERN_EMERG PFX fmt, ##__VA_ARGS__)
+#define ssb_err(fmt, ...)	ssb_printk(KERN_ERR PFX fmt, ##__VA_ARGS__)
+#define ssb_warn(fmt, ...)	ssb_printk(KERN_WARNING PFX fmt, ##__VA_ARGS__)
+#define ssb_notice(fmt, ...)	ssb_printk(KERN_NOTICE PFX fmt, ##__VA_ARGS__)
+#define ssb_info(fmt, ...)	ssb_printk(KERN_INFO PFX fmt, ##__VA_ARGS__)
+#define ssb_cont(fmt, ...)	ssb_printk(KERN_CONT fmt, ##__VA_ARGS__)
+
 /* dprintk: Debugging printk; vanishes for non-debug compilation */
 #ifdef CONFIG_SSB_DEBUG
-# define ssb_dprintk(fmt, x...)	ssb_printk(fmt , ##x)
+# define ssb_dbg(fmt, ...)					\
+	ssb_printk(KERN_DEBUG PFX fmt, ##__VA_ARGS__)
 #else
-# define ssb_dprintk(fmt, x...)	do { /* nothing */ } while (0)
+# define ssb_dbg(fmt, ...)					\
+	do { if (0) printk(KERN_DEBUG PFX fmt, ##__VA_ARGS__); } while (0)
 #endif
 
 #ifdef CONFIG_SSB_DEBUG
diff --git a/drivers/staging/android/Kconfig b/drivers/staging/android/Kconfig
index 9f61d46da157..c0c95be0f969 100644
--- a/drivers/staging/android/Kconfig
+++ b/drivers/staging/android/Kconfig
@@ -54,7 +54,7 @@ config ANDROID_TIMED_OUTPUT
 
 config ANDROID_TIMED_GPIO
 	tristate "Android timed gpio driver"
-	depends on GENERIC_GPIO && ANDROID_TIMED_OUTPUT
+	depends on GPIOLIB && ANDROID_TIMED_OUTPUT
 	default n
 
 config ANDROID_LOW_MEMORY_KILLER
diff --git a/drivers/staging/android/logger.c b/drivers/staging/android/logger.c
index b14a55742559..b040200a5a55 100644
--- a/drivers/staging/android/logger.c
+++ b/drivers/staging/android/logger.c
@@ -28,6 +28,7 @@
 #include <linux/slab.h>
 #include <linux/time.h>
 #include <linux/vmalloc.h>
+#include <linux/aio.h>
 #include "logger.h"
 
 #include <asm/ioctls.h>
diff --git a/drivers/staging/comedi/proc.c b/drivers/staging/comedi/proc.c
index 362c214bcc0b..886c202de9ab 100644
--- a/drivers/staging/comedi/proc.c
+++ b/drivers/staging/comedi/proc.c
@@ -31,17 +31,15 @@
 #include "comedidev.h"
 #include "comedi_internal.h"
 #include <linux/proc_fs.h>
-#include <linux/string.h>
+#include <linux/seq_file.h>
 
-static int comedi_read(char *buf, char **start, off_t offset, int len,
-		       int *eof, void *data)
+static int comedi_read(struct seq_file *m, void *v)
 {
 	int i;
 	int devices_q = 0;
-	int l = 0;
 	struct comedi_driver *driv;
 
-	l += sprintf(buf + l,
+	seq_printf(m,
 		     "comedi version " COMEDI_RELEASE "\n"
 		     "format string: %s\n",
 		     "\"%2d: %-20s %-20s %4d\", i, "
@@ -49,42 +47,51 @@ static int comedi_read(char *buf, char **start, off_t offset, int len,
 
 	for (i = 0; i < COMEDI_NUM_BOARD_MINORS; i++) {
 		struct comedi_device *dev = comedi_dev_from_minor(i);
-
 		if (!dev)
 			continue;
 
 		if (dev->attached) {
 			devices_q = 1;
-			l += sprintf(buf + l, "%2d: %-20s %-20s %4d\n",
-				     i,
-				     dev->driver->driver_name,
-				     dev->board_name, dev->n_subdevices);
+			seq_printf(m, "%2d: %-20s %-20s %4d\n",
+				   i, dev->driver->driver_name,
+				   dev->board_name, dev->n_subdevices);
 		}
 	}
 	if (!devices_q)
-		l += sprintf(buf + l, "no devices\n");
+		seq_puts(m, "no devices\n");
 
 	for (driv = comedi_drivers; driv; driv = driv->next) {
-		l += sprintf(buf + l, "%s:\n", driv->driver_name);
-		for (i = 0; i < driv->num_names; i++) {
-			l += sprintf(buf + l, " %s\n",
-				     *(char **)((char *)driv->board_name +
-						i * driv->offset));
-		}
+		seq_printf(m, "%s:\n", driv->driver_name);
+		for (i = 0; i < driv->num_names; i++)
+			seq_printf(m, " %s\n",
+				   *(char **)((char *)driv->board_name +
+					      i * driv->offset));
+
 		if (!driv->num_names)
-			l += sprintf(buf + l, " %s\n", driv->driver_name);
+			seq_printf(m, " %s\n", driv->driver_name);
 	}
 
-	return l;
+	return 0;
 }
 
-void comedi_proc_init(void)
+/*
+ * seq_file wrappers for procfile show routines.
+ */
+static int comedi_proc_open(struct inode *inode, struct file *file)
 {
-	struct proc_dir_entry *comedi_proc;
+	return single_open(file, comedi_read, NULL);
+}
+
+static const struct file_operations comedi_proc_fops = {
+	.open		= comedi_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 
-	comedi_proc = create_proc_entry("comedi", S_IFREG | S_IRUGO, NULL);
-	if (comedi_proc)
-		comedi_proc->read_proc = comedi_read;
+void comedi_proc_init(void)
+{
+	proc_create("comedi", 0644, NULL, &comedi_proc_fops);
 }
 
 void comedi_proc_cleanup(void)
diff --git a/drivers/staging/csr/csr_wifi_hip_udi.c b/drivers/staging/csr/csr_wifi_hip_udi.c
index a65b822db698..a6b006b0e983 100644
--- a/drivers/staging/csr/csr_wifi_hip_udi.c
+++ b/drivers/staging/csr/csr_wifi_hip_udi.c
@@ -24,10 +24,50 @@
  *
  * ---------------------------------------------------------------------------
  */
+#include <linux/seq_file.h>
 #include "csr_wifi_hip_unifi.h"
 #include "csr_wifi_hip_card.h"
 
 
+static void unifi_print_unsafe_sdio_status(card_t *card, struct seq_file *m)
+{
+#ifdef CSR_UNSAFE_SDIO_ACCESS
+	s32 iostate;
+	CsrResult r;
+	static const char *const states[] = {
+		"AWAKE", "DROWSY", "TORPID"
+	};
+#define SHARED_READ_RETRY_LIMIT 10
+	u8 b;
+
+	seq_printf(m, "Host State: %s\n", states[card->host_state]);
+
+	r = unifi_check_io_status(card, &iostate);
+	if (iostate == 1) {
+		seq_puts(m, remaining, "I/O Check: F1 disabled\n");
+        } else {
+		if (iostate == 1) {
+			seq_puts(m, "I/O Check: pending interrupt\n");
+
+		seq_printf(m, "BH reason interrupt = %d\n", card->bh_reason_unifi);
+		seq_printf(m, "BH reason host      = %d\n", card->bh_reason_host);
+
+		for (i = 0; i < SHARED_READ_RETRY_LIMIT; i++) {
+			r = unifi_read_8_or_16(card, card->sdio_ctrl_addr + 2, &b);
+			if (r == CSR_RESULT_SUCCESS && !(b & 0x80)) {
+				seq_printf(m, "fhsr: %u (driver thinks is %u)\n",
+					   b, card->from_host_signals_r);
+				break;
+			}
+		}
+
+		iostate = unifi_read_shared_count(card, card->sdio_ctrl_addr + 4);
+		seq_printf(m, "thsw: %u (driver thinks is %u)\n",
+			   iostate, card->to_host_signals_w);
+        }
+#endif
+}
+
 /*
  * ---------------------------------------------------------------------------
  *  unifi_print_status
@@ -41,228 +81,93 @@
  *      None.
  * ---------------------------------------------------------------------------
  */
-s32 unifi_print_status(card_t *card, char *str, s32 *remain)
+s32 unifi_print_status(card_t *card, struct seq_file *m)
 {
-    char *p = str;
-    sdio_config_data_t *cfg;
-    u16 i, n;
-    s32 remaining = *remain;
-    s32 written;
-#ifdef CSR_UNSAFE_SDIO_ACCESS
-    s32 iostate;
-    CsrResult r;
-    static const char *const states[] = {
-        "AWAKE", "DROWSY", "TORPID"
-    };
-    #define SHARED_READ_RETRY_LIMIT 10
-    u8 b;
-#endif
-
-    if (remaining <= 0)
-    {
-        return 0;
-    }
-
-    i = n = 0;
-    written = scnprintf(p, remaining, "Chip ID %u\n",
-                          (u16)card->chip_id);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "Chip Version %04X\n",
-                          card->chip_version);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "HIP v%u.%u\n",
-                          (card->config_data.version >> 8) & 0xFF,
-                          card->config_data.version & 0xFF);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "Build %u: %s\n",
-                          card->build_id, card->build_id_string);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-
-    cfg = &card->config_data;
-
-    written = scnprintf(p, remaining, "sdio ctrl offset          %u\n",
-                          cfg->sdio_ctrl_offset);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "fromhost sigbuf handle    %u\n",
-                          cfg->fromhost_sigbuf_handle);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "tohost_sigbuf_handle      %u\n",
-                          cfg->tohost_sigbuf_handle);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "num_fromhost_sig_frags    %u\n",
-                          cfg->num_fromhost_sig_frags);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "num_tohost_sig_frags      %u\n",
-                          cfg->num_tohost_sig_frags);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "num_fromhost_data_slots   %u\n",
-                          cfg->num_fromhost_data_slots);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "num_tohost_data_slots     %u\n",
-                          cfg->num_tohost_data_slots);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "data_slot_size            %u\n",
-                          cfg->data_slot_size);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-
-    /* Added by protocol version 0x0001 */
-    written = scnprintf(p, remaining, "overlay_size              %u\n",
-                          (u16)cfg->overlay_size);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-
-    /* Added by protocol version 0x0300 */
-    written = scnprintf(p, remaining, "data_slot_round           %u\n",
-                          cfg->data_slot_round);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "sig_frag_size             %u\n",
-                          cfg->sig_frag_size);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-
-    /* Added by protocol version 0x0300 */
-    written = scnprintf(p, remaining, "tohost_sig_pad            %u\n",
-                          cfg->tohost_signal_padding);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-
-    written = scnprintf(p, remaining, "\nInternal state:\n");
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-
-    written = scnprintf(p, remaining, "Last PHY PANIC: %04x:%04x\n",
-                          card->last_phy_panic_code, card->last_phy_panic_arg);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "Last MAC PANIC: %04x:%04x\n",
-                          card->last_mac_panic_code, card->last_mac_panic_arg);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-
-    written = scnprintf(p, remaining, "fhsr: %u\n",
-                          (u16)card->from_host_signals_r);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "fhsw: %u\n",
-                          (u16)card->from_host_signals_w);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "thsr: %u\n",
-                          (u16)card->to_host_signals_r);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "thsw: %u\n",
-                          (u16)card->to_host_signals_w);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining,
-                          "fh buffer contains: %d signals, %td bytes\n",
-                          card->fh_buffer.count,
-                          card->fh_buffer.ptr - card->fh_buffer.buf);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-
-    written = scnprintf(p, remaining, "paused: ");
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    for (i = 0; i < sizeof(card->tx_q_paused_flag) / sizeof(card->tx_q_paused_flag[0]); i++)
-    {
-        written = scnprintf(p, remaining, card->tx_q_paused_flag[i]?"1" : "0");
-        UNIFI_SNPRINTF_RET(p, remaining, written);
-    }
-    written = scnprintf(p, remaining, "\n");
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-
-    written = scnprintf(p, remaining,
-                          "fh command q: %u waiting, %u free of %u:\n",
-                          CSR_WIFI_HIP_Q_SLOTS_USED(&card->fh_command_queue),
-                          CSR_WIFI_HIP_Q_SLOTS_FREE(&card->fh_command_queue),
-                          UNIFI_SOFT_COMMAND_Q_LENGTH);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
-    {
-        written = scnprintf(p, remaining,
-                              "fh traffic q[%u]: %u waiting, %u free of %u:\n",
-                              i,
-                              CSR_WIFI_HIP_Q_SLOTS_USED(&card->fh_traffic_queue[i]),
-                              CSR_WIFI_HIP_Q_SLOTS_FREE(&card->fh_traffic_queue[i]),
-                              UNIFI_SOFT_TRAFFIC_Q_LENGTH);
-        UNIFI_SNPRINTF_RET(p, remaining, written);
-    }
-
-    written = scnprintf(p, remaining, "fh data slots free: %u\n",
-                          card->from_host_data?CardGetFreeFromHostDataSlots(card) : 0);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-
-
-    written = scnprintf(p, remaining, "From host data slots:");
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    n = card->config_data.num_fromhost_data_slots;
-    for (i = 0; i < n && card->from_host_data; i++)
-    {
-        written = scnprintf(p, remaining, " %u",
-                              (u16)card->from_host_data[i].bd.data_length);
-        UNIFI_SNPRINTF_RET(p, remaining, written);
-    }
-    written = scnprintf(p, remaining, "\n");
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-
-    written = scnprintf(p, remaining, "To host data slots:");
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    n = card->config_data.num_tohost_data_slots;
-    for (i = 0; i < n && card->to_host_data; i++)
-    {
-        written = scnprintf(p, remaining, " %u",
-                              (u16)card->to_host_data[i].data_length);
-        UNIFI_SNPRINTF_RET(p, remaining, written);
-    }
-
-    written = scnprintf(p, remaining, "\n");
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-
-#ifdef CSR_UNSAFE_SDIO_ACCESS
-    written = scnprintf(p, remaining, "Host State: %s\n", states[card->host_state]);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-
-    r = unifi_check_io_status(card, &iostate);
-    if (iostate == 1)
-    {
-        written = scnprintf(p, remaining, "I/O Check: F1 disabled\n");
-        UNIFI_SNPRINTF_RET(p, remaining, written);
-    }
-    else
-    {
-        if (iostate == 1)
-        {
-            written = scnprintf(p, remaining, "I/O Check: pending interrupt\n");
-            UNIFI_SNPRINTF_RET(p, remaining, written);
-        }
-
-        written = scnprintf(p, remaining, "BH reason interrupt = %d\n",
-                              card->bh_reason_unifi);
-        UNIFI_SNPRINTF_RET(p, remaining, written);
-        written = scnprintf(p, remaining, "BH reason host      = %d\n",
-                              card->bh_reason_host);
-        UNIFI_SNPRINTF_RET(p, remaining, written);
-
-        for (i = 0; i < SHARED_READ_RETRY_LIMIT; i++)
-        {
-            r = unifi_read_8_or_16(card, card->sdio_ctrl_addr + 2, &b);
-            if ((r == CSR_RESULT_SUCCESS) && (!(b & 0x80)))
-            {
-                written = scnprintf(p, remaining, "fhsr: %u (driver thinks is %u)\n",
-                                      b, card->from_host_signals_r);
-                UNIFI_SNPRINTF_RET(p, remaining, written);
-                break;
-            }
-        }
-        iostate = unifi_read_shared_count(card, card->sdio_ctrl_addr + 4);
-        written = scnprintf(p, remaining, "thsw: %u (driver thinks is %u)\n",
-                              iostate, card->to_host_signals_w);
-        UNIFI_SNPRINTF_RET(p, remaining, written);
-    }
-#endif
-
-    written = scnprintf(p, remaining, "\nStats:\n");
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "Total SDIO bytes: R=%u W=%u\n",
-                          card->sdio_bytes_read, card->sdio_bytes_written);
-
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-    written = scnprintf(p, remaining, "Interrupts generated on card: %u\n",
-                          card->unifi_interrupt_seq);
-    UNIFI_SNPRINTF_RET(p, remaining, written);
-
-    *remain = remaining;
-    return (p - str);
-} /* unifi_print_status() */
-
-
+	sdio_config_data_t *cfg;
+	u16 i, n;
+
+	i = n = 0;
+	seq_printf(m, "Chip ID %u\n", card->chip_id);
+	seq_printf(m, "Chip Version %04X\n", card->chip_version);
+	seq_printf(m, "HIP v%u.%u\n",
+		   (card->config_data.version >> 8) & 0xFF,
+		   card->config_data.version & 0xFF);
+	seq_printf(m, "Build %u: %s\n", card->build_id, card->build_id_string);
+
+	cfg = &card->config_data;
+
+	seq_printf(m, "sdio ctrl offset          %u\n", cfg->sdio_ctrl_offset);
+	seq_printf(m, "fromhost sigbuf handle    %u\n", cfg->fromhost_sigbuf_handle);
+	seq_printf(m, "tohost_sigbuf_handle      %u\n", cfg->tohost_sigbuf_handle);
+	seq_printf(m, "num_fromhost_sig_frags    %u\n", cfg->num_fromhost_sig_frags);
+	seq_printf(m, "num_tohost_sig_frags      %u\n", cfg->num_tohost_sig_frags);
+	seq_printf(m, "num_fromhost_data_slots   %u\n", cfg->num_fromhost_data_slots);
+	seq_printf(m, "num_tohost_data_slots     %u\n", cfg->num_tohost_data_slots);
+	seq_printf(m, "data_slot_size            %u\n", cfg->data_slot_size);
+
+	/* Added by protocol version 0x0001 */
+	seq_printf(m, "overlay_size              %u\n", cfg->overlay_size);
+
+	/* Added by protocol version 0x0300 */
+	seq_printf(m, "data_slot_round           %u\n", cfg->data_slot_round);
+	seq_printf(m, "sig_frag_size             %u\n", cfg->sig_frag_size);
+
+	/* Added by protocol version 0x0300 */
+	seq_printf(m, "tohost_sig_pad            %u\n", cfg->tohost_signal_padding);
+
+	seq_puts(m, "\nInternal state:\n");
+
+	seq_printf(m, "Last PHY PANIC: %04x:%04x\n",
+		   card->last_phy_panic_code, card->last_phy_panic_arg);
+	seq_printf(m, "Last MAC PANIC: %04x:%04x\n",
+		   card->last_mac_panic_code, card->last_mac_panic_arg);
+
+	seq_printf(m, "fhsr: %hu\n", (u16)card->from_host_signals_r);
+	seq_printf(m, "fhsw: %hu\n", (u16)card->from_host_signals_w);
+	seq_printf(m, "thsr: %hu\n", (u16)card->to_host_signals_r);
+	seq_printf(m, "thsw: %hu\n", (u16)card->to_host_signals_w);
+	seq_printf(m, "fh buffer contains: %d signals, %td bytes\n",
+		   card->fh_buffer.count,
+		   card->fh_buffer.ptr - card->fh_buffer.buf);
+
+	seq_puts(m, "paused: ");
+	for (i = 0; i < ARRAY_SIZE(card->tx_q_paused_flag); i++)
+		seq_printf(m, card->tx_q_paused_flag[i] ? "1" : "0");
+	seq_putc(m, '\n');
+
+	seq_printf(m, "fh command q: %u waiting, %u free of %u:\n",
+		   CSR_WIFI_HIP_Q_SLOTS_USED(&card->fh_command_queue),
+		   CSR_WIFI_HIP_Q_SLOTS_FREE(&card->fh_command_queue),
+		   UNIFI_SOFT_COMMAND_Q_LENGTH);
+
+	for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
+		seq_printf(m, "fh traffic q[%u]: %u waiting, %u free of %u:\n",
+			   i,
+			   CSR_WIFI_HIP_Q_SLOTS_USED(&card->fh_traffic_queue[i]),
+			   CSR_WIFI_HIP_Q_SLOTS_FREE(&card->fh_traffic_queue[i]),
+			   UNIFI_SOFT_TRAFFIC_Q_LENGTH);
+
+	seq_printf(m, "fh data slots free: %u\n",
+		   card->from_host_data ? CardGetFreeFromHostDataSlots(card) : 0);
+
+	seq_puts(m, "From host data slots:");
+	n = card->config_data.num_fromhost_data_slots;
+	for (i = 0; i < n && card->from_host_data; i++)
+		seq_printf(m, " %hu", (u16)card->from_host_data[i].bd.data_length);
+	seq_putc(m, '\n');
+
+	seq_puts(m, "To host data slots:");
+	n = card->config_data.num_tohost_data_slots;
+	for (i = 0; i < n && card->to_host_data; i++)
+		seq_printf(m, " %hu", (u16)card->to_host_data[i].data_length);
+	seq_putc(m, '\n');
+
+	unifi_print_unsafe_sdio_status(card, m);
+
+	seq_puts(m, "\nStats:\n");
+	seq_printf(m, "Total SDIO bytes: R=%u W=%u\n",
+		   card->sdio_bytes_read, card->sdio_bytes_written);
+
+	seq_printf(m, "Interrupts generated on card: %u\n", card->unifi_interrupt_seq);
+	return 0;
+}
diff --git a/drivers/staging/csr/csr_wifi_hip_unifi_udi.h b/drivers/staging/csr/csr_wifi_hip_unifi_udi.h
index 9d85cfd57616..4126e85bfe9b 100644
--- a/drivers/staging/csr/csr_wifi_hip_unifi_udi.h
+++ b/drivers/staging/csr/csr_wifi_hip_unifi_udi.h
@@ -47,21 +47,6 @@ CsrResult unifi_remove_udi_hook(card_t *card, udi_func_t udi_fn);
  * This is used in the linux /proc interface and might be useful
  * in other systems.
  */
-s32 unifi_print_status(card_t *card, char *str, s32 *remain);
-
-#define UNIFI_SNPRINTF_RET(buf_p, remain, written)                  \
-    do {                                                            \
-        if (written >= remain) {                                    \
-            if (remain >= 2) {                                      \
-                buf_p[remain - 2] = '\n';                           \
-                buf_p[remain - 1] = 0;                              \
-            }                                                       \
-            buf_p += remain;                                        \
-            remain = 0;                                             \
-        } else if (written > 0) {                                   \
-            buf_p += written;                                       \
-            remain -= written;                                      \
-        }                                                           \
-    } while (0)
+s32 unifi_print_status(card_t *card, struct seq_file *m);
 
 #endif /* __CSR_WIFI_HIP_UNIFI_UDI_H__ */
diff --git a/drivers/staging/csr/drv.c b/drivers/staging/csr/drv.c
index 5520d6539f77..bdc25236ab00 100644
--- a/drivers/staging/csr/drv.c
+++ b/drivers/staging/csr/drv.c
@@ -1941,7 +1941,7 @@ uf_sme_queue_message(unifi_priv_t *priv, u8 *buffer, int length)
  *
  ****************************************************************************
  */
-static struct file_operations unifi_fops = {
+static const struct file_operations unifi_fops = {
     .owner      = THIS_MODULE,
     .open       = unifi_open,
     .release    = unifi_release,
@@ -2041,7 +2041,7 @@ void uf_destroy_device_nodes(unifi_priv_t *priv)
  * ----------------------------------------------------------------
  */
 static int
-uf_create_debug_device(struct file_operations *fops)
+uf_create_debug_device(const struct file_operations *fops)
 {
     int ret;
 
diff --git a/drivers/staging/csr/io.c b/drivers/staging/csr/io.c
index af9c28f073b9..fe4a7ba2acc9 100644
--- a/drivers/staging/csr/io.c
+++ b/drivers/staging/csr/io.c
@@ -31,6 +31,7 @@
  * ---------------------------------------------------------------------------
  */
 #include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 
 #include "csr_wifi_hip_unifi.h"
 #include "csr_wifi_hip_unifiversion.h"
@@ -76,9 +77,28 @@ DEFINE_SEMAPHORE(Unifi_instance_mutex);
  */
 DECLARE_WAIT_QUEUE_HEAD(Unifi_cleanup_wq);
 
+#ifdef CONFIG_PROC_FS
+/*
+ * seq_file wrappers for procfile show routines.
+ */
+static int uf_proc_show(struct seq_file *m, void *v);
+
+#define UNIFI_DEBUG_TXT_BUFFER (8 * 1024)
+
+static int uf_proc_open(struct inode *inode, struct file *file)
+{
+	return single_open_size(file, uf_proc_show, PDE_DATA(inode),
+				UNIFI_DEBUG_TXT_BUFFER);
+}
+
+static const struct file_operations uf_proc_fops = {
+	.open		= uf_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 
-static int uf_read_proc(char *page, char **start, off_t offset, int count,
-                        int *eof, void *data);
+#endif /* CONFIG_PROC_FS */
 
 #ifdef CSR_WIFI_RX_PATH_SPLIT
 
@@ -327,8 +347,8 @@ register_unifi_sdio(CsrSdioFunction *sdio_dev, int bus_id, struct device *dev)
      * The following complex casting is in place in order to eliminate 64-bit compilation warning
      * "cast to/from pointer from/to integer of different size"
      */
-    if (!create_proc_read_entry(priv->proc_entry_name, 0, 0,
-                uf_read_proc, (void *)(long)priv->instance))
+    if (!proc_create_data(priv->proc_entry_name, 0, NULL,
+			  &uf_proc_fops, (void *)(long)priv->instance))
     {
         unifi_error(priv, "unifi: can't create /proc/driver/unifi\n");
     }
@@ -827,7 +847,7 @@ uf_put_instance(int inst)
 
 /*
  * ---------------------------------------------------------------------------
- *  uf_read_proc
+ *  uf_proc_show
  *
  *      Read method for driver node in /proc/driver/unifi0
  *
@@ -844,107 +864,54 @@ uf_put_instance(int inst)
  * ---------------------------------------------------------------------------
  */
 #ifdef CONFIG_PROC_FS
-static int
-uf_read_proc(char *page, char **start, off_t offset, int count,
-        int *eof, void *data)
+static int uf_proc_show(struct seq_file *m, void *v)
 {
-#define UNIFI_DEBUG_TXT_BUFFER 8*1024
-    unifi_priv_t *priv;
-    int actual_amount_to_copy;
-    char *p, *orig_p;
-    s32 remain = UNIFI_DEBUG_TXT_BUFFER;
-    s32 written;
-    int i;
-
-    /*
-    * The following complex casting is in place in order to eliminate 64-bit compilation warning
-    * "cast to/from pointer from/to integer of different size"
-    */
-    priv = uf_find_instance((int)(long)data);
-    if (!priv) {
-        return 0;
-    }
-
-    p = kmalloc( UNIFI_DEBUG_TXT_BUFFER, GFP_KERNEL );
-
-    orig_p = p;
-
-    written = scnprintf(p, remain, "UniFi SDIO Driver: %s %s %s\n",
-            CSR_WIFI_VERSION, __DATE__, __TIME__);
-    UNIFI_SNPRINTF_RET(p, remain, written);
+	unifi_priv_t *priv;
+	int i;
+
+	/*
+	 * The following complex casting is in place in order to eliminate
+	 * 64-bit compilation warning "cast to/from pointer from/to integer of
+	 * different size"
+	 */
+	priv = uf_find_instance((long)m->private);
+	if (!priv)
+		return 0;
+
+	seq_printf(m, "UniFi SDIO Driver: %s %s %s\n",
+		   CSR_WIFI_VERSION, __DATE__, __TIME__);
 #ifdef CSR_SME_USERSPACE
-    written = scnprintf(p, remain, "SME: CSR userspace ");
-    UNIFI_SNPRINTF_RET(p, remain, written);
+	seq_puts(m, "SME: CSR userspace ");
 #ifdef CSR_SUPPORT_WEXT
-    written = scnprintf(p, remain, "with WEXT support\n");
+	seq_puts(m, "with WEXT support\n");
 #else
-    written = scnprintf(p, remain, "\n");
+	seq_putc(m, '\n');
 #endif /* CSR_SUPPORT_WEXT */
-    UNIFI_SNPRINTF_RET(p, remain, written);
 #endif /* CSR_SME_USERSPACE */
 #ifdef CSR_NATIVE_LINUX
-    written = scnprintf(p, remain, "SME: native\n");
-    UNIFI_SNPRINTF_RET(p, remain, written);
+	seq_puts(m, "SME: native\n");
 #endif
 
 #ifdef CSR_SUPPORT_SME
-    written = scnprintf(p, remain,
-            "Firmware (ROM) build:%u, Patch:%u\n",
-            priv->card_info.fw_build,
-            priv->sme_versions.firmwarePatch);
-    UNIFI_SNPRINTF_RET(p, remain, written);
+	seq_printf(m, "Firmware (ROM) build:%u, Patch:%u\n",
+		   priv->card_info.fw_build,
+		   priv->sme_versions.firmwarePatch);
 #endif
-    p += unifi_print_status(priv->card, p, &remain);
-
-    written = scnprintf(p, remain, "Last dbg str: %s\n",
-            priv->last_debug_string);
-    UNIFI_SNPRINTF_RET(p, remain, written);
-
-    written = scnprintf(p, remain, "Last dbg16:");
-    UNIFI_SNPRINTF_RET(p, remain, written);
-    for (i = 0; i < 8; i++) {
-        written = scnprintf(p, remain, " %04X",
-                priv->last_debug_word16[i]);
-        UNIFI_SNPRINTF_RET(p, remain, written);
-    }
-    written = scnprintf(p, remain, "\n");
-    UNIFI_SNPRINTF_RET(p, remain, written);
-    written = scnprintf(p, remain, "           ");
-    UNIFI_SNPRINTF_RET(p, remain, written);
-    for (; i < 16; i++) {
-        written = scnprintf(p, remain, " %04X",
-                priv->last_debug_word16[i]);
-        UNIFI_SNPRINTF_RET(p, remain, written);
-    }
-    written = scnprintf(p, remain, "\n");
-    UNIFI_SNPRINTF_RET(p, remain, written);
-    *start = page;
-
-    written = UNIFI_DEBUG_TXT_BUFFER - remain;
-
-    if( offset >= written )
-    {
-        *eof = 1;
-        kfree( orig_p );
-        return(0);
-    }
-
-    if( offset + count > written )
-    {
-        actual_amount_to_copy = written - offset;
-        *eof = 1;
-    }
-    else
-    {
-        actual_amount_to_copy = count;
-    }
 
-    memcpy( page, &(orig_p[offset]), actual_amount_to_copy );
+	unifi_print_status(priv->card, m);
 
-    kfree( orig_p );
+	seq_printf(m, "Last dbg str: %s\n", priv->last_debug_string);
 
-    return( actual_amount_to_copy );
-} /* uf_read_proc() */
+	seq_puts(m, "Last dbg16:");
+	for (i = 0; i < 8; i++)
+		seq_printf(m, " %04X", priv->last_debug_word16[i]);
+	seq_putc(m, '\n');
+	seq_puts(m, "           ");
+	for (; i < 16; i++)
+		seq_printf(m, " %04X", priv->last_debug_word16[i]);
+	seq_putc(m, '\n');
+	return 0;
+}
 #endif
 
 
diff --git a/drivers/staging/cxt1e1/Makefile b/drivers/staging/cxt1e1/Makefile
index e99b82311823..b9ccb7650251 100644
--- a/drivers/staging/cxt1e1/Makefile
+++ b/drivers/staging/cxt1e1/Makefile
@@ -12,8 +12,9 @@ cxt1e1-y := 	\
   linux.o 		\
   functions.o 		\
   hwprobe.o 		\
-  sbeproc.o 		\
   pmc93x6_eeprom.o 	\
   sbecrc.o 		\
   comet_tables.o 	\
   sbeid.o
+
+cxt1e1-$(CONFIG_PROC_FS) += sbeproc.o
diff --git a/drivers/staging/cxt1e1/sbeproc.c b/drivers/staging/cxt1e1/sbeproc.c
index f42531c3d8da..9361dd8ce125 100644
--- a/drivers/staging/cxt1e1/sbeproc.c
+++ b/drivers/staging/cxt1e1/sbeproc.c
@@ -19,6 +19,7 @@
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 #include <linux/sched.h>
 #include <asm/uaccess.h>
 #include "pmcc4_sysdep.h"
@@ -26,332 +27,193 @@
 #include "pmcc4_private.h"
 #include "sbeproc.h"
 
-/* forwards */
-void        sbecom_get_brdinfo (ci_t *, struct sbe_brd_info *, u_int8_t *);
+extern void sbecom_get_brdinfo(ci_t *, struct sbe_brd_info *, u_int8_t *);
 extern struct s_hdw_info hdw_info[MAX_BOARDS];
 
-#ifdef CONFIG_PROC_FS
-
-/********************************************************************/
-/* procfs stuff                                                     */
-/********************************************************************/
-
-
-void
-sbecom_proc_brd_cleanup (ci_t * ci)
+void sbecom_proc_brd_cleanup(ci_t *ci)
 {
-    if (ci->dir_dev)
-    {
-	char dir[7 + SBE_IFACETMPL_SIZE + 1];
-	snprintf(dir, sizeof(dir), "driver/%s", ci->devname);
-        remove_proc_entry("info", ci->dir_dev);
-        remove_proc_entry(dir, NULL);
-        ci->dir_dev = NULL;
-    }
+	if (ci->dir_dev) {
+		char dir[7 + SBE_IFACETMPL_SIZE + 1];
+		snprintf(dir, sizeof(dir), "driver/%s", ci->devname);
+		remove_proc_entry("info", ci->dir_dev);
+		remove_proc_entry(dir, NULL);
+		ci->dir_dev = NULL;
+	}
 }
 
-
-static int
-sbecom_proc_get_sbe_info (char *buffer, char **start, off_t offset,
-                          int length, int *eof, void *priv)
+static void sbecom_proc_get_brdinfo(ci_t *ci, struct sbe_brd_info *bip)
 {
-    ci_t       *ci = (ci_t *) priv;
-    int         len = 0;
-    char       *spd;
-    struct sbe_brd_info *bip;
-
-    if (!(bip = OS_kmalloc (sizeof (struct sbe_brd_info))))
-    {
-        return -ENOMEM;
-    }
-#if 0
-    /** RLD DEBUG **/
-    pr_info(">> sbecom_proc_get_sbe_info: entered, offset %d. length %d.\n",
-            (int) offset, (int) length);
-#endif
-
-    {
-        hdw_info_t *hi = &hdw_info[ci->brdno];
-
-        u_int8_t *bsn = 0;
-
-        switch (hi->promfmt)
-        {
-        case PROM_FORMAT_TYPE1:
-            bsn = (u_int8_t *) hi->mfg_info.pft1.Serial;
-            break;
-        case PROM_FORMAT_TYPE2:
-            bsn = (u_int8_t *) hi->mfg_info.pft2.Serial;
-            break;
-        }
-
-        sbecom_get_brdinfo (ci, bip, bsn);
-    }
-
-#if 0
-    /** RLD DEBUG **/
-    pr_info(">> sbecom_get_brdinfo: returned, first_if %p <%s> last_if %p <%s>\n",
-            (char *) &bip->first_iname, (char *) &bip->first_iname,
-            (char *) &bip->last_iname, (char *) &bip->last_iname);
-#endif
-    len += sprintf (buffer + len, "Board Type:    ");
-    switch (bip->brd_id)
-    {
-    case SBE_BOARD_ID (PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPMC_C1T3):
-        len += sprintf (buffer + len, "wanPMC-C1T3");
-        break;
-    case SBE_BOARD_ID (PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPTMC_256T3_E1):
-        len += sprintf (buffer + len, "wanPTMC-256T3 <E1>");
-        break;
-    case SBE_BOARD_ID (PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPTMC_256T3_T1):
-        len += sprintf (buffer + len, "wanPTMC-256T3 <T1>");
-        break;
-    case SBE_BOARD_ID (PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPTMC_C24TE1):
-        len += sprintf (buffer + len, "wanPTMC-C24TE1");
-        break;
-
-    case SBE_BOARD_ID (PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPMC_C4T1E1):
-    case SBE_BOARD_ID (PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPMC_C4T1E1_L):
-        len += sprintf (buffer + len, "wanPMC-C4T1E1");
-        break;
-    case SBE_BOARD_ID (PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPMC_C2T1E1):
-    case SBE_BOARD_ID (PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPMC_C2T1E1_L):
-        len += sprintf (buffer + len, "wanPMC-C2T1E1");
-        break;
-    case SBE_BOARD_ID (PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPMC_C1T1E1):
-    case SBE_BOARD_ID (PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPMC_C1T1E1_L):
-        len += sprintf (buffer + len, "wanPMC-C1T1E1");
-        break;
-
-    case SBE_BOARD_ID (PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPCI_C4T1E1):
-        len += sprintf (buffer + len, "wanPCI-C4T1E1");
-        break;
-    case SBE_BOARD_ID (PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPCI_C2T1E1):
-        len += sprintf (buffer + len, "wanPCI-C2T1E1");
-        break;
-    case SBE_BOARD_ID (PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPCI_C1T1E1):
-        len += sprintf (buffer + len, "wanPCI-C1T1E1");
-        break;
-
-    default:
-        len += sprintf (buffer + len, "unknown");
-        break;
-    }
-    len += sprintf (buffer + len, "  [%08X]\n", bip->brd_id);
+	hdw_info_t *hi = &hdw_info[ci->brdno];
+	u_int8_t *bsn = 0;
+
+	switch (hi->promfmt)
+	{
+	case PROM_FORMAT_TYPE1:
+		bsn = (u_int8_t *) hi->mfg_info.pft1.Serial;
+		break;
+	case PROM_FORMAT_TYPE2:
+		bsn = (u_int8_t *) hi->mfg_info.pft2.Serial;
+		break;
+	}
+
+	sbecom_get_brdinfo (ci, bip, bsn);
+
+	pr_devel(">> sbecom_get_brdinfo: returned, first_if %p <%s> last_if %p <%s>\n",
+		 bip->first_iname, bip->first_iname,
+		 bip->last_iname, bip->last_iname);
+}
 
-    len += sprintf (buffer + len, "Board Number:  %d\n", bip->brdno);
-    len += sprintf (buffer + len, "Hardware ID:   0x%02X\n", ci->hdw_bid);
-    len += sprintf (buffer + len, "Board SN:      %06X\n", bip->brd_sn);
-	len += sprintf(buffer + len, "Board MAC:     %pMF\n",
-		bip->brd_mac_addr);
-    len += sprintf (buffer + len, "Ports:         %d\n", ci->max_port);
-    len += sprintf (buffer + len, "Channels:      %d\n", bip->brd_chan_cnt);
+/*
+ * Describe the driver state through /proc
+ */
+static int sbecom_proc_get_sbe_info(struct seq_file *m, void *v)
+{
+	ci_t       *ci = m->private;
+	char       *spd;
+	struct sbe_brd_info *bip;
+
+	if (!(bip = OS_kmalloc(sizeof(struct sbe_brd_info))))
+		return -ENOMEM;
+
+	pr_devel(">> sbecom_proc_get_sbe_info: entered\n");
+
+	sbecom_proc_get_brdinfo(ci, bip);
+
+	seq_puts(m, "Board Type:    ");
+	switch (bip->brd_id) {
+	case SBE_BOARD_ID(PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPMC_C1T3):
+		seq_puts(m, "wanPMC-C1T3");
+		break;
+	case SBE_BOARD_ID(PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPTMC_256T3_E1):
+		seq_puts(m, "wanPTMC-256T3 <E1>");
+		break;
+	case SBE_BOARD_ID(PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPTMC_256T3_T1):
+		seq_puts(m, "wanPTMC-256T3 <T1>");
+		break;
+	case SBE_BOARD_ID(PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPTMC_C24TE1):
+		seq_puts(m, "wanPTMC-C24TE1");
+		break;
+
+	case SBE_BOARD_ID(PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPMC_C4T1E1):
+	case SBE_BOARD_ID(PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPMC_C4T1E1_L):
+		seq_puts(m, "wanPMC-C4T1E1");
+		break;
+	case SBE_BOARD_ID(PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPMC_C2T1E1):
+	case SBE_BOARD_ID(PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPMC_C2T1E1_L):
+		seq_puts(m, "wanPMC-C2T1E1");
+		break;
+	case SBE_BOARD_ID(PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPMC_C1T1E1):
+	case SBE_BOARD_ID(PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPMC_C1T1E1_L):
+		seq_puts(m, "wanPMC-C1T1E1");
+		break;
+
+	case SBE_BOARD_ID(PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPCI_C4T1E1):
+		seq_puts(m, "wanPCI-C4T1E1");
+		break;
+	case SBE_BOARD_ID(PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPCI_C2T1E1):
+		seq_puts(m, "wanPCI-C2T1E1");
+		break;
+	case SBE_BOARD_ID(PCI_VENDOR_ID_SBE, PCI_DEVICE_ID_WANPCI_C1T1E1):
+		seq_puts(m, "wanPCI-C1T1E1");
+		break;
+
+	default:
+		seq_puts(m, "unknown");
+		break;
+	}
+
+	seq_printf(m, "  [%08X]\n", bip->brd_id);
+
+	seq_printf(m, "Board Number:  %d\n", bip->brdno);
+	seq_printf(m, "Hardware ID:   0x%02X\n", ci->hdw_bid);
+	seq_printf(m, "Board SN:      %06X\n", bip->brd_sn);
+	seq_printf(m, "Board MAC:     %pMF\n", bip->brd_mac_addr);
+	seq_printf(m, "Ports:         %d\n", ci->max_port);
+	seq_printf(m, "Channels:      %d\n", bip->brd_chan_cnt);
 #if 1
-    len += sprintf (buffer + len, "Interface:     %s -> %s\n",
-                    (char *) &bip->first_iname, (char *) &bip->last_iname);
+	seq_printf(m, "Interface:     %s -> %s\n",
+		   bip->first_iname, bip->last_iname);
 #else
-    len += sprintf (buffer + len, "Interface:     <not available> 1st %p lst %p\n",
-                    (char *) &bip->first_iname, (char *) &bip->last_iname);
+	seq_printf(m, "Interface:     <not available> 1st %p lst %p\n",
+		   bip->first_iname, bip->last_iname);
 #endif
 
-    switch (bip->brd_pci_speed)
-    {
-    case BINFO_PCI_SPEED_33:
-        spd = "33Mhz";
-        break;
-    case BINFO_PCI_SPEED_66:
-        spd = "66Mhz";
-        break;
-    default:
-        spd = "<not available>";
-        break;
-    }
-    len += sprintf (buffer + len, "PCI Bus Speed: %s\n", spd);
-    len += sprintf (buffer + len, "Release:       %s\n", ci->release);
+	switch (bip->brd_pci_speed) {
+	case BINFO_PCI_SPEED_33:
+		spd = "33Mhz";
+		break;
+	case BINFO_PCI_SPEED_66:
+		spd = "66Mhz";
+		break;
+	default:
+		spd = "<not available>";
+		break;
+	}
+	seq_printf(m, "PCI Bus Speed: %s\n", spd);
+	seq_printf(m, "Release:       %s\n", ci->release);
 
 #ifdef SBE_PMCC4_ENABLE
-    {
-               extern int cxt1e1_max_mru;
-#if 0
-        extern int max_chans_used;
-        extern int cxt1e1_max_mtu;
-#endif
-        extern int max_rxdesc_used, max_txdesc_used;
-
-        len += sprintf (buffer + len, "\ncxt1e1_max_mru:         %d\n", cxt1e1_max_mru);
+	{
+		extern int cxt1e1_max_mru;
 #if 0
-        len += sprintf (buffer + len, "\nmax_chans_used:  %d\n", max_chans_used);
-        len += sprintf (buffer + len, "cxt1e1_max_mtu:         %d\n", cxt1e1_max_mtu);
-#endif
-        len += sprintf (buffer + len, "max_rxdesc_used: %d\n", max_rxdesc_used);
-        len += sprintf (buffer + len, "max_txdesc_used: %d\n", max_txdesc_used);
-    }
-#endif
-
-    OS_kfree (bip);                 /* cleanup */
-
-    /***
-     * How to be a proc read function
-     * ------------------------------
-     * Prototype:
-     *    int f(char *buffer, char **start, off_t offset,
-     *          int count, int *peof, void *dat)
-     *
-     * Assume that the buffer is "count" bytes in size.
-     *
-     * If you know you have supplied all the data you
-     * have, set *peof.
-     *
-     * You have three ways to return data:
-     * 0) Leave *start = NULL.  (This is the default.)
-     *    Put the data of the requested offset at that
-     *    offset within the buffer.  Return the number (n)
-     *    of bytes there are from the beginning of the
-     *    buffer up to the last byte of data.  If the
-     *    number of supplied bytes (= n - offset) is
-     *    greater than zero and you didn't signal eof
-     *    and the reader is prepared to take more data
-     *    you will be called again with the requested
-     *    offset advanced by the number of bytes
-     *    absorbed.  This interface is useful for files
-     *    no larger than the buffer.
-     * 1) Set *start = an unsigned long value less than
-     *    the buffer address but greater than zero.
-     *    Put the data of the requested offset at the
-     *    beginning of the buffer.  Return the number of
-     *    bytes of data placed there.  If this number is
-     *    greater than zero and you didn't signal eof
-     *    and the reader is prepared to take more data
-     *    you will be called again with the requested
-     *    offset advanced by *start.  This interface is
-     *    useful when you have a large file consisting
-     *    of a series of blocks which you want to count
-     *    and return as wholes.
-     *    (Hack by Paul.Russell@rustcorp.com.au)
-     * 2) Set *start = an address within the buffer.
-     *    Put the data of the requested offset at *start.
-     *    Return the number of bytes of data placed there.
-     *    If this number is greater than zero and you
-     *    didn't signal eof and the reader is prepared to
-     *    take more data you will be called again with the
-     *    requested offset advanced by the number of bytes
-     *    absorbed.
-     */
-
-#if 1
-    /* #4 - interpretation of above = set EOF, return len */
-    *eof = 1;
+		extern int max_chans_used;
+		extern int cxt1e1_max_mtu;
 #endif
+		extern int max_rxdesc_used, max_txdesc_used;
 
+		seq_printf(m, "\ncxt1e1_max_mru:         %d\n", cxt1e1_max_mru);
 #if 0
-    /*
-     * #1 - from net/wireless/atmel.c RLD NOTE -there's something wrong with
-     * this plagarized code which results in this routine being called TWICE.
-     * The second call returns ZERO, resulting in hidden failure, but at
-     * least only a single message set is being displayed.
-     */
-    if (len <= offset + length)
-        *eof = 1;
-    *start = buffer + offset;
-    len -= offset;
-    if (len > length)
-        len = length;
-    if (len < 0)
-        len = 0;
-#endif
-
-#if 0                               /* #2 from net/tokenring/olympic.c +
-                                     * lanstreamer.c */
-    {
-        off_t       begin = 0;
-        int         size = 0;
-        off_t       pos = 0;
-
-        size = len;
-        pos = begin + size;
-        if (pos < offset)
-        {
-            len = 0;
-            begin = pos;
-        }
-        *start = buffer + (offset - begin);     /* Start of wanted data */
-        len -= (offset - begin);    /* Start slop */
-        if (len > length)
-            len = length;           /* Ending slop */
-    }
+		seq_printf(m, "\nmax_chans_used:  %d\n", max_chans_used);
+		seq_printf(m, "cxt1e1_max_mtu:         %d\n", cxt1e1_max_mtu);
 #endif
-
-#if 0                               /* #3 from
-                                     * char/ftape/lowlevel/ftape-proc.c */
-    len = strlen (buffer);
-    *start = NULL;
-    if (offset + length >= len)
-        *eof = 1;
-    else
-        *eof = 0;
-#endif
-
-#if 0
-    pr_info(">> proc_fs: returned len = %d., start %p\n", len, start);  /* RLD DEBUG */
+		seq_printf(m, "max_rxdesc_used: %d\n", max_rxdesc_used);
+		seq_printf(m, "max_txdesc_used: %d\n", max_txdesc_used);
+	}
 #endif
 
-/***
-   using NONE: returns = 314.314.314.
-   using #1  : returns = 314, 0.
-   using #2  : returns = 314, 0, 0.
-   using #3  : returns = 314, 314.
-   using #4  : returns = 314, 314.
-***/
+	kfree(bip);
 
-    return len;
+	pr_devel(">> proc_fs: finished\n");
+	return 0;
 }
 
-/* initialize the /proc subsystem for the specific SBE driver */
-
-int         __init
-sbecom_proc_brd_init (ci_t * ci)
+/*
+ * seq_file wrappers for procfile show routines.
+ */
+static int sbecom_proc_open(struct inode *inode, struct file *file)
 {
-    struct proc_dir_entry *e;
-    char dir[7 + SBE_IFACETMPL_SIZE + 1];
-
-    /* create a directory in the root procfs */
-    snprintf(dir, sizeof(dir), "driver/%s", ci->devname);
-    ci->dir_dev = proc_mkdir(dir, NULL);
-    if (!ci->dir_dev)
-    {
-        pr_err("Unable to create directory /proc/driver/%s\n", ci->devname);
-        goto fail;
-    }
-    e = create_proc_read_entry ("info", S_IFREG | S_IRUGO,
-                                ci->dir_dev, sbecom_proc_get_sbe_info, ci);
-    if (!e)
-    {
-        pr_err("Unable to create entry /proc/driver/%s/info\n", ci->devname);
-        goto fail;
-    }
-    return 0;
-
-fail:
-    sbecom_proc_brd_cleanup (ci);
-    return 1;
+	return single_open(file, sbecom_proc_get_sbe_info, PDE_DATA(inode));
 }
 
-#else                           /*** ! CONFIG_PROC_FS ***/
+static const struct file_operations sbecom_proc_fops = {
+	.open		= sbecom_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 
-/* stubbed off dummy routines */
-
-void
-sbecom_proc_brd_cleanup (ci_t * ci)
-{
-}
-
-int         __init
-sbecom_proc_brd_init (ci_t * ci)
+/*
+ * Initialize the /proc subsystem for the specific SBE driver
+ */
+int __init sbecom_proc_brd_init(ci_t *ci)
 {
-    return 0;
-}
-
-#endif                          /*** CONFIG_PROC_FS ***/
+	char dir[7 + SBE_IFACETMPL_SIZE + 1];
 
+	snprintf(dir, sizeof(dir), "driver/%s", ci->devname);
+	ci->dir_dev = proc_mkdir(dir, NULL);
+	if (!ci->dir_dev) {
+		pr_err("Unable to create directory /proc/driver/%s\n", ci->devname);
+		goto fail;
+	}
+
+	if (!proc_create_data("info", S_IFREG | S_IRUGO, ci->dir_dev,
+			      &sbecom_proc_fops, ci)) {
+		pr_err("Unable to create entry /proc/driver/%s/info\n", ci->devname);
+		goto fail;
+	}
+	return 0;
 
-/*** End-of-File ***/
+fail:
+	sbecom_proc_brd_cleanup(ci);
+	return 1;
+}
diff --git a/drivers/staging/cxt1e1/sbeproc.h b/drivers/staging/cxt1e1/sbeproc.h
index e82be6afd1e8..e5c072cf1952 100644
--- a/drivers/staging/cxt1e1/sbeproc.h
+++ b/drivers/staging/cxt1e1/sbeproc.h
@@ -23,10 +23,20 @@
 
 
 #ifdef CONFIG_PROC_FS
-#ifdef __KERNEL__
 void        sbecom_proc_brd_cleanup (ci_t *);
 int __init  sbecom_proc_brd_init (ci_t *);
 
-#endif                          /*** __KERNEL__ ***/
+#else
+
+static inline void sbecom_proc_brd_cleanup(ci_t * ci)
+{
+}
+
+static inline int __init sbecom_proc_brd_init(ci_t * ci)
+{
+	return 0;
+}
+
 #endif                          /*** CONFIG_PROC_FS ***/
+
 #endif                          /*** _INC_SBEPROC_H_ ***/
diff --git a/drivers/staging/dgrp/dgrp_common.c b/drivers/staging/dgrp/dgrp_common.c
index 3553998b72bc..9a9b45624ba9 100644
--- a/drivers/staging/dgrp/dgrp_common.c
+++ b/drivers/staging/dgrp/dgrp_common.c
@@ -167,34 +167,3 @@ void dgrp_carrier(struct ch_struct *ch)
 		ch->ch_flag &= ~CH_PHYS_CD;
 
 }
-
-/**
- * dgrp_chk_perm() -- check permissions for net device
- * @inode: pointer to inode structure for the net communication device
- * @op: operation to be tested
- *
- * The file permissions and ownerships are tested to determine whether
- * the operation "op" is permitted on the file pointed to by the inode.
- * Returns 0 if the operation is permitted, -EACCESS otherwise
- */
-int dgrp_chk_perm(int mode, int op)
-{
-	if (!uid_eq(GLOBAL_ROOT_UID, current_euid()))
-		mode >>= 6;
-	else if (in_egroup_p(GLOBAL_ROOT_GID))
-		mode >>= 3;
-
-	if ((mode & op & 0007) == op)
-		return 0;
-
-	if (capable(CAP_SYS_ADMIN))
-		return 0;
-
-	return -EACCES;
-}
-
-/* dgrp_chk_perm wrapper for permission call in struct inode_operations */
-int dgrp_inode_permission(struct inode *inode, int op)
-{
-	return dgrp_chk_perm(inode->i_mode, op);
-}
diff --git a/drivers/staging/dgrp/dgrp_common.h b/drivers/staging/dgrp/dgrp_common.h
index 2832b8e26c4b..23aba6c4d22c 100644
--- a/drivers/staging/dgrp/dgrp_common.h
+++ b/drivers/staging/dgrp/dgrp_common.h
@@ -49,20 +49,20 @@ extern struct dgrp_poll_data dgrp_poll_data;
 extern void dgrp_poll_handler(unsigned long arg);
 
 /* from dgrp_mon_ops.c */
-extern void dgrp_register_mon_hook(struct proc_dir_entry *de);
+extern const struct file_operations dgrp_mon_ops;
 
 /* from dgrp_tty.c */
 extern int dgrp_tty_init(struct nd_struct *nd);
 extern void dgrp_tty_uninit(struct nd_struct *nd);
 
 /* from dgrp_ports_ops.c */
-extern void dgrp_register_ports_hook(struct proc_dir_entry *de);
+extern const struct file_operations dgrp_ports_ops;
 
 /* from dgrp_net_ops.c */
-extern void dgrp_register_net_hook(struct proc_dir_entry *de);
+extern const struct file_operations dgrp_net_ops;
 
 /* from dgrp_dpa_ops.c */
-extern void dgrp_register_dpa_hook(struct proc_dir_entry *de);
+extern const struct file_operations dgrp_dpa_ops;
 extern void dgrp_dpa_data(struct nd_struct *, int, u8 *, int);
 
 /* from dgrp_sysfs.c */
@@ -76,61 +76,6 @@ extern void dgrp_create_tty_sysfs(struct un_struct *un, struct device *c);
 extern void dgrp_remove_tty_sysfs(struct device *c);
 
 /* from dgrp_specproc.c */
-/*
- *  The list of DGRP entries with r/w capabilities.  These
- *  magic numbers are used for identification purposes.
- */
-enum {
-	DGRP_CONFIG = 1,	/* Configure portservers */
-	DGRP_NETDIR = 2,	/* Directory for "net" devices */
-	DGRP_MONDIR = 3,	/* Directory for "mon" devices */
-	DGRP_PORTSDIR = 4,	/* Directory for "ports" devices */
-	DGRP_INFO = 5,		/* Get info. about the running module */
-	DGRP_NODEINFO = 6,	/* Get info. about the configured nodes */
-	DGRP_DPADIR = 7,	/* Directory for the "dpa" devices */
-};
-
-/*
- *  Directions for proc handlers
- */
-enum {
-	INBOUND = 1,		/* Data being written to kernel */
-	OUTBOUND = 2,		/* Data being read from the kernel */
-};
-
-/**
- * dgrp_proc_entry: structure for dgrp proc dirs
- * @id: ID number associated with this particular entry.  Should be
- *    unique across all of DGRP.
- * @name: text name associated with the /proc entry
- * @mode: file access permisssions for the /proc entry
- * @child: pointer to table describing a subdirectory for this entry
- * @de: pointer to directory entry for this object once registered.  Used
- *    to grab the handle of the object for unregistration
- * @excl_sem: semaphore to provide exclusive to struct
- * @excl_cnt: counter of current accesses
- *
- *  Each entry in a DGRP proc directory is described with a
- *  dgrp_proc_entry structure.  A collection of these
- *  entries (in an array) represents the members associated
- *  with a particular /proc directory, and is referred to
- *  as a table.  All tables are terminated by an entry with
- *  zeros for every member.
- */
-struct dgrp_proc_entry {
-	int                  id;          /* Integer identifier */
-	const char        *name;          /* ASCII identifier */
-	mode_t             mode;          /* File access permissions */
-	struct dgrp_proc_entry *child;    /* Child pointer */
-
-	/* file ops to use, pass NULL to use default */
-	struct file_operations *proc_file_ops;
-
-	struct proc_dir_entry *de;        /* proc entry pointer */
-	struct semaphore   excl_sem;      /* Protects exclusive access var */
-	int                excl_cnt;      /* Counts number of curr accesses */
-};
-
 extern void dgrp_unregister_proc(void);
 extern void dgrp_register_proc(void);
 
@@ -144,8 +89,6 @@ extern void dgrp_register_proc(void);
  *-----------------------------------------------------------------------*/
 
 void dgrp_carrier(struct ch_struct *ch);
-extern int dgrp_inode_permission(struct inode *inode, int op);
-extern int dgrp_chk_perm(int mode, int op);
 
 
 /*
diff --git a/drivers/staging/dgrp/dgrp_dpa_ops.c b/drivers/staging/dgrp/dgrp_dpa_ops.c
index ca10a3362e29..114799cddd85 100644
--- a/drivers/staging/dgrp/dgrp_dpa_ops.c
+++ b/drivers/staging/dgrp/dgrp_dpa_ops.c
@@ -40,6 +40,7 @@
 #include <linux/cred.h>
 #include <linux/sched.h>
 #include <linux/ratelimit.h>
+#include <linux/slab.h>
 #include <asm/unaligned.h>
 
 #include "dgrp_common.h"
@@ -52,7 +53,7 @@ static long dgrp_dpa_ioctl(struct file *file, unsigned int cmd,
 			   unsigned long arg);
 static unsigned int dgrp_dpa_select(struct file *, struct poll_table_struct *);
 
-static const struct file_operations dpa_ops = {
+const struct file_operations dgrp_dpa_ops = {
 	.owner   =  THIS_MODULE,
 	.read    =  dgrp_dpa_read,
 	.poll    =  dgrp_dpa_select,
@@ -61,12 +62,6 @@ static const struct file_operations dpa_ops = {
 	.release =  dgrp_dpa_release,
 };
 
-static struct inode_operations dpa_inode_ops = {
-	.permission = dgrp_inode_permission
-};
-
-
-
 struct digi_node {
 	uint	nd_state;		/* Node state: 1 = up, 0 = down. */
 	uint	nd_chan_count;		/* Number of channels found */
@@ -111,17 +106,6 @@ struct digi_debug {
 #define DIGI_SETDEBUG      (('d'<<8) | 247)	/* set debug info */
 
 
-void dgrp_register_dpa_hook(struct proc_dir_entry *de)
-{
-	struct nd_struct *node = de->data;
-
-	de->proc_iops = &dpa_inode_ops;
-	rcu_assign_pointer(de->proc_fops, &dpa_ops);
-
-	node->nd_dpa_de = de;
-	spin_lock_init(&node->nd_dpa_lock);
-}
-
 /*
  * dgrp_dpa_open -- open the DPA device for a particular PortServer
  */
@@ -130,8 +114,6 @@ static int dgrp_dpa_open(struct inode *inode, struct file *file)
 	struct nd_struct *nd;
 	int rtn = 0;
 
-	struct proc_dir_entry *de;
-
 	rtn = try_module_get(THIS_MODULE);
 	if (!rtn)
 		return -ENXIO;
@@ -154,12 +136,7 @@ static int dgrp_dpa_open(struct inode *inode, struct file *file)
 	/*
 	 *  Get the node pointer, and fail if it doesn't exist.
 	 */
-	de = PDE(inode);
-	if (!de) {
-		rtn = -ENXIO;
-		goto done;
-	}
-	nd = (struct nd_struct *)de->data;
+	nd = PDE_DATA(inode);
 	if (!nd) {
 		rtn = -ENXIO;
 		goto done;
diff --git a/drivers/staging/dgrp/dgrp_mon_ops.c b/drivers/staging/dgrp/dgrp_mon_ops.c
index b484fccb494e..d18be4180e3b 100644
--- a/drivers/staging/dgrp/dgrp_mon_ops.c
+++ b/drivers/staging/dgrp/dgrp_mon_ops.c
@@ -37,6 +37,7 @@
 #include <linux/tty.h>
 #include <linux/sched.h>
 #include <asm/unaligned.h>
+#include <linux/slab.h>
 #include <linux/proc_fs.h>
 #include <linux/uaccess.h>
 
@@ -49,7 +50,7 @@ static ssize_t dgrp_mon_read(struct file *, char __user *, size_t, loff_t *);
 static long dgrp_mon_ioctl(struct file *file, unsigned int cmd,
 			   unsigned long arg);
 
-static const struct file_operations mon_ops = {
+const struct file_operations dgrp_mon_ops = {
 	.owner   = THIS_MODULE,
 	.read    = dgrp_mon_read,
 	.unlocked_ioctl = dgrp_mon_ioctl,
@@ -57,20 +58,6 @@ static const struct file_operations mon_ops = {
 	.release = dgrp_mon_release,
 };
 
-static struct inode_operations mon_inode_ops = {
-	.permission = dgrp_inode_permission
-};
-
-void dgrp_register_mon_hook(struct proc_dir_entry *de)
-{
-	struct nd_struct *node = de->data;
-
-	de->proc_iops = &mon_inode_ops;
-	rcu_assign_pointer(de->proc_fops, &mon_ops);
-	node->nd_mon_de = de;
-	sema_init(&node->nd_mon_semaphore, 1);
-}
-
 /**
  * dgrp_mon_open() -- open /proc/dgrp/ports device for a PortServer
  * @inode: struct inode *
@@ -81,7 +68,6 @@ void dgrp_register_mon_hook(struct proc_dir_entry *de)
 static int dgrp_mon_open(struct inode *inode, struct file *file)
 {
 	struct nd_struct *nd;
-	struct proc_dir_entry *de;
 	struct timeval tv;
 	uint32_t time;
 	u8 *buf;
@@ -109,13 +95,7 @@ static int dgrp_mon_open(struct inode *inode, struct file *file)
 	/*
 	 *  Get the node pointer, and fail if it doesn't exist.
 	 */
-	de = PDE(inode);
-	if (!de) {
-		rtn = -ENXIO;
-		goto done;
-	}
-
-	nd = (struct nd_struct *)de->data;
+	nd = PDE_DATA(inode);
 	if (!nd) {
 		rtn = -ENXIO;
 		goto done;
diff --git a/drivers/staging/dgrp/dgrp_net_ops.c b/drivers/staging/dgrp/dgrp_net_ops.c
index 64f48ffb9d4e..5b7833f593ff 100644
--- a/drivers/staging/dgrp/dgrp_net_ops.c
+++ b/drivers/staging/dgrp/dgrp_net_ops.c
@@ -35,7 +35,7 @@
 
 #include <linux/module.h>
 #include <linux/proc_fs.h>
-#include <linux/types.h>
+#include <linux/slab.h>
 #include <linux/string.h>
 #include <linux/device.h>
 #include <linux/tty.h>
@@ -72,7 +72,7 @@ static long dgrp_net_ioctl(struct file *file, unsigned int cmd,
 static unsigned int dgrp_net_select(struct file *file,
 				    struct poll_table_struct *table);
 
-static const struct file_operations net_ops = {
+const struct file_operations dgrp_net_ops = {
 	.owner   =  THIS_MODULE,
 	.read    =  dgrp_net_read,
 	.write   =  dgrp_net_write,
@@ -82,23 +82,6 @@ static const struct file_operations net_ops = {
 	.release =  dgrp_net_release,
 };
 
-static struct inode_operations net_inode_ops = {
-	.permission = dgrp_inode_permission
-};
-
-void dgrp_register_net_hook(struct proc_dir_entry *de)
-{
-	struct nd_struct *node = de->data;
-
-	de->proc_iops = &net_inode_ops;
-	rcu_assign_pointer(de->proc_fops, &net_ops);
-	node->nd_net_de = de;
-	sema_init(&node->nd_net_semaphore, 1);
-	node->nd_state = NS_CLOSED;
-	dgrp_create_node_class_sysfs_files(node);
-}
-
-
 /**
  * dgrp_dump() -- prints memory for debugging purposes.
  * @mem: Memory location which should be printed to the console
@@ -801,7 +784,6 @@ out_err:
 static int dgrp_net_open(struct inode *inode, struct file *file)
 {
 	struct nd_struct *nd;
-	struct proc_dir_entry *de;
 	ulong  lock_flags;
 	int rtn;
 
@@ -825,13 +807,7 @@ static int dgrp_net_open(struct inode *inode, struct file *file)
 	/*
 	 *  Get the node pointer, and fail if it doesn't exist.
 	 */
-	de = PDE(inode);
-	if (!de) {
-		rtn = -ENXIO;
-		goto done;
-	}
-
-	nd = (struct nd_struct *) de->data;
+	nd = PDE_DATA(inode);
 	if (!nd) {
 		rtn = -ENXIO;
 		goto done;
diff --git a/drivers/staging/dgrp/dgrp_ports_ops.c b/drivers/staging/dgrp/dgrp_ports_ops.c
index f93dc1f262f5..4ce030815f27 100644
--- a/drivers/staging/dgrp/dgrp_ports_ops.c
+++ b/drivers/staging/dgrp/dgrp_ports_ops.c
@@ -47,7 +47,7 @@
 /* File operation declarations */
 static int dgrp_ports_open(struct inode *, struct file *);
 
-static const struct file_operations ports_ops = {
+const struct file_operations dgrp_ports_ops = {
 	.owner   = THIS_MODULE,
 	.open    = dgrp_ports_open,
 	.read    = seq_read,
@@ -55,20 +55,6 @@ static const struct file_operations ports_ops = {
 	.release = seq_release
 };
 
-static struct inode_operations ports_inode_ops = {
-	.permission = dgrp_inode_permission
-};
-
-
-void dgrp_register_ports_hook(struct proc_dir_entry *de)
-{
-	struct nd_struct *node = de->data;
-
-	de->proc_iops = &ports_inode_ops;
-	rcu_assign_pointer(de->proc_fops, &ports_ops);
-	node->nd_ports_de = de;
-}
-
 static void *dgrp_ports_seq_start(struct seq_file *seq, loff_t *pos)
 {
 	if (*pos == 0)
@@ -163,7 +149,7 @@ static int dgrp_ports_open(struct inode *inode, struct file *file)
 	rtn = seq_open(file, &ports_seq_ops);
 	if (!rtn) {
 		seq = file->private_data;
-		seq->private = PDE(inode)->data;
+		seq->private = PDE_DATA(inode);
 	}
 
 	return rtn;
diff --git a/drivers/staging/dgrp/dgrp_specproc.c b/drivers/staging/dgrp/dgrp_specproc.c
index d66712c8aa94..205d80ef4455 100644
--- a/drivers/staging/dgrp/dgrp_specproc.c
+++ b/drivers/staging/dgrp/dgrp_specproc.c
@@ -37,6 +37,7 @@
 #include <linux/sched.h>
 #include <linux/cred.h>
 #include <linux/proc_fs.h>
+#include <linux/slab.h>
 #include <linux/ctype.h>
 #include <linux/seq_file.h>
 #include <linux/uaccess.h>
@@ -44,43 +45,17 @@
 
 #include "dgrp_common.h"
 
-static struct dgrp_proc_entry dgrp_table[];
 static struct proc_dir_entry *dgrp_proc_dir_entry;
 
 static int dgrp_add_id(long id);
 static int dgrp_remove_nd(struct nd_struct *nd);
-static void unregister_dgrp_device(struct proc_dir_entry *de);
-static void register_dgrp_device(struct nd_struct *node,
+static struct proc_dir_entry *add_proc_file(struct nd_struct *node,
 				 struct proc_dir_entry *root,
-				 void (*register_hook)(struct proc_dir_entry *de));
+				 const struct file_operations *fops);
 
 /* File operation declarations */
-static int dgrp_gen_proc_open(struct inode *, struct file *);
-static int dgrp_gen_proc_close(struct inode *, struct file *);
 static int parse_write_config(char *);
 
-
-static const struct file_operations dgrp_proc_file_ops = {
-	.owner   = THIS_MODULE,
-	.open    = dgrp_gen_proc_open,
-	.release = dgrp_gen_proc_close,
-};
-
-static struct inode_operations proc_inode_ops = {
-	.permission = dgrp_inode_permission
-};
-
-
-static void register_proc_table(struct dgrp_proc_entry *,
-				struct proc_dir_entry *);
-static void unregister_proc_table(struct dgrp_proc_entry *,
-				  struct proc_dir_entry *);
-
-static struct dgrp_proc_entry dgrp_net_table[];
-static struct dgrp_proc_entry dgrp_mon_table[];
-static struct dgrp_proc_entry dgrp_ports_table[];
-static struct dgrp_proc_entry dgrp_dpa_table[];
-
 static ssize_t dgrp_config_proc_write(struct file *file,
 				      const char __user *buffer,
 				      size_t count, loff_t *pos);
@@ -89,7 +64,7 @@ static int dgrp_nodeinfo_proc_open(struct inode *inode, struct file *file);
 static int dgrp_info_proc_open(struct inode *inode, struct file *file);
 static int dgrp_config_proc_open(struct inode *inode, struct file *file);
 
-static struct file_operations config_proc_file_ops = {
+static const struct file_operations config_proc_file_ops = {
 	.owner	 = THIS_MODULE,
 	.open	 = dgrp_config_proc_open,
 	.read	 = seq_read,
@@ -98,7 +73,7 @@ static struct file_operations config_proc_file_ops = {
 	.write   = dgrp_config_proc_write,
 };
 
-static struct file_operations info_proc_file_ops = {
+static const struct file_operations info_proc_file_ops = {
 	.owner	 = THIS_MODULE,
 	.open	 = dgrp_info_proc_open,
 	.read	 = seq_read,
@@ -106,7 +81,7 @@ static struct file_operations info_proc_file_ops = {
 	.release = single_release,
 };
 
-static struct file_operations nodeinfo_proc_file_ops = {
+static const struct file_operations nodeinfo_proc_file_ops = {
 	.owner	 = THIS_MODULE,
 	.open	 = dgrp_nodeinfo_proc_open,
 	.read	 = seq_read,
@@ -114,71 +89,25 @@ static struct file_operations nodeinfo_proc_file_ops = {
 	.release = seq_release,
 };
 
-static struct dgrp_proc_entry dgrp_table[] = {
-	{
-		.id = DGRP_CONFIG,
-		.name = "config",
-		.mode = 0644,
-		.proc_file_ops = &config_proc_file_ops,
-	},
-	{
-		.id = DGRP_INFO,
-		.name = "info",
-		.mode = 0644,
-		.proc_file_ops = &info_proc_file_ops,
-	},
-	{
-		.id = DGRP_NODEINFO,
-		.name = "nodeinfo",
-		.mode = 0644,
-		.proc_file_ops = &nodeinfo_proc_file_ops,
-	},
-	{
-		.id = DGRP_NETDIR,
-		.name = "net",
-		.mode = 0500,
-		.child = dgrp_net_table
-	},
-	{
-		.id = DGRP_MONDIR,
-		.name = "mon",
-		.mode = 0500,
-		.child = dgrp_mon_table
-	},
-	{
-		.id = DGRP_PORTSDIR,
-		.name = "ports",
-		.mode = 0500,
-		.child = dgrp_ports_table
-	},
-	{
-		.id = DGRP_DPADIR,
-		.name = "dpa",
-		.mode = 0500,
-		.child = dgrp_dpa_table
-	}
-};
-
 static struct proc_dir_entry *net_entry_pointer;
 static struct proc_dir_entry *mon_entry_pointer;
 static struct proc_dir_entry *dpa_entry_pointer;
 static struct proc_dir_entry *ports_entry_pointer;
 
-static struct dgrp_proc_entry dgrp_net_table[] = {
-	{0}
-};
-
-static struct dgrp_proc_entry dgrp_mon_table[] = {
-	{0}
-};
-
-static struct dgrp_proc_entry dgrp_ports_table[] = {
-	{0}
-};
-
-static struct dgrp_proc_entry dgrp_dpa_table[] = {
-	{0}
-};
+static void remove_files(struct nd_struct *nd)
+{
+	char buf[3];
+	ID_TO_CHAR(nd->nd_ID, buf);
+	dgrp_remove_node_class_sysfs_files(nd);
+	if (nd->nd_net_de)
+		remove_proc_entry(buf, net_entry_pointer);
+	if (nd->nd_mon_de)
+		remove_proc_entry(buf, mon_entry_pointer);
+	if (nd->nd_dpa_de)
+		remove_proc_entry(buf, dpa_entry_pointer);
+	if (nd->nd_ports_de)
+		remove_proc_entry(buf, ports_entry_pointer);
+}
 
 void dgrp_unregister_proc(void)
 {
@@ -188,12 +117,19 @@ void dgrp_unregister_proc(void)
 	ports_entry_pointer = NULL;
 
 	if (dgrp_proc_dir_entry) {
-		unregister_proc_table(dgrp_table, dgrp_proc_dir_entry);
-		remove_proc_entry(dgrp_proc_dir_entry->name,
-				  dgrp_proc_dir_entry->parent);
+		struct nd_struct *nd;
+		list_for_each_entry(nd, &nd_struct_list, list)
+			remove_files(nd);
+		remove_proc_entry("dgrp/config", NULL);
+		remove_proc_entry("dgrp/info", NULL);
+		remove_proc_entry("dgrp/nodeinfo", NULL);
+		remove_proc_entry("dgrp/net", NULL);
+		remove_proc_entry("dgrp/mon", NULL);
+		remove_proc_entry("dgrp/dpa", NULL);
+		remove_proc_entry("dgrp/ports", NULL);
+		remove_proc_entry("dgrp", NULL);
 		dgrp_proc_dir_entry = NULL;
 	}
-
 }
 
 void dgrp_register_proc(void)
@@ -201,211 +137,16 @@ void dgrp_register_proc(void)
 	/*
 	 *	Register /proc/dgrp
 	 */
-	dgrp_proc_dir_entry = proc_create("dgrp", S_IFDIR, NULL,
-					  &dgrp_proc_file_ops);
-	register_proc_table(dgrp_table, dgrp_proc_dir_entry);
-}
-
-/*
- * /proc/sys support
- */
-static int dgrp_proc_match(int len, const char *name, struct proc_dir_entry *de)
-{
-	if (!de || !de->low_ino)
-		return 0;
-	if (de->namelen != len)
-		return 0;
-	return !memcmp(name, de->name, len);
-}
-
-
-/*
- *  Scan the entries in table and add them all to /proc at the position
- *  referred to by "root"
- */
-static void register_proc_table(struct dgrp_proc_entry *table,
-				struct proc_dir_entry *root)
-{
-	struct proc_dir_entry *de;
-	int len;
-	mode_t mode;
-
-	if (table == NULL)
+	dgrp_proc_dir_entry = proc_mkdir("dgrp", NULL);
+	if (!dgrp_proc_dir_entry)
 		return;
-	if (root == NULL)
-		return;
-
-	for (; table->id; table++) {
-		/* Can't do anything without a proc name. */
-		if (!table->name)
-			continue;
-
-		/* Maybe we can't do anything with it... */
-		if (!table->proc_file_ops &&
-		    !table->child) {
-			pr_warn("dgrp: Can't register %s\n",
-				table->name);
-			continue;
-		}
-
-		len = strlen(table->name);
-		mode = table->mode;
-
-		de = NULL;
-		if (!table->child)
-			mode |= S_IFREG;
-		else {
-			mode |= S_IFDIR;
-			for (de = root->subdir; de; de = de->next) {
-				if (dgrp_proc_match(len, table->name, de))
-					break;
-			}
-			/* If the subdir exists already, de is non-NULL */
-		}
-
-		if (!de) {
-			de = create_proc_entry(table->name, mode, root);
-			if (!de)
-				continue;
-			de->data = (void *) table;
-			if (!table->child) {
-				de->proc_iops = &proc_inode_ops;
-				if (table->proc_file_ops)
-					rcu_assign_pointer(de->proc_fops,
-							table->proc_file_ops);
-				else
-					rcu_assign_pointer(de->proc_fops,
-							 &dgrp_proc_file_ops);
-			}
-		}
-		table->de = de;
-		if (de->mode & S_IFDIR)
-			register_proc_table(table->child, de);
-
-		if (table->id == DGRP_NETDIR)
-			net_entry_pointer = de;
-
-		if (table->id == DGRP_MONDIR)
-			mon_entry_pointer = de;
-
-		if (table->id == DGRP_DPADIR)
-			dpa_entry_pointer = de;
-
-		if (table->id == DGRP_PORTSDIR)
-			ports_entry_pointer = de;
-	}
-}
-
-/*
- * Unregister a /proc sysctl table and any subdirectories.
- */
-static void unregister_proc_table(struct dgrp_proc_entry *table,
-				  struct proc_dir_entry *root)
-{
-	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);
-			dgrp_remove_node_class_sysfs_files(tmp);
-		}
-
-		if ((table == dgrp_mon_table) && (tmp->nd_mon_de))
-			unregister_dgrp_device(tmp->nd_mon_de);
-
-		if ((table == dgrp_dpa_table) && (tmp->nd_dpa_de))
-			unregister_dgrp_device(tmp->nd_dpa_de);
-
-		if ((table == dgrp_ports_table) && (tmp->nd_ports_de))
-			unregister_dgrp_device(tmp->nd_ports_de);
-	}
-
-	for (; table->id; table++) {
-		de = table->de;
-
-		if (!de)
-			continue;
-		if (de->mode & S_IFDIR) {
-			if (!table->child) {
-				pr_alert("dgrp: malformed sysctl tree on free\n");
-				continue;
-			}
-			unregister_proc_table(table->child, de);
-
-	/* Don't unregister directories which still have entries */
-			if (de->subdir)
-				continue;
-		}
-
-		/* Don't unregister proc entries that are still being used.. */
-		if ((atomic_read(&de->count)) != 1) {
-			pr_alert("proc entry %s in use, not removing\n",
-				de->name);
-			continue;
-		}
-
-		remove_proc_entry(de->name, de->parent);
-		table->de = NULL;
-	}
-}
-
-static int dgrp_gen_proc_open(struct inode *inode, struct file *file)
-{
-	struct proc_dir_entry *de;
-	struct dgrp_proc_entry *entry;
-	int ret = 0;
-
-	de = (struct proc_dir_entry *) PDE(file_inode(file));
-	if (!de || !de->data) {
-		ret = -ENXIO;
-		goto done;
-	}
-
-	entry = (struct dgrp_proc_entry *) de->data;
-	if (!entry) {
-		ret = -ENXIO;
-		goto done;
-	}
-
-	down(&entry->excl_sem);
-
-	if (entry->excl_cnt)
-		ret = -EBUSY;
-	else
-		entry->excl_cnt++;
-
-	up(&entry->excl_sem);
-
-done:
-	return ret;
-}
-
-static int dgrp_gen_proc_close(struct inode *inode, struct file *file)
-{
-	struct proc_dir_entry *de;
-	struct dgrp_proc_entry *entry;
-
-	de = (struct proc_dir_entry *) PDE(file_inode(file));
-	if (!de || !de->data)
-		goto done;
-
-	entry = (struct dgrp_proc_entry *) de->data;
-	if (!entry)
-		goto done;
-
-	down(&entry->excl_sem);
-
-	if (entry->excl_cnt)
-		entry->excl_cnt = 0;
-
-	up(&entry->excl_sem);
-
-done:
-	return 0;
+	proc_create("dgrp/config", 0644, NULL, &config_proc_file_ops);
+	proc_create("dgrp/info", 0644, NULL, &info_proc_file_ops);
+	proc_create("dgrp/nodeinfo", 0644, NULL, &nodeinfo_proc_file_ops);
+	net_entry_pointer = proc_mkdir_mode("dgrp/net", 0500, NULL);
+	mon_entry_pointer = proc_mkdir_mode("dgrp/mon", 0500, NULL);
+	dpa_entry_pointer = proc_mkdir_mode("dgrp/dpa", 0500, NULL);
+	ports_entry_pointer = proc_mkdir_mode("dgrp/ports", 0500, NULL);
 }
 
 static void *dgrp_config_proc_start(struct seq_file *m, loff_t *pos)
@@ -736,6 +477,10 @@ static int dgrp_add_id(long id)
 	init_waitqueue_head(&nd->nd_tx_waitq);
 	init_waitqueue_head(&nd->nd_mon_wqueue);
 	init_waitqueue_head(&nd->nd_dpa_wqueue);
+	sema_init(&nd->nd_mon_semaphore, 1);
+	sema_init(&nd->nd_net_semaphore, 1);
+	spin_lock_init(&nd->nd_dpa_lock);
+	nd->nd_state = NS_CLOSED;
 	for (i = 0; i < SEQ_MAX; i++)
 		init_waitqueue_head(&nd->nd_seq_wque[i]);
 
@@ -750,12 +495,12 @@ static int dgrp_add_id(long id)
 	if (ret)
 		goto error_out;
 
-	register_dgrp_device(nd, net_entry_pointer, dgrp_register_net_hook);
-	register_dgrp_device(nd, mon_entry_pointer, dgrp_register_mon_hook);
-	register_dgrp_device(nd, dpa_entry_pointer, dgrp_register_dpa_hook);
-	register_dgrp_device(nd, ports_entry_pointer,
-			      dgrp_register_ports_hook);
-
+	dgrp_create_node_class_sysfs_files(nd);
+	nd->nd_net_de = add_proc_file(nd, net_entry_pointer, &dgrp_net_ops);
+	nd->nd_mon_de = add_proc_file(nd, mon_entry_pointer, &dgrp_mon_ops);
+	nd->nd_dpa_de = add_proc_file(nd, dpa_entry_pointer, &dgrp_dpa_ops);
+	nd->nd_ports_de = add_proc_file(nd, ports_entry_pointer,
+					&dgrp_ports_ops);
 	return 0;
 
 	/* FIXME this guy should free the tty driver stored in nd and destroy
@@ -774,16 +519,7 @@ static int dgrp_remove_nd(struct nd_struct *nd)
 	if (nd->nd_tty_ref_cnt)
 		return -EBUSY;
 
-	if (nd->nd_net_de) {
-		unregister_dgrp_device(nd->nd_net_de);
-		dgrp_remove_node_class_sysfs_files(nd);
-	}
-
-	unregister_dgrp_device(nd->nd_mon_de);
-
-	unregister_dgrp_device(nd->nd_ports_de);
-
-	unregister_dgrp_device(nd->nd_dpa_de);
+	remove_files(nd);
 
 	dgrp_tty_uninit(nd);
 
@@ -795,38 +531,11 @@ static int dgrp_remove_nd(struct nd_struct *nd)
 	return 0;
 }
 
-static void register_dgrp_device(struct nd_struct *node,
+static struct proc_dir_entry *add_proc_file(struct nd_struct *node,
 				 struct proc_dir_entry *root,
-				 void (*register_hook)(struct proc_dir_entry *de))
+				 const struct file_operations *fops)
 {
 	char buf[3];
-	struct proc_dir_entry *de;
-
 	ID_TO_CHAR(node->nd_ID, buf);
-
-	de = create_proc_entry(buf, 0600 | S_IFREG, root);
-	if (!de)
-		return;
-
-	de->data = (void *) node;
-
-	if (register_hook)
-		register_hook(de);
-
-}
-
-static void unregister_dgrp_device(struct proc_dir_entry *de)
-{
-	if (!de)
-		return;
-
-	/* Don't unregister proc entries that are still being used.. */
-	if ((atomic_read(&de->count)) != 1) {
-		pr_alert("%s - proc entry %s in use. Not removing.\n",
-			 __func__, de->name);
-		return;
-	}
-
-	remove_proc_entry(de->name, de->parent);
-	de = NULL;
+	return proc_create_data(buf, 0600, root, fops, node);
 }
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_proc.c b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_proc.c
index 5337b415d450..94e426e4d98b 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_proc.c
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_proc.c
@@ -19,6 +19,7 @@
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 #include <linux/string.h>
 #include <linux/vmalloc.h>
 #include <linux/netdevice.h>
@@ -29,70 +30,55 @@
 #define FT1000_PROC "ft1000"
 #define MAX_FILE_LEN 255
 
-#define PUTM_TO_PAGE(len, page, args...) \
-	len += snprintf(page+len, PAGE_SIZE - len, args)
-
-#define PUTX_TO_PAGE(len, page, message, size, var) \
-	len += snprintf(page+len, PAGE_SIZE - len, message); \
+#define seq_putx(m, message, size, var) \
+	seq_printf(m, message);	\
 	for(i = 0; i < (size - 1); i++) { \
-		len += snprintf(page+len, PAGE_SIZE - len, "%02x:", var[i]); \
+		seq_printf(m, "%02x:", var[i]); \
 	} \
-	len += snprintf(page+len, PAGE_SIZE - len, "%02x\n", var[i])
+	seq_printf(m, "%02x\n", var[i])
 
-#define PUTD_TO_PAGE(len, page, message, size, var) \
-	len += snprintf(page+len, PAGE_SIZE - len, message); \
+#define seq_putd(m, message, size, var) \
+	seq_printf(m, message); \
 	for(i = 0; i < (size - 1); i++) { \
-		len += snprintf(page+len, PAGE_SIZE - len, "%d.", var[i]); \
+		seq_printf(m, "%d.", var[i]); \
 	} \
-	len += snprintf(page+len, PAGE_SIZE - len, "%d\n", var[i])
+	seq_printf(m, "%d\n", var[i])
 
-static int ft1000ReadProc(char *page, char **start, off_t off,
-			  int count, int *eof, void *data)
+static int ft1000ReadProc(struct seq_file *m, void *v)
 {
-	struct net_device *dev;
-	int len;
-	int i;
-	struct ft1000_info *info;
-	char *status[] = {
+	static const char *status[] = {
 		"Idle (Disconnect)", "Searching", "Active (Connected)",
 		"Waiting for L2", "Sleep", "No Coverage", "", ""
 	};
-	char *signal[] = { "", "*", "**", "***", "****" };
+	static const char *signal[] = { "", "*", "**", "***", "****" };
+
+	struct net_device *dev = m->private;
+	struct ft1000_info *info = netdev_priv(dev);
+	int i;
 	int strength;
 	int quality;
 	struct timeval tv;
 	time_t delta;
 
-	dev = (struct net_device *)data;
-	info = netdev_priv(dev);
-
-	if (off > 0) {
-		*eof = 1;
-		return 0;
-	}
-
-	/* Wrap-around */
-
 	if (info->AsicID == ELECTRABUZZ_ID) {
 		if (info->ProgConStat != 0xFF) {
 			info->LedStat =
 				ft1000_read_dpram(dev, FT1000_DSP_LED);
 			info->ConStat =
-				ft1000_read_dpram(dev,
-						  FT1000_DSP_CON_STATE);
+				ft1000_read_dpram(dev, FT1000_DSP_CON_STATE);
 		} else {
 			info->ConStat = 0xf;
 		}
 	} else {
 		if (info->ProgConStat != 0xFF) {
 			info->LedStat =
-				ntohs(ft1000_read_dpram_mag_16
-				  (dev, FT1000_MAG_DSP_LED,
-				   FT1000_MAG_DSP_LED_INDX));
+				ntohs(ft1000_read_dpram_mag_16(
+					      dev, FT1000_MAG_DSP_LED,
+					      FT1000_MAG_DSP_LED_INDX));
 			info->ConStat =
-				ntohs(ft1000_read_dpram_mag_16
-				  (dev, FT1000_MAG_DSP_CON_STATE,
-				   FT1000_MAG_DSP_CON_STATE_INDX));
+				ntohs(ft1000_read_dpram_mag_16(
+					      dev, FT1000_MAG_DSP_CON_STATE,
+					      FT1000_MAG_DSP_CON_STATE_INDX));
 		} else {
 			info->ConStat = 0xf;
 		}
@@ -135,36 +121,46 @@ static int ft1000ReadProc(char *page, char **start, off_t off,
 	}
 
 	do_gettimeofday(&tv);
-	delta = (tv.tv_sec - info->ConTm);
-	len = 0;
-	PUTM_TO_PAGE(len, page, "Connection Time: %02ld:%02ld:%02ld\n",
+	delta = tv.tv_sec - info->ConTm;
+	seq_printf(m, "Connection Time: %02ld:%02ld:%02ld\n",
 			 ((delta / 3600) % 24), ((delta / 60) % 60), (delta % 60));
-	PUTM_TO_PAGE(len, page, "Connection Time[s]: %ld\n", delta);
-	PUTM_TO_PAGE(len, page, "Asic ID: %s\n",
-			 (info->AsicID) ==
+	seq_printf(m, "Connection Time[s]: %ld\n", delta);
+	seq_printf(m, "Asic ID: %s\n",
+			 info->AsicID ==
 			 ELECTRABUZZ_ID ? "ELECTRABUZZ ASIC" : "MAGNEMITE ASIC");
-	PUTX_TO_PAGE(len, page, "SKU: ", SKUSZ, info->Sku);
-	PUTX_TO_PAGE(len, page, "EUI64: ", EUISZ, info->eui64);
-	PUTD_TO_PAGE(len, page, "DSP version number: ", DSPVERSZ, info->DspVer);
-	PUTX_TO_PAGE(len, page, "Hardware Serial Number: ", HWSERNUMSZ,
-			 info->HwSerNum);
-	PUTX_TO_PAGE(len, page, "Caliberation Version: ", CALVERSZ,
-			 info->RfCalVer);
-	PUTD_TO_PAGE(len, page, "Caliberation Date: ", CALDATESZ,
-			 info->RfCalDate);
-	PUTM_TO_PAGE(len, page, "Media State: %s\n",
+	seq_putx(m, "SKU: ", SKUSZ, info->Sku);
+	seq_putx(m, "EUI64: ", EUISZ, info->eui64);
+	seq_putd(m, "DSP version number: ", DSPVERSZ, info->DspVer);
+	seq_putx(m, "Hardware Serial Number: ", HWSERNUMSZ, info->HwSerNum);
+	seq_putx(m, "Caliberation Version: ", CALVERSZ, info->RfCalVer);
+	seq_putd(m, "Caliberation Date: ", CALDATESZ, info->RfCalDate);
+	seq_printf(m, "Media State: %s\n",
 			 (info->mediastate) ? "link" : "no link");
-	PUTM_TO_PAGE(len, page, "Connection Status: %s\n",
-			 status[((info->ConStat) & 0x7)]);
-	PUTM_TO_PAGE(len, page, "RX packets: %ld\n", info->stats.rx_packets);
-	PUTM_TO_PAGE(len, page, "TX packets: %ld\n", info->stats.tx_packets);
-	PUTM_TO_PAGE(len, page, "RX bytes: %ld\n", info->stats.rx_bytes);
-	PUTM_TO_PAGE(len, page, "TX bytes: %ld\n", info->stats.tx_bytes);
-	PUTM_TO_PAGE(len, page, "Signal Strength: %s\n", signal[strength]);
-	PUTM_TO_PAGE(len, page, "Signal Quality: %s\n", signal[quality]);
-	return len;
+	seq_printf(m, "Connection Status: %s\n", status[info->ConStat & 0x7]);
+	seq_printf(m, "RX packets: %ld\n", info->stats.rx_packets);
+	seq_printf(m, "TX packets: %ld\n", info->stats.tx_packets);
+	seq_printf(m, "RX bytes: %ld\n", info->stats.rx_bytes);
+	seq_printf(m, "TX bytes: %ld\n", info->stats.tx_bytes);
+	seq_printf(m, "Signal Strength: %s\n", signal[strength]);
+	seq_printf(m, "Signal Quality: %s\n", signal[quality]);
+	return 0;
+}
+
+/*
+ * seq_file wrappers for procfile show routines.
+ */
+static int ft1000_proc_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ft1000ReadProc, PDE_DATA(inode));
 }
 
+static const struct file_operations ft1000_proc_fops = {
+	.open		= ft1000_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
 static int ft1000NotifyProc(struct notifier_block *this, unsigned long event,
 				void *ptr)
 {
@@ -176,8 +172,8 @@ static int ft1000NotifyProc(struct notifier_block *this, unsigned long event,
 	switch (event) {
 	case NETDEV_CHANGENAME:
 		remove_proc_entry(info->netdevname, info->ft1000_proc_dir);
-		create_proc_read_entry(dev->name, 0644, info->ft1000_proc_dir,
-					   ft1000ReadProc, dev);
+		proc_create_data(dev->name, 0644, info->ft1000_proc_dir,
+				 &ft1000_proc_fops, dev);
 		snprintf(info->netdevname, IFNAMSIZ, "%s", dev->name);
 		break;
 	}
@@ -195,8 +191,10 @@ void ft1000InitProc(struct net_device *dev)
 	info = netdev_priv(dev);
 
 	info->ft1000_proc_dir = proc_mkdir(FT1000_PROC, init_net.proc_net);
-	create_proc_read_entry(dev->name, 0644, info->ft1000_proc_dir,
-				   ft1000ReadProc, dev);
+
+	proc_create_data(dev->name, 0644, info->ft1000_proc_dir,
+			 &ft1000_proc_fops, dev);
+
 	snprintf(info->netdevname, IFNAMSIZ, "%s", dev->name);
 	register_netdevice_notifier(&ft1000_netdev_notifier);
 }
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
index 297389e8c608..3251d2e073b5 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
@@ -55,7 +55,7 @@ int numofmsgbuf = 0;
 //
 // Table of entry-point routines for char device
 //
-static struct file_operations ft1000fops =
+static const struct file_operations ft1000fops =
 {
 	.unlocked_ioctl	= ft1000_ioctl,
 	.poll		= ft1000_poll_dev,
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_proc.c b/drivers/staging/ft1000/ft1000-usb/ft1000_proc.c
index b99640637fe0..eca6f0292b4b 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_proc.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_proc.c
@@ -22,6 +22,7 @@
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 #include <linux/netdevice.h>
 
 
@@ -30,22 +31,17 @@
 #define FT1000_PROC_DIR "ft1000"
 
 
-#define PUTM_TO_PAGE(len,page,args...) \
-	len += snprintf(page+len, PAGE_SIZE - len, args)
+#define seq_putx(m, message, size, var) \
+	seq_printf(m, message);	\
+	for(i = 0; i < (size - 1); i++) \
+		seq_printf(m, "%02x:", var[i]); \
+	seq_printf(m, "%02x\n", var[i])
 
-#define PUTX_TO_PAGE(len,page,message,size,var) \
-	len += snprintf(page+len, PAGE_SIZE - len, message); \
-	for (i = 0; i < (size - 1); i++) {\
-		len += snprintf(page+len, PAGE_SIZE - len, "%02x:", var[i]); \
-	} \
-	len += snprintf(page+len, PAGE_SIZE - len, "%02x\n", var[i])
-
-#define PUTD_TO_PAGE(len,page,message,size,var) \
-	len += snprintf(page+len, PAGE_SIZE - len, message); \
-	for (i = 0; i < (size - 1); i++) {\
-		len += snprintf(page+len, PAGE_SIZE - len, "%d.", var[i]); \
-	} \
-	len += snprintf(page+len, PAGE_SIZE - len, "%d\n", var[i])
+#define seq_putd(m, message, size, var) \
+	seq_printf(m, message); \
+	for(i = 0; i < (size - 1); i++) \
+		seq_printf(m, "%d.", var[i]); \
+	seq_printf(m, "%d\n", var[i])
 
 
 #define FTNET_PROC init_net.proc_net
@@ -55,19 +51,9 @@ int ft1000_read_dpram16 (struct ft1000_usb *ft1000dev, u16 indx,
 			 u8 *buffer, u8 highlow);
 
 
-static int
-ft1000ReadProc(char *page, char **start, off_t off, int count, int *eof,
-		void *data)
+static int ft1000ReadProc(struct seq_file *m, void *v)
 {
-	struct net_device *dev;
-	int len;
-	int i;
-	unsigned short ledStat;
-	unsigned short conStat;
-
-	struct ft1000_info *info;
-
-	char *status[] = { 
+	static const char *status[] = { 
 		"Idle (Disconnect)", 
 		"Searching",
 		"Active (Connected)",
@@ -77,22 +63,18 @@ ft1000ReadProc(char *page, char **start, off_t off, int count, int *eof,
 		"",
 		"",
 	};
+	static const char *signal[] = { "", "*", "**", "***", "****" };
 
-	char *signal[] = { "", "*", "**", "***", "****" };
+	struct net_device *dev = m->private;
+	struct ft1000_info *info = netdev_priv(dev);
+	int i;
+	unsigned short ledStat;
+	unsigned short conStat;
 	int strength;
 	int quality;
 	struct timeval tv;
 	time_t delta;
 
-	dev = (struct net_device *) data;
-	info = netdev_priv(dev);
-
-	if (off > 0) {
-		*eof = 1;
-		return 0;
-	}
-
-
 	if (info->ProgConStat != 0xFF) {
 		ft1000_read_dpram16(info->priv, FT1000_MAG_DSP_LED,
 			   (u8 *)&ledStat, FT1000_MAG_DSP_LED_INDX);
@@ -144,36 +126,43 @@ ft1000ReadProc(char *page, char **start, off_t off, int count, int *eof,
 		quality = 0;
 	}
 
-	len = 0;
-	PUTM_TO_PAGE(len, page, "Connection Time: %02ld:%02ld:%02ld\n",
+	seq_printf(m, "Connection Time: %02ld:%02ld:%02ld\n",
       		((delta / 3600) % 24), ((delta / 60) % 60), (delta % 60));
-	PUTM_TO_PAGE(len, page, "Connection Time[s]: %ld\n", delta);
-	PUTM_TO_PAGE(len, page, "Asic ID: %s\n",
-      	(info->AsicID) ==
-      	ELECTRABUZZ_ID ? "ELECTRABUZZ ASIC" : "MAGNEMITE ASIC");
-	PUTX_TO_PAGE(len, page, "SKU: ", SKUSZ, info->Sku);
-	PUTX_TO_PAGE(len, page, "EUI64: ", EUISZ, info->eui64);
-	PUTD_TO_PAGE(len, page, "DSP version number: ", DSPVERSZ, info->DspVer);
-	PUTX_TO_PAGE(len, page, "Hardware Serial Number: ", HWSERNUMSZ,
-      		info->HwSerNum);
-	PUTX_TO_PAGE(len, page, "Caliberation Version: ", CALVERSZ,
-      		info->RfCalVer);
-	PUTD_TO_PAGE(len, page, "Caliberation Date: ", CALDATESZ,
-		info->RfCalDate);
-	PUTM_TO_PAGE(len, page, "Media State: %s\n",
-      		(info->mediastate) ? "link" : "no link");
-	PUTM_TO_PAGE(len, page, "Connection Status: %s\n",
-      		status[((info->ConStat) & 0x7)]);
-	PUTM_TO_PAGE(len, page, "RX packets: %ld\n", info->stats.rx_packets);
-	PUTM_TO_PAGE(len, page, "TX packets: %ld\n", info->stats.tx_packets);
-	PUTM_TO_PAGE(len, page, "RX bytes: %ld\n", info->stats.rx_bytes);
-	PUTM_TO_PAGE(len, page, "TX bytes: %ld\n", info->stats.tx_bytes);
-	PUTM_TO_PAGE(len, page, "Signal Strength: %s\n", signal[strength]);
-	PUTM_TO_PAGE(len, page, "Signal Quality: %s\n", signal[quality]);
-
-	return len;
+	seq_printf(m, "Connection Time[s]: %ld\n", delta);
+	seq_printf(m, "Asic ID: %s\n",
+      	(info->AsicID) == ELECTRABUZZ_ID ? "ELECTRABUZZ ASIC" : "MAGNEMITE ASIC");
+	seq_putx(m, "SKU: ", SKUSZ, info->Sku);
+	seq_putx(m, "EUI64: ", EUISZ, info->eui64);
+	seq_putd(m, "DSP version number: ", DSPVERSZ, info->DspVer);
+	seq_putx(m, "Hardware Serial Number: ", HWSERNUMSZ, info->HwSerNum);
+	seq_putx(m, "Caliberation Version: ", CALVERSZ, info->RfCalVer);
+	seq_putd(m, "Caliberation Date: ", CALDATESZ, info->RfCalDate);
+	seq_printf(m, "Media State: %s\n", (info->mediastate) ? "link" : "no link");
+	seq_printf(m, "Connection Status: %s\n", status[info->ConStat & 0x7]);
+	seq_printf(m, "RX packets: %ld\n", info->stats.rx_packets);
+	seq_printf(m, "TX packets: %ld\n", info->stats.tx_packets);
+	seq_printf(m, "RX bytes: %ld\n", info->stats.rx_bytes);
+	seq_printf(m, "TX bytes: %ld\n", info->stats.tx_bytes);
+	seq_printf(m, "Signal Strength: %s\n", signal[strength]);
+	seq_printf(m, "Signal Quality: %s\n", signal[quality]);
+	return 0;
+}
+
+/*
+ * seq_file wrappers for procfile show routines.
+ */
+static int ft1000_proc_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ft1000ReadProc, PDE_DATA(inode));
 }
 
+static const struct file_operations ft1000_proc_fops = {
+	.open		= ft1000_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
 static int
 ft1000NotifyProc(struct notifier_block *this, unsigned long event, void *ptr)
 {
@@ -186,9 +175,9 @@ ft1000NotifyProc(struct notifier_block *this, unsigned long event, void *ptr)
 	switch (event) {
 	case NETDEV_CHANGENAME:
 		remove_proc_entry(info->netdevname, info->ft1000_proc_dir);
-		ft1000_proc_file = create_proc_read_entry(dev->name, 0644,
-					info->ft1000_proc_dir,
-					ft1000ReadProc, dev);
+		ft1000_proc_file =
+			proc_create_data(dev->name, 0644, info->ft1000_proc_dir,
+					 &ft1000_proc_fops, dev);
 		snprintf(info->netdevname, IFNAMSIZ, "%s", dev->name);
 		break;
 	}
@@ -217,10 +206,10 @@ int ft1000_init_proc(struct net_device *dev)
 	}
 
 	ft1000_proc_file =
-		create_proc_read_entry(dev->name, 0644,
-			info->ft1000_proc_dir, ft1000ReadProc, dev);
+		proc_create_data(dev->name, 0644, info->ft1000_proc_dir,
+				 &ft1000_proc_fops, dev);
 
-	if (ft1000_proc_file == NULL) {
+	if (!ft1000_proc_file) {
 		printk(KERN_WARNING "Unable to create /proc entry.\n");
 		goto fail_entry;
 	}
diff --git a/drivers/staging/gdm72xx/netlink_k.c b/drivers/staging/gdm72xx/netlink_k.c
index 8a92605adbff..af7f1c1d0b5c 100644
--- a/drivers/staging/gdm72xx/netlink_k.c
+++ b/drivers/staging/gdm72xx/netlink_k.c
@@ -15,7 +15,7 @@
 
 #include <linux/module.h>
 #include <linux/etherdevice.h>
-#include <linux/netlink.h>
+#include <net/netlink.h>
 #include <asm/byteorder.h>
 #include <net/sock.h>
 #include "netlink_k.h"
@@ -26,12 +26,12 @@
 
 #define ND_MAX_GROUP			30
 #define ND_IFINDEX_LEN			sizeof(int)
-#define ND_NLMSG_SPACE(len)		(NLMSG_SPACE(len) + ND_IFINDEX_LEN)
+#define ND_NLMSG_SPACE(len)		(nlmsg_total_size(len) + ND_IFINDEX_LEN)
 #define ND_NLMSG_DATA(nlh) \
-	((void *)((char *)NLMSG_DATA(nlh) + ND_IFINDEX_LEN))
+	((void *)((char *)nlmsg_data(nlh) + ND_IFINDEX_LEN))
 #define ND_NLMSG_S_LEN(len)		(len+ND_IFINDEX_LEN)
 #define ND_NLMSG_R_LEN(nlh)		(nlh->nlmsg_len-ND_IFINDEX_LEN)
-#define ND_NLMSG_IFIDX(nlh)		NLMSG_DATA(nlh)
+#define ND_NLMSG_IFIDX(nlh)		nlmsg_data(nlh)
 #define ND_MAX_MSG_LEN			8096
 
 #if defined(DEFINE_MUTEX)
@@ -52,7 +52,7 @@ static void netlink_rcv_cb(struct sk_buff *skb)
 	void *msg;
 	int ifindex;
 
-	if (skb->len >= NLMSG_SPACE(0)) {
+	if (skb->len >= NLMSG_HDRLEN) {
 		nlh = (struct nlmsghdr *)skb->data;
 
 		if (skb->len < nlh->nlmsg_len ||
@@ -125,7 +125,7 @@ int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len)
 		return -EINVAL;
 	}
 
-	skb = alloc_skb(NLMSG_SPACE(len), GFP_ATOMIC);
+	skb = nlmsg_new(len, GFP_ATOMIC);
 	if (!skb) {
 		pr_err("netlink_broadcast ret=%d\n", ret);
 		return -ENOMEM;
diff --git a/drivers/staging/iio/accel/Kconfig b/drivers/staging/iio/accel/Kconfig
index e2e786dc9c7b..ad45dfbdf417 100644
--- a/drivers/staging/iio/accel/Kconfig
+++ b/drivers/staging/iio/accel/Kconfig
@@ -61,7 +61,7 @@ config LIS3L02DQ
 	depends on SPI
 	select IIO_TRIGGER if IIO_BUFFER
 	depends on !IIO_BUFFER || IIO_KFIFO_BUF
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  Say yes here to build SPI support for the ST microelectronics
 	  accelerometer. The driver supplies direct access via sysfs files
diff --git a/drivers/staging/iio/adc/Kconfig b/drivers/staging/iio/adc/Kconfig
index d990829008ff..cabc7a367db5 100644
--- a/drivers/staging/iio/adc/Kconfig
+++ b/drivers/staging/iio/adc/Kconfig
@@ -73,7 +73,7 @@ config AD7780
 config AD7816
 	tristate "Analog Devices AD7816/7/8 temperature sensor and ADC driver"
 	depends on SPI
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  Say yes here to build support for Analog Devices AD7816/7/8
 	  temperature sensors and ADC.
diff --git a/drivers/staging/iio/adc/mxs-lradc.c b/drivers/staging/iio/adc/mxs-lradc.c
index 6044e1717b20..2856b8fd44ad 100644
--- a/drivers/staging/iio/adc/mxs-lradc.c
+++ b/drivers/staging/iio/adc/mxs-lradc.c
@@ -36,9 +36,6 @@
 #include <linux/delay.h>
 #include <linux/input.h>
 
-#include <mach/mxs.h>
-#include <mach/common.h>
-
 #include <linux/iio/iio.h>
 #include <linux/iio/buffer.h>
 #include <linux/iio/trigger.h>
diff --git a/drivers/staging/iio/addac/Kconfig b/drivers/staging/iio/addac/Kconfig
index 698a8970b372..e6795e0bed1d 100644
--- a/drivers/staging/iio/addac/Kconfig
+++ b/drivers/staging/iio/addac/Kconfig
@@ -5,7 +5,7 @@ menu "Analog digital bi-direction converters"
 
 config ADT7316
 	tristate "Analog Devices ADT7316/7/8 ADT7516/7/9 temperature sensor, ADC and DAC driver"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  Say yes here to build support for Analog Devices ADT7316, ADT7317, ADT7318
 	  and ADT7516, ADT7517, ADT7519 temperature sensors, ADC and DAC.
diff --git a/drivers/staging/iio/resolver/Kconfig b/drivers/staging/iio/resolver/Kconfig
index 49f69ef986fc..ce360f163216 100644
--- a/drivers/staging/iio/resolver/Kconfig
+++ b/drivers/staging/iio/resolver/Kconfig
@@ -13,7 +13,7 @@ config AD2S90
 config AD2S1200
 	tristate "Analog Devices ad2s1200/ad2s1205 driver"
 	depends on SPI
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  Say yes here to build support for Analog Devices spi resolver
 	  to digital converters, ad2s1200 and ad2s1205, provides direct access
@@ -22,7 +22,7 @@ config AD2S1200
 config AD2S1210
 	tristate "Analog Devices ad2s1210 driver"
 	depends on SPI
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  Say yes here to build support for Analog Devices spi resolver
 	  to digital converters, ad2s1210, provides direct access via sysfs.
diff --git a/drivers/staging/iio/trigger/Kconfig b/drivers/staging/iio/trigger/Kconfig
index d44d3ad26fa5..1a051da62505 100644
--- a/drivers/staging/iio/trigger/Kconfig
+++ b/drivers/staging/iio/trigger/Kconfig
@@ -14,7 +14,7 @@ config IIO_PERIODIC_RTC_TRIGGER
 
 config IIO_GPIO_TRIGGER
 	tristate "GPIO trigger"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  Provides support for using GPIO pins as IIO triggers.
 
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-common.c b/drivers/staging/imx-drm/ipu-v3/ipu-common.c
index 055b99db50ff..0127601c26c7 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-common.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-common.c
@@ -16,6 +16,7 @@
 #include <linux/export.h>
 #include <linux/types.h>
 #include <linux/init.h>
+#include <linux/reset.h>
 #include <linux/platform_device.h>
 #include <linux/err.h>
 #include <linux/spinlock.h>
@@ -25,8 +26,8 @@
 #include <linux/clk.h>
 #include <linux/list.h>
 #include <linux/irq.h>
+#include <linux/irqchip/chained_irq.h>
 #include <linux/of_device.h>
-#include <asm/mach/irq.h>
 
 #include "imx-ipu-v3.h"
 #include "ipu-prv.h"
@@ -661,7 +662,7 @@ int ipu_idmac_disable_channel(struct ipuv3_channel *channel)
 }
 EXPORT_SYMBOL_GPL(ipu_idmac_disable_channel);
 
-static int ipu_reset(struct ipu_soc *ipu)
+static int ipu_memory_reset(struct ipu_soc *ipu)
 {
 	unsigned long timeout;
 
@@ -1105,7 +1106,12 @@ static int ipu_probe(struct platform_device *pdev)
 	if (ret)
 		goto out_failed_irq;
 
-	ret = ipu_reset(ipu);
+	ret = device_reset(&pdev->dev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to reset: %d\n", ret);
+		goto out_failed_reset;
+	}
+	ret = ipu_memory_reset(ipu);
 	if (ret)
 		goto out_failed_reset;
 
@@ -1131,8 +1137,8 @@ static int ipu_probe(struct platform_device *pdev)
 failed_add_clients:
 	ipu_submodules_exit(ipu);
 failed_submodules_init:
-	ipu_irq_exit(ipu);
 out_failed_reset:
+	ipu_irq_exit(ipu);
 out_failed_irq:
 	clk_disable_unprepare(ipu->clk);
 failed_clk_get:
diff --git a/drivers/staging/keucr/scsiglue.c b/drivers/staging/keucr/scsiglue.c
index 083b20e6253e..48e1005349da 100644
--- a/drivers/staging/keucr/scsiglue.c
+++ b/drivers/staging/keucr/scsiglue.c
@@ -229,26 +229,18 @@ void usb_stor_report_bus_reset(struct us_data *us)
 
 /* we use this macro to help us write into the buffer */
 #undef SPRINTF
-#define SPRINTF(args...) \
-	do { \
-		if (pos < buffer+length) \
-			pos += sprintf(pos, ## args); \
-	} while (0)
+#define SPRINTF(args...) seq_printf(m, ##args)
 
-/*
- * proc_info()
- */
-static int proc_info(struct Scsi_Host *host, char *buffer, char **start,
-					off_t offset, int length, int inout)
+static int write_info(struct Scsi_Host *host, char *buffer, int length)
+{
+	return length;
+}
+
+static int show_info(struct seq_file *m, struct Scsi_Host *host)
 {
 	struct us_data *us = host_to_us(host);
-	char *pos = buffer;
 	const char *string;
 
-	/* pr_info("scsiglue --- proc_info\n"); */
-	if (inout)
-		return length;
-
 	/* print the controller name */
 	SPRINTF("   Host scsi%d: usb-storage\n", host->host_no);
 
@@ -278,29 +270,17 @@ static int proc_info(struct Scsi_Host *host, char *buffer, char **start,
 	SPRINTF("    Transport: %s\n", us->transport_name);
 
 	/* show the device flags */
-	if (pos < buffer + length) {
-		pos += sprintf(pos, "       Quirks:");
+	SPRINTF("       Quirks:");
 
 #define US_FLAG(name, value) \
 	do { \
 		if (us->fflags & value) \
-			pos += sprintf(pos, " " #name); \
+			SPRINTF(" " #name); \
 	} while (0);
 US_DO_ALL_FLAGS
 #undef US_FLAG
-
-		*(pos++) = '\n';
-	}
-
-	/* Calculate start of next buffer, and return value. */
-	*start = buffer + offset;
-
-	if ((pos - buffer) < offset)
-		return 0;
-	else if ((pos - buffer - offset) < length)
-		return pos - buffer - offset;
-	else
-		return length;
+	seq_putc(m, '\n');
+	return 0;
 }
 
 /***********************************************************************
@@ -351,7 +331,8 @@ struct scsi_host_template usb_stor_host_template = {
 	/* basic userland interface stuff */
 	.name =				"eucr-storage",
 	.proc_name =			"eucr-storage",
-	.proc_info =			proc_info,
+	.write_info =			write_info,
+	.show_info =			show_info,
 	.info =				host_info,
 
 	/* command interface -- queued only */
diff --git a/drivers/staging/media/as102/Makefile b/drivers/staging/media/as102/Makefile
index d8dfb757f1e2..8916d8a909bc 100644
--- a/drivers/staging/media/as102/Makefile
+++ b/drivers/staging/media/as102/Makefile
@@ -3,4 +3,4 @@ dvb-as102-objs := as102_drv.o as102_fw.o as10x_cmd.o as10x_cmd_stream.o \
 
 obj-$(CONFIG_DVB_AS102) += dvb-as102.o
 
-EXTRA_CFLAGS += -Idrivers/media/dvb-core
+ccflags-y += -Idrivers/media/dvb-core
diff --git a/drivers/staging/media/davinci_vpfe/dm365_ipipe.c b/drivers/staging/media/davinci_vpfe/dm365_ipipe.c
index 92853539cc0a..05673ed45ce4 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_ipipe.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_ipipe.c
@@ -1859,5 +1859,5 @@ void vpfe_ipipe_cleanup(struct vpfe_ipipe_device *ipipe,
 	iounmap(ipipe->isp5_base_addr);
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 4);
 	if (res)
-		release_mem_region(res->start, res->end - res->start + 1);
+		release_mem_region(res->start, resource_size(res));
 }
diff --git a/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c b/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c
index c8cae51d3d00..b2f4ef84f3db 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c
@@ -1065,7 +1065,6 @@ vpfe_ipipeif_cleanup(struct vpfe_ipipeif_device *ipipeif,
 	iounmap(ipipeif->ipipeif_base_addr);
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 3);
 	if (res)
-		release_mem_region(res->start,
-					res->end - res->start + 1);
+		release_mem_region(res->start, resource_size(res));
 
 }
diff --git a/drivers/staging/media/davinci_vpfe/dm365_isif.c b/drivers/staging/media/davinci_vpfe/dm365_isif.c
index 6d4a93c556e4..5829360f74c9 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_isif.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_isif.c
@@ -1953,7 +1953,7 @@ static void isif_remove(struct vpfe_isif_device *isif,
 		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
 		if (res)
 			release_mem_region(res->start,
-					   res->end - res->start + 1);
+					   resource_size(res));
 		i++;
 	}
 }
@@ -2003,7 +2003,7 @@ int vpfe_isif_init(struct vpfe_isif_device *isif, struct platform_device *pdev)
 			status = -ENOENT;
 			goto fail_nobase_res;
 		}
-		res_len = res->end - res->start + 1;
+		res_len = resource_size(res);
 		res = request_mem_region(res->start, res_len, res->name);
 		if (!res) {
 			status = -EBUSY;
diff --git a/drivers/staging/media/davinci_vpfe/dm365_resizer.c b/drivers/staging/media/davinci_vpfe/dm365_resizer.c
index 9cb0262b9b33..126f84c4cb64 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_resizer.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_resizer.c
@@ -1995,5 +1995,5 @@ vpfe_resizer_cleanup(struct vpfe_resizer_device *vpfe_rsz,
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 5);
 	if (res)
 		release_mem_region(res->start,
-					res->end - res->start + 1);
+					resource_size(res));
 }
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
index 7b351717bcbb..b88e1ddce229 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
+++ b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
@@ -243,7 +243,7 @@ static int vpfe_enable_clock(struct vpfe_device *vpfe_dev)
 
 		vpfe_dev->clks[i] =
 				clk_get(vpfe_dev->pdev, vpfe_cfg->clocks[i]);
-		if (vpfe_dev->clks[i] == NULL) {
+		if (IS_ERR(vpfe_dev->clks[i])) {
 			v4l2_err(vpfe_dev->pdev->driver,
 				"Failed to get clock %s\n",
 				vpfe_cfg->clocks[i]);
@@ -264,7 +264,7 @@ static int vpfe_enable_clock(struct vpfe_device *vpfe_dev)
 	return 0;
 out:
 	for (i = 0; i < vpfe_cfg->num_clocks; i++)
-		if (vpfe_dev->clks[i]) {
+		if (!IS_ERR(vpfe_dev->clks[i])) {
 			clk_disable_unprepare(vpfe_dev->clks[i]);
 			clk_put(vpfe_dev->clks[i]);
 		}
@@ -719,22 +719,4 @@ static struct platform_driver vpfe_driver = {
 	.remove = vpfe_remove,
 };
 
-/**
- * vpfe_init : This function registers device driver
- */
-static __init int vpfe_init(void)
-{
-	/* Register driver to the kernel */
-	return platform_driver_register(&vpfe_driver);
-}
-
-/**
- * vpfe_cleanup : This function un-registers device driver
- */
-static void vpfe_cleanup(void)
-{
-	platform_driver_unregister(&vpfe_driver);
-}
-
-module_init(vpfe_init);
-module_exit(vpfe_cleanup);
+module_platform_driver(vpfe_driver);
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_video.c b/drivers/staging/media/davinci_vpfe/vpfe_video.c
index c91d356c13df..ba913f1d955b 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_video.c
+++ b/drivers/staging/media/davinci_vpfe/vpfe_video.c
@@ -924,7 +924,7 @@ static int vpfe_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
  *
  * Return 0 on success, error code otherwise
  */
-static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
+static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id std_id)
 {
 	struct vpfe_video_device *video = video_drvdata(file);
 	struct vpfe_device *vpfe_dev = video->vpfe_dev;
@@ -945,13 +945,13 @@ static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
 		goto unlock_out;
 	}
 	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
-					 core, s_std, *std_id);
+					 core, s_std, std_id);
 	if (ret < 0) {
 		v4l2_err(&vpfe_dev->v4l2_dev, "Failed to set standard\n");
 		video->stdid = V4L2_STD_UNKNOWN;
 		goto unlock_out;
 	}
-	video->stdid = *std_id;
+	video->stdid = std_id;
 unlock_out:
 	mutex_unlock(&video->lock);
 	return ret;
@@ -1016,12 +1016,12 @@ vpfe_query_dv_timings(struct file *file, void *fh,
 }
 
 /*
- * vpfe_s_dv_timings() - set dv_preset on external subdev
+ * vpfe_s_dv_timings() - set dv_timings on external subdev
  * @file: file pointer
  * @priv: void pointer
  * @timings: pointer to v4l2_dv_timings structure
  *
- * set dv_timings pointed by preset on external subdev through
+ * set dv_timings pointed by timings on external subdev through
  * v4l2_device_call_until_err, this configures amplifier also
  *
  * Return 0 on success, error code otherwise
@@ -1042,12 +1042,12 @@ vpfe_s_dv_timings(struct file *file, void *fh,
 }
 
 /*
- * vpfe_g_dv_timings() - get dv_preset which is set on external subdev
+ * vpfe_g_dv_timings() - get dv_timings which is set on external subdev
  * @file: file pointer
  * @priv: void pointer
  * @timings: pointer to v4l2_dv_timings structure
  *
- * get dv_preset which is set on external subdev through
+ * get dv_timings which is set on external subdev through
  * v4l2_subdev_call
  *
  * Return 0 on success, error code otherwise
@@ -1423,7 +1423,7 @@ static int vpfe_dqbuf(struct file *file, void *priv,
 }
 
 /*
- * vpfe_streamon() - get dv_preset which is set on external subdev
+ * vpfe_streamon() - start streaming
  * @file: file pointer
  * @priv: void pointer
  * @buf_type: enum v4l2_buf_type
@@ -1472,7 +1472,7 @@ static int vpfe_streamon(struct file *file, void *priv,
 }
 
 /*
- * vpfe_streamoff() - get dv_preset which is set on external subdev
+ * vpfe_streamoff() - stop streaming
  * @file: file pointer
  * @priv: void pointer
  * @buf_type: enum v4l2_buf_type
diff --git a/drivers/staging/media/dt3155v4l/dt3155v4l.c b/drivers/staging/media/dt3155v4l/dt3155v4l.c
index e33b7f55d84e..c32e0acde4f4 100644
--- a/drivers/staging/media/dt3155v4l/dt3155v4l.c
+++ b/drivers/staging/media/dt3155v4l/dt3155v4l.c
@@ -26,6 +26,7 @@
 #include <linux/slab.h>
 #include <media/v4l2-dev.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-common.h>
 #include <media/videobuf2-dma-contig.h>
 
 #include "dt3155v4l.h"
@@ -341,7 +342,7 @@ dt3155_irq_handler_even(int irq, void *dev_id)
 
 	spin_lock(&ipd->lock);
 	if (ipd->curr_buf) {
-		do_gettimeofday(&ipd->curr_buf->v4l2_buf.timestamp);
+		v4l2_get_timestamp(&ipd->curr_buf->v4l2_buf.timestamp);
 		ipd->curr_buf->v4l2_buf.sequence = (ipd->field_count) >> 1;
 		vb2_buffer_done(ipd->curr_buf, VB2_BUF_STATE_DONE);
 	}
@@ -390,6 +391,7 @@ dt3155_open(struct file *filp)
 			goto err_alloc_queue;
 		}
 		pd->q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		pd->q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 		pd->q->io_modes = VB2_READ | VB2_MMAP;
 		pd->q->ops = &q_ops;
 		pd->q->mem_ops = &vb2_dma_contig_memops;
@@ -398,7 +400,7 @@ dt3155_open(struct file *filp)
 		pd->field_count = 0;
 		ret = vb2_queue_init(pd->q);
 		if (ret < 0)
-			return ret;
+			goto err_request_irq;
 		INIT_LIST_HEAD(&pd->dmaq);
 		spin_lock_init(&pd->lock);
 		/* disable all irqs, clear all irq flags */
@@ -410,6 +412,7 @@ dt3155_open(struct file *filp)
 			goto err_request_irq;
 	}
 	pd->users++;
+	mutex_unlock(&pd->mux);
 	return 0; /* success */
 err_request_irq:
 	kfree(pd->q);
@@ -612,9 +615,9 @@ dt3155_ioc_g_std(struct file *filp, void *p, v4l2_std_id *norm)
 }
 
 static int
-dt3155_ioc_s_std(struct file *filp, void *p, v4l2_std_id *norm)
+dt3155_ioc_s_std(struct file *filp, void *p, v4l2_std_id norm)
 {
-	if (*norm & DT3155_CURRENT_NORM)
+	if (norm & DT3155_CURRENT_NORM)
 		return 0;
 	return -EINVAL;
 }
diff --git a/drivers/staging/media/go7007/Kconfig b/drivers/staging/media/go7007/Kconfig
index 7dfb2815b9ec..04bd0fba7b1f 100644
--- a/drivers/staging/media/go7007/Kconfig
+++ b/drivers/staging/media/go7007/Kconfig
@@ -1,20 +1,25 @@
 config VIDEO_GO7007
 	tristate "WIS GO7007 MPEG encoder support"
-	depends on VIDEO_DEV && PCI && I2C
-	depends on SND
-	select VIDEOBUF_DMA_SG
-	depends on RC_CORE
+	depends on VIDEO_DEV && I2C
+	depends on SND && USB
+	select VIDEOBUF2_VMALLOC
 	select VIDEO_TUNER
-	select VIDEO_TVEEPROM
+	select CYPRESS_FIRMWARE
 	select SND_PCM
-	select CRC32
+	select VIDEO_SONY_BTF_MPX if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_SAA711X if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_TW2804 if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_TW9903 if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_TW9906 if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_OV7640 if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_UDA1342 if MEDIA_SUBDRV_AUTOSELECT
 	default N
 	---help---
 	  This is a video4linux driver for the WIS GO7007 MPEG
 	  encoder chip.
 
 	  To compile this driver as a module, choose M here: the
-	  module will be called go7007
+	  module will be called go7007.
 
 config VIDEO_GO7007_USB
 	tristate "WIS GO7007 USB support"
@@ -25,85 +30,25 @@ config VIDEO_GO7007_USB
 	  encoder chip over USB.
 
 	  To compile this driver as a module, choose M here: the
-	  module will be called go7007-usb
+	  module will be called go7007-usb.
 
-config VIDEO_GO7007_USB_S2250_BOARD
-	tristate "Sensoray 2250/2251 support"
-	depends on VIDEO_GO7007_USB && DVB_USB
-	default N
-	---help---
-	  This is a video4linux driver for the Sensoray 2250/2251 device.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called s2250
-
-config VIDEO_GO7007_OV7640
-	tristate "OV7640 subdev support"
-	depends on VIDEO_GO7007
-	default N
-	---help---
-	  This is a video4linux driver for the OV7640 sub-device.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called wis-ov7640
-
-config VIDEO_GO7007_SAA7113
-	tristate "SAA7113 subdev support"
-	depends on VIDEO_GO7007
-	default N
-	---help---
-	  This is a video4linux driver for the SAA7113 sub-device.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called wis-saa7113
-
-config VIDEO_GO7007_SAA7115
-	tristate "SAA7115 subdev support"
-	depends on VIDEO_GO7007
-	default N
-	---help---
-	  This is a video4linux driver for the SAA7115 sub-device.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called wis-saa7115
-
-config VIDEO_GO7007_TW9903
-	tristate "TW9903 subdev support"
-	depends on VIDEO_GO7007
-	default N
-	---help---
-	  This is a video4linux driver for the TW9903 sub-device.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called wis-tw9903
-
-config VIDEO_GO7007_UDA1342
-	tristate "UDA1342 subdev support"
-	depends on VIDEO_GO7007
-	default N
-	---help---
-	  This is a video4linux driver for the UDA1342 sub-device.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called wis-uda1342
-
-config VIDEO_GO7007_SONY_TUNER
-	tristate "Sony tuner subdev support"
+config VIDEO_GO7007_LOADER
+	tristate "WIS GO7007 Loader support"
 	depends on VIDEO_GO7007
-	default N
+	default y
 	---help---
-	  This is a video4linux driver for the Sony Tuner sub-device.
+	  This is a go7007 firmware loader driver for the WIS GO7007
+	  MPEG encoder chip over USB.
 
 	  To compile this driver as a module, choose M here: the
-	  module will be called wis-sony-tuner
+	  module will be called go7007-loader.
 
-config VIDEO_GO7007_TW2804
-	tristate "TW2804 subdev support"
-	depends on VIDEO_GO7007
+config VIDEO_GO7007_USB_S2250_BOARD
+	tristate "Sensoray 2250/2251 support"
+	depends on VIDEO_GO7007_USB && USB
 	default N
 	---help---
-	  This is a video4linux driver for the TW2804 sub-device.
+	  This is a video4linux driver for the Sensoray 2250/2251 device.
 
 	  To compile this driver as a module, choose M here: the
-	  module will be called wis-tw2804
-
+	  module will be called s2250.
diff --git a/drivers/staging/media/go7007/Makefile b/drivers/staging/media/go7007/Makefile
index 3fdbef5306a0..9c6ad4a263ec 100644
--- a/drivers/staging/media/go7007/Makefile
+++ b/drivers/staging/media/go7007/Makefile
@@ -1,18 +1,7 @@
-#obj-m += go7007.o go7007-usb.o snd-go7007.o wis-saa7115.o wis-tw9903.o \
-		wis-uda1342.o wis-sony-tuner.o wis-saa7113.o wis-ov7640.o \
-		wis-tw2804.o
-
-
 obj-$(CONFIG_VIDEO_GO7007) += go7007.o
 obj-$(CONFIG_VIDEO_GO7007_USB) += go7007-usb.o
-obj-$(CONFIG_VIDEO_GO7007_USB_S2250_BOARD) += s2250.o s2250-loader.o
-obj-$(CONFIG_VIDEO_GO7007_SAA7113) += wis-saa7113.o
-obj-$(CONFIG_VIDEO_GO7007_OV7640) += wis-ov7640.o
-obj-$(CONFIG_VIDEO_GO7007_SAA7115) += wis-saa7115.o
-obj-$(CONFIG_VIDEO_GO7007_TW9903) += wis-tw9903.o
-obj-$(CONFIG_VIDEO_GO7007_UDA1342) += wis-uda1342.o
-obj-$(CONFIG_VIDEO_GO7007_SONY_TUNER) += wis-sony-tuner.o
-obj-$(CONFIG_VIDEO_GO7007_TW2804) += wis-tw2804.o
+obj-$(CONFIG_VIDEO_GO7007_LOADER) += go7007-loader.o
+obj-$(CONFIG_VIDEO_GO7007_USB_S2250_BOARD) += s2250.o
 
 go7007-y := go7007-v4l2.o go7007-driver.o go7007-i2c.o go7007-fw.o \
 		snd-go7007.o
@@ -21,10 +10,6 @@ s2250-y := s2250-board.o
 
 # Uncomment when the saa7134 patches get into upstream
 #obj-$(CONFIG_VIDEO_SAA7134) += saa7134-go7007.o
-#ccflags-$(CONFIG_VIDEO_SAA7134:m=y) += -Idrivers/media/video/saa7134 -DSAA7134_MPEG_GO7007=3
-
-# S2250 needs cypress ezusb loader from dvb-usb
-ccflags-$(CONFIG_VIDEO_GO7007_USB_S2250_BOARD:m=y) += -Idrivers/media/usb/dvb-usb
+#ccflags-$(CONFIG_VIDEO_SAA7134:m=y) += -Idrivers/media/pci/saa7134
 
-ccflags-y += -Idrivers/media/dvb-frontends
-ccflags-y += -Idrivers/media/dvb-core
+ccflags-$(CONFIG_VIDEO_GO7007_LOADER:m=y) += -Idrivers/media/common
diff --git a/drivers/staging/media/go7007/README b/drivers/staging/media/go7007/README
index aeba1324a9c5..3af0d9062811 100644
--- a/drivers/staging/media/go7007/README
+++ b/drivers/staging/media/go7007/README
@@ -1,11 +1,137 @@
 Todo:
-	- checkpatch.pl cleanups
-	- sparse cleanups
-	- lots of little modules, should be merged together
-	  and added to the build.
-	- testing?
-	- handle churn in v4l layer.
+	- create an API for motion detection
+	- let s2250-board use i2c subdevs as well instead of hardcoding
+	  support for the i2c devices.
+	- when the driver is moved out of staging, support for saa7134-go7007
+	  should be added to the saa7134 driver. The patch for that is
+	  included below.
 
-Please send patches to Greg Kroah-Hartman <greg@linuxfoundation.org> and Cc: Ross
-Cohen <rcohen@snurgle.org> as well.
+Patch for saa7134:
 
+diff --git a/drivers/media/pci/saa7134/saa7134-cards.c b/drivers/media/pci/saa7134/saa7134-cards.c
+index dc68cf1..9a53794 100644
+--- a/drivers/media/pci/saa7134/saa7134-cards.c
++++ b/drivers/media/pci/saa7134/saa7134-cards.c
+@@ -5790,6 +5790,29 @@ struct saa7134_board saa7134_boards[] = {
+ 			.gpio = 0x6010000,
+ 		} },
+ 	},
++	[SAA7134_BOARD_WIS_VOYAGER] = {
++		.name           = "WIS Voyager or compatible",
++		.audio_clock    = 0x00200000,
++		.tuner_type	= TUNER_PHILIPS_TDA8290,
++		.radio_type     = UNSET,
++		.tuner_addr     = ADDR_UNSET,
++		.radio_addr     = ADDR_UNSET,
++		.mpeg		= SAA7134_MPEG_GO7007,
++		.inputs		= { {
++			.name = name_comp1,
++			.vmux = 0,
++			.amux = LINE2,
++		}, {
++			.name = name_tv,
++			.vmux = 3,
++			.amux = TV,
++			.tv   = 1,
++		}, {
++			.name = name_svideo,
++			.vmux = 6,
++		.amux = LINE1,
++		} },
++	},
+ 
+ };
+ 
+@@ -7037,6 +7060,12 @@ struct pci_device_id saa7134_pci_tbl[] = {
+ 		.subdevice    = 0x0911,
+ 		.driver_data  = SAA7134_BOARD_SENSORAY811_911,
+ 	}, {
++		.vendor       = PCI_VENDOR_ID_PHILIPS,
++		.device       = PCI_DEVICE_ID_PHILIPS_SAA7133,
++		.subvendor    = 0x1905, /* WIS */
++		.subdevice    = 0x7007,
++		.driver_data  = SAA7134_BOARD_WIS_VOYAGER,
++	}, {
+ 		/* --- boards without eeprom + subsystem ID --- */
+ 		.vendor       = PCI_VENDOR_ID_PHILIPS,
+ 		.device       = PCI_DEVICE_ID_PHILIPS_SAA7134,
+diff --git a/drivers/media/pci/saa7134/saa7134-core.c b/drivers/media/pci/saa7134/saa7134-core.c
+index 8fd24e7..0a849ea 100644
+--- a/drivers/media/pci/saa7134/saa7134-core.c
++++ b/drivers/media/pci/saa7134/saa7134-core.c
+@@ -156,6 +156,8 @@ static void request_module_async(struct work_struct *work){
+ 		request_module("saa7134-empress");
+ 	if (card_is_dvb(dev))
+ 		request_module("saa7134-dvb");
++	if (card_is_go7007(dev))
++		request_module("saa7134-go7007");
+ 	if (alsa) {
+ 		if (dev->pci->device != PCI_DEVICE_ID_PHILIPS_SAA7130)
+ 			request_module("saa7134-alsa");
+@@ -557,8 +559,12 @@ static irqreturn_t saa7134_irq(int irq, void *dev_id)
+ 			saa7134_irq_vbi_done(dev,status);
+ 
+ 		if ((report & SAA7134_IRQ_REPORT_DONE_RA2) &&
+-		    card_has_mpeg(dev))
+-			saa7134_irq_ts_done(dev,status);
++		    card_has_mpeg(dev)) {
++			if (dev->mops->irq_ts_done != NULL)
++				dev->mops->irq_ts_done(dev, status);
++			else
++				saa7134_irq_ts_done(dev, status);
++		}
+ 
+ 		if (report & SAA7134_IRQ_REPORT_GPIO16) {
+ 			switch (dev->has_remote) {
+diff --git a/drivers/media/pci/saa7134/saa7134.h b/drivers/media/pci/saa7134/saa7134.h
+index 62169dd..5fad39a 100644
+--- a/drivers/media/pci/saa7134/saa7134.h
++++ b/drivers/media/pci/saa7134/saa7134.h
+@@ -334,6 +334,7 @@ struct saa7134_card_ir {
+ #define SAA7134_BOARD_KWORLD_PC150U         189
+ #define SAA7134_BOARD_ASUSTeK_PS3_100      190
+ #define SAA7134_BOARD_HAWELL_HW_9004V1      191
++#define SAA7134_BOARD_WIS_VOYAGER           192
+ 
+ #define SAA7134_MAXBOARDS 32
+ #define SAA7134_INPUT_MAX 8
+@@ -364,6 +365,7 @@ enum saa7134_mpeg_type {
+ 	SAA7134_MPEG_UNUSED,
+ 	SAA7134_MPEG_EMPRESS,
+ 	SAA7134_MPEG_DVB,
++	SAA7134_MPEG_GO7007,
+ };
+ 
+ enum saa7134_mpeg_ts_type {
+@@ -403,6 +405,7 @@ struct saa7134_board {
+ #define card_has_radio(dev)   (NULL != saa7134_boards[dev->board].radio.name)
+ #define card_is_empress(dev)  (SAA7134_MPEG_EMPRESS == saa7134_boards[dev->board].mpeg)
+ #define card_is_dvb(dev)      (SAA7134_MPEG_DVB     == saa7134_boards[dev->board].mpeg)
++#define card_is_go7007(dev)   (SAA7134_MPEG_GO7007  == saa7134_boards[dev->board].mpeg)
+ #define card_has_mpeg(dev)    (SAA7134_MPEG_UNUSED  != saa7134_boards[dev->board].mpeg)
+ #define card(dev)             (saa7134_boards[dev->board])
+ #define card_in(dev,n)        (saa7134_boards[dev->board].inputs[n])
+@@ -535,6 +538,8 @@ struct saa7134_mpeg_ops {
+ 	int                        (*init)(struct saa7134_dev *dev);
+ 	int                        (*fini)(struct saa7134_dev *dev);
+ 	void                       (*signal_change)(struct saa7134_dev *dev);
++	void                       (*irq_ts_done)(struct saa7134_dev *dev,
++						  unsigned long status);
+ };
+ 
+ /* global device status */
+diff --git a/drivers/staging/media/go7007/Makefile b/drivers/staging/media/go7007/Makefile
+index 9c6ad4a..1b23689 100644
+--- a/drivers/staging/media/go7007/Makefile
++++ b/drivers/staging/media/go7007/Makefile
+@@ -8,8 +8,7 @@ go7007-y := go7007-v4l2.o go7007-driver.o go7007-i2c.o go7007-fw.o \
+ 
+ s2250-y := s2250-board.o
+ 
+-# Uncomment when the saa7134 patches get into upstream
+-#obj-$(CONFIG_VIDEO_SAA7134) += saa7134-go7007.o
+-#ccflags-$(CONFIG_VIDEO_SAA7134:m=y) += -Idrivers/media/pci/saa7134
++obj-$(CONFIG_VIDEO_SAA7134) += saa7134-go7007.o
++ccflags-$(CONFIG_VIDEO_SAA7134:m=y) += -Idrivers/media/pci/saa7134
+ 
+ ccflags-$(CONFIG_VIDEO_GO7007_LOADER:m=y) += -Idrivers/media/common
diff --git a/drivers/staging/media/go7007/go7007-driver.c b/drivers/staging/media/go7007/go7007-driver.c
index 66950916df23..3640df0aa0c1 100644
--- a/drivers/staging/media/go7007/go7007-driver.c
+++ b/drivers/staging/media/go7007/go7007-driver.c
@@ -35,7 +35,6 @@
 #include <media/v4l2-common.h>
 
 #include "go7007-priv.h"
-#include "wis-i2c.h"
 
 /*
  * Wait for an interrupt to be delivered from the GO7007SB and return
@@ -91,43 +90,43 @@ EXPORT_SYMBOL(go7007_read_addr);
 static int go7007_load_encoder(struct go7007 *go)
 {
 	const struct firmware *fw_entry;
-	char fw_name[] = "go7007fw.bin";
+	char fw_name[] = "go7007/go7007fw.bin";
 	void *bounce;
 	int fw_len, rv = 0;
 	u16 intr_val, intr_data;
 
-	if (request_firmware(&fw_entry, fw_name, go->dev)) {
-		v4l2_err(go, "unable to load firmware from file "
-			"\"%s\"\n", fw_name);
-		return -1;
-	}
-	if (fw_entry->size < 16 || memcmp(fw_entry->data, "WISGO7007FW", 11)) {
-		v4l2_err(go, "file \"%s\" does not appear to be "
-				"go7007 firmware\n", fw_name);
-		release_firmware(fw_entry);
-		return -1;
-	}
-	fw_len = fw_entry->size - 16;
-	bounce = kmemdup(fw_entry->data + 16, fw_len, GFP_KERNEL);
-	if (bounce == NULL) {
-		v4l2_err(go, "unable to allocate %d bytes for "
-				"firmware transfer\n", fw_len);
+	if (go->boot_fw == NULL) {
+		if (request_firmware(&fw_entry, fw_name, go->dev)) {
+			v4l2_err(go, "unable to load firmware from file \"%s\"\n", fw_name);
+			return -1;
+		}
+		if (fw_entry->size < 16 || memcmp(fw_entry->data, "WISGO7007FW", 11)) {
+			v4l2_err(go, "file \"%s\" does not appear to be go7007 firmware\n", fw_name);
+			release_firmware(fw_entry);
+			return -1;
+		}
+		fw_len = fw_entry->size - 16;
+		bounce = kmemdup(fw_entry->data + 16, fw_len, GFP_KERNEL);
+		if (bounce == NULL) {
+			v4l2_err(go, "unable to allocate %d bytes for firmware transfer\n", fw_len);
+			release_firmware(fw_entry);
+			return -1;
+		}
 		release_firmware(fw_entry);
-		return -1;
+		go->boot_fw_len = fw_len;
+		go->boot_fw = bounce;
 	}
-	release_firmware(fw_entry);
 	if (go7007_interface_reset(go) < 0 ||
-			go7007_send_firmware(go, bounce, fw_len) < 0 ||
-			go7007_read_interrupt(go, &intr_val, &intr_data) < 0 ||
+	    go7007_send_firmware(go, go->boot_fw, go->boot_fw_len) < 0 ||
+	    go7007_read_interrupt(go, &intr_val, &intr_data) < 0 ||
 			(intr_val & ~0x1) != 0x5a5a) {
 		v4l2_err(go, "error transferring firmware\n");
 		rv = -1;
 	}
-	kfree(bounce);
 	return rv;
 }
 
-MODULE_FIRMWARE("go7007fw.bin");
+MODULE_FIRMWARE("go7007/go7007fw.bin");
 
 /*
  * Boot the encoder and register the I2C adapter if requested.  Do the
@@ -167,15 +166,24 @@ static int go7007_init_encoder(struct go7007 *go)
 		go7007_write_addr(go, 0x1000, 0x0811);
 		go7007_write_addr(go, 0x1000, 0x0c11);
 	}
-	if (go->board_id == GO7007_BOARDID_MATRIX_REV) {
+	switch (go->board_id) {
+	case GO7007_BOARDID_MATRIX_REV:
 		/* Set GPIO pin 0 to be an output (audio clock control) */
 		go7007_write_addr(go, 0x3c82, 0x0001);
 		go7007_write_addr(go, 0x3c80, 0x00fe);
-	}
-	if (go->board_id == GO7007_BOARDID_ADLINK_MPG24) {
+		break;
+	case GO7007_BOARDID_ADLINK_MPG24:
 		/* set GPIO5 to be an output, currently low */
 		go7007_write_addr(go, 0x3c82, 0x0000);
 		go7007_write_addr(go, 0x3c80, 0x00df);
+		break;
+	case GO7007_BOARDID_ADS_USBAV_709:
+		/* GPIO pin 0: audio clock control */
+		/*      pin 2: TW9906 reset */
+		/*      pin 3: capture LED */
+		go7007_write_addr(go, 0x3c82, 0x000d);
+		go7007_write_addr(go, 0x3c80, 0x00f2);
+		break;
 	}
 	return 0;
 }
@@ -196,18 +204,51 @@ int go7007_reset_encoder(struct go7007 *go)
 /*
  * Attempt to instantiate an I2C client by ID, probably loading a module.
  */
-static int init_i2c_module(struct i2c_adapter *adapter, const char *type,
-			   int addr)
+static int init_i2c_module(struct i2c_adapter *adapter, const struct go_i2c *const i2c)
 {
 	struct go7007 *go = i2c_get_adapdata(adapter);
 	struct v4l2_device *v4l2_dev = &go->v4l2_dev;
-
-	if (v4l2_i2c_new_subdev(v4l2_dev, adapter, type, addr, NULL))
+	struct v4l2_subdev *sd;
+	struct i2c_board_info info;
+
+	memset(&info, 0, sizeof(info));
+	strlcpy(info.type, i2c->type, sizeof(info.type));
+	info.addr = i2c->addr;
+	info.flags = i2c->flags;
+
+	sd = v4l2_i2c_new_subdev_board(v4l2_dev, adapter, &info, NULL);
+	if (sd) {
+		if (i2c->is_video)
+			go->sd_video = sd;
+		if (i2c->is_audio)
+			go->sd_audio = sd;
 		return 0;
+	}
 
-	dev_info(&adapter->dev,
-		 "go7007: probing for module i2c:%s failed\n", type);
-	return -1;
+	printk(KERN_INFO "go7007: probing for module i2c:%s failed\n", i2c->type);
+	return -EINVAL;
+}
+
+/*
+ * Detach and unregister the encoder.  The go7007 struct won't be freed
+ * until v4l2 finishes releasing its resources and all associated fds are
+ * closed by applications.
+ */
+static void go7007_remove(struct v4l2_device *v4l2_dev)
+{
+	struct go7007 *go = container_of(v4l2_dev, struct go7007, v4l2_dev);
+
+	v4l2_device_unregister(v4l2_dev);
+	if (go->hpi_ops->release)
+		go->hpi_ops->release(go);
+	if (go->i2c_adapter_online) {
+		i2c_del_adapter(&go->i2c_adapter);
+		go->i2c_adapter_online = 0;
+	}
+
+	kfree(go->boot_fw);
+	go7007_v4l2_remove(go);
+	kfree(go);
 }
 
 /*
@@ -218,38 +259,63 @@ static int init_i2c_module(struct i2c_adapter *adapter, const char *type,
  *
  * Must NOT be called with the hw_lock held.
  */
-int go7007_register_encoder(struct go7007 *go)
+int go7007_register_encoder(struct go7007 *go, unsigned num_i2c_devs)
 {
 	int i, ret;
 
 	dev_info(go->dev, "go7007: registering new %s\n", go->name);
 
+	go->v4l2_dev.release = go7007_remove;
+	ret = v4l2_device_register(go->dev, &go->v4l2_dev);
+	if (ret < 0)
+		return ret;
+
 	mutex_lock(&go->hw_lock);
 	ret = go7007_init_encoder(go);
 	mutex_unlock(&go->hw_lock);
 	if (ret < 0)
-		return -1;
+		return ret;
 
-	/* v4l2 init must happen before i2c subdevs */
-	ret = go7007_v4l2_init(go);
+	ret = go7007_v4l2_ctrl_init(go);
 	if (ret < 0)
 		return ret;
 
 	if (!go->i2c_adapter_online &&
 			go->board_info->flags & GO7007_BOARD_USE_ONBOARD_I2C) {
-		if (go7007_i2c_init(go) < 0)
-			return -1;
+		ret = go7007_i2c_init(go);
+		if (ret < 0)
+			return ret;
 		go->i2c_adapter_online = 1;
 	}
 	if (go->i2c_adapter_online) {
-		for (i = 0; i < go->board_info->num_i2c_devs; ++i)
-			init_i2c_module(&go->i2c_adapter,
-					go->board_info->i2c_devs[i].type,
-					go->board_info->i2c_devs[i].addr);
+		if (go->board_id == GO7007_BOARDID_ADS_USBAV_709) {
+			/* Reset the TW9906 */
+			go7007_write_addr(go, 0x3c82, 0x0009);
+			msleep(50);
+			go7007_write_addr(go, 0x3c82, 0x000d);
+		}
+		for (i = 0; i < num_i2c_devs; ++i)
+			init_i2c_module(&go->i2c_adapter, &go->board_info->i2c_devs[i]);
+
+		if (go->tuner_type >= 0) {
+			struct tuner_setup setup = {
+				.addr = ADDR_UNSET,
+				.type = go->tuner_type,
+				.mode_mask = T_ANALOG_TV,
+			};
+
+			v4l2_device_call_all(&go->v4l2_dev, 0, tuner,
+				s_type_addr, &setup);
+		}
 		if (go->board_id == GO7007_BOARDID_ADLINK_MPG24)
-			i2c_clients_command(&go->i2c_adapter,
-				DECODER_SET_CHANNEL, &go->channel_number);
+			v4l2_subdev_call(go->sd_video, video, s_routing,
+					0, 0, go->channel_number + 1);
 	}
+
+	ret = go7007_v4l2_init(go);
+	if (ret < 0)
+		return ret;
+
 	if (go->board_info->flags & GO7007_BOARD_HAS_AUDIO) {
 		go->audio_enabled = 1;
 		go7007_snd_init(go);
@@ -309,48 +375,54 @@ start_error:
 /*
  * Store a byte in the current video buffer, if there is one.
  */
-static inline void store_byte(struct go7007_buffer *gobuf, u8 byte)
+static inline void store_byte(struct go7007_buffer *vb, u8 byte)
 {
-	if (gobuf != NULL && gobuf->bytesused < GO7007_BUF_SIZE) {
-		unsigned int pgidx = gobuf->offset >> PAGE_SHIFT;
-		unsigned int pgoff = gobuf->offset & ~PAGE_MASK;
+	if (vb && vb->vb.v4l2_planes[0].bytesused < GO7007_BUF_SIZE) {
+		u8 *ptr = vb2_plane_vaddr(&vb->vb, 0);
 
-		*((u8 *)page_address(gobuf->pages[pgidx]) + pgoff) = byte;
-		++gobuf->offset;
-		++gobuf->bytesused;
+		ptr[vb->vb.v4l2_planes[0].bytesused++] = byte;
 	}
 }
 
 /*
  * Deliver the last video buffer and get a new one to start writing to.
  */
-static void frame_boundary(struct go7007 *go)
+static struct go7007_buffer *frame_boundary(struct go7007 *go, struct go7007_buffer *vb)
 {
-	struct go7007_buffer *gobuf;
+	struct go7007_buffer *vb_tmp = NULL;
+	u32 *bytesused = &vb->vb.v4l2_planes[0].bytesused;
 	int i;
 
-	if (go->active_buf) {
-		if (go->active_buf->modet_active) {
-			if (go->active_buf->bytesused + 216 < GO7007_BUF_SIZE) {
+	if (vb) {
+		if (vb->modet_active) {
+			if (*bytesused + 216 < GO7007_BUF_SIZE) {
 				for (i = 0; i < 216; ++i)
-					store_byte(go->active_buf,
-							go->active_map[i]);
-				go->active_buf->bytesused -= 216;
+					store_byte(vb, go->active_map[i]);
+				*bytesused -= 216;
 			} else
-				go->active_buf->modet_active = 0;
+				vb->modet_active = 0;
 		}
-		go->active_buf->state = BUF_STATE_DONE;
-		wake_up_interruptible(&go->frame_waitq);
-		go->active_buf = NULL;
+		vb->vb.v4l2_buf.sequence = go->next_seq++;
+		v4l2_get_timestamp(&vb->vb.v4l2_buf.timestamp);
+		vb_tmp = vb;
+		spin_lock(&go->spinlock);
+		list_del(&vb->list);
+		if (list_empty(&go->vidq_active))
+			vb = NULL;
+		else
+			vb = list_first_entry(&go->vidq_active, struct go7007_buffer, list);
+		go->active_buf = vb;
+		spin_unlock(&go->spinlock);
+		vb2_buffer_done(&vb_tmp->vb, VB2_BUF_STATE_DONE);
+		return vb;
 	}
-	list_for_each_entry(gobuf, &go->stream, stream)
-		if (gobuf->state == BUF_STATE_QUEUED) {
-			gobuf->seq = go->next_seq;
-			do_gettimeofday(&gobuf->timestamp);
-			go->active_buf = gobuf;
-			break;
-		}
-	++go->next_seq;
+	spin_lock(&go->spinlock);
+	if (!list_empty(&go->vidq_active))
+		vb = go->active_buf =
+			list_first_entry(&go->vidq_active, struct go7007_buffer, list);
+	spin_unlock(&go->spinlock);
+	go->next_seq++;
+	return vb;
 }
 
 static void write_bitmap_word(struct go7007 *go)
@@ -374,30 +446,30 @@ static void write_bitmap_word(struct go7007 *go)
  */
 void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 {
-	int i, seq_start_code = -1, frame_start_code = -1;
-
-	spin_lock(&go->spinlock);
+	struct go7007_buffer *vb = go->active_buf;
+	int i, seq_start_code = -1, gop_start_code = -1, frame_start_code = -1;
 
 	switch (go->format) {
-	case GO7007_FORMAT_MPEG4:
+	case V4L2_PIX_FMT_MPEG4:
 		seq_start_code = 0xB0;
+		gop_start_code = 0xB3;
 		frame_start_code = 0xB6;
 		break;
-	case GO7007_FORMAT_MPEG1:
-	case GO7007_FORMAT_MPEG2:
+	case V4L2_PIX_FMT_MPEG1:
+	case V4L2_PIX_FMT_MPEG2:
 		seq_start_code = 0xB3;
+		gop_start_code = 0xB8;
 		frame_start_code = 0x00;
 		break;
 	}
 
 	for (i = 0; i < length; ++i) {
-		if (go->active_buf != NULL &&
-			    go->active_buf->bytesused >= GO7007_BUF_SIZE - 3) {
+		if (vb && vb->vb.v4l2_planes[0].bytesused >= GO7007_BUF_SIZE - 3) {
 			v4l2_info(&go->v4l2_dev, "dropping oversized frame\n");
-			go->active_buf->offset -= go->active_buf->bytesused;
-			go->active_buf->bytesused = 0;
-			go->active_buf->modet_active = 0;
-			go->active_buf = NULL;
+			vb->vb.v4l2_planes[0].bytesused = 0;
+			vb->frame_offset = 0;
+			vb->modet_active = 0;
+			vb = go->active_buf = NULL;
 		}
 
 		switch (go->state) {
@@ -410,7 +482,7 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 				go->state = STATE_FF;
 				break;
 			default:
-				store_byte(go->active_buf, buf[i]);
+				store_byte(vb, buf[i]);
 				break;
 			}
 			break;
@@ -420,12 +492,12 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 				go->state = STATE_00_00;
 				break;
 			case 0xFF:
-				store_byte(go->active_buf, 0x00);
+				store_byte(vb, 0x00);
 				go->state = STATE_FF;
 				break;
 			default:
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, buf[i]);
+				store_byte(vb, 0x00);
+				store_byte(vb, buf[i]);
 				go->state = STATE_DATA;
 				break;
 			}
@@ -433,21 +505,21 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 		case STATE_00_00:
 			switch (buf[i]) {
 			case 0x00:
-				store_byte(go->active_buf, 0x00);
+				store_byte(vb, 0x00);
 				/* go->state remains STATE_00_00 */
 				break;
 			case 0x01:
 				go->state = STATE_00_00_01;
 				break;
 			case 0xFF:
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x00);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x00);
 				go->state = STATE_FF;
 				break;
 			default:
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, buf[i]);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x00);
+				store_byte(vb, buf[i]);
 				go->state = STATE_DATA;
 				break;
 			}
@@ -455,31 +527,26 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 		case STATE_00_00_01:
 			if (buf[i] == 0xF8 && go->modet_enable == 0) {
 				/* MODET start code, but MODET not enabled */
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x01);
-				store_byte(go->active_buf, 0xF8);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x01);
+				store_byte(vb, 0xF8);
 				go->state = STATE_DATA;
 				break;
 			}
 			/* If this is the start of a new MPEG frame,
 			 * get a new buffer */
-			if ((go->format == GO7007_FORMAT_MPEG1 ||
-					go->format == GO7007_FORMAT_MPEG2 ||
-					go->format == GO7007_FORMAT_MPEG4) &&
-					(buf[i] == seq_start_code ||
-						buf[i] == 0xB8 || /* GOP code */
-						buf[i] == frame_start_code)) {
-				if (go->active_buf == NULL || go->seen_frame)
-					frame_boundary(go);
-				if (buf[i] == frame_start_code) {
-					if (go->active_buf != NULL)
-						go->active_buf->frame_offset =
-							go->active_buf->offset;
-					go->seen_frame = 1;
-				} else {
-					go->seen_frame = 0;
-				}
+			if ((go->format == V4L2_PIX_FMT_MPEG1 ||
+			     go->format == V4L2_PIX_FMT_MPEG2 ||
+			     go->format == V4L2_PIX_FMT_MPEG4) &&
+			    (buf[i] == seq_start_code ||
+			     buf[i] == gop_start_code ||
+			     buf[i] == frame_start_code)) {
+				if (vb == NULL || go->seen_frame)
+					vb = frame_boundary(go, vb);
+				go->seen_frame = buf[i] == frame_start_code;
+				if (vb && go->seen_frame)
+					vb->frame_offset = vb->vb.v4l2_planes[0].bytesused;
 			}
 			/* Handle any special chunk types, or just write the
 			 * start code to the (potentially new) buffer */
@@ -498,16 +565,16 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 				go->state = STATE_MODET_MAP;
 				break;
 			case 0xFF: /* Potential JPEG start code */
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x01);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x01);
 				go->state = STATE_FF;
 				break;
 			default:
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x00);
-				store_byte(go->active_buf, 0x01);
-				store_byte(go->active_buf, buf[i]);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x00);
+				store_byte(vb, 0x01);
+				store_byte(vb, buf[i]);
 				go->state = STATE_DATA;
 				break;
 			}
@@ -515,20 +582,20 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 		case STATE_FF:
 			switch (buf[i]) {
 			case 0x00:
-				store_byte(go->active_buf, 0xFF);
+				store_byte(vb, 0xFF);
 				go->state = STATE_00;
 				break;
 			case 0xFF:
-				store_byte(go->active_buf, 0xFF);
+				store_byte(vb, 0xFF);
 				/* go->state remains STATE_FF */
 				break;
 			case 0xD8:
-				if (go->format == GO7007_FORMAT_MJPEG)
-					frame_boundary(go);
+				if (go->format == V4L2_PIX_FMT_MJPEG)
+					vb = frame_boundary(go, vb);
 				/* fall through */
 			default:
-				store_byte(go->active_buf, 0xFF);
-				store_byte(go->active_buf, buf[i]);
+				store_byte(vb, 0xFF);
+				store_byte(vb, buf[i]);
 				go->state = STATE_DATA;
 				break;
 			}
@@ -551,8 +618,8 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 					write_bitmap_word(go);
 				} else
 					go->modet_word = buf[i] << 8;
-			} else if (go->parse_length == 207 && go->active_buf) {
-				go->active_buf->modet_active = buf[i];
+			} else if (go->parse_length == 207 && vb) {
+				vb->modet_active = buf[i];
 			}
 			if (++go->parse_length == 208)
 				go->state = STATE_DATA;
@@ -563,15 +630,14 @@ void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length)
 			break;
 		}
 	}
-
-	spin_unlock(&go->spinlock);
 }
 EXPORT_SYMBOL(go7007_parse_video_stream);
 
 /*
  * Allocate a new go7007 struct.  Used by the hardware-specific probe.
  */
-struct go7007 *go7007_alloc(struct go7007_board_info *board, struct device *dev)
+struct go7007 *go7007_alloc(const struct go7007_board_info *board,
+						struct device *dev)
 {
 	struct go7007 *go;
 	int i;
@@ -588,33 +654,17 @@ struct go7007 *go7007_alloc(struct go7007_board_info *board, struct device *dev)
 	mutex_init(&go->hw_lock);
 	init_waitqueue_head(&go->frame_waitq);
 	spin_lock_init(&go->spinlock);
-	go->video_dev = NULL;
-	go->ref_count = 0;
 	go->status = STATUS_INIT;
 	memset(&go->i2c_adapter, 0, sizeof(go->i2c_adapter));
 	go->i2c_adapter_online = 0;
 	go->interrupt_available = 0;
 	init_waitqueue_head(&go->interrupt_waitq);
-	go->in_use = 0;
 	go->input = 0;
-	if (board->sensor_flags & GO7007_SENSOR_TV) {
-		go->standard = GO7007_STD_NTSC;
-		go->width = 720;
-		go->height = 480;
-		go->sensor_framerate = 30000;
-	} else {
-		go->standard = GO7007_STD_OTHER;
-		go->width = board->sensor_width;
-		go->height = board->sensor_height;
-		go->sensor_framerate = board->sensor_framerate;
-	}
-	go->encoder_v_offset = board->sensor_v_offset;
-	go->encoder_h_offset = board->sensor_h_offset;
+	go7007_update_board(go);
 	go->encoder_h_halve = 0;
 	go->encoder_v_halve = 0;
 	go->encoder_subsample = 0;
-	go->streaming = 0;
-	go->format = GO7007_FORMAT_MJPEG;
+	go->format = V4L2_PIX_FMT_MJPEG;
 	go->bitrate = 1500000;
 	go->fps_scale = 1;
 	go->pali = 0;
@@ -633,31 +683,30 @@ struct go7007 *go7007_alloc(struct go7007_board_info *board, struct device *dev)
 		go->modet_map[i] = 0;
 	go->audio_deliver = NULL;
 	go->audio_enabled = 0;
-	INIT_LIST_HEAD(&go->stream);
 
 	return go;
 }
 EXPORT_SYMBOL(go7007_alloc);
 
-/*
- * Detach and unregister the encoder.  The go7007 struct won't be freed
- * until v4l2 finishes releasing its resources and all associated fds are
- * closed by applications.
- */
-void go7007_remove(struct go7007 *go)
+void go7007_update_board(struct go7007 *go)
 {
-	if (go->i2c_adapter_online) {
-		if (i2c_del_adapter(&go->i2c_adapter) == 0)
-			go->i2c_adapter_online = 0;
-		else
-			v4l2_err(&go->v4l2_dev,
-				"error removing I2C adapter!\n");
-	}
+	const struct go7007_board_info *board = go->board_info;
 
-	if (go->audio_enabled)
-		go7007_snd_remove(go);
-	go7007_v4l2_remove(go);
+	if (board->sensor_flags & GO7007_SENSOR_TV) {
+		go->standard = GO7007_STD_NTSC;
+		go->std = V4L2_STD_NTSC_M;
+		go->width = 720;
+		go->height = 480;
+		go->sensor_framerate = 30000;
+	} else {
+		go->standard = GO7007_STD_OTHER;
+		go->width = board->sensor_width;
+		go->height = board->sensor_height;
+		go->sensor_framerate = board->sensor_framerate;
+	}
+	go->encoder_v_offset = board->sensor_v_offset;
+	go->encoder_h_offset = board->sensor_h_offset;
 }
-EXPORT_SYMBOL(go7007_remove);
+EXPORT_SYMBOL(go7007_update_board);
 
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/go7007/go7007-fw.c b/drivers/staging/media/go7007/go7007-fw.c
index a5ede1c109d0..c2d0e58afc34 100644
--- a/drivers/staging/media/go7007/go7007-fw.c
+++ b/drivers/staging/media/go7007/go7007-fw.c
@@ -36,6 +36,8 @@
 
 #include "go7007-priv.h"
 
+#define GO7007_FW_NAME "go7007/go7007tv.bin"
+
 /* Constants used in the source firmware image to describe code segments */
 
 #define	FLAG_MODE_MJPEG		(1)
@@ -455,9 +457,9 @@ static int mpeg1_frame_header(struct go7007 *go, unsigned char *buf,
 
 	CODE_ADD(c, frame == PFRAME ? 0x2 : 0x3, 13);
 	CODE_ADD(c, 0xffff, 16);
-	CODE_ADD(c, go->format == GO7007_FORMAT_MPEG2 ? 0x7 : 0x4, 4);
+	CODE_ADD(c, go->format == V4L2_PIX_FMT_MPEG2 ? 0x7 : 0x4, 4);
 	if (frame != PFRAME)
-		CODE_ADD(c, go->format == GO7007_FORMAT_MPEG2 ? 0x7 : 0x4, 4);
+		CODE_ADD(c, go->format == V4L2_PIX_FMT_MPEG2 ? 0x7 : 0x4, 4);
 	else
 		CODE_ADD(c, 0, 4); /* Is this supposed to be here?? */
 	CODE_ADD(c, 0, 3); /* What is this?? */
@@ -466,7 +468,7 @@ static int mpeg1_frame_header(struct go7007 *go, unsigned char *buf,
 	if (j != 8)
 		CODE_ADD(c, 0, j);
 
-	if (go->format == GO7007_FORMAT_MPEG2) {
+	if (go->format == V4L2_PIX_FMT_MPEG2) {
 		CODE_ADD(c, 0x1, 24);
 		CODE_ADD(c, 0xb5, 8);
 		CODE_ADD(c, 0x844, 12);
@@ -537,7 +539,7 @@ static int mpeg1_sequence_header(struct go7007 *go, unsigned char *buf, int ext)
 	int i, aspect_ratio, picture_rate;
 	CODE_GEN(c, buf + 6);
 
-	if (go->format == GO7007_FORMAT_MPEG1) {
+	if (go->format == V4L2_PIX_FMT_MPEG1) {
 		switch (go->aspect_ratio) {
 		case GO7007_RATIO_4_3:
 			aspect_ratio = go->standard == GO7007_STD_NTSC ? 3 : 2;
@@ -587,9 +589,9 @@ static int mpeg1_sequence_header(struct go7007 *go, unsigned char *buf, int ext)
 	CODE_ADD(c, go->height, 12);
 	CODE_ADD(c, aspect_ratio, 4);
 	CODE_ADD(c, picture_rate, 4);
-	CODE_ADD(c, go->format == GO7007_FORMAT_MPEG2 ? 20000 : 0x3ffff, 18);
+	CODE_ADD(c, go->format == V4L2_PIX_FMT_MPEG2 ? 20000 : 0x3ffff, 18);
 	CODE_ADD(c, 1, 1);
-	CODE_ADD(c, go->format == GO7007_FORMAT_MPEG2 ? 112 : 20, 10);
+	CODE_ADD(c, go->format == V4L2_PIX_FMT_MPEG2 ? 112 : 20, 10);
 	CODE_ADD(c, 0, 3);
 
 	/* Byte-align with zeros */
@@ -597,7 +599,7 @@ static int mpeg1_sequence_header(struct go7007 *go, unsigned char *buf, int ext)
 	if (i != 8)
 		CODE_ADD(c, 0, i);
 
-	if (go->format == GO7007_FORMAT_MPEG2) {
+	if (go->format == V4L2_PIX_FMT_MPEG2) {
 		CODE_ADD(c, 0x1, 24);
 		CODE_ADD(c, 0xb5, 8);
 		CODE_ADD(c, 0x148, 12);
@@ -930,10 +932,10 @@ static int brctrl_to_package(struct go7007 *go,
 					__le16 *code, int space, int *framelen)
 {
 	int converge_speed = 0;
-	int lambda = (go->format == GO7007_FORMAT_MJPEG || go->dvd_mode) ?
+	int lambda = (go->format == V4L2_PIX_FMT_MJPEG || go->dvd_mode) ?
 				100 : 0;
 	int peak_rate = 6 * go->bitrate / 5;
-	int vbv_buffer = go->format == GO7007_FORMAT_MJPEG ?
+	int vbv_buffer = go->format == V4L2_PIX_FMT_MJPEG ?
 				go->bitrate :
 				(go->dvd_mode ? 900000 : peak_rate);
 	int fps = go->sensor_framerate / go->fps_scale;
@@ -1096,10 +1098,10 @@ static int config_package(struct go7007 *go, __le16 *code, int space)
 		0xc003,		0x28b4,
 		0xc004,		0x3c5a,
 		0xdc05,		0x2a77,
-		0xc6c3,		go->format == GO7007_FORMAT_MPEG4 ? 0 :
-				(go->format == GO7007_FORMAT_H263 ? 0 : 1),
-		0xc680,		go->format == GO7007_FORMAT_MPEG4 ? 0xf1 :
-				(go->format == GO7007_FORMAT_H263 ? 0x61 :
+		0xc6c3,		go->format == V4L2_PIX_FMT_MPEG4 ? 0 :
+				(go->format == V4L2_PIX_FMT_H263 ? 0 : 1),
+		0xc680,		go->format == V4L2_PIX_FMT_MPEG4 ? 0xf1 :
+				(go->format == V4L2_PIX_FMT_H263 ? 0x61 :
 									0xd3),
 		0xc780,		0x0140,
 		0xe009,		0x0001,
@@ -1123,15 +1125,15 @@ static int config_package(struct go7007 *go, __le16 *code, int space)
 						(!go->interlace_coding) ?
 					0x0008 : 0x0009,
 		0xc404,		go->interlace_coding ? 0x44 :
-				(go->format == GO7007_FORMAT_MPEG4 ? 0x11 :
-				(go->format == GO7007_FORMAT_MPEG1 ? 0x02 :
-				(go->format == GO7007_FORMAT_MPEG2 ? 0x04 :
-				(go->format == GO7007_FORMAT_H263  ? 0x08 :
+				(go->format == V4L2_PIX_FMT_MPEG4 ? 0x11 :
+				(go->format == V4L2_PIX_FMT_MPEG1 ? 0x02 :
+				(go->format == V4L2_PIX_FMT_MPEG2 ? 0x04 :
+				(go->format == V4L2_PIX_FMT_H263  ? 0x08 :
 								     0x20)))),
-		0xbf0a,		(go->format == GO7007_FORMAT_MPEG4 ? 8 :
-				(go->format == GO7007_FORMAT_MPEG1 ? 1 :
-				(go->format == GO7007_FORMAT_MPEG2 ? 2 :
-				(go->format == GO7007_FORMAT_H263 ? 4 : 16)))) |
+		0xbf0a,		(go->format == V4L2_PIX_FMT_MPEG4 ? 8 :
+				(go->format == V4L2_PIX_FMT_MPEG1 ? 1 :
+				(go->format == V4L2_PIX_FMT_MPEG2 ? 2 :
+				(go->format == V4L2_PIX_FMT_H263 ? 4 : 16)))) |
 				((go->repeat_seqhead ? 1 : 0) << 6) |
 				((go->dvd_mode ? 1 : 0) << 9) |
 				((go->gop_header_enable ? 1 : 0) << 10),
@@ -1348,19 +1350,19 @@ static int final_package(struct go7007 *go, __le16 *code, int space)
 			0x41,
 		go->ipb ? 0xd4c : 0x36b,
 		(rows << 8) | (go->width >> 4),
-		go->format == GO7007_FORMAT_MPEG4 ? 0x0404 : 0,
+		go->format == V4L2_PIX_FMT_MPEG4 ? 0x0404 : 0,
 		(1 << 15) | ((go->interlace_coding ? 1 : 0) << 13) |
 			((go->closed_gop ? 1 : 0) << 12) |
-			((go->format == GO7007_FORMAT_MPEG4 ? 1 : 0) << 11) |
+			((go->format == V4L2_PIX_FMT_MPEG4 ? 1 : 0) << 11) |
 		/*	(1 << 9) |   */
 			((go->ipb ? 3 : 0) << 7) |
 			((go->modet_enable ? 1 : 0) << 2) |
 			((go->dvd_mode ? 1 : 0) << 1) | 1,
-		(go->format == GO7007_FORMAT_MPEG1 ? 0x89a0 :
-			(go->format == GO7007_FORMAT_MPEG2 ? 0x89a0 :
-			(go->format == GO7007_FORMAT_MJPEG ? 0x89a0 :
-			(go->format == GO7007_FORMAT_MPEG4 ? 0x8920 :
-			(go->format == GO7007_FORMAT_H263 ? 0x8920 : 0))))),
+		(go->format == V4L2_PIX_FMT_MPEG1 ? 0x89a0 :
+			(go->format == V4L2_PIX_FMT_MPEG2 ? 0x89a0 :
+			(go->format == V4L2_PIX_FMT_MJPEG ? 0x89a0 :
+			(go->format == V4L2_PIX_FMT_MPEG4 ? 0x8920 :
+			(go->format == V4L2_PIX_FMT_H263 ? 0x8920 : 0))))),
 		go->ipb ? 0x1f15 : 0x1f0b,
 		go->ipb ? 0x0015 : 0x000b,
 		go->ipb ? 0xa800 : 0x5800,
@@ -1503,13 +1505,13 @@ static int do_special(struct go7007 *go, u16 type, __le16 *code, int space,
 	switch (type) {
 	case SPECIAL_FRM_HEAD:
 		switch (go->format) {
-		case GO7007_FORMAT_MJPEG:
+		case V4L2_PIX_FMT_MJPEG:
 			return gen_mjpeghdr_to_package(go, code, space);
-		case GO7007_FORMAT_MPEG1:
-		case GO7007_FORMAT_MPEG2:
+		case V4L2_PIX_FMT_MPEG1:
+		case V4L2_PIX_FMT_MPEG2:
 			return gen_mpeg1hdr_to_package(go, code, space,
 								framelen);
-		case GO7007_FORMAT_MPEG4:
+		case V4L2_PIX_FMT_MPEG4:
 			return gen_mpeg4hdr_to_package(go, code, space,
 								framelen);
 		}
@@ -1519,11 +1521,11 @@ static int do_special(struct go7007 *go, u16 type, __le16 *code, int space,
 		return config_package(go, code, space);
 	case SPECIAL_SEQHEAD:
 		switch (go->format) {
-		case GO7007_FORMAT_MPEG1:
-		case GO7007_FORMAT_MPEG2:
+		case V4L2_PIX_FMT_MPEG1:
+		case V4L2_PIX_FMT_MPEG2:
 			return seqhead_to_package(go, code, space,
 					mpeg1_sequence_header);
-		case GO7007_FORMAT_MPEG4:
+		case V4L2_PIX_FMT_MPEG4:
 			return seqhead_to_package(go, code, space,
 					mpeg4_sequence_header);
 		default:
@@ -1553,25 +1555,25 @@ int go7007_construct_fw_image(struct go7007 *go, u8 **fw, int *fwlen)
 	int ret;
 
 	switch (go->format) {
-	case GO7007_FORMAT_MJPEG:
+	case V4L2_PIX_FMT_MJPEG:
 		mode_flag = FLAG_MODE_MJPEG;
 		break;
-	case GO7007_FORMAT_MPEG1:
+	case V4L2_PIX_FMT_MPEG1:
 		mode_flag = FLAG_MODE_MPEG1;
 		break;
-	case GO7007_FORMAT_MPEG2:
+	case V4L2_PIX_FMT_MPEG2:
 		mode_flag = FLAG_MODE_MPEG2;
 		break;
-	case GO7007_FORMAT_MPEG4:
+	case V4L2_PIX_FMT_MPEG4:
 		mode_flag = FLAG_MODE_MPEG4;
 		break;
 	default:
 		return -1;
 	}
-	if (request_firmware(&fw_entry, go->board_info->firmware, go->dev)) {
+	if (request_firmware(&fw_entry, GO7007_FW_NAME, go->dev)) {
 		dev_err(go->dev,
 			"unable to load firmware from file \"%s\"\n",
-			go->board_info->firmware);
+			GO7007_FW_NAME);
 		return -1;
 	}
 	code = kzalloc(codespace * 2, GFP_KERNEL);
@@ -1586,7 +1588,7 @@ int go7007_construct_fw_image(struct go7007 *go, u8 **fw, int *fwlen)
 		if (chunk_len + 2 > srclen) {
 			dev_err(go->dev,
 				"firmware file \"%s\" appears to be corrupted\n",
-				go->board_info->firmware);
+				GO7007_FW_NAME);
 			goto fw_failed;
 		}
 		if (chunk_flags & mode_flag) {
@@ -1622,3 +1624,5 @@ fw_failed:
 	release_firmware(fw_entry);
 	return -1;
 }
+
+MODULE_FIRMWARE(GO7007_FW_NAME);
diff --git a/drivers/staging/media/go7007/go7007-i2c.c b/drivers/staging/media/go7007/go7007-i2c.c
index 39456a36b2c6..74f25e03c326 100644
--- a/drivers/staging/media/go7007/go7007-i2c.c
+++ b/drivers/staging/media/go7007/go7007-i2c.c
@@ -28,7 +28,6 @@
 #include <linux/uaccess.h>
 
 #include "go7007-priv.h"
-#include "wis-i2c.h"
 
 /********************* Driver for on-board I2C adapter *********************/
 
@@ -52,11 +51,11 @@ static DEFINE_MUTEX(adlink_mpg24_i2c_lock);
 static int go7007_i2c_xfer(struct go7007 *go, u16 addr, int read,
 		u16 command, int flags, u8 *data)
 {
-	int i, ret = -1;
+	int i, ret = -EIO;
 	u16 val;
 
 	if (go->status == STATUS_SHUTDOWN)
-		return -1;
+		return -ENODEV;
 
 #ifdef GO7007_I2C_DEBUG
 	if (read)
@@ -146,7 +145,7 @@ static int go7007_smbus_xfer(struct i2c_adapter *adapter, u16 addr,
 	struct go7007 *go = i2c_get_adapdata(adapter);
 
 	if (size != I2C_SMBUS_BYTE_DATA)
-		return -1;
+		return -EIO;
 	return go7007_i2c_xfer(go, addr, read_write == I2C_SMBUS_READ, command,
 			flags & I2C_CLIENT_SCCB ? 0x10 : 0x00, &data->byte);
 }
@@ -170,26 +169,26 @@ static int go7007_i2c_master_xfer(struct i2c_adapter *adapter,
 					(msgs[i].flags & I2C_M_RD) ||
 					!(msgs[i + 1].flags & I2C_M_RD) ||
 					msgs[i + 1].len != 1)
-				return -1;
+				return -EIO;
 			if (go7007_i2c_xfer(go, msgs[i].addr, 1,
 					(msgs[i].buf[0] << 8) | msgs[i].buf[1],
 					0x01, &msgs[i + 1].buf[0]) < 0)
-				return -1;
+				return -EIO;
 			++i;
 		} else if (msgs[i].len == 3) {
 			if (msgs[i].flags & I2C_M_RD)
-				return -1;
+				return -EIO;
 			if (msgs[i].len != 3)
-				return -1;
+				return -EIO;
 			if (go7007_i2c_xfer(go, msgs[i].addr, 0,
 					(msgs[i].buf[0] << 8) | msgs[i].buf[1],
 					0x01, &msgs[i].buf[2]) < 0)
-				return -1;
+				return -EIO;
 		} else
-			return -1;
+			return -EIO;
 	}
 
-	return 0;
+	return num;
 }
 
 static u32 go7007_functionality(struct i2c_adapter *adapter)
diff --git a/drivers/staging/media/go7007/go7007-loader.c b/drivers/staging/media/go7007/go7007-loader.c
new file mode 100644
index 000000000000..f846ad5819dc
--- /dev/null
+++ b/drivers/staging/media/go7007/go7007-loader.c
@@ -0,0 +1,144 @@
+/*
+ * Copyright (C) 2008 Sensoray Company Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License (Version 2) as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+#include <linux/firmware.h>
+#include <cypress_firmware.h>
+
+struct fw_config {
+	u16 vendor;
+	u16 product;
+	const char * const fw_name1;
+	const char * const fw_name2;
+};
+
+struct fw_config fw_configs[] = {
+	{ 0x1943, 0xa250, "go7007/s2250-1.fw", "go7007/s2250-2.fw" },
+	{ 0x093b, 0xa002, "go7007/px-m402u.fw", NULL },
+	{ 0x093b, 0xa004, "go7007/px-tv402u.fw", NULL },
+	{ 0x0eb1, 0x6666, "go7007/lr192.fw", NULL },
+	{ 0x0eb1, 0x6668, "go7007/wis-startrek.fw", NULL },
+	{ 0, 0, NULL, NULL }
+};
+MODULE_FIRMWARE("go7007/s2250-1.fw");
+MODULE_FIRMWARE("go7007/s2250-2.fw");
+MODULE_FIRMWARE("go7007/px-m402u.fw");
+MODULE_FIRMWARE("go7007/px-tv402u.fw");
+MODULE_FIRMWARE("go7007/lr192.fw");
+MODULE_FIRMWARE("go7007/wis-startrek.fw");
+
+static int go7007_loader_probe(struct usb_interface *interface,
+				const struct usb_device_id *id)
+{
+	struct usb_device *usbdev;
+	const struct firmware *fw;
+	u16 vendor, product;
+	const char *fw1, *fw2;
+	int ret;
+	int i;
+
+	usbdev = usb_get_dev(interface_to_usbdev(interface));
+	if (!usbdev)
+		goto failed2;
+
+	if (usbdev->descriptor.bNumConfigurations != 1) {
+		dev_err(&interface->dev, "can't handle multiple config\n");
+		return -ENODEV;
+	}
+
+	vendor = le16_to_cpu(usbdev->descriptor.idVendor);
+	product = le16_to_cpu(usbdev->descriptor.idProduct);
+
+	for (i = 0; fw_configs[i].fw_name1; i++)
+		if (fw_configs[i].vendor == vendor &&
+		    fw_configs[i].product == product)
+			break;
+
+	/* Should never happen */
+	if (fw_configs[i].fw_name1 == NULL)
+		goto failed2;
+
+	fw1 = fw_configs[i].fw_name1;
+	fw2 = fw_configs[i].fw_name2;
+
+	dev_info(&interface->dev, "loading firmware %s\n", fw1);
+
+	if (request_firmware(&fw, fw1, &usbdev->dev)) {
+		dev_err(&interface->dev,
+			"unable to load firmware from file \"%s\"\n", fw1);
+		goto failed2;
+	}
+	ret = cypress_load_firmware(usbdev, fw, CYPRESS_FX2);
+	release_firmware(fw);
+	if (0 != ret) {
+		dev_err(&interface->dev, "loader download failed\n");
+		goto failed2;
+	}
+
+	if (fw2 == NULL)
+		return 0;
+
+	if (request_firmware(&fw, fw2, &usbdev->dev)) {
+		dev_err(&interface->dev,
+			"unable to load firmware from file \"%s\"\n", fw2);
+		goto failed2;
+	}
+	ret = cypress_load_firmware(usbdev, fw, CYPRESS_FX2);
+	release_firmware(fw);
+	if (0 != ret) {
+		dev_err(&interface->dev, "firmware download failed\n");
+		goto failed2;
+	}
+	return 0;
+
+failed2:
+	dev_err(&interface->dev, "probe failed\n");
+	return -ENODEV;
+}
+
+static void go7007_loader_disconnect(struct usb_interface *interface)
+{
+	dev_info(&interface->dev, "disconnect\n");
+	usb_set_intfdata(interface, NULL);
+}
+
+static const struct usb_device_id go7007_loader_ids[] = {
+	{ USB_DEVICE(0x1943, 0xa250) },
+	{ USB_DEVICE(0x093b, 0xa002) },
+	{ USB_DEVICE(0x093b, 0xa004) },
+	{ USB_DEVICE(0x0eb1, 0x6666) },
+	{ USB_DEVICE(0x0eb1, 0x6668) },
+	{}                          /* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, go7007_loader_ids);
+
+static struct usb_driver go7007_loader_driver = {
+	.name		= "go7007-loader",
+	.probe		= go7007_loader_probe,
+	.disconnect	= go7007_loader_disconnect,
+	.id_table	= go7007_loader_ids,
+};
+
+module_usb_driver(go7007_loader_driver);
+
+MODULE_AUTHOR("");
+MODULE_DESCRIPTION("firmware loader for go7007-usb");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/go7007/go7007-priv.h b/drivers/staging/media/go7007/go7007-priv.h
index b58c394c6555..6e16af720504 100644
--- a/drivers/staging/media/go7007/go7007-priv.h
+++ b/drivers/staging/media/go7007/go7007-priv.h
@@ -22,6 +22,9 @@
  */
 
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <media/videobuf2-core.h>
 
 struct go7007;
 
@@ -34,15 +37,13 @@ struct go7007;
 #define GO7007_BOARDID_XMEN_II		5
 #define GO7007_BOARDID_XMEN_III		6
 #define GO7007_BOARDID_MATRIX_REV	7
-#define GO7007_BOARDID_PX_M402U		16
-#define GO7007_BOARDID_PX_TV402U_ANY	17 /* need to check tuner model */
-#define GO7007_BOARDID_PX_TV402U_NA	18 /* detected NTSC tuner */
-#define GO7007_BOARDID_PX_TV402U_EU	19 /* detected PAL tuner */
-#define GO7007_BOARDID_PX_TV402U_JP	20 /* detected NTSC-J tuner */
-#define GO7007_BOARDID_LIFEVIEW_LR192	21 /* TV Walker Ultra */
-#define GO7007_BOARDID_ENDURA		22
-#define GO7007_BOARDID_ADLINK_MPG24	23
-#define GO7007_BOARDID_SENSORAY_2250	24 /* Sensoray 2250/2251 */
+#define GO7007_BOARDID_PX_M402U		8
+#define GO7007_BOARDID_PX_TV402U	9
+#define GO7007_BOARDID_LIFEVIEW_LR192	10 /* TV Walker Ultra */
+#define GO7007_BOARDID_ENDURA		11
+#define GO7007_BOARDID_ADLINK_MPG24	12
+#define GO7007_BOARDID_SENSORAY_2250	13 /* Sensoray 2250/2251 */
+#define GO7007_BOARDID_ADS_USBAV_709    14
 
 /* Various characteristics of each board */
 #define GO7007_BOARD_HAS_AUDIO		(1<<0)
@@ -61,6 +62,7 @@ struct go7007;
 #define GO7007_SENSOR_TV		(1<<7)
 #define GO7007_SENSOR_VBI		(1<<8)
 #define GO7007_SENSOR_SCALING		(1<<9)
+#define GO7007_SENSOR_SAA7115		(1<<10)
 
 /* Characteristics of audio sensor devices */
 #define GO7007_AUDIO_I2S_MODE_1		(1)
@@ -74,7 +76,6 @@ struct go7007;
 #define GO7007_AUDIO_OKI_MODE		(1<<17)
 
 struct go7007_board_info {
-	char *firmware;
 	unsigned int flags;
 	int hpi_buffer_cap;
 	unsigned int sensor_flags;
@@ -88,17 +89,25 @@ struct go7007_board_info {
 	int audio_bclk_div;
 	int audio_main_div;
 	int num_i2c_devs;
-	struct {
+	struct go_i2c {
 		const char *type;
-		int id;
+		unsigned int is_video:1;
+		unsigned int is_audio:1;
 		int addr;
-	} i2c_devs[4];
+		u32 flags;
+	} i2c_devs[5];
 	int num_inputs;
 	struct {
 		int video_input;
-		int audio_input;
+		int audio_index;
 		char *name;
 	} inputs[4];
+	int video_config;
+	int num_aud_inputs;
+	struct {
+		int audio_input;
+		char *name;
+	} aud_inputs[3];
 };
 
 struct go7007_hpi_ops {
@@ -109,6 +118,7 @@ struct go7007_hpi_ops {
 	int (*stream_stop)(struct go7007 *go);
 	int (*send_firmware)(struct go7007 *go, u8 *data, int len);
 	int (*send_command)(struct go7007 *go, unsigned int cmd, void *arg);
+	void (*release)(struct go7007 *go);
 };
 
 /* The video buffer size must be a multiple of PAGE_SIZE */
@@ -116,35 +126,12 @@ struct go7007_hpi_ops {
 #define	GO7007_BUF_SIZE		(GO7007_BUF_PAGES << PAGE_SHIFT)
 
 struct go7007_buffer {
-	struct go7007 *go; /* Reverse reference for VMA ops */
-	int index; /* Reverse reference for DQBUF */
-	enum { BUF_STATE_IDLE, BUF_STATE_QUEUED, BUF_STATE_DONE } state;
-	u32 seq;
-	struct timeval timestamp;
-	struct list_head stream;
-	struct page *pages[GO7007_BUF_PAGES + 1]; /* extra for straddling */
-	unsigned long user_addr;
-	unsigned int page_count;
-	unsigned int offset;
-	unsigned int bytesused;
+	struct vb2_buffer vb;
+	struct list_head list;
 	unsigned int frame_offset;
 	u32 modet_active;
-	int mapped;
 };
 
-struct go7007_file {
-	struct go7007 *go;
-	struct mutex lock;
-	int buf_count;
-	struct go7007_buffer *bufs;
-};
-
-#define	GO7007_FORMAT_MJPEG	0
-#define GO7007_FORMAT_MPEG4	1
-#define GO7007_FORMAT_MPEG1	2
-#define GO7007_FORMAT_MPEG2	3
-#define GO7007_FORMAT_H263	4
-
 #define GO7007_RATIO_1_1	0
 #define GO7007_RATIO_4_3	1
 #define GO7007_RATIO_16_9	2
@@ -163,24 +150,38 @@ enum go7007_parser_state {
 
 struct go7007 {
 	struct device *dev;
-	struct go7007_board_info *board_info;
+	u8 bus_info[32];
+	const struct go7007_board_info *board_info;
 	unsigned int board_id;
 	int tuner_type;
 	int channel_number; /* for multi-channel boards like Adlink PCI-MPG24 */
 	char name[64];
-	struct video_device *video_dev;
+	struct video_device vdev;
+	void *boot_fw;
+	unsigned boot_fw_len;
 	struct v4l2_device v4l2_dev;
-	int ref_count;
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_ctrl *mpeg_video_encoding;
+	struct v4l2_ctrl *mpeg_video_gop_size;
+	struct v4l2_ctrl *mpeg_video_gop_closure;
+	struct v4l2_ctrl *mpeg_video_bitrate;
+	struct v4l2_ctrl *mpeg_video_aspect_ratio;
+	struct v4l2_ctrl *mpeg_video_b_frames;
+	struct v4l2_ctrl *mpeg_video_rep_seqheader;
 	enum { STATUS_INIT, STATUS_ONLINE, STATUS_SHUTDOWN } status;
 	spinlock_t spinlock;
 	struct mutex hw_lock;
-	int streaming;
-	int in_use;
+	struct mutex serialize_lock;
 	int audio_enabled;
+	struct v4l2_subdev *sd_video;
+	struct v4l2_subdev *sd_audio;
+	u8 usb_buf[16];
 
 	/* Video input */
 	int input;
+	int aud_input;
 	enum { GO7007_STD_NTSC, GO7007_STD_PAL, GO7007_STD_OTHER } standard;
+	v4l2_std_id std;
 	int sensor_framerate;
 	int width;
 	int height;
@@ -191,7 +192,7 @@ struct go7007 {
 	unsigned int encoder_subsample:1;
 
 	/* Encoder config */
-	int format;
+	u32 format;
 	int bitrate;
 	int fps_scale;
 	int pali;
@@ -217,14 +218,16 @@ struct go7007 {
 	unsigned char active_map[216];
 
 	/* Video streaming */
-	struct go7007_buffer *active_buf;
+	struct mutex queue_lock;
+	struct vb2_queue vidq;
 	enum go7007_parser_state state;
 	int parse_length;
 	u16 modet_word;
 	int seen_frame;
 	u32 next_seq;
-	struct list_head stream;
+	struct list_head vidq_active;
 	wait_queue_head_t frame_waitq;
+	struct go7007_buffer *active_buf;
 
 	/* Audio streaming */
 	void (*audio_deliver)(struct go7007 *go, u8 *buf, int length);
@@ -267,12 +270,12 @@ int go7007_read_addr(struct go7007 *go, u16 addr, u16 *data);
 int go7007_read_interrupt(struct go7007 *go, u16 *value, u16 *data);
 int go7007_boot_encoder(struct go7007 *go, int init_i2c);
 int go7007_reset_encoder(struct go7007 *go);
-int go7007_register_encoder(struct go7007 *go);
+int go7007_register_encoder(struct go7007 *go, unsigned num_i2c_devs);
 int go7007_start_encoder(struct go7007 *go);
 void go7007_parse_video_stream(struct go7007 *go, u8 *buf, int length);
-struct go7007 *go7007_alloc(struct go7007_board_info *board,
+struct go7007 *go7007_alloc(const struct go7007_board_info *board,
 					struct device *dev);
-void go7007_remove(struct go7007 *go);
+void go7007_update_board(struct go7007 *go);
 
 /* go7007-fw.c */
 int go7007_construct_fw_image(struct go7007 *go, u8 **fw, int *fwlen);
@@ -283,6 +286,7 @@ int go7007_i2c_remove(struct go7007 *go);
 
 /* go7007-v4l2.c */
 int go7007_v4l2_init(struct go7007 *go);
+int go7007_v4l2_ctrl_init(struct go7007 *go);
 void go7007_v4l2_remove(struct go7007 *go);
 
 /* snd-go7007.c */
diff --git a/drivers/staging/media/go7007/go7007-usb.c b/drivers/staging/media/go7007/go7007-usb.c
index 9dbf5ecd05a2..50066e01a6ed 100644
--- a/drivers/staging/media/go7007/go7007-usb.c
+++ b/drivers/staging/media/go7007/go7007-usb.c
@@ -26,10 +26,11 @@
 #include <linux/usb.h>
 #include <linux/i2c.h>
 #include <asm/byteorder.h>
-#include <media/tvaudio.h>
+#include <media/saa7115.h>
+#include <media/tuner.h>
+#include <media/uda1342.h>
 
 #include "go7007-priv.h"
-#include "wis-i2c.h"
 
 static unsigned int assume_endura;
 module_param(assume_endura, int, 0644);
@@ -62,7 +63,7 @@ struct go7007_usb_board {
 };
 
 struct go7007_usb {
-	struct go7007_usb_board *board;
+	const struct go7007_usb_board *board;
 	struct mutex i2c_lock;
 	struct usb_device *usbdev;
 	struct urb *video_urbs[8];
@@ -72,10 +73,9 @@ struct go7007_usb {
 
 /*********************** Product specification data ***********************/
 
-static struct go7007_usb_board board_matrix_ii = {
+static const struct go7007_usb_board board_matrix_ii = {
 	.flags		= GO7007_USB_EZUSB,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = GO7007_BOARD_HAS_AUDIO |
 					GO7007_BOARD_USE_ONBOARD_I2C,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
@@ -87,14 +87,15 @@ static struct go7007_usb_board board_matrix_ii = {
 		.sensor_flags	 = GO7007_SENSOR_656 |
 					GO7007_SENSOR_VALID_ENABLE |
 					GO7007_SENSOR_TV |
+					GO7007_SENSOR_SAA7115 |
 					GO7007_SENSOR_VBI |
 					GO7007_SENSOR_SCALING,
 		.num_i2c_devs	 = 1,
 		.i2c_devs	 = {
 			{
-				.type	= "wis_saa7115",
-				.id	= I2C_DRIVERID_WIS_SAA7115,
+				.type	= "saa7115",
 				.addr	= 0x20,
+				.is_video = 1,
 			},
 		},
 		.num_inputs	 = 2,
@@ -108,13 +109,13 @@ static struct go7007_usb_board board_matrix_ii = {
 				.name		= "S-Video",
 			},
 		},
+		.video_config	= SAA7115_IDQ_IS_DEFAULT,
 	},
 };
 
-static struct go7007_usb_board board_matrix_reload = {
+static const struct go7007_usb_board board_matrix_reload = {
 	.flags		= GO7007_USB_EZUSB,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = GO7007_BOARD_HAS_AUDIO |
 					GO7007_BOARD_USE_ONBOARD_I2C,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
@@ -129,9 +130,9 @@ static struct go7007_usb_board board_matrix_reload = {
 		.num_i2c_devs	 = 1,
 		.i2c_devs	 = {
 			{
-				.type	= "wis_saa7113",
-				.id	= I2C_DRIVERID_WIS_SAA7113,
+				.type	= "saa7113",
 				.addr	= 0x25,
+				.is_video = 1,
 			},
 		},
 		.num_inputs	 = 2,
@@ -145,18 +146,19 @@ static struct go7007_usb_board board_matrix_reload = {
 				.name		= "S-Video",
 			},
 		},
+		.video_config	= SAA7115_IDQ_IS_DEFAULT,
 	},
 };
 
-static struct go7007_usb_board board_star_trek = {
+static const struct go7007_usb_board board_star_trek = {
 	.flags		= GO7007_USB_EZUSB | GO7007_USB_EZUSB_I2C,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = GO7007_BOARD_HAS_AUDIO, /* |
 					GO7007_BOARD_HAS_TUNER, */
 		.sensor_flags	 = GO7007_SENSOR_656 |
 					GO7007_SENSOR_VALID_ENABLE |
 					GO7007_SENSOR_TV |
+					GO7007_SENSOR_SAA7115 |
 					GO7007_SENSOR_VBI |
 					GO7007_SENSOR_SCALING,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
@@ -167,42 +169,43 @@ static struct go7007_usb_board board_star_trek = {
 		.num_i2c_devs	 = 1,
 		.i2c_devs	 = {
 			{
-				.type	= "wis_saa7115",
-				.id	= I2C_DRIVERID_WIS_SAA7115,
+				.type	= "saa7115",
 				.addr	= 0x20,
+				.is_video = 1,
 			},
 		},
 		.num_inputs	 = 2,
 		.inputs		 = {
+		/*	{
+		 *		.video_input	= 3,
+		 *		.audio_index	= AUDIO_TUNER,
+		 *		.name		= "Tuner",
+		 *	},
+		 */
 			{
 				.video_input	= 1,
-			/*	.audio_input	= AUDIO_EXTERN, */
+			/*	.audio_index	= AUDIO_EXTERN, */
 				.name		= "Composite",
 			},
 			{
 				.video_input	= 8,
-			/*	.audio_input	= AUDIO_EXTERN, */
+			/*	.audio_index	= AUDIO_EXTERN, */
 				.name		= "S-Video",
 			},
-		/*	{
-		 *		.video_input	= 3,
-		 *		.audio_input	= AUDIO_TUNER,
-		 *		.name		= "Tuner",
-		 *	},
-		 */
 		},
+		.video_config	= SAA7115_IDQ_IS_DEFAULT,
 	},
 };
 
-static struct go7007_usb_board board_px_tv402u = {
+static const struct go7007_usb_board board_px_tv402u = {
 	.flags		= GO7007_USB_EZUSB | GO7007_USB_EZUSB_I2C,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = GO7007_BOARD_HAS_AUDIO |
 					GO7007_BOARD_HAS_TUNER,
 		.sensor_flags	 = GO7007_SENSOR_656 |
 					GO7007_SENSOR_VALID_ENABLE |
 					GO7007_SENSOR_TV |
+					GO7007_SENSOR_SAA7115 |
 					GO7007_SENSOR_VBI |
 					GO7007_SENSOR_SCALING,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
@@ -210,49 +213,67 @@ static struct go7007_usb_board board_px_tv402u = {
 		.audio_bclk_div	 = 8,
 		.audio_main_div	 = 2,
 		.hpi_buffer_cap  = 7,
-		.num_i2c_devs	 = 3,
+		.num_i2c_devs	 = 5,
 		.i2c_devs	 = {
 			{
-				.type	= "wis_saa7115",
-				.id	= I2C_DRIVERID_WIS_SAA7115,
+				.type	= "saa7115",
 				.addr	= 0x20,
+				.is_video = 1,
 			},
 			{
-				.type	= "wis_uda1342",
-				.id	= I2C_DRIVERID_WIS_UDA1342,
+				.type	= "uda1342",
 				.addr	= 0x1a,
+				.is_audio = 1,
 			},
 			{
-				.type	= "wis_sony_tuner",
-				.id	= I2C_DRIVERID_WIS_SONY_TUNER,
+				.type	= "tuner",
 				.addr	= 0x60,
 			},
+			{
+				.type	= "tuner",
+				.addr	= 0x43,
+			},
+			{
+				.type	= "sony-btf-mpx",
+				.addr	= 0x44,
+			},
 		},
 		.num_inputs	 = 3,
 		.inputs		 = {
 			{
+				.video_input	= 3,
+				.audio_index	= 0,
+				.name		= "Tuner",
+			},
+			{
 				.video_input	= 1,
-				.audio_input	= TVAUDIO_INPUT_EXTERN,
+				.audio_index	= 1,
 				.name		= "Composite",
 			},
 			{
 				.video_input	= 8,
-				.audio_input	= TVAUDIO_INPUT_EXTERN,
+				.audio_index	= 1,
 				.name		= "S-Video",
 			},
+		},
+		.video_config	= SAA7115_IDQ_IS_DEFAULT,
+		.num_aud_inputs	 = 2,
+		.aud_inputs	 = {
 			{
-				.video_input	= 3,
-				.audio_input	= TVAUDIO_INPUT_TUNER,
+				.audio_input	= UDA1342_IN2,
 				.name		= "Tuner",
 			},
+			{
+				.audio_input	= UDA1342_IN1,
+				.name		= "Line In",
+			},
 		},
 	},
 };
 
-static struct go7007_usb_board board_xmen = {
+static const struct go7007_usb_board board_xmen = {
 	.flags		= 0,
 	.main_info	= {
-		.firmware	  = "go7007tv.bin",
 		.flags		  = GO7007_BOARD_USE_ONBOARD_I2C,
 		.hpi_buffer_cap   = 0,
 		.sensor_flags	  = GO7007_SENSOR_VREF_POLAR,
@@ -271,8 +292,7 @@ static struct go7007_usb_board board_xmen = {
 		.num_i2c_devs	  = 1,
 		.i2c_devs	  = {
 			{
-				.type	= "wis_ov7640",
-				.id	= I2C_DRIVERID_WIS_OV7640,
+				.type	= "ov7640",
 				.addr	= 0x21,
 			},
 		},
@@ -285,10 +305,9 @@ static struct go7007_usb_board board_xmen = {
 	},
 };
 
-static struct go7007_usb_board board_matrix_revolution = {
+static const struct go7007_usb_board board_matrix_revolution = {
 	.flags		= GO7007_USB_EZUSB,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = GO7007_BOARD_HAS_AUDIO |
 					GO7007_BOARD_USE_ONBOARD_I2C,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
@@ -304,8 +323,8 @@ static struct go7007_usb_board board_matrix_revolution = {
 		.num_i2c_devs	 = 1,
 		.i2c_devs	 = {
 			{
-				.type	= "wis_tw9903",
-				.id	= I2C_DRIVERID_WIS_TW9903,
+				.type	= "tw9903",
+				.is_video = 1,
 				.addr	= 0x44,
 			},
 		},
@@ -323,10 +342,9 @@ static struct go7007_usb_board board_matrix_revolution = {
 	},
 };
 
-static struct go7007_usb_board board_lifeview_lr192 = {
+static const struct go7007_usb_board board_lifeview_lr192 = {
 	.flags		= GO7007_USB_EZUSB,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = GO7007_BOARD_HAS_AUDIO |
 					GO7007_BOARD_USE_ONBOARD_I2C,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
@@ -351,10 +369,9 @@ static struct go7007_usb_board board_lifeview_lr192 = {
 	},
 };
 
-static struct go7007_usb_board board_endura = {
+static const struct go7007_usb_board board_endura = {
 	.flags		= 0,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = 0,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
 					GO7007_AUDIO_I2S_MASTER |
@@ -376,10 +393,9 @@ static struct go7007_usb_board board_endura = {
 	},
 };
 
-static struct go7007_usb_board board_adlink_mpg24 = {
+static const struct go7007_usb_board board_adlink_mpg24 = {
 	.flags		= 0,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.flags		 = GO7007_BOARD_USE_ONBOARD_I2C,
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
 					GO7007_AUDIO_I2S_MASTER |
@@ -394,9 +410,10 @@ static struct go7007_usb_board board_adlink_mpg24 = {
 		.num_i2c_devs	 = 1,
 		.i2c_devs	 = {
 			{
-				.type	= "wis_tw2804",
-				.id	= I2C_DRIVERID_WIS_TW2804,
+				.type	= "tw2804",
 				.addr	= 0x00, /* yes, really */
+				.flags  = I2C_CLIENT_TEN,
+				.is_video = 1,
 			},
 		},
 		.num_inputs	 = 1,
@@ -408,10 +425,9 @@ static struct go7007_usb_board board_adlink_mpg24 = {
 	},
 };
 
-static struct go7007_usb_board board_sensoray_2250 = {
+static const struct go7007_usb_board board_sensoray_2250 = {
 	.flags		= GO7007_USB_EZUSB | GO7007_USB_EZUSB_I2C,
 	.main_info	= {
-		.firmware	 = "go7007tv.bin",
 		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
 					GO7007_AUDIO_I2S_MASTER |
 					GO7007_AUDIO_WORD_16,
@@ -426,8 +442,9 @@ static struct go7007_usb_board board_sensoray_2250 = {
 		.i2c_devs	 = {
 			{
 				.type	= "s2250",
-				.id	= I2C_DRIVERID_S2250,
 				.addr	= 0x43,
+				.is_video = 1,
+				.is_audio = 1,
 			},
 		},
 		.num_inputs	 = 2,
@@ -441,10 +458,60 @@ static struct go7007_usb_board board_sensoray_2250 = {
 				.name		= "S-Video",
 			},
 		},
+		.num_aud_inputs	 = 3,
+		.aud_inputs	 = {
+			{
+				.audio_input	= 0,
+				.name		= "Line In",
+			},
+			{
+				.audio_input	= 1,
+				.name		= "Mic",
+			},
+			{
+				.audio_input	= 2,
+				.name		= "Mic Boost",
+			},
+		},
 	},
 };
 
-MODULE_FIRMWARE("go7007tv.bin");
+static const struct go7007_usb_board board_ads_usbav_709 = {
+	.flags		= GO7007_USB_EZUSB,
+	.main_info	= {
+		.flags		 = GO7007_BOARD_HAS_AUDIO |
+					GO7007_BOARD_USE_ONBOARD_I2C,
+		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
+					GO7007_AUDIO_I2S_MASTER |
+					GO7007_AUDIO_WORD_16,
+		.audio_rate	 = 48000,
+		.audio_bclk_div	 = 8,
+		.audio_main_div	 = 2,
+		.hpi_buffer_cap  = 7,
+		.sensor_flags	 = GO7007_SENSOR_656 |
+					GO7007_SENSOR_TV |
+					GO7007_SENSOR_VBI,
+		.num_i2c_devs	 = 1,
+		.i2c_devs	 = {
+			{
+				.type	= "tw9906",
+				.is_video = 1,
+				.addr	= 0x44,
+			},
+		},
+		.num_inputs	 = 2,
+		.inputs		 = {
+			{
+				.video_input	= 0,
+				.name		= "Composite",
+			},
+			{
+				.video_input	= 10,
+				.name		= "S-Video",
+			},
+		},
+	},
+};
 
 static const struct usb_device_id go7007_usb_id_table[] = {
 	{
@@ -529,7 +596,7 @@ static const struct usb_device_id go7007_usb_id_table[] = {
 		.idProduct	= 0xa104,  /* Product ID of TV402U */
 		.bcdDevice_lo	= 0x1,
 		.bcdDevice_hi	= 0x1,
-		.driver_info	= (kernel_ulong_t)GO7007_BOARDID_PX_TV402U_ANY,
+		.driver_info	= (kernel_ulong_t)GO7007_BOARDID_PX_TV402U,
 	},
 	{
 		.match_flags	= USB_DEVICE_ID_MATCH_DEVICE_AND_VERSION,
@@ -547,6 +614,14 @@ static const struct usb_device_id go7007_usb_id_table[] = {
 		.bcdDevice_hi	= 0x1,
 		.driver_info	= (kernel_ulong_t)GO7007_BOARDID_SENSORAY_2250,
 	},
+	{
+		.match_flags	= USB_DEVICE_ID_MATCH_DEVICE_AND_VERSION,
+		.idVendor	= 0x06e1,  /* Vendor ID of ADS Technologies */
+		.idProduct	= 0x0709,  /* Product ID of DVD Xpress DX2 */
+		.bcdDevice_lo	= 0x204,
+		.bcdDevice_hi	= 0x204,
+		.driver_info	= (kernel_ulong_t)GO7007_BOARDID_ADS_USBAV_709,
+	},
 	{ }					/* Terminating entry */
 };
 
@@ -578,6 +653,8 @@ static int go7007_usb_interface_reset(struct go7007 *go)
 	struct go7007_usb *usb = go->hpi_context;
 	u16 intr_val, intr_data;
 
+	if (go->status == STATUS_SHUTDOWN)
+		return -1;
 	/* Reset encoder */
 	if (go7007_write_interrupt(go, 0x0001, 0x0001) < 0)
 		return -1;
@@ -613,7 +690,7 @@ static int go7007_usb_ezusb_write_interrupt(struct go7007 *go,
 {
 	struct go7007_usb *usb = go->hpi_context;
 	int i, r;
-	u16 status_reg;
+	u16 status_reg = 0;
 	int timeout = 500;
 
 #ifdef GO7007_USB_DEBUG
@@ -625,15 +702,17 @@ static int go7007_usb_ezusb_write_interrupt(struct go7007 *go,
 		r = usb_control_msg(usb->usbdev,
 				usb_rcvctrlpipe(usb->usbdev, 0), 0x14,
 				USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
-				0, HPI_STATUS_ADDR, &status_reg,
+				0, HPI_STATUS_ADDR, go->usb_buf,
 				sizeof(status_reg), timeout);
 		if (r < 0)
-			goto write_int_error;
-		__le16_to_cpus(&status_reg);
+			break;
+		status_reg = le16_to_cpu(*((u16 *)go->usb_buf));
 		if (!(status_reg & 0x0010))
 			break;
 		msleep(10);
 	}
+	if (r < 0)
+		goto write_int_error;
 	if (i == 100) {
 		printk(KERN_ERR
 			"go7007-usb: device is hung, status reg = 0x%04x\n",
@@ -661,7 +740,6 @@ static int go7007_usb_onboard_write_interrupt(struct go7007 *go,
 						int addr, int data)
 {
 	struct go7007_usb *usb = go->hpi_context;
-	u8 *tbuf;
 	int r;
 	int timeout = 500;
 
@@ -670,17 +748,14 @@ static int go7007_usb_onboard_write_interrupt(struct go7007 *go,
 		"go7007-usb: WriteInterrupt: %04x %04x\n", addr, data);
 #endif
 
-	tbuf = kzalloc(8, GFP_KERNEL);
-	if (tbuf == NULL)
-		return -ENOMEM;
-	tbuf[0] = data & 0xff;
-	tbuf[1] = data >> 8;
-	tbuf[2] = addr & 0xff;
-	tbuf[3] = addr >> 8;
+	go->usb_buf[0] = data & 0xff;
+	go->usb_buf[1] = data >> 8;
+	go->usb_buf[2] = addr & 0xff;
+	go->usb_buf[3] = addr >> 8;
+	go->usb_buf[4] = go->usb_buf[5] = go->usb_buf[6] = go->usb_buf[7] = 0;
 	r = usb_control_msg(usb->usbdev, usb_sndctrlpipe(usb->usbdev, 2), 0x00,
 			USB_TYPE_VENDOR | USB_RECIP_ENDPOINT, 0x55aa,
-			0xf0f0, tbuf, 8, timeout);
-	kfree(tbuf);
+			0xf0f0, go->usb_buf, 8, timeout);
 	if (r < 0) {
 		printk(KERN_ERR "go7007-usb: error in WriteInterrupt: %d\n", r);
 		return r;
@@ -738,7 +813,7 @@ static void go7007_usb_read_video_pipe_complete(struct urb *urb)
 	struct go7007 *go = (struct go7007 *)urb->context;
 	int r, status = urb->status;
 
-	if (!go->streaming) {
+	if (!vb2_is_streaming(&go->vidq)) {
 		wake_up_interruptible(&go->frame_waitq);
 		return;
 	}
@@ -762,7 +837,7 @@ static void go7007_usb_read_audio_pipe_complete(struct urb *urb)
 	struct go7007 *go = (struct go7007 *)urb->context;
 	int r, status = urb->status;
 
-	if (!go->streaming)
+	if (!vb2_is_streaming(&go->vidq))
 		return;
 	if (status) {
 		printk(KERN_ERR "go7007-usb: error in audio pipe: %d\n",
@@ -849,6 +924,37 @@ static int go7007_usb_send_firmware(struct go7007 *go, u8 *data, int len)
 					&transferred, timeout);
 }
 
+static void go7007_usb_release(struct go7007 *go)
+{
+	struct go7007_usb *usb = go->hpi_context;
+	struct urb *vurb, *aurb;
+	int i;
+
+	if (usb->intr_urb) {
+		usb_kill_urb(usb->intr_urb);
+		kfree(usb->intr_urb->transfer_buffer);
+		usb_free_urb(usb->intr_urb);
+	}
+
+	/* Free USB-related structs */
+	for (i = 0; i < 8; ++i) {
+		vurb = usb->video_urbs[i];
+		if (vurb) {
+			usb_kill_urb(vurb);
+			kfree(vurb->transfer_buffer);
+			usb_free_urb(vurb);
+		}
+		aurb = usb->audio_urbs[i];
+		if (aurb) {
+			usb_kill_urb(aurb);
+			kfree(aurb->transfer_buffer);
+			usb_free_urb(aurb);
+		}
+	}
+
+	kfree(go->hpi_context);
+}
+
 static struct go7007_hpi_ops go7007_usb_ezusb_hpi_ops = {
 	.interface_reset	= go7007_usb_interface_reset,
 	.write_interrupt	= go7007_usb_ezusb_write_interrupt,
@@ -856,6 +962,7 @@ static struct go7007_hpi_ops go7007_usb_ezusb_hpi_ops = {
 	.stream_start		= go7007_usb_stream_start,
 	.stream_stop		= go7007_usb_stream_stop,
 	.send_firmware		= go7007_usb_send_firmware,
+	.release		= go7007_usb_release,
 };
 
 static struct go7007_hpi_ops go7007_usb_onboard_hpi_ops = {
@@ -865,6 +972,7 @@ static struct go7007_hpi_ops go7007_usb_onboard_hpi_ops = {
 	.stream_start		= go7007_usb_stream_start,
 	.stream_stop		= go7007_usb_stream_stop,
 	.send_firmware		= go7007_usb_send_firmware,
+	.release		= go7007_usb_release,
 };
 
 /********************* Driver for EZ-USB I2C adapter *********************/
@@ -874,12 +982,12 @@ static int go7007_usb_i2c_master_xfer(struct i2c_adapter *adapter,
 {
 	struct go7007 *go = i2c_get_adapdata(adapter);
 	struct go7007_usb *usb = go->hpi_context;
-	u8 buf[16];
+	u8 *buf = go->usb_buf;
 	int buf_len, i;
-	int ret = -1;
+	int ret = -EIO;
 
 	if (go->status == STATUS_SHUTDOWN)
-		return -1;
+		return -ENODEV;
 
 	mutex_lock(&usb->i2c_lock);
 
@@ -929,14 +1037,14 @@ static int go7007_usb_i2c_master_xfer(struct i2c_adapter *adapter,
 						buf, buf_len, 0) < 0)
 			goto i2c_done;
 		if (msgs[i].flags & I2C_M_RD) {
-			memset(buf, 0, sizeof(buf));
+			memset(buf, 0, msgs[i].len + 1);
 			if (go7007_usb_vendor_request(go, 0x25, 0, 0, buf,
 						msgs[i].len + 1, 1) < 0)
 				goto i2c_done;
 			memcpy(msgs[i].buf, buf + 1, msgs[i].len);
 		}
 	}
-	ret = 0;
+	ret = num;
 
 i2c_done:
 	mutex_unlock(&usb->i2c_lock);
@@ -968,8 +1076,9 @@ static int go7007_usb_probe(struct usb_interface *intf,
 {
 	struct go7007 *go;
 	struct go7007_usb *usb;
-	struct go7007_usb_board *board;
+	const struct go7007_usb_board *board;
 	struct usb_device *usbdev = interface_to_usbdev(intf);
+	unsigned num_i2c_devs;
 	char *name;
 	int video_pipe, i, v_urb_len;
 
@@ -1008,7 +1117,7 @@ static int go7007_usb_probe(struct usb_interface *intf,
 		name = "Plextor PX-M402U";
 		board = &board_matrix_ii;
 		break;
-	case GO7007_BOARDID_PX_TV402U_ANY:
+	case GO7007_BOARDID_PX_TV402U:
 		name = "Plextor PX-TV402U (unknown tuner)";
 		board = &board_px_tv402u;
 		break;
@@ -1024,29 +1133,29 @@ static int go7007_usb_probe(struct usb_interface *intf,
 		name = "Sensoray 2250/2251";
 		board = &board_sensoray_2250;
 		break;
+	case GO7007_BOARDID_ADS_USBAV_709:
+		name = "ADS Tech DVD Xpress DX2";
+		board = &board_ads_usbav_709;
+		break;
 	default:
 		printk(KERN_ERR "go7007-usb: unknown board ID %d!\n",
 				(unsigned int)id->driver_info);
 		return 0;
 	}
 
-	usb = kzalloc(sizeof(struct go7007_usb), GFP_KERNEL);
-	if (usb == NULL)
+	go = go7007_alloc(&board->main_info, &intf->dev);
+	if (go == NULL)
 		return -ENOMEM;
 
-	/* Allocate the URB and buffer for receiving incoming interrupts */
-	usb->intr_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (usb->intr_urb == NULL)
-		goto allocfail;
-	usb->intr_urb->transfer_buffer = kmalloc(2*sizeof(u16), GFP_KERNEL);
-	if (usb->intr_urb->transfer_buffer == NULL)
-		goto allocfail;
+	usb = kzalloc(sizeof(struct go7007_usb), GFP_KERNEL);
+	if (usb == NULL) {
+		kfree(go);
+		return -ENOMEM;
+	}
 
-	go = go7007_alloc(&board->main_info, &intf->dev);
-	if (go == NULL)
-		goto allocfail;
 	usb->board = board;
 	usb->usbdev = usbdev;
+	usb_make_path(usbdev, go->bus_info, sizeof(go->bus_info));
 	go->board_id = id->driver_info;
 	strncpy(go->name, name, sizeof(go->name));
 	if (board->flags & GO7007_USB_EZUSB)
@@ -1054,6 +1163,15 @@ static int go7007_usb_probe(struct usb_interface *intf,
 	else
 		go->hpi_ops = &go7007_usb_onboard_hpi_ops;
 	go->hpi_context = usb;
+
+	/* Allocate the URB and buffer for receiving incoming interrupts */
+	usb->intr_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (usb->intr_urb == NULL)
+		goto allocfail;
+	usb->intr_urb->transfer_buffer = kmalloc(2*sizeof(u16), GFP_KERNEL);
+	if (usb->intr_urb->transfer_buffer == NULL)
+		goto allocfail;
+
 	if (go->board_id == GO7007_BOARDID_SENSORAY_2250)
 		usb_fill_bulk_urb(usb->intr_urb, usb->usbdev,
 			usb_rcvbulkpipe(usb->usbdev, 4),
@@ -1069,7 +1187,7 @@ static int go7007_usb_probe(struct usb_interface *intf,
 	/* Boot the GO7007 */
 	if (go7007_boot_encoder(go, go->board_info->flags &
 					GO7007_BOARD_USE_ONBOARD_I2C) < 0)
-		goto initfail;
+		goto allocfail;
 
 	/* Register the EZ-USB I2C adapter, if we're using it */
 	if (board->flags & GO7007_USB_EZUSB_I2C) {
@@ -1081,7 +1199,7 @@ static int go7007_usb_probe(struct usb_interface *intf,
 		if (i2c_add_adapter(&go->i2c_adapter) < 0) {
 			printk(KERN_ERR
 				"go7007-usb: error: i2c_add_adapter failed\n");
-			goto initfail;
+			goto allocfail;
 		}
 		go->i2c_adapter_online = 1;
 	}
@@ -1121,34 +1239,36 @@ static int go7007_usb_probe(struct usb_interface *intf,
 					"Adlink PCI-MPG24, channel #%d",
 					channel);
 			}
+			go7007_update_board(go);
 		}
 	}
 
-	/* Probe the tuner model on the TV402U */
-	if (go->board_id == GO7007_BOARDID_PX_TV402U_ANY) {
-		u8 data[3];
+	num_i2c_devs = go->board_info->num_i2c_devs;
 
+	/* Probe the tuner model on the TV402U */
+	if (go->board_id == GO7007_BOARDID_PX_TV402U) {
 		/* Board strapping indicates tuner model */
-		if (go7007_usb_vendor_request(go, 0x41, 0, 0, data, 3, 1) < 0) {
+		if (go7007_usb_vendor_request(go, 0x41, 0, 0, go->usb_buf, 3, 1) < 0) {
 			printk(KERN_ERR "go7007-usb: GPIO read failed!\n");
-			goto initfail;
+			goto allocfail;
 		}
-		switch (data[0] >> 6) {
+		switch (go->usb_buf[0] >> 6) {
 		case 1:
-			go->board_id = GO7007_BOARDID_PX_TV402U_EU;
 			go->tuner_type = TUNER_SONY_BTF_PG472Z;
+			go->std = V4L2_STD_PAL;
 			strncpy(go->name, "Plextor PX-TV402U-EU",
 					sizeof(go->name));
 			break;
 		case 2:
-			go->board_id = GO7007_BOARDID_PX_TV402U_JP;
 			go->tuner_type = TUNER_SONY_BTF_PK467Z;
+			go->std = V4L2_STD_NTSC_M_JP;
+			num_i2c_devs -= 2;
 			strncpy(go->name, "Plextor PX-TV402U-JP",
 					sizeof(go->name));
 			break;
 		case 3:
-			go->board_id = GO7007_BOARDID_PX_TV402U_NA;
 			go->tuner_type = TUNER_SONY_BTF_PB463Z;
+			num_i2c_devs -= 2;
 			strncpy(go->name, "Plextor PX-TV402U-NA",
 					sizeof(go->name));
 			break;
@@ -1162,7 +1282,7 @@ static int go7007_usb_probe(struct usb_interface *intf,
 		if (go7007_usb_vendor_request(go, 0x40, 0x7f02, 0,
 					NULL, 0, 0) < 0) {
 			printk(KERN_ERR "go7007-usb: GPIO write failed!\n");
-			goto initfail;
+			goto allocfail;
 		}
 	}
 
@@ -1176,11 +1296,6 @@ static int go7007_usb_probe(struct usb_interface *intf,
 				"port will result in stream corruption, even "
 				"at low bitrates!\n");
 
-	/* Do any final GO7007 initialization, then register the
-	 * V4L2 and ALSA interfaces */
-	if (go7007_register_encoder(go) < 0)
-		goto initfail;
-
 	/* Allocate the URBs and buffers for receiving the video stream */
 	if (board->flags & GO7007_USB_EZUSB) {
 		v_urb_len = 1024;
@@ -1192,80 +1307,65 @@ static int go7007_usb_probe(struct usb_interface *intf,
 	for (i = 0; i < 8; ++i) {
 		usb->video_urbs[i] = usb_alloc_urb(0, GFP_KERNEL);
 		if (usb->video_urbs[i] == NULL)
-			goto initfail;
+			goto allocfail;
 		usb->video_urbs[i]->transfer_buffer =
 						kmalloc(v_urb_len, GFP_KERNEL);
 		if (usb->video_urbs[i]->transfer_buffer == NULL)
-			goto initfail;
+			goto allocfail;
 		usb_fill_bulk_urb(usb->video_urbs[i], usb->usbdev, video_pipe,
 				usb->video_urbs[i]->transfer_buffer, v_urb_len,
 				go7007_usb_read_video_pipe_complete, go);
 	}
 
 	/* Allocate the URBs and buffers for receiving the audio stream */
-	if ((board->flags & GO7007_USB_EZUSB) && go->audio_enabled)
+	if ((board->flags & GO7007_USB_EZUSB) &&
+	    (board->flags & GO7007_BOARD_HAS_AUDIO)) {
 		for (i = 0; i < 8; ++i) {
 			usb->audio_urbs[i] = usb_alloc_urb(0, GFP_KERNEL);
 			if (usb->audio_urbs[i] == NULL)
-				goto initfail;
+				goto allocfail;
 			usb->audio_urbs[i]->transfer_buffer = kmalloc(4096,
 								GFP_KERNEL);
 			if (usb->audio_urbs[i]->transfer_buffer == NULL)
-				goto initfail;
+				goto allocfail;
 			usb_fill_bulk_urb(usb->audio_urbs[i], usb->usbdev,
 				usb_rcvbulkpipe(usb->usbdev, 8),
 				usb->audio_urbs[i]->transfer_buffer, 4096,
 				go7007_usb_read_audio_pipe_complete, go);
 		}
+	}
 
+	/* Do any final GO7007 initialization, then register the
+	 * V4L2 and ALSA interfaces */
+	if (go7007_register_encoder(go, num_i2c_devs) < 0)
+		goto allocfail;
 
 	go->status = STATUS_ONLINE;
 	return 0;
 
-initfail:
-	go->status = STATUS_SHUTDOWN;
-	return 0;
-
 allocfail:
-	if (usb->intr_urb) {
-		kfree(usb->intr_urb->transfer_buffer);
-		usb_free_urb(usb->intr_urb);
-	}
-	kfree(usb);
+	go7007_usb_release(go);
+	kfree(go);
 	return -ENOMEM;
 }
 
 static void go7007_usb_disconnect(struct usb_interface *intf)
 {
 	struct go7007 *go = to_go7007(usb_get_intfdata(intf));
-	struct go7007_usb *usb = go->hpi_context;
-	struct urb *vurb, *aurb;
-	int i;
 
-	usb_kill_urb(usb->intr_urb);
+	mutex_lock(&go->queue_lock);
+	mutex_lock(&go->serialize_lock);
 
-	/* Free USB-related structs */
-	for (i = 0; i < 8; ++i) {
-		vurb = usb->video_urbs[i];
-		if (vurb) {
-			usb_kill_urb(vurb);
-			kfree(vurb->transfer_buffer);
-			usb_free_urb(vurb);
-		}
-		aurb = usb->audio_urbs[i];
-		if (aurb) {
-			usb_kill_urb(aurb);
-			kfree(aurb->transfer_buffer);
-			usb_free_urb(aurb);
-		}
-	}
-	kfree(usb->intr_urb->transfer_buffer);
-	usb_free_urb(usb->intr_urb);
-
-	kfree(go->hpi_context);
+	if (go->audio_enabled)
+		go7007_snd_remove(go);
 
-	go7007_remove(go);
 	go->status = STATUS_SHUTDOWN;
+	v4l2_device_disconnect(&go->v4l2_dev);
+	video_unregister_device(&go->vdev);
+	mutex_unlock(&go->serialize_lock);
+	mutex_unlock(&go->queue_lock);
+
+	v4l2_device_put(&go->v4l2_dev);
 }
 
 static struct usb_driver go7007_usb_driver = {
diff --git a/drivers/staging/media/go7007/go7007-v4l2.c b/drivers/staging/media/go7007/go7007-v4l2.c
index cb9fe33050c7..50eb69a8ef07 100644
--- a/drivers/staging/media/go7007/go7007-v4l2.c
+++ b/drivers/staging/media/go7007/go7007-v4l2.c
@@ -17,7 +17,6 @@
 
 #include <linux/module.h>
 #include <linux/init.h>
-#include <linux/version.h>
 #include <linux/delay.h>
 #include <linux/sched.h>
 #include <linux/spinlock.h>
@@ -27,115 +26,45 @@
 #include <linux/time.h>
 #include <linux/vmalloc.h>
 #include <linux/pagemap.h>
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <linux/uaccess.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-subdev.h>
-#include <linux/i2c.h>
-#include <linux/mutex.h>
-#include <linux/uaccess.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/saa7115.h>
 
 #include "go7007.h"
 #include "go7007-priv.h"
-#include "wis-i2c.h"
-
-/* Temporary defines until accepted in v4l-dvb */
-#ifndef V4L2_MPEG_STREAM_TYPE_MPEG_ELEM
-#define	V4L2_MPEG_STREAM_TYPE_MPEG_ELEM   6 /* MPEG elementary stream */
-#endif
-#ifndef V4L2_MPEG_VIDEO_ENCODING_MPEG_4
-#define	V4L2_MPEG_VIDEO_ENCODING_MPEG_4   3
-#endif
 
 #define call_all(dev, o, f, args...) \
 	v4l2_device_call_until_err(dev, 0, o, f, ##args)
 
-static void deactivate_buffer(struct go7007_buffer *gobuf)
-{
-	int i;
-
-	if (gobuf->state != BUF_STATE_IDLE) {
-		list_del(&gobuf->stream);
-		gobuf->state = BUF_STATE_IDLE;
-	}
-	if (gobuf->page_count > 0) {
-		for (i = 0; i < gobuf->page_count; ++i)
-			page_cache_release(gobuf->pages[i]);
-		gobuf->page_count = 0;
-	}
-}
-
-static void abort_queued(struct go7007 *go)
-{
-	struct go7007_buffer *gobuf, *next;
-
-	list_for_each_entry_safe(gobuf, next, &go->stream, stream) {
-		deactivate_buffer(gobuf);
-	}
-}
-
-static int go7007_streamoff(struct go7007 *go)
-{
-	unsigned long flags;
-
-	mutex_lock(&go->hw_lock);
-	if (go->streaming) {
-		go->streaming = 0;
-		go7007_stream_stop(go);
-		spin_lock_irqsave(&go->spinlock, flags);
-		abort_queued(go);
-		spin_unlock_irqrestore(&go->spinlock, flags);
-		go7007_reset_encoder(go);
-	}
-	mutex_unlock(&go->hw_lock);
-	return 0;
-}
-
-static int go7007_open(struct file *file)
-{
-	struct go7007 *go = video_get_drvdata(video_devdata(file));
-	struct go7007_file *gofh;
-
-	if (go->status != STATUS_ONLINE)
-		return -EBUSY;
-	gofh = kzalloc(sizeof(struct go7007_file), GFP_KERNEL);
-	if (gofh == NULL)
-		return -ENOMEM;
-	++go->ref_count;
-	gofh->go = go;
-	mutex_init(&gofh->lock);
-	gofh->buf_count = 0;
-	file->private_data = gofh;
-	return 0;
-}
-
-static int go7007_release(struct file *file)
+static bool valid_pixelformat(u32 pixelformat)
 {
-	struct go7007_file *gofh = file->private_data;
-	struct go7007 *go = gofh->go;
-
-	if (gofh->buf_count > 0) {
-		go7007_streamoff(go);
-		go->in_use = 0;
-		kfree(gofh->bufs);
-		gofh->buf_count = 0;
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_MJPEG:
+	case V4L2_PIX_FMT_MPEG1:
+	case V4L2_PIX_FMT_MPEG2:
+	case V4L2_PIX_FMT_MPEG4:
+		return true;
+	default:
+		return false;
 	}
-	kfree(gofh);
-	if (--go->ref_count == 0)
-		kfree(go);
-	file->private_data = NULL;
-	return 0;
 }
 
-static u32 get_frame_type_flag(struct go7007_buffer *gobuf, int format)
+static u32 get_frame_type_flag(struct go7007_buffer *vb, int format)
 {
-	u8 *f = page_address(gobuf->pages[0]);
+	u8 *ptr = vb2_plane_vaddr(&vb->vb, 0);
 
 	switch (format) {
-	case GO7007_FORMAT_MJPEG:
+	case V4L2_PIX_FMT_MJPEG:
 		return V4L2_BUF_FLAG_KEYFRAME;
-	case GO7007_FORMAT_MPEG4:
-		switch ((f[gobuf->frame_offset + 4] >> 6) & 0x3) {
+	case V4L2_PIX_FMT_MPEG4:
+		switch ((ptr[vb->frame_offset + 4] >> 6) & 0x3) {
 		case 0:
 			return V4L2_BUF_FLAG_KEYFRAME;
 		case 1:
@@ -145,9 +74,9 @@ static u32 get_frame_type_flag(struct go7007_buffer *gobuf, int format)
 		default:
 			return 0;
 		}
-	case GO7007_FORMAT_MPEG1:
-	case GO7007_FORMAT_MPEG2:
-		switch ((f[gobuf->frame_offset + 5] >> 3) & 0x7) {
+	case V4L2_PIX_FMT_MPEG1:
+	case V4L2_PIX_FMT_MPEG2:
+		switch ((ptr[vb->frame_offset + 5] >> 3) & 0x7) {
 		case 1:
 			return V4L2_BUF_FLAG_KEYFRAME;
 		case 2:
@@ -162,30 +91,111 @@ static u32 get_frame_type_flag(struct go7007_buffer *gobuf, int format)
 	return 0;
 }
 
-static int set_capture_size(struct go7007 *go, struct v4l2_format *fmt, int try)
+static void get_resolution(struct go7007 *go, int *width, int *height)
 {
-	int sensor_height = 0, sensor_width = 0;
-	int width, height, i;
-
-	if (fmt != NULL && fmt->fmt.pix.pixelformat != V4L2_PIX_FMT_MJPEG &&
-			fmt->fmt.pix.pixelformat != V4L2_PIX_FMT_MPEG &&
-			fmt->fmt.pix.pixelformat != V4L2_PIX_FMT_MPEG4)
-		return -EINVAL;
-
 	switch (go->standard) {
 	case GO7007_STD_NTSC:
-		sensor_width = 720;
-		sensor_height = 480;
+		*width = 720;
+		*height = 480;
 		break;
 	case GO7007_STD_PAL:
-		sensor_width = 720;
-		sensor_height = 576;
+		*width = 720;
+		*height = 576;
 		break;
 	case GO7007_STD_OTHER:
-		sensor_width = go->board_info->sensor_width;
-		sensor_height = go->board_info->sensor_height;
+	default:
+		*width = go->board_info->sensor_width;
+		*height = go->board_info->sensor_height;
+		break;
+	}
+}
+
+static void set_formatting(struct go7007 *go)
+{
+	if (go->format == V4L2_PIX_FMT_MJPEG) {
+		go->pali = 0;
+		go->aspect_ratio = GO7007_RATIO_1_1;
+		go->gop_size = 0;
+		go->ipb = 0;
+		go->closed_gop = 0;
+		go->repeat_seqhead = 0;
+		go->seq_header_enable = 0;
+		go->gop_header_enable = 0;
+		go->dvd_mode = 0;
+		return;
+	}
+
+	switch (go->format) {
+	case V4L2_PIX_FMT_MPEG1:
+		go->pali = 0;
+		break;
+	default:
+	case V4L2_PIX_FMT_MPEG2:
+		go->pali = 0x48;
+		break;
+	case V4L2_PIX_FMT_MPEG4:
+		/* For future reference: this is the list of MPEG4
+		 * profiles that are available, although they are
+		 * untested:
+		 *
+		 * Profile		pali
+		 * --------------	----
+		 * PROFILE_S_L0		0x08
+		 * PROFILE_S_L1		0x01
+		 * PROFILE_S_L2		0x02
+		 * PROFILE_S_L3		0x03
+		 * PROFILE_ARTS_L1	0x91
+		 * PROFILE_ARTS_L2	0x92
+		 * PROFILE_ARTS_L3	0x93
+		 * PROFILE_ARTS_L4	0x94
+		 * PROFILE_AS_L0	0xf0
+		 * PROFILE_AS_L1	0xf1
+		 * PROFILE_AS_L2	0xf2
+		 * PROFILE_AS_L3	0xf3
+		 * PROFILE_AS_L4	0xf4
+		 * PROFILE_AS_L5	0xf5
+		 */
+		go->pali = 0xf5;
+		break;
+	}
+	go->gop_size = v4l2_ctrl_g_ctrl(go->mpeg_video_gop_size);
+	go->closed_gop = v4l2_ctrl_g_ctrl(go->mpeg_video_gop_closure);
+	go->ipb = v4l2_ctrl_g_ctrl(go->mpeg_video_b_frames) != 0;
+	go->bitrate = v4l2_ctrl_g_ctrl(go->mpeg_video_bitrate);
+	go->repeat_seqhead = v4l2_ctrl_g_ctrl(go->mpeg_video_rep_seqheader);
+	go->gop_header_enable = 1;
+	go->dvd_mode = 0;
+	if (go->format == V4L2_PIX_FMT_MPEG2)
+		go->dvd_mode =
+			go->bitrate == 9800000 &&
+			go->gop_size == 15 &&
+			go->ipb == 0 &&
+			go->repeat_seqhead == 1 &&
+			go->closed_gop;
+
+	switch (v4l2_ctrl_g_ctrl(go->mpeg_video_aspect_ratio)) {
+	default:
+	case V4L2_MPEG_VIDEO_ASPECT_1x1:
+		go->aspect_ratio = GO7007_RATIO_1_1;
+		break;
+	case V4L2_MPEG_VIDEO_ASPECT_4x3:
+		go->aspect_ratio = GO7007_RATIO_4_3;
+		break;
+	case V4L2_MPEG_VIDEO_ASPECT_16x9:
+		go->aspect_ratio = GO7007_RATIO_16_9;
 		break;
 	}
+}
+
+static int set_capture_size(struct go7007 *go, struct v4l2_format *fmt, int try)
+{
+	int sensor_height = 0, sensor_width = 0;
+	int width, height, i;
+
+	if (fmt != NULL && !valid_pixelformat(fmt->fmt.pix.pixelformat))
+		return -EINVAL;
+
+	get_resolution(go, &sensor_width, &sensor_height);
 
 	if (fmt == NULL) {
 		width = sensor_width;
@@ -205,13 +215,12 @@ static int set_capture_size(struct go7007 *go, struct v4l2_format *fmt, int try)
 		else
 			height = fmt->fmt.pix.height & ~0x0f;
 	} else {
-		int requested_size = fmt->fmt.pix.width * fmt->fmt.pix.height;
-		int sensor_size = sensor_width * sensor_height;
+		width = fmt->fmt.pix.width;
 
-		if (64 * requested_size < 9 * sensor_size) {
+		if (width <= sensor_width / 4) {
 			width = sensor_width / 4;
 			height = sensor_height / 4;
-		} else if (64 * requested_size < 36 * sensor_size) {
+		} else if (width <= sensor_width / 2) {
 			width = sensor_width / 2;
 			height = sensor_height / 2;
 		} else {
@@ -233,12 +242,14 @@ static int set_capture_size(struct go7007 *go, struct v4l2_format *fmt, int try)
 		fmt->fmt.pix.field = V4L2_FIELD_NONE;
 		fmt->fmt.pix.bytesperline = 0;
 		fmt->fmt.pix.sizeimage = GO7007_BUF_SIZE;
-		fmt->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M; /* ?? */
+		fmt->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 	}
 
 	if (try)
 		return 0;
 
+	if (fmt)
+		go->format = fmt->fmt.pix.pixelformat;
 	go->width = width;
 	go->height = height;
 	go->encoder_h_offset = go->board_info->sensor_h_offset;
@@ -252,18 +263,11 @@ static int set_capture_size(struct go7007 *go, struct v4l2_format *fmt, int try)
 		struct v4l2_mbus_framefmt mbus_fmt;
 
 		mbus_fmt.code = V4L2_MBUS_FMT_FIXED;
-		if (fmt != NULL)
-			mbus_fmt.width = fmt->fmt.pix.width;
-		else
-			mbus_fmt.width = width;
-
-		if (height > sensor_height / 2) {
-			mbus_fmt.height = height / 2;
-			go->encoder_v_halve = 0;
-		} else {
-			mbus_fmt.height = height;
-			go->encoder_v_halve = 1;
-		}
+		mbus_fmt.width = fmt ? fmt->fmt.pix.width : width;
+		mbus_fmt.height = height;
+		go->encoder_h_halve = 0;
+		go->encoder_v_halve = 0;
+		go->encoder_subsample = 0;
 		call_all(&go->v4l2_dev, video, s_mbus_fmt, &mbus_fmt);
 	} else {
 		if (width <= sensor_width / 4) {
@@ -280,55 +284,6 @@ static int set_capture_size(struct go7007 *go, struct v4l2_format *fmt, int try)
 			go->encoder_subsample = 0;
 		}
 	}
-
-	if (fmt == NULL)
-		return 0;
-
-	switch (fmt->fmt.pix.pixelformat) {
-	case V4L2_PIX_FMT_MPEG:
-		if (go->format == GO7007_FORMAT_MPEG1 ||
-				go->format == GO7007_FORMAT_MPEG2 ||
-				go->format == GO7007_FORMAT_MPEG4)
-			break;
-		go->format = GO7007_FORMAT_MPEG1;
-		go->pali = 0;
-		go->aspect_ratio = GO7007_RATIO_1_1;
-		go->gop_size = go->sensor_framerate / 1000;
-		go->ipb = 0;
-		go->closed_gop = 1;
-		go->repeat_seqhead = 1;
-		go->seq_header_enable = 1;
-		go->gop_header_enable = 1;
-		go->dvd_mode = 0;
-		break;
-	/* Backwards compatibility only! */
-	case V4L2_PIX_FMT_MPEG4:
-		if (go->format == GO7007_FORMAT_MPEG4)
-			break;
-		go->format = GO7007_FORMAT_MPEG4;
-		go->pali = 0xf5;
-		go->aspect_ratio = GO7007_RATIO_1_1;
-		go->gop_size = go->sensor_framerate / 1000;
-		go->ipb = 0;
-		go->closed_gop = 1;
-		go->repeat_seqhead = 1;
-		go->seq_header_enable = 1;
-		go->gop_header_enable = 1;
-		go->dvd_mode = 0;
-		break;
-	case V4L2_PIX_FMT_MJPEG:
-		go->format = GO7007_FORMAT_MJPEG;
-		go->pali = 0;
-		go->aspect_ratio = GO7007_RATIO_1_1;
-		go->gop_size = 0;
-		go->ipb = 0;
-		go->closed_gop = 0;
-		go->repeat_seqhead = 0;
-		go->seq_header_enable = 0;
-		go->gop_header_enable = 0;
-		go->dvd_mode = 0;
-		break;
-	}
 	return 0;
 }
 
@@ -390,230 +345,23 @@ static int clip_to_modet_map(struct go7007 *go, int region,
 }
 #endif
 
-static int mpeg_query_ctrl(struct v4l2_queryctrl *ctrl)
-{
-	static const u32 mpeg_ctrls[] = {
-		V4L2_CID_MPEG_CLASS,
-		V4L2_CID_MPEG_STREAM_TYPE,
-		V4L2_CID_MPEG_VIDEO_ENCODING,
-		V4L2_CID_MPEG_VIDEO_ASPECT,
-		V4L2_CID_MPEG_VIDEO_GOP_SIZE,
-		V4L2_CID_MPEG_VIDEO_GOP_CLOSURE,
-		V4L2_CID_MPEG_VIDEO_BITRATE,
-		0
-	};
-	static const u32 *ctrl_classes[] = {
-		mpeg_ctrls,
-		NULL
-	};
-
-	ctrl->id = v4l2_ctrl_next(ctrl_classes, ctrl->id);
-
-	switch (ctrl->id) {
-	case V4L2_CID_MPEG_CLASS:
-		return v4l2_ctrl_query_fill(ctrl, 0, 0, 0, 0);
-	case V4L2_CID_MPEG_STREAM_TYPE:
-		return v4l2_ctrl_query_fill(ctrl,
-				V4L2_MPEG_STREAM_TYPE_MPEG2_DVD,
-				V4L2_MPEG_STREAM_TYPE_MPEG_ELEM, 1,
-				V4L2_MPEG_STREAM_TYPE_MPEG_ELEM);
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		return v4l2_ctrl_query_fill(ctrl,
-				V4L2_MPEG_VIDEO_ENCODING_MPEG_1,
-				V4L2_MPEG_VIDEO_ENCODING_MPEG_4, 1,
-				V4L2_MPEG_VIDEO_ENCODING_MPEG_2);
-	case V4L2_CID_MPEG_VIDEO_ASPECT:
-		return v4l2_ctrl_query_fill(ctrl,
-				V4L2_MPEG_VIDEO_ASPECT_1x1,
-				V4L2_MPEG_VIDEO_ASPECT_16x9, 1,
-				V4L2_MPEG_VIDEO_ASPECT_1x1);
-	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
-		return v4l2_ctrl_query_fill(ctrl, 0, 34, 1, 15);
-	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
-		return v4l2_ctrl_query_fill(ctrl, 0, 1, 1, 0);
-	case V4L2_CID_MPEG_VIDEO_BITRATE:
-		return v4l2_ctrl_query_fill(ctrl,
-				64000,
-				10000000, 1,
-				1500000);
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int mpeg_s_ctrl(struct v4l2_control *ctrl, struct go7007 *go)
-{
-	/* pretty sure we can't change any of these while streaming */
-	if (go->streaming)
-		return -EBUSY;
-
-	switch (ctrl->id) {
-	case V4L2_CID_MPEG_STREAM_TYPE:
-		switch (ctrl->value) {
-		case V4L2_MPEG_STREAM_TYPE_MPEG2_DVD:
-			go->format = GO7007_FORMAT_MPEG2;
-			go->bitrate = 9800000;
-			go->gop_size = 15;
-			go->pali = 0x48;
-			go->closed_gop = 1;
-			go->repeat_seqhead = 0;
-			go->seq_header_enable = 1;
-			go->gop_header_enable = 1;
-			go->dvd_mode = 1;
-			break;
-		case V4L2_MPEG_STREAM_TYPE_MPEG_ELEM:
-			/* todo: */
-			break;
-		default:
-			return -EINVAL;
-		}
-		break;
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		switch (ctrl->value) {
-		case V4L2_MPEG_VIDEO_ENCODING_MPEG_1:
-			go->format = GO7007_FORMAT_MPEG1;
-			go->pali = 0;
-			break;
-		case V4L2_MPEG_VIDEO_ENCODING_MPEG_2:
-			go->format = GO7007_FORMAT_MPEG2;
-			/*if (mpeg->pali >> 24 == 2)
-				go->pali = mpeg->pali & 0xff;
-			else*/
-				go->pali = 0x48;
-			break;
-		case V4L2_MPEG_VIDEO_ENCODING_MPEG_4:
-			go->format = GO7007_FORMAT_MPEG4;
-			/*if (mpeg->pali >> 24 == 4)
-				go->pali = mpeg->pali & 0xff;
-			else*/
-				go->pali = 0xf5;
-			break;
-		default:
-			return -EINVAL;
-		}
-		go->gop_header_enable =
-			/*mpeg->flags & GO7007_MPEG_OMIT_GOP_HEADER
-			? 0 :*/ 1;
-		/*if (mpeg->flags & GO7007_MPEG_REPEAT_SEQHEADER)
-			go->repeat_seqhead = 1;
-		else*/
-			go->repeat_seqhead = 0;
-		go->dvd_mode = 0;
-		break;
-	case V4L2_CID_MPEG_VIDEO_ASPECT:
-		if (go->format == GO7007_FORMAT_MJPEG)
-			return -EINVAL;
-		switch (ctrl->value) {
-		case V4L2_MPEG_VIDEO_ASPECT_1x1:
-			go->aspect_ratio = GO7007_RATIO_1_1;
-			break;
-		case V4L2_MPEG_VIDEO_ASPECT_4x3:
-			go->aspect_ratio = GO7007_RATIO_4_3;
-			break;
-		case V4L2_MPEG_VIDEO_ASPECT_16x9:
-			go->aspect_ratio = GO7007_RATIO_16_9;
-			break;
-		case V4L2_MPEG_VIDEO_ASPECT_221x100:
-		default:
-			return -EINVAL;
-		}
-		break;
-	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
-		if (ctrl->value < 0 || ctrl->value > 34)
-			return -EINVAL;
-		go->gop_size = ctrl->value;
-		break;
-	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
-		if (ctrl->value != 0 && ctrl->value != 1)
-			return -EINVAL;
-		go->closed_gop = ctrl->value;
-		break;
-	case V4L2_CID_MPEG_VIDEO_BITRATE:
-		/* Upper bound is kind of arbitrary here */
-		if (ctrl->value < 64000 || ctrl->value > 10000000)
-			return -EINVAL;
-		go->bitrate = ctrl->value;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int mpeg_g_ctrl(struct v4l2_control *ctrl, struct go7007 *go)
-{
-	switch (ctrl->id) {
-	case V4L2_CID_MPEG_STREAM_TYPE:
-		if (go->dvd_mode)
-			ctrl->value = V4L2_MPEG_STREAM_TYPE_MPEG2_DVD;
-		else
-			ctrl->value = V4L2_MPEG_STREAM_TYPE_MPEG_ELEM;
-		break;
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		switch (go->format) {
-		case GO7007_FORMAT_MPEG1:
-			ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_1;
-			break;
-		case GO7007_FORMAT_MPEG2:
-			ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_2;
-			break;
-		case GO7007_FORMAT_MPEG4:
-			ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_4;
-			break;
-		default:
-			return -EINVAL;
-		}
-		break;
-	case V4L2_CID_MPEG_VIDEO_ASPECT:
-		switch (go->aspect_ratio) {
-		case GO7007_RATIO_1_1:
-			ctrl->value = V4L2_MPEG_VIDEO_ASPECT_1x1;
-			break;
-		case GO7007_RATIO_4_3:
-			ctrl->value = V4L2_MPEG_VIDEO_ASPECT_4x3;
-			break;
-		case GO7007_RATIO_16_9:
-			ctrl->value = V4L2_MPEG_VIDEO_ASPECT_16x9;
-			break;
-		default:
-			return -EINVAL;
-		}
-		break;
-	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
-		ctrl->value = go->gop_size;
-		break;
-	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
-		ctrl->value = go->closed_gop;
-		break;
-	case V4L2_CID_MPEG_VIDEO_BITRATE:
-		ctrl->value = go->bitrate;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
 static int vidioc_querycap(struct file *file, void  *priv,
 					struct v4l2_capability *cap)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
 	strlcpy(cap->driver, "go7007", sizeof(cap->driver));
 	strlcpy(cap->card, go->name, sizeof(cap->card));
-#if 0
-	strlcpy(cap->bus_info, dev_name(&dev->udev->dev), sizeof(cap->bus_info));
-#endif
-
-	cap->version = KERNEL_VERSION(0, 9, 8);
+	strlcpy(cap->bus_info, go->bus_info, sizeof(cap->bus_info));
 
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
-			    V4L2_CAP_STREAMING; /* | V4L2_CAP_AUDIO; */
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
+				V4L2_CAP_STREAMING;
 
+	if (go->board_info->num_aud_inputs)
+		cap->device_caps |= V4L2_CAP_AUDIO;
 	if (go->board_info->flags & GO7007_BOARD_HAS_TUNER)
-		cap->capabilities |= V4L2_CAP_TUNER;
-
+		cap->device_caps |= V4L2_CAP_TUNER;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -625,11 +373,19 @@ static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
 	switch (fmt->index) {
 	case 0:
 		fmt->pixelformat = V4L2_PIX_FMT_MJPEG;
-		desc = "Motion-JPEG";
+		desc = "Motion JPEG";
 		break;
 	case 1:
-		fmt->pixelformat = V4L2_PIX_FMT_MPEG;
-		desc = "MPEG1/MPEG2/MPEG4";
+		fmt->pixelformat = V4L2_PIX_FMT_MPEG1;
+		desc = "MPEG-1 ES";
+		break;
+	case 2:
+		fmt->pixelformat = V4L2_PIX_FMT_MPEG2;
+		desc = "MPEG-2 ES";
+		break;
+	case 3:
+		fmt->pixelformat = V4L2_PIX_FMT_MPEG4;
+		desc = "MPEG-4 ES";
 		break;
 	default:
 		return -EINVAL;
@@ -645,13 +401,12 @@ static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 					struct v4l2_format *fmt)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
 	fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	fmt->fmt.pix.width = go->width;
 	fmt->fmt.pix.height = go->height;
-	fmt->fmt.pix.pixelformat = (go->format == GO7007_FORMAT_MJPEG) ?
-				   V4L2_PIX_FMT_MJPEG : V4L2_PIX_FMT_MPEG;
+	fmt->fmt.pix.pixelformat = go->format;
 	fmt->fmt.pix.field = V4L2_FIELD_NONE;
 	fmt->fmt.pix.bytesperline = 0;
 	fmt->fmt.pix.sizeimage = GO7007_BUF_SIZE;
@@ -663,7 +418,7 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 			struct v4l2_format *fmt)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
 	return set_capture_size(go, fmt, 1);
 }
@@ -671,348 +426,137 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 			struct v4l2_format *fmt)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (go->streaming)
+	if (vb2_is_busy(&go->vidq))
 		return -EBUSY;
 
 	return set_capture_size(go, fmt, 0);
 }
 
-static int vidioc_reqbufs(struct file *file, void *priv,
-			  struct v4l2_requestbuffers *req)
+static int go7007_queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
+		unsigned int *num_buffers, unsigned int *num_planes,
+		unsigned int sizes[], void *alloc_ctxs[])
 {
-	struct go7007_file *gofh = priv;
-	struct go7007 *go = gofh->go;
-	int retval = -EBUSY;
-	unsigned int count, i;
-
-	if (go->streaming)
-		return retval;
-
-	if (req->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-			req->memory != V4L2_MEMORY_MMAP)
-		return -EINVAL;
-
-	mutex_lock(&gofh->lock);
-	for (i = 0; i < gofh->buf_count; ++i)
-		if (gofh->bufs[i].mapped > 0)
-			goto unlock_and_return;
-
-	mutex_lock(&go->hw_lock);
-	if (go->in_use > 0 && gofh->buf_count == 0) {
-		mutex_unlock(&go->hw_lock);
-		goto unlock_and_return;
-	}
+	sizes[0] = GO7007_BUF_SIZE;
+	*num_planes = 1;
 
-	if (gofh->buf_count > 0)
-		kfree(gofh->bufs);
-
-	retval = -ENOMEM;
-	count = req->count;
-	if (count > 0) {
-		if (count < 2)
-			count = 2;
-		if (count > 32)
-			count = 32;
-
-		gofh->bufs = kcalloc(count, sizeof(struct go7007_buffer),
-				     GFP_KERNEL);
-
-		if (!gofh->bufs) {
-			mutex_unlock(&go->hw_lock);
-			goto unlock_and_return;
-		}
-
-		for (i = 0; i < count; ++i) {
-			gofh->bufs[i].go = go;
-			gofh->bufs[i].index = i;
-			gofh->bufs[i].state = BUF_STATE_IDLE;
-			gofh->bufs[i].mapped = 0;
-		}
-
-		go->in_use = 1;
-	} else {
-		go->in_use = 0;
-	}
-
-	gofh->buf_count = count;
-	mutex_unlock(&go->hw_lock);
-	mutex_unlock(&gofh->lock);
-
-	memset(req, 0, sizeof(*req));
-
-	req->count = count;
-	req->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	req->memory = V4L2_MEMORY_MMAP;
-
-	return 0;
-
-unlock_and_return:
-	mutex_unlock(&gofh->lock);
-	return retval;
-}
-
-static int vidioc_querybuf(struct file *file, void *priv,
-			   struct v4l2_buffer *buf)
-{
-	struct go7007_file *gofh = priv;
-	int retval = -EINVAL;
-	unsigned int index;
-
-	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return retval;
-
-	index = buf->index;
-
-	mutex_lock(&gofh->lock);
-	if (index >= gofh->buf_count)
-		goto unlock_and_return;
-
-	memset(buf, 0, sizeof(*buf));
-	buf->index = index;
-	buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	switch (gofh->bufs[index].state) {
-	case BUF_STATE_QUEUED:
-		buf->flags = V4L2_BUF_FLAG_QUEUED;
-		break;
-	case BUF_STATE_DONE:
-		buf->flags = V4L2_BUF_FLAG_DONE;
-		break;
-	default:
-		buf->flags = 0;
-	}
-
-	if (gofh->bufs[index].mapped)
-		buf->flags |= V4L2_BUF_FLAG_MAPPED;
-	buf->memory = V4L2_MEMORY_MMAP;
-	buf->m.offset = index * GO7007_BUF_SIZE;
-	buf->length = GO7007_BUF_SIZE;
-	mutex_unlock(&gofh->lock);
+	if (*num_buffers < 2)
+		*num_buffers = 2;
 
 	return 0;
-
-unlock_and_return:
-	mutex_unlock(&gofh->lock);
-	return retval;
 }
 
-static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+static void go7007_buf_queue(struct vb2_buffer *vb)
 {
-	struct go7007_file *gofh = priv;
-	struct go7007 *go = gofh->go;
-	struct go7007_buffer *gobuf;
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct go7007 *go = vb2_get_drv_priv(vq);
+	struct go7007_buffer *go7007_vb =
+		container_of(vb, struct go7007_buffer, vb);
 	unsigned long flags;
-	int retval = -EINVAL;
-	int ret;
 
-	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-			buf->memory != V4L2_MEMORY_MMAP)
-		return retval;
-
-	mutex_lock(&gofh->lock);
-	if (buf->index >= gofh->buf_count)
-		goto unlock_and_return;
-
-	gobuf = &gofh->bufs[buf->index];
-	if (!gobuf->mapped)
-		goto unlock_and_return;
-
-	retval = -EBUSY;
-	if (gobuf->state != BUF_STATE_IDLE)
-		goto unlock_and_return;
-
-	/* offset will be 0 until we really support USERPTR streaming */
-	gobuf->offset = gobuf->user_addr & ~PAGE_MASK;
-	gobuf->bytesused = 0;
-	gobuf->frame_offset = 0;
-	gobuf->modet_active = 0;
-	if (gobuf->offset > 0)
-		gobuf->page_count = GO7007_BUF_PAGES + 1;
-	else
-		gobuf->page_count = GO7007_BUF_PAGES;
-
-	retval = -ENOMEM;
-	down_read(&current->mm->mmap_sem);
-	ret = get_user_pages(current, current->mm,
-			gobuf->user_addr & PAGE_MASK, gobuf->page_count,
-			1, 1, gobuf->pages, NULL);
-	up_read(&current->mm->mmap_sem);
-
-	if (ret != gobuf->page_count) {
-		int i;
-		for (i = 0; i < ret; ++i)
-			page_cache_release(gobuf->pages[i]);
-		gobuf->page_count = 0;
-		goto unlock_and_return;
-	}
-
-	gobuf->state = BUF_STATE_QUEUED;
 	spin_lock_irqsave(&go->spinlock, flags);
-	list_add_tail(&gobuf->stream, &go->stream);
+	list_add_tail(&go7007_vb->list, &go->vidq_active);
 	spin_unlock_irqrestore(&go->spinlock, flags);
-	mutex_unlock(&gofh->lock);
+}
 
-	return 0;
+static int go7007_buf_prepare(struct vb2_buffer *vb)
+{
+	struct go7007_buffer *go7007_vb =
+		container_of(vb, struct go7007_buffer, vb);
 
-unlock_and_return:
-	mutex_unlock(&gofh->lock);
-	return retval;
+	go7007_vb->modet_active = 0;
+	go7007_vb->frame_offset = 0;
+	vb->v4l2_planes[0].bytesused = 0;
+	return 0;
 }
 
-
-static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+static int go7007_buf_finish(struct vb2_buffer *vb)
 {
-	struct go7007_file *gofh = priv;
-	struct go7007 *go = gofh->go;
-	struct go7007_buffer *gobuf;
-	int retval = -EINVAL;
-	unsigned long flags;
-	u32 frame_type_flag;
-	DEFINE_WAIT(wait);
-
-	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return retval;
-	if (buf->memory != V4L2_MEMORY_MMAP)
-		return retval;
-
-	mutex_lock(&gofh->lock);
-	if (list_empty(&go->stream))
-		goto unlock_and_return;
-	gobuf = list_entry(go->stream.next,
-			struct go7007_buffer, stream);
-
-	retval = -EAGAIN;
-	if (gobuf->state != BUF_STATE_DONE &&
-			!(file->f_flags & O_NONBLOCK)) {
-		for (;;) {
-			prepare_to_wait(&go->frame_waitq, &wait,
-					TASK_INTERRUPTIBLE);
-			if (gobuf->state == BUF_STATE_DONE)
-				break;
-			if (signal_pending(current)) {
-				retval = -ERESTARTSYS;
-				break;
-			}
-			schedule();
-		}
-		finish_wait(&go->frame_waitq, &wait);
-	}
-	if (gobuf->state != BUF_STATE_DONE)
-		goto unlock_and_return;
-
-	spin_lock_irqsave(&go->spinlock, flags);
-	deactivate_buffer(gobuf);
-	spin_unlock_irqrestore(&go->spinlock, flags);
-	frame_type_flag = get_frame_type_flag(gobuf, go->format);
-	gobuf->state = BUF_STATE_IDLE;
-
-	memset(buf, 0, sizeof(*buf));
-	buf->index = gobuf->index;
-	buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	buf->bytesused = gobuf->bytesused;
-	buf->flags = V4L2_BUF_FLAG_MAPPED | frame_type_flag;
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct go7007 *go = vb2_get_drv_priv(vq);
+	struct go7007_buffer *go7007_vb =
+		container_of(vb, struct go7007_buffer, vb);
+	u32 frame_type_flag = get_frame_type_flag(go7007_vb, go->format);
+	struct v4l2_buffer *buf = &vb->v4l2_buf;
+
+	buf->flags &= ~(V4L2_BUF_FLAG_KEYFRAME | V4L2_BUF_FLAG_BFRAME |
+			V4L2_BUF_FLAG_PFRAME);
+	buf->flags |= frame_type_flag;
 	buf->field = V4L2_FIELD_NONE;
-	buf->timestamp = gobuf->timestamp;
-	buf->sequence = gobuf->seq;
-	buf->memory = V4L2_MEMORY_MMAP;
-	buf->m.offset = gobuf->index * GO7007_BUF_SIZE;
-	buf->length = GO7007_BUF_SIZE;
-	buf->reserved = gobuf->modet_active;
-
-	mutex_unlock(&gofh->lock);
 	return 0;
-
-unlock_and_return:
-	mutex_unlock(&gofh->lock);
-	return retval;
 }
 
-static int vidioc_streamon(struct file *file, void *priv,
-					enum v4l2_buf_type type)
+static int go7007_start_streaming(struct vb2_queue *q, unsigned int count)
 {
-	struct go7007_file *gofh = priv;
-	struct go7007 *go = gofh->go;
-	int retval = 0;
-
-	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
+	struct go7007 *go = vb2_get_drv_priv(q);
+	int ret;
 
-	mutex_lock(&gofh->lock);
+	set_formatting(go);
 	mutex_lock(&go->hw_lock);
-
-	if (!go->streaming) {
-		go->streaming = 1;
-		go->next_seq = 0;
-		go->active_buf = NULL;
-		if (go7007_start_encoder(go) < 0)
-			retval = -EIO;
-		else
-			retval = 0;
-	}
+	go->next_seq = 0;
+	go->active_buf = NULL;
+	q->streaming = 1;
+	if (go7007_start_encoder(go) < 0)
+		ret = -EIO;
+	else
+		ret = 0;
 	mutex_unlock(&go->hw_lock);
-	mutex_unlock(&gofh->lock);
+	if (ret) {
+		q->streaming = 0;
+		return ret;
+	}
 	call_all(&go->v4l2_dev, video, s_stream, 1);
-
-	return retval;
+	v4l2_ctrl_grab(go->mpeg_video_gop_size, true);
+	v4l2_ctrl_grab(go->mpeg_video_gop_closure, true);
+	v4l2_ctrl_grab(go->mpeg_video_bitrate, true);
+	v4l2_ctrl_grab(go->mpeg_video_aspect_ratio, true);
+	/* Turn on Capture LED */
+	if (go->board_id == GO7007_BOARDID_ADS_USBAV_709)
+		go7007_write_addr(go, 0x3c82, 0x0005);
+	return ret;
 }
 
-static int vidioc_streamoff(struct file *file, void *priv,
-					enum v4l2_buf_type type)
+static int go7007_stop_streaming(struct vb2_queue *q)
 {
-	struct go7007_file *gofh = priv;
-	struct go7007 *go = gofh->go;
+	struct go7007 *go = vb2_get_drv_priv(q);
+	unsigned long flags;
 
-	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-	mutex_lock(&gofh->lock);
-	go7007_streamoff(go);
-	mutex_unlock(&gofh->lock);
+	q->streaming = 0;
+	go7007_stream_stop(go);
+	mutex_lock(&go->hw_lock);
+	go7007_reset_encoder(go);
+	mutex_unlock(&go->hw_lock);
 	call_all(&go->v4l2_dev, video, s_stream, 0);
 
+	spin_lock_irqsave(&go->spinlock, flags);
+	INIT_LIST_HEAD(&go->vidq_active);
+	spin_unlock_irqrestore(&go->spinlock, flags);
+	v4l2_ctrl_grab(go->mpeg_video_gop_size, false);
+	v4l2_ctrl_grab(go->mpeg_video_gop_closure, false);
+	v4l2_ctrl_grab(go->mpeg_video_bitrate, false);
+	v4l2_ctrl_grab(go->mpeg_video_aspect_ratio, false);
+	/* Turn on Capture LED */
+	if (go->board_id == GO7007_BOARDID_ADS_USBAV_709)
+		go7007_write_addr(go, 0x3c82, 0x000d);
 	return 0;
 }
 
-static int vidioc_queryctrl(struct file *file, void *priv,
-			   struct v4l2_queryctrl *query)
-{
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
-	int id = query->id;
-
-	if (0 == call_all(&go->v4l2_dev, core, queryctrl, query))
-		return 0;
-
-	query->id = id;
-	return mpeg_query_ctrl(query);
-}
-
-static int vidioc_g_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
-{
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
-
-	if (0 == call_all(&go->v4l2_dev, core, g_ctrl, ctrl))
-		return 0;
-
-	return mpeg_g_ctrl(ctrl, go);
-}
-
-static int vidioc_s_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
-{
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
-
-	if (0 == call_all(&go->v4l2_dev, core, s_ctrl, ctrl))
-		return 0;
-
-	return mpeg_s_ctrl(ctrl, go);
-}
+static struct vb2_ops go7007_video_qops = {
+	.queue_setup    = go7007_queue_setup,
+	.buf_queue      = go7007_buf_queue,
+	.buf_prepare    = go7007_buf_prepare,
+	.buf_finish     = go7007_buf_finish,
+	.start_streaming = go7007_start_streaming,
+	.stop_streaming = go7007_stop_streaming,
+	.wait_prepare   = vb2_ops_wait_prepare,
+	.wait_finish    = vb2_ops_wait_finish,
+};
 
 static int vidioc_g_parm(struct file *filp, void *priv,
 		struct v4l2_streamparm *parm)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(filp);
 	struct v4l2_fract timeperframe = {
 		.numerator = 1001 *  go->fps_scale,
 		.denominator = go->sensor_framerate,
@@ -1021,7 +565,8 @@ static int vidioc_g_parm(struct file *filp, void *priv,
 	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
-	parm->parm.capture.capability |= V4L2_CAP_TIMEPERFRAME;
+	parm->parm.capture.readbuffers = 2;
+	parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
 	parm->parm.capture.timeperframe = timeperframe;
 
 	return 0;
@@ -1030,13 +575,11 @@ static int vidioc_g_parm(struct file *filp, void *priv,
 static int vidioc_s_parm(struct file *filp, void *priv,
 		struct v4l2_streamparm *parm)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(filp);
 	unsigned int n, d;
 
 	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
-	if (parm->parm.capture.capturemode != 0)
-		return -EINVAL;
 
 	n = go->sensor_framerate *
 		parm->parm.capture.timeperframe.numerator;
@@ -1046,7 +589,7 @@ static int vidioc_s_parm(struct file *filp, void *priv,
 	else
 		go->fps_scale = 1;
 
-	return 0;
+	return vidioc_g_parm(filp, priv, parm);
 }
 
 /* VIDIOC_ENUMSTD on go7007 were used for enumerating the supported fps and
@@ -1062,121 +605,96 @@ static int vidioc_s_parm(struct file *filp, void *priv,
 static int vidioc_enum_framesizes(struct file *filp, void *priv,
 				  struct v4l2_frmsizeenum *fsize)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(filp);
+	int width, height;
 
-	/* Return -EINVAL, if it is a TV board */
-	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) ||
-	    (go->board_info->sensor_flags & GO7007_SENSOR_TV))
+	if (fsize->index > 2)
 		return -EINVAL;
 
-	if (fsize->index > 0)
+	if (!valid_pixelformat(fsize->pixel_format))
 		return -EINVAL;
 
+	get_resolution(go, &width, &height);
 	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
-	fsize->discrete.width = go->board_info->sensor_width;
-	fsize->discrete.height = go->board_info->sensor_height;
-
+	fsize->discrete.width = (width >> fsize->index) & ~0xf;
+	fsize->discrete.height = (height >> fsize->index) & ~0xf;
 	return 0;
 }
 
 static int vidioc_enum_frameintervals(struct file *filp, void *priv,
 				      struct v4l2_frmivalenum *fival)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(filp);
+	int width, height;
+	int i;
 
-	/* Return -EINVAL, if it is a TV board */
-	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) ||
-	    (go->board_info->sensor_flags & GO7007_SENSOR_TV))
+	if (fival->index > 4)
 		return -EINVAL;
 
-	if (fival->index > 0)
+	if (!valid_pixelformat(fival->pixel_format))
 		return -EINVAL;
 
+	if (!(go->board_info->sensor_flags & GO7007_SENSOR_SCALING)) {
+		get_resolution(go, &width, &height);
+		for (i = 0; i <= 2; i++)
+			if (fival->width == ((width >> i) & ~0xf) &&
+			    fival->height == ((height >> i) & ~0xf))
+				break;
+		if (i > 2)
+			return -EINVAL;
+	}
 	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
-	fival->discrete.numerator = 1001;
-	fival->discrete.denominator = go->board_info->sensor_framerate;
-
+	fival->discrete.numerator = 1001 * (fival->index + 1);
+	fival->discrete.denominator = go->sensor_framerate;
 	return 0;
 }
 
 static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *std)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	switch (go->standard) {
-	case GO7007_STD_NTSC:
-		*std = V4L2_STD_NTSC;
-		break;
-	case GO7007_STD_PAL:
-		*std = V4L2_STD_PAL;
-		break;
-	default:
-		return -EINVAL;
+	*std = go->std;
+	return 0;
+}
+
+static int go7007_s_std(struct go7007 *go)
+{
+	if (go->std & V4L2_STD_625_50) {
+		go->standard = GO7007_STD_PAL;
+		go->sensor_framerate = 25025;
+	} else {
+		go->standard = GO7007_STD_NTSC;
+		go->sensor_framerate = 30000;
 	}
 
+	call_all(&go->v4l2_dev, core, s_std, go->std);
+	set_capture_size(go, NULL, 0);
 	return 0;
 }
 
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *std)
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id std)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (go->streaming)
+	if (vb2_is_busy(&go->vidq))
 		return -EBUSY;
 
-	if (!(go->board_info->sensor_flags & GO7007_SENSOR_TV) && *std != 0)
-		return -EINVAL;
-
-	if (*std == 0)
-		return -EINVAL;
-
-	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
-			go->input == go->board_info->num_inputs - 1) {
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		if (call_all(&go->v4l2_dev, core, s_std, *std) < 0)
-			return -EINVAL;
-	}
-
-	if (*std & V4L2_STD_NTSC) {
-		go->standard = GO7007_STD_NTSC;
-		go->sensor_framerate = 30000;
-	} else if (*std & V4L2_STD_PAL) {
-		go->standard = GO7007_STD_PAL;
-		go->sensor_framerate = 25025;
-	} else if (*std & V4L2_STD_SECAM) {
-		go->standard = GO7007_STD_PAL;
-		go->sensor_framerate = 25025;
-	} else
-		return -EINVAL;
-
-	call_all(&go->v4l2_dev, core, s_std, *std);
-	set_capture_size(go, NULL, 0);
+	go->std = std;
 
-	return 0;
+	return go7007_s_std(go);
 }
 
 static int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *std)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
-
-	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
-			go->input == go->board_info->num_inputs - 1) {
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		return call_all(&go->v4l2_dev, video, querystd, std);
-	} else if (go->board_info->sensor_flags & GO7007_SENSOR_TV)
-		*std = V4L2_STD_NTSC | V4L2_STD_PAL | V4L2_STD_SECAM;
-	else
-		*std = 0;
+	struct go7007 *go = video_drvdata(file);
 
-	return 0;
+	return call_all(&go->v4l2_dev, video, querystd, std);
 }
 
 static int vidioc_enum_input(struct file *file, void *priv,
 				struct v4l2_input *inp)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
 	if (inp->index >= go->board_info->num_inputs)
 		return -EINVAL;
@@ -1184,18 +702,20 @@ static int vidioc_enum_input(struct file *file, void *priv,
 	strncpy(inp->name, go->board_info->inputs[inp->index].name,
 			sizeof(inp->name));
 
-	/* If this board has a tuner, it will be the last input */
+	/* If this board has a tuner, it will be the first input */
 	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
-			inp->index == go->board_info->num_inputs - 1)
+			inp->index == 0)
 		inp->type = V4L2_INPUT_TYPE_TUNER;
 	else
 		inp->type = V4L2_INPUT_TYPE_CAMERA;
 
-	inp->audioset = 0;
+	if (go->board_info->num_aud_inputs)
+		inp->audioset = (1 << go->board_info->num_aud_inputs) - 1;
+	else
+		inp->audioset = 0;
 	inp->tuner = 0;
 	if (go->board_info->sensor_flags & GO7007_SENSOR_TV)
-		inp->std = V4L2_STD_NTSC | V4L2_STD_PAL |
-						V4L2_STD_SECAM;
+		inp->std = video_devdata(file)->tvnorms;
 	else
 		inp->std = 0;
 
@@ -1205,203 +725,128 @@ static int vidioc_enum_input(struct file *file, void *priv,
 
 static int vidioc_g_input(struct file *file, void *priv, unsigned int *input)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
 	*input = go->input;
 
 	return 0;
 }
 
-static int vidioc_s_input(struct file *file, void *priv, unsigned int input)
+static int vidioc_enumaudio(struct file *file, void *fh, struct v4l2_audio *a)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (input >= go->board_info->num_inputs)
+	if (a->index >= go->board_info->num_aud_inputs)
 		return -EINVAL;
-	if (go->streaming)
-		return -EBUSY;
+	strlcpy(a->name, go->board_info->aud_inputs[a->index].name, sizeof(a->name));
+	a->capability = V4L2_AUDCAP_STEREO;
+	return 0;
+}
 
-	go->input = input;
+static int vidioc_g_audio(struct file *file, void *fh, struct v4l2_audio *a)
+{
+	struct go7007 *go = video_drvdata(file);
 
-	return call_all(&go->v4l2_dev, video, s_routing, input, 0, 0);
+	a->index = go->aud_input;
+	strlcpy(a->name, go->board_info->aud_inputs[go->aud_input].name, sizeof(a->name));
+	a->capability = V4L2_AUDCAP_STEREO;
+	return 0;
 }
 
-static int vidioc_g_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
+static int vidioc_s_audio(struct file *file, void *fh, const struct v4l2_audio *a)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
-		return -EINVAL;
-	if (t->index != 0)
+	if (a->index >= go->board_info->num_aud_inputs)
 		return -EINVAL;
-	if (!go->i2c_adapter_online)
-		return -EIO;
-
-	return call_all(&go->v4l2_dev, tuner, g_tuner, t);
+	go->aud_input = a->index;
+	v4l2_subdev_call(go->sd_audio, audio, s_routing,
+			go->board_info->aud_inputs[go->aud_input].audio_input, 0, 0);
+	return 0;
 }
 
-static int vidioc_s_tuner(struct file *file, void *priv,
-				struct v4l2_tuner *t)
+static void go7007_s_input(struct go7007 *go)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	unsigned int input = go->input;
 
-	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
-		return -EINVAL;
-	if (t->index != 0)
-		return -EINVAL;
-	if (!go->i2c_adapter_online)
-		return -EIO;
-
-	switch (go->board_id) {
-	case GO7007_BOARDID_PX_TV402U_NA:
-	case GO7007_BOARDID_PX_TV402U_JP:
-		/* No selectable options currently */
-		if (t->audmode != V4L2_TUNER_MODE_STEREO)
-			return -EINVAL;
-		break;
-	}
+	v4l2_subdev_call(go->sd_video, video, s_routing,
+			go->board_info->inputs[input].video_input, 0,
+			go->board_info->video_config);
+	if (go->board_info->num_aud_inputs) {
+		int aud_input = go->board_info->inputs[input].audio_index;
 
-	return call_all(&go->v4l2_dev, tuner, s_tuner, t);
+		v4l2_subdev_call(go->sd_audio, audio, s_routing,
+			go->board_info->aud_inputs[aud_input].audio_input, 0, 0);
+		go->aud_input = aud_input;
+	}
 }
 
-static int vidioc_g_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+static int vidioc_s_input(struct file *file, void *priv, unsigned int input)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
+	if (input >= go->board_info->num_inputs)
 		return -EINVAL;
-	if (!go->i2c_adapter_online)
-		return -EIO;
+	if (vb2_is_busy(&go->vidq))
+		return -EBUSY;
 
-	f->type = V4L2_TUNER_ANALOG_TV;
+	go->input = input;
+	go7007_s_input(go);
 
-	return call_all(&go->v4l2_dev, tuner, g_frequency, f);
+	return 0;
 }
 
-static int vidioc_s_frequency(struct file *file, void *priv,
-				struct v4l2_frequency *f)
+static int vidioc_g_tuner(struct file *file, void *priv,
+				struct v4l2_tuner *t)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
+	if (t->index != 0)
 		return -EINVAL;
-	if (!go->i2c_adapter_online)
-		return -EIO;
 
-	return call_all(&go->v4l2_dev, tuner, s_frequency, f);
+	strlcpy(t->name, "Tuner", sizeof(t->name));
+	return call_all(&go->v4l2_dev, tuner, g_tuner, t);
 }
 
-static int vidioc_cropcap(struct file *file, void *priv,
-					struct v4l2_cropcap *cropcap)
+static int vidioc_s_tuner(struct file *file, void *priv,
+				const struct v4l2_tuner *t)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (cropcap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (t->index != 0)
 		return -EINVAL;
 
-	/* These specify the raw input of the sensor */
-	switch (go->standard) {
-	case GO7007_STD_NTSC:
-		cropcap->bounds.top = 0;
-		cropcap->bounds.left = 0;
-		cropcap->bounds.width = 720;
-		cropcap->bounds.height = 480;
-		cropcap->defrect.top = 0;
-		cropcap->defrect.left = 0;
-		cropcap->defrect.width = 720;
-		cropcap->defrect.height = 480;
-		break;
-	case GO7007_STD_PAL:
-		cropcap->bounds.top = 0;
-		cropcap->bounds.left = 0;
-		cropcap->bounds.width = 720;
-		cropcap->bounds.height = 576;
-		cropcap->defrect.top = 0;
-		cropcap->defrect.left = 0;
-		cropcap->defrect.width = 720;
-		cropcap->defrect.height = 576;
-		break;
-	case GO7007_STD_OTHER:
-		cropcap->bounds.top = 0;
-		cropcap->bounds.left = 0;
-		cropcap->bounds.width = go->board_info->sensor_width;
-		cropcap->bounds.height = go->board_info->sensor_height;
-		cropcap->defrect.top = 0;
-		cropcap->defrect.left = 0;
-		cropcap->defrect.width = go->board_info->sensor_width;
-		cropcap->defrect.height = go->board_info->sensor_height;
-		break;
-	}
-
-	return 0;
+	return call_all(&go->v4l2_dev, tuner, s_tuner, t);
 }
 
-static int vidioc_g_crop(struct file *file, void *priv, struct v4l2_crop *crop)
+static int vidioc_g_frequency(struct file *file, void *priv,
+				struct v4l2_frequency *f)
 {
-	struct go7007 *go = ((struct go7007_file *) priv)->go;
+	struct go7007 *go = video_drvdata(file);
 
-	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (f->tuner)
 		return -EINVAL;
 
-	crop->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	/* These specify the raw input of the sensor */
-	switch (go->standard) {
-	case GO7007_STD_NTSC:
-		crop->c.top = 0;
-		crop->c.left = 0;
-		crop->c.width = 720;
-		crop->c.height = 480;
-		break;
-	case GO7007_STD_PAL:
-		crop->c.top = 0;
-		crop->c.left = 0;
-		crop->c.width = 720;
-		crop->c.height = 576;
-		break;
-	case GO7007_STD_OTHER:
-		crop->c.top = 0;
-		crop->c.left = 0;
-		crop->c.width = go->board_info->sensor_width;
-		crop->c.height = go->board_info->sensor_height;
-		break;
-	}
-
-	return 0;
+	return call_all(&go->v4l2_dev, tuner, g_frequency, f);
 }
 
-/* FIXME: vidioc_s_crop is not really implemented!!!
- */
-static int vidioc_s_crop(struct file *file, void *priv, const struct v4l2_crop *crop)
+static int vidioc_s_frequency(struct file *file, void *priv,
+				const struct v4l2_frequency *f)
 {
-	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	return 0;
-}
+	struct go7007 *go = video_drvdata(file);
 
-static int vidioc_g_jpegcomp(struct file *file, void *priv,
-			 struct v4l2_jpegcompression *params)
-{
-	memset(params, 0, sizeof(*params));
-	params->quality = 50; /* ?? */
-	params->jpeg_markers = V4L2_JPEG_MARKER_DHT |
-				V4L2_JPEG_MARKER_DQT;
+	if (f->tuner)
+		return -EINVAL;
 
-	return 0;
+	return call_all(&go->v4l2_dev, tuner, s_frequency, f);
 }
 
-static int vidioc_s_jpegcomp(struct file *file, void *priv,
-			 const struct v4l2_jpegcompression *params)
+static int vidioc_log_status(struct file *file, void *priv)
 {
-	if (params->quality != 50 ||
-			params->jpeg_markers != (V4L2_JPEG_MARKER_DHT |
-						V4L2_JPEG_MARKER_DQT))
-		return -EINVAL;
+	struct go7007 *go = video_drvdata(file);
 
-	return 0;
+	v4l2_ctrl_log_status(file, priv);
+	return call_all(&go->v4l2_dev, core, log_status);
 }
 
 /* FIXME:
@@ -1412,180 +857,6 @@ static int vidioc_s_jpegcomp(struct file *file, void *priv,
  */
 
 #if 0
-	/* Temporary ioctls for controlling compression characteristics */
-	case GO7007IOC_S_BITRATE:
-	{
-		int *bitrate = arg;
-
-		if (go->streaming)
-			return -EINVAL;
-		/* Upper bound is kind of arbitrary here */
-		if (*bitrate < 64000 || *bitrate > 10000000)
-			return -EINVAL;
-		go->bitrate = *bitrate;
-		return 0;
-	}
-	case GO7007IOC_G_BITRATE:
-	{
-		int *bitrate = arg;
-
-		*bitrate = go->bitrate;
-		return 0;
-	}
-	case GO7007IOC_S_COMP_PARAMS:
-	{
-		struct go7007_comp_params *comp = arg;
-
-		if (go->format == GO7007_FORMAT_MJPEG)
-			return -EINVAL;
-		if (comp->gop_size > 0)
-			go->gop_size = comp->gop_size;
-		else
-			go->gop_size = go->sensor_framerate / 1000;
-		if (go->gop_size != 15)
-			go->dvd_mode = 0;
-		/*go->ipb = comp->max_b_frames > 0;*/ /* completely untested */
-		if (go->board_info->sensor_flags & GO7007_SENSOR_TV) {
-			switch (comp->aspect_ratio) {
-			case GO7007_ASPECT_RATIO_4_3_NTSC:
-			case GO7007_ASPECT_RATIO_4_3_PAL:
-				go->aspect_ratio = GO7007_RATIO_4_3;
-				break;
-			case GO7007_ASPECT_RATIO_16_9_NTSC:
-			case GO7007_ASPECT_RATIO_16_9_PAL:
-				go->aspect_ratio = GO7007_RATIO_16_9;
-				break;
-			default:
-				go->aspect_ratio = GO7007_RATIO_1_1;
-				break;
-			}
-		}
-		if (comp->flags & GO7007_COMP_OMIT_SEQ_HEADER) {
-			go->dvd_mode = 0;
-			go->seq_header_enable = 0;
-		} else {
-			go->seq_header_enable = 1;
-		}
-		/* fall-through */
-	}
-	case GO7007IOC_G_COMP_PARAMS:
-	{
-		struct go7007_comp_params *comp = arg;
-
-		if (go->format == GO7007_FORMAT_MJPEG)
-			return -EINVAL;
-		memset(comp, 0, sizeof(*comp));
-		comp->gop_size = go->gop_size;
-		comp->max_b_frames = go->ipb ? 2 : 0;
-		switch (go->aspect_ratio) {
-		case GO7007_RATIO_4_3:
-			if (go->standard == GO7007_STD_NTSC)
-				comp->aspect_ratio =
-					GO7007_ASPECT_RATIO_4_3_NTSC;
-			else
-				comp->aspect_ratio =
-					GO7007_ASPECT_RATIO_4_3_PAL;
-			break;
-		case GO7007_RATIO_16_9:
-			if (go->standard == GO7007_STD_NTSC)
-				comp->aspect_ratio =
-					GO7007_ASPECT_RATIO_16_9_NTSC;
-			else
-				comp->aspect_ratio =
-					GO7007_ASPECT_RATIO_16_9_PAL;
-			break;
-		default:
-			comp->aspect_ratio = GO7007_ASPECT_RATIO_1_1;
-			break;
-		}
-		if (go->closed_gop)
-			comp->flags |= GO7007_COMP_CLOSED_GOP;
-		if (!go->seq_header_enable)
-			comp->flags |= GO7007_COMP_OMIT_SEQ_HEADER;
-		return 0;
-	}
-	case GO7007IOC_S_MPEG_PARAMS:
-	{
-		struct go7007_mpeg_params *mpeg = arg;
-
-		if (go->format != GO7007_FORMAT_MPEG1 &&
-				go->format != GO7007_FORMAT_MPEG2 &&
-				go->format != GO7007_FORMAT_MPEG4)
-			return -EINVAL;
-
-		if (mpeg->flags & GO7007_MPEG_FORCE_DVD_MODE) {
-			go->format = GO7007_FORMAT_MPEG2;
-			go->bitrate = 9800000;
-			go->gop_size = 15;
-			go->pali = 0x48;
-			go->closed_gop = 1;
-			go->repeat_seqhead = 0;
-			go->seq_header_enable = 1;
-			go->gop_header_enable = 1;
-			go->dvd_mode = 1;
-		} else {
-			switch (mpeg->mpeg_video_standard) {
-			case GO7007_MPEG_VIDEO_MPEG1:
-				go->format = GO7007_FORMAT_MPEG1;
-				go->pali = 0;
-				break;
-			case GO7007_MPEG_VIDEO_MPEG2:
-				go->format = GO7007_FORMAT_MPEG2;
-				if (mpeg->pali >> 24 == 2)
-					go->pali = mpeg->pali & 0xff;
-				else
-					go->pali = 0x48;
-				break;
-			case GO7007_MPEG_VIDEO_MPEG4:
-				go->format = GO7007_FORMAT_MPEG4;
-				if (mpeg->pali >> 24 == 4)
-					go->pali = mpeg->pali & 0xff;
-				else
-					go->pali = 0xf5;
-				break;
-			default:
-				return -EINVAL;
-			}
-			go->gop_header_enable =
-				mpeg->flags & GO7007_MPEG_OMIT_GOP_HEADER
-				? 0 : 1;
-			if (mpeg->flags & GO7007_MPEG_REPEAT_SEQHEADER)
-				go->repeat_seqhead = 1;
-			else
-				go->repeat_seqhead = 0;
-			go->dvd_mode = 0;
-		}
-		/* fall-through */
-	}
-	case GO7007IOC_G_MPEG_PARAMS:
-	{
-		struct go7007_mpeg_params *mpeg = arg;
-
-		memset(mpeg, 0, sizeof(*mpeg));
-		switch (go->format) {
-		case GO7007_FORMAT_MPEG1:
-			mpeg->mpeg_video_standard = GO7007_MPEG_VIDEO_MPEG1;
-			mpeg->pali = 0;
-			break;
-		case GO7007_FORMAT_MPEG2:
-			mpeg->mpeg_video_standard = GO7007_MPEG_VIDEO_MPEG2;
-			mpeg->pali = GO7007_MPEG_PROFILE(2, go->pali);
-			break;
-		case GO7007_FORMAT_MPEG4:
-			mpeg->mpeg_video_standard = GO7007_MPEG_VIDEO_MPEG4;
-			mpeg->pali = GO7007_MPEG_PROFILE(4, go->pali);
-			break;
-		default:
-			return -EINVAL;
-		}
-		if (!go->gop_header_enable)
-			mpeg->flags |= GO7007_MPEG_OMIT_GOP_HEADER;
-		if (go->repeat_seqhead)
-			mpeg->flags |= GO7007_MPEG_REPEAT_SEQHEADER;
-		if (go->dvd_mode)
-			mpeg->flags |= GO7007_MPEG_FORCE_DVD_MODE;
-		return 0;
-	}
 	case GO7007IOC_S_MD_PARAMS:
 	{
 		struct go7007_md_params *mdp = arg;
@@ -1604,25 +875,6 @@ static int vidioc_s_jpegcomp(struct file *file, void *priv,
 			go->modet[mdp->region].enable = 0;
 		/* fall-through */
 	}
-	case GO7007IOC_G_MD_PARAMS:
-	{
-		struct go7007_md_params *mdp = arg;
-		int region = mdp->region;
-
-		if (mdp->region > 3)
-			return -EINVAL;
-		memset(mdp, 0, sizeof(struct go7007_md_params));
-		mdp->region = region;
-		if (!go->modet[region].enable)
-			return 0;
-		mdp->pixel_threshold =
-			(go->modet[region].pixel_threshold << 1) + 1;
-		mdp->motion_threshold =
-			(go->modet[region].motion_threshold << 1) + 1;
-		mdp->trigger =
-			(go->modet[region].mb_threshold << 1) + 1;
-		return 0;
-	}
 	case GO7007IOC_S_MD_REGION:
 	{
 		struct go7007_md_region *region = arg;
@@ -1633,116 +885,14 @@ static int vidioc_s_jpegcomp(struct file *file, void *priv,
 	}
 #endif
 
-static ssize_t go7007_read(struct file *file, char __user *data,
-		size_t count, loff_t *ppos)
-{
-	return -EINVAL;
-}
-
-static void go7007_vm_open(struct vm_area_struct *vma)
-{
-	struct go7007_buffer *gobuf = vma->vm_private_data;
-
-	++gobuf->mapped;
-}
-
-static void go7007_vm_close(struct vm_area_struct *vma)
-{
-	struct go7007_buffer *gobuf = vma->vm_private_data;
-	unsigned long flags;
-
-	if (--gobuf->mapped == 0) {
-		spin_lock_irqsave(&gobuf->go->spinlock, flags);
-		deactivate_buffer(gobuf);
-		spin_unlock_irqrestore(&gobuf->go->spinlock, flags);
-	}
-}
-
-/* Copied from videobuf-dma-sg.c */
-static int go7007_vm_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
-{
-	struct page *page;
-
-	page = alloc_page(GFP_USER | __GFP_DMA32);
-	if (!page)
-		return VM_FAULT_OOM;
-	clear_user_highpage(page, (unsigned long)vmf->virtual_address);
-	vmf->page = page;
-	return 0;
-}
-
-static struct vm_operations_struct go7007_vm_ops = {
-	.open	= go7007_vm_open,
-	.close	= go7007_vm_close,
-	.fault	= go7007_vm_fault,
-};
-
-static int go7007_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct go7007_file *gofh = file->private_data;
-	unsigned int index;
-
-	if (gofh->go->status != STATUS_ONLINE)
-		return -EIO;
-	if (!(vma->vm_flags & VM_SHARED))
-		return -EINVAL; /* only support VM_SHARED mapping */
-	if (vma->vm_end - vma->vm_start != GO7007_BUF_SIZE)
-		return -EINVAL; /* must map exactly one full buffer */
-	mutex_lock(&gofh->lock);
-	index = vma->vm_pgoff / GO7007_BUF_PAGES;
-	if (index >= gofh->buf_count) {
-		mutex_unlock(&gofh->lock);
-		return -EINVAL; /* trying to map beyond requested buffers */
-	}
-	if (index * GO7007_BUF_PAGES != vma->vm_pgoff) {
-		mutex_unlock(&gofh->lock);
-		return -EINVAL; /* offset is not aligned on buffer boundary */
-	}
-	if (gofh->bufs[index].mapped > 0) {
-		mutex_unlock(&gofh->lock);
-		return -EBUSY;
-	}
-	gofh->bufs[index].mapped = 1;
-	gofh->bufs[index].user_addr = vma->vm_start;
-	vma->vm_ops = &go7007_vm_ops;
-	vma->vm_flags |= VM_DONTEXPAND;
-	vma->vm_flags &= ~VM_IO;
-	vma->vm_private_data = &gofh->bufs[index];
-	mutex_unlock(&gofh->lock);
-	return 0;
-}
-
-static unsigned int go7007_poll(struct file *file, poll_table *wait)
-{
-	struct go7007_file *gofh = file->private_data;
-	struct go7007_buffer *gobuf;
-
-	if (list_empty(&gofh->go->stream))
-		return POLLERR;
-	gobuf = list_entry(gofh->go->stream.next, struct go7007_buffer, stream);
-	poll_wait(file, &gofh->go->frame_waitq, wait);
-	if (gobuf->state == BUF_STATE_DONE)
-		return POLLIN | POLLRDNORM;
-	return 0;
-}
-
-static void go7007_vfl_release(struct video_device *vfd)
-{
-	struct go7007 *go = video_get_drvdata(vfd);
-
-	video_device_release(vfd);
-	if (--go->ref_count == 0)
-		kfree(go);
-}
-
 static struct v4l2_file_operations go7007_fops = {
 	.owner		= THIS_MODULE,
-	.open		= go7007_open,
-	.release	= go7007_release,
-	.ioctl		= video_ioctl2,
-	.read		= go7007_read,
-	.mmap		= go7007_mmap,
-	.poll		= go7007_poll,
+	.open		= v4l2_fh_open,
+	.release	= vb2_fop_release,
+	.unlocked_ioctl	= video_ioctl2,
+	.read		= vb2_fop_read,
+	.mmap		= vb2_fop_mmap,
+	.poll		= vb2_fop_poll,
 };
 
 static const struct v4l2_ioctl_ops video_ioctl_ops = {
@@ -1751,21 +901,21 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_g_fmt_vid_cap     = vidioc_g_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap   = vidioc_try_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap     = vidioc_s_fmt_vid_cap,
-	.vidioc_reqbufs           = vidioc_reqbufs,
-	.vidioc_querybuf          = vidioc_querybuf,
-	.vidioc_qbuf              = vidioc_qbuf,
-	.vidioc_dqbuf             = vidioc_dqbuf,
+	.vidioc_reqbufs           = vb2_ioctl_reqbufs,
+	.vidioc_querybuf          = vb2_ioctl_querybuf,
+	.vidioc_qbuf              = vb2_ioctl_qbuf,
+	.vidioc_dqbuf             = vb2_ioctl_dqbuf,
 	.vidioc_g_std             = vidioc_g_std,
 	.vidioc_s_std             = vidioc_s_std,
 	.vidioc_querystd          = vidioc_querystd,
 	.vidioc_enum_input        = vidioc_enum_input,
 	.vidioc_g_input           = vidioc_g_input,
 	.vidioc_s_input           = vidioc_s_input,
-	.vidioc_queryctrl         = vidioc_queryctrl,
-	.vidioc_g_ctrl            = vidioc_g_ctrl,
-	.vidioc_s_ctrl            = vidioc_s_ctrl,
-	.vidioc_streamon          = vidioc_streamon,
-	.vidioc_streamoff         = vidioc_streamoff,
+	.vidioc_enumaudio         = vidioc_enumaudio,
+	.vidioc_g_audio           = vidioc_g_audio,
+	.vidioc_s_audio           = vidioc_s_audio,
+	.vidioc_streamon          = vb2_ioctl_streamon,
+	.vidioc_streamoff         = vb2_ioctl_streamoff,
 	.vidioc_g_tuner           = vidioc_g_tuner,
 	.vidioc_s_tuner           = vidioc_s_tuner,
 	.vidioc_g_frequency       = vidioc_g_frequency,
@@ -1774,66 +924,129 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_s_parm            = vidioc_s_parm,
 	.vidioc_enum_framesizes   = vidioc_enum_framesizes,
 	.vidioc_enum_frameintervals = vidioc_enum_frameintervals,
-	.vidioc_cropcap           = vidioc_cropcap,
-	.vidioc_g_crop            = vidioc_g_crop,
-	.vidioc_s_crop            = vidioc_s_crop,
-	.vidioc_g_jpegcomp        = vidioc_g_jpegcomp,
-	.vidioc_s_jpegcomp        = vidioc_s_jpegcomp,
+	.vidioc_log_status        = vidioc_log_status,
+	.vidioc_subscribe_event   = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static struct video_device go7007_template = {
 	.name		= "go7007",
 	.fops		= &go7007_fops,
-	.release	= go7007_vfl_release,
+	.release	= video_device_release_empty,
 	.ioctl_ops	= &video_ioctl_ops,
 	.tvnorms	= V4L2_STD_ALL,
-	.current_norm	= V4L2_STD_NTSC,
 };
 
+int go7007_v4l2_ctrl_init(struct go7007 *go)
+{
+	struct v4l2_ctrl_handler *hdl = &go->hdl;
+	struct v4l2_ctrl *ctrl;
+
+	v4l2_ctrl_handler_init(hdl, 13);
+	go->mpeg_video_gop_size = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_MPEG_VIDEO_GOP_SIZE, 0, 34, 1, 15);
+	go->mpeg_video_gop_closure = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_MPEG_VIDEO_GOP_CLOSURE, 0, 1, 1, 1);
+	go->mpeg_video_bitrate = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_MPEG_VIDEO_BITRATE,
+			64000, 10000000, 1, 9800000);
+	go->mpeg_video_b_frames = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_MPEG_VIDEO_B_FRAMES, 0, 2, 2, 0);
+	go->mpeg_video_rep_seqheader = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER, 0, 1, 1, 1);
+
+	go->mpeg_video_aspect_ratio = v4l2_ctrl_new_std_menu(hdl, NULL,
+			V4L2_CID_MPEG_VIDEO_ASPECT,
+			V4L2_MPEG_VIDEO_ASPECT_16x9, 0,
+			V4L2_MPEG_VIDEO_ASPECT_1x1);
+	ctrl = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_JPEG_ACTIVE_MARKER, 0,
+			V4L2_JPEG_ACTIVE_MARKER_DQT | V4L2_JPEG_ACTIVE_MARKER_DHT, 0,
+			V4L2_JPEG_ACTIVE_MARKER_DQT | V4L2_JPEG_ACTIVE_MARKER_DHT);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	if (hdl->error) {
+		int rv = hdl->error;
+
+		v4l2_err(&go->v4l2_dev, "Could not register controls\n");
+		return rv;
+	}
+	go->v4l2_dev.ctrl_handler = hdl;
+	return 0;
+}
+
 int go7007_v4l2_init(struct go7007 *go)
 {
+	struct video_device *vdev = &go->vdev;
 	int rv;
 
-	go->video_dev = video_device_alloc();
-	if (go->video_dev == NULL)
-		return -ENOMEM;
-	*go->video_dev = go7007_template;
-	go->video_dev->parent = go->dev;
-	rv = video_register_device(go->video_dev, VFL_TYPE_GRABBER, -1);
-	if (rv < 0) {
-		video_device_release(go->video_dev);
-		go->video_dev = NULL;
+	mutex_init(&go->serialize_lock);
+	mutex_init(&go->queue_lock);
+
+	INIT_LIST_HEAD(&go->vidq_active);
+	go->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	go->vidq.io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
+	go->vidq.ops = &go7007_video_qops;
+	go->vidq.mem_ops = &vb2_vmalloc_memops;
+	go->vidq.drv_priv = go;
+	go->vidq.buf_struct_size = sizeof(struct go7007_buffer);
+	go->vidq.timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	go->vidq.lock = &go->queue_lock;
+	rv = vb2_queue_init(&go->vidq);
+	if (rv)
 		return rv;
+	*vdev = go7007_template;
+	vdev->lock = &go->serialize_lock;
+	vdev->queue = &go->vidq;
+	set_bit(V4L2_FL_USE_FH_PRIO, &vdev->flags);
+	video_set_drvdata(vdev, go);
+	vdev->v4l2_dev = &go->v4l2_dev;
+	if (!v4l2_device_has_op(&go->v4l2_dev, video, querystd))
+		v4l2_disable_ioctl(vdev, VIDIOC_QUERYSTD);
+	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER)) {
+		v4l2_disable_ioctl(vdev, VIDIOC_S_FREQUENCY);
+		v4l2_disable_ioctl(vdev, VIDIOC_G_FREQUENCY);
+		v4l2_disable_ioctl(vdev, VIDIOC_S_TUNER);
+		v4l2_disable_ioctl(vdev, VIDIOC_G_TUNER);
+	} else {
+		struct v4l2_frequency f = {
+			.type = V4L2_TUNER_ANALOG_TV,
+			.frequency = 980,
+		};
+
+		call_all(&go->v4l2_dev, tuner, s_frequency, &f);
 	}
-	rv = v4l2_device_register(go->dev, &go->v4l2_dev);
-	if (rv < 0) {
-		video_device_release(go->video_dev);
-		go->video_dev = NULL;
-		return rv;
+	if (!(go->board_info->sensor_flags & GO7007_SENSOR_TV)) {
+		v4l2_disable_ioctl(vdev, VIDIOC_G_STD);
+		v4l2_disable_ioctl(vdev, VIDIOC_S_STD);
+		vdev->tvnorms = 0;
+	}
+	if (go->board_info->sensor_flags & GO7007_SENSOR_SCALING)
+		v4l2_disable_ioctl(vdev, VIDIOC_ENUM_FRAMESIZES);
+	if (go->board_info->num_aud_inputs == 0) {
+		v4l2_disable_ioctl(vdev, VIDIOC_G_AUDIO);
+		v4l2_disable_ioctl(vdev, VIDIOC_S_AUDIO);
+		v4l2_disable_ioctl(vdev, VIDIOC_ENUMAUDIO);
 	}
-	video_set_drvdata(go->video_dev, go);
-	++go->ref_count;
+	/* Setup correct crystal frequency on this board */
+	if (go->board_info->sensor_flags & GO7007_SENSOR_SAA7115)
+		v4l2_subdev_call(go->sd_video, video, s_crystal_freq,
+				SAA7115_FREQ_24_576_MHZ,
+				SAA7115_FREQ_FL_APLL | SAA7115_FREQ_FL_UCGC |
+				SAA7115_FREQ_FL_DOUBLE_ASCLK);
+	go7007_s_input(go);
+	if (go->board_info->sensor_flags & GO7007_SENSOR_TV)
+		go7007_s_std(go);
+	rv = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
+	if (rv < 0)
+		return rv;
 	dev_info(go->dev, "registered device %s [v4l2]\n",
-		 video_device_node_name(go->video_dev));
+		 video_device_node_name(vdev));
 
 	return 0;
 }
 
 void go7007_v4l2_remove(struct go7007 *go)
 {
-	unsigned long flags;
-
-	mutex_lock(&go->hw_lock);
-	if (go->streaming) {
-		go->streaming = 0;
-		go7007_stream_stop(go);
-		spin_lock_irqsave(&go->spinlock, flags);
-		abort_queued(go);
-		spin_unlock_irqrestore(&go->spinlock, flags);
-	}
-	mutex_unlock(&go->hw_lock);
-	if (go->video_dev)
-		video_unregister_device(go->video_dev);
-	if (go->status != STATUS_SHUTDOWN)
-		v4l2_device_unregister(&go->v4l2_dev);
+	v4l2_ctrl_handler_free(&go->hdl);
 }
diff --git a/drivers/staging/media/go7007/go7007.h b/drivers/staging/media/go7007/go7007.h
index 7399c915a934..54b989738982 100644
--- a/drivers/staging/media/go7007/go7007.h
+++ b/drivers/staging/media/go7007/go7007.h
@@ -17,72 +17,6 @@
  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  */
 
-/* DEPRECATED -- use V4L2_PIX_FMT_MPEG and then call GO7007IOC_S_MPEG_PARAMS
- * to select between MPEG1, MPEG2, and MPEG4 */
-#define V4L2_PIX_FMT_MPEG4     v4l2_fourcc('M', 'P', 'G', '4') /* MPEG4 */
-
-/* These will be replaced with a better interface
- * soon, so don't get too attached to them */
-#define	GO7007IOC_S_BITRATE	_IOW('V', BASE_VIDIOC_PRIVATE + 0, int)
-#define	GO7007IOC_G_BITRATE	_IOR('V', BASE_VIDIOC_PRIVATE + 1, int)
-
-enum go7007_aspect_ratio {
-	GO7007_ASPECT_RATIO_1_1 = 0,
-	GO7007_ASPECT_RATIO_4_3_NTSC = 1,
-	GO7007_ASPECT_RATIO_4_3_PAL = 2,
-	GO7007_ASPECT_RATIO_16_9_NTSC = 3,
-	GO7007_ASPECT_RATIO_16_9_PAL = 4,
-};
-
-/* Used to set generic compression parameters */
-struct go7007_comp_params {
-	__u32 gop_size;
-	__u32 max_b_frames;
-	enum go7007_aspect_ratio aspect_ratio;
-	__u32 flags;
-	__u32 reserved[8];
-};
-
-#define GO7007_COMP_CLOSED_GOP		0x00000001
-#define GO7007_COMP_OMIT_SEQ_HEADER	0x00000002
-
-enum go7007_mpeg_video_standard {
-	GO7007_MPEG_VIDEO_MPEG1 = 0,
-	GO7007_MPEG_VIDEO_MPEG2 = 1,
-	GO7007_MPEG_VIDEO_MPEG4 = 2,
-};
-
-/* Used to set parameters for V4L2_PIX_FMT_MPEG format */
-struct go7007_mpeg_params {
-	enum go7007_mpeg_video_standard mpeg_video_standard;
-	__u32 flags;
-	__u32 pali;
-	__u32 reserved[8];
-};
-
-#define GO7007_MPEG_FORCE_DVD_MODE	0x00000001
-#define GO7007_MPEG_OMIT_GOP_HEADER	0x00000002
-#define GO7007_MPEG_REPEAT_SEQHEADER	0x00000004
-
-#define GO7007_MPEG_PROFILE(format, pali)	(((format)<<24)|(pali))
-
-#define GO7007_MPEG2_PROFILE_MAIN_MAIN		GO7007_MPEG_PROFILE(2, 0x48)
-
-#define GO7007_MPEG4_PROFILE_S_L0		GO7007_MPEG_PROFILE(4, 0x08)
-#define GO7007_MPEG4_PROFILE_S_L1		GO7007_MPEG_PROFILE(4, 0x01)
-#define GO7007_MPEG4_PROFILE_S_L2		GO7007_MPEG_PROFILE(4, 0x02)
-#define GO7007_MPEG4_PROFILE_S_L3		GO7007_MPEG_PROFILE(4, 0x03)
-#define GO7007_MPEG4_PROFILE_ARTS_L1		GO7007_MPEG_PROFILE(4, 0x91)
-#define GO7007_MPEG4_PROFILE_ARTS_L2		GO7007_MPEG_PROFILE(4, 0x92)
-#define GO7007_MPEG4_PROFILE_ARTS_L3		GO7007_MPEG_PROFILE(4, 0x93)
-#define GO7007_MPEG4_PROFILE_ARTS_L4		GO7007_MPEG_PROFILE(4, 0x94)
-#define GO7007_MPEG4_PROFILE_AS_L0		GO7007_MPEG_PROFILE(4, 0xf0)
-#define GO7007_MPEG4_PROFILE_AS_L1		GO7007_MPEG_PROFILE(4, 0xf1)
-#define GO7007_MPEG4_PROFILE_AS_L2		GO7007_MPEG_PROFILE(4, 0xf2)
-#define GO7007_MPEG4_PROFILE_AS_L3		GO7007_MPEG_PROFILE(4, 0xf3)
-#define GO7007_MPEG4_PROFILE_AS_L4		GO7007_MPEG_PROFILE(4, 0xf4)
-#define GO7007_MPEG4_PROFILE_AS_L5		GO7007_MPEG_PROFILE(4, 0xf5)
-
 struct go7007_md_params {
 	__u16 region;
 	__u16 trigger;
@@ -98,14 +32,6 @@ struct go7007_md_region {
 	__u32 reserved[8];
 };
 
-#define	GO7007IOC_S_MPEG_PARAMS	_IOWR('V', BASE_VIDIOC_PRIVATE + 2, \
-					struct go7007_mpeg_params)
-#define	GO7007IOC_G_MPEG_PARAMS	_IOR('V', BASE_VIDIOC_PRIVATE + 3, \
-					struct go7007_mpeg_params)
-#define	GO7007IOC_S_COMP_PARAMS	_IOWR('V', BASE_VIDIOC_PRIVATE + 4, \
-					struct go7007_comp_params)
-#define	GO7007IOC_G_COMP_PARAMS	_IOR('V', BASE_VIDIOC_PRIVATE + 5, \
-					struct go7007_comp_params)
 #define	GO7007IOC_S_MD_PARAMS	_IOWR('V', BASE_VIDIOC_PRIVATE + 6, \
 					struct go7007_md_params)
 #define	GO7007IOC_G_MD_PARAMS	_IOR('V', BASE_VIDIOC_PRIVATE + 7, \
diff --git a/drivers/staging/media/go7007/s2250-board.c b/drivers/staging/media/go7007/s2250-board.c
index 37400bfa6ccb..beaa98b9c85a 100644
--- a/drivers/staging/media/go7007/s2250-board.c
+++ b/drivers/staging/media/go7007/s2250-board.c
@@ -29,6 +29,13 @@
 MODULE_DESCRIPTION("Sensoray 2250/2251 i2c v4l2 subdev driver");
 MODULE_LICENSE("GPL v2");
 
+/*
+ * Note: this board has two i2c devices: a vpx3226f and a tlv320aic23b.
+ * Due to the unusual way these are accessed on this device we do not
+ * reuse the i2c drivers, but instead they are implemented in this
+ * driver. It would be nice to improve on this, though.
+ */
+
 #define TLV320_ADDRESS      0x34
 #define VPX322_ADDR_ANALOGCONTROL1	0x02
 #define VPX322_ADDR_BRIGHTNESS0		0x0127
@@ -116,6 +123,7 @@ static u16 vid_regs_fp_pal[] = {
 
 struct s2250 {
 	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
 	v4l2_std_id std;
 	int input;
 	int brightness;
@@ -353,127 +361,48 @@ static int s2250_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
 	u16 vidsource;
 
 	vidsource = (state->input == 1) ? 0x040 : 0x020;
-	switch (norm) {
-	case V4L2_STD_NTSC:
-		write_regs_fp(client, vid_regs_fp);
-		write_reg_fp(client, 0x20, vidsource | 1);
-		break;
-	case V4L2_STD_PAL:
+	if (norm & V4L2_STD_625_50) {
 		write_regs_fp(client, vid_regs_fp);
 		write_regs_fp(client, vid_regs_fp_pal);
 		write_reg_fp(client, 0x20, vidsource);
-		break;
-	default:
-		return -EINVAL;
+	} else {
+		write_regs_fp(client, vid_regs_fp);
+		write_reg_fp(client, 0x20, vidsource | 1);
 	}
 	state->std = norm;
 	return 0;
 }
 
-static int s2250_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *query)
+static int s2250_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	switch (query->id) {
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(query, 0, 100, 1, 50);
-	case V4L2_CID_CONTRAST:
-		return v4l2_ctrl_query_fill(query, 0, 100, 1, 50);
-	case V4L2_CID_SATURATION:
-		return v4l2_ctrl_query_fill(query, 0, 100, 1, 50);
-	case V4L2_CID_HUE:
-		return v4l2_ctrl_query_fill(query, -50, 50, 1, 0);
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int s2250_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct s2250 *state = to_state(sd);
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	int value1;
+	struct s2250 *state = container_of(ctrl->handler, struct s2250, hdl);
+	struct i2c_client *client = v4l2_get_subdevdata(&state->sd);
 	u16 oldvalue;
 
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		if (ctrl->value > 100)
-			state->brightness = 100;
-		else if (ctrl->value < 0)
-			state->brightness = 0;
-		else
-			state->brightness = ctrl->value;
-		value1 = (state->brightness - 50) * 255 / 100;
 		read_reg_fp(client, VPX322_ADDR_BRIGHTNESS0, &oldvalue);
 		write_reg_fp(client, VPX322_ADDR_BRIGHTNESS0,
-			     value1 | (oldvalue & ~0xff));
+			     ctrl->val | (oldvalue & ~0xff));
 		read_reg_fp(client, VPX322_ADDR_BRIGHTNESS1, &oldvalue);
 		write_reg_fp(client, VPX322_ADDR_BRIGHTNESS1,
-			     value1 | (oldvalue & ~0xff));
+			     ctrl->val | (oldvalue & ~0xff));
 		write_reg_fp(client, 0x140, 0x60);
 		break;
 	case V4L2_CID_CONTRAST:
-		if (ctrl->value > 100)
-			state->contrast = 100;
-		else if (ctrl->value < 0)
-			state->contrast = 0;
-		else
-			state->contrast = ctrl->value;
-		value1 = state->contrast * 0x40 / 100;
-		if (value1 > 0x3f)
-			value1 = 0x3f; /* max */
 		read_reg_fp(client, VPX322_ADDR_CONTRAST0, &oldvalue);
 		write_reg_fp(client, VPX322_ADDR_CONTRAST0,
-			     value1 | (oldvalue & ~0x3f));
+			     ctrl->val | (oldvalue & ~0x3f));
 		read_reg_fp(client, VPX322_ADDR_CONTRAST1, &oldvalue);
 		write_reg_fp(client, VPX322_ADDR_CONTRAST1,
-			     value1 | (oldvalue & ~0x3f));
+			     ctrl->val | (oldvalue & ~0x3f));
 		write_reg_fp(client, 0x140, 0x60);
 		break;
 	case V4L2_CID_SATURATION:
-		if (ctrl->value > 100)
-			state->saturation = 100;
-		else if (ctrl->value < 0)
-			state->saturation = 0;
-		else
-			state->saturation = ctrl->value;
-		value1 = state->saturation * 4140 / 100;
-		if (value1 > 4094)
-			value1 = 4094;
-		write_reg_fp(client, VPX322_ADDR_SAT, value1);
-		break;
-	case V4L2_CID_HUE:
-		if (ctrl->value > 50)
-			state->hue = 50;
-		else if (ctrl->value < -50)
-			state->hue = -50;
-		else
-			state->hue = ctrl->value;
-		/* clamp the hue range */
-		value1 = state->hue * 280 / 50;
-		write_reg_fp(client, VPX322_ADDR_HUE, value1);
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int s2250_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct s2250 *state = to_state(sd);
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = state->brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = state->contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = state->saturation;
+		write_reg_fp(client, VPX322_ADDR_SAT, ctrl->val);
 		break;
 	case V4L2_CID_HUE:
-		ctrl->value = state->hue;
+		write_reg_fp(client, VPX322_ADDR_HUE, ctrl->val);
 		break;
 	default:
 		return -EINVAL;
@@ -531,24 +460,21 @@ static int s2250_log_status(struct v4l2_subdev *sd)
 	v4l2_info(sd, "Input: %s\n", state->input == 0 ? "Composite" :
 					state->input == 1 ? "S-video" :
 					"error");
-	v4l2_info(sd, "Brightness: %d\n", state->brightness);
-	v4l2_info(sd, "Contrast: %d\n", state->contrast);
-	v4l2_info(sd, "Saturation: %d\n", state->saturation);
-	v4l2_info(sd, "Hue: %d\n", state->hue);
 	v4l2_info(sd, "Audio input: %s\n", state->audio_input == 0 ? "Line In" :
 					state->audio_input == 1 ? "Mic" :
 					state->audio_input == 2 ? "Mic Boost" :
 					"error");
-	return 0;
+	return v4l2_ctrl_subdev_log_status(sd);
 }
 
 /* --------------------------------------------------------------------------*/
 
+static const struct v4l2_ctrl_ops s2250_ctrl_ops = {
+	.s_ctrl = s2250_s_ctrl,
+};
+
 static const struct v4l2_subdev_core_ops s2250_core_ops = {
 	.log_status = s2250_log_status,
-	.g_ctrl = s2250_g_ctrl,
-	.s_ctrl = s2250_s_ctrl,
-	.queryctrl = s2250_queryctrl,
 	.s_std = s2250_s_std,
 };
 
@@ -584,7 +510,7 @@ static int s2250_probe(struct i2c_client *client,
 	if (audio == NULL)
 		return -ENOMEM;
 
-	state = kmalloc(sizeof(struct s2250), GFP_KERNEL);
+	state = kzalloc(sizeof(struct s2250), GFP_KERNEL);
 	if (state == NULL) {
 		i2c_unregister_device(audio);
 		return -ENOMEM;
@@ -596,6 +522,24 @@ static int s2250_probe(struct i2c_client *client,
 	v4l2_info(sd, "initializing %s at address 0x%x on %s\n",
 	       "Sensoray 2250/2251", client->addr, client->adapter->name);
 
+	v4l2_ctrl_handler_init(&state->hdl, 4);
+	v4l2_ctrl_new_std(&state->hdl, &s2250_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(&state->hdl, &s2250_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 0x3f, 1, 0x32);
+	v4l2_ctrl_new_std(&state->hdl, &s2250_ctrl_ops,
+		V4L2_CID_SATURATION, 0, 4094, 1, 2070);
+	v4l2_ctrl_new_std(&state->hdl, &s2250_ctrl_ops,
+		V4L2_CID_HUE, -512, 511, 1, 0);
+	sd->ctrl_handler = &state->hdl;
+	if (state->hdl.error) {
+		int err = state->hdl.error;
+
+		v4l2_ctrl_handler_free(&state->hdl);
+		kfree(state);
+		return err;
+	}
+
 	state->std = V4L2_STD_NTSC;
 	state->brightness = 50;
 	state->contrast = 50;
@@ -606,22 +550,16 @@ static int s2250_probe(struct i2c_client *client,
 	/* initialize the audio */
 	if (write_regs(audio, aud_regs) < 0) {
 		dev_err(&client->dev, "error initializing audio\n");
-		i2c_unregister_device(audio);
-		kfree(state);
-		return 0;
+		goto fail;
 	}
 
 	if (write_regs(client, vid_regs) < 0) {
 		dev_err(&client->dev, "error initializing decoder\n");
-		i2c_unregister_device(audio);
-		kfree(state);
-		return 0;
+		goto fail;
 	}
 	if (write_regs_fp(client, vid_regs_fp) < 0) {
 		dev_err(&client->dev, "error initializing decoder\n");
-		i2c_unregister_device(audio);
-		kfree(state);
-		return 0;
+		goto fail;
 	}
 	/* set default channel */
 	/* composite */
@@ -657,14 +595,21 @@ static int s2250_probe(struct i2c_client *client,
 
 	v4l2_info(sd, "initialized successfully\n");
 	return 0;
+
+fail:
+	i2c_unregister_device(audio);
+	v4l2_ctrl_handler_free(&state->hdl);
+	kfree(state);
+	return -EIO;
 }
 
 static int s2250_remove(struct i2c_client *client)
 {
-	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct s2250 *state = to_state(i2c_get_clientdata(client));
 
-	v4l2_device_unregister_subdev(sd);
-	kfree(to_state(sd));
+	v4l2_device_unregister_subdev(&state->sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+	kfree(state);
 	return 0;
 }
 
diff --git a/drivers/staging/media/go7007/s2250-loader.c b/drivers/staging/media/go7007/s2250-loader.c
deleted file mode 100644
index 72e5175fe7e3..000000000000
--- a/drivers/staging/media/go7007/s2250-loader.c
+++ /dev/null
@@ -1,169 +0,0 @@
-/*
- * Copyright (C) 2008 Sensoray Company Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License (Version 2) as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software Foundation,
- * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/slab.h>
-#include <linux/usb.h>
-#include <dvb-usb.h>
-
-#define S2250_LOADER_FIRMWARE	"s2250_loader.fw"
-#define S2250_FIRMWARE		"s2250.fw"
-
-typedef struct device_extension_s {
-    struct kref     kref;
-    int minor;
-    struct usb_device *usbdev;
-} device_extension_t, *pdevice_extension_t;
-
-#define USB_s2250loader_MAJOR 240
-#define USB_s2250loader_MINOR_BASE 0
-#define MAX_DEVICES 256
-
-static pdevice_extension_t s2250_dev_table[MAX_DEVICES];
-static DEFINE_MUTEX(s2250_dev_table_mutex);
-
-#define to_s2250loader_dev_common(d) container_of(d, device_extension_t, kref)
-static void s2250loader_delete(struct kref *kref)
-{
-	pdevice_extension_t s = to_s2250loader_dev_common(kref);
-	s2250_dev_table[s->minor] = NULL;
-	kfree(s);
-}
-
-static int s2250loader_probe(struct usb_interface *interface,
-				const struct usb_device_id *id)
-{
-	struct usb_device *usbdev;
-	int minor, ret;
-	pdevice_extension_t s = NULL;
-	const struct firmware *fw;
-
-	usbdev = usb_get_dev(interface_to_usbdev(interface));
-	if (!usbdev) {
-		dev_err(&interface->dev, "Enter s2250loader_probe failed\n");
-		return -1;
-	}
-	dev_info(&interface->dev, "Enter s2250loader_probe 2.6 kernel\n");
-	dev_info(&interface->dev, "vendor id 0x%x, device id 0x%x devnum:%d\n",
-		 usbdev->descriptor.idVendor, usbdev->descriptor.idProduct,
-		 usbdev->devnum);
-
-	if (usbdev->descriptor.bNumConfigurations != 1) {
-		dev_err(&interface->dev, "can't handle multiple config\n");
-		return -1;
-	}
-	mutex_lock(&s2250_dev_table_mutex);
-
-	for (minor = 0; minor < MAX_DEVICES; minor++) {
-		if (s2250_dev_table[minor] == NULL)
-			break;
-	}
-
-	if (minor < 0 || minor >= MAX_DEVICES) {
-		dev_err(&interface->dev, "Invalid minor: %d\n", minor);
-		goto failed;
-	}
-
-	/* Allocate dev data structure */
-	s = kmalloc(sizeof(device_extension_t), GFP_KERNEL);
-	if (s == NULL)
-		goto failed;
-
-	s2250_dev_table[minor] = s;
-
-	dev_info(&interface->dev,
-		 "s2250loader_probe: Device %d on Bus %d Minor %d\n",
-		 usbdev->devnum, usbdev->bus->busnum, minor);
-
-	memset(s, 0, sizeof(device_extension_t));
-	s->usbdev = usbdev;
-	dev_info(&interface->dev, "loading 2250 loader\n");
-
-	kref_init(&(s->kref));
-
-	mutex_unlock(&s2250_dev_table_mutex);
-
-	if (request_firmware(&fw, S2250_LOADER_FIRMWARE, &usbdev->dev)) {
-		dev_err(&interface->dev,
-			"s2250: unable to load firmware from file \"%s\"\n",
-			S2250_LOADER_FIRMWARE);
-		goto failed2;
-	}
-	ret = usb_cypress_load_firmware(usbdev, fw, CYPRESS_FX2);
-	release_firmware(fw);
-	if (0 != ret) {
-		dev_err(&interface->dev, "loader download failed\n");
-		goto failed2;
-	}
-
-	if (request_firmware(&fw, S2250_FIRMWARE, &usbdev->dev)) {
-		dev_err(&interface->dev,
-			"s2250: unable to load firmware from file \"%s\"\n",
-			S2250_FIRMWARE);
-		goto failed2;
-	}
-	ret = usb_cypress_load_firmware(usbdev, fw, CYPRESS_FX2);
-	release_firmware(fw);
-	if (0 != ret) {
-		dev_err(&interface->dev, "firmware_s2250 download failed\n");
-		goto failed2;
-	}
-
-	usb_set_intfdata(interface, s);
-	return 0;
-
-failed:
-	mutex_unlock(&s2250_dev_table_mutex);
-failed2:
-	if (s)
-		kref_put(&(s->kref), s2250loader_delete);
-
-	dev_err(&interface->dev, "probe failed\n");
-	return -1;
-}
-
-static void s2250loader_disconnect(struct usb_interface *interface)
-{
-	pdevice_extension_t s;
-	dev_info(&interface->dev, "s2250: disconnect\n");
-	s = usb_get_intfdata(interface);
-	usb_set_intfdata(interface, NULL);
-	kref_put(&(s->kref), s2250loader_delete);
-}
-
-static const struct usb_device_id s2250loader_ids[] = {
-	{USB_DEVICE(0x1943, 0xa250)},
-	{}                          /* Terminating entry */
-};
-
-MODULE_DEVICE_TABLE(usb, s2250loader_ids);
-
-static struct usb_driver s2250loader_driver = {
-	.name		= "s2250-loader",
-	.probe		= s2250loader_probe,
-	.disconnect	= s2250loader_disconnect,
-	.id_table	= s2250loader_ids,
-};
-
-module_usb_driver(s2250loader_driver);
-
-MODULE_AUTHOR("");
-MODULE_DESCRIPTION("firmware loader for Sensoray 2250/2251");
-MODULE_LICENSE("GPL v2");
-MODULE_FIRMWARE(S2250_LOADER_FIRMWARE);
-MODULE_FIRMWARE(S2250_FIRMWARE);
diff --git a/drivers/staging/media/go7007/s2250-loader.h b/drivers/staging/media/go7007/s2250-loader.h
deleted file mode 100644
index b7c301af16cc..000000000000
--- a/drivers/staging/media/go7007/s2250-loader.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*
- * Copyright (C) 2005-2006 Micronas USA Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License (Version 2) as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software Foundation,
- * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
- */
-
-#ifndef _S2250_LOADER_H_
-#define _S2250_LOADER_H_
-
-extern int s2250loader_init(void);
-extern void s2250loader_cleanup(void);
-
-#endif
diff --git a/drivers/staging/media/go7007/saa7134-go7007.c b/drivers/staging/media/go7007/saa7134-go7007.c
index cf7c34a99459..d80b235d72ee 100644
--- a/drivers/staging/media/go7007/saa7134-go7007.c
+++ b/drivers/staging/media/go7007/saa7134-go7007.c
@@ -28,12 +28,15 @@
 #include <linux/i2c.h>
 #include <asm/byteorder.h>
 #include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
 
-#include "saa7134-reg.h"
 #include "saa7134.h"
+#include "saa7134-reg.h"
+#include "go7007.h"
 #include "go7007-priv.h"
 
-#define GO7007_HPI_DEBUG
+/*#define GO7007_HPI_DEBUG*/
 
 enum hpi_address {
 	HPI_ADDR_VIDEO_BUFFER = 0xe4,
@@ -57,6 +60,7 @@ enum gpio_command {
 };
 
 struct saa7134_go7007 {
+	struct v4l2_subdev sd;
 	struct saa7134_dev *dev;
 	u8 *top;
 	u8 *bottom;
@@ -64,8 +68,12 @@ struct saa7134_go7007 {
 	dma_addr_t bottom_dma;
 };
 
-static struct go7007_board_info board_voyager = {
-	.firmware	 = "go7007tv.bin",
+static inline struct saa7134_go7007 *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct saa7134_go7007, sd);
+}
+
+static const struct go7007_board_info board_voyager = {
 	.flags		 = 0,
 	.sensor_flags	 = GO7007_SENSOR_656 |
 				GO7007_SENSOR_VALID_ENABLE |
@@ -84,7 +92,6 @@ static struct go7007_board_info board_voyager = {
 		},
 	},
 };
-MODULE_FIRMWARE("go7007tv.bin");
 
 /********************* Driver for GPIO HPI interface *********************/
 
@@ -236,7 +243,7 @@ static void saa7134_go7007_irq_ts_done(struct saa7134_dev *dev,
 	struct go7007 *go = video_get_drvdata(dev->empress_dev);
 	struct saa7134_go7007 *saa = go->hpi_context;
 
-	if (!go->streaming)
+	if (!vb2_is_streaming(&go->vidq))
 		return;
 	if (0 != (status & 0x000f0000))
 		printk(KERN_DEBUG "saa7134-go7007: irq: lost %ld\n",
@@ -261,12 +268,12 @@ static int saa7134_go7007_stream_start(struct go7007 *go)
 
 	saa->top_dma = dma_map_page(&dev->pci->dev, virt_to_page(saa->top),
 			0, PAGE_SIZE, DMA_FROM_DEVICE);
-	if (!saa->top_dma)
+	if (dma_mapping_error(&dev->pci->dev, saa->top_dma))
 		return -ENOMEM;
 	saa->bottom_dma = dma_map_page(&dev->pci->dev,
 			virt_to_page(saa->bottom),
 			0, PAGE_SIZE, DMA_FROM_DEVICE);
-	if (!saa->bottom_dma) {
+	if (dma_mapping_error(&dev->pci->dev, saa->bottom_dma)) {
 		dma_unmap_page(&dev->pci->dev, saa->top_dma, PAGE_SIZE,
 				DMA_FROM_DEVICE);
 		return -ENOMEM;
@@ -380,47 +387,6 @@ static int saa7134_go7007_send_firmware(struct go7007 *go, u8 *data, int len)
 	return 0;
 }
 
-static int saa7134_go7007_send_command(struct go7007 *go, unsigned int cmd,
-					void *arg)
-{
-	struct saa7134_go7007 *saa = go->hpi_context;
-	struct saa7134_dev *dev = saa->dev;
-
-	switch (cmd) {
-	case VIDIOC_S_STD:
-	{
-		v4l2_std_id *std = arg;
-		return saa7134_s_std_internal(dev, NULL, std);
-	}
-	case VIDIOC_G_STD:
-	{
-		v4l2_std_id *std = arg;
-		*std = dev->tvnorm->id;
-		return 0;
-	}
-	case VIDIOC_QUERYCTRL:
-	{
-		struct v4l2_queryctrl *ctrl = arg;
-		if (V4L2_CTRL_ID2CLASS(ctrl->id) == V4L2_CTRL_CLASS_USER)
-			return saa7134_queryctrl(NULL, NULL, ctrl);
-	}
-	case VIDIOC_G_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-		if (V4L2_CTRL_ID2CLASS(ctrl->id) == V4L2_CTRL_CLASS_USER)
-			return saa7134_g_ctrl_internal(dev, NULL, ctrl);
-	}
-	case VIDIOC_S_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-		if (V4L2_CTRL_ID2CLASS(ctrl->id) == V4L2_CTRL_CLASS_USER)
-			return saa7134_s_ctrl_internal(dev, NULL, ctrl);
-	}
-	}
-	return -EINVAL;
-
-}
-
 static struct go7007_hpi_ops saa7134_go7007_hpi_ops = {
 	.interface_reset	= saa7134_go7007_interface_reset,
 	.write_interrupt	= saa7134_go7007_write_interrupt,
@@ -428,21 +394,88 @@ static struct go7007_hpi_ops saa7134_go7007_hpi_ops = {
 	.stream_start		= saa7134_go7007_stream_start,
 	.stream_stop		= saa7134_go7007_stream_stop,
 	.send_firmware		= saa7134_go7007_send_firmware,
-	.send_command		= saa7134_go7007_send_command,
+};
+MODULE_FIRMWARE("go7007/go7007tv.bin");
+
+/* --------------------------------------------------------------------------*/
+
+static int saa7134_go7007_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	struct saa7134_go7007 *saa = to_state(sd);
+	struct saa7134_dev *dev = saa->dev;
+
+	return saa7134_s_std_internal(dev, NULL, norm);
+}
+
+static int saa7134_go7007_queryctrl(struct v4l2_subdev *sd,
+				    struct v4l2_queryctrl *query)
+{
+	return saa7134_queryctrl(NULL, NULL, query);
+}
+static int saa7134_go7007_s_ctrl(struct v4l2_subdev *sd,
+				 struct v4l2_control *ctrl)
+{
+	struct saa7134_go7007 *saa = to_state(sd);
+	struct saa7134_dev *dev = saa->dev;
+	return saa7134_s_ctrl_internal(dev, NULL, ctrl);
+}
+
+static int saa7134_go7007_g_ctrl(struct v4l2_subdev *sd,
+				 struct v4l2_control *ctrl)
+{
+	struct saa7134_go7007 *saa = to_state(sd);
+	struct saa7134_dev *dev = saa->dev;
+	return saa7134_g_ctrl_internal(dev, NULL, ctrl);
+}
+
+/* --------------------------------------------------------------------------*/
+
+static const struct v4l2_subdev_core_ops saa7134_go7007_core_ops = {
+	.g_ctrl = saa7134_go7007_g_ctrl,
+	.s_ctrl = saa7134_go7007_s_ctrl,
+	.queryctrl = saa7134_go7007_queryctrl,
+	.s_std = saa7134_go7007_s_std,
+};
+
+static const struct v4l2_subdev_ops saa7134_go7007_sd_ops = {
+	.core = &saa7134_go7007_core_ops,
 };
 
+/* --------------------------------------------------------------------------*/
+
+
 /********************* Add/remove functions *********************/
 
 static int saa7134_go7007_init(struct saa7134_dev *dev)
 {
 	struct go7007 *go;
 	struct saa7134_go7007 *saa;
+	struct v4l2_subdev *sd;
 
 	printk(KERN_DEBUG "saa7134-go7007: probing new SAA713X board\n");
 
+	go = go7007_alloc(&board_voyager, &dev->pci->dev);
+	if (go == NULL)
+		return -ENOMEM;
+
 	saa = kzalloc(sizeof(struct saa7134_go7007), GFP_KERNEL);
-	if (saa == NULL)
+	if (saa == NULL) {
+		kfree(go);
 		return -ENOMEM;
+	}
+
+	go->board_id = GO7007_BOARDID_PCI_VOYAGER;
+	snprintf(go->bus_info, sizeof(go->bus_info), "PCI:%s", pci_name(dev->pci));
+	strlcpy(go->name, saa7134_boards[dev->board].name, sizeof(go->name));
+	go->hpi_ops = &saa7134_go7007_hpi_ops;
+	go->hpi_context = saa;
+	saa->dev = dev;
+
+	/* Init the subdevice interface */
+	sd = &saa->sd;
+	v4l2_subdev_init(sd, &saa7134_go7007_sd_ops);
+	v4l2_set_subdevdata(sd, saa);
+	strncpy(sd->name, "saa7134-go7007", sizeof(sd->name));
 
 	/* Allocate a couple pages for receiving the compressed stream */
 	saa->top = (u8 *)get_zeroed_page(GFP_KERNEL);
@@ -452,32 +485,23 @@ static int saa7134_go7007_init(struct saa7134_dev *dev)
 	if (!saa->bottom)
 		goto allocfail;
 
-	go = go7007_alloc(&board_voyager, &dev->pci->dev);
-	if (go == NULL)
-		goto allocfail;
-	go->board_id = GO7007_BOARDID_PCI_VOYAGER;
-	strncpy(go->name, saa7134_boards[dev->board].name, sizeof(go->name));
-	go->hpi_ops = &saa7134_go7007_hpi_ops;
-	go->hpi_context = saa;
-	saa->dev = dev;
-
 	/* Boot the GO7007 */
 	if (go7007_boot_encoder(go, go->board_info->flags &
 					GO7007_BOARD_USE_ONBOARD_I2C) < 0)
-		goto initfail;
+		goto allocfail;
 
 	/* Do any final GO7007 initialization, then register the
 	 * V4L2 and ALSA interfaces */
-	if (go7007_register_encoder(go) < 0)
-		goto initfail;
-	dev->empress_dev = go->video_dev;
-	video_set_drvdata(dev->empress_dev, go);
+	if (go7007_register_encoder(go, go->board_info->num_i2c_devs) < 0)
+		goto allocfail;
 
-	go->status = STATUS_ONLINE;
-	return 0;
+	/* Register the subdevice interface with the go7007 device */
+	if (v4l2_device_register_subdev(&go->v4l2_dev, sd) < 0)
+		printk(KERN_INFO "saa7134-go7007: register subdev failed\n");
 
-initfail:
-	go->status = STATUS_SHUTDOWN;
+	dev->empress_dev = &go->vdev;
+
+	go->status = STATUS_ONLINE;
 	return 0;
 
 allocfail:
@@ -486,6 +510,7 @@ allocfail:
 	if (saa->bottom)
 		free_page((unsigned long)saa->bottom);
 	kfree(saa);
+	kfree(go);
 	return -ENOMEM;
 }
 
@@ -498,12 +523,18 @@ static int saa7134_go7007_fini(struct saa7134_dev *dev)
 		return 0;
 
 	go = video_get_drvdata(dev->empress_dev);
+	if (go->audio_enabled)
+		go7007_snd_remove(go);
+
 	saa = go->hpi_context;
 	go->status = STATUS_SHUTDOWN;
 	free_page((unsigned long)saa->top);
 	free_page((unsigned long)saa->bottom);
+	v4l2_device_unregister_subdev(&saa->sd);
 	kfree(saa);
-	go7007_remove(go);
+	video_unregister_device(&go->vdev);
+
+	v4l2_device_put(&go->v4l2_dev);
 	dev->empress_dev = NULL;
 
 	return 0;
diff --git a/drivers/staging/media/go7007/snd-go7007.c b/drivers/staging/media/go7007/snd-go7007.c
index 5af29ff68bfd..4be0fa40a39a 100644
--- a/drivers/staging/media/go7007/snd-go7007.c
+++ b/drivers/staging/media/go7007/snd-go7007.c
@@ -221,8 +221,6 @@ static int go7007_snd_free(struct snd_device *device)
 
 	kfree(go->snd_context);
 	go->snd_context = NULL;
-	if (--go->ref_count == 0)
-		kfree(go);
 	return 0;
 }
 
@@ -267,9 +265,9 @@ int go7007_snd_init(struct go7007 *go)
 		kfree(gosnd);
 		return ret;
 	}
-	strncpy(gosnd->card->driver, "go7007", sizeof(gosnd->card->driver));
-	strncpy(gosnd->card->shortname, go->name, sizeof(gosnd->card->driver));
-	strncpy(gosnd->card->longname, gosnd->card->shortname,
+	strlcpy(gosnd->card->driver, "go7007", sizeof(gosnd->card->driver));
+	strlcpy(gosnd->card->shortname, go->name, sizeof(gosnd->card->driver));
+	strlcpy(gosnd->card->longname, gosnd->card->shortname,
 			sizeof(gosnd->card->longname));
 
 	gosnd->pcm->private_data = go;
@@ -285,8 +283,8 @@ int go7007_snd_init(struct go7007 *go)
 
 	gosnd->substream = NULL;
 	go->snd_context = gosnd;
+	v4l2_device_get(&go->v4l2_dev);
 	++dev;
-	++go->ref_count;
 
 	return 0;
 }
@@ -298,6 +296,7 @@ int go7007_snd_remove(struct go7007 *go)
 
 	snd_card_disconnect(gosnd->card);
 	snd_card_free_when_closed(gosnd->card);
+	v4l2_device_put(&go->v4l2_dev);
 	return 0;
 }
 EXPORT_SYMBOL(go7007_snd_remove);
diff --git a/drivers/staging/media/go7007/wis-i2c.h b/drivers/staging/media/go7007/wis-i2c.h
deleted file mode 100644
index 6d09c06c8560..000000000000
--- a/drivers/staging/media/go7007/wis-i2c.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * Copyright (C) 2005-2006 Micronas USA Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License (Version 2) as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software Foundation,
- * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
- */
-
-/* Temporary I2C IDs -- these need to be replaced with real registered IDs */
-#define	I2C_DRIVERID_WIS_SAA7115	0xf0f0
-#define	I2C_DRIVERID_WIS_UDA1342	0xf0f1
-#define	I2C_DRIVERID_WIS_SONY_TUNER	0xf0f2
-#define	I2C_DRIVERID_WIS_TW9903		0xf0f3
-#define	I2C_DRIVERID_WIS_SAA7113	0xf0f4
-#define	I2C_DRIVERID_WIS_OV7640		0xf0f5
-#define	I2C_DRIVERID_WIS_TW2804		0xf0f6
-#define	I2C_DRIVERID_S2250		0xf0f7
-
-/* Definitions for new video decoder commands */
-
-struct video_decoder_resolution {
-	unsigned int width;
-	unsigned int height;
-};
-
-#define	DECODER_SET_RESOLUTION	_IOW('d', 200, struct video_decoder_resolution)
-#define	DECODER_SET_CHANNEL	_IOW('d', 201, int)
-
-/* Sony tuner types */
-
-#define TUNER_SONY_BTF_PG472Z		200
-#define TUNER_SONY_BTF_PK467Z		201
-#define TUNER_SONY_BTF_PB463Z		202
diff --git a/drivers/staging/media/go7007/wis-saa7113.c b/drivers/staging/media/go7007/wis-saa7113.c
deleted file mode 100644
index 891cde713a47..000000000000
--- a/drivers/staging/media/go7007/wis-saa7113.c
+++ /dev/null
@@ -1,324 +0,0 @@
-/*
- * Copyright (C) 2005-2006 Micronas USA Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License (Version 2) as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software Foundation,
- * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/i2c.h>
-#include <linux/videodev2.h>
-#include <linux/ioctl.h>
-#include <linux/slab.h>
-
-#include "wis-i2c.h"
-
-struct wis_saa7113 {
-	int norm;
-	int brightness;
-	int contrast;
-	int saturation;
-	int hue;
-};
-
-static u8 initial_registers[] = {
-	0x01, 0x08,
-	0x02, 0xc0,
-	0x03, 0x33,
-	0x04, 0x00,
-	0x05, 0x00,
-	0x06, 0xe9,
-	0x07, 0x0d,
-	0x08, 0xd8,
-	0x09, 0x40,
-	0x0a, 0x80,
-	0x0b, 0x47,
-	0x0c, 0x40,
-	0x0d, 0x00,
-	0x0e, 0x01,
-	0x0f, 0x2a,
-	0x10, 0x40,
-	0x11, 0x0c,
-	0x12, 0xfe,
-	0x13, 0x00,
-	0x14, 0x00,
-	0x15, 0x04,
-	0x16, 0x00,
-	0x17, 0x00,
-	0x18, 0x00,
-	0x19, 0x00,
-	0x1a, 0x00,
-	0x1b, 0x00,
-	0x1c, 0x00,
-	0x1d, 0x00,
-	0x1e, 0x00,
-	0x1f, 0xc8,
-	0x40, 0x00,
-	0x41, 0xff,
-	0x42, 0xff,
-	0x43, 0xff,
-	0x44, 0xff,
-	0x45, 0xff,
-	0x46, 0xff,
-	0x47, 0xff,
-	0x48, 0xff,
-	0x49, 0xff,
-	0x4a, 0xff,
-	0x4b, 0xff,
-	0x4c, 0xff,
-	0x4d, 0xff,
-	0x4e, 0xff,
-	0x4f, 0xff,
-	0x50, 0xff,
-	0x51, 0xff,
-	0x52, 0xff,
-	0x53, 0xff,
-	0x54, 0xff,
-	0x55, 0xff,
-	0x56, 0xff,
-	0x57, 0xff,
-	0x58, 0x00,
-	0x59, 0x54,
-	0x5a, 0x07,
-	0x5b, 0x83,
-	0x5c, 0x00,
-	0x5d, 0x00,
-	0x5e, 0x00,
-	0x5f, 0x00,
-	0x60, 0x00,
-	0x61, 0x00,
-	0x00, 0x00, /* Terminator (reg 0x00 is read-only) */
-};
-
-static int write_reg(struct i2c_client *client, u8 reg, u8 value)
-{
-	return i2c_smbus_write_byte_data(client, reg, value);
-}
-
-static int write_regs(struct i2c_client *client, u8 *regs)
-{
-	int i;
-
-	for (i = 0; regs[i] != 0x00; i += 2)
-		if (i2c_smbus_write_byte_data(client, regs[i], regs[i + 1]) < 0)
-			return -1;
-	return 0;
-}
-
-static int wis_saa7113_command(struct i2c_client *client,
-				unsigned int cmd, void *arg)
-{
-	struct wis_saa7113 *dec = i2c_get_clientdata(client);
-
-	switch (cmd) {
-	case VIDIOC_S_INPUT:
-	{
-		int *input = arg;
-
-		i2c_smbus_write_byte_data(client, 0x02, 0xC0 | *input);
-		i2c_smbus_write_byte_data(client, 0x09,
-				*input < 6 ? 0x40 : 0x80);
-		break;
-	}
-	case VIDIOC_S_STD:
-	{
-		v4l2_std_id *input = arg;
-		dec->norm = *input;
-		if (dec->norm & V4L2_STD_NTSC) {
-			write_reg(client, 0x0e, 0x01);
-			write_reg(client, 0x10, 0x40);
-		} else if (dec->norm & V4L2_STD_PAL) {
-			write_reg(client, 0x0e, 0x01);
-			write_reg(client, 0x10, 0x48);
-		} else if (dec->norm & V4L2_STD_SECAM) {
-			write_reg(client, 0x0e, 0x50);
-			write_reg(client, 0x10, 0x48);
-		}
-		break;
-	}
-	case VIDIOC_QUERYCTRL:
-	{
-		struct v4l2_queryctrl *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Brightness", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 255;
-			ctrl->step = 1;
-			ctrl->default_value = 128;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Contrast", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 71;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_SATURATION:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Saturation", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 64;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_HUE:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Hue", sizeof(ctrl->name));
-			ctrl->minimum = -128;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 0;
-			ctrl->flags = 0;
-			break;
-		}
-		break;
-	}
-	case VIDIOC_S_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			if (ctrl->value > 255)
-				dec->brightness = 255;
-			else if (ctrl->value < 0)
-				dec->brightness = 0;
-			else
-				dec->brightness = ctrl->value;
-			write_reg(client, 0x0a, dec->brightness);
-			break;
-		case V4L2_CID_CONTRAST:
-			if (ctrl->value > 127)
-				dec->contrast = 127;
-			else if (ctrl->value < 0)
-				dec->contrast = 0;
-			else
-				dec->contrast = ctrl->value;
-			write_reg(client, 0x0b, dec->contrast);
-			break;
-		case V4L2_CID_SATURATION:
-			if (ctrl->value > 127)
-				dec->saturation = 127;
-			else if (ctrl->value < 0)
-				dec->saturation = 0;
-			else
-				dec->saturation = ctrl->value;
-			write_reg(client, 0x0c, dec->saturation);
-			break;
-		case V4L2_CID_HUE:
-			if (ctrl->value > 127)
-				dec->hue = 127;
-			else if (ctrl->value < -128)
-				dec->hue = -128;
-			else
-				dec->hue = ctrl->value;
-			write_reg(client, 0x0d, dec->hue);
-			break;
-		}
-		break;
-	}
-	case VIDIOC_G_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->value = dec->brightness;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->value = dec->contrast;
-			break;
-		case V4L2_CID_SATURATION:
-			ctrl->value = dec->saturation;
-			break;
-		case V4L2_CID_HUE:
-			ctrl->value = dec->hue;
-			break;
-		}
-		break;
-	}
-	default:
-		break;
-	}
-	return 0;
-}
-
-static int wis_saa7113_probe(struct i2c_client *client,
-			     const struct i2c_device_id *id)
-{
-	struct i2c_adapter *adapter = client->adapter;
-	struct wis_saa7113 *dec;
-
-	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
-		return -ENODEV;
-
-	dec = kmalloc(sizeof(struct wis_saa7113), GFP_KERNEL);
-	if (dec == NULL)
-		return -ENOMEM;
-
-	dec->norm = V4L2_STD_NTSC;
-	dec->brightness = 128;
-	dec->contrast = 71;
-	dec->saturation = 64;
-	dec->hue = 0;
-	i2c_set_clientdata(client, dec);
-
-	dev_dbg(&client->dev,
-		"wis-saa7113: initializing SAA7113 at address %d on %s\n",
-		client->addr, adapter->name);
-
-	if (write_regs(client, initial_registers) < 0) {
-		dev_err(&client->dev,
-			"wis-saa7113: error initializing SAA7113\n");
-		kfree(dec);
-		return -ENODEV;
-	}
-
-	return 0;
-}
-
-static int wis_saa7113_remove(struct i2c_client *client)
-{
-	struct wis_saa7113 *dec = i2c_get_clientdata(client);
-
-	kfree(dec);
-	return 0;
-}
-
-static const struct i2c_device_id wis_saa7113_id[] = {
-	{ "wis_saa7113", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, wis_saa7113_id);
-
-static struct i2c_driver wis_saa7113_driver = {
-	.driver = {
-		.name	= "WIS SAA7113 I2C driver",
-	},
-	.probe		= wis_saa7113_probe,
-	.remove		= wis_saa7113_remove,
-	.command	= wis_saa7113_command,
-	.id_table	= wis_saa7113_id,
-};
-
-module_i2c_driver(wis_saa7113_driver);
-
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/go7007/wis-saa7115.c b/drivers/staging/media/go7007/wis-saa7115.c
deleted file mode 100644
index fa86acd3fdf0..000000000000
--- a/drivers/staging/media/go7007/wis-saa7115.c
+++ /dev/null
@@ -1,457 +0,0 @@
-/*
- * Copyright (C) 2005-2006 Micronas USA Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License (Version 2) as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software Foundation,
- * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/i2c.h>
-#include <linux/videodev2.h>
-#include <linux/ioctl.h>
-#include <linux/slab.h>
-
-#include "wis-i2c.h"
-
-struct wis_saa7115 {
-	int norm;
-	int brightness;
-	int contrast;
-	int saturation;
-	int hue;
-};
-
-static u8 initial_registers[] = {
-	0x01, 0x08,
-	0x02, 0xc0,
-	0x03, 0x20,
-	0x04, 0x80,
-	0x05, 0x80,
-	0x06, 0xeb,
-	0x07, 0xe0,
-	0x08, 0xf0,	/* always toggle FID */
-	0x09, 0x40,
-	0x0a, 0x80,
-	0x0b, 0x40,
-	0x0c, 0x40,
-	0x0d, 0x00,
-	0x0e, 0x03,
-	0x0f, 0x2a,
-	0x10, 0x0e,
-	0x11, 0x00,
-	0x12, 0x8d,
-	0x13, 0x00,
-	0x14, 0x00,
-	0x15, 0x11,
-	0x16, 0x01,
-	0x17, 0xda,
-	0x18, 0x40,
-	0x19, 0x80,
-	0x1a, 0x00,
-	0x1b, 0x42,
-	0x1c, 0xa9,
-	0x30, 0x66,
-	0x31, 0x90,
-	0x32, 0x01,
-	0x34, 0x00,
-	0x35, 0x00,
-	0x36, 0x20,
-	0x38, 0x03,
-	0x39, 0x20,
-	0x3a, 0x88,
-	0x40, 0x00,
-	0x41, 0xff,
-	0x42, 0xff,
-	0x43, 0xff,
-	0x44, 0xff,
-	0x45, 0xff,
-	0x46, 0xff,
-	0x47, 0xff,
-	0x48, 0xff,
-	0x49, 0xff,
-	0x4a, 0xff,
-	0x4b, 0xff,
-	0x4c, 0xff,
-	0x4d, 0xff,
-	0x4e, 0xff,
-	0x4f, 0xff,
-	0x50, 0xff,
-	0x51, 0xff,
-	0x52, 0xff,
-	0x53, 0xff,
-	0x54, 0xf4 /*0xff*/,
-	0x55, 0xff,
-	0x56, 0xff,
-	0x57, 0xff,
-	0x58, 0x40,
-	0x59, 0x47,
-	0x5a, 0x06 /*0x03*/,
-	0x5b, 0x83,
-	0x5d, 0x06,
-	0x5e, 0x00,
-	0x80, 0x30, /* window defined scaler operation, task A and B enabled */
-	0x81, 0x03, /* use scaler datapath generated V */
-	0x83, 0x00,
-	0x84, 0x00,
-	0x85, 0x00,
-	0x86, 0x45,
-	0x87, 0x31,
-	0x88, 0xc0,
-	0x90, 0x02, /* task A process top field */
-	0x91, 0x08,
-	0x92, 0x09,
-	0x93, 0x80,
-	0x94, 0x06,
-	0x95, 0x00,
-	0x96, 0xc0,
-	0x97, 0x02,
-	0x98, 0x12,
-	0x99, 0x00,
-	0x9a, 0xf2,
-	0x9b, 0x00,
-	0x9c, 0xd0,
-	0x9d, 0x02,
-	0x9e, 0xf2,
-	0x9f, 0x00,
-	0xa0, 0x01,
-	0xa1, 0x01,
-	0xa2, 0x01,
-	0xa4, 0x80,
-	0xa5, 0x40,
-	0xa6, 0x40,
-	0xa8, 0x00,
-	0xa9, 0x04,
-	0xaa, 0x00,
-	0xac, 0x00,
-	0xad, 0x02,
-	0xae, 0x00,
-	0xb0, 0x00,
-	0xb1, 0x04,
-	0xb2, 0x00,
-	0xb3, 0x04,
-	0xb4, 0x00,
-	0xb8, 0x00,
-	0xbc, 0x00,
-	0xc0, 0x03,	/* task B process bottom field */
-	0xc1, 0x08,
-	0xc2, 0x09,
-	0xc3, 0x80,
-	0xc4, 0x06,
-	0xc5, 0x00,
-	0xc6, 0xc0,
-	0xc7, 0x02,
-	0xc8, 0x12,
-	0xc9, 0x00,
-	0xca, 0xf2,
-	0xcb, 0x00,
-	0xcc, 0xd0,
-	0xcd, 0x02,
-	0xce, 0xf2,
-	0xcf, 0x00,
-	0xd0, 0x01,
-	0xd1, 0x01,
-	0xd2, 0x01,
-	0xd4, 0x80,
-	0xd5, 0x40,
-	0xd6, 0x40,
-	0xd8, 0x00,
-	0xd9, 0x04,
-	0xda, 0x00,
-	0xdc, 0x00,
-	0xdd, 0x02,
-	0xde, 0x00,
-	0xe0, 0x00,
-	0xe1, 0x04,
-	0xe2, 0x00,
-	0xe3, 0x04,
-	0xe4, 0x00,
-	0xe8, 0x00,
-	0x88, 0xf0, /* End of original static list */
-	0x00, 0x00, /* Terminator (reg 0x00 is read-only) */
-};
-
-static int write_reg(struct i2c_client *client, u8 reg, u8 value)
-{
-	return i2c_smbus_write_byte_data(client, reg, value);
-}
-
-static int write_regs(struct i2c_client *client, u8 *regs)
-{
-	int i;
-
-	for (i = 0; regs[i] != 0x00; i += 2)
-		if (i2c_smbus_write_byte_data(client, regs[i], regs[i + 1]) < 0)
-			return -1;
-	return 0;
-}
-
-static int wis_saa7115_command(struct i2c_client *client,
-				unsigned int cmd, void *arg)
-{
-	struct wis_saa7115 *dec = i2c_get_clientdata(client);
-
-	switch (cmd) {
-	case VIDIOC_S_INPUT:
-	{
-		int *input = arg;
-
-		i2c_smbus_write_byte_data(client, 0x02, 0xC0 | *input);
-		i2c_smbus_write_byte_data(client, 0x09,
-				*input < 6 ? 0x40 : 0xC0);
-		break;
-	}
-	case DECODER_SET_RESOLUTION:
-	{
-		struct video_decoder_resolution *res = arg;
-		/* Course-grained scaler */
-		int h_integer_scaler = res->width < 704 ? 704 / res->width : 1;
-		/* Fine-grained scaler to take care of remainder */
-		int h_scaling_increment = (704 / h_integer_scaler) *
-					1024 / res->width;
-		/* Fine-grained scaler only */
-		int v_scaling_increment = (dec->norm & V4L2_STD_NTSC ?
-				240 : 288) * 1024 / res->height;
-		u8 regs[] = {
-			0x88,	0xc0,
-			0x9c,	res->width & 0xff,
-			0x9d,	res->width >> 8,
-			0x9e,	res->height & 0xff,
-			0x9f,	res->height >> 8,
-			0xa0,	h_integer_scaler,
-			0xa1,	1,
-			0xa2,	1,
-			0xa8,	h_scaling_increment & 0xff,
-			0xa9,	h_scaling_increment >> 8,
-			0xac,	(h_scaling_increment / 2) & 0xff,
-			0xad,	(h_scaling_increment / 2) >> 8,
-			0xb0,	v_scaling_increment & 0xff,
-			0xb1,	v_scaling_increment >> 8,
-			0xb2,	v_scaling_increment & 0xff,
-			0xb3,	v_scaling_increment >> 8,
-			0xcc,	res->width & 0xff,
-			0xcd,	res->width >> 8,
-			0xce,	res->height & 0xff,
-			0xcf,	res->height >> 8,
-			0xd0,	h_integer_scaler,
-			0xd1,	1,
-			0xd2,	1,
-			0xd8,	h_scaling_increment & 0xff,
-			0xd9,	h_scaling_increment >> 8,
-			0xdc,	(h_scaling_increment / 2) & 0xff,
-			0xdd,	(h_scaling_increment / 2) >> 8,
-			0xe0,	v_scaling_increment & 0xff,
-			0xe1,	v_scaling_increment >> 8,
-			0xe2,	v_scaling_increment & 0xff,
-			0xe3,	v_scaling_increment >> 8,
-			0x88,	0xf0,
-			0,	0,
-		};
-		write_regs(client, regs);
-		break;
-	}
-	case VIDIOC_S_STD:
-	{
-		v4l2_std_id *input = arg;
-		u8 regs[] = {
-			0x88,	0xc0,
-			0x98,	*input & V4L2_STD_NTSC ? 0x12 : 0x16,
-			0x9a,	*input & V4L2_STD_NTSC ? 0xf2 : 0x20,
-			0x9b,	*input & V4L2_STD_NTSC ? 0x00 : 0x01,
-			0xc8,	*input & V4L2_STD_NTSC ? 0x12 : 0x16,
-			0xca,	*input & V4L2_STD_NTSC ? 0xf2 : 0x20,
-			0xcb,	*input & V4L2_STD_NTSC ? 0x00 : 0x01,
-			0x88,	0xf0,
-			0x30,	*input & V4L2_STD_NTSC ? 0x66 : 0x00,
-			0x31,	*input & V4L2_STD_NTSC ? 0x90 : 0xe0,
-			0,	0,
-		};
-		write_regs(client, regs);
-		dec->norm = *input;
-		break;
-	}
-	case VIDIOC_QUERYCTRL:
-	{
-		struct v4l2_queryctrl *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Brightness", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 255;
-			ctrl->step = 1;
-			ctrl->default_value = 128;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Contrast", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 64;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_SATURATION:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Saturation", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 64;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_HUE:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Hue", sizeof(ctrl->name));
-			ctrl->minimum = -128;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 0;
-			ctrl->flags = 0;
-			break;
-		}
-		break;
-	}
-	case VIDIOC_S_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			if (ctrl->value > 255)
-				dec->brightness = 255;
-			else if (ctrl->value < 0)
-				dec->brightness = 0;
-			else
-				dec->brightness = ctrl->value;
-			write_reg(client, 0x0a, dec->brightness);
-			break;
-		case V4L2_CID_CONTRAST:
-			if (ctrl->value > 127)
-				dec->contrast = 127;
-			else if (ctrl->value < 0)
-				dec->contrast = 0;
-			else
-				dec->contrast = ctrl->value;
-			write_reg(client, 0x0b, dec->contrast);
-			break;
-		case V4L2_CID_SATURATION:
-			if (ctrl->value > 127)
-				dec->saturation = 127;
-			else if (ctrl->value < 0)
-				dec->saturation = 0;
-			else
-				dec->saturation = ctrl->value;
-			write_reg(client, 0x0c, dec->saturation);
-			break;
-		case V4L2_CID_HUE:
-			if (ctrl->value > 127)
-				dec->hue = 127;
-			else if (ctrl->value < -128)
-				dec->hue = -128;
-			else
-				dec->hue = ctrl->value;
-			write_reg(client, 0x0d, dec->hue);
-			break;
-		}
-		break;
-	}
-	case VIDIOC_G_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->value = dec->brightness;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->value = dec->contrast;
-			break;
-		case V4L2_CID_SATURATION:
-			ctrl->value = dec->saturation;
-			break;
-		case V4L2_CID_HUE:
-			ctrl->value = dec->hue;
-			break;
-		}
-		break;
-	}
-	default:
-		break;
-	}
-	return 0;
-}
-
-static int wis_saa7115_probe(struct i2c_client *client,
-			     const struct i2c_device_id *id)
-{
-	struct i2c_adapter *adapter = client->adapter;
-	struct wis_saa7115 *dec;
-
-	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
-		return -ENODEV;
-
-	dec = kmalloc(sizeof(struct wis_saa7115), GFP_KERNEL);
-	if (dec == NULL)
-		return -ENOMEM;
-
-	dec->norm = V4L2_STD_NTSC;
-	dec->brightness = 128;
-	dec->contrast = 64;
-	dec->saturation = 64;
-	dec->hue = 0;
-	i2c_set_clientdata(client, dec);
-
-	dev_dbg(&client->dev,
-		"wis-saa7115: initializing SAA7115 at address %d on %s\n",
-		client->addr, adapter->name);
-
-	if (write_regs(client, initial_registers) < 0) {
-		dev_err(&client->dev,
-			"wis-saa7115: error initializing SAA7115\n");
-		kfree(dec);
-		return -ENODEV;
-	}
-
-	return 0;
-}
-
-static int wis_saa7115_remove(struct i2c_client *client)
-{
-	struct wis_saa7115 *dec = i2c_get_clientdata(client);
-
-	kfree(dec);
-	return 0;
-}
-
-static const struct i2c_device_id wis_saa7115_id[] = {
-	{ "wis_saa7115", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, wis_saa7115_id);
-
-static struct i2c_driver wis_saa7115_driver = {
-	.driver = {
-		.name	= "WIS SAA7115 I2C driver",
-	},
-	.probe		= wis_saa7115_probe,
-	.remove		= wis_saa7115_remove,
-	.command	= wis_saa7115_command,
-	.id_table	= wis_saa7115_id,
-};
-
-module_i2c_driver(wis_saa7115_driver);
-
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/go7007/wis-sony-tuner.c b/drivers/staging/media/go7007/wis-sony-tuner.c
deleted file mode 100644
index 5d7ff8c81d6d..000000000000
--- a/drivers/staging/media/go7007/wis-sony-tuner.c
+++ /dev/null
@@ -1,707 +0,0 @@
-/*
- * Copyright (C) 2005-2006 Micronas USA Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License (Version 2) as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software Foundation,
- * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/i2c.h>
-#include <linux/videodev2.h>
-#include <linux/slab.h>
-#include <media/tuner.h>
-#include <media/v4l2-common.h>
-#include <media/v4l2-ioctl.h>
-
-#include "wis-i2c.h"
-
-/* #define MPX_DEBUG */
-
-/* AS(IF/MPX) pin:      LOW      HIGH/OPEN
- * IF/MPX address:   0x42/0x40   0x43/0x44
- */
-#define IF_I2C_ADDR	0x43
-#define MPX_I2C_ADDR	0x44
-
-static v4l2_std_id force_band;
-static char force_band_str[] = "-";
-module_param_string(force_band, force_band_str, sizeof(force_band_str), 0644);
-static int force_mpx_mode = -1;
-module_param(force_mpx_mode, int, 0644);
-
-/* Store tuner info in the same format as tuner.c, so maybe we can put the
- * Sony tuner support in there. */
-struct sony_tunertype {
-	char *name;
-	unsigned char Vendor; /* unused here */
-	unsigned char Type; /* unused here */
-
-	unsigned short thresh1; /*  band switch VHF_LO <=> VHF_HI */
-	unsigned short thresh2; /*  band switch VHF_HI <=> UHF */
-	unsigned char VHF_L;
-	unsigned char VHF_H;
-	unsigned char UHF;
-	unsigned char config;
-	unsigned short IFPCoff;
-};
-
-/* This array is indexed by (tuner_type - 200) */
-static struct sony_tunertype sony_tuners[] = {
-	{ "Sony PAL+SECAM (BTF-PG472Z)", 0, 0,
-	  16*144.25, 16*427.25, 0x01, 0x02, 0x04, 0xc6, 623},
-	{ "Sony NTSC_JP (BTF-PK467Z)", 0, 0,
-	  16*220.25, 16*467.25, 0x01, 0x02, 0x04, 0xc6, 940},
-	{ "Sony NTSC (BTF-PB463Z)", 0, 0,
-	  16*130.25, 16*364.25, 0x01, 0x02, 0x04, 0xc6, 732},
-};
-
-struct wis_sony_tuner {
-	int type;
-	v4l2_std_id std;
-	unsigned int freq;
-	int mpxmode;
-	u32 audmode;
-};
-
-/* Basically the same as default_set_tv_freq() in tuner.c */
-static int set_freq(struct i2c_client *client, int freq)
-{
-	struct wis_sony_tuner *t = i2c_get_clientdata(client);
-	char *band_name;
-	int n;
-	int band_select;
-	struct sony_tunertype *tun;
-	u8 buffer[4];
-
-	tun = &sony_tuners[t->type - 200];
-	if (freq < tun->thresh1) {
-		band_name = "VHF_L";
-		band_select = tun->VHF_L;
-	} else if (freq < tun->thresh2) {
-		band_name = "VHF_H";
-		band_select = tun->VHF_H;
-	} else {
-		band_name = "UHF";
-		band_select = tun->UHF;
-	}
-	dev_dbg(&client->dev, "tuning to frequency %d.%04d (%s)\n",
-		freq / 16, (freq % 16) * 625, band_name);
-	n = freq + tun->IFPCoff;
-
-	buffer[0] = n >> 8;
-	buffer[1] = n & 0xff;
-	buffer[2] = tun->config;
-	buffer[3] = band_select;
-	i2c_master_send(client, buffer, 4);
-
-	return 0;
-}
-
-static int mpx_write(struct i2c_client *client, int dev, int addr, int val)
-{
-	u8 buffer[5];
-	struct i2c_msg msg;
-
-	buffer[0] = dev;
-	buffer[1] = addr >> 8;
-	buffer[2] = addr & 0xff;
-	buffer[3] = val >> 8;
-	buffer[4] = val & 0xff;
-	msg.addr = MPX_I2C_ADDR;
-	msg.flags = 0;
-	msg.len = 5;
-	msg.buf = buffer;
-	i2c_transfer(client->adapter, &msg, 1);
-	return 0;
-}
-
-/*
- * MPX register values for the BTF-PG472Z:
- *
- *                                 FM_     NICAM_  SCART_
- *          MODUS  SOURCE    ACB   PRESCAL PRESCAL PRESCAL SYSTEM  VOLUME
- *         10/0030 12/0008 12/0013 12/000E 12/0010 12/0000 10/0020 12/0000
- *         ---------------------------------------------------------------
- * Auto     1003    0020    0100    2603    5000    XXXX    0001    7500
- *
- * B/G
- *  Mono    1003    0020    0100    2603    5000    XXXX    0003    7500
- *  A2      1003    0020    0100    2601    5000    XXXX    0003    7500
- *  NICAM   1003    0120    0100    2603    5000    XXXX    0008    7500
- *
- * I
- *  Mono    1003    0020    0100    2603    7900    XXXX    000A    7500
- *  NICAM   1003    0120    0100    2603    7900    XXXX    000A    7500
- *
- * D/K
- *  Mono    1003    0020    0100    2603    5000    XXXX    0004    7500
- *  A2-1    1003    0020    0100    2601    5000    XXXX    0004    7500
- *  A2-2    1003    0020    0100    2601    5000    XXXX    0005    7500
- *  A2-3    1003    0020    0100    2601    5000    XXXX    0007    7500
- *  NICAM   1003    0120    0100    2603    5000    XXXX    000B    7500
- *
- * L/L'
- *  Mono    0003    0200    0100    7C03    5000    2200    0009    7500
- *  NICAM   0003    0120    0100    7C03    5000    XXXX    0009    7500
- *
- * M
- *  Mono    1003    0200    0100    2B03    5000    2B00    0002    7500
- *
- * For Asia, replace the 0x26XX in FM_PRESCALE with 0x14XX.
- *
- * Bilingual selection in A2/NICAM:
- *
- *         High byte of SOURCE     Left chan   Right chan
- *                 0x01              MAIN         SUB
- *                 0x03              MAIN         MAIN
- *                 0x04              SUB          SUB
- *
- * Force mono in NICAM by setting the high byte of SOURCE to 0x02 (L/L') or
- * 0x00 (all other bands).  Force mono in A2 with FMONO_A2:
- *
- *                      FMONO_A2
- *                      10/0022
- *                      --------
- *     Forced mono ON     07F0
- *     Forced mono OFF    0190
- */
-
-static struct {
-	enum { AUD_MONO, AUD_A2, AUD_NICAM, AUD_NICAM_L } audio_mode;
-	u16 modus;
-	u16 source;
-	u16 acb;
-	u16 fm_prescale;
-	u16 nicam_prescale;
-	u16 scart_prescale;
-	u16 system;
-	u16 volume;
-} mpx_audio_modes[] = {
-	/* Auto */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
-					0x5000, 0x0000, 0x0001, 0x7500 },
-	/* B/G Mono */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
-					0x5000, 0x0000, 0x0003, 0x7500 },
-	/* B/G A2 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
-					0x5000, 0x0000, 0x0003, 0x7500 },
-	/* B/G NICAM */ { AUD_NICAM,	0x1003, 0x0120, 0x0100, 0x2603,
-					0x5000, 0x0000, 0x0008, 0x7500 },
-	/* I Mono */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
-					0x7900, 0x0000, 0x000A, 0x7500 },
-	/* I NICAM */	{ AUD_NICAM,	0x1003, 0x0120, 0x0100, 0x2603,
-					0x7900, 0x0000, 0x000A, 0x7500 },
-	/* D/K Mono */	{ AUD_MONO,	0x1003, 0x0020, 0x0100, 0x2603,
-					0x5000, 0x0000, 0x0004, 0x7500 },
-	/* D/K A2-1 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
-					0x5000, 0x0000, 0x0004, 0x7500 },
-	/* D/K A2-2 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
-					0x5000, 0x0000, 0x0005, 0x7500 },
-	/* D/K A2-3 */	{ AUD_A2,	0x1003, 0x0020, 0x0100, 0x2601,
-					0x5000, 0x0000, 0x0007, 0x7500 },
-	/* D/K NICAM */	{ AUD_NICAM,	0x1003, 0x0120, 0x0100, 0x2603,
-					0x5000, 0x0000, 0x000B, 0x7500 },
-	/* L/L' Mono */	{ AUD_MONO,	0x0003, 0x0200, 0x0100, 0x7C03,
-					0x5000, 0x2200, 0x0009, 0x7500 },
-	/* L/L' NICAM */{ AUD_NICAM_L,	0x0003, 0x0120, 0x0100, 0x7C03,
-					0x5000, 0x0000, 0x0009, 0x7500 },
-};
-
-#define MPX_NUM_MODES	ARRAY_SIZE(mpx_audio_modes)
-
-static int mpx_setup(struct i2c_client *client)
-{
-	struct wis_sony_tuner *t = i2c_get_clientdata(client);
-	u16 source = 0;
-	u8 buffer[3];
-	struct i2c_msg msg;
-
-	/* reset MPX */
-	buffer[0] = 0x00;
-	buffer[1] = 0x80;
-	buffer[2] = 0x00;
-	msg.addr = MPX_I2C_ADDR;
-	msg.flags = 0;
-	msg.len = 3;
-	msg.buf = buffer;
-	i2c_transfer(client->adapter, &msg, 1);
-	buffer[1] = 0x00;
-	i2c_transfer(client->adapter, &msg, 1);
-
-	if (mpx_audio_modes[t->mpxmode].audio_mode != AUD_MONO) {
-		switch (t->audmode) {
-		case V4L2_TUNER_MODE_MONO:
-			switch (mpx_audio_modes[t->mpxmode].audio_mode) {
-			case AUD_A2:
-				source = mpx_audio_modes[t->mpxmode].source;
-				break;
-			case AUD_NICAM:
-				source = 0x0000;
-				break;
-			case AUD_NICAM_L:
-				source = 0x0200;
-				break;
-			default:
-				break;
-			}
-			break;
-		case V4L2_TUNER_MODE_STEREO:
-			source = mpx_audio_modes[t->mpxmode].source;
-			break;
-		case V4L2_TUNER_MODE_LANG1:
-			source = 0x0300;
-			break;
-		case V4L2_TUNER_MODE_LANG2:
-			source = 0x0400;
-			break;
-		}
-		source |= mpx_audio_modes[t->mpxmode].source & 0x00ff;
-	} else
-		source = mpx_audio_modes[t->mpxmode].source;
-
-	mpx_write(client, 0x10, 0x0030, mpx_audio_modes[t->mpxmode].modus);
-	mpx_write(client, 0x12, 0x0008, source);
-	mpx_write(client, 0x12, 0x0013, mpx_audio_modes[t->mpxmode].acb);
-	mpx_write(client, 0x12, 0x000e,
-			mpx_audio_modes[t->mpxmode].fm_prescale);
-	mpx_write(client, 0x12, 0x0010,
-			mpx_audio_modes[t->mpxmode].nicam_prescale);
-	mpx_write(client, 0x12, 0x000d,
-			mpx_audio_modes[t->mpxmode].scart_prescale);
-	mpx_write(client, 0x10, 0x0020, mpx_audio_modes[t->mpxmode].system);
-	mpx_write(client, 0x12, 0x0000, mpx_audio_modes[t->mpxmode].volume);
-	if (mpx_audio_modes[t->mpxmode].audio_mode == AUD_A2)
-		mpx_write(client, 0x10, 0x0022,
-			t->audmode == V4L2_TUNER_MODE_MONO ?  0x07f0 : 0x0190);
-
-#ifdef MPX_DEBUG
-	{
-		u8 buf1[3], buf2[2];
-		struct i2c_msg msgs[2];
-
-		dev_dbg(&client->dev,
-			"MPX registers: %04x %04x %04x %04x %04x %04x %04x %04x\n",
-			mpx_audio_modes[t->mpxmode].modus,
-			source,
-			mpx_audio_modes[t->mpxmode].acb,
-			mpx_audio_modes[t->mpxmode].fm_prescale,
-			mpx_audio_modes[t->mpxmode].nicam_prescale,
-			mpx_audio_modes[t->mpxmode].scart_prescale,
-			mpx_audio_modes[t->mpxmode].system,
-			mpx_audio_modes[t->mpxmode].volume);
-		buf1[0] = 0x11;
-		buf1[1] = 0x00;
-		buf1[2] = 0x7e;
-		msgs[0].addr = MPX_I2C_ADDR;
-		msgs[0].flags = 0;
-		msgs[0].len = 3;
-		msgs[0].buf = buf1;
-		msgs[1].addr = MPX_I2C_ADDR;
-		msgs[1].flags = I2C_M_RD;
-		msgs[1].len = 2;
-		msgs[1].buf = buf2;
-		i2c_transfer(client->adapter, msgs, 2);
-		dev_dbg(&client->dev, "MPX system: %02x%02x\n",
-			buf2[0], buf2[1]);
-		buf1[0] = 0x11;
-		buf1[1] = 0x02;
-		buf1[2] = 0x00;
-		i2c_transfer(client->adapter, msgs, 2);
-		dev_dbg(&client->dev, "MPX status: %02x%02x\n",
-			buf2[0], buf2[1]);
-	}
-#endif
-	return 0;
-}
-
-/*
- * IF configuration values for the BTF-PG472Z:
- *
- *	B/G: 0x94 0x70 0x49
- *	I:   0x14 0x70 0x4a
- *	D/K: 0x14 0x70 0x4b
- *	L:   0x04 0x70 0x4b
- *	L':  0x44 0x70 0x53
- *	M:   0x50 0x30 0x4c
- */
-
-static int set_if(struct i2c_client *client)
-{
-	struct wis_sony_tuner *t = i2c_get_clientdata(client);
-	u8 buffer[4];
-	struct i2c_msg msg;
-	int default_mpx_mode = 0;
-
-	/* configure IF */
-	buffer[0] = 0;
-	if (t->std & V4L2_STD_PAL_BG) {
-		buffer[1] = 0x94;
-		buffer[2] = 0x70;
-		buffer[3] = 0x49;
-		default_mpx_mode = 1;
-	} else if (t->std & V4L2_STD_PAL_I) {
-		buffer[1] = 0x14;
-		buffer[2] = 0x70;
-		buffer[3] = 0x4a;
-		default_mpx_mode = 4;
-	} else if (t->std & V4L2_STD_PAL_DK) {
-		buffer[1] = 0x14;
-		buffer[2] = 0x70;
-		buffer[3] = 0x4b;
-		default_mpx_mode = 6;
-	} else if (t->std & V4L2_STD_SECAM_L) {
-		buffer[1] = 0x04;
-		buffer[2] = 0x70;
-		buffer[3] = 0x4b;
-		default_mpx_mode = 11;
-	}
-	msg.addr = IF_I2C_ADDR;
-	msg.flags = 0;
-	msg.len = 4;
-	msg.buf = buffer;
-	i2c_transfer(client->adapter, &msg, 1);
-
-	/* Select MPX mode if not forced by the user */
-	if (force_mpx_mode >= 0 && force_mpx_mode < MPX_NUM_MODES)
-		t->mpxmode = force_mpx_mode;
-	else
-		t->mpxmode = default_mpx_mode;
-	dev_dbg(&client->dev, "setting MPX to mode %d\n", t->mpxmode);
-	mpx_setup(client);
-
-	return 0;
-}
-
-static int tuner_command(struct i2c_client *client, unsigned int cmd, void *arg)
-{
-	struct wis_sony_tuner *t = i2c_get_clientdata(client);
-
-	switch (cmd) {
-#if 0
-#ifdef TUNER_SET_TYPE_ADDR
-	case TUNER_SET_TYPE_ADDR:
-	{
-		struct tuner_setup *tun_setup = arg;
-		int *type = &tun_setup->type;
-#else
-	case TUNER_SET_TYPE:
-	{
-		int *type = arg;
-#endif
-
-		if (t->type >= 0) {
-			if (t->type != *type)
-				dev_err(&client->dev,
-					"type already set to %d, ignoring request for %d\n",
-					t->type, *type);
-			break;
-		}
-		t->type = *type;
-		switch (t->type) {
-		case TUNER_SONY_BTF_PG472Z:
-			switch (force_band_str[0]) {
-			case 'b':
-			case 'B':
-			case 'g':
-			case 'G':
-				dev_info(&client->dev,
-					 "forcing tuner to PAL-B/G bands\n");
-				force_band = V4L2_STD_PAL_BG;
-				break;
-			case 'i':
-			case 'I':
-				dev_info(&client->dev,
-					 "forcing tuner to PAL-I band\n");
-				force_band = V4L2_STD_PAL_I;
-				break;
-			case 'd':
-			case 'D':
-			case 'k':
-			case 'K':
-				dev_info(&client->dev,
-					 "forcing tuner to PAL-D/K bands\n");
-				force_band = V4L2_STD_PAL_I;
-				break;
-			case 'l':
-			case 'L':
-				dev_info(&client->dev,
-					 "forcing tuner to SECAM-L band\n");
-				force_band = V4L2_STD_SECAM_L;
-				break;
-			default:
-				force_band = 0;
-				break;
-			}
-			if (force_band)
-				t->std = force_band;
-			else
-				t->std = V4L2_STD_PAL_BG;
-			set_if(client);
-			break;
-		case TUNER_SONY_BTF_PK467Z:
-			t->std = V4L2_STD_NTSC_M_JP;
-			break;
-		case TUNER_SONY_BTF_PB463Z:
-			t->std = V4L2_STD_NTSC_M;
-			break;
-		default:
-			dev_err(&client->dev,
-				"tuner type %d is not supported by this module\n",
-				*type);
-			break;
-		}
-		if (type >= 0)
-			dev_info(&clinet->dev,
-				 "type set to %d (%s)\n",
-				 t->type, sony_tuners[t->type - 200].name);
-		break;
-	}
-#endif
-	case VIDIOC_G_FREQUENCY:
-	{
-		struct v4l2_frequency *f = arg;
-
-		f->frequency = t->freq;
-		break;
-	}
-	case VIDIOC_S_FREQUENCY:
-	{
-		struct v4l2_frequency *f = arg;
-
-		t->freq = f->frequency;
-		set_freq(client, t->freq);
-		break;
-	}
-	case VIDIOC_ENUMSTD:
-	{
-		struct v4l2_standard *std = arg;
-
-		switch (t->type) {
-		case TUNER_SONY_BTF_PG472Z:
-			switch (std->index) {
-			case 0:
-				v4l2_video_std_construct(std,
-						V4L2_STD_PAL_BG, "PAL-B/G");
-				break;
-			case 1:
-				v4l2_video_std_construct(std,
-						V4L2_STD_PAL_I, "PAL-I");
-				break;
-			case 2:
-				v4l2_video_std_construct(std,
-						V4L2_STD_PAL_DK, "PAL-D/K");
-				break;
-			case 3:
-				v4l2_video_std_construct(std,
-						V4L2_STD_SECAM_L, "SECAM-L");
-				break;
-			default:
-				std->id = 0; /* hack to indicate EINVAL */
-				break;
-			}
-			break;
-		case TUNER_SONY_BTF_PK467Z:
-			if (std->index != 0) {
-				std->id = 0; /* hack to indicate EINVAL */
-				break;
-			}
-			v4l2_video_std_construct(std,
-					V4L2_STD_NTSC_M_JP, "NTSC-J");
-			break;
-		case TUNER_SONY_BTF_PB463Z:
-			if (std->index != 0) {
-				std->id = 0; /* hack to indicate EINVAL */
-				break;
-			}
-			v4l2_video_std_construct(std, V4L2_STD_NTSC_M, "NTSC");
-			break;
-		}
-		break;
-	}
-	case VIDIOC_G_STD:
-	{
-		v4l2_std_id *std = arg;
-
-		*std = t->std;
-		break;
-	}
-	case VIDIOC_S_STD:
-	{
-		v4l2_std_id *std = arg;
-		v4l2_std_id old = t->std;
-
-		switch (t->type) {
-		case TUNER_SONY_BTF_PG472Z:
-			if (force_band && (*std & force_band) != *std &&
-					*std != V4L2_STD_PAL &&
-					*std != V4L2_STD_SECAM) {
-				dev_dbg(&client->dev,
-					"ignoring requested TV standard in favor of force_band value\n");
-				t->std = force_band;
-			} else if (*std & V4L2_STD_PAL_BG) { /* default */
-				t->std = V4L2_STD_PAL_BG;
-			} else if (*std & V4L2_STD_PAL_I) {
-				t->std = V4L2_STD_PAL_I;
-			} else if (*std & V4L2_STD_PAL_DK) {
-				t->std = V4L2_STD_PAL_DK;
-			} else if (*std & V4L2_STD_SECAM_L) {
-				t->std = V4L2_STD_SECAM_L;
-			} else {
-				dev_err(&client->dev,
-					"TV standard not supported\n");
-				*std = 0; /* hack to indicate EINVAL */
-				break;
-			}
-			if (old != t->std)
-				set_if(client);
-			break;
-		case TUNER_SONY_BTF_PK467Z:
-			if (!(*std & V4L2_STD_NTSC_M_JP)) {
-				dev_err(&client->dev,
-					"TV standard not supported\n");
-				*std = 0; /* hack to indicate EINVAL */
-			}
-			break;
-		case TUNER_SONY_BTF_PB463Z:
-			if (!(*std & V4L2_STD_NTSC_M)) {
-				dev_err(&client->dev,
-					"TV standard not supported\n");
-				*std = 0; /* hack to indicate EINVAL */
-			}
-			break;
-		}
-		break;
-	}
-	case VIDIOC_QUERYSTD:
-	{
-		v4l2_std_id *std = arg;
-
-		switch (t->type) {
-		case TUNER_SONY_BTF_PG472Z:
-			if (force_band)
-				*std = force_band;
-			else
-				*std = V4L2_STD_PAL_BG | V4L2_STD_PAL_I |
-					V4L2_STD_PAL_DK | V4L2_STD_SECAM_L;
-			break;
-		case TUNER_SONY_BTF_PK467Z:
-			*std = V4L2_STD_NTSC_M_JP;
-			break;
-		case TUNER_SONY_BTF_PB463Z:
-			*std = V4L2_STD_NTSC_M;
-			break;
-		}
-		break;
-	}
-	case VIDIOC_G_TUNER:
-	{
-		struct v4l2_tuner *tun = arg;
-
-		memset(tun, 0, sizeof(*tun));
-		strcpy(tun->name, "Television");
-		tun->type = V4L2_TUNER_ANALOG_TV;
-		tun->rangelow = 0UL; /* does anything use these? */
-		tun->rangehigh = 0xffffffffUL;
-		switch (t->type) {
-		case TUNER_SONY_BTF_PG472Z:
-			tun->capability = V4L2_TUNER_CAP_NORM |
-				V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_LANG1 |
-				V4L2_TUNER_CAP_LANG2;
-			tun->rxsubchans = V4L2_TUNER_SUB_MONO |
-				V4L2_TUNER_SUB_STEREO | V4L2_TUNER_SUB_LANG1 |
-				V4L2_TUNER_SUB_LANG2;
-			break;
-		case TUNER_SONY_BTF_PK467Z:
-		case TUNER_SONY_BTF_PB463Z:
-			tun->capability = V4L2_TUNER_CAP_STEREO;
-			tun->rxsubchans = V4L2_TUNER_SUB_MONO |
-						V4L2_TUNER_SUB_STEREO;
-			break;
-		}
-		tun->audmode = t->audmode;
-		return 0;
-	}
-	case VIDIOC_S_TUNER:
-	{
-		struct v4l2_tuner *tun = arg;
-
-		switch (t->type) {
-		case TUNER_SONY_BTF_PG472Z:
-			if (tun->audmode != t->audmode) {
-				t->audmode = tun->audmode;
-				mpx_setup(client);
-			}
-			break;
-		case TUNER_SONY_BTF_PK467Z:
-		case TUNER_SONY_BTF_PB463Z:
-			break;
-		}
-		return 0;
-	}
-	default:
-		break;
-	}
-	return 0;
-}
-
-static int wis_sony_tuner_probe(struct i2c_client *client,
-				const struct i2c_device_id *id)
-{
-	struct i2c_adapter *adapter = client->adapter;
-	struct wis_sony_tuner *t;
-
-	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_I2C_BLOCK))
-		return -ENODEV;
-
-	t = kmalloc(sizeof(struct wis_sony_tuner), GFP_KERNEL);
-	if (t == NULL)
-		return -ENOMEM;
-
-	t->type = -1;
-	t->freq = 0;
-	t->mpxmode = 0;
-	t->audmode = V4L2_TUNER_MODE_STEREO;
-	i2c_set_clientdata(client, t);
-
-	dev_dbg(&client->dev, "initializing tuner at address %d on %s\n",
-		client->addr, adapter->name);
-
-	return 0;
-}
-
-static int wis_sony_tuner_remove(struct i2c_client *client)
-{
-	struct wis_sony_tuner *t = i2c_get_clientdata(client);
-
-	kfree(t);
-	return 0;
-}
-
-static const struct i2c_device_id wis_sony_tuner_id[] = {
-	{ "wis_sony_tuner", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, wis_sony_tuner_id);
-
-static struct i2c_driver wis_sony_tuner_driver = {
-	.driver = {
-		.name	= "WIS Sony TV Tuner I2C driver",
-	},
-	.probe		= wis_sony_tuner_probe,
-	.remove		= wis_sony_tuner_remove,
-	.command	= tuner_command,
-	.id_table	= wis_sony_tuner_id,
-};
-
-module_i2c_driver(wis_sony_tuner_driver);
-
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/go7007/wis-tw2804.c b/drivers/staging/media/go7007/wis-tw2804.c
deleted file mode 100644
index 290fd8c7bfef..000000000000
--- a/drivers/staging/media/go7007/wis-tw2804.c
+++ /dev/null
@@ -1,348 +0,0 @@
-/*
- * Copyright (C) 2005-2006 Micronas USA Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License (Version 2) as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software Foundation,
- * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/i2c.h>
-#include <linux/videodev2.h>
-#include <linux/ioctl.h>
-#include <linux/slab.h>
-
-#include "wis-i2c.h"
-
-struct wis_tw2804 {
-	int channel;
-	int norm;
-	int brightness;
-	int contrast;
-	int saturation;
-	int hue;
-};
-
-static u8 global_registers[] = {
-	0x39, 0x00,
-	0x3a, 0xff,
-	0x3b, 0x84,
-	0x3c, 0x80,
-	0x3d, 0x80,
-	0x3e, 0x82,
-	0x3f, 0x82,
-	0xff, 0xff, /* Terminator (reg 0xff does not exist) */
-};
-
-static u8 channel_registers[] = {
-	0x01, 0xc4,
-	0x02, 0xa5,
-	0x03, 0x20,
-	0x04, 0xd0,
-	0x05, 0x20,
-	0x06, 0xd0,
-	0x07, 0x88,
-	0x08, 0x20,
-	0x09, 0x07,
-	0x0a, 0xf0,
-	0x0b, 0x07,
-	0x0c, 0xf0,
-	0x0d, 0x40,
-	0x0e, 0xd2,
-	0x0f, 0x80,
-	0x10, 0x80,
-	0x11, 0x80,
-	0x12, 0x80,
-	0x13, 0x1f,
-	0x14, 0x00,
-	0x15, 0x00,
-	0x16, 0x00,
-	0x17, 0x00,
-	0x18, 0xff,
-	0x19, 0xff,
-	0x1a, 0xff,
-	0x1b, 0xff,
-	0x1c, 0xff,
-	0x1d, 0xff,
-	0x1e, 0xff,
-	0x1f, 0xff,
-	0x20, 0x07,
-	0x21, 0x07,
-	0x22, 0x00,
-	0x23, 0x91,
-	0x24, 0x51,
-	0x25, 0x03,
-	0x26, 0x00,
-	0x27, 0x00,
-	0x28, 0x00,
-	0x29, 0x00,
-	0x2a, 0x00,
-	0x2b, 0x00,
-	0x2c, 0x00,
-	0x2d, 0x00,
-	0x2e, 0x00,
-	0x2f, 0x00,
-	0x30, 0x00,
-	0x31, 0x00,
-	0x32, 0x00,
-	0x33, 0x00,
-	0x34, 0x00,
-	0x35, 0x00,
-	0x36, 0x00,
-	0x37, 0x00,
-	0xff, 0xff, /* Terminator (reg 0xff does not exist) */
-};
-
-static int write_reg(struct i2c_client *client, u8 reg, u8 value, int channel)
-{
-	return i2c_smbus_write_byte_data(client, reg | (channel << 6), value);
-}
-
-static int write_regs(struct i2c_client *client, u8 *regs, int channel)
-{
-	int i;
-
-	for (i = 0; regs[i] != 0xff; i += 2)
-		if (i2c_smbus_write_byte_data(client,
-				regs[i] | (channel << 6), regs[i + 1]) < 0)
-			return -1;
-	return 0;
-}
-
-static int wis_tw2804_command(struct i2c_client *client,
-				unsigned int cmd, void *arg)
-{
-	struct wis_tw2804 *dec = i2c_get_clientdata(client);
-
-	if (cmd == DECODER_SET_CHANNEL) {
-		int *input = arg;
-
-		if (*input < 0 || *input > 3) {
-			dev_err(&client->dev,
-				"channel %d is not between 0 and 3!\n", *input);
-			return 0;
-		}
-		dec->channel = *input;
-		dev_dbg(&client->dev, "initializing TW2804 channel %d\n",
-			dec->channel);
-		if (dec->channel == 0 &&
-				write_regs(client, global_registers, 0) < 0) {
-			dev_err(&client->dev,
-				"error initializing TW2804 global registers\n");
-			return 0;
-		}
-		if (write_regs(client, channel_registers, dec->channel) < 0) {
-			dev_err(&client->dev,
-				"error initializing TW2804 channel %d\n",
-				dec->channel);
-			return 0;
-		}
-		return 0;
-	}
-
-	if (dec->channel < 0) {
-		dev_dbg(&client->dev,
-			"ignoring command %08x until channel number is set\n",
-			cmd);
-		return 0;
-	}
-
-	switch (cmd) {
-	case VIDIOC_S_STD:
-	{
-		v4l2_std_id *input = arg;
-		u8 regs[] = {
-			0x01, *input & V4L2_STD_NTSC ? 0xc4 : 0x84,
-			0x09, *input & V4L2_STD_NTSC ? 0x07 : 0x04,
-			0x0a, *input & V4L2_STD_NTSC ? 0xf0 : 0x20,
-			0x0b, *input & V4L2_STD_NTSC ? 0x07 : 0x04,
-			0x0c, *input & V4L2_STD_NTSC ? 0xf0 : 0x20,
-			0x0d, *input & V4L2_STD_NTSC ? 0x40 : 0x4a,
-			0x16, *input & V4L2_STD_NTSC ? 0x00 : 0x40,
-			0x17, *input & V4L2_STD_NTSC ? 0x00 : 0x40,
-			0x20, *input & V4L2_STD_NTSC ? 0x07 : 0x0f,
-			0x21, *input & V4L2_STD_NTSC ? 0x07 : 0x0f,
-			0xff,	0xff,
-		};
-		write_regs(client, regs, dec->channel);
-		dec->norm = *input;
-		break;
-	}
-	case VIDIOC_QUERYCTRL:
-	{
-		struct v4l2_queryctrl *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Brightness", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 255;
-			ctrl->step = 1;
-			ctrl->default_value = 128;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Contrast", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 255;
-			ctrl->step = 1;
-			ctrl->default_value = 128;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_SATURATION:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Saturation", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 255;
-			ctrl->step = 1;
-			ctrl->default_value = 128;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_HUE:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Hue", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 255;
-			ctrl->step = 1;
-			ctrl->default_value = 128;
-			ctrl->flags = 0;
-			break;
-		}
-		break;
-	}
-	case VIDIOC_S_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			if (ctrl->value > 255)
-				dec->brightness = 255;
-			else if (ctrl->value < 0)
-				dec->brightness = 0;
-			else
-				dec->brightness = ctrl->value;
-			write_reg(client, 0x12, dec->brightness, dec->channel);
-			break;
-		case V4L2_CID_CONTRAST:
-			if (ctrl->value > 255)
-				dec->contrast = 255;
-			else if (ctrl->value < 0)
-				dec->contrast = 0;
-			else
-				dec->contrast = ctrl->value;
-			write_reg(client, 0x11, dec->contrast, dec->channel);
-			break;
-		case V4L2_CID_SATURATION:
-			if (ctrl->value > 255)
-				dec->saturation = 255;
-			else if (ctrl->value < 0)
-				dec->saturation = 0;
-			else
-				dec->saturation = ctrl->value;
-			write_reg(client, 0x10, dec->saturation, dec->channel);
-			break;
-		case V4L2_CID_HUE:
-			if (ctrl->value > 255)
-				dec->hue = 255;
-			else if (ctrl->value < 0)
-				dec->hue = 0;
-			else
-				dec->hue = ctrl->value;
-			write_reg(client, 0x0f, dec->hue, dec->channel);
-			break;
-		}
-		break;
-	}
-	case VIDIOC_G_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->value = dec->brightness;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->value = dec->contrast;
-			break;
-		case V4L2_CID_SATURATION:
-			ctrl->value = dec->saturation;
-			break;
-		case V4L2_CID_HUE:
-			ctrl->value = dec->hue;
-			break;
-		}
-		break;
-	}
-	default:
-		break;
-	}
-	return 0;
-}
-
-static int wis_tw2804_probe(struct i2c_client *client,
-			    const struct i2c_device_id *id)
-{
-	struct i2c_adapter *adapter = client->adapter;
-	struct wis_tw2804 *dec;
-
-	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
-		return -ENODEV;
-
-	dec = kmalloc(sizeof(struct wis_tw2804), GFP_KERNEL);
-	if (dec == NULL)
-		return -ENOMEM;
-
-	dec->channel = -1;
-	dec->norm = V4L2_STD_NTSC;
-	dec->brightness = 128;
-	dec->contrast = 128;
-	dec->saturation = 128;
-	dec->hue = 128;
-	i2c_set_clientdata(client, dec);
-
-	dev_dbg(&client->dev, "creating TW2804 at address %d on %s\n",
-		client->addr, adapter->name);
-
-	return 0;
-}
-
-static int wis_tw2804_remove(struct i2c_client *client)
-{
-	struct wis_tw2804 *dec = i2c_get_clientdata(client);
-
-	kfree(dec);
-	return 0;
-}
-
-static const struct i2c_device_id wis_tw2804_id[] = {
-	{ "wis_tw2804", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, wis_tw2804_id);
-
-static struct i2c_driver wis_tw2804_driver = {
-	.driver = {
-		.name	= "WIS TW2804 I2C driver",
-	},
-	.probe		= wis_tw2804_probe,
-	.remove		= wis_tw2804_remove,
-	.command	= wis_tw2804_command,
-	.id_table	= wis_tw2804_id,
-};
-
-module_i2c_driver(wis_tw2804_driver);
-
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/go7007/wis-tw9903.c b/drivers/staging/media/go7007/wis-tw9903.c
deleted file mode 100644
index 684ca37f0382..000000000000
--- a/drivers/staging/media/go7007/wis-tw9903.c
+++ /dev/null
@@ -1,328 +0,0 @@
-/*
- * Copyright (C) 2005-2006 Micronas USA Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License (Version 2) as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software Foundation,
- * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/i2c.h>
-#include <linux/videodev2.h>
-#include <linux/ioctl.h>
-#include <linux/slab.h>
-
-#include "wis-i2c.h"
-
-struct wis_tw9903 {
-	int norm;
-	int brightness;
-	int contrast;
-	int hue;
-};
-
-static u8 initial_registers[] = {
-	0x02, 0x44, /* input 1, composite */
-	0x03, 0x92, /* correct digital format */
-	0x04, 0x00,
-	0x05, 0x80, /* or 0x00 for PAL */
-	0x06, 0x40, /* second internal current reference */
-	0x07, 0x02, /* window */
-	0x08, 0x14, /* window */
-	0x09, 0xf0, /* window */
-	0x0a, 0x81, /* window */
-	0x0b, 0xd0, /* window */
-	0x0c, 0x8c,
-	0x0d, 0x00, /* scaling */
-	0x0e, 0x11, /* scaling */
-	0x0f, 0x00, /* scaling */
-	0x10, 0x00, /* brightness */
-	0x11, 0x60, /* contrast */
-	0x12, 0x01, /* sharpness */
-	0x13, 0x7f, /* U gain */
-	0x14, 0x5a, /* V gain */
-	0x15, 0x00, /* hue */
-	0x16, 0xc3, /* sharpness */
-	0x18, 0x00,
-	0x19, 0x58, /* vbi */
-	0x1a, 0x80,
-	0x1c, 0x0f, /* video norm */
-	0x1d, 0x7f, /* video norm */
-	0x20, 0xa0, /* clamping gain (working 0x50) */
-	0x21, 0x22,
-	0x22, 0xf0,
-	0x23, 0xfe,
-	0x24, 0x3c,
-	0x25, 0x38,
-	0x26, 0x44,
-	0x27, 0x20,
-	0x28, 0x00,
-	0x29, 0x15,
-	0x2a, 0xa0,
-	0x2b, 0x44,
-	0x2c, 0x37,
-	0x2d, 0x00,
-	0x2e, 0xa5, /* burst PLL control (working: a9) */
-	0x2f, 0xe0, /* 0xea is blue test frame -- 0xe0 for normal */
-	0x31, 0x00,
-	0x33, 0x22,
-	0x34, 0x11,
-	0x35, 0x35,
-	0x3b, 0x05,
-	0x06, 0xc0, /* reset device */
-	0x00, 0x00, /* Terminator (reg 0x00 is read-only) */
-};
-
-static int write_reg(struct i2c_client *client, u8 reg, u8 value)
-{
-	return i2c_smbus_write_byte_data(client, reg, value);
-}
-
-static int write_regs(struct i2c_client *client, u8 *regs)
-{
-	int i;
-
-	for (i = 0; regs[i] != 0x00; i += 2)
-		if (i2c_smbus_write_byte_data(client, regs[i], regs[i + 1]) < 0)
-			return -1;
-	return 0;
-}
-
-static int wis_tw9903_command(struct i2c_client *client,
-				unsigned int cmd, void *arg)
-{
-	struct wis_tw9903 *dec = i2c_get_clientdata(client);
-
-	switch (cmd) {
-	case VIDIOC_S_INPUT:
-	{
-		int *input = arg;
-
-		i2c_smbus_write_byte_data(client, 0x02, 0x40 | (*input << 1));
-		break;
-	}
-#if 0
-	/* The scaler on this thing seems to be horribly broken */
-	case DECODER_SET_RESOLUTION:
-	{
-		struct video_decoder_resolution *res = arg;
-		/*int hscale = 256 * 720 / res->width;*/
-		int hscale = 256 * 720 / (res->width - (res->width > 704 ? 0 : 8));
-		int vscale = 256 * (dec->norm & V4L2_STD_NTSC ?  240 : 288)
-				/ res->height;
-		u8 regs[] = {
-			0x0d, vscale & 0xff,
-			0x0f, hscale & 0xff,
-			0x0e, ((vscale & 0xf00) >> 4) | ((hscale & 0xf00) >> 8),
-			0x06, 0xc0, /* reset device */
-			0,	0,
-		};
-		dev_dbg(&client->dev, "vscale is %04x, hscale is %04x\n",
-			vscale, hscale);
-		/*write_regs(client, regs);*/
-		break;
-	}
-#endif
-	case VIDIOC_S_STD:
-	{
-		v4l2_std_id *input = arg;
-		u8 regs[] = {
-			0x05, *input & V4L2_STD_NTSC ? 0x80 : 0x00,
-			0x07, *input & V4L2_STD_NTSC ? 0x02 : 0x12,
-			0x08, *input & V4L2_STD_NTSC ? 0x14 : 0x18,
-			0x09, *input & V4L2_STD_NTSC ? 0xf0 : 0x20,
-			0,	0,
-		};
-		write_regs(client, regs);
-		dec->norm = *input;
-		break;
-	}
-	case VIDIOC_QUERYCTRL:
-	{
-		struct v4l2_queryctrl *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Brightness", sizeof(ctrl->name));
-			ctrl->minimum = -128;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 0x00;
-			ctrl->flags = 0;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Contrast", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 255;
-			ctrl->step = 1;
-			ctrl->default_value = 0x60;
-			ctrl->flags = 0;
-			break;
-#if 0
-		/* I don't understand how the Chroma Gain registers work... */
-		case V4L2_CID_SATURATION:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Saturation", sizeof(ctrl->name));
-			ctrl->minimum = 0;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 64;
-			ctrl->flags = 0;
-			break;
-#endif
-		case V4L2_CID_HUE:
-			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
-			strncpy(ctrl->name, "Hue", sizeof(ctrl->name));
-			ctrl->minimum = -128;
-			ctrl->maximum = 127;
-			ctrl->step = 1;
-			ctrl->default_value = 0;
-			ctrl->flags = 0;
-			break;
-		}
-		break;
-	}
-	case VIDIOC_S_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			if (ctrl->value > 127)
-				dec->brightness = 127;
-			else if (ctrl->value < -128)
-				dec->brightness = -128;
-			else
-				dec->brightness = ctrl->value;
-			write_reg(client, 0x10, dec->brightness);
-			break;
-		case V4L2_CID_CONTRAST:
-			if (ctrl->value > 255)
-				dec->contrast = 255;
-			else if (ctrl->value < 0)
-				dec->contrast = 0;
-			else
-				dec->contrast = ctrl->value;
-			write_reg(client, 0x11, dec->contrast);
-			break;
-#if 0
-		case V4L2_CID_SATURATION:
-			if (ctrl->value > 127)
-				dec->saturation = 127;
-			else if (ctrl->value < 0)
-				dec->saturation = 0;
-			else
-				dec->saturation = ctrl->value;
-			/*write_reg(client, 0x0c, dec->saturation);*/
-			break;
-#endif
-		case V4L2_CID_HUE:
-			if (ctrl->value > 127)
-				dec->hue = 127;
-			else if (ctrl->value < -128)
-				dec->hue = -128;
-			else
-				dec->hue = ctrl->value;
-			write_reg(client, 0x15, dec->hue);
-			break;
-		}
-		break;
-	}
-	case VIDIOC_G_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-
-		switch (ctrl->id) {
-		case V4L2_CID_BRIGHTNESS:
-			ctrl->value = dec->brightness;
-			break;
-		case V4L2_CID_CONTRAST:
-			ctrl->value = dec->contrast;
-			break;
-#if 0
-		case V4L2_CID_SATURATION:
-			ctrl->value = dec->saturation;
-			break;
-#endif
-		case V4L2_CID_HUE:
-			ctrl->value = dec->hue;
-			break;
-		}
-		break;
-	}
-	default:
-		break;
-	}
-	return 0;
-}
-
-static int wis_tw9903_probe(struct i2c_client *client,
-			    const struct i2c_device_id *id)
-{
-	struct i2c_adapter *adapter = client->adapter;
-	struct wis_tw9903 *dec;
-
-	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
-		return -ENODEV;
-
-	dec = kmalloc(sizeof(struct wis_tw9903), GFP_KERNEL);
-	if (dec == NULL)
-		return -ENOMEM;
-
-	dec->norm = V4L2_STD_NTSC;
-	dec->brightness = 0;
-	dec->contrast = 0x60;
-	dec->hue = 0;
-	i2c_set_clientdata(client, dec);
-
-	dev_dbg(&client->dev, "initializing TW9903 at address %d on %s\n",
-		client->addr, adapter->name);
-
-	if (write_regs(client, initial_registers) < 0) {
-		dev_err(&client->dev, "error initializing TW9903\n");
-		kfree(dec);
-		return -ENODEV;
-	}
-
-	return 0;
-}
-
-static int wis_tw9903_remove(struct i2c_client *client)
-{
-	struct wis_tw9903 *dec = i2c_get_clientdata(client);
-
-	kfree(dec);
-	return 0;
-}
-
-static const struct i2c_device_id wis_tw9903_id[] = {
-	{ "wis_tw9903", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, wis_tw9903_id);
-
-static struct i2c_driver wis_tw9903_driver = {
-	.driver = {
-		.name	= "WIS TW9903 I2C driver",
-	},
-	.probe		= wis_tw9903_probe,
-	.remove		= wis_tw9903_remove,
-	.command	= wis_tw9903_command,
-	.id_table	= wis_tw9903_id,
-};
-
-module_i2c_driver(wis_tw9903_driver);
-
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/media/lirc/lirc_sir.c b/drivers/staging/media/lirc/lirc_sir.c
index 63a554c36f75..f781c532b3c4 100644
--- a/drivers/staging/media/lirc/lirc_sir.c
+++ b/drivers/staging/media/lirc/lirc_sir.c
@@ -787,12 +787,6 @@ static int init_hardware(void)
 	spin_lock_irqsave(&hardware_lock, flags);
 	/* reset UART */
 #ifdef LIRC_ON_SA1100
-#ifdef CONFIG_SA1100_BITSY
-	if (machine_is_bitsy()) {
-		pr_info("Power on IR module\n");
-		set_bitsy_egpio(EGPIO_BITSY_IR_ON);
-	}
-#endif
 #ifdef CONFIG_SA1100_COLLIE
 	sa1100_irda_set_power_collie(3);	/* power on */
 #endif
@@ -942,10 +936,6 @@ static void drop_hardware(void)
 	Ser2UTCR3 = sr.utcr3;
 
 	Ser2HSCR0 = sr.hscr0;
-#ifdef CONFIG_SA1100_BITSY
-	if (machine_is_bitsy())
-		clr_bitsy_egpio(EGPIO_BITSY_IR_ON);
-#endif
 #ifdef CONFIG_SA1100_COLLIE
 	sa1100_irda_set_power_collie(0);	/* power off */
 #endif
diff --git a/drivers/staging/media/solo6x10/Kconfig b/drivers/staging/media/solo6x10/Kconfig
index 63352de5eabf..ec32776ff547 100644
--- a/drivers/staging/media/solo6x10/Kconfig
+++ b/drivers/staging/media/solo6x10/Kconfig
@@ -1,7 +1,8 @@
 config SOLO6X10
 	tristate "Softlogic 6x10 MPEG codec cards"
 	depends on PCI && VIDEO_DEV && SND && I2C
-	select VIDEOBUF_DMA_SG
+	select VIDEOBUF2_DMA_SG
+	select VIDEOBUF2_DMA_CONTIG
 	select SND_PCM
 	---help---
 	  This driver supports the Softlogic based MPEG-4 and h.264 codec
diff --git a/drivers/staging/media/solo6x10/Makefile b/drivers/staging/media/solo6x10/Makefile
index 337e38c3a0f0..7aae118947b2 100644
--- a/drivers/staging/media/solo6x10/Makefile
+++ b/drivers/staging/media/solo6x10/Makefile
@@ -1,3 +1,5 @@
-solo6x10-y := core.o i2c.o p2m.o v4l2.o tw28.o gpio.o disp.o enc.o v4l2-enc.o g723.o
+solo6x10-y := solo6x10-core.o solo6x10-i2c.o solo6x10-p2m.o solo6x10-v4l2.o \
+		solo6x10-tw28.o solo6x10-gpio.o solo6x10-disp.o solo6x10-enc.o \
+		solo6x10-v4l2-enc.o solo6x10-g723.o solo6x10-eeprom.o
 
 obj-$(CONFIG_SOLO6X10) += solo6x10.o
diff --git a/drivers/staging/media/solo6x10/TODO b/drivers/staging/media/solo6x10/TODO
index 539f739fe9e6..7b8db75b1acb 100644
--- a/drivers/staging/media/solo6x10/TODO
+++ b/drivers/staging/media/solo6x10/TODO
@@ -1,24 +1,15 @@
-TODO (staging => main):
-
-	* Motion detection flags need to be moved to v4l2
-	* Some private CIDs need to be moved to v4l2
-
-TODO (general):
-
-	* encoder on/off controls
-	* mpeg cid bitrate mode (vbr/cbr)
-	* mpeg cid bitrate/bitrate-peak
-	* mpeg encode of user data
-	* mpeg decode of user data
-	* switch between 4 frames/irq to 1 when using mjpeg (and then back
-	  when not)
-	* implement a CID control for motion areas/thresholds
-	* implement CID controls for mozaic areas
-	* allow for higher level of interval (for < 1 fps)
-	* sound:
-	  - implement playback via external sound jack
-	  - implement loopback of external sound jack with incoming audio?
-	  - implement pause/resume
-
-Plase send patches to Mauro Carvalho Chehab <mchehab@redhat.com> and Cc Ben Collins
-<bcollins@bluecherry.net>
+- batch up desc requests for more efficient use of p2m?
+- encoder on/off controls
+- mpeg cid bitrate mode (vbr/cbr)
+- mpeg cid bitrate/bitrate-peak
+- mpeg encode of user data
+- mpeg decode of user data
+- implement CID controls for mozaic areas
+
+- sound
+ - implement playback via external sound jack
+ - implement loopback of external sound jack with incoming audio?
+ - implement pause/resume (make use of in bc-server)
+
+Please send patches to the linux media list <linux-media@vger.kernel.org> and
+Cc Ismael Luceno <ismael.luceno@corp.bluecherry.net>.
diff --git a/drivers/staging/media/solo6x10/core.c b/drivers/staging/media/solo6x10/core.c
deleted file mode 100644
index fd83d6d028bf..000000000000
--- a/drivers/staging/media/solo6x10/core.c
+++ /dev/null
@@ -1,321 +0,0 @@
-/*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
- *
- * 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/kernel.h>
-#include <linux/module.h>
-#include <linux/pci.h>
-#include <linux/interrupt.h>
-#include <linux/slab.h>
-#include <linux/videodev2.h>
-#include "solo6x10.h"
-#include "tw28.h"
-
-MODULE_DESCRIPTION("Softlogic 6x10 MP4/H.264 Encoder/Decoder V4L2/ALSA Driver");
-MODULE_AUTHOR("Ben Collins <bcollins@bluecherry.net>");
-MODULE_VERSION(SOLO6X10_VERSION);
-MODULE_LICENSE("GPL");
-
-void solo_irq_on(struct solo_dev *solo_dev, u32 mask)
-{
-	solo_dev->irq_mask |= mask;
-	solo_reg_write(solo_dev, SOLO_IRQ_ENABLE, solo_dev->irq_mask);
-}
-
-void solo_irq_off(struct solo_dev *solo_dev, u32 mask)
-{
-	solo_dev->irq_mask &= ~mask;
-	solo_reg_write(solo_dev, SOLO_IRQ_ENABLE, solo_dev->irq_mask);
-}
-
-/* XXX We should check the return value of the sub-device ISR's */
-static irqreturn_t solo_isr(int irq, void *data)
-{
-	struct solo_dev *solo_dev = data;
-	u32 status;
-	int i;
-
-	status = solo_reg_read(solo_dev, SOLO_IRQ_STAT);
-	if (!status)
-		return IRQ_NONE;
-
-	if (status & ~solo_dev->irq_mask) {
-		solo_reg_write(solo_dev, SOLO_IRQ_STAT,
-			       status & ~solo_dev->irq_mask);
-		status &= solo_dev->irq_mask;
-	}
-
-	if (status & SOLO_IRQ_PCI_ERR) {
-		u32 err = solo_reg_read(solo_dev, SOLO_PCI_ERR);
-		solo_p2m_error_isr(solo_dev, err);
-		solo_reg_write(solo_dev, SOLO_IRQ_STAT, SOLO_IRQ_PCI_ERR);
-	}
-
-	for (i = 0; i < SOLO_NR_P2M; i++)
-		if (status & SOLO_IRQ_P2M(i))
-			solo_p2m_isr(solo_dev, i);
-
-	if (status & SOLO_IRQ_IIC)
-		solo_i2c_isr(solo_dev);
-
-	if (status & SOLO_IRQ_VIDEO_IN)
-		solo_video_in_isr(solo_dev);
-
-	/* Call this first so enc gets detected flag set */
-	if (status & SOLO_IRQ_MOTION)
-		solo_motion_isr(solo_dev);
-
-	if (status & SOLO_IRQ_ENCODER)
-		solo_enc_v4l2_isr(solo_dev);
-
-	if (status & SOLO_IRQ_G723)
-		solo_g723_isr(solo_dev);
-
-	return IRQ_HANDLED;
-}
-
-static void free_solo_dev(struct solo_dev *solo_dev)
-{
-	struct pci_dev *pdev;
-
-	if (!solo_dev)
-		return;
-
-	pdev = solo_dev->pdev;
-
-	/* If we never initialized the PCI device, then nothing else
-	 * below here needs cleanup */
-	if (!pdev) {
-		kfree(solo_dev);
-		return;
-	}
-
-	/* Bring down the sub-devices first */
-	solo_g723_exit(solo_dev);
-	solo_enc_v4l2_exit(solo_dev);
-	solo_enc_exit(solo_dev);
-	solo_v4l2_exit(solo_dev);
-	solo_disp_exit(solo_dev);
-	solo_gpio_exit(solo_dev);
-	solo_p2m_exit(solo_dev);
-	solo_i2c_exit(solo_dev);
-
-	/* Now cleanup the PCI device */
-	if (solo_dev->reg_base) {
-		solo_irq_off(solo_dev, ~0);
-		pci_iounmap(pdev, solo_dev->reg_base);
-		free_irq(pdev->irq, solo_dev);
-	}
-
-	pci_release_regions(pdev);
-	pci_disable_device(pdev);
-	pci_set_drvdata(pdev, NULL);
-
-	kfree(solo_dev);
-}
-
-static int solo_pci_probe(struct pci_dev *pdev,
-				    const struct pci_device_id *id)
-{
-	struct solo_dev *solo_dev;
-	int ret;
-	int sdram;
-	u8 chip_id;
-	u32 reg;
-
-	solo_dev = kzalloc(sizeof(*solo_dev), GFP_KERNEL);
-	if (solo_dev == NULL)
-		return -ENOMEM;
-
-	solo_dev->pdev = pdev;
-	spin_lock_init(&solo_dev->reg_io_lock);
-	pci_set_drvdata(pdev, solo_dev);
-
-	ret = pci_enable_device(pdev);
-	if (ret)
-		goto fail_probe;
-
-	pci_set_master(pdev);
-
-	ret = pci_request_regions(pdev, SOLO6X10_NAME);
-	if (ret)
-		goto fail_probe;
-
-	solo_dev->reg_base = pci_ioremap_bar(pdev, 0);
-	if (solo_dev->reg_base == NULL) {
-		ret = -ENOMEM;
-		goto fail_probe;
-	}
-
-	chip_id = solo_reg_read(solo_dev, SOLO_CHIP_OPTION) &
-					SOLO_CHIP_ID_MASK;
-	switch (chip_id) {
-	case 7:
-		solo_dev->nr_chans = 16;
-		solo_dev->nr_ext = 5;
-		break;
-	case 6:
-		solo_dev->nr_chans = 8;
-		solo_dev->nr_ext = 2;
-		break;
-	default:
-		dev_warn(&pdev->dev, "Invalid chip_id 0x%02x, "
-			 "defaulting to 4 channels\n",
-			 chip_id);
-	case 5:
-		solo_dev->nr_chans = 4;
-		solo_dev->nr_ext = 1;
-	}
-
-	solo_dev->flags = id->driver_data;
-
-	/* Disable all interrupts to start */
-	solo_irq_off(solo_dev, ~0);
-
-	reg = SOLO_SYS_CFG_SDRAM64BIT;
-	/* Initial global settings */
-	if (!(solo_dev->flags & FLAGS_6110))
-		reg |= SOLO6010_SYS_CFG_INPUTDIV(25) |
-			SOLO6010_SYS_CFG_FEEDBACKDIV((SOLO_CLOCK_MHZ * 2) - 2) |
-			SOLO6010_SYS_CFG_OUTDIV(3);
-	solo_reg_write(solo_dev, SOLO_SYS_CFG, reg);
-
-	if (solo_dev->flags & FLAGS_6110) {
-		u32 sys_clock_MHz = SOLO_CLOCK_MHZ;
-		u32 pll_DIVQ;
-		u32 pll_DIVF;
-
-		if (sys_clock_MHz < 125) {
-			pll_DIVQ = 3;
-			pll_DIVF = (sys_clock_MHz * 4) / 3;
-		} else {
-			pll_DIVQ = 2;
-			pll_DIVF = (sys_clock_MHz * 2) / 3;
-		}
-
-		solo_reg_write(solo_dev, SOLO6110_PLL_CONFIG,
-			       SOLO6110_PLL_RANGE_5_10MHZ |
-			       SOLO6110_PLL_DIVR(9) |
-			       SOLO6110_PLL_DIVQ_EXP(pll_DIVQ) |
-			       SOLO6110_PLL_DIVF(pll_DIVF) | SOLO6110_PLL_FSEN);
-		mdelay(1);      /* PLL Locking time (1ms) */
-
-		solo_reg_write(solo_dev, SOLO_DMA_CTRL1, 3 << 8); /* ? */
-	} else
-		solo_reg_write(solo_dev, SOLO_DMA_CTRL1, 1 << 8); /* ? */
-
-	solo_reg_write(solo_dev, SOLO_TIMER_CLOCK_NUM, SOLO_CLOCK_MHZ - 1);
-
-	/* PLL locking time of 1ms */
-	mdelay(1);
-
-	ret = request_irq(pdev->irq, solo_isr, IRQF_SHARED, SOLO6X10_NAME,
-			  solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	/* Handle this from the start */
-	solo_irq_on(solo_dev, SOLO_IRQ_PCI_ERR);
-
-	ret = solo_i2c_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	/* Setup the DMA engine */
-	sdram = (solo_dev->nr_chans >= 8) ? 2 : 1;
-	solo_reg_write(solo_dev, SOLO_DMA_CTRL,
-		       SOLO_DMA_CTRL_REFRESH_CYCLE(1) |
-		       SOLO_DMA_CTRL_SDRAM_SIZE(sdram) |
-		       SOLO_DMA_CTRL_SDRAM_CLK_INVERT |
-		       SOLO_DMA_CTRL_READ_CLK_SELECT |
-		       SOLO_DMA_CTRL_LATENCY(1));
-
-	ret = solo_p2m_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	ret = solo_disp_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	ret = solo_gpio_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	ret = solo_tw28_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	ret = solo_v4l2_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	ret = solo_enc_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	ret = solo_enc_v4l2_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	ret = solo_g723_init(solo_dev);
-	if (ret)
-		goto fail_probe;
-
-	return 0;
-
-fail_probe:
-	free_solo_dev(solo_dev);
-	return ret;
-}
-
-static void solo_pci_remove(struct pci_dev *pdev)
-{
-	struct solo_dev *solo_dev = pci_get_drvdata(pdev);
-
-	free_solo_dev(solo_dev);
-}
-
-static struct pci_device_id solo_id_table[] = {
-	/* 6010 based cards */
-	{PCI_DEVICE(PCI_VENDOR_ID_SOFTLOGIC, PCI_DEVICE_ID_SOLO6010)},
-	{PCI_DEVICE(PCI_VENDOR_ID_SOFTLOGIC, PCI_DEVICE_ID_SOLO6110),
-	 .driver_data = FLAGS_6110},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_NEUSOLO_4)},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_NEUSOLO_9)},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_NEUSOLO_16)},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_SOLO_4)},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_SOLO_9)},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_SOLO_16)},
-	/* 6110 based cards */
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_6110_4)},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_6110_8)},
-	{PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_6110_16)},
-	{0,}
-};
-
-MODULE_DEVICE_TABLE(pci, solo_id_table);
-
-static struct pci_driver solo_pci_driver = {
-	.name = SOLO6X10_NAME,
-	.id_table = solo_id_table,
-	.probe = solo_pci_probe,
-	.remove = solo_pci_remove,
-};
-
-module_pci_driver(solo_pci_driver);
diff --git a/drivers/staging/media/solo6x10/offsets.h b/drivers/staging/media/solo6x10/offsets.h
deleted file mode 100644
index 3d7e569f1cf8..000000000000
--- a/drivers/staging/media/solo6x10/offsets.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
- *
- * 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.
- */
-
-#ifndef __SOLO6X10_OFFSETS_H
-#define __SOLO6X10_OFFSETS_H
-
-/* Offsets and sizes of the external address */
-#define SOLO_DISP_EXT_ADDR			0x00000000
-#define SOLO_DISP_EXT_SIZE			0x00480000
-
-#define SOLO_DEC2LIVE_EXT_ADDR (SOLO_DISP_EXT_ADDR + SOLO_DISP_EXT_SIZE)
-#define SOLO_DEC2LIVE_EXT_SIZE			0x00240000
-
-#define SOLO_OSG_EXT_ADDR (SOLO_DEC2LIVE_EXT_ADDR + SOLO_DEC2LIVE_EXT_SIZE)
-#define SOLO_OSG_EXT_SIZE			0x00120000
-
-#define SOLO_EOSD_EXT_ADDR (SOLO_OSG_EXT_ADDR + SOLO_OSG_EXT_SIZE)
-#define SOLO_EOSD_EXT_SIZE			0x00010000
-
-#define SOLO_MOTION_EXT_ADDR(__solo) (SOLO_EOSD_EXT_ADDR +	\
-				      (SOLO_EOSD_EXT_SIZE * __solo->nr_chans))
-#define SOLO_MOTION_EXT_SIZE			0x00080000
-
-#define SOLO_G723_EXT_ADDR(__solo) \
-		(SOLO_MOTION_EXT_ADDR(__solo) + SOLO_MOTION_EXT_SIZE)
-#define SOLO_G723_EXT_SIZE			0x00010000
-
-#define SOLO_CAP_EXT_ADDR(__solo) \
-		(SOLO_G723_EXT_ADDR(__solo) + SOLO_G723_EXT_SIZE)
-#define SOLO_CAP_EXT_MAX_PAGE			(18 + 15)
-#define SOLO_CAP_EXT_SIZE			(SOLO_CAP_EXT_MAX_PAGE * 65536)
-
-/* This +1 is very important -- Why?! -- BenC */
-#define SOLO_EREF_EXT_ADDR(__solo) \
-		(SOLO_CAP_EXT_ADDR(__solo) + \
-		 (SOLO_CAP_EXT_SIZE * (__solo->nr_chans + 1)))
-#define SOLO_EREF_EXT_SIZE			0x00140000
-
-#define SOLO_MP4E_EXT_ADDR(__solo) \
-		(SOLO_EREF_EXT_ADDR(__solo) + \
-		 (SOLO_EREF_EXT_SIZE * __solo->nr_chans))
-#define SOLO_MP4E_EXT_SIZE(__solo)		(0x00080000 * __solo->nr_chans)
-
-#define SOLO_DREF_EXT_ADDR(__solo) \
-		(SOLO_MP4E_EXT_ADDR(__solo) + SOLO_MP4E_EXT_SIZE(__solo))
-#define SOLO_DREF_EXT_SIZE			0x00140000
-
-#define SOLO_MP4D_EXT_ADDR(__solo) \
-		(SOLO_DREF_EXT_ADDR(__solo) + \
-		 (SOLO_DREF_EXT_SIZE * __solo->nr_chans))
-#define SOLO_MP4D_EXT_SIZE			0x00080000
-
-#define SOLO_JPEG_EXT_ADDR(__solo) \
-		(SOLO_MP4D_EXT_ADDR(__solo) + \
-		 (SOLO_MP4D_EXT_SIZE * __solo->nr_chans))
-#define SOLO_JPEG_EXT_SIZE(__solo)		(0x00080000 * __solo->nr_chans)
-
-#endif /* __SOLO6X10_OFFSETS_H */
diff --git a/drivers/staging/media/solo6x10/osd-font.h b/drivers/staging/media/solo6x10/osd-font.h
deleted file mode 100644
index 591e0e82e0e8..000000000000
--- a/drivers/staging/media/solo6x10/osd-font.h
+++ /dev/null
@@ -1,154 +0,0 @@
-/*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
- *
- * 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.
- */
-
-#ifndef __SOLO6X10_OSD_FONT_H
-#define __SOLO6X10_OSD_FONT_H
-
-static const unsigned int solo_osd_font[] = {
-	0x00000000, 0x0000c0c8, 0xccfefe0c, 0x08000000,
-	0x00000000, 0x10103838, 0x7c7cfefe, 0x00000000,	/* 0 */
-	0x00000000, 0xfefe7c7c, 0x38381010, 0x10000000,
-	0x00000000, 0x7c82fefe, 0xfefefe7c, 0x00000000,
-	0x00000000, 0x00001038, 0x10000000, 0x00000000,
-	0x00000000, 0x0010387c, 0xfe7c3810, 0x00000000,
-	0x00000000, 0x00384444, 0x44380000, 0x00000000,
-	0x00000000, 0x38448282, 0x82443800, 0x00000000,
-	0x00000000, 0x007c7c7c, 0x7c7c0000, 0x00000000,
-	0x00000000, 0x6c6c6c6c, 0x6c6c6c6c, 0x00000000,
-	0x00000000, 0x061e7efe, 0xfe7e1e06, 0x00000000,
-	0x00000000, 0xc0f0fcfe, 0xfefcf0c0, 0x00000000,
-	0x00000000, 0xc6cedefe, 0xfedecec6, 0x00000000,
-	0x00000000, 0xc6e6f6fe, 0xfef6e6c6, 0x00000000,
-	0x00000000, 0x12367efe, 0xfe7e3612, 0x00000000,
-	0x00000000, 0x90d8fcfe, 0xfefcd890, 0x00000000,
-	0x00000038, 0x7cc692ba, 0x92c67c38, 0x00000000,
-	0x00000038, 0x7cc6aa92, 0xaac67c38, 0x00000000,
-	0x00000038, 0x7830107c, 0xbaa8680c, 0x00000000,
-	0x00000038, 0x3c18127c, 0xb8382c60, 0x00000000,
-	0x00000044, 0xaa6c8254, 0x38eec67c, 0x00000000,
-	0x00000082, 0x44288244, 0x38c6827c, 0x00000000,
-	0x00000038, 0x444444fe, 0xfeeec6fe, 0x00000000,
-	0x00000018, 0x78187818, 0x3c7e7e3c, 0x00000000,
-	0x00000000, 0x3854929a, 0x82443800, 0x00000000,
-	0x00000000, 0x00c0c8cc, 0xfefe0c08, 0x00000000,
-	0x0000e0a0, 0xe040e00e, 0x8a0ea40e, 0x00000000,
-	0x0000e0a0, 0xe040e00e, 0x0a8e440e, 0x00000000,
-	0x0000007c, 0x82829292, 0x929282fe, 0x00000000,
-	0x000000f8, 0xfc046494, 0x946404fc, 0x00000000,
-	0x0000003f, 0x7f404c52, 0x524c407f, 0x00000000,
-	0x0000007c, 0x82ba82ba, 0x82ba82fe, 0x00000000,
-	0x00000000, 0x00000000, 0x00000000, 0x00000000,
-	0x00000000, 0x183c3c3c, 0x18180018, 0x18000000,	/* 32   ! */
-	0x00000066, 0x66240000, 0x00000000, 0x00000000,
-	0x00000000, 0x6c6cfe6c, 0x6c6cfe6c, 0x6c000000,	/* 34 " # */
-	0x00001010, 0x7cd6d616, 0x7cd0d6d6, 0x7c101000,
-	0x00000000, 0x0086c660, 0x30180cc6, 0xc2000000,	/* 36 $ % */
-	0x00000000, 0x386c6c38, 0xdc766666, 0xdc000000,
-	0x0000000c, 0x0c0c0600, 0x00000000, 0x00000000,	/* 38 & ' */
-	0x00000000, 0x30180c0c, 0x0c0c0c18, 0x30000000,
-	0x00000000, 0x0c183030, 0x30303018, 0x0c000000,	/* 40 ( ) */
-	0x00000000, 0x0000663c, 0xff3c6600, 0x00000000,
-	0x00000000, 0x00001818, 0x7e181800, 0x00000000,	/* 42 * + */
-	0x00000000, 0x00000000, 0x00000e0e, 0x0c060000,
-	0x00000000, 0x00000000, 0x7e000000, 0x00000000,	/* 44 , - */
-	0x00000000, 0x00000000, 0x00000006, 0x06000000,
-	0x00000000, 0x80c06030, 0x180c0602, 0x00000000,	/* 46 . / */
-	0x0000007c, 0xc6e6f6de, 0xcec6c67c, 0x00000000,
-	0x00000030, 0x383c3030, 0x303030fc, 0x00000000,	/* 48 0 1 */
-	0x0000007c, 0xc6c06030, 0x180cc6fe, 0x00000000,
-	0x0000007c, 0xc6c0c07c, 0xc0c0c67c, 0x00000000,	/* 50 2 3 */
-	0x00000060, 0x70786c66, 0xfe6060f0, 0x00000000,
-	0x000000fe, 0x0606067e, 0xc0c0c67c, 0x00000000,	/* 52 4 5 */
-	0x00000038, 0x0c06067e, 0xc6c6c67c, 0x00000000,
-	0x000000fe, 0xc6c06030, 0x18181818, 0x00000000,	/* 54 6 7 */
-	0x0000007c, 0xc6c6c67c, 0xc6c6c67c, 0x00000000,
-	0x0000007c, 0xc6c6c6fc, 0xc0c06038, 0x00000000,	/* 56 8 9 */
-	0x00000000, 0x18180000, 0x00181800, 0x00000000,
-	0x00000000, 0x18180000, 0x0018180c, 0x00000000,	/* 58 : ; */
-	0x00000060, 0x30180c06, 0x0c183060, 0x00000000,
-	0x00000000, 0x007e0000, 0x007e0000, 0x00000000,
-	0x00000006, 0x0c183060, 0x30180c06, 0x00000000,
-	0x0000007c, 0xc6c66030, 0x30003030, 0x00000000,
-	0x0000007c, 0xc6f6d6d6, 0x7606067c, 0x00000000,
-	0x00000010, 0x386cc6c6, 0xfec6c6c6, 0x00000000,	/* 64 @ A */
-	0x0000007e, 0xc6c6c67e, 0xc6c6c67e, 0x00000000,
-	0x00000078, 0xcc060606, 0x0606cc78, 0x00000000,	/* 66 */
-	0x0000003e, 0x66c6c6c6, 0xc6c6663e, 0x00000000,
-	0x000000fe, 0x0606063e, 0x060606fe, 0x00000000,	/* 68 */
-	0x000000fe, 0x0606063e, 0x06060606, 0x00000000,
-	0x00000078, 0xcc060606, 0xf6c6ccb8, 0x00000000,	/* 70 */
-	0x000000c6, 0xc6c6c6fe, 0xc6c6c6c6, 0x00000000,
-	0x0000003c, 0x18181818, 0x1818183c, 0x00000000,	/* 72 */
-	0x00000060, 0x60606060, 0x6066663c, 0x00000000,
-	0x000000c6, 0xc666361e, 0x3666c6c6, 0x00000000,	/* 74 */
-	0x00000006, 0x06060606, 0x060606fe, 0x00000000,
-	0x000000c6, 0xeefed6c6, 0xc6c6c6c6, 0x00000000,	/* 76 */
-	0x000000c6, 0xcedefef6, 0xe6c6c6c6, 0x00000000,
-	0x00000038, 0x6cc6c6c6, 0xc6c66c38, 0x00000000,	/* 78 */
-	0x0000007e, 0xc6c6c67e, 0x06060606, 0x00000000,
-	0x00000038, 0x6cc6c6c6, 0xc6d67c38, 0x60000000,	/* 80 */
-	0x0000007e, 0xc6c6c67e, 0x66c6c6c6, 0x00000000,
-	0x0000007c, 0xc6c60c38, 0x60c6c67c, 0x00000000,	/* 82 */
-	0x0000007e, 0x18181818, 0x18181818, 0x00000000,
-	0x000000c6, 0xc6c6c6c6, 0xc6c6c67c, 0x00000000,	/* 84 */
-	0x000000c6, 0xc6c6c6c6, 0xc66c3810, 0x00000000,
-	0x000000c6, 0xc6c6c6c6, 0xd6d6fe6c, 0x00000000,	/* 86 */
-	0x000000c6, 0xc6c66c38, 0x6cc6c6c6, 0x00000000,
-	0x00000066, 0x66666666, 0x3c181818, 0x00000000,	/* 88 */
-	0x000000fe, 0xc0603018, 0x0c0606fe, 0x00000000,
-	0x0000003c, 0x0c0c0c0c, 0x0c0c0c3c, 0x00000000,	/* 90 */
-	0x00000002, 0x060c1830, 0x60c08000, 0x00000000,
-	0x0000003c, 0x30303030, 0x3030303c, 0x00000000,	/* 92 */
-	0x00001038, 0x6cc60000, 0x00000000, 0x00000000,
-	0x00000000, 0x00000000, 0x00000000, 0x00fe0000,
-	0x00001818, 0x30000000, 0x00000000, 0x00000000,
-	0x00000000, 0x00003c60, 0x7c66667c, 0x00000000,
-	0x0000000c, 0x0c0c7ccc, 0xcccccc7c, 0x00000000,
-	0x00000000, 0x00007cc6, 0x0606c67c, 0x00000000,
-	0x00000060, 0x60607c66, 0x6666667c, 0x00000000,
-	0x00000000, 0x00007cc6, 0xfe06c67c, 0x00000000,
-	0x00000078, 0x0c0c0c3e, 0x0c0c0c0c, 0x00000000,
-	0x00000000, 0x00007c66, 0x6666667c, 0x60603e00,
-	0x0000000c, 0x0c0c7ccc, 0xcccccccc, 0x00000000,
-	0x00000030, 0x30003830, 0x30303078, 0x00000000,
-	0x00000030, 0x30003c30, 0x30303030, 0x30301f00,
-	0x0000000c, 0x0c0ccc6c, 0x3c6ccccc, 0x00000000,
-	0x00000030, 0x30303030, 0x30303030, 0x00000000,
-	0x00000000, 0x000066fe, 0xd6d6d6d6, 0x00000000,
-	0x00000000, 0x000078cc, 0xcccccccc, 0x00000000,
-	0x00000000, 0x00007cc6, 0xc6c6c67c, 0x00000000,
-	0x00000000, 0x00007ccc, 0xcccccc7c, 0x0c0c0c00,
-	0x00000000, 0x00007c66, 0x6666667c, 0x60606000,
-	0x00000000, 0x000076dc, 0x0c0c0c0c, 0x00000000,
-	0x00000000, 0x00007cc6, 0x1c70c67c, 0x00000000,
-	0x00000000, 0x1818fe18, 0x18181870, 0x00000000,
-	0x00000000, 0x00006666, 0x6666663c, 0x00000000,
-	0x00000000, 0x0000c6c6, 0xc66c3810, 0x00000000,
-	0x00000000, 0x0000c6d6, 0xd6d6fe6c, 0x00000000,
-	0x00000000, 0x0000c66c, 0x38386cc6, 0x00000000,
-	0x00000000, 0x00006666, 0x6666667c, 0x60603e00,
-	0x00000000, 0x0000fe60, 0x30180cfe, 0x00000000,
-	0x00000070, 0x1818180e, 0x18181870, 0x00000000,
-	0x00000018, 0x18181800, 0x18181818, 0x00000000,
-	0x0000000e, 0x18181870, 0x1818180e, 0x00000000,
-	0x000000dc, 0x76000000, 0x00000000, 0x00000000,
-	0x00000000, 0x0010386c, 0xc6c6fe00, 0x00000000
-};
-
-#endif /* __SOLO6X10_OSD_FONT_H */
diff --git a/drivers/staging/media/solo6x10/p2m.c b/drivers/staging/media/solo6x10/p2m.c
deleted file mode 100644
index 58ab61b1f1d9..000000000000
--- a/drivers/staging/media/solo6x10/p2m.c
+++ /dev/null
@@ -1,306 +0,0 @@
-/*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
- *
- * 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/kernel.h>
-#include <linux/slab.h>
-#include <linux/scatterlist.h>
-#include "solo6x10.h"
-
-/* #define SOLO_TEST_P2M */
-
-int solo_p2m_dma(struct solo_dev *solo_dev, u8 id, int wr,
-		 void *sys_addr, u32 ext_addr, u32 size)
-{
-	dma_addr_t dma_addr;
-	int ret;
-
-	WARN_ON(!size);
-	BUG_ON(id >= SOLO_NR_P2M);
-
-	if (!size)
-		return -EINVAL;
-
-	dma_addr = pci_map_single(solo_dev->pdev, sys_addr, size,
-				  wr ? PCI_DMA_TODEVICE : PCI_DMA_FROMDEVICE);
-
-	ret = solo_p2m_dma_t(solo_dev, id, wr, dma_addr, ext_addr, size);
-
-	pci_unmap_single(solo_dev->pdev, dma_addr, size,
-			 wr ? PCI_DMA_TODEVICE : PCI_DMA_FROMDEVICE);
-
-	return ret;
-}
-
-int solo_p2m_dma_t(struct solo_dev *solo_dev, u8 id, int wr,
-		   dma_addr_t dma_addr, u32 ext_addr, u32 size)
-{
-	struct p2m_desc *desc = kzalloc(sizeof(*desc) * 2, GFP_DMA);
-	int ret;
-
-	if (desc == NULL)
-		return -ENOMEM;
-
-	solo_p2m_push_desc(&desc[1], wr, dma_addr, ext_addr, size, 0, 0);
-	ret = solo_p2m_dma_desc(solo_dev, id, desc, 2);
-	kfree(desc);
-
-	return ret;
-}
-
-void solo_p2m_push_desc(struct p2m_desc *desc, int wr, dma_addr_t dma_addr,
-			u32 ext_addr, u32 size, int repeat, u32 ext_size)
-{
-	desc->ta = cpu_to_le32(dma_addr);
-	desc->fa = cpu_to_le32(ext_addr);
-
-	desc->ext = cpu_to_le32(SOLO_P2M_COPY_SIZE(size >> 2));
-	desc->ctrl = cpu_to_le32(SOLO_P2M_BURST_SIZE(SOLO_P2M_BURST_256) |
-				 (wr ? SOLO_P2M_WRITE : 0) | SOLO_P2M_TRANS_ON);
-
-	/* Ext size only matters when we're repeating */
-	if (repeat) {
-		desc->ext |= cpu_to_le32(SOLO_P2M_EXT_INC(ext_size >> 2));
-		desc->ctrl |=  cpu_to_le32(SOLO_P2M_PCI_INC(size >> 2) |
-					   SOLO_P2M_REPEAT(repeat));
-	}
-}
-
-int solo_p2m_dma_desc(struct solo_dev *solo_dev, u8 id,
-		      struct p2m_desc *desc, int desc_count)
-{
-	struct solo_p2m_dev *p2m_dev;
-	unsigned int timeout;
-	int ret = 0;
-	u32 config = 0;
-	dma_addr_t desc_dma = 0;
-
-	BUG_ON(id >= SOLO_NR_P2M);
-	BUG_ON(!desc_count || desc_count > SOLO_NR_P2M_DESC);
-
-	p2m_dev = &solo_dev->p2m_dev[id];
-
-	mutex_lock(&p2m_dev->mutex);
-
-	solo_reg_write(solo_dev, SOLO_P2M_CONTROL(id), 0);
-
-	INIT_COMPLETION(p2m_dev->completion);
-	p2m_dev->error = 0;
-
-	/* Enable the descriptors */
-	config = solo_reg_read(solo_dev, SOLO_P2M_CONFIG(id));
-	desc_dma = pci_map_single(solo_dev->pdev, desc,
-				  desc_count * sizeof(*desc),
-				  PCI_DMA_TODEVICE);
-	solo_reg_write(solo_dev, SOLO_P2M_DES_ADR(id), desc_dma);
-	solo_reg_write(solo_dev, SOLO_P2M_DESC_ID(id), desc_count - 1);
-	solo_reg_write(solo_dev, SOLO_P2M_CONFIG(id), config |
-		       SOLO_P2M_DESC_MODE);
-
-	/* Should have all descriptors completed from one interrupt */
-	timeout = wait_for_completion_timeout(&p2m_dev->completion, HZ);
-
-	solo_reg_write(solo_dev, SOLO_P2M_CONTROL(id), 0);
-
-	/* Reset back to non-descriptor mode */
-	solo_reg_write(solo_dev, SOLO_P2M_CONFIG(id), config);
-	solo_reg_write(solo_dev, SOLO_P2M_DESC_ID(id), 0);
-	solo_reg_write(solo_dev, SOLO_P2M_DES_ADR(id), 0);
-	pci_unmap_single(solo_dev->pdev, desc_dma,
-			 desc_count * sizeof(*desc),
-			 PCI_DMA_TODEVICE);
-
-	if (p2m_dev->error)
-		ret = -EIO;
-	else if (timeout == 0)
-		ret = -EAGAIN;
-
-	mutex_unlock(&p2m_dev->mutex);
-
-	WARN_ON_ONCE(ret);
-
-	return ret;
-}
-
-int solo_p2m_dma_sg(struct solo_dev *solo_dev, u8 id,
-		    struct p2m_desc *pdesc, int wr,
-		    struct scatterlist *sg, u32 sg_off,
-		    u32 ext_addr, u32 size)
-{
-	int i;
-	int idx;
-
-	BUG_ON(id >= SOLO_NR_P2M);
-
-	if (WARN_ON_ONCE(!size))
-		return -EINVAL;
-
-	memset(pdesc, 0, sizeof(*pdesc));
-
-	/* Should rewrite this to handle > SOLO_NR_P2M_DESC transactions */
-	for (i = 0, idx = 1; idx < SOLO_NR_P2M_DESC && sg && size > 0;
-	     i++, sg = sg_next(sg)) {
-		struct p2m_desc *desc = &pdesc[idx];
-		u32 sg_len = sg_dma_len(sg);
-		u32 len;
-
-		if (sg_off >= sg_len) {
-			sg_off -= sg_len;
-			continue;
-		}
-
-		sg_len -= sg_off;
-		len = min(sg_len, size);
-
-		solo_p2m_push_desc(desc, wr, sg_dma_address(sg) + sg_off,
-				   ext_addr, len, 0, 0);
-
-		size -= len;
-		ext_addr += len;
-		idx++;
-
-		sg_off = 0;
-	}
-
-	WARN_ON_ONCE(size || i >= SOLO_NR_P2M_DESC);
-
-	return solo_p2m_dma_desc(solo_dev, id, pdesc, idx);
-}
-
-#ifdef SOLO_TEST_P2M
-
-#define P2M_TEST_CHAR		0xbe
-
-static unsigned long long p2m_test(struct solo_dev *solo_dev, u8 id,
-				   u32 base, int size)
-{
-	u8 *wr_buf;
-	u8 *rd_buf;
-	int i;
-	unsigned long long err_cnt = 0;
-
-	wr_buf = kmalloc(size, GFP_KERNEL);
-	if (!wr_buf) {
-		printk(SOLO6X10_NAME ": Failed to malloc for p2m_test\n");
-		return size;
-	}
-
-	rd_buf = kmalloc(size, GFP_KERNEL);
-	if (!rd_buf) {
-		printk(SOLO6X10_NAME ": Failed to malloc for p2m_test\n");
-		kfree(wr_buf);
-		return size;
-	}
-
-	memset(wr_buf, P2M_TEST_CHAR, size);
-	memset(rd_buf, P2M_TEST_CHAR + 1, size);
-
-	solo_p2m_dma(solo_dev, id, 1, wr_buf, base, size);
-	solo_p2m_dma(solo_dev, id, 0, rd_buf, base, size);
-
-	for (i = 0; i < size; i++)
-		if (wr_buf[i] != rd_buf[i])
-			err_cnt++;
-
-	kfree(wr_buf);
-	kfree(rd_buf);
-
-	return err_cnt;
-}
-
-#define TEST_CHUNK_SIZE		(8 * 1024)
-
-static void run_p2m_test(struct solo_dev *solo_dev)
-{
-	unsigned long long errs = 0;
-	u32 size = SOLO_JPEG_EXT_ADDR(solo_dev) + SOLO_JPEG_EXT_SIZE(solo_dev);
-	int i, d;
-
-	dev_warn(&solo_dev->pdev->dev, "Testing %u bytes of external ram\n",
-		 size);
-
-	for (i = 0; i < size; i += TEST_CHUNK_SIZE)
-		for (d = 0; d < 4; d++)
-			errs += p2m_test(solo_dev, d, i, TEST_CHUNK_SIZE);
-
-	dev_warn(&solo_dev->pdev->dev, "Found %llu errors during p2m test\n",
-		 errs);
-
-	return;
-}
-#else
-#define run_p2m_test(__solo)	do {} while (0)
-#endif
-
-void solo_p2m_isr(struct solo_dev *solo_dev, int id)
-{
-	struct solo_p2m_dev *p2m_dev = &solo_dev->p2m_dev[id];
-
-	solo_reg_write(solo_dev, SOLO_IRQ_STAT, SOLO_IRQ_P2M(id));
-
-	complete(&p2m_dev->completion);
-}
-
-void solo_p2m_error_isr(struct solo_dev *solo_dev, u32 status)
-{
-	struct solo_p2m_dev *p2m_dev;
-	int i;
-
-	if (!(status & SOLO_PCI_ERR_P2M))
-		return;
-
-	for (i = 0; i < SOLO_NR_P2M; i++) {
-		p2m_dev = &solo_dev->p2m_dev[i];
-		p2m_dev->error = 1;
-		solo_reg_write(solo_dev, SOLO_P2M_CONTROL(i), 0);
-		complete(&p2m_dev->completion);
-	}
-}
-
-void solo_p2m_exit(struct solo_dev *solo_dev)
-{
-	int i;
-
-	for (i = 0; i < SOLO_NR_P2M; i++)
-		solo_irq_off(solo_dev, SOLO_IRQ_P2M(i));
-}
-
-int solo_p2m_init(struct solo_dev *solo_dev)
-{
-	struct solo_p2m_dev *p2m_dev;
-	int i;
-
-	for (i = 0; i < SOLO_NR_P2M; i++) {
-		p2m_dev = &solo_dev->p2m_dev[i];
-
-		mutex_init(&p2m_dev->mutex);
-		init_completion(&p2m_dev->completion);
-
-		solo_reg_write(solo_dev, SOLO_P2M_CONTROL(i), 0);
-		solo_reg_write(solo_dev, SOLO_P2M_CONFIG(i),
-			       SOLO_P2M_CSC_16BIT_565 |
-			       SOLO_P2M_DMA_INTERVAL(3) |
-			       SOLO_P2M_DESC_INTR_OPT |
-			       SOLO_P2M_PCI_MASTER_MODE);
-		solo_irq_on(solo_dev, SOLO_IRQ_P2M(i));
-	}
-
-	run_p2m_test(solo_dev);
-
-	return 0;
-}
diff --git a/drivers/staging/media/solo6x10/solo6x10-core.c b/drivers/staging/media/solo6x10/solo6x10-core.c
new file mode 100644
index 000000000000..36750205d23f
--- /dev/null
+++ b/drivers/staging/media/solo6x10/solo6x10-core.c
@@ -0,0 +1,709 @@
+/*
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
+ *
+ * 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/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/interrupt.h>
+#include <linux/videodev2.h>
+#include <linux/delay.h>
+#include <linux/sysfs.h>
+#include <linux/ktime.h>
+#include <linux/slab.h>
+
+#include "solo6x10.h"
+#include "solo6x10-tw28.h"
+
+MODULE_DESCRIPTION("Softlogic 6x10 MPEG4/H.264/G.723 CODEC V4L2/ALSA Driver");
+MODULE_AUTHOR("Bluecherry <maintainers@bluecherrydvr.com>");
+MODULE_VERSION(SOLO6X10_VERSION);
+MODULE_LICENSE("GPL");
+
+unsigned video_nr = -1;
+module_param(video_nr, uint, 0644);
+MODULE_PARM_DESC(video_nr, "videoX start number, -1 is autodetect (default)");
+
+static int full_eeprom; /* default is only top 64B */
+module_param(full_eeprom, uint, 0644);
+MODULE_PARM_DESC(full_eeprom, "Allow access to full 128B EEPROM (dangerous)");
+
+
+static void solo_set_time(struct solo_dev *solo_dev)
+{
+	struct timespec ts;
+
+	ktime_get_ts(&ts);
+
+	solo_reg_write(solo_dev, SOLO_TIMER_SEC, ts.tv_sec);
+	solo_reg_write(solo_dev, SOLO_TIMER_USEC, ts.tv_nsec / NSEC_PER_USEC);
+}
+
+static void solo_timer_sync(struct solo_dev *solo_dev)
+{
+	u32 sec, usec;
+	struct timespec ts;
+	long diff;
+
+	if (solo_dev->type != SOLO_DEV_6110)
+		return;
+
+	if (++solo_dev->time_sync < 60)
+		return;
+
+	solo_dev->time_sync = 0;
+
+	sec = solo_reg_read(solo_dev, SOLO_TIMER_SEC);
+	usec = solo_reg_read(solo_dev, SOLO_TIMER_USEC);
+
+	ktime_get_ts(&ts);
+
+	diff = (long)ts.tv_sec - (long)sec;
+	diff = (diff * 1000000)
+		+ ((long)(ts.tv_nsec / NSEC_PER_USEC) - (long)usec);
+
+	if (diff > 1000 || diff < -1000) {
+		solo_set_time(solo_dev);
+	} else if (diff) {
+		long usec_lsb = solo_dev->usec_lsb;
+
+		usec_lsb -= diff / 4;
+		if (usec_lsb < 0)
+			usec_lsb = 0;
+		else if (usec_lsb > 255)
+			usec_lsb = 255;
+
+		solo_dev->usec_lsb = usec_lsb;
+		solo_reg_write(solo_dev, SOLO_TIMER_USEC_LSB,
+			       solo_dev->usec_lsb);
+	}
+}
+
+static irqreturn_t solo_isr(int irq, void *data)
+{
+	struct solo_dev *solo_dev = data;
+	u32 status;
+	int i;
+
+	status = solo_reg_read(solo_dev, SOLO_IRQ_STAT);
+	if (!status)
+		return IRQ_NONE;
+
+	if (status & ~solo_dev->irq_mask) {
+		solo_reg_write(solo_dev, SOLO_IRQ_STAT,
+			       status & ~solo_dev->irq_mask);
+		status &= solo_dev->irq_mask;
+	}
+
+	if (status & SOLO_IRQ_PCI_ERR)
+		solo_p2m_error_isr(solo_dev);
+
+	for (i = 0; i < SOLO_NR_P2M; i++)
+		if (status & SOLO_IRQ_P2M(i))
+			solo_p2m_isr(solo_dev, i);
+
+	if (status & SOLO_IRQ_IIC)
+		solo_i2c_isr(solo_dev);
+
+	if (status & SOLO_IRQ_VIDEO_IN) {
+		solo_video_in_isr(solo_dev);
+		solo_timer_sync(solo_dev);
+	}
+
+	if (status & SOLO_IRQ_ENCODER)
+		solo_enc_v4l2_isr(solo_dev);
+
+	if (status & SOLO_IRQ_G723)
+		solo_g723_isr(solo_dev);
+
+	/* Clear all interrupts handled */
+	solo_reg_write(solo_dev, SOLO_IRQ_STAT, status);
+
+	return IRQ_HANDLED;
+}
+
+static void free_solo_dev(struct solo_dev *solo_dev)
+{
+	struct pci_dev *pdev;
+
+	if (!solo_dev)
+		return;
+
+	if (solo_dev->dev.parent)
+		device_unregister(&solo_dev->dev);
+
+	pdev = solo_dev->pdev;
+
+	/* If we never initialized the PCI device, then nothing else
+	 * below here needs cleanup */
+	if (!pdev) {
+		kfree(solo_dev);
+		return;
+	}
+
+	if (solo_dev->reg_base) {
+		/* Bring down the sub-devices first */
+		solo_g723_exit(solo_dev);
+		solo_enc_v4l2_exit(solo_dev);
+		solo_enc_exit(solo_dev);
+		solo_v4l2_exit(solo_dev);
+		solo_disp_exit(solo_dev);
+		solo_gpio_exit(solo_dev);
+		solo_p2m_exit(solo_dev);
+		solo_i2c_exit(solo_dev);
+
+		/* Now cleanup the PCI device */
+		solo_irq_off(solo_dev, ~0);
+		pci_iounmap(pdev, solo_dev->reg_base);
+		if (pdev->irq)
+			free_irq(pdev->irq, solo_dev);
+	}
+
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	v4l2_device_unregister(&solo_dev->v4l2_dev);
+	pci_set_drvdata(pdev, NULL);
+
+	kfree(solo_dev);
+}
+
+static ssize_t eeprom_store(struct device *dev, struct device_attribute *attr,
+			    const char *buf, size_t count)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+	unsigned short *p = (unsigned short *)buf;
+	int i;
+
+	if (count & 0x1)
+		dev_warn(dev, "EEPROM Write not aligned (truncating)\n");
+
+	if (!full_eeprom && count > 64) {
+		dev_warn(dev, "EEPROM Write truncated to 64 bytes\n");
+		count = 64;
+	} else if (full_eeprom && count > 128) {
+		dev_warn(dev, "EEPROM Write truncated to 128 bytes\n");
+		count = 128;
+	}
+
+	solo_eeprom_ewen(solo_dev, 1);
+
+	for (i = full_eeprom ? 0 : 32; i < min((int)(full_eeprom ? 64 : 32),
+					       (int)(count / 2)); i++)
+		solo_eeprom_write(solo_dev, i, cpu_to_be16(p[i]));
+
+	solo_eeprom_ewen(solo_dev, 0);
+
+	return count;
+}
+
+static ssize_t eeprom_show(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+	unsigned short *p = (unsigned short *)buf;
+	int count = (full_eeprom ? 128 : 64);
+	int i;
+
+	for (i = (full_eeprom ? 0 : 32); i < (count / 2); i++)
+		p[i] = be16_to_cpu(solo_eeprom_read(solo_dev, i));
+
+	return count;
+}
+
+static ssize_t p2m_timeouts_show(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+
+	return sprintf(buf, "%d\n", solo_dev->p2m_timeouts);
+}
+
+static ssize_t sdram_size_show(struct device *dev,
+			       struct device_attribute *attr,
+			       char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+
+	return sprintf(buf, "%dMegs\n", solo_dev->sdram_size >> 20);
+}
+
+static ssize_t tw28xx_show(struct device *dev,
+			   struct device_attribute *attr,
+			   char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+
+	return sprintf(buf, "tw2815[%d] tw2864[%d] tw2865[%d]\n",
+		       hweight32(solo_dev->tw2815),
+		       hweight32(solo_dev->tw2864),
+		       hweight32(solo_dev->tw2865));
+}
+
+static ssize_t input_map_show(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+	unsigned int val;
+	char *out = buf;
+
+	val = solo_reg_read(solo_dev, SOLO_VI_CH_SWITCH_0);
+	out += sprintf(out, "Channel 0   => Input %d\n", val & 0x1f);
+	out += sprintf(out, "Channel 1   => Input %d\n", (val >> 5) & 0x1f);
+	out += sprintf(out, "Channel 2   => Input %d\n", (val >> 10) & 0x1f);
+	out += sprintf(out, "Channel 3   => Input %d\n", (val >> 15) & 0x1f);
+	out += sprintf(out, "Channel 4   => Input %d\n", (val >> 20) & 0x1f);
+	out += sprintf(out, "Channel 5   => Input %d\n", (val >> 25) & 0x1f);
+
+	val = solo_reg_read(solo_dev, SOLO_VI_CH_SWITCH_1);
+	out += sprintf(out, "Channel 6   => Input %d\n", val & 0x1f);
+	out += sprintf(out, "Channel 7   => Input %d\n", (val >> 5) & 0x1f);
+	out += sprintf(out, "Channel 8   => Input %d\n", (val >> 10) & 0x1f);
+	out += sprintf(out, "Channel 9   => Input %d\n", (val >> 15) & 0x1f);
+	out += sprintf(out, "Channel 10  => Input %d\n", (val >> 20) & 0x1f);
+	out += sprintf(out, "Channel 11  => Input %d\n", (val >> 25) & 0x1f);
+
+	val = solo_reg_read(solo_dev, SOLO_VI_CH_SWITCH_2);
+	out += sprintf(out, "Channel 12  => Input %d\n", val & 0x1f);
+	out += sprintf(out, "Channel 13  => Input %d\n", (val >> 5) & 0x1f);
+	out += sprintf(out, "Channel 14  => Input %d\n", (val >> 10) & 0x1f);
+	out += sprintf(out, "Channel 15  => Input %d\n", (val >> 15) & 0x1f);
+	out += sprintf(out, "Spot Output => Input %d\n", (val >> 20) & 0x1f);
+
+	return out - buf;
+}
+
+static ssize_t p2m_timeout_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+	unsigned long ms;
+
+	int ret = kstrtoul(buf, 10, &ms);
+	if (ret < 0 || ms > 200)
+		return -EINVAL;
+	solo_dev->p2m_jiffies = msecs_to_jiffies(ms);
+
+	return count;
+}
+
+static ssize_t p2m_timeout_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+
+	return sprintf(buf, "%ums\n", jiffies_to_msecs(solo_dev->p2m_jiffies));
+}
+
+static ssize_t intervals_show(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+	char *out = buf;
+	int fps = solo_dev->fps;
+	int i;
+
+	for (i = 0; i < solo_dev->nr_chans; i++) {
+		out += sprintf(out, "Channel %d: %d/%d (0x%08x)\n",
+			       i, solo_dev->v4l2_enc[i]->interval, fps,
+			       solo_reg_read(solo_dev, SOLO_CAP_CH_INTV(i)));
+	}
+
+	return out - buf;
+}
+
+static ssize_t sdram_offsets_show(struct device *dev,
+				  struct device_attribute *attr,
+				  char *buf)
+{
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+	char *out = buf;
+
+	out += sprintf(out, "DISP: 0x%08x @ 0x%08x\n",
+		       SOLO_DISP_EXT_ADDR,
+		       SOLO_DISP_EXT_SIZE);
+
+	out += sprintf(out, "EOSD: 0x%08x @ 0x%08x (0x%08x * %d)\n",
+		       SOLO_EOSD_EXT_ADDR,
+		       SOLO_EOSD_EXT_AREA(solo_dev),
+		       SOLO_EOSD_EXT_SIZE(solo_dev),
+		       SOLO_EOSD_EXT_AREA(solo_dev) /
+		       SOLO_EOSD_EXT_SIZE(solo_dev));
+
+	out += sprintf(out, "MOTI: 0x%08x @ 0x%08x\n",
+		       SOLO_MOTION_EXT_ADDR(solo_dev),
+		       SOLO_MOTION_EXT_SIZE);
+
+	out += sprintf(out, "G723: 0x%08x @ 0x%08x\n",
+		       SOLO_G723_EXT_ADDR(solo_dev),
+		       SOLO_G723_EXT_SIZE);
+
+	out += sprintf(out, "CAPT: 0x%08x @ 0x%08x (0x%08x * %d)\n",
+		       SOLO_CAP_EXT_ADDR(solo_dev),
+		       SOLO_CAP_EXT_SIZE(solo_dev),
+		       SOLO_CAP_PAGE_SIZE,
+		       SOLO_CAP_EXT_SIZE(solo_dev) / SOLO_CAP_PAGE_SIZE);
+
+	out += sprintf(out, "EREF: 0x%08x @ 0x%08x (0x%08x * %d)\n",
+		       SOLO_EREF_EXT_ADDR(solo_dev),
+		       SOLO_EREF_EXT_AREA(solo_dev),
+		       SOLO_EREF_EXT_SIZE,
+		       SOLO_EREF_EXT_AREA(solo_dev) / SOLO_EREF_EXT_SIZE);
+
+	out += sprintf(out, "MPEG: 0x%08x @ 0x%08x\n",
+		       SOLO_MP4E_EXT_ADDR(solo_dev),
+		       SOLO_MP4E_EXT_SIZE(solo_dev));
+
+	out += sprintf(out, "JPEG: 0x%08x @ 0x%08x\n",
+		       SOLO_JPEG_EXT_ADDR(solo_dev),
+		       SOLO_JPEG_EXT_SIZE(solo_dev));
+
+	return out - buf;
+}
+
+static ssize_t sdram_show(struct file *file, struct kobject *kobj,
+			  struct bin_attribute *a, char *buf,
+			  loff_t off, size_t count)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct solo_dev *solo_dev =
+		container_of(dev, struct solo_dev, dev);
+	const int size = solo_dev->sdram_size;
+
+	if (off >= size)
+		return 0;
+
+	if (off + count > size)
+		count = size - off;
+
+	if (solo_p2m_dma(solo_dev, 0, buf, off, count, 0, 0))
+		return -EIO;
+
+	return count;
+}
+
+static const struct device_attribute solo_dev_attrs[] = {
+	__ATTR(eeprom, 0640, eeprom_show, eeprom_store),
+	__ATTR(p2m_timeout, 0644, p2m_timeout_show, p2m_timeout_store),
+	__ATTR_RO(p2m_timeouts),
+	__ATTR_RO(sdram_size),
+	__ATTR_RO(tw28xx),
+	__ATTR_RO(input_map),
+	__ATTR_RO(intervals),
+	__ATTR_RO(sdram_offsets),
+};
+
+static void solo_device_release(struct device *dev)
+{
+	/* Do nothing */
+}
+
+static int solo_sysfs_init(struct solo_dev *solo_dev)
+{
+	struct bin_attribute *sdram_attr = &solo_dev->sdram_attr;
+	struct device *dev = &solo_dev->dev;
+	const char *driver;
+	int i;
+
+	if (solo_dev->type == SOLO_DEV_6110)
+		driver = "solo6110";
+	else
+		driver = "solo6010";
+
+	dev->release = solo_device_release;
+	dev->parent = &solo_dev->pdev->dev;
+	set_dev_node(dev, dev_to_node(&solo_dev->pdev->dev));
+	dev_set_name(dev, "%s-%d-%d", driver, solo_dev->vfd->num,
+		     solo_dev->nr_chans);
+
+	if (device_register(dev)) {
+		dev->parent = NULL;
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(solo_dev_attrs); i++) {
+		if (device_create_file(dev, &solo_dev_attrs[i])) {
+			device_unregister(dev);
+			return -ENOMEM;
+		}
+	}
+
+	sysfs_attr_init(&sdram_attr->attr);
+	sdram_attr->attr.name = "sdram";
+	sdram_attr->attr.mode = 0440;
+	sdram_attr->read = sdram_show;
+	sdram_attr->size = solo_dev->sdram_size;
+
+	if (device_create_bin_file(dev, sdram_attr)) {
+		device_unregister(dev);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static int solo_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
+{
+	struct solo_dev *solo_dev;
+	int ret;
+	u8 chip_id;
+
+	solo_dev = kzalloc(sizeof(*solo_dev), GFP_KERNEL);
+	if (solo_dev == NULL)
+		return -ENOMEM;
+
+	if (id->driver_data == SOLO_DEV_6010)
+		dev_info(&pdev->dev, "Probing Softlogic 6010\n");
+	else
+		dev_info(&pdev->dev, "Probing Softlogic 6110\n");
+
+	solo_dev->type = id->driver_data;
+	solo_dev->pdev = pdev;
+	spin_lock_init(&solo_dev->reg_io_lock);
+	ret = v4l2_device_register(&pdev->dev, &solo_dev->v4l2_dev);
+	if (ret)
+		goto fail_probe;
+
+	/* Only for during init */
+	solo_dev->p2m_jiffies = msecs_to_jiffies(100);
+
+	ret = pci_enable_device(pdev);
+	if (ret)
+		goto fail_probe;
+
+	pci_set_master(pdev);
+
+	/* RETRY/TRDY Timeout disabled */
+	pci_write_config_byte(pdev, 0x40, 0x00);
+	pci_write_config_byte(pdev, 0x41, 0x00);
+
+	ret = pci_request_regions(pdev, SOLO6X10_NAME);
+	if (ret)
+		goto fail_probe;
+
+	solo_dev->reg_base = pci_ioremap_bar(pdev, 0);
+	if (solo_dev->reg_base == NULL) {
+		ret = -ENOMEM;
+		goto fail_probe;
+	}
+
+	chip_id = solo_reg_read(solo_dev, SOLO_CHIP_OPTION) &
+				SOLO_CHIP_ID_MASK;
+	switch (chip_id) {
+	case 7:
+		solo_dev->nr_chans = 16;
+		solo_dev->nr_ext = 5;
+		break;
+	case 6:
+		solo_dev->nr_chans = 8;
+		solo_dev->nr_ext = 2;
+		break;
+	default:
+		dev_warn(&pdev->dev, "Invalid chip_id 0x%02x, assuming 4 ch\n",
+			 chip_id);
+	case 5:
+		solo_dev->nr_chans = 4;
+		solo_dev->nr_ext = 1;
+	}
+
+	/* Disable all interrupts to start */
+	solo_irq_off(solo_dev, ~0);
+
+	/* Initial global settings */
+	if (solo_dev->type == SOLO_DEV_6010) {
+		solo_dev->clock_mhz = 108;
+		solo_dev->sys_config = SOLO_SYS_CFG_SDRAM64BIT
+			| SOLO_SYS_CFG_INPUTDIV(25)
+			| SOLO_SYS_CFG_FEEDBACKDIV(solo_dev->clock_mhz * 2 - 2)
+			| SOLO_SYS_CFG_OUTDIV(3);
+		solo_reg_write(solo_dev, SOLO_SYS_CFG, solo_dev->sys_config);
+	} else {
+		u32 divq, divf;
+
+		solo_dev->clock_mhz = 135;
+
+		if (solo_dev->clock_mhz < 125) {
+			divq = 3;
+			divf = (solo_dev->clock_mhz * 4) / 3 - 1;
+		} else {
+			divq = 2;
+			divf = (solo_dev->clock_mhz * 2) / 3 - 1;
+		}
+
+		solo_reg_write(solo_dev, SOLO_PLL_CONFIG,
+			       (1 << 20) | /* PLL_RANGE */
+			       (8 << 15) | /* PLL_DIVR  */
+			       (divq << 12) |
+			       (divf <<  4) |
+			       (1 <<  1)   /* PLL_FSEN */);
+
+		solo_dev->sys_config = SOLO_SYS_CFG_SDRAM64BIT;
+	}
+
+	solo_reg_write(solo_dev, SOLO_SYS_CFG, solo_dev->sys_config);
+	solo_reg_write(solo_dev, SOLO_TIMER_CLOCK_NUM,
+		       solo_dev->clock_mhz - 1);
+
+	/* PLL locking time of 1ms */
+	mdelay(1);
+
+	ret = request_irq(pdev->irq, solo_isr, IRQF_SHARED, SOLO6X10_NAME,
+			  solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	/* Handle this from the start */
+	solo_irq_on(solo_dev, SOLO_IRQ_PCI_ERR);
+
+	ret = solo_i2c_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	/* Setup the DMA engine */
+	solo_reg_write(solo_dev, SOLO_DMA_CTRL,
+		       SOLO_DMA_CTRL_REFRESH_CYCLE(1) |
+		       SOLO_DMA_CTRL_SDRAM_SIZE(2) |
+		       SOLO_DMA_CTRL_SDRAM_CLK_INVERT |
+		       SOLO_DMA_CTRL_READ_CLK_SELECT |
+		       SOLO_DMA_CTRL_LATENCY(1));
+
+	/* Undocumented crap */
+	solo_reg_write(solo_dev, SOLO_DMA_CTRL1,
+		       solo_dev->type == SOLO_DEV_6010 ? 0x100 : 0x300);
+
+	if (solo_dev->type != SOLO_DEV_6010) {
+		solo_dev->usec_lsb = 0x3f;
+		solo_set_time(solo_dev);
+	}
+
+	/* Disable watchdog */
+	solo_reg_write(solo_dev, SOLO_WATCHDOG, 0);
+
+	/* Initialize sub components */
+
+	ret = solo_p2m_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_disp_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_gpio_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_tw28_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_v4l2_init(solo_dev, video_nr);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_enc_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_enc_v4l2_init(solo_dev, video_nr);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_g723_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	ret = solo_sysfs_init(solo_dev);
+	if (ret)
+		goto fail_probe;
+
+	/* Now that init is over, set this lower */
+	solo_dev->p2m_jiffies = msecs_to_jiffies(20);
+
+	return 0;
+
+fail_probe:
+	free_solo_dev(solo_dev);
+	return ret;
+}
+
+static void solo_pci_remove(struct pci_dev *pdev)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pdev);
+	struct solo_dev *solo_dev = container_of(v4l2_dev, struct solo_dev, v4l2_dev);
+
+	free_solo_dev(solo_dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(solo_id_table) = {
+	/* 6010 based cards */
+	{ PCI_DEVICE(PCI_VENDOR_ID_SOFTLOGIC, PCI_DEVICE_ID_SOLO6010),
+	  .driver_data = SOLO_DEV_6010 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_NEUSOLO_4),
+	  .driver_data = SOLO_DEV_6010 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_NEUSOLO_9),
+	  .driver_data = SOLO_DEV_6010 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_NEUSOLO_16),
+	  .driver_data = SOLO_DEV_6010 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_SOLO_4),
+	  .driver_data = SOLO_DEV_6010 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_SOLO_9),
+	  .driver_data = SOLO_DEV_6010 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_SOLO_16),
+	  .driver_data = SOLO_DEV_6010 },
+	/* 6110 based cards */
+	{ PCI_DEVICE(PCI_VENDOR_ID_SOFTLOGIC, PCI_DEVICE_ID_SOLO6110),
+	  .driver_data = SOLO_DEV_6110 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_6110_4),
+	  .driver_data = SOLO_DEV_6110 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_6110_8),
+	  .driver_data = SOLO_DEV_6110 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BLUECHERRY, PCI_DEVICE_ID_BC_6110_16),
+	  .driver_data = SOLO_DEV_6110 },
+	{0,}
+};
+
+MODULE_DEVICE_TABLE(pci, solo_id_table);
+
+static struct pci_driver solo_pci_driver = {
+	.name = SOLO6X10_NAME,
+	.id_table = solo_id_table,
+	.probe = solo_pci_probe,
+	.remove = solo_pci_remove,
+};
+
+module_pci_driver(solo_pci_driver);
diff --git a/drivers/staging/media/solo6x10/disp.c b/drivers/staging/media/solo6x10/solo6x10-disp.c
index 884c0eb757c4..32d9953bc36e 100644
--- a/drivers/staging/media/solo6x10/disp.c
+++ b/drivers/staging/media/solo6x10/solo6x10-disp.c
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
  *
  * 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
@@ -21,6 +26,7 @@
 #include <linux/module.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-ioctl.h>
+
 #include "solo6x10.h"
 
 #define SOLO_VCLK_DELAY			3
@@ -30,12 +36,8 @@
 #define SOLO_MOT_THRESH_H		64
 #define SOLO_MOT_THRESH_SIZE		8192
 #define SOLO_MOT_THRESH_REAL		(SOLO_MOT_THRESH_W * SOLO_MOT_THRESH_H)
-#define SOLO_MOT_FLAG_SIZE		512
-#define SOLO_MOT_FLAG_AREA		(SOLO_MOT_FLAG_SIZE * 32)
-
-static unsigned video_type;
-module_param(video_type, uint, 0644);
-MODULE_PARM_DESC(video_type, "video_type (0 = NTSC/Default, 1 = PAL)");
+#define SOLO_MOT_FLAG_SIZE		1024
+#define SOLO_MOT_FLAG_AREA		(SOLO_MOT_FLAG_SIZE * 16)
 
 static void solo_vin_config(struct solo_dev *solo_dev)
 {
@@ -73,7 +75,12 @@ static void solo_vin_config(struct solo_dev *solo_dev)
 	solo_reg_write(solo_dev, SOLO_VI_CH_FORMAT,
 		       SOLO_VI_FD_SEL_MASK(0) | SOLO_VI_PROG_MASK(0));
 
-	solo_reg_write(solo_dev, SOLO_VI_FMT_CFG, 0);
+	/* On 6110, initialize mozaic darkness stength */
+	if (solo_dev->type == SOLO_DEV_6010)
+		solo_reg_write(solo_dev, SOLO_VI_FMT_CFG, 0);
+	else
+		solo_reg_write(solo_dev, SOLO_VI_FMT_CFG, 16 << 22);
+
 	solo_reg_write(solo_dev, SOLO_VI_PAGE_SW, 2);
 
 	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC) {
@@ -97,21 +104,30 @@ static void solo_vin_config(struct solo_dev *solo_dev)
 		       SOLO_VI_PB_HSTOP(16 + 720));
 }
 
-static void solo_disp_config(struct solo_dev *solo_dev)
+static void solo_vout_config_cursor(struct solo_dev *dev)
+{
+	int i;
+
+	/* Load (blank) cursor bitmap mask (2bpp) */
+	for (i = 0; i < 20; i++)
+		solo_reg_write(dev, SOLO_VO_CURSOR_MASK(i), 0);
+
+	solo_reg_write(dev, SOLO_VO_CURSOR_POS, 0);
+
+	solo_reg_write(dev, SOLO_VO_CURSOR_CLR,
+		       (0x80 << 24) | (0x80 << 16) | (0x10 << 8) | 0x80);
+	solo_reg_write(dev, SOLO_VO_CURSOR_CLR2, (0xe0 << 8) | 0x80);
+}
+
+static void solo_vout_config(struct solo_dev *solo_dev)
 {
 	solo_dev->vout_hstart = 6;
 	solo_dev->vout_vstart = 8;
 
-	solo_reg_write(solo_dev, SOLO_VO_BORDER_LINE_COLOR,
-		       (0xa0 << 24) | (0x88 << 16) | (0xa0 << 8) | 0x88);
-	solo_reg_write(solo_dev, SOLO_VO_BORDER_FILL_COLOR,
-		       (0x10 << 24) | (0x8f << 16) | (0x10 << 8) | 0x8f);
-	solo_reg_write(solo_dev, SOLO_VO_BKG_COLOR,
-		       (16 << 24) | (128 << 16) | (16 << 8) | 128);
-
 	solo_reg_write(solo_dev, SOLO_VO_FMT_ENC,
 		       solo_dev->video_type |
 		       SOLO_VO_USER_COLOR_SET_NAV |
+		       SOLO_VO_USER_COLOR_SET_NAH |
 		       SOLO_VO_NA_COLOR_Y(0) |
 		       SOLO_VO_NA_COLOR_CB(0) |
 		       SOLO_VO_NA_COLOR_CR(0));
@@ -130,19 +146,31 @@ static void solo_disp_config(struct solo_dev *solo_dev)
 		       SOLO_VO_H_LEN(solo_dev->video_hsize) |
 		       SOLO_VO_V_LEN(solo_dev->video_vsize));
 
-	solo_reg_write(solo_dev, SOLO_VI_WIN_SW, 5);
+	/* Border & background colors */
+	solo_reg_write(solo_dev, SOLO_VO_BORDER_LINE_COLOR,
+		       (0xa0 << 24) | (0x88 << 16) | (0xa0 << 8) | 0x88);
+	solo_reg_write(solo_dev, SOLO_VO_BORDER_FILL_COLOR,
+		       (0x10 << 24) | (0x8f << 16) | (0x10 << 8) | 0x8f);
+	solo_reg_write(solo_dev, SOLO_VO_BKG_COLOR,
+		       (16 << 24) | (128 << 16) | (16 << 8) | 128);
+
+	solo_reg_write(solo_dev, SOLO_VO_DISP_ERASE, SOLO_VO_DISP_ERASE_ON);
+
+	solo_reg_write(solo_dev, SOLO_VI_WIN_SW, 0);
+
+	solo_reg_write(solo_dev, SOLO_VO_ZOOM_CTRL, 0);
+	solo_reg_write(solo_dev, SOLO_VO_FREEZE_CTRL, 0);
 
 	solo_reg_write(solo_dev, SOLO_VO_DISP_CTRL, SOLO_VO_DISP_ON |
 		       SOLO_VO_DISP_ERASE_COUNT(8) |
 		       SOLO_VO_DISP_BASE(SOLO_DISP_EXT_ADDR));
 
-	solo_reg_write(solo_dev, SOLO_VO_DISP_ERASE, SOLO_VO_DISP_ERASE_ON);
 
-	/* Enable channels we support */
-	solo_reg_write(solo_dev, SOLO_VI_CH_ENA, (1 << solo_dev->nr_chans) - 1);
+	solo_vout_config_cursor(solo_dev);
 
-	/* Disable the watchdog */
-	solo_reg_write(solo_dev, SOLO_WATCHDOG, 0);
+	/* Enable channels we support */
+	solo_reg_write(solo_dev, SOLO_VI_CH_ENA,
+		       (1 << solo_dev->nr_chans) - 1);
 }
 
 static int solo_dma_vin_region(struct solo_dev *solo_dev, u32 off,
@@ -153,29 +181,48 @@ static int solo_dma_vin_region(struct solo_dev *solo_dev, u32 off,
 	int ret = 0;
 
 	for (i = 0; i < sizeof(buf) >> 1; i++)
-		buf[i] = val;
+		buf[i] = cpu_to_le16(val);
 
 	for (i = 0; i < reg_size; i += sizeof(buf))
-		ret |= solo_p2m_dma(solo_dev, SOLO_P2M_DMA_ID_VIN, 1, buf,
+		ret |= solo_p2m_dma(solo_dev, 1, buf,
 				    SOLO_MOTION_EXT_ADDR(solo_dev) + off + i,
-				    sizeof(buf));
+				    sizeof(buf), 0, 0);
 
 	return ret;
 }
 
-void solo_set_motion_threshold(struct solo_dev *solo_dev, u8 ch, u16 val)
+int solo_set_motion_threshold(struct solo_dev *solo_dev, u8 ch, u16 val)
 {
 	if (ch > solo_dev->nr_chans)
-		return;
+		return -EINVAL;
 
-	solo_dma_vin_region(solo_dev, SOLO_MOT_FLAG_AREA +
-			    (ch * SOLO_MOT_THRESH_SIZE * 2),
-			    val, SOLO_MOT_THRESH_REAL);
+	return solo_dma_vin_region(solo_dev, SOLO_MOT_FLAG_AREA +
+				   (ch * SOLO_MOT_THRESH_SIZE * 2),
+				   val, SOLO_MOT_THRESH_SIZE);
+}
+
+int solo_set_motion_block(struct solo_dev *solo_dev, u8 ch,
+		const struct solo_motion_thresholds *thresholds)
+{
+	u32 off = SOLO_MOT_FLAG_AREA + ch * SOLO_MOT_THRESH_SIZE * 2;
+	u16 buf[64];
+	int x, y;
+	int ret = 0;
+
+	memset(buf, 0, sizeof(buf));
+	for (y = 0; y < SOLO_MOTION_SZ; y++) {
+		for (x = 0; x < SOLO_MOTION_SZ; x++)
+			buf[x] = cpu_to_le16(thresholds->thresholds[y][x]);
+		ret |= solo_p2m_dma(solo_dev, 1, buf,
+			SOLO_MOTION_EXT_ADDR(solo_dev) + off + y * sizeof(buf),
+			sizeof(buf), 0, 0);
+	}
+	return ret;
 }
 
 /* First 8k is motion flag (512 bytes * 16). Following that is an 8k+8k
  * threshold and working table for each channel. Atleast that's what the
- * spec says. However, this code (take from rdk) has some mystery 8k
+ * spec says. However, this code (taken from rdk) has some mystery 8k
  * block right after the flag area, before the first thresh table. */
 static void solo_motion_config(struct solo_dev *solo_dev)
 {
@@ -188,9 +235,9 @@ static void solo_motion_config(struct solo_dev *solo_dev)
 
 		/* Clear working cache table */
 		solo_dma_vin_region(solo_dev, SOLO_MOT_FLAG_AREA +
-				    SOLO_MOT_THRESH_SIZE +
-				    (i * SOLO_MOT_THRESH_SIZE * 2),
-				    0x0000, SOLO_MOT_THRESH_REAL);
+				    (i * SOLO_MOT_THRESH_SIZE * 2) +
+				    SOLO_MOT_THRESH_SIZE, 0x0000,
+				    SOLO_MOT_THRESH_SIZE);
 
 		/* Set default threshold table */
 		solo_set_motion_threshold(solo_dev, i, SOLO_DEF_MOT_THRESH);
@@ -202,8 +249,8 @@ static void solo_motion_config(struct solo_dev *solo_dev)
 	solo_reg_write(solo_dev, SOLO_VI_MOT_CTRL,
 		       SOLO_VI_MOTION_FRAME_COUNT(3) |
 		       SOLO_VI_MOTION_SAMPLE_LENGTH(solo_dev->video_hsize / 16)
-		       | /* SOLO_VI_MOTION_INTR_START_STOP | */
-		       SOLO_VI_MOTION_SAMPLE_COUNT(10));
+		       /* | SOLO_VI_MOTION_INTR_START_STOP */
+		       | SOLO_VI_MOTION_SAMPLE_COUNT(10));
 
 	solo_reg_write(solo_dev, SOLO_VI_MOTION_BORDER, 0);
 	solo_reg_write(solo_dev, SOLO_VI_MOTION_BAR, 0);
@@ -214,19 +261,17 @@ int solo_disp_init(struct solo_dev *solo_dev)
 	int i;
 
 	solo_dev->video_hsize = 704;
-	if (video_type == 0) {
-		solo_dev->video_type = SOLO_VO_FMT_TYPE_NTSC;
+	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC) {
 		solo_dev->video_vsize = 240;
 		solo_dev->fps = 30;
 	} else {
-		solo_dev->video_type = SOLO_VO_FMT_TYPE_PAL;
 		solo_dev->video_vsize = 288;
 		solo_dev->fps = 25;
 	}
 
 	solo_vin_config(solo_dev);
 	solo_motion_config(solo_dev);
-	solo_disp_config(solo_dev);
+	solo_vout_config(solo_dev);
 
 	for (i = 0; i < solo_dev->nr_chans; i++)
 		solo_reg_write(solo_dev, SOLO_VI_WIN_ON(i), 1);
@@ -238,8 +283,6 @@ void solo_disp_exit(struct solo_dev *solo_dev)
 {
 	int i;
 
-	solo_irq_off(solo_dev, SOLO_IRQ_MOTION);
-
 	solo_reg_write(solo_dev, SOLO_VO_DISP_CTRL, 0);
 	solo_reg_write(solo_dev, SOLO_VO_ZOOM_CTRL, 0);
 	solo_reg_write(solo_dev, SOLO_VO_FREEZE_CTRL, 0);
diff --git a/drivers/staging/media/solo6x10/solo6x10-eeprom.c b/drivers/staging/media/solo6x10/solo6x10-eeprom.c
new file mode 100644
index 000000000000..9d1c9bb53d6b
--- /dev/null
+++ b/drivers/staging/media/solo6x10/solo6x10-eeprom.c
@@ -0,0 +1,154 @@
+/*
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
+ *
+ * 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/kernel.h>
+#include <linux/delay.h>
+
+#include "solo6x10.h"
+
+/* Control */
+#define EE_SHIFT_CLK	0x04
+#define EE_CS		0x08
+#define EE_DATA_WRITE	0x02
+#define EE_DATA_READ	0x01
+#define EE_ENB		(0x80 | EE_CS)
+
+#define eeprom_delay()	udelay(100)
+#if 0
+#define eeprom_delay()	solo_reg_read(solo_dev, SOLO_EEPROM_CTRL)
+#define eeprom_delay()	({				\
+	int i, ret;					\
+	udelay(100);					\
+	for (i = ret = 0; i < 1000 && !ret; i++)	\
+		ret = solo_eeprom_reg_read(solo_dev);	\
+})
+#endif
+#define ADDR_LEN	6
+
+/* Commands */
+#define EE_EWEN_CMD	4
+#define EE_EWDS_CMD	4
+#define EE_WRITE_CMD	5
+#define EE_READ_CMD	6
+#define EE_ERASE_CMD	7
+
+static unsigned int solo_eeprom_reg_read(struct solo_dev *solo_dev)
+{
+	return solo_reg_read(solo_dev, SOLO_EEPROM_CTRL) & EE_DATA_READ;
+}
+
+static void solo_eeprom_reg_write(struct solo_dev *solo_dev, u32 data)
+{
+	solo_reg_write(solo_dev, SOLO_EEPROM_CTRL, data);
+	eeprom_delay();
+}
+
+static void solo_eeprom_cmd(struct solo_dev *solo_dev, int cmd)
+{
+	int i;
+
+	solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ACCESS_EN);
+	solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE);
+
+	for (i = 4 + ADDR_LEN; i >= 0; i--) {
+		int dataval = (cmd & (1 << i)) ? EE_DATA_WRITE : 0;
+
+		solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE | dataval);
+		solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE |
+				      EE_SHIFT_CLK | dataval);
+	}
+
+	solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE);
+}
+
+unsigned int solo_eeprom_ewen(struct solo_dev *solo_dev, int w_en)
+{
+	int ewen_cmd = (w_en ? 0x3f : 0) | (EE_EWEN_CMD << ADDR_LEN);
+	unsigned int retval = 0;
+	int i;
+
+	solo_eeprom_cmd(solo_dev, ewen_cmd);
+
+	for (i = 0; i < 16; i++) {
+		solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE |
+				      EE_SHIFT_CLK);
+		retval = (retval << 1) | solo_eeprom_reg_read(solo_dev);
+		solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE);
+		retval = (retval << 1) | solo_eeprom_reg_read(solo_dev);
+	}
+
+	solo_eeprom_reg_write(solo_dev, ~EE_CS);
+	retval = (retval << 1) | solo_eeprom_reg_read(solo_dev);
+
+	return retval;
+}
+
+unsigned short solo_eeprom_read(struct solo_dev *solo_dev, int loc)
+{
+	int read_cmd = loc | (EE_READ_CMD << ADDR_LEN);
+	unsigned short retval = 0;
+	int i;
+
+	solo_eeprom_cmd(solo_dev, read_cmd);
+
+	for (i = 0; i < 16; i++) {
+		solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE |
+				      EE_SHIFT_CLK);
+		retval = (retval << 1) | solo_eeprom_reg_read(solo_dev);
+		solo_eeprom_reg_write(solo_dev, SOLO_EEPROM_ENABLE);
+	}
+
+	solo_eeprom_reg_write(solo_dev, ~EE_CS);
+
+	return retval;
+}
+
+int solo_eeprom_write(struct solo_dev *solo_dev, int loc,
+		      unsigned short data)
+{
+	int write_cmd = loc | (EE_WRITE_CMD << ADDR_LEN);
+	unsigned int retval;
+	int i;
+
+	solo_eeprom_cmd(solo_dev, write_cmd);
+
+	for (i = 15; i >= 0; i--) {
+		unsigned int dataval = (data >> i) & 1;
+
+		solo_eeprom_reg_write(solo_dev, EE_ENB);
+		solo_eeprom_reg_write(solo_dev,
+				      EE_ENB | (dataval << 1) | EE_SHIFT_CLK);
+	}
+
+	solo_eeprom_reg_write(solo_dev, EE_ENB);
+	solo_eeprom_reg_write(solo_dev, ~EE_CS);
+	solo_eeprom_reg_write(solo_dev, EE_ENB);
+
+	for (i = retval = 0; i < 10000 && !retval; i++)
+		retval = solo_eeprom_reg_read(solo_dev);
+
+	solo_eeprom_reg_write(solo_dev, ~EE_CS);
+
+	return !retval;
+}
diff --git a/drivers/staging/media/solo6x10/enc.c b/drivers/staging/media/solo6x10/solo6x10-enc.c
index de502599bb19..94d5735abf85 100644
--- a/drivers/staging/media/solo6x10/enc.c
+++ b/drivers/staging/media/solo6x10/solo6x10-enc.c
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
  *
  * 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
@@ -18,30 +23,41 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/font.h>
+#include <linux/bitrev.h>
 #include <linux/slab.h>
-#include "solo6x10.h"
-#include "osd-font.h"
 
-#define CAPTURE_MAX_BANDWIDTH		32	/* D1 4channel (D1 == 4) */
-#define OSG_BUFFER_SIZE			1024
+#include "solo6x10.h"
 
 #define VI_PROG_HSIZE			(1280 - 16)
 #define VI_PROG_VSIZE			(1024 - 16)
 
+#define IRQ_LEVEL			2
+
 static void solo_capture_config(struct solo_dev *solo_dev)
 {
-	int i, j;
 	unsigned long height;
 	unsigned long width;
-	unsigned char *buf;
+	void *buf;
+	int i;
 
 	solo_reg_write(solo_dev, SOLO_CAP_BASE,
-		       SOLO_CAP_MAX_PAGE(SOLO_CAP_EXT_MAX_PAGE *
-					 solo_dev->nr_chans) |
-		       SOLO_CAP_BASE_ADDR(SOLO_CAP_EXT_ADDR(solo_dev) >> 16));
-	solo_reg_write(solo_dev, SOLO_CAP_BTW,
-		       (1 << 17) | SOLO_CAP_PROG_BANDWIDTH(2) |
-		       SOLO_CAP_MAX_BANDWIDTH(CAPTURE_MAX_BANDWIDTH));
+		       SOLO_CAP_MAX_PAGE((SOLO_CAP_EXT_SIZE(solo_dev)
+					  - SOLO_CAP_PAGE_SIZE) >> 16)
+		       | SOLO_CAP_BASE_ADDR(SOLO_CAP_EXT_ADDR(solo_dev) >> 16));
+
+	/* XXX: Undocumented bits at b17 and b24 */
+	if (solo_dev->type == SOLO_DEV_6110) {
+		/* NOTE: Ref driver has (62 << 24) here as well, but it causes
+		 * wacked out frame timing on 4-port 6110. */
+		solo_reg_write(solo_dev, SOLO_CAP_BTW,
+			       (1 << 17) | SOLO_CAP_PROG_BANDWIDTH(2) |
+			       SOLO_CAP_MAX_BANDWIDTH(36));
+	} else {
+		solo_reg_write(solo_dev, SOLO_CAP_BTW,
+			       (1 << 17) | SOLO_CAP_PROG_BANDWIDTH(2) |
+			       SOLO_CAP_MAX_BANDWIDTH(32));
+	}
 
 	/* Set scale 1, 9 dimension */
 	width = solo_dev->video_hsize;
@@ -96,115 +112,212 @@ static void solo_capture_config(struct solo_dev *solo_dev)
 	solo_reg_write(solo_dev, SOLO_VE_OSD_BASE, SOLO_EOSD_EXT_ADDR >> 16);
 	solo_reg_write(solo_dev, SOLO_VE_OSD_CLR,
 		       0xF0 << 16 | 0x80 << 8 | 0x80);
-	solo_reg_write(solo_dev, SOLO_VE_OSD_OPT, 0);
+
+	if (solo_dev->type == SOLO_DEV_6010)
+		solo_reg_write(solo_dev, SOLO_VE_OSD_OPT,
+			       SOLO_VE_OSD_H_SHADOW | SOLO_VE_OSD_V_SHADOW);
+	else
+		solo_reg_write(solo_dev, SOLO_VE_OSD_OPT, SOLO_VE_OSD_V_DOUBLE
+			       | SOLO_VE_OSD_H_SHADOW | SOLO_VE_OSD_V_SHADOW);
 
 	/* Clear OSG buffer */
-	buf = kzalloc(OSG_BUFFER_SIZE, GFP_KERNEL);
+	buf = kzalloc(SOLO_EOSD_EXT_SIZE(solo_dev), GFP_KERNEL);
 	if (!buf)
 		return;
 
 	for (i = 0; i < solo_dev->nr_chans; i++) {
-		for (j = 0; j < SOLO_EOSD_EXT_SIZE; j += OSG_BUFFER_SIZE) {
-			solo_p2m_dma(solo_dev, SOLO_P2M_DMA_ID_MP4E, 1, buf,
-				     SOLO_EOSD_EXT_ADDR +
-				     (i * SOLO_EOSD_EXT_SIZE) + j,
-				     OSG_BUFFER_SIZE);
-		}
+		solo_p2m_dma(solo_dev, 1, buf,
+			     SOLO_EOSD_EXT_ADDR +
+			     (SOLO_EOSD_EXT_SIZE(solo_dev) * i),
+			     SOLO_EOSD_EXT_SIZE(solo_dev), 0, 0);
 	}
 	kfree(buf);
 }
 
+/* Should be called with enable_lock held */
 int solo_osd_print(struct solo_enc_dev *solo_enc)
 {
 	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	char *str = solo_enc->osd_text;
-	u8 *buf;
+	unsigned char *str = solo_enc->osd_text;
+	u8 *buf = solo_enc->osd_buf;
 	u32 reg = solo_reg_read(solo_dev, SOLO_VE_OSD_CH);
-	int len = strlen(str);
+	const struct font_desc *vga = find_font("VGA8x16");
+	const unsigned char *vga_data;
+	int len;
 	int i, j;
-	int x = 1, y = 1;
+
+	if (WARN_ON_ONCE(!vga))
+		return -ENODEV;
+
+	len = strlen(str);
 
 	if (len == 0) {
+		/* Disable OSD on this channel */
 		reg &= ~(1 << solo_enc->ch);
 		solo_reg_write(solo_dev, SOLO_VE_OSD_CH, reg);
 		return 0;
 	}
 
-	buf = kzalloc(SOLO_EOSD_EXT_SIZE, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
+	memset(buf, 0, SOLO_EOSD_EXT_SIZE_MAX);
+	vga_data = (const unsigned char *)vga->data;
 
 	for (i = 0; i < len; i++) {
+		unsigned char c = str[i];
+
 		for (j = 0; j < 16; j++) {
-			buf[(j*2) + (i%2) + ((x + (i/2)) * 32) + (y * 2048)] =
-				(solo_osd_font[(str[i] * 4) + (j / 4)]
-					>> ((3 - (j % 4)) * 8)) & 0xff;
+			buf[(j * 2) + (i % 2) + (i / 2 * 32)] =
+				bitrev8(vga_data[(c * 16) + j]);
 		}
 	}
 
-	solo_p2m_dma(solo_dev, 0, 1, buf, SOLO_EOSD_EXT_ADDR +
-		     (solo_enc->ch * SOLO_EOSD_EXT_SIZE), SOLO_EOSD_EXT_SIZE);
+	solo_p2m_dma(solo_dev, 1, buf,
+		     SOLO_EOSD_EXT_ADDR +
+		     (solo_enc->ch * SOLO_EOSD_EXT_SIZE(solo_dev)),
+		     SOLO_EOSD_EXT_SIZE(solo_dev), 0, 0);
+
+	/* Enable OSD on this channel */
 	reg |= (1 << solo_enc->ch);
 	solo_reg_write(solo_dev, SOLO_VE_OSD_CH, reg);
 
-	kfree(buf);
-
 	return 0;
 }
 
+/**
+ * Set channel Quality Profile (0-3).
+ */
+void solo_s_jpeg_qp(struct solo_dev *solo_dev, unsigned int ch,
+		    unsigned int qp)
+{
+	unsigned long flags;
+	unsigned int idx, reg;
+
+	if ((ch > 31) || (qp > 3))
+		return;
+
+	if (solo_dev->type == SOLO_DEV_6010)
+		return;
+
+	if (ch < 16) {
+		idx = 0;
+		reg = SOLO_VE_JPEG_QP_CH_L;
+	} else {
+		ch -= 16;
+		idx = 1;
+		reg = SOLO_VE_JPEG_QP_CH_H;
+	}
+	ch *= 2;
+
+	spin_lock_irqsave(&solo_dev->jpeg_qp_lock, flags);
+
+	solo_dev->jpeg_qp[idx] &= ~(3 << ch);
+	solo_dev->jpeg_qp[idx] |= (qp & 3) << ch;
+
+	solo_reg_write(solo_dev, reg, solo_dev->jpeg_qp[idx]);
+
+	spin_unlock_irqrestore(&solo_dev->jpeg_qp_lock, flags);
+}
+
+int solo_g_jpeg_qp(struct solo_dev *solo_dev, unsigned int ch)
+{
+	int idx;
+
+	if (solo_dev->type == SOLO_DEV_6010)
+		return 2;
+
+	if (WARN_ON_ONCE(ch > 31))
+		return 2;
+
+	if (ch < 16) {
+		idx = 0;
+	} else {
+		ch -= 16;
+		idx = 1;
+	}
+	ch *= 2;
+
+	return (solo_dev->jpeg_qp[idx] >> ch) & 3;
+}
+
+#define SOLO_QP_INIT 0xaaaaaaaa
+
 static void solo_jpeg_config(struct solo_dev *solo_dev)
 {
-	u32 reg;
-	if (solo_dev->flags & FLAGS_6110)
-		reg = (4 << 24) | (3 << 16) | (2 << 8) | (1 << 0);
-	else
-		reg = (2 << 24) | (2 << 16) | (2 << 8) | (2 << 0);
-	solo_reg_write(solo_dev, SOLO_VE_JPEG_QP_TBL, reg);
-	solo_reg_write(solo_dev, SOLO_VE_JPEG_QP_CH_L, 0);
-	solo_reg_write(solo_dev, SOLO_VE_JPEG_QP_CH_H, 0);
+	if (solo_dev->type == SOLO_DEV_6010) {
+		solo_reg_write(solo_dev, SOLO_VE_JPEG_QP_TBL,
+			       (2 << 24) | (2 << 16) | (2 << 8) | 2);
+	} else {
+		solo_reg_write(solo_dev, SOLO_VE_JPEG_QP_TBL,
+			       (4 << 24) | (3 << 16) | (2 << 8) | 1);
+	}
+
+	spin_lock_init(&solo_dev->jpeg_qp_lock);
+
+	/* Initialize Quality Profile for all channels */
+	solo_dev->jpeg_qp[0] = solo_dev->jpeg_qp[1] = SOLO_QP_INIT;
+	solo_reg_write(solo_dev, SOLO_VE_JPEG_QP_CH_L, SOLO_QP_INIT);
+	solo_reg_write(solo_dev, SOLO_VE_JPEG_QP_CH_H, SOLO_QP_INIT);
+
 	solo_reg_write(solo_dev, SOLO_VE_JPEG_CFG,
 		(SOLO_JPEG_EXT_SIZE(solo_dev) & 0xffff0000) |
 		((SOLO_JPEG_EXT_ADDR(solo_dev) >> 16) & 0x0000ffff));
 	solo_reg_write(solo_dev, SOLO_VE_JPEG_CTRL, 0xffffffff);
-	/* que limit, samp limit, pos limit */
-	solo_reg_write(solo_dev, 0x0688, (0 << 16) | (30 << 8) | 60);
+	if (solo_dev->type == SOLO_DEV_6110) {
+		solo_reg_write(solo_dev, SOLO_VE_JPEG_CFG1,
+			       (0 << 16) | (30 << 8) | 60);
+	}
 }
 
 static void solo_mp4e_config(struct solo_dev *solo_dev)
 {
 	int i;
-	u32 reg;
+	u32 cfg;
 
-	/* We can only use VE_INTR_CTRL(0) if we want to support mjpeg */
 	solo_reg_write(solo_dev, SOLO_VE_CFG0,
-		       SOLO_VE_INTR_CTRL(0) |
+		       SOLO_VE_INTR_CTRL(IRQ_LEVEL) |
 		       SOLO_VE_BLOCK_SIZE(SOLO_MP4E_EXT_SIZE(solo_dev) >> 16) |
 		       SOLO_VE_BLOCK_BASE(SOLO_MP4E_EXT_ADDR(solo_dev) >> 16));
 
-	solo_reg_write(solo_dev, SOLO_VE_CFG1,
-		       SOLO_VE_INSERT_INDEX | SOLO_VE_MOTION_MODE(0));
+
+	cfg = SOLO_VE_BYTE_ALIGN(2) | SOLO_VE_INSERT_INDEX
+		| SOLO_VE_MOTION_MODE(0);
+	if (solo_dev->type != SOLO_DEV_6010) {
+		cfg |= SOLO_VE_MPEG_SIZE_H(
+			(SOLO_MP4E_EXT_SIZE(solo_dev) >> 24) & 0x0f);
+		cfg |= SOLO_VE_JPEG_SIZE_H(
+			(SOLO_JPEG_EXT_SIZE(solo_dev) >> 24) & 0x0f);
+	}
+	solo_reg_write(solo_dev, SOLO_VE_CFG1, cfg);
 
 	solo_reg_write(solo_dev, SOLO_VE_WMRK_POLY, 0);
 	solo_reg_write(solo_dev, SOLO_VE_VMRK_INIT_KEY, 0);
 	solo_reg_write(solo_dev, SOLO_VE_WMRK_STRL, 0);
+	if (solo_dev->type == SOLO_DEV_6110)
+		solo_reg_write(solo_dev, SOLO_VE_WMRK_ENABLE, 0);
 	solo_reg_write(solo_dev, SOLO_VE_ENCRYP_POLY, 0);
 	solo_reg_write(solo_dev, SOLO_VE_ENCRYP_INIT, 0);
 
-	reg = SOLO_VE_LITTLE_ENDIAN | SOLO_COMP_ATTR_FCODE(1) |
-		SOLO_COMP_TIME_INC(0) | SOLO_COMP_TIME_WIDTH(15);
-	if (solo_dev->flags & FLAGS_6110)
-		reg |= SOLO_DCT_INTERVAL(10);
-	else
-		reg |= SOLO_DCT_INTERVAL(36 / 4);
-	solo_reg_write(solo_dev, SOLO_VE_ATTR, reg);
+	solo_reg_write(solo_dev, SOLO_VE_ATTR,
+		       SOLO_VE_LITTLE_ENDIAN |
+		       SOLO_COMP_ATTR_FCODE(1) |
+		       SOLO_COMP_TIME_INC(0) |
+		       SOLO_COMP_TIME_WIDTH(15) |
+		       SOLO_DCT_INTERVAL(solo_dev->type == SOLO_DEV_6010 ? 9 : 10));
 
-	for (i = 0; i < solo_dev->nr_chans; i++)
+	for (i = 0; i < solo_dev->nr_chans; i++) {
 		solo_reg_write(solo_dev, SOLO_VE_CH_REF_BASE(i),
 			       (SOLO_EREF_EXT_ADDR(solo_dev) +
 			       (i * SOLO_EREF_EXT_SIZE)) >> 16);
+		solo_reg_write(solo_dev, SOLO_VE_CH_REF_BASE_E(i),
+			       (SOLO_EREF_EXT_ADDR(solo_dev) +
+			       ((i + 16) * SOLO_EREF_EXT_SIZE)) >> 16);
+	}
 
-	if (solo_dev->flags & FLAGS_6110)
-		solo_reg_write(solo_dev, 0x0634, 0x00040008); /* ? */
+	if (solo_dev->type == SOLO_DEV_6110) {
+		solo_reg_write(solo_dev, SOLO_VE_COMPT_MOT, 0x00040008);
+	} else {
+		for (i = 0; i < solo_dev->nr_chans; i++)
+			solo_reg_write(solo_dev, SOLO_VE_CH_MOT(i), 0x100);
+	}
 }
 
 int solo_enc_init(struct solo_dev *solo_dev)
@@ -220,8 +333,6 @@ int solo_enc_init(struct solo_dev *solo_dev)
 		solo_reg_write(solo_dev, SOLO_CAP_CH_COMP_ENA_E(i), 0);
 	}
 
-	solo_irq_on(solo_dev, SOLO_IRQ_ENCODER);
-
 	return 0;
 }
 
@@ -229,8 +340,6 @@ void solo_enc_exit(struct solo_dev *solo_dev)
 {
 	int i;
 
-	solo_irq_off(solo_dev, SOLO_IRQ_ENCODER);
-
 	for (i = 0; i < solo_dev->nr_chans; i++) {
 		solo_reg_write(solo_dev, SOLO_CAP_CH_SCALE(i), 0);
 		solo_reg_write(solo_dev, SOLO_CAP_CH_COMP_ENA_E(i), 0);
diff --git a/drivers/staging/media/solo6x10/g723.c b/drivers/staging/media/solo6x10/solo6x10-g723.c
index 2cd0de28a633..1db18c7972a0 100644
--- a/drivers/staging/media/solo6x10/g723.c
+++ b/drivers/staging/media/solo6x10/solo6x10-g723.c
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
  *
  * 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
@@ -21,17 +26,18 @@
 #include <linux/mempool.h>
 #include <linux/poll.h>
 #include <linux/kthread.h>
-#include <linux/slab.h>
 #include <linux/freezer.h>
-#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+
 #include <sound/core.h>
 #include <sound/initval.h>
 #include <sound/pcm.h>
 #include <sound/control.h>
+
 #include "solo6x10.h"
-#include "tw28.h"
+#include "solo6x10-tw28.h"
 
-#define G723_INTR_ORDER		0
 #define G723_FDMA_PAGES		32
 #define G723_PERIOD_BYTES	48
 #define G723_PERIOD_BLOCK	1024
@@ -46,36 +52,40 @@
 /* The solo writes to 1k byte pages, 32 pages, in the dma. Each 1k page
  * is broken down to 20 * 48 byte regions (one for each channel possible)
  * with the rest of the page being dummy data. */
-#define MAX_BUFFER		(G723_PERIOD_BYTES * PERIODS_MAX)
-#define IRQ_PAGES		4 /* 0 - 4 */
-#define PERIODS_MIN		(1 << IRQ_PAGES)
+#define G723_MAX_BUFFER		(G723_PERIOD_BYTES * PERIODS_MAX)
+#define G723_INTR_ORDER		4 /* 0 - 4 */
+#define PERIODS_MIN		(1 << G723_INTR_ORDER)
 #define PERIODS_MAX		G723_FDMA_PAGES
 
 struct solo_snd_pcm {
-	int		on;
-	spinlock_t	lock;
-	struct solo_dev	*solo_dev;
-	unsigned char	g723_buf[G723_PERIOD_BYTES];
+	int				on;
+	spinlock_t			lock;
+	struct solo_dev		*solo_dev;
+	unsigned char			*g723_buf;
+	dma_addr_t			g723_dma;
 };
 
 static void solo_g723_config(struct solo_dev *solo_dev)
 {
 	int clk_div;
 
-	clk_div = SOLO_CLOCK_MHZ / (SAMPLERATE * (BITRATE * 2) * 2);
+	clk_div = (solo_dev->clock_mhz * 1000000)
+		/ (SAMPLERATE * (BITRATE * 2) * 2);
 
 	solo_reg_write(solo_dev, SOLO_AUDIO_SAMPLE,
-		       SOLO_AUDIO_BITRATE(BITRATE) |
-		       SOLO_AUDIO_CLK_DIV(clk_div));
+		       SOLO_AUDIO_BITRATE(BITRATE)
+		       | SOLO_AUDIO_CLK_DIV(clk_div));
 
 	solo_reg_write(solo_dev, SOLO_AUDIO_FDMA_INTR,
-		      SOLO_AUDIO_FDMA_INTERVAL(IRQ_PAGES) |
-		      SOLO_AUDIO_INTR_ORDER(G723_INTR_ORDER) |
-		      SOLO_AUDIO_FDMA_BASE(SOLO_G723_EXT_ADDR(solo_dev) >> 16));
+		       SOLO_AUDIO_FDMA_INTERVAL(1)
+		       | SOLO_AUDIO_INTR_ORDER(G723_INTR_ORDER)
+		       | SOLO_AUDIO_FDMA_BASE(SOLO_G723_EXT_ADDR(solo_dev) >> 16));
 
 	solo_reg_write(solo_dev, SOLO_AUDIO_CONTROL,
-		       SOLO_AUDIO_ENABLE | SOLO_AUDIO_I2S_MODE |
-		       SOLO_AUDIO_I2S_MULTI(3) | SOLO_AUDIO_MODE(OUTMODE_MASK));
+		       SOLO_AUDIO_ENABLE
+		       | SOLO_AUDIO_I2S_MODE
+		       | SOLO_AUDIO_I2S_MULTI(3)
+		       | SOLO_AUDIO_MODE(OUTMODE_MASK));
 }
 
 void solo_g723_isr(struct solo_dev *solo_dev)
@@ -85,8 +95,6 @@ void solo_g723_isr(struct solo_dev *solo_dev)
 	struct snd_pcm_substream *ss;
 	struct solo_snd_pcm *solo_pcm;
 
-	solo_reg_write(solo_dev, SOLO_IRQ_STAT, SOLO_IRQ_G723);
-
 	for (ss = pstr->substream; ss != NULL; ss = ss->next) {
 		if (snd_pcm_substream_chip(ss) == NULL)
 			continue;
@@ -115,18 +123,18 @@ static int snd_solo_hw_free(struct snd_pcm_substream *ss)
 	return snd_pcm_lib_free_pages(ss);
 }
 
-static struct snd_pcm_hardware snd_solo_pcm_hw = {
+static const struct snd_pcm_hardware snd_solo_pcm_hw = {
 	.info			= (SNDRV_PCM_INFO_MMAP |
 				   SNDRV_PCM_INFO_INTERLEAVED |
 				   SNDRV_PCM_INFO_BLOCK_TRANSFER |
 				   SNDRV_PCM_INFO_MMAP_VALID),
 	.formats		= SNDRV_PCM_FMTBIT_U8,
 	.rates			= SNDRV_PCM_RATE_8000,
-	.rate_min		= 8000,
-	.rate_max		= 8000,
+	.rate_min		= SAMPLERATE,
+	.rate_max		= SAMPLERATE,
 	.channels_min		= 1,
 	.channels_max		= 1,
-	.buffer_bytes_max	= MAX_BUFFER,
+	.buffer_bytes_max	= G723_MAX_BUFFER,
 	.period_bytes_min	= G723_PERIOD_BYTES,
 	.period_bytes_max	= G723_PERIOD_BYTES,
 	.periods_min		= PERIODS_MIN,
@@ -140,7 +148,13 @@ static int snd_solo_pcm_open(struct snd_pcm_substream *ss)
 
 	solo_pcm = kzalloc(sizeof(*solo_pcm), GFP_KERNEL);
 	if (solo_pcm == NULL)
-		return -ENOMEM;
+		goto oom;
+
+	solo_pcm->g723_buf = pci_alloc_consistent(solo_dev->pdev,
+						  G723_PERIOD_BYTES,
+						  &solo_pcm->g723_dma);
+	if (solo_pcm->g723_buf == NULL)
+		goto oom;
 
 	spin_lock_init(&solo_pcm->lock);
 	solo_pcm->solo_dev = solo_dev;
@@ -149,6 +163,10 @@ static int snd_solo_pcm_open(struct snd_pcm_substream *ss)
 	snd_pcm_substream_chip(ss) = solo_pcm;
 
 	return 0;
+
+oom:
+	kfree(solo_pcm);
+	return -ENOMEM;
 }
 
 static int snd_solo_pcm_close(struct snd_pcm_substream *ss)
@@ -156,6 +174,8 @@ static int snd_solo_pcm_close(struct snd_pcm_substream *ss)
 	struct solo_snd_pcm *solo_pcm = snd_pcm_substream_chip(ss);
 
 	snd_pcm_substream_chip(ss) = solo_pcm->solo_dev;
+	pci_free_consistent(solo_pcm->solo_dev->pdev, G723_PERIOD_BYTES,
+			    solo_pcm->g723_buf, solo_pcm->g723_dma);
 	kfree(solo_pcm);
 
 	return 0;
@@ -220,12 +240,11 @@ static int snd_solo_pcm_copy(struct snd_pcm_substream *ss, int channel,
 	for (i = 0; i < (count / G723_FRAMES_PER_PAGE); i++) {
 		int page = (pos / G723_FRAMES_PER_PAGE) + i;
 
-		err = solo_p2m_dma(solo_dev, SOLO_P2M_DMA_ID_G723E, 0,
-				   solo_pcm->g723_buf,
-				   SOLO_G723_EXT_ADDR(solo_dev) +
-				   (page * G723_PERIOD_BLOCK) +
-				   (ss->number * G723_PERIOD_BYTES),
-				   G723_PERIOD_BYTES);
+		err = solo_p2m_dma_t(solo_dev, 0, solo_pcm->g723_dma,
+				     SOLO_G723_EXT_ADDR(solo_dev) +
+				     (page * G723_PERIOD_BLOCK) +
+				     (ss->number * G723_PERIOD_BYTES),
+				     G723_PERIOD_BYTES, 0, 0);
 		if (err)
 			return err;
 
@@ -325,7 +344,7 @@ static int solo_snd_pcm_init(struct solo_dev *solo_dev)
 	ret = snd_pcm_lib_preallocate_pages_for_all(pcm,
 					SNDRV_DMA_TYPE_CONTINUOUS,
 					snd_dma_continuous_data(GFP_KERNEL),
-					MAX_BUFFER, MAX_BUFFER);
+					G723_MAX_BUFFER, G723_MAX_BUFFER);
 	if (ret < 0)
 		return ret;
 
@@ -368,6 +387,7 @@ int solo_g723_init(struct solo_dev *solo_dev)
 	strcpy(card->mixername, "SOLO-6x10");
 	kctl = snd_solo_capture_volume;
 	kctl.count = solo_dev->nr_chans;
+
 	ret = snd_ctl_add(card, snd_ctl_new1(&kctl, solo_dev));
 	if (ret < 0)
 		return ret;
@@ -393,8 +413,12 @@ snd_error:
 
 void solo_g723_exit(struct solo_dev *solo_dev)
 {
+	if (!solo_dev->snd_card)
+		return;
+
 	solo_reg_write(solo_dev, SOLO_AUDIO_CONTROL, 0);
 	solo_irq_off(solo_dev, SOLO_IRQ_G723);
 
 	snd_card_free(solo_dev->snd_card);
+	solo_dev->snd_card = NULL;
 }
diff --git a/drivers/staging/media/solo6x10/gpio.c b/drivers/staging/media/solo6x10/solo6x10-gpio.c
index 0925e6f33a99..73276dc92875 100644
--- a/drivers/staging/media/solo6x10/gpio.c
+++ b/drivers/staging/media/solo6x10/solo6x10-gpio.c
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
  *
  * 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
@@ -19,7 +24,9 @@
 
 #include <linux/kernel.h>
 #include <linux/fs.h>
-#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+
 #include "solo6x10.h"
 
 static void solo_gpio_mode(struct solo_dev *solo_dev,
diff --git a/drivers/staging/media/solo6x10/i2c.c b/drivers/staging/media/solo6x10/solo6x10-i2c.c
index 398070a3d293..01aa417c9258 100644
--- a/drivers/staging/media/solo6x10/i2c.c
+++ b/drivers/staging/media/solo6x10/solo6x10-i2c.c
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
  *
  * 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
@@ -26,6 +31,7 @@
  * thread context, ACK the interrupt, and move on. -- BenC */
 
 #include <linux/kernel.h>
+
 #include "solo6x10.h"
 
 u8 solo_i2c_readbyte(struct solo_dev *solo_dev, int id, u8 addr, u8 off)
@@ -173,10 +179,9 @@ int solo_i2c_isr(struct solo_dev *solo_dev)
 	u32 status = solo_reg_read(solo_dev, SOLO_IIC_CTRL);
 	int ret = -EINVAL;
 
-	solo_reg_write(solo_dev, SOLO_IRQ_STAT, SOLO_IRQ_IIC);
 
-	if (status & (SOLO_IIC_STATE_TRNS | SOLO_IIC_STATE_SIG_ERR) ||
-	    solo_dev->i2c_id < 0) {
+	if (CHK_FLAGS(status, SOLO_IIC_STATE_TRNS | SOLO_IIC_STATE_SIG_ERR)
+	    || solo_dev->i2c_id < 0) {
 		solo_i2c_stop(solo_dev);
 		return -ENXIO;
 	}
@@ -239,7 +244,8 @@ static int solo_i2c_master_xfer(struct i2c_adapter *adap,
 	timeout = HZ / 2;
 
 	for (;;) {
-		prepare_to_wait(&solo_dev->i2c_wait, &wait, TASK_INTERRUPTIBLE);
+		prepare_to_wait(&solo_dev->i2c_wait, &wait,
+				TASK_INTERRUPTIBLE);
 
 		if (solo_dev->i2c_state == IIC_STATE_STOP)
 			break;
@@ -267,7 +273,7 @@ static u32 solo_i2c_functionality(struct i2c_adapter *adap)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm solo_i2c_algo = {
+static const struct i2c_algorithm solo_i2c_algo = {
 	.master_xfer	= solo_i2c_master_xfer,
 	.functionality	= solo_i2c_functionality,
 };
@@ -288,7 +294,8 @@ int solo_i2c_init(struct solo_dev *solo_dev)
 	for (i = 0; i < SOLO_I2C_ADAPTERS; i++) {
 		struct i2c_adapter *adap = &solo_dev->i2c_adap[i];
 
-		snprintf(adap->name, I2C_NAME_SIZE, "%s I2C %d", SOLO6X10_NAME, i);
+		snprintf(adap->name, I2C_NAME_SIZE, "%s I2C %d",
+			 SOLO6X10_NAME, i);
 		adap->algo = &solo_i2c_algo;
 		adap->algo_data = solo_dev;
 		adap->retries = 1;
@@ -311,9 +318,6 @@ int solo_i2c_init(struct solo_dev *solo_dev)
 		return ret;
 	}
 
-	dev_info(&solo_dev->pdev->dev, "Enabled %d i2c adapters\n",
-		 SOLO_I2C_ADAPTERS);
-
 	return 0;
 }
 
diff --git a/drivers/staging/media/solo6x10/solo6x10-jpeg.h b/drivers/staging/media/solo6x10/solo6x10-jpeg.h
index 50defec318cc..c5218ceeabca 100644
--- a/drivers/staging/media/solo6x10/solo6x10-jpeg.h
+++ b/drivers/staging/media/solo6x10/solo6x10-jpeg.h
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
  *
  * 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
@@ -20,7 +25,7 @@
 #ifndef __SOLO6X10_JPEG_H
 #define __SOLO6X10_JPEG_H
 
-static unsigned char jpeg_header[] = {
+static const unsigned char jpeg_header[] = {
 	0xff, 0xd8, 0xff, 0xfe, 0x00, 0x0d, 0x42, 0x6c,
 	0x75, 0x65, 0x63, 0x68, 0x65, 0x72, 0x72, 0x79,
 	0x20, 0xff, 0xdb, 0x00, 0x43, 0x00, 0x20, 0x16,
@@ -102,4 +107,87 @@ static unsigned char jpeg_header[] = {
 /* This is the byte marker for the start of SOF0: 0xffc0 marker */
 #define SOF0_START	575
 
+/* This is the byte marker for the start of the DQT */
+#define DQT_START	17
+#define DQT_LEN		138
+const unsigned char jpeg_dqt[4][DQT_LEN] = {
+	{
+		0xff, 0xdb, 0x00, 0x43, 0x00,
+		0x08, 0x06, 0x06, 0x07, 0x06, 0x05, 0x08, 0x07,
+		0x07, 0x07, 0x09, 0x09, 0x08, 0x0a, 0x0c, 0x14,
+		0x0d, 0x0c, 0x0b, 0x0b, 0x0c, 0x19, 0x12, 0x13,
+		0x0f, 0x14, 0x1d, 0x1a, 0x1f, 0x1e, 0x1d, 0x1a,
+		0x1c, 0x1c, 0x20, 0x24, 0x2e, 0x27, 0x20, 0x22,
+		0x2c, 0x23, 0x1c, 0x1c, 0x28, 0x37, 0x29, 0x2c,
+		0x30, 0x31, 0x34, 0x34, 0x34, 0x1f, 0x27, 0x39,
+		0x3d, 0x38, 0x32, 0x3c, 0x2e, 0x33, 0x34, 0x32,
+		0xff, 0xdb, 0x00, 0x43, 0x01,
+		0x09, 0x09, 0x09, 0x0c, 0x0b, 0x0c, 0x18, 0x0d,
+		0x0d, 0x18, 0x32, 0x21, 0x1c, 0x21, 0x32, 0x32,
+		0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32,
+		0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32,
+		0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32,
+		0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32,
+		0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32,
+		0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32
+	}, {
+		0xff, 0xdb, 0x00, 0x43, 0x00,
+		0x10, 0x0b, 0x0c, 0x0e, 0x0c, 0x0a, 0x10, 0x0e,
+		0x0d, 0x0e, 0x12, 0x11, 0x10, 0x13, 0x18, 0x28,
+		0x1a, 0x18, 0x16, 0x16, 0x18, 0x31, 0x23, 0x25,
+		0x1d, 0x28, 0x3a, 0x33, 0x3d, 0x3c, 0x39, 0x33,
+		0x38, 0x37, 0x40, 0x48, 0x5c, 0x4e, 0x40, 0x44,
+		0x57, 0x45, 0x37, 0x38, 0x50, 0x6d, 0x51, 0x57,
+		0x5f, 0x62, 0x67, 0x68, 0x67, 0x3e, 0x4d, 0x71,
+		0x79, 0x70, 0x64, 0x78, 0x5c, 0x65, 0x67, 0x63,
+		0xff, 0xdb, 0x00, 0x43, 0x01,
+		0x11, 0x12, 0x12, 0x18, 0x15, 0x18, 0x2f, 0x1a,
+		0x1a, 0x2f, 0x63, 0x42, 0x38, 0x42, 0x63, 0x63,
+		0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+		0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+		0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+		0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+		0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+		0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63
+	}, {
+		0xff, 0xdb, 0x00, 0x43, 0x00,
+		0x20, 0x16, 0x18, 0x1c, 0x18, 0x14, 0x20, 0x1c,
+		0x1a, 0x1c, 0x24, 0x22, 0x20, 0x26, 0x30, 0x50,
+		0x34, 0x30, 0x2c, 0x2c, 0x30, 0x62, 0x46, 0x4a,
+		0x3a, 0x50, 0x74, 0x66, 0x7a, 0x78, 0x72, 0x66,
+		0x70, 0x6e, 0x80, 0x90, 0xb8, 0x9c, 0x80, 0x88,
+		0xae, 0x8a, 0x6e, 0x70, 0xa0, 0xda, 0xa2, 0xae,
+		0xbe, 0xc4, 0xce, 0xd0, 0xce, 0x7c, 0x9a, 0xe2,
+		0xf2, 0xe0, 0xc8, 0xf0, 0xb8, 0xca, 0xce, 0xc6,
+		0xff, 0xdb, 0x00, 0x43, 0x01,
+		0x22, 0x24, 0x24, 0x30, 0x2a, 0x30, 0x5e, 0x34,
+		0x34, 0x5e, 0xc6, 0x84, 0x70, 0x84, 0xc6, 0xc6,
+		0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6,
+		0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6,
+		0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6,
+		0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6,
+		0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6,
+		0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6, 0xc6
+	}, {
+		0xff, 0xdb, 0x00, 0x43, 0x00,
+		0x30, 0x21, 0x24, 0x2a, 0x24, 0x1e, 0x30, 0x2a,
+		0x27, 0x2a, 0x36, 0x33, 0x30, 0x39, 0x48, 0x78,
+		0x4e, 0x48, 0x42, 0x42, 0x48, 0x93, 0x69, 0x6f,
+		0x57, 0x78, 0xae, 0x99, 0xb7, 0xb4, 0xab, 0x99,
+		0xa8, 0xa5, 0xc0, 0xd8, 0xff, 0xea, 0xc0, 0xcc,
+		0xff, 0xcf, 0xa5, 0xa8, 0xf0, 0xff, 0xf3, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xba, 0xe7, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+		0xff, 0xdb, 0x00, 0x43, 0x01,
+		0x33, 0x36, 0x36, 0x48, 0x3f, 0x48, 0x8d, 0x4e,
+		0x4e, 0x8d, 0xff, 0xc6, 0xa8, 0xc6, 0xff, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff
+	}
+};
+
 #endif /* __SOLO6X10_JPEG_H */
diff --git a/drivers/staging/media/solo6x10/solo6x10-offsets.h b/drivers/staging/media/solo6x10/solo6x10-offsets.h
new file mode 100644
index 000000000000..f005dca501f1
--- /dev/null
+++ b/drivers/staging/media/solo6x10/solo6x10-offsets.h
@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
+ *
+ * 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.
+ */
+
+#ifndef __SOLO6X10_OFFSETS_H
+#define __SOLO6X10_OFFSETS_H
+
+#define SOLO_DISP_EXT_ADDR			0x00000000
+#define SOLO_DISP_EXT_SIZE			0x00480000
+
+#define SOLO_EOSD_EXT_ADDR \
+	(SOLO_DISP_EXT_ADDR + SOLO_DISP_EXT_SIZE)
+#define SOLO_EOSD_EXT_SIZE(__solo) \
+	(__solo->type == SOLO_DEV_6010 ? 0x10000 : 0x20000)
+#define SOLO_EOSD_EXT_SIZE_MAX			0x20000
+#define SOLO_EOSD_EXT_AREA(__solo) \
+	(SOLO_EOSD_EXT_SIZE(__solo) * 32)
+
+#define SOLO_MOTION_EXT_ADDR(__solo) \
+	(SOLO_EOSD_EXT_ADDR + SOLO_EOSD_EXT_AREA(__solo))
+#define SOLO_MOTION_EXT_SIZE			0x00080000
+
+#define SOLO_G723_EXT_ADDR(__solo) \
+	(SOLO_MOTION_EXT_ADDR(__solo) + SOLO_MOTION_EXT_SIZE)
+#define SOLO_G723_EXT_SIZE			0x00010000
+
+#define SOLO_CAP_EXT_ADDR(__solo) \
+	(SOLO_G723_EXT_ADDR(__solo) + SOLO_G723_EXT_SIZE)
+
+/* 18 is the maximum number of pages required for PAL@D1, the largest frame
+ * possible */
+#define SOLO_CAP_PAGE_SIZE			(18 << 16)
+
+/* Always allow the encoder enough for 16 channels, even if we have less. The
+ * exception is if we have card with only 32Megs of memory. */
+#define SOLO_CAP_EXT_SIZE(__solo) \
+	((((__solo->sdram_size <= (32 << 20)) ? 4 : 16) + 1)	\
+	 * SOLO_CAP_PAGE_SIZE)
+
+#define SOLO_EREF_EXT_ADDR(__solo) \
+	(SOLO_CAP_EXT_ADDR(__solo) + SOLO_CAP_EXT_SIZE(__solo))
+#define SOLO_EREF_EXT_SIZE			0x00140000
+#define SOLO_EREF_EXT_AREA(__solo) \
+	(SOLO_EREF_EXT_SIZE * __solo->nr_chans * 2)
+
+#define __SOLO_JPEG_MIN_SIZE(__solo)		(__solo->nr_chans * 0x00080000)
+
+#define SOLO_MP4E_EXT_ADDR(__solo) \
+	(SOLO_EREF_EXT_ADDR(__solo) + SOLO_EREF_EXT_AREA(__solo))
+#define SOLO_MP4E_EXT_SIZE(__solo) \
+	max((__solo->nr_chans * 0x00080000),				\
+	    min(((__solo->sdram_size - SOLO_MP4E_EXT_ADDR(__solo)) -	\
+		 __SOLO_JPEG_MIN_SIZE(__solo)), 0x00ff0000))
+
+#define __SOLO_JPEG_MIN_SIZE(__solo)		(__solo->nr_chans * 0x00080000)
+#define SOLO_JPEG_EXT_ADDR(__solo) \
+		(SOLO_MP4E_EXT_ADDR(__solo) + SOLO_MP4E_EXT_SIZE(__solo))
+#define SOLO_JPEG_EXT_SIZE(__solo) \
+	max(__SOLO_JPEG_MIN_SIZE(__solo),				\
+	    min((__solo->sdram_size - SOLO_JPEG_EXT_ADDR(__solo)), 0x00ff0000))
+
+#define SOLO_SDRAM_END(__solo) \
+	(SOLO_JPEG_EXT_ADDR(__solo) + SOLO_JPEG_EXT_SIZE(__solo))
+
+#endif /* __SOLO6X10_OFFSETS_H */
diff --git a/drivers/staging/media/solo6x10/solo6x10-p2m.c b/drivers/staging/media/solo6x10/solo6x10-p2m.c
new file mode 100644
index 000000000000..333594189b81
--- /dev/null
+++ b/drivers/staging/media/solo6x10/solo6x10-p2m.c
@@ -0,0 +1,333 @@
+/*
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
+ *
+ * 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/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+
+#include "solo6x10.h"
+
+static int multi_p2m;
+module_param(multi_p2m, uint, 0644);
+MODULE_PARM_DESC(multi_p2m,
+		 "Use multiple P2M DMA channels (default: no, 6010-only)");
+
+static int desc_mode;
+module_param(desc_mode, uint, 0644);
+MODULE_PARM_DESC(desc_mode,
+		 "Allow use of descriptor mode DMA (default: no, 6010-only)");
+
+int solo_p2m_dma(struct solo_dev *solo_dev, int wr,
+		 void *sys_addr, u32 ext_addr, u32 size,
+		 int repeat, u32 ext_size)
+{
+	dma_addr_t dma_addr;
+	int ret;
+
+	if (WARN_ON_ONCE((unsigned long)sys_addr & 0x03))
+		return -EINVAL;
+	if (WARN_ON_ONCE(!size))
+		return -EINVAL;
+
+	dma_addr = pci_map_single(solo_dev->pdev, sys_addr, size,
+				  wr ? PCI_DMA_TODEVICE : PCI_DMA_FROMDEVICE);
+	if (pci_dma_mapping_error(solo_dev->pdev, dma_addr))
+		return -ENOMEM;
+
+	ret = solo_p2m_dma_t(solo_dev, wr, dma_addr, ext_addr, size,
+			     repeat, ext_size);
+
+	pci_unmap_single(solo_dev->pdev, dma_addr, size,
+			 wr ? PCI_DMA_TODEVICE : PCI_DMA_FROMDEVICE);
+
+	return ret;
+}
+
+/* Mutex must be held for p2m_id before calling this!! */
+int solo_p2m_dma_desc(struct solo_dev *solo_dev,
+		      struct solo_p2m_desc *desc, dma_addr_t desc_dma,
+		      int desc_cnt)
+{
+	struct solo_p2m_dev *p2m_dev;
+	unsigned int timeout;
+	unsigned int config = 0;
+	int ret = 0;
+	int p2m_id = 0;
+
+	/* Get next ID. According to Softlogic, 6110 has problems on !=0 P2M */
+	if (solo_dev->type != SOLO_DEV_6110 && multi_p2m) {
+		p2m_id = atomic_inc_return(&solo_dev->p2m_count) % SOLO_NR_P2M;
+		if (p2m_id < 0)
+			p2m_id = -p2m_id;
+	}
+
+	p2m_dev = &solo_dev->p2m_dev[p2m_id];
+
+	if (mutex_lock_interruptible(&p2m_dev->mutex))
+		return -EINTR;
+
+	INIT_COMPLETION(p2m_dev->completion);
+	p2m_dev->error = 0;
+
+	if (desc_cnt > 1 && solo_dev->type != SOLO_DEV_6110 && desc_mode) {
+		/* For 6010 with more than one desc, we can do a one-shot */
+		p2m_dev->desc_count = p2m_dev->desc_idx = 0;
+		config = solo_reg_read(solo_dev, SOLO_P2M_CONFIG(p2m_id));
+
+		solo_reg_write(solo_dev, SOLO_P2M_DES_ADR(p2m_id), desc_dma);
+		solo_reg_write(solo_dev, SOLO_P2M_DESC_ID(p2m_id), desc_cnt);
+		solo_reg_write(solo_dev, SOLO_P2M_CONFIG(p2m_id), config |
+			       SOLO_P2M_DESC_MODE);
+	} else {
+		/* For single descriptors and 6110, we need to run each desc */
+		p2m_dev->desc_count = desc_cnt;
+		p2m_dev->desc_idx = 1;
+		p2m_dev->descs = desc;
+
+		solo_reg_write(solo_dev, SOLO_P2M_TAR_ADR(p2m_id),
+			       desc[1].dma_addr);
+		solo_reg_write(solo_dev, SOLO_P2M_EXT_ADR(p2m_id),
+			       desc[1].ext_addr);
+		solo_reg_write(solo_dev, SOLO_P2M_EXT_CFG(p2m_id),
+			       desc[1].cfg);
+		solo_reg_write(solo_dev, SOLO_P2M_CONTROL(p2m_id),
+			       desc[1].ctrl);
+	}
+
+	timeout = wait_for_completion_timeout(&p2m_dev->completion,
+					      solo_dev->p2m_jiffies);
+
+	if (WARN_ON_ONCE(p2m_dev->error))
+		ret = -EIO;
+	else if (timeout == 0) {
+		solo_dev->p2m_timeouts++;
+		ret = -EAGAIN;
+	}
+
+	solo_reg_write(solo_dev, SOLO_P2M_CONTROL(p2m_id), 0);
+
+	/* Don't write here for the no_desc_mode case, because config is 0.
+	 * We can't test no_desc_mode again, it might race. */
+	if (desc_cnt > 1 && solo_dev->type != SOLO_DEV_6110 && config)
+		solo_reg_write(solo_dev, SOLO_P2M_CONFIG(p2m_id), config);
+
+	mutex_unlock(&p2m_dev->mutex);
+
+	return ret;
+}
+
+void solo_p2m_fill_desc(struct solo_p2m_desc *desc, int wr,
+			dma_addr_t dma_addr, u32 ext_addr, u32 size,
+			int repeat, u32 ext_size)
+{
+	WARN_ON_ONCE(dma_addr & 0x03);
+	WARN_ON_ONCE(!size);
+
+	desc->cfg = SOLO_P2M_COPY_SIZE(size >> 2);
+	desc->ctrl = SOLO_P2M_BURST_SIZE(SOLO_P2M_BURST_256) |
+		(wr ? SOLO_P2M_WRITE : 0) | SOLO_P2M_TRANS_ON;
+
+	if (repeat) {
+		desc->cfg |= SOLO_P2M_EXT_INC(ext_size >> 2);
+		desc->ctrl |=  SOLO_P2M_PCI_INC(size >> 2) |
+			 SOLO_P2M_REPEAT(repeat);
+	}
+
+	desc->dma_addr = dma_addr;
+	desc->ext_addr = ext_addr;
+}
+
+int solo_p2m_dma_t(struct solo_dev *solo_dev, int wr,
+		   dma_addr_t dma_addr, u32 ext_addr, u32 size,
+		   int repeat, u32 ext_size)
+{
+	struct solo_p2m_desc desc[2];
+
+	solo_p2m_fill_desc(&desc[1], wr, dma_addr, ext_addr, size, repeat,
+			   ext_size);
+
+	/* No need for desc_dma since we know it is a single-shot */
+	return solo_p2m_dma_desc(solo_dev, desc, 0, 1);
+}
+
+void solo_p2m_isr(struct solo_dev *solo_dev, int id)
+{
+	struct solo_p2m_dev *p2m_dev = &solo_dev->p2m_dev[id];
+	struct solo_p2m_desc *desc;
+
+	if (p2m_dev->desc_count <= p2m_dev->desc_idx) {
+		complete(&p2m_dev->completion);
+		return;
+	}
+
+	/* Setup next descriptor */
+	p2m_dev->desc_idx++;
+	desc = &p2m_dev->descs[p2m_dev->desc_idx];
+
+	solo_reg_write(solo_dev, SOLO_P2M_CONTROL(id), 0);
+	solo_reg_write(solo_dev, SOLO_P2M_TAR_ADR(id), desc->dma_addr);
+	solo_reg_write(solo_dev, SOLO_P2M_EXT_ADR(id), desc->ext_addr);
+	solo_reg_write(solo_dev, SOLO_P2M_EXT_CFG(id), desc->cfg);
+	solo_reg_write(solo_dev, SOLO_P2M_CONTROL(id), desc->ctrl);
+}
+
+void solo_p2m_error_isr(struct solo_dev *solo_dev)
+{
+	unsigned int err = solo_reg_read(solo_dev, SOLO_PCI_ERR);
+	struct solo_p2m_dev *p2m_dev;
+	int i;
+
+	if (!(err & (SOLO_PCI_ERR_P2M | SOLO_PCI_ERR_P2M_DESC)))
+		return;
+
+	for (i = 0; i < SOLO_NR_P2M; i++) {
+		p2m_dev = &solo_dev->p2m_dev[i];
+		p2m_dev->error = 1;
+		solo_reg_write(solo_dev, SOLO_P2M_CONTROL(i), 0);
+		complete(&p2m_dev->completion);
+	}
+}
+
+void solo_p2m_exit(struct solo_dev *solo_dev)
+{
+	int i;
+
+	for (i = 0; i < SOLO_NR_P2M; i++)
+		solo_irq_off(solo_dev, SOLO_IRQ_P2M(i));
+}
+
+static int solo_p2m_test(struct solo_dev *solo_dev, int base, int size)
+{
+	u32 *wr_buf;
+	u32 *rd_buf;
+	int i;
+	int ret = -EIO;
+	int order = get_order(size);
+
+	wr_buf = (u32 *)__get_free_pages(GFP_KERNEL, order);
+	if (wr_buf == NULL)
+		return -1;
+
+	rd_buf = (u32 *)__get_free_pages(GFP_KERNEL, order);
+	if (rd_buf == NULL) {
+		free_pages((unsigned long)wr_buf, order);
+		return -1;
+	}
+
+	for (i = 0; i < (size >> 3); i++)
+		*(wr_buf + i) = (i << 16) | (i + 1);
+
+	for (i = (size >> 3); i < (size >> 2); i++)
+		*(wr_buf + i) = ~((i << 16) | (i + 1));
+
+	memset(rd_buf, 0x55, size);
+
+	if (solo_p2m_dma(solo_dev, 1, wr_buf, base, size, 0, 0))
+		goto test_fail;
+
+	if (solo_p2m_dma(solo_dev, 0, rd_buf, base, size, 0, 0))
+		goto test_fail;
+
+	for (i = 0; i < (size >> 2); i++) {
+		if (*(wr_buf + i) != *(rd_buf + i))
+			goto test_fail;
+	}
+
+	ret = 0;
+
+test_fail:
+	free_pages((unsigned long)wr_buf, order);
+	free_pages((unsigned long)rd_buf, order);
+
+	return ret;
+}
+
+int solo_p2m_init(struct solo_dev *solo_dev)
+{
+	struct solo_p2m_dev *p2m_dev;
+	int i;
+
+	for (i = 0; i < SOLO_NR_P2M; i++) {
+		p2m_dev = &solo_dev->p2m_dev[i];
+
+		mutex_init(&p2m_dev->mutex);
+		init_completion(&p2m_dev->completion);
+
+		solo_reg_write(solo_dev, SOLO_P2M_CONTROL(i), 0);
+		solo_reg_write(solo_dev, SOLO_P2M_CONFIG(i),
+			       SOLO_P2M_CSC_16BIT_565 |
+			       SOLO_P2M_DESC_INTR_OPT |
+			       SOLO_P2M_DMA_INTERVAL(0) |
+			       SOLO_P2M_PCI_MASTER_MODE);
+		solo_irq_on(solo_dev, SOLO_IRQ_P2M(i));
+	}
+
+	/* Find correct SDRAM size */
+	for (solo_dev->sdram_size = 0, i = 2; i >= 0; i--) {
+		solo_reg_write(solo_dev, SOLO_DMA_CTRL,
+			       SOLO_DMA_CTRL_REFRESH_CYCLE(1) |
+			       SOLO_DMA_CTRL_SDRAM_SIZE(i) |
+			       SOLO_DMA_CTRL_SDRAM_CLK_INVERT |
+			       SOLO_DMA_CTRL_READ_CLK_SELECT |
+			       SOLO_DMA_CTRL_LATENCY(1));
+
+		solo_reg_write(solo_dev, SOLO_SYS_CFG, solo_dev->sys_config |
+			       SOLO_SYS_CFG_RESET);
+		solo_reg_write(solo_dev, SOLO_SYS_CFG, solo_dev->sys_config);
+
+		switch (i) {
+		case 2:
+			if (solo_p2m_test(solo_dev, 0x07ff0000, 0x00010000) ||
+			    solo_p2m_test(solo_dev, 0x05ff0000, 0x00010000))
+				continue;
+			break;
+
+		case 1:
+			if (solo_p2m_test(solo_dev, 0x03ff0000, 0x00010000))
+				continue;
+			break;
+
+		default:
+			if (solo_p2m_test(solo_dev, 0x01ff0000, 0x00010000))
+				continue;
+		}
+
+		solo_dev->sdram_size = (32 << 20) << i;
+		break;
+	}
+
+	if (!solo_dev->sdram_size) {
+		dev_err(&solo_dev->pdev->dev, "Error detecting SDRAM size\n");
+		return -EIO;
+	}
+
+	if (SOLO_SDRAM_END(solo_dev) > solo_dev->sdram_size) {
+		dev_err(&solo_dev->pdev->dev,
+			"SDRAM is not large enough (%u < %u)\n",
+			solo_dev->sdram_size, SOLO_SDRAM_END(solo_dev));
+		return -EIO;
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/media/solo6x10/registers.h b/drivers/staging/media/solo6x10/solo6x10-regs.h
index aca544472c93..428f6c951180 100644
--- a/drivers/staging/media/solo6x10/registers.h
+++ b/drivers/staging/media/solo6x10/solo6x10-regs.h
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
  *
  * 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
@@ -20,20 +25,20 @@
 #ifndef __SOLO6X10_REGISTERS_H
 #define __SOLO6X10_REGISTERS_H
 
-#include "offsets.h"
+#include "solo6x10-offsets.h"
 
-/* Global 6X10 system configuration */
+/* Global 6010 system configuration */
 #define SOLO_SYS_CFG				0x0000
-#define   SOLO6010_SYS_CFG_FOUT_EN		0x00000001 /* 6010 only */
-#define   SOLO6010_SYS_CFG_PLL_BYPASS		0x00000002 /* 6010 only */
-#define   SOLO6010_SYS_CFG_PLL_PWDN		0x00000004 /* 6010 only */
-#define   SOLO6010_SYS_CFG_OUTDIV(__n)		(((__n) & 0x003) << 3) /* 6010 only */
-#define   SOLO6010_SYS_CFG_FEEDBACKDIV(__n)	(((__n) & 0x1ff) << 5) /* 6010 only */
-#define   SOLO6010_SYS_CFG_INPUTDIV(__n)	(((__n) & 0x01f) << 14) /* 6010 only */
+#define   SOLO_SYS_CFG_FOUT_EN			0x00000001
+#define   SOLO_SYS_CFG_PLL_BYPASS		0x00000002
+#define   SOLO_SYS_CFG_PLL_PWDN			0x00000004
+#define   SOLO_SYS_CFG_OUTDIV(__n)		(((__n) & 0x003) << 3)
+#define   SOLO_SYS_CFG_FEEDBACKDIV(__n)		(((__n) & 0x1ff) << 5)
+#define   SOLO_SYS_CFG_INPUTDIV(__n)		(((__n) & 0x01f) << 14)
 #define   SOLO_SYS_CFG_CLOCK_DIV		0x00080000
 #define   SOLO_SYS_CFG_NCLK_DELAY(__n)		(((__n) & 0x003) << 24)
 #define   SOLO_SYS_CFG_PCLK_DELAY(__n)		(((__n) & 0x00f) << 26)
-#define   SOLO_SYS_CFG_SDRAM64BIT		0x40000000 /* 6110: must be set */
+#define   SOLO_SYS_CFG_SDRAM64BIT		0x40000000
 #define   SOLO_SYS_CFG_RESET			0x80000000
 
 #define	SOLO_DMA_CTRL				0x0004
@@ -45,7 +50,9 @@
 #define	  SOLO_DMA_CTRL_READ_DATA_SELECT	(1<<3)
 #define	  SOLO_DMA_CTRL_READ_CLK_SELECT		(1<<2)
 #define	  SOLO_DMA_CTRL_LATENCY(n)		((n)<<0)
-#define	SOLO_DMA_CTRL1				0x0008
+
+/* Some things we set in this are undocumented. Why Softlogic?!?! */
+#define SOLO_DMA_CTRL1				0x0008
 
 #define SOLO_SYS_VCLK				0x000C
 #define	  SOLO_VCLK_INVERT			(1<<22)
@@ -61,7 +68,7 @@
 #define	  SOLO_VCLK_VIN0001_DELAY(n)		((n)<<0)
 
 #define SOLO_IRQ_STAT				0x0010
-#define SOLO_IRQ_ENABLE				0x0014
+#define SOLO_IRQ_MASK				0x0014
 #define	  SOLO_IRQ_P2M(n)			(1<<((n)+17))
 #define	  SOLO_IRQ_GPIO				(1<<16)
 #define	  SOLO_IRQ_VIDEO_LOSS			(1<<15)
@@ -82,22 +89,7 @@
 #define SOLO_CHIP_OPTION			0x001C
 #define   SOLO_CHIP_ID_MASK			0x00000007
 
-#define SOLO6110_PLL_CONFIG			0x0020
-#define   SOLO6110_PLL_RANGE_BYPASS		(0 << 20)
-#define   SOLO6110_PLL_RANGE_5_10MHZ		(1 << 20)
-#define   SOLO6110_PLL_RANGE_8_16MHZ		(2 << 20)
-#define   SOLO6110_PLL_RANGE_13_26MHZ		(3 << 20)
-#define   SOLO6110_PLL_RANGE_21_42MHZ		(4 << 20)
-#define   SOLO6110_PLL_RANGE_34_68MHZ		(5 << 20)
-#define   SOLO6110_PLL_RANGE_54_108MHZ		(6 << 20)
-#define   SOLO6110_PLL_RANGE_88_200MHZ		(7 << 20)
-#define   SOLO6110_PLL_DIVR(x)			(((x) - 1) << 15)
-#define   SOLO6110_PLL_DIVQ_EXP(x)		((x) << 12)
-#define   SOLO6110_PLL_DIVF(x)			(((x) - 1) << 4)
-#define   SOLO6110_PLL_RESET			(1 << 3)
-#define   SOLO6110_PLL_BYPASS			(1 << 2)
-#define   SOLO6110_PLL_FSEN			(1 << 1)
-#define   SOLO6110_PLL_FB			(1 << 0)
+#define SOLO_PLL_CONFIG				0x0020 /* 6110 Only */
 
 #define SOLO_EEPROM_CTRL			0x0060
 #define	  SOLO_EEPROM_ACCESS_EN			(1<<7)
@@ -105,7 +97,7 @@
 #define	  SOLO_EEPROM_CLK			(1<<2)
 #define	  SOLO_EEPROM_DO			(1<<1)
 #define	  SOLO_EEPROM_DI			(1<<0)
-#define	  SOLO_EEPROM_ENABLE			(EEPROM_ACCESS_EN | EEPROM_CS)
+#define	  SOLO_EEPROM_ENABLE (SOLO_EEPROM_ACCESS_EN | SOLO_EEPROM_CS)
 
 #define SOLO_PCI_ERR				0x0070
 #define   SOLO_PCI_ERR_FATAL			0x00000001
@@ -274,8 +266,8 @@
 #define	  SOLO_VO_FI_CHANGE			(1<<20)
 #define	  SOLO_VO_USER_COLOR_SET_VSYNC		(1<<19)
 #define	  SOLO_VO_USER_COLOR_SET_HSYNC		(1<<18)
-#define	  SOLO_VO_USER_COLOR_SET_NAV		(1<<17)
-#define	  SOLO_VO_USER_COLOR_SET_NAH		(1<<16)
+#define	  SOLO_VO_USER_COLOR_SET_NAH		(1<<17)
+#define	  SOLO_VO_USER_COLOR_SET_NAV		(1<<16)
 #define	  SOLO_VO_NA_COLOR_Y(Y)			((Y)<<8)
 #define	  SOLO_VO_NA_COLOR_CB(CB)		(((CB)/16)<<4)
 #define	  SOLO_VO_NA_COLOR_CR(CR)		(((CR)/16)<<0)
@@ -401,12 +393,13 @@
 #define	  SOLO_VE_BLOCK_BASE(n)			((n)<<0)
 
 #define SOLO_VE_CFG1				0x0614
-#define   SOLO6110_VE_MPEG_SIZE_H(n)		((n)<<28) /* 6110 only */
-#define	  SOLO6010_VE_BYTE_ALIGN(n)		((n)<<24) /* 6010 only */
-#define   SOLO6110_VE_JPEG_SIZE_H(n)		((n)<<20) /* 6110 only */
+#define	  SOLO_VE_BYTE_ALIGN(n)			((n)<<24)
 #define	  SOLO_VE_INSERT_INDEX			(1<<18)
 #define	  SOLO_VE_MOTION_MODE(n)		((n)<<16)
 #define	  SOLO_VE_MOTION_BASE(n)		((n)<<0)
+#define   SOLO_VE_MPEG_SIZE_H(n)		((n)<<28) /* 6110 Only */
+#define   SOLO_VE_JPEG_SIZE_H(n)		((n)<<20) /* 6110 Only */
+#define   SOLO_VE_INSERT_INDEX_JPEG		(1<<19)   /* 6110 Only */
 
 #define SOLO_VE_WMRK_POLY			0x061C
 #define SOLO_VE_VMRK_INIT_KEY			0x0620
@@ -420,6 +413,7 @@
 #define	  SOLO_COMP_TIME_INC(n)			((n)<<25)
 #define	  SOLO_COMP_TIME_WIDTH(n)		((n)<<21)
 #define	  SOLO_DCT_INTERVAL(n)			((n)<<16)
+#define SOLO_VE_COMPT_MOT			0x0634 /* 6110 Only */
 
 #define SOLO_VE_STATE(n)			(0x0640+((n)*4))
 
@@ -428,14 +422,21 @@
 #define SOLO_VE_JPEG_QP_CH_H			0x0678
 #define SOLO_VE_JPEG_CFG			0x067C
 #define SOLO_VE_JPEG_CTRL			0x0680
-
+#define SOLO_VE_CODE_ENCRYPT			0x0684 /* 6110 Only */
+#define SOLO_VE_JPEG_CFG1			0x0688 /* 6110 Only */
+#define SOLO_VE_WMRK_ENABLE			0x068C /* 6110 Only */
 #define SOLO_VE_OSD_CH				0x0690
 #define SOLO_VE_OSD_BASE			0x0694
 #define SOLO_VE_OSD_CLR				0x0698
 #define SOLO_VE_OSD_OPT				0x069C
+#define   SOLO_VE_OSD_V_DOUBLE			(1<<16) /* 6110 Only */
+#define   SOLO_VE_OSD_H_SHADOW			(1<<15)
+#define   SOLO_VE_OSD_V_SHADOW			(1<<14)
+#define   SOLO_VE_OSD_H_OFFSET(n)		((n & 0x7f)<<7)
+#define   SOLO_VE_OSD_V_OFFSET(n)		(n & 0x7f)
 
 #define SOLO_VE_CH_INTL(ch)			(0x0700+((ch)*4))
-#define SOLO6010_VE_CH_MOT(ch)			(0x0740+((ch)*4)) /* 6010 only */
+#define SOLO_VE_CH_MOT(ch)			(0x0740+((ch)*4))
 #define SOLO_VE_CH_QP(ch)			(0x0780+((ch)*4))
 #define SOLO_VE_CH_QP_E(ch)			(0x07C0+((ch)*4))
 #define SOLO_VE_CH_GOP(ch)			(0x0800+((ch)*4))
@@ -447,7 +448,7 @@
 #define SOLO_VE_JPEG_QUE(n)			(0x0A04+((n)*8))
 
 #define SOLO_VD_CFG0				0x0900
-#define	  SOLO6010_VD_CFG_NO_WRITE_NO_WINDOW	(1<<24) /* 6010 only */
+#define	  SOLO_VD_CFG_NO_WRITE_NO_WINDOW	(1<<24)
 #define	  SOLO_VD_CFG_BUSY_WIAT_CODE		(1<<23)
 #define	  SOLO_VD_CFG_BUSY_WIAT_REF		(1<<22)
 #define	  SOLO_VD_CFG_BUSY_WIAT_RES		(1<<21)
@@ -599,9 +600,9 @@
 #define	  SOLO_UART_RX_DATA_POP			(1<<8)
 
 #define SOLO_TIMER_CLOCK_NUM			0x0be0
-#define SOLO_TIMER_WATCHDOG			0x0be4
 #define SOLO_TIMER_USEC				0x0be8
 #define SOLO_TIMER_SEC				0x0bec
+#define SOLO_TIMER_USEC_LSB			0x0d20 /* 6110 Only */
 
 #define SOLO_AUDIO_CONTROL			0x0D00
 #define	  SOLO_AUDIO_ENABLE			(1<<31)
@@ -629,9 +630,10 @@
 #define	  SOLO_AUDIO_EVOL(ch, value)		((value)<<((ch)%10))
 #define SOLO_AUDIO_STA				0x0D14
 
-
-#define SOLO_WATCHDOG				0x0BE4
-#define WATCHDOG_STAT(status)			(status<<8)
-#define WATCHDOG_TIME(sec)			(sec&0xff)
+/*
+ * Watchdog configuration
+ */
+#define SOLO_WATCHDOG				0x0be4
+#define SOLO_WATCHDOG_SET(status, sec)		(status << 8 | (sec & 0xff))
 
 #endif /* __SOLO6X10_REGISTERS_H */
diff --git a/drivers/staging/media/solo6x10/tw28.c b/drivers/staging/media/solo6x10/solo6x10-tw28.c
index db56b42c56c6..ad00e2b60323 100644
--- a/drivers/staging/media/solo6x10/tw28.c
+++ b/drivers/staging/media/solo6x10/solo6x10-tw28.c
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
  *
  * 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
@@ -18,12 +23,12 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/delay.h>
+
 #include "solo6x10.h"
-#include "tw28.h"
+#include "solo6x10-tw28.h"
 
-/* XXX: Some of these values are masked into an 8-bit regs, and shifted
- * around for other 8-bit regs. What are the magic bits in these values? */
-#define DEFAULT_HDELAY_NTSC		(32 - 4)
+#define DEFAULT_HDELAY_NTSC		(32 - 8)
 #define DEFAULT_HACTIVE_NTSC		(720 + 16)
 #define DEFAULT_VDELAY_NTSC		(7 - 2)
 #define DEFAULT_VACTIVE_NTSC		(240 + 4)
@@ -33,15 +38,16 @@
 #define DEFAULT_VDELAY_PAL		(6)
 #define DEFAULT_VACTIVE_PAL		(312-DEFAULT_VDELAY_PAL)
 
-static u8 tbl_tw2864_template[] = {
-	0x00, 0x00, 0x80, 0x10, 0x80, 0x80, 0x00, 0x02, /* 0x00 */
-	0x12, 0xf5, 0x09, 0xd0, 0x00, 0x00, 0x00, 0x7f,
-	0x00, 0x00, 0x80, 0x10, 0x80, 0x80, 0x00, 0x02, /* 0x10 */
-	0x12, 0xf5, 0x09, 0xd0, 0x00, 0x00, 0x00, 0x7f,
-	0x00, 0x00, 0x80, 0x10, 0x80, 0x80, 0x00, 0x02, /* 0x20 */
-	0x12, 0xf5, 0x09, 0xd0, 0x00, 0x00, 0x00, 0x7f,
-	0x00, 0x00, 0x80, 0x10, 0x80, 0x80, 0x00, 0x02, /* 0x30 */
-	0x12, 0xf5, 0x09, 0xd0, 0x00, 0x00, 0x00, 0x7f,
+
+static const u8 tbl_tw2864_ntsc_template[] = {
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x02, /* 0x00 */
+	0x12, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x00, 0x7f,
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x02, /* 0x10 */
+	0x12, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x00, 0x7f,
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x02, /* 0x20 */
+	0x12, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x00, 0x7f,
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x02, /* 0x30 */
+	0x12, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x00, 0x7f,
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x40 */
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x50 */
@@ -61,14 +67,49 @@ static u8 tbl_tw2864_template[] = {
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0xc0 */
 	0x00, 0x00, 0x55, 0x00, 0xb1, 0xe4, 0x40, 0x00,
 	0x77, 0x77, 0x01, 0x13, 0x57, 0x9b, 0xdf, 0x20, /* 0xd0 */
-	0x64, 0xa8, 0xec, 0xd1, 0x0f, 0x11, 0x11, 0x81,
-	0x10, 0xe0, 0xbb, 0xbb, 0x00, 0x11, 0x00, 0x00, /* 0xe0 */
+	0x64, 0xa8, 0xec, 0xc1, 0x0f, 0x11, 0x11, 0x81,
+	0x00, 0xe0, 0xbb, 0xbb, 0x00, 0x11, 0x00, 0x00, /* 0xe0 */
 	0x11, 0x00, 0x00, 0x11, 0x00, 0x00, 0x11, 0x00,
 	0x83, 0xb5, 0x09, 0x78, 0x85, 0x00, 0x01, 0x20, /* 0xf0 */
 	0x64, 0x11, 0x40, 0xaf, 0xff, 0x00, 0x00, 0x00,
 };
 
-static u8 tbl_tw2865_ntsc_template[] = {
+static const u8 tbl_tw2864_pal_template[] = {
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x12, /* 0x00 */
+	0x18, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x01, 0x7f,
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x12, /* 0x10 */
+	0x18, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x01, 0x7f,
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x12, /* 0x20 */
+	0x18, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x01, 0x7f,
+	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x12, /* 0x30 */
+	0x18, 0xf5, 0x0c, 0xd0, 0x00, 0x00, 0x01, 0x7f,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x40 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x50 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x60 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0x70 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xA3, 0x00,
+	0x00, 0x02, 0x00, 0xcc, 0x00, 0x80, 0x44, 0x50, /* 0x80 */
+	0x22, 0x01, 0xd8, 0xbc, 0xb8, 0x44, 0x38, 0x00,
+	0x00, 0x78, 0x72, 0x3e, 0x14, 0xa5, 0xe4, 0x05, /* 0x90 */
+	0x00, 0x28, 0x44, 0x44, 0xa0, 0x90, 0x5a, 0x01,
+	0x0a, 0x0a, 0x0a, 0x0a, 0x1a, 0x1a, 0x1a, 0x1a, /* 0xa0 */
+	0x00, 0x00, 0x00, 0xf0, 0xf0, 0xf0, 0xf0, 0x44,
+	0x44, 0x0a, 0x00, 0xff, 0xef, 0xef, 0xef, 0xef, /* 0xb0 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* 0xc0 */
+	0x00, 0x00, 0x55, 0x00, 0xb1, 0xe4, 0x40, 0x00,
+	0x77, 0x77, 0x01, 0x13, 0x57, 0x9b, 0xdf, 0x20, /* 0xd0 */
+	0x64, 0xa8, 0xec, 0xc1, 0x0f, 0x11, 0x11, 0x81,
+	0x00, 0xe0, 0xbb, 0xbb, 0x00, 0x11, 0x00, 0x00, /* 0xe0 */
+	0x11, 0x00, 0x00, 0x11, 0x00, 0x00, 0x11, 0x00,
+	0x83, 0xb5, 0x09, 0x00, 0xa0, 0x00, 0x01, 0x20, /* 0xf0 */
+	0x64, 0x11, 0x40, 0xaf, 0xff, 0x00, 0x00, 0x00,
+};
+
+static const u8 tbl_tw2865_ntsc_template[] = {
 	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x02, /* 0x00 */
 	0x12, 0xff, 0x09, 0xd0, 0x00, 0x00, 0x00, 0x7f,
 	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x02, /* 0x10 */
@@ -103,7 +144,7 @@ static u8 tbl_tw2865_ntsc_template[] = {
 	0x64, 0x51, 0x40, 0xaf, 0xFF, 0xF0, 0x00, 0xC0,
 };
 
-static u8 tbl_tw2865_pal_template[] = {
+static const u8 tbl_tw2865_pal_template[] = {
 	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x12, /* 0x00 */
 	0x11, 0xff, 0x01, 0xc3, 0x00, 0x00, 0x01, 0x7f,
 	0x00, 0xf0, 0x70, 0x30, 0x80, 0x80, 0x00, 0x12, /* 0x10 */
@@ -180,8 +221,8 @@ static void tw_write_and_verify(struct solo_dev *solo_dev, u8 addr, u8 off,
 		msleep_interruptible(1);
 	}
 
-/*	printk("solo6x10/tw28: Error writing register: %02x->%02x [%02x]\n",
-		addr, off, val); */
+/*	printk("solo6x10/tw28: Error writing register: %02x->%02x [%02x]\n", */
+/*		addr, off, val); */
 }
 
 static int tw2865_setup(struct solo_dev *solo_dev, u8 dev_addr)
@@ -216,16 +257,17 @@ static int tw2865_setup(struct solo_dev *solo_dev, u8 dev_addr)
 
 	for (i = 0; i < 0xff; i++) {
 		/* Skip read only registers */
-		if (i >= 0xb8 && i <= 0xc1)
-			continue;
-		if ((i & ~0x30) == 0x00 ||
-		    (i & ~0x30) == 0x0c ||
-		    (i & ~0x30) == 0x0d)
-			continue;
-		if (i >= 0xc4 && i <= 0xc7)
+		switch (i) {
+		case 0xb8 ... 0xc1:
+		case 0xc4 ... 0xc7:
+		case 0xfd:
 			continue;
-		if (i == 0xfd)
+		}
+		switch (i & ~0x30) {
+		case 0x00:
+		case 0x0c ... 0x0d:
 			continue;
+		}
 
 		tw_write_and_verify(solo_dev, dev_addr, i,
 				    tbl_tw2865_common[i]);
@@ -236,11 +278,15 @@ static int tw2865_setup(struct solo_dev *solo_dev, u8 dev_addr)
 
 static int tw2864_setup(struct solo_dev *solo_dev, u8 dev_addr)
 {
-	u8 tbl_tw2864_common[sizeof(tbl_tw2864_template)];
+	u8 tbl_tw2864_common[256];
 	int i;
 
-	memcpy(tbl_tw2864_common, tbl_tw2864_template,
-	       sizeof(tbl_tw2864_common));
+	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_PAL)
+		memcpy(tbl_tw2864_common, tbl_tw2864_pal_template,
+		       sizeof(tbl_tw2864_common));
+	else
+		memcpy(tbl_tw2864_common, tbl_tw2864_ntsc_template,
+		       sizeof(tbl_tw2864_common));
 
 	if (solo_dev->tw2865 == 0) {
 		/* IRQ Mode */
@@ -285,33 +331,19 @@ static int tw2864_setup(struct solo_dev *solo_dev, u8 dev_addr)
 		}
 	}
 
-	/* NTSC or PAL */
-	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_PAL) {
-		for (i = 0; i < 4; i++) {
-			tbl_tw2864_common[0x07 | (i << 4)] |= 0x10;
-			tbl_tw2864_common[0x08 | (i << 4)] |= 0x06;
-			tbl_tw2864_common[0x0a | (i << 4)] |= 0x08;
-			tbl_tw2864_common[0x0b | (i << 4)] |= 0x13;
-			tbl_tw2864_common[0x0e | (i << 4)] |= 0x01;
-		}
-		tbl_tw2864_common[0x9d] = 0x90;
-		tbl_tw2864_common[0xf3] = 0x00;
-		tbl_tw2864_common[0xf4] = 0xa0;
-	}
-
 	for (i = 0; i < 0xff; i++) {
 		/* Skip read only registers */
-		if (i >= 0xb8 && i <= 0xc1)
+		switch (i) {
+		case 0xb8 ... 0xc1:
+		case 0xfd:
 			continue;
-		if ((i & ~0x30) == 0x00 ||
-		    (i & ~0x30) == 0x0c ||
-		    (i & ~0x30) == 0x0d)
-			continue;
-		if (i == 0x74 || i == 0x77 || i == 0x78 ||
-		    i == 0x79 || i == 0x7a)
-			continue;
-		if (i == 0xfd)
+		}
+		switch (i & ~0x30) {
+		case 0x00:
+		case 0x0c:
+		case 0x0d:
 			continue;
+		}
 
 		tw_write_and_verify(solo_dev, dev_addr, i,
 				    tbl_tw2864_common[i]);
@@ -544,8 +576,10 @@ int solo_tw28_init(struct solo_dev *solo_dev)
 	int i;
 	u8 value;
 
-	/* Detect techwell chip type */
-	for (i = 0; i < TW_NUM_CHIP; i++) {
+	solo_dev->tw28_cnt = 0;
+
+	/* Detect techwell chip type(s) */
+	for (i = 0; i < solo_dev->nr_chans / 4; i++) {
 		value = solo_i2c_readbyte(solo_dev, SOLO_I2C_TW,
 					  TW_CHIP_OFFSET_ADDR(i), 0xFF);
 
@@ -560,7 +594,8 @@ int solo_tw28_init(struct solo_dev *solo_dev)
 			break;
 		default:
 			value = solo_i2c_readbyte(solo_dev, SOLO_I2C_TW,
-						  TW_CHIP_OFFSET_ADDR(i), 0x59);
+						  TW_CHIP_OFFSET_ADDR(i),
+						  0x59);
 			if ((value >> 3) == 0x04) {
 				solo_dev->tw2815 |= 1 << i;
 				solo_dev->tw28_cnt++;
@@ -568,8 +603,11 @@ int solo_tw28_init(struct solo_dev *solo_dev)
 		}
 	}
 
-	if (!solo_dev->tw28_cnt)
+	if (solo_dev->tw28_cnt != (solo_dev->nr_chans >> 2)) {
+		dev_err(&solo_dev->pdev->dev,
+			"Could not initialize any techwell chips\n");
 		return -EINVAL;
+	}
 
 	saa7128_setup(solo_dev);
 
@@ -582,17 +620,6 @@ int solo_tw28_init(struct solo_dev *solo_dev)
 			tw2815_setup(solo_dev, TW_CHIP_OFFSET_ADDR(i));
 	}
 
-	dev_info(&solo_dev->pdev->dev, "Initialized %d tw28xx chip%s:",
-		 solo_dev->tw28_cnt, solo_dev->tw28_cnt == 1 ? "" : "s");
-
-	if (solo_dev->tw2865)
-		printk(" tw2865[%d]", hweight32(solo_dev->tw2865));
-	if (solo_dev->tw2864)
-		printk(" tw2864[%d]", hweight32(solo_dev->tw2864));
-	if (solo_dev->tw2815)
-		printk(" tw2815[%d]", hweight32(solo_dev->tw2815));
-	printk("\n");
-
 	return 0;
 }
 
@@ -610,7 +637,7 @@ int tw28_get_video_status(struct solo_dev *solo_dev, u8 ch)
 	chip_num = ch / 4;
 	ch %= 4;
 
-	val = tw_readbyte(solo_dev, chip_num, TW286X_AV_STAT_ADDR,
+	val = tw_readbyte(solo_dev, chip_num, TW286x_AV_STAT_ADDR,
 			  TW_AV_STAT_ADDR) & 0x0f;
 
 	return val & (1 << ch) ? 1 : 0;
@@ -626,7 +653,7 @@ u16 tw28_get_audio_status(struct solo_dev *solo_dev)
 	int i;
 
 	for (i = 0; i < solo_dev->tw28_cnt; i++) {
-		val = (tw_readbyte(solo_dev, i, TW286X_AV_STAT_ADDR,
+		val = (tw_readbyte(solo_dev, i, TW286x_AV_STAT_ADDR,
 				   TW_AV_STAT_ADDR) & 0xf0) >> 4;
 		status |= val << (i * 4);
 	}
@@ -635,7 +662,13 @@ u16 tw28_get_audio_status(struct solo_dev *solo_dev)
 }
 #endif
 
-int tw28_set_ctrl_val(struct solo_dev *solo_dev, u32 ctrl, u8 ch, s32 val)
+bool tw28_has_sharpness(struct solo_dev *solo_dev, u8 ch)
+{
+	return is_tw286x(solo_dev, ch / 4);
+}
+
+int tw28_set_ctrl_val(struct solo_dev *solo_dev, u32 ctrl, u8 ch,
+		      s32 val)
 {
 	char sval;
 	u8 chip_num;
@@ -650,8 +683,6 @@ int tw28_set_ctrl_val(struct solo_dev *solo_dev, u32 ctrl, u8 ch, s32 val)
 	switch (ctrl) {
 	case V4L2_CID_SHARPNESS:
 		/* Only 286x has sharpness */
-		if (val > 0x0f || val < 0)
-			return -ERANGE;
 		if (is_tw286x(solo_dev, chip_num)) {
 			u8 v = solo_i2c_readbyte(solo_dev, SOLO_I2C_TW,
 						 TW_CHIP_OFFSET_ADDR(chip_num),
@@ -661,8 +692,9 @@ int tw28_set_ctrl_val(struct solo_dev *solo_dev, u32 ctrl, u8 ch, s32 val)
 			solo_i2c_writebyte(solo_dev, SOLO_I2C_TW,
 					   TW_CHIP_OFFSET_ADDR(chip_num),
 					   TW286x_SHARPNESS(chip_num), v);
-		} else if (val != 0)
-			return -ERANGE;
+		} else {
+			return -EINVAL;
+		}
 		break;
 
 	case V4L2_CID_HUE:
@@ -676,6 +708,7 @@ int tw28_set_ctrl_val(struct solo_dev *solo_dev, u32 ctrl, u8 ch, s32 val)
 		break;
 
 	case V4L2_CID_SATURATION:
+		/* 286x chips have a U and V component for saturation */
 		if (is_tw286x(solo_dev, chip_num)) {
 			solo_i2c_writebyte(solo_dev, SOLO_I2C_TW,
 					   TW_CHIP_OFFSET_ADDR(chip_num),
diff --git a/drivers/staging/media/solo6x10/tw28.h b/drivers/staging/media/solo6x10/solo6x10-tw28.h
index a44a03afbd30..1a02c87d4cf0 100644
--- a/drivers/staging/media/solo6x10/tw28.h
+++ b/drivers/staging/media/solo6x10/solo6x10-tw28.h
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
  *
  * 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
@@ -36,7 +41,7 @@
 #define TW_AUDIO_INPUT_GAIN_ADDR(n)		(0x60 + ((n > 1) ? 1 : 0))
 
 /* tw286x */
-#define TW286X_AV_STAT_ADDR			0xfd
+#define TW286x_AV_STAT_ADDR			0xfd
 #define TW286x_HUE_ADDR(n)			(0x06 | ((n) << 4))
 #define TW286x_SATURATIONU_ADDR(n)		(0x04 | ((n) << 4))
 #define TW286x_SATURATIONV_ADDR(n)		(0x05 | ((n) << 4))
@@ -50,6 +55,7 @@ int solo_tw28_init(struct solo_dev *solo_dev);
 
 int tw28_set_ctrl_val(struct solo_dev *solo_dev, u32 ctrl, u8 ch, s32 val);
 int tw28_get_ctrl_val(struct solo_dev *solo_dev, u32 ctrl, u8 ch, s32 *val);
+bool tw28_has_sharpness(struct solo_dev *solo_dev, u8 ch);
 
 u8 tw28_get_audio_gain(struct solo_dev *solo_dev, u8 ch);
 void tw28_set_audio_gain(struct solo_dev *solo_dev, u8 ch, u8 val);
diff --git a/drivers/staging/media/solo6x10/solo6x10-v4l2-enc.c b/drivers/staging/media/solo6x10/solo6x10-v4l2-enc.c
new file mode 100644
index 000000000000..98e2902afd74
--- /dev/null
+++ b/drivers/staging/media/solo6x10/solo6x10-v4l2-enc.c
@@ -0,0 +1,1385 @@
+/*
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
+ *
+ * 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/kernel.h>
+#include <linux/module.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-dma-sg.h>
+
+#include "solo6x10.h"
+#include "solo6x10-tw28.h"
+#include "solo6x10-jpeg.h"
+
+#define MIN_VID_BUFFERS		2
+#define FRAME_BUF_SIZE		(196 * 1024)
+#define MP4_QS			16
+#define DMA_ALIGN		4096
+
+/* 6010 M4V */
+static unsigned char vop_6010_ntsc_d1[] = {
+	0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x20,
+	0x02, 0x48, 0x1d, 0xc0, 0x00, 0x40, 0x00, 0x40,
+	0x00, 0x40, 0x00, 0x80, 0x00, 0x97, 0x53, 0x04,
+	0x1f, 0x4c, 0x58, 0x10, 0xf0, 0x71, 0x18, 0x3f,
+};
+
+static unsigned char vop_6010_ntsc_cif[] = {
+	0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x20,
+	0x02, 0x48, 0x1d, 0xc0, 0x00, 0x40, 0x00, 0x40,
+	0x00, 0x40, 0x00, 0x80, 0x00, 0x97, 0x53, 0x04,
+	0x1f, 0x4c, 0x2c, 0x10, 0x78, 0x51, 0x18, 0x3f,
+};
+
+static unsigned char vop_6010_pal_d1[] = {
+	0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x20,
+	0x02, 0x48, 0x15, 0xc0, 0x00, 0x40, 0x00, 0x40,
+	0x00, 0x40, 0x00, 0x80, 0x00, 0x97, 0x53, 0x04,
+	0x1f, 0x4c, 0x58, 0x11, 0x20, 0x71, 0x18, 0x3f,
+};
+
+static unsigned char vop_6010_pal_cif[] = {
+	0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x20,
+	0x02, 0x48, 0x15, 0xc0, 0x00, 0x40, 0x00, 0x40,
+	0x00, 0x40, 0x00, 0x80, 0x00, 0x97, 0x53, 0x04,
+	0x1f, 0x4c, 0x2c, 0x10, 0x90, 0x51, 0x18, 0x3f,
+};
+
+/* 6110 h.264 */
+static unsigned char vop_6110_ntsc_d1[] = {
+	0x00, 0x00, 0x00, 0x01, 0x67, 0x42, 0x00, 0x1e,
+	0x9a, 0x74, 0x05, 0x81, 0xec, 0x80, 0x00, 0x00,
+	0x00, 0x01, 0x68, 0xce, 0x32, 0x28, 0x00, 0x00,
+};
+
+static unsigned char vop_6110_ntsc_cif[] = {
+	0x00, 0x00, 0x00, 0x01, 0x67, 0x42, 0x00, 0x1e,
+	0x9a, 0x74, 0x0b, 0x0f, 0xc8, 0x00, 0x00, 0x00,
+	0x01, 0x68, 0xce, 0x32, 0x28, 0x00, 0x00, 0x00,
+};
+
+static unsigned char vop_6110_pal_d1[] = {
+	0x00, 0x00, 0x00, 0x01, 0x67, 0x42, 0x00, 0x1e,
+	0x9a, 0x74, 0x05, 0x80, 0x93, 0x20, 0x00, 0x00,
+	0x00, 0x01, 0x68, 0xce, 0x32, 0x28, 0x00, 0x00,
+};
+
+static unsigned char vop_6110_pal_cif[] = {
+	0x00, 0x00, 0x00, 0x01, 0x67, 0x42, 0x00, 0x1e,
+	0x9a, 0x74, 0x0b, 0x04, 0xb2, 0x00, 0x00, 0x00,
+	0x01, 0x68, 0xce, 0x32, 0x28, 0x00, 0x00, 0x00,
+};
+
+struct vop_header {
+	/* VE_STATUS0 */
+	u32 mpeg_size:20, sad_motion_flag:1, video_motion_flag:1, vop_type:2,
+		channel:5, source_fl:1, interlace:1, progressive:1;
+
+	/* VE_STATUS1 */
+	u32 vsize:8, hsize:8, last_queue:4, nop0:8, scale:4;
+
+	/* VE_STATUS2 */
+	u32 mpeg_off;
+
+	/* VE_STATUS3 */
+	u32 jpeg_off;
+
+	/* VE_STATUS4 */
+	u32 jpeg_size:20, interval:10, nop1:2;
+
+	/* VE_STATUS5/6 */
+	u32 sec, usec;
+
+	/* VE_STATUS7/8/9 */
+	u32 nop2[3];
+
+	/* VE_STATUS10 */
+	u32 mpeg_size_alt:20, nop3:12;
+
+	u32 end_nops[5];
+} __packed;
+
+struct solo_enc_buf {
+	enum solo_enc_types	type;
+	struct vop_header	*vh;
+	int			motion;
+};
+
+static int solo_is_motion_on(struct solo_enc_dev *solo_enc)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+
+	return (solo_dev->motion_mask >> solo_enc->ch) & 1;
+}
+
+static int solo_motion_detected(struct solo_enc_dev *solo_enc)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	unsigned long flags;
+	u32 ch_mask = 1 << solo_enc->ch;
+	int ret = 0;
+
+	spin_lock_irqsave(&solo_enc->motion_lock, flags);
+	if (solo_reg_read(solo_dev, SOLO_VI_MOT_STATUS) & ch_mask) {
+		solo_reg_write(solo_dev, SOLO_VI_MOT_CLEAR, ch_mask);
+		ret = 1;
+	}
+	spin_unlock_irqrestore(&solo_enc->motion_lock, flags);
+
+	return ret;
+}
+
+static void solo_motion_toggle(struct solo_enc_dev *solo_enc, int on)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	u32 mask = 1 << solo_enc->ch;
+	unsigned long flags;
+
+	spin_lock_irqsave(&solo_enc->motion_lock, flags);
+
+	if (on)
+		solo_dev->motion_mask |= mask;
+	else
+		solo_dev->motion_mask &= ~mask;
+
+	solo_reg_write(solo_dev, SOLO_VI_MOT_CLEAR, mask);
+
+	solo_reg_write(solo_dev, SOLO_VI_MOT_ADR,
+		       SOLO_VI_MOTION_EN(solo_dev->motion_mask) |
+		       (SOLO_MOTION_EXT_ADDR(solo_dev) >> 16));
+
+	spin_unlock_irqrestore(&solo_enc->motion_lock, flags);
+}
+
+void solo_update_mode(struct solo_enc_dev *solo_enc)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	int vop_len;
+	unsigned char *vop;
+
+	solo_enc->interlaced = (solo_enc->mode & 0x08) ? 1 : 0;
+	solo_enc->bw_weight = max(solo_dev->fps / solo_enc->interval, 1);
+
+	if (solo_enc->mode == SOLO_ENC_MODE_CIF) {
+		solo_enc->width = solo_dev->video_hsize >> 1;
+		solo_enc->height = solo_dev->video_vsize;
+		if (solo_dev->type == SOLO_DEV_6110) {
+			if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC) {
+				vop = vop_6110_ntsc_cif;
+				vop_len = sizeof(vop_6110_ntsc_cif);
+			} else {
+				vop = vop_6110_pal_cif;
+				vop_len = sizeof(vop_6110_pal_cif);
+			}
+		} else {
+			if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC) {
+				vop = vop_6010_ntsc_cif;
+				vop_len = sizeof(vop_6010_ntsc_cif);
+			} else {
+				vop = vop_6010_pal_cif;
+				vop_len = sizeof(vop_6010_pal_cif);
+			}
+		}
+	} else {
+		solo_enc->width = solo_dev->video_hsize;
+		solo_enc->height = solo_dev->video_vsize << 1;
+		solo_enc->bw_weight <<= 2;
+		if (solo_dev->type == SOLO_DEV_6110) {
+			if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC) {
+				vop = vop_6110_ntsc_d1;
+				vop_len = sizeof(vop_6110_ntsc_d1);
+			} else {
+				vop = vop_6110_pal_d1;
+				vop_len = sizeof(vop_6110_pal_d1);
+			}
+		} else {
+			if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC) {
+				vop = vop_6010_ntsc_d1;
+				vop_len = sizeof(vop_6010_ntsc_d1);
+			} else {
+				vop = vop_6010_pal_d1;
+				vop_len = sizeof(vop_6010_pal_d1);
+			}
+		}
+	}
+
+	memcpy(solo_enc->vop, vop, vop_len);
+
+	/* Some fixups for 6010/M4V */
+	if (solo_dev->type == SOLO_DEV_6010) {
+		u16 fps = solo_dev->fps * 1000;
+		u16 interval = solo_enc->interval * 1000;
+
+		vop = solo_enc->vop;
+
+		/* Frame rate and interval */
+		vop[22] = fps >> 4;
+		vop[23] = ((fps << 4) & 0xf0) | 0x0c
+			| ((interval >> 13) & 0x3);
+		vop[24] = (interval >> 5) & 0xff;
+		vop[25] = ((interval << 3) & 0xf8) | 0x04;
+	}
+
+	solo_enc->vop_len = vop_len;
+
+	/* Now handle the jpeg header */
+	vop = solo_enc->jpeg_header;
+	vop[SOF0_START + 5] = 0xff & (solo_enc->height >> 8);
+	vop[SOF0_START + 6] = 0xff & solo_enc->height;
+	vop[SOF0_START + 7] = 0xff & (solo_enc->width >> 8);
+	vop[SOF0_START + 8] = 0xff & solo_enc->width;
+
+	memcpy(vop + DQT_START,
+	       jpeg_dqt[solo_g_jpeg_qp(solo_dev, solo_enc->ch)], DQT_LEN);
+}
+
+static int solo_enc_on(struct solo_enc_dev *solo_enc)
+{
+	u8 ch = solo_enc->ch;
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	u8 interval;
+
+	solo_update_mode(solo_enc);
+
+	/* Make sure to do a bandwidth check */
+	if (solo_enc->bw_weight > solo_dev->enc_bw_remain)
+		return -EBUSY;
+	solo_enc->sequence = 0;
+	solo_dev->enc_bw_remain -= solo_enc->bw_weight;
+
+	if (solo_enc->type == SOLO_ENC_TYPE_EXT)
+		solo_reg_write(solo_dev, SOLO_CAP_CH_COMP_ENA_E(ch), 1);
+
+	/* Disable all encoding for this channel */
+	solo_reg_write(solo_dev, SOLO_CAP_CH_SCALE(ch), 0);
+
+	/* Common for both std and ext encoding */
+	solo_reg_write(solo_dev, SOLO_VE_CH_INTL(ch),
+		       solo_enc->interlaced ? 1 : 0);
+
+	if (solo_enc->interlaced)
+		interval = solo_enc->interval - 1;
+	else
+		interval = solo_enc->interval;
+
+	/* Standard encoding only */
+	solo_reg_write(solo_dev, SOLO_VE_CH_GOP(ch), solo_enc->gop);
+	solo_reg_write(solo_dev, SOLO_VE_CH_QP(ch), solo_enc->qp);
+	solo_reg_write(solo_dev, SOLO_CAP_CH_INTV(ch), interval);
+
+	/* Extended encoding only */
+	solo_reg_write(solo_dev, SOLO_VE_CH_GOP_E(ch), solo_enc->gop);
+	solo_reg_write(solo_dev, SOLO_VE_CH_QP_E(ch), solo_enc->qp);
+	solo_reg_write(solo_dev, SOLO_CAP_CH_INTV_E(ch), interval);
+
+	/* Enables the standard encoder */
+	solo_reg_write(solo_dev, SOLO_CAP_CH_SCALE(ch), solo_enc->mode);
+
+	return 0;
+}
+
+static void solo_enc_off(struct solo_enc_dev *solo_enc)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+
+	solo_dev->enc_bw_remain += solo_enc->bw_weight;
+
+	solo_reg_write(solo_dev, SOLO_CAP_CH_SCALE(solo_enc->ch), 0);
+	solo_reg_write(solo_dev, SOLO_CAP_CH_COMP_ENA_E(solo_enc->ch), 0);
+}
+
+static int enc_get_mpeg_dma(struct solo_dev *solo_dev, dma_addr_t dma,
+			      unsigned int off, unsigned int size)
+{
+	int ret;
+
+	if (off > SOLO_MP4E_EXT_SIZE(solo_dev))
+		return -EINVAL;
+
+	/* Single shot */
+	if (off + size <= SOLO_MP4E_EXT_SIZE(solo_dev)) {
+		return solo_p2m_dma_t(solo_dev, 0, dma,
+				      SOLO_MP4E_EXT_ADDR(solo_dev) + off, size,
+				      0, 0);
+	}
+
+	/* Buffer wrap */
+	ret = solo_p2m_dma_t(solo_dev, 0, dma,
+			     SOLO_MP4E_EXT_ADDR(solo_dev) + off,
+			     SOLO_MP4E_EXT_SIZE(solo_dev) - off, 0, 0);
+
+	if (!ret) {
+		ret = solo_p2m_dma_t(solo_dev, 0,
+			     dma + SOLO_MP4E_EXT_SIZE(solo_dev) - off,
+			     SOLO_MP4E_EXT_ADDR(solo_dev),
+			     size + off - SOLO_MP4E_EXT_SIZE(solo_dev), 0, 0);
+	}
+
+	return ret;
+}
+
+/* Build a descriptor queue out of an SG list and send it to the P2M for
+ * processing. */
+static int solo_send_desc(struct solo_enc_dev *solo_enc, int skip,
+			  struct vb2_dma_sg_desc *vbuf, int off, int size,
+			  unsigned int base, unsigned int base_size)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct scatterlist *sg;
+	int i;
+	int ret;
+
+	if (WARN_ON_ONCE(size > FRAME_BUF_SIZE))
+		return -EINVAL;
+
+	solo_enc->desc_count = 1;
+
+	for_each_sg(vbuf->sglist, sg, vbuf->num_pages, i) {
+		struct solo_p2m_desc *desc;
+		dma_addr_t dma;
+		int len;
+		int left = base_size - off;
+
+		desc = &solo_enc->desc_items[solo_enc->desc_count++];
+		dma = sg_dma_address(sg);
+		len = sg_dma_len(sg);
+
+		/* We assume this is smaller than the scatter size */
+		BUG_ON(skip >= len);
+		if (skip) {
+			len -= skip;
+			dma += skip;
+			size -= skip;
+			skip = 0;
+		}
+
+		len = min(len, size);
+
+		if (len <= left) {
+			/* Single descriptor */
+			solo_p2m_fill_desc(desc, 0, dma, base + off,
+					   len, 0, 0);
+		} else {
+			/* Buffer wrap */
+			/* XXX: Do these as separate DMA requests, to avoid
+			   timeout errors triggered by awkwardly sized
+			   descriptors. See
+			   <https://github.com/bluecherrydvr/solo6x10/issues/8>
+			 */
+			ret = solo_p2m_dma_t(solo_dev, 0, dma, base + off,
+					     left, 0, 0);
+			if (ret)
+				return ret;
+
+			ret = solo_p2m_dma_t(solo_dev, 0, dma + left, base,
+					     len - left, 0, 0);
+			if (ret)
+				return ret;
+
+			solo_enc->desc_count--;
+		}
+
+		size -= len;
+		if (size <= 0)
+			break;
+
+		off += len;
+		if (off >= base_size)
+			off -= base_size;
+
+		/* Because we may use two descriptors per loop */
+		if (solo_enc->desc_count >= (solo_enc->desc_nelts - 1)) {
+			ret = solo_p2m_dma_desc(solo_dev, solo_enc->desc_items,
+						solo_enc->desc_dma,
+						solo_enc->desc_count - 1);
+			if (ret)
+				return ret;
+			solo_enc->desc_count = 1;
+		}
+	}
+
+	if (solo_enc->desc_count <= 1)
+		return 0;
+
+	return solo_p2m_dma_desc(solo_dev, solo_enc->desc_items, solo_enc->desc_dma,
+				 solo_enc->desc_count - 1);
+}
+
+static int solo_fill_jpeg(struct solo_enc_dev *solo_enc,
+		struct vb2_buffer *vb, struct vop_header *vh)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct vb2_dma_sg_desc *vbuf = vb2_dma_sg_plane_desc(vb, 0);
+	int frame_size;
+	int ret;
+
+	vb->v4l2_buf.flags |= V4L2_BUF_FLAG_KEYFRAME;
+
+	if (vb2_plane_size(vb, 0) < vh->jpeg_size + solo_enc->jpeg_len)
+		return -EIO;
+
+	sg_copy_from_buffer(vbuf->sglist, vbuf->num_pages,
+			solo_enc->jpeg_header,
+			solo_enc->jpeg_len);
+
+	frame_size = (vh->jpeg_size + solo_enc->jpeg_len + (DMA_ALIGN - 1))
+		& ~(DMA_ALIGN - 1);
+	vb2_set_plane_payload(vb, 0, vh->jpeg_size + solo_enc->jpeg_len);
+
+	dma_map_sg(&solo_dev->pdev->dev, vbuf->sglist, vbuf->num_pages,
+			DMA_FROM_DEVICE);
+	ret = solo_send_desc(solo_enc, solo_enc->jpeg_len, vbuf, vh->jpeg_off,
+			frame_size, SOLO_JPEG_EXT_ADDR(solo_dev),
+			SOLO_JPEG_EXT_SIZE(solo_dev));
+	dma_unmap_sg(&solo_dev->pdev->dev, vbuf->sglist, vbuf->num_pages,
+			DMA_FROM_DEVICE);
+	return ret;
+}
+
+static int solo_fill_mpeg(struct solo_enc_dev *solo_enc,
+		struct vb2_buffer *vb, struct vop_header *vh)
+{
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct vb2_dma_sg_desc *vbuf = vb2_dma_sg_plane_desc(vb, 0);
+	int frame_off, frame_size;
+	int skip = 0;
+	int ret;
+
+	if (vb2_plane_size(vb, 0) < vh->mpeg_size)
+		return -EIO;
+
+	/* If this is a key frame, add extra header */
+	if (!vh->vop_type) {
+		sg_copy_from_buffer(vbuf->sglist, vbuf->num_pages,
+				solo_enc->vop,
+				solo_enc->vop_len);
+
+		skip = solo_enc->vop_len;
+
+		vb->v4l2_buf.flags |= V4L2_BUF_FLAG_KEYFRAME;
+		vb2_set_plane_payload(vb, 0, vh->mpeg_size + solo_enc->vop_len);
+	} else {
+		vb->v4l2_buf.flags |= V4L2_BUF_FLAG_PFRAME;
+		vb2_set_plane_payload(vb, 0, vh->mpeg_size);
+	}
+
+	/* Now get the actual mpeg payload */
+	frame_off = (vh->mpeg_off + sizeof(*vh))
+		% SOLO_MP4E_EXT_SIZE(solo_dev);
+	frame_size = (vh->mpeg_size + skip + (DMA_ALIGN - 1))
+		& ~(DMA_ALIGN - 1);
+
+	dma_map_sg(&solo_dev->pdev->dev, vbuf->sglist, vbuf->num_pages,
+			DMA_FROM_DEVICE);
+	ret = solo_send_desc(solo_enc, skip, vbuf, frame_off, frame_size,
+			SOLO_MP4E_EXT_ADDR(solo_dev),
+			SOLO_MP4E_EXT_SIZE(solo_dev));
+	dma_unmap_sg(&solo_dev->pdev->dev, vbuf->sglist, vbuf->num_pages,
+			DMA_FROM_DEVICE);
+	return ret;
+}
+
+static int solo_enc_fillbuf(struct solo_enc_dev *solo_enc,
+			    struct vb2_buffer *vb, struct solo_enc_buf *enc_buf)
+{
+	struct vop_header *vh = enc_buf->vh;
+	int ret;
+
+	/* Check for motion flags */
+	vb->v4l2_buf.flags &= ~(V4L2_BUF_FLAG_MOTION_ON |
+				V4L2_BUF_FLAG_MOTION_DETECTED);
+	if (solo_is_motion_on(solo_enc)) {
+		vb->v4l2_buf.flags |= V4L2_BUF_FLAG_MOTION_ON;
+		if (enc_buf->motion)
+			vb->v4l2_buf.flags |= V4L2_BUF_FLAG_MOTION_DETECTED;
+	}
+
+	switch (solo_enc->fmt) {
+	case V4L2_PIX_FMT_MPEG4:
+	case V4L2_PIX_FMT_H264:
+		ret = solo_fill_mpeg(solo_enc, vb, vh);
+		break;
+	default: /* V4L2_PIX_FMT_MJPEG */
+		ret = solo_fill_jpeg(solo_enc, vb, vh);
+		break;
+	}
+
+	if (!ret) {
+		vb->v4l2_buf.sequence = solo_enc->sequence++;
+		vb->v4l2_buf.timestamp.tv_sec = vh->sec;
+		vb->v4l2_buf.timestamp.tv_usec = vh->usec;
+	}
+
+	vb2_buffer_done(vb, ret ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+
+	return ret;
+}
+
+static void solo_enc_handle_one(struct solo_enc_dev *solo_enc,
+				struct solo_enc_buf *enc_buf)
+{
+	struct solo_vb2_buf *vb;
+	unsigned long flags;
+
+	mutex_lock(&solo_enc->lock);
+	if (solo_enc->type != enc_buf->type)
+		goto unlock;
+
+	spin_lock_irqsave(&solo_enc->av_lock, flags);
+	if (list_empty(&solo_enc->vidq_active)) {
+		spin_unlock_irqrestore(&solo_enc->av_lock, flags);
+		goto unlock;
+	}
+	vb = list_first_entry(&solo_enc->vidq_active, struct solo_vb2_buf, list);
+	list_del(&vb->list);
+	spin_unlock_irqrestore(&solo_enc->av_lock, flags);
+
+	solo_enc_fillbuf(solo_enc, &vb->vb, enc_buf);
+unlock:
+	mutex_unlock(&solo_enc->lock);
+}
+
+void solo_enc_v4l2_isr(struct solo_dev *solo_dev)
+{
+	wake_up_interruptible_all(&solo_dev->ring_thread_wait);
+}
+
+static void solo_handle_ring(struct solo_dev *solo_dev)
+{
+	for (;;) {
+		struct solo_enc_dev *solo_enc;
+		struct solo_enc_buf enc_buf;
+		u32 mpeg_current, off;
+		u8 ch;
+		u8 cur_q;
+
+		/* Check if the hardware has any new ones in the queue */
+		cur_q = solo_reg_read(solo_dev, SOLO_VE_STATE(11)) & 0xff;
+		if (cur_q == solo_dev->enc_idx)
+			break;
+
+		mpeg_current = solo_reg_read(solo_dev,
+					SOLO_VE_MPEG4_QUE(solo_dev->enc_idx));
+		solo_dev->enc_idx = (solo_dev->enc_idx + 1) % MP4_QS;
+
+		ch = (mpeg_current >> 24) & 0x1f;
+		off = mpeg_current & 0x00ffffff;
+
+		if (ch >= SOLO_MAX_CHANNELS) {
+			ch -= SOLO_MAX_CHANNELS;
+			enc_buf.type = SOLO_ENC_TYPE_EXT;
+		} else
+			enc_buf.type = SOLO_ENC_TYPE_STD;
+
+		solo_enc = solo_dev->v4l2_enc[ch];
+		if (solo_enc == NULL) {
+			dev_err(&solo_dev->pdev->dev,
+				"Got spurious packet for channel %d\n", ch);
+			continue;
+		}
+
+		/* FAIL... */
+		if (enc_get_mpeg_dma(solo_dev, solo_dev->vh_dma, off,
+				     sizeof(struct vop_header)))
+			continue;
+
+		enc_buf.vh = (struct vop_header *)solo_dev->vh_buf;
+		enc_buf.vh->mpeg_off -= SOLO_MP4E_EXT_ADDR(solo_dev);
+		enc_buf.vh->jpeg_off -= SOLO_JPEG_EXT_ADDR(solo_dev);
+
+		/* Sanity check */
+		if (enc_buf.vh->mpeg_off != off)
+			continue;
+
+		if (solo_motion_detected(solo_enc))
+			enc_buf.motion = 1;
+		else
+			enc_buf.motion = 0;
+
+		solo_enc_handle_one(solo_enc, &enc_buf);
+	}
+}
+
+static int solo_ring_thread(void *data)
+{
+	struct solo_dev *solo_dev = data;
+	DECLARE_WAITQUEUE(wait, current);
+
+	set_freezable();
+	add_wait_queue(&solo_dev->ring_thread_wait, &wait);
+
+	for (;;) {
+		long timeout = schedule_timeout_interruptible(HZ);
+		if (timeout == -ERESTARTSYS || kthread_should_stop())
+			break;
+		solo_irq_off(solo_dev, SOLO_IRQ_ENCODER);
+		solo_handle_ring(solo_dev);
+		solo_irq_on(solo_dev, SOLO_IRQ_ENCODER);
+		try_to_freeze();
+	}
+
+	remove_wait_queue(&solo_dev->ring_thread_wait, &wait);
+
+	return 0;
+}
+
+static int solo_enc_queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
+			   unsigned int *num_buffers, unsigned int *num_planes,
+			   unsigned int sizes[], void *alloc_ctxs[])
+{
+	sizes[0] = FRAME_BUF_SIZE;
+	*num_planes = 1;
+
+	if (*num_buffers < MIN_VID_BUFFERS)
+		*num_buffers = MIN_VID_BUFFERS;
+
+	return 0;
+}
+
+static void solo_enc_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct solo_enc_dev *solo_enc = vb2_get_drv_priv(vq);
+	struct solo_vb2_buf *solo_vb =
+		container_of(vb, struct solo_vb2_buf, vb);
+
+	spin_lock(&solo_enc->av_lock);
+	list_add_tail(&solo_vb->list, &solo_enc->vidq_active);
+	spin_unlock(&solo_enc->av_lock);
+}
+
+static int solo_ring_start(struct solo_dev *solo_dev)
+{
+	solo_dev->ring_thread = kthread_run(solo_ring_thread, solo_dev,
+					    SOLO6X10_NAME "_ring");
+	if (IS_ERR(solo_dev->ring_thread)) {
+		int err = PTR_ERR(solo_dev->ring_thread);
+		solo_dev->ring_thread = NULL;
+		return err;
+	}
+
+	solo_irq_on(solo_dev, SOLO_IRQ_ENCODER);
+
+	return 0;
+}
+
+static void solo_ring_stop(struct solo_dev *solo_dev)
+{
+	if (solo_dev->ring_thread) {
+		kthread_stop(solo_dev->ring_thread);
+		solo_dev->ring_thread = NULL;
+	}
+
+	solo_irq_off(solo_dev, SOLO_IRQ_ENCODER);
+}
+
+static int solo_enc_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct solo_enc_dev *solo_enc = vb2_get_drv_priv(q);
+	int ret;
+
+	ret = solo_enc_on(solo_enc);
+	if (ret)
+		return ret;
+	return solo_ring_start(solo_enc->solo_dev);
+}
+
+static int solo_enc_stop_streaming(struct vb2_queue *q)
+{
+	struct solo_enc_dev *solo_enc = vb2_get_drv_priv(q);
+
+	solo_enc_off(solo_enc);
+	INIT_LIST_HEAD(&solo_enc->vidq_active);
+	solo_ring_stop(solo_enc->solo_dev);
+	return 0;
+}
+
+static struct vb2_ops solo_enc_video_qops = {
+	.queue_setup	= solo_enc_queue_setup,
+	.buf_queue	= solo_enc_buf_queue,
+	.start_streaming = solo_enc_start_streaming,
+	.stop_streaming = solo_enc_stop_streaming,
+	.wait_prepare	= vb2_ops_wait_prepare,
+	.wait_finish	= vb2_ops_wait_finish,
+};
+
+static int solo_enc_querycap(struct file *file, void  *priv,
+			     struct v4l2_capability *cap)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+
+	strcpy(cap->driver, SOLO6X10_NAME);
+	snprintf(cap->card, sizeof(cap->card), "Softlogic 6x10 Enc %d",
+		 solo_enc->ch);
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI:%s",
+		 pci_name(solo_dev->pdev));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE |
+			V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int solo_enc_enum_input(struct file *file, void *priv,
+			       struct v4l2_input *input)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+
+	if (input->index)
+		return -EINVAL;
+
+	snprintf(input->name, sizeof(input->name), "Encoder %d",
+		 solo_enc->ch + 1);
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+	input->std = solo_enc->vfd->tvnorms;
+
+	if (!tw28_get_video_status(solo_dev, solo_enc->ch))
+		input->status = V4L2_IN_ST_NO_SIGNAL;
+
+	return 0;
+}
+
+static int solo_enc_set_input(struct file *file, void *priv,
+			      unsigned int index)
+{
+	if (index)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int solo_enc_get_input(struct file *file, void *priv,
+			      unsigned int *index)
+{
+	*index = 0;
+
+	return 0;
+}
+
+static int solo_enc_enum_fmt_cap(struct file *file, void *priv,
+				 struct v4l2_fmtdesc *f)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	int dev_type = solo_enc->solo_dev->type;
+
+	switch (f->index) {
+	case 0:
+		switch (dev_type) {
+		case SOLO_DEV_6010:
+			f->pixelformat = V4L2_PIX_FMT_MPEG4;
+			strcpy(f->description, "MPEG-4 part 2");
+			break;
+		case SOLO_DEV_6110:
+			f->pixelformat = V4L2_PIX_FMT_H264;
+			strcpy(f->description, "H.264");
+			break;
+		}
+		break;
+	case 1:
+		f->pixelformat = V4L2_PIX_FMT_MJPEG;
+		strcpy(f->description, "MJPEG");
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	f->flags = V4L2_FMT_FLAG_COMPRESSED;
+
+	return 0;
+}
+
+static inline int solo_valid_pixfmt(u32 pixfmt, int dev_type)
+{
+	return (pixfmt == V4L2_PIX_FMT_H264 && dev_type == SOLO_DEV_6110)
+		|| (pixfmt == V4L2_PIX_FMT_MPEG4 && dev_type == SOLO_DEV_6010)
+		|| pixfmt == V4L2_PIX_FMT_MJPEG ? 0 : -EINVAL;
+}
+
+static int solo_enc_try_fmt_cap(struct file *file, void *priv,
+			    struct v4l2_format *f)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	if (solo_valid_pixfmt(pix->pixelformat, solo_dev->type))
+		return -EINVAL;
+
+	if (pix->width < solo_dev->video_hsize ||
+	    pix->height < solo_dev->video_vsize << 1) {
+		/* Default to CIF 1/2 size */
+		pix->width = solo_dev->video_hsize >> 1;
+		pix->height = solo_dev->video_vsize;
+	} else {
+		/* Full frame */
+		pix->width = solo_dev->video_hsize;
+		pix->height = solo_dev->video_vsize << 1;
+	}
+
+	switch (pix->field) {
+	case V4L2_FIELD_NONE:
+	case V4L2_FIELD_INTERLACED:
+		break;
+	case V4L2_FIELD_ANY:
+	default:
+		pix->field = V4L2_FIELD_INTERLACED;
+		break;
+	}
+
+	/* Just set these */
+	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	pix->sizeimage = FRAME_BUF_SIZE;
+	pix->bytesperline = 0;
+	pix->priv = 0;
+
+	return 0;
+}
+
+static int solo_enc_set_fmt_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	int ret;
+
+	if (vb2_is_busy(&solo_enc->vidq))
+		return -EBUSY;
+
+	ret = solo_enc_try_fmt_cap(file, priv, f);
+	if (ret)
+		return ret;
+
+	if (pix->width == solo_dev->video_hsize)
+		solo_enc->mode = SOLO_ENC_MODE_D1;
+	else
+		solo_enc->mode = SOLO_ENC_MODE_CIF;
+
+	/* This does not change the encoder at all */
+	solo_enc->fmt = pix->pixelformat;
+
+	/*
+	 * More information is needed about these 'extended' types. As far
+	 * as I can tell these are basically additional video streams with
+	 * different MPEG encoding attributes that can run in parallel with
+	 * the main stream. If so, then this should be implemented as a
+	 * second video node. Abusing priv like this is certainly not the
+	 * right approach.
+	if (pix->priv)
+		solo_enc->type = SOLO_ENC_TYPE_EXT;
+	 */
+	solo_update_mode(solo_enc);
+	return 0;
+}
+
+static int solo_enc_get_fmt_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	pix->width = solo_enc->width;
+	pix->height = solo_enc->height;
+	pix->pixelformat = solo_enc->fmt;
+	pix->field = solo_enc->interlaced ? V4L2_FIELD_INTERLACED :
+		     V4L2_FIELD_NONE;
+	pix->sizeimage = FRAME_BUF_SIZE;
+	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	pix->priv = 0;
+
+	return 0;
+}
+
+static int solo_enc_g_std(struct file *file, void *priv, v4l2_std_id *i)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+
+	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC)
+		*i = V4L2_STD_NTSC_M;
+	else
+		*i = V4L2_STD_PAL;
+	return 0;
+}
+
+static int solo_enc_s_std(struct file *file, void *priv, v4l2_std_id std)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+
+	return solo_set_video_type(solo_enc->solo_dev, std & V4L2_STD_PAL);
+}
+
+static int solo_enum_framesizes(struct file *file, void *priv,
+				struct v4l2_frmsizeenum *fsize)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+
+	if (solo_valid_pixfmt(fsize->pixel_format, solo_dev->type))
+		return -EINVAL;
+
+	switch (fsize->index) {
+	case 0:
+		fsize->discrete.width = solo_dev->video_hsize >> 1;
+		fsize->discrete.height = solo_dev->video_vsize;
+		break;
+	case 1:
+		fsize->discrete.width = solo_dev->video_hsize;
+		fsize->discrete.height = solo_dev->video_vsize << 1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+
+	return 0;
+}
+
+static int solo_enum_frameintervals(struct file *file, void *priv,
+				    struct v4l2_frmivalenum *fintv)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+
+	if (solo_valid_pixfmt(fintv->pixel_format, solo_dev->type))
+		return -EINVAL;
+	if (fintv->index)
+		return -EINVAL;
+	if ((fintv->width != solo_dev->video_hsize >> 1 ||
+	     fintv->height != solo_dev->video_vsize) &&
+	    (fintv->width != solo_dev->video_hsize ||
+	     fintv->height != solo_dev->video_vsize << 1))
+		return -EINVAL;
+
+	fintv->type = V4L2_FRMIVAL_TYPE_STEPWISE;
+
+	fintv->stepwise.min.numerator = 1;
+	fintv->stepwise.min.denominator = solo_dev->fps;
+
+	fintv->stepwise.max.numerator = 15;
+	fintv->stepwise.max.denominator = solo_dev->fps;
+
+	fintv->stepwise.step.numerator = 1;
+	fintv->stepwise.step.denominator = solo_dev->fps;
+
+	return 0;
+}
+
+static int solo_g_parm(struct file *file, void *priv,
+		       struct v4l2_streamparm *sp)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct v4l2_captureparm *cp = &sp->parm.capture;
+
+	cp->capability = V4L2_CAP_TIMEPERFRAME;
+	cp->timeperframe.numerator = solo_enc->interval;
+	cp->timeperframe.denominator = solo_dev->fps;
+	cp->capturemode = 0;
+	/* XXX: Shouldn't we be able to get/set this from videobuf? */
+	cp->readbuffers = 2;
+
+	return 0;
+}
+
+static int solo_s_parm(struct file *file, void *priv,
+		       struct v4l2_streamparm *sp)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct v4l2_captureparm *cp = &sp->parm.capture;
+
+	if (vb2_is_streaming(&solo_enc->vidq))
+		return -EBUSY;
+
+	if ((cp->timeperframe.numerator == 0) ||
+	    (cp->timeperframe.denominator == 0)) {
+		/* reset framerate */
+		cp->timeperframe.numerator = 1;
+		cp->timeperframe.denominator = solo_dev->fps;
+	}
+
+	if (cp->timeperframe.denominator != solo_dev->fps)
+		cp->timeperframe.denominator = solo_dev->fps;
+
+	if (cp->timeperframe.numerator > 15)
+		cp->timeperframe.numerator = 15;
+
+	solo_enc->interval = cp->timeperframe.numerator;
+
+	cp->capability = V4L2_CAP_TIMEPERFRAME;
+	cp->readbuffers = 2;
+
+	solo_update_mode(solo_enc);
+	return 0;
+}
+
+static long solo_enc_default(struct file *file, void *fh,
+			bool valid_prio, unsigned int cmd, void *arg)
+{
+	struct solo_enc_dev *solo_enc = video_drvdata(file);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	struct solo_motion_thresholds *thresholds = arg;
+
+	switch (cmd) {
+	case SOLO_IOC_G_MOTION_THRESHOLDS:
+		*thresholds = solo_enc->motion_thresholds;
+		return 0;
+
+	case SOLO_IOC_S_MOTION_THRESHOLDS:
+		if (!valid_prio)
+			return -EBUSY;
+		solo_enc->motion_thresholds = *thresholds;
+		if (solo_enc->motion_enabled && !solo_enc->motion_global)
+			return solo_set_motion_block(solo_dev, solo_enc->ch,
+						&solo_enc->motion_thresholds);
+		return 0;
+	default:
+		return -ENOTTY;
+	}
+}
+
+static int solo_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct solo_enc_dev *solo_enc =
+		container_of(ctrl->handler, struct solo_enc_dev, hdl);
+	struct solo_dev *solo_dev = solo_enc->solo_dev;
+	int err;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+	case V4L2_CID_CONTRAST:
+	case V4L2_CID_SATURATION:
+	case V4L2_CID_HUE:
+	case V4L2_CID_SHARPNESS:
+		return tw28_set_ctrl_val(solo_dev, ctrl->id, solo_enc->ch,
+					 ctrl->val);
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+		solo_enc->gop = ctrl->val;
+		return 0;
+	case V4L2_CID_MOTION_THRESHOLD:
+		solo_enc->motion_thresh = ctrl->val;
+		if (!solo_enc->motion_global || !solo_enc->motion_enabled)
+			return 0;
+		return solo_set_motion_threshold(solo_dev, solo_enc->ch, ctrl->val);
+	case V4L2_CID_MOTION_MODE:
+		solo_enc->motion_global = ctrl->val == 1;
+		solo_enc->motion_enabled = ctrl->val > 0;
+		if (ctrl->val) {
+			if (solo_enc->motion_global)
+				solo_set_motion_threshold(solo_dev, solo_enc->ch,
+						solo_enc->motion_thresh);
+			else
+				solo_set_motion_block(solo_dev, solo_enc->ch,
+						&solo_enc->motion_thresholds);
+		}
+		solo_motion_toggle(solo_enc, ctrl->val);
+		return 0;
+	case V4L2_CID_OSD_TEXT:
+		strcpy(solo_enc->osd_text, ctrl->string);
+		err = solo_osd_print(solo_enc);
+		return err;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_file_operations solo_enc_fops = {
+	.owner			= THIS_MODULE,
+	.open			= v4l2_fh_open,
+	.release		= vb2_fop_release,
+	.read			= vb2_fop_read,
+	.poll			= vb2_fop_poll,
+	.mmap			= vb2_fop_mmap,
+	.unlocked_ioctl		= video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops solo_enc_ioctl_ops = {
+	.vidioc_querycap		= solo_enc_querycap,
+	.vidioc_s_std			= solo_enc_s_std,
+	.vidioc_g_std			= solo_enc_g_std,
+	/* Input callbacks */
+	.vidioc_enum_input		= solo_enc_enum_input,
+	.vidioc_s_input			= solo_enc_set_input,
+	.vidioc_g_input			= solo_enc_get_input,
+	/* Video capture format callbacks */
+	.vidioc_enum_fmt_vid_cap	= solo_enc_enum_fmt_cap,
+	.vidioc_try_fmt_vid_cap		= solo_enc_try_fmt_cap,
+	.vidioc_s_fmt_vid_cap		= solo_enc_set_fmt_cap,
+	.vidioc_g_fmt_vid_cap		= solo_enc_get_fmt_cap,
+	/* Streaming I/O */
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+	/* Frame size and interval */
+	.vidioc_enum_framesizes		= solo_enum_framesizes,
+	.vidioc_enum_frameintervals	= solo_enum_frameintervals,
+	/* Video capture parameters */
+	.vidioc_s_parm			= solo_s_parm,
+	.vidioc_g_parm			= solo_g_parm,
+	/* Logging and events */
+	.vidioc_log_status		= v4l2_ctrl_log_status,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+	.vidioc_default			= solo_enc_default,
+};
+
+static const struct video_device solo_enc_template = {
+	.name			= SOLO6X10_NAME,
+	.fops			= &solo_enc_fops,
+	.ioctl_ops		= &solo_enc_ioctl_ops,
+	.minor			= -1,
+	.release		= video_device_release,
+	.tvnorms		= V4L2_STD_NTSC_M | V4L2_STD_PAL,
+};
+
+static const struct v4l2_ctrl_ops solo_ctrl_ops = {
+	.s_ctrl = solo_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config solo_motion_threshold_ctrl = {
+	.ops = &solo_ctrl_ops,
+	.id = V4L2_CID_MOTION_THRESHOLD,
+	.name = "Motion Detection Threshold",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.max = 0xffff,
+	.def = SOLO_DEF_MOT_THRESH,
+	.step = 1,
+	.flags = V4L2_CTRL_FLAG_SLIDER,
+};
+
+static const char * const solo_motion_mode_menu[] = {
+	"Disabled",
+	"Global Threshold",
+	"Regional Threshold",
+	NULL
+};
+
+static const struct v4l2_ctrl_config solo_motion_enable_ctrl = {
+	.ops = &solo_ctrl_ops,
+	.id = V4L2_CID_MOTION_MODE,
+	.name = "Motion Detection Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.qmenu = solo_motion_mode_menu,
+	.max = 2,
+};
+
+static const struct v4l2_ctrl_config solo_osd_text_ctrl = {
+	.ops = &solo_ctrl_ops,
+	.id = V4L2_CID_OSD_TEXT,
+	.name = "OSD Text",
+	.type = V4L2_CTRL_TYPE_STRING,
+	.max = OSD_TEXT_MAX,
+	.step = 1,
+};
+
+static struct solo_enc_dev *solo_enc_alloc(struct solo_dev *solo_dev,
+					   u8 ch, unsigned nr)
+{
+	struct solo_enc_dev *solo_enc;
+	struct v4l2_ctrl_handler *hdl;
+	int ret;
+	int x, y;
+
+	solo_enc = kzalloc(sizeof(*solo_enc), GFP_KERNEL);
+	if (!solo_enc)
+		return ERR_PTR(-ENOMEM);
+
+	hdl = &solo_enc->hdl;
+	v4l2_ctrl_handler_init(hdl, 10);
+	v4l2_ctrl_new_std(hdl, &solo_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &solo_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &solo_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &solo_ctrl_ops,
+			V4L2_CID_HUE, 0, 255, 1, 128);
+	if (tw28_has_sharpness(solo_dev, ch))
+		v4l2_ctrl_new_std(hdl, &solo_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0, 15, 1, 0);
+	v4l2_ctrl_new_std(hdl, &solo_ctrl_ops,
+			V4L2_CID_MPEG_VIDEO_GOP_SIZE, 1, 255, 1, solo_dev->fps);
+	v4l2_ctrl_new_custom(hdl, &solo_motion_threshold_ctrl, NULL);
+	v4l2_ctrl_new_custom(hdl, &solo_motion_enable_ctrl, NULL);
+	v4l2_ctrl_new_custom(hdl, &solo_osd_text_ctrl, NULL);
+	if (hdl->error) {
+		ret = hdl->error;
+		goto hdl_free;
+	}
+
+	solo_enc->solo_dev = solo_dev;
+	solo_enc->ch = ch;
+	mutex_init(&solo_enc->lock);
+	spin_lock_init(&solo_enc->av_lock);
+	INIT_LIST_HEAD(&solo_enc->vidq_active);
+	solo_enc->fmt = (solo_dev->type == SOLO_DEV_6010) ?
+		V4L2_PIX_FMT_MPEG4 : V4L2_PIX_FMT_H264;
+	solo_enc->type = SOLO_ENC_TYPE_STD;
+
+	solo_enc->qp = SOLO_DEFAULT_QP;
+	solo_enc->gop = solo_dev->fps;
+	solo_enc->interval = 1;
+	solo_enc->mode = SOLO_ENC_MODE_CIF;
+	solo_enc->motion_global = true;
+	solo_enc->motion_thresh = SOLO_DEF_MOT_THRESH;
+	for (y = 0; y < SOLO_MOTION_SZ; y++)
+		for (x = 0; x < SOLO_MOTION_SZ; x++)
+			solo_enc->motion_thresholds.thresholds[y][x] =
+							SOLO_DEF_MOT_THRESH;
+
+	solo_enc->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	solo_enc->vidq.io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
+	solo_enc->vidq.ops = &solo_enc_video_qops;
+	solo_enc->vidq.mem_ops = &vb2_dma_sg_memops;
+	solo_enc->vidq.drv_priv = solo_enc;
+	solo_enc->vidq.gfp_flags = __GFP_DMA32;
+	solo_enc->vidq.timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	solo_enc->vidq.buf_struct_size = sizeof(struct solo_vb2_buf);
+	solo_enc->vidq.lock = &solo_enc->lock;
+	ret = vb2_queue_init(&solo_enc->vidq);
+	if (ret)
+		goto hdl_free;
+	solo_update_mode(solo_enc);
+
+	spin_lock_init(&solo_enc->motion_lock);
+
+	/* Initialize this per encoder */
+	solo_enc->jpeg_len = sizeof(jpeg_header);
+	memcpy(solo_enc->jpeg_header, jpeg_header, solo_enc->jpeg_len);
+
+	solo_enc->desc_nelts = 32;
+	solo_enc->desc_items = pci_alloc_consistent(solo_dev->pdev,
+				      sizeof(struct solo_p2m_desc) *
+				      solo_enc->desc_nelts, &solo_enc->desc_dma);
+	ret = -ENOMEM;
+	if (solo_enc->desc_items == NULL)
+		goto hdl_free;
+
+	solo_enc->vfd = video_device_alloc();
+	if (!solo_enc->vfd)
+		goto pci_free;
+
+	*solo_enc->vfd = solo_enc_template;
+	solo_enc->vfd->v4l2_dev = &solo_dev->v4l2_dev;
+	solo_enc->vfd->ctrl_handler = hdl;
+	solo_enc->vfd->queue = &solo_enc->vidq;
+	solo_enc->vfd->lock = &solo_enc->lock;
+	set_bit(V4L2_FL_USE_FH_PRIO, &solo_enc->vfd->flags);
+	video_set_drvdata(solo_enc->vfd, solo_enc);
+	ret = video_register_device(solo_enc->vfd, VFL_TYPE_GRABBER, nr);
+	if (ret < 0)
+		goto vdev_release;
+
+	snprintf(solo_enc->vfd->name, sizeof(solo_enc->vfd->name),
+		 "%s-enc (%i/%i)", SOLO6X10_NAME, solo_dev->vfd->num,
+		 solo_enc->vfd->num);
+
+	return solo_enc;
+
+vdev_release:
+	video_device_release(solo_enc->vfd);
+pci_free:
+	pci_free_consistent(solo_enc->solo_dev->pdev,
+			sizeof(struct solo_p2m_desc) * solo_enc->desc_nelts,
+			solo_enc->desc_items, solo_enc->desc_dma);
+hdl_free:
+	v4l2_ctrl_handler_free(hdl);
+	kfree(solo_enc);
+	return ERR_PTR(ret);
+}
+
+static void solo_enc_free(struct solo_enc_dev *solo_enc)
+{
+	if (solo_enc == NULL)
+		return;
+
+	video_unregister_device(solo_enc->vfd);
+	v4l2_ctrl_handler_free(&solo_enc->hdl);
+	kfree(solo_enc);
+}
+
+int solo_enc_v4l2_init(struct solo_dev *solo_dev, unsigned nr)
+{
+	int i;
+
+	init_waitqueue_head(&solo_dev->ring_thread_wait);
+
+	solo_dev->vh_size = sizeof(struct vop_header);
+	solo_dev->vh_buf = pci_alloc_consistent(solo_dev->pdev,
+						solo_dev->vh_size,
+						&solo_dev->vh_dma);
+	if (solo_dev->vh_buf == NULL)
+		return -ENOMEM;
+
+	for (i = 0; i < solo_dev->nr_chans; i++) {
+		solo_dev->v4l2_enc[i] = solo_enc_alloc(solo_dev, i, nr);
+		if (IS_ERR(solo_dev->v4l2_enc[i]))
+			break;
+	}
+
+	if (i != solo_dev->nr_chans) {
+		int ret = PTR_ERR(solo_dev->v4l2_enc[i]);
+		while (i--)
+			solo_enc_free(solo_dev->v4l2_enc[i]);
+		pci_free_consistent(solo_dev->pdev, solo_dev->vh_size,
+				    solo_dev->vh_buf, solo_dev->vh_dma);
+		solo_dev->vh_buf = NULL;
+		return ret;
+	}
+
+	if (solo_dev->type == SOLO_DEV_6010)
+		solo_dev->enc_bw_remain = solo_dev->fps * 4 * 4;
+	else
+		solo_dev->enc_bw_remain = solo_dev->fps * 4 * 5;
+
+	dev_info(&solo_dev->pdev->dev, "Encoders as /dev/video%d-%d\n",
+		 solo_dev->v4l2_enc[0]->vfd->num,
+		 solo_dev->v4l2_enc[solo_dev->nr_chans - 1]->vfd->num);
+
+	return 0;
+}
+
+void solo_enc_v4l2_exit(struct solo_dev *solo_dev)
+{
+	int i;
+
+	for (i = 0; i < solo_dev->nr_chans; i++)
+		solo_enc_free(solo_dev->v4l2_enc[i]);
+
+	if (solo_dev->vh_buf)
+		pci_free_consistent(solo_dev->pdev, solo_dev->vh_size,
+			    solo_dev->vh_buf, solo_dev->vh_dma);
+}
diff --git a/drivers/staging/media/solo6x10/solo6x10-v4l2.c b/drivers/staging/media/solo6x10/solo6x10-v4l2.c
new file mode 100644
index 000000000000..7b26de3488da
--- /dev/null
+++ b/drivers/staging/media/solo6x10/solo6x10-v4l2.c
@@ -0,0 +1,734 @@
+/*
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
+ *
+ * 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/kernel.h>
+#include <linux/module.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "solo6x10.h"
+#include "solo6x10-tw28.h"
+
+/* Image size is two fields, SOLO_HW_BPL is one horizontal line in hardware */
+#define SOLO_HW_BPL		2048
+#define solo_vlines(__solo)	(__solo->video_vsize * 2)
+#define solo_image_size(__solo) (solo_bytesperline(__solo) * \
+				 solo_vlines(__solo))
+#define solo_bytesperline(__solo) (__solo->video_hsize * 2)
+
+#define MIN_VID_BUFFERS		2
+
+static inline void erase_on(struct solo_dev *solo_dev)
+{
+	solo_reg_write(solo_dev, SOLO_VO_DISP_ERASE, SOLO_VO_DISP_ERASE_ON);
+	solo_dev->erasing = 1;
+	solo_dev->frame_blank = 0;
+}
+
+static inline int erase_off(struct solo_dev *solo_dev)
+{
+	if (!solo_dev->erasing)
+		return 0;
+
+	/* First time around, assert erase off */
+	if (!solo_dev->frame_blank)
+		solo_reg_write(solo_dev, SOLO_VO_DISP_ERASE, 0);
+	/* Keep the erasing flag on for 8 frames minimum */
+	if (solo_dev->frame_blank++ >= 8)
+		solo_dev->erasing = 0;
+
+	return 1;
+}
+
+void solo_video_in_isr(struct solo_dev *solo_dev)
+{
+	wake_up_interruptible_all(&solo_dev->disp_thread_wait);
+}
+
+static void solo_win_setup(struct solo_dev *solo_dev, u8 ch,
+			   int sx, int sy, int ex, int ey, int scale)
+{
+	if (ch >= solo_dev->nr_chans)
+		return;
+
+	/* Here, we just keep window/channel the same */
+	solo_reg_write(solo_dev, SOLO_VI_WIN_CTRL0(ch),
+		       SOLO_VI_WIN_CHANNEL(ch) |
+		       SOLO_VI_WIN_SX(sx) |
+		       SOLO_VI_WIN_EX(ex) |
+		       SOLO_VI_WIN_SCALE(scale));
+
+	solo_reg_write(solo_dev, SOLO_VI_WIN_CTRL1(ch),
+		       SOLO_VI_WIN_SY(sy) |
+		       SOLO_VI_WIN_EY(ey));
+}
+
+static int solo_v4l2_ch_ext_4up(struct solo_dev *solo_dev, u8 idx, int on)
+{
+	u8 ch = idx * 4;
+
+	if (ch >= solo_dev->nr_chans)
+		return -EINVAL;
+
+	if (!on) {
+		u8 i;
+		for (i = ch; i < ch + 4; i++)
+			solo_win_setup(solo_dev, i, solo_dev->video_hsize,
+				       solo_vlines(solo_dev),
+				       solo_dev->video_hsize,
+				       solo_vlines(solo_dev), 0);
+		return 0;
+	}
+
+	/* Row 1 */
+	solo_win_setup(solo_dev, ch, 0, 0, solo_dev->video_hsize / 2,
+		       solo_vlines(solo_dev) / 2, 3);
+	solo_win_setup(solo_dev, ch + 1, solo_dev->video_hsize / 2, 0,
+		       solo_dev->video_hsize, solo_vlines(solo_dev) / 2, 3);
+	/* Row 2 */
+	solo_win_setup(solo_dev, ch + 2, 0, solo_vlines(solo_dev) / 2,
+		       solo_dev->video_hsize / 2, solo_vlines(solo_dev), 3);
+	solo_win_setup(solo_dev, ch + 3, solo_dev->video_hsize / 2,
+		       solo_vlines(solo_dev) / 2, solo_dev->video_hsize,
+		       solo_vlines(solo_dev), 3);
+
+	return 0;
+}
+
+static int solo_v4l2_ch_ext_16up(struct solo_dev *solo_dev, int on)
+{
+	int sy, ysize, hsize, i;
+
+	if (!on) {
+		for (i = 0; i < 16; i++)
+			solo_win_setup(solo_dev, i, solo_dev->video_hsize,
+				       solo_vlines(solo_dev),
+				       solo_dev->video_hsize,
+				       solo_vlines(solo_dev), 0);
+		return 0;
+	}
+
+	ysize = solo_vlines(solo_dev) / 4;
+	hsize = solo_dev->video_hsize / 4;
+
+	for (sy = 0, i = 0; i < 4; i++, sy += ysize) {
+		solo_win_setup(solo_dev, i * 4, 0, sy, hsize,
+			       sy + ysize, 5);
+		solo_win_setup(solo_dev, (i * 4) + 1, hsize, sy,
+			       hsize * 2, sy + ysize, 5);
+		solo_win_setup(solo_dev, (i * 4) + 2, hsize * 2, sy,
+			       hsize * 3, sy + ysize, 5);
+		solo_win_setup(solo_dev, (i * 4) + 3, hsize * 3, sy,
+			       solo_dev->video_hsize, sy + ysize, 5);
+	}
+
+	return 0;
+}
+
+static int solo_v4l2_ch(struct solo_dev *solo_dev, u8 ch, int on)
+{
+	u8 ext_ch;
+
+	if (ch < solo_dev->nr_chans) {
+		solo_win_setup(solo_dev, ch, on ? 0 : solo_dev->video_hsize,
+			       on ? 0 : solo_vlines(solo_dev),
+			       solo_dev->video_hsize, solo_vlines(solo_dev),
+			       on ? 1 : 0);
+		return 0;
+	}
+
+	if (ch >= solo_dev->nr_chans + solo_dev->nr_ext)
+		return -EINVAL;
+
+	ext_ch = ch - solo_dev->nr_chans;
+
+	/* 4up's first */
+	if (ext_ch < 4)
+		return solo_v4l2_ch_ext_4up(solo_dev, ext_ch, on);
+
+	/* Remaining case is 16up for 16-port */
+	return solo_v4l2_ch_ext_16up(solo_dev, on);
+}
+
+static int solo_v4l2_set_ch(struct solo_dev *solo_dev, u8 ch)
+{
+	if (ch >= solo_dev->nr_chans + solo_dev->nr_ext)
+		return -EINVAL;
+
+	erase_on(solo_dev);
+
+	solo_v4l2_ch(solo_dev, solo_dev->cur_disp_ch, 0);
+	solo_v4l2_ch(solo_dev, ch, 1);
+
+	solo_dev->cur_disp_ch = ch;
+
+	return 0;
+}
+
+static void solo_fillbuf(struct solo_dev *solo_dev,
+			 struct vb2_buffer *vb)
+{
+	dma_addr_t vbuf;
+	unsigned int fdma_addr;
+	int error = -1;
+	int i;
+
+	vbuf = vb2_dma_contig_plane_dma_addr(vb, 0);
+	if (!vbuf)
+		goto finish_buf;
+
+	if (erase_off(solo_dev)) {
+		void *p = vb2_plane_vaddr(vb, 0);
+		int image_size = solo_image_size(solo_dev);
+		for (i = 0; i < image_size; i += 2) {
+			((u8 *)p)[i] = 0x80;
+			((u8 *)p)[i + 1] = 0x00;
+		}
+		error = 0;
+	} else {
+		fdma_addr = SOLO_DISP_EXT_ADDR + (solo_dev->old_write *
+				(SOLO_HW_BPL * solo_vlines(solo_dev)));
+
+		error = solo_p2m_dma_t(solo_dev, 0, vbuf, fdma_addr,
+				       solo_bytesperline(solo_dev),
+				       solo_vlines(solo_dev), SOLO_HW_BPL);
+	}
+
+finish_buf:
+	if (!error) {
+		vb2_set_plane_payload(vb, 0,
+			solo_vlines(solo_dev) * solo_bytesperline(solo_dev));
+		vb->v4l2_buf.sequence = solo_dev->sequence++;
+		v4l2_get_timestamp(&vb->v4l2_buf.timestamp);
+	}
+
+	vb2_buffer_done(vb, error ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+}
+
+static void solo_thread_try(struct solo_dev *solo_dev)
+{
+	struct solo_vb2_buf *vb;
+
+	/* Only "break" from this loop if slock is held, otherwise
+	 * just return. */
+	for (;;) {
+		unsigned int cur_write;
+
+		cur_write = SOLO_VI_STATUS0_PAGE(
+			solo_reg_read(solo_dev, SOLO_VI_STATUS0));
+		if (cur_write == solo_dev->old_write)
+			return;
+
+		spin_lock(&solo_dev->slock);
+
+		if (list_empty(&solo_dev->vidq_active))
+			break;
+
+		vb = list_first_entry(&solo_dev->vidq_active, struct solo_vb2_buf,
+				      list);
+
+		solo_dev->old_write = cur_write;
+		list_del(&vb->list);
+
+		spin_unlock(&solo_dev->slock);
+
+		solo_fillbuf(solo_dev, &vb->vb);
+	}
+
+	assert_spin_locked(&solo_dev->slock);
+	spin_unlock(&solo_dev->slock);
+}
+
+static int solo_thread(void *data)
+{
+	struct solo_dev *solo_dev = data;
+	DECLARE_WAITQUEUE(wait, current);
+
+	set_freezable();
+	add_wait_queue(&solo_dev->disp_thread_wait, &wait);
+
+	for (;;) {
+		long timeout = schedule_timeout_interruptible(HZ);
+		if (timeout == -ERESTARTSYS || kthread_should_stop())
+			break;
+		solo_thread_try(solo_dev);
+		try_to_freeze();
+	}
+
+	remove_wait_queue(&solo_dev->disp_thread_wait, &wait);
+
+	return 0;
+}
+
+static int solo_start_thread(struct solo_dev *solo_dev)
+{
+	int ret = 0;
+
+	solo_dev->kthread = kthread_run(solo_thread, solo_dev, SOLO6X10_NAME "_disp");
+
+	if (IS_ERR(solo_dev->kthread)) {
+		ret = PTR_ERR(solo_dev->kthread);
+		solo_dev->kthread = NULL;
+		return ret;
+	}
+	solo_irq_on(solo_dev, SOLO_IRQ_VIDEO_IN);
+
+	return ret;
+}
+
+static void solo_stop_thread(struct solo_dev *solo_dev)
+{
+	if (!solo_dev->kthread)
+		return;
+
+	solo_irq_off(solo_dev, SOLO_IRQ_VIDEO_IN);
+	kthread_stop(solo_dev->kthread);
+	solo_dev->kthread = NULL;
+}
+
+static int solo_queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
+			   unsigned int *num_buffers, unsigned int *num_planes,
+			   unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct solo_dev *solo_dev = vb2_get_drv_priv(q);
+
+	sizes[0] = solo_image_size(solo_dev);
+	alloc_ctxs[0] = solo_dev->alloc_ctx;
+	*num_planes = 1;
+
+	if (*num_buffers < MIN_VID_BUFFERS)
+		*num_buffers = MIN_VID_BUFFERS;
+
+	return 0;
+}
+
+static int solo_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct solo_dev *solo_dev = vb2_get_drv_priv(q);
+
+	solo_dev->sequence = 0;
+	return solo_start_thread(solo_dev);
+}
+
+static int solo_stop_streaming(struct vb2_queue *q)
+{
+	struct solo_dev *solo_dev = vb2_get_drv_priv(q);
+
+	solo_stop_thread(solo_dev);
+	INIT_LIST_HEAD(&solo_dev->vidq_active);
+	return 0;
+}
+
+static void solo_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct solo_dev *solo_dev = vb2_get_drv_priv(vq);
+	struct solo_vb2_buf *solo_vb =
+		container_of(vb, struct solo_vb2_buf, vb);
+
+	spin_lock(&solo_dev->slock);
+	list_add_tail(&solo_vb->list, &solo_dev->vidq_active);
+	spin_unlock(&solo_dev->slock);
+	wake_up_interruptible(&solo_dev->disp_thread_wait);
+}
+
+static const struct vb2_ops solo_video_qops = {
+	.queue_setup	= solo_queue_setup,
+	.buf_queue	= solo_buf_queue,
+	.start_streaming = solo_start_streaming,
+	.stop_streaming = solo_stop_streaming,
+	.wait_prepare	= vb2_ops_wait_prepare,
+	.wait_finish	= vb2_ops_wait_finish,
+};
+
+static int solo_querycap(struct file *file, void  *priv,
+			 struct v4l2_capability *cap)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+
+	strcpy(cap->driver, SOLO6X10_NAME);
+	strcpy(cap->card, "Softlogic 6x10");
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI:%s",
+		 pci_name(solo_dev->pdev));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE |
+			V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int solo_enum_ext_input(struct solo_dev *solo_dev,
+			       struct v4l2_input *input)
+{
+	static const char * const dispnames_1[] = { "4UP" };
+	static const char * const dispnames_2[] = { "4UP-1", "4UP-2" };
+	static const char * const dispnames_5[] = {
+		"4UP-1", "4UP-2", "4UP-3", "4UP-4", "16UP"
+	};
+	const char * const *dispnames;
+
+	if (input->index >= (solo_dev->nr_chans + solo_dev->nr_ext))
+		return -EINVAL;
+
+	if (solo_dev->nr_ext == 5)
+		dispnames = dispnames_5;
+	else if (solo_dev->nr_ext == 2)
+		dispnames = dispnames_2;
+	else
+		dispnames = dispnames_1;
+
+	snprintf(input->name, sizeof(input->name), "Multi %s",
+		 dispnames[input->index - solo_dev->nr_chans]);
+
+	return 0;
+}
+
+static int solo_enum_input(struct file *file, void *priv,
+			   struct v4l2_input *input)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+
+	if (input->index >= solo_dev->nr_chans) {
+		int ret = solo_enum_ext_input(solo_dev, input);
+		if (ret < 0)
+			return ret;
+	} else {
+		snprintf(input->name, sizeof(input->name), "Camera %d",
+			 input->index + 1);
+
+		/* We can only check this for normal inputs */
+		if (!tw28_get_video_status(solo_dev, input->index))
+			input->status = V4L2_IN_ST_NO_SIGNAL;
+	}
+
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+	input->std = solo_dev->vfd->tvnorms;
+	return 0;
+}
+
+static int solo_set_input(struct file *file, void *priv, unsigned int index)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+	int ret = solo_v4l2_set_ch(solo_dev, index);
+
+	if (!ret) {
+		while (erase_off(solo_dev))
+			/* Do nothing */;
+	}
+
+	return ret;
+}
+
+static int solo_get_input(struct file *file, void *priv, unsigned int *index)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+
+	*index = solo_dev->cur_disp_ch;
+
+	return 0;
+}
+
+static int solo_enum_fmt_cap(struct file *file, void *priv,
+			     struct v4l2_fmtdesc *f)
+{
+	if (f->index)
+		return -EINVAL;
+
+	f->pixelformat = V4L2_PIX_FMT_UYVY;
+	strlcpy(f->description, "UYUV 4:2:2 Packed", sizeof(f->description));
+
+	return 0;
+}
+
+static int solo_try_fmt_cap(struct file *file, void *priv,
+			    struct v4l2_format *f)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	int image_size = solo_image_size(solo_dev);
+
+	if (pix->pixelformat != V4L2_PIX_FMT_UYVY)
+		return -EINVAL;
+
+	pix->width = solo_dev->video_hsize;
+	pix->height = solo_vlines(solo_dev);
+	pix->sizeimage = image_size;
+	pix->field = V4L2_FIELD_INTERLACED;
+	pix->pixelformat = V4L2_PIX_FMT_UYVY;
+	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	pix->priv = 0;
+	return 0;
+}
+
+static int solo_set_fmt_cap(struct file *file, void *priv,
+			    struct v4l2_format *f)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+
+	if (vb2_is_busy(&solo_dev->vidq))
+		return -EBUSY;
+
+	/* For right now, if it doesn't match our running config,
+	 * then fail */
+	return solo_try_fmt_cap(file, priv, f);
+}
+
+static int solo_get_fmt_cap(struct file *file, void *priv,
+			    struct v4l2_format *f)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	pix->width = solo_dev->video_hsize;
+	pix->height = solo_vlines(solo_dev);
+	pix->pixelformat = V4L2_PIX_FMT_UYVY;
+	pix->field = V4L2_FIELD_INTERLACED;
+	pix->sizeimage = solo_image_size(solo_dev);
+	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	pix->bytesperline = solo_bytesperline(solo_dev);
+	pix->priv = 0;
+
+	return 0;
+}
+
+static int solo_g_std(struct file *file, void *priv, v4l2_std_id *i)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+
+	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC)
+		*i = V4L2_STD_NTSC_M;
+	else
+		*i = V4L2_STD_PAL;
+	return 0;
+}
+
+int solo_set_video_type(struct solo_dev *solo_dev, bool type)
+{
+	int i;
+
+	/* Make sure all video nodes are idle */
+	if (vb2_is_busy(&solo_dev->vidq))
+		return -EBUSY;
+	for (i = 0; i < solo_dev->nr_chans; i++)
+		if (vb2_is_busy(&solo_dev->v4l2_enc[i]->vidq))
+			return -EBUSY;
+	solo_dev->video_type = type;
+	/* Reconfigure for the new standard */
+	solo_disp_init(solo_dev);
+	solo_enc_init(solo_dev);
+	solo_tw28_init(solo_dev);
+	for (i = 0; i < solo_dev->nr_chans; i++)
+		solo_update_mode(solo_dev->v4l2_enc[i]);
+	return solo_v4l2_set_ch(solo_dev, solo_dev->cur_disp_ch);
+}
+
+static int solo_s_std(struct file *file, void *priv, v4l2_std_id std)
+{
+	struct solo_dev *solo_dev = video_drvdata(file);
+
+	return solo_set_video_type(solo_dev, std & V4L2_STD_PAL);
+}
+
+static int solo_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct solo_dev *solo_dev =
+		container_of(ctrl->handler, struct solo_dev, disp_hdl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_MOTION_TRACE:
+		if (ctrl->val) {
+			solo_reg_write(solo_dev, SOLO_VI_MOTION_BORDER,
+					SOLO_VI_MOTION_Y_ADD |
+					SOLO_VI_MOTION_Y_VALUE(0x20) |
+					SOLO_VI_MOTION_CB_VALUE(0x10) |
+					SOLO_VI_MOTION_CR_VALUE(0x10));
+			solo_reg_write(solo_dev, SOLO_VI_MOTION_BAR,
+					SOLO_VI_MOTION_CR_ADD |
+					SOLO_VI_MOTION_Y_VALUE(0x10) |
+					SOLO_VI_MOTION_CB_VALUE(0x80) |
+					SOLO_VI_MOTION_CR_VALUE(0x10));
+		} else {
+			solo_reg_write(solo_dev, SOLO_VI_MOTION_BORDER, 0);
+			solo_reg_write(solo_dev, SOLO_VI_MOTION_BAR, 0);
+		}
+		return 0;
+	default:
+		break;
+	}
+	return -EINVAL;
+}
+
+static const struct v4l2_file_operations solo_v4l2_fops = {
+	.owner			= THIS_MODULE,
+	.open			= v4l2_fh_open,
+	.release		= vb2_fop_release,
+	.read			= vb2_fop_read,
+	.poll			= vb2_fop_poll,
+	.mmap			= vb2_fop_mmap,
+	.unlocked_ioctl		= video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops solo_v4l2_ioctl_ops = {
+	.vidioc_querycap		= solo_querycap,
+	.vidioc_s_std			= solo_s_std,
+	.vidioc_g_std			= solo_g_std,
+	/* Input callbacks */
+	.vidioc_enum_input		= solo_enum_input,
+	.vidioc_s_input			= solo_set_input,
+	.vidioc_g_input			= solo_get_input,
+	/* Video capture format callbacks */
+	.vidioc_enum_fmt_vid_cap	= solo_enum_fmt_cap,
+	.vidioc_try_fmt_vid_cap		= solo_try_fmt_cap,
+	.vidioc_s_fmt_vid_cap		= solo_set_fmt_cap,
+	.vidioc_g_fmt_vid_cap		= solo_get_fmt_cap,
+	/* Streaming I/O */
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+	/* Logging and events */
+	.vidioc_log_status		= v4l2_ctrl_log_status,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+};
+
+static struct video_device solo_v4l2_template = {
+	.name			= SOLO6X10_NAME,
+	.fops			= &solo_v4l2_fops,
+	.ioctl_ops		= &solo_v4l2_ioctl_ops,
+	.minor			= -1,
+	.release		= video_device_release,
+	.tvnorms		= V4L2_STD_NTSC_M | V4L2_STD_PAL,
+};
+
+static const struct v4l2_ctrl_ops solo_ctrl_ops = {
+	.s_ctrl = solo_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config solo_motion_trace_ctrl = {
+	.ops = &solo_ctrl_ops,
+	.id = V4L2_CID_MOTION_TRACE,
+	.name = "Motion Detection Trace",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+int solo_v4l2_init(struct solo_dev *solo_dev, unsigned nr)
+{
+	int ret;
+	int i;
+
+	init_waitqueue_head(&solo_dev->disp_thread_wait);
+	spin_lock_init(&solo_dev->slock);
+	mutex_init(&solo_dev->lock);
+	INIT_LIST_HEAD(&solo_dev->vidq_active);
+
+	solo_dev->vfd = video_device_alloc();
+	if (!solo_dev->vfd)
+		return -ENOMEM;
+
+	*solo_dev->vfd = solo_v4l2_template;
+	solo_dev->vfd->v4l2_dev = &solo_dev->v4l2_dev;
+	solo_dev->vfd->queue = &solo_dev->vidq;
+	solo_dev->vfd->lock = &solo_dev->lock;
+	v4l2_ctrl_handler_init(&solo_dev->disp_hdl, 1);
+	v4l2_ctrl_new_custom(&solo_dev->disp_hdl, &solo_motion_trace_ctrl, NULL);
+	if (solo_dev->disp_hdl.error) {
+		ret = solo_dev->disp_hdl.error;
+		goto fail;
+	}
+	solo_dev->vfd->ctrl_handler = &solo_dev->disp_hdl;
+	set_bit(V4L2_FL_USE_FH_PRIO, &solo_dev->vfd->flags);
+
+	video_set_drvdata(solo_dev->vfd, solo_dev);
+
+	solo_dev->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	solo_dev->vidq.io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
+	solo_dev->vidq.ops = &solo_video_qops;
+	solo_dev->vidq.mem_ops = &vb2_dma_contig_memops;
+	solo_dev->vidq.drv_priv = solo_dev;
+	solo_dev->vidq.timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	solo_dev->vidq.gfp_flags = __GFP_DMA32;
+	solo_dev->vidq.buf_struct_size = sizeof(struct solo_vb2_buf);
+	solo_dev->vidq.lock = &solo_dev->lock;
+	ret = vb2_queue_init(&solo_dev->vidq);
+	if (ret < 0)
+		goto fail;
+
+	solo_dev->alloc_ctx = vb2_dma_contig_init_ctx(&solo_dev->pdev->dev);
+	if (IS_ERR(solo_dev->alloc_ctx)) {
+		dev_err(&solo_dev->pdev->dev, "Can't allocate buffer context");
+		return PTR_ERR(solo_dev->alloc_ctx);
+	}
+
+	/* Cycle all the channels and clear */
+	for (i = 0; i < solo_dev->nr_chans; i++) {
+		solo_v4l2_set_ch(solo_dev, i);
+		while (erase_off(solo_dev))
+			/* Do nothing */;
+	}
+
+	/* Set the default display channel */
+	solo_v4l2_set_ch(solo_dev, 0);
+	while (erase_off(solo_dev))
+		/* Do nothing */;
+
+	ret = video_register_device(solo_dev->vfd, VFL_TYPE_GRABBER, nr);
+	if (ret < 0)
+		goto fail;
+
+	snprintf(solo_dev->vfd->name, sizeof(solo_dev->vfd->name), "%s (%i)",
+		 SOLO6X10_NAME, solo_dev->vfd->num);
+
+	dev_info(&solo_dev->pdev->dev, "Display as /dev/video%d with "
+		 "%d inputs (%d extended)\n", solo_dev->vfd->num,
+		 solo_dev->nr_chans, solo_dev->nr_ext);
+
+	return 0;
+
+fail:
+	video_device_release(solo_dev->vfd);
+	vb2_dma_contig_cleanup_ctx(solo_dev->alloc_ctx);
+	v4l2_ctrl_handler_free(&solo_dev->disp_hdl);
+	solo_dev->vfd = NULL;
+	return ret;
+}
+
+void solo_v4l2_exit(struct solo_dev *solo_dev)
+{
+	if (solo_dev->vfd == NULL)
+		return;
+
+	video_unregister_device(solo_dev->vfd);
+	vb2_dma_contig_cleanup_ctx(solo_dev->alloc_ctx);
+	v4l2_ctrl_handler_free(&solo_dev->disp_hdl);
+	solo_dev->vfd = NULL;
+}
diff --git a/drivers/staging/media/solo6x10/solo6x10.h b/drivers/staging/media/solo6x10/solo6x10.h
index abee7213202f..6f91d2e34b2a 100644
--- a/drivers/staging/media/solo6x10/solo6x10.h
+++ b/drivers/staging/media/solo6x10/solo6x10.h
@@ -1,6 +1,11 @@
 /*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
+ * Copyright (C) 2010-2013 Bluecherry, LLC <http://www.bluecherrydvr.com>
+ *
+ * Original author:
+ * Ben Collins <bcollins@ubuntu.com>
+ *
+ * Additional work by:
+ * John Brooks <john.brooks@bluecherry.net>
  *
  * 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
@@ -20,21 +25,23 @@
 #ifndef __SOLO6X10_H
 #define __SOLO6X10_H
 
-#include <linux/version.h>
 #include <linux/pci.h>
 #include <linux/i2c.h>
-#include <linux/semaphore.h>
 #include <linux/mutex.h>
 #include <linux/list.h>
 #include <linux/wait.h>
-#include <linux/delay.h>
-#include <linux/slab.h>
-#include <asm/io.h>
+#include <linux/stringify.h>
+#include <linux/io.h>
 #include <linux/atomic.h>
+#include <linux/slab.h>
 #include <linux/videodev2.h>
+
 #include <media/v4l2-dev.h>
-#include <media/videobuf-core.h>
-#include "registers.h"
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/videobuf2-core.h>
+
+#include "solo6x10-regs.h"
 
 #ifndef PCI_VENDOR_ID_SOFTLOGIC
 #define PCI_VENDOR_ID_SOFTLOGIC		0x9413
@@ -58,19 +65,15 @@
 #define PCI_DEVICE_ID_BC_6110_16	0x5310
 #endif /* Bluecherry */
 
+/* Used in pci_device_id, and solo_dev->type */
+#define SOLO_DEV_6010			0
+#define SOLO_DEV_6110			1
+
 #define SOLO6X10_NAME			"solo6x10"
 
 #define SOLO_MAX_CHANNELS		16
 
-/* Make sure these two match */
-#define SOLO6X10_VERSION		"2.1.0"
-#define SOLO6X10_VER_MAJOR		2
-#define SOLO6X10_VER_MINOR		0
-#define SOLO6X10_VER_SUB		0
-#define SOLO6X10_VER_NUM \
-	KERNEL_VERSION(SOLO6X10_VER_MAJOR, SOLO6X10_VER_MINOR, SOLO6X10_VER_SUB)
-
-#define FLAGS_6110			1
+#define SOLO6X10_VERSION		"3.0.0"
 
 /*
  * The SOLO6x10 actually has 8 i2c channels, but we only use 2.
@@ -84,16 +87,7 @@
 /* DMA Engine setup */
 #define SOLO_NR_P2M			4
 #define SOLO_NR_P2M_DESC		256
-/* MPEG and JPEG share the same interrupt and locks so they must be together
- * in the same dma channel. */
-#define SOLO_P2M_DMA_ID_MP4E		0
-#define SOLO_P2M_DMA_ID_JPEG		0
-#define SOLO_P2M_DMA_ID_MP4D		1
-#define SOLO_P2M_DMA_ID_G723D		1
-#define SOLO_P2M_DMA_ID_DISP		2
-#define SOLO_P2M_DMA_ID_OSG		2
-#define SOLO_P2M_DMA_ID_G723E		3
-#define SOLO_P2M_DMA_ID_VIN		3
+#define SOLO_P2M_DESC_SIZE		(SOLO_NR_P2M_DESC * 16)
 
 /* Encoder standard modes */
 #define SOLO_ENC_MODE_CIF		2
@@ -103,23 +97,37 @@
 #define SOLO_DEFAULT_GOP		30
 #define SOLO_DEFAULT_QP			3
 
-/* There is 8MB memory available for solo to buffer MPEG4 frames.
- * This gives us 512 * 16kbyte queues. */
-#define SOLO_NR_RING_BUFS		512
-
-#define SOLO_CLOCK_MHZ			108
-
 #ifndef V4L2_BUF_FLAG_MOTION_ON
-#define V4L2_BUF_FLAG_MOTION_ON		0x0400
-#define V4L2_BUF_FLAG_MOTION_DETECTED	0x0800
-#endif
-#ifndef V4L2_CID_MOTION_ENABLE
-#define PRIVATE_CIDS
-#define V4L2_CID_MOTION_ENABLE		(V4L2_CID_PRIVATE_BASE+0)
-#define V4L2_CID_MOTION_THRESHOLD	(V4L2_CID_PRIVATE_BASE+1)
-#define V4L2_CID_MOTION_TRACE		(V4L2_CID_PRIVATE_BASE+2)
+#define V4L2_BUF_FLAG_MOTION_ON		0x10000
+#define V4L2_BUF_FLAG_MOTION_DETECTED	0x20000
 #endif
 
+#define SOLO_CID_CUSTOM_BASE		(V4L2_CID_USER_BASE | 0xf000)
+#define V4L2_CID_MOTION_MODE		(SOLO_CID_CUSTOM_BASE+0)
+#define V4L2_CID_MOTION_THRESHOLD	(SOLO_CID_CUSTOM_BASE+1)
+#define V4L2_CID_MOTION_TRACE		(SOLO_CID_CUSTOM_BASE+2)
+#define V4L2_CID_OSD_TEXT		(SOLO_CID_CUSTOM_BASE+3)
+
+/*
+ * Motion thresholds are in a table of 64x64 samples, with
+ * each sample representing 16x16 pixels of the source. In
+ * effect, 44x30 samples are used for NTSC, and 44x36 for PAL.
+ * The 5th sample on the 10th row is (10*64)+5 = 645.
+ *
+ * Using a 64x64 array will result in a problem on some architectures like
+ * the powerpc where the size of the argument is limited to 13 bits.
+ * Since both PAL and NTSC do not use the full table anyway I've chosen
+ * to limit the array to 45x45 (45*16 = 720, which is the maximum PAL/NTSC
+ * width).
+ */
+#define SOLO_MOTION_SZ (45)
+struct solo_motion_thresholds {
+	__u16	thresholds[SOLO_MOTION_SZ][SOLO_MOTION_SZ];
+};
+
+#define SOLO_IOC_G_MOTION_THRESHOLDS	_IOR('V', BASE_VIDIOC_PRIVATE+0, struct solo_motion_thresholds)
+#define SOLO_IOC_S_MOTION_THRESHOLDS	_IOW('V', BASE_VIDIOC_PRIVATE+1, struct solo_motion_thresholds)
+
 enum SOLO_I2C_STATE {
 	IIC_STATE_IDLE,
 	IIC_STATE_START,
@@ -128,20 +136,29 @@ enum SOLO_I2C_STATE {
 	IIC_STATE_STOP
 };
 
-struct p2m_desc {
-	u32 ctrl;
-	u32 ext;
-	u32 ta;
-	u32 fa;
+/* Defined in Table 4-16, Page 68-69 of the 6010 Datasheet */
+struct solo_p2m_desc {
+	u32	ctrl;
+	u32	cfg;
+	u32	dma_addr;
+	u32	ext_addr;
 };
 
 struct solo_p2m_dev {
 	struct mutex		mutex;
 	struct completion	completion;
+	int			desc_count;
+	int			desc_idx;
+	struct solo_p2m_desc	*descs;
 	int			error;
 };
 
-#define OSD_TEXT_MAX		30
+#define OSD_TEXT_MAX		44
+
+struct solo_vb2_buf {
+	struct vb2_buffer vb;
+	struct list_head list;
+};
 
 enum solo_enc_types {
 	SOLO_ENC_TYPE_STD,
@@ -149,46 +166,61 @@ enum solo_enc_types {
 };
 
 struct solo_enc_dev {
-	struct solo_dev		*solo_dev;
+	struct solo_dev	*solo_dev;
 	/* V4L2 Items */
+	struct v4l2_ctrl_handler hdl;
 	struct video_device	*vfd;
 	/* General accounting */
-	wait_queue_head_t	thread_wait;
-	spinlock_t		lock;
-	atomic_t		readers;
+	struct mutex		lock;
+	spinlock_t		motion_lock;
 	u8			ch;
 	u8			mode, gop, qp, interlaced, interval;
-	u8			reset_gop;
 	u8			bw_weight;
-	u8			motion_detected;
 	u16			motion_thresh;
+	struct solo_motion_thresholds motion_thresholds;
+	bool			motion_global;
+	bool			motion_enabled;
 	u16			width;
 	u16			height;
+
+	/* OSD buffers */
 	char			osd_text[OSD_TEXT_MAX + 1];
-};
+	u8			osd_buf[SOLO_EOSD_EXT_SIZE_MAX]
+					__aligned(4);
 
-struct solo_enc_buf {
-	u8			vop;
-	u8			ch;
+	/* VOP stuff */
+	unsigned char		vop[64];
+	int			vop_len;
+	unsigned char		jpeg_header[1024];
+	int			jpeg_len;
+
+	u32			fmt;
 	enum solo_enc_types	type;
-	u32			off;
-	u32			size;
-	u32			jpeg_off;
-	u32			jpeg_size;
-	struct timeval		ts;
+	u32			sequence;
+	struct vb2_queue	vidq;
+	struct list_head	vidq_active;
+	int			desc_count;
+	int			desc_nelts;
+	struct solo_p2m_desc	*desc_items;
+	dma_addr_t		desc_dma;
+	spinlock_t		av_lock;
 };
 
 /* The SOLO6x10 PCI Device */
 struct solo_dev {
 	/* General stuff */
 	struct pci_dev		*pdev;
+	int			type;
+	unsigned int		time_sync;
+	unsigned int		usec_lsb;
+	unsigned int		clock_mhz;
 	u8 __iomem		*reg_base;
 	int			nr_chans;
 	int			nr_ext;
-	u32			flags;
 	u32			irq_mask;
 	u32			motion_mask;
 	spinlock_t		reg_io_lock;
+	struct v4l2_device	v4l2_dev;
 
 	/* tw28xx accounting */
 	u8			tw2865, tw2864, tw2815;
@@ -206,6 +238,9 @@ struct solo_dev {
 
 	/* P2M DMA Engine */
 	struct solo_p2m_dev	p2m_dev[SOLO_NR_P2M];
+	atomic_t		p2m_count;
+	int			p2m_jiffies;
+	unsigned int		p2m_timeouts;
 
 	/* V4L2 Display items */
 	struct video_device	*vfd;
@@ -213,15 +248,13 @@ struct solo_dev {
 	unsigned int		frame_blank;
 	u8			cur_disp_ch;
 	wait_queue_head_t	disp_thread_wait;
+	struct v4l2_ctrl_handler disp_hdl;
 
 	/* V4L2 Encoder items */
 	struct solo_enc_dev	*v4l2_enc[SOLO_MAX_CHANNELS];
 	u16			enc_bw_remain;
 	/* IDX into hw mp4 encoder */
 	u8			enc_idx;
-	/* Our software ring of enc buf references */
-	u16			enc_wr_idx;
-	struct solo_enc_buf	enc_buf[SOLO_NR_RING_BUFS];
 
 	/* Current video settings */
 	u32			video_type;
@@ -230,11 +263,40 @@ struct solo_dev {
 	u16			vin_hstart, vin_vstart;
 	u8			fps;
 
+	/* JPEG Qp setting */
+	spinlock_t      jpeg_qp_lock;
+	u32		jpeg_qp[2];
+
 	/* Audio components */
 	struct snd_card		*snd_card;
 	struct snd_pcm		*snd_pcm;
 	atomic_t		snd_users;
 	int			g723_hw_idx;
+
+	/* sysfs stuffs */
+	struct device		dev;
+	int			sdram_size;
+	struct bin_attribute	sdram_attr;
+	unsigned int		sys_config;
+
+	/* Ring thread */
+	struct task_struct	*ring_thread;
+	wait_queue_head_t	ring_thread_wait;
+
+	/* VOP_HEADER handling */
+	void                    *vh_buf;
+	dma_addr_t		vh_dma;
+	int			vh_size;
+
+	/* Buffer handling */
+	struct vb2_queue	vidq;
+	struct vb2_alloc_ctx	*alloc_ctx;
+	u32			sequence;
+	struct task_struct      *kthread;
+	struct mutex		lock;
+	spinlock_t		slock;
+	int			old_write;
+	struct list_head	vidq_active;
 };
 
 static inline u32 solo_reg_read(struct solo_dev *solo_dev, int reg)
@@ -255,7 +317,8 @@ static inline u32 solo_reg_read(struct solo_dev *solo_dev, int reg)
 	return ret;
 }
 
-static inline void solo_reg_write(struct solo_dev *solo_dev, int reg, u32 data)
+static inline void solo_reg_write(struct solo_dev *solo_dev, int reg,
+				  u32 data)
 {
 	unsigned long flags;
 	u16 val;
@@ -270,10 +333,19 @@ static inline void solo_reg_write(struct solo_dev *solo_dev, int reg, u32 data)
 	spin_unlock_irqrestore(&solo_dev->reg_io_lock, flags);
 }
 
-void solo_irq_on(struct solo_dev *solo_dev, u32 mask);
-void solo_irq_off(struct solo_dev *solo_dev, u32 mask);
+static inline void solo_irq_on(struct solo_dev *dev, u32 mask)
+{
+	dev->irq_mask |= mask;
+	solo_reg_write(dev, SOLO_IRQ_MASK, dev->irq_mask);
+}
+
+static inline void solo_irq_off(struct solo_dev *dev, u32 mask)
+{
+	dev->irq_mask &= ~mask;
+	solo_reg_write(dev, SOLO_IRQ_MASK, dev->irq_mask);
+}
 
-/* Init/exit routeines for subsystems */
+/* Init/exit routines for subsystems */
 int solo_disp_init(struct solo_dev *solo_dev);
 void solo_disp_exit(struct solo_dev *solo_dev);
 
@@ -286,13 +358,13 @@ void solo_i2c_exit(struct solo_dev *solo_dev);
 int solo_p2m_init(struct solo_dev *solo_dev);
 void solo_p2m_exit(struct solo_dev *solo_dev);
 
-int solo_v4l2_init(struct solo_dev *solo_dev);
+int solo_v4l2_init(struct solo_dev *solo_dev, unsigned nr);
 void solo_v4l2_exit(struct solo_dev *solo_dev);
 
 int solo_enc_init(struct solo_dev *solo_dev);
 void solo_enc_exit(struct solo_dev *solo_dev);
 
-int solo_enc_v4l2_init(struct solo_dev *solo_dev);
+int solo_enc_v4l2_init(struct solo_dev *solo_dev, unsigned nr);
 void solo_enc_v4l2_exit(struct solo_dev *solo_dev);
 
 int solo_g723_init(struct solo_dev *solo_dev);
@@ -301,7 +373,7 @@ void solo_g723_exit(struct solo_dev *solo_dev);
 /* ISR's */
 int solo_i2c_isr(struct solo_dev *solo_dev);
 void solo_p2m_isr(struct solo_dev *solo_dev, int id);
-void solo_p2m_error_isr(struct solo_dev *solo_dev, u32 status);
+void solo_p2m_error_isr(struct solo_dev *solo_dev);
 void solo_enc_v4l2_isr(struct solo_dev *solo_dev);
 void solo_g723_isr(struct solo_dev *solo_dev);
 void solo_motion_isr(struct solo_dev *solo_dev);
@@ -313,24 +385,43 @@ void solo_i2c_writebyte(struct solo_dev *solo_dev, int id, u8 addr, u8 off,
 			u8 data);
 
 /* P2M DMA */
-int solo_p2m_dma_t(struct solo_dev *solo_dev, u8 id, int wr,
-		   dma_addr_t dma_addr, u32 ext_addr, u32 size);
-int solo_p2m_dma(struct solo_dev *solo_dev, u8 id, int wr,
-		 void *sys_addr, u32 ext_addr, u32 size);
-int solo_p2m_dma_sg(struct solo_dev *solo_dev, u8 id,
-		    struct p2m_desc *pdesc, int wr,
-		    struct scatterlist *sglist, u32 sg_off,
-		    u32 ext_addr, u32 size);
-void solo_p2m_push_desc(struct p2m_desc *desc, int wr, dma_addr_t dma_addr,
-			u32 ext_addr, u32 size, int repeat, u32 ext_size);
-int solo_p2m_dma_desc(struct solo_dev *solo_dev, u8 id,
-		      struct p2m_desc *desc, int desc_count);
+int solo_p2m_dma_t(struct solo_dev *solo_dev, int wr,
+		   dma_addr_t dma_addr, u32 ext_addr, u32 size,
+		   int repeat, u32 ext_size);
+int solo_p2m_dma(struct solo_dev *solo_dev, int wr,
+		 void *sys_addr, u32 ext_addr, u32 size,
+		 int repeat, u32 ext_size);
+void solo_p2m_fill_desc(struct solo_p2m_desc *desc, int wr,
+			dma_addr_t dma_addr, u32 ext_addr, u32 size,
+			int repeat, u32 ext_size);
+int solo_p2m_dma_desc(struct solo_dev *solo_dev,
+		      struct solo_p2m_desc *desc, dma_addr_t desc_dma,
+		      int desc_cnt);
+
+/* Global s_std ioctl */
+int solo_set_video_type(struct solo_dev *solo_dev, bool type);
+void solo_update_mode(struct solo_enc_dev *solo_enc);
 
 /* Set the threshold for motion detection */
-void solo_set_motion_threshold(struct solo_dev *solo_dev, u8 ch, u16 val);
+int solo_set_motion_threshold(struct solo_dev *solo_dev, u8 ch, u16 val);
+int solo_set_motion_block(struct solo_dev *solo_dev, u8 ch,
+		const struct solo_motion_thresholds *thresholds);
 #define SOLO_DEF_MOT_THRESH		0x0300
 
 /* Write text on OSD */
 int solo_osd_print(struct solo_enc_dev *solo_enc);
 
+/* EEPROM commands */
+unsigned int solo_eeprom_ewen(struct solo_dev *solo_dev, int w_en);
+unsigned short solo_eeprom_read(struct solo_dev *solo_dev, int loc);
+int solo_eeprom_write(struct solo_dev *solo_dev, int loc,
+		      unsigned short data);
+
+/* JPEG Qp functions */
+void solo_s_jpeg_qp(struct solo_dev *solo_dev, unsigned int ch,
+		    unsigned int qp);
+int solo_g_jpeg_qp(struct solo_dev *solo_dev, unsigned int ch);
+
+#define CHK_FLAGS(v, flags) (((v) & (flags)) == (flags))
+
 #endif /* __SOLO6X10_H */
diff --git a/drivers/staging/media/solo6x10/v4l2-enc.c b/drivers/staging/media/solo6x10/v4l2-enc.c
deleted file mode 100644
index 4977e869d5b7..000000000000
--- a/drivers/staging/media/solo6x10/v4l2-enc.c
+++ /dev/null
@@ -1,1829 +0,0 @@
-/*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
- *
- * 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/kernel.h>
-#include <linux/module.h>
-#include <linux/kthread.h>
-#include <linux/freezer.h>
-#include <media/v4l2-ioctl.h>
-#include <media/v4l2-common.h>
-#include <media/videobuf-dma-sg.h>
-#include "solo6x10.h"
-#include "tw28.h"
-#include "solo6x10-jpeg.h"
-
-#define MIN_VID_BUFFERS		4
-#define FRAME_BUF_SIZE		(128 * 1024)
-#define MP4_QS			16
-
-static int solo_enc_thread(void *data);
-
-extern unsigned video_nr;
-
-struct solo_enc_fh {
-	struct			solo_enc_dev *enc;
-	u32			fmt;
-	u16			rd_idx;
-	u8			enc_on;
-	enum solo_enc_types	type;
-	struct videobuf_queue	vidq;
-	struct list_head	vidq_active;
-	struct task_struct	*kthread;
-	struct p2m_desc		desc[SOLO_NR_P2M_DESC];
-};
-
-static const u32 solo_user_ctrls[] = {
-	V4L2_CID_BRIGHTNESS,
-	V4L2_CID_CONTRAST,
-	V4L2_CID_SATURATION,
-	V4L2_CID_HUE,
-	V4L2_CID_SHARPNESS,
-	0
-};
-
-static const u32 solo_mpeg_ctrls[] = {
-	V4L2_CID_MPEG_VIDEO_ENCODING,
-	V4L2_CID_MPEG_VIDEO_GOP_SIZE,
-	0
-};
-
-static const u32 solo_private_ctrls[] = {
-	V4L2_CID_MOTION_ENABLE,
-	V4L2_CID_MOTION_THRESHOLD,
-	0
-};
-
-static const u32 solo_fmtx_ctrls[] = {
-	V4L2_CID_RDS_TX_RADIO_TEXT,
-	0
-};
-
-static const u32 *solo_ctrl_classes[] = {
-	solo_user_ctrls,
-	solo_mpeg_ctrls,
-	solo_fmtx_ctrls,
-	solo_private_ctrls,
-	NULL
-};
-
-static int solo_is_motion_on(struct solo_enc_dev *solo_enc)
-{
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	u8 ch = solo_enc->ch;
-
-	if (solo_dev->motion_mask & (1 << ch))
-		return 1;
-	return 0;
-}
-
-static void solo_motion_toggle(struct solo_enc_dev *solo_enc, int on)
-{
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	u8 ch = solo_enc->ch;
-
-	spin_lock(&solo_enc->lock);
-
-	if (on)
-		solo_dev->motion_mask |= (1 << ch);
-	else
-		solo_dev->motion_mask &= ~(1 << ch);
-
-	/* Do this regardless of if we are turning on or off */
-	solo_reg_write(solo_enc->solo_dev, SOLO_VI_MOT_CLEAR,
-		       1 << solo_enc->ch);
-	solo_enc->motion_detected = 0;
-
-	solo_reg_write(solo_dev, SOLO_VI_MOT_ADR,
-		       SOLO_VI_MOTION_EN(solo_dev->motion_mask) |
-		       (SOLO_MOTION_EXT_ADDR(solo_dev) >> 16));
-
-	if (solo_dev->motion_mask)
-		solo_irq_on(solo_dev, SOLO_IRQ_MOTION);
-	else
-		solo_irq_off(solo_dev, SOLO_IRQ_MOTION);
-
-	spin_unlock(&solo_enc->lock);
-}
-
-/* Should be called with solo_enc->lock held */
-static void solo_update_mode(struct solo_enc_dev *solo_enc)
-{
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-	assert_spin_locked(&solo_enc->lock);
-
-	solo_enc->interlaced = (solo_enc->mode & 0x08) ? 1 : 0;
-	solo_enc->bw_weight = max(solo_dev->fps / solo_enc->interval, 1);
-
-	switch (solo_enc->mode) {
-	case SOLO_ENC_MODE_CIF:
-		solo_enc->width = solo_dev->video_hsize >> 1;
-		solo_enc->height = solo_dev->video_vsize;
-		break;
-	case SOLO_ENC_MODE_D1:
-		solo_enc->width = solo_dev->video_hsize;
-		solo_enc->height = solo_dev->video_vsize << 1;
-		solo_enc->bw_weight <<= 2;
-		break;
-	default:
-		WARN(1, "mode is unknown\n");
-	}
-}
-
-/* Should be called with solo_enc->lock held */
-static int solo_enc_on(struct solo_enc_fh *fh)
-{
-	struct solo_enc_dev *solo_enc = fh->enc;
-	u8 ch = solo_enc->ch;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	u8 interval;
-
-	assert_spin_locked(&solo_enc->lock);
-
-	if (fh->enc_on)
-		return 0;
-
-	solo_update_mode(solo_enc);
-
-	/* Make sure to bw check on first reader */
-	if (!atomic_read(&solo_enc->readers)) {
-		if (solo_enc->bw_weight > solo_dev->enc_bw_remain)
-			return -EBUSY;
-		else
-			solo_dev->enc_bw_remain -= solo_enc->bw_weight;
-	}
-
-	fh->enc_on = 1;
-	fh->rd_idx = solo_enc->solo_dev->enc_wr_idx;
-
-	if (fh->type == SOLO_ENC_TYPE_EXT)
-		solo_reg_write(solo_dev, SOLO_CAP_CH_COMP_ENA_E(ch), 1);
-
-	if (atomic_inc_return(&solo_enc->readers) > 1)
-		return 0;
-
-	/* Disable all encoding for this channel */
-	solo_reg_write(solo_dev, SOLO_CAP_CH_SCALE(ch), 0);
-
-	/* Common for both std and ext encoding */
-	solo_reg_write(solo_dev, SOLO_VE_CH_INTL(ch),
-		       solo_enc->interlaced ? 1 : 0);
-
-	if (solo_enc->interlaced)
-		interval = solo_enc->interval - 1;
-	else
-		interval = solo_enc->interval;
-
-	/* Standard encoding only */
-	solo_reg_write(solo_dev, SOLO_VE_CH_GOP(ch), solo_enc->gop);
-	solo_reg_write(solo_dev, SOLO_VE_CH_QP(ch), solo_enc->qp);
-	solo_reg_write(solo_dev, SOLO_CAP_CH_INTV(ch), interval);
-
-	/* Extended encoding only */
-	solo_reg_write(solo_dev, SOLO_VE_CH_GOP_E(ch), solo_enc->gop);
-	solo_reg_write(solo_dev, SOLO_VE_CH_QP_E(ch), solo_enc->qp);
-	solo_reg_write(solo_dev, SOLO_CAP_CH_INTV_E(ch), interval);
-
-	/* Enables the standard encoder */
-	solo_reg_write(solo_dev, SOLO_CAP_CH_SCALE(ch), solo_enc->mode);
-
-	/* Settle down Beavis... */
-	mdelay(10);
-
-	return 0;
-}
-
-static void solo_enc_off(struct solo_enc_fh *fh)
-{
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-	if (!fh->enc_on)
-		return;
-
-	if (fh->kthread) {
-		kthread_stop(fh->kthread);
-		fh->kthread = NULL;
-	}
-
-	solo_dev->enc_bw_remain += solo_enc->bw_weight;
-	fh->enc_on = 0;
-
-	if (atomic_dec_return(&solo_enc->readers) > 0)
-		return;
-
-	solo_reg_write(solo_dev, SOLO_CAP_CH_SCALE(solo_enc->ch), 0);
-	solo_reg_write(solo_dev, SOLO_CAP_CH_COMP_ENA_E(solo_enc->ch), 0);
-}
-
-static int solo_start_fh_thread(struct solo_enc_fh *fh)
-{
-	struct solo_enc_dev *solo_enc = fh->enc;
-
-	fh->kthread = kthread_run(solo_enc_thread, fh, SOLO6X10_NAME "_enc");
-
-	/* Oops, we had a problem */
-	if (IS_ERR(fh->kthread)) {
-		spin_lock(&solo_enc->lock);
-		solo_enc_off(fh);
-		spin_unlock(&solo_enc->lock);
-
-		return PTR_ERR(fh->kthread);
-	}
-
-	return 0;
-}
-
-static void enc_reset_gop(struct solo_dev *solo_dev, u8 ch)
-{
-	BUG_ON(ch >= solo_dev->nr_chans);
-	solo_reg_write(solo_dev, SOLO_VE_CH_GOP(ch), 1);
-	solo_dev->v4l2_enc[ch]->reset_gop = 1;
-}
-
-static int enc_gop_reset(struct solo_dev *solo_dev, u8 ch, u8 vop)
-{
-	BUG_ON(ch >= solo_dev->nr_chans);
-	if (!solo_dev->v4l2_enc[ch]->reset_gop)
-		return 0;
-	if (vop)
-		return 1;
-	solo_dev->v4l2_enc[ch]->reset_gop = 0;
-	solo_reg_write(solo_dev, SOLO_VE_CH_GOP(ch),
-		       solo_dev->v4l2_enc[ch]->gop);
-	return 0;
-}
-
-static void enc_write_sg(struct scatterlist *sglist, void *buf, int size)
-{
-	struct scatterlist *sg;
-	u8 *src = buf;
-
-	for (sg = sglist; sg && size > 0; sg = sg_next(sg)) {
-		u8 *p = sg_virt(sg);
-		size_t len = sg_dma_len(sg);
-		int i;
-
-		for (i = 0; i < len && size; i++)
-			p[i] = *(src++);
-	}
-}
-
-static int enc_get_mpeg_dma_sg(struct solo_dev *solo_dev,
-			       struct p2m_desc *desc,
-			       struct scatterlist *sglist, int skip,
-			       unsigned int off, unsigned int size)
-{
-	int ret;
-
-	if (off > SOLO_MP4E_EXT_SIZE(solo_dev))
-		return -EINVAL;
-
-	if (off + size <= SOLO_MP4E_EXT_SIZE(solo_dev)) {
-		return solo_p2m_dma_sg(solo_dev, SOLO_P2M_DMA_ID_MP4E,
-				       desc, 0, sglist, skip,
-				       SOLO_MP4E_EXT_ADDR(solo_dev) + off, size);
-	}
-
-	/* Buffer wrap */
-	ret = solo_p2m_dma_sg(solo_dev, SOLO_P2M_DMA_ID_MP4E, desc, 0,
-			      sglist, skip, SOLO_MP4E_EXT_ADDR(solo_dev) + off,
-			      SOLO_MP4E_EXT_SIZE(solo_dev) - off);
-
-	ret |= solo_p2m_dma_sg(solo_dev, SOLO_P2M_DMA_ID_MP4E, desc, 0,
-			       sglist, skip + SOLO_MP4E_EXT_SIZE(solo_dev) - off,
-			       SOLO_MP4E_EXT_ADDR(solo_dev),
-			       size + off - SOLO_MP4E_EXT_SIZE(solo_dev));
-
-	return ret;
-}
-
-static int enc_get_mpeg_dma_t(struct solo_dev *solo_dev,
-			      dma_addr_t buf, unsigned int off,
-			      unsigned int size)
-{
-	int ret;
-
-	if (off > SOLO_MP4E_EXT_SIZE(solo_dev))
-		return -EINVAL;
-
-	if (off + size <= SOLO_MP4E_EXT_SIZE(solo_dev)) {
-		return solo_p2m_dma_t(solo_dev, SOLO_P2M_DMA_ID_MP4E, 0, buf,
-				      SOLO_MP4E_EXT_ADDR(solo_dev) + off, size);
-	}
-
-	/* Buffer wrap */
-	ret = solo_p2m_dma_t(solo_dev, SOLO_P2M_DMA_ID_MP4E, 0, buf,
-			     SOLO_MP4E_EXT_ADDR(solo_dev) + off,
-			     SOLO_MP4E_EXT_SIZE(solo_dev) - off);
-
-	ret |= solo_p2m_dma_t(solo_dev, SOLO_P2M_DMA_ID_MP4E, 0,
-			      buf + SOLO_MP4E_EXT_SIZE(solo_dev) - off,
-			      SOLO_MP4E_EXT_ADDR(solo_dev),
-			      size + off - SOLO_MP4E_EXT_SIZE(solo_dev));
-
-	return ret;
-}
-
-static int enc_get_mpeg_dma(struct solo_dev *solo_dev, void *buf,
-			    unsigned int off, unsigned int size)
-{
-	int ret;
-
-	dma_addr_t dma_addr = pci_map_single(solo_dev->pdev, buf, size,
-					     PCI_DMA_FROMDEVICE);
-	ret = enc_get_mpeg_dma_t(solo_dev, dma_addr, off, size);
-	pci_unmap_single(solo_dev->pdev, dma_addr, size, PCI_DMA_FROMDEVICE);
-
-	return ret;
-}
-
-static int enc_get_jpeg_dma_sg(struct solo_dev *solo_dev,
-			       struct p2m_desc *desc,
-			       struct scatterlist *sglist, int skip,
-			       unsigned int off, unsigned int size)
-{
-	int ret;
-
-	if (off > SOLO_JPEG_EXT_SIZE(solo_dev))
-		return -EINVAL;
-
-	if (off + size <= SOLO_JPEG_EXT_SIZE(solo_dev)) {
-		return solo_p2m_dma_sg(solo_dev, SOLO_P2M_DMA_ID_JPEG,
-				       desc, 0, sglist, skip,
-				       SOLO_JPEG_EXT_ADDR(solo_dev) + off, size);
-	}
-
-	/* Buffer wrap */
-	ret = solo_p2m_dma_sg(solo_dev, SOLO_P2M_DMA_ID_JPEG, desc, 0,
-			      sglist, skip, SOLO_JPEG_EXT_ADDR(solo_dev) + off,
-			      SOLO_JPEG_EXT_SIZE(solo_dev) - off);
-
-	ret |= solo_p2m_dma_sg(solo_dev, SOLO_P2M_DMA_ID_JPEG, desc, 0,
-			       sglist, skip + SOLO_JPEG_EXT_SIZE(solo_dev) - off,
-			       SOLO_JPEG_EXT_ADDR(solo_dev),
-			       size + off - SOLO_JPEG_EXT_SIZE(solo_dev));
-
-	return ret;
-}
-
-/* Returns true of __chk is within the first __range bytes of __off */
-#define OFF_IN_RANGE(__off, __range, __chk) \
-	((__off <= __chk) && ((__off + __range) >= __chk))
-
-static void solo_jpeg_header(struct solo_enc_dev *solo_enc,
-			     struct videobuf_dmabuf *vbuf)
-{
-	struct scatterlist *sg;
-	void *src = jpeg_header;
-	size_t copied = 0;
-	size_t to_copy = sizeof(jpeg_header);
-
-	for (sg = vbuf->sglist; sg && copied < to_copy; sg = sg_next(sg)) {
-		size_t this_copy = min(sg_dma_len(sg),
-				       (unsigned int)(to_copy - copied));
-		u8 *p = sg_virt(sg);
-
-		memcpy(p, src + copied, this_copy);
-
-		if (OFF_IN_RANGE(copied, this_copy, SOF0_START + 5))
-			p[(SOF0_START + 5) - copied] =
-				0xff & (solo_enc->height >> 8);
-		if (OFF_IN_RANGE(copied, this_copy, SOF0_START + 6))
-			p[(SOF0_START + 6) - copied] = 0xff & solo_enc->height;
-		if (OFF_IN_RANGE(copied, this_copy, SOF0_START + 7))
-			p[(SOF0_START + 7) - copied] =
-				0xff & (solo_enc->width >> 8);
-		if (OFF_IN_RANGE(copied, this_copy, SOF0_START + 8))
-			p[(SOF0_START + 8) - copied] = 0xff & solo_enc->width;
-
-		copied += this_copy;
-	}
-}
-
-static int solo_fill_jpeg(struct solo_enc_fh *fh, struct solo_enc_buf *enc_buf,
-			  struct videobuf_buffer *vb,
-			  struct videobuf_dmabuf *vbuf)
-{
-	struct solo_dev *solo_dev = fh->enc->solo_dev;
-	int size = enc_buf->jpeg_size;
-
-	/* Copy the header first (direct write) */
-	solo_jpeg_header(fh->enc, vbuf);
-
-	vb->size = size + sizeof(jpeg_header);
-
-	/* Grab the jpeg frame */
-	return enc_get_jpeg_dma_sg(solo_dev, fh->desc, vbuf->sglist,
-				   sizeof(jpeg_header),
-				   enc_buf->jpeg_off, size);
-}
-
-static inline int vop_interlaced(__le32 *vh)
-{
-	return (__le32_to_cpu(vh[0]) >> 30) & 1;
-}
-
-static inline u32 vop_size(__le32 *vh)
-{
-	return __le32_to_cpu(vh[0]) & 0xFFFFF;
-}
-
-static inline u8 vop_hsize(__le32 *vh)
-{
-	return (__le32_to_cpu(vh[1]) >> 8) & 0xFF;
-}
-
-static inline u8 vop_vsize(__le32 *vh)
-{
-	return __le32_to_cpu(vh[1]) & 0xFF;
-}
-
-/* must be called with *bits % 8 = 0 */
-static void write_bytes(u8 **out, unsigned *bits, const u8 *src, unsigned count)
-{
-	memcpy(*out, src, count);
-	*out += count;
-	*bits += count * 8;
-}
-
-static void write_bits(u8 **out, unsigned *bits, u32 value, unsigned count)
-{
-
-	value <<= 32 - count; // shift to the right
-
-	while (count--) {
-		**out <<= 1;
-		**out |= !!(value & (1 << 31)); /* MSB */
-		value <<= 1;
-		if (++(*bits) % 8 == 0)
-			(*out)++;
-	}
-}
-
-static void write_ue(u8 **out, unsigned *bits, unsigned value) /* H.264 only */
-{
-	uint32_t max = 0, cnt = 0;
-
-	while (value > max) {
-		max = (max + 2) * 2 - 2;
-		cnt++;
-	}
-	write_bits(out, bits, 1, cnt + 1);
-	write_bits(out, bits, ~(max - value), cnt);
-}
-
-static void write_se(u8 **out, unsigned *bits, int value) /* H.264 only */
-{
-	if (value <= 0)
-		write_ue(out, bits, -value * 2);
-	else
-		write_ue(out, bits, value * 2 - 1);
-}
-
-static void write_mpeg4_end(u8 **out, unsigned *bits)
-{
-	write_bits(out, bits, 0, 1);
-	/* align on 32-bit boundary */
-	if (*bits % 32)
-		write_bits(out, bits, 0xFFFFFFFF, 32 - *bits % 32);
-}
-
-static void write_h264_end(u8 **out, unsigned *bits, int align)
-{
-	write_bits(out, bits, 1, 1);
-	while ((*bits) % 8)
-		write_bits(out, bits, 0, 1);
-	if (align)
-		while ((*bits) % 32)
-			write_bits(out, bits, 0, 1);
-}
-
-static void mpeg4_write_vol(u8 **out, struct solo_dev *solo_dev,
-			    __le32 *vh, unsigned fps, unsigned interval)
-{
-	static const u8 hdr[] = {
-		0, 0, 1, 0x00 /* video_object_start_code */,
-		0, 0, 1, 0x20 /* video_object_layer_start_code */
-	};
-	unsigned bits = 0;
-	unsigned width = vop_hsize(vh) << 4;
-	unsigned height = vop_vsize(vh) << 4;
-	unsigned interlaced = vop_interlaced(vh);
-
-	write_bytes(out, &bits, hdr, sizeof(hdr));
-	write_bits(out, &bits,    0,  1); /* random_accessible_vol */
-	write_bits(out, &bits, 0x04,  8); /* video_object_type_indication: main */
-	write_bits(out, &bits,    1,  1); /* is_object_layer_identifier */
-	write_bits(out, &bits,    2,  4); /* video_object_layer_verid: table V2-39 */
-	write_bits(out, &bits,    0,  3); /* video_object_layer_priority */
-	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC)
-		write_bits(out, &bits,  3,  4); /* aspect_ratio_info, assuming 4:3 */
-	else
-		write_bits(out, &bits,  2,  4);
-	write_bits(out, &bits,    1,  1); /* vol_control_parameters */
-	write_bits(out, &bits,    1,  2); /* chroma_format: 4:2:0 */
-	write_bits(out, &bits,    1,  1); /* low_delay */
-	write_bits(out, &bits,    0,  1); /* vbv_parameters */
-	write_bits(out, &bits,    0,  2); /* video_object_layer_shape: rectangular */
-	write_bits(out, &bits,    1,  1); /* marker_bit */
-	write_bits(out, &bits,  fps, 16); /* vop_time_increment_resolution */
-	write_bits(out, &bits,    1,  1); /* marker_bit */
-	write_bits(out, &bits,    1,  1); /* fixed_vop_rate */
-	write_bits(out, &bits, interval, 15); /* fixed_vop_time_increment */
-	write_bits(out, &bits,    1,  1); /* marker_bit */
-	write_bits(out, &bits, width, 13); /* video_object_layer_width */
-	write_bits(out, &bits,    1,  1); /* marker_bit */
-	write_bits(out, &bits, height, 13); /* video_object_layer_height */
-	write_bits(out, &bits,    1,  1); /* marker_bit */
-	write_bits(out, &bits, interlaced, 1); /* interlaced */
-	write_bits(out, &bits,    1,  1); /* obmc_disable */
-	write_bits(out, &bits,    0,  2); /* sprite_enable */
-	write_bits(out, &bits,    0,  1); /* not_8_bit */
-	write_bits(out, &bits,    1,  0); /* quant_type */
-	write_bits(out, &bits,    0,  1); /* load_intra_quant_mat */
-	write_bits(out, &bits,    0,  1); /* load_nonintra_quant_mat */
-	write_bits(out, &bits,    0,  1); /* quarter_sample */
-	write_bits(out, &bits,    1,  1); /* complexity_estimation_disable */
-	write_bits(out, &bits,    1,  1); /* resync_marker_disable */
-	write_bits(out, &bits,    0,  1); /* data_partitioned */
-	write_bits(out, &bits,    0,  1); /* newpred_enable */
-	write_bits(out, &bits,    0,  1); /* reduced_resolution_vop_enable */
-	write_bits(out, &bits,    0,  1); /* scalability */
-	write_mpeg4_end(out, &bits);
-}
-
-static void h264_write_vol(u8 **out, struct solo_dev *solo_dev, __le32 *vh)
-{
-	static const u8 sps[] = {
-		0, 0, 0, 1 /* start code */, 0x67, 66 /* profile_idc */,
-		0 /* constraints */, 30 /* level_idc */
-	};
-	static const u8 pps[] = {
-		0, 0, 0, 1 /* start code */, 0x68
-	};
-
-	unsigned bits = 0;
-	unsigned mbs_w = vop_hsize(vh);
-	unsigned mbs_h = vop_vsize(vh);
-
-	write_bytes(out, &bits, sps, sizeof(sps));
-	write_ue(out, &bits,   0);	/* seq_parameter_set_id */
-	write_ue(out, &bits,   5);	/* log2_max_frame_num_minus4 */
-	write_ue(out, &bits,   0);	/* pic_order_cnt_type */
-	write_ue(out, &bits,   6);	/* log2_max_pic_order_cnt_lsb_minus4 */
-	write_ue(out, &bits,   1);	/* max_num_ref_frames */
-	write_bits(out, &bits, 0, 1);	/* gaps_in_frame_num_value_allowed_flag */
-	write_ue(out, &bits, mbs_w - 1);	/* pic_width_in_mbs_minus1 */
-	write_ue(out, &bits, mbs_h - 1);	/* pic_height_in_map_units_minus1 */
-	write_bits(out, &bits, 1, 1);	/* frame_mbs_only_flag */
-	write_bits(out, &bits, 1, 1);	/* direct_8x8_frame_field_flag */
-	write_bits(out, &bits, 0, 1);	/* frame_cropping_flag */
-	write_bits(out, &bits, 0, 1);	/* vui_parameters_present_flag */
-	write_h264_end(out, &bits, 0);
-
-	write_bytes(out, &bits, pps, sizeof(pps));
-	write_ue(out, &bits,   0);	/* pic_parameter_set_id */
-	write_ue(out, &bits,   0);	/* seq_parameter_set_id */
-	write_bits(out, &bits, 0, 1);	/* entropy_coding_mode_flag */
-	write_bits(out, &bits, 0, 1);	/* bottom_field_pic_order_in_frame_present_flag */
-	write_ue(out, &bits,   0);	/* num_slice_groups_minus1 */
-	write_ue(out, &bits,   0);	/* num_ref_idx_l0_default_active_minus1 */
-	write_ue(out, &bits,   0);	/* num_ref_idx_l1_default_active_minus1 */
-	write_bits(out, &bits, 0, 1);	/* weighted_pred_flag */
-	write_bits(out, &bits, 0, 2);	/* weighted_bipred_idc */
-	write_se(out, &bits,   0);	/* pic_init_qp_minus26 */
-	write_se(out, &bits,   0);	/* pic_init_qs_minus26 */
-	write_se(out, &bits,   2);	/* chroma_qp_index_offset */
-	write_bits(out, &bits, 0, 1);	/* deblocking_filter_control_present_flag */
-	write_bits(out, &bits, 1, 1);	/* constrained_intra_pred_flag */
-	write_bits(out, &bits, 0, 1);	/* redundant_pic_cnt_present_flag */
-	write_h264_end(out, &bits, 1);
-}
-
-static int solo_fill_mpeg(struct solo_enc_fh *fh, struct solo_enc_buf *enc_buf,
-			  struct videobuf_buffer *vb,
-			  struct videobuf_dmabuf *vbuf)
-{
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-#define VH_WORDS 16
-#define MAX_VOL_HEADER_LENGTH 64
-
-	__le32 vh[VH_WORDS];
-	int ret;
-	int frame_size, frame_off;
-	int skip = 0;
-
-	if (WARN_ON_ONCE(enc_buf->size <= sizeof(vh)))
-		return -EINVAL;
-
-	/* First get the hardware vop header (not real mpeg) */
-	ret = enc_get_mpeg_dma(solo_dev, vh, enc_buf->off, sizeof(vh));
-	if (WARN_ON_ONCE(ret))
-		return ret;
-
-	if (WARN_ON_ONCE(vop_size(vh) > enc_buf->size))
-		return -EINVAL;
-
-	vb->width = vop_hsize(vh) << 4;
-	vb->height = vop_vsize(vh) << 4;
-	vb->size = vop_size(vh);
-
-	/* If this is a key frame, add extra m4v header */
-	if (!enc_buf->vop) {
-		u8 header[MAX_VOL_HEADER_LENGTH], *out = header;
-
-		if (solo_dev->flags & FLAGS_6110)
-			h264_write_vol(&out, solo_dev, vh);
-		else
-			mpeg4_write_vol(&out, solo_dev, vh,
-					solo_dev->fps * 1000,
-					solo_enc->interval * 1000);
-		skip = out - header;
-		enc_write_sg(vbuf->sglist, header, skip);
-		/* Adjust the dma buffer past this header */
-		vb->size += skip;
-	}
-
-	/* Now get the actual mpeg payload */
-	frame_off = (enc_buf->off + sizeof(vh)) % SOLO_MP4E_EXT_SIZE(solo_dev);
-	frame_size = enc_buf->size - sizeof(vh);
-
-	ret = enc_get_mpeg_dma_sg(solo_dev, fh->desc, vbuf->sglist,
-				  skip, frame_off, frame_size);
-	WARN_ON_ONCE(ret);
-
-	return ret;
-}
-
-static void solo_enc_fillbuf(struct solo_enc_fh *fh,
-			    struct videobuf_buffer *vb)
-{
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	struct solo_enc_buf *enc_buf = NULL;
-	struct videobuf_dmabuf *vbuf;
-	int ret;
-	int error = 1;
-	u16 idx = fh->rd_idx;
-
-	while (idx != solo_dev->enc_wr_idx) {
-		struct solo_enc_buf *ebuf = &solo_dev->enc_buf[idx];
-
-		idx = (idx + 1) % SOLO_NR_RING_BUFS;
-
-		if (ebuf->ch != solo_enc->ch)
-			continue;
-
-		if (fh->fmt == V4L2_PIX_FMT_MPEG) {
-			if (fh->type == ebuf->type) {
-				enc_buf = ebuf;
-				break;
-			}
-		} else {
-			/* For mjpeg, keep reading to the newest frame */
-			enc_buf = ebuf;
-		}
-	}
-
-	fh->rd_idx = idx;
-
-	if (WARN_ON_ONCE(!enc_buf))
-		goto buf_err;
-
-	if ((fh->fmt == V4L2_PIX_FMT_MPEG &&
-	     vb->bsize < enc_buf->size) ||
-	    (fh->fmt == V4L2_PIX_FMT_MJPEG &&
-	     vb->bsize < (enc_buf->jpeg_size + sizeof(jpeg_header)))) {
-		WARN_ON_ONCE(1);
-		goto buf_err;
-	}
-
-	vbuf = videobuf_to_dma(vb);
-	if (WARN_ON_ONCE(!vbuf))
-		goto buf_err;
-
-	if (fh->fmt == V4L2_PIX_FMT_MPEG)
-		ret = solo_fill_mpeg(fh, enc_buf, vb, vbuf);
-	else
-		ret = solo_fill_jpeg(fh, enc_buf, vb, vbuf);
-
-	if (!ret)
-		error = 0;
-
-buf_err:
-	if (error) {
-		vb->state = VIDEOBUF_ERROR;
-	} else {
-		vb->field_count++;
-		vb->ts = enc_buf->ts;
-		vb->state = VIDEOBUF_DONE;
-	}
-
-	wake_up(&vb->done);
-
-	return;
-}
-
-static void solo_enc_thread_try(struct solo_enc_fh *fh)
-{
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	struct videobuf_buffer *vb;
-
-	for (;;) {
-		spin_lock(&solo_enc->lock);
-
-		if (fh->rd_idx == solo_dev->enc_wr_idx)
-			break;
-
-		if (list_empty(&fh->vidq_active))
-			break;
-
-		vb = list_first_entry(&fh->vidq_active,
-				      struct videobuf_buffer, queue);
-
-		if (!waitqueue_active(&vb->done))
-			break;
-
-		list_del(&vb->queue);
-
-		spin_unlock(&solo_enc->lock);
-
-		solo_enc_fillbuf(fh, vb);
-	}
-
-	assert_spin_locked(&solo_enc->lock);
-	spin_unlock(&solo_enc->lock);
-}
-
-static int solo_enc_thread(void *data)
-{
-	struct solo_enc_fh *fh = data;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	DECLARE_WAITQUEUE(wait, current);
-
-	set_freezable();
-	add_wait_queue(&solo_enc->thread_wait, &wait);
-
-	for (;;) {
-		long timeout = schedule_timeout_interruptible(HZ);
-		if (timeout == -ERESTARTSYS || kthread_should_stop())
-			break;
-		solo_enc_thread_try(fh);
-		try_to_freeze();
-	}
-
-	remove_wait_queue(&solo_enc->thread_wait, &wait);
-
-	return 0;
-}
-
-void solo_motion_isr(struct solo_dev *solo_dev)
-{
-	u32 status;
-	int i;
-
-	solo_reg_write(solo_dev, SOLO_IRQ_STAT, SOLO_IRQ_MOTION);
-
-	status = solo_reg_read(solo_dev, SOLO_VI_MOT_STATUS);
-
-	for (i = 0; i < solo_dev->nr_chans; i++) {
-		struct solo_enc_dev *solo_enc = solo_dev->v4l2_enc[i];
-
-		BUG_ON(solo_enc == NULL);
-
-		if (solo_enc->motion_detected)
-			continue;
-		if (!(status & (1 << i)))
-			continue;
-
-		solo_enc->motion_detected = 1;
-	}
-}
-
-void solo_enc_v4l2_isr(struct solo_dev *solo_dev)
-{
-	struct solo_enc_buf *enc_buf;
-	u32 mpeg_current, mpeg_next, mpeg_size;
-	u32 jpeg_current, jpeg_next, jpeg_size;
-	u32 reg_mpeg_size;
-	u8 cur_q, vop_type;
-	u8 ch;
-	enum solo_enc_types enc_type;
-
-	solo_reg_write(solo_dev, SOLO_IRQ_STAT, SOLO_IRQ_ENCODER);
-
-	cur_q = ((solo_reg_read(solo_dev, SOLO_VE_STATE(11)) & 0xF) + 1) % MP4_QS;
-
-	reg_mpeg_size = ((solo_reg_read(solo_dev, SOLO_VE_STATE(0)) & 0xFFFFF) + 64 + 8) & ~7;
-
-	while (solo_dev->enc_idx != cur_q) {
-		mpeg_current = solo_reg_read(solo_dev,
-					SOLO_VE_MPEG4_QUE(solo_dev->enc_idx));
-		jpeg_current = solo_reg_read(solo_dev,
-					SOLO_VE_JPEG_QUE(solo_dev->enc_idx));
-		solo_dev->enc_idx = (solo_dev->enc_idx + 1) % MP4_QS;
-		mpeg_next = solo_reg_read(solo_dev,
-					SOLO_VE_MPEG4_QUE(solo_dev->enc_idx));
-		jpeg_next = solo_reg_read(solo_dev,
-					SOLO_VE_JPEG_QUE(solo_dev->enc_idx));
-
-		ch = (mpeg_current >> 24) & 0x1f;
-		if (ch >= SOLO_MAX_CHANNELS) {
-			ch -= SOLO_MAX_CHANNELS;
-			enc_type = SOLO_ENC_TYPE_EXT;
-		} else
-			enc_type = SOLO_ENC_TYPE_STD;
-
-		vop_type = (mpeg_current >> 29) & 3;
-
-		mpeg_current &= 0x00ffffff;
-		mpeg_next    &= 0x00ffffff;
-		jpeg_current &= 0x00ffffff;
-		jpeg_next    &= 0x00ffffff;
-
-		mpeg_size = (SOLO_MP4E_EXT_SIZE(solo_dev) +
-			     mpeg_next - mpeg_current) %
-			    SOLO_MP4E_EXT_SIZE(solo_dev);
-
-		jpeg_size = (SOLO_JPEG_EXT_SIZE(solo_dev) +
-			     jpeg_next - jpeg_current) %
-			    SOLO_JPEG_EXT_SIZE(solo_dev);
-
-		/* XXX I think this means we had a ring overflow? */
-		if (mpeg_current > mpeg_next && mpeg_size != reg_mpeg_size) {
-			enc_reset_gop(solo_dev, ch);
-			continue;
-		}
-
-		/* When resetting the GOP, skip frames until I-frame */
-		if (enc_gop_reset(solo_dev, ch, vop_type))
-			continue;
-
-		enc_buf = &solo_dev->enc_buf[solo_dev->enc_wr_idx];
-
-		enc_buf->vop = vop_type;
-		enc_buf->ch = ch;
-		enc_buf->off = mpeg_current;
-		enc_buf->size = mpeg_size;
-		enc_buf->jpeg_off = jpeg_current;
-		enc_buf->jpeg_size = jpeg_size;
-		enc_buf->type = enc_type;
-
-		do_gettimeofday(&enc_buf->ts);
-
-		solo_dev->enc_wr_idx = (solo_dev->enc_wr_idx + 1) %
-					SOLO_NR_RING_BUFS;
-
-		wake_up_interruptible(&solo_dev->v4l2_enc[ch]->thread_wait);
-	}
-
-	return;
-}
-
-static int solo_enc_buf_setup(struct videobuf_queue *vq, unsigned int *count,
-			      unsigned int *size)
-{
-	*size = FRAME_BUF_SIZE;
-
-	if (*count < MIN_VID_BUFFERS)
-		*count = MIN_VID_BUFFERS;
-
-	return 0;
-}
-
-static int solo_enc_buf_prepare(struct videobuf_queue *vq,
-				struct videobuf_buffer *vb,
-				enum v4l2_field field)
-{
-	struct solo_enc_fh *fh = vq->priv_data;
-	struct solo_enc_dev *solo_enc = fh->enc;
-
-	vb->size = FRAME_BUF_SIZE;
-	if (vb->baddr != 0 && vb->bsize < vb->size)
-		return -EINVAL;
-
-	/* These properties only change when queue is idle */
-	vb->width = solo_enc->width;
-	vb->height = solo_enc->height;
-	vb->field  = field;
-
-	if (vb->state == VIDEOBUF_NEEDS_INIT) {
-		int rc = videobuf_iolock(vq, vb, NULL);
-		if (rc < 0) {
-			struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
-			videobuf_dma_unmap(vq->dev, dma);
-			videobuf_dma_free(dma);
-			vb->state = VIDEOBUF_NEEDS_INIT;
-			return rc;
-		}
-	}
-	vb->state = VIDEOBUF_PREPARED;
-
-	return 0;
-}
-
-static void solo_enc_buf_queue(struct videobuf_queue *vq,
-			       struct videobuf_buffer *vb)
-{
-	struct solo_enc_fh *fh = vq->priv_data;
-
-	vb->state = VIDEOBUF_QUEUED;
-	list_add_tail(&vb->queue, &fh->vidq_active);
-	wake_up_interruptible(&fh->enc->thread_wait);
-}
-
-static void solo_enc_buf_release(struct videobuf_queue *vq,
-				 struct videobuf_buffer *vb)
-{
-	struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
-
-	videobuf_dma_unmap(vq->dev, dma);
-	videobuf_dma_free(dma);
-	vb->state = VIDEOBUF_NEEDS_INIT;
-}
-
-static struct videobuf_queue_ops solo_enc_video_qops = {
-	.buf_setup	= solo_enc_buf_setup,
-	.buf_prepare	= solo_enc_buf_prepare,
-	.buf_queue	= solo_enc_buf_queue,
-	.buf_release	= solo_enc_buf_release,
-};
-
-static unsigned int solo_enc_poll(struct file *file,
-				  struct poll_table_struct *wait)
-{
-	struct solo_enc_fh *fh = file->private_data;
-
-	return videobuf_poll_stream(file, &fh->vidq, wait);
-}
-
-static int solo_enc_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct solo_enc_fh *fh = file->private_data;
-
-	return videobuf_mmap_mapper(&fh->vidq, vma);
-}
-
-static int solo_enc_open(struct file *file)
-{
-	struct solo_enc_dev *solo_enc = video_drvdata(file);
-	struct solo_enc_fh *fh;
-
-	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
-	if (fh == NULL)
-		return -ENOMEM;
-
-	fh->enc = solo_enc;
-	file->private_data = fh;
-	INIT_LIST_HEAD(&fh->vidq_active);
-	fh->fmt = V4L2_PIX_FMT_MPEG;
-	fh->type = SOLO_ENC_TYPE_STD;
-
-	videobuf_queue_sg_init(&fh->vidq, &solo_enc_video_qops,
-			       &solo_enc->solo_dev->pdev->dev,
-			       &solo_enc->lock,
-			       V4L2_BUF_TYPE_VIDEO_CAPTURE,
-			       V4L2_FIELD_INTERLACED,
-			       sizeof(struct videobuf_buffer), fh, NULL);
-
-	return 0;
-}
-
-static ssize_t solo_enc_read(struct file *file, char __user *data,
-			     size_t count, loff_t *ppos)
-{
-	struct solo_enc_fh *fh = file->private_data;
-	struct solo_enc_dev *solo_enc = fh->enc;
-
-	/* Make sure the encoder is on */
-	if (!fh->enc_on) {
-		int ret;
-
-		spin_lock(&solo_enc->lock);
-		ret = solo_enc_on(fh);
-		spin_unlock(&solo_enc->lock);
-		if (ret)
-			return ret;
-
-		ret = solo_start_fh_thread(fh);
-		if (ret)
-			return ret;
-	}
-
-	return videobuf_read_stream(&fh->vidq, data, count, ppos, 0,
-				    file->f_flags & O_NONBLOCK);
-}
-
-static int solo_enc_release(struct file *file)
-{
-	struct solo_enc_fh *fh = file->private_data;
-	struct solo_enc_dev *solo_enc = fh->enc;
-
-	videobuf_stop(&fh->vidq);
-	videobuf_mmap_free(&fh->vidq);
-
-	spin_lock(&solo_enc->lock);
-	solo_enc_off(fh);
-	spin_unlock(&solo_enc->lock);
-
-	kfree(fh);
-
-	return 0;
-}
-
-static int solo_enc_querycap(struct file *file, void  *priv,
-			     struct v4l2_capability *cap)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-	strcpy(cap->driver, SOLO6X10_NAME);
-	snprintf(cap->card, sizeof(cap->card), "Softlogic 6x10 Enc %d",
-		 solo_enc->ch);
-	snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI %s",
-		 pci_name(solo_dev->pdev));
-	cap->version = SOLO6X10_VER_NUM;
-	cap->capabilities =     V4L2_CAP_VIDEO_CAPTURE |
-				V4L2_CAP_READWRITE |
-				V4L2_CAP_STREAMING;
-	return 0;
-}
-
-static int solo_enc_enum_input(struct file *file, void *priv,
-			       struct v4l2_input *input)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-	if (input->index)
-		return -EINVAL;
-
-	snprintf(input->name, sizeof(input->name), "Encoder %d",
-		 solo_enc->ch + 1);
-	input->type = V4L2_INPUT_TYPE_CAMERA;
-
-	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC)
-		input->std = V4L2_STD_NTSC_M;
-	else
-		input->std = V4L2_STD_PAL_B;
-
-	if (!tw28_get_video_status(solo_dev, solo_enc->ch))
-		input->status = V4L2_IN_ST_NO_SIGNAL;
-
-	return 0;
-}
-
-static int solo_enc_set_input(struct file *file, void *priv, unsigned int index)
-{
-	if (index)
-		return -EINVAL;
-
-	return 0;
-}
-
-static int solo_enc_get_input(struct file *file, void *priv,
-			      unsigned int *index)
-{
-	*index = 0;
-
-	return 0;
-}
-
-static int solo_enc_enum_fmt_cap(struct file *file, void *priv,
-				 struct v4l2_fmtdesc *f)
-{
-	switch (f->index) {
-	case 0:
-		f->pixelformat = V4L2_PIX_FMT_MPEG;
-		strcpy(f->description, "MPEG-4 AVC");
-		break;
-	case 1:
-		f->pixelformat = V4L2_PIX_FMT_MJPEG;
-		strcpy(f->description, "MJPEG");
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	f->flags = V4L2_FMT_FLAG_COMPRESSED;
-
-	return 0;
-}
-
-static int solo_enc_try_fmt_cap(struct file *file, void *priv,
-			    struct v4l2_format *f)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-
-	if (pix->pixelformat != V4L2_PIX_FMT_MPEG &&
-	    pix->pixelformat != V4L2_PIX_FMT_MJPEG)
-		return -EINVAL;
-
-	/* We cannot change width/height in mid read */
-	if (atomic_read(&solo_enc->readers) > 0) {
-		if (pix->width != solo_enc->width ||
-		    pix->height != solo_enc->height)
-			return -EBUSY;
-	}
-
-	if (pix->width < solo_dev->video_hsize ||
-	    pix->height < solo_dev->video_vsize << 1) {
-		/* Default to CIF 1/2 size */
-		pix->width = solo_dev->video_hsize >> 1;
-		pix->height = solo_dev->video_vsize;
-	} else {
-		/* Full frame */
-		pix->width = solo_dev->video_hsize;
-		pix->height = solo_dev->video_vsize << 1;
-	}
-
-	if (pix->field == V4L2_FIELD_ANY)
-		pix->field = V4L2_FIELD_INTERLACED;
-	else if (pix->field != V4L2_FIELD_INTERLACED)
-		pix->field = V4L2_FIELD_INTERLACED;
-
-	/* Just set these */
-	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
-	pix->sizeimage = FRAME_BUF_SIZE;
-
-	return 0;
-}
-
-static int solo_enc_set_fmt_cap(struct file *file, void *priv,
-				struct v4l2_format *f)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-	int ret;
-
-	spin_lock(&solo_enc->lock);
-
-	ret = solo_enc_try_fmt_cap(file, priv, f);
-	if (ret) {
-		spin_unlock(&solo_enc->lock);
-		return ret;
-	}
-
-	if (pix->width == solo_dev->video_hsize)
-		solo_enc->mode = SOLO_ENC_MODE_D1;
-	else
-		solo_enc->mode = SOLO_ENC_MODE_CIF;
-
-	/* This does not change the encoder at all */
-	fh->fmt = pix->pixelformat;
-
-	if (pix->priv)
-		fh->type = SOLO_ENC_TYPE_EXT;
-	ret = solo_enc_on(fh);
-
-	spin_unlock(&solo_enc->lock);
-
-	if (ret)
-		return ret;
-
-	return solo_start_fh_thread(fh);
-}
-
-static int solo_enc_get_fmt_cap(struct file *file, void *priv,
-				struct v4l2_format *f)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-
-	pix->width = solo_enc->width;
-	pix->height = solo_enc->height;
-	pix->pixelformat = fh->fmt;
-	pix->field = solo_enc->interlaced ? V4L2_FIELD_INTERLACED :
-		     V4L2_FIELD_NONE;
-	pix->sizeimage = FRAME_BUF_SIZE;
-	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
-
-	return 0;
-}
-
-static int solo_enc_reqbufs(struct file *file, void *priv,
-			    struct v4l2_requestbuffers *req)
-{
-	struct solo_enc_fh *fh = priv;
-
-	return videobuf_reqbufs(&fh->vidq, req);
-}
-
-static int solo_enc_querybuf(struct file *file, void *priv,
-			     struct v4l2_buffer *buf)
-{
-	struct solo_enc_fh *fh = priv;
-
-	return videobuf_querybuf(&fh->vidq, buf);
-}
-
-static int solo_enc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
-{
-	struct solo_enc_fh *fh = priv;
-
-	return videobuf_qbuf(&fh->vidq, buf);
-}
-
-static int solo_enc_dqbuf(struct file *file, void *priv,
-			  struct v4l2_buffer *buf)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	int ret;
-
-	/* Make sure the encoder is on */
-	if (!fh->enc_on) {
-		spin_lock(&solo_enc->lock);
-		ret = solo_enc_on(fh);
-		spin_unlock(&solo_enc->lock);
-		if (ret)
-			return ret;
-
-		ret = solo_start_fh_thread(fh);
-		if (ret)
-			return ret;
-	}
-
-	ret = videobuf_dqbuf(&fh->vidq, buf, file->f_flags & O_NONBLOCK);
-	if (ret)
-		return ret;
-
-	/* Signal motion detection */
-	if (solo_is_motion_on(solo_enc)) {
-		buf->flags |= V4L2_BUF_FLAG_MOTION_ON;
-		if (solo_enc->motion_detected) {
-			buf->flags |= V4L2_BUF_FLAG_MOTION_DETECTED;
-			solo_reg_write(solo_enc->solo_dev, SOLO_VI_MOT_CLEAR,
-				       1 << solo_enc->ch);
-			solo_enc->motion_detected = 0;
-		}
-	}
-
-	/* Check for key frame on mpeg data */
-	if (fh->fmt == V4L2_PIX_FMT_MPEG) {
-		struct videobuf_dmabuf *vbuf =
-				videobuf_to_dma(fh->vidq.bufs[buf->index]);
-
-		if (vbuf) {
-			u8 *p = sg_virt(vbuf->sglist);
-			if (p[3] == 0x00)
-				buf->flags |= V4L2_BUF_FLAG_KEYFRAME;
-			else
-				buf->flags |= V4L2_BUF_FLAG_PFRAME;
-		}
-	}
-
-	return 0;
-}
-
-static int solo_enc_streamon(struct file *file, void *priv,
-			     enum v4l2_buf_type i)
-{
-	struct solo_enc_fh *fh = priv;
-
-	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	return videobuf_streamon(&fh->vidq);
-}
-
-static int solo_enc_streamoff(struct file *file, void *priv,
-			      enum v4l2_buf_type i)
-{
-	struct solo_enc_fh *fh = priv;
-
-	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	return videobuf_streamoff(&fh->vidq);
-}
-
-static int solo_enc_s_std(struct file *file, void *priv, v4l2_std_id *i)
-{
-	return 0;
-}
-
-static int solo_enum_framesizes(struct file *file, void *priv,
-				struct v4l2_frmsizeenum *fsize)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_dev *solo_dev = fh->enc->solo_dev;
-
-	if (fsize->pixel_format != V4L2_PIX_FMT_MPEG)
-		return -EINVAL;
-
-	switch (fsize->index) {
-	case 0:
-		fsize->discrete.width = solo_dev->video_hsize >> 1;
-		fsize->discrete.height = solo_dev->video_vsize;
-		break;
-	case 1:
-		fsize->discrete.width = solo_dev->video_hsize;
-		fsize->discrete.height = solo_dev->video_vsize << 1;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
-
-	return 0;
-}
-
-static int solo_enum_frameintervals(struct file *file, void *priv,
-				    struct v4l2_frmivalenum *fintv)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_dev *solo_dev = fh->enc->solo_dev;
-
-	if (fintv->pixel_format != V4L2_PIX_FMT_MPEG || fintv->index)
-		return -EINVAL;
-
-	fintv->type = V4L2_FRMIVAL_TYPE_STEPWISE;
-
-	fintv->stepwise.min.numerator = solo_dev->fps;
-	fintv->stepwise.min.denominator = 1;
-
-	fintv->stepwise.max.numerator = solo_dev->fps;
-	fintv->stepwise.max.denominator = 15;
-
-	fintv->stepwise.step.numerator = 1;
-	fintv->stepwise.step.denominator = 1;
-
-	return 0;
-}
-
-static int solo_g_parm(struct file *file, void *priv,
-		       struct v4l2_streamparm *sp)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	struct v4l2_captureparm *cp = &sp->parm.capture;
-
-	cp->capability = V4L2_CAP_TIMEPERFRAME;
-	cp->timeperframe.numerator = solo_enc->interval;
-	cp->timeperframe.denominator = solo_dev->fps;
-	cp->capturemode = 0;
-	/* XXX: Shouldn't we be able to get/set this from videobuf? */
-	cp->readbuffers = 2;
-
-	return 0;
-}
-
-static int solo_s_parm(struct file *file, void *priv,
-		       struct v4l2_streamparm *sp)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-	struct v4l2_captureparm *cp = &sp->parm.capture;
-
-	spin_lock(&solo_enc->lock);
-
-	if (atomic_read(&solo_enc->readers) > 0) {
-		spin_unlock(&solo_enc->lock);
-		return -EBUSY;
-	}
-
-	if ((cp->timeperframe.numerator == 0) ||
-	    (cp->timeperframe.denominator == 0)) {
-		/* reset framerate */
-		cp->timeperframe.numerator = 1;
-		cp->timeperframe.denominator = solo_dev->fps;
-	}
-
-	if (cp->timeperframe.denominator != solo_dev->fps)
-		cp->timeperframe.denominator = solo_dev->fps;
-
-	if (cp->timeperframe.numerator > 15)
-		cp->timeperframe.numerator = 15;
-
-	solo_enc->interval = cp->timeperframe.numerator;
-
-	cp->capability = V4L2_CAP_TIMEPERFRAME;
-
-	solo_enc->gop = max(solo_dev->fps / solo_enc->interval, 1);
-	solo_update_mode(solo_enc);
-
-	spin_unlock(&solo_enc->lock);
-
-	return 0;
-}
-
-static int solo_queryctrl(struct file *file, void *priv,
-			  struct v4l2_queryctrl *qc)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-	qc->id = v4l2_ctrl_next(solo_ctrl_classes, qc->id);
-	if (!qc->id)
-		return -EINVAL;
-
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-	case V4L2_CID_CONTRAST:
-	case V4L2_CID_SATURATION:
-	case V4L2_CID_HUE:
-		return v4l2_ctrl_query_fill(qc, 0x00, 0xff, 1, 0x80);
-	case V4L2_CID_SHARPNESS:
-		return v4l2_ctrl_query_fill(qc, 0x00, 0x0f, 1, 0x00);
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		return v4l2_ctrl_query_fill(
-			qc, V4L2_MPEG_VIDEO_ENCODING_MPEG_1,
-			V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC, 1,
-			V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC);
-	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
-		return v4l2_ctrl_query_fill(qc, 1, 255, 1, solo_dev->fps);
-#ifdef PRIVATE_CIDS
-	case V4L2_CID_MOTION_THRESHOLD:
-		qc->flags |= V4L2_CTRL_FLAG_SLIDER;
-		qc->type = V4L2_CTRL_TYPE_INTEGER;
-		qc->minimum = 0;
-		qc->maximum = 0xffff;
-		qc->step = 1;
-		qc->default_value = SOLO_DEF_MOT_THRESH;
-		strlcpy(qc->name, "Motion Detection Threshold",
-			sizeof(qc->name));
-		return 0;
-	case V4L2_CID_MOTION_ENABLE:
-		qc->type = V4L2_CTRL_TYPE_BOOLEAN;
-		qc->minimum = 0;
-		qc->maximum = qc->step = 1;
-		qc->default_value = 0;
-		strlcpy(qc->name, "Motion Detection Enable", sizeof(qc->name));
-		return 0;
-#else
-	case V4L2_CID_MOTION_THRESHOLD:
-		return v4l2_ctrl_query_fill(qc, 0, 0xffff, 1,
-					    SOLO_DEF_MOT_THRESH);
-	case V4L2_CID_MOTION_ENABLE:
-		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 0);
-#endif
-	case V4L2_CID_RDS_TX_RADIO_TEXT:
-		qc->type = V4L2_CTRL_TYPE_STRING;
-		qc->minimum = 0;
-		qc->maximum = OSD_TEXT_MAX;
-		qc->step = 1;
-		qc->default_value = 0;
-		strlcpy(qc->name, "OSD Text", sizeof(qc->name));
-		return 0;
-	}
-
-	return -EINVAL;
-}
-
-static int solo_querymenu(struct file *file, void *priv,
-			  struct v4l2_querymenu *qmenu)
-{
-	struct v4l2_queryctrl qctrl;
-	int err;
-
-	qctrl.id = qmenu->id;
-	err = solo_queryctrl(file, priv, &qctrl);
-	if (err)
-		return err;
-
-	return v4l2_ctrl_query_menu(qmenu, &qctrl, NULL);
-}
-
-static int solo_g_ctrl(struct file *file, void *priv,
-		       struct v4l2_control *ctrl)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-	case V4L2_CID_CONTRAST:
-	case V4L2_CID_SATURATION:
-	case V4L2_CID_HUE:
-	case V4L2_CID_SHARPNESS:
-		return tw28_get_ctrl_val(solo_dev, ctrl->id, solo_enc->ch,
-					 &ctrl->value);
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC;
-		break;
-	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
-		ctrl->value = solo_enc->gop;
-		break;
-	case V4L2_CID_MOTION_THRESHOLD:
-		ctrl->value = solo_enc->motion_thresh;
-		break;
-	case V4L2_CID_MOTION_ENABLE:
-		ctrl->value = solo_is_motion_on(solo_enc);
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int solo_s_ctrl(struct file *file, void *priv,
-		       struct v4l2_control *ctrl)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	struct solo_dev *solo_dev = solo_enc->solo_dev;
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-	case V4L2_CID_CONTRAST:
-	case V4L2_CID_SATURATION:
-	case V4L2_CID_HUE:
-	case V4L2_CID_SHARPNESS:
-		return tw28_set_ctrl_val(solo_dev, ctrl->id, solo_enc->ch,
-					 ctrl->value);
-	case V4L2_CID_MPEG_VIDEO_ENCODING:
-		if (ctrl->value != V4L2_MPEG_VIDEO_ENCODING_MPEG_4_AVC)
-			return -ERANGE;
-		break;
-	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
-		if (ctrl->value < 1 || ctrl->value > 255)
-			return -ERANGE;
-		solo_enc->gop = ctrl->value;
-		solo_reg_write(solo_dev, SOLO_VE_CH_GOP(solo_enc->ch),
-			       solo_enc->gop);
-		solo_reg_write(solo_dev, SOLO_VE_CH_GOP_E(solo_enc->ch),
-			       solo_enc->gop);
-		break;
-	case V4L2_CID_MOTION_THRESHOLD:
-		/* TODO accept value on lower 16-bits and use high
-		 * 16-bits to assign the value to a specific block */
-		if (ctrl->value < 0 || ctrl->value > 0xffff)
-			return -ERANGE;
-		solo_enc->motion_thresh = ctrl->value;
-		solo_set_motion_threshold(solo_dev, solo_enc->ch, ctrl->value);
-		break;
-	case V4L2_CID_MOTION_ENABLE:
-		solo_motion_toggle(solo_enc, ctrl->value);
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int solo_s_ext_ctrls(struct file *file, void *priv,
-			    struct v4l2_ext_controls *ctrls)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	int i;
-
-	for (i = 0; i < ctrls->count; i++) {
-		struct v4l2_ext_control *ctrl = (ctrls->controls + i);
-		int err;
-
-		switch (ctrl->id) {
-		case V4L2_CID_RDS_TX_RADIO_TEXT:
-			if (ctrl->size - 1 > OSD_TEXT_MAX)
-				err = -ERANGE;
-			else {
-				err = copy_from_user(solo_enc->osd_text,
-						     ctrl->string,
-						     OSD_TEXT_MAX);
-				solo_enc->osd_text[OSD_TEXT_MAX] = '\0';
-				if (!err)
-					err = solo_osd_print(solo_enc);
-				else
-					err = -EFAULT;
-			}
-			break;
-		default:
-			err = -EINVAL;
-		}
-
-		if (err < 0) {
-			ctrls->error_idx = i;
-			return err;
-		}
-	}
-
-	return 0;
-}
-
-static int solo_g_ext_ctrls(struct file *file, void *priv,
-			    struct v4l2_ext_controls *ctrls)
-{
-	struct solo_enc_fh *fh = priv;
-	struct solo_enc_dev *solo_enc = fh->enc;
-	int i;
-
-	for (i = 0; i < ctrls->count; i++) {
-		struct v4l2_ext_control *ctrl = (ctrls->controls + i);
-		int err;
-
-		switch (ctrl->id) {
-		case V4L2_CID_RDS_TX_RADIO_TEXT:
-			if (ctrl->size < OSD_TEXT_MAX) {
-				ctrl->size = OSD_TEXT_MAX;
-				err = -ENOSPC;
-			} else {
-				err = copy_to_user(ctrl->string,
-						   solo_enc->osd_text,
-						   OSD_TEXT_MAX);
-				if (err)
-					err = -EFAULT;
-			}
-			break;
-		default:
-			err = -EINVAL;
-		}
-
-		if (err < 0) {
-			ctrls->error_idx = i;
-			return err;
-		}
-	}
-
-	return 0;
-}
-
-static const struct v4l2_file_operations solo_enc_fops = {
-	.owner			= THIS_MODULE,
-	.open			= solo_enc_open,
-	.release		= solo_enc_release,
-	.read			= solo_enc_read,
-	.poll			= solo_enc_poll,
-	.mmap			= solo_enc_mmap,
-	.ioctl			= video_ioctl2,
-};
-
-static const struct v4l2_ioctl_ops solo_enc_ioctl_ops = {
-	.vidioc_querycap		= solo_enc_querycap,
-	.vidioc_s_std			= solo_enc_s_std,
-	/* Input callbacks */
-	.vidioc_enum_input		= solo_enc_enum_input,
-	.vidioc_s_input			= solo_enc_set_input,
-	.vidioc_g_input			= solo_enc_get_input,
-	/* Video capture format callbacks */
-	.vidioc_enum_fmt_vid_cap	= solo_enc_enum_fmt_cap,
-	.vidioc_try_fmt_vid_cap		= solo_enc_try_fmt_cap,
-	.vidioc_s_fmt_vid_cap		= solo_enc_set_fmt_cap,
-	.vidioc_g_fmt_vid_cap		= solo_enc_get_fmt_cap,
-	/* Streaming I/O */
-	.vidioc_reqbufs			= solo_enc_reqbufs,
-	.vidioc_querybuf		= solo_enc_querybuf,
-	.vidioc_qbuf			= solo_enc_qbuf,
-	.vidioc_dqbuf			= solo_enc_dqbuf,
-	.vidioc_streamon		= solo_enc_streamon,
-	.vidioc_streamoff		= solo_enc_streamoff,
-	/* Frame size and interval */
-	.vidioc_enum_framesizes		= solo_enum_framesizes,
-	.vidioc_enum_frameintervals	= solo_enum_frameintervals,
-	/* Video capture parameters */
-	.vidioc_s_parm			= solo_s_parm,
-	.vidioc_g_parm			= solo_g_parm,
-	/* Controls */
-	.vidioc_queryctrl		= solo_queryctrl,
-	.vidioc_querymenu		= solo_querymenu,
-	.vidioc_g_ctrl			= solo_g_ctrl,
-	.vidioc_s_ctrl			= solo_s_ctrl,
-	.vidioc_g_ext_ctrls		= solo_g_ext_ctrls,
-	.vidioc_s_ext_ctrls		= solo_s_ext_ctrls,
-};
-
-static struct video_device solo_enc_template = {
-	.name			= SOLO6X10_NAME,
-	.fops			= &solo_enc_fops,
-	.ioctl_ops		= &solo_enc_ioctl_ops,
-	.minor			= -1,
-	.release		= video_device_release,
-
-	.tvnorms		= V4L2_STD_NTSC_M | V4L2_STD_PAL_B,
-	.current_norm		= V4L2_STD_NTSC_M,
-};
-
-static struct solo_enc_dev *solo_enc_alloc(struct solo_dev *solo_dev, u8 ch)
-{
-	struct solo_enc_dev *solo_enc;
-	int ret;
-
-	solo_enc = kzalloc(sizeof(*solo_enc), GFP_KERNEL);
-	if (!solo_enc)
-		return ERR_PTR(-ENOMEM);
-
-	solo_enc->vfd = video_device_alloc();
-	if (!solo_enc->vfd) {
-		kfree(solo_enc);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	solo_enc->solo_dev = solo_dev;
-	solo_enc->ch = ch;
-
-	*solo_enc->vfd = solo_enc_template;
-	solo_enc->vfd->parent = &solo_dev->pdev->dev;
-	ret = video_register_device(solo_enc->vfd, VFL_TYPE_GRABBER,
-				    video_nr);
-	if (ret < 0) {
-		video_device_release(solo_enc->vfd);
-		kfree(solo_enc);
-		return ERR_PTR(ret);
-	}
-
-	video_set_drvdata(solo_enc->vfd, solo_enc);
-
-	snprintf(solo_enc->vfd->name, sizeof(solo_enc->vfd->name),
-		 "%s-enc (%i/%i)", SOLO6X10_NAME, solo_dev->vfd->num,
-		 solo_enc->vfd->num);
-
-	if (video_nr != -1)
-		video_nr++;
-
-	spin_lock_init(&solo_enc->lock);
-	init_waitqueue_head(&solo_enc->thread_wait);
-	atomic_set(&solo_enc->readers, 0);
-
-	solo_enc->qp = SOLO_DEFAULT_QP;
-	solo_enc->gop = solo_dev->fps;
-	solo_enc->interval = 1;
-	solo_enc->mode = SOLO_ENC_MODE_CIF;
-	solo_enc->motion_thresh = SOLO_DEF_MOT_THRESH;
-
-	spin_lock(&solo_enc->lock);
-	solo_update_mode(solo_enc);
-	spin_unlock(&solo_enc->lock);
-
-	return solo_enc;
-}
-
-static void solo_enc_free(struct solo_enc_dev *solo_enc)
-{
-	if (solo_enc == NULL)
-		return;
-
-	video_unregister_device(solo_enc->vfd);
-	kfree(solo_enc);
-}
-
-int solo_enc_v4l2_init(struct solo_dev *solo_dev)
-{
-	int i;
-
-	for (i = 0; i < solo_dev->nr_chans; i++) {
-		solo_dev->v4l2_enc[i] = solo_enc_alloc(solo_dev, i);
-		if (IS_ERR(solo_dev->v4l2_enc[i]))
-			break;
-	}
-
-	if (i != solo_dev->nr_chans) {
-		int ret = PTR_ERR(solo_dev->v4l2_enc[i]);
-		while (i--)
-			solo_enc_free(solo_dev->v4l2_enc[i]);
-		return ret;
-	}
-
-	/* D1@MAX-FPS * 4 */
-	solo_dev->enc_bw_remain = solo_dev->fps * 4 * 4;
-
-	dev_info(&solo_dev->pdev->dev, "Encoders as /dev/video%d-%d\n",
-		 solo_dev->v4l2_enc[0]->vfd->num,
-		 solo_dev->v4l2_enc[solo_dev->nr_chans - 1]->vfd->num);
-
-	return 0;
-}
-
-void solo_enc_v4l2_exit(struct solo_dev *solo_dev)
-{
-	int i;
-
-	solo_irq_off(solo_dev, SOLO_IRQ_MOTION);
-
-	for (i = 0; i < solo_dev->nr_chans; i++)
-		solo_enc_free(solo_dev->v4l2_enc[i]);
-}
diff --git a/drivers/staging/media/solo6x10/v4l2.c b/drivers/staging/media/solo6x10/v4l2.c
deleted file mode 100644
index ca774cc57539..000000000000
--- a/drivers/staging/media/solo6x10/v4l2.c
+++ /dev/null
@@ -1,961 +0,0 @@
-/*
- * Copyright (C) 2010 Bluecherry, LLC www.bluecherrydvr.com
- * Copyright (C) 2010 Ben Collins <bcollins@bluecherry.net>
- *
- * 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/kernel.h>
-#include <linux/module.h>
-#include <linux/kthread.h>
-#include <linux/freezer.h>
-#include <media/v4l2-ioctl.h>
-#include <media/v4l2-common.h>
-#include <media/videobuf-dma-sg.h>
-#include "solo6x10.h"
-#include "tw28.h"
-
-#define SOLO_HW_BPL		2048
-#define SOLO_DISP_PIX_FIELD	V4L2_FIELD_INTERLACED
-
-/* Image size is two fields, SOLO_HW_BPL is one horizontal line */
-#define solo_vlines(__solo)	(__solo->video_vsize * 2)
-#define solo_image_size(__solo) (solo_bytesperline(__solo) * \
-				 solo_vlines(__solo))
-#define solo_bytesperline(__solo) (__solo->video_hsize * 2)
-
-#define MIN_VID_BUFFERS		4
-
-/* Simple file handle */
-struct solo_filehandle {
-	struct solo_dev		*solo_dev;
-	struct videobuf_queue	vidq;
-	struct task_struct      *kthread;
-	spinlock_t		slock;
-	int			old_write;
-	struct list_head	vidq_active;
-	struct p2m_desc		desc[SOLO_NR_P2M_DESC];
-	int			desc_idx;
-};
-
-unsigned video_nr = -1;
-module_param(video_nr, uint, 0644);
-MODULE_PARM_DESC(video_nr, "videoX start number, -1 is autodetect (default)");
-
-static void erase_on(struct solo_dev *solo_dev)
-{
-	solo_reg_write(solo_dev, SOLO_VO_DISP_ERASE, SOLO_VO_DISP_ERASE_ON);
-	solo_dev->erasing = 1;
-	solo_dev->frame_blank = 0;
-}
-
-static int erase_off(struct solo_dev *solo_dev)
-{
-	if (!solo_dev->erasing)
-		return 0;
-
-	/* First time around, assert erase off */
-	if (!solo_dev->frame_blank)
-		solo_reg_write(solo_dev, SOLO_VO_DISP_ERASE, 0);
-	/* Keep the erasing flag on for 8 frames minimum */
-	if (solo_dev->frame_blank++ >= 8)
-		solo_dev->erasing = 0;
-
-	return 1;
-}
-
-void solo_video_in_isr(struct solo_dev *solo_dev)
-{
-	solo_reg_write(solo_dev, SOLO_IRQ_STAT, SOLO_IRQ_VIDEO_IN);
-	wake_up_interruptible(&solo_dev->disp_thread_wait);
-}
-
-static void solo_win_setup(struct solo_dev *solo_dev, u8 ch,
-			   int sx, int sy, int ex, int ey, int scale)
-{
-	if (ch >= solo_dev->nr_chans)
-		return;
-
-	/* Here, we just keep window/channel the same */
-	solo_reg_write(solo_dev, SOLO_VI_WIN_CTRL0(ch),
-		       SOLO_VI_WIN_CHANNEL(ch) |
-		       SOLO_VI_WIN_SX(sx) |
-		       SOLO_VI_WIN_EX(ex) |
-		       SOLO_VI_WIN_SCALE(scale));
-
-	solo_reg_write(solo_dev, SOLO_VI_WIN_CTRL1(ch),
-		       SOLO_VI_WIN_SY(sy) |
-		       SOLO_VI_WIN_EY(ey));
-}
-
-static int solo_v4l2_ch_ext_4up(struct solo_dev *solo_dev, u8 idx, int on)
-{
-	u8 ch = idx * 4;
-
-	if (ch >= solo_dev->nr_chans)
-		return -EINVAL;
-
-	if (!on) {
-		u8 i;
-		for (i = ch; i < ch + 4; i++)
-			solo_win_setup(solo_dev, i, solo_dev->video_hsize,
-				       solo_vlines(solo_dev),
-				       solo_dev->video_hsize,
-				       solo_vlines(solo_dev), 0);
-		return 0;
-	}
-
-	/* Row 1 */
-	solo_win_setup(solo_dev, ch, 0, 0, solo_dev->video_hsize / 2,
-		       solo_vlines(solo_dev) / 2, 3);
-	solo_win_setup(solo_dev, ch + 1, solo_dev->video_hsize / 2, 0,
-		       solo_dev->video_hsize, solo_vlines(solo_dev) / 2, 3);
-	/* Row 2 */
-	solo_win_setup(solo_dev, ch + 2, 0, solo_vlines(solo_dev) / 2,
-		       solo_dev->video_hsize / 2, solo_vlines(solo_dev), 3);
-	solo_win_setup(solo_dev, ch + 3, solo_dev->video_hsize / 2,
-		       solo_vlines(solo_dev) / 2, solo_dev->video_hsize,
-		       solo_vlines(solo_dev), 3);
-
-	return 0;
-}
-
-static int solo_v4l2_ch_ext_16up(struct solo_dev *solo_dev, int on)
-{
-	int sy, ysize, hsize, i;
-
-	if (!on) {
-		for (i = 0; i < 16; i++)
-			solo_win_setup(solo_dev, i, solo_dev->video_hsize,
-				       solo_vlines(solo_dev),
-				       solo_dev->video_hsize,
-				       solo_vlines(solo_dev), 0);
-		return 0;
-	}
-
-	ysize = solo_vlines(solo_dev) / 4;
-	hsize = solo_dev->video_hsize / 4;
-
-	for (sy = 0, i = 0; i < 4; i++, sy += ysize) {
-		solo_win_setup(solo_dev, i * 4, 0, sy, hsize,
-			       sy + ysize, 5);
-		solo_win_setup(solo_dev, (i * 4) + 1, hsize, sy,
-			       hsize * 2, sy + ysize, 5);
-		solo_win_setup(solo_dev, (i * 4) + 2, hsize * 2, sy,
-			       hsize * 3, sy + ysize, 5);
-		solo_win_setup(solo_dev, (i * 4) + 3, hsize * 3, sy,
-			       solo_dev->video_hsize, sy + ysize, 5);
-	}
-
-	return 0;
-}
-
-static int solo_v4l2_ch(struct solo_dev *solo_dev, u8 ch, int on)
-{
-	u8 ext_ch;
-
-	if (ch < solo_dev->nr_chans) {
-		solo_win_setup(solo_dev, ch, on ? 0 : solo_dev->video_hsize,
-			       on ? 0 : solo_vlines(solo_dev),
-			       solo_dev->video_hsize, solo_vlines(solo_dev),
-			       on ? 1 : 0);
-		return 0;
-	}
-
-	if (ch >= solo_dev->nr_chans + solo_dev->nr_ext)
-		return -EINVAL;
-
-	ext_ch = ch - solo_dev->nr_chans;
-
-	/* 4up's first */
-	if (ext_ch < 4)
-		return solo_v4l2_ch_ext_4up(solo_dev, ext_ch, on);
-
-	/* Remaining case is 16up for 16-port */
-	return solo_v4l2_ch_ext_16up(solo_dev, on);
-}
-
-static int solo_v4l2_set_ch(struct solo_dev *solo_dev, u8 ch)
-{
-	if (ch >= solo_dev->nr_chans + solo_dev->nr_ext)
-		return -EINVAL;
-
-	erase_on(solo_dev);
-
-	solo_v4l2_ch(solo_dev, solo_dev->cur_disp_ch, 0);
-	solo_v4l2_ch(solo_dev, ch, 1);
-
-	solo_dev->cur_disp_ch = ch;
-
-	return 0;
-}
-
-static void disp_reset_desc(struct solo_filehandle *fh)
-{
-	/* We use desc mode, which ignores desc 0 */
-	memset(fh->desc, 0, sizeof(*fh->desc));
-	fh->desc_idx = 1;
-}
-
-static int disp_flush_descs(struct solo_filehandle *fh)
-{
-	int ret;
-
-	if (!fh->desc_idx)
-		return 0;
-
-	ret = solo_p2m_dma_desc(fh->solo_dev, SOLO_P2M_DMA_ID_DISP,
-				fh->desc, fh->desc_idx);
-	disp_reset_desc(fh);
-
-	return ret;
-}
-
-static int disp_push_desc(struct solo_filehandle *fh, dma_addr_t dma_addr,
-		      u32 ext_addr, int size, int repeat, int ext_size)
-{
-	if (fh->desc_idx >= SOLO_NR_P2M_DESC) {
-		int ret = disp_flush_descs(fh);
-		if (ret)
-			return ret;
-	}
-
-	solo_p2m_push_desc(&fh->desc[fh->desc_idx], 0, dma_addr, ext_addr,
-			   size, repeat, ext_size);
-	fh->desc_idx++;
-
-	return 0;
-}
-
-static void solo_fillbuf(struct solo_filehandle *fh,
-			 struct videobuf_buffer *vb)
-{
-	struct solo_dev *solo_dev = fh->solo_dev;
-	struct videobuf_dmabuf *vbuf;
-	unsigned int fdma_addr;
-	int error = 1;
-	int i;
-	struct scatterlist *sg;
-	dma_addr_t sg_dma;
-	int sg_size_left;
-
-	vbuf = videobuf_to_dma(vb);
-	if (!vbuf)
-		goto finish_buf;
-
-	if (erase_off(solo_dev)) {
-		int i;
-
-		/* Just blit to the entire sg list, ignoring size */
-		for_each_sg(vbuf->sglist, sg, vbuf->sglen, i) {
-			void *p = sg_virt(sg);
-			size_t len = sg_dma_len(sg);
-
-			for (i = 0; i < len; i += 2) {
-				((u8 *)p)[i] = 0x80;
-				((u8 *)p)[i + 1] = 0x00;
-			}
-		}
-
-		error = 0;
-		goto finish_buf;
-	}
-
-	disp_reset_desc(fh);
-	sg = vbuf->sglist;
-	sg_dma = sg_dma_address(sg);
-	sg_size_left = sg_dma_len(sg);
-
-	fdma_addr = SOLO_DISP_EXT_ADDR + (fh->old_write *
-			(SOLO_HW_BPL * solo_vlines(solo_dev)));
-
-	for (i = 0; i < solo_vlines(solo_dev); i++) {
-		int line_len = solo_bytesperline(solo_dev);
-		int lines;
-
-		if (!sg_size_left) {
-			sg = sg_next(sg);
-			if (sg == NULL)
-				goto finish_buf;
-			sg_dma = sg_dma_address(sg);
-			sg_size_left = sg_dma_len(sg);
-		}
-
-		/* No room for an entire line, so chunk it up */
-		if (sg_size_left < line_len) {
-			int this_addr = fdma_addr;
-
-			while (line_len > 0) {
-				int this_write;
-
-				if (!sg_size_left) {
-					sg = sg_next(sg);
-					if (sg == NULL)
-						goto finish_buf;
-					sg_dma = sg_dma_address(sg);
-					sg_size_left = sg_dma_len(sg);
-				}
-
-				this_write = min(sg_size_left, line_len);
-
-				if (disp_push_desc(fh, sg_dma, this_addr,
-						   this_write, 0, 0))
-					goto finish_buf;
-
-				line_len -= this_write;
-				sg_size_left -= this_write;
-				sg_dma += this_write;
-				this_addr += this_write;
-			}
-
-			fdma_addr += SOLO_HW_BPL;
-			continue;
-		}
-
-		/* Shove as many lines into a repeating descriptor as possible */
-		lines = min(sg_size_left / line_len,
-			    solo_vlines(solo_dev) - i);
-
-		if (disp_push_desc(fh, sg_dma, fdma_addr, line_len,
-				   lines - 1, SOLO_HW_BPL))
-			goto finish_buf;
-
-		i += lines - 1;
-		fdma_addr += SOLO_HW_BPL * lines;
-		sg_dma += lines * line_len;
-		sg_size_left -= lines * line_len;
-	}
-
-	error = disp_flush_descs(fh);
-
-finish_buf:
-	if (error) {
-		vb->state = VIDEOBUF_ERROR;
-	} else {
-		vb->size = solo_vlines(solo_dev) * solo_bytesperline(solo_dev);
-		vb->state = VIDEOBUF_DONE;
-		vb->field_count++;
-		do_gettimeofday(&vb->ts);
-	}
-
-	wake_up(&vb->done);
-
-	return;
-}
-
-static void solo_thread_try(struct solo_filehandle *fh)
-{
-	struct videobuf_buffer *vb;
-	unsigned int cur_write;
-
-	for (;;) {
-		spin_lock(&fh->slock);
-
-		if (list_empty(&fh->vidq_active))
-			break;
-
-		vb = list_first_entry(&fh->vidq_active, struct videobuf_buffer,
-				      queue);
-
-		if (!waitqueue_active(&vb->done))
-			break;
-
-		cur_write = SOLO_VI_STATUS0_PAGE(solo_reg_read(fh->solo_dev,
-						 SOLO_VI_STATUS0));
-		if (cur_write == fh->old_write)
-			break;
-
-		fh->old_write = cur_write;
-		list_del(&vb->queue);
-
-		spin_unlock(&fh->slock);
-
-		solo_fillbuf(fh, vb);
-	}
-
-	assert_spin_locked(&fh->slock);
-	spin_unlock(&fh->slock);
-}
-
-static int solo_thread(void *data)
-{
-	struct solo_filehandle *fh = data;
-	struct solo_dev *solo_dev = fh->solo_dev;
-	DECLARE_WAITQUEUE(wait, current);
-
-	set_freezable();
-	add_wait_queue(&solo_dev->disp_thread_wait, &wait);
-
-	for (;;) {
-		long timeout = schedule_timeout_interruptible(HZ);
-		if (timeout == -ERESTARTSYS || kthread_should_stop())
-			break;
-		solo_thread_try(fh);
-		try_to_freeze();
-	}
-
-	remove_wait_queue(&solo_dev->disp_thread_wait, &wait);
-
-	return 0;
-}
-
-static int solo_start_thread(struct solo_filehandle *fh)
-{
-	fh->kthread = kthread_run(solo_thread, fh, SOLO6X10_NAME "_disp");
-
-	return PTR_RET(fh->kthread);
-}
-
-static void solo_stop_thread(struct solo_filehandle *fh)
-{
-	if (fh->kthread) {
-		kthread_stop(fh->kthread);
-		fh->kthread = NULL;
-	}
-}
-
-static int solo_buf_setup(struct videobuf_queue *vq, unsigned int *count,
-			  unsigned int *size)
-{
-	struct solo_filehandle *fh = vq->priv_data;
-	struct solo_dev *solo_dev  = fh->solo_dev;
-
-	*size = solo_image_size(solo_dev);
-
-	if (*count < MIN_VID_BUFFERS)
-		*count = MIN_VID_BUFFERS;
-
-	return 0;
-}
-
-static int solo_buf_prepare(struct videobuf_queue *vq,
-			    struct videobuf_buffer *vb, enum v4l2_field field)
-{
-	struct solo_filehandle *fh  = vq->priv_data;
-	struct solo_dev *solo_dev = fh->solo_dev;
-
-	vb->size = solo_image_size(solo_dev);
-	if (vb->baddr != 0 && vb->bsize < vb->size)
-		return -EINVAL;
-
-	/* XXX: These properties only change when queue is idle */
-	vb->width  = solo_dev->video_hsize;
-	vb->height = solo_vlines(solo_dev);
-	vb->bytesperline = solo_bytesperline(solo_dev);
-	vb->field  = field;
-
-	if (vb->state == VIDEOBUF_NEEDS_INIT) {
-		int rc = videobuf_iolock(vq, vb, NULL);
-		if (rc < 0) {
-			struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
-			videobuf_dma_unmap(vq->dev, dma);
-			videobuf_dma_free(dma);
-			vb->state = VIDEOBUF_NEEDS_INIT;
-			return rc;
-		}
-	}
-	vb->state = VIDEOBUF_PREPARED;
-
-	return 0;
-}
-
-static void solo_buf_queue(struct videobuf_queue *vq,
-			   struct videobuf_buffer *vb)
-{
-	struct solo_filehandle *fh = vq->priv_data;
-	struct solo_dev *solo_dev = fh->solo_dev;
-
-	vb->state = VIDEOBUF_QUEUED;
-	list_add_tail(&vb->queue, &fh->vidq_active);
-	wake_up_interruptible(&solo_dev->disp_thread_wait);
-}
-
-static void solo_buf_release(struct videobuf_queue *vq,
-			     struct videobuf_buffer *vb)
-{
-	struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
-
-	videobuf_dma_unmap(vq->dev, dma);
-	videobuf_dma_free(dma);
-	vb->state = VIDEOBUF_NEEDS_INIT;
-}
-
-static struct videobuf_queue_ops solo_video_qops = {
-	.buf_setup	= solo_buf_setup,
-	.buf_prepare	= solo_buf_prepare,
-	.buf_queue	= solo_buf_queue,
-	.buf_release	= solo_buf_release,
-};
-
-static unsigned int solo_v4l2_poll(struct file *file,
-				   struct poll_table_struct *wait)
-{
-	struct solo_filehandle *fh = file->private_data;
-
-	return videobuf_poll_stream(file, &fh->vidq, wait);
-}
-
-static int solo_v4l2_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct solo_filehandle *fh = file->private_data;
-
-	return videobuf_mmap_mapper(&fh->vidq, vma);
-}
-
-static int solo_v4l2_open(struct file *file)
-{
-	struct solo_dev *solo_dev = video_drvdata(file);
-	struct solo_filehandle *fh;
-	int ret;
-
-	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
-	if (fh == NULL)
-		return -ENOMEM;
-
-	spin_lock_init(&fh->slock);
-	INIT_LIST_HEAD(&fh->vidq_active);
-	fh->solo_dev = solo_dev;
-	file->private_data = fh;
-
-	ret = solo_start_thread(fh);
-	if (ret) {
-		kfree(fh);
-		return ret;
-	}
-
-	videobuf_queue_sg_init(&fh->vidq, &solo_video_qops,
-			       &solo_dev->pdev->dev, &fh->slock,
-			       V4L2_BUF_TYPE_VIDEO_CAPTURE,
-			       SOLO_DISP_PIX_FIELD,
-			       sizeof(struct videobuf_buffer), fh, NULL);
-
-	return 0;
-}
-
-static ssize_t solo_v4l2_read(struct file *file, char __user *data,
-			      size_t count, loff_t *ppos)
-{
-	struct solo_filehandle *fh = file->private_data;
-
-	return videobuf_read_stream(&fh->vidq, data, count, ppos, 0,
-				    file->f_flags & O_NONBLOCK);
-}
-
-static int solo_v4l2_release(struct file *file)
-{
-	struct solo_filehandle *fh = file->private_data;
-
-	videobuf_stop(&fh->vidq);
-	videobuf_mmap_free(&fh->vidq);
-	solo_stop_thread(fh);
-	kfree(fh);
-
-	return 0;
-}
-
-static int solo_querycap(struct file *file, void  *priv,
-			 struct v4l2_capability *cap)
-{
-	struct solo_filehandle  *fh  = priv;
-	struct solo_dev *solo_dev = fh->solo_dev;
-
-	strcpy(cap->driver, SOLO6X10_NAME);
-	strcpy(cap->card, "Softlogic 6x10");
-	snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI %s",
-		 pci_name(solo_dev->pdev));
-	cap->version = SOLO6X10_VER_NUM;
-	cap->capabilities =     V4L2_CAP_VIDEO_CAPTURE |
-				V4L2_CAP_READWRITE |
-				V4L2_CAP_STREAMING;
-	return 0;
-}
-
-static int solo_enum_ext_input(struct solo_dev *solo_dev,
-			       struct v4l2_input *input)
-{
-	static const char *dispnames_1[] = { "4UP" };
-	static const char *dispnames_2[] = { "4UP-1", "4UP-2" };
-	static const char *dispnames_5[] = {
-		"4UP-1", "4UP-2", "4UP-3", "4UP-4", "16UP"
-	};
-	const char **dispnames;
-
-	if (input->index >= (solo_dev->nr_chans + solo_dev->nr_ext))
-		return -EINVAL;
-
-	if (solo_dev->nr_ext == 5)
-		dispnames = dispnames_5;
-	else if (solo_dev->nr_ext == 2)
-		dispnames = dispnames_2;
-	else
-		dispnames = dispnames_1;
-
-	snprintf(input->name, sizeof(input->name), "Multi %s",
-		 dispnames[input->index - solo_dev->nr_chans]);
-
-	return 0;
-}
-
-static int solo_enum_input(struct file *file, void *priv,
-			   struct v4l2_input *input)
-{
-	struct solo_filehandle *fh  = priv;
-	struct solo_dev *solo_dev = fh->solo_dev;
-
-	if (input->index >= solo_dev->nr_chans) {
-		int ret = solo_enum_ext_input(solo_dev, input);
-		if (ret < 0)
-			return ret;
-	} else {
-		snprintf(input->name, sizeof(input->name), "Camera %d",
-			 input->index + 1);
-
-		/* We can only check this for normal inputs */
-		if (!tw28_get_video_status(solo_dev, input->index))
-			input->status = V4L2_IN_ST_NO_SIGNAL;
-	}
-
-	input->type = V4L2_INPUT_TYPE_CAMERA;
-
-	if (solo_dev->video_type == SOLO_VO_FMT_TYPE_NTSC)
-		input->std = V4L2_STD_NTSC_M;
-	else
-		input->std = V4L2_STD_PAL_B;
-
-	return 0;
-}
-
-static int solo_set_input(struct file *file, void *priv, unsigned int index)
-{
-	struct solo_filehandle *fh = priv;
-
-	return solo_v4l2_set_ch(fh->solo_dev, index);
-}
-
-static int solo_get_input(struct file *file, void *priv, unsigned int *index)
-{
-	struct solo_filehandle *fh = priv;
-
-	*index = fh->solo_dev->cur_disp_ch;
-
-	return 0;
-}
-
-static int solo_enum_fmt_cap(struct file *file, void *priv,
-			     struct v4l2_fmtdesc *f)
-{
-	if (f->index)
-		return -EINVAL;
-
-	f->pixelformat = V4L2_PIX_FMT_UYVY;
-	strlcpy(f->description, "UYUV 4:2:2 Packed", sizeof(f->description));
-
-	return 0;
-}
-
-static int solo_try_fmt_cap(struct file *file, void *priv,
-			    struct v4l2_format *f)
-{
-	struct solo_filehandle *fh = priv;
-	struct solo_dev *solo_dev = fh->solo_dev;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-	int image_size = solo_image_size(solo_dev);
-
-	/* Check supported sizes */
-	if (pix->width != solo_dev->video_hsize)
-		pix->width = solo_dev->video_hsize;
-	if (pix->height != solo_vlines(solo_dev))
-		pix->height = solo_vlines(solo_dev);
-	if (pix->sizeimage != image_size)
-		pix->sizeimage = image_size;
-
-	/* Check formats */
-	if (pix->field == V4L2_FIELD_ANY)
-		pix->field = SOLO_DISP_PIX_FIELD;
-
-	if (pix->pixelformat != V4L2_PIX_FMT_UYVY ||
-	    pix->field       != SOLO_DISP_PIX_FIELD ||
-	    pix->colorspace  != V4L2_COLORSPACE_SMPTE170M)
-		return -EINVAL;
-
-	return 0;
-}
-
-static int solo_set_fmt_cap(struct file *file, void *priv,
-			    struct v4l2_format *f)
-{
-	struct solo_filehandle *fh = priv;
-
-	if (videobuf_queue_is_busy(&fh->vidq))
-		return -EBUSY;
-
-	/* For right now, if it doesn't match our running config,
-	 * then fail */
-	return solo_try_fmt_cap(file, priv, f);
-}
-
-static int solo_get_fmt_cap(struct file *file, void *priv,
-			    struct v4l2_format *f)
-{
-	struct solo_filehandle *fh = priv;
-	struct solo_dev *solo_dev = fh->solo_dev;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-
-	pix->width = solo_dev->video_hsize;
-	pix->height = solo_vlines(solo_dev);
-	pix->pixelformat = V4L2_PIX_FMT_UYVY;
-	pix->field = SOLO_DISP_PIX_FIELD;
-	pix->sizeimage = solo_image_size(solo_dev);
-	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
-	pix->bytesperline = solo_bytesperline(solo_dev);
-
-	return 0;
-}
-
-static int solo_reqbufs(struct file *file, void *priv,
-			struct v4l2_requestbuffers *req)
-{
-	struct solo_filehandle *fh = priv;
-
-	return videobuf_reqbufs(&fh->vidq, req);
-}
-
-static int solo_querybuf(struct file *file, void *priv, struct v4l2_buffer *buf)
-{
-	struct solo_filehandle *fh = priv;
-
-	return videobuf_querybuf(&fh->vidq, buf);
-}
-
-static int solo_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
-{
-	struct solo_filehandle *fh = priv;
-
-	return videobuf_qbuf(&fh->vidq, buf);
-}
-
-static int solo_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
-{
-	struct solo_filehandle *fh = priv;
-
-	return videobuf_dqbuf(&fh->vidq, buf, file->f_flags & O_NONBLOCK);
-}
-
-static int solo_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
-{
-	struct solo_filehandle *fh = priv;
-
-	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	return videobuf_streamon(&fh->vidq);
-}
-
-static int solo_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
-{
-	struct solo_filehandle *fh = priv;
-
-	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	return videobuf_streamoff(&fh->vidq);
-}
-
-static int solo_s_std(struct file *file, void *priv, v4l2_std_id *i)
-{
-	return 0;
-}
-
-static const u32 solo_motion_ctrls[] = {
-	V4L2_CID_MOTION_TRACE,
-	0
-};
-
-static const u32 *solo_ctrl_classes[] = {
-	solo_motion_ctrls,
-	NULL
-};
-
-static int solo_disp_queryctrl(struct file *file, void *priv,
-			       struct v4l2_queryctrl *qc)
-{
-	qc->id = v4l2_ctrl_next(solo_ctrl_classes, qc->id);
-	if (!qc->id)
-		return -EINVAL;
-
-	switch (qc->id) {
-#ifdef PRIVATE_CIDS
-	case V4L2_CID_MOTION_TRACE:
-		qc->type = V4L2_CTRL_TYPE_BOOLEAN;
-		qc->minimum = 0;
-		qc->maximum = qc->step = 1;
-		qc->default_value = 0;
-		strlcpy(qc->name, "Motion Detection Trace", sizeof(qc->name));
-		return 0;
-#else
-	case V4L2_CID_MOTION_TRACE:
-		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 0);
-#endif
-	}
-	return -EINVAL;
-}
-
-static int solo_disp_g_ctrl(struct file *file, void *priv,
-			    struct v4l2_control *ctrl)
-{
-	struct solo_filehandle *fh = priv;
-	struct solo_dev *solo_dev = fh->solo_dev;
-
-	switch (ctrl->id) {
-	case V4L2_CID_MOTION_TRACE:
-		ctrl->value = solo_reg_read(solo_dev, SOLO_VI_MOTION_BAR)
-			? 1 : 0;
-		return 0;
-	}
-	return -EINVAL;
-}
-
-static int solo_disp_s_ctrl(struct file *file, void *priv,
-			    struct v4l2_control *ctrl)
-{
-	struct solo_filehandle *fh = priv;
-	struct solo_dev *solo_dev = fh->solo_dev;
-
-	switch (ctrl->id) {
-	case V4L2_CID_MOTION_TRACE:
-		if (ctrl->value) {
-			solo_reg_write(solo_dev, SOLO_VI_MOTION_BORDER,
-					SOLO_VI_MOTION_Y_ADD |
-					SOLO_VI_MOTION_Y_VALUE(0x20) |
-					SOLO_VI_MOTION_CB_VALUE(0x10) |
-					SOLO_VI_MOTION_CR_VALUE(0x10));
-			solo_reg_write(solo_dev, SOLO_VI_MOTION_BAR,
-					SOLO_VI_MOTION_CR_ADD |
-					SOLO_VI_MOTION_Y_VALUE(0x10) |
-					SOLO_VI_MOTION_CB_VALUE(0x80) |
-					SOLO_VI_MOTION_CR_VALUE(0x10));
-		} else {
-			solo_reg_write(solo_dev, SOLO_VI_MOTION_BORDER, 0);
-			solo_reg_write(solo_dev, SOLO_VI_MOTION_BAR, 0);
-		}
-		return 0;
-	}
-	return -EINVAL;
-}
-
-static const struct v4l2_file_operations solo_v4l2_fops = {
-	.owner			= THIS_MODULE,
-	.open			= solo_v4l2_open,
-	.release		= solo_v4l2_release,
-	.read			= solo_v4l2_read,
-	.poll			= solo_v4l2_poll,
-	.mmap			= solo_v4l2_mmap,
-	.ioctl			= video_ioctl2,
-};
-
-static const struct v4l2_ioctl_ops solo_v4l2_ioctl_ops = {
-	.vidioc_querycap		= solo_querycap,
-	.vidioc_s_std			= solo_s_std,
-	/* Input callbacks */
-	.vidioc_enum_input		= solo_enum_input,
-	.vidioc_s_input			= solo_set_input,
-	.vidioc_g_input			= solo_get_input,
-	/* Video capture format callbacks */
-	.vidioc_enum_fmt_vid_cap	= solo_enum_fmt_cap,
-	.vidioc_try_fmt_vid_cap		= solo_try_fmt_cap,
-	.vidioc_s_fmt_vid_cap		= solo_set_fmt_cap,
-	.vidioc_g_fmt_vid_cap		= solo_get_fmt_cap,
-	/* Streaming I/O */
-	.vidioc_reqbufs			= solo_reqbufs,
-	.vidioc_querybuf		= solo_querybuf,
-	.vidioc_qbuf			= solo_qbuf,
-	.vidioc_dqbuf			= solo_dqbuf,
-	.vidioc_streamon		= solo_streamon,
-	.vidioc_streamoff		= solo_streamoff,
-	/* Controls */
-	.vidioc_queryctrl		= solo_disp_queryctrl,
-	.vidioc_g_ctrl			= solo_disp_g_ctrl,
-	.vidioc_s_ctrl			= solo_disp_s_ctrl,
-};
-
-static struct video_device solo_v4l2_template = {
-	.name			= SOLO6X10_NAME,
-	.fops			= &solo_v4l2_fops,
-	.ioctl_ops		= &solo_v4l2_ioctl_ops,
-	.minor			= -1,
-	.release		= video_device_release,
-
-	.tvnorms		= V4L2_STD_NTSC_M | V4L2_STD_PAL_B,
-	.current_norm		= V4L2_STD_NTSC_M,
-};
-
-int solo_v4l2_init(struct solo_dev *solo_dev)
-{
-	int ret;
-	int i;
-
-	init_waitqueue_head(&solo_dev->disp_thread_wait);
-
-	solo_dev->vfd = video_device_alloc();
-	if (!solo_dev->vfd)
-		return -ENOMEM;
-
-	*solo_dev->vfd = solo_v4l2_template;
-	solo_dev->vfd->parent = &solo_dev->pdev->dev;
-
-	ret = video_register_device(solo_dev->vfd, VFL_TYPE_GRABBER, video_nr);
-	if (ret < 0) {
-		video_device_release(solo_dev->vfd);
-		solo_dev->vfd = NULL;
-		return ret;
-	}
-
-	video_set_drvdata(solo_dev->vfd, solo_dev);
-
-	snprintf(solo_dev->vfd->name, sizeof(solo_dev->vfd->name), "%s (%i)",
-		 SOLO6X10_NAME, solo_dev->vfd->num);
-
-	if (video_nr != -1)
-		video_nr++;
-
-	dev_info(&solo_dev->pdev->dev, "Display as /dev/video%d with "
-		 "%d inputs (%d extended)\n", solo_dev->vfd->num,
-		 solo_dev->nr_chans, solo_dev->nr_ext);
-
-	/* Cycle all the channels and clear */
-	for (i = 0; i < solo_dev->nr_chans; i++) {
-		solo_v4l2_set_ch(solo_dev, i);
-		while (erase_off(solo_dev))
-			;/* Do nothing */
-	}
-
-	/* Set the default display channel */
-	solo_v4l2_set_ch(solo_dev, 0);
-	while (erase_off(solo_dev))
-		;/* Do nothing */
-
-	solo_irq_on(solo_dev, SOLO_IRQ_VIDEO_IN);
-
-	return 0;
-}
-
-void solo_v4l2_exit(struct solo_dev *solo_dev)
-{
-	solo_irq_off(solo_dev, SOLO_IRQ_VIDEO_IN);
-	if (solo_dev->vfd) {
-		video_unregister_device(solo_dev->vfd);
-		solo_dev->vfd = NULL;
-	}
-}
diff --git a/drivers/staging/rtl8187se/r8180.h b/drivers/staging/rtl8187se/r8180.h
index 70ea4145b4c8..edacc8001640 100644
--- a/drivers/staging/rtl8187se/r8180.h
+++ b/drivers/staging/rtl8187se/r8180.h
@@ -372,7 +372,6 @@ typedef struct r8180_priv
 	struct Stats stats;
 	struct _link_detect_t link_detect;  //YJ,add,080828
 	struct iw_statistics wstats;
-	struct proc_dir_entry *dir_dev;
 
 	/*RX stuff*/
 	u32 *rxring;
diff --git a/drivers/staging/rtl8187se/r8180_core.c b/drivers/staging/rtl8187se/r8180_core.c
index d10d75e8a33f..ca691550436a 100644
--- a/drivers/staging/rtl8187se/r8180_core.c
+++ b/drivers/staging/rtl8187se/r8180_core.c
@@ -36,6 +36,8 @@
 #include <linux/syscalls.h>
 #include <linux/eeprom_93cx6.h>
 #include <linux/interrupt.h>
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 
 #include "r8180_hw.h"
 #include "r8180.h"
@@ -204,51 +206,35 @@ void rtl8180_start_tx_beacon(struct net_device *dev);
 
 static struct proc_dir_entry *rtl8180_proc;
 
-static int proc_get_registers(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
+static int proc_get_registers(struct seq_file *m, void *v)
 {
-	struct net_device *dev = data;
-	int len = 0;
-	int i, n;
-	int max = 0xff;
+	struct net_device *dev = m->private;
+	int i, n, max = 0xff;
 
 	/* This dump the current register page */
 	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", n);
+		seq_printf(m, "\nD:  %2x > ", n);
 
 		for (i = 0; i < 16 && n <= max; i++, n++)
-			len += snprintf(page + len, count - len, "%2x ",
-					read_nic_byte(dev, n));
+			seq_printf(m, "%2x ", read_nic_byte(dev, n));
 	}
-	len += snprintf(page + len, count - len, "\n");
-
-	*eof = 1;
-	return len;
+	seq_putc(m, '\n');
+	return 0;
 }
 
 int get_curr_tx_free_desc(struct net_device *dev, int priority);
 
-static int proc_get_stats_hw(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
+static int proc_get_stats_hw(struct seq_file *m, void *v)
 {
-	int len = 0;
-
-	*eof = 1;
-	return len;
+	return 0;
 }
 
-static int proc_get_stats_rx(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
+static int proc_get_stats_rx(struct seq_file *m, void *v)
 {
-	struct net_device *dev = data;
+	struct net_device *dev = m->private;
 	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
 
-	int len = 0;
-
-	len += snprintf(page + len, count - len,
+	seq_printf(m,
 		"RX OK: %lu\n"
 		"RX Retry: %lu\n"
 		"RX CRC Error(0-500): %lu\n"
@@ -263,22 +249,17 @@ static int proc_get_stats_rx(char *page, char **start,
 		priv->stats.rxicverr
 		);
 
-	*eof = 1;
-	return len;
+	return 0;
 }
 
-static int proc_get_stats_tx(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
+static int proc_get_stats_tx(struct seq_file *m, void *v)
 {
-	struct net_device *dev = data;
+	struct net_device *dev = m->private;
 	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	int len = 0;
 	unsigned long totalOK;
 
 	totalOK = priv->stats.txnpokint+priv->stats.txhpokint+priv->stats.txlpokint;
-	len += snprintf(page + len, count - len,
+	seq_printf(m,
 		"TX OK: %lu\n"
 		"TX Error: %lu\n"
 		"TX Retry: %lu\n"
@@ -291,8 +272,7 @@ static int proc_get_stats_tx(char *page, char **start,
 		priv->stats.txbeaconerr
 	);
 
-	*eof = 1;
-	return len;
+	return 0;
 }
 
 void rtl8180_proc_module_init(void)
@@ -308,59 +288,61 @@ void rtl8180_proc_module_remove(void)
 
 void rtl8180_proc_remove_one(struct net_device *dev)
 {
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	if (priv->dir_dev) {
-		remove_proc_entry("stats-hw", priv->dir_dev);
-		remove_proc_entry("stats-tx", priv->dir_dev);
-		remove_proc_entry("stats-rx", priv->dir_dev);
-		remove_proc_entry("registers", priv->dir_dev);
-		priv->dir_dev = NULL;
-	}
+	remove_proc_subtree(dev->name, rtl8180_proc);
 }
 
-void rtl8180_proc_init_one(struct net_device *dev)
+/*
+ * seq_file wrappers for procfile show routines.
+ */
+static int rtl8180_proc_open(struct inode *inode, struct file *file)
 {
-	struct proc_dir_entry *e;
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct net_device *dev = proc_get_parent_data(inode);
+	int (*show)(struct seq_file *, void *) = PDE_DATA(inode);
 
-	priv->dir_dev = rtl8180_proc;
-	if (!priv->dir_dev) {
-		DMESGE("Unable to initialize /proc/net/r8180/%s\n",
-		      dev->name);
-		return;
-	}
+	return single_open(file, show, dev);
+}
 
-	e = create_proc_read_entry("stats-hw", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_stats_hw, dev);
-	if (!e) {
-		DMESGE("Unable to initialize "
-		      "/proc/net/r8180/%s/stats-hw\n",
-		      dev->name);
-	}
+static const struct file_operations rtl8180_proc_fops = {
+	.open		= rtl8180_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 
-	e = create_proc_read_entry("stats-rx", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_stats_rx, dev);
-	if (!e) {
-		DMESGE("Unable to initialize "
-		      "/proc/net/r8180/%s/stats-rx\n",
-		      dev->name);
-	}
+/*
+ * Table of proc files we need to create.
+ */
+struct rtl8180_proc_file {
+	char name[12];
+	int (*show)(struct seq_file *, void *);
+};
 
+static const struct rtl8180_proc_file rtl8180_proc_files[] = {
+	{ "stats-hw",	&proc_get_stats_hw },
+	{ "stats-rx",	&proc_get_stats_rx },
+	{ "stats-tx",	&proc_get_stats_tx },
+	{ "registers",	&proc_get_registers },
+	{ "" }
+};
+
+void rtl8180_proc_init_one(struct net_device *dev)
+{
+	const struct rtl8180_proc_file *f;
+	struct proc_dir_entry *dir;
 
-	e = create_proc_read_entry("stats-tx", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_stats_tx, dev);
-	if (!e) {
-		DMESGE("Unable to initialize "
-		      "/proc/net/r8180/%s/stats-tx\n",
-		      dev->name);
+	dir = proc_mkdir_data(dev->name, 0, rtl8180_proc, dev);
+	if (!dir) {
+		DMESGE("Unable to initialize /proc/net/r8180/%s\n", dev->name);
+		return;
 	}
 
-	e = create_proc_read_entry("registers", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers, dev);
-	if (!e) {
-		DMESGE("Unable to initialize "
-		      "/proc/net/r8180/%s/registers\n",
-		      dev->name);
+	for (f = rtl8180_proc_files; f->name[0]; f++) {
+		if (!proc_create_data(f->name, S_IFREG | S_IRUGO, dir,
+				      &rtl8180_proc_fops, f->show)) {
+			DMESGE("Unable to initialize /proc/net/r8180/%s/%s\n",
+			       dev->name, f->name);
+			return;
+		}
 	}
 }
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/Makefile b/drivers/staging/rtl8192e/rtl8192e/Makefile
index 313a92ec6833..a2c4fb4ba1af 100644
--- a/drivers/staging/rtl8192e/rtl8192e/Makefile
+++ b/drivers/staging/rtl8192e/rtl8192e/Makefile
@@ -7,7 +7,6 @@ r8192e_pci-objs :=		\
 	r8190P_rtl8256.o	\
 	rtl_cam.o		\
 	rtl_core.o		\
-	rtl_debug.o		\
 	rtl_dm.o		\
 	rtl_eeprom.o		\
 	rtl_ethtool.o		\
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
index 4ebf99b30975..2b6c61c5d3d8 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
@@ -2966,8 +2966,6 @@ static int rtl8192_pci_probe(struct pci_dev *pdev,
 		goto err_free_irq;
 	RT_TRACE(COMP_INIT, "dev name: %s\n", dev->name);
 
-	rtl8192_proc_init_one(dev);
-
 	if (priv->polling_timer_on == 0)
 		check_rfctrl_gpio_timer((unsigned long)dev);
 
@@ -3003,7 +3001,6 @@ static void rtl8192_pci_disconnect(struct pci_dev *pdev)
 		del_timer_sync(&priv->gpio_polling_timer);
 		cancel_delayed_work(&priv->gpio_change_rf_wq);
 		priv->polling_timer_on = 0;
-		rtl8192_proc_remove_one(dev);
 		rtl8192_down(dev, true);
 		deinit_hal_dm(dev);
 		if (priv->pFirmware) {
@@ -3093,7 +3090,6 @@ static int __init rtl8192_pci_module_init(void)
 	printk(KERN_INFO "\nLinux kernel driver for RTL8192E WLAN cards\n");
 	printk(KERN_INFO "Copyright (c) 2007-2008, Realsil Wlan Driver\n");
 
-	rtl8192_proc_module_init();
 	if (0 != pci_register_driver(&rtl8192_pci_driver)) {
 		DMESG("No device found");
 		/*pci_unregister_driver (&rtl8192_pci_driver);*/
@@ -3107,7 +3103,6 @@ static void __exit rtl8192_pci_module_exit(void)
 	pci_unregister_driver(&rtl8192_pci_driver);
 
 	RT_TRACE(COMP_DOWN, "Exiting");
-	rtl8192_proc_module_remove();
 }
 
 void check_rfctrl_gpio_timer(unsigned long data)
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
index 320d5fc026b4..87d4d349c890 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
@@ -1085,10 +1085,4 @@ void ActUpdateChannelAccessSetting(struct net_device *dev,
 			   enum wireless_mode WirelessMode,
 			   struct channel_access_setting *ChnlAccessSetting);
 
-/* proc stuff from rtl_debug.c */
-void rtl8192_proc_init_one(struct net_device *dev);
-void rtl8192_proc_remove_one(struct net_device *dev);
-void rtl8192_proc_module_init(void);
-void rtl8192_proc_module_remove(void);
-
 #endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_debug.c b/drivers/staging/rtl8192e/rtl8192e/rtl_debug.c
deleted file mode 100644
index c19b14cd6f77..000000000000
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_debug.c
+++ /dev/null
@@ -1,1029 +0,0 @@
-/******************************************************************************
- * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
- *
- * Based on the r8180 driver, which is:
- * Copyright 2004-2005 Andrea Merello <andreamrl@tiscali.it>, et al.
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
-#include "rtl_core.h"
-#include "r8192E_phy.h"
-#include "r8192E_phyreg.h"
-#include "r8190P_rtl8256.h" /* RTL8225 Radio frontend */
-#include "r8192E_cmdpkt.h"
-
-/****************************************************************************
-   -----------------------------PROCFS STUFF-------------------------
-*****************************************************************************/
-/*This part is related to PROC, which will record some statistics. */
-static struct proc_dir_entry *rtl8192_proc;
-
-static int proc_get_stats_ap(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
-	struct rtllib_device *ieee = priv->rtllib;
-	struct rtllib_network *target;
-	int len = 0;
-
-	list_for_each_entry(target, &ieee->network_list, list) {
-
-		len += snprintf(page + len, count - len,
-				"%s ", target->ssid);
-
-		if (target->wpa_ie_len > 0 || target->rsn_ie_len > 0)
-			len += snprintf(page + len, count - len,
-					"WPA\n");
-		else
-			len += snprintf(page + len, count - len,
-					"non_WPA\n");
-
-	}
-
-	*eof = 1;
-	return len;
-}
-
-static int proc_get_registers_0(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n, page0;
-
-	int max = 0xff;
-	page0 = 0x000;
-
-	len += snprintf(page + len, count - len,
-			"\n####################page %x##################\n ",
-			(page0>>8));
-	len += snprintf(page + len, count - len,
-			"\nD:  OF > 00 01 02 03 04 05 06 07 08 09 0A 0B "
-			"0C 0D 0E 0F");
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", n);
-		for (i = 0; i < 16 && n <= max; n++, i++)
-			len += snprintf(page + len, count - len,
-					"%2.2x ", read_nic_byte(dev,
-					(page0 | n)));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-
-}
-static int proc_get_registers_1(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n, page0;
-
-	int max = 0xff;
-	page0 = 0x100;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n####################page %x##################\n ",
-			(page0>>8));
-	len += snprintf(page + len, count - len,
-			"\nD:  OF > 00 01 02 03 04 05 06 07 08 09 0A 0B "
-			"0C 0D 0E 0F");
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len,
-				"\nD:  %2x > ", n);
-		for (i = 0; i < 16 && n <= max; i++, n++)
-			len += snprintf(page + len, count - len,
-					"%2.2x ", read_nic_byte(dev,
-					(page0 | n)));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-
-}
-static int proc_get_registers_2(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n, page0;
-
-	int max = 0xff;
-	page0 = 0x200;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n####################page %x##################\n ",
-			(page0 >> 8));
-	len += snprintf(page + len, count - len,
-			"\nD:  OF > 00 01 02 03 04 05 06 07 08 09 0A 0B 0C "
-			"0D 0E 0F");
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len,
-				"\nD:  %2x > ", n);
-		for (i = 0; i < 16 && n <= max; i++, n++)
-			len += snprintf(page + len, count - len,
-					"%2.2x ", read_nic_byte(dev,
-					(page0 | n)));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-
-}
-static int proc_get_registers_3(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n, page0;
-
-	int max = 0xff;
-	page0 = 0x300;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n####################page %x##################\n ",
-			(page0>>8));
-	len += snprintf(page + len, count - len,
-			"\nD:  OF > 00 01 02 03 04 05 06 07 08 09 0A 0B "
-			"0C 0D 0E 0F");
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len,
-				"\nD:  %2x > ", n);
-		for (i = 0; i < 16 && n <= max; i++, n++)
-			len += snprintf(page + len, count - len,
-					"%2.2x ", read_nic_byte(dev,
-					(page0 | n)));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-
-}
-static int proc_get_registers_4(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n, page0;
-
-	int max = 0xff;
-	page0 = 0x400;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n####################page %x##################\n ",
-			(page0>>8));
-	len += snprintf(page + len, count - len,
-			"\nD:  OF > 00 01 02 03 04 05 06 07 08 09 0A 0B "
-			"0C 0D 0E 0F");
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len,
-				"\nD:  %2x > ", n);
-		for (i = 0; i < 16 && n <= max; i++, n++)
-			len += snprintf(page + len, count - len,
-					"%2.2x ", read_nic_byte(dev,
-					(page0 | n)));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-
-}
-static int proc_get_registers_5(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n, page0;
-
-	int max = 0xff;
-	page0 = 0x500;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n####################page %x##################\n ",
-			(page0 >> 8));
-	len += snprintf(page + len, count - len,
-			"\nD:  OF > 00 01 02 03 04 05 06 07 08 09 0A 0B "
-			"0C 0D 0E 0F");
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len,
-				"\nD:  %2x > ", n);
-		for (i = 0; i < 16 && n <= max; i++, n++)
-			len += snprintf(page + len, count - len,
-					"%2.2x ", read_nic_byte(dev,
-					(page0 | n)));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-
-}
-static int proc_get_registers_6(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n, page0;
-
-	int max = 0xff;
-	page0 = 0x600;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n####################page %x##################\n ",
-			(page0>>8));
-	len += snprintf(page + len, count - len,
-			"\nD:  OF > 00 01 02 03 04 05 06 07 08 09 0A 0B "
-			"0C 0D 0E 0F");
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len,
-				"\nD:  %2x > ", n);
-		for (i = 0; i < 16 && n <= max; i++, n++)
-			len += snprintf(page + len, count - len,
-					"%2.2x ", read_nic_byte(dev,
-					(page0 | n)));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-
-}
-static int proc_get_registers_7(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n, page0;
-
-	int max = 0xff;
-	page0 = 0x700;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n####################page %x##################\n ",
-			(page0 >> 8));
-	len += snprintf(page + len, count - len,
-			"\nD:  OF > 00 01 02 03 04 05 06 07 08 09 0A 0B 0C "
-			"0D 0E 0F");
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len,
-				"\nD:  %2x > ", n);
-		for (i = 0; i < 16 && n <= max; i++, n++)
-			len += snprintf(page + len, count - len,
-					"%2.2x ", read_nic_byte(dev,
-					(page0 | n)));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-
-}
-static int proc_get_registers_8(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n, page0;
-
-	int max = 0xff;
-	page0 = 0x800;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n####################page %x##################\n",
-			(page0 >> 8));
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", n);
-		for (i = 0; i < 4 && n <= max; n += 4, i++)
-			len += snprintf(page + len, count - len,
-					"%8.8x ", rtl8192_QueryBBReg(dev,
-					(page0 | n), bMaskDWord));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-
-}
-static int proc_get_registers_9(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n, page0;
-
-	int max = 0xff;
-	page0 = 0x900;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n####################page %x##################\n",
-			(page0>>8));
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", n);
-		for (i = 0; i < 4 && n <= max; n += 4, i++)
-			len += snprintf(page + len, count - len,
-					"%8.8x ", rtl8192_QueryBBReg(dev,
-					(page0 | n), bMaskDWord));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-}
-static int proc_get_registers_a(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n, page0;
-
-	int max = 0xff;
-	page0 = 0xa00;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n####################page %x##################\n",
-			(page0>>8));
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", n);
-		for (i = 0; i < 4 && n <= max; n += 4, i++)
-			len += snprintf(page + len, count - len,
-					"%8.8x ", rtl8192_QueryBBReg(dev,
-					(page0 | n), bMaskDWord));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-}
-static int proc_get_registers_b(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n, page0;
-
-	int max = 0xff;
-	page0 = 0xb00;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n####################page %x##################\n",
-			(page0 >> 8));
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", n);
-		for (i = 0; i < 4 && n <= max; n += 4, i++)
-			len += snprintf(page + len, count - len,
-					"%8.8x ", rtl8192_QueryBBReg(dev,
-					(page0 | n), bMaskDWord));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-}
-static int proc_get_registers_c(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n, page0;
-
-	int max = 0xff;
-	page0 = 0xc00;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n####################page %x##################\n",
-			(page0>>8));
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", n);
-		for (i = 0; i < 4 && n <= max; n += 4, i++)
-			len += snprintf(page + len, count - len,
-					"%8.8x ", rtl8192_QueryBBReg(dev,
-					(page0 | n), bMaskDWord));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-}
-static int proc_get_registers_d(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n, page0;
-
-	int max = 0xff;
-	page0 = 0xd00;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n####################page %x##################\n",
-			(page0>>8));
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", n);
-		for (i = 0; i < 4 && n <= max; n += 4, i++)
-			len += snprintf(page + len, count - len,
-					"%8.8x ", rtl8192_QueryBBReg(dev,
-					(page0 | n), bMaskDWord));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-}
-static int proc_get_registers_e(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n, page0;
-
-	int max = 0xff;
-	page0 = 0xe00;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n####################page %x##################\n",
-			(page0>>8));
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", n);
-		for (i = 0; i < 4 && n <= max; n += 4, i++)
-			len += snprintf(page + len, count - len,
-					"%8.8x ", rtl8192_QueryBBReg(dev,
-					(page0 | n), bMaskDWord));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-}
-
-static int proc_get_reg_rf_a(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n;
-
-	int max = 0xff;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n#################### RF-A ##################\n ");
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", n);
-		for (i = 0; i < 4 && n <= max; n += 4, i++)
-			len += snprintf(page + len, count - len,
-					"%8.8x ", rtl8192_phy_QueryRFReg(dev,
-					(enum rf90_radio_path)RF90_PATH_A, n,
-					bMaskDWord));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-}
-
-static int proc_get_reg_rf_b(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n;
-
-	int max = 0xff;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n#################### RF-B ##################\n ");
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", n);
-		for (i = 0; i < 4 && n <= max; n += 4, i++)
-			len += snprintf(page + len, count - len,
-					"%8.8x ", rtl8192_phy_QueryRFReg(dev,
-					(enum rf90_radio_path)RF90_PATH_B, n,
-					bMaskDWord));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-}
-
-static int proc_get_reg_rf_c(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n;
-
-	int max = 0xff;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n#################### RF-C ##################\n");
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", n);
-		for (i = 0; i < 4 && n <= max; n += 4, i++)
-			len += snprintf(page + len, count - len,
-					"%8.8x ", rtl8192_phy_QueryRFReg(dev,
-					(enum rf90_radio_path)RF90_PATH_C, n,
-					bMaskDWord));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-}
-
-static int proc_get_reg_rf_d(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-
-	int len = 0;
-	int i, n;
-
-	int max = 0xff;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n#################### RF-D ##################\n ");
-	for (n = 0; n <= max;) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", n);
-		for (i = 0; i < 4 && n <= max; n += 4, i++)
-			len += snprintf(page + len, count - len,
-					"%8.8x ", rtl8192_phy_QueryRFReg(dev,
-					(enum rf90_radio_path)RF90_PATH_D, n,
-					bMaskDWord));
-	}
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-}
-
-static int proc_get_cam_register_1(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-	u32 target_command = 0;
-	u32 target_content = 0;
-	u8 entry_i = 0;
-	u32 ulStatus;
-	int len = 0;
-	int i = 100, j = 0;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n#################### SECURITY CAM (0-10) ######"
-			"############\n ");
-	for (j = 0; j < 11; j++) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", j);
-		for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
-			target_command = entry_i+CAM_CONTENT_COUNT*j;
-			target_command = target_command | BIT31;
-
-			while ((i--) >= 0) {
-				ulStatus = read_nic_dword(dev, RWCAM);
-				if (ulStatus & BIT31)
-					continue;
-				else
-					break;
-			}
-			write_nic_dword(dev, RWCAM, target_command);
-			target_content = read_nic_dword(dev, RCAMO);
-			len += snprintf(page + len, count - len, "%8.8x ",
-					target_content);
-		}
-	}
-
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-}
-
-static int proc_get_cam_register_2(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-	u32 target_command = 0;
-	u32 target_content = 0;
-	u8 entry_i = 0;
-	u32 ulStatus;
-	int len = 0;
-	int i = 100, j = 0;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n#################### SECURITY CAM (11-21) "
-			"##################\n ");
-	for (j = 11; j < 22; j++) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", j);
-		for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
-			target_command = entry_i + CAM_CONTENT_COUNT * j;
-			target_command = target_command | BIT31;
-
-			while ((i--) >= 0) {
-				ulStatus = read_nic_dword(dev, RWCAM);
-				if (ulStatus & BIT31)
-					continue;
-				else
-					break;
-			}
-			write_nic_dword(dev, RWCAM, target_command);
-			target_content = read_nic_dword(dev, RCAMO);
-			len += snprintf(page + len, count - len, "%8.8x ",
-					target_content);
-		}
-	}
-
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-}
-
-static int proc_get_cam_register_3(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-	u32 target_command = 0;
-	u32 target_content = 0;
-	u8 entry_i = 0;
-	u32 ulStatus;
-	int len = 0;
-	int i = 100, j = 0;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n#################### SECURITY CAM (22-31) ######"
-			"############\n ");
-	for (j = 22; j < TOTAL_CAM_ENTRY; j++) {
-		len += snprintf(page + len, count - len, "\nD:  %2x > ", j);
-		for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
-			target_command = entry_i + CAM_CONTENT_COUNT * j;
-			target_command = target_command | BIT31;
-
-			while ((i--) >= 0) {
-				ulStatus = read_nic_dword(dev, RWCAM);
-				if (ulStatus & BIT31)
-					continue;
-				else
-					break;
-			}
-			write_nic_dword(dev, RWCAM, target_command);
-			target_content = read_nic_dword(dev, RCAMO);
-			len += snprintf(page + len, count - len, "%8.8x ",
-					target_content);
-		}
-	}
-
-	len += snprintf(page + len, count - len, "\n");
-	*eof = 1;
-	return len;
-}
-static int proc_get_stats_tx(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
-
-	int len = 0;
-
-	len += snprintf(page + len, count - len,
-		"TX VI priority ok int: %lu\n"
-		"TX VO priority ok int: %lu\n"
-		"TX BE priority ok int: %lu\n"
-		"TX BK priority ok int: %lu\n"
-		"TX MANAGE priority ok int: %lu\n"
-		"TX BEACON priority ok int: %lu\n"
-		"TX BEACON priority error int: %lu\n"
-		"TX CMDPKT priority ok int: %lu\n"
-		"TX queue stopped?: %d\n"
-		"TX fifo overflow: %lu\n"
-		"TX total data packets %lu\n"
-		"TX total data bytes :%lu\n",
-		priv->stats.txviokint,
-		priv->stats.txvookint,
-		priv->stats.txbeokint,
-		priv->stats.txbkokint,
-		priv->stats.txmanageokint,
-		priv->stats.txbeaconokint,
-		priv->stats.txbeaconerr,
-		priv->stats.txcmdpktokint,
-		netif_queue_stopped(dev),
-		priv->stats.txoverflow,
-		priv->rtllib->stats.tx_packets,
-		priv->rtllib->stats.tx_bytes
-
-
-		);
-
-	*eof = 1;
-	return len;
-}
-
-
-
-static int proc_get_stats_rx(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
-{
-	struct net_device *dev = data;
-	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
-
-	int len = 0;
-
-	len += snprintf(page + len, count - len,
-		"RX packets: %lu\n"
-		"RX data crc err: %lu\n"
-		"RX mgmt crc err: %lu\n"
-		"RX desc err: %lu\n"
-		"RX rx overflow error: %lu\n",
-		priv->stats.rxint,
-		priv->stats.rxdatacrcerr,
-		priv->stats.rxmgmtcrcerr,
-		priv->stats.rxrdu,
-		priv->stats.rxoverflow);
-
-	*eof = 1;
-	return len;
-}
-
-void rtl8192_proc_module_init(void)
-{
-	RT_TRACE(COMP_INIT, "Initializing proc filesystem");
-	rtl8192_proc = create_proc_entry(DRV_NAME, S_IFDIR, init_net.proc_net);
-}
-
-
-void rtl8192_proc_module_remove(void)
-{
-	remove_proc_entry(DRV_NAME, init_net.proc_net);
-}
-
-
-void rtl8192_proc_remove_one(struct net_device *dev)
-{
-	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
-
-	printk(KERN_INFO "dev name %s\n", dev->name);
-
-	if (priv->dir_dev) {
-		remove_proc_entry("stats-tx", priv->dir_dev);
-		remove_proc_entry("stats-rx", priv->dir_dev);
-		remove_proc_entry("stats-ap", priv->dir_dev);
-		remove_proc_entry("registers-0", priv->dir_dev);
-		remove_proc_entry("registers-1", priv->dir_dev);
-		remove_proc_entry("registers-2", priv->dir_dev);
-		remove_proc_entry("registers-3", priv->dir_dev);
-		remove_proc_entry("registers-4", priv->dir_dev);
-		remove_proc_entry("registers-5", priv->dir_dev);
-		remove_proc_entry("registers-6", priv->dir_dev);
-		remove_proc_entry("registers-7", priv->dir_dev);
-		remove_proc_entry("registers-8", priv->dir_dev);
-		remove_proc_entry("registers-9", priv->dir_dev);
-		remove_proc_entry("registers-a", priv->dir_dev);
-		remove_proc_entry("registers-b", priv->dir_dev);
-		remove_proc_entry("registers-c", priv->dir_dev);
-		remove_proc_entry("registers-d", priv->dir_dev);
-		remove_proc_entry("registers-e", priv->dir_dev);
-		remove_proc_entry("RF-A", priv->dir_dev);
-		remove_proc_entry("RF-B", priv->dir_dev);
-		remove_proc_entry("RF-C", priv->dir_dev);
-		remove_proc_entry("RF-D", priv->dir_dev);
-		remove_proc_entry("SEC-CAM-1", priv->dir_dev);
-		remove_proc_entry("SEC-CAM-2", priv->dir_dev);
-		remove_proc_entry("SEC-CAM-3", priv->dir_dev);
-		remove_proc_entry("wlan0", rtl8192_proc);
-		priv->dir_dev = NULL;
-	}
-}
-
-
-void rtl8192_proc_init_one(struct net_device *dev)
-{
-	struct proc_dir_entry *e;
-	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
-
-	priv->dir_dev = create_proc_entry(dev->name,
-					  S_IFDIR | S_IRUGO | S_IXUGO,
-					  rtl8192_proc);
-	if (!priv->dir_dev) {
-		RT_TRACE(COMP_ERR, "Unable to initialize /proc/net/rtl8192"
-			 "/%s\n", dev->name);
-		return;
-	}
-	e = create_proc_read_entry("stats-rx", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_stats_rx, dev);
-
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/stats-rx\n",
-		      dev->name);
-
-	e = create_proc_read_entry("stats-tx", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_stats_tx, dev);
-
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/stats-tx\n",
-		      dev->name);
-
-	e = create_proc_read_entry("stats-ap", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_stats_ap, dev);
-
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/stats-ap\n",
-		      dev->name);
-
-	e = create_proc_read_entry("registers-0", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers_0, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers-0\n",
-		      dev->name);
-	e = create_proc_read_entry("registers-1", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers_1, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers-1\n",
-		      dev->name);
-	e = create_proc_read_entry("registers-2", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers_2, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers-2\n",
-		      dev->name);
-	e = create_proc_read_entry("registers-3", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers_3, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers-3\n",
-		      dev->name);
-	e = create_proc_read_entry("registers-4", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers_4, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers-4\n",
-		      dev->name);
-	e = create_proc_read_entry("registers-5", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers_5, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers-5\n",
-		      dev->name);
-	e = create_proc_read_entry("registers-6", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers_6, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers-6\n",
-		      dev->name);
-	e = create_proc_read_entry("registers-7", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers_7, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers-7\n",
-		      dev->name);
-	e = create_proc_read_entry("registers-8", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers_8, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers-8\n",
-		      dev->name);
-	e = create_proc_read_entry("registers-9", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers_9, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers-9\n",
-		      dev->name);
-	e = create_proc_read_entry("registers-a", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers_a, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers-a\n",
-		      dev->name);
-	e = create_proc_read_entry("registers-b", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers_b, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers-b\n",
-		      dev->name);
-	e = create_proc_read_entry("registers-c", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers_c, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers-c\n",
-		      dev->name);
-	e = create_proc_read_entry("registers-d", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers_d, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers-d\n",
-		      dev->name);
-	e = create_proc_read_entry("registers-e", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers_e, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers-e\n",
-		      dev->name);
-	e = create_proc_read_entry("RF-A", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_reg_rf_a, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/RF-A\n",
-		      dev->name);
-	e = create_proc_read_entry("RF-B", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_reg_rf_b, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/RF-B\n",
-		      dev->name);
-	e = create_proc_read_entry("RF-C", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_reg_rf_c, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/RF-C\n",
-		      dev->name);
-	e = create_proc_read_entry("RF-D", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_reg_rf_d, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/RF-D\n",
-		      dev->name);
-	e = create_proc_read_entry("SEC-CAM-1", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_cam_register_1, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/SEC-CAM-1\n",
-		      dev->name);
-	e = create_proc_read_entry("SEC-CAM-2", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_cam_register_2, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/SEC-CAM-2\n",
-		      dev->name);
-	e = create_proc_read_entry("SEC-CAM-3", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_cam_register_3, dev);
-	if (!e)
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/SEC-CAM-3\n",
-		      dev->name);
-}
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c b/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
index 4217b88e6fc3..e51cb49ce10e 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
@@ -412,19 +412,18 @@ static int rtllib_ccmp_get_key(void *key, int len, u8 *seq, void *priv)
 }
 
 
-static char *rtllib_ccmp_print_stats(char *p, void *priv)
+static void rtllib_ccmp_print_stats(struct seq_file *m, void *priv)
 {
 	struct rtllib_ccmp_data *ccmp = priv;
-	p += sprintf(p, "key[%d] alg=CCMP key_set=%d "
-		     "tx_pn=%pM rx_pn=%pM "
-		     "format_errors=%d replays=%d decrypt_errors=%d\n",
-		     ccmp->key_idx, ccmp->key_set,
-		     ccmp->tx_pn, ccmp->rx_pn,
-		     ccmp->dot11RSNAStatsCCMPFormatErrors,
-		     ccmp->dot11RSNAStatsCCMPReplays,
-		     ccmp->dot11RSNAStatsCCMPDecryptErrors);
-
-	return p;
+	seq_printf(m,
+		   "key[%d] alg=CCMP key_set=%d "
+		   "tx_pn=%pM rx_pn=%pM "
+		   "format_errors=%d replays=%d decrypt_errors=%d\n",
+		   ccmp->key_idx, ccmp->key_set,
+		   ccmp->tx_pn, ccmp->rx_pn,
+		   ccmp->dot11RSNAStatsCCMPFormatErrors,
+		   ccmp->dot11RSNAStatsCCMPReplays,
+		   ccmp->dot11RSNAStatsCCMPDecryptErrors);
 }
 
 static struct lib80211_crypto_ops rtllib_crypt_ccmp = {
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_tkip.c b/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
index 800925053fb0..5cfd73baf1cc 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
@@ -708,30 +708,30 @@ static int rtllib_tkip_get_key(void *key, int len, u8 *seq, void *priv)
 }
 
 
-static char *rtllib_tkip_print_stats(char *p, void *priv)
+static void rtllib_tkip_print_stats(struct seq_file *m, void *priv)
 {
 	struct rtllib_tkip_data *tkip = priv;
-	p += sprintf(p, "key[%d] alg=TKIP key_set=%d "
-		     "tx_pn=%02x%02x%02x%02x%02x%02x "
-		     "rx_pn=%02x%02x%02x%02x%02x%02x "
-		     "replays=%d icv_errors=%d local_mic_failures=%d\n",
-		     tkip->key_idx, tkip->key_set,
-		     (tkip->tx_iv32 >> 24) & 0xff,
-		     (tkip->tx_iv32 >> 16) & 0xff,
-		     (tkip->tx_iv32 >> 8) & 0xff,
-		     tkip->tx_iv32 & 0xff,
-		     (tkip->tx_iv16 >> 8) & 0xff,
-		     tkip->tx_iv16 & 0xff,
-		     (tkip->rx_iv32 >> 24) & 0xff,
-		     (tkip->rx_iv32 >> 16) & 0xff,
-		     (tkip->rx_iv32 >> 8) & 0xff,
-		     tkip->rx_iv32 & 0xff,
-		     (tkip->rx_iv16 >> 8) & 0xff,
-		     tkip->rx_iv16 & 0xff,
-		     tkip->dot11RSNAStatsTKIPReplays,
-		     tkip->dot11RSNAStatsTKIPICVErrors,
-		     tkip->dot11RSNAStatsTKIPLocalMICFailures);
-	return p;
+	seq_printf(m,
+		   "key[%d] alg=TKIP key_set=%d "
+		   "tx_pn=%02x%02x%02x%02x%02x%02x "
+		   "rx_pn=%02x%02x%02x%02x%02x%02x "
+		   "replays=%d icv_errors=%d local_mic_failures=%d\n",
+		   tkip->key_idx, tkip->key_set,
+		   (tkip->tx_iv32 >> 24) & 0xff,
+		   (tkip->tx_iv32 >> 16) & 0xff,
+		   (tkip->tx_iv32 >> 8) & 0xff,
+		   tkip->tx_iv32 & 0xff,
+		   (tkip->tx_iv16 >> 8) & 0xff,
+		   tkip->tx_iv16 & 0xff,
+		   (tkip->rx_iv32 >> 24) & 0xff,
+		   (tkip->rx_iv32 >> 16) & 0xff,
+		   (tkip->rx_iv32 >> 8) & 0xff,
+		   tkip->rx_iv32 & 0xff,
+		   (tkip->rx_iv16 >> 8) & 0xff,
+		   tkip->rx_iv16 & 0xff,
+		   tkip->dot11RSNAStatsTKIPReplays,
+		   tkip->dot11RSNAStatsTKIPICVErrors,
+		   tkip->dot11RSNAStatsTKIPLocalMICFailures);
 }
 
 static struct lib80211_crypto_ops rtllib_crypt_tkip = {
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_wep.c b/drivers/staging/rtl8192e/rtllib_crypt_wep.c
index 8cdf38913a33..c4df6e01ef74 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt_wep.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt_wep.c
@@ -247,12 +247,10 @@ static int prism2_wep_get_key(void *key, int len, u8 *seq, void *priv)
 }
 
 
-static char *prism2_wep_print_stats(char *p, void *priv)
+static void prism2_wep_print_stats(struct seq_file *m, void *priv)
 {
 	struct prism2_wep_data *wep = priv;
-	p += sprintf(p, "key[%d] alg=WEP len=%d\n",
-		     wep->key_idx, wep->key_len);
-	return p;
+	seq_printf(m, "key[%d] alg=WEP len=%d\n", wep->key_idx, wep->key_len);
 }
 
 static struct lib80211_crypto_ops rtllib_crypt_wep = {
diff --git a/drivers/staging/rtl8192e/rtllib_module.c b/drivers/staging/rtl8192e/rtllib_module.c
index f9dae958a5d4..84ea721d5d8e 100644
--- a/drivers/staging/rtl8192e/rtllib_module.c
+++ b/drivers/staging/rtl8192e/rtllib_module.c
@@ -208,61 +208,51 @@ static int debug = \
 			    ;
 static struct proc_dir_entry *rtllib_proc;
 
-static int show_debug_level(char *page, char **start, off_t offset,
-			    int count, int *eof, void *data)
+static int show_debug_level(struct seq_file *m, void *v)
 {
-	return snprintf(page, count, "0x%08X\n", rtllib_debug_level);
+	return seq_printf(m, "0x%08X\n", rtllib_debug_level);
 }
 
-static int store_debug_level(struct file *file, const char __user *buffer,
-			     unsigned long count, void *data)
+static ssize_t write_debug_level(struct file *file, const char __user *buffer,
+			     size_t count, loff_t *ppos)
 {
-	char buf[] = "0x00000000";
-	unsigned long len = min((unsigned long)sizeof(buf) - 1, count);
-	char *p = (char *)buf;
 	unsigned long val;
+	int err = kstrtoul_from_user(buffer, count, 0, &val);
+	if (err)
+		return err;
+	rtllib_debug_level = val;
+	return count;
+}
 
-	if (copy_from_user(buf, buffer, len))
-		return count;
-	buf[len] = 0;
-	if (p[1] == 'x' || p[1] == 'X' || p[0] == 'x' || p[0] == 'X') {
-		p++;
-		if (p[0] == 'x' || p[0] == 'X')
-			p++;
-		val = simple_strtoul(p, &p, 16);
-	} else
-		val = simple_strtoul(p, &p, 10);
-	if (p == buf)
-		printk(KERN_INFO DRV_NAME
-		       ": %s is not in hex or decimal form.\n", buf);
-	else
-		rtllib_debug_level = val;
-
-	return strnlen(buf, count);
+static int open_debug_level(struct inode *inode, struct file *file)
+{
+	return single_open(file, show_debug_level, NULL);
 }
 
+static const struct file_operations fops = {
+	.open = open_debug_level,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.write = write_debug_level
+};
+
 int __init rtllib_init(void)
 {
 	struct proc_dir_entry *e;
 
 	rtllib_debug_level = debug;
-	rtllib_proc = create_proc_entry(DRV_NAME, S_IFDIR, init_net.proc_net);
+	rtllib_proc = proc_mkdir(DRV_NAME, init_net.proc_net);
 	if (rtllib_proc == NULL) {
 		RTLLIB_ERROR("Unable to create " DRV_NAME
 				" proc directory\n");
 		return -EIO;
 	}
-	e = create_proc_entry("debug_level", S_IFREG | S_IRUGO | S_IWUSR,
-			      rtllib_proc);
+	e = proc_create("debug_level", S_IRUGO | S_IWUSR, rtllib_proc, &fops);
 	if (!e) {
 		remove_proc_entry(DRV_NAME, init_net.proc_net);
 		rtllib_proc = NULL;
 		return -EIO;
 	}
-	e->read_proc = show_debug_level;
-	e->write_proc = store_debug_level;
-	e->data = NULL;
-
 	return 0;
 }
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
index 76c56e5aed79..e0870c05a5e0 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
@@ -243,39 +243,34 @@ static int debug = \
 			    ;
 struct proc_dir_entry *ieee80211_proc;
 
-static int show_debug_level(char *page, char **start, off_t offset,
-			    int count, int *eof, void *data)
+static int show_debug_level(struct seq_file *m, void *v)
 {
-	return snprintf(page, count, "0x%08X\n", ieee80211_debug_level);
+	return seq_printf(m, "0x%08X\n", ieee80211_debug_level);
 }
 
-static int store_debug_level(struct file *file, const char *buffer,
-			     unsigned long count, void *data)
+static ssize_t write_debug_level(struct file *file, const char __user *buffer,
+			     size_t count, loff_t *ppos)
 {
-	char buf[] = "0x00000000";
-	unsigned long len = min_t(unsigned long, sizeof(buf) - 1, count);
-	char *p = (char *)buf;
 	unsigned long val;
+	int err = kstrtoul_from_user(buffer, count, 0, &val);
+	if (err)
+		return err;
+	ieee80211_debug_level = val;
+	return count;
+}
 
-	if (copy_from_user(buf, buffer, len))
-		return count;
-	buf[len] = 0;
-	if (p[1] == 'x' || p[1] == 'X' || p[0] == 'x' || p[0] == 'X') {
-		p++;
-		if (p[0] == 'x' || p[0] == 'X')
-			p++;
-		val = simple_strtoul(p, &p, 16);
-	} else
-		val = simple_strtoul(p, &p, 10);
-	if (p == buf)
-		printk(KERN_INFO DRV_NAME
-		       ": %s is not in hex or decimal form.\n", buf);
-	else
-		ieee80211_debug_level = val;
-
-	return strnlen(buf, count);
+static int open_debug_level(struct inode *inode, struct file *file)
+{
+	return single_open(file, show_debug_level, NULL);
 }
 
+static const struct file_operations fops = {
+	.open = open_debug_level,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.write = write_debug_level
+};
+
 int __init ieee80211_debug_init(void)
 {
 	struct proc_dir_entry *e;
@@ -288,17 +283,13 @@ int __init ieee80211_debug_init(void)
 				" proc directory\n");
 		return -EIO;
 	}
-	e = create_proc_entry("debug_level", S_IFREG | S_IRUGO | S_IWUSR,
-			      ieee80211_proc);
+	e = proc_create("debug_level", S_IRUGO | S_IWUSR, 
+			      ieee80211_proc, &fops);
 	if (!e) {
 		remove_proc_entry(DRV_NAME, init_net.proc_net);
 		ieee80211_proc = NULL;
 		return -EIO;
 	}
-	e->read_proc = show_debug_level;
-	e->write_proc = store_debug_level;
-	e->data = NULL;
-
 	return 0;
 }
 
diff --git a/drivers/staging/rtl8192u/ieee80211/proc.c b/drivers/staging/rtl8192u/ieee80211/proc.c
index 6eda928e4090..c426dfdd9fdd 100644
--- a/drivers/staging/rtl8192u/ieee80211/proc.c
+++ b/drivers/staging/rtl8192u/ieee80211/proc.c
@@ -99,7 +99,7 @@ static int crypto_info_open(struct inode *inode, struct file *file)
 	return seq_open(file, &crypto_seq_ops);
 }
 
-static struct file_operations proc_crypto_ops = {
+static const struct file_operations proc_crypto_ops = {
 	.open		= crypto_info_open,
 	.read		= seq_read,
 	.llseek		= seq_lseek,
@@ -108,9 +108,5 @@ static struct file_operations proc_crypto_ops = {
 
 void __init crypto_init_proc(void)
 {
-	struct proc_dir_entry *proc;
-
-	proc = create_proc_entry("crypto", 0, NULL);
-	if (proc)
-		proc->proc_fops = &proc_crypto_ops;
+	proc_create("crypto", 0, NULL, &proc_crypto_ops);
 }
diff --git a/drivers/staging/rtl8192u/r8192U.h b/drivers/staging/rtl8192u/r8192U.h
index e538e026b512..bedeb330ad4f 100644
--- a/drivers/staging/rtl8192u/r8192U.h
+++ b/drivers/staging/rtl8192u/r8192U.h
@@ -946,7 +946,6 @@ typedef struct r8192_priv {
 	/*stats*/
 	struct Stats stats;
 	struct iw_statistics wstats;
-	struct proc_dir_entry *dir_dev;
 
 	/*RX stuff*/
 //	u32 *rxring;
diff --git a/drivers/staging/rtl8192u/r8192U_core.c b/drivers/staging/rtl8192u/r8192U_core.c
index f7de2f6d49a5..71f5cde9ed1c 100644
--- a/drivers/staging/rtl8192u/r8192U_core.c
+++ b/drivers/staging/rtl8192u/r8192U_core.c
@@ -71,6 +71,8 @@ double __extendsfdf2(float a) {return a;}
 //#include "r8192xU_phyreg.h"
 #include <linux/usb.h>
 #include <linux/slab.h>
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 // FIXME: check if 2.6.7 is ok
 
 #ifdef CONFIG_RTL8192_PM
@@ -472,103 +474,73 @@ void watch_dog_timer_callback(unsigned long data);
 
 static struct proc_dir_entry *rtl8192_proc;
 
-static int proc_get_stats_ap(char *page, char **start, off_t offset, int count,
-							int *eof, void *data)
+static int proc_get_stats_ap(struct seq_file *m, void *v)
 {
-	struct net_device *dev = data;
+	struct net_device *dev = m->private;
 	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
 	struct ieee80211_device *ieee = priv->ieee80211;
 	struct ieee80211_network *target;
 
-	int len = 0;
-
 	list_for_each_entry(target, &ieee->network_list, list) {
-
-		len += snprintf(page + len, count - len, "%s ", target->ssid);
-
+		const char *wpa = "non_WPA";
 		if (target->wpa_ie_len > 0 || target->rsn_ie_len > 0)
-			len += snprintf(page + len, count - len, "WPA\n");
-		else
-			len += snprintf(page + len, count - len, "non_WPA\n");
+			wpa = "WPA";
+
+		seq_printf(m, "%s %s\n", target->ssid, wpa);
 	}
 
-	*eof = 1;
-	return len;
+	return 0;
 }
 
-static int proc_get_registers(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
+static int proc_get_registers(struct seq_file *m, void *v)
 {
-	struct net_device *dev = data;
-//	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct net_device *dev = m->private;
+	int i,n, max = 0xff;
 
-	int len = 0;
-	int i,n;
-
-	int max=0xff;
-
-	/* This dump the current register page */
-	len += snprintf(page + len, count - len,
-			"\n####################page 0##################\n ");
+	seq_puts(m, "\n####################page 0##################\n ");
 
 	for (n=0;n<=max;) {
 		//printk( "\nD: %2x> ", n);
-		len += snprintf(page + len, count - len,
-			"\nD:  %2x > ",n);
+		seq_printf(m, "\nD:  %2x > ",n);
 
 		for (i=0;i<16 && n<=max;i++,n++)
-			len += snprintf(page + len, count - len,
-					"%2x ",read_nic_byte(dev,0x000|n));
+			seq_printf(m, "%2x ",read_nic_byte(dev,0x000|n));
 
 		//	printk("%2x ",read_nic_byte(dev,n));
 	}
-	len += snprintf(page + len, count - len,
-			"\n####################page 1##################\n ");
+
+	seq_puts(m, "\n####################page 1##################\n ");
 	for (n=0;n<=max;) {
 		//printk( "\nD: %2x> ", n);
-		len += snprintf(page + len, count - len,
-				"\nD:  %2x > ",n);
+		seq_printf(m, "\nD:  %2x > ",n);
 
 		for (i=0;i<16 && n<=max;i++,n++)
-			len += snprintf(page + len, count - len,
-					"%2x ",read_nic_byte(dev,0x100|n));
+			seq_printf(m, "%2x ",read_nic_byte(dev,0x100|n));
 
 		//      printk("%2x ",read_nic_byte(dev,n));
 	}
-	len += snprintf(page + len, count - len,
-			"\n####################page 3##################\n ");
+
+	seq_puts(m, "\n####################page 3##################\n ");
 	for (n=0;n<=max;) {
 		//printk( "\nD: %2x> ", n);
-		len += snprintf(page + len, count - len,
-			"\nD:  %2x > ",n);
+		seq_printf(m, "\nD:  %2x > ",n);
 
 		for(i=0;i<16 && n<=max;i++,n++)
-			len += snprintf(page + len, count - len,
-					"%2x ",read_nic_byte(dev,0x300|n));
+			seq_printf(m, "%2x ",read_nic_byte(dev,0x300|n));
 
 		//      printk("%2x ",read_nic_byte(dev,n));
 	}
 
-	len += snprintf(page + len, count - len,"\n");
-	*eof = 1;
-	return len;
+	seq_putc(m, '\n');
+	return 0;
 }
 
-
-
-
-
-static int proc_get_stats_tx(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
+static int proc_get_stats_tx(struct seq_file *m, void *v)
 {
-	struct net_device *dev = data;
+	struct net_device *dev = m->private;
 	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
 
-	int len = 0;
-
-	len += snprintf(page + len, count - len,
+	seq_printf(m,
 		"TX VI priority ok int: %lu\n"
 		"TX VI priority error int: %lu\n"
 		"TX VO priority ok int: %lu\n"
@@ -629,22 +601,15 @@ static int proc_get_stats_tx(char *page, char **start,
 //		priv->stats.txbeaconerr
 		);
 
-	*eof = 1;
-	return len;
+	return 0;
 }
 
-
-
-static int proc_get_stats_rx(char *page, char **start,
-			  off_t offset, int count,
-			  int *eof, void *data)
+static int proc_get_stats_rx(struct seq_file *m, void *v)
 {
-	struct net_device *dev = data;
+	struct net_device *dev = m->private;
 	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
 
-	int len = 0;
-
-	len += snprintf(page + len, count - len,
+	seq_printf(m,
 		"RX packets: %lu\n"
 		"RX urb status error: %lu\n"
 		"RX invalid urb error: %lu\n",
@@ -652,9 +617,9 @@ static int proc_get_stats_rx(char *page, char **start,
 		priv->stats.rxstaterr,
 		priv->stats.rxurberr);
 
-	*eof = 1;
-	return len;
+	return 0;
 }
+
 void rtl8192_proc_module_init(void)
 {
 	RT_TRACE(COMP_INIT, "Initializing proc filesystem");
@@ -667,74 +632,70 @@ void rtl8192_proc_module_remove(void)
 	remove_proc_entry(RTL819xU_MODULE_NAME, init_net.proc_net);
 }
 
-
-void rtl8192_proc_remove_one(struct net_device *dev)
+/*
+ * seq_file wrappers for procfile show routines.
+ */
+static int rtl8192_proc_open(struct inode *inode, struct file *file)
 {
-	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
+	struct net_device *dev = proc_get_parent_data(inode);
+	int (*show)(struct seq_file *, void *) = PDE_DATA(inode);
 
-
-	if (priv->dir_dev) {
-	//	remove_proc_entry("stats-hw", priv->dir_dev);
-		remove_proc_entry("stats-tx", priv->dir_dev);
-		remove_proc_entry("stats-rx", priv->dir_dev);
-	//	remove_proc_entry("stats-ieee", priv->dir_dev);
-		remove_proc_entry("stats-ap", priv->dir_dev);
-		remove_proc_entry("registers", priv->dir_dev);
-	//	remove_proc_entry("cck-registers",priv->dir_dev);
-	//	remove_proc_entry("ofdm-registers",priv->dir_dev);
-		//remove_proc_entry(dev->name, rtl8192_proc);
-		remove_proc_entry("wlan0", rtl8192_proc);
-		priv->dir_dev = NULL;
-	}
+	return single_open(file, show, dev);
 }
 
+static const struct file_operations rtl8192_proc_fops = {
+	.open		= rtl8192_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 
-void rtl8192_proc_init_one(struct net_device *dev)
-{
-	struct proc_dir_entry *e;
-	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
-	priv->dir_dev = proc_mkdir(dev->name, rtl8192_proc);
-	if (!priv->dir_dev) {
-		RT_TRACE(COMP_ERR, "Unable to initialize /proc/net/rtl8192/%s\n",
-		      dev->name);
-		return;
-	}
-	e = create_proc_read_entry("stats-rx", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_stats_rx, dev);
-
-	if (!e) {
-		RT_TRACE(COMP_ERR,"Unable to initialize "
-		      "/proc/net/rtl8192/%s/stats-rx\n",
-		      dev->name);
-	}
-
+/*
+ * Table of proc files we need to create.
+ */
+struct rtl8192_proc_file {
+	char name[12];
+	int (*show)(struct seq_file *, void *);
+};
 
-	e = create_proc_read_entry("stats-tx", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_stats_tx, dev);
+static const struct rtl8192_proc_file rtl8192_proc_files[] = {
+	{ "stats-rx",	&proc_get_stats_rx },
+	{ "stats-tx",	&proc_get_stats_tx },
+	{ "stats-ap",	&proc_get_stats_ap },
+	{ "registers",	&proc_get_registers },
+	{ "" }
+};
 
-	if (!e) {
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/stats-tx\n",
-		      dev->name);
-	}
+void rtl8192_proc_init_one(struct net_device *dev)
+{
+	const struct rtl8192_proc_file *f;
+	struct proc_dir_entry *dir;
 
-	e = create_proc_read_entry("stats-ap", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_stats_ap, dev);
+	if (rtl8192_proc) {
+		dir = proc_mkdir_data(dev->name, 0, rtl8192_proc, dev);
+		if (!dir) {
+			RT_TRACE(COMP_ERR, "Unable to initialize /proc/net/rtl8192/%s\n",
+				 dev->name);
+			return;
+		}
 
-	if (!e) {
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/stats-ap\n",
-		      dev->name);
+		for (f = rtl8192_proc_files; f->name[0]; f++) {
+			if (!proc_create_data(f->name, S_IFREG | S_IRUGO, dir,
+					      &rtl8192_proc_fops, f->show)) {
+				RT_TRACE(COMP_ERR, "Unable to initialize "
+					 "/proc/net/rtl8192/%s/%s\n",
+					 dev->name, f->name);
+				return;
+			}
+		}
 	}
+}
 
-	e = create_proc_read_entry("registers", S_IFREG | S_IRUGO,
-				   priv->dir_dev, proc_get_registers, dev);
-	if (!e) {
-		RT_TRACE(COMP_ERR, "Unable to initialize "
-		      "/proc/net/rtl8192/%s/registers\n",
-		      dev->name);
-	}
+void rtl8192_proc_remove_one(struct net_device *dev)
+{
+	remove_proc_subtree(dev->name, rtl8192_proc);
 }
+
 /****************************************************************************
    -----------------------------MISC STUFF-------------------------
 *****************************************************************************/
diff --git a/drivers/staging/rts5139/rts51x_scsi.c b/drivers/staging/rts5139/rts51x_scsi.c
index b58f1dfe006d..610870546406 100644
--- a/drivers/staging/rts5139/rts51x_scsi.c
+++ b/drivers/staging/rts5139/rts51x_scsi.c
@@ -1968,18 +1968,16 @@ int slave_configure(struct scsi_device *sdev)
 
 /* we use this macro to help us write into the buffer */
 #undef SPRINTF
-#define SPRINTF(args...) \
-	do { if (pos < buffer+length) pos += sprintf(pos, ## args); } while (0)
+#define SPRINTF(args...) seq_printf(m, ##args)
 
-int proc_info(struct Scsi_Host *host, char *buffer,
-	      char **start, off_t offset, int length, int inout)
+static int write_info(struct Scsi_Host *host, char *buffer, int length)
 {
-	char *pos = buffer;
-
 	/* if someone is sending us data, just throw it away */
-	if (inout)
-		return length;
+	return length;
+}
 
+static int show_info(struct seq_file *m, struct Scsi_Host *host)
+{
 	/* print the controller name */
 	SPRINTF("   Host scsi%d: %s\n", host->host_no, RTS51X_NAME);
 
@@ -1988,18 +1986,7 @@ int proc_info(struct Scsi_Host *host, char *buffer,
 	SPRINTF("      Product: RTS51xx USB Card Reader\n");
 	SPRINTF("      Version: %s\n", DRIVER_VERSION);
 	SPRINTF("        Build: %s\n", __TIME__);
-
-	/*
-	 * Calculate start of next buffer, and return value.
-	 */
-	*start = buffer + offset;
-
-	if ((pos - buffer) < offset)
-		return 0;
-	else if ((pos - buffer - offset) < length)
-		return pos - buffer - offset;
-	else
-		return length;
+	return 0;
 }
 
 /* queue a command */
@@ -2100,7 +2087,8 @@ struct scsi_host_template rts51x_host_template = {
 	/* basic userland interface stuff */
 	.name = RTS51X_NAME,
 	.proc_name = RTS51X_NAME,
-	.proc_info = proc_info,
+	.show_info = show_info,
+	.write_info = write_info,
 	.info = rts5139_info,
 
 	/* command interface -- queued only */
diff --git a/drivers/staging/rts5139/rts51x_scsi.h b/drivers/staging/rts5139/rts51x_scsi.h
index 3a5213611d21..1a0d70566186 100644
--- a/drivers/staging/rts5139/rts51x_scsi.h
+++ b/drivers/staging/rts5139/rts51x_scsi.h
@@ -147,8 +147,6 @@ struct scsi_cmnd;
 
 int slave_alloc(struct scsi_device *sdev);
 int slave_configure(struct scsi_device *sdev);
-int proc_info(struct Scsi_Host *host, char *buffer,
-	      char **start, off_t offset, int length, int inout);
 int queuecommand(struct Scsi_Host *, struct scsi_cmnd *);
 int command_abort(struct scsi_cmnd *srb);
 int bus_reset(struct scsi_cmnd *srb);
diff --git a/drivers/staging/silicom/Makefile b/drivers/staging/silicom/Makefile
index 80e6d12d156b..ca8359481c48 100644
--- a/drivers/staging/silicom/Makefile
+++ b/drivers/staging/silicom/Makefile
@@ -4,6 +4,3 @@
 
 obj-$(CONFIG_BPCTL) += bpctl_mod.o
 obj-$(CONFIG_SBYPASS) += bypasslib/
-
-
-bpctl_mod-objs := bp_mod.o  bp_proc.o
diff --git a/drivers/staging/silicom/bp_proc.c b/drivers/staging/silicom/bp_proc.c
deleted file mode 100644
index a01ca97b7665..000000000000
--- a/drivers/staging/silicom/bp_proc.c
+++ /dev/null
@@ -1,1327 +0,0 @@
-/******************************************************************************/
-/*                                                                            */
-/* Copyright (c) 2004-2006 Silicom, Ltd                                       */
-/* All rights reserved.                                                       */
-/*                                                                            */
-/* This program is free software; you can redistribute it and/or modify       */
-/* it under the terms of the GNU General Public License as published by       */
-/* the Free Software Foundation, located in the file LICENSE.                 */
-/*                                                                            */
-/*                                                                            */
-/******************************************************************************/
-
-#if defined(CONFIG_SMP) && !defined(__SMP__)
-#define __SMP__
-#endif
-
-#include <linux/proc_fs.h>
-#include <linux/netdevice.h>
-#include <asm/uaccess.h>
-/* #include <linux/smp_lock.h> */
-#include "bp_mod.h"
-
-#define BP_PROC_DIR "bypass"
-/* #define BYPASS_SUPPORT "bypass" */
-
-#ifdef BYPASS_SUPPORT
-
-#define GPIO6_SET_ENTRY_SD           "gpio6_set"
-#define GPIO6_CLEAR_ENTRY_SD         "gpio6_clear"
-
-#define GPIO7_SET_ENTRY_SD           "gpio7_set"
-#define GPIO7_CLEAR_ENTRY_SD         "gpio7_clear"
-
-#define PULSE_SET_ENTRY_SD            "pulse_set"
-#define ZERO_SET_ENTRY_SD            "zero_set"
-#define PULSE_GET1_ENTRY_SD            "pulse_get1"
-#define PULSE_GET2_ENTRY_SD            "pulse_get2"
-
-#define CMND_ON_ENTRY_SD              "cmnd_on"
-#define CMND_OFF_ENTRY_SD             "cmnd_off"
-#define RESET_CONT_ENTRY_SD           "reset_cont"
-
- /*COMMANDS*/
-#define BYPASS_INFO_ENTRY_SD     "bypass_info"
-#define BYPASS_SLAVE_ENTRY_SD    "bypass_slave"
-#define BYPASS_CAPS_ENTRY_SD     "bypass_caps"
-#define WD_SET_CAPS_ENTRY_SD     "wd_set_caps"
-#define BYPASS_ENTRY_SD          "bypass"
-#define BYPASS_CHANGE_ENTRY_SD   "bypass_change"
-#define BYPASS_WD_ENTRY_SD       "bypass_wd"
-#define WD_EXPIRE_TIME_ENTRY_SD  "wd_expire_time"
-#define RESET_BYPASS_WD_ENTRY_SD "reset_bypass_wd"
-#define DIS_BYPASS_ENTRY_SD      "dis_bypass"
-#define BYPASS_PWUP_ENTRY_SD     "bypass_pwup"
-#define BYPASS_PWOFF_ENTRY_SD     "bypass_pwoff"
-#define STD_NIC_ENTRY_SD         "std_nic"
-#define STD_NIC_ENTRY_SD         "std_nic"
-#define TAP_ENTRY_SD             "tap"
-#define TAP_CHANGE_ENTRY_SD      "tap_change"
-#define DIS_TAP_ENTRY_SD         "dis_tap"
-#define TAP_PWUP_ENTRY_SD        "tap_pwup"
-#define TWO_PORT_LINK_ENTRY_SD   "two_port_link"
-#define WD_EXP_MODE_ENTRY_SD     "wd_exp_mode"
-#define WD_AUTORESET_ENTRY_SD    "wd_autoreset"
-#define TPL_ENTRY_SD             "tpl"
-#define WAIT_AT_PWUP_ENTRY_SD    "wait_at_pwup"
-#define HW_RESET_ENTRY_SD        "hw_reset"
-#define DISC_ENTRY_SD             "disc"
-#define DISC_CHANGE_ENTRY_SD      "disc_change"
-#define DIS_DISC_ENTRY_SD         "dis_disc"
-#define DISC_PWUP_ENTRY_SD        "disc_pwup"
-
-static struct proc_dir_entry *bp_procfs_dir;
-
-static struct proc_dir_entry *proc_getdir(char *name,
-					  struct proc_dir_entry *proc_dir)
-{
-	struct proc_dir_entry *pde = proc_dir;
-	for (pde = pde->subdir; pde; pde = pde->next) {
-		if (pde->namelen && (strcmp(name, pde->name) == 0)) {
-			/* directory exists */
-			break;
-		}
-	}
-	if (pde == (struct proc_dir_entry *)0) {
-		/* create the directory */
-		pde = create_proc_entry(name, S_IFDIR, proc_dir);
-		if (pde == (struct proc_dir_entry *)0)
-			return pde;
-	}
-	return pde;
-}
-
-int
-bypass_proc_create_entry_sd(struct pfs_unit *pfs_unit_curr,
-			    char *proc_name,
-			    write_proc_t *write_proc,
-			    read_proc_t *read_proc,
-			    struct proc_dir_entry *parent_pfs, void *data)
-{
-	strcpy(pfs_unit_curr->proc_name, proc_name);
-	pfs_unit_curr->proc_entry = create_proc_entry(pfs_unit_curr->proc_name,
-						      S_IFREG | S_IRUSR |
-						      S_IWUSR | S_IRGRP |
-						      S_IROTH, parent_pfs);
-	if (pfs_unit_curr->proc_entry == 0)
-		return -1;
-
-	pfs_unit_curr->proc_entry->read_proc = read_proc;
-	pfs_unit_curr->proc_entry->write_proc = write_proc;
-	pfs_unit_curr->proc_entry->data = data;
-
-	return 0;
-
-}
-
-int
-get_bypass_info_pfs(char *page, char **start, off_t off, int count,
-		    int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-	int len = 0;
-
-	len += sprintf(page, "Name\t\t\t%s\n", pbp_device_block->bp_name);
-	len +=
-	    sprintf(page + len, "Firmware version\t0x%x\n",
-		    pbp_device_block->bp_fw_ver);
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_bypass_slave_pfs(char *page, char **start, off_t off, int count,
-		     int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	struct pci_dev *pci_slave_dev = pbp_device_block->bp_slave;
-	struct net_device *net_slave_dev;
-	int len = 0;
-
-	if (is_bypass_fn(pbp_device_block)) {
-		net_slave_dev = pci_get_drvdata(pci_slave_dev);
-		if (net_slave_dev)
-			len = sprintf(page, "%s\n", net_slave_dev->name);
-		else
-			len = sprintf(page, "fail\n");
-	} else
-		len = sprintf(page, "fail\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_bypass_caps_pfs(char *page, char **start, off_t off, int count,
-		    int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_bypass_caps_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "-1\n");
-	else
-		len = sprintf(page, "0x%x\n", ret);
-	*eof = 1;
-	return len;
-
-}
-
-int
-get_wd_set_caps_pfs(char *page, char **start, off_t off, int count,
-		    int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_wd_set_caps_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "-1\n");
-	else
-		len = sprintf(page, "0x%x\n", ret);
-	*eof = 1;
-	return len;
-}
-
-int
-set_bypass_pfs(struct file *file, const char *buffer,
-	       unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int bypass_param = 0, length = 0;
-
-	if (count > (sizeof(kbuf) - 1))
-		return -1;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		bypass_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		bypass_param = 0;
-
-	set_bypass_fn(pbp_device_block, bypass_param);
-
-	return count;
-}
-
-int
-set_tap_pfs(struct file *file, const char *buffer,
-	    unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tap_param = 0, length = 0;
-
-	if (count > (sizeof(kbuf) - 1))
-		return -1;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tap_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tap_param = 0;
-
-	set_tap_fn(pbp_device_block, tap_param);
-
-	return count;
-}
-
-int
-set_disc_pfs(struct file *file, const char *buffer,
-	     unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tap_param = 0, length = 0;
-
-	if (count > (sizeof(kbuf) - 1))
-		return -1;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tap_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tap_param = 0;
-
-	set_disc_fn(pbp_device_block, tap_param);
-
-	return count;
-}
-
-int
-get_bypass_pfs(char *page, char **start, off_t off, int count,
-	       int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_bypass_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 1)
-		len = sprintf(page, "on\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_tap_pfs(char *page, char **start, off_t off, int count,
-	    int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_tap_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 1)
-		len = sprintf(page, "on\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_disc_pfs(char *page, char **start, off_t off, int count,
-	     int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_disc_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 1)
-		len = sprintf(page, "on\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_bypass_change_pfs(char *page, char **start, off_t off, int count,
-		      int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_bypass_change_fn(pbp_device_block);
-	if (ret == 1)
-		len = sprintf(page, "on\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-	else
-		len = sprintf(page, "fail\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_tap_change_pfs(char *page, char **start, off_t off, int count,
-		   int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_tap_change_fn(pbp_device_block);
-	if (ret == 1)
-		len = sprintf(page, "on\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-	else
-		len = sprintf(page, "fail\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_disc_change_pfs(char *page, char **start, off_t off, int count,
-		    int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_disc_change_fn(pbp_device_block);
-	if (ret == 1)
-		len = sprintf(page, "on\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-	else
-		len = sprintf(page, "fail\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-set_bypass_wd_pfs(struct file *file, const char *buffer,
-		  unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	unsigned int timeout = 0;
-	char *timeout_ptr = kbuf;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	timeout_ptr = kbuf;
-	timeout = atoi(&timeout_ptr);
-
-	set_bypass_wd_fn(pbp_device_block, timeout);
-
-	return count;
-}
-
-int
-get_bypass_wd_pfs(char *page, char **start, off_t off, int count,
-		  int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0, timeout = 0;
-
-	ret = get_bypass_wd_fn(pbp_device_block, &timeout);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (timeout == -1)
-		len = sprintf(page, "unknown\n");
-	else if (timeout == 0)
-		len = sprintf(page, "disable\n");
-	else
-		len = sprintf(page, "%d\n", timeout);
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_wd_expire_time_pfs(char *page, char **start, off_t off, int count,
-		       int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0, timeout = 0;
-
-	ret = get_wd_expire_time_fn(pbp_device_block, &timeout);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (timeout == -1)
-		len = sprintf(page, "expire\n");
-	else if (timeout == 0)
-		len = sprintf(page, "disable\n");
-
-	else
-		len = sprintf(page, "%d\n", timeout);
-	*eof = 1;
-	return len;
-}
-
-int
-get_tpl_pfs(char *page, char **start, off_t off, int count,
-	    int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_tpl_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 1)
-		len = sprintf(page, "on\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-
-	*eof = 1;
-	return len;
-}
-
-#ifdef PMC_FIX_FLAG
-int
-get_wait_at_pwup_pfs(char *page, char **start, off_t off, int count,
-		     int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_bp_wait_at_pwup_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 1)
-		len = sprintf(page, "on\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_hw_reset_pfs(char *page, char **start, off_t off, int count,
-		 int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_bp_hw_reset_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 1)
-		len = sprintf(page, "on\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-
-	*eof = 1;
-	return len;
-}
-
-#endif				/*PMC_WAIT_FLAG */
-
-int
-reset_bypass_wd_pfs(char *page, char **start, off_t off, int count,
-		    int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = reset_bypass_wd_timer_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 0)
-		len = sprintf(page, "disable\n");
-	else if (ret == 1)
-		len = sprintf(page, "success\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-set_dis_bypass_pfs(struct file *file, const char *buffer,
-		   unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int bypass_param = 0, length = 0;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		bypass_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		bypass_param = 0;
-
-	set_dis_bypass_fn(pbp_device_block, bypass_param);
-
-	return count;
-}
-
-int
-set_dis_tap_pfs(struct file *file, const char *buffer,
-		unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tap_param = 0, length = 0;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tap_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tap_param = 0;
-
-	set_dis_tap_fn(pbp_device_block, tap_param);
-
-	return count;
-}
-
-int
-set_dis_disc_pfs(struct file *file, const char *buffer,
-		 unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tap_param = 0, length = 0;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tap_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tap_param = 0;
-
-	set_dis_disc_fn(pbp_device_block, tap_param);
-
-	return count;
-}
-
-int
-get_dis_bypass_pfs(char *page, char **start, off_t off, int count,
-		   int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_dis_bypass_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-	else
-		len = sprintf(page, "on\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_dis_tap_pfs(char *page, char **start, off_t off, int count,
-		int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_dis_tap_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-	else
-		len = sprintf(page, "on\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_dis_disc_pfs(char *page, char **start, off_t off, int count,
-		 int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_dis_disc_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-	else
-		len = sprintf(page, "on\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-set_bypass_pwup_pfs(struct file *file, const char *buffer,
-		    unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int bypass_param = 0, length = 0;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		bypass_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		bypass_param = 0;
-
-	set_bypass_pwup_fn(pbp_device_block, bypass_param);
-
-	return count;
-}
-
-int
-set_bypass_pwoff_pfs(struct file *file, const char *buffer,
-		     unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int bypass_param = 0, length = 0;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		bypass_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		bypass_param = 0;
-
-	set_bypass_pwoff_fn(pbp_device_block, bypass_param);
-
-	return count;
-}
-
-int
-set_tap_pwup_pfs(struct file *file, const char *buffer,
-		 unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tap_param = 0, length = 0;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tap_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tap_param = 0;
-
-	set_tap_pwup_fn(pbp_device_block, tap_param);
-
-	return count;
-}
-
-int
-set_disc_pwup_pfs(struct file *file, const char *buffer,
-		  unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tap_param = 0, length = 0;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tap_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tap_param = 0;
-
-	set_disc_pwup_fn(pbp_device_block, tap_param);
-
-	return count;
-}
-
-int
-get_bypass_pwup_pfs(char *page, char **start, off_t off, int count,
-		    int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_bypass_pwup_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-	else
-		len = sprintf(page, "on\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_bypass_pwoff_pfs(char *page, char **start, off_t off, int count,
-		     int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_bypass_pwoff_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-	else
-		len = sprintf(page, "on\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_tap_pwup_pfs(char *page, char **start, off_t off, int count,
-		 int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_tap_pwup_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-	else
-		len = sprintf(page, "on\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_disc_pwup_pfs(char *page, char **start, off_t off, int count,
-		  int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_disc_pwup_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-	else
-		len = sprintf(page, "on\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-set_std_nic_pfs(struct file *file, const char *buffer,
-		unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int bypass_param = 0, length = 0;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		bypass_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		bypass_param = 0;
-
-	set_std_nic_fn(pbp_device_block, bypass_param);
-
-	return count;
-}
-
-int
-get_std_nic_pfs(char *page, char **start, off_t off, int count,
-		int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_std_nic_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-	else
-		len = sprintf(page, "on\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_wd_exp_mode_pfs(char *page, char **start, off_t off, int count,
-		    int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_wd_exp_mode_fn(pbp_device_block);
-	if (ret == 1)
-		len = sprintf(page, "tap\n");
-	else if (ret == 0)
-		len = sprintf(page, "bypass\n");
-	else if (ret == 2)
-		len = sprintf(page, "disc\n");
-
-	else
-		len = sprintf(page, "fail\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-set_wd_exp_mode_pfs(struct file *file, const char *buffer,
-		    unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int bypass_param = 0, length = 0;
-
-	if (count > (sizeof(kbuf) - 1))
-		return -1;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "tap") == 0)
-		bypass_param = 1;
-	else if (strcmp(kbuf, "bypass") == 0)
-		bypass_param = 0;
-	else if (strcmp(kbuf, "disc") == 0)
-		bypass_param = 2;
-
-	set_wd_exp_mode_fn(pbp_device_block, bypass_param);
-
-	return count;
-}
-
-int
-get_wd_autoreset_pfs(char *page, char **start, off_t off, int count,
-		     int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_wd_autoreset_fn(pbp_device_block);
-	if (ret >= 0)
-		len = sprintf(page, "%d\n", ret);
-	else
-		len = sprintf(page, "fail\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-set_wd_autoreset_pfs(struct file *file, const char *buffer,
-		     unsigned long count, void *data)
-{
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-	u32 timeout = 0;
-	char *timeout_ptr = kbuf;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	timeout_ptr = kbuf;
-	timeout = atoi(&timeout_ptr);
-
-	set_wd_autoreset_fn(pbp_device_block, timeout);
-
-	return count;
-}
-
-int
-set_tpl_pfs(struct file *file, const char *buffer,
-	    unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tpl_param = 0, length = 0;
-
-	if (count > (sizeof(kbuf) - 1))
-		return -1;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tpl_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tpl_param = 0;
-
-	set_tpl_fn(pbp_device_block, tpl_param);
-
-	return count;
-}
-
-#ifdef PMC_FIX_FLAG
-int
-set_wait_at_pwup_pfs(struct file *file, const char *buffer,
-		     unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tpl_param = 0, length = 0;
-
-	if (count > (sizeof(kbuf) - 1))
-		return -1;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tpl_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tpl_param = 0;
-
-	set_bp_wait_at_pwup_fn(pbp_device_block, tpl_param);
-
-	return count;
-}
-
-int
-set_hw_reset_pfs(struct file *file, const char *buffer,
-		 unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tpl_param = 0, length = 0;
-
-	if (count > (sizeof(kbuf) - 1))
-		return -1;
-
-	if (copy_from_user(&kbuf, buffer, count))
-		return -1;
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tpl_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tpl_param = 0;
-
-	set_bp_hw_reset_fn(pbp_device_block, tpl_param);
-
-	return count;
-}
-
-#endif				/*PMC_FIX_FLAG */
-
-int bypass_proc_create_dev_sd(bpctl_dev_t *pbp_device_block)
-{
-	struct bypass_pfs_sd *current_pfs = &(pbp_device_block->bypass_pfs_set);
-	static struct proc_dir_entry *procfs_dir;
-	int ret = 0;
-
-	sprintf(current_pfs->dir_name, "bypass_%s", dev->name);
-
-	if (!bp_procfs_dir)
-		return -1;
-
-	/* create device proc dir */
-	procfs_dir = proc_getdir(current_pfs->dir_name, bp_procfs_dir);
-	if (procfs_dir == 0) {
-		printk(KERN_DEBUG "Could not create procfs directory %s\n",
-		       current_pfs->dir_name);
-		return -1;
-	}
-	current_pfs->bypass_entry = procfs_dir;
-
-	if (bypass_proc_create_entry(&(current_pfs->bypass_info), BYPASS_INFO_ENTRY_SD, NULL,	/* write */
-				     get_bypass_info_pfs,	/* read  */
-				     procfs_dir, pbp_device_block))
-		ret = -1;
-
-	if (pbp_device_block->bp_caps & SW_CTL_CAP) {
-
-		/* Create set param proc's */
-		if (bypass_proc_create_entry_sd(&(current_pfs->bypass_slave), BYPASS_SLAVE_ENTRY_SD, NULL,	/* write */
-						get_bypass_slave_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
-		if (bypass_proc_create_entry_sd(&(current_pfs->bypass_caps), BYPASS_CAPS_ENTRY_SD, NULL,	/* write */
-						get_bypass_caps_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
-		if (bypass_proc_create_entry_sd(&(current_pfs->wd_set_caps), WD_SET_CAPS_ENTRY_SD, NULL,	/* write */
-						get_wd_set_caps_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-		if (bypass_proc_create_entry_sd(&(current_pfs->bypass_wd), BYPASS_WD_ENTRY_SD, set_bypass_wd_pfs,	/* write */
-						get_bypass_wd_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
-		if (bypass_proc_create_entry_sd(&(current_pfs->wd_expire_time), WD_EXPIRE_TIME_ENTRY_SD, NULL,	/* write */
-						get_wd_expire_time_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
-		if (bypass_proc_create_entry_sd(&(current_pfs->reset_bypass_wd), RESET_BYPASS_WD_ENTRY_SD, NULL,	/* write */
-						reset_bypass_wd_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
-		if (bypass_proc_create_entry_sd(&(current_pfs->std_nic), STD_NIC_ENTRY_SD, set_std_nic_pfs,	/* write */
-						get_std_nic_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
-		if (pbp_device_block->bp_caps & BP_CAP) {
-			if (bypass_proc_create_entry_sd(&(current_pfs->bypass), BYPASS_ENTRY_SD, set_bypass_pfs,	/* write */
-							get_bypass_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->dis_bypass), DIS_BYPASS_ENTRY_SD, set_dis_bypass_pfs,	/* write */
-							get_dis_bypass_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->bypass_pwup), BYPASS_PWUP_ENTRY_SD, set_bypass_pwup_pfs,	/* write */
-							get_bypass_pwup_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-			if (bypass_proc_create_entry_sd(&(current_pfs->bypass_pwoff), BYPASS_PWOFF_ENTRY_SD, set_bypass_pwoff_pfs,	/* write */
-							get_bypass_pwoff_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->bypass_change), BYPASS_CHANGE_ENTRY_SD, NULL,	/* write */
-							get_bypass_change_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-		}
-
-		if (pbp_device_block->bp_caps & TAP_CAP) {
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->tap), TAP_ENTRY_SD, set_tap_pfs,	/* write */
-							get_tap_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->dis_tap), DIS_TAP_ENTRY_SD, set_dis_tap_pfs,	/* write */
-							get_dis_tap_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->tap_pwup), TAP_PWUP_ENTRY_SD, set_tap_pwup_pfs,	/* write */
-							get_tap_pwup_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->tap_change), TAP_CHANGE_ENTRY_SD, NULL,	/* write */
-							get_tap_change_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-		}
-		if (pbp_device_block->bp_caps & DISC_CAP) {
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->tap), DISC_ENTRY_SD, set_disc_pfs,	/* write */
-							get_disc_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-#if 1
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->dis_tap), DIS_DISC_ENTRY_SD, set_dis_disc_pfs,	/* write */
-							get_dis_disc_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-#endif
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->tap_pwup), DISC_PWUP_ENTRY_SD, set_disc_pwup_pfs,	/* write */
-							get_disc_pwup_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->tap_change), DISC_CHANGE_ENTRY_SD, NULL,	/* write */
-							get_disc_change_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-		}
-
-		if (bypass_proc_create_entry_sd(&(current_pfs->wd_exp_mode), WD_EXP_MODE_ENTRY_SD, set_wd_exp_mode_pfs,	/* write */
-						get_wd_exp_mode_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
-		if (bypass_proc_create_entry_sd(&(current_pfs->wd_autoreset), WD_AUTORESET_ENTRY_SD, set_wd_autoreset_pfs,	/* write */
-						get_wd_autoreset_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-		if (bypass_proc_create_entry_sd(&(current_pfs->tpl), TPL_ENTRY_SD, set_tpl_pfs,	/* write */
-						get_tpl_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-#ifdef PMC_FIX_FLAG
-		if (bypass_proc_create_entry_sd(&(current_pfs->tpl), WAIT_AT_PWUP_ENTRY_SD, set_wait_at_pwup_pfs,	/* write */
-						get_wait_at_pwup_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-		if (bypass_proc_create_entry_sd(&(current_pfs->tpl), HW_RESET_ENTRY_SD, set_hw_reset_pfs,	/* write */
-						get_hw_reset_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
-#endif
-
-	}
-	if (ret < 0)
-		printk(KERN_DEBUG "Create proc entry failed\n");
-
-	return ret;
-}
-
-int bypass_proc_remove_dev_sd(bpctl_dev_t *pbp_device_block)
-{
-
-	struct bypass_pfs_sd *current_pfs = &pbp_device_block->bypass_pfs_set;
-	struct proc_dir_entry *pde = current_pfs->bypass_entry, *pde_curr =
-	    NULL;
-	char name[256];
-
-	for (pde = pde->subdir; pde;) {
-		strcpy(name, pde->name);
-		pde_curr = pde;
-		pde = pde->next;
-		remove_proc_entry(name, current_pfs->bypass_entry);
-	}
-	if (!pde)
-		remove_proc_entry(current_pfs->dir_name, bp_procfs_dir);
-
-	return 0;
-}
-
-#endif				/* BYPASS_SUPPORT */
diff --git a/drivers/staging/silicom/bp_mod.c b/drivers/staging/silicom/bpctl_mod.c
index 45a222723207..b7e570ccb759 100644
--- a/drivers/staging/silicom/bp_mod.c
+++ b/drivers/staging/silicom/bpctl_mod.c
@@ -35,7 +35,6 @@
 #define BP_MOD_DESCR "Silicom Bypass-SD Control driver"
 #define BP_SYNC_FLAG 1
 
-static int Device_Open = 0;
 static int major_num = 0;
 
 MODULE_AUTHOR("Anna Lukin, annal@silicom.co.il");
@@ -60,35 +59,9 @@ typedef enum {
 	bp_none,
 } bp_media_type;
 
-struct pfs_unit_sd {
-	struct proc_dir_entry *proc_entry;
-	char proc_name[32];
-};
-
 struct bypass_pfs_sd {
 	char dir_name[32];
 	struct proc_dir_entry *bypass_entry;
-	struct pfs_unit_sd bypass_info;
-	struct pfs_unit_sd bypass_slave;
-	struct pfs_unit_sd bypass_caps;
-	struct pfs_unit_sd wd_set_caps;
-	struct pfs_unit_sd bypass;
-	struct pfs_unit_sd bypass_change;
-	struct pfs_unit_sd bypass_wd;
-	struct pfs_unit_sd wd_expire_time;
-	struct pfs_unit_sd reset_bypass_wd;
-	struct pfs_unit_sd dis_bypass;
-	struct pfs_unit_sd bypass_pwup;
-	struct pfs_unit_sd bypass_pwoff;
-	struct pfs_unit_sd std_nic;
-	struct pfs_unit_sd tap;
-	struct pfs_unit_sd dis_tap;
-	struct pfs_unit_sd tap_pwup;
-	struct pfs_unit_sd tap_change;
-	struct pfs_unit_sd wd_exp_mode;
-	struct pfs_unit_sd wd_autoreset;
-	struct pfs_unit_sd tpl;
-
 };
 
 typedef struct _bpctl_dev {
@@ -315,27 +288,6 @@ static struct notifier_block bp_notifier_block = {
 	.notifier_call = bp_device_event,
 };
 
-static int device_open(struct inode *inode, struct file *file)
-{
-#ifdef DEBUG
-	printk("device_open(%p)\n", file);
-#endif
-	Device_Open++;
-/*
-* Initialize the message
-*/
-	return SUCCESS;
-}
-
-static int device_release(struct inode *inode, struct file *file)
-{
-#ifdef DEBUG
-	printk("device_release(%p,%p)\n", inode, file);
-#endif
-	Device_Open--;
-	return SUCCESS;
-}
-
 int is_bypass_fn(bpctl_dev_t *pbpctl_dev);
 int wdt_time_left(bpctl_dev_t *pbpctl_dev);
 
@@ -1728,62 +1680,33 @@ int gpio6_clear_fn(bpctl_dev_t *pbpctl_dev)
 }
 #endif				/*BYPASS_DEBUG */
 
-static bpctl_dev_t *get_status_port_fn(bpctl_dev_t *pbpctl_dev)
+static bpctl_dev_t *lookup_port(bpctl_dev_t *dev)
 {
-	int idx_dev = 0;
-
-	if (pbpctl_dev == NULL)
-		return NULL;
-
-	if ((pbpctl_dev->func == 0) || (pbpctl_dev->func == 2)) {
-		for (idx_dev = 0;
-		     ((bpctl_dev_arr[idx_dev].pdev != NULL)
-		      && (idx_dev < device_num)); idx_dev++) {
-			if ((bpctl_dev_arr[idx_dev].bus == pbpctl_dev->bus)
-			    && (bpctl_dev_arr[idx_dev].slot == pbpctl_dev->slot)
-			    && ((bpctl_dev_arr[idx_dev].func == 1)
-				&& (pbpctl_dev->func == 0))) {
-
-				return &(bpctl_dev_arr[idx_dev]);
-			}
-			if ((bpctl_dev_arr[idx_dev].bus == pbpctl_dev->bus) &&
-			    (bpctl_dev_arr[idx_dev].slot == pbpctl_dev->slot) &&
-			    ((bpctl_dev_arr[idx_dev].func == 3)
-			     && (pbpctl_dev->func == 2))) {
+	bpctl_dev_t *p;
+	int n;
+	for (n = 0, p = bpctl_dev_arr; n < device_num && p->pdev; n++) {
+		if (p->bus == dev->bus
+		    && p->slot == dev->slot
+		    && p->func == (dev->func ^ 1))
+			return p;
+	}
+	return NULL;
+}
 
-				return &(bpctl_dev_arr[idx_dev]);
-			}
-		}
+static bpctl_dev_t *get_status_port_fn(bpctl_dev_t *pbpctl_dev)
+{
+	if (pbpctl_dev) {
+		if (pbpctl_dev->func == 0 || pbpctl_dev->func == 2)
+			return lookup_port(pbpctl_dev);
 	}
 	return NULL;
 }
 
 static bpctl_dev_t *get_master_port_fn(bpctl_dev_t *pbpctl_dev)
 {
-	int idx_dev = 0;
-
-	if (pbpctl_dev == NULL)
-		return NULL;
-
-	if ((pbpctl_dev->func == 1) || (pbpctl_dev->func == 3)) {
-		for (idx_dev = 0;
-		     ((bpctl_dev_arr[idx_dev].pdev != NULL)
-		      && (idx_dev < device_num)); idx_dev++) {
-			if ((bpctl_dev_arr[idx_dev].bus == pbpctl_dev->bus)
-			    && (bpctl_dev_arr[idx_dev].slot == pbpctl_dev->slot)
-			    && ((bpctl_dev_arr[idx_dev].func == 0)
-				&& (pbpctl_dev->func == 1))) {
-
-				return &(bpctl_dev_arr[idx_dev]);
-			}
-			if ((bpctl_dev_arr[idx_dev].bus == pbpctl_dev->bus) &&
-			    (bpctl_dev_arr[idx_dev].slot == pbpctl_dev->slot) &&
-			    ((bpctl_dev_arr[idx_dev].func == 2)
-			     && (pbpctl_dev->func == 3))) {
-
-				return &(bpctl_dev_arr[idx_dev]);
-			}
-		}
+	if (pbpctl_dev) {
+		if (pbpctl_dev->func == 1 || pbpctl_dev->func == 3)
+			return lookup_port(pbpctl_dev);
 	}
 	return NULL;
 }
@@ -5858,11 +5781,9 @@ static long device_ioctl(struct file *file,	/* see include/linux/fs.h */
 	return ret;
 }
 
-struct file_operations Fops = {
+static const struct file_operations Fops = {
 	.owner = THIS_MODULE,
 	.unlocked_ioctl = device_ioctl,
-	.open = device_open,
-	.release = device_release,	/* a.k.a. close */
 };
 
 #ifndef PCI_DEVICE
@@ -6686,6 +6607,118 @@ static bpmod_info_t tx_ctl_pci_tbl[] = {
 	{0,}
 };
 
+static void find_fw(bpctl_dev_t *dev)
+{
+	unsigned long mmio_start, mmio_len;
+	struct pci_dev *pdev1 = dev->pdev;
+
+	if ((OLD_IF_SERIES(dev->subdevice)) ||
+	    (INTEL_IF_SERIES(dev->subdevice)))
+		dev->bp_fw_ver = 0xff;
+	else
+		dev->bp_fw_ver = bypass_fw_ver(dev);
+
+	if (dev->bp_10gb == 1 && dev->bp_fw_ver == 0xff) {
+		int cnt = 100;
+		while (cnt--) {
+			iounmap((void *)dev->mem_map);
+			mmio_start = pci_resource_start(pdev1, 0);
+			mmio_len = pci_resource_len(pdev1, 0);
+
+			dev->mem_map = (unsigned long)
+			    ioremap(mmio_start, mmio_len);
+
+			dev->bp_fw_ver = bypass_fw_ver(dev);
+			if (dev-> bp_fw_ver == 0xa8)
+				break;
+		}
+	}
+	/* dev->bp_fw_ver=0xa8; */
+	printk("firmware version: 0x%x\n", dev->bp_fw_ver);
+}
+
+static int init_one(bpctl_dev_t *dev, bpmod_info_t *info, struct pci_dev *pdev1)
+{
+	unsigned long mmio_start, mmio_len;
+
+	dev->pdev = pdev1;
+	mmio_start = pci_resource_start(pdev1, 0);
+	mmio_len = pci_resource_len(pdev1, 0);
+
+	dev->desc = dev_desc[info->index].name;
+	dev->name = info->bp_name;
+	dev->device = info->device;
+	dev->vendor = info->vendor;
+	dev->subdevice = info->subdevice;
+	dev->subvendor = info->subvendor;
+	dev->func = PCI_FUNC(pdev1->devfn);
+	dev->slot = PCI_SLOT(pdev1->devfn);
+	dev->bus = pdev1->bus->number;
+	dev->mem_map = (unsigned long)ioremap(mmio_start, mmio_len);
+#ifdef BP_SYNC_FLAG
+	spin_lock_init(&dev->bypass_wr_lock);
+#endif
+	if (BP10G9_IF_SERIES(dev->subdevice))
+		dev->bp_10g9 = 1;
+	if (BP10G_IF_SERIES(dev->subdevice))
+		dev->bp_10g = 1;
+	if (PEG540_IF_SERIES(dev->subdevice))
+		dev->bp_540 = 1;
+	if (PEGF5_IF_SERIES(dev->subdevice))
+		dev->bp_fiber5 = 1;
+	if (PEG80_IF_SERIES(dev->subdevice))
+		dev->bp_i80 = 1;
+	if (PEGF80_IF_SERIES(dev->subdevice))
+		dev->bp_i80 = 1;
+	if ((dev->subdevice & 0xa00) == 0xa00)
+		dev->bp_i80 = 1;
+	if (BP10GB_IF_SERIES(dev->subdevice)) {
+		if (dev->ifindex == 0) {
+			unregister_chrdev(major_num, DEVICE_NAME);
+			printk("Please load network driver for %s adapter!\n",
+			     dev->name);
+			return -1;
+		}
+
+		if (dev->ndev && !(dev->ndev->flags & IFF_UP)) {
+			unregister_chrdev(major_num, DEVICE_NAME);
+			printk("Please bring up network interfaces for %s adapter!\n",
+			     dev->name);
+			return -1;
+		}
+		dev->bp_10gb = 1;
+	}
+
+	if (!dev->bp_10g9) {
+		if (is_bypass_fn(dev)) {
+			printk(KERN_INFO "%s found, ",
+			       dev->name);
+			find_fw(dev);
+		}
+		dev->wdt_status = WDT_STATUS_UNKNOWN;
+		dev->reset_time = 0;
+		atomic_set(&dev->wdt_busy, 0);
+		dev->bp_status_un = 1;
+
+		bypass_caps_init(dev);
+
+		init_bypass_wd_auto(dev);
+		init_bypass_tpl_auto(dev);
+		if (NOKIA_SERIES(dev->subdevice))
+			reset_cont(dev);
+	}
+#ifdef BP_SELF_TEST
+	if ((dev->bp_tx_data = kzalloc(BPTEST_DATA_LEN, GFP_KERNEL))) {
+		memset(dev->bp_tx_data, 0xff, 6);
+		memset(dev->bp_tx_data + 6, 0x0, 1);
+		memset(dev->bp_tx_data + 7, 0xaa, 5);
+		*(__be16 *)(dev->bp_tx_data + 12) = htons(ETH_P_BPTEST);
+	} else
+		printk("bp_ctl: Memory allocation error!\n");
+#endif
+	return 0;
+}
+
 /*
 * Initialize the module - Register the character device
 */
@@ -6694,7 +6727,7 @@ static int __init bypass_init_module(void)
 {
 	int ret_val, idx, idx_dev = 0;
 	struct pci_dev *pdev1 = NULL;
-	unsigned long mmio_start, mmio_len;
+	bpctl_dev_t *dev;
 
 	printk(BP_MOD_DESCR " v" BP_MOD_VER "\n");
 	ret_val = register_chrdev(major_num, DEVICE_NAME, &Fops);
@@ -6729,181 +6762,16 @@ static int __init bypass_init_module(void)
 	memset(bpctl_dev_arr, 0, ((device_num) * sizeof(bpctl_dev_t)));
 
 	pdev1 = NULL;
+	dev = bpctl_dev_arr;
 	for (idx = 0; tx_ctl_pci_tbl[idx].vendor; idx++) {
 		while ((pdev1 = pci_get_subsys(tx_ctl_pci_tbl[idx].vendor,
 					       tx_ctl_pci_tbl[idx].device,
 					       tx_ctl_pci_tbl[idx].subvendor,
 					       tx_ctl_pci_tbl[idx].subdevice,
 					       pdev1))) {
-			bpctl_dev_arr[idx_dev].pdev = pdev1;
-
-			mmio_start = pci_resource_start(pdev1, 0);
-			mmio_len = pci_resource_len(pdev1, 0);
-
-			bpctl_dev_arr[idx_dev].desc =
-			    dev_desc[tx_ctl_pci_tbl[idx].index].name;
-			bpctl_dev_arr[idx_dev].name =
-			    tx_ctl_pci_tbl[idx].bp_name;
-			bpctl_dev_arr[idx_dev].device =
-			    tx_ctl_pci_tbl[idx].device;
-			bpctl_dev_arr[idx_dev].vendor =
-			    tx_ctl_pci_tbl[idx].vendor;
-			bpctl_dev_arr[idx_dev].subdevice =
-			    tx_ctl_pci_tbl[idx].subdevice;
-			bpctl_dev_arr[idx_dev].subvendor =
-			    tx_ctl_pci_tbl[idx].subvendor;
-			/* bpctl_dev_arr[idx_dev].pdev=pdev1; */
-			bpctl_dev_arr[idx_dev].func = PCI_FUNC(pdev1->devfn);
-			bpctl_dev_arr[idx_dev].slot = PCI_SLOT(pdev1->devfn);
-			bpctl_dev_arr[idx_dev].bus = pdev1->bus->number;
-			bpctl_dev_arr[idx_dev].mem_map =
-			    (unsigned long)ioremap(mmio_start, mmio_len);
-#ifdef BP_SYNC_FLAG
-			spin_lock_init(&bpctl_dev_arr[idx_dev].bypass_wr_lock);
-#endif
-			if (BP10G9_IF_SERIES(bpctl_dev_arr[idx_dev].subdevice))
-				bpctl_dev_arr[idx_dev].bp_10g9 = 1;
-			if (BP10G_IF_SERIES(bpctl_dev_arr[idx_dev].subdevice))
-				bpctl_dev_arr[idx_dev].bp_10g = 1;
-			if (PEG540_IF_SERIES(bpctl_dev_arr[idx_dev].subdevice)) {
-
-				bpctl_dev_arr[idx_dev].bp_540 = 1;
-			}
-			if (PEGF5_IF_SERIES(bpctl_dev_arr[idx_dev].subdevice))
-				bpctl_dev_arr[idx_dev].bp_fiber5 = 1;
-			if (PEG80_IF_SERIES(bpctl_dev_arr[idx_dev].subdevice))
-				bpctl_dev_arr[idx_dev].bp_i80 = 1;
-			if (PEGF80_IF_SERIES(bpctl_dev_arr[idx_dev].subdevice))
-				bpctl_dev_arr[idx_dev].bp_i80 = 1;
-			if ((bpctl_dev_arr[idx_dev].subdevice & 0xa00) == 0xa00)
-				bpctl_dev_arr[idx_dev].bp_i80 = 1;
-			if (BP10GB_IF_SERIES(bpctl_dev_arr[idx_dev].subdevice)) {
-				if (bpctl_dev_arr[idx_dev].ifindex == 0) {
-					unregister_chrdev(major_num,
-							  DEVICE_NAME);
-					printk
-					    ("Please load network driver for %s adapter!\n",
-					     bpctl_dev_arr[idx_dev].name);
-					return -1;
-				}
-
-				if (bpctl_dev_arr[idx_dev].ndev) {
-					if (!
-					    (bpctl_dev_arr[idx_dev].ndev->
-					     flags & IFF_UP)) {
-						if (!
-						    (bpctl_dev_arr[idx_dev].
-						     ndev->flags & IFF_UP)) {
-							unregister_chrdev
-							    (major_num,
-							     DEVICE_NAME);
-							printk
-							    ("Please bring up network interfaces for %s adapter!\n",
-							     bpctl_dev_arr
-							     [idx_dev].name);
-							return -1;
-						}
-
-					}
-				}
-				bpctl_dev_arr[idx_dev].bp_10gb = 1;
-			}
-
-			if (!bpctl_dev_arr[idx_dev].bp_10g9) {
-
-				if (is_bypass_fn(&bpctl_dev_arr[idx_dev])) {
-					printk(KERN_INFO "%s found, ",
-					       bpctl_dev_arr[idx_dev].name);
-					if ((OLD_IF_SERIES
-					     (bpctl_dev_arr[idx_dev].subdevice))
-					    ||
-					    (INTEL_IF_SERIES
-					     (bpctl_dev_arr[idx_dev].
-					      subdevice)))
-						bpctl_dev_arr[idx_dev].
-						    bp_fw_ver = 0xff;
-					else
-						bpctl_dev_arr[idx_dev].
-						    bp_fw_ver =
-						    bypass_fw_ver(&bpctl_dev_arr
-								  [idx_dev]);
-					if ((bpctl_dev_arr[idx_dev].bp_10gb ==
-					     1)
-					    && (bpctl_dev_arr[idx_dev].
-						bp_fw_ver == 0xff)) {
-						int cnt = 100;
-						while (cnt--) {
-							iounmap((void
-								 *)
-								(bpctl_dev_arr
-								 [idx_dev].
-								 mem_map));
-							mmio_start =
-							    pci_resource_start
-							    (pdev1, 0);
-							mmio_len =
-							    pci_resource_len
-							    (pdev1, 0);
-
-							bpctl_dev_arr[idx_dev].
-							    mem_map =
-							    (unsigned long)
-							    ioremap(mmio_start,
-								    mmio_len);
-
-							bpctl_dev_arr[idx_dev].
-							    bp_fw_ver =
-							    bypass_fw_ver
-							    (&bpctl_dev_arr
-							     [idx_dev]);
-							if (bpctl_dev_arr
-							    [idx_dev].
-							    bp_fw_ver == 0xa8)
-								break;
-
-						}
-					}
-					/* bpctl_dev_arr[idx_dev].bp_fw_ver=0xa8; */
-					printk("firmware version: 0x%x\n",
-					       bpctl_dev_arr[idx_dev].
-					       bp_fw_ver);
-				}
-				bpctl_dev_arr[idx_dev].wdt_status =
-				    WDT_STATUS_UNKNOWN;
-				bpctl_dev_arr[idx_dev].reset_time = 0;
-				atomic_set(&bpctl_dev_arr[idx_dev].wdt_busy, 0);
-				bpctl_dev_arr[idx_dev].bp_status_un = 1;
-
-				bypass_caps_init(&bpctl_dev_arr[idx_dev]);
-
-				init_bypass_wd_auto(&bpctl_dev_arr[idx_dev]);
-				init_bypass_tpl_auto(&bpctl_dev_arr[idx_dev]);
-				if (NOKIA_SERIES
-				    (bpctl_dev_arr[idx_dev].subdevice))
-					reset_cont(&bpctl_dev_arr[idx_dev]);
-			}
-#ifdef BP_SELF_TEST
-			if ((bpctl_dev_arr[idx_dev].bp_tx_data =
-			     kmalloc(BPTEST_DATA_LEN, GFP_KERNEL))) {
-
-				memset(bpctl_dev_arr[idx_dev].bp_tx_data, 0x0,
-				       BPTEST_DATA_LEN);
-
-				memset(bpctl_dev_arr[idx_dev].bp_tx_data, 0xff,
-				       6);
-				memset(bpctl_dev_arr[idx_dev].bp_tx_data + 6,
-				       0x0, 1);
-				memset(bpctl_dev_arr[idx_dev].bp_tx_data + 7,
-				       0xaa, 5);
-
-				*(__be16 *) (bpctl_dev_arr[idx_dev].bp_tx_data +
-					     12) = htons(ETH_P_BPTEST);
-
-			} else
-				printk("bp_ctl: Memory allocation error!\n");
-#endif
-			idx_dev++;
-
+			if (init_one(dev, &tx_ctl_pci_tbl[idx], pdev1) < 0)
+				return -1;
+			dev++;
 		}
 	}
 	if_scan_init();
@@ -6913,33 +6781,27 @@ static int __init bypass_init_module(void)
 	{
 
 		bpctl_dev_t *pbpctl_dev_c = NULL;
-		for (idx_dev = 0;
-		     ((bpctl_dev_arr[idx_dev].pdev != NULL)
-		      && (idx_dev < device_num)); idx_dev++) {
-			if (bpctl_dev_arr[idx_dev].bp_10g9) {
-				pbpctl_dev_c =
-				    get_status_port_fn(&bpctl_dev_arr[idx_dev]);
-				if (is_bypass_fn(&bpctl_dev_arr[idx_dev])) {
+		for (idx_dev = 0, dev = bpctl_dev_arr;
+		     idx_dev < device_num && dev->pdev;
+		     idx_dev++, dev++) {
+			if (dev->bp_10g9) {
+				pbpctl_dev_c = get_status_port_fn(dev);
+				if (is_bypass_fn(dev)) {
 					printk(KERN_INFO "%s found, ",
-					       bpctl_dev_arr[idx_dev].name);
-					bpctl_dev_arr[idx_dev].bp_fw_ver =
-					    bypass_fw_ver(&bpctl_dev_arr
-							  [idx_dev]);
+					       dev->name);
+					dev->bp_fw_ver = bypass_fw_ver(dev);
 					printk("firmware version: 0x%x\n",
-					       bpctl_dev_arr[idx_dev].
-					       bp_fw_ver);
-
+					       dev->bp_fw_ver);
 				}
-				bpctl_dev_arr[idx_dev].wdt_status =
-				    WDT_STATUS_UNKNOWN;
-				bpctl_dev_arr[idx_dev].reset_time = 0;
-				atomic_set(&bpctl_dev_arr[idx_dev].wdt_busy, 0);
-				bpctl_dev_arr[idx_dev].bp_status_un = 1;
+				dev->wdt_status = WDT_STATUS_UNKNOWN;
+				dev->reset_time = 0;
+				atomic_set(&dev->wdt_busy, 0);
+				dev->bp_status_un = 1;
 
-				bypass_caps_init(&bpctl_dev_arr[idx_dev]);
+				bypass_caps_init(dev);
 
-				init_bypass_wd_auto(&bpctl_dev_arr[idx_dev]);
-				init_bypass_tpl_auto(&bpctl_dev_arr[idx_dev]);
+				init_bypass_wd_auto(dev);
+				init_bypass_tpl_auto(dev);
 
 			}
 
@@ -7336,78 +7198,11 @@ EXPORT_SYMBOL_NOVERS(bp_if_scan_sd);
 
 #define BP_PROC_DIR "bypass"
 
-#define GPIO6_SET_ENTRY_SD           "gpio6_set"
-#define GPIO6_CLEAR_ENTRY_SD         "gpio6_clear"
-
-#define GPIO7_SET_ENTRY_SD           "gpio7_set"
-#define GPIO7_CLEAR_ENTRY_SD         "gpio7_clear"
-
-#define PULSE_SET_ENTRY_SD            "pulse_set"
-#define ZERO_SET_ENTRY_SD            "zero_set"
-#define PULSE_GET1_ENTRY_SD            "pulse_get1"
-#define PULSE_GET2_ENTRY_SD            "pulse_get2"
-
-#define CMND_ON_ENTRY_SD              "cmnd_on"
-#define CMND_OFF_ENTRY_SD             "cmnd_off"
-#define RESET_CONT_ENTRY_SD           "reset_cont"
-
- /*COMMANDS*/
-#define BYPASS_INFO_ENTRY_SD     "bypass_info"
-#define BYPASS_SLAVE_ENTRY_SD    "bypass_slave"
-#define BYPASS_CAPS_ENTRY_SD     "bypass_caps"
-#define WD_SET_CAPS_ENTRY_SD     "wd_set_caps"
-#define BYPASS_ENTRY_SD          "bypass"
-#define BYPASS_CHANGE_ENTRY_SD   "bypass_change"
-#define BYPASS_WD_ENTRY_SD       "bypass_wd"
-#define WD_EXPIRE_TIME_ENTRY_SD  "wd_expire_time"
-#define RESET_BYPASS_WD_ENTRY_SD "reset_bypass_wd"
-#define DIS_BYPASS_ENTRY_SD      "dis_bypass"
-#define BYPASS_PWUP_ENTRY_SD     "bypass_pwup"
-#define BYPASS_PWOFF_ENTRY_SD     "bypass_pwoff"
-#define STD_NIC_ENTRY_SD         "std_nic"
-#define STD_NIC_ENTRY_SD         "std_nic"
-#define TAP_ENTRY_SD             "tap"
-#define TAP_CHANGE_ENTRY_SD      "tap_change"
-#define DIS_TAP_ENTRY_SD         "dis_tap"
-#define TAP_PWUP_ENTRY_SD        "tap_pwup"
-#define TWO_PORT_LINK_ENTRY_SD   "two_port_link"
-#define WD_EXP_MODE_ENTRY_SD     "wd_exp_mode"
-#define WD_AUTORESET_ENTRY_SD    "wd_autoreset"
-#define TPL_ENTRY_SD             "tpl"
-#define WAIT_AT_PWUP_ENTRY_SD    "wait_at_pwup"
-#define HW_RESET_ENTRY_SD        "hw_reset"
-#define DISC_ENTRY_SD             "disc"
-#define DISC_CHANGE_ENTRY_SD      "disc_change"
-#define DIS_DISC_ENTRY_SD         "dis_disc"
-#define DISC_PWUP_ENTRY_SD        "disc_pwup"
 static struct proc_dir_entry *bp_procfs_dir;
 
-static struct proc_dir_entry *proc_getdir(char *name,
-					  struct proc_dir_entry *proc_dir)
-{
-	struct proc_dir_entry *pde = proc_dir;
-
-	for (pde = pde->subdir; pde; pde = pde->next) {
-		if (pde->namelen && (strcmp(name, pde->name) == 0)) {
-			/* directory exists */
-			break;
-		}
-	}
-	if (pde == (struct proc_dir_entry *)0) {
-		/* create the directory */
-		pde = proc_mkdir(name, proc_dir);
-		if (pde == (struct proc_dir_entry *)0) {
-
-			return pde;
-		}
-	}
-
-	return pde;
-}
-
 int bp_proc_create(void)
 {
-	bp_procfs_dir = proc_getdir(BP_PROC_DIR, init_net.proc_net);
+	bp_procfs_dir = proc_mkdir(BP_PROC_DIR, init_net.proc_net);
 	if (bp_procfs_dir == (struct proc_dir_entry *)0) {
 		printk(KERN_DEBUG
 		       "Could not create procfs nicinfo directory %s\n",
@@ -7417,144 +7212,99 @@ int bp_proc_create(void)
 	return 0;
 }
 
-int
-bypass_proc_create_entry_sd(struct pfs_unit_sd *pfs_unit_curr,
-			    char *proc_name,
-			    write_proc_t *write_proc,
-			    read_proc_t *read_proc,
-			    struct proc_dir_entry *parent_pfs, void *data)
+static int procfs_add(char *proc_name, const struct file_operations *fops,
+		      bpctl_dev_t *dev)
 {
-	strcpy(pfs_unit_curr->proc_name, proc_name);
-	pfs_unit_curr->proc_entry = create_proc_entry(pfs_unit_curr->proc_name,
-						      S_IFREG | S_IRUSR |
-						      S_IWUSR | S_IRGRP |
-						      S_IROTH, parent_pfs);
-	if (pfs_unit_curr->proc_entry == NULL)
+	struct bypass_pfs_sd *pfs = &dev->bypass_pfs_set;
+	if (!proc_create_data(proc_name, 0644, pfs->bypass_entry, fops, dev))
 		return -1;
-
-	pfs_unit_curr->proc_entry->read_proc = read_proc;
-	pfs_unit_curr->proc_entry->write_proc = write_proc;
-	pfs_unit_curr->proc_entry->data = data;
-
 	return 0;
-
 }
 
-int
-get_bypass_info_pfs(char *page, char **start, off_t off, int count,
-		    int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-	int len = 0;
-
-	len += sprintf(page, "Name\t\t\t%s\n", pbp_device_block->name);
-	len +=
-	    sprintf(page + len, "Firmware version\t0x%x\n",
-		    pbp_device_block->bp_fw_ver);
+#define RO_FOPS(name)	\
+static int name##_open(struct inode *inode, struct file *file)	\
+{								\
+	return single_open(file, show_##name, PDE_DATA(inode));\
+}								\
+static const struct file_operations name##_ops = {		\
+	.open = name##_open,					\
+	.read = seq_read,					\
+	.llseek = seq_lseek,					\
+	.release = single_release,				\
+};
 
-	*eof = 1;
-	return len;
-}
+#define RW_FOPS(name)	\
+static int name##_open(struct inode *inode, struct file *file)	\
+{								\
+	return single_open(file, show_##name, PDE_DATA(inode));\
+}								\
+static const struct file_operations name##_ops = {		\
+	.open = name##_open,					\
+	.read = seq_read,					\
+	.write = name##_write,					\
+	.llseek = seq_lseek,					\
+	.release = single_release,				\
+};
 
-int
-get_bypass_slave_pfs(char *page, char **start, off_t off, int count,
-		     int *eof, void *data)
+static int show_bypass_info(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+	bpctl_dev_t *dev = m->private;
 
-	int len = 0;
-	bpctl_dev_t *pbp_device_block_slave = NULL;
-	int idx_dev = 0;
-	struct net_device *net_slave_dev = NULL;
-
-	if ((pbp_device_block->func == 0) || (pbp_device_block->func == 2)) {
-		for (idx_dev = 0;
-		     ((bpctl_dev_arr[idx_dev].pdev != NULL)
-		      && (idx_dev < device_num)); idx_dev++) {
-			if ((bpctl_dev_arr[idx_dev].bus ==
-			     pbp_device_block->bus)
-			    && (bpctl_dev_arr[idx_dev].slot ==
-				pbp_device_block->slot)) {
-				if ((pbp_device_block->func == 0)
-				    && (bpctl_dev_arr[idx_dev].func == 1)) {
-					pbp_device_block_slave =
-					    &bpctl_dev_arr[idx_dev];
-					break;
-				}
-				if ((pbp_device_block->func == 2) &&
-				    (bpctl_dev_arr[idx_dev].func == 3)) {
-					pbp_device_block_slave =
-					    &bpctl_dev_arr[idx_dev];
-					break;
-				}
-			}
-		}
-	} else
-		pbp_device_block_slave = pbp_device_block;
-	if (!pbp_device_block_slave) {
-		len = sprintf(page, "fail\n");
-		*eof = 1;
-		return len;
-	}
-	net_slave_dev = pbp_device_block_slave->ndev;
-	if (net_slave_dev)
-		len = sprintf(page, "%s\n", net_slave_dev->name);
-
-	*eof = 1;
-	return len;
+	seq_printf(m, "Name\t\t\t%s\n", dev->name);
+	seq_printf(m, "Firmware version\t0x%x\n", dev->bp_fw_ver);
+	return 0;
 }
+RO_FOPS(bypass_info)
 
-int
-get_bypass_caps_pfs(char *page, char **start, off_t off, int count,
-		    int *eof, void *data)
+static int show_bypass_slave(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
+	bpctl_dev_t *dev = m->private;
+	bpctl_dev_t *slave = get_status_port_fn(dev);
+	if (!slave)
+		slave = dev;
+	if (!slave)
+		seq_printf(m, "fail\n");
+	else if (slave->ndev)
+		seq_printf(m, "%s\n", slave->ndev->name);
+	return 0;
+}
+RO_FOPS(bypass_slave)
 
-	ret = get_bypass_caps_fn(pbp_device_block);
+static int show_bypass_caps(struct seq_file *m, void *v)
+{
+	bpctl_dev_t *dev = m->private;
+	int ret = get_bypass_caps_fn(dev);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "-1\n");
+		seq_printf(m, "-1\n");
 	else
-		len = sprintf(page, "0x%x\n", ret);
-	*eof = 1;
-	return len;
-
+		seq_printf(m, "0x%x\n", ret);
+	return 0;
 }
+RO_FOPS(bypass_caps)
 
-int
-get_wd_set_caps_pfs(char *page, char **start, off_t off, int count,
-		    int *eof, void *data)
+static int show_wd_set_caps(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_wd_set_caps_fn(pbp_device_block);
+	bpctl_dev_t *dev = m->private;
+	int ret = get_wd_set_caps_fn(dev);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "-1\n");
+		seq_printf(m, "-1\n");
 	else
-		len = sprintf(page, "0x%x\n", ret);
-	*eof = 1;
-	return len;
+		seq_printf(m, "0x%x\n", ret);
+	return 0;
 }
+RO_FOPS(wd_set_caps)
 
-int
-set_bypass_pfs(struct file *file, const char *buffer,
-	       unsigned long count, void *data)
+static int user_on_off(const void __user *buffer, size_t count)
 {
 
 	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int bypass_param = 0, length = 0;
+	int length = 0;
 
 	if (count > (sizeof(kbuf) - 1))
 		return -1;
 
-	if (copy_from_user(&kbuf, buffer, count)) {
+	if (copy_from_user(&kbuf, buffer, count))
 		return -1;
-	}
 
 	kbuf[count] = '\0';
 	length = strlen(kbuf);
@@ -7562,806 +7312,467 @@ set_bypass_pfs(struct file *file, const char *buffer,
 		kbuf[--length] = '\0';
 
 	if (strcmp(kbuf, "on") == 0)
-		bypass_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		bypass_param = 0;
-
-	set_bypass_fn(pbp_device_block, bypass_param);
-
-	return count;
+		return 1;
+	if (strcmp(kbuf, "off") == 0)
+		return 0;
+	return 0;
 }
 
-int
-set_tap_pfs(struct file *file, const char *buffer,
-	    unsigned long count, void *data)
+static ssize_t bypass_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tap_param = 0, length = 0;
-
-	if (count > (sizeof(kbuf) - 1))
+	int bypass_param = user_on_off(buffer, count);
+	if (bypass_param < 0)
 		return -1;
 
-	if (copy_from_user(&kbuf, buffer, count)) {
-		return -1;
-	}
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tap_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tap_param = 0;
-
-	set_tap_fn(pbp_device_block, tap_param);
-
+	set_bypass_fn(PDE_DATA(file_inode(file)), bypass_param);
 	return count;
 }
-
-int
-set_disc_pfs(struct file *file, const char *buffer,
-	     unsigned long count, void *data)
+static int show_bypass(struct seq_file *m, void *v)
 {
+	bpctl_dev_t *dev = m->private;
+	int ret = get_bypass_fn(dev);
+	if (ret == BP_NOT_CAP)
+		seq_printf(m, "fail\n");
+	else if (ret == 1)
+		seq_printf(m, "on\n");
+	else if (ret == 0)
+		seq_printf(m, "off\n");
+	return 0;
+}
+RW_FOPS(bypass)
 
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tap_param = 0, length = 0;
-
-	if (count > (sizeof(kbuf) - 1))
-		return -1;
-
-	if (copy_from_user(&kbuf, buffer, count)) {
+static ssize_t tap_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
+{
+	int tap_param = user_on_off(buffer, count);
+	if (tap_param < 0)
 		return -1;
-	}
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tap_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tap_param = 0;
-
-	set_disc_fn(pbp_device_block, tap_param);
 
+	set_tap_fn(PDE_DATA(file_inode(file)), tap_param);
 	return count;
 }
-
-int
-get_bypass_pfs(char *page, char **start, off_t off, int count,
-	       int *eof, void *data)
+static int show_tap(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_bypass_fn(pbp_device_block);
+	bpctl_dev_t *dev = m->private;
+	int ret = get_tap_fn(dev);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
+		seq_printf(m, "fail\n");
 	else if (ret == 1)
-		len = sprintf(page, "on\n");
+		seq_printf(m, "on\n");
 	else if (ret == 0)
-		len = sprintf(page, "off\n");
-
-	*eof = 1;
-	return len;
+		seq_printf(m, "off\n");
+	return 0;
 }
+RW_FOPS(tap)
 
-int
-get_tap_pfs(char *page, char **start, off_t off, int count,
-	    int *eof, void *data)
+static ssize_t disc_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_tap_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 1)
-		len = sprintf(page, "on\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
+	int tap_param = user_on_off(buffer, count);
+	if (tap_param < 0)
+		return -1;
 
-	*eof = 1;
-	return len;
+	set_disc_fn(PDE_DATA(file_inode(file)), tap_param);
+	return count;
 }
-
-int
-get_disc_pfs(char *page, char **start, off_t off, int count,
-	     int *eof, void *data)
+static int show_disc(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_disc_fn(pbp_device_block);
+	bpctl_dev_t *dev = m->private;
+	int ret = get_disc_fn(dev);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
+		seq_printf(m, "fail\n");
 	else if (ret == 1)
-		len = sprintf(page, "on\n");
+		seq_printf(m, "on\n");
 	else if (ret == 0)
-		len = sprintf(page, "off\n");
-
-	*eof = 1;
-	return len;
+		seq_printf(m, "off\n");
+	return 0;
 }
+RW_FOPS(disc)
 
-int
-get_bypass_change_pfs(char *page, char **start, off_t off, int count,
-		      int *eof, void *data)
+static int show_bypass_change(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_bypass_change_fn(pbp_device_block);
+	bpctl_dev_t *dev = m->private;
+	int ret = get_bypass_change_fn(dev);
 	if (ret == 1)
-		len = sprintf(page, "on\n");
+		seq_printf(m, "on\n");
 	else if (ret == 0)
-		len = sprintf(page, "off\n");
+		seq_printf(m, "off\n");
 	else
-		len = sprintf(page, "fail\n");
-
-	*eof = 1;
-	return len;
+		seq_printf(m, "fail\n");
+	return 0;
 }
+RO_FOPS(bypass_change)
 
-int
-get_tap_change_pfs(char *page, char **start, off_t off, int count,
-		   int *eof, void *data)
+static int show_tap_change(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_tap_change_fn(pbp_device_block);
+	bpctl_dev_t *dev = m->private;
+	int ret = get_tap_change_fn(dev);
 	if (ret == 1)
-		len = sprintf(page, "on\n");
+		seq_printf(m, "on\n");
 	else if (ret == 0)
-		len = sprintf(page, "off\n");
+		seq_printf(m, "off\n");
 	else
-		len = sprintf(page, "fail\n");
-
-	*eof = 1;
-	return len;
+		seq_printf(m, "fail\n");
+	return 0;
 }
+RO_FOPS(tap_change)
 
-int
-get_disc_change_pfs(char *page, char **start, off_t off, int count,
-		    int *eof, void *data)
+static int show_disc_change(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_disc_change_fn(pbp_device_block);
+	bpctl_dev_t *dev = m->private;
+	int ret = get_disc_change_fn(dev);
 	if (ret == 1)
-		len = sprintf(page, "on\n");
+		seq_printf(m, "on\n");
 	else if (ret == 0)
-		len = sprintf(page, "off\n");
+		seq_printf(m, "off\n");
 	else
-		len = sprintf(page, "fail\n");
-
-	*eof = 1;
-	return len;
-}
-
-#define isdigit(c) (c >= '0' && c <= '9')
-__inline static int atoi(char **s)
-{
-	int i = 0;
-	while (isdigit(**s))
-		i = i * 10 + *((*s)++) - '0';
-	return i;
+		seq_printf(m, "fail\n");
+	return 0;
 }
+RO_FOPS(disc_change)
 
-int
-set_bypass_wd_pfs(struct file *file, const char *buffer,
-		  unsigned long count, void *data)
+static ssize_t bypass_wd_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+	bpctl_dev_t *dev = PDE_DATA(file_inode(file));
 	int timeout;
-	int ret;
-
-	ret = kstrtoint_from_user(buffer, count, 10, &timeout);
+	int ret = kstrtoint_from_user(buffer, count, 10, &timeout);
 	if (ret)
 		return ret;
-	set_bypass_wd_fn(pbp_device_block, timeout);
-
+	set_bypass_wd_fn(dev, timeout);
 	return count;
 }
-
-int
-get_bypass_wd_pfs(char *page, char **start, off_t off, int count,
-		  int *eof, void *data)
+static int show_bypass_wd(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
+	bpctl_dev_t *dev = m->private;
+	int ret = 0, timeout = 0;
 
-	int len = 0, ret = 0, timeout = 0;
-
-	ret = get_bypass_wd_fn(pbp_device_block, &timeout);
+	ret = get_bypass_wd_fn(dev, &timeout);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
+		seq_printf(m,  "fail\n");
 	else if (timeout == -1)
-		len = sprintf(page, "unknown\n");
+		seq_printf(m,  "unknown\n");
 	else if (timeout == 0)
-		len = sprintf(page, "disable\n");
+		seq_printf(m,  "disable\n");
 	else
-		len = sprintf(page, "%d\n", timeout);
-
-	*eof = 1;
-	return len;
+		seq_printf(m, "%d\n", timeout);
+	return 0;
 }
+RW_FOPS(bypass_wd)
 
-int
-get_wd_expire_time_pfs(char *page, char **start, off_t off, int count,
-		       int *eof, void *data)
+static int show_wd_expire_time(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0, timeout = 0;
-
-	ret = get_wd_expire_time_fn(pbp_device_block, &timeout);
+	bpctl_dev_t *dev = m->private;
+	int ret = 0, timeout = 0;
+	ret = get_wd_expire_time_fn(dev, &timeout);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
+		seq_printf(m, "fail\n");
 	else if (timeout == -1)
-		len = sprintf(page, "expire\n");
+		seq_printf(m, "expire\n");
 	else if (timeout == 0)
-		len = sprintf(page, "disable\n");
-
+		seq_printf(m, "disable\n");
 	else
-		len = sprintf(page, "%d\n", timeout);
-	*eof = 1;
-	return len;
+		seq_printf(m, "%d\n", timeout);
+	return 0;
 }
+RO_FOPS(wd_expire_time)
 
-int
-get_tpl_pfs(char *page, char **start, off_t off, int count,
-	    int *eof, void *data)
+static ssize_t tpl_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
+	bpctl_dev_t *dev = PDE_DATA(file_inode(file));
+	int tpl_param = user_on_off(buffer, count);
+	if (tpl_param < 0)
+		return -1;
 
-	ret = get_tpl_fn(pbp_device_block);
+	set_tpl_fn(dev, tpl_param);
+	return count;
+}
+static int show_tpl(struct seq_file *m, void *v)
+{
+	bpctl_dev_t *dev = m->private;
+	int ret = get_tpl_fn(dev);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
+		seq_printf(m, "fail\n");
 	else if (ret == 1)
-		len = sprintf(page, "on\n");
+		seq_printf(m, "on\n");
 	else if (ret == 0)
-		len = sprintf(page, "off\n");
-
-	*eof = 1;
-	return len;
+		seq_printf(m, "off\n");
+	return 0;
 }
+RW_FOPS(tpl)
 
 #ifdef PMC_FIX_FLAG
-int
-get_wait_at_pwup_pfs(char *page, char **start, off_t off, int count,
-		     int *eof, void *data)
+static ssize_t wait_at_pwup_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
+	bpctl_dev_t *dev = PDE_DATA(file_inode(file));
+	int tpl_param = user_on_off(buffer, count);
+	if (tpl_param < 0)
+		return -1;
 
-	ret = get_bp_wait_at_pwup_fn(pbp_device_block);
+	set_bp_wait_at_pwup_fn(dev, tpl_param);
+	return count;
+}
+static int show_wait_at_pwup(struct seq_file *m, void *v)
+{
+	bpctl_dev_t *dev = m->private;
+	int ret = get_bp_wait_at_pwup_fn(dev);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
+		seq_printf(m, "fail\n");
 	else if (ret == 1)
-		len = sprintf(page, "on\n");
+		seq_printf(m, "on\n");
 	else if (ret == 0)
-		len = sprintf(page, "off\n");
-
-	*eof = 1;
-	return len;
+		seq_printf(m, "off\n");
+	return 0;
 }
+RW_FOPS(wait_at_pwup)
 
-int
-get_hw_reset_pfs(char *page, char **start, off_t off, int count,
-		 int *eof, void *data)
+static ssize_t hw_reset_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
+	bpctl_dev_t *dev = PDE_DATA(file_inode(file));
+	int tpl_param = user_on_off(buffer, count);
+	if (tpl_param < 0)
+		return -1;
 
-	ret = get_bp_hw_reset_fn(pbp_device_block);
+	set_bp_hw_reset_fn(dev, tpl_param);
+	return count;
+}
+static int show_hw_reset(struct seq_file *m, void *v)
+{
+	bpctl_dev_t *dev = m->private;
+	int ret = get_bp_hw_reset_fn(dev);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
+		seq_printf(m, "fail\n");
 	else if (ret == 1)
-		len = sprintf(page, "on\n");
+		seq_printf(m, "on\n");
 	else if (ret == 0)
-		len = sprintf(page, "off\n");
-
-	*eof = 1;
-	return len;
+		seq_printf(m, "off\n");
+	return 0;
 }
+RW_FOPS(hw_reset)
 
 #endif				/*PMC_WAIT_FLAG */
 
-int
-reset_bypass_wd_pfs(char *page, char **start, off_t off, int count,
-		    int *eof, void *data)
+static int show_reset_bypass_wd(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = reset_bypass_wd_timer_fn(pbp_device_block);
+	bpctl_dev_t *dev = m->private;
+	int ret = reset_bypass_wd_timer_fn(dev);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
+		seq_printf(m, "fail\n");
 	else if (ret == 0)
-		len = sprintf(page, "disable\n");
+		seq_printf(m, "disable\n");
 	else if (ret == 1)
-		len = sprintf(page, "success\n");
-
-	*eof = 1;
-	return len;
+		seq_printf(m, "success\n");
+	return 0;
 }
+RO_FOPS(reset_bypass_wd)
 
-int
-set_dis_bypass_pfs(struct file *file, const char *buffer,
-		   unsigned long count, void *data)
+static ssize_t dis_bypass_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int bypass_param = 0, length = 0;
-
-	if (count >= sizeof(kbuf))
+	int bypass_param = user_on_off(buffer, count);
+	if (bypass_param < 0)
 		return -EINVAL;
 
-	if (copy_from_user(&kbuf, buffer, count)) {
-		return -1;
-	}
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		bypass_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		bypass_param = 0;
-
-	set_dis_bypass_fn(pbp_device_block, bypass_param);
-
+	set_dis_bypass_fn(PDE_DATA(file_inode(file)), bypass_param);
 	return count;
 }
-
-int
-set_dis_tap_pfs(struct file *file, const char *buffer,
-		unsigned long count, void *data)
+static int show_dis_bypass(struct seq_file *m, void *v)
 {
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tap_param = 0, length = 0;
-
-	if (count >= sizeof(kbuf))
-		return -EINVAL;
-
-	if (copy_from_user(&kbuf, buffer, count)) {
-		return -1;
-	}
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tap_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tap_param = 0;
-
-	set_dis_tap_fn(pbp_device_block, tap_param);
-
-	return count;
+	bpctl_dev_t *dev = m->private;
+	int ret = get_dis_bypass_fn(dev);
+	if (ret == BP_NOT_CAP)
+		seq_printf(m, "fail\n");
+	else if (ret == 0)
+		seq_printf(m, "off\n");
+	else
+		seq_printf(m, "on\n");
+	return 0;
 }
+RW_FOPS(dis_bypass)
 
-int
-set_dis_disc_pfs(struct file *file, const char *buffer,
-		 unsigned long count, void *data)
+static ssize_t dis_tap_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tap_param = 0, length = 0;
-
-	if (count >= sizeof(kbuf))
+	int tap_param = user_on_off(buffer, count);
+	if (tap_param < 0)
 		return -EINVAL;
 
-	if (copy_from_user(&kbuf, buffer, count)) {
-		return -1;
-	}
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tap_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tap_param = 0;
-
-	set_dis_disc_fn(pbp_device_block, tap_param);
-
+	set_dis_tap_fn(PDE_DATA(file_inode(file)), tap_param);
 	return count;
 }
-
-int
-get_dis_bypass_pfs(char *page, char **start, off_t off, int count,
-		   int *eof, void *data)
+static int show_dis_tap(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_dis_bypass_fn(pbp_device_block);
+	bpctl_dev_t *dev = m->private;
+	int ret = get_dis_tap_fn(dev);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
+		seq_printf(m, "fail\n");
 	else if (ret == 0)
-		len = sprintf(page, "off\n");
+		seq_printf(m, "off\n");
 	else
-		len = sprintf(page, "on\n");
-
-	*eof = 1;
-	return len;
+		seq_printf(m, "on\n");
+	return 0;
 }
+RW_FOPS(dis_tap)
 
-int
-get_dis_tap_pfs(char *page, char **start, off_t off, int count,
-		int *eof, void *data)
+static ssize_t dis_disc_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_dis_tap_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-	else
-		len = sprintf(page, "on\n");
+	int tap_param = user_on_off(buffer, count);
+	if (tap_param < 0)
+		return -EINVAL;
 
-	*eof = 1;
-	return len;
+	set_dis_disc_fn(PDE_DATA(file_inode(file)), tap_param);
+	return count;
 }
-
-int
-get_dis_disc_pfs(char *page, char **start, off_t off, int count,
-		 int *eof, void *data)
+static int show_dis_disc(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_dis_disc_fn(pbp_device_block);
+	bpctl_dev_t *dev = m->private;
+	int ret = get_dis_disc_fn(dev);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
+		seq_printf(m, "fail\n");
 	else if (ret == 0)
-		len = sprintf(page, "off\n");
+		seq_printf(m, "off\n");
 	else
-		len = sprintf(page, "on\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-set_bypass_pwup_pfs(struct file *file, const char *buffer,
-		    unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int bypass_param = 0, length = 0;
-
-	if (count >= sizeof(kbuf))
-		return -EINVAL;
-
-	if (copy_from_user(&kbuf, buffer, count)) {
-		return -1;
-	}
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		bypass_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		bypass_param = 0;
-
-	set_bypass_pwup_fn(pbp_device_block, bypass_param);
-
-	return count;
+		seq_printf(m, "on\n");
+	return 0;
 }
+RW_FOPS(dis_disc)
 
-int
-set_bypass_pwoff_pfs(struct file *file, const char *buffer,
-		     unsigned long count, void *data)
+static ssize_t bypass_pwup_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int bypass_param = 0, length = 0;
-
-	if (count >= sizeof(kbuf))
+	int bypass_param = user_on_off(buffer, count);
+	if (bypass_param < 0)
 		return -EINVAL;
 
-	if (copy_from_user(&kbuf, buffer, count)) {
-		return -1;
-	}
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		bypass_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		bypass_param = 0;
-
-	set_bypass_pwoff_fn(pbp_device_block, bypass_param);
-
+	set_bypass_pwup_fn(PDE_DATA(file_inode(file)), bypass_param);
 	return count;
 }
-
-int
-set_tap_pwup_pfs(struct file *file, const char *buffer,
-		 unsigned long count, void *data)
+static int show_bypass_pwup(struct seq_file *m, void *v)
 {
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tap_param = 0, length = 0;
-
-	if (count >= sizeof(kbuf))
-		return -EINVAL;
-
-	if (copy_from_user(&kbuf, buffer, count)) {
-		return -1;
-	}
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tap_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tap_param = 0;
-
-	set_tap_pwup_fn(pbp_device_block, tap_param);
-
-	return count;
+	bpctl_dev_t *dev = m->private;
+	int ret = get_bypass_pwup_fn(dev);
+	if (ret == BP_NOT_CAP)
+		seq_printf(m, "fail\n");
+	else if (ret == 0)
+		seq_printf(m, "off\n");
+	else
+		seq_printf(m, "on\n");
+	return 0;
 }
+RW_FOPS(bypass_pwup)
 
-int
-set_disc_pwup_pfs(struct file *file, const char *buffer,
-		  unsigned long count, void *data)
+static ssize_t bypass_pwoff_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tap_param = 0, length = 0;
-
-	if (count >= sizeof(kbuf))
+	int bypass_param = user_on_off(buffer, count);
+	if (bypass_param < 0)
 		return -EINVAL;
 
-	if (copy_from_user(&kbuf, buffer, count)) {
-		return -1;
-	}
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tap_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tap_param = 0;
-
-	set_disc_pwup_fn(pbp_device_block, tap_param);
-
+	set_bypass_pwoff_fn(PDE_DATA(file_inode(file)), bypass_param);
 	return count;
 }
-
-int
-get_bypass_pwup_pfs(char *page, char **start, off_t off, int count,
-		    int *eof, void *data)
+static int show_bypass_pwoff(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_bypass_pwup_fn(pbp_device_block);
+	bpctl_dev_t *dev = m->private;
+	int ret = get_bypass_pwoff_fn(dev);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
+		seq_printf(m, "fail\n");
 	else if (ret == 0)
-		len = sprintf(page, "off\n");
+		seq_printf(m, "off\n");
 	else
-		len = sprintf(page, "on\n");
-
-	*eof = 1;
-	return len;
+		seq_printf(m, "on\n");
+	return 0;
 }
+RW_FOPS(bypass_pwoff)
 
-int
-get_bypass_pwoff_pfs(char *page, char **start, off_t off, int count,
-		     int *eof, void *data)
+static ssize_t tap_pwup_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_bypass_pwoff_fn(pbp_device_block);
-	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
-	else if (ret == 0)
-		len = sprintf(page, "off\n");
-	else
-		len = sprintf(page, "on\n");
+	int tap_param = user_on_off(buffer, count);
+	if (tap_param < 0)
+		return -EINVAL;
 
-	*eof = 1;
-	return len;
+	set_tap_pwup_fn(PDE_DATA(file_inode(file)), tap_param);
+	return count;
 }
-
-int
-get_tap_pwup_pfs(char *page, char **start, off_t off, int count,
-		 int *eof, void *data)
+static int show_tap_pwup(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_tap_pwup_fn(pbp_device_block);
+	bpctl_dev_t *dev = m->private;
+	int ret = get_tap_pwup_fn(dev);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
+		seq_printf(m, "fail\n");
 	else if (ret == 0)
-		len = sprintf(page, "off\n");
+		seq_printf(m, "off\n");
 	else
-		len = sprintf(page, "on\n");
-
-	*eof = 1;
-	return len;
+		seq_printf(m, "on\n");
+	return 0;
 }
+RW_FOPS(tap_pwup)
 
-int
-get_disc_pwup_pfs(char *page, char **start, off_t off, int count,
-		  int *eof, void *data)
+static ssize_t disc_pwup_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
+	int tap_param = user_on_off(buffer, count);
+	if (tap_param < 0)
+		return -EINVAL;
 
-	ret = get_disc_pwup_fn(pbp_device_block);
+	set_disc_pwup_fn(PDE_DATA(file_inode(file)), tap_param);
+	return count;
+}
+static int show_disc_pwup(struct seq_file *m, void *v)
+{
+	bpctl_dev_t *dev = m->private;
+	int ret = get_disc_pwup_fn(dev);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
+		seq_printf(m, "fail\n");
 	else if (ret == 0)
-		len = sprintf(page, "off\n");
+		seq_printf(m, "off\n");
 	else
-		len = sprintf(page, "on\n");
-
-	*eof = 1;
-	return len;
+		seq_printf(m, "on\n");
+	return 0;
 }
+RW_FOPS(disc_pwup)
 
-int
-set_std_nic_pfs(struct file *file, const char *buffer,
-		unsigned long count, void *data)
+static ssize_t std_nic_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int bypass_param = 0, length = 0;
-
-	if (count >= sizeof(kbuf))
+	int bypass_param = user_on_off(buffer, count);
+	if (bypass_param < 0)
 		return -EINVAL;
 
-	if (copy_from_user(&kbuf, buffer, count)) {
-		return -1;
-	}
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		bypass_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		bypass_param = 0;
-
-	set_std_nic_fn(pbp_device_block, bypass_param);
-
+	set_std_nic_fn(PDE_DATA(file_inode(file)), bypass_param);
 	return count;
 }
-
-int
-get_std_nic_pfs(char *page, char **start, off_t off, int count,
-		int *eof, void *data)
+static int show_std_nic(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_std_nic_fn(pbp_device_block);
+	bpctl_dev_t *dev = m->private;
+	int ret = get_std_nic_fn(dev);
 	if (ret == BP_NOT_CAP)
-		len = sprintf(page, "fail\n");
+		seq_printf(m, "fail\n");
 	else if (ret == 0)
-		len = sprintf(page, "off\n");
+		seq_printf(m, "off\n");
 	else
-		len = sprintf(page, "on\n");
-
-	*eof = 1;
-	return len;
-}
-
-int
-get_wd_exp_mode_pfs(char *page, char **start, off_t off, int count,
-		    int *eof, void *data)
-{
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_wd_exp_mode_fn(pbp_device_block);
-	if (ret == 1)
-		len = sprintf(page, "tap\n");
-	else if (ret == 0)
-		len = sprintf(page, "bypass\n");
-	else if (ret == 2)
-		len = sprintf(page, "disc\n");
-
-	else
-		len = sprintf(page, "fail\n");
-
-	*eof = 1;
-	return len;
+		seq_printf(m, "on\n");
+	return 0;
 }
+RW_FOPS(std_nic)
 
-int
-set_wd_exp_mode_pfs(struct file *file, const char *buffer,
-		    unsigned long count, void *data)
+static ssize_t wd_exp_mode_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-
 	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
 	int bypass_param = 0, length = 0;
 
 	if (count > (sizeof(kbuf) - 1))
 		return -1;
 
-	if (copy_from_user(&kbuf, buffer, count)) {
+	if (copy_from_user(&kbuf, buffer, count))
 		return -1;
-	}
 
 	kbuf[count] = '\0';
 	length = strlen(kbuf);
@@ -8375,143 +7786,47 @@ set_wd_exp_mode_pfs(struct file *file, const char *buffer,
 	else if (strcmp(kbuf, "disc") == 0)
 		bypass_param = 2;
 
-	set_wd_exp_mode_fn(pbp_device_block, bypass_param);
+	set_wd_exp_mode_fn(PDE_DATA(file_inode(file)), bypass_param);
 
 	return count;
 }
-
-int
-get_wd_autoreset_pfs(char *page, char **start, off_t off, int count,
-		     int *eof, void *data)
+static int show_wd_exp_mode(struct seq_file *m, void *v)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int len = 0, ret = 0;
-
-	ret = get_wd_autoreset_fn(pbp_device_block);
-	if (ret >= 0)
-		len = sprintf(page, "%d\n", ret);
+	bpctl_dev_t *dev = m->private;
+	int ret = get_wd_exp_mode_fn(dev);
+	if (ret == 1)
+		seq_printf(m, "tap\n");
+	else if (ret == 0)
+		seq_printf(m, "bypass\n");
+	else if (ret == 2)
+		seq_printf(m, "disc\n");
 	else
-		len = sprintf(page, "fail\n");
-
-	*eof = 1;
-	return len;
+		seq_printf(m, "fail\n");
+	return 0;
 }
+RW_FOPS(wd_exp_mode)
 
-int
-set_wd_autoreset_pfs(struct file *file, const char *buffer,
-		     unsigned long count, void *data)
+static ssize_t wd_autoreset_write(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos)
 {
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
 	int timeout;
-	int ret;
-
-	ret = kstrtoint_from_user(buffer, count, 10, &timeout);
+	int ret = kstrtoint_from_user(buffer, count, 10, &timeout);
 	if (ret)
 		return ret;
-	set_wd_autoreset_fn(pbp_device_block, timeout);
-
-	return count;
-}
-
-int
-set_tpl_pfs(struct file *file, const char *buffer,
-	    unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tpl_param = 0, length = 0;
-
-	if (count > (sizeof(kbuf) - 1))
-		return -1;
-
-	if (copy_from_user(&kbuf, buffer, count)) {
-		return -1;
-	}
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tpl_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tpl_param = 0;
-
-	set_tpl_fn(pbp_device_block, tpl_param);
-
+	set_wd_autoreset_fn(PDE_DATA(file_inode(file)), timeout);
 	return count;
 }
-
-#ifdef PMC_FIX_FLAG
-int
-set_wait_at_pwup_pfs(struct file *file, const char *buffer,
-		     unsigned long count, void *data)
-{
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tpl_param = 0, length = 0;
-
-	if (count > (sizeof(kbuf) - 1))
-		return -1;
-
-	if (copy_from_user(&kbuf, buffer, count)) {
-		return -1;
-	}
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tpl_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tpl_param = 0;
-
-	set_bp_wait_at_pwup_fn(pbp_device_block, tpl_param);
-
-	return count;
-}
-
-int
-set_hw_reset_pfs(struct file *file, const char *buffer,
-		 unsigned long count, void *data)
+static int show_wd_autoreset(struct seq_file *m, void *v)
 {
-
-	char kbuf[256];
-	bpctl_dev_t *pbp_device_block = (bpctl_dev_t *) data;
-
-	int tpl_param = 0, length = 0;
-
-	if (count > (sizeof(kbuf) - 1))
-		return -1;
-
-	if (copy_from_user(&kbuf, buffer, count)) {
-		return -1;
-	}
-
-	kbuf[count] = '\0';
-	length = strlen(kbuf);
-	if (kbuf[length - 1] == '\n')
-		kbuf[--length] = '\0';
-
-	if (strcmp(kbuf, "on") == 0)
-		tpl_param = 1;
-	else if (strcmp(kbuf, "off") == 0)
-		tpl_param = 0;
-
-	set_bp_hw_reset_fn(pbp_device_block, tpl_param);
-
-	return count;
+	bpctl_dev_t *dev = m->private;
+	int ret = get_wd_autoreset_fn(dev);
+	if (ret >= 0)
+		seq_printf(m, "%d\n", ret);
+	else
+		seq_printf(m, "fail\n");
+	return 0;
 }
-
-#endif				/*PMC_FIX_FLAG */
+RW_FOPS(wd_autoreset)
 
 int bypass_proc_create_dev_sd(bpctl_dev_t *pbp_device_block)
 {
@@ -8528,168 +7843,54 @@ int bypass_proc_create_dev_sd(bpctl_dev_t *pbp_device_block)
 		return -1;
 
 	/* create device proc dir */
-	procfs_dir = proc_getdir(current_pfs->dir_name, bp_procfs_dir);
-	if (procfs_dir == 0) {
+	procfs_dir = proc_mkdir(current_pfs->dir_name, bp_procfs_dir);
+	if (!procfs_dir) {
 		printk(KERN_DEBUG "Could not create procfs directory %s\n",
 		       current_pfs->dir_name);
 		return -1;
 	}
 	current_pfs->bypass_entry = procfs_dir;
 
-	if (bypass_proc_create_entry_sd(&(current_pfs->bypass_info), BYPASS_INFO_ENTRY_SD, NULL,	/* write */
-					get_bypass_info_pfs,	/* read  */
-					procfs_dir, pbp_device_block))
-		ret = -1;
-
+#define ENTRY(x) ret |= procfs_add(#x, &x##_ops, pbp_device_block)
+	ENTRY(bypass_info);
 	if (pbp_device_block->bp_caps & SW_CTL_CAP) {
-
 		/* Create set param proc's */
-		if (bypass_proc_create_entry_sd(&(current_pfs->bypass_slave), BYPASS_SLAVE_ENTRY_SD, NULL,	/* write */
-						get_bypass_slave_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
-		if (bypass_proc_create_entry_sd(&(current_pfs->bypass_caps), BYPASS_CAPS_ENTRY_SD, NULL,	/* write */
-						get_bypass_caps_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
-		if (bypass_proc_create_entry_sd(&(current_pfs->wd_set_caps), WD_SET_CAPS_ENTRY_SD, NULL,	/* write */
-						get_wd_set_caps_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-		if (bypass_proc_create_entry_sd(&(current_pfs->bypass_wd), BYPASS_WD_ENTRY_SD, set_bypass_wd_pfs,	/* write */
-						get_bypass_wd_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
-		if (bypass_proc_create_entry_sd(&(current_pfs->wd_expire_time), WD_EXPIRE_TIME_ENTRY_SD, NULL,	/* write */
-						get_wd_expire_time_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
-		if (bypass_proc_create_entry_sd(&(current_pfs->reset_bypass_wd), RESET_BYPASS_WD_ENTRY_SD, NULL,	/* write */
-						reset_bypass_wd_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
-		if (bypass_proc_create_entry_sd(&(current_pfs->std_nic), STD_NIC_ENTRY_SD, set_std_nic_pfs,	/* write */
-						get_std_nic_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
+		ENTRY(bypass_slave);
+		ENTRY(bypass_caps);
+		ENTRY(wd_set_caps);
+		ENTRY(bypass_wd);
+		ENTRY(wd_expire_time);
+		ENTRY(reset_bypass_wd);
+		ENTRY(std_nic);
 		if (pbp_device_block->bp_caps & BP_CAP) {
-			if (bypass_proc_create_entry_sd(&(current_pfs->bypass), BYPASS_ENTRY_SD, set_bypass_pfs,	/* write */
-							get_bypass_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->dis_bypass), DIS_BYPASS_ENTRY_SD, set_dis_bypass_pfs,	/* write */
-							get_dis_bypass_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->bypass_pwup), BYPASS_PWUP_ENTRY_SD, set_bypass_pwup_pfs,	/* write */
-							get_bypass_pwup_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-			if (bypass_proc_create_entry_sd(&(current_pfs->bypass_pwoff), BYPASS_PWOFF_ENTRY_SD, set_bypass_pwoff_pfs,	/* write */
-							get_bypass_pwoff_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->bypass_change), BYPASS_CHANGE_ENTRY_SD, NULL,	/* write */
-							get_bypass_change_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
+			ENTRY(bypass);
+			ENTRY(dis_bypass);
+			ENTRY(bypass_pwup);
+			ENTRY(bypass_pwoff);
+			ENTRY(bypass_change);
 		}
-
 		if (pbp_device_block->bp_caps & TAP_CAP) {
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->tap), TAP_ENTRY_SD, set_tap_pfs,	/* write */
-							get_tap_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->dis_tap), DIS_TAP_ENTRY_SD, set_dis_tap_pfs,	/* write */
-							get_dis_tap_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->tap_pwup), TAP_PWUP_ENTRY_SD, set_tap_pwup_pfs,	/* write */
-							get_tap_pwup_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->tap_change), TAP_CHANGE_ENTRY_SD, NULL,	/* write */
-							get_tap_change_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
+			ENTRY(tap);
+			ENTRY(dis_tap);
+			ENTRY(tap_pwup);
+			ENTRY(tap_change);
 		}
 		if (pbp_device_block->bp_caps & DISC_CAP) {
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->tap), DISC_ENTRY_SD, set_disc_pfs,	/* write */
-							get_disc_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-#if 1
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->dis_tap), DIS_DISC_ENTRY_SD, set_dis_disc_pfs,	/* write */
-							get_dis_disc_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-#endif
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->tap_pwup), DISC_PWUP_ENTRY_SD, set_disc_pwup_pfs,	/* write */
-							get_disc_pwup_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
-
-			if (bypass_proc_create_entry_sd(&(current_pfs->tap_change), DISC_CHANGE_ENTRY_SD, NULL,	/* write */
-							get_disc_change_pfs,	/* read  */
-							procfs_dir,
-							pbp_device_block))
-				ret = -1;
+			ENTRY(disc);
+			ENTRY(dis_disc);
+			ENTRY(disc_pwup);
+			ENTRY(disc_change);
 		}
 
-		if (bypass_proc_create_entry_sd(&(current_pfs->wd_exp_mode), WD_EXP_MODE_ENTRY_SD, set_wd_exp_mode_pfs,	/* write */
-						get_wd_exp_mode_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
-		if (bypass_proc_create_entry_sd(&(current_pfs->wd_autoreset), WD_AUTORESET_ENTRY_SD, set_wd_autoreset_pfs,	/* write */
-						get_wd_autoreset_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-		if (bypass_proc_create_entry_sd(&(current_pfs->tpl), TPL_ENTRY_SD, set_tpl_pfs,	/* write */
-						get_tpl_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
+		ENTRY(wd_exp_mode);
+		ENTRY(wd_autoreset);
+		ENTRY(tpl);
 #ifdef PMC_FIX_FLAG
-		if (bypass_proc_create_entry_sd(&(current_pfs->tpl), WAIT_AT_PWUP_ENTRY_SD, set_wait_at_pwup_pfs,	/* write */
-						get_wait_at_pwup_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-		if (bypass_proc_create_entry_sd(&(current_pfs->tpl), HW_RESET_ENTRY_SD, set_hw_reset_pfs,	/* write */
-						get_hw_reset_pfs,	/* read  */
-						procfs_dir, pbp_device_block))
-			ret = -1;
-
+		ENTRY(wait_at_pwup);
+		ENTRY(hw_reset);
 #endif
-
 	}
+#undef ENTRY
 	if (ret < 0)
 		printk(KERN_DEBUG "Create proc entry failed\n");
 
@@ -8700,21 +7901,7 @@ int bypass_proc_remove_dev_sd(bpctl_dev_t *pbp_device_block)
 {
 
 	struct bypass_pfs_sd *current_pfs = &pbp_device_block->bypass_pfs_set;
-	struct proc_dir_entry *pde = current_pfs->bypass_entry, *pde_curr =
-	    NULL;
-	char name[256];
-
-	if (!pde)
-		return 0;
-	for (pde = pde->subdir; pde;) {
-		strcpy(name, pde->name);
-		pde_curr = pde;
-		pde = pde->next;
-		remove_proc_entry(name, current_pfs->bypass_entry);
-	}
-	if (!pde)
-		remove_proc_entry(current_pfs->dir_name, bp_procfs_dir);
+	remove_proc_subtree(current_pfs->dir_name, bp_procfs_dir);
 	current_pfs->bypass_entry = NULL;
-
 	return 0;
 }
diff --git a/drivers/staging/speakup/kobjects.c b/drivers/staging/speakup/kobjects.c
index d6d9264e4ca7..943b6c134a22 100644
--- a/drivers/staging/speakup/kobjects.c
+++ b/drivers/staging/speakup/kobjects.c
@@ -15,6 +15,7 @@
 #include <linux/kernel.h>
 #include <linux/kobject.h>
 #include <linux/string.h>
+#include <linux/string_helpers.h>
 #include <linux/sysfs.h>
 #include <linux/ctype.h>
 
@@ -417,7 +418,7 @@ static ssize_t synth_direct_store(struct kobject *kobj,
 		bytes = min_t(size_t, len, 250);
 		strncpy(tmp, ptr, bytes);
 		tmp[bytes] = '\0';
-		spk_xlate(tmp);
+		string_unescape_any_inplace(tmp);
 		synth_printf("%s", tmp);
 		ptr += bytes;
 		len -= bytes;
@@ -605,7 +606,8 @@ ssize_t spk_var_store(struct kobject *kobj, struct kobj_attribute *attr,
 	if (param->data == NULL)
 		return 0;
 	ret = 0;
-	cp = spk_xlate((char *) buf);
+	cp = (char *)buf;
+	string_unescape_any_inplace(cp);
 
 	spk_lock(flags);
 	switch (param->var_type) {
diff --git a/drivers/staging/speakup/speakup.h b/drivers/staging/speakup/speakup.h
index c387a02fc1c2..0126f714821a 100644
--- a/drivers/staging/speakup/speakup.h
+++ b/drivers/staging/speakup/speakup.h
@@ -54,7 +54,6 @@ void spk_get_index_count(int *linecount, int *sentcount);
 extern int spk_set_key_info(const u_char *key_info, u_char *k_buffer);
 extern char *spk_strlwr(char *s);
 extern char *spk_s2uchar(char *start, char *dest);
-extern char *spk_xlate(char *s);
 extern int speakup_kobj_init(void);
 extern void speakup_kobj_exit(void);
 extern int spk_chartab_get_value(char *keyword);
diff --git a/drivers/staging/speakup/varhandlers.c b/drivers/staging/speakup/varhandlers.c
index 0099cb12e560..7f6288fc2299 100644
--- a/drivers/staging/speakup/varhandlers.c
+++ b/drivers/staging/speakup/varhandlers.c
@@ -328,49 +328,3 @@ char *spk_s2uchar(char *start, char *dest)
 	*dest = (u_char)val;
 	return start;
 }
-
-char *spk_xlate(char *s)
-{
-	static const char finds[] = "nrtvafe";
-	static const char subs[] = "\n\r\t\013\001\014\033";
-	static const char hx[] = "0123456789abcdefABCDEF";
-	char *p = s, *p1, *p2, c;
-	int num;
-	while ((p = strchr(p, '\\'))) {
-		p1 = p+1;
-		p2 = strchr(finds, *p1);
-		if (p2) {
-			*p++ = subs[p2-finds];
-			p1++;
-		} else if (*p1 >= '0' && *p1 <= '7') {
-			num = (*p1++)&7;
-			while (num < 32 && *p1 >= '0' && *p1 <= '7') {
-				num <<= 3;
-				num += (*p1++)&7;
-			}
-			*p++ = num;
-		} else if (*p1 == 'x' &&
-				strchr(hx, p1[1]) && strchr(hx, p1[2])) {
-			p1++;
-			c = *p1++;
-			if (c > '9')
-				c = (c - '7') & 0x0f;
-			else
-				c -= '0';
-			num = c << 4;
-			c = *p1++;
-			if (c > '9')
-				c = (c-'7')&0x0f;
-			else
-				c -= '0';
-			num += c;
-			*p++ = num;
-		} else
-			*p++ = *p1++;
-		p2 = p;
-		while (*p1)
-			*p2++ = *p1++;
-		*p2 = '\0';
-	}
-	return s;
-}
diff --git a/drivers/staging/ste_rmi4/Makefile b/drivers/staging/ste_rmi4/Makefile
index e4c03351420f..6cce2ed187ef 100644
--- a/drivers/staging/ste_rmi4/Makefile
+++ b/drivers/staging/ste_rmi4/Makefile
@@ -2,4 +2,3 @@
 # Makefile for the RMI4 touchscreen driver.
 #
 obj-$(CONFIG_TOUCHSCREEN_SYNAPTICS_I2C_RMI4) += synaptics_i2c_rmi4.o
-obj-$(CONFIG_MACH_MOP500) += board-mop500-u8500uib-rmi4.o
diff --git a/drivers/staging/ste_rmi4/board-mop500-u8500uib-rmi4.c b/drivers/staging/ste_rmi4/board-mop500-u8500uib-rmi4.c
deleted file mode 100644
index 47439c3f7258..000000000000
--- a/drivers/staging/ste_rmi4/board-mop500-u8500uib-rmi4.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * Some platform data for the RMI4 touchscreen that will override the __weak
- * platform data in the Ux500 machine if this driver is activated.
- */
-#include <linux/i2c.h>
-#include <linux/gpio.h>
-#include <linux/interrupt.h>
-#include <mach/irqs.h>
-#include "synaptics_i2c_rmi4.h"
-
-/*
- * Synaptics RMI4 touchscreen interface on the U8500 UIB
- */
-
-/*
- * Descriptor structure.
- * Describes the number of i2c devices on the bus that speak RMI.
- */
-static struct synaptics_rmi4_platform_data rmi4_i2c_dev_platformdata = {
-	.irq_number     = NOMADIK_GPIO_TO_IRQ(84),
-	.irq_type       = (IRQF_TRIGGER_FALLING | IRQF_SHARED),
-	.x_flip		= false,
-	.y_flip		= true,
-};
-
-struct i2c_board_info __initdata mop500_i2c3_devices_u8500[] = {
-	{
-		I2C_BOARD_INFO("synaptics_rmi4_i2c", 0x4B),
-		.platform_data = &rmi4_i2c_dev_platformdata,
-	},
-};
diff --git a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
index a126b25c82d5..fe667dde43ce 100644
--- a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
+++ b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
@@ -864,6 +864,16 @@ static int synaptics_rmi4_i2c_query_device(struct synaptics_rmi4_data *pdata)
 	return 0;
 }
 
+/*
+ * Descriptor structure.
+ * Describes the number of i2c devices on the bus that speak RMI.
+ */
+static struct synaptics_rmi4_platform_data synaptics_rmi4_platformdata = {
+	.irq_type       = (IRQF_TRIGGER_FALLING | IRQF_SHARED),
+	.x_flip		= false,
+	.y_flip		= true,
+};
+
 /**
  * synaptics_rmi4_probe() - Initialze the i2c-client touchscreen driver
  * @i2c: i2c client structure pointer
@@ -890,10 +900,8 @@ static int synaptics_rmi4_probe
 		return -EIO;
 	}
 
-	if (!platformdata) {
-		dev_err(&client->dev, "%s: no platform data\n", __func__);
-		return -EINVAL;
-	}
+	if (!platformdata)
+		platformdata = &synaptics_rmi4_platformdata;
 
 	/* Allocate and initialize the instance data for this client */
 	rmi4_data = kcalloc(2, sizeof(struct synaptics_rmi4_data),
@@ -977,13 +985,13 @@ static int synaptics_rmi4_probe
 	synaptics_rmi4_i2c_block_read(rmi4_data,
 			rmi4_data->fn01_data_base_addr + 1, intr_status,
 				rmi4_data->number_of_interrupt_register);
-	retval = request_threaded_irq(platformdata->irq_number, NULL,
+	retval = request_threaded_irq(client->irq, NULL,
 					synaptics_rmi4_irq,
 					platformdata->irq_type,
 					DRIVER_NAME, rmi4_data);
 	if (retval) {
 		dev_err(&client->dev, "%s:Unable to get attn irq %d\n",
-				__func__, platformdata->irq_number);
+				__func__, client->irq);
 		goto err_query_dev;
 	}
 
@@ -996,7 +1004,7 @@ static int synaptics_rmi4_probe
 	return retval;
 
 err_free_irq:
-	free_irq(platformdata->irq_number, rmi4_data);
+	free_irq(client->irq, rmi4_data);
 err_query_dev:
 	regulator_disable(rmi4_data->regulator);
 err_regulator_enable:
@@ -1019,11 +1027,10 @@ err_input:
 static int synaptics_rmi4_remove(struct i2c_client *client)
 {
 	struct synaptics_rmi4_data *rmi4_data = i2c_get_clientdata(client);
-	const struct synaptics_rmi4_platform_data *pdata = rmi4_data->board;
 
 	rmi4_data->touch_stopped = true;
 	wake_up(&rmi4_data->wait);
-	free_irq(pdata->irq_number, rmi4_data);
+	free_irq(client->irq, rmi4_data);
 	input_unregister_device(rmi4_data->input_dev);
 	regulator_disable(rmi4_data->regulator);
 	regulator_put(rmi4_data->regulator);
@@ -1046,10 +1053,9 @@ static int synaptics_rmi4_suspend(struct device *dev)
 	int retval;
 	unsigned char intr_status;
 	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
-	const struct synaptics_rmi4_platform_data *pdata = rmi4_data->board;
 
 	rmi4_data->touch_stopped = true;
-	disable_irq(pdata->irq_number);
+	disable_irq(rmi4_data->i2c_client->irq);
 
 	retval = synaptics_rmi4_i2c_block_read(rmi4_data,
 				rmi4_data->fn01_data_base_addr + 1,
@@ -1080,11 +1086,10 @@ static int synaptics_rmi4_resume(struct device *dev)
 	int retval;
 	unsigned char intr_status;
 	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
-	const struct synaptics_rmi4_platform_data *pdata = rmi4_data->board;
 
 	regulator_enable(rmi4_data->regulator);
 
-	enable_irq(pdata->irq_number);
+	enable_irq(rmi4_data->i2c_client->irq);
 	rmi4_data->touch_stopped = false;
 
 	retval = synaptics_rmi4_i2c_block_read(rmi4_data,
diff --git a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.h b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.h
index 384436ef8068..8c9166ba71c7 100644
--- a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.h
+++ b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.h
@@ -38,7 +38,6 @@
  * This structure gives platform data for rmi4.
  */
 struct synaptics_rmi4_platform_data {
-	int irq_number;
 	int irq_type;
 	bool x_flip;
 	bool y_flip;
diff --git a/drivers/staging/vt6655/device_main.c b/drivers/staging/vt6655/device_main.c
index be4f6c2ca3ff..08b250f01dae 100644
--- a/drivers/staging/vt6655/device_main.c
+++ b/drivers/staging/vt6655/device_main.c
@@ -60,6 +60,7 @@
  */
 #undef __NO_VERSION__
 
+#include <linux/file.h>
 #include "device.h"
 #include "card.h"
 #include "channel.h"
@@ -2737,83 +2738,50 @@ static int Config_FileGetParameter(unsigned char *string,
 	return true;
 }
 
-int Config_FileOperation(PSDevice pDevice, bool fwrite, unsigned char *Parameter) {
-	unsigned char *config_path = CONFIG_PATH;
-	unsigned char *buffer = NULL;
+int Config_FileOperation(PSDevice pDevice,bool fwrite,unsigned char *Parameter)
+{
+	unsigned char *buffer = kmalloc(1024, GFP_KERNEL);
 	unsigned char tmpbuffer[20];
-	struct file   *filp = NULL;
-	mm_segment_t old_fs = get_fs();
-	//int oldfsuid=0,oldfsgid=0;
-	int result = 0;
-
-	set_fs(KERNEL_DS);
-
-	/* Can't do this anymore, so we rely on correct filesystem permissions:
-	//Make sure a caller can read or write power as root
-	oldfsuid=current->cred->fsuid;
-	oldfsgid=current->cred->fsgid;
-	current->cred->fsuid = 0;
-	current->cred->fsgid = 0;
-	*/
-
-	//open file
-	filp = filp_open(config_path, O_RDWR, 0);
-	if (IS_ERR(filp)) {
-		printk("Config_FileOperation:open file fail?\n");
-		result = -1;
-		goto error2;
-	}
+	struct file *file;
+	int result=0;
 
-	if (!(filp->f_op) || !(filp->f_op->read) || !(filp->f_op->write)) {
-		printk("file %s cann't readable or writable?\n", config_path);
-		result = -1;
-		goto error1;
-	}
-
-	buffer = kmalloc(1024, GFP_KERNEL);
-	if (buffer == NULL) {
+	if (!buffer) {
 		printk("allocate mem for file fail?\n");
-		result = -1;
-		goto error1;
+		return -1;
+	}
+	file = filp_open(CONFIG_PATH, O_RDONLY, 0);
+	if (IS_ERR(file)) {
+		kfree(buffer);
+		printk("Config_FileOperation:open file fail?\n");
+		return -1;
 	}
 
-	if (filp->f_op->read(filp, buffer, 1024, &filp->f_pos) < 0) {
+	if (kernel_read(file, 0, buffer, 1024) < 0) {
 		printk("read file error?\n");
 		result = -1;
 		goto error1;
 	}
 
-	if (Config_FileGetParameter("ZONETYPE", tmpbuffer, buffer) != true) {
+	if (Config_FileGetParameter("ZONETYPE",tmpbuffer,buffer)!=true) {
 		printk("get parameter error?\n");
 		result = -1;
 		goto error1;
 	}
 
-	if (memcmp(tmpbuffer, "USA", 3) == 0) {
+	if (memcmp(tmpbuffer,"USA",3)==0) {
 		result = ZoneType_USA;
-	} else if (memcmp(tmpbuffer, "JAPAN", 5) == 0) {
+	} else if(memcmp(tmpbuffer,"JAPAN",5)==0) {
 		result = ZoneType_Japan;
-	} else if (memcmp(tmpbuffer, "EUROPE", 5) == 0) {
+	} else if(memcmp(tmpbuffer,"EUROPE",5)==0) {
 		result = ZoneType_Europe;
 	} else {
 		result = -1;
-		printk("Unknown Zonetype[%s]?\n", tmpbuffer);
+		printk("Unknown Zonetype[%s]?\n",tmpbuffer);
 	}
 
 error1:
 	kfree(buffer);
-
-	if (filp_close(filp, NULL))
-		printk("Config_FileOperation:close file fail\n");
-
-error2:
-	set_fs(old_fs);
-
-	/*
-	  current->cred->fsuid=oldfsuid;
-	  current->cred->fsgid=oldfsgid;
-	*/
-
+	fput(file);
 	return result;
 }
 
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index 2161af83eaa0..3a3fdc58b6da 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -46,6 +46,7 @@
  */
 #undef __NO_VERSION__
 
+#include <linux/file.h>
 #include "device.h"
 #include "card.h"
 #include "baseband.h"
@@ -1273,53 +1274,29 @@ static int Config_FileGetParameter(unsigned char *string,
 /* if read fails, return NULL, or return data pointer */
 static unsigned char *Config_FileOperation(struct vnt_private *pDevice)
 {
-    unsigned char *config_path = CONFIG_PATH;
-    unsigned char *buffer = NULL;
-    struct file   *filp=NULL;
-    mm_segment_t old_fs = get_fs();
+	unsigned char *buffer = kmalloc(1024, GFP_KERNEL);
+	struct file   *file;
 
-    int result = 0;
-
-    set_fs (KERNEL_DS);
-
-    /* open file */
-      filp = filp_open(config_path, O_RDWR, 0);
-        if (IS_ERR(filp)) {
-	     printk("Config_FileOperation file Not exist\n");
-	     result=-1;
-             goto error2;
-	  }
-
-     if(!(filp->f_op) || !(filp->f_op->read) ||!(filp->f_op->write)) {
-           printk("file %s is not read or writeable?\n",config_path);
-	  result = -1;
-	  goto error1;
-     	}
-
-    buffer = kmalloc(1024, GFP_KERNEL);
-    if(buffer==NULL) {
-      printk("allocate mem for file fail?\n");
-      result = -1;
-      goto error1;
-    }
-
-    if(filp->f_op->read(filp, buffer, 1024, &filp->f_pos)<0) {
-     printk("read file error?\n");
-     result = -1;
-    }
+	if (!buffer) {
+		printk("allocate mem for file fail?\n");
+		return NULL;
+	}
 
-error1:
-  if(filp_close(filp,NULL))
-       printk("Config_FileOperation:close file fail\n");
+	file = filp_open(CONFIG_PATH, O_RDONLY, 0);
+	if (IS_ERR(file)) {
+		kfree(buffer);
+		printk("Config_FileOperation file Not exist\n");
+		return NULL;
+	}
 
-error2:
-  set_fs (old_fs);
+	if (kernel_read(file, 0, buffer, 1024) < 0) {
+		printk("read file error?\n");
+		kfree(buffer);
+		buffer = NULL;
+	}
 
-if(result!=0) {
-    kfree(buffer);
-    buffer=NULL;
-}
-  return buffer;
+	fput(file);
+	return buffer;
 }
 
 /* return --->-1:fail; >=0:successful */
diff --git a/drivers/staging/wlags49_h2/wl_main.c b/drivers/staging/wlags49_h2/wl_main.c
index f5f120a62460..f28f15baea96 100644
--- a/drivers/staging/wlags49_h2/wl_main.c
+++ b/drivers/staging/wlags49_h2/wl_main.c
@@ -73,6 +73,7 @@
 
 #include <linux/module.h>
 #include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 #include <linux/types.h>
 #include <linux/kernel.h>
 // #include <linux/sched.h>
@@ -144,10 +145,23 @@
 void wl_isr_handler( unsigned long p );
 
 #if 0 //SCULL_USE_PROC /* don't waste space if unused */
-//int scull_read_procmem(char *buf, char **start, off_t offset, int len, int unused);
-int scull_read_procmem(char *buf, char **start, off_t offset, int len, int *eof, void *data );
+static int scull_read_procmem(struct seq_file *m, void *v);
 static int write_int(struct file *file, const char *buffer, unsigned long count, void *data);
-static void proc_write(const char *name, write_proc_t *w, void *data);
+
+/*
+ * seq_file wrappers for procfile show routines.
+ */
+static int scull_read_procmem_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, scull_read_procmem, PDE_DATA(inode));
+}
+
+static const struct file_operations scull_read_procmem_fops = {
+	.open		= scull_read_procmem_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 
 #endif /* SCULL_USE_PROC */
 
@@ -908,9 +922,8 @@ int wl_insert( struct net_device *dev )
 	}
 
 #if 0 //SCULL_USE_PROC /* don't waste space if unused */
-	create_proc_read_entry( "wlags", 0, NULL, scull_read_procmem, dev );
+	proc_create_data( "wlags", 0, NULL, &scull_read_procmem_fops, dev );
 	proc_mkdir("driver/wlags49", 0);
-	proc_write("driver/wlags49/wlags49_type", write_int, &lp->wlags49_type);
 #endif /* SCULL_USE_PROC */
 
 	DBG_LEAVE( DbgInfo );
@@ -2097,7 +2110,7 @@ static void __exit wl_module_exit( void )
 
 	wl_adapter_cleanup_module( );
 #if 0 //SCULL_USE_PROC /* don't waste space if unused */
-	remove_proc_entry( "wlags", NULL );		//;?why so a-symmetric compared to location of create_proc_read_entry
+	remove_proc_entry( "wlags", NULL );		//;?why so a-symmetric compared to location of proc_create_data
 #endif
 
 	DBG_LEAVE( DbgInfo );
@@ -3531,229 +3544,215 @@ void wl_wds_netdev_deregister( struct wl_private *lp )
 /*
  * The proc filesystem: function to read and entry
  */
-int printf_hcf_16( char *s, char *buf, hcf_16* p, int n );
-int printf_hcf_16( char *s, char *buf, hcf_16* p, int n ) {
+static void printf_hcf_16(struct seq_file *m, const char *s, hcf_16 *p, int n)
+{
+	int i, len;
 
-int i, len;
+	seq_printf(m, "%-20.20s: ", s);
+	len = 22;
 
-	len = sprintf(buf, "%s", s );
-	while ( len < 20 ) len += sprintf(buf+len, " " );
-	len += sprintf(buf+len,": " );
-	for ( i = 0; i < n; i++ ) {
-		if ( len % 80 > 75 ) {
-			len += sprintf(buf+len,"\n" );
-		}
-		len += sprintf(buf+len,"%04X ", p[i] );
+	for (i = 0; i < n; i++) {
+		if (len % 80 > 75)
+			seq_putc(m, '\n');
+		seq_printf(m, "%04X ", p[i]);
 	}
-	len += sprintf(buf+len,"\n" );
-	return len;
-} // printf_hcf_16
+	seq_putc(m, '\n');
+}
 
-int printf_hcf_8( char *s, char *buf, hcf_8* p, int n );
-int printf_hcf_8( char *s, char *buf, hcf_8* p, int n ) {
+static void printf_hcf_8(struct seq_file *m, const char *s, hcf_8 *p, int n)
+{
+	int i, len;
 
-int i, len;
+	seq_printf(m, "%-20.20s: ", s);
+	len = 22;
 
-	len = sprintf(buf, "%s", s );
-	while ( len < 20 ) len += sprintf(buf+len, " " );
-	len += sprintf(buf+len,": " );
-	for ( i = 0; i <= n; i++ ) {
-		if ( len % 80 > 77 ) {
-			len += sprintf(buf+len,"\n" );
-		}
-		len += sprintf(buf+len,"%02X ", p[i] );
+	for (i = 0; i <= n; i++) {
+		if (len % 80 > 77)
+			seq_putc(m, '\n');
+		seq_printf(m, "%02X ", p[i]);
 	}
-	len += sprintf(buf+len,"\n" );
-	return len;
-} // printf_hcf8
+	seq_putc(m, '\n');
+}
 
-int printf_strct( char *s, char *buf, hcf_16* p );
-int printf_strct( char *s, char *buf, hcf_16* p ) {
+static void printf_strct(struct seq_file *m, const char *s, hcf_16 *p)
+{
+	int i, len;
 
-int i, len;
+	seq_printf(m, "%-20.20s: ", s);
+	len = 22;
 
-	len = sprintf(buf, "%s", s );
-	while ( len < 20 ) len += sprintf(buf+len, " " );
-	len += sprintf(buf+len,": " );
 	for ( i = 0; i <= *p; i++ ) {
-		if ( len % 80 > 75 ) {
-			len += sprintf(buf+len,"\n" );
-		}
-		len += sprintf(buf+len,"%04X ", p[i] );
+		if (len % 80 > 75)
+			seq_putc(m, '\n');
+		seq_printf(m,"%04X ", p[i]);
 	}
-	len += sprintf(buf+len,"\n" );
-	return len;
-} // printf_strct
+	seq_putc(m, '\n');
+}
 
-int scull_read_procmem(char *buf, char **start, off_t offset, int len, int *eof, void *data )
+int scull_read_procmem(struct seq_file *m, void *v)
 {
-	struct wl_private	*lp = NULL;
+	struct wl_private	*lp = m->private;
 	IFBP				ifbp;
    	CFG_HERMES_TALLIES_STRCT *p;
 
-    #define LIMIT (PAGE_SIZE-80) /* don't print any more after this size */
-
-    len=0;
-
-	lp = ((struct net_device *)data)->priv;
 	if (lp == NULL) {
-        len += sprintf(buf+len,"No wl_private in scull_read_procmem\n" );
+		seq_puts(m, "No wl_private in scull_read_procmem\n" );
 	} else if ( lp->wlags49_type == 0 ){
-   	    ifbp = &lp->hcfCtx;
-   	    len += sprintf(buf+len,"Magic:               0x%04X\n", ifbp->IFB_Magic );
-   	    len += sprintf(buf+len,"IOBase:              0x%04X\n", ifbp->IFB_IOBase );
-   	    len += sprintf(buf+len,"LinkStat:            0x%04X\n", ifbp->IFB_LinkStat );
-   	    len += sprintf(buf+len,"DSLinkStat:          0x%04X\n", ifbp->IFB_DSLinkStat );
-   	    len += sprintf(buf+len,"TickIni:         0x%08lX\n", ifbp->IFB_TickIni );
-   	    len += sprintf(buf+len,"TickCnt:             0x%04X\n", ifbp->IFB_TickCnt );
-   	    len += sprintf(buf+len,"IntOffCnt:           0x%04X\n", ifbp->IFB_IntOffCnt );
-		len += printf_hcf_16( "IFB_FWIdentity", &buf[len],
-							  &ifbp->IFB_FWIdentity.len, ifbp->IFB_FWIdentity.len + 1 );
+		ifbp = &lp->hcfCtx;
+		seq_printf(m, "Magic:               0x%04X\n", ifbp->IFB_Magic );
+		seq_printf(m, "IOBase:              0x%04X\n", ifbp->IFB_IOBase );
+		seq_printf(m, "LinkStat:            0x%04X\n", ifbp->IFB_LinkStat );
+		seq_printf(m, "DSLinkStat:          0x%04X\n", ifbp->IFB_DSLinkStat );
+		seq_printf(m, "TickIni:         0x%08lX\n", ifbp->IFB_TickIni );
+		seq_printf(m, "TickCnt:             0x%04X\n", ifbp->IFB_TickCnt );
+		seq_printf(m, "IntOffCnt:           0x%04X\n", ifbp->IFB_IntOffCnt );
+		printf_hcf_16(m, "IFB_FWIdentity",
+			      &ifbp->IFB_FWIdentity.len, ifbp->IFB_FWIdentity.len + 1 );
 	} else if ( lp->wlags49_type == 1 ) {
-   	    len += sprintf(buf+len,"Channel:              0x%04X\n", lp->Channel );
-/****** len += sprintf(buf+len,"slock:                  %d\n", lp->slock );		*/
+		seq_printf(m, "Channel:              0x%04X\n", lp->Channel );
+/****** seq_printf(m, "slock:                  %d\n", lp->slock );		*/
 //x		struct tq_struct            "task:               0x%04X\n", lp->task );
 //x		struct net_device_stats     "stats:              0x%04X\n", lp->stats );
 #ifdef WIRELESS_EXT
 //x		struct iw_statistics        "wstats:             0x%04X\n", lp->wstats );
-//x   	    len += sprintf(buf+len,"spy_number:           0x%04X\n", lp->spy_number );
+//x   	    seq_printf(m, "spy_number:           0x%04X\n", lp->spy_number );
 //x		u_char                      spy_address[IW_MAX_SPY][ETH_ALEN];
 //x		struct iw_quality           spy_stat[IW_MAX_SPY];
 #endif // WIRELESS_EXT
-   	    len += sprintf(buf+len,"IFB:                  0x%p\n", &lp->hcfCtx );
-   	    len += sprintf(buf+len,"flags:                %#.8lX\n", lp->flags );  //;?use this format from now on
-   	    len += sprintf(buf+len,"DebugFlag(wl_private) 0x%04X\n", lp->DebugFlag );
+		seq_printf(m, "IFB:                  0x%p\n", &lp->hcfCtx );
+		seq_printf(m, "flags:                %#.8lX\n", lp->flags );  //;?use this format from now on
+		seq_printf(m, "DebugFlag(wl_private) 0x%04X\n", lp->DebugFlag );
 #if DBG
-   	    len += sprintf(buf+len,"DebugFlag (DbgInfo):   0x%08lX\n", DbgInfo->DebugFlag );
+		seq_printf(m, "DebugFlag (DbgInfo):   0x%08lX\n", DbgInfo->DebugFlag );
 #endif // DBG
-   	    len += sprintf(buf+len,"is_registered:        0x%04X\n", lp->is_registered );
+		seq_printf(m, "is_registered:        0x%04X\n", lp->is_registered );
 //x		CFG_DRV_INFO_STRCT          "driverInfo:         0x%04X\n", lp->driverInfo );
-		len += printf_strct( "driverInfo", &buf[len], (hcf_16*)&lp->driverInfo );
+		printf_strct( m, "driverInfo", (hcf_16*)&lp->driverInfo );
 //x		CFG_IDENTITY_STRCT          "driverIdentity:     0x%04X\n", lp->driverIdentity );
-		len += printf_strct( "driverIdentity", &buf[len], (hcf_16*)&lp->driverIdentity );
+		printf_strct( m, "driverIdentity", (hcf_16*)&lp->driverIdentity );
 //x		CFG_FW_IDENTITY_STRCT       "StationIdentity:    0x%04X\n", lp->StationIdentity );
-		len += printf_strct( "StationIdentity", &buf[len], (hcf_16*)&lp->StationIdentity );
+		printf_strct( m, "StationIdentity", (hcf_16*)&lp->StationIdentity );
 //x		CFG_PRI_IDENTITY_STRCT      "PrimaryIdentity:    0x%04X\n", lp->PrimaryIdentity );
-		len += printf_strct( "PrimaryIdentity", &buf[len], (hcf_16*)&lp->hcfCtx.IFB_PRIIdentity );
-		len += printf_strct( "PrimarySupplier", &buf[len], (hcf_16*)&lp->hcfCtx.IFB_PRISup );
+		printf_strct( m, "PrimaryIdentity", (hcf_16*)&lp->hcfCtx.IFB_PRIIdentity );
+		printf_strct( m, "PrimarySupplier", (hcf_16*)&lp->hcfCtx.IFB_PRISup );
 //x		CFG_PRI_IDENTITY_STRCT      "NICIdentity:        0x%04X\n", lp->NICIdentity );
-		len += printf_strct( "NICIdentity", &buf[len], (hcf_16*)&lp->NICIdentity );
+		printf_strct( m, "NICIdentity", (hcf_16*)&lp->NICIdentity );
 //x		ltv_t                       "ltvRecord:          0x%04X\n", lp->ltvRecord );
-   	    len += sprintf(buf+len,"txBytes:              0x%08lX\n", lp->txBytes );
-   	    len += sprintf(buf+len,"maxPort:              0x%04X\n", lp->maxPort );        /* 0 for STA, 6 for AP */
-	/* Elements used for async notification from hardware */
+		seq_printf(m, "txBytes:              0x%08lX\n", lp->txBytes );
+		seq_printf(m, "maxPort:              0x%04X\n", lp->maxPort );        /* 0 for STA, 6 for AP */
+		/* Elements used for async notification from hardware */
 //x		RID_LOG_STRCT				RidList[10];
 //x		ltv_t                       "updatedRecord:      0x%04X\n", lp->updatedRecord );
 //x		PROBE_RESP				    "ProbeResp:                    0x%04X\n", lp->ProbeResp );
 //x		ASSOC_STATUS_STRCT          "assoc_stat:         0x%04X\n", lp->assoc_stat );
 //x		SECURITY_STATUS_STRCT       "sec_stat:           0x%04X\n", lp->sec_stat );
 //x		u_char                      lookAheadBuf[WVLAN_MAX_LOOKAHEAD];
-   	    len += sprintf(buf+len,"PortType:             0x%04X\n", lp->PortType );           // 1 - 3 (1 [Normal] | 3 [AdHoc])
-   	    len += sprintf(buf+len,"Channel:              0x%04X\n", lp->Channel );            // 0 - 14 (0)
+		seq_printf(m, "PortType:             0x%04X\n", lp->PortType );           // 1 - 3 (1 [Normal] | 3 [AdHoc])
+		seq_printf(m, "Channel:              0x%04X\n", lp->Channel );            // 0 - 14 (0)
 //x		hcf_16                      TxRateControl[2];
-   	    len += sprintf(buf+len,"TxRateControl[2]:     0x%04X 0x%04X\n",
-						lp->TxRateControl[0], lp->TxRateControl[1] );
-   	    len += sprintf(buf+len,"DistanceBetweenAPs:   0x%04X\n", lp->DistanceBetweenAPs ); // 1 - 3 (1)
-   	    len += sprintf(buf+len,"RTSThreshold:         0x%04X\n", lp->RTSThreshold );       // 0 - 2347 (2347)
-   	    len += sprintf(buf+len,"PMEnabled:            0x%04X\n", lp->PMEnabled );          // 0 - 2, 8001 - 8002 (0)
-   	    len += sprintf(buf+len,"MicrowaveRobustness:  0x%04X\n", lp->MicrowaveRobustness );// 0 - 1 (0)
-   	    len += sprintf(buf+len,"CreateIBSS:           0x%04X\n", lp->CreateIBSS );         // 0 - 1 (0)
-   	    len += sprintf(buf+len,"MulticastReceive:     0x%04X\n", lp->MulticastReceive );   // 0 - 1 (1)
-   	    len += sprintf(buf+len,"MaxSleepDuration:     0x%04X\n", lp->MaxSleepDuration );   // 0 - 65535 (100)
+		seq_printf(m, "TxRateControl[2]:     0x%04X 0x%04X\n",
+			       lp->TxRateControl[0], lp->TxRateControl[1] );
+		seq_printf(m, "DistanceBetweenAPs:   0x%04X\n", lp->DistanceBetweenAPs ); // 1 - 3 (1)
+		seq_printf(m, "RTSThreshold:         0x%04X\n", lp->RTSThreshold );       // 0 - 2347 (2347)
+		seq_printf(m, "PMEnabled:            0x%04X\n", lp->PMEnabled );          // 0 - 2, 8001 - 8002 (0)
+		seq_printf(m, "MicrowaveRobustness:  0x%04X\n", lp->MicrowaveRobustness );// 0 - 1 (0)
+		seq_printf(m, "CreateIBSS:           0x%04X\n", lp->CreateIBSS );         // 0 - 1 (0)
+		seq_printf(m, "MulticastReceive:     0x%04X\n", lp->MulticastReceive );   // 0 - 1 (1)
+		seq_printf(m, "MaxSleepDuration:     0x%04X\n", lp->MaxSleepDuration );   // 0 - 65535 (100)
 //x		hcf_8                       MACAddress[ETH_ALEN];
-		len += printf_hcf_8( "MACAddress", &buf[len], lp->MACAddress, ETH_ALEN );
+		printf_hcf_8(m, "MACAddress", lp->MACAddress, ETH_ALEN );
 //x		char                        NetworkName[HCF_MAX_NAME_LEN+1];
-   	    len += sprintf(buf+len,"NetworkName:          %.32s\n", lp->NetworkName );
+		seq_printf(m, "NetworkName:          %.32s\n", lp->NetworkName );
 //x		char                        StationName[HCF_MAX_NAME_LEN+1];
-   	    len += sprintf(buf+len,"EnableEncryption:     0x%04X\n", lp->EnableEncryption );   // 0 - 1 (0)
+		seq_printf(m, "EnableEncryption:     0x%04X\n", lp->EnableEncryption );   // 0 - 1 (0)
 //x		char                        Key1[MAX_KEY_LEN+1];
-		len += printf_hcf_8( "Key1", &buf[len], lp->Key1, MAX_KEY_LEN );
+		printf_hcf_8( m, "Key1", lp->Key1, MAX_KEY_LEN );
 //x		char                        Key2[MAX_KEY_LEN+1];
 //x		char                        Key3[MAX_KEY_LEN+1];
 //x		char                        Key4[MAX_KEY_LEN+1];
-   	    len += sprintf(buf+len,"TransmitKeyID:        0x%04X\n", lp->TransmitKeyID );      // 1 - 4 (1)
+		seq_printf(m, "TransmitKeyID:        0x%04X\n", lp->TransmitKeyID );      // 1 - 4 (1)
 //x		CFG_DEFAULT_KEYS_STRCT	    "DefaultKeys:         0x%04X\n", lp->DefaultKeys );
 //x		u_char                      mailbox[MB_SIZE];
 //x		char                        szEncryption[MAX_ENC_LEN];
-   	    len += sprintf(buf+len,"driverEnable:         0x%04X\n", lp->driverEnable );
-   	    len += sprintf(buf+len,"wolasEnable:          0x%04X\n", lp->wolasEnable );
-   	    len += sprintf(buf+len,"atimWindow:           0x%04X\n", lp->atimWindow );
-   	    len += sprintf(buf+len,"holdoverDuration:     0x%04X\n", lp->holdoverDuration );
+		seq_printf(m, "driverEnable:         0x%04X\n", lp->driverEnable );
+		seq_printf(m, "wolasEnable:          0x%04X\n", lp->wolasEnable );
+		seq_printf(m, "atimWindow:           0x%04X\n", lp->atimWindow );
+		seq_printf(m, "holdoverDuration:     0x%04X\n", lp->holdoverDuration );
 //x		hcf_16                      MulticastRate[2];
-   	    len += sprintf(buf+len,"authentication:       0x%04X\n", lp->authentication ); // is this AP specific?
-   	    len += sprintf(buf+len,"promiscuousMode:      0x%04X\n", lp->promiscuousMode );
-   	    len += sprintf(buf+len,"DownloadFirmware:     0x%04X\n", lp->DownloadFirmware );   // 0 - 2 (0 [None] | 1 [STA] | 2 [AP])
-   	    len += sprintf(buf+len,"AuthKeyMgmtSuite:     0x%04X\n", lp->AuthKeyMgmtSuite );
-   	    len += sprintf(buf+len,"loadBalancing:        0x%04X\n", lp->loadBalancing );
-   	    len += sprintf(buf+len,"mediumDistribution:   0x%04X\n", lp->mediumDistribution );
-   	    len += sprintf(buf+len,"txPowLevel:           0x%04X\n", lp->txPowLevel );
-//   	    len += sprintf(buf+len,"shortRetryLimit:    0x%04X\n", lp->shortRetryLimit );
-//   	    len += sprintf(buf+len,"longRetryLimit:     0x%04X\n", lp->longRetryLimit );
+		seq_printf(m, "authentication:       0x%04X\n", lp->authentication ); // is this AP specific?
+		seq_printf(m, "promiscuousMode:      0x%04X\n", lp->promiscuousMode );
+		seq_printf(m, "DownloadFirmware:     0x%04X\n", lp->DownloadFirmware );   // 0 - 2 (0 [None] | 1 [STA] | 2 [AP])
+		seq_printf(m, "AuthKeyMgmtSuite:     0x%04X\n", lp->AuthKeyMgmtSuite );
+		seq_printf(m, "loadBalancing:        0x%04X\n", lp->loadBalancing );
+		seq_printf(m, "mediumDistribution:   0x%04X\n", lp->mediumDistribution );
+		seq_printf(m, "txPowLevel:           0x%04X\n", lp->txPowLevel );
+//   	    seq_printf(m, "shortRetryLimit:    0x%04X\n", lp->shortRetryLimit );
+//   	    seq_printf(m, "longRetryLimit:     0x%04X\n", lp->longRetryLimit );
 //x		hcf_16                      srsc[2];
 //x		hcf_16                      brsc[2];
-   	    len += sprintf(buf+len,"connectionControl:    0x%04X\n", lp->connectionControl );
+		seq_printf(m, "connectionControl:    0x%04X\n", lp->connectionControl );
 //x		//hcf_16                      probeDataRates[2];
-   	    len += sprintf(buf+len,"ownBeaconInterval:    0x%04X\n", lp->ownBeaconInterval );
-   	    len += sprintf(buf+len,"coexistence:          0x%04X\n", lp->coexistence );
+		seq_printf(m, "ownBeaconInterval:    0x%04X\n", lp->ownBeaconInterval );
+		seq_printf(m, "coexistence:          0x%04X\n", lp->coexistence );
 //x		WVLAN_FRAME                 "txF:                0x%04X\n", lp->txF );
 //x		WVLAN_LFRAME                txList[DEFAULT_NUM_TX_FRAMES];
 //x		struct list_head            "txFree:             0x%04X\n", lp->txFree );
 //x		struct list_head            txQ[WVLAN_MAX_TX_QUEUES];
-   	    len += sprintf(buf+len,"netif_queue_on:       0x%04X\n", lp->netif_queue_on );
-   	    len += sprintf(buf+len,"txQ_count:            0x%04X\n", lp->txQ_count );
+		seq_printf(m, "netif_queue_on:       0x%04X\n", lp->netif_queue_on );
+		seq_printf(m, "txQ_count:            0x%04X\n", lp->txQ_count );
 //x		DESC_STRCT                  "desc_rx:            0x%04X\n", lp->desc_rx );
 //x		DESC_STRCT                  "desc_tx:            0x%04X\n", lp->desc_tx );
 //x		WVLAN_PORT_STATE            "portState:          0x%04X\n", lp->portState );
 //x		ScanResult                  "scan_results:       0x%04X\n", lp->scan_results );
 //x		ProbeResult                 "probe_results:      0x%04X\n", lp->probe_results );
-   	    len += sprintf(buf+len,"probe_num_aps:        0x%04X\n", lp->probe_num_aps );
-   	    len += sprintf(buf+len,"use_dma:              0x%04X\n", lp->use_dma );
+		seq_printf(m, "probe_num_aps:        0x%04X\n", lp->probe_num_aps );
+		seq_printf(m, "use_dma:              0x%04X\n", lp->use_dma );
 //x		DMA_STRCT                   "dma:                0x%04X\n", lp->dma );
 #ifdef USE_RTS
-   	    len += sprintf(buf+len,"useRTS:               0x%04X\n", lp->useRTS );
+		seq_printf(m, "useRTS:               0x%04X\n", lp->useRTS );
 #endif  // USE_RTS
 #if 1 //;? (HCF_TYPE) & HCF_TYPE_AP
 		//;?should we restore this to allow smaller memory footprint
 		//;?I guess not. This should be brought under Debug mode only
-   	    len += sprintf(buf+len,"DTIMPeriod:           0x%04X\n", lp->DTIMPeriod );         // 1 - 255 (1)
-   	    len += sprintf(buf+len,"multicastPMBuffering: 0x%04X\n", lp->multicastPMBuffering );
-   	    len += sprintf(buf+len,"RejectAny:            0x%04X\n", lp->RejectAny );          // 0 - 1 (0)
-   	    len += sprintf(buf+len,"ExcludeUnencrypted:   0x%04X\n", lp->ExcludeUnencrypted ); // 0 - 1 (1)
-   	    len += sprintf(buf+len,"intraBSSRelay:        0x%04X\n", lp->intraBSSRelay );
-   	    len += sprintf(buf+len,"wlags49_type:             0x%08lX\n", lp->wlags49_type );
+		seq_printf(m, "DTIMPeriod:           0x%04X\n", lp->DTIMPeriod );         // 1 - 255 (1)
+		seq_printf(m, "multicastPMBuffering: 0x%04X\n", lp->multicastPMBuffering );
+		seq_printf(m, "RejectAny:            0x%04X\n", lp->RejectAny );          // 0 - 1 (0)
+		seq_printf(m, "ExcludeUnencrypted:   0x%04X\n", lp->ExcludeUnencrypted ); // 0 - 1 (1)
+		seq_printf(m, "intraBSSRelay:        0x%04X\n", lp->intraBSSRelay );
+		seq_printf(m, "wlags49_type:             0x%08lX\n", lp->wlags49_type );
 #ifdef USE_WDS
 //x		WVLAN_WDS_IF                wds_port[NUM_WDS_PORTS];
 #endif // USE_WDS
 #endif // HCF_AP
 	} else if ( lp->wlags49_type == 2 ){
-        len += sprintf(buf+len,"tallies to be added\n" );
+		seq_printf(m, "tallies to be added\n" );
 //Hermes Tallies (IFB substructure) {
-   	    p = &lp->hcfCtx.IFB_NIC_Tallies;
-        len += sprintf(buf+len,"TxUnicastFrames:          %08lX\n", p->TxUnicastFrames );
-        len += sprintf(buf+len,"TxMulticastFrames:        %08lX\n", p->TxMulticastFrames );
-        len += sprintf(buf+len,"TxFragments:              %08lX\n", p->TxFragments );
-        len += sprintf(buf+len,"TxUnicastOctets:          %08lX\n", p->TxUnicastOctets );
-        len += sprintf(buf+len,"TxMulticastOctets:        %08lX\n", p->TxMulticastOctets );
-        len += sprintf(buf+len,"TxDeferredTransmissions:  %08lX\n", p->TxDeferredTransmissions );
-        len += sprintf(buf+len,"TxSingleRetryFrames:      %08lX\n", p->TxSingleRetryFrames );
-        len += sprintf(buf+len,"TxMultipleRetryFrames:    %08lX\n", p->TxMultipleRetryFrames );
-        len += sprintf(buf+len,"TxRetryLimitExceeded:     %08lX\n", p->TxRetryLimitExceeded );
-        len += sprintf(buf+len,"TxDiscards:               %08lX\n", p->TxDiscards );
-        len += sprintf(buf+len,"RxUnicastFrames:          %08lX\n", p->RxUnicastFrames );
-        len += sprintf(buf+len,"RxMulticastFrames:        %08lX\n", p->RxMulticastFrames );
-        len += sprintf(buf+len,"RxFragments:              %08lX\n", p->RxFragments );
-        len += sprintf(buf+len,"RxUnicastOctets:          %08lX\n", p->RxUnicastOctets );
-        len += sprintf(buf+len,"RxMulticastOctets:        %08lX\n", p->RxMulticastOctets );
-        len += sprintf(buf+len,"RxFCSErrors:              %08lX\n", p->RxFCSErrors );
-        len += sprintf(buf+len,"RxDiscardsNoBuffer:       %08lX\n", p->RxDiscardsNoBuffer );
-        len += sprintf(buf+len,"TxDiscardsWrongSA:        %08lX\n", p->TxDiscardsWrongSA );
-        len += sprintf(buf+len,"RxWEPUndecryptable:       %08lX\n", p->RxWEPUndecryptable );
-        len += sprintf(buf+len,"RxMsgInMsgFragments:      %08lX\n", p->RxMsgInMsgFragments );
-        len += sprintf(buf+len,"RxMsgInBadMsgFragments:   %08lX\n", p->RxMsgInBadMsgFragments );
-        len += sprintf(buf+len,"RxDiscardsWEPICVError:    %08lX\n", p->RxDiscardsWEPICVError );
-        len += sprintf(buf+len,"RxDiscardsWEPExcluded:    %08lX\n", p->RxDiscardsWEPExcluded );
+		p = &lp->hcfCtx.IFB_NIC_Tallies;
+		seq_printf(m, "TxUnicastFrames:          %08lX\n", p->TxUnicastFrames );
+		seq_printf(m, "TxMulticastFrames:        %08lX\n", p->TxMulticastFrames );
+		seq_printf(m, "TxFragments:              %08lX\n", p->TxFragments );
+		seq_printf(m, "TxUnicastOctets:          %08lX\n", p->TxUnicastOctets );
+		seq_printf(m, "TxMulticastOctets:        %08lX\n", p->TxMulticastOctets );
+		seq_printf(m, "TxDeferredTransmissions:  %08lX\n", p->TxDeferredTransmissions );
+		seq_printf(m, "TxSingleRetryFrames:      %08lX\n", p->TxSingleRetryFrames );
+		seq_printf(m, "TxMultipleRetryFrames:    %08lX\n", p->TxMultipleRetryFrames );
+		seq_printf(m, "TxRetryLimitExceeded:     %08lX\n", p->TxRetryLimitExceeded );
+		seq_printf(m, "TxDiscards:               %08lX\n", p->TxDiscards );
+		seq_printf(m, "RxUnicastFrames:          %08lX\n", p->RxUnicastFrames );
+		seq_printf(m, "RxMulticastFrames:        %08lX\n", p->RxMulticastFrames );
+		seq_printf(m, "RxFragments:              %08lX\n", p->RxFragments );
+		seq_printf(m, "RxUnicastOctets:          %08lX\n", p->RxUnicastOctets );
+		seq_printf(m, "RxMulticastOctets:        %08lX\n", p->RxMulticastOctets );
+		seq_printf(m, "RxFCSErrors:              %08lX\n", p->RxFCSErrors );
+		seq_printf(m, "RxDiscardsNoBuffer:       %08lX\n", p->RxDiscardsNoBuffer );
+		seq_printf(m, "TxDiscardsWrongSA:        %08lX\n", p->TxDiscardsWrongSA );
+		seq_printf(m, "RxWEPUndecryptable:       %08lX\n", p->RxWEPUndecryptable );
+		seq_printf(m, "RxMsgInMsgFragments:      %08lX\n", p->RxMsgInMsgFragments );
+		seq_printf(m, "RxMsgInBadMsgFragments:   %08lX\n", p->RxMsgInBadMsgFragments );
+		seq_printf(m, "RxDiscardsWEPICVError:    %08lX\n", p->RxDiscardsWEPICVError );
+		seq_printf(m, "RxDiscardsWEPExcluded:    %08lX\n", p->RxDiscardsWEPExcluded );
 #if (HCF_EXT) & HCF_EXT_TALLIES_FW
-        //to be added ;?
+		//to be added ;?
 #endif // HCF_EXT_TALLIES_FW
 	} else if ( lp->wlags49_type & 0x8000 ) {	//;?kludgy but it is unclear to me were else to place this
 #if DBG
@@ -3761,27 +3760,19 @@ int scull_read_procmem(char *buf, char **start, off_t offset, int len, int *eof,
 #endif // DBG
 		lp->wlags49_type = 0;				//default to IFB again ;?
 	} else {
-        len += sprintf(buf+len,"unknown value for wlags49_type: 0x%08lX\n", lp->wlags49_type );
-        len += sprintf(buf+len,"0x0000 - IFB\n" );
-        len += sprintf(buf+len,"0x0001 - wl_private\n" );
-        len += sprintf(buf+len,"0x0002 - Tallies\n" );
-        len += sprintf(buf+len,"0x8xxx - Change debufflag\n" );
-        len += sprintf(buf+len,"ERROR    0001\nWARNING  0002\nNOTICE   0004\nTRACE    0008\n" );
-        len += sprintf(buf+len,"VERBOSE  0010\nPARAM    0020\nBREAK    0040\nRX       0100\n" );
-        len += sprintf(buf+len,"TX       0200\nDS       0400\n" );
-	}
-    return len;
+		seq_printf(m, "unknown value for wlags49_type: 0x%08lX\n", lp->wlags49_type );
+		seq_puts(m,
+			 "0x0000 - IFB\n"
+			 "0x0001 - wl_private\n"
+			 "0x0002 - Tallies\n"
+			 "0x8xxx - Change debufflag\n"
+			 "ERROR    0001\nWARNING  0002\nNOTICE   0004\nTRACE    0008\n"
+			 "VERBOSE  0010\nPARAM    0020\nBREAK    0040\nRX       0100\n"
+			 "TX       0200\nDS       0400\n");
+	}
+	return 0;
 } // scull_read_procmem
 
-static void proc_write(const char *name, write_proc_t *w, void *data)
-{
-	struct proc_dir_entry * entry = create_proc_entry(name, S_IFREG | S_IWUSR, NULL);
-	if (entry) {
-		entry->write_proc = w;
-		entry->data = data;
-	}
-} // proc_write
-
 static int write_int(struct file *file, const char *buffer, unsigned long count, void *data)
 {
 	static char		proc_number[11];
diff --git a/drivers/staging/zcache/Kconfig b/drivers/staging/zcache/Kconfig
index 05e87a1e5d93..2d7b2da3b9e0 100644
--- a/drivers/staging/zcache/Kconfig
+++ b/drivers/staging/zcache/Kconfig
@@ -1,5 +1,5 @@
 config ZCACHE
-	bool "Dynamic compression of swap pages and clean pagecache pages"
+	tristate "Dynamic compression of swap pages and clean pagecache pages"
 	depends on CRYPTO=y && SWAP=y && CLEANCACHE && FRONTSWAP
 	select CRYPTO_LZO
 	default n
@@ -19,8 +19,8 @@ config ZCACHE_DEBUG
 	  how zcache is doing. You probably want to set this to 'N'.
 
 config RAMSTER
-	bool "Cross-machine RAM capacity sharing, aka peer-to-peer tmem"
-	depends on CONFIGFS_FS=y && SYSFS=y && !HIGHMEM && ZCACHE=y
+	tristate "Cross-machine RAM capacity sharing, aka peer-to-peer tmem"
+	depends on CONFIGFS_FS=y && SYSFS=y && !HIGHMEM && ZCACHE
 	depends on NET
 	# must ensure struct page is 8-byte aligned
 	select HAVE_ALIGNED_STRUCT_PAGE if !64BIT
diff --git a/drivers/staging/zcache/ramster.h b/drivers/staging/zcache/ramster.h
index 1b71aea2ff62..e1f91d5a0f6a 100644
--- a/drivers/staging/zcache/ramster.h
+++ b/drivers/staging/zcache/ramster.h
@@ -11,10 +11,14 @@
 #ifndef _ZCACHE_RAMSTER_H_
 #define _ZCACHE_RAMSTER_H_
 
+#ifdef CONFIG_RAMSTER_MODULE
+#define CONFIG_RAMSTER
+#endif
+
 #ifdef CONFIG_RAMSTER
 #include "ramster/ramster.h"
 #else
-static inline void ramster_init(bool x, bool y, bool z)
+static inline void ramster_init(bool x, bool y, bool z, bool w)
 {
 }
 
diff --git a/drivers/staging/zcache/ramster/debug.c b/drivers/staging/zcache/ramster/debug.c
index bf34133cc631..327e4f0d98e1 100644
--- a/drivers/staging/zcache/ramster/debug.c
+++ b/drivers/staging/zcache/ramster/debug.c
@@ -43,7 +43,7 @@ static struct debug_entry {
 };
 #undef ATTR
 
-int __init ramster_debugfs_init(void)
+int ramster_debugfs_init(void)
 {
 	int i;
 	struct dentry *root = debugfs_create_dir("ramster", NULL);
diff --git a/drivers/staging/zcache/ramster/nodemanager.c b/drivers/staging/zcache/ramster/nodemanager.c
index c0f48158735d..2cfe93342c0d 100644
--- a/drivers/staging/zcache/ramster/nodemanager.c
+++ b/drivers/staging/zcache/ramster/nodemanager.c
@@ -949,7 +949,7 @@ static void __exit exit_r2nm(void)
 	r2hb_exit();
 }
 
-static int __init init_r2nm(void)
+int r2nm_init(void)
 {
 	int ret = -1;
 
@@ -986,10 +986,11 @@ out_r2hb:
 out:
 	return ret;
 }
+EXPORT_SYMBOL_GPL(r2nm_init);
 
 MODULE_AUTHOR("Oracle");
 MODULE_LICENSE("GPL");
 
-/* module_init(init_r2nm) */
-late_initcall(init_r2nm);
-/* module_exit(exit_r2nm) */
+#ifndef CONFIG_RAMSTER_MODULE
+late_initcall(r2nm_init);
+#endif
diff --git a/drivers/staging/zcache/ramster/ramster.c b/drivers/staging/zcache/ramster/ramster.c
index 87816279ce3c..b18b887db79f 100644
--- a/drivers/staging/zcache/ramster/ramster.c
+++ b/drivers/staging/zcache/ramster/ramster.c
@@ -121,6 +121,7 @@ int ramster_do_preload_flnode(struct tmem_pool *pool)
 		kmem_cache_free(ramster_flnode_cache, flnode);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(ramster_do_preload_flnode);
 
 /*
  * Called by the message handler after a (still compressed) page has been
@@ -388,6 +389,7 @@ void *ramster_pampd_free(void *pampd, struct tmem_pool *pool,
 	}
 	return local_pampd;
 }
+EXPORT_SYMBOL_GPL(ramster_pampd_free);
 
 void ramster_count_foreign_pages(bool eph, int count)
 {
@@ -408,6 +410,7 @@ void ramster_count_foreign_pages(bool eph, int count)
 		}
 	}
 }
+EXPORT_SYMBOL_GPL(ramster_count_foreign_pages);
 
 /*
  * For now, just push over a few pages every few seconds to
@@ -593,7 +596,7 @@ requeue:
 	ramster_remotify_queue_delayed_work(HZ);
 }
 
-void __init ramster_remotify_init(void)
+void ramster_remotify_init(void)
 {
 	unsigned long n = 60UL;
 	ramster_remotify_workqueue =
@@ -768,8 +771,10 @@ static bool frontswap_selfshrinking __read_mostly;
 static void selfshrink_process(struct work_struct *work);
 static DECLARE_DELAYED_WORK(selfshrink_worker, selfshrink_process);
 
+#ifndef CONFIG_RAMSTER_MODULE
 /* Enable/disable with kernel boot option. */
-static bool use_frontswap_selfshrink __initdata = true;
+static bool use_frontswap_selfshrink = true;
+#endif
 
 /*
  * The default values for the following parameters were deemed reasonable
@@ -824,6 +829,7 @@ static void frontswap_selfshrink(void)
 	frontswap_shrink(tgt_frontswap_pages);
 }
 
+#ifndef CONFIG_RAMSTER_MODULE
 static int __init ramster_nofrontswap_selfshrink_setup(char *s)
 {
 	use_frontswap_selfshrink = false;
@@ -831,6 +837,7 @@ static int __init ramster_nofrontswap_selfshrink_setup(char *s)
 }
 
 __setup("noselfshrink", ramster_nofrontswap_selfshrink_setup);
+#endif
 
 static void selfshrink_process(struct work_struct *work)
 {
@@ -849,6 +856,7 @@ void ramster_cpu_up(int cpu)
 	per_cpu(ramster_remoteputmem1, cpu) = p1;
 	per_cpu(ramster_remoteputmem2, cpu) = p2;
 }
+EXPORT_SYMBOL_GPL(ramster_cpu_up);
 
 void ramster_cpu_down(int cpu)
 {
@@ -864,6 +872,7 @@ void ramster_cpu_down(int cpu)
 		kp->flnode = NULL;
 	}
 }
+EXPORT_SYMBOL_GPL(ramster_cpu_down);
 
 void ramster_register_pamops(struct tmem_pamops *pamops)
 {
@@ -874,9 +883,11 @@ void ramster_register_pamops(struct tmem_pamops *pamops)
 	pamops->repatriate = ramster_pampd_repatriate;
 	pamops->repatriate_preload = ramster_pampd_repatriate_preload;
 }
+EXPORT_SYMBOL_GPL(ramster_register_pamops);
 
-void __init ramster_init(bool cleancache, bool frontswap,
-				bool frontswap_exclusive_gets)
+void ramster_init(bool cleancache, bool frontswap,
+				bool frontswap_exclusive_gets,
+				bool frontswap_selfshrink)
 {
 	int ret = 0;
 
@@ -891,10 +902,17 @@ void __init ramster_init(bool cleancache, bool frontswap,
 	if (ret)
 		pr_err("ramster: can't create sysfs for ramster\n");
 	(void)r2net_register_handlers();
+#ifdef CONFIG_RAMSTER_MODULE
+	ret = r2nm_init();
+	if (ret)
+		pr_err("ramster: can't init r2net\n");
+	frontswap_selfshrinking = frontswap_selfshrink;
+#else
+	frontswap_selfshrinking = use_frontswap_selfshrink;
+#endif
 	INIT_LIST_HEAD(&ramster_rem_op_list);
 	ramster_flnode_cache = kmem_cache_create("ramster_flnode",
 				sizeof(struct flushlist_node), 0, 0, NULL);
-	frontswap_selfshrinking = use_frontswap_selfshrink;
 	if (frontswap_selfshrinking) {
 		pr_info("ramster: Initializing frontswap selfshrink driver.\n");
 		schedule_delayed_work(&selfshrink_worker,
@@ -902,3 +920,4 @@ void __init ramster_init(bool cleancache, bool frontswap,
 	}
 	ramster_remotify_init();
 }
+EXPORT_SYMBOL_GPL(ramster_init);
diff --git a/drivers/staging/zcache/ramster/ramster.h b/drivers/staging/zcache/ramster/ramster.h
index 12ae56f09ca4..6d41a7a772e3 100644
--- a/drivers/staging/zcache/ramster/ramster.h
+++ b/drivers/staging/zcache/ramster/ramster.h
@@ -147,7 +147,7 @@ extern int r2net_register_handlers(void);
 extern int r2net_remote_target_node_set(int);
 
 extern int ramster_remotify_pageframe(bool);
-extern void ramster_init(bool, bool, bool);
+extern void ramster_init(bool, bool, bool, bool);
 extern void ramster_register_pamops(struct tmem_pamops *);
 extern int ramster_localify(int, struct tmem_oid *oidp, uint32_t, char *,
 				unsigned int, void *);
diff --git a/drivers/staging/zcache/ramster/ramster_nodemanager.h b/drivers/staging/zcache/ramster/ramster_nodemanager.h
index 49f879d943ab..dbaae34ea613 100644
--- a/drivers/staging/zcache/ramster/ramster_nodemanager.h
+++ b/drivers/staging/zcache/ramster/ramster_nodemanager.h
@@ -36,4 +36,6 @@
 /* host name, group name, cluster name all 64 bytes */
 #define R2NM_MAX_NAME_LEN        64    /* __NEW_UTS_LEN */
 
+extern int r2nm_init(void);
+
 #endif /* _RAMSTER_NODEMANAGER_H */
diff --git a/drivers/staging/zcache/tmem.c b/drivers/staging/zcache/tmem.c
index a2b7e03b6062..d7e51e4152eb 100644
--- a/drivers/staging/zcache/tmem.c
+++ b/drivers/staging/zcache/tmem.c
@@ -35,7 +35,8 @@
 #include <linux/list.h>
 #include <linux/spinlock.h>
 #include <linux/atomic.h>
-#ifdef CONFIG_RAMSTER
+#include <linux/export.h>
+#if defined(CONFIG_RAMSTER) || defined(CONFIG_RAMSTER_MODULE)
 #include <linux/delay.h>
 #endif
 
@@ -641,6 +642,7 @@ void *tmem_localify_get_pampd(struct tmem_pool *pool, struct tmem_oid *oidp,
 	/* note, hashbucket remains locked */
 	return pampd;
 }
+EXPORT_SYMBOL_GPL(tmem_localify_get_pampd);
 
 void tmem_localify_finish(struct tmem_obj *obj, uint32_t index,
 			  void *pampd, void *saved_hb, bool delete)
@@ -658,6 +660,7 @@ void tmem_localify_finish(struct tmem_obj *obj, uint32_t index,
 	}
 	spin_unlock(&hb->lock);
 }
+EXPORT_SYMBOL_GPL(tmem_localify_finish);
 
 /*
  * For ramster only.  Helper function to support asynchronous tmem_get.
@@ -719,6 +722,7 @@ out:
 	spin_unlock(&hb->lock);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(tmem_replace);
 #endif
 
 /*
diff --git a/drivers/staging/zcache/tmem.h b/drivers/staging/zcache/tmem.h
index adbe5a8f28aa..d128ce290f1f 100644
--- a/drivers/staging/zcache/tmem.h
+++ b/drivers/staging/zcache/tmem.h
@@ -126,7 +126,7 @@ static inline unsigned tmem_oid_hash(struct tmem_oid *oidp)
 				TMEM_HASH_BUCKET_BITS);
 }
 
-#ifdef CONFIG_RAMSTER
+#if defined(CONFIG_RAMSTER) || defined(CONFIG_RAMSTER_MODULE)
 struct tmem_xhandle {
 	uint8_t client_id;
 	uint8_t xh_data_cksum;
@@ -171,7 +171,7 @@ struct tmem_obj {
 	unsigned int objnode_tree_height;
 	unsigned long objnode_count;
 	long pampd_count;
-#ifdef CONFIG_RAMSTER
+#if defined(CONFIG_RAMSTER) || defined(CONFIG_RAMSTER_MODULE)
 	/*
 	 * for current design of ramster, all pages belonging to
 	 * an object reside on the same remotenode and extra is
@@ -215,7 +215,7 @@ struct tmem_pamops {
 				uint32_t);
 	void (*free)(void *, struct tmem_pool *,
 				struct tmem_oid *, uint32_t, bool);
-#ifdef CONFIG_RAMSTER
+#if defined(CONFIG_RAMSTER) || defined(CONFIG_RAMSTER_MODULE)
 	void (*new_obj)(struct tmem_obj *);
 	void (*free_obj)(struct tmem_pool *, struct tmem_obj *, bool);
 	void *(*repatriate_preload)(void *, struct tmem_pool *,
@@ -247,7 +247,7 @@ extern int tmem_flush_page(struct tmem_pool *, struct tmem_oid *,
 extern int tmem_flush_object(struct tmem_pool *, struct tmem_oid *);
 extern int tmem_destroy_pool(struct tmem_pool *);
 extern void tmem_new_pool(struct tmem_pool *, uint32_t);
-#ifdef CONFIG_RAMSTER
+#if defined(CONFIG_RAMSTER) || defined(CONFIG_RAMSTER_MODULE)
 extern int tmem_replace(struct tmem_pool *, struct tmem_oid *, uint32_t index,
 			void *);
 extern void *tmem_localify_get_pampd(struct tmem_pool *, struct tmem_oid *,
diff --git a/drivers/staging/zcache/zcache-main.c b/drivers/staging/zcache/zcache-main.c
index e23d814b5392..522cb8e55142 100644
--- a/drivers/staging/zcache/zcache-main.c
+++ b/drivers/staging/zcache/zcache-main.c
@@ -37,8 +37,10 @@
 #include "debug.h"
 #ifdef CONFIG_RAMSTER
 static bool ramster_enabled __read_mostly;
+static int disable_frontswap_selfshrink;
 #else
 #define ramster_enabled false
+#define disable_frontswap_selfshrink 0
 #endif
 
 #ifndef __PG_WAS_ACTIVE
@@ -81,8 +83,12 @@ static char *namestr __read_mostly = "zcache";
 	(__GFP_FS | __GFP_NORETRY | __GFP_NOWARN | __GFP_NOMEMALLOC)
 
 /* crypto API for zcache  */
+#ifdef CONFIG_ZCACHE_MODULE
+static char *zcache_comp_name = "lzo";
+#else
 #define ZCACHE_COMP_NAME_SZ CRYPTO_MAX_ALG_NAME
 static char zcache_comp_name[ZCACHE_COMP_NAME_SZ] __read_mostly;
+#endif
 static struct crypto_comp * __percpu *zcache_comp_pcpu_tfms __read_mostly;
 
 enum comp_op {
@@ -1576,9 +1582,9 @@ static struct cleancache_ops zcache_cleancache_ops = {
 	.init_fs = zcache_cleancache_init_fs
 };
 
-struct cleancache_ops zcache_cleancache_register_ops(void)
+struct cleancache_ops *zcache_cleancache_register_ops(void)
 {
-	struct cleancache_ops old_ops =
+	struct cleancache_ops *old_ops =
 		cleancache_register_ops(&zcache_cleancache_ops);
 
 	return old_ops;
@@ -1707,9 +1713,9 @@ static struct frontswap_ops zcache_frontswap_ops = {
 	.init = zcache_frontswap_init
 };
 
-struct frontswap_ops zcache_frontswap_register_ops(void)
+struct frontswap_ops *zcache_frontswap_register_ops(void)
 {
-	struct frontswap_ops old_ops =
+	struct frontswap_ops *old_ops =
 		frontswap_register_ops(&zcache_frontswap_ops);
 
 	return old_ops;
@@ -1721,6 +1727,7 @@ struct frontswap_ops zcache_frontswap_register_ops(void)
  * OR NOTHING HAPPENS!
  */
 
+#ifndef CONFIG_ZCACHE_MODULE
 static int __init enable_zcache(char *s)
 {
 	zcache_enabled = true;
@@ -1787,18 +1794,27 @@ static int __init enable_zcache_compressor(char *s)
 	return 1;
 }
 __setup("zcache=", enable_zcache_compressor);
+#endif
 
 
-static int __init zcache_comp_init(void)
+static int zcache_comp_init(void)
 {
 	int ret = 0;
 
 	/* check crypto algorithm */
+#ifdef CONFIG_ZCACHE_MODULE
+	ret = crypto_has_comp(zcache_comp_name, 0, 0);
+	if (!ret) {
+		ret = -1;
+		goto out;
+	}
+#else
 	if (*zcache_comp_name != '\0') {
 		ret = crypto_has_comp(zcache_comp_name, 0, 0);
 		if (!ret)
 			pr_info("zcache: %s not supported\n",
 					zcache_comp_name);
+		goto out;
 	}
 	if (!ret)
 		strcpy(zcache_comp_name, "lzo");
@@ -1807,6 +1823,7 @@ static int __init zcache_comp_init(void)
 		ret = 1;
 		goto out;
 	}
+#endif
 	pr_info("zcache: using %s compressor\n", zcache_comp_name);
 
 	/* alloc percpu transforms */
@@ -1818,10 +1835,13 @@ out:
 	return ret;
 }
 
-static int __init zcache_init(void)
+static int zcache_init(void)
 {
 	int ret = 0;
 
+#ifdef CONFIG_ZCACHE_MODULE
+	zcache_enabled = 1;
+#endif
 	if (ramster_enabled) {
 		namestr = "ramster";
 		ramster_register_pamops(&zcache_pamops);
@@ -1860,7 +1880,7 @@ static int __init zcache_init(void)
 	}
 	zbud_init();
 	if (zcache_enabled && !disable_cleancache) {
-		struct cleancache_ops old_ops;
+		struct cleancache_ops *old_ops;
 
 		register_shrinker(&zcache_shrinker);
 		old_ops = zcache_cleancache_register_ops();
@@ -1870,11 +1890,11 @@ static int __init zcache_init(void)
 		pr_info("%s: cleancache: ignorenonactive = %d\n",
 			namestr, !disable_cleancache_ignore_nonactive);
 #endif
-		if (old_ops.init_fs != NULL)
+		if (old_ops != NULL)
 			pr_warn("%s: cleancache_ops overridden\n", namestr);
 	}
 	if (zcache_enabled && !disable_frontswap) {
-		struct frontswap_ops old_ops;
+		struct frontswap_ops *old_ops;
 
 		old_ops = zcache_frontswap_register_ops();
 		if (frontswap_has_exclusive_gets)
@@ -1886,14 +1906,36 @@ static int __init zcache_init(void)
 			namestr, frontswap_has_exclusive_gets,
 			!disable_frontswap_ignore_nonactive);
 #endif
-		if (old_ops.init != NULL)
+		if (IS_ERR(old_ops) || old_ops) {
+			if (IS_ERR(old_ops))
+				return PTR_RET(old_ops);
 			pr_warn("%s: frontswap_ops overridden\n", namestr);
+		}
 	}
 	if (ramster_enabled)
 		ramster_init(!disable_cleancache, !disable_frontswap,
-				frontswap_has_exclusive_gets);
+				frontswap_has_exclusive_gets,
+				!disable_frontswap_selfshrink);
 out:
 	return ret;
 }
 
+#ifdef CONFIG_ZCACHE_MODULE
+#ifdef CONFIG_RAMSTER
+module_param(ramster_enabled, int, S_IRUGO);
+module_param(disable_frontswap_selfshrink, int, S_IRUGO);
+#endif
+module_param(disable_cleancache, int, S_IRUGO);
+module_param(disable_frontswap, int, S_IRUGO);
+#ifdef FRONTSWAP_HAS_EXCLUSIVE_GETS
+module_param(frontswap_has_exclusive_gets, bool, S_IRUGO);
+#endif
+module_param(disable_frontswap_ignore_nonactive, int, S_IRUGO);
+module_param(zcache_comp_name, charp, S_IRUGO);
+module_init(zcache_init);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Dan Magenheimer <dan.magenheimer@oracle.com>");
+MODULE_DESCRIPTION("In-kernel compression of cleancache/frontswap pages");
+#else
 late_initcall(zcache_init);
+#endif
diff --git a/drivers/staging/zcache/zcache.h b/drivers/staging/zcache/zcache.h
index 81722b33b087..849120095e79 100644
--- a/drivers/staging/zcache/zcache.h
+++ b/drivers/staging/zcache/zcache.h
@@ -39,7 +39,7 @@ extern int zcache_flush_page(int, int, struct tmem_oid *, uint32_t);
 extern int zcache_flush_object(int, int, struct tmem_oid *);
 extern void zcache_decompress_to_page(char *, unsigned int, struct page *);
 
-#ifdef CONFIG_RAMSTER
+#if defined(CONFIG_RAMSTER) || defined(CONFIG_RAMSTER_MODULE)
 extern void *zcache_pampd_create(char *, unsigned int, bool, int,
 				struct tmem_handle *);
 int zcache_autocreate_pool(unsigned int cli_id, unsigned int pool_id, bool eph);
diff --git a/drivers/target/iscsi/Makefile b/drivers/target/iscsi/Makefile
index 5b9a2cf7f0a9..13a92403fe3e 100644
--- a/drivers/target/iscsi/Makefile
+++ b/drivers/target/iscsi/Makefile
@@ -15,6 +15,7 @@ iscsi_target_mod-y +=		iscsi_target_parameters.o \
 				iscsi_target_util.o \
 				iscsi_target.o \
 				iscsi_target_configfs.o \
-				iscsi_target_stat.o
+				iscsi_target_stat.o \
+				iscsi_target_transport.o
 
 obj-$(CONFIG_ISCSI_TARGET)	+= iscsi_target_mod.o
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index 7ea246a07731..ffbc6a94be52 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -49,6 +49,8 @@
 #include "iscsi_target_device.h"
 #include "iscsi_target_stat.h"
 
+#include <target/iscsi/iscsi_transport.h>
+
 static LIST_HEAD(g_tiqn_list);
 static LIST_HEAD(g_np_list);
 static DEFINE_SPINLOCK(tiqn_lock);
@@ -68,8 +70,7 @@ struct kmem_cache *lio_ooo_cache;
 struct kmem_cache *lio_r2t_cache;
 
 static int iscsit_handle_immediate_data(struct iscsi_cmd *,
-			unsigned char *buf, u32);
-static int iscsit_logout_post_handler(struct iscsi_cmd *, struct iscsi_conn *);
+			struct iscsi_scsi_req *, u32);
 
 struct iscsi_tiqn *iscsit_get_tiqn_for_login(unsigned char *buf)
 {
@@ -401,8 +402,7 @@ struct iscsi_np *iscsit_add_np(
 	spin_unlock_bh(&np_lock);
 
 	pr_debug("CORE[0] - Added Network Portal: %s:%hu on %s\n",
-		np->np_ip, np->np_port, (np->np_network_transport == ISCSI_TCP) ?
-		"TCP" : "SCTP");
+		np->np_ip, np->np_port, np->np_transport->name);
 
 	return np;
 }
@@ -441,11 +441,10 @@ int iscsit_reset_np_thread(
 	return 0;
 }
 
-static int iscsit_del_np_comm(struct iscsi_np *np)
+static void iscsit_free_np(struct iscsi_np *np)
 {
 	if (np->np_socket)
 		sock_release(np->np_socket);
-	return 0;
 }
 
 int iscsit_del_np(struct iscsi_np *np)
@@ -467,20 +466,47 @@ int iscsit_del_np(struct iscsi_np *np)
 		send_sig(SIGINT, np->np_thread, 1);
 		kthread_stop(np->np_thread);
 	}
-	iscsit_del_np_comm(np);
+
+	np->np_transport->iscsit_free_np(np);
 
 	spin_lock_bh(&np_lock);
 	list_del(&np->np_list);
 	spin_unlock_bh(&np_lock);
 
 	pr_debug("CORE[0] - Removed Network Portal: %s:%hu on %s\n",
-		np->np_ip, np->np_port, (np->np_network_transport == ISCSI_TCP) ?
-		"TCP" : "SCTP");
+		np->np_ip, np->np_port, np->np_transport->name);
 
+	iscsit_put_transport(np->np_transport);
 	kfree(np);
 	return 0;
 }
 
+static int iscsit_immediate_queue(struct iscsi_conn *, struct iscsi_cmd *, int);
+static int iscsit_response_queue(struct iscsi_conn *, struct iscsi_cmd *, int);
+
+static int iscsit_queue_rsp(struct iscsi_conn *conn, struct iscsi_cmd *cmd)
+{
+	iscsit_add_cmd_to_response_queue(cmd, cmd->conn, cmd->i_state);
+	return 0;
+}
+
+static struct iscsit_transport iscsi_target_transport = {
+	.name			= "iSCSI/TCP",
+	.transport_type		= ISCSI_TCP,
+	.owner			= NULL,
+	.iscsit_setup_np	= iscsit_setup_np,
+	.iscsit_accept_np	= iscsit_accept_np,
+	.iscsit_free_np		= iscsit_free_np,
+	.iscsit_alloc_cmd	= iscsit_alloc_cmd,
+	.iscsit_get_login_rx	= iscsit_get_login_rx,
+	.iscsit_put_login_tx	= iscsit_put_login_tx,
+	.iscsit_get_dataout	= iscsit_build_r2ts_for_cmd,
+	.iscsit_immediate_queue	= iscsit_immediate_queue,
+	.iscsit_response_queue	= iscsit_response_queue,
+	.iscsit_queue_data_in	= iscsit_queue_rsp,
+	.iscsit_queue_status	= iscsit_queue_rsp,
+};
+
 static int __init iscsi_target_init_module(void)
 {
 	int ret = 0;
@@ -557,6 +583,8 @@ static int __init iscsi_target_init_module(void)
 		goto ooo_out;
 	}
 
+	iscsit_register_transport(&iscsi_target_transport);
+
 	if (iscsit_load_discovery_tpg() < 0)
 		goto r2t_out;
 
@@ -587,6 +615,7 @@ static void __exit iscsi_target_cleanup_module(void)
 	iscsi_deallocate_thread_sets();
 	iscsi_thread_set_free();
 	iscsit_release_discovery_tpg();
+	iscsit_unregister_transport(&iscsi_target_transport);
 	kmem_cache_destroy(lio_cmd_cache);
 	kmem_cache_destroy(lio_qr_cache);
 	kmem_cache_destroy(lio_dr_cache);
@@ -682,11 +711,20 @@ int iscsit_add_reject_from_cmd(
 	iscsit_add_cmd_to_response_queue(cmd, conn, cmd->i_state);
 
 	ret = wait_for_completion_interruptible(&cmd->reject_comp);
+	/*
+	 * Perform the kref_put now if se_cmd has already been setup by
+	 * scsit_setup_scsi_cmd()
+	 */
+	if (cmd->se_cmd.se_tfo != NULL) {
+		pr_debug("iscsi reject: calling target_put_sess_cmd >>>>>>\n");
+		target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
+	}
 	if (ret != 0)
 		return -1;
 
 	return (!fail_conn) ? 0 : -1;
 }
+EXPORT_SYMBOL(iscsit_add_reject_from_cmd);
 
 /*
  * Map some portion of the allocated scatterlist to an iovec, suitable for
@@ -745,6 +783,9 @@ static void iscsit_ack_from_expstatsn(struct iscsi_conn *conn, u32 exp_statsn)
 
 	conn->exp_statsn = exp_statsn;
 
+	if (conn->sess->sess_ops->RDMAExtensions)
+		return;
+
 	spin_lock_bh(&conn->cmd_lock);
 	list_for_each_entry(cmd, &conn->conn_cmd_list, i_conn_node) {
 		spin_lock(&cmd->istate_lock);
@@ -777,12 +818,10 @@ static int iscsit_allocate_iovecs(struct iscsi_cmd *cmd)
 	return 0;
 }
 
-static int iscsit_handle_scsi_cmd(
-	struct iscsi_conn *conn,
-	unsigned char *buf)
+int iscsit_setup_scsi_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+			  unsigned char *buf)
 {
-	int data_direction, payload_length, cmdsn_ret = 0, immed_ret;
-	struct iscsi_cmd *cmd = NULL;
+	int data_direction, payload_length;
 	struct iscsi_scsi_req *hdr;
 	int iscsi_task_attr;
 	int sam_task_attr;
@@ -805,8 +844,8 @@ static int iscsit_handle_scsi_cmd(
 	    !(hdr->flags & ISCSI_FLAG_CMD_FINAL)) {
 		pr_err("ISCSI_FLAG_CMD_WRITE & ISCSI_FLAG_CMD_FINAL"
 				" not set. Bad iSCSI Initiator.\n");
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_INVALID, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_BOOKMARK_INVALID,
+				1, 1, buf, cmd);
 	}
 
 	if (((hdr->flags & ISCSI_FLAG_CMD_READ) ||
@@ -826,8 +865,8 @@ static int iscsit_handle_scsi_cmd(
 		pr_err("ISCSI_FLAG_CMD_READ or ISCSI_FLAG_CMD_WRITE"
 			" set when Expected Data Transfer Length is 0 for"
 			" CDB: 0x%02x. Bad iSCSI Initiator.\n", hdr->cdb[0]);
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_INVALID, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_BOOKMARK_INVALID,
+				1, 1, buf, cmd);
 	}
 done:
 
@@ -836,29 +875,29 @@ done:
 		pr_err("ISCSI_FLAG_CMD_READ and/or ISCSI_FLAG_CMD_WRITE"
 			" MUST be set if Expected Data Transfer Length is not 0."
 			" Bad iSCSI Initiator\n");
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_INVALID, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_BOOKMARK_INVALID,
+				1, 1, buf, cmd);
 	}
 
 	if ((hdr->flags & ISCSI_FLAG_CMD_READ) &&
 	    (hdr->flags & ISCSI_FLAG_CMD_WRITE)) {
 		pr_err("Bidirectional operations not supported!\n");
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_INVALID, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_BOOKMARK_INVALID,
+				1, 1, buf, cmd);
 	}
 
 	if (hdr->opcode & ISCSI_OP_IMMEDIATE) {
 		pr_err("Illegally set Immediate Bit in iSCSI Initiator"
 				" Scsi Command PDU.\n");
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_INVALID, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_BOOKMARK_INVALID,
+				1, 1, buf, cmd);
 	}
 
 	if (payload_length && !conn->sess->sess_ops->ImmediateData) {
 		pr_err("ImmediateData=No but DataSegmentLength=%u,"
 			" protocol error.\n", payload_length);
-		return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_PROTOCOL_ERROR,
+				1, 1, buf, cmd);
 	}
 
 	if ((be32_to_cpu(hdr->data_length )== payload_length) &&
@@ -866,43 +905,38 @@ done:
 		pr_err("Expected Data Transfer Length and Length of"
 			" Immediate Data are the same, but ISCSI_FLAG_CMD_FINAL"
 			" bit is not set protocol error\n");
-		return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_PROTOCOL_ERROR,
+				1, 1, buf, cmd);
 	}
 
 	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);
-		return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_PROTOCOL_ERROR,
+				1, 1, buf, cmd);
 	}
 
 	if (payload_length > conn->conn_ops->MaxXmitDataSegmentLength) {
 		pr_err("DataSegmentLength: %u is greater than"
 			" MaxXmitDataSegmentLength: %u, protocol error.\n",
 			payload_length, conn->conn_ops->MaxXmitDataSegmentLength);
-		return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
-				buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_PROTOCOL_ERROR,
+				1, 1, buf, cmd);
 	}
 
 	if (payload_length > conn->sess->sess_ops->FirstBurstLength) {
 		pr_err("DataSegmentLength: %u is greater than"
 			" FirstBurstLength: %u, protocol error.\n",
 			payload_length, conn->sess->sess_ops->FirstBurstLength);
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_INVALID, 1,
-					buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_BOOKMARK_INVALID,
+				1, 1, buf, cmd);
 	}
 
 	data_direction = (hdr->flags & ISCSI_FLAG_CMD_WRITE) ? DMA_TO_DEVICE :
 			 (hdr->flags & ISCSI_FLAG_CMD_READ) ? DMA_FROM_DEVICE :
 			  DMA_NONE;
 
-	cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
-	if (!cmd)
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES, 1,
-					 buf, conn);
-
 	cmd->data_direction = data_direction;
 	iscsi_task_attr = hdr->flags & ISCSI_FLAG_CMD_ATTR_MASK;
 	/*
@@ -945,7 +979,8 @@ done:
 	cmd->exp_stat_sn	= be32_to_cpu(hdr->exp_statsn);
 	cmd->first_burst_len	= payload_length;
 
-	if (cmd->data_direction == DMA_FROM_DEVICE) {
+	if (!conn->sess->sess_ops->RDMAExtensions &&
+	     cmd->data_direction == DMA_FROM_DEVICE) {
 		struct iscsi_datain_req *dr;
 
 		dr = iscsit_allocate_datain_req();
@@ -967,7 +1002,10 @@ done:
 
 	pr_debug("Got SCSI Command, ITT: 0x%08x, CmdSN: 0x%08x,"
 		" ExpXferLen: %u, Length: %u, CID: %hu\n", hdr->itt,
-		hdr->cmdsn, hdr->data_length, payload_length, conn->cid);
+		hdr->cmdsn, be32_to_cpu(hdr->data_length), payload_length,
+		conn->cid);
+
+	target_get_sess_cmd(conn->sess->se_sess, &cmd->se_cmd, true);
 
 	cmd->sense_reason = transport_lookup_cmd_lun(&cmd->se_cmd,
 						     scsilun_to_int(&hdr->lun));
@@ -1001,12 +1039,24 @@ attach_cmd:
 	 */
 	core_alua_check_nonop_delay(&cmd->se_cmd);
 
-	if (iscsit_allocate_iovecs(cmd) < 0) {
-		return iscsit_add_reject_from_cmd(
-				ISCSI_REASON_BOOKMARK_NO_RESOURCES,
-				1, 0, buf, cmd);
-	}
+	return 0;
+}
+EXPORT_SYMBOL(iscsit_setup_scsi_cmd);
+
+void iscsit_set_unsoliticed_dataout(struct iscsi_cmd *cmd)
+{
+	iscsit_set_dataout_sequence_values(cmd);
+
+	spin_lock_bh(&cmd->dataout_timeout_lock);
+	iscsit_start_dataout_timer(cmd, cmd->conn);
+	spin_unlock_bh(&cmd->dataout_timeout_lock);
+}
+EXPORT_SYMBOL(iscsit_set_unsoliticed_dataout);
 
+int iscsit_process_scsi_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+			    struct iscsi_scsi_req *hdr)
+{
+	int cmdsn_ret = 0;
 	/*
 	 * Check the CmdSN against ExpCmdSN/MaxCmdSN here if
 	 * the Immediate Bit is not set, and no Immediate
@@ -1019,12 +1069,17 @@ attach_cmd:
 	 */
 	if (!cmd->immediate_data) {
 		cmdsn_ret = iscsit_sequence_cmd(conn, cmd, hdr->cmdsn);
-		if (cmdsn_ret == CMDSN_LOWER_THAN_EXP)
+		if (cmdsn_ret == CMDSN_LOWER_THAN_EXP) {
+			if (!cmd->sense_reason)
+				return 0;
+
+			target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
 			return 0;
-		else if (cmdsn_ret == CMDSN_ERROR_CANNOT_RECOVER)
+		} else if (cmdsn_ret == CMDSN_ERROR_CANNOT_RECOVER) {
 			return iscsit_add_reject_from_cmd(
 				ISCSI_REASON_PROTOCOL_ERROR,
-				1, 0, buf, cmd);
+				1, 0, (unsigned char *)hdr, cmd);
+		}
 	}
 
 	iscsit_ack_from_expstatsn(conn, be32_to_cpu(hdr->exp_statsn));
@@ -1033,25 +1088,23 @@ attach_cmd:
 	 * If no Immediate Data is attached, it's OK to return now.
 	 */
 	if (!cmd->immediate_data) {
-		if (!cmd->sense_reason && cmd->unsolicited_data) {
-			iscsit_set_dataout_sequence_values(cmd);
-
-			spin_lock_bh(&cmd->dataout_timeout_lock);
-			iscsit_start_dataout_timer(cmd, cmd->conn);
-			spin_unlock_bh(&cmd->dataout_timeout_lock);
-		}
+		if (!cmd->sense_reason && cmd->unsolicited_data)
+			iscsit_set_unsoliticed_dataout(cmd);
+		if (!cmd->sense_reason)
+			return 0;
 
+		target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
 		return 0;
 	}
 
 	/*
-	 * Early CHECK_CONDITIONs never make it to the transport processing
-	 * thread.  They are processed in CmdSN order by
-	 * iscsit_check_received_cmdsn() below.
+	 * Early CHECK_CONDITIONs with ImmediateData never make it to command
+	 * execution.  These exceptions are processed in CmdSN order using
+	 * iscsit_check_received_cmdsn() in iscsit_get_immediate_data() below.
 	 */
 	if (cmd->sense_reason) {
-		immed_ret = IMMEDIATE_DATA_NORMAL_OPERATION;
-		goto after_immediate_data;
+		target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
+		return 1;
 	}
 	/*
 	 * Call directly into transport_generic_new_cmd() to perform
@@ -1059,11 +1112,27 @@ attach_cmd:
 	 */
 	cmd->sense_reason = transport_generic_new_cmd(&cmd->se_cmd);
 	if (cmd->sense_reason) {
-		immed_ret = IMMEDIATE_DATA_NORMAL_OPERATION;
-		goto after_immediate_data;
+		target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
+		return 1;
 	}
 
-	immed_ret = iscsit_handle_immediate_data(cmd, buf, payload_length);
+	return 0;
+}
+EXPORT_SYMBOL(iscsit_process_scsi_cmd);
+
+static int
+iscsit_get_immediate_data(struct iscsi_cmd *cmd, struct iscsi_scsi_req *hdr,
+			  bool dump_payload)
+{
+	int cmdsn_ret = 0, immed_ret = IMMEDIATE_DATA_NORMAL_OPERATION;
+	/*
+	 * Special case for Unsupported SAM WRITE Opcodes and ImmediateData=Yes.
+	 */
+	if (dump_payload == true)
+		goto after_immediate_data;
+
+	immed_ret = iscsit_handle_immediate_data(cmd, hdr,
+					cmd->first_burst_len);
 after_immediate_data:
 	if (immed_ret == IMMEDIATE_DATA_NORMAL_OPERATION) {
 		/*
@@ -1071,26 +1140,19 @@ after_immediate_data:
 		 * DataCRC, check against ExpCmdSN/MaxCmdSN if
 		 * Immediate Bit is not set.
 		 */
-		cmdsn_ret = iscsit_sequence_cmd(conn, cmd, hdr->cmdsn);
-		/*
-		 * Special case for Unsupported SAM WRITE Opcodes
-		 * and ImmediateData=Yes.
-		 */
+		cmdsn_ret = iscsit_sequence_cmd(cmd->conn, cmd, hdr->cmdsn);
+
 		if (cmd->sense_reason) {
-			if (iscsit_dump_data_payload(conn, payload_length, 1) < 0)
+			if (iscsit_dump_data_payload(cmd->conn,
+					cmd->first_burst_len, 1) < 0)
 				return -1;
-		} else if (cmd->unsolicited_data) {
-			iscsit_set_dataout_sequence_values(cmd);
-
-			spin_lock_bh(&cmd->dataout_timeout_lock);
-			iscsit_start_dataout_timer(cmd, cmd->conn);
-			spin_unlock_bh(&cmd->dataout_timeout_lock);
-		}
+		} else if (cmd->unsolicited_data)
+			iscsit_set_unsoliticed_dataout(cmd);
 
 		if (cmdsn_ret == CMDSN_ERROR_CANNOT_RECOVER)
 			return iscsit_add_reject_from_cmd(
 				ISCSI_REASON_PROTOCOL_ERROR,
-				1, 0, buf, cmd);
+				1, 0, (unsigned char *)hdr, cmd);
 
 	} else if (immed_ret == IMMEDIATE_DATA_ERL1_CRC_FAILURE) {
 		/*
@@ -1105,13 +1167,47 @@ after_immediate_data:
 		 * CmdSN and issue a retry to plug the sequence.
 		 */
 		cmd->i_state = ISTATE_REMOVE;
-		iscsit_add_cmd_to_immediate_queue(cmd, conn, cmd->i_state);
+		iscsit_add_cmd_to_immediate_queue(cmd, cmd->conn, cmd->i_state);
 	} else /* immed_ret == IMMEDIATE_DATA_CANNOT_RECOVER */
 		return -1;
 
 	return 0;
 }
 
+static int
+iscsit_handle_scsi_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+			   unsigned char *buf)
+{
+	struct iscsi_scsi_req *hdr = (struct iscsi_scsi_req *)buf;
+	int rc, immed_data;
+	bool dump_payload = false;
+
+	rc = iscsit_setup_scsi_cmd(conn, cmd, buf);
+	if (rc < 0)
+		return rc;
+	/*
+	 * Allocation iovecs needed for struct socket operations for
+	 * traditional iSCSI block I/O.
+	 */
+	if (iscsit_allocate_iovecs(cmd) < 0) {
+		return iscsit_add_reject_from_cmd(
+				ISCSI_REASON_BOOKMARK_NO_RESOURCES,
+				1, 0, buf, cmd);
+	}
+	immed_data = cmd->immediate_data;
+
+	rc = iscsit_process_scsi_cmd(conn, cmd, hdr);
+	if (rc < 0)
+		return rc;
+	else if (rc > 0)
+		dump_payload = true;
+
+	if (!immed_data)
+		return 0;
+
+	return iscsit_get_immediate_data(cmd, hdr, dump_payload);
+}
+
 static u32 iscsit_do_crypto_hash_sg(
 	struct hash_desc *hash,
 	struct iscsi_cmd *cmd,
@@ -1174,20 +1270,16 @@ static void iscsit_do_crypto_hash_buf(
 	crypto_hash_final(hash, data_crc);
 }
 
-static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
+int
+iscsit_check_dataout_hdr(struct iscsi_conn *conn, unsigned char *buf,
+			  struct iscsi_cmd **out_cmd)
 {
-	int iov_ret, ooo_cmdsn = 0, ret;
-	u8 data_crc_failed = 0;
-	u32 checksum, iov_count = 0, padding = 0, rx_got = 0;
-	u32 rx_size = 0, payload_length;
+	struct iscsi_data *hdr = (struct iscsi_data *)buf;
 	struct iscsi_cmd *cmd = NULL;
 	struct se_cmd *se_cmd;
-	struct iscsi_data *hdr;
-	struct kvec *iov;
 	unsigned long flags;
-
-	hdr			= (struct iscsi_data *) buf;
-	payload_length		= ntoh24(hdr->dlength);
+	u32 payload_length = ntoh24(hdr->dlength);
+	int rc;
 
 	if (!payload_length) {
 		pr_err("DataOUT payload is ZERO, protocol error.\n");
@@ -1220,7 +1312,7 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 
 	pr_debug("Got DataOut ITT: 0x%08x, TTT: 0x%08x,"
 		" DataSN: 0x%08x, Offset: %u, Length: %u, CID: %hu\n",
-		hdr->itt, hdr->ttt, hdr->datasn, hdr->offset,
+		hdr->itt, hdr->ttt, hdr->datasn, ntohl(hdr->offset),
 		payload_length, conn->cid);
 
 	if (cmd->cmd_flags & ICF_GOT_LAST_DATAOUT) {
@@ -1312,12 +1404,26 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 	 * Preform DataSN, DataSequenceInOrder, DataPDUInOrder, and
 	 * within-command recovery checks before receiving the payload.
 	 */
-	ret = iscsit_check_pre_dataout(cmd, buf);
-	if (ret == DATAOUT_WITHIN_COMMAND_RECOVERY)
+	rc = iscsit_check_pre_dataout(cmd, buf);
+	if (rc == DATAOUT_WITHIN_COMMAND_RECOVERY)
 		return 0;
-	else if (ret == DATAOUT_CANNOT_RECOVER)
+	else if (rc == DATAOUT_CANNOT_RECOVER)
 		return -1;
 
+	*out_cmd = cmd;
+	return 0;
+}
+EXPORT_SYMBOL(iscsit_check_dataout_hdr);
+
+static int
+iscsit_get_dataout(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+		   struct iscsi_data *hdr)
+{
+	struct kvec *iov;
+	u32 checksum, iov_count = 0, padding = 0, rx_got = 0, rx_size = 0;
+	u32 payload_length = ntoh24(hdr->dlength);
+	int iov_ret, data_crc_failed = 0;
+
 	rx_size += payload_length;
 	iov = &cmd->iov_data[0];
 
@@ -1370,17 +1476,27 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 				payload_length);
 		}
 	}
+
+	return data_crc_failed;
+}
+
+int
+iscsit_check_dataout_payload(struct iscsi_cmd *cmd, struct iscsi_data *hdr,
+			     bool data_crc_failed)
+{
+	struct iscsi_conn *conn = cmd->conn;
+	int rc, ooo_cmdsn;
 	/*
 	 * Increment post receive data and CRC values or perform
 	 * within-command recovery.
 	 */
-	ret = iscsit_check_post_dataout(cmd, buf, data_crc_failed);
-	if ((ret == DATAOUT_NORMAL) || (ret == DATAOUT_WITHIN_COMMAND_RECOVERY))
+	rc = iscsit_check_post_dataout(cmd, (unsigned char *)hdr, data_crc_failed);
+	if ((rc == DATAOUT_NORMAL) || (rc == DATAOUT_WITHIN_COMMAND_RECOVERY))
 		return 0;
-	else if (ret == DATAOUT_SEND_R2T) {
+	else if (rc == DATAOUT_SEND_R2T) {
 		iscsit_set_dataout_sequence_values(cmd);
-		iscsit_build_r2ts_for_cmd(cmd, conn, false);
-	} else if (ret == DATAOUT_SEND_TO_TRANSPORT) {
+		conn->conn_transport->iscsit_get_dataout(conn, cmd, false);
+	} else if (rc == DATAOUT_SEND_TO_TRANSPORT) {
 		/*
 		 * Handle extra special case for out of order
 		 * Unsolicited Data Out.
@@ -1401,15 +1517,37 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 
 	return 0;
 }
+EXPORT_SYMBOL(iscsit_check_dataout_payload);
 
-static int iscsit_handle_nop_out(
-	struct iscsi_conn *conn,
-	unsigned char *buf)
+static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
+{
+	struct iscsi_cmd *cmd;
+	struct iscsi_data *hdr = (struct iscsi_data *)buf;
+	int rc;
+	bool data_crc_failed = false;
+
+	rc = iscsit_check_dataout_hdr(conn, buf, &cmd);
+	if (rc < 0)
+		return rc;
+	else if (!cmd)
+		return 0;
+
+	rc = iscsit_get_dataout(conn, cmd, hdr);
+	if (rc < 0)
+		return rc;
+	else if (rc > 0)
+		data_crc_failed = true;
+
+	return iscsit_check_dataout_payload(cmd, hdr, data_crc_failed);
+}
+
+int iscsit_handle_nop_out(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+			unsigned char *buf)
 {
 	unsigned char *ping_data = NULL;
 	int cmdsn_ret, niov = 0, ret = 0, rx_got, rx_size;
 	u32 checksum, data_crc, padding = 0, payload_length;
-	struct iscsi_cmd *cmd = NULL;
+	struct iscsi_cmd *cmd_p = NULL;
 	struct kvec *iov = NULL;
 	struct iscsi_nopout *hdr;
 
@@ -1432,7 +1570,7 @@ static int iscsit_handle_nop_out(
 					buf, conn);
 	}
 
-	pr_debug("Got NOPOUT Ping %s ITT: 0x%08x, TTT: 0x%09x,"
+	pr_debug("Got NOPOUT Ping %s ITT: 0x%08x, TTT: 0x%08x,"
 		" CmdSN: 0x%08x, ExpStatSN: 0x%08x, Length: %u\n",
 		hdr->itt == RESERVED_ITT ? "Response" : "Request",
 		hdr->itt, hdr->ttt, hdr->cmdsn, hdr->exp_statsn,
@@ -1445,7 +1583,6 @@ static int iscsit_handle_nop_out(
 	 * can contain ping data.
 	 */
 	if (hdr->ttt == cpu_to_be32(0xFFFFFFFF)) {
-		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
 		if (!cmd)
 			return iscsit_add_reject(
 					ISCSI_REASON_BOOKMARK_NO_RESOURCES,
@@ -1580,14 +1717,14 @@ static int iscsit_handle_nop_out(
 		/*
 		 * This was a response to a unsolicited NOPIN ping.
 		 */
-		cmd = iscsit_find_cmd_from_ttt(conn, be32_to_cpu(hdr->ttt));
-		if (!cmd)
+		cmd_p = iscsit_find_cmd_from_ttt(conn, be32_to_cpu(hdr->ttt));
+		if (!cmd_p)
 			return -1;
 
 		iscsit_stop_nopin_response_timer(conn);
 
-		cmd->i_state = ISTATE_REMOVE;
-		iscsit_add_cmd_to_immediate_queue(cmd, conn, cmd->i_state);
+		cmd_p->i_state = ISTATE_REMOVE;
+		iscsit_add_cmd_to_immediate_queue(cmd_p, conn, cmd_p->i_state);
 		iscsit_start_nopin_timer(conn);
 	} else {
 		/*
@@ -1611,12 +1748,12 @@ ping_out:
 	kfree(ping_data);
 	return ret;
 }
+EXPORT_SYMBOL(iscsit_handle_nop_out);
 
-static int iscsit_handle_task_mgt_cmd(
-	struct iscsi_conn *conn,
-	unsigned char *buf)
+int
+iscsit_handle_task_mgt_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+			   unsigned char *buf)
 {
-	struct iscsi_cmd *cmd;
 	struct se_tmr_req *se_tmr;
 	struct iscsi_tmr_req *tmr_req;
 	struct iscsi_tm *hdr;
@@ -1645,18 +1782,13 @@ static int iscsit_handle_task_mgt_cmd(
 		pr_err("Task Management Request TASK_REASSIGN not"
 			" issued as immediate command, bad iSCSI Initiator"
 				"implementation\n");
-		return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
-					buf, conn);
+		return iscsit_add_reject_from_cmd(ISCSI_REASON_PROTOCOL_ERROR,
+					1, 1, buf, cmd);
 	}
 	if ((function != ISCSI_TM_FUNC_ABORT_TASK) &&
 	    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)
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES,
-					 1, buf, conn);
-
 	cmd->data_direction = DMA_NONE;
 
 	cmd->tmr_req = kzalloc(sizeof(struct iscsi_tmr_req), GFP_KERNEL);
@@ -1827,6 +1959,7 @@ attach:
 	iscsit_add_cmd_to_response_queue(cmd, conn, cmd->i_state);
 	return 0;
 }
+EXPORT_SYMBOL(iscsit_handle_task_mgt_cmd);
 
 /* #warning FIXME: Support Text Command parameters besides SendTargets */
 static int iscsit_handle_text_cmd(
@@ -2089,13 +2222,12 @@ int iscsit_logout_removeconnforrecovery(struct iscsi_cmd *cmd, struct iscsi_conn
 	return 0;
 }
 
-static int iscsit_handle_logout_cmd(
-	struct iscsi_conn *conn,
-	unsigned char *buf)
+int
+iscsit_handle_logout_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+			unsigned char *buf)
 {
 	int cmdsn_ret, logout_remove = 0;
 	u8 reason_code = 0;
-	struct iscsi_cmd *cmd;
 	struct iscsi_logout *hdr;
 	struct iscsi_tiqn *tiqn = iscsit_snmp_get_tiqn(conn);
 
@@ -2119,14 +2251,10 @@ static int iscsit_handle_logout_cmd(
 	if (conn->conn_state != TARG_CONN_STATE_LOGGED_IN) {
 		pr_err("Received logout request on connection that"
 			" is not in logged in state, ignoring request.\n");
+		iscsit_release_cmd(cmd);
 		return 0;
 	}
 
-	cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
-	if (!cmd)
-		return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES, 1,
-					buf, conn);
-
 	cmd->iscsi_opcode       = ISCSI_OP_LOGOUT;
 	cmd->i_state            = ISTATE_SEND_LOGOUTRSP;
 	cmd->immediate_cmd      = ((hdr->opcode & ISCSI_OP_IMMEDIATE) ? 1 : 0);
@@ -2176,6 +2304,7 @@ static int iscsit_handle_logout_cmd(
 
 	return logout_remove;
 }
+EXPORT_SYMBOL(iscsit_handle_logout_cmd);
 
 static int iscsit_handle_snack(
 	struct iscsi_conn *conn,
@@ -2243,7 +2372,7 @@ static void iscsit_rx_thread_wait_for_tcp(struct iscsi_conn *conn)
 
 static int iscsit_handle_immediate_data(
 	struct iscsi_cmd *cmd,
-	unsigned char *buf,
+	struct iscsi_scsi_req *hdr,
 	u32 length)
 {
 	int iov_ret, rx_got = 0, rx_size = 0;
@@ -2299,12 +2428,12 @@ static int iscsit_handle_immediate_data(
 					" in ERL=0.\n");
 				iscsit_add_reject_from_cmd(
 						ISCSI_REASON_DATA_DIGEST_ERROR,
-						1, 0, buf, cmd);
+						1, 0, (unsigned char *)hdr, cmd);
 				return IMMEDIATE_DATA_CANNOT_RECOVER;
 			} else {
 				iscsit_add_reject_from_cmd(
 						ISCSI_REASON_DATA_DIGEST_ERROR,
-						0, 0, buf, cmd);
+						0, 0, (unsigned char *)hdr, cmd);
 				return IMMEDIATE_DATA_ERL1_CRC_FAILURE;
 			}
 		} else {
@@ -2424,18 +2553,60 @@ static void iscsit_tx_thread_wait_for_tcp(struct iscsi_conn *conn)
 	}
 }
 
-static int iscsit_send_data_in(
-	struct iscsi_cmd *cmd,
-	struct iscsi_conn *conn)
+static void
+iscsit_build_datain_pdu(struct iscsi_cmd *cmd, struct iscsi_conn *conn,
+			struct iscsi_datain *datain, struct iscsi_data_rsp *hdr,
+			bool set_statsn)
 {
-	int iov_ret = 0, set_statsn = 0;
-	u32 iov_count = 0, tx_size = 0;
+	hdr->opcode		= ISCSI_OP_SCSI_DATA_IN;
+	hdr->flags		= datain->flags;
+	if (hdr->flags & ISCSI_FLAG_DATA_STATUS) {
+		if (cmd->se_cmd.se_cmd_flags & SCF_OVERFLOW_BIT) {
+			hdr->flags |= ISCSI_FLAG_DATA_OVERFLOW;
+			hdr->residual_count = cpu_to_be32(cmd->se_cmd.residual_count);
+		} else if (cmd->se_cmd.se_cmd_flags & SCF_UNDERFLOW_BIT) {
+			hdr->flags |= ISCSI_FLAG_DATA_UNDERFLOW;
+			hdr->residual_count = cpu_to_be32(cmd->se_cmd.residual_count);
+		}
+	}
+	hton24(hdr->dlength, datain->length);
+	if (hdr->flags & ISCSI_FLAG_DATA_ACK)
+		int_to_scsilun(cmd->se_cmd.orig_fe_lun,
+				(struct scsi_lun *)&hdr->lun);
+	else
+		put_unaligned_le64(0xFFFFFFFFFFFFFFFFULL, &hdr->lun);
+
+	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);
+	hdr->offset		= cpu_to_be32(datain->offset);
+
+	pr_debug("Built DataIN ITT: 0x%08x, StatSN: 0x%08x,"
+		" DataSN: 0x%08x, Offset: %u, Length: %u, CID: %hu\n",
+		cmd->init_task_tag, ntohl(hdr->statsn), ntohl(hdr->datasn),
+		ntohl(hdr->offset), datain->length, conn->cid);
+}
+
+static int iscsit_send_datain(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+{
+	struct iscsi_data_rsp *hdr = (struct iscsi_data_rsp *)&cmd->pdu[0];
 	struct iscsi_datain datain;
 	struct iscsi_datain_req *dr;
-	struct iscsi_data_rsp *hdr;
 	struct kvec *iov;
-	int eodr = 0;
-	int ret;
+	u32 iov_count = 0, tx_size = 0;
+	int eodr = 0, ret, iov_ret;
+	bool set_statsn = false;
 
 	memset(&datain, 0, sizeof(struct iscsi_datain));
 	dr = iscsit_get_datain_values(cmd, &datain);
@@ -2444,7 +2615,6 @@ static int iscsit_send_data_in(
 				cmd->init_task_tag);
 		return -1;
 	}
-
 	/*
 	 * Be paranoid and double check the logic for now.
 	 */
@@ -2452,7 +2622,7 @@ static int iscsit_send_data_in(
 		pr_err("Command ITT: 0x%08x, datain.offset: %u and"
 			" datain.length: %u exceeds cmd->data_length: %u\n",
 			cmd->init_task_tag, datain.offset, datain.length,
-				cmd->se_cmd.data_length);
+			cmd->se_cmd.data_length);
 		return -1;
 	}
 
@@ -2476,47 +2646,13 @@ static int iscsit_send_data_in(
 		    (dr->dr_complete == DATAIN_COMPLETE_CONNECTION_RECOVERY)) {
 			iscsit_increment_maxcmdsn(cmd, conn->sess);
 			cmd->stat_sn = conn->stat_sn++;
-			set_statsn = 1;
+			set_statsn = true;
 		} else if (dr->dr_complete ==
-				DATAIN_COMPLETE_WITHIN_COMMAND_RECOVERY)
-			set_statsn = 1;
+			   DATAIN_COMPLETE_WITHIN_COMMAND_RECOVERY)
+			set_statsn = true;
 	}
 
-	hdr	= (struct iscsi_data_rsp *) cmd->pdu;
-	memset(hdr, 0, ISCSI_HDR_LEN);
-	hdr->opcode		= ISCSI_OP_SCSI_DATA_IN;
-	hdr->flags		= datain.flags;
-	if (hdr->flags & ISCSI_FLAG_DATA_STATUS) {
-		if (cmd->se_cmd.se_cmd_flags & SCF_OVERFLOW_BIT) {
-			hdr->flags |= ISCSI_FLAG_DATA_OVERFLOW;
-			hdr->residual_count = cpu_to_be32(cmd->se_cmd.residual_count);
-		} else if (cmd->se_cmd.se_cmd_flags & SCF_UNDERFLOW_BIT) {
-			hdr->flags |= ISCSI_FLAG_DATA_UNDERFLOW;
-			hdr->residual_count = cpu_to_be32(cmd->se_cmd.residual_count);
-		}
-	}
-	hton24(hdr->dlength, datain.length);
-	if (hdr->flags & ISCSI_FLAG_DATA_ACK)
-		int_to_scsilun(cmd->se_cmd.orig_fe_lun,
-				(struct scsi_lun *)&hdr->lun);
-	else
-		put_unaligned_le64(0xFFFFFFFFFFFFFFFFULL, &hdr->lun);
-
-	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);
-	hdr->offset		= cpu_to_be32(datain.offset);
+	iscsit_build_datain_pdu(cmd, conn, &datain, hdr, set_statsn);
 
 	iov = &cmd->iov_data[0];
 	iov[iov_count].iov_base	= cmd->pdu;
@@ -2527,7 +2663,7 @@ static int iscsit_send_data_in(
 		u32 *header_digest = (u32 *)&cmd->pdu[ISCSI_HDR_LEN];
 
 		iscsit_do_crypto_hash_buf(&conn->conn_tx_hash,
-				(unsigned char *)hdr, ISCSI_HDR_LEN,
+				(unsigned char *)cmd->pdu, ISCSI_HDR_LEN,
 				0, NULL, (u8 *)header_digest);
 
 		iov[0].iov_len += ISCSI_CRC_LEN;
@@ -2537,7 +2673,8 @@ static int iscsit_send_data_in(
 			" for DataIN PDU 0x%08x\n", *header_digest);
 	}
 
-	iov_ret = iscsit_map_iovec(cmd, &cmd->iov_data[1], datain.offset, datain.length);
+	iov_ret = iscsit_map_iovec(cmd, &cmd->iov_data[1],
+				datain.offset, datain.length);
 	if (iov_ret < 0)
 		return -1;
 
@@ -2568,11 +2705,6 @@ static int iscsit_send_data_in(
 	cmd->iov_data_count = iov_count;
 	cmd->tx_size = tx_size;
 
-	pr_debug("Built DataIN ITT: 0x%08x, StatSN: 0x%08x,"
-		" DataSN: 0x%08x, Offset: %u, Length: %u, CID: %hu\n",
-		cmd->init_task_tag, ntohl(hdr->statsn), ntohl(hdr->datasn),
-		ntohl(hdr->offset), datain.length, conn->cid);
-
 	/* sendpage is preferred but can't insert markers */
 	if (!conn->conn_ops->IFMarker)
 		ret = iscsit_fe_sendpage_sg(cmd, conn);
@@ -2595,16 +2727,13 @@ static int iscsit_send_data_in(
 	return eodr;
 }
 
-static int iscsit_send_logout_response(
-	struct iscsi_cmd *cmd,
-	struct iscsi_conn *conn)
+int
+iscsit_build_logout_rsp(struct iscsi_cmd *cmd, struct iscsi_conn *conn,
+			struct iscsi_logout_rsp *hdr)
 {
-	int niov = 0, tx_size;
 	struct iscsi_conn *logout_conn = NULL;
 	struct iscsi_conn_recovery *cr = NULL;
 	struct iscsi_session *sess = conn->sess;
-	struct kvec *iov;
-	struct iscsi_logout_rsp *hdr;
 	/*
 	 * The actual shutting down of Sessions and/or Connections
 	 * for CLOSESESSION and CLOSECONNECTION Logout Requests
@@ -2673,9 +2802,6 @@ static int iscsit_send_logout_response(
 		return -1;
 	}
 
-	tx_size = ISCSI_HDR_LEN;
-	hdr			= (struct iscsi_logout_rsp *)cmd->pdu;
-	memset(hdr, 0, ISCSI_HDR_LEN);
 	hdr->opcode		= ISCSI_OP_LOGOUT_RSP;
 	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
 	hdr->response		= cmd->logout_response;
@@ -2687,6 +2813,27 @@ static int iscsit_send_logout_response(
 	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
 	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
 
+	pr_debug("Built Logout Response ITT: 0x%08x StatSN:"
+		" 0x%08x Response: 0x%02x CID: %hu on CID: %hu\n",
+		cmd->init_task_tag, cmd->stat_sn, hdr->response,
+		cmd->logout_cid, conn->cid);
+
+	return 0;
+}
+EXPORT_SYMBOL(iscsit_build_logout_rsp);
+
+static int
+iscsit_send_logout(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+{
+	struct kvec *iov;
+	int niov = 0, tx_size, rc;
+
+	rc = iscsit_build_logout_rsp(cmd, conn,
+			(struct iscsi_logout_rsp *)&cmd->pdu[0]);
+	if (rc < 0)
+		return rc;
+
+	tx_size = ISCSI_HDR_LEN;
 	iov = &cmd->iov_misc[0];
 	iov[niov].iov_base	= cmd->pdu;
 	iov[niov++].iov_len	= ISCSI_HDR_LEN;
@@ -2695,7 +2842,7 @@ static int iscsit_send_logout_response(
 		u32 *header_digest = (u32 *)&cmd->pdu[ISCSI_HDR_LEN];
 
 		iscsit_do_crypto_hash_buf(&conn->conn_tx_hash,
-				(unsigned char *)hdr, ISCSI_HDR_LEN,
+				(unsigned char *)&cmd->pdu[0], ISCSI_HDR_LEN,
 				0, NULL, (u8 *)header_digest);
 
 		iov[0].iov_len += ISCSI_CRC_LEN;
@@ -2706,14 +2853,37 @@ static int iscsit_send_logout_response(
 	cmd->iov_misc_count = niov;
 	cmd->tx_size = tx_size;
 
-	pr_debug("Sending Logout Response ITT: 0x%08x StatSN:"
-		" 0x%08x Response: 0x%02x CID: %hu on CID: %hu\n",
-		cmd->init_task_tag, cmd->stat_sn, hdr->response,
-		cmd->logout_cid, conn->cid);
-
 	return 0;
 }
 
+void
+iscsit_build_nopin_rsp(struct iscsi_cmd *cmd, struct iscsi_conn *conn,
+		       struct iscsi_nopin *hdr, bool nopout_response)
+{
+	hdr->opcode		= ISCSI_OP_NOOP_IN;
+	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
+        hton24(hdr->dlength, cmd->buf_ptr_size);
+	if (nopout_response)
+		put_unaligned_le64(0xFFFFFFFFFFFFFFFFULL, &hdr->lun);
+	hdr->itt		= cmd->init_task_tag;
+	hdr->ttt		= cpu_to_be32(cmd->targ_xfer_tag);
+	cmd->stat_sn		= (nopout_response) ? conn->stat_sn++ :
+				  conn->stat_sn;
+	hdr->statsn		= cpu_to_be32(cmd->stat_sn);
+
+	if (nopout_response)
+		iscsit_increment_maxcmdsn(cmd, conn->sess);
+
+	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
+	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
+
+	pr_debug("Built NOPIN %s Response ITT: 0x%08x, TTT: 0x%08x,"
+		" StatSN: 0x%08x, Length %u\n", (nopout_response) ?
+		"Solicitied" : "Unsolicitied", cmd->init_task_tag,
+		cmd->targ_xfer_tag, cmd->stat_sn, cmd->buf_ptr_size);
+}
+EXPORT_SYMBOL(iscsit_build_nopin_rsp);
+
 /*
  *	Unsolicited NOPIN, either requesting a response or not.
  */
@@ -2722,20 +2892,10 @@ static int iscsit_send_unsolicited_nopin(
 	struct iscsi_conn *conn,
 	int want_response)
 {
-	int tx_size = ISCSI_HDR_LEN;
-	struct iscsi_nopin *hdr;
-	int ret;
+	struct iscsi_nopin *hdr = (struct iscsi_nopin *)&cmd->pdu[0];
+	int tx_size = ISCSI_HDR_LEN, ret;
 
-	hdr			= (struct iscsi_nopin *) cmd->pdu;
-	memset(hdr, 0, ISCSI_HDR_LEN);
-	hdr->opcode		= ISCSI_OP_NOOP_IN;
-	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
-	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);
-	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
-	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
+	iscsit_build_nopin_rsp(cmd, conn, hdr, false);
 
 	if (conn->conn_ops->HeaderDigest) {
 		u32 *header_digest = (u32 *)&cmd->pdu[ISCSI_HDR_LEN];
@@ -2771,31 +2931,17 @@ static int iscsit_send_unsolicited_nopin(
 	return 0;
 }
 
-static int iscsit_send_nopin_response(
-	struct iscsi_cmd *cmd,
-	struct iscsi_conn *conn)
+static int
+iscsit_send_nopin(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
 {
-	int niov = 0, tx_size;
-	u32 padding = 0;
+	struct iscsi_nopin *hdr = (struct iscsi_nopin *)&cmd->pdu[0];
 	struct kvec *iov;
-	struct iscsi_nopin *hdr;
-
-	tx_size = ISCSI_HDR_LEN;
-	hdr			= (struct iscsi_nopin *) cmd->pdu;
-	memset(hdr, 0, ISCSI_HDR_LEN);
-	hdr->opcode		= ISCSI_OP_NOOP_IN;
-	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
-	hton24(hdr->dlength, cmd->buf_ptr_size);
-	put_unaligned_le64(0xFFFFFFFFFFFFFFFFULL, &hdr->lun);
-	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);
+	u32 padding = 0;
+	int niov = 0, tx_size;
 
-	iscsit_increment_maxcmdsn(cmd, conn->sess);
-	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
-	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
+	iscsit_build_nopin_rsp(cmd, conn, hdr, true);
 
+	tx_size = ISCSI_HDR_LEN;
 	iov = &cmd->iov_misc[0];
 	iov[niov].iov_base	= cmd->pdu;
 	iov[niov++].iov_len	= ISCSI_HDR_LEN;
@@ -2851,10 +2997,6 @@ static int iscsit_send_nopin_response(
 	cmd->iov_misc_count = niov;
 	cmd->tx_size = tx_size;
 
-	pr_debug("Sending NOPIN Response ITT: 0x%08x, TTT:"
-		" 0x%08x, StatSN: 0x%08x, Length %u\n", cmd->init_task_tag,
-		cmd->targ_xfer_tag, cmd->stat_sn, cmd->buf_ptr_size);
-
 	return 0;
 }
 
@@ -2939,8 +3081,8 @@ static int iscsit_send_r2t(
  *		connection recovery.
  */
 int iscsit_build_r2ts_for_cmd(
-	struct iscsi_cmd *cmd,
 	struct iscsi_conn *conn,
+	struct iscsi_cmd *cmd,
 	bool recovery)
 {
 	int first_r2t = 1;
@@ -3015,24 +3157,16 @@ int iscsit_build_r2ts_for_cmd(
 	return 0;
 }
 
-static int iscsit_send_status(
-	struct iscsi_cmd *cmd,
-	struct iscsi_conn *conn)
+void iscsit_build_rsp_pdu(struct iscsi_cmd *cmd, struct iscsi_conn *conn,
+			bool inc_stat_sn, struct iscsi_scsi_rsp *hdr)
 {
-	u8 iov_count = 0, recovery;
-	u32 padding = 0, tx_size = 0;
-	struct iscsi_scsi_rsp *hdr;
-	struct kvec *iov;
-
-	recovery = (cmd->i_state != ISTATE_SEND_STATUS);
-	if (!recovery)
+	if (inc_stat_sn)
 		cmd->stat_sn = conn->stat_sn++;
 
 	spin_lock_bh(&conn->sess->session_stats_lock);
 	conn->sess->rsp_pdus++;
 	spin_unlock_bh(&conn->sess->session_stats_lock);
 
-	hdr			= (struct iscsi_scsi_rsp *) cmd->pdu;
 	memset(hdr, 0, ISCSI_HDR_LEN);
 	hdr->opcode		= ISCSI_OP_SCSI_CMD_RSP;
 	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
@@ -3052,6 +3186,23 @@ static int iscsit_send_status(
 	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
 	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
 
+	pr_debug("Built SCSI Response, ITT: 0x%08x, StatSN: 0x%08x,"
+		" Response: 0x%02x, SAM Status: 0x%02x, CID: %hu\n",
+		cmd->init_task_tag, cmd->stat_sn, cmd->se_cmd.scsi_status,
+		cmd->se_cmd.scsi_status, conn->cid);
+}
+EXPORT_SYMBOL(iscsit_build_rsp_pdu);
+
+static int iscsit_send_response(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+{
+	struct iscsi_scsi_rsp *hdr = (struct iscsi_scsi_rsp *)&cmd->pdu[0];
+	struct kvec *iov;
+	u32 padding = 0, tx_size = 0;
+	int iov_count = 0;
+	bool inc_stat_sn = (cmd->i_state == ISTATE_SEND_STATUS);
+
+	iscsit_build_rsp_pdu(cmd, conn, inc_stat_sn, hdr);
+
 	iov = &cmd->iov_misc[0];
 	iov[iov_count].iov_base	= cmd->pdu;
 	iov[iov_count++].iov_len = ISCSI_HDR_LEN;
@@ -3106,7 +3257,7 @@ static int iscsit_send_status(
 		u32 *header_digest = (u32 *)&cmd->pdu[ISCSI_HDR_LEN];
 
 		iscsit_do_crypto_hash_buf(&conn->conn_tx_hash,
-				(unsigned char *)hdr, ISCSI_HDR_LEN,
+				(unsigned char *)cmd->pdu, ISCSI_HDR_LEN,
 				0, NULL, (u8 *)header_digest);
 
 		iov[0].iov_len += ISCSI_CRC_LEN;
@@ -3118,11 +3269,6 @@ static int iscsit_send_status(
 	cmd->iov_misc_count = iov_count;
 	cmd->tx_size = tx_size;
 
-	pr_debug("Built %sSCSI Response, ITT: 0x%08x, StatSN: 0x%08x,"
-		" Response: 0x%02x, SAM Status: 0x%02x, CID: %hu\n",
-		(!recovery) ? "" : "Recovery ", cmd->init_task_tag,
-		cmd->stat_sn, 0x00, cmd->se_cmd.scsi_status, conn->cid);
-
 	return 0;
 }
 
@@ -3145,16 +3291,12 @@ static u8 iscsit_convert_tcm_tmr_rsp(struct se_tmr_req *se_tmr)
 	}
 }
 
-static int iscsit_send_task_mgt_rsp(
-	struct iscsi_cmd *cmd,
-	struct iscsi_conn *conn)
+void
+iscsit_build_task_mgt_rsp(struct iscsi_cmd *cmd, struct iscsi_conn *conn,
+			  struct iscsi_tm_rsp *hdr)
 {
 	struct se_tmr_req *se_tmr = cmd->se_cmd.se_tmr_req;
-	struct iscsi_tm_rsp *hdr;
-	u32 tx_size = 0;
 
-	hdr			= (struct iscsi_tm_rsp *) cmd->pdu;
-	memset(hdr, 0, ISCSI_HDR_LEN);
 	hdr->opcode		= ISCSI_OP_SCSI_TMFUNC_RSP;
 	hdr->flags		= ISCSI_FLAG_CMD_FINAL;
 	hdr->response		= iscsit_convert_tcm_tmr_rsp(se_tmr);
@@ -3166,6 +3308,20 @@ static int iscsit_send_task_mgt_rsp(
 	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
 	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
 
+	pr_debug("Built Task Management Response ITT: 0x%08x,"
+		" StatSN: 0x%08x, Response: 0x%02x, CID: %hu\n",
+		cmd->init_task_tag, cmd->stat_sn, hdr->response, conn->cid);
+}
+EXPORT_SYMBOL(iscsit_build_task_mgt_rsp);
+
+static int
+iscsit_send_task_mgt_rsp(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+{
+	struct iscsi_tm_rsp *hdr = (struct iscsi_tm_rsp *)&cmd->pdu[0];
+	u32 tx_size = 0;
+
+	iscsit_build_task_mgt_rsp(cmd, conn, hdr);
+
 	cmd->iov_misc[0].iov_base	= cmd->pdu;
 	cmd->iov_misc[0].iov_len	= ISCSI_HDR_LEN;
 	tx_size += ISCSI_HDR_LEN;
@@ -3186,10 +3342,6 @@ static int iscsit_send_task_mgt_rsp(
 	cmd->iov_misc_count = 1;
 	cmd->tx_size = tx_size;
 
-	pr_debug("Built Task Management Response ITT: 0x%08x,"
-		" StatSN: 0x%08x, Response: 0x%02x, CID: %hu\n",
-		cmd->init_task_tag, cmd->stat_sn, hdr->response, conn->cid);
-
 	return 0;
 }
 
@@ -3385,6 +3537,22 @@ static int iscsit_send_text_rsp(
 	return 0;
 }
 
+void
+iscsit_build_reject(struct iscsi_cmd *cmd, struct iscsi_conn *conn,
+		    struct iscsi_reject *hdr)
+{
+	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);
+	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
+
+}
+EXPORT_SYMBOL(iscsit_build_reject);
+
 static int iscsit_send_reject(
 	struct iscsi_cmd *cmd,
 	struct iscsi_conn *conn)
@@ -3393,18 +3561,9 @@ static int iscsit_send_reject(
 	struct iscsi_reject *hdr;
 	struct kvec *iov;
 
-	hdr			= (struct iscsi_reject *) cmd->pdu;
-	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);
-	hdr->max_cmdsn	= cpu_to_be32(conn->sess->max_cmd_sn);
+	iscsit_build_reject(cmd, conn, (struct iscsi_reject *)&cmd->pdu[0]);
 
 	iov = &cmd->iov_misc[0];
-
 	iov[iov_count].iov_base = cmd->pdu;
 	iov[iov_count++].iov_len = ISCSI_HDR_LEN;
 	iov[iov_count].iov_base = cmd->buf_ptr;
@@ -3501,55 +3660,41 @@ static inline void iscsit_thread_check_cpumask(
 	set_cpus_allowed_ptr(p, conn->conn_cpumask);
 }
 
-static int handle_immediate_queue(struct iscsi_conn *conn)
+static int
+iscsit_immediate_queue(struct iscsi_conn *conn, struct iscsi_cmd *cmd, int state)
 {
-	struct iscsi_queue_req *qr;
-	struct iscsi_cmd *cmd;
-	u8 state;
 	int ret;
 
-	while ((qr = iscsit_get_cmd_from_immediate_queue(conn))) {
-		atomic_set(&conn->check_immediate_queue, 0);
-		cmd = qr->cmd;
-		state = qr->state;
-		kmem_cache_free(lio_qr_cache, qr);
-
-		switch (state) {
-		case ISTATE_SEND_R2T:
-			ret = iscsit_send_r2t(cmd, conn);
-			if (ret < 0)
-				goto err;
-			break;
-		case ISTATE_REMOVE:
-			if (cmd->data_direction == DMA_TO_DEVICE)
-				iscsit_stop_dataout_timer(cmd);
-
-			spin_lock_bh(&conn->cmd_lock);
-			list_del(&cmd->i_conn_node);
-			spin_unlock_bh(&conn->cmd_lock);
+	switch (state) {
+	case ISTATE_SEND_R2T:
+		ret = iscsit_send_r2t(cmd, conn);
+		if (ret < 0)
+			goto err;
+		break;
+	case ISTATE_REMOVE:
+		spin_lock_bh(&conn->cmd_lock);
+		list_del(&cmd->i_conn_node);
+		spin_unlock_bh(&conn->cmd_lock);
 
-			iscsit_free_cmd(cmd);
-			continue;
-		case ISTATE_SEND_NOPIN_WANT_RESPONSE:
-			iscsit_mod_nopin_response_timer(conn);
-			ret = iscsit_send_unsolicited_nopin(cmd,
-							    conn, 1);
-			if (ret < 0)
-				goto err;
-			break;
-		case ISTATE_SEND_NOPIN_NO_RESPONSE:
-			ret = iscsit_send_unsolicited_nopin(cmd,
-							    conn, 0);
-			if (ret < 0)
-				goto err;
-			break;
-		default:
-			pr_err("Unknown Opcode: 0x%02x ITT:"
-			       " 0x%08x, i_state: %d on CID: %hu\n",
-			       cmd->iscsi_opcode, cmd->init_task_tag, state,
-			       conn->cid);
+		iscsit_free_cmd(cmd);
+		break;
+	case ISTATE_SEND_NOPIN_WANT_RESPONSE:
+		iscsit_mod_nopin_response_timer(conn);
+		ret = iscsit_send_unsolicited_nopin(cmd, conn, 1);
+		if (ret < 0)
 			goto err;
-		}
+		break;
+	case ISTATE_SEND_NOPIN_NO_RESPONSE:
+		ret = iscsit_send_unsolicited_nopin(cmd, conn, 0);
+		if (ret < 0)
+			goto err;
+		break;
+	default:
+		pr_err("Unknown Opcode: 0x%02x ITT:"
+		       " 0x%08x, i_state: %d on CID: %hu\n",
+		       cmd->iscsi_opcode, cmd->init_task_tag, state,
+		       conn->cid);
+		goto err;
 	}
 
 	return 0;
@@ -3558,128 +3703,143 @@ err:
 	return -1;
 }
 
-static int handle_response_queue(struct iscsi_conn *conn)
+static int
+iscsit_handle_immediate_queue(struct iscsi_conn *conn)
 {
+	struct iscsit_transport *t = conn->conn_transport;
 	struct iscsi_queue_req *qr;
 	struct iscsi_cmd *cmd;
 	u8 state;
 	int ret;
 
-	while ((qr = iscsit_get_cmd_from_response_queue(conn))) {
+	while ((qr = iscsit_get_cmd_from_immediate_queue(conn))) {
+		atomic_set(&conn->check_immediate_queue, 0);
 		cmd = qr->cmd;
 		state = qr->state;
 		kmem_cache_free(lio_qr_cache, qr);
 
-check_rsp_state:
-		switch (state) {
-		case ISTATE_SEND_DATAIN:
-			ret = iscsit_send_data_in(cmd, conn);
-			if (ret < 0)
-				goto err;
-			else if (!ret)
-				/* more drs */
-				goto check_rsp_state;
-			else if (ret == 1) {
-				/* all done */
-				spin_lock_bh(&cmd->istate_lock);
-				cmd->i_state = ISTATE_SENT_STATUS;
-				spin_unlock_bh(&cmd->istate_lock);
-
-				if (atomic_read(&conn->check_immediate_queue))
-					return 1;
+		ret = t->iscsit_immediate_queue(conn, cmd, state);
+		if (ret < 0)
+			return ret;
+	}
 
-				continue;
-			} else if (ret == 2) {
-				/* Still must send status,
-				   SCF_TRANSPORT_TASK_SENSE was set */
-				spin_lock_bh(&cmd->istate_lock);
-				cmd->i_state = ISTATE_SEND_STATUS;
-				spin_unlock_bh(&cmd->istate_lock);
-				state = ISTATE_SEND_STATUS;
-				goto check_rsp_state;
-			}
+	return 0;
+}
 
-			break;
-		case ISTATE_SEND_STATUS:
-		case ISTATE_SEND_STATUS_RECOVERY:
-			ret = iscsit_send_status(cmd, conn);
-			break;
-		case ISTATE_SEND_LOGOUTRSP:
-			ret = iscsit_send_logout_response(cmd, conn);
-			break;
-		case ISTATE_SEND_ASYNCMSG:
-			ret = iscsit_send_conn_drop_async_message(
-				cmd, conn);
-			break;
-		case ISTATE_SEND_NOPIN:
-			ret = iscsit_send_nopin_response(cmd, conn);
-			break;
-		case ISTATE_SEND_REJECT:
-			ret = iscsit_send_reject(cmd, conn);
-			break;
-		case ISTATE_SEND_TASKMGTRSP:
-			ret = iscsit_send_task_mgt_rsp(cmd, conn);
-			if (ret != 0)
-				break;
-			ret = iscsit_tmr_post_handler(cmd, conn);
-			if (ret != 0)
-				iscsit_fall_back_to_erl0(conn->sess);
-			break;
-		case ISTATE_SEND_TEXTRSP:
-			ret = iscsit_send_text_rsp(cmd, conn);
-			break;
-		default:
-			pr_err("Unknown Opcode: 0x%02x ITT:"
-			       " 0x%08x, i_state: %d on CID: %hu\n",
-			       cmd->iscsi_opcode, cmd->init_task_tag,
-			       state, conn->cid);
-			goto err;
-		}
+static int
+iscsit_response_queue(struct iscsi_conn *conn, struct iscsi_cmd *cmd, int state)
+{
+	int ret;
+
+check_rsp_state:
+	switch (state) {
+	case ISTATE_SEND_DATAIN:
+		ret = iscsit_send_datain(cmd, conn);
 		if (ret < 0)
 			goto err;
+		else if (!ret)
+			/* more drs */
+			goto check_rsp_state;
+		else if (ret == 1) {
+			/* all done */
+			spin_lock_bh(&cmd->istate_lock);
+			cmd->i_state = ISTATE_SENT_STATUS;
+			spin_unlock_bh(&cmd->istate_lock);
 
-		if (iscsit_send_tx_data(cmd, conn, 1) < 0) {
-			iscsit_tx_thread_wait_for_tcp(conn);
-			iscsit_unmap_iovec(cmd);
-			goto err;
-		}
-		iscsit_unmap_iovec(cmd);
+			if (atomic_read(&conn->check_immediate_queue))
+				return 1;
 
-		switch (state) {
-		case ISTATE_SEND_LOGOUTRSP:
-			if (!iscsit_logout_post_handler(cmd, conn))
-				goto restart;
-			/* fall through */
-		case ISTATE_SEND_STATUS:
-		case ISTATE_SEND_ASYNCMSG:
-		case ISTATE_SEND_NOPIN:
-		case ISTATE_SEND_STATUS_RECOVERY:
-		case ISTATE_SEND_TEXTRSP:
-		case ISTATE_SEND_TASKMGTRSP:
+			return 0;
+		} else if (ret == 2) {
+			/* Still must send status,
+			   SCF_TRANSPORT_TASK_SENSE was set */
 			spin_lock_bh(&cmd->istate_lock);
-			cmd->i_state = ISTATE_SENT_STATUS;
+			cmd->i_state = ISTATE_SEND_STATUS;
 			spin_unlock_bh(&cmd->istate_lock);
+			state = ISTATE_SEND_STATUS;
+			goto check_rsp_state;
+		}
+
+		break;
+	case ISTATE_SEND_STATUS:
+	case ISTATE_SEND_STATUS_RECOVERY:
+		ret = iscsit_send_response(cmd, conn);
+		break;
+	case ISTATE_SEND_LOGOUTRSP:
+		ret = iscsit_send_logout(cmd, conn);
+		break;
+	case ISTATE_SEND_ASYNCMSG:
+		ret = iscsit_send_conn_drop_async_message(
+			cmd, conn);
+		break;
+	case ISTATE_SEND_NOPIN:
+		ret = iscsit_send_nopin(cmd, conn);
+		break;
+	case ISTATE_SEND_REJECT:
+		ret = iscsit_send_reject(cmd, conn);
+		break;
+	case ISTATE_SEND_TASKMGTRSP:
+		ret = iscsit_send_task_mgt_rsp(cmd, conn);
+		if (ret != 0)
 			break;
-		case ISTATE_SEND_REJECT:
-			if (cmd->cmd_flags & ICF_REJECT_FAIL_CONN) {
-				cmd->cmd_flags &= ~ICF_REJECT_FAIL_CONN;
-				complete(&cmd->reject_comp);
-				goto err;
-			}
+		ret = iscsit_tmr_post_handler(cmd, conn);
+		if (ret != 0)
+			iscsit_fall_back_to_erl0(conn->sess);
+		break;
+	case ISTATE_SEND_TEXTRSP:
+		ret = iscsit_send_text_rsp(cmd, conn);
+		break;
+	default:
+		pr_err("Unknown Opcode: 0x%02x ITT:"
+		       " 0x%08x, i_state: %d on CID: %hu\n",
+		       cmd->iscsi_opcode, cmd->init_task_tag,
+		       state, conn->cid);
+		goto err;
+	}
+	if (ret < 0)
+		goto err;
+
+	if (iscsit_send_tx_data(cmd, conn, 1) < 0) {
+		iscsit_tx_thread_wait_for_tcp(conn);
+		iscsit_unmap_iovec(cmd);
+		goto err;
+	}
+	iscsit_unmap_iovec(cmd);
+
+	switch (state) {
+	case ISTATE_SEND_LOGOUTRSP:
+		if (!iscsit_logout_post_handler(cmd, conn))
+			goto restart;
+		/* fall through */
+	case ISTATE_SEND_STATUS:
+	case ISTATE_SEND_ASYNCMSG:
+	case ISTATE_SEND_NOPIN:
+	case ISTATE_SEND_STATUS_RECOVERY:
+	case ISTATE_SEND_TEXTRSP:
+	case ISTATE_SEND_TASKMGTRSP:
+		spin_lock_bh(&cmd->istate_lock);
+		cmd->i_state = ISTATE_SENT_STATUS;
+		spin_unlock_bh(&cmd->istate_lock);
+		break;
+	case ISTATE_SEND_REJECT:
+		if (cmd->cmd_flags & ICF_REJECT_FAIL_CONN) {
+			cmd->cmd_flags &= ~ICF_REJECT_FAIL_CONN;
 			complete(&cmd->reject_comp);
-			break;
-		default:
-			pr_err("Unknown Opcode: 0x%02x ITT:"
-			       " 0x%08x, i_state: %d on CID: %hu\n",
-			       cmd->iscsi_opcode, cmd->init_task_tag,
-			       cmd->i_state, conn->cid);
 			goto err;
 		}
-
-		if (atomic_read(&conn->check_immediate_queue))
-			return 1;
+		complete(&cmd->reject_comp);
+		break;
+	default:
+		pr_err("Unknown Opcode: 0x%02x ITT:"
+		       " 0x%08x, i_state: %d on CID: %hu\n",
+		       cmd->iscsi_opcode, cmd->init_task_tag,
+		       cmd->i_state, conn->cid);
+		goto err;
 	}
 
+	if (atomic_read(&conn->check_immediate_queue))
+		return 1;
+
 	return 0;
 
 err:
@@ -3688,6 +3848,27 @@ restart:
 	return -EAGAIN;
 }
 
+static int iscsit_handle_response_queue(struct iscsi_conn *conn)
+{
+	struct iscsit_transport *t = conn->conn_transport;
+	struct iscsi_queue_req *qr;
+	struct iscsi_cmd *cmd;
+	u8 state;
+	int ret;
+
+	while ((qr = iscsit_get_cmd_from_response_queue(conn))) {
+		cmd = qr->cmd;
+		state = qr->state;
+		kmem_cache_free(lio_qr_cache, qr);
+
+		ret = t->iscsit_response_queue(conn, cmd, state);
+		if (ret == 1 || ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
 int iscsi_target_tx_thread(void *arg)
 {
 	int ret = 0;
@@ -3722,11 +3903,11 @@ restart:
 			goto transport_err;
 
 get_immediate:
-		ret = handle_immediate_queue(conn);
+		ret = iscsit_handle_immediate_queue(conn);
 		if (ret < 0)
 			goto transport_err;
 
-		ret = handle_response_queue(conn);
+		ret = iscsit_handle_response_queue(conn);
 		if (ret == 1)
 			goto get_immediate;
 		else if (ret == -EAGAIN)
@@ -3742,6 +3923,83 @@ out:
 	return 0;
 }
 
+static int iscsi_target_rx_opcode(struct iscsi_conn *conn, unsigned char *buf)
+{
+	struct iscsi_hdr *hdr = (struct iscsi_hdr *)buf;
+	struct iscsi_cmd *cmd;
+	int ret = 0;
+
+	switch (hdr->opcode & ISCSI_OPCODE_MASK) {
+	case ISCSI_OP_SCSI_CMD:
+		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		if (!cmd)
+			return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES,
+						1, buf, conn);
+
+		ret = iscsit_handle_scsi_cmd(conn, cmd, buf);
+		break;
+	case ISCSI_OP_SCSI_DATA_OUT:
+		ret = iscsit_handle_data_out(conn, buf);
+		break;
+	case ISCSI_OP_NOOP_OUT:
+		cmd = NULL;
+		if (hdr->ttt == cpu_to_be32(0xFFFFFFFF)) {
+			cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+			if (!cmd)
+				return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES,
+						1, buf, conn);
+		}
+		ret = iscsit_handle_nop_out(conn, cmd, buf);
+		break;
+	case ISCSI_OP_SCSI_TMFUNC:
+		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		if (!cmd)
+			return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES,
+						1, buf, conn);
+
+		ret = iscsit_handle_task_mgt_cmd(conn, cmd, buf);
+		break;
+	case ISCSI_OP_TEXT:
+		ret = iscsit_handle_text_cmd(conn, buf);
+		break;
+	case ISCSI_OP_LOGOUT:
+		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		if (!cmd)
+			return iscsit_add_reject(ISCSI_REASON_BOOKMARK_NO_RESOURCES,
+						1, buf, conn);
+
+		ret = iscsit_handle_logout_cmd(conn, cmd, buf);
+		if (ret > 0)
+			wait_for_completion_timeout(&conn->conn_logout_comp,
+					SECONDS_FOR_LOGOUT_COMP * HZ);
+		break;
+	case ISCSI_OP_SNACK:
+		ret = iscsit_handle_snack(conn, buf);
+		break;
+	default:
+		pr_err("Got unknown iSCSI OpCode: 0x%02x\n", hdr->opcode);
+		if (!conn->sess->sess_ops->ErrorRecoveryLevel) {
+			pr_err("Cannot recover from unknown"
+			" opcode while ERL=0, closing iSCSI connection.\n");
+			return -1;
+		}
+		if (!conn->conn_ops->OFMarker) {
+			pr_err("Unable to recover from unknown"
+			" opcode while OFMarker=No, closing iSCSI"
+				" connection.\n");
+			return -1;
+		}
+		if (iscsit_recover_from_unknown_opcode(conn) < 0) {
+			pr_err("Unable to recover from unknown"
+				" opcode, closing iSCSI connection.\n");
+			return -1;
+		}
+		break;
+	}
+
+	return ret;
+}
+
 int iscsi_target_rx_thread(void *arg)
 {
 	int ret;
@@ -3761,6 +4019,18 @@ restart:
 	if (!conn)
 		goto out;
 
+	if (conn->conn_transport->transport_type == ISCSI_INFINIBAND) {
+		struct completion comp;
+		int rc;
+
+		init_completion(&comp);
+		rc = wait_for_completion_interruptible(&comp);
+		if (rc < 0)
+			goto transport_err;
+
+		goto out;
+	}
+
 	while (!kthread_should_stop()) {
 		/*
 		 * Ensure that both TX and RX per connection kthreads
@@ -3832,62 +4102,9 @@ restart:
 			goto transport_err;
 		}
 
-		switch (opcode) {
-		case ISCSI_OP_SCSI_CMD:
-			if (iscsit_handle_scsi_cmd(conn, buffer) < 0)
-				goto transport_err;
-			break;
-		case ISCSI_OP_SCSI_DATA_OUT:
-			if (iscsit_handle_data_out(conn, buffer) < 0)
-				goto transport_err;
-			break;
-		case ISCSI_OP_NOOP_OUT:
-			if (iscsit_handle_nop_out(conn, buffer) < 0)
-				goto transport_err;
-			break;
-		case ISCSI_OP_SCSI_TMFUNC:
-			if (iscsit_handle_task_mgt_cmd(conn, buffer) < 0)
-				goto transport_err;
-			break;
-		case ISCSI_OP_TEXT:
-			if (iscsit_handle_text_cmd(conn, buffer) < 0)
-				goto transport_err;
-			break;
-		case ISCSI_OP_LOGOUT:
-			ret = iscsit_handle_logout_cmd(conn, buffer);
-			if (ret > 0) {
-				wait_for_completion_timeout(&conn->conn_logout_comp,
-						SECONDS_FOR_LOGOUT_COMP * HZ);
-				goto transport_err;
-			} else if (ret < 0)
-				goto transport_err;
-			break;
-		case ISCSI_OP_SNACK:
-			if (iscsit_handle_snack(conn, buffer) < 0)
-				goto transport_err;
-			break;
-		default:
-			pr_err("Got unknown iSCSI OpCode: 0x%02x\n",
-					opcode);
-			if (!conn->sess->sess_ops->ErrorRecoveryLevel) {
-				pr_err("Cannot recover from unknown"
-				" opcode while ERL=0, closing iSCSI connection"
-				".\n");
-				goto transport_err;
-			}
-			if (!conn->conn_ops->OFMarker) {
-				pr_err("Unable to recover from unknown"
-				" opcode while OFMarker=No, closing iSCSI"
-					" connection.\n");
-				goto transport_err;
-			}
-			if (iscsit_recover_from_unknown_opcode(conn) < 0) {
-				pr_err("Unable to recover from unknown"
-					" opcode, closing iSCSI connection.\n");
-				goto transport_err;
-			}
-			break;
-		}
+		ret = iscsi_target_rx_opcode(conn, buffer);
+		if (ret < 0)
+			goto transport_err;
 	}
 
 transport_err:
@@ -4053,6 +4270,12 @@ int iscsit_close_connection(
 
 	if (conn->sock)
 		sock_release(conn->sock);
+
+	if (conn->conn_transport->iscsit_free_conn)
+		conn->conn_transport->iscsit_free_conn(conn);
+
+	iscsit_put_transport(conn->conn_transport);
+
 	conn->thread_set = NULL;
 
 	pr_debug("Moving to TARG_CONN_STATE_FREE.\n");
@@ -4284,7 +4507,7 @@ static void iscsit_logout_post_handler_diffcid(
 /*
  *	Return of 0 causes the TX thread to restart.
  */
-static int iscsit_logout_post_handler(
+int iscsit_logout_post_handler(
 	struct iscsi_cmd *cmd,
 	struct iscsi_conn *conn)
 {
@@ -4342,6 +4565,7 @@ static int iscsit_logout_post_handler(
 	}
 	return ret;
 }
+EXPORT_SYMBOL(iscsit_logout_post_handler);
 
 void iscsit_fail_session(struct iscsi_session *sess)
 {
diff --git a/drivers/target/iscsi/iscsi_target.h b/drivers/target/iscsi/iscsi_target.h
index b1a1e6350707..a0050b2f294e 100644
--- a/drivers/target/iscsi/iscsi_target.h
+++ b/drivers/target/iscsi/iscsi_target.h
@@ -16,11 +16,12 @@ extern int iscsit_reset_np_thread(struct iscsi_np *, struct iscsi_tpg_np *,
 				struct iscsi_portal_group *);
 extern int iscsit_del_np(struct iscsi_np *);
 extern int iscsit_add_reject_from_cmd(u8, int, int, unsigned char *, struct iscsi_cmd *);
+extern void iscsit_set_unsoliticed_dataout(struct iscsi_cmd *);
 extern int iscsit_logout_closesession(struct iscsi_cmd *, struct iscsi_conn *);
 extern int iscsit_logout_closeconnection(struct iscsi_cmd *, struct iscsi_conn *);
 extern int iscsit_logout_removeconnforrecovery(struct iscsi_cmd *, struct iscsi_conn *);
 extern int iscsit_send_async_msg(struct iscsi_conn *, u16, u8, u8);
-extern int iscsit_build_r2ts_for_cmd(struct iscsi_cmd *, struct iscsi_conn *, bool recovery);
+extern int iscsit_build_r2ts_for_cmd(struct iscsi_conn *, struct iscsi_cmd *, bool recovery);
 extern void iscsit_thread_get_cpumask(struct iscsi_conn *);
 extern int iscsi_target_tx_thread(void *);
 extern int iscsi_target_rx_thread(void *);
diff --git a/drivers/target/iscsi/iscsi_target_auth.c b/drivers/target/iscsi/iscsi_target_auth.c
index a0fc7b9eea65..cee17543278c 100644
--- a/drivers/target/iscsi/iscsi_target_auth.c
+++ b/drivers/target/iscsi/iscsi_target_auth.c
@@ -49,32 +49,6 @@ static void chap_binaryhex_to_asciihex(char *dst, char *src, int src_len)
 	}
 }
 
-static void chap_set_random(char *data, int length)
-{
-	long r;
-	unsigned n;
-
-	while (length > 0) {
-		get_random_bytes(&r, sizeof(long));
-		r = r ^ (r >> 8);
-		r = r ^ (r >> 4);
-		n = r & 0x7;
-
-		get_random_bytes(&r, sizeof(long));
-		r = r ^ (r >> 8);
-		r = r ^ (r >> 5);
-		n = (n << 3) | (r & 0x7);
-
-		get_random_bytes(&r, sizeof(long));
-		r = r ^ (r >> 8);
-		r = r ^ (r >> 5);
-		n = (n << 2) | (r & 0x3);
-
-		*data++ = n;
-		length--;
-	}
-}
-
 static void chap_gen_challenge(
 	struct iscsi_conn *conn,
 	int caller,
@@ -86,7 +60,7 @@ static void chap_gen_challenge(
 
 	memset(challenge_asciihex, 0, CHAP_CHALLENGE_LENGTH * 2 + 1);
 
-	chap_set_random(chap->challenge, CHAP_CHALLENGE_LENGTH);
+	get_random_bytes(chap->challenge, CHAP_CHALLENGE_LENGTH);
 	chap_binaryhex_to_asciihex(challenge_asciihex, chap->challenge,
 				CHAP_CHALLENGE_LENGTH);
 	/*
diff --git a/drivers/target/iscsi/iscsi_target_configfs.c b/drivers/target/iscsi/iscsi_target_configfs.c
index 78d75c8567d0..13e9e715ad2e 100644
--- a/drivers/target/iscsi/iscsi_target_configfs.c
+++ b/drivers/target/iscsi/iscsi_target_configfs.c
@@ -27,6 +27,7 @@
 #include <target/target_core_fabric_configfs.h>
 #include <target/target_core_configfs.h>
 #include <target/configfs_macros.h>
+#include <target/iscsi/iscsi_transport.h>
 
 #include "iscsi_target_core.h"
 #include "iscsi_target_parameters.h"
@@ -124,8 +125,87 @@ out:
 
 TF_NP_BASE_ATTR(lio_target, sctp, S_IRUGO | S_IWUSR);
 
+static ssize_t lio_target_np_show_iser(
+	struct se_tpg_np *se_tpg_np,
+	char *page)
+{
+	struct iscsi_tpg_np *tpg_np = container_of(se_tpg_np,
+				struct iscsi_tpg_np, se_tpg_np);
+	struct iscsi_tpg_np *tpg_np_iser;
+	ssize_t rb;
+
+	tpg_np_iser = iscsit_tpg_locate_child_np(tpg_np, ISCSI_INFINIBAND);
+	if (tpg_np_iser)
+		rb = sprintf(page, "1\n");
+	else
+		rb = sprintf(page, "0\n");
+
+	return rb;
+}
+
+static ssize_t lio_target_np_store_iser(
+	struct se_tpg_np *se_tpg_np,
+	const char *page,
+	size_t count)
+{
+	struct iscsi_np *np;
+	struct iscsi_portal_group *tpg;
+	struct iscsi_tpg_np *tpg_np = container_of(se_tpg_np,
+				struct iscsi_tpg_np, se_tpg_np);
+	struct iscsi_tpg_np *tpg_np_iser = NULL;
+	char *endptr;
+	u32 op;
+	int rc;
+
+	op = simple_strtoul(page, &endptr, 0);
+	if ((op != 1) && (op != 0)) {
+		pr_err("Illegal value for tpg_enable: %u\n", op);
+		return -EINVAL;
+	}
+	np = tpg_np->tpg_np;
+	if (!np) {
+		pr_err("Unable to locate struct iscsi_np from"
+				" struct iscsi_tpg_np\n");
+		return -EINVAL;
+	}
+
+	tpg = tpg_np->tpg;
+	if (iscsit_get_tpg(tpg) < 0)
+		return -EINVAL;
+
+	if (op) {
+		int rc = request_module("ib_isert");
+		if (rc != 0)
+			pr_warn("Unable to request_module for ib_isert\n");
+
+		tpg_np_iser = iscsit_tpg_add_network_portal(tpg, &np->np_sockaddr,
+				np->np_ip, tpg_np, ISCSI_INFINIBAND);
+		if (!tpg_np_iser || IS_ERR(tpg_np_iser))
+			goto out;
+	} else {
+		tpg_np_iser = iscsit_tpg_locate_child_np(tpg_np, ISCSI_INFINIBAND);
+		if (!tpg_np_iser)
+			goto out;
+
+		rc = iscsit_tpg_del_network_portal(tpg, tpg_np_iser);
+		if (rc < 0)
+			goto out;
+	}
+
+	printk("lio_target_np_store_iser() done, op: %d\n", op);
+
+	iscsit_put_tpg(tpg);
+	return count;
+out:
+	iscsit_put_tpg(tpg);
+	return -EINVAL;
+}
+
+TF_NP_BASE_ATTR(lio_target, iser, S_IRUGO | S_IWUSR);
+
 static struct configfs_attribute *lio_target_portal_attrs[] = {
 	&lio_target_np_sctp.attr,
+	&lio_target_np_iser.attr,
 	NULL,
 };
 
@@ -1536,16 +1616,18 @@ static int lio_queue_data_in(struct se_cmd *se_cmd)
 	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
 
 	cmd->i_state = ISTATE_SEND_DATAIN;
-	iscsit_add_cmd_to_response_queue(cmd, cmd->conn, cmd->i_state);
+	cmd->conn->conn_transport->iscsit_queue_data_in(cmd->conn, cmd);
+
 	return 0;
 }
 
 static int lio_write_pending(struct se_cmd *se_cmd)
 {
 	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
+	struct iscsi_conn *conn = cmd->conn;
 
 	if (!cmd->immediate_data && !cmd->unsolicited_data)
-		return iscsit_build_r2ts_for_cmd(cmd, cmd->conn, false);
+		return conn->conn_transport->iscsit_get_dataout(conn, cmd, false);
 
 	return 0;
 }
@@ -1567,7 +1649,8 @@ static int lio_queue_status(struct se_cmd *se_cmd)
 	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
 
 	cmd->i_state = ISTATE_SEND_STATUS;
-	iscsit_add_cmd_to_response_queue(cmd, cmd->conn, cmd->i_state);
+	cmd->conn->conn_transport->iscsit_queue_status(cmd->conn, cmd);
+
 	return 0;
 }
 
@@ -1696,11 +1779,17 @@ static void lio_set_default_node_attributes(struct se_node_acl *se_acl)
 	iscsit_set_default_node_attribues(acl);
 }
 
+static int lio_check_stop_free(struct se_cmd *se_cmd)
+{
+	return target_put_sess_cmd(se_cmd->se_sess, se_cmd);
+}
+
 static void lio_release_cmd(struct se_cmd *se_cmd)
 {
 	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
 
-	iscsit_release_cmd(cmd);
+	pr_debug("Entering lio_release_cmd for se_cmd: %p\n", se_cmd);
+	cmd->release_cmd(cmd);
 }
 
 /* End functions for target_core_fabric_ops */
@@ -1740,6 +1829,7 @@ int iscsi_target_register_configfs(void)
 	fabric->tf_ops.tpg_alloc_fabric_acl = &lio_tpg_alloc_fabric_acl;
 	fabric->tf_ops.tpg_release_fabric_acl = &lio_tpg_release_fabric_acl;
 	fabric->tf_ops.tpg_get_inst_index = &lio_tpg_get_inst_index;
+	fabric->tf_ops.check_stop_free = &lio_check_stop_free,
 	fabric->tf_ops.release_cmd = &lio_release_cmd;
 	fabric->tf_ops.shutdown_session = &lio_tpg_shutdown_session;
 	fabric->tf_ops.close_session = &lio_tpg_close_session;
diff --git a/drivers/target/iscsi/iscsi_target_core.h b/drivers/target/iscsi/iscsi_target_core.h
index 7a333d28d9a2..60ec4b92be03 100644
--- a/drivers/target/iscsi/iscsi_target_core.h
+++ b/drivers/target/iscsi/iscsi_target_core.h
@@ -60,7 +60,7 @@
 
 #define ISCSI_IOV_DATA_BUFFER		5
 
-enum tpg_np_network_transport_table {
+enum iscsit_transport_type {
 	ISCSI_TCP				= 0,
 	ISCSI_SCTP_TCP				= 1,
 	ISCSI_SCTP_UDP				= 2,
@@ -244,6 +244,11 @@ struct iscsi_conn_ops {
 	u8	IFMarker;			/* [0,1] == [No,Yes] */
 	u32	OFMarkInt;			/* [1..65535] */
 	u32	IFMarkInt;			/* [1..65535] */
+	/*
+	 * iSER specific connection parameters
+	 */
+	u32	InitiatorRecvDataSegmentLength;	/* [512..2**24-1] */
+	u32	TargetRecvDataSegmentLength;	/* [512..2**24-1] */
 };
 
 struct iscsi_sess_ops {
@@ -265,6 +270,10 @@ struct iscsi_sess_ops {
 	u8	DataSequenceInOrder;		/* [0,1] == [No,Yes] */
 	u8	ErrorRecoveryLevel;		/* [0..2] */
 	u8	SessionType;			/* [0,1] == [Normal,Discovery]*/
+	/*
+	 * iSER specific session parameters
+	 */
+	u8	RDMAExtensions;			/* [0,1] == [No,Yes] */
 };
 
 struct iscsi_queue_req {
@@ -284,6 +293,7 @@ struct iscsi_data_count {
 };
 
 struct iscsi_param_list {
+	bool			iser;
 	struct list_head	param_list;
 	struct list_head	extra_response_list;
 };
@@ -475,6 +485,7 @@ struct iscsi_cmd {
 	u32			first_data_sg_off;
 	u32			kmapped_nents;
 	sense_reason_t		sense_reason;
+	void (*release_cmd)(struct iscsi_cmd *);
 }  ____cacheline_aligned;
 
 struct iscsi_tmr_req {
@@ -503,6 +514,7 @@ struct iscsi_conn {
 	u16			login_port;
 	u16			local_port;
 	int			net_size;
+	int			login_family;
 	u32			auth_id;
 	u32			conn_flags;
 	/* Used for iscsi_tx_login_rsp() */
@@ -562,9 +574,12 @@ struct iscsi_conn {
 	struct list_head	immed_queue_list;
 	struct list_head	response_queue_list;
 	struct iscsi_conn_ops	*conn_ops;
+	struct iscsi_login	*conn_login;
+	struct iscsit_transport *conn_transport;
 	struct iscsi_param_list	*param_list;
 	/* Used for per connection auth state machine */
 	void			*auth_protocol;
+	void			*context;
 	struct iscsi_login_thread_s *login_thread;
 	struct iscsi_portal_group *tpg;
 	/* Pointer to parent session */
@@ -663,6 +678,8 @@ struct iscsi_login {
 	u8 first_request;
 	u8 version_min;
 	u8 version_max;
+	u8 login_complete;
+	u8 login_failed;
 	char isid[6];
 	u32 cmd_sn;
 	itt_t init_task_tag;
@@ -670,10 +687,11 @@ struct iscsi_login {
 	u32 rsp_length;
 	u16 cid;
 	u16 tsih;
-	char *req;
-	char *rsp;
+	char req[ISCSI_HDR_LEN];
+	char rsp[ISCSI_HDR_LEN];
 	char *req_buf;
 	char *rsp_buf;
+	struct iscsi_conn *conn;
 } ____cacheline_aligned;
 
 struct iscsi_node_attrib {
@@ -754,6 +772,8 @@ struct iscsi_np {
 	struct task_struct	*np_thread;
 	struct timer_list	np_login_timer;
 	struct iscsi_portal_group *np_login_tpg;
+	void			*np_context;
+	struct iscsit_transport *np_transport;
 	struct list_head	np_list;
 } ____cacheline_aligned;
 
diff --git a/drivers/target/iscsi/iscsi_target_device.c b/drivers/target/iscsi/iscsi_target_device.c
index bcc409853a67..1b74033510a0 100644
--- a/drivers/target/iscsi/iscsi_target_device.c
+++ b/drivers/target/iscsi/iscsi_target_device.c
@@ -60,8 +60,13 @@ void iscsit_increment_maxcmdsn(struct iscsi_cmd *cmd, struct iscsi_session *sess
 
 	cmd->maxcmdsn_inc = 1;
 
-	mutex_lock(&sess->cmdsn_mutex);
+	if (!mutex_trylock(&sess->cmdsn_mutex)) {
+		sess->max_cmd_sn += 1;
+		pr_debug("Updated MaxCmdSN to 0x%08x\n", sess->max_cmd_sn);
+		return;
+	}
 	sess->max_cmd_sn += 1;
 	pr_debug("Updated MaxCmdSN to 0x%08x\n", sess->max_cmd_sn);
 	mutex_unlock(&sess->cmdsn_mutex);
 }
+EXPORT_SYMBOL(iscsit_increment_maxcmdsn);
diff --git a/drivers/target/iscsi/iscsi_target_erl1.c b/drivers/target/iscsi/iscsi_target_erl1.c
index 0b52a2371305..7816af6cdd12 100644
--- a/drivers/target/iscsi/iscsi_target_erl1.c
+++ b/drivers/target/iscsi/iscsi_target_erl1.c
@@ -22,6 +22,7 @@
 #include <scsi/iscsi_proto.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
+#include <target/iscsi/iscsi_transport.h>
 
 #include "iscsi_target_core.h"
 #include "iscsi_target_seq_pdu_list.h"
@@ -53,6 +54,9 @@ int iscsit_dump_data_payload(
 	u32 length, padding, offset = 0, size;
 	struct kvec iov;
 
+	if (conn->sess->sess_ops->RDMAExtensions)
+		return 0;
+
 	length = (buf_len > OFFLOAD_BUF_SIZE) ? OFFLOAD_BUF_SIZE : buf_len;
 
 	buf = kzalloc(length, GFP_ATOMIC);
@@ -919,6 +923,7 @@ int iscsit_execute_ooo_cmdsns(struct iscsi_session *sess)
 int iscsit_execute_cmd(struct iscsi_cmd *cmd, int ooo)
 {
 	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct iscsi_conn *conn = cmd->conn;
 	int lr = 0;
 
 	spin_lock_bh(&cmd->istate_lock);
@@ -981,7 +986,7 @@ int iscsit_execute_cmd(struct iscsi_cmd *cmd, int ooo)
 					return 0;
 
 				iscsit_set_dataout_sequence_values(cmd);
-				iscsit_build_r2ts_for_cmd(cmd, cmd->conn, false);
+				conn->conn_transport->iscsit_get_dataout(conn, cmd, false);
 			}
 			return 0;
 		}
@@ -999,10 +1004,7 @@ int iscsit_execute_cmd(struct iscsi_cmd *cmd, int ooo)
 			if (transport_check_aborted_status(se_cmd, 1) != 0)
 				return 0;
 
-			iscsit_set_dataout_sequence_values(cmd);
-			spin_lock_bh(&cmd->dataout_timeout_lock);
-			iscsit_start_dataout_timer(cmd, cmd->conn);
-			spin_unlock_bh(&cmd->dataout_timeout_lock);
+			iscsit_set_unsoliticed_dataout(cmd);
 		}
 		return transport_handle_cdb_direct(&cmd->se_cmd);
 
@@ -1290,3 +1292,4 @@ void iscsit_stop_dataout_timer(struct iscsi_cmd *cmd)
 			cmd->init_task_tag);
 	spin_unlock_bh(&cmd->dataout_timeout_lock);
 }
+EXPORT_SYMBOL(iscsit_stop_dataout_timer);
diff --git a/drivers/target/iscsi/iscsi_target_login.c b/drivers/target/iscsi/iscsi_target_login.c
index 2535d4d46c0e..bb5d5c5bce65 100644
--- a/drivers/target/iscsi/iscsi_target_login.c
+++ b/drivers/target/iscsi/iscsi_target_login.c
@@ -39,8 +39,39 @@
 #include "iscsi_target.h"
 #include "iscsi_target_parameters.h"
 
-static int iscsi_login_init_conn(struct iscsi_conn *conn)
+#include <target/iscsi/iscsi_transport.h>
+
+static struct iscsi_login *iscsi_login_init_conn(struct iscsi_conn *conn)
 {
+	struct iscsi_login *login;
+
+	login = kzalloc(sizeof(struct iscsi_login), GFP_KERNEL);
+	if (!login) {
+		pr_err("Unable to allocate memory for struct iscsi_login.\n");
+		return NULL;
+	}
+	login->conn = conn;
+	login->first_request = 1;
+
+	login->req_buf = kzalloc(MAX_KEY_VALUE_PAIRS, GFP_KERNEL);
+	if (!login->req_buf) {
+		pr_err("Unable to allocate memory for response buffer.\n");
+		goto out_login;
+	}
+
+	login->rsp_buf = kzalloc(MAX_KEY_VALUE_PAIRS, GFP_KERNEL);
+	if (!login->rsp_buf) {
+		pr_err("Unable to allocate memory for request buffer.\n");
+		goto out_req_buf;
+	}
+
+	conn->conn_ops = kzalloc(sizeof(struct iscsi_conn_ops), GFP_KERNEL);
+	if (!conn->conn_ops) {
+		pr_err("Unable to allocate memory for"
+			" struct iscsi_conn_ops.\n");
+		goto out_rsp_buf;
+	}
+
 	init_waitqueue_head(&conn->queues_wq);
 	INIT_LIST_HEAD(&conn->conn_list);
 	INIT_LIST_HEAD(&conn->conn_cmd_list);
@@ -62,10 +93,21 @@ static int iscsi_login_init_conn(struct iscsi_conn *conn)
 
 	if (!zalloc_cpumask_var(&conn->conn_cpumask, GFP_KERNEL)) {
 		pr_err("Unable to allocate conn->conn_cpumask\n");
-		return -ENOMEM;
+		goto out_conn_ops;
 	}
+	conn->conn_login = login;
 
-	return 0;
+	return login;
+
+out_conn_ops:
+	kfree(conn->conn_ops);
+out_rsp_buf:
+	kfree(login->rsp_buf);
+out_req_buf:
+	kfree(login->req_buf);
+out_login:
+	kfree(login);
+	return NULL;
 }
 
 /*
@@ -298,6 +340,7 @@ static int iscsi_login_zero_tsih_s2(
 	struct iscsi_node_attrib *na;
 	struct iscsi_session *sess = conn->sess;
 	unsigned char buf[32];
+	bool iser = false;
 
 	sess->tpg = conn->tpg;
 
@@ -319,7 +362,10 @@ static int iscsi_login_zero_tsih_s2(
 		return -1;
 	}
 
-	iscsi_set_keys_to_negotiate(0, conn->param_list);
+	if (conn->conn_transport->transport_type == ISCSI_INFINIBAND)
+		iser = true;
+
+	iscsi_set_keys_to_negotiate(conn->param_list, iser);
 
 	if (sess->sess_ops->SessionType)
 		return iscsi_set_keys_irrelevant_for_discovery(
@@ -357,6 +403,56 @@ static int iscsi_login_zero_tsih_s2(
 
 	if (iscsi_login_disable_FIM_keys(conn->param_list, conn) < 0)
 		return -1;
+	/*
+	 * Set RDMAExtensions=Yes by default for iSER enabled network portals
+	 */
+	if (iser) {
+		struct iscsi_param *param;
+		unsigned long mrdsl, off;
+		int rc;
+
+		sprintf(buf, "RDMAExtensions=Yes");
+		if (iscsi_change_param_value(buf, conn->param_list, 0) < 0) {
+			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
+				ISCSI_LOGIN_STATUS_NO_RESOURCES);
+			return -1;
+		}
+		/*
+		 * Make MaxRecvDataSegmentLength PAGE_SIZE aligned for
+		 * Immediate Data + Unsolicitied Data-OUT if necessary..
+		 */
+		param = iscsi_find_param_from_key("MaxRecvDataSegmentLength",
+						  conn->param_list);
+		if (!param) {
+			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
+				ISCSI_LOGIN_STATUS_NO_RESOURCES);
+			return -1;
+		}
+		rc = strict_strtoul(param->value, 0, &mrdsl);
+		if (rc < 0) {
+			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
+				ISCSI_LOGIN_STATUS_NO_RESOURCES);
+			return -1;
+		}
+		off = mrdsl % PAGE_SIZE;
+		if (!off)
+			return 0;
+
+		if (mrdsl < PAGE_SIZE)
+			mrdsl = PAGE_SIZE;
+		else
+			mrdsl -= off;
+
+		pr_warn("Aligning ISER MaxRecvDataSegmentLength: %lu down"
+			" to PAGE_SIZE\n", mrdsl);
+
+		sprintf(buf, "MaxRecvDataSegmentLength=%lu\n", mrdsl);
+		if (iscsi_change_param_value(buf, conn->param_list, 0) < 0) {
+			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
+				ISCSI_LOGIN_STATUS_NO_RESOURCES);
+			return -1;
+		}
+	}
 
 	return 0;
 }
@@ -436,6 +532,7 @@ static int iscsi_login_non_zero_tsih_s2(
 	struct se_portal_group *se_tpg = &tpg->tpg_se_tpg;
 	struct se_session *se_sess, *se_sess_tmp;
 	struct iscsi_login_req *pdu = (struct iscsi_login_req *)buf;
+	bool iser = false;
 
 	spin_lock_bh(&se_tpg->session_lock);
 	list_for_each_entry_safe(se_sess, se_sess_tmp, &se_tpg->tpg_sess_list,
@@ -485,7 +582,10 @@ static int iscsi_login_non_zero_tsih_s2(
 		return -1;
 	}
 
-	iscsi_set_keys_to_negotiate(0, conn->param_list);
+	if (conn->conn_transport->transport_type == ISCSI_INFINIBAND)
+		iser = true;
+
+	iscsi_set_keys_to_negotiate(conn->param_list, iser);
 	/*
 	 * Need to send TargetPortalGroupTag back in first login response
 	 * on any iSCSI connection where the Initiator provides TargetName.
@@ -574,6 +674,11 @@ int iscsi_login_post_auth_non_zero_tsih(
 static void iscsi_post_login_start_timers(struct iscsi_conn *conn)
 {
 	struct iscsi_session *sess = conn->sess;
+	/*
+	 * FIXME: Unsolicitied NopIN support for ISER
+	 */
+	if (conn->conn_transport->transport_type == ISCSI_INFINIBAND)
+		return;
 
 	if (!sess->sess_ops->SessionType)
 		iscsit_start_nopin_timer(conn);
@@ -632,6 +737,7 @@ static int iscsi_post_login_handler(
 		spin_unlock_bh(&sess->conn_lock);
 
 		iscsi_post_login_start_timers(conn);
+
 		iscsi_activate_thread_set(conn, ts);
 		/*
 		 * Determine CPU mask to ensure connection's RX and TX kthreads
@@ -761,11 +867,11 @@ static void iscsi_stop_login_thread_timer(struct iscsi_np *np)
 	spin_unlock_bh(&np->np_thread_lock);
 }
 
-int iscsi_target_setup_login_socket(
+int iscsit_setup_np(
 	struct iscsi_np *np,
 	struct __kernel_sockaddr_storage *sockaddr)
 {
-	struct socket *sock;
+	struct socket *sock = NULL;
 	int backlog = 5, ret, opt = 0, len;
 
 	switch (np->np_network_transport) {
@@ -781,15 +887,15 @@ int iscsi_target_setup_login_socket(
 		np->np_ip_proto = IPPROTO_SCTP;
 		np->np_sock_type = SOCK_SEQPACKET;
 		break;
-	case ISCSI_IWARP_TCP:
-	case ISCSI_IWARP_SCTP:
-	case ISCSI_INFINIBAND:
 	default:
 		pr_err("Unsupported network_transport: %d\n",
 				np->np_network_transport);
 		return -EINVAL;
 	}
 
+	np->np_ip_proto = IPPROTO_TCP;
+	np->np_sock_type = SOCK_STREAM;
+
 	ret = sock_create(sockaddr->ss_family, np->np_sock_type,
 			np->np_ip_proto, &sock);
 	if (ret < 0) {
@@ -853,7 +959,6 @@ int iscsi_target_setup_login_socket(
 	}
 
 	return 0;
-
 fail:
 	np->np_socket = NULL;
 	if (sock)
@@ -861,21 +966,169 @@ fail:
 	return ret;
 }
 
+int iscsi_target_setup_login_socket(
+	struct iscsi_np *np,
+	struct __kernel_sockaddr_storage *sockaddr)
+{
+	struct iscsit_transport *t;
+	int rc;
+
+	t = iscsit_get_transport(np->np_network_transport);
+	if (!t)
+		return -EINVAL;
+
+	rc = t->iscsit_setup_np(np, sockaddr);
+	if (rc < 0) {
+		iscsit_put_transport(t);
+		return rc;
+	}
+
+	np->np_transport = t;
+	printk("Set np->np_transport to %p -> %s\n", np->np_transport,
+				np->np_transport->name);
+	return 0;
+}
+
+int iscsit_accept_np(struct iscsi_np *np, struct iscsi_conn *conn)
+{
+	struct socket *new_sock, *sock = np->np_socket;
+	struct sockaddr_in sock_in;
+	struct sockaddr_in6 sock_in6;
+	int rc, err;
+
+	rc = kernel_accept(sock, &new_sock, 0);
+	if (rc < 0)
+		return rc;
+
+	conn->sock = new_sock;
+	conn->login_family = np->np_sockaddr.ss_family;
+	printk("iSCSI/TCP: Setup conn->sock from new_sock: %p\n", new_sock);
+
+	if (np->np_sockaddr.ss_family == AF_INET6) {
+		memset(&sock_in6, 0, sizeof(struct sockaddr_in6));
+
+		rc = conn->sock->ops->getname(conn->sock,
+				(struct sockaddr *)&sock_in6, &err, 1);
+		if (!rc) {
+			snprintf(conn->login_ip, sizeof(conn->login_ip), "%pI6c",
+				&sock_in6.sin6_addr.in6_u);
+			conn->login_port = ntohs(sock_in6.sin6_port);
+		}
+
+		rc = conn->sock->ops->getname(conn->sock,
+				(struct sockaddr *)&sock_in6, &err, 0);
+		if (!rc) {
+			snprintf(conn->local_ip, sizeof(conn->local_ip), "%pI6c",
+				&sock_in6.sin6_addr.in6_u);
+			conn->local_port = ntohs(sock_in6.sin6_port);
+		}
+	} else {
+		memset(&sock_in, 0, sizeof(struct sockaddr_in));
+
+		rc = conn->sock->ops->getname(conn->sock,
+				(struct sockaddr *)&sock_in, &err, 1);
+		if (!rc) {
+			sprintf(conn->login_ip, "%pI4",
+					&sock_in.sin_addr.s_addr);
+			conn->login_port = ntohs(sock_in.sin_port);
+		}
+
+		rc = conn->sock->ops->getname(conn->sock,
+				(struct sockaddr *)&sock_in, &err, 0);
+		if (!rc) {
+			sprintf(conn->local_ip, "%pI4",
+					&sock_in.sin_addr.s_addr);
+			conn->local_port = ntohs(sock_in.sin_port);
+		}
+	}
+
+	return 0;
+}
+
+int iscsit_get_login_rx(struct iscsi_conn *conn, struct iscsi_login *login)
+{
+	struct iscsi_login_req *login_req;
+	u32 padding = 0, payload_length;
+
+	if (iscsi_login_rx_data(conn, login->req, ISCSI_HDR_LEN) < 0)
+		return -1;
+
+	login_req = (struct iscsi_login_req *)login->req;
+	payload_length	= ntoh24(login_req->dlength);
+	padding = ((-payload_length) & 3);
+
+	pr_debug("Got Login Command, Flags 0x%02x, ITT: 0x%08x,"
+		" CmdSN: 0x%08x, ExpStatSN: 0x%08x, CID: %hu, Length: %u\n",
+		login_req->flags, login_req->itt, login_req->cmdsn,
+		login_req->exp_statsn, login_req->cid, payload_length);
+	/*
+	 * Setup the initial iscsi_login values from the leading
+	 * login request PDU.
+	 */
+	if (login->first_request) {
+		login_req = (struct iscsi_login_req *)login->req;
+		login->leading_connection = (!login_req->tsih) ? 1 : 0;
+		login->current_stage	= ISCSI_LOGIN_CURRENT_STAGE(login_req->flags);
+		login->version_min	= login_req->min_version;
+		login->version_max	= login_req->max_version;
+		memcpy(login->isid, login_req->isid, 6);
+		login->cmd_sn		= be32_to_cpu(login_req->cmdsn);
+		login->init_task_tag	= login_req->itt;
+		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_check_login_request(conn, login) < 0)
+		return -1;
+
+	memset(login->req_buf, 0, MAX_KEY_VALUE_PAIRS);
+	if (iscsi_login_rx_data(conn, login->req_buf,
+				payload_length + padding) < 0)
+		return -1;
+
+	return 0;
+}
+
+int iscsit_put_login_tx(struct iscsi_conn *conn, struct iscsi_login *login,
+			u32 length)
+{
+	if (iscsi_login_tx_data(conn, login->rsp, login->rsp_buf, length) < 0)
+		return -1;
+
+	return 0;
+}
+
+static int
+iscsit_conn_set_transport(struct iscsi_conn *conn, struct iscsit_transport *t)
+{
+	int rc;
+
+	if (!t->owner) {
+		conn->conn_transport = t;
+		return 0;
+	}
+
+	rc = try_module_get(t->owner);
+	if (!rc) {
+		pr_err("try_module_get() failed for %s\n", t->name);
+		return -EINVAL;
+	}
+
+	conn->conn_transport = t;
+	return 0;
+}
+
 static int __iscsi_target_login_thread(struct iscsi_np *np)
 {
-	u8 buffer[ISCSI_HDR_LEN], iscsi_opcode, zero_tsih = 0;
-	int err, ret = 0, stop;
+	u8 *buffer, zero_tsih = 0;
+	int ret = 0, rc, stop;
 	struct iscsi_conn *conn = NULL;
 	struct iscsi_login *login;
 	struct iscsi_portal_group *tpg = NULL;
-	struct socket *new_sock, *sock;
-	struct kvec iov;
 	struct iscsi_login_req *pdu;
-	struct sockaddr_in sock_in;
-	struct sockaddr_in6 sock_in6;
 
 	flush_signals(current);
-	sock = np->np_socket;
 
 	spin_lock_bh(&np->np_thread_lock);
 	if (np->np_thread_state == ISCSI_NP_THREAD_RESET) {
@@ -886,75 +1139,76 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
 	}
 	spin_unlock_bh(&np->np_thread_lock);
 
-	if (kernel_accept(sock, &new_sock, 0) < 0) {
-		spin_lock_bh(&np->np_thread_lock);
-		if (np->np_thread_state == ISCSI_NP_THREAD_RESET) {
-			spin_unlock_bh(&np->np_thread_lock);
-			complete(&np->np_restart_comp);
-			/* Get another socket */
-			return 1;
-		}
-		spin_unlock_bh(&np->np_thread_lock);
-		goto out;
-	}
-	iscsi_start_login_thread_timer(np);
-
 	conn = kzalloc(sizeof(struct iscsi_conn), GFP_KERNEL);
 	if (!conn) {
 		pr_err("Could not allocate memory for"
 			" new connection\n");
-		sock_release(new_sock);
 		/* Get another socket */
 		return 1;
 	}
-
 	pr_debug("Moving to TARG_CONN_STATE_FREE.\n");
 	conn->conn_state = TARG_CONN_STATE_FREE;
-	conn->sock = new_sock;
 
-	pr_debug("Moving to TARG_CONN_STATE_XPT_UP.\n");
-	conn->conn_state = TARG_CONN_STATE_XPT_UP;
+	if (iscsit_conn_set_transport(conn, np->np_transport) < 0) {
+		kfree(conn);
+		return 1;
+	}
 
-	/*
-	 * Allocate conn->conn_ops early as a failure calling
-	 * iscsit_tx_login_rsp() below will call tx_data().
-	 */
-	conn->conn_ops = kzalloc(sizeof(struct iscsi_conn_ops), GFP_KERNEL);
-	if (!conn->conn_ops) {
-		pr_err("Unable to allocate memory for"
-			" struct iscsi_conn_ops.\n");
-		goto new_sess_out;
+	rc = np->np_transport->iscsit_accept_np(np, conn);
+	if (rc == -ENOSYS) {
+		complete(&np->np_restart_comp);
+		iscsit_put_transport(conn->conn_transport);
+		kfree(conn);
+		conn = NULL;
+		goto exit;
+	} else if (rc < 0) {
+		spin_lock_bh(&np->np_thread_lock);
+		if (np->np_thread_state == ISCSI_NP_THREAD_RESET) {
+			spin_unlock_bh(&np->np_thread_lock);
+			complete(&np->np_restart_comp);
+			if (ret == -ENODEV) {
+				iscsit_put_transport(conn->conn_transport);
+				kfree(conn);
+				conn = NULL;
+				goto out;
+			}
+			/* Get another socket */
+			return 1;
+		}
+		spin_unlock_bh(&np->np_thread_lock);
+		iscsit_put_transport(conn->conn_transport);
+		kfree(conn);
+		conn = NULL;
+		goto out;
 	}
 	/*
 	 * Perform the remaining iSCSI connection initialization items..
 	 */
-	if (iscsi_login_init_conn(conn) < 0)
-		goto new_sess_out;
-
-	memset(buffer, 0, ISCSI_HDR_LEN);
-	memset(&iov, 0, sizeof(struct kvec));
-	iov.iov_base	= buffer;
-	iov.iov_len	= ISCSI_HDR_LEN;
-
-	if (rx_data(conn, &iov, 1, ISCSI_HDR_LEN) <= 0) {
-		pr_err("rx_data() returned an error.\n");
+	login = iscsi_login_init_conn(conn);
+	if (!login) {
 		goto new_sess_out;
 	}
 
-	iscsi_opcode = (buffer[0] & ISCSI_OPCODE_MASK);
-	if (!(iscsi_opcode & ISCSI_OP_LOGIN)) {
-		pr_err("First opcode is not login request,"
-			" failing login request.\n");
-		goto new_sess_out;
-	}
+	iscsi_start_login_thread_timer(np);
 
-	pdu			= (struct iscsi_login_req *) buffer;
+	pr_debug("Moving to TARG_CONN_STATE_XPT_UP.\n");
+	conn->conn_state = TARG_CONN_STATE_XPT_UP;
+	/*
+	 * This will process the first login request + payload..
+	 */
+	rc = np->np_transport->iscsit_get_login_rx(conn, login);
+	if (rc == 1)
+		return 1;
+	else if (rc < 0)
+		goto new_sess_out;
 
+	buffer = &login->req[0];
+	pdu = (struct iscsi_login_req *)buffer;
 	/*
 	 * Used by iscsit_tx_login_rsp() for Login Resonses PDUs
 	 * when Status-Class != 0.
 	*/
-	conn->login_itt		= pdu->itt;
+	conn->login_itt	= pdu->itt;
 
 	spin_lock_bh(&np->np_thread_lock);
 	if (np->np_thread_state != ISCSI_NP_THREAD_ACTIVE) {
@@ -967,61 +1221,11 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
 	}
 	spin_unlock_bh(&np->np_thread_lock);
 
-	if (np->np_sockaddr.ss_family == AF_INET6) {
-		memset(&sock_in6, 0, sizeof(struct sockaddr_in6));
-
-		if (conn->sock->ops->getname(conn->sock,
-				(struct sockaddr *)&sock_in6, &err, 1) < 0) {
-			pr_err("sock_ops->getname() failed.\n");
-			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-					ISCSI_LOGIN_STATUS_TARGET_ERROR);
-			goto new_sess_out;
-		}
-		snprintf(conn->login_ip, sizeof(conn->login_ip), "%pI6c",
-				&sock_in6.sin6_addr.in6_u);
-		conn->login_port = ntohs(sock_in6.sin6_port);
-
-		if (conn->sock->ops->getname(conn->sock,
-				(struct sockaddr *)&sock_in6, &err, 0) < 0) {
-			pr_err("sock_ops->getname() failed.\n");
-			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-					ISCSI_LOGIN_STATUS_TARGET_ERROR);
-			goto new_sess_out;
-		}
-		snprintf(conn->local_ip, sizeof(conn->local_ip), "%pI6c",
-				&sock_in6.sin6_addr.in6_u);
-		conn->local_port = ntohs(sock_in6.sin6_port);
-
-	} else {
-		memset(&sock_in, 0, sizeof(struct sockaddr_in));
-
-		if (conn->sock->ops->getname(conn->sock,
-				(struct sockaddr *)&sock_in, &err, 1) < 0) {
-			pr_err("sock_ops->getname() failed.\n");
-			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-					ISCSI_LOGIN_STATUS_TARGET_ERROR);
-			goto new_sess_out;
-		}
-		sprintf(conn->login_ip, "%pI4", &sock_in.sin_addr.s_addr);
-		conn->login_port = ntohs(sock_in.sin_port);
-
-		if (conn->sock->ops->getname(conn->sock,
-				(struct sockaddr *)&sock_in, &err, 0) < 0) {
-			pr_err("sock_ops->getname() failed.\n");
-			iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-					ISCSI_LOGIN_STATUS_TARGET_ERROR);
-			goto new_sess_out;
-		}
-		sprintf(conn->local_ip, "%pI4", &sock_in.sin_addr.s_addr);
-		conn->local_port = ntohs(sock_in.sin_port);
-	}
-
 	conn->network_transport = np->np_network_transport;
 
 	pr_debug("Received iSCSI login request from %s on %s Network"
-			" Portal %s:%hu\n", conn->login_ip,
-		(conn->network_transport == ISCSI_TCP) ? "TCP" : "SCTP",
-			conn->local_ip, conn->local_port);
+		" Portal %s:%hu\n", conn->login_ip, np->np_transport->name,
+		conn->local_ip, conn->local_port);
 
 	pr_debug("Moving to TARG_CONN_STATE_IN_LOGIN.\n");
 	conn->conn_state	= TARG_CONN_STATE_IN_LOGIN;
@@ -1050,13 +1254,17 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
 		if (iscsi_login_non_zero_tsih_s1(conn, buffer) < 0)
 			goto new_sess_out;
 	}
-
 	/*
-	 * This will process the first login request, and call
-	 * iscsi_target_locate_portal(), and return a valid struct iscsi_login.
+	 * SessionType: Discovery
+	 *
+	 * 	Locates Default Portal
+	 *
+	 * SessionType: Normal
+	 *
+	 * 	Locates Target Portal from NP -> Target IQN
 	 */
-	login = iscsi_target_init_negotiation(np, conn, buffer);
-	if (!login) {
+	rc = iscsi_target_locate_portal(np, conn, login);
+	if (rc < 0) {
 		tpg = conn->tpg;
 		goto new_sess_out;
 	}
@@ -1068,15 +1276,11 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
 	}
 
 	if (zero_tsih) {
-		if (iscsi_login_zero_tsih_s2(conn) < 0) {
-			iscsi_target_nego_release(login, conn);
+		if (iscsi_login_zero_tsih_s2(conn) < 0)
 			goto new_sess_out;
-		}
 	} else {
-		if (iscsi_login_non_zero_tsih_s2(conn, buffer) < 0) {
-			iscsi_target_nego_release(login, conn);
+		if (iscsi_login_non_zero_tsih_s2(conn, buffer) < 0)
 			goto old_sess_out;
-		}
 	}
 
 	if (iscsi_target_start_negotiation(login, conn) < 0)
@@ -1153,8 +1357,18 @@ old_sess_out:
 		iscsi_release_param_list(conn->param_list);
 		conn->param_list = NULL;
 	}
-	if (conn->sock)
+	iscsi_target_nego_release(conn);
+
+	if (conn->sock) {
 		sock_release(conn->sock);
+		conn->sock = NULL;
+	}
+
+	if (conn->conn_transport->iscsit_free_conn)
+		conn->conn_transport->iscsit_free_conn(conn);
+
+	iscsit_put_transport(conn->conn_transport);
+
 	kfree(conn);
 
 	if (tpg) {
@@ -1172,11 +1386,13 @@ out:
 	/* Wait for another socket.. */
 	if (!stop)
 		return 1;
-
+exit:
 	iscsi_stop_login_thread_timer(np);
 	spin_lock_bh(&np->np_thread_lock);
 	np->np_thread_state = ISCSI_NP_THREAD_EXIT;
+	np->np_thread = NULL;
 	spin_unlock_bh(&np->np_thread_lock);
+
 	return 0;
 }
 
diff --git a/drivers/target/iscsi/iscsi_target_login.h b/drivers/target/iscsi/iscsi_target_login.h
index 091dcae2532b..63efd2878451 100644
--- a/drivers/target/iscsi/iscsi_target_login.h
+++ b/drivers/target/iscsi/iscsi_target_login.h
@@ -4,8 +4,14 @@
 extern int iscsi_login_setup_crypto(struct iscsi_conn *);
 extern int iscsi_check_for_session_reinstatement(struct iscsi_conn *);
 extern int iscsi_login_post_auth_non_zero_tsih(struct iscsi_conn *, u16, u32);
+extern int iscsit_setup_np(struct iscsi_np *,
+				struct __kernel_sockaddr_storage *);
 extern int iscsi_target_setup_login_socket(struct iscsi_np *,
 				struct __kernel_sockaddr_storage *);
+extern int iscsit_accept_np(struct iscsi_np *, struct iscsi_conn *);
+extern int iscsit_get_login_rx(struct iscsi_conn *, struct iscsi_login *);
+extern int iscsit_put_login_tx(struct iscsi_conn *, struct iscsi_login *, u32);
+extern void iscsit_free_conn(struct iscsi_np *, struct iscsi_conn *);
 extern int iscsi_target_login_thread(void *);
 extern int iscsi_login_disable_FIM_keys(struct iscsi_param_list *, struct iscsi_conn *);
 
diff --git a/drivers/target/iscsi/iscsi_target_nego.c b/drivers/target/iscsi/iscsi_target_nego.c
index 9d902aefe01a..7ad912060e21 100644
--- a/drivers/target/iscsi/iscsi_target_nego.c
+++ b/drivers/target/iscsi/iscsi_target_nego.c
@@ -22,6 +22,7 @@
 #include <scsi/iscsi_proto.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
+#include <target/iscsi/iscsi_transport.h>
 
 #include "iscsi_target_core.h"
 #include "iscsi_target_parameters.h"
@@ -169,7 +170,7 @@ static void iscsi_remove_failed_auth_entry(struct iscsi_conn *conn)
 	kfree(conn->auth_protocol);
 }
 
-static int iscsi_target_check_login_request(
+int iscsi_target_check_login_request(
 	struct iscsi_conn *conn,
 	struct iscsi_login *login)
 {
@@ -200,8 +201,8 @@ static int iscsi_target_check_login_request(
 		return -1;
 	}
 
-	req_csg = (login_req->flags & ISCSI_FLAG_LOGIN_CURRENT_STAGE_MASK) >> 2;
-	req_nsg = (login_req->flags & ISCSI_FLAG_LOGIN_NEXT_STAGE_MASK);
+	req_csg = ISCSI_LOGIN_CURRENT_STAGE(login_req->flags);
+	req_nsg = ISCSI_LOGIN_NEXT_STAGE(login_req->flags);
 
 	if (req_csg != login->current_stage) {
 		pr_err("Initiator unexpectedly changed login stage"
@@ -352,11 +353,8 @@ static int iscsi_target_do_tx_login_io(struct iscsi_conn *conn, struct iscsi_log
 
 	padding = ((-login->rsp_length) & 3);
 
-	if (iscsi_login_tx_data(
-			conn,
-			login->rsp,
-			login->rsp_buf,
-			login->rsp_length + padding) < 0)
+	if (conn->conn_transport->iscsit_put_login_tx(conn, login,
+					login->rsp_length + padding) < 0)
 		return -1;
 
 	login->rsp_length		= 0;
@@ -368,72 +366,12 @@ static int iscsi_target_do_tx_login_io(struct iscsi_conn *conn, struct iscsi_log
 	return 0;
 }
 
-static int iscsi_target_do_rx_login_io(struct iscsi_conn *conn, struct iscsi_login *login)
-{
-	u32 padding = 0, payload_length;
-	struct iscsi_login_req *login_req;
-
-	if (iscsi_login_rx_data(conn, login->req, ISCSI_HDR_LEN) < 0)
-		return -1;
-
-	login_req = (struct iscsi_login_req *) login->req;
-	payload_length			= ntoh24(login_req->dlength);
-
-	pr_debug("Got Login Command, Flags 0x%02x, ITT: 0x%08x,"
-		" CmdSN: 0x%08x, ExpStatSN: 0x%08x, CID: %hu, Length: %u\n",
-		 login_req->flags, login_req->itt, login_req->cmdsn,
-		 login_req->exp_statsn, login_req->cid, payload_length);
-
-	if (iscsi_target_check_login_request(conn, login) < 0)
-		return -1;
-
-	padding = ((-payload_length) & 3);
-	memset(login->req_buf, 0, MAX_KEY_VALUE_PAIRS);
-
-	if (iscsi_login_rx_data(
-			conn,
-			login->req_buf,
-			payload_length + padding) < 0)
-		return -1;
-
-	return 0;
-}
-
 static int iscsi_target_do_login_io(struct iscsi_conn *conn, struct iscsi_login *login)
 {
 	if (iscsi_target_do_tx_login_io(conn, login) < 0)
 		return -1;
 
-	if (iscsi_target_do_rx_login_io(conn, login) < 0)
-		return -1;
-
-	return 0;
-}
-
-static int iscsi_target_get_initial_payload(
-	struct iscsi_conn *conn,
-	struct iscsi_login *login)
-{
-	u32 padding = 0, payload_length;
-	struct iscsi_login_req *login_req;
-
-	login_req = (struct iscsi_login_req *) login->req;
-	payload_length = ntoh24(login_req->dlength);
-
-	pr_debug("Got Login Command, Flags 0x%02x, ITT: 0x%08x,"
-		" CmdSN: 0x%08x, ExpStatSN: 0x%08x, Length: %u\n",
-		login_req->flags, login_req->itt, login_req->cmdsn,
-		login_req->exp_statsn, payload_length);
-
-	if (iscsi_target_check_login_request(conn, login) < 0)
-		return -1;
-
-	padding = ((-payload_length) & 3);
-
-	if (iscsi_login_rx_data(
-			conn,
-			login->req_buf,
-			payload_length + padding) < 0)
+	if (conn->conn_transport->iscsit_get_login_rx(conn, login) < 0)
 		return -1;
 
 	return 0;
@@ -681,9 +619,9 @@ static int iscsi_target_do_login(struct iscsi_conn *conn, struct iscsi_login *lo
 			return -1;
 		}
 
-		switch ((login_req->flags & ISCSI_FLAG_LOGIN_CURRENT_STAGE_MASK) >> 2) {
+		switch (ISCSI_LOGIN_CURRENT_STAGE(login_req->flags)) {
 		case 0:
-			login_rsp->flags |= (0 & ISCSI_FLAG_LOGIN_CURRENT_STAGE_MASK);
+			login_rsp->flags &= ~ISCSI_FLAG_LOGIN_CURRENT_STAGE_MASK;
 			if (iscsi_target_handle_csg_zero(conn, login) < 0)
 				return -1;
 			break;
@@ -693,6 +631,7 @@ static int iscsi_target_do_login(struct iscsi_conn *conn, struct iscsi_login *lo
 				return -1;
 			if (login_rsp->flags & ISCSI_FLAG_LOGIN_TRANSIT) {
 				login->tsih = conn->sess->tsih;
+				login->login_complete = 1;
 				if (iscsi_target_do_tx_login_io(conn,
 						login) < 0)
 					return -1;
@@ -702,8 +641,7 @@ static int iscsi_target_do_login(struct iscsi_conn *conn, struct iscsi_login *lo
 		default:
 			pr_err("Illegal CSG: %d received from"
 				" Initiator, protocol error.\n",
-				(login_req->flags & ISCSI_FLAG_LOGIN_CURRENT_STAGE_MASK)
-				>> 2);
+				ISCSI_LOGIN_CURRENT_STAGE(login_req->flags));
 			break;
 		}
 
@@ -737,7 +675,7 @@ static void iscsi_initiatorname_tolower(
 /*
  * Processes the first Login Request..
  */
-static int iscsi_target_locate_portal(
+int iscsi_target_locate_portal(
 	struct iscsi_np *np,
 	struct iscsi_conn *conn,
 	struct iscsi_login *login)
@@ -753,22 +691,6 @@ static int iscsi_target_locate_portal(
 	login_req = (struct iscsi_login_req *) login->req;
 	payload_length = ntoh24(login_req->dlength);
 
-	login->first_request	= 1;
-	login->leading_connection = (!login_req->tsih) ? 1 : 0;
-	login->current_stage	=
-		(login_req->flags & ISCSI_FLAG_LOGIN_CURRENT_STAGE_MASK) >> 2;
-	login->version_min	= login_req->min_version;
-	login->version_max	= login_req->max_version;
-	memcpy(login->isid, login_req->isid, 6);
-	login->cmd_sn		= be32_to_cpu(login_req->cmdsn);
-	login->init_task_tag	= login_req->itt;
-	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;
-
 	tmpbuf = kzalloc(payload_length + 1, GFP_KERNEL);
 	if (!tmpbuf) {
 		pr_err("Unable to allocate memory for tmpbuf.\n");
@@ -800,6 +722,8 @@ static int iscsi_target_locate_portal(
 		start += strlen(key) + strlen(value) + 2;
 	}
 
+	printk("i_buf: %s, s_buf: %s, t_buf: %s\n", i_buf, s_buf, t_buf);
+
 	/*
 	 * See 5.3.  Login Phase.
 	 */
@@ -958,100 +882,30 @@ out:
 	return ret;
 }
 
-struct iscsi_login *iscsi_target_init_negotiation(
-	struct iscsi_np *np,
-	struct iscsi_conn *conn,
-	char *login_pdu)
-{
-	struct iscsi_login *login;
-
-	login = kzalloc(sizeof(struct iscsi_login), GFP_KERNEL);
-	if (!login) {
-		pr_err("Unable to allocate memory for struct iscsi_login.\n");
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
-		return NULL;
-	}
-
-	login->req = kmemdup(login_pdu, ISCSI_HDR_LEN, GFP_KERNEL);
-	if (!login->req) {
-		pr_err("Unable to allocate memory for Login Request.\n");
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
-		goto out;
-	}
-
-	login->req_buf = kzalloc(MAX_KEY_VALUE_PAIRS, GFP_KERNEL);
-	if (!login->req_buf) {
-		pr_err("Unable to allocate memory for response buffer.\n");
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
-		goto out;
-	}
-	/*
-	 * SessionType: Discovery
-	 *
-	 *	Locates Default Portal
-	 *
-	 * SessionType: Normal
-	 *
-	 *	Locates Target Portal from NP -> Target IQN
-	 */
-	if (iscsi_target_locate_portal(np, conn, login) < 0) {
-		goto out;
-	}
-
-	return login;
-out:
-	kfree(login->req);
-	kfree(login->req_buf);
-	kfree(login);
-
-	return NULL;
-}
-
 int iscsi_target_start_negotiation(
 	struct iscsi_login *login,
 	struct iscsi_conn *conn)
 {
-	int ret = -1;
-
-	login->rsp = kzalloc(ISCSI_HDR_LEN, GFP_KERNEL);
-	if (!login->rsp) {
-		pr_err("Unable to allocate memory for"
-				" Login Response.\n");
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
-		ret = -1;
-		goto out;
-	}
-
-	login->rsp_buf = kzalloc(MAX_KEY_VALUE_PAIRS, GFP_KERNEL);
-	if (!login->rsp_buf) {
-		pr_err("Unable to allocate memory for"
-			" request buffer.\n");
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
-		ret = -1;
-		goto out;
-	}
+	int ret;
 
 	ret = iscsi_target_do_login(conn, login);
-out:
 	if (ret != 0)
 		iscsi_remove_failed_auth_entry(conn);
 
-	iscsi_target_nego_release(login, conn);
+	iscsi_target_nego_release(conn);
 	return ret;
 }
 
-void iscsi_target_nego_release(
-	struct iscsi_login *login,
-	struct iscsi_conn *conn)
+void iscsi_target_nego_release(struct iscsi_conn *conn)
 {
-	kfree(login->req);
-	kfree(login->rsp);
+	struct iscsi_login *login = conn->conn_login;
+
+	if (!login)
+		return;
+
 	kfree(login->req_buf);
 	kfree(login->rsp_buf);
 	kfree(login);
+
+	conn->conn_login = NULL;
 }
diff --git a/drivers/target/iscsi/iscsi_target_nego.h b/drivers/target/iscsi/iscsi_target_nego.h
index 92e133a5158f..f021cbd330e5 100644
--- a/drivers/target/iscsi/iscsi_target_nego.h
+++ b/drivers/target/iscsi/iscsi_target_nego.h
@@ -7,11 +7,14 @@
 extern void convert_null_to_semi(char *, int);
 extern int extract_param(const char *, const char *, unsigned int, char *,
 		unsigned char *);
-extern struct iscsi_login *iscsi_target_init_negotiation(
-		struct iscsi_np *, struct iscsi_conn *, char *);
+extern int iscsi_target_check_login_request(struct iscsi_conn *,
+		struct iscsi_login *);
+extern int iscsi_target_get_initial_payload(struct iscsi_conn *,
+		struct iscsi_login *);
+extern int iscsi_target_locate_portal(struct iscsi_np *, struct iscsi_conn *,
+		struct iscsi_login *);
 extern int iscsi_target_start_negotiation(
 		struct iscsi_login *, struct iscsi_conn *);
-extern void iscsi_target_nego_release(
-		struct iscsi_login *, struct iscsi_conn *);
+extern void iscsi_target_nego_release(struct iscsi_conn *);
 
 #endif /* ISCSI_TARGET_NEGO_H */
diff --git a/drivers/target/iscsi/iscsi_target_parameters.c b/drivers/target/iscsi/iscsi_target_parameters.c
index ca2be406f141..f690be9e5293 100644
--- a/drivers/target/iscsi/iscsi_target_parameters.c
+++ b/drivers/target/iscsi/iscsi_target_parameters.c
@@ -59,7 +59,7 @@ int iscsi_login_tx_data(
 	char *text_buf,
 	int text_length)
 {
-	int length, tx_sent;
+	int length, tx_sent, iov_cnt = 1;
 	struct kvec iov[2];
 
 	length = (ISCSI_HDR_LEN + text_length);
@@ -67,8 +67,12 @@ int iscsi_login_tx_data(
 	memset(&iov[0], 0, 2 * sizeof(struct kvec));
 	iov[0].iov_len		= ISCSI_HDR_LEN;
 	iov[0].iov_base		= pdu_buf;
-	iov[1].iov_len		= text_length;
-	iov[1].iov_base		= text_buf;
+
+	if (text_buf && text_length) {
+		iov[1].iov_len	= text_length;
+		iov[1].iov_base	= text_buf;
+		iov_cnt++;
+	}
 
 	/*
 	 * Initial Marker-less Interval.
@@ -77,7 +81,7 @@ int iscsi_login_tx_data(
 	 */
 	conn->if_marker += length;
 
-	tx_sent = tx_data(conn, &iov[0], 2, length);
+	tx_sent = tx_data(conn, &iov[0], iov_cnt, length);
 	if (tx_sent != length) {
 		pr_err("tx_data returned %d, expecting %d.\n",
 				tx_sent, length);
@@ -429,6 +433,28 @@ int iscsi_create_default_params(struct iscsi_param_list **param_list_ptr)
 			TYPERANGE_MARKINT, USE_INITIAL_ONLY);
 	if (!param)
 		goto out;
+	/*
+	 * Extra parameters for ISER from RFC-5046
+	 */
+	param = iscsi_set_default_param(pl, RDMAEXTENTIONS, INITIAL_RDMAEXTENTIONS,
+			PHASE_OPERATIONAL, SCOPE_SESSION_WIDE, SENDER_BOTH,
+			TYPERANGE_BOOL_AND, USE_LEADING_ONLY);
+	if (!param)
+		goto out;
+
+	param = iscsi_set_default_param(pl, INITIATORRECVDATASEGMENTLENGTH,
+			INITIAL_INITIATORRECVDATASEGMENTLENGTH,
+			PHASE_OPERATIONAL, SCOPE_CONNECTION_ONLY, SENDER_BOTH,
+			TYPERANGE_512_TO_16777215, USE_ALL);
+	if (!param)
+		goto out;
+
+	param = iscsi_set_default_param(pl, TARGETRECVDATASEGMENTLENGTH,
+			INITIAL_TARGETRECVDATASEGMENTLENGTH,
+			PHASE_OPERATIONAL, SCOPE_CONNECTION_ONLY, SENDER_BOTH,
+			TYPERANGE_512_TO_16777215, USE_ALL);
+	if (!param)
+		goto out;
 
 	*param_list_ptr = pl;
 	return 0;
@@ -438,19 +464,23 @@ out:
 }
 
 int iscsi_set_keys_to_negotiate(
-	int sessiontype,
-	struct iscsi_param_list *param_list)
+	struct iscsi_param_list *param_list,
+	bool iser)
 {
 	struct iscsi_param *param;
 
+	param_list->iser = iser;
+
 	list_for_each_entry(param, &param_list->param_list, p_list) {
 		param->state = 0;
 		if (!strcmp(param->name, AUTHMETHOD)) {
 			SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, HEADERDIGEST)) {
-			SET_PSTATE_NEGOTIATE(param);
+			if (iser == false)
+				SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, DATADIGEST)) {
-			SET_PSTATE_NEGOTIATE(param);
+			if (iser == false)
+				SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, MAXCONNECTIONS)) {
 			SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, TARGETNAME)) {
@@ -469,7 +499,8 @@ int iscsi_set_keys_to_negotiate(
 		} else if (!strcmp(param->name, IMMEDIATEDATA)) {
 			SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, MAXRECVDATASEGMENTLENGTH)) {
-			SET_PSTATE_NEGOTIATE(param);
+			if (iser == false)
+				SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, MAXXMITDATASEGMENTLENGTH)) {
 			continue;
 		} else if (!strcmp(param->name, MAXBURSTLENGTH)) {
@@ -498,6 +529,15 @@ int iscsi_set_keys_to_negotiate(
 			SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, OFMARKINT)) {
 			SET_PSTATE_NEGOTIATE(param);
+		} else if (!strcmp(param->name, RDMAEXTENTIONS)) {
+			if (iser == true)
+				SET_PSTATE_NEGOTIATE(param);
+		} else if (!strcmp(param->name, INITIATORRECVDATASEGMENTLENGTH)) {
+			if (iser == true)
+				SET_PSTATE_NEGOTIATE(param);
+		} else if (!strcmp(param->name, TARGETRECVDATASEGMENTLENGTH)) {
+			if (iser == true)
+				SET_PSTATE_NEGOTIATE(param);
 		}
 	}
 
@@ -540,6 +580,12 @@ int iscsi_set_keys_irrelevant_for_discovery(
 			param->state &= ~PSTATE_NEGOTIATE;
 		else if (!strcmp(param->name, OFMARKINT))
 			param->state &= ~PSTATE_NEGOTIATE;
+		else if (!strcmp(param->name, RDMAEXTENTIONS))
+			param->state &= ~PSTATE_NEGOTIATE;
+		else if (!strcmp(param->name, INITIATORRECVDATASEGMENTLENGTH))
+			param->state &= ~PSTATE_NEGOTIATE;
+		else if (!strcmp(param->name, TARGETRECVDATASEGMENTLENGTH))
+			param->state &= ~PSTATE_NEGOTIATE;
 	}
 
 	return 0;
@@ -1755,6 +1801,9 @@ void iscsi_set_connection_parameters(
 		 * this key is not sent over the wire.
 		 */
 		if (!strcmp(param->name, MAXXMITDATASEGMENTLENGTH)) {
+			if (param_list->iser == true)
+				continue;
+
 			ops->MaxXmitDataSegmentLength =
 				simple_strtoul(param->value, &tmpptr, 0);
 			pr_debug("MaxXmitDataSegmentLength:     %s\n",
@@ -1800,6 +1849,22 @@ void iscsi_set_connection_parameters(
 				simple_strtoul(param->value, &tmpptr, 0);
 			pr_debug("IFMarkInt:                    %s\n",
 				param->value);
+		} else if (!strcmp(param->name, INITIATORRECVDATASEGMENTLENGTH)) {
+			ops->InitiatorRecvDataSegmentLength =
+				simple_strtoul(param->value, &tmpptr, 0);
+			pr_debug("InitiatorRecvDataSegmentLength: %s\n",
+				param->value);
+			ops->MaxRecvDataSegmentLength =
+					ops->InitiatorRecvDataSegmentLength;
+			pr_debug("Set MRDSL from InitiatorRecvDataSegmentLength\n");
+		} else if (!strcmp(param->name, TARGETRECVDATASEGMENTLENGTH)) {
+			ops->TargetRecvDataSegmentLength =
+				simple_strtoul(param->value, &tmpptr, 0);
+			pr_debug("TargetRecvDataSegmentLength:  %s\n",
+				param->value);
+			ops->MaxXmitDataSegmentLength =
+					ops->TargetRecvDataSegmentLength;
+			pr_debug("Set MXDSL from TargetRecvDataSegmentLength\n");
 		}
 	}
 	pr_debug("----------------------------------------------------"
@@ -1912,6 +1977,10 @@ void iscsi_set_session_parameters(
 			ops->SessionType = !strcmp(param->value, DISCOVERY);
 			pr_debug("SessionType:                  %s\n",
 				param->value);
+		} else if (!strcmp(param->name, RDMAEXTENTIONS)) {
+			ops->RDMAExtensions = !strcmp(param->value, YES);
+			pr_debug("RDMAExtensions:               %s\n",
+				param->value);
 		}
 	}
 	pr_debug("----------------------------------------------------"
diff --git a/drivers/target/iscsi/iscsi_target_parameters.h b/drivers/target/iscsi/iscsi_target_parameters.h
index 1e1b7504a76b..f31b9c4b83f2 100644
--- a/drivers/target/iscsi/iscsi_target_parameters.h
+++ b/drivers/target/iscsi/iscsi_target_parameters.h
@@ -27,7 +27,7 @@ extern void iscsi_dump_conn_ops(struct iscsi_conn_ops *);
 extern void iscsi_dump_sess_ops(struct iscsi_sess_ops *);
 extern void iscsi_print_params(struct iscsi_param_list *);
 extern int iscsi_create_default_params(struct iscsi_param_list **);
-extern int iscsi_set_keys_to_negotiate(int, struct iscsi_param_list *);
+extern int iscsi_set_keys_to_negotiate(struct iscsi_param_list *, bool);
 extern int iscsi_set_keys_irrelevant_for_discovery(struct iscsi_param_list *);
 extern int iscsi_copy_param_list(struct iscsi_param_list **,
 			struct iscsi_param_list *, int);
@@ -89,6 +89,13 @@ extern void iscsi_set_session_parameters(struct iscsi_sess_ops *,
 #define X_EXTENSIONKEY_CISCO_OLD	"X-com.cisco.iscsi.draft"
 
 /*
+ * Parameter names of iSCSI Extentions for RDMA (iSER).  See RFC-5046
+ */
+#define RDMAEXTENTIONS			"RDMAExtensions"
+#define INITIATORRECVDATASEGMENTLENGTH	"InitiatorRecvDataSegmentLength"
+#define TARGETRECVDATASEGMENTLENGTH	"TargetRecvDataSegmentLength"
+
+/*
  * For AuthMethod.
  */
 #define KRB5				"KRB5"
@@ -133,6 +140,13 @@ extern void iscsi_set_session_parameters(struct iscsi_sess_ops *,
 #define INITIAL_OFMARKINT			"2048~65535"
 
 /*
+ * Initial values for iSER parameters following RFC-5046 Section 6
+ */
+#define INITIAL_RDMAEXTENTIONS			NO
+#define INITIAL_INITIATORRECVDATASEGMENTLENGTH	"262144"
+#define INITIAL_TARGETRECVDATASEGMENTLENGTH	"8192"
+
+/*
  * For [Header,Data]Digests.
  */
 #define CRC32C				"CRC32C"
diff --git a/drivers/target/iscsi/iscsi_target_tmr.c b/drivers/target/iscsi/iscsi_target_tmr.c
index 9d4417aae921..b997e5da47d3 100644
--- a/drivers/target/iscsi/iscsi_target_tmr.c
+++ b/drivers/target/iscsi/iscsi_target_tmr.c
@@ -23,6 +23,7 @@
 #include <scsi/iscsi_proto.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
+#include <target/iscsi/iscsi_transport.h>
 
 #include "iscsi_target_core.h"
 #include "iscsi_target_seq_pdu_list.h"
@@ -301,7 +302,7 @@ static int iscsit_task_reassign_complete_write(
 	/*
 	 * iscsit_build_r2ts_for_cmd() can handle the rest from here.
 	 */
-	return iscsit_build_r2ts_for_cmd(cmd, conn, true);
+	return conn->conn_transport->iscsit_get_dataout(conn, cmd, true);
 }
 
 static int iscsit_task_reassign_complete_read(
@@ -471,6 +472,7 @@ int iscsit_tmr_post_handler(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
 
 	return 0;
 }
+EXPORT_SYMBOL(iscsit_tmr_post_handler);
 
 /*
  *	Nothing to do here, but leave it for good measure. :-)
diff --git a/drivers/target/iscsi/iscsi_target_tpg.c b/drivers/target/iscsi/iscsi_target_tpg.c
index ee8f8c66248d..439260b7d87f 100644
--- a/drivers/target/iscsi/iscsi_target_tpg.c
+++ b/drivers/target/iscsi/iscsi_target_tpg.c
@@ -31,6 +31,8 @@
 #include "iscsi_target.h"
 #include "iscsi_target_parameters.h"
 
+#include <target/iscsi/iscsi_transport.h>
+
 struct iscsi_portal_group *iscsit_alloc_portal_group(struct iscsi_tiqn *tiqn, u16 tpgt)
 {
 	struct iscsi_portal_group *tpg;
@@ -508,7 +510,7 @@ struct iscsi_tpg_np *iscsit_tpg_add_network_portal(
 
 	pr_debug("CORE[%s] - Added Network Portal: %s:%hu,%hu on %s\n",
 		tpg->tpg_tiqn->tiqn, np->np_ip, np->np_port, tpg->tpgt,
-		(np->np_network_transport == ISCSI_TCP) ? "TCP" : "SCTP");
+		np->np_transport->name);
 
 	return tpg_np;
 }
@@ -522,7 +524,7 @@ static int iscsit_tpg_release_np(
 
 	pr_debug("CORE[%s] - Removed Network Portal: %s:%hu,%hu on %s\n",
 		tpg->tpg_tiqn->tiqn, np->np_ip, np->np_port, tpg->tpgt,
-		(np->np_network_transport == ISCSI_TCP) ? "TCP" : "SCTP");
+		np->np_transport->name);
 
 	tpg_np->tpg_np = NULL;
 	tpg_np->tpg = NULL;
diff --git a/drivers/target/iscsi/iscsi_target_transport.c b/drivers/target/iscsi/iscsi_target_transport.c
new file mode 100644
index 000000000000..882728fac30c
--- /dev/null
+++ b/drivers/target/iscsi/iscsi_target_transport.c
@@ -0,0 +1,55 @@
+#include <linux/spinlock.h>
+#include <linux/list.h>
+#include <target/iscsi/iscsi_transport.h>
+
+static LIST_HEAD(g_transport_list);
+static DEFINE_MUTEX(transport_mutex);
+
+struct iscsit_transport *iscsit_get_transport(int type)
+{
+	struct iscsit_transport *t;
+
+	mutex_lock(&transport_mutex);
+	list_for_each_entry(t, &g_transport_list, t_node) {
+		if (t->transport_type == type) {
+			if (t->owner && !try_module_get(t->owner)) {
+				t = NULL;
+			}
+			mutex_unlock(&transport_mutex);
+			return t;
+		}
+	}
+	mutex_unlock(&transport_mutex);
+
+	return NULL;
+}
+
+void iscsit_put_transport(struct iscsit_transport *t)
+{
+	if (t->owner)
+		module_put(t->owner);
+}
+
+int iscsit_register_transport(struct iscsit_transport *t)
+{
+	INIT_LIST_HEAD(&t->t_node);
+
+	mutex_lock(&transport_mutex);
+	list_add_tail(&t->t_node, &g_transport_list);
+	mutex_unlock(&transport_mutex);
+
+	pr_debug("Registered iSCSI transport: %s\n", t->name);
+
+	return 0;
+}
+EXPORT_SYMBOL(iscsit_register_transport);
+
+void iscsit_unregister_transport(struct iscsit_transport *t)
+{
+	mutex_lock(&transport_mutex);
+	list_del(&t->t_node);
+	mutex_unlock(&transport_mutex);
+
+	pr_debug("Unregistered iSCSI transport: %s\n", t->name);
+}
+EXPORT_SYMBOL(iscsit_unregister_transport);
diff --git a/drivers/target/iscsi/iscsi_target_util.c b/drivers/target/iscsi/iscsi_target_util.c
index 7ce350578c82..2cc6c9a3ffb8 100644
--- a/drivers/target/iscsi/iscsi_target_util.c
+++ b/drivers/target/iscsi/iscsi_target_util.c
@@ -24,6 +24,7 @@
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
 #include <target/target_core_configfs.h>
+#include <target/iscsi/iscsi_transport.h>
 
 #include "iscsi_target_core.h"
 #include "iscsi_target_parameters.h"
@@ -148,6 +149,18 @@ void iscsit_free_r2ts_from_list(struct iscsi_cmd *cmd)
 	spin_unlock_bh(&cmd->r2t_lock);
 }
 
+struct iscsi_cmd *iscsit_alloc_cmd(struct iscsi_conn *conn, gfp_t gfp_mask)
+{
+	struct iscsi_cmd *cmd;
+
+	cmd = kmem_cache_zalloc(lio_cmd_cache, gfp_mask);
+	if (!cmd)
+		return NULL;
+
+	cmd->release_cmd = &iscsit_release_cmd;
+	return cmd;
+}
+
 /*
  * May be called from software interrupt (timer) context for allocating
  * iSCSI NopINs.
@@ -156,13 +169,12 @@ struct iscsi_cmd *iscsit_allocate_cmd(struct iscsi_conn *conn, gfp_t gfp_mask)
 {
 	struct iscsi_cmd *cmd;
 
-	cmd = kmem_cache_zalloc(lio_cmd_cache, gfp_mask);
+	cmd = conn->conn_transport->iscsit_alloc_cmd(conn, gfp_mask);
 	if (!cmd) {
 		pr_err("Unable to allocate memory for struct iscsi_cmd.\n");
 		return NULL;
 	}
-
-	cmd->conn	= conn;
+	cmd->conn = conn;
 	INIT_LIST_HEAD(&cmd->i_conn_node);
 	INIT_LIST_HEAD(&cmd->datain_list);
 	INIT_LIST_HEAD(&cmd->cmd_r2t_list);
@@ -175,6 +187,7 @@ struct iscsi_cmd *iscsit_allocate_cmd(struct iscsi_conn *conn, gfp_t gfp_mask)
 
 	return cmd;
 }
+EXPORT_SYMBOL(iscsit_allocate_cmd);
 
 struct iscsi_seq *iscsit_get_seq_holder_for_datain(
 	struct iscsi_cmd *cmd,
@@ -304,6 +317,7 @@ int iscsit_sequence_cmd(
 
 	return ret;
 }
+EXPORT_SYMBOL(iscsit_sequence_cmd);
 
 int iscsit_check_unsolicited_dataout(struct iscsi_cmd *cmd, unsigned char *buf)
 {
@@ -689,6 +703,11 @@ void iscsit_free_cmd(struct iscsi_cmd *cmd)
 	 */
 	switch (cmd->iscsi_opcode) {
 	case ISCSI_OP_SCSI_CMD:
+		if (cmd->data_direction == DMA_TO_DEVICE)
+			iscsit_stop_dataout_timer(cmd);
+		/*
+		 * Fallthrough
+		 */
 	case ISCSI_OP_SCSI_TMFUNC:
 		transport_generic_free_cmd(&cmd->se_cmd, 1);
 		break;
@@ -704,7 +723,7 @@ void iscsit_free_cmd(struct iscsi_cmd *cmd)
 		}
 		/* Fall-through */
 	default:
-		iscsit_release_cmd(cmd);
+		cmd->release_cmd(cmd);
 		break;
 	}
 }
@@ -1226,34 +1245,19 @@ send_datacrc:
  */
 int iscsit_tx_login_rsp(struct iscsi_conn *conn, u8 status_class, u8 status_detail)
 {
-	u8 iscsi_hdr[ISCSI_HDR_LEN];
-	int err;
-	struct kvec iov;
 	struct iscsi_login_rsp *hdr;
+	struct iscsi_login *login = conn->conn_login;
 
+	login->login_failed = 1;
 	iscsit_collect_login_stats(conn, status_class, status_detail);
 
-	memset(&iov, 0, sizeof(struct kvec));
-	memset(&iscsi_hdr, 0x0, ISCSI_HDR_LEN);
-
-	hdr	= (struct iscsi_login_rsp *)&iscsi_hdr;
+	hdr	= (struct iscsi_login_rsp *)&login->rsp[0];
 	hdr->opcode		= ISCSI_OP_LOGIN_RSP;
 	hdr->status_class	= status_class;
 	hdr->status_detail	= status_detail;
 	hdr->itt		= conn->login_itt;
 
-	iov.iov_base		= &iscsi_hdr;
-	iov.iov_len		= ISCSI_HDR_LEN;
-
-	PRINT_BUFF(iscsi_hdr, ISCSI_HDR_LEN);
-
-	err = tx_data(conn, &iov, 1, ISCSI_HDR_LEN);
-	if (err != ISCSI_HDR_LEN) {
-		pr_err("tx_data returned less than expected\n");
-		return -1;
-	}
-
-	return 0;
+	return conn->conn_transport->iscsit_put_login_tx(conn, login, 0);
 }
 
 void iscsit_print_session_params(struct iscsi_session *sess)
@@ -1432,7 +1436,8 @@ void iscsit_collect_login_stats(
 		strcpy(ls->last_intr_fail_name,
 		       (intrname ? intrname->value : "Unknown"));
 
-		ls->last_intr_fail_ip_family = conn->sock->sk->sk_family;
+		ls->last_intr_fail_ip_family = conn->login_family;
+
 		snprintf(ls->last_intr_fail_ip_addr, IPV6_ADDRESS_SPACE,
 				"%s", conn->login_ip);
 		ls->last_fail_time = get_jiffies_64();
diff --git a/drivers/target/iscsi/iscsi_target_util.h b/drivers/target/iscsi/iscsi_target_util.h
index 894d0f837924..4f8e01a47081 100644
--- a/drivers/target/iscsi/iscsi_target_util.h
+++ b/drivers/target/iscsi/iscsi_target_util.h
@@ -8,6 +8,7 @@ extern struct iscsi_r2t *iscsit_get_r2t_for_eos(struct iscsi_cmd *, u32, u32);
 extern struct iscsi_r2t *iscsit_get_r2t_from_list(struct iscsi_cmd *);
 extern void iscsit_free_r2t(struct iscsi_r2t *, struct iscsi_cmd *);
 extern void iscsit_free_r2ts_from_list(struct iscsi_cmd *);
+extern struct iscsi_cmd *iscsit_alloc_cmd(struct iscsi_conn *, gfp_t);
 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 *);
diff --git a/drivers/target/loopback/tcm_loop.c b/drivers/target/loopback/tcm_loop.c
index 2d444b1ccd33..7c908141cc8a 100644
--- a/drivers/target/loopback/tcm_loop.c
+++ b/drivers/target/loopback/tcm_loop.c
@@ -79,11 +79,10 @@ static void tcm_loop_release_cmd(struct se_cmd *se_cmd)
 	kmem_cache_free(tcm_loop_cmd_cache, tl_cmd);
 }
 
-static int tcm_loop_proc_info(struct Scsi_Host *host, char *buffer,
-				char **start, off_t offset,
-				int length, int inout)
+static int tcm_loop_show_info(struct seq_file *m, struct Scsi_Host *host)
 {
-	return sprintf(buffer, "tcm_loop_proc_info()\n");
+	seq_printf(m, "tcm_loop_proc_info()\n");
+	return 0;
 }
 
 static int tcm_loop_driver_probe(struct device *);
@@ -336,7 +335,7 @@ static int tcm_loop_slave_configure(struct scsi_device *sd)
 }
 
 static struct scsi_host_template tcm_loop_driver_template = {
-	.proc_info		= tcm_loop_proc_info,
+	.show_info		= tcm_loop_show_info,
 	.proc_name		= "tcm_loopback",
 	.name			= "TCM_Loopback",
 	.queuecommand		= tcm_loop_queuecommand,
diff --git a/drivers/target/target_core_file.c b/drivers/target/target_core_file.c
index 17a6acbc3ab0..58ed683e04ae 100644
--- a/drivers/target/target_core_file.c
+++ b/drivers/target/target_core_file.c
@@ -30,8 +30,10 @@
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/module.h>
+#include <linux/falloc.h>
 #include <scsi/scsi.h>
 #include <scsi/scsi_host.h>
+#include <asm/unaligned.h>
 
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
@@ -166,6 +168,33 @@ static int fd_configure_device(struct se_device *dev)
 			" block_device blocks: %llu logical_block_size: %d\n",
 			dev_size, div_u64(dev_size, fd_dev->fd_block_size),
 			fd_dev->fd_block_size);
+		/*
+		 * Check if the underlying struct block_device request_queue supports
+		 * the QUEUE_FLAG_DISCARD bit for UNMAP/WRITE_SAME in SCSI + TRIM
+		 * in ATA and we need to set TPE=1
+		 */
+		if (blk_queue_discard(q)) {
+			dev->dev_attrib.max_unmap_lba_count =
+				q->limits.max_discard_sectors;
+			/*
+			 * Currently hardcoded to 1 in Linux/SCSI code..
+			 */
+			dev->dev_attrib.max_unmap_block_desc_count = 1;
+			dev->dev_attrib.unmap_granularity =
+				q->limits.discard_granularity >> 9;
+			dev->dev_attrib.unmap_granularity_alignment =
+				q->limits.discard_alignment;
+			pr_debug("IFILE: BLOCK Discard support available,"
+					" disabled by default\n");
+		}
+		/*
+		 * Enable write same emulation for IBLOCK and use 0xFFFF as
+		 * the smaller WRITE_SAME(10) only has a two-byte block count.
+		 */
+		dev->dev_attrib.max_write_same_len = 0xFFFF;
+
+		if (blk_queue_nonrot(q))
+			dev->dev_attrib.is_nonrot = 1;
 	} else {
 		if (!(fd_dev->fbd_flags & FBDF_HAS_SIZE)) {
 			pr_err("FILEIO: Missing fd_dev_size="
@@ -176,6 +205,23 @@ static int fd_configure_device(struct se_device *dev)
 
 		dev->dev_attrib.hw_block_size = FD_BLOCKSIZE;
 		dev->dev_attrib.hw_max_sectors = FD_MAX_SECTORS;
+
+		/*
+		 * Limit UNMAP emulation to 8k Number of LBAs (NoLB)
+		 */
+		dev->dev_attrib.max_unmap_lba_count = 0x2000;
+		/*
+		 * Currently hardcoded to 1 in Linux/SCSI code..
+		 */
+		dev->dev_attrib.max_unmap_block_desc_count = 1;
+		dev->dev_attrib.unmap_granularity = 1;
+		dev->dev_attrib.unmap_granularity_alignment = 0;
+
+		/*
+		 * Limit WRITE_SAME w/ UNMAP=0 emulation to 8k Number of LBAs (NoLB)
+		 * based upon struct iovec limit for vfs_writev()
+		 */
+		dev->dev_attrib.max_write_same_len = 0x1000;
 	}
 
 	fd_dev->fd_block_size = dev->dev_attrib.hw_block_size;
@@ -190,11 +236,6 @@ static int fd_configure_device(struct se_device *dev)
 
 	fd_dev->fd_dev_id = fd_host->fd_host_dev_id_count++;
 	fd_dev->fd_queue_depth = dev->queue_depth;
-	/*
-	 * Limit WRITE_SAME w/ UNMAP=0 emulation to 8k Number of LBAs (NoLB)
-	 * based upon struct iovec limit for vfs_writev()
-	 */
-	dev->dev_attrib.max_write_same_len = 0x1000;
 
 	pr_debug("CORE_FILE[%u] - Added TCM FILEIO Device ID: %u at %s,"
 		" %llu total bytes\n", fd_host->fd_host_id, fd_dev->fd_dev_id,
@@ -442,6 +483,75 @@ fd_execute_write_same(struct se_cmd *cmd)
 }
 
 static sense_reason_t
+fd_do_unmap(struct se_cmd *cmd, void *priv, sector_t lba, sector_t nolb)
+{
+	struct file *file = priv;
+	struct inode *inode = file->f_mapping->host;
+	int ret;
+
+	if (S_ISBLK(inode->i_mode)) {
+		/* The backend is block device, use discard */
+		struct block_device *bdev = inode->i_bdev;
+
+		ret = blkdev_issue_discard(bdev, lba,
+				nolb, GFP_KERNEL, 0);
+		if (ret < 0) {
+			pr_warn("FILEIO: blkdev_issue_discard() failed: %d\n",
+				ret);
+			return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+		}
+	} else {
+		/* The backend is normal file, use fallocate */
+		struct se_device *se_dev = cmd->se_dev;
+		loff_t pos = lba * se_dev->dev_attrib.block_size;
+		unsigned int len = nolb * se_dev->dev_attrib.block_size;
+		int mode = FALLOC_FL_PUNCH_HOLE | FALLOC_FL_KEEP_SIZE;
+
+		if (!file->f_op->fallocate)
+			return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+
+		ret = file->f_op->fallocate(file, mode, pos, len);
+		if (ret < 0) {
+			pr_warn("FILEIO: fallocate() failed: %d\n", ret);
+			return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+		}
+	}
+
+	return 0;
+}
+
+static sense_reason_t
+fd_execute_write_same_unmap(struct se_cmd *cmd)
+{
+	struct se_device *se_dev = cmd->se_dev;
+	struct fd_dev *fd_dev = FD_DEV(se_dev);
+	struct file *file = fd_dev->fd_file;
+	sector_t lba = cmd->t_task_lba;
+	sector_t nolb = sbc_get_write_same_sectors(cmd);
+	int ret;
+
+	if (!nolb) {
+		target_complete_cmd(cmd, SAM_STAT_GOOD);
+		return 0;
+	}
+
+	ret = fd_do_unmap(cmd, file, lba, nolb);
+	if (ret)
+		return ret;
+
+	target_complete_cmd(cmd, GOOD);
+	return 0;
+}
+
+static sense_reason_t
+fd_execute_unmap(struct se_cmd *cmd)
+{
+	struct file *file = FD_DEV(cmd->se_dev)->fd_file;
+
+	return sbc_execute_unmap(cmd, fd_do_unmap, file);
+}
+
+static sense_reason_t
 fd_execute_rw(struct se_cmd *cmd)
 {
 	struct scatterlist *sgl = cmd->t_data_sg;
@@ -600,6 +710,8 @@ static struct sbc_ops fd_sbc_ops = {
 	.execute_rw		= fd_execute_rw,
 	.execute_sync_cache	= fd_execute_sync_cache,
 	.execute_write_same	= fd_execute_write_same,
+	.execute_write_same_unmap = fd_execute_write_same_unmap,
+	.execute_unmap		= fd_execute_unmap,
 };
 
 static sense_reason_t
diff --git a/drivers/target/target_core_iblock.c b/drivers/target/target_core_iblock.c
index 8bcc514ec8b6..07f5f94634bb 100644
--- a/drivers/target/target_core_iblock.c
+++ b/drivers/target/target_core_iblock.c
@@ -380,104 +380,40 @@ iblock_execute_sync_cache(struct se_cmd *cmd)
 }
 
 static sense_reason_t
-iblock_execute_unmap(struct se_cmd *cmd)
+iblock_do_unmap(struct se_cmd *cmd, void *priv,
+		sector_t lba, sector_t nolb)
 {
-	struct se_device *dev = cmd->se_dev;
-	struct iblock_dev *ib_dev = IBLOCK_DEV(dev);
-	unsigned char *buf, *ptr = NULL;
-	sector_t lba;
-	int size;
-	u32 range;
-	sense_reason_t ret = 0;
-	int dl, bd_dl, err;
-
-	/* We never set ANC_SUP */
-	if (cmd->t_task_cdb[1])
-		return TCM_INVALID_CDB_FIELD;
-
-	if (cmd->data_length == 0) {
-		target_complete_cmd(cmd, SAM_STAT_GOOD);
-		return 0;
-	}
+	struct block_device *bdev = priv;
+	int ret;
 
-	if (cmd->data_length < 8) {
-		pr_warn("UNMAP parameter list length %u too small\n",
-			cmd->data_length);
-		return TCM_PARAMETER_LIST_LENGTH_ERROR;
-	}
-
-	buf = transport_kmap_data_sg(cmd);
-	if (!buf)
+	ret = blkdev_issue_discard(bdev, lba, nolb, GFP_KERNEL, 0);
+	if (ret < 0) {
+		pr_err("blkdev_issue_discard() failed: %d\n", ret);
 		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-
-	dl = get_unaligned_be16(&buf[0]);
-	bd_dl = get_unaligned_be16(&buf[2]);
-
-	size = cmd->data_length - 8;
-	if (bd_dl > size)
-		pr_warn("UNMAP parameter list length %u too small, ignoring bd_dl %u\n",
-			cmd->data_length, bd_dl);
-	else
-		size = bd_dl;
-
-	if (size / 16 > dev->dev_attrib.max_unmap_block_desc_count) {
-		ret = TCM_INVALID_PARAMETER_LIST;
-		goto err;
 	}
 
-	/* First UNMAP block descriptor starts at 8 byte offset */
-	ptr = &buf[8];
-	pr_debug("UNMAP: Sub: %s Using dl: %u bd_dl: %u size: %u"
-		" ptr: %p\n", dev->transport->name, dl, bd_dl, size, ptr);
-
-	while (size >= 16) {
-		lba = get_unaligned_be64(&ptr[0]);
-		range = get_unaligned_be32(&ptr[8]);
-		pr_debug("UNMAP: Using lba: %llu and range: %u\n",
-				 (unsigned long long)lba, range);
-
-		if (range > dev->dev_attrib.max_unmap_lba_count) {
-			ret = TCM_INVALID_PARAMETER_LIST;
-			goto err;
-		}
-
-		if (lba + range > dev->transport->get_blocks(dev) + 1) {
-			ret = TCM_ADDRESS_OUT_OF_RANGE;
-			goto err;
-		}
-
-		err = blkdev_issue_discard(ib_dev->ibd_bd, lba, range,
-					   GFP_KERNEL, 0);
-		if (err < 0) {
-			pr_err("blkdev_issue_discard() failed: %d\n",
-					err);
-			ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-			goto err;
-		}
+	return 0;
+}
 
-		ptr += 16;
-		size -= 16;
-	}
+static sense_reason_t
+iblock_execute_unmap(struct se_cmd *cmd)
+{
+	struct block_device *bdev = IBLOCK_DEV(cmd->se_dev)->ibd_bd;
 
-err:
-	transport_kunmap_data_sg(cmd);
-	if (!ret)
-		target_complete_cmd(cmd, GOOD);
-	return ret;
+	return sbc_execute_unmap(cmd, iblock_do_unmap, bdev);
 }
 
 static sense_reason_t
 iblock_execute_write_same_unmap(struct se_cmd *cmd)
 {
-	struct iblock_dev *ib_dev = IBLOCK_DEV(cmd->se_dev);
-	int rc;
-
-	rc = blkdev_issue_discard(ib_dev->ibd_bd, cmd->t_task_lba,
-			sbc_get_write_same_sectors(cmd), GFP_KERNEL, 0);
-	if (rc < 0) {
-		pr_warn("blkdev_issue_discard() failed: %d\n", rc);
-		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-	}
+	struct block_device *bdev = IBLOCK_DEV(cmd->se_dev)->ibd_bd;
+	sector_t lba = cmd->t_task_lba;
+	sector_t nolb = sbc_get_write_same_sectors(cmd);
+	int ret;
+
+	ret = iblock_do_unmap(cmd, bdev, lba, nolb);
+	if (ret)
+		return ret;
 
 	target_complete_cmd(cmd, GOOD);
 	return 0;
diff --git a/drivers/target/target_core_sbc.c b/drivers/target/target_core_sbc.c
index 60d4b5185f32..bbc5b0ee2bdc 100644
--- a/drivers/target/target_core_sbc.c
+++ b/drivers/target/target_core_sbc.c
@@ -596,3 +596,88 @@ u32 sbc_get_device_type(struct se_device *dev)
 	return TYPE_DISK;
 }
 EXPORT_SYMBOL(sbc_get_device_type);
+
+sense_reason_t
+sbc_execute_unmap(struct se_cmd *cmd,
+	sense_reason_t (*do_unmap_fn)(struct se_cmd *, void *,
+				      sector_t, sector_t),
+	void *priv)
+{
+	struct se_device *dev = cmd->se_dev;
+	unsigned char *buf, *ptr = NULL;
+	sector_t lba;
+	int size;
+	u32 range;
+	sense_reason_t ret = 0;
+	int dl, bd_dl;
+
+	/* We never set ANC_SUP */
+	if (cmd->t_task_cdb[1])
+		return TCM_INVALID_CDB_FIELD;
+
+	if (cmd->data_length == 0) {
+		target_complete_cmd(cmd, SAM_STAT_GOOD);
+		return 0;
+	}
+
+	if (cmd->data_length < 8) {
+		pr_warn("UNMAP parameter list length %u too small\n",
+			cmd->data_length);
+		return TCM_PARAMETER_LIST_LENGTH_ERROR;
+	}
+
+	buf = transport_kmap_data_sg(cmd);
+	if (!buf)
+		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+
+	dl = get_unaligned_be16(&buf[0]);
+	bd_dl = get_unaligned_be16(&buf[2]);
+
+	size = cmd->data_length - 8;
+	if (bd_dl > size)
+		pr_warn("UNMAP parameter list length %u too small, ignoring bd_dl %u\n",
+			cmd->data_length, bd_dl);
+	else
+		size = bd_dl;
+
+	if (size / 16 > dev->dev_attrib.max_unmap_block_desc_count) {
+		ret = TCM_INVALID_PARAMETER_LIST;
+		goto err;
+	}
+
+	/* First UNMAP block descriptor starts at 8 byte offset */
+	ptr = &buf[8];
+	pr_debug("UNMAP: Sub: %s Using dl: %u bd_dl: %u size: %u"
+		" ptr: %p\n", dev->transport->name, dl, bd_dl, size, ptr);
+
+	while (size >= 16) {
+		lba = get_unaligned_be64(&ptr[0]);
+		range = get_unaligned_be32(&ptr[8]);
+		pr_debug("UNMAP: Using lba: %llu and range: %u\n",
+				 (unsigned long long)lba, range);
+
+		if (range > dev->dev_attrib.max_unmap_lba_count) {
+			ret = TCM_INVALID_PARAMETER_LIST;
+			goto err;
+		}
+
+		if (lba + range > dev->transport->get_blocks(dev) + 1) {
+			ret = TCM_ADDRESS_OUT_OF_RANGE;
+			goto err;
+		}
+
+		ret = do_unmap_fn(cmd, priv, lba, range);
+		if (ret)
+			goto err;
+
+		ptr += 16;
+		size -= 16;
+	}
+
+err:
+	transport_kunmap_data_sg(cmd);
+	if (!ret)
+		target_complete_cmd(cmd, GOOD);
+	return ret;
+}
+EXPORT_SYMBOL(sbc_execute_unmap);
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index 3243ea790eab..f8388b4024aa 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -65,7 +65,6 @@ static void transport_complete_task_attr(struct se_cmd *cmd);
 static void transport_handle_queue_full(struct se_cmd *cmd,
 		struct se_device *dev);
 static int transport_generic_get_mem(struct se_cmd *cmd);
-static int target_get_sess_cmd(struct se_session *, struct se_cmd *, bool);
 static void transport_put_cmd(struct se_cmd *cmd);
 static void target_complete_ok_work(struct work_struct *work);
 
@@ -2179,7 +2178,7 @@ EXPORT_SYMBOL(transport_generic_free_cmd);
  * @se_cmd:	command descriptor to add
  * @ack_kref:	Signal that fabric will perform an ack target_put_sess_cmd()
  */
-static int target_get_sess_cmd(struct se_session *se_sess, struct se_cmd *se_cmd,
+int target_get_sess_cmd(struct se_session *se_sess, struct se_cmd *se_cmd,
 			       bool ack_kref)
 {
 	unsigned long flags;
@@ -2208,6 +2207,7 @@ out:
 	spin_unlock_irqrestore(&se_sess->sess_cmd_lock, flags);
 	return ret;
 }
+EXPORT_SYMBOL(target_get_sess_cmd);
 
 static void target_release_cmd_kref(struct kref *kref)
 {
@@ -2765,8 +2765,13 @@ transport_send_check_condition_and_sense(struct se_cmd *cmd,
 		/* CURRENT ERROR */
 		buffer[0] = 0x70;
 		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
-		/* ILLEGAL REQUEST */
-		buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+		/*
+		 * Returning ILLEGAL REQUEST would cause immediate IO errors on
+		 * Solaris initiators.  Returning NOT READY instead means the
+		 * operations will be retried a finite number of times and we
+		 * can survive intermittent errors.
+		 */
+		buffer[SPC_SENSE_KEY_OFFSET] = NOT_READY;
 		/* LOGICAL UNIT COMMUNICATION FAILURE */
 		buffer[SPC_ASC_KEY_OFFSET] = 0x08;
 		break;
diff --git a/drivers/target/tcm_fc/tfc_io.c b/drivers/target/tcm_fc/tfc_io.c
index b6fd4cf42840..e415af32115a 100644
--- a/drivers/target/tcm_fc/tfc_io.c
+++ b/drivers/target/tcm_fc/tfc_io.c
@@ -103,6 +103,13 @@ int ft_queue_data_in(struct se_cmd *se_cmd)
 	use_sg = !(remaining % 4);
 
 	while (remaining) {
+		struct fc_seq *seq = cmd->seq;
+
+		if (!seq) {
+			pr_debug("%s: Command aborted, xid 0x%x\n",
+				 __func__, ep->xid);
+			break;
+		}
 		if (!mem_len) {
 			sg = sg_next(sg);
 			mem_len = min((size_t)sg->length, remaining);
@@ -169,7 +176,7 @@ int ft_queue_data_in(struct se_cmd *se_cmd)
 			f_ctl |= FC_FC_END_SEQ;
 		fc_fill_fc_hdr(fp, FC_RCTL_DD_SOL_DATA, ep->did, ep->sid,
 			       FC_TYPE_FCP, f_ctl, fh_off);
-		error = lport->tt.seq_send(lport, cmd->seq, fp);
+		error = lport->tt.seq_send(lport, seq, fp);
 		if (error) {
 			/* XXX For now, initiator will retry */
 			pr_err_ratelimited("%s: Failed to send frame %p, "
diff --git a/drivers/target/tcm_fc/tfc_sess.c b/drivers/target/tcm_fc/tfc_sess.c
index 113f33598b9f..4859505ae2ed 100644
--- a/drivers/target/tcm_fc/tfc_sess.c
+++ b/drivers/target/tcm_fc/tfc_sess.c
@@ -428,19 +428,12 @@ static int ft_prli(struct fc_rport_priv *rdata, u32 spp_len,
 	return ret;
 }
 
-static void ft_sess_rcu_free(struct rcu_head *rcu)
-{
-	struct ft_sess *sess = container_of(rcu, struct ft_sess, rcu);
-
-	kfree(sess);
-}
-
 static void ft_sess_free(struct kref *kref)
 {
 	struct ft_sess *sess = container_of(kref, struct ft_sess, kref);
 
 	transport_deregister_session(sess->se_sess);
-	call_rcu(&sess->rcu, ft_sess_rcu_free);
+	kfree_rcu(sess, rcu);
 }
 
 void ft_sess_put(struct ft_sess *sess)
diff --git a/drivers/thermal/Kconfig b/drivers/thermal/Kconfig
index a764f165b589..5e3c02554d99 100644
--- a/drivers/thermal/Kconfig
+++ b/drivers/thermal/Kconfig
@@ -67,15 +67,16 @@ config THERMAL_GOV_USER_SPACE
 	  Enable this to let the user space manage the platform thermals.
 
 config CPU_THERMAL
-	tristate "generic cpu cooling support"
+	bool "generic cpu cooling support"
 	depends on 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).
+	  reduction. 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 THERMAL_EMULATION
@@ -86,6 +87,10 @@ config THERMAL_EMULATION
 	  user can manually input temperature and test the different trip
 	  threshold behaviour for simulation purpose.
 
+	  WARNING: Be careful while enabling this option on production systems,
+	  because userland can easily disable the thermal policy by simply
+	  flooding this sysfs node with low temperature values.
+
 config SPEAR_THERMAL
 	bool "SPEAr thermal sensor driver"
 	depends on PLAT_SPEAR
@@ -117,15 +122,6 @@ config EXYNOS_THERMAL
 	  If you say yes here you get support for TMU (Thermal Management
 	  Unit) on SAMSUNG EXYNOS series of SoC.
 
-config EXYNOS_THERMAL_EMUL
-	bool "EXYNOS TMU emulation mode support"
-	depends on EXYNOS_THERMAL
-	help
-	  Exynos 4412 and 4414 and 5 series has emulation mode on TMU.
-	  Enable this option will be make sysfs node in exynos thermal platform
-	  device directory to support emulation mode. With emulation mode sysfs
-	  node, you can manually input temperature to TMU for simulation purpose.
-
 config DOVE_THERMAL
 	tristate "Temperature sensor on Marvell Dove SoCs"
 	depends on ARCH_DOVE
@@ -144,6 +140,14 @@ config DB8500_THERMAL
 	  created. Cooling devices can be bound to the trip points to cool this
 	  thermal zone if trip points reached.
 
+config ARMADA_THERMAL
+	tristate "Armada 370/XP thermal management"
+	depends on ARCH_MVEBU
+	depends on OF
+	help
+	  Enable this option if you want to have support for thermal management
+	  controller present in Armada 370 and Armada XP SoC.
+
 config DB8500_CPUFREQ_COOLING
 	tristate "DB8500 cpufreq cooling"
 	depends on ARCH_U8500
diff --git a/drivers/thermal/Makefile b/drivers/thermal/Makefile
index d3a2b38c31e8..c054d410ac3f 100644
--- a/drivers/thermal/Makefile
+++ b/drivers/thermal/Makefile
@@ -3,14 +3,15 @@
 #
 
 obj-$(CONFIG_THERMAL)		+= thermal_sys.o
+thermal_sys-y			+= thermal_core.o
 
 # governors
-obj-$(CONFIG_THERMAL_GOV_FAIR_SHARE)	+= fair_share.o
-obj-$(CONFIG_THERMAL_GOV_STEP_WISE)	+= step_wise.o
-obj-$(CONFIG_THERMAL_GOV_USER_SPACE)	+= user_space.o
+thermal_sys-$(CONFIG_THERMAL_GOV_FAIR_SHARE)	+= fair_share.o
+thermal_sys-$(CONFIG_THERMAL_GOV_STEP_WISE)	+= step_wise.o
+thermal_sys-$(CONFIG_THERMAL_GOV_USER_SPACE)	+= user_space.o
 
 # cpufreq cooling
-obj-$(CONFIG_CPU_THERMAL)	+= cpu_cooling.o
+thermal_sys-$(CONFIG_CPU_THERMAL)	+= cpu_cooling.o
 
 # platform thermal drivers
 obj-$(CONFIG_SPEAR_THERMAL)	+= spear_thermal.o
@@ -19,6 +20,7 @@ obj-$(CONFIG_KIRKWOOD_THERMAL)  += kirkwood_thermal.o
 obj-$(CONFIG_EXYNOS_THERMAL)	+= exynos_thermal.o
 obj-$(CONFIG_DOVE_THERMAL)  	+= dove_thermal.o
 obj-$(CONFIG_DB8500_THERMAL)	+= db8500_thermal.o
+obj-$(CONFIG_ARMADA_THERMAL)	+= armada_thermal.o
 obj-$(CONFIG_DB8500_CPUFREQ_COOLING)	+= db8500_cpufreq_cooling.o
 obj-$(CONFIG_INTEL_POWERCLAMP)	+= intel_powerclamp.o
 
diff --git a/drivers/thermal/armada_thermal.c b/drivers/thermal/armada_thermal.c
new file mode 100644
index 000000000000..5b4d75fd7b49
--- /dev/null
+++ b/drivers/thermal/armada_thermal.c
@@ -0,0 +1,232 @@
+/*
+ * Marvell Armada 370/XP thermal sensor driver
+ *
+ * Copyright (C) 2013 Marvell
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/of_device.h>
+#include <linux/thermal.h>
+
+#define THERMAL_VALID_OFFSET		9
+#define THERMAL_VALID_MASK		0x1
+#define THERMAL_TEMP_OFFSET		10
+#define THERMAL_TEMP_MASK		0x1ff
+
+/* Thermal Manager Control and Status Register */
+#define PMU_TDC0_SW_RST_MASK		(0x1 << 1)
+#define PMU_TM_DISABLE_OFFS		0
+#define PMU_TM_DISABLE_MASK		(0x1 << PMU_TM_DISABLE_OFFS)
+#define PMU_TDC0_REF_CAL_CNT_OFFS	11
+#define PMU_TDC0_REF_CAL_CNT_MASK	(0x1ff << PMU_TDC0_REF_CAL_CNT_OFFS)
+#define PMU_TDC0_OTF_CAL_MASK		(0x1 << 30)
+#define PMU_TDC0_START_CAL_MASK		(0x1 << 25)
+
+struct armada_thermal_ops;
+
+/* Marvell EBU Thermal Sensor Dev Structure */
+struct armada_thermal_priv {
+	void __iomem *sensor;
+	void __iomem *control;
+	struct armada_thermal_ops *ops;
+};
+
+struct armada_thermal_ops {
+	/* Initialize the sensor */
+	void (*init_sensor)(struct armada_thermal_priv *);
+
+	/* Test for a valid sensor value (optional) */
+	bool (*is_valid)(struct armada_thermal_priv *);
+};
+
+static void armadaxp_init_sensor(struct armada_thermal_priv *priv)
+{
+	unsigned long reg;
+
+	reg = readl_relaxed(priv->control);
+	reg |= PMU_TDC0_OTF_CAL_MASK;
+	writel(reg, priv->control);
+
+	/* Reference calibration value */
+	reg &= ~PMU_TDC0_REF_CAL_CNT_MASK;
+	reg |= (0xf1 << PMU_TDC0_REF_CAL_CNT_OFFS);
+	writel(reg, priv->control);
+
+	/* Reset the sensor */
+	reg = readl_relaxed(priv->control);
+	writel((reg | PMU_TDC0_SW_RST_MASK), priv->control);
+
+	writel(reg, priv->control);
+
+	/* Enable the sensor */
+	reg = readl_relaxed(priv->sensor);
+	reg &= ~PMU_TM_DISABLE_MASK;
+	writel(reg, priv->sensor);
+}
+
+static void armada370_init_sensor(struct armada_thermal_priv *priv)
+{
+	unsigned long reg;
+
+	reg = readl_relaxed(priv->control);
+	reg |= PMU_TDC0_OTF_CAL_MASK;
+	writel(reg, priv->control);
+
+	/* Reference calibration value */
+	reg &= ~PMU_TDC0_REF_CAL_CNT_MASK;
+	reg |= (0xf1 << PMU_TDC0_REF_CAL_CNT_OFFS);
+	writel(reg, priv->control);
+
+	reg &= ~PMU_TDC0_START_CAL_MASK;
+	writel(reg, priv->control);
+
+	mdelay(10);
+}
+
+static bool armada_is_valid(struct armada_thermal_priv *priv)
+{
+	unsigned long reg = readl_relaxed(priv->sensor);
+
+	return (reg >> THERMAL_VALID_OFFSET) & THERMAL_VALID_MASK;
+}
+
+static int armada_get_temp(struct thermal_zone_device *thermal,
+			  unsigned long *temp)
+{
+	struct armada_thermal_priv *priv = thermal->devdata;
+	unsigned long reg;
+
+	/* Valid check */
+	if (priv->ops->is_valid && !priv->ops->is_valid(priv)) {
+		dev_err(&thermal->device,
+			"Temperature sensor reading not valid\n");
+		return -EIO;
+	}
+
+	reg = readl_relaxed(priv->sensor);
+	reg = (reg >> THERMAL_TEMP_OFFSET) & THERMAL_TEMP_MASK;
+	*temp = (3153000000UL - (10000000UL*reg)) / 13825;
+	return 0;
+}
+
+static struct thermal_zone_device_ops ops = {
+	.get_temp = armada_get_temp,
+};
+
+static const struct armada_thermal_ops armadaxp_ops = {
+	.init_sensor = armadaxp_init_sensor,
+};
+
+static const struct armada_thermal_ops armada370_ops = {
+	.is_valid = armada_is_valid,
+	.init_sensor = armada370_init_sensor,
+};
+
+static const struct of_device_id armada_thermal_id_table[] = {
+	{
+		.compatible = "marvell,armadaxp-thermal",
+		.data       = &armadaxp_ops,
+	},
+	{
+		.compatible = "marvell,armada370-thermal",
+		.data       = &armada370_ops,
+	},
+	{
+		/* sentinel */
+	},
+};
+MODULE_DEVICE_TABLE(of, armada_thermal_id_table);
+
+static int armada_thermal_probe(struct platform_device *pdev)
+{
+	struct thermal_zone_device *thermal;
+	const struct of_device_id *match;
+	struct armada_thermal_priv *priv;
+	struct resource *res;
+
+	match = of_match_device(armada_thermal_id_table, &pdev->dev);
+	if (!match)
+		return -ENODEV;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get platform resource\n");
+		return -ENODEV;
+	}
+
+	priv->sensor = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->sensor))
+		return PTR_ERR(priv->sensor);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get platform resource\n");
+		return -ENODEV;
+	}
+
+	priv->control = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->control))
+		return PTR_ERR(priv->control);
+
+	priv->ops = (struct armada_thermal_ops *)match->data;
+	priv->ops->init_sensor(priv);
+
+	thermal = thermal_zone_device_register("armada_thermal", 0, 0,
+					       priv, &ops, NULL, 0, 0);
+	if (IS_ERR(thermal)) {
+		dev_err(&pdev->dev,
+			"Failed to register thermal zone device\n");
+		return PTR_ERR(thermal);
+	}
+
+	platform_set_drvdata(pdev, thermal);
+
+	return 0;
+}
+
+static int armada_thermal_exit(struct platform_device *pdev)
+{
+	struct thermal_zone_device *armada_thermal =
+		platform_get_drvdata(pdev);
+
+	thermal_zone_device_unregister(armada_thermal);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver armada_thermal_driver = {
+	.probe = armada_thermal_probe,
+	.remove = armada_thermal_exit,
+	.driver = {
+		.name = "armada_thermal",
+		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(armada_thermal_id_table),
+	},
+};
+
+module_platform_driver(armada_thermal_driver);
+
+MODULE_AUTHOR("Ezequiel Garcia <ezequiel.garcia@free-electrons.com>");
+MODULE_DESCRIPTION("Armada 370/XP thermal driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/thermal/cpu_cooling.c b/drivers/thermal/cpu_cooling.c
index 8dc44cbb3e09..c94bf2e5de62 100644
--- a/drivers/thermal/cpu_cooling.c
+++ b/drivers/thermal/cpu_cooling.c
@@ -20,10 +20,8 @@
  *
  * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  */
-#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>
@@ -31,21 +29,19 @@
 #include <linux/cpu_cooling.h>
 
 /**
- * struct cpufreq_cooling_device
+ * struct cpufreq_cooling_device - data for cooling device with cpufreq
  * @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.
+ * @cool_dev: thermal_cooling_device pointer to keep track of the
+ *	registered 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
+ * cpufreq_cooling_device registered. In order to prevent corruption of this a
  * mutex lock cooling_cpufreq_lock is used.
  */
 struct cpufreq_cooling_device {
@@ -54,9 +50,7 @@ struct cpufreq_cooling_device {
 	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 DEFINE_MUTEX(cooling_cpufreq_lock);
 
@@ -70,6 +64,11 @@ static 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.
+ *
+ * This function will populate @id with an unique
+ * id, using the idr API.
+ *
+ * Return: 0 on success, an error code on failure.
  */
 static int get_idr(struct idr *idr, int *id)
 {
@@ -81,6 +80,7 @@ static int get_idr(struct idr *idr, int *id)
 	if (unlikely(ret < 0))
 		return ret;
 	*id = ret;
+
 	return 0;
 }
 
@@ -99,63 +99,162 @@ static void release_idr(struct idr *idr, int id)
 /* 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.
+ * is_cpufreq_valid - function to check frequency transitioning capability.
  * @cpu: cpu for which check is needed.
+ *
+ * This function will check the current state of the system if
+ * it is capable of changing the frequency for a given @cpu.
+ *
+ * Return: 0 if the system is not currently capable of changing
+ * the frequency of given cpu. !0 in case the frequency is changeable.
  */
 static int is_cpufreq_valid(int cpu)
 {
 	struct cpufreq_policy policy;
+
 	return !cpufreq_get_policy(&policy, cpu);
 }
 
+enum cpufreq_cooling_property {
+	GET_LEVEL,
+	GET_FREQ,
+	GET_MAXL,
+};
+
 /**
- * get_cpu_frequency - get the absolute value of frequency from level.
- * @cpu: cpu for which frequency is fetched.
- * @level: level of frequency, equals cooling state of cpu cooling device
- *	e.g level=0 --> 1st MAX FREQ, level=1 ---> 2nd MAX FREQ, .... etc
+ * get_property - fetch a property of interest for a give cpu.
+ * @cpu: cpu for which the property is required
+ * @input: query parameter
+ * @output: query return
+ * @property: type of query (frequency, level, max level)
+ *
+ * This is the common function to
+ * 1. get maximum cpu cooling states
+ * 2. translate frequency to cooling state
+ * 3. translate cooling state to frequency
+ * Note that the code may be not in good shape
+ * but it is written in this way in order to:
+ * a) reduce duplicate code as most of the code can be shared.
+ * b) make sure the logic is consistent when translating between
+ *    cooling states and frequencies.
+ *
+ * Return: 0 on success, -EINVAL when invalid parameters are passed.
  */
-static unsigned int get_cpu_frequency(unsigned int cpu, unsigned long level)
+static int get_property(unsigned int cpu, unsigned long input,
+			unsigned int *output,
+			enum cpufreq_cooling_property property)
 {
-	int ret = 0, i = 0;
-	unsigned long level_index;
-	bool descend = false;
+	int i, j;
+	unsigned long max_level = 0, level = 0;
+	unsigned int freq = CPUFREQ_ENTRY_INVALID;
+	int descend = -1;
 	struct cpufreq_frequency_table *table =
 					cpufreq_frequency_get_table(cpu);
+
+	if (!output)
+		return -EINVAL;
+
 	if (!table)
-		return ret;
+		return -EINVAL;
 
-	while (table[i].frequency != CPUFREQ_TABLE_END) {
+	for (i = 0; table[i].frequency != CPUFREQ_TABLE_END; i++) {
+		/* ignore invalid entries */
 		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;
-		}
+		/* ignore duplicate entry */
+		if (freq == table[i].frequency)
+			continue;
+
+		/* get the frequency order */
+		if (freq != CPUFREQ_ENTRY_INVALID && descend != -1)
+			descend = !!(freq > table[i].frequency);
 
-		/*return if level matched and table in descending order*/
-		if (descend && i == level)
-			return table[i].frequency;
-		i++;
+		freq = table[i].frequency;
+		max_level++;
 	}
-	i--;
 
-	if (level > i || descend)
-		return ret;
-	level_index = i - level;
+	/* get max level */
+	if (property == GET_MAXL) {
+		*output = (unsigned int)max_level;
+		return 0;
+	}
 
-	/*Scan the table in reverse order and match the level*/
-	while (i >= 0) {
+	if (property == GET_FREQ)
+		level = descend ? input : (max_level - input - 1);
+
+	for (i = 0, j = 0; table[i].frequency != CPUFREQ_TABLE_END; i++) {
+		/* ignore invalid entry */
 		if (table[i].frequency == CPUFREQ_ENTRY_INVALID)
 			continue;
-		/*return if level matched*/
-		if (i == level_index)
-			return table[i].frequency;
-		i--;
+
+		/* ignore duplicate entry */
+		if (freq == table[i].frequency)
+			continue;
+
+		/* now we have a valid frequency entry */
+		freq = table[i].frequency;
+
+		if (property == GET_LEVEL && (unsigned int)input == freq) {
+			/* get level by frequency */
+			*output = descend ? j : (max_level - j - 1);
+			return 0;
+		}
+		if (property == GET_FREQ && level == j) {
+			/* get frequency by level */
+			*output = freq;
+			return 0;
+		}
+		j++;
 	}
-	return ret;
+
+	return -EINVAL;
+}
+
+/**
+ * cpufreq_cooling_get_level - for a give cpu, return the cooling level.
+ * @cpu: cpu for which the level is required
+ * @freq: the frequency of interest
+ *
+ * This function will match the cooling level corresponding to the
+ * requested @freq and return it.
+ *
+ * Return: The matched cooling level on success or THERMAL_CSTATE_INVALID
+ * otherwise.
+ */
+unsigned long cpufreq_cooling_get_level(unsigned int cpu, unsigned int freq)
+{
+	unsigned int val;
+
+	if (get_property(cpu, (unsigned long)freq, &val, GET_LEVEL))
+		return THERMAL_CSTATE_INVALID;
+
+	return (unsigned long)val;
+}
+EXPORT_SYMBOL_GPL(cpufreq_cooling_get_level);
+
+/**
+ * get_cpu_frequency - get the absolute value of frequency from level.
+ * @cpu: cpu for which frequency is fetched.
+ * @level: cooling level
+ *
+ * This function matches cooling level with frequency. Based on a cooling level
+ * of frequency, equals cooling state of cpu cooling device, it will return
+ * the corresponding frequency.
+ *	e.g level=0 --> 1st MAX FREQ, level=1 ---> 2nd MAX FREQ, .... etc
+ *
+ * Return: 0 on error, the corresponding frequency otherwise.
+ */
+static unsigned int get_cpu_frequency(unsigned int cpu, unsigned long level)
+{
+	int ret = 0;
+	unsigned int freq;
+
+	ret = get_property(cpu, level, &freq, GET_FREQ);
+	if (ret)
+		return 0;
+
+	return freq;
 }
 
 /**
@@ -163,13 +262,19 @@ static unsigned int get_cpu_frequency(unsigned int cpu, unsigned long level)
  * @cpufreq_device: cpufreq_cooling_device pointer containing frequency
  *	clipping data.
  * @cooling_state: value of the cooling state.
+ *
+ * Function used to make sure the cpufreq layer is aware of current thermal
+ * limits. The limits are applied by updating the cpufreq policy.
+ *
+ * Return: 0 on success, an error code otherwise (-EINVAL in case wrong
+ * cooling state).
  */
 static int cpufreq_apply_cooling(struct cpufreq_cooling_device *cpufreq_device,
-				unsigned long cooling_state)
+				 unsigned long cooling_state)
 {
 	unsigned int cpuid, clip_freq;
-	struct cpumask *maskPtr = &cpufreq_device->allowed_cpus;
-	unsigned int cpu = cpumask_any(maskPtr);
+	struct cpumask *mask = &cpufreq_device->allowed_cpus;
+	unsigned int cpu = cpumask_any(mask);
 
 
 	/* Check if the old cooling action is same as new cooling action */
@@ -184,7 +289,7 @@ static int cpufreq_apply_cooling(struct cpufreq_cooling_device *cpufreq_device,
 	cpufreq_device->cpufreq_val = clip_freq;
 	notify_device = cpufreq_device;
 
-	for_each_cpu(cpuid, maskPtr) {
+	for_each_cpu(cpuid, mask) {
 		if (is_cpufreq_valid(cpuid))
 			cpufreq_update_policy(cpuid);
 	}
@@ -199,9 +304,15 @@ static int cpufreq_apply_cooling(struct cpufreq_cooling_device *cpufreq_device,
  * @nb:	struct notifier_block * with callback info.
  * @event: value showing cpufreq event for which this function invoked.
  * @data: callback-specific data
+ *
+ * Callback to highjack the notification on cpufreq policy transition.
+ * Every time there is a change in policy, we will intercept and
+ * update the cpufreq policy with thermal constraints.
+ *
+ * Return: 0 (success)
  */
 static int cpufreq_thermal_notifier(struct notifier_block *nb,
-					unsigned long event, void *data)
+				    unsigned long event, void *data)
 {
 	struct cpufreq_policy *policy = data;
 	unsigned long max_freq = 0;
@@ -212,7 +323,7 @@ static int cpufreq_thermal_notifier(struct notifier_block *nb,
 	if (cpumask_test_cpu(policy->cpu, &notify_device->allowed_cpus))
 		max_freq = notify_device->cpufreq_val;
 
-	/* Never exceed user_policy.max*/
+	/* Never exceed user_policy.max */
 	if (max_freq > policy->user_policy.max)
 		max_freq = policy->user_policy.max;
 
@@ -222,50 +333,46 @@ static int cpufreq_thermal_notifier(struct notifier_block *nb,
 	return 0;
 }
 
-/*
- * cpufreq cooling device callback functions are defined below
- */
+/* 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.
+ *
+ * Callback for the thermal cooling device to return the cpufreq
+ * max cooling state.
+ *
+ * Return: 0 on success, an error code otherwise.
  */
 static int cpufreq_get_max_state(struct thermal_cooling_device *cdev,
 				 unsigned long *state)
 {
 	struct cpufreq_cooling_device *cpufreq_device = cdev->devdata;
-	struct cpumask *maskPtr = &cpufreq_device->allowed_cpus;
+	struct cpumask *mask = &cpufreq_device->allowed_cpus;
 	unsigned int cpu;
-	struct cpufreq_frequency_table *table;
-	unsigned long count = 0;
-	int i = 0;
-
-	cpu = cpumask_any(maskPtr);
-	table = cpufreq_frequency_get_table(cpu);
-	if (!table) {
-		*state = 0;
-		return 0;
-	}
+	unsigned int count = 0;
+	int ret;
 
-	for (i = 0; (table[i].frequency != CPUFREQ_TABLE_END); i++) {
-		if (table[i].frequency == CPUFREQ_ENTRY_INVALID)
-			continue;
-		count++;
-	}
+	cpu = cpumask_any(mask);
 
-	if (count > 0) {
-		*state = --count;
-		return 0;
-	}
+	ret = get_property(cpu, 0, &count, GET_MAXL);
 
-	return -EINVAL;
+	if (count > 0)
+		*state = count;
+
+	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.
+ *
+ * Callback for the thermal cooling device to return the cpufreq
+ * current cooling state.
+ *
+ * Return: 0 on success, an error code otherwise.
  */
 static int cpufreq_get_cur_state(struct thermal_cooling_device *cdev,
 				 unsigned long *state)
@@ -273,6 +380,7 @@ static int cpufreq_get_cur_state(struct thermal_cooling_device *cdev,
 	struct cpufreq_cooling_device *cpufreq_device = cdev->devdata;
 
 	*state = cpufreq_device->cpufreq_state;
+
 	return 0;
 }
 
@@ -280,6 +388,11 @@ static int cpufreq_get_cur_state(struct thermal_cooling_device *cdev,
  * 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.
+ *
+ * Callback for the thermal cooling device to change the cpufreq
+ * current cooling state.
+ *
+ * Return: 0 on success, an error code otherwise.
  */
 static int cpufreq_set_cur_state(struct thermal_cooling_device *cdev,
 				 unsigned long state)
@@ -304,9 +417,16 @@ static struct notifier_block thermal_cpufreq_notifier_block = {
 /**
  * cpufreq_cooling_register - function to create cpufreq cooling device.
  * @clip_cpus: cpumask of cpus where the frequency constraints will happen.
+ *
+ * This interface function registers the cpufreq cooling device with the name
+ * "thermal-cpufreq-%x". This api can support multiple instances of cpufreq
+ * cooling devices.
+ *
+ * Return: a valid struct thermal_cooling_device pointer on success,
+ * on failure, it returns a corresponding ERR_PTR().
  */
-struct thermal_cooling_device *cpufreq_cooling_register(
-	const struct cpumask *clip_cpus)
+struct thermal_cooling_device *
+cpufreq_cooling_register(const struct cpumask *clip_cpus)
 {
 	struct thermal_cooling_device *cool_dev;
 	struct cpufreq_cooling_device *cpufreq_dev = NULL;
@@ -315,9 +435,9 @@ struct thermal_cooling_device *cpufreq_cooling_register(
 	int ret = 0, i;
 	struct cpufreq_policy policy;
 
-	/*Verify that all the clip cpus have same freq_min, freq_max limit*/
+	/* 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*/
+		/* continue if cpufreq policy not found and not return error */
 		if (!cpufreq_get_policy(&policy, i))
 			continue;
 		if (min == 0 && max == 0) {
@@ -325,12 +445,12 @@ struct thermal_cooling_device *cpufreq_cooling_register(
 			max = policy.cpuinfo.max_freq;
 		} else {
 			if (min != policy.cpuinfo.min_freq ||
-				max != policy.cpuinfo.max_freq)
+			    max != policy.cpuinfo.max_freq)
 				return ERR_PTR(-EINVAL);
 		}
 	}
 	cpufreq_dev = kzalloc(sizeof(struct cpufreq_cooling_device),
-			GFP_KERNEL);
+			      GFP_KERNEL);
 	if (!cpufreq_dev)
 		return ERR_PTR(-ENOMEM);
 
@@ -342,10 +462,11 @@ struct thermal_cooling_device *cpufreq_cooling_register(
 		return ERR_PTR(-EINVAL);
 	}
 
-	sprintf(dev_name, "thermal-cpufreq-%d", cpufreq_dev->id);
+	snprintf(dev_name, sizeof(dev_name), "thermal-cpufreq-%d",
+		 cpufreq_dev->id);
 
 	cool_dev = thermal_cooling_device_register(dev_name, cpufreq_dev,
-						&cpufreq_cooling_ops);
+						   &cpufreq_cooling_ops);
 	if (!cool_dev) {
 		release_idr(&cpufreq_idr, cpufreq_dev->id);
 		kfree(cpufreq_dev);
@@ -358,17 +479,20 @@ struct thermal_cooling_device *cpufreq_cooling_register(
 	/* Register the notifier for first cpufreq cooling device */
 	if (cpufreq_dev_count == 0)
 		cpufreq_register_notifier(&thermal_cpufreq_notifier_block,
-						CPUFREQ_POLICY_NOTIFIER);
+					  CPUFREQ_POLICY_NOTIFIER);
 	cpufreq_dev_count++;
 
 	mutex_unlock(&cooling_cpufreq_lock);
+
 	return cool_dev;
 }
-EXPORT_SYMBOL(cpufreq_cooling_register);
+EXPORT_SYMBOL_GPL(cpufreq_cooling_register);
 
 /**
  * cpufreq_cooling_unregister - function to remove cpufreq cooling device.
  * @cdev: thermal cooling device pointer.
+ *
+ * This interface function unregisters the "thermal-cpufreq-%x" cooling device.
  */
 void cpufreq_cooling_unregister(struct thermal_cooling_device *cdev)
 {
@@ -378,14 +502,13 @@ void cpufreq_cooling_unregister(struct thermal_cooling_device *cdev)
 	cpufreq_dev_count--;
 
 	/* Unregister the notifier for the last cpufreq cooling device */
-	if (cpufreq_dev_count == 0) {
+	if (cpufreq_dev_count == 0)
 		cpufreq_unregister_notifier(&thermal_cpufreq_notifier_block,
-					CPUFREQ_POLICY_NOTIFIER);
-	}
+					    CPUFREQ_POLICY_NOTIFIER);
 	mutex_unlock(&cooling_cpufreq_lock);
 
 	thermal_cooling_device_unregister(cpufreq_dev->cool_dev);
 	release_idr(&cpufreq_idr, cpufreq_dev->id);
 	kfree(cpufreq_dev);
 }
-EXPORT_SYMBOL(cpufreq_cooling_unregister);
+EXPORT_SYMBOL_GPL(cpufreq_cooling_unregister);
diff --git a/drivers/thermal/db8500_cpufreq_cooling.c b/drivers/thermal/db8500_cpufreq_cooling.c
index 21419851fc02..786d19263ab0 100644
--- a/drivers/thermal/db8500_cpufreq_cooling.c
+++ b/drivers/thermal/db8500_cpufreq_cooling.c
@@ -37,7 +37,7 @@ static int db8500_cpufreq_cooling_probe(struct platform_device *pdev)
 	cpumask_set_cpu(0, &mask_val);
 	cdev = cpufreq_cooling_register(&mask_val);
 
-	if (IS_ERR_OR_NULL(cdev)) {
+	if (IS_ERR(cdev)) {
 		dev_err(&pdev->dev, "Failed to register cooling device\n");
 		return PTR_ERR(cdev);
 	}
diff --git a/drivers/thermal/db8500_thermal.c b/drivers/thermal/db8500_thermal.c
index 61ce60a35921..1e3b3bf9f993 100644
--- a/drivers/thermal/db8500_thermal.c
+++ b/drivers/thermal/db8500_thermal.c
@@ -419,7 +419,8 @@ static int db8500_thermal_probe(struct platform_device *pdev)
 	low_irq = platform_get_irq_byname(pdev, "IRQ_HOTMON_LOW");
 	if (low_irq < 0) {
 		dev_err(&pdev->dev, "Get IRQ_HOTMON_LOW failed.\n");
-		return low_irq;
+		ret = low_irq;
+		goto out_unlock;
 	}
 
 	ret = devm_request_threaded_irq(&pdev->dev, low_irq, NULL,
@@ -427,13 +428,14 @@ static int db8500_thermal_probe(struct platform_device *pdev)
 		"dbx500_temp_low", pzone);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Failed to allocate temp low irq.\n");
-		return ret;
+		goto out_unlock;
 	}
 
 	high_irq = platform_get_irq_byname(pdev, "IRQ_HOTMON_HIGH");
 	if (high_irq < 0) {
 		dev_err(&pdev->dev, "Get IRQ_HOTMON_HIGH failed.\n");
-		return high_irq;
+		ret = high_irq;
+		goto out_unlock;
 	}
 
 	ret = devm_request_threaded_irq(&pdev->dev, high_irq, NULL,
@@ -441,15 +443,16 @@ static int db8500_thermal_probe(struct platform_device *pdev)
 		"dbx500_temp_high", pzone);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Failed to allocate temp high irq.\n");
-		return ret;
+		goto out_unlock;
 	}
 
 	pzone->therm_dev = thermal_zone_device_register("db8500_thermal_zone",
 		ptrips->num_trips, 0, pzone, &thdev_ops, NULL, 0, 0);
 
-	if (IS_ERR_OR_NULL(pzone->therm_dev)) {
+	if (IS_ERR(pzone->therm_dev)) {
 		dev_err(&pdev->dev, "Register thermal zone device failed.\n");
-		return PTR_ERR(pzone->therm_dev);
+		ret = PTR_ERR(pzone->therm_dev);
+		goto out_unlock;
 	}
 	dev_info(&pdev->dev, "Thermal zone device registered.\n");
 
@@ -461,9 +464,11 @@ static int db8500_thermal_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, pzone);
 	pzone->mode = THERMAL_DEVICE_ENABLED;
+
+out_unlock:
 	mutex_unlock(&pzone->th_lock);
 
-	return 0;
+	return ret;
 }
 
 static int db8500_thermal_remove(struct platform_device *pdev)
diff --git a/drivers/thermal/dove_thermal.c b/drivers/thermal/dove_thermal.c
index 3078c403b42d..4b15a5f270dc 100644
--- a/drivers/thermal/dove_thermal.c
+++ b/drivers/thermal/dove_thermal.c
@@ -107,12 +107,13 @@ static int dove_get_temp(struct thermal_zone_device *thermal,
 	}
 
 	/*
-	 * Calculate temperature. See Section 8.10.1 of 88AP510,
-	 * Documentation/arm/Marvell/README
+	 * Calculate temperature. According to Marvell internal
+	 * documentation the formula for this is:
+	 * Celsius = (322-reg)/1.3625
 	 */
 	reg = readl_relaxed(priv->sensor);
 	reg = (reg >> DOVE_THERMAL_TEMP_OFFSET) & DOVE_THERMAL_TEMP_MASK;
-	*temp = ((2281638UL - (7298*reg)) / 10);
+	*temp = ((3220000000UL - (10000000UL * reg)) / 13625);
 
 	return 0;
 }
diff --git a/drivers/thermal/exynos_thermal.c b/drivers/thermal/exynos_thermal.c
index 46568c078dee..d20ce9e61403 100644
--- a/drivers/thermal/exynos_thermal.c
+++ b/drivers/thermal/exynos_thermal.c
@@ -39,8 +39,6 @@
 #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
@@ -100,13 +98,13 @@
 #define IDLE_INTERVAL 10000
 #define MCELSIUS	1000
 
-#ifdef CONFIG_EXYNOS_THERMAL_EMUL
+#ifdef CONFIG_THERMAL_EMULATION
 #define EXYNOS_EMUL_TIME	0x57F0
 #define EXYNOS_EMUL_TIME_SHIFT	16
 #define EXYNOS_EMUL_DATA_SHIFT	8
 #define EXYNOS_EMUL_DATA_MASK	0xFF
 #define EXYNOS_EMUL_ENABLE	0x1
-#endif /* CONFIG_EXYNOS_THERMAL_EMUL */
+#endif /* CONFIG_THERMAL_EMULATION */
 
 /* CPU Zone information */
 #define PANIC_ZONE      4
@@ -145,6 +143,7 @@ struct	thermal_cooling_conf {
 struct thermal_sensor_conf {
 	char name[SENSOR_NAME_LEN];
 	int (*read_temperature)(void *data);
+	int (*write_emul_temp)(void *drv_data, unsigned long temp);
 	struct thermal_trip_point_conf trip_data;
 	struct thermal_cooling_conf cooling_data;
 	void *private_data;
@@ -242,26 +241,6 @@ static int exynos_get_crit_temp(struct thermal_zone_device *thermal,
 	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)
@@ -288,8 +267,8 @@ static int exynos_bind(struct thermal_zone_device *thermal,
 	/* 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)
+		level = cpufreq_cooling_get_level(0, clip_data->freq_clip_max);
+		if (level == THERMAL_CSTATE_INVALID)
 			return 0;
 		switch (GET_ZONE(i)) {
 		case MONITOR_ZONE:
@@ -369,6 +348,23 @@ static int exynos_get_temp(struct thermal_zone_device *thermal,
 	return 0;
 }
 
+/* Get temperature callback functions for thermal zone */
+static int exynos_set_emul_temp(struct thermal_zone_device *thermal,
+						unsigned long temp)
+{
+	void *data;
+	int ret = -EINVAL;
+
+	if (!th_zone->sensor_conf) {
+		pr_info("Temperature sensor not initialised\n");
+		return -EINVAL;
+	}
+	data = th_zone->sensor_conf->private_data;
+	if (th_zone->sensor_conf->write_emul_temp)
+		ret = th_zone->sensor_conf->write_emul_temp(data, temp);
+	return ret;
+}
+
 /* Get the temperature trend */
 static int exynos_get_trend(struct thermal_zone_device *thermal,
 			int trip, enum thermal_trend *trend)
@@ -392,6 +388,7 @@ static struct thermal_zone_device_ops const exynos_dev_ops = {
 	.bind = exynos_bind,
 	.unbind = exynos_unbind,
 	.get_temp = exynos_get_temp,
+	.set_emul_temp = exynos_set_emul_temp,
 	.get_trend = exynos_get_trend,
 	.get_mode = exynos_get_mode,
 	.set_mode = exynos_set_mode,
@@ -714,6 +711,47 @@ static int exynos_tmu_read(struct exynos_tmu_data *data)
 	return temp;
 }
 
+#ifdef CONFIG_THERMAL_EMULATION
+static int exynos_tmu_set_emulation(void *drv_data, unsigned long temp)
+{
+	struct exynos_tmu_data *data = drv_data;
+	unsigned int reg;
+	int ret = -EINVAL;
+
+	if (data->soc == SOC_ARCH_EXYNOS4210)
+		goto out;
+
+	if (temp && temp < MCELSIUS)
+		goto out;
+
+	mutex_lock(&data->lock);
+	clk_enable(data->clk);
+
+	reg = readl(data->base + EXYNOS_EMUL_CON);
+
+	if (temp) {
+		temp /= MCELSIUS;
+
+		reg = (EXYNOS_EMUL_TIME << EXYNOS_EMUL_TIME_SHIFT) |
+			(temp_to_code(data, temp)
+			 << EXYNOS_EMUL_DATA_SHIFT) | EXYNOS_EMUL_ENABLE;
+	} else {
+		reg &= ~EXYNOS_EMUL_ENABLE;
+	}
+
+	writel(reg, data->base + EXYNOS_EMUL_CON);
+
+	clk_disable(data->clk);
+	mutex_unlock(&data->lock);
+	return 0;
+out:
+	return ret;
+}
+#else
+static int exynos_tmu_set_emulation(void *drv_data,	unsigned long temp)
+	{ return -EINVAL; }
+#endif/*CONFIG_THERMAL_EMULATION*/
+
 static void exynos_tmu_work(struct work_struct *work)
 {
 	struct exynos_tmu_data *data = container_of(work,
@@ -747,6 +785,7 @@ static irqreturn_t exynos_tmu_irq(int irq, void *id)
 static struct thermal_sensor_conf exynos_sensor_conf = {
 	.name			= "exynos-therm",
 	.read_temperature	= (int (*)(void *))exynos_tmu_read,
+	.write_emul_temp	= exynos_tmu_set_emulation,
 };
 
 #if defined(CONFIG_CPU_EXYNOS4210)
@@ -816,6 +855,10 @@ static const struct of_device_id exynos_tmu_match[] = {
 		.data = (void *)EXYNOS4210_TMU_DRV_DATA,
 	},
 	{
+		.compatible = "samsung,exynos4412-tmu",
+		.data = (void *)EXYNOS_TMU_DRV_DATA,
+	},
+	{
 		.compatible = "samsung,exynos5250-tmu",
 		.data = (void *)EXYNOS_TMU_DRV_DATA,
 	},
@@ -853,93 +896,6 @@ static inline struct  exynos_tmu_platform_data *exynos_get_driver_data(
 			platform_get_device_id(pdev)->driver_data;
 }
 
-#ifdef CONFIG_EXYNOS_THERMAL_EMUL
-static ssize_t exynos_tmu_emulation_show(struct device *dev,
-					 struct device_attribute *attr,
-					 char *buf)
-{
-	struct platform_device *pdev = container_of(dev,
-					struct platform_device, dev);
-	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
-	unsigned int reg;
-	u8 temp_code;
-	int temp = 0;
-
-	if (data->soc == SOC_ARCH_EXYNOS4210)
-		goto out;
-
-	mutex_lock(&data->lock);
-	clk_enable(data->clk);
-	reg = readl(data->base + EXYNOS_EMUL_CON);
-	clk_disable(data->clk);
-	mutex_unlock(&data->lock);
-
-	if (reg & EXYNOS_EMUL_ENABLE) {
-		reg >>= EXYNOS_EMUL_DATA_SHIFT;
-		temp_code = reg & EXYNOS_EMUL_DATA_MASK;
-		temp = code_to_temp(data, temp_code);
-	}
-out:
-	return sprintf(buf, "%d\n", temp * MCELSIUS);
-}
-
-static ssize_t exynos_tmu_emulation_store(struct device *dev,
-					struct device_attribute *attr,
-					const char *buf, size_t count)
-{
-	struct platform_device *pdev = container_of(dev,
-					struct platform_device, dev);
-	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
-	unsigned int reg;
-	int temp;
-
-	if (data->soc == SOC_ARCH_EXYNOS4210)
-		goto out;
-
-	if (!sscanf(buf, "%d\n", &temp) || temp < 0)
-		return -EINVAL;
-
-	mutex_lock(&data->lock);
-	clk_enable(data->clk);
-
-	reg = readl(data->base + EXYNOS_EMUL_CON);
-
-	if (temp) {
-		/* Both CELSIUS and MCELSIUS type are available for input */
-		if (temp > MCELSIUS)
-			temp /= MCELSIUS;
-
-		reg = (EXYNOS_EMUL_TIME << EXYNOS_EMUL_TIME_SHIFT) |
-			(temp_to_code(data, (temp / MCELSIUS))
-			 << EXYNOS_EMUL_DATA_SHIFT) | EXYNOS_EMUL_ENABLE;
-	} else {
-		reg &= ~EXYNOS_EMUL_ENABLE;
-	}
-
-	writel(reg, data->base + EXYNOS_EMUL_CON);
-
-	clk_disable(data->clk);
-	mutex_unlock(&data->lock);
-
-out:
-	return count;
-}
-
-static DEVICE_ATTR(emulation, 0644, exynos_tmu_emulation_show,
-					exynos_tmu_emulation_store);
-static int create_emulation_sysfs(struct device *dev)
-{
-	return device_create_file(dev, &dev_attr_emulation);
-}
-static void remove_emulation_sysfs(struct device *dev)
-{
-	device_remove_file(dev, &dev_attr_emulation);
-}
-#else
-static inline int create_emulation_sysfs(struct device *dev) { return 0; }
-static inline void remove_emulation_sysfs(struct device *dev) {}
-#endif
-
 static int exynos_tmu_probe(struct platform_device *pdev)
 {
 	struct exynos_tmu_data *data;
@@ -985,12 +941,16 @@ static int exynos_tmu_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	data->clk = clk_get(NULL, "tmu_apbif");
+	data->clk = devm_clk_get(&pdev->dev, "tmu_apbif");
 	if (IS_ERR(data->clk)) {
 		dev_err(&pdev->dev, "Failed to get clock\n");
 		return  PTR_ERR(data->clk);
 	}
 
+	ret = clk_prepare(data->clk);
+	if (ret)
+		return ret;
+
 	if (pdata->type == SOC_ARCH_EXYNOS ||
 				pdata->type == SOC_ARCH_EXYNOS4210)
 		data->soc = pdata->type;
@@ -1039,14 +999,10 @@ static int exynos_tmu_probe(struct platform_device *pdev)
 		goto err_clk;
 	}
 
-	ret = create_emulation_sysfs(&pdev->dev);
-	if (ret)
-		dev_err(&pdev->dev, "Failed to create emulation mode sysfs node\n");
-
 	return 0;
 err_clk:
 	platform_set_drvdata(pdev, NULL);
-	clk_put(data->clk);
+	clk_unprepare(data->clk);
 	return ret;
 }
 
@@ -1054,13 +1010,11 @@ static int exynos_tmu_remove(struct platform_device *pdev)
 {
 	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
 
-	remove_emulation_sysfs(&pdev->dev);
-
 	exynos_tmu_control(pdev, false);
 
 	exynos_unregister_thermal();
 
-	clk_put(data->clk);
+	clk_unprepare(data->clk);
 
 	platform_set_drvdata(pdev, NULL);
 
diff --git a/drivers/thermal/fair_share.c b/drivers/thermal/fair_share.c
index 792479f2b64b..944ba2f340c8 100644
--- a/drivers/thermal/fair_share.c
+++ b/drivers/thermal/fair_share.c
@@ -22,9 +22,6 @@
  * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  */
 
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/module.h>
 #include <linux/thermal.h>
 
 #include "thermal_core.h"
@@ -111,23 +108,15 @@ static int fair_share_throttle(struct thermal_zone_device *tz, int trip)
 static struct thermal_governor thermal_gov_fair_share = {
 	.name		= "fair_share",
 	.throttle	= fair_share_throttle,
-	.owner		= THIS_MODULE,
 };
 
-static int __init thermal_gov_fair_share_init(void)
+int thermal_gov_fair_share_register(void)
 {
 	return thermal_register_governor(&thermal_gov_fair_share);
 }
 
-static void __exit thermal_gov_fair_share_exit(void)
+void thermal_gov_fair_share_unregister(void)
 {
 	thermal_unregister_governor(&thermal_gov_fair_share);
 }
 
-/* This should load after thermal framework */
-fs_initcall(thermal_gov_fair_share_init);
-module_exit(thermal_gov_fair_share_exit);
-
-MODULE_AUTHOR("Durgadoss R");
-MODULE_DESCRIPTION("A simple weight based thermal throttling governor");
-MODULE_LICENSE("GPL");
diff --git a/drivers/thermal/kirkwood_thermal.c b/drivers/thermal/kirkwood_thermal.c
index e5500edb5285..dfeceaffbc03 100644
--- a/drivers/thermal/kirkwood_thermal.c
+++ b/drivers/thermal/kirkwood_thermal.c
@@ -41,21 +41,21 @@ static int kirkwood_get_temp(struct thermal_zone_device *thermal,
 	reg = readl_relaxed(priv->sensor);
 
 	/* Valid check */
-	if (!(reg >> KIRKWOOD_THERMAL_VALID_OFFSET) &
-	    KIRKWOOD_THERMAL_VALID_MASK) {
+	if (!((reg >> KIRKWOOD_THERMAL_VALID_OFFSET) &
+	    KIRKWOOD_THERMAL_VALID_MASK)) {
 		dev_err(&thermal->device,
 			"Temperature sensor reading not valid\n");
 		return -EIO;
 	}
 
 	/*
-	 * Calculate temperature. See Section 8.10.1 of the 88AP510,
-	 * datasheet, which has the same sensor.
-	 * Documentation/arm/Marvell/README
+	 * Calculate temperature. According to Marvell internal
+	 * documentation the formula for this is:
+	 * Celsius = (322-reg)/1.3625
 	 */
 	reg = (reg >> KIRKWOOD_THERMAL_TEMP_OFFSET) &
 		KIRKWOOD_THERMAL_TEMP_MASK;
-	*temp = ((2281638UL - (7298*reg)) / 10);
+	*temp = ((3220000000UL - (10000000UL * reg)) / 13625);
 
 	return 0;
 }
diff --git a/drivers/thermal/rcar_thermal.c b/drivers/thermal/rcar_thermal.c
index 2cc5b6115e3e..8d7edd4c8228 100644
--- a/drivers/thermal/rcar_thermal.c
+++ b/drivers/thermal/rcar_thermal.c
@@ -24,6 +24,7 @@
 #include <linux/io.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
 #include <linux/reboot.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
@@ -377,6 +378,9 @@ static int rcar_thermal_probe(struct platform_device *pdev)
 	spin_lock_init(&common->lock);
 	common->dev = dev;
 
+	pm_runtime_enable(dev);
+	pm_runtime_get_sync(dev);
+
 	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (irq) {
 		int ret;
@@ -419,12 +423,15 @@ static int rcar_thermal_probe(struct platform_device *pdev)
 		priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
 		if (!priv) {
 			dev_err(dev, "Could not allocate priv\n");
-			return -ENOMEM;
+			ret = -ENOMEM;
+			goto error_unregister;
 		}
 
 		priv->base = devm_ioremap_resource(dev, res);
-		if (IS_ERR(priv->base))
-			return PTR_ERR(priv->base);
+		if (IS_ERR(priv->base)) {
+			ret = PTR_ERR(priv->base);
+			goto error_unregister;
+		}
 
 		priv->common = common;
 		priv->id = i;
@@ -443,10 +450,10 @@ static int rcar_thermal_probe(struct platform_device *pdev)
 			goto error_unregister;
 		}
 
-		list_move_tail(&priv->list, &common->head);
-
 		if (rcar_has_irq_support(priv))
 			rcar_thermal_irq_enable(priv);
+
+		list_move_tail(&priv->list, &common->head);
 	}
 
 	platform_set_drvdata(pdev, common);
@@ -456,8 +463,14 @@ static int rcar_thermal_probe(struct platform_device *pdev)
 	return 0;
 
 error_unregister:
-	rcar_thermal_for_each_priv(priv, common)
+	rcar_thermal_for_each_priv(priv, common) {
 		thermal_zone_device_unregister(priv->zone);
+		if (rcar_has_irq_support(priv))
+			rcar_thermal_irq_disable(priv);
+	}
+
+	pm_runtime_put_sync(dev);
+	pm_runtime_disable(dev);
 
 	return ret;
 }
@@ -465,13 +478,20 @@ error_unregister:
 static int rcar_thermal_remove(struct platform_device *pdev)
 {
 	struct rcar_thermal_common *common = platform_get_drvdata(pdev);
+	struct device *dev = &pdev->dev;
 	struct rcar_thermal_priv *priv;
 
-	rcar_thermal_for_each_priv(priv, common)
+	rcar_thermal_for_each_priv(priv, common) {
 		thermal_zone_device_unregister(priv->zone);
+		if (rcar_has_irq_support(priv))
+			rcar_thermal_irq_disable(priv);
+	}
 
 	platform_set_drvdata(pdev, NULL);
 
+	pm_runtime_put_sync(dev);
+	pm_runtime_disable(dev);
+
 	return 0;
 }
 
diff --git a/drivers/thermal/step_wise.c b/drivers/thermal/step_wise.c
index 407cde3211c1..4d4ddae1a991 100644
--- a/drivers/thermal/step_wise.c
+++ b/drivers/thermal/step_wise.c
@@ -22,9 +22,6 @@
  * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  */
 
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/module.h>
 #include <linux/thermal.h>
 
 #include "thermal_core.h"
@@ -59,9 +56,12 @@ static unsigned long get_target_state(struct thermal_instance *instance,
 
 	switch (trend) {
 	case THERMAL_TREND_RAISING:
-		if (throttle)
+		if (throttle) {
 			cur_state = cur_state < instance->upper ?
 				    (cur_state + 1) : instance->upper;
+			if (cur_state < instance->lower)
+				cur_state = instance->lower;
+		}
 		break;
 	case THERMAL_TREND_RAISE_FULL:
 		if (throttle)
@@ -71,8 +71,11 @@ static unsigned long get_target_state(struct thermal_instance *instance,
 		if (cur_state == instance->lower) {
 			if (!throttle)
 				cur_state = -1;
-		} else
+		} else {
 			cur_state -= 1;
+			if (cur_state > instance->upper)
+				cur_state = instance->upper;
+		}
 		break;
 	case THERMAL_TREND_DROP_FULL:
 		if (cur_state == instance->lower) {
@@ -180,23 +183,14 @@ static int step_wise_throttle(struct thermal_zone_device *tz, int trip)
 static struct thermal_governor thermal_gov_step_wise = {
 	.name		= "step_wise",
 	.throttle	= step_wise_throttle,
-	.owner		= THIS_MODULE,
 };
 
-static int __init thermal_gov_step_wise_init(void)
+int thermal_gov_step_wise_register(void)
 {
 	return thermal_register_governor(&thermal_gov_step_wise);
 }
 
-static void __exit thermal_gov_step_wise_exit(void)
+void thermal_gov_step_wise_unregister(void)
 {
 	thermal_unregister_governor(&thermal_gov_step_wise);
 }
-
-/* This should load after thermal framework */
-fs_initcall(thermal_gov_step_wise_init);
-module_exit(thermal_gov_step_wise_exit);
-
-MODULE_AUTHOR("Durgadoss R");
-MODULE_DESCRIPTION("A step-by-step thermal throttling governor");
-MODULE_LICENSE("GPL");
diff --git a/drivers/thermal/thermal_sys.c b/drivers/thermal/thermal_core.c
index 5b7863a03f98..d755440791b7 100644
--- a/drivers/thermal/thermal_sys.c
+++ b/drivers/thermal/thermal_core.c
@@ -40,7 +40,7 @@
 
 MODULE_AUTHOR("Zhang Rui");
 MODULE_DESCRIPTION("Generic thermal management sysfs support");
-MODULE_LICENSE("GPL");
+MODULE_LICENSE("GPL v2");
 
 static DEFINE_IDR(thermal_tz_idr);
 static DEFINE_IDR(thermal_cdev_idr);
@@ -99,7 +99,6 @@ int thermal_register_governor(struct thermal_governor *governor)
 
 	return err;
 }
-EXPORT_SYMBOL_GPL(thermal_register_governor);
 
 void thermal_unregister_governor(struct thermal_governor *governor)
 {
@@ -127,7 +126,6 @@ exit:
 	mutex_unlock(&thermal_governor_lock);
 	return;
 }
-EXPORT_SYMBOL_GPL(thermal_unregister_governor);
 
 static int get_idr(struct idr *idr, struct mutex *lock, int *id)
 {
@@ -371,16 +369,28 @@ static void handle_thermal_trip(struct thermal_zone_device *tz, int trip)
 	monitor_thermal_zone(tz);
 }
 
-static int thermal_zone_get_temp(struct thermal_zone_device *tz,
-				unsigned long *temp)
+/**
+ * thermal_zone_get_temp() - returns its the temperature of thermal zone
+ * @tz: a valid pointer to a struct thermal_zone_device
+ * @temp: a valid pointer to where to store the resulting temperature.
+ *
+ * When a valid thermal zone reference is passed, it will fetch its
+ * temperature and fill @temp.
+ *
+ * Return: On success returns 0, an error code otherwise
+ */
+int thermal_zone_get_temp(struct thermal_zone_device *tz, unsigned long *temp)
 {
-	int ret = 0;
+	int ret = -EINVAL;
 #ifdef CONFIG_THERMAL_EMULATION
 	int count;
 	unsigned long crit_temp = -1UL;
 	enum thermal_trip_type type;
 #endif
 
+	if (!tz || IS_ERR(tz))
+		goto exit;
+
 	mutex_lock(&tz->lock);
 
 	ret = tz->ops->get_temp(tz, temp);
@@ -404,8 +414,10 @@ static int thermal_zone_get_temp(struct thermal_zone_device *tz,
 skip_emul:
 #endif
 	mutex_unlock(&tz->lock);
+exit:
 	return ret;
 }
+EXPORT_SYMBOL_GPL(thermal_zone_get_temp);
 
 static void update_temperature(struct thermal_zone_device *tz)
 {
@@ -434,7 +446,7 @@ void thermal_zone_device_update(struct thermal_zone_device *tz)
 	for (count = 0; count < tz->trips; count++)
 		handle_thermal_trip(tz, count);
 }
-EXPORT_SYMBOL(thermal_zone_device_update);
+EXPORT_SYMBOL_GPL(thermal_zone_device_update);
 
 static void thermal_zone_device_check(struct work_struct *work)
 {
@@ -1097,13 +1109,23 @@ thermal_remove_hwmon_sysfs(struct thermal_zone_device *tz)
 #endif
 
 /**
- * thermal_zone_bind_cooling_device - bind a cooling device to a thermal zone
- * @tz:		thermal zone device
+ * thermal_zone_bind_cooling_device() - bind a cooling device to a thermal zone
+ * @tz:		pointer to struct thermal_zone_device
  * @trip:	indicates which trip point the cooling devices is
  *		associated with in this thermal zone.
- * @cdev:	thermal cooling device
+ * @cdev:	pointer to struct thermal_cooling_device
+ * @upper:	the Maximum cooling state for this trip point.
+ *		THERMAL_NO_LIMIT means no upper limit,
+ *		and the cooling device can be in max_state.
+ * @lower:	the Minimum cooling state can be used for this trip point.
+ *		THERMAL_NO_LIMIT means no lower limit,
+ *		and the cooling device can be in cooling state 0.
  *
+ * This interface function bind a thermal cooling device to the certain trip
+ * point of a thermal zone device.
  * This function is usually called in the thermal zone device .bind callback.
+ *
+ * Return: 0 on success, the proper error value otherwise.
  */
 int thermal_zone_bind_cooling_device(struct thermal_zone_device *tz,
 				     int trip,
@@ -1197,16 +1219,21 @@ free_mem:
 	kfree(dev);
 	return result;
 }
-EXPORT_SYMBOL(thermal_zone_bind_cooling_device);
+EXPORT_SYMBOL_GPL(thermal_zone_bind_cooling_device);
 
 /**
- * thermal_zone_unbind_cooling_device - unbind a cooling device from a thermal zone
- * @tz:		thermal zone device
+ * thermal_zone_unbind_cooling_device() - unbind a cooling device from a
+ *					  thermal zone.
+ * @tz:		pointer to a struct thermal_zone_device.
  * @trip:	indicates which trip point the cooling devices is
  *		associated with in this thermal zone.
- * @cdev:	thermal cooling device
+ * @cdev:	pointer to a struct thermal_cooling_device.
  *
+ * This interface function unbind a thermal cooling device from the certain
+ * trip point of a thermal zone device.
  * This function is usually called in the thermal zone device .unbind callback.
+ *
+ * Return: 0 on success, the proper error value otherwise.
  */
 int thermal_zone_unbind_cooling_device(struct thermal_zone_device *tz,
 				       int trip,
@@ -1237,7 +1264,7 @@ unbind:
 	kfree(pos);
 	return 0;
 }
-EXPORT_SYMBOL(thermal_zone_unbind_cooling_device);
+EXPORT_SYMBOL_GPL(thermal_zone_unbind_cooling_device);
 
 static void thermal_release(struct device *dev)
 {
@@ -1260,10 +1287,17 @@ static struct class thermal_class = {
 };
 
 /**
- * thermal_cooling_device_register - register a new thermal cooling device
+ * thermal_cooling_device_register() - register a new thermal cooling device
  * @type:	the thermal cooling device type.
  * @devdata:	device private data.
  * @ops:		standard thermal cooling devices callbacks.
+ *
+ * This interface function adds a new thermal cooling device (fan/processor/...)
+ * to /sys/class/thermal/ folder as cooling_device[0-*]. It tries to bind itself
+ * to all the thermal zone devices registered at the same time.
+ *
+ * Return: a pointer to the created struct thermal_cooling_device or an
+ * ERR_PTR. Caller must check return value with IS_ERR*() helpers.
  */
 struct thermal_cooling_device *
 thermal_cooling_device_register(char *type, void *devdata,
@@ -1289,7 +1323,7 @@ thermal_cooling_device_register(char *type, void *devdata,
 		return ERR_PTR(result);
 	}
 
-	strcpy(cdev->type, type ? : "");
+	strlcpy(cdev->type, type ? : "", sizeof(cdev->type));
 	mutex_init(&cdev->lock);
 	INIT_LIST_HEAD(&cdev->thermal_instances);
 	cdev->ops = ops;
@@ -1334,7 +1368,7 @@ unregister:
 	device_unregister(&cdev->device);
 	return ERR_PTR(result);
 }
-EXPORT_SYMBOL(thermal_cooling_device_register);
+EXPORT_SYMBOL_GPL(thermal_cooling_device_register);
 
 /**
  * thermal_cooling_device_unregister - removes the registered thermal cooling device
@@ -1394,7 +1428,7 @@ void thermal_cooling_device_unregister(struct thermal_cooling_device *cdev)
 	device_unregister(&cdev->device);
 	return;
 }
-EXPORT_SYMBOL(thermal_cooling_device_unregister);
+EXPORT_SYMBOL_GPL(thermal_cooling_device_unregister);
 
 void thermal_cdev_update(struct thermal_cooling_device *cdev)
 {
@@ -1420,7 +1454,7 @@ void thermal_cdev_update(struct thermal_cooling_device *cdev)
 EXPORT_SYMBOL(thermal_cdev_update);
 
 /**
- * notify_thermal_framework - Sensor drivers use this API to notify framework
+ * thermal_notify_framework - Sensor drivers use this API to notify framework
  * @tz:		thermal zone device
  * @trip:	indicates which trip point has been crossed
  *
@@ -1431,16 +1465,21 @@ EXPORT_SYMBOL(thermal_cdev_update);
  * The throttling policy is based on the configured platform data; if no
  * platform data is provided, this uses the step_wise throttling policy.
  */
-void notify_thermal_framework(struct thermal_zone_device *tz, int trip)
+void thermal_notify_framework(struct thermal_zone_device *tz, int trip)
 {
 	handle_thermal_trip(tz, trip);
 }
-EXPORT_SYMBOL(notify_thermal_framework);
+EXPORT_SYMBOL_GPL(thermal_notify_framework);
 
 /**
- * create_trip_attrs - create attributes for trip points
+ * create_trip_attrs() - create attributes for trip points
  * @tz:		the thermal zone device
  * @mask:	Writeable trip point bitmap.
+ *
+ * helper function to instantiate sysfs entries for every trip
+ * point and its properties of a struct thermal_zone_device.
+ *
+ * Return: 0 on success, the proper error value otherwise.
  */
 static int create_trip_attrs(struct thermal_zone_device *tz, int mask)
 {
@@ -1541,7 +1580,7 @@ static void remove_trip_attrs(struct thermal_zone_device *tz)
 }
 
 /**
- * thermal_zone_device_register - register a new thermal zone device
+ * thermal_zone_device_register() - register a new thermal zone device
  * @type:	the thermal zone device type
  * @trips:	the number of trip points the thermal zone support
  * @mask:	a bit string indicating the writeablility of trip points
@@ -1554,8 +1593,15 @@ static void remove_trip_attrs(struct thermal_zone_device *tz)
  *		   whether trip points have been crossed (0 for interrupt
  *		   driven systems)
  *
+ * This interface function adds a new thermal zone device (sensor) to
+ * /sys/class/thermal folder as thermal_zone[0-*]. It tries to bind all the
+ * thermal cooling devices registered at the same time.
  * thermal_zone_device_unregister() must be called when the device is no
  * longer needed. The passive cooling depends on the .get_trend() return value.
+ *
+ * Return: a pointer to the created struct thermal_zone_device or an
+ * in case of error, an ERR_PTR. Caller must check return value with
+ * IS_ERR*() helpers.
  */
 struct thermal_zone_device *thermal_zone_device_register(const char *type,
 	int trips, int mask, void *devdata,
@@ -1594,7 +1640,7 @@ struct thermal_zone_device *thermal_zone_device_register(const char *type,
 		return ERR_PTR(result);
 	}
 
-	strcpy(tz->type, type ? : "");
+	strlcpy(tz->type, type ? : "", sizeof(tz->type));
 	tz->ops = ops;
 	tz->tzp = tzp;
 	tz->device.class = &thermal_class;
@@ -1687,7 +1733,7 @@ unregister:
 	device_unregister(&tz->device);
 	return ERR_PTR(result);
 }
-EXPORT_SYMBOL(thermal_zone_device_register);
+EXPORT_SYMBOL_GPL(thermal_zone_device_register);
 
 /**
  * thermal_device_unregister - removes the registered thermal zone device
@@ -1754,7 +1800,45 @@ void thermal_zone_device_unregister(struct thermal_zone_device *tz)
 	device_unregister(&tz->device);
 	return;
 }
-EXPORT_SYMBOL(thermal_zone_device_unregister);
+EXPORT_SYMBOL_GPL(thermal_zone_device_unregister);
+
+/**
+ * thermal_zone_get_zone_by_name() - search for a zone and returns its ref
+ * @name: thermal zone name to fetch the temperature
+ *
+ * When only one zone is found with the passed name, returns a reference to it.
+ *
+ * Return: On success returns a reference to an unique thermal zone with
+ * matching name equals to @name, an ERR_PTR otherwise (-EINVAL for invalid
+ * paramenters, -ENODEV for not found and -EEXIST for multiple matches).
+ */
+struct thermal_zone_device *thermal_zone_get_zone_by_name(const char *name)
+{
+	struct thermal_zone_device *pos = NULL, *ref = ERR_PTR(-EINVAL);
+	unsigned int found = 0;
+
+	if (!name)
+		goto exit;
+
+	mutex_lock(&thermal_list_lock);
+	list_for_each_entry(pos, &thermal_tz_list, node)
+		if (!strnicmp(name, pos->type, THERMAL_NAME_LENGTH)) {
+			found++;
+			ref = pos;
+		}
+	mutex_unlock(&thermal_list_lock);
+
+	/* nothing has been found, thus an error code for it */
+	if (found == 0)
+		ref = ERR_PTR(-ENODEV);
+	else if (found > 1)
+	/* Success only when an unique zone is found */
+		ref = ERR_PTR(-EEXIST);
+
+exit:
+	return ref;
+}
+EXPORT_SYMBOL_GPL(thermal_zone_get_zone_by_name);
 
 #ifdef CONFIG_NET
 static struct genl_family thermal_event_genl_family = {
@@ -1832,7 +1916,7 @@ int thermal_generate_netlink_event(struct thermal_zone_device *tz,
 
 	return result;
 }
-EXPORT_SYMBOL(thermal_generate_netlink_event);
+EXPORT_SYMBOL_GPL(thermal_generate_netlink_event);
 
 static int genetlink_init(void)
 {
@@ -1858,30 +1942,69 @@ static inline int genetlink_init(void) { return 0; }
 static inline void genetlink_exit(void) {}
 #endif /* !CONFIG_NET */
 
+static int __init thermal_register_governors(void)
+{
+	int result;
+
+	result = thermal_gov_step_wise_register();
+	if (result)
+		return result;
+
+	result = thermal_gov_fair_share_register();
+	if (result)
+		return result;
+
+	return thermal_gov_user_space_register();
+}
+
+static void thermal_unregister_governors(void)
+{
+	thermal_gov_step_wise_unregister();
+	thermal_gov_fair_share_unregister();
+	thermal_gov_user_space_unregister();
+}
+
 static int __init thermal_init(void)
 {
-	int result = 0;
+	int result;
+
+	result = thermal_register_governors();
+	if (result)
+		goto error;
 
 	result = class_register(&thermal_class);
-	if (result) {
-		idr_destroy(&thermal_tz_idr);
-		idr_destroy(&thermal_cdev_idr);
-		mutex_destroy(&thermal_idr_lock);
-		mutex_destroy(&thermal_list_lock);
-		return result;
-	}
+	if (result)
+		goto unregister_governors;
+
 	result = genetlink_init();
+	if (result)
+		goto unregister_class;
+
+	return 0;
+
+unregister_governors:
+	thermal_unregister_governors();
+unregister_class:
+	class_unregister(&thermal_class);
+error:
+	idr_destroy(&thermal_tz_idr);
+	idr_destroy(&thermal_cdev_idr);
+	mutex_destroy(&thermal_idr_lock);
+	mutex_destroy(&thermal_list_lock);
+	mutex_destroy(&thermal_governor_lock);
 	return result;
 }
 
 static void __exit thermal_exit(void)
 {
+	genetlink_exit();
 	class_unregister(&thermal_class);
+	thermal_unregister_governors();
 	idr_destroy(&thermal_tz_idr);
 	idr_destroy(&thermal_cdev_idr);
 	mutex_destroy(&thermal_idr_lock);
 	mutex_destroy(&thermal_list_lock);
-	genetlink_exit();
+	mutex_destroy(&thermal_governor_lock);
 }
 
 fs_initcall(thermal_init);
diff --git a/drivers/thermal/thermal_core.h b/drivers/thermal/thermal_core.h
index 0d3205a18112..7cf2f6626251 100644
--- a/drivers/thermal/thermal_core.h
+++ b/drivers/thermal/thermal_core.h
@@ -50,4 +50,31 @@ struct thermal_instance {
 	struct list_head cdev_node; /* node in cdev->thermal_instances */
 };
 
+int thermal_register_governor(struct thermal_governor *);
+void thermal_unregister_governor(struct thermal_governor *);
+
+#ifdef CONFIG_THERMAL_GOV_STEP_WISE
+int thermal_gov_step_wise_register(void);
+void thermal_gov_step_wise_unregister(void);
+#else
+static inline int thermal_gov_step_wise_register(void) { return 0; }
+static inline void thermal_gov_step_wise_unregister(void) {}
+#endif /* CONFIG_THERMAL_GOV_STEP_WISE */
+
+#ifdef CONFIG_THERMAL_GOV_FAIR_SHARE
+int thermal_gov_fair_share_register(void);
+void thermal_gov_fair_share_unregister(void);
+#else
+static inline int thermal_gov_fair_share_register(void) { return 0; }
+static inline void thermal_gov_fair_share_unregister(void) {}
+#endif /* CONFIG_THERMAL_GOV_FAIR_SHARE */
+
+#ifdef CONFIG_THERMAL_GOV_USER_SPACE
+int thermal_gov_user_space_register(void);
+void thermal_gov_user_space_unregister(void);
+#else
+static inline int thermal_gov_user_space_register(void) { return 0; }
+static inline void thermal_gov_user_space_unregister(void) {}
+#endif /* CONFIG_THERMAL_GOV_USER_SPACE */
+
 #endif /* __THERMAL_CORE_H__ */
diff --git a/drivers/thermal/user_space.c b/drivers/thermal/user_space.c
index 6bbb380b6d19..10adcddc8821 100644
--- a/drivers/thermal/user_space.c
+++ b/drivers/thermal/user_space.c
@@ -22,9 +22,6 @@
  * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  */
 
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/module.h>
 #include <linux/thermal.h>
 
 #include "thermal_core.h"
@@ -46,23 +43,15 @@ static int notify_user_space(struct thermal_zone_device *tz, int trip)
 static struct thermal_governor thermal_gov_user_space = {
 	.name		= "user_space",
 	.throttle	= notify_user_space,
-	.owner		= THIS_MODULE,
 };
 
-static int __init thermal_gov_user_space_init(void)
+int thermal_gov_user_space_register(void)
 {
 	return thermal_register_governor(&thermal_gov_user_space);
 }
 
-static void __exit thermal_gov_user_space_exit(void)
+void thermal_gov_user_space_unregister(void)
 {
 	thermal_unregister_governor(&thermal_gov_user_space);
 }
 
-/* This should load after thermal framework */
-fs_initcall(thermal_gov_user_space_init);
-module_exit(thermal_gov_user_space_exit);
-
-MODULE_AUTHOR("Durgadoss R");
-MODULE_DESCRIPTION("A user space Thermal notifier");
-MODULE_LICENSE("GPL");
diff --git a/drivers/tty/pty.c b/drivers/tty/pty.c
index a62798fcc014..59bfaecc4e14 100644
--- a/drivers/tty/pty.c
+++ b/drivers/tty/pty.c
@@ -681,6 +681,9 @@ static int ptmx_open(struct inode *inode, struct file *filp)
 
 	nonseekable_open(inode, filp);
 
+	/* We refuse fsnotify events on ptmx, since it's a shared resource */
+	filp->f_mode |= FMODE_NONOTIFY;
+
 	retval = tty_alloc_file(filp);
 	if (retval)
 		return retval;
diff --git a/drivers/tty/serial/68328serial.c b/drivers/tty/serial/68328serial.c
index ef2e08e9b590..5dc9c4bfa66e 100644
--- a/drivers/tty/serial/68328serial.c
+++ b/drivers/tty/serial/68328serial.c
@@ -14,7 +14,6 @@
  * 2.4/2.5 port                 David McCullough
  */
 
-#include <asm/dbg.h>
 #include <linux/module.h>
 #include <linux/errno.h>
 #include <linux/serial.h>
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index b2e9e177a354..8ab70a620919 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -267,7 +267,7 @@ static void pl011_sgbuf_free(struct dma_chan *chan, struct pl011_sgbuf *sg,
 	}
 }
 
-static void pl011_dma_probe_initcall(struct uart_amba_port *uap)
+static void pl011_dma_probe_initcall(struct device *dev, struct uart_amba_port *uap)
 {
 	/* DMA is the sole user of the platform data right now */
 	struct amba_pl011_data *plat = uap->port.dev->platform_data;
@@ -281,20 +281,25 @@ static void pl011_dma_probe_initcall(struct uart_amba_port *uap)
 	struct dma_chan *chan;
 	dma_cap_mask_t mask;
 
-	/* We need platform data */
-	if (!plat || !plat->dma_filter) {
-		dev_info(uap->port.dev, "no DMA platform data\n");
-		return;
-	}
+	chan = dma_request_slave_channel(dev, "tx");
 
-	/* Try to acquire a generic DMA engine slave TX channel */
-	dma_cap_zero(mask);
-	dma_cap_set(DMA_SLAVE, mask);
-
-	chan = dma_request_channel(mask, plat->dma_filter, plat->dma_tx_param);
 	if (!chan) {
-		dev_err(uap->port.dev, "no TX DMA channel!\n");
-		return;
+		/* We need platform data */
+		if (!plat || !plat->dma_filter) {
+			dev_info(uap->port.dev, "no DMA platform data\n");
+			return;
+		}
+
+		/* Try to acquire a generic DMA engine slave TX channel */
+		dma_cap_zero(mask);
+		dma_cap_set(DMA_SLAVE, mask);
+
+		chan = dma_request_channel(mask, plat->dma_filter,
+						plat->dma_tx_param);
+		if (!chan) {
+			dev_err(uap->port.dev, "no TX DMA channel!\n");
+			return;
+		}
 	}
 
 	dmaengine_slave_config(chan, &tx_conf);
@@ -304,7 +309,18 @@ static void pl011_dma_probe_initcall(struct uart_amba_port *uap)
 		 dma_chan_name(uap->dmatx.chan));
 
 	/* Optionally make use of an RX channel as well */
-	if (plat->dma_rx_param) {
+	chan = dma_request_slave_channel(dev, "rx");
+	
+	if (!chan && plat->dma_rx_param) {
+		chan = dma_request_channel(mask, plat->dma_filter, plat->dma_rx_param);
+
+		if (!chan) {
+			dev_err(uap->port.dev, "no RX DMA channel!\n");
+			return;
+		}
+	}
+
+	if (chan) {
 		struct dma_slave_config rx_conf = {
 			.src_addr = uap->port.mapbase + UART01x_DR,
 			.src_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE,
@@ -313,12 +329,6 @@ static void pl011_dma_probe_initcall(struct uart_amba_port *uap)
 			.device_fc = false,
 		};
 
-		chan = dma_request_channel(mask, plat->dma_filter, plat->dma_rx_param);
-		if (!chan) {
-			dev_err(uap->port.dev, "no RX DMA channel!\n");
-			return;
-		}
-
 		dmaengine_slave_config(chan, &rx_conf);
 		uap->dmarx.chan = chan;
 
@@ -360,6 +370,7 @@ static void pl011_dma_probe_initcall(struct uart_amba_port *uap)
 struct dma_uap {
 	struct list_head node;
 	struct uart_amba_port *uap;
+	struct device *dev;
 };
 
 static LIST_HEAD(pl011_dma_uarts);
@@ -370,7 +381,7 @@ static int __init pl011_dma_initcall(void)
 
 	list_for_each_safe(node, tmp, &pl011_dma_uarts) {
 		struct dma_uap *dmau = list_entry(node, struct dma_uap, node);
-		pl011_dma_probe_initcall(dmau->uap);
+		pl011_dma_probe_initcall(dmau->dev, dmau->uap);
 		list_del(node);
 		kfree(dmau);
 	}
@@ -379,18 +390,19 @@ static int __init pl011_dma_initcall(void)
 
 device_initcall(pl011_dma_initcall);
 
-static void pl011_dma_probe(struct uart_amba_port *uap)
+static void pl011_dma_probe(struct device *dev, struct uart_amba_port *uap)
 {
 	struct dma_uap *dmau = kzalloc(sizeof(struct dma_uap), GFP_KERNEL);
 	if (dmau) {
 		dmau->uap = uap;
+		dmau->dev = dev;
 		list_add_tail(&dmau->node, &pl011_dma_uarts);
 	}
 }
 #else
-static void pl011_dma_probe(struct uart_amba_port *uap)
+static void pl011_dma_probe(struct device *dev, struct uart_amba_port *uap)
 {
-	pl011_dma_probe_initcall(uap);
+	pl011_dma_probe_initcall(dev, uap);
 }
 #endif
 
@@ -1096,7 +1108,7 @@ static inline bool pl011_dma_rx_running(struct uart_amba_port *uap)
 
 #else
 /* Blank functions if the DMA engine is not available */
-static inline void pl011_dma_probe(struct uart_amba_port *uap)
+static inline void pl011_dma_probe(struct device *dev, struct uart_amba_port *uap)
 {
 }
 
@@ -2155,7 +2167,7 @@ static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
 	uap->port.ops = &amba_pl011_pops;
 	uap->port.flags = UPF_BOOT_AUTOCONF;
 	uap->port.line = i;
-	pl011_dma_probe(uap);
+	pl011_dma_probe(&dev->dev, uap);
 
 	/* Ensure interrupts from this UART are masked and cleared */
 	writew(0, uap->port.membase + UART011_IMSC);
diff --git a/drivers/tty/serial/bcm63xx_uart.c b/drivers/tty/serial/bcm63xx_uart.c
index 52a3ecd40421..6fa2ae77fffd 100644
--- a/drivers/tty/serial/bcm63xx_uart.c
+++ b/drivers/tty/serial/bcm63xx_uart.c
@@ -30,7 +30,6 @@
 #include <linux/serial.h>
 #include <linux/serial_core.h>
 
-#include <bcm63xx_clk.h>
 #include <bcm63xx_irq.h>
 #include <bcm63xx_regs.h>
 #include <bcm63xx_io.h>
diff --git a/drivers/tty/serial/mxs-auart.c b/drivers/tty/serial/mxs-auart.c
index 62e7d3b015a1..4f5f161896a1 100644
--- a/drivers/tty/serial/mxs-auart.c
+++ b/drivers/tty/serial/mxs-auart.c
@@ -35,7 +35,7 @@
 #include <linux/pinctrl/consumer.h>
 #include <linux/of_device.h>
 #include <linux/dma-mapping.h>
-#include <linux/fsl/mxs-dma.h>
+#include <linux/dmaengine.h>
 
 #include <asm/cacheflush.h>
 
@@ -148,11 +148,6 @@ struct mxs_auart_port {
 	struct device *dev;
 
 	/* for DMA */
-	struct mxs_dma_data dma_data;
-	int dma_channel_rx, dma_channel_tx;
-	int dma_irq_rx, dma_irq_tx;
-	int dma_channel;
-
 	struct scatterlist tx_sgl;
 	struct dma_chan	*tx_dma_chan;
 	void *tx_dma_buf;
@@ -440,20 +435,6 @@ static u32 mxs_auart_get_mctrl(struct uart_port *u)
 	return mctrl;
 }
 
-static bool mxs_auart_dma_filter(struct dma_chan *chan, void *param)
-{
-	struct mxs_auart_port *s = param;
-
-	if (!mxs_dma_is_apbx(chan))
-		return false;
-
-	if (s->dma_channel == chan->chan_id) {
-		chan->private = &s->dma_data;
-		return true;
-	}
-	return false;
-}
-
 static int mxs_auart_dma_prep_rx(struct mxs_auart_port *s);
 static void dma_rx_callback(void *arg)
 {
@@ -545,21 +526,11 @@ static void mxs_auart_dma_exit(struct mxs_auart_port *s)
 
 static int mxs_auart_dma_init(struct mxs_auart_port *s)
 {
-	dma_cap_mask_t mask;
-
 	if (auart_dma_enabled(s))
 		return 0;
 
-	/* We do not get the right DMA channels. */
-	if (s->dma_channel_rx == -1 || s->dma_channel_tx == -1)
-		return -EINVAL;
-
 	/* init for RX */
-	dma_cap_zero(mask);
-	dma_cap_set(DMA_SLAVE, mask);
-	s->dma_channel = s->dma_channel_rx;
-	s->dma_data.chan_irq = s->dma_irq_rx;
-	s->rx_dma_chan = dma_request_channel(mask, mxs_auart_dma_filter, s);
+	s->rx_dma_chan = dma_request_slave_channel(s->dev, "rx");
 	if (!s->rx_dma_chan)
 		goto err_out;
 	s->rx_dma_buf = kzalloc(UART_XMIT_SIZE, GFP_KERNEL | GFP_DMA);
@@ -567,9 +538,7 @@ static int mxs_auart_dma_init(struct mxs_auart_port *s)
 		goto err_out;
 
 	/* init for TX */
-	s->dma_channel = s->dma_channel_tx;
-	s->dma_data.chan_irq = s->dma_irq_tx;
-	s->tx_dma_chan = dma_request_channel(mask, mxs_auart_dma_filter, s);
+	s->tx_dma_chan = dma_request_slave_channel(s->dev, "tx");
 	if (!s->tx_dma_chan)
 		goto err_out;
 	s->tx_dma_buf = kzalloc(UART_XMIT_SIZE, GFP_KERNEL | GFP_DMA);
@@ -1020,7 +989,6 @@ static int serial_mxs_probe_dt(struct mxs_auart_port *s,
 		struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
-	u32 dma_channel[2];
 	int ret;
 
 	if (!np)
@@ -1034,20 +1002,8 @@ static int serial_mxs_probe_dt(struct mxs_auart_port *s,
 	}
 	s->port.line = ret;
 
-	s->dma_irq_rx = platform_get_irq(pdev, 1);
-	s->dma_irq_tx = platform_get_irq(pdev, 2);
+	s->flags |= MXS_AUART_DMA_CONFIG;
 
-	ret = of_property_read_u32_array(np, "fsl,auart-dma-channel",
-					dma_channel, 2);
-	if (ret == 0) {
-		s->dma_channel_rx = dma_channel[0];
-		s->dma_channel_tx = dma_channel[1];
-
-		s->flags |= MXS_AUART_DMA_CONFIG;
-	} else {
-		s->dma_channel_rx = -1;
-		s->dma_channel_tx = -1;
-	}
 	return 0;
 }
 
diff --git a/drivers/tty/serial/serial_core.c b/drivers/tty/serial/serial_core.c
index 8fbb6d22cdc8..f87dbfd32770 100644
--- a/drivers/tty/serial/serial_core.c
+++ b/drivers/tty/serial/serial_core.c
@@ -1711,7 +1711,7 @@ static int uart_proc_show(struct seq_file *m, void *v)
 
 static int uart_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, uart_proc_show, PDE(inode)->data);
+	return single_open(file, uart_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations uart_proc_fops = {
diff --git a/drivers/tty/serial/sunsu.c b/drivers/tty/serial/sunsu.c
index 451687cb9685..0d8465728473 100644
--- a/drivers/tty/serial/sunsu.c
+++ b/drivers/tty/serial/sunsu.c
@@ -1592,6 +1592,7 @@ static int __init sunsu_init(void)
 
 static void __exit sunsu_exit(void)
 {
+	platform_driver_unregister(&su_driver);
 	if (sunsu_reg.nr)
 		sunserial_unregister_minors(&sunsu_reg, sunsu_reg.nr);
 }
diff --git a/drivers/tty/sysrq.c b/drivers/tty/sysrq.c
index 0a0de333c765..b51c15408ff3 100644
--- a/drivers/tty/sysrq.c
+++ b/drivers/tty/sysrq.c
@@ -43,6 +43,7 @@
 #include <linux/input.h>
 #include <linux/uaccess.h>
 #include <linux/moduleparam.h>
+#include <linux/jiffies.h>
 
 #include <asm/ptrace.h>
 #include <asm/irq_regs.h>
@@ -51,6 +52,9 @@
 static int __read_mostly sysrq_enabled = SYSRQ_DEFAULT_ENABLE;
 static bool __read_mostly sysrq_always_enabled;
 
+unsigned short platform_sysrq_reset_seq[] __weak = { KEY_RESERVED };
+int sysrq_reset_downtime_ms __weak;
+
 static bool sysrq_on(void)
 {
 	return sysrq_enabled || sysrq_always_enabled;
@@ -586,6 +590,7 @@ struct sysrq_state {
 	int reset_seq_len;
 	int reset_seq_cnt;
 	int reset_seq_version;
+	struct timer_list keyreset_timer;
 };
 
 #define SYSRQ_KEY_RESET_MAX	20 /* Should be plenty */
@@ -619,29 +624,51 @@ static void sysrq_parse_reset_sequence(struct sysrq_state *state)
 	state->reset_seq_version = sysrq_reset_seq_version;
 }
 
-static bool sysrq_detect_reset_sequence(struct sysrq_state *state,
+static void sysrq_do_reset(unsigned long dummy)
+{
+	__handle_sysrq(sysrq_xlate[KEY_B], false);
+}
+
+static void sysrq_handle_reset_request(struct sysrq_state *state)
+{
+	if (sysrq_reset_downtime_ms)
+		mod_timer(&state->keyreset_timer,
+			jiffies + msecs_to_jiffies(sysrq_reset_downtime_ms));
+	else
+		sysrq_do_reset(0);
+}
+
+static void sysrq_detect_reset_sequence(struct sysrq_state *state,
 					unsigned int code, int value)
 {
 	if (!test_bit(code, state->reset_keybit)) {
 		/*
 		 * Pressing any key _not_ in reset sequence cancels
-		 * the reset sequence.
+		 * the reset sequence.  Also cancelling the timer in
+		 * case additional keys were pressed after a reset
+		 * has been requested.
 		 */
-		if (value && state->reset_seq_cnt)
+		if (value && state->reset_seq_cnt) {
 			state->reset_canceled = true;
+			del_timer(&state->keyreset_timer);
+		}
 	} else if (value == 0) {
-		/* key release */
+		/*
+		 * Key release - all keys in the reset sequence need
+		 * to be pressed and held for the reset timeout
+		 * to hold.
+		 */
+		del_timer(&state->keyreset_timer);
+
 		if (--state->reset_seq_cnt == 0)
 			state->reset_canceled = false;
 	} else if (value == 1) {
 		/* key press, not autorepeat */
 		if (++state->reset_seq_cnt == state->reset_seq_len &&
 		    !state->reset_canceled) {
-			return true;
+			sysrq_handle_reset_request(state);
 		}
 	}
-
-	return false;
 }
 
 static void sysrq_reinject_alt_sysrq(struct work_struct *work)
@@ -748,10 +775,8 @@ static bool sysrq_handle_keypress(struct sysrq_state *sysrq,
 		if (was_active)
 			schedule_work(&sysrq->reinject_work);
 
-		if (sysrq_detect_reset_sequence(sysrq, code, value)) {
-			/* Force emergency reboot */
-			__handle_sysrq(sysrq_xlate[KEY_B], false);
-		}
+		/* Check for reset sequence */
+		sysrq_detect_reset_sequence(sysrq, code, value);
 
 	} else if (value == 0 && test_and_clear_bit(code, sysrq->key_down)) {
 		/*
@@ -812,6 +837,7 @@ static int sysrq_connect(struct input_handler *handler,
 	sysrq->handle.handler = handler;
 	sysrq->handle.name = "sysrq";
 	sysrq->handle.private = sysrq;
+	setup_timer(&sysrq->keyreset_timer, sysrq_do_reset, 0);
 
 	error = input_register_handle(&sysrq->handle);
 	if (error) {
@@ -841,6 +867,7 @@ static void sysrq_disconnect(struct input_handle *handle)
 
 	input_close_device(handle);
 	cancel_work_sync(&sysrq->reinject_work);
+	del_timer_sync(&sysrq->keyreset_timer);
 	input_unregister_handle(handle);
 	kfree(sysrq);
 }
@@ -870,8 +897,6 @@ static struct input_handler sysrq_handler = {
 
 static bool sysrq_handler_registered;
 
-unsigned short platform_sysrq_reset_seq[] __weak = { KEY_RESERVED };
-
 static inline void sysrq_register_handler(void)
 {
 	unsigned short key;
@@ -931,6 +956,8 @@ static struct kernel_param_ops param_ops_sysrq_reset_seq = {
 module_param_array_named(reset_seq, sysrq_reset_seq, sysrq_reset_seq,
 			 &sysrq_reset_seq_len, 0644);
 
+module_param_named(sysrq_downtime_ms, sysrq_reset_downtime_ms, int, 0644);
+
 #else
 
 static inline void sysrq_register_handler(void)
diff --git a/drivers/tty/tty_io.c b/drivers/tty/tty_io.c
index 97ebc8c5864e..6464029e4860 100644
--- a/drivers/tty/tty_io.c
+++ b/drivers/tty/tty_io.c
@@ -988,10 +988,10 @@ void start_tty(struct tty_struct *tty)
 
 EXPORT_SYMBOL(start_tty);
 
+/* We limit tty time update visibility to every 8 seconds or so. */
 static void tty_update_time(struct timespec *time)
 {
-	unsigned long sec = get_seconds();
-	sec -= sec % 60;
+	unsigned long sec = get_seconds() & ~7;
 	if ((long)(sec - time->tv_sec) > 0)
 		time->tv_sec = sec;
 }
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index 171d7a9df3ae..9b1cbcf8fb7f 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -962,6 +962,10 @@ static int acm_probe(struct usb_interface *intf,
 
 	/* normal quirks */
 	quirks = (unsigned long)id->driver_info;
+
+	if (quirks == IGNORE_DEVICE)
+		return -ENODEV;
+
 	num_rx_buf = (quirks == SINGLE_RX_URB) ? 1 : ACM_NR;
 
 	/* handle quirks deadly to normal probing*/
@@ -1675,6 +1679,15 @@ static const struct usb_device_id acm_ids[] = {
 	.driver_info = NO_DATA_INTERFACE,
 	},
 
+#if IS_ENABLED(CONFIG_INPUT_IMS_PCU)
+	{ USB_DEVICE(0x04d8, 0x0082),	/* Application mode */
+	.driver_info = IGNORE_DEVICE,
+	},
+	{ USB_DEVICE(0x04d8, 0x0083),	/* Bootloader mode */
+	.driver_info = IGNORE_DEVICE,
+	},
+#endif
+
 	/* control interfaces without any protocol set */
 	{ USB_INTERFACE_INFO(USB_CLASS_COMM, USB_CDC_SUBCLASS_ACM,
 		USB_CDC_PROTO_NONE) },
diff --git a/drivers/usb/class/cdc-acm.h b/drivers/usb/class/cdc-acm.h
index 35ef887b7417..0f76e4af600e 100644
--- a/drivers/usb/class/cdc-acm.h
+++ b/drivers/usb/class/cdc-acm.h
@@ -128,3 +128,4 @@ struct acm {
 #define NO_CAP_LINE			4
 #define NOT_A_MODEM			8
 #define NO_DATA_INTERFACE		16
+#define IGNORE_DEVICE			32
diff --git a/drivers/usb/gadget/at91_udc.c b/drivers/usb/gadget/at91_udc.c
index a690d64217f4..073b938f9135 100644
--- a/drivers/usb/gadget/at91_udc.c
+++ b/drivers/usb/gadget/at91_udc.c
@@ -221,7 +221,7 @@ static int proc_udc_show(struct seq_file *s, void *unused)
 
 static int proc_udc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, proc_udc_show, PDE(inode)->data);
+	return single_open(file, proc_udc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations proc_ops = {
diff --git a/drivers/usb/gadget/f_fs.c b/drivers/usb/gadget/f_fs.c
index c377ff84bf2c..f394f295d63d 100644
--- a/drivers/usb/gadget/f_fs.c
+++ b/drivers/usb/gadget/f_fs.c
@@ -727,7 +727,6 @@ static long ffs_ep0_ioctl(struct file *file, unsigned code, unsigned long value)
 }
 
 static const struct file_operations ffs_ep0_operations = {
-	.owner =	THIS_MODULE,
 	.llseek =	no_llseek,
 
 	.open =		ffs_ep0_open,
@@ -947,7 +946,6 @@ static long ffs_epfile_ioctl(struct file *file, unsigned code,
 }
 
 static const struct file_operations ffs_epfile_operations = {
-	.owner =	THIS_MODULE,
 	.llseek =	no_llseek,
 
 	.open =		ffs_epfile_open,
diff --git a/drivers/usb/gadget/fsl_udc_core.c b/drivers/usb/gadget/fsl_udc_core.c
index 7c2a101d19ac..a766a4ca1cb7 100644
--- a/drivers/usb/gadget/fsl_udc_core.c
+++ b/drivers/usb/gadget/fsl_udc_core.c
@@ -2010,47 +2010,37 @@ static int fsl_udc_stop(struct usb_gadget *g,
 
 static const char proc_filename[] = "driver/fsl_usb2_udc";
 
-static int fsl_proc_read(char *page, char **start, off_t off, int count,
-		int *eof, void *_dev)
+static int fsl_proc_read(struct seq_file *m, void *v)
 {
-	char *buf = page;
-	char *next = buf;
-	unsigned size = count;
 	unsigned long flags;
-	int t, i;
+	int i;
 	u32 tmp_reg;
 	struct fsl_ep *ep = NULL;
 	struct fsl_req *req;
 
 	struct fsl_udc *udc = udc_controller;
-	if (off != 0)
-		return 0;
 
 	spin_lock_irqsave(&udc->lock, flags);
 
 	/* ------basic driver information ---- */
-	t = scnprintf(next, size,
+	seq_printf(m,
 			DRIVER_DESC "\n"
 			"%s version: %s\n"
 			"Gadget driver: %s\n\n",
 			driver_name, DRIVER_VERSION,
 			udc->driver ? udc->driver->driver.name : "(none)");
-	size -= t;
-	next += t;
 
 	/* ------ DR Registers ----- */
 	tmp_reg = fsl_readl(&dr_regs->usbcmd);
-	t = scnprintf(next, size,
+	seq_printf(m,
 			"USBCMD reg:\n"
 			"SetupTW: %d\n"
 			"Run/Stop: %s\n\n",
 			(tmp_reg & USB_CMD_SUTW) ? 1 : 0,
 			(tmp_reg & USB_CMD_RUN_STOP) ? "Run" : "Stop");
-	size -= t;
-	next += t;
 
 	tmp_reg = fsl_readl(&dr_regs->usbsts);
-	t = scnprintf(next, size,
+	seq_printf(m,
 			"USB Status Reg:\n"
 			"Dr Suspend: %d Reset Received: %d System Error: %s "
 			"USB Error Interrupt: %s\n\n",
@@ -2058,11 +2048,9 @@ static int fsl_proc_read(char *page, char **start, off_t off, int count,
 			(tmp_reg & USB_STS_RESET) ? 1 : 0,
 			(tmp_reg & USB_STS_SYS_ERR) ? "Err" : "Normal",
 			(tmp_reg & USB_STS_ERR) ? "Err detected" : "No err");
-	size -= t;
-	next += t;
 
 	tmp_reg = fsl_readl(&dr_regs->usbintr);
-	t = scnprintf(next, size,
+	seq_printf(m,
 			"USB Interrupt Enable Reg:\n"
 			"Sleep Enable: %d SOF Received Enable: %d "
 			"Reset Enable: %d\n"
@@ -2076,33 +2064,25 @@ static int fsl_proc_read(char *page, char **start, off_t off, int count,
 			(tmp_reg & USB_INTR_PTC_DETECT_EN) ? 1 : 0,
 			(tmp_reg & USB_INTR_ERR_INT_EN) ? 1 : 0,
 			(tmp_reg & USB_INTR_INT_EN) ? 1 : 0);
-	size -= t;
-	next += t;
 
 	tmp_reg = fsl_readl(&dr_regs->frindex);
-	t = scnprintf(next, size,
+	seq_printf(m,
 			"USB Frame Index Reg: Frame Number is 0x%x\n\n",
 			(tmp_reg & USB_FRINDEX_MASKS));
-	size -= t;
-	next += t;
 
 	tmp_reg = fsl_readl(&dr_regs->deviceaddr);
-	t = scnprintf(next, size,
+	seq_printf(m,
 			"USB Device Address Reg: Device Addr is 0x%x\n\n",
 			(tmp_reg & USB_DEVICE_ADDRESS_MASK));
-	size -= t;
-	next += t;
 
 	tmp_reg = fsl_readl(&dr_regs->endpointlistaddr);
-	t = scnprintf(next, size,
+	seq_printf(m,
 			"USB Endpoint List Address Reg: "
 			"Device Addr is 0x%x\n\n",
 			(tmp_reg & USB_EP_LIST_ADDRESS_MASK));
-	size -= t;
-	next += t;
 
 	tmp_reg = fsl_readl(&dr_regs->portsc1);
-	t = scnprintf(next, size,
+	seq_printf(m,
 		"USB Port Status&Control Reg:\n"
 		"Port Transceiver Type : %s Port Speed: %s\n"
 		"PHY Low Power Suspend: %s Port Reset: %s "
@@ -2111,7 +2091,7 @@ static int fsl_proc_read(char *page, char **start, off_t off, int count,
 		"Port Enable/Disable Change: %s\n"
 		"Port Enabled/Disabled: %s "
 		"Current Connect Status: %s\n\n", ( {
-			char *s;
+			const char *s;
 			switch (tmp_reg & PORTSCX_PTS_FSLS) {
 			case PORTSCX_PTS_UTMI:
 				s = "UTMI"; break;
@@ -2137,13 +2117,11 @@ static int fsl_proc_read(char *page, char **start, off_t off, int count,
 		"Not correct",
 		(tmp_reg & PORTSCX_CURRENT_CONNECT_STATUS) ?
 		"Attached" : "Not-Att");
-	size -= t;
-	next += t;
 
 	tmp_reg = fsl_readl(&dr_regs->usbmode);
-	t = scnprintf(next, size,
+	seq_printf(m,
 			"USB Mode Reg: Controller Mode is: %s\n\n", ( {
-				char *s;
+				const char *s;
 				switch (tmp_reg & USB_MODE_CTRL_MODE_HOST) {
 				case USB_MODE_CTRL_MODE_IDLE:
 					s = "Idle"; break;
@@ -2156,103 +2134,87 @@ static int fsl_proc_read(char *page, char **start, off_t off, int count,
 				}
 				s;
 			} ));
-	size -= t;
-	next += t;
 
 	tmp_reg = fsl_readl(&dr_regs->endptsetupstat);
-	t = scnprintf(next, size,
+	seq_printf(m,
 			"Endpoint Setup Status Reg: SETUP on ep 0x%x\n\n",
 			(tmp_reg & EP_SETUP_STATUS_MASK));
-	size -= t;
-	next += t;
 
 	for (i = 0; i < udc->max_ep / 2; i++) {
 		tmp_reg = fsl_readl(&dr_regs->endptctrl[i]);
-		t = scnprintf(next, size, "EP Ctrl Reg [0x%x]: = [0x%x]\n",
-				i, tmp_reg);
-		size -= t;
-		next += t;
+		seq_printf(m, "EP Ctrl Reg [0x%x]: = [0x%x]\n", i, tmp_reg);
 	}
 	tmp_reg = fsl_readl(&dr_regs->endpointprime);
-	t = scnprintf(next, size, "EP Prime Reg = [0x%x]\n\n", tmp_reg);
-	size -= t;
-	next += t;
+	seq_printf(m, "EP Prime Reg = [0x%x]\n\n", tmp_reg);
 
 #ifndef CONFIG_ARCH_MXC
 	if (udc->pdata->have_sysif_regs) {
 		tmp_reg = usb_sys_regs->snoop1;
-		t = scnprintf(next, size, "Snoop1 Reg : = [0x%x]\n\n", tmp_reg);
-		size -= t;
-		next += t;
+		seq_printf(m, "Snoop1 Reg : = [0x%x]\n\n", tmp_reg);
 
 		tmp_reg = usb_sys_regs->control;
-		t = scnprintf(next, size, "General Control Reg : = [0x%x]\n\n",
-				tmp_reg);
-		size -= t;
-		next += t;
+		seq_printf(m, "General Control Reg : = [0x%x]\n\n", tmp_reg);
 	}
 #endif
 
 	/* ------fsl_udc, fsl_ep, fsl_request structure information ----- */
 	ep = &udc->eps[0];
-	t = scnprintf(next, size, "For %s Maxpkt is 0x%x index is 0x%x\n",
+	seq_printf(m, "For %s Maxpkt is 0x%x index is 0x%x\n",
 			ep->ep.name, ep_maxpacket(ep), ep_index(ep));
-	size -= t;
-	next += t;
 
 	if (list_empty(&ep->queue)) {
-		t = scnprintf(next, size, "its req queue is empty\n\n");
-		size -= t;
-		next += t;
+		seq_puts(m, "its req queue is empty\n\n");
 	} else {
 		list_for_each_entry(req, &ep->queue, queue) {
-			t = scnprintf(next, size,
+			seq_printf(m,
 				"req %p actual 0x%x length 0x%x buf %p\n",
 				&req->req, req->req.actual,
 				req->req.length, req->req.buf);
-			size -= t;
-			next += t;
 		}
 	}
 	/* other gadget->eplist ep */
 	list_for_each_entry(ep, &udc->gadget.ep_list, ep.ep_list) {
 		if (ep->ep.desc) {
-			t = scnprintf(next, size,
+			seq_printf(m,
 					"\nFor %s Maxpkt is 0x%x "
 					"index is 0x%x\n",
 					ep->ep.name, ep_maxpacket(ep),
 					ep_index(ep));
-			size -= t;
-			next += t;
 
 			if (list_empty(&ep->queue)) {
-				t = scnprintf(next, size,
-						"its req queue is empty\n\n");
-				size -= t;
-				next += t;
+				seq_puts(m, "its req queue is empty\n\n");
 			} else {
 				list_for_each_entry(req, &ep->queue, queue) {
-					t = scnprintf(next, size,
+					seq_printf(m,
 						"req %p actual 0x%x length "
 						"0x%x  buf %p\n",
 						&req->req, req->req.actual,
 						req->req.length, req->req.buf);
-					size -= t;
-					next += t;
-					}	/* end for each_entry of ep req */
-				}	/* end for else */
-			}	/* end for if(ep->queue) */
-		}		/* end (ep->desc) */
+				}	/* end for each_entry of ep req */
+			}	/* end for else */
+		}	/* end for if(ep->queue) */
+	}	/* end (ep->desc) */
 
 	spin_unlock_irqrestore(&udc->lock, flags);
+	return 0;
+}
 
-	*eof = 1;
-	return count - size;
+/*
+ * seq_file wrappers for procfile show routines.
+ */
+static int fsl_proc_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, fsl_proc_read, NULL);
 }
 
-#define create_proc_file()	create_proc_read_entry(proc_filename, \
-				0, NULL, fsl_proc_read, NULL)
+static const struct file_operations fsl_proc_fops = {
+	.open		= fsl_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
 
+#define create_proc_file()	proc_create(proc_filename, 0, NULL, &fsl_proc_fops)
 #define remove_proc_file()	remove_proc_entry(proc_filename, NULL)
 
 #else				/* !CONFIG_USB_GADGET_DEBUG_FILES */
diff --git a/drivers/usb/gadget/goku_udc.c b/drivers/usb/gadget/goku_udc.c
index 991aba390d9d..52dd6cc6c0aa 100644
--- a/drivers/usb/gadget/goku_udc.c
+++ b/drivers/usb/gadget/goku_udc.c
@@ -35,6 +35,7 @@
 #include <linux/list.h>
 #include <linux/interrupt.h>
 #include <linux/proc_fs.h>
+#include <linux/seq_file.h>
 #include <linux/device.h>
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
@@ -1005,7 +1006,7 @@ static const struct usb_gadget_ops goku_ops = {
 
 /*-------------------------------------------------------------------------*/
 
-static inline char *dmastr(void)
+static inline const char *dmastr(void)
 {
 	if (use_dma == 0)
 		return "(dma disabled)";
@@ -1022,13 +1023,10 @@ static const char proc_node_name [] = "driver/udc";
 #define FOURBITS "%s%s%s%s"
 #define EIGHTBITS FOURBITS FOURBITS
 
-static void
-dump_intmask(const char *label, u32 mask, char **next, unsigned *size)
+static void dump_intmask(struct seq_file *m, const char *label, u32 mask)
 {
-	int t;
-
 	/* int_status is the same format ... */
-	t = scnprintf(*next, *size,
+	seq_printf(m,
 		"%s %05X =" FOURBITS EIGHTBITS EIGHTBITS "\n",
 		label, mask,
 		(mask & INT_PWRDETECT) ? " power" : "",
@@ -1055,33 +1053,23 @@ dump_intmask(const char *label, u32 mask, char **next, unsigned *size)
 		(mask & INT_ENDPOINT0) ? " ep0" : "",
 		(mask & INT_USBRESET) ? " reset" : "",
 		(mask & INT_SUSPEND) ? " suspend" : "");
-	*size -= t;
-	*next += t;
 }
 
 
-static int
-udc_proc_read(char *buffer, char **start, off_t off, int count,
-		int *eof, void *_dev)
+static int udc_proc_read(struct seq_file *m, void *v)
 {
-	char				*buf = buffer;
-	struct goku_udc			*dev = _dev;
+	struct goku_udc			*dev = m->private;
 	struct goku_udc_regs __iomem	*regs = dev->regs;
-	char				*next = buf;
-	unsigned			size = count;
 	unsigned long			flags;
-	int				i, t, is_usb_connected;
+	int				i, is_usb_connected;
 	u32				tmp;
 
-	if (off != 0)
-		return 0;
-
 	local_irq_save(flags);
 
 	/* basic device status */
 	tmp = readl(&regs->power_detect);
 	is_usb_connected = tmp & PW_DETECT;
-	t = scnprintf(next, size,
+	seq_printf(m,
 		"%s - %s\n"
 		"%s version: %s %s\n"
 		"Gadget driver: %s\n"
@@ -1093,7 +1081,7 @@ udc_proc_read(char *buffer, char **start, off_t off, int count,
 		is_usb_connected
 			? ((tmp & PW_PULLUP) ? "full speed" : "powered")
 			: "disconnected",
-		({char *state;
+		({const char *state;
 		switch(dev->ep0state){
 		case EP0_DISCONNECT:	state = "ep0_disconnect"; break;
 		case EP0_IDLE:		state = "ep0_idle"; break;
@@ -1105,27 +1093,24 @@ udc_proc_read(char *buffer, char **start, off_t off, int count,
 		default:		state = "ep0_?"; break;
 		} state; })
 		);
-	size -= t;
-	next += t;
 
-	dump_intmask("int_status", readl(&regs->int_status), &next, &size);
-	dump_intmask("int_enable", readl(&regs->int_enable), &next, &size);
+	dump_intmask(m, "int_status", readl(&regs->int_status));
+	dump_intmask(m, "int_enable", readl(&regs->int_enable));
 
 	if (!is_usb_connected || !dev->driver || (tmp & PW_PULLUP) == 0)
 		goto done;
 
 	/* registers for (active) device and ep0 */
-	t = scnprintf(next, size, "\nirqs %lu\ndataset %02x "
+	if (seq_printf(m, "\nirqs %lu\ndataset %02x "
 			"single.bcs %02x.%02x state %x addr %u\n",
 			dev->irqs, readl(&regs->DataSet),
 			readl(&regs->EPxSingle), readl(&regs->EPxBCS),
 			readl(&regs->UsbState),
-			readl(&regs->address));
-	size -= t;
-	next += t;
+			readl(&regs->address)) < 0)
+		goto done;
 
 	tmp = readl(&regs->dma_master);
-	t = scnprintf(next, size,
+	if (seq_printf(m,
 		"dma %03X =" EIGHTBITS "%s %s\n", tmp,
 		(tmp & MST_EOPB_DIS) ? " eopb-" : "",
 		(tmp & MST_EOPB_ENA) ? " eopb+" : "",
@@ -1140,9 +1125,8 @@ udc_proc_read(char *buffer, char **start, off_t off, int count,
 		(tmp & MST_WR_ENA) ? " OUT" : "",
 		(tmp & MST_CONNECTION)
 			? "ep1in/ep2out"
-			: "ep1out/ep2in");
-	size -= t;
-	next += t;
+			: "ep1out/ep2in") < 0)
+		goto done;
 
 	/* dump endpoint queues */
 	for (i = 0; i < 4; i++) {
@@ -1153,7 +1137,7 @@ udc_proc_read(char *buffer, char **start, off_t off, int count,
 			continue;
 
 		tmp = readl(ep->reg_status);
-		t = scnprintf(next, size,
+		if (seq_printf(m,
 			"%s %s max %u %s, irqs %lu, "
 			"status %02x (%s) " FOURBITS "\n",
 			ep->ep.name,
@@ -1186,18 +1170,12 @@ udc_proc_read(char *buffer, char **start, off_t off, int count,
 			(tmp & EPxSTATUS_SUSPEND) ? " suspend" : "",
 			(tmp & EPxSTATUS_FIFO_DISABLE) ? " disable" : "",
 			(tmp & EPxSTATUS_STAGE_ERROR) ? " ep0stat" : ""
-			);
-		if (t <= 0 || t > size)
+			) < 0)
 			goto done;
-		size -= t;
-		next += t;
 
 		if (list_empty(&ep->queue)) {
-			t = scnprintf(next, size, "\t(nothing queued)\n");
-			if (t <= 0 || t > size)
+			if (seq_puts(m, "\t(nothing queued)\n") < 0)
 				goto done;
-			size -= t;
-			next += t;
 			continue;
 		}
 		list_for_each_entry(req, &ep->queue, queue) {
@@ -1211,23 +1189,34 @@ udc_proc_read(char *buffer, char **start, off_t off, int count,
 			} else
 				tmp = req->req.actual;
 
-			t = scnprintf(next, size,
+			if (seq_printf(m,
 				"\treq %p len %u/%u buf %p\n",
 				&req->req, tmp, req->req.length,
-				req->req.buf);
-			if (t <= 0 || t > size)
+				req->req.buf) < 0)
 				goto done;
-			size -= t;
-			next += t;
 		}
 	}
 
 done:
 	local_irq_restore(flags);
-	*eof = 1;
-	return count - size;
+	return 0;
+}
+
+/*
+ * seq_file wrappers for procfile show routines.
+ */
+static int udc_proc_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, udc_proc_read, PDE_DATA(file_inode(file)));
 }
 
+static const struct file_operations udc_proc_fops = {
+	.open		= udc_proc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
 #endif	/* CONFIG_USB_GADGET_DEBUG_FILES */
 
 /*-------------------------------------------------------------------------*/
@@ -1796,7 +1785,7 @@ static int goku_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 
 #ifdef CONFIG_USB_GADGET_DEBUG_FILES
-	create_proc_read_entry(proc_node_name, 0, NULL, udc_proc_read, dev);
+	proc_create_data(proc_node_name, 0, NULL, &udc_proc_fops, dev);
 #endif
 
 	retval = usb_add_gadget_udc_release(&pdev->dev, &dev->gadget,
diff --git a/drivers/usb/gadget/inode.c b/drivers/usb/gadget/inode.c
index e2b2e9cf254a..570c005062ab 100644
--- a/drivers/usb/gadget/inode.c
+++ b/drivers/usb/gadget/inode.c
@@ -24,6 +24,8 @@
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <linux/poll.h>
+#include <linux/mmu_context.h>
+#include <linux/aio.h>
 
 #include <linux/device.h>
 #include <linux/moduleparam.h>
@@ -513,6 +515,9 @@ static long ep_ioctl(struct file *fd, unsigned code, unsigned long value)
 struct kiocb_priv {
 	struct usb_request	*req;
 	struct ep_data		*epdata;
+	struct kiocb		*iocb;
+	struct mm_struct	*mm;
+	struct work_struct	work;
 	void			*buf;
 	const struct iovec	*iv;
 	unsigned long		nr_segs;
@@ -528,7 +533,6 @@ static int ep_aio_cancel(struct kiocb *iocb, struct io_event *e)
 	local_irq_disable();
 	epdata = priv->epdata;
 	// spin_lock(&epdata->dev->lock);
-	kiocbSetCancelled(iocb);
 	if (likely(epdata && epdata->ep && priv->req))
 		value = usb_ep_dequeue (epdata->ep, priv->req);
 	else
@@ -540,15 +544,12 @@ static int ep_aio_cancel(struct kiocb *iocb, struct io_event *e)
 	return value;
 }
 
-static ssize_t ep_aio_read_retry(struct kiocb *iocb)
+static ssize_t ep_copy_to_user(struct kiocb_priv *priv)
 {
-	struct kiocb_priv	*priv = iocb->private;
 	ssize_t			len, total;
 	void			*to_copy;
 	int			i;
 
-	/* we "retry" to get the right mm context for this: */
-
 	/* copy stuff into user buffers */
 	total = priv->actual;
 	len = 0;
@@ -568,9 +569,26 @@ static ssize_t ep_aio_read_retry(struct kiocb *iocb)
 		if (total == 0)
 			break;
 	}
+
+	return len;
+}
+
+static void ep_user_copy_worker(struct work_struct *work)
+{
+	struct kiocb_priv *priv = container_of(work, struct kiocb_priv, work);
+	struct mm_struct *mm = priv->mm;
+	struct kiocb *iocb = priv->iocb;
+	size_t ret;
+
+	use_mm(mm);
+	ret = ep_copy_to_user(priv);
+	unuse_mm(mm);
+
+	/* completing the iocb can drop the ctx and mm, don't touch mm after */
+	aio_complete(iocb, ret, ret);
+
 	kfree(priv->buf);
 	kfree(priv);
-	return len;
 }
 
 static void ep_aio_complete(struct usb_ep *ep, struct usb_request *req)
@@ -596,14 +614,14 @@ static void ep_aio_complete(struct usb_ep *ep, struct usb_request *req)
 		aio_complete(iocb, req->actual ? req->actual : req->status,
 				req->status);
 	} else {
-		/* retry() won't report both; so we hide some faults */
+		/* ep_copy_to_user() won't report both; we hide some faults */
 		if (unlikely(0 != req->status))
 			DBG(epdata->dev, "%s fault %d len %d\n",
 				ep->name, req->status, req->actual);
 
 		priv->buf = req->buf;
 		priv->actual = req->actual;
-		kick_iocb(iocb);
+		schedule_work(&priv->work);
 	}
 	spin_unlock(&epdata->dev->lock);
 
@@ -633,8 +651,10 @@ fail:
 		return value;
 	}
 	iocb->private = priv;
+	priv->iocb = iocb;
 	priv->iv = iv;
 	priv->nr_segs = nr_segs;
+	INIT_WORK(&priv->work, ep_user_copy_worker);
 
 	value = get_ready_ep(iocb->ki_filp->f_flags, epdata);
 	if (unlikely(value < 0)) {
@@ -642,10 +662,11 @@ fail:
 		goto fail;
 	}
 
-	iocb->ki_cancel = ep_aio_cancel;
+	kiocb_set_cancel_fn(iocb, ep_aio_cancel);
 	get_ep(epdata);
 	priv->epdata = epdata;
 	priv->actual = 0;
+	priv->mm = current->mm; /* mm teardown waits for iocbs in exit_aio() */
 
 	/* each kiocb is coupled to one usb_request, but we can't
 	 * allocate or submit those if the host disconnected.
@@ -674,7 +695,7 @@ fail:
 		kfree(priv);
 		put_ep(epdata);
 	} else
-		value = (iv ? -EIOCBRETRY : -EIOCBQUEUED);
+		value = -EIOCBQUEUED;
 	return value;
 }
 
@@ -692,7 +713,6 @@ ep_aio_read(struct kiocb *iocb, const struct iovec *iov,
 	if (unlikely(!buf))
 		return -ENOMEM;
 
-	iocb->ki_retry = ep_aio_read_retry;
 	return ep_aio_rwtail(iocb, buf, iocb->ki_left, epdata, iov, nr_segs);
 }
 
@@ -887,7 +907,6 @@ ep_open (struct inode *inode, struct file *fd)
 
 /* used before endpoint configuration */
 static const struct file_operations ep_config_operations = {
-	.owner =	THIS_MODULE,
 	.llseek =	no_llseek,
 
 	.open =		ep_open,
@@ -1940,7 +1959,6 @@ dev_open (struct inode *inode, struct file *fd)
 }
 
 static const struct file_operations dev_init_operations = {
-	.owner =	THIS_MODULE,
 	.llseek =	no_llseek,
 
 	.open =		dev_open,
diff --git a/drivers/usb/gadget/lpc32xx_udc.c b/drivers/usb/gadget/lpc32xx_udc.c
index b943d8cdfbf7..67128be1e1b7 100644
--- a/drivers/usb/gadget/lpc32xx_udc.c
+++ b/drivers/usb/gadget/lpc32xx_udc.c
@@ -565,7 +565,7 @@ static int proc_udc_show(struct seq_file *s, void *unused)
 
 static int proc_udc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, proc_udc_show, PDE(inode)->data);
+	return single_open(file, proc_udc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations proc_ops = {
diff --git a/drivers/usb/gadget/rndis.c b/drivers/usb/gadget/rndis.c
index d9297eebbf73..1e4cfb05f70b 100644
--- a/drivers/usb/gadget/rndis.c
+++ b/drivers/usb/gadget/rndis.c
@@ -1065,7 +1065,7 @@ static int rndis_proc_show(struct seq_file *m, void *v)
 static ssize_t rndis_proc_write(struct file *file, const char __user *buffer,
 				size_t count, loff_t *ppos)
 {
-	rndis_params *p = PDE(file_inode(file))->data;
+	rndis_params *p = PDE_DATA(file_inode(file));
 	u32 speed = 0;
 	int i, fl_speed = 0;
 
@@ -1109,7 +1109,7 @@ static ssize_t rndis_proc_write(struct file *file, const char __user *buffer,
 
 static int rndis_proc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, rndis_proc_show, PDE(inode)->data);
+	return single_open(file, rndis_proc_show, PDE_DATA(inode));
 }
 
 static const struct file_operations rndis_proc_fops = {
diff --git a/drivers/usb/host/ehci-tilegx.c b/drivers/usb/host/ehci-tilegx.c
index 1d215cdb9dea..b083a350eea3 100644
--- a/drivers/usb/host/ehci-tilegx.c
+++ b/drivers/usb/host/ehci-tilegx.c
@@ -118,8 +118,10 @@ static int ehci_hcd_tilegx_drv_probe(struct platform_device *pdev)
 
 	hcd = usb_create_hcd(&ehci_tilegx_hc_driver, &pdev->dev,
 			     dev_name(&pdev->dev));
-	if (!hcd)
-		return -ENOMEM;
+	if (!hcd) {
+          ret = -ENOMEM;
+          goto err_hcd;
+        }
 
 	/*
 	 * We don't use rsrc_start to map in our registers, but seems like
@@ -176,6 +178,7 @@ err_have_irq:
 err_no_irq:
 	tilegx_stop_ehc();
 	usb_put_hcd(hcd);
+err_hcd:
 	gxio_usb_host_destroy(&pdata->usb_ctx);
 	return ret;
 }
diff --git a/drivers/usb/host/isp1362-hcd.c b/drivers/usb/host/isp1362-hcd.c
index 974480c516fa..b04e8ece4d35 100644
--- a/drivers/usb/host/isp1362-hcd.c
+++ b/drivers/usb/host/isp1362-hcd.c
@@ -2175,7 +2175,7 @@ static int proc_isp1362_show(struct seq_file *s, void *unused)
 
 static int proc_isp1362_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, proc_isp1362_show, PDE(inode)->data);
+	return single_open(file, proc_isp1362_show, PDE_DATA(inode));
 }
 
 static const struct file_operations proc_ops = {
@@ -2192,14 +2192,11 @@ static void create_debug_file(struct isp1362_hcd *isp1362_hcd)
 {
 	struct proc_dir_entry *pde;
 
-	pde = create_proc_entry(proc_filename, 0, NULL);
+	pde = proc_create_data(proc_filename, 0, NULL, &proc_ops, isp1362_hcd);
 	if (pde == NULL) {
 		pr_warning("%s: Failed to create debug file '%s'\n", __func__, proc_filename);
 		return;
 	}
-
-	pde->proc_fops = &proc_ops;
-	pde->data = isp1362_hcd;
 	isp1362_hcd->pde = pde;
 }
 
diff --git a/drivers/usb/host/ohci-exynos.c b/drivers/usb/host/ohci-exynos.c
index 114583a8e92b..07592c00af26 100644
--- a/drivers/usb/host/ohci-exynos.c
+++ b/drivers/usb/host/ohci-exynos.c
@@ -14,7 +14,7 @@
 #include <linux/clk.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
-#include <linux/platform_data/usb-exynos.h>
+#include <linux/platform_data/usb-ohci-exynos.h>
 #include <linux/usb/phy.h>
 #include <linux/usb/samsung_usb_phy.h>
 
diff --git a/drivers/usb/host/ohci-tilegx.c b/drivers/usb/host/ohci-tilegx.c
index 1ae7b28a71c2..ea73009de623 100644
--- a/drivers/usb/host/ohci-tilegx.c
+++ b/drivers/usb/host/ohci-tilegx.c
@@ -112,8 +112,10 @@ static int ohci_hcd_tilegx_drv_probe(struct platform_device *pdev)
 
 	hcd = usb_create_hcd(&ohci_tilegx_hc_driver, &pdev->dev,
 			     dev_name(&pdev->dev));
-	if (!hcd)
-		return -ENOMEM;
+	if (!hcd) {
+		ret = -ENOMEM;
+		goto err_hcd;
+	}
 
 	/*
 	 * We don't use rsrc_start to map in our registers, but seems like
@@ -165,6 +167,7 @@ err_have_irq:
 err_no_irq:
 	tilegx_stop_ohc();
 	usb_put_hcd(hcd);
+err_hcd:
 	gxio_usb_host_destroy(&pdata->usb_ctx);
 	return ret;
 }
diff --git a/drivers/usb/host/sl811-hcd.c b/drivers/usb/host/sl811-hcd.c
index 15ed7e8d887f..ad4483efb6d6 100644
--- a/drivers/usb/host/sl811-hcd.c
+++ b/drivers/usb/host/sl811-hcd.c
@@ -1494,7 +1494,7 @@ static int proc_sl811h_show(struct seq_file *s, void *unused)
 
 static int proc_sl811h_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, proc_sl811h_show, PDE(inode)->data);
+	return single_open(file, proc_sl811h_show, PDE_DATA(inode));
 }
 
 static const struct file_operations proc_ops = {
diff --git a/drivers/usb/misc/yurex.c b/drivers/usb/misc/yurex.c
index 42ad2e6d86c4..b6ab515bfc6c 100644
--- a/drivers/usb/misc/yurex.c
+++ b/drivers/usb/misc/yurex.c
@@ -407,8 +407,6 @@ static int yurex_release(struct inode *inode, struct file *file)
 	if (dev == NULL)
 		return -ENODEV;
 
-	yurex_fasync(-1, file, 0);
-
 	/* decrement the count on our device */
 	kref_put(&dev->kref, yurex_delete);
 	return 0;
diff --git a/drivers/usb/phy/Kconfig b/drivers/usb/phy/Kconfig
index aab2ab2fbc90..371d0e74e909 100644
--- a/drivers/usb/phy/Kconfig
+++ b/drivers/usb/phy/Kconfig
@@ -128,7 +128,7 @@ config TWL6030_USB
 
 config USB_GPIO_VBUS
 	tristate "GPIO based peripheral-only VBUS sensing 'transceiver'"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  Provides simple GPIO VBUS sensing for controllers with an
 	  internal transceiver via the usb_phy interface, and
diff --git a/drivers/usb/storage/realtek_cr.c b/drivers/usb/storage/realtek_cr.c
index 4797228747fb..8623577bbbe7 100644
--- a/drivers/usb/storage/realtek_cr.c
+++ b/drivers/usb/storage/realtek_cr.c
@@ -933,14 +933,11 @@ static int realtek_cr_autosuspend_setup(struct us_data *us)
 
 static void realtek_cr_destructor(void *extra)
 {
-	struct rts51x_chip *chip = (struct rts51x_chip *)extra;
-	struct us_data *us;
+	struct rts51x_chip *chip = extra;
 
 	if (!chip)
 		return;
 
-	us = chip->us;
-
 #ifdef CONFIG_REALTEK_AUTOPM
 	if (ss_en) {
 		del_timer(&chip->rts51x_suspend_timer);
diff --git a/drivers/usb/storage/scsiglue.c b/drivers/usb/storage/scsiglue.c
index 4faa982807f2..92b05d95ec5e 100644
--- a/drivers/usb/storage/scsiglue.c
+++ b/drivers/usb/storage/scsiglue.c
@@ -437,22 +437,21 @@ void usb_stor_report_bus_reset(struct us_data *us)
  * /proc/scsi/ functions
  ***********************************************************************/
 
+static int write_info(struct Scsi_Host *host, char *buffer, int length)
+{
+	/* if someone is sending us data, just throw it away */
+	return length;
+}
+
 /* we use this macro to help us write into the buffer */
 #undef SPRINTF
-#define SPRINTF(args...) \
-	do { if (pos < buffer+length) pos += sprintf(pos, ## args); } while (0)
+#define SPRINTF(args...) seq_printf(m, ## args)
 
-static int proc_info (struct Scsi_Host *host, char *buffer,
-		char **start, off_t offset, int length, int inout)
+static int show_info (struct seq_file *m, struct Scsi_Host *host)
 {
 	struct us_data *us = host_to_us(host);
-	char *pos = buffer;
 	const char *string;
 
-	/* if someone is sending us data, just throw it away */
-	if (inout)
-		return length;
-
 	/* print the controller name */
 	SPRINTF("   Host scsi%d: usb-storage\n", host->host_no);
 
@@ -482,28 +481,14 @@ static int proc_info (struct Scsi_Host *host, char *buffer,
 	SPRINTF("    Transport: %s\n", us->transport_name);
 
 	/* show the device flags */
-	if (pos < buffer + length) {
-		pos += sprintf(pos, "       Quirks:");
+	SPRINTF("       Quirks:");
 
 #define US_FLAG(name, value) \
-	if (us->fflags & value) pos += sprintf(pos, " " #name);
+	if (us->fflags & value) seq_printf(m, " " #name);
 US_DO_ALL_FLAGS
 #undef US_FLAG
-
-		*(pos++) = '\n';
-	}
-
-	/*
-	 * Calculate start of next buffer, and return value.
-	 */
-	*start = buffer + offset;
-
-	if ((pos - buffer) < offset)
-		return (0);
-	else if ((pos - buffer - offset) < length)
-		return (pos - buffer - offset);
-	else
-		return (length);
+	seq_putc(m, '\n');
+	return 0;
 }
 
 /***********************************************************************
@@ -548,7 +533,8 @@ struct scsi_host_template usb_stor_host_template = {
 	/* basic userland interface stuff */
 	.name =				"usb-storage",
 	.proc_name =			"usb-storage",
-	.proc_info =			proc_info,
+	.show_info =			show_info,
+	.write_info =			write_info,
 	.info =				host_info,
 
 	/* command interface -- queued only */
diff --git a/drivers/vfio/pci/vfio_pci.c b/drivers/vfio/pci/vfio_pci.c
index 09d2e3ffd6fc..ac3725440d64 100644
--- a/drivers/vfio/pci/vfio_pci.c
+++ b/drivers/vfio/pci/vfio_pci.c
@@ -201,7 +201,9 @@ static int vfio_pci_get_irq_count(struct vfio_pci_device *vdev, int irq_type)
 
 			return (flags & PCI_MSIX_FLAGS_QSIZE) + 1;
 		}
-	}
+	} else if (irq_type == VFIO_PCI_ERR_IRQ_INDEX)
+		if (pci_is_pcie(vdev->pdev))
+			return 1;
 
 	return 0;
 }
@@ -317,6 +319,17 @@ static long vfio_pci_ioctl(void *device_data,
 		if (info.argsz < minsz || info.index >= VFIO_PCI_NUM_IRQS)
 			return -EINVAL;
 
+		switch (info.index) {
+		case VFIO_PCI_INTX_IRQ_INDEX ... VFIO_PCI_MSIX_IRQ_INDEX:
+			break;
+		case VFIO_PCI_ERR_IRQ_INDEX:
+			if (pci_is_pcie(vdev->pdev))
+				break;
+		/* pass thru to return error */
+		default:
+			return -EINVAL;
+		}
+
 		info.flags = VFIO_IRQ_INFO_EVENTFD;
 
 		info.count = vfio_pci_get_irq_count(vdev, info.index);
@@ -552,11 +565,40 @@ static void vfio_pci_remove(struct pci_dev *pdev)
 	kfree(vdev);
 }
 
+static pci_ers_result_t vfio_pci_aer_err_detected(struct pci_dev *pdev,
+						  pci_channel_state_t state)
+{
+	struct vfio_pci_device *vdev;
+	struct vfio_device *device;
+
+	device = vfio_device_get_from_dev(&pdev->dev);
+	if (device == NULL)
+		return PCI_ERS_RESULT_DISCONNECT;
+
+	vdev = vfio_device_data(device);
+	if (vdev == NULL) {
+		vfio_device_put(device);
+		return PCI_ERS_RESULT_DISCONNECT;
+	}
+
+	if (vdev->err_trigger)
+		eventfd_signal(vdev->err_trigger, 1);
+
+	vfio_device_put(device);
+
+	return PCI_ERS_RESULT_CAN_RECOVER;
+}
+
+static struct pci_error_handlers vfio_err_handlers = {
+	.error_detected = vfio_pci_aer_err_detected,
+};
+
 static struct pci_driver vfio_pci_driver = {
 	.name		= "vfio-pci",
 	.id_table	= NULL, /* only dynamic ids */
 	.probe		= vfio_pci_probe,
 	.remove		= vfio_pci_remove,
+	.err_handler	= &vfio_err_handlers,
 };
 
 static void __exit vfio_pci_cleanup(void)
diff --git a/drivers/vfio/pci/vfio_pci_config.c b/drivers/vfio/pci/vfio_pci_config.c
index aeb00fc2d3be..affa34745be9 100644
--- a/drivers/vfio/pci/vfio_pci_config.c
+++ b/drivers/vfio/pci/vfio_pci_config.c
@@ -274,9 +274,10 @@ static int vfio_direct_config_read(struct vfio_pci_device *vdev, int pos,
 	return count;
 }
 
-static int vfio_direct_config_write(struct vfio_pci_device *vdev, int pos,
-				    int count, struct perm_bits *perm,
-				    int offset, __le32 val)
+/* Raw access skips any kind of virtualization */
+static int vfio_raw_config_write(struct vfio_pci_device *vdev, int pos,
+				 int count, struct perm_bits *perm,
+				 int offset, __le32 val)
 {
 	int ret;
 
@@ -287,13 +288,36 @@ static int vfio_direct_config_write(struct vfio_pci_device *vdev, int pos,
 	return count;
 }
 
-/* Default all regions to read-only, no-virtualization */
+static int vfio_raw_config_read(struct vfio_pci_device *vdev, int pos,
+				int count, struct perm_bits *perm,
+				int offset, __le32 *val)
+{
+	int ret;
+
+	ret = vfio_user_config_read(vdev->pdev, pos, val, count);
+	if (ret)
+		return pcibios_err_to_errno(ret);
+
+	return count;
+}
+
+/* Default capability regions to read-only, no-virtualization */
 static struct perm_bits cap_perms[PCI_CAP_ID_MAX + 1] = {
 	[0 ... PCI_CAP_ID_MAX] = { .readfn = vfio_direct_config_read }
 };
 static struct perm_bits ecap_perms[PCI_EXT_CAP_ID_MAX + 1] = {
 	[0 ... PCI_EXT_CAP_ID_MAX] = { .readfn = vfio_direct_config_read }
 };
+/*
+ * Default unassigned regions to raw read-write access.  Some devices
+ * require this to function as they hide registers between the gaps in
+ * config space (be2net).  Like MMIO and I/O port registers, we have
+ * to trust the hardware isolation.
+ */
+static struct perm_bits unassigned_perms = {
+	.readfn = vfio_raw_config_read,
+	.writefn = vfio_raw_config_write
+};
 
 static void free_perm_bits(struct perm_bits *perm)
 {
@@ -779,16 +803,16 @@ int __init vfio_pci_init_perm_bits(void)
 
 	/* Capabilities */
 	ret |= init_pci_cap_pm_perm(&cap_perms[PCI_CAP_ID_PM]);
-	cap_perms[PCI_CAP_ID_VPD].writefn = vfio_direct_config_write;
+	cap_perms[PCI_CAP_ID_VPD].writefn = vfio_raw_config_write;
 	ret |= init_pci_cap_pcix_perm(&cap_perms[PCI_CAP_ID_PCIX]);
-	cap_perms[PCI_CAP_ID_VNDR].writefn = vfio_direct_config_write;
+	cap_perms[PCI_CAP_ID_VNDR].writefn = vfio_raw_config_write;
 	ret |= init_pci_cap_exp_perm(&cap_perms[PCI_CAP_ID_EXP]);
 	ret |= init_pci_cap_af_perm(&cap_perms[PCI_CAP_ID_AF]);
 
 	/* Extended capabilities */
 	ret |= init_pci_ext_cap_err_perm(&ecap_perms[PCI_EXT_CAP_ID_ERR]);
 	ret |= init_pci_ext_cap_pwr_perm(&ecap_perms[PCI_EXT_CAP_ID_PWR]);
-	ecap_perms[PCI_EXT_CAP_ID_VNDR].writefn = vfio_direct_config_write;
+	ecap_perms[PCI_EXT_CAP_ID_VNDR].writefn = vfio_raw_config_write;
 
 	if (ret)
 		vfio_pci_uninit_perm_bits();
@@ -801,9 +825,6 @@ static int vfio_find_cap_start(struct vfio_pci_device *vdev, int pos)
 	u8 cap;
 	int base = (pos >= PCI_CFG_SPACE_SIZE) ? PCI_CFG_SPACE_SIZE :
 						 PCI_STD_HEADER_SIZEOF;
-	base /= 4;
-	pos /= 4;
-
 	cap = vdev->pci_config_map[pos];
 
 	if (cap == PCI_CAP_ID_BASIC)
@@ -813,7 +834,7 @@ static int vfio_find_cap_start(struct vfio_pci_device *vdev, int pos)
 	while (pos - 1 >= base && vdev->pci_config_map[pos - 1] == cap)
 		pos--;
 
-	return pos * 4;
+	return pos;
 }
 
 static int vfio_msi_config_read(struct vfio_pci_device *vdev, int pos,
@@ -1017,13 +1038,9 @@ static int vfio_cap_len(struct vfio_pci_device *vdev, u8 cap, u8 pos)
 		return byte;
 	case PCI_CAP_ID_EXP:
 		/* length based on version */
-		ret = pci_read_config_word(pdev, pos + PCI_EXP_FLAGS, &word);
-		if (ret)
-			return pcibios_err_to_errno(ret);
-
 		vdev->extended_caps = true;
 
-		if ((word & PCI_EXP_FLAGS_VERS) == 1)
+		if ((pcie_caps_reg(pdev) & PCI_EXP_FLAGS_VERS) == 1)
 			return PCI_CAP_EXP_ENDPOINT_SIZEOF_V1;
 		else
 			return PCI_CAP_EXP_ENDPOINT_SIZEOF_V2;
@@ -1230,8 +1247,8 @@ static int vfio_cap_init(struct vfio_pci_device *vdev)
 		}
 
 		/* Sanity check, do we overlap other capabilities? */
-		for (i = 0; i < len; i += 4) {
-			if (likely(map[(pos + i) / 4] == PCI_CAP_ID_INVALID))
+		for (i = 0; i < len; i++) {
+			if (likely(map[pos + i] == PCI_CAP_ID_INVALID))
 				continue;
 
 			pr_warn("%s: %s pci config conflict @0x%x, was cap 0x%x now cap 0x%x\n",
@@ -1239,7 +1256,7 @@ static int vfio_cap_init(struct vfio_pci_device *vdev)
 				pos + i, map[pos + i], cap);
 		}
 
-		memset(map + (pos / 4), cap, len / 4);
+		memset(map + pos, cap, len);
 		ret = vfio_fill_vconfig_bytes(vdev, pos, len);
 		if (ret)
 			return ret;
@@ -1314,8 +1331,8 @@ static int vfio_ecap_init(struct vfio_pci_device *vdev)
 			hidden = true;
 		}
 
-		for (i = 0; i < len; i += 4) {
-			if (likely(map[(epos + i) / 4] == PCI_CAP_ID_INVALID))
+		for (i = 0; i < len; i++) {
+			if (likely(map[epos + i] == PCI_CAP_ID_INVALID))
 				continue;
 
 			pr_warn("%s: %s pci config conflict @0x%x, was ecap 0x%x now ecap 0x%x\n",
@@ -1330,7 +1347,7 @@ static int vfio_ecap_init(struct vfio_pci_device *vdev)
 		 */
 		BUILD_BUG_ON(PCI_EXT_CAP_ID_MAX >= PCI_CAP_ID_INVALID);
 
-		memset(map + (epos / 4), ecap, len / 4);
+		memset(map + epos, ecap, len);
 		ret = vfio_fill_vconfig_bytes(vdev, epos, len);
 		if (ret)
 			return ret;
@@ -1377,10 +1394,12 @@ int vfio_config_init(struct vfio_pci_device *vdev)
 	int ret;
 
 	/*
-	 * Config space, caps and ecaps are all dword aligned, so we can
-	 * use one byte per dword to record the type.
+	 * Config space, caps and ecaps are all dword aligned, so we could
+	 * use one byte per dword to record the type.  However, there are
+	 * no requiremenst on the length of a capability, so the gap between
+	 * capabilities needs byte granularity.
 	 */
-	map = kmalloc(pdev->cfg_size / 4, GFP_KERNEL);
+	map = kmalloc(pdev->cfg_size, GFP_KERNEL);
 	if (!map)
 		return -ENOMEM;
 
@@ -1393,9 +1412,9 @@ int vfio_config_init(struct vfio_pci_device *vdev)
 	vdev->pci_config_map = map;
 	vdev->vconfig = vconfig;
 
-	memset(map, PCI_CAP_ID_BASIC, PCI_STD_HEADER_SIZEOF / 4);
-	memset(map + (PCI_STD_HEADER_SIZEOF / 4), PCI_CAP_ID_INVALID,
-	       (pdev->cfg_size - PCI_STD_HEADER_SIZEOF) / 4);
+	memset(map, PCI_CAP_ID_BASIC, PCI_STD_HEADER_SIZEOF);
+	memset(map + PCI_STD_HEADER_SIZEOF, PCI_CAP_ID_INVALID,
+	       pdev->cfg_size - PCI_STD_HEADER_SIZEOF);
 
 	ret = vfio_fill_vconfig_bytes(vdev, 0, PCI_STD_HEADER_SIZEOF);
 	if (ret)
@@ -1450,6 +1469,22 @@ void vfio_config_free(struct vfio_pci_device *vdev)
 	vdev->msi_perm = NULL;
 }
 
+/*
+ * Find the remaining number of bytes in a dword that match the given
+ * position.  Stop at either the end of the capability or the dword boundary.
+ */
+static size_t vfio_pci_cap_remaining_dword(struct vfio_pci_device *vdev,
+					   loff_t pos)
+{
+	u8 cap = vdev->pci_config_map[pos];
+	size_t i;
+
+	for (i = 1; (pos + i) % 4 && vdev->pci_config_map[pos + i] == cap; i++)
+		/* nop */;
+
+	return i;
+}
+
 static ssize_t vfio_config_do_rw(struct vfio_pci_device *vdev, char __user *buf,
 				 size_t count, loff_t *ppos, bool iswrite)
 {
@@ -1458,55 +1493,48 @@ static ssize_t vfio_config_do_rw(struct vfio_pci_device *vdev, char __user *buf,
 	__le32 val = 0;
 	int cap_start = 0, offset;
 	u8 cap_id;
-	ssize_t ret = count;
+	ssize_t ret;
 
-	if (*ppos < 0 || *ppos + count > pdev->cfg_size)
+	if (*ppos < 0 || *ppos >= pdev->cfg_size ||
+	    *ppos + count > pdev->cfg_size)
 		return -EFAULT;
 
 	/*
-	 * gcc can't seem to figure out we're a static function, only called
-	 * with count of 1/2/4 and hits copy_from_user_overflow without this.
+	 * Chop accesses into aligned chunks containing no more than a
+	 * single capability.  Caller increments to the next chunk.
 	 */
-	if (count > sizeof(val))
-		return -EINVAL;
-
-	cap_id = vdev->pci_config_map[*ppos / 4];
-
-	if (cap_id == PCI_CAP_ID_INVALID) {
-		if (iswrite)
-			return ret; /* drop */
-
-		/*
-		 * Per PCI spec 3.0, section 6.1, reads from reserved and
-		 * unimplemented registers return 0
-		 */
-		if (copy_to_user(buf, &val, count))
-			return -EFAULT;
-
-		return ret;
-	}
+	count = min(count, vfio_pci_cap_remaining_dword(vdev, *ppos));
+	if (count >= 4 && !(*ppos % 4))
+		count = 4;
+	else if (count >= 2 && !(*ppos % 2))
+		count = 2;
+	else
+		count = 1;
 
-	/*
-	 * All capabilities are minimum 4 bytes and aligned on dword
-	 * boundaries.  Since we don't support unaligned accesses, we're
-	 * only ever accessing a single capability.
-	 */
-	if (*ppos >= PCI_CFG_SPACE_SIZE) {
-		WARN_ON(cap_id > PCI_EXT_CAP_ID_MAX);
+	ret = count;
 
-		perm = &ecap_perms[cap_id];
-		cap_start = vfio_find_cap_start(vdev, *ppos);
+	cap_id = vdev->pci_config_map[*ppos];
 
+	if (cap_id == PCI_CAP_ID_INVALID) {
+		perm = &unassigned_perms;
+		cap_start = *ppos;
 	} else {
-		WARN_ON(cap_id > PCI_CAP_ID_MAX);
+		if (*ppos >= PCI_CFG_SPACE_SIZE) {
+			WARN_ON(cap_id > PCI_EXT_CAP_ID_MAX);
 
-		perm = &cap_perms[cap_id];
+			perm = &ecap_perms[cap_id];
+			cap_start = vfio_find_cap_start(vdev, *ppos);
+		} else {
+			WARN_ON(cap_id > PCI_CAP_ID_MAX);
 
-		if (cap_id == PCI_CAP_ID_MSI)
-			perm = vdev->msi_perm;
+			perm = &cap_perms[cap_id];
 
-		if (cap_id > PCI_CAP_ID_BASIC)
-			cap_start = vfio_find_cap_start(vdev, *ppos);
+			if (cap_id == PCI_CAP_ID_MSI)
+				perm = vdev->msi_perm;
+
+			if (cap_id > PCI_CAP_ID_BASIC)
+				cap_start = vfio_find_cap_start(vdev, *ppos);
+		}
 	}
 
 	WARN_ON(!cap_start && cap_id != PCI_CAP_ID_BASIC);
@@ -1546,20 +1574,8 @@ ssize_t vfio_pci_config_rw(struct vfio_pci_device *vdev, char __user *buf,
 
 	pos &= VFIO_PCI_OFFSET_MASK;
 
-	/*
-	 * We want to both keep the access size the caller users as well as
-	 * support reading large chunks of config space in a single call.
-	 * PCI doesn't support unaligned accesses, so we can safely break
-	 * those apart.
-	 */
 	while (count) {
-		if (count >= 4 && !(pos % 4))
-			ret = vfio_config_do_rw(vdev, buf, 4, &pos, iswrite);
-		else if (count >= 2 && !(pos % 2))
-			ret = vfio_config_do_rw(vdev, buf, 2, &pos, iswrite);
-		else
-			ret = vfio_config_do_rw(vdev, buf, 1, &pos, iswrite);
-
+		ret = vfio_config_do_rw(vdev, buf, count, &pos, iswrite);
 		if (ret < 0)
 			return ret;
 
diff --git a/drivers/vfio/pci/vfio_pci_intrs.c b/drivers/vfio/pci/vfio_pci_intrs.c
index a96509187deb..4bc704e1b7c7 100644
--- a/drivers/vfio/pci/vfio_pci_intrs.c
+++ b/drivers/vfio/pci/vfio_pci_intrs.c
@@ -287,7 +287,8 @@ void vfio_pci_intx_mask(struct vfio_pci_device *vdev)
  * a signal is necessary, which can then be handled via a work queue
  * or directly depending on the caller.
  */
-int vfio_pci_intx_unmask_handler(struct vfio_pci_device *vdev, void *unused)
+static int vfio_pci_intx_unmask_handler(struct vfio_pci_device *vdev,
+					void *unused)
 {
 	struct pci_dev *pdev = vdev->pdev;
 	unsigned long flags;
@@ -746,6 +747,63 @@ static int vfio_pci_set_msi_trigger(struct vfio_pci_device *vdev,
 	return 0;
 }
 
+static int vfio_pci_set_err_trigger(struct vfio_pci_device *vdev,
+				    unsigned index, unsigned start,
+				    unsigned count, uint32_t flags, void *data)
+{
+	int32_t fd = *(int32_t *)data;
+	struct pci_dev *pdev = vdev->pdev;
+
+	if ((index != VFIO_PCI_ERR_IRQ_INDEX) ||
+	    !(flags & VFIO_IRQ_SET_DATA_TYPE_MASK))
+		return -EINVAL;
+
+	/*
+	 * device_lock synchronizes setting and checking of
+	 * err_trigger. The vfio_pci_aer_err_detected() is also
+	 * called with device_lock held.
+	 */
+
+	/* DATA_NONE/DATA_BOOL enables loopback testing */
+
+	if (flags & VFIO_IRQ_SET_DATA_NONE) {
+		device_lock(&pdev->dev);
+		if (vdev->err_trigger)
+			eventfd_signal(vdev->err_trigger, 1);
+		device_unlock(&pdev->dev);
+		return 0;
+	} else if (flags & VFIO_IRQ_SET_DATA_BOOL) {
+		uint8_t trigger = *(uint8_t *)data;
+		device_lock(&pdev->dev);
+		if (trigger && vdev->err_trigger)
+			eventfd_signal(vdev->err_trigger, 1);
+		device_unlock(&pdev->dev);
+		return 0;
+	}
+
+	/* Handle SET_DATA_EVENTFD */
+
+	if (fd == -1) {
+		device_lock(&pdev->dev);
+		if (vdev->err_trigger)
+			eventfd_ctx_put(vdev->err_trigger);
+		vdev->err_trigger = NULL;
+		device_unlock(&pdev->dev);
+		return 0;
+	} else if (fd >= 0) {
+		struct eventfd_ctx *efdctx;
+		efdctx = eventfd_ctx_fdget(fd);
+		if (IS_ERR(efdctx))
+			return PTR_ERR(efdctx);
+		device_lock(&pdev->dev);
+		if (vdev->err_trigger)
+			eventfd_ctx_put(vdev->err_trigger);
+		vdev->err_trigger = efdctx;
+		device_unlock(&pdev->dev);
+		return 0;
+	} else
+		return -EINVAL;
+}
 int vfio_pci_set_irqs_ioctl(struct vfio_pci_device *vdev, uint32_t flags,
 			    unsigned index, unsigned start, unsigned count,
 			    void *data)
@@ -780,6 +838,13 @@ int vfio_pci_set_irqs_ioctl(struct vfio_pci_device *vdev, uint32_t flags,
 			break;
 		}
 		break;
+	case VFIO_PCI_ERR_IRQ_INDEX:
+		switch (flags & VFIO_IRQ_SET_ACTION_TYPE_MASK) {
+		case VFIO_IRQ_SET_ACTION_TRIGGER:
+			if (pci_is_pcie(vdev->pdev))
+				func = vfio_pci_set_err_trigger;
+			break;
+		}
 	}
 
 	if (!func)
diff --git a/drivers/vfio/pci/vfio_pci_private.h b/drivers/vfio/pci/vfio_pci_private.h
index d7e55d03f49e..9c6d5d0f3b02 100644
--- a/drivers/vfio/pci/vfio_pci_private.h
+++ b/drivers/vfio/pci/vfio_pci_private.h
@@ -56,6 +56,7 @@ struct vfio_pci_device {
 	bool			has_vga;
 	struct pci_saved_state	*pci_saved_state;
 	atomic_t		refcnt;
+	struct eventfd_ctx	*err_trigger;
 };
 
 #define is_intx(vdev) (vdev->irq_type == VFIO_PCI_INTX_IRQ_INDEX)
diff --git a/drivers/vfio/vfio.c b/drivers/vfio/vfio.c
index fcc12f3e60a3..acb7121a9316 100644
--- a/drivers/vfio/vfio.c
+++ b/drivers/vfio/vfio.c
@@ -24,8 +24,10 @@
 #include <linux/list.h>
 #include <linux/module.h>
 #include <linux/mutex.h>
+#include <linux/rwsem.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
+#include <linux/stat.h>
 #include <linux/string.h>
 #include <linux/uaccess.h>
 #include <linux/vfio.h>
@@ -57,7 +59,7 @@ struct vfio_iommu_driver {
 struct vfio_container {
 	struct kref			kref;
 	struct list_head		group_list;
-	struct mutex			group_lock;
+	struct rw_semaphore		group_lock;
 	struct vfio_iommu_driver	*iommu_driver;
 	void				*iommu_data;
 };
@@ -392,12 +394,13 @@ static void vfio_device_release(struct kref *kref)
 }
 
 /* Device reference always implies a group reference */
-static void vfio_device_put(struct vfio_device *device)
+void vfio_device_put(struct vfio_device *device)
 {
 	struct vfio_group *group = device->group;
 	kref_put_mutex(&device->kref, vfio_device_release, &group->device_lock);
 	vfio_group_put(group);
 }
+EXPORT_SYMBOL_GPL(vfio_device_put);
 
 static void vfio_device_get(struct vfio_device *device)
 {
@@ -627,6 +630,33 @@ int vfio_add_group_dev(struct device *dev,
 }
 EXPORT_SYMBOL_GPL(vfio_add_group_dev);
 
+/**
+ * Get a reference to the vfio_device for a device that is known to
+ * be bound to a vfio driver.  The driver implicitly holds a
+ * vfio_device reference between vfio_add_group_dev and
+ * vfio_del_group_dev.  We can therefore use drvdata to increment
+ * that reference from the struct device.  This additional
+ * reference must be released by calling vfio_device_put.
+ */
+struct vfio_device *vfio_device_get_from_dev(struct device *dev)
+{
+	struct vfio_device *device = dev_get_drvdata(dev);
+
+	vfio_device_get(device);
+
+	return device;
+}
+EXPORT_SYMBOL_GPL(vfio_device_get_from_dev);
+
+/*
+ * Caller must hold a reference to the vfio_device
+ */
+void *vfio_device_data(struct vfio_device *device)
+{
+	return device->device_data;
+}
+EXPORT_SYMBOL_GPL(vfio_device_data);
+
 /* Given a referenced group, check if it contains the device */
 static bool vfio_dev_present(struct vfio_group *group, struct device *dev)
 {
@@ -675,9 +705,13 @@ EXPORT_SYMBOL_GPL(vfio_del_group_dev);
 static long vfio_ioctl_check_extension(struct vfio_container *container,
 				       unsigned long arg)
 {
-	struct vfio_iommu_driver *driver = container->iommu_driver;
+	struct vfio_iommu_driver *driver;
 	long ret = 0;
 
+	down_read(&container->group_lock);
+
+	driver = container->iommu_driver;
+
 	switch (arg) {
 		/* No base extensions yet */
 	default:
@@ -707,10 +741,12 @@ static long vfio_ioctl_check_extension(struct vfio_container *container,
 						 VFIO_CHECK_EXTENSION, arg);
 	}
 
+	up_read(&container->group_lock);
+
 	return ret;
 }
 
-/* hold container->group_lock */
+/* hold write lock on container->group_lock */
 static int __vfio_container_attach_groups(struct vfio_container *container,
 					  struct vfio_iommu_driver *driver,
 					  void *data)
@@ -741,7 +777,7 @@ static long vfio_ioctl_set_iommu(struct vfio_container *container,
 	struct vfio_iommu_driver *driver;
 	long ret = -ENODEV;
 
-	mutex_lock(&container->group_lock);
+	down_write(&container->group_lock);
 
 	/*
 	 * The container is designed to be an unprivileged interface while
@@ -752,7 +788,7 @@ static long vfio_ioctl_set_iommu(struct vfio_container *container,
 	 * the container is deprivileged and returns to an unset state.
 	 */
 	if (list_empty(&container->group_list) || container->iommu_driver) {
-		mutex_unlock(&container->group_lock);
+		up_write(&container->group_lock);
 		return -EINVAL;
 	}
 
@@ -799,7 +835,7 @@ static long vfio_ioctl_set_iommu(struct vfio_container *container,
 
 	mutex_unlock(&vfio.iommu_drivers_lock);
 skip_drivers_unlock:
-	mutex_unlock(&container->group_lock);
+	up_write(&container->group_lock);
 
 	return ret;
 }
@@ -815,9 +851,6 @@ static long vfio_fops_unl_ioctl(struct file *filep,
 	if (!container)
 		return ret;
 
-	driver = container->iommu_driver;
-	data = container->iommu_data;
-
 	switch (cmd) {
 	case VFIO_GET_API_VERSION:
 		ret = VFIO_API_VERSION;
@@ -829,8 +862,15 @@ static long vfio_fops_unl_ioctl(struct file *filep,
 		ret = vfio_ioctl_set_iommu(container, arg);
 		break;
 	default:
+		down_read(&container->group_lock);
+
+		driver = container->iommu_driver;
+		data = container->iommu_data;
+
 		if (driver) /* passthrough all unrecognized ioctls */
 			ret = driver->ops->ioctl(data, cmd, arg);
+
+		up_read(&container->group_lock);
 	}
 
 	return ret;
@@ -854,7 +894,7 @@ static int vfio_fops_open(struct inode *inode, struct file *filep)
 		return -ENOMEM;
 
 	INIT_LIST_HEAD(&container->group_list);
-	mutex_init(&container->group_lock);
+	init_rwsem(&container->group_lock);
 	kref_init(&container->kref);
 
 	filep->private_data = container;
@@ -881,35 +921,55 @@ static ssize_t vfio_fops_read(struct file *filep, char __user *buf,
 			      size_t count, loff_t *ppos)
 {
 	struct vfio_container *container = filep->private_data;
-	struct vfio_iommu_driver *driver = container->iommu_driver;
+	struct vfio_iommu_driver *driver;
+	ssize_t ret = -EINVAL;
 
-	if (unlikely(!driver || !driver->ops->read))
-		return -EINVAL;
+	down_read(&container->group_lock);
 
-	return driver->ops->read(container->iommu_data, buf, count, ppos);
+	driver = container->iommu_driver;
+	if (likely(driver && driver->ops->read))
+		ret = driver->ops->read(container->iommu_data,
+					buf, count, ppos);
+
+	up_read(&container->group_lock);
+
+	return ret;
 }
 
 static ssize_t vfio_fops_write(struct file *filep, const char __user *buf,
 			       size_t count, loff_t *ppos)
 {
 	struct vfio_container *container = filep->private_data;
-	struct vfio_iommu_driver *driver = container->iommu_driver;
+	struct vfio_iommu_driver *driver;
+	ssize_t ret = -EINVAL;
 
-	if (unlikely(!driver || !driver->ops->write))
-		return -EINVAL;
+	down_read(&container->group_lock);
 
-	return driver->ops->write(container->iommu_data, buf, count, ppos);
+	driver = container->iommu_driver;
+	if (likely(driver && driver->ops->write))
+		ret = driver->ops->write(container->iommu_data,
+					 buf, count, ppos);
+
+	up_read(&container->group_lock);
+
+	return ret;
 }
 
 static int vfio_fops_mmap(struct file *filep, struct vm_area_struct *vma)
 {
 	struct vfio_container *container = filep->private_data;
-	struct vfio_iommu_driver *driver = container->iommu_driver;
+	struct vfio_iommu_driver *driver;
+	int ret = -EINVAL;
 
-	if (unlikely(!driver || !driver->ops->mmap))
-		return -EINVAL;
+	down_read(&container->group_lock);
 
-	return driver->ops->mmap(container->iommu_data, vma);
+	driver = container->iommu_driver;
+	if (likely(driver && driver->ops->mmap))
+		ret = driver->ops->mmap(container->iommu_data, vma);
+
+	up_read(&container->group_lock);
+
+	return ret;
 }
 
 static const struct file_operations vfio_fops = {
@@ -933,7 +993,7 @@ static void __vfio_group_unset_container(struct vfio_group *group)
 	struct vfio_container *container = group->container;
 	struct vfio_iommu_driver *driver;
 
-	mutex_lock(&container->group_lock);
+	down_write(&container->group_lock);
 
 	driver = container->iommu_driver;
 	if (driver)
@@ -951,7 +1011,7 @@ static void __vfio_group_unset_container(struct vfio_group *group)
 		container->iommu_data = NULL;
 	}
 
-	mutex_unlock(&container->group_lock);
+	up_write(&container->group_lock);
 
 	vfio_container_put(container);
 }
@@ -1011,7 +1071,7 @@ static int vfio_group_set_container(struct vfio_group *group, int container_fd)
 	container = f.file->private_data;
 	WARN_ON(!container); /* fget ensures we don't race vfio_release */
 
-	mutex_lock(&container->group_lock);
+	down_write(&container->group_lock);
 
 	driver = container->iommu_driver;
 	if (driver) {
@@ -1029,7 +1089,7 @@ static int vfio_group_set_container(struct vfio_group *group, int container_fd)
 	atomic_inc(&group->container_users);
 
 unlock_out:
-	mutex_unlock(&container->group_lock);
+	up_write(&container->group_lock);
 	fdput(f);
 	return ret;
 }
@@ -1300,6 +1360,9 @@ static const struct file_operations vfio_device_fops = {
  */
 static char *vfio_devnode(struct device *dev, umode_t *mode)
 {
+	if (MINOR(dev->devt) == 0)
+		*mode = S_IRUGO | S_IWUGO;
+
 	return kasprintf(GFP_KERNEL, "vfio/%s", dev_name(dev));
 }
 
diff --git a/drivers/vhost/Kconfig b/drivers/vhost/Kconfig
index bf243177ffe1..8b9226da3f54 100644
--- a/drivers/vhost/Kconfig
+++ b/drivers/vhost/Kconfig
@@ -1,6 +1,7 @@
 config VHOST_NET
 	tristate "Host kernel accelerator for virtio net"
 	depends on NET && EVENTFD && (TUN || !TUN) && (MACVTAP || !MACVTAP)
+	select VHOST_RING
 	---help---
 	  This kernel module can be loaded in host kernel to accelerate
 	  guest networking with virtio_net. Not to be confused with virtio_net
@@ -9,6 +10,17 @@ config VHOST_NET
 	  To compile this driver as a module, choose M here: the module will
 	  be called vhost_net.
 
-if STAGING
-source "drivers/vhost/Kconfig.tcm"
-endif
+config VHOST_SCSI
+	tristate "VHOST_SCSI TCM fabric driver"
+	depends on TARGET_CORE && EVENTFD && m
+	select VHOST_RING
+	default n
+	---help---
+	Say M here to enable the vhost_scsi TCM fabric module
+	for use with virtio-scsi guests
+
+config VHOST_RING
+	tristate
+	---help---
+	  This option is selected by any driver which needs to access
+	  the host side of a virtio ring.
diff --git a/drivers/vhost/Kconfig.tcm b/drivers/vhost/Kconfig.tcm
deleted file mode 100644
index 7e3aa28d999e..000000000000
--- a/drivers/vhost/Kconfig.tcm
+++ /dev/null
@@ -1,6 +0,0 @@
-config TCM_VHOST
-	tristate "TCM_VHOST fabric module"
-	depends on TARGET_CORE && EVENTFD && m
-	default n
-	---help---
-	Say M here to enable the TCM_VHOST fabric module for use with virtio-scsi guests
diff --git a/drivers/vhost/Makefile b/drivers/vhost/Makefile
index a27b053bc9ab..654e9afb11f5 100644
--- a/drivers/vhost/Makefile
+++ b/drivers/vhost/Makefile
@@ -1,4 +1,7 @@
 obj-$(CONFIG_VHOST_NET) += vhost_net.o
 vhost_net-y := vhost.o net.o
 
-obj-$(CONFIG_TCM_VHOST) += tcm_vhost.o
+obj-$(CONFIG_VHOST_SCSI) += vhost_scsi.o
+vhost_scsi-y := scsi.o
+
+obj-$(CONFIG_VHOST_RING) += vringh.o
diff --git a/drivers/vhost/net.c b/drivers/vhost/net.c
index ec6fb3fa59bb..2b51e2336aa2 100644
--- a/drivers/vhost/net.c
+++ b/drivers/vhost/net.c
@@ -59,25 +59,47 @@ MODULE_PARM_DESC(experimental_zcopytx, "Enable Zero Copy TX;"
 #define VHOST_DMA_IS_DONE(len) ((len) >= VHOST_DMA_DONE_LEN)
 
 enum {
+	VHOST_NET_FEATURES = VHOST_FEATURES |
+			 (1ULL << VHOST_NET_F_VIRTIO_NET_HDR) |
+			 (1ULL << VIRTIO_NET_F_MRG_RXBUF),
+};
+
+enum {
 	VHOST_NET_VQ_RX = 0,
 	VHOST_NET_VQ_TX = 1,
 	VHOST_NET_VQ_MAX = 2,
 };
 
-enum vhost_net_poll_state {
-	VHOST_NET_POLL_DISABLED = 0,
-	VHOST_NET_POLL_STARTED = 1,
-	VHOST_NET_POLL_STOPPED = 2,
+struct vhost_net_ubuf_ref {
+	struct kref kref;
+	wait_queue_head_t wait;
+	struct vhost_virtqueue *vq;
+};
+
+struct vhost_net_virtqueue {
+	struct vhost_virtqueue vq;
+	/* hdr is used to store the virtio header.
+	 * Since each iovec has >= 1 byte length, we never need more than
+	 * header length entries to store the header. */
+	struct iovec hdr[sizeof(struct virtio_net_hdr_mrg_rxbuf)];
+	size_t vhost_hlen;
+	size_t sock_hlen;
+	/* vhost zerocopy support fields below: */
+	/* last used idx for outstanding DMA zerocopy buffers */
+	int upend_idx;
+	/* first used idx for DMA done zerocopy buffers */
+	int done_idx;
+	/* an array of userspace buffers info */
+	struct ubuf_info *ubuf_info;
+	/* Reference counting for outstanding ubufs.
+	 * Protected by vq mutex. Writers must also take device mutex. */
+	struct vhost_net_ubuf_ref *ubufs;
 };
 
 struct vhost_net {
 	struct vhost_dev dev;
-	struct vhost_virtqueue vqs[VHOST_NET_VQ_MAX];
+	struct vhost_net_virtqueue vqs[VHOST_NET_VQ_MAX];
 	struct vhost_poll poll[VHOST_NET_VQ_MAX];
-	/* Tells us whether we are polling a socket for TX.
-	 * We only do this when socket buffer fills up.
-	 * Protected by tx vq lock. */
-	enum vhost_net_poll_state tx_poll_state;
 	/* Number of TX recently submitted.
 	 * Protected by tx vq lock. */
 	unsigned tx_packets;
@@ -88,6 +110,104 @@ struct vhost_net {
 	bool tx_flush;
 };
 
+static unsigned vhost_net_zcopy_mask __read_mostly;
+
+static void vhost_net_enable_zcopy(int vq)
+{
+	vhost_net_zcopy_mask |= 0x1 << vq;
+}
+
+static void vhost_net_zerocopy_done_signal(struct kref *kref)
+{
+	struct vhost_net_ubuf_ref *ubufs;
+
+	ubufs = container_of(kref, struct vhost_net_ubuf_ref, kref);
+	wake_up(&ubufs->wait);
+}
+
+static struct vhost_net_ubuf_ref *
+vhost_net_ubuf_alloc(struct vhost_virtqueue *vq, bool zcopy)
+{
+	struct vhost_net_ubuf_ref *ubufs;
+	/* No zero copy backend? Nothing to count. */
+	if (!zcopy)
+		return NULL;
+	ubufs = kmalloc(sizeof(*ubufs), GFP_KERNEL);
+	if (!ubufs)
+		return ERR_PTR(-ENOMEM);
+	kref_init(&ubufs->kref);
+	init_waitqueue_head(&ubufs->wait);
+	ubufs->vq = vq;
+	return ubufs;
+}
+
+static void vhost_net_ubuf_put(struct vhost_net_ubuf_ref *ubufs)
+{
+	kref_put(&ubufs->kref, vhost_net_zerocopy_done_signal);
+}
+
+static void vhost_net_ubuf_put_and_wait(struct vhost_net_ubuf_ref *ubufs)
+{
+	kref_put(&ubufs->kref, vhost_net_zerocopy_done_signal);
+	wait_event(ubufs->wait, !atomic_read(&ubufs->kref.refcount));
+	kfree(ubufs);
+}
+
+static void vhost_net_clear_ubuf_info(struct vhost_net *n)
+{
+
+	bool zcopy;
+	int i;
+
+	for (i = 0; i < n->dev.nvqs; ++i) {
+		zcopy = vhost_net_zcopy_mask & (0x1 << i);
+		if (zcopy)
+			kfree(n->vqs[i].ubuf_info);
+	}
+}
+
+int vhost_net_set_ubuf_info(struct vhost_net *n)
+{
+	bool zcopy;
+	int i;
+
+	for (i = 0; i < n->dev.nvqs; ++i) {
+		zcopy = vhost_net_zcopy_mask & (0x1 << i);
+		if (!zcopy)
+			continue;
+		n->vqs[i].ubuf_info = kmalloc(sizeof(*n->vqs[i].ubuf_info) *
+					      UIO_MAXIOV, GFP_KERNEL);
+		if  (!n->vqs[i].ubuf_info)
+			goto err;
+	}
+	return 0;
+
+err:
+	while (i--) {
+		zcopy = vhost_net_zcopy_mask & (0x1 << i);
+		if (!zcopy)
+			continue;
+		kfree(n->vqs[i].ubuf_info);
+	}
+	return -ENOMEM;
+}
+
+void vhost_net_vq_reset(struct vhost_net *n)
+{
+	int i;
+
+	for (i = 0; i < VHOST_NET_VQ_MAX; i++) {
+		n->vqs[i].done_idx = 0;
+		n->vqs[i].upend_idx = 0;
+		n->vqs[i].ubufs = NULL;
+		kfree(n->vqs[i].ubuf_info);
+		n->vqs[i].ubuf_info = NULL;
+		n->vqs[i].vhost_hlen = 0;
+		n->vqs[i].sock_hlen = 0;
+	}
+
+}
+
 static void vhost_net_tx_packet(struct vhost_net *net)
 {
 	++net->tx_packets;
@@ -155,28 +275,6 @@ static void copy_iovec_hdr(const struct iovec *from, struct iovec *to,
 	}
 }
 
-/* Caller must have TX VQ lock */
-static void tx_poll_stop(struct vhost_net *net)
-{
-	if (likely(net->tx_poll_state != VHOST_NET_POLL_STARTED))
-		return;
-	vhost_poll_stop(net->poll + VHOST_NET_VQ_TX);
-	net->tx_poll_state = VHOST_NET_POLL_STOPPED;
-}
-
-/* Caller must have TX VQ lock */
-static int tx_poll_start(struct vhost_net *net, struct socket *sock)
-{
-	int ret;
-
-	if (unlikely(net->tx_poll_state != VHOST_NET_POLL_STOPPED))
-		return 0;
-	ret = vhost_poll_start(net->poll + VHOST_NET_VQ_TX, sock->file);
-	if (!ret)
-		net->tx_poll_state = VHOST_NET_POLL_STARTED;
-	return ret;
-}
-
 /* In case of DMA done not in order in lower device driver for some reason.
  * upend_idx is used to track end of used idx, done_idx is used to track head
  * of used idx. Once lower device DMA done contiguously, we will signal KVM
@@ -185,10 +283,12 @@ static int tx_poll_start(struct vhost_net *net, struct socket *sock)
 static int vhost_zerocopy_signal_used(struct vhost_net *net,
 				      struct vhost_virtqueue *vq)
 {
+	struct vhost_net_virtqueue *nvq =
+		container_of(vq, struct vhost_net_virtqueue, vq);
 	int i;
 	int j = 0;
 
-	for (i = vq->done_idx; i != vq->upend_idx; i = (i + 1) % UIO_MAXIOV) {
+	for (i = nvq->done_idx; i != nvq->upend_idx; i = (i + 1) % UIO_MAXIOV) {
 		if (vq->heads[i].len == VHOST_DMA_FAILED_LEN)
 			vhost_net_tx_err(net);
 		if (VHOST_DMA_IS_DONE(vq->heads[i].len)) {
@@ -200,13 +300,13 @@ static int vhost_zerocopy_signal_used(struct vhost_net *net,
 			break;
 	}
 	if (j)
-		vq->done_idx = i;
+		nvq->done_idx = i;
 	return j;
 }
 
 static void vhost_zerocopy_callback(struct ubuf_info *ubuf, bool success)
 {
-	struct vhost_ubuf_ref *ubufs = ubuf->ctx;
+	struct vhost_net_ubuf_ref *ubufs = ubuf->ctx;
 	struct vhost_virtqueue *vq = ubufs->vq;
 	int cnt = atomic_read(&ubufs->kref.refcount);
 
@@ -223,14 +323,15 @@ static void vhost_zerocopy_callback(struct ubuf_info *ubuf, bool success)
 	/* set len to mark this desc buffers done DMA */
 	vq->heads[ubuf->desc].len = success ?
 		VHOST_DMA_DONE_LEN : VHOST_DMA_FAILED_LEN;
-	vhost_ubuf_put(ubufs);
+	vhost_net_ubuf_put(ubufs);
 }
 
 /* Expects to be always run from workqueue - which acts as
  * read-size critical section for our kind of RCU. */
 static void handle_tx(struct vhost_net *net)
 {
-	struct vhost_virtqueue *vq = &net->dev.vqs[VHOST_NET_VQ_TX];
+	struct vhost_net_virtqueue *nvq = &net->vqs[VHOST_NET_VQ_TX];
+	struct vhost_virtqueue *vq = &nvq->vq;
 	unsigned out, in, s;
 	int head;
 	struct msghdr msg = {
@@ -242,10 +343,10 @@ static void handle_tx(struct vhost_net *net)
 		.msg_flags = MSG_DONTWAIT,
 	};
 	size_t len, total_len = 0;
-	int err, wmem;
+	int err;
 	size_t hdr_size;
 	struct socket *sock;
-	struct vhost_ubuf_ref *uninitialized_var(ubufs);
+	struct vhost_net_ubuf_ref *uninitialized_var(ubufs);
 	bool zcopy, zcopy_used;
 
 	/* TODO: check that we are running from vhost_worker? */
@@ -253,21 +354,11 @@ static void handle_tx(struct vhost_net *net)
 	if (!sock)
 		return;
 
-	wmem = atomic_read(&sock->sk->sk_wmem_alloc);
-	if (wmem >= sock->sk->sk_sndbuf) {
-		mutex_lock(&vq->mutex);
-		tx_poll_start(net, sock);
-		mutex_unlock(&vq->mutex);
-		return;
-	}
-
 	mutex_lock(&vq->mutex);
 	vhost_disable_notify(&net->dev, vq);
 
-	if (wmem < sock->sk->sk_sndbuf / 2)
-		tx_poll_stop(net);
-	hdr_size = vq->vhost_hlen;
-	zcopy = vq->ubufs;
+	hdr_size = nvq->vhost_hlen;
+	zcopy = nvq->ubufs;
 
 	for (;;) {
 		/* Release DMAs done buffers first */
@@ -285,23 +376,15 @@ static void handle_tx(struct vhost_net *net)
 		if (head == vq->num) {
 			int num_pends;
 
-			wmem = atomic_read(&sock->sk->sk_wmem_alloc);
-			if (wmem >= sock->sk->sk_sndbuf * 3 / 4) {
-				tx_poll_start(net, sock);
-				set_bit(SOCK_ASYNC_NOSPACE, &sock->flags);
-				break;
-			}
 			/* If more outstanding DMAs, queue the work.
 			 * Handle upend_idx wrap around
 			 */
-			num_pends = likely(vq->upend_idx >= vq->done_idx) ?
-				    (vq->upend_idx - vq->done_idx) :
-				    (vq->upend_idx + UIO_MAXIOV - vq->done_idx);
-			if (unlikely(num_pends > VHOST_MAX_PEND)) {
-				tx_poll_start(net, sock);
-				set_bit(SOCK_ASYNC_NOSPACE, &sock->flags);
+			num_pends = likely(nvq->upend_idx >= nvq->done_idx) ?
+				    (nvq->upend_idx - nvq->done_idx) :
+				    (nvq->upend_idx + UIO_MAXIOV -
+				     nvq->done_idx);
+			if (unlikely(num_pends > VHOST_MAX_PEND))
 				break;
-			}
 			if (unlikely(vhost_enable_notify(&net->dev, vq))) {
 				vhost_disable_notify(&net->dev, vq);
 				continue;
@@ -314,58 +397,56 @@ static void handle_tx(struct vhost_net *net)
 			break;
 		}
 		/* Skip header. TODO: support TSO. */
-		s = move_iovec_hdr(vq->iov, vq->hdr, hdr_size, out);
+		s = move_iovec_hdr(vq->iov, nvq->hdr, hdr_size, out);
 		msg.msg_iovlen = out;
 		len = iov_length(vq->iov, out);
 		/* Sanity check */
 		if (!len) {
 			vq_err(vq, "Unexpected header len for TX: "
 			       "%zd expected %zd\n",
-			       iov_length(vq->hdr, s), hdr_size);
+			       iov_length(nvq->hdr, s), hdr_size);
 			break;
 		}
 		zcopy_used = zcopy && (len >= VHOST_GOODCOPY_LEN ||
-				       vq->upend_idx != vq->done_idx);
+				       nvq->upend_idx != nvq->done_idx);
 
 		/* use msg_control to pass vhost zerocopy ubuf info to skb */
 		if (zcopy_used) {
-			vq->heads[vq->upend_idx].id = head;
+			vq->heads[nvq->upend_idx].id = head;
 			if (!vhost_net_tx_select_zcopy(net) ||
 			    len < VHOST_GOODCOPY_LEN) {
 				/* copy don't need to wait for DMA done */
-				vq->heads[vq->upend_idx].len =
+				vq->heads[nvq->upend_idx].len =
 							VHOST_DMA_DONE_LEN;
 				msg.msg_control = NULL;
 				msg.msg_controllen = 0;
 				ubufs = NULL;
 			} else {
 				struct ubuf_info *ubuf;
-				ubuf = vq->ubuf_info + vq->upend_idx;
+				ubuf = nvq->ubuf_info + nvq->upend_idx;
 
-				vq->heads[vq->upend_idx].len =
+				vq->heads[nvq->upend_idx].len =
 					VHOST_DMA_IN_PROGRESS;
 				ubuf->callback = vhost_zerocopy_callback;
-				ubuf->ctx = vq->ubufs;
-				ubuf->desc = vq->upend_idx;
+				ubuf->ctx = nvq->ubufs;
+				ubuf->desc = nvq->upend_idx;
 				msg.msg_control = ubuf;
 				msg.msg_controllen = sizeof(ubuf);
-				ubufs = vq->ubufs;
+				ubufs = nvq->ubufs;
 				kref_get(&ubufs->kref);
 			}
-			vq->upend_idx = (vq->upend_idx + 1) % UIO_MAXIOV;
+			nvq->upend_idx = (nvq->upend_idx + 1) % UIO_MAXIOV;
 		}
 		/* TODO: Check specific error and bomb out unless ENOBUFS? */
 		err = sock->ops->sendmsg(NULL, sock, &msg, len);
 		if (unlikely(err < 0)) {
 			if (zcopy_used) {
 				if (ubufs)
-					vhost_ubuf_put(ubufs);
-				vq->upend_idx = ((unsigned)vq->upend_idx - 1) %
-					UIO_MAXIOV;
+					vhost_net_ubuf_put(ubufs);
+				nvq->upend_idx = ((unsigned)nvq->upend_idx - 1)
+					% UIO_MAXIOV;
 			}
 			vhost_discard_vq_desc(vq, 1);
-			if (err == -EAGAIN || err == -ENOBUFS)
-				tx_poll_start(net, sock);
 			break;
 		}
 		if (err != len)
@@ -470,7 +551,8 @@ err:
  * read-size critical section for our kind of RCU. */
 static void handle_rx(struct vhost_net *net)
 {
-	struct vhost_virtqueue *vq = &net->dev.vqs[VHOST_NET_VQ_RX];
+	struct vhost_net_virtqueue *nvq = &net->vqs[VHOST_NET_VQ_RX];
+	struct vhost_virtqueue *vq = &nvq->vq;
 	unsigned uninitialized_var(in), log;
 	struct vhost_log *vq_log;
 	struct msghdr msg = {
@@ -498,8 +580,8 @@ static void handle_rx(struct vhost_net *net)
 
 	mutex_lock(&vq->mutex);
 	vhost_disable_notify(&net->dev, vq);
-	vhost_hlen = vq->vhost_hlen;
-	sock_hlen = vq->sock_hlen;
+	vhost_hlen = nvq->vhost_hlen;
+	sock_hlen = nvq->sock_hlen;
 
 	vq_log = unlikely(vhost_has_feature(&net->dev, VHOST_F_LOG_ALL)) ?
 		vq->log : NULL;
@@ -529,11 +611,11 @@ static void handle_rx(struct vhost_net *net)
 		/* We don't need to be notified again. */
 		if (unlikely((vhost_hlen)))
 			/* Skip header. TODO: support TSO. */
-			move_iovec_hdr(vq->iov, vq->hdr, vhost_hlen, in);
+			move_iovec_hdr(vq->iov, nvq->hdr, vhost_hlen, in);
 		else
 			/* Copy the header for use in VIRTIO_NET_F_MRG_RXBUF:
 			 * needed because recvmsg can modify msg_iov. */
-			copy_iovec_hdr(vq->iov, vq->hdr, sock_hlen, in);
+			copy_iovec_hdr(vq->iov, nvq->hdr, sock_hlen, in);
 		msg.msg_iovlen = in;
 		err = sock->ops->recvmsg(NULL, sock, &msg,
 					 sock_len, MSG_DONTWAIT | MSG_TRUNC);
@@ -547,7 +629,7 @@ static void handle_rx(struct vhost_net *net)
 			continue;
 		}
 		if (unlikely(vhost_hlen) &&
-		    memcpy_toiovecend(vq->hdr, (unsigned char *)&hdr, 0,
+		    memcpy_toiovecend(nvq->hdr, (unsigned char *)&hdr, 0,
 				      vhost_hlen)) {
 			vq_err(vq, "Unable to write vnet_hdr at addr %p\n",
 			       vq->iov->iov_base);
@@ -555,7 +637,7 @@ static void handle_rx(struct vhost_net *net)
 		}
 		/* TODO: Should check and handle checksum. */
 		if (likely(mergeable) &&
-		    memcpy_toiovecend(vq->hdr, (unsigned char *)&headcount,
+		    memcpy_toiovecend(nvq->hdr, (unsigned char *)&headcount,
 				      offsetof(typeof(hdr), num_buffers),
 				      sizeof hdr.num_buffers)) {
 			vq_err(vq, "Failed num_buffers write");
@@ -612,23 +694,39 @@ static int vhost_net_open(struct inode *inode, struct file *f)
 {
 	struct vhost_net *n = kmalloc(sizeof *n, GFP_KERNEL);
 	struct vhost_dev *dev;
-	int r;
+	struct vhost_virtqueue **vqs;
+	int r, i;
 
 	if (!n)
 		return -ENOMEM;
+	vqs = kmalloc(VHOST_NET_VQ_MAX * sizeof(*vqs), GFP_KERNEL);
+	if (!vqs) {
+		kfree(n);
+		return -ENOMEM;
+	}
 
 	dev = &n->dev;
-	n->vqs[VHOST_NET_VQ_TX].handle_kick = handle_tx_kick;
-	n->vqs[VHOST_NET_VQ_RX].handle_kick = handle_rx_kick;
-	r = vhost_dev_init(dev, n->vqs, VHOST_NET_VQ_MAX);
+	vqs[VHOST_NET_VQ_TX] = &n->vqs[VHOST_NET_VQ_TX].vq;
+	vqs[VHOST_NET_VQ_RX] = &n->vqs[VHOST_NET_VQ_RX].vq;
+	n->vqs[VHOST_NET_VQ_TX].vq.handle_kick = handle_tx_kick;
+	n->vqs[VHOST_NET_VQ_RX].vq.handle_kick = handle_rx_kick;
+	for (i = 0; i < VHOST_NET_VQ_MAX; i++) {
+		n->vqs[i].ubufs = NULL;
+		n->vqs[i].ubuf_info = NULL;
+		n->vqs[i].upend_idx = 0;
+		n->vqs[i].done_idx = 0;
+		n->vqs[i].vhost_hlen = 0;
+		n->vqs[i].sock_hlen = 0;
+	}
+	r = vhost_dev_init(dev, vqs, VHOST_NET_VQ_MAX);
 	if (r < 0) {
 		kfree(n);
+		kfree(vqs);
 		return r;
 	}
 
 	vhost_poll_init(n->poll + VHOST_NET_VQ_TX, handle_tx_net, POLLOUT, dev);
 	vhost_poll_init(n->poll + VHOST_NET_VQ_RX, handle_rx_net, POLLIN, dev);
-	n->tx_poll_state = VHOST_NET_POLL_DISABLED;
 
 	f->private_data = n;
 
@@ -638,32 +736,28 @@ static int vhost_net_open(struct inode *inode, struct file *f)
 static void vhost_net_disable_vq(struct vhost_net *n,
 				 struct vhost_virtqueue *vq)
 {
+	struct vhost_net_virtqueue *nvq =
+		container_of(vq, struct vhost_net_virtqueue, vq);
+	struct vhost_poll *poll = n->poll + (nvq - n->vqs);
 	if (!vq->private_data)
 		return;
-	if (vq == n->vqs + VHOST_NET_VQ_TX) {
-		tx_poll_stop(n);
-		n->tx_poll_state = VHOST_NET_POLL_DISABLED;
-	} else
-		vhost_poll_stop(n->poll + VHOST_NET_VQ_RX);
+	vhost_poll_stop(poll);
 }
 
 static int vhost_net_enable_vq(struct vhost_net *n,
 				struct vhost_virtqueue *vq)
 {
+	struct vhost_net_virtqueue *nvq =
+		container_of(vq, struct vhost_net_virtqueue, vq);
+	struct vhost_poll *poll = n->poll + (nvq - n->vqs);
 	struct socket *sock;
-	int ret;
 
 	sock = rcu_dereference_protected(vq->private_data,
 					 lockdep_is_held(&vq->mutex));
 	if (!sock)
 		return 0;
-	if (vq == n->vqs + VHOST_NET_VQ_TX) {
-		n->tx_poll_state = VHOST_NET_POLL_STOPPED;
-		ret = tx_poll_start(n, sock);
-	} else
-		ret = vhost_poll_start(n->poll + VHOST_NET_VQ_RX, sock->file);
 
-	return ret;
+	return vhost_poll_start(poll, sock->file);
 }
 
 static struct socket *vhost_net_stop_vq(struct vhost_net *n,
@@ -683,30 +777,30 @@ static struct socket *vhost_net_stop_vq(struct vhost_net *n,
 static void vhost_net_stop(struct vhost_net *n, struct socket **tx_sock,
 			   struct socket **rx_sock)
 {
-	*tx_sock = vhost_net_stop_vq(n, n->vqs + VHOST_NET_VQ_TX);
-	*rx_sock = vhost_net_stop_vq(n, n->vqs + VHOST_NET_VQ_RX);
+	*tx_sock = vhost_net_stop_vq(n, &n->vqs[VHOST_NET_VQ_TX].vq);
+	*rx_sock = vhost_net_stop_vq(n, &n->vqs[VHOST_NET_VQ_RX].vq);
 }
 
 static void vhost_net_flush_vq(struct vhost_net *n, int index)
 {
 	vhost_poll_flush(n->poll + index);
-	vhost_poll_flush(&n->dev.vqs[index].poll);
+	vhost_poll_flush(&n->vqs[index].vq.poll);
 }
 
 static void vhost_net_flush(struct vhost_net *n)
 {
 	vhost_net_flush_vq(n, VHOST_NET_VQ_TX);
 	vhost_net_flush_vq(n, VHOST_NET_VQ_RX);
-	if (n->dev.vqs[VHOST_NET_VQ_TX].ubufs) {
-		mutex_lock(&n->dev.vqs[VHOST_NET_VQ_TX].mutex);
+	if (n->vqs[VHOST_NET_VQ_TX].ubufs) {
+		mutex_lock(&n->vqs[VHOST_NET_VQ_TX].vq.mutex);
 		n->tx_flush = true;
-		mutex_unlock(&n->dev.vqs[VHOST_NET_VQ_TX].mutex);
+		mutex_unlock(&n->vqs[VHOST_NET_VQ_TX].vq.mutex);
 		/* Wait for all lower device DMAs done. */
-		vhost_ubuf_put_and_wait(n->dev.vqs[VHOST_NET_VQ_TX].ubufs);
-		mutex_lock(&n->dev.vqs[VHOST_NET_VQ_TX].mutex);
+		vhost_net_ubuf_put_and_wait(n->vqs[VHOST_NET_VQ_TX].ubufs);
+		mutex_lock(&n->vqs[VHOST_NET_VQ_TX].vq.mutex);
 		n->tx_flush = false;
-		kref_init(&n->dev.vqs[VHOST_NET_VQ_TX].ubufs->kref);
-		mutex_unlock(&n->dev.vqs[VHOST_NET_VQ_TX].mutex);
+		kref_init(&n->vqs[VHOST_NET_VQ_TX].ubufs->kref);
+		mutex_unlock(&n->vqs[VHOST_NET_VQ_TX].vq.mutex);
 	}
 }
 
@@ -720,6 +814,7 @@ static int vhost_net_release(struct inode *inode, struct file *f)
 	vhost_net_flush(n);
 	vhost_dev_stop(&n->dev);
 	vhost_dev_cleanup(&n->dev, false);
+	vhost_net_vq_reset(n);
 	if (tx_sock)
 		fput(tx_sock->file);
 	if (rx_sock)
@@ -727,6 +822,7 @@ static int vhost_net_release(struct inode *inode, struct file *f)
 	/* We do an extra flush before freeing memory,
 	 * since jobs can re-queue themselves. */
 	vhost_net_flush(n);
+	kfree(n->dev.vqs);
 	kfree(n);
 	return 0;
 }
@@ -800,7 +896,8 @@ static long vhost_net_set_backend(struct vhost_net *n, unsigned index, int fd)
 {
 	struct socket *sock, *oldsock;
 	struct vhost_virtqueue *vq;
-	struct vhost_ubuf_ref *ubufs, *oldubufs = NULL;
+	struct vhost_net_virtqueue *nvq;
+	struct vhost_net_ubuf_ref *ubufs, *oldubufs = NULL;
 	int r;
 
 	mutex_lock(&n->dev.mutex);
@@ -812,7 +909,8 @@ static long vhost_net_set_backend(struct vhost_net *n, unsigned index, int fd)
 		r = -ENOBUFS;
 		goto err;
 	}
-	vq = n->vqs + index;
+	vq = &n->vqs[index].vq;
+	nvq = &n->vqs[index];
 	mutex_lock(&vq->mutex);
 
 	/* Verify that ring has been setup correctly. */
@@ -830,7 +928,8 @@ static long vhost_net_set_backend(struct vhost_net *n, unsigned index, int fd)
 	oldsock = rcu_dereference_protected(vq->private_data,
 					    lockdep_is_held(&vq->mutex));
 	if (sock != oldsock) {
-		ubufs = vhost_ubuf_alloc(vq, sock && vhost_sock_zcopy(sock));
+		ubufs = vhost_net_ubuf_alloc(vq,
+					     sock && vhost_sock_zcopy(sock));
 		if (IS_ERR(ubufs)) {
 			r = PTR_ERR(ubufs);
 			goto err_ubufs;
@@ -845,8 +944,8 @@ static long vhost_net_set_backend(struct vhost_net *n, unsigned index, int fd)
 		if (r)
 			goto err_used;
 
-		oldubufs = vq->ubufs;
-		vq->ubufs = ubufs;
+		oldubufs = nvq->ubufs;
+		nvq->ubufs = ubufs;
 
 		n->tx_packets = 0;
 		n->tx_zcopy_err = 0;
@@ -856,7 +955,7 @@ static long vhost_net_set_backend(struct vhost_net *n, unsigned index, int fd)
 	mutex_unlock(&vq->mutex);
 
 	if (oldubufs) {
-		vhost_ubuf_put_and_wait(oldubufs);
+		vhost_net_ubuf_put_and_wait(oldubufs);
 		mutex_lock(&vq->mutex);
 		vhost_zerocopy_signal_used(n, vq);
 		mutex_unlock(&vq->mutex);
@@ -874,7 +973,7 @@ err_used:
 	rcu_assign_pointer(vq->private_data, oldsock);
 	vhost_net_enable_vq(n, vq);
 	if (ubufs)
-		vhost_ubuf_put_and_wait(ubufs);
+		vhost_net_ubuf_put_and_wait(ubufs);
 err_ubufs:
 	fput(sock->file);
 err_vq:
@@ -889,14 +988,21 @@ static long vhost_net_reset_owner(struct vhost_net *n)
 	struct socket *tx_sock = NULL;
 	struct socket *rx_sock = NULL;
 	long err;
+	struct vhost_memory *memory;
 
 	mutex_lock(&n->dev.mutex);
 	err = vhost_dev_check_owner(&n->dev);
 	if (err)
 		goto done;
+	memory = vhost_dev_reset_owner_prepare();
+	if (!memory) {
+		err = -ENOMEM;
+		goto done;
+	}
 	vhost_net_stop(n, &tx_sock, &rx_sock);
 	vhost_net_flush(n);
-	err = vhost_dev_reset_owner(&n->dev);
+	vhost_dev_reset_owner(&n->dev, memory);
+	vhost_net_vq_reset(n);
 done:
 	mutex_unlock(&n->dev.mutex);
 	if (tx_sock)
@@ -932,16 +1038,33 @@ static int vhost_net_set_features(struct vhost_net *n, u64 features)
 	n->dev.acked_features = features;
 	smp_wmb();
 	for (i = 0; i < VHOST_NET_VQ_MAX; ++i) {
-		mutex_lock(&n->vqs[i].mutex);
+		mutex_lock(&n->vqs[i].vq.mutex);
 		n->vqs[i].vhost_hlen = vhost_hlen;
 		n->vqs[i].sock_hlen = sock_hlen;
-		mutex_unlock(&n->vqs[i].mutex);
+		mutex_unlock(&n->vqs[i].vq.mutex);
 	}
 	vhost_net_flush(n);
 	mutex_unlock(&n->dev.mutex);
 	return 0;
 }
 
+static long vhost_net_set_owner(struct vhost_net *n)
+{
+	int r;
+
+	mutex_lock(&n->dev.mutex);
+	r = vhost_net_set_ubuf_info(n);
+	if (r)
+		goto out;
+	r = vhost_dev_set_owner(&n->dev);
+	if (r)
+		vhost_net_clear_ubuf_info(n);
+	vhost_net_flush(n);
+out:
+	mutex_unlock(&n->dev.mutex);
+	return r;
+}
+
 static long vhost_net_ioctl(struct file *f, unsigned int ioctl,
 			    unsigned long arg)
 {
@@ -970,6 +1093,8 @@ static long vhost_net_ioctl(struct file *f, unsigned int ioctl,
 		return vhost_net_set_features(n, features);
 	case VHOST_RESET_OWNER:
 		return vhost_net_reset_owner(n);
+	case VHOST_SET_OWNER:
+		return vhost_net_set_owner(n);
 	default:
 		mutex_lock(&n->dev.mutex);
 		r = vhost_dev_ioctl(&n->dev, ioctl, argp);
@@ -1010,7 +1135,7 @@ static struct miscdevice vhost_net_misc = {
 static int vhost_net_init(void)
 {
 	if (experimental_zcopytx)
-		vhost_enable_zcopy(VHOST_NET_VQ_TX);
+		vhost_net_enable_zcopy(VHOST_NET_VQ_TX);
 	return misc_register(&vhost_net_misc);
 }
 module_init(vhost_net_init);
diff --git a/drivers/vhost/tcm_vhost.c b/drivers/vhost/scsi.c
index 957a0b98a5d9..701420297225 100644
--- a/drivers/vhost/tcm_vhost.c
+++ b/drivers/vhost/scsi.c
@@ -45,14 +45,116 @@
 #include <target/target_core_configfs.h>
 #include <target/configfs_macros.h>
 #include <linux/vhost.h>
-#include <linux/virtio_net.h> /* TODO vhost.h currently depends on this */
 #include <linux/virtio_scsi.h>
 #include <linux/llist.h>
 #include <linux/bitmap.h>
 
 #include "vhost.c"
 #include "vhost.h"
-#include "tcm_vhost.h"
+
+#define TCM_VHOST_VERSION  "v0.1"
+#define TCM_VHOST_NAMELEN 256
+#define TCM_VHOST_MAX_CDB_SIZE 32
+
+struct vhost_scsi_inflight {
+	/* Wait for the flush operation to finish */
+	struct completion comp;
+	/* Refcount for the inflight reqs */
+	struct kref kref;
+};
+
+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 */
+	u32 tvc_sgl_count;
+	/* Saved unpacked SCSI LUN for tcm_vhost_submission_work() */
+	u32 tvc_lun;
+	/* Pointer to the SGL formatted memory from virtio-scsi */
+	struct scatterlist *tvc_sgl;
+	/* Pointer to response */
+	struct virtio_scsi_cmd_resp __user *tvc_resp;
+	/* Pointer to vhost_scsi for our device */
+	struct vhost_scsi *tvc_vhost;
+	/* Pointer to vhost_virtqueue for the cmd */
+	struct vhost_virtqueue *tvc_vq;
+	/* 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() */
+	struct work_struct work;
+	/* Copy of the incoming SCSI command descriptor block (CDB) */
+	unsigned char tvc_cdb[TCM_VHOST_MAX_CDB_SIZE];
+	/* Sense buffer that will be mapped into outgoing status */
+	unsigned char tvc_sense_buf[TRANSPORT_SENSE_BUFFER];
+	/* Completed commands list, serviced from vhost worker thread */
+	struct llist_node tvc_completion_list;
+	/* Used to track inflight cmd */
+	struct vhost_scsi_inflight *inflight;
+};
+
+struct tcm_vhost_nexus {
+	/* Pointer to TCM session for I_T Nexus */
+	struct se_session *tvn_se_sess;
+};
+
+struct tcm_vhost_nacl {
+	/* Binary World Wide unique Port Name for Vhost Initiator port */
+	u64 iport_wwpn;
+	/* ASCII formatted WWPN for Sas Initiator port */
+	char iport_name[TCM_VHOST_NAMELEN];
+	/* Returned by tcm_vhost_make_nodeacl() */
+	struct se_node_acl se_node_acl;
+};
+
+struct vhost_scsi;
+struct tcm_vhost_tpg {
+	/* Vhost port target portal group tag for TCM */
+	u16 tport_tpgt;
+	/* Used to track number of TPG Port/Lun Links wrt to explict I_T Nexus shutdown */
+	int tv_tpg_port_count;
+	/* Used for vhost_scsi device reference to tpg_nexus, protected by tv_tpg_mutex */
+	int tv_tpg_vhost_count;
+	/* list for tcm_vhost_list */
+	struct list_head tv_tpg_list;
+	/* Used to protect access for tpg_nexus */
+	struct mutex tv_tpg_mutex;
+	/* Pointer to the TCM VHost I_T Nexus for this TPG endpoint */
+	struct tcm_vhost_nexus *tpg_nexus;
+	/* Pointer back to tcm_vhost_tport */
+	struct tcm_vhost_tport *tport;
+	/* Returned by tcm_vhost_make_tpg() */
+	struct se_portal_group se_tpg;
+	/* Pointer back to vhost_scsi, protected by tv_tpg_mutex */
+	struct vhost_scsi *vhost_scsi;
+};
+
+struct tcm_vhost_tport {
+	/* SCSI protocol the tport is providing */
+	u8 tport_proto_id;
+	/* Binary World Wide unique Port Name for Vhost Target port */
+	u64 tport_wwpn;
+	/* ASCII formatted WWPN for Vhost Target port */
+	char tport_name[TCM_VHOST_NAMELEN];
+	/* Returned by tcm_vhost_make_tport() */
+	struct se_wwn tport_wwn;
+};
+
+struct tcm_vhost_evt {
+	/* event to be sent to guest */
+	struct virtio_scsi_event event;
+	/* event list, serviced from vhost worker thread */
+	struct llist_node list;
+};
 
 enum {
 	VHOST_SCSI_VQ_CTL = 0,
@@ -60,17 +162,28 @@ enum {
 	VHOST_SCSI_VQ_IO = 2,
 };
 
-/*
- * VIRTIO_RING_F_EVENT_IDX seems broken. Not sure the bug is in
- * kernel but disabling it helps.
- * TODO: debug and remove the workaround.
- */
 enum {
-	VHOST_SCSI_FEATURES = VHOST_FEATURES & (~VIRTIO_RING_F_EVENT_IDX)
+	VHOST_SCSI_FEATURES = VHOST_FEATURES | (1ULL << VIRTIO_SCSI_F_HOTPLUG)
 };
 
 #define VHOST_SCSI_MAX_TARGET	256
 #define VHOST_SCSI_MAX_VQ	128
+#define VHOST_SCSI_MAX_EVENT	128
+
+struct vhost_scsi_virtqueue {
+	struct vhost_virtqueue vq;
+	/*
+	 * Reference counting for inflight reqs, used for flush operation. At
+	 * each time, one reference tracks new commands submitted, while we
+	 * wait for another one to reach 0.
+	 */
+	struct vhost_scsi_inflight inflights[2];
+	/*
+	 * Indicate current inflight in use, protected by vq->mutex.
+	 * Writers must also take dev mutex and flush under it.
+	 */
+	int inflight_idx;
+};
 
 struct vhost_scsi {
 	/* Protected by vhost_scsi->dev.mutex */
@@ -78,10 +191,16 @@ struct vhost_scsi {
 	char vs_vhost_wwpn[TRANSPORT_IQN_LEN];
 
 	struct vhost_dev dev;
-	struct vhost_virtqueue vqs[VHOST_SCSI_MAX_VQ];
+	struct vhost_scsi_virtqueue vqs[VHOST_SCSI_MAX_VQ];
 
 	struct vhost_work vs_completion_work; /* cmd completion work item */
 	struct llist_head vs_completion_list; /* cmd completion queue */
+
+	struct vhost_work vs_event_work; /* evt injection work item */
+	struct llist_head vs_event_list; /* evt injection queue */
+
+	bool vs_events_missed; /* any missed events, protected by vq->mutex */
+	int vs_events_nr; /* num of pending events, protected by vq->mutex */
 };
 
 /* Local pointer to allocated TCM configfs fabric module */
@@ -99,6 +218,59 @@ static int iov_num_pages(struct iovec *iov)
 	       ((unsigned long)iov->iov_base & PAGE_MASK)) >> PAGE_SHIFT;
 }
 
+void tcm_vhost_done_inflight(struct kref *kref)
+{
+	struct vhost_scsi_inflight *inflight;
+
+	inflight = container_of(kref, struct vhost_scsi_inflight, kref);
+	complete(&inflight->comp);
+}
+
+static void tcm_vhost_init_inflight(struct vhost_scsi *vs,
+				    struct vhost_scsi_inflight *old_inflight[])
+{
+	struct vhost_scsi_inflight *new_inflight;
+	struct vhost_virtqueue *vq;
+	int idx, i;
+
+	for (i = 0; i < VHOST_SCSI_MAX_VQ; i++) {
+		vq = &vs->vqs[i].vq;
+
+		mutex_lock(&vq->mutex);
+
+		/* store old infight */
+		idx = vs->vqs[i].inflight_idx;
+		if (old_inflight)
+			old_inflight[i] = &vs->vqs[i].inflights[idx];
+
+		/* setup new infight */
+		vs->vqs[i].inflight_idx = idx ^ 1;
+		new_inflight = &vs->vqs[i].inflights[idx ^ 1];
+		kref_init(&new_inflight->kref);
+		init_completion(&new_inflight->comp);
+
+		mutex_unlock(&vq->mutex);
+	}
+}
+
+static struct vhost_scsi_inflight *
+tcm_vhost_get_inflight(struct vhost_virtqueue *vq)
+{
+	struct vhost_scsi_inflight *inflight;
+	struct vhost_scsi_virtqueue *svq;
+
+	svq = container_of(vq, struct vhost_scsi_virtqueue, vq);
+	inflight = &svq->inflights[svq->inflight_idx];
+	kref_get(&inflight->kref);
+
+	return inflight;
+}
+
+static void tcm_vhost_put_inflight(struct vhost_scsi_inflight *inflight)
+{
+	kref_put(&inflight->kref, tcm_vhost_done_inflight);
+}
+
 static int tcm_vhost_check_true(struct se_portal_group *se_tpg)
 {
 	return 1;
@@ -349,6 +521,37 @@ static int tcm_vhost_queue_tm_rsp(struct se_cmd *se_cmd)
 	return 0;
 }
 
+static void tcm_vhost_free_evt(struct vhost_scsi *vs, struct tcm_vhost_evt *evt)
+{
+	vs->vs_events_nr--;
+	kfree(evt);
+}
+
+static struct tcm_vhost_evt *tcm_vhost_allocate_evt(struct vhost_scsi *vs,
+	u32 event, u32 reason)
+{
+	struct vhost_virtqueue *vq = &vs->vqs[VHOST_SCSI_VQ_EVT].vq;
+	struct tcm_vhost_evt *evt;
+
+	if (vs->vs_events_nr > VHOST_SCSI_MAX_EVENT) {
+		vs->vs_events_missed = true;
+		return NULL;
+	}
+
+	evt = kzalloc(sizeof(*evt), GFP_KERNEL);
+	if (!evt) {
+		vq_err(vq, "Failed to allocate tcm_vhost_evt\n");
+		vs->vs_events_missed = true;
+		return NULL;
+	}
+
+	evt->event.event = event;
+	evt->event.reason = reason;
+	vs->vs_events_nr++;
+
+	return evt;
+}
+
 static void vhost_scsi_free_cmd(struct tcm_vhost_cmd *tv_cmd)
 {
 	struct se_cmd *se_cmd = &tv_cmd->tvc_se_cmd;
@@ -364,9 +567,80 @@ static void vhost_scsi_free_cmd(struct tcm_vhost_cmd *tv_cmd)
 		kfree(tv_cmd->tvc_sgl);
 	}
 
+	tcm_vhost_put_inflight(tv_cmd->inflight);
+
 	kfree(tv_cmd);
 }
 
+static void tcm_vhost_do_evt_work(struct vhost_scsi *vs,
+	struct tcm_vhost_evt *evt)
+{
+	struct vhost_virtqueue *vq = &vs->vqs[VHOST_SCSI_VQ_EVT].vq;
+	struct virtio_scsi_event *event = &evt->event;
+	struct virtio_scsi_event __user *eventp;
+	unsigned out, in;
+	int head, ret;
+
+	if (!vq->private_data) {
+		vs->vs_events_missed = true;
+		return;
+	}
+
+again:
+	vhost_disable_notify(&vs->dev, vq);
+	head = vhost_get_vq_desc(&vs->dev, vq, vq->iov,
+			ARRAY_SIZE(vq->iov), &out, &in,
+			NULL, NULL);
+	if (head < 0) {
+		vs->vs_events_missed = true;
+		return;
+	}
+	if (head == vq->num) {
+		if (vhost_enable_notify(&vs->dev, vq))
+			goto again;
+		vs->vs_events_missed = true;
+		return;
+	}
+
+	if ((vq->iov[out].iov_len != sizeof(struct virtio_scsi_event))) {
+		vq_err(vq, "Expecting virtio_scsi_event, got %zu bytes\n",
+				vq->iov[out].iov_len);
+		vs->vs_events_missed = true;
+		return;
+	}
+
+	if (vs->vs_events_missed) {
+		event->event |= VIRTIO_SCSI_T_EVENTS_MISSED;
+		vs->vs_events_missed = false;
+	}
+
+	eventp = vq->iov[out].iov_base;
+	ret = __copy_to_user(eventp, event, sizeof(*event));
+	if (!ret)
+		vhost_add_used_and_signal(&vs->dev, vq, head, 0);
+	else
+		vq_err(vq, "Faulted on tcm_vhost_send_event\n");
+}
+
+static void tcm_vhost_evt_work(struct vhost_work *work)
+{
+	struct vhost_scsi *vs = container_of(work, struct vhost_scsi,
+					vs_event_work);
+	struct vhost_virtqueue *vq = &vs->vqs[VHOST_SCSI_VQ_EVT].vq;
+	struct tcm_vhost_evt *evt;
+	struct llist_node *llnode;
+
+	mutex_lock(&vq->mutex);
+	llnode = llist_del_all(&vs->vs_event_list);
+	while (llnode) {
+		evt = llist_entry(llnode, struct tcm_vhost_evt, list);
+		llnode = llist_next(llnode);
+		tcm_vhost_do_evt_work(vs, evt);
+		tcm_vhost_free_evt(vs, evt);
+	}
+	mutex_unlock(&vq->mutex);
+}
+
 /* Fill in status and signal that we are done processing this command
  *
  * This is scheduled in the vhost work queue so we are called with the owner
@@ -403,8 +677,10 @@ static void vhost_scsi_complete_cmd_work(struct vhost_work *work)
 		       v_rsp.sense_len);
 		ret = copy_to_user(tv_cmd->tvc_resp, &v_rsp, sizeof(v_rsp));
 		if (likely(ret == 0)) {
+			struct vhost_scsi_virtqueue *q;
 			vhost_add_used(tv_cmd->tvc_vq, tv_cmd->tvc_vq_desc, 0);
-			vq = tv_cmd->tvc_vq - vs->vqs;
+			q = container_of(tv_cmd->tvc_vq, struct vhost_scsi_virtqueue, vq);
+			vq = q - vs->vqs;
 			__set_bit(vq, signal);
 		} else
 			pr_err("Faulted on virtio_scsi_cmd_resp\n");
@@ -415,10 +691,11 @@ static void vhost_scsi_complete_cmd_work(struct vhost_work *work)
 	vq = -1;
 	while ((vq = find_next_bit(signal, VHOST_SCSI_MAX_VQ, vq + 1))
 		< VHOST_SCSI_MAX_VQ)
-		vhost_signal(&vs->dev, &vs->vqs[vq]);
+		vhost_signal(&vs->dev, &vs->vqs[vq].vq);
 }
 
 static struct tcm_vhost_cmd *vhost_scsi_allocate_cmd(
+	struct vhost_virtqueue *vq,
 	struct tcm_vhost_tpg *tv_tpg,
 	struct virtio_scsi_cmd_req *v_req,
 	u32 exp_data_len,
@@ -443,6 +720,7 @@ static struct tcm_vhost_cmd *vhost_scsi_allocate_cmd(
 	tv_cmd->tvc_exp_data_len = exp_data_len;
 	tv_cmd->tvc_data_direction = data_direction;
 	tv_cmd->tvc_nexus = tv_nexus;
+	tv_cmd->inflight = tcm_vhost_get_inflight(vq);
 
 	return tv_cmd;
 }
@@ -698,7 +976,7 @@ static void vhost_scsi_handle_vq(struct vhost_scsi *vs,
 		for (i = 0; i < data_num; i++)
 			exp_data_len += vq->iov[data_first + i].iov_len;
 
-		tv_cmd = vhost_scsi_allocate_cmd(tv_tpg, &v_req,
+		tv_cmd = vhost_scsi_allocate_cmd(vq, tv_tpg, &v_req,
 					exp_data_len, data_direction);
 		if (IS_ERR(tv_cmd)) {
 			vq_err(vq, "vhost_scsi_allocate_cmd failed %ld\n",
@@ -777,9 +1055,46 @@ static void vhost_scsi_ctl_handle_kick(struct vhost_work *work)
 	pr_debug("%s: The handling func for control queue.\n", __func__);
 }
 
+static void tcm_vhost_send_evt(struct vhost_scsi *vs, struct tcm_vhost_tpg *tpg,
+	struct se_lun *lun, u32 event, u32 reason)
+{
+	struct tcm_vhost_evt *evt;
+
+	evt = tcm_vhost_allocate_evt(vs, event, reason);
+	if (!evt)
+		return;
+
+	if (tpg && lun) {
+		/* TODO: share lun setup code with virtio-scsi.ko */
+		/*
+		 * Note: evt->event is zeroed when we allocate it and
+		 * lun[4-7] need to be zero according to virtio-scsi spec.
+		 */
+		evt->event.lun[0] = 0x01;
+		evt->event.lun[1] = tpg->tport_tpgt & 0xFF;
+		if (lun->unpacked_lun >= 256)
+			evt->event.lun[2] = lun->unpacked_lun >> 8 | 0x40 ;
+		evt->event.lun[3] = lun->unpacked_lun & 0xFF;
+	}
+
+	llist_add(&evt->list, &vs->vs_event_list);
+	vhost_work_queue(&vs->dev, &vs->vs_event_work);
+}
+
 static void vhost_scsi_evt_handle_kick(struct vhost_work *work)
 {
-	pr_debug("%s: The handling func for event queue.\n", __func__);
+	struct vhost_virtqueue *vq = container_of(work, struct vhost_virtqueue,
+						poll.work);
+	struct vhost_scsi *vs = container_of(vq->dev, struct vhost_scsi, dev);
+
+	mutex_lock(&vq->mutex);
+	if (!vq->private_data)
+		goto out;
+
+	if (vs->vs_events_missed)
+		tcm_vhost_send_evt(vs, NULL, NULL, VIRTIO_SCSI_T_NO_EVENT, 0);
+out:
+	mutex_unlock(&vq->mutex);
 }
 
 static void vhost_scsi_handle_kick(struct vhost_work *work)
@@ -793,21 +1108,43 @@ static void vhost_scsi_handle_kick(struct vhost_work *work)
 
 static void vhost_scsi_flush_vq(struct vhost_scsi *vs, int index)
 {
-	vhost_poll_flush(&vs->dev.vqs[index].poll);
+	vhost_poll_flush(&vs->vqs[index].vq.poll);
 }
 
+/* Callers must hold dev mutex */
 static void vhost_scsi_flush(struct vhost_scsi *vs)
 {
+	struct vhost_scsi_inflight *old_inflight[VHOST_SCSI_MAX_VQ];
 	int i;
 
+	/* Init new inflight and remember the old inflight */
+	tcm_vhost_init_inflight(vs, old_inflight);
+
+	/*
+	 * The inflight->kref was initialized to 1. We decrement it here to
+	 * indicate the start of the flush operation so that it will reach 0
+	 * when all the reqs are finished.
+	 */
+	for (i = 0; i < VHOST_SCSI_MAX_VQ; i++)
+		kref_put(&old_inflight[i]->kref, tcm_vhost_done_inflight);
+
+	/* Flush both the vhost poll and vhost work */
 	for (i = 0; i < VHOST_SCSI_MAX_VQ; i++)
 		vhost_scsi_flush_vq(vs, i);
 	vhost_work_flush(&vs->dev, &vs->vs_completion_work);
+	vhost_work_flush(&vs->dev, &vs->vs_event_work);
+
+	/* Wait for all reqs issued before the flush to be finished */
+	for (i = 0; i < VHOST_SCSI_MAX_VQ; i++)
+		wait_for_completion(&old_inflight[i]->comp);
 }
 
 /*
  * Called from vhost_scsi_ioctl() context to walk the list of available
  * tcm_vhost_tpg with an active struct tcm_vhost_nexus
+ *
+ *  The lock nesting rule is:
+ *    tcm_vhost_mutex -> vs->dev.mutex -> tpg->tv_tpg_mutex -> vq->mutex
  */
 static int vhost_scsi_set_endpoint(
 	struct vhost_scsi *vs,
@@ -820,26 +1157,27 @@ static int vhost_scsi_set_endpoint(
 	int index, ret, i, len;
 	bool match = false;
 
+	mutex_lock(&tcm_vhost_mutex);
 	mutex_lock(&vs->dev.mutex);
+
 	/* Verify that ring has been setup correctly. */
 	for (index = 0; index < vs->dev.nvqs; ++index) {
 		/* Verify that ring has been setup correctly. */
-		if (!vhost_vq_access_ok(&vs->vqs[index])) {
-			mutex_unlock(&vs->dev.mutex);
-			return -EFAULT;
+		if (!vhost_vq_access_ok(&vs->vqs[index].vq)) {
+			ret = -EFAULT;
+			goto out;
 		}
 	}
 
 	len = sizeof(vs_tpg[0]) * VHOST_SCSI_MAX_TARGET;
 	vs_tpg = kzalloc(len, GFP_KERNEL);
 	if (!vs_tpg) {
-		mutex_unlock(&vs->dev.mutex);
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto out;
 	}
 	if (vs->vs_tpg)
 		memcpy(vs_tpg, vs->vs_tpg, len);
 
-	mutex_lock(&tcm_vhost_mutex);
 	list_for_each_entry(tv_tpg, &tcm_vhost_list, tv_tpg_list) {
 		mutex_lock(&tv_tpg->tv_tpg_mutex);
 		if (!tv_tpg->tpg_nexus) {
@@ -854,26 +1192,25 @@ static int vhost_scsi_set_endpoint(
 
 		if (!strcmp(tv_tport->tport_name, t->vhost_wwpn)) {
 			if (vs->vs_tpg && vs->vs_tpg[tv_tpg->tport_tpgt]) {
-				mutex_unlock(&tv_tpg->tv_tpg_mutex);
-				mutex_unlock(&tcm_vhost_mutex);
-				mutex_unlock(&vs->dev.mutex);
 				kfree(vs_tpg);
-				return -EEXIST;
+				mutex_unlock(&tv_tpg->tv_tpg_mutex);
+				ret = -EEXIST;
+				goto out;
 			}
 			tv_tpg->tv_tpg_vhost_count++;
+			tv_tpg->vhost_scsi = vs;
 			vs_tpg[tv_tpg->tport_tpgt] = tv_tpg;
 			smp_mb__after_atomic_inc();
 			match = true;
 		}
 		mutex_unlock(&tv_tpg->tv_tpg_mutex);
 	}
-	mutex_unlock(&tcm_vhost_mutex);
 
 	if (match) {
 		memcpy(vs->vs_vhost_wwpn, t->vhost_wwpn,
 		       sizeof(vs->vs_vhost_wwpn));
 		for (i = 0; i < VHOST_SCSI_MAX_VQ; i++) {
-			vq = &vs->vqs[i];
+			vq = &vs->vqs[i].vq;
 			/* Flushing the vhost_work acts as synchronize_rcu */
 			mutex_lock(&vq->mutex);
 			rcu_assign_pointer(vq->private_data, vs_tpg);
@@ -893,7 +1230,9 @@ static int vhost_scsi_set_endpoint(
 	kfree(vs->vs_tpg);
 	vs->vs_tpg = vs_tpg;
 
+out:
 	mutex_unlock(&vs->dev.mutex);
+	mutex_unlock(&tcm_vhost_mutex);
 	return ret;
 }
 
@@ -908,18 +1247,19 @@ static int vhost_scsi_clear_endpoint(
 	int index, ret, i;
 	u8 target;
 
+	mutex_lock(&tcm_vhost_mutex);
 	mutex_lock(&vs->dev.mutex);
 	/* Verify that ring has been setup correctly. */
 	for (index = 0; index < vs->dev.nvqs; ++index) {
-		if (!vhost_vq_access_ok(&vs->vqs[index])) {
+		if (!vhost_vq_access_ok(&vs->vqs[index].vq)) {
 			ret = -EFAULT;
 			goto err_dev;
 		}
 	}
 
 	if (!vs->vs_tpg) {
-		mutex_unlock(&vs->dev.mutex);
-		return 0;
+		ret = 0;
+		goto err_dev;
 	}
 
 	for (i = 0; i < VHOST_SCSI_MAX_TARGET; i++) {
@@ -944,13 +1284,14 @@ static int vhost_scsi_clear_endpoint(
 			goto err_tpg;
 		}
 		tv_tpg->tv_tpg_vhost_count--;
+		tv_tpg->vhost_scsi = NULL;
 		vs->vs_tpg[target] = NULL;
 		match = true;
 		mutex_unlock(&tv_tpg->tv_tpg_mutex);
 	}
 	if (match) {
 		for (i = 0; i < VHOST_SCSI_MAX_VQ; i++) {
-			vq = &vs->vqs[i];
+			vq = &vs->vqs[i].vq;
 			/* Flushing the vhost_work acts as synchronize_rcu */
 			mutex_lock(&vq->mutex);
 			rcu_assign_pointer(vq->private_data, NULL);
@@ -964,14 +1305,16 @@ static int vhost_scsi_clear_endpoint(
 	vhost_scsi_flush(vs);
 	kfree(vs->vs_tpg);
 	vs->vs_tpg = NULL;
+	WARN_ON(vs->vs_events_nr);
 	mutex_unlock(&vs->dev.mutex);
-
+	mutex_unlock(&tcm_vhost_mutex);
 	return 0;
 
 err_tpg:
 	mutex_unlock(&tv_tpg->tv_tpg_mutex);
 err_dev:
 	mutex_unlock(&vs->dev.mutex);
+	mutex_unlock(&tcm_vhost_mutex);
 	return ret;
 }
 
@@ -996,20 +1339,39 @@ static int vhost_scsi_set_features(struct vhost_scsi *vs, u64 features)
 static int vhost_scsi_open(struct inode *inode, struct file *f)
 {
 	struct vhost_scsi *s;
+	struct vhost_virtqueue **vqs;
 	int r, i;
 
 	s = kzalloc(sizeof(*s), GFP_KERNEL);
 	if (!s)
 		return -ENOMEM;
 
+	vqs = kmalloc(VHOST_SCSI_MAX_VQ * sizeof(*vqs), GFP_KERNEL);
+	if (!vqs) {
+		kfree(s);
+		return -ENOMEM;
+	}
+
 	vhost_work_init(&s->vs_completion_work, vhost_scsi_complete_cmd_work);
+	vhost_work_init(&s->vs_event_work, tcm_vhost_evt_work);
+
+	s->vs_events_nr = 0;
+	s->vs_events_missed = false;
+
+	vqs[VHOST_SCSI_VQ_CTL] = &s->vqs[VHOST_SCSI_VQ_CTL].vq;
+	vqs[VHOST_SCSI_VQ_EVT] = &s->vqs[VHOST_SCSI_VQ_EVT].vq;
+	s->vqs[VHOST_SCSI_VQ_CTL].vq.handle_kick = vhost_scsi_ctl_handle_kick;
+	s->vqs[VHOST_SCSI_VQ_EVT].vq.handle_kick = vhost_scsi_evt_handle_kick;
+	for (i = VHOST_SCSI_VQ_IO; i < VHOST_SCSI_MAX_VQ; i++) {
+		vqs[i] = &s->vqs[i].vq;
+		s->vqs[i].vq.handle_kick = vhost_scsi_handle_kick;
+	}
+	r = vhost_dev_init(&s->dev, vqs, VHOST_SCSI_MAX_VQ);
+
+	tcm_vhost_init_inflight(s, NULL);
 
-	s->vqs[VHOST_SCSI_VQ_CTL].handle_kick = vhost_scsi_ctl_handle_kick;
-	s->vqs[VHOST_SCSI_VQ_EVT].handle_kick = vhost_scsi_evt_handle_kick;
-	for (i = VHOST_SCSI_VQ_IO; i < VHOST_SCSI_MAX_VQ; i++)
-		s->vqs[i].handle_kick = vhost_scsi_handle_kick;
-	r = vhost_dev_init(&s->dev, s->vqs, VHOST_SCSI_MAX_VQ);
 	if (r < 0) {
+		kfree(vqs);
 		kfree(s);
 		return r;
 	}
@@ -1029,6 +1391,9 @@ static int vhost_scsi_release(struct inode *inode, struct file *f)
 	vhost_scsi_clear_endpoint(s, &t);
 	vhost_dev_stop(&s->dev);
 	vhost_dev_cleanup(&s->dev, false);
+	/* Jobs can re-queue themselves in evt kick handler. Do extra flush. */
+	vhost_scsi_flush(s);
+	kfree(s->dev.vqs);
 	kfree(s);
 	return 0;
 }
@@ -1040,8 +1405,11 @@ static long vhost_scsi_ioctl(struct file *f, unsigned int ioctl,
 	struct vhost_scsi_target backend;
 	void __user *argp = (void __user *)arg;
 	u64 __user *featurep = argp;
+	u32 __user *eventsp = argp;
+	u32 events_missed;
 	u64 features;
 	int r, abi_version = VHOST_SCSI_ABI_VERSION;
+	struct vhost_virtqueue *vq = &vs->vqs[VHOST_SCSI_VQ_EVT].vq;
 
 	switch (ioctl) {
 	case VHOST_SCSI_SET_ENDPOINT:
@@ -1062,6 +1430,20 @@ static long vhost_scsi_ioctl(struct file *f, unsigned int ioctl,
 		if (copy_to_user(argp, &abi_version, sizeof abi_version))
 			return -EFAULT;
 		return 0;
+	case VHOST_SCSI_SET_EVENTS_MISSED:
+		if (get_user(events_missed, eventsp))
+			return -EFAULT;
+		mutex_lock(&vq->mutex);
+		vs->vs_events_missed = events_missed;
+		mutex_unlock(&vq->mutex);
+		return 0;
+	case VHOST_SCSI_GET_EVENTS_MISSED:
+		mutex_lock(&vq->mutex);
+		events_missed = vs->vs_events_missed;
+		mutex_unlock(&vq->mutex);
+		if (put_user(events_missed, eventsp))
+			return -EFAULT;
+		return 0;
 	case VHOST_GET_FEATURES:
 		features = VHOST_SCSI_FEATURES;
 		if (copy_to_user(featurep, &features, sizeof features))
@@ -1133,28 +1515,80 @@ static char *tcm_vhost_dump_proto_id(struct tcm_vhost_tport *tport)
 	return "Unknown";
 }
 
+static void tcm_vhost_do_plug(struct tcm_vhost_tpg *tpg,
+	struct se_lun *lun, bool plug)
+{
+
+	struct vhost_scsi *vs = tpg->vhost_scsi;
+	struct vhost_virtqueue *vq;
+	u32 reason;
+
+	if (!vs)
+		return;
+
+	mutex_lock(&vs->dev.mutex);
+	if (!vhost_has_feature(&vs->dev, VIRTIO_SCSI_F_HOTPLUG)) {
+		mutex_unlock(&vs->dev.mutex);
+		return;
+	}
+
+	if (plug)
+		reason = VIRTIO_SCSI_EVT_RESET_RESCAN;
+	else
+		reason = VIRTIO_SCSI_EVT_RESET_REMOVED;
+
+	vq = &vs->vqs[VHOST_SCSI_VQ_EVT].vq;
+	mutex_lock(&vq->mutex);
+	tcm_vhost_send_evt(vs, tpg, lun,
+			VIRTIO_SCSI_T_TRANSPORT_RESET, reason);
+	mutex_unlock(&vq->mutex);
+	mutex_unlock(&vs->dev.mutex);
+}
+
+static void tcm_vhost_hotplug(struct tcm_vhost_tpg *tpg, struct se_lun *lun)
+{
+	tcm_vhost_do_plug(tpg, lun, true);
+}
+
+static void tcm_vhost_hotunplug(struct tcm_vhost_tpg *tpg, struct se_lun *lun)
+{
+	tcm_vhost_do_plug(tpg, lun, false);
+}
+
 static int tcm_vhost_port_link(struct se_portal_group *se_tpg,
 	struct se_lun *lun)
 {
 	struct tcm_vhost_tpg *tv_tpg = container_of(se_tpg,
 				struct tcm_vhost_tpg, se_tpg);
 
+	mutex_lock(&tcm_vhost_mutex);
+
 	mutex_lock(&tv_tpg->tv_tpg_mutex);
 	tv_tpg->tv_tpg_port_count++;
 	mutex_unlock(&tv_tpg->tv_tpg_mutex);
 
+	tcm_vhost_hotplug(tv_tpg, lun);
+
+	mutex_unlock(&tcm_vhost_mutex);
+
 	return 0;
 }
 
 static void tcm_vhost_port_unlink(struct se_portal_group *se_tpg,
-	struct se_lun *se_lun)
+	struct se_lun *lun)
 {
 	struct tcm_vhost_tpg *tv_tpg = container_of(se_tpg,
 				struct tcm_vhost_tpg, se_tpg);
 
+	mutex_lock(&tcm_vhost_mutex);
+
 	mutex_lock(&tv_tpg->tv_tpg_mutex);
 	tv_tpg->tv_tpg_port_count--;
 	mutex_unlock(&tv_tpg->tv_tpg_mutex);
+
+	tcm_vhost_hotunplug(tv_tpg, lun);
+
+	mutex_unlock(&tcm_vhost_mutex);
 }
 
 static struct se_node_acl *tcm_vhost_make_nodeacl(
@@ -1696,7 +2130,8 @@ static void tcm_vhost_exit(void)
 	destroy_workqueue(tcm_vhost_workqueue);
 };
 
-MODULE_DESCRIPTION("TCM_VHOST series fabric driver");
+MODULE_DESCRIPTION("VHOST_SCSI series fabric driver");
+MODULE_ALIAS("tcm_vhost");
 MODULE_LICENSE("GPL");
 module_init(tcm_vhost_init);
 module_exit(tcm_vhost_exit);
diff --git a/drivers/vhost/tcm_vhost.h b/drivers/vhost/tcm_vhost.h
deleted file mode 100644
index 1d2ae7a60e11..000000000000
--- a/drivers/vhost/tcm_vhost.h
+++ /dev/null
@@ -1,115 +0,0 @@
-#define TCM_VHOST_VERSION  "v0.1"
-#define TCM_VHOST_NAMELEN 256
-#define TCM_VHOST_MAX_CDB_SIZE 32
-
-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 */
-	u32 tvc_sgl_count;
-	/* Saved unpacked SCSI LUN for tcm_vhost_submission_work() */
-	u32 tvc_lun;
-	/* Pointer to the SGL formatted memory from virtio-scsi */
-	struct scatterlist *tvc_sgl;
-	/* Pointer to response */
-	struct virtio_scsi_cmd_resp __user *tvc_resp;
-	/* Pointer to vhost_scsi for our device */
-	struct vhost_scsi *tvc_vhost;
-	/* Pointer to vhost_virtqueue for the cmd */
-	struct vhost_virtqueue *tvc_vq;
-	/* 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() */
-	struct work_struct work;
-	/* Copy of the incoming SCSI command descriptor block (CDB) */
-	unsigned char tvc_cdb[TCM_VHOST_MAX_CDB_SIZE];
-	/* Sense buffer that will be mapped into outgoing status */
-	unsigned char tvc_sense_buf[TRANSPORT_SENSE_BUFFER];
-	/* Completed commands list, serviced from vhost worker thread */
-	struct llist_node tvc_completion_list;
-};
-
-struct tcm_vhost_nexus {
-	/* Pointer to TCM session for I_T Nexus */
-	struct se_session *tvn_se_sess;
-};
-
-struct tcm_vhost_nacl {
-	/* Binary World Wide unique Port Name for Vhost Initiator port */
-	u64 iport_wwpn;
-	/* ASCII formatted WWPN for Sas Initiator port */
-	char iport_name[TCM_VHOST_NAMELEN];
-	/* Returned by tcm_vhost_make_nodeacl() */
-	struct se_node_acl se_node_acl;
-};
-
-struct tcm_vhost_tpg {
-	/* Vhost port target portal group tag for TCM */
-	u16 tport_tpgt;
-	/* Used to track number of TPG Port/Lun Links wrt to explict I_T Nexus shutdown */
-	int tv_tpg_port_count;
-	/* Used for vhost_scsi device reference to tpg_nexus, protected by tv_tpg_mutex */
-	int tv_tpg_vhost_count;
-	/* list for tcm_vhost_list */
-	struct list_head tv_tpg_list;
-	/* Used to protect access for tpg_nexus */
-	struct mutex tv_tpg_mutex;
-	/* Pointer to the TCM VHost I_T Nexus for this TPG endpoint */
-	struct tcm_vhost_nexus *tpg_nexus;
-	/* Pointer back to tcm_vhost_tport */
-	struct tcm_vhost_tport *tport;
-	/* Returned by tcm_vhost_make_tpg() */
-	struct se_portal_group se_tpg;
-};
-
-struct tcm_vhost_tport {
-	/* SCSI protocol the tport is providing */
-	u8 tport_proto_id;
-	/* Binary World Wide unique Port Name for Vhost Target port */
-	u64 tport_wwpn;
-	/* ASCII formatted WWPN for Vhost Target port */
-	char tport_name[TCM_VHOST_NAMELEN];
-	/* Returned by tcm_vhost_make_tport() */
-	struct se_wwn tport_wwn;
-};
-
-/*
- * As per request from MST, keep TCM_VHOST related ioctl defines out of
- * linux/vhost.h (user-space) for now..
- */
-
-#include <linux/vhost.h>
-
-/*
- * Used by QEMU userspace to ensure a consistent vhost-scsi ABI.
- *
- * ABI Rev 0: July 2012 version starting point for v3.6-rc merge candidate +
- *            RFC-v2 vhost-scsi userspace.  Add GET_ABI_VERSION ioctl usage
- * ABI Rev 1: January 2013. Ignore vhost_tpgt filed in struct vhost_scsi_target.
- *            All the targets under vhost_wwpn can be seen and used by guset.
- */
-
-#define VHOST_SCSI_ABI_VERSION	1
-
-struct vhost_scsi_target {
-	int abi_version;
-	char vhost_wwpn[TRANSPORT_IQN_LEN];
-	unsigned short vhost_tpgt;
-	unsigned short reserved;
-};
-
-/* VHOST_SCSI specific defines */
-#define VHOST_SCSI_SET_ENDPOINT _IOW(VHOST_VIRTIO, 0x40, struct vhost_scsi_target)
-#define VHOST_SCSI_CLEAR_ENDPOINT _IOW(VHOST_VIRTIO, 0x41, struct vhost_scsi_target)
-/* Changing this breaks userspace. */
-#define VHOST_SCSI_GET_ABI_VERSION _IOW(VHOST_VIRTIO, 0x42, int)
diff --git a/drivers/vhost/test.c b/drivers/vhost/test.c
index 91d6f060aade..1ee45bc85f67 100644
--- a/drivers/vhost/test.c
+++ b/drivers/vhost/test.c
@@ -219,13 +219,20 @@ static long vhost_test_reset_owner(struct vhost_test *n)
 {
 	void *priv = NULL;
 	long err;
+	struct vhost_memory *memory;
+
 	mutex_lock(&n->dev.mutex);
 	err = vhost_dev_check_owner(&n->dev);
 	if (err)
 		goto done;
+	memory = vhost_dev_reset_owner_prepare();
+	if (!memory) {
+		err = -ENOMEM;
+		goto done;
+	}
 	vhost_test_stop(n, &priv);
 	vhost_test_flush(n);
-	err = vhost_dev_reset_owner(&n->dev);
+	vhost_dev_reset_owner(&n->dev, memory);
 done:
 	mutex_unlock(&n->dev.mutex);
 	return err;
@@ -275,7 +282,9 @@ static long vhost_test_ioctl(struct file *f, unsigned int ioctl,
 		return vhost_test_reset_owner(n);
 	default:
 		mutex_lock(&n->dev.mutex);
-		r = vhost_dev_ioctl(&n->dev, ioctl, arg);
+		r = vhost_dev_ioctl(&n->dev, ioctl, argp);
+                if (r == -ENOIOCTLCMD)
+                        r = vhost_vring_ioctl(&n->dev, ioctl, argp);
 		vhost_test_flush(n);
 		mutex_unlock(&n->dev.mutex);
 		return r;
diff --git a/drivers/vhost/vhost.c b/drivers/vhost/vhost.c
index 9759249e6d90..beee7f5787e6 100644
--- a/drivers/vhost/vhost.c
+++ b/drivers/vhost/vhost.c
@@ -13,7 +13,7 @@
 
 #include <linux/eventfd.h>
 #include <linux/vhost.h>
-#include <linux/virtio_net.h>
+#include <linux/socket.h> /* memcpy_fromiovec */
 #include <linux/mm.h>
 #include <linux/mmu_context.h>
 #include <linux/miscdevice.h>
@@ -33,8 +33,6 @@ enum {
 	VHOST_MEMORY_F_LOG = 0x1,
 };
 
-static unsigned vhost_zcopy_mask __read_mostly;
-
 #define vhost_used_event(vq) ((u16 __user *)&vq->avail->ring[vq->num])
 #define vhost_avail_event(vq) ((u16 __user *)&vq->used->ring[vq->num])
 
@@ -89,6 +87,9 @@ int vhost_poll_start(struct vhost_poll *poll, struct file *file)
 	unsigned long mask;
 	int ret = 0;
 
+	if (poll->wqh)
+		return 0;
+
 	mask = file->f_op->poll(file, &poll->table);
 	if (mask)
 		vhost_poll_wakeup(&poll->wait, 0, 0, (void *)mask);
@@ -178,8 +179,6 @@ static void vhost_vq_reset(struct vhost_dev *dev,
 	vq->used_flags = 0;
 	vq->log_used = false;
 	vq->log_addr = -1ull;
-	vq->vhost_hlen = 0;
-	vq->sock_hlen = 0;
 	vq->private_data = NULL;
 	vq->log_base = NULL;
 	vq->error_ctx = NULL;
@@ -188,9 +187,6 @@ static void vhost_vq_reset(struct vhost_dev *dev,
 	vq->call_ctx = NULL;
 	vq->call = NULL;
 	vq->log_ctx = NULL;
-	vq->upend_idx = 0;
-	vq->done_idx = 0;
-	vq->ubufs = NULL;
 }
 
 static int vhost_worker(void *data)
@@ -250,43 +246,29 @@ static void vhost_vq_free_iovecs(struct vhost_virtqueue *vq)
 	vq->log = NULL;
 	kfree(vq->heads);
 	vq->heads = NULL;
-	kfree(vq->ubuf_info);
-	vq->ubuf_info = NULL;
-}
-
-void vhost_enable_zcopy(int vq)
-{
-	vhost_zcopy_mask |= 0x1 << vq;
 }
 
 /* Helper to allocate iovec buffers for all vqs. */
 static long vhost_dev_alloc_iovecs(struct vhost_dev *dev)
 {
 	int i;
-	bool zcopy;
 
 	for (i = 0; i < dev->nvqs; ++i) {
-		dev->vqs[i].indirect = kmalloc(sizeof *dev->vqs[i].indirect *
+		dev->vqs[i]->indirect = kmalloc(sizeof *dev->vqs[i]->indirect *
 					       UIO_MAXIOV, GFP_KERNEL);
-		dev->vqs[i].log = kmalloc(sizeof *dev->vqs[i].log * UIO_MAXIOV,
+		dev->vqs[i]->log = kmalloc(sizeof *dev->vqs[i]->log * UIO_MAXIOV,
 					  GFP_KERNEL);
-		dev->vqs[i].heads = kmalloc(sizeof *dev->vqs[i].heads *
+		dev->vqs[i]->heads = kmalloc(sizeof *dev->vqs[i]->heads *
 					    UIO_MAXIOV, GFP_KERNEL);
-		zcopy = vhost_zcopy_mask & (0x1 << i);
-		if (zcopy)
-			dev->vqs[i].ubuf_info =
-				kmalloc(sizeof *dev->vqs[i].ubuf_info *
-					UIO_MAXIOV, GFP_KERNEL);
-		if (!dev->vqs[i].indirect || !dev->vqs[i].log ||
-			!dev->vqs[i].heads ||
-			(zcopy && !dev->vqs[i].ubuf_info))
+		if (!dev->vqs[i]->indirect || !dev->vqs[i]->log ||
+			!dev->vqs[i]->heads)
 			goto err_nomem;
 	}
 	return 0;
 
 err_nomem:
 	for (; i >= 0; --i)
-		vhost_vq_free_iovecs(&dev->vqs[i]);
+		vhost_vq_free_iovecs(dev->vqs[i]);
 	return -ENOMEM;
 }
 
@@ -295,11 +277,11 @@ static void vhost_dev_free_iovecs(struct vhost_dev *dev)
 	int i;
 
 	for (i = 0; i < dev->nvqs; ++i)
-		vhost_vq_free_iovecs(&dev->vqs[i]);
+		vhost_vq_free_iovecs(dev->vqs[i]);
 }
 
 long vhost_dev_init(struct vhost_dev *dev,
-		    struct vhost_virtqueue *vqs, int nvqs)
+		    struct vhost_virtqueue **vqs, int nvqs)
 {
 	int i;
 
@@ -315,16 +297,15 @@ long vhost_dev_init(struct vhost_dev *dev,
 	dev->worker = NULL;
 
 	for (i = 0; i < dev->nvqs; ++i) {
-		dev->vqs[i].log = NULL;
-		dev->vqs[i].indirect = NULL;
-		dev->vqs[i].heads = NULL;
-		dev->vqs[i].ubuf_info = NULL;
-		dev->vqs[i].dev = dev;
-		mutex_init(&dev->vqs[i].mutex);
-		vhost_vq_reset(dev, dev->vqs + i);
-		if (dev->vqs[i].handle_kick)
-			vhost_poll_init(&dev->vqs[i].poll,
-					dev->vqs[i].handle_kick, POLLIN, dev);
+		dev->vqs[i]->log = NULL;
+		dev->vqs[i]->indirect = NULL;
+		dev->vqs[i]->heads = NULL;
+		dev->vqs[i]->dev = dev;
+		mutex_init(&dev->vqs[i]->mutex);
+		vhost_vq_reset(dev, dev->vqs[i]);
+		if (dev->vqs[i]->handle_kick)
+			vhost_poll_init(&dev->vqs[i]->poll,
+					dev->vqs[i]->handle_kick, POLLIN, dev);
 	}
 
 	return 0;
@@ -363,7 +344,7 @@ static int vhost_attach_cgroups(struct vhost_dev *dev)
 }
 
 /* Caller should have device mutex */
-static long vhost_dev_set_owner(struct vhost_dev *dev)
+long vhost_dev_set_owner(struct vhost_dev *dev)
 {
 	struct task_struct *worker;
 	int err;
@@ -405,21 +386,19 @@ err_mm:
 	return err;
 }
 
-/* Caller should have device mutex */
-long vhost_dev_reset_owner(struct vhost_dev *dev)
+struct vhost_memory *vhost_dev_reset_owner_prepare(void)
 {
-	struct vhost_memory *memory;
-
-	/* Restore memory to default empty mapping. */
-	memory = kmalloc(offsetof(struct vhost_memory, regions), GFP_KERNEL);
-	if (!memory)
-		return -ENOMEM;
+	return kmalloc(offsetof(struct vhost_memory, regions), GFP_KERNEL);
+}
 
+/* Caller should have device mutex */
+void vhost_dev_reset_owner(struct vhost_dev *dev, struct vhost_memory *memory)
+{
 	vhost_dev_cleanup(dev, true);
 
+	/* Restore memory to default empty mapping. */
 	memory->nregions = 0;
 	RCU_INIT_POINTER(dev->memory, memory);
-	return 0;
 }
 
 void vhost_dev_stop(struct vhost_dev *dev)
@@ -427,9 +406,9 @@ void vhost_dev_stop(struct vhost_dev *dev)
 	int i;
 
 	for (i = 0; i < dev->nvqs; ++i) {
-		if (dev->vqs[i].kick && dev->vqs[i].handle_kick) {
-			vhost_poll_stop(&dev->vqs[i].poll);
-			vhost_poll_flush(&dev->vqs[i].poll);
+		if (dev->vqs[i]->kick && dev->vqs[i]->handle_kick) {
+			vhost_poll_stop(&dev->vqs[i]->poll);
+			vhost_poll_flush(&dev->vqs[i]->poll);
 		}
 	}
 }
@@ -440,17 +419,17 @@ void vhost_dev_cleanup(struct vhost_dev *dev, bool locked)
 	int i;
 
 	for (i = 0; i < dev->nvqs; ++i) {
-		if (dev->vqs[i].error_ctx)
-			eventfd_ctx_put(dev->vqs[i].error_ctx);
-		if (dev->vqs[i].error)
-			fput(dev->vqs[i].error);
-		if (dev->vqs[i].kick)
-			fput(dev->vqs[i].kick);
-		if (dev->vqs[i].call_ctx)
-			eventfd_ctx_put(dev->vqs[i].call_ctx);
-		if (dev->vqs[i].call)
-			fput(dev->vqs[i].call);
-		vhost_vq_reset(dev, dev->vqs + i);
+		if (dev->vqs[i]->error_ctx)
+			eventfd_ctx_put(dev->vqs[i]->error_ctx);
+		if (dev->vqs[i]->error)
+			fput(dev->vqs[i]->error);
+		if (dev->vqs[i]->kick)
+			fput(dev->vqs[i]->kick);
+		if (dev->vqs[i]->call_ctx)
+			eventfd_ctx_put(dev->vqs[i]->call_ctx);
+		if (dev->vqs[i]->call)
+			fput(dev->vqs[i]->call);
+		vhost_vq_reset(dev, dev->vqs[i]);
 	}
 	vhost_dev_free_iovecs(dev);
 	if (dev->log_ctx)
@@ -521,14 +500,14 @@ static int memory_access_ok(struct vhost_dev *d, struct vhost_memory *mem,
 
 	for (i = 0; i < d->nvqs; ++i) {
 		int ok;
-		mutex_lock(&d->vqs[i].mutex);
+		mutex_lock(&d->vqs[i]->mutex);
 		/* If ring is inactive, will check when it's enabled. */
-		if (d->vqs[i].private_data)
-			ok = vq_memory_access_ok(d->vqs[i].log_base, mem,
+		if (d->vqs[i]->private_data)
+			ok = vq_memory_access_ok(d->vqs[i]->log_base, mem,
 						 log_all);
 		else
 			ok = 1;
-		mutex_unlock(&d->vqs[i].mutex);
+		mutex_unlock(&d->vqs[i]->mutex);
 		if (!ok)
 			return 0;
 	}
@@ -638,7 +617,7 @@ long vhost_vring_ioctl(struct vhost_dev *d, int ioctl, void __user *argp)
 	if (idx >= d->nvqs)
 		return -ENOBUFS;
 
-	vq = d->vqs + idx;
+	vq = d->vqs[idx];
 
 	mutex_lock(&vq->mutex);
 
@@ -849,7 +828,7 @@ long vhost_dev_ioctl(struct vhost_dev *d, unsigned int ioctl, void __user *argp)
 		for (i = 0; i < d->nvqs; ++i) {
 			struct vhost_virtqueue *vq;
 			void __user *base = (void __user *)(unsigned long)p;
-			vq = d->vqs + i;
+			vq = d->vqs[i];
 			mutex_lock(&vq->mutex);
 			/* If ring is inactive, will check when it's enabled. */
 			if (vq->private_data && !vq_log_access_ok(d, vq, base))
@@ -876,9 +855,9 @@ long vhost_dev_ioctl(struct vhost_dev *d, unsigned int ioctl, void __user *argp)
 		} else
 			filep = eventfp;
 		for (i = 0; i < d->nvqs; ++i) {
-			mutex_lock(&d->vqs[i].mutex);
-			d->vqs[i].log_ctx = d->log_ctx;
-			mutex_unlock(&d->vqs[i].mutex);
+			mutex_lock(&d->vqs[i]->mutex);
+			d->vqs[i]->log_ctx = d->log_ctx;
+			mutex_unlock(&d->vqs[i]->mutex);
 		}
 		if (ctx)
 			eventfd_ctx_put(ctx);
@@ -1548,38 +1527,3 @@ void vhost_disable_notify(struct vhost_dev *dev, struct vhost_virtqueue *vq)
 			       &vq->used->flags, r);
 	}
 }
-
-static void vhost_zerocopy_done_signal(struct kref *kref)
-{
-	struct vhost_ubuf_ref *ubufs = container_of(kref, struct vhost_ubuf_ref,
-						    kref);
-	wake_up(&ubufs->wait);
-}
-
-struct vhost_ubuf_ref *vhost_ubuf_alloc(struct vhost_virtqueue *vq,
-					bool zcopy)
-{
-	struct vhost_ubuf_ref *ubufs;
-	/* No zero copy backend? Nothing to count. */
-	if (!zcopy)
-		return NULL;
-	ubufs = kmalloc(sizeof *ubufs, GFP_KERNEL);
-	if (!ubufs)
-		return ERR_PTR(-ENOMEM);
-	kref_init(&ubufs->kref);
-	init_waitqueue_head(&ubufs->wait);
-	ubufs->vq = vq;
-	return ubufs;
-}
-
-void vhost_ubuf_put(struct vhost_ubuf_ref *ubufs)
-{
-	kref_put(&ubufs->kref, vhost_zerocopy_done_signal);
-}
-
-void vhost_ubuf_put_and_wait(struct vhost_ubuf_ref *ubufs)
-{
-	kref_put(&ubufs->kref, vhost_zerocopy_done_signal);
-	wait_event(ubufs->wait, !atomic_read(&ubufs->kref.refcount));
-	kfree(ubufs);
-}
diff --git a/drivers/vhost/vhost.h b/drivers/vhost/vhost.h
index 17261e277c02..a7ad63592987 100644
--- a/drivers/vhost/vhost.h
+++ b/drivers/vhost/vhost.h
@@ -54,18 +54,6 @@ struct vhost_log {
 
 struct vhost_virtqueue;
 
-struct vhost_ubuf_ref {
-	struct kref kref;
-	wait_queue_head_t wait;
-	struct vhost_virtqueue *vq;
-};
-
-struct vhost_ubuf_ref *vhost_ubuf_alloc(struct vhost_virtqueue *, bool zcopy);
-void vhost_ubuf_put(struct vhost_ubuf_ref *);
-void vhost_ubuf_put_and_wait(struct vhost_ubuf_ref *);
-
-struct ubuf_info;
-
 /* The virtqueue structure describes a queue attached to a device. */
 struct vhost_virtqueue {
 	struct vhost_dev *dev;
@@ -111,13 +99,7 @@ struct vhost_virtqueue {
 	u64 log_addr;
 
 	struct iovec iov[UIO_MAXIOV];
-	/* hdr is used to store the virtio header.
-	 * Since each iovec has >= 1 byte length, we never need more than
-	 * header length entries to store the header. */
-	struct iovec hdr[sizeof(struct virtio_net_hdr_mrg_rxbuf)];
 	struct iovec *indirect;
-	size_t vhost_hlen;
-	size_t sock_hlen;
 	struct vring_used_elem *heads;
 	/* We use a kind of RCU to access private pointer.
 	 * All readers access it from worker, which makes it possible to
@@ -130,16 +112,6 @@ struct vhost_virtqueue {
 	/* Log write descriptors */
 	void __user *log_base;
 	struct vhost_log *log;
-	/* vhost zerocopy support fields below: */
-	/* last used idx for outstanding DMA zerocopy buffers */
-	int upend_idx;
-	/* first used idx for DMA done zerocopy buffers */
-	int done_idx;
-	/* an array of userspace buffers info */
-	struct ubuf_info *ubuf_info;
-	/* Reference counting for outstanding ubufs.
-	 * Protected by vq mutex. Writers must also take device mutex. */
-	struct vhost_ubuf_ref *ubufs;
 };
 
 struct vhost_dev {
@@ -150,7 +122,7 @@ struct vhost_dev {
 	struct mm_struct *mm;
 	struct mutex mutex;
 	unsigned acked_features;
-	struct vhost_virtqueue *vqs;
+	struct vhost_virtqueue **vqs;
 	int nvqs;
 	struct file *log_file;
 	struct eventfd_ctx *log_ctx;
@@ -159,9 +131,11 @@ struct vhost_dev {
 	struct task_struct *worker;
 };
 
-long vhost_dev_init(struct vhost_dev *, struct vhost_virtqueue *vqs, int nvqs);
+long vhost_dev_init(struct vhost_dev *, struct vhost_virtqueue **vqs, int nvqs);
+long vhost_dev_set_owner(struct vhost_dev *dev);
 long vhost_dev_check_owner(struct vhost_dev *);
-long vhost_dev_reset_owner(struct vhost_dev *);
+struct vhost_memory *vhost_dev_reset_owner_prepare(void);
+void vhost_dev_reset_owner(struct vhost_dev *, struct vhost_memory *);
 void vhost_dev_cleanup(struct vhost_dev *, bool locked);
 void vhost_dev_stop(struct vhost_dev *);
 long vhost_dev_ioctl(struct vhost_dev *, unsigned int ioctl, void __user *argp);
@@ -201,9 +175,6 @@ enum {
 			 (1ULL << VIRTIO_RING_F_INDIRECT_DESC) |
 			 (1ULL << VIRTIO_RING_F_EVENT_IDX) |
 			 (1ULL << VHOST_F_LOG_ALL),
-	VHOST_NET_FEATURES = VHOST_FEATURES |
-			 (1ULL << VHOST_NET_F_VIRTIO_NET_HDR) |
-			 (1ULL << VIRTIO_NET_F_MRG_RXBUF),
 };
 
 static inline int vhost_has_feature(struct vhost_dev *dev, int bit)
@@ -215,7 +186,4 @@ static inline int vhost_has_feature(struct vhost_dev *dev, int bit)
 	acked_features = rcu_dereference_index_check(dev->acked_features, 1);
 	return acked_features & (1 << bit);
 }
-
-void vhost_enable_zcopy(int vq);
-
 #endif
diff --git a/drivers/vhost/vringh.c b/drivers/vhost/vringh.c
new file mode 100644
index 000000000000..bff0775e258c
--- /dev/null
+++ b/drivers/vhost/vringh.c
@@ -0,0 +1,1007 @@
+/*
+ * Helpers for the host side of a virtio ring.
+ *
+ * Since these may be in userspace, we use (inline) accessors.
+ */
+#include <linux/vringh.h>
+#include <linux/virtio_ring.h>
+#include <linux/kernel.h>
+#include <linux/ratelimit.h>
+#include <linux/uaccess.h>
+#include <linux/slab.h>
+#include <linux/export.h>
+
+static __printf(1,2) __cold void vringh_bad(const char *fmt, ...)
+{
+	static DEFINE_RATELIMIT_STATE(vringh_rs,
+				      DEFAULT_RATELIMIT_INTERVAL,
+				      DEFAULT_RATELIMIT_BURST);
+	if (__ratelimit(&vringh_rs)) {
+		va_list ap;
+		va_start(ap, fmt);
+		printk(KERN_NOTICE "vringh:");
+		vprintk(fmt, ap);
+		va_end(ap);
+	}
+}
+
+/* Returns vring->num if empty, -ve on error. */
+static inline int __vringh_get_head(const struct vringh *vrh,
+				    int (*getu16)(u16 *val, const u16 *p),
+				    u16 *last_avail_idx)
+{
+	u16 avail_idx, i, head;
+	int err;
+
+	err = getu16(&avail_idx, &vrh->vring.avail->idx);
+	if (err) {
+		vringh_bad("Failed to access avail idx at %p",
+			   &vrh->vring.avail->idx);
+		return err;
+	}
+
+	if (*last_avail_idx == avail_idx)
+		return vrh->vring.num;
+
+	/* Only get avail ring entries after they have been exposed by guest. */
+	virtio_rmb(vrh->weak_barriers);
+
+	i = *last_avail_idx & (vrh->vring.num - 1);
+
+	err = getu16(&head, &vrh->vring.avail->ring[i]);
+	if (err) {
+		vringh_bad("Failed to read head: idx %d address %p",
+			   *last_avail_idx, &vrh->vring.avail->ring[i]);
+		return err;
+	}
+
+	if (head >= vrh->vring.num) {
+		vringh_bad("Guest says index %u > %u is available",
+			   head, vrh->vring.num);
+		return -EINVAL;
+	}
+
+	(*last_avail_idx)++;
+	return head;
+}
+
+/* Copy some bytes to/from the iovec.  Returns num copied. */
+static inline ssize_t vringh_iov_xfer(struct vringh_kiov *iov,
+				      void *ptr, size_t len,
+				      int (*xfer)(void *addr, void *ptr,
+						  size_t len))
+{
+	int err, done = 0;
+
+	while (len && iov->i < iov->used) {
+		size_t partlen;
+
+		partlen = min(iov->iov[iov->i].iov_len, len);
+		err = xfer(iov->iov[iov->i].iov_base, ptr, partlen);
+		if (err)
+			return err;
+		done += partlen;
+		len -= partlen;
+		ptr += partlen;
+		iov->consumed += partlen;
+		iov->iov[iov->i].iov_len -= partlen;
+		iov->iov[iov->i].iov_base += partlen;
+
+		if (!iov->iov[iov->i].iov_len) {
+			/* Fix up old iov element then increment. */
+			iov->iov[iov->i].iov_len = iov->consumed;
+			iov->iov[iov->i].iov_base -= iov->consumed;
+			
+			iov->consumed = 0;
+			iov->i++;
+		}
+	}
+	return done;
+}
+
+/* May reduce *len if range is shorter. */
+static inline bool range_check(struct vringh *vrh, u64 addr, size_t *len,
+			       struct vringh_range *range,
+			       bool (*getrange)(struct vringh *,
+						u64, struct vringh_range *))
+{
+	if (addr < range->start || addr > range->end_incl) {
+		if (!getrange(vrh, addr, range))
+			return false;
+	}
+	BUG_ON(addr < range->start || addr > range->end_incl);
+
+	/* To end of memory? */
+	if (unlikely(addr + *len == 0)) {
+		if (range->end_incl == -1ULL)
+			return true;
+		goto truncate;
+	}
+
+	/* Otherwise, don't wrap. */
+	if (addr + *len < addr) {
+		vringh_bad("Wrapping descriptor %zu@0x%llx",
+			   *len, (unsigned long long)addr);
+		return false;
+	}
+
+	if (unlikely(addr + *len - 1 > range->end_incl))
+		goto truncate;
+	return true;
+
+truncate:
+	*len = range->end_incl + 1 - addr;
+	return true;
+}
+
+static inline bool no_range_check(struct vringh *vrh, u64 addr, size_t *len,
+				  struct vringh_range *range,
+				  bool (*getrange)(struct vringh *,
+						   u64, struct vringh_range *))
+{
+	return true;
+}
+
+/* No reason for this code to be inline. */
+static int move_to_indirect(int *up_next, u16 *i, void *addr,
+			    const struct vring_desc *desc,
+			    struct vring_desc **descs, int *desc_max)
+{
+	/* Indirect tables can't have indirect. */
+	if (*up_next != -1) {
+		vringh_bad("Multilevel indirect %u->%u", *up_next, *i);
+		return -EINVAL;
+	}
+
+	if (unlikely(desc->len % sizeof(struct vring_desc))) {
+		vringh_bad("Strange indirect len %u", desc->len);
+		return -EINVAL;
+	}
+
+	/* We will check this when we follow it! */
+	if (desc->flags & VRING_DESC_F_NEXT)
+		*up_next = desc->next;
+	else
+		*up_next = -2;
+	*descs = addr;
+	*desc_max = desc->len / sizeof(struct vring_desc);
+
+	/* Now, start at the first indirect. */
+	*i = 0;
+	return 0;
+}
+
+static int resize_iovec(struct vringh_kiov *iov, gfp_t gfp)
+{
+	struct kvec *new;
+	unsigned int flag, new_num = (iov->max_num & ~VRINGH_IOV_ALLOCATED) * 2;
+
+	if (new_num < 8)
+		new_num = 8;
+
+	flag = (iov->max_num & VRINGH_IOV_ALLOCATED);
+	if (flag)
+		new = krealloc(iov->iov, new_num * sizeof(struct iovec), gfp);
+	else {
+		new = kmalloc(new_num * sizeof(struct iovec), gfp);
+		if (new) {
+			memcpy(new, iov->iov,
+			       iov->max_num * sizeof(struct iovec));
+			flag = VRINGH_IOV_ALLOCATED;
+		}
+	}
+	if (!new)
+		return -ENOMEM;
+	iov->iov = new;
+	iov->max_num = (new_num | flag);
+	return 0;
+}
+
+static u16 __cold return_from_indirect(const struct vringh *vrh, int *up_next,
+				       struct vring_desc **descs, int *desc_max)
+{
+	u16 i = *up_next;
+
+	*up_next = -1;
+	*descs = vrh->vring.desc;
+	*desc_max = vrh->vring.num;
+	return i;
+}
+
+static int slow_copy(struct vringh *vrh, void *dst, const void *src,
+		     bool (*rcheck)(struct vringh *vrh, u64 addr, size_t *len,
+				    struct vringh_range *range,
+				    bool (*getrange)(struct vringh *vrh,
+						     u64,
+						     struct vringh_range *)),
+		     bool (*getrange)(struct vringh *vrh,
+				      u64 addr,
+				      struct vringh_range *r),
+		     struct vringh_range *range,
+		     int (*copy)(void *dst, const void *src, size_t len))
+{
+	size_t part, len = sizeof(struct vring_desc);
+
+	do {
+		u64 addr;
+		int err;
+
+		part = len;
+		addr = (u64)(unsigned long)src - range->offset;
+
+		if (!rcheck(vrh, addr, &part, range, getrange))
+			return -EINVAL;
+
+		err = copy(dst, src, part);
+		if (err)
+			return err;
+
+		dst += part;
+		src += part;
+		len -= part;
+	} while (len);
+	return 0;
+}
+
+static inline int
+__vringh_iov(struct vringh *vrh, u16 i,
+	     struct vringh_kiov *riov,
+	     struct vringh_kiov *wiov,
+	     bool (*rcheck)(struct vringh *vrh, u64 addr, size_t *len,
+			    struct vringh_range *range,
+			    bool (*getrange)(struct vringh *, u64,
+					     struct vringh_range *)),
+	     bool (*getrange)(struct vringh *, u64, struct vringh_range *),
+	     gfp_t gfp,
+	     int (*copy)(void *dst, const void *src, size_t len))
+{
+	int err, count = 0, up_next, desc_max;
+	struct vring_desc desc, *descs;
+	struct vringh_range range = { -1ULL, 0 }, slowrange;
+	bool slow = false;
+
+	/* We start traversing vring's descriptor table. */
+	descs = vrh->vring.desc;
+	desc_max = vrh->vring.num;
+	up_next = -1;
+
+	if (riov)
+		riov->i = riov->used = 0;
+	else if (wiov)
+		wiov->i = wiov->used = 0;
+	else
+		/* You must want something! */
+		BUG();
+
+	for (;;) {
+		void *addr;
+		struct vringh_kiov *iov;
+		size_t len;
+
+		if (unlikely(slow))
+			err = slow_copy(vrh, &desc, &descs[i], rcheck, getrange,
+					&slowrange, copy);
+		else
+			err = copy(&desc, &descs[i], sizeof(desc));
+		if (unlikely(err))
+			goto fail;
+
+		if (unlikely(desc.flags & VRING_DESC_F_INDIRECT)) {
+			/* Make sure it's OK, and get offset. */
+			len = desc.len;
+			if (!rcheck(vrh, desc.addr, &len, &range, getrange)) {
+				err = -EINVAL;
+				goto fail;
+			}
+
+			if (unlikely(len != desc.len)) {
+				slow = true;
+				/* We need to save this range to use offset */
+				slowrange = range;
+			}
+
+			addr = (void *)(long)(desc.addr + range.offset);
+			err = move_to_indirect(&up_next, &i, addr, &desc,
+					       &descs, &desc_max);
+			if (err)
+				goto fail;
+			continue;
+		}
+
+		if (count++ == vrh->vring.num) {
+			vringh_bad("Descriptor loop in %p", descs);
+			err = -ELOOP;
+			goto fail;
+		}
+
+		if (desc.flags & VRING_DESC_F_WRITE)
+			iov = wiov;
+		else {
+			iov = riov;
+			if (unlikely(wiov && wiov->i)) {
+				vringh_bad("Readable desc %p after writable",
+					   &descs[i]);
+				err = -EINVAL;
+				goto fail;
+			}
+		}
+
+		if (!iov) {
+			vringh_bad("Unexpected %s desc",
+				   !wiov ? "writable" : "readable");
+			err = -EPROTO;
+			goto fail;
+		}
+
+	again:
+		/* Make sure it's OK, and get offset. */
+		len = desc.len;
+		if (!rcheck(vrh, desc.addr, &len, &range, getrange)) {
+			err = -EINVAL;
+			goto fail;
+		}
+		addr = (void *)(unsigned long)(desc.addr + range.offset);
+
+		if (unlikely(iov->used == (iov->max_num & ~VRINGH_IOV_ALLOCATED))) {
+			err = resize_iovec(iov, gfp);
+			if (err)
+				goto fail;
+		}
+
+		iov->iov[iov->used].iov_base = addr;
+		iov->iov[iov->used].iov_len = len;
+		iov->used++;
+
+		if (unlikely(len != desc.len)) {
+			desc.len -= len;
+			desc.addr += len;
+			goto again;
+		}
+
+		if (desc.flags & VRING_DESC_F_NEXT) {
+			i = desc.next;
+		} else {
+			/* Just in case we need to finish traversing above. */
+			if (unlikely(up_next > 0)) {
+				i = return_from_indirect(vrh, &up_next,
+							 &descs, &desc_max);
+				slow = false;
+			} else
+				break;
+		}
+
+		if (i >= desc_max) {
+			vringh_bad("Chained index %u > %u", i, desc_max);
+			err = -EINVAL;
+			goto fail;
+		}
+	}
+
+	return 0;
+
+fail:
+	return err;
+}
+
+static inline int __vringh_complete(struct vringh *vrh,
+				    const struct vring_used_elem *used,
+				    unsigned int num_used,
+				    int (*putu16)(u16 *p, u16 val),
+				    int (*putused)(struct vring_used_elem *dst,
+						   const struct vring_used_elem
+						   *src, unsigned num))
+{
+	struct vring_used *used_ring;
+	int err;
+	u16 used_idx, off;
+
+	used_ring = vrh->vring.used;
+	used_idx = vrh->last_used_idx + vrh->completed;
+
+	off = used_idx % vrh->vring.num;
+
+	/* Compiler knows num_used == 1 sometimes, hence extra check */
+	if (num_used > 1 && unlikely(off + num_used >= vrh->vring.num)) {
+		u16 part = vrh->vring.num - off;
+		err = putused(&used_ring->ring[off], used, part);
+		if (!err)
+			err = putused(&used_ring->ring[0], used + part,
+				      num_used - part);
+	} else
+		err = putused(&used_ring->ring[off], used, num_used);
+
+	if (err) {
+		vringh_bad("Failed to write %u used entries %u at %p",
+			   num_used, off, &used_ring->ring[off]);
+		return err;
+	}
+
+	/* Make sure buffer is written before we update index. */
+	virtio_wmb(vrh->weak_barriers);
+
+	err = putu16(&vrh->vring.used->idx, used_idx + num_used);
+	if (err) {
+		vringh_bad("Failed to update used index at %p",
+			   &vrh->vring.used->idx);
+		return err;
+	}
+
+	vrh->completed += num_used;
+	return 0;
+}
+
+
+static inline int __vringh_need_notify(struct vringh *vrh,
+				       int (*getu16)(u16 *val, const u16 *p))
+{
+	bool notify;
+	u16 used_event;
+	int err;
+
+	/* Flush out used index update. This is paired with the
+	 * barrier that the Guest executes when enabling
+	 * interrupts. */
+	virtio_mb(vrh->weak_barriers);
+
+	/* Old-style, without event indices. */
+	if (!vrh->event_indices) {
+		u16 flags;
+		err = getu16(&flags, &vrh->vring.avail->flags);
+		if (err) {
+			vringh_bad("Failed to get flags at %p",
+				   &vrh->vring.avail->flags);
+			return err;
+		}
+		return (!(flags & VRING_AVAIL_F_NO_INTERRUPT));
+	}
+
+	/* Modern: we know when other side wants to know. */
+	err = getu16(&used_event, &vring_used_event(&vrh->vring));
+	if (err) {
+		vringh_bad("Failed to get used event idx at %p",
+			   &vring_used_event(&vrh->vring));
+		return err;
+	}
+
+	/* Just in case we added so many that we wrap. */
+	if (unlikely(vrh->completed > 0xffff))
+		notify = true;
+	else
+		notify = vring_need_event(used_event,
+					  vrh->last_used_idx + vrh->completed,
+					  vrh->last_used_idx);
+
+	vrh->last_used_idx += vrh->completed;
+	vrh->completed = 0;
+	return notify;
+}
+
+static inline bool __vringh_notify_enable(struct vringh *vrh,
+					  int (*getu16)(u16 *val, const u16 *p),
+					  int (*putu16)(u16 *p, u16 val))
+{
+	u16 avail;
+
+	if (!vrh->event_indices) {
+		/* Old-school; update flags. */
+		if (putu16(&vrh->vring.used->flags, 0) != 0) {
+			vringh_bad("Clearing used flags %p",
+				   &vrh->vring.used->flags);
+			return true;
+		}
+	} else {
+		if (putu16(&vring_avail_event(&vrh->vring),
+			   vrh->last_avail_idx) != 0) {
+			vringh_bad("Updating avail event index %p",
+				   &vring_avail_event(&vrh->vring));
+			return true;
+		}
+	}
+
+	/* They could have slipped one in as we were doing that: make
+	 * sure it's written, then check again. */
+	virtio_mb(vrh->weak_barriers);
+
+	if (getu16(&avail, &vrh->vring.avail->idx) != 0) {
+		vringh_bad("Failed to check avail idx at %p",
+			   &vrh->vring.avail->idx);
+		return true;
+	}
+
+	/* This is unlikely, so we just leave notifications enabled
+	 * (if we're using event_indices, we'll only get one
+	 * notification anyway). */
+	return avail == vrh->last_avail_idx;
+}
+
+static inline void __vringh_notify_disable(struct vringh *vrh,
+					   int (*putu16)(u16 *p, u16 val))
+{
+	if (!vrh->event_indices) {
+		/* Old-school; update flags. */
+		if (putu16(&vrh->vring.used->flags, VRING_USED_F_NO_NOTIFY)) {
+			vringh_bad("Setting used flags %p",
+				   &vrh->vring.used->flags);
+		}
+	}
+}
+
+/* Userspace access helpers: in this case, addresses are really userspace. */
+static inline int getu16_user(u16 *val, const u16 *p)
+{
+	return get_user(*val, (__force u16 __user *)p);
+}
+
+static inline int putu16_user(u16 *p, u16 val)
+{
+	return put_user(val, (__force u16 __user *)p);
+}
+
+static inline int copydesc_user(void *dst, const void *src, size_t len)
+{
+	return copy_from_user(dst, (__force void __user *)src, len) ?
+		-EFAULT : 0;
+}
+
+static inline int putused_user(struct vring_used_elem *dst,
+			       const struct vring_used_elem *src,
+			       unsigned int num)
+{
+	return copy_to_user((__force void __user *)dst, src,
+			    sizeof(*dst) * num) ? -EFAULT : 0;
+}
+
+static inline int xfer_from_user(void *src, void *dst, size_t len)
+{
+	return copy_from_user(dst, (__force void __user *)src, len) ?
+		-EFAULT : 0;
+}
+
+static inline int xfer_to_user(void *dst, void *src, size_t len)
+{
+	return copy_to_user((__force void __user *)dst, src, len) ?
+		-EFAULT : 0;
+}
+
+/**
+ * vringh_init_user - initialize a vringh for a userspace vring.
+ * @vrh: the vringh to initialize.
+ * @features: the feature bits for this ring.
+ * @num: the number of elements.
+ * @weak_barriers: true if we only need memory barriers, not I/O.
+ * @desc: the userpace descriptor pointer.
+ * @avail: the userpace avail pointer.
+ * @used: the userpace used pointer.
+ *
+ * Returns an error if num is invalid: you should check pointers
+ * yourself!
+ */
+int vringh_init_user(struct vringh *vrh, u32 features,
+		     unsigned int num, bool weak_barriers,
+		     struct vring_desc __user *desc,
+		     struct vring_avail __user *avail,
+		     struct vring_used __user *used)
+{
+	/* Sane power of 2 please! */
+	if (!num || num > 0xffff || (num & (num - 1))) {
+		vringh_bad("Bad ring size %u", num);
+		return -EINVAL;
+	}
+
+	vrh->event_indices = (features & (1 << VIRTIO_RING_F_EVENT_IDX));
+	vrh->weak_barriers = weak_barriers;
+	vrh->completed = 0;
+	vrh->last_avail_idx = 0;
+	vrh->last_used_idx = 0;
+	vrh->vring.num = num;
+	/* vring expects kernel addresses, but only used via accessors. */
+	vrh->vring.desc = (__force struct vring_desc *)desc;
+	vrh->vring.avail = (__force struct vring_avail *)avail;
+	vrh->vring.used = (__force struct vring_used *)used;
+	return 0;
+}
+EXPORT_SYMBOL(vringh_init_user);
+
+/**
+ * vringh_getdesc_user - get next available descriptor from userspace ring.
+ * @vrh: the userspace vring.
+ * @riov: where to put the readable descriptors (or NULL)
+ * @wiov: where to put the writable descriptors (or NULL)
+ * @getrange: function to call to check ranges.
+ * @head: head index we received, for passing to vringh_complete_user().
+ *
+ * Returns 0 if there was no descriptor, 1 if there was, or -errno.
+ *
+ * Note that on error return, you can tell the difference between an
+ * invalid ring and a single invalid descriptor: in the former case,
+ * *head will be vrh->vring.num.  You may be able to ignore an invalid
+ * descriptor, but there's not much you can do with an invalid ring.
+ *
+ * Note that you may need to clean up riov and wiov, even on error!
+ */
+int vringh_getdesc_user(struct vringh *vrh,
+			struct vringh_iov *riov,
+			struct vringh_iov *wiov,
+			bool (*getrange)(struct vringh *vrh,
+					 u64 addr, struct vringh_range *r),
+			u16 *head)
+{
+	int err;
+
+	*head = vrh->vring.num;
+	err = __vringh_get_head(vrh, getu16_user, &vrh->last_avail_idx);
+	if (err < 0)
+		return err;
+
+	/* Empty... */
+	if (err == vrh->vring.num)
+		return 0;
+
+	/* We need the layouts to be the identical for this to work */
+	BUILD_BUG_ON(sizeof(struct vringh_kiov) != sizeof(struct vringh_iov));
+	BUILD_BUG_ON(offsetof(struct vringh_kiov, iov) !=
+		     offsetof(struct vringh_iov, iov));
+	BUILD_BUG_ON(offsetof(struct vringh_kiov, i) !=
+		     offsetof(struct vringh_iov, i));
+	BUILD_BUG_ON(offsetof(struct vringh_kiov, used) !=
+		     offsetof(struct vringh_iov, used));
+	BUILD_BUG_ON(offsetof(struct vringh_kiov, max_num) !=
+		     offsetof(struct vringh_iov, max_num));
+	BUILD_BUG_ON(sizeof(struct iovec) != sizeof(struct kvec));
+	BUILD_BUG_ON(offsetof(struct iovec, iov_base) !=
+		     offsetof(struct kvec, iov_base));
+	BUILD_BUG_ON(offsetof(struct iovec, iov_len) !=
+		     offsetof(struct kvec, iov_len));
+	BUILD_BUG_ON(sizeof(((struct iovec *)NULL)->iov_base)
+		     != sizeof(((struct kvec *)NULL)->iov_base));
+	BUILD_BUG_ON(sizeof(((struct iovec *)NULL)->iov_len)
+		     != sizeof(((struct kvec *)NULL)->iov_len));
+
+	*head = err;
+	err = __vringh_iov(vrh, *head, (struct vringh_kiov *)riov,
+			   (struct vringh_kiov *)wiov,
+			   range_check, getrange, GFP_KERNEL, copydesc_user);
+	if (err)
+		return err;
+
+	return 1;
+}
+EXPORT_SYMBOL(vringh_getdesc_user);
+
+/**
+ * vringh_iov_pull_user - copy bytes from vring_iov.
+ * @riov: the riov as passed to vringh_getdesc_user() (updated as we consume)
+ * @dst: the place to copy.
+ * @len: the maximum length to copy.
+ *
+ * Returns the bytes copied <= len or a negative errno.
+ */
+ssize_t vringh_iov_pull_user(struct vringh_iov *riov, void *dst, size_t len)
+{
+	return vringh_iov_xfer((struct vringh_kiov *)riov,
+			       dst, len, xfer_from_user);
+}
+EXPORT_SYMBOL(vringh_iov_pull_user);
+
+/**
+ * vringh_iov_push_user - copy bytes into vring_iov.
+ * @wiov: the wiov as passed to vringh_getdesc_user() (updated as we consume)
+ * @dst: the place to copy.
+ * @len: the maximum length to copy.
+ *
+ * Returns the bytes copied <= len or a negative errno.
+ */
+ssize_t vringh_iov_push_user(struct vringh_iov *wiov,
+			     const void *src, size_t len)
+{
+	return vringh_iov_xfer((struct vringh_kiov *)wiov,
+			       (void *)src, len, xfer_to_user);
+}
+EXPORT_SYMBOL(vringh_iov_push_user);
+
+/**
+ * vringh_abandon_user - we've decided not to handle the descriptor(s).
+ * @vrh: the vring.
+ * @num: the number of descriptors to put back (ie. num
+ *	 vringh_get_user() to undo).
+ *
+ * The next vringh_get_user() will return the old descriptor(s) again.
+ */
+void vringh_abandon_user(struct vringh *vrh, unsigned int num)
+{
+	/* We only update vring_avail_event(vr) when we want to be notified,
+	 * so we haven't changed that yet. */
+	vrh->last_avail_idx -= num;
+}
+EXPORT_SYMBOL(vringh_abandon_user);
+
+/**
+ * vringh_complete_user - we've finished with descriptor, publish it.
+ * @vrh: the vring.
+ * @head: the head as filled in by vringh_getdesc_user.
+ * @len: the length of data we have written.
+ *
+ * You should check vringh_need_notify_user() after one or more calls
+ * to this function.
+ */
+int vringh_complete_user(struct vringh *vrh, u16 head, u32 len)
+{
+	struct vring_used_elem used;
+
+	used.id = head;
+	used.len = len;
+	return __vringh_complete(vrh, &used, 1, putu16_user, putused_user);
+}
+EXPORT_SYMBOL(vringh_complete_user);
+
+/**
+ * vringh_complete_multi_user - we've finished with many descriptors.
+ * @vrh: the vring.
+ * @used: the head, length pairs.
+ * @num_used: the number of used elements.
+ *
+ * You should check vringh_need_notify_user() after one or more calls
+ * to this function.
+ */
+int vringh_complete_multi_user(struct vringh *vrh,
+			       const struct vring_used_elem used[],
+			       unsigned num_used)
+{
+	return __vringh_complete(vrh, used, num_used,
+				 putu16_user, putused_user);
+}
+EXPORT_SYMBOL(vringh_complete_multi_user);
+
+/**
+ * vringh_notify_enable_user - we want to know if something changes.
+ * @vrh: the vring.
+ *
+ * This always enables notifications, but returns false if there are
+ * now more buffers available in the vring.
+ */
+bool vringh_notify_enable_user(struct vringh *vrh)
+{
+	return __vringh_notify_enable(vrh, getu16_user, putu16_user);
+}
+EXPORT_SYMBOL(vringh_notify_enable_user);
+
+/**
+ * vringh_notify_disable_user - don't tell us if something changes.
+ * @vrh: the vring.
+ *
+ * This is our normal running state: we disable and then only enable when
+ * we're going to sleep.
+ */
+void vringh_notify_disable_user(struct vringh *vrh)
+{
+	__vringh_notify_disable(vrh, putu16_user);
+}
+EXPORT_SYMBOL(vringh_notify_disable_user);
+
+/**
+ * vringh_need_notify_user - must we tell the other side about used buffers?
+ * @vrh: the vring we've called vringh_complete_user() on.
+ *
+ * Returns -errno or 0 if we don't need to tell the other side, 1 if we do.
+ */
+int vringh_need_notify_user(struct vringh *vrh)
+{
+	return __vringh_need_notify(vrh, getu16_user);
+}
+EXPORT_SYMBOL(vringh_need_notify_user);
+
+/* Kernelspace access helpers. */
+static inline int getu16_kern(u16 *val, const u16 *p)
+{
+	*val = ACCESS_ONCE(*p);
+	return 0;
+}
+
+static inline int putu16_kern(u16 *p, u16 val)
+{
+	ACCESS_ONCE(*p) = val;
+	return 0;
+}
+
+static inline int copydesc_kern(void *dst, const void *src, size_t len)
+{
+	memcpy(dst, src, len);
+	return 0;
+}
+
+static inline int putused_kern(struct vring_used_elem *dst,
+			       const struct vring_used_elem *src,
+			       unsigned int num)
+{
+	memcpy(dst, src, num * sizeof(*dst));
+	return 0;
+}
+
+static inline int xfer_kern(void *src, void *dst, size_t len)
+{
+	memcpy(dst, src, len);
+	return 0;
+}
+
+/**
+ * vringh_init_kern - initialize a vringh for a kernelspace vring.
+ * @vrh: the vringh to initialize.
+ * @features: the feature bits for this ring.
+ * @num: the number of elements.
+ * @weak_barriers: true if we only need memory barriers, not I/O.
+ * @desc: the userpace descriptor pointer.
+ * @avail: the userpace avail pointer.
+ * @used: the userpace used pointer.
+ *
+ * Returns an error if num is invalid.
+ */
+int vringh_init_kern(struct vringh *vrh, u32 features,
+		     unsigned int num, bool weak_barriers,
+		     struct vring_desc *desc,
+		     struct vring_avail *avail,
+		     struct vring_used *used)
+{
+	/* Sane power of 2 please! */
+	if (!num || num > 0xffff || (num & (num - 1))) {
+		vringh_bad("Bad ring size %u", num);
+		return -EINVAL;
+	}
+
+	vrh->event_indices = (features & (1 << VIRTIO_RING_F_EVENT_IDX));
+	vrh->weak_barriers = weak_barriers;
+	vrh->completed = 0;
+	vrh->last_avail_idx = 0;
+	vrh->last_used_idx = 0;
+	vrh->vring.num = num;
+	vrh->vring.desc = desc;
+	vrh->vring.avail = avail;
+	vrh->vring.used = used;
+	return 0;
+}
+EXPORT_SYMBOL(vringh_init_kern);
+
+/**
+ * vringh_getdesc_kern - get next available descriptor from kernelspace ring.
+ * @vrh: the kernelspace vring.
+ * @riov: where to put the readable descriptors (or NULL)
+ * @wiov: where to put the writable descriptors (or NULL)
+ * @head: head index we received, for passing to vringh_complete_kern().
+ * @gfp: flags for allocating larger riov/wiov.
+ *
+ * Returns 0 if there was no descriptor, 1 if there was, or -errno.
+ *
+ * Note that on error return, you can tell the difference between an
+ * invalid ring and a single invalid descriptor: in the former case,
+ * *head will be vrh->vring.num.  You may be able to ignore an invalid
+ * descriptor, but there's not much you can do with an invalid ring.
+ *
+ * Note that you may need to clean up riov and wiov, even on error!
+ */
+int vringh_getdesc_kern(struct vringh *vrh,
+			struct vringh_kiov *riov,
+			struct vringh_kiov *wiov,
+			u16 *head,
+			gfp_t gfp)
+{
+	int err;
+
+	err = __vringh_get_head(vrh, getu16_kern, &vrh->last_avail_idx);
+	if (err < 0)
+		return err;
+
+	/* Empty... */
+	if (err == vrh->vring.num)
+		return 0;
+
+	*head = err;
+	err = __vringh_iov(vrh, *head, riov, wiov, no_range_check, NULL,
+			   gfp, copydesc_kern);
+	if (err)
+		return err;
+
+	return 1;
+}
+EXPORT_SYMBOL(vringh_getdesc_kern);
+
+/**
+ * vringh_iov_pull_kern - copy bytes from vring_iov.
+ * @riov: the riov as passed to vringh_getdesc_kern() (updated as we consume)
+ * @dst: the place to copy.
+ * @len: the maximum length to copy.
+ *
+ * Returns the bytes copied <= len or a negative errno.
+ */
+ssize_t vringh_iov_pull_kern(struct vringh_kiov *riov, void *dst, size_t len)
+{
+	return vringh_iov_xfer(riov, dst, len, xfer_kern);
+}
+EXPORT_SYMBOL(vringh_iov_pull_kern);
+
+/**
+ * vringh_iov_push_kern - copy bytes into vring_iov.
+ * @wiov: the wiov as passed to vringh_getdesc_kern() (updated as we consume)
+ * @dst: the place to copy.
+ * @len: the maximum length to copy.
+ *
+ * Returns the bytes copied <= len or a negative errno.
+ */
+ssize_t vringh_iov_push_kern(struct vringh_kiov *wiov,
+			     const void *src, size_t len)
+{
+	return vringh_iov_xfer(wiov, (void *)src, len, xfer_kern);
+}
+EXPORT_SYMBOL(vringh_iov_push_kern);
+
+/**
+ * vringh_abandon_kern - we've decided not to handle the descriptor(s).
+ * @vrh: the vring.
+ * @num: the number of descriptors to put back (ie. num
+ *	 vringh_get_kern() to undo).
+ *
+ * The next vringh_get_kern() will return the old descriptor(s) again.
+ */
+void vringh_abandon_kern(struct vringh *vrh, unsigned int num)
+{
+	/* We only update vring_avail_event(vr) when we want to be notified,
+	 * so we haven't changed that yet. */
+	vrh->last_avail_idx -= num;
+}
+EXPORT_SYMBOL(vringh_abandon_kern);
+
+/**
+ * vringh_complete_kern - we've finished with descriptor, publish it.
+ * @vrh: the vring.
+ * @head: the head as filled in by vringh_getdesc_kern.
+ * @len: the length of data we have written.
+ *
+ * You should check vringh_need_notify_kern() after one or more calls
+ * to this function.
+ */
+int vringh_complete_kern(struct vringh *vrh, u16 head, u32 len)
+{
+	struct vring_used_elem used;
+
+	used.id = head;
+	used.len = len;
+
+	return __vringh_complete(vrh, &used, 1, putu16_kern, putused_kern);
+}
+EXPORT_SYMBOL(vringh_complete_kern);
+
+/**
+ * vringh_notify_enable_kern - we want to know if something changes.
+ * @vrh: the vring.
+ *
+ * This always enables notifications, but returns false if there are
+ * now more buffers available in the vring.
+ */
+bool vringh_notify_enable_kern(struct vringh *vrh)
+{
+	return __vringh_notify_enable(vrh, getu16_kern, putu16_kern);
+}
+EXPORT_SYMBOL(vringh_notify_enable_kern);
+
+/**
+ * vringh_notify_disable_kern - don't tell us if something changes.
+ * @vrh: the vring.
+ *
+ * This is our normal running state: we disable and then only enable when
+ * we're going to sleep.
+ */
+void vringh_notify_disable_kern(struct vringh *vrh)
+{
+	__vringh_notify_disable(vrh, putu16_kern);
+}
+EXPORT_SYMBOL(vringh_notify_disable_kern);
+
+/**
+ * vringh_need_notify_kern - must we tell the other side about used buffers?
+ * @vrh: the vring we've called vringh_complete_kern() on.
+ *
+ * Returns -errno or 0 if we don't need to tell the other side, 1 if we do.
+ */
+int vringh_need_notify_kern(struct vringh *vrh)
+{
+	return __vringh_need_notify(vrh, getu16_kern);
+}
+EXPORT_SYMBOL(vringh_need_notify_kern);
diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 76be61701c97..d71d60f94fc1 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -21,6 +21,8 @@ source "drivers/gpu/vga/Kconfig"
 
 source "drivers/gpu/drm/Kconfig"
 
+source "drivers/gpu/host1x/Kconfig"
+
 config VGASTATE
        tristate
        default n
@@ -2426,6 +2428,8 @@ config FB_MXS
 	select FB_CFB_FILLRECT
 	select FB_CFB_COPYAREA
 	select FB_CFB_IMAGEBLIT
+	select FB_MODE_HELPERS
+	select VIDEOMODE_HELPERS
 	help
 	  Framebuffer support for the MXS SoC.
 
@@ -2479,7 +2483,7 @@ config FB_SSD1307
 	tristate "Solomon SSD1307 framebuffer support"
 	depends on FB && I2C
 	depends on OF
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	select FB_SYS_FOPS
 	select FB_SYS_FILLRECT
 	select FB_SYS_COPYAREA
diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index 98348ec0b3ce..540909de6247 100644
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -34,6 +34,77 @@
 #define ATMEL_LCDC_DMA_BURST_LEN	8	/* words */
 #define ATMEL_LCDC_FIFO_SIZE		512	/* words */
 
+struct atmel_lcdfb_config {
+	bool have_alt_pixclock;
+	bool have_hozval;
+	bool have_intensity_bit;
+};
+
+static struct atmel_lcdfb_config at91sam9261_config = {
+	.have_hozval		= true,
+	.have_intensity_bit	= true,
+};
+
+static struct atmel_lcdfb_config at91sam9263_config = {
+	.have_intensity_bit	= true,
+};
+
+static struct atmel_lcdfb_config at91sam9g10_config = {
+	.have_hozval		= true,
+};
+
+static struct atmel_lcdfb_config at91sam9g45_config = {
+	.have_alt_pixclock	= true,
+};
+
+static struct atmel_lcdfb_config at91sam9g45es_config = {
+};
+
+static struct atmel_lcdfb_config at91sam9rl_config = {
+	.have_intensity_bit	= true,
+};
+
+static struct atmel_lcdfb_config at32ap_config = {
+	.have_hozval		= true,
+};
+
+static const struct platform_device_id atmel_lcdfb_devtypes[] = {
+	{
+		.name = "at91sam9261-lcdfb",
+		.driver_data = (unsigned long)&at91sam9261_config,
+	}, {
+		.name = "at91sam9263-lcdfb",
+		.driver_data = (unsigned long)&at91sam9263_config,
+	}, {
+		.name = "at91sam9g10-lcdfb",
+		.driver_data = (unsigned long)&at91sam9g10_config,
+	}, {
+		.name = "at91sam9g45-lcdfb",
+		.driver_data = (unsigned long)&at91sam9g45_config,
+	}, {
+		.name = "at91sam9g45es-lcdfb",
+		.driver_data = (unsigned long)&at91sam9g45es_config,
+	}, {
+		.name = "at91sam9rl-lcdfb",
+		.driver_data = (unsigned long)&at91sam9rl_config,
+	}, {
+		.name = "at32ap-lcdfb",
+		.driver_data = (unsigned long)&at32ap_config,
+	}, {
+		/* terminator */
+	}
+};
+
+static struct atmel_lcdfb_config *
+atmel_lcdfb_get_config(struct platform_device *pdev)
+{
+	unsigned long data;
+
+	data = platform_get_device_id(pdev)->driver_data;
+
+	return (struct atmel_lcdfb_config *)data;
+}
+
 #if defined(CONFIG_ARCH_AT91)
 #define	ATMEL_LCDFB_FBINFO_DEFAULT	(FBINFO_DEFAULT \
 					 | FBINFO_PARTIAL_PAN_OK \
@@ -193,14 +264,16 @@ static struct fb_fix_screeninfo atmel_lcdfb_fix __initdata = {
 	.accel		= FB_ACCEL_NONE,
 };
 
-static unsigned long compute_hozval(unsigned long xres, unsigned long lcdcon2)
+static unsigned long compute_hozval(struct atmel_lcdfb_info *sinfo,
+							unsigned long xres)
 {
+	unsigned long lcdcon2;
 	unsigned long value;
 
-	if (!(cpu_is_at91sam9261() || cpu_is_at91sam9g10()
-		|| cpu_is_at32ap7000()))
+	if (!sinfo->config->have_hozval)
 		return xres;
 
+	lcdcon2 = lcdc_readl(sinfo, ATMEL_LCDC_LCDCON2);
 	value = xres;
 	if ((lcdcon2 & ATMEL_LCDC_DISTYPE) != ATMEL_LCDC_DISTYPE_TFT) {
 		/* STN display */
@@ -423,7 +496,7 @@ static int atmel_lcdfb_check_var(struct fb_var_screeninfo *var,
 		break;
 	case 16:
 		/* Older SOCs use IBGR:555 rather than BGR:565. */
-		if (sinfo->have_intensity_bit)
+		if (sinfo->config->have_intensity_bit)
 			var->green.length = 5;
 		else
 			var->green.length = 6;
@@ -531,7 +604,7 @@ static int atmel_lcdfb_set_par(struct fb_info *info)
 	/* Now, the LCDC core... */
 
 	/* Set pixel clock */
-	if (cpu_is_at91sam9g45() && !cpu_is_at91sam9g45es())
+	if (sinfo->config->have_alt_pixclock)
 		pix_factor = 1;
 
 	clk_value_khz = clk_get_rate(sinfo->lcdc_clk) / 1000;
@@ -591,8 +664,7 @@ static int atmel_lcdfb_set_par(struct fb_info *info)
 	lcdc_writel(sinfo, ATMEL_LCDC_TIM2, value);
 
 	/* Horizontal value (aka line size) */
-	hozval_linesz = compute_hozval(info->var.xres,
-					lcdc_readl(sinfo, ATMEL_LCDC_LCDCON2));
+	hozval_linesz = compute_hozval(sinfo, info->var.xres);
 
 	/* Display size */
 	value = (hozval_linesz - 1) << ATMEL_LCDC_HOZVAL_OFFSET;
@@ -684,7 +756,7 @@ static int atmel_lcdfb_setcolreg(unsigned int regno, unsigned int red,
 
 	case FB_VISUAL_PSEUDOCOLOR:
 		if (regno < 256) {
-			if (sinfo->have_intensity_bit) {
+			if (sinfo->config->have_intensity_bit) {
 				/* old style I+BGR:555 */
 				val  = ((red   >> 11) & 0x001f);
 				val |= ((green >>  6) & 0x03e0);
@@ -821,15 +893,13 @@ static int __init atmel_lcdfb_init_fbinfo(struct atmel_lcdfb_info *sinfo)
 
 static void atmel_lcdfb_start_clock(struct atmel_lcdfb_info *sinfo)
 {
-	if (sinfo->bus_clk)
-		clk_enable(sinfo->bus_clk);
+	clk_enable(sinfo->bus_clk);
 	clk_enable(sinfo->lcdc_clk);
 }
 
 static void atmel_lcdfb_stop_clock(struct atmel_lcdfb_info *sinfo)
 {
-	if (sinfo->bus_clk)
-		clk_disable(sinfo->bus_clk);
+	clk_disable(sinfo->bus_clk);
 	clk_disable(sinfo->lcdc_clk);
 }
 
@@ -874,10 +944,9 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
 	}
 	sinfo->info = info;
 	sinfo->pdev = pdev;
-	if (cpu_is_at91sam9261() || cpu_is_at91sam9263() ||
-							cpu_is_at91sam9rl()) {
-		sinfo->have_intensity_bit = true;
-	}
+	sinfo->config = atmel_lcdfb_get_config(pdev);
+	if (!sinfo->config)
+		goto free_info;
 
 	strcpy(info->fix.id, sinfo->pdev->name);
 	info->flags = ATMEL_LCDFB_FBINFO_DEFAULT;
@@ -888,13 +957,10 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
 	info->fix = atmel_lcdfb_fix;
 
 	/* Enable LCDC Clocks */
-	if (cpu_is_at91sam9261() || cpu_is_at91sam9g10()
-	 || cpu_is_at32ap7000()) {
-		sinfo->bus_clk = clk_get(dev, "hck1");
-		if (IS_ERR(sinfo->bus_clk)) {
-			ret = PTR_ERR(sinfo->bus_clk);
-			goto free_info;
-		}
+	sinfo->bus_clk = clk_get(dev, "hclk");
+	if (IS_ERR(sinfo->bus_clk)) {
+		ret = PTR_ERR(sinfo->bus_clk);
+		goto free_info;
 	}
 	sinfo->lcdc_clk = clk_get(dev, "lcdc_clk");
 	if (IS_ERR(sinfo->lcdc_clk)) {
@@ -1055,8 +1121,7 @@ stop_clk:
 	atmel_lcdfb_stop_clock(sinfo);
 	clk_put(sinfo->lcdc_clk);
 put_bus_clk:
-	if (sinfo->bus_clk)
-		clk_put(sinfo->bus_clk);
+	clk_put(sinfo->bus_clk);
 free_info:
 	framebuffer_release(info);
 out:
@@ -1081,8 +1146,7 @@ static int __exit atmel_lcdfb_remove(struct platform_device *pdev)
 	unregister_framebuffer(info);
 	atmel_lcdfb_stop_clock(sinfo);
 	clk_put(sinfo->lcdc_clk);
-	if (sinfo->bus_clk)
-		clk_put(sinfo->bus_clk);
+	clk_put(sinfo->bus_clk);
 	fb_dealloc_cmap(&info->cmap);
 	free_irq(sinfo->irq_base, info);
 	iounmap(sinfo->mmio);
@@ -1151,7 +1215,7 @@ static struct platform_driver atmel_lcdfb_driver = {
 	.remove		= __exit_p(atmel_lcdfb_remove),
 	.suspend	= atmel_lcdfb_suspend,
 	.resume		= atmel_lcdfb_resume,
-
+	.id_table	= atmel_lcdfb_devtypes,
 	.driver		= {
 		.name	= "atmel_lcdfb",
 		.owner	= THIS_MODULE,
diff --git a/drivers/video/au1100fb.c b/drivers/video/au1100fb.c
index ddabaa867b0d..700cac067b46 100644
--- a/drivers/video/au1100fb.c
+++ b/drivers/video/au1100fb.c
@@ -111,30 +111,16 @@ static int au1100fb_fb_blank(int blank_mode, struct fb_info *fbi)
 	switch (blank_mode) {
 
 	case VESA_NO_BLANKING:
-			/* Turn on panel */
-			fbdev->regs->lcd_control |= LCD_CONTROL_GO;
-#ifdef CONFIG_MIPS_PB1100
-			if (fbdev->panel_idx == 1) {
-				au_writew(au_readw(PB1100_G_CONTROL)
-					  | (PB1100_G_CONTROL_BL | PB1100_G_CONTROL_VDD),
-			PB1100_G_CONTROL);
-			}
-#endif
+		/* Turn on panel */
+		fbdev->regs->lcd_control |= LCD_CONTROL_GO;
 		au_sync();
 		break;
 
 	case VESA_VSYNC_SUSPEND:
 	case VESA_HSYNC_SUSPEND:
 	case VESA_POWERDOWN:
-			/* Turn off panel */
-			fbdev->regs->lcd_control &= ~LCD_CONTROL_GO;
-#ifdef CONFIG_MIPS_PB1100
-			if (fbdev->panel_idx == 1) {
-				au_writew(au_readw(PB1100_G_CONTROL)
-				  	  & ~(PB1100_G_CONTROL_BL | PB1100_G_CONTROL_VDD),
-			PB1100_G_CONTROL);
-			}
-#endif
+		/* Turn off panel */
+		fbdev->regs->lcd_control &= ~LCD_CONTROL_GO;
 		au_sync();
 		break;
 	default:
diff --git a/drivers/video/backlight/Kconfig b/drivers/video/backlight/Kconfig
index 2e166c3fc4c3..d5ab6583f440 100644
--- a/drivers/video/backlight/Kconfig
+++ b/drivers/video/backlight/Kconfig
@@ -36,14 +36,14 @@ config LCD_CORGI
 
 config LCD_L4F00242T03
 	tristate "Epson L4F00242T03 LCD"
-	depends on SPI_MASTER && GENERIC_GPIO
+	depends on SPI_MASTER && GPIOLIB
 	help
 	  SPI driver for Epson L4F00242T03. This provides basic support
 	  for init and powering the LCD up/down through a sysfs interface.
 
 config LCD_LMS283GF05
 	tristate "Samsung LMS283GF05 LCD"
-	depends on SPI_MASTER && GENERIC_GPIO
+	depends on SPI_MASTER && GPIOLIB
 	help
 	  SPI driver for Samsung LMS283GF05. This provides basic support
 	  for powering the LCD up/down through a sysfs interface.
diff --git a/drivers/video/backlight/pwm_bl.c b/drivers/video/backlight/pwm_bl.c
index fa00304a63d8..1fea627394d7 100644
--- a/drivers/video/backlight/pwm_bl.c
+++ b/drivers/video/backlight/pwm_bl.c
@@ -274,7 +274,7 @@ static int pwm_backlight_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int pwm_backlight_suspend(struct device *dev)
 {
 	struct backlight_device *bl = dev_get_drvdata(dev);
@@ -296,19 +296,16 @@ static int pwm_backlight_resume(struct device *dev)
 	backlight_update_status(bl);
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(pwm_backlight_pm_ops, pwm_backlight_suspend,
 			 pwm_backlight_resume);
 
-#endif
-
 static struct platform_driver pwm_backlight_driver = {
 	.driver		= {
 		.name		= "pwm-backlight",
 		.owner		= THIS_MODULE,
-#ifdef CONFIG_PM
 		.pm		= &pwm_backlight_pm_ops,
-#endif
 		.of_match_table	= of_match_ptr(pwm_backlight_of_match),
 	},
 	.probe		= pwm_backlight_probe,
diff --git a/drivers/video/bfin_adv7393fb.c b/drivers/video/bfin_adv7393fb.c
index 8d411a3c9966..a54f7f7d763b 100644
--- a/drivers/video/bfin_adv7393fb.c
+++ b/drivers/video/bfin_adv7393fb.c
@@ -333,29 +333,23 @@ static int proc_output(char *buf)
 	return p - buf;
 }
 
-static int
-adv7393_read_proc(char *page, char **start, off_t off,
-		  int count, int *eof, void *data)
+static ssize_t
+adv7393_read_proc(struct file *file, char __user *buf,
+		  size_t size, loff_t *ppos)
 {
-	int len;
-
-	len = proc_output(page);
-	if (len <= off + count)
-		*eof = 1;
-	*start = page + off;
-	len -= off;
-	if (len > count)
-		len = count;
-	if (len < 0)
-		len = 0;
-	return len;
+	static const char message[] = "Usage:\n"
+		"echo 0x[REG][Value] > adv7393\n"
+		"example: echo 0x1234 >adv7393\n"
+		"writes 0x34 into Register 0x12\n";
+	return simple_read_from_buffer(buf, size, ppos, message,
+					sizeof(message));
 }
 
-static int
+static ssize_t
 adv7393_write_proc(struct file *file, const char __user * buffer,
-		   size_t count, void *data)
+		   size_t count, loff_t *ppos)
 {
-	struct adv7393fb_device *fbdev = data;
+	struct adv7393fb_device *fbdev = PDE_DATA(file_inode(file));
 	unsigned int val;
 	int ret;
 
@@ -368,6 +362,12 @@ adv7393_write_proc(struct file *file, const char __user * buffer,
 	return count;
 }
 
+static const struct file_operations fops = {
+	.read = adv7393_read_proc,
+	.write = adv7393_write_proc,
+	.llseek = default_llseek,
+};
+
 static int bfin_adv7393_fb_probe(struct i2c_client *client,
 				 const struct i2c_device_id *id)
 {
@@ -506,17 +506,12 @@ static int bfin_adv7393_fb_probe(struct i2c_client *client,
 	       fbdev->info.node, fbdev->info.fix.id);
 	dev_info(&client->dev, "fb memory address : 0x%p\n", fbdev->fb_mem);
 
-	entry = create_proc_entry("driver/adv7393", 0, NULL);
+	entry = proc_create_data("driver/adv7393", 0, NULL, &fops, fbdev);
 	if (!entry) {
 		dev_err(&client->dev, "unable to create /proc entry\n");
 		ret = -EFAULT;
 		goto free_fb;
 	}
-
-	entry->read_proc = adv7393_read_proc;
-	entry->write_proc = adv7393_write_proc;
-	entry->data = fbdev;
-
 	return 0;
 
 free_fb:
diff --git a/drivers/video/cirrusfb.c b/drivers/video/cirrusfb.c
index c3dbbe6e3acf..97db3ba8f237 100644
--- a/drivers/video/cirrusfb.c
+++ b/drivers/video/cirrusfb.c
@@ -53,12 +53,6 @@
 #ifdef CONFIG_AMIGA
 #include <asm/amigahw.h>
 #endif
-#ifdef CONFIG_PPC_PREP
-#include <asm/machdep.h>
-#define isPReP machine_is(prep)
-#else
-#define isPReP 0
-#endif
 
 #include <video/vga.h>
 #include <video/cirrus.h>
@@ -557,30 +551,18 @@ static int cirrusfb_check_var(struct fb_var_screeninfo *var,
 		break;
 
 	case 16:
-		if (isPReP) {
-			var->red.offset = 2;
-			var->green.offset = -3;
-			var->blue.offset = 8;
-		} else {
-			var->red.offset = 11;
-			var->green.offset = 5;
-			var->blue.offset = 0;
-		}
+		var->red.offset = 11;
+		var->green.offset = 5;
+		var->blue.offset = 0;
 		var->red.length = 5;
 		var->green.length = 6;
 		var->blue.length = 5;
 		break;
 
 	case 24:
-		if (isPReP) {
-			var->red.offset = 0;
-			var->green.offset = 8;
-			var->blue.offset = 16;
-		} else {
-			var->red.offset = 16;
-			var->green.offset = 8;
-			var->blue.offset = 0;
-		}
+		var->red.offset = 16;
+		var->green.offset = 8;
+		var->blue.offset = 0;
 		var->red.length = 8;
 		var->green.length = 8;
 		var->blue.length = 8;
@@ -1874,17 +1856,6 @@ static void cirrusfb_imageblit(struct fb_info *info,
 	}
 }
 
-#ifdef CONFIG_PPC_PREP
-#define PREP_VIDEO_BASE ((volatile unsigned long) 0xC0000000)
-#define PREP_IO_BASE    ((volatile unsigned char *) 0x80000000)
-static void get_prep_addrs(unsigned long *display, unsigned long *registers)
-{
-	*display = PREP_VIDEO_BASE;
-	*registers = (unsigned long) PREP_IO_BASE;
-}
-
-#endif				/* CONFIG_PPC_PREP */
-
 #ifdef CONFIG_PCI
 static int release_io_ports;
 
@@ -2139,21 +2110,12 @@ static int cirrusfb_pci_register(struct pci_dev *pdev,
 	dev_dbg(info->device, " base address 1 is 0x%Lx\n",
 		(unsigned long long)pdev->resource[1].start);
 
-	if (isPReP) {
-		pci_write_config_dword(pdev, PCI_BASE_ADDRESS_0, 0x00000000);
-#ifdef CONFIG_PPC_PREP
-		get_prep_addrs(&board_addr, &info->fix.mmio_start);
-#endif
-	/* PReP dies if we ioremap the IO registers, but it works w/out... */
-		cinfo->regbase = (char __iomem *) info->fix.mmio_start;
-	} else {
-		dev_dbg(info->device,
-			"Attempt to get PCI info for Cirrus Graphics Card\n");
-		get_pci_addrs(pdev, &board_addr, &info->fix.mmio_start);
-		/* FIXME: this forces VGA.  alternatives? */
-		cinfo->regbase = NULL;
-		cinfo->laguna_mmio = ioremap(info->fix.mmio_start, 0x1000);
-	}
+	dev_dbg(info->device,
+		"Attempt to get PCI info for Cirrus Graphics Card\n");
+	get_pci_addrs(pdev, &board_addr, &info->fix.mmio_start);
+	/* FIXME: this forces VGA.  alternatives? */
+	cinfo->regbase = NULL;
+	cinfo->laguna_mmio = ioremap(info->fix.mmio_start, 0x1000);
 
 	dev_dbg(info->device, "Board address: 0x%lx, register address: 0x%lx\n",
 		board_addr, info->fix.mmio_start);
diff --git a/drivers/video/console/fbcon.c b/drivers/video/console/fbcon.c
index 3cd675927826..a92783e480e6 100644
--- a/drivers/video/console/fbcon.c
+++ b/drivers/video/console/fbcon.c
@@ -1228,6 +1228,8 @@ static void fbcon_deinit(struct vc_data *vc)
 finished:
 
 	fbcon_free_font(p, free_font);
+	if (free_font)
+		vc->vc_font.data = NULL;
 
 	if (!con_is_bound(&fb_con))
 		fbcon_exit();
diff --git a/drivers/video/fbmem.c b/drivers/video/fbmem.c
index dcb669eb4532..098bfc64cfb9 100644
--- a/drivers/video/fbmem.c
+++ b/drivers/video/fbmem.c
@@ -1639,6 +1639,11 @@ static int do_register_framebuffer(struct fb_info *fb_info)
 	if (!fb_info->modelist.prev || !fb_info->modelist.next)
 		INIT_LIST_HEAD(&fb_info->modelist);
 
+	if (fb_info->skip_vt_switch)
+		pm_vt_switch_required(fb_info->dev, false);
+	else
+		pm_vt_switch_required(fb_info->dev, true);
+
 	fb_var_to_videomode(&mode, &fb_info->var);
 	fb_add_videomode(&mode, &fb_info->modelist);
 	registered_fb[i] = fb_info;
@@ -1673,6 +1678,8 @@ static int do_unregister_framebuffer(struct fb_info *fb_info)
 	if (ret)
 		return -EINVAL;
 
+	pm_vt_switch_unregister(fb_info->dev);
+
 	unlink_framebuffer(fb_info);
 	if (fb_info->pixmap.addr &&
 	    (fb_info->pixmap.flags & FB_PIXMAP_DEFAULT))
diff --git a/drivers/video/hdmi.c b/drivers/video/hdmi.c
index ab23c9b79143..40178338b619 100644
--- a/drivers/video/hdmi.c
+++ b/drivers/video/hdmi.c
@@ -1,9 +1,24 @@
 /*
  * Copyright (C) 2012 Avionic Design GmbH
  *
- * 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.
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sub license,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
  */
 
 #include <linux/bitops.h>
diff --git a/drivers/video/mxsfb.c b/drivers/video/mxsfb.c
index 45169cbaba6e..21223d475b39 100644
--- a/drivers/video/mxsfb.c
+++ b/drivers/video/mxsfb.c
@@ -42,13 +42,15 @@
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/of_device.h>
-#include <linux/of_gpio.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
 #include <linux/dma-mapping.h>
 #include <linux/io.h>
 #include <linux/pinctrl/consumer.h>
-#include <linux/mxsfb.h>
+#include <linux/fb.h>
+#include <linux/regulator/consumer.h>
+#include <video/of_display_timing.h>
+#include <video/videomode.h>
 
 #define REG_SET	4
 #define REG_CLR	8
@@ -107,7 +109,7 @@
 #define VDCTRL0_ENABLE_PRESENT		(1 << 28)
 #define VDCTRL0_VSYNC_ACT_HIGH		(1 << 27)
 #define VDCTRL0_HSYNC_ACT_HIGH		(1 << 26)
-#define VDCTRL0_DOTCLK_ACT_FAILING	(1 << 25)
+#define VDCTRL0_DOTCLK_ACT_FALLING	(1 << 25)
 #define VDCTRL0_ENABLE_ACT_HIGH		(1 << 24)
 #define VDCTRL0_VSYNC_PERIOD_UNIT	(1 << 21)
 #define VDCTRL0_VSYNC_PULSE_WIDTH_UNIT	(1 << 20)
@@ -142,6 +144,14 @@
 #define BLUE 2
 #define TRANSP 3
 
+#define STMLCDIF_8BIT  1 /** pixel data bus to the display is of 8 bit width */
+#define STMLCDIF_16BIT 0 /** pixel data bus to the display is of 16 bit width */
+#define STMLCDIF_18BIT 2 /** pixel data bus to the display is of 18 bit width */
+#define STMLCDIF_24BIT 3 /** pixel data bus to the display is of 24 bit width */
+
+#define MXSFB_SYNC_DATA_ENABLE_HIGH_ACT	(1 << 6)
+#define MXSFB_SYNC_DOTCLK_FALLING_ACT	(1 << 7) /* negtive edge sampling */
+
 enum mxsfb_devtype {
 	MXSFB_V3,
 	MXSFB_V4,
@@ -168,8 +178,8 @@ struct mxsfb_info {
 	unsigned ld_intf_width;
 	unsigned dotclk_delay;
 	const struct mxsfb_devdata *devdata;
-	int mapped;
 	u32 sync;
+	struct regulator *reg_lcd;
 };
 
 #define mxsfb_is_v3(host) (host->devdata->ipversion == 3)
@@ -329,9 +339,19 @@ static void mxsfb_enable_controller(struct fb_info *fb_info)
 {
 	struct mxsfb_info *host = to_imxfb_host(fb_info);
 	u32 reg;
+	int ret;
 
 	dev_dbg(&host->pdev->dev, "%s\n", __func__);
 
+	if (host->reg_lcd) {
+		ret = regulator_enable(host->reg_lcd);
+		if (ret) {
+			dev_err(&host->pdev->dev,
+				"lcd regulator enable failed:	%d\n", ret);
+			return;
+		}
+	}
+
 	clk_prepare_enable(host->clk);
 	clk_set_rate(host->clk, PICOS2KHZ(fb_info->var.pixclock) * 1000U);
 
@@ -353,6 +373,7 @@ static void mxsfb_disable_controller(struct fb_info *fb_info)
 	struct mxsfb_info *host = to_imxfb_host(fb_info);
 	unsigned loop;
 	u32 reg;
+	int ret;
 
 	dev_dbg(&host->pdev->dev, "%s\n", __func__);
 
@@ -376,6 +397,13 @@ static void mxsfb_disable_controller(struct fb_info *fb_info)
 	clk_disable_unprepare(host->clk);
 
 	host->enabled = 0;
+
+	if (host->reg_lcd) {
+		ret = regulator_disable(host->reg_lcd);
+		if (ret)
+			dev_err(&host->pdev->dev,
+				"lcd regulator disable failed: %d\n", ret);
+	}
 }
 
 static int mxsfb_set_par(struct fb_info *fb_info)
@@ -459,8 +487,8 @@ static int mxsfb_set_par(struct fb_info *fb_info)
 		vdctrl0 |= VDCTRL0_VSYNC_ACT_HIGH;
 	if (host->sync & MXSFB_SYNC_DATA_ENABLE_HIGH_ACT)
 		vdctrl0 |= VDCTRL0_ENABLE_ACT_HIGH;
-	if (host->sync & MXSFB_SYNC_DOTCLK_FAILING_ACT)
-		vdctrl0 |= VDCTRL0_DOTCLK_ACT_FAILING;
+	if (host->sync & MXSFB_SYNC_DOTCLK_FALLING_ACT)
+		vdctrl0 |= VDCTRL0_DOTCLK_ACT_FALLING;
 
 	writel(vdctrl0, host->base + LCDC_VDCTRL0);
 
@@ -679,14 +707,105 @@ static int mxsfb_restore_mode(struct mxsfb_info *host)
 	return 0;
 }
 
+static int mxsfb_init_fbinfo_dt(struct mxsfb_info *host)
+{
+	struct fb_info *fb_info = &host->fb_info;
+	struct fb_var_screeninfo *var = &fb_info->var;
+	struct device *dev = &host->pdev->dev;
+	struct device_node *np = host->pdev->dev.of_node;
+	struct device_node *display_np;
+	struct device_node *timings_np;
+	struct display_timings *timings;
+	u32 width;
+	int i;
+	int ret = 0;
+
+	display_np = of_parse_phandle(np, "display", 0);
+	if (!display_np) {
+		dev_err(dev, "failed to find display phandle\n");
+		return -ENOENT;
+	}
+
+	ret = of_property_read_u32(display_np, "bus-width", &width);
+	if (ret < 0) {
+		dev_err(dev, "failed to get property bus-width\n");
+		goto put_display_node;
+	}
+
+	switch (width) {
+	case 8:
+		host->ld_intf_width = STMLCDIF_8BIT;
+		break;
+	case 16:
+		host->ld_intf_width = STMLCDIF_16BIT;
+		break;
+	case 18:
+		host->ld_intf_width = STMLCDIF_18BIT;
+		break;
+	case 24:
+		host->ld_intf_width = STMLCDIF_24BIT;
+		break;
+	default:
+		dev_err(dev, "invalid bus-width value\n");
+		ret = -EINVAL;
+		goto put_display_node;
+	}
+
+	ret = of_property_read_u32(display_np, "bits-per-pixel",
+				   &var->bits_per_pixel);
+	if (ret < 0) {
+		dev_err(dev, "failed to get property bits-per-pixel\n");
+		goto put_display_node;
+	}
+
+	timings = of_get_display_timings(display_np);
+	if (!timings) {
+		dev_err(dev, "failed to get display timings\n");
+		ret = -ENOENT;
+		goto put_display_node;
+	}
+
+	timings_np = of_find_node_by_name(display_np,
+					  "display-timings");
+	if (!timings_np) {
+		dev_err(dev, "failed to find display-timings node\n");
+		ret = -ENOENT;
+		goto put_display_node;
+	}
+
+	for (i = 0; i < of_get_child_count(timings_np); i++) {
+		struct videomode vm;
+		struct fb_videomode fb_vm;
+
+		ret = videomode_from_timings(timings, &vm, i);
+		if (ret < 0)
+			goto put_timings_node;
+		ret = fb_videomode_from_videomode(&vm, &fb_vm);
+		if (ret < 0)
+			goto put_timings_node;
+
+		if (vm.flags & DISPLAY_FLAGS_DE_HIGH)
+			host->sync |= MXSFB_SYNC_DATA_ENABLE_HIGH_ACT;
+		if (vm.flags & DISPLAY_FLAGS_PIXDATA_NEGEDGE)
+			host->sync |= MXSFB_SYNC_DOTCLK_FALLING_ACT;
+		fb_add_videomode(&fb_vm, &fb_info->modelist);
+	}
+
+put_timings_node:
+	of_node_put(timings_np);
+put_display_node:
+	of_node_put(display_np);
+	return ret;
+}
+
 static int mxsfb_init_fbinfo(struct mxsfb_info *host)
 {
 	struct fb_info *fb_info = &host->fb_info;
 	struct fb_var_screeninfo *var = &fb_info->var;
-	struct mxsfb_platform_data *pdata = host->pdev->dev.platform_data;
 	dma_addr_t fb_phys;
 	void *fb_virt;
-	unsigned fb_size = pdata->fb_size;
+	unsigned fb_size;
+	int ret;
 
 	fb_info->fbops = &mxsfb_ops;
 	fb_info->flags = FBINFO_FLAG_DEFAULT | FBINFO_READS_FAST;
@@ -696,40 +815,22 @@ static int mxsfb_init_fbinfo(struct mxsfb_info *host)
 	fb_info->fix.visual = FB_VISUAL_TRUECOLOR,
 	fb_info->fix.accel = FB_ACCEL_NONE;
 
-	var->bits_per_pixel = pdata->default_bpp ? pdata->default_bpp : 16;
+	ret = mxsfb_init_fbinfo_dt(host);
+	if (ret)
+		return ret;
+
 	var->nonstd = 0;
 	var->activate = FB_ACTIVATE_NOW;
 	var->accel_flags = 0;
 	var->vmode = FB_VMODE_NONINTERLACED;
 
-	host->dotclk_delay = pdata->dotclk_delay;
-	host->ld_intf_width = pdata->ld_intf_width;
-
 	/* Memory allocation for framebuffer */
-	if (pdata->fb_phys) {
-		if (!fb_size)
-			return -EINVAL;
-
-		fb_phys = pdata->fb_phys;
-
-		if (!request_mem_region(fb_phys, fb_size, host->pdev->name))
-			return -ENOMEM;
+	fb_size = SZ_2M;
+	fb_virt = alloc_pages_exact(fb_size, GFP_DMA);
+	if (!fb_virt)
+		return -ENOMEM;
 
-		fb_virt = ioremap(fb_phys, fb_size);
-		if (!fb_virt) {
-			release_mem_region(fb_phys, fb_size);
-			return -ENOMEM;
-		}
-		host->mapped = 1;
-	} else {
-		if (!fb_size)
-			fb_size = SZ_2M; /* default */
-		fb_virt = alloc_pages_exact(fb_size, GFP_DMA);
-		if (!fb_virt)
-			return -ENOMEM;
-
-		fb_phys = virt_to_phys(fb_virt);
-	}
+	fb_phys = virt_to_phys(fb_virt);
 
 	fb_info->fix.smem_start = fb_phys;
 	fb_info->screen_base = fb_virt;
@@ -745,13 +846,7 @@ static void mxsfb_free_videomem(struct mxsfb_info *host)
 {
 	struct fb_info *fb_info = &host->fb_info;
 
-	if (host->mapped) {
-		iounmap(fb_info->screen_base);
-		release_mem_region(fb_info->fix.smem_start,
-				fb_info->screen_size);
-	} else {
-		free_pages_exact(fb_info->screen_base, fb_info->fix.smem_len);
-	}
+	free_pages_exact(fb_info->screen_base, fb_info->fix.smem_len);
 }
 
 static struct platform_device_id mxsfb_devtype[] = {
@@ -778,47 +873,35 @@ static int mxsfb_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *of_id =
 			of_match_device(mxsfb_dt_ids, &pdev->dev);
-	struct mxsfb_platform_data *pdata = pdev->dev.platform_data;
 	struct resource *res;
 	struct mxsfb_info *host;
 	struct fb_info *fb_info;
 	struct fb_modelist *modelist;
 	struct pinctrl *pinctrl;
-	int panel_enable;
-	enum of_gpio_flags flags;
-	int i, ret;
+	int ret;
 
 	if (of_id)
 		pdev->id_entry = of_id->data;
 
-	if (!pdata) {
-		dev_err(&pdev->dev, "No platformdata. Giving up\n");
-		return -ENODEV;
-	}
-
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_err(&pdev->dev, "Cannot get memory IO resource\n");
 		return -ENODEV;
 	}
 
-	if (!request_mem_region(res->start, resource_size(res), pdev->name))
-		return -EBUSY;
-
 	fb_info = framebuffer_alloc(sizeof(struct mxsfb_info), &pdev->dev);
 	if (!fb_info) {
 		dev_err(&pdev->dev, "Failed to allocate fbdev\n");
-		ret = -ENOMEM;
-		goto error_alloc_info;
+		return -ENOMEM;
 	}
 
 	host = to_imxfb_host(fb_info);
 
-	host->base = ioremap(res->start, resource_size(res));
-	if (!host->base) {
+	host->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(host->base)) {
 		dev_err(&pdev->dev, "ioremap failed\n");
-		ret = -ENOMEM;
-		goto error_ioremap;
+		ret = PTR_ERR(host->base);
+		goto fb_release;
 	}
 
 	host->pdev = pdev;
@@ -829,47 +912,31 @@ static int mxsfb_probe(struct platform_device *pdev)
 	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
 	if (IS_ERR(pinctrl)) {
 		ret = PTR_ERR(pinctrl);
-		goto error_getpin;
+		goto fb_release;
 	}
 
-	host->clk = clk_get(&host->pdev->dev, NULL);
+	host->clk = devm_clk_get(&host->pdev->dev, NULL);
 	if (IS_ERR(host->clk)) {
 		ret = PTR_ERR(host->clk);
-		goto error_getclock;
+		goto fb_release;
 	}
 
-	panel_enable = of_get_named_gpio_flags(pdev->dev.of_node,
-					       "panel-enable-gpios", 0, &flags);
-	if (gpio_is_valid(panel_enable)) {
-		unsigned long f = GPIOF_OUT_INIT_HIGH;
-		if (flags == OF_GPIO_ACTIVE_LOW)
-			f = GPIOF_OUT_INIT_LOW;
-		ret = devm_gpio_request_one(&pdev->dev, panel_enable,
-					    f, "panel-enable");
-		if (ret) {
-			dev_err(&pdev->dev,
-				"failed to request gpio %d: %d\n",
-				panel_enable, ret);
-			goto error_panel_enable;
-		}
-	}
+	host->reg_lcd = devm_regulator_get(&pdev->dev, "lcd");
+	if (IS_ERR(host->reg_lcd))
+		host->reg_lcd = NULL;
 
-	fb_info->pseudo_palette = kmalloc(sizeof(u32) * 16, GFP_KERNEL);
+	fb_info->pseudo_palette = devm_kzalloc(&pdev->dev, sizeof(u32) * 16,
+					       GFP_KERNEL);
 	if (!fb_info->pseudo_palette) {
 		ret = -ENOMEM;
-		goto error_pseudo_pallette;
+		goto fb_release;
 	}
 
 	INIT_LIST_HEAD(&fb_info->modelist);
 
-	host->sync = pdata->sync;
-
 	ret = mxsfb_init_fbinfo(host);
 	if (ret != 0)
-		goto error_init_fb;
-
-	for (i = 0; i < pdata->mode_count; i++)
-		fb_add_videomode(&pdata->mode_list[i], &fb_info->modelist);
+		goto fb_release;
 
 	modelist = list_first_entry(&fb_info->modelist,
 			struct fb_modelist, list);
@@ -883,7 +950,7 @@ static int mxsfb_probe(struct platform_device *pdev)
 	ret = register_framebuffer(fb_info);
 	if (ret != 0) {
 		dev_err(&pdev->dev,"Failed to register framebuffer\n");
-		goto error_register;
+		goto fb_destroy;
 	}
 
 	if (!host->enabled) {
@@ -896,22 +963,12 @@ static int mxsfb_probe(struct platform_device *pdev)
 
 	return 0;
 
-error_register:
+fb_destroy:
 	if (host->enabled)
 		clk_disable_unprepare(host->clk);
 	fb_destroy_modelist(&fb_info->modelist);
-error_init_fb:
-	kfree(fb_info->pseudo_palette);
-error_pseudo_pallette:
-error_panel_enable:
-	clk_put(host->clk);
-error_getclock:
-error_getpin:
-	iounmap(host->base);
-error_ioremap:
+fb_release:
 	framebuffer_release(fb_info);
-error_alloc_info:
-	release_mem_region(res->start, resource_size(res));
 
 	return ret;
 }
@@ -920,19 +977,14 @@ static int mxsfb_remove(struct platform_device *pdev)
 {
 	struct fb_info *fb_info = platform_get_drvdata(pdev);
 	struct mxsfb_info *host = to_imxfb_host(fb_info);
-	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
 	if (host->enabled)
 		mxsfb_disable_controller(fb_info);
 
 	unregister_framebuffer(fb_info);
-	kfree(fb_info->pseudo_palette);
 	mxsfb_free_videomem(host);
-	iounmap(host->base);
-	clk_put(host->clk);
 
 	framebuffer_release(fb_info);
-	release_mem_region(res->start, resource_size(res));
 
 	platform_set_drvdata(pdev, NULL);
 
diff --git a/drivers/video/omap2/Makefile b/drivers/video/omap2/Makefile
index 5ea7cb9aed17..296e5c5281c5 100644
--- a/drivers/video/omap2/Makefile
+++ b/drivers/video/omap2/Makefile
@@ -1,5 +1,5 @@
 obj-$(CONFIG_OMAP2_VRFB) += vrfb.o
 
 obj-$(CONFIG_OMAP2_DSS) += dss/
-obj-$(CONFIG_FB_OMAP2) += omapfb/
 obj-y += displays/
+obj-$(CONFIG_FB_OMAP2) += omapfb/
diff --git a/drivers/video/omap2/displays/panel-acx565akm.c b/drivers/video/omap2/displays/panel-acx565akm.c
index 72699f88c002..d7f69c09ecf1 100644
--- a/drivers/video/omap2/displays/panel-acx565akm.c
+++ b/drivers/video/omap2/displays/panel-acx565akm.c
@@ -29,8 +29,10 @@
 #include <linux/sched.h>
 #include <linux/backlight.h>
 #include <linux/fb.h>
+#include <linux/gpio.h>
 
 #include <video/omapdss.h>
+#include <video/omap-panel-data.h>
 
 #define MIPID_CMD_READ_DISP_ID		0x04
 #define MIPID_CMD_READ_RED		0x06
@@ -336,8 +338,6 @@ static int acx565akm_bl_update_status(struct backlight_device *dev)
 	r = 0;
 	if (md->has_bc)
 		acx565akm_set_brightness(md, level);
-	else if (md->dssdev->set_backlight)
-		r = md->dssdev->set_backlight(md->dssdev, level);
 	else
 		r = -ENODEV;
 
@@ -352,7 +352,7 @@ static int acx565akm_bl_get_intensity(struct backlight_device *dev)
 
 	dev_dbg(&dev->dev, "%s\n", __func__);
 
-	if (!md->has_bc && md->dssdev->set_backlight == NULL)
+	if (!md->has_bc)
 		return -ENODEV;
 
 	if (dev->props.fb_blank == FB_BLANK_UNBLANK &&
@@ -496,21 +496,38 @@ static struct omap_video_timings acx_panel_timings = {
 	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 };
 
+static struct panel_acx565akm_data *get_panel_data(struct omap_dss_device *dssdev)
+{
+	return (struct panel_acx565akm_data *) dssdev->data;
+}
+
 static int acx_panel_probe(struct omap_dss_device *dssdev)
 {
 	int r;
 	struct acx565akm_device *md = &acx_dev;
+	struct panel_acx565akm_data *panel_data = get_panel_data(dssdev);
 	struct backlight_device *bldev;
 	int max_brightness, brightness;
 	struct backlight_properties props;
 
 	dev_dbg(&dssdev->dev, "%s\n", __func__);
 
+	if (!panel_data)
+		return -EINVAL;
+
 	/* FIXME AC bias ? */
 	dssdev->panel.timings = acx_panel_timings;
 
-	if (dssdev->platform_enable)
-		dssdev->platform_enable(dssdev);
+	if (gpio_is_valid(panel_data->reset_gpio)) {
+		r = devm_gpio_request_one(&dssdev->dev, panel_data->reset_gpio,
+				GPIOF_OUT_INIT_LOW, "lcd reset");
+		if (r)
+			return r;
+	}
+
+	if (gpio_is_valid(panel_data->reset_gpio))
+		gpio_set_value(panel_data->reset_gpio, 1);
+
 	/*
 	 * After reset we have to wait 5 msec before the first
 	 * command can be sent.
@@ -522,8 +539,9 @@ static int acx_panel_probe(struct omap_dss_device *dssdev)
 	r = panel_detect(md);
 	if (r) {
 		dev_err(&dssdev->dev, "%s panel detect error\n", __func__);
-		if (!md->enabled && dssdev->platform_disable)
-			dssdev->platform_disable(dssdev);
+		if (!md->enabled && gpio_is_valid(panel_data->reset_gpio))
+			gpio_set_value(panel_data->reset_gpio, 0);
+
 		return r;
 	}
 
@@ -532,8 +550,8 @@ static int acx_panel_probe(struct omap_dss_device *dssdev)
 	mutex_unlock(&acx_dev.mutex);
 
 	if (!md->enabled) {
-		if (dssdev->platform_disable)
-			dssdev->platform_disable(dssdev);
+		if (gpio_is_valid(panel_data->reset_gpio))
+			gpio_set_value(panel_data->reset_gpio, 0);
 	}
 
 	/*------- Backlight control --------*/
@@ -557,15 +575,10 @@ static int acx_panel_probe(struct omap_dss_device *dssdev)
 		md->cabc_mode = get_hw_cabc_mode(md);
 	}
 
-	if (md->has_bc)
-		max_brightness = 255;
-	else
-		max_brightness = dssdev->max_backlight_level;
+	max_brightness = 255;
 
 	if (md->has_bc)
 		brightness = acx565akm_get_actual_brightness(md);
-	else if (dssdev->get_backlight)
-		brightness = dssdev->get_backlight(dssdev);
 	else
 		brightness = 0;
 
@@ -591,6 +604,7 @@ static void acx_panel_remove(struct omap_dss_device *dssdev)
 static int acx_panel_power_on(struct omap_dss_device *dssdev)
 {
 	struct acx565akm_device *md = &acx_dev;
+	struct panel_acx565akm_data *panel_data = get_panel_data(dssdev);
 	int r;
 
 	dev_dbg(&dssdev->dev, "%s\n", __func__);
@@ -612,11 +626,8 @@ static int acx_panel_power_on(struct omap_dss_device *dssdev)
 	/*FIXME tweak me */
 	msleep(50);
 
-	if (dssdev->platform_enable) {
-		r = dssdev->platform_enable(dssdev);
-		if (r)
-			goto fail;
-	}
+	if (gpio_is_valid(panel_data->reset_gpio))
+		gpio_set_value(panel_data->reset_gpio, 1);
 
 	if (md->enabled) {
 		dev_dbg(&md->spi->dev, "panel already enabled\n");
@@ -645,8 +656,7 @@ static int acx_panel_power_on(struct omap_dss_device *dssdev)
 	mutex_unlock(&md->mutex);
 
 	return acx565akm_bl_update_status(md->bl_dev);
-fail:
-	omapdss_sdi_display_disable(dssdev);
+
 fail_unlock:
 	mutex_unlock(&md->mutex);
 	return r;
@@ -655,6 +665,7 @@ fail_unlock:
 static void acx_panel_power_off(struct omap_dss_device *dssdev)
 {
 	struct acx565akm_device *md = &acx_dev;
+	struct panel_acx565akm_data *panel_data = get_panel_data(dssdev);
 
 	dev_dbg(&dssdev->dev, "%s\n", __func__);
 
@@ -678,8 +689,8 @@ static void acx_panel_power_off(struct omap_dss_device *dssdev)
 	 */
 	msleep(50);
 
-	if (dssdev->platform_disable)
-		dssdev->platform_disable(dssdev);
+	if (gpio_is_valid(panel_data->reset_gpio))
+		gpio_set_value(panel_data->reset_gpio, 0);
 
 	/* FIXME need to tweak this delay */
 	msleep(100);
diff --git a/drivers/video/omap2/displays/panel-generic-dpi.c b/drivers/video/omap2/displays/panel-generic-dpi.c
index c904f42d81c1..97363f733683 100644
--- a/drivers/video/omap2/displays/panel-generic-dpi.c
+++ b/drivers/video/omap2/displays/panel-generic-dpi.c
@@ -33,9 +33,10 @@
 #include <linux/module.h>
 #include <linux/delay.h>
 #include <linux/slab.h>
+#include <linux/gpio.h>
 #include <video/omapdss.h>
 
-#include <video/omap-panel-generic-dpi.h>
+#include <video/omap-panel-data.h>
 
 struct panel_config {
 	struct omap_video_timings timings;
@@ -533,7 +534,7 @@ static inline struct panel_generic_dpi_data
 
 static int generic_dpi_panel_power_on(struct omap_dss_device *dssdev)
 {
-	int r;
+	int r, i;
 	struct panel_generic_dpi_data *panel_data = get_panel_data(dssdev);
 	struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
 	struct panel_config *panel_config = drv_data->panel_config;
@@ -552,15 +553,13 @@ static int generic_dpi_panel_power_on(struct omap_dss_device *dssdev)
 	if (panel_config->power_on_delay)
 		msleep(panel_config->power_on_delay);
 
-	if (panel_data->platform_enable) {
-		r = panel_data->platform_enable(dssdev);
-		if (r)
-			goto err1;
+	for (i = 0; i < panel_data->num_gpios; ++i) {
+		gpio_set_value_cansleep(panel_data->gpios[i],
+				panel_data->gpio_invert[i] ? 0 : 1);
 	}
 
 	return 0;
-err1:
-	omapdss_dpi_display_disable(dssdev);
+
 err0:
 	return r;
 }
@@ -570,12 +569,15 @@ static void generic_dpi_panel_power_off(struct omap_dss_device *dssdev)
 	struct panel_generic_dpi_data *panel_data = get_panel_data(dssdev);
 	struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
 	struct panel_config *panel_config = drv_data->panel_config;
+	int i;
 
 	if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
 		return;
 
-	if (panel_data->platform_disable)
-		panel_data->platform_disable(dssdev);
+	for (i = panel_data->num_gpios - 1; i >= 0; --i) {
+		gpio_set_value_cansleep(panel_data->gpios[i],
+				panel_data->gpio_invert[i] ? 1 : 0);
+	}
 
 	/* wait couple of vsyncs after disabling the LCD */
 	if (panel_config->power_off_delay)
@@ -589,7 +591,7 @@ static int generic_dpi_panel_probe(struct omap_dss_device *dssdev)
 	struct panel_generic_dpi_data *panel_data = get_panel_data(dssdev);
 	struct panel_config *panel_config = NULL;
 	struct panel_drv_data *drv_data = NULL;
-	int i;
+	int i, r;
 
 	dev_dbg(&dssdev->dev, "probe\n");
 
@@ -606,9 +608,18 @@ static int generic_dpi_panel_probe(struct omap_dss_device *dssdev)
 	if (!panel_config)
 		return -EINVAL;
 
+	for (i = 0; i < panel_data->num_gpios; ++i) {
+		r = devm_gpio_request_one(&dssdev->dev, panel_data->gpios[i],
+				panel_data->gpio_invert[i] ?
+				GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW,
+				"panel gpio");
+		if (r)
+			return r;
+	}
+
 	dssdev->panel.timings = panel_config->timings;
 
-	drv_data = kzalloc(sizeof(*drv_data), GFP_KERNEL);
+	drv_data = devm_kzalloc(&dssdev->dev, sizeof(*drv_data), GFP_KERNEL);
 	if (!drv_data)
 		return -ENOMEM;
 
@@ -624,12 +635,8 @@ static int generic_dpi_panel_probe(struct omap_dss_device *dssdev)
 
 static void __exit generic_dpi_panel_remove(struct omap_dss_device *dssdev)
 {
-	struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
-
 	dev_dbg(&dssdev->dev, "remove\n");
 
-	kfree(drv_data);
-
 	dev_set_drvdata(&dssdev->dev, NULL);
 }
 
diff --git a/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
index 6e5abe8fd2dd..4ea6548c0ae9 100644
--- a/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
+++ b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
@@ -20,8 +20,10 @@
 #include <linux/delay.h>
 #include <linux/spi/spi.h>
 #include <linux/mutex.h>
+#include <linux/gpio.h>
 
 #include <video/omapdss.h>
+#include <video/omap-panel-data.h>
 
 struct lb035q02_data {
 	struct mutex lock;
@@ -48,9 +50,16 @@ static struct omap_video_timings lb035q02_timings = {
 	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 };
 
+static inline struct panel_generic_dpi_data
+*get_panel_data(const struct omap_dss_device *dssdev)
+{
+	return (struct panel_generic_dpi_data *) dssdev->data;
+}
+
 static int lb035q02_panel_power_on(struct omap_dss_device *dssdev)
 {
-	int r;
+	struct panel_generic_dpi_data *panel_data = get_panel_data(dssdev);
+	int r, i;
 
 	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
 		return 0;
@@ -62,54 +71,65 @@ static int lb035q02_panel_power_on(struct omap_dss_device *dssdev)
 	if (r)
 		goto err0;
 
-	if (dssdev->platform_enable) {
-		r = dssdev->platform_enable(dssdev);
-		if (r)
-			goto err1;
+	for (i = 0; i < panel_data->num_gpios; ++i) {
+		gpio_set_value_cansleep(panel_data->gpios[i],
+				panel_data->gpio_invert[i] ? 0 : 1);
 	}
 
 	return 0;
-err1:
-	omapdss_dpi_display_disable(dssdev);
+
 err0:
 	return r;
 }
 
 static void lb035q02_panel_power_off(struct omap_dss_device *dssdev)
 {
+	struct panel_generic_dpi_data *panel_data = get_panel_data(dssdev);
+	int i;
+
 	if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
 		return;
 
-	if (dssdev->platform_disable)
-		dssdev->platform_disable(dssdev);
+	for (i = panel_data->num_gpios - 1; i >= 0; --i) {
+		gpio_set_value_cansleep(panel_data->gpios[i],
+				panel_data->gpio_invert[i] ? 1 : 0);
+	}
 
 	omapdss_dpi_display_disable(dssdev);
 }
 
 static int lb035q02_panel_probe(struct omap_dss_device *dssdev)
 {
+	struct panel_generic_dpi_data *panel_data = get_panel_data(dssdev);
 	struct lb035q02_data *ld;
-	int r;
+	int r, i;
+
+	if (!panel_data)
+		return -EINVAL;
 
 	dssdev->panel.timings = lb035q02_timings;
 
-	ld = kzalloc(sizeof(*ld), GFP_KERNEL);
-	if (!ld) {
-		r = -ENOMEM;
-		goto err;
+	ld = devm_kzalloc(&dssdev->dev, sizeof(*ld), GFP_KERNEL);
+	if (!ld)
+		return -ENOMEM;
+
+	for (i = 0; i < panel_data->num_gpios; ++i) {
+		r = devm_gpio_request_one(&dssdev->dev, panel_data->gpios[i],
+				panel_data->gpio_invert[i] ?
+				GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW,
+				"panel gpio");
+		if (r)
+			return r;
 	}
+
 	mutex_init(&ld->lock);
 	dev_set_drvdata(&dssdev->dev, ld);
+
 	return 0;
-err:
-	return r;
 }
 
 static void lb035q02_panel_remove(struct omap_dss_device *dssdev)
 {
-	struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
-
-	kfree(ld);
 }
 
 static int lb035q02_panel_enable(struct omap_dss_device *dssdev)
diff --git a/drivers/video/omap2/displays/panel-n8x0.c b/drivers/video/omap2/displays/panel-n8x0.c
index dd1294750802..f94ead6a3183 100644
--- a/drivers/video/omap2/displays/panel-n8x0.c
+++ b/drivers/video/omap2/displays/panel-n8x0.c
@@ -5,11 +5,10 @@
 #include <linux/slab.h>
 #include <linux/gpio.h>
 #include <linux/spi/spi.h>
-#include <linux/backlight.h>
 #include <linux/fb.h>
 
 #include <video/omapdss.h>
-#include <video/omap-panel-n8x0.h>
+#include <video/omap-panel-data.h>
 
 #define BLIZZARD_REV_CODE                      0x00
 #define BLIZZARD_CONFIG                        0x02
@@ -69,7 +68,6 @@ static struct panel_drv_data {
 
 	struct omap_dss_device *dssdev;
 	struct spi_device *spidev;
-	struct backlight_device *bldev;
 
 	int blizzard_ver;
 } s_drv_data;
@@ -297,12 +295,6 @@ static int n8x0_panel_power_on(struct omap_dss_device *dssdev)
 
 	gpio_direction_output(bdata->ctrl_pwrdown, 1);
 
-	if (bdata->platform_enable) {
-		r = bdata->platform_enable(dssdev);
-		if (r)
-			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);
@@ -375,9 +367,6 @@ err_inv_panel:
 err_inv_chip:
 	omapdss_rfbi_display_disable(dssdev);
 err_rfbi_en:
-	if (bdata->platform_disable)
-		bdata->platform_disable(dssdev);
-err_plat_en:
 	gpio_direction_output(bdata->ctrl_pwrdown, 0);
 	return r;
 }
@@ -394,9 +383,6 @@ static void n8x0_panel_power_off(struct omap_dss_device *dssdev)
 	send_display_off(spi);
 	send_sleep_in(spi);
 
-	if (bdata->platform_disable)
-		bdata->platform_disable(dssdev);
-
 	/*
 	 * HACK: we should turn off the panel here, but there is some problem
 	 * with the initialization sequence, and we fail to init the panel if we
@@ -424,54 +410,10 @@ static const struct rfbi_timings n8x0_panel_timings = {
 	.cs_pulse_width = 0,
 };
 
-static int n8x0_bl_update_status(struct backlight_device *dev)
-{
-	struct omap_dss_device *dssdev = dev_get_drvdata(&dev->dev);
-	struct panel_n8x0_data *bdata = get_board_data(dssdev);
-	struct panel_drv_data *ddata = get_drv_data(dssdev);
-	int r;
-	int level;
-
-	mutex_lock(&ddata->lock);
-
-	if (dev->props.fb_blank == FB_BLANK_UNBLANK &&
-			dev->props.power == FB_BLANK_UNBLANK)
-		level = dev->props.brightness;
-	else
-		level = 0;
-
-	dev_dbg(&dssdev->dev, "update brightness to %d\n", level);
-
-	if (!bdata->set_backlight)
-		r = -EINVAL;
-	else
-		r = bdata->set_backlight(dssdev, level);
-
-	mutex_unlock(&ddata->lock);
-
-	return r;
-}
-
-static int n8x0_bl_get_intensity(struct backlight_device *dev)
-{
-	if (dev->props.fb_blank == FB_BLANK_UNBLANK &&
-			dev->props.power == FB_BLANK_UNBLANK)
-		return dev->props.brightness;
-
-	return 0;
-}
-
-static const struct backlight_ops n8x0_bl_ops = {
-	.get_brightness = n8x0_bl_get_intensity,
-	.update_status  = n8x0_bl_update_status,
-};
-
 static int n8x0_panel_probe(struct omap_dss_device *dssdev)
 {
 	struct panel_n8x0_data *bdata = get_board_data(dssdev);
 	struct panel_drv_data *ddata;
-	struct backlight_device *bldev;
-	struct backlight_properties props;
 	int r;
 
 	dev_dbg(&dssdev->dev, "probe\n");
@@ -491,40 +433,27 @@ static int n8x0_panel_probe(struct omap_dss_device *dssdev)
 	dssdev->ctrl.rfbi_timings = n8x0_panel_timings;
 	dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE;
 
-	memset(&props, 0, sizeof(props));
-	props.max_brightness = 127;
-	props.type = BACKLIGHT_PLATFORM;
-	bldev = backlight_device_register(dev_name(&dssdev->dev), &dssdev->dev,
-			dssdev, &n8x0_bl_ops, &props);
-	if (IS_ERR(bldev)) {
-		r = PTR_ERR(bldev);
-		dev_err(&dssdev->dev, "register backlight failed\n");
-		return r;
+	if (gpio_is_valid(bdata->panel_reset)) {
+		r = devm_gpio_request_one(&dssdev->dev, bdata->panel_reset,
+				GPIOF_OUT_INIT_LOW, "PANEL RESET");
+		if (r)
+			return r;
 	}
 
-	ddata->bldev = bldev;
-
-	bldev->props.fb_blank = FB_BLANK_UNBLANK;
-	bldev->props.power = FB_BLANK_UNBLANK;
-	bldev->props.brightness = 127;
-
-	n8x0_bl_update_status(bldev);
+	if (gpio_is_valid(bdata->ctrl_pwrdown)) {
+		r = devm_gpio_request_one(&dssdev->dev, bdata->ctrl_pwrdown,
+				GPIOF_OUT_INIT_LOW, "PANEL PWRDOWN");
+		if (r)
+			return r;
+	}
 
 	return 0;
 }
 
 static void n8x0_panel_remove(struct omap_dss_device *dssdev)
 {
-	struct panel_drv_data *ddata = get_drv_data(dssdev);
-	struct backlight_device *bldev;
-
 	dev_dbg(&dssdev->dev, "remove\n");
 
-	bldev = ddata->bldev;
-	bldev->props.power = FB_BLANK_POWERDOWN;
-	n8x0_bl_update_status(bldev);
-	backlight_device_unregister(bldev);
-
 	dev_set_drvdata(&dssdev->dev, NULL);
 }
 
diff --git a/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c b/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
index c4e9c2b1b465..20c3cd91ff9b 100644
--- a/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
+++ b/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
@@ -19,10 +19,11 @@
 #include <linux/module.h>
 #include <linux/delay.h>
 #include <linux/spi/spi.h>
-#include <linux/backlight.h>
 #include <linux/fb.h>
+#include <linux/gpio.h>
 
 #include <video/omapdss.h>
+#include <video/omap-panel-data.h>
 
 #define LCD_XRES		800
 #define LCD_YRES		480
@@ -32,10 +33,6 @@
  */
 #define LCD_PIXEL_CLOCK		23800
 
-struct nec_8048_data {
-	struct backlight_device *bl;
-};
-
 static const struct {
 	unsigned char addr;
 	unsigned char dat;
@@ -84,93 +81,47 @@ static struct omap_video_timings nec_8048_panel_timings = {
 	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
 };
 
-static int nec_8048_bl_update_status(struct backlight_device *bl)
-{
-	struct omap_dss_device *dssdev = dev_get_drvdata(&bl->dev);
-	int level;
-
-	if (!dssdev->set_backlight)
-		return -EINVAL;
-
-	if (bl->props.fb_blank == FB_BLANK_UNBLANK &&
-			bl->props.power == FB_BLANK_UNBLANK)
-		level = bl->props.brightness;
-	else
-		level = 0;
-
-	return dssdev->set_backlight(dssdev, level);
-}
-
-static int nec_8048_bl_get_brightness(struct backlight_device *bl)
+static inline struct panel_nec_nl8048_data
+*get_panel_data(const struct omap_dss_device *dssdev)
 {
-	if (bl->props.fb_blank == FB_BLANK_UNBLANK &&
-			bl->props.power == FB_BLANK_UNBLANK)
-		return bl->props.brightness;
-
-	return 0;
+	return (struct panel_nec_nl8048_data *) dssdev->data;
 }
 
-static const struct backlight_ops nec_8048_bl_ops = {
-	.get_brightness	= nec_8048_bl_get_brightness,
-	.update_status	= nec_8048_bl_update_status,
-};
-
 static int nec_8048_panel_probe(struct omap_dss_device *dssdev)
 {
-	struct backlight_device *bl;
-	struct nec_8048_data *necd;
-	struct backlight_properties props;
+	struct panel_nec_nl8048_data *pd = get_panel_data(dssdev);
 	int r;
 
-	dssdev->panel.timings = nec_8048_panel_timings;
-
-	necd = kzalloc(sizeof(*necd), GFP_KERNEL);
-	if (!necd)
-		return -ENOMEM;
-
-	dev_set_drvdata(&dssdev->dev, necd);
+	if (!pd)
+		return -EINVAL;
 
-	memset(&props, 0, sizeof(struct backlight_properties));
-	props.max_brightness = 255;
+	dssdev->panel.timings = nec_8048_panel_timings;
 
-	bl = backlight_device_register("nec-8048", &dssdev->dev, dssdev,
-			&nec_8048_bl_ops, &props);
-	if (IS_ERR(bl)) {
-		r = PTR_ERR(bl);
-		kfree(necd);
-		return r;
+	if (gpio_is_valid(pd->qvga_gpio)) {
+		r = devm_gpio_request_one(&dssdev->dev, pd->qvga_gpio,
+				GPIOF_OUT_INIT_HIGH, "lcd QVGA");
+		if (r)
+			return r;
 	}
-	necd->bl = bl;
-
-	bl->props.fb_blank = FB_BLANK_UNBLANK;
-	bl->props.power = FB_BLANK_UNBLANK;
-	bl->props.max_brightness = dssdev->max_backlight_level;
-	bl->props.brightness = dssdev->max_backlight_level;
 
-	r = nec_8048_bl_update_status(bl);
-	if (r < 0)
-		dev_err(&dssdev->dev, "failed to set lcd brightness\n");
+	if (gpio_is_valid(pd->res_gpio)) {
+		r = devm_gpio_request_one(&dssdev->dev, pd->res_gpio,
+				GPIOF_OUT_INIT_LOW, "lcd RES");
+		if (r)
+			return r;
+	}
 
 	return 0;
 }
 
 static void nec_8048_panel_remove(struct omap_dss_device *dssdev)
 {
-	struct nec_8048_data *necd = dev_get_drvdata(&dssdev->dev);
-	struct backlight_device *bl = necd->bl;
-
-	bl->props.power = FB_BLANK_POWERDOWN;
-	nec_8048_bl_update_status(bl);
-	backlight_device_unregister(bl);
-
-	kfree(necd);
 }
 
 static int nec_8048_panel_power_on(struct omap_dss_device *dssdev)
 {
+	struct panel_nec_nl8048_data *pd = get_panel_data(dssdev);
 	int r;
-	struct nec_8048_data *necd = dev_get_drvdata(&dssdev->dev);
-	struct backlight_device *bl = necd->bl;
 
 	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
 		return 0;
@@ -182,36 +133,24 @@ static int nec_8048_panel_power_on(struct omap_dss_device *dssdev)
 	if (r)
 		goto err0;
 
-	if (dssdev->platform_enable) {
-		r = dssdev->platform_enable(dssdev);
-		if (r)
-			goto err1;
-	}
-
-	r = nec_8048_bl_update_status(bl);
-	if (r < 0)
-		dev_err(&dssdev->dev, "failed to set lcd brightness\n");
+	if (gpio_is_valid(pd->res_gpio))
+		gpio_set_value_cansleep(pd->res_gpio, 1);
 
 	return 0;
-err1:
-	omapdss_dpi_display_disable(dssdev);
+
 err0:
 	return r;
 }
 
 static void nec_8048_panel_power_off(struct omap_dss_device *dssdev)
 {
-	struct nec_8048_data *necd = dev_get_drvdata(&dssdev->dev);
-	struct backlight_device *bl = necd->bl;
+	struct panel_nec_nl8048_data *pd = get_panel_data(dssdev);
 
 	if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
 		return;
 
-	bl->props.brightness = 0;
-	nec_8048_bl_update_status(bl);
-
-	if (dssdev->platform_disable)
-		dssdev->platform_disable(dssdev);
+	if (gpio_is_valid(pd->res_gpio))
+		gpio_set_value_cansleep(pd->res_gpio, 0);
 
 	omapdss_dpi_display_disable(dssdev);
 }
@@ -303,16 +242,22 @@ static int nec_8048_spi_remove(struct spi_device *spi)
 	return 0;
 }
 
-static int nec_8048_spi_suspend(struct spi_device *spi, pm_message_t mesg)
+#ifdef CONFIG_PM_SLEEP
+
+static int nec_8048_spi_suspend(struct device *dev)
 {
+	struct spi_device *spi = to_spi_device(dev);
+
 	nec_8048_spi_send(spi, 2, 0x01);
 	mdelay(40);
 
 	return 0;
 }
 
-static int nec_8048_spi_resume(struct spi_device *spi)
+static int nec_8048_spi_resume(struct device *dev)
 {
+	struct spi_device *spi = to_spi_device(dev);
+
 	/* reinitialize the panel */
 	spi_setup(spi);
 	nec_8048_spi_send(spi, 2, 0x00);
@@ -321,14 +266,20 @@ static int nec_8048_spi_resume(struct spi_device *spi)
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(nec_8048_spi_pm_ops, nec_8048_spi_suspend,
+		nec_8048_spi_resume);
+#define NEC_8048_SPI_PM_OPS (&nec_8048_spi_pm_ops)
+#else
+#define NEC_8048_SPI_PM_OPS NULL
+#endif
+
 static struct spi_driver nec_8048_spi_driver = {
 	.probe		= nec_8048_spi_probe,
 	.remove		= nec_8048_spi_remove,
-	.suspend	= nec_8048_spi_suspend,
-	.resume		= nec_8048_spi_resume,
 	.driver		= {
 		.name	= "nec_8048_spi",
 		.owner	= THIS_MODULE,
+		.pm	= NEC_8048_SPI_PM_OPS,
 	},
 };
 
diff --git a/drivers/video/omap2/displays/panel-picodlp.c b/drivers/video/omap2/displays/panel-picodlp.c
index 1b94018aac3e..62f2db04fbc8 100644
--- a/drivers/video/omap2/displays/panel-picodlp.c
+++ b/drivers/video/omap2/displays/panel-picodlp.c
@@ -31,7 +31,7 @@
 #include <linux/gpio.h>
 
 #include <video/omapdss.h>
-#include <video/omap-panel-picodlp.h>
+#include <video/omap-panel-data.h>
 
 #include "panel-picodlp.h"
 
@@ -354,12 +354,6 @@ static int picodlp_panel_power_on(struct omap_dss_device *dssdev)
 	struct picodlp_data *picod = dev_get_drvdata(&dssdev->dev);
 	struct picodlp_panel_data *picodlp_pdata = get_panel_data(dssdev);
 
-	if (dssdev->platform_enable) {
-		r = dssdev->platform_enable(dssdev);
-		if (r)
-			return r;
-	}
-
 	gpio_set_value(picodlp_pdata->pwrgood_gpio, 0);
 	msleep(1);
 	gpio_set_value(picodlp_pdata->pwrgood_gpio, 1);
@@ -398,9 +392,6 @@ static int picodlp_panel_power_on(struct omap_dss_device *dssdev)
 err:
 	omapdss_dpi_display_disable(dssdev);
 err1:
-	if (dssdev->platform_disable)
-		dssdev->platform_disable(dssdev);
-
 	return r;
 }
 
@@ -412,9 +403,6 @@ static void picodlp_panel_power_off(struct omap_dss_device *dssdev)
 
 	gpio_set_value(picodlp_pdata->emu_done_gpio, 0);
 	gpio_set_value(picodlp_pdata->pwrgood_gpio, 0);
-
-	if (dssdev->platform_disable)
-		dssdev->platform_disable(dssdev);
 }
 
 static int picodlp_panel_probe(struct omap_dss_device *dssdev)
@@ -423,11 +411,14 @@ static int picodlp_panel_probe(struct omap_dss_device *dssdev)
 	struct picodlp_panel_data *picodlp_pdata = get_panel_data(dssdev);
 	struct i2c_adapter *adapter;
 	struct i2c_client *picodlp_i2c_client;
-	int r = 0, picodlp_adapter_id;
+	int r, picodlp_adapter_id;
 
 	dssdev->panel.timings = pico_ls_timings;
 
-	picod =  kzalloc(sizeof(struct picodlp_data), GFP_KERNEL);
+	if (!picodlp_pdata)
+		return -EINVAL;
+
+	picod = devm_kzalloc(&dssdev->dev, sizeof(*picod), GFP_KERNEL);
 	if (!picod)
 		return -ENOMEM;
 
@@ -438,25 +429,37 @@ static int picodlp_panel_probe(struct omap_dss_device *dssdev)
 	adapter = i2c_get_adapter(picodlp_adapter_id);
 	if (!adapter) {
 		dev_err(&dssdev->dev, "can't get i2c adapter\n");
-		r = -ENODEV;
-		goto err;
+		return -ENODEV;
 	}
 
 	picodlp_i2c_client = i2c_new_device(adapter, &picodlp_i2c_board_info);
 	if (!picodlp_i2c_client) {
 		dev_err(&dssdev->dev, "can't add i2c device::"
 					 " picodlp_i2c_client is NULL\n");
-		r = -ENODEV;
-		goto err;
+		return -ENODEV;
 	}
 
 	picod->picodlp_i2c_client = picodlp_i2c_client;
 
 	dev_set_drvdata(&dssdev->dev, picod);
-	return r;
-err:
-	kfree(picod);
-	return r;
+
+	if (gpio_is_valid(picodlp_pdata->emu_done_gpio)) {
+		r = devm_gpio_request_one(&dssdev->dev,
+				picodlp_pdata->emu_done_gpio,
+				GPIOF_IN, "DLP EMU DONE");
+		if (r)
+			return r;
+	}
+
+	if (gpio_is_valid(picodlp_pdata->pwrgood_gpio)) {
+		r = devm_gpio_request_one(&dssdev->dev,
+				picodlp_pdata->pwrgood_gpio,
+				GPIOF_OUT_INIT_LOW, "DLP PWRGOOD");
+		if (r)
+			return r;
+	}
+
+	return 0;
 }
 
 static void picodlp_panel_remove(struct omap_dss_device *dssdev)
diff --git a/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c b/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
index cada8c621e01..74cb0eb45311 100644
--- a/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
+++ b/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
@@ -20,16 +20,13 @@
 #include <linux/module.h>
 #include <linux/delay.h>
 #include <linux/device.h>
-#include <linux/backlight.h>
 #include <linux/fb.h>
 #include <linux/err.h>
 #include <linux/slab.h>
+#include <linux/gpio.h>
 
 #include <video/omapdss.h>
-
-struct sharp_data {
-	struct backlight_device *bl;
-};
+#include <video/omap-panel-data.h>
 
 static struct omap_video_timings sharp_ls_timings = {
 	.x_res = 480,
@@ -52,91 +49,67 @@ static struct omap_video_timings sharp_ls_timings = {
 	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 };
 
-static int sharp_ls_bl_update_status(struct backlight_device *bl)
+static inline struct panel_sharp_ls037v7dw01_data
+*get_panel_data(const struct omap_dss_device *dssdev)
 {
-	struct omap_dss_device *dssdev = dev_get_drvdata(&bl->dev);
-	int level;
-
-	if (!dssdev->set_backlight)
-		return -EINVAL;
-
-	if (bl->props.fb_blank == FB_BLANK_UNBLANK &&
-			bl->props.power == FB_BLANK_UNBLANK)
-		level = bl->props.brightness;
-	else
-		level = 0;
-
-	return dssdev->set_backlight(dssdev, level);
+	return (struct panel_sharp_ls037v7dw01_data *) dssdev->data;
 }
 
-static int sharp_ls_bl_get_brightness(struct backlight_device *bl)
-{
-	if (bl->props.fb_blank == FB_BLANK_UNBLANK &&
-			bl->props.power == FB_BLANK_UNBLANK)
-		return bl->props.brightness;
-
-	return 0;
-}
-
-static const struct backlight_ops sharp_ls_bl_ops = {
-	.get_brightness = sharp_ls_bl_get_brightness,
-	.update_status  = sharp_ls_bl_update_status,
-};
-
-
-
 static int sharp_ls_panel_probe(struct omap_dss_device *dssdev)
 {
-	struct backlight_properties props;
-	struct backlight_device *bl;
-	struct sharp_data *sd;
+	struct panel_sharp_ls037v7dw01_data *pd = get_panel_data(dssdev);
 	int r;
 
+	if (!pd)
+		return -EINVAL;
+
 	dssdev->panel.timings = sharp_ls_timings;
 
-	sd = kzalloc(sizeof(*sd), GFP_KERNEL);
-	if (!sd)
-		return -ENOMEM;
+	if (gpio_is_valid(pd->mo_gpio)) {
+		r = devm_gpio_request_one(&dssdev->dev, pd->mo_gpio,
+				GPIOF_OUT_INIT_LOW, "lcd MO");
+		if (r)
+			return r;
+	}
 
-	dev_set_drvdata(&dssdev->dev, sd);
+	if (gpio_is_valid(pd->lr_gpio)) {
+		r = devm_gpio_request_one(&dssdev->dev, pd->lr_gpio,
+				GPIOF_OUT_INIT_HIGH, "lcd LR");
+		if (r)
+			return r;
+	}
 
-	memset(&props, 0, sizeof(struct backlight_properties));
-	props.max_brightness = dssdev->max_backlight_level;
-	props.type = BACKLIGHT_RAW;
+	if (gpio_is_valid(pd->ud_gpio)) {
+		r = devm_gpio_request_one(&dssdev->dev, pd->ud_gpio,
+				GPIOF_OUT_INIT_HIGH, "lcd UD");
+		if (r)
+			return r;
+	}
 
-	bl = backlight_device_register("sharp-ls", &dssdev->dev, dssdev,
-			&sharp_ls_bl_ops, &props);
-	if (IS_ERR(bl)) {
-		r = PTR_ERR(bl);
-		kfree(sd);
-		return r;
+	if (gpio_is_valid(pd->resb_gpio)) {
+		r = devm_gpio_request_one(&dssdev->dev, pd->resb_gpio,
+				GPIOF_OUT_INIT_LOW, "lcd RESB");
+		if (r)
+			return r;
 	}
-	sd->bl = bl;
 
-	bl->props.fb_blank = FB_BLANK_UNBLANK;
-	bl->props.power = FB_BLANK_UNBLANK;
-	bl->props.brightness = dssdev->max_backlight_level;
-	r = sharp_ls_bl_update_status(bl);
-	if (r < 0)
-		dev_err(&dssdev->dev, "failed to set lcd brightness\n");
+	if (gpio_is_valid(pd->ini_gpio)) {
+		r = devm_gpio_request_one(&dssdev->dev, pd->ini_gpio,
+				GPIOF_OUT_INIT_LOW, "lcd INI");
+		if (r)
+			return r;
+	}
 
 	return 0;
 }
 
 static void __exit sharp_ls_panel_remove(struct omap_dss_device *dssdev)
 {
-	struct sharp_data *sd = dev_get_drvdata(&dssdev->dev);
-	struct backlight_device *bl = sd->bl;
-
-	bl->props.power = FB_BLANK_POWERDOWN;
-	sharp_ls_bl_update_status(bl);
-	backlight_device_unregister(bl);
-
-	kfree(sd);
 }
 
 static int sharp_ls_power_on(struct omap_dss_device *dssdev)
 {
+	struct panel_sharp_ls037v7dw01_data *pd = get_panel_data(dssdev);
 	int r = 0;
 
 	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
@@ -152,26 +125,29 @@ static int sharp_ls_power_on(struct omap_dss_device *dssdev)
 	/* wait couple of vsyncs until enabling the LCD */
 	msleep(50);
 
-	if (dssdev->platform_enable) {
-		r = dssdev->platform_enable(dssdev);
-		if (r)
-			goto err1;
-	}
+	if (gpio_is_valid(pd->resb_gpio))
+		gpio_set_value_cansleep(pd->resb_gpio, 1);
+
+	if (gpio_is_valid(pd->ini_gpio))
+		gpio_set_value_cansleep(pd->ini_gpio, 1);
 
 	return 0;
-err1:
-	omapdss_dpi_display_disable(dssdev);
 err0:
 	return r;
 }
 
 static void sharp_ls_power_off(struct omap_dss_device *dssdev)
 {
+	struct panel_sharp_ls037v7dw01_data *pd = get_panel_data(dssdev);
+
 	if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
 		return;
 
-	if (dssdev->platform_disable)
-		dssdev->platform_disable(dssdev);
+	if (gpio_is_valid(pd->ini_gpio))
+		gpio_set_value_cansleep(pd->ini_gpio, 0);
+
+	if (gpio_is_valid(pd->resb_gpio))
+		gpio_set_value_cansleep(pd->resb_gpio, 0);
 
 	/* wait at least 5 vsyncs after disabling the LCD */
 
diff --git a/drivers/video/omap2/displays/panel-taal.c b/drivers/video/omap2/displays/panel-taal.c
index a32407a5735a..c4f78bda115a 100644
--- a/drivers/video/omap2/displays/panel-taal.c
+++ b/drivers/video/omap2/displays/panel-taal.c
@@ -33,7 +33,7 @@
 #include <linux/mutex.h>
 
 #include <video/omapdss.h>
-#include <video/omap-panel-nokia-dsi.h>
+#include <video/omap-panel-data.h>
 #include <video/mipi_display.h>
 
 /* DSI Virtual channel. Hardcoded for now. */
@@ -54,61 +54,6 @@ static int _taal_enable_te(struct omap_dss_device *dssdev, bool enable);
 
 static int taal_panel_reset(struct omap_dss_device *dssdev);
 
-/**
- * struct panel_config - panel configuration
- * @name: panel name
- * @type: panel type
- * @timings: panel resolution
- * @sleep: various panel specific delays, passed to msleep() if non-zero
- * @reset_sequence: reset sequence timings, passed to udelay() if non-zero
- * @regulators: array of panel regulators
- * @num_regulators: number of regulators in the array
- */
-struct panel_config {
-	const char *name;
-	int type;
-
-	struct omap_video_timings timings;
-
-	struct {
-		unsigned int sleep_in;
-		unsigned int sleep_out;
-		unsigned int hw_reset;
-		unsigned int enable_te;
-	} sleep;
-
-	struct {
-		unsigned int high;
-		unsigned int low;
-	} reset_sequence;
-
-};
-
-enum {
-	PANEL_TAAL,
-};
-
-static struct panel_config panel_configs[] = {
-	{
-		.name		= "taal",
-		.type		= PANEL_TAAL,
-		.timings	= {
-			.x_res		= 864,
-			.y_res		= 480,
-		},
-		.sleep		= {
-			.sleep_in	= 5,
-			.sleep_out	= 5,
-			.hw_reset	= 5,
-			.enable_te	= 100, /* possible panel bug */
-		},
-		.reset_sequence	= {
-			.high		= 10,
-			.low		= 10,
-		},
-	},
-};
-
 struct taal_data {
 	struct mutex lock;
 
@@ -121,9 +66,6 @@ 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;
@@ -134,8 +76,6 @@ struct taal_data {
 
 	/* runtime variables */
 	bool enabled;
-	u8 rotate;
-	bool mirror;
 
 	bool te_enabled;
 
@@ -221,8 +161,7 @@ static int taal_sleep_in(struct taal_data *td)
 
 	hw_guard_start(td, 120);
 
-	if (td->panel_config->sleep.sleep_in)
-		msleep(td->panel_config->sleep.sleep_in);
+	msleep(5);
 
 	return 0;
 }
@@ -239,8 +178,7 @@ static int taal_sleep_out(struct taal_data *td)
 
 	hw_guard_start(td, 120);
 
-	if (td->panel_config->sleep.sleep_out)
-		msleep(td->panel_config->sleep.sleep_out);
+	msleep(5);
 
 	return 0;
 }
@@ -262,49 +200,6 @@ static int taal_get_id(struct taal_data *td, u8 *id1, u8 *id2, u8 *id3)
 	return 0;
 }
 
-static int taal_set_addr_mode(struct taal_data *td, u8 rotate, bool mirror)
-{
-	int r;
-	u8 mode;
-	int b5, b6, b7;
-
-	r = taal_dcs_read_1(td, MIPI_DCS_GET_ADDRESS_MODE, &mode);
-	if (r)
-		return r;
-
-	switch (rotate) {
-	default:
-	case 0:
-		b7 = 0;
-		b6 = 0;
-		b5 = 0;
-		break;
-	case 1:
-		b7 = 0;
-		b6 = 1;
-		b5 = 1;
-		break;
-	case 2:
-		b7 = 1;
-		b6 = 1;
-		b5 = 0;
-		break;
-	case 3:
-		b7 = 1;
-		b6 = 0;
-		b5 = 1;
-		break;
-	}
-
-	if (mirror)
-		b6 = !b6;
-
-	mode &= ~((1<<7) | (1<<6) | (1<<5));
-	mode |= (b7 << 7) | (b6 << 6) | (b5 << 5);
-
-	return taal_dcs_write_1(td, MIPI_DCS_SET_ADDRESS_MODE, mode);
-}
-
 static int taal_set_update_window(struct taal_data *td,
 		u16 x, u16 y, u16 w, u16 h)
 {
@@ -515,15 +410,8 @@ static const struct backlight_ops taal_bl_ops = {
 static void taal_get_resolution(struct omap_dss_device *dssdev,
 		u16 *xres, u16 *yres)
 {
-	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-
-	if (td->rotate == 0 || td->rotate == 2) {
-		*xres = dssdev->panel.timings.x_res;
-		*yres = dssdev->panel.timings.y_res;
-	} else {
-		*yres = dssdev->panel.timings.x_res;
-		*xres = dssdev->panel.timings.y_res;
-	}
+	*xres = dssdev->panel.timings.x_res;
+	*yres = dssdev->panel.timings.y_res;
 }
 
 static ssize_t taal_num_errors_show(struct device *dev,
@@ -845,17 +733,14 @@ static void taal_hw_reset(struct omap_dss_device *dssdev)
 		return;
 
 	gpio_set_value(td->reset_gpio, 1);
-	if (td->panel_config->reset_sequence.high)
-		udelay(td->panel_config->reset_sequence.high);
+	udelay(10);
 	/* reset the panel */
 	gpio_set_value(td->reset_gpio, 0);
 	/* assert reset */
-	if (td->panel_config->reset_sequence.low)
-		udelay(td->panel_config->reset_sequence.low);
+	udelay(10);
 	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);
+	msleep(5);
 }
 
 static void taal_probe_pdata(struct taal_data *td,
@@ -881,8 +766,7 @@ static int taal_probe(struct omap_dss_device *dssdev)
 	struct backlight_properties props;
 	struct taal_data *td;
 	struct backlight_device *bldev = NULL;
-	int r, i;
-	const char *panel_name;
+	int r;
 
 	dev_dbg(&dssdev->dev, "probe\n");
 
@@ -897,26 +781,13 @@ static int taal_probe(struct omap_dss_device *dssdev)
 		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_name, panel_configs[i].name) == 0) {
-			td->panel_config = &panel_configs[i];
-			break;
-		}
-	}
-
-	if (!td->panel_config)
-		return -EINVAL;
-
-	dssdev->panel.timings = td->panel_config->timings;
+	dssdev->panel.timings.x_res = 864;
+	dssdev->panel.timings.y_res = 480;
+	dssdev->panel.timings.pixel_clock = DIV_ROUND_UP(864 * 480 * 60, 1000);
 	dssdev->panel.dsi_pix_fmt = OMAP_DSS_DSI_FMT_RGB888;
 	dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE |
 		OMAP_DSS_DISPLAY_CAP_TEAR_ELIM;
@@ -1049,6 +920,15 @@ static int taal_power_on(struct omap_dss_device *dssdev)
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
 	u8 id1, id2, id3;
 	int r;
+	struct omap_dss_dsi_config dsi_config = {
+		.mode = OMAP_DSS_DSI_CMD_MODE,
+		.pixel_format = OMAP_DSS_DSI_FMT_RGB888,
+		.timings = &dssdev->panel.timings,
+		.hs_clk_min = 150000000,
+		.hs_clk_max = 300000000,
+		.lp_clk_min = 7000000,
+		.lp_clk_max = 10000000,
+	};
 
 	r = omapdss_dsi_configure_pins(dssdev, &td->pin_config);
 	if (r) {
@@ -1056,14 +936,9 @@ static int taal_power_on(struct omap_dss_device *dssdev)
 		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);
+	r = omapdss_dsi_set_config(dssdev, &dsi_config);
 	if (r) {
-		dev_err(&dssdev->dev, "failed to set HS and LP clocks\n");
+		dev_err(&dssdev->dev, "failed to configure DSI\n");
 		goto err0;
 	}
 
@@ -1086,8 +961,7 @@ static int taal_power_on(struct omap_dss_device *dssdev)
 		goto err;
 
 	/* on early Taal revisions CABC is broken */
-	if (td->panel_config->type == PANEL_TAAL &&
-		(id2 == 0x00 || id2 == 0xff || id2 == 0x81))
+	if (id2 == 0x00 || id2 == 0xff || id2 == 0x81)
 		td->cabc_broken = true;
 
 	r = taal_dcs_write_1(td, DCS_BRIGHTNESS, 0xff);
@@ -1104,10 +978,6 @@ static int taal_power_on(struct omap_dss_device *dssdev)
 	if (r)
 		goto err;
 
-	r = taal_set_addr_mode(td, td->rotate, td->mirror);
-	if (r)
-		goto err;
-
 	if (!td->cabc_broken) {
 		r = taal_dcs_write_1(td, DCS_WRITE_CABC, td->cabc_mode);
 		if (r)
@@ -1129,8 +999,8 @@ static int taal_power_on(struct omap_dss_device *dssdev)
 	td->enabled = 1;
 
 	if (!td->intro_printed) {
-		dev_info(&dssdev->dev, "%s panel revision %02x.%02x.%02x\n",
-			td->panel_config->name, id1, id2, id3);
+		dev_info(&dssdev->dev, "panel revision %02x.%02x.%02x\n",
+			id1, id2, id3);
 		if (td->cabc_broken)
 			dev_info(&dssdev->dev,
 					"old Taal version, CABC disabled\n");
@@ -1311,8 +1181,8 @@ static int taal_update(struct omap_dss_device *dssdev,
 
 	/* XXX no need to send this every frame, but dsi break if not done */
 	r = taal_set_update_window(td, 0, 0,
-			td->panel_config->timings.x_res,
-			td->panel_config->timings.y_res);
+			dssdev->panel.timings.x_res,
+			dssdev->panel.timings.y_res);
 	if (r)
 		goto err;
 
@@ -1365,8 +1235,8 @@ static int _taal_enable_te(struct omap_dss_device *dssdev, bool enable)
 	if (!gpio_is_valid(td->ext_te_gpio))
 		omapdss_dsi_enable_te(dssdev, enable);
 
-	if (td->panel_config->sleep.enable_te)
-		msleep(td->panel_config->sleep.enable_te);
+	/* possible panel bug */
+	msleep(100);
 
 	return r;
 }
@@ -1419,112 +1289,6 @@ static int taal_get_te(struct omap_dss_device *dssdev)
 	return r;
 }
 
-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);
-
-	mutex_lock(&td->lock);
-
-	if (td->rotate == rotate)
-		goto end;
-
-	dsi_bus_lock(dssdev);
-
-	if (td->enabled) {
-		r = taal_wake_up(dssdev);
-		if (r)
-			goto err;
-
-		r = taal_set_addr_mode(td, rotate, td->mirror);
-		if (r)
-			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);
-end:
-	mutex_unlock(&td->lock);
-	return 0;
-err:
-	dsi_bus_unlock(dssdev);
-	mutex_unlock(&td->lock);
-	return r;
-}
-
-static u8 taal_get_rotate(struct omap_dss_device *dssdev)
-{
-	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-	int r;
-
-	mutex_lock(&td->lock);
-	r = td->rotate;
-	mutex_unlock(&td->lock);
-
-	return r;
-}
-
-static int taal_mirror(struct omap_dss_device *dssdev, bool enable)
-{
-	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-	int r;
-
-	dev_dbg(&dssdev->dev, "mirror %d\n", enable);
-
-	mutex_lock(&td->lock);
-
-	if (td->mirror == enable)
-		goto end;
-
-	dsi_bus_lock(dssdev);
-	if (td->enabled) {
-		r = taal_wake_up(dssdev);
-		if (r)
-			goto err;
-
-		r = taal_set_addr_mode(td, td->rotate, enable);
-		if (r)
-			goto err;
-	}
-
-	td->mirror = enable;
-
-	dsi_bus_unlock(dssdev);
-end:
-	mutex_unlock(&td->lock);
-	return 0;
-err:
-	dsi_bus_unlock(dssdev);
-	mutex_unlock(&td->lock);
-	return r;
-}
-
-static bool taal_get_mirror(struct omap_dss_device *dssdev)
-{
-	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-	int r;
-
-	mutex_lock(&td->lock);
-	r = td->mirror;
-	mutex_unlock(&td->lock);
-
-	return r;
-}
-
 static int taal_run_test(struct omap_dss_device *dssdev, int test_num)
 {
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
@@ -1758,10 +1522,6 @@ static struct omap_dss_driver taal_driver = {
 	.enable_te	= taal_enable_te,
 	.get_te		= taal_get_te,
 
-	.set_rotate	= taal_rotate,
-	.get_rotate	= taal_get_rotate,
-	.set_mirror	= taal_mirror,
-	.get_mirror	= taal_get_mirror,
 	.run_test	= taal_run_test,
 	.memory_read	= taal_memory_read,
 
diff --git a/drivers/video/omap2/displays/panel-tfp410.c b/drivers/video/omap2/displays/panel-tfp410.c
index 8281baafe1ef..46039c4bf1ed 100644
--- a/drivers/video/omap2/displays/panel-tfp410.c
+++ b/drivers/video/omap2/displays/panel-tfp410.c
@@ -24,7 +24,7 @@
 #include <linux/gpio.h>
 #include <drm/drm_edid.h>
 
-#include <video/omap-panel-tfp410.h>
+#include <video/omap-panel-data.h>
 
 static const struct omap_video_timings tfp410_default_timings = {
 	.x_res		= 640,
@@ -135,7 +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);
-			return -EINVAL;
+			return -EPROBE_DEFER;
 		}
 
 		ddata->i2c_adapter = adapter;
diff --git a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
index 048c98381ef6..abf2bc4a18ab 100644
--- a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
+++ b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
@@ -18,6 +18,7 @@
 #include <linux/slab.h>
 
 #include <video/omapdss.h>
+#include <video/omap-panel-data.h>
 
 #define TPO_R02_MODE(x)		((x) & 7)
 #define TPO_R02_MODE_800x480	7
@@ -278,9 +279,14 @@ static const struct omap_video_timings tpo_td043_timings = {
 	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 };
 
+static inline struct panel_tpo_td043_data
+*get_panel_data(const struct omap_dss_device *dssdev)
+{
+	return (struct panel_tpo_td043_data *) dssdev->data;
+}
+
 static int tpo_td043_power_on(struct tpo_td043_device *tpo_td043)
 {
-	int nreset_gpio = tpo_td043->nreset_gpio;
 	int r;
 
 	if (tpo_td043->powered_on)
@@ -293,8 +299,8 @@ static int tpo_td043_power_on(struct tpo_td043_device *tpo_td043)
 	/* wait for panel to stabilize */
 	msleep(160);
 
-	if (gpio_is_valid(nreset_gpio))
-		gpio_set_value(nreset_gpio, 1);
+	if (gpio_is_valid(tpo_td043->nreset_gpio))
+		gpio_set_value(tpo_td043->nreset_gpio, 1);
 
 	tpo_td043_write(tpo_td043->spi, 2,
 			TPO_R02_MODE(tpo_td043->mode) | TPO_R02_NCLK_RISING);
@@ -311,16 +317,14 @@ static int tpo_td043_power_on(struct tpo_td043_device *tpo_td043)
 
 static void tpo_td043_power_off(struct tpo_td043_device *tpo_td043)
 {
-	int nreset_gpio = tpo_td043->nreset_gpio;
-
 	if (!tpo_td043->powered_on)
 		return;
 
 	tpo_td043_write(tpo_td043->spi, 3,
 			TPO_R03_VAL_STANDBY | TPO_R03_EN_PWM);
 
-	if (gpio_is_valid(nreset_gpio))
-		gpio_set_value(nreset_gpio, 0);
+	if (gpio_is_valid(tpo_td043->nreset_gpio))
+		gpio_set_value(tpo_td043->nreset_gpio, 0);
 
 	/* wait for at least 2 vsyncs before cutting off power */
 	msleep(50);
@@ -347,12 +351,6 @@ static int tpo_td043_enable_dss(struct omap_dss_device *dssdev)
 	if (r)
 		goto err0;
 
-	if (dssdev->platform_enable) {
-		r = dssdev->platform_enable(dssdev);
-		if (r)
-			goto err1;
-	}
-
 	/*
 	 * If we are resuming from system suspend, SPI clocks might not be
 	 * enabled yet, so we'll program the LCD from SPI PM resume callback.
@@ -379,9 +377,6 @@ static void tpo_td043_disable_dss(struct omap_dss_device *dssdev)
 	if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
 		return;
 
-	if (dssdev->platform_disable)
-		dssdev->platform_disable(dssdev);
-
 	omapdss_dpi_display_disable(dssdev);
 
 	if (!tpo_td043->spi_suspended)
@@ -407,7 +402,7 @@ static void tpo_td043_disable(struct omap_dss_device *dssdev)
 static int tpo_td043_probe(struct omap_dss_device *dssdev)
 {
 	struct tpo_td043_device *tpo_td043 = g_tpo_td043;
-	int nreset_gpio = dssdev->reset_gpio;
+	struct panel_tpo_td043_data *pdata = get_panel_data(dssdev);
 	int ret = 0;
 
 	dev_dbg(&dssdev->dev, "probe\n");
@@ -417,6 +412,11 @@ static int tpo_td043_probe(struct omap_dss_device *dssdev)
 		return -ENODEV;
 	}
 
+	if (!pdata)
+		return -EINVAL;
+
+	tpo_td043->nreset_gpio = pdata->nreset_gpio;
+
 	dssdev->panel.timings = tpo_td043_timings;
 	dssdev->ctrl.pixel_size = 24;
 
@@ -430,9 +430,10 @@ static int tpo_td043_probe(struct omap_dss_device *dssdev)
 		goto fail_regulator;
 	}
 
-	if (gpio_is_valid(nreset_gpio)) {
-		ret = gpio_request_one(nreset_gpio, GPIOF_OUT_INIT_LOW,
-					"lcd reset");
+	if (gpio_is_valid(tpo_td043->nreset_gpio)) {
+		ret = devm_gpio_request_one(&dssdev->dev,
+				tpo_td043->nreset_gpio, GPIOF_OUT_INIT_LOW,
+				"lcd reset");
 		if (ret < 0) {
 			dev_err(&dssdev->dev, "couldn't request reset GPIO\n");
 			goto fail_gpio_req;
@@ -457,14 +458,11 @@ fail_regulator:
 static void tpo_td043_remove(struct omap_dss_device *dssdev)
 {
 	struct tpo_td043_device *tpo_td043 = dev_get_drvdata(&dssdev->dev);
-	int nreset_gpio = dssdev->reset_gpio;
 
 	dev_dbg(&dssdev->dev, "remove\n");
 
 	sysfs_remove_group(&dssdev->dev.kobj, &tpo_td043_attr_group);
 	regulator_put(tpo_td043->vcc_reg);
-	if (gpio_is_valid(nreset_gpio))
-		gpio_free(nreset_gpio);
 }
 
 static void tpo_td043_set_timings(struct omap_dss_device *dssdev,
@@ -527,7 +525,6 @@ static int tpo_td043_spi_probe(struct spi_device *spi)
 		return -ENOMEM;
 
 	tpo_td043->spi = spi;
-	tpo_td043->nreset_gpio = dssdev->reset_gpio;
 	dev_set_drvdata(&spi->dev, tpo_td043);
 	g_tpo_td043 = tpo_td043;
 
diff --git a/drivers/video/omap2/dss/apply.c b/drivers/video/omap2/dss/apply.c
index d446bdfc4c82..a4b356a9780d 100644
--- a/drivers/video/omap2/dss/apply.c
+++ b/drivers/video/omap2/dss/apply.c
@@ -435,20 +435,27 @@ static inline struct omap_dss_device *dss_mgr_get_device(struct omap_overlay_man
 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;
 
+	if (mgr->output == NULL)
+		return -ENODEV;
+
 	r = dispc_runtime_get();
 	if (r)
 		return r;
 
-	if (dssdev->type == OMAP_DISPLAY_TYPE_VENC)
+	switch (mgr->output->id) {
+	case OMAP_DSS_OUTPUT_VENC:
 		irq = DISPC_IRQ_EVSYNC_ODD;
-	else if (dssdev->type == OMAP_DISPLAY_TYPE_HDMI)
+		break;
+	case OMAP_DSS_OUTPUT_HDMI:
 		irq = DISPC_IRQ_EVSYNC_EVEN;
-	else
+		break;
+	default:
 		irq = dispc_mgr_get_vsync_irq(mgr->id);
+		break;
+	}
 
 	r = omap_dispc_wait_for_irq_interruptible_timeout(irq, timeout);
 
diff --git a/drivers/video/omap2/dss/core.c b/drivers/video/omap2/dss/core.c
index f8779d4750ba..60cc6fee6548 100644
--- a/drivers/video/omap2/dss/core.c
+++ b/drivers/video/omap2/dss/core.c
@@ -181,10 +181,7 @@ int dss_debugfs_create_file(const char *name, void (*write)(struct seq_file *))
 	d = debugfs_create_file(name, S_IRUGO, dss_debugfs_dir,
 			write, &dss_debug_fops);
 
-	if (IS_ERR(d))
-		return PTR_ERR(d);
-
-	return 0;
+	return PTR_RET(d);
 }
 #else /* CONFIG_OMAP2_DSS_DEBUGFS */
 static inline int dss_initialize_debugfs(void)
diff --git a/drivers/video/omap2/dss/dispc.c b/drivers/video/omap2/dss/dispc.c
index 05ff2b91d9e8..b33b0169bb3b 100644
--- a/drivers/video/omap2/dss/dispc.c
+++ b/drivers/video/omap2/dss/dispc.c
@@ -69,6 +69,8 @@ struct dispc_features {
 	u8 mgr_height_start;
 	u16 mgr_width_max;
 	u16 mgr_height_max;
+	unsigned long max_lcd_pclk;
+	unsigned long max_tv_pclk;
 	int (*calc_scaling) (unsigned long pclk, unsigned long lclk,
 		const struct omap_video_timings *mgr_timings,
 		u16 width, u16 height, u16 out_width, u16 out_height,
@@ -85,6 +87,9 @@ struct dispc_features {
 
 	/* no DISPC_IRQ_FRAMEDONETV on this SoC */
 	bool no_framedone_tv:1;
+
+	/* revert to the OMAP4 mechanism of DISPC Smart Standby operation */
+	bool mstandby_workaround:1;
 };
 
 #define DISPC_MAX_NR_FIFOS 5
@@ -97,6 +102,8 @@ static struct {
 
 	int irq;
 
+	unsigned long core_clk_rate;
+
 	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];
@@ -1584,6 +1591,7 @@ static void dispc_ovl_set_scaling(enum omap_plane plane,
 }
 
 static void dispc_ovl_set_rotation_attrs(enum omap_plane plane, u8 rotation,
+		enum omap_dss_rotation_type rotation_type,
 		bool mirroring, enum omap_color_mode color_mode)
 {
 	bool row_repeat = false;
@@ -1634,6 +1642,15 @@ static void dispc_ovl_set_rotation_attrs(enum omap_plane plane, u8 rotation,
 	if (dss_has_feature(FEAT_ROWREPEATENABLE))
 		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane),
 			row_repeat ? 1 : 0, 18, 18);
+
+	if (color_mode == OMAP_DSS_COLOR_NV12) {
+		bool doublestride = (rotation_type == OMAP_DSS_ROT_TILER) &&
+					(rotation == OMAP_DSS_ROT_0 ||
+					rotation == OMAP_DSS_ROT_180);
+		/* DOUBLESTRIDE */
+		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), doublestride, 22, 22);
+	}
+
 }
 
 static int color_mode_to_bpp(enum omap_color_mode color_mode)
@@ -2512,7 +2529,8 @@ static int dispc_ovl_setup_common(enum omap_plane plane,
 		dispc_ovl_set_vid_color_conv(plane, cconv);
 	}
 
-	dispc_ovl_set_rotation_attrs(plane, rotation, mirror, color_mode);
+	dispc_ovl_set_rotation_attrs(plane, rotation, rotation_type, mirror,
+			color_mode);
 
 	dispc_ovl_set_zorder(plane, caps, zorder);
 	dispc_ovl_set_pre_mult_alpha(plane, caps, pre_mult_alpha);
@@ -2823,6 +2841,15 @@ static bool _dispc_lcd_timings_ok(int hsw, int hfp, int hbp,
 	return true;
 }
 
+static bool _dispc_mgr_pclk_ok(enum omap_channel channel,
+		unsigned long pclk)
+{
+	if (dss_mgr_is_lcd(channel))
+		return pclk <= dispc.feat->max_lcd_pclk ? true : false;
+	else
+		return pclk <= dispc.feat->max_tv_pclk ? true : false;
+}
+
 bool dispc_mgr_timings_ok(enum omap_channel channel,
 		const struct omap_video_timings *timings)
 {
@@ -2830,11 +2857,13 @@ bool dispc_mgr_timings_ok(enum omap_channel channel,
 
 	timings_ok = _dispc_mgr_size_ok(timings->x_res, timings->y_res);
 
-	if (dss_mgr_is_lcd(channel))
-		timings_ok =  timings_ok && _dispc_lcd_timings_ok(timings->hsw,
-						timings->hfp, timings->hbp,
-						timings->vsw, timings->vfp,
-						timings->vbp);
+	timings_ok &= _dispc_mgr_pclk_ok(channel, timings->pixel_clock * 1000);
+
+	if (dss_mgr_is_lcd(channel)) {
+		timings_ok &= _dispc_lcd_timings_ok(timings->hsw, timings->hfp,
+				timings->hbp, timings->vsw, timings->vfp,
+				timings->vbp);
+	}
 
 	return timings_ok;
 }
@@ -2951,6 +2980,10 @@ static void dispc_mgr_set_lcd_divisor(enum omap_channel channel, u16 lck_div,
 
 	dispc_write_reg(DISPC_DIVISORo(channel),
 			FLD_VAL(lck_div, 23, 16) | FLD_VAL(pck_div, 7, 0));
+
+	if (dss_has_feature(FEAT_CORE_CLK_DIV) == false &&
+			channel == OMAP_DSS_CHANNEL_LCD)
+		dispc.core_clk_rate = dispc_fclk_rate() / lck_div;
 }
 
 static void dispc_mgr_get_lcd_divisor(enum omap_channel channel, int *lck_div,
@@ -3056,15 +3089,7 @@ unsigned long dispc_mgr_pclk_rate(enum omap_channel channel)
 
 unsigned long dispc_core_clk_rate(void)
 {
-	int lcd;
-	unsigned long fclk = dispc_fclk_rate();
-
-	if (dss_has_feature(FEAT_CORE_CLK_DIV))
-		lcd = REG_GET(DISPC_DIVISOR, 23, 16);
-	else
-		lcd = REG_GET(DISPC_DIVISORo(OMAP_DSS_CHANNEL_LCD), 23, 16);
-
-	return fclk / lcd;
+	return dispc.core_clk_rate;
 }
 
 static unsigned long dispc_plane_pclk_rate(enum omap_plane plane)
@@ -3313,67 +3338,79 @@ static void dispc_dump_regs(struct seq_file *s)
 #undef DUMPREG
 }
 
-/* with fck as input clock rate, find dispc dividers that produce req_pck */
-void dispc_find_clk_divs(unsigned long req_pck, unsigned long fck,
+/* calculate clock rates using dividers in cinfo */
+int dispc_calc_clock_rates(unsigned long dispc_fclk_rate,
 		struct dispc_clock_info *cinfo)
 {
-	u16 pcd_min, pcd_max;
-	unsigned long best_pck;
-	u16 best_ld, cur_ld;
-	u16 best_pd, cur_pd;
+	if (cinfo->lck_div > 255 || cinfo->lck_div == 0)
+		return -EINVAL;
+	if (cinfo->pck_div < 1 || cinfo->pck_div > 255)
+		return -EINVAL;
 
-	pcd_min = dss_feat_get_param_min(FEAT_PARAM_DSS_PCD);
-	pcd_max = dss_feat_get_param_max(FEAT_PARAM_DSS_PCD);
+	cinfo->lck = dispc_fclk_rate / cinfo->lck_div;
+	cinfo->pck = cinfo->lck / cinfo->pck_div;
 
-	best_pck = 0;
-	best_ld = 0;
-	best_pd = 0;
+	return 0;
+}
 
-	for (cur_ld = 1; cur_ld <= 255; ++cur_ld) {
-		unsigned long lck = fck / cur_ld;
+bool dispc_div_calc(unsigned long dispc,
+		unsigned long pck_min, unsigned long pck_max,
+		dispc_div_calc_func func, void *data)
+{
+	int lckd, lckd_start, lckd_stop;
+	int pckd, pckd_start, pckd_stop;
+	unsigned long pck, lck;
+	unsigned long lck_max;
+	unsigned long pckd_hw_min, pckd_hw_max;
+	unsigned min_fck_per_pck;
+	unsigned long fck;
 
-		for (cur_pd = pcd_min; cur_pd <= pcd_max; ++cur_pd) {
-			unsigned long pck = lck / cur_pd;
-			long old_delta = abs(best_pck - req_pck);
-			long new_delta = abs(pck - req_pck);
+#ifdef CONFIG_OMAP2_DSS_MIN_FCK_PER_PCK
+	min_fck_per_pck = CONFIG_OMAP2_DSS_MIN_FCK_PER_PCK;
+#else
+	min_fck_per_pck = 0;
+#endif
 
-			if (best_pck == 0 || new_delta < old_delta) {
-				best_pck = pck;
-				best_ld = cur_ld;
-				best_pd = cur_pd;
+	pckd_hw_min = dss_feat_get_param_min(FEAT_PARAM_DSS_PCD);
+	pckd_hw_max = dss_feat_get_param_max(FEAT_PARAM_DSS_PCD);
 
-				if (pck == req_pck)
-					goto found;
-			}
+	lck_max = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
 
-			if (pck < req_pck)
-				break;
-		}
+	pck_min = pck_min ? pck_min : 1;
+	pck_max = pck_max ? pck_max : ULONG_MAX;
 
-		if (lck / pcd_min < req_pck)
-			break;
-	}
+	lckd_start = max(DIV_ROUND_UP(dispc, lck_max), 1ul);
+	lckd_stop = min(dispc / pck_min, 255ul);
 
-found:
-	cinfo->lck_div = best_ld;
-	cinfo->pck_div = best_pd;
-	cinfo->lck = fck / cinfo->lck_div;
-	cinfo->pck = cinfo->lck / cinfo->pck_div;
-}
+	for (lckd = lckd_start; lckd <= lckd_stop; ++lckd) {
+		lck = dispc / lckd;
 
-/* calculate clock rates using dividers in cinfo */
-int dispc_calc_clock_rates(unsigned long dispc_fclk_rate,
-		struct dispc_clock_info *cinfo)
-{
-	if (cinfo->lck_div > 255 || cinfo->lck_div == 0)
-		return -EINVAL;
-	if (cinfo->pck_div < 1 || cinfo->pck_div > 255)
-		return -EINVAL;
+		pckd_start = max(DIV_ROUND_UP(lck, pck_max), pckd_hw_min);
+		pckd_stop = min(lck / pck_min, pckd_hw_max);
 
-	cinfo->lck = dispc_fclk_rate / cinfo->lck_div;
-	cinfo->pck = cinfo->lck / cinfo->pck_div;
+		for (pckd = pckd_start; pckd <= pckd_stop; ++pckd) {
+			pck = lck / pckd;
 
-	return 0;
+			/*
+			 * For OMAP2/3 the DISPC fclk is the same as LCD's logic
+			 * clock, which means we're configuring DISPC fclk here
+			 * also. Thus we need to use the calculated lck. For
+			 * OMAP4+ the DISPC fclk is a separate clock.
+			 */
+			if (dss_has_feature(FEAT_CORE_CLK_DIV))
+				fck = dispc_core_clk_rate();
+			else
+				fck = lck;
+
+			if (fck < pck * min_fck_per_pck)
+				continue;
+
+			if (func(lckd, pckd, lck, pck, data))
+				return true;
+		}
+	}
+
+	return false;
 }
 
 void dispc_mgr_set_clock_div(enum omap_channel channel,
@@ -3451,6 +3488,8 @@ static void _omap_dispc_initial_config(void)
 		l = FLD_MOD(l, 1, 0, 0);
 		l = FLD_MOD(l, 1, 23, 16);
 		dispc_write_reg(DISPC_DIVISOR, l);
+
+		dispc.core_clk_rate = dispc_fclk_rate();
 	}
 
 	/* FUNCGATED */
@@ -3466,6 +3505,9 @@ static void _omap_dispc_initial_config(void)
 	dispc_configure_burst_sizes();
 
 	dispc_ovl_enable_zorder_planes();
+
+	if (dispc.feat->mstandby_workaround)
+		REG_FLD_MOD(DISPC_MSTANDBY_CTRL, 1, 0, 0);
 }
 
 static const struct dispc_features omap24xx_dispc_feats __initconst = {
@@ -3479,6 +3521,7 @@ static const struct dispc_features omap24xx_dispc_feats __initconst = {
 	.mgr_height_start	=	26,
 	.mgr_width_max		=	2048,
 	.mgr_height_max		=	2048,
+	.max_lcd_pclk		=	66500000,
 	.calc_scaling		=	dispc_ovl_calc_scaling_24xx,
 	.calc_core_clk		=	calc_core_clk_24xx,
 	.num_fifos		=	3,
@@ -3496,6 +3539,8 @@ static const struct dispc_features omap34xx_rev1_0_dispc_feats __initconst = {
 	.mgr_height_start	=	26,
 	.mgr_width_max		=	2048,
 	.mgr_height_max		=	2048,
+	.max_lcd_pclk		=	173000000,
+	.max_tv_pclk		=	59000000,
 	.calc_scaling		=	dispc_ovl_calc_scaling_34xx,
 	.calc_core_clk		=	calc_core_clk_34xx,
 	.num_fifos		=	3,
@@ -3513,6 +3558,8 @@ static const struct dispc_features omap34xx_rev3_0_dispc_feats __initconst = {
 	.mgr_height_start	=	26,
 	.mgr_width_max		=	2048,
 	.mgr_height_max		=	2048,
+	.max_lcd_pclk		=	173000000,
+	.max_tv_pclk		=	59000000,
 	.calc_scaling		=	dispc_ovl_calc_scaling_34xx,
 	.calc_core_clk		=	calc_core_clk_34xx,
 	.num_fifos		=	3,
@@ -3530,6 +3577,8 @@ static const struct dispc_features omap44xx_dispc_feats __initconst = {
 	.mgr_height_start	=	26,
 	.mgr_width_max		=	2048,
 	.mgr_height_max		=	2048,
+	.max_lcd_pclk		=	170000000,
+	.max_tv_pclk		=	185625000,
 	.calc_scaling		=	dispc_ovl_calc_scaling_44xx,
 	.calc_core_clk		=	calc_core_clk_44xx,
 	.num_fifos		=	5,
@@ -3547,10 +3596,13 @@ static const struct dispc_features omap54xx_dispc_feats __initconst = {
 	.mgr_height_start	=	27,
 	.mgr_width_max		=	4096,
 	.mgr_height_max		=	4096,
+	.max_lcd_pclk		=	170000000,
+	.max_tv_pclk		=	186000000,
 	.calc_scaling		=	dispc_ovl_calc_scaling_44xx,
 	.calc_core_clk		=	calc_core_clk_44xx,
 	.num_fifos		=	5,
 	.gfx_fifo_workaround	=	true,
+	.mstandby_workaround	=	true,
 };
 
 static int __init dispc_init_features(struct platform_device *pdev)
diff --git a/drivers/video/omap2/dss/dispc.h b/drivers/video/omap2/dss/dispc.h
index 222363c6e623..de4863d21ab7 100644
--- a/drivers/video/omap2/dss/dispc.h
+++ b/drivers/video/omap2/dss/dispc.h
@@ -39,6 +39,7 @@
 #define DISPC_GLOBAL_BUFFER		0x0800
 #define DISPC_CONTROL3                  0x0848
 #define DISPC_CONFIG3                   0x084C
+#define DISPC_MSTANDBY_CTRL		0x0858
 
 /* DISPC overlay registers */
 #define DISPC_OVL_BA0(n)		(DISPC_OVL_BASE(n) + \
diff --git a/drivers/video/omap2/dss/dpi.c b/drivers/video/omap2/dss/dpi.c
index 4af136a04e53..757b57f7275a 100644
--- a/drivers/video/omap2/dss/dpi.c
+++ b/drivers/video/omap2/dss/dpi.c
@@ -63,15 +63,29 @@ static struct platform_device *dpi_get_dsidev(enum omap_channel channel)
 	case OMAPDSS_VER_OMAP3630:
 	case OMAPDSS_VER_AM35xx:
 		return NULL;
-	default:
-		break;
-	}
 
-	switch (channel) {
-	case OMAP_DSS_CHANNEL_LCD:
-		return dsi_get_dsidev_from_id(0);
-	case OMAP_DSS_CHANNEL_LCD2:
-		return dsi_get_dsidev_from_id(1);
+	case OMAPDSS_VER_OMAP4430_ES1:
+	case OMAPDSS_VER_OMAP4430_ES2:
+	case OMAPDSS_VER_OMAP4:
+		switch (channel) {
+		case OMAP_DSS_CHANNEL_LCD:
+			return dsi_get_dsidev_from_id(0);
+		case OMAP_DSS_CHANNEL_LCD2:
+			return dsi_get_dsidev_from_id(1);
+		default:
+			return NULL;
+		}
+
+	case OMAPDSS_VER_OMAP5:
+		switch (channel) {
+		case OMAP_DSS_CHANNEL_LCD:
+			return dsi_get_dsidev_from_id(0);
+		case OMAP_DSS_CHANNEL_LCD3:
+			return dsi_get_dsidev_from_id(1);
+		default:
+			return NULL;
+		}
+
 	default:
 		return NULL;
 	}
@@ -91,75 +105,211 @@ static enum omap_dss_clk_source dpi_get_alt_clk_src(enum omap_channel channel)
 	}
 }
 
-static int dpi_set_dsi_clk(struct omap_dss_device *dssdev,
+struct dpi_clk_calc_ctx {
+	struct platform_device *dsidev;
+
+	/* inputs */
+
+	unsigned long pck_min, pck_max;
+
+	/* outputs */
+
+	struct dsi_clock_info dsi_cinfo;
+	struct dss_clock_info dss_cinfo;
+	struct dispc_clock_info dispc_cinfo;
+};
+
+static bool dpi_calc_dispc_cb(int lckd, int pckd, unsigned long lck,
+		unsigned long pck, void *data)
+{
+	struct dpi_clk_calc_ctx *ctx = data;
+
+	/*
+	 * Odd dividers give us uneven duty cycle, causing problem when level
+	 * shifted. So skip all odd dividers when the pixel clock is on the
+	 * higher side.
+	 */
+	if (ctx->pck_min >= 1000000) {
+		if (lckd > 1 && lckd % 2 != 0)
+			return false;
+
+		if (pckd > 1 && pckd % 2 != 0)
+			return false;
+	}
+
+	ctx->dispc_cinfo.lck_div = lckd;
+	ctx->dispc_cinfo.pck_div = pckd;
+	ctx->dispc_cinfo.lck = lck;
+	ctx->dispc_cinfo.pck = pck;
+
+	return true;
+}
+
+
+static bool dpi_calc_hsdiv_cb(int regm_dispc, unsigned long dispc,
+		void *data)
+{
+	struct dpi_clk_calc_ctx *ctx = data;
+
+	/*
+	 * Odd dividers give us uneven duty cycle, causing problem when level
+	 * shifted. So skip all odd dividers when the pixel clock is on the
+	 * higher side.
+	 */
+	if (regm_dispc > 1 && regm_dispc % 2 != 0 && ctx->pck_min >= 1000000)
+		return false;
+
+	ctx->dsi_cinfo.regm_dispc = regm_dispc;
+	ctx->dsi_cinfo.dsi_pll_hsdiv_dispc_clk = dispc;
+
+	return dispc_div_calc(dispc, ctx->pck_min, ctx->pck_max,
+			dpi_calc_dispc_cb, ctx);
+}
+
+
+static bool dpi_calc_pll_cb(int regn, int regm, unsigned long fint,
+		unsigned long pll,
+		void *data)
+{
+	struct dpi_clk_calc_ctx *ctx = data;
+
+	ctx->dsi_cinfo.regn = regn;
+	ctx->dsi_cinfo.regm = regm;
+	ctx->dsi_cinfo.fint = fint;
+	ctx->dsi_cinfo.clkin4ddr = pll;
+
+	return dsi_hsdiv_calc(ctx->dsidev, pll, ctx->pck_min,
+			dpi_calc_hsdiv_cb, ctx);
+}
+
+static bool dpi_calc_dss_cb(int fckd, unsigned long fck, void *data)
+{
+	struct dpi_clk_calc_ctx *ctx = data;
+
+	ctx->dss_cinfo.fck = fck;
+	ctx->dss_cinfo.fck_div = fckd;
+
+	return dispc_div_calc(fck, ctx->pck_min, ctx->pck_max,
+			dpi_calc_dispc_cb, ctx);
+}
+
+static bool dpi_dsi_clk_calc(unsigned long pck, struct dpi_clk_calc_ctx *ctx)
+{
+	unsigned long clkin;
+	unsigned long pll_min, pll_max;
+
+	clkin = dsi_get_pll_clkin(dpi.dsidev);
+
+	memset(ctx, 0, sizeof(*ctx));
+	ctx->dsidev = dpi.dsidev;
+	ctx->pck_min = pck - 1000;
+	ctx->pck_max = pck + 1000;
+	ctx->dsi_cinfo.clkin = clkin;
+
+	pll_min = 0;
+	pll_max = 0;
+
+	return dsi_pll_calc(dpi.dsidev, clkin,
+			pll_min, pll_max,
+			dpi_calc_pll_cb, ctx);
+}
+
+static bool dpi_dss_clk_calc(unsigned long pck, struct dpi_clk_calc_ctx *ctx)
+{
+	int i;
+
+	/*
+	 * DSS fck gives us very few possibilities, so finding a good pixel
+	 * clock may not be possible. We try multiple times to find the clock,
+	 * each time widening the pixel clock range we look for, up to
+	 * +/- ~15MHz.
+	 */
+
+	for (i = 0; i < 25; ++i) {
+		bool ok;
+
+		memset(ctx, 0, sizeof(*ctx));
+		if (pck > 1000 * i * i * i)
+			ctx->pck_min = max(pck - 1000 * i * i * i, 0lu);
+		else
+			ctx->pck_min = 0;
+		ctx->pck_max = pck + 1000 * i * i * i;
+
+		ok = dss_div_calc(ctx->pck_min, dpi_calc_dss_cb, ctx);
+		if (ok)
+			return ok;
+	}
+
+	return false;
+}
+
+
+
+static int dpi_set_dsi_clk(enum omap_channel channel,
 		unsigned long pck_req, unsigned long *fck, int *lck_div,
 		int *pck_div)
 {
-	struct omap_overlay_manager *mgr = dssdev->output->manager;
-	struct dsi_clock_info dsi_cinfo;
-	struct dispc_clock_info dispc_cinfo;
+	struct dpi_clk_calc_ctx ctx;
 	int r;
+	bool ok;
 
-	r = dsi_pll_calc_clock_div_pck(dpi.dsidev, pck_req, &dsi_cinfo,
-			&dispc_cinfo);
-	if (r)
-		return r;
+	ok = dpi_dsi_clk_calc(pck_req, &ctx);
+	if (!ok)
+		return -EINVAL;
 
-	r = dsi_pll_set_clock_div(dpi.dsidev, &dsi_cinfo);
+	r = dsi_pll_set_clock_div(dpi.dsidev, &ctx.dsi_cinfo);
 	if (r)
 		return r;
 
-	dss_select_lcd_clk_source(mgr->id,
-			dpi_get_alt_clk_src(mgr->id));
+	dss_select_lcd_clk_source(channel,
+			dpi_get_alt_clk_src(channel));
 
-	dpi.mgr_config.clock_info = dispc_cinfo;
+	dpi.mgr_config.clock_info = ctx.dispc_cinfo;
 
-	*fck = dsi_cinfo.dsi_pll_hsdiv_dispc_clk;
-	*lck_div = dispc_cinfo.lck_div;
-	*pck_div = dispc_cinfo.pck_div;
+	*fck = ctx.dsi_cinfo.dsi_pll_hsdiv_dispc_clk;
+	*lck_div = ctx.dispc_cinfo.lck_div;
+	*pck_div = ctx.dispc_cinfo.pck_div;
 
 	return 0;
 }
 
-static int dpi_set_dispc_clk(struct omap_dss_device *dssdev,
-		unsigned long pck_req, unsigned long *fck, int *lck_div,
-		int *pck_div)
+static int dpi_set_dispc_clk(unsigned long pck_req, unsigned long *fck,
+		int *lck_div, int *pck_div)
 {
-	struct dss_clock_info dss_cinfo;
-	struct dispc_clock_info dispc_cinfo;
+	struct dpi_clk_calc_ctx ctx;
 	int r;
+	bool ok;
 
-	r = dss_calc_clock_div(pck_req, &dss_cinfo, &dispc_cinfo);
-	if (r)
-		return r;
+	ok = dpi_dss_clk_calc(pck_req, &ctx);
+	if (!ok)
+		return -EINVAL;
 
-	r = dss_set_clock_div(&dss_cinfo);
+	r = dss_set_clock_div(&ctx.dss_cinfo);
 	if (r)
 		return r;
 
-	dpi.mgr_config.clock_info = dispc_cinfo;
+	dpi.mgr_config.clock_info = ctx.dispc_cinfo;
 
-	*fck = dss_cinfo.fck;
-	*lck_div = dispc_cinfo.lck_div;
-	*pck_div = dispc_cinfo.pck_div;
+	*fck = ctx.dss_cinfo.fck;
+	*lck_div = ctx.dispc_cinfo.lck_div;
+	*pck_div = ctx.dispc_cinfo.pck_div;
 
 	return 0;
 }
 
-static int dpi_set_mode(struct omap_dss_device *dssdev)
+static int dpi_set_mode(struct omap_overlay_manager *mgr)
 {
 	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;
 	int r = 0;
 
 	if (dpi.dsidev)
-		r = dpi_set_dsi_clk(dssdev, t->pixel_clock * 1000, &fck,
+		r = dpi_set_dsi_clk(mgr->id, t->pixel_clock * 1000, &fck,
 				&lck_div, &pck_div);
 	else
-		r = dpi_set_dispc_clk(dssdev, t->pixel_clock * 1000, &fck,
+		r = dpi_set_dispc_clk(t->pixel_clock * 1000, &fck,
 				&lck_div, &pck_div);
 	if (r)
 		return r;
@@ -179,10 +329,8 @@ static int dpi_set_mode(struct omap_dss_device *dssdev)
 	return 0;
 }
 
-static void dpi_config_lcd_manager(struct omap_dss_device *dssdev)
+static void dpi_config_lcd_manager(struct omap_overlay_manager *mgr)
 {
-	struct omap_overlay_manager *mgr = dssdev->output->manager;
-
 	dpi.mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
 
 	dpi.mgr_config.stallmode = false;
@@ -197,7 +345,7 @@ static void dpi_config_lcd_manager(struct omap_dss_device *dssdev)
 
 int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
 {
-	struct omap_dss_output *out = dssdev->output;
+	struct omap_dss_output *out = &dpi.output;
 	int r;
 
 	mutex_lock(&dpi.lock);
@@ -230,7 +378,7 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
 	if (r)
 		goto err_get_dispc;
 
-	r = dss_dpi_select_source(dssdev->channel);
+	r = dss_dpi_select_source(out->manager->id);
 	if (r)
 		goto err_src_sel;
 
@@ -244,11 +392,11 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
 			goto err_dsi_pll_init;
 	}
 
-	r = dpi_set_mode(dssdev);
+	r = dpi_set_mode(out->manager);
 	if (r)
 		goto err_set_mode;
 
-	dpi_config_lcd_manager(dssdev);
+	dpi_config_lcd_manager(out->manager);
 
 	mdelay(2);
 
@@ -285,7 +433,7 @@ EXPORT_SYMBOL(omapdss_dpi_display_enable);
 
 void omapdss_dpi_display_disable(struct omap_dss_device *dssdev)
 {
-	struct omap_overlay_manager *mgr = dssdev->output->manager;
+	struct omap_overlay_manager *mgr = dpi.output.manager;
 
 	mutex_lock(&dpi.lock);
 
@@ -324,12 +472,12 @@ 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;
+	struct omap_overlay_manager *mgr = dpi.output.manager;
 	int lck_div, pck_div;
 	unsigned long fck;
 	unsigned long pck;
-	struct dispc_clock_info dispc_cinfo;
+	struct dpi_clk_calc_ctx ctx;
+	bool ok;
 
 	if (mgr && !dispc_mgr_timings_ok(mgr->id, timings))
 		return -EINVAL;
@@ -338,28 +486,21 @@ int dpi_check_timings(struct omap_dss_device *dssdev,
 		return -EINVAL;
 
 	if (dpi.dsidev) {
-		struct dsi_clock_info dsi_cinfo;
-		r = dsi_pll_calc_clock_div_pck(dpi.dsidev,
-				timings->pixel_clock * 1000,
-				&dsi_cinfo, &dispc_cinfo);
+		ok = dpi_dsi_clk_calc(timings->pixel_clock * 1000, &ctx);
+		if (!ok)
+			return -EINVAL;
 
-		if (r)
-			return r;
-
-		fck = dsi_cinfo.dsi_pll_hsdiv_dispc_clk;
+		fck = ctx.dsi_cinfo.dsi_pll_hsdiv_dispc_clk;
 	} else {
-		struct dss_clock_info dss_cinfo;
-		r = dss_calc_clock_div(timings->pixel_clock * 1000,
-				&dss_cinfo, &dispc_cinfo);
+		ok = dpi_dss_clk_calc(timings->pixel_clock * 1000, &ctx);
+		if (!ok)
+			return -EINVAL;
 
-		if (r)
-			return r;
-
-		fck = dss_cinfo.fck;
+		fck = ctx.dss_cinfo.fck;
 	}
 
-	lck_div = dispc_cinfo.lck_div;
-	pck_div = dispc_cinfo.pck_div;
+	lck_div = ctx.dispc_cinfo.lck_div;
+	pck_div = ctx.dispc_cinfo.pck_div;
 
 	pck = fck / lck_div / pck_div / 1000;
 
@@ -379,7 +520,7 @@ void omapdss_dpi_set_data_lines(struct omap_dss_device *dssdev, int data_lines)
 }
 EXPORT_SYMBOL(omapdss_dpi_set_data_lines);
 
-static int __init dpi_verify_dsi_pll(struct platform_device *dsidev)
+static int dpi_verify_dsi_pll(struct platform_device *dsidev)
 {
 	int r;
 
@@ -401,7 +542,37 @@ static int __init dpi_verify_dsi_pll(struct platform_device *dsidev)
 	return 0;
 }
 
-static int __init dpi_init_display(struct omap_dss_device *dssdev)
+/*
+ * Return a hardcoded channel for the DPI output. This should work for
+ * current use cases, but this can be later expanded to either resolve
+ * the channel in some more dynamic manner, or get the channel as a user
+ * parameter.
+ */
+static enum omap_channel dpi_get_channel(void)
+{
+	switch (omapdss_get_version()) {
+	case OMAPDSS_VER_OMAP24xx:
+	case OMAPDSS_VER_OMAP34xx_ES1:
+	case OMAPDSS_VER_OMAP34xx_ES3:
+	case OMAPDSS_VER_OMAP3630:
+	case OMAPDSS_VER_AM35xx:
+		return OMAP_DSS_CHANNEL_LCD;
+
+	case OMAPDSS_VER_OMAP4430_ES1:
+	case OMAPDSS_VER_OMAP4430_ES2:
+	case OMAPDSS_VER_OMAP4:
+		return OMAP_DSS_CHANNEL_LCD2;
+
+	case OMAPDSS_VER_OMAP5:
+		return OMAP_DSS_CHANNEL_LCD3;
+
+	default:
+		DSSWARN("unsupported DSS version\n");
+		return OMAP_DSS_CHANNEL_LCD;
+	}
+}
+
+static int dpi_init_display(struct omap_dss_device *dssdev)
 {
 	struct platform_device *dsidev;
 
@@ -421,12 +592,7 @@ static int __init dpi_init_display(struct omap_dss_device *dssdev)
 		dpi.vdds_dsi_reg = vdds_dsi;
 	}
 
-	/*
-	 * XXX We shouldn't need dssdev->channel for this. The dsi pll clock
-	 * source for DPI is SoC integration detail, not something that should
-	 * be configured in the dssdev
-	 */
-	dsidev = dpi_get_dsidev(dssdev->channel);
+	dsidev = dpi_get_dsidev(dpi.output.dispc_channel);
 
 	if (dsidev && dpi_verify_dsi_pll(dsidev)) {
 		dsidev = NULL;
@@ -441,7 +607,7 @@ static int __init dpi_init_display(struct omap_dss_device *dssdev)
 	return 0;
 }
 
-static struct omap_dss_device * __init dpi_find_dssdev(struct platform_device *pdev)
+static struct omap_dss_device *dpi_find_dssdev(struct platform_device *pdev)
 {
 	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
 	const char *def_disp_name = omapdss_get_default_display_name();
@@ -469,7 +635,7 @@ static struct omap_dss_device * __init dpi_find_dssdev(struct platform_device *p
 	return def_dssdev;
 }
 
-static void __init dpi_probe_pdata(struct platform_device *dpidev)
+static int dpi_probe_pdata(struct platform_device *dpidev)
 {
 	struct omap_dss_device *plat_dssdev;
 	struct omap_dss_device *dssdev;
@@ -478,11 +644,11 @@ static void __init dpi_probe_pdata(struct platform_device *dpidev)
 	plat_dssdev = dpi_find_dssdev(dpidev);
 
 	if (!plat_dssdev)
-		return;
+		return 0;
 
 	dssdev = dss_alloc_and_init_device(&dpidev->dev);
 	if (!dssdev)
-		return;
+		return -ENOMEM;
 
 	dss_copy_device_pdata(dssdev, plat_dssdev);
 
@@ -490,7 +656,7 @@ static void __init dpi_probe_pdata(struct platform_device *dpidev)
 	if (r) {
 		DSSERR("device %s init failed: %d\n", dssdev->name, r);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
 
 	r = omapdss_output_set_device(&dpi.output, dssdev);
@@ -498,7 +664,7 @@ static void __init dpi_probe_pdata(struct platform_device *dpidev)
 		DSSERR("failed to connect output to new device: %s\n",
 				dssdev->name);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
 
 	r = dss_add_device(dssdev);
@@ -506,17 +672,21 @@ static void __init dpi_probe_pdata(struct platform_device *dpidev)
 		DSSERR("device %s register failed: %d\n", dssdev->name, r);
 		omapdss_output_unset_device(&dpi.output);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
+
+	return 0;
 }
 
-static void __init dpi_init_output(struct platform_device *pdev)
+static void 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;
+	out->name = "dpi.0";
+	out->dispc_channel = dpi_get_channel();
 
 	dss_register_output(out);
 }
@@ -528,13 +698,19 @@ static void __exit dpi_uninit_output(struct platform_device *pdev)
 	dss_unregister_output(out);
 }
 
-static int __init omap_dpi_probe(struct platform_device *pdev)
+static int omap_dpi_probe(struct platform_device *pdev)
 {
+	int r;
+
 	mutex_init(&dpi.lock);
 
 	dpi_init_output(pdev);
 
-	dpi_probe_pdata(pdev);
+	r = dpi_probe_pdata(pdev);
+	if (r) {
+		dpi_uninit_output(pdev);
+		return r;
+	}
 
 	return 0;
 }
@@ -549,6 +725,7 @@ static int __exit omap_dpi_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver omap_dpi_driver = {
+	.probe		= omap_dpi_probe,
 	.remove         = __exit_p(omap_dpi_remove),
 	.driver         = {
 		.name   = "omapdss_dpi",
@@ -558,7 +735,7 @@ static struct platform_driver omap_dpi_driver = {
 
 int __init dpi_init_platform_driver(void)
 {
-	return platform_driver_probe(&omap_dpi_driver, omap_dpi_probe);
+	return platform_driver_register(&omap_dpi_driver);
 }
 
 void __exit dpi_uninit_platform_driver(void)
diff --git a/drivers/video/omap2/dss/dsi.c b/drivers/video/omap2/dss/dsi.c
index 28d41d16b7be..a73dedc33101 100644
--- a/drivers/video/omap2/dss/dsi.c
+++ b/drivers/video/omap2/dss/dsi.c
@@ -200,6 +200,11 @@ struct dsi_reg { u16 idx; };
 
 typedef void (*omap_dsi_isr_t) (void *arg, u32 mask);
 
+static int dsi_display_init_dispc(struct platform_device *dsidev,
+	struct omap_overlay_manager *mgr);
+static void dsi_display_uninit_dispc(struct platform_device *dsidev,
+	struct omap_overlay_manager *mgr);
+
 #define DSI_MAX_NR_ISRS                2
 #define DSI_MAX_NR_LANES	5
 
@@ -250,6 +255,24 @@ struct dsi_isr_tables {
 	struct dsi_isr_data isr_table_cio[DSI_MAX_NR_ISRS];
 };
 
+struct dsi_clk_calc_ctx {
+	struct platform_device *dsidev;
+
+	/* inputs */
+
+	const struct omap_dss_dsi_config *config;
+
+	unsigned long req_pck_min, req_pck_nom, req_pck_max;
+
+	/* outputs */
+
+	struct dsi_clock_info dsi_cinfo;
+	struct dispc_clock_info dispc_cinfo;
+
+	struct omap_video_timings dispc_vm;
+	struct omap_dss_dsi_videomode_timings dsi_vm;
+};
+
 struct dsi_data {
 	struct platform_device *pdev;
 	void __iomem	*base;
@@ -261,6 +284,9 @@ struct dsi_data {
 	struct clk *dss_clk;
 	struct clk *sys_clk;
 
+	struct dispc_clock_info user_dispc_cinfo;
+	struct dsi_clock_info user_dsi_cinfo;
+
 	struct dsi_clock_info current_cinfo;
 
 	bool vdds_dsi_enabled;
@@ -324,6 +350,7 @@ struct dsi_data {
 	unsigned long lpdiv_max;
 
 	unsigned num_lanes_supported;
+	unsigned line_buffer_size;
 
 	struct dsi_lane_config lanes[DSI_MAX_NR_LANES];
 	unsigned num_lanes_used;
@@ -1192,15 +1219,33 @@ static unsigned long dsi_fclk_rate(struct platform_device *dsidev)
 	return r;
 }
 
-static int dsi_set_lp_clk_divisor(struct omap_dss_device *dssdev)
+static int dsi_lp_clock_calc(struct dsi_clock_info *cinfo,
+		unsigned long lp_clk_min, unsigned long lp_clk_max)
+{
+	unsigned long dsi_fclk = cinfo->dsi_pll_hsdiv_dsi_clk;
+	unsigned lp_clk_div;
+	unsigned long lp_clk;
+
+	lp_clk_div = DIV_ROUND_UP(dsi_fclk, lp_clk_max * 2);
+	lp_clk = dsi_fclk / 2 / lp_clk_div;
+
+	if (lp_clk < lp_clk_min || lp_clk > lp_clk_max)
+		return -EINVAL;
+
+	cinfo->lp_clk_div = lp_clk_div;
+	cinfo->lp_clk = lp_clk;
+
+	return 0;
+}
+
+static int dsi_set_lp_clk_divisor(struct platform_device *dsidev)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	unsigned long dsi_fclk;
 	unsigned lp_clk_div;
 	unsigned long lp_clk;
 
-	lp_clk_div = dssdev->clocks.dsi.lp_clk_div;
+	lp_clk_div = dsi->user_dsi_cinfo.lp_clk_div;
 
 	if (lp_clk_div == 0 || lp_clk_div > dsi->lpdiv_max)
 		return -EINVAL;
@@ -1272,6 +1317,75 @@ static int dsi_pll_power(struct platform_device *dsidev,
 	return 0;
 }
 
+unsigned long dsi_get_pll_clkin(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	return clk_get_rate(dsi->sys_clk);
+}
+
+bool dsi_hsdiv_calc(struct platform_device *dsidev, unsigned long pll,
+		unsigned long out_min, dsi_hsdiv_calc_func func, void *data)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int regm, regm_start, regm_stop;
+	unsigned long out_max;
+	unsigned long out;
+
+	out_min = out_min ? out_min : 1;
+	out_max = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
+
+	regm_start = max(DIV_ROUND_UP(pll, out_max), 1ul);
+	regm_stop = min(pll / out_min, dsi->regm_dispc_max);
+
+	for (regm = regm_start; regm <= regm_stop; ++regm) {
+		out = pll / regm;
+
+		if (func(regm, out, data))
+			return true;
+	}
+
+	return false;
+}
+
+bool dsi_pll_calc(struct platform_device *dsidev, unsigned long clkin,
+		unsigned long pll_min, unsigned long pll_max,
+		dsi_pll_calc_func func, void *data)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int regn, regn_start, regn_stop;
+	int regm, regm_start, regm_stop;
+	unsigned long fint, pll;
+	const unsigned long pll_hw_max = 1800000000;
+	unsigned long fint_hw_min, fint_hw_max;
+
+	fint_hw_min = dsi->fint_min;
+	fint_hw_max = dsi->fint_max;
+
+	regn_start = max(DIV_ROUND_UP(clkin, fint_hw_max), 1ul);
+	regn_stop = min(clkin / fint_hw_min, dsi->regn_max);
+
+	pll_max = pll_max ? pll_max : ULONG_MAX;
+
+	for (regn = regn_start; regn <= regn_stop; ++regn) {
+		fint = clkin / regn;
+
+		regm_start = max(DIV_ROUND_UP(DIV_ROUND_UP(pll_min, fint), 2),
+				1ul);
+		regm_stop = min3(pll_max / fint / 2,
+				pll_hw_max / fint / 2,
+				dsi->regm_max);
+
+		for (regm = regm_start; regm <= regm_stop; ++regm) {
+			pll = 2 * regm * fint;
+
+			if (func(regn, regm, fint, pll, data))
+				return true;
+		}
+	}
+
+	return false;
+}
+
 /* calculate clock rates using dividers in cinfo */
 static int dsi_calc_clock_rates(struct platform_device *dsidev,
 		struct dsi_clock_info *cinfo)
@@ -1316,192 +1430,7 @@ static int dsi_calc_clock_rates(struct platform_device *dsidev,
 	return 0;
 }
 
-int dsi_pll_calc_clock_div_pck(struct platform_device *dsidev,
-		unsigned long req_pck, struct dsi_clock_info *dsi_cinfo,
-		struct dispc_clock_info *dispc_cinfo)
-{
-	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
-	struct dsi_clock_info cur, best;
-	struct dispc_clock_info best_dispc;
-	int min_fck_per_pck;
-	int match = 0;
-	unsigned long dss_sys_clk, max_dss_fck;
-
-	dss_sys_clk = clk_get_rate(dsi->sys_clk);
-
-	max_dss_fck = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
-
-	if (req_pck == dsi->cache_req_pck &&
-			dsi->cache_cinfo.clkin == dss_sys_clk) {
-		DSSDBG("DSI clock info found from cache\n");
-		*dsi_cinfo = dsi->cache_cinfo;
-		dispc_find_clk_divs(req_pck, dsi_cinfo->dsi_pll_hsdiv_dispc_clk,
-			dispc_cinfo);
-		return 0;
-	}
-
-	min_fck_per_pck = CONFIG_OMAP2_DSS_MIN_FCK_PER_PCK;
-
-	if (min_fck_per_pck &&
-		req_pck * min_fck_per_pck > 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;
-	}
-
-	DSSDBG("dsi_pll_calc\n");
-
-retry:
-	memset(&best, 0, sizeof(best));
-	memset(&best_dispc, 0, sizeof(best_dispc));
-
-	memset(&cur, 0, sizeof(cur));
-	cur.clkin = dss_sys_clk;
-
-	/* 0.75MHz < Fint = clkin / regn < 2.1MHz */
-	/* To reduce PLL lock time, keep Fint high (around 2 MHz) */
-	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;
-
-			/* dsi_pll_hsdiv_dispc_clk(MHz) =
-			 * DSIPHY(MHz) / regm_dispc  < 173MHz/186Mhz */
-			for (cur.regm_dispc = 1; cur.regm_dispc <
-					dsi->regm_dispc_max; ++cur.regm_dispc) {
-				struct dispc_clock_info cur_dispc;
-				cur.dsi_pll_hsdiv_dispc_clk =
-					cur.clkin4ddr / cur.regm_dispc;
-
-				if (cur.regm_dispc > 1 &&
-						cur.regm_dispc % 2 != 0 &&
-						req_pck >= 1000000)
-					continue;
-
-				/* this will narrow down the search a bit,
-				 * but still give pixclocks below what was
-				 * requested */
-				if (cur.dsi_pll_hsdiv_dispc_clk  < req_pck)
-					break;
-
-				if (cur.dsi_pll_hsdiv_dispc_clk > max_dss_fck)
-					continue;
-
-				if (min_fck_per_pck &&
-					cur.dsi_pll_hsdiv_dispc_clk <
-						req_pck * min_fck_per_pck)
-					continue;
-
-				match = 1;
-
-				dispc_find_clk_divs(req_pck,
-						cur.dsi_pll_hsdiv_dispc_clk,
-						&cur_dispc);
-
-				if (abs(cur_dispc.pck - req_pck) <
-						abs(best_dispc.pck - req_pck)) {
-					best = cur;
-					best_dispc = cur_dispc;
-
-					if (cur_dispc.pck == req_pck)
-						goto found;
-				}
-			}
-		}
-	}
-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;
-	}
-
-	/* dsi_pll_hsdiv_dsi_clk (regm_dsi) is not used */
-	best.regm_dsi = 0;
-	best.dsi_pll_hsdiv_dsi_clk = 0;
-
-	if (dsi_cinfo)
-		*dsi_cinfo = best;
-	if (dispc_cinfo)
-		*dispc_cinfo = best_dispc;
-
-	dsi->cache_req_pck = req_pck;
-	dsi->cache_clk_freq = 0;
-	dsi->cache_cinfo = best;
-
-	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)
+static void dsi_pll_calc_dsi_fck(struct dsi_clock_info *cinfo)
 {
 	unsigned long max_dsi_fck;
 
@@ -1511,90 +1440,6 @@ static void dsi_pll_calc_dsi_fck(struct platform_device *dsidev,
 	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)
 {
@@ -2783,6 +2628,7 @@ static int dsi_vc_enable(struct platform_device *dsidev, int channel,
 
 static void dsi_vc_initial_config(struct platform_device *dsidev, int channel)
 {
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	u32 r;
 
 	DSSDBG("Initial config of virtual channel %d", channel);
@@ -2807,6 +2653,8 @@ static void dsi_vc_initial_config(struct platform_device *dsidev, int channel)
 	r = FLD_MOD(r, 4, 23, 21); /* DMA_TX_REQ_NB = no dma */
 
 	dsi_write_reg(dsidev, DSI_VC_CTRL(channel), r);
+
+	dsi->vc[channel].source = DSI_VC_SOURCE_L4;
 }
 
 static int dsi_vc_config_source(struct platform_device *dsidev, int channel,
@@ -3777,13 +3625,12 @@ static void dsi_config_vp_num_line_buffers(struct platform_device *dsidev)
 
 	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 = &dsi->timings;
 		/*
 		 * Don't use line buffers if width is greater than the video
 		 * port's line buffer size
 		 */
-		if (line_buf_size <= timings->x_res * bpp / 8)
+		if (dsi->line_buffer_size <= timings->x_res * bpp / 8)
 			num_line_buffers = 0;
 		else
 			num_line_buffers = 2;
@@ -3799,18 +3646,22 @@ static void dsi_config_vp_num_line_buffers(struct platform_device *dsidev)
 static void dsi_config_vp_sync_events(struct platform_device *dsidev)
 {
 	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;
+	bool sync_end;
 	u32 r;
 
+	if (dsi->vm_timings.trans_mode == OMAP_DSS_DSI_PULSE_MODE)
+		sync_end = true;
+	else
+		sync_end = false;
+
 	r = dsi_read_reg(dsidev, DSI_CTRL);
 	r = FLD_MOD(r, 1, 9, 9);		/* VP_DE_POL */
 	r = FLD_MOD(r, 1, 10, 10);		/* VP_HSYNC_POL */
 	r = FLD_MOD(r, 1, 11, 11);		/* VP_VSYNC_POL */
 	r = FLD_MOD(r, 1, 15, 15);		/* VP_VSYNC_START */
-	r = FLD_MOD(r, vsync_end, 16, 16);	/* VP_VSYNC_END */
+	r = FLD_MOD(r, sync_end, 16, 16);	/* VP_VSYNC_END */
 	r = FLD_MOD(r, 1, 17, 17);		/* VP_HSYNC_START */
-	r = FLD_MOD(r, hsync_end, 18, 18);	/* VP_HSYNC_END */
+	r = FLD_MOD(r, sync_end, 18, 18);	/* VP_HSYNC_END */
 	dsi_write_reg(dsidev, DSI_CTRL, r);
 }
 
@@ -3897,9 +3748,8 @@ static int dsi_compute_interleave_lp(int blank, int enter_hs, int exit_hs,
 	return max(lp_inter, 0);
 }
 
-static void dsi_config_cmd_mode_interleaving(struct omap_dss_device *dssdev)
+static void dsi_config_cmd_mode_interleaving(struct platform_device *dsidev)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	int blanking_mode;
 	int hfp_blanking_mode, hbp_blanking_mode, hsa_blanking_mode;
@@ -3910,7 +3760,7 @@ static void dsi_config_cmd_mode_interleaving(struct omap_dss_device *dssdev)
 	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 dsi_fclk_hsdiv = dsi->user_dsi_cinfo.regm_dsi + 1;
 	int hsa_interleave_hs = 0, hsa_interleave_lp = 0;
 	int hfp_interleave_hs = 0, hfp_interleave_lp = 0;
 	int hbp_interleave_hs = 0, hbp_interleave_lp = 0;
@@ -4015,9 +3865,8 @@ static void dsi_config_cmd_mode_interleaving(struct omap_dss_device *dssdev)
 	dsi_write_reg(dsidev, DSI_VM_TIMING6, r);
 }
 
-static int dsi_proto_config(struct omap_dss_device *dssdev)
+static int dsi_proto_config(struct platform_device *dsidev)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	u32 r;
 	int buswidth = 0;
@@ -4075,7 +3924,7 @@ static int dsi_proto_config(struct omap_dss_device *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);
+		dsi_config_cmd_mode_interleaving(dsidev);
 	}
 
 	dsi_vc_initial_config(dsidev, 0);
@@ -4159,11 +4008,12 @@ static void dsi_proto_timings(struct platform_device *dsidev)
 		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;
+		bool hsync_end;
 		struct omap_video_timings *timings = &dsi->timings;
 		int bpp = dsi_get_pixel_size(dsi->pix_fmt);
 		int tl, t_he, width_bytes;
 
+		hsync_end = dsi->vm_timings.trans_mode == OMAP_DSS_DSI_PULSE_MODE;
 		t_he = hsync_end ?
 			((hsa == 0 && ndl == 3) ? 1 : DIV_ROUND_UP(4, ndl)) : 0;
 
@@ -4266,82 +4116,26 @@ int omapdss_dsi_configure_pins(struct omap_dss_device *dssdev,
 }
 EXPORT_SYMBOL(omapdss_dsi_configure_pins);
 
-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;
-
-	DSSDBG("Setting DSI clocks: 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;
+	struct omap_overlay_manager *mgr = dsi->output.manager;
 	int bpp = dsi_get_pixel_size(dsi->pix_fmt);
+	struct omap_dss_output *out = &dsi->output;
 	u8 data_type;
 	u16 word_count;
 	int r;
 
+	if (out == NULL || out->manager == NULL) {
+		DSSERR("failed to enable display: no output/manager\n");
+		return -ENODEV;
+	}
+
+	r = dsi_display_init_dispc(dsidev, mgr);
+	if (r)
+		goto err_init_dispc;
+
 	if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
 		switch (dsi->pix_fmt) {
 		case OMAP_DSS_DSI_FMT_RGB888:
@@ -4357,8 +4151,8 @@ int dsi_enable_video_output(struct omap_dss_device *dssdev, int channel)
 			data_type = MIPI_DSI_PACKED_PIXEL_STREAM_16;
 			break;
 		default:
-			BUG();
-			return -EINVAL;
+			r = -EINVAL;
+			goto err_pix_fmt;
 		};
 
 		dsi_if_enable(dsidev, false);
@@ -4377,16 +4171,20 @@ int dsi_enable_video_output(struct omap_dss_device *dssdev, int channel)
 	}
 
 	r = dss_mgr_enable(mgr);
-	if (r) {
-		if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
-			dsi_if_enable(dsidev, false);
-			dsi_vc_enable(dsidev, channel, false);
-		}
-
-		return r;
-	}
+	if (r)
+		goto err_mgr_enable;
 
 	return 0;
+
+err_mgr_enable:
+	if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+		dsi_if_enable(dsidev, false);
+		dsi_vc_enable(dsidev, channel, false);
+	}
+err_pix_fmt:
+	dsi_display_uninit_dispc(dsidev, mgr);
+err_init_dispc:
+	return r;
 }
 EXPORT_SYMBOL(dsi_enable_video_output);
 
@@ -4394,7 +4192,7 @@ 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;
+	struct omap_overlay_manager *mgr = dsi->output.manager;
 
 	if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
 		dsi_if_enable(dsidev, false);
@@ -4408,14 +4206,15 @@ void dsi_disable_video_output(struct omap_dss_device *dssdev, int channel)
 	}
 
 	dss_mgr_disable(mgr);
+
+	dsi_display_uninit_dispc(dsidev, mgr);
 }
 EXPORT_SYMBOL(dsi_disable_video_output);
 
-static void dsi_update_screen_dispc(struct omap_dss_device *dssdev)
+static void dsi_update_screen_dispc(struct platform_device *dsidev)
 {
-	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;
+	struct omap_overlay_manager *mgr = dsi->output.manager;
 	unsigned bytespp;
 	unsigned bytespl;
 	unsigned bytespf;
@@ -4425,7 +4224,7 @@ static void dsi_update_screen_dispc(struct omap_dss_device *dssdev)
 	u32 l;
 	int r;
 	const unsigned channel = dsi->update_channel;
-	const unsigned line_buf_size = dsi_get_line_buf_size(dsidev);
+	const unsigned line_buf_size = dsi->line_buffer_size;
 	u16 w = dsi->timings.x_res;
 	u16 h = dsi->timings.y_res;
 
@@ -4571,7 +4370,7 @@ int omap_dsi_update(struct omap_dss_device *dssdev, int channel,
 	dsi->update_bytes = dw * dh *
 		dsi_get_pixel_size(dsi->pix_fmt) / 8;
 #endif
-	dsi_update_screen_dispc(dssdev);
+	dsi_update_screen_dispc(dsidev);
 
 	return 0;
 }
@@ -4579,18 +4378,17 @@ EXPORT_SYMBOL(omap_dsi_update);
 
 /* Display funcs */
 
-static int dsi_configure_dispc_clocks(struct omap_dss_device *dssdev)
+static int dsi_configure_dispc_clocks(struct platform_device *dsidev)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	struct dispc_clock_info dispc_cinfo;
 	int r;
-	unsigned long long fck;
+	unsigned long fck;
 
 	fck = dsi_get_pll_hsdiv_dispc_rate(dsidev);
 
-	dispc_cinfo.lck_div = dssdev->clocks.dispc.channel.lck_div;
-	dispc_cinfo.pck_div = dssdev->clocks.dispc.channel.pck_div;
+	dispc_cinfo.lck_div = dsi->user_dispc_cinfo.lck_div;
+	dispc_cinfo.pck_div = dsi->user_dispc_cinfo.pck_div;
 
 	r = dispc_calc_clock_rates(fck, &dispc_cinfo);
 	if (r) {
@@ -4603,21 +4401,17 @@ static int dsi_configure_dispc_clocks(struct omap_dss_device *dssdev)
 	return 0;
 }
 
-static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
+static int dsi_display_init_dispc(struct platform_device *dsidev,
+		struct omap_overlay_manager *mgr)
 {
-	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;
 
-	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;
+	dss_select_lcd_clk_source(mgr->id, dsi->module_id == 0 ?
+			OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC :
+			OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC);
 
+	if (dsi->mode == OMAP_DSS_DSI_CMD_MODE) {
 		r = dss_mgr_register_framedone_handler(mgr,
 				dsi_framedone_irq_callback, dsidev);
 		if (r) {
@@ -4645,7 +4439,7 @@ static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
 
 	dss_mgr_set_timings(mgr, &dsi->timings);
 
-	r = dsi_configure_dispc_clocks(dssdev);
+	r = dsi_configure_dispc_clocks(dsidev);
 	if (r)
 		goto err1;
 
@@ -4662,30 +4456,30 @@ err1:
 		dss_mgr_unregister_framedone_handler(mgr,
 				dsi_framedone_irq_callback, dsidev);
 err:
+	dss_select_lcd_clk_source(mgr->id, OMAP_DSS_CLK_SRC_FCK);
 	return r;
 }
 
-static void dsi_display_uninit_dispc(struct omap_dss_device *dssdev)
+static void dsi_display_uninit_dispc(struct platform_device *dsidev,
+		struct omap_overlay_manager *mgr)
 {
-	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)
 		dss_mgr_unregister_framedone_handler(mgr,
 				dsi_framedone_irq_callback, dsidev);
+
+	dss_select_lcd_clk_source(mgr->id, OMAP_DSS_CLK_SRC_FCK);
 }
 
-static int dsi_configure_dsi_clocks(struct omap_dss_device *dssdev)
+static int dsi_configure_dsi_clocks(struct platform_device *dsidev)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	struct dsi_clock_info cinfo;
 	int r;
 
-	cinfo.regn  = dssdev->clocks.dsi.regn;
-	cinfo.regm  = dssdev->clocks.dsi.regm;
-	cinfo.regm_dispc = dssdev->clocks.dsi.regm_dispc;
-	cinfo.regm_dsi = dssdev->clocks.dsi.regm_dsi;
+	cinfo = dsi->user_dsi_cinfo;
+
 	r = dsi_calc_clock_rates(dsidev, &cinfo);
 	if (r) {
 		DSSERR("Failed to calc dsi clocks\n");
@@ -4701,24 +4495,22 @@ static int dsi_configure_dsi_clocks(struct omap_dss_device *dssdev)
 	return 0;
 }
 
-static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
+static int dsi_display_init_dsi(struct platform_device *dsidev)
 {
-	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);
 	if (r)
 		goto err0;
 
-	r = dsi_configure_dsi_clocks(dssdev);
+	r = dsi_configure_dsi_clocks(dsidev);
 	if (r)
 		goto err1;
 
-	dss_select_dsi_clk_source(dsi->module_id, dssdev->clocks.dsi.dsi_fclk_src);
-	dss_select_lcd_clk_source(mgr->id,
-			dssdev->clocks.dispc.channel.lcd_clk_src);
+	dss_select_dsi_clk_source(dsi->module_id, dsi->module_id == 0 ?
+			OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI :
+			OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI);
 
 	DSSDBG("PLL OK\n");
 
@@ -4729,12 +4521,12 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 	_dsi_print_reset_status(dsidev);
 
 	dsi_proto_timings(dsidev);
-	dsi_set_lp_clk_divisor(dssdev);
+	dsi_set_lp_clk_divisor(dsidev);
 
 	if (1)
 		_dsi_print_reset_status(dsidev);
 
-	r = dsi_proto_config(dssdev);
+	r = dsi_proto_config(dsidev);
 	if (r)
 		goto err3;
 
@@ -4751,20 +4543,16 @@ err3:
 	dsi_cio_uninit(dsidev);
 err2:
 	dss_select_dsi_clk_source(dsi->module_id, OMAP_DSS_CLK_SRC_FCK);
-	dss_select_lcd_clk_source(mgr->id, OMAP_DSS_CLK_SRC_FCK);
-
 err1:
 	dsi_pll_uninit(dsidev, true);
 err0:
 	return r;
 }
 
-static void dsi_display_uninit_dsi(struct omap_dss_device *dssdev,
+static void dsi_display_uninit_dsi(struct platform_device *dsidev,
 		bool disconnect_lanes, bool enter_ulps)
 {
-	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);
@@ -4777,7 +4565,6 @@ static void dsi_display_uninit_dsi(struct omap_dss_device *dssdev,
 	dsi_vc_enable(dsidev, 3, 0);
 
 	dss_select_dsi_clk_source(dsi->module_id, OMAP_DSS_CLK_SRC_FCK);
-	dss_select_lcd_clk_source(mgr->id, OMAP_DSS_CLK_SRC_FCK);
 	dsi_cio_uninit(dsidev);
 	dsi_pll_uninit(dsidev, disconnect_lanes);
 }
@@ -4786,7 +4573,6 @@ 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");
@@ -4795,12 +4581,6 @@ int omapdss_dsi_display_enable(struct omap_dss_device *dssdev)
 
 	mutex_lock(&dsi->lock);
 
-	if (out == NULL || out->manager == NULL) {
-		DSSERR("failed to enable display: no output/manager\n");
-		r = -ENODEV;
-		goto err_start_dev;
-	}
-
 	r = omap_dss_start_device(dssdev);
 	if (r) {
 		DSSERR("failed to start device\n");
@@ -4815,11 +4595,7 @@ int omapdss_dsi_display_enable(struct omap_dss_device *dssdev)
 
 	_dsi_initialize_irq(dsidev);
 
-	r = dsi_display_init_dispc(dssdev);
-	if (r)
-		goto err_init_dispc;
-
-	r = dsi_display_init_dsi(dssdev);
+	r = dsi_display_init_dsi(dsidev);
 	if (r)
 		goto err_init_dsi;
 
@@ -4828,8 +4604,6 @@ int omapdss_dsi_display_enable(struct omap_dss_device *dssdev)
 	return 0;
 
 err_init_dsi:
-	dsi_display_uninit_dispc(dssdev);
-err_init_dispc:
 	dsi_enable_pll_clock(dsidev, 0);
 	dsi_runtime_put(dsidev);
 err_get_dsi:
@@ -4858,9 +4632,7 @@ void omapdss_dsi_display_disable(struct omap_dss_device *dssdev,
 	dsi_sync_vc(dsidev, 2);
 	dsi_sync_vc(dsidev, 3);
 
-	dsi_display_uninit_dispc(dssdev);
-
-	dsi_display_uninit_dsi(dssdev, disconnect_lanes, enter_ulps);
+	dsi_display_uninit_dsi(dsidev, disconnect_lanes, enter_ulps);
 
 	dsi_runtime_put(dsidev);
 	dsi_enable_pll_clock(dsidev, 0);
@@ -4881,77 +4653,579 @@ int omapdss_dsi_enable_te(struct omap_dss_device *dssdev, bool enable)
 }
 EXPORT_SYMBOL(omapdss_dsi_enable_te);
 
-void omapdss_dsi_set_timings(struct omap_dss_device *dssdev,
-		struct omap_video_timings *timings)
+#ifdef PRINT_VERBOSE_VM_TIMINGS
+static void print_dsi_vm(const char *str,
+		const struct omap_dss_dsi_videomode_timings *t)
+{
+	unsigned long byteclk = t->hsclk / 4;
+	int bl, wc, pps, tot;
+
+	wc = DIV_ROUND_UP(t->hact * t->bitspp, 8);
+	pps = DIV_ROUND_UP(wc + 6, t->ndl); /* pixel packet size */
+	bl = t->hss + t->hsa + t->hse + t->hbp + t->hfp;
+	tot = bl + pps;
+
+#define TO_DSI_T(x) ((u32)div64_u64((u64)x * 1000000000llu, byteclk))
+
+	pr_debug("%s bck %lu, %u/%u/%u/%u/%u/%u = %u+%u = %u, "
+			"%u/%u/%u/%u/%u/%u = %u + %u = %u\n",
+			str,
+			byteclk,
+			t->hss, t->hsa, t->hse, t->hbp, pps, t->hfp,
+			bl, pps, tot,
+			TO_DSI_T(t->hss),
+			TO_DSI_T(t->hsa),
+			TO_DSI_T(t->hse),
+			TO_DSI_T(t->hbp),
+			TO_DSI_T(pps),
+			TO_DSI_T(t->hfp),
+
+			TO_DSI_T(bl),
+			TO_DSI_T(pps),
+
+			TO_DSI_T(tot));
+#undef TO_DSI_T
+}
+
+static void print_dispc_vm(const char *str, const struct omap_video_timings *t)
+{
+	unsigned long pck = t->pixel_clock * 1000;
+	int hact, bl, tot;
+
+	hact = t->x_res;
+	bl = t->hsw + t->hbp + t->hfp;
+	tot = hact + bl;
+
+#define TO_DISPC_T(x) ((u32)div64_u64((u64)x * 1000000000llu, pck))
+
+	pr_debug("%s pck %lu, %u/%u/%u/%u = %u+%u = %u, "
+			"%u/%u/%u/%u = %u + %u = %u\n",
+			str,
+			pck,
+			t->hsw, t->hbp, hact, t->hfp,
+			bl, hact, tot,
+			TO_DISPC_T(t->hsw),
+			TO_DISPC_T(t->hbp),
+			TO_DISPC_T(hact),
+			TO_DISPC_T(t->hfp),
+			TO_DISPC_T(bl),
+			TO_DISPC_T(hact),
+			TO_DISPC_T(tot));
+#undef TO_DISPC_T
+}
+
+/* note: this is not quite accurate */
+static void print_dsi_dispc_vm(const char *str,
+		const struct omap_dss_dsi_videomode_timings *t)
+{
+	struct omap_video_timings vm = { 0 };
+	unsigned long byteclk = t->hsclk / 4;
+	unsigned long pck;
+	u64 dsi_tput;
+	int dsi_hact, dsi_htot;
+
+	dsi_tput = (u64)byteclk * t->ndl * 8;
+	pck = (u32)div64_u64(dsi_tput, t->bitspp);
+	dsi_hact = DIV_ROUND_UP(DIV_ROUND_UP(t->hact * t->bitspp, 8) + 6, t->ndl);
+	dsi_htot = t->hss + t->hsa + t->hse + t->hbp + dsi_hact + t->hfp;
+
+	vm.pixel_clock = pck / 1000;
+	vm.hsw = div64_u64((u64)(t->hsa + t->hse) * pck, byteclk);
+	vm.hbp = div64_u64((u64)t->hbp * pck, byteclk);
+	vm.hfp = div64_u64((u64)t->hfp * pck, byteclk);
+	vm.x_res = t->hact;
+
+	print_dispc_vm(str, &vm);
+}
+#endif /* PRINT_VERBOSE_VM_TIMINGS */
+
+static bool dsi_cm_calc_dispc_cb(int lckd, int pckd, unsigned long lck,
+		unsigned long pck, void *data)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
-	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct dsi_clk_calc_ctx *ctx = data;
+	struct omap_video_timings *t = &ctx->dispc_vm;
 
-	mutex_lock(&dsi->lock);
+	ctx->dispc_cinfo.lck_div = lckd;
+	ctx->dispc_cinfo.pck_div = pckd;
+	ctx->dispc_cinfo.lck = lck;
+	ctx->dispc_cinfo.pck = pck;
 
-	dsi->timings = *timings;
+	*t = *ctx->config->timings;
+	t->pixel_clock = pck / 1000;
+	t->x_res = ctx->config->timings->x_res;
+	t->y_res = ctx->config->timings->y_res;
+	t->hsw = t->hfp = t->hbp = t->vsw = 1;
+	t->vfp = t->vbp = 0;
 
-	mutex_unlock(&dsi->lock);
+	return true;
 }
-EXPORT_SYMBOL(omapdss_dsi_set_timings);
 
-void omapdss_dsi_set_size(struct omap_dss_device *dssdev, u16 w, u16 h)
+static bool dsi_cm_calc_hsdiv_cb(int regm_dispc, unsigned long dispc,
+		void *data)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
-	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct dsi_clk_calc_ctx *ctx = data;
 
-	mutex_lock(&dsi->lock);
+	ctx->dsi_cinfo.regm_dispc = regm_dispc;
+	ctx->dsi_cinfo.dsi_pll_hsdiv_dispc_clk = dispc;
 
-	dsi->timings.x_res = w;
-	dsi->timings.y_res = h;
+	return dispc_div_calc(dispc, ctx->req_pck_min, ctx->req_pck_max,
+			dsi_cm_calc_dispc_cb, ctx);
+}
 
-	mutex_unlock(&dsi->lock);
+static bool dsi_cm_calc_pll_cb(int regn, int regm, unsigned long fint,
+		unsigned long pll, void *data)
+{
+	struct dsi_clk_calc_ctx *ctx = data;
+
+	ctx->dsi_cinfo.regn = regn;
+	ctx->dsi_cinfo.regm = regm;
+	ctx->dsi_cinfo.fint = fint;
+	ctx->dsi_cinfo.clkin4ddr = pll;
+
+	return dsi_hsdiv_calc(ctx->dsidev, pll, ctx->req_pck_min,
+			dsi_cm_calc_hsdiv_cb, ctx);
 }
-EXPORT_SYMBOL(omapdss_dsi_set_size);
 
-void omapdss_dsi_set_pixel_format(struct omap_dss_device *dssdev,
-		enum omap_dss_dsi_pixel_format fmt)
+static bool dsi_cm_calc(struct dsi_data *dsi,
+		const struct omap_dss_dsi_config *cfg,
+		struct dsi_clk_calc_ctx *ctx)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
-	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long clkin;
+	int bitspp, ndl;
+	unsigned long pll_min, pll_max;
+	unsigned long pck, txbyteclk;
 
-	mutex_lock(&dsi->lock);
+	clkin = clk_get_rate(dsi->sys_clk);
+	bitspp = dsi_get_pixel_size(cfg->pixel_format);
+	ndl = dsi->num_lanes_used - 1;
 
-	dsi->pix_fmt = fmt;
+	/*
+	 * Here we should calculate minimum txbyteclk to be able to send the
+	 * frame in time, and also to handle TE. That's not very simple, though,
+	 * especially as we go to LP between each pixel packet due to HW
+	 * "feature". So let's just estimate very roughly and multiply by 1.5.
+	 */
+	pck = cfg->timings->pixel_clock * 1000;
+	pck = pck * 3 / 2;
+	txbyteclk = pck * bitspp / 8 / ndl;
 
-	mutex_unlock(&dsi->lock);
+	memset(ctx, 0, sizeof(*ctx));
+	ctx->dsidev = dsi->pdev;
+	ctx->config = cfg;
+	ctx->req_pck_min = pck;
+	ctx->req_pck_nom = pck;
+	ctx->req_pck_max = pck * 3 / 2;
+	ctx->dsi_cinfo.clkin = clkin;
+
+	pll_min = max(cfg->hs_clk_min * 4, txbyteclk * 4 * 4);
+	pll_max = cfg->hs_clk_max * 4;
+
+	return dsi_pll_calc(dsi->pdev, clkin,
+			pll_min, pll_max,
+			dsi_cm_calc_pll_cb, ctx);
 }
-EXPORT_SYMBOL(omapdss_dsi_set_pixel_format);
 
-void omapdss_dsi_set_operation_mode(struct omap_dss_device *dssdev,
-		enum omap_dss_dsi_mode mode)
+static bool dsi_vm_calc_blanking(struct dsi_clk_calc_ctx *ctx)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
-	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(ctx->dsidev);
+	const struct omap_dss_dsi_config *cfg = ctx->config;
+	int bitspp = dsi_get_pixel_size(cfg->pixel_format);
+	int ndl = dsi->num_lanes_used - 1;
+	unsigned long hsclk = ctx->dsi_cinfo.clkin4ddr / 4;
+	unsigned long byteclk = hsclk / 4;
 
-	mutex_lock(&dsi->lock);
+	unsigned long dispc_pck, req_pck_min, req_pck_nom, req_pck_max;
+	int xres;
+	int panel_htot, panel_hbl; /* pixels */
+	int dispc_htot, dispc_hbl; /* pixels */
+	int dsi_htot, dsi_hact, dsi_hbl, hss, hse; /* byteclks */
+	int hfp, hsa, hbp;
+	const struct omap_video_timings *req_vm;
+	struct omap_video_timings *dispc_vm;
+	struct omap_dss_dsi_videomode_timings *dsi_vm;
+	u64 dsi_tput, dispc_tput;
 
-	dsi->mode = mode;
+	dsi_tput = (u64)byteclk * ndl * 8;
 
-	mutex_unlock(&dsi->lock);
+	req_vm = cfg->timings;
+	req_pck_min = ctx->req_pck_min;
+	req_pck_max = ctx->req_pck_max;
+	req_pck_nom = ctx->req_pck_nom;
+
+	dispc_pck = ctx->dispc_cinfo.pck;
+	dispc_tput = (u64)dispc_pck * bitspp;
+
+	xres = req_vm->x_res;
+
+	panel_hbl = req_vm->hfp + req_vm->hbp + req_vm->hsw;
+	panel_htot = xres + panel_hbl;
+
+	dsi_hact = DIV_ROUND_UP(DIV_ROUND_UP(xres * bitspp, 8) + 6, ndl);
+
+	/*
+	 * When there are no line buffers, DISPC and DSI must have the
+	 * same tput. Otherwise DISPC tput needs to be higher than DSI's.
+	 */
+	if (dsi->line_buffer_size < xres * bitspp / 8) {
+		if (dispc_tput != dsi_tput)
+			return false;
+	} else {
+		if (dispc_tput < dsi_tput)
+			return false;
+	}
+
+	/* DSI tput must be over the min requirement */
+	if (dsi_tput < (u64)bitspp * req_pck_min)
+		return false;
+
+	/* When non-burst mode, DSI tput must be below max requirement. */
+	if (cfg->trans_mode != OMAP_DSS_DSI_BURST_MODE) {
+		if (dsi_tput > (u64)bitspp * req_pck_max)
+			return false;
+	}
+
+	hss = DIV_ROUND_UP(4, ndl);
+
+	if (cfg->trans_mode == OMAP_DSS_DSI_PULSE_MODE) {
+		if (ndl == 3 && req_vm->hsw == 0)
+			hse = 1;
+		else
+			hse = DIV_ROUND_UP(4, ndl);
+	} else {
+		hse = 0;
+	}
+
+	/* DSI htot to match the panel's nominal pck */
+	dsi_htot = div64_u64((u64)panel_htot * byteclk, req_pck_nom);
+
+	/* fail if there would be no time for blanking */
+	if (dsi_htot < hss + hse + dsi_hact)
+		return false;
+
+	/* total DSI blanking needed to achieve panel's TL */
+	dsi_hbl = dsi_htot - dsi_hact;
+
+	/* DISPC htot to match the DSI TL */
+	dispc_htot = div64_u64((u64)dsi_htot * dispc_pck, byteclk);
+
+	/* verify that the DSI and DISPC TLs are the same */
+	if ((u64)dsi_htot * dispc_pck != (u64)dispc_htot * byteclk)
+		return false;
+
+	dispc_hbl = dispc_htot - xres;
+
+	/* setup DSI videomode */
+
+	dsi_vm = &ctx->dsi_vm;
+	memset(dsi_vm, 0, sizeof(*dsi_vm));
+
+	dsi_vm->hsclk = hsclk;
+
+	dsi_vm->ndl = ndl;
+	dsi_vm->bitspp = bitspp;
+
+	if (cfg->trans_mode != OMAP_DSS_DSI_PULSE_MODE) {
+		hsa = 0;
+	} else if (ndl == 3 && req_vm->hsw == 0) {
+		hsa = 0;
+	} else {
+		hsa = div64_u64((u64)req_vm->hsw * byteclk, req_pck_nom);
+		hsa = max(hsa - hse, 1);
+	}
+
+	hbp = div64_u64((u64)req_vm->hbp * byteclk, req_pck_nom);
+	hbp = max(hbp, 1);
+
+	hfp = dsi_hbl - (hss + hsa + hse + hbp);
+	if (hfp < 1) {
+		int t;
+		/* we need to take cycles from hbp */
+
+		t = 1 - hfp;
+		hbp = max(hbp - t, 1);
+		hfp = dsi_hbl - (hss + hsa + hse + hbp);
+
+		if (hfp < 1 && hsa > 0) {
+			/* we need to take cycles from hsa */
+			t = 1 - hfp;
+			hsa = max(hsa - t, 1);
+			hfp = dsi_hbl - (hss + hsa + hse + hbp);
+		}
+	}
+
+	if (hfp < 1)
+		return false;
+
+	dsi_vm->hss = hss;
+	dsi_vm->hsa = hsa;
+	dsi_vm->hse = hse;
+	dsi_vm->hbp = hbp;
+	dsi_vm->hact = xres;
+	dsi_vm->hfp = hfp;
+
+	dsi_vm->vsa = req_vm->vsw;
+	dsi_vm->vbp = req_vm->vbp;
+	dsi_vm->vact = req_vm->y_res;
+	dsi_vm->vfp = req_vm->vfp;
+
+	dsi_vm->trans_mode = cfg->trans_mode;
+
+	dsi_vm->blanking_mode = 0;
+	dsi_vm->hsa_blanking_mode = 1;
+	dsi_vm->hfp_blanking_mode = 1;
+	dsi_vm->hbp_blanking_mode = 1;
+
+	dsi_vm->ddr_clk_always_on = cfg->ddr_clk_always_on;
+	dsi_vm->window_sync = 4;
+
+	/* setup DISPC videomode */
+
+	dispc_vm = &ctx->dispc_vm;
+	*dispc_vm = *req_vm;
+	dispc_vm->pixel_clock = dispc_pck / 1000;
+
+	if (cfg->trans_mode == OMAP_DSS_DSI_PULSE_MODE) {
+		hsa = div64_u64((u64)req_vm->hsw * dispc_pck,
+				req_pck_nom);
+		hsa = max(hsa, 1);
+	} else {
+		hsa = 1;
+	}
+
+	hbp = div64_u64((u64)req_vm->hbp * dispc_pck, req_pck_nom);
+	hbp = max(hbp, 1);
+
+	hfp = dispc_hbl - hsa - hbp;
+	if (hfp < 1) {
+		int t;
+		/* we need to take cycles from hbp */
+
+		t = 1 - hfp;
+		hbp = max(hbp - t, 1);
+		hfp = dispc_hbl - hsa - hbp;
+
+		if (hfp < 1) {
+			/* we need to take cycles from hsa */
+			t = 1 - hfp;
+			hsa = max(hsa - t, 1);
+			hfp = dispc_hbl - hsa - hbp;
+		}
+	}
+
+	if (hfp < 1)
+		return false;
+
+	dispc_vm->hfp = hfp;
+	dispc_vm->hsw = hsa;
+	dispc_vm->hbp = hbp;
+
+	return true;
 }
-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)
+
+static bool dsi_vm_calc_dispc_cb(int lckd, int pckd, unsigned long lck,
+		unsigned long pck, void *data)
+{
+	struct dsi_clk_calc_ctx *ctx = data;
+
+	ctx->dispc_cinfo.lck_div = lckd;
+	ctx->dispc_cinfo.pck_div = pckd;
+	ctx->dispc_cinfo.lck = lck;
+	ctx->dispc_cinfo.pck = pck;
+
+	if (dsi_vm_calc_blanking(ctx) == false)
+		return false;
+
+#ifdef PRINT_VERBOSE_VM_TIMINGS
+	print_dispc_vm("dispc", &ctx->dispc_vm);
+	print_dsi_vm("dsi  ", &ctx->dsi_vm);
+	print_dispc_vm("req  ", ctx->config->timings);
+	print_dsi_dispc_vm("act  ", &ctx->dsi_vm);
+#endif
+
+	return true;
+}
+
+static bool dsi_vm_calc_hsdiv_cb(int regm_dispc, unsigned long dispc,
+		void *data)
+{
+	struct dsi_clk_calc_ctx *ctx = data;
+	unsigned long pck_max;
+
+	ctx->dsi_cinfo.regm_dispc = regm_dispc;
+	ctx->dsi_cinfo.dsi_pll_hsdiv_dispc_clk = dispc;
+
+	/*
+	 * In burst mode we can let the dispc pck be arbitrarily high, but it
+	 * limits our scaling abilities. So for now, don't aim too high.
+	 */
+
+	if (ctx->config->trans_mode == OMAP_DSS_DSI_BURST_MODE)
+		pck_max = ctx->req_pck_max + 10000000;
+	else
+		pck_max = ctx->req_pck_max;
+
+	return dispc_div_calc(dispc, ctx->req_pck_min, pck_max,
+			dsi_vm_calc_dispc_cb, ctx);
+}
+
+static bool dsi_vm_calc_pll_cb(int regn, int regm, unsigned long fint,
+		unsigned long pll, void *data)
+{
+	struct dsi_clk_calc_ctx *ctx = data;
+
+	ctx->dsi_cinfo.regn = regn;
+	ctx->dsi_cinfo.regm = regm;
+	ctx->dsi_cinfo.fint = fint;
+	ctx->dsi_cinfo.clkin4ddr = pll;
+
+	return dsi_hsdiv_calc(ctx->dsidev, pll, ctx->req_pck_min,
+			dsi_vm_calc_hsdiv_cb, ctx);
+}
+
+static bool dsi_vm_calc(struct dsi_data *dsi,
+		const struct omap_dss_dsi_config *cfg,
+		struct dsi_clk_calc_ctx *ctx)
+{
+	const struct omap_video_timings *t = cfg->timings;
+	unsigned long clkin;
+	unsigned long pll_min;
+	unsigned long pll_max;
+	int ndl = dsi->num_lanes_used - 1;
+	int bitspp = dsi_get_pixel_size(cfg->pixel_format);
+	unsigned long byteclk_min;
+
+	clkin = clk_get_rate(dsi->sys_clk);
+
+	memset(ctx, 0, sizeof(*ctx));
+	ctx->dsidev = dsi->pdev;
+	ctx->config = cfg;
+
+	ctx->dsi_cinfo.clkin = clkin;
+
+	/* these limits should come from the panel driver */
+	ctx->req_pck_min = t->pixel_clock * 1000 - 1000;
+	ctx->req_pck_nom = t->pixel_clock * 1000;
+	ctx->req_pck_max = t->pixel_clock * 1000 + 1000;
+
+	byteclk_min = div64_u64((u64)ctx->req_pck_min * bitspp, ndl * 8);
+	pll_min = max(cfg->hs_clk_min * 4, byteclk_min * 4 * 4);
+
+	if (cfg->trans_mode == OMAP_DSS_DSI_BURST_MODE) {
+		pll_max = cfg->hs_clk_max * 4;
+	} else {
+		unsigned long byteclk_max;
+		byteclk_max = div64_u64((u64)ctx->req_pck_max * bitspp,
+				ndl * 8);
+
+		pll_max = byteclk_max * 4 * 4;
+	}
+
+	return dsi_pll_calc(dsi->pdev, clkin,
+			pll_min, pll_max,
+			dsi_vm_calc_pll_cb, ctx);
+}
+
+int omapdss_dsi_set_config(struct omap_dss_device *dssdev,
+		const struct omap_dss_dsi_config *config)
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct dsi_clk_calc_ctx ctx;
+	bool ok;
+	int r;
 
 	mutex_lock(&dsi->lock);
 
-	dsi->vm_timings = *timings;
+	dsi->pix_fmt = config->pixel_format;
+	dsi->mode = config->mode;
+
+	if (config->mode == OMAP_DSS_DSI_VIDEO_MODE)
+		ok = dsi_vm_calc(dsi, config, &ctx);
+	else
+		ok = dsi_cm_calc(dsi, config, &ctx);
+
+	if (!ok) {
+		DSSERR("failed to find suitable DSI clock settings\n");
+		r = -EINVAL;
+		goto err;
+	}
+
+	dsi_pll_calc_dsi_fck(&ctx.dsi_cinfo);
+
+	r = dsi_lp_clock_calc(&ctx.dsi_cinfo, config->lp_clk_min,
+			config->lp_clk_max);
+	if (r) {
+		DSSERR("failed to find suitable DSI LP clock settings\n");
+		goto err;
+	}
+
+	dsi->user_dsi_cinfo = ctx.dsi_cinfo;
+	dsi->user_dispc_cinfo = ctx.dispc_cinfo;
+
+	dsi->timings = ctx.dispc_vm;
+	dsi->vm_timings = ctx.dsi_vm;
 
 	mutex_unlock(&dsi->lock);
+
+	return 0;
+err:
+	mutex_unlock(&dsi->lock);
+
+	return r;
 }
-EXPORT_SYMBOL(omapdss_dsi_set_videomode_timings);
+EXPORT_SYMBOL(omapdss_dsi_set_config);
+
+/*
+ * Return a hardcoded channel for the DSI output. This should work for
+ * current use cases, but this can be later expanded to either resolve
+ * the channel in some more dynamic manner, or get the channel as a user
+ * parameter.
+ */
+static enum omap_channel dsi_get_channel(int module_id)
+{
+	switch (omapdss_get_version()) {
+	case OMAPDSS_VER_OMAP24xx:
+		DSSWARN("DSI not supported\n");
+		return OMAP_DSS_CHANNEL_LCD;
+
+	case OMAPDSS_VER_OMAP34xx_ES1:
+	case OMAPDSS_VER_OMAP34xx_ES3:
+	case OMAPDSS_VER_OMAP3630:
+	case OMAPDSS_VER_AM35xx:
+		return OMAP_DSS_CHANNEL_LCD;
+
+	case OMAPDSS_VER_OMAP4430_ES1:
+	case OMAPDSS_VER_OMAP4430_ES2:
+	case OMAPDSS_VER_OMAP4:
+		switch (module_id) {
+		case 0:
+			return OMAP_DSS_CHANNEL_LCD;
+		case 1:
+			return OMAP_DSS_CHANNEL_LCD2;
+		default:
+			DSSWARN("unsupported module id\n");
+			return OMAP_DSS_CHANNEL_LCD;
+		}
+
+	case OMAPDSS_VER_OMAP5:
+		switch (module_id) {
+		case 0:
+			return OMAP_DSS_CHANNEL_LCD;
+		case 1:
+			return OMAP_DSS_CHANNEL_LCD3;
+		default:
+			DSSWARN("unsupported module id\n");
+			return OMAP_DSS_CHANNEL_LCD;
+		}
 
-static int __init dsi_init_display(struct omap_dss_device *dssdev)
+	default:
+		DSSWARN("unsupported DSS version\n");
+		return OMAP_DSS_CHANNEL_LCD;
+	}
+}
+
+static int dsi_init_display(struct omap_dss_device *dssdev)
 {
 	struct platform_device *dsidev =
 			dsi_get_dsidev_from_id(dssdev->phy.dsi.module);
@@ -5073,7 +5347,7 @@ static int dsi_get_clocks(struct platform_device *dsidev)
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	struct clk *clk;
 
-	clk = clk_get(&dsidev->dev, "fck");
+	clk = devm_clk_get(&dsidev->dev, "fck");
 	if (IS_ERR(clk)) {
 		DSSERR("can't get fck\n");
 		return PTR_ERR(clk);
@@ -5081,11 +5355,9 @@ static int dsi_get_clocks(struct platform_device *dsidev)
 
 	dsi->dss_clk = clk;
 
-	clk = clk_get(&dsidev->dev, "sys_clk");
+	clk = devm_clk_get(&dsidev->dev, "sys_clk");
 	if (IS_ERR(clk)) {
 		DSSERR("can't get sys_clk\n");
-		clk_put(dsi->dss_clk);
-		dsi->dss_clk = NULL;
 		return PTR_ERR(clk);
 	}
 
@@ -5094,17 +5366,7 @@ static int dsi_get_clocks(struct platform_device *dsidev)
 	return 0;
 }
 
-static void dsi_put_clocks(struct platform_device *dsidev)
-{
-	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
-
-	if (dsi->dss_clk)
-		clk_put(dsi->dss_clk);
-	if (dsi->sys_clk)
-		clk_put(dsi->sys_clk);
-}
-
-static struct omap_dss_device * __init dsi_find_dssdev(struct platform_device *pdev)
+static struct omap_dss_device *dsi_find_dssdev(struct platform_device *pdev)
 {
 	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
 	struct dsi_data *dsi = dsi_get_dsidrv_data(pdev);
@@ -5136,7 +5398,7 @@ static struct omap_dss_device * __init dsi_find_dssdev(struct platform_device *p
 	return def_dssdev;
 }
 
-static void __init dsi_probe_pdata(struct platform_device *dsidev)
+static int dsi_probe_pdata(struct platform_device *dsidev)
 {
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	struct omap_dss_device *plat_dssdev;
@@ -5146,11 +5408,11 @@ static void __init dsi_probe_pdata(struct platform_device *dsidev)
 	plat_dssdev = dsi_find_dssdev(dsidev);
 
 	if (!plat_dssdev)
-		return;
+		return 0;
 
 	dssdev = dss_alloc_and_init_device(&dsidev->dev);
 	if (!dssdev)
-		return;
+		return -ENOMEM;
 
 	dss_copy_device_pdata(dssdev, plat_dssdev);
 
@@ -5158,7 +5420,7 @@ static void __init dsi_probe_pdata(struct platform_device *dsidev)
 	if (r) {
 		DSSERR("device %s init failed: %d\n", dssdev->name, r);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
 
 	r = omapdss_output_set_device(&dsi->output, dssdev);
@@ -5166,7 +5428,7 @@ static void __init dsi_probe_pdata(struct platform_device *dsidev)
 		DSSERR("failed to connect output to new device: %s\n",
 				dssdev->name);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
 
 	r = dss_add_device(dssdev);
@@ -5174,11 +5436,13 @@ static void __init dsi_probe_pdata(struct platform_device *dsidev)
 		DSSERR("device %s register failed: %d\n", dssdev->name, r);
 		omapdss_output_unset_device(&dsi->output);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
+
+	return 0;
 }
 
-static void __init dsi_init_output(struct platform_device *dsidev)
+static void dsi_init_output(struct platform_device *dsidev)
 {
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	struct omap_dss_output *out = &dsi->output;
@@ -5188,11 +5452,13 @@ static void __init dsi_init_output(struct platform_device *dsidev)
 			OMAP_DSS_OUTPUT_DSI1 : OMAP_DSS_OUTPUT_DSI2;
 
 	out->type = OMAP_DISPLAY_TYPE_DSI;
+	out->name = dsi->module_id == 0 ? "dsi.0" : "dsi.1";
+	out->dispc_channel = dsi_get_channel(dsi->module_id);
 
 	dss_register_output(out);
 }
 
-static void __exit dsi_uninit_output(struct platform_device *dsidev)
+static void dsi_uninit_output(struct platform_device *dsidev)
 {
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	struct omap_dss_output *out = &dsi->output;
@@ -5201,7 +5467,7 @@ static void __exit dsi_uninit_output(struct platform_device *dsidev)
 }
 
 /* DSI1 HW IP initialisation */
-static int __init omap_dsihw_probe(struct platform_device *dsidev)
+static int omap_dsihw_probe(struct platform_device *dsidev)
 {
 	u32 rev;
 	int r, i;
@@ -5293,9 +5559,17 @@ static int __init omap_dsihw_probe(struct platform_device *dsidev)
 	else
 		dsi->num_lanes_supported = 3;
 
+	dsi->line_buffer_size = dsi_get_line_buf_size(dsidev);
+
 	dsi_init_output(dsidev);
 
-	dsi_probe_pdata(dsidev);
+	r = dsi_probe_pdata(dsidev);
+	if (r) {
+		dsi_runtime_put(dsidev);
+		dsi_uninit_output(dsidev);
+		pm_runtime_disable(&dsidev->dev);
+		return r;
+	}
 
 	dsi_runtime_put(dsidev);
 
@@ -5314,7 +5588,6 @@ static int __init omap_dsihw_probe(struct platform_device *dsidev)
 
 err_runtime_get:
 	pm_runtime_disable(&dsidev->dev);
-	dsi_put_clocks(dsidev);
 	return r;
 }
 
@@ -5330,8 +5603,6 @@ static int __exit omap_dsihw_remove(struct platform_device *dsidev)
 
 	pm_runtime_disable(&dsidev->dev);
 
-	dsi_put_clocks(dsidev);
-
 	if (dsi->vdds_dsi_reg != NULL) {
 		if (dsi->vdds_dsi_enabled) {
 			regulator_disable(dsi->vdds_dsi_reg);
@@ -5369,6 +5640,7 @@ static const struct dev_pm_ops dsi_pm_ops = {
 };
 
 static struct platform_driver omap_dsihw_driver = {
+	.probe		= omap_dsihw_probe,
 	.remove         = __exit_p(omap_dsihw_remove),
 	.driver         = {
 		.name   = "omapdss_dsi",
@@ -5379,7 +5651,7 @@ static struct platform_driver omap_dsihw_driver = {
 
 int __init dsi_init_platform_driver(void)
 {
-	return platform_driver_probe(&omap_dsihw_driver, omap_dsihw_probe);
+	return platform_driver_register(&omap_dsihw_driver);
 }
 
 void __exit dsi_uninit_platform_driver(void)
diff --git a/drivers/video/omap2/dss/dss.c b/drivers/video/omap2/dss/dss.c
index 054c2a22b3f1..94f66f9f10a3 100644
--- a/drivers/video/omap2/dss/dss.c
+++ b/drivers/video/omap2/dss/dss.c
@@ -473,6 +473,47 @@ int dss_calc_clock_rates(struct dss_clock_info *cinfo)
 	return 0;
 }
 
+bool dss_div_calc(unsigned long fck_min, dss_div_calc_func func, void *data)
+{
+	int fckd, fckd_start, fckd_stop;
+	unsigned long fck;
+	unsigned long fck_hw_max;
+	unsigned long fckd_hw_max;
+	unsigned long prate;
+	unsigned m;
+
+	if (dss.dpll4_m4_ck == NULL) {
+		/*
+		 * TODO: dss1_fclk can be changed on OMAP2, but the available
+		 * dividers are not continuous. We just use the pre-set rate for
+		 * now.
+		 */
+		fck = clk_get_rate(dss.dss_clk);
+		fckd = 1;
+		return func(fckd, fck, data);
+	}
+
+	fck_hw_max = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
+	fckd_hw_max = dss.feat->fck_div_max;
+
+	m = dss.feat->dss_fck_multiplier;
+	prate = dss_get_dpll4_rate();
+
+	fck_min = fck_min ? fck_min : 1;
+
+	fckd_start = min(prate * m / fck_min, fckd_hw_max);
+	fckd_stop = max(DIV_ROUND_UP(prate * m, fck_hw_max), 1ul);
+
+	for (fckd = fckd_start; fckd >= fckd_stop; --fckd) {
+		fck = prate / fckd * m;
+
+		if (func(fckd, fck, data))
+			return true;
+	}
+
+	return false;
+}
+
 int dss_set_clock_div(struct dss_clock_info *cinfo)
 {
 	if (dss.dpll4_m4_ck) {
@@ -482,7 +523,8 @@ int dss_set_clock_div(struct dss_clock_info *cinfo)
 		prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
 		DSSDBG("dpll4_m4 = %ld\n", prate);
 
-		r = clk_set_rate(dss.dpll4_m4_ck, prate / cinfo->fck_div);
+		r = clk_set_rate(dss.dpll4_m4_ck,
+				DIV_ROUND_UP(prate, cinfo->fck_div));
 		if (r)
 			return r;
 	} else {
@@ -492,7 +534,9 @@ int dss_set_clock_div(struct dss_clock_info *cinfo)
 
 	dss.dss_clk_rate = clk_get_rate(dss.dss_clk);
 
-	WARN_ONCE(dss.dss_clk_rate != cinfo->fck, "clk rate mismatch");
+	WARN_ONCE(dss.dss_clk_rate != cinfo->fck,
+			"clk rate mismatch: %lu != %lu", dss.dss_clk_rate,
+			cinfo->fck);
 
 	DSSDBG("fck = %ld (%d)\n", cinfo->fck, cinfo->fck_div);
 
@@ -542,121 +586,6 @@ static int dss_setup_default_clock(void)
 	return 0;
 }
 
-int dss_calc_clock_div(unsigned long req_pck, struct dss_clock_info *dss_cinfo,
-		struct dispc_clock_info *dispc_cinfo)
-{
-	unsigned long prate;
-	struct dss_clock_info best_dss;
-	struct dispc_clock_info best_dispc;
-
-	unsigned long fck, max_dss_fck;
-
-	u16 fck_div;
-
-	int match = 0;
-	int min_fck_per_pck;
-
-	prate = dss_get_dpll4_rate();
-
-	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 && 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;
-		return 0;
-	}
-
-	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:
-	memset(&best_dss, 0, sizeof(best_dss));
-	memset(&best_dispc, 0, sizeof(best_dispc));
-
-	if (dss.dpll4_m4_ck == NULL) {
-		struct dispc_clock_info cur_dispc;
-		/* XXX can we change the clock on omap2? */
-		fck = clk_get_rate(dss.dss_clk);
-		fck_div = 1;
-
-		dispc_find_clk_divs(req_pck, fck, &cur_dispc);
-		match = 1;
-
-		best_dss.fck = fck;
-		best_dss.fck_div = fck_div;
-
-		best_dispc = cur_dispc;
-
-		goto found;
-	} else {
-		for (fck_div = dss.feat->fck_div_max; fck_div > 0; --fck_div) {
-			struct dispc_clock_info cur_dispc;
-
-			fck = prate / fck_div * dss.feat->dss_fck_multiplier;
-
-			if (fck > max_dss_fck)
-				continue;
-
-			if (min_fck_per_pck &&
-					fck < req_pck * min_fck_per_pck)
-				continue;
-
-			match = 1;
-
-			dispc_find_clk_divs(req_pck, fck, &cur_dispc);
-
-			if (abs(cur_dispc.pck - req_pck) <
-					abs(best_dispc.pck - req_pck)) {
-
-				best_dss.fck = fck;
-				best_dss.fck_div = fck_div;
-
-				best_dispc = cur_dispc;
-
-				if (cur_dispc.pck == req_pck)
-					goto found;
-			}
-		}
-	}
-
-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;
-	}
-
-	if (dss_cinfo)
-		*dss_cinfo = best_dss;
-	if (dispc_cinfo)
-		*dispc_cinfo = best_dispc;
-
-	dss.cache_req_pck = req_pck;
-	dss.cache_prate = prate;
-	dss.cache_dss_cinfo = best_dss;
-	dss.cache_dispc_cinfo = best_dispc;
-
-	return 0;
-}
-
 void dss_set_venc_output(enum omap_dss_venc_type type)
 {
 	int l = 0;
@@ -767,13 +696,11 @@ int dss_dpi_select_source(enum omap_channel channel)
 static int dss_get_clocks(void)
 {
 	struct clk *clk;
-	int r;
 
-	clk = clk_get(&dss.pdev->dev, "fck");
+	clk = devm_clk_get(&dss.pdev->dev, "fck");
 	if (IS_ERR(clk)) {
 		DSSERR("can't get clock fck\n");
-		r = PTR_ERR(clk);
-		goto err;
+		return PTR_ERR(clk);
 	}
 
 	dss.dss_clk = clk;
@@ -782,8 +709,7 @@ static int dss_get_clocks(void)
 		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;
+			return PTR_ERR(clk);
 		}
 	} else {
 		clk = NULL;
@@ -792,21 +718,12 @@ static int dss_get_clocks(void)
 	dss.dpll4_m4_ck = clk;
 
 	return 0;
-
-err:
-	if (dss.dss_clk)
-		clk_put(dss.dss_clk);
-	if (dss.dpll4_m4_ck)
-		clk_put(dss.dpll4_m4_ck);
-
-	return r;
 }
 
 static void dss_put_clocks(void)
 {
 	if (dss.dpll4_m4_ck)
 		clk_put(dss.dpll4_m4_ck);
-	clk_put(dss.dss_clk);
 }
 
 static int dss_runtime_get(void)
diff --git a/drivers/video/omap2/dss/dss.h b/drivers/video/omap2/dss/dss.h
index 610c8e563daa..84758936429d 100644
--- a/drivers/video/omap2/dss/dss.h
+++ b/drivers/video/omap2/dss/dss.h
@@ -268,14 +268,21 @@ 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_calc_clock_div(unsigned long req_pck, struct dss_clock_info *dss_cinfo,
-		struct dispc_clock_info *dispc_cinfo);
+
+typedef bool (*dss_div_calc_func)(int fckd, unsigned long fck, void *data);
+bool dss_div_calc(unsigned long fck_min, dss_div_calc_func func, void *data);
 
 /* SDI */
 int sdi_init_platform_driver(void) __init;
 void sdi_uninit_platform_driver(void) __exit;
 
 /* DSI */
+
+typedef bool (*dsi_pll_calc_func)(int regn, int regm, unsigned long fint,
+		unsigned long pll, void *data);
+typedef bool (*dsi_hsdiv_calc_func)(int regm_dispc, unsigned long dispc,
+		void *data);
+
 #ifdef CONFIG_OMAP2_DSS_DSI
 
 struct dentry;
@@ -292,12 +299,17 @@ void dsi_dump_clocks(struct seq_file *s);
 void dsi_irq_handler(void);
 u8 dsi_get_pixel_size(enum omap_dss_dsi_pixel_format fmt);
 
+unsigned long dsi_get_pll_clkin(struct platform_device *dsidev);
+
+bool dsi_hsdiv_calc(struct platform_device *dsidev, unsigned long pll,
+		unsigned long out_min, dsi_hsdiv_calc_func func, void *data);
+bool dsi_pll_calc(struct platform_device *dsidev, unsigned long clkin,
+		unsigned long pll_min, unsigned long pll_max,
+		dsi_pll_calc_func func, void *data);
+
 unsigned long dsi_get_pll_hsdiv_dispc_rate(struct platform_device *dsidev);
 int dsi_pll_set_clock_div(struct platform_device *dsidev,
 		struct dsi_clock_info *cinfo);
-int dsi_pll_calc_clock_div_pck(struct platform_device *dsidev,
-		unsigned long req_pck, struct dsi_clock_info *cinfo,
-		struct dispc_clock_info *dispc_cinfo);
 int dsi_pll_init(struct platform_device *dsidev, bool enable_hsclk,
 		bool enable_hsdiv);
 void dsi_pll_uninit(struct platform_device *dsidev, bool disconnect_lanes);
@@ -328,14 +340,6 @@ static inline int dsi_pll_set_clock_div(struct platform_device *dsidev,
 	WARN("%s: DSI not compiled in\n", __func__);
 	return -ENODEV;
 }
-static inline int dsi_pll_calc_clock_div_pck(struct platform_device *dsidev,
-		unsigned long req_pck,
-		struct dsi_clock_info *dsi_cinfo,
-		struct dispc_clock_info *dispc_cinfo)
-{
-	WARN("%s: DSI not compiled in\n", __func__);
-	return -ENODEV;
-}
 static inline int dsi_pll_init(struct platform_device *dsidev,
 		bool enable_hsclk, bool enable_hsdiv)
 {
@@ -356,6 +360,27 @@ static inline struct platform_device *dsi_get_dsidev_from_id(int module)
 {
 	return NULL;
 }
+
+static inline unsigned long dsi_get_pll_clkin(struct platform_device *dsidev)
+{
+	return 0;
+}
+
+static inline bool dsi_hsdiv_calc(struct platform_device *dsidev,
+		unsigned long pll, unsigned long out_min,
+		dsi_hsdiv_calc_func func, void *data)
+{
+	return false;
+}
+
+static inline bool dsi_pll_calc(struct platform_device *dsidev,
+		unsigned long clkin,
+		unsigned long pll_min, unsigned long pll_max,
+		dsi_pll_calc_func func, void *data)
+{
+	return false;
+}
+
 #endif
 
 /* DPI */
@@ -376,11 +401,15 @@ void dispc_enable_fifomerge(bool enable);
 void dispc_enable_gamma_table(bool enable);
 void dispc_set_loadmode(enum omap_dss_load_mode mode);
 
+typedef bool (*dispc_div_calc_func)(int lckd, int pckd, unsigned long lck,
+		unsigned long pck, void *data);
+bool dispc_div_calc(unsigned long dispc,
+		unsigned long pck_min, unsigned long pck_max,
+		dispc_div_calc_func func, void *data);
+
 bool dispc_mgr_timings_ok(enum omap_channel channel,
 		const struct omap_video_timings *timings);
 unsigned long dispc_fclk_rate(void);
-void dispc_find_clk_divs(unsigned long req_pck, unsigned long fck,
-		struct dispc_clock_info *cinfo);
 int dispc_calc_clock_rates(unsigned long dispc_fclk_rate,
 		struct dispc_clock_info *cinfo);
 
diff --git a/drivers/video/omap2/dss/dss_features.c b/drivers/video/omap2/dss/dss_features.c
index 7f791aeda4d2..77dbe0cfb34c 100644
--- a/drivers/video/omap2/dss/dss_features.c
+++ b/drivers/video/omap2/dss/dss_features.c
@@ -414,7 +414,7 @@ static const char * const omap5_dss_clk_source_names[] = {
 };
 
 static const struct dss_param_range omap2_dss_param_range[] = {
-	[FEAT_PARAM_DSS_FCK]			= { 0, 173000000 },
+	[FEAT_PARAM_DSS_FCK]			= { 0, 133000000 },
 	[FEAT_PARAM_DSS_PCD]			= { 2, 255 },
 	[FEAT_PARAM_DSIPLL_REGN]		= { 0, 0 },
 	[FEAT_PARAM_DSIPLL_REGM]		= { 0, 0 },
@@ -459,15 +459,15 @@ static const struct dss_param_range omap4_dss_param_range[] = {
 };
 
 static const struct dss_param_range omap5_dss_param_range[] = {
-	[FEAT_PARAM_DSS_FCK]			= { 0, 200000000 },
+	[FEAT_PARAM_DSS_FCK]			= { 0, 209250000 },
 	[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_FINT]		= { 150000, 52000000 },
 	[FEAT_PARAM_DSIPLL_LPDIV]		= { 0, (1 << 13) - 1 },
-	[FEAT_PARAM_DSI_FCK]			= { 0, 170000000 },
+	[FEAT_PARAM_DSI_FCK]			= { 0, 209250000 },
 	[FEAT_PARAM_DOWNSCALE]			= { 1, 4 },
 	[FEAT_PARAM_LINEWIDTH]			= { 1, 2048 },
 };
diff --git a/drivers/video/omap2/dss/hdmi.c b/drivers/video/omap2/dss/hdmi.c
index 72923645dcce..17f4d55c621c 100644
--- a/drivers/video/omap2/dss/hdmi.c
+++ b/drivers/video/omap2/dss/hdmi.c
@@ -328,7 +328,7 @@ static void hdmi_runtime_put(void)
 	WARN_ON(r < 0 && r != -ENOSYS);
 }
 
-static int __init hdmi_init_display(struct omap_dss_device *dssdev)
+static int hdmi_init_display(struct omap_dss_device *dssdev)
 {
 	int r;
 
@@ -472,17 +472,12 @@ static void hdmi_compute_pll(struct omap_dss_device *dssdev, int phy,
 	 * Input clock is predivided by N + 1
 	 * out put of which is reference clk
 	 */
-	if (dssdev->clocks.hdmi.regn == 0)
-		pi->regn = HDMI_DEFAULT_REGN;
-	else
-		pi->regn = dssdev->clocks.hdmi.regn;
+
+	pi->regn = HDMI_DEFAULT_REGN;
 
 	refclk = clkin / pi->regn;
 
-	if (dssdev->clocks.hdmi.regm2 == 0)
-		pi->regm2 = HDMI_DEFAULT_REGM2;
-	else
-		pi->regm2 = dssdev->clocks.hdmi.regm2;
+	pi->regm2 = HDMI_DEFAULT_REGM2;
 
 	/*
 	 * multiplier is pixel_clk/ref_clk
@@ -804,7 +799,7 @@ static int hdmi_get_clocks(struct platform_device *pdev)
 {
 	struct clk *clk;
 
-	clk = clk_get(&pdev->dev, "sys_clk");
+	clk = devm_clk_get(&pdev->dev, "sys_clk");
 	if (IS_ERR(clk)) {
 		DSSERR("can't get sys_clk\n");
 		return PTR_ERR(clk);
@@ -815,12 +810,6 @@ static int hdmi_get_clocks(struct platform_device *pdev)
 	return 0;
 }
 
-static void hdmi_put_clocks(void)
-{
-	if (hdmi.sys_clk)
-		clk_put(hdmi.sys_clk);
-}
-
 #if defined(CONFIG_OMAP4_DSS_HDMI_AUDIO)
 int hdmi_compute_acr(u32 sample_freq, u32 *n, u32 *cts)
 {
@@ -965,7 +954,7 @@ int hdmi_audio_config(struct omap_dss_audio *audio)
 
 #endif
 
-static struct omap_dss_device * __init hdmi_find_dssdev(struct platform_device *pdev)
+static struct omap_dss_device *hdmi_find_dssdev(struct platform_device *pdev)
 {
 	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
 	const char *def_disp_name = omapdss_get_default_display_name();
@@ -993,7 +982,7 @@ static struct omap_dss_device * __init hdmi_find_dssdev(struct platform_device *
 	return def_dssdev;
 }
 
-static void __init hdmi_probe_pdata(struct platform_device *pdev)
+static int hdmi_probe_pdata(struct platform_device *pdev)
 {
 	struct omap_dss_device *plat_dssdev;
 	struct omap_dss_device *dssdev;
@@ -1003,11 +992,11 @@ static void __init hdmi_probe_pdata(struct platform_device *pdev)
 	plat_dssdev = hdmi_find_dssdev(pdev);
 
 	if (!plat_dssdev)
-		return;
+		return 0;
 
 	dssdev = dss_alloc_and_init_device(&pdev->dev);
 	if (!dssdev)
-		return;
+		return -ENOMEM;
 
 	dss_copy_device_pdata(dssdev, plat_dssdev);
 
@@ -1017,13 +1006,11 @@ static void __init hdmi_probe_pdata(struct platform_device *pdev)
 	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;
+		return r;
 	}
 
 	r = omapdss_output_set_device(&hdmi.output, dssdev);
@@ -1031,7 +1018,7 @@ static void __init hdmi_probe_pdata(struct platform_device *pdev)
 		DSSERR("failed to connect output to new device: %s\n",
 				dssdev->name);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
 
 	r = dss_add_device(dssdev);
@@ -1040,17 +1027,21 @@ static void __init hdmi_probe_pdata(struct platform_device *pdev)
 		omapdss_output_unset_device(&hdmi.output);
 		hdmi_uninit_display(dssdev);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
+
+	return 0;
 }
 
-static void __init hdmi_init_output(struct platform_device *pdev)
+static void 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;
+	out->name = "hdmi.0";
+	out->dispc_channel = OMAP_DSS_CHANNEL_DIGIT;
 
 	dss_register_output(out);
 }
@@ -1063,7 +1054,7 @@ static void __exit hdmi_uninit_output(struct platform_device *pdev)
 }
 
 /* HDMI HW IP initialisation */
-static int __init omapdss_hdmihw_probe(struct platform_device *pdev)
+static int omapdss_hdmihw_probe(struct platform_device *pdev)
 {
 	struct resource *res;
 	int r;
@@ -1097,23 +1088,25 @@ static int __init omapdss_hdmihw_probe(struct platform_device *pdev)
 	hdmi.ip_data.pll_offset = HDMI_PLLCTRL;
 	hdmi.ip_data.phy_offset = HDMI_PHY;
 
+	hdmi_init_output(pdev);
+
 	r = hdmi_panel_init();
 	if (r) {
 		DSSERR("can't init panel\n");
-		goto err_panel_init;
+		return r;
 	}
 
 	dss_debugfs_create_file("hdmi", hdmi_dump_regs);
 
-	hdmi_init_output(pdev);
-
-	hdmi_probe_pdata(pdev);
+	r = hdmi_probe_pdata(pdev);
+	if (r) {
+		hdmi_panel_exit();
+		hdmi_uninit_output(pdev);
+		pm_runtime_disable(&pdev->dev);
+		return r;
+	}
 
 	return 0;
-
-err_panel_init:
-	hdmi_put_clocks();
-	return r;
 }
 
 static int __exit hdmi_remove_child(struct device *dev, void *data)
@@ -1135,8 +1128,6 @@ static int __exit omapdss_hdmihw_remove(struct platform_device *pdev)
 
 	pm_runtime_disable(&pdev->dev);
 
-	hdmi_put_clocks();
-
 	return 0;
 }
 
@@ -1168,6 +1159,7 @@ static const struct dev_pm_ops hdmi_pm_ops = {
 };
 
 static struct platform_driver omapdss_hdmihw_driver = {
+	.probe		= omapdss_hdmihw_probe,
 	.remove         = __exit_p(omapdss_hdmihw_remove),
 	.driver         = {
 		.name   = "omapdss_hdmi",
@@ -1178,7 +1170,7 @@ static struct platform_driver omapdss_hdmihw_driver = {
 
 int __init hdmi_init_platform_driver(void)
 {
-	return platform_driver_probe(&omapdss_hdmihw_driver, omapdss_hdmihw_probe);
+	return platform_driver_register(&omapdss_hdmihw_driver);
 }
 
 void __exit hdmi_uninit_platform_driver(void)
diff --git a/drivers/video/omap2/dss/output.c b/drivers/video/omap2/dss/output.c
index 79dea1a1a732..5214df63e0a9 100644
--- a/drivers/video/omap2/dss/output.c
+++ b/drivers/video/omap2/dss/output.c
@@ -113,6 +113,7 @@ struct omap_dss_output *omap_dss_get_output(enum omap_dss_output_id id)
 
 	return NULL;
 }
+EXPORT_SYMBOL(omap_dss_get_output);
 
 static const struct dss_mgr_ops *dss_mgr_ops;
 
diff --git a/drivers/video/omap2/dss/rfbi.c b/drivers/video/omap2/dss/rfbi.c
index e903dd3f54d9..1a17dd1447dc 100644
--- a/drivers/video/omap2/dss/rfbi.c
+++ b/drivers/video/omap2/dss/rfbi.c
@@ -943,13 +943,13 @@ void omapdss_rfbi_display_disable(struct omap_dss_device *dssdev)
 }
 EXPORT_SYMBOL(omapdss_rfbi_display_disable);
 
-static int __init rfbi_init_display(struct omap_dss_device *dssdev)
+static int rfbi_init_display(struct omap_dss_device *dssdev)
 {
 	rfbi.dssdev[dssdev->phy.rfbi.channel] = dssdev;
 	return 0;
 }
 
-static struct omap_dss_device * __init rfbi_find_dssdev(struct platform_device *pdev)
+static struct omap_dss_device *rfbi_find_dssdev(struct platform_device *pdev)
 {
 	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
 	const char *def_disp_name = omapdss_get_default_display_name();
@@ -977,7 +977,7 @@ static struct omap_dss_device * __init rfbi_find_dssdev(struct platform_device *
 	return def_dssdev;
 }
 
-static void __init rfbi_probe_pdata(struct platform_device *rfbidev)
+static int rfbi_probe_pdata(struct platform_device *rfbidev)
 {
 	struct omap_dss_device *plat_dssdev;
 	struct omap_dss_device *dssdev;
@@ -986,11 +986,11 @@ static void __init rfbi_probe_pdata(struct platform_device *rfbidev)
 	plat_dssdev = rfbi_find_dssdev(rfbidev);
 
 	if (!plat_dssdev)
-		return;
+		return 0;
 
 	dssdev = dss_alloc_and_init_device(&rfbidev->dev);
 	if (!dssdev)
-		return;
+		return -ENOMEM;
 
 	dss_copy_device_pdata(dssdev, plat_dssdev);
 
@@ -998,7 +998,7 @@ static void __init rfbi_probe_pdata(struct platform_device *rfbidev)
 	if (r) {
 		DSSERR("device %s init failed: %d\n", dssdev->name, r);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
 
 	r = omapdss_output_set_device(&rfbi.output, dssdev);
@@ -1006,7 +1006,7 @@ static void __init rfbi_probe_pdata(struct platform_device *rfbidev)
 		DSSERR("failed to connect output to new device: %s\n",
 				dssdev->name);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
 
 	r = dss_add_device(dssdev);
@@ -1014,17 +1014,21 @@ static void __init rfbi_probe_pdata(struct platform_device *rfbidev)
 		DSSERR("device %s register failed: %d\n", dssdev->name, r);
 		omapdss_output_unset_device(&rfbi.output);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
+
+	return 0;
 }
 
-static void __init rfbi_init_output(struct platform_device *pdev)
+static void 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;
+	out->name = "rfbi.0";
+	out->dispc_channel = OMAP_DSS_CHANNEL_LCD;
 
 	dss_register_output(out);
 }
@@ -1037,7 +1041,7 @@ static void __exit rfbi_uninit_output(struct platform_device *pdev)
 }
 
 /* RFBI HW IP initialisation */
-static int __init omap_rfbihw_probe(struct platform_device *pdev)
+static int omap_rfbihw_probe(struct platform_device *pdev)
 {
 	u32 rev;
 	struct resource *rfbi_mem;
@@ -1089,7 +1093,12 @@ static int __init omap_rfbihw_probe(struct platform_device *pdev)
 
 	rfbi_init_output(pdev);
 
-	rfbi_probe_pdata(pdev);
+	r = rfbi_probe_pdata(pdev);
+	if (r) {
+		rfbi_uninit_output(pdev);
+		pm_runtime_disable(&pdev->dev);
+		return r;
+	}
 
 	return 0;
 
@@ -1133,6 +1142,7 @@ static const struct dev_pm_ops rfbi_pm_ops = {
 };
 
 static struct platform_driver omap_rfbihw_driver = {
+	.probe		= omap_rfbihw_probe,
 	.remove         = __exit_p(omap_rfbihw_remove),
 	.driver         = {
 		.name   = "omapdss_rfbi",
@@ -1143,7 +1153,7 @@ static struct platform_driver omap_rfbihw_driver = {
 
 int __init rfbi_init_platform_driver(void)
 {
-	return platform_driver_probe(&omap_rfbihw_driver, omap_rfbihw_probe);
+	return platform_driver_register(&omap_rfbihw_driver);
 }
 
 void __exit rfbi_uninit_platform_driver(void)
diff --git a/drivers/video/omap2/dss/sdi.c b/drivers/video/omap2/dss/sdi.c
index 62b5374ce438..0bcd30272f69 100644
--- a/drivers/video/omap2/dss/sdi.c
+++ b/drivers/video/omap2/dss/sdi.c
@@ -41,6 +41,72 @@ static struct {
 	struct omap_dss_output output;
 } sdi;
 
+struct sdi_clk_calc_ctx {
+	unsigned long pck_min, pck_max;
+
+	struct dss_clock_info dss_cinfo;
+	struct dispc_clock_info dispc_cinfo;
+};
+
+static bool dpi_calc_dispc_cb(int lckd, int pckd, unsigned long lck,
+		unsigned long pck, void *data)
+{
+	struct sdi_clk_calc_ctx *ctx = data;
+
+	ctx->dispc_cinfo.lck_div = lckd;
+	ctx->dispc_cinfo.pck_div = pckd;
+	ctx->dispc_cinfo.lck = lck;
+	ctx->dispc_cinfo.pck = pck;
+
+	return true;
+}
+
+static bool dpi_calc_dss_cb(int fckd, unsigned long fck, void *data)
+{
+	struct sdi_clk_calc_ctx *ctx = data;
+
+	ctx->dss_cinfo.fck = fck;
+	ctx->dss_cinfo.fck_div = fckd;
+
+	return dispc_div_calc(fck, ctx->pck_min, ctx->pck_max,
+			dpi_calc_dispc_cb, ctx);
+}
+
+static int sdi_calc_clock_div(unsigned long pclk,
+		struct dss_clock_info *dss_cinfo,
+		struct dispc_clock_info *dispc_cinfo)
+{
+	int i;
+	struct sdi_clk_calc_ctx ctx;
+
+	/*
+	 * DSS fclk gives us very few possibilities, so finding a good pixel
+	 * clock may not be possible. We try multiple times to find the clock,
+	 * each time widening the pixel clock range we look for, up to
+	 * +/- 1MHz.
+	 */
+
+	for (i = 0; i < 10; ++i) {
+		bool ok;
+
+		memset(&ctx, 0, sizeof(ctx));
+		if (pclk > 1000 * i * i * i)
+			ctx.pck_min = max(pclk - 1000 * i * i * i, 0lu);
+		else
+			ctx.pck_min = 0;
+		ctx.pck_max = pclk + 1000 * i * i * i;
+
+		ok = dss_div_calc(ctx.pck_min, dpi_calc_dss_cb, &ctx);
+		if (ok) {
+			*dss_cinfo = ctx.dss_cinfo;
+			*dispc_cinfo = ctx.dispc_cinfo;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
 static void sdi_config_lcd_manager(struct omap_dss_device *dssdev)
 {
 	struct omap_overlay_manager *mgr = dssdev->output->manager;
@@ -88,7 +154,7 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 	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);
+	r = sdi_calc_clock_div(t->pixel_clock * 1000, &dss_cinfo, &dispc_cinfo);
 	if (r)
 		goto err_calc_clock_div;
 
@@ -182,7 +248,7 @@ void omapdss_sdi_set_datapairs(struct omap_dss_device *dssdev, int datapairs)
 }
 EXPORT_SYMBOL(omapdss_sdi_set_datapairs);
 
-static int __init sdi_init_display(struct omap_dss_device *dssdev)
+static int sdi_init_display(struct omap_dss_device *dssdev)
 {
 	DSSDBG("SDI init\n");
 
@@ -202,7 +268,7 @@ static int __init sdi_init_display(struct omap_dss_device *dssdev)
 	return 0;
 }
 
-static struct omap_dss_device * __init sdi_find_dssdev(struct platform_device *pdev)
+static struct omap_dss_device *sdi_find_dssdev(struct platform_device *pdev)
 {
 	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
 	const char *def_disp_name = omapdss_get_default_display_name();
@@ -230,7 +296,7 @@ static struct omap_dss_device * __init sdi_find_dssdev(struct platform_device *p
 	return def_dssdev;
 }
 
-static void __init sdi_probe_pdata(struct platform_device *sdidev)
+static int sdi_probe_pdata(struct platform_device *sdidev)
 {
 	struct omap_dss_device *plat_dssdev;
 	struct omap_dss_device *dssdev;
@@ -239,11 +305,11 @@ static void __init sdi_probe_pdata(struct platform_device *sdidev)
 	plat_dssdev = sdi_find_dssdev(sdidev);
 
 	if (!plat_dssdev)
-		return;
+		return 0;
 
 	dssdev = dss_alloc_and_init_device(&sdidev->dev);
 	if (!dssdev)
-		return;
+		return -ENOMEM;
 
 	dss_copy_device_pdata(dssdev, plat_dssdev);
 
@@ -251,7 +317,7 @@ static void __init sdi_probe_pdata(struct platform_device *sdidev)
 	if (r) {
 		DSSERR("device %s init failed: %d\n", dssdev->name, r);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
 
 	r = omapdss_output_set_device(&sdi.output, dssdev);
@@ -259,7 +325,7 @@ static void __init sdi_probe_pdata(struct platform_device *sdidev)
 		DSSERR("failed to connect output to new device: %s\n",
 				dssdev->name);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
 
 	r = dss_add_device(dssdev);
@@ -267,17 +333,21 @@ static void __init sdi_probe_pdata(struct platform_device *sdidev)
 		DSSERR("device %s register failed: %d\n", dssdev->name, r);
 		omapdss_output_unset_device(&sdi.output);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
+
+	return 0;
 }
 
-static void __init sdi_init_output(struct platform_device *pdev)
+static void 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;
+	out->name = "sdi.0";
+	out->dispc_channel = OMAP_DSS_CHANNEL_LCD;
 
 	dss_register_output(out);
 }
@@ -289,11 +359,17 @@ static void __exit sdi_uninit_output(struct platform_device *pdev)
 	dss_unregister_output(out);
 }
 
-static int __init omap_sdi_probe(struct platform_device *pdev)
+static int omap_sdi_probe(struct platform_device *pdev)
 {
+	int r;
+
 	sdi_init_output(pdev);
 
-	sdi_probe_pdata(pdev);
+	r = sdi_probe_pdata(pdev);
+	if (r) {
+		sdi_uninit_output(pdev);
+		return r;
+	}
 
 	return 0;
 }
@@ -308,6 +384,7 @@ static int __exit omap_sdi_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver omap_sdi_driver = {
+	.probe		= omap_sdi_probe,
 	.remove         = __exit_p(omap_sdi_remove),
 	.driver         = {
 		.name   = "omapdss_sdi",
@@ -317,7 +394,7 @@ static struct platform_driver omap_sdi_driver = {
 
 int __init sdi_init_platform_driver(void)
 {
-	return platform_driver_probe(&omap_sdi_driver, omap_sdi_probe);
+	return platform_driver_register(&omap_sdi_driver);
 }
 
 void __exit sdi_uninit_platform_driver(void)
diff --git a/drivers/video/omap2/dss/venc.c b/drivers/video/omap2/dss/venc.c
index 006caf3cb509..74fdb3ee209e 100644
--- a/drivers/video/omap2/dss/venc.c
+++ b/drivers/video/omap2/dss/venc.c
@@ -519,10 +519,6 @@ int omapdss_venc_display_enable(struct omap_dss_device *dssdev)
 		goto err0;
 	}
 
-	if (dssdev->platform_enable)
-		dssdev->platform_enable(dssdev);
-
-
 	r = venc_power_on(dssdev);
 	if (r)
 		goto err1;
@@ -533,8 +529,6 @@ int omapdss_venc_display_enable(struct omap_dss_device *dssdev)
 
 	return 0;
 err1:
-	if (dssdev->platform_disable)
-		dssdev->platform_disable(dssdev);
 	omap_dss_stop_device(dssdev);
 err0:
 	mutex_unlock(&venc.venc_lock);
@@ -551,9 +545,6 @@ void omapdss_venc_display_disable(struct omap_dss_device *dssdev)
 
 	omap_dss_stop_device(dssdev);
 
-	if (dssdev->platform_disable)
-		dssdev->platform_disable(dssdev);
-
 	mutex_unlock(&venc.venc_lock);
 }
 
@@ -642,7 +633,7 @@ void omapdss_venc_invert_vid_out_polarity(struct omap_dss_device *dssdev,
 	mutex_unlock(&venc.venc_lock);
 }
 
-static int __init venc_init_display(struct omap_dss_device *dssdev)
+static int venc_init_display(struct omap_dss_device *dssdev)
 {
 	DSSDBG("init_display\n");
 
@@ -721,7 +712,7 @@ static int venc_get_clocks(struct platform_device *pdev)
 	struct clk *clk;
 
 	if (dss_has_feature(FEAT_VENC_REQUIRES_TV_DAC_CLK)) {
-		clk = clk_get(&pdev->dev, "tv_dac_clk");
+		clk = devm_clk_get(&pdev->dev, "tv_dac_clk");
 		if (IS_ERR(clk)) {
 			DSSERR("can't get tv_dac_clk\n");
 			return PTR_ERR(clk);
@@ -735,13 +726,7 @@ static int venc_get_clocks(struct platform_device *pdev)
 	return 0;
 }
 
-static void venc_put_clocks(void)
-{
-	if (venc.tv_dac_clk)
-		clk_put(venc.tv_dac_clk);
-}
-
-static struct omap_dss_device * __init venc_find_dssdev(struct platform_device *pdev)
+static struct omap_dss_device *venc_find_dssdev(struct platform_device *pdev)
 {
 	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
 	const char *def_disp_name = omapdss_get_default_display_name();
@@ -769,7 +754,7 @@ static struct omap_dss_device * __init venc_find_dssdev(struct platform_device *
 	return def_dssdev;
 }
 
-static void __init venc_probe_pdata(struct platform_device *vencdev)
+static int venc_probe_pdata(struct platform_device *vencdev)
 {
 	struct omap_dss_device *plat_dssdev;
 	struct omap_dss_device *dssdev;
@@ -778,21 +763,19 @@ static void __init venc_probe_pdata(struct platform_device *vencdev)
 	plat_dssdev = venc_find_dssdev(vencdev);
 
 	if (!plat_dssdev)
-		return;
+		return 0;
 
 	dssdev = dss_alloc_and_init_device(&vencdev->dev);
 	if (!dssdev)
-		return;
+		return -ENOMEM;
 
 	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;
+		return r;
 	}
 
 	r = omapdss_output_set_device(&venc.output, dssdev);
@@ -800,7 +783,7 @@ static void __init venc_probe_pdata(struct platform_device *vencdev)
 		DSSERR("failed to connect output to new device: %s\n",
 				dssdev->name);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
 
 	r = dss_add_device(dssdev);
@@ -808,17 +791,21 @@ static void __init venc_probe_pdata(struct platform_device *vencdev)
 		DSSERR("device %s register failed: %d\n", dssdev->name, r);
 		omapdss_output_unset_device(&venc.output);
 		dss_put_device(dssdev);
-		return;
+		return r;
 	}
+
+	return 0;
 }
 
-static void __init venc_init_output(struct platform_device *pdev)
+static void 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;
+	out->name = "venc.0";
+	out->dispc_channel = OMAP_DSS_CHANNEL_DIGIT;
 
 	dss_register_output(out);
 }
@@ -831,7 +818,7 @@ static void __exit venc_uninit_output(struct platform_device *pdev)
 }
 
 /* VENC HW IP initialisation */
-static int __init omap_venchw_probe(struct platform_device *pdev)
+static int omap_venchw_probe(struct platform_device *pdev)
 {
 	u8 rev_id;
 	struct resource *venc_mem;
@@ -879,14 +866,19 @@ static int __init omap_venchw_probe(struct platform_device *pdev)
 
 	venc_init_output(pdev);
 
-	venc_probe_pdata(pdev);
+	r = venc_probe_pdata(pdev);
+	if (r) {
+		venc_panel_exit();
+		venc_uninit_output(pdev);
+		pm_runtime_disable(&pdev->dev);
+		return r;
+	}
 
 	return 0;
 
 err_panel_init:
 err_runtime_get:
 	pm_runtime_disable(&pdev->dev);
-	venc_put_clocks();
 	return r;
 }
 
@@ -904,7 +896,6 @@ static int __exit omap_venchw_remove(struct platform_device *pdev)
 	venc_uninit_output(pdev);
 
 	pm_runtime_disable(&pdev->dev);
-	venc_put_clocks();
 
 	return 0;
 }
@@ -939,6 +930,7 @@ static const struct dev_pm_ops venc_pm_ops = {
 };
 
 static struct platform_driver omap_venchw_driver = {
+	.probe		= omap_venchw_probe,
 	.remove         = __exit_p(omap_venchw_remove),
 	.driver         = {
 		.name   = "omapdss_venc",
@@ -949,7 +941,7 @@ static struct platform_driver omap_venchw_driver = {
 
 int __init venc_init_platform_driver(void)
 {
-	return platform_driver_probe(&omap_venchw_driver, omap_venchw_probe);
+	return platform_driver_register(&omap_venchw_driver);
 }
 
 void __exit venc_uninit_platform_driver(void)
diff --git a/drivers/video/omap2/omapfb/omapfb-main.c b/drivers/video/omap2/omapfb/omapfb-main.c
index 717f13a93351..c84bb8a4d0c4 100644
--- a/drivers/video/omap2/omapfb/omapfb-main.c
+++ b/drivers/video/omap2/omapfb/omapfb-main.c
@@ -2372,7 +2372,7 @@ static int omapfb_init_connections(struct omapfb2_device *fbdev,
 		struct omap_dss_device *dssdev = fbdev->displays[i].dssdev;
 		struct omap_dss_output *out = dssdev->output;
 
-		mgr = omap_dss_get_overlay_manager(dssdev->channel);
+		mgr = omap_dss_get_overlay_manager(out->dispc_channel);
 
 		if (!mgr || !out)
 			continue;
@@ -2406,7 +2406,7 @@ static int omapfb_init_connections(struct omapfb2_device *fbdev,
 	return 0;
 }
 
-static int __init omapfb_probe(struct platform_device *pdev)
+static int omapfb_probe(struct platform_device *pdev)
 {
 	struct omapfb2_device *fbdev = NULL;
 	int r = 0;
@@ -2468,7 +2468,7 @@ static int __init omapfb_probe(struct platform_device *pdev)
 
 	if (fbdev->num_displays == 0) {
 		dev_err(&pdev->dev, "no displays\n");
-		r = -EINVAL;
+		r = -EPROBE_DEFER;
 		goto cleanup;
 	}
 
@@ -2579,6 +2579,7 @@ static int __exit omapfb_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver omapfb_driver = {
+	.probe		= omapfb_probe,
 	.remove         = __exit_p(omapfb_remove),
 	.driver         = {
 		.name   = "omapfb",
@@ -2586,36 +2587,13 @@ static struct platform_driver omapfb_driver = {
 	},
 };
 
-static int __init omapfb_init(void)
-{
-	DBG("omapfb_init\n");
-
-	if (platform_driver_probe(&omapfb_driver, omapfb_probe)) {
-		printk(KERN_ERR "failed to register omapfb driver\n");
-		return -ENODEV;
-	}
-
-	return 0;
-}
-
-static void __exit omapfb_exit(void)
-{
-	DBG("omapfb_exit\n");
-	platform_driver_unregister(&omapfb_driver);
-}
-
 module_param_named(mode, def_mode, charp, 0);
 module_param_named(vram, def_vram, charp, 0);
 module_param_named(rotate, def_rotate, int, 0);
 module_param_named(vrfb, def_vrfb, bool, 0);
 module_param_named(mirror, def_mirror, bool, 0);
 
-/* late_initcall to let panel/ctrl drivers loaded first.
- * I guess better option would be a more dynamic approach,
- * so that omapfb reacts to new panels when they are loaded */
-late_initcall(omapfb_init);
-/*module_init(omapfb_init);*/
-module_exit(omapfb_exit);
+module_platform_driver(omapfb_driver);
 
 MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@nokia.com>");
 MODULE_DESCRIPTION("OMAP2/3 Framebuffer");
diff --git a/drivers/video/pxa3xx-gcu.c b/drivers/video/pxa3xx-gcu.c
index 6c984eacc7e3..97563c55af63 100644
--- a/drivers/video/pxa3xx-gcu.c
+++ b/drivers/video/pxa3xx-gcu.c
@@ -101,7 +101,6 @@ struct pxa3xx_gcu_priv {
 	dma_addr_t		  shared_phys;
 	struct resource		 *resource_mem;
 	struct miscdevice	  misc_dev;
-	struct file_operations	  misc_fops;
 	wait_queue_head_t	  wait_idle;
 	wait_queue_head_t	  wait_free;
 	spinlock_t		  spinlock;
@@ -369,15 +368,20 @@ pxa3xx_gcu_wait_free(struct pxa3xx_gcu_priv *priv)
 
 /* Misc device layer */
 
+static inline struct pxa3xx_gcu_priv *file_dev(struct file *file)
+{
+	struct miscdevice *dev = file->private_data;
+	return container_of(dev, struct pxa3xx_gcu_priv, misc_dev);
+}
+
 static ssize_t
-pxa3xx_gcu_misc_write(struct file *filp, const char *buff,
+pxa3xx_gcu_misc_write(struct file *file, const char *buff,
 		      size_t count, loff_t *offp)
 {
 	int ret;
 	unsigned long flags;
 	struct pxa3xx_gcu_batch	*buffer;
-	struct pxa3xx_gcu_priv *priv =
-		container_of(filp->f_op, struct pxa3xx_gcu_priv, misc_fops);
+	struct pxa3xx_gcu_priv *priv = file_dev(file);
 
 	int words = count / 4;
 
@@ -450,11 +454,10 @@ pxa3xx_gcu_misc_write(struct file *filp, const char *buff,
 
 
 static long
-pxa3xx_gcu_misc_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+pxa3xx_gcu_misc_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
 	unsigned long flags;
-	struct pxa3xx_gcu_priv *priv =
-		container_of(filp->f_op, struct pxa3xx_gcu_priv, misc_fops);
+	struct pxa3xx_gcu_priv *priv = file_dev(file);
 
 	switch (cmd) {
 	case PXA3XX_GCU_IOCTL_RESET:
@@ -471,11 +474,10 @@ pxa3xx_gcu_misc_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 }
 
 static int
-pxa3xx_gcu_misc_mmap(struct file *filp, struct vm_area_struct *vma)
+pxa3xx_gcu_misc_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	unsigned int size = vma->vm_end - vma->vm_start;
-	struct pxa3xx_gcu_priv *priv =
-		container_of(filp->f_op, struct pxa3xx_gcu_priv, misc_fops);
+	struct pxa3xx_gcu_priv *priv = file_dev(file);
 
 	switch (vma->vm_pgoff) {
 	case 0:
@@ -574,6 +576,13 @@ free_buffers(struct platform_device *dev,
 	priv->free = NULL;
 }
 
+static const struct file_operations misc_fops = {
+	.owner	= THIS_MODULE,
+	.write	= pxa3xx_gcu_misc_write,
+	.unlocked_ioctl = pxa3xx_gcu_misc_ioctl,
+	.mmap	= pxa3xx_gcu_misc_mmap
+};
+
 static int pxa3xx_gcu_probe(struct platform_device *dev)
 {
 	int i, ret, irq;
@@ -601,14 +610,9 @@ static int pxa3xx_gcu_probe(struct platform_device *dev)
 	 * container_of(). This isn't really necessary as we have a fixed minor
 	 * number anyway, but this is to avoid statics. */
 
-	priv->misc_fops.owner	= THIS_MODULE;
-	priv->misc_fops.write	= pxa3xx_gcu_misc_write;
-	priv->misc_fops.unlocked_ioctl = pxa3xx_gcu_misc_ioctl;
-	priv->misc_fops.mmap	= pxa3xx_gcu_misc_mmap;
-
 	priv->misc_dev.minor	= MISCDEV_MINOR,
 	priv->misc_dev.name	= DRV_NAME,
-	priv->misc_dev.fops	= &priv->misc_fops,
+	priv->misc_dev.fops	= &misc_fops,
 
 	/* register misc device */
 	ret = misc_register(&priv->misc_dev);
diff --git a/drivers/virtio/virtio_balloon.c b/drivers/virtio/virtio_balloon.c
index 8dab163c5ef0..bd3ae324a1a2 100644
--- a/drivers/virtio/virtio_balloon.c
+++ b/drivers/virtio/virtio_balloon.c
@@ -108,7 +108,7 @@ static void tell_host(struct virtio_balloon *vb, struct virtqueue *vq)
 	sg_init_one(&sg, vb->pfns, sizeof(vb->pfns[0]) * vb->num_pfns);
 
 	/* We should always be able to add one buffer to an empty queue. */
-	if (virtqueue_add_buf(vq, &sg, 1, 0, vb, GFP_KERNEL) < 0)
+	if (virtqueue_add_outbuf(vq, &sg, 1, vb, GFP_KERNEL) < 0)
 		BUG();
 	virtqueue_kick(vq);
 
@@ -256,7 +256,7 @@ static void stats_handle_request(struct virtio_balloon *vb)
 	if (!virtqueue_get_buf(vq, &len))
 		return;
 	sg_init_one(&sg, vb->stats, sizeof(vb->stats));
-	if (virtqueue_add_buf(vq, &sg, 1, 0, vb, GFP_KERNEL) < 0)
+	if (virtqueue_add_outbuf(vq, &sg, 1, vb, GFP_KERNEL) < 0)
 		BUG();
 	virtqueue_kick(vq);
 }
@@ -341,7 +341,7 @@ static int init_vqs(struct virtio_balloon *vb)
 		 * use it to signal us later.
 		 */
 		sg_init_one(&sg, vb->stats, sizeof vb->stats);
-		if (virtqueue_add_buf(vb->stats_vq, &sg, 1, 0, vb, GFP_KERNEL)
+		if (virtqueue_add_outbuf(vb->stats_vq, &sg, 1, vb, GFP_KERNEL)
 		    < 0)
 			BUG();
 		virtqueue_kick(vb->stats_vq);
diff --git a/drivers/virtio/virtio_ring.c b/drivers/virtio/virtio_ring.c
index ffd7e7da5d3b..5217baf5528c 100644
--- a/drivers/virtio/virtio_ring.c
+++ b/drivers/virtio/virtio_ring.c
@@ -24,27 +24,6 @@
 #include <linux/module.h>
 #include <linux/hrtimer.h>
 
-/* virtio guest is communicating with a virtual "device" that actually runs on
- * a host processor.  Memory barriers are used to control SMP effects. */
-#ifdef CONFIG_SMP
-/* Where possible, use SMP barriers which are more lightweight than mandatory
- * barriers, because mandatory barriers control MMIO effects on accesses
- * through relaxed memory I/O windows (which virtio-pci does not use). */
-#define virtio_mb(vq) \
-	do { if ((vq)->weak_barriers) smp_mb(); else mb(); } while(0)
-#define virtio_rmb(vq) \
-	do { if ((vq)->weak_barriers) smp_rmb(); else rmb(); } while(0)
-#define virtio_wmb(vq) \
-	do { if ((vq)->weak_barriers) smp_wmb(); else wmb(); } while(0)
-#else
-/* We must force memory ordering even if guest is UP since host could be
- * running on another CPU, but SMP barriers are defined to barrier() in that
- * configuration. So fall back to mandatory barriers instead. */
-#define virtio_mb(vq) mb()
-#define virtio_rmb(vq) rmb()
-#define virtio_wmb(vq) wmb()
-#endif
-
 #ifdef DEBUG
 /* For development, we want to crash whenever the ring is screwed. */
 #define BAD_RING(_vq, fmt, args...)				\
@@ -119,16 +98,36 @@ struct vring_virtqueue
 
 #define to_vvq(_vq) container_of(_vq, struct vring_virtqueue, vq)
 
+static inline struct scatterlist *sg_next_chained(struct scatterlist *sg,
+						  unsigned int *count)
+{
+	return sg_next(sg);
+}
+
+static inline struct scatterlist *sg_next_arr(struct scatterlist *sg,
+					      unsigned int *count)
+{
+	if (--(*count) == 0)
+		return NULL;
+	return sg + 1;
+}
+
 /* Set up an indirect table of descriptors and add it to the queue. */
-static int vring_add_indirect(struct vring_virtqueue *vq,
-			      struct scatterlist sg[],
-			      unsigned int out,
-			      unsigned int in,
-			      gfp_t gfp)
+static inline int vring_add_indirect(struct vring_virtqueue *vq,
+				     struct scatterlist *sgs[],
+				     struct scatterlist *(*next)
+				       (struct scatterlist *, unsigned int *),
+				     unsigned int total_sg,
+				     unsigned int total_out,
+				     unsigned int total_in,
+				     unsigned int out_sgs,
+				     unsigned int in_sgs,
+				     gfp_t gfp)
 {
 	struct vring_desc *desc;
 	unsigned head;
-	int i;
+	struct scatterlist *sg;
+	int i, n;
 
 	/*
 	 * We require lowmem mappings for the descriptors because
@@ -137,25 +136,31 @@ static int vring_add_indirect(struct vring_virtqueue *vq,
 	 */
 	gfp &= ~(__GFP_HIGHMEM | __GFP_HIGH);
 
-	desc = kmalloc((out + in) * sizeof(struct vring_desc), gfp);
+	desc = kmalloc(total_sg * sizeof(struct vring_desc), gfp);
 	if (!desc)
 		return -ENOMEM;
 
-	/* Transfer entries from the sg list into the indirect page */
-	for (i = 0; i < out; i++) {
-		desc[i].flags = VRING_DESC_F_NEXT;
-		desc[i].addr = sg_phys(sg);
-		desc[i].len = sg->length;
-		desc[i].next = i+1;
-		sg++;
+	/* Transfer entries from the sg lists into the indirect page */
+	i = 0;
+	for (n = 0; n < out_sgs; n++) {
+		for (sg = sgs[n]; sg; sg = next(sg, &total_out)) {
+			desc[i].flags = VRING_DESC_F_NEXT;
+			desc[i].addr = sg_phys(sg);
+			desc[i].len = sg->length;
+			desc[i].next = i+1;
+			i++;
+		}
 	}
-	for (; i < (out + in); i++) {
-		desc[i].flags = VRING_DESC_F_NEXT|VRING_DESC_F_WRITE;
-		desc[i].addr = sg_phys(sg);
-		desc[i].len = sg->length;
-		desc[i].next = i+1;
-		sg++;
+	for (; n < (out_sgs + in_sgs); n++) {
+		for (sg = sgs[n]; sg; sg = next(sg, &total_in)) {
+			desc[i].flags = VRING_DESC_F_NEXT|VRING_DESC_F_WRITE;
+			desc[i].addr = sg_phys(sg);
+			desc[i].len = sg->length;
+			desc[i].next = i+1;
+			i++;
+		}
 	}
+	BUG_ON(i != total_sg);
 
 	/* Last one doesn't continue. */
 	desc[i-1].flags &= ~VRING_DESC_F_NEXT;
@@ -176,29 +181,20 @@ static int vring_add_indirect(struct vring_virtqueue *vq,
 	return head;
 }
 
-/**
- * virtqueue_add_buf - expose buffer to other end
- * @vq: the struct virtqueue we're talking about.
- * @sg: the description of the buffer(s).
- * @out_num: the number of sg readable by other side
- * @in_num: the number of sg which are writable (after readable ones)
- * @data: the token identifying the buffer.
- * @gfp: how to do memory allocations (if necessary).
- *
- * Caller must ensure we don't call this with other virtqueue operations
- * at the same time (except where noted).
- *
- * Returns zero or a negative error (ie. ENOSPC, ENOMEM).
- */
-int virtqueue_add_buf(struct virtqueue *_vq,
-		      struct scatterlist sg[],
-		      unsigned int out,
-		      unsigned int in,
-		      void *data,
-		      gfp_t gfp)
+static inline int virtqueue_add(struct virtqueue *_vq,
+				struct scatterlist *sgs[],
+				struct scatterlist *(*next)
+				  (struct scatterlist *, unsigned int *),
+				unsigned int total_out,
+				unsigned int total_in,
+				unsigned int out_sgs,
+				unsigned int in_sgs,
+				void *data,
+				gfp_t gfp)
 {
 	struct vring_virtqueue *vq = to_vvq(_vq);
-	unsigned int i, avail, uninitialized_var(prev);
+	struct scatterlist *sg;
+	unsigned int i, n, avail, uninitialized_var(prev), total_sg;
 	int head;
 
 	START_USE(vq);
@@ -218,46 +214,54 @@ int virtqueue_add_buf(struct virtqueue *_vq,
 	}
 #endif
 
+	total_sg = total_in + total_out;
+
 	/* If the host supports indirect descriptor tables, and we have multiple
 	 * buffers, then go indirect. FIXME: tune this threshold */
-	if (vq->indirect && (out + in) > 1 && vq->vq.num_free) {
-		head = vring_add_indirect(vq, sg, out, in, gfp);
+	if (vq->indirect && total_sg > 1 && vq->vq.num_free) {
+		head = vring_add_indirect(vq, sgs, next, total_sg, total_out,
+					  total_in,
+					  out_sgs, in_sgs, gfp);
 		if (likely(head >= 0))
 			goto add_head;
 	}
 
-	BUG_ON(out + in > vq->vring.num);
-	BUG_ON(out + in == 0);
+	BUG_ON(total_sg > vq->vring.num);
+	BUG_ON(total_sg == 0);
 
-	if (vq->vq.num_free < out + in) {
+	if (vq->vq.num_free < total_sg) {
 		pr_debug("Can't add buf len %i - avail = %i\n",
-			 out + in, vq->vq.num_free);
+			 total_sg, vq->vq.num_free);
 		/* FIXME: for historical reasons, we force a notify here if
 		 * there are outgoing parts to the buffer.  Presumably the
 		 * host should service the ring ASAP. */
-		if (out)
+		if (out_sgs)
 			vq->notify(&vq->vq);
 		END_USE(vq);
 		return -ENOSPC;
 	}
 
 	/* We're about to use some buffers from the free list. */
-	vq->vq.num_free -= out + in;
-
-	head = vq->free_head;
-	for (i = vq->free_head; out; i = vq->vring.desc[i].next, out--) {
-		vq->vring.desc[i].flags = VRING_DESC_F_NEXT;
-		vq->vring.desc[i].addr = sg_phys(sg);
-		vq->vring.desc[i].len = sg->length;
-		prev = i;
-		sg++;
+	vq->vq.num_free -= total_sg;
+
+	head = i = vq->free_head;
+	for (n = 0; n < out_sgs; n++) {
+		for (sg = sgs[n]; sg; sg = next(sg, &total_out)) {
+			vq->vring.desc[i].flags = VRING_DESC_F_NEXT;
+			vq->vring.desc[i].addr = sg_phys(sg);
+			vq->vring.desc[i].len = sg->length;
+			prev = i;
+			i = vq->vring.desc[i].next;
+		}
 	}
-	for (; in; i = vq->vring.desc[i].next, in--) {
-		vq->vring.desc[i].flags = VRING_DESC_F_NEXT|VRING_DESC_F_WRITE;
-		vq->vring.desc[i].addr = sg_phys(sg);
-		vq->vring.desc[i].len = sg->length;
-		prev = i;
-		sg++;
+	for (; n < (out_sgs + in_sgs); n++) {
+		for (sg = sgs[n]; sg; sg = next(sg, &total_in)) {
+			vq->vring.desc[i].flags = VRING_DESC_F_NEXT|VRING_DESC_F_WRITE;
+			vq->vring.desc[i].addr = sg_phys(sg);
+			vq->vring.desc[i].len = sg->length;
+			prev = i;
+			i = vq->vring.desc[i].next;
+		}
 	}
 	/* Last one doesn't continue. */
 	vq->vring.desc[prev].flags &= ~VRING_DESC_F_NEXT;
@@ -276,7 +280,7 @@ add_head:
 
 	/* Descriptors and available array need to be set before we expose the
 	 * new available array entries. */
-	virtio_wmb(vq);
+	virtio_wmb(vq->weak_barriers);
 	vq->vring.avail->idx++;
 	vq->num_added++;
 
@@ -290,9 +294,122 @@ add_head:
 
 	return 0;
 }
+
+/**
+ * virtqueue_add_buf - expose buffer to other end
+ * @vq: the struct virtqueue we're talking about.
+ * @sg: the description of the buffer(s).
+ * @out_num: the number of sg readable by other side
+ * @in_num: the number of sg which are writable (after readable ones)
+ * @data: the token identifying the buffer.
+ * @gfp: how to do memory allocations (if necessary).
+ *
+ * Caller must ensure we don't call this with other virtqueue operations
+ * at the same time (except where noted).
+ *
+ * Returns zero or a negative error (ie. ENOSPC, ENOMEM).
+ */
+int virtqueue_add_buf(struct virtqueue *_vq,
+		      struct scatterlist sg[],
+		      unsigned int out,
+		      unsigned int in,
+		      void *data,
+		      gfp_t gfp)
+{
+	struct scatterlist *sgs[2];
+
+	sgs[0] = sg;
+	sgs[1] = sg + out;
+
+	return virtqueue_add(_vq, sgs, sg_next_arr,
+			     out, in, out ? 1 : 0, in ? 1 : 0, data, gfp);
+}
 EXPORT_SYMBOL_GPL(virtqueue_add_buf);
 
 /**
+ * virtqueue_add_sgs - expose buffers to other end
+ * @vq: the struct virtqueue we're talking about.
+ * @sgs: array of terminated scatterlists.
+ * @out_num: the number of scatterlists readable by other side
+ * @in_num: the number of scatterlists which are writable (after readable ones)
+ * @data: the token identifying the buffer.
+ * @gfp: how to do memory allocations (if necessary).
+ *
+ * Caller must ensure we don't call this with other virtqueue operations
+ * at the same time (except where noted).
+ *
+ * Returns zero or a negative error (ie. ENOSPC, ENOMEM).
+ */
+int virtqueue_add_sgs(struct virtqueue *_vq,
+		      struct scatterlist *sgs[],
+		      unsigned int out_sgs,
+		      unsigned int in_sgs,
+		      void *data,
+		      gfp_t gfp)
+{
+	unsigned int i, total_out, total_in;
+
+	/* Count them first. */
+	for (i = total_out = total_in = 0; i < out_sgs; i++) {
+		struct scatterlist *sg;
+		for (sg = sgs[i]; sg; sg = sg_next(sg))
+			total_out++;
+	}
+	for (; i < out_sgs + in_sgs; i++) {
+		struct scatterlist *sg;
+		for (sg = sgs[i]; sg; sg = sg_next(sg))
+			total_in++;
+	}
+	return virtqueue_add(_vq, sgs, sg_next_chained,
+			     total_out, total_in, out_sgs, in_sgs, data, gfp);
+}
+EXPORT_SYMBOL_GPL(virtqueue_add_sgs);
+
+/**
+ * virtqueue_add_outbuf - expose output buffers to other end
+ * @vq: the struct virtqueue we're talking about.
+ * @sgs: array of scatterlists (need not be terminated!)
+ * @num: the number of scatterlists readable by other side
+ * @data: the token identifying the buffer.
+ * @gfp: how to do memory allocations (if necessary).
+ *
+ * Caller must ensure we don't call this with other virtqueue operations
+ * at the same time (except where noted).
+ *
+ * Returns zero or a negative error (ie. ENOSPC, ENOMEM).
+ */
+int virtqueue_add_outbuf(struct virtqueue *vq,
+			 struct scatterlist sg[], unsigned int num,
+			 void *data,
+			 gfp_t gfp)
+{
+	return virtqueue_add(vq, &sg, sg_next_arr, num, 0, 1, 0, data, gfp);
+}
+EXPORT_SYMBOL_GPL(virtqueue_add_outbuf);
+
+/**
+ * virtqueue_add_inbuf - expose input buffers to other end
+ * @vq: the struct virtqueue we're talking about.
+ * @sgs: array of scatterlists (need not be terminated!)
+ * @num: the number of scatterlists writable by other side
+ * @data: the token identifying the buffer.
+ * @gfp: how to do memory allocations (if necessary).
+ *
+ * Caller must ensure we don't call this with other virtqueue operations
+ * at the same time (except where noted).
+ *
+ * Returns zero or a negative error (ie. ENOSPC, ENOMEM).
+ */
+int virtqueue_add_inbuf(struct virtqueue *vq,
+			struct scatterlist sg[], unsigned int num,
+			void *data,
+			gfp_t gfp)
+{
+	return virtqueue_add(vq, &sg, sg_next_arr, 0, num, 0, 1, data, gfp);
+}
+EXPORT_SYMBOL_GPL(virtqueue_add_inbuf);
+
+/**
  * virtqueue_kick_prepare - first half of split virtqueue_kick call.
  * @vq: the struct virtqueue
  *
@@ -312,7 +429,7 @@ bool virtqueue_kick_prepare(struct virtqueue *_vq)
 	START_USE(vq);
 	/* We need to expose available array entries before checking avail
 	 * event. */
-	virtio_mb(vq);
+	virtio_mb(vq->weak_barriers);
 
 	old = vq->vring.avail->idx - vq->num_added;
 	new = vq->vring.avail->idx;
@@ -436,7 +553,7 @@ void *virtqueue_get_buf(struct virtqueue *_vq, unsigned int *len)
 	}
 
 	/* Only get used array entries after they have been exposed by host. */
-	virtio_rmb(vq);
+	virtio_rmb(vq->weak_barriers);
 
 	last_used = (vq->last_used_idx & (vq->vring.num - 1));
 	i = vq->vring.used->ring[last_used].id;
@@ -460,7 +577,7 @@ void *virtqueue_get_buf(struct virtqueue *_vq, unsigned int *len)
 	 * the read in the next get_buf call. */
 	if (!(vq->vring.avail->flags & VRING_AVAIL_F_NO_INTERRUPT)) {
 		vring_used_event(&vq->vring) = vq->last_used_idx;
-		virtio_mb(vq);
+		virtio_mb(vq->weak_barriers);
 	}
 
 #ifdef DEBUG
@@ -513,7 +630,7 @@ bool virtqueue_enable_cb(struct virtqueue *_vq)
 	 * entry. Always do both to keep code simple. */
 	vq->vring.avail->flags &= ~VRING_AVAIL_F_NO_INTERRUPT;
 	vring_used_event(&vq->vring) = vq->last_used_idx;
-	virtio_mb(vq);
+	virtio_mb(vq->weak_barriers);
 	if (unlikely(more_used(vq))) {
 		END_USE(vq);
 		return false;
@@ -553,7 +670,7 @@ bool virtqueue_enable_cb_delayed(struct virtqueue *_vq)
 	/* TODO: tune this threshold */
 	bufs = (u16)(vq->vring.avail->idx - vq->last_used_idx) * 3 / 4;
 	vring_used_event(&vq->vring) = vq->last_used_idx + bufs;
-	virtio_mb(vq);
+	virtio_mb(vq->weak_barriers);
 	if (unlikely((u16)(vq->vring.used->idx - vq->last_used_idx) > bufs)) {
 		END_USE(vq);
 		return false;
diff --git a/drivers/w1/masters/Kconfig b/drivers/w1/masters/Kconfig
index e8ca63a82b97..2bd1257dcc1c 100644
--- a/drivers/w1/masters/Kconfig
+++ b/drivers/w1/masters/Kconfig
@@ -50,7 +50,7 @@ config W1_MASTER_DS1WM
 
 config W1_MASTER_GPIO
 	tristate "GPIO 1-wire busmaster"
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  Say Y here if you want to communicate with your 1-wire devices using
 	  GPIO pins. This driver uses the GPIO API to control the wire.
diff --git a/drivers/w1/slaves/w1_bq27000.c b/drivers/w1/slaves/w1_bq27000.c
index 773dca5beafe..afbefed5f2c9 100644
--- a/drivers/w1/slaves/w1_bq27000.c
+++ b/drivers/w1/slaves/w1_bq27000.c
@@ -57,6 +57,8 @@ static int w1_bq27000_add_slave(struct w1_slave *sl)
 	ret = platform_device_add_data(pdev,
 				       &bq27000_battery_info,
 				       sizeof(bq27000_battery_info));
+	if (ret)
+		goto pdev_add_failed;
 	pdev->dev.parent = &sl->dev;
 
 	ret = platform_device_add(pdev);
@@ -68,7 +70,7 @@ static int w1_bq27000_add_slave(struct w1_slave *sl)
 	goto success;
 
 pdev_add_failed:
-	platform_device_unregister(pdev);
+	platform_device_put(pdev);
 success:
 	return ret;
 }
diff --git a/drivers/w1/slaves/w1_ds2760.c b/drivers/w1/slaves/w1_ds2760.c
index aa7bd5fa2fa8..e86a69dc411e 100644
--- a/drivers/w1/slaves/w1_ds2760.c
+++ b/drivers/w1/slaves/w1_ds2760.c
@@ -148,8 +148,9 @@ static int w1_ds2760_add_slave(struct w1_slave *sl)
 	goto success;
 
 bin_attr_failed:
+	platform_device_del(pdev);
 pdev_add_failed:
-	platform_device_unregister(pdev);
+	platform_device_put(pdev);
 pdev_alloc_failed:
 	ida_simple_remove(&bat_ida, id);
 noid:
diff --git a/drivers/w1/slaves/w1_ds2780.c b/drivers/w1/slaves/w1_ds2780.c
index 7b09307de0ef..98ed9c49cf50 100644
--- a/drivers/w1/slaves/w1_ds2780.c
+++ b/drivers/w1/slaves/w1_ds2780.c
@@ -141,8 +141,9 @@ static int w1_ds2780_add_slave(struct w1_slave *sl)
 	return 0;
 
 bin_attr_failed:
+	platform_device_del(pdev);
 pdev_add_failed:
-	platform_device_unregister(pdev);
+	platform_device_put(pdev);
 pdev_alloc_failed:
 	ida_simple_remove(&bat_ida, id);
 noid:
diff --git a/drivers/w1/slaves/w1_ds2781.c b/drivers/w1/slaves/w1_ds2781.c
index 877daf74159c..5140d7be67ab 100644
--- a/drivers/w1/slaves/w1_ds2781.c
+++ b/drivers/w1/slaves/w1_ds2781.c
@@ -139,8 +139,9 @@ static int w1_ds2781_add_slave(struct w1_slave *sl)
 	return 0;
 
 bin_attr_failed:
+	platform_device_del(pdev);
 pdev_add_failed:
-	platform_device_unregister(pdev);
+	platform_device_put(pdev);
 pdev_alloc_failed:
 	ida_simple_remove(&bat_ida, id);
 noid:
diff --git a/drivers/watchdog/ath79_wdt.c b/drivers/watchdog/ath79_wdt.c
index 898799074a13..d184c48a0482 100644
--- a/drivers/watchdog/ath79_wdt.c
+++ b/drivers/watchdog/ath79_wdt.c
@@ -253,11 +253,9 @@ static int ath79_wdt_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	wdt_base = devm_request_and_ioremap(&pdev->dev, res);
-	if (!wdt_base) {
-		dev_err(&pdev->dev, "unable to remap memory region\n");
-		return -ENOMEM;
-	}
+	wdt_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(wdt_base))
+		return PTR_ERR(wdt_base);
 
 	wdt_clk = devm_clk_get(&pdev->dev, "wdt");
 	if (IS_ERR(wdt_clk))
diff --git a/drivers/watchdog/davinci_wdt.c b/drivers/watchdog/davinci_wdt.c
index 7df1fdca9e78..100d4fbfde2a 100644
--- a/drivers/watchdog/davinci_wdt.c
+++ b/drivers/watchdog/davinci_wdt.c
@@ -27,6 +27,7 @@
 #include <linux/device.h>
 #include <linux/clk.h>
 #include <linux/slab.h>
+#include <linux/err.h>
 
 #define MODULE_NAME "DAVINCI-WDT: "
 
@@ -221,11 +222,9 @@ static int davinci_wdt_probe(struct platform_device *pdev)
 		return -ENOENT;
 	}
 
-	wdt_base = devm_request_and_ioremap(dev, wdt_mem);
-	if (!wdt_base) {
-		dev_err(dev, "ioremap failed\n");
-		return -EADDRNOTAVAIL;
-	}
+	wdt_base = devm_ioremap_resource(dev, wdt_mem);
+	if (IS_ERR(wdt_base))
+		return PTR_ERR(wdt_base);
 
 	ret = misc_register(&davinci_wdt_miscdev);
 	if (ret < 0) {
diff --git a/drivers/watchdog/s3c2410_wdt.c b/drivers/watchdog/s3c2410_wdt.c
index c1a221cbeae4..ee03135f5abd 100644
--- a/drivers/watchdog/s3c2410_wdt.c
+++ b/drivers/watchdog/s3c2410_wdt.c
@@ -330,10 +330,9 @@ static int s3c2410wdt_probe(struct platform_device *pdev)
 	}
 
 	/* get the memory region for the watchdog timer */
-	wdt_base = devm_request_and_ioremap(dev, wdt_mem);
-	if (wdt_base == NULL) {
-		dev_err(dev, "failed to devm_request_and_ioremap() region\n");
-		ret = -ENOMEM;
+	wdt_base = devm_ioremap_resource(dev, wdt_mem);
+	if (IS_ERR(wdt_base)) {
+		ret = PTR_ERR(wdt_base);
 		goto err;
 	}
 
diff --git a/drivers/watchdog/shwdt.c b/drivers/watchdog/shwdt.c
index 6a89e4045fbd..6185af2b3310 100644
--- a/drivers/watchdog/shwdt.c
+++ b/drivers/watchdog/shwdt.c
@@ -34,6 +34,7 @@
 #include <linux/slab.h>
 #include <linux/io.h>
 #include <linux/clk.h>
+#include <linux/err.h>
 #include <asm/watchdog.h>
 
 #define DRV_NAME "sh-wdt"
@@ -249,9 +250,9 @@ static int sh_wdt_probe(struct platform_device *pdev)
 		wdt->clk = NULL;
 	}
 
-	wdt->base = devm_request_and_ioremap(wdt->dev, res);
-	if (unlikely(!wdt->base)) {
-		rc = -EADDRNOTAVAIL;
+	wdt->base = devm_ioremap_resource(wdt->dev, res);
+	if (IS_ERR(wdt->base)) {
+		rc = PTR_ERR(wdt->base);
 		goto err;
 	}
 
diff --git a/drivers/watchdog/watchdog_dev.c b/drivers/watchdog/watchdog_dev.c
index 08b48bbf9f4b..faf4e189fe42 100644
--- a/drivers/watchdog/watchdog_dev.c
+++ b/drivers/watchdog/watchdog_dev.c
@@ -523,6 +523,7 @@ int watchdog_dev_register(struct watchdog_device *watchdog)
 	int err, devno;
 
 	if (watchdog->id == 0) {
+		old_wdd = watchdog;
 		watchdog_miscdev.parent = watchdog->parent;
 		err = misc_register(&watchdog_miscdev);
 		if (err != 0) {
@@ -531,9 +532,9 @@ int watchdog_dev_register(struct watchdog_device *watchdog)
 			if (err == -EBUSY)
 				pr_err("%s: a legacy watchdog module is probably present.\n",
 					watchdog->info->identity);
+			old_wdd = NULL;
 			return err;
 		}
-		old_wdd = watchdog;
 	}
 
 	/* Fill in the data structures */
diff --git a/drivers/xen/Kconfig b/drivers/xen/Kconfig
index 67af155cf602..dd4d9cb86243 100644
--- a/drivers/xen/Kconfig
+++ b/drivers/xen/Kconfig
@@ -145,9 +145,9 @@ config SWIOTLB_XEN
 	select SWIOTLB
 
 config XEN_TMEM
-	bool
+	tristate
 	depends on !ARM
-	default y if (CLEANCACHE || FRONTSWAP)
+	default m if (CLEANCACHE || FRONTSWAP)
 	help
 	  Shim to interface in-kernel Transcendent Memory hooks
 	  (e.g. cleancache and frontswap) to Xen tmem hypercalls.
diff --git a/drivers/xen/tmem.c b/drivers/xen/tmem.c
index 3ee836d42581..e3600be4e7fa 100644
--- a/drivers/xen/tmem.c
+++ b/drivers/xen/tmem.c
@@ -5,6 +5,7 @@
  * Author: Dan Magenheimer
  */
 
+#include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/types.h>
 #include <linux/init.h>
@@ -128,6 +129,7 @@ static int xen_tmem_flush_object(u32 pool_id, struct tmem_oid oid)
 	return xen_tmem_op(TMEM_FLUSH_OBJECT, pool_id, oid, 0, 0, 0, 0, 0);
 }
 
+#ifndef CONFIG_XEN_TMEM_MODULE
 bool __read_mostly tmem_enabled = false;
 
 static int __init enable_tmem(char *s)
@@ -136,6 +138,7 @@ static int __init enable_tmem(char *s)
 	return 1;
 }
 __setup("tmem", enable_tmem);
+#endif
 
 #ifdef CONFIG_CLEANCACHE
 static int xen_tmem_destroy_pool(u32 pool_id)
@@ -227,16 +230,21 @@ static int tmem_cleancache_init_shared_fs(char *uuid, size_t pagesize)
 	return xen_tmem_new_pool(shared_uuid, TMEM_POOL_SHARED, pagesize);
 }
 
-static bool __initdata use_cleancache = true;
-
+static bool disable_cleancache __read_mostly;
+static bool disable_selfballooning __read_mostly;
+#ifdef CONFIG_XEN_TMEM_MODULE
+module_param(disable_cleancache, bool, S_IRUGO);
+module_param(disable_selfballooning, bool, S_IRUGO);
+#else
 static int __init no_cleancache(char *s)
 {
-	use_cleancache = false;
+	disable_cleancache = true;
 	return 1;
 }
 __setup("nocleancache", no_cleancache);
+#endif
 
-static struct cleancache_ops __initdata tmem_cleancache_ops = {
+static struct cleancache_ops tmem_cleancache_ops = {
 	.put_page = tmem_cleancache_put_page,
 	.get_page = tmem_cleancache_get_page,
 	.invalidate_page = tmem_cleancache_flush_page,
@@ -353,54 +361,71 @@ static void tmem_frontswap_init(unsigned ignored)
 		    xen_tmem_new_pool(private, TMEM_POOL_PERSIST, PAGE_SIZE);
 }
 
-static bool __initdata use_frontswap = true;
-
+static bool disable_frontswap __read_mostly;
+static bool disable_frontswap_selfshrinking __read_mostly;
+#ifdef CONFIG_XEN_TMEM_MODULE
+module_param(disable_frontswap, bool, S_IRUGO);
+module_param(disable_frontswap_selfshrinking, bool, S_IRUGO);
+#else
 static int __init no_frontswap(char *s)
 {
-	use_frontswap = false;
+	disable_frontswap = true;
 	return 1;
 }
 __setup("nofrontswap", no_frontswap);
+#endif
 
-static struct frontswap_ops __initdata tmem_frontswap_ops = {
+static struct frontswap_ops tmem_frontswap_ops = {
 	.store = tmem_frontswap_store,
 	.load = tmem_frontswap_load,
 	.invalidate_page = tmem_frontswap_flush_page,
 	.invalidate_area = tmem_frontswap_flush_area,
 	.init = tmem_frontswap_init
 };
+#else	/* CONFIG_FRONTSWAP */
+#define disable_frontswap_selfshrinking 1
 #endif
 
-static int __init xen_tmem_init(void)
+static int xen_tmem_init(void)
 {
 	if (!xen_domain())
 		return 0;
 #ifdef CONFIG_FRONTSWAP
-	if (tmem_enabled && use_frontswap) {
+	if (tmem_enabled && !disable_frontswap) {
 		char *s = "";
-		struct frontswap_ops old_ops =
+		struct frontswap_ops *old_ops =
 			frontswap_register_ops(&tmem_frontswap_ops);
 
 		tmem_frontswap_poolid = -1;
-		if (old_ops.init != NULL)
+		if (IS_ERR(old_ops) || old_ops) {
+			if (IS_ERR(old_ops))
+				return PTR_ERR(old_ops);
 			s = " (WARNING: frontswap_ops overridden)";
+		}
 		printk(KERN_INFO "frontswap enabled, RAM provided by "
 				 "Xen Transcendent Memory%s\n", s);
 	}
 #endif
 #ifdef CONFIG_CLEANCACHE
 	BUG_ON(sizeof(struct cleancache_filekey) != sizeof(struct tmem_oid));
-	if (tmem_enabled && use_cleancache) {
+	if (tmem_enabled && !disable_cleancache) {
 		char *s = "";
-		struct cleancache_ops old_ops =
+		struct cleancache_ops *old_ops =
 			cleancache_register_ops(&tmem_cleancache_ops);
-		if (old_ops.init_fs != NULL)
+		if (old_ops)
 			s = " (WARNING: cleancache_ops overridden)";
 		printk(KERN_INFO "cleancache enabled, RAM provided by "
 				 "Xen Transcendent Memory%s\n", s);
 	}
 #endif
+#ifdef CONFIG_XEN_SELFBALLOONING
+	xen_selfballoon_init(!disable_selfballooning,
+				!disable_frontswap_selfshrinking);
+#endif
 	return 0;
 }
 
 module_init(xen_tmem_init)
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Dan Magenheimer <dan.magenheimer@oracle.com>");
+MODULE_DESCRIPTION("Shim to Xen transcendent memory");
diff --git a/drivers/xen/xen-selfballoon.c b/drivers/xen/xen-selfballoon.c
index 2552d3e0a70f..f2ef569c7cc1 100644
--- a/drivers/xen/xen-selfballoon.c
+++ b/drivers/xen/xen-selfballoon.c
@@ -121,7 +121,7 @@ static DECLARE_DELAYED_WORK(selfballoon_worker, selfballoon_process);
 static bool frontswap_selfshrinking __read_mostly;
 
 /* Enable/disable with kernel boot option. */
-static bool use_frontswap_selfshrink __initdata = true;
+static bool use_frontswap_selfshrink = true;
 
 /*
  * The default values for the following parameters were deemed reasonable
@@ -185,7 +185,7 @@ static int __init xen_nofrontswap_selfshrink_setup(char *s)
 __setup("noselfshrink", xen_nofrontswap_selfshrink_setup);
 
 /* Disable with kernel boot option. */
-static bool use_selfballooning __initdata = true;
+static bool use_selfballooning = true;
 
 static int __init xen_noselfballooning_setup(char *s)
 {
@@ -196,7 +196,7 @@ static int __init xen_noselfballooning_setup(char *s)
 __setup("noselfballooning", xen_noselfballooning_setup);
 #else /* !CONFIG_FRONTSWAP */
 /* Enable with kernel boot option. */
-static bool use_selfballooning __initdata = false;
+static bool use_selfballooning;
 
 static int __init xen_selfballooning_setup(char *s)
 {
@@ -537,7 +537,7 @@ int register_xen_selfballooning(struct device *dev)
 }
 EXPORT_SYMBOL(register_xen_selfballooning);
 
-static int __init xen_selfballoon_init(void)
+int xen_selfballoon_init(bool use_selfballooning, bool use_frontswap_selfshrink)
 {
 	bool enable = false;
 
@@ -571,7 +571,4 @@ static int __init xen_selfballoon_init(void)
 
 	return 0;
 }
-
-subsys_initcall(xen_selfballoon_init);
-
-MODULE_LICENSE("GPL");
+EXPORT_SYMBOL(xen_selfballoon_init);
diff --git a/drivers/zorro/proc.c b/drivers/zorro/proc.c
index 73b33837e12c..1c15ee7456b6 100644
--- a/drivers/zorro/proc.c
+++ b/drivers/zorro/proc.c
@@ -47,9 +47,7 @@ proc_bus_zorro_lseek(struct file *file, loff_t off, int whence)
 static ssize_t
 proc_bus_zorro_read(struct file *file, char __user *buf, size_t nbytes, loff_t *ppos)
 {
-	struct inode *ino = file_inode(file);
-	struct proc_dir_entry *dp = PDE(ino);
-	struct zorro_dev *z = dp->data;
+	struct zorro_dev *z = PDE_DATA(file_inode(file));
 	struct ConfigDev cd;
 	loff_t pos = *ppos;
 
@@ -141,7 +139,7 @@ static int __init zorro_proc_attach_device(unsigned int slot)
 				 &zorro_autocon[slot]);
 	if (!entry)
 		return -ENOMEM;
-	entry->size = sizeof(struct zorro_dev);
+	proc_set_size(entry, sizeof(struct zorro_dev));
 	return 0;
 }